4750 files changed, 278545 insertions, 226524 deletions

diff --git a/drivers/Kconfig b/drivers/Kconfig
index 622fa266b29e..1a693d3f9d51 100644
--- a/drivers/Kconfig
+++ b/drivers/Kconfig
@@ -148,6 +148,8 @@ source "drivers/remoteproc/Kconfig"
 
 source "drivers/rpmsg/Kconfig"
 
+source "drivers/soc/Kconfig"
+
 source "drivers/devfreq/Kconfig"
 
 source "drivers/extcon/Kconfig"
diff --git a/drivers/acpi/Kconfig b/drivers/acpi/Kconfig
index d0f3265fb85d..b23fe37f67c0 100644
--- a/drivers/acpi/Kconfig
+++ b/drivers/acpi/Kconfig
@@ -144,7 +144,7 @@ config ACPI_VIDEO
 
 config ACPI_FAN
 	tristate "Fan"
-	select THERMAL
+	depends on THERMAL
 	default y
 	help
 	  This driver supports ACPI fan devices, allowing user-mode
diff --git a/drivers/acpi/Makefile b/drivers/acpi/Makefile
index 505d4d79fe3e..c3b2fcb729f3 100644
--- a/drivers/acpi/Makefile
+++ b/drivers/acpi/Makefile
@@ -43,6 +43,7 @@ acpi-y				+= pci_root.o pci_link.o pci_irq.o
 acpi-y				+= acpi_lpss.o
 acpi-y				+= acpi_platform.o
 acpi-y				+= acpi_pnp.o
+acpi-y				+= int340x_thermal.o
 acpi-y				+= power.o
 acpi-y				+= event.o
 acpi-y				+= sysfs.o
diff --git a/drivers/acpi/acpi_cmos_rtc.c b/drivers/acpi/acpi_cmos_rtc.c
index 2da8660262e5..81dc75033f15 100644
--- a/drivers/acpi/acpi_cmos_rtc.c
+++ b/drivers/acpi/acpi_cmos_rtc.c
@@ -33,7 +33,7 @@ acpi_cmos_rtc_space_handler(u32 function, acpi_physical_address address,
 		      void *handler_context, void *region_context)
 {
 	int i;
-	u8 *value = (u8 *)&value64;
+	u8 *value = (u8 *)value64;
 
 	if (address > 0xff || !value64)
 		return AE_BAD_PARAMETER;
diff --git a/drivers/acpi/acpi_lpss.c b/drivers/acpi/acpi_lpss.c
index ce06149088c5..93d160661f4c 100644
--- a/drivers/acpi/acpi_lpss.c
+++ b/drivers/acpi/acpi_lpss.c
@@ -54,55 +54,58 @@ ACPI_MODULE_NAME("acpi_lpss");
 
 #define LPSS_PRV_REG_COUNT		9
 
-struct lpss_shared_clock {
-	const char *name;
-	unsigned long rate;
-	struct clk *clk;
-};
+/* LPSS Flags */
+#define LPSS_CLK			BIT(0)
+#define LPSS_CLK_GATE			BIT(1)
+#define LPSS_CLK_DIVIDER		BIT(2)
+#define LPSS_LTR			BIT(3)
+#define LPSS_SAVE_CTX			BIT(4)
 
 struct lpss_private_data;
 
 struct lpss_device_desc {
-	bool clk_required;
-	const char *clkdev_name;
-	bool ltr_required;
+	unsigned int flags;
 	unsigned int prv_offset;
 	size_t prv_size_override;
-	bool clk_divider;
-	bool clk_gate;
-	bool save_ctx;
-	struct lpss_shared_clock *shared_clock;
 	void (*setup)(struct lpss_private_data *pdata);
 };
 
 static struct lpss_device_desc lpss_dma_desc = {
-	.clk_required = true,
-	.clkdev_name = "hclk",
+	.flags = LPSS_CLK,
 };
 
 struct lpss_private_data {
 	void __iomem *mmio_base;
 	resource_size_t mmio_size;
+	unsigned int fixed_clk_rate;
 	struct clk *clk;
 	const struct lpss_device_desc *dev_desc;
 	u32 prv_reg_ctx[LPSS_PRV_REG_COUNT];
 };
 
+/* UART Component Parameter Register */
+#define LPSS_UART_CPR			0xF4
+#define LPSS_UART_CPR_AFCE		BIT(4)
+
 static void lpss_uart_setup(struct lpss_private_data *pdata)
 {
 	unsigned int offset;
-	u32 reg;
+	u32 val;
 
 	offset = pdata->dev_desc->prv_offset + LPSS_TX_INT;
-	reg = readl(pdata->mmio_base + offset);
-	writel(reg | LPSS_TX_INT_MASK, pdata->mmio_base + offset);
-
-	offset = pdata->dev_desc->prv_offset + LPSS_GENERAL;
-	reg = readl(pdata->mmio_base + offset);
-	writel(reg | LPSS_GENERAL_UART_RTS_OVRD, pdata->mmio_base + offset);
+	val = readl(pdata->mmio_base + offset);
+	writel(val | LPSS_TX_INT_MASK, pdata->mmio_base + offset);
+
+	val = readl(pdata->mmio_base + LPSS_UART_CPR);
+	if (!(val & LPSS_UART_CPR_AFCE)) {
+		offset = pdata->dev_desc->prv_offset + LPSS_GENERAL;
+		val = readl(pdata->mmio_base + offset);
+		val |= LPSS_GENERAL_UART_RTS_OVRD;
+		writel(val, pdata->mmio_base + offset);
+	}
 }
 
-static void lpss_i2c_setup(struct lpss_private_data *pdata)
+static void byt_i2c_setup(struct lpss_private_data *pdata)
 {
 	unsigned int offset;
 	u32 val;
@@ -111,89 +114,56 @@ static void lpss_i2c_setup(struct lpss_private_data *pdata)
 	val = readl(pdata->mmio_base + offset);
 	val |= LPSS_RESETS_RESET_APB | LPSS_RESETS_RESET_FUNC;
 	writel(val, pdata->mmio_base + offset);
-}
 
-static struct lpss_device_desc wpt_dev_desc = {
-	.clk_required = true,
-	.prv_offset = 0x800,
-	.ltr_required = true,
-	.clk_divider = true,
-	.clk_gate = true,
-};
+	if (readl(pdata->mmio_base + pdata->dev_desc->prv_offset))
+		pdata->fixed_clk_rate = 133000000;
+}
 
 static struct lpss_device_desc lpt_dev_desc = {
-	.clk_required = true,
+	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_LTR,
 	.prv_offset = 0x800,
-	.ltr_required = true,
-	.clk_divider = true,
-	.clk_gate = true,
 };
 
 static struct lpss_device_desc lpt_i2c_dev_desc = {
-	.clk_required = true,
+	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_LTR,
 	.prv_offset = 0x800,
-	.ltr_required = true,
-	.clk_gate = true,
 };
 
 static struct lpss_device_desc lpt_uart_dev_desc = {
-	.clk_required = true,
+	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_LTR,
 	.prv_offset = 0x800,
-	.ltr_required = true,
-	.clk_divider = true,
-	.clk_gate = true,
 	.setup = lpss_uart_setup,
 };
 
 static struct lpss_device_desc lpt_sdio_dev_desc = {
+	.flags = LPSS_LTR,
 	.prv_offset = 0x1000,
 	.prv_size_override = 0x1018,
-	.ltr_required = true,
-};
-
-static struct lpss_shared_clock pwm_clock = {
-	.name = "pwm_clk",
-	.rate = 25000000,
 };
 
 static struct lpss_device_desc byt_pwm_dev_desc = {
-	.clk_required = true,
-	.save_ctx = true,
-	.shared_clock = &pwm_clock,
+	.flags = LPSS_SAVE_CTX,
 };
 
 static struct lpss_device_desc byt_uart_dev_desc = {
-	.clk_required = true,
+	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_SAVE_CTX,
 	.prv_offset = 0x800,
-	.clk_divider = true,
-	.clk_gate = true,
-	.save_ctx = true,
 	.setup = lpss_uart_setup,
 };
 
 static struct lpss_device_desc byt_spi_dev_desc = {
-	.clk_required = true,
+	.flags = LPSS_CLK | LPSS_CLK_GATE | LPSS_CLK_DIVIDER | LPSS_SAVE_CTX,
 	.prv_offset = 0x400,
-	.clk_divider = true,
-	.clk_gate = true,
-	.save_ctx = true,
 };
 
 static struct lpss_device_desc byt_sdio_dev_desc = {
-	.clk_required = true,
-};
-
-static struct lpss_shared_clock i2c_clock = {
-	.name = "i2c_clk",
-	.rate = 100000000,
+	.flags = LPSS_CLK,
 };
 
 static struct lpss_device_desc byt_i2c_dev_desc = {
-	.clk_required = true,
+	.flags = LPSS_CLK | LPSS_SAVE_CTX,
 	.prv_offset = 0x800,
-	.save_ctx = true,
-	.shared_clock = &i2c_clock,
-	.setup = lpss_i2c_setup,
+	.setup = byt_i2c_setup,
 };
 
 #else
@@ -225,6 +195,12 @@ static const struct acpi_device_id acpi_lpss_device_ids[] = {
 	{ "INT33B2", },
 	{ "INT33FC", },
 
+	/* Braswell LPSS devices */
+	{ "80862288", LPSS_ADDR(byt_pwm_dev_desc) },
+	{ "8086228A", LPSS_ADDR(byt_uart_dev_desc) },
+	{ "8086228E", LPSS_ADDR(byt_spi_dev_desc) },
+	{ "808622C1", LPSS_ADDR(byt_i2c_dev_desc) },
+
 	{ "INT3430", LPSS_ADDR(lpt_dev_desc) },
 	{ "INT3431", LPSS_ADDR(lpt_dev_desc) },
 	{ "INT3432", LPSS_ADDR(lpt_i2c_dev_desc) },
@@ -234,7 +210,8 @@ static const struct acpi_device_id acpi_lpss_device_ids[] = {
 	{ "INT3436", LPSS_ADDR(lpt_sdio_dev_desc) },
 	{ "INT3437", },
 
-	{ "INT3438", LPSS_ADDR(wpt_dev_desc) },
+	/* Wildcat Point LPSS devices */
+	{ "INT3438", LPSS_ADDR(lpt_dev_desc) },
 
 	{ }
 };
@@ -259,7 +236,6 @@ static int register_device_clock(struct acpi_device *adev,
 				 struct lpss_private_data *pdata)
 {
 	const struct lpss_device_desc *dev_desc = pdata->dev_desc;
-	struct lpss_shared_clock *shared_clock = dev_desc->shared_clock;
 	const char *devname = dev_name(&adev->dev);
 	struct clk *clk = ERR_PTR(-ENODEV);
 	struct lpss_clk_data *clk_data;
@@ -272,12 +248,7 @@ static int register_device_clock(struct acpi_device *adev,
 	clk_data = platform_get_drvdata(lpss_clk_dev);
 	if (!clk_data)
 		return -ENODEV;
-
-	if (dev_desc->clkdev_name) {
-		clk_register_clkdev(clk_data->clk, dev_desc->clkdev_name,
-				    devname);
-		return 0;
-	}
+	clk = clk_data->clk;
 
 	if (!pdata->mmio_base
 	    || pdata->mmio_size < dev_desc->prv_offset + LPSS_CLK_SIZE)
@@ -286,24 +257,19 @@ static int register_device_clock(struct acpi_device *adev,
 	parent = clk_data->name;
 	prv_base = pdata->mmio_base + dev_desc->prv_offset;
 
-	if (shared_clock) {
-		clk = shared_clock->clk;
-		if (!clk) {
-			clk = clk_register_fixed_rate(NULL, shared_clock->name,
-						      "lpss_clk", 0,
-						      shared_clock->rate);
-			shared_clock->clk = clk;
-		}
-		parent = shared_clock->name;
+	if (pdata->fixed_clk_rate) {
+		clk = clk_register_fixed_rate(NULL, devname, parent, 0,
+					      pdata->fixed_clk_rate);
+		goto out;
 	}
 
-	if (dev_desc->clk_gate) {
+	if (dev_desc->flags & LPSS_CLK_GATE) {
 		clk = clk_register_gate(NULL, devname, parent, 0,
 					prv_base, 0, 0, NULL);
 		parent = devname;
 	}
 
-	if (dev_desc->clk_divider) {
+	if (dev_desc->flags & LPSS_CLK_DIVIDER) {
 		/* Prevent division by zero */
 		if (!readl(prv_base))
 			writel(LPSS_CLK_DIVIDER_DEF_MASK, prv_base);
@@ -327,7 +293,7 @@ static int register_device_clock(struct acpi_device *adev,
 		kfree(parent);
 		kfree(clk_name);
 	}
-
+out:
 	if (IS_ERR(clk))
 		return PTR_ERR(clk);
 
@@ -375,7 +341,10 @@ static int acpi_lpss_create_device(struct acpi_device *adev,
 
 	pdata->dev_desc = dev_desc;
 
-	if (dev_desc->clk_required) {
+	if (dev_desc->setup)
+		dev_desc->setup(pdata);
+
+	if (dev_desc->flags & LPSS_CLK) {
 		ret = register_device_clock(adev, pdata);
 		if (ret) {
 			/* Skip the device, but continue the namespace scan. */
@@ -396,13 +365,9 @@ static int acpi_lpss_create_device(struct acpi_device *adev,
 		goto err_out;
 	}
 
-	if (dev_desc->setup)
-		dev_desc->setup(pdata);
-
 	adev->driver_data = pdata;
 	pdev = acpi_create_platform_device(adev);
 	if (!IS_ERR_OR_NULL(pdev)) {
-		device_enable_async_suspend(&pdev->dev);
 		return 1;
 	}
 
@@ -593,7 +558,7 @@ static int acpi_lpss_suspend_late(struct device *dev)
 	return acpi_dev_suspend_late(dev);
 }
 
-static int acpi_lpss_restore_early(struct device *dev)
+static int acpi_lpss_resume_early(struct device *dev)
 {
 	int ret = acpi_dev_resume_early(dev);
 
@@ -633,15 +598,15 @@ static int acpi_lpss_runtime_resume(struct device *dev)
 static struct dev_pm_domain acpi_lpss_pm_domain = {
 	.ops = {
 #ifdef CONFIG_PM_SLEEP
-		.suspend_late = acpi_lpss_suspend_late,
-		.restore_early = acpi_lpss_restore_early,
 		.prepare = acpi_subsys_prepare,
 		.complete = acpi_subsys_complete,
 		.suspend = acpi_subsys_suspend,
-		.resume_early = acpi_subsys_resume_early,
+		.suspend_late = acpi_lpss_suspend_late,
+		.resume_early = acpi_lpss_resume_early,
 		.freeze = acpi_subsys_freeze,
 		.poweroff = acpi_subsys_suspend,
-		.poweroff_late = acpi_subsys_suspend_late,
+		.poweroff_late = acpi_lpss_suspend_late,
+		.restore_early = acpi_lpss_resume_early,
 #endif
 #ifdef CONFIG_PM_RUNTIME
 		.runtime_suspend = acpi_lpss_runtime_suspend,
@@ -676,19 +641,19 @@ static int acpi_lpss_platform_notify(struct notifier_block *nb,
 
 	switch (action) {
 	case BUS_NOTIFY_BOUND_DRIVER:
-		if (pdata->dev_desc->save_ctx)
+		if (pdata->dev_desc->flags & LPSS_SAVE_CTX)
 			pdev->dev.pm_domain = &acpi_lpss_pm_domain;
 		break;
 	case BUS_NOTIFY_UNBOUND_DRIVER:
-		if (pdata->dev_desc->save_ctx)
+		if (pdata->dev_desc->flags & LPSS_SAVE_CTX)
 			pdev->dev.pm_domain = NULL;
 		break;
 	case BUS_NOTIFY_ADD_DEVICE:
-		if (pdata->dev_desc->ltr_required)
+		if (pdata->dev_desc->flags & LPSS_LTR)
 			return sysfs_create_group(&pdev->dev.kobj,
 						  &lpss_attr_group);
 	case BUS_NOTIFY_DEL_DEVICE:
-		if (pdata->dev_desc->ltr_required)
+		if (pdata->dev_desc->flags & LPSS_LTR)
 			sysfs_remove_group(&pdev->dev.kobj, &lpss_attr_group);
 	default:
 		break;
@@ -705,7 +670,7 @@ static void acpi_lpss_bind(struct device *dev)
 {
 	struct lpss_private_data *pdata = acpi_driver_data(ACPI_COMPANION(dev));
 
-	if (!pdata || !pdata->mmio_base || !pdata->dev_desc->ltr_required)
+	if (!pdata || !pdata->mmio_base || !(pdata->dev_desc->flags & LPSS_LTR))
 		return;
 
 	if (pdata->mmio_size >= pdata->dev_desc->prv_offset + LPSS_LTR_SIZE)
diff --git a/drivers/acpi/acpi_platform.c b/drivers/acpi/acpi_platform.c
index 2bf9082f7523..6ba8beb6b9d2 100644
--- a/drivers/acpi/acpi_platform.c
+++ b/drivers/acpi/acpi_platform.c
@@ -16,6 +16,7 @@
 #include <linux/err.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/dma-mapping.h>
 #include <linux/platform_device.h>
 
 #include "internal.h"
@@ -102,6 +103,7 @@ struct platform_device *acpi_create_platform_device(struct acpi_device *adev)
 	pdevinfo.res = resources;
 	pdevinfo.num_res = count;
 	pdevinfo.acpi_node.companion = adev;
+	pdevinfo.dma_mask = DMA_BIT_MASK(32);
 	pdev = platform_device_register_full(&pdevinfo);
 	if (IS_ERR(pdev))
 		dev_err(&adev->dev, "platform device creation failed: %ld\n",
@@ -113,3 +115,4 @@ struct platform_device *acpi_create_platform_device(struct acpi_device *adev)
 	kfree(resources);
 	return pdev;
 }
+EXPORT_SYMBOL_GPL(acpi_create_platform_device);
diff --git a/drivers/acpi/acpi_pnp.c b/drivers/acpi/acpi_pnp.c
index 996fa1959eea..b193f8425999 100644
--- a/drivers/acpi/acpi_pnp.c
+++ b/drivers/acpi/acpi_pnp.c
@@ -15,8 +15,6 @@
 #include <linux/ctype.h>
 
 static const struct acpi_device_id acpi_pnp_device_ids[] = {
-	/* soc_button_array */
-	{"PNP0C40"},
 	/* pata_isapnp */
 	{"PNP0600"},		/* Generic ESDI/IDE/ATA compatible hard disk controller */
 	/* floppy */
@@ -132,10 +130,6 @@ static const struct acpi_device_id acpi_pnp_device_ids[] = {
 	{"PNP0401"},		/* ECP Printer Port */
 	/* apple-gmux */
 	{"APP000B"},
-	/* fujitsu-laptop.c */
-	{"FUJ02bf"},
-	{"FUJ02B1"},
-	{"FUJ02E3"},
 	/* system */
 	{"PNP0c02"},		/* General ID for reserving resources */
 	{"PNP0c01"},		/* memory controller */
diff --git a/drivers/acpi/acpica/achware.h b/drivers/acpi/acpica/achware.h
index 2ad2351a9833..c318d3e27893 100644
--- a/drivers/acpi/acpica/achware.h
+++ b/drivers/acpi/acpica/achware.h
@@ -127,7 +127,7 @@ acpi_hw_clear_gpe_block(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 
 acpi_status
 acpi_hw_get_gpe_status(struct acpi_gpe_event_info *gpe_event_info,
-		       acpi_event_status * event_status);
+		       acpi_event_status *event_status);
 
 acpi_status acpi_hw_disable_all_gpes(void);
 
diff --git a/drivers/acpi/acpica/aclocal.h b/drivers/acpi/acpica/aclocal.h
index 1f9aba5fb81f..c00e7e41ad75 100644
--- a/drivers/acpi/acpica/aclocal.h
+++ b/drivers/acpi/acpica/aclocal.h
@@ -254,6 +254,7 @@ struct acpi_create_field_info {
 	u32 field_bit_position;
 	u32 field_bit_length;
 	u16 resource_length;
+	u16 pin_number_index;
 	u8 field_flags;
 	u8 attribute;
 	u8 field_type;
@@ -412,8 +413,8 @@ struct acpi_gpe_handler_info {
 	acpi_gpe_handler address;	/* Address of handler, if any */
 	void *context;		/* Context to be passed to handler */
 	struct acpi_namespace_node *method_node;	/* Method node for this GPE level (saved) */
-	u8 original_flags;      /* Original (pre-handler) GPE info */
-	u8 originally_enabled;  /* True if GPE was originally enabled */
+	u8 original_flags;	/* Original (pre-handler) GPE info */
+	u8 originally_enabled;	/* True if GPE was originally enabled */
 };
 
 /* Notify info for implicit notify, multiple device objects */
diff --git a/drivers/acpi/acpica/acobject.h b/drivers/acpi/acpica/acobject.h
index 22fb6449d3d6..8abb393dafab 100644
--- a/drivers/acpi/acpica/acobject.h
+++ b/drivers/acpi/acpica/acobject.h
@@ -264,6 +264,7 @@ struct acpi_object_region_field {
 	ACPI_OBJECT_COMMON_HEADER ACPI_COMMON_FIELD_INFO u16 resource_length;
 	union acpi_operand_object *region_obj;	/* Containing op_region object */
 	u8 *resource_buffer;	/* resource_template for serial regions/fields */
+	u16 pin_number_index;	/* Index relative to previous Connection/Template */
 };
 
 struct acpi_object_bank_field {
diff --git a/drivers/acpi/acpica/actables.h b/drivers/acpi/acpica/actables.h
index f14882788eee..1afe46e44dac 100644
--- a/drivers/acpi/acpica/actables.h
+++ b/drivers/acpi/acpica/actables.h
@@ -49,6 +49,8 @@ acpi_status acpi_allocate_root_table(u32 initial_table_count);
 /*
  * tbxfroot - Root pointer utilities
  */
+u32 acpi_tb_get_rsdp_length(struct acpi_table_rsdp *rsdp);
+
 acpi_status acpi_tb_validate_rsdp(struct acpi_table_rsdp *rsdp);
 
 u8 *acpi_tb_scan_memory_for_rsdp(u8 *start_address, u32 length);
diff --git a/drivers/acpi/acpica/amlresrc.h b/drivers/acpi/acpica/amlresrc.h
index f3f834408441..3a0beeb86ba5 100644
--- a/drivers/acpi/acpica/amlresrc.h
+++ b/drivers/acpi/acpica/amlresrc.h
@@ -117,6 +117,12 @@ struct asl_resource_node {
 	struct asl_resource_node *next;
 };
 
+struct asl_resource_info {
+	union acpi_parse_object *descriptor_type_op;	/* Resource descriptor parse node */
+	union acpi_parse_object *mapping_op;	/* Used for mapfile support */
+	u32 current_byte_offset;	/* Offset in resource template */
+};
+
 /* Macros used to generate AML resource length fields */
 
 #define ACPI_AML_SIZE_LARGE(r)      (sizeof (r) - sizeof (struct aml_resource_large_header))
@@ -449,4 +455,32 @@ union aml_resource {
 	u8 byte_item;
 };
 
+/* Interfaces used by both the disassembler and compiler */
+
+void
+mp_save_gpio_info(union acpi_parse_object *op,
+		  union aml_resource *resource,
+		  u32 pin_count, u16 *pin_list, char *device_name);
+
+void
+mp_save_serial_info(union acpi_parse_object *op,
+		    union aml_resource *resource, char *device_name);
+
+char *mp_get_hid_from_parse_tree(struct acpi_namespace_node *hid_node);
+
+char *mp_get_hid_via_namestring(char *device_name);
+
+char *mp_get_connection_info(union acpi_parse_object *op,
+			     u32 pin_index,
+			     struct acpi_namespace_node **target_node,
+			     char **target_name);
+
+char *mp_get_parent_device_hid(union acpi_parse_object *op,
+			       struct acpi_namespace_node **target_node,
+			       char **parent_device_name);
+
+char *mp_get_ddn_value(char *device_name);
+
+char *mp_get_hid_value(struct acpi_namespace_node *device_node);
+
 #endif
diff --git a/drivers/acpi/acpica/dsfield.c b/drivers/acpi/acpica/dsfield.c
index 3661c8e90540..c57666196672 100644
--- a/drivers/acpi/acpica/dsfield.c
+++ b/drivers/acpi/acpica/dsfield.c
@@ -360,6 +360,7 @@ acpi_ds_get_field_names(struct acpi_create_field_info *info,
 			 */
 			info->resource_buffer = NULL;
 			info->connection_node = NULL;
+			info->pin_number_index = 0;
 
 			/*
 			 * A Connection() is either an actual resource descriptor (buffer)
@@ -437,6 +438,7 @@ acpi_ds_get_field_names(struct acpi_create_field_info *info,
 			}
 
 			info->field_bit_position += info->field_bit_length;
+			info->pin_number_index++;	/* Index relative to previous Connection() */
 			break;
 
 		default:
diff --git a/drivers/acpi/acpica/evgpe.c b/drivers/acpi/acpica/evgpe.c
index e4ba4dec86af..2095dfb72bcb 100644
--- a/drivers/acpi/acpica/evgpe.c
+++ b/drivers/acpi/acpica/evgpe.c
@@ -100,13 +100,14 @@ acpi_ev_update_gpe_enable_mask(struct acpi_gpe_event_info *gpe_event_info)
  *
  * FUNCTION:    acpi_ev_enable_gpe
  *
- * PARAMETERS:  gpe_event_info  - GPE to enable
+ * PARAMETERS:  gpe_event_info          - GPE to enable
  *
  * RETURN:      Status
  *
  * DESCRIPTION: Clear a GPE of stale events and enable it.
  *
  ******************************************************************************/
+
 acpi_status acpi_ev_enable_gpe(struct acpi_gpe_event_info *gpe_event_info)
 {
 	acpi_status status;
@@ -125,6 +126,7 @@ acpi_status acpi_ev_enable_gpe(struct acpi_gpe_event_info *gpe_event_info)
 	}
 
 	/* Clear the GPE (of stale events) */
+
 	status = acpi_hw_clear_gpe(gpe_event_info);
 	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
@@ -136,7 +138,6 @@ acpi_status acpi_ev_enable_gpe(struct acpi_gpe_event_info *gpe_event_info)
 	return_ACPI_STATUS(status);
 }
 
-
 /*******************************************************************************
  *
  * FUNCTION:    acpi_ev_add_gpe_reference
@@ -212,7 +213,7 @@ acpi_ev_remove_gpe_reference(struct acpi_gpe_event_info *gpe_event_info)
 		if (ACPI_SUCCESS(status)) {
 			status =
 			    acpi_hw_low_set_gpe(gpe_event_info,
-						     ACPI_GPE_DISABLE);
+						ACPI_GPE_DISABLE);
 		}
 
 		if (ACPI_FAILURE(status)) {
@@ -334,7 +335,7 @@ struct acpi_gpe_event_info *acpi_ev_get_gpe_event_info(acpi_handle gpe_device,
  *
  ******************************************************************************/
 
-u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info * gpe_xrupt_list)
+u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info *gpe_xrupt_list)
 {
 	acpi_status status;
 	struct acpi_gpe_block_info *gpe_block;
@@ -427,7 +428,7 @@ u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info * gpe_xrupt_list)
 
 			/* Check if there is anything active at all in this register */
 
-			enabled_status_byte = (u8) (status_reg & enable_reg);
+			enabled_status_byte = (u8)(status_reg & enable_reg);
 			if (!enabled_status_byte) {
 
 				/* No active GPEs in this register, move on */
@@ -450,7 +451,7 @@ u32 acpi_ev_gpe_detect(struct acpi_gpe_xrupt_info * gpe_xrupt_list)
 					    acpi_ev_gpe_dispatch(gpe_block->
 								 node,
 								 &gpe_block->
-						event_info[((acpi_size) i * ACPI_GPE_REGISTER_WIDTH) + j], j + gpe_register_info->base_gpe_number);
+								 event_info[((acpi_size) i * ACPI_GPE_REGISTER_WIDTH) + j], j + gpe_register_info->base_gpe_number);
 				}
 			}
 		}
@@ -636,7 +637,7 @@ static void ACPI_SYSTEM_XFACE acpi_ev_asynch_enable_gpe(void *context)
  *
  ******************************************************************************/
 
-acpi_status acpi_ev_finish_gpe(struct acpi_gpe_event_info *gpe_event_info)
+acpi_status acpi_ev_finish_gpe(struct acpi_gpe_event_info * gpe_event_info)
 {
 	acpi_status status;
 
@@ -666,9 +667,9 @@ acpi_status acpi_ev_finish_gpe(struct acpi_gpe_event_info *gpe_event_info)
  *
  * FUNCTION:    acpi_ev_gpe_dispatch
  *
- * PARAMETERS:  gpe_device      - Device node. NULL for GPE0/GPE1
- *              gpe_event_info  - Info for this GPE
- *              gpe_number      - Number relative to the parent GPE block
+ * PARAMETERS:  gpe_device          - Device node. NULL for GPE0/GPE1
+ *              gpe_event_info      - Info for this GPE
+ *              gpe_number          - Number relative to the parent GPE block
  *
  * RETURN:      INTERRUPT_HANDLED or INTERRUPT_NOT_HANDLED
  *
@@ -681,7 +682,7 @@ acpi_status acpi_ev_finish_gpe(struct acpi_gpe_event_info *gpe_event_info)
 
 u32
 acpi_ev_gpe_dispatch(struct acpi_namespace_node *gpe_device,
-		    struct acpi_gpe_event_info *gpe_event_info, u32 gpe_number)
+		     struct acpi_gpe_event_info *gpe_event_info, u32 gpe_number)
 {
 	acpi_status status;
 	u32 return_value;
diff --git a/drivers/acpi/acpica/evgpeinit.c b/drivers/acpi/acpica/evgpeinit.c
index 49fc7effd961..7be928379879 100644
--- a/drivers/acpi/acpica/evgpeinit.c
+++ b/drivers/acpi/acpica/evgpeinit.c
@@ -424,6 +424,7 @@ acpi_ev_match_gpe_method(acpi_handle obj_handle,
 	}
 
 	/* Disable the GPE in case it's been enabled already. */
+
 	(void)acpi_hw_low_set_gpe(gpe_event_info, ACPI_GPE_DISABLE);
 
 	/*
diff --git a/drivers/acpi/acpica/evregion.c b/drivers/acpi/acpica/evregion.c
index 9957297d1580..8eb8575e8c16 100644
--- a/drivers/acpi/acpica/evregion.c
+++ b/drivers/acpi/acpica/evregion.c
@@ -142,6 +142,7 @@ acpi_ev_address_space_dispatch(union acpi_operand_object *region_obj,
 	union acpi_operand_object *region_obj2;
 	void *region_context = NULL;
 	struct acpi_connection_info *context;
+	acpi_physical_address address;
 
 	ACPI_FUNCTION_TRACE(ev_address_space_dispatch);
 
@@ -231,25 +232,23 @@ acpi_ev_address_space_dispatch(union acpi_operand_object *region_obj,
 	/* We have everything we need, we can invoke the address space handler */
 
 	handler = handler_desc->address_space.handler;
-
-	ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
-			  "Handler %p (@%p) Address %8.8X%8.8X [%s]\n",
-			  &region_obj->region.handler->address_space, handler,
-			  ACPI_FORMAT_NATIVE_UINT(region_obj->region.address +
-						  region_offset),
-			  acpi_ut_get_region_name(region_obj->region.
-						  space_id)));
+	address = (region_obj->region.address + region_offset);
 
 	/*
 	 * Special handling for generic_serial_bus and general_purpose_io:
 	 * There are three extra parameters that must be passed to the
 	 * handler via the context:
-	 *   1) Connection buffer, a resource template from Connection() op.
-	 *   2) Length of the above buffer.
-	 *   3) Actual access length from the access_as() op.
+	 *   1) Connection buffer, a resource template from Connection() op
+	 *   2) Length of the above buffer
+	 *   3) Actual access length from the access_as() op
+	 *
+	 * In addition, for general_purpose_io, the Address and bit_width fields
+	 * are defined as follows:
+	 *   1) Address is the pin number index of the field (bit offset from
+	 *      the previous Connection)
+	 *   2) bit_width is the actual bit length of the field (number of pins)
 	 */
-	if (((region_obj->region.space_id == ACPI_ADR_SPACE_GSBUS) ||
-	     (region_obj->region.space_id == ACPI_ADR_SPACE_GPIO)) &&
+	if ((region_obj->region.space_id == ACPI_ADR_SPACE_GSBUS) &&
 	    context && field_obj) {
 
 		/* Get the Connection (resource_template) buffer */
@@ -258,6 +257,24 @@ acpi_ev_address_space_dispatch(union acpi_operand_object *region_obj,
 		context->length = field_obj->field.resource_length;
 		context->access_length = field_obj->field.access_length;
 	}
+	if ((region_obj->region.space_id == ACPI_ADR_SPACE_GPIO) &&
+	    context && field_obj) {
+
+		/* Get the Connection (resource_template) buffer */
+
+		context->connection = field_obj->field.resource_buffer;
+		context->length = field_obj->field.resource_length;
+		context->access_length = field_obj->field.access_length;
+		address = field_obj->field.pin_number_index;
+		bit_width = field_obj->field.bit_length;
+	}
+
+	ACPI_DEBUG_PRINT((ACPI_DB_OPREGION,
+			  "Handler %p (@%p) Address %8.8X%8.8X [%s]\n",
+			  &region_obj->region.handler->address_space, handler,
+			  ACPI_FORMAT_NATIVE_UINT(address),
+			  acpi_ut_get_region_name(region_obj->region.
+						  space_id)));
 
 	if (!(handler_desc->address_space.handler_flags &
 	      ACPI_ADDR_HANDLER_DEFAULT_INSTALLED)) {
@@ -271,9 +288,7 @@ acpi_ev_address_space_dispatch(union acpi_operand_object *region_obj,
 
 	/* Call the handler */
 
-	status = handler(function,
-			 (region_obj->region.address + region_offset),
-			 bit_width, value, context,
+	status = handler(function, address, bit_width, value, context,
 			 region_obj2->extra.region_context);
 
 	if (ACPI_FAILURE(status)) {
diff --git a/drivers/acpi/acpica/evxface.c b/drivers/acpi/acpica/evxface.c
index 11e5803b8b41..55a58f3ec8df 100644
--- a/drivers/acpi/acpica/evxface.c
+++ b/drivers/acpi/acpica/evxface.c
@@ -786,18 +786,26 @@ acpi_install_gpe_handler(acpi_handle gpe_device,
 	handler->method_node = gpe_event_info->dispatch.method_node;
 	handler->original_flags = (u8)(gpe_event_info->flags &
 				       (ACPI_GPE_XRUPT_TYPE_MASK |
-				        ACPI_GPE_DISPATCH_MASK));
+					ACPI_GPE_DISPATCH_MASK));
 
 	/*
 	 * If the GPE is associated with a method, it may have been enabled
 	 * automatically during initialization, in which case it has to be
 	 * disabled now to avoid spurious execution of the handler.
 	 */
-
-	if ((handler->original_flags & ACPI_GPE_DISPATCH_METHOD)
-	    && gpe_event_info->runtime_count) {
-		handler->originally_enabled = 1;
+	if (((handler->original_flags & ACPI_GPE_DISPATCH_METHOD) ||
+	     (handler->original_flags & ACPI_GPE_DISPATCH_NOTIFY)) &&
+	    gpe_event_info->runtime_count) {
+		handler->originally_enabled = TRUE;
 		(void)acpi_ev_remove_gpe_reference(gpe_event_info);
+
+		/* Sanity check of original type against new type */
+
+		if (type !=
+		    (u32)(gpe_event_info->flags & ACPI_GPE_XRUPT_TYPE_MASK)) {
+			ACPI_WARNING((AE_INFO,
+				      "GPE type mismatch (level/edge)"));
+		}
 	}
 
 	/* Install the handler */
@@ -808,7 +816,7 @@ acpi_install_gpe_handler(acpi_handle gpe_device,
 
 	gpe_event_info->flags &=
 	    ~(ACPI_GPE_XRUPT_TYPE_MASK | ACPI_GPE_DISPATCH_MASK);
-	gpe_event_info->flags |= (u8) (type | ACPI_GPE_DISPATCH_HANDLER);
+	gpe_event_info->flags |= (u8)(type | ACPI_GPE_DISPATCH_HANDLER);
 
 	acpi_os_release_lock(acpi_gbl_gpe_lock, flags);
 
@@ -893,7 +901,7 @@ acpi_remove_gpe_handler(acpi_handle gpe_device,
 
 	gpe_event_info->dispatch.method_node = handler->method_node;
 	gpe_event_info->flags &=
-		~(ACPI_GPE_XRUPT_TYPE_MASK | ACPI_GPE_DISPATCH_MASK);
+	    ~(ACPI_GPE_XRUPT_TYPE_MASK | ACPI_GPE_DISPATCH_MASK);
 	gpe_event_info->flags |= handler->original_flags;
 
 	/*
@@ -901,7 +909,8 @@ acpi_remove_gpe_handler(acpi_handle gpe_device,
 	 * enabled, it should be enabled at this point to restore the
 	 * post-initialization configuration.
 	 */
-	if ((handler->original_flags & ACPI_GPE_DISPATCH_METHOD) &&
+	if (((handler->original_flags & ACPI_GPE_DISPATCH_METHOD) ||
+	     (handler->original_flags & ACPI_GPE_DISPATCH_NOTIFY)) &&
 	    handler->originally_enabled) {
 		(void)acpi_ev_add_gpe_reference(gpe_event_info);
 	}
@@ -946,7 +955,7 @@ ACPI_EXPORT_SYMBOL(acpi_remove_gpe_handler)
  * handle is returned.
  *
  ******************************************************************************/
-acpi_status acpi_acquire_global_lock(u16 timeout, u32 * handle)
+acpi_status acpi_acquire_global_lock(u16 timeout, u32 *handle)
 {
 	acpi_status status;
 
diff --git a/drivers/acpi/acpica/evxfevnt.c b/drivers/acpi/acpica/evxfevnt.c
index e286640ad4ff..bb8cbf5961bf 100644
--- a/drivers/acpi/acpica/evxfevnt.c
+++ b/drivers/acpi/acpica/evxfevnt.c
@@ -324,8 +324,9 @@ ACPI_EXPORT_SYMBOL(acpi_clear_event)
  ******************************************************************************/
 acpi_status acpi_get_event_status(u32 event, acpi_event_status * event_status)
 {
-	acpi_status status = AE_OK;
-	u32 value;
+	acpi_status status;
+	acpi_event_status local_event_status = 0;
+	u32 in_byte;
 
 	ACPI_FUNCTION_TRACE(acpi_get_event_status);
 
@@ -339,29 +340,40 @@ acpi_status acpi_get_event_status(u32 event, acpi_event_status * event_status)
 		return_ACPI_STATUS(AE_BAD_PARAMETER);
 	}
 
-	/* Get the status of the requested fixed event */
+	/* Fixed event currently can be dispatched? */
+
+	if (acpi_gbl_fixed_event_handlers[event].handler) {
+		local_event_status |= ACPI_EVENT_FLAG_HAS_HANDLER;
+	}
+
+	/* Fixed event currently enabled? */
 
 	status =
 	    acpi_read_bit_register(acpi_gbl_fixed_event_info[event].
-			      enable_register_id, &value);
-	if (ACPI_FAILURE(status))
+				   enable_register_id, &in_byte);
+	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
+	}
 
-	*event_status = value;
+	if (in_byte) {
+		local_event_status |= ACPI_EVENT_FLAG_ENABLED;
+	}
+
+	/* Fixed event currently active? */
 
 	status =
 	    acpi_read_bit_register(acpi_gbl_fixed_event_info[event].
-			      status_register_id, &value);
-	if (ACPI_FAILURE(status))
+				   status_register_id, &in_byte);
+	if (ACPI_FAILURE(status)) {
 		return_ACPI_STATUS(status);
+	}
 
-	if (value)
-		*event_status |= ACPI_EVENT_FLAG_SET;
-
-	if (acpi_gbl_fixed_event_handlers[event].handler)
-		*event_status |= ACPI_EVENT_FLAG_HANDLE;
+	if (in_byte) {
+		local_event_status |= ACPI_EVENT_FLAG_SET;
+	}
 
-	return_ACPI_STATUS(status);
+	(*event_status) = local_event_status;
+	return_ACPI_STATUS(AE_OK);
 }
 
 ACPI_EXPORT_SYMBOL(acpi_get_event_status)
diff --git a/drivers/acpi/acpica/evxfgpe.c b/drivers/acpi/acpica/evxfgpe.c
index 0cf159cc6e6d..e889a5304abd 100644
--- a/drivers/acpi/acpica/evxfgpe.c
+++ b/drivers/acpi/acpica/evxfgpe.c
@@ -106,8 +106,8 @@ ACPI_EXPORT_SYMBOL(acpi_update_all_gpes)
  *
  * FUNCTION:    acpi_enable_gpe
  *
- * PARAMETERS:  gpe_device      - Parent GPE Device. NULL for GPE0/GPE1
- *              gpe_number      - GPE level within the GPE block
+ * PARAMETERS:  gpe_device          - Parent GPE Device. NULL for GPE0/GPE1
+ *              gpe_number          - GPE level within the GPE block
  *
  * RETURN:      Status
  *
@@ -115,7 +115,6 @@ ACPI_EXPORT_SYMBOL(acpi_update_all_gpes)
  *              hardware-enabled.
  *
  ******************************************************************************/
-
 acpi_status acpi_enable_gpe(acpi_handle gpe_device, u32 gpe_number)
 {
 	acpi_status status = AE_BAD_PARAMETER;
@@ -490,8 +489,8 @@ ACPI_EXPORT_SYMBOL(acpi_clear_gpe)
  *
  * FUNCTION:    acpi_get_gpe_status
  *
- * PARAMETERS:  gpe_device      - Parent GPE Device. NULL for GPE0/GPE1
- *              gpe_number      - GPE level within the GPE block
+ * PARAMETERS:  gpe_device          - Parent GPE Device. NULL for GPE0/GPE1
+ *              gpe_number          - GPE level within the GPE block
  *              event_status        - Where the current status of the event
  *                                    will be returned
  *
@@ -524,9 +523,6 @@ acpi_get_gpe_status(acpi_handle gpe_device,
 
 	status = acpi_hw_get_gpe_status(gpe_event_info, event_status);
 
-	if (gpe_event_info->flags & ACPI_GPE_DISPATCH_MASK)
-		*event_status |= ACPI_EVENT_FLAG_HANDLE;
-
 unlock_and_exit:
 	acpi_os_release_lock(acpi_gbl_gpe_lock, flags);
 	return_ACPI_STATUS(status);
@@ -596,6 +592,38 @@ acpi_status acpi_enable_all_runtime_gpes(void)
 
 ACPI_EXPORT_SYMBOL(acpi_enable_all_runtime_gpes)
 
+/******************************************************************************
+ *
+ * FUNCTION:    acpi_enable_all_wakeup_gpes
+ *
+ * PARAMETERS:  None
+ *
+ * RETURN:      Status
+ *
+ * DESCRIPTION: Enable all "wakeup" GPEs and disable all of the other GPEs, in
+ *              all GPE blocks.
+ *
+ ******************************************************************************/
+
+acpi_status acpi_enable_all_wakeup_gpes(void)
+{
+	acpi_status status;
+
+	ACPI_FUNCTION_TRACE(acpi_enable_all_wakeup_gpes);
+
+	status = acpi_ut_acquire_mutex(ACPI_MTX_EVENTS);
+	if (ACPI_FAILURE(status)) {
+		return_ACPI_STATUS(status);
+	}
+
+	status = acpi_hw_enable_all_wakeup_gpes();
+	(void)acpi_ut_release_mutex(ACPI_MTX_EVENTS);
+
+	return_ACPI_STATUS(status);
+}
+
+ACPI_EXPORT_SYMBOL(acpi_enable_all_wakeup_gpes)
+
 /*******************************************************************************
  *
  * FUNCTION:    acpi_install_gpe_block
diff --git a/drivers/acpi/acpica/exfield.c b/drivers/acpi/acpica/exfield.c
index 6907ce0c704c..b994845ed359 100644
--- a/drivers/acpi/acpica/exfield.c
+++ b/drivers/acpi/acpica/exfield.c
@@ -253,6 +253,37 @@ acpi_ex_read_data_from_field(struct acpi_walk_state * walk_state,
 		buffer = &buffer_desc->integer.value;
 	}
 
+	if ((obj_desc->common.type == ACPI_TYPE_LOCAL_REGION_FIELD) &&
+	    (obj_desc->field.region_obj->region.space_id ==
+	     ACPI_ADR_SPACE_GPIO)) {
+		/*
+		 * For GPIO (general_purpose_io), the Address will be the bit offset
+		 * from the previous Connection() operator, making it effectively a
+		 * pin number index. The bit_length is the length of the field, which
+		 * is thus the number of pins.
+		 */
+		ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
+				  "GPIO FieldRead [FROM]:  Pin %u Bits %u\n",
+				  obj_desc->field.pin_number_index,
+				  obj_desc->field.bit_length));
+
+		/* Lock entire transaction if requested */
+
+		acpi_ex_acquire_global_lock(obj_desc->common_field.field_flags);
+
+		/* Perform the write */
+
+		status = acpi_ex_access_region(obj_desc, 0,
+					       (u64 *)buffer, ACPI_READ);
+		acpi_ex_release_global_lock(obj_desc->common_field.field_flags);
+		if (ACPI_FAILURE(status)) {
+			acpi_ut_remove_reference(buffer_desc);
+		} else {
+			*ret_buffer_desc = buffer_desc;
+		}
+		return_ACPI_STATUS(status);
+	}
+
 	ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
 			  "FieldRead [TO]:   Obj %p, Type %X, Buf %p, ByteLen %X\n",
 			  obj_desc, obj_desc->common.type, buffer,
@@ -413,6 +444,42 @@ acpi_ex_write_data_to_field(union acpi_operand_object *source_desc,
 
 		*result_desc = buffer_desc;
 		return_ACPI_STATUS(status);
+	} else if ((obj_desc->common.type == ACPI_TYPE_LOCAL_REGION_FIELD) &&
+		   (obj_desc->field.region_obj->region.space_id ==
+		    ACPI_ADR_SPACE_GPIO)) {
+		/*
+		 * For GPIO (general_purpose_io), we will bypass the entire field
+		 * mechanism and handoff the bit address and bit width directly to
+		 * the handler. The Address will be the bit offset
+		 * from the previous Connection() operator, making it effectively a
+		 * pin number index. The bit_length is the length of the field, which
+		 * is thus the number of pins.
+		 */
+		if (source_desc->common.type != ACPI_TYPE_INTEGER) {
+			return_ACPI_STATUS(AE_AML_OPERAND_TYPE);
+		}
+
+		ACPI_DEBUG_PRINT((ACPI_DB_BFIELD,
+				  "GPIO FieldWrite [FROM]: (%s:%X), Val %.8X  [TO]:  Pin %u Bits %u\n",
+				  acpi_ut_get_type_name(source_desc->common.
+							type),
+				  source_desc->common.type,
+				  (u32)source_desc->integer.value,
+				  obj_desc->field.pin_number_index,
+				  obj_desc->field.bit_length));
+
+		buffer = &source_desc->integer.value;
+
+		/* Lock entire transaction if requested */
+
+		acpi_ex_acquire_global_lock(obj_desc->common_field.field_flags);
+
+		/* Perform the write */
+
+		status = acpi_ex_access_region(obj_desc, 0,
+					       (u64 *)buffer, ACPI_WRITE);
+		acpi_ex_release_global_lock(obj_desc->common_field.field_flags);
+		return_ACPI_STATUS(status);
 	}
 
 	/* Get a pointer to the data to be written */
diff --git a/drivers/acpi/acpica/exprep.c b/drivers/acpi/acpica/exprep.c
index ee3f872870bc..118e942005e5 100644
--- a/drivers/acpi/acpica/exprep.c
+++ b/drivers/acpi/acpica/exprep.c
@@ -484,6 +484,8 @@ acpi_status acpi_ex_prep_field_value(struct acpi_create_field_info *info)
 			obj_desc->field.resource_length = info->resource_length;
 		}
 
+		obj_desc->field.pin_number_index = info->pin_number_index;
+
 		/* Allow full data read from EC address space */
 
 		if ((obj_desc->field.region_obj->region.space_id ==
diff --git a/drivers/acpi/acpica/hwgpe.c b/drivers/acpi/acpica/hwgpe.c
index 2e6caabba07a..48ac7b7b59cd 100644
--- a/drivers/acpi/acpica/hwgpe.c
+++ b/drivers/acpi/acpica/hwgpe.c
@@ -202,7 +202,7 @@ acpi_status acpi_hw_clear_gpe(struct acpi_gpe_event_info * gpe_event_info)
 
 acpi_status
 acpi_hw_get_gpe_status(struct acpi_gpe_event_info * gpe_event_info,
-		       acpi_event_status * event_status)
+		       acpi_event_status *event_status)
 {
 	u32 in_byte;
 	u32 register_bit;
@@ -216,6 +216,13 @@ acpi_hw_get_gpe_status(struct acpi_gpe_event_info * gpe_event_info,
 		return (AE_BAD_PARAMETER);
 	}
 
+	/* GPE currently handled? */
+
+	if ((gpe_event_info->flags & ACPI_GPE_DISPATCH_MASK) !=
+	    ACPI_GPE_DISPATCH_NONE) {
+		local_event_status |= ACPI_EVENT_FLAG_HAS_HANDLER;
+	}
+
 	/* Get the info block for the entire GPE register */
 
 	gpe_register_info = gpe_event_info->register_info;
@@ -396,11 +403,11 @@ acpi_hw_enable_wakeup_gpe_block(struct acpi_gpe_xrupt_info *gpe_xrupt_info,
 	/* Examine each GPE Register within the block */
 
 	for (i = 0; i < gpe_block->register_count; i++) {
-		if (!gpe_block->register_info[i].enable_for_wake) {
-			continue;
-		}
 
-		/* Enable all "wake" GPEs in this register */
+		/*
+		 * Enable all "wake" GPEs in this register and disable the
+		 * remaining ones.
+		 */
 
 		status =
 		    acpi_hw_write(gpe_block->register_info[i].enable_for_wake,
diff --git a/drivers/acpi/acpica/nsprepkg.c b/drivers/acpi/acpica/nsprepkg.c
index 68f725839eb6..1b13b921dda9 100644
--- a/drivers/acpi/acpica/nsprepkg.c
+++ b/drivers/acpi/acpica/nsprepkg.c
@@ -316,6 +316,45 @@ acpi_ns_check_package(struct acpi_evaluate_info *info,
 		    acpi_ns_check_package_list(info, package, elements, count);
 		break;
 
+	case ACPI_PTYPE2_UUID_PAIR:
+
+		/* The package must contain pairs of (UUID + type) */
+
+		if (count & 1) {
+			expected_count = count + 1;
+			goto package_too_small;
+		}
+
+		while (count > 0) {
+			status = acpi_ns_check_object_type(info, elements,
+							   package->ret_info.
+							   object_type1, 0);
+			if (ACPI_FAILURE(status)) {
+				return (status);
+			}
+
+			/* Validate length of the UUID buffer */
+
+			if ((*elements)->buffer.length != 16) {
+				ACPI_WARN_PREDEFINED((AE_INFO,
+						      info->full_pathname,
+						      info->node_flags,
+						      "Invalid length for UUID Buffer"));
+				return (AE_AML_OPERAND_VALUE);
+			}
+
+			status = acpi_ns_check_object_type(info, elements + 1,
+							   package->ret_info.
+							   object_type2, 0);
+			if (ACPI_FAILURE(status)) {
+				return (status);
+			}
+
+			elements += 2;
+			count -= 2;
+		}
+		break;
+
 	default:
 
 		/* Should not get here if predefined info table is correct */
diff --git a/drivers/acpi/acpica/tbxfroot.c b/drivers/acpi/acpica/tbxfroot.c
index 65ab8fed3d5e..43a54af2b548 100644
--- a/drivers/acpi/acpica/tbxfroot.c
+++ b/drivers/acpi/acpica/tbxfroot.c
@@ -50,6 +50,36 @@ ACPI_MODULE_NAME("tbxfroot")
 
 /*******************************************************************************
  *
+ * FUNCTION:    acpi_tb_get_rsdp_length
+ *
+ * PARAMETERS:  rsdp                - Pointer to RSDP
+ *
+ * RETURN:      Table length
+ *
+ * DESCRIPTION: Get the length of the RSDP
+ *
+ ******************************************************************************/
+u32 acpi_tb_get_rsdp_length(struct acpi_table_rsdp *rsdp)
+{
+
+	if (!ACPI_VALIDATE_RSDP_SIG(rsdp->signature)) {
+
+		/* BAD Signature */
+
+		return (0);
+	}
+
+	/* "Length" field is available if table version >= 2 */
+
+	if (rsdp->revision >= 2) {
+		return (rsdp->length);
+	} else {
+		return (ACPI_RSDP_CHECKSUM_LENGTH);
+	}
+}
+
+/*******************************************************************************
+ *
  * FUNCTION:    acpi_tb_validate_rsdp
  *
  * PARAMETERS:  rsdp                - Pointer to unvalidated RSDP
@@ -59,7 +89,8 @@ ACPI_MODULE_NAME("tbxfroot")
  * DESCRIPTION: Validate the RSDP (ptr)
  *
  ******************************************************************************/
-acpi_status acpi_tb_validate_rsdp(struct acpi_table_rsdp *rsdp)
+
+acpi_status acpi_tb_validate_rsdp(struct acpi_table_rsdp * rsdp)
 {
 
 	/*
diff --git a/drivers/acpi/acpica/utresrc.c b/drivers/acpi/acpica/utresrc.c
index 14cb6c0c8be2..5cd017c7ac0e 100644
--- a/drivers/acpi/acpica/utresrc.c
+++ b/drivers/acpi/acpica/utresrc.c
@@ -87,7 +87,9 @@ const char *acpi_gbl_io_decode[] = {
 
 const char *acpi_gbl_ll_decode[] = {
 	"ActiveHigh",
-	"ActiveLow"
+	"ActiveLow",
+	"ActiveBoth",
+	"Reserved"
 };
 
 const char *acpi_gbl_max_decode[] = {
diff --git a/drivers/acpi/battery.c b/drivers/acpi/battery.c
index 1c162e7be045..8ec8a89a20ab 100644
--- a/drivers/acpi/battery.c
+++ b/drivers/acpi/battery.c
@@ -534,20 +534,6 @@ static int acpi_battery_get_state(struct acpi_battery *battery)
 			" invalid.\n");
 	}
 
-	/*
-	 * When fully charged, some batteries wrongly report
-	 * capacity_now = design_capacity instead of = full_charge_capacity
-	 */
-	if (battery->capacity_now > battery->full_charge_capacity
-	    && battery->full_charge_capacity != ACPI_BATTERY_VALUE_UNKNOWN) {
-		if (battery->capacity_now != battery->design_capacity)
-			printk_once(KERN_WARNING FW_BUG
-				"battery: reported current charge level (%d) "
-				"is higher than reported maximum charge level (%d).\n",
-				battery->capacity_now, battery->full_charge_capacity);
-		battery->capacity_now = battery->full_charge_capacity;
-	}
-
 	if (test_bit(ACPI_BATTERY_QUIRK_PERCENTAGE_CAPACITY, &battery->flags)
 	    && battery->capacity_now >= 0 && battery->capacity_now <= 100)
 		battery->capacity_now = (battery->capacity_now *
@@ -709,7 +695,7 @@ static void acpi_battery_quirks(struct acpi_battery *battery)
 	if (battery->power_unit && dmi_name_in_vendors("LENOVO")) {
 		const char *s;
 		s = dmi_get_system_info(DMI_PRODUCT_VERSION);
-		if (s && !strnicmp(s, "ThinkPad", 8)) {
+		if (s && !strncasecmp(s, "ThinkPad", 8)) {
 			dmi_walk(find_battery, battery);
 			if (test_bit(ACPI_BATTERY_QUIRK_THINKPAD_MAH,
 				     &battery->flags) &&
diff --git a/drivers/acpi/blacklist.c b/drivers/acpi/blacklist.c
index 36eb42e3b0bb..ed122e17636e 100644
--- a/drivers/acpi/blacklist.c
+++ b/drivers/acpi/blacklist.c
@@ -247,8 +247,8 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 	},
 
 	/*
-	 * These machines will power on immediately after shutdown when
-	 * reporting the Windows 2012 OSI.
+	 * The wireless hotkey does not work on those machines when
+	 * returning true for _OSI("Windows 2012")
 	 */
 	{
 	.callback = dmi_disable_osi_win8,
@@ -258,6 +258,38 @@ static struct dmi_system_id acpi_osi_dmi_table[] __initdata = {
 		    DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 7737"),
 		},
 	},
+	{
+	.callback = dmi_disable_osi_win8,
+	.ident = "Dell Inspiron 7537",
+	.matches = {
+		    DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		    DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 7537"),
+		},
+	},
+	{
+	.callback = dmi_disable_osi_win8,
+	.ident = "Dell Inspiron 5437",
+	.matches = {
+		    DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		    DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 5437"),
+		},
+	},
+	{
+	.callback = dmi_disable_osi_win8,
+	.ident = "Dell Inspiron 3437",
+	.matches = {
+		    DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		    DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 3437"),
+		},
+	},
+	{
+	.callback = dmi_disable_osi_win8,
+	.ident = "Dell Vostro 3446",
+	.matches = {
+		    DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+		    DMI_MATCH(DMI_PRODUCT_NAME, "Vostro 3446"),
+		},
+	},
 
 	/*
 	 * BIOS invocation of _OSI(Linux) is almost always a BIOS bug.
diff --git a/drivers/acpi/bus.c b/drivers/acpi/bus.c
index 8581f5b84f48..8b67bd0f6bb5 100644
--- a/drivers/acpi/bus.c
+++ b/drivers/acpi/bus.c
@@ -177,16 +177,6 @@ void acpi_bus_detach_private_data(acpi_handle handle)
 }
 EXPORT_SYMBOL_GPL(acpi_bus_detach_private_data);
 
-void acpi_bus_no_hotplug(acpi_handle handle)
-{
-	struct acpi_device *adev = NULL;
-
-	acpi_bus_get_device(handle, &adev);
-	if (adev)
-		adev->flags.no_hotplug = true;
-}
-EXPORT_SYMBOL_GPL(acpi_bus_no_hotplug);
-
 static void acpi_print_osc_error(acpi_handle handle,
 	struct acpi_osc_context *context, char *error)
 {
diff --git a/drivers/acpi/container.c b/drivers/acpi/container.c
index 76f7cff64594..c8ead9f97375 100644
--- a/drivers/acpi/container.c
+++ b/drivers/acpi/container.c
@@ -99,6 +99,13 @@ static void container_device_detach(struct acpi_device *adev)
 		device_unregister(dev);
 }
 
+static void container_device_online(struct acpi_device *adev)
+{
+	struct device *dev = acpi_driver_data(adev);
+
+	kobject_uevent(&dev->kobj, KOBJ_ONLINE);
+}
+
 static struct acpi_scan_handler container_handler = {
 	.ids = container_device_ids,
 	.attach = container_device_attach,
@@ -106,6 +113,7 @@ static struct acpi_scan_handler container_handler = {
 	.hotplug = {
 		.enabled = true,
 		.demand_offline = true,
+		.notify_online = container_device_online,
 	},
 };
 
diff --git a/drivers/acpi/device_pm.c b/drivers/acpi/device_pm.c
index 67075f800e34..143ec6ea1468 100644
--- a/drivers/acpi/device_pm.c
+++ b/drivers/acpi/device_pm.c
@@ -343,6 +343,7 @@ int acpi_device_update_power(struct acpi_device *device, int *state_p)
 
 	return 0;
 }
+EXPORT_SYMBOL_GPL(acpi_device_update_power);
 
 int acpi_bus_update_power(acpi_handle handle, int *state_p)
 {
@@ -710,7 +711,7 @@ int acpi_pm_device_run_wake(struct device *phys_dev, bool enable)
 		return -ENODEV;
 	}
 
-	return acpi_device_wakeup(adev, enable, ACPI_STATE_S0);
+	return acpi_device_wakeup(adev, ACPI_STATE_S0, enable);
 }
 EXPORT_SYMBOL(acpi_pm_device_run_wake);
 #endif /* CONFIG_PM_RUNTIME */
@@ -1041,6 +1042,40 @@ static struct dev_pm_domain acpi_general_pm_domain = {
 };
 
 /**
+ * acpi_dev_pm_detach - Remove ACPI power management from the device.
+ * @dev: Device to take care of.
+ * @power_off: Whether or not to try to remove power from the device.
+ *
+ * Remove the device from the general ACPI PM domain and remove its wakeup
+ * notifier.  If @power_off is set, additionally remove power from the device if
+ * possible.
+ *
+ * Callers must ensure proper synchronization of this function with power
+ * management callbacks.
+ */
+static void acpi_dev_pm_detach(struct device *dev, bool power_off)
+{
+	struct acpi_device *adev = ACPI_COMPANION(dev);
+
+	if (adev && dev->pm_domain == &acpi_general_pm_domain) {
+		dev->pm_domain = NULL;
+		acpi_remove_pm_notifier(adev);
+		if (power_off) {
+			/*
+			 * If the device's PM QoS resume latency limit or flags
+			 * have been exposed to user space, they have to be
+			 * hidden at this point, so that they don't affect the
+			 * choice of the low-power state to put the device into.
+			 */
+			dev_pm_qos_hide_latency_limit(dev);
+			dev_pm_qos_hide_flags(dev);
+			acpi_device_wakeup(adev, ACPI_STATE_S0, false);
+			acpi_dev_pm_low_power(dev, adev, ACPI_STATE_S0);
+		}
+	}
+}
+
+/**
  * acpi_dev_pm_attach - Prepare device for ACPI power management.
  * @dev: Device to prepare.
  * @power_on: Whether or not to power on the device.
@@ -1072,42 +1107,9 @@ int acpi_dev_pm_attach(struct device *dev, bool power_on)
 		acpi_dev_pm_full_power(adev);
 		acpi_device_wakeup(adev, ACPI_STATE_S0, false);
 	}
+
+	dev->pm_domain->detach = acpi_dev_pm_detach;
 	return 0;
 }
 EXPORT_SYMBOL_GPL(acpi_dev_pm_attach);
-
-/**
- * acpi_dev_pm_detach - Remove ACPI power management from the device.
- * @dev: Device to take care of.
- * @power_off: Whether or not to try to remove power from the device.
- *
- * Remove the device from the general ACPI PM domain and remove its wakeup
- * notifier.  If @power_off is set, additionally remove power from the device if
- * possible.
- *
- * Callers must ensure proper synchronization of this function with power
- * management callbacks.
- */
-void acpi_dev_pm_detach(struct device *dev, bool power_off)
-{
-	struct acpi_device *adev = ACPI_COMPANION(dev);
-
-	if (adev && dev->pm_domain == &acpi_general_pm_domain) {
-		dev->pm_domain = NULL;
-		acpi_remove_pm_notifier(adev);
-		if (power_off) {
-			/*
-			 * If the device's PM QoS resume latency limit or flags
-			 * have been exposed to user space, they have to be
-			 * hidden at this point, so that they don't affect the
-			 * choice of the low-power state to put the device into.
-			 */
-			dev_pm_qos_hide_latency_limit(dev);
-			dev_pm_qos_hide_flags(dev);
-			acpi_device_wakeup(adev, ACPI_STATE_S0, false);
-			acpi_dev_pm_low_power(dev, adev, ACPI_STATE_S0);
-		}
-	}
-}
-EXPORT_SYMBOL_GPL(acpi_dev_pm_detach);
 #endif /* CONFIG_PM */
diff --git a/drivers/acpi/ec.c b/drivers/acpi/ec.c
index a66ab658abbc..3d304ff7f095 100644
--- a/drivers/acpi/ec.c
+++ b/drivers/acpi/ec.c
@@ -128,12 +128,13 @@ static int EC_FLAGS_SKIP_DSDT_SCAN; /* Not all BIOS survive early DSDT scan */
 static int EC_FLAGS_CLEAR_ON_RESUME; /* Needs acpi_ec_clear() on boot/resume */
 
 /* --------------------------------------------------------------------------
-                             Transaction Management
-   -------------------------------------------------------------------------- */
+ *                           Transaction Management
+ * -------------------------------------------------------------------------- */
 
 static inline u8 acpi_ec_read_status(struct acpi_ec *ec)
 {
 	u8 x = inb(ec->command_addr);
+
 	pr_debug("EC_SC(R) = 0x%2.2x "
 		 "SCI_EVT=%d BURST=%d CMD=%d IBF=%d OBF=%d\n",
 		 x,
@@ -148,6 +149,7 @@ static inline u8 acpi_ec_read_status(struct acpi_ec *ec)
 static inline u8 acpi_ec_read_data(struct acpi_ec *ec)
 {
 	u8 x = inb(ec->data_addr);
+
 	pr_debug("EC_DATA(R) = 0x%2.2x\n", x);
 	return x;
 }
@@ -164,10 +166,32 @@ static inline void acpi_ec_write_data(struct acpi_ec *ec, u8 data)
 	outb(data, ec->data_addr);
 }
 
+#ifdef DEBUG
+static const char *acpi_ec_cmd_string(u8 cmd)
+{
+	switch (cmd) {
+	case 0x80:
+		return "RD_EC";
+	case 0x81:
+		return "WR_EC";
+	case 0x82:
+		return "BE_EC";
+	case 0x83:
+		return "BD_EC";
+	case 0x84:
+		return "QR_EC";
+	}
+	return "UNKNOWN";
+}
+#else
+#define acpi_ec_cmd_string(cmd)		"UNDEF"
+#endif
+
 static int ec_transaction_completed(struct acpi_ec *ec)
 {
 	unsigned long flags;
 	int ret = 0;
+
 	spin_lock_irqsave(&ec->lock, flags);
 	if (ec->curr && (ec->curr->flags & ACPI_EC_COMMAND_COMPLETE))
 		ret = 1;
@@ -181,7 +205,8 @@ static bool advance_transaction(struct acpi_ec *ec)
 	u8 status;
 	bool wakeup = false;
 
-	pr_debug("===== %s =====\n", in_interrupt() ? "IRQ" : "TASK");
+	pr_debug("===== %s (%d) =====\n",
+		 in_interrupt() ? "IRQ" : "TASK", smp_processor_id());
 	status = acpi_ec_read_status(ec);
 	t = ec->curr;
 	if (!t)
@@ -197,6 +222,9 @@ static bool advance_transaction(struct acpi_ec *ec)
 				t->rdata[t->ri++] = acpi_ec_read_data(ec);
 				if (t->rlen == t->ri) {
 					t->flags |= ACPI_EC_COMMAND_COMPLETE;
+					if (t->command == ACPI_EC_COMMAND_QUERY)
+						pr_debug("***** Command(%s) hardware completion *****\n",
+							 acpi_ec_cmd_string(t->command));
 					wakeup = true;
 				}
 			} else
@@ -208,7 +236,21 @@ static bool advance_transaction(struct acpi_ec *ec)
 		}
 		return wakeup;
 	} else {
-		if ((status & ACPI_EC_FLAG_IBF) == 0) {
+		/*
+		 * There is firmware refusing to respond QR_EC when SCI_EVT
+		 * is not set, for which case, we complete the QR_EC
+		 * without issuing it to the firmware.
+		 * https://bugzilla.kernel.org/show_bug.cgi?id=86211
+		 */
+		if (!(status & ACPI_EC_FLAG_SCI) &&
+		    (t->command == ACPI_EC_COMMAND_QUERY)) {
+			t->flags |= ACPI_EC_COMMAND_POLL;
+			t->rdata[t->ri++] = 0x00;
+			t->flags |= ACPI_EC_COMMAND_COMPLETE;
+			pr_debug("***** Command(%s) software completion *****\n",
+				 acpi_ec_cmd_string(t->command));
+			wakeup = true;
+		} else if ((status & ACPI_EC_FLAG_IBF) == 0) {
 			acpi_ec_write_cmd(ec, t->command);
 			t->flags |= ACPI_EC_COMMAND_POLL;
 		} else
@@ -249,6 +291,7 @@ static int ec_poll(struct acpi_ec *ec)
 {
 	unsigned long flags;
 	int repeat = 5; /* number of command restarts */
+
 	while (repeat--) {
 		unsigned long delay = jiffies +
 			msecs_to_jiffies(ec_delay);
@@ -281,18 +324,25 @@ static int acpi_ec_transaction_unlocked(struct acpi_ec *ec,
 {
 	unsigned long tmp;
 	int ret = 0;
+
 	if (EC_FLAGS_MSI)
 		udelay(ACPI_EC_MSI_UDELAY);
 	/* start transaction */
 	spin_lock_irqsave(&ec->lock, tmp);
 	/* following two actions should be kept atomic */
 	ec->curr = t;
+	pr_debug("***** Command(%s) started *****\n",
+		 acpi_ec_cmd_string(t->command));
 	start_transaction(ec);
-	if (ec->curr->command == ACPI_EC_COMMAND_QUERY)
-		clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags);
 	spin_unlock_irqrestore(&ec->lock, tmp);
 	ret = ec_poll(ec);
 	spin_lock_irqsave(&ec->lock, tmp);
+	if (ec->curr->command == ACPI_EC_COMMAND_QUERY) {
+		clear_bit(EC_FLAGS_QUERY_PENDING, &ec->flags);
+		pr_debug("***** Event stopped *****\n");
+	}
+	pr_debug("***** Command(%s) stopped *****\n",
+		 acpi_ec_cmd_string(t->command));
 	ec->curr = NULL;
 	spin_unlock_irqrestore(&ec->lock, tmp);
 	return ret;
@@ -302,6 +352,7 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
 {
 	int status;
 	u32 glk;
+
 	if (!ec || (!t) || (t->wlen && !t->wdata) || (t->rlen && !t->rdata))
 		return -EINVAL;
 	if (t->rdata)
@@ -318,8 +369,6 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
 			goto unlock;
 		}
 	}
-	pr_debug("transaction start (cmd=0x%02x, addr=0x%02x)\n",
-			t->command, t->wdata ? t->wdata[0] : 0);
 	/* disable GPE during transaction if storm is detected */
 	if (test_bit(EC_FLAGS_GPE_STORM, &ec->flags)) {
 		/* It has to be disabled, so that it doesn't trigger. */
@@ -340,7 +389,6 @@ static int acpi_ec_transaction(struct acpi_ec *ec, struct transaction *t)
 			t->irq_count);
 		set_bit(EC_FLAGS_GPE_STORM, &ec->flags);
 	}
-	pr_debug("transaction end\n");
 	if (ec->global_lock)
 		acpi_release_global_lock(glk);
 unlock:
@@ -368,7 +416,7 @@ static int acpi_ec_burst_disable(struct acpi_ec *ec)
 				acpi_ec_transaction(ec, &t) : 0;
 }
 
-static int acpi_ec_read(struct acpi_ec *ec, u8 address, u8 * data)
+static int acpi_ec_read(struct acpi_ec *ec, u8 address, u8 *data)
 {
 	int result;
 	u8 d;
@@ -404,10 +452,9 @@ int ec_read(u8 addr, u8 *val)
 	if (!err) {
 		*val = temp_data;
 		return 0;
-	} else
-		return err;
+	}
+	return err;
 }
-
 EXPORT_SYMBOL(ec_read);
 
 int ec_write(u8 addr, u8 val)
@@ -421,22 +468,21 @@ int ec_write(u8 addr, u8 val)
 
 	return err;
 }
-
 EXPORT_SYMBOL(ec_write);
 
 int ec_transaction(u8 command,
-		   const u8 * wdata, unsigned wdata_len,
-		   u8 * rdata, unsigned rdata_len)
+		   const u8 *wdata, unsigned wdata_len,
+		   u8 *rdata, unsigned rdata_len)
 {
 	struct transaction t = {.command = command,
 				.wdata = wdata, .rdata = rdata,
 				.wlen = wdata_len, .rlen = rdata_len};
+
 	if (!first_ec)
 		return -ENODEV;
 
 	return acpi_ec_transaction(first_ec, &t);
 }
-
 EXPORT_SYMBOL(ec_transaction);
 
 /* Get the handle to the EC device */
@@ -446,7 +492,6 @@ acpi_handle ec_get_handle(void)
 		return NULL;
 	return first_ec->handle;
 }
-
 EXPORT_SYMBOL(ec_get_handle);
 
 /*
@@ -510,13 +555,14 @@ void acpi_ec_unblock_transactions_early(void)
 		clear_bit(EC_FLAGS_BLOCKED, &first_ec->flags);
 }
 
-static int acpi_ec_query_unlocked(struct acpi_ec *ec, u8 * data)
+static int acpi_ec_query_unlocked(struct acpi_ec *ec, u8 *data)
 {
 	int result;
 	u8 d;
 	struct transaction t = {.command = ACPI_EC_COMMAND_QUERY,
 				.wdata = NULL, .rdata = &d,
 				.wlen = 0, .rlen = 1};
+
 	if (!ec || !data)
 		return -EINVAL;
 	/*
@@ -542,6 +588,7 @@ int acpi_ec_add_query_handler(struct acpi_ec *ec, u8 query_bit,
 {
 	struct acpi_ec_query_handler *handler =
 	    kzalloc(sizeof(struct acpi_ec_query_handler), GFP_KERNEL);
+
 	if (!handler)
 		return -ENOMEM;
 
@@ -554,12 +601,12 @@ int acpi_ec_add_query_handler(struct acpi_ec *ec, u8 query_bit,
 	mutex_unlock(&ec->mutex);
 	return 0;
 }
-
 EXPORT_SYMBOL_GPL(acpi_ec_add_query_handler);
 
 void acpi_ec_remove_query_handler(struct acpi_ec *ec, u8 query_bit)
 {
 	struct acpi_ec_query_handler *handler, *tmp;
+
 	mutex_lock(&ec->mutex);
 	list_for_each_entry_safe(handler, tmp, &ec->list, node) {
 		if (query_bit == handler->query_bit) {
@@ -569,20 +616,20 @@ void acpi_ec_remove_query_handler(struct acpi_ec *ec, u8 query_bit)
 	}
 	mutex_unlock(&ec->mutex);
 }
-
 EXPORT_SYMBOL_GPL(acpi_ec_remove_query_handler);
 
 static void acpi_ec_run(void *cxt)
 {
 	struct acpi_ec_query_handler *handler = cxt;
+
 	if (!handler)
 		return;
-	pr_debug("start query execution\n");
+	pr_debug("##### Query(0x%02x) started #####\n", handler->query_bit);
 	if (handler->func)
 		handler->func(handler->data);
 	else if (handler->handle)
 		acpi_evaluate_object(handler->handle, NULL, NULL, NULL);
-	pr_debug("stop query execution\n");
+	pr_debug("##### Query(0x%02x) stopped #####\n", handler->query_bit);
 	kfree(handler);
 }
 
@@ -605,8 +652,8 @@ static int acpi_ec_sync_query(struct acpi_ec *ec, u8 *data)
 			if (!copy)
 				return -ENOMEM;
 			memcpy(copy, handler, sizeof(*copy));
-			pr_debug("push query execution (0x%2x) on queue\n",
-				value);
+			pr_debug("##### Query(0x%02x) scheduled #####\n",
+				 handler->query_bit);
 			return acpi_os_execute((copy->func) ?
 				OSL_NOTIFY_HANDLER : OSL_GPE_HANDLER,
 				acpi_ec_run, copy);
@@ -618,6 +665,7 @@ static int acpi_ec_sync_query(struct acpi_ec *ec, u8 *data)
 static void acpi_ec_gpe_query(void *ec_cxt)
 {
 	struct acpi_ec *ec = ec_cxt;
+
 	if (!ec)
 		return;
 	mutex_lock(&ec->mutex);
@@ -629,7 +677,7 @@ static int ec_check_sci(struct acpi_ec *ec, u8 state)
 {
 	if (state & ACPI_EC_FLAG_SCI) {
 		if (!test_and_set_bit(EC_FLAGS_QUERY_PENDING, &ec->flags)) {
-			pr_debug("push gpe query to the queue\n");
+			pr_debug("***** Event started *****\n");
 			return acpi_os_execute(OSL_NOTIFY_HANDLER,
 				acpi_ec_gpe_query, ec);
 		}
@@ -652,8 +700,8 @@ static u32 acpi_ec_gpe_handler(acpi_handle gpe_device,
 }
 
 /* --------------------------------------------------------------------------
-                             Address Space Management
-   -------------------------------------------------------------------------- */
+ *                           Address Space Management
+ * -------------------------------------------------------------------------- */
 
 static acpi_status
 acpi_ec_space_handler(u32 function, acpi_physical_address address,
@@ -684,27 +732,26 @@ acpi_ec_space_handler(u32 function, acpi_physical_address address,
 	switch (result) {
 	case -EINVAL:
 		return AE_BAD_PARAMETER;
-		break;
 	case -ENODEV:
 		return AE_NOT_FOUND;
-		break;
 	case -ETIME:
 		return AE_TIME;
-		break;
 	default:
 		return AE_OK;
 	}
 }
 
 /* --------------------------------------------------------------------------
-                               Driver Interface
-   -------------------------------------------------------------------------- */
+ *                             Driver Interface
+ * -------------------------------------------------------------------------- */
+
 static acpi_status
 ec_parse_io_ports(struct acpi_resource *resource, void *context);
 
 static struct acpi_ec *make_acpi_ec(void)
 {
 	struct acpi_ec *ec = kzalloc(sizeof(struct acpi_ec), GFP_KERNEL);
+
 	if (!ec)
 		return NULL;
 	ec->flags = 1 << EC_FLAGS_QUERY_PENDING;
@@ -727,9 +774,8 @@ acpi_ec_register_query_methods(acpi_handle handle, u32 level,
 
 	status = acpi_get_name(handle, ACPI_SINGLE_NAME, &buffer);
 
-	if (ACPI_SUCCESS(status) && sscanf(node_name, "_Q%x", &value) == 1) {
+	if (ACPI_SUCCESS(status) && sscanf(node_name, "_Q%x", &value) == 1)
 		acpi_ec_add_query_handler(ec, value, handle, NULL, NULL);
-	}
 	return AE_OK;
 }
 
@@ -738,7 +784,6 @@ ec_parse_device(acpi_handle handle, u32 Level, void *context, void **retval)
 {
 	acpi_status status;
 	unsigned long long tmp = 0;
-
 	struct acpi_ec *ec = context;
 
 	/* clear addr values, ec_parse_io_ports depend on it */
@@ -766,6 +811,7 @@ ec_parse_device(acpi_handle handle, u32 Level, void *context, void **retval)
 static int ec_install_handlers(struct acpi_ec *ec)
 {
 	acpi_status status;
+
 	if (test_bit(EC_FLAGS_HANDLERS_INSTALLED, &ec->flags))
 		return 0;
 	status = acpi_install_gpe_handler(NULL, ec->gpe,
@@ -1015,6 +1061,10 @@ static struct dmi_system_id ec_dmi_table[] __initdata = {
 	DMI_MATCH(DMI_SYS_VENDOR, "Quanta"),
 	DMI_MATCH(DMI_PRODUCT_NAME, "TW9/SW9"),}, NULL},
 	{
+	ec_flag_msi, "Clevo W350etq", {
+	DMI_MATCH(DMI_SYS_VENDOR, "CLEVO CO."),
+	DMI_MATCH(DMI_PRODUCT_NAME, "W35_37ET"),}, NULL},
+	{
 	ec_validate_ecdt, "ASUS hardware", {
 	DMI_MATCH(DMI_BIOS_VENDOR, "ASUS") }, NULL},
 	{
@@ -1059,7 +1109,8 @@ int __init acpi_ec_ecdt_probe(void)
 		boot_ec->data_addr = ecdt_ptr->data.address;
 		boot_ec->gpe = ecdt_ptr->gpe;
 		boot_ec->handle = ACPI_ROOT_OBJECT;
-		acpi_get_handle(ACPI_ROOT_OBJECT, ecdt_ptr->id, &boot_ec->handle);
+		acpi_get_handle(ACPI_ROOT_OBJECT, ecdt_ptr->id,
+				&boot_ec->handle);
 		/* Don't trust ECDT, which comes from ASUSTek */
 		if (!EC_FLAGS_VALIDATE_ECDT)
 			goto install;
@@ -1143,6 +1194,5 @@ static void __exit acpi_ec_exit(void)
 {
 
 	acpi_bus_unregister_driver(&acpi_ec_driver);
-	return;
 }
 #endif	/* 0 */
diff --git a/drivers/acpi/fan.c b/drivers/acpi/fan.c
index 8acf53e62966..caf9b76b7ef8 100644
--- a/drivers/acpi/fan.c
+++ b/drivers/acpi/fan.c
@@ -27,27 +27,22 @@
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/types.h>
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 #include <linux/thermal.h>
 #include <linux/acpi.h>
-
-#define PREFIX "ACPI: "
-
-#define ACPI_FAN_CLASS			"fan"
-#define ACPI_FAN_FILE_STATE		"state"
-
-#define _COMPONENT		ACPI_FAN_COMPONENT
-ACPI_MODULE_NAME("fan");
+#include <linux/platform_device.h>
+#include <linux/sort.h>
 
 MODULE_AUTHOR("Paul Diefenbaugh");
 MODULE_DESCRIPTION("ACPI Fan Driver");
 MODULE_LICENSE("GPL");
 
-static int acpi_fan_add(struct acpi_device *device);
-static int acpi_fan_remove(struct acpi_device *device);
+static int acpi_fan_probe(struct platform_device *pdev);
+static int acpi_fan_remove(struct platform_device *pdev);
 
 static const struct acpi_device_id fan_device_ids[] = {
 	{"PNP0C0B", 0},
+	{"INT3404", 0},
 	{"", 0},
 };
 MODULE_DEVICE_TABLE(acpi, fan_device_ids);
@@ -66,37 +61,100 @@ static struct dev_pm_ops acpi_fan_pm = {
 #define FAN_PM_OPS_PTR NULL
 #endif
 
-static struct acpi_driver acpi_fan_driver = {
-	.name = "fan",
-	.class = ACPI_FAN_CLASS,
-	.ids = fan_device_ids,
-	.ops = {
-		.add = acpi_fan_add,
-		.remove = acpi_fan_remove,
-		},
-	.drv.pm = FAN_PM_OPS_PTR,
+struct acpi_fan_fps {
+	u64 control;
+	u64 trip_point;
+	u64 speed;
+	u64 noise_level;
+	u64 power;
+};
+
+struct acpi_fan_fif {
+	u64 revision;
+	u64 fine_grain_ctrl;
+	u64 step_size;
+	u64 low_speed_notification;
+};
+
+struct acpi_fan {
+	bool acpi4;
+	struct acpi_fan_fif fif;
+	struct acpi_fan_fps *fps;
+	int fps_count;
+	struct thermal_cooling_device *cdev;
+};
+
+static struct platform_driver acpi_fan_driver = {
+	.probe = acpi_fan_probe,
+	.remove = acpi_fan_remove,
+	.driver = {
+		.name = "acpi-fan",
+		.acpi_match_table = fan_device_ids,
+		.pm = FAN_PM_OPS_PTR,
+	},
 };
 
 /* thermal cooling device callbacks */
 static int fan_get_max_state(struct thermal_cooling_device *cdev, unsigned long
 			     *state)
 {
-	/* ACPI fan device only support two states: ON/OFF */
-	*state = 1;
+	struct acpi_device *device = cdev->devdata;
+	struct acpi_fan *fan = acpi_driver_data(device);
+
+	if (fan->acpi4)
+		*state = fan->fps_count - 1;
+	else
+		*state = 1;
 	return 0;
 }
 
-static int fan_get_cur_state(struct thermal_cooling_device *cdev, unsigned long
-			     *state)
+static int fan_get_state_acpi4(struct acpi_device *device, unsigned long *state)
+{
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	struct acpi_fan *fan = acpi_driver_data(device);
+	union acpi_object *obj;
+	acpi_status status;
+	int control, i;
+
+	status = acpi_evaluate_object(device->handle, "_FST", NULL, &buffer);
+	if (ACPI_FAILURE(status)) {
+		dev_err(&device->dev, "Get fan state failed\n");
+		return status;
+	}
+
+	obj = buffer.pointer;
+	if (!obj || obj->type != ACPI_TYPE_PACKAGE ||
+	    obj->package.count != 3 ||
+	    obj->package.elements[1].type != ACPI_TYPE_INTEGER) {
+		dev_err(&device->dev, "Invalid _FST data\n");
+		status = -EINVAL;
+		goto err;
+	}
+
+	control = obj->package.elements[1].integer.value;
+	for (i = 0; i < fan->fps_count; i++) {
+		if (control == fan->fps[i].control)
+			break;
+	}
+	if (i == fan->fps_count) {
+		dev_dbg(&device->dev, "Invalid control value returned\n");
+		status = -EINVAL;
+		goto err;
+	}
+
+	*state = i;
+
+err:
+	kfree(obj);
+	return status;
+}
+
+static int fan_get_state(struct acpi_device *device, unsigned long *state)
 {
-	struct acpi_device *device = cdev->devdata;
 	int result;
 	int acpi_state = ACPI_STATE_D0;
 
-	if (!device)
-		return -EINVAL;
-
-	result = acpi_bus_update_power(device->handle, &acpi_state);
+	result = acpi_device_update_power(device, &acpi_state);
 	if (result)
 		return result;
 
@@ -105,21 +163,57 @@ static int fan_get_cur_state(struct thermal_cooling_device *cdev, unsigned long
 	return 0;
 }
 
-static int
-fan_set_cur_state(struct thermal_cooling_device *cdev, unsigned long state)
+static int fan_get_cur_state(struct thermal_cooling_device *cdev, unsigned long
+			     *state)
 {
 	struct acpi_device *device = cdev->devdata;
-	int result;
+	struct acpi_fan *fan = acpi_driver_data(device);
 
-	if (!device || (state != 0 && state != 1))
+	if (fan->acpi4)
+		return fan_get_state_acpi4(device, state);
+	else
+		return fan_get_state(device, state);
+}
+
+static int fan_set_state(struct acpi_device *device, unsigned long state)
+{
+	if (state != 0 && state != 1)
 		return -EINVAL;
 
-	result = acpi_bus_set_power(device->handle,
-				state ? ACPI_STATE_D0 : ACPI_STATE_D3_COLD);
+	return acpi_device_set_power(device,
+				     state ? ACPI_STATE_D0 : ACPI_STATE_D3_COLD);
+}
 
-	return result;
+static int fan_set_state_acpi4(struct acpi_device *device, unsigned long state)
+{
+	struct acpi_fan *fan = acpi_driver_data(device);
+	acpi_status status;
+
+	if (state >= fan->fps_count)
+		return -EINVAL;
+
+	status = acpi_execute_simple_method(device->handle, "_FSL",
+					    fan->fps[state].control);
+	if (ACPI_FAILURE(status)) {
+		dev_dbg(&device->dev, "Failed to set state by _FSL\n");
+		return status;
+	}
+
+	return 0;
 }
 
+static int
+fan_set_cur_state(struct thermal_cooling_device *cdev, unsigned long state)
+{
+	struct acpi_device *device = cdev->devdata;
+	struct acpi_fan *fan = acpi_driver_data(device);
+
+	if (fan->acpi4)
+		return fan_set_state_acpi4(device, state);
+	else
+		return fan_set_state(device, state);
+ }
+
 static const struct thermal_cooling_device_ops fan_cooling_ops = {
 	.get_max_state = fan_get_max_state,
 	.get_cur_state = fan_get_cur_state,
@@ -127,24 +221,129 @@ static const struct thermal_cooling_device_ops fan_cooling_ops = {
 };
 
 /* --------------------------------------------------------------------------
-                                 Driver Interface
-   -------------------------------------------------------------------------- */
+ *                               Driver Interface
+ * --------------------------------------------------------------------------
+*/
 
-static int acpi_fan_add(struct acpi_device *device)
+static bool acpi_fan_is_acpi4(struct acpi_device *device)
 {
-	int result = 0;
-	struct thermal_cooling_device *cdev;
+	return acpi_has_method(device->handle, "_FIF") &&
+	       acpi_has_method(device->handle, "_FPS") &&
+	       acpi_has_method(device->handle, "_FSL") &&
+	       acpi_has_method(device->handle, "_FST");
+}
 
-	if (!device)
-		return -EINVAL;
+static int acpi_fan_get_fif(struct acpi_device *device)
+{
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	struct acpi_fan *fan = acpi_driver_data(device);
+	struct acpi_buffer format = { sizeof("NNNN"), "NNNN" };
+	struct acpi_buffer fif = { sizeof(fan->fif), &fan->fif };
+	union acpi_object *obj;
+	acpi_status status;
+
+	status = acpi_evaluate_object(device->handle, "_FIF", NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		return status;
+
+	obj = buffer.pointer;
+	if (!obj || obj->type != ACPI_TYPE_PACKAGE) {
+		dev_err(&device->dev, "Invalid _FIF data\n");
+		status = -EINVAL;
+		goto err;
+	}
 
-	strcpy(acpi_device_name(device), "Fan");
-	strcpy(acpi_device_class(device), ACPI_FAN_CLASS);
+	status = acpi_extract_package(obj, &format, &fif);
+	if (ACPI_FAILURE(status)) {
+		dev_err(&device->dev, "Invalid _FIF element\n");
+		status = -EINVAL;
+	}
 
-	result = acpi_bus_update_power(device->handle, NULL);
-	if (result) {
-		printk(KERN_ERR PREFIX "Setting initial power state\n");
-		goto end;
+err:
+	kfree(obj);
+	return status;
+}
+
+static int acpi_fan_speed_cmp(const void *a, const void *b)
+{
+	const struct acpi_fan_fps *fps1 = a;
+	const struct acpi_fan_fps *fps2 = b;
+	return fps1->speed - fps2->speed;
+}
+
+static int acpi_fan_get_fps(struct acpi_device *device)
+{
+	struct acpi_fan *fan = acpi_driver_data(device);
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *obj;
+	acpi_status status;
+	int i;
+
+	status = acpi_evaluate_object(device->handle, "_FPS", NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		return status;
+
+	obj = buffer.pointer;
+	if (!obj || obj->type != ACPI_TYPE_PACKAGE || obj->package.count < 2) {
+		dev_err(&device->dev, "Invalid _FPS data\n");
+		status = -EINVAL;
+		goto err;
+	}
+
+	fan->fps_count = obj->package.count - 1; /* minus revision field */
+	fan->fps = devm_kzalloc(&device->dev,
+				fan->fps_count * sizeof(struct acpi_fan_fps),
+				GFP_KERNEL);
+	if (!fan->fps) {
+		dev_err(&device->dev, "Not enough memory\n");
+		status = -ENOMEM;
+		goto err;
+	}
+	for (i = 0; i < fan->fps_count; i++) {
+		struct acpi_buffer format = { sizeof("NNNNN"), "NNNNN" };
+		struct acpi_buffer fps = { sizeof(fan->fps[i]), &fan->fps[i] };
+		status = acpi_extract_package(&obj->package.elements[i + 1],
+					      &format, &fps);
+		if (ACPI_FAILURE(status)) {
+			dev_err(&device->dev, "Invalid _FPS element\n");
+			break;
+		}
+	}
+
+	/* sort the state array according to fan speed in increase order */
+	sort(fan->fps, fan->fps_count, sizeof(*fan->fps),
+	     acpi_fan_speed_cmp, NULL);
+
+err:
+	kfree(obj);
+	return status;
+}
+
+static int acpi_fan_probe(struct platform_device *pdev)
+{
+	int result = 0;
+	struct thermal_cooling_device *cdev;
+	struct acpi_fan *fan;
+	struct acpi_device *device = ACPI_COMPANION(&pdev->dev);
+
+	fan = devm_kzalloc(&pdev->dev, sizeof(*fan), GFP_KERNEL);
+	if (!fan) {
+		dev_err(&device->dev, "No memory for fan\n");
+		return -ENOMEM;
+	}
+	device->driver_data = fan;
+	platform_set_drvdata(pdev, fan);
+
+	if (acpi_fan_is_acpi4(device)) {
+		if (acpi_fan_get_fif(device) || acpi_fan_get_fps(device))
+			goto end;
+		fan->acpi4 = true;
+	} else {
+		result = acpi_device_update_power(device, NULL);
+		if (result) {
+			dev_err(&device->dev, "Setting initial power state\n");
+			goto end;
+		}
 	}
 
 	cdev = thermal_cooling_device_register("Fan", device,
@@ -154,45 +353,32 @@ static int acpi_fan_add(struct acpi_device *device)
 		goto end;
 	}
 
-	dev_dbg(&device->dev, "registered as cooling_device%d\n", cdev->id);
+	dev_dbg(&pdev->dev, "registered as cooling_device%d\n", cdev->id);
 
-	device->driver_data = cdev;
-	result = sysfs_create_link(&device->dev.kobj,
+	fan->cdev = cdev;
+	result = sysfs_create_link(&pdev->dev.kobj,
 				   &cdev->device.kobj,
 				   "thermal_cooling");
 	if (result)
-		dev_err(&device->dev, "Failed to create sysfs link "
-			"'thermal_cooling'\n");
+		dev_err(&pdev->dev, "Failed to create sysfs link 'thermal_cooling'\n");
 
 	result = sysfs_create_link(&cdev->device.kobj,
-				   &device->dev.kobj,
+				   &pdev->dev.kobj,
 				   "device");
 	if (result)
-		dev_err(&device->dev, "Failed to create sysfs link "
-			"'device'\n");
-
-	printk(KERN_INFO PREFIX "%s [%s] (%s)\n",
-	       acpi_device_name(device), acpi_device_bid(device),
-	       !device->power.state ? "on" : "off");
+		dev_err(&pdev->dev, "Failed to create sysfs link 'device'\n");
 
 end:
 	return result;
 }
 
-static int acpi_fan_remove(struct acpi_device *device)
+static int acpi_fan_remove(struct platform_device *pdev)
 {
-	struct thermal_cooling_device *cdev;
-
-	if (!device)
-		return -EINVAL;
-
-	cdev =  acpi_driver_data(device);
-	if (!cdev)
-		return -EINVAL;
+	struct acpi_fan *fan = platform_get_drvdata(pdev);
 
-	sysfs_remove_link(&device->dev.kobj, "thermal_cooling");
-	sysfs_remove_link(&cdev->device.kobj, "device");
-	thermal_cooling_device_unregister(cdev);
+	sysfs_remove_link(&pdev->dev.kobj, "thermal_cooling");
+	sysfs_remove_link(&fan->cdev->device.kobj, "device");
+	thermal_cooling_device_unregister(fan->cdev);
 
 	return 0;
 }
@@ -200,10 +386,11 @@ static int acpi_fan_remove(struct acpi_device *device)
 #ifdef CONFIG_PM_SLEEP
 static int acpi_fan_suspend(struct device *dev)
 {
-	if (!dev)
-		return -EINVAL;
+	struct acpi_fan *fan = dev_get_drvdata(dev);
+	if (fan->acpi4)
+		return 0;
 
-	acpi_bus_set_power(to_acpi_device(dev)->handle, ACPI_STATE_D0);
+	acpi_device_set_power(ACPI_COMPANION(dev), ACPI_STATE_D0);
 
 	return AE_OK;
 }
@@ -211,16 +398,17 @@ static int acpi_fan_suspend(struct device *dev)
 static int acpi_fan_resume(struct device *dev)
 {
 	int result;
+	struct acpi_fan *fan = dev_get_drvdata(dev);
 
-	if (!dev)
-		return -EINVAL;
+	if (fan->acpi4)
+		return 0;
 
-	result = acpi_bus_update_power(to_acpi_device(dev)->handle, NULL);
+	result = acpi_device_update_power(ACPI_COMPANION(dev), NULL);
 	if (result)
-		printk(KERN_ERR PREFIX "Error updating fan power state\n");
+		dev_err(dev, "Error updating fan power state\n");
 
 	return result;
 }
 #endif
 
-module_acpi_driver(acpi_fan_driver);
+module_platform_driver(acpi_fan_driver);
diff --git a/drivers/acpi/int340x_thermal.c b/drivers/acpi/int340x_thermal.c
new file mode 100644
index 000000000000..a27d31d1ba24
--- /dev/null
+++ b/drivers/acpi/int340x_thermal.c
@@ -0,0 +1,51 @@
+/*
+ * ACPI support for int340x thermal drivers
+ *
+ * Copyright (C) 2014, Intel Corporation
+ * Authors: Zhang Rui <rui.zhang@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/acpi.h>
+#include <linux/module.h>
+
+#include "internal.h"
+
+#define DO_ENUMERATION 0x01
+static const struct acpi_device_id int340x_thermal_device_ids[] = {
+	{"INT3400", DO_ENUMERATION },
+	{"INT3401"},
+	{"INT3402"},
+	{"INT3403"},
+	{"INT3404"},
+	{"INT3406"},
+	{"INT3407"},
+	{"INT3408"},
+	{"INT3409"},
+	{"INT340A"},
+	{"INT340B"},
+	{""},
+};
+
+static int int340x_thermal_handler_attach(struct acpi_device *adev,
+					const struct acpi_device_id *id)
+{
+#if defined(CONFIG_INT340X_THERMAL) || defined(CONFIG_INT340X_THERMAL_MODULE)
+	if (id->driver_data == DO_ENUMERATION)
+		acpi_create_platform_device(adev);
+#endif
+	return 1;
+}
+
+static struct acpi_scan_handler int340x_thermal_handler = {
+	.ids = int340x_thermal_device_ids,
+	.attach = int340x_thermal_handler_attach,
+};
+
+void __init acpi_int340x_thermal_init(void)
+{
+	acpi_scan_add_handler(&int340x_thermal_handler);
+}
diff --git a/drivers/acpi/internal.h b/drivers/acpi/internal.h
index 4c5cf77e7576..447f6d679b29 100644
--- a/drivers/acpi/internal.h
+++ b/drivers/acpi/internal.h
@@ -31,6 +31,7 @@ void acpi_pci_link_init(void);
 void acpi_processor_init(void);
 void acpi_platform_init(void);
 void acpi_pnp_init(void);
+void acpi_int340x_thermal_init(void);
 int acpi_sysfs_init(void);
 void acpi_container_init(void);
 void acpi_memory_hotplug_init(void);
@@ -103,8 +104,6 @@ int acpi_power_get_inferred_state(struct acpi_device *device, int *state);
 int acpi_power_on_resources(struct acpi_device *device, int state);
 int acpi_power_transition(struct acpi_device *device, int state);
 
-int acpi_device_update_power(struct acpi_device *device, int *state_p);
-
 int acpi_wakeup_device_init(void);
 
 #ifdef CONFIG_ARCH_MIGHT_HAVE_ACPI_PDC
@@ -168,13 +167,6 @@ static inline void suspend_nvs_restore(void) {}
 #endif
 
 /*--------------------------------------------------------------------------
-				Platform bus support
-  -------------------------------------------------------------------------- */
-struct platform_device;
-
-struct platform_device *acpi_create_platform_device(struct acpi_device *adev);
-
-/*--------------------------------------------------------------------------
 					Video
   -------------------------------------------------------------------------- */
 #if defined(CONFIG_ACPI_VIDEO) || defined(CONFIG_ACPI_VIDEO_MODULE)
diff --git a/drivers/acpi/osl.c b/drivers/acpi/osl.c
index 3abe9b223ba7..9964f70be98d 100644
--- a/drivers/acpi/osl.c
+++ b/drivers/acpi/osl.c
@@ -152,6 +152,16 @@ static u32 acpi_osi_handler(acpi_string interface, u32 supported)
 			osi_linux.dmi ? " via DMI" : "");
 	}
 
+	if (!strcmp("Darwin", interface)) {
+		/*
+		 * Apple firmware will behave poorly if it receives positive
+		 * answers to "Darwin" and any other OS. Respond positively
+		 * to Darwin and then disable all other vendor strings.
+		 */
+		acpi_update_interfaces(ACPI_DISABLE_ALL_VENDOR_STRINGS);
+		supported = ACPI_UINT32_MAX;
+	}
+
 	return supported;
 }
 
@@ -825,7 +835,7 @@ acpi_os_install_interrupt_handler(u32 gsi, acpi_osd_handler handler,
 
 	acpi_irq_handler = handler;
 	acpi_irq_context = context;
-	if (request_irq(irq, acpi_irq, IRQF_SHARED | IRQF_NO_SUSPEND, "acpi", acpi_irq)) {
+	if (request_irq(irq, acpi_irq, IRQF_SHARED, "acpi", acpi_irq)) {
 		printk(KERN_ERR PREFIX "SCI (IRQ%d) allocation failed\n", irq);
 		acpi_irq_handler = NULL;
 		return AE_NOT_ACQUIRED;
diff --git a/drivers/acpi/pci_irq.c b/drivers/acpi/pci_irq.c
index c96887d5289e..6e6b80eb0bba 100644
--- a/drivers/acpi/pci_irq.c
+++ b/drivers/acpi/pci_irq.c
@@ -484,6 +484,10 @@ void acpi_pci_irq_disable(struct pci_dev *dev)
 	/* Keep IOAPIC pin configuration when suspending */
 	if (dev->dev.power.is_prepared)
 		return;
+#ifdef	CONFIG_PM_RUNTIME
+	if (dev->dev.power.runtime_status == RPM_SUSPENDING)
+		return;
+#endif
 
 	entry = acpi_pci_irq_lookup(dev, pin);
 	if (!entry)
diff --git a/drivers/acpi/pci_root.c b/drivers/acpi/pci_root.c
index e6ae603ed1a1..cd4de7e038ea 100644
--- a/drivers/acpi/pci_root.c
+++ b/drivers/acpi/pci_root.c
@@ -35,6 +35,7 @@
 #include <linux/pci-aspm.h>
 #include <linux/acpi.h>
 #include <linux/slab.h>
+#include <linux/dmi.h>
 #include <acpi/apei.h>	/* for acpi_hest_init() */
 
 #include "internal.h"
@@ -430,6 +431,19 @@ static void negotiate_os_control(struct acpi_pci_root *root, int *no_aspm,
 	acpi_handle handle = device->handle;
 
 	/*
+	 * Apple always return failure on _OSC calls when _OSI("Darwin") has
+	 * been called successfully. We know the feature set supported by the
+	 * platform, so avoid calling _OSC at all
+	 */
+
+	if (dmi_match(DMI_SYS_VENDOR, "Apple Inc.")) {
+		root->osc_control_set = ~OSC_PCI_EXPRESS_PME_CONTROL;
+		decode_osc_control(root, "OS assumes control of",
+				   root->osc_control_set);
+		return;
+	}
+
+	/*
 	 * All supported architectures that use ACPI have support for
 	 * PCI domains, so we indicate this in _OSC support capabilities.
 	 */
diff --git a/drivers/acpi/processor_core.c b/drivers/acpi/processor_core.c
index e32321ce9a5c..ef58f46c8442 100644
--- a/drivers/acpi/processor_core.c
+++ b/drivers/acpi/processor_core.c
@@ -16,7 +16,7 @@ static int map_lapic_id(struct acpi_subtable_header *entry,
 		 u32 acpi_id, int *apic_id)
 {
 	struct acpi_madt_local_apic *lapic =
-		(struct acpi_madt_local_apic *)entry;
+		container_of(entry, struct acpi_madt_local_apic, header);
 
 	if (!(lapic->lapic_flags & ACPI_MADT_ENABLED))
 		return -ENODEV;
@@ -32,7 +32,7 @@ static int map_x2apic_id(struct acpi_subtable_header *entry,
 			 int device_declaration, u32 acpi_id, int *apic_id)
 {
 	struct acpi_madt_local_x2apic *apic =
-		(struct acpi_madt_local_x2apic *)entry;
+		container_of(entry, struct acpi_madt_local_x2apic, header);
 
 	if (!(apic->lapic_flags & ACPI_MADT_ENABLED))
 		return -ENODEV;
@@ -49,7 +49,7 @@ static int map_lsapic_id(struct acpi_subtable_header *entry,
 		int device_declaration, u32 acpi_id, int *apic_id)
 {
 	struct acpi_madt_local_sapic *lsapic =
-		(struct acpi_madt_local_sapic *)entry;
+		container_of(entry, struct acpi_madt_local_sapic, header);
 
 	if (!(lsapic->lapic_flags & ACPI_MADT_ENABLED))
 		return -ENODEV;
diff --git a/drivers/acpi/processor_idle.c b/drivers/acpi/processor_idle.c
index 3dca36d4ad26..17f9ec501972 100644
--- a/drivers/acpi/processor_idle.c
+++ b/drivers/acpi/processor_idle.c
@@ -1071,9 +1071,9 @@ int acpi_processor_cst_has_changed(struct acpi_processor *pr)
 
 	if (pr->id == 0 && cpuidle_get_driver() == &acpi_idle_driver) {
 
-		cpuidle_pause_and_lock();
 		/* Protect against cpu-hotplug */
 		get_online_cpus();
+		cpuidle_pause_and_lock();
 
 		/* Disable all cpuidle devices */
 		for_each_online_cpu(cpu) {
@@ -1100,8 +1100,8 @@ int acpi_processor_cst_has_changed(struct acpi_processor *pr)
 				cpuidle_enable_device(dev);
 			}
 		}
-		put_online_cpus();
 		cpuidle_resume_and_unlock();
+		put_online_cpus();
 	}
 
 	return 0;
diff --git a/drivers/acpi/sbs.c b/drivers/acpi/sbs.c
index 366ca40a6f70..a7a3edd28beb 100644
--- a/drivers/acpi/sbs.c
+++ b/drivers/acpi/sbs.c
@@ -35,6 +35,7 @@
 #include <linux/jiffies.h>
 #include <linux/delay.h>
 #include <linux/power_supply.h>
+#include <linux/dmi.h>
 
 #include "sbshc.h"
 #include "battery.h"
@@ -61,6 +62,8 @@ static unsigned int cache_time = 1000;
 module_param(cache_time, uint, 0644);
 MODULE_PARM_DESC(cache_time, "cache time in milliseconds");
 
+static bool sbs_manager_broken;
+
 #define MAX_SBS_BAT			4
 #define ACPI_SBS_BLOCK_MAX		32
 
@@ -109,6 +112,7 @@ struct acpi_sbs {
 	u8 batteries_supported:4;
 	u8 manager_present:1;
 	u8 charger_present:1;
+	u8 charger_exists:1;
 };
 
 #define to_acpi_sbs(x) container_of(x, struct acpi_sbs, charger)
@@ -429,9 +433,19 @@ static int acpi_ac_get_present(struct acpi_sbs *sbs)
 
 	result = acpi_smbus_read(sbs->hc, SMBUS_READ_WORD, ACPI_SBS_CHARGER,
 				 0x13, (u8 *) & status);
-	if (!result)
-		sbs->charger_present = (status >> 15) & 0x1;
-	return result;
+
+	if (result)
+		return result;
+
+	/*
+	 * The spec requires that bit 4 always be 1. If it's not set, assume
+	 * that the implementation doesn't support an SBS charger
+	 */
+	if (!((status >> 4) & 0x1))
+		return -ENODEV;
+
+	sbs->charger_present = (status >> 15) & 0x1;
+	return 0;
 }
 
 static ssize_t acpi_battery_alarm_show(struct device *dev,
@@ -483,16 +497,21 @@ static int acpi_battery_read(struct acpi_battery *battery)
 				  ACPI_SBS_MANAGER, 0x01, (u8 *)&state, 2);
 	} else if (battery->id == 0)
 		battery->present = 1;
+
 	if (result || !battery->present)
 		return result;
 
 	if (saved_present != battery->present) {
 		battery->update_time = 0;
 		result = acpi_battery_get_info(battery);
-		if (result)
+		if (result) {
+			battery->present = 0;
 			return result;
+		}
 	}
 	result = acpi_battery_get_state(battery);
+	if (result)
+		battery->present = 0;
 	return result;
 }
 
@@ -524,6 +543,7 @@ static int acpi_battery_add(struct acpi_sbs *sbs, int id)
 	result = power_supply_register(&sbs->device->dev, &battery->bat);
 	if (result)
 		goto end;
+
 	result = device_create_file(battery->bat.dev, &alarm_attr);
 	if (result)
 		goto end;
@@ -554,6 +574,7 @@ static int acpi_charger_add(struct acpi_sbs *sbs)
 	if (result)
 		goto end;
 
+	sbs->charger_exists = 1;
 	sbs->charger.name = "sbs-charger";
 	sbs->charger.type = POWER_SUPPLY_TYPE_MAINS;
 	sbs->charger.properties = sbs_ac_props;
@@ -580,9 +601,12 @@ static void acpi_sbs_callback(void *context)
 	struct acpi_battery *bat;
 	u8 saved_charger_state = sbs->charger_present;
 	u8 saved_battery_state;
-	acpi_ac_get_present(sbs);
-	if (sbs->charger_present != saved_charger_state)
-		kobject_uevent(&sbs->charger.dev->kobj, KOBJ_CHANGE);
+
+	if (sbs->charger_exists) {
+		acpi_ac_get_present(sbs);
+		if (sbs->charger_present != saved_charger_state)
+			kobject_uevent(&sbs->charger.dev->kobj, KOBJ_CHANGE);
+	}
 
 	if (sbs->manager_present) {
 		for (id = 0; id < MAX_SBS_BAT; ++id) {
@@ -598,12 +622,31 @@ static void acpi_sbs_callback(void *context)
 	}
 }
 
+static int disable_sbs_manager(const struct dmi_system_id *d)
+{
+	sbs_manager_broken = true;
+	return 0;
+}
+
+static struct dmi_system_id acpi_sbs_dmi_table[] = {
+	{
+		.callback = disable_sbs_manager,
+		.ident = "Apple",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Apple Inc.")
+		},
+	},
+	{ },
+};
+
 static int acpi_sbs_add(struct acpi_device *device)
 {
 	struct acpi_sbs *sbs;
 	int result = 0;
 	int id;
 
+	dmi_check_system(acpi_sbs_dmi_table);
+
 	sbs = kzalloc(sizeof(struct acpi_sbs), GFP_KERNEL);
 	if (!sbs) {
 		result = -ENOMEM;
@@ -619,17 +662,24 @@ static int acpi_sbs_add(struct acpi_device *device)
 	device->driver_data = sbs;
 
 	result = acpi_charger_add(sbs);
-	if (result)
+	if (result && result != -ENODEV)
 		goto end;
 
-	result = acpi_manager_get_info(sbs);
-	if (!result) {
-		sbs->manager_present = 1;
-		for (id = 0; id < MAX_SBS_BAT; ++id)
-			if ((sbs->batteries_supported & (1 << id)))
-				acpi_battery_add(sbs, id);
-	} else
+	result = 0;
+
+	if (!sbs_manager_broken) {
+		result = acpi_manager_get_info(sbs);
+		if (!result) {
+			sbs->manager_present = 0;
+			for (id = 0; id < MAX_SBS_BAT; ++id)
+				if ((sbs->batteries_supported & (1 << id)))
+					acpi_battery_add(sbs, id);
+		}
+	}
+
+	if (!sbs->manager_present)
 		acpi_battery_add(sbs, 0);
+
 	acpi_smbus_register_callback(sbs->hc, acpi_sbs_callback, sbs);
       end:
 	if (result)
diff --git a/drivers/acpi/scan.c b/drivers/acpi/scan.c
index 0a817ad24f16..d670158a26c5 100644
--- a/drivers/acpi/scan.c
+++ b/drivers/acpi/scan.c
@@ -130,7 +130,7 @@ static int create_modalias(struct acpi_device *acpi_dev, char *modalias,
 	list_for_each_entry(id, &acpi_dev->pnp.ids, list) {
 		count = snprintf(&modalias[len], size, "%s:", id->id);
 		if (count < 0)
-			return EINVAL;
+			return -EINVAL;
 		if (count >= size)
 			return -ENOMEM;
 		len += count;
@@ -667,8 +667,14 @@ static ssize_t
 acpi_device_sun_show(struct device *dev, struct device_attribute *attr,
 		     char *buf) {
 	struct acpi_device *acpi_dev = to_acpi_device(dev);
+	acpi_status status;
+	unsigned long long sun;
+
+	status = acpi_evaluate_integer(acpi_dev->handle, "_SUN", NULL, &sun);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
 
-	return sprintf(buf, "%lu\n", acpi_dev->pnp.sun);
+	return sprintf(buf, "%llu\n", sun);
 }
 static DEVICE_ATTR(sun, 0444, acpi_device_sun_show, NULL);
 
@@ -690,7 +696,6 @@ static int acpi_device_setup_files(struct acpi_device *dev)
 {
 	struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER, NULL};
 	acpi_status status;
-	unsigned long long sun;
 	int result = 0;
 
 	/*
@@ -731,14 +736,10 @@ static int acpi_device_setup_files(struct acpi_device *dev)
 	if (dev->pnp.unique_id)
 		result = device_create_file(&dev->dev, &dev_attr_uid);
 
-	status = acpi_evaluate_integer(dev->handle, "_SUN", NULL, &sun);
-	if (ACPI_SUCCESS(status)) {
-		dev->pnp.sun = (unsigned long)sun;
+	if (acpi_has_method(dev->handle, "_SUN")) {
 		result = device_create_file(&dev->dev, &dev_attr_sun);
 		if (result)
 			goto end;
-	} else {
-		dev->pnp.sun = (unsigned long)-1;
 	}
 
 	if (acpi_has_method(dev->handle, "_STA")) {
@@ -922,12 +923,17 @@ static void acpi_device_notify(acpi_handle handle, u32 event, void *data)
 	device->driver->ops.notify(device, event);
 }
 
-static acpi_status acpi_device_notify_fixed(void *data)
+static void acpi_device_notify_fixed(void *data)
 {
 	struct acpi_device *device = data;
 
 	/* Fixed hardware devices have no handles */
 	acpi_device_notify(NULL, ACPI_FIXED_HARDWARE_EVENT, device);
+}
+
+static acpi_status acpi_device_fixed_event(void *data)
+{
+	acpi_os_execute(OSL_NOTIFY_HANDLER, acpi_device_notify_fixed, data);
 	return AE_OK;
 }
 
@@ -938,12 +944,12 @@ static int acpi_device_install_notify_handler(struct acpi_device *device)
 	if (device->device_type == ACPI_BUS_TYPE_POWER_BUTTON)
 		status =
 		    acpi_install_fixed_event_handler(ACPI_EVENT_POWER_BUTTON,
-						     acpi_device_notify_fixed,
+						     acpi_device_fixed_event,
 						     device);
 	else if (device->device_type == ACPI_BUS_TYPE_SLEEP_BUTTON)
 		status =
 		    acpi_install_fixed_event_handler(ACPI_EVENT_SLEEP_BUTTON,
-						     acpi_device_notify_fixed,
+						     acpi_device_fixed_event,
 						     device);
 	else
 		status = acpi_install_notify_handler(device->handle,
@@ -960,10 +966,10 @@ static void acpi_device_remove_notify_handler(struct acpi_device *device)
 {
 	if (device->device_type == ACPI_BUS_TYPE_POWER_BUTTON)
 		acpi_remove_fixed_event_handler(ACPI_EVENT_POWER_BUTTON,
-						acpi_device_notify_fixed);
+						acpi_device_fixed_event);
 	else if (device->device_type == ACPI_BUS_TYPE_SLEEP_BUTTON)
 		acpi_remove_fixed_event_handler(ACPI_EVENT_SLEEP_BUTTON,
-						acpi_device_notify_fixed);
+						acpi_device_fixed_event);
 	else
 		acpi_remove_notify_handler(device->handle, ACPI_DEVICE_NOTIFY,
 					   acpi_device_notify);
@@ -975,7 +981,7 @@ static int acpi_device_probe(struct device *dev)
 	struct acpi_driver *acpi_drv = to_acpi_driver(dev->driver);
 	int ret;
 
-	if (acpi_dev->handler)
+	if (acpi_dev->handler && !acpi_is_pnp_device(acpi_dev))
 		return -EINVAL;
 
 	if (!acpi_drv->ops.add)
@@ -1464,7 +1470,7 @@ static void acpi_wakeup_gpe_init(struct acpi_device *device)
 	if (ACPI_FAILURE(status))
 		return;
 
-	wakeup->flags.run_wake = !!(event_status & ACPI_EVENT_FLAG_HANDLE);
+	wakeup->flags.run_wake = !!(event_status & ACPI_EVENT_FLAG_HAS_HANDLER);
 }
 
 static void acpi_bus_get_wakeup_device_flags(struct acpi_device *device)
@@ -2183,6 +2189,9 @@ static void acpi_bus_attach(struct acpi_device *device)
  ok:
 	list_for_each_entry(child, &device->children, node)
 		acpi_bus_attach(child);
+
+	if (device->handler && device->handler->hotplug.notify_online)
+		device->handler->hotplug.notify_online(device);
 }
 
 /**
@@ -2306,6 +2315,7 @@ int __init acpi_scan_init(void)
 	acpi_container_init();
 	acpi_memory_hotplug_init();
 	acpi_pnp_init();
+	acpi_int340x_thermal_init();
 
 	mutex_lock(&acpi_scan_lock);
 	/*
diff --git a/drivers/acpi/sleep.c b/drivers/acpi/sleep.c
index 54da4a3fe65e..05a31b573fc3 100644
--- a/drivers/acpi/sleep.c
+++ b/drivers/acpi/sleep.c
@@ -14,6 +14,7 @@
 #include <linux/irq.h>
 #include <linux/dmi.h>
 #include <linux/device.h>
+#include <linux/interrupt.h>
 #include <linux/suspend.h>
 #include <linux/reboot.h>
 #include <linux/acpi.h>
@@ -626,6 +627,19 @@ static int acpi_freeze_begin(void)
 	return 0;
 }
 
+static int acpi_freeze_prepare(void)
+{
+	acpi_enable_all_wakeup_gpes();
+	enable_irq_wake(acpi_gbl_FADT.sci_interrupt);
+	return 0;
+}
+
+static void acpi_freeze_restore(void)
+{
+	disable_irq_wake(acpi_gbl_FADT.sci_interrupt);
+	acpi_enable_all_runtime_gpes();
+}
+
 static void acpi_freeze_end(void)
 {
 	acpi_scan_lock_release();
@@ -633,6 +647,8 @@ static void acpi_freeze_end(void)
 
 static const struct platform_freeze_ops acpi_freeze_ops = {
 	.begin = acpi_freeze_begin,
+	.prepare = acpi_freeze_prepare,
+	.restore = acpi_freeze_restore,
 	.end = acpi_freeze_end,
 };
 
diff --git a/drivers/acpi/sysfs.c b/drivers/acpi/sysfs.c
index 38cb9782d4b8..13e577c80201 100644
--- a/drivers/acpi/sysfs.c
+++ b/drivers/acpi/sysfs.c
@@ -537,7 +537,7 @@ static ssize_t counter_show(struct kobject *kobj,
 	if (result)
 		goto end;
 
-	if (!(status & ACPI_EVENT_FLAG_HANDLE))
+	if (!(status & ACPI_EVENT_FLAG_HAS_HANDLER))
 		size += sprintf(buf + size, "   invalid");
 	else if (status & ACPI_EVENT_FLAG_ENABLED)
 		size += sprintf(buf + size, "   enabled");
@@ -581,7 +581,7 @@ static ssize_t counter_set(struct kobject *kobj,
 	if (result)
 		goto end;
 
-	if (!(status & ACPI_EVENT_FLAG_HANDLE)) {
+	if (!(status & ACPI_EVENT_FLAG_HAS_HANDLER)) {
 		printk(KERN_WARNING PREFIX
 		       "Can not change Invalid GPE/Fixed Event status\n");
 		return -EINVAL;
diff --git a/drivers/acpi/thermal.c b/drivers/acpi/thermal.c
index 112817e963e0..d24fa1964eb8 100644
--- a/drivers/acpi/thermal.c
+++ b/drivers/acpi/thermal.c
@@ -528,7 +528,6 @@ static void acpi_thermal_check(void *data)
 }
 
 /* sys I/F for generic thermal sysfs support */
-#define KELVIN_TO_MILLICELSIUS(t, off) (((t) - (off)) * 100)
 
 static int thermal_get_temp(struct thermal_zone_device *thermal,
 			    unsigned long *temp)
@@ -543,7 +542,8 @@ static int thermal_get_temp(struct thermal_zone_device *thermal,
 	if (result)
 		return result;
 
-	*temp = KELVIN_TO_MILLICELSIUS(tz->temperature, tz->kelvin_offset);
+	*temp = DECI_KELVIN_TO_MILLICELSIUS_WITH_OFFSET(tz->temperature,
+							tz->kelvin_offset);
 	return 0;
 }
 
@@ -647,7 +647,7 @@ static int thermal_get_trip_temp(struct thermal_zone_device *thermal,
 
 	if (tz->trips.critical.flags.valid) {
 		if (!trip) {
-			*temp = KELVIN_TO_MILLICELSIUS(
+			*temp = DECI_KELVIN_TO_MILLICELSIUS_WITH_OFFSET(
 				tz->trips.critical.temperature,
 				tz->kelvin_offset);
 			return 0;
@@ -657,7 +657,7 @@ static int thermal_get_trip_temp(struct thermal_zone_device *thermal,
 
 	if (tz->trips.hot.flags.valid) {
 		if (!trip) {
-			*temp = KELVIN_TO_MILLICELSIUS(
+			*temp = DECI_KELVIN_TO_MILLICELSIUS_WITH_OFFSET(
 				tz->trips.hot.temperature,
 				tz->kelvin_offset);
 			return 0;
@@ -667,7 +667,7 @@ static int thermal_get_trip_temp(struct thermal_zone_device *thermal,
 
 	if (tz->trips.passive.flags.valid) {
 		if (!trip) {
-			*temp = KELVIN_TO_MILLICELSIUS(
+			*temp = DECI_KELVIN_TO_MILLICELSIUS_WITH_OFFSET(
 				tz->trips.passive.temperature,
 				tz->kelvin_offset);
 			return 0;
@@ -678,7 +678,7 @@ static int thermal_get_trip_temp(struct thermal_zone_device *thermal,
 	for (i = 0; i < ACPI_THERMAL_MAX_ACTIVE &&
 		tz->trips.active[i].flags.valid; i++) {
 		if (!trip) {
-			*temp = KELVIN_TO_MILLICELSIUS(
+			*temp = DECI_KELVIN_TO_MILLICELSIUS_WITH_OFFSET(
 				tz->trips.active[i].temperature,
 				tz->kelvin_offset);
 			return 0;
@@ -694,7 +694,7 @@ static int thermal_get_crit_temp(struct thermal_zone_device *thermal,
 	struct acpi_thermal *tz = thermal->devdata;
 
 	if (tz->trips.critical.flags.valid) {
-		*temperature = KELVIN_TO_MILLICELSIUS(
+		*temperature = DECI_KELVIN_TO_MILLICELSIUS_WITH_OFFSET(
 				tz->trips.critical.temperature,
 				tz->kelvin_offset);
 		return 0;
@@ -714,8 +714,8 @@ static int thermal_get_trend(struct thermal_zone_device *thermal,
 
 	if (type == THERMAL_TRIP_ACTIVE) {
 		unsigned long trip_temp;
-		unsigned long temp = KELVIN_TO_MILLICELSIUS(tz->temperature,
-							tz->kelvin_offset);
+		unsigned long temp = DECI_KELVIN_TO_MILLICELSIUS_WITH_OFFSET(
+					tz->temperature, tz->kelvin_offset);
 		if (thermal_get_trip_temp(thermal, trip, &trip_temp))
 			return -EINVAL;
 
diff --git a/drivers/acpi/utils.c b/drivers/acpi/utils.c
index 07c8c5a5ee95..371ac12d25b1 100644
--- a/drivers/acpi/utils.c
+++ b/drivers/acpi/utils.c
@@ -149,6 +149,21 @@ acpi_extract_package(union acpi_object *package,
 				break;
 			}
 			break;
+		case ACPI_TYPE_LOCAL_REFERENCE:
+			switch (format_string[i]) {
+			case 'R':
+				size_required += sizeof(void *);
+				tail_offset += sizeof(void *);
+				break;
+			default:
+				printk(KERN_WARNING PREFIX "Invalid package element"
+					      " [%d] got reference,"
+					      " expecting [%c]\n",
+					      i, format_string[i]);
+				return AE_BAD_DATA;
+				break;
+			}
+			break;
 
 		case ACPI_TYPE_PACKAGE:
 		default:
@@ -247,7 +262,18 @@ acpi_extract_package(union acpi_object *package,
 				break;
 			}
 			break;
-
+		case ACPI_TYPE_LOCAL_REFERENCE:
+			switch (format_string[i]) {
+			case 'R':
+				*(void **)head =
+				    (void *)element->reference.handle;
+				head += sizeof(void *);
+				break;
+			default:
+				/* Should never get here */
+				break;
+			}
+			break;
 		case ACPI_TYPE_PACKAGE:
 			/* TBD: handle nested packages... */
 		default:
@@ -661,7 +687,6 @@ EXPORT_SYMBOL(acpi_evaluate_dsm);
  * @uuid: UUID of requested functions, should be 16 bytes at least
  * @rev: revision number of requested functions
  * @funcs: bitmap of requested functions
- * @exclude: excluding special value, used to support i915 and nouveau
  *
  * Evaluate device's _DSM method to check whether it supports requested
  * functions. Currently only support 64 functions at maximum, should be
diff --git a/drivers/acpi/video.c b/drivers/acpi/video.c
index 826884392e6b..807a88a0f394 100644
--- a/drivers/acpi/video.c
+++ b/drivers/acpi/video.c
@@ -82,9 +82,9 @@ module_param(allow_duplicates, bool, 0644);
  * For Windows 8 systems: used to decide if video module
  * should skip registering backlight interface of its own.
  */
-static int use_native_backlight_param = 1;
+static int use_native_backlight_param = -1;
 module_param_named(use_native_backlight, use_native_backlight_param, int, 0444);
-static bool use_native_backlight_dmi = false;
+static bool use_native_backlight_dmi = true;
 
 static int register_count;
 static struct mutex video_list_lock;
@@ -411,9 +411,9 @@ static int __init video_set_bqc_offset(const struct dmi_system_id *d)
 	return 0;
 }
 
-static int __init video_set_use_native_backlight(const struct dmi_system_id *d)
+static int __init video_disable_native_backlight(const struct dmi_system_id *d)
 {
-	use_native_backlight_dmi = true;
+	use_native_backlight_dmi = false;
 	return 0;
 }
 
@@ -461,263 +461,47 @@ static struct dmi_system_id video_dmi_table[] __initdata = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 7720"),
 		},
 	},
+
+	/*
+	 * These models have a working acpi_video backlight control, and using
+	 * native backlight causes a regression where backlight does not work
+	 * when userspace is not handling brightness key events. Disable
+	 * native_backlight on these to fix this:
+	 * https://bugzilla.kernel.org/show_bug.cgi?id=81691
+	 */
 	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad X230",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X230"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad T430 and T430s",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T430"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad T430",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "2349D15"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad T431s",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "20AACTO1WW"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad Edge E530",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "3259A2G"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad Edge E530",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "3259CTO"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad Edge E530",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "3259HJG"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ThinkPad W530",
+	 .callback = video_disable_native_backlight,
+	 .ident = "ThinkPad T420",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad W530"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T420"),
 		},
 	},
 	{
-	 .callback = video_set_use_native_backlight,
-	.ident = "ThinkPad X1 Carbon",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X1 Carbon"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Lenovo Yoga 13",
+	 .callback = video_disable_native_backlight,
+	 .ident = "ThinkPad T520",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo IdeaPad Yoga 13"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad T520"),
 		},
 	},
 	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Lenovo Yoga 2 11",
+	 .callback = video_disable_native_backlight,
+	 .ident = "ThinkPad X201s",
 	 .matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo Yoga 2 11"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "Thinkpad Helix",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad Helix"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Dell Inspiron 7520",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-		DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 7520"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer Aspire 5733Z",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 5733Z"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer Aspire 5742G",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 5742G"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer Aspire V5-171",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "V5-171"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer Aspire V5-431",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "Aspire V5-431"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer Aspire V5-471G",
-	 .matches = {
-		DMI_MATCH(DMI_BOARD_VENDOR, "Acer"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "Aspire V5-471G"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer TravelMate B113",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "TravelMate B113"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer Aspire V5-572G",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Acer Aspire"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "V5-572G/Dazzle_CX"),
-		},
-	},
-	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "Acer Aspire V5-573G",
-	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Acer Aspire"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "V5-573G/Dazzle_HW"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "ThinkPad X201s"),
 		},
 	},
+
+	/* The native backlight controls do not work on some older machines */
 	{
-	 .callback = video_set_use_native_backlight,
-	 .ident = "ASUS Zenbook Prime UX31A",
+	 /* https://bugs.freedesktop.org/show_bug.cgi?id=81515 */
+	 .callback = video_disable_native_backlight,
+	 .ident = "HP ENVY 15 Notebook",
 	 .matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
-		DMI_MATCH(DMI_PRODUCT_NAME, "UX31A"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP ProBook 4340s",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "HP ProBook 4340s"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP ProBook 4540s",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_VERSION, "HP ProBook 4540s"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP ProBook 2013 models",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP ProBook "),
-		DMI_MATCH(DMI_PRODUCT_NAME, " G1"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP EliteBook 2013 models",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP EliteBook "),
-		DMI_MATCH(DMI_PRODUCT_NAME, " G1"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP EliteBook 2014 models",
-	.matches = {
 		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP EliteBook "),
-		DMI_MATCH(DMI_PRODUCT_NAME, " G2"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP ZBook 14",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP ZBook 14"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP ZBook 15",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP ZBook 15"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP ZBook 17",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP ZBook 17"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP EliteBook 8470p",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP EliteBook 8470p"),
-		},
-	},
-	{
-	.callback = video_set_use_native_backlight,
-	.ident = "HP EliteBook 8780w",
-	.matches = {
-		DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-		DMI_MATCH(DMI_PRODUCT_NAME, "HP EliteBook 8780w"),
+		DMI_MATCH(DMI_PRODUCT_NAME, "HP ENVY 15 Notebook PC"),
 		},
 	},
 	{}
@@ -1370,6 +1154,23 @@ acpi_video_device_bind(struct acpi_video_bus *video,
 	}
 }
 
+static bool acpi_video_device_in_dod(struct acpi_video_device *device)
+{
+	struct acpi_video_bus *video = device->video;
+	int i;
+
+	/* If we have a broken _DOD, no need to test */
+	if (!video->attached_count)
+		return true;
+
+	for (i = 0; i < video->attached_count; i++) {
+		if (video->attached_array[i].bind_info == device)
+			return true;
+	}
+
+	return false;
+}
+
 /*
  *  Arg:
  *	video	: video bus device
@@ -1809,6 +1610,15 @@ static void acpi_video_dev_register_backlight(struct acpi_video_device *device)
 	static int count;
 	char *name;
 
+	/*
+	 * Do not create backlight device for video output
+	 * device that is not in the enumerated list.
+	 */
+	if (!acpi_video_device_in_dod(device)) {
+		dev_dbg(&device->dev->dev, "not in _DOD list, ignore\n");
+		return;
+	}
+
 	result = acpi_video_init_brightness(device);
 	if (result)
 		return;
diff --git a/drivers/acpi/video_detect.c b/drivers/acpi/video_detect.c
index c42feb2bacd0..27c43499977a 100644
--- a/drivers/acpi/video_detect.c
+++ b/drivers/acpi/video_detect.c
@@ -174,6 +174,14 @@ static struct dmi_system_id video_detect_dmi_table[] = {
 		DMI_MATCH(DMI_PRODUCT_NAME, "Inspiron 5737"),
 		},
 	},
+	{
+	.callback = video_detect_force_vendor,
+	.ident = "Lenovo IdeaPad Z570",
+	.matches = {
+		DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
+		DMI_MATCH(DMI_PRODUCT_VERSION, "Ideapad Z570"),
+		},
+	},
 	{ },
 };
 
diff --git a/drivers/amba/bus.c b/drivers/amba/bus.c
index 3cf61a127ee5..47bbdc1b5be3 100644
--- a/drivers/amba/bus.c
+++ b/drivers/amba/bus.c
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/pm.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_domain.h>
 #include <linux/amba/bus.h>
 #include <linux/sizes.h>
 
@@ -182,9 +183,15 @@ static int amba_probe(struct device *dev)
 	int ret;
 
 	do {
+		ret = dev_pm_domain_attach(dev, true);
+		if (ret == -EPROBE_DEFER)
+			break;
+
 		ret = amba_get_enable_pclk(pcdev);
-		if (ret)
+		if (ret) {
+			dev_pm_domain_detach(dev, true);
 			break;
+		}
 
 		pm_runtime_get_noresume(dev);
 		pm_runtime_set_active(dev);
@@ -199,6 +206,7 @@ static int amba_probe(struct device *dev)
 		pm_runtime_put_noidle(dev);
 
 		amba_put_disable_pclk(pcdev);
+		dev_pm_domain_detach(dev, true);
 	} while (0);
 
 	return ret;
@@ -220,6 +228,7 @@ static int amba_remove(struct device *dev)
 	pm_runtime_put_noidle(dev);
 
 	amba_put_disable_pclk(pcdev);
+	dev_pm_domain_detach(dev, true);
 
 	return ret;
 }
diff --git a/drivers/ata/Kconfig b/drivers/ata/Kconfig
index e1b92788c225..cd4cccbfd2ab 100644
--- a/drivers/ata/Kconfig
+++ b/drivers/ata/Kconfig
@@ -299,7 +299,7 @@ config SATA_HIGHBANK
 
 config SATA_MV
 	tristate "Marvell SATA support"
-	depends on PCI || ARCH_DOVE || ARCH_KIRKWOOD || ARCH_MV78XX0 || \
+	depends on PCI || ARCH_DOVE || ARCH_MV78XX0 || \
 		   ARCH_MVEBU || ARCH_ORION5X || COMPILE_TEST
 	select GENERIC_PHY
 	help
diff --git a/drivers/ata/acard-ahci.c b/drivers/ata/acard-ahci.c
index 25d0ac32e721..c962886d7e71 100644
--- a/drivers/ata/acard-ahci.c
+++ b/drivers/ata/acard-ahci.c
@@ -498,8 +498,7 @@ static int acard_ahci_init_one(struct pci_dev *pdev, const struct pci_device_id
 	acard_ahci_pci_print_info(host);
 
 	pci_set_master(pdev);
-	return ata_host_activate(host, pdev->irq, ahci_interrupt, IRQF_SHARED,
-				 &acard_ahci_sht);
+	return ahci_host_activate(host, pdev->irq, &acard_ahci_sht);
 }
 
 module_pci_driver(acard_ahci_pci_driver);
diff --git a/drivers/ata/ahci.c b/drivers/ata/ahci.c
index a29f8012fb08..5f039f191067 100644
--- a/drivers/ata/ahci.c
+++ b/drivers/ata/ahci.c
@@ -305,6 +305,14 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	{ PCI_VDEVICE(INTEL, 0x9c85), board_ahci }, /* Wildcat Point-LP RAID */
 	{ PCI_VDEVICE(INTEL, 0x9c87), board_ahci }, /* Wildcat Point-LP RAID */
 	{ PCI_VDEVICE(INTEL, 0x9c8f), board_ahci }, /* Wildcat Point-LP RAID */
+	{ PCI_VDEVICE(INTEL, 0x8c82), board_ahci }, /* 9 Series AHCI */
+	{ PCI_VDEVICE(INTEL, 0x8c83), board_ahci }, /* 9 Series AHCI */
+	{ PCI_VDEVICE(INTEL, 0x8c84), board_ahci }, /* 9 Series RAID */
+	{ PCI_VDEVICE(INTEL, 0x8c85), board_ahci }, /* 9 Series RAID */
+	{ PCI_VDEVICE(INTEL, 0x8c86), board_ahci }, /* 9 Series RAID */
+	{ PCI_VDEVICE(INTEL, 0x8c87), board_ahci }, /* 9 Series RAID */
+	{ PCI_VDEVICE(INTEL, 0x8c8e), board_ahci }, /* 9 Series RAID */
+	{ PCI_VDEVICE(INTEL, 0x8c8f), board_ahci }, /* 9 Series RAID */
 
 	/* JMicron 360/1/3/5/6, match class to avoid IDE function */
 	{ PCI_VENDOR_ID_JMICRON, PCI_ANY_ID, PCI_ANY_ID, PCI_ANY_ID,
@@ -442,6 +450,8 @@ static const struct pci_device_id ahci_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x917a),
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9172 */
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x9172),
+	  .driver_data = board_ahci_yes_fbs },			/* 88se9182 */
+	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x9182),
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9172 */
 	{ PCI_DEVICE(PCI_VENDOR_ID_MARVELL_EXT, 0x9192),
 	  .driver_data = board_ahci_yes_fbs },			/* 88se9172 on some Gigabyte */
@@ -780,7 +790,7 @@ static void ahci_pci_print_info(struct ata_host *host)
  */
 static void ahci_p5wdh_workaround(struct ata_host *host)
 {
-	static struct dmi_system_id sysids[] = {
+	static const struct dmi_system_id sysids[] = {
 		{
 			.ident = "P5W DH Deluxe",
 			.matches = {
@@ -1211,6 +1221,9 @@ static int ahci_init_interrupts(struct pci_dev *pdev, unsigned int n_ports,
 		goto single_msi;
 	}
 
+	if (nvec > 1)
+		hpriv->flags |= AHCI_HFLAG_MULTI_MSI;
+
 	return nvec;
 
 single_msi:
@@ -1223,71 +1236,6 @@ intx:
 	return 0;
 }
 
-/**
- *	ahci_host_activate - start AHCI host, request IRQs and register it
- *	@host: target ATA host
- *	@irq: base IRQ number to request
- *	@n_msis: number of MSIs allocated for this host
- *	@irq_handler: irq_handler used when requesting IRQs
- *	@irq_flags: irq_flags used when requesting IRQs
- *
- *	Similar to ata_host_activate, but requests IRQs according to AHCI-1.1
- *	when multiple MSIs were allocated. That is one MSI per port, starting
- *	from @irq.
- *
- *	LOCKING:
- *	Inherited from calling layer (may sleep).
- *
- *	RETURNS:
- *	0 on success, -errno otherwise.
- */
-int ahci_host_activate(struct ata_host *host, int irq, unsigned int n_msis)
-{
-	int i, rc;
-
-	/* Sharing Last Message among several ports is not supported */
-	if (n_msis < host->n_ports)
-		return -EINVAL;
-
-	rc = ata_host_start(host);
-	if (rc)
-		return rc;
-
-	for (i = 0; i < host->n_ports; i++) {
-		struct ahci_port_priv *pp = host->ports[i]->private_data;
-
-		/* Do not receive interrupts sent by dummy ports */
-		if (!pp) {
-			disable_irq(irq + i);
-			continue;
-		}
-
-		rc = devm_request_threaded_irq(host->dev, irq + i,
-					       ahci_hw_interrupt,
-					       ahci_thread_fn, IRQF_SHARED,
-					       pp->irq_desc, host->ports[i]);
-		if (rc)
-			goto out_free_irqs;
-	}
-
-	for (i = 0; i < host->n_ports; i++)
-		ata_port_desc(host->ports[i], "irq %d", irq + i);
-
-	rc = ata_host_register(host, &ahci_sht);
-	if (rc)
-		goto out_free_all_irqs;
-
-	return 0;
-
-out_free_all_irqs:
-	i = host->n_ports;
-out_free_irqs:
-	for (i--; i >= 0; i--)
-		devm_free_irq(host->dev, irq + i, host->ports[i]);
-
-	return rc;
-}
-
 static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	unsigned int board_id = ent->driver_data;
@@ -1296,7 +1244,7 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct device *dev = &pdev->dev;
 	struct ahci_host_priv *hpriv;
 	struct ata_host *host;
-	int n_ports, n_msis, i, rc;
+	int n_ports, i, rc;
 	int ahci_pci_bar = AHCI_PCI_BAR_STANDARD;
 
 	VPRINTK("ENTER\n");
@@ -1329,6 +1277,18 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	else if (pdev->vendor == 0x1c44 && pdev->device == 0x8000)
 		ahci_pci_bar = AHCI_PCI_BAR_ENMOTUS;
 
+	/*
+	 * The JMicron chip 361/363 contains one SATA controller and one
+	 * PATA controller,for powering on these both controllers, we must
+	 * follow the sequence one by one, otherwise one of them can not be
+	 * powered on successfully, so here we disable the async suspend
+	 * method for these chips.
+	 */
+	if (pdev->vendor == PCI_VENDOR_ID_JMICRON &&
+		(pdev->device == PCI_DEVICE_ID_JMICRON_JMB363 ||
+		pdev->device == PCI_DEVICE_ID_JMICRON_JMB361))
+		device_disable_async_suspend(&pdev->dev);
+
 	/* acquire resources */
 	rc = pcim_enable_device(pdev);
 	if (rc)
@@ -1437,9 +1397,7 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	 */
 	n_ports = max(ahci_nr_ports(hpriv->cap), fls(hpriv->port_map));
 
-	n_msis = ahci_init_interrupts(pdev, n_ports, hpriv);
-	if (n_msis > 1)
-		hpriv->flags |= AHCI_HFLAG_MULTI_MSI;
+	ahci_init_interrupts(pdev, n_ports, hpriv);
 
 	host = ata_host_alloc_pinfo(&pdev->dev, ppi, n_ports);
 	if (!host)
@@ -1491,11 +1449,7 @@ static int ahci_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	pci_set_master(pdev);
 
-	if (hpriv->flags & AHCI_HFLAG_MULTI_MSI)
-		return ahci_host_activate(host, pdev->irq, n_msis);
-
-	return ata_host_activate(host, pdev->irq, ahci_interrupt, IRQF_SHARED,
-				 &ahci_sht);
+	return ahci_host_activate(host, pdev->irq, &ahci_sht);
 }
 
 module_pci_driver(ahci_pci_driver);
diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 59ae0ee00149..40f0e34f17af 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -53,7 +53,7 @@
 
 enum {
 	AHCI_MAX_PORTS		= 32,
-	AHCI_MAX_CLKS		= 4,
+	AHCI_MAX_CLKS		= 5,
 	AHCI_MAX_SG		= 168, /* hardware max is 64K */
 	AHCI_DMA_BOUNDARY	= 0xffffffff,
 	AHCI_MAX_CMDS		= 32,
@@ -304,7 +304,7 @@ struct ahci_port_priv {
 	unsigned int		ncq_saw_d2h:1;
 	unsigned int		ncq_saw_dmas:1;
 	unsigned int		ncq_saw_sdb:1;
-	u32			intr_status;	/* interrupts to handle */
+	atomic_t		intr_status;	/* interrupts to handle */
 	spinlock_t		lock;		/* protects parent ata_port */
 	u32 			intr_mask;	/* interrupts to enable */
 	bool			fbs_supported;	/* set iff FBS is supported */
@@ -388,11 +388,9 @@ int ahci_port_resume(struct ata_port *ap);
 void ahci_set_em_messages(struct ahci_host_priv *hpriv,
 			  struct ata_port_info *pi);
 int ahci_reset_em(struct ata_host *host);
-irqreturn_t ahci_interrupt(int irq, void *dev_instance);
-irqreturn_t ahci_hw_interrupt(int irq, void *dev_instance);
-irqreturn_t ahci_thread_fn(int irq, void *dev_instance);
 void ahci_print_info(struct ata_host *host, const char *scc_s);
-int ahci_host_activate(struct ata_host *host, int irq, unsigned int n_msis);
+int ahci_host_activate(struct ata_host *host, int irq,
+		       struct scsi_host_template *sht);
 void ahci_error_handler(struct ata_port *ap);
 
 static inline void __iomem *__ahci_port_base(struct ata_host *host,
diff --git a/drivers/ata/ahci_platform.c b/drivers/ata/ahci_platform.c
index f61ddb9146d6..06f1d59fa678 100644
--- a/drivers/ata/ahci_platform.c
+++ b/drivers/ata/ahci_platform.c
@@ -32,7 +32,6 @@ static const struct ata_port_info ahci_port_info = {
 static int ahci_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
-	struct ahci_platform_data *pdata = dev_get_platdata(dev);
 	struct ahci_host_priv *hpriv;
 	int rc;
 
@@ -44,29 +43,14 @@ static int ahci_probe(struct platform_device *pdev)
 	if (rc)
 		return rc;
 
-	/*
-	 * Some platforms might need to prepare for mmio region access,
-	 * which could be done in the following init call. So, the mmio
-	 * region shouldn't be accessed before init (if provided) has
-	 * returned successfully.
-	 */
-	if (pdata && pdata->init) {
-		rc = pdata->init(dev, hpriv->mmio);
-		if (rc)
-			goto disable_resources;
-	}
-
 	if (of_device_is_compatible(dev->of_node, "hisilicon,hisi-ahci"))
 		hpriv->flags |= AHCI_HFLAG_NO_FBS | AHCI_HFLAG_NO_NCQ;
 
 	rc = ahci_platform_init_host(pdev, hpriv, &ahci_port_info);
 	if (rc)
-		goto pdata_exit;
+		goto disable_resources;
 
 	return 0;
-pdata_exit:
-	if (pdata && pdata->exit)
-		pdata->exit(dev);
 disable_resources:
 	ahci_platform_disable_resources(hpriv);
 	return rc;
diff --git a/drivers/ata/ahci_tegra.c b/drivers/ata/ahci_tegra.c
index fc3df47fca35..032904402c95 100644
--- a/drivers/ata/ahci_tegra.c
+++ b/drivers/ata/ahci_tegra.c
@@ -18,14 +18,17 @@
  */
 
 #include <linux/ahci_platform.h>
-#include <linux/reset.h>
 #include <linux/errno.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
-#include <linux/tegra-powergate.h>
 #include <linux/regulator/consumer.h>
+#include <linux/reset.h>
+
+#include <soc/tegra/fuse.h>
+#include <soc/tegra/pmc.h>
+
 #include "ahci.h"
 
 #define SATA_CONFIGURATION_0				0x180
@@ -180,9 +183,12 @@ static int tegra_ahci_controller_init(struct ahci_host_priv *hpriv)
 
 	/* Pad calibration */
 
-	/* FIXME Always use calibration 0. Change this to read the calibration
-	 * fuse once the fuse driver has landed. */
-	val = 0;
+	ret = tegra_fuse_readl(FUSE_SATA_CALIB, &val);
+	if (ret) {
+		dev_err(&tegra->pdev->dev,
+			"failed to read calibration fuse: %d\n", ret);
+		return ret;
+	}
 
 	calib = tegra124_pad_calibration[val & FUSE_SATA_CALIB_MASK];
 
diff --git a/drivers/ata/ahci_xgene.c b/drivers/ata/ahci_xgene.c
index bc281115490b..0f8538f238b6 100644
--- a/drivers/ata/ahci_xgene.c
+++ b/drivers/ata/ahci_xgene.c
@@ -78,6 +78,9 @@
 #define CFG_MEM_RAM_SHUTDOWN		0x00000070
 #define BLOCK_MEM_RDY			0x00000074
 
+/* Max retry for link down */
+#define MAX_LINK_DOWN_RETRY 3
+
 struct xgene_ahci_context {
 	struct ahci_host_priv *hpriv;
 	struct device *dev;
@@ -145,6 +148,14 @@ static unsigned int xgene_ahci_qc_issue(struct ata_queued_cmd *qc)
 	return rc;
 }
 
+static bool xgene_ahci_is_memram_inited(struct xgene_ahci_context *ctx)
+{
+	void __iomem *diagcsr = ctx->csr_diag;
+
+	return (readl(diagcsr + CFG_MEM_RAM_SHUTDOWN) == 0 &&
+	        readl(diagcsr + BLOCK_MEM_RDY) == 0xFFFFFFFF);
+}
+
 /**
  * xgene_ahci_read_id - Read ID data from the specified device
  * @dev: device
@@ -229,8 +240,11 @@ static void xgene_ahci_set_phy_cfg(struct xgene_ahci_context *ctx, int channel)
  * and Gen1 (1.5Gbps). Otherwise during long IO stress test, the PHY will
  * report disparity error and etc. In addition, during COMRESET, there can
  * be error reported in the register PORT_SCR_ERR. For SERR_DISPARITY and
- * SERR_10B_8B_ERR, the PHY receiver line must be reseted. The following
- * algorithm is followed to proper configure the hardware PHY during COMRESET:
+ * SERR_10B_8B_ERR, the PHY receiver line must be reseted. Also during long
+ * reboot cycle regression, sometimes the PHY reports link down even if the
+ * device is present because of speed negotiation failure. so need to retry
+ * the COMRESET to get the link up. The following algorithm is followed to
+ * proper configure the hardware PHY during COMRESET:
  *
  * Alg Part 1:
  * 1. Start the PHY at Gen3 speed (default setting)
@@ -246,9 +260,15 @@ static void xgene_ahci_set_phy_cfg(struct xgene_ahci_context *ctx, int channel)
  * Alg Part 2:
  * 1. On link up, if there are any SERR_DISPARITY and SERR_10B_8B_ERR error
  *    reported in the register PORT_SCR_ERR, then reset the PHY receiver line
- * 2. Go to Alg Part 3
+ * 2. Go to Alg Part 4
  *
  * Alg Part 3:
+ * 1. Check the PORT_SCR_STAT to see whether device presence detected but PHY
+ *    communication establishment failed and maximum link down attempts are
+ *    less than Max attempts 3 then goto Alg Part 1.
+ * 2. Go to Alg Part 4.
+ *
+ * Alg Part 4:
  * 1. Clear any pending from register PORT_SCR_ERR.
  *
  * NOTE: For the initial version, we will NOT support Gen1/Gen2. In addition
@@ -267,19 +287,27 @@ static int xgene_ahci_do_hardreset(struct ata_link *link,
 	u8 *d2h_fis = pp->rx_fis + RX_FIS_D2H_REG;
 	void __iomem *port_mmio = ahci_port_base(ap);
 	struct ata_taskfile tf;
+	int link_down_retry = 0;
 	int rc;
-	u32 val;
-
-	/* clear D2H reception area to properly wait for D2H FIS */
-	ata_tf_init(link->device, &tf);
-	tf.command = ATA_BUSY;
-	ata_tf_to_fis(&tf, 0, 0, d2h_fis);
-	rc = sata_link_hardreset(link, timing, deadline, online,
+	u32 val, sstatus;
+
+	do {
+		/* clear D2H reception area to properly wait for D2H FIS */
+		ata_tf_init(link->device, &tf);
+		tf.command = ATA_BUSY;
+		ata_tf_to_fis(&tf, 0, 0, d2h_fis);
+		rc = sata_link_hardreset(link, timing, deadline, online,
 				 ahci_check_ready);
+		if (*online) {
+			val = readl(port_mmio + PORT_SCR_ERR);
+			if (val & (SERR_DISPARITY | SERR_10B_8B_ERR))
+				dev_warn(ctx->dev, "link has error\n");
+			break;
+		}
 
-	val = readl(port_mmio + PORT_SCR_ERR);
-	if (val & (SERR_DISPARITY | SERR_10B_8B_ERR))
-		dev_warn(ctx->dev, "link has error\n");
+		sata_scr_read(link, SCR_STATUS, &sstatus);
+	} while (link_down_retry++ < MAX_LINK_DOWN_RETRY &&
+		 (sstatus & 0xff) == 0x1);
 
 	/* clear all errors if any pending */
 	val = readl(port_mmio + PORT_SCR_ERR);
@@ -344,7 +372,7 @@ static struct ata_port_operations xgene_ahci_ops = {
 };
 
 static const struct ata_port_info xgene_ahci_port_info = {
-	.flags = AHCI_FLAG_COMMON | ATA_FLAG_NCQ,
+	.flags = AHCI_FLAG_COMMON,
 	.pio_mask = ATA_PIO4,
 	.udma_mask = ATA_UDMA6,
 	.port_ops = &xgene_ahci_ops,
@@ -406,7 +434,7 @@ static int xgene_ahci_mux_select(struct xgene_ahci_context *ctx)
 	u32 val;
 
 	/* Check for optional MUX resource */
-	if (IS_ERR(ctx->csr_mux))
+	if (!ctx->csr_mux)
 		return 0;
 
 	val = readl(ctx->csr_mux + SATA_ENET_CONFIG_REG);
@@ -456,7 +484,13 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 
 	/* Retrieve the optional IP mux resource */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 4);
-	ctx->csr_mux = devm_ioremap_resource(dev, res);
+	if (res) {
+		void __iomem *csr = devm_ioremap_resource(dev, res);
+		if (IS_ERR(csr))
+			return PTR_ERR(csr);
+
+		ctx->csr_mux = csr;
+	}
 
 	dev_dbg(dev, "VAddr 0x%p Mmio VAddr 0x%p\n", ctx->csr_core,
 		hpriv->mmio);
@@ -467,6 +501,11 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 		return -ENODEV;
 	}
 
+	if (xgene_ahci_is_memram_inited(ctx)) {
+		dev_info(dev, "skip clock and PHY initialization\n");
+		goto skip_clk_phy;
+	}
+
 	/* Due to errata, HW requires full toggle transition */
 	rc = ahci_platform_enable_clks(hpriv);
 	if (rc)
@@ -479,8 +518,8 @@ static int xgene_ahci_probe(struct platform_device *pdev)
 
 	/* Configure the host controller */
 	xgene_ahci_hw_init(hpriv);
-
-	hpriv->flags = AHCI_HFLAG_NO_PMP | AHCI_HFLAG_YES_NCQ;
+skip_clk_phy:
+	hpriv->flags = AHCI_HFLAG_NO_PMP | AHCI_HFLAG_NO_NCQ;
 
 	rc = ahci_platform_init_host(pdev, hpriv, &xgene_ahci_port_info);
 	if (rc)
diff --git a/drivers/ata/ata_piix.c b/drivers/ata/ata_piix.c
index 893e30e9a9ef..ffbe625e6fd2 100644
--- a/drivers/ata/ata_piix.c
+++ b/drivers/ata/ata_piix.c
@@ -340,6 +340,14 @@ static const struct pci_device_id piix_pci_tbl[] = {
 	{ 0x8086, 0x0F21, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata_byt },
 	/* SATA Controller IDE (Coleto Creek) */
 	{ 0x8086, 0x23a6, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata },
+	/* SATA Controller IDE (9 Series) */
+	{ 0x8086, 0x8c88, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata_snb },
+	/* SATA Controller IDE (9 Series) */
+	{ 0x8086, 0x8c89, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_2port_sata_snb },
+	/* SATA Controller IDE (9 Series) */
+	{ 0x8086, 0x8c80, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
+	/* SATA Controller IDE (9 Series) */
+	{ 0x8086, 0x8c81, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ich8_sata_snb },
 
 	{ }	/* terminate list */
 };
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index b784e9de426a..5eb61c9e63da 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -1778,30 +1778,28 @@ static void ahci_handle_port_interrupt(struct ata_port *ap,
 	}
 }
 
-static void ahci_port_intr(struct ata_port *ap)
+static void ahci_update_intr_status(struct ata_port *ap)
 {
 	void __iomem *port_mmio = ahci_port_base(ap);
+	struct ahci_port_priv *pp = ap->private_data;
 	u32 status;
 
 	status = readl(port_mmio + PORT_IRQ_STAT);
 	writel(status, port_mmio + PORT_IRQ_STAT);
 
-	ahci_handle_port_interrupt(ap, port_mmio, status);
+	atomic_or(status, &pp->intr_status);
 }
 
-irqreturn_t ahci_thread_fn(int irq, void *dev_instance)
+static irqreturn_t ahci_port_thread_fn(int irq, void *dev_instance)
 {
 	struct ata_port *ap = dev_instance;
 	struct ahci_port_priv *pp = ap->private_data;
 	void __iomem *port_mmio = ahci_port_base(ap);
-	unsigned long flags;
 	u32 status;
 
-	spin_lock_irqsave(&ap->host->lock, flags);
-	status = pp->intr_status;
-	if (status)
-		pp->intr_status = 0;
-	spin_unlock_irqrestore(&ap->host->lock, flags);
+	status = atomic_xchg(&pp->intr_status, 0);
+	if (!status)
+		return IRQ_NONE;
 
 	spin_lock_bh(ap->lock);
 	ahci_handle_port_interrupt(ap, port_mmio, status);
@@ -1809,47 +1807,13 @@ irqreturn_t ahci_thread_fn(int irq, void *dev_instance)
 
 	return IRQ_HANDLED;
 }
-EXPORT_SYMBOL_GPL(ahci_thread_fn);
-
-static void ahci_hw_port_interrupt(struct ata_port *ap)
-{
-	void __iomem *port_mmio = ahci_port_base(ap);
-	struct ahci_port_priv *pp = ap->private_data;
-	u32 status;
-
-	status = readl(port_mmio + PORT_IRQ_STAT);
-	writel(status, port_mmio + PORT_IRQ_STAT);
 
-	pp->intr_status |= status;
-}
-
-irqreturn_t ahci_hw_interrupt(int irq, void *dev_instance)
+irqreturn_t ahci_thread_fn(int irq, void *dev_instance)
 {
-	struct ata_port *ap_this = dev_instance;
-	struct ahci_port_priv *pp = ap_this->private_data;
-	struct ata_host *host = ap_this->host;
+	struct ata_host *host = dev_instance;
 	struct ahci_host_priv *hpriv = host->private_data;
-	void __iomem *mmio = hpriv->mmio;
+	u32 irq_masked = hpriv->port_map;
 	unsigned int i;
-	u32 irq_stat, irq_masked;
-
-	VPRINTK("ENTER\n");
-
-	spin_lock(&host->lock);
-
-	irq_stat = readl(mmio + HOST_IRQ_STAT);
-
-	if (!irq_stat) {
-		u32 status = pp->intr_status;
-
-		spin_unlock(&host->lock);
-
-		VPRINTK("EXIT\n");
-
-		return status ? IRQ_WAKE_THREAD : IRQ_NONE;
-	}
-
-	irq_masked = irq_stat & hpriv->port_map;
 
 	for (i = 0; i < host->n_ports; i++) {
 		struct ata_port *ap;
@@ -1859,7 +1823,7 @@ irqreturn_t ahci_hw_interrupt(int irq, void *dev_instance)
 
 		ap = host->ports[i];
 		if (ap) {
-			ahci_hw_port_interrupt(ap);
+			ahci_port_thread_fn(irq, ap);
 			VPRINTK("port %u\n", i);
 		} else {
 			VPRINTK("port %u (no irq)\n", i);
@@ -1869,17 +1833,29 @@ irqreturn_t ahci_hw_interrupt(int irq, void *dev_instance)
 		}
 	}
 
-	writel(irq_stat, mmio + HOST_IRQ_STAT);
+	return IRQ_HANDLED;
+}
 
-	spin_unlock(&host->lock);
+static irqreturn_t ahci_multi_irqs_intr(int irq, void *dev_instance)
+{
+	struct ata_port *ap = dev_instance;
+	void __iomem *port_mmio = ahci_port_base(ap);
+	struct ahci_port_priv *pp = ap->private_data;
+	u32 status;
+
+	VPRINTK("ENTER\n");
+
+	status = readl(port_mmio + PORT_IRQ_STAT);
+	writel(status, port_mmio + PORT_IRQ_STAT);
+
+	atomic_or(status, &pp->intr_status);
 
 	VPRINTK("EXIT\n");
 
 	return IRQ_WAKE_THREAD;
 }
-EXPORT_SYMBOL_GPL(ahci_hw_interrupt);
 
-irqreturn_t ahci_interrupt(int irq, void *dev_instance)
+static irqreturn_t ahci_single_irq_intr(int irq, void *dev_instance)
 {
 	struct ata_host *host = dev_instance;
 	struct ahci_host_priv *hpriv;
@@ -1899,8 +1875,6 @@ irqreturn_t ahci_interrupt(int irq, void *dev_instance)
 
 	irq_masked = irq_stat & hpriv->port_map;
 
-	spin_lock(&host->lock);
-
 	for (i = 0; i < host->n_ports; i++) {
 		struct ata_port *ap;
 
@@ -1909,7 +1883,7 @@ irqreturn_t ahci_interrupt(int irq, void *dev_instance)
 
 		ap = host->ports[i];
 		if (ap) {
-			ahci_port_intr(ap);
+			ahci_update_intr_status(ap);
 			VPRINTK("port %u\n", i);
 		} else {
 			VPRINTK("port %u (no irq)\n", i);
@@ -1932,13 +1906,10 @@ irqreturn_t ahci_interrupt(int irq, void *dev_instance)
 	 */
 	writel(irq_stat, mmio + HOST_IRQ_STAT);
 
-	spin_unlock(&host->lock);
-
 	VPRINTK("EXIT\n");
 
-	return IRQ_RETVAL(handled);
+	return handled ? IRQ_WAKE_THREAD : IRQ_NONE;
 }
-EXPORT_SYMBOL_GPL(ahci_interrupt);
 
 unsigned int ahci_qc_issue(struct ata_queued_cmd *qc)
 {
@@ -2349,13 +2320,8 @@ static int ahci_port_start(struct ata_port *ap)
 	 */
 	pp->intr_mask = DEF_PORT_IRQ;
 
-	/*
-	 * Switch to per-port locking in case each port has its own MSI vector.
-	 */
-	if ((hpriv->flags & AHCI_HFLAG_MULTI_MSI)) {
-		spin_lock_init(&pp->lock);
-		ap->lock = &pp->lock;
-	}
+	spin_lock_init(&pp->lock);
+	ap->lock = &pp->lock;
 
 	ap->private_data = pp;
 
@@ -2472,6 +2438,105 @@ void ahci_set_em_messages(struct ahci_host_priv *hpriv,
 }
 EXPORT_SYMBOL_GPL(ahci_set_em_messages);
 
+static int ahci_host_activate_multi_irqs(struct ata_host *host, int irq,
+					 struct scsi_host_template *sht)
+{
+	int i, rc;
+
+	rc = ata_host_start(host);
+	if (rc)
+		return rc;
+
+	for (i = 0; i < host->n_ports; i++) {
+		struct ahci_port_priv *pp = host->ports[i]->private_data;
+
+		/* Do not receive interrupts sent by dummy ports */
+		if (!pp) {
+			disable_irq(irq + i);
+			continue;
+		}
+
+		rc = devm_request_threaded_irq(host->dev, irq + i,
+					       ahci_multi_irqs_intr,
+					       ahci_port_thread_fn, IRQF_SHARED,
+					       pp->irq_desc, host->ports[i]);
+		if (rc)
+			goto out_free_irqs;
+	}
+
+	for (i = 0; i < host->n_ports; i++)
+		ata_port_desc(host->ports[i], "irq %d", irq + i);
+
+	rc = ata_host_register(host, sht);
+	if (rc)
+		goto out_free_all_irqs;
+
+	return 0;
+
+out_free_all_irqs:
+	i = host->n_ports;
+out_free_irqs:
+	for (i--; i >= 0; i--)
+		devm_free_irq(host->dev, irq + i, host->ports[i]);
+
+	return rc;
+}
+
+static int ahci_host_activate_single_irq(struct ata_host *host, int irq,
+					 struct scsi_host_template *sht)
+{
+	int i, rc;
+
+	rc = ata_host_start(host);
+	if (rc)
+		return rc;
+
+	rc = devm_request_threaded_irq(host->dev, irq, ahci_single_irq_intr,
+				       ahci_thread_fn, IRQF_SHARED,
+				       dev_driver_string(host->dev), host);
+	if (rc)
+		return rc;
+
+	for (i = 0; i < host->n_ports; i++)
+		ata_port_desc(host->ports[i], "irq %d", irq);
+
+	rc = ata_host_register(host, sht);
+	if (rc)
+		devm_free_irq(host->dev, irq, host);
+
+	return rc;
+}
+
+/**
+ *	ahci_host_activate - start AHCI host, request IRQs and register it
+ *	@host: target ATA host
+ *	@irq: base IRQ number to request
+ *	@sht: scsi_host_template to use when registering the host
+ *
+ *	Similar to ata_host_activate, but requests IRQs according to AHCI-1.1
+ *	when multiple MSIs were allocated. That is one MSI per port, starting
+ *	from @irq.
+ *
+ *	LOCKING:
+ *	Inherited from calling layer (may sleep).
+ *
+ *	RETURNS:
+ *	0 on success, -errno otherwise.
+ */
+int ahci_host_activate(struct ata_host *host, int irq,
+		       struct scsi_host_template *sht)
+{
+	struct ahci_host_priv *hpriv = host->private_data;
+	int rc;
+
+	if (hpriv->flags & AHCI_HFLAG_MULTI_MSI)
+		rc = ahci_host_activate_multi_irqs(host, irq, sht);
+	else
+		rc = ahci_host_activate_single_irq(host, irq, sht);
+	return rc;
+}
+EXPORT_SYMBOL_GPL(ahci_host_activate);
+
 MODULE_AUTHOR("Jeff Garzik");
 MODULE_DESCRIPTION("Common AHCI SATA low-level routines");
 MODULE_LICENSE("GPL");
diff --git a/drivers/ata/libahci_platform.c b/drivers/ata/libahci_platform.c
index 5b92c290e6c6..0b03f9056692 100644
--- a/drivers/ata/libahci_platform.c
+++ b/drivers/ata/libahci_platform.c
@@ -49,7 +49,7 @@ static struct scsi_host_template ahci_platform_sht = {
  * RETURNS:
  * 0 on success otherwise a negative error code
  */
-int ahci_platform_enable_phys(struct ahci_host_priv *hpriv)
+static int ahci_platform_enable_phys(struct ahci_host_priv *hpriv)
 {
 	int rc, i;
 
@@ -77,7 +77,6 @@ disable_phys:
 	}
 	return rc;
 }
-EXPORT_SYMBOL_GPL(ahci_platform_enable_phys);
 
 /**
  * ahci_platform_disable_phys - Disable PHYs
@@ -85,7 +84,7 @@ EXPORT_SYMBOL_GPL(ahci_platform_enable_phys);
  *
  * This function disables all PHYs found in hpriv->phys.
  */
-void ahci_platform_disable_phys(struct ahci_host_priv *hpriv)
+static void ahci_platform_disable_phys(struct ahci_host_priv *hpriv)
 {
 	int i;
 
@@ -97,7 +96,6 @@ void ahci_platform_disable_phys(struct ahci_host_priv *hpriv)
 		phy_exit(hpriv->phys[i]);
 	}
 }
-EXPORT_SYMBOL_GPL(ahci_platform_disable_phys);
 
 /**
  * ahci_platform_enable_clks - Enable platform clocks
@@ -495,20 +493,14 @@ int ahci_platform_init_host(struct platform_device *pdev,
 	ahci_init_controller(host);
 	ahci_print_info(host, "platform");
 
-	return ata_host_activate(host, irq, ahci_interrupt, IRQF_SHARED,
-				 &ahci_platform_sht);
+	return ahci_host_activate(host, irq, &ahci_platform_sht);
 }
 EXPORT_SYMBOL_GPL(ahci_platform_init_host);
 
 static void ahci_host_stop(struct ata_host *host)
 {
-	struct device *dev = host->dev;
-	struct ahci_platform_data *pdata = dev_get_platdata(dev);
 	struct ahci_host_priv *hpriv = host->private_data;
 
-	if (pdata && pdata->exit)
-		pdata->exit(dev);
-
 	ahci_platform_disable_resources(hpriv);
 }
 
@@ -592,7 +584,6 @@ EXPORT_SYMBOL_GPL(ahci_platform_resume_host);
  */
 int ahci_platform_suspend(struct device *dev)
 {
-	struct ahci_platform_data *pdata = dev_get_platdata(dev);
 	struct ata_host *host = dev_get_drvdata(dev);
 	struct ahci_host_priv *hpriv = host->private_data;
 	int rc;
@@ -601,19 +592,9 @@ int ahci_platform_suspend(struct device *dev)
 	if (rc)
 		return rc;
 
-	if (pdata && pdata->suspend) {
-		rc = pdata->suspend(dev);
-		if (rc)
-			goto resume_host;
-	}
-
 	ahci_platform_disable_resources(hpriv);
 
 	return 0;
-
-resume_host:
-	ahci_platform_resume_host(dev);
-	return rc;
 }
 EXPORT_SYMBOL_GPL(ahci_platform_suspend);
 
@@ -629,7 +610,6 @@ EXPORT_SYMBOL_GPL(ahci_platform_suspend);
  */
 int ahci_platform_resume(struct device *dev)
 {
-	struct ahci_platform_data *pdata = dev_get_platdata(dev);
 	struct ata_host *host = dev_get_drvdata(dev);
 	struct ahci_host_priv *hpriv = host->private_data;
 	int rc;
@@ -638,12 +618,6 @@ int ahci_platform_resume(struct device *dev)
 	if (rc)
 		return rc;
 
-	if (pdata && pdata->resume) {
-		rc = pdata->resume(dev);
-		if (rc)
-			goto disable_resources;
-	}
-
 	rc = ahci_platform_resume_host(dev);
 	if (rc)
 		goto disable_resources;
diff --git a/drivers/ata/libata-core.c b/drivers/ata/libata-core.c
index dbdc5d32343f..c5ba15af87d3 100644
--- a/drivers/ata/libata-core.c
+++ b/drivers/ata/libata-core.c
@@ -4228,7 +4228,7 @@ static const struct ata_blacklist_entry ata_device_blacklist [] = {
 	{ "Micron_M500*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
 	{ "Crucial_CT???M500SSD*",	NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
 	{ "Micron_M550*",		NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
-	{ "Crucial_CT???M550SSD*",	NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
+	{ "Crucial_CT*M550SSD*",	NULL,	ATA_HORKAGE_NO_NCQ_TRIM, },
 
 	/*
 	 * Some WD SATA-I drives spin up and down erratically when the link
@@ -4261,10 +4261,10 @@ static unsigned long ata_dev_blacklisted(const struct ata_device *dev)
 	ata_id_c_string(dev->id, model_rev, ATA_ID_FW_REV, sizeof(model_rev));
 
 	while (ad->model_num) {
-		if (glob_match(model_num, ad->model_num)) {
+		if (glob_match(ad->model_num, model_num)) {
 			if (ad->model_rev == NULL)
 				return ad->horkage;
-			if (glob_match(model_rev, ad->model_rev))
+			if (glob_match(ad->model_rev, model_rev))
 				return ad->horkage;
 		}
 		ad++;
@@ -6227,7 +6227,7 @@ int ata_host_activate(struct ata_host *host, int irq,
 	}
 
 	rc = devm_request_irq(host->dev, irq, irq_handler, irq_flags,
-			      dev_driver_string(host->dev), host);
+			      dev_name(host->dev), host);
 	if (rc)
 		return rc;
 
@@ -6772,32 +6772,28 @@ const struct ata_port_info ata_dummy_port_info = {
 /*
  * Utility print functions
  */
-int ata_port_printk(const struct ata_port *ap, const char *level,
-		    const char *fmt, ...)
+void ata_port_printk(const struct ata_port *ap, const char *level,
+		     const char *fmt, ...)
 {
 	struct va_format vaf;
 	va_list args;
-	int r;
 
 	va_start(args, fmt);
 
 	vaf.fmt = fmt;
 	vaf.va = &args;
 
-	r = printk("%sata%u: %pV", level, ap->print_id, &vaf);
+	printk("%sata%u: %pV", level, ap->print_id, &vaf);
 
 	va_end(args);
-
-	return r;
 }
 EXPORT_SYMBOL(ata_port_printk);
 
-int ata_link_printk(const struct ata_link *link, const char *level,
-		    const char *fmt, ...)
+void ata_link_printk(const struct ata_link *link, const char *level,
+		     const char *fmt, ...)
 {
 	struct va_format vaf;
 	va_list args;
-	int r;
 
 	va_start(args, fmt);
 
@@ -6805,37 +6801,32 @@ int ata_link_printk(const struct ata_link *link, const char *level,
 	vaf.va = &args;
 
 	if (sata_pmp_attached(link->ap) || link->ap->slave_link)
-		r = printk("%sata%u.%02u: %pV",
-			   level, link->ap->print_id, link->pmp, &vaf);
+		printk("%sata%u.%02u: %pV",
+		       level, link->ap->print_id, link->pmp, &vaf);
 	else
-		r = printk("%sata%u: %pV",
-			   level, link->ap->print_id, &vaf);
+		printk("%sata%u: %pV",
+		       level, link->ap->print_id, &vaf);
 
 	va_end(args);
-
-	return r;
 }
 EXPORT_SYMBOL(ata_link_printk);
 
-int ata_dev_printk(const struct ata_device *dev, const char *level,
+void ata_dev_printk(const struct ata_device *dev, const char *level,
 		    const char *fmt, ...)
 {
 	struct va_format vaf;
 	va_list args;
-	int r;
 
 	va_start(args, fmt);
 
 	vaf.fmt = fmt;
 	vaf.va = &args;
 
-	r = printk("%sata%u.%02u: %pV",
-		   level, dev->link->ap->print_id, dev->link->pmp + dev->devno,
-		   &vaf);
+	printk("%sata%u.%02u: %pV",
+	       level, dev->link->ap->print_id, dev->link->pmp + dev->devno,
+	       &vaf);
 
 	va_end(args);
-
-	return r;
 }
 EXPORT_SYMBOL(ata_dev_printk);
 
diff --git a/drivers/ata/libata-sff.c b/drivers/ata/libata-sff.c
index 1121153f1ecd..db90aa35cb71 100644
--- a/drivers/ata/libata-sff.c
+++ b/drivers/ata/libata-sff.c
@@ -2008,13 +2008,15 @@ static int ata_bus_softreset(struct ata_port *ap, unsigned int devmask,
 
 	DPRINTK("ata%u: bus reset via SRST\n", ap->print_id);
 
-	/* software reset.  causes dev0 to be selected */
-	iowrite8(ap->ctl, ioaddr->ctl_addr);
-	udelay(20);	/* FIXME: flush */
-	iowrite8(ap->ctl | ATA_SRST, ioaddr->ctl_addr);
-	udelay(20);	/* FIXME: flush */
-	iowrite8(ap->ctl, ioaddr->ctl_addr);
-	ap->last_ctl = ap->ctl;
+	if (ap->ioaddr.ctl_addr) {
+		/* software reset.  causes dev0 to be selected */
+		iowrite8(ap->ctl, ioaddr->ctl_addr);
+		udelay(20);	/* FIXME: flush */
+		iowrite8(ap->ctl | ATA_SRST, ioaddr->ctl_addr);
+		udelay(20);	/* FIXME: flush */
+		iowrite8(ap->ctl, ioaddr->ctl_addr);
+		ap->last_ctl = ap->ctl;
+	}
 
 	/* wait the port to become ready */
 	return ata_sff_wait_after_reset(&ap->link, devmask, deadline);
@@ -2215,10 +2217,6 @@ void ata_sff_error_handler(struct ata_port *ap)
 
 	spin_unlock_irqrestore(ap->lock, flags);
 
-	/* ignore ata_sff_softreset if ctl isn't accessible */
-	if (softreset == ata_sff_softreset && !ap->ioaddr.ctl_addr)
-		softreset = NULL;
-
 	/* ignore built-in hardresets if SCR access is not available */
 	if ((hardreset == sata_std_hardreset ||
 	     hardreset == sata_sff_hardreset) && !sata_scr_valid(&ap->link))
diff --git a/drivers/ata/pata_arasan_cf.c b/drivers/ata/pata_arasan_cf.c
index 4edb1a81f63f..38216b991474 100644
--- a/drivers/ata/pata_arasan_cf.c
+++ b/drivers/ata/pata_arasan_cf.c
@@ -420,7 +420,7 @@ dma_xfer(struct arasan_cf_dev *acdev, dma_addr_t src, dma_addr_t dest, u32 len)
 
 	/* Wait for DMA to complete */
 	if (!wait_for_completion_timeout(&acdev->dma_completion, TIMEOUT)) {
-		chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(chan);
 		dev_err(acdev->host->dev, "wait_for_completion_timeout\n");
 		return -ETIMEDOUT;
 	}
@@ -928,8 +928,7 @@ static int arasan_cf_suspend(struct device *dev)
 	struct arasan_cf_dev *acdev = host->ports[0]->private_data;
 
 	if (acdev->dma_chan)
-		acdev->dma_chan->device->device_control(acdev->dma_chan,
-				DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(acdev->dma_chan);
 
 	cf_exit(acdev);
 	return ata_host_suspend(host, PMSG_SUSPEND);
diff --git a/drivers/ata/pata_imx.c b/drivers/ata/pata_imx.c
index af424573c2ff..989ff5ac69ec 100644
--- a/drivers/ata/pata_imx.c
+++ b/drivers/ata/pata_imx.c
@@ -221,13 +221,10 @@ static int pata_imx_resume(struct device *dev)
 
 	return 0;
 }
-
-static const struct dev_pm_ops pata_imx_pm_ops = {
-	.suspend	= pata_imx_suspend,
-	.resume		= pata_imx_resume,
-};
 #endif
 
+static SIMPLE_DEV_PM_OPS(pata_imx_pm_ops, pata_imx_suspend, pata_imx_resume);
+
 static const struct of_device_id imx_pata_dt_ids[] = {
 	{
 		.compatible = "fsl,imx27-pata",
@@ -244,9 +241,7 @@ static struct platform_driver pata_imx_driver = {
 		.name		= DRV_NAME,
 		.of_match_table	= imx_pata_dt_ids,
 		.owner		= THIS_MODULE,
-#ifdef CONFIG_PM_SLEEP
 		.pm		= &pata_imx_pm_ops,
-#endif
 	},
 };
 
diff --git a/drivers/ata/pata_jmicron.c b/drivers/ata/pata_jmicron.c
index 4d1a5d2c4287..47e418b8c8ba 100644
--- a/drivers/ata/pata_jmicron.c
+++ b/drivers/ata/pata_jmicron.c
@@ -143,6 +143,18 @@ static int jmicron_init_one (struct pci_dev *pdev, const struct pci_device_id *i
 	};
 	const struct ata_port_info *ppi[] = { &info, NULL };
 
+	/*
+	 * The JMicron chip 361/363 contains one SATA controller and one
+	 * PATA controller,for powering on these both controllers, we must
+	 * follow the sequence one by one, otherwise one of them can not be
+	 * powered on successfully, so here we disable the async suspend
+	 * method for these chips.
+	 */
+	if (pdev->vendor == PCI_VENDOR_ID_JMICRON &&
+		(pdev->device == PCI_DEVICE_ID_JMICRON_JMB363 ||
+		pdev->device == PCI_DEVICE_ID_JMICRON_JMB361))
+		device_disable_async_suspend(&pdev->dev);
+
 	return ata_pci_bmdma_init_one(pdev, ppi, &jmicron_sht, NULL, 0);
 }
 
diff --git a/drivers/ata/pata_of_platform.c b/drivers/ata/pata_of_platform.c
index a7e95a54c782..64965398914a 100644
--- a/drivers/ata/pata_of_platform.c
+++ b/drivers/ata/pata_of_platform.c
@@ -35,25 +35,14 @@ static int pata_of_platform_probe(struct platform_device *ofdev)
 		return -EINVAL;
 	}
 
-	if (of_device_is_compatible(dn, "electra-ide")) {
-		/* Altstatus is really at offset 0x3f6 from the primary window
-		 * on electra-ide. Adjust ctl_res and io_res accordingly.
-		 */
-		ctl_res = io_res;
-		ctl_res.start = ctl_res.start+0x3f6;
-		io_res.end = ctl_res.start-1;
-	} else {
-		ret = of_address_to_resource(dn, 1, &ctl_res);
-		if (ret) {
-			dev_err(&ofdev->dev, "can't get CTL address from "
-				"device tree\n");
-			return -EINVAL;
-		}
+	ret = of_address_to_resource(dn, 1, &ctl_res);
+	if (ret) {
+		dev_err(&ofdev->dev, "can't get CTL address from "
+			"device tree\n");
+		return -EINVAL;
 	}
 
 	irq_res = platform_get_resource(ofdev, IORESOURCE_IRQ, 0);
-	if (irq_res)
-		irq_res->flags = 0;
 
 	prop = of_get_property(dn, "reg-shift", NULL);
 	if (prop)
@@ -79,8 +68,7 @@ static int pata_of_platform_probe(struct platform_device *ofdev)
 
 static struct of_device_id pata_of_platform_match[] = {
 	{ .compatible = "ata-generic", },
-	{ .compatible = "electra-ide", },
-	{},
+	{ },
 };
 MODULE_DEVICE_TABLE(of, pata_of_platform_match);
 
diff --git a/drivers/ata/pata_platform.c b/drivers/ata/pata_platform.c
index a5579b55e332..f8cff3e247c5 100644
--- a/drivers/ata/pata_platform.c
+++ b/drivers/ata/pata_platform.c
@@ -118,7 +118,7 @@ int __pata_platform_probe(struct device *dev, struct resource *io_res,
 	 */
 	if (irq_res && irq_res->start > 0) {
 		irq = irq_res->start;
-		irq_flags = irq_res->flags;
+		irq_flags = irq_res->flags & IRQF_TRIGGER_MASK;
 	}
 
 	/*
@@ -213,8 +213,6 @@ static int pata_platform_probe(struct platform_device *pdev)
 	 * And the IRQ
 	 */
 	irq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (irq_res)
-		irq_res->flags = pp_info ? pp_info->irq_flags : 0;
 
 	return __pata_platform_probe(&pdev->dev, io_res, ctl_res, irq_res,
 				     pp_info ? pp_info->ioport_shift : 0,
diff --git a/drivers/ata/pata_samsung_cf.c b/drivers/ata/pata_samsung_cf.c
index 2578fc16960a..1a24a5dc3940 100644
--- a/drivers/ata/pata_samsung_cf.c
+++ b/drivers/ata/pata_samsung_cf.c
@@ -360,7 +360,7 @@ static int pata_s3c_wait_after_reset(struct ata_link *link,
 /*
  * pata_s3c_bus_softreset - PATA device software reset
  */
-static unsigned int pata_s3c_bus_softreset(struct ata_port *ap,
+static int pata_s3c_bus_softreset(struct ata_port *ap,
 		unsigned long deadline)
 {
 	struct ata_ioports *ioaddr = &ap->ioaddr;
diff --git a/drivers/ata/pata_scc.c b/drivers/ata/pata_scc.c
index 4e006d74bef8..7f4cb76ed9fa 100644
--- a/drivers/ata/pata_scc.c
+++ b/drivers/ata/pata_scc.c
@@ -585,7 +585,7 @@ static int scc_wait_after_reset(struct ata_link *link, unsigned int devmask,
  *	Note: Original code is ata_bus_softreset().
  */
 
-static unsigned int scc_bus_softreset(struct ata_port *ap, unsigned int devmask,
+static int scc_bus_softreset(struct ata_port *ap, unsigned int devmask,
                                       unsigned long deadline)
 {
 	struct ata_ioports *ioaddr = &ap->ioaddr;
@@ -599,9 +599,7 @@ static unsigned int scc_bus_softreset(struct ata_port *ap, unsigned int devmask,
 	udelay(20);
 	out_be32(ioaddr->ctl_addr, ap->ctl);
 
-	scc_wait_after_reset(&ap->link, devmask, deadline);
-
-	return 0;
+	return scc_wait_after_reset(&ap->link, devmask, deadline);
 }
 
 /**
@@ -618,7 +616,8 @@ static int scc_softreset(struct ata_link *link, unsigned int *classes,
 {
 	struct ata_port *ap = link->ap;
 	unsigned int slave_possible = ap->flags & ATA_FLAG_SLAVE_POSS;
-	unsigned int devmask = 0, err_mask;
+	unsigned int devmask = 0;
+	int rc;
 	u8 err;
 
 	DPRINTK("ENTER\n");
@@ -634,9 +633,9 @@ static int scc_softreset(struct ata_link *link, unsigned int *classes,
 
 	/* issue bus reset */
 	DPRINTK("about to softreset, devmask=%x\n", devmask);
-	err_mask = scc_bus_softreset(ap, devmask, deadline);
-	if (err_mask) {
-		ata_port_err(ap, "SRST failed (err_mask=0x%x)\n", err_mask);
+	rc = scc_bus_softreset(ap, devmask, deadline);
+	if (rc) {
+		ata_port_err(ap, "SRST failed (err_mask=0x%x)\n", rc);
 		return -EIO;
 	}
 
diff --git a/drivers/ata/pata_serverworks.c b/drivers/ata/pata_serverworks.c
index fc5f31d4828e..57de02123c4c 100644
--- a/drivers/ata/pata_serverworks.c
+++ b/drivers/ata/pata_serverworks.c
@@ -251,12 +251,18 @@ static void serverworks_set_dmamode(struct ata_port *ap, struct ata_device *adev
 	pci_write_config_byte(pdev, 0x54, ultra_cfg);
 }
 
-static struct scsi_host_template serverworks_sht = {
+static struct scsi_host_template serverworks_osb4_sht = {
+	ATA_BMDMA_SHT(DRV_NAME),
+	.sg_tablesize	= LIBATA_DUMB_MAX_PRD,
+};
+
+static struct scsi_host_template serverworks_csb_sht = {
 	ATA_BMDMA_SHT(DRV_NAME),
 };
 
 static struct ata_port_operations serverworks_osb4_port_ops = {
 	.inherits	= &ata_bmdma_port_ops,
+	.qc_prep	= ata_bmdma_dumb_qc_prep,
 	.cable_detect	= serverworks_cable_detect,
 	.mode_filter	= serverworks_osb4_filter,
 	.set_piomode	= serverworks_set_piomode,
@@ -265,6 +271,7 @@ static struct ata_port_operations serverworks_osb4_port_ops = {
 
 static struct ata_port_operations serverworks_csb_port_ops = {
 	.inherits	= &serverworks_osb4_port_ops,
+	.qc_prep	= ata_bmdma_qc_prep,
 	.mode_filter	= serverworks_csb_filter,
 };
 
@@ -404,6 +411,7 @@ static int serverworks_init_one(struct pci_dev *pdev, const struct pci_device_id
 		}
 	};
 	const struct ata_port_info *ppi[] = { &info[id->driver_data], NULL };
+	struct scsi_host_template *sht = &serverworks_csb_sht;
 	int rc;
 
 	rc = pcim_enable_device(pdev);
@@ -417,6 +425,7 @@ static int serverworks_init_one(struct pci_dev *pdev, const struct pci_device_id
 		/* Select non UDMA capable OSB4 if we can't do fixups */
 		if (rc < 0)
 			ppi[0] = &info[1];
+		sht = &serverworks_osb4_sht;
 	}
 	/* setup CSB5/CSB6 : South Bridge and IDE option RAID */
 	else if ((pdev->device == PCI_DEVICE_ID_SERVERWORKS_CSB5IDE) ||
@@ -433,7 +442,7 @@ static int serverworks_init_one(struct pci_dev *pdev, const struct pci_device_id
 			ppi[1] = &ata_dummy_port_info;
 	}
 
-	return ata_pci_bmdma_init_one(pdev, ppi, &serverworks_sht, NULL, 0);
+	return ata_pci_bmdma_init_one(pdev, ppi, sht, NULL, 0);
 }
 
 #ifdef CONFIG_PM_SLEEP
diff --git a/drivers/ata/sata_highbank.c b/drivers/ata/sata_highbank.c
index da3bc2709c63..ce2b99a1ed70 100644
--- a/drivers/ata/sata_highbank.c
+++ b/drivers/ata/sata_highbank.c
@@ -568,8 +568,7 @@ static int ahci_highbank_probe(struct platform_device *pdev)
 	ahci_init_controller(host);
 	ahci_print_info(host, "platform");
 
-	rc = ata_host_activate(host, irq, ahci_interrupt, 0,
-					&ahci_highbank_platform_sht);
+	rc = ahci_host_activate(host, irq, &ahci_highbank_platform_sht);
 	if (rc)
 		goto err0;
 
diff --git a/drivers/atm/lanai.c b/drivers/atm/lanai.c
index fa7d701933ba..93eaf8d94492 100644
--- a/drivers/atm/lanai.c
+++ b/drivers/atm/lanai.c
@@ -2614,27 +2614,7 @@ static struct pci_driver lanai_driver = {
 	.probe    = lanai_init_one,
 };
 
-static int __init lanai_module_init(void)
-{
-	int x;
-
-	x = pci_register_driver(&lanai_driver);
-	if (x != 0)
-		printk(KERN_ERR DEV_LABEL ": no adapter found\n");
-	return x;
-}
-
-static void __exit lanai_module_exit(void)
-{
-	/* We'll only get called when all the interfaces are already
-	 * gone, so there isn't much to do
-	 */
-	DPRINTK("cleanup_module()\n");
-	pci_unregister_driver(&lanai_driver);
-}
-
-module_init(lanai_module_init);
-module_exit(lanai_module_exit);
+module_pci_driver(lanai_driver);
 
 MODULE_AUTHOR("Mitchell Blank Jr <mitch@sfgoth.com>");
 MODULE_DESCRIPTION("Efficient Networks Speedstream 3010 driver");
diff --git a/drivers/base/Kconfig b/drivers/base/Kconfig
index 4e7f0ff83ae7..61a33f4ba608 100644
--- a/drivers/base/Kconfig
+++ b/drivers/base/Kconfig
@@ -165,6 +165,27 @@ config FW_LOADER_USER_HELPER_FALLBACK
 
 	  If you are unsure about this, say N here.
 
+config WANT_DEV_COREDUMP
+	bool
+	help
+	  Drivers should "select" this option if they desire to use the
+	  device coredump mechanism.
+
+config DISABLE_DEV_COREDUMP
+	bool "Disable device coredump" if EXPERT
+	help
+	  Disable the device coredump mechanism despite drivers wanting to
+	  use it; this allows for more sensitive systems or systems that
+	  don't want to ever access the information to not have the code,
+	  nor keep any data.
+
+	  If unsure, say N.
+
+config DEV_COREDUMP
+	bool
+	default y if WANT_DEV_COREDUMP
+	depends on !DISABLE_DEV_COREDUMP
+
 config DEBUG_DRIVER
 	bool "Driver Core verbose debug messages"
 	depends on DEBUG_KERNEL
@@ -231,6 +252,9 @@ config DMA_CMA
 	  to allocate big physically-contiguous blocks of memory for use with
 	  hardware components that do not support I/O map nor scatter-gather.
 
+	  You can disable CMA by specifying "cma=0" on the kernel's command
+	  line.
+
 	  For more information see <include/linux/dma-contiguous.h>.
 	  If unsure, say "n".
 
diff --git a/drivers/base/Makefile b/drivers/base/Makefile
index 4aab26ec0292..6922cd6850a2 100644
--- a/drivers/base/Makefile
+++ b/drivers/base/Makefile
@@ -21,6 +21,7 @@ obj-$(CONFIG_SYS_HYPERVISOR) += hypervisor.o
 obj-$(CONFIG_REGMAP)	+= regmap/
 obj-$(CONFIG_SOC_BUS) += soc.o
 obj-$(CONFIG_PINCTRL) += pinctrl.o
+obj-$(CONFIG_DEV_COREDUMP) += devcoredump.o
 
 ccflags-$(CONFIG_DEBUG_DRIVER) := -DDEBUG
 
diff --git a/drivers/base/attribute_container.c b/drivers/base/attribute_container.c
index b84ca8f13f9e..3ead3af4be61 100644
--- a/drivers/base/attribute_container.c
+++ b/drivers/base/attribute_container.c
@@ -74,9 +74,9 @@ int
 attribute_container_register(struct attribute_container *cont)
 {
 	INIT_LIST_HEAD(&cont->node);
-	klist_init(&cont->containers,internal_container_klist_get,
+	klist_init(&cont->containers, internal_container_klist_get,
 		   internal_container_klist_put);
-		
+
 	mutex_lock(&attribute_container_mutex);
 	list_add_tail(&cont->node, &attribute_container_list);
 	mutex_unlock(&attribute_container_mutex);
@@ -104,14 +104,14 @@ attribute_container_unregister(struct attribute_container *cont)
 	spin_unlock(&cont->containers.k_lock);
 	mutex_unlock(&attribute_container_mutex);
 	return retval;
-		
+
 }
 EXPORT_SYMBOL_GPL(attribute_container_unregister);
 
 /* private function used as class release */
 static void attribute_container_release(struct device *classdev)
 {
-	struct internal_container *ic 
+	struct internal_container *ic
 		= container_of(classdev, struct internal_container, classdev);
 	struct device *dev = classdev->parent;
 
@@ -184,8 +184,8 @@ attribute_container_add_device(struct device *dev,
 		struct klist_node *n = klist_next(iter); \
 		n ? container_of(n, typeof(*pos), member) : \
 			({ klist_iter_exit(iter) ; NULL; }); \
-	}) ) != NULL; )
-			
+	})) != NULL;)
+
 
 /**
  * attribute_container_remove_device - make device eligible for removal.
@@ -247,7 +247,7 @@ attribute_container_remove_device(struct device *dev,
  * container, then use attribute_container_trigger() instead.
  */
 void
-attribute_container_device_trigger(struct device *dev, 
+attribute_container_device_trigger(struct device *dev,
 				   int (*fn)(struct attribute_container *,
 					     struct device *,
 					     struct device *))
diff --git a/drivers/base/core.c b/drivers/base/core.c
index 20da3ad1696b..14d162952c3b 100644
--- a/drivers/base/core.c
+++ b/drivers/base/core.c
@@ -1211,6 +1211,9 @@ void device_del(struct device *dev)
 	 */
 	if (platform_notify_remove)
 		platform_notify_remove(dev);
+	if (dev->bus)
+		blocking_notifier_call_chain(&dev->bus->p->bus_notifier,
+					     BUS_NOTIFY_REMOVED_DEVICE, dev);
 	kobject_uevent(&dev->kobj, KOBJ_REMOVE);
 	cleanup_device_parent(dev);
 	kobject_del(&dev->kobj);
@@ -2007,6 +2010,8 @@ create_syslog_header(const struct device *dev, char *hdr, size_t hdrlen)
 		return 0;
 
 	pos += snprintf(hdr + pos, hdrlen - pos, "SUBSYSTEM=%s", subsys);
+	if (pos >= hdrlen)
+		goto overflow;
 
 	/*
 	 * Add device identifier DEVICE=:
@@ -2038,7 +2043,14 @@ create_syslog_header(const struct device *dev, char *hdr, size_t hdrlen)
 				"DEVICE=+%s:%s", subsys, dev_name(dev));
 	}
 
+	if (pos >= hdrlen)
+		goto overflow;
+
 	return pos;
+
+overflow:
+	dev_WARN(dev, "device/subsystem name too long");
+	return 0;
 }
 
 int dev_vprintk_emit(int level, const struct device *dev,
diff --git a/drivers/base/dd.c b/drivers/base/dd.c
index e4ffbcf2f519..cdc779cf79a3 100644
--- a/drivers/base/dd.c
+++ b/drivers/base/dd.c
@@ -54,7 +54,7 @@ static LIST_HEAD(deferred_probe_active_list);
 static struct workqueue_struct *deferred_wq;
 static atomic_t deferred_trigger_count = ATOMIC_INIT(0);
 
-/**
+/*
  * deferred_probe_work_func() - Retry probing devices in the active list.
  */
 static void deferred_probe_work_func(struct work_struct *work)
diff --git a/drivers/base/devcoredump.c b/drivers/base/devcoredump.c
new file mode 100644
index 000000000000..96614b04544c
--- /dev/null
+++ b/drivers/base/devcoredump.c
@@ -0,0 +1,265 @@
+/*
+ * This file is provided under the GPLv2 license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * Author: Johannes Berg <johannes@sipsolutions.net>
+ */
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/devcoredump.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/workqueue.h>
+
+/* if data isn't read by userspace after 5 minutes then delete it */
+#define DEVCD_TIMEOUT	(HZ * 60 * 5)
+
+struct devcd_entry {
+	struct device devcd_dev;
+	const void *data;
+	size_t datalen;
+	struct module *owner;
+	ssize_t (*read)(char *buffer, loff_t offset, size_t count,
+			const void *data, size_t datalen);
+	void (*free)(const void *data);
+	struct delayed_work del_wk;
+	struct device *failing_dev;
+};
+
+static struct devcd_entry *dev_to_devcd(struct device *dev)
+{
+	return container_of(dev, struct devcd_entry, devcd_dev);
+}
+
+static void devcd_dev_release(struct device *dev)
+{
+	struct devcd_entry *devcd = dev_to_devcd(dev);
+
+	devcd->free(devcd->data);
+	module_put(devcd->owner);
+
+	/*
+	 * this seems racy, but I don't see a notifier or such on
+	 * a struct device to know when it goes away?
+	 */
+	if (devcd->failing_dev->kobj.sd)
+		sysfs_delete_link(&devcd->failing_dev->kobj, &dev->kobj,
+				  "devcoredump");
+
+	put_device(devcd->failing_dev);
+	kfree(devcd);
+}
+
+static void devcd_del(struct work_struct *wk)
+{
+	struct devcd_entry *devcd;
+
+	devcd = container_of(wk, struct devcd_entry, del_wk.work);
+
+	device_del(&devcd->devcd_dev);
+	put_device(&devcd->devcd_dev);
+}
+
+static ssize_t devcd_data_read(struct file *filp, struct kobject *kobj,
+			       struct bin_attribute *bin_attr,
+			       char *buffer, loff_t offset, size_t count)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct devcd_entry *devcd = dev_to_devcd(dev);
+
+	return devcd->read(buffer, offset, count, devcd->data, devcd->datalen);
+}
+
+static ssize_t devcd_data_write(struct file *filp, struct kobject *kobj,
+				struct bin_attribute *bin_attr,
+				char *buffer, loff_t offset, size_t count)
+{
+	struct device *dev = kobj_to_dev(kobj);
+	struct devcd_entry *devcd = dev_to_devcd(dev);
+
+	mod_delayed_work(system_wq, &devcd->del_wk, 0);
+
+	return count;
+}
+
+static struct bin_attribute devcd_attr_data = {
+	.attr = { .name = "data", .mode = S_IRUSR | S_IWUSR, },
+	.size = 0,
+	.read = devcd_data_read,
+	.write = devcd_data_write,
+};
+
+static struct bin_attribute *devcd_dev_bin_attrs[] = {
+	&devcd_attr_data, NULL,
+};
+
+static const struct attribute_group devcd_dev_group = {
+	.bin_attrs = devcd_dev_bin_attrs,
+};
+
+static const struct attribute_group *devcd_dev_groups[] = {
+	&devcd_dev_group, NULL,
+};
+
+static struct class devcd_class = {
+	.name		= "devcoredump",
+	.owner		= THIS_MODULE,
+	.dev_release	= devcd_dev_release,
+	.dev_groups	= devcd_dev_groups,
+};
+
+static ssize_t devcd_readv(char *buffer, loff_t offset, size_t count,
+			   const void *data, size_t datalen)
+{
+	if (offset > datalen)
+		return -EINVAL;
+
+	if (offset + count > datalen)
+		count = datalen - offset;
+
+	if (count)
+		memcpy(buffer, ((u8 *)data) + offset, count);
+
+	return count;
+}
+
+/**
+ * dev_coredumpv - create device coredump with vmalloc data
+ * @dev: the struct device for the crashed device
+ * @data: vmalloc data containing the device coredump
+ * @datalen: length of the data
+ * @gfp: allocation flags
+ *
+ * This function takes ownership of the vmalloc'ed data and will free
+ * it when it is no longer used. See dev_coredumpm() for more information.
+ */
+void dev_coredumpv(struct device *dev, const void *data, size_t datalen,
+		   gfp_t gfp)
+{
+	dev_coredumpm(dev, NULL, data, datalen, gfp, devcd_readv, vfree);
+}
+EXPORT_SYMBOL_GPL(dev_coredumpv);
+
+static int devcd_match_failing(struct device *dev, const void *failing)
+{
+	struct devcd_entry *devcd = dev_to_devcd(dev);
+
+	return devcd->failing_dev == failing;
+}
+
+/**
+ * dev_coredumpm - create device coredump with read/free methods
+ * @dev: the struct device for the crashed device
+ * @owner: the module that contains the read/free functions, use %THIS_MODULE
+ * @data: data cookie for the @read/@free functions
+ * @datalen: length of the data
+ * @gfp: allocation flags
+ * @read: function to read from the given buffer
+ * @free: function to free the given buffer
+ *
+ * Creates a new device coredump for the given device. If a previous one hasn't
+ * been read yet, the new coredump is discarded. The data lifetime is determined
+ * by the device coredump framework and when it is no longer needed the @free
+ * function will be called to free the data.
+ */
+void dev_coredumpm(struct device *dev, struct module *owner,
+		   const void *data, size_t datalen, gfp_t gfp,
+		   ssize_t (*read)(char *buffer, loff_t offset, size_t count,
+				   const void *data, size_t datalen),
+		   void (*free)(const void *data))
+{
+	static atomic_t devcd_count = ATOMIC_INIT(0);
+	struct devcd_entry *devcd;
+	struct device *existing;
+
+	existing = class_find_device(&devcd_class, NULL, dev,
+				     devcd_match_failing);
+	if (existing) {
+		put_device(existing);
+		goto free;
+	}
+
+	if (!try_module_get(owner))
+		goto free;
+
+	devcd = kzalloc(sizeof(*devcd), gfp);
+	if (!devcd)
+		goto put_module;
+
+	devcd->owner = owner;
+	devcd->data = data;
+	devcd->datalen = datalen;
+	devcd->read = read;
+	devcd->free = free;
+	devcd->failing_dev = get_device(dev);
+
+	device_initialize(&devcd->devcd_dev);
+
+	dev_set_name(&devcd->devcd_dev, "devcd%d",
+		     atomic_inc_return(&devcd_count));
+	devcd->devcd_dev.class = &devcd_class;
+
+	if (device_add(&devcd->devcd_dev))
+		goto put_device;
+
+	if (sysfs_create_link(&devcd->devcd_dev.kobj, &dev->kobj,
+			      "failing_device"))
+		/* nothing - symlink will be missing */;
+
+	if (sysfs_create_link(&dev->kobj, &devcd->devcd_dev.kobj,
+			      "devcoredump"))
+		/* nothing - symlink will be missing */;
+
+	INIT_DELAYED_WORK(&devcd->del_wk, devcd_del);
+	schedule_delayed_work(&devcd->del_wk, DEVCD_TIMEOUT);
+
+	return;
+ put_device:
+	put_device(&devcd->devcd_dev);
+ put_module:
+	module_put(owner);
+ free:
+	free(data);
+}
+EXPORT_SYMBOL_GPL(dev_coredumpm);
+
+static int __init devcoredump_init(void)
+{
+	return class_register(&devcd_class);
+}
+__initcall(devcoredump_init);
+
+static int devcd_free(struct device *dev, void *data)
+{
+	struct devcd_entry *devcd = dev_to_devcd(dev);
+
+	flush_delayed_work(&devcd->del_wk);
+	return 0;
+}
+
+static void __exit devcoredump_exit(void)
+{
+	class_for_each_device(&devcd_class, NULL, NULL, devcd_free);
+	class_unregister(&devcd_class);
+}
+__exitcall(devcoredump_exit);
diff --git a/drivers/base/devres.c b/drivers/base/devres.c
index 69d9b0c89a01..c8a53d1e019f 100644
--- a/drivers/base/devres.c
+++ b/drivers/base/devres.c
@@ -817,13 +817,13 @@ char *devm_kstrdup(struct device *dev, const char *s, gfp_t gfp)
 EXPORT_SYMBOL_GPL(devm_kstrdup);
 
 /**
- * devm_kvasprintf - Allocate resource managed space
- *			for the formatted string.
+ * devm_kvasprintf - Allocate resource managed space and format a string
+ *		     into that.
  * @dev: Device to allocate memory for
  * @gfp: the GFP mask used in the devm_kmalloc() call when
  *       allocating memory
- * @fmt: the formatted string to duplicate
- * @ap: the list of tokens to be placed in the formatted string
+ * @fmt: The printf()-style format string
+ * @ap: Arguments for the format string
  * RETURNS:
  * Pointer to allocated string on success, NULL on failure.
  */
@@ -849,12 +849,13 @@ char *devm_kvasprintf(struct device *dev, gfp_t gfp, const char *fmt,
 EXPORT_SYMBOL(devm_kvasprintf);
 
 /**
- * devm_kasprintf - Allocate resource managed space
- *		and copy an existing formatted string into that
+ * devm_kasprintf - Allocate resource managed space and format a string
+ *		    into that.
  * @dev: Device to allocate memory for
  * @gfp: the GFP mask used in the devm_kmalloc() call when
  *       allocating memory
- * @fmt: the string to duplicate
+ * @fmt: The printf()-style format string
+ * @...: Arguments for the format string
  * RETURNS:
  * Pointer to allocated string on success, NULL on failure.
  */
diff --git a/drivers/base/dma-coherent.c b/drivers/base/dma-coherent.c
index 7d6e84a51424..55b83983a9c0 100644
--- a/drivers/base/dma-coherent.c
+++ b/drivers/base/dma-coherent.c
@@ -14,11 +14,14 @@ struct dma_coherent_mem {
 	int		size;
 	int		flags;
 	unsigned long	*bitmap;
+	spinlock_t	spinlock;
 };
 
-int dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
-				dma_addr_t device_addr, size_t size, int flags)
+static int dma_init_coherent_memory(phys_addr_t phys_addr, dma_addr_t device_addr,
+			     size_t size, int flags,
+			     struct dma_coherent_mem **mem)
 {
+	struct dma_coherent_mem *dma_mem = NULL;
 	void __iomem *mem_base = NULL;
 	int pages = size >> PAGE_SHIFT;
 	int bitmap_size = BITS_TO_LONGS(pages) * sizeof(long);
@@ -27,40 +30,77 @@ int dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
 		goto out;
 	if (!size)
 		goto out;
-	if (dev->dma_mem)
-		goto out;
-
-	/* FIXME: this routine just ignores DMA_MEMORY_INCLUDES_CHILDREN */
 
 	mem_base = ioremap(phys_addr, size);
 	if (!mem_base)
 		goto out;
 
-	dev->dma_mem = kzalloc(sizeof(struct dma_coherent_mem), GFP_KERNEL);
-	if (!dev->dma_mem)
+	dma_mem = kzalloc(sizeof(struct dma_coherent_mem), GFP_KERNEL);
+	if (!dma_mem)
 		goto out;
-	dev->dma_mem->bitmap = kzalloc(bitmap_size, GFP_KERNEL);
-	if (!dev->dma_mem->bitmap)
-		goto free1_out;
+	dma_mem->bitmap = kzalloc(bitmap_size, GFP_KERNEL);
+	if (!dma_mem->bitmap)
+		goto out;
+
+	dma_mem->virt_base = mem_base;
+	dma_mem->device_base = device_addr;
+	dma_mem->pfn_base = PFN_DOWN(phys_addr);
+	dma_mem->size = pages;
+	dma_mem->flags = flags;
+	spin_lock_init(&dma_mem->spinlock);
 
-	dev->dma_mem->virt_base = mem_base;
-	dev->dma_mem->device_base = device_addr;
-	dev->dma_mem->pfn_base = PFN_DOWN(phys_addr);
-	dev->dma_mem->size = pages;
-	dev->dma_mem->flags = flags;
+	*mem = dma_mem;
 
 	if (flags & DMA_MEMORY_MAP)
 		return DMA_MEMORY_MAP;
 
 	return DMA_MEMORY_IO;
 
- free1_out:
-	kfree(dev->dma_mem);
- out:
+out:
+	kfree(dma_mem);
 	if (mem_base)
 		iounmap(mem_base);
 	return 0;
 }
+
+static void dma_release_coherent_memory(struct dma_coherent_mem *mem)
+{
+	if (!mem)
+		return;
+	iounmap(mem->virt_base);
+	kfree(mem->bitmap);
+	kfree(mem);
+}
+
+static int dma_assign_coherent_memory(struct device *dev,
+				      struct dma_coherent_mem *mem)
+{
+	if (dev->dma_mem)
+		return -EBUSY;
+
+	dev->dma_mem = mem;
+	/* FIXME: this routine just ignores DMA_MEMORY_INCLUDES_CHILDREN */
+
+	return 0;
+}
+
+int dma_declare_coherent_memory(struct device *dev, phys_addr_t phys_addr,
+				dma_addr_t device_addr, size_t size, int flags)
+{
+	struct dma_coherent_mem *mem;
+	int ret;
+
+	ret = dma_init_coherent_memory(phys_addr, device_addr, size, flags,
+				       &mem);
+	if (ret == 0)
+		return 0;
+
+	if (dma_assign_coherent_memory(dev, mem) == 0)
+		return ret;
+
+	dma_release_coherent_memory(mem);
+	return 0;
+}
 EXPORT_SYMBOL(dma_declare_coherent_memory);
 
 void dma_release_declared_memory(struct device *dev)
@@ -69,10 +109,8 @@ void dma_release_declared_memory(struct device *dev)
 
 	if (!mem)
 		return;
+	dma_release_coherent_memory(mem);
 	dev->dma_mem = NULL;
-	iounmap(mem->virt_base);
-	kfree(mem->bitmap);
-	kfree(mem);
 }
 EXPORT_SYMBOL(dma_release_declared_memory);
 
@@ -80,6 +118,7 @@ void *dma_mark_declared_memory_occupied(struct device *dev,
 					dma_addr_t device_addr, size_t size)
 {
 	struct dma_coherent_mem *mem = dev->dma_mem;
+	unsigned long flags;
 	int pos, err;
 
 	size += device_addr & ~PAGE_MASK;
@@ -87,8 +126,11 @@ void *dma_mark_declared_memory_occupied(struct device *dev,
 	if (!mem)
 		return ERR_PTR(-EINVAL);
 
+	spin_lock_irqsave(&mem->spinlock, flags);
 	pos = (device_addr - mem->device_base) >> PAGE_SHIFT;
 	err = bitmap_allocate_region(mem->bitmap, pos, get_order(size));
+	spin_unlock_irqrestore(&mem->spinlock, flags);
+
 	if (err != 0)
 		return ERR_PTR(err);
 	return mem->virt_base + (pos << PAGE_SHIFT);
@@ -115,6 +157,7 @@ int dma_alloc_from_coherent(struct device *dev, ssize_t size,
 {
 	struct dma_coherent_mem *mem;
 	int order = get_order(size);
+	unsigned long flags;
 	int pageno;
 
 	if (!dev)
@@ -124,6 +167,7 @@ int dma_alloc_from_coherent(struct device *dev, ssize_t size,
 		return 0;
 
 	*ret = NULL;
+	spin_lock_irqsave(&mem->spinlock, flags);
 
 	if (unlikely(size > (mem->size << PAGE_SHIFT)))
 		goto err;
@@ -138,10 +182,12 @@ int dma_alloc_from_coherent(struct device *dev, ssize_t size,
 	*dma_handle = mem->device_base + (pageno << PAGE_SHIFT);
 	*ret = mem->virt_base + (pageno << PAGE_SHIFT);
 	memset(*ret, 0, size);
+	spin_unlock_irqrestore(&mem->spinlock, flags);
 
 	return 1;
 
 err:
+	spin_unlock_irqrestore(&mem->spinlock, flags);
 	/*
 	 * In the case where the allocation can not be satisfied from the
 	 * per-device area, try to fall back to generic memory if the
@@ -171,8 +217,11 @@ int dma_release_from_coherent(struct device *dev, int order, void *vaddr)
 	if (mem && vaddr >= mem->virt_base && vaddr <
 		   (mem->virt_base + (mem->size << PAGE_SHIFT))) {
 		int page = (vaddr - mem->virt_base) >> PAGE_SHIFT;
+		unsigned long flags;
 
+		spin_lock_irqsave(&mem->spinlock, flags);
 		bitmap_release_region(mem->bitmap, page, order);
+		spin_unlock_irqrestore(&mem->spinlock, flags);
 		return 1;
 	}
 	return 0;
@@ -218,3 +267,61 @@ int dma_mmap_from_coherent(struct device *dev, struct vm_area_struct *vma,
 	return 0;
 }
 EXPORT_SYMBOL(dma_mmap_from_coherent);
+
+/*
+ * Support for reserved memory regions defined in device tree
+ */
+#ifdef CONFIG_OF_RESERVED_MEM
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+#include <linux/of_reserved_mem.h>
+
+static int rmem_dma_device_init(struct reserved_mem *rmem, struct device *dev)
+{
+	struct dma_coherent_mem *mem = rmem->priv;
+
+	if (!mem &&
+	    dma_init_coherent_memory(rmem->base, rmem->base, rmem->size,
+				     DMA_MEMORY_MAP | DMA_MEMORY_EXCLUSIVE,
+				     &mem) != DMA_MEMORY_MAP) {
+		pr_err("Reserved memory: failed to init DMA memory pool at %pa, size %ld MiB\n",
+			&rmem->base, (unsigned long)rmem->size / SZ_1M);
+		return -ENODEV;
+	}
+	rmem->priv = mem;
+	dma_assign_coherent_memory(dev, mem);
+	return 0;
+}
+
+static void rmem_dma_device_release(struct reserved_mem *rmem,
+				    struct device *dev)
+{
+	dev->dma_mem = NULL;
+}
+
+static const struct reserved_mem_ops rmem_dma_ops = {
+	.device_init	= rmem_dma_device_init,
+	.device_release	= rmem_dma_device_release,
+};
+
+static int __init rmem_dma_setup(struct reserved_mem *rmem)
+{
+	unsigned long node = rmem->fdt_node;
+
+	if (of_get_flat_dt_prop(node, "reusable", NULL))
+		return -EINVAL;
+
+#ifdef CONFIG_ARM
+	if (!of_get_flat_dt_prop(node, "no-map", NULL)) {
+		pr_err("Reserved memory: regions without no-map are not yet supported\n");
+		return -EINVAL;
+	}
+#endif
+
+	rmem->ops = &rmem_dma_ops;
+	pr_info("Reserved memory: created DMA memory pool at %pa, size %ld MiB\n",
+		&rmem->base, (unsigned long)rmem->size / SZ_1M);
+	return 0;
+}
+RESERVEDMEM_OF_DECLARE(dma, "shared-dma-pool", rmem_dma_setup);
+#endif
diff --git a/drivers/base/dma-contiguous.c b/drivers/base/dma-contiguous.c
index 6606abdf880c..473ff4892401 100644
--- a/drivers/base/dma-contiguous.c
+++ b/drivers/base/dma-contiguous.c
@@ -211,3 +211,69 @@ bool dma_release_from_contiguous(struct device *dev, struct page *pages,
 {
 	return cma_release(dev_get_cma_area(dev), pages, count);
 }
+
+/*
+ * Support for reserved memory regions defined in device tree
+ */
+#ifdef CONFIG_OF_RESERVED_MEM
+#include <linux/of.h>
+#include <linux/of_fdt.h>
+#include <linux/of_reserved_mem.h>
+
+#undef pr_fmt
+#define pr_fmt(fmt) fmt
+
+static void rmem_cma_device_init(struct reserved_mem *rmem, struct device *dev)
+{
+	dev_set_cma_area(dev, rmem->priv);
+}
+
+static void rmem_cma_device_release(struct reserved_mem *rmem,
+				    struct device *dev)
+{
+	dev_set_cma_area(dev, NULL);
+}
+
+static const struct reserved_mem_ops rmem_cma_ops = {
+	.device_init	= rmem_cma_device_init,
+	.device_release = rmem_cma_device_release,
+};
+
+static int __init rmem_cma_setup(struct reserved_mem *rmem)
+{
+	phys_addr_t align = PAGE_SIZE << max(MAX_ORDER - 1, pageblock_order);
+	phys_addr_t mask = align - 1;
+	unsigned long node = rmem->fdt_node;
+	struct cma *cma;
+	int err;
+
+	if (!of_get_flat_dt_prop(node, "reusable", NULL) ||
+	    of_get_flat_dt_prop(node, "no-map", NULL))
+		return -EINVAL;
+
+	if ((rmem->base & mask) || (rmem->size & mask)) {
+		pr_err("Reserved memory: incorrect alignment of CMA region\n");
+		return -EINVAL;
+	}
+
+	err = cma_init_reserved_mem(rmem->base, rmem->size, 0, &cma);
+	if (err) {
+		pr_err("Reserved memory: unable to setup CMA region\n");
+		return err;
+	}
+	/* Architecture specific contiguous memory fixup. */
+	dma_contiguous_early_fixup(rmem->base, rmem->size);
+
+	if (of_get_flat_dt_prop(node, "linux,cma-default", NULL))
+		dma_contiguous_set_default(cma);
+
+	rmem->ops = &rmem_cma_ops;
+	rmem->priv = cma;
+
+	pr_info("Reserved memory: created CMA memory pool at %pa, size %ld MiB\n",
+		&rmem->base, (unsigned long)rmem->size / SZ_1M);
+
+	return 0;
+}
+RESERVEDMEM_OF_DECLARE(cma, "shared-dma-pool", rmem_cma_setup);
+#endif
diff --git a/drivers/base/dma-mapping.c b/drivers/base/dma-mapping.c
index 6cd08e145bfa..9e8bbdd470ca 100644
--- a/drivers/base/dma-mapping.c
+++ b/drivers/base/dma-mapping.c
@@ -10,6 +10,8 @@
 #include <linux/dma-mapping.h>
 #include <linux/export.h>
 #include <linux/gfp.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
 #include <asm-generic/dma-coherent.h>
 
 /*
@@ -267,3 +269,73 @@ int dma_common_mmap(struct device *dev, struct vm_area_struct *vma,
 	return ret;
 }
 EXPORT_SYMBOL(dma_common_mmap);
+
+#ifdef CONFIG_MMU
+/*
+ * remaps an array of PAGE_SIZE pages into another vm_area
+ * Cannot be used in non-sleeping contexts
+ */
+void *dma_common_pages_remap(struct page **pages, size_t size,
+			unsigned long vm_flags, pgprot_t prot,
+			const void *caller)
+{
+	struct vm_struct *area;
+
+	area = get_vm_area_caller(size, vm_flags, caller);
+	if (!area)
+		return NULL;
+
+	area->pages = pages;
+
+	if (map_vm_area(area, prot, pages)) {
+		vunmap(area->addr);
+		return NULL;
+	}
+
+	return area->addr;
+}
+
+/*
+ * remaps an allocated contiguous region into another vm_area.
+ * Cannot be used in non-sleeping contexts
+ */
+
+void *dma_common_contiguous_remap(struct page *page, size_t size,
+			unsigned long vm_flags,
+			pgprot_t prot, const void *caller)
+{
+	int i;
+	struct page **pages;
+	void *ptr;
+	unsigned long pfn;
+
+	pages = kmalloc(sizeof(struct page *) << get_order(size), GFP_KERNEL);
+	if (!pages)
+		return NULL;
+
+	for (i = 0, pfn = page_to_pfn(page); i < (size >> PAGE_SHIFT); i++)
+		pages[i] = pfn_to_page(pfn + i);
+
+	ptr = dma_common_pages_remap(pages, size, vm_flags, prot, caller);
+
+	kfree(pages);
+
+	return ptr;
+}
+
+/*
+ * unmaps a range previously mapped by dma_common_*_remap
+ */
+void dma_common_free_remap(void *cpu_addr, size_t size, unsigned long vm_flags)
+{
+	struct vm_struct *area = find_vm_area(cpu_addr);
+
+	if (!area || (area->flags & vm_flags) != vm_flags) {
+		WARN(1, "trying to free invalid coherent area: %p\n", cpu_addr);
+		return;
+	}
+
+	unmap_kernel_range((unsigned long)cpu_addr, size);
+	vunmap(cpu_addr);
+}
+#endif
diff --git a/drivers/base/firmware_class.c b/drivers/base/firmware_class.c
index bf424305f3dc..3d785ebb48d3 100644
--- a/drivers/base/firmware_class.c
+++ b/drivers/base/firmware_class.c
@@ -1105,6 +1105,9 @@ _request_firmware(const struct firmware **firmware_p, const char *name,
 	if (!firmware_p)
 		return -EINVAL;
 
+	if (!name || name[0] == '\0')
+		return -EINVAL;
+
 	ret = _request_firmware_prepare(&fw, name, device);
 	if (ret <= 0) /* error or already assigned */
 		goto out;
diff --git a/drivers/base/memory.c b/drivers/base/memory.c
index a2e13e250bba..7c5d87191b28 100644
--- a/drivers/base/memory.c
+++ b/drivers/base/memory.c
@@ -373,6 +373,45 @@ static ssize_t show_phys_device(struct device *dev,
 	return sprintf(buf, "%d\n", mem->phys_device);
 }
 
+#ifdef CONFIG_MEMORY_HOTREMOVE
+static ssize_t show_valid_zones(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct memory_block *mem = to_memory_block(dev);
+	unsigned long start_pfn, end_pfn;
+	unsigned long nr_pages = PAGES_PER_SECTION * sections_per_block;
+	struct page *first_page;
+	struct zone *zone;
+
+	start_pfn = section_nr_to_pfn(mem->start_section_nr);
+	end_pfn = start_pfn + nr_pages;
+	first_page = pfn_to_page(start_pfn);
+
+	/* The block contains more than one zone can not be offlined. */
+	if (!test_pages_in_a_zone(start_pfn, end_pfn))
+		return sprintf(buf, "none\n");
+
+	zone = page_zone(first_page);
+
+	if (zone_idx(zone) == ZONE_MOVABLE - 1) {
+		/*The mem block is the last memoryblock of this zone.*/
+		if (end_pfn == zone_end_pfn(zone))
+			return sprintf(buf, "%s %s\n",
+					zone->name, (zone + 1)->name);
+	}
+
+	if (zone_idx(zone) == ZONE_MOVABLE) {
+		/*The mem block is the first memoryblock of ZONE_MOVABLE.*/
+		if (start_pfn == zone->zone_start_pfn)
+			return sprintf(buf, "%s %s\n",
+					zone->name, (zone - 1)->name);
+	}
+
+	return sprintf(buf, "%s\n", zone->name);
+}
+static DEVICE_ATTR(valid_zones, 0444, show_valid_zones, NULL);
+#endif
+
 static DEVICE_ATTR(phys_index, 0444, show_mem_start_phys_index, NULL);
 static DEVICE_ATTR(state, 0644, show_mem_state, store_mem_state);
 static DEVICE_ATTR(phys_device, 0444, show_phys_device, NULL);
@@ -523,6 +562,9 @@ static struct attribute *memory_memblk_attrs[] = {
 	&dev_attr_state.attr,
 	&dev_attr_phys_device.attr,
 	&dev_attr_removable.attr,
+#ifdef CONFIG_MEMORY_HOTREMOVE
+	&dev_attr_valid_zones.attr,
+#endif
 	NULL
 };
 
diff --git a/drivers/base/node.c b/drivers/base/node.c
index c6d3ae05f1ca..472168cd0c97 100644
--- a/drivers/base/node.c
+++ b/drivers/base/node.c
@@ -289,8 +289,6 @@ static int register_node(struct node *node, int num, struct node *parent)
 		device_create_file(&node->dev, &dev_attr_distance);
 		device_create_file(&node->dev, &dev_attr_vmstat);
 
-		scan_unevictable_register_node(node);
-
 		hugetlb_register_node(node);
 
 		compaction_register_node(node);
@@ -314,7 +312,6 @@ void unregister_node(struct node *node)
 	device_remove_file(&node->dev, &dev_attr_distance);
 	device_remove_file(&node->dev, &dev_attr_vmstat);
 
-	scan_unevictable_unregister_node(node);
 	hugetlb_unregister_node(node);		/* no-op, if memoryless node */
 
 	device_unregister(&node->dev);
@@ -603,7 +600,6 @@ void unregister_one_node(int nid)
 		return;
 
 	unregister_node(node_devices[nid]);
-	kfree(node_devices[nid]);
 	node_devices[nid] = NULL;
 }
 
diff --git a/drivers/base/platform.c b/drivers/base/platform.c
index ab4f4ce02722..b2afc29403f9 100644
--- a/drivers/base/platform.c
+++ b/drivers/base/platform.c
@@ -21,6 +21,7 @@
 #include <linux/err.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_domain.h>
 #include <linux/idr.h>
 #include <linux/acpi.h>
 #include <linux/clk/clk-conf.h>
@@ -506,11 +507,12 @@ static int platform_drv_probe(struct device *_dev)
 	if (ret < 0)
 		return ret;
 
-	acpi_dev_pm_attach(_dev, true);
-
-	ret = drv->probe(dev);
-	if (ret)
-		acpi_dev_pm_detach(_dev, true);
+	ret = dev_pm_domain_attach(_dev, true);
+	if (ret != -EPROBE_DEFER) {
+		ret = drv->probe(dev);
+		if (ret)
+			dev_pm_domain_detach(_dev, true);
+	}
 
 	if (drv->prevent_deferred_probe && ret == -EPROBE_DEFER) {
 		dev_warn(_dev, "probe deferral not supported\n");
@@ -532,7 +534,7 @@ static int platform_drv_remove(struct device *_dev)
 	int ret;
 
 	ret = drv->remove(dev);
-	acpi_dev_pm_detach(_dev, true);
+	dev_pm_domain_detach(_dev, true);
 
 	return ret;
 }
@@ -543,7 +545,7 @@ static void platform_drv_shutdown(struct device *_dev)
 	struct platform_device *dev = to_platform_device(_dev);
 
 	drv->shutdown(dev);
-	acpi_dev_pm_detach(_dev, true);
+	dev_pm_domain_detach(_dev, true);
 }
 
 /**
diff --git a/drivers/base/power/clock_ops.c b/drivers/base/power/clock_ops.c
index b99e6c06ee67..78369305e069 100644
--- a/drivers/base/power/clock_ops.c
+++ b/drivers/base/power/clock_ops.c
@@ -368,8 +368,13 @@ int pm_clk_suspend(struct device *dev)
 
 	spin_lock_irqsave(&psd->lock, flags);
 
-	list_for_each_entry_reverse(ce, &psd->clock_list, node)
-		clk_disable(ce->clk);
+	list_for_each_entry_reverse(ce, &psd->clock_list, node) {
+		if (ce->status < PCE_STATUS_ERROR) {
+			if (ce->status == PCE_STATUS_ENABLED)
+				clk_disable(ce->clk);
+			ce->status = PCE_STATUS_ACQUIRED;
+		}
+	}
 
 	spin_unlock_irqrestore(&psd->lock, flags);
 
@@ -385,6 +390,7 @@ int pm_clk_resume(struct device *dev)
 	struct pm_subsys_data *psd = dev_to_psd(dev);
 	struct pm_clock_entry *ce;
 	unsigned long flags;
+	int ret;
 
 	dev_dbg(dev, "%s()\n", __func__);
 
@@ -394,8 +400,13 @@ int pm_clk_resume(struct device *dev)
 
 	spin_lock_irqsave(&psd->lock, flags);
 
-	list_for_each_entry(ce, &psd->clock_list, node)
-		__pm_clk_enable(dev, ce->clk);
+	list_for_each_entry(ce, &psd->clock_list, node) {
+		if (ce->status < PCE_STATUS_ERROR) {
+			ret = __pm_clk_enable(dev, ce->clk);
+			if (!ret)
+				ce->status = PCE_STATUS_ENABLED;
+		}
+	}
 
 	spin_unlock_irqrestore(&psd->lock, flags);
 
diff --git a/drivers/base/power/common.c b/drivers/base/power/common.c
index df2e5eeaeb05..b0f138806bbc 100644
--- a/drivers/base/power/common.c
+++ b/drivers/base/power/common.c
@@ -11,6 +11,8 @@
 #include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/pm_clock.h>
+#include <linux/acpi.h>
+#include <linux/pm_domain.h>
 
 /**
  * dev_pm_get_subsys_data - Create or refcount power.subsys_data for device.
@@ -82,3 +84,53 @@ int dev_pm_put_subsys_data(struct device *dev)
 	return ret;
 }
 EXPORT_SYMBOL_GPL(dev_pm_put_subsys_data);
+
+/**
+ * dev_pm_domain_attach - Attach a device to its PM domain.
+ * @dev: Device to attach.
+ * @power_on: Used to indicate whether we should power on the device.
+ *
+ * The @dev may only be attached to a single PM domain. By iterating through
+ * the available alternatives we try to find a valid PM domain for the device.
+ * As attachment succeeds, the ->detach() callback in the struct dev_pm_domain
+ * should be assigned by the corresponding attach function.
+ *
+ * This function should typically be invoked from subsystem level code during
+ * the probe phase. Especially for those that holds devices which requires
+ * power management through PM domains.
+ *
+ * Callers must ensure proper synchronization of this function with power
+ * management callbacks.
+ *
+ * Returns 0 on successfully attached PM domain or negative error code.
+ */
+int dev_pm_domain_attach(struct device *dev, bool power_on)
+{
+	int ret;
+
+	ret = acpi_dev_pm_attach(dev, power_on);
+	if (ret)
+		ret = genpd_dev_pm_attach(dev);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(dev_pm_domain_attach);
+
+/**
+ * dev_pm_domain_detach - Detach a device from its PM domain.
+ * @dev: Device to attach.
+ * @power_off: Used to indicate whether we should power off the device.
+ *
+ * This functions will reverse the actions from dev_pm_domain_attach() and thus
+ * try to detach the @dev from its PM domain. Typically it should be invoked
+ * from subsystem level code during the remove phase.
+ *
+ * Callers must ensure proper synchronization of this function with power
+ * management callbacks.
+ */
+void dev_pm_domain_detach(struct device *dev, bool power_off)
+{
+	if (dev->pm_domain && dev->pm_domain->detach)
+		dev->pm_domain->detach(dev, power_off);
+}
+EXPORT_SYMBOL_GPL(dev_pm_domain_detach);
diff --git a/drivers/base/power/domain.c b/drivers/base/power/domain.c
index eee55c1e5fde..40bc2f4072cc 100644
--- a/drivers/base/power/domain.c
+++ b/drivers/base/power/domain.c
@@ -8,6 +8,7 @@
 
 #include <linux/kernel.h>
 #include <linux/io.h>
+#include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/pm_domain.h>
 #include <linux/pm_qos.h>
@@ -25,10 +26,6 @@
 	__routine = genpd->dev_ops.callback; 			\
 	if (__routine) {					\
 		__ret = __routine(dev); 			\
-	} else {						\
-		__routine = dev_gpd_data(dev)->ops.callback;	\
-		if (__routine) 					\
-			__ret = __routine(dev);			\
 	}							\
 	__ret;							\
 })
@@ -70,8 +67,6 @@ static struct generic_pm_domain *pm_genpd_lookup_name(const char *domain_name)
 	return genpd;
 }
 
-#ifdef CONFIG_PM
-
 struct generic_pm_domain *dev_to_genpd(struct device *dev)
 {
 	if (IS_ERR_OR_NULL(dev->pm_domain))
@@ -147,13 +142,13 @@ static void genpd_recalc_cpu_exit_latency(struct generic_pm_domain *genpd)
 {
 	s64 usecs64;
 
-	if (!genpd->cpu_data)
+	if (!genpd->cpuidle_data)
 		return;
 
 	usecs64 = genpd->power_on_latency_ns;
 	do_div(usecs64, NSEC_PER_USEC);
-	usecs64 += genpd->cpu_data->saved_exit_latency;
-	genpd->cpu_data->idle_state->exit_latency = usecs64;
+	usecs64 += genpd->cpuidle_data->saved_exit_latency;
+	genpd->cpuidle_data->idle_state->exit_latency = usecs64;
 }
 
 /**
@@ -193,9 +188,9 @@ static int __pm_genpd_poweron(struct generic_pm_domain *genpd)
 		return 0;
 	}
 
-	if (genpd->cpu_data) {
+	if (genpd->cpuidle_data) {
 		cpuidle_pause_and_lock();
-		genpd->cpu_data->idle_state->disabled = true;
+		genpd->cpuidle_data->idle_state->disabled = true;
 		cpuidle_resume_and_unlock();
 		goto out;
 	}
@@ -285,8 +280,6 @@ int pm_genpd_name_poweron(const char *domain_name)
 	return genpd ? pm_genpd_poweron(genpd) : -EINVAL;
 }
 
-#endif /* CONFIG_PM */
-
 #ifdef CONFIG_PM_RUNTIME
 
 static int genpd_start_dev_no_timing(struct generic_pm_domain *genpd,
@@ -430,7 +423,7 @@ static bool genpd_abort_poweroff(struct generic_pm_domain *genpd)
  * Queue up the execution of pm_genpd_poweroff() unless it's already been done
  * before.
  */
-void genpd_queue_power_off_work(struct generic_pm_domain *genpd)
+static void genpd_queue_power_off_work(struct generic_pm_domain *genpd)
 {
 	queue_work(pm_wq, &genpd->power_off_work);
 }
@@ -520,17 +513,17 @@ static int pm_genpd_poweroff(struct generic_pm_domain *genpd)
 		}
 	}
 
-	if (genpd->cpu_data) {
+	if (genpd->cpuidle_data) {
 		/*
-		 * If cpu_data is set, cpuidle should turn the domain off when
-		 * the CPU in it is idle.  In that case we don't decrement the
-		 * subdomain counts of the master domains, so that power is not
-		 * removed from the current domain prematurely as a result of
-		 * cutting off the masters' power.
+		 * If cpuidle_data is set, cpuidle should turn the domain off
+		 * when the CPU in it is idle.  In that case we don't decrement
+		 * the subdomain counts of the master domains, so that power is
+		 * not removed from the current domain prematurely as a result
+		 * of cutting off the masters' power.
 		 */
 		genpd->status = GPD_STATE_POWER_OFF;
 		cpuidle_pause_and_lock();
-		genpd->cpu_data->idle_state->disabled = false;
+		genpd->cpuidle_data->idle_state->disabled = false;
 		cpuidle_resume_and_unlock();
 		goto out;
 	}
@@ -619,8 +612,6 @@ static int pm_genpd_runtime_suspend(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	might_sleep_if(!genpd->dev_irq_safe);
-
 	stop_ok = genpd->gov ? genpd->gov->stop_ok : NULL;
 	if (stop_ok && !stop_ok(dev))
 		return -EBUSY;
@@ -665,8 +656,6 @@ static int pm_genpd_runtime_resume(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	might_sleep_if(!genpd->dev_irq_safe);
-
 	/* If power.irq_safe, the PM domain is never powered off. */
 	if (dev->power.irq_safe)
 		return genpd_start_dev_no_timing(genpd, dev);
@@ -733,6 +722,13 @@ void pm_genpd_poweroff_unused(void)
 	mutex_unlock(&gpd_list_lock);
 }
 
+static int __init genpd_poweroff_unused(void)
+{
+	pm_genpd_poweroff_unused();
+	return 0;
+}
+late_initcall(genpd_poweroff_unused);
+
 #else
 
 static inline int genpd_dev_pm_qos_notifier(struct notifier_block *nb,
@@ -741,6 +737,9 @@ static inline int genpd_dev_pm_qos_notifier(struct notifier_block *nb,
 	return NOTIFY_DONE;
 }
 
+static inline void
+genpd_queue_power_off_work(struct generic_pm_domain *genpd) {}
+
 static inline void genpd_power_off_work_fn(struct work_struct *work) {}
 
 #define pm_genpd_runtime_suspend	NULL
@@ -774,46 +773,6 @@ static bool genpd_dev_active_wakeup(struct generic_pm_domain *genpd,
 	return GENPD_DEV_CALLBACK(genpd, bool, active_wakeup, dev);
 }
 
-static int genpd_suspend_dev(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, suspend, dev);
-}
-
-static int genpd_suspend_late(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, suspend_late, dev);
-}
-
-static int genpd_resume_early(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, resume_early, dev);
-}
-
-static int genpd_resume_dev(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, resume, dev);
-}
-
-static int genpd_freeze_dev(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, freeze, dev);
-}
-
-static int genpd_freeze_late(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, freeze_late, dev);
-}
-
-static int genpd_thaw_early(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, thaw_early, dev);
-}
-
-static int genpd_thaw_dev(struct generic_pm_domain *genpd, struct device *dev)
-{
-	return GENPD_DEV_CALLBACK(genpd, int, thaw, dev);
-}
-
 /**
  * pm_genpd_sync_poweroff - Synchronously power off a PM domain and its masters.
  * @genpd: PM domain to power off, if possible.
@@ -995,7 +954,7 @@ static int pm_genpd_suspend(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_suspend_dev(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_suspend(dev);
 }
 
 /**
@@ -1016,7 +975,7 @@ static int pm_genpd_suspend_late(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_suspend_late(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_suspend_late(dev);
 }
 
 /**
@@ -1103,7 +1062,7 @@ static int pm_genpd_resume_early(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_resume_early(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_resume_early(dev);
 }
 
 /**
@@ -1124,7 +1083,7 @@ static int pm_genpd_resume(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_resume_dev(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_resume(dev);
 }
 
 /**
@@ -1145,7 +1104,7 @@ static int pm_genpd_freeze(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_freeze_dev(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_freeze(dev);
 }
 
 /**
@@ -1167,7 +1126,7 @@ static int pm_genpd_freeze_late(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_freeze_late(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_freeze_late(dev);
 }
 
 /**
@@ -1231,7 +1190,7 @@ static int pm_genpd_thaw_early(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_thaw_early(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_thaw_early(dev);
 }
 
 /**
@@ -1252,7 +1211,7 @@ static int pm_genpd_thaw(struct device *dev)
 	if (IS_ERR(genpd))
 		return -EINVAL;
 
-	return genpd->suspend_power_off ? 0 : genpd_thaw_dev(genpd, dev);
+	return genpd->suspend_power_off ? 0 : pm_generic_thaw(dev);
 }
 
 /**
@@ -1344,13 +1303,13 @@ static void pm_genpd_complete(struct device *dev)
 }
 
 /**
- * pm_genpd_syscore_switch - Switch power during system core suspend or resume.
+ * genpd_syscore_switch - Switch power during system core suspend or resume.
  * @dev: Device that normally is marked as "always on" to switch power for.
  *
  * This routine may only be called during the system core (syscore) suspend or
  * resume phase for devices whose "always on" flags are set.
  */
-void pm_genpd_syscore_switch(struct device *dev, bool suspend)
+static void genpd_syscore_switch(struct device *dev, bool suspend)
 {
 	struct generic_pm_domain *genpd;
 
@@ -1366,7 +1325,18 @@ void pm_genpd_syscore_switch(struct device *dev, bool suspend)
 		genpd->suspended_count--;
 	}
 }
-EXPORT_SYMBOL_GPL(pm_genpd_syscore_switch);
+
+void pm_genpd_syscore_poweroff(struct device *dev)
+{
+	genpd_syscore_switch(dev, true);
+}
+EXPORT_SYMBOL_GPL(pm_genpd_syscore_poweroff);
+
+void pm_genpd_syscore_poweron(struct device *dev)
+{
+	genpd_syscore_switch(dev, false);
+}
+EXPORT_SYMBOL_GPL(pm_genpd_syscore_poweron);
 
 #else
 
@@ -1466,6 +1436,9 @@ int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 
 	spin_unlock_irq(&dev->power.lock);
 
+	if (genpd->attach_dev)
+		genpd->attach_dev(dev);
+
 	mutex_lock(&gpd_data->lock);
 	gpd_data->base.dev = dev;
 	list_add_tail(&gpd_data->base.list_node, &genpd->dev_list);
@@ -1484,39 +1457,6 @@ int __pm_genpd_add_device(struct generic_pm_domain *genpd, struct device *dev,
 }
 
 /**
- * __pm_genpd_of_add_device - Add a device to an I/O PM domain.
- * @genpd_node: Device tree node pointer representing a PM domain to which the
- *   the device is added to.
- * @dev: Device to be added.
- * @td: Set of PM QoS timing parameters to attach to the device.
- */
-int __pm_genpd_of_add_device(struct device_node *genpd_node, struct device *dev,
-			     struct gpd_timing_data *td)
-{
-	struct generic_pm_domain *genpd = NULL, *gpd;
-
-	dev_dbg(dev, "%s()\n", __func__);
-
-	if (IS_ERR_OR_NULL(genpd_node) || IS_ERR_OR_NULL(dev))
-		return -EINVAL;
-
-	mutex_lock(&gpd_list_lock);
-	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
-		if (gpd->of_node == genpd_node) {
-			genpd = gpd;
-			break;
-		}
-	}
-	mutex_unlock(&gpd_list_lock);
-
-	if (!genpd)
-		return -EINVAL;
-
-	return __pm_genpd_add_device(genpd, dev, td);
-}
-
-
-/**
  * __pm_genpd_name_add_device - Find I/O PM domain and add a device to it.
  * @domain_name: Name of the PM domain to add the device to.
  * @dev: Device to be added.
@@ -1558,6 +1498,9 @@ int pm_genpd_remove_device(struct generic_pm_domain *genpd,
 	genpd->device_count--;
 	genpd->max_off_time_changed = true;
 
+	if (genpd->detach_dev)
+		genpd->detach_dev(dev);
+
 	spin_lock_irq(&dev->power.lock);
 
 	dev->pm_domain = NULL;
@@ -1744,112 +1687,6 @@ int pm_genpd_remove_subdomain(struct generic_pm_domain *genpd,
 }
 
 /**
- * pm_genpd_add_callbacks - Add PM domain callbacks to a given device.
- * @dev: Device to add the callbacks to.
- * @ops: Set of callbacks to add.
- * @td: Timing data to add to the device along with the callbacks (optional).
- *
- * Every call to this routine should be balanced with a call to
- * __pm_genpd_remove_callbacks() and they must not be nested.
- */
-int pm_genpd_add_callbacks(struct device *dev, struct gpd_dev_ops *ops,
-			   struct gpd_timing_data *td)
-{
-	struct generic_pm_domain_data *gpd_data_new, *gpd_data = NULL;
-	int ret = 0;
-
-	if (!(dev && ops))
-		return -EINVAL;
-
-	gpd_data_new = __pm_genpd_alloc_dev_data(dev);
-	if (!gpd_data_new)
-		return -ENOMEM;
-
-	pm_runtime_disable(dev);
-	device_pm_lock();
-
-	ret = dev_pm_get_subsys_data(dev);
-	if (ret)
-		goto out;
-
-	spin_lock_irq(&dev->power.lock);
-
-	if (dev->power.subsys_data->domain_data) {
-		gpd_data = to_gpd_data(dev->power.subsys_data->domain_data);
-	} else {
-		gpd_data = gpd_data_new;
-		dev->power.subsys_data->domain_data = &gpd_data->base;
-	}
-	gpd_data->refcount++;
-	gpd_data->ops = *ops;
-	if (td)
-		gpd_data->td = *td;
-
-	spin_unlock_irq(&dev->power.lock);
-
- out:
-	device_pm_unlock();
-	pm_runtime_enable(dev);
-
-	if (gpd_data != gpd_data_new)
-		__pm_genpd_free_dev_data(dev, gpd_data_new);
-
-	return ret;
-}
-EXPORT_SYMBOL_GPL(pm_genpd_add_callbacks);
-
-/**
- * __pm_genpd_remove_callbacks - Remove PM domain callbacks from a given device.
- * @dev: Device to remove the callbacks from.
- * @clear_td: If set, clear the device's timing data too.
- *
- * This routine can only be called after pm_genpd_add_callbacks().
- */
-int __pm_genpd_remove_callbacks(struct device *dev, bool clear_td)
-{
-	struct generic_pm_domain_data *gpd_data = NULL;
-	bool remove = false;
-	int ret = 0;
-
-	if (!(dev && dev->power.subsys_data))
-		return -EINVAL;
-
-	pm_runtime_disable(dev);
-	device_pm_lock();
-
-	spin_lock_irq(&dev->power.lock);
-
-	if (dev->power.subsys_data->domain_data) {
-		gpd_data = to_gpd_data(dev->power.subsys_data->domain_data);
-		gpd_data->ops = (struct gpd_dev_ops){ NULL };
-		if (clear_td)
-			gpd_data->td = (struct gpd_timing_data){ 0 };
-
-		if (--gpd_data->refcount == 0) {
-			dev->power.subsys_data->domain_data = NULL;
-			remove = true;
-		}
-	} else {
-		ret = -EINVAL;
-	}
-
-	spin_unlock_irq(&dev->power.lock);
-
-	device_pm_unlock();
-	pm_runtime_enable(dev);
-
-	if (ret)
-		return ret;
-
-	dev_pm_put_subsys_data(dev);
-	if (remove)
-		__pm_genpd_free_dev_data(dev, gpd_data);
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(__pm_genpd_remove_callbacks);
-
-/**
  * pm_genpd_attach_cpuidle - Connect the given PM domain with cpuidle.
  * @genpd: PM domain to be connected with cpuidle.
  * @state: cpuidle state this domain can disable/enable.
@@ -1861,7 +1698,7 @@ EXPORT_SYMBOL_GPL(__pm_genpd_remove_callbacks);
 int pm_genpd_attach_cpuidle(struct generic_pm_domain *genpd, int state)
 {
 	struct cpuidle_driver *cpuidle_drv;
-	struct gpd_cpu_data *cpu_data;
+	struct gpd_cpuidle_data *cpuidle_data;
 	struct cpuidle_state *idle_state;
 	int ret = 0;
 
@@ -1870,12 +1707,12 @@ int pm_genpd_attach_cpuidle(struct generic_pm_domain *genpd, int state)
 
 	genpd_acquire_lock(genpd);
 
-	if (genpd->cpu_data) {
+	if (genpd->cpuidle_data) {
 		ret = -EEXIST;
 		goto out;
 	}
-	cpu_data = kzalloc(sizeof(*cpu_data), GFP_KERNEL);
-	if (!cpu_data) {
+	cpuidle_data = kzalloc(sizeof(*cpuidle_data), GFP_KERNEL);
+	if (!cpuidle_data) {
 		ret = -ENOMEM;
 		goto out;
 	}
@@ -1893,9 +1730,9 @@ int pm_genpd_attach_cpuidle(struct generic_pm_domain *genpd, int state)
 		ret = -EAGAIN;
 		goto err;
 	}
-	cpu_data->idle_state = idle_state;
-	cpu_data->saved_exit_latency = idle_state->exit_latency;
-	genpd->cpu_data = cpu_data;
+	cpuidle_data->idle_state = idle_state;
+	cpuidle_data->saved_exit_latency = idle_state->exit_latency;
+	genpd->cpuidle_data = cpuidle_data;
 	genpd_recalc_cpu_exit_latency(genpd);
 
  out:
@@ -1906,7 +1743,7 @@ int pm_genpd_attach_cpuidle(struct generic_pm_domain *genpd, int state)
 	cpuidle_driver_unref();
 
  err_drv:
-	kfree(cpu_data);
+	kfree(cpuidle_data);
 	goto out;
 }
 
@@ -1929,7 +1766,7 @@ int pm_genpd_name_attach_cpuidle(const char *name, int state)
  */
 int pm_genpd_detach_cpuidle(struct generic_pm_domain *genpd)
 {
-	struct gpd_cpu_data *cpu_data;
+	struct gpd_cpuidle_data *cpuidle_data;
 	struct cpuidle_state *idle_state;
 	int ret = 0;
 
@@ -1938,20 +1775,20 @@ int pm_genpd_detach_cpuidle(struct generic_pm_domain *genpd)
 
 	genpd_acquire_lock(genpd);
 
-	cpu_data = genpd->cpu_data;
-	if (!cpu_data) {
+	cpuidle_data = genpd->cpuidle_data;
+	if (!cpuidle_data) {
 		ret = -ENODEV;
 		goto out;
 	}
-	idle_state = cpu_data->idle_state;
+	idle_state = cpuidle_data->idle_state;
 	if (!idle_state->disabled) {
 		ret = -EAGAIN;
 		goto out;
 	}
-	idle_state->exit_latency = cpu_data->saved_exit_latency;
+	idle_state->exit_latency = cpuidle_data->saved_exit_latency;
 	cpuidle_driver_unref();
-	genpd->cpu_data = NULL;
-	kfree(cpu_data);
+	genpd->cpuidle_data = NULL;
+	kfree(cpuidle_data);
 
  out:
 	genpd_release_lock(genpd);
@@ -1970,17 +1807,13 @@ int pm_genpd_name_detach_cpuidle(const char *name)
 /* Default device callbacks for generic PM domains. */
 
 /**
- * pm_genpd_default_save_state - Default "save device state" for PM domians.
+ * pm_genpd_default_save_state - Default "save device state" for PM domains.
  * @dev: Device to handle.
  */
 static int pm_genpd_default_save_state(struct device *dev)
 {
 	int (*cb)(struct device *__dev);
 
-	cb = dev_gpd_data(dev)->ops.save_state;
-	if (cb)
-		return cb(dev);
-
 	if (dev->type && dev->type->pm)
 		cb = dev->type->pm->runtime_suspend;
 	else if (dev->class && dev->class->pm)
@@ -1997,17 +1830,13 @@ static int pm_genpd_default_save_state(struct device *dev)
 }
 
 /**
- * pm_genpd_default_restore_state - Default PM domians "restore device state".
+ * pm_genpd_default_restore_state - Default PM domains "restore device state".
  * @dev: Device to handle.
  */
 static int pm_genpd_default_restore_state(struct device *dev)
 {
 	int (*cb)(struct device *__dev);
 
-	cb = dev_gpd_data(dev)->ops.restore_state;
-	if (cb)
-		return cb(dev);
-
 	if (dev->type && dev->type->pm)
 		cb = dev->type->pm->runtime_resume;
 	else if (dev->class && dev->class->pm)
@@ -2023,109 +1852,6 @@ static int pm_genpd_default_restore_state(struct device *dev)
 	return cb ? cb(dev) : 0;
 }
 
-#ifdef CONFIG_PM_SLEEP
-
-/**
- * pm_genpd_default_suspend - Default "device suspend" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_suspend(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.suspend;
-
-	return cb ? cb(dev) : pm_generic_suspend(dev);
-}
-
-/**
- * pm_genpd_default_suspend_late - Default "late device suspend" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_suspend_late(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.suspend_late;
-
-	return cb ? cb(dev) : pm_generic_suspend_late(dev);
-}
-
-/**
- * pm_genpd_default_resume_early - Default "early device resume" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_resume_early(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.resume_early;
-
-	return cb ? cb(dev) : pm_generic_resume_early(dev);
-}
-
-/**
- * pm_genpd_default_resume - Default "device resume" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_resume(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.resume;
-
-	return cb ? cb(dev) : pm_generic_resume(dev);
-}
-
-/**
- * pm_genpd_default_freeze - Default "device freeze" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_freeze(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.freeze;
-
-	return cb ? cb(dev) : pm_generic_freeze(dev);
-}
-
-/**
- * pm_genpd_default_freeze_late - Default "late device freeze" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_freeze_late(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.freeze_late;
-
-	return cb ? cb(dev) : pm_generic_freeze_late(dev);
-}
-
-/**
- * pm_genpd_default_thaw_early - Default "early device thaw" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_thaw_early(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.thaw_early;
-
-	return cb ? cb(dev) : pm_generic_thaw_early(dev);
-}
-
-/**
- * pm_genpd_default_thaw - Default "device thaw" for PM domians.
- * @dev: Device to handle.
- */
-static int pm_genpd_default_thaw(struct device *dev)
-{
-	int (*cb)(struct device *__dev) = dev_gpd_data(dev)->ops.thaw;
-
-	return cb ? cb(dev) : pm_generic_thaw(dev);
-}
-
-#else /* !CONFIG_PM_SLEEP */
-
-#define pm_genpd_default_suspend	NULL
-#define pm_genpd_default_suspend_late	NULL
-#define pm_genpd_default_resume_early	NULL
-#define pm_genpd_default_resume		NULL
-#define pm_genpd_default_freeze		NULL
-#define pm_genpd_default_freeze_late	NULL
-#define pm_genpd_default_thaw_early	NULL
-#define pm_genpd_default_thaw		NULL
-
-#endif /* !CONFIG_PM_SLEEP */
-
 /**
  * pm_genpd_init - Initialize a generic I/O PM domain object.
  * @genpd: PM domain object to initialize.
@@ -2177,15 +1903,452 @@ void pm_genpd_init(struct generic_pm_domain *genpd,
 	genpd->domain.ops.complete = pm_genpd_complete;
 	genpd->dev_ops.save_state = pm_genpd_default_save_state;
 	genpd->dev_ops.restore_state = pm_genpd_default_restore_state;
-	genpd->dev_ops.suspend = pm_genpd_default_suspend;
-	genpd->dev_ops.suspend_late = pm_genpd_default_suspend_late;
-	genpd->dev_ops.resume_early = pm_genpd_default_resume_early;
-	genpd->dev_ops.resume = pm_genpd_default_resume;
-	genpd->dev_ops.freeze = pm_genpd_default_freeze;
-	genpd->dev_ops.freeze_late = pm_genpd_default_freeze_late;
-	genpd->dev_ops.thaw_early = pm_genpd_default_thaw_early;
-	genpd->dev_ops.thaw = pm_genpd_default_thaw;
 	mutex_lock(&gpd_list_lock);
 	list_add(&genpd->gpd_list_node, &gpd_list);
 	mutex_unlock(&gpd_list_lock);
 }
+
+#ifdef CONFIG_PM_GENERIC_DOMAINS_OF
+/*
+ * Device Tree based PM domain providers.
+ *
+ * The code below implements generic device tree based PM domain providers that
+ * bind device tree nodes with generic PM domains registered in the system.
+ *
+ * Any driver that registers generic PM domains and needs to support binding of
+ * devices to these domains is supposed to register a PM domain provider, which
+ * maps a PM domain specifier retrieved from the device tree to a PM domain.
+ *
+ * Two simple mapping functions have been provided for convenience:
+ *  - __of_genpd_xlate_simple() for 1:1 device tree node to PM domain mapping.
+ *  - __of_genpd_xlate_onecell() for mapping of multiple PM domains per node by
+ *    index.
+ */
+
+/**
+ * struct of_genpd_provider - PM domain provider registration structure
+ * @link: Entry in global list of PM domain providers
+ * @node: Pointer to device tree node of PM domain provider
+ * @xlate: Provider-specific xlate callback mapping a set of specifier cells
+ *         into a PM domain.
+ * @data: context pointer to be passed into @xlate callback
+ */
+struct of_genpd_provider {
+	struct list_head link;
+	struct device_node *node;
+	genpd_xlate_t xlate;
+	void *data;
+};
+
+/* List of registered PM domain providers. */
+static LIST_HEAD(of_genpd_providers);
+/* Mutex to protect the list above. */
+static DEFINE_MUTEX(of_genpd_mutex);
+
+/**
+ * __of_genpd_xlate_simple() - Xlate function for direct node-domain mapping
+ * @genpdspec: OF phandle args to map into a PM domain
+ * @data: xlate function private data - pointer to struct generic_pm_domain
+ *
+ * This is a generic xlate function that can be used to model PM domains that
+ * have their own device tree nodes. The private data of xlate function needs
+ * to be a valid pointer to struct generic_pm_domain.
+ */
+struct generic_pm_domain *__of_genpd_xlate_simple(
+					struct of_phandle_args *genpdspec,
+					void *data)
+{
+	if (genpdspec->args_count != 0)
+		return ERR_PTR(-EINVAL);
+	return data;
+}
+EXPORT_SYMBOL_GPL(__of_genpd_xlate_simple);
+
+/**
+ * __of_genpd_xlate_onecell() - Xlate function using a single index.
+ * @genpdspec: OF phandle args to map into a PM domain
+ * @data: xlate function private data - pointer to struct genpd_onecell_data
+ *
+ * This is a generic xlate function that can be used to model simple PM domain
+ * controllers that have one device tree node and provide multiple PM domains.
+ * A single cell is used as an index into an array of PM domains specified in
+ * the genpd_onecell_data struct when registering the provider.
+ */
+struct generic_pm_domain *__of_genpd_xlate_onecell(
+					struct of_phandle_args *genpdspec,
+					void *data)
+{
+	struct genpd_onecell_data *genpd_data = data;
+	unsigned int idx = genpdspec->args[0];
+
+	if (genpdspec->args_count != 1)
+		return ERR_PTR(-EINVAL);
+
+	if (idx >= genpd_data->num_domains) {
+		pr_err("%s: invalid domain index %u\n", __func__, idx);
+		return ERR_PTR(-EINVAL);
+	}
+
+	if (!genpd_data->domains[idx])
+		return ERR_PTR(-ENOENT);
+
+	return genpd_data->domains[idx];
+}
+EXPORT_SYMBOL_GPL(__of_genpd_xlate_onecell);
+
+/**
+ * __of_genpd_add_provider() - Register a PM domain provider for a node
+ * @np: Device node pointer associated with the PM domain provider.
+ * @xlate: Callback for decoding PM domain from phandle arguments.
+ * @data: Context pointer for @xlate callback.
+ */
+int __of_genpd_add_provider(struct device_node *np, genpd_xlate_t xlate,
+			void *data)
+{
+	struct of_genpd_provider *cp;
+
+	cp = kzalloc(sizeof(*cp), GFP_KERNEL);
+	if (!cp)
+		return -ENOMEM;
+
+	cp->node = of_node_get(np);
+	cp->data = data;
+	cp->xlate = xlate;
+
+	mutex_lock(&of_genpd_mutex);
+	list_add(&cp->link, &of_genpd_providers);
+	mutex_unlock(&of_genpd_mutex);
+	pr_debug("Added domain provider from %s\n", np->full_name);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(__of_genpd_add_provider);
+
+/**
+ * of_genpd_del_provider() - Remove a previously registered PM domain provider
+ * @np: Device node pointer associated with the PM domain provider
+ */
+void of_genpd_del_provider(struct device_node *np)
+{
+	struct of_genpd_provider *cp;
+
+	mutex_lock(&of_genpd_mutex);
+	list_for_each_entry(cp, &of_genpd_providers, link) {
+		if (cp->node == np) {
+			list_del(&cp->link);
+			of_node_put(cp->node);
+			kfree(cp);
+			break;
+		}
+	}
+	mutex_unlock(&of_genpd_mutex);
+}
+EXPORT_SYMBOL_GPL(of_genpd_del_provider);
+
+/**
+ * of_genpd_get_from_provider() - Look-up PM domain
+ * @genpdspec: OF phandle args to use for look-up
+ *
+ * Looks for a PM domain provider under the node specified by @genpdspec and if
+ * found, uses xlate function of the provider to map phandle args to a PM
+ * domain.
+ *
+ * Returns a valid pointer to struct generic_pm_domain on success or ERR_PTR()
+ * on failure.
+ */
+static struct generic_pm_domain *of_genpd_get_from_provider(
+					struct of_phandle_args *genpdspec)
+{
+	struct generic_pm_domain *genpd = ERR_PTR(-ENOENT);
+	struct of_genpd_provider *provider;
+
+	mutex_lock(&of_genpd_mutex);
+
+	/* Check if we have such a provider in our array */
+	list_for_each_entry(provider, &of_genpd_providers, link) {
+		if (provider->node == genpdspec->np)
+			genpd = provider->xlate(genpdspec, provider->data);
+		if (!IS_ERR(genpd))
+			break;
+	}
+
+	mutex_unlock(&of_genpd_mutex);
+
+	return genpd;
+}
+
+/**
+ * genpd_dev_pm_detach - Detach a device from its PM domain.
+ * @dev: Device to attach.
+ * @power_off: Currently not used
+ *
+ * Try to locate a corresponding generic PM domain, which the device was
+ * attached to previously. If such is found, the device is detached from it.
+ */
+static void genpd_dev_pm_detach(struct device *dev, bool power_off)
+{
+	struct generic_pm_domain *pd = NULL, *gpd;
+	int ret = 0;
+
+	if (!dev->pm_domain)
+		return;
+
+	mutex_lock(&gpd_list_lock);
+	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
+		if (&gpd->domain == dev->pm_domain) {
+			pd = gpd;
+			break;
+		}
+	}
+	mutex_unlock(&gpd_list_lock);
+
+	if (!pd)
+		return;
+
+	dev_dbg(dev, "removing from PM domain %s\n", pd->name);
+
+	while (1) {
+		ret = pm_genpd_remove_device(pd, dev);
+		if (ret != -EAGAIN)
+			break;
+		cond_resched();
+	}
+
+	if (ret < 0) {
+		dev_err(dev, "failed to remove from PM domain %s: %d",
+			pd->name, ret);
+		return;
+	}
+
+	/* Check if PM domain can be powered off after removing this device. */
+	genpd_queue_power_off_work(pd);
+}
+
+/**
+ * genpd_dev_pm_attach - Attach a device to its PM domain using DT.
+ * @dev: Device to attach.
+ *
+ * Parse device's OF node to find a PM domain specifier. If such is found,
+ * attaches the device to retrieved pm_domain ops.
+ *
+ * Both generic and legacy Samsung-specific DT bindings are supported to keep
+ * backwards compatibility with existing DTBs.
+ *
+ * Returns 0 on successfully attached PM domain or negative error code.
+ */
+int genpd_dev_pm_attach(struct device *dev)
+{
+	struct of_phandle_args pd_args;
+	struct generic_pm_domain *pd;
+	int ret;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	if (dev->pm_domain)
+		return -EEXIST;
+
+	ret = of_parse_phandle_with_args(dev->of_node, "power-domains",
+					"#power-domain-cells", 0, &pd_args);
+	if (ret < 0) {
+		if (ret != -ENOENT)
+			return ret;
+
+		/*
+		 * Try legacy Samsung-specific bindings
+		 * (for backwards compatibility of DT ABI)
+		 */
+		pd_args.args_count = 0;
+		pd_args.np = of_parse_phandle(dev->of_node,
+						"samsung,power-domain", 0);
+		if (!pd_args.np)
+			return -ENOENT;
+	}
+
+	pd = of_genpd_get_from_provider(&pd_args);
+	if (IS_ERR(pd)) {
+		dev_dbg(dev, "%s() failed to find PM domain: %ld\n",
+			__func__, PTR_ERR(pd));
+		of_node_put(dev->of_node);
+		return PTR_ERR(pd);
+	}
+
+	dev_dbg(dev, "adding to PM domain %s\n", pd->name);
+
+	while (1) {
+		ret = pm_genpd_add_device(pd, dev);
+		if (ret != -EAGAIN)
+			break;
+		cond_resched();
+	}
+
+	if (ret < 0) {
+		dev_err(dev, "failed to add to PM domain %s: %d",
+			pd->name, ret);
+		of_node_put(dev->of_node);
+		return ret;
+	}
+
+	dev->pm_domain->detach = genpd_dev_pm_detach;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(genpd_dev_pm_attach);
+#endif
+
+
+/***        debugfs support        ***/
+
+#ifdef CONFIG_PM_ADVANCED_DEBUG
+#include <linux/pm.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/init.h>
+#include <linux/kobject.h>
+static struct dentry *pm_genpd_debugfs_dir;
+
+/*
+ * TODO: This function is a slightly modified version of rtpm_status_show
+ * from sysfs.c, but dependencies between PM_GENERIC_DOMAINS and PM_RUNTIME
+ * are too loose to generalize it.
+ */
+#ifdef CONFIG_PM_RUNTIME
+static void rtpm_status_str(struct seq_file *s, struct device *dev)
+{
+	static const char * const status_lookup[] = {
+		[RPM_ACTIVE] = "active",
+		[RPM_RESUMING] = "resuming",
+		[RPM_SUSPENDED] = "suspended",
+		[RPM_SUSPENDING] = "suspending"
+	};
+	const char *p = "";
+
+	if (dev->power.runtime_error)
+		p = "error";
+	else if (dev->power.disable_depth)
+		p = "unsupported";
+	else if (dev->power.runtime_status < ARRAY_SIZE(status_lookup))
+		p = status_lookup[dev->power.runtime_status];
+	else
+		WARN_ON(1);
+
+	seq_puts(s, p);
+}
+#else
+static void rtpm_status_str(struct seq_file *s, struct device *dev)
+{
+	seq_puts(s, "active");
+}
+#endif
+
+static int pm_genpd_summary_one(struct seq_file *s,
+		struct generic_pm_domain *gpd)
+{
+	static const char * const status_lookup[] = {
+		[GPD_STATE_ACTIVE] = "on",
+		[GPD_STATE_WAIT_MASTER] = "wait-master",
+		[GPD_STATE_BUSY] = "busy",
+		[GPD_STATE_REPEAT] = "off-in-progress",
+		[GPD_STATE_POWER_OFF] = "off"
+	};
+	struct pm_domain_data *pm_data;
+	const char *kobj_path;
+	struct gpd_link *link;
+	int ret;
+
+	ret = mutex_lock_interruptible(&gpd->lock);
+	if (ret)
+		return -ERESTARTSYS;
+
+	if (WARN_ON(gpd->status >= ARRAY_SIZE(status_lookup)))
+		goto exit;
+	seq_printf(s, "%-30s  %-15s  ", gpd->name, status_lookup[gpd->status]);
+
+	/*
+	 * Modifications on the list require holding locks on both
+	 * master and slave, so we are safe.
+	 * Also gpd->name is immutable.
+	 */
+	list_for_each_entry(link, &gpd->master_links, master_node) {
+		seq_printf(s, "%s", link->slave->name);
+		if (!list_is_last(&link->master_node, &gpd->master_links))
+			seq_puts(s, ", ");
+	}
+
+	list_for_each_entry(pm_data, &gpd->dev_list, list_node) {
+		kobj_path = kobject_get_path(&pm_data->dev->kobj, GFP_KERNEL);
+		if (kobj_path == NULL)
+			continue;
+
+		seq_printf(s, "\n    %-50s  ", kobj_path);
+		rtpm_status_str(s, pm_data->dev);
+		kfree(kobj_path);
+	}
+
+	seq_puts(s, "\n");
+exit:
+	mutex_unlock(&gpd->lock);
+
+	return 0;
+}
+
+static int pm_genpd_summary_show(struct seq_file *s, void *data)
+{
+	struct generic_pm_domain *gpd;
+	int ret = 0;
+
+	seq_puts(s, "    domain                      status         slaves\n");
+	seq_puts(s, "           /device                                      runtime status\n");
+	seq_puts(s, "----------------------------------------------------------------------\n");
+
+	ret = mutex_lock_interruptible(&gpd_list_lock);
+	if (ret)
+		return -ERESTARTSYS;
+
+	list_for_each_entry(gpd, &gpd_list, gpd_list_node) {
+		ret = pm_genpd_summary_one(s, gpd);
+		if (ret)
+			break;
+	}
+	mutex_unlock(&gpd_list_lock);
+
+	return ret;
+}
+
+static int pm_genpd_summary_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, pm_genpd_summary_show, NULL);
+}
+
+static const struct file_operations pm_genpd_summary_fops = {
+	.open = pm_genpd_summary_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+static int __init pm_genpd_debug_init(void)
+{
+	struct dentry *d;
+
+	pm_genpd_debugfs_dir = debugfs_create_dir("pm_genpd", NULL);
+
+	if (!pm_genpd_debugfs_dir)
+		return -ENOMEM;
+
+	d = debugfs_create_file("pm_genpd_summary", S_IRUGO,
+			pm_genpd_debugfs_dir, NULL, &pm_genpd_summary_fops);
+	if (!d)
+		return -ENOMEM;
+
+	return 0;
+}
+late_initcall(pm_genpd_debug_init);
+
+static void __exit pm_genpd_debug_exit(void)
+{
+	debugfs_remove_recursive(pm_genpd_debugfs_dir);
+}
+__exitcall(pm_genpd_debug_exit);
+#endif /* CONFIG_PM_ADVANCED_DEBUG */
diff --git a/drivers/base/power/domain_governor.c b/drivers/base/power/domain_governor.c
index a089e3bcdfbc..d88a62e104d4 100644
--- a/drivers/base/power/domain_governor.c
+++ b/drivers/base/power/domain_governor.c
@@ -42,7 +42,7 @@ static int dev_update_qos_constraint(struct device *dev, void *data)
  * default_stop_ok - Default PM domain governor routine for stopping devices.
  * @dev: Device to check.
  */
-bool default_stop_ok(struct device *dev)
+static bool default_stop_ok(struct device *dev)
 {
 	struct gpd_timing_data *td = &dev_gpd_data(dev)->td;
 	unsigned long flags;
@@ -229,10 +229,7 @@ static bool always_on_power_down_ok(struct dev_pm_domain *domain)
 
 #else /* !CONFIG_PM_RUNTIME */
 
-bool default_stop_ok(struct device *dev)
-{
-	return false;
-}
+static inline bool default_stop_ok(struct device *dev) { return false; }
 
 #define default_power_down_ok	NULL
 #define always_on_power_down_ok	NULL
diff --git a/drivers/base/power/main.c b/drivers/base/power/main.c
index b67d9aef9fe4..44973196d3fd 100644
--- a/drivers/base/power/main.c
+++ b/drivers/base/power/main.c
@@ -540,7 +540,7 @@ static void async_resume_noirq(void *data, async_cookie_t cookie)
  * Call the "noirq" resume handlers for all devices in dpm_noirq_list and
  * enable device drivers to receive interrupts.
  */
-static void dpm_resume_noirq(pm_message_t state)
+void dpm_resume_noirq(pm_message_t state)
 {
 	struct device *dev;
 	ktime_t starttime = ktime_get();
@@ -662,7 +662,7 @@ static void async_resume_early(void *data, async_cookie_t cookie)
  * dpm_resume_early - Execute "early resume" callbacks for all devices.
  * @state: PM transition of the system being carried out.
  */
-static void dpm_resume_early(pm_message_t state)
+void dpm_resume_early(pm_message_t state)
 {
 	struct device *dev;
 	ktime_t starttime = ktime_get();
@@ -1093,7 +1093,7 @@ static int device_suspend_noirq(struct device *dev)
  * Prevent device drivers from receiving interrupts and call the "noirq" suspend
  * handlers for all non-sysdev devices.
  */
-static int dpm_suspend_noirq(pm_message_t state)
+int dpm_suspend_noirq(pm_message_t state)
 {
 	ktime_t starttime = ktime_get();
 	int error = 0;
@@ -1232,7 +1232,7 @@ static int device_suspend_late(struct device *dev)
  * dpm_suspend_late - Execute "late suspend" callbacks for all devices.
  * @state: PM transition of the system being carried out.
  */
-static int dpm_suspend_late(pm_message_t state)
+int dpm_suspend_late(pm_message_t state)
 {
 	ktime_t starttime = ktime_get();
 	int error = 0;
diff --git a/drivers/base/power/sysfs.c b/drivers/base/power/sysfs.c
index 95b181d1ca6d..a9d26ed11bf4 100644
--- a/drivers/base/power/sysfs.c
+++ b/drivers/base/power/sysfs.c
@@ -92,9 +92,6 @@
  *	wakeup_count - Report the number of wakeup events related to the device
  */
 
-static const char enabled[] = "enabled";
-static const char disabled[] = "disabled";
-
 const char power_group_name[] = "power";
 EXPORT_SYMBOL_GPL(power_group_name);
 
@@ -336,11 +333,14 @@ static DEVICE_ATTR(pm_qos_remote_wakeup, 0644,
 #endif /* CONFIG_PM_RUNTIME */
 
 #ifdef CONFIG_PM_SLEEP
+static const char _enabled[] = "enabled";
+static const char _disabled[] = "disabled";
+
 static ssize_t
 wake_show(struct device * dev, struct device_attribute *attr, char * buf)
 {
 	return sprintf(buf, "%s\n", device_can_wakeup(dev)
-		? (device_may_wakeup(dev) ? enabled : disabled)
+		? (device_may_wakeup(dev) ? _enabled : _disabled)
 		: "");
 }
 
@@ -357,11 +357,11 @@ wake_store(struct device * dev, struct device_attribute *attr,
 	cp = memchr(buf, '\n', n);
 	if (cp)
 		len = cp - buf;
-	if (len == sizeof enabled - 1
-			&& strncmp(buf, enabled, sizeof enabled - 1) == 0)
+	if (len == sizeof _enabled - 1
+			&& strncmp(buf, _enabled, sizeof _enabled - 1) == 0)
 		device_set_wakeup_enable(dev, 1);
-	else if (len == sizeof disabled - 1
-			&& strncmp(buf, disabled, sizeof disabled - 1) == 0)
+	else if (len == sizeof _disabled - 1
+			&& strncmp(buf, _disabled, sizeof _disabled - 1) == 0)
 		device_set_wakeup_enable(dev, 0);
 	else
 		return -EINVAL;
@@ -570,7 +570,8 @@ static ssize_t async_show(struct device *dev, struct device_attribute *attr,
 			  char *buf)
 {
 	return sprintf(buf, "%s\n",
-			device_async_suspend_enabled(dev) ? enabled : disabled);
+			device_async_suspend_enabled(dev) ?
+				_enabled : _disabled);
 }
 
 static ssize_t async_store(struct device *dev, struct device_attribute *attr,
@@ -582,9 +583,10 @@ static ssize_t async_store(struct device *dev, struct device_attribute *attr,
 	cp = memchr(buf, '\n', n);
 	if (cp)
 		len = cp - buf;
-	if (len == sizeof enabled - 1 && strncmp(buf, enabled, len) == 0)
+	if (len == sizeof _enabled - 1 && strncmp(buf, _enabled, len) == 0)
 		device_enable_async_suspend(dev);
-	else if (len == sizeof disabled - 1 && strncmp(buf, disabled, len) == 0)
+	else if (len == sizeof _disabled - 1 &&
+		 strncmp(buf, _disabled, len) == 0)
 		device_disable_async_suspend(dev);
 	else
 		return -EINVAL;
diff --git a/drivers/base/power/wakeup.c b/drivers/base/power/wakeup.c
index eb1bd2ecad8b..c2744b30d5d9 100644
--- a/drivers/base/power/wakeup.c
+++ b/drivers/base/power/wakeup.c
@@ -24,6 +24,9 @@
  */
 bool events_check_enabled __read_mostly;
 
+/* If set and the system is suspending, terminate the suspend. */
+static bool pm_abort_suspend __read_mostly;
+
 /*
  * Combined counters of registered wakeup events and wakeup events in progress.
  * They need to be modified together atomically, so it's better to use one
@@ -719,7 +722,18 @@ bool pm_wakeup_pending(void)
 		pm_print_active_wakeup_sources();
 	}
 
-	return ret;
+	return ret || pm_abort_suspend;
+}
+
+void pm_system_wakeup(void)
+{
+	pm_abort_suspend = true;
+	freeze_wake();
+}
+
+void pm_wakeup_clear(void)
+{
+	pm_abort_suspend = false;
 }
 
 /**
diff --git a/drivers/base/regmap/Kconfig b/drivers/base/regmap/Kconfig
index 4251570610c9..8a3f51f7b1b9 100644
--- a/drivers/base/regmap/Kconfig
+++ b/drivers/base/regmap/Kconfig
@@ -11,12 +11,15 @@ config REGMAP
 
 config REGMAP_I2C
 	tristate
+	depends on I2C
 
 config REGMAP_SPI
 	tristate
+	depends on SPI
 
 config REGMAP_SPMI
 	tristate
+	depends on SPMI
 
 config REGMAP_MMIO
 	tristate
diff --git a/drivers/base/regmap/internal.h b/drivers/base/regmap/internal.h
index 7d1326985bee..0da5865df5b1 100644
--- a/drivers/base/regmap/internal.h
+++ b/drivers/base/regmap/internal.h
@@ -49,8 +49,10 @@ struct regmap_async {
 };
 
 struct regmap {
-	struct mutex mutex;
-	spinlock_t spinlock;
+	union {
+		struct mutex mutex;
+		spinlock_t spinlock;
+	};
 	unsigned long spinlock_flags;
 	regmap_lock lock;
 	regmap_unlock unlock;
@@ -146,6 +148,9 @@ struct regcache_ops {
 	enum regcache_type type;
 	int (*init)(struct regmap *map);
 	int (*exit)(struct regmap *map);
+#ifdef CONFIG_DEBUG_FS
+	void (*debugfs_init)(struct regmap *map);
+#endif
 	int (*read)(struct regmap *map, unsigned int reg, unsigned int *value);
 	int (*write)(struct regmap *map, unsigned int reg, unsigned int value);
 	int (*sync)(struct regmap *map, unsigned int min, unsigned int max);
diff --git a/drivers/base/regmap/regcache-rbtree.c b/drivers/base/regmap/regcache-rbtree.c
index 6a7e4fa12854..f3e8fe0cc650 100644
--- a/drivers/base/regmap/regcache-rbtree.c
+++ b/drivers/base/regmap/regcache-rbtree.c
@@ -194,10 +194,6 @@ static void rbtree_debugfs_init(struct regmap *map)
 {
 	debugfs_create_file("rbtree", 0400, map->debugfs, map, &rbtree_fops);
 }
-#else
-static void rbtree_debugfs_init(struct regmap *map)
-{
-}
 #endif
 
 static int regcache_rbtree_init(struct regmap *map)
@@ -222,8 +218,6 @@ static int regcache_rbtree_init(struct regmap *map)
 			goto err;
 	}
 
-	rbtree_debugfs_init(map);
-
 	return 0;
 
 err:
@@ -532,6 +526,9 @@ struct regcache_ops regcache_rbtree_ops = {
 	.name = "rbtree",
 	.init = regcache_rbtree_init,
 	.exit = regcache_rbtree_exit,
+#ifdef CONFIG_DEBUG_FS
+	.debugfs_init = rbtree_debugfs_init,
+#endif
 	.read = regcache_rbtree_read,
 	.write = regcache_rbtree_write,
 	.sync = regcache_rbtree_sync,
diff --git a/drivers/base/regmap/regcache.c b/drivers/base/regmap/regcache.c
index 29b4128da0b0..f1280dc356d0 100644
--- a/drivers/base/regmap/regcache.c
+++ b/drivers/base/regmap/regcache.c
@@ -269,8 +269,11 @@ static int regcache_default_sync(struct regmap *map, unsigned int min,
 		map->cache_bypass = 1;
 		ret = _regmap_write(map, reg, val);
 		map->cache_bypass = 0;
-		if (ret)
+		if (ret) {
+			dev_err(map->dev, "Unable to sync register %#x. %d\n",
+				reg, ret);
 			return ret;
+		}
 		dev_dbg(map->dev, "Synced register %#x, value %#x\n", reg, val);
 	}
 
@@ -615,8 +618,11 @@ static int regcache_sync_block_single(struct regmap *map, void *block,
 		ret = _regmap_write(map, regtmp, val);
 
 		map->cache_bypass = 0;
-		if (ret != 0)
+		if (ret != 0) {
+			dev_err(map->dev, "Unable to sync register %#x. %d\n",
+				regtmp, ret);
 			return ret;
+		}
 		dev_dbg(map->dev, "Synced register %#x, value %#x\n",
 			regtmp, val);
 	}
@@ -641,6 +647,9 @@ static int regcache_sync_block_raw_flush(struct regmap *map, const void **data,
 	map->cache_bypass = 1;
 
 	ret = _regmap_raw_write(map, base, *data, count * val_bytes);
+	if (ret)
+		dev_err(map->dev, "Unable to sync registers %#x-%#x. %d\n",
+			base, cur - map->reg_stride, ret);
 
 	map->cache_bypass = 0;
 
@@ -698,7 +707,7 @@ int regcache_sync_block(struct regmap *map, void *block,
 			unsigned int block_base, unsigned int start,
 			unsigned int end)
 {
-	if (regmap_can_raw_write(map))
+	if (regmap_can_raw_write(map) && !map->use_single_rw)
 		return regcache_sync_block_raw(map, block, cache_present,
 					       block_base, start, end);
 	else
diff --git a/drivers/base/regmap/regmap-debugfs.c b/drivers/base/regmap/regmap-debugfs.c
index 45d812c0ea77..5799a0b9e6cc 100644
--- a/drivers/base/regmap/regmap-debugfs.c
+++ b/drivers/base/regmap/regmap-debugfs.c
@@ -473,6 +473,7 @@ void regmap_debugfs_init(struct regmap *map, const char *name)
 {
 	struct rb_node *next;
 	struct regmap_range_node *range_node;
+	const char *devname = "dummy";
 
 	/* If we don't have the debugfs root yet, postpone init */
 	if (!regmap_debugfs_root) {
@@ -491,12 +492,15 @@ void regmap_debugfs_init(struct regmap *map, const char *name)
 	INIT_LIST_HEAD(&map->debugfs_off_cache);
 	mutex_init(&map->cache_lock);
 
+	if (map->dev)
+		devname = dev_name(map->dev);
+
 	if (name) {
 		map->debugfs_name = kasprintf(GFP_KERNEL, "%s-%s",
-					      dev_name(map->dev), name);
+					      devname, name);
 		name = map->debugfs_name;
 	} else {
-		name = dev_name(map->dev);
+		name = devname;
 	}
 
 	map->debugfs = debugfs_create_dir(name, regmap_debugfs_root);
@@ -512,7 +516,14 @@ void regmap_debugfs_init(struct regmap *map, const char *name)
 			    map, &regmap_reg_ranges_fops);
 
 	if (map->max_register || regmap_readable(map, 0)) {
-		debugfs_create_file("registers", 0400, map->debugfs,
+		umode_t registers_mode;
+
+		if (IS_ENABLED(REGMAP_ALLOW_WRITE_DEBUGFS))
+			registers_mode = 0600;
+		else
+			registers_mode = 0400;
+
+		debugfs_create_file("registers", registers_mode, map->debugfs,
 				    map, &regmap_map_fops);
 		debugfs_create_file("access", 0400, map->debugfs,
 				    map, &regmap_access_fops);
@@ -538,6 +549,9 @@ void regmap_debugfs_init(struct regmap *map, const char *name)
 
 		next = rb_next(&range_node->node);
 	}
+
+	if (map->cache_ops && map->cache_ops->debugfs_init)
+		map->cache_ops->debugfs_init(map);
 }
 
 void regmap_debugfs_exit(struct regmap *map)
diff --git a/drivers/base/regmap/regmap-i2c.c b/drivers/base/regmap/regmap-i2c.c
index ca193d1ef47c..053150a7f9f2 100644
--- a/drivers/base/regmap/regmap-i2c.c
+++ b/drivers/base/regmap/regmap-i2c.c
@@ -168,6 +168,8 @@ static struct regmap_bus regmap_i2c = {
 	.write = regmap_i2c_write,
 	.gather_write = regmap_i2c_gather_write,
 	.read = regmap_i2c_read,
+	.reg_format_endian_default = REGMAP_ENDIAN_BIG,
+	.val_format_endian_default = REGMAP_ENDIAN_BIG,
 };
 
 static const struct regmap_bus *regmap_get_i2c_bus(struct i2c_client *i2c,
diff --git a/drivers/base/regmap/regmap-spi.c b/drivers/base/regmap/regmap-spi.c
index 0eb3097c0d76..53d1148e80a0 100644
--- a/drivers/base/regmap/regmap-spi.c
+++ b/drivers/base/regmap/regmap-spi.c
@@ -109,6 +109,8 @@ static struct regmap_bus regmap_spi = {
 	.async_alloc = regmap_spi_async_alloc,
 	.read = regmap_spi_read,
 	.read_flag_mask = 0x80,
+	.reg_format_endian_default = REGMAP_ENDIAN_BIG,
+	.val_format_endian_default = REGMAP_ENDIAN_BIG,
 };
 
 /**
diff --git a/drivers/base/regmap/regmap.c b/drivers/base/regmap/regmap.c
index 78f43fb2fe84..d2f8a818d200 100644
--- a/drivers/base/regmap/regmap.c
+++ b/drivers/base/regmap/regmap.c
@@ -15,6 +15,7 @@
 #include <linux/export.h>
 #include <linux/mutex.h>
 #include <linux/err.h>
+#include <linux/of.h>
 #include <linux/rbtree.h>
 #include <linux/sched.h>
 
@@ -109,7 +110,7 @@ bool regmap_readable(struct regmap *map, unsigned int reg)
 
 bool regmap_volatile(struct regmap *map, unsigned int reg)
 {
-	if (!regmap_readable(map, reg))
+	if (!map->format.format_write && !regmap_readable(map, reg))
 		return false;
 
 	if (map->volatile_reg)
@@ -448,6 +449,71 @@ int regmap_attach_dev(struct device *dev, struct regmap *map,
 }
 EXPORT_SYMBOL_GPL(regmap_attach_dev);
 
+static enum regmap_endian regmap_get_reg_endian(const struct regmap_bus *bus,
+					const struct regmap_config *config)
+{
+	enum regmap_endian endian;
+
+	/* Retrieve the endianness specification from the regmap config */
+	endian = config->reg_format_endian;
+
+	/* If the regmap config specified a non-default value, use that */
+	if (endian != REGMAP_ENDIAN_DEFAULT)
+		return endian;
+
+	/* Retrieve the endianness specification from the bus config */
+	if (bus && bus->reg_format_endian_default)
+		endian = bus->reg_format_endian_default;
+
+	/* If the bus specified a non-default value, use that */
+	if (endian != REGMAP_ENDIAN_DEFAULT)
+		return endian;
+
+	/* Use this if no other value was found */
+	return REGMAP_ENDIAN_BIG;
+}
+
+static enum regmap_endian regmap_get_val_endian(struct device *dev,
+					const struct regmap_bus *bus,
+					const struct regmap_config *config)
+{
+	struct device_node *np;
+	enum regmap_endian endian;
+
+	/* Retrieve the endianness specification from the regmap config */
+	endian = config->val_format_endian;
+
+	/* If the regmap config specified a non-default value, use that */
+	if (endian != REGMAP_ENDIAN_DEFAULT)
+		return endian;
+
+	/* If the dev and dev->of_node exist try to get endianness from DT */
+	if (dev && dev->of_node) {
+		np = dev->of_node;
+
+		/* Parse the device's DT node for an endianness specification */
+		if (of_property_read_bool(np, "big-endian"))
+			endian = REGMAP_ENDIAN_BIG;
+		else if (of_property_read_bool(np, "little-endian"))
+			endian = REGMAP_ENDIAN_LITTLE;
+
+		/* If the endianness was specified in DT, use that */
+		if (endian != REGMAP_ENDIAN_DEFAULT)
+			return endian;
+	}
+
+	/* Retrieve the endianness specification from the bus config */
+	if (bus && bus->val_format_endian_default)
+		endian = bus->val_format_endian_default;
+
+	/* If the bus specified a non-default value, use that */
+	if (endian != REGMAP_ENDIAN_DEFAULT)
+		return endian;
+
+	/* Use this if no other value was found */
+	return REGMAP_ENDIAN_BIG;
+}
+
 /**
  * regmap_init(): Initialise register map
  *
@@ -551,17 +617,8 @@ struct regmap *regmap_init(struct device *dev,
 		map->reg_read  = _regmap_bus_read;
 	}
 
-	reg_endian = config->reg_format_endian;
-	if (reg_endian == REGMAP_ENDIAN_DEFAULT)
-		reg_endian = bus->reg_format_endian_default;
-	if (reg_endian == REGMAP_ENDIAN_DEFAULT)
-		reg_endian = REGMAP_ENDIAN_BIG;
-
-	val_endian = config->val_format_endian;
-	if (val_endian == REGMAP_ENDIAN_DEFAULT)
-		val_endian = bus->val_format_endian_default;
-	if (val_endian == REGMAP_ENDIAN_DEFAULT)
-		val_endian = REGMAP_ENDIAN_BIG;
+	reg_endian = regmap_get_reg_endian(bus, config);
+	val_endian = regmap_get_val_endian(dev, bus, config);
 
 	switch (config->reg_bits + map->reg_shift) {
 	case 2:
@@ -1408,7 +1465,7 @@ int _regmap_write(struct regmap *map, unsigned int reg,
 	}
 
 #ifdef LOG_DEVICE
-	if (strcmp(dev_name(map->dev), LOG_DEVICE) == 0)
+	if (map->dev && strcmp(dev_name(map->dev), LOG_DEVICE) == 0)
 		dev_info(map->dev, "%x <= %x\n", reg, val);
 #endif
 
@@ -1659,6 +1716,9 @@ out:
 	} else {
 		void *wval;
 
+		if (!val_count)
+			return -EINVAL;
+
 		wval = kmemdup(val, val_count * val_bytes, GFP_KERNEL);
 		if (!wval) {
 			dev_err(map->dev, "Error in memory allocation\n");
@@ -2058,7 +2118,7 @@ static int _regmap_read(struct regmap *map, unsigned int reg,
 	ret = map->reg_read(context, reg, val);
 	if (ret == 0) {
 #ifdef LOG_DEVICE
-		if (strcmp(dev_name(map->dev), LOG_DEVICE) == 0)
+		if (map->dev && strcmp(dev_name(map->dev), LOG_DEVICE) == 0)
 			dev_info(map->dev, "%x => %x\n", reg, *val);
 #endif
 
diff --git a/drivers/base/syscore.c b/drivers/base/syscore.c
index dbb8350ea8dc..8d98a329f6ea 100644
--- a/drivers/base/syscore.c
+++ b/drivers/base/syscore.c
@@ -9,7 +9,7 @@
 #include <linux/syscore_ops.h>
 #include <linux/mutex.h>
 #include <linux/module.h>
-#include <linux/interrupt.h>
+#include <linux/suspend.h>
 #include <trace/events/power.h>
 
 static LIST_HEAD(syscore_ops_list);
@@ -54,9 +54,8 @@ int syscore_suspend(void)
 	pr_debug("Checking wakeup interrupts\n");
 
 	/* Return error code if there are any wakeup interrupts pending. */
-	ret = check_wakeup_irqs();
-	if (ret)
-		return ret;
+	if (pm_wakeup_pending())
+		return -EBUSY;
 
 	WARN_ONCE(!irqs_disabled(),
 		"Interrupts enabled before system core suspend.\n");
diff --git a/drivers/bcma/Makefile b/drivers/bcma/Makefile
index 91290f7f61b8..838b4b9d352f 100644
--- a/drivers/bcma/Makefile
+++ b/drivers/bcma/Makefile
@@ -1,5 +1,6 @@
 bcma-y					+= main.o scan.o core.o sprom.o
 bcma-y					+= driver_chipcommon.o driver_chipcommon_pmu.o
+bcma-y					+= driver_chipcommon_b.o
 bcma-$(CONFIG_BCMA_SFLASH)		+= driver_chipcommon_sflash.o
 bcma-$(CONFIG_BCMA_NFLASH)		+= driver_chipcommon_nflash.o
 bcma-y					+= driver_pci.o
diff --git a/drivers/bcma/bcma_private.h b/drivers/bcma/bcma_private.h
index 09b632ad0fe2..b6412b2d748d 100644
--- a/drivers/bcma/bcma_private.h
+++ b/drivers/bcma/bcma_private.h
@@ -50,6 +50,10 @@ void bcma_chipco_serial_init(struct bcma_drv_cc *cc);
 extern struct platform_device bcma_pflash_dev;
 #endif /* CONFIG_BCMA_DRIVER_MIPS */
 
+/* driver_chipcommon_b.c */
+int bcma_core_chipcommon_b_init(struct bcma_drv_cc_b *ccb);
+void bcma_core_chipcommon_b_free(struct bcma_drv_cc_b *ccb);
+
 /* driver_chipcommon_pmu.c */
 u32 bcma_pmu_get_alp_clock(struct bcma_drv_cc *cc);
 u32 bcma_pmu_get_cpu_clock(struct bcma_drv_cc *cc);
@@ -84,6 +88,20 @@ extern int __init bcma_host_pci_init(void);
 extern void __exit bcma_host_pci_exit(void);
 #endif /* CONFIG_BCMA_HOST_PCI */
 
+/* host_soc.c */
+#if defined(CONFIG_BCMA_HOST_SOC) && defined(CONFIG_OF)
+extern int __init bcma_host_soc_register_driver(void);
+extern void __exit bcma_host_soc_unregister_driver(void);
+#else
+static inline int __init bcma_host_soc_register_driver(void)
+{
+	return 0;
+}
+static inline void __exit bcma_host_soc_unregister_driver(void)
+{
+}
+#endif /* CONFIG_BCMA_HOST_SOC && CONFIG_OF */
+
 /* driver_pci.c */
 u32 bcma_pcie_read(struct bcma_drv_pci *pc, u32 address);
 
diff --git a/drivers/bcma/driver_chipcommon_b.c b/drivers/bcma/driver_chipcommon_b.c
new file mode 100644
index 000000000000..c20b5f4ff290
--- /dev/null
+++ b/drivers/bcma/driver_chipcommon_b.c
@@ -0,0 +1,61 @@
+/*
+ * Broadcom specific AMBA
+ * ChipCommon B Unit driver
+ *
+ * Copyright 2014, Hauke Mehrtens <hauke@hauke-m.de>
+ *
+ * Licensed under the GNU/GPL. See COPYING for details.
+ */
+
+#include "bcma_private.h"
+#include <linux/export.h>
+#include <linux/bcma/bcma.h>
+
+static bool bcma_wait_reg(struct bcma_bus *bus, void __iomem *addr, u32 mask,
+			  u32 value, int timeout)
+{
+	unsigned long deadline = jiffies + timeout;
+	u32 val;
+
+	do {
+		val = readl(addr);
+		if ((val & mask) == value)
+			return true;
+		cpu_relax();
+		udelay(10);
+	} while (!time_after_eq(jiffies, deadline));
+
+	bcma_err(bus, "Timeout waiting for register %p\n", addr);
+
+	return false;
+}
+
+void bcma_chipco_b_mii_write(struct bcma_drv_cc_b *ccb, u32 offset, u32 value)
+{
+	struct bcma_bus *bus = ccb->core->bus;
+
+	writel(offset, ccb->mii + 0x00);
+	bcma_wait_reg(bus, ccb->mii + 0x00, 0x0100, 0x0000, 100);
+	writel(value, ccb->mii + 0x04);
+	bcma_wait_reg(bus, ccb->mii + 0x00, 0x0100, 0x0000, 100);
+}
+EXPORT_SYMBOL_GPL(bcma_chipco_b_mii_write);
+
+int bcma_core_chipcommon_b_init(struct bcma_drv_cc_b *ccb)
+{
+	if (ccb->setup_done)
+		return 0;
+
+	ccb->setup_done = 1;
+	ccb->mii = ioremap_nocache(ccb->core->addr_s[1], BCMA_CORE_SIZE);
+	if (!ccb->mii)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void bcma_core_chipcommon_b_free(struct bcma_drv_cc_b *ccb)
+{
+	if (ccb->mii)
+		iounmap(ccb->mii);
+}
diff --git a/drivers/bcma/driver_gpio.c b/drivers/bcma/driver_gpio.c
index aec9f850b4a8..706b9ae0dcfb 100644
--- a/drivers/bcma/driver_gpio.c
+++ b/drivers/bcma/driver_gpio.c
@@ -76,7 +76,7 @@ static void bcma_gpio_free(struct gpio_chip *chip, unsigned gpio)
 	bcma_chipco_gpio_pullup(cc, 1 << gpio, 0);
 }
 
-#if IS_BUILTIN(CONFIG_BCMA_HOST_SOC)
+#if IS_BUILTIN(CONFIG_BCM47XX)
 static int bcma_gpio_to_irq(struct gpio_chip *chip, unsigned gpio)
 {
 	struct bcma_drv_cc *cc = bcma_gpio_get_cc(chip);
@@ -215,9 +215,13 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 	chip->set		= bcma_gpio_set_value;
 	chip->direction_input	= bcma_gpio_direction_input;
 	chip->direction_output	= bcma_gpio_direction_output;
-#if IS_BUILTIN(CONFIG_BCMA_HOST_SOC)
+#if IS_BUILTIN(CONFIG_BCM47XX)
 	chip->to_irq		= bcma_gpio_to_irq;
 #endif
+#if IS_BUILTIN(CONFIG_OF)
+	if (cc->core->bus->hosttype == BCMA_HOSTTYPE_SOC)
+		chip->of_node	= cc->core->dev.of_node;
+#endif
 	switch (cc->core->bus->chipinfo.id) {
 	case BCMA_CHIP_ID_BCM5357:
 	case BCMA_CHIP_ID_BCM53572:
@@ -251,5 +255,6 @@ int bcma_gpio_init(struct bcma_drv_cc *cc)
 int bcma_gpio_unregister(struct bcma_drv_cc *cc)
 {
 	bcma_gpio_irq_domain_exit(cc);
-	return gpiochip_remove(&cc->gpio);
+	gpiochip_remove(&cc->gpio);
+	return 0;
 }
diff --git a/drivers/bcma/driver_mips.c b/drivers/bcma/driver_mips.c
index 11115bbe115c..004d6aa671ce 100644
--- a/drivers/bcma/driver_mips.c
+++ b/drivers/bcma/driver_mips.c
@@ -21,6 +21,14 @@
 #include <linux/serial_reg.h>
 #include <linux/time.h>
 
+enum bcma_boot_dev {
+	BCMA_BOOT_DEV_UNK = 0,
+	BCMA_BOOT_DEV_ROM,
+	BCMA_BOOT_DEV_PARALLEL,
+	BCMA_BOOT_DEV_SERIAL,
+	BCMA_BOOT_DEV_NAND,
+};
+
 static const char * const part_probes[] = { "bcm47xxpart", NULL };
 
 static struct physmap_flash_data bcma_pflash_data = {
@@ -229,11 +237,51 @@ u32 bcma_cpu_clock(struct bcma_drv_mips *mcore)
 }
 EXPORT_SYMBOL(bcma_cpu_clock);
 
+static enum bcma_boot_dev bcma_boot_dev(struct bcma_bus *bus)
+{
+	struct bcma_drv_cc *cc = &bus->drv_cc;
+	u8 cc_rev = cc->core->id.rev;
+
+	if (cc_rev == 42) {
+		struct bcma_device *core;
+
+		core = bcma_find_core(bus, BCMA_CORE_NS_ROM);
+		if (core) {
+			switch (bcma_aread32(core, BCMA_IOST) &
+				BCMA_NS_ROM_IOST_BOOT_DEV_MASK) {
+			case BCMA_NS_ROM_IOST_BOOT_DEV_NOR:
+				return BCMA_BOOT_DEV_SERIAL;
+			case BCMA_NS_ROM_IOST_BOOT_DEV_NAND:
+				return BCMA_BOOT_DEV_NAND;
+			case BCMA_NS_ROM_IOST_BOOT_DEV_ROM:
+			default:
+				return BCMA_BOOT_DEV_ROM;
+			}
+		}
+	} else {
+		if (cc_rev == 38) {
+			if (cc->status & BCMA_CC_CHIPST_5357_NAND_BOOT)
+				return BCMA_BOOT_DEV_NAND;
+			else if (cc->status & BIT(5))
+				return BCMA_BOOT_DEV_ROM;
+		}
+
+		if ((cc->capabilities & BCMA_CC_CAP_FLASHT) ==
+		    BCMA_CC_FLASHT_PARA)
+			return BCMA_BOOT_DEV_PARALLEL;
+		else
+			return BCMA_BOOT_DEV_SERIAL;
+	}
+
+	return BCMA_BOOT_DEV_SERIAL;
+}
+
 static void bcma_core_mips_flash_detect(struct bcma_drv_mips *mcore)
 {
 	struct bcma_bus *bus = mcore->core->bus;
 	struct bcma_drv_cc *cc = &bus->drv_cc;
 	struct bcma_pflash *pflash = &cc->pflash;
+	enum bcma_boot_dev boot_dev;
 
 	switch (cc->capabilities & BCMA_CC_CAP_FLASHT) {
 	case BCMA_CC_FLASHT_STSER:
@@ -269,6 +317,20 @@ static void bcma_core_mips_flash_detect(struct bcma_drv_mips *mcore)
 			bcma_nflash_init(cc);
 		}
 	}
+
+	/* Determine flash type this SoC boots from */
+	boot_dev = bcma_boot_dev(bus);
+	switch (boot_dev) {
+	case BCMA_BOOT_DEV_PARALLEL:
+	case BCMA_BOOT_DEV_SERIAL:
+		/* TODO: Init NVRAM using BCMA_SOC_FLASH2 window */
+		break;
+	case BCMA_BOOT_DEV_NAND:
+		/* TODO: Init NVRAM using BCMA_SOC_FLASH1 window */
+		break;
+	default:
+		break;
+	}
 }
 
 void bcma_core_mips_early_init(struct bcma_drv_mips *mcore)
diff --git a/drivers/bcma/host_pci.c b/drivers/bcma/host_pci.c
index 294a7dd25190..1e5ac0a79696 100644
--- a/drivers/bcma/host_pci.c
+++ b/drivers/bcma/host_pci.c
@@ -208,6 +208,9 @@ static int bcma_host_pci_probe(struct pci_dev *dev,
 	bus->boardinfo.vendor = bus->host_pci->subsystem_vendor;
 	bus->boardinfo.type = bus->host_pci->subsystem_device;
 
+	/* Initialize struct, detect chip */
+	bcma_init_bus(bus);
+
 	/* Register */
 	err = bcma_bus_register(bus);
 	if (err)
@@ -282,6 +285,7 @@ static const struct pci_device_id bcma_pci_bridge_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x43a9) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x43aa) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4727) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 43227) },	/* 0xA8DB */
 	{ 0, },
 };
 MODULE_DEVICE_TABLE(pci, bcma_pci_bridge_tbl);
diff --git a/drivers/bcma/host_soc.c b/drivers/bcma/host_soc.c
index 3475e600011a..335cbcfd945b 100644
--- a/drivers/bcma/host_soc.c
+++ b/drivers/bcma/host_soc.c
@@ -7,6 +7,9 @@
 
 #include "bcma_private.h"
 #include "scan.h"
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
 #include <linux/bcma/bcma.h>
 #include <linux/bcma/bcma_soc.h>
 
@@ -134,12 +137,16 @@ static void bcma_host_soc_block_write(struct bcma_device *core,
 
 static u32 bcma_host_soc_aread32(struct bcma_device *core, u16 offset)
 {
+	if (WARN_ONCE(!core->io_wrap, "Accessed core has no wrapper/agent\n"))
+		return ~0;
 	return readl(core->io_wrap + offset);
 }
 
 static void bcma_host_soc_awrite32(struct bcma_device *core, u16 offset,
 				  u32 value)
 {
+	if (WARN_ONCE(!core->io_wrap, "Accessed core has no wrapper/agent\n"))
+		return;
 	writel(value, core->io_wrap + offset);
 }
 
@@ -161,7 +168,6 @@ static const struct bcma_host_ops bcma_host_soc_ops = {
 int __init bcma_host_soc_register(struct bcma_soc *soc)
 {
 	struct bcma_bus *bus = &soc->bus;
-	int err;
 
 	/* iomap only first core. We have to read some register on this core
 	 * to scan the bus.
@@ -173,11 +179,100 @@ int __init bcma_host_soc_register(struct bcma_soc *soc)
 	/* Host specific */
 	bus->hosttype = BCMA_HOSTTYPE_SOC;
 	bus->ops = &bcma_host_soc_ops;
+	bus->host_pdev = NULL;
 
-	/* Register */
+	/* Initialize struct, detect chip */
+	bcma_init_bus(bus);
+
+	return 0;
+}
+
+int __init bcma_host_soc_init(struct bcma_soc *soc)
+{
+	struct bcma_bus *bus = &soc->bus;
+	int err;
+
+	/* Scan bus and initialize it */
 	err = bcma_bus_early_register(bus, &soc->core_cc, &soc->core_mips);
 	if (err)
 		iounmap(bus->mmio);
 
 	return err;
 }
+
+#ifdef CONFIG_OF
+static int bcma_host_soc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct bcma_bus *bus;
+	int err;
+
+	/* Alloc */
+	bus = devm_kzalloc(dev, sizeof(*bus), GFP_KERNEL);
+	if (!bus)
+		return -ENOMEM;
+
+	/* Map MMIO */
+	bus->mmio = of_iomap(np, 0);
+	if (!bus->mmio)
+		return -ENOMEM;
+
+	/* Host specific */
+	bus->hosttype = BCMA_HOSTTYPE_SOC;
+	bus->ops = &bcma_host_soc_ops;
+	bus->host_pdev = pdev;
+
+	/* Initialize struct, detect chip */
+	bcma_init_bus(bus);
+
+	/* Register */
+	err = bcma_bus_register(bus);
+	if (err)
+		goto err_unmap_mmio;
+
+	platform_set_drvdata(pdev, bus);
+
+	return err;
+
+err_unmap_mmio:
+	iounmap(bus->mmio);
+	return err;
+}
+
+static int bcma_host_soc_remove(struct platform_device *pdev)
+{
+	struct bcma_bus *bus = platform_get_drvdata(pdev);
+
+	bcma_bus_unregister(bus);
+	iounmap(bus->mmio);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static const struct of_device_id bcma_host_soc_of_match[] = {
+	{ .compatible = "brcm,bus-axi", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, bcma_host_soc_of_match);
+
+static struct platform_driver bcma_host_soc_driver = {
+	.driver = {
+		.name = "bcma-host-soc",
+		.of_match_table = bcma_host_soc_of_match,
+	},
+	.probe		= bcma_host_soc_probe,
+	.remove		= bcma_host_soc_remove,
+};
+
+int __init bcma_host_soc_register_driver(void)
+{
+	return platform_driver_register(&bcma_host_soc_driver);
+}
+
+void __exit bcma_host_soc_unregister_driver(void)
+{
+	platform_driver_unregister(&bcma_host_soc_driver);
+}
+#endif /* CONFIG_OF */
diff --git a/drivers/bcma/main.c b/drivers/bcma/main.c
index 0ff8d58831ef..d1656c2f70af 100644
--- a/drivers/bcma/main.c
+++ b/drivers/bcma/main.c
@@ -10,6 +10,7 @@
 #include <linux/platform_device.h>
 #include <linux/bcma/bcma.h>
 #include <linux/slab.h>
+#include <linux/of_address.h>
 
 MODULE_DESCRIPTION("Broadcom's specific AMBA driver");
 MODULE_LICENSE("GPL");
@@ -120,16 +121,103 @@ static void bcma_release_core_dev(struct device *dev)
 	kfree(core);
 }
 
-static int bcma_register_cores(struct bcma_bus *bus)
+static bool bcma_is_core_needed_early(u16 core_id)
+{
+	switch (core_id) {
+	case BCMA_CORE_NS_NAND:
+	case BCMA_CORE_NS_QSPI:
+		return true;
+	}
+
+	return false;
+}
+
+#ifdef CONFIG_OF
+static struct device_node *bcma_of_find_child_device(struct platform_device *parent,
+						     struct bcma_device *core)
+{
+	struct device_node *node;
+	u64 size;
+	const __be32 *reg;
+
+	if (!parent || !parent->dev.of_node)
+		return NULL;
+
+	for_each_child_of_node(parent->dev.of_node, node) {
+		reg = of_get_address(node, 0, &size, NULL);
+		if (!reg)
+			continue;
+		if (of_translate_address(node, reg) == core->addr)
+			return node;
+	}
+	return NULL;
+}
+
+static void bcma_of_fill_device(struct platform_device *parent,
+				struct bcma_device *core)
+{
+	struct device_node *node;
+
+	node = bcma_of_find_child_device(parent, core);
+	if (node)
+		core->dev.of_node = node;
+}
+#else
+static void bcma_of_fill_device(struct platform_device *parent,
+				struct bcma_device *core)
+{
+}
+#endif /* CONFIG_OF */
+
+static void bcma_register_core(struct bcma_bus *bus, struct bcma_device *core)
+{
+	int err;
+
+	core->dev.release = bcma_release_core_dev;
+	core->dev.bus = &bcma_bus_type;
+	dev_set_name(&core->dev, "bcma%d:%d", bus->num, core->core_index);
+
+	switch (bus->hosttype) {
+	case BCMA_HOSTTYPE_PCI:
+		core->dev.parent = &bus->host_pci->dev;
+		core->dma_dev = &bus->host_pci->dev;
+		core->irq = bus->host_pci->irq;
+		break;
+	case BCMA_HOSTTYPE_SOC:
+		core->dev.dma_mask = &core->dev.coherent_dma_mask;
+		if (bus->host_pdev) {
+			core->dma_dev = &bus->host_pdev->dev;
+			core->dev.parent = &bus->host_pdev->dev;
+			bcma_of_fill_device(bus->host_pdev, core);
+		} else {
+			core->dma_dev = &core->dev;
+		}
+		break;
+	case BCMA_HOSTTYPE_SDIO:
+		break;
+	}
+
+	err = device_register(&core->dev);
+	if (err) {
+		bcma_err(bus, "Could not register dev for core 0x%03X\n",
+			 core->id.id);
+		put_device(&core->dev);
+		return;
+	}
+	core->dev_registered = true;
+}
+
+static int bcma_register_devices(struct bcma_bus *bus)
 {
 	struct bcma_device *core;
-	int err, dev_id = 0;
+	int err;
 
 	list_for_each_entry(core, &bus->cores, list) {
 		/* We support that cores ourself */
 		switch (core->id.id) {
 		case BCMA_CORE_4706_CHIPCOMMON:
 		case BCMA_CORE_CHIPCOMMON:
+		case BCMA_CORE_NS_CHIPCOMMON_B:
 		case BCMA_CORE_PCI:
 		case BCMA_CORE_PCIE:
 		case BCMA_CORE_PCIE2:
@@ -138,39 +226,16 @@ static int bcma_register_cores(struct bcma_bus *bus)
 			continue;
 		}
 
+		/* Early cores were already registered */
+		if (bcma_is_core_needed_early(core->id.id))
+			continue;
+
 		/* Only first GMAC core on BCM4706 is connected and working */
 		if (core->id.id == BCMA_CORE_4706_MAC_GBIT &&
 		    core->core_unit > 0)
 			continue;
 
-		core->dev.release = bcma_release_core_dev;
-		core->dev.bus = &bcma_bus_type;
-		dev_set_name(&core->dev, "bcma%d:%d", bus->num, dev_id);
-
-		switch (bus->hosttype) {
-		case BCMA_HOSTTYPE_PCI:
-			core->dev.parent = &bus->host_pci->dev;
-			core->dma_dev = &bus->host_pci->dev;
-			core->irq = bus->host_pci->irq;
-			break;
-		case BCMA_HOSTTYPE_SOC:
-			core->dev.dma_mask = &core->dev.coherent_dma_mask;
-			core->dma_dev = &core->dev;
-			break;
-		case BCMA_HOSTTYPE_SDIO:
-			break;
-		}
-
-		err = device_register(&core->dev);
-		if (err) {
-			bcma_err(bus,
-				 "Could not register dev for core 0x%03X\n",
-				 core->id.id);
-			put_device(&core->dev);
-			continue;
-		}
-		core->dev_registered = true;
-		dev_id++;
+		bcma_register_core(bus, core);
 	}
 
 #ifdef CONFIG_BCMA_DRIVER_MIPS
@@ -247,6 +312,12 @@ int bcma_bus_register(struct bcma_bus *bus)
 		bcma_core_chipcommon_early_init(&bus->drv_cc);
 	}
 
+	/* Cores providing flash access go before SPROM init */
+	list_for_each_entry(core, &bus->cores, list) {
+		if (bcma_is_core_needed_early(core->id.id))
+			bcma_register_core(bus, core);
+	}
+
 	/* Try to get SPROM */
 	err = bcma_sprom_get(bus);
 	if (err == -ENOENT) {
@@ -261,6 +332,13 @@ int bcma_bus_register(struct bcma_bus *bus)
 		bcma_core_chipcommon_init(&bus->drv_cc);
 	}
 
+	/* Init CC core */
+	core = bcma_find_core(bus, BCMA_CORE_NS_CHIPCOMMON_B);
+	if (core) {
+		bus->drv_cc_b.core = core;
+		bcma_core_chipcommon_b_init(&bus->drv_cc_b);
+	}
+
 	/* Init MIPS core */
 	core = bcma_find_core(bus, BCMA_CORE_MIPS_74K);
 	if (core) {
@@ -297,7 +375,7 @@ int bcma_bus_register(struct bcma_bus *bus)
 	}
 
 	/* Register found cores */
-	bcma_register_cores(bus);
+	bcma_register_devices(bus);
 
 	bcma_info(bus, "Bus registered\n");
 
@@ -315,6 +393,8 @@ void bcma_bus_unregister(struct bcma_bus *bus)
 	else if (err)
 		bcma_err(bus, "Can not unregister GPIO driver: %i\n", err);
 
+	bcma_core_chipcommon_b_free(&bus->drv_cc_b);
+
 	cores[0] = bcma_find_core(bus, BCMA_CORE_MIPS_74K);
 	cores[1] = bcma_find_core(bus, BCMA_CORE_PCIE);
 	cores[2] = bcma_find_core(bus, BCMA_CORE_4706_MAC_GBIT_COMMON);
@@ -334,8 +414,6 @@ int __init bcma_bus_early_register(struct bcma_bus *bus,
 	struct bcma_device *core;
 	struct bcma_device_id match;
 
-	bcma_init_bus(bus);
-
 	match.manuf = BCMA_MANUF_BCM;
 	match.id = bcma_cc_core_id(bus);
 	match.class = BCMA_CL_SIM;
@@ -494,6 +572,11 @@ static int __init bcma_modinit(void)
 	if (err)
 		return err;
 
+	err = bcma_host_soc_register_driver();
+	if (err) {
+		pr_err("SoC host initialization failed\n");
+		err = 0;
+	}
 #ifdef CONFIG_BCMA_HOST_PCI
 	err = bcma_host_pci_init();
 	if (err) {
@@ -511,6 +594,7 @@ static void __exit bcma_modexit(void)
 #ifdef CONFIG_BCMA_HOST_PCI
 	bcma_host_pci_exit();
 #endif
+	bcma_host_soc_unregister_driver();
 	bus_unregister(&bcma_bus_type);
 }
 module_exit(bcma_modexit)
diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index b4764c6bcf17..14b56561a36f 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -276,7 +276,7 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 			      struct bcma_device *core)
 {
 	u32 tmp;
-	u8 i, j;
+	u8 i, j, k;
 	s32 cia, cib;
 	u8 ports[2], wrappers[2];
 
@@ -314,6 +314,7 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 		/* Some specific cores don't need wrappers */
 		switch (core->id.id) {
 		case BCMA_CORE_4706_MAC_GBIT_COMMON:
+		case BCMA_CORE_NS_CHIPCOMMON_B:
 		/* Not used yet: case BCMA_CORE_OOB_ROUTER: */
 			break;
 		default:
@@ -367,6 +368,7 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 	core->addr = tmp;
 
 	/* get & parse slave ports */
+	k = 0;
 	for (i = 0; i < ports[1]; i++) {
 		for (j = 0; ; j++) {
 			tmp = bcma_erom_get_addr_desc(bus, eromptr,
@@ -376,9 +378,9 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 				/* pr_debug("erom: slave port %d "
 				 * "has %d descriptors\n", i, j); */
 				break;
-			} else {
-				if (i == 0 && j == 0)
-					core->addr1 = tmp;
+			} else if (k < ARRAY_SIZE(core->addr_s)) {
+				core->addr_s[k] = tmp;
+				k++;
 			}
 		}
 	}
@@ -421,10 +423,13 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 		core->io_addr = ioremap_nocache(core->addr, BCMA_CORE_SIZE);
 		if (!core->io_addr)
 			return -ENOMEM;
-		core->io_wrap = ioremap_nocache(core->wrap, BCMA_CORE_SIZE);
-		if (!core->io_wrap) {
-			iounmap(core->io_addr);
-			return -ENOMEM;
+		if (core->wrap) {
+			core->io_wrap = ioremap_nocache(core->wrap,
+							BCMA_CORE_SIZE);
+			if (!core->io_wrap) {
+				iounmap(core->io_addr);
+				return -ENOMEM;
+			}
 		}
 	}
 	return 0;
@@ -434,9 +439,7 @@ void bcma_init_bus(struct bcma_bus *bus)
 {
 	s32 tmp;
 	struct bcma_chipinfo *chipinfo = &(bus->chipinfo);
-
-	if (bus->init_done)
-		return;
+	char chip_id[8];
 
 	INIT_LIST_HEAD(&bus->cores);
 	bus->nr_cores = 0;
@@ -447,10 +450,11 @@ void bcma_init_bus(struct bcma_bus *bus)
 	chipinfo->id = (tmp & BCMA_CC_ID_ID) >> BCMA_CC_ID_ID_SHIFT;
 	chipinfo->rev = (tmp & BCMA_CC_ID_REV) >> BCMA_CC_ID_REV_SHIFT;
 	chipinfo->pkg = (tmp & BCMA_CC_ID_PKG) >> BCMA_CC_ID_PKG_SHIFT;
-	bcma_info(bus, "Found chip with id 0x%04X, rev 0x%02X and package 0x%02X\n",
-		  chipinfo->id, chipinfo->rev, chipinfo->pkg);
 
-	bus->init_done = true;
+	snprintf(chip_id, ARRAY_SIZE(chip_id),
+		 (chipinfo->id > 0x9999) ? "%d" : "0x%04X", chipinfo->id);
+	bcma_info(bus, "Found chip with id %s, rev 0x%02X and package 0x%02X\n",
+		  chip_id, chipinfo->rev, chipinfo->pkg);
 }
 
 int bcma_bus_scan(struct bcma_bus *bus)
@@ -460,8 +464,6 @@ int bcma_bus_scan(struct bcma_bus *bus)
 
 	int err, core_num = 0;
 
-	bcma_init_bus(bus);
-
 	erombase = bcma_scan_read32(bus, 0, BCMA_CC_EROM);
 	if (bus->hosttype == BCMA_HOSTTYPE_SOC) {
 		eromptr = ioremap_nocache(erombase, BCMA_CORE_SIZE);
diff --git a/drivers/block/brd.c b/drivers/block/brd.c
index c7d138eca731..3598110d2cef 100644
--- a/drivers/block/brd.c
+++ b/drivers/block/brd.c
@@ -442,12 +442,15 @@ static int rd_nr;
 int rd_size = CONFIG_BLK_DEV_RAM_SIZE;
 static int max_part;
 static int part_shift;
+static int part_show = 0;
 module_param(rd_nr, int, S_IRUGO);
 MODULE_PARM_DESC(rd_nr, "Maximum number of brd devices");
 module_param(rd_size, int, S_IRUGO);
 MODULE_PARM_DESC(rd_size, "Size of each RAM disk in kbytes.");
 module_param(max_part, int, S_IRUGO);
 MODULE_PARM_DESC(max_part, "Maximum number of partitions per RAM disk");
+module_param(part_show, int, S_IRUGO);
+MODULE_PARM_DESC(part_show, "Control RAM disk visibility in /proc/partitions");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS_BLOCKDEV_MAJOR(RAMDISK_MAJOR);
 MODULE_ALIAS("rd");
@@ -501,7 +504,8 @@ static struct brd_device *brd_alloc(int i)
 	disk->fops		= &brd_fops;
 	disk->private_data	= brd;
 	disk->queue		= brd->brd_queue;
-	disk->flags |= GENHD_FL_SUPPRESS_PARTITION_INFO;
+	if (!part_show)
+		disk->flags |= GENHD_FL_SUPPRESS_PARTITION_INFO;
 	sprintf(disk->disk_name, "ram%d", i);
 	set_capacity(disk, rd_size * 2);
 
diff --git a/drivers/block/drbd/drbd_actlog.c b/drivers/block/drbd/drbd_actlog.c
index d26a3fa63688..a2dfa169237d 100644
--- a/drivers/block/drbd/drbd_actlog.c
+++ b/drivers/block/drbd/drbd_actlog.c
@@ -158,14 +158,14 @@ static int _drbd_md_sync_page_io(struct drbd_device *device,
 	if (bio_add_page(bio, device->md_io.page, size, 0) != size)
 		goto out;
 	bio->bi_private = device;
-	bio->bi_end_io = drbd_md_io_complete;
+	bio->bi_end_io = drbd_md_endio;
 	bio->bi_rw = rw;
 
 	if (!(rw & WRITE) && device->state.disk == D_DISKLESS && device->ldev == NULL)
 		/* special case, drbd_md_read() during drbd_adm_attach(): no get_ldev */
 		;
 	else if (!get_ldev_if_state(device, D_ATTACHING)) {
-		/* Corresponding put_ldev in drbd_md_io_complete() */
+		/* Corresponding put_ldev in drbd_md_endio() */
 		drbd_err(device, "ASSERT FAILED: get_ldev_if_state() == 1 in _drbd_md_sync_page_io()\n");
 		err = -ENODEV;
 		goto out;
diff --git a/drivers/block/drbd/drbd_bitmap.c b/drivers/block/drbd/drbd_bitmap.c
index 426c97aef900..434c77dcc99e 100644
--- a/drivers/block/drbd/drbd_bitmap.c
+++ b/drivers/block/drbd/drbd_bitmap.c
@@ -941,7 +941,7 @@ static void drbd_bm_aio_ctx_destroy(struct kref *kref)
 }
 
 /* bv_page may be a copy, or may be the original */
-static void bm_async_io_complete(struct bio *bio, int error)
+static void drbd_bm_endio(struct bio *bio, int error)
 {
 	struct drbd_bm_aio_ctx *ctx = bio->bi_private;
 	struct drbd_device *device = ctx->device;
@@ -1027,7 +1027,7 @@ static void bm_page_io_async(struct drbd_bm_aio_ctx *ctx, int page_nr) __must_ho
 	 * according to api.  Do we want to assert that? */
 	bio_add_page(bio, page, len, 0);
 	bio->bi_private = ctx;
-	bio->bi_end_io = bm_async_io_complete;
+	bio->bi_end_io = drbd_bm_endio;
 
 	if (drbd_insert_fault(device, (rw & WRITE) ? DRBD_FAULT_MD_WR : DRBD_FAULT_MD_RD)) {
 		bio->bi_rw |= rw;
@@ -1125,7 +1125,7 @@ static int bm_rw(struct drbd_device *device, const unsigned int flags, unsigned
 	}
 
 	/*
-	 * We initialize ctx->in_flight to one to make sure bm_async_io_complete
+	 * We initialize ctx->in_flight to one to make sure drbd_bm_endio
 	 * will not set ctx->done early, and decrement / test it here.  If there
 	 * are still some bios in flight, we need to wait for them here.
 	 * If all IO is done already (or nothing had been submitted), there is
diff --git a/drivers/block/drbd/drbd_debugfs.c b/drivers/block/drbd/drbd_debugfs.c
index 5c20b18540b8..900d4d3272d1 100644
--- a/drivers/block/drbd/drbd_debugfs.c
+++ b/drivers/block/drbd/drbd_debugfs.c
@@ -695,7 +695,7 @@ static void resync_dump_detail(struct seq_file *m, struct lc_element *e)
 {
        struct bm_extent *bme = lc_entry(e, struct bm_extent, lce);
 
-       seq_printf(m, "%5d %s %s %s\n", bme->rs_left,
+       seq_printf(m, "%5d %s %s %s", bme->rs_left,
 		  test_bit(BME_NO_WRITES, &bme->flags) ? "NO_WRITES" : "---------",
 		  test_bit(BME_LOCKED, &bme->flags) ? "LOCKED" : "------",
 		  test_bit(BME_PRIORITY, &bme->flags) ? "PRIORITY" : "--------"
diff --git a/drivers/block/drbd/drbd_int.h b/drivers/block/drbd/drbd_int.h
index 1a000016ccdf..9b22f8f01b57 100644
--- a/drivers/block/drbd/drbd_int.h
+++ b/drivers/block/drbd/drbd_int.h
@@ -61,8 +61,6 @@
 # define __must_hold(x)
 #endif
 
-#define __no_warn(lock, stmt) do { __acquire(lock); stmt; __release(lock); } while (0)
-
 /* module parameter, defined in drbd_main.c */
 extern unsigned int minor_count;
 extern bool disable_sendpage;
@@ -1483,7 +1481,7 @@ extern int drbd_khelper(struct drbd_device *device, char *cmd);
 
 /* drbd_worker.c */
 /* bi_end_io handlers */
-extern void drbd_md_io_complete(struct bio *bio, int error);
+extern void drbd_md_endio(struct bio *bio, int error);
 extern void drbd_peer_request_endio(struct bio *bio, int error);
 extern void drbd_request_endio(struct bio *bio, int error);
 extern int drbd_worker(struct drbd_thread *thi);
@@ -2100,16 +2098,19 @@ static inline bool is_sync_state(enum drbd_conns connection_state)
 
 /**
  * get_ldev() - Increase the ref count on device->ldev. Returns 0 if there is no ldev
- * @M:		DRBD device.
+ * @_device:		DRBD device.
+ * @_min_state:		Minimum device state required for success.
  *
  * You have to call put_ldev() when finished working with device->ldev.
  */
-#define get_ldev(M) __cond_lock(local, _get_ldev_if_state(M,D_INCONSISTENT))
-#define get_ldev_if_state(M,MINS) __cond_lock(local, _get_ldev_if_state(M,MINS))
+#define get_ldev_if_state(_device, _min_state)				\
+	(_get_ldev_if_state((_device), (_min_state)) ?			\
+	 ({ __acquire(x); true; }) : false)
+#define get_ldev(_device) get_ldev_if_state(_device, D_INCONSISTENT)
 
 static inline void put_ldev(struct drbd_device *device)
 {
-	enum drbd_disk_state ds = device->state.disk;
+	enum drbd_disk_state disk_state = device->state.disk;
 	/* We must check the state *before* the atomic_dec becomes visible,
 	 * or we have a theoretical race where someone hitting zero,
 	 * while state still D_FAILED, will then see D_DISKLESS in the
@@ -2122,10 +2123,10 @@ static inline void put_ldev(struct drbd_device *device)
 	__release(local);
 	D_ASSERT(device, i >= 0);
 	if (i == 0) {
-		if (ds == D_DISKLESS)
+		if (disk_state == D_DISKLESS)
 			/* even internal references gone, safe to destroy */
 			drbd_device_post_work(device, DESTROY_DISK);
-		if (ds == D_FAILED)
+		if (disk_state == D_FAILED)
 			/* all application IO references gone. */
 			if (!test_and_set_bit(GOING_DISKLESS, &device->flags))
 				drbd_device_post_work(device, GO_DISKLESS);
diff --git a/drivers/block/drbd/drbd_interval.c b/drivers/block/drbd/drbd_interval.c
index 89c497c630b4..51b25ad85251 100644
--- a/drivers/block/drbd/drbd_interval.c
+++ b/drivers/block/drbd/drbd_interval.c
@@ -37,40 +37,8 @@ compute_subtree_last(struct drbd_interval *node)
 	return max;
 }
 
-static void augment_propagate(struct rb_node *rb, struct rb_node *stop)
-{
-	while (rb != stop) {
-		struct drbd_interval *node = rb_entry(rb, struct drbd_interval, rb);
-		sector_t subtree_last = compute_subtree_last(node);
-		if (node->end == subtree_last)
-			break;
-		node->end = subtree_last;
-		rb = rb_parent(&node->rb);
-	}
-}
-
-static void augment_copy(struct rb_node *rb_old, struct rb_node *rb_new)
-{
-	struct drbd_interval *old = rb_entry(rb_old, struct drbd_interval, rb);
-	struct drbd_interval *new = rb_entry(rb_new, struct drbd_interval, rb);
-
-	new->end = old->end;
-}
-
-static void augment_rotate(struct rb_node *rb_old, struct rb_node *rb_new)
-{
-	struct drbd_interval *old = rb_entry(rb_old, struct drbd_interval, rb);
-	struct drbd_interval *new = rb_entry(rb_new, struct drbd_interval, rb);
-
-	new->end = old->end;
-	old->end = compute_subtree_last(old);
-}
-
-static const struct rb_augment_callbacks augment_callbacks = {
-	augment_propagate,
-	augment_copy,
-	augment_rotate,
-};
+RB_DECLARE_CALLBACKS(static, augment_callbacks, struct drbd_interval, rb,
+		     sector_t, end, compute_subtree_last);
 
 /**
  * drbd_insert_interval  -  insert a new interval into a tree
@@ -79,6 +47,7 @@ bool
 drbd_insert_interval(struct rb_root *root, struct drbd_interval *this)
 {
 	struct rb_node **new = &root->rb_node, *parent = NULL;
+	sector_t this_end = this->sector + (this->size >> 9);
 
 	BUG_ON(!IS_ALIGNED(this->size, 512));
 
@@ -87,6 +56,8 @@ drbd_insert_interval(struct rb_root *root, struct drbd_interval *this)
 			rb_entry(*new, struct drbd_interval, rb);
 
 		parent = *new;
+		if (here->end < this_end)
+			here->end = this_end;
 		if (this->sector < here->sector)
 			new = &(*new)->rb_left;
 		else if (this->sector > here->sector)
@@ -99,6 +70,7 @@ drbd_insert_interval(struct rb_root *root, struct drbd_interval *this)
 			return false;
 	}
 
+	this->end = this_end;
 	rb_link_node(&this->rb, parent, new);
 	rb_insert_augmented(&this->rb, root, &augment_callbacks);
 	return true;
diff --git a/drivers/block/drbd/drbd_main.c b/drivers/block/drbd/drbd_main.c
index 9b465bb68487..973c185c9cfe 100644
--- a/drivers/block/drbd/drbd_main.c
+++ b/drivers/block/drbd/drbd_main.c
@@ -1622,13 +1622,13 @@ int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *
 	struct drbd_socket *sock;
 	struct p_data *p;
 	unsigned int dp_flags = 0;
-	int dgs;
+	int digest_size;
 	int err;
 
 	sock = &peer_device->connection->data;
 	p = drbd_prepare_command(peer_device, sock);
-	dgs = peer_device->connection->integrity_tfm ?
-	      crypto_hash_digestsize(peer_device->connection->integrity_tfm) : 0;
+	digest_size = peer_device->connection->integrity_tfm ?
+		      crypto_hash_digestsize(peer_device->connection->integrity_tfm) : 0;
 
 	if (!p)
 		return -EIO;
@@ -1659,9 +1659,9 @@ int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *
 
 	/* our digest is still only over the payload.
 	 * TRIM does not carry any payload. */
-	if (dgs)
+	if (digest_size)
 		drbd_csum_bio(peer_device->connection->integrity_tfm, req->master_bio, p + 1);
-	err = __send_command(peer_device->connection, device->vnr, sock, P_DATA, sizeof(*p) + dgs, NULL, req->i.size);
+	err = __send_command(peer_device->connection, device->vnr, sock, P_DATA, sizeof(*p) + digest_size, NULL, req->i.size);
 	if (!err) {
 		/* For protocol A, we have to memcpy the payload into
 		 * socket buffers, as we may complete right away
@@ -1674,23 +1674,23 @@ int drbd_send_dblock(struct drbd_peer_device *peer_device, struct drbd_request *
 		 * out ok after sending on this side, but does not fit on the
 		 * receiving side, we sure have detected corruption elsewhere.
 		 */
-		if (!(req->rq_state & (RQ_EXP_RECEIVE_ACK | RQ_EXP_WRITE_ACK)) || dgs)
+		if (!(req->rq_state & (RQ_EXP_RECEIVE_ACK | RQ_EXP_WRITE_ACK)) || digest_size)
 			err = _drbd_send_bio(peer_device, req->master_bio);
 		else
 			err = _drbd_send_zc_bio(peer_device, req->master_bio);
 
 		/* double check digest, sometimes buffers have been modified in flight. */
-		if (dgs > 0 && dgs <= 64) {
+		if (digest_size > 0 && digest_size <= 64) {
 			/* 64 byte, 512 bit, is the largest digest size
 			 * currently supported in kernel crypto. */
 			unsigned char digest[64];
 			drbd_csum_bio(peer_device->connection->integrity_tfm, req->master_bio, digest);
-			if (memcmp(p + 1, digest, dgs)) {
+			if (memcmp(p + 1, digest, digest_size)) {
 				drbd_warn(device,
 					"Digest mismatch, buffer modified by upper layers during write: %llus +%u\n",
 					(unsigned long long)req->i.sector, req->i.size);
 			}
-		} /* else if (dgs > 64) {
+		} /* else if (digest_size > 64) {
 		     ... Be noisy about digest too large ...
 		} */
 	}
@@ -1711,13 +1711,13 @@ int drbd_send_block(struct drbd_peer_device *peer_device, enum drbd_packet cmd,
 	struct drbd_socket *sock;
 	struct p_data *p;
 	int err;
-	int dgs;
+	int digest_size;
 
 	sock = &peer_device->connection->data;
 	p = drbd_prepare_command(peer_device, sock);
 
-	dgs = peer_device->connection->integrity_tfm ?
-	      crypto_hash_digestsize(peer_device->connection->integrity_tfm) : 0;
+	digest_size = peer_device->connection->integrity_tfm ?
+		      crypto_hash_digestsize(peer_device->connection->integrity_tfm) : 0;
 
 	if (!p)
 		return -EIO;
@@ -1725,9 +1725,9 @@ int drbd_send_block(struct drbd_peer_device *peer_device, enum drbd_packet cmd,
 	p->block_id = peer_req->block_id;
 	p->seq_num = 0;  /* unused */
 	p->dp_flags = 0;
-	if (dgs)
+	if (digest_size)
 		drbd_csum_ee(peer_device->connection->integrity_tfm, peer_req, p + 1);
-	err = __send_command(peer_device->connection, device->vnr, sock, cmd, sizeof(*p) + dgs, NULL, peer_req->i.size);
+	err = __send_command(peer_device->connection, device->vnr, sock, cmd, sizeof(*p) + digest_size, NULL, peer_req->i.size);
 	if (!err)
 		err = _drbd_send_zc_ee(peer_device, peer_req);
 	mutex_unlock(&sock->mutex);  /* locked by drbd_prepare_command() */
diff --git a/drivers/block/drbd/drbd_proc.c b/drivers/block/drbd/drbd_proc.c
index 06e6147c7601..3b10fa6cb039 100644
--- a/drivers/block/drbd/drbd_proc.c
+++ b/drivers/block/drbd/drbd_proc.c
@@ -142,10 +142,12 @@ static void drbd_syncer_progress(struct drbd_device *device, struct seq_file *se
 			    (unsigned long) Bit2KB(rs_left >> 10),
 			    (unsigned long) Bit2KB(rs_total >> 10));
 	else
-		seq_printf(seq, "(%lu/%lu)K\n\t",
+		seq_printf(seq, "(%lu/%lu)K",
 			    (unsigned long) Bit2KB(rs_left),
 			    (unsigned long) Bit2KB(rs_total));
 
+	seq_printf(seq, "\n\t");
+
 	/* see drivers/md/md.c
 	 * We do not want to overflow, so the order of operands and
 	 * the * 100 / 100 trick are important. We do a +1 to be
diff --git a/drivers/block/drbd/drbd_receiver.c b/drivers/block/drbd/drbd_receiver.c
index 9342b8da73ab..6960fb064731 100644
--- a/drivers/block/drbd/drbd_receiver.c
+++ b/drivers/block/drbd/drbd_receiver.c
@@ -1371,9 +1371,9 @@ int drbd_submit_peer_request(struct drbd_device *device,
 	struct bio *bio;
 	struct page *page = peer_req->pages;
 	sector_t sector = peer_req->i.sector;
-	unsigned ds = peer_req->i.size;
+	unsigned data_size = peer_req->i.size;
 	unsigned n_bios = 0;
-	unsigned nr_pages = (ds + PAGE_SIZE -1) >> PAGE_SHIFT;
+	unsigned nr_pages = (data_size + PAGE_SIZE -1) >> PAGE_SHIFT;
 	int err = -ENOMEM;
 
 	if (peer_req->flags & EE_IS_TRIM_USE_ZEROOUT) {
@@ -1388,7 +1388,7 @@ int drbd_submit_peer_request(struct drbd_device *device,
 		list_add_tail(&peer_req->w.list, &device->active_ee);
 		spin_unlock_irq(&device->resource->req_lock);
 		if (blkdev_issue_zeroout(device->ldev->backing_bdev,
-			sector, ds >> 9, GFP_NOIO))
+			sector, data_size >> 9, GFP_NOIO))
 			peer_req->flags |= EE_WAS_ERROR;
 		drbd_endio_write_sec_final(peer_req);
 		return 0;
@@ -1426,12 +1426,12 @@ next_bio:
 	++n_bios;
 
 	if (rw & REQ_DISCARD) {
-		bio->bi_iter.bi_size = ds;
+		bio->bi_iter.bi_size = data_size;
 		goto submit;
 	}
 
 	page_chain_for_each(page) {
-		unsigned len = min_t(unsigned, ds, PAGE_SIZE);
+		unsigned len = min_t(unsigned, data_size, PAGE_SIZE);
 		if (!bio_add_page(bio, page, len, 0)) {
 			/* A single page must always be possible!
 			 * But in case it fails anyways,
@@ -1446,11 +1446,11 @@ next_bio:
 			}
 			goto next_bio;
 		}
-		ds -= len;
+		data_size -= len;
 		sector += len >> 9;
 		--nr_pages;
 	}
-	D_ASSERT(device, ds == 0);
+	D_ASSERT(device, data_size == 0);
 submit:
 	D_ASSERT(device, page == NULL);
 
@@ -1591,24 +1591,24 @@ read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
 	const sector_t capacity = drbd_get_capacity(device->this_bdev);
 	struct drbd_peer_request *peer_req;
 	struct page *page;
-	int dgs, ds, err;
-	unsigned int data_size = pi->size;
+	int digest_size, err;
+	unsigned int data_size = pi->size, ds;
 	void *dig_in = peer_device->connection->int_dig_in;
 	void *dig_vv = peer_device->connection->int_dig_vv;
 	unsigned long *data;
 	struct p_trim *trim = (pi->cmd == P_TRIM) ? pi->data : NULL;
 
-	dgs = 0;
+	digest_size = 0;
 	if (!trim && peer_device->connection->peer_integrity_tfm) {
-		dgs = crypto_hash_digestsize(peer_device->connection->peer_integrity_tfm);
+		digest_size = crypto_hash_digestsize(peer_device->connection->peer_integrity_tfm);
 		/*
 		 * FIXME: Receive the incoming digest into the receive buffer
 		 *	  here, together with its struct p_data?
 		 */
-		err = drbd_recv_all_warn(peer_device->connection, dig_in, dgs);
+		err = drbd_recv_all_warn(peer_device->connection, dig_in, digest_size);
 		if (err)
 			return NULL;
-		data_size -= dgs;
+		data_size -= digest_size;
 	}
 
 	if (trim) {
@@ -1661,16 +1661,16 @@ read_in_block(struct drbd_peer_device *peer_device, u64 id, sector_t sector,
 		ds -= len;
 	}
 
-	if (dgs) {
+	if (digest_size) {
 		drbd_csum_ee(peer_device->connection->peer_integrity_tfm, peer_req, dig_vv);
-		if (memcmp(dig_in, dig_vv, dgs)) {
+		if (memcmp(dig_in, dig_vv, digest_size)) {
 			drbd_err(device, "Digest integrity check FAILED: %llus +%u\n",
 				(unsigned long long)sector, data_size);
 			drbd_free_peer_req(device, peer_req);
 			return NULL;
 		}
 	}
-	device->recv_cnt += data_size>>9;
+	device->recv_cnt += data_size >> 9;
 	return peer_req;
 }
 
@@ -1708,17 +1708,17 @@ static int recv_dless_read(struct drbd_peer_device *peer_device, struct drbd_req
 	struct bio_vec bvec;
 	struct bvec_iter iter;
 	struct bio *bio;
-	int dgs, err, expect;
+	int digest_size, err, expect;
 	void *dig_in = peer_device->connection->int_dig_in;
 	void *dig_vv = peer_device->connection->int_dig_vv;
 
-	dgs = 0;
+	digest_size = 0;
 	if (peer_device->connection->peer_integrity_tfm) {
-		dgs = crypto_hash_digestsize(peer_device->connection->peer_integrity_tfm);
-		err = drbd_recv_all_warn(peer_device->connection, dig_in, dgs);
+		digest_size = crypto_hash_digestsize(peer_device->connection->peer_integrity_tfm);
+		err = drbd_recv_all_warn(peer_device->connection, dig_in, digest_size);
 		if (err)
 			return err;
-		data_size -= dgs;
+		data_size -= digest_size;
 	}
 
 	/* optimistically update recv_cnt.  if receiving fails below,
@@ -1738,9 +1738,9 @@ static int recv_dless_read(struct drbd_peer_device *peer_device, struct drbd_req
 		data_size -= expect;
 	}
 
-	if (dgs) {
+	if (digest_size) {
 		drbd_csum_bio(peer_device->connection->peer_integrity_tfm, bio, dig_vv);
-		if (memcmp(dig_in, dig_vv, dgs)) {
+		if (memcmp(dig_in, dig_vv, digest_size)) {
 			drbd_err(peer_device, "Digest integrity check FAILED. Broken NICs?\n");
 			return -EINVAL;
 		}
@@ -5561,6 +5561,7 @@ int drbd_asender(struct drbd_thread *thi)
 		 * rv <  expected: "woken" by signal during receive
 		 * rv == 0	 : "connection shut down by peer"
 		 */
+received_more:
 		if (likely(rv > 0)) {
 			received += rv;
 			buf	 += rv;
@@ -5636,6 +5637,11 @@ int drbd_asender(struct drbd_thread *thi)
 			expect	 = header_size;
 			cmd	 = NULL;
 		}
+		if (test_bit(SEND_PING, &connection->flags))
+			continue;
+		rv = drbd_recv_short(connection->meta.socket, buf, expect-received, MSG_DONTWAIT);
+		if (rv > 0)
+			goto received_more;
 	}
 
 	if (0) {
diff --git a/drivers/block/drbd/drbd_req.c b/drivers/block/drbd/drbd_req.c
index c67717d572d1..5a01c53dddeb 100644
--- a/drivers/block/drbd/drbd_req.c
+++ b/drivers/block/drbd/drbd_req.c
@@ -1645,6 +1645,6 @@ void request_timer_fn(unsigned long data)
 		? oldest_submit_jif + dt : now + et;
 	nt = time_before(ent, dt) ? ent : dt;
 out:
-	spin_unlock_irq(&connection->resource->req_lock);
+	spin_unlock_irq(&device->resource->req_lock);
 	mod_timer(&device->request_timer, nt);
 }
diff --git a/drivers/block/drbd/drbd_state.c b/drivers/block/drbd/drbd_state.c
index c35c0f001bb7..84b11f887d73 100644
--- a/drivers/block/drbd/drbd_state.c
+++ b/drivers/block/drbd/drbd_state.c
@@ -136,50 +136,50 @@ enum drbd_role conn_highest_peer(struct drbd_connection *connection)
 
 enum drbd_disk_state conn_highest_disk(struct drbd_connection *connection)
 {
-	enum drbd_disk_state ds = D_DISKLESS;
+	enum drbd_disk_state disk_state = D_DISKLESS;
 	struct drbd_peer_device *peer_device;
 	int vnr;
 
 	rcu_read_lock();
 	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 		struct drbd_device *device = peer_device->device;
-		ds = max_t(enum drbd_disk_state, ds, device->state.disk);
+		disk_state = max_t(enum drbd_disk_state, disk_state, device->state.disk);
 	}
 	rcu_read_unlock();
 
-	return ds;
+	return disk_state;
 }
 
 enum drbd_disk_state conn_lowest_disk(struct drbd_connection *connection)
 {
-	enum drbd_disk_state ds = D_MASK;
+	enum drbd_disk_state disk_state = D_MASK;
 	struct drbd_peer_device *peer_device;
 	int vnr;
 
 	rcu_read_lock();
 	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 		struct drbd_device *device = peer_device->device;
-		ds = min_t(enum drbd_disk_state, ds, device->state.disk);
+		disk_state = min_t(enum drbd_disk_state, disk_state, device->state.disk);
 	}
 	rcu_read_unlock();
 
-	return ds;
+	return disk_state;
 }
 
 enum drbd_disk_state conn_highest_pdsk(struct drbd_connection *connection)
 {
-	enum drbd_disk_state ds = D_DISKLESS;
+	enum drbd_disk_state disk_state = D_DISKLESS;
 	struct drbd_peer_device *peer_device;
 	int vnr;
 
 	rcu_read_lock();
 	idr_for_each_entry(&connection->peer_devices, peer_device, vnr) {
 		struct drbd_device *device = peer_device->device;
-		ds = max_t(enum drbd_disk_state, ds, device->state.pdsk);
+		disk_state = max_t(enum drbd_disk_state, disk_state, device->state.pdsk);
 	}
 	rcu_read_unlock();
 
-	return ds;
+	return disk_state;
 }
 
 enum drbd_conns conn_lowest_conn(struct drbd_connection *connection)
diff --git a/drivers/block/drbd/drbd_worker.c b/drivers/block/drbd/drbd_worker.c
index 50776b362828..d2d1f97511bd 100644
--- a/drivers/block/drbd/drbd_worker.c
+++ b/drivers/block/drbd/drbd_worker.c
@@ -43,10 +43,10 @@ static int make_ov_request(struct drbd_device *, int);
 static int make_resync_request(struct drbd_device *, int);
 
 /* endio handlers:
- *   drbd_md_io_complete (defined here)
+ *   drbd_md_endio (defined here)
  *   drbd_request_endio (defined here)
  *   drbd_peer_request_endio (defined here)
- *   bm_async_io_complete (defined in drbd_bitmap.c)
+ *   drbd_bm_endio (defined in drbd_bitmap.c)
  *
  * For all these callbacks, note the following:
  * The callbacks will be called in irq context by the IDE drivers,
@@ -65,7 +65,7 @@ rwlock_t global_state_lock;
 /* used for synchronous meta data and bitmap IO
  * submitted by drbd_md_sync_page_io()
  */
-void drbd_md_io_complete(struct bio *bio, int error)
+void drbd_md_endio(struct bio *bio, int error)
 {
 	struct drbd_device *device;
 
@@ -1853,9 +1853,12 @@ static void drbd_ldev_destroy(struct drbd_device *device)
 	device->resync = NULL;
 	lc_destroy(device->act_log);
 	device->act_log = NULL;
-	__no_warn(local,
-		drbd_free_ldev(device->ldev);
-		device->ldev = NULL;);
+
+	__acquire(local);
+	drbd_free_ldev(device->ldev);
+	device->ldev = NULL;
+	__release(local);
+
 	clear_bit(GOING_DISKLESS, &device->flags);
 	wake_up(&device->misc_wait);
 }
@@ -1928,19 +1931,18 @@ void __update_timing_details(
 	++(*cb_nr);
 }
 
-#define WORK_PENDING(work_bit, todo)	(todo & (1UL << work_bit))
 static void do_device_work(struct drbd_device *device, const unsigned long todo)
 {
-	if (WORK_PENDING(MD_SYNC, todo))
+	if (test_bit(MD_SYNC, &todo))
 		do_md_sync(device);
-	if (WORK_PENDING(RS_DONE, todo) ||
-	    WORK_PENDING(RS_PROGRESS, todo))
-		update_on_disk_bitmap(device, WORK_PENDING(RS_DONE, todo));
-	if (WORK_PENDING(GO_DISKLESS, todo))
+	if (test_bit(RS_DONE, &todo) ||
+	    test_bit(RS_PROGRESS, &todo))
+		update_on_disk_bitmap(device, test_bit(RS_DONE, &todo));
+	if (test_bit(GO_DISKLESS, &todo))
 		go_diskless(device);
-	if (WORK_PENDING(DESTROY_DISK, todo))
+	if (test_bit(DESTROY_DISK, &todo))
 		drbd_ldev_destroy(device);
-	if (WORK_PENDING(RS_START, todo))
+	if (test_bit(RS_START, &todo))
 		do_start_resync(device);
 }
 
@@ -1992,22 +1994,13 @@ static bool dequeue_work_batch(struct drbd_work_queue *queue, struct list_head *
 	return !list_empty(work_list);
 }
 
-static bool dequeue_work_item(struct drbd_work_queue *queue, struct list_head *work_list)
-{
-	spin_lock_irq(&queue->q_lock);
-	if (!list_empty(&queue->q))
-		list_move(queue->q.next, work_list);
-	spin_unlock_irq(&queue->q_lock);
-	return !list_empty(work_list);
-}
-
 static void wait_for_work(struct drbd_connection *connection, struct list_head *work_list)
 {
 	DEFINE_WAIT(wait);
 	struct net_conf *nc;
 	int uncork, cork;
 
-	dequeue_work_item(&connection->sender_work, work_list);
+	dequeue_work_batch(&connection->sender_work, work_list);
 	if (!list_empty(work_list))
 		return;
 
@@ -2033,8 +2026,6 @@ static void wait_for_work(struct drbd_connection *connection, struct list_head *
 		prepare_to_wait(&connection->sender_work.q_wait, &wait, TASK_INTERRUPTIBLE);
 		spin_lock_irq(&connection->resource->req_lock);
 		spin_lock(&connection->sender_work.q_lock);	/* FIXME get rid of this one? */
-		/* dequeue single item only,
-		 * we still use drbd_queue_work_front() in some places */
 		if (!list_empty(&connection->sender_work.q))
 			list_splice_tail_init(&connection->sender_work.q, work_list);
 		spin_unlock(&connection->sender_work.q_lock);	/* FIXME get rid of this one? */
@@ -2121,7 +2112,7 @@ int drbd_worker(struct drbd_thread *thi)
 		if (get_t_state(thi) != RUNNING)
 			break;
 
-		while (!list_empty(&work_list)) {
+		if (!list_empty(&work_list)) {
 			w = list_first_entry(&work_list, struct drbd_work, list);
 			list_del_init(&w->list);
 			update_worker_timing_details(connection, w->cb);
@@ -2137,13 +2128,13 @@ int drbd_worker(struct drbd_thread *thi)
 			update_worker_timing_details(connection, do_unqueued_work);
 			do_unqueued_work(connection);
 		}
-		while (!list_empty(&work_list)) {
+		if (!list_empty(&work_list)) {
 			w = list_first_entry(&work_list, struct drbd_work, list);
 			list_del_init(&w->list);
 			update_worker_timing_details(connection, w->cb);
 			w->cb(w, 1);
-		}
-		dequeue_work_batch(&connection->sender_work, &work_list);
+		} else
+			dequeue_work_batch(&connection->sender_work, &work_list);
 	} while (!list_empty(&work_list) || test_bit(DEVICE_WORK_PENDING, &connection->flags));
 
 	rcu_read_lock();
diff --git a/drivers/block/hd.c b/drivers/block/hd.c
index 8a290c08262f..3abb121825bc 100644
--- a/drivers/block/hd.c
+++ b/drivers/block/hd.c
@@ -694,16 +694,6 @@ static const struct block_device_operations hd_fops = {
 	.getgeo =	hd_getgeo,
 };
 
-/*
- * This is the hard disk IRQ description. The IRQF_DISABLED in sa_flags
- * means we run the IRQ-handler with interrupts disabled:  this is bad for
- * interrupt latency, but anything else has led to problems on some
- * machines.
- *
- * We enable interrupts in some of the routines after making sure it's
- * safe.
- */
-
 static int __init hd_init(void)
 {
 	int drive;
@@ -761,7 +751,7 @@ static int __init hd_init(void)
 			p->cyl, p->head, p->sect);
 	}
 
-	if (request_irq(HD_IRQ, hd_interrupt, IRQF_DISABLED, "hd", NULL)) {
+	if (request_irq(HD_IRQ, hd_interrupt, 0, "hd", NULL)) {
 		printk("hd: unable to get IRQ%d for the hard disk driver\n",
 			HD_IRQ);
 		goto out1;
diff --git a/drivers/block/mtip32xx/mtip32xx.c b/drivers/block/mtip32xx/mtip32xx.c
index db1e9560d8a7..1bd5f523f8fd 100644
--- a/drivers/block/mtip32xx/mtip32xx.c
+++ b/drivers/block/mtip32xx/mtip32xx.c
@@ -247,7 +247,7 @@ static void mtip_async_complete(struct mtip_port *port,
 	if (unlikely(cmd->unaligned))
 		up(&port->cmd_slot_unal);
 
-	blk_mq_end_io(rq, status ? -EIO : 0);
+	blk_mq_end_request(rq, status ? -EIO : 0);
 }
 
 /*
@@ -3739,7 +3739,7 @@ static int mtip_submit_request(struct blk_mq_hw_ctx *hctx, struct request *rq)
 		int err;
 
 		err = mtip_send_trim(dd, blk_rq_pos(rq), blk_rq_sectors(rq));
-		blk_mq_end_io(rq, err);
+		blk_mq_end_request(rq, err);
 		return 0;
 	}
 
@@ -3775,13 +3775,16 @@ static bool mtip_check_unal_depth(struct blk_mq_hw_ctx *hctx,
 	return false;
 }
 
-static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
+static int mtip_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq,
+		bool last)
 {
 	int ret;
 
 	if (unlikely(mtip_check_unal_depth(hctx, rq)))
 		return BLK_MQ_RQ_QUEUE_BUSY;
 
+	blk_mq_start_request(rq);
+
 	ret = mtip_submit_request(hctx, rq);
 	if (likely(!ret))
 		return BLK_MQ_RQ_QUEUE_OK;
@@ -3918,7 +3921,6 @@ skip_create_disk:
 	if (rv) {
 		dev_err(&dd->pdev->dev,
 			"Unable to allocate request queue\n");
-		rv = -ENOMEM;
 		goto block_queue_alloc_init_error;
 	}
 
@@ -3952,6 +3954,7 @@ skip_create_disk:
 
 	/* Set device limits. */
 	set_bit(QUEUE_FLAG_NONROT, &dd->queue->queue_flags);
+	clear_bit(QUEUE_FLAG_ADD_RANDOM, &dd->queue->queue_flags);
 	blk_queue_max_segments(dd->queue, MTIP_MAX_SG);
 	blk_queue_physical_block_size(dd->queue, 4096);
 	blk_queue_max_hw_sectors(dd->queue, 0xffff);
diff --git a/drivers/block/nbd.c b/drivers/block/nbd.c
index fb31b8ee4372..4bc2a5cb9935 100644
--- a/drivers/block/nbd.c
+++ b/drivers/block/nbd.c
@@ -847,6 +847,7 @@ static int __init nbd_init(void)
 		 * Tell the block layer that we are not a rotational device
 		 */
 		queue_flag_set_unlocked(QUEUE_FLAG_NONROT, disk->queue);
+		queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, disk->queue);
 		disk->queue->limits.discard_granularity = 512;
 		disk->queue->limits.max_discard_sectors = UINT_MAX;
 		disk->queue->limits.discard_zeroes_data = 0;
diff --git a/drivers/block/null_blk.c b/drivers/block/null_blk.c
index a3b042c4d448..2671a3f02f0c 100644
--- a/drivers/block/null_blk.c
+++ b/drivers/block/null_blk.c
@@ -177,7 +177,7 @@ static void end_cmd(struct nullb_cmd *cmd)
 {
 	switch (queue_mode)  {
 	case NULL_Q_MQ:
-		blk_mq_end_io(cmd->rq, 0);
+		blk_mq_end_request(cmd->rq, 0);
 		return;
 	case NULL_Q_RQ:
 		INIT_LIST_HEAD(&cmd->rq->queuelist);
@@ -313,13 +313,16 @@ static void null_request_fn(struct request_queue *q)
 	}
 }
 
-static int null_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq)
+static int null_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *rq,
+		bool last)
 {
 	struct nullb_cmd *cmd = blk_mq_rq_to_pdu(rq);
 
 	cmd->rq = rq;
 	cmd->nq = hctx->driver_data;
 
+	blk_mq_start_request(rq);
+
 	null_handle_cmd(cmd);
 	return BLK_MQ_RQ_QUEUE_OK;
 }
@@ -462,17 +465,21 @@ static int null_add_dev(void)
 	struct gendisk *disk;
 	struct nullb *nullb;
 	sector_t size;
+	int rv;
 
 	nullb = kzalloc_node(sizeof(*nullb), GFP_KERNEL, home_node);
-	if (!nullb)
+	if (!nullb) {
+		rv = -ENOMEM;
 		goto out;
+	}
 
 	spin_lock_init(&nullb->lock);
 
 	if (queue_mode == NULL_Q_MQ && use_per_node_hctx)
 		submit_queues = nr_online_nodes;
 
-	if (setup_queues(nullb))
+	rv = setup_queues(nullb);
+	if (rv)
 		goto out_free_nullb;
 
 	if (queue_mode == NULL_Q_MQ) {
@@ -484,22 +491,29 @@ static int null_add_dev(void)
 		nullb->tag_set.flags = BLK_MQ_F_SHOULD_MERGE;
 		nullb->tag_set.driver_data = nullb;
 
-		if (blk_mq_alloc_tag_set(&nullb->tag_set))
+		rv = blk_mq_alloc_tag_set(&nullb->tag_set);
+		if (rv)
 			goto out_cleanup_queues;
 
 		nullb->q = blk_mq_init_queue(&nullb->tag_set);
-		if (!nullb->q)
+		if (!nullb->q) {
+			rv = -ENOMEM;
 			goto out_cleanup_tags;
+		}
 	} else if (queue_mode == NULL_Q_BIO) {
 		nullb->q = blk_alloc_queue_node(GFP_KERNEL, home_node);
-		if (!nullb->q)
+		if (!nullb->q) {
+			rv = -ENOMEM;
 			goto out_cleanup_queues;
+		}
 		blk_queue_make_request(nullb->q, null_queue_bio);
 		init_driver_queues(nullb);
 	} else {
 		nullb->q = blk_init_queue_node(null_request_fn, &nullb->lock, home_node);
-		if (!nullb->q)
+		if (!nullb->q) {
+			rv = -ENOMEM;
 			goto out_cleanup_queues;
+		}
 		blk_queue_prep_rq(nullb->q, null_rq_prep_fn);
 		blk_queue_softirq_done(nullb->q, null_softirq_done_fn);
 		init_driver_queues(nullb);
@@ -507,10 +521,13 @@ static int null_add_dev(void)
 
 	nullb->q->queuedata = nullb;
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, nullb->q);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, nullb->q);
 
 	disk = nullb->disk = alloc_disk_node(1, home_node);
-	if (!disk)
+	if (!disk) {
+		rv = -ENOMEM;
 		goto out_cleanup_blk_queue;
+	}
 
 	mutex_lock(&lock);
 	list_add_tail(&nullb->list, &nullb_list);
@@ -544,7 +561,7 @@ out_cleanup_queues:
 out_free_nullb:
 	kfree(nullb);
 out:
-	return -ENOMEM;
+	return rv;
 }
 
 static int __init null_init(void)
diff --git a/drivers/block/nvme-core.c b/drivers/block/nvme-core.c
index 02351e217165..e2bb8afbeae5 100644
--- a/drivers/block/nvme-core.c
+++ b/drivers/block/nvme-core.c
@@ -1916,6 +1916,7 @@ static struct nvme_ns *nvme_alloc_ns(struct nvme_dev *dev, unsigned nsid,
 	ns->queue->queue_flags = QUEUE_FLAG_DEFAULT;
 	queue_flag_set_unlocked(QUEUE_FLAG_NOMERGES, ns->queue);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, ns->queue);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, ns->queue);
 	blk_queue_make_request(ns->queue, nvme_make_request);
 	ns->dev = dev;
 	ns->queue->queuedata = ns;
diff --git a/drivers/block/paride/pcd.c b/drivers/block/paride/pcd.c
index 719cb1bc1640..3b7c9f1be484 100644
--- a/drivers/block/paride/pcd.c
+++ b/drivers/block/paride/pcd.c
@@ -69,8 +69,8 @@
             nice        This parameter controls the driver's use of
                         idle CPU time, at the expense of some speed.
  
-	If this driver is built into the kernel, you can use kernel
-        the following command line parameters, with the same values
+	If this driver is built into the kernel, you can use the
+        following kernel command line parameters, with the same values
         as the corresponding module parameters listed above:
 
 	    pcd.drive0
diff --git a/drivers/block/paride/pd.c b/drivers/block/paride/pd.c
index fea7e76a00de..d48715b287e6 100644
--- a/drivers/block/paride/pd.c
+++ b/drivers/block/paride/pd.c
@@ -722,6 +722,8 @@ static int pd_special_command(struct pd_unit *disk,
 	int err = 0;
 
 	rq = blk_get_request(disk->gd->queue, READ, __GFP_WAIT);
+	if (IS_ERR(rq))
+		return PTR_ERR(rq);
 
 	rq->cmd_type = REQ_TYPE_SPECIAL;
 	rq->special = func;
diff --git a/drivers/block/pktcdvd.c b/drivers/block/pktcdvd.c
index 758ac442c5b5..09e628dafd9d 100644
--- a/drivers/block/pktcdvd.c
+++ b/drivers/block/pktcdvd.c
@@ -704,6 +704,8 @@ static int pkt_generic_packet(struct pktcdvd_device *pd, struct packet_command *
 
 	rq = blk_get_request(q, (cgc->data_direction == CGC_DATA_WRITE) ?
 			     WRITE : READ, __GFP_WAIT);
+	if (IS_ERR(rq))
+		return PTR_ERR(rq);
 	blk_rq_set_block_pc(rq);
 
 	if (cgc->buflen) {
diff --git a/drivers/block/rbd.c b/drivers/block/rbd.c
index 623c84145b79..0a54c588e433 100644
--- a/drivers/block/rbd.c
+++ b/drivers/block/rbd.c
@@ -210,6 +210,12 @@ enum obj_request_type {
 	OBJ_REQUEST_NODATA, OBJ_REQUEST_BIO, OBJ_REQUEST_PAGES
 };
 
+enum obj_operation_type {
+	OBJ_OP_WRITE,
+	OBJ_OP_READ,
+	OBJ_OP_DISCARD,
+};
+
 enum obj_req_flags {
 	OBJ_REQ_DONE,		/* completion flag: not done = 0, done = 1 */
 	OBJ_REQ_IMG_DATA,	/* object usage: standalone = 0, image = 1 */
@@ -276,6 +282,7 @@ enum img_req_flags {
 	IMG_REQ_WRITE,		/* I/O direction: read = 0, write = 1 */
 	IMG_REQ_CHILD,		/* initiator: block = 0, child image = 1 */
 	IMG_REQ_LAYERED,	/* ENOENT handling: normal = 0, layered = 1 */
+	IMG_REQ_DISCARD,	/* discard: normal = 0, discard request = 1 */
 };
 
 struct rbd_img_request {
@@ -785,6 +792,20 @@ static int parse_rbd_opts_token(char *c, void *private)
 	return 0;
 }
 
+static char* obj_op_name(enum obj_operation_type op_type)
+{
+	switch (op_type) {
+	case OBJ_OP_READ:
+		return "read";
+	case OBJ_OP_WRITE:
+		return "write";
+	case OBJ_OP_DISCARD:
+		return "discard";
+	default:
+		return "???";
+	}
+}
+
 /*
  * Get a ceph client with specific addr and configuration, if one does
  * not exist create it.  Either way, ceph_opts is consumed by this
@@ -1600,6 +1621,21 @@ static bool img_request_write_test(struct rbd_img_request *img_request)
 	return test_bit(IMG_REQ_WRITE, &img_request->flags) != 0;
 }
 
+/*
+ * Set the discard flag when the img_request is an discard request
+ */
+static void img_request_discard_set(struct rbd_img_request *img_request)
+{
+	set_bit(IMG_REQ_DISCARD, &img_request->flags);
+	smp_mb();
+}
+
+static bool img_request_discard_test(struct rbd_img_request *img_request)
+{
+	smp_mb();
+	return test_bit(IMG_REQ_DISCARD, &img_request->flags) != 0;
+}
+
 static void img_request_child_set(struct rbd_img_request *img_request)
 {
 	set_bit(IMG_REQ_CHILD, &img_request->flags);
@@ -1636,6 +1672,17 @@ static bool img_request_layered_test(struct rbd_img_request *img_request)
 	return test_bit(IMG_REQ_LAYERED, &img_request->flags) != 0;
 }
 
+static enum obj_operation_type
+rbd_img_request_op_type(struct rbd_img_request *img_request)
+{
+	if (img_request_write_test(img_request))
+		return OBJ_OP_WRITE;
+	else if (img_request_discard_test(img_request))
+		return OBJ_OP_DISCARD;
+	else
+		return OBJ_OP_READ;
+}
+
 static void
 rbd_img_obj_request_read_callback(struct rbd_obj_request *obj_request)
 {
@@ -1722,6 +1769,21 @@ static void rbd_osd_write_callback(struct rbd_obj_request *obj_request)
 	obj_request_done_set(obj_request);
 }
 
+static void rbd_osd_discard_callback(struct rbd_obj_request *obj_request)
+{
+	dout("%s: obj %p result %d %llu\n", __func__, obj_request,
+		obj_request->result, obj_request->length);
+	/*
+	 * There is no such thing as a successful short discard.  Set
+	 * it to our originally-requested length.
+	 */
+	obj_request->xferred = obj_request->length;
+	/* discarding a non-existent object is not a problem */
+	if (obj_request->result == -ENOENT)
+		obj_request->result = 0;
+	obj_request_done_set(obj_request);
+}
+
 /*
  * For a simple stat call there's nothing to do.  We'll do more if
  * this is part of a write sequence for a layered image.
@@ -1773,6 +1835,11 @@ static void rbd_osd_req_callback(struct ceph_osd_request *osd_req,
 	case CEPH_OSD_OP_STAT:
 		rbd_osd_stat_callback(obj_request);
 		break;
+	case CEPH_OSD_OP_DELETE:
+	case CEPH_OSD_OP_TRUNCATE:
+	case CEPH_OSD_OP_ZERO:
+		rbd_osd_discard_callback(obj_request);
+		break;
 	case CEPH_OSD_OP_CALL:
 	case CEPH_OSD_OP_NOTIFY_ACK:
 	case CEPH_OSD_OP_WATCH:
@@ -1823,7 +1890,7 @@ static void rbd_osd_req_format_write(struct rbd_obj_request *obj_request)
  */
 static struct ceph_osd_request *rbd_osd_req_create(
 					struct rbd_device *rbd_dev,
-					bool write_request,
+					enum obj_operation_type op_type,
 					unsigned int num_ops,
 					struct rbd_obj_request *obj_request)
 {
@@ -1831,16 +1898,18 @@ static struct ceph_osd_request *rbd_osd_req_create(
 	struct ceph_osd_client *osdc;
 	struct ceph_osd_request *osd_req;
 
-	if (obj_request_img_data_test(obj_request)) {
+	if (obj_request_img_data_test(obj_request) &&
+		(op_type == OBJ_OP_DISCARD || op_type == OBJ_OP_WRITE)) {
 		struct rbd_img_request *img_request = obj_request->img_request;
-
-		rbd_assert(write_request ==
-				img_request_write_test(img_request));
-		if (write_request)
-			snapc = img_request->snapc;
+		if (op_type == OBJ_OP_WRITE) {
+			rbd_assert(img_request_write_test(img_request));
+		} else {
+			rbd_assert(img_request_discard_test(img_request));
+		}
+		snapc = img_request->snapc;
 	}
 
-	rbd_assert(num_ops == 1 || (write_request && num_ops == 2));
+	rbd_assert(num_ops == 1 || ((op_type == OBJ_OP_WRITE) && num_ops == 2));
 
 	/* Allocate and initialize the request, for the num_ops ops */
 
@@ -1850,7 +1919,7 @@ static struct ceph_osd_request *rbd_osd_req_create(
 	if (!osd_req)
 		return NULL;	/* ENOMEM */
 
-	if (write_request)
+	if (op_type == OBJ_OP_WRITE || op_type == OBJ_OP_DISCARD)
 		osd_req->r_flags = CEPH_OSD_FLAG_WRITE | CEPH_OSD_FLAG_ONDISK;
 	else
 		osd_req->r_flags = CEPH_OSD_FLAG_READ;
@@ -1865,9 +1934,10 @@ static struct ceph_osd_request *rbd_osd_req_create(
 }
 
 /*
- * Create a copyup osd request based on the information in the
- * object request supplied.  A copyup request has three osd ops,
- * a copyup method call, a hint op, and a write op.
+ * Create a copyup osd request based on the information in the object
+ * request supplied.  A copyup request has two or three osd ops, a
+ * copyup method call, potentially a hint op, and a write or truncate
+ * or zero op.
  */
 static struct ceph_osd_request *
 rbd_osd_req_create_copyup(struct rbd_obj_request *obj_request)
@@ -1877,18 +1947,24 @@ rbd_osd_req_create_copyup(struct rbd_obj_request *obj_request)
 	struct rbd_device *rbd_dev;
 	struct ceph_osd_client *osdc;
 	struct ceph_osd_request *osd_req;
+	int num_osd_ops = 3;
 
 	rbd_assert(obj_request_img_data_test(obj_request));
 	img_request = obj_request->img_request;
 	rbd_assert(img_request);
-	rbd_assert(img_request_write_test(img_request));
+	rbd_assert(img_request_write_test(img_request) ||
+			img_request_discard_test(img_request));
+
+	if (img_request_discard_test(img_request))
+		num_osd_ops = 2;
 
-	/* Allocate and initialize the request, for the three ops */
+	/* Allocate and initialize the request, for all the ops */
 
 	snapc = img_request->snapc;
 	rbd_dev = img_request->rbd_dev;
 	osdc = &rbd_dev->rbd_client->client->osdc;
-	osd_req = ceph_osdc_alloc_request(osdc, snapc, 3, false, GFP_ATOMIC);
+	osd_req = ceph_osdc_alloc_request(osdc, snapc, num_osd_ops,
+						false, GFP_ATOMIC);
 	if (!osd_req)
 		return NULL;	/* ENOMEM */
 
@@ -2057,7 +2133,8 @@ static bool rbd_dev_parent_get(struct rbd_device *rbd_dev)
 static struct rbd_img_request *rbd_img_request_create(
 					struct rbd_device *rbd_dev,
 					u64 offset, u64 length,
-					bool write_request)
+					enum obj_operation_type op_type,
+					struct ceph_snap_context *snapc)
 {
 	struct rbd_img_request *img_request;
 
@@ -2065,20 +2142,17 @@ static struct rbd_img_request *rbd_img_request_create(
 	if (!img_request)
 		return NULL;
 
-	if (write_request) {
-		down_read(&rbd_dev->header_rwsem);
-		ceph_get_snap_context(rbd_dev->header.snapc);
-		up_read(&rbd_dev->header_rwsem);
-	}
-
 	img_request->rq = NULL;
 	img_request->rbd_dev = rbd_dev;
 	img_request->offset = offset;
 	img_request->length = length;
 	img_request->flags = 0;
-	if (write_request) {
+	if (op_type == OBJ_OP_DISCARD) {
+		img_request_discard_set(img_request);
+		img_request->snapc = snapc;
+	} else if (op_type == OBJ_OP_WRITE) {
 		img_request_write_set(img_request);
-		img_request->snapc = rbd_dev->header.snapc;
+		img_request->snapc = snapc;
 	} else {
 		img_request->snap_id = rbd_dev->spec->snap_id;
 	}
@@ -2093,8 +2167,7 @@ static struct rbd_img_request *rbd_img_request_create(
 	kref_init(&img_request->kref);
 
 	dout("%s: rbd_dev %p %s %llu/%llu -> img %p\n", __func__, rbd_dev,
-		write_request ? "write" : "read", offset, length,
-		img_request);
+		obj_op_name(op_type), offset, length, img_request);
 
 	return img_request;
 }
@@ -2118,7 +2191,8 @@ static void rbd_img_request_destroy(struct kref *kref)
 		rbd_dev_parent_put(img_request->rbd_dev);
 	}
 
-	if (img_request_write_test(img_request))
+	if (img_request_write_test(img_request) ||
+		img_request_discard_test(img_request))
 		ceph_put_snap_context(img_request->snapc);
 
 	kmem_cache_free(rbd_img_request_cache, img_request);
@@ -2134,8 +2208,8 @@ static struct rbd_img_request *rbd_parent_request_create(
 	rbd_assert(obj_request->img_request);
 	rbd_dev = obj_request->img_request->rbd_dev;
 
-	parent_request = rbd_img_request_create(rbd_dev->parent,
-						img_offset, length, false);
+	parent_request = rbd_img_request_create(rbd_dev->parent, img_offset,
+						length, OBJ_OP_READ, NULL);
 	if (!parent_request)
 		return NULL;
 
@@ -2176,11 +2250,18 @@ static bool rbd_img_obj_end_request(struct rbd_obj_request *obj_request)
 	result = obj_request->result;
 	if (result) {
 		struct rbd_device *rbd_dev = img_request->rbd_dev;
+		enum obj_operation_type op_type;
+
+		if (img_request_discard_test(img_request))
+			op_type = OBJ_OP_DISCARD;
+		else if (img_request_write_test(img_request))
+			op_type = OBJ_OP_WRITE;
+		else
+			op_type = OBJ_OP_READ;
 
 		rbd_warn(rbd_dev, "%s %llx at %llx (%llx)",
-			img_request_write_test(img_request) ? "write" : "read",
-			obj_request->length, obj_request->img_offset,
-			obj_request->offset);
+			obj_op_name(op_type), obj_request->length,
+			obj_request->img_offset, obj_request->offset);
 		rbd_warn(rbd_dev, "  result %d xferred %x",
 			result, xferred);
 		if (!img_request->result)
@@ -2245,6 +2326,67 @@ out:
 }
 
 /*
+ * Add individual osd ops to the given ceph_osd_request and prepare
+ * them for submission. num_ops is the current number of
+ * osd operations already to the object request.
+ */
+static void rbd_img_obj_request_fill(struct rbd_obj_request *obj_request,
+				struct ceph_osd_request *osd_request,
+				enum obj_operation_type op_type,
+				unsigned int num_ops)
+{
+	struct rbd_img_request *img_request = obj_request->img_request;
+	struct rbd_device *rbd_dev = img_request->rbd_dev;
+	u64 object_size = rbd_obj_bytes(&rbd_dev->header);
+	u64 offset = obj_request->offset;
+	u64 length = obj_request->length;
+	u64 img_end;
+	u16 opcode;
+
+	if (op_type == OBJ_OP_DISCARD) {
+		if (!offset && length == object_size &&
+		    (!img_request_layered_test(img_request) ||
+		     !obj_request_overlaps_parent(obj_request))) {
+			opcode = CEPH_OSD_OP_DELETE;
+		} else if ((offset + length == object_size)) {
+			opcode = CEPH_OSD_OP_TRUNCATE;
+		} else {
+			down_read(&rbd_dev->header_rwsem);
+			img_end = rbd_dev->header.image_size;
+			up_read(&rbd_dev->header_rwsem);
+
+			if (obj_request->img_offset + length == img_end)
+				opcode = CEPH_OSD_OP_TRUNCATE;
+			else
+				opcode = CEPH_OSD_OP_ZERO;
+		}
+	} else if (op_type == OBJ_OP_WRITE) {
+		opcode = CEPH_OSD_OP_WRITE;
+		osd_req_op_alloc_hint_init(osd_request, num_ops,
+					object_size, object_size);
+		num_ops++;
+	} else {
+		opcode = CEPH_OSD_OP_READ;
+	}
+
+	osd_req_op_extent_init(osd_request, num_ops, opcode, offset, length,
+				0, 0);
+	if (obj_request->type == OBJ_REQUEST_BIO)
+		osd_req_op_extent_osd_data_bio(osd_request, num_ops,
+					obj_request->bio_list, length);
+	else if (obj_request->type == OBJ_REQUEST_PAGES)
+		osd_req_op_extent_osd_data_pages(osd_request, num_ops,
+					obj_request->pages, length,
+					offset & ~PAGE_MASK, false, false);
+
+	/* Discards are also writes */
+	if (op_type == OBJ_OP_WRITE || op_type == OBJ_OP_DISCARD)
+		rbd_osd_req_format_write(obj_request);
+	else
+		rbd_osd_req_format_read(obj_request);
+}
+
+/*
  * Split up an image request into one or more object requests, each
  * to a different object.  The "type" parameter indicates whether
  * "data_desc" is the pointer to the head of a list of bio
@@ -2259,28 +2401,26 @@ static int rbd_img_request_fill(struct rbd_img_request *img_request,
 	struct rbd_device *rbd_dev = img_request->rbd_dev;
 	struct rbd_obj_request *obj_request = NULL;
 	struct rbd_obj_request *next_obj_request;
-	bool write_request = img_request_write_test(img_request);
 	struct bio *bio_list = NULL;
 	unsigned int bio_offset = 0;
 	struct page **pages = NULL;
+	enum obj_operation_type op_type;
 	u64 img_offset;
 	u64 resid;
-	u16 opcode;
 
 	dout("%s: img %p type %d data_desc %p\n", __func__, img_request,
 		(int)type, data_desc);
 
-	opcode = write_request ? CEPH_OSD_OP_WRITE : CEPH_OSD_OP_READ;
 	img_offset = img_request->offset;
 	resid = img_request->length;
 	rbd_assert(resid > 0);
+	op_type = rbd_img_request_op_type(img_request);
 
 	if (type == OBJ_REQUEST_BIO) {
 		bio_list = data_desc;
 		rbd_assert(img_offset ==
 			   bio_list->bi_iter.bi_sector << SECTOR_SHIFT);
-	} else {
-		rbd_assert(type == OBJ_REQUEST_PAGES);
+	} else if (type == OBJ_REQUEST_PAGES) {
 		pages = data_desc;
 	}
 
@@ -2289,7 +2429,6 @@ static int rbd_img_request_fill(struct rbd_img_request *img_request,
 		const char *object_name;
 		u64 offset;
 		u64 length;
-		unsigned int which = 0;
 
 		object_name = rbd_segment_name(rbd_dev, img_offset);
 		if (!object_name)
@@ -2321,7 +2460,7 @@ static int rbd_img_request_fill(struct rbd_img_request *img_request,
 								GFP_ATOMIC);
 			if (!obj_request->bio_list)
 				goto out_unwind;
-		} else {
+		} else if (type == OBJ_REQUEST_PAGES) {
 			unsigned int page_count;
 
 			obj_request->pages = pages;
@@ -2332,38 +2471,19 @@ static int rbd_img_request_fill(struct rbd_img_request *img_request,
 			pages += page_count;
 		}
 
-		osd_req = rbd_osd_req_create(rbd_dev, write_request,
-					     (write_request ? 2 : 1),
-					     obj_request);
+		osd_req = rbd_osd_req_create(rbd_dev, op_type,
+					(op_type == OBJ_OP_WRITE) ? 2 : 1,
+					obj_request);
 		if (!osd_req)
 			goto out_unwind;
+
 		obj_request->osd_req = osd_req;
 		obj_request->callback = rbd_img_obj_callback;
-		rbd_img_request_get(img_request);
-
-		if (write_request) {
-			osd_req_op_alloc_hint_init(osd_req, which,
-					     rbd_obj_bytes(&rbd_dev->header),
-					     rbd_obj_bytes(&rbd_dev->header));
-			which++;
-		}
-
-		osd_req_op_extent_init(osd_req, which, opcode, offset, length,
-				       0, 0);
-		if (type == OBJ_REQUEST_BIO)
-			osd_req_op_extent_osd_data_bio(osd_req, which,
-					obj_request->bio_list, length);
-		else
-			osd_req_op_extent_osd_data_pages(osd_req, which,
-					obj_request->pages, length,
-					offset & ~PAGE_MASK, false, false);
+		obj_request->img_offset = img_offset;
 
-		if (write_request)
-			rbd_osd_req_format_write(obj_request);
-		else
-			rbd_osd_req_format_read(obj_request);
+		rbd_img_obj_request_fill(obj_request, osd_req, op_type, 0);
 
-		obj_request->img_offset = img_offset;
+		rbd_img_request_get(img_request);
 
 		img_offset += length;
 		resid -= length;
@@ -2386,7 +2506,8 @@ rbd_img_obj_copyup_callback(struct rbd_obj_request *obj_request)
 	struct page **pages;
 	u32 page_count;
 
-	rbd_assert(obj_request->type == OBJ_REQUEST_BIO);
+	rbd_assert(obj_request->type == OBJ_REQUEST_BIO ||
+		obj_request->type == OBJ_REQUEST_NODATA);
 	rbd_assert(obj_request_img_data_test(obj_request));
 	img_request = obj_request->img_request;
 	rbd_assert(img_request);
@@ -2424,11 +2545,10 @@ rbd_img_obj_parent_read_full_callback(struct rbd_img_request *img_request)
 	struct ceph_osd_client *osdc;
 	struct rbd_device *rbd_dev;
 	struct page **pages;
+	enum obj_operation_type op_type;
 	u32 page_count;
 	int img_result;
 	u64 parent_length;
-	u64 offset;
-	u64 length;
 
 	rbd_assert(img_request_child_test(img_request));
 
@@ -2492,26 +2612,10 @@ rbd_img_obj_parent_read_full_callback(struct rbd_img_request *img_request)
 	osd_req_op_cls_request_data_pages(osd_req, 0, pages, parent_length, 0,
 						false, false);
 
-	/* Then the hint op */
+	/* Add the other op(s) */
 
-	osd_req_op_alloc_hint_init(osd_req, 1, rbd_obj_bytes(&rbd_dev->header),
-				   rbd_obj_bytes(&rbd_dev->header));
-
-	/* And the original write request op */
-
-	offset = orig_request->offset;
-	length = orig_request->length;
-	osd_req_op_extent_init(osd_req, 2, CEPH_OSD_OP_WRITE,
-					offset, length, 0, 0);
-	if (orig_request->type == OBJ_REQUEST_BIO)
-		osd_req_op_extent_osd_data_bio(osd_req, 2,
-					orig_request->bio_list, length);
-	else
-		osd_req_op_extent_osd_data_pages(osd_req, 2,
-					orig_request->pages, length,
-					offset & ~PAGE_MASK, false, false);
-
-	rbd_osd_req_format_write(orig_request);
+	op_type = rbd_img_request_op_type(orig_request->img_request);
+	rbd_img_obj_request_fill(orig_request, osd_req, op_type, 1);
 
 	/* All set, send it off. */
 
@@ -2728,7 +2832,7 @@ static int rbd_img_obj_exists_submit(struct rbd_obj_request *obj_request)
 
 	rbd_assert(obj_request->img_request);
 	rbd_dev = obj_request->img_request->rbd_dev;
-	stat_request->osd_req = rbd_osd_req_create(rbd_dev, false, 1,
+	stat_request->osd_req = rbd_osd_req_create(rbd_dev, OBJ_OP_READ, 1,
 						   stat_request);
 	if (!stat_request->osd_req)
 		goto out;
@@ -2748,11 +2852,10 @@ out:
 	return ret;
 }
 
-static int rbd_img_obj_request_submit(struct rbd_obj_request *obj_request)
+static bool img_obj_request_simple(struct rbd_obj_request *obj_request)
 {
 	struct rbd_img_request *img_request;
 	struct rbd_device *rbd_dev;
-	bool known;
 
 	rbd_assert(obj_request_img_data_test(obj_request));
 
@@ -2760,22 +2863,44 @@ static int rbd_img_obj_request_submit(struct rbd_obj_request *obj_request)
 	rbd_assert(img_request);
 	rbd_dev = img_request->rbd_dev;
 
+	/* Reads */
+	if (!img_request_write_test(img_request) &&
+	    !img_request_discard_test(img_request))
+		return true;
+
+	/* Non-layered writes */
+	if (!img_request_layered_test(img_request))
+		return true;
+
+	/*
+	 * Layered writes outside of the parent overlap range don't
+	 * share any data with the parent.
+	 */
+	if (!obj_request_overlaps_parent(obj_request))
+		return true;
+
 	/*
-	 * Only writes to layered images need special handling.
-	 * Reads and non-layered writes are simple object requests.
-	 * Layered writes that start beyond the end of the overlap
-	 * with the parent have no parent data, so they too are
-	 * simple object requests.  Finally, if the target object is
-	 * known to already exist, its parent data has already been
-	 * copied, so a write to the object can also be handled as a
-	 * simple object request.
+	 * Entire-object layered writes - we will overwrite whatever
+	 * parent data there is anyway.
 	 */
-	if (!img_request_write_test(img_request) ||
-		!img_request_layered_test(img_request) ||
-		!obj_request_overlaps_parent(obj_request) ||
-		((known = obj_request_known_test(obj_request)) &&
-			obj_request_exists_test(obj_request))) {
+	if (!obj_request->offset &&
+	    obj_request->length == rbd_obj_bytes(&rbd_dev->header))
+		return true;
 
+	/*
+	 * If the object is known to already exist, its parent data has
+	 * already been copied.
+	 */
+	if (obj_request_known_test(obj_request) &&
+	    obj_request_exists_test(obj_request))
+		return true;
+
+	return false;
+}
+
+static int rbd_img_obj_request_submit(struct rbd_obj_request *obj_request)
+{
+	if (img_obj_request_simple(obj_request)) {
 		struct rbd_device *rbd_dev;
 		struct ceph_osd_client *osdc;
 
@@ -2791,7 +2916,7 @@ static int rbd_img_obj_request_submit(struct rbd_obj_request *obj_request)
 	 * start by reading the data for the full target object from
 	 * the parent so we can use it for a copyup to the target.
 	 */
-	if (known)
+	if (obj_request_known_test(obj_request))
 		return rbd_img_obj_parent_read_full(obj_request);
 
 	/* We don't know whether the target exists.  Go find out. */
@@ -2932,7 +3057,7 @@ static int rbd_obj_notify_ack_sync(struct rbd_device *rbd_dev, u64 notify_id)
 		return -ENOMEM;
 
 	ret = -ENOMEM;
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false, 1,
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, OBJ_OP_READ, 1,
 						  obj_request);
 	if (!obj_request->osd_req)
 		goto out;
@@ -2995,7 +3120,7 @@ static struct rbd_obj_request *rbd_obj_watch_request_helper(
 	if (!obj_request)
 		return ERR_PTR(-ENOMEM);
 
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, true, 1,
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, OBJ_OP_WRITE, 1,
 						  obj_request);
 	if (!obj_request->osd_req) {
 		ret = -ENOMEM;
@@ -3133,7 +3258,7 @@ static int rbd_obj_method_sync(struct rbd_device *rbd_dev,
 	obj_request->pages = pages;
 	obj_request->page_count = page_count;
 
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false, 1,
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, OBJ_OP_READ, 1,
 						  obj_request);
 	if (!obj_request->osd_req)
 		goto out;
@@ -3183,11 +3308,20 @@ out:
 static void rbd_handle_request(struct rbd_device *rbd_dev, struct request *rq)
 {
 	struct rbd_img_request *img_request;
+	struct ceph_snap_context *snapc = NULL;
 	u64 offset = (u64)blk_rq_pos(rq) << SECTOR_SHIFT;
 	u64 length = blk_rq_bytes(rq);
-	bool wr = rq_data_dir(rq) == WRITE;
+	enum obj_operation_type op_type;
+	u64 mapping_size;
 	int result;
 
+	if (rq->cmd_flags & REQ_DISCARD)
+		op_type = OBJ_OP_DISCARD;
+	else if (rq->cmd_flags & REQ_WRITE)
+		op_type = OBJ_OP_WRITE;
+	else
+		op_type = OBJ_OP_READ;
+
 	/* Ignore/skip any zero-length requests */
 
 	if (!length) {
@@ -3196,9 +3330,9 @@ static void rbd_handle_request(struct rbd_device *rbd_dev, struct request *rq)
 		goto err_rq;
 	}
 
-	/* Disallow writes to a read-only device */
+	/* Only reads are allowed to a read-only device */
 
-	if (wr) {
+	if (op_type != OBJ_OP_READ) {
 		if (rbd_dev->mapping.read_only) {
 			result = -EROFS;
 			goto err_rq;
@@ -3226,21 +3360,35 @@ static void rbd_handle_request(struct rbd_device *rbd_dev, struct request *rq)
 		goto err_rq;	/* Shouldn't happen */
 	}
 
-	if (offset + length > rbd_dev->mapping.size) {
+	down_read(&rbd_dev->header_rwsem);
+	mapping_size = rbd_dev->mapping.size;
+	if (op_type != OBJ_OP_READ) {
+		snapc = rbd_dev->header.snapc;
+		ceph_get_snap_context(snapc);
+	}
+	up_read(&rbd_dev->header_rwsem);
+
+	if (offset + length > mapping_size) {
 		rbd_warn(rbd_dev, "beyond EOD (%llu~%llu > %llu)", offset,
-			 length, rbd_dev->mapping.size);
+			 length, mapping_size);
 		result = -EIO;
 		goto err_rq;
 	}
 
-	img_request = rbd_img_request_create(rbd_dev, offset, length, wr);
+	img_request = rbd_img_request_create(rbd_dev, offset, length, op_type,
+					     snapc);
 	if (!img_request) {
 		result = -ENOMEM;
 		goto err_rq;
 	}
 	img_request->rq = rq;
 
-	result = rbd_img_request_fill(img_request, OBJ_REQUEST_BIO, rq->bio);
+	if (op_type == OBJ_OP_DISCARD)
+		result = rbd_img_request_fill(img_request, OBJ_REQUEST_NODATA,
+					      NULL);
+	else
+		result = rbd_img_request_fill(img_request, OBJ_REQUEST_BIO,
+					      rq->bio);
 	if (result)
 		goto err_img_request;
 
@@ -3255,7 +3403,9 @@ err_img_request:
 err_rq:
 	if (result)
 		rbd_warn(rbd_dev, "%s %llx at %llx result %d",
-			 wr ? "write" : "read", length, offset, result);
+			 obj_op_name(op_type), length, offset, result);
+	if (snapc)
+		ceph_put_snap_context(snapc);
 	blk_end_request_all(rq, result);
 }
 
@@ -3393,7 +3543,7 @@ static int rbd_obj_read_sync(struct rbd_device *rbd_dev,
 	obj_request->pages = pages;
 	obj_request->page_count = page_count;
 
-	obj_request->osd_req = rbd_osd_req_create(rbd_dev, false, 1,
+	obj_request->osd_req = rbd_osd_req_create(rbd_dev, OBJ_OP_READ, 1,
 						  obj_request);
 	if (!obj_request->osd_req)
 		goto out;
@@ -3610,6 +3760,13 @@ static int rbd_init_disk(struct rbd_device *rbd_dev)
 	blk_queue_io_min(q, segment_size);
 	blk_queue_io_opt(q, segment_size);
 
+	/* enable the discard support */
+	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, q);
+	q->limits.discard_granularity = segment_size;
+	q->limits.discard_alignment = segment_size;
+	q->limits.max_discard_sectors = segment_size / SECTOR_SIZE;
+	q->limits.discard_zeroes_data = 1;
+
 	blk_queue_merge_bvec(q, rbd_merge_bvec);
 	disk->queue = q;
 
@@ -4924,7 +5081,7 @@ static int rbd_dev_image_id(struct rbd_device *rbd_dev)
 		ret = image_id ? 0 : -ENOMEM;
 		if (!ret)
 			rbd_dev->image_format = 1;
-	} else if (ret > sizeof (__le32)) {
+	} else if (ret >= 0) {
 		void *p = response;
 
 		image_id = ceph_extract_encoded_string(&p, p + ret,
@@ -4932,8 +5089,6 @@ static int rbd_dev_image_id(struct rbd_device *rbd_dev)
 		ret = PTR_ERR_OR_ZERO(image_id);
 		if (!ret)
 			rbd_dev->image_format = 2;
-	} else {
-		ret = -EINVAL;
 	}
 
 	if (!ret) {
@@ -5087,9 +5242,12 @@ static int rbd_dev_device_setup(struct rbd_device *rbd_dev)
 	set_capacity(rbd_dev->disk, rbd_dev->mapping.size / SECTOR_SIZE);
 	set_disk_ro(rbd_dev->disk, rbd_dev->mapping.read_only);
 
-	rbd_dev->rq_wq = alloc_workqueue(rbd_dev->disk->disk_name, 0, 0);
-	if (!rbd_dev->rq_wq)
+	rbd_dev->rq_wq = alloc_workqueue("%s", WQ_MEM_RECLAIM, 0,
+					 rbd_dev->disk->disk_name);
+	if (!rbd_dev->rq_wq) {
+		ret = -ENOMEM;
 		goto err_out_mapping;
+	}
 
 	ret = rbd_bus_add_dev(rbd_dev);
 	if (ret)
diff --git a/drivers/block/rsxx/core.c b/drivers/block/rsxx/core.c
index 820b4009d5f7..d8b2488aaade 100644
--- a/drivers/block/rsxx/core.c
+++ b/drivers/block/rsxx/core.c
@@ -62,12 +62,6 @@ static DEFINE_SPINLOCK(rsxx_ida_lock);
 
 /* --------------------Debugfs Setup ------------------- */
 
-struct rsxx_cram {
-	u32 f_pos;
-	u32 offset;
-	void *i_private;
-};
-
 static int rsxx_attr_pci_regs_show(struct seq_file *m, void *p)
 {
 	struct rsxx_cardinfo *card = m->private;
@@ -184,93 +178,50 @@ static int rsxx_attr_pci_regs_open(struct inode *inode, struct file *file)
 static ssize_t rsxx_cram_read(struct file *fp, char __user *ubuf,
 			      size_t cnt, loff_t *ppos)
 {
-	struct rsxx_cram *info = fp->private_data;
-	struct rsxx_cardinfo *card = info->i_private;
+	struct rsxx_cardinfo *card = file_inode(fp)->i_private;
 	char *buf;
-	int st;
+	ssize_t st;
 
-	buf = kzalloc(sizeof(*buf) * cnt, GFP_KERNEL);
+	buf = kzalloc(cnt, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
-	info->f_pos = (u32)*ppos + info->offset;
-
-	st = rsxx_creg_read(card, CREG_ADD_CRAM + info->f_pos, cnt, buf, 1);
-	if (st)
-		return st;
-
-	st = copy_to_user(ubuf, buf, cnt);
+	st = rsxx_creg_read(card, CREG_ADD_CRAM + (u32)*ppos, cnt, buf, 1);
+	if (!st)
+		st = copy_to_user(ubuf, buf, cnt);
+	kfree(buf);
 	if (st)
 		return st;
-
-	info->offset += cnt;
-
-	kfree(buf);
-
+	*ppos += cnt;
 	return cnt;
 }
 
 static ssize_t rsxx_cram_write(struct file *fp, const char __user *ubuf,
 			       size_t cnt, loff_t *ppos)
 {
-	struct rsxx_cram *info = fp->private_data;
-	struct rsxx_cardinfo *card = info->i_private;
+	struct rsxx_cardinfo *card = file_inode(fp)->i_private;
 	char *buf;
-	int st;
+	ssize_t st;
 
-	buf = kzalloc(sizeof(*buf) * cnt, GFP_KERNEL);
+	buf = kzalloc(cnt, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
 	st = copy_from_user(buf, ubuf, cnt);
+	if (!st)
+		st = rsxx_creg_write(card, CREG_ADD_CRAM + (u32)*ppos, cnt,
+				     buf, 1);
+	kfree(buf);
 	if (st)
 		return st;
-
-	info->f_pos = (u32)*ppos + info->offset;
-
-	st = rsxx_creg_write(card, CREG_ADD_CRAM + info->f_pos, cnt, buf, 1);
-	if (st)
-		return st;
-
-	info->offset += cnt;
-
-	kfree(buf);
-
+	*ppos += cnt;
 	return cnt;
 }
 
-static int rsxx_cram_open(struct inode *inode, struct file *file)
-{
-	struct rsxx_cram *info = kzalloc(sizeof(*info), GFP_KERNEL);
-	if (!info)
-		return -ENOMEM;
-
-	info->i_private = inode->i_private;
-	info->f_pos = file->f_pos;
-	file->private_data = info;
-
-	return 0;
-}
-
-static int rsxx_cram_release(struct inode *inode, struct file *file)
-{
-	struct rsxx_cram *info = file->private_data;
-
-	if (!info)
-		return 0;
-
-	kfree(info);
-	file->private_data = NULL;
-
-	return 0;
-}
-
 static const struct file_operations debugfs_cram_fops = {
 	.owner		= THIS_MODULE,
-	.open		= rsxx_cram_open,
 	.read		= rsxx_cram_read,
 	.write		= rsxx_cram_write,
-	.release	= rsxx_cram_release,
 };
 
 static const struct file_operations debugfs_stats_fops = {
@@ -886,7 +837,7 @@ static int rsxx_pci_probe(struct pci_dev *dev,
 				"Failed to enable MSI\n");
 	}
 
-	st = request_irq(dev->irq, rsxx_isr, IRQF_DISABLED | IRQF_SHARED,
+	st = request_irq(dev->irq, rsxx_isr, IRQF_SHARED,
 			 DRIVER_NAME, card);
 	if (st) {
 		dev_err(CARD_TO_DEV(card),
diff --git a/drivers/block/rsxx/dev.c b/drivers/block/rsxx/dev.c
index 2839d37e5af7..40ee7705df63 100644
--- a/drivers/block/rsxx/dev.c
+++ b/drivers/block/rsxx/dev.c
@@ -307,6 +307,7 @@ int rsxx_setup_dev(struct rsxx_cardinfo *card)
 	blk_queue_physical_block_size(card->queue, RSXX_HW_BLK_SIZE);
 
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, card->queue);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, card->queue);
 	if (rsxx_discard_supported(card)) {
 		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, card->queue);
 		blk_queue_max_discard_sectors(card->queue,
diff --git a/drivers/block/skd_main.c b/drivers/block/skd_main.c
index 8fcdcfb4b472..1e46eb2305c0 100644
--- a/drivers/block/skd_main.c
+++ b/drivers/block/skd_main.c
@@ -4426,6 +4426,7 @@ static int skd_cons_disk(struct skd_device *skdev)
 	q->limits.discard_zeroes_data = 1;
 	queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, q);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, q);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, q);
 
 	spin_lock_irqsave(&skdev->lock, flags);
 	pr_debug("%s:%s:%d stopping %s queue\n",
diff --git a/drivers/block/sunvdc.c b/drivers/block/sunvdc.c
index 5814deb6963d..756b8ec00f16 100644
--- a/drivers/block/sunvdc.c
+++ b/drivers/block/sunvdc.c
@@ -9,6 +9,7 @@
 #include <linux/blkdev.h>
 #include <linux/hdreg.h>
 #include <linux/genhd.h>
+#include <linux/cdrom.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/completion.h>
@@ -22,8 +23,8 @@
 
 #define DRV_MODULE_NAME		"sunvdc"
 #define PFX DRV_MODULE_NAME	": "
-#define DRV_MODULE_VERSION	"1.0"
-#define DRV_MODULE_RELDATE	"June 25, 2007"
+#define DRV_MODULE_VERSION	"1.1"
+#define DRV_MODULE_RELDATE	"February 13, 2013"
 
 static char version[] =
 	DRV_MODULE_NAME ".c:v" DRV_MODULE_VERSION " (" DRV_MODULE_RELDATE ")\n";
@@ -32,7 +33,7 @@ MODULE_DESCRIPTION("Sun LDOM virtual disk client driver");
 MODULE_LICENSE("GPL");
 MODULE_VERSION(DRV_MODULE_VERSION);
 
-#define VDC_TX_RING_SIZE	256
+#define VDC_TX_RING_SIZE	512
 
 #define WAITING_FOR_LINK_UP	0x01
 #define WAITING_FOR_TX_SPACE	0x02
@@ -65,10 +66,10 @@ struct vdc_port {
 	u64			operations;
 	u32			vdisk_size;
 	u8			vdisk_type;
+	u8			vdisk_mtype;
 
 	char			disk_name[32];
 
-	struct vio_disk_geom	geom;
 	struct vio_disk_vtoc	label;
 };
 
@@ -79,9 +80,16 @@ static inline struct vdc_port *to_vdc_port(struct vio_driver_state *vio)
 
 /* Ordered from largest major to lowest */
 static struct vio_version vdc_versions[] = {
+	{ .major = 1, .minor = 1 },
 	{ .major = 1, .minor = 0 },
 };
 
+static inline int vdc_version_supported(struct vdc_port *port,
+					u16 major, u16 minor)
+{
+	return port->vio.ver.major == major && port->vio.ver.minor >= minor;
+}
+
 #define VDCBLK_NAME	"vdisk"
 static int vdc_major;
 #define PARTITION_SHIFT	3
@@ -94,18 +102,54 @@ static inline u32 vdc_tx_dring_avail(struct vio_dring_state *dr)
 static int vdc_getgeo(struct block_device *bdev, struct hd_geometry *geo)
 {
 	struct gendisk *disk = bdev->bd_disk;
-	struct vdc_port *port = disk->private_data;
+	sector_t nsect = get_capacity(disk);
+	sector_t cylinders = nsect;
 
-	geo->heads = (u8) port->geom.num_hd;
-	geo->sectors = (u8) port->geom.num_sec;
-	geo->cylinders = port->geom.num_cyl;
+	geo->heads = 0xff;
+	geo->sectors = 0x3f;
+	sector_div(cylinders, geo->heads * geo->sectors);
+	geo->cylinders = cylinders;
+	if ((sector_t)(geo->cylinders + 1) * geo->heads * geo->sectors < nsect)
+		geo->cylinders = 0xffff;
 
 	return 0;
 }
 
+/* Add ioctl/CDROM_GET_CAPABILITY to support cdrom_id in udev
+ * when vdisk_mtype is VD_MEDIA_TYPE_CD or VD_MEDIA_TYPE_DVD.
+ * Needed to be able to install inside an ldom from an iso image.
+ */
+static int vdc_ioctl(struct block_device *bdev, fmode_t mode,
+		     unsigned command, unsigned long argument)
+{
+	int i;
+	struct gendisk *disk;
+
+	switch (command) {
+	case CDROMMULTISESSION:
+		pr_debug(PFX "Multisession CDs not supported\n");
+		for (i = 0; i < sizeof(struct cdrom_multisession); i++)
+			if (put_user(0, (char __user *)(argument + i)))
+				return -EFAULT;
+		return 0;
+
+	case CDROM_GET_CAPABILITY:
+		disk = bdev->bd_disk;
+
+		if (bdev->bd_disk && (disk->flags & GENHD_FL_CD))
+			return 0;
+		return -EINVAL;
+
+	default:
+		pr_debug(PFX "ioctl %08x not supported\n", command);
+		return -EINVAL;
+	}
+}
+
 static const struct block_device_operations vdc_fops = {
 	.owner		= THIS_MODULE,
 	.getgeo		= vdc_getgeo,
+	.ioctl		= vdc_ioctl,
 };
 
 static void vdc_finish(struct vio_driver_state *vio, int err, int waiting_for)
@@ -165,9 +209,9 @@ static int vdc_handle_attr(struct vio_driver_state *vio, void *arg)
 	struct vio_disk_attr_info *pkt = arg;
 
 	viodbg(HS, "GOT ATTR stype[0x%x] ops[%llx] disk_size[%llu] disk_type[%x] "
-	       "xfer_mode[0x%x] blksz[%u] max_xfer[%llu]\n",
+	       "mtype[0x%x] xfer_mode[0x%x] blksz[%u] max_xfer[%llu]\n",
 	       pkt->tag.stype, pkt->operations,
-	       pkt->vdisk_size, pkt->vdisk_type,
+	       pkt->vdisk_size, pkt->vdisk_type, pkt->vdisk_mtype,
 	       pkt->xfer_mode, pkt->vdisk_block_size,
 	       pkt->max_xfer_size);
 
@@ -192,8 +236,11 @@ static int vdc_handle_attr(struct vio_driver_state *vio, void *arg)
 		}
 
 		port->operations = pkt->operations;
-		port->vdisk_size = pkt->vdisk_size;
 		port->vdisk_type = pkt->vdisk_type;
+		if (vdc_version_supported(port, 1, 1)) {
+			port->vdisk_size = pkt->vdisk_size;
+			port->vdisk_mtype = pkt->vdisk_mtype;
+		}
 		if (pkt->max_xfer_size < port->max_xfer_size)
 			port->max_xfer_size = pkt->max_xfer_size;
 		port->vdisk_block_size = pkt->vdisk_block_size;
@@ -236,7 +283,9 @@ static void vdc_end_one(struct vdc_port *port, struct vio_dring_state *dr,
 
 	__blk_end_request(req, (desc->status ? -EIO : 0), desc->size);
 
-	if (blk_queue_stopped(port->disk->queue))
+	/* restart blk queue when ring is half emptied */
+	if (blk_queue_stopped(port->disk->queue) &&
+	    vdc_tx_dring_avail(dr) * 100 / VDC_TX_RING_SIZE >= 50)
 		blk_start_queue(port->disk->queue);
 }
 
@@ -388,12 +437,6 @@ static int __send_request(struct request *req)
 	for (i = 0; i < nsg; i++)
 		len += sg[i].length;
 
-	if (unlikely(vdc_tx_dring_avail(dr) < 1)) {
-		blk_stop_queue(port->disk->queue);
-		err = -ENOMEM;
-		goto out;
-	}
-
 	desc = vio_dring_cur(dr);
 
 	err = ldc_map_sg(port->vio.lp, sg, nsg,
@@ -433,21 +476,32 @@ static int __send_request(struct request *req)
 		port->req_id++;
 		dr->prod = (dr->prod + 1) & (VDC_TX_RING_SIZE - 1);
 	}
-out:
 
 	return err;
 }
 
-static void do_vdc_request(struct request_queue *q)
+static void do_vdc_request(struct request_queue *rq)
 {
-	while (1) {
-		struct request *req = blk_fetch_request(q);
+	struct request *req;
 
-		if (!req)
-			break;
+	while ((req = blk_peek_request(rq)) != NULL) {
+		struct vdc_port *port;
+		struct vio_dring_state *dr;
+
+		port = req->rq_disk->private_data;
+		dr = &port->vio.drings[VIO_DRIVER_TX_RING];
+		if (unlikely(vdc_tx_dring_avail(dr) < 1))
+			goto wait;
+
+		blk_start_request(req);
 
-		if (__send_request(req) < 0)
-			__blk_end_request_all(req, -EIO);
+		if (__send_request(req) < 0) {
+			blk_requeue_request(rq, req);
+wait:
+			/* Avoid pointless unplugs. */
+			blk_stop_queue(rq);
+			break;
+		}
 	}
 }
 
@@ -663,18 +717,27 @@ static int probe_disk(struct vdc_port *port)
 		return err;
 	}
 
-	err = generic_request(port, VD_OP_GET_DISKGEOM,
-			      &port->geom, sizeof(port->geom));
-	if (err < 0) {
-		printk(KERN_ERR PFX "VD_OP_GET_DISKGEOM returns "
-		       "error %d\n", err);
-		return err;
+	if (vdc_version_supported(port, 1, 1)) {
+		/* vdisk_size should be set during the handshake, if it wasn't
+		 * then the underlying disk is reserved by another system
+		 */
+		if (port->vdisk_size == -1)
+			return -ENODEV;
+	} else {
+		struct vio_disk_geom geom;
+
+		err = generic_request(port, VD_OP_GET_DISKGEOM,
+				      &geom, sizeof(geom));
+		if (err < 0) {
+			printk(KERN_ERR PFX "VD_OP_GET_DISKGEOM returns "
+			       "error %d\n", err);
+			return err;
+		}
+		port->vdisk_size = ((u64)geom.num_cyl *
+				    (u64)geom.num_hd *
+				    (u64)geom.num_sec);
 	}
 
-	port->vdisk_size = ((u64)port->geom.num_cyl *
-			    (u64)port->geom.num_hd *
-			    (u64)port->geom.num_sec);
-
 	q = blk_init_queue(do_vdc_request, &port->vio.lock);
 	if (!q) {
 		printk(KERN_ERR PFX "%s: Could not allocate queue.\n",
@@ -691,6 +754,10 @@ static int probe_disk(struct vdc_port *port)
 
 	port->disk = g;
 
+	/* Each segment in a request is up to an aligned page in size. */
+	blk_queue_segment_boundary(q, PAGE_SIZE - 1);
+	blk_queue_max_segment_size(q, PAGE_SIZE);
+
 	blk_queue_max_segments(q, port->ring_cookies);
 	blk_queue_max_hw_sectors(q, port->max_xfer_size);
 	g->major = vdc_major;
@@ -704,9 +771,32 @@ static int probe_disk(struct vdc_port *port)
 
 	set_capacity(g, port->vdisk_size);
 
-	printk(KERN_INFO PFX "%s: %u sectors (%u MB)\n",
+	if (vdc_version_supported(port, 1, 1)) {
+		switch (port->vdisk_mtype) {
+		case VD_MEDIA_TYPE_CD:
+			pr_info(PFX "Virtual CDROM %s\n", port->disk_name);
+			g->flags |= GENHD_FL_CD;
+			g->flags |= GENHD_FL_REMOVABLE;
+			set_disk_ro(g, 1);
+			break;
+
+		case VD_MEDIA_TYPE_DVD:
+			pr_info(PFX "Virtual DVD %s\n", port->disk_name);
+			g->flags |= GENHD_FL_CD;
+			g->flags |= GENHD_FL_REMOVABLE;
+			set_disk_ro(g, 1);
+			break;
+
+		case VD_MEDIA_TYPE_FIXED:
+			pr_info(PFX "Virtual Hard disk %s\n", port->disk_name);
+			break;
+		}
+	}
+
+	pr_info(PFX "%s: %u sectors (%u MB) protocol %d.%d\n",
 	       g->disk_name,
-	       port->vdisk_size, (port->vdisk_size >> (20 - 9)));
+	       port->vdisk_size, (port->vdisk_size >> (20 - 9)),
+	       port->vio.ver.major, port->vio.ver.minor);
 
 	add_disk(g);
 
@@ -765,6 +855,7 @@ static int vdc_port_probe(struct vio_dev *vdev, const struct vio_device_id *id)
 	else
 		snprintf(port->disk_name, sizeof(port->disk_name),
 			 VDCBLK_NAME "%c", 'a' + ((int)vdev->dev_no % 26));
+	port->vdisk_size = -1;
 
 	err = vio_driver_init(&port->vio, vdev, VDEV_DISK,
 			      vdc_versions, ARRAY_SIZE(vdc_versions),
diff --git a/drivers/block/sx8.c b/drivers/block/sx8.c
index d5e2d12b9d9e..5d552857de41 100644
--- a/drivers/block/sx8.c
+++ b/drivers/block/sx8.c
@@ -568,7 +568,7 @@ static struct carm_request *carm_get_special(struct carm_host *host)
 		return NULL;
 
 	rq = blk_get_request(host->oob_q, WRITE /* bogus */, GFP_KERNEL);
-	if (!rq) {
+	if (IS_ERR(rq)) {
 		spin_lock_irqsave(&host->lock, flags);
 		carm_put_request(host, crq);
 		spin_unlock_irqrestore(&host->lock, flags);
diff --git a/drivers/block/virtio_blk.c b/drivers/block/virtio_blk.c
index 0a581400de0f..c6a27d54ad62 100644
--- a/drivers/block/virtio_blk.c
+++ b/drivers/block/virtio_blk.c
@@ -41,12 +41,6 @@ struct virtio_blk
 	/* Process context for config space updates */
 	struct work_struct config_work;
 
-	/* Lock for config space updates */
-	struct mutex config_lock;
-
-	/* enable config space updates */
-	bool config_enable;
-
 	/* What host tells us, plus 2 for header & tailer. */
 	unsigned int sg_elems;
 
@@ -135,7 +129,7 @@ static inline void virtblk_request_done(struct request *req)
 		req->errors = (error != 0);
 	}
 
-	blk_mq_end_io(req, error);
+	blk_mq_end_request(req, error);
 }
 
 static void virtblk_done(struct virtqueue *vq)
@@ -164,14 +158,14 @@ static void virtblk_done(struct virtqueue *vq)
 	spin_unlock_irqrestore(&vblk->vqs[qid].lock, flags);
 }
 
-static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
+static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req,
+		bool last)
 {
 	struct virtio_blk *vblk = hctx->queue->queuedata;
 	struct virtblk_req *vbr = blk_mq_rq_to_pdu(req);
 	unsigned long flags;
 	unsigned int num;
 	int qid = hctx->queue_num;
-	const bool last = (req->cmd_flags & REQ_END) != 0;
 	int err;
 	bool notify = false;
 
@@ -205,6 +199,8 @@ static int virtio_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 		}
 	}
 
+	blk_mq_start_request(req);
+
 	num = blk_rq_map_sg(hctx->queue, vbr->req, vbr->sg);
 	if (num) {
 		if (rq_data_dir(vbr->req) == WRITE)
@@ -347,10 +343,6 @@ static void virtblk_config_changed_work(struct work_struct *work)
 	char *envp[] = { "RESIZE=1", NULL };
 	u64 capacity, size;
 
-	mutex_lock(&vblk->config_lock);
-	if (!vblk->config_enable)
-		goto done;
-
 	/* Host must always specify the capacity. */
 	virtio_cread(vdev, struct virtio_blk_config, capacity, &capacity);
 
@@ -374,8 +366,6 @@ static void virtblk_config_changed_work(struct work_struct *work)
 	set_capacity(vblk->disk, capacity);
 	revalidate_disk(vblk->disk);
 	kobject_uevent_env(&disk_to_dev(vblk->disk)->kobj, KOBJ_CHANGE, envp);
-done:
-	mutex_unlock(&vblk->config_lock);
 }
 
 static void virtblk_config_changed(struct virtio_device *vdev)
@@ -606,10 +596,8 @@ static int virtblk_probe(struct virtio_device *vdev)
 
 	vblk->vdev = vdev;
 	vblk->sg_elems = sg_elems;
-	mutex_init(&vblk->config_lock);
 
 	INIT_WORK(&vblk->config_work, virtblk_config_changed_work);
-	vblk->config_enable = true;
 
 	err = init_vq(vblk);
 	if (err)
@@ -733,6 +721,8 @@ static int virtblk_probe(struct virtio_device *vdev)
 	if (!err && opt_io_size)
 		blk_queue_io_opt(q, blk_size * opt_io_size);
 
+	virtio_device_ready(vdev);
+
 	add_disk(vblk->disk);
 	err = device_create_file(disk_to_dev(vblk->disk), &dev_attr_serial);
 	if (err)
@@ -771,10 +761,8 @@ static void virtblk_remove(struct virtio_device *vdev)
 	int index = vblk->index;
 	int refc;
 
-	/* Prevent config work handler from accessing the device. */
-	mutex_lock(&vblk->config_lock);
-	vblk->config_enable = false;
-	mutex_unlock(&vblk->config_lock);
+	/* Make sure no work handler is accessing the device. */
+	flush_work(&vblk->config_work);
 
 	del_gendisk(vblk->disk);
 	blk_cleanup_queue(vblk->disk->queue);
@@ -784,8 +772,6 @@ static void virtblk_remove(struct virtio_device *vdev)
 	/* Stop all the virtqueues. */
 	vdev->config->reset(vdev);
 
-	flush_work(&vblk->config_work);
-
 	refc = atomic_read(&disk_to_dev(vblk->disk)->kobj.kref.refcount);
 	put_disk(vblk->disk);
 	vdev->config->del_vqs(vdev);
@@ -805,11 +791,7 @@ static int virtblk_freeze(struct virtio_device *vdev)
 	/* Ensure we don't receive any more interrupts */
 	vdev->config->reset(vdev);
 
-	/* Prevent config work handler from accessing the device. */
-	mutex_lock(&vblk->config_lock);
-	vblk->config_enable = false;
-	mutex_unlock(&vblk->config_lock);
-
+	/* Make sure no work handler is accessing the device. */
 	flush_work(&vblk->config_work);
 
 	blk_mq_stop_hw_queues(vblk->disk->queue);
@@ -823,12 +805,14 @@ static int virtblk_restore(struct virtio_device *vdev)
 	struct virtio_blk *vblk = vdev->priv;
 	int ret;
 
-	vblk->config_enable = true;
 	ret = init_vq(vdev->priv);
-	if (!ret)
-		blk_mq_start_stopped_hw_queues(vblk->disk->queue, true);
+	if (ret)
+		return ret;
+
+	virtio_device_ready(vdev);
 
-	return ret;
+	blk_mq_start_stopped_hw_queues(vblk->disk->queue, true);
+	return 0;
 }
 #endif
 
diff --git a/drivers/block/xen-blkback/blkback.c b/drivers/block/xen-blkback/blkback.c
index 64c60edcdfbc..63fc7f06a014 100644
--- a/drivers/block/xen-blkback/blkback.c
+++ b/drivers/block/xen-blkback/blkback.c
@@ -763,6 +763,7 @@ again:
 			BUG_ON(new_map_idx >= segs_to_map);
 			if (unlikely(map[new_map_idx].status != 0)) {
 				pr_debug(DRV_PFX "invalid buffer -- could not remap it\n");
+				put_free_pages(blkif, &pages[seg_idx]->page, 1);
 				pages[seg_idx]->handle = BLKBACK_INVALID_HANDLE;
 				ret |= 1;
 				goto next;
diff --git a/drivers/block/xen-blkback/xenbus.c b/drivers/block/xen-blkback/xenbus.c
index 3a8b810b4980..630a489e757d 100644
--- a/drivers/block/xen-blkback/xenbus.c
+++ b/drivers/block/xen-blkback/xenbus.c
@@ -270,6 +270,9 @@ static int xen_blkif_disconnect(struct xen_blkif *blkif)
 		blkif->blk_rings.common.sring = NULL;
 	}
 
+	/* Remove all persistent grants and the cache of ballooned pages. */
+	xen_blkbk_free_caches(blkif);
+
 	return 0;
 }
 
@@ -281,9 +284,6 @@ static void xen_blkif_free(struct xen_blkif *blkif)
 	xen_blkif_disconnect(blkif);
 	xen_vbd_free(&blkif->vbd);
 
-	/* Remove all persistent grants and the cache of ballooned pages. */
-	xen_blkbk_free_caches(blkif);
-
 	/* Make sure everything is drained before shutting down */
 	BUG_ON(blkif->persistent_gnt_c != 0);
 	BUG_ON(atomic_read(&blkif->persistent_gnt_in_use) != 0);
@@ -907,22 +907,17 @@ static int connect_ring(struct backend_info *be)
 	return 0;
 }
 
-
-/* ** Driver Registration ** */
-
-
 static const struct xenbus_device_id xen_blkbk_ids[] = {
 	{ "vbd" },
 	{ "" }
 };
 
-
-static DEFINE_XENBUS_DRIVER(xen_blkbk, ,
+static struct xenbus_driver xen_blkbk_driver = {
+	.ids  = xen_blkbk_ids,
 	.probe = xen_blkbk_probe,
 	.remove = xen_blkbk_remove,
 	.otherend_changed = frontend_changed
-);
-
+};
 
 int xen_blkif_xenbus_init(void)
 {
diff --git a/drivers/block/xen-blkfront.c b/drivers/block/xen-blkfront.c
index 5deb235bd18f..5ac312f6e0be 100644
--- a/drivers/block/xen-blkfront.c
+++ b/drivers/block/xen-blkfront.c
@@ -582,6 +582,14 @@ static inline void flush_requests(struct blkfront_info *info)
 		notify_remote_via_irq(info->irq);
 }
 
+static inline bool blkif_request_flush_valid(struct request *req,
+					     struct blkfront_info *info)
+{
+	return ((req->cmd_type != REQ_TYPE_FS) ||
+		((req->cmd_flags & (REQ_FLUSH | REQ_FUA)) &&
+		!info->flush_op));
+}
+
 /*
  * do_blkif_request
  *  read a block; request is in a request queue
@@ -604,9 +612,7 @@ static void do_blkif_request(struct request_queue *rq)
 
 		blk_start_request(req);
 
-		if ((req->cmd_type != REQ_TYPE_FS) ||
-		    ((req->cmd_flags & (REQ_FLUSH | REQ_FUA)) &&
-		    !info->flush_op)) {
+		if (blkif_request_flush_valid(req, info)) {
 			__blk_end_request_all(req, -EIO);
 			continue;
 		}
@@ -2055,13 +2061,14 @@ static const struct xenbus_device_id blkfront_ids[] = {
 	{ "" }
 };
 
-static DEFINE_XENBUS_DRIVER(blkfront, ,
+static struct xenbus_driver blkfront_driver = {
+	.ids  = blkfront_ids,
 	.probe = blkfront_probe,
 	.remove = blkfront_remove,
 	.resume = blkfront_resume,
 	.otherend_changed = blkback_changed,
 	.is_ready = blkfront_is_ready,
-);
+};
 
 static int __init xlblk_init(void)
 {
diff --git a/drivers/block/xsysace.c b/drivers/block/xsysace.c
index ab3ea62e5dfc..c4328d9d9981 100644
--- a/drivers/block/xsysace.c
+++ b/drivers/block/xsysace.c
@@ -1203,7 +1203,6 @@ static struct platform_driver ace_platform_driver = {
 	.probe = ace_probe,
 	.remove = ace_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = "xsysace",
 		.of_match_table = ace_of_match,
 	},
diff --git a/drivers/block/zram/zram_drv.c b/drivers/block/zram/zram_drv.c
index dfa4024c448a..0e63e8aa8279 100644
--- a/drivers/block/zram/zram_drv.c
+++ b/drivers/block/zram/zram_drv.c
@@ -103,10 +103,10 @@ static ssize_t mem_used_total_show(struct device *dev,
 
 	down_read(&zram->init_lock);
 	if (init_done(zram))
-		val = zs_get_total_size_bytes(meta->mem_pool);
+		val = zs_get_total_pages(meta->mem_pool);
 	up_read(&zram->init_lock);
 
-	return scnprintf(buf, PAGE_SIZE, "%llu\n", val);
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", val << PAGE_SHIFT);
 }
 
 static ssize_t max_comp_streams_show(struct device *dev,
@@ -122,6 +122,72 @@ static ssize_t max_comp_streams_show(struct device *dev,
 	return scnprintf(buf, PAGE_SIZE, "%d\n", val);
 }
 
+static ssize_t mem_limit_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	u64 val;
+	struct zram *zram = dev_to_zram(dev);
+
+	down_read(&zram->init_lock);
+	val = zram->limit_pages;
+	up_read(&zram->init_lock);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", val << PAGE_SHIFT);
+}
+
+static ssize_t mem_limit_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	u64 limit;
+	char *tmp;
+	struct zram *zram = dev_to_zram(dev);
+
+	limit = memparse(buf, &tmp);
+	if (buf == tmp) /* no chars parsed, invalid input */
+		return -EINVAL;
+
+	down_write(&zram->init_lock);
+	zram->limit_pages = PAGE_ALIGN(limit) >> PAGE_SHIFT;
+	up_write(&zram->init_lock);
+
+	return len;
+}
+
+static ssize_t mem_used_max_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	u64 val = 0;
+	struct zram *zram = dev_to_zram(dev);
+
+	down_read(&zram->init_lock);
+	if (init_done(zram))
+		val = atomic_long_read(&zram->stats.max_used_pages);
+	up_read(&zram->init_lock);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", val << PAGE_SHIFT);
+}
+
+static ssize_t mem_used_max_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t len)
+{
+	int err;
+	unsigned long val;
+	struct zram *zram = dev_to_zram(dev);
+	struct zram_meta *meta = zram->meta;
+
+	err = kstrtoul(buf, 10, &val);
+	if (err || val != 0)
+		return -EINVAL;
+
+	down_read(&zram->init_lock);
+	if (init_done(zram))
+		atomic_long_set(&zram->stats.max_used_pages,
+				zs_get_total_pages(meta->mem_pool));
+	up_read(&zram->init_lock);
+
+	return len;
+}
+
 static ssize_t max_comp_streams_store(struct device *dev,
 		struct device_attribute *attr, const char *buf, size_t len)
 {
@@ -378,7 +444,6 @@ static int zram_decompress_page(struct zram *zram, char *mem, u32 index)
 	/* Should NEVER happen. Return bio error if it does. */
 	if (unlikely(ret)) {
 		pr_err("Decompression failed! err=%d, page=%u\n", ret, index);
-		atomic64_inc(&zram->stats.failed_reads);
 		return ret;
 	}
 
@@ -435,6 +500,21 @@ out_cleanup:
 	return ret;
 }
 
+static inline void update_used_max(struct zram *zram,
+					const unsigned long pages)
+{
+	int old_max, cur_max;
+
+	old_max = atomic_long_read(&zram->stats.max_used_pages);
+
+	do {
+		cur_max = old_max;
+		if (pages > cur_max)
+			old_max = atomic_long_cmpxchg(
+				&zram->stats.max_used_pages, cur_max, pages);
+	} while (old_max != cur_max);
+}
+
 static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 			   int offset)
 {
@@ -446,6 +526,7 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 	struct zram_meta *meta = zram->meta;
 	struct zcomp_strm *zstrm;
 	bool locked = false;
+	unsigned long alloced_pages;
 
 	page = bvec->bv_page;
 	if (is_partial_io(bvec)) {
@@ -514,6 +595,16 @@ static int zram_bvec_write(struct zram *zram, struct bio_vec *bvec, u32 index,
 		ret = -ENOMEM;
 		goto out;
 	}
+
+	alloced_pages = zs_get_total_pages(meta->mem_pool);
+	if (zram->limit_pages && alloced_pages > zram->limit_pages) {
+		zs_free(meta->mem_pool, handle);
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	update_used_max(zram, alloced_pages);
+
 	cmem = zs_map_object(meta->mem_pool, handle, ZS_MM_WO);
 
 	if ((clen == PAGE_SIZE) && !is_partial_io(bvec)) {
@@ -547,8 +638,6 @@ out:
 		zcomp_strm_release(zram->comp, zstrm);
 	if (is_partial_io(bvec))
 		kfree(uncmem);
-	if (ret)
-		atomic64_inc(&zram->stats.failed_writes);
 	return ret;
 }
 
@@ -566,6 +655,13 @@ static int zram_bvec_rw(struct zram *zram, struct bio_vec *bvec, u32 index,
 		ret = zram_bvec_write(zram, bvec, index, offset);
 	}
 
+	if (unlikely(ret)) {
+		if (rw == READ)
+			atomic64_inc(&zram->stats.failed_reads);
+		else
+			atomic64_inc(&zram->stats.failed_writes);
+	}
+
 	return ret;
 }
 
@@ -602,6 +698,7 @@ static void zram_bio_discard(struct zram *zram, u32 index,
 		bit_spin_lock(ZRAM_ACCESS, &meta->table[index].value);
 		zram_free_page(zram, index);
 		bit_spin_unlock(ZRAM_ACCESS, &meta->table[index].value);
+		atomic64_inc(&zram->stats.notify_free);
 		index++;
 		n -= PAGE_SIZE;
 	}
@@ -613,6 +710,9 @@ static void zram_reset_device(struct zram *zram, bool reset_capacity)
 	struct zram_meta *meta;
 
 	down_write(&zram->init_lock);
+
+	zram->limit_pages = 0;
+
 	if (!init_done(zram)) {
 		up_write(&zram->init_lock);
 		return;
@@ -853,6 +953,10 @@ static DEVICE_ATTR(initstate, S_IRUGO, initstate_show, NULL);
 static DEVICE_ATTR(reset, S_IWUSR, NULL, reset_store);
 static DEVICE_ATTR(orig_data_size, S_IRUGO, orig_data_size_show, NULL);
 static DEVICE_ATTR(mem_used_total, S_IRUGO, mem_used_total_show, NULL);
+static DEVICE_ATTR(mem_limit, S_IRUGO | S_IWUSR, mem_limit_show,
+		mem_limit_store);
+static DEVICE_ATTR(mem_used_max, S_IRUGO | S_IWUSR, mem_used_max_show,
+		mem_used_max_store);
 static DEVICE_ATTR(max_comp_streams, S_IRUGO | S_IWUSR,
 		max_comp_streams_show, max_comp_streams_store);
 static DEVICE_ATTR(comp_algorithm, S_IRUGO | S_IWUSR,
@@ -881,6 +985,8 @@ static struct attribute *zram_disk_attrs[] = {
 	&dev_attr_orig_data_size.attr,
 	&dev_attr_compr_data_size.attr,
 	&dev_attr_mem_used_total.attr,
+	&dev_attr_mem_limit.attr,
+	&dev_attr_mem_used_max.attr,
 	&dev_attr_max_comp_streams.attr,
 	&dev_attr_comp_algorithm.attr,
 	NULL,
@@ -925,6 +1031,7 @@ static int create_device(struct zram *zram, int device_id)
 	set_capacity(zram->disk, 0);
 	/* zram devices sort of resembles non-rotational disks */
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, zram->disk->queue);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, zram->disk->queue);
 	/*
 	 * To ensure that we always get PAGE_SIZE aligned
 	 * and n*PAGE_SIZED sized I/O requests.
diff --git a/drivers/block/zram/zram_drv.h b/drivers/block/zram/zram_drv.h
index 5b0afde729cd..c6ee271317f5 100644
--- a/drivers/block/zram/zram_drv.h
+++ b/drivers/block/zram/zram_drv.h
@@ -84,12 +84,13 @@ struct zram_stats {
 	atomic64_t compr_data_size;	/* compressed size of pages stored */
 	atomic64_t num_reads;	/* failed + successful */
 	atomic64_t num_writes;	/* --do-- */
-	atomic64_t failed_reads;	/* should NEVER! happen */
+	atomic64_t failed_reads;	/* can happen when memory is too low */
 	atomic64_t failed_writes;	/* can happen when memory is too low */
 	atomic64_t invalid_io;	/* non-page-aligned I/O requests */
 	atomic64_t notify_free;	/* no. of swap slot free notifications */
 	atomic64_t zero_pages;		/* no. of zero filled pages */
 	atomic64_t pages_stored;	/* no. of pages currently stored */
+	atomic_long_t max_used_pages;	/* no. of maximum pages stored */
 };
 
 struct zram_meta {
@@ -112,6 +113,11 @@ struct zram {
 	u64 disksize;	/* bytes */
 	int max_comp_streams;
 	struct zram_stats stats;
+	/*
+	 * the number of pages zram can consume for storing compressed data
+	 */
+	unsigned long limit_pages;
+
 	char compressor[10];
 };
 #endif
diff --git a/drivers/bluetooth/Kconfig b/drivers/bluetooth/Kconfig
index fa7fd62ddffa..4547dc238fc7 100644
--- a/drivers/bluetooth/Kconfig
+++ b/drivers/bluetooth/Kconfig
@@ -201,7 +201,7 @@ config BT_MRVL
 	  The core driver to support Marvell Bluetooth devices.
 
 	  This driver is required if you want to support
-	  Marvell Bluetooth devices, such as 8688/8787/8797/8897.
+	  Marvell Bluetooth devices, such as 8688/8787/8797/8887/8897.
 
 	  Say Y here to compile Marvell Bluetooth driver
 	  into the kernel or say M to compile it as module.
@@ -214,7 +214,7 @@ config BT_MRVL_SDIO
 	  The driver for Marvell Bluetooth chipsets with SDIO interface.
 
 	  This driver is required if you want to use Marvell Bluetooth
-	  devices with SDIO interface. Currently SD8688/SD8787/SD8797/SD8897
+	  devices with SDIO interface. Currently SD8688/SD8787/SD8797/SD8887/SD8897
 	  chipsets are supported.
 
 	  Say Y here to compile support for Marvell BT-over-SDIO driver
diff --git a/drivers/bluetooth/ath3k.c b/drivers/bluetooth/ath3k.c
index a0d7355ef127..d85ced27ebd5 100644
--- a/drivers/bluetooth/ath3k.c
+++ b/drivers/bluetooth/ath3k.c
@@ -88,6 +88,7 @@ static const struct usb_device_id ath3k_table[] = {
 	{ USB_DEVICE(0x04CA, 0x300b) },
 	{ USB_DEVICE(0x0930, 0x0219) },
 	{ USB_DEVICE(0x0930, 0x0220) },
+	{ USB_DEVICE(0x0930, 0x0227) },
 	{ USB_DEVICE(0x0b05, 0x17d0) },
 	{ USB_DEVICE(0x0CF3, 0x0036) },
 	{ USB_DEVICE(0x0CF3, 0x3004) },
@@ -138,6 +139,7 @@ static const struct usb_device_id ath3k_blist_tbl[] = {
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0220), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0930, 0x0227), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0b05, 0x17d0), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0CF3, 0x0036), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
diff --git a/drivers/bluetooth/bluecard_cs.c b/drivers/bluetooth/bluecard_cs.c
index dfa5043e68ba..35e63aaa6f80 100644
--- a/drivers/bluetooth/bluecard_cs.c
+++ b/drivers/bluetooth/bluecard_cs.c
@@ -61,7 +61,7 @@ MODULE_LICENSE("GPL");
 /* ======================== Local structures ======================== */
 
 
-typedef struct bluecard_info_t {
+struct bluecard_info {
 	struct pcmcia_device *p_dev;
 
 	struct hci_dev *hdev;
@@ -78,7 +78,7 @@ typedef struct bluecard_info_t {
 
 	unsigned char ctrl_reg;
 	unsigned long hw_state;		/* Status of the hardware and LED control */
-} bluecard_info_t;
+};
 
 
 static int bluecard_config(struct pcmcia_device *link);
@@ -157,7 +157,7 @@ static void bluecard_detach(struct pcmcia_device *p_dev);
 
 static void bluecard_activity_led_timeout(u_long arg)
 {
-	bluecard_info_t *info = (bluecard_info_t *)arg;
+	struct bluecard_info *info = (struct bluecard_info *)arg;
 	unsigned int iobase = info->p_dev->resource[0]->start;
 
 	if (!test_bit(CARD_HAS_PCCARD_ID, &(info->hw_state)))
@@ -173,7 +173,7 @@ static void bluecard_activity_led_timeout(u_long arg)
 }
 
 
-static void bluecard_enable_activity_led(bluecard_info_t *info)
+static void bluecard_enable_activity_led(struct bluecard_info *info)
 {
 	unsigned int iobase = info->p_dev->resource[0]->start;
 
@@ -215,7 +215,7 @@ static int bluecard_write(unsigned int iobase, unsigned int offset, __u8 *buf, i
 }
 
 
-static void bluecard_write_wakeup(bluecard_info_t *info)
+static void bluecard_write_wakeup(struct bluecard_info *info)
 {
 	if (!info) {
 		BT_ERR("Unknown device");
@@ -368,7 +368,8 @@ static int bluecard_read(unsigned int iobase, unsigned int offset, __u8 *buf, in
 }
 
 
-static void bluecard_receive(bluecard_info_t *info, unsigned int offset)
+static void bluecard_receive(struct bluecard_info *info,
+			     unsigned int offset)
 {
 	unsigned int iobase;
 	unsigned char buf[31];
@@ -497,7 +498,7 @@ static void bluecard_receive(bluecard_info_t *info, unsigned int offset)
 
 static irqreturn_t bluecard_interrupt(int irq, void *dev_inst)
 {
-	bluecard_info_t *info = dev_inst;
+	struct bluecard_info *info = dev_inst;
 	unsigned int iobase;
 	unsigned char reg;
 
@@ -562,7 +563,7 @@ static irqreturn_t bluecard_interrupt(int irq, void *dev_inst)
 
 static int bluecard_hci_set_baud_rate(struct hci_dev *hdev, int baud)
 {
-	bluecard_info_t *info = hci_get_drvdata(hdev);
+	struct bluecard_info *info = hci_get_drvdata(hdev);
 	struct sk_buff *skb;
 
 	/* Ericsson baud rate command */
@@ -611,7 +612,7 @@ static int bluecard_hci_set_baud_rate(struct hci_dev *hdev, int baud)
 
 static int bluecard_hci_flush(struct hci_dev *hdev)
 {
-	bluecard_info_t *info = hci_get_drvdata(hdev);
+	struct bluecard_info *info = hci_get_drvdata(hdev);
 
 	/* Drop TX queue */
 	skb_queue_purge(&(info->txq));
@@ -622,7 +623,7 @@ static int bluecard_hci_flush(struct hci_dev *hdev)
 
 static int bluecard_hci_open(struct hci_dev *hdev)
 {
-	bluecard_info_t *info = hci_get_drvdata(hdev);
+	struct bluecard_info *info = hci_get_drvdata(hdev);
 
 	if (test_bit(CARD_HAS_PCCARD_ID, &(info->hw_state)))
 		bluecard_hci_set_baud_rate(hdev, DEFAULT_BAUD_RATE);
@@ -643,7 +644,7 @@ static int bluecard_hci_open(struct hci_dev *hdev)
 
 static int bluecard_hci_close(struct hci_dev *hdev)
 {
-	bluecard_info_t *info = hci_get_drvdata(hdev);
+	struct bluecard_info *info = hci_get_drvdata(hdev);
 
 	if (!test_and_clear_bit(HCI_RUNNING, &(hdev->flags)))
 		return 0;
@@ -663,7 +664,7 @@ static int bluecard_hci_close(struct hci_dev *hdev)
 
 static int bluecard_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-	bluecard_info_t *info = hci_get_drvdata(hdev);
+	struct bluecard_info *info = hci_get_drvdata(hdev);
 
 	switch (bt_cb(skb)->pkt_type) {
 	case HCI_COMMAND_PKT:
@@ -691,7 +692,7 @@ static int bluecard_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 /* ======================== Card services HCI interaction ======================== */
 
 
-static int bluecard_open(bluecard_info_t *info)
+static int bluecard_open(struct bluecard_info *info)
 {
 	unsigned int iobase = info->p_dev->resource[0]->start;
 	struct hci_dev *hdev;
@@ -806,7 +807,7 @@ static int bluecard_open(bluecard_info_t *info)
 }
 
 
-static int bluecard_close(bluecard_info_t *info)
+static int bluecard_close(struct bluecard_info *info)
 {
 	unsigned int iobase = info->p_dev->resource[0]->start;
 	struct hci_dev *hdev = info->hdev;
@@ -833,7 +834,7 @@ static int bluecard_close(bluecard_info_t *info)
 
 static int bluecard_probe(struct pcmcia_device *link)
 {
-	bluecard_info_t *info;
+	struct bluecard_info *info;
 
 	/* Create new info device */
 	info = devm_kzalloc(&link->dev, sizeof(*info), GFP_KERNEL);
@@ -857,7 +858,7 @@ static void bluecard_detach(struct pcmcia_device *link)
 
 static int bluecard_config(struct pcmcia_device *link)
 {
-	bluecard_info_t *info = link->priv;
+	struct bluecard_info *info = link->priv;
 	int i, n;
 
 	link->config_index = 0x20;
@@ -897,7 +898,7 @@ failed:
 
 static void bluecard_release(struct pcmcia_device *link)
 {
-	bluecard_info_t *info = link->priv;
+	struct bluecard_info *info = link->priv;
 
 	bluecard_close(info);
 
diff --git a/drivers/bluetooth/bt3c_cs.c b/drivers/bluetooth/bt3c_cs.c
index 1d82721cf9c6..4f7e8d400bc0 100644
--- a/drivers/bluetooth/bt3c_cs.c
+++ b/drivers/bluetooth/bt3c_cs.c
@@ -67,7 +67,7 @@ MODULE_FIRMWARE("BT3CPCC.bin");
 /* ======================== Local structures ======================== */
 
 
-typedef struct bt3c_info_t {
+struct bt3c_info {
 	struct pcmcia_device *p_dev;
 
 	struct hci_dev *hdev;
@@ -80,7 +80,7 @@ typedef struct bt3c_info_t {
 	unsigned long rx_state;
 	unsigned long rx_count;
 	struct sk_buff *rx_skb;
-} bt3c_info_t;
+};
 
 
 static int bt3c_config(struct pcmcia_device *link);
@@ -175,7 +175,7 @@ static int bt3c_write(unsigned int iobase, int fifo_size, __u8 *buf, int len)
 }
 
 
-static void bt3c_write_wakeup(bt3c_info_t *info)
+static void bt3c_write_wakeup(struct bt3c_info *info)
 {
 	if (!info) {
 		BT_ERR("Unknown device");
@@ -214,7 +214,7 @@ static void bt3c_write_wakeup(bt3c_info_t *info)
 }
 
 
-static void bt3c_receive(bt3c_info_t *info)
+static void bt3c_receive(struct bt3c_info *info)
 {
 	unsigned int iobase;
 	int size = 0, avail;
@@ -336,7 +336,7 @@ static void bt3c_receive(bt3c_info_t *info)
 
 static irqreturn_t bt3c_interrupt(int irq, void *dev_inst)
 {
-	bt3c_info_t *info = dev_inst;
+	struct bt3c_info *info = dev_inst;
 	unsigned int iobase;
 	int iir;
 	irqreturn_t r = IRQ_NONE;
@@ -388,7 +388,7 @@ static irqreturn_t bt3c_interrupt(int irq, void *dev_inst)
 
 static int bt3c_hci_flush(struct hci_dev *hdev)
 {
-	bt3c_info_t *info = hci_get_drvdata(hdev);
+	struct bt3c_info *info = hci_get_drvdata(hdev);
 
 	/* Drop TX queue */
 	skb_queue_purge(&(info->txq));
@@ -418,7 +418,7 @@ static int bt3c_hci_close(struct hci_dev *hdev)
 
 static int bt3c_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-	bt3c_info_t *info = hci_get_drvdata(hdev);
+	struct bt3c_info *info = hci_get_drvdata(hdev);
 	unsigned long flags;
 
 	switch (bt_cb(skb)->pkt_type) {
@@ -451,7 +451,8 @@ static int bt3c_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 /* ======================== Card services HCI interaction ======================== */
 
 
-static int bt3c_load_firmware(bt3c_info_t *info, const unsigned char *firmware,
+static int bt3c_load_firmware(struct bt3c_info *info,
+			      const unsigned char *firmware,
 			      int count)
 {
 	char *ptr = (char *) firmware;
@@ -536,7 +537,7 @@ error:
 }
 
 
-static int bt3c_open(bt3c_info_t *info)
+static int bt3c_open(struct bt3c_info *info)
 {
 	const struct firmware *firmware;
 	struct hci_dev *hdev;
@@ -603,7 +604,7 @@ error:
 }
 
 
-static int bt3c_close(bt3c_info_t *info)
+static int bt3c_close(struct bt3c_info *info)
 {
 	struct hci_dev *hdev = info->hdev;
 
@@ -620,7 +621,7 @@ static int bt3c_close(bt3c_info_t *info)
 
 static int bt3c_probe(struct pcmcia_device *link)
 {
-	bt3c_info_t *info;
+	struct bt3c_info *info;
 
 	/* Create new info device */
 	info = devm_kzalloc(&link->dev, sizeof(*info), GFP_KERNEL);
@@ -683,7 +684,7 @@ static int bt3c_check_config_notpicky(struct pcmcia_device *p_dev,
 
 static int bt3c_config(struct pcmcia_device *link)
 {
-	bt3c_info_t *info = link->priv;
+	struct bt3c_info *info = link->priv;
 	int i;
 	unsigned long try;
 
@@ -724,7 +725,7 @@ failed:
 
 static void bt3c_release(struct pcmcia_device *link)
 {
-	bt3c_info_t *info = link->priv;
+	struct bt3c_info *info = link->priv;
 
 	bt3c_close(info);
 
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 3e683b153259..550bce089fa6 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -84,7 +84,27 @@ static const struct btmrvl_sdio_card_reg btmrvl_reg_87xx = {
 	.int_read_to_clear = false,
 };
 
-static const struct btmrvl_sdio_card_reg btmrvl_reg_88xx = {
+static const struct btmrvl_sdio_card_reg btmrvl_reg_8887 = {
+	.cfg = 0x00,
+	.host_int_mask = 0x08,
+	.host_intstatus = 0x0C,
+	.card_status = 0x5C,
+	.sq_read_base_addr_a0 = 0x6C,
+	.sq_read_base_addr_a1 = 0x6D,
+	.card_revision = 0xC8,
+	.card_fw_status0 = 0x88,
+	.card_fw_status1 = 0x89,
+	.card_rx_len = 0x8A,
+	.card_rx_unit = 0x8B,
+	.io_port_0 = 0xE4,
+	.io_port_1 = 0xE5,
+	.io_port_2 = 0xE6,
+	.int_read_to_clear = true,
+	.host_int_rsr = 0x04,
+	.card_misc_cfg = 0xD8,
+};
+
+static const struct btmrvl_sdio_card_reg btmrvl_reg_8897 = {
 	.cfg = 0x00,
 	.host_int_mask = 0x02,
 	.host_intstatus = 0x03,
@@ -128,10 +148,18 @@ static const struct btmrvl_sdio_device btmrvl_sdio_sd8797 = {
 	.sd_blksz_fw_dl	= 256,
 };
 
+static const struct btmrvl_sdio_device btmrvl_sdio_sd8887 = {
+	.helper		= NULL,
+	.firmware	= "mrvl/sd8887_uapsta.bin",
+	.reg		= &btmrvl_reg_8887,
+	.support_pscan_win_report = true,
+	.sd_blksz_fw_dl	= 256,
+};
+
 static const struct btmrvl_sdio_device btmrvl_sdio_sd8897 = {
 	.helper		= NULL,
 	.firmware	= "mrvl/sd8897_uapsta.bin",
-	.reg		= &btmrvl_reg_88xx,
+	.reg		= &btmrvl_reg_8897,
 	.support_pscan_win_report = true,
 	.sd_blksz_fw_dl	= 256,
 };
@@ -149,6 +177,9 @@ static const struct sdio_device_id btmrvl_sdio_ids[] = {
 	/* Marvell SD8797 Bluetooth device */
 	{ SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x912A),
 			.driver_data = (unsigned long) &btmrvl_sdio_sd8797 },
+	/* Marvell SD8887 Bluetooth device */
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x9136),
+			.driver_data = (unsigned long)&btmrvl_sdio_sd8887 },
 	/* Marvell SD8897 Bluetooth device */
 	{ SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, 0x912E),
 			.driver_data = (unsigned long) &btmrvl_sdio_sd8897 },
@@ -1280,4 +1311,5 @@ MODULE_FIRMWARE("mrvl/sd8688_helper.bin");
 MODULE_FIRMWARE("mrvl/sd8688.bin");
 MODULE_FIRMWARE("mrvl/sd8787_uapsta.bin");
 MODULE_FIRMWARE("mrvl/sd8797_uapsta.bin");
+MODULE_FIRMWARE("mrvl/sd8887_uapsta.bin");
 MODULE_FIRMWARE("mrvl/sd8897_uapsta.bin");
diff --git a/drivers/bluetooth/btuart_cs.c b/drivers/bluetooth/btuart_cs.c
index fb948f02eda5..abb4d2106db4 100644
--- a/drivers/bluetooth/btuart_cs.c
+++ b/drivers/bluetooth/btuart_cs.c
@@ -62,7 +62,7 @@ MODULE_LICENSE("GPL");
 /* ======================== Local structures ======================== */
 
 
-typedef struct btuart_info_t {
+struct btuart_info {
 	struct pcmcia_device *p_dev;
 
 	struct hci_dev *hdev;
@@ -75,7 +75,7 @@ typedef struct btuart_info_t {
 	unsigned long rx_state;
 	unsigned long rx_count;
 	struct sk_buff *rx_skb;
-} btuart_info_t;
+};
 
 
 static int btuart_config(struct pcmcia_device *link);
@@ -127,7 +127,7 @@ static int btuart_write(unsigned int iobase, int fifo_size, __u8 *buf, int len)
 }
 
 
-static void btuart_write_wakeup(btuart_info_t *info)
+static void btuart_write_wakeup(struct btuart_info *info)
 {
 	if (!info) {
 		BT_ERR("Unknown device");
@@ -172,7 +172,7 @@ static void btuart_write_wakeup(btuart_info_t *info)
 }
 
 
-static void btuart_receive(btuart_info_t *info)
+static void btuart_receive(struct btuart_info *info)
 {
 	unsigned int iobase;
 	int boguscount = 0;
@@ -286,7 +286,7 @@ static void btuart_receive(btuart_info_t *info)
 
 static irqreturn_t btuart_interrupt(int irq, void *dev_inst)
 {
-	btuart_info_t *info = dev_inst;
+	struct btuart_info *info = dev_inst;
 	unsigned int iobase;
 	int boguscount = 0;
 	int iir, lsr;
@@ -340,7 +340,8 @@ static irqreturn_t btuart_interrupt(int irq, void *dev_inst)
 }
 
 
-static void btuart_change_speed(btuart_info_t *info, unsigned int speed)
+static void btuart_change_speed(struct btuart_info *info,
+				unsigned int speed)
 {
 	unsigned long flags;
 	unsigned int iobase;
@@ -397,7 +398,7 @@ static void btuart_change_speed(btuart_info_t *info, unsigned int speed)
 
 static int btuart_hci_flush(struct hci_dev *hdev)
 {
-	btuart_info_t *info = hci_get_drvdata(hdev);
+	struct btuart_info *info = hci_get_drvdata(hdev);
 
 	/* Drop TX queue */
 	skb_queue_purge(&(info->txq));
@@ -427,7 +428,7 @@ static int btuart_hci_close(struct hci_dev *hdev)
 
 static int btuart_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-	btuart_info_t *info = hci_get_drvdata(hdev);
+	struct btuart_info *info = hci_get_drvdata(hdev);
 
 	switch (bt_cb(skb)->pkt_type) {
 	case HCI_COMMAND_PKT:
@@ -455,7 +456,7 @@ static int btuart_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 /* ======================== Card services HCI interaction ======================== */
 
 
-static int btuart_open(btuart_info_t *info)
+static int btuart_open(struct btuart_info *info)
 {
 	unsigned long flags;
 	unsigned int iobase = info->p_dev->resource[0]->start;
@@ -521,7 +522,7 @@ static int btuart_open(btuart_info_t *info)
 }
 
 
-static int btuart_close(btuart_info_t *info)
+static int btuart_close(struct btuart_info *info)
 {
 	unsigned long flags;
 	unsigned int iobase = info->p_dev->resource[0]->start;
@@ -550,7 +551,7 @@ static int btuart_close(btuart_info_t *info)
 
 static int btuart_probe(struct pcmcia_device *link)
 {
-	btuart_info_t *info;
+	struct btuart_info *info;
 
 	/* Create new info device */
 	info = devm_kzalloc(&link->dev, sizeof(*info), GFP_KERNEL);
@@ -613,7 +614,7 @@ static int btuart_check_config_notpicky(struct pcmcia_device *p_dev,
 
 static int btuart_config(struct pcmcia_device *link)
 {
-	btuart_info_t *info = link->priv;
+	struct btuart_info *info = link->priv;
 	int i;
 	int try;
 
@@ -654,7 +655,7 @@ failed:
 
 static void btuart_release(struct pcmcia_device *link)
 {
-	btuart_info_t *info = link->priv;
+	struct btuart_info *info = link->priv;
 
 	btuart_close(info);
 
diff --git a/drivers/bluetooth/btusb.c b/drivers/bluetooth/btusb.c
index 292c38e8aa17..edfc17bfcd44 100644
--- a/drivers/bluetooth/btusb.c
+++ b/drivers/bluetooth/btusb.c
@@ -165,6 +165,7 @@ static const struct usb_device_id blacklist_table[] = {
 	{ USB_DEVICE(0x04ca, 0x300b), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0219), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0930, 0x0220), .driver_info = BTUSB_ATH3012 },
+	{ USB_DEVICE(0x0930, 0x0227), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0b05, 0x17d0), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x0036), .driver_info = BTUSB_ATH3012 },
 	{ USB_DEVICE(0x0cf3, 0x3004), .driver_info = BTUSB_ATH3012 },
@@ -267,20 +268,24 @@ struct btusb_data {
 	struct usb_interface *intf;
 	struct usb_interface *isoc;
 
-	spinlock_t lock;
-
 	unsigned long flags;
 
 	struct work_struct work;
 	struct work_struct waker;
 
+	struct usb_anchor deferred;
 	struct usb_anchor tx_anchor;
+	int tx_in_flight;
+	spinlock_t txlock;
+
 	struct usb_anchor intr_anchor;
 	struct usb_anchor bulk_anchor;
 	struct usb_anchor isoc_anchor;
-	struct usb_anchor deferred;
-	int tx_in_flight;
-	spinlock_t txlock;
+	spinlock_t rxlock;
+
+	struct sk_buff *evt_skb;
+	struct sk_buff *acl_skb;
+	struct sk_buff *sco_skb;
 
 	struct usb_endpoint_descriptor *intr_ep;
 	struct usb_endpoint_descriptor *bulk_tx_ep;
@@ -295,18 +300,189 @@ struct btusb_data {
 	int suspend_count;
 };
 
-static int inc_tx(struct btusb_data *data)
+static inline void btusb_free_frags(struct btusb_data *data)
 {
 	unsigned long flags;
-	int rv;
 
-	spin_lock_irqsave(&data->txlock, flags);
-	rv = test_bit(BTUSB_SUSPENDING, &data->flags);
-	if (!rv)
-		data->tx_in_flight++;
-	spin_unlock_irqrestore(&data->txlock, flags);
+	spin_lock_irqsave(&data->rxlock, flags);
+
+	kfree_skb(data->evt_skb);
+	data->evt_skb = NULL;
+
+	kfree_skb(data->acl_skb);
+	data->acl_skb = NULL;
+
+	kfree_skb(data->sco_skb);
+	data->sco_skb = NULL;
+
+	spin_unlock_irqrestore(&data->rxlock, flags);
+}
+
+static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count)
+{
+	struct sk_buff *skb;
+	int err = 0;
+
+	spin_lock(&data->rxlock);
+	skb = data->evt_skb;
+
+	while (count) {
+		int len;
+
+		if (!skb) {
+			skb = bt_skb_alloc(HCI_MAX_EVENT_SIZE, GFP_ATOMIC);
+			if (!skb) {
+				err = -ENOMEM;
+				break;
+			}
+
+			bt_cb(skb)->pkt_type = HCI_EVENT_PKT;
+			bt_cb(skb)->expect = HCI_EVENT_HDR_SIZE;
+		}
+
+		len = min_t(uint, bt_cb(skb)->expect, count);
+		memcpy(skb_put(skb, len), buffer, len);
+
+		count -= len;
+		buffer += len;
+		bt_cb(skb)->expect -= len;
+
+		if (skb->len == HCI_EVENT_HDR_SIZE) {
+			/* Complete event header */
+			bt_cb(skb)->expect = hci_event_hdr(skb)->plen;
+
+			if (skb_tailroom(skb) < bt_cb(skb)->expect) {
+				kfree_skb(skb);
+				skb = NULL;
+
+				err = -EILSEQ;
+				break;
+			}
+		}
+
+		if (bt_cb(skb)->expect == 0) {
+			/* Complete frame */
+			hci_recv_frame(data->hdev, skb);
+			skb = NULL;
+		}
+	}
+
+	data->evt_skb = skb;
+	spin_unlock(&data->rxlock);
 
-	return rv;
+	return err;
+}
+
+static int btusb_recv_bulk(struct btusb_data *data, void *buffer, int count)
+{
+	struct sk_buff *skb;
+	int err = 0;
+
+	spin_lock(&data->rxlock);
+	skb = data->acl_skb;
+
+	while (count) {
+		int len;
+
+		if (!skb) {
+			skb = bt_skb_alloc(HCI_MAX_FRAME_SIZE, GFP_ATOMIC);
+			if (!skb) {
+				err = -ENOMEM;
+				break;
+			}
+
+			bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT;
+			bt_cb(skb)->expect = HCI_ACL_HDR_SIZE;
+		}
+
+		len = min_t(uint, bt_cb(skb)->expect, count);
+		memcpy(skb_put(skb, len), buffer, len);
+
+		count -= len;
+		buffer += len;
+		bt_cb(skb)->expect -= len;
+
+		if (skb->len == HCI_ACL_HDR_SIZE) {
+			__le16 dlen = hci_acl_hdr(skb)->dlen;
+
+			/* Complete ACL header */
+			bt_cb(skb)->expect = __le16_to_cpu(dlen);
+
+			if (skb_tailroom(skb) < bt_cb(skb)->expect) {
+				kfree_skb(skb);
+				skb = NULL;
+
+				err = -EILSEQ;
+				break;
+			}
+		}
+
+		if (bt_cb(skb)->expect == 0) {
+			/* Complete frame */
+			hci_recv_frame(data->hdev, skb);
+			skb = NULL;
+		}
+	}
+
+	data->acl_skb = skb;
+	spin_unlock(&data->rxlock);
+
+	return err;
+}
+
+static int btusb_recv_isoc(struct btusb_data *data, void *buffer, int count)
+{
+	struct sk_buff *skb;
+	int err = 0;
+
+	spin_lock(&data->rxlock);
+	skb = data->sco_skb;
+
+	while (count) {
+		int len;
+
+		if (!skb) {
+			skb = bt_skb_alloc(HCI_MAX_SCO_SIZE, GFP_ATOMIC);
+			if (!skb) {
+				err = -ENOMEM;
+				break;
+			}
+
+			bt_cb(skb)->pkt_type = HCI_SCODATA_PKT;
+			bt_cb(skb)->expect = HCI_SCO_HDR_SIZE;
+		}
+
+		len = min_t(uint, bt_cb(skb)->expect, count);
+		memcpy(skb_put(skb, len), buffer, len);
+
+		count -= len;
+		buffer += len;
+		bt_cb(skb)->expect -= len;
+
+		if (skb->len == HCI_SCO_HDR_SIZE) {
+			/* Complete SCO header */
+			bt_cb(skb)->expect = hci_sco_hdr(skb)->dlen;
+
+			if (skb_tailroom(skb) < bt_cb(skb)->expect) {
+				kfree_skb(skb);
+				skb = NULL;
+
+				err = -EILSEQ;
+				break;
+			}
+		}
+
+		if (bt_cb(skb)->expect == 0) {
+			/* Complete frame */
+			hci_recv_frame(data->hdev, skb);
+			skb = NULL;
+		}
+	}
+
+	data->sco_skb = skb;
+	spin_unlock(&data->rxlock);
+
+	return err;
 }
 
 static void btusb_intr_complete(struct urb *urb)
@@ -315,8 +491,8 @@ static void btusb_intr_complete(struct urb *urb)
 	struct btusb_data *data = hci_get_drvdata(hdev);
 	int err;
 
-	BT_DBG("%s urb %p status %d count %d", hdev->name,
-					urb, urb->status, urb->actual_length);
+	BT_DBG("%s urb %p status %d count %d", hdev->name, urb, urb->status,
+	       urb->actual_length);
 
 	if (!test_bit(HCI_RUNNING, &hdev->flags))
 		return;
@@ -324,12 +500,14 @@ static void btusb_intr_complete(struct urb *urb)
 	if (urb->status == 0) {
 		hdev->stat.byte_rx += urb->actual_length;
 
-		if (hci_recv_fragment(hdev, HCI_EVENT_PKT,
-						urb->transfer_buffer,
-						urb->actual_length) < 0) {
+		if (btusb_recv_intr(data, urb->transfer_buffer,
+				    urb->actual_length) < 0) {
 			BT_ERR("%s corrupted event packet", hdev->name);
 			hdev->stat.err_rx++;
 		}
+	} else if (urb->status == -ENOENT) {
+		/* Avoid suspend failed when usb_kill_urb */
+		return;
 	}
 
 	if (!test_bit(BTUSB_INTR_RUNNING, &data->flags))
@@ -344,7 +522,7 @@ static void btusb_intr_complete(struct urb *urb)
 		 * -ENODEV: device got disconnected */
 		if (err != -EPERM && err != -ENODEV)
 			BT_ERR("%s urb %p failed to resubmit (%d)",
-						hdev->name, urb, -err);
+			       hdev->name, urb, -err);
 		usb_unanchor_urb(urb);
 	}
 }
@@ -377,8 +555,7 @@ static int btusb_submit_intr_urb(struct hci_dev *hdev, gfp_t mem_flags)
 	pipe = usb_rcvintpipe(data->udev, data->intr_ep->bEndpointAddress);
 
 	usb_fill_int_urb(urb, data->udev, pipe, buf, size,
-						btusb_intr_complete, hdev,
-						data->intr_ep->bInterval);
+			 btusb_intr_complete, hdev, data->intr_ep->bInterval);
 
 	urb->transfer_flags |= URB_FREE_BUFFER;
 
@@ -388,7 +565,7 @@ static int btusb_submit_intr_urb(struct hci_dev *hdev, gfp_t mem_flags)
 	if (err < 0) {
 		if (err != -EPERM && err != -ENODEV)
 			BT_ERR("%s urb %p submission failed (%d)",
-						hdev->name, urb, -err);
+			       hdev->name, urb, -err);
 		usb_unanchor_urb(urb);
 	}
 
@@ -403,8 +580,8 @@ static void btusb_bulk_complete(struct urb *urb)
 	struct btusb_data *data = hci_get_drvdata(hdev);
 	int err;
 
-	BT_DBG("%s urb %p status %d count %d", hdev->name,
-					urb, urb->status, urb->actual_length);
+	BT_DBG("%s urb %p status %d count %d", hdev->name, urb, urb->status,
+	       urb->actual_length);
 
 	if (!test_bit(HCI_RUNNING, &hdev->flags))
 		return;
@@ -412,12 +589,14 @@ static void btusb_bulk_complete(struct urb *urb)
 	if (urb->status == 0) {
 		hdev->stat.byte_rx += urb->actual_length;
 
-		if (hci_recv_fragment(hdev, HCI_ACLDATA_PKT,
-						urb->transfer_buffer,
-						urb->actual_length) < 0) {
+		if (btusb_recv_bulk(data, urb->transfer_buffer,
+				    urb->actual_length) < 0) {
 			BT_ERR("%s corrupted ACL packet", hdev->name);
 			hdev->stat.err_rx++;
 		}
+	} else if (urb->status == -ENOENT) {
+		/* Avoid suspend failed when usb_kill_urb */
+		return;
 	}
 
 	if (!test_bit(BTUSB_BULK_RUNNING, &data->flags))
@@ -432,7 +611,7 @@ static void btusb_bulk_complete(struct urb *urb)
 		 * -ENODEV: device got disconnected */
 		if (err != -EPERM && err != -ENODEV)
 			BT_ERR("%s urb %p failed to resubmit (%d)",
-						hdev->name, urb, -err);
+			       hdev->name, urb, -err);
 		usb_unanchor_urb(urb);
 	}
 }
@@ -462,8 +641,8 @@ static int btusb_submit_bulk_urb(struct hci_dev *hdev, gfp_t mem_flags)
 
 	pipe = usb_rcvbulkpipe(data->udev, data->bulk_rx_ep->bEndpointAddress);
 
-	usb_fill_bulk_urb(urb, data->udev, pipe,
-					buf, size, btusb_bulk_complete, hdev);
+	usb_fill_bulk_urb(urb, data->udev, pipe, buf, size,
+			  btusb_bulk_complete, hdev);
 
 	urb->transfer_flags |= URB_FREE_BUFFER;
 
@@ -474,7 +653,7 @@ static int btusb_submit_bulk_urb(struct hci_dev *hdev, gfp_t mem_flags)
 	if (err < 0) {
 		if (err != -EPERM && err != -ENODEV)
 			BT_ERR("%s urb %p submission failed (%d)",
-						hdev->name, urb, -err);
+			       hdev->name, urb, -err);
 		usb_unanchor_urb(urb);
 	}
 
@@ -489,8 +668,8 @@ static void btusb_isoc_complete(struct urb *urb)
 	struct btusb_data *data = hci_get_drvdata(hdev);
 	int i, err;
 
-	BT_DBG("%s urb %p status %d count %d", hdev->name,
-					urb, urb->status, urb->actual_length);
+	BT_DBG("%s urb %p status %d count %d", hdev->name, urb, urb->status,
+	       urb->actual_length);
 
 	if (!test_bit(HCI_RUNNING, &hdev->flags))
 		return;
@@ -505,13 +684,15 @@ static void btusb_isoc_complete(struct urb *urb)
 
 			hdev->stat.byte_rx += length;
 
-			if (hci_recv_fragment(hdev, HCI_SCODATA_PKT,
-						urb->transfer_buffer + offset,
-								length) < 0) {
+			if (btusb_recv_isoc(data, urb->transfer_buffer + offset,
+					    length) < 0) {
 				BT_ERR("%s corrupted SCO packet", hdev->name);
 				hdev->stat.err_rx++;
 			}
 		}
+	} else if (urb->status == -ENOENT) {
+		/* Avoid suspend failed when usb_kill_urb */
+		return;
 	}
 
 	if (!test_bit(BTUSB_ISOC_RUNNING, &data->flags))
@@ -525,7 +706,7 @@ static void btusb_isoc_complete(struct urb *urb)
 		 * -ENODEV: device got disconnected */
 		if (err != -EPERM && err != -ENODEV)
 			BT_ERR("%s urb %p failed to resubmit (%d)",
-						hdev->name, urb, -err);
+			       hdev->name, urb, -err);
 		usb_unanchor_urb(urb);
 	}
 }
@@ -580,12 +761,12 @@ static int btusb_submit_isoc_urb(struct hci_dev *hdev, gfp_t mem_flags)
 	pipe = usb_rcvisocpipe(data->udev, data->isoc_rx_ep->bEndpointAddress);
 
 	usb_fill_int_urb(urb, data->udev, pipe, buf, size, btusb_isoc_complete,
-				hdev, data->isoc_rx_ep->bInterval);
+			 hdev, data->isoc_rx_ep->bInterval);
 
-	urb->transfer_flags  = URB_FREE_BUFFER | URB_ISO_ASAP;
+	urb->transfer_flags = URB_FREE_BUFFER | URB_ISO_ASAP;
 
 	__fill_isoc_descriptor(urb, size,
-			le16_to_cpu(data->isoc_rx_ep->wMaxPacketSize));
+			       le16_to_cpu(data->isoc_rx_ep->wMaxPacketSize));
 
 	usb_anchor_urb(urb, &data->isoc_anchor);
 
@@ -593,7 +774,7 @@ static int btusb_submit_isoc_urb(struct hci_dev *hdev, gfp_t mem_flags)
 	if (err < 0) {
 		if (err != -EPERM && err != -ENODEV)
 			BT_ERR("%s urb %p submission failed (%d)",
-						hdev->name, urb, -err);
+			       hdev->name, urb, -err);
 		usb_unanchor_urb(urb);
 	}
 
@@ -605,11 +786,11 @@ static int btusb_submit_isoc_urb(struct hci_dev *hdev, gfp_t mem_flags)
 static void btusb_tx_complete(struct urb *urb)
 {
 	struct sk_buff *skb = urb->context;
-	struct hci_dev *hdev = (struct hci_dev *) skb->dev;
+	struct hci_dev *hdev = (struct hci_dev *)skb->dev;
 	struct btusb_data *data = hci_get_drvdata(hdev);
 
-	BT_DBG("%s urb %p status %d count %d", hdev->name,
-					urb, urb->status, urb->actual_length);
+	BT_DBG("%s urb %p status %d count %d", hdev->name, urb, urb->status,
+	       urb->actual_length);
 
 	if (!test_bit(HCI_RUNNING, &hdev->flags))
 		goto done;
@@ -632,10 +813,10 @@ done:
 static void btusb_isoc_tx_complete(struct urb *urb)
 {
 	struct sk_buff *skb = urb->context;
-	struct hci_dev *hdev = (struct hci_dev *) skb->dev;
+	struct hci_dev *hdev = (struct hci_dev *)skb->dev;
 
-	BT_DBG("%s urb %p status %d count %d", hdev->name,
-					urb, urb->status, urb->actual_length);
+	BT_DBG("%s urb %p status %d count %d", hdev->name, urb, urb->status,
+	       urb->actual_length);
 
 	if (!test_bit(HCI_RUNNING, &hdev->flags))
 		goto done;
@@ -719,6 +900,8 @@ static int btusb_close(struct hci_dev *hdev)
 	clear_bit(BTUSB_INTR_RUNNING, &data->flags);
 
 	btusb_stop_traffic(data);
+	btusb_free_frags(data);
+
 	err = usb_autopm_get_interface(data->intf);
 	if (err < 0)
 		goto failed;
@@ -738,122 +921,181 @@ static int btusb_flush(struct hci_dev *hdev)
 	BT_DBG("%s", hdev->name);
 
 	usb_kill_anchored_urbs(&data->tx_anchor);
+	btusb_free_frags(data);
 
 	return 0;
 }
 
-static int btusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
+static struct urb *alloc_ctrl_urb(struct hci_dev *hdev, struct sk_buff *skb)
 {
 	struct btusb_data *data = hci_get_drvdata(hdev);
 	struct usb_ctrlrequest *dr;
 	struct urb *urb;
 	unsigned int pipe;
-	int err;
 
-	BT_DBG("%s", hdev->name);
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb)
+		return ERR_PTR(-ENOMEM);
 
-	if (!test_bit(HCI_RUNNING, &hdev->flags))
-		return -EBUSY;
+	dr = kmalloc(sizeof(*dr), GFP_KERNEL);
+	if (!dr) {
+		usb_free_urb(urb);
+		return ERR_PTR(-ENOMEM);
+	}
 
-	skb->dev = (void *) hdev;
+	dr->bRequestType = data->cmdreq_type;
+	dr->bRequest     = 0;
+	dr->wIndex       = 0;
+	dr->wValue       = 0;
+	dr->wLength      = __cpu_to_le16(skb->len);
 
-	switch (bt_cb(skb)->pkt_type) {
-	case HCI_COMMAND_PKT:
-		urb = usb_alloc_urb(0, GFP_ATOMIC);
-		if (!urb)
-			return -ENOMEM;
-
-		dr = kmalloc(sizeof(*dr), GFP_ATOMIC);
-		if (!dr) {
-			usb_free_urb(urb);
-			return -ENOMEM;
-		}
+	pipe = usb_sndctrlpipe(data->udev, 0x00);
 
-		dr->bRequestType = data->cmdreq_type;
-		dr->bRequest     = 0;
-		dr->wIndex       = 0;
-		dr->wValue       = 0;
-		dr->wLength      = __cpu_to_le16(skb->len);
+	usb_fill_control_urb(urb, data->udev, pipe, (void *)dr,
+			     skb->data, skb->len, btusb_tx_complete, skb);
 
-		pipe = usb_sndctrlpipe(data->udev, 0x00);
+	skb->dev = (void *)hdev;
 
-		usb_fill_control_urb(urb, data->udev, pipe, (void *) dr,
-				skb->data, skb->len, btusb_tx_complete, skb);
+	return urb;
+}
 
-		hdev->stat.cmd_tx++;
-		break;
+static struct urb *alloc_bulk_urb(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct btusb_data *data = hci_get_drvdata(hdev);
+	struct urb *urb;
+	unsigned int pipe;
 
-	case HCI_ACLDATA_PKT:
-		if (!data->bulk_tx_ep)
-			return -ENODEV;
+	if (!data->bulk_tx_ep)
+		return ERR_PTR(-ENODEV);
+
+	urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (!urb)
+		return ERR_PTR(-ENOMEM);
 
-		urb = usb_alloc_urb(0, GFP_ATOMIC);
-		if (!urb)
-			return -ENOMEM;
+	pipe = usb_sndbulkpipe(data->udev, data->bulk_tx_ep->bEndpointAddress);
 
-		pipe = usb_sndbulkpipe(data->udev,
-					data->bulk_tx_ep->bEndpointAddress);
+	usb_fill_bulk_urb(urb, data->udev, pipe,
+			  skb->data, skb->len, btusb_tx_complete, skb);
 
-		usb_fill_bulk_urb(urb, data->udev, pipe,
-				skb->data, skb->len, btusb_tx_complete, skb);
+	skb->dev = (void *)hdev;
 
-		hdev->stat.acl_tx++;
-		break;
+	return urb;
+}
 
-	case HCI_SCODATA_PKT:
-		if (!data->isoc_tx_ep || hci_conn_num(hdev, SCO_LINK) < 1)
-			return -ENODEV;
+static struct urb *alloc_isoc_urb(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct btusb_data *data = hci_get_drvdata(hdev);
+	struct urb *urb;
+	unsigned int pipe;
 
-		urb = usb_alloc_urb(BTUSB_MAX_ISOC_FRAMES, GFP_ATOMIC);
-		if (!urb)
-			return -ENOMEM;
+	if (!data->isoc_tx_ep)
+		return ERR_PTR(-ENODEV);
 
-		pipe = usb_sndisocpipe(data->udev,
-					data->isoc_tx_ep->bEndpointAddress);
+	urb = usb_alloc_urb(BTUSB_MAX_ISOC_FRAMES, GFP_KERNEL);
+	if (!urb)
+		return ERR_PTR(-ENOMEM);
 
-		usb_fill_int_urb(urb, data->udev, pipe,
-				skb->data, skb->len, btusb_isoc_tx_complete,
-				skb, data->isoc_tx_ep->bInterval);
+	pipe = usb_sndisocpipe(data->udev, data->isoc_tx_ep->bEndpointAddress);
 
-		urb->transfer_flags  = URB_ISO_ASAP;
+	usb_fill_int_urb(urb, data->udev, pipe,
+			 skb->data, skb->len, btusb_isoc_tx_complete,
+			 skb, data->isoc_tx_ep->bInterval);
 
-		__fill_isoc_descriptor(urb, skb->len,
-				le16_to_cpu(data->isoc_tx_ep->wMaxPacketSize));
+	urb->transfer_flags  = URB_ISO_ASAP;
 
-		hdev->stat.sco_tx++;
-		goto skip_waking;
+	__fill_isoc_descriptor(urb, skb->len,
+			       le16_to_cpu(data->isoc_tx_ep->wMaxPacketSize));
 
-	default:
-		return -EILSEQ;
-	}
+	skb->dev = (void *)hdev;
 
-	err = inc_tx(data);
-	if (err) {
-		usb_anchor_urb(urb, &data->deferred);
-		schedule_work(&data->waker);
-		err = 0;
-		goto done;
-	}
+	return urb;
+}
+
+static int submit_tx_urb(struct hci_dev *hdev, struct urb *urb)
+{
+	struct btusb_data *data = hci_get_drvdata(hdev);
+	int err;
 
-skip_waking:
 	usb_anchor_urb(urb, &data->tx_anchor);
 
-	err = usb_submit_urb(urb, GFP_ATOMIC);
+	err = usb_submit_urb(urb, GFP_KERNEL);
 	if (err < 0) {
 		if (err != -EPERM && err != -ENODEV)
 			BT_ERR("%s urb %p submission failed (%d)",
-						hdev->name, urb, -err);
+			       hdev->name, urb, -err);
 		kfree(urb->setup_packet);
 		usb_unanchor_urb(urb);
 	} else {
 		usb_mark_last_busy(data->udev);
 	}
 
-done:
 	usb_free_urb(urb);
 	return err;
 }
 
+static int submit_or_queue_tx_urb(struct hci_dev *hdev, struct urb *urb)
+{
+	struct btusb_data *data = hci_get_drvdata(hdev);
+	unsigned long flags;
+	bool suspending;
+
+	spin_lock_irqsave(&data->txlock, flags);
+	suspending = test_bit(BTUSB_SUSPENDING, &data->flags);
+	if (!suspending)
+		data->tx_in_flight++;
+	spin_unlock_irqrestore(&data->txlock, flags);
+
+	if (!suspending)
+		return submit_tx_urb(hdev, urb);
+
+	usb_anchor_urb(urb, &data->deferred);
+	schedule_work(&data->waker);
+
+	usb_free_urb(urb);
+	return 0;
+}
+
+static int btusb_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
+{
+	struct urb *urb;
+
+	BT_DBG("%s", hdev->name);
+
+	if (!test_bit(HCI_RUNNING, &hdev->flags))
+		return -EBUSY;
+
+	switch (bt_cb(skb)->pkt_type) {
+	case HCI_COMMAND_PKT:
+		urb = alloc_ctrl_urb(hdev, skb);
+		if (IS_ERR(urb))
+			return PTR_ERR(urb);
+
+		hdev->stat.cmd_tx++;
+		return submit_or_queue_tx_urb(hdev, urb);
+
+	case HCI_ACLDATA_PKT:
+		urb = alloc_bulk_urb(hdev, skb);
+		if (IS_ERR(urb))
+			return PTR_ERR(urb);
+
+		hdev->stat.acl_tx++;
+		return submit_or_queue_tx_urb(hdev, urb);
+
+	case HCI_SCODATA_PKT:
+		if (hci_conn_num(hdev, SCO_LINK) < 1)
+			return -ENODEV;
+
+		urb = alloc_isoc_urb(hdev, skb);
+		if (IS_ERR(urb))
+			return PTR_ERR(urb);
+
+		hdev->stat.sco_tx++;
+		return submit_tx_urb(hdev, urb);
+	}
+
+	return -EILSEQ;
+}
+
 static void btusb_notify(struct hci_dev *hdev, unsigned int evt)
 {
 	struct btusb_data *data = hci_get_drvdata(hdev);
@@ -930,6 +1172,7 @@ static void btusb_work(struct work_struct *work)
 
 		if (hdev->voice_setting & 0x0020) {
 			static const int alts[3] = { 2, 4, 5 };
+
 			new_alts = alts[data->sco_num - 1];
 		} else {
 			new_alts = data->sco_num;
@@ -1002,7 +1245,7 @@ static int btusb_setup_csr(struct hci_dev *hdev)
 		return -PTR_ERR(skb);
 	}
 
-	rp = (struct hci_rp_read_local_version *) skb->data;
+	rp = (struct hci_rp_read_local_version *)skb->data;
 
 	if (!rp->status) {
 		if (le16_to_cpu(rp->manufacturer) != 10) {
@@ -1040,7 +1283,7 @@ struct intel_version {
 } __packed;
 
 static const struct firmware *btusb_setup_intel_get_fw(struct hci_dev *hdev,
-						struct intel_version *ver)
+						       struct intel_version *ver)
 {
 	const struct firmware *fw;
 	char fwname[64];
@@ -1216,7 +1459,7 @@ static int btusb_check_bdaddr_intel(struct hci_dev *hdev)
 		return -EIO;
 	}
 
-	rp = (struct hci_rp_read_bd_addr *) skb->data;
+	rp = (struct hci_rp_read_bd_addr *)skb->data;
 	if (rp->status) {
 		BT_ERR("%s Intel device address result failed (%02x)",
 		       hdev->name, rp->status);
@@ -1346,6 +1589,7 @@ static int btusb_setup_intel(struct hci_dev *hdev)
 
 	if (skb->data[0]) {
 		u8 evt_status = skb->data[0];
+
 		BT_ERR("%s enable Intel manufacturer mode event failed (%02x)",
 		       hdev->name, evt_status);
 		kfree_skb(skb);
@@ -1455,7 +1699,7 @@ static int btusb_set_bdaddr_intel(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
 		BT_ERR("%s: changing Intel device address failed (%ld)",
-			hdev->name, ret);
+		       hdev->name, ret);
 		return ret;
 	}
 	kfree_skb(skb);
@@ -1530,19 +1774,19 @@ static int btusb_setup_bcm_patchram(struct hci_dev *hdev)
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
 		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION failed (%ld)",
-			hdev->name, ret);
+		       hdev->name, ret);
 		goto done;
 	}
 
 	if (skb->len != sizeof(*ver)) {
 		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION event length mismatch",
-			hdev->name);
+		       hdev->name);
 		kfree_skb(skb);
 		ret = -EIO;
 		goto done;
 	}
 
-	ver = (struct hci_rp_read_local_version *) skb->data;
+	ver = (struct hci_rp_read_local_version *)skb->data;
 	BT_INFO("%s: BCM: patching hci_ver=%02x hci_rev=%04x lmp_ver=%02x "
 		"lmp_subver=%04x", hdev->name, ver->hci_ver, ver->hci_rev,
 		ver->lmp_ver, ver->lmp_subver);
@@ -1553,7 +1797,7 @@ static int btusb_setup_bcm_patchram(struct hci_dev *hdev)
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
 		BT_ERR("%s: BCM: Download Minidrv command failed (%ld)",
-			hdev->name, ret);
+		       hdev->name, ret);
 		goto reset_fw;
 	}
 	kfree_skb(skb);
@@ -1565,13 +1809,13 @@ static int btusb_setup_bcm_patchram(struct hci_dev *hdev)
 	fw_size = fw->size;
 
 	while (fw_size >= sizeof(*cmd)) {
-		cmd = (struct hci_command_hdr *) fw_ptr;
+		cmd = (struct hci_command_hdr *)fw_ptr;
 		fw_ptr += sizeof(*cmd);
 		fw_size -= sizeof(*cmd);
 
 		if (fw_size < cmd->plen) {
 			BT_ERR("%s: BCM: patch %s is corrupted",
-				hdev->name, fw_name);
+			       hdev->name, fw_name);
 			ret = -EINVAL;
 			goto reset_fw;
 		}
@@ -1587,7 +1831,7 @@ static int btusb_setup_bcm_patchram(struct hci_dev *hdev)
 		if (IS_ERR(skb)) {
 			ret = PTR_ERR(skb);
 			BT_ERR("%s: BCM: patch command %04x failed (%ld)",
-				hdev->name, opcode, ret);
+			       hdev->name, opcode, ret);
 			goto reset_fw;
 		}
 		kfree_skb(skb);
@@ -1612,19 +1856,19 @@ reset_fw:
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
 		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION failed (%ld)",
-			hdev->name, ret);
+		       hdev->name, ret);
 		goto done;
 	}
 
 	if (skb->len != sizeof(*ver)) {
 		BT_ERR("%s: HCI_OP_READ_LOCAL_VERSION event length mismatch",
-			hdev->name);
+		       hdev->name);
 		kfree_skb(skb);
 		ret = -EIO;
 		goto done;
 	}
 
-	ver = (struct hci_rp_read_local_version *) skb->data;
+	ver = (struct hci_rp_read_local_version *)skb->data;
 	BT_INFO("%s: BCM: firmware hci_ver=%02x hci_rev=%04x lmp_ver=%02x "
 		"lmp_subver=%04x", hdev->name, ver->hci_ver, ver->hci_rev,
 		ver->lmp_ver, ver->lmp_subver);
@@ -1636,19 +1880,19 @@ reset_fw:
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
 		BT_ERR("%s: HCI_OP_READ_BD_ADDR failed (%ld)",
-			hdev->name, ret);
+		       hdev->name, ret);
 		goto done;
 	}
 
 	if (skb->len != sizeof(*bda)) {
 		BT_ERR("%s: HCI_OP_READ_BD_ADDR event length mismatch",
-			hdev->name);
+		       hdev->name);
 		kfree_skb(skb);
 		ret = -EIO;
 		goto done;
 	}
 
-	bda = (struct hci_rp_read_bd_addr *) skb->data;
+	bda = (struct hci_rp_read_bd_addr *)skb->data;
 	if (bda->status) {
 		BT_ERR("%s: HCI_OP_READ_BD_ADDR error status (%02x)",
 		       hdev->name, bda->status);
@@ -1683,7 +1927,7 @@ static int btusb_set_bdaddr_bcm(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 	if (IS_ERR(skb)) {
 		ret = PTR_ERR(skb);
 		BT_ERR("%s: BCM: Change address command failed (%ld)",
-			hdev->name, ret);
+		       hdev->name, ret);
 		return ret;
 	}
 	kfree_skb(skb);
@@ -1692,7 +1936,7 @@ static int btusb_set_bdaddr_bcm(struct hci_dev *hdev, const bdaddr_t *bdaddr)
 }
 
 static int btusb_probe(struct usb_interface *intf,
-				const struct usb_device_id *id)
+		       const struct usb_device_id *id)
 {
 	struct usb_endpoint_descriptor *ep_desc;
 	struct btusb_data *data;
@@ -1707,6 +1951,7 @@ static int btusb_probe(struct usb_interface *intf,
 
 	if (!id->driver_info) {
 		const struct usb_device_id *match;
+
 		match = usb_match_id(intf, blacklist_table);
 		if (match)
 			id = match;
@@ -1755,17 +2000,16 @@ static int btusb_probe(struct usb_interface *intf,
 	data->udev = interface_to_usbdev(intf);
 	data->intf = intf;
 
-	spin_lock_init(&data->lock);
-
 	INIT_WORK(&data->work, btusb_work);
 	INIT_WORK(&data->waker, btusb_waker);
+	init_usb_anchor(&data->deferred);
+	init_usb_anchor(&data->tx_anchor);
 	spin_lock_init(&data->txlock);
 
-	init_usb_anchor(&data->tx_anchor);
 	init_usb_anchor(&data->intr_anchor);
 	init_usb_anchor(&data->bulk_anchor);
 	init_usb_anchor(&data->isoc_anchor);
-	init_usb_anchor(&data->deferred);
+	spin_lock_init(&data->rxlock);
 
 	hdev = hci_alloc_dev();
 	if (!hdev)
@@ -1857,7 +2101,7 @@ static int btusb_probe(struct usb_interface *intf,
 
 	if (data->isoc) {
 		err = usb_driver_claim_interface(&btusb_driver,
-							data->isoc, data);
+						 data->isoc, data);
 		if (err < 0) {
 			hci_free_dev(hdev);
 			return err;
@@ -1898,6 +2142,7 @@ static void btusb_disconnect(struct usb_interface *intf)
 	else if (data->isoc)
 		usb_driver_release_interface(&btusb_driver, data->isoc);
 
+	btusb_free_frags(data);
 	hci_free_dev(hdev);
 }
 
diff --git a/drivers/bluetooth/dtl1_cs.c b/drivers/bluetooth/dtl1_cs.c
index 2bd8fad17206..78e10f0c65b2 100644
--- a/drivers/bluetooth/dtl1_cs.c
+++ b/drivers/bluetooth/dtl1_cs.c
@@ -62,7 +62,7 @@ MODULE_LICENSE("GPL");
 /* ======================== Local structures ======================== */
 
 
-typedef struct dtl1_info_t {
+struct dtl1_info {
 	struct pcmcia_device *p_dev;
 
 	struct hci_dev *hdev;
@@ -78,7 +78,7 @@ typedef struct dtl1_info_t {
 	unsigned long rx_state;
 	unsigned long rx_count;
 	struct sk_buff *rx_skb;
-} dtl1_info_t;
+};
 
 
 static int dtl1_config(struct pcmcia_device *link);
@@ -94,11 +94,11 @@ static int dtl1_config(struct pcmcia_device *link);
 #define RECV_WAIT_DATA  1
 
 
-typedef struct {
+struct nsh {
 	u8 type;
 	u8 zero;
 	u16 len;
-} __packed nsh_t;	/* Nokia Specific Header */
+} __packed;	/* Nokia Specific Header */
 
 #define NSHL  4				/* Nokia Specific Header Length */
 
@@ -126,7 +126,7 @@ static int dtl1_write(unsigned int iobase, int fifo_size, __u8 *buf, int len)
 }
 
 
-static void dtl1_write_wakeup(dtl1_info_t *info)
+static void dtl1_write_wakeup(struct dtl1_info *info)
 {
 	if (!info) {
 		BT_ERR("Unknown device");
@@ -176,7 +176,7 @@ static void dtl1_write_wakeup(dtl1_info_t *info)
 }
 
 
-static void dtl1_control(dtl1_info_t *info, struct sk_buff *skb)
+static void dtl1_control(struct dtl1_info *info, struct sk_buff *skb)
 {
 	u8 flowmask = *(u8 *)skb->data;
 	int i;
@@ -199,10 +199,10 @@ static void dtl1_control(dtl1_info_t *info, struct sk_buff *skb)
 }
 
 
-static void dtl1_receive(dtl1_info_t *info)
+static void dtl1_receive(struct dtl1_info *info)
 {
 	unsigned int iobase;
-	nsh_t *nsh;
+	struct nsh *nsh;
 	int boguscount = 0;
 
 	if (!info) {
@@ -227,7 +227,7 @@ static void dtl1_receive(dtl1_info_t *info)
 		}
 
 		*skb_put(info->rx_skb, 1) = inb(iobase + UART_RX);
-		nsh = (nsh_t *)info->rx_skb->data;
+		nsh = (struct nsh *)info->rx_skb->data;
 
 		info->rx_count--;
 
@@ -287,7 +287,7 @@ static void dtl1_receive(dtl1_info_t *info)
 
 static irqreturn_t dtl1_interrupt(int irq, void *dev_inst)
 {
-	dtl1_info_t *info = dev_inst;
+	struct dtl1_info *info = dev_inst;
 	unsigned int iobase;
 	unsigned char msr;
 	int boguscount = 0;
@@ -365,7 +365,7 @@ static int dtl1_hci_open(struct hci_dev *hdev)
 
 static int dtl1_hci_flush(struct hci_dev *hdev)
 {
-	dtl1_info_t *info = hci_get_drvdata(hdev);
+	struct dtl1_info *info = hci_get_drvdata(hdev);
 
 	/* Drop TX queue */
 	skb_queue_purge(&(info->txq));
@@ -387,9 +387,9 @@ static int dtl1_hci_close(struct hci_dev *hdev)
 
 static int dtl1_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 {
-	dtl1_info_t *info = hci_get_drvdata(hdev);
+	struct dtl1_info *info = hci_get_drvdata(hdev);
 	struct sk_buff *s;
-	nsh_t nsh;
+	struct nsh nsh;
 
 	switch (bt_cb(skb)->pkt_type) {
 	case HCI_COMMAND_PKT:
@@ -436,7 +436,7 @@ static int dtl1_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
 /* ======================== Card services HCI interaction ======================== */
 
 
-static int dtl1_open(dtl1_info_t *info)
+static int dtl1_open(struct dtl1_info *info)
 {
 	unsigned long flags;
 	unsigned int iobase = info->p_dev->resource[0]->start;
@@ -505,7 +505,7 @@ static int dtl1_open(dtl1_info_t *info)
 }
 
 
-static int dtl1_close(dtl1_info_t *info)
+static int dtl1_close(struct dtl1_info *info)
 {
 	unsigned long flags;
 	unsigned int iobase = info->p_dev->resource[0]->start;
@@ -534,7 +534,7 @@ static int dtl1_close(dtl1_info_t *info)
 
 static int dtl1_probe(struct pcmcia_device *link)
 {
-	dtl1_info_t *info;
+	struct dtl1_info *info;
 
 	/* Create new info device */
 	info = devm_kzalloc(&link->dev, sizeof(*info), GFP_KERNEL);
@@ -552,7 +552,7 @@ static int dtl1_probe(struct pcmcia_device *link)
 
 static void dtl1_detach(struct pcmcia_device *link)
 {
-	dtl1_info_t *info = link->priv;
+	struct dtl1_info *info = link->priv;
 
 	dtl1_close(info);
 	pcmcia_disable_device(link);
@@ -571,7 +571,7 @@ static int dtl1_confcheck(struct pcmcia_device *p_dev, void *priv_data)
 
 static int dtl1_config(struct pcmcia_device *link)
 {
-	dtl1_info_t *info = link->priv;
+	struct dtl1_info *info = link->priv;
 	int ret;
 
 	/* Look for a generic full-sized window */
diff --git a/drivers/bluetooth/hci_h5.c b/drivers/bluetooth/hci_h5.c
index caacb422995d..a22838669b4e 100644
--- a/drivers/bluetooth/hci_h5.c
+++ b/drivers/bluetooth/hci_h5.c
@@ -237,7 +237,7 @@ static void h5_pkt_cull(struct h5 *h5)
 			break;
 
 		to_remove--;
-		seq = (seq - 1) % 8;
+		seq = (seq - 1) & 0x07;
 	}
 
 	if (seq != h5->rx_ack)
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index 5bb5872ffee6..6653473f2757 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -160,13 +160,11 @@ static int vhci_create_device(struct vhci_data *data, __u8 opcode)
 }
 
 static inline ssize_t vhci_get_user(struct vhci_data *data,
-				    const struct iovec *iov,
-				    unsigned long count)
+				    struct iov_iter *from)
 {
-	size_t len = iov_length(iov, count);
+	size_t len = iov_iter_count(from);
 	struct sk_buff *skb;
 	__u8 pkt_type, opcode;
-	unsigned long i;
 	int ret;
 
 	if (len < 2 || len > HCI_MAX_FRAME_SIZE)
@@ -176,12 +174,9 @@ static inline ssize_t vhci_get_user(struct vhci_data *data,
 	if (!skb)
 		return -ENOMEM;
 
-	for (i = 0; i < count; i++) {
-		if (copy_from_user(skb_put(skb, iov[i].iov_len),
-				   iov[i].iov_base, iov[i].iov_len)) {
-			kfree_skb(skb);
-			return -EFAULT;
-		}
+	if (copy_from_iter(skb_put(skb, len), len, from) != len) {
+		kfree_skb(skb);
+		return -EFAULT;
 	}
 
 	pkt_type = *((__u8 *) skb->data);
@@ -294,13 +289,12 @@ static ssize_t vhci_read(struct file *file,
 	return ret;
 }
 
-static ssize_t vhci_write(struct kiocb *iocb, const struct iovec *iov,
-			  unsigned long count, loff_t pos)
+static ssize_t vhci_write(struct kiocb *iocb, struct iov_iter *from)
 {
 	struct file *file = iocb->ki_filp;
 	struct vhci_data *data = file->private_data;
 
-	return vhci_get_user(data, iov, count);
+	return vhci_get_user(data, from);
 }
 
 static unsigned int vhci_poll(struct file *file, poll_table *wait)
@@ -365,7 +359,7 @@ static int vhci_release(struct inode *inode, struct file *file)
 static const struct file_operations vhci_fops = {
 	.owner		= THIS_MODULE,
 	.read		= vhci_read,
-	.aio_write	= vhci_write,
+	.write_iter	= vhci_write,
 	.poll		= vhci_poll,
 	.open		= vhci_open,
 	.release	= vhci_release,
diff --git a/drivers/bus/arm-ccn.c b/drivers/bus/arm-ccn.c
index 3266f8ff9311..aaa0f2a87118 100644
--- a/drivers/bus/arm-ccn.c
+++ b/drivers/bus/arm-ccn.c
@@ -57,6 +57,7 @@
 #define CCN_DT_PMCCNTRSR		0x0190
 #define CCN_DT_PMOVSR			0x0198
 #define CCN_DT_PMOVSR_CLR		0x01a0
+#define CCN_DT_PMOVSR_CLR__MASK				0x1f
 #define CCN_DT_PMCR			0x01a8
 #define CCN_DT_PMCR__OVFL_INTR_EN			(1 << 6)
 #define CCN_DT_PMCR__PMU_EN				(1 << 0)
@@ -586,6 +587,30 @@ static int arm_ccn_pmu_type_eq(u32 a, u32 b)
 	return 0;
 }
 
+static void arm_ccn_pmu_event_destroy(struct perf_event *event)
+{
+	struct arm_ccn *ccn = pmu_to_arm_ccn(event->pmu);
+	struct hw_perf_event *hw = &event->hw;
+
+	if (hw->idx == CCN_IDX_PMU_CYCLE_COUNTER) {
+		clear_bit(CCN_IDX_PMU_CYCLE_COUNTER, ccn->dt.pmu_counters_mask);
+	} else {
+		struct arm_ccn_component *source =
+				ccn->dt.pmu_counters[hw->idx].source;
+
+		if (CCN_CONFIG_TYPE(event->attr.config) == CCN_TYPE_XP &&
+				CCN_CONFIG_EVENT(event->attr.config) ==
+				CCN_EVENT_WATCHPOINT)
+			clear_bit(hw->config_base, source->xp.dt_cmp_mask);
+		else
+			clear_bit(hw->config_base, source->pmu_events_mask);
+		clear_bit(hw->idx, ccn->dt.pmu_counters_mask);
+	}
+
+	ccn->dt.pmu_counters[hw->idx].source = NULL;
+	ccn->dt.pmu_counters[hw->idx].event = NULL;
+}
+
 static int arm_ccn_pmu_event_init(struct perf_event *event)
 {
 	struct arm_ccn *ccn;
@@ -599,6 +624,7 @@ static int arm_ccn_pmu_event_init(struct perf_event *event)
 		return -ENOENT;
 
 	ccn = pmu_to_arm_ccn(event->pmu);
+	event->destroy = arm_ccn_pmu_event_destroy;
 
 	if (hw->sample_period) {
 		dev_warn(ccn->dev, "Sampling not supported!\n");
@@ -662,7 +688,7 @@ static int arm_ccn_pmu_event_init(struct perf_event *event)
 		}
 		if (e->num_vcs && vc >= e->num_vcs) {
 			dev_warn(ccn->dev, "Invalid vc %d for node/XP %d!\n",
-					port, node_xp);
+					vc, node_xp);
 			return -EINVAL;
 		}
 		valid = 1;
@@ -731,30 +757,6 @@ static int arm_ccn_pmu_event_init(struct perf_event *event)
 	return 0;
 }
 
-static void arm_ccn_pmu_event_free(struct perf_event *event)
-{
-	struct arm_ccn *ccn = pmu_to_arm_ccn(event->pmu);
-	struct hw_perf_event *hw = &event->hw;
-
-	if (hw->idx == CCN_IDX_PMU_CYCLE_COUNTER) {
-		clear_bit(CCN_IDX_PMU_CYCLE_COUNTER, ccn->dt.pmu_counters_mask);
-	} else {
-		struct arm_ccn_component *source =
-				ccn->dt.pmu_counters[hw->idx].source;
-
-		if (CCN_CONFIG_TYPE(event->attr.config) == CCN_TYPE_XP &&
-				CCN_CONFIG_EVENT(event->attr.config) ==
-				CCN_EVENT_WATCHPOINT)
-			clear_bit(hw->config_base, source->xp.dt_cmp_mask);
-		else
-			clear_bit(hw->config_base, source->pmu_events_mask);
-		clear_bit(hw->idx, ccn->dt.pmu_counters_mask);
-	}
-
-	ccn->dt.pmu_counters[hw->idx].source = NULL;
-	ccn->dt.pmu_counters[hw->idx].event = NULL;
-}
-
 static u64 arm_ccn_pmu_read_counter(struct arm_ccn *ccn, int idx)
 {
 	u64 res;
@@ -1027,8 +1029,6 @@ static int arm_ccn_pmu_event_add(struct perf_event *event, int flags)
 static void arm_ccn_pmu_event_del(struct perf_event *event, int flags)
 {
 	arm_ccn_pmu_event_stop(event, PERF_EF_UPDATE);
-
-	arm_ccn_pmu_event_free(event);
 }
 
 static void arm_ccn_pmu_event_read(struct perf_event *event)
@@ -1052,7 +1052,8 @@ static irqreturn_t arm_ccn_pmu_overflow_handler(struct arm_ccn_dt *dt)
 		struct perf_event *event = dt->pmu_counters[idx].event;
 		int overflowed = pmovsr & BIT(idx);
 
-		WARN_ON_ONCE(overflowed && !event);
+		WARN_ON_ONCE(overflowed && !event &&
+				idx != CCN_IDX_PMU_CYCLE_COUNTER);
 
 		if (!event || !overflowed)
 			continue;
@@ -1088,6 +1089,7 @@ static int arm_ccn_pmu_init(struct arm_ccn *ccn)
 	/* Initialize DT subsystem */
 	ccn->dt.base = ccn->base + CCN_REGION_SIZE;
 	spin_lock_init(&ccn->dt.config_lock);
+	writel(CCN_DT_PMOVSR_CLR__MASK, ccn->dt.base + CCN_DT_PMOVSR_CLR);
 	writel(CCN_DT_CTL__DT_EN, ccn->dt.base + CCN_DT_CTL);
 	writel(CCN_DT_PMCR__OVFL_INTR_EN | CCN_DT_PMCR__PMU_EN,
 			ccn->dt.base + CCN_DT_PMCR);
diff --git a/drivers/bus/omap_l3_noc.h b/drivers/bus/omap_l3_noc.h
index 551e01061434..95254585db86 100644
--- a/drivers/bus/omap_l3_noc.h
+++ b/drivers/bus/omap_l3_noc.h
@@ -188,31 +188,31 @@ static struct l3_flagmux_data omap_l3_flagmux_clk3 = {
 };
 
 static struct l3_masters_data omap_l3_masters[] = {
-	{ 0x0 , "MPU"},
-	{ 0x10, "CS_ADP"},
-	{ 0x14, "xxx"},
-	{ 0x20, "DSP"},
-	{ 0x30, "IVAHD"},
-	{ 0x40, "ISS"},
-	{ 0x44, "DucatiM3"},
-	{ 0x48, "FaceDetect"},
-	{ 0x50, "SDMA_Rd"},
-	{ 0x54, "SDMA_Wr"},
-	{ 0x58, "xxx"},
-	{ 0x5C, "xxx"},
-	{ 0x60, "SGX"},
-	{ 0x70, "DSS"},
-	{ 0x80, "C2C"},
-	{ 0x88, "xxx"},
-	{ 0x8C, "xxx"},
-	{ 0x90, "HSI"},
-	{ 0xA0, "MMC1"},
-	{ 0xA4, "MMC2"},
-	{ 0xA8, "MMC6"},
-	{ 0xB0, "UNIPRO1"},
-	{ 0xC0, "USBHOSTHS"},
-	{ 0xC4, "USBOTGHS"},
-	{ 0xC8, "USBHOSTFS"}
+	{ 0x00, "MPU"},
+	{ 0x04, "CS_ADP"},
+	{ 0x05, "xxx"},
+	{ 0x08, "DSP"},
+	{ 0x0C, "IVAHD"},
+	{ 0x10, "ISS"},
+	{ 0x11, "DucatiM3"},
+	{ 0x12, "FaceDetect"},
+	{ 0x14, "SDMA_Rd"},
+	{ 0x15, "SDMA_Wr"},
+	{ 0x16, "xxx"},
+	{ 0x17, "xxx"},
+	{ 0x18, "SGX"},
+	{ 0x1C, "DSS"},
+	{ 0x20, "C2C"},
+	{ 0x22, "xxx"},
+	{ 0x23, "xxx"},
+	{ 0x24, "HSI"},
+	{ 0x28, "MMC1"},
+	{ 0x29, "MMC2"},
+	{ 0x2A, "MMC6"},
+	{ 0x2C, "UNIPRO1"},
+	{ 0x30, "USBHOSTHS"},
+	{ 0x31, "USBOTGHS"},
+	{ 0x32, "USBHOSTFS"}
 };
 
 static struct l3_flagmux_data *omap_l3_flagmux[] = {
diff --git a/drivers/cdrom/cdrom.c b/drivers/cdrom/cdrom.c
index 898b84bba28a..5d28a45d2960 100644
--- a/drivers/cdrom/cdrom.c
+++ b/drivers/cdrom/cdrom.c
@@ -2180,8 +2180,8 @@ static int cdrom_read_cdda_bpc(struct cdrom_device_info *cdi, __u8 __user *ubuf,
 		len = nr * CD_FRAMESIZE_RAW;
 
 		rq = blk_get_request(q, READ, GFP_KERNEL);
-		if (!rq) {
-			ret = -ENOMEM;
+		if (IS_ERR(rq)) {
+			ret = PTR_ERR(rq);
 			break;
 		}
 		blk_rq_set_block_pc(rq);
diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 6e9f74a5c095..efefd12a0f7b 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -600,5 +600,7 @@ config TILE_SROM
 	  device appear much like a simple EEPROM, and knows
 	  how to partition a single ROM for multiple purposes.
 
+source "drivers/char/xillybus/Kconfig"
+
 endmenu
 
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index a324f9303e36..d06cde26031b 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -61,3 +61,4 @@ obj-$(CONFIG_JS_RTC)		+= js-rtc.o
 js-rtc-y = rtc.o
 
 obj-$(CONFIG_TILE_SROM)		+= tile-srom.o
+obj-$(CONFIG_XILLYBUS)		+= xillybus/
diff --git a/drivers/char/hw_random/Kconfig b/drivers/char/hw_random/Kconfig
index 836b061ced35..91a04ae8003c 100644
--- a/drivers/char/hw_random/Kconfig
+++ b/drivers/char/hw_random/Kconfig
@@ -333,6 +333,19 @@ config HW_RANDOM_MSM
 
 	  If unsure, say Y.
 
+config HW_RANDOM_XGENE
+	tristate "APM X-Gene True Random Number Generator (TRNG) support"
+	depends on HW_RANDOM && ARCH_XGENE
+	default HW_RANDOM
+	---help---
+	  This driver provides kernel-side support for the Random Number
+	  Generator hardware found on APM X-Gene SoC.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called xgene_rng.
+
+	  If unsure, say Y.
+
 endif # HW_RANDOM
 
 config UML_RANDOM
diff --git a/drivers/char/hw_random/Makefile b/drivers/char/hw_random/Makefile
index 199ed283e149..0b4cd57f4e24 100644
--- a/drivers/char/hw_random/Makefile
+++ b/drivers/char/hw_random/Makefile
@@ -29,3 +29,4 @@ obj-$(CONFIG_HW_RANDOM_EXYNOS)	+= exynos-rng.o
 obj-$(CONFIG_HW_RANDOM_TPM) += tpm-rng.o
 obj-$(CONFIG_HW_RANDOM_BCM2835) += bcm2835-rng.o
 obj-$(CONFIG_HW_RANDOM_MSM) += msm-rng.o
+obj-$(CONFIG_HW_RANDOM_XGENE) += xgene-rng.o
diff --git a/drivers/char/hw_random/amd-rng.c b/drivers/char/hw_random/amd-rng.c
index c6af038682f1..48f6a83cdd61 100644
--- a/drivers/char/hw_random/amd-rng.c
+++ b/drivers/char/hw_random/amd-rng.c
@@ -142,10 +142,10 @@ found:
 	amd_rng.priv = (unsigned long)pmbase;
 	amd_pdev = pdev;
 
-	printk(KERN_INFO "AMD768 RNG detected\n");
+	pr_info("AMD768 RNG detected\n");
 	err = hwrng_register(&amd_rng);
 	if (err) {
-		printk(KERN_ERR PFX "RNG registering failed (%d)\n",
+		pr_err(PFX "RNG registering failed (%d)\n",
 		       err);
 		release_region(pmbase + 0xF0, 8);
 		goto out;
diff --git a/drivers/char/hw_random/geode-rng.c b/drivers/char/hw_random/geode-rng.c
index 4c4d4e140f98..0d0579fe465e 100644
--- a/drivers/char/hw_random/geode-rng.c
+++ b/drivers/char/hw_random/geode-rng.c
@@ -109,10 +109,10 @@ found:
 		goto out;
 	geode_rng.priv = (unsigned long)mem;
 
-	printk(KERN_INFO "AMD Geode RNG detected\n");
+	pr_info("AMD Geode RNG detected\n");
 	err = hwrng_register(&geode_rng);
 	if (err) {
-		printk(KERN_ERR PFX "RNG registering failed (%d)\n",
+		pr_err(PFX "RNG registering failed (%d)\n",
 		       err);
 		goto err_unmap;
 	}
diff --git a/drivers/char/hw_random/intel-rng.c b/drivers/char/hw_random/intel-rng.c
index 86fe45c19968..290c880266bf 100644
--- a/drivers/char/hw_random/intel-rng.c
+++ b/drivers/char/hw_random/intel-rng.c
@@ -199,7 +199,7 @@ static int intel_rng_init(struct hwrng *rng)
 	if ((hw_status & INTEL_RNG_ENABLED) == 0)
 		hw_status = hwstatus_set(mem, hw_status | INTEL_RNG_ENABLED);
 	if ((hw_status & INTEL_RNG_ENABLED) == 0) {
-		printk(KERN_ERR PFX "cannot enable RNG, aborting\n");
+		pr_err(PFX "cannot enable RNG, aborting\n");
 		goto out;
 	}
 	err = 0;
@@ -216,7 +216,7 @@ static void intel_rng_cleanup(struct hwrng *rng)
 	if (hw_status & INTEL_RNG_ENABLED)
 		hwstatus_set(mem, hw_status & ~INTEL_RNG_ENABLED);
 	else
-		printk(KERN_WARNING PFX "unusual: RNG already disabled\n");
+		pr_warn(PFX "unusual: RNG already disabled\n");
 }
 
 
@@ -274,7 +274,7 @@ static int __init intel_rng_hw_init(void *_intel_rng_hw)
 	if (mfc != INTEL_FWH_MANUFACTURER_CODE ||
 	    (dvc != INTEL_FWH_DEVICE_CODE_8M &&
 	     dvc != INTEL_FWH_DEVICE_CODE_4M)) {
-		printk(KERN_NOTICE PFX "FWH not detected\n");
+		pr_notice(PFX "FWH not detected\n");
 		return -ENODEV;
 	}
 
@@ -306,7 +306,6 @@ static int __init intel_init_hw_struct(struct intel_rng_hw *intel_rng_hw,
 	     (BIOS_CNTL_LOCK_ENABLE_MASK|BIOS_CNTL_WRITE_ENABLE_MASK))
 	    == BIOS_CNTL_LOCK_ENABLE_MASK) {
 		static __initdata /*const*/ char warning[] =
-			KERN_WARNING
 PFX "Firmware space is locked read-only. If you can't or\n"
 PFX "don't want to disable this in firmware setup, and if\n"
 PFX "you are certain that your system has a functional\n"
@@ -314,7 +313,7 @@ PFX "RNG, try using the 'no_fwh_detect' option.\n";
 
 		if (no_fwh_detect)
 			return -ENODEV;
-		printk(warning);
+		pr_warn("%s", warning);
 		return -EBUSY;
 	}
 
@@ -392,10 +391,10 @@ fwh_done:
 		goto out;
 	}
 
-	printk(KERN_INFO "Intel 82802 RNG detected\n");
+	pr_info("Intel 82802 RNG detected\n");
 	err = hwrng_register(&intel_rng);
 	if (err) {
-		printk(KERN_ERR PFX "RNG registering failed (%d)\n",
+		pr_err(PFX "RNG registering failed (%d)\n",
 		       err);
 		iounmap(mem);
 	}
diff --git a/drivers/char/hw_random/pasemi-rng.c b/drivers/char/hw_random/pasemi-rng.c
index c66279bb6ef3..c0347d1dded0 100644
--- a/drivers/char/hw_random/pasemi-rng.c
+++ b/drivers/char/hw_random/pasemi-rng.c
@@ -113,7 +113,7 @@ static int rng_probe(struct platform_device *ofdev)
 
 	pasemi_rng.priv = (unsigned long)rng_regs;
 
-	printk(KERN_INFO "Registering PA Semi RNG\n");
+	pr_info("Registering PA Semi RNG\n");
 
 	err = hwrng_register(&pasemi_rng);
 
diff --git a/drivers/char/hw_random/pseries-rng.c b/drivers/char/hw_random/pseries-rng.c
index ab7ffdec0ec3..6226aa08c36a 100644
--- a/drivers/char/hw_random/pseries-rng.c
+++ b/drivers/char/hw_random/pseries-rng.c
@@ -86,7 +86,7 @@ static struct vio_driver pseries_rng_driver = {
 
 static int __init rng_init(void)
 {
-	printk(KERN_INFO "Registering IBM pSeries RNG driver\n");
+	pr_info("Registering IBM pSeries RNG driver\n");
 	return vio_register_driver(&pseries_rng_driver);
 }
 
diff --git a/drivers/char/hw_random/via-rng.c b/drivers/char/hw_random/via-rng.c
index de5a6dcfb3e2..a3bebef255ad 100644
--- a/drivers/char/hw_random/via-rng.c
+++ b/drivers/char/hw_random/via-rng.c
@@ -141,7 +141,7 @@ static int via_rng_init(struct hwrng *rng)
 	 * register */
 	if ((c->x86 == 6) && (c->x86_model >= 0x0f)) {
 		if (!cpu_has_xstore_enabled) {
-			printk(KERN_ERR PFX "can't enable hardware RNG "
+			pr_err(PFX "can't enable hardware RNG "
 				"if XSTORE is not enabled\n");
 			return -ENODEV;
 		}
@@ -180,7 +180,7 @@ static int via_rng_init(struct hwrng *rng)
 	   unneeded */
 	rdmsr(MSR_VIA_RNG, lo, hi);
 	if ((lo & VIA_RNG_ENABLE) == 0) {
-		printk(KERN_ERR PFX "cannot enable VIA C3 RNG, aborting\n");
+		pr_err(PFX "cannot enable VIA C3 RNG, aborting\n");
 		return -ENODEV;
 	}
 
@@ -202,10 +202,10 @@ static int __init mod_init(void)
 
 	if (!cpu_has_xstore)
 		return -ENODEV;
-	printk(KERN_INFO "VIA RNG detected\n");
+	pr_info("VIA RNG detected\n");
 	err = hwrng_register(&via_rng);
 	if (err) {
-		printk(KERN_ERR PFX "RNG registering failed (%d)\n",
+		pr_err(PFX "RNG registering failed (%d)\n",
 		       err);
 		goto out;
 	}
diff --git a/drivers/char/hw_random/virtio-rng.c b/drivers/char/hw_random/virtio-rng.c
index 2e3139eda93b..72295ea2fd1c 100644
--- a/drivers/char/hw_random/virtio-rng.c
+++ b/drivers/char/hw_random/virtio-rng.c
@@ -36,6 +36,7 @@ struct virtrng_info {
 	int index;
 	bool busy;
 	bool hwrng_register_done;
+	bool hwrng_removed;
 };
 
 
@@ -68,6 +69,9 @@ static int virtio_read(struct hwrng *rng, void *buf, size_t size, bool wait)
 	int ret;
 	struct virtrng_info *vi = (struct virtrng_info *)rng->priv;
 
+	if (vi->hwrng_removed)
+		return -ENODEV;
+
 	if (!vi->busy) {
 		vi->busy = true;
 		init_completion(&vi->have_data);
@@ -105,8 +109,8 @@ static int probe_common(struct virtio_device *vdev)
 
 	vi->index = index = ida_simple_get(&rng_index_ida, 0, 0, GFP_KERNEL);
 	if (index < 0) {
-		kfree(vi);
-		return index;
+		err = index;
+		goto err_ida;
 	}
 	sprintf(vi->name, "virtio_rng.%d", index);
 	init_completion(&vi->have_data);
@@ -124,19 +128,25 @@ static int probe_common(struct virtio_device *vdev)
 	vi->vq = virtio_find_single_vq(vdev, random_recv_done, "input");
 	if (IS_ERR(vi->vq)) {
 		err = PTR_ERR(vi->vq);
-		vi->vq = NULL;
-		kfree(vi);
-		ida_simple_remove(&rng_index_ida, index);
-		return err;
+		goto err_find;
 	}
 
 	return 0;
+
+err_find:
+	ida_simple_remove(&rng_index_ida, index);
+err_ida:
+	kfree(vi);
+	return err;
 }
 
 static void remove_common(struct virtio_device *vdev)
 {
 	struct virtrng_info *vi = vdev->priv;
 
+	vi->hwrng_removed = true;
+	vi->data_avail = 0;
+	complete(&vi->have_data);
 	vdev->config->reset(vdev);
 	vi->busy = false;
 	if (vi->hwrng_register_done)
diff --git a/drivers/char/hw_random/xgene-rng.c b/drivers/char/hw_random/xgene-rng.c
new file mode 100644
index 000000000000..23caa05380a8
--- /dev/null
+++ b/drivers/char/hw_random/xgene-rng.c
@@ -0,0 +1,423 @@
+/*
+ * APM X-Gene SoC RNG Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Author: Rameshwar Prasad Sahu <rsahu@apm.com>
+ *	   Shamal Winchurkar <swinchurkar@apm.com>
+ *	   Feng Kan <fkan@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/hw_random.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/timer.h>
+
+#define RNG_MAX_DATUM			4
+#define MAX_TRY				100
+#define XGENE_RNG_RETRY_COUNT		20
+#define XGENE_RNG_RETRY_INTERVAL	10
+
+/* RNG  Registers */
+#define RNG_INOUT_0			0x00
+#define RNG_INTR_STS_ACK		0x10
+#define RNG_CONTROL			0x14
+#define RNG_CONFIG			0x18
+#define RNG_ALARMCNT			0x1c
+#define RNG_FROENABLE			0x20
+#define RNG_FRODETUNE			0x24
+#define RNG_ALARMMASK			0x28
+#define RNG_ALARMSTOP			0x2c
+#define RNG_OPTIONS			0x78
+#define RNG_EIP_REV			0x7c
+
+#define MONOBIT_FAIL_MASK		BIT(7)
+#define POKER_FAIL_MASK			BIT(6)
+#define LONG_RUN_FAIL_MASK		BIT(5)
+#define RUN_FAIL_MASK			BIT(4)
+#define NOISE_FAIL_MASK			BIT(3)
+#define STUCK_OUT_MASK			BIT(2)
+#define SHUTDOWN_OFLO_MASK		BIT(1)
+#define READY_MASK			BIT(0)
+
+#define MAJOR_HW_REV_RD(src)		(((src) & 0x0f000000) >> 24)
+#define MINOR_HW_REV_RD(src)		(((src) & 0x00f00000) >> 20)
+#define HW_PATCH_LEVEL_RD(src)		(((src) & 0x000f0000) >> 16)
+#define MAX_REFILL_CYCLES_SET(dst, src) \
+			((dst & ~0xffff0000) | (((u32)src << 16) & 0xffff0000))
+#define MIN_REFILL_CYCLES_SET(dst, src) \
+			((dst & ~0x000000ff) | (((u32)src) & 0x000000ff))
+#define ALARM_THRESHOLD_SET(dst, src) \
+			((dst & ~0x000000ff) | (((u32)src) & 0x000000ff))
+#define ENABLE_RNG_SET(dst, src) \
+			((dst & ~BIT(10)) | (((u32)src << 10) & BIT(10)))
+#define REGSPEC_TEST_MODE_SET(dst, src) \
+			((dst & ~BIT(8)) | (((u32)src << 8) & BIT(8)))
+#define MONOBIT_FAIL_MASK_SET(dst, src) \
+			((dst & ~BIT(7)) | (((u32)src << 7) & BIT(7)))
+#define POKER_FAIL_MASK_SET(dst, src) \
+			((dst & ~BIT(6)) | (((u32)src << 6) & BIT(6)))
+#define LONG_RUN_FAIL_MASK_SET(dst, src) \
+			((dst & ~BIT(5)) | (((u32)src << 5) & BIT(5)))
+#define RUN_FAIL_MASK_SET(dst, src) \
+			((dst & ~BIT(4)) | (((u32)src << 4) & BIT(4)))
+#define NOISE_FAIL_MASK_SET(dst, src) \
+			((dst & ~BIT(3)) | (((u32)src << 3) & BIT(3)))
+#define STUCK_OUT_MASK_SET(dst, src) \
+			((dst & ~BIT(2)) | (((u32)src << 2) & BIT(2)))
+#define SHUTDOWN_OFLO_MASK_SET(dst, src) \
+			((dst & ~BIT(1)) | (((u32)src << 1) & BIT(1)))
+
+struct xgene_rng_dev {
+	u32 irq;
+	void  __iomem *csr_base;
+	u32 revision;
+	u32 datum_size;
+	u32 failure_cnt;	/* Failure count last minute */
+	unsigned long failure_ts;/* First failure timestamp */
+	struct timer_list failure_timer;
+	struct device *dev;
+	struct clk *clk;
+};
+
+static void xgene_rng_expired_timer(unsigned long arg)
+{
+	struct xgene_rng_dev *ctx = (struct xgene_rng_dev *) arg;
+
+	/* Clear failure counter as timer expired */
+	disable_irq(ctx->irq);
+	ctx->failure_cnt = 0;
+	del_timer(&ctx->failure_timer);
+	enable_irq(ctx->irq);
+}
+
+static void xgene_rng_start_timer(struct xgene_rng_dev *ctx)
+{
+	ctx->failure_timer.data = (unsigned long) ctx;
+	ctx->failure_timer.function = xgene_rng_expired_timer;
+	ctx->failure_timer.expires = jiffies + 120 * HZ;
+	add_timer(&ctx->failure_timer);
+}
+
+/*
+ * Initialize or reinit free running oscillators (FROs)
+ */
+static void xgene_rng_init_fro(struct xgene_rng_dev *ctx, u32 fro_val)
+{
+	writel(fro_val, ctx->csr_base + RNG_FRODETUNE);
+	writel(0x00000000, ctx->csr_base + RNG_ALARMMASK);
+	writel(0x00000000, ctx->csr_base + RNG_ALARMSTOP);
+	writel(0xFFFFFFFF, ctx->csr_base + RNG_FROENABLE);
+}
+
+static void xgene_rng_chk_overflow(struct xgene_rng_dev *ctx)
+{
+	u32 val;
+
+	val = readl(ctx->csr_base + RNG_INTR_STS_ACK);
+	if (val & MONOBIT_FAIL_MASK)
+		/*
+		 * LFSR detected an out-of-bounds number of 1s after
+		 * checking 20,000 bits (test T1 as specified in the
+		 * AIS-31 standard)
+		 */
+		dev_err(ctx->dev, "test monobit failure error 0x%08X\n", val);
+	if (val & POKER_FAIL_MASK)
+		/*
+		 * LFSR detected an out-of-bounds value in at least one
+		 * of the 16 poker_count_X counters or an out of bounds sum
+		 * of squares value after checking 20,000 bits (test T2 as
+		 * specified in the AIS-31 standard)
+		 */
+		dev_err(ctx->dev, "test poker failure error 0x%08X\n", val);
+	if (val & LONG_RUN_FAIL_MASK)
+		/*
+		 * LFSR detected a sequence of 34 identical bits
+		 * (test T4 as specified in the AIS-31 standard)
+		 */
+		dev_err(ctx->dev, "test long run failure error 0x%08X\n", val);
+	if (val & RUN_FAIL_MASK)
+		/*
+		 * LFSR detected an outof-bounds value for at least one
+		 * of the running counters after checking 20,000 bits
+		 * (test T3 as specified in the AIS-31 standard)
+		 */
+		dev_err(ctx->dev, "test run failure error 0x%08X\n", val);
+	if (val & NOISE_FAIL_MASK)
+		/* LFSR detected a sequence of 48 identical bits */
+		dev_err(ctx->dev, "noise failure error 0x%08X\n", val);
+	if (val & STUCK_OUT_MASK)
+		/*
+		 * Detected output data registers generated same value twice
+		 * in a row
+		 */
+		dev_err(ctx->dev, "stuck out failure error 0x%08X\n", val);
+
+	if (val & SHUTDOWN_OFLO_MASK) {
+		u32 frostopped;
+
+		/* FROs shut down after a second error event. Try recover. */
+		if (++ctx->failure_cnt == 1) {
+			/* 1st time, just recover */
+			ctx->failure_ts = jiffies;
+			frostopped = readl(ctx->csr_base + RNG_ALARMSTOP);
+			xgene_rng_init_fro(ctx, frostopped);
+
+			/*
+			 * We must start a timer to clear out this error
+			 * in case the system timer wrap around
+			 */
+			xgene_rng_start_timer(ctx);
+		} else {
+			/* 2nd time failure in lesser than 1 minute? */
+			if (time_after(ctx->failure_ts + 60 * HZ, jiffies)) {
+				dev_err(ctx->dev,
+					"FRO shutdown failure error 0x%08X\n",
+					val);
+			} else {
+				/* 2nd time failure after 1 minutes, recover */
+				ctx->failure_ts = jiffies;
+				ctx->failure_cnt = 1;
+				/*
+				 * We must start a timer to clear out this
+				 * error in case the system timer wrap
+				 * around
+				 */
+				xgene_rng_start_timer(ctx);
+			}
+			frostopped = readl(ctx->csr_base + RNG_ALARMSTOP);
+			xgene_rng_init_fro(ctx, frostopped);
+		}
+	}
+	/* Clear them all */
+	writel(val, ctx->csr_base + RNG_INTR_STS_ACK);
+}
+
+static irqreturn_t xgene_rng_irq_handler(int irq, void *id)
+{
+	struct xgene_rng_dev *ctx = (struct xgene_rng_dev *) id;
+
+	/* RNG Alarm Counter overflow */
+	xgene_rng_chk_overflow(ctx);
+
+	return IRQ_HANDLED;
+}
+
+static int xgene_rng_data_present(struct hwrng *rng, int wait)
+{
+	struct xgene_rng_dev *ctx = (struct xgene_rng_dev *) rng->priv;
+	u32 i, val = 0;
+
+	for (i = 0; i < XGENE_RNG_RETRY_COUNT; i++) {
+		val = readl(ctx->csr_base + RNG_INTR_STS_ACK);
+		if ((val & READY_MASK) || !wait)
+			break;
+		udelay(XGENE_RNG_RETRY_INTERVAL);
+	}
+
+	return (val & READY_MASK);
+}
+
+static int xgene_rng_data_read(struct hwrng *rng, u32 *data)
+{
+	struct xgene_rng_dev *ctx = (struct xgene_rng_dev *) rng->priv;
+	int i;
+
+	for (i = 0; i < ctx->datum_size; i++)
+		data[i] = readl(ctx->csr_base + RNG_INOUT_0 + i * 4);
+
+	/* Clear ready bit to start next transaction */
+	writel(READY_MASK, ctx->csr_base + RNG_INTR_STS_ACK);
+
+	return ctx->datum_size << 2;
+}
+
+static void xgene_rng_init_internal(struct xgene_rng_dev *ctx)
+{
+	u32 val;
+
+	writel(0x00000000, ctx->csr_base + RNG_CONTROL);
+
+	val = MAX_REFILL_CYCLES_SET(0, 10);
+	val = MIN_REFILL_CYCLES_SET(val, 10);
+	writel(val, ctx->csr_base + RNG_CONFIG);
+
+	val = ALARM_THRESHOLD_SET(0, 0xFF);
+	writel(val, ctx->csr_base + RNG_ALARMCNT);
+
+	xgene_rng_init_fro(ctx, 0);
+
+	writel(MONOBIT_FAIL_MASK |
+		POKER_FAIL_MASK	|
+		LONG_RUN_FAIL_MASK |
+		RUN_FAIL_MASK |
+		NOISE_FAIL_MASK |
+		STUCK_OUT_MASK |
+		SHUTDOWN_OFLO_MASK |
+		READY_MASK, ctx->csr_base + RNG_INTR_STS_ACK);
+
+	val = ENABLE_RNG_SET(0, 1);
+	val = MONOBIT_FAIL_MASK_SET(val, 1);
+	val = POKER_FAIL_MASK_SET(val, 1);
+	val = LONG_RUN_FAIL_MASK_SET(val, 1);
+	val = RUN_FAIL_MASK_SET(val, 1);
+	val = NOISE_FAIL_MASK_SET(val, 1);
+	val = STUCK_OUT_MASK_SET(val, 1);
+	val = SHUTDOWN_OFLO_MASK_SET(val, 1);
+	writel(val, ctx->csr_base + RNG_CONTROL);
+}
+
+static int xgene_rng_init(struct hwrng *rng)
+{
+	struct xgene_rng_dev *ctx = (struct xgene_rng_dev *) rng->priv;
+
+	ctx->failure_cnt = 0;
+	init_timer(&ctx->failure_timer);
+
+	ctx->revision = readl(ctx->csr_base + RNG_EIP_REV);
+
+	dev_dbg(ctx->dev, "Rev %d.%d.%d\n",
+		MAJOR_HW_REV_RD(ctx->revision),
+		MINOR_HW_REV_RD(ctx->revision),
+		HW_PATCH_LEVEL_RD(ctx->revision));
+
+	dev_dbg(ctx->dev, "Options 0x%08X",
+		readl(ctx->csr_base + RNG_OPTIONS));
+
+	xgene_rng_init_internal(ctx);
+
+	ctx->datum_size = RNG_MAX_DATUM;
+
+	return 0;
+}
+
+static struct hwrng xgene_rng_func = {
+	.name		= "xgene-rng",
+	.init		= xgene_rng_init,
+	.data_present	= xgene_rng_data_present,
+	.data_read	= xgene_rng_data_read,
+};
+
+static int xgene_rng_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct xgene_rng_dev *ctx;
+	int rc = 0;
+
+	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->dev = &pdev->dev;
+	platform_set_drvdata(pdev, ctx);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	ctx->csr_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(ctx->csr_base))
+		return PTR_ERR(ctx->csr_base);
+
+	ctx->irq = platform_get_irq(pdev, 0);
+	if (ctx->irq < 0) {
+		dev_err(&pdev->dev, "No IRQ resource\n");
+		return ctx->irq;
+	}
+
+	dev_dbg(&pdev->dev, "APM X-Gene RNG BASE %p ALARM IRQ %d",
+		ctx->csr_base, ctx->irq);
+
+	rc = devm_request_irq(&pdev->dev, ctx->irq, xgene_rng_irq_handler, 0,
+				dev_name(&pdev->dev), ctx);
+	if (rc) {
+		dev_err(&pdev->dev, "Could not request RNG alarm IRQ\n");
+		return rc;
+	}
+
+	/* Enable IP clock */
+	ctx->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(ctx->clk)) {
+		dev_warn(&pdev->dev, "Couldn't get the clock for RNG\n");
+	} else {
+		rc = clk_prepare_enable(ctx->clk);
+		if (rc) {
+			dev_warn(&pdev->dev,
+				 "clock prepare enable failed for RNG");
+			return rc;
+		}
+	}
+
+	xgene_rng_func.priv = (unsigned long) ctx;
+
+	rc = hwrng_register(&xgene_rng_func);
+	if (rc) {
+		dev_err(&pdev->dev, "RNG registering failed error %d\n", rc);
+		if (!IS_ERR(ctx->clk))
+			clk_disable_unprepare(ctx->clk);
+		return rc;
+	}
+
+	rc = device_init_wakeup(&pdev->dev, 1);
+	if (rc) {
+		dev_err(&pdev->dev, "RNG device_init_wakeup failed error %d\n",
+			rc);
+		if (!IS_ERR(ctx->clk))
+			clk_disable_unprepare(ctx->clk);
+		hwrng_unregister(&xgene_rng_func);
+		return rc;
+	}
+
+	return 0;
+}
+
+static int xgene_rng_remove(struct platform_device *pdev)
+{
+	struct xgene_rng_dev *ctx = platform_get_drvdata(pdev);
+	int rc;
+
+	rc = device_init_wakeup(&pdev->dev, 0);
+	if (rc)
+		dev_err(&pdev->dev, "RNG init wakeup failed error %d\n", rc);
+	if (!IS_ERR(ctx->clk))
+		clk_disable_unprepare(ctx->clk);
+	hwrng_unregister(&xgene_rng_func);
+
+	return rc;
+}
+
+static const struct of_device_id xgene_rng_of_match[] = {
+	{ .compatible = "apm,xgene-rng" },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, xgene_rng_of_match);
+
+static struct platform_driver xgene_rng_driver = {
+	.probe = xgene_rng_probe,
+	.remove	= xgene_rng_remove,
+	.driver = {
+		.name		= "xgene-rng",
+		.of_match_table = xgene_rng_of_match,
+	},
+};
+
+module_platform_driver(xgene_rng_driver);
+MODULE_DESCRIPTION("APM X-Gene RNG driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/char/i8k.c b/drivers/char/i8k.c
index 65525c7e903c..34174d01462e 100644
--- a/drivers/char/i8k.c
+++ b/drivers/char/i8k.c
@@ -651,6 +651,7 @@ struct i8k_config_data {
 
 enum i8k_configs {
 	DELL_LATITUDE_D520,
+	DELL_LATITUDE_E6540,
 	DELL_PRECISION_490,
 	DELL_STUDIO,
 	DELL_XPS_M140,
@@ -661,6 +662,10 @@ static const struct i8k_config_data i8k_config_data[] = {
 		.fan_mult = 1,
 		.fan_max = I8K_FAN_TURBO,
 	},
+	[DELL_LATITUDE_E6540] = {
+		.fan_mult = 1,
+		.fan_max = I8K_FAN_HIGH,
+	},
 	[DELL_PRECISION_490] = {
 		.fan_mult = 1,
 		.fan_max = I8K_FAN_TURBO,
@@ -706,6 +711,14 @@ static struct dmi_system_id i8k_dmi_table[] __initdata = {
 		.driver_data = (void *)&i8k_config_data[DELL_LATITUDE_D520],
 	},
 	{
+		.ident = "Dell Latitude E6540",
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "Latitude E6540"),
+		},
+		.driver_data = (void *)&i8k_config_data[DELL_LATITUDE_E6540],
+	},
+	{
 		.ident = "Dell Latitude 2",
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
diff --git a/drivers/char/ipmi/ipmi_msghandler.c b/drivers/char/ipmi/ipmi_msghandler.c
index e6db9381b2c7..f816211f062f 100644
--- a/drivers/char/ipmi/ipmi_msghandler.c
+++ b/drivers/char/ipmi/ipmi_msghandler.c
@@ -2796,7 +2796,6 @@ channel_handler(ipmi_smi_t intf, struct ipmi_recv_msg *msg)
 					= IPMI_CHANNEL_MEDIUM_IPMB;
 				intf->channels[0].protocol
 					= IPMI_CHANNEL_PROTOCOL_IPMB;
-				rv = -ENOSYS;
 
 				intf->curr_channel = IPMI_MAX_CHANNELS;
 				wake_up(&intf->waitq);
@@ -2821,12 +2820,12 @@ channel_handler(ipmi_smi_t intf, struct ipmi_recv_msg *msg)
 
 		if (rv) {
 			/* Got an error somehow, just give up. */
+			printk(KERN_WARNING PFX
+			       "Error sending channel information for channel"
+			       " %d: %d\n", intf->curr_channel, rv);
+
 			intf->curr_channel = IPMI_MAX_CHANNELS;
 			wake_up(&intf->waitq);
-
-			printk(KERN_WARNING PFX
-			       "Error sending channel information: %d\n",
-			       rv);
 		}
 	}
  out:
@@ -2964,8 +2963,12 @@ int ipmi_register_smi(struct ipmi_smi_handlers *handlers,
 		intf->null_user_handler = channel_handler;
 		intf->curr_channel = 0;
 		rv = send_channel_info_cmd(intf, 0);
-		if (rv)
+		if (rv) {
+			printk(KERN_WARNING PFX
+			       "Error sending channel information for channel"
+			       " 0, %d\n", rv);
 			goto out;
+		}
 
 		/* Wait for the channel info to be read. */
 		wait_event(intf->waitq,
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index 5d665680ae33..5c4e1f625bbb 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -965,9 +965,9 @@ static inline int ipmi_si_is_busy(struct timespec *ts)
 	return ts->tv_nsec != -1;
 }
 
-static int ipmi_thread_busy_wait(enum si_sm_result smi_result,
-				 const struct smi_info *smi_info,
-				 struct timespec *busy_until)
+static inline int ipmi_thread_busy_wait(enum si_sm_result smi_result,
+					const struct smi_info *smi_info,
+					struct timespec *busy_until)
 {
 	unsigned int max_busy_us = 0;
 
@@ -2658,6 +2658,9 @@ static int ipmi_probe(struct platform_device *dev)
 	if (!match)
 		return -EINVAL;
 
+	if (!of_device_is_available(np))
+		return -EINVAL;
+
 	ret = of_address_to_resource(np, 0, &resource);
 	if (ret) {
 		dev_warn(&dev->dev, PFX "invalid address from OF\n");
@@ -3655,6 +3658,9 @@ static void cleanup_one_si(struct smi_info *to_clean)
 	if (!to_clean)
 		return;
 
+	if (to_clean->dev)
+		dev_set_drvdata(to_clean->dev, NULL);
+
 	list_del(&to_clean->link);
 
 	/* Tell the driver that we are shutting down. */
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 917403fe10da..524b707894ef 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -622,53 +622,23 @@ static ssize_t splice_write_null(struct pipe_inode_info *pipe, struct file *out,
 	return splice_from_pipe(pipe, out, ppos, len, flags, pipe_to_null);
 }
 
-static ssize_t read_zero(struct file *file, char __user *buf,
-			 size_t count, loff_t *ppos)
+static ssize_t read_iter_zero(struct kiocb *iocb, struct iov_iter *iter)
 {
-	size_t written;
-
-	if (!count)
-		return 0;
-
-	if (!access_ok(VERIFY_WRITE, buf, count))
-		return -EFAULT;
-
-	written = 0;
-	while (count) {
-		unsigned long unwritten;
-		size_t chunk = count;
+	size_t written = 0;
 
+	while (iov_iter_count(iter)) {
+		size_t chunk = iov_iter_count(iter), n;
 		if (chunk > PAGE_SIZE)
 			chunk = PAGE_SIZE;	/* Just for latency reasons */
-		unwritten = __clear_user(buf, chunk);
-		written += chunk - unwritten;
-		if (unwritten)
-			break;
+		n = iov_iter_zero(chunk, iter);
+		if (!n && iov_iter_count(iter))
+			return written ? written : -EFAULT;
+		written += n;
 		if (signal_pending(current))
 			return written ? written : -ERESTARTSYS;
-		buf += chunk;
-		count -= chunk;
 		cond_resched();
 	}
-	return written ? written : -EFAULT;
-}
-
-static ssize_t aio_read_zero(struct kiocb *iocb, const struct iovec *iov,
-			     unsigned long nr_segs, loff_t pos)
-{
-	size_t written = 0;
-	unsigned long i;
-	ssize_t ret;
-
-	for (i = 0; i < nr_segs; i++) {
-		ret = read_zero(iocb->ki_filp, iov[i].iov_base, iov[i].iov_len,
-				&pos);
-		if (ret < 0)
-			break;
-		written += ret;
-	}
-
-	return written ? written : -EFAULT;
+	return written;
 }
 
 static int mmap_zero(struct file *file, struct vm_area_struct *vma)
@@ -738,7 +708,6 @@ static int open_port(struct inode *inode, struct file *filp)
 #define zero_lseek	null_lseek
 #define full_lseek      null_lseek
 #define write_zero	write_null
-#define read_full       read_zero
 #define aio_write_zero	aio_write_null
 #define open_mem	open_port
 #define open_kmem	open_mem
@@ -783,9 +752,9 @@ static const struct file_operations port_fops = {
 
 static const struct file_operations zero_fops = {
 	.llseek		= zero_lseek,
-	.read		= read_zero,
+	.read		= new_sync_read,
 	.write		= write_zero,
-	.aio_read	= aio_read_zero,
+	.read_iter	= read_iter_zero,
 	.aio_write	= aio_write_zero,
 	.mmap		= mmap_zero,
 };
@@ -802,7 +771,8 @@ static struct backing_dev_info zero_bdi = {
 
 static const struct file_operations full_fops = {
 	.llseek		= full_lseek,
-	.read		= read_full,
+	.read		= new_sync_read,
+	.read_iter	= read_iter_zero,
 	.write		= write_full,
 };
 
diff --git a/drivers/char/random.c b/drivers/char/random.c
index 8c86a95203a0..04645c09fe5e 100644
--- a/drivers/char/random.c
+++ b/drivers/char/random.c
@@ -874,7 +874,7 @@ static __u32 get_reg(struct fast_pool *f, struct pt_regs *regs)
 void add_interrupt_randomness(int irq, int irq_flags)
 {
 	struct entropy_store	*r;
-	struct fast_pool	*fast_pool = &__get_cpu_var(irq_randomness);
+	struct fast_pool	*fast_pool = this_cpu_ptr(&irq_randomness);
 	struct pt_regs		*regs = get_irq_regs();
 	unsigned long		now = jiffies;
 	cycles_t		cycles = random_get_entropy();
diff --git a/drivers/char/tile-srom.c b/drivers/char/tile-srom.c
index bd377472dcfb..02e76ac6d282 100644
--- a/drivers/char/tile-srom.c
+++ b/drivers/char/tile-srom.c
@@ -76,6 +76,7 @@ MODULE_LICENSE("GPL");
 
 static int srom_devs;			/* Number of SROM partitions */
 static struct cdev srom_cdev;
+static struct platform_device *srom_parent;
 static struct class *srom_class;
 static struct srom_dev *srom_devices;
 
@@ -350,7 +351,7 @@ static int srom_setup_minor(struct srom_dev *srom, int index)
 		       SROM_PAGE_SIZE_OFF, sizeof(srom->page_size)) < 0)
 		return -EIO;
 
-	dev = device_create(srom_class, &platform_bus,
+	dev = device_create(srom_class, &srom_parent->dev,
 			    MKDEV(srom_major, index), srom, "%d", index);
 	return PTR_ERR_OR_ZERO(dev);
 }
@@ -415,6 +416,13 @@ static int srom_init(void)
 	if (result < 0)
 		goto fail_chrdev;
 
+	/* Create a parent device */
+	srom_parent = platform_device_register_simple("srom", -1, NULL, 0);
+	if (IS_ERR(srom_parent)) {
+		result = PTR_ERR(srom_parent);
+		goto fail_pdev;
+	}
+
 	/* Create a sysfs class. */
 	srom_class = class_create(THIS_MODULE, "srom");
 	if (IS_ERR(srom_class)) {
@@ -438,6 +446,8 @@ fail_class:
 		device_destroy(srom_class, MKDEV(srom_major, i));
 	class_destroy(srom_class);
 fail_cdev:
+	platform_device_unregister(srom_parent);
+fail_pdev:
 	cdev_del(&srom_cdev);
 fail_chrdev:
 	unregister_chrdev_region(dev, srom_devs);
@@ -454,6 +464,7 @@ static void srom_cleanup(void)
 		device_destroy(srom_class, MKDEV(srom_major, i));
 	class_destroy(srom_class);
 	cdev_del(&srom_cdev);
+	platform_device_unregister(srom_parent);
 	unregister_chrdev_region(MKDEV(srom_major, 0), srom_devs);
 	kfree(srom_devices);
 }
diff --git a/drivers/char/tpm/xen-tpmfront.c b/drivers/char/tpm/xen-tpmfront.c
index 2064b4527040..441b44e54226 100644
--- a/drivers/char/tpm/xen-tpmfront.c
+++ b/drivers/char/tpm/xen-tpmfront.c
@@ -367,12 +367,13 @@ static const struct xenbus_device_id tpmfront_ids[] = {
 };
 MODULE_ALIAS("xen:vtpm");
 
-static DEFINE_XENBUS_DRIVER(tpmfront, ,
-		.probe = tpmfront_probe,
-		.remove = tpmfront_remove,
-		.resume = tpmfront_resume,
-		.otherend_changed = backend_changed,
-	);
+static struct xenbus_driver tpmfront_driver = {
+	.ids = tpmfront_ids,
+	.probe = tpmfront_probe,
+	.remove = tpmfront_remove,
+	.resume = tpmfront_resume,
+	.otherend_changed = backend_changed,
+};
 
 static int __init xen_tpmfront_init(void)
 {
diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index b585b4789822..bfa640023e64 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -1449,6 +1449,8 @@ static int add_port(struct ports_device *portdev, u32 id)
 	spin_lock_init(&port->outvq_lock);
 	init_waitqueue_head(&port->waitqueue);
 
+	virtio_device_ready(portdev->vdev);
+
 	/* Fill the in_vq with buffers so the host can send us data. */
 	nr_added_bufs = fill_queue(port->in_vq, &port->inbuf_lock);
 	if (!nr_added_bufs) {
@@ -2182,6 +2184,8 @@ static int virtcons_restore(struct virtio_device *vdev)
 	if (ret)
 		return ret;
 
+	virtio_device_ready(portdev->vdev);
+
 	if (use_multiport(portdev))
 		fill_queue(portdev->c_ivq, &portdev->c_ivq_lock);
 
diff --git a/drivers/char/xilinx_hwicap/xilinx_hwicap.c b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
index 9b1a5ac4881d..c07dfe5c4da3 100644
--- a/drivers/char/xilinx_hwicap/xilinx_hwicap.c
+++ b/drivers/char/xilinx_hwicap/xilinx_hwicap.c
@@ -843,7 +843,6 @@ static struct platform_driver hwicap_platform_driver = {
 	.probe = hwicap_drv_probe,
 	.remove = hwicap_drv_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name = DRIVER_NAME,
 		.of_match_table = hwicap_of_match,
 	},
diff --git a/drivers/staging/xillybus/Kconfig b/drivers/char/xillybus/Kconfig
index b53bdf12da0d..b53bdf12da0d 100644
--- a/drivers/staging/xillybus/Kconfig
+++ b/drivers/char/xillybus/Kconfig
diff --git a/drivers/staging/xillybus/Makefile b/drivers/char/xillybus/Makefile
index b68b7ebfd381..b68b7ebfd381 100644
--- a/drivers/staging/xillybus/Makefile
+++ b/drivers/char/xillybus/Makefile
diff --git a/drivers/staging/xillybus/xillybus.h b/drivers/char/xillybus/xillybus.h
index a0806b5ee2cb..b9a9eb6d4f72 100644
--- a/drivers/staging/xillybus/xillybus.h
+++ b/drivers/char/xillybus/xillybus.h
@@ -146,7 +146,6 @@ struct xilly_mapping {
 	int direction;
 };
 
-
 irqreturn_t xillybus_isr(int irq, void *data);
 
 struct xilly_endpoint *xillybus_init_endpoint(struct pci_dev *pdev,
diff --git a/drivers/staging/xillybus/xillybus_core.c b/drivers/char/xillybus/xillybus_core.c
index 0214009f7513..b827fa095f1b 100644
--- a/drivers/staging/xillybus/xillybus_core.c
+++ b/drivers/char/xillybus/xillybus_core.c
@@ -133,30 +133,22 @@ irqreturn_t xillybus_isr(int irq, void *data)
 	unsigned int msg_channel, msg_bufno, msg_data, msg_dir;
 	struct xilly_channel *channel;
 
-	/*
-	 * The endpoint structure is altered during periods when it's
-	 * guaranteed no interrupt will occur, but in theory, the cache
-	 * lines may not be updated. So a memory barrier is issued.
-	 */
-	smp_rmb();
-
 	buf = ep->msgbuf_addr;
 	buf_size = ep->msg_buf_size/sizeof(u32);
 
-
 	ep->ephw->hw_sync_sgl_for_cpu(ep,
 				      ep->msgbuf_dma_addr,
 				      ep->msg_buf_size,
 				      DMA_FROM_DEVICE);
 
-	for (i = 0; i < buf_size; i += 2)
+	for (i = 0; i < buf_size; i += 2) {
 		if (((buf[i+1] >> 28) & 0xf) != ep->msg_counter) {
 			malformed_message(ep, &buf[i]);
 			dev_warn(ep->dev,
 				 "Sending a NACK on counter %x (instead of %x) on entry %d\n",
-				((buf[i+1] >> 28) & 0xf),
-				ep->msg_counter,
-				i/2);
+				 ((buf[i+1] >> 28) & 0xf),
+				 ep->msg_counter,
+				 i/2);
 
 			if (++ep->failed_messages > 10) {
 				dev_err(ep->dev,
@@ -174,15 +166,16 @@ irqreturn_t xillybus_isr(int irq, void *data)
 			return IRQ_HANDLED;
 		} else if (buf[i] & (1 << 22)) /* Last message */
 			break;
+	}
 
 	if (i >= buf_size) {
 		dev_err(ep->dev, "Bad interrupt message. Stopping.\n");
 		return IRQ_HANDLED;
 	}
 
-	buf_size = i;
+	buf_size = i + 2;
 
-	for (i = 0; i <= buf_size; i += 2) { /* Scan through messages */
+	for (i = 0; i < buf_size; i += 2) { /* Scan through messages */
 		opcode = (buf[i] >> 24) & 0xff;
 
 		msg_dir = buf[i] & 1;
@@ -192,7 +185,6 @@ irqreturn_t xillybus_isr(int irq, void *data)
 
 		switch (opcode) {
 		case XILLYMSG_OPCODE_RELEASEBUF:
-
 			if ((msg_channel > ep->num_channels) ||
 			    (msg_channel == 0)) {
 				malformed_message(ep, &buf[i]);
@@ -337,10 +329,9 @@ static int xilly_get_dma_buffers(struct xilly_endpoint *ep,
 	struct xilly_buffer *this_buffer = NULL; /* Init to silence warning */
 
 	if (buffers) { /* Not the message buffer */
-		this_buffer = devm_kzalloc(
-			dev, bufnum * sizeof(struct xilly_buffer),
-			GFP_KERNEL);
-
+		this_buffer = devm_kcalloc(dev, bufnum,
+					   sizeof(struct xilly_buffer),
+					   GFP_KERNEL);
 		if (!this_buffer)
 			return -ENOMEM;
 	}
@@ -372,16 +363,15 @@ static int xilly_get_dma_buffers(struct xilly_endpoint *ep,
 				dev,
 				GFP_KERNEL | __GFP_DMA32 | __GFP_ZERO,
 				allocorder);
-
 			if (!s->salami)
 				return -ENOMEM;
+
 			s->left_of_salami = allocsize;
 		}
 
 		rc = ep->ephw->map_single(ep, s->salami,
 					  bytebufsize, s->direction,
 					  &dma_addr);
-
 		if (rc)
 			return rc;
 
@@ -389,7 +379,6 @@ static int xilly_get_dma_buffers(struct xilly_endpoint *ep,
 			  ep->registers + fpga_dma_bufaddr_lowaddr_reg);
 		iowrite32(((u32) ((((u64) dma_addr) >> 32) & 0xffffffff)),
 			  ep->registers + fpga_dma_bufaddr_highaddr_reg);
-		mmiowb();
 
 		if (buffers) { /* Not the message buffer */
 			this_buffer->addr = s->salami;
@@ -410,13 +399,12 @@ static int xilly_get_dma_buffers(struct xilly_endpoint *ep,
 		s->left_of_salami -= bytebufsize;
 		s->salami += bytebufsize;
 	}
-	return 0; /* Success */
+	return 0;
 }
 
 static int xilly_setupchannels(struct xilly_endpoint *ep,
 			       unsigned char *chandesc,
-			       int entries
-	)
+			       int entries)
 {
 	struct device *dev = ep->dev;
 	int i, entry, rc;
@@ -443,18 +431,16 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 		.regdirection = 0x80000000,
 	};
 
-	channel = devm_kzalloc(dev, ep->num_channels *
+	channel = devm_kcalloc(dev, ep->num_channels,
 			       sizeof(struct xilly_channel), GFP_KERNEL);
-
 	if (!channel)
-		goto memfail;
+		return -ENOMEM;
 
-	ep->channels = devm_kzalloc(dev, (ep->num_channels + 1) *
+	ep->channels = devm_kcalloc(dev, ep->num_channels + 1,
 				    sizeof(struct xilly_channel *),
 				    GFP_KERNEL);
-
 	if (!ep->channels)
-		goto memfail;
+		return -ENOMEM;
 
 	ep->channels[0] = NULL; /* Channel 0 is message buf. */
 
@@ -526,12 +512,11 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 			bytebufsize = channel->rd_buf_size = bufsize *
 				(1 << channel->log2_element_size);
 
-			buffers = devm_kzalloc(dev,
-				bufnum * sizeof(struct xilly_buffer *),
-				GFP_KERNEL);
-
+			buffers = devm_kcalloc(dev, bufnum,
+					       sizeof(struct xilly_buffer *),
+					       GFP_KERNEL);
 			if (!buffers)
-				goto memfail;
+				return -ENOMEM;
 		} else {
 			bytebufsize = bufsize << 2;
 		}
@@ -566,7 +551,7 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 		}
 
 		if (rc)
-			goto memfail;
+			return -ENOMEM;
 	}
 
 	if (!msg_buf_done) {
@@ -575,15 +560,10 @@ static int xilly_setupchannels(struct xilly_endpoint *ep,
 		return -ENODEV;
 	}
 	return 0;
-
-memfail:
-	dev_err(ep->dev,
-		"Failed to assign DMA buffer memory. Aborting.\n");
-	return -ENOMEM;
 }
 
-static void xilly_scan_idt(struct xilly_endpoint *endpoint,
-			   struct xilly_idt_handle *idt_handle)
+static int xilly_scan_idt(struct xilly_endpoint *endpoint,
+			  struct xilly_idt_handle *idt_handle)
 {
 	int count = 0;
 	unsigned char *idt = endpoint->channels[1]->wr_buffers[0]->addr;
@@ -607,53 +587,49 @@ static void xilly_scan_idt(struct xilly_endpoint *endpoint,
 	if (scan > end_of_idt) {
 		dev_err(endpoint->dev,
 			"IDT device name list overflow. Aborting.\n");
-		idt_handle->chandesc = NULL;
-		return;
+		return -ENODEV;
 	}
 	idt_handle->chandesc = scan;
 
 	len = endpoint->idtlen - (3 + ((int) (scan - idt)));
 
 	if (len & 0x03) {
-		idt_handle->chandesc = NULL;
-
 		dev_err(endpoint->dev,
 			"Corrupt IDT device name list. Aborting.\n");
+		return -ENODEV;
 	}
 
 	idt_handle->entries = len >> 2;
-
 	endpoint->num_channels = count;
+
+	return 0;
 }
 
 static int xilly_obtain_idt(struct xilly_endpoint *endpoint)
 {
-	int rc = 0;
 	struct xilly_channel *channel;
 	unsigned char *version;
+	long t;
 
 	channel = endpoint->channels[1]; /* This should be generated ad-hoc */
 
 	channel->wr_sleepy = 1;
-	wmb(); /* Setting wr_sleepy must come before the command */
 
 	iowrite32(1 |
-		   (3 << 24), /* Opcode 3 for channel 0 = Send IDT */
-		   endpoint->registers + fpga_buf_ctrl_reg);
-	mmiowb(); /* Just to appear safe */
+		  (3 << 24), /* Opcode 3 for channel 0 = Send IDT */
+		  endpoint->registers + fpga_buf_ctrl_reg);
 
-	wait_event_interruptible_timeout(channel->wr_wait,
-					 (!channel->wr_sleepy),
-					 XILLY_TIMEOUT);
+	t = wait_event_interruptible_timeout(channel->wr_wait,
+					     (!channel->wr_sleepy),
+					     XILLY_TIMEOUT);
 
-	if (channel->wr_sleepy) {
+	if (t <= 0) {
 		dev_err(endpoint->dev, "Failed to obtain IDT. Aborting.\n");
 
 		if (endpoint->fatal_error)
 			return -EIO;
 
-		rc = -ENODEV;
-		return rc;
+		return -ENODEV;
 	}
 
 	endpoint->ephw->hw_sync_sgl_for_cpu(
@@ -665,16 +641,14 @@ static int xilly_obtain_idt(struct xilly_endpoint *endpoint)
 	if (channel->wr_buffers[0]->end_offset != endpoint->idtlen) {
 		dev_err(endpoint->dev,
 			"IDT length mismatch (%d != %d). Aborting.\n",
-		       channel->wr_buffers[0]->end_offset, endpoint->idtlen);
-		rc = -ENODEV;
-		return rc;
+			channel->wr_buffers[0]->end_offset, endpoint->idtlen);
+		return -ENODEV;
 	}
 
 	if (crc32_le(~0, channel->wr_buffers[0]->addr,
 		     endpoint->idtlen+1) != 0) {
 		dev_err(endpoint->dev, "IDT failed CRC check. Aborting.\n");
-		rc = -ENODEV;
-		return rc;
+		return -ENODEV;
 	}
 
 	version = channel->wr_buffers[0]->addr;
@@ -683,12 +657,11 @@ static int xilly_obtain_idt(struct xilly_endpoint *endpoint)
 	if (*version > 0x82) {
 		dev_err(endpoint->dev,
 			"No support for IDT version 0x%02x. Maybe the xillybus driver needs an upgarde. Aborting.\n",
-		       (int) *version);
-		rc = -ENODEV;
-		return rc;
+			*version);
+		return -ENODEV;
 	}
 
-	return 0; /* Success */
+	return 0;
 }
 
 static ssize_t xillybus_read(struct file *filp, char __user *userbuf,
@@ -713,12 +686,9 @@ static ssize_t xillybus_read(struct file *filp, char __user *userbuf,
 	deadline = jiffies + 1 + XILLY_RX_TIMEOUT;
 
 	rc = mutex_lock_interruptible(&channel->wr_mutex);
-
 	if (rc)
 		return rc;
 
-	rc = 0; /* Just to be clear about it. Compiler optimizes this out */
-
 	while (1) { /* Note that we may drop mutex within this loop */
 		int bytes_to_do = count - bytes_done;
 
@@ -793,14 +763,11 @@ static ssize_t xillybus_read(struct file *filp, char __user *userbuf,
 			bytes_done += howmany;
 
 			if (bufferdone) {
-				channel->endpoint->ephw->
-					hw_sync_sgl_for_device
-					(
-						channel->endpoint,
-						channel->wr_buffers[bufidx]->
-						dma_addr,
-						channel->wr_buf_size,
-						DMA_FROM_DEVICE);
+				channel->endpoint->ephw->hw_sync_sgl_for_device(
+					channel->endpoint,
+					channel->wr_buffers[bufidx]->dma_addr,
+					channel->wr_buf_size,
+					DMA_FROM_DEVICE);
 
 				/*
 				 * Tell FPGA the buffer is done with. It's an
@@ -810,11 +777,10 @@ static ssize_t xillybus_read(struct file *filp, char __user *userbuf,
 				 * the channel-specific mutex.
 				 */
 
-				iowrite32(1 | (channel->chan_num << 1)
-					   | (bufidx << 12),
-					   channel->endpoint->registers +
-					   fpga_buf_ctrl_reg);
-				mmiowb(); /* Just to appear safe */
+				iowrite32(1 | (channel->chan_num << 1) |
+					  (bufidx << 12),
+					  channel->endpoint->registers +
+					  fpga_buf_ctrl_reg);
 			}
 
 			if (rc) {
@@ -851,7 +817,7 @@ static ssize_t xillybus_read(struct file *filp, char __user *userbuf,
 			if (ready)
 				goto desperate;
 
-			bytes_done = -EAGAIN;
+			rc = -EAGAIN;
 			break;
 		}
 
@@ -895,26 +861,21 @@ static ssize_t xillybus_read(struct file *filp, char __user *userbuf,
 
 			if (channel->wr_synchronous ||
 			    (offsetlimit < (buf_elements - 1))) {
-
 				mutex_lock(&channel->endpoint->register_mutex);
 
 				iowrite32(offsetlimit,
 					  channel->endpoint->registers +
 					  fpga_buf_offset_reg);
-				mmiowb();
 
 				iowrite32(1 | (channel->chan_num << 1) |
-					   (2 << 24) |  /* 2 = offset limit */
-					   (waiting_bufidx << 12),
-					   channel->endpoint->registers +
-					   fpga_buf_ctrl_reg);
-
-				mmiowb(); /* Just to appear safe */
+					  (2 << 24) |  /* 2 = offset limit */
+					  (waiting_bufidx << 12),
+					  channel->endpoint->registers +
+					  fpga_buf_ctrl_reg);
 
 				mutex_unlock(&channel->endpoint->
 					     register_mutex);
 			}
-
 		}
 
 		/*
@@ -925,7 +886,6 @@ static ssize_t xillybus_read(struct file *filp, char __user *userbuf,
 
 		if (!channel->wr_allow_partial ||
 		    (no_time_left && (bytes_done == 0))) {
-
 			/*
 			 * This do-loop will run more than once if another
 			 * thread reasserted wr_sleepy before we got the mutex
@@ -972,7 +932,7 @@ interrupted: /* Mutex is not held if got here */
 					(!channel->wr_sleepy),
 					left_to_sleep);
 
-			if (!channel->wr_sleepy)
+			if (left_to_sleep > 0) /* wr_sleepy deasserted */
 				continue;
 
 			if (left_to_sleep < 0) { /* Interrupt */
@@ -997,18 +957,18 @@ desperate:
 			 */
 
 			iowrite32(1 | (channel->chan_num << 1) |
-				   (3 << 24) |  /* Opcode 3, flush it all! */
-				   (waiting_bufidx << 12),
-				   channel->endpoint->registers +
-				   fpga_buf_ctrl_reg);
-			mmiowb(); /* Just to appear safe */
+				  (3 << 24) |  /* Opcode 3, flush it all! */
+				  (waiting_bufidx << 12),
+				  channel->endpoint->registers +
+				  fpga_buf_ctrl_reg);
 		}
 
 		/*
-		 * Formally speaking, we should block for data at this point.
-		 * But to keep the code cleaner, we'll just finish the loop,
-		 * make the unlikely check for data, and then block at the
-		 * usual place.
+		 * Reaching here means that we *do* have data in the buffer,
+		 * but the "partial" flag disallows returning less than
+		 * required. And we don't have as much. So loop again,
+		 * which is likely to end up blocking indefinitely until
+		 * enough data has arrived.
 		 */
 	}
 
@@ -1017,6 +977,9 @@ desperate:
 	if (channel->endpoint->fatal_error)
 		return -EIO;
 
+	if (rc)
+		return rc;
+
 	return bytes_done;
 }
 
@@ -1029,7 +992,7 @@ desperate:
 
 static int xillybus_myflush(struct xilly_channel *channel, long timeout)
 {
-	int rc = 0;
+	int rc;
 	unsigned long flags;
 
 	int end_offset_plus1;
@@ -1041,7 +1004,6 @@ static int xillybus_myflush(struct xilly_channel *channel, long timeout)
 	if (channel->endpoint->fatal_error)
 		return -EIO;
 	rc = mutex_lock_interruptible(&channel->rd_mutex);
-
 	if (rc)
 		return rc;
 
@@ -1056,7 +1018,9 @@ static int xillybus_myflush(struct xilly_channel *channel, long timeout)
 
 	bufidx = channel->rd_host_buf_idx;
 
-	bufidx_minus1 = (bufidx == 0) ? channel->num_rd_buffers - 1 : bufidx-1;
+	bufidx_minus1 = (bufidx == 0) ?
+		channel->num_rd_buffers - 1 :
+		bufidx - 1;
 
 	end_offset_plus1 = channel->rd_host_buf_pos >>
 		channel->log2_element_size;
@@ -1113,26 +1077,24 @@ static int xillybus_myflush(struct xilly_channel *channel, long timeout)
 
 		iowrite32(end_offset_plus1 - 1,
 			  channel->endpoint->registers + fpga_buf_offset_reg);
-		mmiowb();
 
 		iowrite32((channel->chan_num << 1) | /* Channel ID */
-			   (2 << 24) |  /* Opcode 2, submit buffer */
-			   (bufidx << 12),
-			   channel->endpoint->registers + fpga_buf_ctrl_reg);
-		mmiowb(); /* Just to appear safe */
+			  (2 << 24) |  /* Opcode 2, submit buffer */
+			  (bufidx << 12),
+			  channel->endpoint->registers + fpga_buf_ctrl_reg);
 
 		mutex_unlock(&channel->endpoint->register_mutex);
-	} else if (bufidx == 0)
+	} else if (bufidx == 0) {
 		bufidx = channel->num_rd_buffers - 1;
-	else
+	} else {
 		bufidx--;
+	}
 
 	channel->rd_host_buf_pos = new_rd_host_buf_pos;
 
 	if (timeout < 0)
 		goto done; /* Autoflush */
 
-
 	/*
 	 * bufidx is now the last buffer written to (or equal to
 	 * rd_fpga_buf_idx if buffer was never written to), and
@@ -1141,8 +1103,6 @@ static int xillybus_myflush(struct xilly_channel *channel, long timeout)
 	 * If bufidx == channel->rd_fpga_buf_idx we're either empty or full.
 	 */
 
-	rc = 0;
-
 	while (1) { /* Loop waiting for draining of buffers */
 		spin_lock_irqsave(&channel->rd_spinlock, flags);
 
@@ -1173,7 +1133,7 @@ static int xillybus_myflush(struct xilly_channel *channel, long timeout)
 				 (!channel->rd_full),
 				 timeout) == 0) {
 			dev_warn(channel->endpoint->dev,
-				"Timed out while flushing. Output data may be lost.\n");
+				 "Timed out while flushing. Output data may be lost.\n");
 
 			rc = -ETIMEDOUT;
 			break;
@@ -1211,7 +1171,6 @@ static void xillybus_autoflush(struct work_struct *work)
 	int rc;
 
 	rc = xillybus_myflush(channel, -1);
-
 	if (rc == -EINTR)
 		dev_warn(channel->endpoint->dev,
 			 "Autoflush failed because work queue thread got a signal.\n");
@@ -1238,12 +1197,9 @@ static ssize_t xillybus_write(struct file *filp, const char __user *userbuf,
 		return -EIO;
 
 	rc = mutex_lock_interruptible(&channel->rd_mutex);
-
 	if (rc)
 		return rc;
 
-	rc = 0; /* Just to be clear about it. Compiler optimizes this out */
-
 	while (1) {
 		int bytes_to_do = count - bytes_done;
 
@@ -1351,26 +1307,23 @@ static ssize_t xillybus_write(struct file *filp, const char __user *userbuf,
 			bytes_done += howmany;
 
 			if (bufferdone) {
-				channel->endpoint->ephw->
-					hw_sync_sgl_for_device(
-						channel->endpoint,
-						channel->rd_buffers[bufidx]->
-						dma_addr,
-						channel->rd_buf_size,
-						DMA_TO_DEVICE);
+				channel->endpoint->ephw->hw_sync_sgl_for_device(
+					channel->endpoint,
+					channel->rd_buffers[bufidx]->dma_addr,
+					channel->rd_buf_size,
+					DMA_TO_DEVICE);
 
 				mutex_lock(&channel->endpoint->register_mutex);
 
 				iowrite32(end_offset_plus1 - 1,
 					  channel->endpoint->registers +
 					  fpga_buf_offset_reg);
-				mmiowb();
+
 				iowrite32((channel->chan_num << 1) |
-					   (2 << 24) |  /* 2 = submit buffer */
-					   (bufidx << 12),
-					   channel->endpoint->registers +
-					   fpga_buf_ctrl_reg);
-				mmiowb(); /* Just to appear safe */
+					  (2 << 24) |  /* 2 = submit buffer */
+					  (bufidx << 12),
+					  channel->endpoint->registers +
+					  fpga_buf_ctrl_reg);
 
 				mutex_unlock(&channel->endpoint->
 					     register_mutex);
@@ -1411,14 +1364,12 @@ static ssize_t xillybus_write(struct file *filp, const char __user *userbuf,
 		 */
 
 		if (filp->f_flags & O_NONBLOCK) {
-			bytes_done = -EAGAIN;
+			rc = -EAGAIN;
 			break;
 		}
 
-		wait_event_interruptible(channel->rd_wait,
-					 (!channel->rd_full));
-
-		if (channel->rd_full) {
+		if (wait_event_interruptible(channel->rd_wait,
+					     (!channel->rd_full))) {
 			mutex_unlock(&channel->rd_mutex);
 
 			if (channel->endpoint->fatal_error)
@@ -1437,6 +1388,12 @@ static ssize_t xillybus_write(struct file *filp, const char __user *userbuf,
 				   &channel->rd_workitem,
 				   XILLY_RX_TIMEOUT);
 
+	if (channel->endpoint->fatal_error)
+		return -EIO;
+
+	if (rc)
+		return rc;
+
 	if ((channel->rd_synchronous) && (bytes_done > 0)) {
 		rc = xillybus_myflush(filp->private_data, 0); /* No timeout */
 
@@ -1444,9 +1401,6 @@ static ssize_t xillybus_write(struct file *filp, const char __user *userbuf,
 			return rc;
 	}
 
-	if (channel->endpoint->fatal_error)
-		return -EIO;
-
 	return bytes_done;
 }
 
@@ -1484,7 +1438,6 @@ static int xillybus_open(struct inode *inode, struct file *filp)
 	channel = endpoint->channels[1 + minor - endpoint->lowest_minor];
 	filp->private_data = channel;
 
-
 	/*
 	 * It gets complicated because:
 	 * 1. We don't want to take a mutex we don't have to
@@ -1545,7 +1498,6 @@ static int xillybus_open(struct inode *inode, struct file *filp)
 		goto unlock;
 	}
 
-
 	if (filp->f_mode & FMODE_READ) {
 		if (channel->wr_ref_count == 0) { /* First open of file */
 			/* Move the host to first buffer */
@@ -1566,7 +1518,6 @@ static int xillybus_open(struct inode *inode, struct file *filp)
 				  ((channel->wr_synchronous & 1) << 23),
 				  channel->endpoint->registers +
 				  fpga_buf_ctrl_reg);
-			mmiowb(); /* Just to appear safe */
 		}
 
 		channel->wr_ref_count++;
@@ -1588,7 +1539,6 @@ static int xillybus_open(struct inode *inode, struct file *filp)
 				  (4 << 24),   /* Opcode 4, open channel */
 				  channel->endpoint->registers +
 				  fpga_buf_ctrl_reg);
-			mmiowb(); /* Just to appear safe */
 		}
 
 		channel->rd_ref_count++;
@@ -1609,7 +1559,6 @@ unlock_wr:
 
 static int xillybus_release(struct inode *inode, struct file *filp)
 {
-	int rc;
 	unsigned long flags;
 	struct xilly_channel *channel = filp->private_data;
 
@@ -1620,18 +1569,11 @@ static int xillybus_release(struct inode *inode, struct file *filp)
 		return -EIO;
 
 	if (filp->f_mode & FMODE_WRITE) {
-		rc = mutex_lock_interruptible(&channel->rd_mutex);
-
-		if (rc) {
-			dev_warn(channel->endpoint->dev,
-				 "Failed to close file. Hardware left in messy state.\n");
-			return rc;
-		}
+		mutex_lock(&channel->rd_mutex);
 
 		channel->rd_ref_count--;
 
 		if (channel->rd_ref_count == 0) {
-
 			/*
 			 * We rely on the kernel calling flush()
 			 * before we get here.
@@ -1641,28 +1583,20 @@ static int xillybus_release(struct inode *inode, struct file *filp)
 				  (5 << 24),  /* Opcode 5, close channel */
 				  channel->endpoint->registers +
 				  fpga_buf_ctrl_reg);
-			mmiowb(); /* Just to appear safe */
 		}
 		mutex_unlock(&channel->rd_mutex);
 	}
 
 	if (filp->f_mode & FMODE_READ) {
-		rc = mutex_lock_interruptible(&channel->wr_mutex);
-		if (rc) {
-			dev_warn(channel->endpoint->dev,
-				 "Failed to close file. Hardware left in messy state.\n");
-			return rc;
-		}
+		mutex_lock(&channel->wr_mutex);
 
 		channel->wr_ref_count--;
 
 		if (channel->wr_ref_count == 0) {
-
 			iowrite32(1 | (channel->chan_num << 1) |
-				   (5 << 24),  /* Opcode 5, close channel */
-				   channel->endpoint->registers +
-				   fpga_buf_ctrl_reg);
-			mmiowb(); /* Just to appear safe */
+				  (5 << 24),  /* Opcode 5, close channel */
+				  channel->endpoint->registers +
+				  fpga_buf_ctrl_reg);
 
 			/*
 			 * This is crazily cautious: We make sure that not
@@ -1723,6 +1657,7 @@ static int xillybus_release(struct inode *inode, struct file *filp)
 
 	return 0;
 }
+
 static loff_t xillybus_llseek(struct file *filp, loff_t offset, int whence)
 {
 	struct xilly_channel *channel = filp->private_data;
@@ -1743,13 +1678,13 @@ static loff_t xillybus_llseek(struct file *filp, loff_t offset, int whence)
 	mutex_lock(&channel->rd_mutex);
 
 	switch (whence) {
-	case 0:
+	case SEEK_SET:
 		pos = offset;
 		break;
-	case 1:
+	case SEEK_CUR:
 		pos += offset;
 		break;
-	case 2:
+	case SEEK_END:
 		pos = offset; /* Going to the end => to the beginning */
 		break;
 	default:
@@ -1767,11 +1702,10 @@ static loff_t xillybus_llseek(struct file *filp, loff_t offset, int whence)
 
 	iowrite32(pos >> channel->log2_element_size,
 		  channel->endpoint->registers + fpga_buf_offset_reg);
-	mmiowb();
+
 	iowrite32((channel->chan_num << 1) |
 		  (6 << 24),  /* Opcode 6, set address */
 		  channel->endpoint->registers + fpga_buf_ctrl_reg);
-	mmiowb(); /* Just to appear safe */
 
 	mutex_unlock(&channel->endpoint->register_mutex);
 
@@ -1876,10 +1810,9 @@ static int xillybus_init_chrdev(struct xilly_endpoint *endpoint,
 	rc = alloc_chrdev_region(&dev, 0, /* minor start */
 				 endpoint->num_channels,
 				 xillyname);
-
 	if (rc) {
 		dev_warn(endpoint->dev, "Failed to obtain major/minors");
-		goto error1;
+		return rc;
 	}
 
 	endpoint->major = major = MAJOR(dev);
@@ -1891,7 +1824,7 @@ static int xillybus_init_chrdev(struct xilly_endpoint *endpoint,
 		      endpoint->num_channels);
 	if (rc) {
 		dev_warn(endpoint->dev, "Failed to add cdev. Aborting.\n");
-		goto error2;
+		goto unregister_chrdev;
 	}
 
 	idt++;
@@ -1916,7 +1849,8 @@ static int xillybus_init_chrdev(struct xilly_endpoint *endpoint,
 			dev_warn(endpoint->dev,
 				 "Failed to create %s device. Aborting.\n",
 				 devname);
-			goto error3;
+			rc = -ENODEV;
+			goto unroll_device_create;
 		}
 	}
 
@@ -1924,15 +1858,14 @@ static int xillybus_init_chrdev(struct xilly_endpoint *endpoint,
 		 endpoint->num_channels);
 	return 0; /* succeed */
 
-error3:
+unroll_device_create:
 	devnum--; i--;
 	for (; devnum >= 0; devnum--, i--)
 		device_destroy(xillybus_class, MKDEV(major, i));
 
 	cdev_del(&endpoint->cdev);
-error2:
+unregister_chrdev:
 	unregister_chrdev_region(MKDEV(major, minor), endpoint->num_channels);
-error1:
 
 	return rc;
 }
@@ -1954,7 +1887,6 @@ static void xillybus_cleanup_chrdev(struct xilly_endpoint *endpoint)
 		 endpoint->num_channels);
 }
 
-
 struct xilly_endpoint *xillybus_init_endpoint(struct pci_dev *pdev,
 					      struct device *dev,
 					      struct xilly_endpoint_hardware
@@ -1982,27 +1914,28 @@ EXPORT_SYMBOL(xillybus_init_endpoint);
 
 static int xilly_quiesce(struct xilly_endpoint *endpoint)
 {
+	long t;
+
 	endpoint->idtlen = -1;
-	wmb(); /* Make sure idtlen is set before sending command */
+
 	iowrite32((u32) (endpoint->dma_using_dac & 0x0001),
 		  endpoint->registers + fpga_dma_control_reg);
-	mmiowb();
 
-	wait_event_interruptible_timeout(endpoint->ep_wait,
-					 (endpoint->idtlen >= 0),
-					 XILLY_TIMEOUT);
-
-	if (endpoint->idtlen < 0) {
+	t = wait_event_interruptible_timeout(endpoint->ep_wait,
+					     (endpoint->idtlen >= 0),
+					     XILLY_TIMEOUT);
+	if (t <= 0) {
 		dev_err(endpoint->dev,
 			"Failed to quiesce the device on exit.\n");
 		return -ENODEV;
 	}
-	return 0; /* Success */
+	return 0;
 }
 
 int xillybus_endpoint_discovery(struct xilly_endpoint *endpoint)
 {
-	int rc = 0;
+	int rc;
+	long t;
 
 	void *bootstrap_resources;
 	int idtbuffersize = (1 << PAGE_SHIFT);
@@ -2026,7 +1959,6 @@ int xillybus_endpoint_discovery(struct xilly_endpoint *endpoint)
 	 */
 
 	iowrite32(1, endpoint->registers + fpga_endian_reg);
-	mmiowb(); /* Writes below are affected by the one above. */
 
 	/* Bootstrap phase I: Allocate temporary message buffer */
 
@@ -2037,39 +1969,32 @@ int xillybus_endpoint_discovery(struct xilly_endpoint *endpoint)
 	endpoint->num_channels = 0;
 
 	rc = xilly_setupchannels(endpoint, bogus_idt, 1);
-
 	if (rc)
 		return rc;
 
 	/* Clear the message subsystem (and counter in particular) */
 	iowrite32(0x04, endpoint->registers + fpga_msg_ctrl_reg);
-	mmiowb();
 
 	endpoint->idtlen = -1;
 
-	smp_wmb();
-
 	/*
 	 * Set DMA 32/64 bit mode, quiesce the device (?!) and get IDT
 	 * buffer size.
 	 */
 	iowrite32((u32) (endpoint->dma_using_dac & 0x0001),
-		   endpoint->registers + fpga_dma_control_reg);
-	mmiowb();
-
-	wait_event_interruptible_timeout(endpoint->ep_wait,
-					 (endpoint->idtlen >= 0),
-					 XILLY_TIMEOUT);
+		  endpoint->registers + fpga_dma_control_reg);
 
-	if (endpoint->idtlen < 0) {
+	t = wait_event_interruptible_timeout(endpoint->ep_wait,
+					     (endpoint->idtlen >= 0),
+					     XILLY_TIMEOUT);
+	if (t <= 0) {
 		dev_err(endpoint->dev, "No response from FPGA. Aborting.\n");
 		return -ENODEV;
 	}
 
 	/* Enable DMA */
 	iowrite32((u32) (0x0002 | (endpoint->dma_using_dac & 0x0001)),
-		   endpoint->registers + fpga_dma_control_reg);
-	mmiowb();
+		  endpoint->registers + fpga_dma_control_reg);
 
 	/* Bootstrap phase II: Allocate buffer for IDT and obtain it */
 	while (endpoint->idtlen >= idtbuffersize) {
@@ -2080,23 +2005,16 @@ int xillybus_endpoint_discovery(struct xilly_endpoint *endpoint)
 	endpoint->num_channels = 1;
 
 	rc = xilly_setupchannels(endpoint, bogus_idt, 2);
-
 	if (rc)
 		goto failed_idt;
 
-	smp_wmb();
-
 	rc = xilly_obtain_idt(endpoint);
-
 	if (rc)
 		goto failed_idt;
 
-	xilly_scan_idt(endpoint, &idt_handle);
-
-	if (!idt_handle.chandesc) {
-		rc = -ENODEV;
+	rc = xilly_scan_idt(endpoint, &idt_handle);
+	if (rc)
 		goto failed_idt;
-	}
 
 	devres_close_group(dev, bootstrap_resources);
 
@@ -2105,12 +2023,9 @@ int xillybus_endpoint_discovery(struct xilly_endpoint *endpoint)
 	rc = xilly_setupchannels(endpoint,
 				 idt_handle.chandesc,
 				 idt_handle.entries);
-
 	if (rc)
 		goto failed_idt;
 
-	smp_wmb(); /* mutex_lock below should suffice, but won't hurt.*/
-
 	/*
 	 * endpoint is now completely configured. We put it on the list
 	 * available to open() before registering the char device(s)
@@ -2121,7 +2036,6 @@ int xillybus_endpoint_discovery(struct xilly_endpoint *endpoint)
 	mutex_unlock(&ep_list_lock);
 
 	rc = xillybus_init_chrdev(endpoint, idt_handle.idt);
-
 	if (rc)
 		goto failed_chrdevs;
 
@@ -2162,25 +2076,19 @@ EXPORT_SYMBOL(xillybus_endpoint_remove);
 
 static int __init xillybus_init(void)
 {
-	int rc = 0;
-
 	mutex_init(&ep_list_lock);
 
 	xillybus_class = class_create(THIS_MODULE, xillyname);
-	if (IS_ERR(xillybus_class)) {
-		rc = PTR_ERR(xillybus_class);
-		pr_warn("Failed to register class xillybus\n");
-
-		return rc;
-	}
+	if (IS_ERR(xillybus_class))
+		return PTR_ERR(xillybus_class);
 
 	xillybus_wq = alloc_workqueue(xillyname, 0, 0);
 	if (!xillybus_wq) {
 		class_destroy(xillybus_class);
-		rc = -ENOMEM;
+		return -ENOMEM;
 	}
 
-	return rc;
+	return 0;
 }
 
 static void __exit xillybus_exit(void)
diff --git a/drivers/staging/xillybus/xillybus_of.c b/drivers/char/xillybus/xillybus_of.c
index e0ae2346b820..1ca0c7a4f1be 100644
--- a/drivers/staging/xillybus/xillybus_of.c
+++ b/drivers/char/xillybus/xillybus_of.c
@@ -106,9 +106,10 @@ static int xilly_map_single_of(struct xilly_endpoint *ep,
 	if (rc) {
 		dma_unmap_single(ep->dev, addr, size, direction);
 		kfree(this);
+		return rc;
 	}
 
-	return rc;
+	return 0;
 }
 
 static struct xilly_endpoint_hardware of_hw = {
@@ -129,7 +130,7 @@ static int xilly_drv_probe(struct platform_device *op)
 {
 	struct device *dev = &op->dev;
 	struct xilly_endpoint *endpoint;
-	int rc = 0;
+	int rc;
 	int irq;
 	struct resource res;
 	struct xilly_endpoint_hardware *ephw = &of_hw;
diff --git a/drivers/staging/xillybus/xillybus_pcie.c b/drivers/char/xillybus/xillybus_pcie.c
index 96c2c9f67e0c..d8266bc2ae35 100644
--- a/drivers/staging/xillybus/xillybus_pcie.c
+++ b/drivers/char/xillybus/xillybus_pcie.c
@@ -98,7 +98,7 @@ static int xilly_map_single_pci(struct xilly_endpoint *ep,
 	int pci_direction;
 	dma_addr_t addr;
 	struct xilly_mapping *this;
-	int rc = 0;
+	int rc;
 
 	this = kzalloc(sizeof(*this), GFP_KERNEL);
 	if (!this)
@@ -121,13 +121,13 @@ static int xilly_map_single_pci(struct xilly_endpoint *ep,
 	*ret_dma_handle = addr;
 
 	rc = devm_add_action(ep->dev, xilly_pci_unmap, this);
-
 	if (rc) {
 		pci_unmap_single(ep->pdev, addr, size, pci_direction);
 		kfree(this);
+		return rc;
 	}
 
-	return rc;
+	return 0;
 }
 
 static struct xilly_endpoint_hardware pci_hw = {
@@ -138,10 +138,10 @@ static struct xilly_endpoint_hardware pci_hw = {
 };
 
 static int xilly_probe(struct pci_dev *pdev,
-				 const struct pci_device_id *ent)
+		       const struct pci_device_id *ent)
 {
 	struct xilly_endpoint *endpoint;
-	int rc = 0;
+	int rc;
 
 	endpoint = xillybus_init_endpoint(pdev, &pdev->dev, &pci_hw);
 
@@ -151,7 +151,6 @@ static int xilly_probe(struct pci_dev *pdev,
 	pci_set_drvdata(pdev, endpoint);
 
 	rc = pcim_enable_device(pdev);
-
 	if (rc) {
 		dev_err(endpoint->dev,
 			"pcim_enable_device() failed. Aborting.\n");
@@ -187,7 +186,6 @@ static int xilly_probe(struct pci_dev *pdev,
 	}
 	rc = devm_request_irq(&pdev->dev, pdev->irq, xillybus_isr, 0,
 			      xillyname, endpoint);
-
 	if (rc) {
 		dev_err(endpoint->dev,
 			"Failed to register MSI handler. Aborting.\n");
@@ -201,9 +199,9 @@ static int xilly_probe(struct pci_dev *pdev,
 	 * nobody and use 32 bits DMA addressing in any case.
 	 */
 
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32)))
+	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
 		endpoint->dma_using_dac = 0;
-	else {
+	} else {
 		dev_err(endpoint->dev, "Failed to set DMA mask. Aborting.\n");
 		return -ENODEV;
 	}
diff --git a/drivers/clk/Kconfig b/drivers/clk/Kconfig
index cfd3af7b2cbd..455fd17d938e 100644
--- a/drivers/clk/Kconfig
+++ b/drivers/clk/Kconfig
@@ -32,12 +32,32 @@ config COMMON_CLK_WM831X
 
 source "drivers/clk/versatile/Kconfig"
 
+config COMMON_CLK_MAX_GEN
+        bool
+
 config COMMON_CLK_MAX77686
 	tristate "Clock driver for Maxim 77686 MFD"
 	depends on MFD_MAX77686
+	select COMMON_CLK_MAX_GEN
 	---help---
 	  This driver supports Maxim 77686 crystal oscillator clock. 
 
+config COMMON_CLK_MAX77802
+	tristate "Clock driver for Maxim 77802 PMIC"
+	depends on MFD_MAX77686
+	select COMMON_CLK_MAX_GEN
+	---help---
+	  This driver supports Maxim 77802 crystal oscillator clock.
+
+config COMMON_CLK_RK808
+	tristate "Clock driver for RK808"
+	depends on MFD_RK808
+	---help---
+	  This driver supports RK808 crystal oscillator clock. These
+	  multi-function devices have two fixed-rate oscillators,
+	  clocked at 32KHz each. Clkout1 is always on, Clkout2 can off
+	  by control register.
+
 config COMMON_CLK_SI5351
 	tristate "Clock driver for SiLabs 5351A/B/C"
 	depends on I2C
@@ -109,6 +129,11 @@ config COMMON_CLK_PALMAS
 	  This driver supports TI Palmas devices 32KHz output KG and KG_AUDIO
 	  using common clock framework.
 
+config COMMON_CLK_PXA
+	def_bool COMMON_CLK && ARCH_PXA
+	---help---
+	  Sypport for the Marvell PXA SoC.
+
 source "drivers/clk/qcom/Kconfig"
 
 endmenu
diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index f537a0b1f798..d5fba5bc6e1b 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_COMMON_CLK)	+= clk-gate.o
 obj-$(CONFIG_COMMON_CLK)	+= clk-mux.o
 obj-$(CONFIG_COMMON_CLK)	+= clk-composite.o
 obj-$(CONFIG_COMMON_CLK)	+= clk-fractional-divider.o
+obj-$(CONFIG_COMMON_CLK)	+= clk-gpio-gate.o
 ifeq ($(CONFIG_OF), y)
 obj-$(CONFIG_COMMON_CLK)	+= clk-conf.o
 endif
@@ -22,12 +23,15 @@ obj-$(CONFIG_ARCH_CLPS711X)		+= clk-clps711x.o
 obj-$(CONFIG_ARCH_EFM32)		+= clk-efm32gg.o
 obj-$(CONFIG_ARCH_HIGHBANK)		+= clk-highbank.o
 obj-$(CONFIG_MACH_LOONGSON1)		+= clk-ls1x.o
+obj-$(CONFIG_COMMON_CLK_MAX_GEN)	+= clk-max-gen.o
 obj-$(CONFIG_COMMON_CLK_MAX77686)	+= clk-max77686.o
+obj-$(CONFIG_COMMON_CLK_MAX77802)	+= clk-max77802.o
 obj-$(CONFIG_ARCH_MOXART)		+= clk-moxart.o
 obj-$(CONFIG_ARCH_NOMADIK)		+= clk-nomadik.o
 obj-$(CONFIG_ARCH_NSPIRE)		+= clk-nspire.o
 obj-$(CONFIG_COMMON_CLK_PALMAS)		+= clk-palmas.o
 obj-$(CONFIG_CLK_PPC_CORENET)		+= clk-ppc-corenet.o
+obj-$(CONFIG_COMMON_CLK_RK808)		+= clk-rk808.o
 obj-$(CONFIG_COMMON_CLK_S2MPS11)	+= clk-s2mps11.o
 obj-$(CONFIG_COMMON_CLK_SI5351)		+= clk-si5351.o
 obj-$(CONFIG_COMMON_CLK_SI570)		+= clk-si570.o
@@ -48,6 +52,7 @@ obj-$(CONFIG_ARCH_MMP)			+= mmp/
 endif
 obj-$(CONFIG_PLAT_ORION)		+= mvebu/
 obj-$(CONFIG_ARCH_MXS)			+= mxs/
+obj-$(CONFIG_COMMON_CLK_PXA)		+= pxa/
 obj-$(CONFIG_COMMON_CLK_QCOM)		+= qcom/
 obj-$(CONFIG_ARCH_ROCKCHIP)		+= rockchip/
 obj-$(CONFIG_COMMON_CLK_SAMSUNG)	+= samsung/
diff --git a/drivers/clk/at91/Makefile b/drivers/clk/at91/Makefile
index 4998aee59267..89a48a7bd5df 100644
--- a/drivers/clk/at91/Makefile
+++ b/drivers/clk/at91/Makefile
@@ -9,3 +9,4 @@ obj-y += clk-system.o clk-peripheral.o clk-programmable.o
 obj-$(CONFIG_HAVE_AT91_UTMI)		+= clk-utmi.o
 obj-$(CONFIG_HAVE_AT91_USB_CLK)		+= clk-usb.o
 obj-$(CONFIG_HAVE_AT91_SMD)		+= clk-smd.o
+obj-$(CONFIG_HAVE_AT91_H32MX)		+= clk-h32mx.o
diff --git a/drivers/clk/at91/clk-h32mx.c b/drivers/clk/at91/clk-h32mx.c
new file mode 100644
index 000000000000..152dcb3f7b5f
--- /dev/null
+++ b/drivers/clk/at91/clk-h32mx.c
@@ -0,0 +1,123 @@
+/*
+ * clk-h32mx.c
+ *
+ *  Copyright (C) 2014 Atmel
+ *
+ * Alexandre Belloni <alexandre.belloni@free-electrons.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/clk/at91_pmc.h>
+#include <linux/delay.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+
+#include "pmc.h"
+
+#define H32MX_MAX_FREQ	90000000
+
+struct clk_sama5d4_h32mx {
+	struct clk_hw hw;
+	struct at91_pmc *pmc;
+};
+
+#define to_clk_sama5d4_h32mx(hw) container_of(hw, struct clk_sama5d4_h32mx, hw)
+
+static unsigned long clk_sama5d4_h32mx_recalc_rate(struct clk_hw *hw,
+						 unsigned long parent_rate)
+{
+	struct clk_sama5d4_h32mx *h32mxclk = to_clk_sama5d4_h32mx(hw);
+
+	if (pmc_read(h32mxclk->pmc, AT91_PMC_MCKR) & AT91_PMC_H32MXDIV)
+		return parent_rate / 2;
+
+	if (parent_rate > H32MX_MAX_FREQ)
+		pr_warn("H32MX clock is too fast\n");
+	return parent_rate;
+}
+
+static long clk_sama5d4_h32mx_round_rate(struct clk_hw *hw, unsigned long rate,
+				       unsigned long *parent_rate)
+{
+	unsigned long div;
+
+	if (rate > *parent_rate)
+		return *parent_rate;
+	div = *parent_rate / 2;
+	if (rate < div)
+		return div;
+
+	if (rate - div < *parent_rate - rate)
+		return div;
+
+	return *parent_rate;
+}
+
+static int clk_sama5d4_h32mx_set_rate(struct clk_hw *hw, unsigned long rate,
+				    unsigned long parent_rate)
+{
+	struct clk_sama5d4_h32mx *h32mxclk = to_clk_sama5d4_h32mx(hw);
+	struct at91_pmc *pmc = h32mxclk->pmc;
+	u32 tmp;
+
+	if (parent_rate != rate && (parent_rate / 2) != rate)
+		return -EINVAL;
+
+	pmc_lock(pmc);
+	tmp = pmc_read(pmc, AT91_PMC_MCKR) & ~AT91_PMC_H32MXDIV;
+	if ((parent_rate / 2) == rate)
+		tmp |= AT91_PMC_H32MXDIV;
+	pmc_write(pmc, AT91_PMC_MCKR, tmp);
+	pmc_unlock(pmc);
+
+	return 0;
+}
+
+static const struct clk_ops h32mx_ops = {
+	.recalc_rate = clk_sama5d4_h32mx_recalc_rate,
+	.round_rate = clk_sama5d4_h32mx_round_rate,
+	.set_rate = clk_sama5d4_h32mx_set_rate,
+};
+
+void __init of_sama5d4_clk_h32mx_setup(struct device_node *np,
+				     struct at91_pmc *pmc)
+{
+	struct clk_sama5d4_h32mx *h32mxclk;
+	struct clk_init_data init;
+	const char *parent_name;
+	struct clk *clk;
+
+	h32mxclk = kzalloc(sizeof(*h32mxclk), GFP_KERNEL);
+	if (!h32mxclk)
+		return;
+
+	parent_name = of_clk_get_parent_name(np, 0);
+
+	init.name = np->name;
+	init.ops = &h32mx_ops;
+	init.parent_names = parent_name ? &parent_name : NULL;
+	init.num_parents = parent_name ? 1 : 0;
+	init.flags = CLK_SET_RATE_GATE;
+
+	h32mxclk->hw.init = &init;
+	h32mxclk->pmc = pmc;
+
+	clk = clk_register(NULL, &h32mxclk->hw);
+	if (!clk)
+		return;
+
+	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+}
diff --git a/drivers/clk/at91/clk-pll.c b/drivers/clk/at91/clk-pll.c
index cf6ed023504c..6ec79dbc0840 100644
--- a/drivers/clk/at91/clk-pll.c
+++ b/drivers/clk/at91/clk-pll.c
@@ -15,6 +15,7 @@
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/io.h>
+#include <linux/kernel.h>
 #include <linux/wait.h>
 #include <linux/sched.h>
 #include <linux/interrupt.h>
@@ -29,9 +30,12 @@
 #define PLL_DIV(reg)		((reg) & PLL_DIV_MASK)
 #define PLL_MUL(reg, layout)	(((reg) >> (layout)->mul_shift) & \
 				 (layout)->mul_mask)
+#define PLL_MUL_MIN		2
+#define PLL_MUL_MASK(layout)	((layout)->mul_mask)
+#define PLL_MUL_MAX(layout)	(PLL_MUL_MASK(layout) + 1)
 #define PLL_ICPR_SHIFT(id)	((id) * 16)
 #define PLL_ICPR_MASK(id)	(0xffff << PLL_ICPR_SHIFT(id))
-#define PLL_MAX_COUNT		0x3ff
+#define PLL_MAX_COUNT		0x3f
 #define PLL_COUNT_SHIFT		8
 #define PLL_OUT_SHIFT		14
 #define PLL_MAX_ID		1
@@ -147,115 +151,113 @@ static unsigned long clk_pll_recalc_rate(struct clk_hw *hw,
 					 unsigned long parent_rate)
 {
 	struct clk_pll *pll = to_clk_pll(hw);
-	const struct clk_pll_layout *layout = pll->layout;
-	struct at91_pmc *pmc = pll->pmc;
-	int offset = PLL_REG(pll->id);
-	u32 tmp = pmc_read(pmc, offset) & layout->pllr_mask;
-	u8 div = PLL_DIV(tmp);
-	u16 mul = PLL_MUL(tmp, layout);
-	if (!div || !mul)
+
+	if (!pll->div || !pll->mul)
 		return 0;
 
-	return (parent_rate * (mul + 1)) / div;
+	return (parent_rate / pll->div) * (pll->mul + 1);
 }
 
 static long clk_pll_get_best_div_mul(struct clk_pll *pll, unsigned long rate,
 				     unsigned long parent_rate,
 				     u32 *div, u32 *mul,
 				     u32 *index) {
-	unsigned long maxrate;
-	unsigned long minrate;
-	unsigned long divrate;
-	unsigned long bestdiv = 1;
-	unsigned long bestmul;
-	unsigned long tmpdiv;
-	unsigned long roundup;
-	unsigned long rounddown;
-	unsigned long remainder;
-	unsigned long bestremainder;
-	unsigned long maxmul;
-	unsigned long maxdiv;
-	unsigned long mindiv;
-	int i = 0;
 	const struct clk_pll_layout *layout = pll->layout;
 	const struct clk_pll_characteristics *characteristics =
 							pll->characteristics;
+	unsigned long bestremainder = ULONG_MAX;
+	unsigned long maxdiv, mindiv, tmpdiv;
+	long bestrate = -ERANGE;
+	unsigned long bestdiv;
+	unsigned long bestmul;
+	int i = 0;
 
-	/* Minimum divider = 1 */
-	/* Maximum multiplier = max_mul */
-	maxmul = layout->mul_mask + 1;
-	maxrate = (parent_rate * maxmul) / 1;
-
-	/* Maximum divider = max_div */
-	/* Minimum multiplier = 2 */
-	maxdiv = PLL_DIV_MAX;
-	minrate = (parent_rate * 2) / maxdiv;
-
+	/* Check if parent_rate is a valid input rate */
 	if (parent_rate < characteristics->input.min ||
-	    parent_rate < characteristics->input.max)
-		return -ERANGE;
-
-	if (parent_rate < minrate || parent_rate > maxrate)
+	    parent_rate > characteristics->input.max)
 		return -ERANGE;
 
-	for (i = 0; i < characteristics->num_output; i++) {
-		if (parent_rate >= characteristics->output[i].min &&
-		    parent_rate <= characteristics->output[i].max)
-			break;
-	}
-
-	if (i >= characteristics->num_output)
-		return -ERANGE;
-
-	bestmul = rate / parent_rate;
-	rounddown = parent_rate % rate;
-	roundup = rate - rounddown;
-	bestremainder = roundup < rounddown ? roundup : rounddown;
-
-	if (!bestremainder) {
-		if (div)
-			*div = bestdiv;
-		if (mul)
-			*mul = bestmul;
-		if (index)
-			*index = i;
-		return rate;
-	}
-
-	maxdiv = 255 / (bestmul + 1);
-	if (parent_rate / maxdiv < characteristics->input.min)
-		maxdiv = parent_rate / characteristics->input.min;
-	mindiv = parent_rate / characteristics->input.max;
-	if (parent_rate % characteristics->input.max)
-		mindiv++;
-
-	for (tmpdiv = mindiv; tmpdiv < maxdiv; tmpdiv++) {
-		divrate = parent_rate / tmpdiv;
-
-		rounddown = rate % divrate;
-		roundup = divrate - rounddown;
-		remainder = roundup < rounddown ? roundup : rounddown;
-
+	/*
+	 * Calculate minimum divider based on the minimum multiplier, the
+	 * parent_rate and the requested rate.
+	 * Should always be 2 according to the input and output characteristics
+	 * of the PLL blocks.
+	 */
+	mindiv = (parent_rate * PLL_MUL_MIN) / rate;
+	if (!mindiv)
+		mindiv = 1;
+
+	/*
+	 * Calculate the maximum divider which is limited by PLL register
+	 * layout (limited by the MUL or DIV field size).
+	 */
+	maxdiv = DIV_ROUND_UP(parent_rate * PLL_MUL_MAX(layout), rate);
+	if (maxdiv > PLL_DIV_MAX)
+		maxdiv = PLL_DIV_MAX;
+
+	/*
+	 * Iterate over the acceptable divider values to find the best
+	 * divider/multiplier pair (the one that generates the closest
+	 * rate to the requested one).
+	 */
+	for (tmpdiv = mindiv; tmpdiv <= maxdiv; tmpdiv++) {
+		unsigned long remainder;
+		unsigned long tmprate;
+		unsigned long tmpmul;
+
+		/*
+		 * Calculate the multiplier associated with the current
+		 * divider that provide the closest rate to the requested one.
+		 */
+		tmpmul = DIV_ROUND_CLOSEST(rate, parent_rate / tmpdiv);
+		tmprate = (parent_rate / tmpdiv) * tmpmul;
+		if (tmprate > rate)
+			remainder = tmprate - rate;
+		else
+			remainder = rate - tmprate;
+
+		/*
+		 * Compare the remainder with the best remainder found until
+		 * now and elect a new best multiplier/divider pair if the
+		 * current remainder is smaller than the best one.
+		 */
 		if (remainder < bestremainder) {
 			bestremainder = remainder;
-			bestmul = rate / divrate;
 			bestdiv = tmpdiv;
+			bestmul = tmpmul;
+			bestrate = tmprate;
 		}
 
+		/*
+		 * We've found a perfect match!
+		 * Stop searching now and use this multiplier/divider pair.
+		 */
 		if (!remainder)
 			break;
 	}
 
-	rate = (parent_rate / bestdiv) * bestmul;
+	/* We haven't found any multiplier/divider pair => return -ERANGE */
+	if (bestrate < 0)
+		return bestrate;
+
+	/* Check if bestrate is a valid output rate  */
+	for (i = 0; i < characteristics->num_output; i++) {
+		if (bestrate >= characteristics->output[i].min &&
+		    bestrate <= characteristics->output[i].max)
+			break;
+	}
+
+	if (i >= characteristics->num_output)
+		return -ERANGE;
 
 	if (div)
 		*div = bestdiv;
 	if (mul)
-		*mul = bestmul;
+		*mul = bestmul - 1;
 	if (index)
 		*index = i;
 
-	return rate;
+	return bestrate;
 }
 
 static long clk_pll_round_rate(struct clk_hw *hw, unsigned long rate,
diff --git a/drivers/clk/at91/clk-slow.c b/drivers/clk/at91/clk-slow.c
index 0300c46ee247..32f7c1b36204 100644
--- a/drivers/clk/at91/clk-slow.c
+++ b/drivers/clk/at91/clk-slow.c
@@ -447,7 +447,7 @@ void __init of_at91sam9260_clk_slow_setup(struct device_node *np,
 	int i;
 
 	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
-	if (num_parents <= 0 || num_parents > 1)
+	if (num_parents != 2)
 		return;
 
 	for (i = 0; i < num_parents; ++i) {
diff --git a/drivers/clk/at91/clk-system.c b/drivers/clk/at91/clk-system.c
index 8c96307d7363..a76d03fd577b 100644
--- a/drivers/clk/at91/clk-system.c
+++ b/drivers/clk/at91/clk-system.c
@@ -119,13 +119,7 @@ at91_clk_register_system(struct at91_pmc *pmc, const char *name,
 	init.ops = &system_ops;
 	init.parent_names = &parent_name;
 	init.num_parents = 1;
-	/*
-	 * CLK_IGNORE_UNUSED is used to avoid ddrck switch off.
-	 * TODO : we should implement a driver supporting at91 ddr controller
-	 * (see drivers/memory) which would request and enable the ddrck clock.
-	 * When this is done we will be able to remove CLK_IGNORE_UNUSED flag.
-	 */
-	init.flags = CLK_SET_RATE_PARENT | CLK_IGNORE_UNUSED;
+	init.flags = CLK_SET_RATE_PARENT;
 
 	sys->id = id;
 	sys->hw.init = &init;
diff --git a/drivers/clk/at91/clk-usb.c b/drivers/clk/at91/clk-usb.c
index 7d1d26a4bd04..24b5b020753a 100644
--- a/drivers/clk/at91/clk-usb.c
+++ b/drivers/clk/at91/clk-usb.c
@@ -238,16 +238,22 @@ static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate,
 					  unsigned long *parent_rate)
 {
 	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
+	struct clk *parent = __clk_get_parent(hw->clk);
 	unsigned long bestrate = 0;
 	int bestdiff = -1;
 	unsigned long tmprate;
 	int tmpdiff;
 	int i = 0;
 
-	for (i = 0; i < 4; i++) {
+	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
+		unsigned long tmp_parent_rate;
+
 		if (!usb->divisors[i])
 			continue;
-		tmprate = *parent_rate / usb->divisors[i];
+
+		tmp_parent_rate = rate * usb->divisors[i];
+		tmp_parent_rate = __clk_round_rate(parent, tmp_parent_rate);
+		tmprate = tmp_parent_rate / usb->divisors[i];
 		if (tmprate < rate)
 			tmpdiff = rate - tmprate;
 		else
@@ -256,6 +262,7 @@ static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate,
 		if (bestdiff < 0 || bestdiff > tmpdiff) {
 			bestrate = tmprate;
 			bestdiff = tmpdiff;
+			*parent_rate = tmp_parent_rate;
 		}
 
 		if (!bestdiff)
@@ -272,10 +279,13 @@ static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
 	int i;
 	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
 	struct at91_pmc *pmc = usb->pmc;
-	unsigned long div = parent_rate / rate;
+	unsigned long div;
 
-	if (parent_rate % rate)
+	if (!rate || parent_rate % rate)
 		return -EINVAL;
+
+	div = parent_rate / rate;
+
 	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
 		if (usb->divisors[i] == div) {
 			tmp = pmc_read(pmc, AT91_CKGR_PLLBR) &
@@ -311,7 +321,7 @@ at91rm9200_clk_register_usb(struct at91_pmc *pmc, const char *name,
 	init.ops = &at91rm9200_usb_ops;
 	init.parent_names = &parent_name;
 	init.num_parents = 1;
-	init.flags = 0;
+	init.flags = CLK_SET_RATE_PARENT;
 
 	usb->hw.init = &init;
 	usb->pmc = pmc;
diff --git a/drivers/clk/at91/pmc.c b/drivers/clk/at91/pmc.c
index 524196bb35a5..386999b4f8eb 100644
--- a/drivers/clk/at91/pmc.c
+++ b/drivers/clk/at91/pmc.c
@@ -337,6 +337,12 @@ static const struct of_device_id pmc_clk_ids[] __initconst = {
 		.data = of_at91sam9x5_clk_smd_setup,
 	},
 #endif
+#if defined(CONFIG_HAVE_AT91_H32MX)
+	{
+		.compatible = "atmel,sama5d4-clk-h32mx",
+		.data = of_sama5d4_clk_h32mx_setup,
+	},
+#endif
 	{ /*sentinel*/ }
 };
 
diff --git a/drivers/clk/at91/pmc.h b/drivers/clk/at91/pmc.h
index 6c7625976113..52d2041fa3f6 100644
--- a/drivers/clk/at91/pmc.h
+++ b/drivers/clk/at91/pmc.h
@@ -120,4 +120,9 @@ extern void __init of_at91sam9x5_clk_smd_setup(struct device_node *np,
 					       struct at91_pmc *pmc);
 #endif
 
+#if defined(CONFIG_HAVE_AT91_SMD)
+extern void __init of_sama5d4_clk_h32mx_setup(struct device_node *np,
+					      struct at91_pmc *pmc);
+#endif
+
 #endif /* __PMC_H_ */
diff --git a/drivers/clk/clk-axi-clkgen.c b/drivers/clk/clk-axi-clkgen.c
index 1127ee46b802..e619285c6def 100644
--- a/drivers/clk/clk-axi-clkgen.c
+++ b/drivers/clk/clk-axi-clkgen.c
@@ -544,7 +544,6 @@ static int axi_clkgen_remove(struct platform_device *pdev)
 static struct platform_driver axi_clkgen_driver = {
 	.driver = {
 		.name = "adi-axi-clkgen",
-		.owner = THIS_MODULE,
 		.of_match_table = axi_clkgen_ids,
 	},
 	.probe = axi_clkgen_probe,
diff --git a/drivers/clk/clk-efm32gg.c b/drivers/clk/clk-efm32gg.c
index bac2ddf49d02..73a8d0ff530c 100644
--- a/drivers/clk/clk-efm32gg.c
+++ b/drivers/clk/clk-efm32gg.c
@@ -22,7 +22,7 @@ static struct clk_onecell_data clk_data = {
 	.clk_num = ARRAY_SIZE(clk),
 };
 
-static int __init efm32gg_cmu_init(struct device_node *np)
+static void __init efm32gg_cmu_init(struct device_node *np)
 {
 	int i;
 	void __iomem *base;
@@ -33,7 +33,7 @@ static int __init efm32gg_cmu_init(struct device_node *np)
 	base = of_iomap(np, 0);
 	if (!base) {
 		pr_warn("Failed to map address range for efm32gg,cmu node\n");
-		return -EADDRNOTAVAIL;
+		return;
 	}
 
 	clk[clk_HFXO] = clk_register_fixed_rate(NULL, "HFXO", NULL,
@@ -76,6 +76,6 @@ static int __init efm32gg_cmu_init(struct device_node *np)
 	clk[clk_HFPERCLKDAC0] = clk_register_gate(NULL, "HFPERCLK.DAC0",
 			"HFXO", 0, base + CMU_HFPERCLKEN0, 17, 0, NULL);
 
-	return of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
+	of_clk_add_provider(np, of_clk_src_onecell_get, &clk_data);
 }
 CLK_OF_DECLARE(efm32ggcmu, "efm32gg,cmu", efm32gg_cmu_init);
diff --git a/drivers/clk/clk-fractional-divider.c b/drivers/clk/clk-fractional-divider.c
index ede685ca0d20..82a59d0086cc 100644
--- a/drivers/clk/clk-fractional-divider.c
+++ b/drivers/clk/clk-fractional-divider.c
@@ -36,7 +36,7 @@ static unsigned long clk_fd_recalc_rate(struct clk_hw *hw,
 	m = (val & fd->mmask) >> fd->mshift;
 	n = (val & fd->nmask) >> fd->nshift;
 
-	ret = parent_rate * m;
+	ret = (u64)parent_rate * m;
 	do_div(ret, n);
 
 	return ret;
diff --git a/drivers/clk/clk-gate.c b/drivers/clk/clk-gate.c
index 4a58c55255bd..51fd87fb7ba6 100644
--- a/drivers/clk/clk-gate.c
+++ b/drivers/clk/clk-gate.c
@@ -45,7 +45,7 @@ static void clk_gate_endisable(struct clk_hw *hw, int enable)
 {
 	struct clk_gate *gate = to_clk_gate(hw);
 	int set = gate->flags & CLK_GATE_SET_TO_DISABLE ? 1 : 0;
-	unsigned long flags = 0;
+	unsigned long uninitialized_var(flags);
 	u32 reg;
 
 	set ^= enable;
diff --git a/drivers/clk/clk-gpio-gate.c b/drivers/clk/clk-gpio-gate.c
new file mode 100644
index 000000000000..08e43224fd52
--- /dev/null
+++ b/drivers/clk/clk-gpio-gate.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (C) 2013 - 2014 Texas Instruments Incorporated - http://www.ti.com
+ * Author: Jyri Sarha <jsarha@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Gpio gated clock implementation
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/gpio.h>
+#include <linux/gpio/consumer.h>
+#include <linux/of_gpio.h>
+#include <linux/err.h>
+#include <linux/device.h>
+
+/**
+ * DOC: basic gpio gated clock which can be enabled and disabled
+ *      with gpio output
+ * Traits of this clock:
+ * prepare - clk_(un)prepare only ensures parent is (un)prepared
+ * enable - clk_enable and clk_disable are functional & control gpio
+ * rate - inherits rate from parent.  No clk_set_rate support
+ * parent - fixed parent.  No clk_set_parent support
+ */
+
+#define to_clk_gpio(_hw) container_of(_hw, struct clk_gpio, hw)
+
+static int clk_gpio_gate_enable(struct clk_hw *hw)
+{
+	struct clk_gpio *clk = to_clk_gpio(hw);
+
+	gpiod_set_value(clk->gpiod, 1);
+
+	return 0;
+}
+
+static void clk_gpio_gate_disable(struct clk_hw *hw)
+{
+	struct clk_gpio *clk = to_clk_gpio(hw);
+
+	gpiod_set_value(clk->gpiod, 0);
+}
+
+static int clk_gpio_gate_is_enabled(struct clk_hw *hw)
+{
+	struct clk_gpio *clk = to_clk_gpio(hw);
+
+	return gpiod_get_value(clk->gpiod);
+}
+
+const struct clk_ops clk_gpio_gate_ops = {
+	.enable = clk_gpio_gate_enable,
+	.disable = clk_gpio_gate_disable,
+	.is_enabled = clk_gpio_gate_is_enabled,
+};
+EXPORT_SYMBOL_GPL(clk_gpio_gate_ops);
+
+/**
+ * clk_register_gpio - register a gpip clock with the clock framework
+ * @dev: device that is registering this clock
+ * @name: name of this clock
+ * @parent_name: name of this clock's parent
+ * @gpiod: gpio descriptor to gate this clock
+ */
+struct clk *clk_register_gpio_gate(struct device *dev, const char *name,
+		const char *parent_name, struct gpio_desc *gpiod,
+		unsigned long flags)
+{
+	struct clk_gpio *clk_gpio = NULL;
+	struct clk *clk = ERR_PTR(-EINVAL);
+	struct clk_init_data init = { NULL };
+	unsigned long gpio_flags;
+	int err;
+
+	if (gpiod_is_active_low(gpiod))
+		gpio_flags = GPIOF_OUT_INIT_HIGH;
+	else
+		gpio_flags = GPIOF_OUT_INIT_LOW;
+
+	if (dev)
+		err = devm_gpio_request_one(dev, desc_to_gpio(gpiod),
+					    gpio_flags, name);
+	else
+		err = gpio_request_one(desc_to_gpio(gpiod), gpio_flags, name);
+
+	if (err) {
+		pr_err("%s: %s: Error requesting clock control gpio %u\n",
+		       __func__, name, desc_to_gpio(gpiod));
+		return ERR_PTR(err);
+	}
+
+	if (dev)
+		clk_gpio = devm_kzalloc(dev, sizeof(struct clk_gpio),
+					GFP_KERNEL);
+	else
+		clk_gpio = kzalloc(sizeof(struct clk_gpio), GFP_KERNEL);
+
+	if (!clk_gpio) {
+		clk = ERR_PTR(-ENOMEM);
+		goto clk_register_gpio_gate_err;
+	}
+
+	init.name = name;
+	init.ops = &clk_gpio_gate_ops;
+	init.flags = flags | CLK_IS_BASIC;
+	init.parent_names = (parent_name ? &parent_name : NULL);
+	init.num_parents = (parent_name ? 1 : 0);
+
+	clk_gpio->gpiod = gpiod;
+	clk_gpio->hw.init = &init;
+
+	clk = clk_register(dev, &clk_gpio->hw);
+
+	if (!IS_ERR(clk))
+		return clk;
+
+	if (!dev)
+		kfree(clk_gpio);
+
+clk_register_gpio_gate_err:
+	gpiod_put(gpiod);
+
+	return clk;
+}
+EXPORT_SYMBOL_GPL(clk_register_gpio_gate);
+
+#ifdef CONFIG_OF
+/**
+ * The clk_register_gpio_gate has to be delayed, because the EPROBE_DEFER
+ * can not be handled properly at of_clk_init() call time.
+ */
+
+struct clk_gpio_gate_delayed_register_data {
+	struct device_node *node;
+	struct mutex lock;
+	struct clk *clk;
+};
+
+static struct clk *of_clk_gpio_gate_delayed_register_get(
+		struct of_phandle_args *clkspec,
+		void *_data)
+{
+	struct clk_gpio_gate_delayed_register_data *data = _data;
+	struct clk *clk;
+	const char *clk_name = data->node->name;
+	const char *parent_name;
+	struct gpio_desc *gpiod;
+	int gpio;
+
+	mutex_lock(&data->lock);
+
+	if (data->clk) {
+		mutex_unlock(&data->lock);
+		return data->clk;
+	}
+
+	gpio = of_get_named_gpio_flags(data->node, "enable-gpios", 0, NULL);
+	if (gpio < 0) {
+		mutex_unlock(&data->lock);
+		if (gpio != -EPROBE_DEFER)
+			pr_err("%s: %s: Can't get 'enable-gpios' DT property\n",
+			       __func__, clk_name);
+		return ERR_PTR(gpio);
+	}
+	gpiod = gpio_to_desc(gpio);
+
+	parent_name = of_clk_get_parent_name(data->node, 0);
+
+	clk = clk_register_gpio_gate(NULL, clk_name, parent_name, gpiod, 0);
+	if (IS_ERR(clk)) {
+		mutex_unlock(&data->lock);
+		return clk;
+	}
+
+	data->clk = clk;
+	mutex_unlock(&data->lock);
+
+	return clk;
+}
+
+/**
+ * of_gpio_gate_clk_setup() - Setup function for gpio controlled clock
+ */
+void __init of_gpio_gate_clk_setup(struct device_node *node)
+{
+	struct clk_gpio_gate_delayed_register_data *data;
+
+	data = kzalloc(sizeof(struct clk_gpio_gate_delayed_register_data),
+		       GFP_KERNEL);
+	if (!data)
+		return;
+
+	data->node = node;
+	mutex_init(&data->lock);
+
+	of_clk_add_provider(node, of_clk_gpio_gate_delayed_register_get, data);
+}
+EXPORT_SYMBOL_GPL(of_gpio_gate_clk_setup);
+CLK_OF_DECLARE(gpio_gate_clk, "gpio-gate-clock", of_gpio_gate_clk_setup);
+#endif
diff --git a/drivers/clk/clk-max-gen.c b/drivers/clk/clk-max-gen.c
new file mode 100644
index 000000000000..6505049d50f1
--- /dev/null
+++ b/drivers/clk/clk-max-gen.c
@@ -0,0 +1,192 @@
+/*
+ * clk-max-gen.c - Generic clock driver for Maxim PMICs clocks
+ *
+ * Copyright (C) 2014 Google, Inc
+ *
+ * Copyright (C) 2012 Samsung Electornics
+ * Jonghwa Lee <jonghwa3.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * This driver is based on clk-max77686.c
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/regmap.h>
+#include <linux/platform_device.h>
+#include <linux/clk-provider.h>
+#include <linux/mutex.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+#include <linux/export.h>
+
+struct max_gen_clk {
+	struct regmap *regmap;
+	u32 mask;
+	u32 reg;
+	struct clk_hw hw;
+};
+
+static struct max_gen_clk *to_max_gen_clk(struct clk_hw *hw)
+{
+	return container_of(hw, struct max_gen_clk, hw);
+}
+
+static int max_gen_clk_prepare(struct clk_hw *hw)
+{
+	struct max_gen_clk *max_gen = to_max_gen_clk(hw);
+
+	return regmap_update_bits(max_gen->regmap, max_gen->reg,
+				  max_gen->mask, max_gen->mask);
+}
+
+static void max_gen_clk_unprepare(struct clk_hw *hw)
+{
+	struct max_gen_clk *max_gen = to_max_gen_clk(hw);
+
+	regmap_update_bits(max_gen->regmap, max_gen->reg,
+			   max_gen->mask, ~max_gen->mask);
+}
+
+static int max_gen_clk_is_prepared(struct clk_hw *hw)
+{
+	struct max_gen_clk *max_gen = to_max_gen_clk(hw);
+	int ret;
+	u32 val;
+
+	ret = regmap_read(max_gen->regmap, max_gen->reg, &val);
+
+	if (ret < 0)
+		return -EINVAL;
+
+	return val & max_gen->mask;
+}
+
+static unsigned long max_gen_recalc_rate(struct clk_hw *hw,
+					 unsigned long parent_rate)
+{
+	return 32768;
+}
+
+struct clk_ops max_gen_clk_ops = {
+	.prepare	= max_gen_clk_prepare,
+	.unprepare	= max_gen_clk_unprepare,
+	.is_prepared	= max_gen_clk_is_prepared,
+	.recalc_rate	= max_gen_recalc_rate,
+};
+EXPORT_SYMBOL_GPL(max_gen_clk_ops);
+
+static struct clk *max_gen_clk_register(struct device *dev,
+					struct max_gen_clk *max_gen)
+{
+	struct clk *clk;
+	struct clk_hw *hw = &max_gen->hw;
+	int ret;
+
+	clk = devm_clk_register(dev, hw);
+	if (IS_ERR(clk))
+		return clk;
+
+	ret = clk_register_clkdev(clk, hw->init->name, NULL);
+
+	if (ret)
+		return ERR_PTR(ret);
+
+	return clk;
+}
+
+int max_gen_clk_probe(struct platform_device *pdev, struct regmap *regmap,
+		      u32 reg, struct clk_init_data *clks_init, int num_init)
+{
+	int i, ret;
+	struct max_gen_clk *max_gen_clks;
+	struct clk **clocks;
+	struct device *dev = pdev->dev.parent;
+	const char *clk_name;
+	struct clk_init_data *init;
+
+	clocks = devm_kzalloc(dev, sizeof(struct clk *) * num_init, GFP_KERNEL);
+	if (!clocks)
+		return -ENOMEM;
+
+	max_gen_clks = devm_kzalloc(dev, sizeof(struct max_gen_clk)
+				    * num_init, GFP_KERNEL);
+	if (!max_gen_clks)
+		return -ENOMEM;
+
+	for (i = 0; i < num_init; i++) {
+		max_gen_clks[i].regmap = regmap;
+		max_gen_clks[i].mask = 1 << i;
+		max_gen_clks[i].reg = reg;
+
+		init = devm_kzalloc(dev, sizeof(*init), GFP_KERNEL);
+		if (!init)
+			return -ENOMEM;
+
+		if (dev->of_node &&
+		    !of_property_read_string_index(dev->of_node,
+						   "clock-output-names",
+						   i, &clk_name))
+			init->name = clk_name;
+		else
+			init->name = clks_init[i].name;
+
+		init->ops = clks_init[i].ops;
+		init->flags = clks_init[i].flags;
+
+		max_gen_clks[i].hw.init = init;
+
+		clocks[i] = max_gen_clk_register(dev, &max_gen_clks[i]);
+		if (IS_ERR(clocks[i])) {
+			ret = PTR_ERR(clocks[i]);
+			dev_err(dev, "failed to register %s\n",
+				max_gen_clks[i].hw.init->name);
+			return ret;
+		}
+	}
+
+	platform_set_drvdata(pdev, clocks);
+
+	if (dev->of_node) {
+		struct clk_onecell_data *of_data;
+
+		of_data = devm_kzalloc(dev, sizeof(*of_data), GFP_KERNEL);
+		if (!of_data)
+			return -ENOMEM;
+
+		of_data->clks = clocks;
+		of_data->clk_num = num_init;
+		ret = of_clk_add_provider(dev->of_node, of_clk_src_onecell_get,
+					  of_data);
+
+		if (ret) {
+			dev_err(dev, "failed to register OF clock provider\n");
+			return ret;
+		}
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max_gen_clk_probe);
+
+int max_gen_clk_remove(struct platform_device *pdev, int num_init)
+{
+	struct device *dev = pdev->dev.parent;
+
+	if (dev->of_node)
+		of_clk_del_provider(dev->of_node);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(max_gen_clk_remove);
diff --git a/drivers/clk/clk-max-gen.h b/drivers/clk/clk-max-gen.h
new file mode 100644
index 000000000000..997e86fc3f4d
--- /dev/null
+++ b/drivers/clk/clk-max-gen.h
@@ -0,0 +1,32 @@
+/*
+ * clk-max-gen.h - Generic clock driver for Maxim PMICs clocks
+ *
+ * Copyright (C) 2014 Google, Inc
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#ifndef __CLK_MAX_GEN_H__
+#define __CLK_MAX_GEN_H__
+
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/clkdev.h>
+#include <linux/regmap.h>
+#include <linux/platform_device.h>
+
+int max_gen_clk_probe(struct platform_device *pdev, struct regmap *regmap,
+		      u32 reg, struct clk_init_data *clks_init, int num_init);
+int max_gen_clk_remove(struct platform_device *pdev, int num_init);
+extern struct clk_ops max_gen_clk_ops;
+
+#endif /* __CLK_MAX_GEN_H__ */
diff --git a/drivers/clk/clk-max77686.c b/drivers/clk/clk-max77686.c
index 3d7e8dd8fd58..86cdb3a28629 100644
--- a/drivers/clk/clk-max77686.c
+++ b/drivers/clk/clk-max77686.c
@@ -30,193 +30,38 @@
 #include <linux/mutex.h>
 #include <linux/clkdev.h>
 
-enum {
-	MAX77686_CLK_AP = 0,
-	MAX77686_CLK_CP,
-	MAX77686_CLK_PMIC,
-	MAX77686_CLKS_NUM,
-};
-
-struct max77686_clk {
-	struct max77686_dev *iodev;
-	u32 mask;
-	struct clk_hw hw;
-	struct clk_lookup *lookup;
-};
-
-static struct max77686_clk *to_max77686_clk(struct clk_hw *hw)
-{
-	return container_of(hw, struct max77686_clk, hw);
-}
-
-static int max77686_clk_prepare(struct clk_hw *hw)
-{
-	struct max77686_clk *max77686 = to_max77686_clk(hw);
-
-	return regmap_update_bits(max77686->iodev->regmap,
-				  MAX77686_REG_32KHZ, max77686->mask,
-				  max77686->mask);
-}
-
-static void max77686_clk_unprepare(struct clk_hw *hw)
-{
-	struct max77686_clk *max77686 = to_max77686_clk(hw);
-
-	regmap_update_bits(max77686->iodev->regmap,
-		MAX77686_REG_32KHZ, max77686->mask, ~max77686->mask);
-}
-
-static int max77686_clk_is_prepared(struct clk_hw *hw)
-{
-	struct max77686_clk *max77686 = to_max77686_clk(hw);
-	int ret;
-	u32 val;
-
-	ret = regmap_read(max77686->iodev->regmap,
-				MAX77686_REG_32KHZ, &val);
-
-	if (ret < 0)
-		return -EINVAL;
-
-	return val & max77686->mask;
-}
-
-static unsigned long max77686_recalc_rate(struct clk_hw *hw,
-					  unsigned long parent_rate)
-{
-	return 32768;
-}
-
-static struct clk_ops max77686_clk_ops = {
-	.prepare	= max77686_clk_prepare,
-	.unprepare	= max77686_clk_unprepare,
-	.is_prepared	= max77686_clk_is_prepared,
-	.recalc_rate	= max77686_recalc_rate,
-};
+#include <dt-bindings/clock/maxim,max77686.h>
+#include "clk-max-gen.h"
 
 static struct clk_init_data max77686_clks_init[MAX77686_CLKS_NUM] = {
 	[MAX77686_CLK_AP] = {
 		.name = "32khz_ap",
-		.ops = &max77686_clk_ops,
+		.ops = &max_gen_clk_ops,
 		.flags = CLK_IS_ROOT,
 	},
 	[MAX77686_CLK_CP] = {
 		.name = "32khz_cp",
-		.ops = &max77686_clk_ops,
+		.ops = &max_gen_clk_ops,
 		.flags = CLK_IS_ROOT,
 	},
 	[MAX77686_CLK_PMIC] = {
 		.name = "32khz_pmic",
-		.ops = &max77686_clk_ops,
+		.ops = &max_gen_clk_ops,
 		.flags = CLK_IS_ROOT,
 	},
 };
 
-static struct clk *max77686_clk_register(struct device *dev,
-				struct max77686_clk *max77686)
-{
-	struct clk *clk;
-	struct clk_hw *hw = &max77686->hw;
-
-	clk = clk_register(dev, hw);
-	if (IS_ERR(clk))
-		return clk;
-
-	max77686->lookup = kzalloc(sizeof(struct clk_lookup), GFP_KERNEL);
-	if (!max77686->lookup)
-		return ERR_PTR(-ENOMEM);
-
-	max77686->lookup->con_id = hw->init->name;
-	max77686->lookup->clk = clk;
-
-	clkdev_add(max77686->lookup);
-
-	return clk;
-}
-
 static int max77686_clk_probe(struct platform_device *pdev)
 {
 	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	struct max77686_clk *max77686_clks[MAX77686_CLKS_NUM];
-	struct clk **clocks;
-	int i, ret;
-
-	clocks = devm_kzalloc(&pdev->dev, sizeof(struct clk *)
-					* MAX77686_CLKS_NUM, GFP_KERNEL);
-	if (!clocks)
-		return -ENOMEM;
-
-	for (i = 0; i < MAX77686_CLKS_NUM; i++) {
-		max77686_clks[i] = devm_kzalloc(&pdev->dev,
-					sizeof(struct max77686_clk), GFP_KERNEL);
-		if (!max77686_clks[i])
-			return -ENOMEM;
-	}
-
-	for (i = 0; i < MAX77686_CLKS_NUM; i++) {
-		max77686_clks[i]->iodev = iodev;
-		max77686_clks[i]->mask = 1 << i;
-		max77686_clks[i]->hw.init = &max77686_clks_init[i];
-
-		clocks[i] = max77686_clk_register(&pdev->dev, max77686_clks[i]);
-		if (IS_ERR(clocks[i])) {
-			ret = PTR_ERR(clocks[i]);
-			dev_err(&pdev->dev, "failed to register %s\n",
-				max77686_clks[i]->hw.init->name);
-			goto err_clocks;
-		}
-	}
-
-	platform_set_drvdata(pdev, clocks);
-
-	if (iodev->dev->of_node) {
-		struct clk_onecell_data *of_data;
 
-		of_data = devm_kzalloc(&pdev->dev,
-					sizeof(*of_data), GFP_KERNEL);
-		if (!of_data) {
-			ret = -ENOMEM;
-			goto err_clocks;
-		}
-
-		of_data->clks = clocks;
-		of_data->clk_num = MAX77686_CLKS_NUM;
-		ret = of_clk_add_provider(iodev->dev->of_node,
-					of_clk_src_onecell_get, of_data);
-		if (ret) {
-			dev_err(&pdev->dev, "failed to register OF clock provider\n");
-			goto err_clocks;
-		}
-	}
-
-	return 0;
-
-err_clocks:
-	for (--i; i >= 0; --i) {
-		clkdev_drop(max77686_clks[i]->lookup);
-		clk_unregister(max77686_clks[i]->hw.clk);
-	}
-
-	return ret;
+	return max_gen_clk_probe(pdev, iodev->regmap, MAX77686_REG_32KHZ,
+				 max77686_clks_init, MAX77686_CLKS_NUM);
 }
 
 static int max77686_clk_remove(struct platform_device *pdev)
 {
-	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
-	struct clk **clocks = platform_get_drvdata(pdev);
-	int i;
-
-	if (iodev->dev->of_node)
-		of_clk_del_provider(iodev->dev->of_node);
-
-	for (i = 0; i < MAX77686_CLKS_NUM; i++) {
-		struct clk_hw *hw = __clk_get_hw(clocks[i]);
-		struct max77686_clk *max77686 = to_max77686_clk(hw);
-
-		clkdev_drop(max77686->lookup);
-		clk_unregister(clocks[i]);
-	}
-	return 0;
+	return max_gen_clk_remove(pdev, MAX77686_CLKS_NUM);
 }
 
 static const struct platform_device_id max77686_clk_id[] = {
@@ -228,24 +73,13 @@ MODULE_DEVICE_TABLE(platform, max77686_clk_id);
 static struct platform_driver max77686_clk_driver = {
 	.driver = {
 		.name  = "max77686-clk",
-		.owner = THIS_MODULE,
 	},
 	.probe = max77686_clk_probe,
 	.remove = max77686_clk_remove,
 	.id_table = max77686_clk_id,
 };
 
-static int __init max77686_clk_init(void)
-{
-	return platform_driver_register(&max77686_clk_driver);
-}
-subsys_initcall(max77686_clk_init);
-
-static void __init max77686_clk_cleanup(void)
-{
-	platform_driver_unregister(&max77686_clk_driver);
-}
-module_exit(max77686_clk_cleanup);
+module_platform_driver(max77686_clk_driver);
 
 MODULE_DESCRIPTION("MAXIM 77686 Clock Driver");
 MODULE_AUTHOR("Jonghwa Lee <jonghwa3.lee@samsung.com>");
diff --git a/drivers/clk/clk-max77802.c b/drivers/clk/clk-max77802.c
new file mode 100644
index 000000000000..0729dc723a8f
--- /dev/null
+++ b/drivers/clk/clk-max77802.c
@@ -0,0 +1,97 @@
+/*
+ * clk-max77802.c - Clock driver for Maxim 77802
+ *
+ * Copyright (C) 2014 Google, Inc
+ *
+ * Copyright (C) 2012 Samsung Electornics
+ * Jonghwa Lee <jonghwa3.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * This driver is based on clk-max77686.c
+ */
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/max77686-private.h>
+#include <linux/clk-provider.h>
+#include <linux/mutex.h>
+#include <linux/clkdev.h>
+
+#include <dt-bindings/clock/maxim,max77802.h>
+#include "clk-max-gen.h"
+
+#define MAX77802_CLOCK_OPMODE_MASK	0x1
+#define MAX77802_CLOCK_LOW_JITTER_SHIFT 0x3
+
+static struct clk_init_data max77802_clks_init[MAX77802_CLKS_NUM] = {
+	[MAX77802_CLK_32K_AP] = {
+		.name = "32khz_ap",
+		.ops = &max_gen_clk_ops,
+		.flags = CLK_IS_ROOT,
+	},
+	[MAX77802_CLK_32K_CP] = {
+		.name = "32khz_cp",
+		.ops = &max_gen_clk_ops,
+		.flags = CLK_IS_ROOT,
+	},
+};
+
+static int max77802_clk_probe(struct platform_device *pdev)
+{
+	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	int ret;
+
+	ret = max_gen_clk_probe(pdev, iodev->regmap, MAX77802_REG_32KHZ,
+				max77802_clks_init, MAX77802_CLKS_NUM);
+
+	if (ret) {
+		dev_err(&pdev->dev, "generic probe failed %d\n", ret);
+		return ret;
+	}
+
+	/* Enable low-jitter mode on the 32khz clocks. */
+	ret = regmap_update_bits(iodev->regmap, MAX77802_REG_32KHZ,
+				 1 << MAX77802_CLOCK_LOW_JITTER_SHIFT,
+				 1 << MAX77802_CLOCK_LOW_JITTER_SHIFT);
+	if (ret < 0)
+		dev_err(&pdev->dev, "failed to enable low-jitter mode\n");
+
+	return ret;
+}
+
+static int max77802_clk_remove(struct platform_device *pdev)
+{
+	return max_gen_clk_remove(pdev, MAX77802_CLKS_NUM);
+}
+
+static const struct platform_device_id max77802_clk_id[] = {
+	{ "max77802-clk", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, max77802_clk_id);
+
+static struct platform_driver max77802_clk_driver = {
+	.driver = {
+		.name  = "max77802-clk",
+	},
+	.probe = max77802_clk_probe,
+	.remove = max77802_clk_remove,
+	.id_table = max77802_clk_id,
+};
+
+module_platform_driver(max77802_clk_driver);
+
+MODULE_DESCRIPTION("MAXIM 77802 Clock Driver");
+MODULE_AUTHOR("Javier Martinez Canillas <javier.martinez@collabora.co.uk>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/clk/clk-palmas.c b/drivers/clk/clk-palmas.c
index 781630e1372b..8d459923a15f 100644
--- a/drivers/clk/clk-palmas.c
+++ b/drivers/clk/clk-palmas.c
@@ -292,7 +292,6 @@ static int palmas_clks_remove(struct platform_device *pdev)
 static struct platform_driver palmas_clks_driver = {
 	.driver = {
 		.name = "palmas-clk",
-		.owner = THIS_MODULE,
 		.of_match_table = palmas_clks_of_match,
 	},
 	.probe = palmas_clks_probe,
diff --git a/drivers/clk/clk-rk808.c b/drivers/clk/clk-rk808.c
new file mode 100644
index 000000000000..83902b9cd49e
--- /dev/null
+++ b/drivers/clk/clk-rk808.c
@@ -0,0 +1,170 @@
+/*
+ * Clkout driver for Rockchip RK808
+ *
+ * Copyright (c) 2014, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * Author:Chris Zhong <zyw@rock-chips.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/rk808.h>
+#include <linux/i2c.h>
+
+#define RK808_NR_OUTPUT 2
+
+struct rk808_clkout {
+	struct rk808 *rk808;
+	struct clk_onecell_data clk_data;
+	struct clk_hw		clkout1_hw;
+	struct clk_hw		clkout2_hw;
+};
+
+static unsigned long rk808_clkout_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	return 32768;
+}
+
+static int rk808_clkout2_enable(struct clk_hw *hw, bool enable)
+{
+	struct rk808_clkout *rk808_clkout = container_of(hw,
+							 struct rk808_clkout,
+							 clkout2_hw);
+	struct rk808 *rk808 = rk808_clkout->rk808;
+
+	return regmap_update_bits(rk808->regmap, RK808_CLK32OUT_REG,
+				  CLK32KOUT2_EN, enable ? CLK32KOUT2_EN : 0);
+}
+
+static int rk808_clkout2_prepare(struct clk_hw *hw)
+{
+	return rk808_clkout2_enable(hw, true);
+}
+
+static void rk808_clkout2_unprepare(struct clk_hw *hw)
+{
+	rk808_clkout2_enable(hw, false);
+}
+
+static int rk808_clkout2_is_prepared(struct clk_hw *hw)
+{
+	struct rk808_clkout *rk808_clkout = container_of(hw,
+							 struct rk808_clkout,
+							 clkout2_hw);
+	struct rk808 *rk808 = rk808_clkout->rk808;
+	uint32_t val;
+
+	int ret = regmap_read(rk808->regmap, RK808_CLK32OUT_REG, &val);
+
+	if (ret < 0)
+		return ret;
+
+	return (val & CLK32KOUT2_EN) ? 1 : 0;
+}
+
+static const struct clk_ops rk808_clkout1_ops = {
+	.recalc_rate = rk808_clkout_recalc_rate,
+};
+
+static const struct clk_ops rk808_clkout2_ops = {
+	.prepare = rk808_clkout2_prepare,
+	.unprepare = rk808_clkout2_unprepare,
+	.is_prepared = rk808_clkout2_is_prepared,
+	.recalc_rate = rk808_clkout_recalc_rate,
+};
+
+static int rk808_clkout_probe(struct platform_device *pdev)
+{
+	struct rk808 *rk808 = dev_get_drvdata(pdev->dev.parent);
+	struct i2c_client *client = rk808->i2c;
+	struct device_node *node = client->dev.of_node;
+	struct clk_init_data init = {};
+	struct clk **clk_table;
+	struct rk808_clkout *rk808_clkout;
+
+	rk808_clkout = devm_kzalloc(&client->dev,
+				    sizeof(*rk808_clkout), GFP_KERNEL);
+	if (!rk808_clkout)
+		return -ENOMEM;
+
+	rk808_clkout->rk808 = rk808;
+
+	clk_table = devm_kcalloc(&client->dev, RK808_NR_OUTPUT,
+				 sizeof(struct clk *), GFP_KERNEL);
+	if (!clk_table)
+		return -ENOMEM;
+
+	init.flags = CLK_IS_ROOT;
+	init.parent_names = NULL;
+	init.num_parents = 0;
+	init.name = "rk808-clkout1";
+	init.ops = &rk808_clkout1_ops;
+	rk808_clkout->clkout1_hw.init = &init;
+
+	/* optional override of the clockname */
+	of_property_read_string_index(node, "clock-output-names",
+				      0, &init.name);
+
+	clk_table[0] = devm_clk_register(&client->dev,
+					 &rk808_clkout->clkout1_hw);
+	if (IS_ERR(clk_table[0]))
+		return PTR_ERR(clk_table[0]);
+
+	init.name = "rk808-clkout2";
+	init.ops = &rk808_clkout2_ops;
+	rk808_clkout->clkout2_hw.init = &init;
+
+	/* optional override of the clockname */
+	of_property_read_string_index(node, "clock-output-names",
+				      1, &init.name);
+
+	clk_table[1] = devm_clk_register(&client->dev,
+					 &rk808_clkout->clkout2_hw);
+	if (IS_ERR(clk_table[1]))
+		return PTR_ERR(clk_table[1]);
+
+	rk808_clkout->clk_data.clks = clk_table;
+	rk808_clkout->clk_data.clk_num = RK808_NR_OUTPUT;
+
+	return of_clk_add_provider(node, of_clk_src_onecell_get,
+				   &rk808_clkout->clk_data);
+}
+
+static int rk808_clkout_remove(struct platform_device *pdev)
+{
+	struct rk808 *rk808 = dev_get_drvdata(pdev->dev.parent);
+	struct i2c_client *client = rk808->i2c;
+	struct device_node *node = client->dev.of_node;
+
+	of_clk_del_provider(node);
+
+	return 0;
+}
+
+static struct platform_driver rk808_clkout_driver = {
+	.probe = rk808_clkout_probe,
+	.remove = rk808_clkout_remove,
+	.driver		= {
+		.name	= "rk808-clkout",
+	},
+};
+
+module_platform_driver(rk808_clkout_driver);
+
+MODULE_DESCRIPTION("Clkout driver for the rk808 series PMICs");
+MODULE_AUTHOR("Chris Zhong <zyw@rock-chips.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rk808-clkout");
diff --git a/drivers/clk/clk-twl6040.c b/drivers/clk/clk-twl6040.c
index 1ada79a28052..4a755135bcd3 100644
--- a/drivers/clk/clk-twl6040.c
+++ b/drivers/clk/clk-twl6040.c
@@ -112,7 +112,6 @@ static int twl6040_clk_remove(struct platform_device *pdev)
 static struct platform_driver twl6040_clk_driver = {
 	.driver = {
 		.name = "twl6040-clk",
-		.owner = THIS_MODULE,
 	},
 	.probe = twl6040_clk_probe,
 	.remove = twl6040_clk_remove,
diff --git a/drivers/clk/clk-wm831x.c b/drivers/clk/clk-wm831x.c
index b131041c8f48..ef67719f4e52 100644
--- a/drivers/clk/clk-wm831x.c
+++ b/drivers/clk/clk-wm831x.c
@@ -395,7 +395,6 @@ static struct platform_driver wm831x_clk_driver = {
 	.probe = wm831x_clk_probe,
 	.driver		= {
 		.name	= "wm831x-clk",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/clk/clk.c b/drivers/clk/clk.c
index b76fa69b44cb..4896ae9e23da 100644
--- a/drivers/clk/clk.c
+++ b/drivers/clk/clk.c
@@ -100,6 +100,8 @@ static void clk_enable_unlock(unsigned long flags)
 
 static struct dentry *rootdir;
 static int inited = 0;
+static DEFINE_MUTEX(clk_debug_lock);
+static HLIST_HEAD(clk_debug_list);
 
 static struct hlist_head *all_lists[] = {
 	&clk_root_list,
@@ -117,11 +119,11 @@ static void clk_summary_show_one(struct seq_file *s, struct clk *c, int level)
 	if (!c)
 		return;
 
-	seq_printf(s, "%*s%-*s %11d %12d %11lu %10lu\n",
+	seq_printf(s, "%*s%-*s %11d %12d %11lu %10lu %-3d\n",
 		   level * 3 + 1, "",
 		   30 - level * 3, c->name,
 		   c->enable_count, c->prepare_count, clk_get_rate(c),
-		   clk_get_accuracy(c));
+		   clk_get_accuracy(c), clk_get_phase(c));
 }
 
 static void clk_summary_show_subtree(struct seq_file *s, struct clk *c,
@@ -143,8 +145,8 @@ static int clk_summary_show(struct seq_file *s, void *data)
 	struct clk *c;
 	struct hlist_head **lists = (struct hlist_head **)s->private;
 
-	seq_puts(s, "   clock                         enable_cnt  prepare_cnt        rate   accuracy\n");
-	seq_puts(s, "--------------------------------------------------------------------------------\n");
+	seq_puts(s, "   clock                         enable_cnt  prepare_cnt        rate   accuracy   phase\n");
+	seq_puts(s, "----------------------------------------------------------------------------------------\n");
 
 	clk_prepare_lock();
 
@@ -180,6 +182,7 @@ static void clk_dump_one(struct seq_file *s, struct clk *c, int level)
 	seq_printf(s, "\"prepare_count\": %d,", c->prepare_count);
 	seq_printf(s, "\"rate\": %lu", clk_get_rate(c));
 	seq_printf(s, "\"accuracy\": %lu", clk_get_accuracy(c));
+	seq_printf(s, "\"phase\": %d", clk_get_phase(c));
 }
 
 static void clk_dump_subtree(struct seq_file *s, struct clk *c, int level)
@@ -264,6 +267,11 @@ static int clk_debug_create_one(struct clk *clk, struct dentry *pdentry)
 	if (!d)
 		goto err_out;
 
+	d = debugfs_create_u32("clk_phase", S_IRUGO, clk->dentry,
+			(u32 *)&clk->phase);
+	if (!d)
+		goto err_out;
+
 	d = debugfs_create_x32("clk_flags", S_IRUGO, clk->dentry,
 			(u32 *)&clk->flags);
 	if (!d)
@@ -300,28 +308,6 @@ out:
 	return ret;
 }
 
-/* caller must hold prepare_lock */
-static int clk_debug_create_subtree(struct clk *clk, struct dentry *pdentry)
-{
-	struct clk *child;
-	int ret = -EINVAL;;
-
-	if (!clk || !pdentry)
-		goto out;
-
-	ret = clk_debug_create_one(clk, pdentry);
-
-	if (ret)
-		goto out;
-
-	hlist_for_each_entry(child, &clk->children, child_node)
-		clk_debug_create_subtree(child, pdentry);
-
-	ret = 0;
-out:
-	return ret;
-}
-
 /**
  * clk_debug_register - add a clk node to the debugfs clk tree
  * @clk: the clk being added to the debugfs clk tree
@@ -329,20 +315,21 @@ out:
  * Dynamically adds a clk to the debugfs clk tree if debugfs has been
  * initialized.  Otherwise it bails out early since the debugfs clk tree
  * will be created lazily by clk_debug_init as part of a late_initcall.
- *
- * Caller must hold prepare_lock.  Only clk_init calls this function (so
- * far) so this is taken care.
  */
 static int clk_debug_register(struct clk *clk)
 {
 	int ret = 0;
 
+	mutex_lock(&clk_debug_lock);
+	hlist_add_head(&clk->debug_node, &clk_debug_list);
+
 	if (!inited)
-		goto out;
+		goto unlock;
 
-	ret = clk_debug_create_subtree(clk, rootdir);
+	ret = clk_debug_create_one(clk, rootdir);
+unlock:
+	mutex_unlock(&clk_debug_lock);
 
-out:
 	return ret;
 }
 
@@ -353,12 +340,18 @@ out:
  * Dynamically removes a clk and all it's children clk nodes from the
  * debugfs clk tree if clk->dentry points to debugfs created by
  * clk_debug_register in __clk_init.
- *
- * Caller must hold prepare_lock.
  */
 static void clk_debug_unregister(struct clk *clk)
 {
+	mutex_lock(&clk_debug_lock);
+	if (!clk->dentry)
+		goto out;
+
+	hlist_del_init(&clk->debug_node);
 	debugfs_remove_recursive(clk->dentry);
+	clk->dentry = NULL;
+out:
+	mutex_unlock(&clk_debug_lock);
 }
 
 struct dentry *clk_debugfs_add_file(struct clk *clk, char *name, umode_t mode,
@@ -415,17 +408,12 @@ static int __init clk_debug_init(void)
 	if (!d)
 		return -ENOMEM;
 
-	clk_prepare_lock();
-
-	hlist_for_each_entry(clk, &clk_root_list, child_node)
-		clk_debug_create_subtree(clk, rootdir);
-
-	hlist_for_each_entry(clk, &clk_orphan_list, child_node)
-		clk_debug_create_subtree(clk, rootdir);
+	mutex_lock(&clk_debug_lock);
+	hlist_for_each_entry(clk, &clk_debug_list, debug_node)
+		clk_debug_create_one(clk, rootdir);
 
 	inited = 1;
-
-	clk_prepare_unlock();
+	mutex_unlock(&clk_debug_lock);
 
 	return 0;
 }
@@ -1467,6 +1455,7 @@ static struct clk *clk_propagate_rate_change(struct clk *clk, unsigned long even
 static void clk_change_rate(struct clk *clk)
 {
 	struct clk *child;
+	struct hlist_node *tmp;
 	unsigned long old_rate;
 	unsigned long best_parent_rate = 0;
 	bool skip_set_rate = false;
@@ -1502,7 +1491,11 @@ static void clk_change_rate(struct clk *clk)
 	if (clk->notifier_count && old_rate != clk->rate)
 		__clk_notify(clk, POST_RATE_CHANGE, old_rate, clk->rate);
 
-	hlist_for_each_entry(child, &clk->children, child_node) {
+	/*
+	 * Use safe iteration, as change_rate can actually swap parents
+	 * for certain clock types.
+	 */
+	hlist_for_each_entry_safe(child, tmp, &clk->children, child_node) {
 		/* Skip children who will be reparented to another clock */
 		if (child->new_parent && child->new_parent != clk)
 			continue;
@@ -1739,6 +1732,77 @@ out:
 EXPORT_SYMBOL_GPL(clk_set_parent);
 
 /**
+ * clk_set_phase - adjust the phase shift of a clock signal
+ * @clk: clock signal source
+ * @degrees: number of degrees the signal is shifted
+ *
+ * Shifts the phase of a clock signal by the specified
+ * degrees. Returns 0 on success, -EERROR otherwise.
+ *
+ * This function makes no distinction about the input or reference
+ * signal that we adjust the clock signal phase against. For example
+ * phase locked-loop clock signal generators we may shift phase with
+ * respect to feedback clock signal input, but for other cases the
+ * clock phase may be shifted with respect to some other, unspecified
+ * signal.
+ *
+ * Additionally the concept of phase shift does not propagate through
+ * the clock tree hierarchy, which sets it apart from clock rates and
+ * clock accuracy. A parent clock phase attribute does not have an
+ * impact on the phase attribute of a child clock.
+ */
+int clk_set_phase(struct clk *clk, int degrees)
+{
+	int ret = 0;
+
+	if (!clk)
+		goto out;
+
+	/* sanity check degrees */
+	degrees %= 360;
+	if (degrees < 0)
+		degrees += 360;
+
+	clk_prepare_lock();
+
+	if (!clk->ops->set_phase)
+		goto out_unlock;
+
+	ret = clk->ops->set_phase(clk->hw, degrees);
+
+	if (!ret)
+		clk->phase = degrees;
+
+out_unlock:
+	clk_prepare_unlock();
+
+out:
+	return ret;
+}
+
+/**
+ * clk_get_phase - return the phase shift of a clock signal
+ * @clk: clock signal source
+ *
+ * Returns the phase shift of a clock node in degrees, otherwise returns
+ * -EERROR.
+ */
+int clk_get_phase(struct clk *clk)
+{
+	int ret = 0;
+
+	if (!clk)
+		goto out;
+
+	clk_prepare_lock();
+	ret = clk->phase;
+	clk_prepare_unlock();
+
+out:
+	return ret;
+}
+
+/**
  * __clk_init - initialize the data structures in a struct clk
  * @dev:	device initializing this clk, placeholder for now
  * @clk:	clk being initialized
@@ -1857,6 +1921,16 @@ int __clk_init(struct device *dev, struct clk *clk)
 		clk->accuracy = 0;
 
 	/*
+	 * Set clk's phase.
+	 * Since a phase is by definition relative to its parent, just
+	 * query the current clock phase, or just assume it's in phase.
+	 */
+	if (clk->ops->get_phase)
+		clk->phase = clk->ops->get_phase(clk->hw);
+	else
+		clk->phase = 0;
+
+	/*
 	 * Set clk's rate.  The preferred method is to use .recalc_rate.  For
 	 * simple clocks and lazy developers the default fallback is to use the
 	 * parent's rate.  If a clock doesn't have a parent (or is orphaned)
@@ -2087,14 +2161,16 @@ void clk_unregister(struct clk *clk)
 {
 	unsigned long flags;
 
-       if (!clk || WARN_ON_ONCE(IS_ERR(clk)))
-               return;
+	if (!clk || WARN_ON_ONCE(IS_ERR(clk)))
+		return;
+
+	clk_debug_unregister(clk);
 
 	clk_prepare_lock();
 
 	if (clk->ops == &clk_nodrv_ops) {
 		pr_err("%s: unregistered clock: %s\n", __func__, clk->name);
-		goto out;
+		return;
 	}
 	/*
 	 * Assign empty clock ops for consumers that might still hold
@@ -2113,16 +2189,13 @@ void clk_unregister(struct clk *clk)
 			clk_set_parent(child, NULL);
 	}
 
-	clk_debug_unregister(clk);
-
 	hlist_del_init(&clk->child_node);
 
 	if (clk->prepare_count)
 		pr_warn("%s: unregistering prepared clock: %s\n",
 					__func__, clk->name);
-
 	kref_put(&clk->ref, __clk_release);
-out:
+
 	clk_prepare_unlock();
 }
 EXPORT_SYMBOL_GPL(clk_unregister);
diff --git a/drivers/clk/hisilicon/clk-hix5hd2.c b/drivers/clk/hisilicon/clk-hix5hd2.c
index e5fcfb4e32ef..3f369c60fe56 100644
--- a/drivers/clk/hisilicon/clk-hix5hd2.c
+++ b/drivers/clk/hisilicon/clk-hix5hd2.c
@@ -9,6 +9,8 @@
 
 #include <linux/of_address.h>
 #include <dt-bindings/clock/hix5hd2-clock.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
 #include "clk.h"
 
 static struct hisi_fixed_rate_clock hix5hd2_fixed_rate_clks[] __initdata = {
@@ -48,9 +50,9 @@ static const char *sfc_mux_p[] __initconst = {
 		"24m", "150m", "200m", "100m", "75m", };
 static u32 sfc_mux_table[] = {0, 4, 5, 6, 7};
 
-static const char *sdio1_mux_p[] __initconst = {
+static const char *sdio_mux_p[] __initconst = {
 		"75m", "100m", "50m", "15m", };
-static u32 sdio1_mux_table[] = {0, 1, 2, 3};
+static u32 sdio_mux_table[] = {0, 1, 2, 3};
 
 static const char *fephy_mux_p[] __initconst = { "25m", "125m"};
 static u32 fephy_mux_table[] = {0, 1};
@@ -59,28 +61,243 @@ static u32 fephy_mux_table[] = {0, 1};
 static struct hisi_mux_clock hix5hd2_mux_clks[] __initdata = {
 	{ HIX5HD2_SFC_MUX, "sfc_mux", sfc_mux_p, ARRAY_SIZE(sfc_mux_p),
 		CLK_SET_RATE_PARENT, 0x5c, 8, 3, 0, sfc_mux_table, },
-	{ HIX5HD2_MMC_MUX, "mmc_mux", sdio1_mux_p, ARRAY_SIZE(sdio1_mux_p),
-		CLK_SET_RATE_PARENT, 0xa0, 8, 2, 0, sdio1_mux_table, },
+	{ HIX5HD2_MMC_MUX, "mmc_mux", sdio_mux_p, ARRAY_SIZE(sdio_mux_p),
+		CLK_SET_RATE_PARENT, 0xa0, 8, 2, 0, sdio_mux_table, },
+	{ HIX5HD2_SD_MUX, "sd_mux", sdio_mux_p, ARRAY_SIZE(sdio_mux_p),
+		CLK_SET_RATE_PARENT, 0x9c, 8, 2, 0, sdio_mux_table, },
 	{ HIX5HD2_FEPHY_MUX, "fephy_mux",
 		fephy_mux_p, ARRAY_SIZE(fephy_mux_p),
 		CLK_SET_RATE_PARENT, 0x120, 8, 2, 0, fephy_mux_table, },
 };
 
 static struct hisi_gate_clock hix5hd2_gate_clks[] __initdata = {
-	/*sfc*/
+	/* sfc */
 	{ HIX5HD2_SFC_CLK, "clk_sfc", "sfc_mux",
 		CLK_SET_RATE_PARENT, 0x5c, 0, 0, },
 	{ HIX5HD2_SFC_RST, "rst_sfc", "clk_sfc",
 		CLK_SET_RATE_PARENT, 0x5c, 4, CLK_GATE_SET_TO_DISABLE, },
-	/*sdio1*/
+	/* sdio0 */
+	{ HIX5HD2_SD_BIU_CLK, "clk_sd_biu", "200m",
+		CLK_SET_RATE_PARENT, 0x9c, 0, 0, },
+	{ HIX5HD2_SD_CIU_CLK, "clk_sd_ciu", "sd_mux",
+		CLK_SET_RATE_PARENT, 0x9c, 1, 0, },
+	{ HIX5HD2_SD_CIU_RST, "rst_sd_ciu", "clk_sd_ciu",
+		CLK_SET_RATE_PARENT, 0x9c, 4, CLK_GATE_SET_TO_DISABLE, },
+	/* sdio1 */
 	{ HIX5HD2_MMC_BIU_CLK, "clk_mmc_biu", "200m",
 		CLK_SET_RATE_PARENT, 0xa0, 0, 0, },
 	{ HIX5HD2_MMC_CIU_CLK, "clk_mmc_ciu", "mmc_mux",
 		CLK_SET_RATE_PARENT, 0xa0, 1, 0, },
 	{ HIX5HD2_MMC_CIU_RST, "rst_mmc_ciu", "clk_mmc_ciu",
 		CLK_SET_RATE_PARENT, 0xa0, 4, CLK_GATE_SET_TO_DISABLE, },
+	/* gsf */
+	{ HIX5HD2_FWD_BUS_CLK, "clk_fwd_bus", NULL, 0, 0xcc, 0, 0, },
+	{ HIX5HD2_FWD_SYS_CLK, "clk_fwd_sys", "clk_fwd_bus", 0, 0xcc, 5, 0, },
+	{ HIX5HD2_MAC0_PHY_CLK, "clk_fephy", "clk_fwd_sys",
+		 CLK_SET_RATE_PARENT, 0x120, 0, 0, },
+	/* wdg0 */
+	{ HIX5HD2_WDG0_CLK, "clk_wdg0", "24m",
+		CLK_SET_RATE_PARENT, 0x178, 0, 0, },
+	{ HIX5HD2_WDG0_RST, "rst_wdg0", "clk_wdg0",
+		CLK_SET_RATE_PARENT, 0x178, 4, CLK_GATE_SET_TO_DISABLE, },
+	/* I2C */
+	{HIX5HD2_I2C0_CLK, "clk_i2c0", "100m",
+		 CLK_SET_RATE_PARENT, 0x06c, 4, 0, },
+	{HIX5HD2_I2C0_RST, "rst_i2c0", "clk_i2c0",
+		 CLK_SET_RATE_PARENT, 0x06c, 5, CLK_GATE_SET_TO_DISABLE, },
+	{HIX5HD2_I2C1_CLK, "clk_i2c1", "100m",
+		 CLK_SET_RATE_PARENT, 0x06c, 8, 0, },
+	{HIX5HD2_I2C1_RST, "rst_i2c1", "clk_i2c1",
+		 CLK_SET_RATE_PARENT, 0x06c, 9, CLK_GATE_SET_TO_DISABLE, },
+	{HIX5HD2_I2C2_CLK, "clk_i2c2", "100m",
+		 CLK_SET_RATE_PARENT, 0x06c, 12, 0, },
+	{HIX5HD2_I2C2_RST, "rst_i2c2", "clk_i2c2",
+		 CLK_SET_RATE_PARENT, 0x06c, 13, CLK_GATE_SET_TO_DISABLE, },
+	{HIX5HD2_I2C3_CLK, "clk_i2c3", "100m",
+		 CLK_SET_RATE_PARENT, 0x06c, 16, 0, },
+	{HIX5HD2_I2C3_RST, "rst_i2c3", "clk_i2c3",
+		 CLK_SET_RATE_PARENT, 0x06c, 17, CLK_GATE_SET_TO_DISABLE, },
+	{HIX5HD2_I2C4_CLK, "clk_i2c4", "100m",
+		 CLK_SET_RATE_PARENT, 0x06c, 20, 0, },
+	{HIX5HD2_I2C4_RST, "rst_i2c4", "clk_i2c4",
+		 CLK_SET_RATE_PARENT, 0x06c, 21, CLK_GATE_SET_TO_DISABLE, },
+	{HIX5HD2_I2C5_CLK, "clk_i2c5", "100m",
+		 CLK_SET_RATE_PARENT, 0x06c, 0, 0, },
+	{HIX5HD2_I2C5_RST, "rst_i2c5", "clk_i2c5",
+		 CLK_SET_RATE_PARENT, 0x06c, 1, CLK_GATE_SET_TO_DISABLE, },
 };
 
+enum hix5hd2_clk_type {
+	TYPE_COMPLEX,
+	TYPE_ETHER,
+};
+
+struct hix5hd2_complex_clock {
+	const char	*name;
+	const char	*parent_name;
+	u32		id;
+	u32		ctrl_reg;
+	u32		ctrl_clk_mask;
+	u32		ctrl_rst_mask;
+	u32		phy_reg;
+	u32		phy_clk_mask;
+	u32		phy_rst_mask;
+	enum hix5hd2_clk_type type;
+};
+
+struct hix5hd2_clk_complex {
+	struct clk_hw	hw;
+	u32		id;
+	void __iomem	*ctrl_reg;
+	u32		ctrl_clk_mask;
+	u32		ctrl_rst_mask;
+	void __iomem	*phy_reg;
+	u32		phy_clk_mask;
+	u32		phy_rst_mask;
+};
+
+static struct hix5hd2_complex_clock hix5hd2_complex_clks[] __initdata = {
+	{"clk_mac0", "clk_fephy", HIX5HD2_MAC0_CLK,
+		0xcc, 0xa, 0x500, 0x120, 0, 0x10, TYPE_ETHER},
+	{"clk_mac1", "clk_fwd_sys", HIX5HD2_MAC1_CLK,
+		0xcc, 0x14, 0xa00, 0x168, 0x2, 0, TYPE_ETHER},
+	{"clk_sata", NULL, HIX5HD2_SATA_CLK,
+		0xa8, 0x1f, 0x300, 0xac, 0x1, 0x0, TYPE_COMPLEX},
+	{"clk_usb", NULL, HIX5HD2_USB_CLK,
+		0xb8, 0xff, 0x3f000, 0xbc, 0x7, 0x3f00, TYPE_COMPLEX},
+};
+
+#define to_complex_clk(_hw) container_of(_hw, struct hix5hd2_clk_complex, hw)
+
+static int clk_ether_prepare(struct clk_hw *hw)
+{
+	struct hix5hd2_clk_complex *clk = to_complex_clk(hw);
+	u32 val;
+
+	val = readl_relaxed(clk->ctrl_reg);
+	val |= clk->ctrl_clk_mask | clk->ctrl_rst_mask;
+	writel_relaxed(val, clk->ctrl_reg);
+	val &= ~(clk->ctrl_rst_mask);
+	writel_relaxed(val, clk->ctrl_reg);
+
+	val = readl_relaxed(clk->phy_reg);
+	val |= clk->phy_clk_mask;
+	val &= ~(clk->phy_rst_mask);
+	writel_relaxed(val, clk->phy_reg);
+	mdelay(10);
+
+	val &= ~(clk->phy_clk_mask);
+	val |= clk->phy_rst_mask;
+	writel_relaxed(val, clk->phy_reg);
+	mdelay(10);
+
+	val |= clk->phy_clk_mask;
+	val &= ~(clk->phy_rst_mask);
+	writel_relaxed(val, clk->phy_reg);
+	mdelay(30);
+	return 0;
+}
+
+static void clk_ether_unprepare(struct clk_hw *hw)
+{
+	struct hix5hd2_clk_complex *clk = to_complex_clk(hw);
+	u32 val;
+
+	val = readl_relaxed(clk->ctrl_reg);
+	val &= ~(clk->ctrl_clk_mask);
+	writel_relaxed(val, clk->ctrl_reg);
+}
+
+static struct clk_ops clk_ether_ops = {
+	.prepare = clk_ether_prepare,
+	.unprepare = clk_ether_unprepare,
+};
+
+static int clk_complex_enable(struct clk_hw *hw)
+{
+	struct hix5hd2_clk_complex *clk = to_complex_clk(hw);
+	u32 val;
+
+	val = readl_relaxed(clk->ctrl_reg);
+	val |= clk->ctrl_clk_mask;
+	val &= ~(clk->ctrl_rst_mask);
+	writel_relaxed(val, clk->ctrl_reg);
+
+	val = readl_relaxed(clk->phy_reg);
+	val |= clk->phy_clk_mask;
+	val &= ~(clk->phy_rst_mask);
+	writel_relaxed(val, clk->phy_reg);
+
+	return 0;
+}
+
+static void clk_complex_disable(struct clk_hw *hw)
+{
+	struct hix5hd2_clk_complex *clk = to_complex_clk(hw);
+	u32 val;
+
+	val = readl_relaxed(clk->ctrl_reg);
+	val |= clk->ctrl_rst_mask;
+	val &= ~(clk->ctrl_clk_mask);
+	writel_relaxed(val, clk->ctrl_reg);
+
+	val = readl_relaxed(clk->phy_reg);
+	val |= clk->phy_rst_mask;
+	val &= ~(clk->phy_clk_mask);
+	writel_relaxed(val, clk->phy_reg);
+}
+
+static struct clk_ops clk_complex_ops = {
+	.enable = clk_complex_enable,
+	.disable = clk_complex_disable,
+};
+
+void __init hix5hd2_clk_register_complex(struct hix5hd2_complex_clock *clks,
+					 int nums, struct hisi_clock_data *data)
+{
+	void __iomem *base = data->base;
+	int i;
+
+	for (i = 0; i < nums; i++) {
+		struct hix5hd2_clk_complex *p_clk;
+		struct clk *clk;
+		struct clk_init_data init;
+
+		p_clk = kzalloc(sizeof(*p_clk), GFP_KERNEL);
+		if (!p_clk)
+			return;
+
+		init.name = clks[i].name;
+		if (clks[i].type == TYPE_ETHER)
+			init.ops = &clk_ether_ops;
+		else
+			init.ops = &clk_complex_ops;
+
+		init.flags = CLK_IS_BASIC;
+		init.parent_names =
+			(clks[i].parent_name ? &clks[i].parent_name : NULL);
+		init.num_parents = (clks[i].parent_name ? 1 : 0);
+
+		p_clk->ctrl_reg = base + clks[i].ctrl_reg;
+		p_clk->ctrl_clk_mask = clks[i].ctrl_clk_mask;
+		p_clk->ctrl_rst_mask = clks[i].ctrl_rst_mask;
+		p_clk->phy_reg = base + clks[i].phy_reg;
+		p_clk->phy_clk_mask = clks[i].phy_clk_mask;
+		p_clk->phy_rst_mask = clks[i].phy_rst_mask;
+		p_clk->hw.init = &init;
+
+		clk = clk_register(NULL, &p_clk->hw);
+		if (IS_ERR(clk)) {
+			kfree(p_clk);
+			pr_err("%s: failed to register clock %s\n",
+			       __func__, clks[i].name);
+			continue;
+		}
+
+		data->clk_data.clks[clks[i].id] = clk;
+	}
+}
+
 static void __init hix5hd2_clk_init(struct device_node *np)
 {
 	struct hisi_clock_data *clk_data;
@@ -96,6 +313,9 @@ static void __init hix5hd2_clk_init(struct device_node *np)
 					clk_data);
 	hisi_clk_register_gate(hix5hd2_gate_clks,
 			ARRAY_SIZE(hix5hd2_gate_clks), clk_data);
+	hix5hd2_clk_register_complex(hix5hd2_complex_clks,
+				     ARRAY_SIZE(hix5hd2_complex_clks),
+				     clk_data);
 }
 
 CLK_OF_DECLARE(hix5hd2_clk, "hisilicon,hix5hd2-clock", hix5hd2_clk_init);
diff --git a/drivers/clk/mvebu/armada-370.c b/drivers/clk/mvebu/armada-370.c
index bef198a83863..756f0f39d6a3 100644
--- a/drivers/clk/mvebu/armada-370.c
+++ b/drivers/clk/mvebu/armada-370.c
@@ -23,6 +23,7 @@
  */
 
 #define SARL				0	/* Low part [0:31] */
+#define	 SARL_A370_SSCG_ENABLE		BIT(10)
 #define	 SARL_A370_PCLK_FREQ_OPT	11
 #define	 SARL_A370_PCLK_FREQ_OPT_MASK	0xF
 #define	 SARL_A370_FAB_FREQ_OPT		15
@@ -133,10 +134,17 @@ static void __init a370_get_clk_ratio(
 	}
 }
 
+static bool a370_is_sscg_enabled(void __iomem *sar)
+{
+	return !(readl(sar) & SARL_A370_SSCG_ENABLE);
+}
+
 static const struct coreclk_soc_desc a370_coreclks = {
 	.get_tclk_freq = a370_get_tclk_freq,
 	.get_cpu_freq = a370_get_cpu_freq,
 	.get_clk_ratio = a370_get_clk_ratio,
+	.is_sscg_enabled = a370_is_sscg_enabled,
+	.fix_sscg_deviation = kirkwood_fix_sscg_deviation,
 	.ratios = a370_coreclk_ratios,
 	.num_ratios = ARRAY_SIZE(a370_coreclk_ratios),
 };
diff --git a/drivers/clk/mvebu/armada-375.c b/drivers/clk/mvebu/armada-375.c
index c991a4d95e10..c7af2242b796 100644
--- a/drivers/clk/mvebu/armada-375.c
+++ b/drivers/clk/mvebu/armada-375.c
@@ -27,14 +27,14 @@
  * all modified at the same time, and not separately as for the Armada
  * 370 or the Armada XP SoCs.
  *
- * SAR0[21:17]   : CPU frequency    DDR frequency   L2 frequency
+ * SAR1[21:17]   : CPU frequency    DDR frequency   L2 frequency
  *		 6   =  400 MHz	    400 MHz	    200 MHz
  *		 15  =  600 MHz	    600 MHz	    300 MHz
  *		 21  =  800 MHz	    534 MHz	    400 MHz
  *		 25  = 1000 MHz	    500 MHz	    500 MHz
  *		 others reserved.
  *
- * SAR0[22]   : TCLK frequency
+ * SAR1[22]   : TCLK frequency
  *		 0 = 166 MHz
  *		 1 = 200 MHz
  */
diff --git a/drivers/clk/mvebu/common.c b/drivers/clk/mvebu/common.c
index 25ceccf939ad..b7fcb469c87a 100644
--- a/drivers/clk/mvebu/common.c
+++ b/drivers/clk/mvebu/common.c
@@ -26,8 +26,85 @@
  * Core Clocks
  */
 
+#define SSCG_CONF_MODE(reg)	(((reg) >> 16) & 0x3)
+#define SSCG_SPREAD_DOWN	0x0
+#define SSCG_SPREAD_UP		0x1
+#define SSCG_SPREAD_CENTRAL	0x2
+#define SSCG_CONF_LOW(reg)	(((reg) >> 8) & 0xFF)
+#define SSCG_CONF_HIGH(reg)	((reg) & 0xFF)
+
 static struct clk_onecell_data clk_data;
 
+/*
+ * This function can be used by the Kirkwood, the Armada 370, the
+ * Armada XP and the Armada 375 SoC. The name of the function was
+ * chosen following the dt convention: using the first known SoC
+ * compatible with it.
+ */
+u32 kirkwood_fix_sscg_deviation(u32 system_clk)
+{
+	struct device_node *sscg_np = NULL;
+	void __iomem *sscg_map;
+	u32 sscg_reg;
+	s32 low_bound, high_bound;
+	u64 freq_swing_half;
+
+	sscg_np = of_find_node_by_name(NULL, "sscg");
+	if (sscg_np == NULL) {
+		pr_err("cannot get SSCG register node\n");
+		return system_clk;
+	}
+
+	sscg_map = of_iomap(sscg_np, 0);
+	if (sscg_map == NULL) {
+		pr_err("cannot map SSCG register\n");
+		goto out;
+	}
+
+	sscg_reg = readl(sscg_map);
+	high_bound = SSCG_CONF_HIGH(sscg_reg);
+	low_bound = SSCG_CONF_LOW(sscg_reg);
+
+	if ((high_bound - low_bound) <= 0)
+		goto out;
+	/*
+	 * From Marvell engineer we got the following formula (when
+	 * this code was written, the datasheet was erroneous)
+	 * Spread percentage = 1/96 * (H - L) / H
+	 * H = SSCG_High_Boundary
+	 * L = SSCG_Low_Boundary
+	 *
+	 * As the deviation is half of spread then it lead to the
+	 * following formula in the code.
+	 *
+	 * To avoid an overflow and not lose any significant digit in
+	 * the same time we have to use a 64 bit integer.
+	 */
+
+	freq_swing_half = (((u64)high_bound - (u64)low_bound)
+			* (u64)system_clk);
+	do_div(freq_swing_half, (2 * 96 * high_bound));
+
+	switch (SSCG_CONF_MODE(sscg_reg)) {
+	case SSCG_SPREAD_DOWN:
+		system_clk -= freq_swing_half;
+		break;
+	case SSCG_SPREAD_UP:
+		system_clk += freq_swing_half;
+		break;
+	case SSCG_SPREAD_CENTRAL:
+	default:
+		break;
+	}
+
+	iounmap(sscg_map);
+
+out:
+	of_node_put(sscg_np);
+
+	return system_clk;
+}
+
 void __init mvebu_coreclk_setup(struct device_node *np,
 				const struct coreclk_soc_desc *desc)
 {
@@ -62,6 +139,11 @@ void __init mvebu_coreclk_setup(struct device_node *np,
 	of_property_read_string_index(np, "clock-output-names", 1,
 				      &cpuclk_name);
 	rate = desc->get_cpu_freq(base);
+
+	if (desc->is_sscg_enabled && desc->fix_sscg_deviation
+		&& desc->is_sscg_enabled(base))
+		rate = desc->fix_sscg_deviation(rate);
+
 	clk_data.clks[1] = clk_register_fixed_rate(NULL, cpuclk_name, NULL,
 						   CLK_IS_ROOT, rate);
 	WARN_ON(IS_ERR(clk_data.clks[1]));
@@ -89,8 +171,10 @@ void __init mvebu_coreclk_setup(struct device_node *np,
  * Clock Gating Control
  */
 
+DEFINE_SPINLOCK(ctrl_gating_lock);
+
 struct clk_gating_ctrl {
-	spinlock_t lock;
+	spinlock_t *lock;
 	struct clk **gates;
 	int num_gates;
 };
@@ -138,7 +222,8 @@ void __init mvebu_clk_gating_setup(struct device_node *np,
 	if (WARN_ON(!ctrl))
 		goto ctrl_out;
 
-	spin_lock_init(&ctrl->lock);
+	/* lock must already be initialized */
+	ctrl->lock = &ctrl_gating_lock;
 
 	/* Count, allocate, and register clock gates */
 	for (n = 0; desc[n].name;)
@@ -155,7 +240,7 @@ void __init mvebu_clk_gating_setup(struct device_node *np,
 			(desc[n].parent) ? desc[n].parent : default_parent;
 		ctrl->gates[n] = clk_register_gate(NULL, desc[n].name, parent,
 					desc[n].flags, base, desc[n].bit_idx,
-					0, &ctrl->lock);
+					0, ctrl->lock);
 		WARN_ON(IS_ERR(ctrl->gates[n]));
 	}
 
diff --git a/drivers/clk/mvebu/common.h b/drivers/clk/mvebu/common.h
index f968b4d9df92..783b5631a453 100644
--- a/drivers/clk/mvebu/common.h
+++ b/drivers/clk/mvebu/common.h
@@ -17,6 +17,8 @@
 
 #include <linux/kernel.h>
 
+extern spinlock_t ctrl_gating_lock;
+
 struct device_node;
 
 struct coreclk_ratio {
@@ -28,6 +30,8 @@ struct coreclk_soc_desc {
 	u32 (*get_tclk_freq)(void __iomem *sar);
 	u32 (*get_cpu_freq)(void __iomem *sar);
 	void (*get_clk_ratio)(void __iomem *sar, int id, int *mult, int *div);
+	bool (*is_sscg_enabled)(void __iomem *sar);
+	u32 (*fix_sscg_deviation)(u32 system_clk);
 	const struct coreclk_ratio *ratios;
 	int num_ratios;
 };
@@ -45,4 +49,9 @@ void __init mvebu_coreclk_setup(struct device_node *np,
 void __init mvebu_clk_gating_setup(struct device_node *np,
 				   const struct clk_gating_soc_desc *desc);
 
+/*
+ * This function is shared among the Kirkwood, Armada 370, Armada XP
+ * and Armada 375 SoC
+ */
+u32 kirkwood_fix_sscg_deviation(u32 system_clk);
 #endif
diff --git a/drivers/clk/mvebu/kirkwood.c b/drivers/clk/mvebu/kirkwood.c
index ddb666a86500..99550f25975e 100644
--- a/drivers/clk/mvebu/kirkwood.c
+++ b/drivers/clk/mvebu/kirkwood.c
@@ -13,9 +13,11 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/slab.h>
 #include <linux/clk-provider.h>
 #include <linux/io.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include "common.h"
 
 /*
@@ -214,7 +216,6 @@ static const struct clk_gating_soc_desc kirkwood_gating_desc[] __initconst = {
 	{ "runit", NULL, 7, 0 },
 	{ "xor0", NULL, 8, 0 },
 	{ "audio", NULL, 9, 0 },
-	{ "powersave", "cpuclk", 11, 0 },
 	{ "sata0", NULL, 14, 0 },
 	{ "sata1", NULL, 15, 0 },
 	{ "xor1", NULL, 16, 0 },
@@ -225,6 +226,101 @@ static const struct clk_gating_soc_desc kirkwood_gating_desc[] __initconst = {
 	{ }
 };
 
+
+/*
+ * Clock Muxing Control
+ */
+
+struct clk_muxing_soc_desc {
+	const char *name;
+	const char **parents;
+	int num_parents;
+	int shift;
+	int width;
+	unsigned long flags;
+};
+
+struct clk_muxing_ctrl {
+	spinlock_t *lock;
+	struct clk **muxes;
+	int num_muxes;
+};
+
+static const char *powersave_parents[] = {
+	"cpuclk",
+	"ddrclk",
+};
+
+static const struct clk_muxing_soc_desc kirkwood_mux_desc[] __initconst = {
+	{ "powersave", powersave_parents, ARRAY_SIZE(powersave_parents),
+		11, 1, 0 },
+};
+
+#define to_clk_mux(_hw) container_of(_hw, struct clk_mux, hw)
+
+static struct clk *clk_muxing_get_src(
+	struct of_phandle_args *clkspec, void *data)
+{
+	struct clk_muxing_ctrl *ctrl = (struct clk_muxing_ctrl *)data;
+	int n;
+
+	if (clkspec->args_count < 1)
+		return ERR_PTR(-EINVAL);
+
+	for (n = 0; n < ctrl->num_muxes; n++) {
+		struct clk_mux *mux =
+			to_clk_mux(__clk_get_hw(ctrl->muxes[n]));
+		if (clkspec->args[0] == mux->shift)
+			return ctrl->muxes[n];
+	}
+	return ERR_PTR(-ENODEV);
+}
+
+static void __init kirkwood_clk_muxing_setup(struct device_node *np,
+				   const struct clk_muxing_soc_desc *desc)
+{
+	struct clk_muxing_ctrl *ctrl;
+	void __iomem *base;
+	int n;
+
+	base = of_iomap(np, 0);
+	if (WARN_ON(!base))
+		return;
+
+	ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL);
+	if (WARN_ON(!ctrl))
+		goto ctrl_out;
+
+	/* lock must already be initialized */
+	ctrl->lock = &ctrl_gating_lock;
+
+	/* Count, allocate, and register clock muxes */
+	for (n = 0; desc[n].name;)
+		n++;
+
+	ctrl->num_muxes = n;
+	ctrl->muxes = kcalloc(ctrl->num_muxes, sizeof(struct clk *),
+			GFP_KERNEL);
+	if (WARN_ON(!ctrl->muxes))
+		goto muxes_out;
+
+	for (n = 0; n < ctrl->num_muxes; n++) {
+		ctrl->muxes[n] = clk_register_mux(NULL, desc[n].name,
+				desc[n].parents, desc[n].num_parents,
+				desc[n].flags, base, desc[n].shift,
+				desc[n].width, desc[n].flags, ctrl->lock);
+		WARN_ON(IS_ERR(ctrl->muxes[n]));
+	}
+
+	of_clk_add_provider(np, clk_muxing_get_src, ctrl);
+
+	return;
+muxes_out:
+	kfree(ctrl);
+ctrl_out:
+	iounmap(base);
+}
+
 static void __init kirkwood_clk_init(struct device_node *np)
 {
 	struct device_node *cgnp =
@@ -236,8 +332,10 @@ static void __init kirkwood_clk_init(struct device_node *np)
 	else
 		mvebu_coreclk_setup(np, &kirkwood_coreclks);
 
-	if (cgnp)
+	if (cgnp) {
 		mvebu_clk_gating_setup(cgnp, kirkwood_gating_desc);
+		kirkwood_clk_muxing_setup(cgnp, kirkwood_mux_desc);
+	}
 }
 CLK_OF_DECLARE(kirkwood_clk, "marvell,kirkwood-core-clock",
 	       kirkwood_clk_init);
diff --git a/drivers/clk/pxa/Makefile b/drivers/clk/pxa/Makefile
new file mode 100644
index 000000000000..4ff2abcd500b
--- /dev/null
+++ b/drivers/clk/pxa/Makefile
@@ -0,0 +1,2 @@
+obj-y				+= clk-pxa.o
+obj-$(CONFIG_PXA27x)		+= clk-pxa27x.o
diff --git a/drivers/clk/pxa/clk-pxa.c b/drivers/clk/pxa/clk-pxa.c
new file mode 100644
index 000000000000..ef3c05389c0a
--- /dev/null
+++ b/drivers/clk/pxa/clk-pxa.c
@@ -0,0 +1,97 @@
+/*
+ * Marvell PXA family clocks
+ *
+ * Copyright (C) 2014 Robert Jarzmik
+ *
+ * Common clock code for PXA clocks ("CKEN" type clocks + DT)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ */
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+
+#include <dt-bindings/clock/pxa-clock.h>
+#include "clk-pxa.h"
+
+DEFINE_SPINLOCK(lock);
+
+static struct clk *pxa_clocks[CLK_MAX];
+static struct clk_onecell_data onecell_data = {
+	.clks = pxa_clocks,
+	.clk_num = CLK_MAX,
+};
+
+#define to_pxa_clk(_hw) container_of(_hw, struct pxa_clk_cken, hw)
+
+static unsigned long cken_recalc_rate(struct clk_hw *hw,
+				      unsigned long parent_rate)
+{
+	struct pxa_clk_cken *pclk = to_pxa_clk(hw);
+	struct clk_fixed_factor *fix;
+
+	if (!pclk->is_in_low_power || pclk->is_in_low_power())
+		fix = &pclk->lp;
+	else
+		fix = &pclk->hp;
+	fix->hw.clk = hw->clk;
+	return clk_fixed_factor_ops.recalc_rate(&fix->hw, parent_rate);
+}
+
+static struct clk_ops cken_rate_ops = {
+	.recalc_rate = cken_recalc_rate,
+};
+
+static u8 cken_get_parent(struct clk_hw *hw)
+{
+	struct pxa_clk_cken *pclk = to_pxa_clk(hw);
+
+	if (!pclk->is_in_low_power)
+		return 0;
+	return pclk->is_in_low_power() ? 0 : 1;
+}
+
+static struct clk_ops cken_mux_ops = {
+	.get_parent = cken_get_parent,
+	.set_parent = dummy_clk_set_parent,
+};
+
+void __init clkdev_pxa_register(int ckid, const char *con_id,
+				const char *dev_id, struct clk *clk)
+{
+	if (!IS_ERR(clk) && (ckid != CLK_NONE))
+		pxa_clocks[ckid] = clk;
+	if (!IS_ERR(clk))
+		clk_register_clkdev(clk, con_id, dev_id);
+}
+
+int __init clk_pxa_cken_init(struct pxa_clk_cken *clks, int nb_clks)
+{
+	int i;
+	struct pxa_clk_cken *pclk;
+	struct clk *clk;
+
+	for (i = 0; i < nb_clks; i++) {
+		pclk = clks + i;
+		pclk->gate.lock = &lock;
+		clk = clk_register_composite(NULL, pclk->name,
+					     pclk->parent_names, 2,
+					     &pclk->hw, &cken_mux_ops,
+					     &pclk->hw, &cken_rate_ops,
+					     &pclk->gate.hw, &clk_gate_ops,
+					     pclk->flags);
+		clkdev_pxa_register(pclk->ckid, pclk->con_id, pclk->dev_id,
+				    clk);
+	}
+	return 0;
+}
+
+static void __init pxa_dt_clocks_init(struct device_node *np)
+{
+	of_clk_add_provider(np, of_clk_src_onecell_get, &onecell_data);
+}
+CLK_OF_DECLARE(pxa_clks, "marvell,pxa-clocks", pxa_dt_clocks_init);
diff --git a/drivers/clk/pxa/clk-pxa.h b/drivers/clk/pxa/clk-pxa.h
new file mode 100644
index 000000000000..5fe219d06b49
--- /dev/null
+++ b/drivers/clk/pxa/clk-pxa.h
@@ -0,0 +1,107 @@
+/*
+ * Marvell PXA family clocks
+ *
+ * Copyright (C) 2014 Robert Jarzmik
+ *
+ * Common clock code for PXA clocks ("CKEN" type clocks + DT)
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ */
+#ifndef _CLK_PXA_
+#define _CLK_PXA_
+
+#define PARENTS(name) \
+	static const char *name ## _parents[] __initconst
+#define MUX_RO_RATE_RO_OPS(name, clk_name)			\
+	static struct clk_hw name ## _mux_hw;			\
+	static struct clk_hw name ## _rate_hw;			\
+	static struct clk_ops name ## _mux_ops = {		\
+		.get_parent = name ## _get_parent,		\
+		.set_parent = dummy_clk_set_parent,		\
+	};							\
+	static struct clk_ops name ## _rate_ops = {		\
+		.recalc_rate = name ## _get_rate,		\
+	};							\
+	static struct clk *clk_register_ ## name(void)		\
+	{							\
+		return clk_register_composite(NULL, clk_name,	\
+			name ## _parents,			\
+			ARRAY_SIZE(name ## _parents),		\
+			&name ## _mux_hw, &name ## _mux_ops,	\
+			&name ## _rate_hw, &name ## _rate_ops,	\
+			NULL, NULL, CLK_GET_RATE_NOCACHE);	\
+	}
+
+#define RATE_RO_OPS(name, clk_name)			\
+	static struct clk_hw name ## _rate_hw;			\
+	static struct clk_ops name ## _rate_ops = {		\
+		.recalc_rate = name ## _get_rate,		\
+	};							\
+	static struct clk *clk_register_ ## name(void)		\
+	{							\
+		return clk_register_composite(NULL, clk_name,	\
+			name ## _parents,			\
+			ARRAY_SIZE(name ## _parents),		\
+			NULL, NULL,				\
+			&name ## _rate_hw, &name ## _rate_ops,	\
+			NULL, NULL, CLK_GET_RATE_NOCACHE);	\
+	}
+
+/*
+ * CKEN clock type
+ * This clock takes it source from 2 possible parents :
+ *  - a low power parent
+ *  - a normal parent
+ *
+ *  +------------+     +-----------+
+ *  |  Low Power | --- | x mult_lp |
+ *  |    Clock   |     | / div_lp  |\
+ *  +------------+     +-----------+ \+-----+   +-----------+
+ *                                    | Mux |---| CKEN gate |
+ *  +------------+     +-----------+ /+-----+   +-----------+
+ *  | High Power |     | x mult_hp |/
+ *  |    Clock   | --- | / div_hp  |
+ *  +------------+     +-----------+
+ */
+struct pxa_clk_cken {
+	struct clk_hw hw;
+	int ckid;
+	const char *name;
+	const char *dev_id;
+	const char *con_id;
+	const char **parent_names;
+	struct clk_fixed_factor lp;
+	struct clk_fixed_factor hp;
+	struct clk_gate gate;
+	bool (*is_in_low_power)(void);
+	const unsigned long flags;
+};
+
+#define PXA_CKEN(_dev_id, _con_id, _name, parents, _mult_lp, _div_lp,	\
+		 _mult_hp, _div_hp, is_lp, _cken_reg, _cken_bit, flag)	\
+	{ .ckid = CLK_ ## _name, .name = #_name,			\
+	  .dev_id = _dev_id, .con_id = _con_id,	.parent_names = parents,\
+	  .lp = { .mult = _mult_lp, .div = _div_lp },			\
+	  .hp = { .mult = _mult_hp, .div = _div_hp },			\
+	  .is_in_low_power = is_lp,					\
+	  .gate = { .reg = (void __iomem *)_cken_reg, .bit_idx = _cken_bit }, \
+	  .flags = flag,						\
+	}
+#define PXA_CKEN_1RATE(dev_id, con_id, name, parents, cken_reg,		\
+			    cken_bit, flag)				\
+	PXA_CKEN(dev_id, con_id, name, parents, 1, 1, 1, 1,		\
+		 NULL, cken_reg, cken_bit, flag)
+
+static int dummy_clk_set_parent(struct clk_hw *hw, u8 index)
+{
+	return 0;
+}
+
+extern void clkdev_pxa_register(int ckid, const char *con_id,
+				const char *dev_id, struct clk *clk);
+extern int clk_pxa_cken_init(struct pxa_clk_cken *clks, int nb_clks);
+
+#endif
diff --git a/drivers/clk/pxa/clk-pxa27x.c b/drivers/clk/pxa/clk-pxa27x.c
new file mode 100644
index 000000000000..b345cc791e5d
--- /dev/null
+++ b/drivers/clk/pxa/clk-pxa27x.c
@@ -0,0 +1,370 @@
+/*
+ * Marvell PXA27x family clocks
+ *
+ * Copyright (C) 2014 Robert Jarzmik
+ *
+ * Heavily inspired from former arch/arm/mach-pxa/clock.c.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ */
+#include <linux/clk-provider.h>
+#include <mach/pxa2xx-regs.h>
+#include <linux/io.h>
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/of.h>
+
+#include <dt-bindings/clock/pxa-clock.h>
+#include "clk-pxa.h"
+
+#define KHz 1000
+#define MHz (1000 * 1000)
+
+enum {
+	PXA_CORE_13Mhz = 0,
+	PXA_CORE_RUN,
+	PXA_CORE_TURBO,
+};
+
+enum {
+	PXA_BUS_13Mhz = 0,
+	PXA_BUS_RUN,
+};
+
+enum {
+	PXA_LCD_13Mhz = 0,
+	PXA_LCD_RUN,
+};
+
+enum {
+	PXA_MEM_13Mhz = 0,
+	PXA_MEM_SYSTEM_BUS,
+	PXA_MEM_RUN,
+};
+
+static const char * const get_freq_khz[] = {
+	"core", "run", "cpll", "memory",
+	"system_bus"
+};
+
+/*
+ * Get the clock frequency as reflected by CCSR and the turbo flag.
+ * We assume these values have been applied via a fcs.
+ * If info is not 0 we also display the current settings.
+ */
+unsigned int pxa27x_get_clk_frequency_khz(int info)
+{
+	struct clk *clk;
+	unsigned long clks[5];
+	int i;
+
+	for (i = 0; i < 5; i++) {
+		clk = clk_get(NULL, get_freq_khz[i]);
+		if (IS_ERR(clk)) {
+			clks[i] = 0;
+		} else {
+			clks[i] = clk_get_rate(clk);
+			clk_put(clk);
+		}
+	}
+	if (info) {
+		pr_info("Run Mode clock: %ld.%02ldMHz\n",
+			clks[1] / 1000000, (clks[1] % 1000000) / 10000);
+		pr_info("Turbo Mode clock: %ld.%02ldMHz\n",
+			clks[2] / 1000000, (clks[2] % 1000000) / 10000);
+		pr_info("Memory clock: %ld.%02ldMHz\n",
+			clks[3] / 1000000, (clks[3] % 1000000) / 10000);
+		pr_info("System bus clock: %ld.%02ldMHz\n",
+			clks[4] / 1000000, (clks[4] % 1000000) / 10000);
+	}
+	return (unsigned int)clks[0];
+}
+
+bool pxa27x_is_ppll_disabled(void)
+{
+	unsigned long ccsr = CCSR;
+
+	return ccsr & (1 << CCCR_PPDIS_BIT);
+}
+
+#define PXA27X_CKEN(dev_id, con_id, parents, mult_hp, div_hp,		\
+		    bit, is_lp, flags)					\
+	PXA_CKEN(dev_id, con_id, bit, parents, 1, 1, mult_hp, div_hp,	\
+		 is_lp,  &CKEN, CKEN_ ## bit, flags)
+#define PXA27X_PBUS_CKEN(dev_id, con_id, bit, mult_hp, div_hp, delay)	\
+	PXA27X_CKEN(dev_id, con_id, pxa27x_pbus_parents, mult_hp,	\
+		    div_hp, bit, pxa27x_is_ppll_disabled, 0)
+
+PARENTS(pxa27x_pbus) = { "osc_13mhz", "ppll_312mhz" };
+PARENTS(pxa27x_sbus) = { "system_bus", "system_bus" };
+PARENTS(pxa27x_32Mhz_bus) = { "osc_32_768khz", "osc_32_768khz" };
+PARENTS(pxa27x_lcd_bus) = { "lcd_base", "lcd_base" };
+PARENTS(pxa27x_membus) = { "lcd_base", "lcd_base" };
+
+#define PXA27X_CKEN_1RATE(dev_id, con_id, bit, parents, delay)		\
+	PXA_CKEN_1RATE(dev_id, con_id, bit, parents,			\
+		       &CKEN, CKEN_ ## bit, 0)
+#define PXA27X_CKEN_1RATE_AO(dev_id, con_id, bit, parents, delay)	\
+	PXA_CKEN_1RATE(dev_id, con_id, bit, parents,			\
+		       &CKEN, CKEN_ ## bit, CLK_IGNORE_UNUSED)
+
+static struct pxa_clk_cken pxa27x_clocks[] = {
+	PXA27X_PBUS_CKEN("pxa2xx-uart.0", NULL, FFUART, 2, 42, 1),
+	PXA27X_PBUS_CKEN("pxa2xx-uart.1", NULL, BTUART, 2, 42, 1),
+	PXA27X_PBUS_CKEN("pxa2xx-uart.2", NULL, STUART, 2, 42, 1),
+	PXA27X_PBUS_CKEN("pxa2xx-i2s", NULL, I2S, 2, 51, 0),
+	PXA27X_PBUS_CKEN("pxa2xx-i2c.0", NULL, I2C, 2, 19, 0),
+	PXA27X_PBUS_CKEN("pxa27x-udc", NULL, USB, 2, 13, 5),
+	PXA27X_PBUS_CKEN("pxa2xx-mci.0", NULL, MMC, 2, 32, 0),
+	PXA27X_PBUS_CKEN("pxa2xx-ir", "FICPCLK", FICP, 2, 13, 0),
+	PXA27X_PBUS_CKEN("pxa27x-ohci", NULL, USBHOST, 2, 13, 0),
+	PXA27X_PBUS_CKEN("pxa2xx-i2c.1", NULL, PWRI2C, 1, 24, 0),
+	PXA27X_PBUS_CKEN("pxa27x-ssp.0", NULL, SSP1, 1, 24, 0),
+	PXA27X_PBUS_CKEN("pxa27x-ssp.1", NULL, SSP2, 1, 24, 0),
+	PXA27X_PBUS_CKEN("pxa27x-ssp.2", NULL, SSP3, 1, 24, 0),
+	PXA27X_PBUS_CKEN("pxa27x-pwm.0", NULL, PWM0, 1, 24, 0),
+	PXA27X_PBUS_CKEN("pxa27x-pwm.1", NULL, PWM1, 1, 24, 0),
+	PXA27X_PBUS_CKEN(NULL, "MSLCLK", MSL, 2, 13, 0),
+	PXA27X_PBUS_CKEN(NULL, "USIMCLK", USIM, 2, 13, 0),
+	PXA27X_PBUS_CKEN(NULL, "MSTKCLK", MEMSTK, 2, 32, 0),
+	PXA27X_PBUS_CKEN(NULL, "AC97CLK", AC97, 1, 1, 0),
+	PXA27X_PBUS_CKEN(NULL, "AC97CONFCLK", AC97CONF, 1, 1, 0),
+	PXA27X_PBUS_CKEN(NULL, "OSTIMER0", OSTIMER, 1, 96, 0),
+
+	PXA27X_CKEN_1RATE("pxa27x-keypad", NULL, KEYPAD,
+			  pxa27x_32Mhz_bus_parents, 0),
+	PXA27X_CKEN_1RATE(NULL, "IMCLK", IM, pxa27x_sbus_parents, 0),
+	PXA27X_CKEN_1RATE("pxa2xx-fb", NULL, LCD, pxa27x_lcd_bus_parents, 0),
+	PXA27X_CKEN_1RATE("pxa27x-camera.0", NULL, CAMERA,
+			  pxa27x_lcd_bus_parents, 0),
+	PXA27X_CKEN_1RATE_AO("pxa2xx-pcmcia", NULL, MEMC,
+			     pxa27x_membus_parents, 0),
+
+};
+
+static unsigned long clk_pxa27x_cpll_get_rate(struct clk_hw *hw,
+	unsigned long parent_rate)
+{
+	unsigned long clkcfg;
+	unsigned int t, ht;
+	unsigned int l, L, n2, N;
+	unsigned long ccsr = CCSR;
+
+	asm("mrc\tp14, 0, %0, c6, c0, 0" : "=r" (clkcfg));
+	t  = clkcfg & (1 << 0);
+	ht = clkcfg & (1 << 2);
+
+	l  = ccsr & CCSR_L_MASK;
+	n2 = (ccsr & CCSR_N2_MASK) >> CCSR_N2_SHIFT;
+	L  = l * parent_rate;
+	N  = (L * n2) / 2;
+
+	return t ? N : L;
+}
+PARENTS(clk_pxa27x_cpll) = { "osc_13mhz" };
+RATE_RO_OPS(clk_pxa27x_cpll, "cpll");
+
+static unsigned long clk_pxa27x_lcd_base_get_rate(struct clk_hw *hw,
+						  unsigned long parent_rate)
+{
+	unsigned int l, osc_forced;
+	unsigned long ccsr = CCSR;
+	unsigned long cccr = CCCR;
+
+	l  = ccsr & CCSR_L_MASK;
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	if (osc_forced) {
+		if (cccr & (1 << CCCR_LCD_26_BIT))
+			return parent_rate * 2;
+		else
+			return parent_rate;
+	}
+
+	if (l <= 7)
+		return parent_rate;
+	if (l <= 16)
+		return parent_rate / 2;
+	return parent_rate / 4;
+}
+
+static u8 clk_pxa27x_lcd_base_get_parent(struct clk_hw *hw)
+{
+	unsigned int osc_forced;
+	unsigned long ccsr = CCSR;
+
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	if (osc_forced)
+		return PXA_LCD_13Mhz;
+	else
+		return PXA_LCD_RUN;
+}
+
+PARENTS(clk_pxa27x_lcd_base) = { "osc_13mhz", "run" };
+MUX_RO_RATE_RO_OPS(clk_pxa27x_lcd_base, "lcd_base");
+
+static void __init pxa27x_register_plls(void)
+{
+	clk_register_fixed_rate(NULL, "osc_13mhz", NULL,
+				CLK_GET_RATE_NOCACHE | CLK_IS_ROOT,
+				13 * MHz);
+	clk_register_fixed_rate(NULL, "osc_32_768khz", NULL,
+				CLK_GET_RATE_NOCACHE | CLK_IS_ROOT,
+				32768 * KHz);
+	clk_register_fixed_rate(NULL, "clk_dummy", NULL, CLK_IS_ROOT, 0);
+	clk_register_fixed_factor(NULL, "ppll_312mhz", "osc_13mhz", 0, 24, 1);
+}
+
+static unsigned long clk_pxa27x_core_get_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	unsigned long clkcfg;
+	unsigned int t, ht, b, osc_forced;
+	unsigned long ccsr = CCSR;
+
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	asm("mrc\tp14, 0, %0, c6, c0, 0" : "=r" (clkcfg));
+	t  = clkcfg & (1 << 0);
+	ht = clkcfg & (1 << 2);
+	b  = clkcfg & (1 << 3);
+
+	if (osc_forced)
+		return parent_rate;
+	if (ht)
+		return parent_rate / 2;
+	else
+		return parent_rate;
+}
+
+static u8 clk_pxa27x_core_get_parent(struct clk_hw *hw)
+{
+	unsigned long clkcfg;
+	unsigned int t, ht, b, osc_forced;
+	unsigned long ccsr = CCSR;
+
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	if (osc_forced)
+		return PXA_CORE_13Mhz;
+
+	asm("mrc\tp14, 0, %0, c6, c0, 0" : "=r" (clkcfg));
+	t  = clkcfg & (1 << 0);
+	ht = clkcfg & (1 << 2);
+	b  = clkcfg & (1 << 3);
+
+	if (ht || t)
+		return PXA_CORE_TURBO;
+	return PXA_CORE_RUN;
+}
+PARENTS(clk_pxa27x_core) = { "osc_13mhz", "run", "cpll" };
+MUX_RO_RATE_RO_OPS(clk_pxa27x_core, "core");
+
+static unsigned long clk_pxa27x_run_get_rate(struct clk_hw *hw,
+					     unsigned long parent_rate)
+{
+	unsigned long ccsr = CCSR;
+	unsigned int n2 = (ccsr & CCSR_N2_MASK) >> CCSR_N2_SHIFT;
+
+	return (parent_rate / n2) * 2;
+}
+PARENTS(clk_pxa27x_run) = { "cpll" };
+RATE_RO_OPS(clk_pxa27x_run, "run");
+
+static void __init pxa27x_register_core(void)
+{
+	clk_register_clk_pxa27x_cpll();
+	clk_register_clk_pxa27x_run();
+
+	clkdev_pxa_register(CLK_CORE, "core", NULL,
+			    clk_register_clk_pxa27x_core());
+}
+
+static unsigned long clk_pxa27x_system_bus_get_rate(struct clk_hw *hw,
+						    unsigned long parent_rate)
+{
+	unsigned long clkcfg;
+	unsigned int b, osc_forced;
+	unsigned long ccsr = CCSR;
+
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	asm("mrc\tp14, 0, %0, c6, c0, 0" : "=r" (clkcfg));
+	b  = clkcfg & (1 << 3);
+
+	if (osc_forced)
+		return parent_rate;
+	if (b)
+		return parent_rate / 2;
+	else
+		return parent_rate;
+}
+
+static u8 clk_pxa27x_system_bus_get_parent(struct clk_hw *hw)
+{
+	unsigned int osc_forced;
+	unsigned long ccsr = CCSR;
+
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	if (osc_forced)
+		return PXA_BUS_13Mhz;
+	else
+		return PXA_BUS_RUN;
+}
+
+PARENTS(clk_pxa27x_system_bus) = { "osc_13mhz", "run" };
+MUX_RO_RATE_RO_OPS(clk_pxa27x_system_bus, "system_bus");
+
+static unsigned long clk_pxa27x_memory_get_rate(struct clk_hw *hw,
+						unsigned long parent_rate)
+{
+	unsigned int a, l, osc_forced;
+	unsigned long cccr = CCCR;
+	unsigned long ccsr = CCSR;
+
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	a = cccr & CCCR_A_BIT;
+	l  = ccsr & CCSR_L_MASK;
+
+	if (osc_forced || a)
+		return parent_rate;
+	if (l <= 10)
+		return parent_rate;
+	if (l <= 20)
+		return parent_rate / 2;
+	return parent_rate / 4;
+}
+
+static u8 clk_pxa27x_memory_get_parent(struct clk_hw *hw)
+{
+	unsigned int osc_forced, a;
+	unsigned long cccr = CCCR;
+	unsigned long ccsr = CCSR;
+
+	osc_forced = ccsr & (1 << CCCR_CPDIS_BIT);
+	a = cccr & CCCR_A_BIT;
+	if (osc_forced)
+		return PXA_MEM_13Mhz;
+	if (a)
+		return PXA_MEM_SYSTEM_BUS;
+	else
+		return PXA_MEM_RUN;
+}
+
+PARENTS(clk_pxa27x_memory) = { "osc_13mhz", "system_bus", "run" };
+MUX_RO_RATE_RO_OPS(clk_pxa27x_memory, "memory");
+
+static void __init pxa27x_base_clocks_init(void)
+{
+	pxa27x_register_plls();
+	pxa27x_register_core();
+	clk_register_clk_pxa27x_system_bus();
+	clk_register_clk_pxa27x_memory();
+	clk_register_clk_pxa27x_lcd_base();
+}
+
+static int __init pxa27x_clocks_init(void)
+{
+	pxa27x_base_clocks_init();
+	return clk_pxa_cken_init(pxa27x_clocks, ARRAY_SIZE(pxa27x_clocks));
+}
+postcore_initcall(pxa27x_clocks_init);
diff --git a/drivers/clk/qcom/clk-pll.c b/drivers/clk/qcom/clk-pll.c
index 9db03d3b1657..b823bc3b6250 100644
--- a/drivers/clk/qcom/clk-pll.c
+++ b/drivers/clk/qcom/clk-pll.c
@@ -97,7 +97,7 @@ static unsigned long
 clk_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 {
 	struct clk_pll *pll = to_clk_pll(hw);
-	u32 l, m, n;
+	u32 l, m, n, config;
 	unsigned long rate;
 	u64 tmp;
 
@@ -116,13 +116,79 @@ clk_pll_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 		do_div(tmp, n);
 		rate += tmp;
 	}
+	if (pll->post_div_width) {
+		regmap_read(pll->clkr.regmap, pll->config_reg, &config);
+		config >>= pll->post_div_shift;
+		config &= BIT(pll->post_div_width) - 1;
+		rate /= config + 1;
+	}
+
 	return rate;
 }
 
+static const
+struct pll_freq_tbl *find_freq(const struct pll_freq_tbl *f, unsigned long rate)
+{
+	if (!f)
+		return NULL;
+
+	for (; f->freq; f++)
+		if (rate <= f->freq)
+			return f;
+
+	return NULL;
+}
+
+static long
+clk_pll_determine_rate(struct clk_hw *hw, unsigned long rate,
+		       unsigned long *p_rate, struct clk **p)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+	const struct pll_freq_tbl *f;
+
+	f = find_freq(pll->freq_tbl, rate);
+	if (!f)
+		return clk_pll_recalc_rate(hw, *p_rate);
+
+	return f->freq;
+}
+
+static int
+clk_pll_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long p_rate)
+{
+	struct clk_pll *pll = to_clk_pll(hw);
+	const struct pll_freq_tbl *f;
+	bool enabled;
+	u32 mode;
+	u32 enable_mask = PLL_OUTCTRL | PLL_BYPASSNL | PLL_RESET_N;
+
+	f = find_freq(pll->freq_tbl, rate);
+	if (!f)
+		return -EINVAL;
+
+	regmap_read(pll->clkr.regmap, pll->mode_reg, &mode);
+	enabled = (mode & enable_mask) == enable_mask;
+
+	if (enabled)
+		clk_pll_disable(hw);
+
+	regmap_update_bits(pll->clkr.regmap, pll->l_reg, 0x3ff, f->l);
+	regmap_update_bits(pll->clkr.regmap, pll->m_reg, 0x7ffff, f->m);
+	regmap_update_bits(pll->clkr.regmap, pll->n_reg, 0x7ffff, f->n);
+	regmap_write(pll->clkr.regmap, pll->config_reg, f->ibits);
+
+	if (enabled)
+		clk_pll_enable(hw);
+
+	return 0;
+}
+
 const struct clk_ops clk_pll_ops = {
 	.enable = clk_pll_enable,
 	.disable = clk_pll_disable,
 	.recalc_rate = clk_pll_recalc_rate,
+	.determine_rate = clk_pll_determine_rate,
+	.set_rate = clk_pll_set_rate,
 };
 EXPORT_SYMBOL_GPL(clk_pll_ops);
 
diff --git a/drivers/clk/qcom/clk-pll.h b/drivers/clk/qcom/clk-pll.h
index 3003e9962472..c9c0cda306d0 100644
--- a/drivers/clk/qcom/clk-pll.h
+++ b/drivers/clk/qcom/clk-pll.h
@@ -18,6 +18,21 @@
 #include "clk-regmap.h"
 
 /**
+ * struct pll_freq_tbl - PLL frequency table
+ * @l: L value
+ * @m: M value
+ * @n: N value
+ * @ibits: internal values
+ */
+struct pll_freq_tbl {
+	unsigned long freq;
+	u16 l;
+	u16 m;
+	u16 n;
+	u32 ibits;
+};
+
+/**
  * struct clk_pll - phase locked loop (PLL)
  * @l_reg: L register
  * @m_reg: M register
@@ -26,6 +41,7 @@
  * @mode_reg: mode register
  * @status_reg: status register
  * @status_bit: ANDed with @status_reg to determine if PLL is enabled
+ * @freq_tbl: PLL frequency table
  * @hw: handle between common and hardware-specific interfaces
  */
 struct clk_pll {
@@ -36,6 +52,10 @@ struct clk_pll {
 	u32	mode_reg;
 	u32	status_reg;
 	u8	status_bit;
+	u8	post_div_width;
+	u8	post_div_shift;
+
+	const struct pll_freq_tbl *freq_tbl;
 
 	struct clk_regmap clkr;
 };
diff --git a/drivers/clk/qcom/clk-rcg.c b/drivers/clk/qcom/clk-rcg.c
index b638c5846dbf..b6e6959e89aa 100644
--- a/drivers/clk/qcom/clk-rcg.c
+++ b/drivers/clk/qcom/clk-rcg.c
@@ -21,6 +21,7 @@
 #include <asm/div64.h>
 
 #include "clk-rcg.h"
+#include "common.h"
 
 static u32 ns_to_src(struct src_sel *s, u32 ns)
 {
@@ -67,16 +68,16 @@ static u8 clk_dyn_rcg_get_parent(struct clk_hw *hw)
 {
 	struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
 	int num_parents = __clk_get_num_parents(hw->clk);
-	u32 ns, ctl;
+	u32 ns, reg;
 	int bank;
 	int i;
 	struct src_sel *s;
 
-	regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
-	bank = reg_to_bank(rcg, ctl);
+	regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
+	bank = reg_to_bank(rcg, reg);
 	s = &rcg->s[bank];
 
-	regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
+	regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
 	ns = ns_to_src(s, ns);
 
 	for (i = 0; i < num_parents; i++)
@@ -192,90 +193,93 @@ static u32 mn_to_reg(struct mn *mn, u32 m, u32 n, u32 val)
 
 static void configure_bank(struct clk_dyn_rcg *rcg, const struct freq_tbl *f)
 {
-	u32 ns, md, ctl, *regp;
+	u32 ns, md, reg;
 	int bank, new_bank;
 	struct mn *mn;
 	struct pre_div *p;
 	struct src_sel *s;
 	bool enabled;
-	u32 md_reg;
-	u32 bank_reg;
+	u32 md_reg, ns_reg;
 	bool banked_mn = !!rcg->mn[1].width;
+	bool banked_p = !!rcg->p[1].pre_div_width;
 	struct clk_hw *hw = &rcg->clkr.hw;
 
 	enabled = __clk_is_enabled(hw->clk);
 
-	regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
-	regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
-
-	if (banked_mn) {
-		regp = &ctl;
-		bank_reg = rcg->clkr.enable_reg;
-	} else {
-		regp = &ns;
-		bank_reg = rcg->ns_reg;
-	}
-
-	bank = reg_to_bank(rcg, *regp);
+	regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
+	bank = reg_to_bank(rcg, reg);
 	new_bank = enabled ? !bank : bank;
 
+	ns_reg = rcg->ns_reg[new_bank];
+	regmap_read(rcg->clkr.regmap, ns_reg, &ns);
+
 	if (banked_mn) {
 		mn = &rcg->mn[new_bank];
 		md_reg = rcg->md_reg[new_bank];
 
 		ns |= BIT(mn->mnctr_reset_bit);
-		regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
+		regmap_write(rcg->clkr.regmap, ns_reg, ns);
 
 		regmap_read(rcg->clkr.regmap, md_reg, &md);
 		md = mn_to_md(mn, f->m, f->n, md);
 		regmap_write(rcg->clkr.regmap, md_reg, md);
 
 		ns = mn_to_ns(mn, f->m, f->n, ns);
-		regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
+		regmap_write(rcg->clkr.regmap, ns_reg, ns);
 
-		ctl = mn_to_reg(mn, f->m, f->n, ctl);
-		regmap_write(rcg->clkr.regmap, rcg->clkr.enable_reg, ctl);
+		/* Two NS registers means mode control is in NS register */
+		if (rcg->ns_reg[0] != rcg->ns_reg[1]) {
+			ns = mn_to_reg(mn, f->m, f->n, ns);
+			regmap_write(rcg->clkr.regmap, ns_reg, ns);
+		} else {
+			reg = mn_to_reg(mn, f->m, f->n, reg);
+			regmap_write(rcg->clkr.regmap, rcg->bank_reg, reg);
+		}
 
 		ns &= ~BIT(mn->mnctr_reset_bit);
-		regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
-	} else {
+		regmap_write(rcg->clkr.regmap, ns_reg, ns);
+	}
+
+	if (banked_p) {
 		p = &rcg->p[new_bank];
 		ns = pre_div_to_ns(p, f->pre_div - 1, ns);
 	}
 
 	s = &rcg->s[new_bank];
 	ns = src_to_ns(s, s->parent_map[f->src], ns);
-	regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
+	regmap_write(rcg->clkr.regmap, ns_reg, ns);
 
 	if (enabled) {
-		*regp ^= BIT(rcg->mux_sel_bit);
-		regmap_write(rcg->clkr.regmap, bank_reg, *regp);
+		regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
+		reg ^= BIT(rcg->mux_sel_bit);
+		regmap_write(rcg->clkr.regmap, rcg->bank_reg, reg);
 	}
 }
 
 static int clk_dyn_rcg_set_parent(struct clk_hw *hw, u8 index)
 {
 	struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
-	u32 ns, ctl, md, reg;
+	u32 ns, md, reg;
 	int bank;
 	struct freq_tbl f = { 0 };
 	bool banked_mn = !!rcg->mn[1].width;
+	bool banked_p = !!rcg->p[1].pre_div_width;
 
-	regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
-	regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
-	reg = banked_mn ? ctl : ns;
-
+	regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
 	bank = reg_to_bank(rcg, reg);
 
+	regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
+
 	if (banked_mn) {
 		regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
 		f.m = md_to_m(&rcg->mn[bank], md);
 		f.n = ns_m_to_n(&rcg->mn[bank], ns, f.m);
-	} else {
-		f.pre_div = ns_to_pre_div(&rcg->p[bank], ns) + 1;
 	}
-	f.src = index;
 
+	if (banked_p)
+		f.pre_div = ns_to_pre_div(&rcg->p[bank], ns) + 1;
+
+	f.src = index;
 	configure_bank(rcg, &f);
 
 	return 0;
@@ -336,41 +340,30 @@ clk_dyn_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 	u32 m, n, pre_div, ns, md, mode, reg;
 	int bank;
 	struct mn *mn;
+	bool banked_p = !!rcg->p[1].pre_div_width;
 	bool banked_mn = !!rcg->mn[1].width;
 
-	regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
-
-	if (banked_mn)
-		regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &reg);
-	else
-		reg = ns;
-
+	regmap_read(rcg->clkr.regmap, rcg->bank_reg, &reg);
 	bank = reg_to_bank(rcg, reg);
 
+	regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
+	m = n = pre_div = mode = 0;
+
 	if (banked_mn) {
 		mn = &rcg->mn[bank];
 		regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
 		m = md_to_m(mn, md);
 		n = ns_m_to_n(mn, ns, m);
+		/* Two NS registers means mode control is in NS register */
+		if (rcg->ns_reg[0] != rcg->ns_reg[1])
+			reg = ns;
 		mode = reg_to_mnctr_mode(mn, reg);
-		return calc_rate(parent_rate, m, n, mode, 0);
-	} else {
-		pre_div = ns_to_pre_div(&rcg->p[bank], ns);
-		return calc_rate(parent_rate, 0, 0, 0, pre_div);
 	}
-}
 
-static const
-struct freq_tbl *find_freq(const struct freq_tbl *f, unsigned long rate)
-{
-	if (!f)
-		return NULL;
-
-	for (; f->freq; f++)
-		if (rate <= f->freq)
-			return f;
+	if (banked_p)
+		pre_div = ns_to_pre_div(&rcg->p[bank], ns);
 
-	return NULL;
+	return calc_rate(parent_rate, m, n, mode, pre_div);
 }
 
 static long _freq_tbl_determine_rate(struct clk_hw *hw,
@@ -379,7 +372,7 @@ static long _freq_tbl_determine_rate(struct clk_hw *hw,
 {
 	unsigned long clk_flags;
 
-	f = find_freq(f, rate);
+	f = qcom_find_freq(f, rate);
 	if (!f)
 		return -EINVAL;
 
@@ -477,7 +470,7 @@ static int clk_rcg_set_rate(struct clk_hw *hw, unsigned long rate,
 	struct clk_rcg *rcg = to_clk_rcg(hw);
 	const struct freq_tbl *f;
 
-	f = find_freq(rcg->freq_tbl, rate);
+	f = qcom_find_freq(rcg->freq_tbl, rate);
 	if (!f)
 		return -EINVAL;
 
@@ -497,7 +490,7 @@ static int __clk_dyn_rcg_set_rate(struct clk_hw *hw, unsigned long rate)
 	struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
 	const struct freq_tbl *f;
 
-	f = find_freq(rcg->freq_tbl, rate);
+	f = qcom_find_freq(rcg->freq_tbl, rate);
 	if (!f)
 		return -EINVAL;
 
diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h
index ba0523cefd2e..687e41f91d7c 100644
--- a/drivers/clk/qcom/clk-rcg.h
+++ b/drivers/clk/qcom/clk-rcg.h
@@ -103,8 +103,9 @@ extern const struct clk_ops clk_rcg_bypass_ops;
  * struct clk_dyn_rcg - root clock generator with glitch free mux
  *
  * @mux_sel_bit: bit to switch glitch free mux
- * @ns_reg: NS register
+ * @ns_reg: NS0 and NS1 register
  * @md_reg: MD0 and MD1 register
+ * @bank_reg: register to XOR @mux_sel_bit into to switch glitch free mux
  * @mn: mn counter (banked)
  * @s: source selector (banked)
  * @freq_tbl: frequency table
@@ -113,8 +114,9 @@ extern const struct clk_ops clk_rcg_bypass_ops;
  *
  */
 struct clk_dyn_rcg {
-	u32	ns_reg;
+	u32	ns_reg[2];
 	u32	md_reg[2];
+	u32	bank_reg;
 
 	u8	mux_sel_bit;
 
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index cd185d5cc67a..cfa9eb4fe9ca 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -24,6 +24,7 @@
 #include <asm/div64.h>
 
 #include "clk-rcg.h"
+#include "common.h"
 
 #define CMD_REG			0x0
 #define CMD_UPDATE		BIT(0)
@@ -172,27 +173,13 @@ clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
 	return calc_rate(parent_rate, m, n, mode, hid_div);
 }
 
-static const
-struct freq_tbl *find_freq(const struct freq_tbl *f, unsigned long rate)
-{
-	if (!f)
-		return NULL;
-
-	for (; f->freq; f++)
-		if (rate <= f->freq)
-			return f;
-
-	/* Default to our fastest rate */
-	return f - 1;
-}
-
 static long _freq_tbl_determine_rate(struct clk_hw *hw,
 		const struct freq_tbl *f, unsigned long rate,
 		unsigned long *p_rate, struct clk **p)
 {
 	unsigned long clk_flags;
 
-	f = find_freq(f, rate);
+	f = qcom_find_freq(f, rate);
 	if (!f)
 		return -EINVAL;
 
@@ -268,7 +255,7 @@ static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate)
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
 	const struct freq_tbl *f;
 
-	f = find_freq(rcg->freq_tbl, rate);
+	f = qcom_find_freq(rcg->freq_tbl, rate);
 	if (!f)
 		return -EINVAL;
 
diff --git a/drivers/clk/qcom/common.c b/drivers/clk/qcom/common.c
index eeb3eea01f4c..e20d947db3e5 100644
--- a/drivers/clk/qcom/common.c
+++ b/drivers/clk/qcom/common.c
@@ -18,6 +18,7 @@
 #include <linux/reset-controller.h>
 
 #include "common.h"
+#include "clk-rcg.h"
 #include "clk-regmap.h"
 #include "reset.h"
 
@@ -27,6 +28,21 @@ struct qcom_cc {
 	struct clk *clks[];
 };
 
+const
+struct freq_tbl *qcom_find_freq(const struct freq_tbl *f, unsigned long rate)
+{
+	if (!f)
+		return NULL;
+
+	for (; f->freq; f++)
+		if (rate <= f->freq)
+			return f;
+
+	/* Default to our fastest rate */
+	return f - 1;
+}
+EXPORT_SYMBOL_GPL(qcom_find_freq);
+
 struct regmap *
 qcom_cc_map(struct platform_device *pdev, const struct qcom_cc_desc *desc)
 {
diff --git a/drivers/clk/qcom/common.h b/drivers/clk/qcom/common.h
index 2765e9d3da97..f519322acdf3 100644
--- a/drivers/clk/qcom/common.h
+++ b/drivers/clk/qcom/common.h
@@ -18,6 +18,7 @@ struct regmap_config;
 struct clk_regmap;
 struct qcom_reset_map;
 struct regmap;
+struct freq_tbl;
 
 struct qcom_cc_desc {
 	const struct regmap_config *config;
@@ -27,6 +28,9 @@ struct qcom_cc_desc {
 	size_t num_resets;
 };
 
+extern const struct freq_tbl *qcom_find_freq(const struct freq_tbl *f,
+					     unsigned long rate);
+
 extern struct regmap *qcom_cc_map(struct platform_device *pdev,
 				  const struct qcom_cc_desc *desc);
 extern int qcom_cc_really_probe(struct platform_device *pdev,
diff --git a/drivers/clk/qcom/gcc-ipq806x.c b/drivers/clk/qcom/gcc-ipq806x.c
index 4032e510d9aa..5cd62a709ac7 100644
--- a/drivers/clk/qcom/gcc-ipq806x.c
+++ b/drivers/clk/qcom/gcc-ipq806x.c
@@ -32,6 +32,33 @@
 #include "clk-branch.h"
 #include "reset.h"
 
+static struct clk_pll pll0 = {
+	.l_reg = 0x30c4,
+	.m_reg = 0x30c8,
+	.n_reg = 0x30cc,
+	.config_reg = 0x30d4,
+	.mode_reg = 0x30c0,
+	.status_reg = 0x30d8,
+	.status_bit = 16,
+	.clkr.hw.init = &(struct clk_init_data){
+		.name = "pll0",
+		.parent_names = (const char *[]){ "pxo" },
+		.num_parents = 1,
+		.ops = &clk_pll_ops,
+	},
+};
+
+static struct clk_regmap pll0_vote = {
+	.enable_reg = 0x34c0,
+	.enable_mask = BIT(0),
+	.hw.init = &(struct clk_init_data){
+		.name = "pll0_vote",
+		.parent_names = (const char *[]){ "pll0" },
+		.num_parents = 1,
+		.ops = &clk_pll_vote_ops,
+	},
+};
+
 static struct clk_pll pll3 = {
 	.l_reg = 0x3164,
 	.m_reg = 0x3168,
@@ -154,7 +181,7 @@ static const u8 gcc_pxo_pll8_pll0[] = {
 static const char *gcc_pxo_pll8_pll0_map[] = {
 	"pxo",
 	"pll8_vote",
-	"pll0",
+	"pll0_vote",
 };
 
 static struct freq_tbl clk_tbl_gsbi_uart[] = {
@@ -1095,7 +1122,7 @@ static struct clk_branch prng_clk = {
 };
 
 static const struct freq_tbl clk_tbl_sdc[] = {
-	{    144000, P_PXO,   5, 18,625 },
+	{    200000, P_PXO,   2, 2, 125 },
 	{    400000, P_PLL8,  4, 1, 240 },
 	{  16000000, P_PLL8,  4, 1,   6 },
 	{  17070000, P_PLL8,  1, 2,  45 },
@@ -2133,6 +2160,8 @@ static struct clk_branch usb_fs1_h_clk = {
 };
 
 static struct clk_regmap *gcc_ipq806x_clks[] = {
+	[PLL0] = &pll0.clkr,
+	[PLL0_VOTE] = &pll0_vote,
 	[PLL3] = &pll3.clkr,
 	[PLL8] = &pll8.clkr,
 	[PLL8_VOTE] = &pll8_vote,
diff --git a/drivers/clk/qcom/mmcc-apq8084.c b/drivers/clk/qcom/mmcc-apq8084.c
index 751eea376a2b..dab988ab8cf1 100644
--- a/drivers/clk/qcom/mmcc-apq8084.c
+++ b/drivers/clk/qcom/mmcc-apq8084.c
@@ -3341,7 +3341,6 @@ static struct platform_driver mmcc_apq8084_driver = {
 	.remove		= mmcc_apq8084_remove,
 	.driver		= {
 		.name	= "mmcc-apq8084",
-		.owner	= THIS_MODULE,
 		.of_match_table = mmcc_apq8084_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/mmcc-msm8960.c b/drivers/clk/qcom/mmcc-msm8960.c
index 2e80a219b8ea..e8b33bbc362f 100644
--- a/drivers/clk/qcom/mmcc-msm8960.c
+++ b/drivers/clk/qcom/mmcc-msm8960.c
@@ -773,9 +773,11 @@ static struct freq_tbl clk_tbl_gfx2d[] = {
 };
 
 static struct clk_dyn_rcg gfx2d0_src = {
-	.ns_reg = 0x0070,
+	.ns_reg[0] = 0x0070,
+	.ns_reg[1] = 0x0070,
 	.md_reg[0] = 0x0064,
 	.md_reg[1] = 0x0068,
+	.bank_reg = 0x0060,
 	.mn[0] = {
 		.mnctr_en_bit = 8,
 		.mnctr_reset_bit = 25,
@@ -831,9 +833,11 @@ static struct clk_branch gfx2d0_clk = {
 };
 
 static struct clk_dyn_rcg gfx2d1_src = {
-	.ns_reg = 0x007c,
+	.ns_reg[0] = 0x007c,
+	.ns_reg[1] = 0x007c,
 	.md_reg[0] = 0x0078,
 	.md_reg[1] = 0x006c,
+	.bank_reg = 0x0074,
 	.mn[0] = {
 		.mnctr_en_bit = 8,
 		.mnctr_reset_bit = 25,
@@ -930,9 +934,11 @@ static struct freq_tbl clk_tbl_gfx3d_8064[] = {
 };
 
 static struct clk_dyn_rcg gfx3d_src = {
-	.ns_reg = 0x008c,
+	.ns_reg[0] = 0x008c,
+	.ns_reg[1] = 0x008c,
 	.md_reg[0] = 0x0084,
 	.md_reg[1] = 0x0088,
+	.bank_reg = 0x0080,
 	.mn[0] = {
 		.mnctr_en_bit = 8,
 		.mnctr_reset_bit = 25,
@@ -1006,9 +1012,11 @@ static struct freq_tbl clk_tbl_vcap[] = {
 };
 
 static struct clk_dyn_rcg vcap_src = {
-	.ns_reg = 0x021c,
+	.ns_reg[0] = 0x021c,
+	.ns_reg[1] = 0x021c,
 	.md_reg[0] = 0x01ec,
 	.md_reg[1] = 0x0218,
+	.bank_reg = 0x0178,
 	.mn[0] = {
 		.mnctr_en_bit = 8,
 		.mnctr_reset_bit = 23,
@@ -1211,9 +1219,11 @@ static struct freq_tbl clk_tbl_mdp[] = {
 };
 
 static struct clk_dyn_rcg mdp_src = {
-	.ns_reg = 0x00d0,
+	.ns_reg[0] = 0x00d0,
+	.ns_reg[1] = 0x00d0,
 	.md_reg[0] = 0x00c4,
 	.md_reg[1] = 0x00c8,
+	.bank_reg = 0x00c0,
 	.mn[0] = {
 		.mnctr_en_bit = 8,
 		.mnctr_reset_bit = 31,
@@ -1318,7 +1328,9 @@ static struct freq_tbl clk_tbl_rot[] = {
 };
 
 static struct clk_dyn_rcg rot_src = {
-	.ns_reg = 0x00e8,
+	.ns_reg[0] = 0x00e8,
+	.ns_reg[1] = 0x00e8,
+	.bank_reg = 0x00e8,
 	.p[0] = {
 		.pre_div_shift = 22,
 		.pre_div_width = 4,
@@ -1542,9 +1554,11 @@ static struct freq_tbl clk_tbl_vcodec[] = {
 };
 
 static struct clk_dyn_rcg vcodec_src = {
-	.ns_reg = 0x0100,
+	.ns_reg[0] = 0x0100,
+	.ns_reg[1] = 0x0100,
 	.md_reg[0] = 0x00fc,
 	.md_reg[1] = 0x0128,
+	.bank_reg = 0x00f8,
 	.mn[0] = {
 		.mnctr_en_bit = 5,
 		.mnctr_reset_bit = 31,
@@ -2679,7 +2693,6 @@ static struct platform_driver mmcc_msm8960_driver = {
 	.remove		= mmcc_msm8960_remove,
 	.driver		= {
 		.name	= "mmcc-msm8960",
-		.owner	= THIS_MODULE,
 		.of_match_table = mmcc_msm8960_match_table,
 	},
 };
diff --git a/drivers/clk/qcom/mmcc-msm8974.c b/drivers/clk/qcom/mmcc-msm8974.c
index bc8f519c47aa..be94c54a9a4f 100644
--- a/drivers/clk/qcom/mmcc-msm8974.c
+++ b/drivers/clk/qcom/mmcc-msm8974.c
@@ -2570,7 +2570,6 @@ static struct platform_driver mmcc_msm8974_driver = {
 	.remove		= mmcc_msm8974_remove,
 	.driver		= {
 		.name	= "mmcc-msm8974",
-		.owner	= THIS_MODULE,
 		.of_match_table = mmcc_msm8974_match_table,
 	},
 };
diff --git a/drivers/clk/rockchip/Makefile b/drivers/clk/rockchip/Makefile
index ee6b077381e1..bd8514d63634 100644
--- a/drivers/clk/rockchip/Makefile
+++ b/drivers/clk/rockchip/Makefile
@@ -5,6 +5,7 @@
 obj-y	+= clk-rockchip.o
 obj-y	+= clk.o
 obj-y	+= clk-pll.o
+obj-y	+= clk-cpu.o
 obj-$(CONFIG_RESET_CONTROLLER)	+= softrst.o
 
 obj-y	+= clk-rk3188.o
diff --git a/drivers/clk/rockchip/clk-cpu.c b/drivers/clk/rockchip/clk-cpu.c
new file mode 100644
index 000000000000..75c8c45ef728
--- /dev/null
+++ b/drivers/clk/rockchip/clk-cpu.c
@@ -0,0 +1,329 @@
+/*
+ * Copyright (c) 2014 MundoReader S.L.
+ * Author: Heiko Stuebner <heiko@sntech.de>
+ *
+ * based on clk/samsung/clk-cpu.c
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ * Author: Thomas Abraham <thomas.ab@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * A CPU clock is defined as a clock supplied to a CPU or a group of CPUs.
+ * The CPU clock is typically derived from a hierarchy of clock
+ * blocks which includes mux and divider blocks. There are a number of other
+ * auxiliary clocks supplied to the CPU domain such as the debug blocks and AXI
+ * clock for CPU domain. The rates of these auxiliary clocks are related to the
+ * CPU clock rate and this relation is usually specified in the hardware manual
+ * of the SoC or supplied after the SoC characterization.
+ *
+ * The below implementation of the CPU clock allows the rate changes of the CPU
+ * clock and the corresponding rate changes of the auxillary clocks of the CPU
+ * domain. The platform clock driver provides a clock register configuration
+ * for each configurable rate which is then used to program the clock hardware
+ * registers to acheive a fast co-oridinated rate change for all the CPU domain
+ * clocks.
+ *
+ * On a rate change request for the CPU clock, the rate change is propagated
+ * upto the PLL supplying the clock to the CPU domain clock blocks. While the
+ * CPU domain PLL is reconfigured, the CPU domain clocks are driven using an
+ * alternate clock source. If required, the alternate clock source is divided
+ * down in order to keep the output clock rate within the previous OPP limits.
+ */
+
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/clk-provider.h>
+#include "clk.h"
+
+/**
+ * struct rockchip_cpuclk: information about clock supplied to a CPU core.
+ * @hw:		handle between ccf and cpu clock.
+ * @alt_parent:	alternate parent clock to use when switching the speed
+ *		of the primary parent clock.
+ * @reg_base:	base register for cpu-clock values.
+ * @clk_nb:	clock notifier registered for changes in clock speed of the
+ *		primary parent clock.
+ * @rate_count:	number of rates in the rate_table
+ * @rate_table:	pll-rates and their associated dividers
+ * @reg_data:	cpu-specific register settings
+ * @lock:	clock lock
+ */
+struct rockchip_cpuclk {
+	struct clk_hw				hw;
+
+	struct clk_mux				cpu_mux;
+	const struct clk_ops			*cpu_mux_ops;
+
+	struct clk				*alt_parent;
+	void __iomem				*reg_base;
+	struct notifier_block			clk_nb;
+	unsigned int				rate_count;
+	struct rockchip_cpuclk_rate_table	*rate_table;
+	const struct rockchip_cpuclk_reg_data	*reg_data;
+	spinlock_t				*lock;
+};
+
+#define to_rockchip_cpuclk_hw(hw) container_of(hw, struct rockchip_cpuclk, hw)
+#define to_rockchip_cpuclk_nb(nb) \
+			container_of(nb, struct rockchip_cpuclk, clk_nb)
+
+static const struct rockchip_cpuclk_rate_table *rockchip_get_cpuclk_settings(
+			    struct rockchip_cpuclk *cpuclk, unsigned long rate)
+{
+	const struct rockchip_cpuclk_rate_table *rate_table =
+							cpuclk->rate_table;
+	int i;
+
+	for (i = 0; i < cpuclk->rate_count; i++) {
+		if (rate == rate_table[i].prate)
+			return &rate_table[i];
+	}
+
+	return NULL;
+}
+
+static unsigned long rockchip_cpuclk_recalc_rate(struct clk_hw *hw,
+					unsigned long parent_rate)
+{
+	struct rockchip_cpuclk *cpuclk = to_rockchip_cpuclk_hw(hw);
+	const struct rockchip_cpuclk_reg_data *reg_data = cpuclk->reg_data;
+	u32 clksel0 = readl_relaxed(cpuclk->reg_base + reg_data->core_reg);
+
+	clksel0 >>= reg_data->div_core_shift;
+	clksel0 &= reg_data->div_core_mask;
+	return parent_rate / (clksel0 + 1);
+}
+
+static const struct clk_ops rockchip_cpuclk_ops = {
+	.recalc_rate = rockchip_cpuclk_recalc_rate,
+};
+
+static void rockchip_cpuclk_set_dividers(struct rockchip_cpuclk *cpuclk,
+				const struct rockchip_cpuclk_rate_table *rate)
+{
+	int i;
+
+	/* alternate parent is active now. set the dividers */
+	for (i = 0; i < ARRAY_SIZE(rate->divs); i++) {
+		const struct rockchip_cpuclk_clksel *clksel = &rate->divs[i];
+
+		if (!clksel->reg)
+			continue;
+
+		pr_debug("%s: setting reg 0x%x to 0x%x\n",
+			 __func__, clksel->reg, clksel->val);
+		writel(clksel->val , cpuclk->reg_base + clksel->reg);
+	}
+}
+
+static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk,
+					   struct clk_notifier_data *ndata)
+{
+	const struct rockchip_cpuclk_reg_data *reg_data = cpuclk->reg_data;
+	unsigned long alt_prate, alt_div;
+
+	alt_prate = clk_get_rate(cpuclk->alt_parent);
+
+	spin_lock(cpuclk->lock);
+
+	/*
+	 * If the old parent clock speed is less than the clock speed
+	 * of the alternate parent, then it should be ensured that at no point
+	 * the armclk speed is more than the old_rate until the dividers are
+	 * set.
+	 */
+	if (alt_prate > ndata->old_rate) {
+		/* calculate dividers */
+		alt_div =  DIV_ROUND_UP(alt_prate, ndata->old_rate) - 1;
+		if (alt_div > reg_data->div_core_mask) {
+			pr_warn("%s: limiting alt-divider %lu to %d\n",
+				__func__, alt_div, reg_data->div_core_mask);
+			alt_div = reg_data->div_core_mask;
+		}
+
+		/*
+		 * Change parents and add dividers in a single transaction.
+		 *
+		 * NOTE: we do this in a single transaction so we're never
+		 * dividing the primary parent by the extra dividers that were
+		 * needed for the alt.
+		 */
+		pr_debug("%s: setting div %lu as alt-rate %lu > old-rate %lu\n",
+			 __func__, alt_div, alt_prate, ndata->old_rate);
+
+		writel(HIWORD_UPDATE(alt_div, reg_data->div_core_mask,
+					      reg_data->div_core_shift) |
+		       HIWORD_UPDATE(1, 1, reg_data->mux_core_shift),
+		       cpuclk->reg_base + reg_data->core_reg);
+	} else {
+		/* select alternate parent */
+		writel(HIWORD_UPDATE(1, 1, reg_data->mux_core_shift),
+			cpuclk->reg_base + reg_data->core_reg);
+	}
+
+	spin_unlock(cpuclk->lock);
+	return 0;
+}
+
+static int rockchip_cpuclk_post_rate_change(struct rockchip_cpuclk *cpuclk,
+					    struct clk_notifier_data *ndata)
+{
+	const struct rockchip_cpuclk_reg_data *reg_data = cpuclk->reg_data;
+	const struct rockchip_cpuclk_rate_table *rate;
+
+	rate = rockchip_get_cpuclk_settings(cpuclk, ndata->new_rate);
+	if (!rate) {
+		pr_err("%s: Invalid rate : %lu for cpuclk\n",
+		       __func__, ndata->new_rate);
+		return -EINVAL;
+	}
+
+	spin_lock(cpuclk->lock);
+
+	if (ndata->old_rate < ndata->new_rate)
+		rockchip_cpuclk_set_dividers(cpuclk, rate);
+
+	/*
+	 * post-rate change event, re-mux to primary parent and remove dividers.
+	 *
+	 * NOTE: we do this in a single transaction so we're never dividing the
+	 * primary parent by the extra dividers that were needed for the alt.
+	 */
+
+	writel(HIWORD_UPDATE(0, reg_data->div_core_mask,
+				reg_data->div_core_shift) |
+	       HIWORD_UPDATE(0, 1, reg_data->mux_core_shift),
+	       cpuclk->reg_base + reg_data->core_reg);
+
+	if (ndata->old_rate > ndata->new_rate)
+		rockchip_cpuclk_set_dividers(cpuclk, rate);
+
+	spin_unlock(cpuclk->lock);
+	return 0;
+}
+
+/*
+ * This clock notifier is called when the frequency of the parent clock
+ * of cpuclk is to be changed. This notifier handles the setting up all
+ * the divider clocks, remux to temporary parent and handling the safe
+ * frequency levels when using temporary parent.
+ */
+static int rockchip_cpuclk_notifier_cb(struct notifier_block *nb,
+					unsigned long event, void *data)
+{
+	struct clk_notifier_data *ndata = data;
+	struct rockchip_cpuclk *cpuclk = to_rockchip_cpuclk_nb(nb);
+	int ret = 0;
+
+	pr_debug("%s: event %lu, old_rate %lu, new_rate: %lu\n",
+		 __func__, event, ndata->old_rate, ndata->new_rate);
+	if (event == PRE_RATE_CHANGE)
+		ret = rockchip_cpuclk_pre_rate_change(cpuclk, ndata);
+	else if (event == POST_RATE_CHANGE)
+		ret = rockchip_cpuclk_post_rate_change(cpuclk, ndata);
+
+	return notifier_from_errno(ret);
+}
+
+struct clk *rockchip_clk_register_cpuclk(const char *name,
+			const char **parent_names, u8 num_parents,
+			const struct rockchip_cpuclk_reg_data *reg_data,
+			const struct rockchip_cpuclk_rate_table *rates,
+			int nrates, void __iomem *reg_base, spinlock_t *lock)
+{
+	struct rockchip_cpuclk *cpuclk;
+	struct clk_init_data init;
+	struct clk *clk, *cclk;
+	int ret;
+
+	if (num_parents != 2) {
+		pr_err("%s: needs two parent clocks\n", __func__);
+		return ERR_PTR(-EINVAL);
+	}
+
+	cpuclk = kzalloc(sizeof(*cpuclk), GFP_KERNEL);
+	if (!cpuclk)
+		return ERR_PTR(-ENOMEM);
+
+	init.name = name;
+	init.parent_names = &parent_names[0];
+	init.num_parents = 1;
+	init.ops = &rockchip_cpuclk_ops;
+
+	/* only allow rate changes when we have a rate table */
+	init.flags = (nrates > 0) ? CLK_SET_RATE_PARENT : 0;
+
+	/* disallow automatic parent changes by ccf */
+	init.flags |= CLK_SET_RATE_NO_REPARENT;
+
+	init.flags |= CLK_GET_RATE_NOCACHE;
+
+	cpuclk->reg_base = reg_base;
+	cpuclk->lock = lock;
+	cpuclk->reg_data = reg_data;
+	cpuclk->clk_nb.notifier_call = rockchip_cpuclk_notifier_cb;
+	cpuclk->hw.init = &init;
+
+	cpuclk->alt_parent = __clk_lookup(parent_names[1]);
+	if (!cpuclk->alt_parent) {
+		pr_err("%s: could not lookup alternate parent\n",
+		       __func__);
+		ret = -EINVAL;
+		goto free_cpuclk;
+	}
+
+	ret = clk_prepare_enable(cpuclk->alt_parent);
+	if (ret) {
+		pr_err("%s: could not enable alternate parent\n",
+		       __func__);
+		goto free_cpuclk;
+	}
+
+	clk = __clk_lookup(parent_names[0]);
+	if (!clk) {
+		pr_err("%s: could not lookup parent clock %s\n",
+		       __func__, parent_names[0]);
+		ret = -EINVAL;
+		goto free_cpuclk;
+	}
+
+	ret = clk_notifier_register(clk, &cpuclk->clk_nb);
+	if (ret) {
+		pr_err("%s: failed to register clock notifier for %s\n",
+				__func__, name);
+		goto free_cpuclk;
+	}
+
+	if (nrates > 0) {
+		cpuclk->rate_count = nrates;
+		cpuclk->rate_table = kmemdup(rates,
+					     sizeof(*rates) * nrates,
+					     GFP_KERNEL);
+		if (!cpuclk->rate_table) {
+			pr_err("%s: could not allocate memory for cpuclk rates\n",
+			       __func__);
+			ret = -ENOMEM;
+			goto unregister_notifier;
+		}
+	}
+
+	cclk = clk_register(NULL, &cpuclk->hw);
+	if (IS_ERR(clk)) {
+		pr_err("%s: could not register cpuclk %s\n", __func__,	name);
+		ret = PTR_ERR(clk);
+		goto free_rate_table;
+	}
+
+	return cclk;
+
+free_rate_table:
+	kfree(cpuclk->rate_table);
+unregister_notifier:
+	clk_notifier_unregister(clk, &cpuclk->clk_nb);
+free_cpuclk:
+	kfree(cpuclk);
+	return ERR_PTR(ret);
+}
diff --git a/drivers/clk/rockchip/clk-pll.c b/drivers/clk/rockchip/clk-pll.c
index f2a1c7abf4d9..a3e886a38480 100644
--- a/drivers/clk/rockchip/clk-pll.c
+++ b/drivers/clk/rockchip/clk-pll.c
@@ -34,7 +34,6 @@ struct rockchip_clk_pll {
 	const struct clk_ops	*pll_mux_ops;
 
 	struct notifier_block	clk_nb;
-	bool			rate_change_remuxed;
 
 	void __iomem		*reg_base;
 	int			lock_offset;
@@ -109,38 +108,6 @@ static int rockchip_pll_wait_lock(struct rockchip_clk_pll *pll)
 }
 
 /**
- * Set pll mux when changing the pll rate.
- * This makes sure to move the pll mux away from the actual pll before
- * changing its rate and back to the original parent after the change.
- */
-static int rockchip_pll_notifier_cb(struct notifier_block *nb,
-					unsigned long event, void *data)
-{
-	struct rockchip_clk_pll *pll = to_rockchip_clk_pll_nb(nb);
-	struct clk_mux *pll_mux = &pll->pll_mux;
-	const struct clk_ops *pll_mux_ops = pll->pll_mux_ops;
-	int cur_parent;
-
-	switch (event) {
-	case PRE_RATE_CHANGE:
-		cur_parent = pll_mux_ops->get_parent(&pll_mux->hw);
-		if (cur_parent == PLL_MODE_NORM) {
-			pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_SLOW);
-			pll->rate_change_remuxed = 1;
-		}
-		break;
-	case POST_RATE_CHANGE:
-		if (pll->rate_change_remuxed) {
-			pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_NORM);
-			pll->rate_change_remuxed = 0;
-		}
-		break;
-	}
-
-	return NOTIFY_OK;
-}
-
-/**
  * PLL used in RK3066, RK3188 and RK3288
  */
 
@@ -194,6 +161,10 @@ static int rockchip_rk3066_pll_set_rate(struct clk_hw *hw, unsigned long drate,
 	const struct rockchip_pll_rate_table *rate;
 	unsigned long old_rate = rockchip_rk3066_pll_recalc_rate(hw, prate);
 	struct regmap *grf = rockchip_clk_get_grf();
+	struct clk_mux *pll_mux = &pll->pll_mux;
+	const struct clk_ops *pll_mux_ops = pll->pll_mux_ops;
+	int rate_change_remuxed = 0;
+	int cur_parent;
 	int ret;
 
 	if (IS_ERR(grf)) {
@@ -216,6 +187,12 @@ static int rockchip_rk3066_pll_set_rate(struct clk_hw *hw, unsigned long drate,
 	pr_debug("%s: rate settings for %lu (nr, no, nf): (%d, %d, %d)\n",
 		 __func__, rate->rate, rate->nr, rate->no, rate->nf);
 
+	cur_parent = pll_mux_ops->get_parent(&pll_mux->hw);
+	if (cur_parent == PLL_MODE_NORM) {
+		pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_SLOW);
+		rate_change_remuxed = 1;
+	}
+
 	/* enter reset mode */
 	writel(HIWORD_UPDATE(RK3066_PLLCON3_RESET, RK3066_PLLCON3_RESET, 0),
 	       pll->reg_base + RK3066_PLLCON(3));
@@ -247,6 +224,9 @@ static int rockchip_rk3066_pll_set_rate(struct clk_hw *hw, unsigned long drate,
 		rockchip_rk3066_pll_set_rate(hw, old_rate, prate);
 	}
 
+	if (rate_change_remuxed)
+		pll_mux_ops->set_parent(&pll_mux->hw, PLL_MODE_NORM);
+
 	return ret;
 }
 
@@ -310,7 +290,6 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 	struct clk_mux *pll_mux;
 	struct clk *pll_clk, *mux_clk;
 	char pll_name[20];
-	int ret;
 
 	if (num_parents != 2) {
 		pr_err("%s: needs two parent clocks\n", __func__);
@@ -367,7 +346,6 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 	pll->lock_offset = grf_lock_offset;
 	pll->lock_shift = lock_shift;
 	pll->lock = lock;
-	pll->clk_nb.notifier_call = rockchip_pll_notifier_cb;
 
 	pll_clk = clk_register(NULL, &pll->hw);
 	if (IS_ERR(pll_clk)) {
@@ -377,14 +355,6 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 		goto err_pll;
 	}
 
-	ret = clk_notifier_register(pll_clk, &pll->clk_nb);
-	if (ret) {
-		pr_err("%s: failed to register clock notifier for %s : %d\n",
-				__func__, name, ret);
-		mux_clk = ERR_PTR(ret);
-		goto err_pll_notifier;
-	}
-
 	/* create the mux on top of the real pll */
 	pll->pll_mux_ops = &clk_mux_ops;
 	pll_mux = &pll->pll_mux;
@@ -417,13 +387,6 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 	return mux_clk;
 
 err_mux:
-	ret = clk_notifier_unregister(pll_clk, &pll->clk_nb);
-	if (ret) {
-		pr_err("%s: could not unregister clock notifier in error path : %d\n",
-		       __func__, ret);
-		return mux_clk;
-	}
-err_pll_notifier:
 	clk_unregister(pll_clk);
 err_pll:
 	kfree(pll);
diff --git a/drivers/clk/rockchip/clk-rk3188.c b/drivers/clk/rockchip/clk-rk3188.c
index a83a6d8d0fb6..beed49c79126 100644
--- a/drivers/clk/rockchip/clk-rk3188.c
+++ b/drivers/clk/rockchip/clk-rk3188.c
@@ -19,6 +19,7 @@
 #include <dt-bindings/clock/rk3188-cru-common.h>
 #include "clk.h"
 
+#define RK3066_GRF_SOC_STATUS	0x15c
 #define RK3188_GRF_SOC_STATUS	0xac
 
 enum rk3188_plls {
@@ -100,6 +101,98 @@ struct rockchip_pll_rate_table rk3188_pll_rates[] = {
 	{ /* sentinel */ },
 };
 
+#define RK3066_DIV_CORE_PERIPH_MASK	0x3
+#define RK3066_DIV_CORE_PERIPH_SHIFT	6
+#define RK3066_DIV_ACLK_CORE_MASK	0x7
+#define RK3066_DIV_ACLK_CORE_SHIFT	0
+#define RK3066_DIV_ACLK_HCLK_MASK	0x3
+#define RK3066_DIV_ACLK_HCLK_SHIFT	8
+#define RK3066_DIV_ACLK_PCLK_MASK	0x3
+#define RK3066_DIV_ACLK_PCLK_SHIFT	12
+#define RK3066_DIV_AHB2APB_MASK		0x3
+#define RK3066_DIV_AHB2APB_SHIFT	14
+
+#define RK3066_CLKSEL0(_core_peri)					\
+	{								\
+		.reg = RK2928_CLKSEL_CON(0),				\
+		.val = HIWORD_UPDATE(_core_peri, RK3066_DIV_CORE_PERIPH_MASK, \
+				RK3066_DIV_CORE_PERIPH_SHIFT)		\
+	}
+#define RK3066_CLKSEL1(_aclk_core, _aclk_hclk, _aclk_pclk, _ahb2apb)	\
+	{								\
+		.reg = RK2928_CLKSEL_CON(1),				\
+		.val = HIWORD_UPDATE(_aclk_core, RK3066_DIV_ACLK_CORE_MASK, \
+				RK3066_DIV_ACLK_CORE_SHIFT) |		\
+		       HIWORD_UPDATE(_aclk_hclk, RK3066_DIV_ACLK_HCLK_MASK, \
+				RK3066_DIV_ACLK_HCLK_SHIFT) |		\
+		       HIWORD_UPDATE(_aclk_pclk, RK3066_DIV_ACLK_PCLK_MASK, \
+				RK3066_DIV_ACLK_PCLK_SHIFT) |		\
+		       HIWORD_UPDATE(_ahb2apb, RK3066_DIV_AHB2APB_MASK,	\
+				RK3066_DIV_AHB2APB_SHIFT),		\
+	}
+
+#define RK3066_CPUCLK_RATE(_prate, _core_peri, _acore, _ahclk, _apclk, _h2p) \
+	{								\
+		.prate = _prate,					\
+		.divs = {						\
+			RK3066_CLKSEL0(_core_peri),			\
+			RK3066_CLKSEL1(_acore, _ahclk, _apclk, _h2p),	\
+		},							\
+	}
+
+static struct rockchip_cpuclk_rate_table rk3066_cpuclk_rates[] __initdata = {
+	RK3066_CPUCLK_RATE(1416000000, 2, 3, 1, 2, 1),
+	RK3066_CPUCLK_RATE(1200000000, 2, 3, 1, 2, 1),
+	RK3066_CPUCLK_RATE(1008000000, 2, 2, 1, 2, 1),
+	RK3066_CPUCLK_RATE( 816000000, 2, 2, 1, 2, 1),
+	RK3066_CPUCLK_RATE( 600000000, 1, 2, 1, 2, 1),
+	RK3066_CPUCLK_RATE( 504000000, 1, 1, 1, 2, 1),
+	RK3066_CPUCLK_RATE( 312000000, 0, 1, 1, 1, 0),
+};
+
+static const struct rockchip_cpuclk_reg_data rk3066_cpuclk_data = {
+	.core_reg = RK2928_CLKSEL_CON(0),
+	.div_core_shift = 0,
+	.div_core_mask = 0x1f,
+	.mux_core_shift = 8,
+};
+
+#define RK3188_DIV_ACLK_CORE_MASK	0x7
+#define RK3188_DIV_ACLK_CORE_SHIFT	3
+
+#define RK3188_CLKSEL1(_aclk_core)		\
+	{					\
+		.reg = RK2928_CLKSEL_CON(1),	\
+		.val = HIWORD_UPDATE(_aclk_core, RK3188_DIV_ACLK_CORE_MASK,\
+				 RK3188_DIV_ACLK_CORE_SHIFT) \
+	}
+#define RK3188_CPUCLK_RATE(_prate, _core_peri, _aclk_core)	\
+	{							\
+		.prate = _prate,				\
+		.divs = {					\
+			RK3066_CLKSEL0(_core_peri),		\
+			RK3188_CLKSEL1(_aclk_core),		\
+		},						\
+	}
+
+static struct rockchip_cpuclk_rate_table rk3188_cpuclk_rates[] __initdata = {
+	RK3188_CPUCLK_RATE(1608000000, 2, 3),
+	RK3188_CPUCLK_RATE(1416000000, 2, 3),
+	RK3188_CPUCLK_RATE(1200000000, 2, 3),
+	RK3188_CPUCLK_RATE(1008000000, 2, 3),
+	RK3188_CPUCLK_RATE( 816000000, 2, 3),
+	RK3188_CPUCLK_RATE( 600000000, 1, 3),
+	RK3188_CPUCLK_RATE( 504000000, 1, 3),
+	RK3188_CPUCLK_RATE( 312000000, 0, 1),
+};
+
+static const struct rockchip_cpuclk_reg_data rk3188_cpuclk_data = {
+	.core_reg = RK2928_CLKSEL_CON(0),
+	.div_core_shift = 9,
+	.div_core_mask = 0x1f,
+	.mux_core_shift = 8,
+};
+
 PNAME(mux_pll_p)		= { "xin24m", "xin32k" };
 PNAME(mux_armclk_p)		= { "apll", "gpll_armclk" };
 PNAME(mux_ddrphy_p)		= { "dpll", "gpll_ddr" };
@@ -173,17 +266,10 @@ static struct rockchip_clk_branch common_clk_branches[] __initdata = {
 	GATE(0, "aclk_cpu", "aclk_cpu_pre", 0,
 			RK2928_CLKGATE_CON(0), 3, GFLAGS),
 
-	DIV(0, "pclk_cpu_pre", "aclk_cpu_pre", 0,
-			RK2928_CLKSEL_CON(1), 12, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
 	GATE(0, "atclk_cpu", "pclk_cpu_pre", 0,
 			RK2928_CLKGATE_CON(0), 6, GFLAGS),
 	GATE(0, "pclk_cpu", "pclk_cpu_pre", 0,
 			RK2928_CLKGATE_CON(0), 5, GFLAGS),
-	DIV(0, "hclk_cpu_pre", "aclk_cpu_pre", 0,
-			RK2928_CLKSEL_CON(1), 8, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
-	COMPOSITE_NOMUX(0, "hclk_ahb2apb", "hclk_cpu_pre", 0,
-			RK2928_CLKSEL_CON(1), 14, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
-			RK2928_CLKGATE_CON(4), 9, GFLAGS),
 	GATE(0, "hclk_cpu", "hclk_cpu_pre", 0,
 			RK2928_CLKGATE_CON(0), 4, GFLAGS),
 
@@ -412,10 +498,18 @@ static struct clk_div_table div_aclk_cpu_t[] = {
 };
 
 static struct rockchip_clk_branch rk3066a_clk_branches[] __initdata = {
-	COMPOSITE_NOGATE(0, "armclk", mux_armclk_p, 0,
-			RK2928_CLKSEL_CON(0), 8, 1, MFLAGS, 0, 5, DFLAGS),
 	DIVTBL(0, "aclk_cpu_pre", "armclk", 0,
-			RK2928_CLKSEL_CON(1), 0, 3, DFLAGS, div_aclk_cpu_t),
+			RK2928_CLKSEL_CON(1), 0, 3, DFLAGS | CLK_DIVIDER_READ_ONLY, div_aclk_cpu_t),
+	DIV(0, "pclk_cpu_pre", "aclk_cpu_pre", 0,
+			RK2928_CLKSEL_CON(1), 12, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO
+							    | CLK_DIVIDER_READ_ONLY),
+	DIV(0, "hclk_cpu_pre", "aclk_cpu_pre", 0,
+			RK2928_CLKSEL_CON(1), 8, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO
+							   | CLK_DIVIDER_READ_ONLY),
+	COMPOSITE_NOMUX(0, "hclk_ahb2apb", "hclk_cpu_pre", 0,
+			RK2928_CLKSEL_CON(1), 14, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO
+							    | CLK_DIVIDER_READ_ONLY,
+			RK2928_CLKGATE_CON(4), 9, GFLAGS),
 
 	GATE(CORE_L2C, "core_l2c", "aclk_cpu", 0,
 			RK2928_CLKGATE_CON(9), 4, GFLAGS),
@@ -524,8 +618,6 @@ PNAME(mux_hsicphy_p)		= { "sclk_otgphy0", "sclk_otgphy1",
 				    "gpll", "cpll" };
 
 static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
-	COMPOSITE_NOGATE(0, "armclk", mux_armclk_p, 0,
-			RK2928_CLKSEL_CON(0), 8, 1, MFLAGS, 9, 5, DFLAGS),
 	COMPOSITE_NOMUX_DIVTBL(0, "aclk_core", "armclk", 0,
 			RK2928_CLKSEL_CON(1), 3, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			div_rk3188_aclk_core_t, RK2928_CLKGATE_CON(0), 7, GFLAGS),
@@ -533,6 +625,13 @@ static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
 	/* do not source aclk_cpu_pre from the apll, to keep complexity down */
 	COMPOSITE_NOGATE(0, "aclk_cpu_pre", mux_aclk_cpu_p, CLK_SET_RATE_NO_REPARENT,
 			RK2928_CLKSEL_CON(0), 5, 1, MFLAGS, 0, 5, DFLAGS),
+	DIV(0, "pclk_cpu_pre", "aclk_cpu_pre", 0,
+			RK2928_CLKSEL_CON(1), 12, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
+	DIV(0, "hclk_cpu_pre", "aclk_cpu_pre", 0,
+			RK2928_CLKSEL_CON(1), 8, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO),
+	COMPOSITE_NOMUX(0, "hclk_ahb2apb", "hclk_cpu_pre", 0,
+			RK2928_CLKSEL_CON(1), 14, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
+			RK2928_CLKGATE_CON(4), 9, GFLAGS),
 
 	GATE(CORE_L2C, "core_l2c", "armclk", 0,
 			RK2928_CLKGATE_CON(9), 4, GFLAGS),
@@ -599,6 +698,12 @@ static struct rockchip_clk_branch rk3188_clk_branches[] __initdata = {
 	GATE(ACLK_GPS, "aclk_gps", "aclk_peri", 0, RK2928_CLKGATE_CON(8), 13, GFLAGS),
 };
 
+static const char *rk3188_critical_clocks[] __initconst = {
+	"aclk_cpu",
+	"aclk_peri",
+	"hclk_peri",
+};
+
 static void __init rk3188_common_clk_init(struct device_node *np)
 {
 	void __iomem *reg_base;
@@ -623,29 +728,65 @@ static void __init rk3188_common_clk_init(struct device_node *np)
 		pr_warn("%s: could not register clock usb480m: %ld\n",
 			__func__, PTR_ERR(clk));
 
-	rockchip_clk_register_plls(rk3188_pll_clks,
-				   ARRAY_SIZE(rk3188_pll_clks),
-				   RK3188_GRF_SOC_STATUS);
 	rockchip_clk_register_branches(common_clk_branches,
 				  ARRAY_SIZE(common_clk_branches));
+	rockchip_clk_protect_critical(rk3188_critical_clocks,
+				      ARRAY_SIZE(rk3188_critical_clocks));
 
 	rockchip_register_softrst(np, 9, reg_base + RK2928_SOFTRST_CON(0),
 				  ROCKCHIP_SOFTRST_HIWORD_MASK);
+
+	rockchip_register_restart_notifier(RK2928_GLB_SRST_FST);
 }
 
 static void __init rk3066a_clk_init(struct device_node *np)
 {
 	rk3188_common_clk_init(np);
+	rockchip_clk_register_plls(rk3188_pll_clks,
+				   ARRAY_SIZE(rk3188_pll_clks),
+				   RK3066_GRF_SOC_STATUS);
 	rockchip_clk_register_branches(rk3066a_clk_branches,
 				  ARRAY_SIZE(rk3066a_clk_branches));
+	rockchip_clk_register_armclk(ARMCLK, "armclk",
+			mux_armclk_p, ARRAY_SIZE(mux_armclk_p),
+			&rk3066_cpuclk_data, rk3066_cpuclk_rates,
+			ARRAY_SIZE(rk3066_cpuclk_rates));
 }
 CLK_OF_DECLARE(rk3066a_cru, "rockchip,rk3066a-cru", rk3066a_clk_init);
 
 static void __init rk3188a_clk_init(struct device_node *np)
 {
+	struct clk *clk1, *clk2;
+	unsigned long rate;
+	int ret;
+
 	rk3188_common_clk_init(np);
+	rockchip_clk_register_plls(rk3188_pll_clks,
+				   ARRAY_SIZE(rk3188_pll_clks),
+				   RK3188_GRF_SOC_STATUS);
 	rockchip_clk_register_branches(rk3188_clk_branches,
 				  ARRAY_SIZE(rk3188_clk_branches));
+	rockchip_clk_register_armclk(ARMCLK, "armclk",
+				  mux_armclk_p, ARRAY_SIZE(mux_armclk_p),
+				  &rk3188_cpuclk_data, rk3188_cpuclk_rates,
+				  ARRAY_SIZE(rk3188_cpuclk_rates));
+
+	/* reparent aclk_cpu_pre from apll */
+	clk1 = __clk_lookup("aclk_cpu_pre");
+	clk2 = __clk_lookup("gpll");
+	if (clk1 && clk2) {
+		rate = clk_get_rate(clk1);
+
+		ret = clk_set_parent(clk1, clk2);
+		if (ret < 0)
+			pr_warn("%s: could not reparent aclk_cpu_pre to gpll\n",
+				__func__);
+
+		clk_set_rate(clk1, rate);
+	} else {
+		pr_warn("%s: missing clocks to reparent aclk_cpu_pre to gpll\n",
+			__func__);
+	}
 }
 CLK_OF_DECLARE(rk3188a_cru, "rockchip,rk3188a-cru", rk3188a_clk_init);
 
diff --git a/drivers/clk/rockchip/clk-rk3288.c b/drivers/clk/rockchip/clk-rk3288.c
index 0d8c6c59a75e..23278291da44 100644
--- a/drivers/clk/rockchip/clk-rk3288.c
+++ b/drivers/clk/rockchip/clk-rk3288.c
@@ -20,7 +20,7 @@
 #include "clk.h"
 
 #define RK3288_GRF_SOC_CON(x)	(0x244 + x * 4)
-#define RK3288_GRF_SOC_STATUS	0x280
+#define RK3288_GRF_SOC_STATUS1	0x284
 
 enum rk3288_plls {
 	apll, dpll, cpll, gpll, npll,
@@ -101,6 +101,70 @@ struct rockchip_pll_rate_table rk3288_pll_rates[] = {
 	{ /* sentinel */ },
 };
 
+#define RK3288_DIV_ACLK_CORE_M0_MASK	0xf
+#define RK3288_DIV_ACLK_CORE_M0_SHIFT	0
+#define RK3288_DIV_ACLK_CORE_MP_MASK	0xf
+#define RK3288_DIV_ACLK_CORE_MP_SHIFT	4
+#define RK3288_DIV_L2RAM_MASK		0x7
+#define RK3288_DIV_L2RAM_SHIFT		0
+#define RK3288_DIV_ATCLK_MASK		0x1f
+#define RK3288_DIV_ATCLK_SHIFT		4
+#define RK3288_DIV_PCLK_DBGPRE_MASK	0x1f
+#define RK3288_DIV_PCLK_DBGPRE_SHIFT	9
+
+#define RK3288_CLKSEL0(_core_m0, _core_mp)				\
+	{								\
+		.reg = RK3288_CLKSEL_CON(0),				\
+		.val = HIWORD_UPDATE(_core_m0, RK3288_DIV_ACLK_CORE_M0_MASK, \
+				RK3288_DIV_ACLK_CORE_M0_SHIFT) |	\
+		       HIWORD_UPDATE(_core_mp, RK3288_DIV_ACLK_CORE_MP_MASK, \
+				RK3288_DIV_ACLK_CORE_MP_SHIFT),		\
+	}
+#define RK3288_CLKSEL37(_l2ram, _atclk, _pclk_dbg_pre)			\
+	{								\
+		.reg = RK3288_CLKSEL_CON(37),				\
+		.val = HIWORD_UPDATE(_l2ram, RK3288_DIV_L2RAM_MASK,	\
+				RK3288_DIV_L2RAM_SHIFT) |		\
+		       HIWORD_UPDATE(_atclk, RK3288_DIV_ATCLK_MASK,	\
+				RK3288_DIV_ATCLK_SHIFT) |		\
+		       HIWORD_UPDATE(_pclk_dbg_pre,			\
+				RK3288_DIV_PCLK_DBGPRE_MASK,		\
+				RK3288_DIV_PCLK_DBGPRE_SHIFT),		\
+	}
+
+#define RK3288_CPUCLK_RATE(_prate, _core_m0, _core_mp, _l2ram, _atclk, _pdbg) \
+	{								\
+		.prate = _prate,					\
+		.divs = {						\
+			RK3288_CLKSEL0(_core_m0, _core_mp),		\
+			RK3288_CLKSEL37(_l2ram, _atclk, _pdbg),		\
+		},							\
+	}
+
+static struct rockchip_cpuclk_rate_table rk3288_cpuclk_rates[] __initdata = {
+	RK3288_CPUCLK_RATE(1800000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE(1704000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE(1608000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE(1512000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE(1416000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE(1200000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE(1008000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE( 816000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE( 696000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE( 600000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE( 408000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE( 312000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE( 216000000, 2, 4, 2, 4, 4),
+	RK3288_CPUCLK_RATE( 126000000, 2, 4, 2, 4, 4),
+};
+
+static const struct rockchip_cpuclk_reg_data rk3288_cpuclk_data = {
+	.core_reg = RK3288_CLKSEL_CON(0),
+	.div_core_shift = 8,
+	.div_core_mask = 0x1f,
+	.mux_core_shift = 15,
+};
+
 PNAME(mux_pll_p)		= { "xin24m", "xin32k" };
 PNAME(mux_armclk_p)		= { "apll_core", "gpll_core" };
 PNAME(mux_ddrphy_p)		= { "dpll_ddr", "gpll_ddr" };
@@ -143,7 +207,7 @@ static struct rockchip_pll_clock rk3288_pll_clks[] __initdata = {
 	[gpll] = PLL(pll_rk3066, PLL_GPLL, "gpll", mux_pll_p, 0, RK3288_PLL_CON(12),
 		     RK3288_MODE_CON, 12, 8, rk3288_pll_rates),
 	[npll] = PLL(pll_rk3066, PLL_NPLL, "npll",  mux_pll_p, 0, RK3288_PLL_CON(16),
-		     RK3288_MODE_CON, 14, 9, NULL),
+		     RK3288_MODE_CON, 14, 9, rk3288_pll_rates),
 };
 
 static struct clk_div_table div_hclk_cpu_t[] = {
@@ -166,35 +230,33 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKGATE_CON(0), 1, GFLAGS),
 	GATE(0, "gpll_core", "gpll", 0,
 			RK3288_CLKGATE_CON(0), 2, GFLAGS),
-	COMPOSITE_NOGATE(0, "armclk", mux_armclk_p, 0,
-			RK3288_CLKSEL_CON(0), 15, 1, MFLAGS, 8, 5, DFLAGS),
 
 	COMPOSITE_NOMUX(0, "armcore0", "armclk", 0,
-			RK3288_CLKSEL_CON(36), 0, 3, DFLAGS,
+			RK3288_CLKSEL_CON(36), 0, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 0, GFLAGS),
 	COMPOSITE_NOMUX(0, "armcore1", "armclk", 0,
-			RK3288_CLKSEL_CON(36), 4, 3, DFLAGS,
+			RK3288_CLKSEL_CON(36), 4, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 1, GFLAGS),
 	COMPOSITE_NOMUX(0, "armcore2", "armclk", 0,
-			RK3288_CLKSEL_CON(36), 8, 3, DFLAGS,
+			RK3288_CLKSEL_CON(36), 8, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 2, GFLAGS),
 	COMPOSITE_NOMUX(0, "armcore3", "armclk", 0,
-			RK3288_CLKSEL_CON(36), 12, 3, DFLAGS,
+			RK3288_CLKSEL_CON(36), 12, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 3, GFLAGS),
 	COMPOSITE_NOMUX(0, "l2ram", "armclk", 0,
-			RK3288_CLKSEL_CON(37), 0, 3, DFLAGS,
+			RK3288_CLKSEL_CON(37), 0, 3, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 4, GFLAGS),
 	COMPOSITE_NOMUX(0, "aclk_core_m0", "armclk", 0,
-			RK3288_CLKSEL_CON(0), 0, 4, DFLAGS,
+			RK3288_CLKSEL_CON(0), 0, 4, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 5, GFLAGS),
 	COMPOSITE_NOMUX(0, "aclk_core_mp", "armclk", 0,
-			RK3288_CLKSEL_CON(0), 4, 4, DFLAGS,
+			RK3288_CLKSEL_CON(0), 4, 4, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 6, GFLAGS),
 	COMPOSITE_NOMUX(0, "atclk", "armclk", 0,
-			RK3288_CLKSEL_CON(37), 4, 5, DFLAGS,
+			RK3288_CLKSEL_CON(37), 4, 5, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 7, GFLAGS),
 	COMPOSITE_NOMUX(0, "pclk_dbg_pre", "armclk", 0,
-			RK3288_CLKSEL_CON(37), 9, 5, DFLAGS,
+			RK3288_CLKSEL_CON(37), 9, 5, DFLAGS | CLK_DIVIDER_READ_ONLY,
 			RK3288_CLKGATE_CON(12), 8, GFLAGS),
 	GATE(0, "pclk_dbg", "pclk_dbg_pre", 0,
 			RK3288_CLKGATE_CON(12), 9, GFLAGS),
@@ -219,12 +281,12 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKSEL_CON(1), 15, 1, MFLAGS, 3, 5, DFLAGS),
 	DIV(0, "aclk_cpu_pre", "aclk_cpu_src", 0,
 			RK3288_CLKSEL_CON(1), 0, 3, DFLAGS),
-	GATE(0, "aclk_cpu", "aclk_cpu_pre", 0,
+	GATE(ACLK_CPU, "aclk_cpu", "aclk_cpu_pre", 0,
 			RK3288_CLKGATE_CON(0), 3, GFLAGS),
-	COMPOSITE_NOMUX(0, "pclk_cpu", "aclk_cpu_pre", 0,
+	COMPOSITE_NOMUX(PCLK_CPU, "pclk_cpu", "aclk_cpu_pre", 0,
 			RK3288_CLKSEL_CON(1), 12, 3, DFLAGS,
 			RK3288_CLKGATE_CON(0), 5, GFLAGS),
-	COMPOSITE_NOMUX_DIVTBL(0, "hclk_cpu", "aclk_cpu_pre", 0,
+	COMPOSITE_NOMUX_DIVTBL(HCLK_CPU, "hclk_cpu", "aclk_cpu_pre", 0,
 			RK3288_CLKSEL_CON(1), 8, 2, DFLAGS, div_hclk_cpu_t,
 			RK3288_CLKGATE_CON(0), 4, GFLAGS),
 	GATE(0, "c2c_host", "aclk_cpu_src", 0,
@@ -238,15 +300,15 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE(0, "i2s_src", mux_pll_src_cpll_gpll_p, 0,
 			RK3288_CLKSEL_CON(4), 15, 1, MFLAGS, 0, 7, DFLAGS,
 			RK3288_CLKGATE_CON(4), 1, GFLAGS),
-	COMPOSITE_FRAC(0, "i2s_frac", "i2s_src", 0,
+	COMPOSITE_FRAC(0, "i2s_frac", "i2s_src", CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(8), 0,
 			RK3288_CLKGATE_CON(4), 2, GFLAGS),
-	MUX(0, "i2s_pre", mux_i2s_pre_p, 0,
+	MUX(0, "i2s_pre", mux_i2s_pre_p, CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(4), 8, 2, MFLAGS),
-	COMPOSITE_NODIV(0, "i2s0_clkout", mux_i2s_clkout_p, 0,
+	COMPOSITE_NODIV(0, "i2s0_clkout", mux_i2s_clkout_p, CLK_SET_RATE_PARENT,
 			RK3288_CLKSEL_CON(4), 12, 1, MFLAGS,
 			RK3288_CLKGATE_CON(4), 0, GFLAGS),
-	GATE(SCLK_I2S0, "sclk_i2s0", "i2s_pre", 0,
+	GATE(SCLK_I2S0, "sclk_i2s0", "i2s_pre", CLK_SET_RATE_PARENT,
 			RK3288_CLKGATE_CON(4), 3, GFLAGS),
 
 	MUX(0, "spdif_src", mux_pll_src_cpll_gpll_p, 0,
@@ -296,6 +358,20 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE(0, "aclk_vdpu", mux_pll_src_cpll_gpll_usb480m_p, 0,
 			RK3288_CLKSEL_CON(32), 14, 2, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(3), 11, GFLAGS),
+	/*
+	 * We use aclk_vdpu by default GRF_SOC_CON0[7] setting in system,
+	 * so we ignore the mux and make clocks nodes as following,
+	 */
+	GATE(ACLK_VCODEC, "aclk_vcodec", "aclk_vdpu", 0,
+		RK3288_CLKGATE_CON(9), 0, GFLAGS),
+	/*
+	 * We introduce a virtul node of hclk_vodec_pre_v to split one clock
+	 * struct with a gate and a fix divider into two node in software.
+	 */
+	GATE(0, "hclk_vcodec_pre_v", "aclk_vdpu", 0,
+		RK3288_CLKGATE_CON(3), 10, GFLAGS),
+	GATE(HCLK_VCODEC, "hclk_vcodec", "hclk_vcodec_pre", 0,
+		RK3288_CLKGATE_CON(9), 1, GFLAGS),
 
 	COMPOSITE(0, "aclk_vio0", mux_pll_src_cpll_gpll_usb480m_p, 0,
 			RK3288_CLKSEL_CON(31), 6, 2, MFLAGS, 0, 5, DFLAGS,
@@ -309,7 +385,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE(0, "aclk_rga_pre", mux_pll_src_cpll_gpll_usb480m_p, 0,
 			RK3288_CLKSEL_CON(30), 6, 2, MFLAGS, 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(3), 5, GFLAGS),
-	COMPOSITE(0, "sclk_rga", mux_pll_src_cpll_gpll_usb480m_p, 0,
+	COMPOSITE(SCLK_RGA, "sclk_rga", mux_pll_src_cpll_gpll_usb480m_p, 0,
 			RK3288_CLKSEL_CON(30), 14, 2, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(3), 4, GFLAGS),
 
@@ -320,35 +396,35 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 			RK3288_CLKSEL_CON(29), 6, 2, MFLAGS, 8, 8, DFLAGS,
 			RK3288_CLKGATE_CON(3), 3, GFLAGS),
 
-	COMPOSITE_NODIV(0, "sclk_edp_24m", mux_edp_24m_p, 0,
+	COMPOSITE_NODIV(SCLK_EDP_24M, "sclk_edp_24m", mux_edp_24m_p, 0,
 			RK3288_CLKSEL_CON(28), 15, 1, MFLAGS,
 			RK3288_CLKGATE_CON(3), 12, GFLAGS),
-	COMPOSITE(0, "sclk_edp", mux_pll_src_cpll_gpll_npll_p, 0,
+	COMPOSITE(SCLK_EDP, "sclk_edp", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3288_CLKSEL_CON(28), 6, 2, MFLAGS, 0, 6, DFLAGS,
 			RK3288_CLKGATE_CON(3), 13, GFLAGS),
 
-	COMPOSITE(0, "sclk_isp", mux_pll_src_cpll_gpll_npll_p, 0,
+	COMPOSITE(SCLK_ISP, "sclk_isp", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3288_CLKSEL_CON(6), 6, 2, MFLAGS, 0, 6, DFLAGS,
 			RK3288_CLKGATE_CON(3), 14, GFLAGS),
-	COMPOSITE(0, "sclk_isp_jpe", mux_pll_src_cpll_gpll_npll_p, 0,
+	COMPOSITE(SCLK_ISP_JPE, "sclk_isp_jpe", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3288_CLKSEL_CON(6), 14, 2, MFLAGS, 8, 6, DFLAGS,
 			RK3288_CLKGATE_CON(3), 15, GFLAGS),
 
-	GATE(0, "sclk_hdmi_hdcp", "xin24m", 0,
+	GATE(SCLK_HDMI_HDCP, "sclk_hdmi_hdcp", "xin24m", 0,
 			RK3288_CLKGATE_CON(5), 12, GFLAGS),
-	GATE(0, "sclk_hdmi_cec", "xin32k", 0,
+	GATE(SCLK_HDMI_CEC, "sclk_hdmi_cec", "xin32k", 0,
 			RK3288_CLKGATE_CON(5), 11, GFLAGS),
 
-	COMPOSITE(0, "aclk_hevc", mux_pll_src_cpll_gpll_npll_p, 0,
+	COMPOSITE(ACLK_HEVC, "aclk_hevc", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3288_CLKSEL_CON(39), 14, 2, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(13), 13, GFLAGS),
-	DIV(0, "hclk_hevc", "aclk_hevc", 0,
+	DIV(HCLK_HEVC, "hclk_hevc", "aclk_hevc", 0,
 			RK3288_CLKSEL_CON(40), 12, 2, DFLAGS),
 
-	COMPOSITE(0, "sclk_hevc_cabac", mux_pll_src_cpll_gpll_npll_p, 0,
+	COMPOSITE(SCLK_HEVC_CABAC, "sclk_hevc_cabac", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3288_CLKSEL_CON(42), 6, 2, MFLAGS, 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(13), 14, GFLAGS),
-	COMPOSITE(0, "sclk_hevc_core", mux_pll_src_cpll_gpll_npll_p, 0,
+	COMPOSITE(SCLK_HEVC_CORE, "sclk_hevc_core", mux_pll_src_cpll_gpll_npll_p, 0,
 			RK3288_CLKSEL_CON(42), 14, 2, MFLAGS, 8, 5, DFLAGS,
 			RK3288_CLKGATE_CON(13), 15, GFLAGS),
 
@@ -371,13 +447,13 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	COMPOSITE(0, "aclk_peri_src", mux_pll_src_cpll_gpll_p, 0,
 			RK3288_CLKSEL_CON(10), 15, 1, MFLAGS, 0, 5, DFLAGS,
 			RK3288_CLKGATE_CON(2), 0, GFLAGS),
-	COMPOSITE_NOMUX(0, "pclk_peri", "aclk_peri_src", 0,
+	COMPOSITE_NOMUX(PCLK_PERI, "pclk_peri", "aclk_peri_src", 0,
 			RK3288_CLKSEL_CON(10), 12, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
 			RK3288_CLKGATE_CON(2), 3, GFLAGS),
-	COMPOSITE_NOMUX(0, "hclk_peri", "aclk_peri_src", 0,
+	COMPOSITE_NOMUX(HCLK_PERI, "hclk_peri", "aclk_peri_src", 0,
 			RK3288_CLKSEL_CON(10), 8, 2, DFLAGS | CLK_DIVIDER_POWER_OF_TWO,
 			RK3288_CLKGATE_CON(2), 2, GFLAGS),
-	GATE(0, "aclk_peri", "aclk_peri_src", 0,
+	GATE(ACLK_PERI, "aclk_peri", "aclk_peri_src", 0,
 			RK3288_CLKGATE_CON(2), 1, GFLAGS),
 
 	/*
@@ -545,7 +621,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(PCLK_PWM, "pclk_pwm", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 0, GFLAGS),
 	GATE(PCLK_TIMER, "pclk_timer", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 1, GFLAGS),
 	GATE(PCLK_I2C0, "pclk_i2c0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 2, GFLAGS),
-	GATE(PCLK_I2C1, "pclk_i2c1", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 3, GFLAGS),
+	GATE(PCLK_I2C2, "pclk_i2c2", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 3, GFLAGS),
 	GATE(0, "pclk_ddrupctl0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 14, GFLAGS),
 	GATE(0, "pclk_publ0", "pclk_cpu", 0, RK3288_CLKGATE_CON(10), 15, GFLAGS),
 	GATE(0, "pclk_ddrupctl1", "pclk_cpu", 0, RK3288_CLKGATE_CON(11), 0, GFLAGS),
@@ -603,7 +679,7 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(PCLK_I2C4, "pclk_i2c4", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 15, GFLAGS),
 	GATE(PCLK_UART3, "pclk_uart3", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 11, GFLAGS),
 	GATE(PCLK_UART4, "pclk_uart4", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 12, GFLAGS),
-	GATE(PCLK_I2C2, "pclk_i2c2", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 13, GFLAGS),
+	GATE(PCLK_I2C1, "pclk_i2c1", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 13, GFLAGS),
 	GATE(PCLK_I2C3, "pclk_i2c3", "pclk_peri", 0, RK3288_CLKGATE_CON(6), 14, GFLAGS),
 	GATE(PCLK_SARADC, "pclk_saradc", "pclk_peri", 0, RK3288_CLKGATE_CON(7), 1, GFLAGS),
 	GATE(PCLK_TSADC, "pclk_tsadc", "pclk_peri", 0, RK3288_CLKGATE_CON(7), 2, GFLAGS),
@@ -643,34 +719,34 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(HCLK_RGA, "hclk_rga", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 1, GFLAGS),
 	GATE(HCLK_VOP0, "hclk_vop0", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 6, GFLAGS),
 	GATE(HCLK_VOP1, "hclk_vop1", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 8, GFLAGS),
-	GATE(0, "hclk_vio_ahb_arbi", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 9, GFLAGS),
-	GATE(0, "hclk_vio_niu", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 10, GFLAGS),
-	GATE(0, "hclk_vip", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 15, GFLAGS),
+	GATE(HCLK_VIO_AHB_ARBI, "hclk_vio_ahb_arbi", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 9, GFLAGS),
+	GATE(HCLK_VIO_NIU, "hclk_vio_niu", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 10, GFLAGS),
+	GATE(HCLK_VIP, "hclk_vip", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 15, GFLAGS),
 	GATE(HCLK_IEP, "hclk_iep", "hclk_vio", 0, RK3288_CLKGATE_CON(15), 3, GFLAGS),
 	GATE(HCLK_ISP, "hclk_isp", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 1, GFLAGS),
-	GATE(0, "hclk_vio2_h2p", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 10, GFLAGS),
-	GATE(0, "pclk_mipi_dsi0", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 4, GFLAGS),
-	GATE(0, "pclk_mipi_dsi1", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 5, GFLAGS),
-	GATE(0, "pclk_mipi_csi", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 6, GFLAGS),
-	GATE(0, "pclk_lvds_phy", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 7, GFLAGS),
-	GATE(0, "pclk_edp_ctrl", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 8, GFLAGS),
-	GATE(0, "pclk_hdmi_ctrl", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 9, GFLAGS),
-	GATE(0, "pclk_vio2_h2p", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 11, GFLAGS),
+	GATE(HCLK_VIO2_H2P, "hclk_vio2_h2p", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 10, GFLAGS),
+	GATE(PCLK_MIPI_DSI0, "pclk_mipi_dsi0", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 4, GFLAGS),
+	GATE(PCLK_MIPI_DSI1, "pclk_mipi_dsi1", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 5, GFLAGS),
+	GATE(PCLK_MIPI_CSI, "pclk_mipi_csi", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 6, GFLAGS),
+	GATE(PCLK_LVDS_PHY, "pclk_lvds_phy", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 7, GFLAGS),
+	GATE(PCLK_EDP_CTRL, "pclk_edp_ctrl", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 8, GFLAGS),
+	GATE(PCLK_HDMI_CTRL, "pclk_hdmi_ctrl", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 9, GFLAGS),
+	GATE(PCLK_VIO2_H2P, "pclk_vio2_h2p", "hclk_vio", 0, RK3288_CLKGATE_CON(16), 11, GFLAGS),
 
 	/* aclk_vio0 gates */
 	GATE(ACLK_VOP0, "aclk_vop0", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 5, GFLAGS),
-	GATE(0, "aclk_iep", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 2, GFLAGS),
-	GATE(0, "aclk_vio0_niu", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 11, GFLAGS),
-	GATE(0, "aclk_vip", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 14, GFLAGS),
+	GATE(ACLK_IEP, "aclk_iep", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 2, GFLAGS),
+	GATE(ACLK_VIO0_NIU, "aclk_vio0_niu", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 11, GFLAGS),
+	GATE(ACLK_VIP, "aclk_vip", "aclk_vio0", 0, RK3288_CLKGATE_CON(15), 14, GFLAGS),
 
 	/* aclk_vio1 gates */
 	GATE(ACLK_VOP1, "aclk_vop1", "aclk_vio1", 0, RK3288_CLKGATE_CON(15), 7, GFLAGS),
-	GATE(0, "aclk_isp", "aclk_vio1", 0, RK3288_CLKGATE_CON(16), 2, GFLAGS),
-	GATE(0, "aclk_vio1_niu", "aclk_vio1", 0, RK3288_CLKGATE_CON(15), 12, GFLAGS),
+	GATE(ACLK_ISP, "aclk_isp", "aclk_vio1", 0, RK3288_CLKGATE_CON(16), 2, GFLAGS),
+	GATE(ACLK_VIO1_NIU, "aclk_vio1_niu", "aclk_vio1", 0, RK3288_CLKGATE_CON(15), 12, GFLAGS),
 
 	/* aclk_rga_pre gates */
 	GATE(ACLK_RGA, "aclk_rga", "aclk_rga_pre", 0, RK3288_CLKGATE_CON(15), 0, GFLAGS),
-	GATE(0, "aclk_rga_niu", "aclk_rga_pre", 0, RK3288_CLKGATE_CON(15), 13, GFLAGS),
+	GATE(ACLK_RGA_NIU, "aclk_rga_niu", "aclk_rga_pre", 0, RK3288_CLKGATE_CON(15), 13, GFLAGS),
 
 	/*
 	 * Other ungrouped clocks.
@@ -680,6 +756,12 @@ static struct rockchip_clk_branch rk3288_clk_branches[] __initdata = {
 	GATE(0, "pclk_isp_in", "ext_isp", 0, RK3288_CLKGATE_CON(16), 3, GFLAGS),
 };
 
+static const char *rk3288_critical_clocks[] __initconst = {
+	"aclk_cpu",
+	"aclk_peri",
+	"hclk_peri",
+};
+
 static void __init rk3288_clk_init(struct device_node *np)
 {
 	void __iomem *reg_base;
@@ -705,13 +787,28 @@ static void __init rk3288_clk_init(struct device_node *np)
 		pr_warn("%s: could not register clock usb480m: %ld\n",
 			__func__, PTR_ERR(clk));
 
+	clk = clk_register_fixed_factor(NULL, "hclk_vcodec_pre",
+					"hclk_vcodec_pre_v", 0, 1, 4);
+	if (IS_ERR(clk))
+		pr_warn("%s: could not register clock hclk_vcodec_pre: %ld\n",
+			__func__, PTR_ERR(clk));
+
 	rockchip_clk_register_plls(rk3288_pll_clks,
 				   ARRAY_SIZE(rk3288_pll_clks),
-				   RK3288_GRF_SOC_STATUS);
+				   RK3288_GRF_SOC_STATUS1);
 	rockchip_clk_register_branches(rk3288_clk_branches,
 				  ARRAY_SIZE(rk3288_clk_branches));
+	rockchip_clk_protect_critical(rk3288_critical_clocks,
+				      ARRAY_SIZE(rk3288_critical_clocks));
 
-	rockchip_register_softrst(np, 9, reg_base + RK3288_SOFTRST_CON(0),
+	rockchip_clk_register_armclk(ARMCLK, "armclk",
+			mux_armclk_p, ARRAY_SIZE(mux_armclk_p),
+			&rk3288_cpuclk_data, rk3288_cpuclk_rates,
+			ARRAY_SIZE(rk3288_cpuclk_rates));
+
+	rockchip_register_softrst(np, 12, reg_base + RK3288_SOFTRST_CON(0),
 				  ROCKCHIP_SOFTRST_HIWORD_MASK);
+
+	rockchip_register_restart_notifier(RK3288_GLB_SRST_FST);
 }
 CLK_OF_DECLARE(rk3288_cru, "rockchip,rk3288-cru", rk3288_clk_init);
diff --git a/drivers/clk/rockchip/clk.c b/drivers/clk/rockchip/clk.c
index 278cf9dd1e23..1e68bff481b8 100644
--- a/drivers/clk/rockchip/clk.c
+++ b/drivers/clk/rockchip/clk.c
@@ -25,6 +25,7 @@
 #include <linux/clk-provider.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
+#include <linux/reboot.h>
 #include "clk.h"
 
 /**
@@ -37,7 +38,7 @@
  *
  * sometimes without one of those components.
  */
-struct clk *rockchip_clk_register_branch(const char *name,
+static struct clk *rockchip_clk_register_branch(const char *name,
 		const char **parent_names, u8 num_parents, void __iomem *base,
 		int muxdiv_offset, u8 mux_shift, u8 mux_width, u8 mux_flags,
 		u8 div_shift, u8 div_width, u8 div_flags,
@@ -103,6 +104,54 @@ struct clk *rockchip_clk_register_branch(const char *name,
 	return clk;
 }
 
+static struct clk *rockchip_clk_register_frac_branch(const char *name,
+		const char **parent_names, u8 num_parents, void __iomem *base,
+		int muxdiv_offset, u8 div_flags,
+		int gate_offset, u8 gate_shift, u8 gate_flags,
+		unsigned long flags, spinlock_t *lock)
+{
+	struct clk *clk;
+	struct clk_gate *gate = NULL;
+	struct clk_fractional_divider *div = NULL;
+	const struct clk_ops *div_ops = NULL, *gate_ops = NULL;
+
+	if (gate_offset >= 0) {
+		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
+		if (!gate)
+			return ERR_PTR(-ENOMEM);
+
+		gate->flags = gate_flags;
+		gate->reg = base + gate_offset;
+		gate->bit_idx = gate_shift;
+		gate->lock = lock;
+		gate_ops = &clk_gate_ops;
+	}
+
+	if (muxdiv_offset < 0)
+		return ERR_PTR(-EINVAL);
+
+	div = kzalloc(sizeof(*div), GFP_KERNEL);
+	if (!div)
+		return ERR_PTR(-ENOMEM);
+
+	div->flags = div_flags;
+	div->reg = base + muxdiv_offset;
+	div->mshift = 16;
+	div->mmask = 0xffff0000;
+	div->nshift = 0;
+	div->nmask = 0xffff;
+	div->lock = lock;
+	div_ops = &clk_fractional_divider_ops;
+
+	clk = clk_register_composite(NULL, name, parent_names, num_parents,
+				     NULL, NULL,
+				     &div->hw, div_ops,
+				     gate ? &gate->hw : NULL, gate_ops,
+				     flags);
+
+	return clk;
+}
+
 static DEFINE_SPINLOCK(clk_lock);
 static struct clk **clk_table;
 static void __iomem *reg_base;
@@ -197,8 +246,14 @@ void __init rockchip_clk_register_branches(
 					list->div_flags, &clk_lock);
 			break;
 		case branch_fraction_divider:
-			/* unimplemented */
-			continue;
+			/* keep all gates untouched for now */
+			flags |= CLK_IGNORE_UNUSED;
+
+			clk = rockchip_clk_register_frac_branch(list->name,
+				list->parent_names, list->num_parents,
+				reg_base, list->muxdiv_offset, list->div_flags,
+				list->gate_offset, list->gate_shift,
+				list->gate_flags, flags, &clk_lock);
 			break;
 		case branch_gate:
 			flags |= CLK_SET_RATE_PARENT;
@@ -242,3 +297,61 @@ void __init rockchip_clk_register_branches(
 		rockchip_clk_add_lookup(clk, list->id);
 	}
 }
+
+void __init rockchip_clk_register_armclk(unsigned int lookup_id,
+			const char *name, const char **parent_names,
+			u8 num_parents,
+			const struct rockchip_cpuclk_reg_data *reg_data,
+			const struct rockchip_cpuclk_rate_table *rates,
+			int nrates)
+{
+	struct clk *clk;
+
+	clk = rockchip_clk_register_cpuclk(name, parent_names, num_parents,
+					   reg_data, rates, nrates, reg_base,
+					   &clk_lock);
+	if (IS_ERR(clk)) {
+		pr_err("%s: failed to register clock %s: %ld\n",
+		       __func__, name, PTR_ERR(clk));
+		return;
+	}
+
+	rockchip_clk_add_lookup(clk, lookup_id);
+}
+
+void __init rockchip_clk_protect_critical(const char *clocks[], int nclocks)
+{
+	int i;
+
+	/* Protect the clocks that needs to stay on */
+	for (i = 0; i < nclocks; i++) {
+		struct clk *clk = __clk_lookup(clocks[i]);
+
+		if (clk)
+			clk_prepare_enable(clk);
+	}
+}
+
+static unsigned int reg_restart;
+static int rockchip_restart_notify(struct notifier_block *this,
+				   unsigned long mode, void *cmd)
+{
+	writel(0xfdb9, reg_base + reg_restart);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block rockchip_restart_handler = {
+	.notifier_call = rockchip_restart_notify,
+	.priority = 128,
+};
+
+void __init rockchip_register_restart_notifier(unsigned int reg)
+{
+	int ret;
+
+	reg_restart = reg;
+	ret = register_restart_handler(&rockchip_restart_handler);
+	if (ret)
+		pr_err("%s: cannot register restart handler, %d\n",
+		       __func__, ret);
+}
diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h
index 887cbdeca2aa..ca009ab0a33a 100644
--- a/drivers/clk/rockchip/clk.h
+++ b/drivers/clk/rockchip/clk.h
@@ -120,6 +120,38 @@ struct clk *rockchip_clk_register_pll(enum rockchip_pll_type pll_type,
 		struct rockchip_pll_rate_table *rate_table,
 		spinlock_t *lock);
 
+struct rockchip_cpuclk_clksel {
+	int reg;
+	u32 val;
+};
+
+#define ROCKCHIP_CPUCLK_NUM_DIVIDERS	2
+struct rockchip_cpuclk_rate_table {
+	unsigned long prate;
+	struct rockchip_cpuclk_clksel divs[ROCKCHIP_CPUCLK_NUM_DIVIDERS];
+};
+
+/**
+ * struct rockchip_cpuclk_reg_data: describes register offsets and masks of the cpuclock
+ * @core_reg:		register offset of the core settings register
+ * @div_core_shift:	core divider offset used to divide the pll value
+ * @div_core_mask:	core divider mask
+ * @mux_core_shift:	offset of the core multiplexer
+ */
+struct rockchip_cpuclk_reg_data {
+	int		core_reg;
+	u8		div_core_shift;
+	u32		div_core_mask;
+	int		mux_core_reg;
+	u8		mux_core_shift;
+};
+
+struct clk *rockchip_clk_register_cpuclk(const char *name,
+			const char **parent_names, u8 num_parents,
+			const struct rockchip_cpuclk_reg_data *reg_data,
+			const struct rockchip_cpuclk_rate_table *rates,
+			int nrates, void __iomem *reg_base, spinlock_t *lock);
+
 #define PNAME(x) static const char *x[] __initconst
 
 enum rockchip_clk_branch_type {
@@ -329,6 +361,13 @@ void rockchip_clk_register_branches(struct rockchip_clk_branch *clk_list,
 				    unsigned int nr_clk);
 void rockchip_clk_register_plls(struct rockchip_pll_clock *pll_list,
 				unsigned int nr_pll, int grf_lock_offset);
+void rockchip_clk_register_armclk(unsigned int lookup_id, const char *name,
+			const char **parent_names, u8 num_parents,
+			const struct rockchip_cpuclk_reg_data *reg_data,
+			const struct rockchip_cpuclk_rate_table *rates,
+			int nrates);
+void rockchip_clk_protect_critical(const char *clocks[], int nclocks);
+void rockchip_register_restart_notifier(unsigned int reg);
 
 #define ROCKCHIP_SOFTRST_HIWORD_MASK	BIT(0)
 
diff --git a/drivers/clk/samsung/clk-exynos3250.c b/drivers/clk/samsung/clk-exynos3250.c
index dc85f8e7a2d7..6e6cca392082 100644
--- a/drivers/clk/samsung/clk-exynos3250.c
+++ b/drivers/clk/samsung/clk-exynos3250.c
@@ -110,7 +110,14 @@ enum exynos3250_plls {
 	nr_plls
 };
 
+/* list of PLLs in DMC block to be registered */
+enum exynos3250_dmc_plls {
+	bpll, epll,
+	nr_dmc_plls
+};
+
 static void __iomem *reg_base;
+static void __iomem *dmc_reg_base;
 
 /*
  * Support for CMU save/restore across system suspends
@@ -266,6 +273,7 @@ PNAME(group_sclk_cam_blk_p)	= { "xxti", "xusbxti",
 				    "none", "none", "none",
 				    "none", "div_mpll_pre",
 				    "mout_epll_user", "mout_vpll",
+				    "none", "none", "none",
 				    "div_cam_blk_320", };
 PNAME(group_sclk_fimd0_p)	= { "xxti", "xusbxti",
 				    "m_bitclkhsdiv4_2l", "none",
@@ -353,8 +361,8 @@ static struct samsung_mux_clock mux_clks[] __initdata = {
 
 	/* SRC_FSYS */
 	MUX(CLK_MOUT_TSADC, "mout_tsadc", group_sclk_p, SRC_FSYS, 28, 4),
-	MUX(CLK_MOUT_MMC1, "mout_mmc1", group_sclk_p, SRC_FSYS, 4, 3),
-	MUX(CLK_MOUT_MMC0, "mout_mmc0", group_sclk_p, SRC_FSYS, 0, 3),
+	MUX(CLK_MOUT_MMC1, "mout_mmc1", group_sclk_p, SRC_FSYS, 4, 4),
+	MUX(CLK_MOUT_MMC0, "mout_mmc0", group_sclk_p, SRC_FSYS, 0, 4),
 
 	/* SRC_PERIL0 */
 	MUX(CLK_MOUT_UART1, "mout_uart1", group_sclk_p, SRC_PERIL0, 4, 4),
@@ -423,7 +431,7 @@ static struct samsung_div_clock div_clks[] __initdata = {
 	DIV(CLK_DIV_SPI1_ISP, "div_spi1_isp", "mout_spi1_isp", DIV_ISP, 16, 4),
 	DIV_F(CLK_DIV_SPI0_ISP_PRE, "div_spi0_isp_pre", "div_spi0_isp",
 		DIV_ISP, 8, 8, CLK_SET_RATE_PARENT, 0),
-	DIV(CLK_DIV_SPI0_ISP, "div_spi0_isp", "mout_spi0_isp", DIV_ISP, 0, 4),
+	DIV(CLK_DIV_SPI0_ISP, "div_spi0_isp", "mout_spi0_isp", DIV_ISP, 4, 4),
 
 	/* DIV_FSYS0 */
 	DIV_F(CLK_DIV_TSADC_PRE, "div_tsadc_pre", "div_tsadc", DIV_FSYS0, 8, 8,
@@ -724,6 +732,25 @@ static struct samsung_pll_rate_table exynos3250_pll_rates[] = {
 	{ /* sentinel */ }
 };
 
+/* EPLL */
+static struct samsung_pll_rate_table exynos3250_epll_rates[] = {
+	PLL_36XX_RATE(800000000, 200, 3, 1,     0),
+	PLL_36XX_RATE(288000000,  96, 2, 2,     0),
+	PLL_36XX_RATE(192000000, 128, 2, 3,     0),
+	PLL_36XX_RATE(144000000,  96, 2, 3,     0),
+	PLL_36XX_RATE( 96000000, 128, 2, 4,     0),
+	PLL_36XX_RATE( 84000000, 112, 2, 4,     0),
+	PLL_36XX_RATE( 80000004, 106, 2, 4, 43691),
+	PLL_36XX_RATE( 73728000,  98, 2, 4, 19923),
+	PLL_36XX_RATE( 67737598, 270, 3, 5, 62285),
+	PLL_36XX_RATE( 65535999, 174, 2, 5, 49982),
+	PLL_36XX_RATE( 50000000, 200, 3, 5,     0),
+	PLL_36XX_RATE( 49152002, 131, 2, 5,  4719),
+	PLL_36XX_RATE( 48000000, 128, 2, 5,     0),
+	PLL_36XX_RATE( 45158401, 180, 3, 5, 41524),
+	{ /* sentinel */ }
+};
+
 /* VPLL */
 static struct samsung_pll_rate_table exynos3250_vpll_rates[] = {
 	PLL_36XX_RATE(600000000, 100, 2, 1,     0),
@@ -821,3 +848,172 @@ static void __init exynos3250_cmu_init(struct device_node *np)
 	samsung_clk_of_add_provider(np, ctx);
 }
 CLK_OF_DECLARE(exynos3250_cmu, "samsung,exynos3250-cmu", exynos3250_cmu_init);
+
+/*
+ * CMU DMC
+ */
+
+#define BPLL_LOCK		0x0118
+#define BPLL_CON0		0x0218
+#define BPLL_CON1		0x021c
+#define BPLL_CON2		0x0220
+#define SRC_DMC			0x0300
+#define DIV_DMC1		0x0504
+#define GATE_BUS_DMC0		0x0700
+#define GATE_BUS_DMC1		0x0704
+#define GATE_BUS_DMC2		0x0708
+#define GATE_BUS_DMC3		0x070c
+#define GATE_SCLK_DMC		0x0800
+#define GATE_IP_DMC0		0x0900
+#define GATE_IP_DMC1		0x0904
+#define EPLL_LOCK		0x1110
+#define EPLL_CON0		0x1114
+#define EPLL_CON1		0x1118
+#define EPLL_CON2		0x111c
+#define SRC_EPLL		0x1120
+
+/*
+ * Support for CMU save/restore across system suspends
+ */
+#ifdef CONFIG_PM_SLEEP
+static struct samsung_clk_reg_dump *exynos3250_dmc_clk_regs;
+
+static unsigned long exynos3250_cmu_dmc_clk_regs[] __initdata = {
+	BPLL_LOCK,
+	BPLL_CON0,
+	BPLL_CON1,
+	BPLL_CON2,
+	SRC_DMC,
+	DIV_DMC1,
+	GATE_BUS_DMC0,
+	GATE_BUS_DMC1,
+	GATE_BUS_DMC2,
+	GATE_BUS_DMC3,
+	GATE_SCLK_DMC,
+	GATE_IP_DMC0,
+	GATE_IP_DMC1,
+	EPLL_LOCK,
+	EPLL_CON0,
+	EPLL_CON1,
+	EPLL_CON2,
+	SRC_EPLL,
+};
+
+static int exynos3250_dmc_clk_suspend(void)
+{
+	samsung_clk_save(dmc_reg_base, exynos3250_dmc_clk_regs,
+				ARRAY_SIZE(exynos3250_cmu_dmc_clk_regs));
+	return 0;
+}
+
+static void exynos3250_dmc_clk_resume(void)
+{
+	samsung_clk_restore(dmc_reg_base, exynos3250_dmc_clk_regs,
+				ARRAY_SIZE(exynos3250_cmu_dmc_clk_regs));
+}
+
+static struct syscore_ops exynos3250_dmc_clk_syscore_ops = {
+	.suspend = exynos3250_dmc_clk_suspend,
+	.resume = exynos3250_dmc_clk_resume,
+};
+
+static void exynos3250_dmc_clk_sleep_init(void)
+{
+	exynos3250_dmc_clk_regs =
+		samsung_clk_alloc_reg_dump(exynos3250_cmu_dmc_clk_regs,
+				   ARRAY_SIZE(exynos3250_cmu_dmc_clk_regs));
+	if (!exynos3250_dmc_clk_regs) {
+		pr_warn("%s: Failed to allocate sleep save data\n", __func__);
+		goto err;
+	}
+
+	register_syscore_ops(&exynos3250_dmc_clk_syscore_ops);
+	return;
+err:
+	kfree(exynos3250_dmc_clk_regs);
+}
+#else
+static inline void exynos3250_dmc_clk_sleep_init(void) { }
+#endif
+
+PNAME(mout_epll_p)	= { "fin_pll", "fout_epll", };
+PNAME(mout_bpll_p)	= { "fin_pll", "fout_bpll", };
+PNAME(mout_mpll_mif_p)	= { "fin_pll", "sclk_mpll_mif", };
+PNAME(mout_dphy_p)	= { "mout_mpll_mif", "mout_bpll", };
+
+static struct samsung_mux_clock dmc_mux_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* SRC_DMC */
+	MUX(CLK_MOUT_MPLL_MIF, "mout_mpll_mif", mout_mpll_mif_p, SRC_DMC, 12, 1),
+	MUX(CLK_MOUT_BPLL, "mout_bpll", mout_bpll_p, SRC_DMC, 10, 1),
+	MUX(CLK_MOUT_DPHY, "mout_dphy", mout_dphy_p, SRC_DMC, 8, 1),
+	MUX(CLK_MOUT_DMC_BUS, "mout_dmc_bus", mout_dphy_p, SRC_DMC,  4, 1),
+
+	/* SRC_EPLL */
+	MUX(CLK_MOUT_EPLL, "mout_epll", mout_epll_p, SRC_EPLL, 4, 1),
+};
+
+static struct samsung_div_clock dmc_div_clks[] __initdata = {
+	/*
+	 * NOTE: Following table is sorted by register address in ascending
+	 * order and then bitfield shift in descending order, as it is done
+	 * in the User's Manual. When adding new entries, please make sure
+	 * that the order is preserved, to avoid merge conflicts and make
+	 * further work with defined data easier.
+	 */
+
+	/* DIV_DMC1 */
+	DIV(CLK_DIV_DMC, "div_dmc", "div_dmc_pre", DIV_DMC1, 27, 3),
+	DIV(CLK_DIV_DPHY, "div_dphy", "mout_dphy", DIV_DMC1, 23, 3),
+	DIV(CLK_DIV_DMC_PRE, "div_dmc_pre", "mout_dmc_bus", DIV_DMC1, 19, 2),
+	DIV(CLK_DIV_DMCP, "div_dmcp", "div_dmcd", DIV_DMC1, 15, 3),
+	DIV(CLK_DIV_DMCD, "div_dmcd", "div_dmc", DIV_DMC1, 11, 3),
+};
+
+static struct samsung_pll_clock exynos3250_dmc_plls[nr_dmc_plls] __initdata = {
+	[bpll] = PLL(pll_35xx, CLK_FOUT_BPLL, "fout_bpll", "fin_pll",
+			BPLL_LOCK, BPLL_CON0, NULL),
+	[epll] = PLL(pll_36xx, CLK_FOUT_EPLL, "fout_epll", "fin_pll",
+			EPLL_LOCK, EPLL_CON0, NULL),
+};
+
+static void __init exynos3250_cmu_dmc_init(struct device_node *np)
+{
+	struct samsung_clk_provider *ctx;
+
+	dmc_reg_base = of_iomap(np, 0);
+	if (!dmc_reg_base)
+		panic("%s: failed to map registers\n", __func__);
+
+	ctx = samsung_clk_init(np, dmc_reg_base, NR_CLKS_DMC);
+	if (!ctx)
+		panic("%s: unable to allocate context.\n", __func__);
+
+	exynos3250_dmc_plls[bpll].rate_table = exynos3250_pll_rates;
+	exynos3250_dmc_plls[epll].rate_table = exynos3250_epll_rates;
+
+	pr_err("CLK registering epll bpll: %d, %d, %d, %d\n",
+			exynos3250_dmc_plls[bpll].rate_table[0].rate,
+			exynos3250_dmc_plls[bpll].rate_table[0].mdiv,
+			exynos3250_dmc_plls[bpll].rate_table[0].pdiv,
+			exynos3250_dmc_plls[bpll].rate_table[0].sdiv
+	      );
+	samsung_clk_register_pll(ctx, exynos3250_dmc_plls,
+				ARRAY_SIZE(exynos3250_dmc_plls), dmc_reg_base);
+
+	samsung_clk_register_mux(ctx, dmc_mux_clks, ARRAY_SIZE(dmc_mux_clks));
+	samsung_clk_register_div(ctx, dmc_div_clks, ARRAY_SIZE(dmc_div_clks));
+
+	exynos3250_dmc_clk_sleep_init();
+
+	samsung_clk_of_add_provider(np, ctx);
+}
+CLK_OF_DECLARE(exynos3250_cmu_dmc, "samsung,exynos3250-cmu-dmc",
+		exynos3250_cmu_dmc_init);
diff --git a/drivers/clk/samsung/clk-exynos4.c b/drivers/clk/samsung/clk-exynos4.c
index ac163d7f5bc3..940f02837b82 100644
--- a/drivers/clk/samsung/clk-exynos4.c
+++ b/drivers/clk/samsung/clk-exynos4.c
@@ -517,7 +517,7 @@ static struct samsung_fixed_factor_clock exynos4_fixed_factor_clks[] __initdata
 	FFACTOR(0, "sclk_apll_div_2", "sclk_apll", 1, 2, 0),
 	FFACTOR(0, "fout_mpll_div_2", "fout_mpll", 1, 2, 0),
 	FFACTOR(0, "fout_apll_div_2", "fout_apll", 1, 2, 0),
-	FFACTOR(0, "arm_clk_div_2", "arm_clk", 1, 2, 0),
+	FFACTOR(0, "arm_clk_div_2", "div_core2", 1, 2, 0),
 };
 
 static struct samsung_fixed_factor_clock exynos4210_fixed_factor_clks[] __initdata = {
@@ -535,7 +535,7 @@ static struct samsung_fixed_factor_clock exynos4x12_fixed_factor_clks[] __initda
 static struct samsung_mux_clock exynos4_mux_clks[] __initdata = {
 	MUX_FA(CLK_MOUT_APLL, "mout_apll", mout_apll_p, SRC_CPU, 0, 1,
 			CLK_SET_RATE_PARENT, 0, "mout_apll"),
-	MUX(0, "mout_hdmi", mout_hdmi_p, SRC_TV, 0, 1),
+	MUX(CLK_MOUT_HDMI, "mout_hdmi", mout_hdmi_p, SRC_TV, 0, 1),
 	MUX(0, "mout_mfc1", sclk_evpll_p, SRC_MFC, 4, 1),
 	MUX(0, "mout_mfc", mout_mfc_p, SRC_MFC, 8, 1),
 	MUX_F(CLK_MOUT_G3D1, "mout_g3d1", sclk_evpll_p, SRC_G3D, 4, 1,
@@ -569,7 +569,7 @@ static struct samsung_mux_clock exynos4210_mux_clks[] __initdata = {
 	MUX(0, "mout_aclk100", sclk_ampll_p4210, SRC_TOP0, 16, 1),
 	MUX(0, "mout_aclk160", sclk_ampll_p4210, SRC_TOP0, 20, 1),
 	MUX(0, "mout_aclk133", sclk_ampll_p4210, SRC_TOP0, 24, 1),
-	MUX(0, "mout_mixer", mout_mixer_p4210, SRC_TV, 4, 1),
+	MUX(CLK_MOUT_MIXER, "mout_mixer", mout_mixer_p4210, SRC_TV, 4, 1),
 	MUX(0, "mout_dac", mout_dac_p4210, SRC_TV, 8, 1),
 	MUX(0, "mout_g2d0", sclk_ampll_p4210, E4210_SRC_IMAGE, 0, 1),
 	MUX(0, "mout_g2d1", sclk_evpll_p, E4210_SRC_IMAGE, 4, 1),
@@ -719,7 +719,7 @@ static struct samsung_div_clock exynos4_div_clks[] __initdata = {
 	DIV(0, "div_periph", "div_core2", DIV_CPU0, 12, 3),
 	DIV(0, "div_atb", "mout_core", DIV_CPU0, 16, 3),
 	DIV(0, "div_pclk_dbg", "div_atb", DIV_CPU0, 20, 3),
-	DIV(0, "div_core2", "div_core", DIV_CPU0, 28, 3),
+	DIV(CLK_ARM_CLK, "div_core2", "div_core", DIV_CPU0, 28, 3),
 	DIV(0, "div_copy", "mout_hpm", DIV_CPU1, 0, 3),
 	DIV(0, "div_hpm", "div_copy", DIV_CPU1, 4, 3),
 	DIV(0, "div_clkout_cpu", "mout_clkout_cpu", CLKOUT_CMU_CPU, 8, 6),
@@ -733,8 +733,7 @@ static struct samsung_div_clock exynos4_div_clks[] __initdata = {
 	DIV(0, "div_csis0", "mout_csis0", DIV_CAM, 24, 4),
 	DIV(0, "div_csis1", "mout_csis1", DIV_CAM, 28, 4),
 	DIV(CLK_SCLK_MFC, "sclk_mfc", "mout_mfc", DIV_MFC, 0, 4),
-	DIV_F(0, "div_g3d", "mout_g3d", DIV_G3D, 0, 4,
-			CLK_SET_RATE_PARENT, 0),
+	DIV(CLK_SCLK_G3D, "sclk_g3d", "mout_g3d", DIV_G3D, 0, 4),
 	DIV(0, "div_fimd0", "mout_fimd0", DIV_LCD0, 0, 4),
 	DIV(0, "div_mipi0", "mout_mipi0", DIV_LCD0, 16, 4),
 	DIV(0, "div_audio0", "mout_audio0", DIV_MAUDIO, 0, 4),
@@ -769,7 +768,6 @@ static struct samsung_div_clock exynos4_div_clks[] __initdata = {
 	DIV(0, "div_spi_pre2", "div_spi2", DIV_PERIL2, 8, 8),
 	DIV(0, "div_audio1", "mout_audio1", DIV_PERIL4, 0, 4),
 	DIV(0, "div_audio2", "mout_audio2", DIV_PERIL4, 16, 4),
-	DIV(CLK_ARM_CLK, "arm_clk", "div_core2", DIV_CPU0, 28, 3),
 	DIV(CLK_SCLK_APLL, "sclk_apll", "mout_apll", DIV_CPU0, 24, 3),
 	DIV_F(0, "div_mipi_pre0", "div_mipi0", DIV_LCD0, 20, 4,
 			CLK_SET_RATE_PARENT, 0),
@@ -857,8 +855,7 @@ static struct samsung_gate_clock exynos4_gate_clks[] __initdata = {
 		0),
 	GATE(CLK_TSI, "tsi", "aclk133", GATE_IP_FSYS, 4, 0, 0),
 	GATE(CLK_SROMC, "sromc", "aclk133", GATE_IP_FSYS, 11, 0, 0),
-	GATE(CLK_SCLK_G3D, "sclk_g3d", "div_g3d", GATE_IP_G3D, 0,
-			CLK_SET_RATE_PARENT, 0),
+	GATE(CLK_G3D, "g3d", "aclk200", GATE_IP_G3D, 0, 0, 0),
 	GATE(CLK_PPMUG3D, "ppmug3d", "aclk200", GATE_IP_G3D, 1, 0, 0),
 	GATE(CLK_USB_DEVICE, "usb_device", "aclk133", GATE_IP_FSYS, 13, 0, 0),
 	GATE(CLK_ONENAND, "onenand", "aclk133", GATE_IP_FSYS, 15, 0, 0),
@@ -1183,6 +1180,7 @@ static struct samsung_gate_clock exynos4x12_gate_clks[] __initdata = {
 	GATE(CLK_SPI1_ISP, "spi1_isp", "aclk200", E4X12_GATE_ISP1, 13,
 			CLK_IGNORE_UNUSED | CLK_GET_RATE_NOCACHE, 0),
 	GATE(CLK_G2D, "g2d", "aclk200", GATE_IP_DMC, 23, 0, 0),
+	GATE(CLK_SMMU_G2D, "smmu_g2d", "aclk200", GATE_IP_DMC, 24, 0, 0),
 	GATE(CLK_TMU_APBIF, "tmu_apbif", "aclk100", E4X12_GATE_IP_PERIR, 17, 0,
 		0),
 };
@@ -1486,7 +1484,7 @@ static void __init exynos4_clk_init(struct device_node *np,
 		exynos4_soc == EXYNOS4210 ? "Exynos4210" : "Exynos4x12",
 		_get_rate("sclk_apll"),	_get_rate("sclk_mpll"),
 		_get_rate("sclk_epll"), _get_rate("sclk_vpll"),
-		_get_rate("arm_clk"));
+		_get_rate("div_core2"));
 }
 
 
diff --git a/drivers/clk/samsung/clk-exynos5260.c b/drivers/clk/samsung/clk-exynos5260.c
index ce3de97e5f11..2527e39aadcf 100644
--- a/drivers/clk/samsung/clk-exynos5260.c
+++ b/drivers/clk/samsung/clk-exynos5260.c
@@ -1581,7 +1581,7 @@ struct samsung_fixed_rate_clock fixed_rate_clks[] __initdata = {
 	FRATE(PHYCLK_HDMI_LINK_O_TMDS_CLKHI, "phyclk_hdmi_link_o_tmds_clkhi",
 			NULL, CLK_IS_ROOT, 125000000),
 	FRATE(PHYCLK_MIPI_DPHY_4L_M_TXBYTECLKHS,
-			"phyclk_mipi_dphy_4l_m_txbyteclkhs" , NULL,
+			"phyclk_mipi_dphy_4l_m_txbyte_clkhs" , NULL,
 			CLK_IS_ROOT, 187500000),
 	FRATE(PHYCLK_DPTX_PHY_O_REF_CLK_24M, "phyclk_dptx_phy_o_ref_clk_24m",
 			NULL, CLK_IS_ROOT, 24000000),
diff --git a/drivers/clk/samsung/clk-s3c2410-dclk.c b/drivers/clk/samsung/clk-s3c2410-dclk.c
index 0449cc0458ed..f4f29ed6bd25 100644
--- a/drivers/clk/samsung/clk-s3c2410-dclk.c
+++ b/drivers/clk/samsung/clk-s3c2410-dclk.c
@@ -426,7 +426,6 @@ MODULE_DEVICE_TABLE(platform, s3c24xx_dclk_driver_ids);
 static struct platform_driver s3c24xx_dclk_driver = {
 	.driver = {
 		.name		= "s3c24xx-dclk",
-		.owner		= THIS_MODULE,
 		.pm		= &s3c24xx_dclk_pm_ops,
 	},
 	.probe = s3c24xx_dclk_probe,
diff --git a/drivers/clk/samsung/clk-s3c2412.c b/drivers/clk/samsung/clk-s3c2412.c
index 34af09f6a155..2ceedaf8ce18 100644
--- a/drivers/clk/samsung/clk-s3c2412.c
+++ b/drivers/clk/samsung/clk-s3c2412.c
@@ -14,6 +14,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/syscore_ops.h>
+#include <linux/reboot.h>
 
 #include <dt-bindings/clock/s3c2412.h>
 
@@ -26,6 +27,7 @@
 #define CLKCON		0x0c
 #define CLKDIVN		0x14
 #define CLKSRC		0x1c
+#define SWRST		0x30
 
 /* list of PLLs to be registered */
 enum s3c2412_plls {
@@ -204,6 +206,28 @@ struct samsung_clock_alias s3c2412_aliases[] __initdata = {
 	ALIAS(MSYSCLK, NULL, "fclk"),
 };
 
+static int s3c2412_restart(struct notifier_block *this,
+			   unsigned long mode, void *cmd)
+{
+	/* errata "Watch-dog/Software Reset Problem" specifies that
+	 * this reset must be done with the SYSCLK sourced from
+	 * EXTCLK instead of FOUT to avoid a glitch in the reset
+	 * mechanism.
+	 *
+	 * See the watchdog section of the S3C2412 manual for more
+	 * information on this fix.
+	 */
+
+	__raw_writel(0x00, reg_base + CLKSRC);
+	__raw_writel(0x533C2412, reg_base + SWRST);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block s3c2412_restart_handler = {
+	.notifier_call = s3c2412_restart,
+	.priority = 129,
+};
+
 /*
  * fixed rate clocks generated outside the soc
  * Only necessary until the devicetree-move is complete
@@ -233,6 +257,7 @@ void __init s3c2412_common_clk_init(struct device_node *np, unsigned long xti_f,
 				    unsigned long ext_f, void __iomem *base)
 {
 	struct samsung_clk_provider *ctx;
+	int ret;
 	reg_base = base;
 
 	if (np) {
@@ -267,6 +292,10 @@ void __init s3c2412_common_clk_init(struct device_node *np, unsigned long xti_f,
 	s3c2412_clk_sleep_init();
 
 	samsung_clk_of_add_provider(np, ctx);
+
+	ret = register_restart_handler(&s3c2412_restart_handler);
+	if (ret)
+		pr_warn("cannot register restart handler, %d\n", ret);
 }
 
 static void __init s3c2412_clk_init(struct device_node *np)
diff --git a/drivers/clk/samsung/clk-s3c2443.c b/drivers/clk/samsung/clk-s3c2443.c
index c92f853fca9f..0c3c182b902a 100644
--- a/drivers/clk/samsung/clk-s3c2443.c
+++ b/drivers/clk/samsung/clk-s3c2443.c
@@ -14,6 +14,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/syscore_ops.h>
+#include <linux/reboot.h>
 
 #include <dt-bindings/clock/s3c2443.h>
 
@@ -33,6 +34,7 @@
 #define HCLKCON		0x30
 #define PCLKCON		0x34
 #define SCLKCON		0x38
+#define SWRST		0x44
 
 /* the soc types */
 enum supported_socs {
@@ -354,6 +356,18 @@ struct samsung_clock_alias s3c2450_aliases[] __initdata = {
 	ALIAS(PCLK_I2C1, "s3c2410-i2c.1", "i2c"),
 };
 
+static int s3c2443_restart(struct notifier_block *this,
+			   unsigned long mode, void *cmd)
+{
+	__raw_writel(0x533c2443, reg_base + SWRST);
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block s3c2443_restart_handler = {
+	.notifier_call = s3c2443_restart,
+	.priority = 129,
+};
+
 /*
  * fixed rate clocks generated outside the soc
  * Only necessary until the devicetree-move is complete
@@ -378,6 +392,7 @@ void __init s3c2443_common_clk_init(struct device_node *np, unsigned long xti_f,
 				    void __iomem *base)
 {
 	struct samsung_clk_provider *ctx;
+	int ret;
 	reg_base = base;
 
 	if (np) {
@@ -447,6 +462,10 @@ void __init s3c2443_common_clk_init(struct device_node *np, unsigned long xti_f,
 	s3c2443_clk_sleep_init();
 
 	samsung_clk_of_add_provider(np, ctx);
+
+	ret = register_restart_handler(&s3c2443_restart_handler);
+	if (ret)
+		pr_warn("cannot register restart handler, %d\n", ret);
 }
 
 static void __init s3c2416_clk_init(struct device_node *np)
diff --git a/drivers/clk/shmobile/Makefile b/drivers/clk/shmobile/Makefile
index e0029237827a..960bf22d42ae 100644
--- a/drivers/clk/shmobile/Makefile
+++ b/drivers/clk/shmobile/Makefile
@@ -4,7 +4,6 @@ obj-$(CONFIG_ARCH_R8A7740)		+= clk-r8a7740.o
 obj-$(CONFIG_ARCH_R8A7779)		+= clk-r8a7779.o
 obj-$(CONFIG_ARCH_R8A7790)		+= clk-rcar-gen2.o
 obj-$(CONFIG_ARCH_R8A7791)		+= clk-rcar-gen2.o
+obj-$(CONFIG_ARCH_R8A7794)		+= clk-rcar-gen2.o
 obj-$(CONFIG_ARCH_SHMOBILE_MULTI)	+= clk-div6.o
 obj-$(CONFIG_ARCH_SHMOBILE_MULTI)	+= clk-mstp.o
-# for emply built-in.o
-obj-n	:= dummy
diff --git a/drivers/clk/shmobile/clk-rcar-gen2.c b/drivers/clk/shmobile/clk-rcar-gen2.c
index dff7f79a19b9..e996425d06a9 100644
--- a/drivers/clk/shmobile/clk-rcar-gen2.c
+++ b/drivers/clk/shmobile/clk-rcar-gen2.c
@@ -202,6 +202,7 @@ static const struct clk_div_table cpg_sdh_div_table[] = {
 };
 
 static const struct clk_div_table cpg_sd01_div_table[] = {
+	{  4,  8 },
 	{  5, 12 }, {  6, 16 }, {  7, 18 }, {  8, 24 },
 	{ 10, 36 }, { 11, 48 }, { 12, 10 }, {  0,  0 },
 };
diff --git a/drivers/clk/sunxi/Makefile b/drivers/clk/sunxi/Makefile
index 6850cba35871..7ddc2b553846 100644
--- a/drivers/clk/sunxi/Makefile
+++ b/drivers/clk/sunxi/Makefile
@@ -5,6 +5,8 @@
 obj-y += clk-sunxi.o clk-factors.o
 obj-y += clk-a10-hosc.o
 obj-y += clk-a20-gmac.o
+obj-y += clk-mod0.o
+obj-y += clk-sun8i-mbus.o
 
 obj-$(CONFIG_MFD_SUN6I_PRCM) += \
 	clk-sun6i-ar100.o clk-sun6i-apb0.o clk-sun6i-apb0-gates.o \
diff --git a/drivers/clk/sunxi/clk-factors.c b/drivers/clk/sunxi/clk-factors.c
index 2057c8ac648f..f83ba097126c 100644
--- a/drivers/clk/sunxi/clk-factors.c
+++ b/drivers/clk/sunxi/clk-factors.c
@@ -9,18 +9,18 @@
  */
 
 #include <linux/clk-provider.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
 #include <linux/module.h>
+#include <linux/of_address.h>
 #include <linux/slab.h>
-#include <linux/io.h>
-#include <linux/err.h>
 #include <linux/string.h>
 
-#include <linux/delay.h>
-
 #include "clk-factors.h"
 
 /*
- * DOC: basic adjustable factor-based clock that cannot gate
+ * DOC: basic adjustable factor-based clock
  *
  * Traits of this clock:
  * prepare - clk_prepare only ensures that parents are prepared
@@ -32,6 +32,8 @@
 
 #define to_clk_factors(_hw) container_of(_hw, struct clk_factors, hw)
 
+#define FACTORS_MAX_PARENTS		5
+
 #define SETMASK(len, pos)		(((1U << (len)) - 1) << (pos))
 #define CLRMASK(len, pos)		(~(SETMASK(len, pos)))
 #define FACTOR_GET(bit, len, reg)	(((reg) & SETMASK(len, bit)) >> (bit))
@@ -147,9 +149,96 @@ static int clk_factors_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
-const struct clk_ops clk_factors_ops = {
+static const struct clk_ops clk_factors_ops = {
 	.determine_rate = clk_factors_determine_rate,
 	.recalc_rate = clk_factors_recalc_rate,
 	.round_rate = clk_factors_round_rate,
 	.set_rate = clk_factors_set_rate,
 };
+
+struct clk * __init sunxi_factors_register(struct device_node *node,
+					   const struct factors_data *data,
+					   spinlock_t *lock)
+{
+	struct clk *clk;
+	struct clk_factors *factors;
+	struct clk_gate *gate = NULL;
+	struct clk_mux *mux = NULL;
+	struct clk_hw *gate_hw = NULL;
+	struct clk_hw *mux_hw = NULL;
+	const char *clk_name = node->name;
+	const char *parents[FACTORS_MAX_PARENTS];
+	void __iomem *reg;
+	int i = 0;
+
+	reg = of_iomap(node, 0);
+
+	/* if we have a mux, we will have >1 parents */
+	while (i < FACTORS_MAX_PARENTS &&
+	       (parents[i] = of_clk_get_parent_name(node, i)) != NULL)
+		i++;
+
+	/*
+	 * some factor clocks, such as pll5 and pll6, may have multiple
+	 * outputs, and have their name designated in factors_data
+	 */
+	if (data->name)
+		clk_name = data->name;
+	else
+		of_property_read_string(node, "clock-output-names", &clk_name);
+
+	factors = kzalloc(sizeof(struct clk_factors), GFP_KERNEL);
+	if (!factors)
+		return NULL;
+
+	/* set up factors properties */
+	factors->reg = reg;
+	factors->config = data->table;
+	factors->get_factors = data->getter;
+	factors->lock = lock;
+
+	/* Add a gate if this factor clock can be gated */
+	if (data->enable) {
+		gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
+		if (!gate) {
+			kfree(factors);
+			return NULL;
+		}
+
+		/* set up gate properties */
+		gate->reg = reg;
+		gate->bit_idx = data->enable;
+		gate->lock = factors->lock;
+		gate_hw = &gate->hw;
+	}
+
+	/* Add a mux if this factor clock can be muxed */
+	if (data->mux) {
+		mux = kzalloc(sizeof(struct clk_mux), GFP_KERNEL);
+		if (!mux) {
+			kfree(factors);
+			kfree(gate);
+			return NULL;
+		}
+
+		/* set up gate properties */
+		mux->reg = reg;
+		mux->shift = data->mux;
+		mux->mask = SUNXI_FACTORS_MUX_MASK;
+		mux->lock = factors->lock;
+		mux_hw = &mux->hw;
+	}
+
+	clk = clk_register_composite(NULL, clk_name,
+			parents, i,
+			mux_hw, &clk_mux_ops,
+			&factors->hw, &clk_factors_ops,
+			gate_hw, &clk_gate_ops, 0);
+
+	if (!IS_ERR(clk)) {
+		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+		clk_register_clkdev(clk, clk_name, NULL);
+	}
+
+	return clk;
+}
diff --git a/drivers/clk/sunxi/clk-factors.h b/drivers/clk/sunxi/clk-factors.h
index d2d0efa39379..9913840018d3 100644
--- a/drivers/clk/sunxi/clk-factors.h
+++ b/drivers/clk/sunxi/clk-factors.h
@@ -3,9 +3,12 @@
 
 #include <linux/clk-provider.h>
 #include <linux/clkdev.h>
+#include <linux/spinlock.h>
 
 #define SUNXI_FACTORS_NOT_APPLICABLE	(0)
 
+#define SUNXI_FACTORS_MUX_MASK 0x3
+
 struct clk_factors_config {
 	u8 nshift;
 	u8 nwidth;
@@ -18,6 +21,14 @@ struct clk_factors_config {
 	u8 n_start;
 };
 
+struct factors_data {
+	int enable;
+	int mux;
+	struct clk_factors_config *table;
+	void (*getter) (u32 *rate, u32 parent_rate, u8 *n, u8 *k, u8 *m, u8 *p);
+	const char *name;
+};
+
 struct clk_factors {
 	struct clk_hw hw;
 	void __iomem *reg;
@@ -26,5 +37,8 @@ struct clk_factors {
 	spinlock_t *lock;
 };
 
-extern const struct clk_ops clk_factors_ops;
+struct clk * __init sunxi_factors_register(struct device_node *node,
+					   const struct factors_data *data,
+					   spinlock_t *lock);
+
 #endif
diff --git a/drivers/clk/sunxi/clk-mod0.c b/drivers/clk/sunxi/clk-mod0.c
new file mode 100644
index 000000000000..4a563850ee6e
--- /dev/null
+++ b/drivers/clk/sunxi/clk-mod0.c
@@ -0,0 +1,283 @@
+/*
+ * Copyright 2013 Emilio López
+ *
+ * Emilio López <emilio@elopez.com.ar>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of_address.h>
+
+#include "clk-factors.h"
+
+/**
+ * sun4i_get_mod0_factors() - calculates m, n factors for MOD0-style clocks
+ * MOD0 rate is calculated as follows
+ * rate = (parent_rate >> p) / (m + 1);
+ */
+
+static void sun4i_a10_get_mod0_factors(u32 *freq, u32 parent_rate,
+				       u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	u8 div, calcm, calcp;
+
+	/* These clocks can only divide, so we will never be able to achieve
+	 * frequencies higher than the parent frequency */
+	if (*freq > parent_rate)
+		*freq = parent_rate;
+
+	div = DIV_ROUND_UP(parent_rate, *freq);
+
+	if (div < 16)
+		calcp = 0;
+	else if (div / 2 < 16)
+		calcp = 1;
+	else if (div / 4 < 16)
+		calcp = 2;
+	else
+		calcp = 3;
+
+	calcm = DIV_ROUND_UP(div, 1 << calcp);
+
+	*freq = (parent_rate >> calcp) / calcm;
+
+	/* we were called to round the frequency, we can now return */
+	if (n == NULL)
+		return;
+
+	*m = calcm - 1;
+	*p = calcp;
+}
+
+/* user manual says "n" but it's really "p" */
+static struct clk_factors_config sun4i_a10_mod0_config = {
+	.mshift = 0,
+	.mwidth = 4,
+	.pshift = 16,
+	.pwidth = 2,
+};
+
+static const struct factors_data sun4i_a10_mod0_data __initconst = {
+	.enable = 31,
+	.mux = 24,
+	.table = &sun4i_a10_mod0_config,
+	.getter = sun4i_a10_get_mod0_factors,
+};
+
+static DEFINE_SPINLOCK(sun4i_a10_mod0_lock);
+
+static void __init sun4i_a10_mod0_setup(struct device_node *node)
+{
+	sunxi_factors_register(node, &sun4i_a10_mod0_data, &sun4i_a10_mod0_lock);
+}
+CLK_OF_DECLARE(sun4i_a10_mod0, "allwinner,sun4i-a10-mod0-clk", sun4i_a10_mod0_setup);
+
+static DEFINE_SPINLOCK(sun5i_a13_mbus_lock);
+
+static void __init sun5i_a13_mbus_setup(struct device_node *node)
+{
+	struct clk *mbus = sunxi_factors_register(node, &sun4i_a10_mod0_data, &sun5i_a13_mbus_lock);
+
+	/* The MBUS clocks needs to be always enabled */
+	__clk_get(mbus);
+	clk_prepare_enable(mbus);
+}
+CLK_OF_DECLARE(sun5i_a13_mbus, "allwinner,sun5i-a13-mbus-clk", sun5i_a13_mbus_setup);
+
+struct mmc_phase_data {
+	u8	offset;
+};
+
+struct mmc_phase {
+	struct clk_hw		hw;
+	void __iomem		*reg;
+	struct mmc_phase_data	*data;
+	spinlock_t		*lock;
+};
+
+#define to_mmc_phase(_hw) container_of(_hw, struct mmc_phase, hw)
+
+static int mmc_get_phase(struct clk_hw *hw)
+{
+	struct clk *mmc, *mmc_parent, *clk = hw->clk;
+	struct mmc_phase *phase = to_mmc_phase(hw);
+	unsigned int mmc_rate, mmc_parent_rate;
+	u16 step, mmc_div;
+	u32 value;
+	u8 delay;
+
+	value = readl(phase->reg);
+	delay = (value >> phase->data->offset) & 0x3;
+
+	if (!delay)
+		return 180;
+
+	/* Get the main MMC clock */
+	mmc = clk_get_parent(clk);
+	if (!mmc)
+		return -EINVAL;
+
+	/* And its rate */
+	mmc_rate = clk_get_rate(mmc);
+	if (!mmc_rate)
+		return -EINVAL;
+
+	/* Now, get the MMC parent (most likely some PLL) */
+	mmc_parent = clk_get_parent(mmc);
+	if (!mmc_parent)
+		return -EINVAL;
+
+	/* And its rate */
+	mmc_parent_rate = clk_get_rate(mmc_parent);
+	if (!mmc_parent_rate)
+		return -EINVAL;
+
+	/* Get MMC clock divider */
+	mmc_div = mmc_parent_rate / mmc_rate;
+
+	step = DIV_ROUND_CLOSEST(360, mmc_div);
+	return delay * step;
+}
+
+static int mmc_set_phase(struct clk_hw *hw, int degrees)
+{
+	struct clk *mmc, *mmc_parent, *clk = hw->clk;
+	struct mmc_phase *phase = to_mmc_phase(hw);
+	unsigned int mmc_rate, mmc_parent_rate;
+	unsigned long flags;
+	u32 value;
+	u8 delay;
+
+	/* Get the main MMC clock */
+	mmc = clk_get_parent(clk);
+	if (!mmc)
+		return -EINVAL;
+
+	/* And its rate */
+	mmc_rate = clk_get_rate(mmc);
+	if (!mmc_rate)
+		return -EINVAL;
+
+	/* Now, get the MMC parent (most likely some PLL) */
+	mmc_parent = clk_get_parent(mmc);
+	if (!mmc_parent)
+		return -EINVAL;
+
+	/* And its rate */
+	mmc_parent_rate = clk_get_rate(mmc_parent);
+	if (!mmc_parent_rate)
+		return -EINVAL;
+
+	if (degrees != 180) {
+		u16 step, mmc_div;
+
+		/* Get MMC clock divider */
+		mmc_div = mmc_parent_rate / mmc_rate;
+
+		/*
+		 * We can only outphase the clocks by multiple of the
+		 * PLL's period.
+		 *
+		 * Since the MMC clock in only a divider, and the
+		 * formula to get the outphasing in degrees is deg =
+		 * 360 * delta / period
+		 *
+		 * If we simplify this formula, we can see that the
+		 * only thing that we're concerned about is the number
+		 * of period we want to outphase our clock from, and
+		 * the divider set by the MMC clock.
+		 */
+		step = DIV_ROUND_CLOSEST(360, mmc_div);
+		delay = DIV_ROUND_CLOSEST(degrees, step);
+	} else {
+		delay = 0;
+	}
+
+	spin_lock_irqsave(phase->lock, flags);
+	value = readl(phase->reg);
+	value &= ~GENMASK(phase->data->offset + 3, phase->data->offset);
+	value |= delay << phase->data->offset;
+	writel(value, phase->reg);
+	spin_unlock_irqrestore(phase->lock, flags);
+
+	return 0;
+}
+
+static const struct clk_ops mmc_clk_ops = {
+	.get_phase	= mmc_get_phase,
+	.set_phase	= mmc_set_phase,
+};
+
+static void __init sun4i_a10_mmc_phase_setup(struct device_node *node,
+					     struct mmc_phase_data *data)
+{
+	const char *parent_names[1] = { of_clk_get_parent_name(node, 0) };
+	struct clk_init_data init = {
+		.num_parents	= 1,
+		.parent_names	= parent_names,
+		.ops		= &mmc_clk_ops,
+	};
+
+	struct mmc_phase *phase;
+	struct clk *clk;
+
+	phase = kmalloc(sizeof(*phase), GFP_KERNEL);
+	if (!phase)
+		return;
+
+	phase->hw.init = &init;
+
+	phase->reg = of_iomap(node, 0);
+	if (!phase->reg)
+		goto err_free;
+
+	phase->data = data;
+	phase->lock = &sun4i_a10_mod0_lock;
+
+	if (of_property_read_string(node, "clock-output-names", &init.name))
+		init.name = node->name;
+
+	clk = clk_register(NULL, &phase->hw);
+	if (IS_ERR(clk))
+		goto err_unmap;
+
+	of_clk_add_provider(node, of_clk_src_simple_get, clk);
+
+	return;
+
+err_unmap:
+	iounmap(phase->reg);
+err_free:
+	kfree(phase);
+}
+
+
+static struct mmc_phase_data mmc_output_clk = {
+	.offset	= 8,
+};
+
+static struct mmc_phase_data mmc_sample_clk = {
+	.offset	= 20,
+};
+
+static void __init sun4i_a10_mmc_output_setup(struct device_node *node)
+{
+	sun4i_a10_mmc_phase_setup(node, &mmc_output_clk);
+}
+CLK_OF_DECLARE(sun4i_a10_mmc_output, "allwinner,sun4i-a10-mmc-output-clk", sun4i_a10_mmc_output_setup);
+
+static void __init sun4i_a10_mmc_sample_setup(struct device_node *node)
+{
+	sun4i_a10_mmc_phase_setup(node, &mmc_sample_clk);
+}
+CLK_OF_DECLARE(sun4i_a10_mmc_sample, "allwinner,sun4i-a10-mmc-sample-clk", sun4i_a10_mmc_sample_setup);
diff --git a/drivers/clk/sunxi/clk-sun6i-apb0-gates.c b/drivers/clk/sunxi/clk-sun6i-apb0-gates.c
index e10d0521ec76..64f3e46d383c 100644
--- a/drivers/clk/sunxi/clk-sun6i-apb0-gates.c
+++ b/drivers/clk/sunxi/clk-sun6i-apb0-gates.c
@@ -99,7 +99,6 @@ static int sun6i_a31_apb0_gates_clk_probe(struct platform_device *pdev)
 static struct platform_driver sun6i_a31_apb0_gates_clk_driver = {
 	.driver = {
 		.name = "sun6i-a31-apb0-gates-clk",
-		.owner = THIS_MODULE,
 		.of_match_table = sun6i_a31_apb0_gates_clk_dt_ids,
 	},
 	.probe = sun6i_a31_apb0_gates_clk_probe,
diff --git a/drivers/clk/sunxi/clk-sun6i-apb0.c b/drivers/clk/sunxi/clk-sun6i-apb0.c
index 1fa23371c8c6..70763600aeae 100644
--- a/drivers/clk/sunxi/clk-sun6i-apb0.c
+++ b/drivers/clk/sunxi/clk-sun6i-apb0.c
@@ -65,7 +65,6 @@ static const struct of_device_id sun6i_a31_apb0_clk_dt_ids[] = {
 static struct platform_driver sun6i_a31_apb0_clk_driver = {
 	.driver = {
 		.name = "sun6i-a31-apb0-clk",
-		.owner = THIS_MODULE,
 		.of_match_table = sun6i_a31_apb0_clk_dt_ids,
 	},
 	.probe = sun6i_a31_apb0_clk_probe,
diff --git a/drivers/clk/sunxi/clk-sun6i-ar100.c b/drivers/clk/sunxi/clk-sun6i-ar100.c
index eca8ca025b6a..acca53290be2 100644
--- a/drivers/clk/sunxi/clk-sun6i-ar100.c
+++ b/drivers/clk/sunxi/clk-sun6i-ar100.c
@@ -221,7 +221,6 @@ static const struct of_device_id sun6i_a31_ar100_clk_dt_ids[] = {
 static struct platform_driver sun6i_a31_ar100_clk_driver = {
 	.driver = {
 		.name = "sun6i-a31-ar100-clk",
-		.owner = THIS_MODULE,
 		.of_match_table = sun6i_a31_ar100_clk_dt_ids,
 	},
 	.probe = sun6i_a31_ar100_clk_probe,
diff --git a/drivers/clk/sunxi/clk-sun8i-apb0.c b/drivers/clk/sunxi/clk-sun8i-apb0.c
index 1f5ba9b4b8cd..155d0022194f 100644
--- a/drivers/clk/sunxi/clk-sun8i-apb0.c
+++ b/drivers/clk/sunxi/clk-sun8i-apb0.c
@@ -56,7 +56,6 @@ static const struct of_device_id sun8i_a23_apb0_clk_dt_ids[] = {
 static struct platform_driver sun8i_a23_apb0_clk_driver = {
 	.driver = {
 		.name = "sun8i-a23-apb0-clk",
-		.owner = THIS_MODULE,
 		.of_match_table = sun8i_a23_apb0_clk_dt_ids,
 	},
 	.probe = sun8i_a23_apb0_clk_probe,
diff --git a/drivers/clk/sunxi/clk-sun8i-mbus.c b/drivers/clk/sunxi/clk-sun8i-mbus.c
new file mode 100644
index 000000000000..8e49b44cee41
--- /dev/null
+++ b/drivers/clk/sunxi/clk-sun8i-mbus.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright 2014 Chen-Yu Tsai
+ *
+ * Chen-Yu Tsai <wens@csie.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+#include <linux/of_address.h>
+
+#include "clk-factors.h"
+
+/**
+ * sun8i_a23_get_mbus_factors() - calculates m factor for MBUS clocks
+ * MBUS rate is calculated as follows
+ * rate = parent_rate / (m + 1);
+ */
+
+static void sun8i_a23_get_mbus_factors(u32 *freq, u32 parent_rate,
+				       u8 *n, u8 *k, u8 *m, u8 *p)
+{
+	u8 div;
+
+	/*
+	 * These clocks can only divide, so we will never be able to
+	 * achieve frequencies higher than the parent frequency
+	 */
+	if (*freq > parent_rate)
+		*freq = parent_rate;
+
+	div = DIV_ROUND_UP(parent_rate, *freq);
+
+	if (div > 8)
+		div = 8;
+
+	*freq = parent_rate / div;
+
+	/* we were called to round the frequency, we can now return */
+	if (m == NULL)
+		return;
+
+	*m = div - 1;
+}
+
+static struct clk_factors_config sun8i_a23_mbus_config = {
+	.mshift = 0,
+	.mwidth = 3,
+};
+
+static const struct factors_data sun8i_a23_mbus_data __initconst = {
+	.enable = 31,
+	.mux = 24,
+	.table = &sun8i_a23_mbus_config,
+	.getter = sun8i_a23_get_mbus_factors,
+};
+
+static DEFINE_SPINLOCK(sun8i_a23_mbus_lock);
+
+static void __init sun8i_a23_mbus_setup(struct device_node *node)
+{
+	struct clk *mbus = sunxi_factors_register(node, &sun8i_a23_mbus_data,
+						  &sun8i_a23_mbus_lock);
+
+	/* The MBUS clocks needs to be always enabled */
+	__clk_get(mbus);
+	clk_prepare_enable(mbus);
+}
+CLK_OF_DECLARE(sun8i_a23_mbus, "allwinner,sun8i-a23-mbus-clk", sun8i_a23_mbus_setup);
diff --git a/drivers/clk/sunxi/clk-sunxi.c b/drivers/clk/sunxi/clk-sunxi.c
index b654b7b1d137..d5dc951264ca 100644
--- a/drivers/clk/sunxi/clk-sunxi.c
+++ b/drivers/clk/sunxi/clk-sunxi.c
@@ -19,6 +19,7 @@
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/reset-controller.h>
+#include <linux/spinlock.h>
 
 #include "clk-factors.h"
 
@@ -319,46 +320,6 @@ static void sun4i_get_apb1_factors(u32 *freq, u32 parent_rate,
 
 
 
-/**
- * sun4i_get_mod0_factors() - calculates m, n factors for MOD0-style clocks
- * MOD0 rate is calculated as follows
- * rate = (parent_rate >> p) / (m + 1);
- */
-
-static void sun4i_get_mod0_factors(u32 *freq, u32 parent_rate,
-				   u8 *n, u8 *k, u8 *m, u8 *p)
-{
-	u8 div, calcm, calcp;
-
-	/* These clocks can only divide, so we will never be able to achieve
-	 * frequencies higher than the parent frequency */
-	if (*freq > parent_rate)
-		*freq = parent_rate;
-
-	div = DIV_ROUND_UP(parent_rate, *freq);
-
-	if (div < 16)
-		calcp = 0;
-	else if (div / 2 < 16)
-		calcp = 1;
-	else if (div / 4 < 16)
-		calcp = 2;
-	else
-		calcp = 3;
-
-	calcm = DIV_ROUND_UP(div, 1 << calcp);
-
-	*freq = (parent_rate >> calcp) / calcm;
-
-	/* we were called to round the frequency, we can now return */
-	if (n == NULL)
-		return;
-
-	*m = calcm - 1;
-	*p = calcp;
-}
-
-
 
 /**
  * sun7i_a20_get_out_factors() - calculates m, p factors for CLK_OUT_A/B
@@ -440,16 +401,6 @@ EXPORT_SYMBOL(clk_sunxi_mmc_phase_control);
  * sunxi_factors_clk_setup() - Setup function for factor clocks
  */
 
-#define SUNXI_FACTORS_MUX_MASK 0x3
-
-struct factors_data {
-	int enable;
-	int mux;
-	struct clk_factors_config *table;
-	void (*getter) (u32 *rate, u32 parent_rate, u8 *n, u8 *k, u8 *m, u8 *p);
-	const char *name;
-};
-
 static struct clk_factors_config sun4i_pll1_config = {
 	.nshift = 8,
 	.nwidth = 5,
@@ -504,14 +455,6 @@ static struct clk_factors_config sun4i_apb1_config = {
 };
 
 /* user manual says "n" but it's really "p" */
-static struct clk_factors_config sun4i_mod0_config = {
-	.mshift = 0,
-	.mwidth = 4,
-	.pshift = 16,
-	.pwidth = 2,
-};
-
-/* user manual says "n" but it's really "p" */
 static struct clk_factors_config sun7i_a20_out_config = {
 	.mshift = 8,
 	.mwidth = 5,
@@ -568,13 +511,6 @@ static const struct factors_data sun4i_apb1_data __initconst = {
 	.getter = sun4i_get_apb1_factors,
 };
 
-static const struct factors_data sun4i_mod0_data __initconst = {
-	.enable = 31,
-	.mux = 24,
-	.table = &sun4i_mod0_config,
-	.getter = sun4i_get_mod0_factors,
-};
-
 static const struct factors_data sun7i_a20_out_data __initconst = {
 	.enable = 31,
 	.mux = 24,
@@ -583,89 +519,9 @@ static const struct factors_data sun7i_a20_out_data __initconst = {
 };
 
 static struct clk * __init sunxi_factors_clk_setup(struct device_node *node,
-						const struct factors_data *data)
+						   const struct factors_data *data)
 {
-	struct clk *clk;
-	struct clk_factors *factors;
-	struct clk_gate *gate = NULL;
-	struct clk_mux *mux = NULL;
-	struct clk_hw *gate_hw = NULL;
-	struct clk_hw *mux_hw = NULL;
-	const char *clk_name = node->name;
-	const char *parents[SUNXI_MAX_PARENTS];
-	void __iomem *reg;
-	int i = 0;
-
-	reg = of_iomap(node, 0);
-
-	/* if we have a mux, we will have >1 parents */
-	while (i < SUNXI_MAX_PARENTS &&
-	       (parents[i] = of_clk_get_parent_name(node, i)) != NULL)
-		i++;
-
-	/*
-	 * some factor clocks, such as pll5 and pll6, may have multiple
-	 * outputs, and have their name designated in factors_data
-	 */
-	if (data->name)
-		clk_name = data->name;
-	else
-		of_property_read_string(node, "clock-output-names", &clk_name);
-
-	factors = kzalloc(sizeof(struct clk_factors), GFP_KERNEL);
-	if (!factors)
-		return NULL;
-
-	/* Add a gate if this factor clock can be gated */
-	if (data->enable) {
-		gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
-		if (!gate) {
-			kfree(factors);
-			return NULL;
-		}
-
-		/* set up gate properties */
-		gate->reg = reg;
-		gate->bit_idx = data->enable;
-		gate->lock = &clk_lock;
-		gate_hw = &gate->hw;
-	}
-
-	/* Add a mux if this factor clock can be muxed */
-	if (data->mux) {
-		mux = kzalloc(sizeof(struct clk_mux), GFP_KERNEL);
-		if (!mux) {
-			kfree(factors);
-			kfree(gate);
-			return NULL;
-		}
-
-		/* set up gate properties */
-		mux->reg = reg;
-		mux->shift = data->mux;
-		mux->mask = SUNXI_FACTORS_MUX_MASK;
-		mux->lock = &clk_lock;
-		mux_hw = &mux->hw;
-	}
-
-	/* set up factors properties */
-	factors->reg = reg;
-	factors->config = data->table;
-	factors->get_factors = data->getter;
-	factors->lock = &clk_lock;
-
-	clk = clk_register_composite(NULL, clk_name,
-			parents, i,
-			mux_hw, &clk_mux_ops,
-			&factors->hw, &clk_factors_ops,
-			gate_hw, &clk_gate_ops, 0);
-
-	if (!IS_ERR(clk)) {
-		of_clk_add_provider(node, of_clk_src_simple_get, clk);
-		clk_register_clkdev(clk, clk_name, NULL);
-	}
-
-	return clk;
+	return sunxi_factors_register(node, data, &clk_lock);
 }
 
 
@@ -762,10 +618,19 @@ static const struct div_data sun4i_ahb_data __initconst = {
 	.width	= 2,
 };
 
+static const struct clk_div_table sun4i_apb0_table[] __initconst = {
+	{ .val = 0, .div = 2 },
+	{ .val = 1, .div = 2 },
+	{ .val = 2, .div = 4 },
+	{ .val = 3, .div = 8 },
+	{ } /* sentinel */
+};
+
 static const struct div_data sun4i_apb0_data __initconst = {
 	.shift	= 8,
 	.pow	= 1,
 	.width	= 2,
+	.table	= sun4i_apb0_table,
 };
 
 static const struct div_data sun6i_a31_apb2_div_data __initconst = {
@@ -1199,7 +1064,6 @@ static const struct of_device_id clk_factors_match[] __initconst = {
 	{.compatible = "allwinner,sun7i-a20-pll4-clk", .data = &sun7i_a20_pll4_data,},
 	{.compatible = "allwinner,sun6i-a31-pll6-clk", .data = &sun6i_a31_pll6_data,},
 	{.compatible = "allwinner,sun4i-a10-apb1-clk", .data = &sun4i_apb1_data,},
-	{.compatible = "allwinner,sun4i-a10-mod0-clk", .data = &sun4i_mod0_data,},
 	{.compatible = "allwinner,sun7i-a20-out-clk", .data = &sun7i_a20_out_data,},
 	{}
 };
@@ -1311,7 +1175,6 @@ static void __init sun4i_a10_init_clocks(struct device_node *node)
 CLK_OF_DECLARE(sun4i_a10_clk_init, "allwinner,sun4i-a10", sun4i_a10_init_clocks);
 
 static const char *sun5i_critical_clocks[] __initdata = {
-	"mbus",
 	"pll5_ddr",
 	"ahb_sdram",
 };
diff --git a/drivers/clk/tegra/clk-tegra124.c b/drivers/clk/tegra/clk-tegra124.c
index 9525c684d149..e3a85842ce0c 100644
--- a/drivers/clk/tegra/clk-tegra124.c
+++ b/drivers/clk/tegra/clk-tegra124.c
@@ -1166,6 +1166,12 @@ static void __init tegra124_pll_init(void __iomem *clk_base,
 	clk_register_clkdev(clk, "pll_c_out1", NULL);
 	clks[TEGRA124_CLK_PLL_C_OUT1] = clk;
 
+	/* PLLC_UD */
+	clk = clk_register_fixed_factor(NULL, "pll_c_ud", "pll_c",
+					CLK_SET_RATE_PARENT, 1, 1);
+	clk_register_clkdev(clk, "pll_c_ud", NULL);
+	clks[TEGRA124_CLK_PLL_C_UD] = clk;
+
 	/* PLLC2 */
 	clk = tegra_clk_register_pllc("pll_c2", "pll_ref", clk_base, pmc, 0,
 			     &pll_c2_params, NULL);
@@ -1198,6 +1204,8 @@ static void __init tegra124_pll_init(void __iomem *clk_base,
 	/* PLLM_UD */
 	clk = clk_register_fixed_factor(NULL, "pll_m_ud", "pll_m",
 					CLK_SET_RATE_PARENT, 1, 1);
+	clk_register_clkdev(clk, "pll_m_ud", NULL);
+	clks[TEGRA124_CLK_PLL_M_UD] = clk;
 
 	/* PLLU */
 	val = readl(clk_base + pll_u_params.base_reg);
diff --git a/drivers/clk/tegra/clk.c b/drivers/clk/tegra/clk.c
index f87c609e8f72..97dc8595c3cd 100644
--- a/drivers/clk/tegra/clk.c
+++ b/drivers/clk/tegra/clk.c
@@ -207,8 +207,13 @@ void __init tegra_init_from_table(struct tegra_clk_init_table *tbl,
 
 	for (; tbl->clk_id < clk_max; tbl++) {
 		clk = clks[tbl->clk_id];
-		if (IS_ERR_OR_NULL(clk))
-			return;
+		if (IS_ERR_OR_NULL(clk)) {
+			pr_err("%s: invalid entry %ld in clks array for id %d\n",
+			       __func__, PTR_ERR(clk), tbl->clk_id);
+			WARN_ON(1);
+
+			continue;
+		}
 
 		if (tbl->parent_id < clk_max) {
 			struct clk *parent = clks[tbl->parent_id];
diff --git a/drivers/clk/ti/clk-dra7-atl.c b/drivers/clk/ti/clk-dra7-atl.c
index 4a65b410e4d5..59bb4b39d12e 100644
--- a/drivers/clk/ti/clk-dra7-atl.c
+++ b/drivers/clk/ti/clk-dra7-atl.c
@@ -139,9 +139,13 @@ static long atl_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 static int atl_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 			    unsigned long parent_rate)
 {
-	struct dra7_atl_desc *cdesc = to_atl_desc(hw);
+	struct dra7_atl_desc *cdesc;
 	u32 divider;
 
+	if (!hw || !rate)
+		return -EINVAL;
+
+	cdesc = to_atl_desc(hw);
 	divider = ((parent_rate + rate / 2) / rate) - 1;
 	if (divider > DRA7_ATL_DIVIDER_MASK)
 		divider = DRA7_ATL_DIVIDER_MASK;
@@ -199,6 +203,7 @@ static void __init of_dra7_atl_clock_setup(struct device_node *node)
 
 	if (!IS_ERR(clk)) {
 		of_clk_add_provider(node, of_clk_src_simple_get, clk);
+		kfree(parent_names);
 		return;
 	}
 cleanup:
@@ -224,6 +229,7 @@ static int of_dra7_atl_clk_probe(struct platform_device *pdev)
 	cinfo->iobase = of_iomap(node, 0);
 	cinfo->dev = &pdev->dev;
 	pm_runtime_enable(cinfo->dev);
+	pm_runtime_irq_safe(cinfo->dev);
 
 	pm_runtime_get_sync(cinfo->dev);
 	atl_write(cinfo, DRA7_ATL_PCLKMUX_REG(0), DRA7_ATL_PCLKMUX);
@@ -297,7 +303,6 @@ MODULE_DEVICE_TABLE(of, of_dra7_atl_clk_match_tbl);
 static struct platform_driver dra7_atl_clk_driver = {
 	.driver = {
 		.name = "dra7-atl",
-		.owner = THIS_MODULE,
 		.of_match_table = of_dra7_atl_clk_match_tbl,
 	},
 	.probe = of_dra7_atl_clk_probe,
diff --git a/drivers/clk/ti/clk.c b/drivers/clk/ti/clk.c
index b1a6f7144f3f..337abe5909e1 100644
--- a/drivers/clk/ti/clk.c
+++ b/drivers/clk/ti/clk.c
@@ -25,8 +25,8 @@
 #undef pr_fmt
 #define pr_fmt(fmt) "%s: " fmt, __func__
 
-static int ti_dt_clk_memmap_index;
 struct ti_clk_ll_ops *ti_clk_ll_ops;
+static struct device_node *clocks_node_ptr[CLK_MAX_MEMMAPS];
 
 /**
  * ti_dt_clocks_register - register DT alias clocks during boot
@@ -108,9 +108,21 @@ void __iomem *ti_clk_get_reg_addr(struct device_node *node, int index)
 	struct clk_omap_reg *reg;
 	u32 val;
 	u32 tmp;
+	int i;
 
 	reg = (struct clk_omap_reg *)&tmp;
-	reg->index = ti_dt_clk_memmap_index;
+
+	for (i = 0; i < CLK_MAX_MEMMAPS; i++) {
+		if (clocks_node_ptr[i] == node->parent)
+			break;
+	}
+
+	if (i == CLK_MAX_MEMMAPS) {
+		pr_err("clk-provider not found for %s!\n", node->name);
+		return NULL;
+	}
+
+	reg->index = i;
 
 	if (of_property_read_u32_index(node, "reg", index, &val)) {
 		pr_err("%s must have reg[%d]!\n", node->name, index);
@@ -127,20 +139,14 @@ void __iomem *ti_clk_get_reg_addr(struct device_node *node, int index)
  * @parent: master node
  * @index: internal index for clk_reg_ops
  *
- * Initializes a master clock IP block and its child clock nodes.
- * Regmap is provided for accessing the register space for the
- * IP block and all the clocks under it.
+ * Initializes a master clock IP block. This basically sets up the
+ * mapping from clocks node to the memory map index. All the clocks
+ * are then initialized through the common of_clk_init call, and the
+ * clocks will access their memory maps based on the node layout.
  */
 void ti_dt_clk_init_provider(struct device_node *parent, int index)
 {
-	const struct of_device_id *match;
-	struct device_node *np;
 	struct device_node *clocks;
-	of_clk_init_cb_t clk_init_cb;
-	struct clk_init_item *retry;
-	struct clk_init_item *tmp;
-
-	ti_dt_clk_memmap_index = index;
 
 	/* get clocks for this parent */
 	clocks = of_get_child_by_name(parent, "clocks");
@@ -149,19 +155,31 @@ void ti_dt_clk_init_provider(struct device_node *parent, int index)
 		return;
 	}
 
-	for_each_child_of_node(clocks, np) {
-		match = of_match_node(&__clk_of_table, np);
-		if (!match)
-			continue;
-		clk_init_cb = (of_clk_init_cb_t)match->data;
-		pr_debug("%s: initializing: %s\n", __func__, np->name);
-		clk_init_cb(np);
-	}
+	/* add clocks node info */
+	clocks_node_ptr[index] = clocks;
+}
 
-	list_for_each_entry_safe(retry, tmp, &retry_list, link) {
-		pr_debug("retry-init: %s\n", retry->node->name);
-		retry->func(retry->hw, retry->node);
-		list_del(&retry->link);
-		kfree(retry);
+/**
+ * ti_dt_clk_init_retry_clks - init clocks from the retry list
+ *
+ * Initializes any clocks that have failed to initialize before,
+ * reasons being missing parent node(s) during earlier init. This
+ * typically happens only for DPLLs which need to have both of their
+ * parent clocks ready during init.
+ */
+void ti_dt_clk_init_retry_clks(void)
+{
+	struct clk_init_item *retry;
+	struct clk_init_item *tmp;
+	int retries = 5;
+
+	while (!list_empty(&retry_list) && retries) {
+		list_for_each_entry_safe(retry, tmp, &retry_list, link) {
+			pr_debug("retry-init: %s\n", retry->node->name);
+			retry->func(retry->hw, retry->node);
+			list_del(&retry->link);
+			kfree(retry);
+		}
+		retries--;
 	}
 }
diff --git a/drivers/clk/ti/clockdomain.c b/drivers/clk/ti/clockdomain.c
index f1e0038d76ac..b4c5faccaece 100644
--- a/drivers/clk/ti/clockdomain.c
+++ b/drivers/clk/ti/clockdomain.c
@@ -36,6 +36,11 @@ static void __init of_ti_clockdomain_setup(struct device_node *node)
 
 	for (i = 0; i < num_clks; i++) {
 		clk = of_clk_get(node, i);
+		if (IS_ERR(clk)) {
+			pr_err("%s: Failed get %s' clock nr %d (%ld)\n",
+			       __func__, node->full_name, i, PTR_ERR(clk));
+			continue;
+		}
 		if (__clk_get_flags(clk) & CLK_IS_BASIC) {
 			pr_warn("can't setup clkdm for basic clk %s\n",
 				__clk_get_name(clk));
diff --git a/drivers/clk/ti/divider.c b/drivers/clk/ti/divider.c
index e6aa10db7bba..bff2b5b8ff59 100644
--- a/drivers/clk/ti/divider.c
+++ b/drivers/clk/ti/divider.c
@@ -211,11 +211,16 @@ static long ti_clk_divider_round_rate(struct clk_hw *hw, unsigned long rate,
 static int ti_clk_divider_set_rate(struct clk_hw *hw, unsigned long rate,
 				   unsigned long parent_rate)
 {
-	struct clk_divider *divider = to_clk_divider(hw);
+	struct clk_divider *divider;
 	unsigned int div, value;
 	unsigned long flags = 0;
 	u32 val;
 
+	if (!hw || !rate)
+		return -EINVAL;
+
+	divider = to_clk_divider(hw);
+
 	div = DIV_ROUND_UP(parent_rate, rate);
 	value = _get_val(divider, div);
 
@@ -295,8 +300,8 @@ static struct clk *_register_divider(struct device *dev, const char *name,
 	return clk;
 }
 
-static struct clk_div_table
-__init *ti_clk_get_div_table(struct device_node *node)
+static struct clk_div_table *
+__init ti_clk_get_div_table(struct device_node *node)
 {
 	struct clk_div_table *table;
 	const __be32 *divspec;
diff --git a/drivers/clk/zynq/clkc.c b/drivers/clk/zynq/clkc.c
index 246cf1226eaa..9037bebd69f7 100644
--- a/drivers/clk/zynq/clkc.c
+++ b/drivers/clk/zynq/clkc.c
@@ -85,24 +85,22 @@ static DEFINE_SPINLOCK(canmioclk_lock);
 static DEFINE_SPINLOCK(dbgclk_lock);
 static DEFINE_SPINLOCK(aperclk_lock);
 
-static const char dummy_nm[] __initconst = "dummy_name";
-
-static const char *armpll_parents[] __initdata = {"armpll_int", "ps_clk"};
-static const char *ddrpll_parents[] __initdata = {"ddrpll_int", "ps_clk"};
-static const char *iopll_parents[] __initdata = {"iopll_int", "ps_clk"};
-static const char *gem0_mux_parents[] __initdata = {"gem0_div1", dummy_nm};
-static const char *gem1_mux_parents[] __initdata = {"gem1_div1", dummy_nm};
-static const char *can0_mio_mux2_parents[] __initdata = {"can0_gate",
+static const char *armpll_parents[] __initconst = {"armpll_int", "ps_clk"};
+static const char *ddrpll_parents[] __initconst = {"ddrpll_int", "ps_clk"};
+static const char *iopll_parents[] __initconst = {"iopll_int", "ps_clk"};
+static const char *gem0_mux_parents[] __initconst = {"gem0_div1", "dummy_name"};
+static const char *gem1_mux_parents[] __initconst = {"gem1_div1", "dummy_name"};
+static const char *can0_mio_mux2_parents[] __initconst = {"can0_gate",
 	"can0_mio_mux"};
-static const char *can1_mio_mux2_parents[] __initdata = {"can1_gate",
+static const char *can1_mio_mux2_parents[] __initconst = {"can1_gate",
 	"can1_mio_mux"};
-static const char *dbg_emio_mux_parents[] __initdata = {"dbg_div",
-	dummy_nm};
+static const char *dbg_emio_mux_parents[] __initconst = {"dbg_div",
+	"dummy_name"};
 
-static const char *dbgtrc_emio_input_names[] __initdata = {"trace_emio_clk"};
-static const char *gem0_emio_input_names[] __initdata = {"gem0_emio_clk"};
-static const char *gem1_emio_input_names[] __initdata = {"gem1_emio_clk"};
-static const char *swdt_ext_clk_input_names[] __initdata = {"swdt_ext_clk"};
+static const char *dbgtrc_emio_input_names[] __initconst = {"trace_emio_clk"};
+static const char *gem0_emio_input_names[] __initconst = {"gem0_emio_clk"};
+static const char *gem1_emio_input_names[] __initconst = {"gem1_emio_clk"};
+static const char *swdt_ext_clk_input_names[] __initconst = {"swdt_ext_clk"};
 
 static void __init zynq_clk_register_fclk(enum zynq_clk fclk,
 		const char *clk_name, void __iomem *fclk_ctrl_reg,
@@ -230,6 +228,7 @@ static void __init zynq_clk_setup(struct device_node *np)
 	const char *periph_parents[4];
 	const char *swdt_ext_clk_mux_parents[2];
 	const char *can_mio_mux_parents[NUM_MIO_PINS];
+	const char *dummy_nm = "dummy_name";
 
 	pr_info("Zynq clock init\n");
 
@@ -619,5 +618,4 @@ void __init zynq_clock_init(void)
 np_err:
 	of_node_put(np);
 	BUG();
-	return;
 }
diff --git a/drivers/clk/zynq/pll.c b/drivers/clk/zynq/pll.c
index cec97596fe65..00d72fb5c036 100644
--- a/drivers/clk/zynq/pll.c
+++ b/drivers/clk/zynq/pll.c
@@ -211,10 +211,8 @@ struct clk *clk_register_zynq_pll(const char *name, const char *parent,
 	};
 
 	pll = kmalloc(sizeof(*pll), GFP_KERNEL);
-	if (!pll) {
-		pr_err("%s: Could not allocate Zynq PLL clk.\n", __func__);
+	if (!pll)
 		return ERR_PTR(-ENOMEM);
-	}
 
 	/* Populate the struct */
 	pll->hw.init = &initd;
diff --git a/drivers/clocksource/Kconfig b/drivers/clocksource/Kconfig
index cfd6519df661..90420600e1eb 100644
--- a/drivers/clocksource/Kconfig
+++ b/drivers/clocksource/Kconfig
@@ -30,6 +30,9 @@ config ARMADA_370_XP_TIMER
 	bool
 	select CLKSRC_OF
 
+config MESON6_TIMER
+	bool
+
 config ORION_TIMER
 	select CLKSRC_OF
 	select CLKSRC_MMIO
@@ -120,6 +123,10 @@ config CLKSRC_ARM_GLOBAL_TIMER_SCHED_CLOCK
 	help
 	 Use ARM global timer clock source as sched_clock
 
+config ATMEL_PIT
+	select CLKSRC_OF if OF
+	def_bool SOC_AT91SAM9 || SOC_SAMA5
+
 config CLKSRC_METAG_GENERIC
 	def_bool y if METAG
 	help
diff --git a/drivers/clocksource/Makefile b/drivers/clocksource/Makefile
index 7fd9fd1dff42..756f6f10efa0 100644
--- a/drivers/clocksource/Makefile
+++ b/drivers/clocksource/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_CLKSRC_OF)	+= clksrc-of.o
+obj-$(CONFIG_ATMEL_PIT)		+= timer-atmel-pit.o
 obj-$(CONFIG_ATMEL_TCB_CLKSRC)	+= tcb_clksrc.o
 obj-$(CONFIG_X86_PM_TIMER)	+= acpi_pm.o
 obj-$(CONFIG_SCx200HR_TIMER)	+= scx200_hrt.o
@@ -25,6 +26,7 @@ obj-$(CONFIG_ARCH_PRIMA2)	+= timer-prima2.o
 obj-$(CONFIG_ARCH_U300)		+= timer-u300.o
 obj-$(CONFIG_SUN4I_TIMER)	+= sun4i_timer.o
 obj-$(CONFIG_SUN5I_HSTIMER)	+= timer-sun5i.o
+obj-$(CONFIG_MESON6_TIMER)	+= meson6_timer.o
 obj-$(CONFIG_ARCH_TEGRA)	+= tegra20_timer.o
 obj-$(CONFIG_VT8500_TIMER)	+= vt8500_timer.o
 obj-$(CONFIG_ARCH_NSPIRE)	+= zevio-timer.o
diff --git a/drivers/clocksource/arm_arch_timer.c b/drivers/clocksource/arm_arch_timer.c
index 5163ec13429d..2133f9d59d06 100644
--- a/drivers/clocksource/arm_arch_timer.c
+++ b/drivers/clocksource/arm_arch_timer.c
@@ -299,6 +299,21 @@ static void __arch_timer_setup(unsigned type,
 	clockevents_config_and_register(clk, arch_timer_rate, 0xf, 0x7fffffff);
 }
 
+static void arch_timer_evtstrm_enable(int divider)
+{
+	u32 cntkctl = arch_timer_get_cntkctl();
+
+	cntkctl &= ~ARCH_TIMER_EVT_TRIGGER_MASK;
+	/* Set the divider and enable virtual event stream */
+	cntkctl |= (divider << ARCH_TIMER_EVT_TRIGGER_SHIFT)
+			| ARCH_TIMER_VIRT_EVT_EN;
+	arch_timer_set_cntkctl(cntkctl);
+	elf_hwcap |= HWCAP_EVTSTRM;
+#ifdef CONFIG_COMPAT
+	compat_elf_hwcap |= COMPAT_HWCAP_EVTSTRM;
+#endif
+}
+
 static void arch_timer_configure_evtstream(void)
 {
 	int evt_stream_div, pos;
@@ -312,6 +327,23 @@ static void arch_timer_configure_evtstream(void)
 	arch_timer_evtstrm_enable(min(pos, 15));
 }
 
+static void arch_counter_set_user_access(void)
+{
+	u32 cntkctl = arch_timer_get_cntkctl();
+
+	/* Disable user access to the timers and the physical counter */
+	/* Also disable virtual event stream */
+	cntkctl &= ~(ARCH_TIMER_USR_PT_ACCESS_EN
+			| ARCH_TIMER_USR_VT_ACCESS_EN
+			| ARCH_TIMER_VIRT_EVT_EN
+			| ARCH_TIMER_USR_PCT_ACCESS_EN);
+
+	/* Enable user access to the virtual counter */
+	cntkctl |= ARCH_TIMER_USR_VCT_ACCESS_EN;
+
+	arch_timer_set_cntkctl(cntkctl);
+}
+
 static int arch_timer_setup(struct clock_event_device *clk)
 {
 	__arch_timer_setup(ARCH_CP15_TIMER, clk);
@@ -429,11 +461,19 @@ static void __init arch_counter_register(unsigned type)
 	u64 start_count;
 
 	/* Register the CP15 based counter if we have one */
-	if (type & ARCH_CP15_TIMER)
+	if (type & ARCH_CP15_TIMER) {
 		arch_timer_read_counter = arch_counter_get_cntvct;
-	else
+	} else {
 		arch_timer_read_counter = arch_counter_get_cntvct_mem;
 
+		/* If the clocksource name is "arch_sys_counter" the
+		 * VDSO will attempt to read the CP15-based counter.
+		 * Ensure this does not happen when CP15-based
+		 * counter is not available.
+		 */
+		clocksource_counter.name = "arch_mem_counter";
+	}
+
 	start_count = arch_timer_read_counter();
 	clocksource_register_hz(&clocksource_counter, arch_timer_rate);
 	cyclecounter.mult = clocksource_counter.mult;
@@ -616,17 +656,29 @@ static const struct of_device_id arch_timer_mem_of_match[] __initconst = {
 	{},
 };
 
+static bool __init
+arch_timer_probed(int type, const struct of_device_id *matches)
+{
+	struct device_node *dn;
+	bool probed = false;
+
+	dn = of_find_matching_node(NULL, matches);
+	if (dn && of_device_is_available(dn) && (arch_timers_present & type))
+		probed = true;
+	of_node_put(dn);
+
+	return probed;
+}
+
 static void __init arch_timer_common_init(void)
 {
 	unsigned mask = ARCH_CP15_TIMER | ARCH_MEM_TIMER;
 
 	/* Wait until both nodes are probed if we have two timers */
 	if ((arch_timers_present & mask) != mask) {
-		if (of_find_matching_node(NULL, arch_timer_mem_of_match) &&
-				!(arch_timers_present & ARCH_MEM_TIMER))
+		if (!arch_timer_probed(ARCH_MEM_TIMER, arch_timer_mem_of_match))
 			return;
-		if (of_find_matching_node(NULL, arch_timer_of_match) &&
-				!(arch_timers_present & ARCH_CP15_TIMER))
+		if (!arch_timer_probed(ARCH_CP15_TIMER, arch_timer_of_match))
 			return;
 	}
 
diff --git a/drivers/clocksource/cadence_ttc_timer.c b/drivers/clocksource/cadence_ttc_timer.c
index 7a08811df9aa..510c8a1d37b3 100644
--- a/drivers/clocksource/cadence_ttc_timer.c
+++ b/drivers/clocksource/cadence_ttc_timer.c
@@ -25,7 +25,7 @@
 #include <linux/sched_clock.h>
 
 /*
- * This driver configures the 2 16-bit count-up timers as follows:
+ * This driver configures the 2 16/32-bit count-up timers as follows:
  *
  * T1: Timer 1, clocksource for generic timekeeping
  * T2: Timer 2, clockevent source for hrtimers
@@ -321,7 +321,8 @@ static int ttc_rate_change_clocksource_cb(struct notifier_block *nb,
 	return NOTIFY_DONE;
 }
 
-static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base)
+static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base,
+					 u32 timer_width)
 {
 	struct ttc_timer_clocksource *ttccs;
 	int err;
@@ -351,7 +352,7 @@ static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base)
 	ttccs->cs.name = "ttc_clocksource";
 	ttccs->cs.rating = 200;
 	ttccs->cs.read = __ttc_clocksource_read;
-	ttccs->cs.mask = CLOCKSOURCE_MASK(16);
+	ttccs->cs.mask = CLOCKSOURCE_MASK(timer_width);
 	ttccs->cs.flags = CLOCK_SOURCE_IS_CONTINUOUS;
 
 	/*
@@ -372,7 +373,8 @@ static void __init ttc_setup_clocksource(struct clk *clk, void __iomem *base)
 	}
 
 	ttc_sched_clock_val_reg = base + TTC_COUNT_VAL_OFFSET;
-	sched_clock_register(ttc_sched_clock_read, 16, ttccs->ttc.freq / PRESCALE);
+	sched_clock_register(ttc_sched_clock_read, timer_width,
+			     ttccs->ttc.freq / PRESCALE);
 }
 
 static int ttc_rate_change_clockevent_cb(struct notifier_block *nb,
@@ -467,6 +469,7 @@ static void __init ttc_timer_init(struct device_node *timer)
 	struct clk *clk_cs, *clk_ce;
 	static int initialized;
 	int clksel;
+	u32 timer_width = 16;
 
 	if (initialized)
 		return;
@@ -490,6 +493,8 @@ static void __init ttc_timer_init(struct device_node *timer)
 		BUG();
 	}
 
+	of_property_read_u32(timer, "timer-width", &timer_width);
+
 	clksel = readl_relaxed(timer_baseaddr + TTC_CLK_CNTRL_OFFSET);
 	clksel = !!(clksel & TTC_CLK_CNTRL_CSRC_MASK);
 	clk_cs = of_clk_get(timer, clksel);
@@ -506,7 +511,7 @@ static void __init ttc_timer_init(struct device_node *timer)
 		BUG();
 	}
 
-	ttc_setup_clocksource(clk_cs, timer_baseaddr);
+	ttc_setup_clocksource(clk_cs, timer_baseaddr, timer_width);
 	ttc_setup_clockevent(clk_ce, timer_baseaddr + 4, irq);
 
 	pr_info("%s #0 at %p, irq=%d\n", timer->name, timer_baseaddr, irq);
diff --git a/drivers/clocksource/dummy_timer.c b/drivers/clocksource/dummy_timer.c
index ad3572541728..31990600fcff 100644
--- a/drivers/clocksource/dummy_timer.c
+++ b/drivers/clocksource/dummy_timer.c
@@ -28,7 +28,7 @@ static void dummy_timer_set_mode(enum clock_event_mode mode,
 static void dummy_timer_setup(void)
 {
 	int cpu = smp_processor_id();
-	struct clock_event_device *evt = __this_cpu_ptr(&dummy_timer_evt);
+	struct clock_event_device *evt = raw_cpu_ptr(&dummy_timer_evt);
 
 	evt->name	= "dummy_timer";
 	evt->features	= CLOCK_EVT_FEAT_PERIODIC |
diff --git a/drivers/clocksource/meson6_timer.c b/drivers/clocksource/meson6_timer.c
new file mode 100644
index 000000000000..5c15cba41dca
--- /dev/null
+++ b/drivers/clocksource/meson6_timer.c
@@ -0,0 +1,167 @@
+/*
+ * Amlogic Meson6 SoCs timer handling.
+ *
+ * Copyright (C) 2014 Carlo Caione <carlo@caione.org>
+ *
+ * Based on code from Amlogic, Inc
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqreturn.h>
+#include <linux/sched_clock.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#define CED_ID			0
+#define CSD_ID			4
+
+#define TIMER_ISA_MUX		0
+#define TIMER_ISA_VAL(t)	(((t) + 1) << 2)
+
+#define TIMER_INPUT_BIT(t)	(2 * (t))
+#define TIMER_ENABLE_BIT(t)	(16 + (t))
+#define TIMER_PERIODIC_BIT(t)	(12 + (t))
+
+#define TIMER_CED_INPUT_MASK	(3UL << TIMER_INPUT_BIT(CED_ID))
+#define TIMER_CSD_INPUT_MASK	(7UL << TIMER_INPUT_BIT(CSD_ID))
+
+#define TIMER_CED_UNIT_1US	0
+#define TIMER_CSD_UNIT_1US	1
+
+static void __iomem *timer_base;
+
+static u64 notrace meson6_timer_sched_read(void)
+{
+	return (u64)readl(timer_base + TIMER_ISA_VAL(CSD_ID));
+}
+
+static void meson6_clkevt_time_stop(unsigned char timer)
+{
+	u32 val = readl(timer_base + TIMER_ISA_MUX);
+
+	writel(val & ~TIMER_ENABLE_BIT(timer), timer_base + TIMER_ISA_MUX);
+}
+
+static void meson6_clkevt_time_setup(unsigned char timer, unsigned long delay)
+{
+	writel(delay, timer_base + TIMER_ISA_VAL(timer));
+}
+
+static void meson6_clkevt_time_start(unsigned char timer, bool periodic)
+{
+	u32 val = readl(timer_base + TIMER_ISA_MUX);
+
+	if (periodic)
+		val |= TIMER_PERIODIC_BIT(timer);
+	else
+		val &= ~TIMER_PERIODIC_BIT(timer);
+
+	writel(val | TIMER_ENABLE_BIT(timer), timer_base + TIMER_ISA_MUX);
+}
+
+static void meson6_clkevt_mode(enum clock_event_mode mode,
+			       struct clock_event_device *clk)
+{
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		meson6_clkevt_time_stop(CED_ID);
+		meson6_clkevt_time_setup(CED_ID, USEC_PER_SEC/HZ - 1);
+		meson6_clkevt_time_start(CED_ID, true);
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+		meson6_clkevt_time_stop(CED_ID);
+		meson6_clkevt_time_start(CED_ID, false);
+		break;
+	case CLOCK_EVT_MODE_UNUSED:
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	default:
+		meson6_clkevt_time_stop(CED_ID);
+		break;
+	}
+}
+
+static int meson6_clkevt_next_event(unsigned long evt,
+				    struct clock_event_device *unused)
+{
+	meson6_clkevt_time_stop(CED_ID);
+	meson6_clkevt_time_setup(CED_ID, evt);
+	meson6_clkevt_time_start(CED_ID, false);
+
+	return 0;
+}
+
+static struct clock_event_device meson6_clockevent = {
+	.name		= "meson6_tick",
+	.rating		= 400,
+	.features	= CLOCK_EVT_FEAT_PERIODIC | CLOCK_EVT_FEAT_ONESHOT,
+	.set_mode	= meson6_clkevt_mode,
+	.set_next_event	= meson6_clkevt_next_event,
+};
+
+static irqreturn_t meson6_timer_interrupt(int irq, void *dev_id)
+{
+	struct clock_event_device *evt = (struct clock_event_device *)dev_id;
+
+	evt->event_handler(evt);
+
+	return IRQ_HANDLED;
+}
+
+static struct irqaction meson6_timer_irq = {
+	.name		= "meson6_timer",
+	.flags		= IRQF_TIMER | IRQF_IRQPOLL,
+	.handler	= meson6_timer_interrupt,
+	.dev_id		= &meson6_clockevent,
+};
+
+static void __init meson6_timer_init(struct device_node *node)
+{
+	u32 val;
+	int ret, irq;
+
+	timer_base = of_io_request_and_map(node, 0, "meson6-timer");
+	if (IS_ERR(timer_base))
+		panic("Can't map registers");
+
+	irq = irq_of_parse_and_map(node, 0);
+	if (irq <= 0)
+		panic("Can't parse IRQ");
+
+	/* Set 1us for timer E */
+	val = readl(timer_base + TIMER_ISA_MUX);
+	val &= ~TIMER_CSD_INPUT_MASK;
+	val |= TIMER_CSD_UNIT_1US << TIMER_INPUT_BIT(CSD_ID);
+	writel(val, timer_base + TIMER_ISA_MUX);
+
+	sched_clock_register(meson6_timer_sched_read, 32, USEC_PER_SEC);
+	clocksource_mmio_init(timer_base + TIMER_ISA_VAL(CSD_ID), node->name,
+			      1000 * 1000, 300, 32, clocksource_mmio_readl_up);
+
+	/* Timer A base 1us */
+	val &= ~TIMER_CED_INPUT_MASK;
+	val |= TIMER_CED_UNIT_1US << TIMER_INPUT_BIT(CED_ID);
+	writel(val, timer_base + TIMER_ISA_MUX);
+
+	/* Stop the timer A */
+	meson6_clkevt_time_stop(CED_ID);
+
+	ret = setup_irq(irq, &meson6_timer_irq);
+	if (ret)
+		pr_warn("failed to setup irq %d\n", irq);
+
+	meson6_clockevent.cpumask = cpu_possible_mask;
+	meson6_clockevent.irq = irq;
+
+	clockevents_config_and_register(&meson6_clockevent, USEC_PER_SEC,
+					1, 0xfffe);
+}
+CLOCKSOURCE_OF_DECLARE(meson6, "amlogic,meson6-timer",
+		       meson6_timer_init);
diff --git a/drivers/clocksource/metag_generic.c b/drivers/clocksource/metag_generic.c
index 9e4db41abe3c..b7384b853e5a 100644
--- a/drivers/clocksource/metag_generic.c
+++ b/drivers/clocksource/metag_generic.c
@@ -90,7 +90,7 @@ static struct clocksource clocksource_metag = {
 
 static irqreturn_t metag_timer_interrupt(int irq, void *dummy)
 {
-	struct clock_event_device *evt = &__get_cpu_var(local_clockevent);
+	struct clock_event_device *evt = this_cpu_ptr(&local_clockevent);
 
 	evt->event_handler(evt);
 
diff --git a/drivers/clocksource/qcom-timer.c b/drivers/clocksource/qcom-timer.c
index 8d115db1e651..098c542e5c53 100644
--- a/drivers/clocksource/qcom-timer.c
+++ b/drivers/clocksource/qcom-timer.c
@@ -219,7 +219,7 @@ static void __init msm_timer_init(u32 dgt_hz, int sched_bits, int irq,
 		}
 
 		/* Immediately configure the timer on the boot CPU */
-		msm_local_timer_setup(__this_cpu_ptr(msm_evt));
+		msm_local_timer_setup(raw_cpu_ptr(msm_evt));
 	}
 
 err:
diff --git a/drivers/clocksource/tcb_clksrc.c b/drivers/clocksource/tcb_clksrc.c
index a8d7ea14f183..8bdbc45c6dad 100644
--- a/drivers/clocksource/tcb_clksrc.c
+++ b/drivers/clocksource/tcb_clksrc.c
@@ -178,12 +178,6 @@ static irqreturn_t ch2_irq(int irq, void *handle)
 	return IRQ_NONE;
 }
 
-static struct irqaction tc_irqaction = {
-	.name		= "tc_clkevt",
-	.flags		= IRQF_TIMER,
-	.handler	= ch2_irq,
-};
-
 static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
 {
 	int ret;
@@ -198,15 +192,16 @@ static int __init setup_clkevents(struct atmel_tc *tc, int clk32k_divisor_idx)
 
 	clkevt.regs = tc->regs;
 	clkevt.clk = t2_clk;
-	tc_irqaction.dev_id = &clkevt;
 
 	timer_clock = clk32k_divisor_idx;
 
 	clkevt.clkevt.cpumask = cpumask_of(0);
 
-	ret = setup_irq(irq, &tc_irqaction);
-	if (ret)
+	ret = request_irq(irq, ch2_irq, IRQF_TIMER, "tc_clkevt", &clkevt);
+	if (ret) {
+		clk_disable_unprepare(t2_clk);
 		return ret;
+	}
 
 	clockevents_config_and_register(&clkevt.clkevt, 32768, 1, 0xffff);
 
@@ -279,7 +274,7 @@ static int __init tcb_clksrc_init(void)
 	int i;
 	int ret;
 
-	tc = atmel_tc_alloc(CONFIG_ATMEL_TCB_CLKSRC_BLOCK, clksrc.name);
+	tc = atmel_tc_alloc(CONFIG_ATMEL_TCB_CLKSRC_BLOCK);
 	if (!tc) {
 		pr_debug("can't alloc TC for clocksource\n");
 		return -ENODEV;
diff --git a/drivers/clocksource/timer-atmel-pit.c b/drivers/clocksource/timer-atmel-pit.c
new file mode 100644
index 000000000000..d5289098b3df
--- /dev/null
+++ b/drivers/clocksource/timer-atmel-pit.c
@@ -0,0 +1,296 @@
+/*
+ * at91sam926x_time.c - Periodic Interval Timer (PIT) for at91sam926x
+ *
+ * Copyright (C) 2005-2006 M. Amine SAYA, ATMEL Rousset, France
+ * Revision	 2005 M. Nicolas Diremdjian, ATMEL Rousset, France
+ * Converted to ClockSource/ClockEvents by David Brownell.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define pr_fmt(fmt)	"AT91: PIT: " fmt
+
+#include <linux/clk.h>
+#include <linux/clockchips.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/slab.h>
+
+#define AT91_PIT_MR		0x00			/* Mode Register */
+#define AT91_PIT_PITIEN			BIT(25)			/* Timer Interrupt Enable */
+#define AT91_PIT_PITEN			BIT(24)			/* Timer Enabled */
+#define AT91_PIT_PIV			GENMASK(19, 0)		/* Periodic Interval Value */
+
+#define AT91_PIT_SR		0x04			/* Status Register */
+#define AT91_PIT_PITS			BIT(0)			/* Timer Status */
+
+#define AT91_PIT_PIVR		0x08			/* Periodic Interval Value Register */
+#define AT91_PIT_PIIR		0x0c			/* Periodic Interval Image Register */
+#define AT91_PIT_PICNT			GENMASK(31, 20)		/* Interval Counter */
+#define AT91_PIT_CPIV			GENMASK(19, 0)		/* Inverval Value */
+
+#define PIT_CPIV(x)	((x) & AT91_PIT_CPIV)
+#define PIT_PICNT(x)	(((x) & AT91_PIT_PICNT) >> 20)
+
+struct pit_data {
+	struct clock_event_device	clkevt;
+	struct clocksource		clksrc;
+
+	void __iomem	*base;
+	u32		cycle;
+	u32		cnt;
+	unsigned int	irq;
+	struct clk	*mck;
+};
+
+static inline struct pit_data *clksrc_to_pit_data(struct clocksource *clksrc)
+{
+	return container_of(clksrc, struct pit_data, clksrc);
+}
+
+static inline struct pit_data *clkevt_to_pit_data(struct clock_event_device *clkevt)
+{
+	return container_of(clkevt, struct pit_data, clkevt);
+}
+
+static inline unsigned int pit_read(void __iomem *base, unsigned int reg_offset)
+{
+	return __raw_readl(base + reg_offset);
+}
+
+static inline void pit_write(void __iomem *base, unsigned int reg_offset, unsigned long value)
+{
+	__raw_writel(value, base + reg_offset);
+}
+
+/*
+ * Clocksource:  just a monotonic counter of MCK/16 cycles.
+ * We don't care whether or not PIT irqs are enabled.
+ */
+static cycle_t read_pit_clk(struct clocksource *cs)
+{
+	struct pit_data *data = clksrc_to_pit_data(cs);
+	unsigned long flags;
+	u32 elapsed;
+	u32 t;
+
+	raw_local_irq_save(flags);
+	elapsed = data->cnt;
+	t = pit_read(data->base, AT91_PIT_PIIR);
+	raw_local_irq_restore(flags);
+
+	elapsed += PIT_PICNT(t) * data->cycle;
+	elapsed += PIT_CPIV(t);
+	return elapsed;
+}
+
+/*
+ * Clockevent device:  interrupts every 1/HZ (== pit_cycles * MCK/16)
+ */
+static void
+pit_clkevt_mode(enum clock_event_mode mode, struct clock_event_device *dev)
+{
+	struct pit_data *data = clkevt_to_pit_data(dev);
+
+	switch (mode) {
+	case CLOCK_EVT_MODE_PERIODIC:
+		/* update clocksource counter */
+		data->cnt += data->cycle * PIT_PICNT(pit_read(data->base, AT91_PIT_PIVR));
+		pit_write(data->base, AT91_PIT_MR,
+			  (data->cycle - 1) | AT91_PIT_PITEN | AT91_PIT_PITIEN);
+		break;
+	case CLOCK_EVT_MODE_ONESHOT:
+		BUG();
+		/* FALLTHROUGH */
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_UNUSED:
+		/* disable irq, leaving the clocksource active */
+		pit_write(data->base, AT91_PIT_MR,
+			  (data->cycle - 1) | AT91_PIT_PITEN);
+		break;
+	case CLOCK_EVT_MODE_RESUME:
+		break;
+	}
+}
+
+static void at91sam926x_pit_suspend(struct clock_event_device *cedev)
+{
+	struct pit_data *data = clkevt_to_pit_data(cedev);
+
+	/* Disable timer */
+	pit_write(data->base, AT91_PIT_MR, 0);
+}
+
+static void at91sam926x_pit_reset(struct pit_data *data)
+{
+	/* Disable timer and irqs */
+	pit_write(data->base, AT91_PIT_MR, 0);
+
+	/* Clear any pending interrupts, wait for PIT to stop counting */
+	while (PIT_CPIV(pit_read(data->base, AT91_PIT_PIVR)) != 0)
+		cpu_relax();
+
+	/* Start PIT but don't enable IRQ */
+	pit_write(data->base, AT91_PIT_MR,
+		  (data->cycle - 1) | AT91_PIT_PITEN);
+}
+
+static void at91sam926x_pit_resume(struct clock_event_device *cedev)
+{
+	struct pit_data *data = clkevt_to_pit_data(cedev);
+
+	at91sam926x_pit_reset(data);
+}
+
+/*
+ * IRQ handler for the timer.
+ */
+static irqreturn_t at91sam926x_pit_interrupt(int irq, void *dev_id)
+{
+	struct pit_data *data = dev_id;
+
+	/*
+	 * irqs should be disabled here, but as the irq is shared they are only
+	 * guaranteed to be off if the timer irq is registered first.
+	 */
+	WARN_ON_ONCE(!irqs_disabled());
+
+	/* The PIT interrupt may be disabled, and is shared */
+	if ((data->clkevt.mode == CLOCK_EVT_MODE_PERIODIC) &&
+	    (pit_read(data->base, AT91_PIT_SR) & AT91_PIT_PITS)) {
+		unsigned nr_ticks;
+
+		/* Get number of ticks performed before irq, and ack it */
+		nr_ticks = PIT_PICNT(pit_read(data->base, AT91_PIT_PIVR));
+		do {
+			data->cnt += data->cycle;
+			data->clkevt.event_handler(&data->clkevt);
+			nr_ticks--;
+		} while (nr_ticks);
+
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+/*
+ * Set up both clocksource and clockevent support.
+ */
+static void __init at91sam926x_pit_common_init(struct pit_data *data)
+{
+	unsigned long	pit_rate;
+	unsigned	bits;
+	int		ret;
+
+	/*
+	 * Use our actual MCK to figure out how many MCK/16 ticks per
+	 * 1/HZ period (instead of a compile-time constant LATCH).
+	 */
+	pit_rate = clk_get_rate(data->mck) / 16;
+	data->cycle = DIV_ROUND_CLOSEST(pit_rate, HZ);
+	WARN_ON(((data->cycle - 1) & ~AT91_PIT_PIV) != 0);
+
+	/* Initialize and enable the timer */
+	at91sam926x_pit_reset(data);
+
+	/*
+	 * Register clocksource.  The high order bits of PIV are unused,
+	 * so this isn't a 32-bit counter unless we get clockevent irqs.
+	 */
+	bits = 12 /* PICNT */ + ilog2(data->cycle) /* PIV */;
+	data->clksrc.mask = CLOCKSOURCE_MASK(bits);
+	data->clksrc.name = "pit";
+	data->clksrc.rating = 175;
+	data->clksrc.read = read_pit_clk,
+	data->clksrc.flags = CLOCK_SOURCE_IS_CONTINUOUS,
+	clocksource_register_hz(&data->clksrc, pit_rate);
+
+	/* Set up irq handler */
+	ret = request_irq(data->irq, at91sam926x_pit_interrupt,
+			  IRQF_SHARED | IRQF_TIMER | IRQF_IRQPOLL,
+			  "at91_tick", data);
+	if (ret)
+		panic(pr_fmt("Unable to setup IRQ\n"));
+
+	/* Set up and register clockevents */
+	data->clkevt.name = "pit";
+	data->clkevt.features = CLOCK_EVT_FEAT_PERIODIC;
+	data->clkevt.shift = 32;
+	data->clkevt.mult = div_sc(pit_rate, NSEC_PER_SEC, data->clkevt.shift);
+	data->clkevt.rating = 100;
+	data->clkevt.cpumask = cpumask_of(0);
+
+	data->clkevt.set_mode = pit_clkevt_mode;
+	data->clkevt.resume = at91sam926x_pit_resume;
+	data->clkevt.suspend = at91sam926x_pit_suspend;
+	clockevents_register_device(&data->clkevt);
+}
+
+static void __init at91sam926x_pit_dt_init(struct device_node *node)
+{
+	struct pit_data *data;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		panic(pr_fmt("Unable to allocate memory\n"));
+
+	data->base = of_iomap(node, 0);
+	if (!data->base)
+		panic(pr_fmt("Could not map PIT address\n"));
+
+	data->mck = of_clk_get(node, 0);
+	if (IS_ERR(data->mck))
+		/* Fallback on clkdev for !CCF-based boards */
+		data->mck = clk_get(NULL, "mck");
+
+	if (IS_ERR(data->mck))
+		panic(pr_fmt("Unable to get mck clk\n"));
+
+	/* Get the interrupts property */
+	data->irq = irq_of_parse_and_map(node, 0);
+	if (!data->irq)
+		panic(pr_fmt("Unable to get IRQ from DT\n"));
+
+	at91sam926x_pit_common_init(data);
+}
+CLOCKSOURCE_OF_DECLARE(at91sam926x_pit, "atmel,at91sam9260-pit",
+		       at91sam926x_pit_dt_init);
+
+static void __iomem *pit_base_addr;
+
+void __init at91sam926x_pit_init(int irq)
+{
+	struct pit_data *data;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		panic(pr_fmt("Unable to allocate memory\n"));
+
+	data->base = pit_base_addr;
+
+	data->mck = clk_get(NULL, "mck");
+	if (IS_ERR(data->mck))
+		panic(pr_fmt("Unable to get mck clk\n"));
+
+	data->irq = irq;
+
+	at91sam926x_pit_common_init(data);
+}
+
+void __init at91sam926x_ioremap_pit(u32 addr)
+{
+	if (of_have_populated_dt())
+		return;
+
+	pit_base_addr = ioremap(addr, 16);
+
+	if (!pit_base_addr)
+		panic(pr_fmt("Impossible to ioremap PIT\n"));
+}
diff --git a/drivers/clocksource/timer-marco.c b/drivers/clocksource/timer-marco.c
index 330e93064692..caf7a2030461 100644
--- a/drivers/clocksource/timer-marco.c
+++ b/drivers/clocksource/timer-marco.c
@@ -63,7 +63,7 @@ static inline void sirfsoc_timer_count_disable(int idx)
 /* enable count and interrupt */
 static inline void sirfsoc_timer_count_enable(int idx)
 {
-	writel_relaxed(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL + 4 * idx) | 0x7,
+	writel_relaxed(readl_relaxed(sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL + 4 * idx) | 0x3,
 		sirfsoc_timer_base + SIRFSOC_TIMER_32COUNTER_0_CTRL + 4 * idx);
 }
 
@@ -103,6 +103,9 @@ static int sirfsoc_timer_set_next_event(unsigned long delta,
 {
 	int cpu = smp_processor_id();
 
+	/* disable timer first, then modify the related registers */
+	sirfsoc_timer_count_disable(cpu);
+
 	writel_relaxed(0, sirfsoc_timer_base + SIRFSOC_TIMER_COUNTER_0 +
 		4 * cpu);
 	writel_relaxed(delta, sirfsoc_timer_base + SIRFSOC_TIMER_MATCH_0 +
diff --git a/drivers/clocksource/vf_pit_timer.c b/drivers/clocksource/vf_pit_timer.c
index a918bc481c52..b45ac6229b57 100644
--- a/drivers/clocksource/vf_pit_timer.c
+++ b/drivers/clocksource/vf_pit_timer.c
@@ -93,6 +93,10 @@ static void pit_set_mode(enum clock_event_mode mode,
 	case CLOCK_EVT_MODE_PERIODIC:
 		pit_set_next_event(cycle_per_jiffy, evt);
 		break;
+	case CLOCK_EVT_MODE_SHUTDOWN:
+	case CLOCK_EVT_MODE_UNUSED:
+		pit_timer_disable();
+		break;
 	default:
 		break;
 	}
diff --git a/drivers/cpufreq/Kconfig b/drivers/cpufreq/Kconfig
index ffe350f86bca..3489f8f5fada 100644
--- a/drivers/cpufreq/Kconfig
+++ b/drivers/cpufreq/Kconfig
@@ -183,14 +183,14 @@ config CPU_FREQ_GOV_CONSERVATIVE
 
 	  If in doubt, say N.
 
-config GENERIC_CPUFREQ_CPU0
-	tristate "Generic CPU0 cpufreq driver"
+config CPUFREQ_DT
+	tristate "Generic DT based cpufreq driver"
 	depends on HAVE_CLK && OF
-	# if CPU_THERMAL is on and THERMAL=m, CPU0 cannot be =y:
+	# if CPU_THERMAL is on and THERMAL=m, CPUFREQ_DT cannot be =y:
 	depends on !CPU_THERMAL || THERMAL
 	select PM_OPP
 	help
-	  This adds a generic cpufreq driver for CPU0 frequency management.
+	  This adds a generic DT based cpufreq driver for frequency management.
 	  It supports both uniprocessor (UP) and symmetric multiprocessor (SMP)
 	  systems which share clock and voltage across all CPUs.
 
diff --git a/drivers/cpufreq/Kconfig.arm b/drivers/cpufreq/Kconfig.arm
index 7364a538e056..83a75dc84761 100644
--- a/drivers/cpufreq/Kconfig.arm
+++ b/drivers/cpufreq/Kconfig.arm
@@ -77,7 +77,7 @@ config ARM_EXYNOS5440_CPUFREQ
 
 config ARM_EXYNOS_CPU_FREQ_BOOST_SW
 	bool "EXYNOS Frequency Overclocking - Software"
-	depends on ARM_EXYNOS_CPUFREQ
+	depends on ARM_EXYNOS_CPUFREQ && THERMAL
 	select CPU_FREQ_BOOST_SW
 	select EXYNOS_THERMAL
 	help
@@ -92,7 +92,7 @@ config ARM_EXYNOS_CPU_FREQ_BOOST_SW
 
 config ARM_HIGHBANK_CPUFREQ
 	tristate "Calxeda Highbank-based"
-	depends on ARCH_HIGHBANK && GENERIC_CPUFREQ_CPU0 && REGULATOR
+	depends on ARCH_HIGHBANK && CPUFREQ_DT && REGULATOR
 	default m
 	help
 	  This adds the CPUFreq driver for Calxeda Highbank SoC
@@ -119,7 +119,7 @@ config ARM_INTEGRATOR
 	  If in doubt, say Y.
 
 config ARM_KIRKWOOD_CPUFREQ
-	def_bool ARCH_KIRKWOOD || MACH_KIRKWOOD
+	def_bool MACH_KIRKWOOD
 	help
 	  This adds the CPUFreq driver for Marvell Kirkwood
 	  SoCs.
diff --git a/drivers/cpufreq/Makefile b/drivers/cpufreq/Makefile
index db6d9a2fea4d..40c53dc1937e 100644
--- a/drivers/cpufreq/Makefile
+++ b/drivers/cpufreq/Makefile
@@ -13,7 +13,7 @@ obj-$(CONFIG_CPU_FREQ_GOV_ONDEMAND)	+= cpufreq_ondemand.o
 obj-$(CONFIG_CPU_FREQ_GOV_CONSERVATIVE)	+= cpufreq_conservative.o
 obj-$(CONFIG_CPU_FREQ_GOV_COMMON)		+= cpufreq_governor.o
 
-obj-$(CONFIG_GENERIC_CPUFREQ_CPU0)	+= cpufreq-cpu0.o
+obj-$(CONFIG_CPUFREQ_DT)		+= cpufreq-dt.o
 
 ##################################################################################
 # x86 drivers.
diff --git a/drivers/cpufreq/cpufreq-cpu0.c b/drivers/cpufreq/cpufreq-cpu0.c
deleted file mode 100644
index 0d2172b07765..000000000000
--- a/drivers/cpufreq/cpufreq-cpu0.c
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- * Copyright (C) 2012 Freescale Semiconductor, Inc.
- *
- * The OPP code in function cpu0_set_target() is reused from
- * drivers/cpufreq/omap-cpufreq.c
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
-
-#include <linux/clk.h>
-#include <linux/cpu.h>
-#include <linux/cpu_cooling.h>
-#include <linux/cpufreq.h>
-#include <linux/cpumask.h>
-#include <linux/err.h>
-#include <linux/module.h>
-#include <linux/of.h>
-#include <linux/pm_opp.h>
-#include <linux/platform_device.h>
-#include <linux/regulator/consumer.h>
-#include <linux/slab.h>
-#include <linux/thermal.h>
-
-static unsigned int transition_latency;
-static unsigned int voltage_tolerance; /* in percentage */
-
-static struct device *cpu_dev;
-static struct clk *cpu_clk;
-static struct regulator *cpu_reg;
-static struct cpufreq_frequency_table *freq_table;
-static struct thermal_cooling_device *cdev;
-
-static int cpu0_set_target(struct cpufreq_policy *policy, unsigned int index)
-{
-	struct dev_pm_opp *opp;
-	unsigned long volt = 0, volt_old = 0, tol = 0;
-	unsigned int old_freq, new_freq;
-	long freq_Hz, freq_exact;
-	int ret;
-
-	freq_Hz = clk_round_rate(cpu_clk, freq_table[index].frequency * 1000);
-	if (freq_Hz <= 0)
-		freq_Hz = freq_table[index].frequency * 1000;
-
-	freq_exact = freq_Hz;
-	new_freq = freq_Hz / 1000;
-	old_freq = clk_get_rate(cpu_clk) / 1000;
-
-	if (!IS_ERR(cpu_reg)) {
-		rcu_read_lock();
-		opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_Hz);
-		if (IS_ERR(opp)) {
-			rcu_read_unlock();
-			pr_err("failed to find OPP for %ld\n", freq_Hz);
-			return PTR_ERR(opp);
-		}
-		volt = dev_pm_opp_get_voltage(opp);
-		rcu_read_unlock();
-		tol = volt * voltage_tolerance / 100;
-		volt_old = regulator_get_voltage(cpu_reg);
-	}
-
-	pr_debug("%u MHz, %ld mV --> %u MHz, %ld mV\n",
-		 old_freq / 1000, volt_old ? volt_old / 1000 : -1,
-		 new_freq / 1000, volt ? volt / 1000 : -1);
-
-	/* scaling up?  scale voltage before frequency */
-	if (!IS_ERR(cpu_reg) && new_freq > old_freq) {
-		ret = regulator_set_voltage_tol(cpu_reg, volt, tol);
-		if (ret) {
-			pr_err("failed to scale voltage up: %d\n", ret);
-			return ret;
-		}
-	}
-
-	ret = clk_set_rate(cpu_clk, freq_exact);
-	if (ret) {
-		pr_err("failed to set clock rate: %d\n", ret);
-		if (!IS_ERR(cpu_reg))
-			regulator_set_voltage_tol(cpu_reg, volt_old, tol);
-		return ret;
-	}
-
-	/* scaling down?  scale voltage after frequency */
-	if (!IS_ERR(cpu_reg) && new_freq < old_freq) {
-		ret = regulator_set_voltage_tol(cpu_reg, volt, tol);
-		if (ret) {
-			pr_err("failed to scale voltage down: %d\n", ret);
-			clk_set_rate(cpu_clk, old_freq * 1000);
-		}
-	}
-
-	return ret;
-}
-
-static int cpu0_cpufreq_init(struct cpufreq_policy *policy)
-{
-	policy->clk = cpu_clk;
-	return cpufreq_generic_init(policy, freq_table, transition_latency);
-}
-
-static struct cpufreq_driver cpu0_cpufreq_driver = {
-	.flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK,
-	.verify = cpufreq_generic_frequency_table_verify,
-	.target_index = cpu0_set_target,
-	.get = cpufreq_generic_get,
-	.init = cpu0_cpufreq_init,
-	.name = "generic_cpu0",
-	.attr = cpufreq_generic_attr,
-};
-
-static int cpu0_cpufreq_probe(struct platform_device *pdev)
-{
-	struct device_node *np;
-	int ret;
-
-	cpu_dev = get_cpu_device(0);
-	if (!cpu_dev) {
-		pr_err("failed to get cpu0 device\n");
-		return -ENODEV;
-	}
-
-	np = of_node_get(cpu_dev->of_node);
-	if (!np) {
-		pr_err("failed to find cpu0 node\n");
-		return -ENOENT;
-	}
-
-	cpu_reg = regulator_get_optional(cpu_dev, "cpu0");
-	if (IS_ERR(cpu_reg)) {
-		/*
-		 * If cpu0 regulator supply node is present, but regulator is
-		 * not yet registered, we should try defering probe.
-		 */
-		if (PTR_ERR(cpu_reg) == -EPROBE_DEFER) {
-			dev_dbg(cpu_dev, "cpu0 regulator not ready, retry\n");
-			ret = -EPROBE_DEFER;
-			goto out_put_node;
-		}
-		pr_warn("failed to get cpu0 regulator: %ld\n",
-			PTR_ERR(cpu_reg));
-	}
-
-	cpu_clk = clk_get(cpu_dev, NULL);
-	if (IS_ERR(cpu_clk)) {
-		ret = PTR_ERR(cpu_clk);
-		pr_err("failed to get cpu0 clock: %d\n", ret);
-		goto out_put_reg;
-	}
-
-	/* OPPs might be populated at runtime, don't check for error here */
-	of_init_opp_table(cpu_dev);
-
-	ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table);
-	if (ret) {
-		pr_err("failed to init cpufreq table: %d\n", ret);
-		goto out_put_clk;
-	}
-
-	of_property_read_u32(np, "voltage-tolerance", &voltage_tolerance);
-
-	if (of_property_read_u32(np, "clock-latency", &transition_latency))
-		transition_latency = CPUFREQ_ETERNAL;
-
-	if (!IS_ERR(cpu_reg)) {
-		struct dev_pm_opp *opp;
-		unsigned long min_uV, max_uV;
-		int i;
-
-		/*
-		 * OPP is maintained in order of increasing frequency, and
-		 * freq_table initialised from OPP is therefore sorted in the
-		 * same order.
-		 */
-		for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
-			;
-		rcu_read_lock();
-		opp = dev_pm_opp_find_freq_exact(cpu_dev,
-				freq_table[0].frequency * 1000, true);
-		min_uV = dev_pm_opp_get_voltage(opp);
-		opp = dev_pm_opp_find_freq_exact(cpu_dev,
-				freq_table[i-1].frequency * 1000, true);
-		max_uV = dev_pm_opp_get_voltage(opp);
-		rcu_read_unlock();
-		ret = regulator_set_voltage_time(cpu_reg, min_uV, max_uV);
-		if (ret > 0)
-			transition_latency += ret * 1000;
-	}
-
-	ret = cpufreq_register_driver(&cpu0_cpufreq_driver);
-	if (ret) {
-		pr_err("failed register driver: %d\n", ret);
-		goto out_free_table;
-	}
-
-	/*
-	 * For now, just loading the cooling device;
-	 * thermal DT code takes care of matching them.
-	 */
-	if (of_find_property(np, "#cooling-cells", NULL)) {
-		cdev = of_cpufreq_cooling_register(np, cpu_present_mask);
-		if (IS_ERR(cdev))
-			pr_err("running cpufreq without cooling device: %ld\n",
-			       PTR_ERR(cdev));
-	}
-
-	of_node_put(np);
-	return 0;
-
-out_free_table:
-	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
-out_put_clk:
-	if (!IS_ERR(cpu_clk))
-		clk_put(cpu_clk);
-out_put_reg:
-	if (!IS_ERR(cpu_reg))
-		regulator_put(cpu_reg);
-out_put_node:
-	of_node_put(np);
-	return ret;
-}
-
-static int cpu0_cpufreq_remove(struct platform_device *pdev)
-{
-	cpufreq_cooling_unregister(cdev);
-	cpufreq_unregister_driver(&cpu0_cpufreq_driver);
-	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
-
-	return 0;
-}
-
-static struct platform_driver cpu0_cpufreq_platdrv = {
-	.driver = {
-		.name	= "cpufreq-cpu0",
-		.owner	= THIS_MODULE,
-	},
-	.probe		= cpu0_cpufreq_probe,
-	.remove		= cpu0_cpufreq_remove,
-};
-module_platform_driver(cpu0_cpufreq_platdrv);
-
-MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
-MODULE_DESCRIPTION("Generic CPU0 cpufreq driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/cpufreq-dt.c b/drivers/cpufreq/cpufreq-dt.c
new file mode 100644
index 000000000000..92c162af5045
--- /dev/null
+++ b/drivers/cpufreq/cpufreq-dt.c
@@ -0,0 +1,377 @@
+/*
+ * Copyright (C) 2012 Freescale Semiconductor, Inc.
+ *
+ * Copyright (C) 2014 Linaro.
+ * Viresh Kumar <viresh.kumar@linaro.org>
+ *
+ * The OPP code in function set_target() is reused from
+ * drivers/cpufreq/omap-cpufreq.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/clk.h>
+#include <linux/cpu.h>
+#include <linux/cpu_cooling.h>
+#include <linux/cpufreq.h>
+#include <linux/cpufreq-dt.h>
+#include <linux/cpumask.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/pm_opp.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/thermal.h>
+
+struct private_data {
+	struct device *cpu_dev;
+	struct regulator *cpu_reg;
+	struct thermal_cooling_device *cdev;
+	unsigned int voltage_tolerance; /* in percentage */
+};
+
+static int set_target(struct cpufreq_policy *policy, unsigned int index)
+{
+	struct dev_pm_opp *opp;
+	struct cpufreq_frequency_table *freq_table = policy->freq_table;
+	struct clk *cpu_clk = policy->clk;
+	struct private_data *priv = policy->driver_data;
+	struct device *cpu_dev = priv->cpu_dev;
+	struct regulator *cpu_reg = priv->cpu_reg;
+	unsigned long volt = 0, volt_old = 0, tol = 0;
+	unsigned int old_freq, new_freq;
+	long freq_Hz, freq_exact;
+	int ret;
+
+	freq_Hz = clk_round_rate(cpu_clk, freq_table[index].frequency * 1000);
+	if (freq_Hz <= 0)
+		freq_Hz = freq_table[index].frequency * 1000;
+
+	freq_exact = freq_Hz;
+	new_freq = freq_Hz / 1000;
+	old_freq = clk_get_rate(cpu_clk) / 1000;
+
+	if (!IS_ERR(cpu_reg)) {
+		rcu_read_lock();
+		opp = dev_pm_opp_find_freq_ceil(cpu_dev, &freq_Hz);
+		if (IS_ERR(opp)) {
+			rcu_read_unlock();
+			dev_err(cpu_dev, "failed to find OPP for %ld\n",
+				freq_Hz);
+			return PTR_ERR(opp);
+		}
+		volt = dev_pm_opp_get_voltage(opp);
+		rcu_read_unlock();
+		tol = volt * priv->voltage_tolerance / 100;
+		volt_old = regulator_get_voltage(cpu_reg);
+	}
+
+	dev_dbg(cpu_dev, "%u MHz, %ld mV --> %u MHz, %ld mV\n",
+		old_freq / 1000, volt_old ? volt_old / 1000 : -1,
+		new_freq / 1000, volt ? volt / 1000 : -1);
+
+	/* scaling up?  scale voltage before frequency */
+	if (!IS_ERR(cpu_reg) && new_freq > old_freq) {
+		ret = regulator_set_voltage_tol(cpu_reg, volt, tol);
+		if (ret) {
+			dev_err(cpu_dev, "failed to scale voltage up: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	ret = clk_set_rate(cpu_clk, freq_exact);
+	if (ret) {
+		dev_err(cpu_dev, "failed to set clock rate: %d\n", ret);
+		if (!IS_ERR(cpu_reg))
+			regulator_set_voltage_tol(cpu_reg, volt_old, tol);
+		return ret;
+	}
+
+	/* scaling down?  scale voltage after frequency */
+	if (!IS_ERR(cpu_reg) && new_freq < old_freq) {
+		ret = regulator_set_voltage_tol(cpu_reg, volt, tol);
+		if (ret) {
+			dev_err(cpu_dev, "failed to scale voltage down: %d\n",
+				ret);
+			clk_set_rate(cpu_clk, old_freq * 1000);
+		}
+	}
+
+	return ret;
+}
+
+static int allocate_resources(int cpu, struct device **cdev,
+			      struct regulator **creg, struct clk **cclk)
+{
+	struct device *cpu_dev;
+	struct regulator *cpu_reg;
+	struct clk *cpu_clk;
+	int ret = 0;
+	char *reg_cpu0 = "cpu0", *reg_cpu = "cpu", *reg;
+
+	cpu_dev = get_cpu_device(cpu);
+	if (!cpu_dev) {
+		pr_err("failed to get cpu%d device\n", cpu);
+		return -ENODEV;
+	}
+
+	/* Try "cpu0" for older DTs */
+	if (!cpu)
+		reg = reg_cpu0;
+	else
+		reg = reg_cpu;
+
+try_again:
+	cpu_reg = regulator_get_optional(cpu_dev, reg);
+	if (IS_ERR(cpu_reg)) {
+		/*
+		 * If cpu's regulator supply node is present, but regulator is
+		 * not yet registered, we should try defering probe.
+		 */
+		if (PTR_ERR(cpu_reg) == -EPROBE_DEFER) {
+			dev_dbg(cpu_dev, "cpu%d regulator not ready, retry\n",
+				cpu);
+			return -EPROBE_DEFER;
+		}
+
+		/* Try with "cpu-supply" */
+		if (reg == reg_cpu0) {
+			reg = reg_cpu;
+			goto try_again;
+		}
+
+		dev_dbg(cpu_dev, "no regulator for cpu%d: %ld\n",
+			cpu, PTR_ERR(cpu_reg));
+	}
+
+	cpu_clk = clk_get(cpu_dev, NULL);
+	if (IS_ERR(cpu_clk)) {
+		/* put regulator */
+		if (!IS_ERR(cpu_reg))
+			regulator_put(cpu_reg);
+
+		ret = PTR_ERR(cpu_clk);
+
+		/*
+		 * If cpu's clk node is present, but clock is not yet
+		 * registered, we should try defering probe.
+		 */
+		if (ret == -EPROBE_DEFER)
+			dev_dbg(cpu_dev, "cpu%d clock not ready, retry\n", cpu);
+		else
+			dev_err(cpu_dev, "failed to get cpu%d clock: %d\n", ret,
+				cpu);
+	} else {
+		*cdev = cpu_dev;
+		*creg = cpu_reg;
+		*cclk = cpu_clk;
+	}
+
+	return ret;
+}
+
+static int cpufreq_init(struct cpufreq_policy *policy)
+{
+	struct cpufreq_dt_platform_data *pd;
+	struct cpufreq_frequency_table *freq_table;
+	struct thermal_cooling_device *cdev;
+	struct device_node *np;
+	struct private_data *priv;
+	struct device *cpu_dev;
+	struct regulator *cpu_reg;
+	struct clk *cpu_clk;
+	unsigned int transition_latency;
+	int ret;
+
+	ret = allocate_resources(policy->cpu, &cpu_dev, &cpu_reg, &cpu_clk);
+	if (ret) {
+		pr_err("%s: Failed to allocate resources\n: %d", __func__, ret);
+		return ret;
+	}
+
+	np = of_node_get(cpu_dev->of_node);
+	if (!np) {
+		dev_err(cpu_dev, "failed to find cpu%d node\n", policy->cpu);
+		ret = -ENOENT;
+		goto out_put_reg_clk;
+	}
+
+	/* OPPs might be populated at runtime, don't check for error here */
+	of_init_opp_table(cpu_dev);
+
+	ret = dev_pm_opp_init_cpufreq_table(cpu_dev, &freq_table);
+	if (ret) {
+		dev_err(cpu_dev, "failed to init cpufreq table: %d\n", ret);
+		goto out_put_node;
+	}
+
+	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
+	if (!priv) {
+		ret = -ENOMEM;
+		goto out_free_table;
+	}
+
+	of_property_read_u32(np, "voltage-tolerance", &priv->voltage_tolerance);
+
+	if (of_property_read_u32(np, "clock-latency", &transition_latency))
+		transition_latency = CPUFREQ_ETERNAL;
+
+	if (!IS_ERR(cpu_reg)) {
+		struct dev_pm_opp *opp;
+		unsigned long min_uV, max_uV;
+		int i;
+
+		/*
+		 * OPP is maintained in order of increasing frequency, and
+		 * freq_table initialised from OPP is therefore sorted in the
+		 * same order.
+		 */
+		for (i = 0; freq_table[i].frequency != CPUFREQ_TABLE_END; i++)
+			;
+		rcu_read_lock();
+		opp = dev_pm_opp_find_freq_exact(cpu_dev,
+				freq_table[0].frequency * 1000, true);
+		min_uV = dev_pm_opp_get_voltage(opp);
+		opp = dev_pm_opp_find_freq_exact(cpu_dev,
+				freq_table[i-1].frequency * 1000, true);
+		max_uV = dev_pm_opp_get_voltage(opp);
+		rcu_read_unlock();
+		ret = regulator_set_voltage_time(cpu_reg, min_uV, max_uV);
+		if (ret > 0)
+			transition_latency += ret * 1000;
+	}
+
+	/*
+	 * For now, just loading the cooling device;
+	 * thermal DT code takes care of matching them.
+	 */
+	if (of_find_property(np, "#cooling-cells", NULL)) {
+		cdev = of_cpufreq_cooling_register(np, cpu_present_mask);
+		if (IS_ERR(cdev))
+			dev_err(cpu_dev,
+				"running cpufreq without cooling device: %ld\n",
+				PTR_ERR(cdev));
+		else
+			priv->cdev = cdev;
+	}
+
+	priv->cpu_dev = cpu_dev;
+	priv->cpu_reg = cpu_reg;
+	policy->driver_data = priv;
+
+	policy->clk = cpu_clk;
+	ret = cpufreq_table_validate_and_show(policy, freq_table);
+	if (ret) {
+		dev_err(cpu_dev, "%s: invalid frequency table: %d\n", __func__,
+			ret);
+		goto out_cooling_unregister;
+	}
+
+	policy->cpuinfo.transition_latency = transition_latency;
+
+	pd = cpufreq_get_driver_data();
+	if (pd && !pd->independent_clocks)
+		cpumask_setall(policy->cpus);
+
+	of_node_put(np);
+
+	return 0;
+
+out_cooling_unregister:
+	cpufreq_cooling_unregister(priv->cdev);
+	kfree(priv);
+out_free_table:
+	dev_pm_opp_free_cpufreq_table(cpu_dev, &freq_table);
+out_put_node:
+	of_node_put(np);
+out_put_reg_clk:
+	clk_put(cpu_clk);
+	if (!IS_ERR(cpu_reg))
+		regulator_put(cpu_reg);
+
+	return ret;
+}
+
+static int cpufreq_exit(struct cpufreq_policy *policy)
+{
+	struct private_data *priv = policy->driver_data;
+
+	cpufreq_cooling_unregister(priv->cdev);
+	dev_pm_opp_free_cpufreq_table(priv->cpu_dev, &policy->freq_table);
+	clk_put(policy->clk);
+	if (!IS_ERR(priv->cpu_reg))
+		regulator_put(priv->cpu_reg);
+	kfree(priv);
+
+	return 0;
+}
+
+static struct cpufreq_driver dt_cpufreq_driver = {
+	.flags = CPUFREQ_STICKY | CPUFREQ_NEED_INITIAL_FREQ_CHECK,
+	.verify = cpufreq_generic_frequency_table_verify,
+	.target_index = set_target,
+	.get = cpufreq_generic_get,
+	.init = cpufreq_init,
+	.exit = cpufreq_exit,
+	.name = "cpufreq-dt",
+	.attr = cpufreq_generic_attr,
+};
+
+static int dt_cpufreq_probe(struct platform_device *pdev)
+{
+	struct device *cpu_dev;
+	struct regulator *cpu_reg;
+	struct clk *cpu_clk;
+	int ret;
+
+	/*
+	 * All per-cluster (CPUs sharing clock/voltages) initialization is done
+	 * from ->init(). In probe(), we just need to make sure that clk and
+	 * regulators are available. Else defer probe and retry.
+	 *
+	 * FIXME: Is checking this only for CPU0 sufficient ?
+	 */
+	ret = allocate_resources(0, &cpu_dev, &cpu_reg, &cpu_clk);
+	if (ret)
+		return ret;
+
+	clk_put(cpu_clk);
+	if (!IS_ERR(cpu_reg))
+		regulator_put(cpu_reg);
+
+	dt_cpufreq_driver.driver_data = dev_get_platdata(&pdev->dev);
+
+	ret = cpufreq_register_driver(&dt_cpufreq_driver);
+	if (ret)
+		dev_err(cpu_dev, "failed register driver: %d\n", ret);
+
+	return ret;
+}
+
+static int dt_cpufreq_remove(struct platform_device *pdev)
+{
+	cpufreq_unregister_driver(&dt_cpufreq_driver);
+	return 0;
+}
+
+static struct platform_driver dt_cpufreq_platdrv = {
+	.driver = {
+		.name	= "cpufreq-dt",
+		.owner	= THIS_MODULE,
+	},
+	.probe		= dt_cpufreq_probe,
+	.remove		= dt_cpufreq_remove,
+};
+module_platform_driver(dt_cpufreq_platdrv);
+
+MODULE_AUTHOR("Viresh Kumar <viresh.kumar@linaro.org>");
+MODULE_AUTHOR("Shawn Guo <shawn.guo@linaro.org>");
+MODULE_DESCRIPTION("Generic cpufreq driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/cpufreq/cpufreq.c b/drivers/cpufreq/cpufreq.c
index d9fdeddcef96..644b54e1e7d1 100644
--- a/drivers/cpufreq/cpufreq.c
+++ b/drivers/cpufreq/cpufreq.c
@@ -437,7 +437,7 @@ static struct cpufreq_governor *__find_governor(const char *str_governor)
 	struct cpufreq_governor *t;
 
 	list_for_each_entry(t, &cpufreq_governor_list, governor_list)
-		if (!strnicmp(str_governor, t->name, CPUFREQ_NAME_LEN))
+		if (!strncasecmp(str_governor, t->name, CPUFREQ_NAME_LEN))
 			return t;
 
 	return NULL;
@@ -455,10 +455,10 @@ static int cpufreq_parse_governor(char *str_governor, unsigned int *policy,
 		goto out;
 
 	if (cpufreq_driver->setpolicy) {
-		if (!strnicmp(str_governor, "performance", CPUFREQ_NAME_LEN)) {
+		if (!strncasecmp(str_governor, "performance", CPUFREQ_NAME_LEN)) {
 			*policy = CPUFREQ_POLICY_PERFORMANCE;
 			err = 0;
-		} else if (!strnicmp(str_governor, "powersave",
+		} else if (!strncasecmp(str_governor, "powersave",
 						CPUFREQ_NAME_LEN)) {
 			*policy = CPUFREQ_POLICY_POWERSAVE;
 			err = 0;
@@ -512,7 +512,18 @@ show_one(cpuinfo_max_freq, cpuinfo.max_freq);
 show_one(cpuinfo_transition_latency, cpuinfo.transition_latency);
 show_one(scaling_min_freq, min);
 show_one(scaling_max_freq, max);
-show_one(scaling_cur_freq, cur);
+
+static ssize_t show_scaling_cur_freq(
+	struct cpufreq_policy *policy, char *buf)
+{
+	ssize_t ret;
+
+	if (cpufreq_driver && cpufreq_driver->setpolicy && cpufreq_driver->get)
+		ret = sprintf(buf, "%u\n", cpufreq_driver->get(policy->cpu));
+	else
+		ret = sprintf(buf, "%u\n", policy->cur);
+	return ret;
+}
 
 static int cpufreq_set_policy(struct cpufreq_policy *policy,
 				struct cpufreq_policy *new_policy);
@@ -906,11 +917,11 @@ static int cpufreq_add_dev_interface(struct cpufreq_policy *policy,
 		if (ret)
 			goto err_out_kobj_put;
 	}
-	if (has_target()) {
-		ret = sysfs_create_file(&policy->kobj, &scaling_cur_freq.attr);
-		if (ret)
-			goto err_out_kobj_put;
-	}
+
+	ret = sysfs_create_file(&policy->kobj, &scaling_cur_freq.attr);
+	if (ret)
+		goto err_out_kobj_put;
+
 	if (cpufreq_driver->bios_limit) {
 		ret = sysfs_create_file(&policy->kobj, &bios_limit.attr);
 		if (ret)
@@ -1289,6 +1300,8 @@ err_get_freq:
 		per_cpu(cpufreq_cpu_data, j) = NULL;
 	write_unlock_irqrestore(&cpufreq_driver_lock, flags);
 
+	up_write(&policy->rwsem);
+
 	if (cpufreq_driver->exit)
 		cpufreq_driver->exit(policy);
 err_set_policy_cpu:
@@ -1380,7 +1393,7 @@ static int __cpufreq_remove_dev_prepare(struct device *dev,
 		if (!cpufreq_suspended)
 			pr_debug("%s: policy Kobject moved to cpu: %d from: %d\n",
 				 __func__, new_cpu, cpu);
-	} else if (cpufreq_driver->stop_cpu && cpufreq_driver->setpolicy) {
+	} else if (cpufreq_driver->stop_cpu) {
 		cpufreq_driver->stop_cpu(policy);
 	}
 
@@ -1657,7 +1670,7 @@ void cpufreq_suspend(void)
 		return;
 
 	if (!has_target())
-		return;
+		goto suspend;
 
 	pr_debug("%s: Suspending Governors\n", __func__);
 
@@ -1671,6 +1684,7 @@ void cpufreq_suspend(void)
 				policy);
 	}
 
+suspend:
 	cpufreq_suspended = true;
 }
 
@@ -1687,13 +1701,13 @@ void cpufreq_resume(void)
 	if (!cpufreq_driver)
 		return;
 
+	cpufreq_suspended = false;
+
 	if (!has_target())
 		return;
 
 	pr_debug("%s: Resuming Governors\n", __func__);
 
-	cpufreq_suspended = false;
-
 	list_for_each_entry(policy, &cpufreq_policy_list, policy_list) {
 		if (cpufreq_driver->resume && cpufreq_driver->resume(policy))
 			pr_err("%s: Failed to resume driver: %p\n", __func__,
@@ -1728,6 +1742,21 @@ const char *cpufreq_get_current_driver(void)
 }
 EXPORT_SYMBOL_GPL(cpufreq_get_current_driver);
 
+/**
+ *	cpufreq_get_driver_data - return current driver data
+ *
+ *	Return the private data of the currently loaded cpufreq
+ *	driver, or NULL if no cpufreq driver is loaded.
+ */
+void *cpufreq_get_driver_data(void)
+{
+	if (cpufreq_driver)
+		return cpufreq_driver->driver_data;
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(cpufreq_get_driver_data);
+
 /*********************************************************************
  *                     NOTIFIER LISTS INTERFACE                      *
  *********************************************************************/
diff --git a/drivers/cpufreq/cpufreq_opp.c b/drivers/cpufreq/cpufreq_opp.c
index f7a32d2326c6..773bcde893c0 100644
--- a/drivers/cpufreq/cpufreq_opp.c
+++ b/drivers/cpufreq/cpufreq_opp.c
@@ -60,7 +60,7 @@ int dev_pm_opp_init_cpufreq_table(struct device *dev,
 		goto out;
 	}
 
-	freq_table = kcalloc(sizeof(*freq_table), (max_opps + 1), GFP_ATOMIC);
+	freq_table = kcalloc((max_opps + 1), sizeof(*freq_table), GFP_ATOMIC);
 	if (!freq_table) {
 		ret = -ENOMEM;
 		goto out;
diff --git a/drivers/cpufreq/exynos4210-cpufreq.c b/drivers/cpufreq/exynos4210-cpufreq.c
index 61a54310a1b9..843ec824fd91 100644
--- a/drivers/cpufreq/exynos4210-cpufreq.c
+++ b/drivers/cpufreq/exynos4210-cpufreq.c
@@ -127,7 +127,7 @@ int exynos4210_cpufreq_init(struct exynos_dvfs_info *info)
 	 * dependencies on platform headers. It is necessary to enable
 	 * Exynos multi-platform support and will be removed together with
 	 * this whole driver as soon as Exynos gets migrated to use
-	 * cpufreq-cpu0 driver.
+	 * cpufreq-dt driver.
 	 */
 	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4210-clock");
 	if (!np) {
diff --git a/drivers/cpufreq/exynos4x12-cpufreq.c b/drivers/cpufreq/exynos4x12-cpufreq.c
index 351a2074cfea..9e78a850e29f 100644
--- a/drivers/cpufreq/exynos4x12-cpufreq.c
+++ b/drivers/cpufreq/exynos4x12-cpufreq.c
@@ -174,7 +174,7 @@ int exynos4x12_cpufreq_init(struct exynos_dvfs_info *info)
 	 * dependencies on platform headers. It is necessary to enable
 	 * Exynos multi-platform support and will be removed together with
 	 * this whole driver as soon as Exynos gets migrated to use
-	 * cpufreq-cpu0 driver.
+	 * cpufreq-dt driver.
 	 */
 	np = of_find_compatible_node(NULL, NULL, "samsung,exynos4412-clock");
 	if (!np) {
diff --git a/drivers/cpufreq/exynos5250-cpufreq.c b/drivers/cpufreq/exynos5250-cpufreq.c
index c91ce69dc631..3eafdc7ba787 100644
--- a/drivers/cpufreq/exynos5250-cpufreq.c
+++ b/drivers/cpufreq/exynos5250-cpufreq.c
@@ -153,7 +153,7 @@ int exynos5250_cpufreq_init(struct exynos_dvfs_info *info)
 	 * dependencies on platform headers. It is necessary to enable
 	 * Exynos multi-platform support and will be removed together with
 	 * this whole driver as soon as Exynos gets migrated to use
-	 * cpufreq-cpu0 driver.
+	 * cpufreq-dt driver.
 	 */
 	np = of_find_compatible_node(NULL, NULL, "samsung,exynos5250-clock");
 	if (!np) {
diff --git a/drivers/cpufreq/highbank-cpufreq.c b/drivers/cpufreq/highbank-cpufreq.c
index bf8902a0866d..1608f7105c9f 100644
--- a/drivers/cpufreq/highbank-cpufreq.c
+++ b/drivers/cpufreq/highbank-cpufreq.c
@@ -6,7 +6,7 @@
  * published by the Free Software Foundation.
  *
  * This driver provides the clk notifier callbacks that are used when
- * the cpufreq-cpu0 driver changes to frequency to alert the highbank
+ * the cpufreq-dt driver changes to frequency to alert the highbank
  * EnergyCore Management Engine (ECME) about the need to change
  * voltage. The ECME interfaces with the actual voltage regulators.
  */
@@ -19,7 +19,7 @@
 #include <linux/cpu.h>
 #include <linux/err.h>
 #include <linux/of.h>
-#include <linux/mailbox.h>
+#include <linux/pl320-ipc.h>
 #include <linux/platform_device.h>
 
 #define HB_CPUFREQ_CHANGE_NOTE	0x80000001
@@ -60,7 +60,7 @@ static struct notifier_block hb_cpufreq_clk_nb = {
 
 static int hb_cpufreq_driver_init(void)
 {
-	struct platform_device_info devinfo = { .name = "cpufreq-cpu0", };
+	struct platform_device_info devinfo = { .name = "cpufreq-dt", };
 	struct device *cpu_dev;
 	struct clk *cpu_clk;
 	struct device_node *np;
@@ -95,7 +95,7 @@ static int hb_cpufreq_driver_init(void)
 		goto out_put_node;
 	}
 
-	/* Instantiate cpufreq-cpu0 */
+	/* Instantiate cpufreq-dt */
 	platform_device_register_full(&devinfo);
 
 out_put_node:
diff --git a/drivers/cpufreq/integrator-cpufreq.c b/drivers/cpufreq/integrator-cpufreq.c
index c1320528b9d0..6bd69adc3c5e 100644
--- a/drivers/cpufreq/integrator-cpufreq.c
+++ b/drivers/cpufreq/integrator-cpufreq.c
@@ -213,9 +213,9 @@ static int __init integrator_cpufreq_probe(struct platform_device *pdev)
 	return cpufreq_register_driver(&integrator_driver);
 }
 
-static void __exit integrator_cpufreq_remove(struct platform_device *pdev)
+static int __exit integrator_cpufreq_remove(struct platform_device *pdev)
 {
-	cpufreq_unregister_driver(&integrator_driver);
+	return cpufreq_unregister_driver(&integrator_driver);
 }
 
 static const struct of_device_id integrator_cpufreq_match[] = {
diff --git a/drivers/cpufreq/intel_pstate.c b/drivers/cpufreq/intel_pstate.c
index c5eac949760d..27bb6d3877ed 100644
--- a/drivers/cpufreq/intel_pstate.c
+++ b/drivers/cpufreq/intel_pstate.c
@@ -52,6 +52,17 @@ static inline int32_t div_fp(int32_t x, int32_t y)
 	return div_s64((int64_t)x << FRAC_BITS, y);
 }
 
+static inline int ceiling_fp(int32_t x)
+{
+	int mask, ret;
+
+	ret = fp_toint(x);
+	mask = (1 << FRAC_BITS) - 1;
+	if (x & mask)
+		ret += 1;
+	return ret;
+}
+
 struct sample {
 	int32_t core_pct_busy;
 	u64 aperf;
@@ -64,6 +75,7 @@ struct pstate_data {
 	int	current_pstate;
 	int	min_pstate;
 	int	max_pstate;
+	int	scaling;
 	int	turbo_pstate;
 };
 
@@ -113,6 +125,7 @@ struct pstate_funcs {
 	int (*get_max)(void);
 	int (*get_min)(void);
 	int (*get_turbo)(void);
+	int (*get_scaling)(void);
 	void (*set)(struct cpudata*, int pstate);
 	void (*get_vid)(struct cpudata *);
 };
@@ -138,6 +151,7 @@ struct perf_limits {
 
 static struct perf_limits limits = {
 	.no_turbo = 0,
+	.turbo_disabled = 0,
 	.max_perf_pct = 100,
 	.max_perf = int_tofp(1),
 	.min_perf_pct = 0,
@@ -218,6 +232,18 @@ static inline void intel_pstate_reset_all_pid(void)
 	}
 }
 
+static inline void update_turbo_state(void)
+{
+	u64 misc_en;
+	struct cpudata *cpu;
+
+	cpu = all_cpu_data[0];
+	rdmsrl(MSR_IA32_MISC_ENABLE, misc_en);
+	limits.turbo_disabled =
+		(misc_en & MSR_IA32_MISC_ENABLE_TURBO_DISABLE ||
+		 cpu->pstate.max_pstate == cpu->pstate.turbo_pstate);
+}
+
 /************************** debugfs begin ************************/
 static int pid_param_set(void *data, u64 val)
 {
@@ -274,6 +300,20 @@ static void __init intel_pstate_debug_expose_params(void)
 		return sprintf(buf, "%u\n", limits.object);		\
 	}
 
+static ssize_t show_no_turbo(struct kobject *kobj,
+			     struct attribute *attr, char *buf)
+{
+	ssize_t ret;
+
+	update_turbo_state();
+	if (limits.turbo_disabled)
+		ret = sprintf(buf, "%u\n", limits.turbo_disabled);
+	else
+		ret = sprintf(buf, "%u\n", limits.no_turbo);
+
+	return ret;
+}
+
 static ssize_t store_no_turbo(struct kobject *a, struct attribute *b,
 			      const char *buf, size_t count)
 {
@@ -283,11 +323,14 @@ static ssize_t store_no_turbo(struct kobject *a, struct attribute *b,
 	ret = sscanf(buf, "%u", &input);
 	if (ret != 1)
 		return -EINVAL;
-	limits.no_turbo = clamp_t(int, input, 0 , 1);
+
+	update_turbo_state();
 	if (limits.turbo_disabled) {
 		pr_warn("Turbo disabled by BIOS or unavailable on processor\n");
-		limits.no_turbo = limits.turbo_disabled;
+		return -EPERM;
 	}
+	limits.no_turbo = clamp_t(int, input, 0, 1);
+
 	return count;
 }
 
@@ -323,7 +366,6 @@ static ssize_t store_min_perf_pct(struct kobject *a, struct attribute *b,
 	return count;
 }
 
-show_one(no_turbo, no_turbo);
 show_one(max_perf_pct, max_perf_pct);
 show_one(min_perf_pct, min_perf_pct);
 
@@ -394,7 +436,7 @@ static void byt_set_pstate(struct cpudata *cpudata, int pstate)
 		cpudata->vid.ratio);
 
 	vid_fp = clamp_t(int32_t, vid_fp, cpudata->vid.min, cpudata->vid.max);
-	vid = fp_toint(vid_fp);
+	vid = ceiling_fp(vid_fp);
 
 	if (pstate > cpudata->pstate.max_pstate)
 		vid = cpudata->vid.turbo;
@@ -404,6 +446,22 @@ static void byt_set_pstate(struct cpudata *cpudata, int pstate)
 	wrmsrl(MSR_IA32_PERF_CTL, val);
 }
 
+#define BYT_BCLK_FREQS 5
+static int byt_freq_table[BYT_BCLK_FREQS] = { 833, 1000, 1333, 1167, 800};
+
+static int byt_get_scaling(void)
+{
+	u64 value;
+	int i;
+
+	rdmsrl(MSR_FSB_FREQ, value);
+	i = value & 0x3;
+
+	BUG_ON(i > BYT_BCLK_FREQS);
+
+	return byt_freq_table[i] * 100;
+}
+
 static void byt_get_vid(struct cpudata *cpudata)
 {
 	u64 value;
@@ -449,6 +507,11 @@ static int core_get_turbo_pstate(void)
 	return ret;
 }
 
+static inline int core_get_scaling(void)
+{
+	return 100000;
+}
+
 static void core_set_pstate(struct cpudata *cpudata, int pstate)
 {
 	u64 val;
@@ -473,6 +536,7 @@ static struct cpu_defaults core_params = {
 		.get_max = core_get_max_pstate,
 		.get_min = core_get_min_pstate,
 		.get_turbo = core_get_turbo_pstate,
+		.get_scaling = core_get_scaling,
 		.set = core_set_pstate,
 	},
 };
@@ -491,6 +555,7 @@ static struct cpu_defaults byt_params = {
 		.get_min = byt_get_min_pstate,
 		.get_turbo = byt_get_turbo_pstate,
 		.set = byt_set_pstate,
+		.get_scaling = byt_get_scaling,
 		.get_vid = byt_get_vid,
 	},
 };
@@ -501,7 +566,7 @@ static void intel_pstate_get_min_max(struct cpudata *cpu, int *min, int *max)
 	int max_perf_adj;
 	int min_perf;
 
-	if (limits.no_turbo)
+	if (limits.no_turbo || limits.turbo_disabled)
 		max_perf = cpu->pstate.max_pstate;
 
 	max_perf_adj = fp_toint(mul_fp(int_tofp(max_perf), limits.max_perf));
@@ -516,6 +581,8 @@ static void intel_pstate_set_pstate(struct cpudata *cpu, int pstate)
 {
 	int max_perf, min_perf;
 
+	update_turbo_state();
+
 	intel_pstate_get_min_max(cpu, &min_perf, &max_perf);
 
 	pstate = clamp_t(int, pstate, min_perf, max_perf);
@@ -523,7 +590,7 @@ static void intel_pstate_set_pstate(struct cpudata *cpu, int pstate)
 	if (pstate == cpu->pstate.current_pstate)
 		return;
 
-	trace_cpu_frequency(pstate * 100000, cpu->cpu);
+	trace_cpu_frequency(pstate * cpu->pstate.scaling, cpu->cpu);
 
 	cpu->pstate.current_pstate = pstate;
 
@@ -535,6 +602,7 @@ static void intel_pstate_get_cpu_pstates(struct cpudata *cpu)
 	cpu->pstate.min_pstate = pstate_funcs.get_min();
 	cpu->pstate.max_pstate = pstate_funcs.get_max();
 	cpu->pstate.turbo_pstate = pstate_funcs.get_turbo();
+	cpu->pstate.scaling = pstate_funcs.get_scaling();
 
 	if (pstate_funcs.get_vid)
 		pstate_funcs.get_vid(cpu);
@@ -550,7 +618,9 @@ static inline void intel_pstate_calc_busy(struct cpudata *cpu)
 	core_pct = div64_u64(core_pct, int_tofp(sample->mperf));
 
 	sample->freq = fp_toint(
-		mul_fp(int_tofp(cpu->pstate.max_pstate * 1000), core_pct));
+		mul_fp(int_tofp(
+			cpu->pstate.max_pstate * cpu->pstate.scaling / 100),
+			core_pct));
 
 	sample->core_pct_busy = (int32_t)core_pct;
 }
@@ -660,6 +730,7 @@ static const struct x86_cpu_id intel_pstate_cpu_ids[] = {
 	ICPU(0x3f, core_params),
 	ICPU(0x45, core_params),
 	ICPU(0x46, core_params),
+	ICPU(0x4c, byt_params),
 	ICPU(0x4f, core_params),
 	ICPU(0x56, core_params),
 	{}
@@ -670,7 +741,9 @@ static int intel_pstate_init_cpu(unsigned int cpunum)
 {
 	struct cpudata *cpu;
 
-	all_cpu_data[cpunum] = kzalloc(sizeof(struct cpudata), GFP_KERNEL);
+	if (!all_cpu_data[cpunum])
+		all_cpu_data[cpunum] = kzalloc(sizeof(struct cpudata),
+					       GFP_KERNEL);
 	if (!all_cpu_data[cpunum])
 		return -ENOMEM;
 
@@ -688,7 +761,7 @@ static int intel_pstate_init_cpu(unsigned int cpunum)
 
 	add_timer_on(&cpu->timer, cpunum);
 
-	pr_info("Intel pstate controlling: cpu %d\n", cpunum);
+	pr_debug("Intel pstate controlling: cpu %d\n", cpunum);
 
 	return 0;
 }
@@ -707,19 +780,16 @@ static unsigned int intel_pstate_get(unsigned int cpu_num)
 
 static int intel_pstate_set_policy(struct cpufreq_policy *policy)
 {
-	struct cpudata *cpu;
-
-	cpu = all_cpu_data[policy->cpu];
-
 	if (!policy->cpuinfo.max_freq)
 		return -ENODEV;
 
 	if (policy->policy == CPUFREQ_POLICY_PERFORMANCE) {
 		limits.min_perf_pct = 100;
 		limits.min_perf = int_tofp(1);
+		limits.max_policy_pct = 100;
 		limits.max_perf_pct = 100;
 		limits.max_perf = int_tofp(1);
-		limits.no_turbo = limits.turbo_disabled;
+		limits.no_turbo = 0;
 		return 0;
 	}
 	limits.min_perf_pct = (policy->min * 100) / policy->cpuinfo.max_freq;
@@ -754,15 +824,12 @@ static void intel_pstate_stop_cpu(struct cpufreq_policy *policy)
 
 	del_timer_sync(&all_cpu_data[cpu_num]->timer);
 	intel_pstate_set_pstate(cpu, cpu->pstate.min_pstate);
-	kfree(all_cpu_data[cpu_num]);
-	all_cpu_data[cpu_num] = NULL;
 }
 
 static int intel_pstate_cpu_init(struct cpufreq_policy *policy)
 {
 	struct cpudata *cpu;
 	int rc;
-	u64 misc_en;
 
 	rc = intel_pstate_init_cpu(policy->cpu);
 	if (rc)
@@ -770,23 +837,18 @@ static int intel_pstate_cpu_init(struct cpufreq_policy *policy)
 
 	cpu = all_cpu_data[policy->cpu];
 
-	rdmsrl(MSR_IA32_MISC_ENABLE, misc_en);
-	if (misc_en & MSR_IA32_MISC_ENABLE_TURBO_DISABLE ||
-	    cpu->pstate.max_pstate == cpu->pstate.turbo_pstate) {
-		limits.turbo_disabled = 1;
-		limits.no_turbo = 1;
-	}
 	if (limits.min_perf_pct == 100 && limits.max_perf_pct == 100)
 		policy->policy = CPUFREQ_POLICY_PERFORMANCE;
 	else
 		policy->policy = CPUFREQ_POLICY_POWERSAVE;
 
-	policy->min = cpu->pstate.min_pstate * 100000;
-	policy->max = cpu->pstate.turbo_pstate * 100000;
+	policy->min = cpu->pstate.min_pstate * cpu->pstate.scaling;
+	policy->max = cpu->pstate.turbo_pstate * cpu->pstate.scaling;
 
 	/* cpuinfo and default policy values */
-	policy->cpuinfo.min_freq = cpu->pstate.min_pstate * 100000;
-	policy->cpuinfo.max_freq = cpu->pstate.turbo_pstate * 100000;
+	policy->cpuinfo.min_freq = cpu->pstate.min_pstate * cpu->pstate.scaling;
+	policy->cpuinfo.max_freq =
+		cpu->pstate.turbo_pstate * cpu->pstate.scaling;
 	policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
 	cpumask_set_cpu(policy->cpu, policy->cpus);
 
@@ -844,6 +906,7 @@ static void copy_cpu_funcs(struct pstate_funcs *funcs)
 	pstate_funcs.get_max   = funcs->get_max;
 	pstate_funcs.get_min   = funcs->get_min;
 	pstate_funcs.get_turbo = funcs->get_turbo;
+	pstate_funcs.get_scaling = funcs->get_scaling;
 	pstate_funcs.set       = funcs->set;
 	pstate_funcs.get_vid   = funcs->get_vid;
 }
diff --git a/drivers/cpufreq/kirkwood-cpufreq.c b/drivers/cpufreq/kirkwood-cpufreq.c
index 37a480680cd0..7906d4acfe40 100644
--- a/drivers/cpufreq/kirkwood-cpufreq.c
+++ b/drivers/cpufreq/kirkwood-cpufreq.c
@@ -12,7 +12,6 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/clk.h>
-#include <linux/clk-provider.h>
 #include <linux/cpufreq.h>
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
@@ -39,8 +38,7 @@ static struct priv
  * - cpu clk
  * - ddr clk
  *
- * The frequencies are set at runtime before registering this *
- * table.
+ * The frequencies are set at runtime before registering this table.
  */
 static struct cpufreq_frequency_table kirkwood_freq_table[] = {
 	{0, STATE_CPU_FREQ,	0}, /* CPU uses cpuclk */
@@ -50,9 +48,7 @@ static struct cpufreq_frequency_table kirkwood_freq_table[] = {
 
 static unsigned int kirkwood_cpufreq_get_cpu_frequency(unsigned int cpu)
 {
-	if (__clk_is_enabled(priv.powersave_clk))
-		return kirkwood_freq_table[1].frequency;
-	return kirkwood_freq_table[0].frequency;
+	return clk_get_rate(priv.powersave_clk) / 1000;
 }
 
 static int kirkwood_cpufreq_target(struct cpufreq_policy *policy,
@@ -70,10 +66,10 @@ static int kirkwood_cpufreq_target(struct cpufreq_policy *policy,
 
 	switch (state) {
 	case STATE_CPU_FREQ:
-		clk_disable(priv.powersave_clk);
+		clk_set_parent(priv.powersave_clk, priv.cpu_clk);
 		break;
 	case STATE_DDR_FREQ:
-		clk_enable(priv.powersave_clk);
+		clk_set_parent(priv.powersave_clk, priv.ddr_clk);
 		break;
 	}
 
@@ -150,7 +146,7 @@ static int kirkwood_cpufreq_probe(struct platform_device *pdev)
 		err = PTR_ERR(priv.powersave_clk);
 		goto out_ddr;
 	}
-	clk_prepare(priv.powersave_clk);
+	clk_prepare_enable(priv.powersave_clk);
 
 	of_node_put(np);
 	np = NULL;
diff --git a/drivers/cpufreq/pcc-cpufreq.c b/drivers/cpufreq/pcc-cpufreq.c
index 728a2d879499..4d2c8e861089 100644
--- a/drivers/cpufreq/pcc-cpufreq.c
+++ b/drivers/cpufreq/pcc-cpufreq.c
@@ -204,7 +204,6 @@ static int pcc_cpufreq_target(struct cpufreq_policy *policy,
 	u32 input_buffer;
 	int cpu;
 
-	spin_lock(&pcc_lock);
 	cpu = policy->cpu;
 	pcc_cpu_data = per_cpu_ptr(pcc_cpu_info, cpu);
 
@@ -216,6 +215,7 @@ static int pcc_cpufreq_target(struct cpufreq_policy *policy,
 	freqs.old = policy->cur;
 	freqs.new = target_freq;
 	cpufreq_freq_transition_begin(policy, &freqs);
+	spin_lock(&pcc_lock);
 
 	input_buffer = 0x1 | (((target_freq * 100)
 			       / (ioread32(&pcch_hdr->nominal) * 1000)) << 8);
diff --git a/drivers/cpufreq/pmac32-cpufreq.c b/drivers/cpufreq/pmac32-cpufreq.c
index 7615180d7ee3..1f49d97a70ea 100644
--- a/drivers/cpufreq/pmac32-cpufreq.c
+++ b/drivers/cpufreq/pmac32-cpufreq.c
@@ -611,7 +611,7 @@ static int __init pmac_cpufreq_setup(void)
 	struct device_node	*cpunode;
 	const u32		*value;
 
-	if (strstr(cmd_line, "nocpufreq"))
+	if (strstr(boot_command_line, "nocpufreq"))
 		return 0;
 
 	/* Get first CPU node */
diff --git a/drivers/cpufreq/powernv-cpufreq.c b/drivers/cpufreq/powernv-cpufreq.c
index 379c0837f5a9..2dfd4fdb5a52 100644
--- a/drivers/cpufreq/powernv-cpufreq.c
+++ b/drivers/cpufreq/powernv-cpufreq.c
@@ -26,6 +26,7 @@
 #include <linux/cpufreq.h>
 #include <linux/smp.h>
 #include <linux/of.h>
+#include <linux/reboot.h>
 
 #include <asm/cputhreads.h>
 #include <asm/firmware.h>
@@ -35,6 +36,7 @@
 #define POWERNV_MAX_PSTATES	256
 
 static struct cpufreq_frequency_table powernv_freqs[POWERNV_MAX_PSTATES+1];
+static bool rebooting;
 
 /*
  * Note: The set of pstates consists of contiguous integers, the
@@ -284,6 +286,15 @@ static void set_pstate(void *freq_data)
 }
 
 /*
+ * get_nominal_index: Returns the index corresponding to the nominal
+ * pstate in the cpufreq table
+ */
+static inline unsigned int get_nominal_index(void)
+{
+	return powernv_pstate_info.max - powernv_pstate_info.nominal;
+}
+
+/*
  * powernv_cpufreq_target_index: Sets the frequency corresponding to
  * the cpufreq table entry indexed by new_index on the cpus in the
  * mask policy->cpus
@@ -293,6 +304,9 @@ static int powernv_cpufreq_target_index(struct cpufreq_policy *policy,
 {
 	struct powernv_smp_call_data freq_data;
 
+	if (unlikely(rebooting) && new_index != get_nominal_index())
+		return 0;
+
 	freq_data.pstate_id = powernv_freqs[new_index].driver_data;
 
 	/*
@@ -317,6 +331,33 @@ static int powernv_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	return cpufreq_table_validate_and_show(policy, powernv_freqs);
 }
 
+static int powernv_cpufreq_reboot_notifier(struct notifier_block *nb,
+				unsigned long action, void *unused)
+{
+	int cpu;
+	struct cpufreq_policy cpu_policy;
+
+	rebooting = true;
+	for_each_online_cpu(cpu) {
+		cpufreq_get_policy(&cpu_policy, cpu);
+		powernv_cpufreq_target_index(&cpu_policy, get_nominal_index());
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block powernv_cpufreq_reboot_nb = {
+	.notifier_call = powernv_cpufreq_reboot_notifier,
+};
+
+static void powernv_cpufreq_stop_cpu(struct cpufreq_policy *policy)
+{
+	struct powernv_smp_call_data freq_data;
+
+	freq_data.pstate_id = powernv_pstate_info.min;
+	smp_call_function_single(policy->cpu, set_pstate, &freq_data, 1);
+}
+
 static struct cpufreq_driver powernv_cpufreq_driver = {
 	.name		= "powernv-cpufreq",
 	.flags		= CPUFREQ_CONST_LOOPS,
@@ -324,6 +365,7 @@ static struct cpufreq_driver powernv_cpufreq_driver = {
 	.verify		= cpufreq_generic_frequency_table_verify,
 	.target_index	= powernv_cpufreq_target_index,
 	.get		= powernv_cpufreq_get,
+	.stop_cpu	= powernv_cpufreq_stop_cpu,
 	.attr		= powernv_cpu_freq_attr,
 };
 
@@ -342,12 +384,14 @@ static int __init powernv_cpufreq_init(void)
 		return rc;
 	}
 
+	register_reboot_notifier(&powernv_cpufreq_reboot_nb);
 	return cpufreq_register_driver(&powernv_cpufreq_driver);
 }
 module_init(powernv_cpufreq_init);
 
 static void __exit powernv_cpufreq_exit(void)
 {
+	unregister_reboot_notifier(&powernv_cpufreq_reboot_nb);
 	cpufreq_unregister_driver(&powernv_cpufreq_driver);
 }
 module_exit(powernv_cpufreq_exit);
diff --git a/drivers/cpufreq/ppc-corenet-cpufreq.c b/drivers/cpufreq/ppc-corenet-cpufreq.c
index 3607070797af..bee5df7794d3 100644
--- a/drivers/cpufreq/ppc-corenet-cpufreq.c
+++ b/drivers/cpufreq/ppc-corenet-cpufreq.c
@@ -199,7 +199,6 @@ static int corenet_cpufreq_cpu_init(struct cpufreq_policy *policy)
 	}
 
 	data->table = table;
-	per_cpu(cpu_data, cpu) = data;
 
 	/* update ->cpus if we have cluster, no harm if not */
 	cpumask_copy(policy->cpus, per_cpu(cpu_mask, cpu));
diff --git a/drivers/cpufreq/s5pv210-cpufreq.c b/drivers/cpufreq/s5pv210-cpufreq.c
index 9a68225a757e..567caa6313ff 100644
--- a/drivers/cpufreq/s5pv210-cpufreq.c
+++ b/drivers/cpufreq/s5pv210-cpufreq.c
@@ -501,7 +501,7 @@ static int check_mem_type(void __iomem *dmc_reg)
 	return val >> 8;
 }
 
-static int __init s5pv210_cpu_init(struct cpufreq_policy *policy)
+static int s5pv210_cpu_init(struct cpufreq_policy *policy)
 {
 	unsigned long mem_type;
 	int ret;
@@ -597,7 +597,7 @@ static int s5pv210_cpufreq_probe(struct platform_device *pdev)
 	 * and dependencies on platform headers. It is necessary to enable
 	 * S5PV210 multi-platform support and will be removed together with
 	 * this whole driver as soon as S5PV210 gets migrated to use
-	 * cpufreq-cpu0 driver.
+	 * cpufreq-dt driver.
 	 */
 	np = of_find_compatible_node(NULL, NULL, "samsung,s5pv210-clock");
 	if (!np) {
diff --git a/drivers/cpuidle/Kconfig b/drivers/cpuidle/Kconfig
index 32748c36c477..c5029c1209b4 100644
--- a/drivers/cpuidle/Kconfig
+++ b/drivers/cpuidle/Kconfig
@@ -25,11 +25,19 @@ config CPU_IDLE_GOV_MENU
 	bool "Menu governor (for tickless system)"
 	default y
 
+config DT_IDLE_STATES
+	bool
+
 menu "ARM CPU Idle Drivers"
 depends on ARM
 source "drivers/cpuidle/Kconfig.arm"
 endmenu
 
+menu "ARM64 CPU Idle Drivers"
+depends on ARM64
+source "drivers/cpuidle/Kconfig.arm64"
+endmenu
+
 menu "MIPS CPU Idle Drivers"
 depends on MIPS
 source "drivers/cpuidle/Kconfig.mips"
diff --git a/drivers/cpuidle/Kconfig.arm b/drivers/cpuidle/Kconfig.arm
index 38cff69ffe06..8c16ab20fb15 100644
--- a/drivers/cpuidle/Kconfig.arm
+++ b/drivers/cpuidle/Kconfig.arm
@@ -7,6 +7,7 @@ config ARM_BIG_LITTLE_CPUIDLE
 	depends on MCPM
 	select ARM_CPU_SUSPEND
 	select CPU_IDLE_MULTIPLE_DRIVERS
+	select DT_IDLE_STATES
 	help
 	  Select this option to enable CPU idle driver for big.LITTLE based
 	  ARM systems. Driver manages CPUs coordination through MCPM and
@@ -28,7 +29,7 @@ config ARM_HIGHBANK_CPUIDLE
 
 config ARM_KIRKWOOD_CPUIDLE
 	bool "CPU Idle Driver for Marvell Kirkwood SoCs"
-	depends on ARCH_KIRKWOOD || MACH_KIRKWOOD
+	depends on MACH_KIRKWOOD
 	help
 	  This adds the CPU Idle driver for Marvell Kirkwood SoCs.
 
diff --git a/drivers/cpuidle/Kconfig.arm64 b/drivers/cpuidle/Kconfig.arm64
new file mode 100644
index 000000000000..d0a08ed1b2ee
--- /dev/null
+++ b/drivers/cpuidle/Kconfig.arm64
@@ -0,0 +1,14 @@
+#
+# ARM64 CPU Idle drivers
+#
+
+config ARM64_CPUIDLE
+	bool "Generic ARM64 CPU idle Driver"
+	select ARM64_CPU_SUSPEND
+	select DT_IDLE_STATES
+	help
+	  Select this to enable generic cpuidle driver for ARM64.
+	  It provides a generic idle driver whose idle states are configured
+	  at run-time through DT nodes. The CPUidle suspend backend is
+	  initialized by calling the CPU operations init idle hook
+	  provided by architecture code.
diff --git a/drivers/cpuidle/Makefile b/drivers/cpuidle/Makefile
index 11edb31c55e9..4d177b916f75 100644
--- a/drivers/cpuidle/Makefile
+++ b/drivers/cpuidle/Makefile
@@ -4,6 +4,7 @@
 
 obj-y += cpuidle.o driver.o governor.o sysfs.o governors/
 obj-$(CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED) += coupled.o
+obj-$(CONFIG_DT_IDLE_STATES)		  += dt_idle_states.o
 
 ##################################################################################
 # ARM SoC drivers
@@ -22,6 +23,10 @@ obj-$(CONFIG_ARM_EXYNOS_CPUIDLE)        += cpuidle-exynos.o
 obj-$(CONFIG_MIPS_CPS_CPUIDLE)		+= cpuidle-cps.o
 
 ###############################################################################
+# ARM64 drivers
+obj-$(CONFIG_ARM64_CPUIDLE)		+= cpuidle-arm64.o
+
+###############################################################################
 # POWERPC drivers
 obj-$(CONFIG_PSERIES_CPUIDLE)		+= cpuidle-pseries.o
 obj-$(CONFIG_POWERNV_CPUIDLE)		+= cpuidle-powernv.o
diff --git a/drivers/cpuidle/cpuidle-arm64.c b/drivers/cpuidle/cpuidle-arm64.c
new file mode 100644
index 000000000000..50997ea942fc
--- /dev/null
+++ b/drivers/cpuidle/cpuidle-arm64.c
@@ -0,0 +1,133 @@
+/*
+ * ARM64 generic CPU idle driver.
+ *
+ * Copyright (C) 2014 ARM Ltd.
+ * Author: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define pr_fmt(fmt) "CPUidle arm64: " fmt
+
+#include <linux/cpuidle.h>
+#include <linux/cpumask.h>
+#include <linux/cpu_pm.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+
+#include <asm/cpuidle.h>
+#include <asm/suspend.h>
+
+#include "dt_idle_states.h"
+
+/*
+ * arm64_enter_idle_state - Programs CPU to enter the specified state
+ *
+ * dev: cpuidle device
+ * drv: cpuidle driver
+ * idx: state index
+ *
+ * Called from the CPUidle framework to program the device to the
+ * specified target state selected by the governor.
+ */
+static int arm64_enter_idle_state(struct cpuidle_device *dev,
+				  struct cpuidle_driver *drv, int idx)
+{
+	int ret;
+
+	if (!idx) {
+		cpu_do_idle();
+		return idx;
+	}
+
+	ret = cpu_pm_enter();
+	if (!ret) {
+		/*
+		 * Pass idle state index to cpu_suspend which in turn will
+		 * call the CPU ops suspend protocol with idle index as a
+		 * parameter.
+		 */
+		ret = cpu_suspend(idx);
+
+		cpu_pm_exit();
+	}
+
+	return ret ? -1 : idx;
+}
+
+static struct cpuidle_driver arm64_idle_driver = {
+	.name = "arm64_idle",
+	.owner = THIS_MODULE,
+	/*
+	 * State at index 0 is standby wfi and considered standard
+	 * on all ARM platforms. If in some platforms simple wfi
+	 * can't be used as "state 0", DT bindings must be implemented
+	 * to work around this issue and allow installing a special
+	 * handler for idle state index 0.
+	 */
+	.states[0] = {
+		.enter                  = arm64_enter_idle_state,
+		.exit_latency           = 1,
+		.target_residency       = 1,
+		.power_usage		= UINT_MAX,
+		.flags                  = CPUIDLE_FLAG_TIME_VALID,
+		.name                   = "WFI",
+		.desc                   = "ARM64 WFI",
+	}
+};
+
+static const struct of_device_id arm64_idle_state_match[] __initconst = {
+	{ .compatible = "arm,idle-state",
+	  .data = arm64_enter_idle_state },
+	{ },
+};
+
+/*
+ * arm64_idle_init
+ *
+ * Registers the arm64 specific cpuidle driver with the cpuidle
+ * framework. It relies on core code to parse the idle states
+ * and initialize them using driver data structures accordingly.
+ */
+static int __init arm64_idle_init(void)
+{
+	int cpu, ret;
+	struct cpuidle_driver *drv = &arm64_idle_driver;
+
+	/*
+	 * Initialize idle states data, starting at index 1.
+	 * This driver is DT only, if no DT idle states are detected (ret == 0)
+	 * let the driver initialization fail accordingly since there is no
+	 * reason to initialize the idle driver if only wfi is supported.
+	 */
+	ret = dt_init_idle_driver(drv, arm64_idle_state_match, 1);
+	if (ret <= 0) {
+		if (ret)
+			pr_err("failed to initialize idle states\n");
+		return ret ? : -ENODEV;
+	}
+
+	/*
+	 * Call arch CPU operations in order to initialize
+	 * idle states suspend back-end specific data
+	 */
+	for_each_possible_cpu(cpu) {
+		ret = cpu_init_idle(cpu);
+		if (ret) {
+			pr_err("CPU %d failed to init idle CPU ops\n", cpu);
+			return ret;
+		}
+	}
+
+	ret = cpuidle_register(drv, NULL);
+	if (ret) {
+		pr_err("failed to register cpuidle driver\n");
+		return ret;
+	}
+
+	return 0;
+}
+device_initcall(arm64_idle_init);
diff --git a/drivers/cpuidle/cpuidle-big_little.c b/drivers/cpuidle/cpuidle-big_little.c
index 344d79fa3407..fbc00a1d3c48 100644
--- a/drivers/cpuidle/cpuidle-big_little.c
+++ b/drivers/cpuidle/cpuidle-big_little.c
@@ -24,6 +24,8 @@
 #include <asm/smp_plat.h>
 #include <asm/suspend.h>
 
+#include "dt_idle_states.h"
+
 static int bl_enter_powerdown(struct cpuidle_device *dev,
 			      struct cpuidle_driver *drv, int idx);
 
@@ -73,6 +75,12 @@ static struct cpuidle_driver bl_idle_little_driver = {
 	.state_count = 2,
 };
 
+static const struct of_device_id bl_idle_state_match[] __initconst = {
+	{ .compatible = "arm,idle-state",
+	  .data = bl_enter_powerdown },
+	{ },
+};
+
 static struct cpuidle_driver bl_idle_big_driver = {
 	.name = "big_idle",
 	.owner = THIS_MODULE,
@@ -138,25 +146,18 @@ static int bl_enter_powerdown(struct cpuidle_device *dev,
 	return idx;
 }
 
-static int __init bl_idle_driver_init(struct cpuidle_driver *drv, int cpu_id)
+static int __init bl_idle_driver_init(struct cpuidle_driver *drv, int part_id)
 {
-	struct cpuinfo_arm *cpu_info;
 	struct cpumask *cpumask;
-	unsigned long cpuid;
 	int cpu;
 
 	cpumask = kzalloc(cpumask_size(), GFP_KERNEL);
 	if (!cpumask)
 		return -ENOMEM;
 
-	for_each_possible_cpu(cpu) {
-		cpu_info = &per_cpu(cpu_data, cpu);
-		cpuid = is_smp() ? cpu_info->cpuid : read_cpuid_id();
-
-		/* read cpu id part number */
-		if ((cpuid & 0xFFF0) == cpu_id)
+	for_each_possible_cpu(cpu)
+		if (smp_cpuid_part(cpu) == part_id)
 			cpumask_set_cpu(cpu, cpumask);
-	}
 
 	drv->cpumask = cpumask;
 
@@ -166,6 +167,7 @@ static int __init bl_idle_driver_init(struct cpuidle_driver *drv, int cpu_id)
 static const struct of_device_id compatible_machine_match[] = {
 	{ .compatible = "arm,vexpress,v2p-ca15_a7" },
 	{ .compatible = "samsung,exynos5420" },
+	{ .compatible = "samsung,exynos5800" },
 	{},
 };
 
@@ -197,6 +199,17 @@ static int __init bl_idle_init(void)
 	if (ret)
 		goto out_uninit_little;
 
+	/* Start at index 1, index 0 standard WFI */
+	ret = dt_init_idle_driver(&bl_idle_big_driver, bl_idle_state_match, 1);
+	if (ret < 0)
+		goto out_uninit_big;
+
+	/* Start at index 1, index 0 standard WFI */
+	ret = dt_init_idle_driver(&bl_idle_little_driver,
+				  bl_idle_state_match, 1);
+	if (ret < 0)
+		goto out_uninit_big;
+
 	ret = cpuidle_register(&bl_idle_little_driver, NULL);
 	if (ret)
 		goto out_uninit_big;
diff --git a/drivers/cpuidle/cpuidle-powernv.c b/drivers/cpuidle/cpuidle-powernv.c
index a64be578dab2..7d3a3497dd4c 100644
--- a/drivers/cpuidle/cpuidle-powernv.c
+++ b/drivers/cpuidle/cpuidle-powernv.c
@@ -163,7 +163,8 @@ static int powernv_add_idle_states(void)
 	int nr_idle_states = 1; /* Snooze */
 	int dt_idle_states;
 	const __be32 *idle_state_flags;
-	u32 len_flags, flags;
+	const __be32 *idle_state_latency;
+	u32 len_flags, flags, latency_ns;
 	int i;
 
 	/* Currently we have snooze statically defined */
@@ -180,18 +181,32 @@ static int powernv_add_idle_states(void)
 		return nr_idle_states;
 	}
 
+	idle_state_latency = of_get_property(power_mgt,
+			"ibm,cpu-idle-state-latencies-ns", NULL);
+	if (!idle_state_latency) {
+		pr_warn("DT-PowerMgmt: missing ibm,cpu-idle-state-latencies-ns\n");
+		return nr_idle_states;
+	}
+
 	dt_idle_states = len_flags / sizeof(u32);
 
 	for (i = 0; i < dt_idle_states; i++) {
 
 		flags = be32_to_cpu(idle_state_flags[i]);
+
+		/* Cpuidle accepts exit_latency in us and we estimate
+		 * target residency to be 10x exit_latency
+		 */
+		latency_ns = be32_to_cpu(idle_state_latency[i]);
 		if (flags & IDLE_USE_INST_NAP) {
 			/* Add NAP state */
 			strcpy(powernv_states[nr_idle_states].name, "Nap");
 			strcpy(powernv_states[nr_idle_states].desc, "Nap");
 			powernv_states[nr_idle_states].flags = CPUIDLE_FLAG_TIME_VALID;
-			powernv_states[nr_idle_states].exit_latency = 10;
-			powernv_states[nr_idle_states].target_residency = 100;
+			powernv_states[nr_idle_states].exit_latency =
+					((unsigned int)latency_ns) / 1000;
+			powernv_states[nr_idle_states].target_residency =
+					((unsigned int)latency_ns / 100);
 			powernv_states[nr_idle_states].enter = &nap_loop;
 			nr_idle_states++;
 		}
@@ -202,8 +217,10 @@ static int powernv_add_idle_states(void)
 			strcpy(powernv_states[nr_idle_states].desc, "FastSleep");
 			powernv_states[nr_idle_states].flags =
 				CPUIDLE_FLAG_TIME_VALID | CPUIDLE_FLAG_TIMER_STOP;
-			powernv_states[nr_idle_states].exit_latency = 300;
-			powernv_states[nr_idle_states].target_residency = 1000000;
+			powernv_states[nr_idle_states].exit_latency =
+					((unsigned int)latency_ns) / 1000;
+			powernv_states[nr_idle_states].target_residency =
+					((unsigned int)latency_ns / 100);
 			powernv_states[nr_idle_states].enter = &fastsleep_loop;
 			nr_idle_states++;
 		}
diff --git a/drivers/cpuidle/cpuidle-zynq.c b/drivers/cpuidle/cpuidle-zynq.c
index aded75928028..c61b8b2a7c77 100644
--- a/drivers/cpuidle/cpuidle-zynq.c
+++ b/drivers/cpuidle/cpuidle-zynq.c
@@ -26,7 +26,6 @@
  */
 
 #include <linux/init.h>
-#include <linux/cpu_pm.h>
 #include <linux/cpuidle.h>
 #include <linux/platform_device.h>
 #include <asm/proc-fns.h>
@@ -38,15 +37,9 @@
 static int zynq_enter_idle(struct cpuidle_device *dev,
 			   struct cpuidle_driver *drv, int index)
 {
-	/* Devices must be stopped here */
-	cpu_pm_enter();
-
 	/* Add code for DDR self refresh start */
 	cpu_do_idle();
 
-	/* Add code for DDR self refresh stop */
-	cpu_pm_exit();
-
 	return index;
 }
 
@@ -59,8 +52,7 @@ static struct cpuidle_driver zynq_idle_driver = {
 			.enter			= zynq_enter_idle,
 			.exit_latency		= 10,
 			.target_residency	= 10000,
-			.flags			= CPUIDLE_FLAG_TIME_VALID |
-						  CPUIDLE_FLAG_TIMER_STOP,
+			.flags			= CPUIDLE_FLAG_TIME_VALID,
 			.name			= "RAM_SR",
 			.desc			= "WFI and RAM Self Refresh",
 		},
diff --git a/drivers/cpuidle/cpuidle.c b/drivers/cpuidle/cpuidle.c
index ee9df5e3f5eb..125150dc6e81 100644
--- a/drivers/cpuidle/cpuidle.c
+++ b/drivers/cpuidle/cpuidle.c
@@ -223,8 +223,14 @@ void cpuidle_uninstall_idle_handler(void)
 {
 	if (enabled_devices) {
 		initialized = 0;
-		kick_all_cpus_sync();
+		wake_up_all_idle_cpus();
 	}
+
+	/*
+	 * Make sure external observers (such as the scheduler)
+	 * are done looking at pointed idle states.
+	 */
+	synchronize_rcu();
 }
 
 /**
@@ -530,11 +536,6 @@ EXPORT_SYMBOL_GPL(cpuidle_register);
 
 #ifdef CONFIG_SMP
 
-static void smp_callback(void *v)
-{
-	/* we already woke the CPU up, nothing more to do */
-}
-
 /*
  * This function gets called when a part of the kernel has a new latency
  * requirement.  This means we need to get all processors out of their C-state,
@@ -544,7 +545,7 @@ static void smp_callback(void *v)
 static int cpuidle_latency_notify(struct notifier_block *b,
 		unsigned long l, void *v)
 {
-	smp_call_function(smp_callback, NULL, 1);
+	wake_up_all_idle_cpus();
 	return NOTIFY_OK;
 }
 
diff --git a/drivers/cpuidle/dt_idle_states.c b/drivers/cpuidle/dt_idle_states.c
new file mode 100644
index 000000000000..52f4d11bbf3f
--- /dev/null
+++ b/drivers/cpuidle/dt_idle_states.c
@@ -0,0 +1,213 @@
+/*
+ * DT idle states parsing code.
+ *
+ * Copyright (C) 2014 ARM Ltd.
+ * Author: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define pr_fmt(fmt) "DT idle-states: " fmt
+
+#include <linux/cpuidle.h>
+#include <linux/cpumask.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+
+#include "dt_idle_states.h"
+
+static int init_state_node(struct cpuidle_state *idle_state,
+			   const struct of_device_id *matches,
+			   struct device_node *state_node)
+{
+	int err;
+	const struct of_device_id *match_id;
+
+	match_id = of_match_node(matches, state_node);
+	if (!match_id)
+		return -ENODEV;
+	/*
+	 * CPUidle drivers are expected to initialize the const void *data
+	 * pointer of the passed in struct of_device_id array to the idle
+	 * state enter function.
+	 */
+	idle_state->enter = match_id->data;
+
+	err = of_property_read_u32(state_node, "wakeup-latency-us",
+				   &idle_state->exit_latency);
+	if (err) {
+		u32 entry_latency, exit_latency;
+
+		err = of_property_read_u32(state_node, "entry-latency-us",
+					   &entry_latency);
+		if (err) {
+			pr_debug(" * %s missing entry-latency-us property\n",
+				 state_node->full_name);
+			return -EINVAL;
+		}
+
+		err = of_property_read_u32(state_node, "exit-latency-us",
+					   &exit_latency);
+		if (err) {
+			pr_debug(" * %s missing exit-latency-us property\n",
+				 state_node->full_name);
+			return -EINVAL;
+		}
+		/*
+		 * If wakeup-latency-us is missing, default to entry+exit
+		 * latencies as defined in idle states bindings
+		 */
+		idle_state->exit_latency = entry_latency + exit_latency;
+	}
+
+	err = of_property_read_u32(state_node, "min-residency-us",
+				   &idle_state->target_residency);
+	if (err) {
+		pr_debug(" * %s missing min-residency-us property\n",
+			     state_node->full_name);
+		return -EINVAL;
+	}
+
+	idle_state->flags = CPUIDLE_FLAG_TIME_VALID;
+	if (of_property_read_bool(state_node, "local-timer-stop"))
+		idle_state->flags |= CPUIDLE_FLAG_TIMER_STOP;
+	/*
+	 * TODO:
+	 *	replace with kstrdup and pointer assignment when name
+	 *	and desc become string pointers
+	 */
+	strncpy(idle_state->name, state_node->name, CPUIDLE_NAME_LEN - 1);
+	strncpy(idle_state->desc, state_node->name, CPUIDLE_DESC_LEN - 1);
+	return 0;
+}
+
+/*
+ * Check that the idle state is uniform across all CPUs in the CPUidle driver
+ * cpumask
+ */
+static bool idle_state_valid(struct device_node *state_node, unsigned int idx,
+			     const cpumask_t *cpumask)
+{
+	int cpu;
+	struct device_node *cpu_node, *curr_state_node;
+	bool valid = true;
+
+	/*
+	 * Compare idle state phandles for index idx on all CPUs in the
+	 * CPUidle driver cpumask. Start from next logical cpu following
+	 * cpumask_first(cpumask) since that's the CPU state_node was
+	 * retrieved from. If a mismatch is found bail out straight
+	 * away since we certainly hit a firmware misconfiguration.
+	 */
+	for (cpu = cpumask_next(cpumask_first(cpumask), cpumask);
+	     cpu < nr_cpu_ids; cpu = cpumask_next(cpu, cpumask)) {
+		cpu_node = of_cpu_device_node_get(cpu);
+		curr_state_node = of_parse_phandle(cpu_node, "cpu-idle-states",
+						   idx);
+		if (state_node != curr_state_node)
+			valid = false;
+
+		of_node_put(curr_state_node);
+		of_node_put(cpu_node);
+		if (!valid)
+			break;
+	}
+
+	return valid;
+}
+
+/**
+ * dt_init_idle_driver() - Parse the DT idle states and initialize the
+ *			   idle driver states array
+ * @drv:	  Pointer to CPU idle driver to be initialized
+ * @matches:	  Array of of_device_id match structures to search in for
+ *		  compatible idle state nodes. The data pointer for each valid
+ *		  struct of_device_id entry in the matches array must point to
+ *		  a function with the following signature, that corresponds to
+ *		  the CPUidle state enter function signature:
+ *
+ *		  int (*)(struct cpuidle_device *dev,
+ *			  struct cpuidle_driver *drv,
+ *			  int index);
+ *
+ * @start_idx:    First idle state index to be initialized
+ *
+ * If DT idle states are detected and are valid the state count and states
+ * array entries in the cpuidle driver are initialized accordingly starting
+ * from index start_idx.
+ *
+ * Return: number of valid DT idle states parsed, <0 on failure
+ */
+int dt_init_idle_driver(struct cpuidle_driver *drv,
+			const struct of_device_id *matches,
+			unsigned int start_idx)
+{
+	struct cpuidle_state *idle_state;
+	struct device_node *state_node, *cpu_node;
+	int i, err = 0;
+	const cpumask_t *cpumask;
+	unsigned int state_idx = start_idx;
+
+	if (state_idx >= CPUIDLE_STATE_MAX)
+		return -EINVAL;
+	/*
+	 * We get the idle states for the first logical cpu in the
+	 * driver mask (or cpu_possible_mask if the driver cpumask is not set)
+	 * and we check through idle_state_valid() if they are uniform
+	 * across CPUs, otherwise we hit a firmware misconfiguration.
+	 */
+	cpumask = drv->cpumask ? : cpu_possible_mask;
+	cpu_node = of_cpu_device_node_get(cpumask_first(cpumask));
+
+	for (i = 0; ; i++) {
+		state_node = of_parse_phandle(cpu_node, "cpu-idle-states", i);
+		if (!state_node)
+			break;
+
+		if (!idle_state_valid(state_node, i, cpumask)) {
+			pr_warn("%s idle state not valid, bailing out\n",
+				state_node->full_name);
+			err = -EINVAL;
+			break;
+		}
+
+		if (state_idx == CPUIDLE_STATE_MAX) {
+			pr_warn("State index reached static CPU idle driver states array size\n");
+			break;
+		}
+
+		idle_state = &drv->states[state_idx++];
+		err = init_state_node(idle_state, matches, state_node);
+		if (err) {
+			pr_err("Parsing idle state node %s failed with err %d\n",
+			       state_node->full_name, err);
+			err = -EINVAL;
+			break;
+		}
+		of_node_put(state_node);
+	}
+
+	of_node_put(state_node);
+	of_node_put(cpu_node);
+	if (err)
+		return err;
+	/*
+	 * Update the driver state count only if some valid DT idle states
+	 * were detected
+	 */
+	if (i)
+		drv->state_count = state_idx;
+
+	/*
+	 * Return the number of present and valid DT idle states, which can
+	 * also be 0 on platforms with missing DT idle states or legacy DT
+	 * configuration predating the DT idle states bindings.
+	 */
+	return i;
+}
+EXPORT_SYMBOL_GPL(dt_init_idle_driver);
diff --git a/drivers/cpuidle/dt_idle_states.h b/drivers/cpuidle/dt_idle_states.h
new file mode 100644
index 000000000000..4818134bc65b
--- /dev/null
+++ b/drivers/cpuidle/dt_idle_states.h
@@ -0,0 +1,7 @@
+#ifndef __DT_IDLE_STATES
+#define __DT_IDLE_STATES
+
+int dt_init_idle_driver(struct cpuidle_driver *drv,
+			const struct of_device_id *matches,
+			unsigned int start_idx);
+#endif
diff --git a/drivers/cpuidle/governor.c b/drivers/cpuidle/governor.c
index ca89412f5122..fb9f511cca23 100644
--- a/drivers/cpuidle/governor.c
+++ b/drivers/cpuidle/governor.c
@@ -28,7 +28,7 @@ static struct cpuidle_governor * __cpuidle_find_governor(const char *str)
 	struct cpuidle_governor *gov;
 
 	list_for_each_entry(gov, &cpuidle_governors, governor_list)
-		if (!strnicmp(str, gov->name, CPUIDLE_NAME_LEN))
+		if (!strncasecmp(str, gov->name, CPUIDLE_NAME_LEN))
 			return gov;
 
 	return NULL;
diff --git a/drivers/cpuidle/governors/ladder.c b/drivers/cpuidle/governors/ladder.c
index 044ee0df5871..06b57c4c4d80 100644
--- a/drivers/cpuidle/governors/ladder.c
+++ b/drivers/cpuidle/governors/ladder.c
@@ -66,7 +66,7 @@ static inline void ladder_do_selection(struct ladder_device *ldev,
 static int ladder_select_state(struct cpuidle_driver *drv,
 				struct cpuidle_device *dev)
 {
-	struct ladder_device *ldev = &__get_cpu_var(ladder_devices);
+	struct ladder_device *ldev = this_cpu_ptr(&ladder_devices);
 	struct ladder_device_state *last_state;
 	int last_residency, last_idx = ldev->last_state_idx;
 	int latency_req = pm_qos_request(PM_QOS_CPU_DMA_LATENCY);
@@ -170,7 +170,7 @@ static int ladder_enable_device(struct cpuidle_driver *drv,
  */
 static void ladder_reflect(struct cpuidle_device *dev, int index)
 {
-	struct ladder_device *ldev = &__get_cpu_var(ladder_devices);
+	struct ladder_device *ldev = this_cpu_ptr(&ladder_devices);
 	if (index > 0)
 		ldev->last_state_idx = index;
 }
diff --git a/drivers/cpuidle/governors/menu.c b/drivers/cpuidle/governors/menu.c
index 34db2fb3ef1e..710a233b9b0d 100644
--- a/drivers/cpuidle/governors/menu.c
+++ b/drivers/cpuidle/governors/menu.c
@@ -289,7 +289,7 @@ again:
  */
 static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 {
-	struct menu_device *data = &__get_cpu_var(menu_devices);
+	struct menu_device *data = this_cpu_ptr(&menu_devices);
 	int latency_req = pm_qos_request(PM_QOS_CPU_DMA_LATENCY);
 	int i;
 	unsigned int interactivity_req;
@@ -372,7 +372,7 @@ static int menu_select(struct cpuidle_driver *drv, struct cpuidle_device *dev)
  */
 static void menu_reflect(struct cpuidle_device *dev, int index)
 {
-	struct menu_device *data = &__get_cpu_var(menu_devices);
+	struct menu_device *data = this_cpu_ptr(&menu_devices);
 	data->last_state_idx = index;
 	if (index >= 0)
 		data->needs_update = 1;
@@ -385,7 +385,7 @@ static void menu_reflect(struct cpuidle_device *dev, int index)
  */
 static void menu_update(struct cpuidle_driver *drv, struct cpuidle_device *dev)
 {
-	struct menu_device *data = &__get_cpu_var(menu_devices);
+	struct menu_device *data = this_cpu_ptr(&menu_devices);
 	int last_idx = data->last_state_idx;
 	struct cpuidle_state *target = &drv->states[last_idx];
 	unsigned int measured_us;
diff --git a/drivers/crypto/caam/caamhash.c b/drivers/crypto/caam/caamhash.c
index b464d03ebf40..f347ab7eea95 100644
--- a/drivers/crypto/caam/caamhash.c
+++ b/drivers/crypto/caam/caamhash.c
@@ -836,8 +836,9 @@ static int ahash_update_ctx(struct ahash_request *req)
 					   edesc->sec4_sg + sec4_sg_src_index,
 					   chained);
 			if (*next_buflen) {
-				sg_copy_part(next_buf, req->src, to_hash -
-					     *buflen, req->nbytes);
+				scatterwalk_map_and_copy(next_buf, req->src,
+							 to_hash - *buflen,
+							 *next_buflen, 0);
 				state->current_buf = !state->current_buf;
 			}
 		} else {
@@ -878,7 +879,8 @@ static int ahash_update_ctx(struct ahash_request *req)
 			kfree(edesc);
 		}
 	} else if (*next_buflen) {
-		sg_copy(buf + *buflen, req->src, req->nbytes);
+		scatterwalk_map_and_copy(buf + *buflen, req->src, 0,
+					 req->nbytes, 0);
 		*buflen = *next_buflen;
 		*next_buflen = last_buflen;
 	}
@@ -1262,8 +1264,9 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 		src_map_to_sec4_sg(jrdev, req->src, src_nents,
 				   edesc->sec4_sg + 1, chained);
 		if (*next_buflen) {
-			sg_copy_part(next_buf, req->src, to_hash - *buflen,
-				    req->nbytes);
+			scatterwalk_map_and_copy(next_buf, req->src,
+						 to_hash - *buflen,
+						 *next_buflen, 0);
 			state->current_buf = !state->current_buf;
 		}
 
@@ -1304,7 +1307,8 @@ static int ahash_update_no_ctx(struct ahash_request *req)
 			kfree(edesc);
 		}
 	} else if (*next_buflen) {
-		sg_copy(buf + *buflen, req->src, req->nbytes);
+		scatterwalk_map_and_copy(buf + *buflen, req->src, 0,
+					 req->nbytes, 0);
 		*buflen = *next_buflen;
 		*next_buflen = 0;
 	}
@@ -1413,9 +1417,9 @@ static int ahash_update_first(struct ahash_request *req)
 	struct device *jrdev = ctx->jrdev;
 	gfp_t flags = (req->base.flags & (CRYPTO_TFM_REQ_MAY_BACKLOG |
 		       CRYPTO_TFM_REQ_MAY_SLEEP)) ? GFP_KERNEL : GFP_ATOMIC;
-	u8 *next_buf = state->buf_0 + state->current_buf *
-		       CAAM_MAX_HASH_BLOCK_SIZE;
-	int *next_buflen = &state->buflen_0 + state->current_buf;
+	u8 *next_buf = state->current_buf ? state->buf_1 : state->buf_0;
+	int *next_buflen = state->current_buf ?
+		&state->buflen_1 : &state->buflen_0;
 	int to_hash;
 	u32 *sh_desc = ctx->sh_desc_update_first, *desc;
 	dma_addr_t ptr = ctx->sh_desc_update_first_dma;
@@ -1476,7 +1480,8 @@ static int ahash_update_first(struct ahash_request *req)
 		}
 
 		if (*next_buflen)
-			sg_copy_part(next_buf, req->src, to_hash, req->nbytes);
+			scatterwalk_map_and_copy(next_buf, req->src, to_hash,
+						 *next_buflen, 0);
 
 		sh_len = desc_len(sh_desc);
 		desc = edesc->hw_desc;
@@ -1511,7 +1516,8 @@ static int ahash_update_first(struct ahash_request *req)
 		state->update = ahash_update_no_ctx;
 		state->finup = ahash_finup_no_ctx;
 		state->final = ahash_final_no_ctx;
-		sg_copy(next_buf, req->src, req->nbytes);
+		scatterwalk_map_and_copy(next_buf, req->src, 0,
+					 req->nbytes, 0);
 	}
 #ifdef DEBUG
 	print_hex_dump(KERN_ERR, "next buf@"__stringify(__LINE__)": ",
diff --git a/drivers/crypto/caam/ctrl.c b/drivers/crypto/caam/ctrl.c
index 3cade79ea41e..31000c8c4a90 100644
--- a/drivers/crypto/caam/ctrl.c
+++ b/drivers/crypto/caam/ctrl.c
@@ -1,5 +1,4 @@
-/*
- * CAAM control-plane driver backend
+/* * CAAM control-plane driver backend
  * Controller-level driver, kernel property detection, initialization
  *
  * Copyright 2008-2012 Freescale Semiconductor, Inc.
@@ -81,38 +80,37 @@ static inline int run_descriptor_deco0(struct device *ctrldev, u32 *desc,
 					u32 *status)
 {
 	struct caam_drv_private *ctrlpriv = dev_get_drvdata(ctrldev);
-	struct caam_full __iomem *topregs;
+	struct caam_ctrl __iomem *ctrl = ctrlpriv->ctrl;
+	struct caam_deco __iomem *deco = ctrlpriv->deco;
 	unsigned int timeout = 100000;
 	u32 deco_dbg_reg, flags;
 	int i;
 
-	/* Set the bit to request direct access to DECO0 */
-	topregs = (struct caam_full __iomem *)ctrlpriv->ctrl;
 
 	if (ctrlpriv->virt_en == 1) {
-		setbits32(&topregs->ctrl.deco_rsr, DECORSR_JR0);
+		setbits32(&ctrl->deco_rsr, DECORSR_JR0);
 
-		while (!(rd_reg32(&topregs->ctrl.deco_rsr) & DECORSR_VALID) &&
+		while (!(rd_reg32(&ctrl->deco_rsr) & DECORSR_VALID) &&
 		       --timeout)
 			cpu_relax();
 
 		timeout = 100000;
 	}
 
-	setbits32(&topregs->ctrl.deco_rq, DECORR_RQD0ENABLE);
+	setbits32(&ctrl->deco_rq, DECORR_RQD0ENABLE);
 
-	while (!(rd_reg32(&topregs->ctrl.deco_rq) & DECORR_DEN0) &&
+	while (!(rd_reg32(&ctrl->deco_rq) & DECORR_DEN0) &&
 								 --timeout)
 		cpu_relax();
 
 	if (!timeout) {
 		dev_err(ctrldev, "failed to acquire DECO 0\n");
-		clrbits32(&topregs->ctrl.deco_rq, DECORR_RQD0ENABLE);
+		clrbits32(&ctrl->deco_rq, DECORR_RQD0ENABLE);
 		return -ENODEV;
 	}
 
 	for (i = 0; i < desc_len(desc); i++)
-		wr_reg32(&topregs->deco.descbuf[i], *(desc + i));
+		wr_reg32(&deco->descbuf[i], *(desc + i));
 
 	flags = DECO_JQCR_WHL;
 	/*
@@ -123,11 +121,11 @@ static inline int run_descriptor_deco0(struct device *ctrldev, u32 *desc,
 		flags |= DECO_JQCR_FOUR;
 
 	/* Instruct the DECO to execute it */
-	wr_reg32(&topregs->deco.jr_ctl_hi, flags);
+	wr_reg32(&deco->jr_ctl_hi, flags);
 
 	timeout = 10000000;
 	do {
-		deco_dbg_reg = rd_reg32(&topregs->deco.desc_dbg);
+		deco_dbg_reg = rd_reg32(&deco->desc_dbg);
 		/*
 		 * If an error occured in the descriptor, then
 		 * the DECO status field will be set to 0x0D
@@ -138,14 +136,14 @@ static inline int run_descriptor_deco0(struct device *ctrldev, u32 *desc,
 		cpu_relax();
 	} while ((deco_dbg_reg & DESC_DBG_DECO_STAT_VALID) && --timeout);
 
-	*status = rd_reg32(&topregs->deco.op_status_hi) &
+	*status = rd_reg32(&deco->op_status_hi) &
 		  DECO_OP_STATUS_HI_ERR_MASK;
 
 	if (ctrlpriv->virt_en == 1)
-		clrbits32(&topregs->ctrl.deco_rsr, DECORSR_JR0);
+		clrbits32(&ctrl->deco_rsr, DECORSR_JR0);
 
 	/* Mark the DECO as free */
-	clrbits32(&topregs->ctrl.deco_rq, DECORR_RQD0ENABLE);
+	clrbits32(&ctrl->deco_rq, DECORR_RQD0ENABLE);
 
 	if (!timeout)
 		return -EAGAIN;
@@ -176,13 +174,13 @@ static int instantiate_rng(struct device *ctrldev, int state_handle_mask,
 			   int gen_sk)
 {
 	struct caam_drv_private *ctrlpriv = dev_get_drvdata(ctrldev);
-	struct caam_full __iomem *topregs;
+	struct caam_ctrl __iomem *ctrl;
 	struct rng4tst __iomem *r4tst;
 	u32 *desc, status, rdsta_val;
 	int ret = 0, sh_idx;
 
-	topregs = (struct caam_full __iomem *)ctrlpriv->ctrl;
-	r4tst = &topregs->ctrl.r4tst[0];
+	ctrl = (struct caam_ctrl __iomem *)ctrlpriv->ctrl;
+	r4tst = &ctrl->r4tst[0];
 
 	desc = kmalloc(CAAM_CMD_SZ * 7, GFP_KERNEL);
 	if (!desc)
@@ -212,12 +210,11 @@ static int instantiate_rng(struct device *ctrldev, int state_handle_mask,
 		 * CAAM eras), then try again.
 		 */
 		rdsta_val =
-			rd_reg32(&topregs->ctrl.r4tst[0].rdsta) & RDSTA_IFMASK;
+			rd_reg32(&ctrl->r4tst[0].rdsta) & RDSTA_IFMASK;
 		if (status || !(rdsta_val & (1 << sh_idx)))
 			ret = -EAGAIN;
 		if (ret)
 			break;
-
 		dev_info(ctrldev, "Instantiated RNG4 SH%d\n", sh_idx);
 		/* Clear the contents before recreating the descriptor */
 		memset(desc, 0x00, CAAM_CMD_SZ * 7);
@@ -285,12 +282,12 @@ static int caam_remove(struct platform_device *pdev)
 {
 	struct device *ctrldev;
 	struct caam_drv_private *ctrlpriv;
-	struct caam_full __iomem *topregs;
+	struct caam_ctrl __iomem *ctrl;
 	int ring, ret = 0;
 
 	ctrldev = &pdev->dev;
 	ctrlpriv = dev_get_drvdata(ctrldev);
-	topregs = (struct caam_full __iomem *)ctrlpriv->ctrl;
+	ctrl = (struct caam_ctrl __iomem *)ctrlpriv->ctrl;
 
 	/* Remove platform devices for JobRs */
 	for (ring = 0; ring < ctrlpriv->total_jobrs; ring++) {
@@ -308,7 +305,7 @@ static int caam_remove(struct platform_device *pdev)
 #endif
 
 	/* Unmap controller region */
-	iounmap(&topregs->ctrl);
+	iounmap(&ctrl);
 
 	return ret;
 }
@@ -323,12 +320,12 @@ static void kick_trng(struct platform_device *pdev, int ent_delay)
 {
 	struct device *ctrldev = &pdev->dev;
 	struct caam_drv_private *ctrlpriv = dev_get_drvdata(ctrldev);
-	struct caam_full __iomem *topregs;
+	struct caam_ctrl __iomem *ctrl;
 	struct rng4tst __iomem *r4tst;
 	u32 val;
 
-	topregs = (struct caam_full __iomem *)ctrlpriv->ctrl;
-	r4tst = &topregs->ctrl.r4tst[0];
+	ctrl = (struct caam_ctrl __iomem *)ctrlpriv->ctrl;
+	r4tst = &ctrl->r4tst[0];
 
 	/* put RNG4 into program mode */
 	setbits32(&r4tst->rtmctl, RTMCTL_PRGM);
@@ -355,10 +352,19 @@ static void kick_trng(struct platform_device *pdev, int ent_delay)
 	wr_reg32(&r4tst->rtsdctl, val);
 	/* min. freq. count, equal to 1/4 of the entropy sample length */
 	wr_reg32(&r4tst->rtfrqmin, ent_delay >> 2);
-	/* max. freq. count, equal to 8 times the entropy sample length */
-	wr_reg32(&r4tst->rtfrqmax, ent_delay << 3);
+	/* disable maximum frequency count */
+	wr_reg32(&r4tst->rtfrqmax, RTFRQMAX_DISABLE);
+	/* read the control register */
+	val = rd_reg32(&r4tst->rtmctl);
+	/*
+	 * select raw sampling in both entropy shifter
+	 * and statistical checker
+	 */
+	setbits32(&val, RTMCTL_SAMP_MODE_RAW_ES_SC);
 	/* put RNG4 into run mode */
-	clrbits32(&r4tst->rtmctl, RTMCTL_PRGM);
+	clrbits32(&val, RTMCTL_PRGM);
+	/* write back the control register */
+	wr_reg32(&r4tst->rtmctl, val);
 }
 
 /**
@@ -387,13 +393,14 @@ static int caam_probe(struct platform_device *pdev)
 	struct device *dev;
 	struct device_node *nprop, *np;
 	struct caam_ctrl __iomem *ctrl;
-	struct caam_full __iomem *topregs;
 	struct caam_drv_private *ctrlpriv;
 #ifdef CONFIG_DEBUG_FS
 	struct caam_perfmon *perfmon;
 #endif
 	u32 scfgr, comp_params;
 	u32 cha_vid_ls;
+	int pg_size;
+	int BLOCK_OFFSET = 0;
 
 	ctrlpriv = devm_kzalloc(&pdev->dev, sizeof(struct caam_drv_private),
 				GFP_KERNEL);
@@ -412,10 +419,27 @@ static int caam_probe(struct platform_device *pdev)
 		dev_err(dev, "caam: of_iomap() failed\n");
 		return -ENOMEM;
 	}
-	ctrlpriv->ctrl = (struct caam_ctrl __force *)ctrl;
+	/* Finding the page size for using the CTPR_MS register */
+	comp_params = rd_reg32(&ctrl->perfmon.comp_parms_ms);
+	pg_size = (comp_params & CTPR_MS_PG_SZ_MASK) >> CTPR_MS_PG_SZ_SHIFT;
 
-	/* topregs used to derive pointers to CAAM sub-blocks only */
-	topregs = (struct caam_full __iomem *)ctrl;
+	/* Allocating the BLOCK_OFFSET based on the supported page size on
+	 * the platform
+	 */
+	if (pg_size == 0)
+		BLOCK_OFFSET = PG_SIZE_4K;
+	else
+		BLOCK_OFFSET = PG_SIZE_64K;
+
+	ctrlpriv->ctrl = (struct caam_ctrl __force *)ctrl;
+	ctrlpriv->assure = (struct caam_assurance __force *)
+			   ((uint8_t *)ctrl +
+			    BLOCK_OFFSET * ASSURE_BLOCK_NUMBER
+			   );
+	ctrlpriv->deco = (struct caam_deco __force *)
+			 ((uint8_t *)ctrl +
+			 BLOCK_OFFSET * DECO_BLOCK_NUMBER
+			 );
 
 	/* Get the IRQ of the controller (for security violations only) */
 	ctrlpriv->secvio_irq = irq_of_parse_and_map(nprop, 0);
@@ -424,15 +448,14 @@ static int caam_probe(struct platform_device *pdev)
 	 * Enable DECO watchdogs and, if this is a PHYS_ADDR_T_64BIT kernel,
 	 * long pointers in master configuration register
 	 */
-	setbits32(&topregs->ctrl.mcr, MCFGR_WDENABLE |
+	setbits32(&ctrl->mcr, MCFGR_WDENABLE |
 		  (sizeof(dma_addr_t) == sizeof(u64) ? MCFGR_LONG_PTR : 0));
 
 	/*
 	 *  Read the Compile Time paramters and SCFGR to determine
 	 * if Virtualization is enabled for this platform
 	 */
-	comp_params = rd_reg32(&topregs->ctrl.perfmon.comp_parms_ms);
-	scfgr = rd_reg32(&topregs->ctrl.scfgr);
+	scfgr = rd_reg32(&ctrl->scfgr);
 
 	ctrlpriv->virt_en = 0;
 	if (comp_params & CTPR_MS_VIRT_EN_INCL) {
@@ -450,7 +473,7 @@ static int caam_probe(struct platform_device *pdev)
 	}
 
 	if (ctrlpriv->virt_en == 1)
-		setbits32(&topregs->ctrl.jrstart, JRSTART_JR0_START |
+		setbits32(&ctrl->jrstart, JRSTART_JR0_START |
 			  JRSTART_JR1_START | JRSTART_JR2_START |
 			  JRSTART_JR3_START);
 
@@ -477,7 +500,7 @@ static int caam_probe(struct platform_device *pdev)
 					sizeof(struct platform_device *) * rspec,
 					GFP_KERNEL);
 	if (ctrlpriv->jrpdev == NULL) {
-		iounmap(&topregs->ctrl);
+		iounmap(&ctrl);
 		return -ENOMEM;
 	}
 
@@ -493,18 +516,26 @@ static int caam_probe(struct platform_device *pdev)
 					ring);
 				continue;
 			}
+			ctrlpriv->jr[ring] = (struct caam_job_ring __force *)
+					     ((uint8_t *)ctrl +
+					     (ring + JR_BLOCK_NUMBER) *
+					      BLOCK_OFFSET
+					     );
 			ctrlpriv->total_jobrs++;
 			ring++;
-		}
+	}
 
 	/* Check to see if QI present. If so, enable */
 	ctrlpriv->qi_present =
-			!!(rd_reg32(&topregs->ctrl.perfmon.comp_parms_ms) &
+			!!(rd_reg32(&ctrl->perfmon.comp_parms_ms) &
 			   CTPR_MS_QI_MASK);
 	if (ctrlpriv->qi_present) {
-		ctrlpriv->qi = (struct caam_queue_if __force *)&topregs->qi;
+		ctrlpriv->qi = (struct caam_queue_if __force *)
+			       ((uint8_t *)ctrl +
+				 BLOCK_OFFSET * QI_BLOCK_NUMBER
+			       );
 		/* This is all that's required to physically enable QI */
-		wr_reg32(&topregs->qi.qi_control_lo, QICTL_DQEN);
+		wr_reg32(&ctrlpriv->qi->qi_control_lo, QICTL_DQEN);
 	}
 
 	/* If no QI and no rings specified, quit and go home */
@@ -514,7 +545,7 @@ static int caam_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	cha_vid_ls = rd_reg32(&topregs->ctrl.perfmon.cha_id_ls);
+	cha_vid_ls = rd_reg32(&ctrl->perfmon.cha_id_ls);
 
 	/*
 	 * If SEC has RNG version >= 4 and RNG state handle has not been
@@ -522,7 +553,7 @@ static int caam_probe(struct platform_device *pdev)
 	 */
 	if ((cha_vid_ls & CHA_ID_LS_RNG_MASK) >> CHA_ID_LS_RNG_SHIFT >= 4) {
 		ctrlpriv->rng4_sh_init =
-			rd_reg32(&topregs->ctrl.r4tst[0].rdsta);
+			rd_reg32(&ctrl->r4tst[0].rdsta);
 		/*
 		 * If the secure keys (TDKEK, JDKEK, TDSK), were already
 		 * generated, signal this to the function that is instantiating
@@ -533,7 +564,7 @@ static int caam_probe(struct platform_device *pdev)
 		ctrlpriv->rng4_sh_init &= RDSTA_IFMASK;
 		do {
 			int inst_handles =
-				rd_reg32(&topregs->ctrl.r4tst[0].rdsta) &
+				rd_reg32(&ctrl->r4tst[0].rdsta) &
 								RDSTA_IFMASK;
 			/*
 			 * If either SH were instantiated by somebody else
@@ -544,6 +575,9 @@ static int caam_probe(struct platform_device *pdev)
 			 * the TRNG parameters.
 			 */
 			if (!(ctrlpriv->rng4_sh_init || inst_handles)) {
+				dev_info(dev,
+					 "Entropy delay = %u\n",
+					 ent_delay);
 				kick_trng(pdev, ent_delay);
 				ent_delay += 400;
 			}
@@ -556,6 +590,12 @@ static int caam_probe(struct platform_device *pdev)
 			 */
 			ret = instantiate_rng(dev, inst_handles,
 					      gen_sk);
+			if (ret == -EAGAIN)
+				/*
+				 * if here, the loop will rerun,
+				 * so don't hog the CPU
+				 */
+				cpu_relax();
 		} while ((ret == -EAGAIN) && (ent_delay < RTSDCTL_ENT_DLY_MAX));
 		if (ret) {
 			dev_err(dev, "failed to instantiate RNG");
@@ -569,13 +609,13 @@ static int caam_probe(struct platform_device *pdev)
 		ctrlpriv->rng4_sh_init = ~ctrlpriv->rng4_sh_init & RDSTA_IFMASK;
 
 		/* Enable RDB bit so that RNG works faster */
-		setbits32(&topregs->ctrl.scfgr, SCFGR_RDBENABLE);
+		setbits32(&ctrl->scfgr, SCFGR_RDBENABLE);
 	}
 
 	/* NOTE: RTIC detection ought to go here, around Si time */
 
-	caam_id = (u64)rd_reg32(&topregs->ctrl.perfmon.caam_id_ms) << 32 |
-		  (u64)rd_reg32(&topregs->ctrl.perfmon.caam_id_ls);
+	caam_id = (u64)rd_reg32(&ctrl->perfmon.caam_id_ms) << 32 |
+		  (u64)rd_reg32(&ctrl->perfmon.caam_id_ls);
 
 	/* Report "alive" for developer to see */
 	dev_info(dev, "device ID = 0x%016llx (Era %d)\n", caam_id,
diff --git a/drivers/crypto/caam/intern.h b/drivers/crypto/caam/intern.h
index 97363db4e56e..89b94cc9e7a2 100644
--- a/drivers/crypto/caam/intern.h
+++ b/drivers/crypto/caam/intern.h
@@ -70,10 +70,11 @@ struct caam_drv_private {
 	struct platform_device *pdev;
 
 	/* Physical-presence section */
-	struct caam_ctrl *ctrl; /* controller region */
-	struct caam_deco **deco; /* DECO/CCB views */
-	struct caam_assurance *ac;
-	struct caam_queue_if *qi; /* QI control region */
+	struct caam_ctrl __iomem *ctrl; /* controller region */
+	struct caam_deco __iomem *deco; /* DECO/CCB views */
+	struct caam_assurance __iomem *assure;
+	struct caam_queue_if __iomem *qi; /* QI control region */
+	struct caam_job_ring __iomem *jr[4];	/* JobR's register space */
 
 	/*
 	 * Detected geometry block. Filled in from device tree if powerpc,
diff --git a/drivers/crypto/caam/regs.h b/drivers/crypto/caam/regs.h
index f48e344ffc39..378ddc17f60e 100644
--- a/drivers/crypto/caam/regs.h
+++ b/drivers/crypto/caam/regs.h
@@ -194,6 +194,8 @@ struct caam_perfmon {
 #define CTPR_MS_QI_MASK		(0x1ull << CTPR_MS_QI_SHIFT)
 #define CTPR_MS_VIRT_EN_INCL	0x00000001
 #define CTPR_MS_VIRT_EN_POR	0x00000002
+#define CTPR_MS_PG_SZ_MASK	0x10
+#define CTPR_MS_PG_SZ_SHIFT	4
 	u32 comp_parms_ms;	/* CTPR - Compile Parameters Register	*/
 	u32 comp_parms_ls;	/* CTPR - Compile Parameters Register	*/
 	u64 rsvd1[2];
@@ -269,6 +271,16 @@ struct rngtst {
 /* RNG4 TRNG test registers */
 struct rng4tst {
 #define RTMCTL_PRGM	0x00010000	/* 1 -> program mode, 0 -> run mode */
+#define RTMCTL_SAMP_MODE_VON_NEUMANN_ES_SC	0 /* use von Neumann data in
+						     both entropy shifter and
+						     statistical checker */
+#define RTMCTL_SAMP_MODE_RAW_ES_SC		1 /* use raw data in both
+						     entropy shifter and
+						     statistical checker */
+#define RTMCTL_SAMP_MODE_VON_NEUMANN_ES_RAW_SC	2 /* use von Neumann data in
+						     entropy shifter, raw data
+						     in statistical checker */
+#define RTMCTL_SAMP_MODE_INVALID		3 /* invalid combination */
 	u32 rtmctl;		/* misc. control register */
 	u32 rtscmisc;		/* statistical check misc. register */
 	u32 rtpkrrng;		/* poker range register */
@@ -278,7 +290,7 @@ struct rng4tst {
 	};
 #define RTSDCTL_ENT_DLY_SHIFT 16
 #define RTSDCTL_ENT_DLY_MASK (0xffff << RTSDCTL_ENT_DLY_SHIFT)
-#define RTSDCTL_ENT_DLY_MIN 1200
+#define RTSDCTL_ENT_DLY_MIN 3200
 #define RTSDCTL_ENT_DLY_MAX 12800
 	u32 rtsdctl;		/* seed control register */
 	union {
@@ -286,6 +298,7 @@ struct rng4tst {
 		u32 rttotsam;	/* PRGM=0: total samples register */
 	};
 	u32 rtfrqmin;		/* frequency count min. limit register */
+#define RTFRQMAX_DISABLE	(1 << 20)
 	union {
 		u32 rtfrqmax;	/* PRGM=1: freq. count max. limit register */
 		u32 rtfrqcnt;	/* PRGM=0: freq. count register */
@@ -758,34 +771,10 @@ struct caam_deco {
 #define DECO_JQCR_WHL		0x20000000
 #define DECO_JQCR_FOUR		0x10000000
 
-/*
- * Current top-level view of memory map is:
- *
- * 0x0000 - 0x0fff - CAAM Top-Level Control
- * 0x1000 - 0x1fff - Job Ring 0
- * 0x2000 - 0x2fff - Job Ring 1
- * 0x3000 - 0x3fff - Job Ring 2
- * 0x4000 - 0x4fff - Job Ring 3
- * 0x5000 - 0x5fff - (unused)
- * 0x6000 - 0x6fff - Assurance Controller
- * 0x7000 - 0x7fff - Queue Interface
- * 0x8000 - 0x8fff - DECO-CCB 0
- * 0x9000 - 0x9fff - DECO-CCB 1
- * 0xa000 - 0xafff - DECO-CCB 2
- * 0xb000 - 0xbfff - DECO-CCB 3
- * 0xc000 - 0xcfff - DECO-CCB 4
- *
- * caam_full describes the full register view of CAAM if useful,
- * although many configurations may choose to implement parts of
- * the register map separately, in differing privilege regions
- */
-struct caam_full {
-	struct caam_ctrl __iomem ctrl;
-	struct caam_job_ring jr[4];
-	u64 rsvd[512];
-	struct caam_assurance assure;
-	struct caam_queue_if qi;
-	struct caam_deco deco;
-};
-
+#define JR_BLOCK_NUMBER		1
+#define ASSURE_BLOCK_NUMBER	6
+#define QI_BLOCK_NUMBER		7
+#define DECO_BLOCK_NUMBER	8
+#define PG_SIZE_4K		0x1000
+#define PG_SIZE_64K		0x10000
 #endif /* REGS_H */
diff --git a/drivers/crypto/caam/sg_sw_sec4.h b/drivers/crypto/caam/sg_sw_sec4.h
index b12ff85f4241..ce28a563effc 100644
--- a/drivers/crypto/caam/sg_sw_sec4.h
+++ b/drivers/crypto/caam/sg_sw_sec4.h
@@ -116,57 +116,3 @@ static int dma_unmap_sg_chained(struct device *dev, struct scatterlist *sg,
 	}
 	return nents;
 }
-
-/* Map SG page in kernel virtual address space and copy */
-static inline void sg_map_copy(u8 *dest, struct scatterlist *sg,
-			       int len, int offset)
-{
-	u8 *mapped_addr;
-
-	/*
-	 * Page here can be user-space pinned using get_user_pages
-	 * Same must be kmapped before use and kunmapped subsequently
-	 */
-	mapped_addr = kmap_atomic(sg_page(sg));
-	memcpy(dest, mapped_addr + offset, len);
-	kunmap_atomic(mapped_addr);
-}
-
-/* Copy from len bytes of sg to dest, starting from beginning */
-static inline void sg_copy(u8 *dest, struct scatterlist *sg, unsigned int len)
-{
-	struct scatterlist *current_sg = sg;
-	int cpy_index = 0, next_cpy_index = current_sg->length;
-
-	while (next_cpy_index < len) {
-		sg_map_copy(dest + cpy_index, current_sg, current_sg->length,
-			    current_sg->offset);
-		current_sg = scatterwalk_sg_next(current_sg);
-		cpy_index = next_cpy_index;
-		next_cpy_index += current_sg->length;
-	}
-	if (cpy_index < len)
-		sg_map_copy(dest + cpy_index, current_sg, len-cpy_index,
-			    current_sg->offset);
-}
-
-/* Copy sg data, from to_skip to end, to dest */
-static inline void sg_copy_part(u8 *dest, struct scatterlist *sg,
-				      int to_skip, unsigned int end)
-{
-	struct scatterlist *current_sg = sg;
-	int sg_index, cpy_index, offset;
-
-	sg_index = current_sg->length;
-	while (sg_index <= to_skip) {
-		current_sg = scatterwalk_sg_next(current_sg);
-		sg_index += current_sg->length;
-	}
-	cpy_index = sg_index - to_skip;
-	offset = current_sg->offset + current_sg->length - cpy_index;
-	sg_map_copy(dest, current_sg, cpy_index, offset);
-	if (end - sg_index) {
-		current_sg = scatterwalk_sg_next(current_sg);
-		sg_copy(dest + cpy_index, current_sg, end - sg_index);
-	}
-}
diff --git a/drivers/crypto/ccp/ccp-crypto-main.c b/drivers/crypto/ccp/ccp-crypto-main.c
index 20dc848481e7..4d4e016d755b 100644
--- a/drivers/crypto/ccp/ccp-crypto-main.c
+++ b/drivers/crypto/ccp/ccp-crypto-main.c
@@ -367,6 +367,10 @@ static int ccp_crypto_init(void)
 {
 	int ret;
 
+	ret = ccp_present();
+	if (ret)
+		return ret;
+
 	spin_lock_init(&req_queue_lock);
 	INIT_LIST_HEAD(&req_queue.cmds);
 	req_queue.backlog = &req_queue.cmds;
diff --git a/drivers/crypto/ccp/ccp-crypto-sha.c b/drivers/crypto/ccp/ccp-crypto-sha.c
index 873f23425245..96531571f7cf 100644
--- a/drivers/crypto/ccp/ccp-crypto-sha.c
+++ b/drivers/crypto/ccp/ccp-crypto-sha.c
@@ -198,10 +198,9 @@ static int ccp_sha_setkey(struct crypto_ahash *tfm, const u8 *key,
 {
 	struct ccp_ctx *ctx = crypto_tfm_ctx(crypto_ahash_tfm(tfm));
 	struct crypto_shash *shash = ctx->u.sha.hmac_tfm;
-	struct {
-		struct shash_desc sdesc;
-		char ctx[crypto_shash_descsize(shash)];
-	} desc;
+
+	SHASH_DESC_ON_STACK(sdesc, shash);
+
 	unsigned int block_size = crypto_shash_blocksize(shash);
 	unsigned int digest_size = crypto_shash_digestsize(shash);
 	int i, ret;
@@ -216,11 +215,11 @@ static int ccp_sha_setkey(struct crypto_ahash *tfm, const u8 *key,
 
 	if (key_len > block_size) {
 		/* Must hash the input key */
-		desc.sdesc.tfm = shash;
-		desc.sdesc.flags = crypto_ahash_get_flags(tfm) &
+		sdesc->tfm = shash;
+		sdesc->flags = crypto_ahash_get_flags(tfm) &
 			CRYPTO_TFM_REQ_MAY_SLEEP;
 
-		ret = crypto_shash_digest(&desc.sdesc, key, key_len,
+		ret = crypto_shash_digest(sdesc, key, key_len,
 					  ctx->u.sha.key);
 		if (ret) {
 			crypto_ahash_set_flags(tfm, CRYPTO_TFM_RES_BAD_KEY_LEN);
diff --git a/drivers/crypto/ccp/ccp-dev.c b/drivers/crypto/ccp/ccp-dev.c
index a7d110652a74..c6e6171eb6d3 100644
--- a/drivers/crypto/ccp/ccp-dev.c
+++ b/drivers/crypto/ccp/ccp-dev.c
@@ -55,6 +55,20 @@ static inline void ccp_del_device(struct ccp_device *ccp)
 }
 
 /**
+ * ccp_present - check if a CCP device is present
+ *
+ * Returns zero if a CCP device is present, -ENODEV otherwise.
+ */
+int ccp_present(void)
+{
+	if (ccp_get_device())
+		return 0;
+
+	return -ENODEV;
+}
+EXPORT_SYMBOL_GPL(ccp_present);
+
+/**
  * ccp_enqueue_cmd - queue an operation for processing by the CCP
  *
  * @cmd: ccp_cmd struct to be processed
diff --git a/drivers/crypto/mv_cesa.c b/drivers/crypto/mv_cesa.c
index 29d0ee504907..032c72c1f953 100644
--- a/drivers/crypto/mv_cesa.c
+++ b/drivers/crypto/mv_cesa.c
@@ -402,26 +402,23 @@ static int mv_hash_final_fallback(struct ahash_request *req)
 {
 	const struct mv_tfm_hash_ctx *tfm_ctx = crypto_tfm_ctx(req->base.tfm);
 	struct mv_req_hash_ctx *req_ctx = ahash_request_ctx(req);
-	struct {
-		struct shash_desc shash;
-		char ctx[crypto_shash_descsize(tfm_ctx->fallback)];
-	} desc;
+	SHASH_DESC_ON_STACK(shash, tfm_ctx->fallback);
 	int rc;
 
-	desc.shash.tfm = tfm_ctx->fallback;
-	desc.shash.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+	shash->tfm = tfm_ctx->fallback;
+	shash->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
 	if (unlikely(req_ctx->first_hash)) {
-		crypto_shash_init(&desc.shash);
-		crypto_shash_update(&desc.shash, req_ctx->buffer,
+		crypto_shash_init(shash);
+		crypto_shash_update(shash, req_ctx->buffer,
 				    req_ctx->extra_bytes);
 	} else {
 		/* only SHA1 for now....
 		 */
-		rc = mv_hash_import_sha1_ctx(req_ctx, &desc.shash);
+		rc = mv_hash_import_sha1_ctx(req_ctx, shash);
 		if (rc)
 			goto out;
 	}
-	rc = crypto_shash_final(&desc.shash, req->result);
+	rc = crypto_shash_final(shash, req->result);
 out:
 	return rc;
 }
@@ -794,23 +791,21 @@ static int mv_hash_setkey(struct crypto_ahash *tfm, const u8 * key,
 	ss = crypto_shash_statesize(ctx->base_hash);
 
 	{
-		struct {
-			struct shash_desc shash;
-			char ctx[crypto_shash_descsize(ctx->base_hash)];
-		} desc;
+		SHASH_DESC_ON_STACK(shash, ctx->base_hash);
+
 		unsigned int i;
 		char ipad[ss];
 		char opad[ss];
 
-		desc.shash.tfm = ctx->base_hash;
-		desc.shash.flags = crypto_shash_get_flags(ctx->base_hash) &
+		shash->tfm = ctx->base_hash;
+		shash->flags = crypto_shash_get_flags(ctx->base_hash) &
 		    CRYPTO_TFM_REQ_MAY_SLEEP;
 
 		if (keylen > bs) {
 			int err;
 
 			err =
-			    crypto_shash_digest(&desc.shash, key, keylen, ipad);
+			    crypto_shash_digest(shash, key, keylen, ipad);
 			if (err)
 				return err;
 
@@ -826,12 +821,12 @@ static int mv_hash_setkey(struct crypto_ahash *tfm, const u8 * key,
 			opad[i] ^= 0x5c;
 		}
 
-		rc = crypto_shash_init(&desc.shash) ? :
-		    crypto_shash_update(&desc.shash, ipad, bs) ? :
-		    crypto_shash_export(&desc.shash, ipad) ? :
-		    crypto_shash_init(&desc.shash) ? :
-		    crypto_shash_update(&desc.shash, opad, bs) ? :
-		    crypto_shash_export(&desc.shash, opad);
+		rc = crypto_shash_init(shash) ? :
+		    crypto_shash_update(shash, ipad, bs) ? :
+		    crypto_shash_export(shash, ipad) ? :
+		    crypto_shash_init(shash) ? :
+		    crypto_shash_update(shash, opad, bs) ? :
+		    crypto_shash_export(shash, opad);
 
 		if (rc == 0)
 			mv_hash_init_ivs(ctx, ipad, opad);
diff --git a/drivers/crypto/mv_cesa.h b/drivers/crypto/mv_cesa.h
index 08fcb1116d90..9249d3ed184b 100644
--- a/drivers/crypto/mv_cesa.h
+++ b/drivers/crypto/mv_cesa.h
@@ -1,4 +1,5 @@
 #ifndef __MV_CRYPTO_H__
+#define __MV_CRYPTO_H__
 
 #define DIGEST_INITIAL_VAL_A	0xdd00
 #define DIGEST_INITIAL_VAL_B	0xdd04
diff --git a/drivers/crypto/n2_core.c b/drivers/crypto/n2_core.c
index 7263c10a56ee..f8e3207fecb1 100644
--- a/drivers/crypto/n2_core.c
+++ b/drivers/crypto/n2_core.c
@@ -445,10 +445,7 @@ static int n2_hmac_async_setkey(struct crypto_ahash *tfm, const u8 *key,
 	struct n2_hmac_ctx *ctx = crypto_ahash_ctx(tfm);
 	struct crypto_shash *child_shash = ctx->child_shash;
 	struct crypto_ahash *fallback_tfm;
-	struct {
-		struct shash_desc shash;
-		char ctx[crypto_shash_descsize(child_shash)];
-	} desc;
+	SHASH_DESC_ON_STACK(shash, child_shash);
 	int err, bs, ds;
 
 	fallback_tfm = ctx->base.fallback_tfm;
@@ -456,15 +453,15 @@ static int n2_hmac_async_setkey(struct crypto_ahash *tfm, const u8 *key,
 	if (err)
 		return err;
 
-	desc.shash.tfm = child_shash;
-	desc.shash.flags = crypto_ahash_get_flags(tfm) &
+	shash->tfm = child_shash;
+	shash->flags = crypto_ahash_get_flags(tfm) &
 		CRYPTO_TFM_REQ_MAY_SLEEP;
 
 	bs = crypto_shash_blocksize(child_shash);
 	ds = crypto_shash_digestsize(child_shash);
 	BUG_ON(ds > N2_HASH_KEY_MAX);
 	if (keylen > bs) {
-		err = crypto_shash_digest(&desc.shash, key, keylen,
+		err = crypto_shash_digest(shash, key, keylen,
 					  ctx->hash_key);
 		if (err)
 			return err;
diff --git a/drivers/crypto/omap-sham.c b/drivers/crypto/omap-sham.c
index 710d86386965..24ef48965e45 100644
--- a/drivers/crypto/omap-sham.c
+++ b/drivers/crypto/omap-sham.c
@@ -949,17 +949,14 @@ static int omap_sham_finish_hmac(struct ahash_request *req)
 	struct omap_sham_hmac_ctx *bctx = tctx->base;
 	int bs = crypto_shash_blocksize(bctx->shash);
 	int ds = crypto_shash_digestsize(bctx->shash);
-	struct {
-		struct shash_desc shash;
-		char ctx[crypto_shash_descsize(bctx->shash)];
-	} desc;
+	SHASH_DESC_ON_STACK(shash, bctx->shash);
 
-	desc.shash.tfm = bctx->shash;
-	desc.shash.flags = 0; /* not CRYPTO_TFM_REQ_MAY_SLEEP */
+	shash->tfm = bctx->shash;
+	shash->flags = 0; /* not CRYPTO_TFM_REQ_MAY_SLEEP */
 
-	return crypto_shash_init(&desc.shash) ?:
-	       crypto_shash_update(&desc.shash, bctx->opad, bs) ?:
-	       crypto_shash_finup(&desc.shash, req->result, ds, req->result);
+	return crypto_shash_init(shash) ?:
+	       crypto_shash_update(shash, bctx->opad, bs) ?:
+	       crypto_shash_finup(shash, req->result, ds, req->result);
 }
 
 static int omap_sham_finish(struct ahash_request *req)
@@ -1118,18 +1115,15 @@ static int omap_sham_update(struct ahash_request *req)
 	return omap_sham_enqueue(req, OP_UPDATE);
 }
 
-static int omap_sham_shash_digest(struct crypto_shash *shash, u32 flags,
+static int omap_sham_shash_digest(struct crypto_shash *tfm, u32 flags,
 				  const u8 *data, unsigned int len, u8 *out)
 {
-	struct {
-		struct shash_desc shash;
-		char ctx[crypto_shash_descsize(shash)];
-	} desc;
+	SHASH_DESC_ON_STACK(shash, tfm);
 
-	desc.shash.tfm = shash;
-	desc.shash.flags = flags & CRYPTO_TFM_REQ_MAY_SLEEP;
+	shash->tfm = tfm;
+	shash->flags = flags & CRYPTO_TFM_REQ_MAY_SLEEP;
 
-	return crypto_shash_digest(&desc.shash, data, len, out);
+	return crypto_shash_digest(shash, data, len, out);
 }
 
 static int omap_sham_final_shash(struct ahash_request *req)
diff --git a/drivers/crypto/qat/qat_common/adf_ctl_drv.c b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
index d97069b8a8e4..244d73378f0e 100644
--- a/drivers/crypto/qat/qat_common/adf_ctl_drv.c
+++ b/drivers/crypto/qat/qat_common/adf_ctl_drv.c
@@ -111,7 +111,7 @@ static int adf_chr_drv_create(void)
 	drv_device = device_create(adt_ctl_drv.drv_class, NULL,
 				   MKDEV(adt_ctl_drv.major, 0),
 				   NULL, DEVICE_NAME);
-	if (!drv_device) {
+	if (IS_ERR(drv_device)) {
 		pr_err("QAT: failed to create device\n");
 		goto err_cdev_del;
 	}
@@ -436,7 +436,7 @@ static long adf_ctl_ioctl(struct file *fp, unsigned int cmd, unsigned long arg)
 		ret = adf_ctl_ioctl_get_status(fp, cmd, arg);
 		break;
 	default:
-		pr_err("QAT: Invalid ioclt\n");
+		pr_err("QAT: Invalid ioctl\n");
 		ret = -EFAULT;
 		break;
 	}
diff --git a/drivers/crypto/qat/qat_common/adf_transport_internal.h b/drivers/crypto/qat/qat_common/adf_transport_internal.h
index f854bac276b0..c40546079981 100644
--- a/drivers/crypto/qat/qat_common/adf_transport_internal.h
+++ b/drivers/crypto/qat/qat_common/adf_transport_internal.h
@@ -75,7 +75,7 @@ struct adf_etr_ring_data {
 
 struct adf_etr_bank_data {
 	struct adf_etr_ring_data rings[ADF_ETR_MAX_RINGS_PER_BANK];
-	struct tasklet_struct resp_hanlder;
+	struct tasklet_struct resp_handler;
 	void __iomem *csr_addr;
 	struct adf_accel_dev *accel_dev;
 	uint32_t irq_coalesc_timer;
diff --git a/drivers/crypto/qat/qat_common/qat_algs.c b/drivers/crypto/qat/qat_common/qat_algs.c
index 59df48872955..f2e2f158cfbe 100644
--- a/drivers/crypto/qat/qat_common/qat_algs.c
+++ b/drivers/crypto/qat/qat_common/qat_algs.c
@@ -105,7 +105,7 @@ struct qat_alg_cd {
 #define MAX_AUTH_STATE_SIZE sizeof(struct icp_qat_hw_auth_algo_blk)
 
 struct qat_auth_state {
-	uint8_t data[MAX_AUTH_STATE_SIZE];
+	uint8_t data[MAX_AUTH_STATE_SIZE + 64];
 } __aligned(64);
 
 struct qat_alg_session_ctx {
@@ -113,10 +113,6 @@ struct qat_alg_session_ctx {
 	dma_addr_t enc_cd_paddr;
 	struct qat_alg_cd *dec_cd;
 	dma_addr_t dec_cd_paddr;
-	struct qat_auth_state *auth_hw_state_enc;
-	dma_addr_t auth_state_enc_paddr;
-	struct qat_auth_state *auth_hw_state_dec;
-	dma_addr_t auth_state_dec_paddr;
 	struct icp_qat_fw_la_bulk_req enc_fw_req_tmpl;
 	struct icp_qat_fw_la_bulk_req dec_fw_req_tmpl;
 	struct qat_crypto_instance *inst;
@@ -150,29 +146,28 @@ static int qat_get_inter_state_size(enum icp_qat_hw_auth_algo qat_hash_alg)
 static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 				  struct qat_alg_session_ctx *ctx,
 				  const uint8_t *auth_key,
-				  unsigned int auth_keylen, uint8_t *auth_state)
+				  unsigned int auth_keylen)
 {
-	struct {
-		struct shash_desc shash;
-		char ctx[crypto_shash_descsize(ctx->hash_tfm)];
-	} desc;
+	struct qat_auth_state auth_state;
+	SHASH_DESC_ON_STACK(shash, ctx->hash_tfm);
 	struct sha1_state sha1;
 	struct sha256_state sha256;
 	struct sha512_state sha512;
 	int block_size = crypto_shash_blocksize(ctx->hash_tfm);
 	int digest_size = crypto_shash_digestsize(ctx->hash_tfm);
-	uint8_t *ipad = auth_state;
+	uint8_t *ipad = auth_state.data;
 	uint8_t *opad = ipad + block_size;
 	__be32 *hash_state_out;
 	__be64 *hash512_state_out;
 	int i, offset;
 
-	desc.shash.tfm = ctx->hash_tfm;
-	desc.shash.flags = 0x0;
+	memset(auth_state.data, '\0', MAX_AUTH_STATE_SIZE + 64);
+	shash->tfm = ctx->hash_tfm;
+	shash->flags = 0x0;
 
 	if (auth_keylen > block_size) {
 		char buff[SHA512_BLOCK_SIZE];
-		int ret = crypto_shash_digest(&desc.shash, auth_key,
+		int ret = crypto_shash_digest(shash, auth_key,
 					      auth_keylen, buff);
 		if (ret)
 			return ret;
@@ -195,10 +190,10 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 		*opad_ptr ^= 0x5C;
 	}
 
-	if (crypto_shash_init(&desc.shash))
+	if (crypto_shash_init(shash))
 		return -EFAULT;
 
-	if (crypto_shash_update(&desc.shash, ipad, block_size))
+	if (crypto_shash_update(shash, ipad, block_size))
 		return -EFAULT;
 
 	hash_state_out = (__be32 *)hash->sha.state1;
@@ -206,19 +201,19 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 
 	switch (ctx->qat_hash_alg) {
 	case ICP_QAT_HW_AUTH_ALGO_SHA1:
-		if (crypto_shash_export(&desc.shash, &sha1))
+		if (crypto_shash_export(shash, &sha1))
 			return -EFAULT;
 		for (i = 0; i < digest_size >> 2; i++, hash_state_out++)
 			*hash_state_out = cpu_to_be32(*(sha1.state + i));
 		break;
 	case ICP_QAT_HW_AUTH_ALGO_SHA256:
-		if (crypto_shash_export(&desc.shash, &sha256))
+		if (crypto_shash_export(shash, &sha256))
 			return -EFAULT;
 		for (i = 0; i < digest_size >> 2; i++, hash_state_out++)
 			*hash_state_out = cpu_to_be32(*(sha256.state + i));
 		break;
 	case ICP_QAT_HW_AUTH_ALGO_SHA512:
-		if (crypto_shash_export(&desc.shash, &sha512))
+		if (crypto_shash_export(shash, &sha512))
 			return -EFAULT;
 		for (i = 0; i < digest_size >> 3; i++, hash512_state_out++)
 			*hash512_state_out = cpu_to_be64(*(sha512.state + i));
@@ -227,10 +222,10 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 		return -EFAULT;
 	}
 
-	if (crypto_shash_init(&desc.shash))
+	if (crypto_shash_init(shash))
 		return -EFAULT;
 
-	if (crypto_shash_update(&desc.shash, opad, block_size))
+	if (crypto_shash_update(shash, opad, block_size))
 		return -EFAULT;
 
 	offset = round_up(qat_get_inter_state_size(ctx->qat_hash_alg), 8);
@@ -239,19 +234,19 @@ static int qat_alg_do_precomputes(struct icp_qat_hw_auth_algo_blk *hash,
 
 	switch (ctx->qat_hash_alg) {
 	case ICP_QAT_HW_AUTH_ALGO_SHA1:
-		if (crypto_shash_export(&desc.shash, &sha1))
+		if (crypto_shash_export(shash, &sha1))
 			return -EFAULT;
 		for (i = 0; i < digest_size >> 2; i++, hash_state_out++)
 			*hash_state_out = cpu_to_be32(*(sha1.state + i));
 		break;
 	case ICP_QAT_HW_AUTH_ALGO_SHA256:
-		if (crypto_shash_export(&desc.shash, &sha256))
+		if (crypto_shash_export(shash, &sha256))
 			return -EFAULT;
 		for (i = 0; i < digest_size >> 2; i++, hash_state_out++)
 			*hash_state_out = cpu_to_be32(*(sha256.state + i));
 		break;
 	case ICP_QAT_HW_AUTH_ALGO_SHA512:
-		if (crypto_shash_export(&desc.shash, &sha512))
+		if (crypto_shash_export(shash, &sha512))
 			return -EFAULT;
 		for (i = 0; i < digest_size >> 3; i++, hash512_state_out++)
 			*hash512_state_out = cpu_to_be64(*(sha512.state + i));
@@ -298,10 +293,6 @@ static int qat_alg_init_enc_session(struct qat_alg_session_ctx *ctx,
 	void *ptr = &req_tmpl->cd_ctrl;
 	struct icp_qat_fw_cipher_cd_ctrl_hdr *cipher_cd_ctrl = ptr;
 	struct icp_qat_fw_auth_cd_ctrl_hdr *hash_cd_ctrl = ptr;
-	struct icp_qat_fw_la_auth_req_params *auth_param =
-		(struct icp_qat_fw_la_auth_req_params *)
-		((char *)&req_tmpl->serv_specif_rqpars +
-		 sizeof(struct icp_qat_fw_la_cipher_req_params));
 
 	/* CD setup */
 	cipher->aes.cipher_config.val = QAT_AES_HW_CONFIG_ENC(alg);
@@ -312,8 +303,7 @@ static int qat_alg_init_enc_session(struct qat_alg_session_ctx *ctx,
 	hash->sha.inner_setup.auth_counter.counter =
 		cpu_to_be32(crypto_shash_blocksize(ctx->hash_tfm));
 
-	if (qat_alg_do_precomputes(hash, ctx, keys->authkey, keys->authkeylen,
-				   (uint8_t *)ctx->auth_hw_state_enc))
+	if (qat_alg_do_precomputes(hash, ctx, keys->authkey, keys->authkeylen))
 		return -EFAULT;
 
 	/* Request setup */
@@ -359,9 +349,6 @@ static int qat_alg_init_enc_session(struct qat_alg_session_ctx *ctx,
 	hash_cd_ctrl->inner_state2_offset = hash_cd_ctrl->hash_cfg_offset +
 			((sizeof(struct icp_qat_hw_auth_setup) +
 			 round_up(hash_cd_ctrl->inner_state1_sz, 8)) >> 3);
-	auth_param->u1.auth_partial_st_prefix = ctx->auth_state_enc_paddr +
-			sizeof(struct icp_qat_hw_auth_counter) +
-			round_up(hash_cd_ctrl->inner_state1_sz, 8);
 	ICP_QAT_FW_COMN_CURR_ID_SET(hash_cd_ctrl, ICP_QAT_FW_SLICE_AUTH);
 	ICP_QAT_FW_COMN_NEXT_ID_SET(hash_cd_ctrl, ICP_QAT_FW_SLICE_DRAM_WR);
 	return 0;
@@ -399,8 +386,7 @@ static int qat_alg_init_dec_session(struct qat_alg_session_ctx *ctx,
 	hash->sha.inner_setup.auth_counter.counter =
 		cpu_to_be32(crypto_shash_blocksize(ctx->hash_tfm));
 
-	if (qat_alg_do_precomputes(hash, ctx, keys->authkey, keys->authkeylen,
-				   (uint8_t *)ctx->auth_hw_state_dec))
+	if (qat_alg_do_precomputes(hash, ctx, keys->authkey, keys->authkeylen))
 		return -EFAULT;
 
 	/* Request setup */
@@ -450,9 +436,6 @@ static int qat_alg_init_dec_session(struct qat_alg_session_ctx *ctx,
 	hash_cd_ctrl->inner_state2_offset = hash_cd_ctrl->hash_cfg_offset +
 			((sizeof(struct icp_qat_hw_auth_setup) +
 			 round_up(hash_cd_ctrl->inner_state1_sz, 8)) >> 3);
-	auth_param->u1.auth_partial_st_prefix = ctx->auth_state_enc_paddr +
-			sizeof(struct icp_qat_hw_auth_counter) +
-			round_up(hash_cd_ctrl->inner_state1_sz, 8);
 	auth_param->auth_res_sz = digestsize;
 	ICP_QAT_FW_COMN_CURR_ID_SET(hash_cd_ctrl, ICP_QAT_FW_SLICE_AUTH);
 	ICP_QAT_FW_COMN_NEXT_ID_SET(hash_cd_ctrl, ICP_QAT_FW_SLICE_CIPHER);
@@ -512,10 +495,6 @@ static int qat_alg_setkey(struct crypto_aead *tfm, const uint8_t *key,
 		dev = &GET_DEV(ctx->inst->accel_dev);
 		memset(ctx->enc_cd, 0, sizeof(struct qat_alg_cd));
 		memset(ctx->dec_cd, 0, sizeof(struct qat_alg_cd));
-		memset(ctx->auth_hw_state_enc, 0,
-		       sizeof(struct qat_auth_state));
-		memset(ctx->auth_hw_state_dec, 0,
-		       sizeof(struct qat_auth_state));
 		memset(&ctx->enc_fw_req_tmpl, 0,
 		       sizeof(struct icp_qat_fw_la_bulk_req));
 		memset(&ctx->dec_fw_req_tmpl, 0,
@@ -548,22 +527,6 @@ static int qat_alg_setkey(struct crypto_aead *tfm, const uint8_t *key,
 			spin_unlock(&ctx->lock);
 			goto out_free_enc;
 		}
-		ctx->auth_hw_state_enc =
-			dma_zalloc_coherent(dev, sizeof(struct qat_auth_state),
-					    &ctx->auth_state_enc_paddr,
-					    GFP_ATOMIC);
-		if (!ctx->auth_hw_state_enc) {
-			spin_unlock(&ctx->lock);
-			goto out_free_dec;
-		}
-		ctx->auth_hw_state_dec =
-			dma_zalloc_coherent(dev, sizeof(struct qat_auth_state),
-					    &ctx->auth_state_dec_paddr,
-					    GFP_ATOMIC);
-		if (!ctx->auth_hw_state_dec) {
-			spin_unlock(&ctx->lock);
-			goto out_free_auth_enc;
-		}
 	}
 	spin_unlock(&ctx->lock);
 	if (qat_alg_init_sessions(ctx, key, keylen))
@@ -572,14 +535,6 @@ static int qat_alg_setkey(struct crypto_aead *tfm, const uint8_t *key,
 	return 0;
 
 out_free_all:
-	dma_free_coherent(dev, sizeof(struct qat_auth_state),
-			  ctx->auth_hw_state_dec, ctx->auth_state_dec_paddr);
-	ctx->auth_hw_state_dec = NULL;
-out_free_auth_enc:
-	dma_free_coherent(dev, sizeof(struct qat_auth_state),
-			  ctx->auth_hw_state_enc, ctx->auth_state_enc_paddr);
-	ctx->auth_hw_state_enc = NULL;
-out_free_dec:
 	dma_free_coherent(dev, sizeof(struct qat_alg_cd),
 			  ctx->dec_cd, ctx->dec_cd_paddr);
 	ctx->dec_cd = NULL;
@@ -924,16 +879,6 @@ static void qat_alg_exit(struct crypto_tfm *tfm)
 	if (ctx->dec_cd)
 		dma_free_coherent(dev, sizeof(struct qat_alg_cd),
 				  ctx->dec_cd, ctx->dec_cd_paddr);
-	if (ctx->auth_hw_state_enc)
-		dma_free_coherent(dev, sizeof(struct qat_auth_state),
-				  ctx->auth_hw_state_enc,
-				  ctx->auth_state_enc_paddr);
-
-	if (ctx->auth_hw_state_dec)
-		dma_free_coherent(dev, sizeof(struct qat_auth_state),
-				  ctx->auth_hw_state_dec,
-				  ctx->auth_state_dec_paddr);
-
 	qat_crypto_put_instance(inst);
 }
 
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
index b707f292b377..65dd1ff93d3b 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_dh895xcc_hw_data.h
@@ -66,7 +66,7 @@
 #define ADF_DH895XCC_ETR_MAX_BANKS 32
 #define ADF_DH895XCC_SMIAPF0_MASK_OFFSET (0x3A000 + 0x28)
 #define ADF_DH895XCC_SMIAPF1_MASK_OFFSET (0x3A000 + 0x30)
-#define ADF_DH895XCC_SMIA0_MASK 0xFFFF
+#define ADF_DH895XCC_SMIA0_MASK 0xFFFFFFFF
 #define ADF_DH895XCC_SMIA1_MASK 0x1
 /* Error detection and correction */
 #define ADF_DH895XCC_AE_CTX_ENABLES(i) (i * 0x1000 + 0x20818)
diff --git a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c b/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
index d4172dedf775..67ec61e51185 100644
--- a/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
+++ b/drivers/crypto/qat/qat_dh895xcc/adf_isr.c
@@ -70,9 +70,9 @@ static int adf_enable_msix(struct adf_accel_dev *accel_dev)
 	for (i = 0; i < msix_num_entries; i++)
 		pci_dev_info->msix_entries.entries[i].entry = i;
 
-	if (pci_enable_msix(pci_dev_info->pci_dev,
-			    pci_dev_info->msix_entries.entries,
-			    msix_num_entries)) {
+	if (pci_enable_msix_exact(pci_dev_info->pci_dev,
+				  pci_dev_info->msix_entries.entries,
+				  msix_num_entries)) {
 		pr_err("QAT: Failed to enable MSIX IRQ\n");
 		return -EFAULT;
 	}
@@ -89,7 +89,7 @@ static irqreturn_t adf_msix_isr_bundle(int irq, void *bank_ptr)
 	struct adf_etr_bank_data *bank = bank_ptr;
 
 	WRITE_CSR_INT_FLAG_AND_COL(bank->csr_addr, bank->bank_number, 0);
-	tasklet_hi_schedule(&bank->resp_hanlder);
+	tasklet_hi_schedule(&bank->resp_handler);
 	return IRQ_HANDLED;
 }
 
@@ -217,7 +217,7 @@ static int adf_setup_bh(struct adf_accel_dev *accel_dev)
 	int i;
 
 	for (i = 0; i < hw_data->num_banks; i++)
-		tasklet_init(&priv_data->banks[i].resp_hanlder,
+		tasklet_init(&priv_data->banks[i].resp_handler,
 			     adf_response_handler,
 			     (unsigned long)&priv_data->banks[i]);
 	return 0;
@@ -230,8 +230,8 @@ static void adf_cleanup_bh(struct adf_accel_dev *accel_dev)
 	int i;
 
 	for (i = 0; i < hw_data->num_banks; i++) {
-		tasklet_disable(&priv_data->banks[i].resp_hanlder);
-		tasklet_kill(&priv_data->banks[i].resp_hanlder);
+		tasklet_disable(&priv_data->banks[i].resp_handler);
+		tasklet_kill(&priv_data->banks[i].resp_handler);
 	}
 }
 
diff --git a/drivers/crypto/qce/dma.h b/drivers/crypto/qce/dma.h
index 805e378d59e9..65bedb81de0b 100644
--- a/drivers/crypto/qce/dma.h
+++ b/drivers/crypto/qce/dma.h
@@ -14,6 +14,8 @@
 #ifndef _DMA_H_
 #define _DMA_H_
 
+#include <linux/dmaengine.h>
+
 /* maximum data transfer block size between BAM and CE */
 #define QCE_BAM_BURST_SIZE		64
 
diff --git a/drivers/devfreq/Kconfig b/drivers/devfreq/Kconfig
index 3dced0a9eae3..faf4e70c42e0 100644
--- a/drivers/devfreq/Kconfig
+++ b/drivers/devfreq/Kconfig
@@ -78,9 +78,8 @@ config ARM_EXYNOS4_BUS_DEVFREQ
 	  This does not yet operate with optimal voltages.
 
 config ARM_EXYNOS5_BUS_DEVFREQ
-	bool "ARM Exynos5250 Bus DEVFREQ Driver"
+	tristate "ARM Exynos5250 Bus DEVFREQ Driver"
 	depends on SOC_EXYNOS5250
-	select ARCH_HAS_OPP
 	select DEVFREQ_GOV_SIMPLE_ONDEMAND
 	select PM_OPP
 	help
diff --git a/drivers/devfreq/devfreq.c b/drivers/devfreq/devfreq.c
index 9f90369dd6bd..30b538d8cc90 100644
--- a/drivers/devfreq/devfreq.c
+++ b/drivers/devfreq/devfreq.c
@@ -1119,6 +1119,7 @@ struct dev_pm_opp *devfreq_recommended_opp(struct device *dev,
 
 	return opp;
 }
+EXPORT_SYMBOL(devfreq_recommended_opp);
 
 /**
  * devfreq_register_opp_notifier() - Helper function to get devfreq notified
@@ -1142,6 +1143,7 @@ int devfreq_register_opp_notifier(struct device *dev, struct devfreq *devfreq)
 
 	return ret;
 }
+EXPORT_SYMBOL(devfreq_register_opp_notifier);
 
 /**
  * devfreq_unregister_opp_notifier() - Helper function to stop getting devfreq
@@ -1168,6 +1170,7 @@ int devfreq_unregister_opp_notifier(struct device *dev, struct devfreq *devfreq)
 
 	return ret;
 }
+EXPORT_SYMBOL(devfreq_unregister_opp_notifier);
 
 static void devm_devfreq_opp_release(struct device *dev, void *res)
 {
diff --git a/drivers/devfreq/exynos/exynos_ppmu.c b/drivers/devfreq/exynos/exynos_ppmu.c
index 75fcc5140ffb..97b75e513d29 100644
--- a/drivers/devfreq/exynos/exynos_ppmu.c
+++ b/drivers/devfreq/exynos/exynos_ppmu.c
@@ -73,6 +73,7 @@ void busfreq_mon_reset(struct busfreq_ppmu_data *ppmu_data)
 		exynos_ppmu_start(ppmu_base);
 	}
 }
+EXPORT_SYMBOL(busfreq_mon_reset);
 
 void exynos_read_ppmu(struct busfreq_ppmu_data *ppmu_data)
 {
@@ -97,6 +98,7 @@ void exynos_read_ppmu(struct busfreq_ppmu_data *ppmu_data)
 
 	busfreq_mon_reset(ppmu_data);
 }
+EXPORT_SYMBOL(exynos_read_ppmu);
 
 int exynos_get_busier_ppmu(struct busfreq_ppmu_data *ppmu_data)
 {
@@ -114,3 +116,4 @@ int exynos_get_busier_ppmu(struct busfreq_ppmu_data *ppmu_data)
 
 	return busy;
 }
+EXPORT_SYMBOL(exynos_get_busier_ppmu);
diff --git a/drivers/dma-buf/dma-buf.c b/drivers/dma-buf/dma-buf.c
index f3014c448e1e..5be225c2ba98 100644
--- a/drivers/dma-buf/dma-buf.c
+++ b/drivers/dma-buf/dma-buf.c
@@ -799,7 +799,7 @@ static int dma_buf_describe(struct seq_file *s)
 		seq_printf(s, "%08zu\t%08x\t%08x\t%08ld\t%s\n",
 				buf_obj->size,
 				buf_obj->file->f_flags, buf_obj->file->f_mode,
-				(long)(buf_obj->file->f_count.counter),
+				file_count(buf_obj->file),
 				buf_obj->exp_name);
 
 		seq_puts(s, "\tAttached Devices:\n");
diff --git a/drivers/dma-buf/fence.c b/drivers/dma-buf/fence.c
index 4222cb2aa96a..7bb9d65d9a2c 100644
--- a/drivers/dma-buf/fence.c
+++ b/drivers/dma-buf/fence.c
@@ -29,7 +29,7 @@
 EXPORT_TRACEPOINT_SYMBOL(fence_annotate_wait_on);
 EXPORT_TRACEPOINT_SYMBOL(fence_emit);
 
-/**
+/*
  * fence context counter: each execution context should have its own
  * fence context, this allows checking if fences belong to the same
  * context or not. One device can have multiple separate contexts,
diff --git a/drivers/dma/Kconfig b/drivers/dma/Kconfig
index 9b1ea0ef59af..de469821bc1b 100644
--- a/drivers/dma/Kconfig
+++ b/drivers/dma/Kconfig
@@ -270,7 +270,7 @@ config IMX_SDMA
 	select DMA_ENGINE
 	help
 	  Support the i.MX SDMA engine. This engine is integrated into
-	  Freescale i.MX25/31/35/51/53 chips.
+	  Freescale i.MX25/31/35/51/53/6 chips.
 
 config IMX_DMA
 	tristate "i.MX DMA support"
@@ -427,18 +427,6 @@ config DMA_OF
 comment "DMA Clients"
 	depends on DMA_ENGINE
 
-config NET_DMA
-	bool "Network: TCP receive copy offload"
-	depends on DMA_ENGINE && NET
-	default (INTEL_IOATDMA || FSL_DMA)
-	depends on BROKEN
-	help
-	  This enables the use of DMA engines in the network stack to
-	  offload receive copy-to-user operations, freeing CPU cycles.
-
-	  Say Y here if you enabled INTEL_IOATDMA or FSL_DMA, otherwise
-	  say N.
-
 config ASYNC_TX_DMA
 	bool "Async_tx: Offload support for the async_tx api"
 	depends on DMA_ENGINE
diff --git a/drivers/dma/Makefile b/drivers/dma/Makefile
index c6adb925f0b9..cb626c179911 100644
--- a/drivers/dma/Makefile
+++ b/drivers/dma/Makefile
@@ -6,7 +6,6 @@ obj-$(CONFIG_DMA_VIRTUAL_CHANNELS) += virt-dma.o
 obj-$(CONFIG_DMA_ACPI) += acpi-dma.o
 obj-$(CONFIG_DMA_OF) += of-dma.o
 
-obj-$(CONFIG_NET_DMA) += iovlock.o
 obj-$(CONFIG_INTEL_MID_DMAC) += intel_mid_dma.o
 obj-$(CONFIG_DMATEST) += dmatest.o
 obj-$(CONFIG_INTEL_IOATDMA) += ioat/
diff --git a/drivers/dma/coh901318.c b/drivers/dma/coh901318.c
index 3c6716e0b78e..e88588d8ecd3 100644
--- a/drivers/dma/coh901318.c
+++ b/drivers/dma/coh901318.c
@@ -2156,7 +2156,7 @@ coh901318_free_chan_resources(struct dma_chan *chan)
 
 	spin_unlock_irqrestore(&cohc->lock, flags);
 
-	chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+	dmaengine_terminate_all(chan);
 }
 
 
diff --git a/drivers/dma/cppi41.c b/drivers/dma/cppi41.c
index 8f8b0b608875..a58eec3b2cad 100644
--- a/drivers/dma/cppi41.c
+++ b/drivers/dma/cppi41.c
@@ -938,7 +938,7 @@ static int cppi41_dma_probe(struct platform_device *pdev)
 	if (!glue_info)
 		return -EINVAL;
 
-	cdd = kzalloc(sizeof(*cdd), GFP_KERNEL);
+	cdd = devm_kzalloc(&pdev->dev, sizeof(*cdd), GFP_KERNEL);
 	if (!cdd)
 		return -ENOMEM;
 
@@ -959,10 +959,8 @@ static int cppi41_dma_probe(struct platform_device *pdev)
 	cdd->qmgr_mem = of_iomap(dev->of_node, 3);
 
 	if (!cdd->usbss_mem || !cdd->ctrl_mem || !cdd->sched_mem ||
-			!cdd->qmgr_mem) {
-		ret = -ENXIO;
-		goto err_remap;
-	}
+			!cdd->qmgr_mem)
+		return -ENXIO;
 
 	pm_runtime_enable(dev);
 	ret = pm_runtime_get_sync(dev);
@@ -989,7 +987,7 @@ static int cppi41_dma_probe(struct platform_device *pdev)
 
 	cppi_writel(USBSS_IRQ_PD_COMP, cdd->usbss_mem + USBSS_IRQ_ENABLER);
 
-	ret = request_irq(irq, glue_info->isr, IRQF_SHARED,
+	ret = devm_request_irq(&pdev->dev, irq, glue_info->isr, IRQF_SHARED,
 			dev_name(dev), cdd);
 	if (ret)
 		goto err_irq;
@@ -1009,7 +1007,6 @@ static int cppi41_dma_probe(struct platform_device *pdev)
 err_of:
 	dma_async_device_unregister(&cdd->ddev);
 err_dma_reg:
-	free_irq(irq, cdd);
 err_irq:
 	cppi_writel(0, cdd->usbss_mem + USBSS_IRQ_CLEARR);
 	cleanup_chans(cdd);
@@ -1023,8 +1020,6 @@ err_get_sync:
 	iounmap(cdd->ctrl_mem);
 	iounmap(cdd->sched_mem);
 	iounmap(cdd->qmgr_mem);
-err_remap:
-	kfree(cdd);
 	return ret;
 }
 
@@ -1036,7 +1031,7 @@ static int cppi41_dma_remove(struct platform_device *pdev)
 	dma_async_device_unregister(&cdd->ddev);
 
 	cppi_writel(0, cdd->usbss_mem + USBSS_IRQ_CLEARR);
-	free_irq(cdd->irq, cdd);
+	devm_free_irq(&pdev->dev, cdd->irq, cdd);
 	cleanup_chans(cdd);
 	deinit_cppi41(&pdev->dev, cdd);
 	iounmap(cdd->usbss_mem);
@@ -1045,7 +1040,6 @@ static int cppi41_dma_remove(struct platform_device *pdev)
 	iounmap(cdd->qmgr_mem);
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-	kfree(cdd);
 	return 0;
 }
 
diff --git a/drivers/dma/dma-jz4740.c b/drivers/dma/dma-jz4740.c
index 6a9d89c93b1f..ae2ab14e64b3 100644
--- a/drivers/dma/dma-jz4740.c
+++ b/drivers/dma/dma-jz4740.c
@@ -362,8 +362,9 @@ static void jz4740_dma_chan_irq(struct jz4740_dmaengine_chan *chan)
 			vchan_cyclic_callback(&chan->desc->vdesc);
 		} else {
 			if (chan->next_sg == chan->desc->num_sgs) {
-				chan->desc = NULL;
+				list_del(&chan->desc->vdesc.node);
 				vchan_cookie_complete(&chan->desc->vdesc);
+				chan->desc = NULL;
 			}
 		}
 	}
diff --git a/drivers/dma/dmaengine.c b/drivers/dma/dmaengine.c
index d5d30ed863ce..24bfaf0b92ba 100644
--- a/drivers/dma/dmaengine.c
+++ b/drivers/dma/dmaengine.c
@@ -1081,110 +1081,6 @@ dmaengine_get_unmap_data(struct device *dev, int nr, gfp_t flags)
 }
 EXPORT_SYMBOL(dmaengine_get_unmap_data);
 
-/**
- * dma_async_memcpy_pg_to_pg - offloaded copy from page to page
- * @chan: DMA channel to offload copy to
- * @dest_pg: destination page
- * @dest_off: offset in page to copy to
- * @src_pg: source page
- * @src_off: offset in page to copy from
- * @len: length
- *
- * Both @dest_page/@dest_off and @src_page/@src_off must be mappable to a bus
- * address according to the DMA mapping API rules for streaming mappings.
- * Both @dest_page/@dest_off and @src_page/@src_off must stay memory resident
- * (kernel memory or locked user space pages).
- */
-dma_cookie_t
-dma_async_memcpy_pg_to_pg(struct dma_chan *chan, struct page *dest_pg,
-	unsigned int dest_off, struct page *src_pg, unsigned int src_off,
-	size_t len)
-{
-	struct dma_device *dev = chan->device;
-	struct dma_async_tx_descriptor *tx;
-	struct dmaengine_unmap_data *unmap;
-	dma_cookie_t cookie;
-	unsigned long flags;
-
-	unmap = dmaengine_get_unmap_data(dev->dev, 2, GFP_NOWAIT);
-	if (!unmap)
-		return -ENOMEM;
-
-	unmap->to_cnt = 1;
-	unmap->from_cnt = 1;
-	unmap->addr[0] = dma_map_page(dev->dev, src_pg, src_off, len,
-				      DMA_TO_DEVICE);
-	unmap->addr[1] = dma_map_page(dev->dev, dest_pg, dest_off, len,
-				      DMA_FROM_DEVICE);
-	unmap->len = len;
-	flags = DMA_CTRL_ACK;
-	tx = dev->device_prep_dma_memcpy(chan, unmap->addr[1], unmap->addr[0],
-					 len, flags);
-
-	if (!tx) {
-		dmaengine_unmap_put(unmap);
-		return -ENOMEM;
-	}
-
-	dma_set_unmap(tx, unmap);
-	cookie = tx->tx_submit(tx);
-	dmaengine_unmap_put(unmap);
-
-	preempt_disable();
-	__this_cpu_add(chan->local->bytes_transferred, len);
-	__this_cpu_inc(chan->local->memcpy_count);
-	preempt_enable();
-
-	return cookie;
-}
-EXPORT_SYMBOL(dma_async_memcpy_pg_to_pg);
-
-/**
- * dma_async_memcpy_buf_to_buf - offloaded copy between virtual addresses
- * @chan: DMA channel to offload copy to
- * @dest: destination address (virtual)
- * @src: source address (virtual)
- * @len: length
- *
- * Both @dest and @src must be mappable to a bus address according to the
- * DMA mapping API rules for streaming mappings.
- * Both @dest and @src must stay memory resident (kernel memory or locked
- * user space pages).
- */
-dma_cookie_t
-dma_async_memcpy_buf_to_buf(struct dma_chan *chan, void *dest,
-			    void *src, size_t len)
-{
-	return dma_async_memcpy_pg_to_pg(chan, virt_to_page(dest),
-					 (unsigned long) dest & ~PAGE_MASK,
-					 virt_to_page(src),
-					 (unsigned long) src & ~PAGE_MASK, len);
-}
-EXPORT_SYMBOL(dma_async_memcpy_buf_to_buf);
-
-/**
- * dma_async_memcpy_buf_to_pg - offloaded copy from address to page
- * @chan: DMA channel to offload copy to
- * @page: destination page
- * @offset: offset in page to copy to
- * @kdata: source address (virtual)
- * @len: length
- *
- * Both @page/@offset and @kdata must be mappable to a bus address according
- * to the DMA mapping API rules for streaming mappings.
- * Both @page/@offset and @kdata must stay memory resident (kernel memory or
- * locked user space pages)
- */
-dma_cookie_t
-dma_async_memcpy_buf_to_pg(struct dma_chan *chan, struct page *page,
-			   unsigned int offset, void *kdata, size_t len)
-{
-	return dma_async_memcpy_pg_to_pg(chan, page, offset,
-					 virt_to_page(kdata),
-					 (unsigned long) kdata & ~PAGE_MASK, len);
-}
-EXPORT_SYMBOL(dma_async_memcpy_buf_to_pg);
-
 void dma_async_tx_descriptor_init(struct dma_async_tx_descriptor *tx,
 	struct dma_chan *chan)
 {
diff --git a/drivers/dma/dmatest.c b/drivers/dma/dmatest.c
index e27cec25c59e..a8d7809e2f4c 100644
--- a/drivers/dma/dmatest.c
+++ b/drivers/dma/dmatest.c
@@ -688,14 +688,14 @@ static int dmatest_func(void *data)
 	runtime = ktime_us_delta(ktime_get(), ktime);
 
 	ret = 0;
+err_dstbuf:
 	for (i = 0; thread->dsts[i]; i++)
 		kfree(thread->dsts[i]);
-err_dstbuf:
 	kfree(thread->dsts);
 err_dsts:
+err_srcbuf:
 	for (i = 0; thread->srcs[i]; i++)
 		kfree(thread->srcs[i]);
-err_srcbuf:
 	kfree(thread->srcs);
 err_srcs:
 	kfree(pq_coefs);
diff --git a/drivers/dma/dw/core.c b/drivers/dma/dw/core.c
index 1af731b83b3f..244722170410 100644
--- a/drivers/dma/dw/core.c
+++ b/drivers/dma/dw/core.c
@@ -11,7 +11,6 @@
  */
 
 #include <linux/bitops.h>
-#include <linux/clk.h>
 #include <linux/delay.h>
 #include <linux/dmaengine.h>
 #include <linux/dma-mapping.h>
@@ -37,24 +36,6 @@
  * support descriptor writeback.
  */
 
-static inline bool is_request_line_unset(struct dw_dma_chan *dwc)
-{
-	return dwc->request_line == (typeof(dwc->request_line))~0;
-}
-
-static inline void dwc_set_masters(struct dw_dma_chan *dwc)
-{
-	struct dw_dma *dw = to_dw_dma(dwc->chan.device);
-	struct dw_dma_slave *dws = dwc->chan.private;
-	unsigned char mmax = dw->nr_masters - 1;
-
-	if (!is_request_line_unset(dwc))
-		return;
-
-	dwc->src_master = min_t(unsigned char, mmax, dwc_get_sms(dws));
-	dwc->dst_master = min_t(unsigned char, mmax, dwc_get_dms(dws));
-}
-
 #define DWC_DEFAULT_CTLLO(_chan) ({				\
 		struct dw_dma_chan *_dwc = to_dw_dma_chan(_chan);	\
 		struct dma_slave_config	*_sconfig = &_dwc->dma_sconfig;	\
@@ -155,13 +136,11 @@ static void dwc_initialize(struct dw_dma_chan *dwc)
 		 */
 		BUG_ON(!dws->dma_dev || dws->dma_dev != dw->dma.dev);
 
-		cfghi = dws->cfg_hi;
-		cfglo |= dws->cfg_lo & ~DWC_CFGL_CH_PRIOR_MASK;
+		cfghi |= DWC_CFGH_DST_PER(dws->dst_id);
+		cfghi |= DWC_CFGH_SRC_PER(dws->src_id);
 	} else {
-		if (dwc->direction == DMA_MEM_TO_DEV)
-			cfghi = DWC_CFGH_DST_PER(dwc->request_line);
-		else if (dwc->direction == DMA_DEV_TO_MEM)
-			cfghi = DWC_CFGH_SRC_PER(dwc->request_line);
+		cfghi |= DWC_CFGH_DST_PER(dwc->dst_id);
+		cfghi |= DWC_CFGH_SRC_PER(dwc->src_id);
 	}
 
 	channel_writel(dwc, CFG_LO, cfglo);
@@ -939,6 +918,26 @@ err_desc_get:
 	return NULL;
 }
 
+bool dw_dma_filter(struct dma_chan *chan, void *param)
+{
+	struct dw_dma_chan *dwc = to_dw_dma_chan(chan);
+	struct dw_dma_slave *dws = param;
+
+	if (!dws || dws->dma_dev != chan->device->dev)
+		return false;
+
+	/* We have to copy data since dws can be temporary storage */
+
+	dwc->src_id = dws->src_id;
+	dwc->dst_id = dws->dst_id;
+
+	dwc->src_master = dws->src_master;
+	dwc->dst_master = dws->dst_master;
+
+	return true;
+}
+EXPORT_SYMBOL_GPL(dw_dma_filter);
+
 /*
  * Fix sconfig's burst size according to dw_dmac. We need to convert them as:
  * 1 -> 0, 4 -> 1, 8 -> 2, 16 -> 3.
@@ -967,10 +966,6 @@ set_runtime_config(struct dma_chan *chan, struct dma_slave_config *sconfig)
 	memcpy(&dwc->dma_sconfig, sconfig, sizeof(*sconfig));
 	dwc->direction = sconfig->direction;
 
-	/* Take the request line from slave_id member */
-	if (is_request_line_unset(dwc))
-		dwc->request_line = sconfig->slave_id;
-
 	convert_burst(&dwc->dma_sconfig.src_maxburst);
 	convert_burst(&dwc->dma_sconfig.dst_maxburst);
 
@@ -1099,6 +1094,31 @@ static void dwc_issue_pending(struct dma_chan *chan)
 	spin_unlock_irqrestore(&dwc->lock, flags);
 }
 
+/*----------------------------------------------------------------------*/
+
+static void dw_dma_off(struct dw_dma *dw)
+{
+	int i;
+
+	dma_writel(dw, CFG, 0);
+
+	channel_clear_bit(dw, MASK.XFER, dw->all_chan_mask);
+	channel_clear_bit(dw, MASK.SRC_TRAN, dw->all_chan_mask);
+	channel_clear_bit(dw, MASK.DST_TRAN, dw->all_chan_mask);
+	channel_clear_bit(dw, MASK.ERROR, dw->all_chan_mask);
+
+	while (dma_readl(dw, CFG) & DW_CFG_DMA_EN)
+		cpu_relax();
+
+	for (i = 0; i < dw->dma.chancnt; i++)
+		dw->chan[i].initialized = false;
+}
+
+static void dw_dma_on(struct dw_dma *dw)
+{
+	dma_writel(dw, CFG, DW_CFG_DMA_EN);
+}
+
 static int dwc_alloc_chan_resources(struct dma_chan *chan)
 {
 	struct dw_dma_chan	*dwc = to_dw_dma_chan(chan);
@@ -1123,7 +1143,10 @@ static int dwc_alloc_chan_resources(struct dma_chan *chan)
 	 * doesn't mean what you think it means), and status writeback.
 	 */
 
-	dwc_set_masters(dwc);
+	/* Enable controller here if needed */
+	if (!dw->in_use)
+		dw_dma_on(dw);
+	dw->in_use |= dwc->mask;
 
 	spin_lock_irqsave(&dwc->lock, flags);
 	i = dwc->descs_allocated;
@@ -1182,7 +1205,6 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 	list_splice_init(&dwc->free_list, &list);
 	dwc->descs_allocated = 0;
 	dwc->initialized = false;
-	dwc->request_line = ~0;
 
 	/* Disable interrupts */
 	channel_clear_bit(dw, MASK.XFER, dwc->mask);
@@ -1190,6 +1212,11 @@ static void dwc_free_chan_resources(struct dma_chan *chan)
 
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
+	/* Disable controller in case it was a last user */
+	dw->in_use &= ~dwc->mask;
+	if (!dw->in_use)
+		dw_dma_off(dw);
+
 	list_for_each_entry_safe(desc, _desc, &list, desc_node) {
 		dev_vdbg(chan2dev(chan), "  freeing descriptor %p\n", desc);
 		dma_pool_free(dw->desc_pool, desc, desc->txd.phys);
@@ -1460,24 +1487,6 @@ EXPORT_SYMBOL(dw_dma_cyclic_free);
 
 /*----------------------------------------------------------------------*/
 
-static void dw_dma_off(struct dw_dma *dw)
-{
-	int i;
-
-	dma_writel(dw, CFG, 0);
-
-	channel_clear_bit(dw, MASK.XFER, dw->all_chan_mask);
-	channel_clear_bit(dw, MASK.SRC_TRAN, dw->all_chan_mask);
-	channel_clear_bit(dw, MASK.DST_TRAN, dw->all_chan_mask);
-	channel_clear_bit(dw, MASK.ERROR, dw->all_chan_mask);
-
-	while (dma_readl(dw, CFG) & DW_CFG_DMA_EN)
-		cpu_relax();
-
-	for (i = 0; i < dw->dma.chancnt; i++)
-		dw->chan[i].initialized = false;
-}
-
 int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 {
 	struct dw_dma		*dw;
@@ -1495,13 +1504,6 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 	dw->regs = chip->regs;
 	chip->dw = dw;
 
-	dw->clk = devm_clk_get(chip->dev, "hclk");
-	if (IS_ERR(dw->clk))
-		return PTR_ERR(dw->clk);
-	err = clk_prepare_enable(dw->clk);
-	if (err)
-		return err;
-
 	dw_params = dma_read_byaddr(chip->regs, DW_PARAMS);
 	autocfg = dw_params >> DW_PARAMS_EN & 0x1;
 
@@ -1604,7 +1606,6 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 		channel_clear_bit(dw, CH_EN, dwc->mask);
 
 		dwc->direction = DMA_TRANS_NONE;
-		dwc->request_line = ~0;
 
 		/* Hardware configuration */
 		if (autocfg) {
@@ -1659,8 +1660,6 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 	dw->dma.device_tx_status = dwc_tx_status;
 	dw->dma.device_issue_pending = dwc_issue_pending;
 
-	dma_writel(dw, CFG, DW_CFG_DMA_EN);
-
 	err = dma_async_device_register(&dw->dma);
 	if (err)
 		goto err_dma_register;
@@ -1673,7 +1672,6 @@ int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata)
 err_dma_register:
 	free_irq(chip->irq, dw);
 err_pdata:
-	clk_disable_unprepare(dw->clk);
 	return err;
 }
 EXPORT_SYMBOL_GPL(dw_dma_probe);
@@ -1695,46 +1693,27 @@ int dw_dma_remove(struct dw_dma_chip *chip)
 		channel_clear_bit(dw, CH_EN, dwc->mask);
 	}
 
-	clk_disable_unprepare(dw->clk);
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(dw_dma_remove);
 
-void dw_dma_shutdown(struct dw_dma_chip *chip)
-{
-	struct dw_dma *dw = chip->dw;
-
-	dw_dma_off(dw);
-	clk_disable_unprepare(dw->clk);
-}
-EXPORT_SYMBOL_GPL(dw_dma_shutdown);
-
-#ifdef CONFIG_PM_SLEEP
-
-int dw_dma_suspend(struct dw_dma_chip *chip)
+int dw_dma_disable(struct dw_dma_chip *chip)
 {
 	struct dw_dma *dw = chip->dw;
 
 	dw_dma_off(dw);
-	clk_disable_unprepare(dw->clk);
-
 	return 0;
 }
-EXPORT_SYMBOL_GPL(dw_dma_suspend);
+EXPORT_SYMBOL_GPL(dw_dma_disable);
 
-int dw_dma_resume(struct dw_dma_chip *chip)
+int dw_dma_enable(struct dw_dma_chip *chip)
 {
 	struct dw_dma *dw = chip->dw;
 
-	clk_prepare_enable(dw->clk);
-	dma_writel(dw, CFG, DW_CFG_DMA_EN);
-
+	dw_dma_on(dw);
 	return 0;
 }
-EXPORT_SYMBOL_GPL(dw_dma_resume);
-
-#endif /* CONFIG_PM_SLEEP */
+EXPORT_SYMBOL_GPL(dw_dma_enable);
 
 MODULE_LICENSE("GPL v2");
 MODULE_DESCRIPTION("Synopsys DesignWare DMA Controller core driver");
diff --git a/drivers/dma/dw/internal.h b/drivers/dma/dw/internal.h
index 32667f9e0dda..41439732ff6b 100644
--- a/drivers/dma/dw/internal.h
+++ b/drivers/dma/dw/internal.h
@@ -8,63 +8,16 @@
  * published by the Free Software Foundation.
  */
 
-#ifndef _DW_DMAC_INTERNAL_H
-#define _DW_DMAC_INTERNAL_H
+#ifndef _DMA_DW_INTERNAL_H
+#define _DMA_DW_INTERNAL_H
 
-#include <linux/device.h>
-#include <linux/dw_dmac.h>
+#include <linux/dma/dw.h>
 
 #include "regs.h"
 
-/**
- * struct dw_dma_chip - representation of DesignWare DMA controller hardware
- * @dev:		struct device of the DMA controller
- * @irq:		irq line
- * @regs:		memory mapped I/O space
- * @dw:			struct dw_dma that is filed by dw_dma_probe()
- */
-struct dw_dma_chip {
-	struct device	*dev;
-	int		irq;
-	void __iomem	*regs;
-	struct dw_dma	*dw;
-};
-
-/* Export to the platform drivers */
-int dw_dma_probe(struct dw_dma_chip *chip, struct dw_dma_platform_data *pdata);
-int dw_dma_remove(struct dw_dma_chip *chip);
-
-void dw_dma_shutdown(struct dw_dma_chip *chip);
-
-#ifdef CONFIG_PM_SLEEP
-
-int dw_dma_suspend(struct dw_dma_chip *chip);
-int dw_dma_resume(struct dw_dma_chip *chip);
-
-#endif /* CONFIG_PM_SLEEP */
+int dw_dma_disable(struct dw_dma_chip *chip);
+int dw_dma_enable(struct dw_dma_chip *chip);
 
-/**
- * dwc_get_dms - get destination master
- * @slave:	pointer to the custom slave configuration
- *
- * Returns destination master in the custom slave configuration if defined, or
- * default value otherwise.
- */
-static inline unsigned int dwc_get_dms(struct dw_dma_slave *slave)
-{
-	return slave ? slave->dst_master : 0;
-}
-
-/**
- * dwc_get_sms - get source master
- * @slave:	pointer to the custom slave configuration
- *
- * Returns source master in the custom slave configuration if defined, or
- * default value otherwise.
- */
-static inline unsigned int dwc_get_sms(struct dw_dma_slave *slave)
-{
-	return slave ? slave->src_master : 1;
-}
+extern bool dw_dma_filter(struct dma_chan *chan, void *param);
 
-#endif /* _DW_DMAC_INTERNAL_H */
+#endif /* _DMA_DW_INTERNAL_H */
diff --git a/drivers/dma/dw/pci.c b/drivers/dma/dw/pci.c
index 39e30c3c7a9d..b144706b3d85 100644
--- a/drivers/dma/dw/pci.c
+++ b/drivers/dma/dw/pci.c
@@ -82,7 +82,7 @@ static int dw_pci_suspend_late(struct device *dev)
 	struct pci_dev *pci = to_pci_dev(dev);
 	struct dw_dma_chip *chip = pci_get_drvdata(pci);
 
-	return dw_dma_suspend(chip);
+	return dw_dma_disable(chip);
 };
 
 static int dw_pci_resume_early(struct device *dev)
@@ -90,7 +90,7 @@ static int dw_pci_resume_early(struct device *dev)
 	struct pci_dev *pci = to_pci_dev(dev);
 	struct dw_dma_chip *chip = pci_get_drvdata(pci);
 
-	return dw_dma_resume(chip);
+	return dw_dma_enable(chip);
 };
 
 #endif /* CONFIG_PM_SLEEP */
@@ -108,6 +108,10 @@ static const struct pci_device_id dw_pci_id_table[] = {
 	{ PCI_VDEVICE(INTEL, 0x0f06), (kernel_ulong_t)&dw_pci_pdata },
 	{ PCI_VDEVICE(INTEL, 0x0f40), (kernel_ulong_t)&dw_pci_pdata },
 
+	/* Braswell */
+	{ PCI_VDEVICE(INTEL, 0x2286), (kernel_ulong_t)&dw_pci_pdata },
+	{ PCI_VDEVICE(INTEL, 0x22c0), (kernel_ulong_t)&dw_pci_pdata },
+
 	/* Haswell */
 	{ PCI_VDEVICE(INTEL, 0x9c60), (kernel_ulong_t)&dw_pci_pdata },
 	{ }
diff --git a/drivers/dma/dw/platform.c b/drivers/dma/dw/platform.c
index c5b339af6be5..a630161473a4 100644
--- a/drivers/dma/dw/platform.c
+++ b/drivers/dma/dw/platform.c
@@ -25,72 +25,49 @@
 
 #include "internal.h"
 
-struct dw_dma_of_filter_args {
-	struct dw_dma *dw;
-	unsigned int req;
-	unsigned int src;
-	unsigned int dst;
-};
-
-static bool dw_dma_of_filter(struct dma_chan *chan, void *param)
-{
-	struct dw_dma_chan *dwc = to_dw_dma_chan(chan);
-	struct dw_dma_of_filter_args *fargs = param;
-
-	/* Ensure the device matches our channel */
-	if (chan->device != &fargs->dw->dma)
-		return false;
-
-	dwc->request_line = fargs->req;
-	dwc->src_master	= fargs->src;
-	dwc->dst_master	= fargs->dst;
-
-	return true;
-}
-
 static struct dma_chan *dw_dma_of_xlate(struct of_phandle_args *dma_spec,
 					struct of_dma *ofdma)
 {
 	struct dw_dma *dw = ofdma->of_dma_data;
-	struct dw_dma_of_filter_args fargs = {
-		.dw = dw,
+	struct dw_dma_slave slave = {
+		.dma_dev = dw->dma.dev,
 	};
 	dma_cap_mask_t cap;
 
 	if (dma_spec->args_count != 3)
 		return NULL;
 
-	fargs.req = dma_spec->args[0];
-	fargs.src = dma_spec->args[1];
-	fargs.dst = dma_spec->args[2];
+	slave.src_id = dma_spec->args[0];
+	slave.dst_id = dma_spec->args[0];
+	slave.src_master = dma_spec->args[1];
+	slave.dst_master = dma_spec->args[2];
 
-	if (WARN_ON(fargs.req >= DW_DMA_MAX_NR_REQUESTS ||
-		    fargs.src >= dw->nr_masters ||
-		    fargs.dst >= dw->nr_masters))
+	if (WARN_ON(slave.src_id >= DW_DMA_MAX_NR_REQUESTS ||
+		    slave.dst_id >= DW_DMA_MAX_NR_REQUESTS ||
+		    slave.src_master >= dw->nr_masters ||
+		    slave.dst_master >= dw->nr_masters))
 		return NULL;
 
 	dma_cap_zero(cap);
 	dma_cap_set(DMA_SLAVE, cap);
 
 	/* TODO: there should be a simpler way to do this */
-	return dma_request_channel(cap, dw_dma_of_filter, &fargs);
+	return dma_request_channel(cap, dw_dma_filter, &slave);
 }
 
 #ifdef CONFIG_ACPI
 static bool dw_dma_acpi_filter(struct dma_chan *chan, void *param)
 {
-	struct dw_dma_chan *dwc = to_dw_dma_chan(chan);
 	struct acpi_dma_spec *dma_spec = param;
+	struct dw_dma_slave slave = {
+		.dma_dev = dma_spec->dev,
+		.src_id = dma_spec->slave_id,
+		.dst_id = dma_spec->slave_id,
+		.src_master = 1,
+		.dst_master = 0,
+	};
 
-	if (chan->device->dev != dma_spec->dev ||
-	    chan->chan_id != dma_spec->chan_id)
-		return false;
-
-	dwc->request_line = dma_spec->slave_id;
-	dwc->src_master = dwc_get_sms(NULL);
-	dwc->dst_master = dwc_get_dms(NULL);
-
-	return true;
+	return dw_dma_filter(chan, &slave);
 }
 
 static void dw_dma_acpi_controller_register(struct dw_dma *dw)
@@ -201,10 +178,17 @@ static int dw_probe(struct platform_device *pdev)
 
 	chip->dev = dev;
 
-	err = dw_dma_probe(chip, pdata);
+	chip->clk = devm_clk_get(chip->dev, "hclk");
+	if (IS_ERR(chip->clk))
+		return PTR_ERR(chip->clk);
+	err = clk_prepare_enable(chip->clk);
 	if (err)
 		return err;
 
+	err = dw_dma_probe(chip, pdata);
+	if (err)
+		goto err_dw_dma_probe;
+
 	platform_set_drvdata(pdev, chip);
 
 	if (pdev->dev.of_node) {
@@ -219,6 +203,10 @@ static int dw_probe(struct platform_device *pdev)
 		dw_dma_acpi_controller_register(chip->dw);
 
 	return 0;
+
+err_dw_dma_probe:
+	clk_disable_unprepare(chip->clk);
+	return err;
 }
 
 static int dw_remove(struct platform_device *pdev)
@@ -228,14 +216,18 @@ static int dw_remove(struct platform_device *pdev)
 	if (pdev->dev.of_node)
 		of_dma_controller_free(pdev->dev.of_node);
 
-	return dw_dma_remove(chip);
+	dw_dma_remove(chip);
+	clk_disable_unprepare(chip->clk);
+
+	return 0;
 }
 
 static void dw_shutdown(struct platform_device *pdev)
 {
 	struct dw_dma_chip *chip = platform_get_drvdata(pdev);
 
-	dw_dma_shutdown(chip);
+	dw_dma_disable(chip);
+	clk_disable_unprepare(chip->clk);
 }
 
 #ifdef CONFIG_OF
@@ -261,7 +253,10 @@ static int dw_suspend_late(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct dw_dma_chip *chip = platform_get_drvdata(pdev);
 
-	return dw_dma_suspend(chip);
+	dw_dma_disable(chip);
+	clk_disable_unprepare(chip->clk);
+
+	return 0;
 }
 
 static int dw_resume_early(struct device *dev)
@@ -269,7 +264,8 @@ static int dw_resume_early(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 	struct dw_dma_chip *chip = platform_get_drvdata(pdev);
 
-	return dw_dma_resume(chip);
+	clk_prepare_enable(chip->clk);
+	return dw_dma_enable(chip);
 }
 
 #endif /* CONFIG_PM_SLEEP */
@@ -281,7 +277,7 @@ static const struct dev_pm_ops dw_dev_pm_ops = {
 static struct platform_driver dw_driver = {
 	.probe		= dw_probe,
 	.remove		= dw_remove,
-	.shutdown	= dw_shutdown,
+	.shutdown       = dw_shutdown,
 	.driver = {
 		.name	= "dw_dmac",
 		.pm	= &dw_dev_pm_ops,
diff --git a/drivers/dma/dw/regs.h b/drivers/dma/dw/regs.h
index bb98d3e91e8b..848e232f7cc7 100644
--- a/drivers/dma/dw/regs.h
+++ b/drivers/dma/dw/regs.h
@@ -11,7 +11,6 @@
 
 #include <linux/interrupt.h>
 #include <linux/dmaengine.h>
-#include <linux/dw_dmac.h>
 
 #define DW_DMA_MAX_NR_CHANNELS	8
 #define DW_DMA_MAX_NR_REQUESTS	16
@@ -132,6 +131,18 @@ struct dw_dma_regs {
 /* Bitfields in DWC_PARAMS */
 #define DWC_PARAMS_MBLK_EN	11		/* multi block transfer */
 
+/* bursts size */
+enum dw_dma_msize {
+	DW_DMA_MSIZE_1,
+	DW_DMA_MSIZE_4,
+	DW_DMA_MSIZE_8,
+	DW_DMA_MSIZE_16,
+	DW_DMA_MSIZE_32,
+	DW_DMA_MSIZE_64,
+	DW_DMA_MSIZE_128,
+	DW_DMA_MSIZE_256,
+};
+
 /* Bitfields in CTL_LO */
 #define DWC_CTLL_INT_EN		(1 << 0)	/* irqs enabled? */
 #define DWC_CTLL_DST_WIDTH(n)	((n)<<1)	/* bytes per element */
@@ -161,20 +172,35 @@ struct dw_dma_regs {
 #define DWC_CTLH_DONE		0x00001000
 #define DWC_CTLH_BLOCK_TS_MASK	0x00000fff
 
-/* Bitfields in CFG_LO. Platform-configurable bits are in <linux/dw_dmac.h> */
+/* Bitfields in CFG_LO */
 #define DWC_CFGL_CH_PRIOR_MASK	(0x7 << 5)	/* priority mask */
 #define DWC_CFGL_CH_PRIOR(x)	((x) << 5)	/* priority */
 #define DWC_CFGL_CH_SUSP	(1 << 8)	/* pause xfer */
 #define DWC_CFGL_FIFO_EMPTY	(1 << 9)	/* pause xfer */
 #define DWC_CFGL_HS_DST		(1 << 10)	/* handshake w/dst */
 #define DWC_CFGL_HS_SRC		(1 << 11)	/* handshake w/src */
+#define DWC_CFGL_LOCK_CH_XFER	(0 << 12)	/* scope of LOCK_CH */
+#define DWC_CFGL_LOCK_CH_BLOCK	(1 << 12)
+#define DWC_CFGL_LOCK_CH_XACT	(2 << 12)
+#define DWC_CFGL_LOCK_BUS_XFER	(0 << 14)	/* scope of LOCK_BUS */
+#define DWC_CFGL_LOCK_BUS_BLOCK	(1 << 14)
+#define DWC_CFGL_LOCK_BUS_XACT	(2 << 14)
+#define DWC_CFGL_LOCK_CH	(1 << 15)	/* channel lockout */
+#define DWC_CFGL_LOCK_BUS	(1 << 16)	/* busmaster lockout */
+#define DWC_CFGL_HS_DST_POL	(1 << 18)	/* dst handshake active low */
+#define DWC_CFGL_HS_SRC_POL	(1 << 19)	/* src handshake active low */
 #define DWC_CFGL_MAX_BURST(x)	((x) << 20)
 #define DWC_CFGL_RELOAD_SAR	(1 << 30)
 #define DWC_CFGL_RELOAD_DAR	(1 << 31)
 
-/* Bitfields in CFG_HI. Platform-configurable bits are in <linux/dw_dmac.h> */
+/* Bitfields in CFG_HI */
+#define DWC_CFGH_FCMODE		(1 << 0)
+#define DWC_CFGH_FIFO_MODE	(1 << 1)
+#define DWC_CFGH_PROTCTL(x)	((x) << 2)
 #define DWC_CFGH_DS_UPD_EN	(1 << 5)
 #define DWC_CFGH_SS_UPD_EN	(1 << 6)
+#define DWC_CFGH_SRC_PER(x)	((x) << 7)
+#define DWC_CFGH_DST_PER(x)	((x) << 11)
 
 /* Bitfields in SGR */
 #define DWC_SGR_SGI(x)		((x) << 0)
@@ -221,9 +247,10 @@ struct dw_dma_chan {
 	bool			nollp;
 
 	/* custom slave configuration */
-	unsigned int		request_line;
-	unsigned char		src_master;
-	unsigned char		dst_master;
+	u8			src_id;
+	u8			dst_id;
+	u8			src_master;
+	u8			dst_master;
 
 	/* configuration passed via DMA_SLAVE_CONFIG */
 	struct dma_slave_config dma_sconfig;
@@ -250,11 +277,11 @@ struct dw_dma {
 	void __iomem		*regs;
 	struct dma_pool		*desc_pool;
 	struct tasklet_struct	tasklet;
-	struct clk		*clk;
 
 	/* channels */
 	struct dw_dma_chan	*chan;
 	u8			all_chan_mask;
+	u8			in_use;
 
 	/* hardware configuration */
 	unsigned char		nr_masters;
diff --git a/drivers/dma/edma.c b/drivers/dma/edma.c
index 7b65633f495e..123f578d6dd3 100644
--- a/drivers/dma/edma.c
+++ b/drivers/dma/edma.c
@@ -288,7 +288,7 @@ static int edma_slave_config(struct edma_chan *echan,
 static int edma_dma_pause(struct edma_chan *echan)
 {
 	/* Pause/Resume only allowed with cyclic mode */
-	if (!echan->edesc->cyclic)
+	if (!echan->edesc || !echan->edesc->cyclic)
 		return -EINVAL;
 
 	edma_pause(echan->ch_num);
diff --git a/drivers/dma/fsldma.c b/drivers/dma/fsldma.c
index d5d6885ab341..994bcb2c6b92 100644
--- a/drivers/dma/fsldma.c
+++ b/drivers/dma/fsldma.c
@@ -36,7 +36,7 @@
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
-
+#include <linux/fsldma.h>
 #include "dmaengine.h"
 #include "fsldma.h"
 
@@ -367,6 +367,20 @@ static void fsl_chan_toggle_ext_start(struct fsldma_chan *chan, int enable)
 		chan->feature &= ~FSL_DMA_CHAN_START_EXT;
 }
 
+int fsl_dma_external_start(struct dma_chan *dchan, int enable)
+{
+	struct fsldma_chan *chan;
+
+	if (!dchan)
+		return -EINVAL;
+
+	chan = to_fsl_chan(dchan);
+
+	fsl_chan_toggle_ext_start(chan, enable);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(fsl_dma_external_start);
+
 static void append_ld_queue(struct fsldma_chan *chan, struct fsl_desc_sw *desc)
 {
 	struct fsl_desc_sw *tail = to_fsl_desc(chan->ld_pending.prev);
@@ -998,15 +1012,6 @@ static int fsl_dma_device_control(struct dma_chan *dchan,
 		chan->set_request_count(chan, size);
 		return 0;
 
-	case FSLDMA_EXTERNAL_START:
-
-		/* make sure the channel supports external start */
-		if (!chan->toggle_ext_start)
-			return -ENXIO;
-
-		chan->toggle_ext_start(chan, arg);
-		return 0;
-
 	default:
 		return -ENXIO;
 	}
diff --git a/drivers/dma/imx-sdma.c b/drivers/dma/imx-sdma.c
index f7626e37d0b8..88afc48c2ca7 100644
--- a/drivers/dma/imx-sdma.c
+++ b/drivers/dma/imx-sdma.c
@@ -1334,7 +1334,7 @@ err_firmware:
 	release_firmware(fw);
 }
 
-static int __init sdma_get_firmware(struct sdma_engine *sdma,
+static int sdma_get_firmware(struct sdma_engine *sdma,
 		const char *fw_name)
 {
 	int ret;
@@ -1448,7 +1448,7 @@ static struct dma_chan *sdma_xlate(struct of_phandle_args *dma_spec,
 	return dma_request_channel(mask, sdma_filter_fn, &data);
 }
 
-static int __init sdma_probe(struct platform_device *pdev)
+static int sdma_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id =
 			of_match_device(sdma_dt_ids, &pdev->dev);
@@ -1603,6 +1603,8 @@ static int __init sdma_probe(struct platform_device *pdev)
 	sdma->dma_device.dev->dma_parms = &sdma->dma_parms;
 	dma_set_max_seg_size(sdma->dma_device.dev, 65535);
 
+	platform_set_drvdata(pdev, sdma);
+
 	ret = dma_async_device_register(&sdma->dma_device);
 	if (ret) {
 		dev_err(&pdev->dev, "unable to register\n");
@@ -1640,7 +1642,27 @@ err_irq:
 
 static int sdma_remove(struct platform_device *pdev)
 {
-	return -EBUSY;
+	struct sdma_engine *sdma = platform_get_drvdata(pdev);
+	struct resource *iores = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	int irq = platform_get_irq(pdev, 0);
+	int i;
+
+	dma_async_device_unregister(&sdma->dma_device);
+	kfree(sdma->script_addrs);
+	free_irq(irq, sdma);
+	iounmap(sdma->regs);
+	release_mem_region(iores->start, resource_size(iores));
+	/* Kill the tasklet */
+	for (i = 0; i < MAX_DMA_CHANNELS; i++) {
+		struct sdma_channel *sdmac = &sdma->channel[i];
+
+		tasklet_kill(&sdmac->tasklet);
+	}
+	kfree(sdma);
+
+	platform_set_drvdata(pdev, NULL);
+	dev_info(&pdev->dev, "Removed...\n");
+	return 0;
 }
 
 static struct platform_driver sdma_driver = {
@@ -1650,13 +1672,10 @@ static struct platform_driver sdma_driver = {
 	},
 	.id_table	= sdma_devtypes,
 	.remove		= sdma_remove,
+	.probe		= sdma_probe,
 };
 
-static int __init sdma_module_init(void)
-{
-	return platform_driver_probe(&sdma_driver, sdma_probe);
-}
-module_init(sdma_module_init);
+module_platform_driver(sdma_driver);
 
 MODULE_AUTHOR("Sascha Hauer, Pengutronix <s.hauer@pengutronix.de>");
 MODULE_DESCRIPTION("i.MX SDMA driver");
diff --git a/drivers/dma/ioat/dca.c b/drivers/dma/ioat/dca.c
index 9e84d5bc9307..3b55bb8d969a 100644
--- a/drivers/dma/ioat/dca.c
+++ b/drivers/dma/ioat/dca.c
@@ -35,6 +35,7 @@
 
 #include "dma.h"
 #include "registers.h"
+#include "dma_v2.h"
 
 /*
  * Bit 7 of a tag map entry is the "valid" bit, if it is set then bits 0:6
@@ -147,7 +148,7 @@ static int ioat_dca_add_requester(struct dca_provider *dca, struct device *dev)
 	u16 id;
 
 	/* This implementation only supports PCI-Express */
-	if (dev->bus != &pci_bus_type)
+	if (!dev_is_pci(dev))
 		return -ENODEV;
 	pdev = to_pci_dev(dev);
 	id = dcaid_from_pcidev(pdev);
@@ -179,7 +180,7 @@ static int ioat_dca_remove_requester(struct dca_provider *dca,
 	int i;
 
 	/* This implementation only supports PCI-Express */
-	if (dev->bus != &pci_bus_type)
+	if (!dev_is_pci(dev))
 		return -ENODEV;
 	pdev = to_pci_dev(dev);
 
@@ -320,7 +321,7 @@ static int ioat2_dca_add_requester(struct dca_provider *dca, struct device *dev)
 	u16 global_req_table;
 
 	/* This implementation only supports PCI-Express */
-	if (dev->bus != &pci_bus_type)
+	if (!dev_is_pci(dev))
 		return -ENODEV;
 	pdev = to_pci_dev(dev);
 	id = dcaid_from_pcidev(pdev);
@@ -354,7 +355,7 @@ static int ioat2_dca_remove_requester(struct dca_provider *dca,
 	u16 global_req_table;
 
 	/* This implementation only supports PCI-Express */
-	if (dev->bus != &pci_bus_type)
+	if (!dev_is_pci(dev))
 		return -ENODEV;
 	pdev = to_pci_dev(dev);
 
@@ -496,7 +497,7 @@ static int ioat3_dca_add_requester(struct dca_provider *dca, struct device *dev)
 	u16 global_req_table;
 
 	/* This implementation only supports PCI-Express */
-	if (dev->bus != &pci_bus_type)
+	if (!dev_is_pci(dev))
 		return -ENODEV;
 	pdev = to_pci_dev(dev);
 	id = dcaid_from_pcidev(pdev);
@@ -530,7 +531,7 @@ static int ioat3_dca_remove_requester(struct dca_provider *dca,
 	u16 global_req_table;
 
 	/* This implementation only supports PCI-Express */
-	if (dev->bus != &pci_bus_type)
+	if (!dev_is_pci(dev))
 		return -ENODEV;
 	pdev = to_pci_dev(dev);
 
diff --git a/drivers/dma/ioat/dma.c b/drivers/dma/ioat/dma.c
index 4e3549a16132..940c1502a8b5 100644
--- a/drivers/dma/ioat/dma.c
+++ b/drivers/dma/ioat/dma.c
@@ -947,7 +947,7 @@ msix:
 	for (i = 0; i < msixcnt; i++)
 		device->msix_entries[i].entry = i;
 
-	err = pci_enable_msix(pdev, device->msix_entries, msixcnt);
+	err = pci_enable_msix_exact(pdev, device->msix_entries, msixcnt);
 	if (err)
 		goto msi;
 
@@ -1222,7 +1222,6 @@ int ioat1_dma_probe(struct ioatdma_device *device, int dca)
 	err = ioat_probe(device);
 	if (err)
 		return err;
-	ioat_set_tcp_copy_break(4096);
 	err = ioat_register(device);
 	if (err)
 		return err;
diff --git a/drivers/dma/ioat/dma.h b/drivers/dma/ioat/dma.h
index e982f00a9843..d63f68b1aa35 100644
--- a/drivers/dma/ioat/dma.h
+++ b/drivers/dma/ioat/dma.h
@@ -214,13 +214,6 @@ __dump_desc_dbg(struct ioat_chan_common *chan, struct ioat_dma_descriptor *hw,
 #define dump_desc_dbg(c, d) \
 	({ if (d) __dump_desc_dbg(&c->base, d->hw, &d->txd, desc_id(d)); 0; })
 
-static inline void ioat_set_tcp_copy_break(unsigned long copybreak)
-{
-	#ifdef CONFIG_NET_DMA
-	sysctl_tcp_dma_copybreak = copybreak;
-	#endif
-}
-
 static inline struct ioat_chan_common *
 ioat_chan_by_index(struct ioatdma_device *device, int index)
 {
diff --git a/drivers/dma/ioat/dma_v2.c b/drivers/dma/ioat/dma_v2.c
index 8d1058085eeb..695483e6be32 100644
--- a/drivers/dma/ioat/dma_v2.c
+++ b/drivers/dma/ioat/dma_v2.c
@@ -735,7 +735,8 @@ int ioat2_check_space_lock(struct ioat2_dma_chan *ioat, int num_descs)
 	 * called under bh_disabled so we need to trigger the timer
 	 * event directly
 	 */
-	if (jiffies > chan->timer.expires && timer_pending(&chan->timer)) {
+	if (time_is_before_jiffies(chan->timer.expires)
+	    && timer_pending(&chan->timer)) {
 		struct ioatdma_device *device = chan->device;
 
 		mod_timer(&chan->timer, jiffies + COMPLETION_TIMEOUT);
@@ -899,7 +900,6 @@ int ioat2_dma_probe(struct ioatdma_device *device, int dca)
 	err = ioat_probe(device);
 	if (err)
 		return err;
-	ioat_set_tcp_copy_break(2048);
 
 	list_for_each_entry(c, &dma->channels, device_node) {
 		chan = to_chan_common(c);
diff --git a/drivers/dma/ioat/dma_v3.c b/drivers/dma/ioat/dma_v3.c
index b9b38a1cf92f..895f869d6c2c 100644
--- a/drivers/dma/ioat/dma_v3.c
+++ b/drivers/dma/ioat/dma_v3.c
@@ -740,7 +740,7 @@ ioat3_prep_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 	return __ioat3_prep_xor_lock(chan, NULL, dest, src, src_cnt, len, flags);
 }
 
-struct dma_async_tx_descriptor *
+static struct dma_async_tx_descriptor *
 ioat3_prep_xor_val(struct dma_chan *chan, dma_addr_t *src,
 		    unsigned int src_cnt, size_t len,
 		    enum sum_check_flags *result, unsigned long flags)
@@ -1091,7 +1091,7 @@ ioat3_prep_pq(struct dma_chan *chan, dma_addr_t *dst, dma_addr_t *src,
 	}
 }
 
-struct dma_async_tx_descriptor *
+static struct dma_async_tx_descriptor *
 ioat3_prep_pq_val(struct dma_chan *chan, dma_addr_t *pq, dma_addr_t *src,
 		  unsigned int src_cnt, const unsigned char *scf, size_t len,
 		  enum sum_check_flags *pqres, unsigned long flags)
@@ -1133,7 +1133,7 @@ ioat3_prep_pqxor(struct dma_chan *chan, dma_addr_t dst, dma_addr_t *src,
 				     flags);
 }
 
-struct dma_async_tx_descriptor *
+static struct dma_async_tx_descriptor *
 ioat3_prep_pqxor_val(struct dma_chan *chan, dma_addr_t *src,
 		     unsigned int src_cnt, size_t len,
 		     enum sum_check_flags *result, unsigned long flags)
@@ -1655,7 +1655,6 @@ int ioat3_dma_probe(struct ioatdma_device *device, int dca)
 	err = ioat_probe(device);
 	if (err)
 		return err;
-	ioat_set_tcp_copy_break(262144);
 
 	list_for_each_entry(c, &dma->channels, device_node) {
 		chan = to_chan_common(c);
diff --git a/drivers/dma/iovlock.c b/drivers/dma/iovlock.c
deleted file mode 100644
index bb48a57c2fc1..000000000000
--- a/drivers/dma/iovlock.c
+++ /dev/null
@@ -1,280 +0,0 @@
-/*
- * Copyright(c) 2004 - 2006 Intel Corporation. All rights reserved.
- * Portions based on net/core/datagram.c and copyrighted by their authors.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of the GNU General Public License as published by the Free
- * Software Foundation; either version 2 of the License, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc., 59
- * Temple Place - Suite 330, Boston, MA  02111-1307, USA.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called COPYING.
- */
-
-/*
- * This code allows the net stack to make use of a DMA engine for
- * skb to iovec copies.
- */
-
-#include <linux/dmaengine.h>
-#include <linux/pagemap.h>
-#include <linux/slab.h>
-#include <net/tcp.h> /* for memcpy_toiovec */
-#include <asm/io.h>
-#include <asm/uaccess.h>
-
-static int num_pages_spanned(struct iovec *iov)
-{
-	return
-	((PAGE_ALIGN((unsigned long)iov->iov_base + iov->iov_len) -
-	((unsigned long)iov->iov_base & PAGE_MASK)) >> PAGE_SHIFT);
-}
-
-/*
- * Pin down all the iovec pages needed for len bytes.
- * Return a struct dma_pinned_list to keep track of pages pinned down.
- *
- * We are allocating a single chunk of memory, and then carving it up into
- * 3 sections, the latter 2 whose size depends on the number of iovecs and the
- * total number of pages, respectively.
- */
-struct dma_pinned_list *dma_pin_iovec_pages(struct iovec *iov, size_t len)
-{
-	struct dma_pinned_list *local_list;
-	struct page **pages;
-	int i;
-	int ret;
-	int nr_iovecs = 0;
-	int iovec_len_used = 0;
-	int iovec_pages_used = 0;
-
-	/* don't pin down non-user-based iovecs */
-	if (segment_eq(get_fs(), KERNEL_DS))
-		return NULL;
-
-	/* determine how many iovecs/pages there are, up front */
-	do {
-		iovec_len_used += iov[nr_iovecs].iov_len;
-		iovec_pages_used += num_pages_spanned(&iov[nr_iovecs]);
-		nr_iovecs++;
-	} while (iovec_len_used < len);
-
-	/* single kmalloc for pinned list, page_list[], and the page arrays */
-	local_list = kmalloc(sizeof(*local_list)
-		+ (nr_iovecs * sizeof (struct dma_page_list))
-		+ (iovec_pages_used * sizeof (struct page*)), GFP_KERNEL);
-	if (!local_list)
-		goto out;
-
-	/* list of pages starts right after the page list array */
-	pages = (struct page **) &local_list->page_list[nr_iovecs];
-
-	local_list->nr_iovecs = 0;
-
-	for (i = 0; i < nr_iovecs; i++) {
-		struct dma_page_list *page_list = &local_list->page_list[i];
-
-		len -= iov[i].iov_len;
-
-		if (!access_ok(VERIFY_WRITE, iov[i].iov_base, iov[i].iov_len))
-			goto unpin;
-
-		page_list->nr_pages = num_pages_spanned(&iov[i]);
-		page_list->base_address = iov[i].iov_base;
-
-		page_list->pages = pages;
-		pages += page_list->nr_pages;
-
-		/* pin pages down */
-		down_read(&current->mm->mmap_sem);
-		ret = get_user_pages(
-			current,
-			current->mm,
-			(unsigned long) iov[i].iov_base,
-			page_list->nr_pages,
-			1,	/* write */
-			0,	/* force */
-			page_list->pages,
-			NULL);
-		up_read(&current->mm->mmap_sem);
-
-		if (ret != page_list->nr_pages)
-			goto unpin;
-
-		local_list->nr_iovecs = i + 1;
-	}
-
-	return local_list;
-
-unpin:
-	dma_unpin_iovec_pages(local_list);
-out:
-	return NULL;
-}
-
-void dma_unpin_iovec_pages(struct dma_pinned_list *pinned_list)
-{
-	int i, j;
-
-	if (!pinned_list)
-		return;
-
-	for (i = 0; i < pinned_list->nr_iovecs; i++) {
-		struct dma_page_list *page_list = &pinned_list->page_list[i];
-		for (j = 0; j < page_list->nr_pages; j++) {
-			set_page_dirty_lock(page_list->pages[j]);
-			page_cache_release(page_list->pages[j]);
-		}
-	}
-
-	kfree(pinned_list);
-}
-
-
-/*
- * We have already pinned down the pages we will be using in the iovecs.
- * Each entry in iov array has corresponding entry in pinned_list->page_list.
- * Using array indexing to keep iov[] and page_list[] in sync.
- * Initial elements in iov array's iov->iov_len will be 0 if already copied into
- *   by another call.
- * iov array length remaining guaranteed to be bigger than len.
- */
-dma_cookie_t dma_memcpy_to_iovec(struct dma_chan *chan, struct iovec *iov,
-	struct dma_pinned_list *pinned_list, unsigned char *kdata, size_t len)
-{
-	int iov_byte_offset;
-	int copy;
-	dma_cookie_t dma_cookie = 0;
-	int iovec_idx;
-	int page_idx;
-
-	if (!chan)
-		return memcpy_toiovec(iov, kdata, len);
-
-	iovec_idx = 0;
-	while (iovec_idx < pinned_list->nr_iovecs) {
-		struct dma_page_list *page_list;
-
-		/* skip already used-up iovecs */
-		while (!iov[iovec_idx].iov_len)
-			iovec_idx++;
-
-		page_list = &pinned_list->page_list[iovec_idx];
-
-		iov_byte_offset = ((unsigned long)iov[iovec_idx].iov_base & ~PAGE_MASK);
-		page_idx = (((unsigned long)iov[iovec_idx].iov_base & PAGE_MASK)
-			 - ((unsigned long)page_list->base_address & PAGE_MASK)) >> PAGE_SHIFT;
-
-		/* break up copies to not cross page boundary */
-		while (iov[iovec_idx].iov_len) {
-			copy = min_t(int, PAGE_SIZE - iov_byte_offset, len);
-			copy = min_t(int, copy, iov[iovec_idx].iov_len);
-
-			dma_cookie = dma_async_memcpy_buf_to_pg(chan,
-					page_list->pages[page_idx],
-					iov_byte_offset,
-					kdata,
-					copy);
-			/* poll for a descriptor slot */
-			if (unlikely(dma_cookie < 0)) {
-				dma_async_issue_pending(chan);
-				continue;
-			}
-
-			len -= copy;
-			iov[iovec_idx].iov_len -= copy;
-			iov[iovec_idx].iov_base += copy;
-
-			if (!len)
-				return dma_cookie;
-
-			kdata += copy;
-			iov_byte_offset = 0;
-			page_idx++;
-		}
-		iovec_idx++;
-	}
-
-	/* really bad if we ever run out of iovecs */
-	BUG();
-	return -EFAULT;
-}
-
-dma_cookie_t dma_memcpy_pg_to_iovec(struct dma_chan *chan, struct iovec *iov,
-	struct dma_pinned_list *pinned_list, struct page *page,
-	unsigned int offset, size_t len)
-{
-	int iov_byte_offset;
-	int copy;
-	dma_cookie_t dma_cookie = 0;
-	int iovec_idx;
-	int page_idx;
-	int err;
-
-	/* this needs as-yet-unimplemented buf-to-buff, so punt. */
-	/* TODO: use dma for this */
-	if (!chan || !pinned_list) {
-		u8 *vaddr = kmap(page);
-		err = memcpy_toiovec(iov, vaddr + offset, len);
-		kunmap(page);
-		return err;
-	}
-
-	iovec_idx = 0;
-	while (iovec_idx < pinned_list->nr_iovecs) {
-		struct dma_page_list *page_list;
-
-		/* skip already used-up iovecs */
-		while (!iov[iovec_idx].iov_len)
-			iovec_idx++;
-
-		page_list = &pinned_list->page_list[iovec_idx];
-
-		iov_byte_offset = ((unsigned long)iov[iovec_idx].iov_base & ~PAGE_MASK);
-		page_idx = (((unsigned long)iov[iovec_idx].iov_base & PAGE_MASK)
-			 - ((unsigned long)page_list->base_address & PAGE_MASK)) >> PAGE_SHIFT;
-
-		/* break up copies to not cross page boundary */
-		while (iov[iovec_idx].iov_len) {
-			copy = min_t(int, PAGE_SIZE - iov_byte_offset, len);
-			copy = min_t(int, copy, iov[iovec_idx].iov_len);
-
-			dma_cookie = dma_async_memcpy_pg_to_pg(chan,
-					page_list->pages[page_idx],
-					iov_byte_offset,
-					page,
-					offset,
-					copy);
-			/* poll for a descriptor slot */
-			if (unlikely(dma_cookie < 0)) {
-				dma_async_issue_pending(chan);
-				continue;
-			}
-
-			len -= copy;
-			iov[iovec_idx].iov_len -= copy;
-			iov[iovec_idx].iov_base += copy;
-
-			if (!len)
-				return dma_cookie;
-
-			offset += copy;
-			iov_byte_offset = 0;
-			page_idx++;
-		}
-		iovec_idx++;
-	}
-
-	/* really bad if we ever run out of iovecs */
-	BUG();
-	return -EFAULT;
-}
diff --git a/drivers/dma/mmp_tdma.c b/drivers/dma/mmp_tdma.c
index 6ad30e2c5038..c6bd015b7165 100644
--- a/drivers/dma/mmp_tdma.c
+++ b/drivers/dma/mmp_tdma.c
@@ -148,10 +148,16 @@ static void mmp_tdma_chan_set_desc(struct mmp_tdma_chan *tdmac, dma_addr_t phys)
 					tdmac->reg_base + TDCR);
 }
 
+static void mmp_tdma_enable_irq(struct mmp_tdma_chan *tdmac, bool enable)
+{
+	if (enable)
+		writel(TDIMR_COMP, tdmac->reg_base + TDIMR);
+	else
+		writel(0, tdmac->reg_base + TDIMR);
+}
+
 static void mmp_tdma_enable_chan(struct mmp_tdma_chan *tdmac)
 {
-	/* enable irq */
-	writel(TDIMR_COMP, tdmac->reg_base + TDIMR);
 	/* enable dma chan */
 	writel(readl(tdmac->reg_base + TDCR) | TDCR_CHANEN,
 					tdmac->reg_base + TDCR);
@@ -163,9 +169,6 @@ static void mmp_tdma_disable_chan(struct mmp_tdma_chan *tdmac)
 	writel(readl(tdmac->reg_base + TDCR) & ~TDCR_CHANEN,
 					tdmac->reg_base + TDCR);
 
-	/* disable irq */
-	writel(0, tdmac->reg_base + TDIMR);
-
 	tdmac->status = DMA_COMPLETE;
 }
 
@@ -434,6 +437,10 @@ static struct dma_async_tx_descriptor *mmp_tdma_prep_dma_cyclic(
 		i++;
 	}
 
+	/* enable interrupt */
+	if (flags & DMA_PREP_INTERRUPT)
+		mmp_tdma_enable_irq(tdmac, true);
+
 	tdmac->buf_len = buf_len;
 	tdmac->period_len = period_len;
 	tdmac->pos = 0;
@@ -455,6 +462,8 @@ static int mmp_tdma_control(struct dma_chan *chan, enum dma_ctrl_cmd cmd,
 	switch (cmd) {
 	case DMA_TERMINATE_ALL:
 		mmp_tdma_disable_chan(tdmac);
+		/* disable interrupt */
+		mmp_tdma_enable_irq(tdmac, false);
 		break;
 	case DMA_PAUSE:
 		mmp_tdma_pause_chan(tdmac);
diff --git a/drivers/dma/mv_xor.c b/drivers/dma/mv_xor.c
index 394cbc5c93e3..a63837ca1410 100644
--- a/drivers/dma/mv_xor.c
+++ b/drivers/dma/mv_xor.c
@@ -45,19 +45,18 @@ static void mv_xor_issue_pending(struct dma_chan *chan);
 #define mv_chan_to_devp(chan)           \
 	((chan)->dmadev.dev)
 
-static void mv_desc_init(struct mv_xor_desc_slot *desc, unsigned long flags)
+static void mv_desc_init(struct mv_xor_desc_slot *desc,
+			 dma_addr_t addr, u32 byte_count,
+			 enum dma_ctrl_flags flags)
 {
 	struct mv_xor_desc *hw_desc = desc->hw_desc;
 
-	hw_desc->status = (1 << 31);
+	hw_desc->status = XOR_DESC_DMA_OWNED;
 	hw_desc->phy_next_desc = 0;
-	hw_desc->desc_command = (1 << 31);
-}
-
-static void mv_desc_set_byte_count(struct mv_xor_desc_slot *desc,
-				   u32 byte_count)
-{
-	struct mv_xor_desc *hw_desc = desc->hw_desc;
+	/* Enable end-of-descriptor interrupts only for DMA_PREP_INTERRUPT */
+	hw_desc->desc_command = (flags & DMA_PREP_INTERRUPT) ?
+				XOR_DESC_EOD_INT_EN : 0;
+	hw_desc->phy_dest_addr = addr;
 	hw_desc->byte_count = byte_count;
 }
 
@@ -75,20 +74,6 @@ static void mv_desc_clear_next_desc(struct mv_xor_desc_slot *desc)
 	hw_desc->phy_next_desc = 0;
 }
 
-static void mv_desc_set_dest_addr(struct mv_xor_desc_slot *desc,
-				  dma_addr_t addr)
-{
-	struct mv_xor_desc *hw_desc = desc->hw_desc;
-	hw_desc->phy_dest_addr = addr;
-}
-
-static int mv_chan_memset_slot_count(size_t len)
-{
-	return 1;
-}
-
-#define mv_chan_memcpy_slot_count(c) mv_chan_memset_slot_count(c)
-
 static void mv_desc_set_src_addr(struct mv_xor_desc_slot *desc,
 				 int index, dma_addr_t addr)
 {
@@ -123,17 +108,12 @@ static u32 mv_chan_get_intr_cause(struct mv_xor_chan *chan)
 	return intr_cause;
 }
 
-static int mv_is_err_intr(u32 intr_cause)
-{
-	if (intr_cause & ((1<<4)|(1<<5)|(1<<6)|(1<<7)|(1<<8)|(1<<9)))
-		return 1;
-
-	return 0;
-}
-
 static void mv_xor_device_clear_eoc_cause(struct mv_xor_chan *chan)
 {
-	u32 val = ~(1 << (chan->idx * 16));
+	u32 val;
+
+	val = XOR_INT_END_OF_DESC | XOR_INT_END_OF_CHAIN | XOR_INT_STOPPED;
+	val = ~(val << (chan->idx * 16));
 	dev_dbg(mv_chan_to_devp(chan), "%s, val 0x%08x\n", __func__, val);
 	writel_relaxed(val, XOR_INTR_CAUSE(chan));
 }
@@ -144,17 +124,6 @@ static void mv_xor_device_clear_err_status(struct mv_xor_chan *chan)
 	writel_relaxed(val, XOR_INTR_CAUSE(chan));
 }
 
-static int mv_can_chain(struct mv_xor_desc_slot *desc)
-{
-	struct mv_xor_desc_slot *chain_old_tail = list_entry(
-		desc->chain_node.prev, struct mv_xor_desc_slot, chain_node);
-
-	if (chain_old_tail->type != desc->type)
-		return 0;
-
-	return 1;
-}
-
 static void mv_set_mode(struct mv_xor_chan *chan,
 			       enum dma_transaction_type type)
 {
@@ -206,11 +175,6 @@ static char mv_chan_is_busy(struct mv_xor_chan *chan)
 	return (state == 1) ? 1 : 0;
 }
 
-static int mv_chan_xor_slot_count(size_t len, int src_cnt)
-{
-	return 1;
-}
-
 /**
  * mv_xor_free_slots - flags descriptor slots for reuse
  * @slot: Slot to free
@@ -222,7 +186,7 @@ static void mv_xor_free_slots(struct mv_xor_chan *mv_chan,
 	dev_dbg(mv_chan_to_devp(mv_chan), "%s %d slot %p\n",
 		__func__, __LINE__, slot);
 
-	slot->slots_per_op = 0;
+	slot->slot_used = 0;
 
 }
 
@@ -236,13 +200,11 @@ static void mv_xor_start_new_chain(struct mv_xor_chan *mv_chan,
 {
 	dev_dbg(mv_chan_to_devp(mv_chan), "%s %d: sw_desc %p\n",
 		__func__, __LINE__, sw_desc);
-	if (sw_desc->type != mv_chan->current_type)
-		mv_set_mode(mv_chan, sw_desc->type);
 
 	/* set the hardware chain */
 	mv_chan_set_next_descriptor(mv_chan, sw_desc->async_tx.phys);
 
-	mv_chan->pending += sw_desc->slot_cnt;
+	mv_chan->pending++;
 	mv_xor_issue_pending(&mv_chan->dmachan);
 }
 
@@ -263,8 +225,6 @@ mv_xor_run_tx_complete_actions(struct mv_xor_desc_slot *desc,
 				desc->async_tx.callback_param);
 
 		dma_descriptor_unmap(&desc->async_tx);
-		if (desc->group_head)
-			desc->group_head = NULL;
 	}
 
 	/* run dependent operations */
@@ -310,7 +270,8 @@ mv_xor_clean_slot(struct mv_xor_desc_slot *desc,
 	return 0;
 }
 
-static void __mv_xor_slot_cleanup(struct mv_xor_chan *mv_chan)
+/* This function must be called with the mv_xor_chan spinlock held */
+static void mv_xor_slot_cleanup(struct mv_xor_chan *mv_chan)
 {
 	struct mv_xor_desc_slot *iter, *_iter;
 	dma_cookie_t cookie = 0;
@@ -366,34 +327,26 @@ static void __mv_xor_slot_cleanup(struct mv_xor_chan *mv_chan)
 		mv_chan->dmachan.completed_cookie = cookie;
 }
 
-static void
-mv_xor_slot_cleanup(struct mv_xor_chan *mv_chan)
-{
-	spin_lock_bh(&mv_chan->lock);
-	__mv_xor_slot_cleanup(mv_chan);
-	spin_unlock_bh(&mv_chan->lock);
-}
-
 static void mv_xor_tasklet(unsigned long data)
 {
 	struct mv_xor_chan *chan = (struct mv_xor_chan *) data;
+
+	spin_lock_bh(&chan->lock);
 	mv_xor_slot_cleanup(chan);
+	spin_unlock_bh(&chan->lock);
 }
 
 static struct mv_xor_desc_slot *
-mv_xor_alloc_slots(struct mv_xor_chan *mv_chan, int num_slots,
-		    int slots_per_op)
+mv_xor_alloc_slot(struct mv_xor_chan *mv_chan)
 {
-	struct mv_xor_desc_slot *iter, *_iter, *alloc_start = NULL;
-	LIST_HEAD(chain);
-	int slots_found, retry = 0;
+	struct mv_xor_desc_slot *iter, *_iter;
+	int retry = 0;
 
 	/* start search from the last allocated descrtiptor
 	 * if a contiguous allocation can not be found start searching
 	 * from the beginning of the list
 	 */
 retry:
-	slots_found = 0;
 	if (retry == 0)
 		iter = mv_chan->last_used;
 	else
@@ -403,55 +356,29 @@ retry:
 
 	list_for_each_entry_safe_continue(
 		iter, _iter, &mv_chan->all_slots, slot_node) {
+
 		prefetch(_iter);
 		prefetch(&_iter->async_tx);
-		if (iter->slots_per_op) {
+		if (iter->slot_used) {
 			/* give up after finding the first busy slot
 			 * on the second pass through the list
 			 */
 			if (retry)
 				break;
-
-			slots_found = 0;
 			continue;
 		}
 
-		/* start the allocation if the slot is correctly aligned */
-		if (!slots_found++)
-			alloc_start = iter;
-
-		if (slots_found == num_slots) {
-			struct mv_xor_desc_slot *alloc_tail = NULL;
-			struct mv_xor_desc_slot *last_used = NULL;
-			iter = alloc_start;
-			while (num_slots) {
-				int i;
-
-				/* pre-ack all but the last descriptor */
-				async_tx_ack(&iter->async_tx);
-
-				list_add_tail(&iter->chain_node, &chain);
-				alloc_tail = iter;
-				iter->async_tx.cookie = 0;
-				iter->slot_cnt = num_slots;
-				iter->xor_check_result = NULL;
-				for (i = 0; i < slots_per_op; i++) {
-					iter->slots_per_op = slots_per_op - i;
-					last_used = iter;
-					iter = list_entry(iter->slot_node.next,
-						struct mv_xor_desc_slot,
-						slot_node);
-				}
-				num_slots -= slots_per_op;
-			}
-			alloc_tail->group_head = alloc_start;
-			alloc_tail->async_tx.cookie = -EBUSY;
-			list_splice(&chain, &alloc_tail->tx_list);
-			mv_chan->last_used = last_used;
-			mv_desc_clear_next_desc(alloc_start);
-			mv_desc_clear_next_desc(alloc_tail);
-			return alloc_tail;
-		}
+		/* pre-ack descriptor */
+		async_tx_ack(&iter->async_tx);
+
+		iter->slot_used = 1;
+		INIT_LIST_HEAD(&iter->chain_node);
+		iter->async_tx.cookie = -EBUSY;
+		mv_chan->last_used = iter;
+		mv_desc_clear_next_desc(iter);
+
+		return iter;
+
 	}
 	if (!retry++)
 		goto retry;
@@ -468,7 +395,7 @@ mv_xor_tx_submit(struct dma_async_tx_descriptor *tx)
 {
 	struct mv_xor_desc_slot *sw_desc = to_mv_xor_slot(tx);
 	struct mv_xor_chan *mv_chan = to_mv_xor_chan(tx->chan);
-	struct mv_xor_desc_slot *grp_start, *old_chain_tail;
+	struct mv_xor_desc_slot *old_chain_tail;
 	dma_cookie_t cookie;
 	int new_hw_chain = 1;
 
@@ -476,30 +403,24 @@ mv_xor_tx_submit(struct dma_async_tx_descriptor *tx)
 		"%s sw_desc %p: async_tx %p\n",
 		__func__, sw_desc, &sw_desc->async_tx);
 
-	grp_start = sw_desc->group_head;
-
 	spin_lock_bh(&mv_chan->lock);
 	cookie = dma_cookie_assign(tx);
 
 	if (list_empty(&mv_chan->chain))
-		list_splice_init(&sw_desc->tx_list, &mv_chan->chain);
+		list_add_tail(&sw_desc->chain_node, &mv_chan->chain);
 	else {
 		new_hw_chain = 0;
 
 		old_chain_tail = list_entry(mv_chan->chain.prev,
 					    struct mv_xor_desc_slot,
 					    chain_node);
-		list_splice_init(&grp_start->tx_list,
-				 &old_chain_tail->chain_node);
-
-		if (!mv_can_chain(grp_start))
-			goto submit_done;
+		list_add_tail(&sw_desc->chain_node, &mv_chan->chain);
 
 		dev_dbg(mv_chan_to_devp(mv_chan), "Append to last desc %pa\n",
 			&old_chain_tail->async_tx.phys);
 
 		/* fix up the hardware chain */
-		mv_desc_set_next_desc(old_chain_tail, grp_start->async_tx.phys);
+		mv_desc_set_next_desc(old_chain_tail, sw_desc->async_tx.phys);
 
 		/* if the channel is not busy */
 		if (!mv_chan_is_busy(mv_chan)) {
@@ -514,9 +435,8 @@ mv_xor_tx_submit(struct dma_async_tx_descriptor *tx)
 	}
 
 	if (new_hw_chain)
-		mv_xor_start_new_chain(mv_chan, grp_start);
+		mv_xor_start_new_chain(mv_chan, sw_desc);
 
-submit_done:
 	spin_unlock_bh(&mv_chan->lock);
 
 	return cookie;
@@ -537,8 +457,9 @@ static int mv_xor_alloc_chan_resources(struct dma_chan *chan)
 	while (idx < num_descs_in_pool) {
 		slot = kzalloc(sizeof(*slot), GFP_KERNEL);
 		if (!slot) {
-			printk(KERN_INFO "MV XOR Channel only initialized"
-				" %d descriptor slots", idx);
+			dev_info(mv_chan_to_devp(mv_chan),
+				 "channel only initialized %d descriptor slots",
+				 idx);
 			break;
 		}
 		virt_desc = mv_chan->dma_desc_pool_virt;
@@ -548,7 +469,6 @@ static int mv_xor_alloc_chan_resources(struct dma_chan *chan)
 		slot->async_tx.tx_submit = mv_xor_tx_submit;
 		INIT_LIST_HEAD(&slot->chain_node);
 		INIT_LIST_HEAD(&slot->slot_node);
-		INIT_LIST_HEAD(&slot->tx_list);
 		dma_desc = mv_chan->dma_desc_pool;
 		slot->async_tx.phys = dma_desc + idx * MV_XOR_SLOT_SIZE;
 		slot->idx = idx++;
@@ -572,51 +492,11 @@ static int mv_xor_alloc_chan_resources(struct dma_chan *chan)
 }
 
 static struct dma_async_tx_descriptor *
-mv_xor_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
-		size_t len, unsigned long flags)
-{
-	struct mv_xor_chan *mv_chan = to_mv_xor_chan(chan);
-	struct mv_xor_desc_slot *sw_desc, *grp_start;
-	int slot_cnt;
-
-	dev_dbg(mv_chan_to_devp(mv_chan),
-		"%s dest: %pad src %pad len: %u flags: %ld\n",
-		__func__, &dest, &src, len, flags);
-	if (unlikely(len < MV_XOR_MIN_BYTE_COUNT))
-		return NULL;
-
-	BUG_ON(len > MV_XOR_MAX_BYTE_COUNT);
-
-	spin_lock_bh(&mv_chan->lock);
-	slot_cnt = mv_chan_memcpy_slot_count(len);
-	sw_desc = mv_xor_alloc_slots(mv_chan, slot_cnt, 1);
-	if (sw_desc) {
-		sw_desc->type = DMA_MEMCPY;
-		sw_desc->async_tx.flags = flags;
-		grp_start = sw_desc->group_head;
-		mv_desc_init(grp_start, flags);
-		mv_desc_set_byte_count(grp_start, len);
-		mv_desc_set_dest_addr(sw_desc->group_head, dest);
-		mv_desc_set_src_addr(grp_start, 0, src);
-		sw_desc->unmap_src_cnt = 1;
-		sw_desc->unmap_len = len;
-	}
-	spin_unlock_bh(&mv_chan->lock);
-
-	dev_dbg(mv_chan_to_devp(mv_chan),
-		"%s sw_desc %p async_tx %p\n",
-		__func__, sw_desc, sw_desc ? &sw_desc->async_tx : NULL);
-
-	return sw_desc ? &sw_desc->async_tx : NULL;
-}
-
-static struct dma_async_tx_descriptor *
 mv_xor_prep_dma_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 		    unsigned int src_cnt, size_t len, unsigned long flags)
 {
 	struct mv_xor_chan *mv_chan = to_mv_xor_chan(chan);
-	struct mv_xor_desc_slot *sw_desc, *grp_start;
-	int slot_cnt;
+	struct mv_xor_desc_slot *sw_desc;
 
 	if (unlikely(len < MV_XOR_MIN_BYTE_COUNT))
 		return NULL;
@@ -628,20 +508,13 @@ mv_xor_prep_dma_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 		__func__, src_cnt, len, &dest, flags);
 
 	spin_lock_bh(&mv_chan->lock);
-	slot_cnt = mv_chan_xor_slot_count(len, src_cnt);
-	sw_desc = mv_xor_alloc_slots(mv_chan, slot_cnt, 1);
+	sw_desc = mv_xor_alloc_slot(mv_chan);
 	if (sw_desc) {
 		sw_desc->type = DMA_XOR;
 		sw_desc->async_tx.flags = flags;
-		grp_start = sw_desc->group_head;
-		mv_desc_init(grp_start, flags);
-		/* the byte count field is the same as in memcpy desc*/
-		mv_desc_set_byte_count(grp_start, len);
-		mv_desc_set_dest_addr(sw_desc->group_head, dest);
-		sw_desc->unmap_src_cnt = src_cnt;
-		sw_desc->unmap_len = len;
+		mv_desc_init(sw_desc, dest, len, flags);
 		while (src_cnt--)
-			mv_desc_set_src_addr(grp_start, src_cnt, src[src_cnt]);
+			mv_desc_set_src_addr(sw_desc, src_cnt, src[src_cnt]);
 	}
 	spin_unlock_bh(&mv_chan->lock);
 	dev_dbg(mv_chan_to_devp(mv_chan),
@@ -650,15 +523,45 @@ mv_xor_prep_dma_xor(struct dma_chan *chan, dma_addr_t dest, dma_addr_t *src,
 	return sw_desc ? &sw_desc->async_tx : NULL;
 }
 
+static struct dma_async_tx_descriptor *
+mv_xor_prep_dma_memcpy(struct dma_chan *chan, dma_addr_t dest, dma_addr_t src,
+		size_t len, unsigned long flags)
+{
+	/*
+	 * A MEMCPY operation is identical to an XOR operation with only
+	 * a single source address.
+	 */
+	return mv_xor_prep_dma_xor(chan, dest, &src, 1, len, flags);
+}
+
+static struct dma_async_tx_descriptor *
+mv_xor_prep_dma_interrupt(struct dma_chan *chan, unsigned long flags)
+{
+	struct mv_xor_chan *mv_chan = to_mv_xor_chan(chan);
+	dma_addr_t src, dest;
+	size_t len;
+
+	src = mv_chan->dummy_src_addr;
+	dest = mv_chan->dummy_dst_addr;
+	len = MV_XOR_MIN_BYTE_COUNT;
+
+	/*
+	 * We implement the DMA_INTERRUPT operation as a minimum sized
+	 * XOR operation with a single dummy source address.
+	 */
+	return mv_xor_prep_dma_xor(chan, dest, &src, 1, len, flags);
+}
+
 static void mv_xor_free_chan_resources(struct dma_chan *chan)
 {
 	struct mv_xor_chan *mv_chan = to_mv_xor_chan(chan);
 	struct mv_xor_desc_slot *iter, *_iter;
 	int in_use_descs = 0;
 
+	spin_lock_bh(&mv_chan->lock);
+
 	mv_xor_slot_cleanup(mv_chan);
 
-	spin_lock_bh(&mv_chan->lock);
 	list_for_each_entry_safe(iter, _iter, &mv_chan->chain,
 					chain_node) {
 		in_use_descs++;
@@ -700,11 +603,12 @@ static enum dma_status mv_xor_status(struct dma_chan *chan,
 	enum dma_status ret;
 
 	ret = dma_cookie_status(chan, cookie, txstate);
-	if (ret == DMA_COMPLETE) {
-		mv_xor_clean_completed_slots(mv_chan);
+	if (ret == DMA_COMPLETE)
 		return ret;
-	}
+
+	spin_lock_bh(&mv_chan->lock);
 	mv_xor_slot_cleanup(mv_chan);
+	spin_unlock_bh(&mv_chan->lock);
 
 	return dma_cookie_status(chan, cookie, txstate);
 }
@@ -735,18 +639,16 @@ static void mv_dump_xor_regs(struct mv_xor_chan *chan)
 static void mv_xor_err_interrupt_handler(struct mv_xor_chan *chan,
 					 u32 intr_cause)
 {
-	if (intr_cause & (1 << 4)) {
-	     dev_dbg(mv_chan_to_devp(chan),
-		     "ignore this error\n");
-	     return;
+	if (intr_cause & XOR_INT_ERR_DECODE) {
+		dev_dbg(mv_chan_to_devp(chan), "ignoring address decode error\n");
+		return;
 	}
 
-	dev_err(mv_chan_to_devp(chan),
-		"error on chan %d. intr cause 0x%08x\n",
+	dev_err(mv_chan_to_devp(chan), "error on chan %d. intr cause 0x%08x\n",
 		chan->idx, intr_cause);
 
 	mv_dump_xor_regs(chan);
-	BUG();
+	WARN_ON(1);
 }
 
 static irqreturn_t mv_xor_interrupt_handler(int irq, void *data)
@@ -756,7 +658,7 @@ static irqreturn_t mv_xor_interrupt_handler(int irq, void *data)
 
 	dev_dbg(mv_chan_to_devp(chan), "intr cause %x\n", intr_cause);
 
-	if (mv_is_err_intr(intr_cause))
+	if (intr_cause & XOR_INTR_ERRORS)
 		mv_xor_err_interrupt_handler(chan, intr_cause);
 
 	tasklet_schedule(&chan->irq_tasklet);
@@ -782,7 +684,7 @@ static void mv_xor_issue_pending(struct dma_chan *chan)
 
 static int mv_xor_memcpy_self_test(struct mv_xor_chan *mv_chan)
 {
-	int i;
+	int i, ret;
 	void *src, *dest;
 	dma_addr_t src_dma, dest_dma;
 	struct dma_chan *dma_chan;
@@ -819,19 +721,44 @@ static int mv_xor_memcpy_self_test(struct mv_xor_chan *mv_chan)
 
 	src_dma = dma_map_page(dma_chan->device->dev, virt_to_page(src), 0,
 				 PAGE_SIZE, DMA_TO_DEVICE);
-	unmap->to_cnt = 1;
 	unmap->addr[0] = src_dma;
 
+	ret = dma_mapping_error(dma_chan->device->dev, src_dma);
+	if (ret) {
+		err = -ENOMEM;
+		goto free_resources;
+	}
+	unmap->to_cnt = 1;
+
 	dest_dma = dma_map_page(dma_chan->device->dev, virt_to_page(dest), 0,
 				  PAGE_SIZE, DMA_FROM_DEVICE);
-	unmap->from_cnt = 1;
 	unmap->addr[1] = dest_dma;
 
+	ret = dma_mapping_error(dma_chan->device->dev, dest_dma);
+	if (ret) {
+		err = -ENOMEM;
+		goto free_resources;
+	}
+	unmap->from_cnt = 1;
 	unmap->len = PAGE_SIZE;
 
 	tx = mv_xor_prep_dma_memcpy(dma_chan, dest_dma, src_dma,
 				    PAGE_SIZE, 0);
+	if (!tx) {
+		dev_err(dma_chan->device->dev,
+			"Self-test cannot prepare operation, disabling\n");
+		err = -ENODEV;
+		goto free_resources;
+	}
+
 	cookie = mv_xor_tx_submit(tx);
+	if (dma_submit_error(cookie)) {
+		dev_err(dma_chan->device->dev,
+			"Self-test submit error, disabling\n");
+		err = -ENODEV;
+		goto free_resources;
+	}
+
 	mv_xor_issue_pending(dma_chan);
 	async_tx_ack(tx);
 	msleep(1);
@@ -866,7 +793,7 @@ out:
 static int
 mv_xor_xor_self_test(struct mv_xor_chan *mv_chan)
 {
-	int i, src_idx;
+	int i, src_idx, ret;
 	struct page *dest;
 	struct page *xor_srcs[MV_XOR_NUM_SRC_TEST];
 	dma_addr_t dma_srcs[MV_XOR_NUM_SRC_TEST];
@@ -929,19 +856,42 @@ mv_xor_xor_self_test(struct mv_xor_chan *mv_chan)
 		unmap->addr[i] = dma_map_page(dma_chan->device->dev, xor_srcs[i],
 					      0, PAGE_SIZE, DMA_TO_DEVICE);
 		dma_srcs[i] = unmap->addr[i];
+		ret = dma_mapping_error(dma_chan->device->dev, unmap->addr[i]);
+		if (ret) {
+			err = -ENOMEM;
+			goto free_resources;
+		}
 		unmap->to_cnt++;
 	}
 
 	unmap->addr[src_count] = dma_map_page(dma_chan->device->dev, dest, 0, PAGE_SIZE,
 				      DMA_FROM_DEVICE);
 	dest_dma = unmap->addr[src_count];
+	ret = dma_mapping_error(dma_chan->device->dev, unmap->addr[src_count]);
+	if (ret) {
+		err = -ENOMEM;
+		goto free_resources;
+	}
 	unmap->from_cnt = 1;
 	unmap->len = PAGE_SIZE;
 
 	tx = mv_xor_prep_dma_xor(dma_chan, dest_dma, dma_srcs,
 				 src_count, PAGE_SIZE, 0);
+	if (!tx) {
+		dev_err(dma_chan->device->dev,
+			"Self-test cannot prepare operation, disabling\n");
+		err = -ENODEV;
+		goto free_resources;
+	}
 
 	cookie = mv_xor_tx_submit(tx);
+	if (dma_submit_error(cookie)) {
+		dev_err(dma_chan->device->dev,
+			"Self-test submit error, disabling\n");
+		err = -ENODEV;
+		goto free_resources;
+	}
+
 	mv_xor_issue_pending(dma_chan);
 	async_tx_ack(tx);
 	msleep(8);
@@ -995,6 +945,10 @@ static int mv_xor_channel_remove(struct mv_xor_chan *mv_chan)
 
 	dma_free_coherent(dev, MV_XOR_POOL_SIZE,
 			  mv_chan->dma_desc_pool_virt, mv_chan->dma_desc_pool);
+	dma_unmap_single(dev, mv_chan->dummy_src_addr,
+			 MV_XOR_MIN_BYTE_COUNT, DMA_FROM_DEVICE);
+	dma_unmap_single(dev, mv_chan->dummy_dst_addr,
+			 MV_XOR_MIN_BYTE_COUNT, DMA_TO_DEVICE);
 
 	list_for_each_entry_safe(chan, _chan, &mv_chan->dmadev.channels,
 				 device_node) {
@@ -1024,6 +978,16 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 
 	dma_dev = &mv_chan->dmadev;
 
+	/*
+	 * These source and destination dummy buffers are used to implement
+	 * a DMA_INTERRUPT operation as a minimum-sized XOR operation.
+	 * Hence, we only need to map the buffers at initialization-time.
+	 */
+	mv_chan->dummy_src_addr = dma_map_single(dma_dev->dev,
+		mv_chan->dummy_src, MV_XOR_MIN_BYTE_COUNT, DMA_FROM_DEVICE);
+	mv_chan->dummy_dst_addr = dma_map_single(dma_dev->dev,
+		mv_chan->dummy_dst, MV_XOR_MIN_BYTE_COUNT, DMA_TO_DEVICE);
+
 	/* allocate coherent memory for hardware descriptors
 	 * note: writecombine gives slightly better performance, but
 	 * requires that we explicitly flush the writes
@@ -1048,6 +1012,8 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 	dma_dev->dev = &pdev->dev;
 
 	/* set prep routines based on capability */
+	if (dma_has_cap(DMA_INTERRUPT, dma_dev->cap_mask))
+		dma_dev->device_prep_dma_interrupt = mv_xor_prep_dma_interrupt;
 	if (dma_has_cap(DMA_MEMCPY, dma_dev->cap_mask))
 		dma_dev->device_prep_dma_memcpy = mv_xor_prep_dma_memcpy;
 	if (dma_has_cap(DMA_XOR, dma_dev->cap_mask)) {
@@ -1070,7 +1036,7 @@ mv_xor_channel_add(struct mv_xor_device *xordev,
 
 	mv_chan_unmask_interrupts(mv_chan);
 
-	mv_set_mode(mv_chan, DMA_MEMCPY);
+	mv_set_mode(mv_chan, DMA_XOR);
 
 	spin_lock_init(&mv_chan->lock);
 	INIT_LIST_HEAD(&mv_chan->chain);
diff --git a/drivers/dma/mv_xor.h b/drivers/dma/mv_xor.h
index d0749229c875..78edc7e44569 100644
--- a/drivers/dma/mv_xor.h
+++ b/drivers/dma/mv_xor.h
@@ -23,17 +23,22 @@
 #include <linux/dmaengine.h>
 #include <linux/interrupt.h>
 
-#define USE_TIMER
 #define MV_XOR_POOL_SIZE		PAGE_SIZE
 #define MV_XOR_SLOT_SIZE		64
 #define MV_XOR_THRESHOLD		1
 #define MV_XOR_MAX_CHANNELS             2
 
+#define MV_XOR_MIN_BYTE_COUNT		SZ_128
+#define MV_XOR_MAX_BYTE_COUNT		(SZ_16M - 1)
+
 /* Values for the XOR_CONFIG register */
 #define XOR_OPERATION_MODE_XOR		0
 #define XOR_OPERATION_MODE_MEMCPY	2
 #define XOR_DESCRIPTOR_SWAP		BIT(14)
 
+#define XOR_DESC_DMA_OWNED		BIT(31)
+#define XOR_DESC_EOD_INT_EN		BIT(31)
+
 #define XOR_CURR_DESC(chan)	(chan->mmr_high_base + 0x10 + (chan->idx * 4))
 #define XOR_NEXT_DESC(chan)	(chan->mmr_high_base + 0x00 + (chan->idx * 4))
 #define XOR_BYTE_COUNT(chan)	(chan->mmr_high_base + 0x20 + (chan->idx * 4))
@@ -48,7 +53,24 @@
 #define XOR_INTR_MASK(chan)	(chan->mmr_base + 0x40)
 #define XOR_ERROR_CAUSE(chan)	(chan->mmr_base + 0x50)
 #define XOR_ERROR_ADDR(chan)	(chan->mmr_base + 0x60)
-#define XOR_INTR_MASK_VALUE	0x3F5
+
+#define XOR_INT_END_OF_DESC	BIT(0)
+#define XOR_INT_END_OF_CHAIN	BIT(1)
+#define XOR_INT_STOPPED		BIT(2)
+#define XOR_INT_PAUSED		BIT(3)
+#define XOR_INT_ERR_DECODE	BIT(4)
+#define XOR_INT_ERR_RDPROT	BIT(5)
+#define XOR_INT_ERR_WRPROT	BIT(6)
+#define XOR_INT_ERR_OWN		BIT(7)
+#define XOR_INT_ERR_PAR		BIT(8)
+#define XOR_INT_ERR_MBUS	BIT(9)
+
+#define XOR_INTR_ERRORS		(XOR_INT_ERR_DECODE | XOR_INT_ERR_RDPROT | \
+				 XOR_INT_ERR_WRPROT | XOR_INT_ERR_OWN    | \
+				 XOR_INT_ERR_PAR    | XOR_INT_ERR_MBUS)
+
+#define XOR_INTR_MASK_VALUE	(XOR_INT_END_OF_DESC | XOR_INT_END_OF_CHAIN | \
+				 XOR_INT_STOPPED     | XOR_INTR_ERRORS)
 
 #define WINDOW_BASE(w)		(0x50 + ((w) << 2))
 #define WINDOW_SIZE(w)		(0x70 + ((w) << 2))
@@ -97,10 +119,9 @@ struct mv_xor_chan {
 	struct list_head	all_slots;
 	int			slots_allocated;
 	struct tasklet_struct	irq_tasklet;
-#ifdef USE_TIMER
-	unsigned long		cleanup_time;
-	u32			current_on_last_cleanup;
-#endif
+	char			dummy_src[MV_XOR_MIN_BYTE_COUNT];
+	char			dummy_dst[MV_XOR_MIN_BYTE_COUNT];
+	dma_addr_t		dummy_src_addr, dummy_dst_addr;
 };
 
 /**
@@ -110,16 +131,10 @@ struct mv_xor_chan {
  * @completed_node: node on the mv_xor_chan.completed_slots list
  * @hw_desc: virtual address of the hardware descriptor chain
  * @phys: hardware address of the hardware descriptor chain
- * @group_head: first operation in a transaction
- * @slot_cnt: total slots used in an transaction (group of operations)
- * @slots_per_op: number of slots per operation
+ * @slot_used: slot in use or not
  * @idx: pool index
- * @unmap_src_cnt: number of xor sources
- * @unmap_len: transaction bytecount
  * @tx_list: list of slots that make up a multi-descriptor transaction
  * @async_tx: support for the async_tx api
- * @xor_check_result: result of zero sum
- * @crc32_result: result crc calculation
  */
 struct mv_xor_desc_slot {
 	struct list_head	slot_node;
@@ -127,23 +142,9 @@ struct mv_xor_desc_slot {
 	struct list_head	completed_node;
 	enum dma_transaction_type	type;
 	void			*hw_desc;
-	struct mv_xor_desc_slot	*group_head;
-	u16			slot_cnt;
-	u16			slots_per_op;
+	u16			slot_used;
 	u16			idx;
-	u16			unmap_src_cnt;
-	u32			value;
-	size_t			unmap_len;
-	struct list_head	tx_list;
 	struct dma_async_tx_descriptor	async_tx;
-	union {
-		u32		*xor_check_result;
-		u32		*crc32_result;
-	};
-#ifdef USE_TIMER
-	unsigned long		arrival_time;
-	struct timer_list	timeout;
-#endif
 };
 
 /*
@@ -189,9 +190,4 @@ struct mv_xor_desc {
 #define mv_hw_desc_slot_idx(hw_desc, idx)	\
 	((void *)(((unsigned long)hw_desc) + ((idx) << 5)))
 
-#define MV_XOR_MIN_BYTE_COUNT	(128)
-#define XOR_MAX_BYTE_COUNT	((16 * 1024 * 1024) - 1)
-#define MV_XOR_MAX_BYTE_COUNT	XOR_MAX_BYTE_COUNT
-
-
 #endif
diff --git a/drivers/dma/omap-dma.c b/drivers/dma/omap-dma.c
index 4cf7d9a950d7..bbea8243f9e8 100644
--- a/drivers/dma/omap-dma.c
+++ b/drivers/dma/omap-dma.c
@@ -1017,6 +1017,11 @@ static int omap_dma_resume(struct omap_chan *c)
 		return -EINVAL;
 
 	if (c->paused) {
+		mb();
+
+		/* Restore channel link register */
+		omap_dma_chan_write(c, CLNK_CTRL, c->desc->clnk_ctrl);
+
 		omap_dma_start(c, c->desc);
 		c->paused = false;
 	}
diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index d5149aacd2fe..4839bfa74a10 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -1367,17 +1367,10 @@ static int pl330_submit_req(struct pl330_thread *thrd,
 	struct pl330_dmac *pl330 = thrd->dmac;
 	struct _xfer_spec xs;
 	unsigned long flags;
-	void __iomem *regs;
 	unsigned idx;
 	u32 ccr;
 	int ret = 0;
 
-	/* No Req or Unacquired Channel or DMAC */
-	if (!desc || !thrd || thrd->free)
-		return -EINVAL;
-
-	regs = thrd->dmac->base;
-
 	if (pl330->state == DYING
 		|| pl330->dmac_tbd.reset_chan & (1 << thrd->id)) {
 		dev_info(thrd->dmac->ddma.dev, "%s:%d\n",
@@ -2755,8 +2748,10 @@ probe_err3:
 		list_del(&pch->chan.device_node);
 
 		/* Flush the channel */
-		pl330_control(&pch->chan, DMA_TERMINATE_ALL, 0);
-		pl330_free_chan_resources(&pch->chan);
+		if (pch->thread) {
+			pl330_control(&pch->chan, DMA_TERMINATE_ALL, 0);
+			pl330_free_chan_resources(&pch->chan);
+		}
 	}
 probe_err2:
 	pl330_del(pl330);
@@ -2782,8 +2777,10 @@ static int pl330_remove(struct amba_device *adev)
 		list_del(&pch->chan.device_node);
 
 		/* Flush the channel */
-		pl330_control(&pch->chan, DMA_TERMINATE_ALL, 0);
-		pl330_free_chan_resources(&pch->chan);
+		if (pch->thread) {
+			pl330_control(&pch->chan, DMA_TERMINATE_ALL, 0);
+			pl330_free_chan_resources(&pch->chan);
+		}
 	}
 
 	pl330_del(pl330);
diff --git a/drivers/dma/sh/rcar-audmapp.c b/drivers/dma/sh/rcar-audmapp.c
index dabbf0aba2e9..80fd2aeb4870 100644
--- a/drivers/dma/sh/rcar-audmapp.c
+++ b/drivers/dma/sh/rcar-audmapp.c
@@ -117,7 +117,7 @@ static void audmapp_start_xfer(struct shdma_chan *schan,
 	audmapp_write(auchan, chcr,	PDMACHCR);
 }
 
-static void audmapp_get_config(struct audmapp_chan *auchan, int slave_id,
+static int audmapp_get_config(struct audmapp_chan *auchan, int slave_id,
 			      u32 *chcr, dma_addr_t *dst)
 {
 	struct audmapp_device *audev = to_dev(auchan);
@@ -131,20 +131,22 @@ static void audmapp_get_config(struct audmapp_chan *auchan, int slave_id,
 	if (!pdata) { /* DT */
 		*chcr = ((u32)slave_id) << 16;
 		auchan->shdma_chan.slave_id = (slave_id) >> 8;
-		return;
+		return 0;
 	}
 
 	/* non-DT */
 
 	if (slave_id >= AUDMAPP_SLAVE_NUMBER)
-		return;
+		return -ENXIO;
 
 	for (i = 0, cfg = pdata->slave; i < pdata->slave_num; i++, cfg++)
 		if (cfg->slave_id == slave_id) {
 			*chcr	= cfg->chcr;
 			*dst	= cfg->dst;
-			break;
+			return 0;
 		}
+
+	return -ENXIO;
 }
 
 static int audmapp_set_slave(struct shdma_chan *schan, int slave_id,
@@ -153,8 +155,11 @@ static int audmapp_set_slave(struct shdma_chan *schan, int slave_id,
 	struct audmapp_chan *auchan = to_chan(schan);
 	u32 chcr;
 	dma_addr_t dst;
+	int ret;
 
-	audmapp_get_config(auchan, slave_id, &chcr, &dst);
+	ret = audmapp_get_config(auchan, slave_id, &chcr, &dst);
+	if (ret < 0)
+		return ret;
 
 	if (try)
 		return 0;
diff --git a/drivers/dma/sun6i-dma.c b/drivers/dma/sun6i-dma.c
index 1f92a56fd2b6..3aa10b328254 100644
--- a/drivers/dma/sun6i-dma.c
+++ b/drivers/dma/sun6i-dma.c
@@ -862,7 +862,6 @@ static int sun6i_dma_probe(struct platform_device *pdev)
 {
 	struct sun6i_dma_dev *sdc;
 	struct resource *res;
-	struct clk *mux, *pll6;
 	int ret, i;
 
 	sdc = devm_kzalloc(&pdev->dev, sizeof(*sdc), GFP_KERNEL);
@@ -886,28 +885,6 @@ static int sun6i_dma_probe(struct platform_device *pdev)
 		return PTR_ERR(sdc->clk);
 	}
 
-	mux = clk_get(NULL, "ahb1_mux");
-	if (IS_ERR(mux)) {
-		dev_err(&pdev->dev, "Couldn't get AHB1 Mux\n");
-		return PTR_ERR(mux);
-	}
-
-	pll6 = clk_get(NULL, "pll6");
-	if (IS_ERR(pll6)) {
-		dev_err(&pdev->dev, "Couldn't get PLL6\n");
-		clk_put(mux);
-		return PTR_ERR(pll6);
-	}
-
-	ret = clk_set_parent(mux, pll6);
-	clk_put(pll6);
-	clk_put(mux);
-
-	if (ret) {
-		dev_err(&pdev->dev, "Couldn't reparent AHB1 on PLL6\n");
-		return ret;
-	}
-
 	sdc->rstc = devm_reset_control_get(&pdev->dev, NULL);
 	if (IS_ERR(sdc->rstc)) {
 		dev_err(&pdev->dev, "No reset controller specified\n");
diff --git a/drivers/dma/xilinx/xilinx_vdma.c b/drivers/dma/xilinx/xilinx_vdma.c
index 42a13e8d4607..a6e64767186e 100644
--- a/drivers/dma/xilinx/xilinx_vdma.c
+++ b/drivers/dma/xilinx/xilinx_vdma.c
@@ -1365,7 +1365,6 @@ static const struct of_device_id xilinx_vdma_of_ids[] = {
 static struct platform_driver xilinx_vdma_driver = {
 	.driver = {
 		.name = "xilinx-vdma",
-		.owner = THIS_MODULE,
 		.of_match_table = xilinx_vdma_of_ids,
 	},
 	.probe = xilinx_vdma_probe,
diff --git a/drivers/edac/Kconfig b/drivers/edac/Kconfig
index fd89ca982748..7072c2892d63 100644
--- a/drivers/edac/Kconfig
+++ b/drivers/edac/Kconfig
@@ -376,4 +376,13 @@ config EDAC_OCTEON_PCI
 	  Support for error detection and correction on the
 	  Cavium Octeon family of SOCs.
 
+config EDAC_ALTERA_MC
+	tristate "Altera SDRAM Memory Controller EDAC"
+	depends on EDAC_MM_EDAC && ARCH_SOCFPGA
+	help
+	  Support for error detection and correction on the
+	  Altera SDRAM memory controller. Note that the
+	  preloader must initialize the SDRAM before loading
+	  the kernel.
+
 endif # EDAC
diff --git a/drivers/edac/Makefile b/drivers/edac/Makefile
index c479a24d8f77..359aa499b200 100644
--- a/drivers/edac/Makefile
+++ b/drivers/edac/Makefile
@@ -65,3 +65,5 @@ obj-$(CONFIG_EDAC_OCTEON_PC)		+= octeon_edac-pc.o
 obj-$(CONFIG_EDAC_OCTEON_L2C)		+= octeon_edac-l2c.o
 obj-$(CONFIG_EDAC_OCTEON_LMC)		+= octeon_edac-lmc.o
 obj-$(CONFIG_EDAC_OCTEON_PCI)		+= octeon_edac-pci.o
+
+obj-$(CONFIG_EDAC_ALTERA_MC)		+= altera_edac.o
diff --git a/drivers/edac/altera_edac.c b/drivers/edac/altera_edac.c
new file mode 100644
index 000000000000..3c4929fda9d5
--- /dev/null
+++ b/drivers/edac/altera_edac.c
@@ -0,0 +1,410 @@
+/*
+ *  Copyright Altera Corporation (C) 2014. All rights reserved.
+ *  Copyright 2011-2012 Calxeda, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Adapted from the highbank_mc_edac driver.
+ */
+
+#include <linux/ctype.h>
+#include <linux/edac.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/types.h>
+#include <linux/uaccess.h>
+
+#include "edac_core.h"
+#include "edac_module.h"
+
+#define EDAC_MOD_STR		"altera_edac"
+#define EDAC_VERSION		"1"
+
+/* SDRAM Controller CtrlCfg Register */
+#define CTLCFG_OFST             0x00
+
+/* SDRAM Controller CtrlCfg Register Bit Masks */
+#define CTLCFG_ECC_EN           0x400
+#define CTLCFG_ECC_CORR_EN      0x800
+#define CTLCFG_GEN_SB_ERR       0x2000
+#define CTLCFG_GEN_DB_ERR       0x4000
+
+#define CTLCFG_ECC_AUTO_EN	(CTLCFG_ECC_EN | \
+				 CTLCFG_ECC_CORR_EN)
+
+/* SDRAM Controller Address Width Register */
+#define DRAMADDRW_OFST          0x2C
+
+/* SDRAM Controller Address Widths Field Register */
+#define DRAMADDRW_COLBIT_MASK   0x001F
+#define DRAMADDRW_COLBIT_SHIFT  0
+#define DRAMADDRW_ROWBIT_MASK   0x03E0
+#define DRAMADDRW_ROWBIT_SHIFT  5
+#define DRAMADDRW_BANKBIT_MASK	0x1C00
+#define DRAMADDRW_BANKBIT_SHIFT 10
+#define DRAMADDRW_CSBIT_MASK	0xE000
+#define DRAMADDRW_CSBIT_SHIFT   13
+
+/* SDRAM Controller Interface Data Width Register */
+#define DRAMIFWIDTH_OFST        0x30
+
+/* SDRAM Controller Interface Data Width Defines */
+#define DRAMIFWIDTH_16B_ECC     24
+#define DRAMIFWIDTH_32B_ECC     40
+
+/* SDRAM Controller DRAM Status Register */
+#define DRAMSTS_OFST            0x38
+
+/* SDRAM Controller DRAM Status Register Bit Masks */
+#define DRAMSTS_SBEERR          0x04
+#define DRAMSTS_DBEERR          0x08
+#define DRAMSTS_CORR_DROP       0x10
+
+/* SDRAM Controller DRAM IRQ Register */
+#define DRAMINTR_OFST           0x3C
+
+/* SDRAM Controller DRAM IRQ Register Bit Masks */
+#define DRAMINTR_INTREN         0x01
+#define DRAMINTR_SBEMASK        0x02
+#define DRAMINTR_DBEMASK        0x04
+#define DRAMINTR_CORRDROPMASK   0x08
+#define DRAMINTR_INTRCLR        0x10
+
+/* SDRAM Controller Single Bit Error Count Register */
+#define SBECOUNT_OFST           0x40
+
+/* SDRAM Controller Single Bit Error Count Register Bit Masks */
+#define SBECOUNT_MASK           0x0F
+
+/* SDRAM Controller Double Bit Error Count Register */
+#define DBECOUNT_OFST           0x44
+
+/* SDRAM Controller Double Bit Error Count Register Bit Masks */
+#define DBECOUNT_MASK           0x0F
+
+/* SDRAM Controller ECC Error Address Register */
+#define ERRADDR_OFST            0x48
+
+/* SDRAM Controller ECC Error Address Register Bit Masks */
+#define ERRADDR_MASK            0xFFFFFFFF
+
+/* Altera SDRAM Memory Controller data */
+struct altr_sdram_mc_data {
+	struct regmap *mc_vbase;
+};
+
+static irqreturn_t altr_sdram_mc_err_handler(int irq, void *dev_id)
+{
+	struct mem_ctl_info *mci = dev_id;
+	struct altr_sdram_mc_data *drvdata = mci->pvt_info;
+	u32 status, err_count, err_addr;
+
+	/* Error Address is shared by both SBE & DBE */
+	regmap_read(drvdata->mc_vbase, ERRADDR_OFST, &err_addr);
+
+	regmap_read(drvdata->mc_vbase, DRAMSTS_OFST, &status);
+
+	if (status & DRAMSTS_DBEERR) {
+		regmap_read(drvdata->mc_vbase, DBECOUNT_OFST, &err_count);
+		panic("\nEDAC: [%d Uncorrectable errors @ 0x%08X]\n",
+		      err_count, err_addr);
+	}
+	if (status & DRAMSTS_SBEERR) {
+		regmap_read(drvdata->mc_vbase, SBECOUNT_OFST, &err_count);
+		edac_mc_handle_error(HW_EVENT_ERR_CORRECTED, mci, err_count,
+				     err_addr >> PAGE_SHIFT,
+				     err_addr & ~PAGE_MASK, 0,
+				     0, 0, -1, mci->ctl_name, "");
+	}
+
+	regmap_write(drvdata->mc_vbase,	DRAMINTR_OFST,
+		     (DRAMINTR_INTRCLR | DRAMINTR_INTREN));
+
+	return IRQ_HANDLED;
+}
+
+#ifdef CONFIG_EDAC_DEBUG
+static ssize_t altr_sdr_mc_err_inject_write(struct file *file,
+					    const char __user *data,
+					    size_t count, loff_t *ppos)
+{
+	struct mem_ctl_info *mci = file->private_data;
+	struct altr_sdram_mc_data *drvdata = mci->pvt_info;
+	u32 *ptemp;
+	dma_addr_t dma_handle;
+	u32 reg, read_reg;
+
+	ptemp = dma_alloc_coherent(mci->pdev, 16, &dma_handle, GFP_KERNEL);
+	if (!ptemp) {
+		dma_free_coherent(mci->pdev, 16, ptemp, dma_handle);
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Inject: Buffer Allocation error\n");
+		return -ENOMEM;
+	}
+
+	regmap_read(drvdata->mc_vbase, CTLCFG_OFST, &read_reg);
+	read_reg &= ~(CTLCFG_GEN_SB_ERR | CTLCFG_GEN_DB_ERR);
+
+	/* Error are injected by writing a word while the SBE or DBE
+	 * bit in the CTLCFG register is set. Reading the word will
+	 * trigger the SBE or DBE error and the corresponding IRQ.
+	 */
+	if (count == 3) {
+		edac_printk(KERN_ALERT, EDAC_MC,
+			    "Inject Double bit error\n");
+		regmap_write(drvdata->mc_vbase, CTLCFG_OFST,
+			     (read_reg | CTLCFG_GEN_DB_ERR));
+	} else {
+		edac_printk(KERN_ALERT, EDAC_MC,
+			    "Inject Single bit error\n");
+		regmap_write(drvdata->mc_vbase,	CTLCFG_OFST,
+			     (read_reg | CTLCFG_GEN_SB_ERR));
+	}
+
+	ptemp[0] = 0x5A5A5A5A;
+	ptemp[1] = 0xA5A5A5A5;
+
+	/* Clear the error injection bits */
+	regmap_write(drvdata->mc_vbase,	CTLCFG_OFST, read_reg);
+	/* Ensure it has been written out */
+	wmb();
+
+	/*
+	 * To trigger the error, we need to read the data back
+	 * (the data was written with errors above).
+	 * The ACCESS_ONCE macros and printk are used to prevent the
+	 * the compiler optimizing these reads out.
+	 */
+	reg = ACCESS_ONCE(ptemp[0]);
+	read_reg = ACCESS_ONCE(ptemp[1]);
+	/* Force Read */
+	rmb();
+
+	edac_printk(KERN_ALERT, EDAC_MC, "Read Data [0x%X, 0x%X]\n",
+		    reg, read_reg);
+
+	dma_free_coherent(mci->pdev, 16, ptemp, dma_handle);
+
+	return count;
+}
+
+static const struct file_operations altr_sdr_mc_debug_inject_fops = {
+	.open = simple_open,
+	.write = altr_sdr_mc_err_inject_write,
+	.llseek = generic_file_llseek,
+};
+
+static void altr_sdr_mc_create_debugfs_nodes(struct mem_ctl_info *mci)
+{
+	if (mci->debugfs)
+		debugfs_create_file("inject_ctrl", S_IWUSR, mci->debugfs, mci,
+				    &altr_sdr_mc_debug_inject_fops);
+}
+#else
+static void altr_sdr_mc_create_debugfs_nodes(struct mem_ctl_info *mci)
+{}
+#endif
+
+/* Get total memory size in bytes */
+static u32 altr_sdram_get_total_mem_size(struct regmap *mc_vbase)
+{
+	u32 size, read_reg, row, bank, col, cs, width;
+
+	if (regmap_read(mc_vbase, DRAMADDRW_OFST, &read_reg) < 0)
+		return 0;
+
+	if (regmap_read(mc_vbase, DRAMIFWIDTH_OFST, &width) < 0)
+		return 0;
+
+	col = (read_reg & DRAMADDRW_COLBIT_MASK) >>
+		DRAMADDRW_COLBIT_SHIFT;
+	row = (read_reg & DRAMADDRW_ROWBIT_MASK) >>
+		DRAMADDRW_ROWBIT_SHIFT;
+	bank = (read_reg & DRAMADDRW_BANKBIT_MASK) >>
+		DRAMADDRW_BANKBIT_SHIFT;
+	cs = (read_reg & DRAMADDRW_CSBIT_MASK) >>
+		DRAMADDRW_CSBIT_SHIFT;
+
+	/* Correct for ECC as its not addressible */
+	if (width == DRAMIFWIDTH_32B_ECC)
+		width = 32;
+	if (width == DRAMIFWIDTH_16B_ECC)
+		width = 16;
+
+	/* calculate the SDRAM size base on this info */
+	size = 1 << (row + bank + col);
+	size = size * cs * (width / 8);
+	return size;
+}
+
+static int altr_sdram_probe(struct platform_device *pdev)
+{
+	struct edac_mc_layer layers[2];
+	struct mem_ctl_info *mci;
+	struct altr_sdram_mc_data *drvdata;
+	struct regmap *mc_vbase;
+	struct dimm_info *dimm;
+	u32 read_reg, mem_size;
+	int irq;
+	int res = 0;
+
+	/* Validate the SDRAM controller has ECC enabled */
+	/* Grab the register range from the sdr controller in device tree */
+	mc_vbase = syscon_regmap_lookup_by_phandle(pdev->dev.of_node,
+						   "altr,sdr-syscon");
+	if (IS_ERR(mc_vbase)) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "regmap for altr,sdr-syscon lookup failed.\n");
+		return -ENODEV;
+	}
+
+	if (regmap_read(mc_vbase, CTLCFG_OFST, &read_reg) ||
+	    ((read_reg & CTLCFG_ECC_AUTO_EN) !=	CTLCFG_ECC_AUTO_EN)) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "No ECC/ECC disabled [0x%08X]\n", read_reg);
+		return -ENODEV;
+	}
+
+	/* Grab memory size from device tree. */
+	mem_size = altr_sdram_get_total_mem_size(mc_vbase);
+	if (!mem_size) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Unable to calculate memory size\n");
+		return -ENODEV;
+	}
+
+	/* Ensure the SDRAM Interrupt is disabled and cleared */
+	if (regmap_write(mc_vbase, DRAMINTR_OFST, DRAMINTR_INTRCLR)) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "Error clearing SDRAM ECC IRQ\n");
+		return -ENODEV;
+	}
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		edac_printk(KERN_ERR, EDAC_MC,
+			    "No irq %d in DT\n", irq);
+		return -ENODEV;
+	}
+
+	layers[0].type = EDAC_MC_LAYER_CHIP_SELECT;
+	layers[0].size = 1;
+	layers[0].is_virt_csrow = true;
+	layers[1].type = EDAC_MC_LAYER_CHANNEL;
+	layers[1].size = 1;
+	layers[1].is_virt_csrow = false;
+	mci = edac_mc_alloc(0, ARRAY_SIZE(layers), layers,
+			    sizeof(struct altr_sdram_mc_data));
+	if (!mci)
+		return -ENOMEM;
+
+	mci->pdev = &pdev->dev;
+	drvdata = mci->pvt_info;
+	drvdata->mc_vbase = mc_vbase;
+	platform_set_drvdata(pdev, mci);
+
+	if (!devres_open_group(&pdev->dev, NULL, GFP_KERNEL)) {
+		res = -ENOMEM;
+		goto free;
+	}
+
+	mci->mtype_cap = MEM_FLAG_DDR3;
+	mci->edac_ctl_cap = EDAC_FLAG_NONE | EDAC_FLAG_SECDED;
+	mci->edac_cap = EDAC_FLAG_SECDED;
+	mci->mod_name = EDAC_MOD_STR;
+	mci->mod_ver = EDAC_VERSION;
+	mci->ctl_name = dev_name(&pdev->dev);
+	mci->scrub_mode = SCRUB_SW_SRC;
+	mci->dev_name = dev_name(&pdev->dev);
+
+	dimm = *mci->dimms;
+	dimm->nr_pages = ((mem_size - 1) >> PAGE_SHIFT) + 1;
+	dimm->grain = 8;
+	dimm->dtype = DEV_X8;
+	dimm->mtype = MEM_DDR3;
+	dimm->edac_mode = EDAC_SECDED;
+
+	res = edac_mc_add_mc(mci);
+	if (res < 0)
+		goto err;
+
+	res = devm_request_irq(&pdev->dev, irq, altr_sdram_mc_err_handler,
+			       0, dev_name(&pdev->dev), mci);
+	if (res < 0) {
+		edac_mc_printk(mci, KERN_ERR,
+			       "Unable to request irq %d\n", irq);
+		res = -ENODEV;
+		goto err2;
+	}
+
+	if (regmap_write(drvdata->mc_vbase, DRAMINTR_OFST,
+			 (DRAMINTR_INTRCLR | DRAMINTR_INTREN))) {
+		edac_mc_printk(mci, KERN_ERR,
+			       "Error enabling SDRAM ECC IRQ\n");
+		res = -ENODEV;
+		goto err2;
+	}
+
+	altr_sdr_mc_create_debugfs_nodes(mci);
+
+	devres_close_group(&pdev->dev, NULL);
+
+	return 0;
+
+err2:
+	edac_mc_del_mc(&pdev->dev);
+err:
+	devres_release_group(&pdev->dev, NULL);
+free:
+	edac_mc_free(mci);
+	edac_printk(KERN_ERR, EDAC_MC,
+		    "EDAC Probe Failed; Error %d\n", res);
+
+	return res;
+}
+
+static int altr_sdram_remove(struct platform_device *pdev)
+{
+	struct mem_ctl_info *mci = platform_get_drvdata(pdev);
+
+	edac_mc_del_mc(&pdev->dev);
+	edac_mc_free(mci);
+	platform_set_drvdata(pdev, NULL);
+
+	return 0;
+}
+
+static const struct of_device_id altr_sdram_ctrl_of_match[] = {
+	{ .compatible = "altr,sdram-edac", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, altr_sdram_ctrl_of_match);
+
+static struct platform_driver altr_sdram_edac_driver = {
+	.probe = altr_sdram_probe,
+	.remove = altr_sdram_remove,
+	.driver = {
+		.name = "altr_sdram_edac",
+		.of_match_table = altr_sdram_ctrl_of_match,
+	},
+};
+
+module_platform_driver(altr_sdram_edac_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Thor Thayer");
+MODULE_DESCRIPTION("EDAC Driver for Altera SDRAM Controller");
diff --git a/drivers/edac/amd64_edac.c b/drivers/edac/amd64_edac.c
index f8bf00010d45..bbd65149cdb2 100644
--- a/drivers/edac/amd64_edac.c
+++ b/drivers/edac/amd64_edac.c
@@ -87,61 +87,73 @@ int __amd64_write_pci_cfg_dword(struct pci_dev *pdev, int offset,
 }
 
 /*
+ * Select DCT to which PCI cfg accesses are routed
+ */
+static void f15h_select_dct(struct amd64_pvt *pvt, u8 dct)
+{
+	u32 reg = 0;
+
+	amd64_read_pci_cfg(pvt->F1, DCT_CFG_SEL, &reg);
+	reg &= (pvt->model == 0x30) ? ~3 : ~1;
+	reg |= dct;
+	amd64_write_pci_cfg(pvt->F1, DCT_CFG_SEL, reg);
+}
+
+/*
  *
  * Depending on the family, F2 DCT reads need special handling:
  *
- * K8: has a single DCT only
+ * K8: has a single DCT only and no address offsets >= 0x100
  *
  * F10h: each DCT has its own set of regs
  *	DCT0 -> F2x040..
  *	DCT1 -> F2x140..
  *
- * F15h: we select which DCT we access using F1x10C[DctCfgSel]
- *
  * F16h: has only 1 DCT
+ *
+ * F15h: we select which DCT we access using F1x10C[DctCfgSel]
  */
-static int k8_read_dct_pci_cfg(struct amd64_pvt *pvt, int addr, u32 *val,
-			       const char *func)
+static inline int amd64_read_dct_pci_cfg(struct amd64_pvt *pvt, u8 dct,
+					 int offset, u32 *val)
 {
-	if (addr >= 0x100)
-		return -EINVAL;
-
-	return __amd64_read_pci_cfg_dword(pvt->F2, addr, val, func);
-}
+	switch (pvt->fam) {
+	case 0xf:
+		if (dct || offset >= 0x100)
+			return -EINVAL;
+		break;
 
-static int f10_read_dct_pci_cfg(struct amd64_pvt *pvt, int addr, u32 *val,
-				 const char *func)
-{
-	return __amd64_read_pci_cfg_dword(pvt->F2, addr, val, func);
-}
+	case 0x10:
+		if (dct) {
+			/*
+			 * Note: If ganging is enabled, barring the regs
+			 * F2x[1,0]98 and F2x[1,0]9C; reads reads to F2x1xx
+			 * return 0. (cf. Section 2.8.1 F10h BKDG)
+			 */
+			if (dct_ganging_enabled(pvt))
+				return 0;
 
-/*
- * Select DCT to which PCI cfg accesses are routed
- */
-static void f15h_select_dct(struct amd64_pvt *pvt, u8 dct)
-{
-	u32 reg = 0;
+			offset += 0x100;
+		}
+		break;
 
-	amd64_read_pci_cfg(pvt->F1, DCT_CFG_SEL, &reg);
-	reg &= (pvt->model >= 0x30) ? ~3 : ~1;
-	reg |= dct;
-	amd64_write_pci_cfg(pvt->F1, DCT_CFG_SEL, reg);
-}
+	case 0x15:
+		/*
+		 * F15h: F2x1xx addresses do not map explicitly to DCT1.
+		 * We should select which DCT we access using F1x10C[DctCfgSel]
+		 */
+		dct = (dct && pvt->model == 0x30) ? 3 : dct;
+		f15h_select_dct(pvt, dct);
+		break;
 
-static int f15_read_dct_pci_cfg(struct amd64_pvt *pvt, int addr, u32 *val,
-				 const char *func)
-{
-	u8 dct  = 0;
+	case 0x16:
+		if (dct)
+			return -EINVAL;
+		break;
 
-	/* For F15 M30h, the second dct is DCT 3, refer to BKDG Section 2.10 */
-	if (addr >= 0x140 && addr <= 0x1a0) {
-		dct   = (pvt->model >= 0x30) ? 3 : 1;
-		addr -= 0x100;
+	default:
+		break;
 	}
-
-	f15h_select_dct(pvt, dct);
-
-	return __amd64_read_pci_cfg_dword(pvt->F2, addr, val, func);
+	return amd64_read_pci_cfg(pvt->F2, offset, val);
 }
 
 /*
@@ -768,16 +780,17 @@ static void read_dct_base_mask(struct amd64_pvt *pvt)
 		u32 *base0 = &pvt->csels[0].csbases[cs];
 		u32 *base1 = &pvt->csels[1].csbases[cs];
 
-		if (!amd64_read_dct_pci_cfg(pvt, reg0, base0))
+		if (!amd64_read_dct_pci_cfg(pvt, 0, reg0, base0))
 			edac_dbg(0, "  DCSB0[%d]=0x%08x reg: F2x%x\n",
 				 cs, *base0, reg0);
 
-		if (pvt->fam == 0xf || dct_ganging_enabled(pvt))
+		if (pvt->fam == 0xf)
 			continue;
 
-		if (!amd64_read_dct_pci_cfg(pvt, reg1, base1))
+		if (!amd64_read_dct_pci_cfg(pvt, 1, reg0, base1))
 			edac_dbg(0, "  DCSB1[%d]=0x%08x reg: F2x%x\n",
-				 cs, *base1, reg1);
+				 cs, *base1, (pvt->fam == 0x10) ? reg1
+								: reg0);
 	}
 
 	for_each_chip_select_mask(cs, 0, pvt) {
@@ -786,16 +799,17 @@ static void read_dct_base_mask(struct amd64_pvt *pvt)
 		u32 *mask0 = &pvt->csels[0].csmasks[cs];
 		u32 *mask1 = &pvt->csels[1].csmasks[cs];
 
-		if (!amd64_read_dct_pci_cfg(pvt, reg0, mask0))
+		if (!amd64_read_dct_pci_cfg(pvt, 0, reg0, mask0))
 			edac_dbg(0, "    DCSM0[%d]=0x%08x reg: F2x%x\n",
 				 cs, *mask0, reg0);
 
-		if (pvt->fam == 0xf || dct_ganging_enabled(pvt))
+		if (pvt->fam == 0xf)
 			continue;
 
-		if (!amd64_read_dct_pci_cfg(pvt, reg1, mask1))
+		if (!amd64_read_dct_pci_cfg(pvt, 1, reg0, mask1))
 			edac_dbg(0, "    DCSM1[%d]=0x%08x reg: F2x%x\n",
-				 cs, *mask1, reg1);
+				 cs, *mask1, (pvt->fam == 0x10) ? reg1
+								: reg0);
 	}
 }
 
@@ -1198,7 +1212,7 @@ static void read_dram_ctl_register(struct amd64_pvt *pvt)
 	if (pvt->fam == 0xf)
 		return;
 
-	if (!amd64_read_dct_pci_cfg(pvt, DCT_SEL_LO, &pvt->dct_sel_lo)) {
+	if (!amd64_read_pci_cfg(pvt->F2, DCT_SEL_LO, &pvt->dct_sel_lo)) {
 		edac_dbg(0, "F2x110 (DCTSelLow): 0x%08x, High range addrs at: 0x%x\n",
 			 pvt->dct_sel_lo, dct_sel_baseaddr(pvt));
 
@@ -1219,7 +1233,7 @@ static void read_dram_ctl_register(struct amd64_pvt *pvt)
 			 dct_sel_interleave_addr(pvt));
 	}
 
-	amd64_read_dct_pci_cfg(pvt, DCT_SEL_HI, &pvt->dct_sel_hi);
+	amd64_read_pci_cfg(pvt->F2, DCT_SEL_HI, &pvt->dct_sel_hi);
 }
 
 /*
@@ -1430,7 +1444,7 @@ static u64 f1x_swap_interleaved_region(struct amd64_pvt *pvt, u64 sys_addr)
 			return sys_addr;
 	}
 
-	amd64_read_dct_pci_cfg(pvt, SWAP_INTLV_REG, &swap_reg);
+	amd64_read_pci_cfg(pvt->F2, SWAP_INTLV_REG, &swap_reg);
 
 	if (!(swap_reg & 0x1))
 		return sys_addr;
@@ -1723,10 +1737,16 @@ static void debug_display_dimm_sizes(struct amd64_pvt *pvt, u8 ctrl)
 		       WARN_ON(ctrl != 0);
 	}
 
-	dbam = (ctrl && !dct_ganging_enabled(pvt)) ? pvt->dbam1 : pvt->dbam0;
-	dcsb = (ctrl && !dct_ganging_enabled(pvt)) ? pvt->csels[1].csbases
-						   : pvt->csels[0].csbases;
-
+	if (pvt->fam == 0x10) {
+		dbam = (ctrl && !dct_ganging_enabled(pvt)) ? pvt->dbam1
+							   : pvt->dbam0;
+		dcsb = (ctrl && !dct_ganging_enabled(pvt)) ?
+				 pvt->csels[1].csbases :
+				 pvt->csels[0].csbases;
+	} else if (ctrl) {
+		dbam = pvt->dbam0;
+		dcsb = pvt->csels[1].csbases;
+	}
 	edac_dbg(1, "F2x%d80 (DRAM Bank Address Mapping): 0x%08x\n",
 		 ctrl, dbam);
 
@@ -1760,7 +1780,6 @@ static struct amd64_family_type family_types[] = {
 			.early_channel_count	= k8_early_channel_count,
 			.map_sysaddr_to_csrow	= k8_map_sysaddr_to_csrow,
 			.dbam_to_cs		= k8_dbam_to_chip_select,
-			.read_dct_pci_cfg	= k8_read_dct_pci_cfg,
 		}
 	},
 	[F10_CPUS] = {
@@ -1771,7 +1790,6 @@ static struct amd64_family_type family_types[] = {
 			.early_channel_count	= f1x_early_channel_count,
 			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
 			.dbam_to_cs		= f10_dbam_to_chip_select,
-			.read_dct_pci_cfg	= f10_read_dct_pci_cfg,
 		}
 	},
 	[F15_CPUS] = {
@@ -1782,7 +1800,6 @@ static struct amd64_family_type family_types[] = {
 			.early_channel_count	= f1x_early_channel_count,
 			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
 			.dbam_to_cs		= f15_dbam_to_chip_select,
-			.read_dct_pci_cfg	= f15_read_dct_pci_cfg,
 		}
 	},
 	[F15_M30H_CPUS] = {
@@ -1793,7 +1810,6 @@ static struct amd64_family_type family_types[] = {
 			.early_channel_count	= f1x_early_channel_count,
 			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
 			.dbam_to_cs		= f16_dbam_to_chip_select,
-			.read_dct_pci_cfg	= f15_read_dct_pci_cfg,
 		}
 	},
 	[F16_CPUS] = {
@@ -1804,7 +1820,6 @@ static struct amd64_family_type family_types[] = {
 			.early_channel_count	= f1x_early_channel_count,
 			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
 			.dbam_to_cs		= f16_dbam_to_chip_select,
-			.read_dct_pci_cfg	= f10_read_dct_pci_cfg,
 		}
 	},
 	[F16_M30H_CPUS] = {
@@ -1815,7 +1830,6 @@ static struct amd64_family_type family_types[] = {
 			.early_channel_count	= f1x_early_channel_count,
 			.map_sysaddr_to_csrow	= f1x_map_sysaddr_to_csrow,
 			.dbam_to_cs		= f16_dbam_to_chip_select,
-			.read_dct_pci_cfg	= f10_read_dct_pci_cfg,
 		}
 	},
 };
@@ -2148,25 +2162,25 @@ static void read_mc_regs(struct amd64_pvt *pvt)
 	read_dct_base_mask(pvt);
 
 	amd64_read_pci_cfg(pvt->F1, DHAR, &pvt->dhar);
-	amd64_read_dct_pci_cfg(pvt, DBAM0, &pvt->dbam0);
+	amd64_read_dct_pci_cfg(pvt, 0, DBAM0, &pvt->dbam0);
 
 	amd64_read_pci_cfg(pvt->F3, F10_ONLINE_SPARE, &pvt->online_spare);
 
-	amd64_read_dct_pci_cfg(pvt, DCLR0, &pvt->dclr0);
-	amd64_read_dct_pci_cfg(pvt, DCHR0, &pvt->dchr0);
+	amd64_read_dct_pci_cfg(pvt, 0, DCLR0, &pvt->dclr0);
+	amd64_read_dct_pci_cfg(pvt, 0, DCHR0, &pvt->dchr0);
 
 	if (!dct_ganging_enabled(pvt)) {
-		amd64_read_dct_pci_cfg(pvt, DCLR1, &pvt->dclr1);
-		amd64_read_dct_pci_cfg(pvt, DCHR1, &pvt->dchr1);
+		amd64_read_dct_pci_cfg(pvt, 1, DCLR0, &pvt->dclr1);
+		amd64_read_dct_pci_cfg(pvt, 1, DCHR0, &pvt->dchr1);
 	}
 
 	pvt->ecc_sym_sz = 4;
 
 	if (pvt->fam >= 0x10) {
 		amd64_read_pci_cfg(pvt->F3, EXT_NB_MCA_CFG, &tmp);
+		/* F16h has only DCT0, so no need to read dbam1 */
 		if (pvt->fam != 0x16)
-			/* F16h has only DCT0 */
-			amd64_read_dct_pci_cfg(pvt, DBAM1, &pvt->dbam1);
+			amd64_read_dct_pci_cfg(pvt, 1, DBAM0, &pvt->dbam1);
 
 		/* F10h, revD and later can do x8 ECC too */
 		if ((pvt->fam > 0x10 || pvt->model > 7) && tmp & BIT(25))
diff --git a/drivers/edac/amd64_edac.h b/drivers/edac/amd64_edac.h
index d903e0c21144..55fb5941c6d4 100644
--- a/drivers/edac/amd64_edac.h
+++ b/drivers/edac/amd64_edac.h
@@ -481,8 +481,6 @@ struct low_ops {
 	void (*map_sysaddr_to_csrow)	(struct mem_ctl_info *mci, u64 sys_addr,
 					 struct err_info *);
 	int (*dbam_to_cs)		(struct amd64_pvt *pvt, u8 dct, unsigned cs_mode);
-	int (*read_dct_pci_cfg)		(struct amd64_pvt *pvt, int offset,
-					 u32 *val, const char *func);
 };
 
 struct amd64_family_type {
@@ -502,9 +500,6 @@ int __amd64_write_pci_cfg_dword(struct pci_dev *pdev, int offset,
 #define amd64_write_pci_cfg(pdev, offset, val)	\
 	__amd64_write_pci_cfg_dword(pdev, offset, val, __func__)
 
-#define amd64_read_dct_pci_cfg(pvt, offset, val) \
-	pvt->ops->read_dct_pci_cfg(pvt, offset, val, __func__)
-
 int amd64_get_dram_hole_info(struct mem_ctl_info *mci, u64 *hole_base,
 			     u64 *hole_offset, u64 *hole_size);
 
diff --git a/drivers/edac/edac_core.h b/drivers/edac/edac_core.h
index 3c2625e7980d..6c9f381e8fe6 100644
--- a/drivers/edac/edac_core.h
+++ b/drivers/edac/edac_core.h
@@ -66,7 +66,7 @@
 #define EDAC_PCI "PCI"
 #define EDAC_DEBUG "DEBUG"
 
-extern const char *edac_mem_types[];
+extern const char * const edac_mem_types[];
 
 #ifdef CONFIG_EDAC_DEBUG
 extern int edac_debug_level;
diff --git a/drivers/edac/edac_mc.c b/drivers/edac/edac_mc.c
index 9f134823fa75..c3893b0ddb18 100644
--- a/drivers/edac/edac_mc.c
+++ b/drivers/edac/edac_mc.c
@@ -128,7 +128,7 @@ static void edac_mc_dump_mci(struct mem_ctl_info *mci)
 /*
  * keep those in sync with the enum mem_type
  */
-const char *edac_mem_types[] = {
+const char * const edac_mem_types[] = {
 	"Empty csrow",
 	"Reserved csrow type",
 	"Unknown csrow type",
diff --git a/drivers/edac/mpc85xx_edac.c b/drivers/edac/mpc85xx_edac.c
index f4aec2e6ef56..7d3742edbaa2 100644
--- a/drivers/edac/mpc85xx_edac.c
+++ b/drivers/edac/mpc85xx_edac.c
@@ -633,7 +633,7 @@ static int mpc85xx_l2_err_probe(struct platform_device *op)
 	if (edac_op_state == EDAC_OPSTATE_INT) {
 		pdata->irq = irq_of_parse_and_map(op->dev.of_node, 0);
 		res = devm_request_irq(&op->dev, pdata->irq,
-				       mpc85xx_l2_isr, 0,
+				       mpc85xx_l2_isr, IRQF_SHARED,
 				       "[EDAC] L2 err", edac_dev);
 		if (res < 0) {
 			printk(KERN_ERR
diff --git a/drivers/edac/ppc4xx_edac.c b/drivers/edac/ppc4xx_edac.c
index ef6b7e08f485..0f04d5ead521 100644
--- a/drivers/edac/ppc4xx_edac.c
+++ b/drivers/edac/ppc4xx_edac.c
@@ -974,7 +974,7 @@ static int ppc4xx_edac_init_csrows(struct mem_ctl_info *mci, u32 mcopt1)
 		 * page size (PAGE_SIZE) or the memory width (2 or 4).
 		 */
 		for (j = 0; j < csi->nr_channels; j++) {
-			struct dimm_info *dimm = csi->channels[j].dimm;
+			struct dimm_info *dimm = csi->channels[j]->dimm;
 
 			dimm->nr_pages  = nr_pages / csi->nr_channels;
 			dimm->grain	= 1;
diff --git a/drivers/edac/sb_edac.c b/drivers/edac/sb_edac.c
index 0034c4844428..e9bb1af67c8d 100644
--- a/drivers/edac/sb_edac.c
+++ b/drivers/edac/sb_edac.c
@@ -52,36 +52,6 @@ static int probed;
 #define GET_BITFIELD(v, lo, hi)	\
 	(((v) & GENMASK_ULL(hi, lo)) >> (lo))
 
-/*
- * sbridge Memory Controller Registers
- */
-
-/*
- * FIXME: For now, let's order by device function, as it makes
- * easier for driver's development process. This table should be
- * moved to pci_id.h when submitted upstream
- */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_SAD0	0x3cf4	/* 12.6 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_SAD1	0x3cf6	/* 12.7 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_BR		0x3cf5	/* 13.6 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0	0x3ca0	/* 14.0 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA	0x3ca8	/* 15.0 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_RAS	0x3c71	/* 15.1 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD0	0x3caa	/* 15.2 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD1	0x3cab	/* 15.3 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD2	0x3cac	/* 15.4 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TAD3	0x3cad	/* 15.5 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_DDRIO	0x3cb8	/* 17.0 */
-
-	/*
-	 * Currently, unused, but will be needed in the future
-	 * implementations, as they hold the error counters
-	 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_ERR0	0x3c72	/* 16.2 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_ERR1	0x3c73	/* 16.3 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_ERR2	0x3c76	/* 16.6 */
-#define PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_ERR3	0x3c77	/* 16.7 */
-
 /* Devices 12 Function 6, Offsets 0x80 to 0xcc */
 static const u32 sbridge_dram_rule[] = {
 	0x80, 0x88, 0x90, 0x98, 0xa0,
@@ -283,8 +253,9 @@ static const u32 correrrthrsld[] = {
  * sbridge structs
  */
 
-#define NUM_CHANNELS	4
-#define MAX_DIMMS	3		/* Max DIMMS per channel */
+#define NUM_CHANNELS		4
+#define MAX_DIMMS		3	/* Max DIMMS per channel */
+#define CHANNEL_UNSPECIFIED	0xf	/* Intel IA32 SDM 15-14 */
 
 enum type {
 	SANDY_BRIDGE,
@@ -529,7 +500,7 @@ static const struct pci_id_table pci_dev_descr_haswell_table[] = {
  *	pci_device_id	table for which devices we are looking for
  */
 static const struct pci_device_id sbridge_pci_tbl[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_TA)},
+	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SBRIDGE_IMC_HA0)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_IBRIDGE_IMC_HA0_TA)},
 	{PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_HASWELL_IMC_HA0)},
 	{0,}			/* 0 terminated list. */
@@ -1991,6 +1962,9 @@ static void sbridge_mce_output_error(struct mem_ctl_info *mci,
 
 	/* FIXME: need support for channel mask */
 
+	if (channel == CHANNEL_UNSPECIFIED)
+		channel = -1;
+
 	/* Call the helper to output message */
 	edac_mc_handle_error(tp_event, mci, core_err_cnt,
 			     m->addr >> PAGE_SHIFT, m->addr & ~PAGE_MASK, 0,
diff --git a/drivers/extcon/Kconfig b/drivers/extcon/Kconfig
index 6f2f4727de2c..6a1f7de6fa54 100644
--- a/drivers/extcon/Kconfig
+++ b/drivers/extcon/Kconfig
@@ -70,8 +70,21 @@ config EXTCON_PALMAS
 	  Say Y here to enable support for USB peripheral and USB host
 	  detection by palmas usb.
 
+config EXTCON_RT8973A
+	tristate "RT8973A EXTCON support"
+	depends on I2C
+	select IRQ_DOMAIN
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	help
+	  If you say yes here you get support for the MUIC device of
+	  Richtek RT8973A. The RT8973A is a USB port accessory detector
+	  and switch that is optimized to protect low voltage system
+	  from abnormal high input voltage (up to 28V).
+
 config EXTCON_SM5502
 	tristate "SM5502 EXTCON support"
+	depends on I2C
 	select IRQ_DOMAIN
 	select REGMAP_I2C
 	select REGMAP_IRQ
diff --git a/drivers/extcon/Makefile b/drivers/extcon/Makefile
index b38546eb522a..0370b42e5a27 100644
--- a/drivers/extcon/Makefile
+++ b/drivers/extcon/Makefile
@@ -10,4 +10,5 @@ obj-$(CONFIG_EXTCON_MAX14577)	+= extcon-max14577.o
 obj-$(CONFIG_EXTCON_MAX77693)	+= extcon-max77693.o
 obj-$(CONFIG_EXTCON_MAX8997)	+= extcon-max8997.o
 obj-$(CONFIG_EXTCON_PALMAS)	+= extcon-palmas.o
+obj-$(CONFIG_EXTCON_RT8973A)	+= extcon-rt8973a.o
 obj-$(CONFIG_EXTCON_SM5502)	+= extcon-sm5502.o
diff --git a/drivers/extcon/extcon-gpio.c b/drivers/extcon/extcon-gpio.c
index 5b7ec274cb63..72f19a37fd01 100644
--- a/drivers/extcon/extcon-gpio.c
+++ b/drivers/extcon/extcon-gpio.c
@@ -20,16 +20,16 @@
  *
 */
 
-#include <linux/module.h>
-#include <linux/kernel.h>
+#include <linux/extcon.h>
+#include <linux/extcon/extcon-gpio.h>
+#include <linux/gpio.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/workqueue.h>
-#include <linux/gpio.h>
-#include <linux/extcon.h>
-#include <linux/extcon/extcon-gpio.h>
 
 struct gpio_extcon_data {
 	struct extcon_dev *edev;
diff --git a/drivers/extcon/extcon-max77693.c b/drivers/extcon/extcon-max77693.c
index 77460f2c1ca1..490e27475bac 100644
--- a/drivers/extcon/extcon-max77693.c
+++ b/drivers/extcon/extcon-max77693.c
@@ -255,10 +255,14 @@ static int max77693_muic_set_debounce_time(struct max77693_muic_info *info,
 	case ADC_DEBOUNCE_TIME_10MS:
 	case ADC_DEBOUNCE_TIME_25MS:
 	case ADC_DEBOUNCE_TIME_38_62MS:
-		ret = regmap_update_bits(info->max77693->regmap_muic,
-					  MAX77693_MUIC_REG_CTRL3,
-					  CONTROL3_ADCDBSET_MASK,
-					  time << CONTROL3_ADCDBSET_SHIFT);
+		/*
+		 * Don't touch BTLDset, JIGset when you want to change adc
+		 * debounce time. If it writes other than 0 to BTLDset, JIGset
+		 * muic device will be reset and loose current state.
+		 */
+		ret = regmap_write(info->max77693->regmap_muic,
+				  MAX77693_MUIC_REG_CTRL3,
+				  time << CONTROL3_ADCDBSET_SHIFT);
 		if (ret) {
 			dev_err(info->dev, "failed to set ADC debounce time\n");
 			return ret;
@@ -1155,13 +1159,11 @@ static int max77693_muic_probe(struct platform_device *pdev)
 
 		virq = regmap_irq_get_virq(max77693->irq_data_muic,
 					muic_irq->irq);
-		if (!virq) {
-			ret = -EINVAL;
-			goto err_irq;
-		}
+		if (!virq)
+			return -EINVAL;
 		muic_irq->virq = virq;
 
-		ret = request_threaded_irq(virq, NULL,
+		ret = devm_request_threaded_irq(&pdev->dev, virq, NULL,
 				max77693_muic_irq_handler,
 				IRQF_NO_SUSPEND,
 				muic_irq->name, info);
@@ -1170,7 +1172,7 @@ static int max77693_muic_probe(struct platform_device *pdev)
 				"failed: irq request (IRQ: %d,"
 				" error :%d)\n",
 				muic_irq->irq, ret);
-			goto err_irq;
+			return ret;
 		}
 	}
 
@@ -1179,15 +1181,14 @@ static int max77693_muic_probe(struct platform_device *pdev)
 					      max77693_extcon_cable);
 	if (IS_ERR(info->edev)) {
 		dev_err(&pdev->dev, "failed to allocate memory for extcon\n");
-		ret = -ENOMEM;
-		goto err_irq;
+		return -ENOMEM;
 	}
 	info->edev->name = DEV_NAME;
 
 	ret = devm_extcon_dev_register(&pdev->dev, info->edev);
 	if (ret) {
 		dev_err(&pdev->dev, "failed to register extcon device\n");
-		goto err_irq;
+		return ret;
 	}
 
 	/* Initialize MUIC register by using platform data or default data */
@@ -1265,7 +1266,7 @@ static int max77693_muic_probe(struct platform_device *pdev)
 			MAX77693_MUIC_REG_ID, &id);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "failed to read revision number\n");
-		goto err_irq;
+		return ret;
 	}
 	dev_info(info->dev, "device ID : 0x%x\n", id);
 
@@ -1285,20 +1286,12 @@ static int max77693_muic_probe(struct platform_device *pdev)
 			delay_jiffies);
 
 	return ret;
-
-err_irq:
-	while (--i >= 0)
-		free_irq(muic_irqs[i].virq, info);
-	return ret;
 }
 
 static int max77693_muic_remove(struct platform_device *pdev)
 {
 	struct max77693_muic_info *info = platform_get_drvdata(pdev);
-	int i;
 
-	for (i = 0; i < ARRAY_SIZE(muic_irqs); i++)
-		free_irq(muic_irqs[i].virq, info);
 	cancel_work_sync(&info->irq_work);
 	input_unregister_device(info->dock);
 
diff --git a/drivers/extcon/extcon-rt8973a.c b/drivers/extcon/extcon-rt8973a.c
new file mode 100644
index 000000000000..a784b2d5ee72
--- /dev/null
+++ b/drivers/extcon/extcon-rt8973a.c
@@ -0,0 +1,740 @@
+/*
+ * extcon-rt8973a.c - Richtek RT8973A extcon driver to support USB switches
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd
+ * Author: Chanwoo Choi <cw00.choi@samsung.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/extcon.h>
+
+#include "extcon-rt8973a.h"
+
+#define	DELAY_MS_DEFAULT		20000	/* unit: millisecond */
+
+struct muic_irq {
+	unsigned int irq;
+	const char *name;
+	unsigned int virq;
+};
+
+struct reg_data {
+	u8 reg;
+	u8 mask;
+	u8 val;
+	bool invert;
+};
+
+struct rt8973a_muic_info {
+	struct device *dev;
+	struct extcon_dev *edev;
+
+	struct i2c_client *i2c;
+	struct regmap *regmap;
+
+	struct regmap_irq_chip_data *irq_data;
+	struct muic_irq *muic_irqs;
+	unsigned int num_muic_irqs;
+	int irq;
+	bool irq_attach;
+	bool irq_detach;
+	bool irq_ovp;
+	bool irq_otp;
+	struct work_struct irq_work;
+
+	struct reg_data *reg_data;
+	unsigned int num_reg_data;
+	bool auto_config;
+
+	struct mutex mutex;
+
+	/*
+	 * Use delayed workqueue to detect cable state and then
+	 * notify cable state to notifiee/platform through uevent.
+	 * After completing the booting of platform, the extcon provider
+	 * driver should notify cable state to upper layer.
+	 */
+	struct delayed_work wq_detcable;
+};
+
+/* Default value of RT8973A register to bring up MUIC device. */
+static struct reg_data rt8973a_reg_data[] = {
+	{
+		.reg = RT8973A_REG_CONTROL1,
+		.mask = RT8973A_REG_CONTROL1_ADC_EN_MASK
+			| RT8973A_REG_CONTROL1_USB_CHD_EN_MASK
+			| RT8973A_REG_CONTROL1_CHGTYP_MASK
+			| RT8973A_REG_CONTROL1_SWITCH_OPEN_MASK
+			| RT8973A_REG_CONTROL1_AUTO_CONFIG_MASK
+			| RT8973A_REG_CONTROL1_INTM_MASK,
+		.val = RT8973A_REG_CONTROL1_ADC_EN_MASK
+			| RT8973A_REG_CONTROL1_USB_CHD_EN_MASK
+			| RT8973A_REG_CONTROL1_CHGTYP_MASK,
+		.invert = false,
+	},
+	{ /* sentinel */ }
+};
+
+/* List of detectable cables */
+enum {
+	EXTCON_CABLE_USB = 0,
+	EXTCON_CABLE_USB_HOST,
+	EXTCON_CABLE_TA,
+	EXTCON_CABLE_JIG_OFF_USB,
+	EXTCON_CABLE_JIG_ON_USB,
+	EXTCON_CABLE_JIG_OFF_UART,
+	EXTCON_CABLE_JIG_ON_UART,
+
+	EXTCON_CABLE_END,
+};
+
+static const char *rt8973a_extcon_cable[] = {
+	[EXTCON_CABLE_USB]		= "USB",
+	[EXTCON_CABLE_USB_HOST]		= "USB-Host",
+	[EXTCON_CABLE_TA]		= "TA",
+	[EXTCON_CABLE_JIG_OFF_USB]	= "JIG-USB-OFF",
+	[EXTCON_CABLE_JIG_ON_USB]	= "JIG-USB-ON",
+	[EXTCON_CABLE_JIG_OFF_UART]	= "JIG-UART-OFF",
+	[EXTCON_CABLE_JIG_ON_UART]	= "JIG-UART-ON",
+	NULL,
+};
+
+/* Define OVP (Over Voltage Protection), OTP (Over Temperature Protection) */
+enum rt8973a_event_type {
+	RT8973A_EVENT_ATTACH = 1,
+	RT8973A_EVENT_DETACH,
+	RT8973A_EVENT_OVP,
+	RT8973A_EVENT_OTP,
+};
+
+/* Define supported accessory type */
+enum rt8973a_muic_acc_type {
+	RT8973A_MUIC_ADC_OTG = 0x0,
+	RT8973A_MUIC_ADC_AUDIO_SEND_END_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S1_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S2_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S3_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S4_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S5_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S6_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S7_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S8_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S9_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S10_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S11_BUTTON,
+	RT8973A_MUIC_ADC_AUDIO_REMOTE_S12_BUTTON,
+	RT8973A_MUIC_ADC_RESERVED_ACC_1,
+	RT8973A_MUIC_ADC_RESERVED_ACC_2,
+	RT8973A_MUIC_ADC_RESERVED_ACC_3,
+	RT8973A_MUIC_ADC_RESERVED_ACC_4,
+	RT8973A_MUIC_ADC_RESERVED_ACC_5,
+	RT8973A_MUIC_ADC_AUDIO_TYPE2,
+	RT8973A_MUIC_ADC_PHONE_POWERED_DEV,
+	RT8973A_MUIC_ADC_UNKNOWN_ACC_1,
+	RT8973A_MUIC_ADC_UNKNOWN_ACC_2,
+	RT8973A_MUIC_ADC_TA,
+	RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_OFF_USB,
+	RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_ON_USB,
+	RT8973A_MUIC_ADC_UNKNOWN_ACC_3,
+	RT8973A_MUIC_ADC_UNKNOWN_ACC_4,
+	RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_OFF_UART,
+	RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_ON_UART,
+	RT8973A_MUIC_ADC_UNKNOWN_ACC_5,
+	RT8973A_MUIC_ADC_OPEN = 0x1f,
+
+	/* The below accessories has same ADC value (0x1f).
+	   So, Device type1 is used to separate specific accessory. */
+					/* |---------|--ADC| */
+					/* |    [7:5]|[4:0]| */
+	RT8973A_MUIC_ADC_USB = 0x3f,	/* |      001|11111| */
+};
+
+/* List of supported interrupt for RT8973A */
+static struct muic_irq rt8973a_muic_irqs[] = {
+	{ RT8973A_INT1_ATTACH,		"muic-attach" },
+	{ RT8973A_INT1_DETACH,		"muic-detach" },
+	{ RT8973A_INT1_CHGDET,		"muic-chgdet" },
+	{ RT8973A_INT1_DCD_T,		"muic-dcd-t" },
+	{ RT8973A_INT1_OVP,		"muic-ovp" },
+	{ RT8973A_INT1_CONNECT,		"muic-connect" },
+	{ RT8973A_INT1_ADC_CHG,		"muic-adc-chg" },
+	{ RT8973A_INT1_OTP,		"muic-otp" },
+	{ RT8973A_INT2_UVLO,		"muic-uvlo" },
+	{ RT8973A_INT2_POR,		"muic-por" },
+	{ RT8973A_INT2_OTP_FET,		"muic-otp-fet" },
+	{ RT8973A_INT2_OVP_FET,		"muic-ovp-fet" },
+	{ RT8973A_INT2_OCP_LATCH,	"muic-ocp-latch" },
+	{ RT8973A_INT2_OCP,		"muic-ocp" },
+	{ RT8973A_INT2_OVP_OCP,		"muic-ovp-ocp" },
+};
+
+/* Define interrupt list of RT8973A to register regmap_irq */
+static const struct regmap_irq rt8973a_irqs[] = {
+	/* INT1 interrupts */
+	{ .reg_offset = 0, .mask = RT8973A_INT1_ATTACH_MASK, },
+	{ .reg_offset = 0, .mask = RT8973A_INT1_DETACH_MASK, },
+	{ .reg_offset = 0, .mask = RT8973A_INT1_CHGDET_MASK, },
+	{ .reg_offset = 0, .mask = RT8973A_INT1_DCD_T_MASK, },
+	{ .reg_offset = 0, .mask = RT8973A_INT1_OVP_MASK, },
+	{ .reg_offset = 0, .mask = RT8973A_INT1_CONNECT_MASK, },
+	{ .reg_offset = 0, .mask = RT8973A_INT1_ADC_CHG_MASK, },
+	{ .reg_offset = 0, .mask = RT8973A_INT1_OTP_MASK, },
+
+	/* INT2 interrupts */
+	{ .reg_offset = 1, .mask = RT8973A_INT2_UVLOT_MASK,},
+	{ .reg_offset = 1, .mask = RT8973A_INT2_POR_MASK, },
+	{ .reg_offset = 1, .mask = RT8973A_INT2_OTP_FET_MASK, },
+	{ .reg_offset = 1, .mask = RT8973A_INT2_OVP_FET_MASK, },
+	{ .reg_offset = 1, .mask = RT8973A_INT2_OCP_LATCH_MASK, },
+	{ .reg_offset = 1, .mask = RT8973A_INT2_OCP_MASK, },
+	{ .reg_offset = 1, .mask = RT8973A_INT2_OVP_OCP_MASK, },
+};
+
+static const struct regmap_irq_chip rt8973a_muic_irq_chip = {
+	.name			= "rt8973a",
+	.status_base		= RT8973A_REG_INT1,
+	.mask_base		= RT8973A_REG_INTM1,
+	.mask_invert		= false,
+	.num_regs		= 2,
+	.irqs			= rt8973a_irqs,
+	.num_irqs		= ARRAY_SIZE(rt8973a_irqs),
+};
+
+/* Define regmap configuration of RT8973A for I2C communication  */
+static bool rt8973a_muic_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case RT8973A_REG_INTM1:
+	case RT8973A_REG_INTM2:
+		return true;
+	default:
+		break;
+	}
+	return false;
+}
+
+static const struct regmap_config rt8973a_muic_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.volatile_reg	= rt8973a_muic_volatile_reg,
+	.max_register	= RT8973A_REG_END,
+};
+
+/* Change DM_CON/DP_CON/VBUSIN switch according to cable type */
+static int rt8973a_muic_set_path(struct rt8973a_muic_info *info,
+				unsigned int con_sw, bool attached)
+{
+	int ret;
+
+	/*
+	 * Don't need to set h/w path according to cable type
+	 * if Auto-configuration mode of CONTROL1 register is true.
+	 */
+	if (info->auto_config)
+		return 0;
+
+	if (!attached)
+		con_sw	= DM_DP_SWITCH_UART;
+
+	switch (con_sw) {
+	case DM_DP_SWITCH_OPEN:
+	case DM_DP_SWITCH_USB:
+	case DM_DP_SWITCH_UART:
+		ret = regmap_update_bits(info->regmap, RT8973A_REG_MANUAL_SW1,
+					RT8973A_REG_MANUAL_SW1_DP_MASK |
+					RT8973A_REG_MANUAL_SW1_DM_MASK,
+					con_sw);
+		if (ret < 0) {
+			dev_err(info->dev,
+				"cannot update DM_CON/DP_CON switch\n");
+			return ret;
+		}
+		break;
+	default:
+		dev_err(info->dev, "Unknown DM_CON/DP_CON switch type (%d)\n",
+				con_sw);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int rt8973a_muic_get_cable_type(struct rt8973a_muic_info *info)
+{
+	unsigned int adc, dev1;
+	int ret, cable_type;
+
+	/* Read ADC value according to external cable or button */
+	ret = regmap_read(info->regmap, RT8973A_REG_ADC, &adc);
+	if (ret) {
+		dev_err(info->dev, "failed to read ADC register\n");
+		return ret;
+	}
+	cable_type = adc & RT8973A_REG_ADC_MASK;
+
+	/* Read Device 1 reigster to identify correct cable type */
+	ret = regmap_read(info->regmap, RT8973A_REG_DEV1, &dev1);
+	if (ret) {
+		dev_err(info->dev, "failed to read DEV1 register\n");
+		return ret;
+	}
+
+	switch (adc) {
+	case RT8973A_MUIC_ADC_OPEN:
+		if (dev1 & RT8973A_REG_DEV1_USB_MASK)
+			cable_type = RT8973A_MUIC_ADC_USB;
+		else if (dev1 & RT8973A_REG_DEV1_DCPORT_MASK)
+			cable_type = RT8973A_MUIC_ADC_TA;
+		else
+			cable_type = RT8973A_MUIC_ADC_OPEN;
+		break;
+	default:
+		break;
+	}
+
+	return cable_type;
+}
+
+static int rt8973a_muic_cable_handler(struct rt8973a_muic_info *info,
+					enum rt8973a_event_type event)
+{
+	static unsigned int prev_cable_type;
+	const char **cable_names = info->edev->supported_cable;
+	unsigned int con_sw = DM_DP_SWITCH_UART;
+	int ret, idx = 0, cable_type;
+	bool attached = false;
+
+	if (!cable_names)
+		return 0;
+
+	switch (event) {
+	case RT8973A_EVENT_ATTACH:
+		cable_type = rt8973a_muic_get_cable_type(info);
+		attached = true;
+		break;
+	case RT8973A_EVENT_DETACH:
+		cable_type = prev_cable_type;
+		attached = false;
+		break;
+	case RT8973A_EVENT_OVP:
+	case RT8973A_EVENT_OTP:
+		dev_warn(info->dev,
+			"happen Over %s issue. Need to disconnect all cables\n",
+			event == RT8973A_EVENT_OVP ? "Voltage" : "Temperature");
+		cable_type = prev_cable_type;
+		attached = false;
+		break;
+	default:
+		dev_err(info->dev,
+			"Cannot handle this event (event:%d)\n", event);
+		return -EINVAL;
+	}
+	prev_cable_type = cable_type;
+
+	switch (cable_type) {
+	case RT8973A_MUIC_ADC_OTG:
+		idx = EXTCON_CABLE_USB_HOST;
+		con_sw = DM_DP_SWITCH_USB;
+		break;
+	case RT8973A_MUIC_ADC_TA:
+		idx = EXTCON_CABLE_TA;
+		con_sw = DM_DP_SWITCH_OPEN;
+		break;
+	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_OFF_USB:
+		idx = EXTCON_CABLE_JIG_OFF_USB;
+		con_sw = DM_DP_SWITCH_UART;
+		break;
+	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_ON_USB:
+		idx = EXTCON_CABLE_JIG_ON_USB;
+		con_sw = DM_DP_SWITCH_UART;
+		break;
+	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_OFF_UART:
+		idx = EXTCON_CABLE_JIG_OFF_UART;
+		con_sw = DM_DP_SWITCH_UART;
+		break;
+	case RT8973A_MUIC_ADC_FACTORY_MODE_BOOT_ON_UART:
+		idx = EXTCON_CABLE_JIG_ON_UART;
+		con_sw = DM_DP_SWITCH_UART;
+		break;
+	case RT8973A_MUIC_ADC_USB:
+		idx = EXTCON_CABLE_USB;
+		con_sw = DM_DP_SWITCH_USB;
+		break;
+	case RT8973A_MUIC_ADC_OPEN:
+		return 0;
+	case RT8973A_MUIC_ADC_UNKNOWN_ACC_1:
+	case RT8973A_MUIC_ADC_UNKNOWN_ACC_2:
+	case RT8973A_MUIC_ADC_UNKNOWN_ACC_3:
+	case RT8973A_MUIC_ADC_UNKNOWN_ACC_4:
+	case RT8973A_MUIC_ADC_UNKNOWN_ACC_5:
+		dev_warn(info->dev,
+			"Unknown accessory type (adc:0x%x)\n", cable_type);
+		return 0;
+	case RT8973A_MUIC_ADC_AUDIO_SEND_END_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S1_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S2_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S3_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S4_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S5_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S6_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S7_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S8_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S9_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S10_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S11_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_REMOTE_S12_BUTTON:
+	case RT8973A_MUIC_ADC_AUDIO_TYPE2:
+		dev_warn(info->dev,
+			"Audio device/button type (adc:0x%x)\n", cable_type);
+		return 0;
+	case RT8973A_MUIC_ADC_RESERVED_ACC_1:
+	case RT8973A_MUIC_ADC_RESERVED_ACC_2:
+	case RT8973A_MUIC_ADC_RESERVED_ACC_3:
+	case RT8973A_MUIC_ADC_RESERVED_ACC_4:
+	case RT8973A_MUIC_ADC_RESERVED_ACC_5:
+	case RT8973A_MUIC_ADC_PHONE_POWERED_DEV:
+		return 0;
+	default:
+		dev_err(info->dev,
+			"Cannot handle this cable_type (adc:0x%x)\n",
+			cable_type);
+		return -EINVAL;
+	}
+
+	/* Change internal hardware path(DM_CON/DP_CON) */
+	ret = rt8973a_muic_set_path(info, con_sw, attached);
+	if (ret < 0)
+		return ret;
+
+	/* Change the state of external accessory */
+	extcon_set_cable_state(info->edev, cable_names[idx], attached);
+
+	return 0;
+}
+
+static void rt8973a_muic_irq_work(struct work_struct *work)
+{
+	struct rt8973a_muic_info *info = container_of(work,
+			struct rt8973a_muic_info, irq_work);
+	int ret = 0;
+
+	if (!info->edev)
+		return;
+
+	mutex_lock(&info->mutex);
+
+	/* Detect attached or detached cables */
+	if (info->irq_attach) {
+		ret = rt8973a_muic_cable_handler(info, RT8973A_EVENT_ATTACH);
+		info->irq_attach = false;
+	}
+
+	if (info->irq_detach) {
+		ret = rt8973a_muic_cable_handler(info, RT8973A_EVENT_DETACH);
+		info->irq_detach = false;
+	}
+
+	if (info->irq_ovp) {
+		ret = rt8973a_muic_cable_handler(info, RT8973A_EVENT_OVP);
+		info->irq_ovp = false;
+	}
+
+	if (info->irq_otp) {
+		ret = rt8973a_muic_cable_handler(info, RT8973A_EVENT_OTP);
+		info->irq_otp = false;
+	}
+
+	if (ret < 0)
+		dev_err(info->dev, "failed to handle MUIC interrupt\n");
+
+	mutex_unlock(&info->mutex);
+}
+
+static irqreturn_t rt8973a_muic_irq_handler(int irq, void *data)
+{
+	struct rt8973a_muic_info *info = data;
+	int i, irq_type = -1;
+
+	for (i = 0; i < info->num_muic_irqs; i++)
+		if (irq == info->muic_irqs[i].virq)
+			irq_type = info->muic_irqs[i].irq;
+
+	switch (irq_type) {
+	case RT8973A_INT1_ATTACH:
+		info->irq_attach = true;
+		break;
+	case RT8973A_INT1_DETACH:
+		info->irq_detach = true;
+		break;
+	case RT8973A_INT1_OVP:
+		info->irq_ovp = true;
+		break;
+	case RT8973A_INT1_OTP:
+		info->irq_otp = true;
+		break;
+	case RT8973A_INT1_CHGDET:
+	case RT8973A_INT1_DCD_T:
+	case RT8973A_INT1_CONNECT:
+	case RT8973A_INT1_ADC_CHG:
+	case RT8973A_INT2_UVLO:
+	case RT8973A_INT2_POR:
+	case RT8973A_INT2_OTP_FET:
+	case RT8973A_INT2_OVP_FET:
+	case RT8973A_INT2_OCP_LATCH:
+	case RT8973A_INT2_OCP:
+	case RT8973A_INT2_OVP_OCP:
+	default:
+		dev_dbg(info->dev,
+			"Cannot handle this interrupt (%d)\n", irq_type);
+		break;
+	}
+
+	schedule_work(&info->irq_work);
+
+	return IRQ_HANDLED;
+}
+
+static void rt8973a_muic_detect_cable_wq(struct work_struct *work)
+{
+	struct rt8973a_muic_info *info = container_of(to_delayed_work(work),
+				struct rt8973a_muic_info, wq_detcable);
+	int ret;
+
+	/* Notify the state of connector cable or not  */
+	ret = rt8973a_muic_cable_handler(info, RT8973A_EVENT_ATTACH);
+	if (ret < 0)
+		dev_warn(info->dev, "failed to detect cable state\n");
+}
+
+static void rt8973a_init_dev_type(struct rt8973a_muic_info *info)
+{
+	unsigned int data, vendor_id, version_id;
+	int i, ret;
+
+	/* To test I2C, Print version_id and vendor_id of RT8973A */
+	ret = regmap_read(info->regmap, RT8973A_REG_DEVICE_ID, &data);
+	if (ret) {
+		dev_err(info->dev,
+			"failed to read DEVICE_ID register: %d\n", ret);
+		return;
+	}
+
+	vendor_id = ((data & RT8973A_REG_DEVICE_ID_VENDOR_MASK) >>
+				RT8973A_REG_DEVICE_ID_VENDOR_SHIFT);
+	version_id = ((data & RT8973A_REG_DEVICE_ID_VERSION_MASK) >>
+				RT8973A_REG_DEVICE_ID_VERSION_SHIFT);
+
+	dev_info(info->dev, "Device type: version: 0x%x, vendor: 0x%x\n",
+			    version_id, vendor_id);
+
+	/* Initiazle the register of RT8973A device to bring-up */
+	for (i = 0; i < info->num_reg_data; i++) {
+		u8 reg = info->reg_data[i].reg;
+		u8 mask = info->reg_data[i].mask;
+		u8 val = 0;
+
+		if (info->reg_data[i].invert)
+			val = ~info->reg_data[i].val;
+		else
+			val = info->reg_data[i].val;
+
+		regmap_update_bits(info->regmap, reg, mask, val);
+	}
+
+	/* Check whether RT8973A is auto swithcing mode or not */
+	ret = regmap_read(info->regmap, RT8973A_REG_CONTROL1, &data);
+	if (ret) {
+		dev_err(info->dev,
+			"failed to read CONTROL1 register: %d\n", ret);
+		return;
+	}
+
+	data &= RT8973A_REG_CONTROL1_AUTO_CONFIG_MASK;
+	if (data) {
+		info->auto_config = true;
+		dev_info(info->dev,
+			"Enable Auto-configuration for internal path\n");
+	}
+}
+
+static int rt8973a_muic_i2c_probe(struct i2c_client *i2c,
+				 const struct i2c_device_id *id)
+{
+	struct device_node *np = i2c->dev.of_node;
+	struct rt8973a_muic_info *info;
+	int i, ret, irq_flags;
+
+	if (!np)
+		return -EINVAL;
+
+	info = devm_kzalloc(&i2c->dev, sizeof(*info), GFP_KERNEL);
+	if (!info) {
+		dev_err(&i2c->dev, "failed to allocate memory\n");
+		return -ENOMEM;
+	}
+	i2c_set_clientdata(i2c, info);
+
+	info->dev = &i2c->dev;
+	info->i2c = i2c;
+	info->irq = i2c->irq;
+	info->muic_irqs = rt8973a_muic_irqs;
+	info->num_muic_irqs = ARRAY_SIZE(rt8973a_muic_irqs);
+	info->reg_data = rt8973a_reg_data;
+	info->num_reg_data = ARRAY_SIZE(rt8973a_reg_data);
+
+	mutex_init(&info->mutex);
+
+	INIT_WORK(&info->irq_work, rt8973a_muic_irq_work);
+
+	info->regmap = devm_regmap_init_i2c(i2c, &rt8973a_muic_regmap_config);
+	if (IS_ERR(info->regmap)) {
+		ret = PTR_ERR(info->regmap);
+		dev_err(info->dev, "failed to allocate register map: %d\n",
+				   ret);
+		return ret;
+	}
+
+	/* Support irq domain for RT8973A MUIC device */
+	irq_flags = IRQF_TRIGGER_FALLING | IRQF_ONESHOT | IRQF_SHARED;
+	ret = regmap_add_irq_chip(info->regmap, info->irq, irq_flags, 0,
+				  &rt8973a_muic_irq_chip, &info->irq_data);
+	if (ret != 0) {
+		dev_err(info->dev, "failed to add irq_chip (irq:%d, err:%d)\n",
+				    info->irq, ret);
+		return ret;
+	}
+
+	for (i = 0; i < info->num_muic_irqs; i++) {
+		struct muic_irq *muic_irq = &info->muic_irqs[i];
+		unsigned int virq = 0;
+
+		virq = regmap_irq_get_virq(info->irq_data, muic_irq->irq);
+		if (virq <= 0)
+			return -EINVAL;
+		muic_irq->virq = virq;
+
+		ret = devm_request_threaded_irq(info->dev, virq, NULL,
+						rt8973a_muic_irq_handler,
+						IRQF_NO_SUSPEND,
+						muic_irq->name, info);
+		if (ret) {
+			dev_err(info->dev,
+				"failed: irq request (IRQ: %d, error :%d)\n",
+				muic_irq->irq, ret);
+			return ret;
+		}
+	}
+
+	/* Allocate extcon device */
+	info->edev = devm_extcon_dev_allocate(info->dev, rt8973a_extcon_cable);
+	if (IS_ERR(info->edev)) {
+		dev_err(info->dev, "failed to allocate memory for extcon\n");
+		return -ENOMEM;
+	}
+	info->edev->name = np->name;
+
+	/* Register extcon device */
+	ret = devm_extcon_dev_register(info->dev, info->edev);
+	if (ret) {
+		dev_err(info->dev, "failed to register extcon device\n");
+		return ret;
+	}
+
+	/*
+	 * Detect accessory after completing the initialization of platform
+	 *
+	 * - Use delayed workqueue to detect cable state and then
+	 * notify cable state to notifiee/platform through uevent.
+	 * After completing the booting of platform, the extcon provider
+	 * driver should notify cable state to upper layer.
+	 */
+	INIT_DELAYED_WORK(&info->wq_detcable, rt8973a_muic_detect_cable_wq);
+	queue_delayed_work(system_power_efficient_wq, &info->wq_detcable,
+			msecs_to_jiffies(DELAY_MS_DEFAULT));
+
+	/* Initialize RT8973A device and print vendor id and version id */
+	rt8973a_init_dev_type(info);
+
+	return 0;
+}
+
+static int rt8973a_muic_i2c_remove(struct i2c_client *i2c)
+{
+	struct rt8973a_muic_info *info = i2c_get_clientdata(i2c);
+
+	regmap_del_irq_chip(info->irq, info->irq_data);
+
+	return 0;
+}
+
+static struct of_device_id rt8973a_dt_match[] = {
+	{ .compatible = "richtek,rt8973a-muic" },
+	{ },
+};
+
+#ifdef CONFIG_PM_SLEEP
+static int rt8973a_muic_suspend(struct device *dev)
+{
+	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct rt8973a_muic_info *info = i2c_get_clientdata(i2c);
+
+	enable_irq_wake(info->irq);
+
+	return 0;
+}
+
+static int rt8973a_muic_resume(struct device *dev)
+{
+	struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+	struct rt8973a_muic_info *info = i2c_get_clientdata(i2c);
+
+	disable_irq_wake(info->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(rt8973a_muic_pm_ops,
+			 rt8973a_muic_suspend, rt8973a_muic_resume);
+
+static const struct i2c_device_id rt8973a_i2c_id[] = {
+	{ "rt8973a", TYPE_RT8973A },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, rt8973a_i2c_id);
+
+static struct i2c_driver rt8973a_muic_i2c_driver = {
+	.driver		= {
+		.name	= "rt8973a",
+		.owner	= THIS_MODULE,
+		.pm	= &rt8973a_muic_pm_ops,
+		.of_match_table = rt8973a_dt_match,
+	},
+	.probe	= rt8973a_muic_i2c_probe,
+	.remove	= rt8973a_muic_i2c_remove,
+	.id_table = rt8973a_i2c_id,
+};
+
+static int __init rt8973a_muic_i2c_init(void)
+{
+	return i2c_add_driver(&rt8973a_muic_i2c_driver);
+}
+subsys_initcall(rt8973a_muic_i2c_init);
+
+MODULE_DESCRIPTION("Richtek RT8973A Extcon driver");
+MODULE_AUTHOR("Chanwoo Choi <cw00.choi@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/extcon/extcon-rt8973a.h b/drivers/extcon/extcon-rt8973a.h
new file mode 100644
index 000000000000..9dc3e0227eb7
--- /dev/null
+++ b/drivers/extcon/extcon-rt8973a.h
@@ -0,0 +1,203 @@
+/*
+ * rt8973a.h
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#ifndef __LINUX_EXTCON_RT8973A_H
+#define __LINUX_EXTCON_RT8973A_H
+
+enum rt8973a_types {
+	TYPE_RT8973A,
+};
+
+/* RT8973A registers */
+enum rt8973A_reg {
+	RT8973A_REG_DEVICE_ID = 0x1,
+	RT8973A_REG_CONTROL1,
+	RT8973A_REG_INT1,
+	RT8973A_REG_INT2,
+	RT8973A_REG_INTM1,
+	RT8973A_REG_INTM2,
+	RT8973A_REG_ADC,
+	RT8973A_REG_RSVD_1,
+	RT8973A_REG_RSVD_2,
+	RT8973A_REG_DEV1,
+	RT8973A_REG_DEV2,
+	RT8973A_REG_RSVD_3,
+	RT8973A_REG_RSVD_4,
+	RT8973A_REG_RSVD_5,
+	RT8973A_REG_RSVD_6,
+	RT8973A_REG_RSVD_7,
+	RT8973A_REG_RSVD_8,
+	RT8973A_REG_RSVD_9,
+	RT8973A_REG_MANUAL_SW1,
+	RT8973A_REG_MANUAL_SW2,
+	RT8973A_REG_RSVD_10,
+	RT8973A_REG_RSVD_11,
+	RT8973A_REG_RSVD_12,
+	RT8973A_REG_RSVD_13,
+	RT8973A_REG_RSVD_14,
+	RT8973A_REG_RSVD_15,
+	RT8973A_REG_RESET,
+
+	RT8973A_REG_END,
+};
+
+/* Define RT8973A MASK/SHIFT constant */
+#define RT8973A_REG_DEVICE_ID_VENDOR_SHIFT	0
+#define RT8973A_REG_DEVICE_ID_VERSION_SHIFT	3
+#define RT8973A_REG_DEVICE_ID_VENDOR_MASK	(0x7 << RT8973A_REG_DEVICE_ID_VENDOR_SHIFT)
+#define RT8973A_REG_DEVICE_ID_VERSION_MASK	(0x1f << RT8973A_REG_DEVICE_ID_VERSION_SHIFT)
+
+#define RT8973A_REG_CONTROL1_INTM_SHIFT	0
+#define RT8973A_REG_CONTROL1_AUTO_CONFIG_SHIFT	2
+#define RT8973A_REG_CONTROL1_I2C_RST_EN_SHIFT	3
+#define RT8973A_REG_CONTROL1_SWITCH_OPEN_SHIFT	4
+#define RT8973A_REG_CONTROL1_CHGTYP_SHIFT	5
+#define RT8973A_REG_CONTROL1_USB_CHD_EN_SHIFT	6
+#define RT8973A_REG_CONTROL1_ADC_EN_SHIFT	7
+#define RT8973A_REG_CONTROL1_INTM_MASK		(0x1 << RT8973A_REG_CONTROL1_INTM_SHIFT)
+#define RT8973A_REG_CONTROL1_AUTO_CONFIG_MASK	(0x1 << RT8973A_REG_CONTROL1_AUTO_CONFIG_SHIFT)
+#define RT8973A_REG_CONTROL1_I2C_RST_EN_MASK	(0x1 << RT8973A_REG_CONTROL1_I2C_RST_EN_SHIFT)
+#define RT8973A_REG_CONTROL1_SWITCH_OPEN_MASK	(0x1 << RT8973A_REG_CONTROL1_SWITCH_OPEN_SHIFT)
+#define RT8973A_REG_CONTROL1_CHGTYP_MASK	(0x1 << RT8973A_REG_CONTROL1_CHGTYP_SHIFT)
+#define RT8973A_REG_CONTROL1_USB_CHD_EN_MASK	(0x1 << RT8973A_REG_CONTROL1_USB_CHD_EN_SHIFT)
+#define RT8973A_REG_CONTROL1_ADC_EN_MASK	(0x1 << RT8973A_REG_CONTROL1_ADC_EN_SHIFT)
+
+#define RT9873A_REG_INTM1_ATTACH_SHIFT		0
+#define RT9873A_REG_INTM1_DETACH_SHIFT		1
+#define RT9873A_REG_INTM1_CHGDET_SHIFT		2
+#define RT9873A_REG_INTM1_DCD_T_SHIFT		3
+#define RT9873A_REG_INTM1_OVP_SHIFT		4
+#define RT9873A_REG_INTM1_CONNECT_SHIFT		5
+#define RT9873A_REG_INTM1_ADC_CHG_SHIFT		6
+#define RT9873A_REG_INTM1_OTP_SHIFT		7
+#define RT9873A_REG_INTM1_ATTACH_MASK		(0x1 << RT9873A_REG_INTM1_ATTACH_SHIFT)
+#define RT9873A_REG_INTM1_DETACH_MASK		(0x1 <<  RT9873A_REG_INTM1_DETACH_SHIFT)
+#define RT9873A_REG_INTM1_CHGDET_MASK		(0x1 <<  RT9873A_REG_INTM1_CHGDET_SHIFT)
+#define RT9873A_REG_INTM1_DCD_T_MASK		(0x1 <<  RT9873A_REG_INTM1_DCD_T_SHIFT)
+#define RT9873A_REG_INTM1_OVP_MASK		(0x1 <<  RT9873A_REG_INTM1_OVP_SHIFT)
+#define RT9873A_REG_INTM1_CONNECT_MASK		(0x1 <<  RT9873A_REG_INTM1_CONNECT_SHIFT)
+#define RT9873A_REG_INTM1_ADC_CHG_MASK		(0x1 <<  RT9873A_REG_INTM1_ADC_CHG_SHIFT)
+#define RT9873A_REG_INTM1_OTP_MASK		(0x1 <<  RT9873A_REG_INTM1_OTP_SHIFT)
+
+#define RT9873A_REG_INTM2_UVLO_SHIFT		1
+#define RT9873A_REG_INTM2_POR_SHIFT		2
+#define RT9873A_REG_INTM2_OTP_FET_SHIFT		3
+#define RT9873A_REG_INTM2_OVP_FET_SHIFT		4
+#define RT9873A_REG_INTM2_OCP_LATCH_SHIFT	5
+#define RT9873A_REG_INTM2_OCP_SHIFT		6
+#define RT9873A_REG_INTM2_OVP_OCP_SHIFT		7
+#define RT9873A_REG_INTM2_UVLO_MASK		(0x1 << RT9873A_REG_INTM2_UVLO_SHIFT)
+#define RT9873A_REG_INTM2_POR_MASK		(0x1 <<  RT9873A_REG_INTM2_POR_SHIFT)
+#define RT9873A_REG_INTM2_OTP_FET_MASK		(0x1 <<  RT9873A_REG_INTM2_OTP_FET_SHIFT)
+#define RT9873A_REG_INTM2_OVP_FET_MASK		(0x1 <<  RT9873A_REG_INTM2_OVP_FET_SHIFT)
+#define RT9873A_REG_INTM2_OCP_LATCH_MASK	(0x1 <<  RT9873A_REG_INTM2_OCP_LATCH_SHIFT)
+#define RT9873A_REG_INTM2_OCP_MASK		(0x1 <<  RT9873A_REG_INTM2_OCP_SHIFT)
+#define RT9873A_REG_INTM2_OVP_OCP_MASK		(0x1 <<  RT9873A_REG_INTM2_OVP_OCP_SHIFT)
+
+#define RT8973A_REG_ADC_SHIFT			0
+#define RT8973A_REG_ADC_MASK			(0x1f << RT8973A_REG_ADC_SHIFT)
+
+#define RT8973A_REG_DEV1_OTG_SHIFT		0
+#define RT8973A_REG_DEV1_SDP_SHIFT		2
+#define RT8973A_REG_DEV1_UART_SHIFT		3
+#define RT8973A_REG_DEV1_CAR_KIT_TYPE1_SHIFT	4
+#define RT8973A_REG_DEV1_CDPORT_SHIFT		5
+#define RT8973A_REG_DEV1_DCPORT_SHIFT		6
+#define RT8973A_REG_DEV1_OTG_MASK		(0x1 << RT8973A_REG_DEV1_OTG_SHIFT)
+#define RT8973A_REG_DEV1_SDP_MASK		(0x1 << RT8973A_REG_DEV1_SDP_SHIFT)
+#define RT8973A_REG_DEV1_UART_MASK		(0x1 << RT8973A_REG_DEV1_UART_SHIFT)
+#define RT8973A_REG_DEV1_CAR_KIT_TYPE1_MASK	(0x1 << RT8973A_REG_DEV1_CAR_KIT_TYPE1_SHIFT)
+#define RT8973A_REG_DEV1_CDPORT_MASK		(0x1 << RT8973A_REG_DEV1_CDPORT_SHIFT)
+#define RT8973A_REG_DEV1_DCPORT_MASK		(0x1 << RT8973A_REG_DEV1_DCPORT_SHIFT)
+#define RT8973A_REG_DEV1_USB_MASK		(RT8973A_REG_DEV1_SDP_MASK \
+						| RT8973A_REG_DEV1_CDPORT_MASK)
+
+#define RT8973A_REG_DEV2_JIG_USB_ON_SHIFT	0
+#define RT8973A_REG_DEV2_JIG_USB_OFF_SHIFT	1
+#define RT8973A_REG_DEV2_JIG_UART_ON_SHIFT	2
+#define RT8973A_REG_DEV2_JIG_UART_OFF_SHIFT	3
+#define RT8973A_REG_DEV2_JIG_USB_ON_MASK	(0x1 << RT8973A_REG_DEV2_JIG_USB_ON_SHIFT)
+#define RT8973A_REG_DEV2_JIG_USB_OFF_MASK	(0x1 << RT8973A_REG_DEV2_JIG_USB_OFF_SHIFT)
+#define RT8973A_REG_DEV2_JIG_UART_ON_MASK	(0x1 << RT8973A_REG_DEV2_JIG_UART_ON_SHIFT)
+#define RT8973A_REG_DEV2_JIG_UART_OFF_MASK	(0x1 << RT8973A_REG_DEV2_JIG_UART_OFF_SHIFT)
+
+#define RT8973A_REG_MANUAL_SW1_DP_SHIFT		2
+#define RT8973A_REG_MANUAL_SW1_DM_SHIFT		5
+#define RT8973A_REG_MANUAL_SW1_DP_MASK		(0x7 << RT8973A_REG_MANUAL_SW1_DP_SHIFT)
+#define RT8973A_REG_MANUAL_SW1_DM_MASK		(0x7 << RT8973A_REG_MANUAL_SW1_DM_SHIFT)
+#define DM_DP_CON_SWITCH_OPEN			0x0
+#define DM_DP_CON_SWITCH_USB			0x1
+#define DM_DP_CON_SWITCH_UART			0x3
+#define DM_DP_SWITCH_OPEN			((DM_DP_CON_SWITCH_OPEN << RT8973A_REG_MANUAL_SW1_DP_SHIFT) \
+						| (DM_DP_CON_SWITCH_OPEN << RT8973A_REG_MANUAL_SW1_DM_SHIFT))
+#define DM_DP_SWITCH_USB			((DM_DP_CON_SWITCH_USB << RT8973A_REG_MANUAL_SW1_DP_SHIFT) \
+						| (DM_DP_CON_SWITCH_USB << RT8973A_REG_MANUAL_SW1_DM_SHIFT))
+#define DM_DP_SWITCH_UART			((DM_DP_CON_SWITCH_UART << RT8973A_REG_MANUAL_SW1_DP_SHIFT) \
+						| (DM_DP_CON_SWITCH_UART << RT8973A_REG_MANUAL_SW1_DM_SHIFT))
+
+#define RT8973A_REG_MANUAL_SW2_FET_ON_SHIFT	0
+#define RT8973A_REG_MANUAL_SW2_JIG_ON_SHIFT	2
+#define RT8973A_REG_MANUAL_SW2_BOOT_SW_SHIFT	3
+#define RT8973A_REG_MANUAL_SW2_FET_ON_MASK	(0x1 << RT8973A_REG_MANUAL_SW2_FET_ON_SHIFT)
+#define RT8973A_REG_MANUAL_SW2_JIG_ON_MASK	(0x1 << RT8973A_REG_MANUAL_SW2_JIG_ON_SHIFT)
+#define RT8973A_REG_MANUAL_SW2_BOOT_SW_MASK	(0x1 << RT8973A_REG_MANUAL_SW2_BOOT_SW_SHIFT)
+#define RT8973A_REG_MANUAL_SW2_FET_ON		0
+#define RT8973A_REG_MANUAL_SW2_FET_OFF		0x1
+#define RT8973A_REG_MANUAL_SW2_JIG_OFF		0
+#define RT8973A_REG_MANUAL_SW2_JIG_ON		0x1
+#define RT8973A_REG_MANUAL_SW2_BOOT_SW_ON	0
+#define RT8973A_REG_MANUAL_SW2_BOOT_SW_OFF	0x1
+
+#define RT8973A_REG_RESET_SHIFT			0
+#define RT8973A_REG_RESET_MASK			(0x1 << RT8973A_REG_RESET_SHIFT)
+#define RT8973A_REG_RESET			0x1
+
+/* RT8973A Interrupts */
+enum rt8973a_irq {
+	/* Interrupt1*/
+	RT8973A_INT1_ATTACH,
+	RT8973A_INT1_DETACH,
+	RT8973A_INT1_CHGDET,
+	RT8973A_INT1_DCD_T,
+	RT8973A_INT1_OVP,
+	RT8973A_INT1_CONNECT,
+	RT8973A_INT1_ADC_CHG,
+	RT8973A_INT1_OTP,
+
+	/* Interrupt2*/
+	RT8973A_INT2_UVLO,
+	RT8973A_INT2_POR,
+	RT8973A_INT2_OTP_FET,
+	RT8973A_INT2_OVP_FET,
+	RT8973A_INT2_OCP_LATCH,
+	RT8973A_INT2_OCP,
+	RT8973A_INT2_OVP_OCP,
+
+	RT8973A_NUM,
+};
+
+#define RT8973A_INT1_ATTACH_MASK		BIT(0)
+#define RT8973A_INT1_DETACH_MASK		BIT(1)
+#define RT8973A_INT1_CHGDET_MASK		BIT(2)
+#define RT8973A_INT1_DCD_T_MASK			BIT(3)
+#define RT8973A_INT1_OVP_MASK			BIT(4)
+#define RT8973A_INT1_CONNECT_MASK		BIT(5)
+#define RT8973A_INT1_ADC_CHG_MASK		BIT(6)
+#define RT8973A_INT1_OTP_MASK			BIT(7)
+#define RT8973A_INT2_UVLOT_MASK			BIT(0)
+#define RT8973A_INT2_POR_MASK			BIT(1)
+#define RT8973A_INT2_OTP_FET_MASK		BIT(2)
+#define RT8973A_INT2_OVP_FET_MASK		BIT(3)
+#define RT8973A_INT2_OCP_LATCH_MASK		BIT(4)
+#define RT8973A_INT2_OCP_MASK			BIT(5)
+#define RT8973A_INT2_OVP_OCP_MASK		BIT(6)
+
+#endif /*  __LINUX_EXTCON_RT8973A_H */
diff --git a/drivers/extcon/extcon-sm5502.c b/drivers/extcon/extcon-sm5502.c
index 560d7dccec7b..b0f7bd82af90 100644
--- a/drivers/extcon/extcon-sm5502.c
+++ b/drivers/extcon/extcon-sm5502.c
@@ -8,16 +8,10 @@
  * under  the terms of  the GNU General  Public License as published by the
  * Free Software Foundation;  either version 2 of the  License, or (at your
  * option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
  */
 
 #include <linux/err.h>
 #include <linux/i2c.h>
-#include <linux/input.h>
 #include <linux/interrupt.h>
 #include <linux/irqdomain.h>
 #include <linux/kernel.h>
@@ -26,7 +20,8 @@
 #include <linux/regmap.h>
 #include <linux/slab.h>
 #include <linux/extcon.h>
-#include <linux/extcon/sm5502.h>
+
+#include "extcon-sm5502.h"
 
 #define	DELAY_MS_DEFAULT		17000	/* unit: millisecond */
 
@@ -300,7 +295,7 @@ static unsigned int sm5502_muic_get_cable_type(struct sm5502_muic_info *info)
 	 * If ADC is SM5502_MUIC_ADC_GROUND(0x0), external cable hasn't
 	 * connected with to MUIC device.
 	 */
-	cable_type &= SM5502_REG_ADC_MASK;
+	cable_type = adc & SM5502_REG_ADC_MASK;
 	if (cable_type == SM5502_MUIC_ADC_GROUND)
 		return SM5502_MUIC_ADC_GROUND;
 
@@ -395,7 +390,7 @@ static int sm5502_muic_cable_handler(struct sm5502_muic_info *info,
 	/* Get the type of attached or detached cable */
 	if (attached)
 		cable_type = sm5502_muic_get_cable_type(info);
-	else if (!attached)
+	else
 		cable_type = prev_cable_type;
 	prev_cable_type = cable_type;
 
@@ -457,8 +452,6 @@ static void sm5502_muic_irq_work(struct work_struct *work)
 		dev_err(info->dev, "failed to handle MUIC interrupt\n");
 
 	mutex_unlock(&info->mutex);
-
-	return;
 }
 
 /*
@@ -617,8 +610,9 @@ static int sm5022_muic_i2c_probe(struct i2c_client *i2c,
 						IRQF_NO_SUSPEND,
 						muic_irq->name, info);
 		if (ret) {
-			dev_err(info->dev, "failed: irq request (IRQ: %d,"
-				" error :%d)\n", muic_irq->irq, ret);
+			dev_err(info->dev,
+				"failed: irq request (IRQ: %d, error :%d)\n",
+				muic_irq->irq, ret);
 			return ret;
 		}
 	}
diff --git a/drivers/extcon/extcon-sm5502.h b/drivers/extcon/extcon-sm5502.h
new file mode 100644
index 000000000000..974b53222f56
--- /dev/null
+++ b/drivers/extcon/extcon-sm5502.h
@@ -0,0 +1,282 @@
+/*
+ * sm5502.h
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#ifndef __LINUX_EXTCON_SM5502_H
+#define __LINUX_EXTCON_SM5502_H
+
+enum sm5502_types {
+	TYPE_SM5502,
+};
+
+/* SM5502 registers */
+enum sm5502_reg {
+	SM5502_REG_DEVICE_ID = 0x01,
+	SM5502_REG_CONTROL,
+	SM5502_REG_INT1,
+	SM5502_REG_INT2,
+	SM5502_REG_INTMASK1,
+	SM5502_REG_INTMASK2,
+	SM5502_REG_ADC,
+	SM5502_REG_TIMING_SET1,
+	SM5502_REG_TIMING_SET2,
+	SM5502_REG_DEV_TYPE1,
+	SM5502_REG_DEV_TYPE2,
+	SM5502_REG_BUTTON1,
+	SM5502_REG_BUTTON2,
+	SM5502_REG_CAR_KIT_STATUS,
+	SM5502_REG_RSVD1,
+	SM5502_REG_RSVD2,
+	SM5502_REG_RSVD3,
+	SM5502_REG_RSVD4,
+	SM5502_REG_MANUAL_SW1,
+	SM5502_REG_MANUAL_SW2,
+	SM5502_REG_DEV_TYPE3,
+	SM5502_REG_RSVD5,
+	SM5502_REG_RSVD6,
+	SM5502_REG_RSVD7,
+	SM5502_REG_RSVD8,
+	SM5502_REG_RSVD9,
+	SM5502_REG_RESET,
+	SM5502_REG_RSVD10,
+	SM5502_REG_RESERVED_ID1,
+	SM5502_REG_RSVD11,
+	SM5502_REG_RSVD12,
+	SM5502_REG_RESERVED_ID2,
+	SM5502_REG_RSVD13,
+	SM5502_REG_OCP,
+	SM5502_REG_RSVD14,
+	SM5502_REG_RSVD15,
+	SM5502_REG_RSVD16,
+	SM5502_REG_RSVD17,
+	SM5502_REG_RSVD18,
+	SM5502_REG_RSVD19,
+	SM5502_REG_RSVD20,
+	SM5502_REG_RSVD21,
+	SM5502_REG_RSVD22,
+	SM5502_REG_RSVD23,
+	SM5502_REG_RSVD24,
+	SM5502_REG_RSVD25,
+	SM5502_REG_RSVD26,
+	SM5502_REG_RSVD27,
+	SM5502_REG_RSVD28,
+	SM5502_REG_RSVD29,
+	SM5502_REG_RSVD30,
+	SM5502_REG_RSVD31,
+	SM5502_REG_RSVD32,
+	SM5502_REG_RSVD33,
+	SM5502_REG_RSVD34,
+	SM5502_REG_RSVD35,
+	SM5502_REG_RSVD36,
+	SM5502_REG_RESERVED_ID3,
+
+	SM5502_REG_END,
+};
+
+/* Define SM5502 MASK/SHIFT constant */
+#define SM5502_REG_DEVICE_ID_VENDOR_SHIFT	0
+#define SM5502_REG_DEVICE_ID_VERSION_SHIFT	3
+#define SM5502_REG_DEVICE_ID_VENDOR_MASK	(0x3 << SM5502_REG_DEVICE_ID_VENDOR_SHIFT)
+#define SM5502_REG_DEVICE_ID_VERSION_MASK	(0x1f << SM5502_REG_DEVICE_ID_VERSION_SHIFT)
+
+#define SM5502_REG_CONTROL_MASK_INT_SHIFT	0
+#define SM5502_REG_CONTROL_WAIT_SHIFT		1
+#define SM5502_REG_CONTROL_MANUAL_SW_SHIFT	2
+#define SM5502_REG_CONTROL_RAW_DATA_SHIFT	3
+#define SM5502_REG_CONTROL_SW_OPEN_SHIFT	4
+#define SM5502_REG_CONTROL_MASK_INT_MASK	(0x1 << SM5502_REG_CONTROL_MASK_INT_SHIFT)
+#define SM5502_REG_CONTROL_WAIT_MASK		(0x1 << SM5502_REG_CONTROL_WAIT_SHIFT)
+#define SM5502_REG_CONTROL_MANUAL_SW_MASK	(0x1 << SM5502_REG_CONTROL_MANUAL_SW_SHIFT)
+#define SM5502_REG_CONTROL_RAW_DATA_MASK	(0x1 << SM5502_REG_CONTROL_RAW_DATA_SHIFT)
+#define SM5502_REG_CONTROL_SW_OPEN_MASK		(0x1 << SM5502_REG_CONTROL_SW_OPEN_SHIFT)
+
+#define SM5502_REG_INTM1_ATTACH_SHIFT		0
+#define SM5502_REG_INTM1_DETACH_SHIFT		1
+#define SM5502_REG_INTM1_KP_SHIFT		2
+#define SM5502_REG_INTM1_LKP_SHIFT		3
+#define SM5502_REG_INTM1_LKR_SHIFT		4
+#define SM5502_REG_INTM1_OVP_EVENT_SHIFT	5
+#define SM5502_REG_INTM1_OCP_EVENT_SHIFT	6
+#define SM5502_REG_INTM1_OVP_OCP_DIS_SHIFT	7
+#define SM5502_REG_INTM1_ATTACH_MASK		(0x1 << SM5502_REG_INTM1_ATTACH_SHIFT)
+#define SM5502_REG_INTM1_DETACH_MASK		(0x1 << SM5502_REG_INTM1_DETACH_SHIFT)
+#define SM5502_REG_INTM1_KP_MASK		(0x1 << SM5502_REG_INTM1_KP_SHIFT)
+#define SM5502_REG_INTM1_LKP_MASK		(0x1 << SM5502_REG_INTM1_LKP_SHIFT)
+#define SM5502_REG_INTM1_LKR_MASK		(0x1 << SM5502_REG_INTM1_LKR_SHIFT)
+#define SM5502_REG_INTM1_OVP_EVENT_MASK		(0x1 << SM5502_REG_INTM1_OVP_EVENT_SHIFT)
+#define SM5502_REG_INTM1_OCP_EVENT_MASK		(0x1 << SM5502_REG_INTM1_OCP_EVENT_SHIFT)
+#define SM5502_REG_INTM1_OVP_OCP_DIS_MASK	(0x1 << SM5502_REG_INTM1_OVP_OCP_DIS_SHIFT)
+
+#define SM5502_REG_INTM2_VBUS_DET_SHIFT		0
+#define SM5502_REG_INTM2_REV_ACCE_SHIFT		1
+#define SM5502_REG_INTM2_ADC_CHG_SHIFT		2
+#define SM5502_REG_INTM2_STUCK_KEY_SHIFT	3
+#define SM5502_REG_INTM2_STUCK_KEY_RCV_SHIFT	4
+#define SM5502_REG_INTM2_MHL_SHIFT		5
+#define SM5502_REG_INTM2_VBUS_DET_MASK		(0x1 << SM5502_REG_INTM2_VBUS_DET_SHIFT)
+#define SM5502_REG_INTM2_REV_ACCE_MASK		(0x1 << SM5502_REG_INTM2_REV_ACCE_SHIFT)
+#define SM5502_REG_INTM2_ADC_CHG_MASK		(0x1 << SM5502_REG_INTM2_ADC_CHG_SHIFT)
+#define SM5502_REG_INTM2_STUCK_KEY_MASK		(0x1 << SM5502_REG_INTM2_STUCK_KEY_SHIFT)
+#define SM5502_REG_INTM2_STUCK_KEY_RCV_MASK	(0x1 << SM5502_REG_INTM2_STUCK_KEY_RCV_SHIFT)
+#define SM5502_REG_INTM2_MHL_MASK		(0x1 << SM5502_REG_INTM2_MHL_SHIFT)
+
+#define SM5502_REG_ADC_SHIFT			0
+#define SM5502_REG_ADC_MASK			(0x1f << SM5502_REG_ADC_SHIFT)
+
+#define SM5502_REG_TIMING_SET1_KEY_PRESS_SHIFT	4
+#define SM5502_REG_TIMING_SET1_KEY_PRESS_MASK	(0xf << SM5502_REG_TIMING_SET1_KEY_PRESS_SHIFT)
+#define TIMING_KEY_PRESS_100MS			0x0
+#define TIMING_KEY_PRESS_200MS			0x1
+#define TIMING_KEY_PRESS_300MS			0x2
+#define TIMING_KEY_PRESS_400MS			0x3
+#define TIMING_KEY_PRESS_500MS			0x4
+#define TIMING_KEY_PRESS_600MS			0x5
+#define TIMING_KEY_PRESS_700MS			0x6
+#define TIMING_KEY_PRESS_800MS			0x7
+#define TIMING_KEY_PRESS_900MS			0x8
+#define TIMING_KEY_PRESS_1000MS			0x9
+#define SM5502_REG_TIMING_SET1_ADC_DET_SHIFT	0
+#define SM5502_REG_TIMING_SET1_ADC_DET_MASK	(0xf << SM5502_REG_TIMING_SET1_ADC_DET_SHIFT)
+#define TIMING_ADC_DET_50MS			0x0
+#define TIMING_ADC_DET_100MS			0x1
+#define TIMING_ADC_DET_150MS			0x2
+#define TIMING_ADC_DET_200MS			0x3
+#define TIMING_ADC_DET_300MS			0x4
+#define TIMING_ADC_DET_400MS			0x5
+#define TIMING_ADC_DET_500MS			0x6
+#define TIMING_ADC_DET_600MS			0x7
+#define TIMING_ADC_DET_700MS			0x8
+#define TIMING_ADC_DET_800MS			0x9
+#define TIMING_ADC_DET_900MS			0xA
+#define TIMING_ADC_DET_1000MS			0xB
+
+#define SM5502_REG_TIMING_SET2_SW_WAIT_SHIFT	4
+#define SM5502_REG_TIMING_SET2_SW_WAIT_MASK	(0xf << SM5502_REG_TIMING_SET2_SW_WAIT_SHIFT)
+#define TIMING_SW_WAIT_10MS			0x0
+#define TIMING_SW_WAIT_30MS			0x1
+#define TIMING_SW_WAIT_50MS			0x2
+#define TIMING_SW_WAIT_70MS			0x3
+#define TIMING_SW_WAIT_90MS			0x4
+#define TIMING_SW_WAIT_110MS			0x5
+#define TIMING_SW_WAIT_130MS			0x6
+#define TIMING_SW_WAIT_150MS			0x7
+#define TIMING_SW_WAIT_170MS			0x8
+#define TIMING_SW_WAIT_190MS			0x9
+#define TIMING_SW_WAIT_210MS			0xA
+#define SM5502_REG_TIMING_SET2_LONG_KEY_SHIFT	0
+#define SM5502_REG_TIMING_SET2_LONG_KEY_MASK	(0xf << SM5502_REG_TIMING_SET2_LONG_KEY_SHIFT)
+#define TIMING_LONG_KEY_300MS			0x0
+#define TIMING_LONG_KEY_400MS			0x1
+#define TIMING_LONG_KEY_500MS			0x2
+#define TIMING_LONG_KEY_600MS			0x3
+#define TIMING_LONG_KEY_700MS			0x4
+#define TIMING_LONG_KEY_800MS			0x5
+#define TIMING_LONG_KEY_900MS			0x6
+#define TIMING_LONG_KEY_1000MS			0x7
+#define TIMING_LONG_KEY_1100MS			0x8
+#define TIMING_LONG_KEY_1200MS			0x9
+#define TIMING_LONG_KEY_1300MS			0xA
+#define TIMING_LONG_KEY_1400MS			0xB
+#define TIMING_LONG_KEY_1500MS			0xC
+
+#define SM5502_REG_DEV_TYPE1_AUDIO_TYPE1_SHIFT		0
+#define SM5502_REG_DEV_TYPE1_AUDIO_TYPE2_SHIFT		1
+#define SM5502_REG_DEV_TYPE1_USB_SDP_SHIFT		2
+#define SM5502_REG_DEV_TYPE1_UART_SHIFT			3
+#define SM5502_REG_DEV_TYPE1_CAR_KIT_CHARGER_SHIFT	4
+#define SM5502_REG_DEV_TYPE1_USB_CHG_SHIFT		5
+#define SM5502_REG_DEV_TYPE1_DEDICATED_CHG_SHIFT	6
+#define SM5502_REG_DEV_TYPE1_USB_OTG_SHIFT		7
+#define SM5502_REG_DEV_TYPE1_AUDIO_TYPE1_MASK		(0x1 << SM5502_REG_DEV_TYPE1_AUDIO_TYPE1_SHIFT)
+#define SM5502_REG_DEV_TYPE1_AUDIO_TYPE1__MASK		(0x1 << SM5502_REG_DEV_TYPE1_AUDIO_TYPE2_SHIFT)
+#define SM5502_REG_DEV_TYPE1_USB_SDP_MASK		(0x1 << SM5502_REG_DEV_TYPE1_USB_SDP_SHIFT)
+#define SM5502_REG_DEV_TYPE1_UART_MASK			(0x1 << SM5502_REG_DEV_TYPE1_UART_SHIFT)
+#define SM5502_REG_DEV_TYPE1_CAR_KIT_CHARGER_MASK	(0x1 << SM5502_REG_DEV_TYPE1_CAR_KIT_CHARGER_SHIFT)
+#define SM5502_REG_DEV_TYPE1_USB_CHG_MASK		(0x1 << SM5502_REG_DEV_TYPE1_USB_CHG_SHIFT)
+#define SM5502_REG_DEV_TYPE1_DEDICATED_CHG_MASK		(0x1 << SM5502_REG_DEV_TYPE1_DEDICATED_CHG_SHIFT)
+#define SM5502_REG_DEV_TYPE1_USB_OTG_MASK		(0x1 << SM5502_REG_DEV_TYPE1_USB_OTG_SHIFT)
+
+#define SM5502_REG_DEV_TYPE2_JIG_USB_ON_SHIFT		0
+#define SM5502_REG_DEV_TYPE2_JIG_USB_OFF_SHIFT		1
+#define SM5502_REG_DEV_TYPE2_JIG_UART_ON_SHIFT		2
+#define SM5502_REG_DEV_TYPE2_JIG_UART_OFF_SHIFT		3
+#define SM5502_REG_DEV_TYPE2_PPD_SHIFT			4
+#define SM5502_REG_DEV_TYPE2_TTY_SHIFT			5
+#define SM5502_REG_DEV_TYPE2_AV_CABLE_SHIFT		6
+#define SM5502_REG_DEV_TYPE2_JIG_USB_ON_MASK		(0x1 << SM5502_REG_DEV_TYPE2_JIG_USB_ON_SHIFT)
+#define SM5502_REG_DEV_TYPE2_JIG_USB_OFF_MASK		(0x1 << SM5502_REG_DEV_TYPE2_JIG_USB_OFF_SHIFT)
+#define SM5502_REG_DEV_TYPE2_JIG_UART_ON_MASK		(0x1 << SM5502_REG_DEV_TYPE2_JIG_UART_ON_SHIFT)
+#define SM5502_REG_DEV_TYPE2_JIG_UART_OFF_MASK		(0x1 << SM5502_REG_DEV_TYPE2_JIG_UART_OFF_SHIFT)
+#define SM5502_REG_DEV_TYPE2_PPD_MASK			(0x1 << SM5502_REG_DEV_TYPE2_PPD_SHIFT)
+#define SM5502_REG_DEV_TYPE2_TTY_MASK			(0x1 << SM5502_REG_DEV_TYPE2_TTY_SHIFT)
+#define SM5502_REG_DEV_TYPE2_AV_CABLE_MASK		(0x1 << SM5502_REG_DEV_TYPE2_AV_CABLE_SHIFT)
+
+#define SM5502_REG_MANUAL_SW1_VBUSIN_SHIFT	0
+#define SM5502_REG_MANUAL_SW1_DP_SHIFT		2
+#define SM5502_REG_MANUAL_SW1_DM_SHIFT		5
+#define SM5502_REG_MANUAL_SW1_VBUSIN_MASK	(0x3 << SM5502_REG_MANUAL_SW1_VBUSIN_SHIFT)
+#define SM5502_REG_MANUAL_SW1_DP_MASK		(0x7 << SM5502_REG_MANUAL_SW1_DP_SHIFT)
+#define SM5502_REG_MANUAL_SW1_DM_MASK		(0x7 << SM5502_REG_MANUAL_SW1_DM_SHIFT)
+#define VBUSIN_SWITCH_OPEN			0x0
+#define VBUSIN_SWITCH_VBUSOUT			0x1
+#define VBUSIN_SWITCH_MIC			0x2
+#define VBUSIN_SWITCH_VBUSOUT_WITH_USB		0x3
+#define DM_DP_CON_SWITCH_OPEN			0x0
+#define DM_DP_CON_SWITCH_USB			0x1
+#define DM_DP_CON_SWITCH_AUDIO			0x2
+#define DM_DP_CON_SWITCH_UART			0x3
+#define DM_DP_SWITCH_OPEN			((DM_DP_CON_SWITCH_OPEN <<SM5502_REG_MANUAL_SW1_DP_SHIFT) \
+						| (DM_DP_CON_SWITCH_OPEN <<SM5502_REG_MANUAL_SW1_DM_SHIFT))
+#define DM_DP_SWITCH_USB			((DM_DP_CON_SWITCH_USB <<SM5502_REG_MANUAL_SW1_DP_SHIFT) \
+						| (DM_DP_CON_SWITCH_USB <<SM5502_REG_MANUAL_SW1_DM_SHIFT))
+#define DM_DP_SWITCH_AUDIO			((DM_DP_CON_SWITCH_AUDIO <<SM5502_REG_MANUAL_SW1_DP_SHIFT) \
+						| (DM_DP_CON_SWITCH_AUDIO <<SM5502_REG_MANUAL_SW1_DM_SHIFT))
+#define DM_DP_SWITCH_UART			((DM_DP_CON_SWITCH_UART <<SM5502_REG_MANUAL_SW1_DP_SHIFT) \
+						| (DM_DP_CON_SWITCH_UART <<SM5502_REG_MANUAL_SW1_DM_SHIFT))
+
+/* SM5502 Interrupts */
+enum sm5502_irq {
+	/* INT1 */
+	SM5502_IRQ_INT1_ATTACH,
+	SM5502_IRQ_INT1_DETACH,
+	SM5502_IRQ_INT1_KP,
+	SM5502_IRQ_INT1_LKP,
+	SM5502_IRQ_INT1_LKR,
+	SM5502_IRQ_INT1_OVP_EVENT,
+	SM5502_IRQ_INT1_OCP_EVENT,
+	SM5502_IRQ_INT1_OVP_OCP_DIS,
+
+	/* INT2 */
+	SM5502_IRQ_INT2_VBUS_DET,
+	SM5502_IRQ_INT2_REV_ACCE,
+	SM5502_IRQ_INT2_ADC_CHG,
+	SM5502_IRQ_INT2_STUCK_KEY,
+	SM5502_IRQ_INT2_STUCK_KEY_RCV,
+	SM5502_IRQ_INT2_MHL,
+
+	SM5502_IRQ_NUM,
+};
+
+#define SM5502_IRQ_INT1_ATTACH_MASK		BIT(0)
+#define SM5502_IRQ_INT1_DETACH_MASK		BIT(1)
+#define SM5502_IRQ_INT1_KP_MASK			BIT(2)
+#define SM5502_IRQ_INT1_LKP_MASK		BIT(3)
+#define SM5502_IRQ_INT1_LKR_MASK		BIT(4)
+#define SM5502_IRQ_INT1_OVP_EVENT_MASK		BIT(5)
+#define SM5502_IRQ_INT1_OCP_EVENT_MASK		BIT(6)
+#define SM5502_IRQ_INT1_OVP_OCP_DIS_MASK	BIT(7)
+#define SM5502_IRQ_INT2_VBUS_DET_MASK		BIT(0)
+#define SM5502_IRQ_INT2_REV_ACCE_MASK		BIT(1)
+#define SM5502_IRQ_INT2_ADC_CHG_MASK		BIT(2)
+#define SM5502_IRQ_INT2_STUCK_KEY_MASK		BIT(3)
+#define SM5502_IRQ_INT2_STUCK_KEY_RCV_MASK	BIT(4)
+#define SM5502_IRQ_INT2_MHL_MASK		BIT(5)
+
+#endif /*  __LINUX_EXTCON_SM5502_H */
diff --git a/drivers/firmware/efi/Makefile b/drivers/firmware/efi/Makefile
index d8be608a9f3b..aef6a95adef5 100644
--- a/drivers/firmware/efi/Makefile
+++ b/drivers/firmware/efi/Makefile
@@ -7,4 +7,4 @@ obj-$(CONFIG_EFI_VARS_PSTORE)		+= efi-pstore.o
 obj-$(CONFIG_UEFI_CPER)			+= cper.o
 obj-$(CONFIG_EFI_RUNTIME_MAP)		+= runtime-map.o
 obj-$(CONFIG_EFI_RUNTIME_WRAPPERS)	+= runtime-wrappers.o
-obj-$(CONFIG_EFI_STUB)			+= libstub/
+obj-$(CONFIG_EFI_ARM_STUB)		+= libstub/
diff --git a/drivers/firmware/efi/efi.c b/drivers/firmware/efi/efi.c
index 64ecbb501c50..8590099ac148 100644
--- a/drivers/firmware/efi/efi.c
+++ b/drivers/firmware/efi/efi.c
@@ -41,6 +41,28 @@ struct efi __read_mostly efi = {
 };
 EXPORT_SYMBOL(efi);
 
+static bool disable_runtime;
+static int __init setup_noefi(char *arg)
+{
+	disable_runtime = true;
+	return 0;
+}
+early_param("noefi", setup_noefi);
+
+bool efi_runtime_disabled(void)
+{
+	return disable_runtime;
+}
+
+static int __init parse_efi_cmdline(char *str)
+{
+	if (parse_option_str(str, "noruntime"))
+		disable_runtime = true;
+
+	return 0;
+}
+early_param("efi", parse_efi_cmdline);
+
 static struct kobject *efi_kobj;
 static struct kobject *efivars_kobj;
 
@@ -423,3 +445,60 @@ int __init efi_get_fdt_params(struct efi_fdt_params *params, int verbose)
 	return ret;
 }
 #endif /* CONFIG_EFI_PARAMS_FROM_FDT */
+
+static __initdata char memory_type_name[][20] = {
+	"Reserved",
+	"Loader Code",
+	"Loader Data",
+	"Boot Code",
+	"Boot Data",
+	"Runtime Code",
+	"Runtime Data",
+	"Conventional Memory",
+	"Unusable Memory",
+	"ACPI Reclaim Memory",
+	"ACPI Memory NVS",
+	"Memory Mapped I/O",
+	"MMIO Port Space",
+	"PAL Code"
+};
+
+char * __init efi_md_typeattr_format(char *buf, size_t size,
+				     const efi_memory_desc_t *md)
+{
+	char *pos;
+	int type_len;
+	u64 attr;
+
+	pos = buf;
+	if (md->type >= ARRAY_SIZE(memory_type_name))
+		type_len = snprintf(pos, size, "[type=%u", md->type);
+	else
+		type_len = snprintf(pos, size, "[%-*s",
+				    (int)(sizeof(memory_type_name[0]) - 1),
+				    memory_type_name[md->type]);
+	if (type_len >= size)
+		return buf;
+
+	pos += type_len;
+	size -= type_len;
+
+	attr = md->attribute;
+	if (attr & ~(EFI_MEMORY_UC | EFI_MEMORY_WC | EFI_MEMORY_WT |
+		     EFI_MEMORY_WB | EFI_MEMORY_UCE | EFI_MEMORY_WP |
+		     EFI_MEMORY_RP | EFI_MEMORY_XP | EFI_MEMORY_RUNTIME))
+		snprintf(pos, size, "|attr=0x%016llx]",
+			 (unsigned long long)attr);
+	else
+		snprintf(pos, size, "|%3s|%2s|%2s|%2s|%3s|%2s|%2s|%2s|%2s]",
+			 attr & EFI_MEMORY_RUNTIME ? "RUN" : "",
+			 attr & EFI_MEMORY_XP      ? "XP"  : "",
+			 attr & EFI_MEMORY_RP      ? "RP"  : "",
+			 attr & EFI_MEMORY_WP      ? "WP"  : "",
+			 attr & EFI_MEMORY_UCE     ? "UCE" : "",
+			 attr & EFI_MEMORY_WB      ? "WB"  : "",
+			 attr & EFI_MEMORY_WT      ? "WT"  : "",
+			 attr & EFI_MEMORY_WC      ? "WC"  : "",
+			 attr & EFI_MEMORY_UC      ? "UC"  : "");
+	return buf;
+}
diff --git a/drivers/firmware/efi/libstub/arm-stub.c b/drivers/firmware/efi/libstub/arm-stub.c
index 480339b6b110..75ee05964cbc 100644
--- a/drivers/firmware/efi/libstub/arm-stub.c
+++ b/drivers/firmware/efi/libstub/arm-stub.c
@@ -226,6 +226,10 @@ unsigned long __init efi_entry(void *handle, efi_system_table_t *sys_table,
 		goto fail_free_image;
 	}
 
+	status = efi_parse_options(cmdline_ptr);
+	if (status != EFI_SUCCESS)
+		pr_efi_err(sys_table, "Failed to parse EFI cmdline options\n");
+
 	/*
 	 * Unauthenticated device tree data is a security hazard, so
 	 * ignore 'dtb=' unless UEFI Secure Boot is disabled.
diff --git a/drivers/firmware/efi/libstub/efi-stub-helper.c b/drivers/firmware/efi/libstub/efi-stub-helper.c
index 32d5cca30f49..a920fec8fe88 100644
--- a/drivers/firmware/efi/libstub/efi-stub-helper.c
+++ b/drivers/firmware/efi/libstub/efi-stub-helper.c
@@ -15,8 +15,23 @@
 
 #include "efistub.h"
 
+/*
+ * Some firmware implementations have problems reading files in one go.
+ * A read chunk size of 1MB seems to work for most platforms.
+ *
+ * Unfortunately, reading files in chunks triggers *other* bugs on some
+ * platforms, so we provide a way to disable this workaround, which can
+ * be done by passing "efi=nochunk" on the EFI boot stub command line.
+ *
+ * If you experience issues with initrd images being corrupt it's worth
+ * trying efi=nochunk, but chunking is enabled by default because there
+ * are far more machines that require the workaround than those that
+ * break with it enabled.
+ */
 #define EFI_READ_CHUNK_SIZE	(1024 * 1024)
 
+static unsigned long __chunk_size = EFI_READ_CHUNK_SIZE;
+
 struct file_info {
 	efi_file_handle_t *handle;
 	u64 size;
@@ -281,6 +296,49 @@ void efi_free(efi_system_table_t *sys_table_arg, unsigned long size,
 	efi_call_early(free_pages, addr, nr_pages);
 }
 
+/*
+ * Parse the ASCII string 'cmdline' for EFI options, denoted by the efi=
+ * option, e.g. efi=nochunk.
+ *
+ * It should be noted that efi= is parsed in two very different
+ * environments, first in the early boot environment of the EFI boot
+ * stub, and subsequently during the kernel boot.
+ */
+efi_status_t efi_parse_options(char *cmdline)
+{
+	char *str;
+
+	/*
+	 * If no EFI parameters were specified on the cmdline we've got
+	 * nothing to do.
+	 */
+	str = strstr(cmdline, "efi=");
+	if (!str)
+		return EFI_SUCCESS;
+
+	/* Skip ahead to first argument */
+	str += strlen("efi=");
+
+	/*
+	 * Remember, because efi= is also used by the kernel we need to
+	 * skip over arguments we don't understand.
+	 */
+	while (*str) {
+		if (!strncmp(str, "nochunk", 7)) {
+			str += strlen("nochunk");
+			__chunk_size = -1UL;
+		}
+
+		/* Group words together, delimited by "," */
+		while (*str && *str != ',')
+			str++;
+
+		if (*str == ',')
+			str++;
+	}
+
+	return EFI_SUCCESS;
+}
 
 /*
  * Check the cmdline for a LILO-style file= arguments.
@@ -423,8 +481,8 @@ efi_status_t handle_cmdline_files(efi_system_table_t *sys_table_arg,
 			size = files[j].size;
 			while (size) {
 				unsigned long chunksize;
-				if (size > EFI_READ_CHUNK_SIZE)
-					chunksize = EFI_READ_CHUNK_SIZE;
+				if (size > __chunk_size)
+					chunksize = __chunk_size;
 				else
 					chunksize = size;
 
diff --git a/drivers/firmware/efi/libstub/fdt.c b/drivers/firmware/efi/libstub/fdt.c
index a56bb3528755..c846a9608cbd 100644
--- a/drivers/firmware/efi/libstub/fdt.c
+++ b/drivers/firmware/efi/libstub/fdt.c
@@ -22,7 +22,7 @@ efi_status_t update_fdt(efi_system_table_t *sys_table, void *orig_fdt,
 			unsigned long map_size, unsigned long desc_size,
 			u32 desc_ver)
 {
-	int node, prev;
+	int node, prev, num_rsv;
 	int status;
 	u32 fdt_val32;
 	u64 fdt_val64;
@@ -73,6 +73,14 @@ efi_status_t update_fdt(efi_system_table_t *sys_table, void *orig_fdt,
 		prev = node;
 	}
 
+	/*
+	 * Delete all memory reserve map entries. When booting via UEFI,
+	 * kernel will use the UEFI memory map to find reserved regions.
+	 */
+	num_rsv = fdt_num_mem_rsv(fdt);
+	while (num_rsv-- > 0)
+		fdt_del_mem_rsv(fdt, num_rsv);
+
 	node = fdt_subnode_offset(fdt, 0, "chosen");
 	if (node < 0) {
 		node = fdt_add_subnode(fdt, 0, "chosen");
diff --git a/drivers/firmware/efi/runtime-wrappers.c b/drivers/firmware/efi/runtime-wrappers.c
index 10daa4bbb258..228bbf910461 100644
--- a/drivers/firmware/efi/runtime-wrappers.c
+++ b/drivers/firmware/efi/runtime-wrappers.c
@@ -14,11 +14,80 @@
  * This file is released under the GPLv2.
  */
 
+#include <linux/bug.h>
 #include <linux/efi.h>
-#include <linux/spinlock.h>             /* spinlock_t */
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
 #include <asm/efi.h>
 
 /*
+ * According to section 7.1 of the UEFI spec, Runtime Services are not fully
+ * reentrant, and there are particular combinations of calls that need to be
+ * serialized. (source: UEFI Specification v2.4A)
+ *
+ * Table 31. Rules for Reentry Into Runtime Services
+ * +------------------------------------+-------------------------------+
+ * | If previous call is busy in	| Forbidden to call		|
+ * +------------------------------------+-------------------------------+
+ * | Any				| SetVirtualAddressMap()	|
+ * +------------------------------------+-------------------------------+
+ * | ConvertPointer()			| ConvertPointer()		|
+ * +------------------------------------+-------------------------------+
+ * | SetVariable()			| ResetSystem()			|
+ * | UpdateCapsule()			|				|
+ * | SetTime()				|				|
+ * | SetWakeupTime()			|				|
+ * | GetNextHighMonotonicCount()	|				|
+ * +------------------------------------+-------------------------------+
+ * | GetVariable()			| GetVariable()			|
+ * | GetNextVariableName()		| GetNextVariableName()		|
+ * | SetVariable()			| SetVariable()			|
+ * | QueryVariableInfo()		| QueryVariableInfo()		|
+ * | UpdateCapsule()			| UpdateCapsule()		|
+ * | QueryCapsuleCapabilities()		| QueryCapsuleCapabilities()	|
+ * | GetNextHighMonotonicCount()	| GetNextHighMonotonicCount()	|
+ * +------------------------------------+-------------------------------+
+ * | GetTime()				| GetTime()			|
+ * | SetTime()				| SetTime()			|
+ * | GetWakeupTime()			| GetWakeupTime()		|
+ * | SetWakeupTime()			| SetWakeupTime()		|
+ * +------------------------------------+-------------------------------+
+ *
+ * Due to the fact that the EFI pstore may write to the variable store in
+ * interrupt context, we need to use a spinlock for at least the groups that
+ * contain SetVariable() and QueryVariableInfo(). That leaves little else, as
+ * none of the remaining functions are actually ever called at runtime.
+ * So let's just use a single spinlock to serialize all Runtime Services calls.
+ */
+static DEFINE_SPINLOCK(efi_runtime_lock);
+
+/*
+ * Some runtime services calls can be reentrant under NMI, even if the table
+ * above says they are not. (source: UEFI Specification v2.4A)
+ *
+ * Table 32. Functions that may be called after Machine Check, INIT and NMI
+ * +----------------------------+------------------------------------------+
+ * | Function			| Called after Machine Check, INIT and NMI |
+ * +----------------------------+------------------------------------------+
+ * | GetTime()			| Yes, even if previously busy.		   |
+ * | GetVariable()		| Yes, even if previously busy		   |
+ * | GetNextVariableName()	| Yes, even if previously busy		   |
+ * | QueryVariableInfo()	| Yes, even if previously busy		   |
+ * | SetVariable()		| Yes, even if previously busy		   |
+ * | UpdateCapsule()		| Yes, even if previously busy		   |
+ * | QueryCapsuleCapabilities()	| Yes, even if previously busy		   |
+ * | ResetSystem()		| Yes, even if previously busy		   |
+ * +----------------------------+------------------------------------------+
+ *
+ * In order to prevent deadlocks under NMI, the wrappers for these functions
+ * may only grab the efi_runtime_lock or rtc_lock spinlocks if !efi_in_nmi().
+ * However, not all of the services listed are reachable through NMI code paths,
+ * so the the special handling as suggested by the UEFI spec is only implemented
+ * for QueryVariableInfo() and SetVariable(), as these can be reached in NMI
+ * context through efi_pstore_write().
+ */
+
+/*
  * As per commit ef68c8f87ed1 ("x86: Serialize EFI time accesses on rtc_lock"),
  * the EFI specification requires that callers of the time related runtime
  * functions serialize with other CMOS accesses in the kernel, as the EFI time
@@ -32,7 +101,9 @@ static efi_status_t virt_efi_get_time(efi_time_t *tm, efi_time_cap_t *tc)
 	efi_status_t status;
 
 	spin_lock_irqsave(&rtc_lock, flags);
+	spin_lock(&efi_runtime_lock);
 	status = efi_call_virt(get_time, tm, tc);
+	spin_unlock(&efi_runtime_lock);
 	spin_unlock_irqrestore(&rtc_lock, flags);
 	return status;
 }
@@ -43,7 +114,9 @@ static efi_status_t virt_efi_set_time(efi_time_t *tm)
 	efi_status_t status;
 
 	spin_lock_irqsave(&rtc_lock, flags);
+	spin_lock(&efi_runtime_lock);
 	status = efi_call_virt(set_time, tm);
+	spin_unlock(&efi_runtime_lock);
 	spin_unlock_irqrestore(&rtc_lock, flags);
 	return status;
 }
@@ -56,7 +129,9 @@ static efi_status_t virt_efi_get_wakeup_time(efi_bool_t *enabled,
 	efi_status_t status;
 
 	spin_lock_irqsave(&rtc_lock, flags);
+	spin_lock(&efi_runtime_lock);
 	status = efi_call_virt(get_wakeup_time, enabled, pending, tm);
+	spin_unlock(&efi_runtime_lock);
 	spin_unlock_irqrestore(&rtc_lock, flags);
 	return status;
 }
@@ -67,7 +142,9 @@ static efi_status_t virt_efi_set_wakeup_time(efi_bool_t enabled, efi_time_t *tm)
 	efi_status_t status;
 
 	spin_lock_irqsave(&rtc_lock, flags);
+	spin_lock(&efi_runtime_lock);
 	status = efi_call_virt(set_wakeup_time, enabled, tm);
+	spin_unlock(&efi_runtime_lock);
 	spin_unlock_irqrestore(&rtc_lock, flags);
 	return status;
 }
@@ -78,14 +155,27 @@ static efi_status_t virt_efi_get_variable(efi_char16_t *name,
 					  unsigned long *data_size,
 					  void *data)
 {
-	return efi_call_virt(get_variable, name, vendor, attr, data_size, data);
+	unsigned long flags;
+	efi_status_t status;
+
+	spin_lock_irqsave(&efi_runtime_lock, flags);
+	status = efi_call_virt(get_variable, name, vendor, attr, data_size,
+			       data);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
 }
 
 static efi_status_t virt_efi_get_next_variable(unsigned long *name_size,
 					       efi_char16_t *name,
 					       efi_guid_t *vendor)
 {
-	return efi_call_virt(get_next_variable, name_size, name, vendor);
+	unsigned long flags;
+	efi_status_t status;
+
+	spin_lock_irqsave(&efi_runtime_lock, flags);
+	status = efi_call_virt(get_next_variable, name_size, name, vendor);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
 }
 
 static efi_status_t virt_efi_set_variable(efi_char16_t *name,
@@ -94,24 +184,61 @@ static efi_status_t virt_efi_set_variable(efi_char16_t *name,
 					  unsigned long data_size,
 					  void *data)
 {
-	return efi_call_virt(set_variable, name, vendor, attr, data_size, data);
+	unsigned long flags;
+	efi_status_t status;
+
+	spin_lock_irqsave(&efi_runtime_lock, flags);
+	status = efi_call_virt(set_variable, name, vendor, attr, data_size,
+			       data);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
 }
 
+static efi_status_t
+virt_efi_set_variable_nonblocking(efi_char16_t *name, efi_guid_t *vendor,
+				  u32 attr, unsigned long data_size,
+				  void *data)
+{
+	unsigned long flags;
+	efi_status_t status;
+
+	if (!spin_trylock_irqsave(&efi_runtime_lock, flags))
+		return EFI_NOT_READY;
+
+	status = efi_call_virt(set_variable, name, vendor, attr, data_size,
+			       data);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
+}
+
+
 static efi_status_t virt_efi_query_variable_info(u32 attr,
 						 u64 *storage_space,
 						 u64 *remaining_space,
 						 u64 *max_variable_size)
 {
+	unsigned long flags;
+	efi_status_t status;
+
 	if (efi.runtime_version < EFI_2_00_SYSTEM_TABLE_REVISION)
 		return EFI_UNSUPPORTED;
 
-	return efi_call_virt(query_variable_info, attr, storage_space,
-			     remaining_space, max_variable_size);
+	spin_lock_irqsave(&efi_runtime_lock, flags);
+	status = efi_call_virt(query_variable_info, attr, storage_space,
+			       remaining_space, max_variable_size);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
 }
 
 static efi_status_t virt_efi_get_next_high_mono_count(u32 *count)
 {
-	return efi_call_virt(get_next_high_mono_count, count);
+	unsigned long flags;
+	efi_status_t status;
+
+	spin_lock_irqsave(&efi_runtime_lock, flags);
+	status = efi_call_virt(get_next_high_mono_count, count);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
 }
 
 static void virt_efi_reset_system(int reset_type,
@@ -119,17 +246,27 @@ static void virt_efi_reset_system(int reset_type,
 				  unsigned long data_size,
 				  efi_char16_t *data)
 {
+	unsigned long flags;
+
+	spin_lock_irqsave(&efi_runtime_lock, flags);
 	__efi_call_virt(reset_system, reset_type, status, data_size, data);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
 }
 
 static efi_status_t virt_efi_update_capsule(efi_capsule_header_t **capsules,
 					    unsigned long count,
 					    unsigned long sg_list)
 {
+	unsigned long flags;
+	efi_status_t status;
+
 	if (efi.runtime_version < EFI_2_00_SYSTEM_TABLE_REVISION)
 		return EFI_UNSUPPORTED;
 
-	return efi_call_virt(update_capsule, capsules, count, sg_list);
+	spin_lock_irqsave(&efi_runtime_lock, flags);
+	status = efi_call_virt(update_capsule, capsules, count, sg_list);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
 }
 
 static efi_status_t virt_efi_query_capsule_caps(efi_capsule_header_t **capsules,
@@ -137,11 +274,17 @@ static efi_status_t virt_efi_query_capsule_caps(efi_capsule_header_t **capsules,
 						u64 *max_size,
 						int *reset_type)
 {
+	unsigned long flags;
+	efi_status_t status;
+
 	if (efi.runtime_version < EFI_2_00_SYSTEM_TABLE_REVISION)
 		return EFI_UNSUPPORTED;
 
-	return efi_call_virt(query_capsule_caps, capsules, count, max_size,
-			     reset_type);
+	spin_lock_irqsave(&efi_runtime_lock, flags);
+	status = efi_call_virt(query_capsule_caps, capsules, count, max_size,
+			       reset_type);
+	spin_unlock_irqrestore(&efi_runtime_lock, flags);
+	return status;
 }
 
 void efi_native_runtime_setup(void)
@@ -153,6 +296,7 @@ void efi_native_runtime_setup(void)
 	efi.get_variable = virt_efi_get_variable;
 	efi.get_next_variable = virt_efi_get_next_variable;
 	efi.set_variable = virt_efi_set_variable;
+	efi.set_variable_nonblocking = virt_efi_set_variable_nonblocking;
 	efi.get_next_high_mono_count = virt_efi_get_next_high_mono_count;
 	efi.reset_system = virt_efi_reset_system;
 	efi.query_variable_info = virt_efi_query_variable_info;
diff --git a/drivers/firmware/efi/vars.c b/drivers/firmware/efi/vars.c
index f0a43646a2f3..70a0fb10517f 100644
--- a/drivers/firmware/efi/vars.c
+++ b/drivers/firmware/efi/vars.c
@@ -321,11 +321,11 @@ static unsigned long var_name_strnsize(efi_char16_t *variable_name,
  * Print a warning when duplicate EFI variables are encountered and
  * disable the sysfs workqueue since the firmware is buggy.
  */
-static void dup_variable_bug(efi_char16_t *s16, efi_guid_t *vendor_guid,
+static void dup_variable_bug(efi_char16_t *str16, efi_guid_t *vendor_guid,
 			     unsigned long len16)
 {
 	size_t i, len8 = len16 / sizeof(efi_char16_t);
-	char *s8;
+	char *str8;
 
 	/*
 	 * Disable the workqueue since the algorithm it uses for
@@ -334,16 +334,16 @@ static void dup_variable_bug(efi_char16_t *s16, efi_guid_t *vendor_guid,
 	 */
 	efivar_wq_enabled = false;
 
-	s8 = kzalloc(len8, GFP_KERNEL);
-	if (!s8)
+	str8 = kzalloc(len8, GFP_KERNEL);
+	if (!str8)
 		return;
 
 	for (i = 0; i < len8; i++)
-		s8[i] = s16[i];
+		str8[i] = str16[i];
 
 	printk(KERN_WARNING "efivars: duplicate variable: %s-%pUl\n",
-	       s8, vendor_guid);
-	kfree(s8);
+	       str8, vendor_guid);
+	kfree(str8);
 }
 
 /**
@@ -481,7 +481,7 @@ EXPORT_SYMBOL_GPL(efivar_entry_remove);
  */
 static void efivar_entry_list_del_unlock(struct efivar_entry *entry)
 {
-	WARN_ON(!spin_is_locked(&__efivars->lock));
+	lockdep_assert_held(&__efivars->lock);
 
 	list_del(&entry->list);
 	spin_unlock_irq(&__efivars->lock);
@@ -507,7 +507,7 @@ int __efivar_entry_delete(struct efivar_entry *entry)
 	const struct efivar_operations *ops = __efivars->ops;
 	efi_status_t status;
 
-	WARN_ON(!spin_is_locked(&__efivars->lock));
+	lockdep_assert_held(&__efivars->lock);
 
 	status = ops->set_variable(entry->var.VariableName,
 				   &entry->var.VendorGuid,
@@ -595,6 +595,39 @@ int efivar_entry_set(struct efivar_entry *entry, u32 attributes,
 }
 EXPORT_SYMBOL_GPL(efivar_entry_set);
 
+/*
+ * efivar_entry_set_nonblocking - call set_variable_nonblocking()
+ *
+ * This function is guaranteed to not block and is suitable for calling
+ * from crash/panic handlers.
+ *
+ * Crucially, this function will not block if it cannot acquire
+ * __efivars->lock. Instead, it returns -EBUSY.
+ */
+static int
+efivar_entry_set_nonblocking(efi_char16_t *name, efi_guid_t vendor,
+			     u32 attributes, unsigned long size, void *data)
+{
+	const struct efivar_operations *ops = __efivars->ops;
+	unsigned long flags;
+	efi_status_t status;
+
+	if (!spin_trylock_irqsave(&__efivars->lock, flags))
+		return -EBUSY;
+
+	status = check_var_size(attributes, size + ucs2_strsize(name, 1024));
+	if (status != EFI_SUCCESS) {
+		spin_unlock_irqrestore(&__efivars->lock, flags);
+		return -ENOSPC;
+	}
+
+	status = ops->set_variable_nonblocking(name, &vendor, attributes,
+					       size, data);
+
+	spin_unlock_irqrestore(&__efivars->lock, flags);
+	return efi_status_to_err(status);
+}
+
 /**
  * efivar_entry_set_safe - call set_variable() if enough space in firmware
  * @name: buffer containing the variable name
@@ -622,6 +655,20 @@ int efivar_entry_set_safe(efi_char16_t *name, efi_guid_t vendor, u32 attributes,
 	if (!ops->query_variable_store)
 		return -ENOSYS;
 
+	/*
+	 * If the EFI variable backend provides a non-blocking
+	 * ->set_variable() operation and we're in a context where we
+	 * cannot block, then we need to use it to avoid live-locks,
+	 * since the implication is that the regular ->set_variable()
+	 * will block.
+	 *
+	 * If no ->set_variable_nonblocking() is provided then
+	 * ->set_variable() is assumed to be non-blocking.
+	 */
+	if (!block && ops->set_variable_nonblocking)
+		return efivar_entry_set_nonblocking(name, vendor, attributes,
+						    size, data);
+
 	if (!block) {
 		if (!spin_trylock_irqsave(&__efivars->lock, flags))
 			return -EBUSY;
@@ -667,7 +714,7 @@ struct efivar_entry *efivar_entry_find(efi_char16_t *name, efi_guid_t guid,
 	int strsize1, strsize2;
 	bool found = false;
 
-	WARN_ON(!spin_is_locked(&__efivars->lock));
+	lockdep_assert_held(&__efivars->lock);
 
 	list_for_each_entry_safe(entry, n, head, list) {
 		strsize1 = ucs2_strsize(name, 1024);
@@ -739,7 +786,7 @@ int __efivar_entry_get(struct efivar_entry *entry, u32 *attributes,
 	const struct efivar_operations *ops = __efivars->ops;
 	efi_status_t status;
 
-	WARN_ON(!spin_is_locked(&__efivars->lock));
+	lockdep_assert_held(&__efivars->lock);
 
 	status = ops->get_variable(entry->var.VariableName,
 				   &entry->var.VendorGuid,
diff --git a/drivers/firmware/memmap.c b/drivers/firmware/memmap.c
index 79f18e6d9c4f..cc016c615c19 100644
--- a/drivers/firmware/memmap.c
+++ b/drivers/firmware/memmap.c
@@ -184,6 +184,9 @@ static int add_sysfs_fw_map_entry(struct firmware_map_entry *entry)
 	static int map_entries_nr;
 	static struct kset *mmap_kset;
 
+	if (entry->kobj.state_in_sysfs)
+		return -EEXIST;
+
 	if (!mmap_kset) {
 		mmap_kset = kset_create_and_add("memmap", NULL, firmware_kobj);
 		if (!mmap_kset)
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index 9de1515e5808..0959ca9b6b27 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -136,7 +136,6 @@ config GPIO_DWAPB
 	tristate "Synopsys DesignWare APB GPIO driver"
 	select GPIO_GENERIC
 	select GENERIC_IRQ_CHIP
-	depends on OF_GPIO
 	help
 	  Say Y or M here to build support for the Synopsys DesignWare APB
 	  GPIO block.
@@ -334,6 +333,15 @@ config GPIO_TZ1090_PDC
 	help
 	  Say yes here to support Toumaz Xenif TZ1090 PDC GPIOs.
 
+config GPIO_XGENE
+	bool "APM X-Gene GPIO controller support"
+	depends on ARM64 && OF_GPIO
+	help
+	  This driver is to support the GPIO block within the APM X-Gene SoC
+	  platform's generic flash controller. The GPIO pins are muxed with
+	  the generic flash controller's address and data pins. Say yes
+	  here to enable the GFC GPIO functionality.
+
 config GPIO_XILINX
 	bool "Xilinx GPIO support"
 	depends on PPC_OF || MICROBLAZE || ARCH_ZYNQ
@@ -681,6 +689,7 @@ config GPIO_ADP5588_IRQ
 config GPIO_ADNP
 	tristate "Avionic Design N-bit GPIO expander"
 	depends on I2C && OF_GPIO
+	select GPIOLIB_IRQCHIP
 	help
 	  This option enables support for N GPIOs found on Avionic Design
 	  I2C GPIO expanders. The register space will be extended by powers
@@ -796,7 +805,6 @@ config GPIO_MAX7301
 
 config GPIO_MCP23S08
 	tristate "Microchip MCP23xxx I/O expander"
-	depends on OF_GPIO
 	depends on (SPI_MASTER && !I2C) || I2C
 	help
 	  SPI/I2C driver for Microchip MCP23S08/MCP23S17/MCP23008/MCP23017
@@ -880,7 +888,7 @@ config GPIO_MSIC
 
 config GPIO_BCM_KONA
 	bool "Broadcom Kona GPIO"
-	depends on OF_GPIO
+	depends on OF_GPIO && (ARCH_BCM_MOBILE || COMPILE_TEST)
 	help
 	  Turn on GPIO support for Broadcom "Kona" chips.
 
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 5d024e396622..e5d346cf3b6e 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -101,6 +101,7 @@ obj-$(CONFIG_GPIO_VX855)	+= gpio-vx855.o
 obj-$(CONFIG_GPIO_WM831X)	+= gpio-wm831x.o
 obj-$(CONFIG_GPIO_WM8350)	+= gpio-wm8350.o
 obj-$(CONFIG_GPIO_WM8994)	+= gpio-wm8994.o
+obj-$(CONFIG_GPIO_XGENE)	+= gpio-xgene.o
 obj-$(CONFIG_GPIO_XILINX)	+= gpio-xilinx.o
 obj-$(CONFIG_GPIO_XTENSA)	+= gpio-xtensa.o
 obj-$(CONFIG_GPIO_ZEVIO)	+= gpio-zevio.o
diff --git a/drivers/gpio/devres.c b/drivers/gpio/devres.c
index 41b2f40578d5..954b9f6b0ef8 100644
--- a/drivers/gpio/devres.c
+++ b/drivers/gpio/devres.c
@@ -90,7 +90,7 @@ struct gpio_desc *__must_check __devm_gpiod_get_index(struct device *dev,
 	struct gpio_desc **dr;
 	struct gpio_desc *desc;
 
-	dr = devres_alloc(devm_gpiod_release, sizeof(struct gpiod_desc *),
+	dr = devres_alloc(devm_gpiod_release, sizeof(struct gpio_desc *),
 			  GFP_KERNEL);
 	if (!dr)
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/gpio/gpio-adnp.c b/drivers/gpio/gpio-adnp.c
index 416b2200d4f1..d3d0a90fe542 100644
--- a/drivers/gpio/gpio-adnp.c
+++ b/drivers/gpio/gpio-adnp.c
@@ -6,10 +6,9 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/gpio.h>
+#include <linux/gpio/driver.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
-#include <linux/irqdomain.h>
 #include <linux/module.h>
 #include <linux/of_irq.h>
 #include <linux/seq_file.h>
@@ -27,8 +26,6 @@ struct adnp {
 	unsigned int reg_shift;
 
 	struct mutex i2c_lock;
-
-	struct irq_domain *domain;
 	struct mutex irq_lock;
 
 	u8 *irq_enable;
@@ -253,6 +250,7 @@ static void adnp_gpio_dbg_show(struct seq_file *s, struct gpio_chip *chip)
 static int adnp_gpio_setup(struct adnp *adnp, unsigned int num_gpios)
 {
 	struct gpio_chip *chip = &adnp->gpio;
+	int err;
 
 	adnp->reg_shift = get_count_order(num_gpios) - 3;
 
@@ -272,6 +270,10 @@ static int adnp_gpio_setup(struct adnp *adnp, unsigned int num_gpios)
 	chip->of_node = chip->dev->of_node;
 	chip->owner = THIS_MODULE;
 
+	err = gpiochip_add(chip);
+	if (err)
+		return err;
+
 	return 0;
 }
 
@@ -326,7 +328,8 @@ static irqreturn_t adnp_irq(int irq, void *data)
 
 		for_each_set_bit(bit, &pending, 8) {
 			unsigned int child_irq;
-			child_irq = irq_find_mapping(adnp->domain, base + bit);
+			child_irq = irq_find_mapping(adnp->gpio.irqdomain,
+						     base + bit);
 			handle_nested_irq(child_irq);
 		}
 	}
@@ -334,35 +337,32 @@ static irqreturn_t adnp_irq(int irq, void *data)
 	return IRQ_HANDLED;
 }
 
-static int adnp_gpio_to_irq(struct gpio_chip *chip, unsigned offset)
-{
-	struct adnp *adnp = to_adnp(chip);
-	return irq_create_mapping(adnp->domain, offset);
-}
-
-static void adnp_irq_mask(struct irq_data *data)
+static void adnp_irq_mask(struct irq_data *d)
 {
-	struct adnp *adnp = irq_data_get_irq_chip_data(data);
-	unsigned int reg = data->hwirq >> adnp->reg_shift;
-	unsigned int pos = data->hwirq & 7;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct adnp *adnp = to_adnp(gc);
+	unsigned int reg = d->hwirq >> adnp->reg_shift;
+	unsigned int pos = d->hwirq & 7;
 
 	adnp->irq_enable[reg] &= ~BIT(pos);
 }
 
-static void adnp_irq_unmask(struct irq_data *data)
+static void adnp_irq_unmask(struct irq_data *d)
 {
-	struct adnp *adnp = irq_data_get_irq_chip_data(data);
-	unsigned int reg = data->hwirq >> adnp->reg_shift;
-	unsigned int pos = data->hwirq & 7;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct adnp *adnp = to_adnp(gc);
+	unsigned int reg = d->hwirq >> adnp->reg_shift;
+	unsigned int pos = d->hwirq & 7;
 
 	adnp->irq_enable[reg] |= BIT(pos);
 }
 
-static int adnp_irq_set_type(struct irq_data *data, unsigned int type)
+static int adnp_irq_set_type(struct irq_data *d, unsigned int type)
 {
-	struct adnp *adnp = irq_data_get_irq_chip_data(data);
-	unsigned int reg = data->hwirq >> adnp->reg_shift;
-	unsigned int pos = data->hwirq & 7;
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct adnp *adnp = to_adnp(gc);
+	unsigned int reg = d->hwirq >> adnp->reg_shift;
+	unsigned int pos = d->hwirq & 7;
 
 	if (type & IRQ_TYPE_EDGE_RISING)
 		adnp->irq_rise[reg] |= BIT(pos);
@@ -387,16 +387,18 @@ static int adnp_irq_set_type(struct irq_data *data, unsigned int type)
 	return 0;
 }
 
-static void adnp_irq_bus_lock(struct irq_data *data)
+static void adnp_irq_bus_lock(struct irq_data *d)
 {
-	struct adnp *adnp = irq_data_get_irq_chip_data(data);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct adnp *adnp = to_adnp(gc);
 
 	mutex_lock(&adnp->irq_lock);
 }
 
-static void adnp_irq_bus_unlock(struct irq_data *data)
+static void adnp_irq_bus_unlock(struct irq_data *d)
 {
-	struct adnp *adnp = irq_data_get_irq_chip_data(data);
+	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
+	struct adnp *adnp = to_adnp(gc);
 	unsigned int num_regs = 1 << adnp->reg_shift, i;
 
 	mutex_lock(&adnp->i2c_lock);
@@ -408,26 +410,6 @@ static void adnp_irq_bus_unlock(struct irq_data *data)
 	mutex_unlock(&adnp->irq_lock);
 }
 
-static int adnp_irq_reqres(struct irq_data *data)
-{
-	struct adnp *adnp = irq_data_get_irq_chip_data(data);
-
-	if (gpio_lock_as_irq(&adnp->gpio, data->hwirq)) {
-		dev_err(adnp->gpio.dev,
-			"unable to lock HW IRQ %lu for IRQ\n",
-			data->hwirq);
-		return -EINVAL;
-	}
-	return 0;
-}
-
-static void adnp_irq_relres(struct irq_data *data)
-{
-	struct adnp *adnp = irq_data_get_irq_chip_data(data);
-
-	gpio_unlock_as_irq(&adnp->gpio, data->hwirq);
-}
-
 static struct irq_chip adnp_irq_chip = {
 	.name = "gpio-adnp",
 	.irq_mask = adnp_irq_mask,
@@ -435,29 +417,6 @@ static struct irq_chip adnp_irq_chip = {
 	.irq_set_type = adnp_irq_set_type,
 	.irq_bus_lock = adnp_irq_bus_lock,
 	.irq_bus_sync_unlock = adnp_irq_bus_unlock,
-	.irq_request_resources = adnp_irq_reqres,
-	.irq_release_resources = adnp_irq_relres,
-};
-
-static int adnp_irq_map(struct irq_domain *domain, unsigned int irq,
-			irq_hw_number_t hwirq)
-{
-	irq_set_chip_data(irq, domain->host_data);
-	irq_set_chip(irq, &adnp_irq_chip);
-	irq_set_nested_thread(irq, true);
-
-#ifdef CONFIG_ARM
-	set_irq_flags(irq, IRQF_VALID);
-#else
-	irq_set_noprobe(irq);
-#endif
-
-	return 0;
-}
-
-static const struct irq_domain_ops adnp_irq_domain_ops = {
-	.map = adnp_irq_map,
-	.xlate = irq_domain_xlate_twocell,
 };
 
 static int adnp_irq_setup(struct adnp *adnp)
@@ -503,35 +462,28 @@ static int adnp_irq_setup(struct adnp *adnp)
 		adnp->irq_enable[i] = 0x00;
 	}
 
-	adnp->domain = irq_domain_add_linear(chip->of_node, chip->ngpio,
-					     &adnp_irq_domain_ops, adnp);
-
-	err = request_threaded_irq(adnp->client->irq, NULL, adnp_irq,
-				   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
-				   dev_name(chip->dev), adnp);
+	err = devm_request_threaded_irq(chip->dev, adnp->client->irq,
+					NULL, adnp_irq,
+					IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					dev_name(chip->dev), adnp);
 	if (err != 0) {
 		dev_err(chip->dev, "can't request IRQ#%d: %d\n",
 			adnp->client->irq, err);
 		return err;
 	}
 
-	chip->to_irq = adnp_gpio_to_irq;
-	return 0;
-}
-
-static void adnp_irq_teardown(struct adnp *adnp)
-{
-	unsigned int irq, i;
-
-	free_irq(adnp->client->irq, adnp);
-
-	for (i = 0; i < adnp->gpio.ngpio; i++) {
-		irq = irq_find_mapping(adnp->domain, i);
-		if (irq > 0)
-			irq_dispose_mapping(irq);
+	err = gpiochip_irqchip_add(chip,
+				   &adnp_irq_chip,
+				   0,
+				   handle_simple_irq,
+				   IRQ_TYPE_NONE);
+	if (err) {
+		dev_err(chip->dev,
+			"could not connect irqchip to gpiochip\n");
+		return err;
 	}
 
-	irq_domain_remove(adnp->domain);
+	return 0;
 }
 
 static int adnp_i2c_probe(struct i2c_client *client,
@@ -558,38 +510,25 @@ static int adnp_i2c_probe(struct i2c_client *client,
 	adnp->client = client;
 
 	err = adnp_gpio_setup(adnp, num_gpios);
-	if (err < 0)
+	if (err)
 		return err;
 
 	if (of_find_property(np, "interrupt-controller", NULL)) {
 		err = adnp_irq_setup(adnp);
-		if (err < 0)
-			goto teardown;
+		if (err)
+			return err;
 	}
 
-	err = gpiochip_add(&adnp->gpio);
-	if (err < 0)
-		goto teardown;
-
 	i2c_set_clientdata(client, adnp);
-	return 0;
 
-teardown:
-	if (of_find_property(np, "interrupt-controller", NULL))
-		adnp_irq_teardown(adnp);
-
-	return err;
+	return 0;
 }
 
 static int adnp_i2c_remove(struct i2c_client *client)
 {
 	struct adnp *adnp = i2c_get_clientdata(client);
-	struct device_node *np = client->dev.of_node;
 
 	gpiochip_remove(&adnp->gpio);
-	if (of_find_property(np, "interrupt-controller", NULL))
-		adnp_irq_teardown(adnp);
-
 	return 0;
 }
 
diff --git a/drivers/gpio/gpio-bcm-kona.c b/drivers/gpio/gpio-bcm-kona.c
index 3f6b33ce9bd4..de0801e9767a 100644
--- a/drivers/gpio/gpio-bcm-kona.c
+++ b/drivers/gpio/gpio-bcm-kona.c
@@ -496,7 +496,7 @@ static struct irq_chip bcm_gpio_irq_chip = {
 	.irq_release_resources = bcm_kona_gpio_irq_relres,
 };
 
-static struct __initconst of_device_id bcm_kona_gpio_of_match[] = {
+static struct of_device_id const bcm_kona_gpio_of_match[] = {
 	{ .compatible = "brcm,kona-gpio" },
 	{}
 };
diff --git a/drivers/gpio/gpio-bt8xx.c b/drivers/gpio/gpio-bt8xx.c
index 6557147d9331..7e4c43c18960 100644
--- a/drivers/gpio/gpio-bt8xx.c
+++ b/drivers/gpio/gpio-bt8xx.c
@@ -241,9 +241,6 @@ static void bt8xxgpio_remove(struct pci_dev *pdev)
 	bgwrite(~0x0, BT848_INT_STAT);
 	bgwrite(0x0, BT848_GPIO_OUT_EN);
 
-	iounmap(bg->mmio);
-	release_mem_region(pci_resource_start(pdev, 0),
-			   pci_resource_len(pdev, 0));
 	pci_disable_device(pdev);
 }
 
diff --git a/drivers/gpio/gpio-crystalcove.c b/drivers/gpio/gpio-crystalcove.c
index 934462f5bd22..bbfe7f508502 100644
--- a/drivers/gpio/gpio-crystalcove.c
+++ b/drivers/gpio/gpio-crystalcove.c
@@ -24,6 +24,7 @@
 #include <linux/mfd/intel_soc_pmic.h>
 
 #define CRYSTALCOVE_GPIO_NUM	16
+#define CRYSTALCOVE_VGPIO_NUM	94
 
 #define UPDATE_IRQ_TYPE		BIT(0)
 #define UPDATE_IRQ_MASK		BIT(1)
@@ -130,6 +131,9 @@ static int crystalcove_gpio_dir_in(struct gpio_chip *chip, unsigned gpio)
 {
 	struct crystalcove_gpio *cg = to_cg(chip);
 
+	if (gpio > CRYSTALCOVE_VGPIO_NUM)
+		return 0;
+
 	return regmap_write(cg->regmap, to_reg(gpio, CTRL_OUT),
 			    CTLO_INPUT_SET);
 }
@@ -139,6 +143,9 @@ static int crystalcove_gpio_dir_out(struct gpio_chip *chip, unsigned gpio,
 {
 	struct crystalcove_gpio *cg = to_cg(chip);
 
+	if (gpio > CRYSTALCOVE_VGPIO_NUM)
+		return 0;
+
 	return regmap_write(cg->regmap, to_reg(gpio, CTRL_OUT),
 			    CTLO_OUTPUT_SET | value);
 }
@@ -149,6 +156,9 @@ static int crystalcove_gpio_get(struct gpio_chip *chip, unsigned gpio)
 	int ret;
 	unsigned int val;
 
+	if (gpio > CRYSTALCOVE_VGPIO_NUM)
+		return 0;
+
 	ret = regmap_read(cg->regmap, to_reg(gpio, CTRL_IN), &val);
 	if (ret)
 		return ret;
@@ -161,6 +171,9 @@ static void crystalcove_gpio_set(struct gpio_chip *chip,
 {
 	struct crystalcove_gpio *cg = to_cg(chip);
 
+	if (gpio > CRYSTALCOVE_VGPIO_NUM)
+		return;
+
 	if (value)
 		regmap_update_bits(cg->regmap, to_reg(gpio, CTRL_OUT), 1, 1);
 	else
@@ -256,7 +269,7 @@ static irqreturn_t crystalcove_gpio_irq_handler(int irq, void *data)
 
 	pending = p0 | p1 << 8;
 
-	for (gpio = 0; gpio < cg->chip.ngpio; gpio++) {
+	for (gpio = 0; gpio < CRYSTALCOVE_GPIO_NUM; gpio++) {
 		if (pending & BIT(gpio)) {
 			virq = irq_find_mapping(cg->chip.irqdomain, gpio);
 			generic_handle_irq(virq);
@@ -273,7 +286,7 @@ static void crystalcove_gpio_dbg_show(struct seq_file *s,
 	int gpio, offset;
 	unsigned int ctlo, ctli, mirqs0, mirqsx, irq;
 
-	for (gpio = 0; gpio < cg->chip.ngpio; gpio++) {
+	for (gpio = 0; gpio < CRYSTALCOVE_GPIO_NUM; gpio++) {
 		regmap_read(cg->regmap, to_reg(gpio, CTRL_OUT), &ctlo);
 		regmap_read(cg->regmap, to_reg(gpio, CTRL_IN), &ctli);
 		regmap_read(cg->regmap, gpio < 8 ? MGPIO0IRQS0 : MGPIO1IRQS0,
@@ -320,7 +333,7 @@ static int crystalcove_gpio_probe(struct platform_device *pdev)
 	cg->chip.get = crystalcove_gpio_get;
 	cg->chip.set = crystalcove_gpio_set;
 	cg->chip.base = -1;
-	cg->chip.ngpio = CRYSTALCOVE_GPIO_NUM;
+	cg->chip.ngpio = CRYSTALCOVE_VGPIO_NUM;
 	cg->chip.can_sleep = true;
 	cg->chip.dev = dev;
 	cg->chip.dbg_show = crystalcove_gpio_dbg_show;
@@ -346,7 +359,7 @@ static int crystalcove_gpio_probe(struct platform_device *pdev)
 	return 0;
 
 out_remove_gpio:
-	WARN_ON(gpiochip_remove(&cg->chip));
+	gpiochip_remove(&cg->chip);
 	return retval;
 }
 
@@ -354,14 +367,11 @@ static int crystalcove_gpio_remove(struct platform_device *pdev)
 {
 	struct crystalcove_gpio *cg = platform_get_drvdata(pdev);
 	int irq = platform_get_irq(pdev, 0);
-	int err;
-
-	err = gpiochip_remove(&cg->chip);
 
+	gpiochip_remove(&cg->chip);
 	if (irq >= 0)
 		free_irq(irq, cg);
-
-	return err;
+	return 0;
 }
 
 static struct platform_driver crystalcove_gpio_driver = {
diff --git a/drivers/gpio/gpio-cs5535.c b/drivers/gpio/gpio-cs5535.c
index 92ec58fa9236..668127fe90ef 100644
--- a/drivers/gpio/gpio-cs5535.c
+++ b/drivers/gpio/gpio-cs5535.c
@@ -201,7 +201,8 @@ EXPORT_SYMBOL_GPL(cs5535_gpio_setup_event);
 
 static int chip_gpio_request(struct gpio_chip *c, unsigned offset)
 {
-	struct cs5535_gpio_chip *chip = (struct cs5535_gpio_chip *) c;
+	struct cs5535_gpio_chip *chip =
+		container_of(c, struct cs5535_gpio_chip, chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
@@ -241,7 +242,8 @@ static void chip_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 
 static int chip_direction_input(struct gpio_chip *c, unsigned offset)
 {
-	struct cs5535_gpio_chip *chip = (struct cs5535_gpio_chip *) c;
+	struct cs5535_gpio_chip *chip =
+		container_of(c, struct cs5535_gpio_chip, chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
@@ -254,7 +256,8 @@ static int chip_direction_input(struct gpio_chip *c, unsigned offset)
 
 static int chip_direction_output(struct gpio_chip *c, unsigned offset, int val)
 {
-	struct cs5535_gpio_chip *chip = (struct cs5535_gpio_chip *) c;
+	struct cs5535_gpio_chip *chip =
+		container_of(c, struct cs5535_gpio_chip, chip);
 	unsigned long flags;
 
 	spin_lock_irqsave(&chip->lock, flags);
diff --git a/drivers/gpio/gpio-dwapb.c b/drivers/gpio/gpio-dwapb.c
index d6618a6e2399..b43cd84b61f1 100644
--- a/drivers/gpio/gpio-dwapb.c
+++ b/drivers/gpio/gpio-dwapb.c
@@ -21,6 +21,8 @@
 #include <linux/of_irq.h>
 #include <linux/platform_device.h>
 #include <linux/spinlock.h>
+#include <linux/platform_data/gpio-dwapb.h>
+#include <linux/slab.h>
 
 #define GPIO_SWPORTA_DR		0x00
 #define GPIO_SWPORTA_DDR	0x04
@@ -35,6 +37,7 @@
 #define GPIO_INTTYPE_LEVEL	0x38
 #define GPIO_INT_POLARITY	0x3c
 #define GPIO_INTSTATUS		0x40
+#define GPIO_PORTA_DEBOUNCE	0x48
 #define GPIO_PORTA_EOI		0x4c
 #define GPIO_EXT_PORTA		0x50
 #define GPIO_EXT_PORTB		0x54
@@ -48,10 +51,28 @@
 
 struct dwapb_gpio;
 
+#ifdef CONFIG_PM_SLEEP
+/* Store GPIO context across system-wide suspend/resume transitions */
+struct dwapb_context {
+	u32 data;
+	u32 dir;
+	u32 ext;
+	u32 int_en;
+	u32 int_mask;
+	u32 int_type;
+	u32 int_pol;
+	u32 int_deb;
+};
+#endif
+
 struct dwapb_gpio_port {
 	struct bgpio_chip	bgc;
 	bool			is_registered;
 	struct dwapb_gpio	*gpio;
+#ifdef CONFIG_PM_SLEEP
+	struct dwapb_context	*ctx;
+#endif
+	unsigned int		idx;
 };
 
 struct dwapb_gpio {
@@ -62,11 +83,33 @@ struct dwapb_gpio {
 	struct irq_domain	*domain;
 };
 
+static inline struct dwapb_gpio_port *
+to_dwapb_gpio_port(struct bgpio_chip *bgc)
+{
+	return container_of(bgc, struct dwapb_gpio_port, bgc);
+}
+
+static inline u32 dwapb_read(struct dwapb_gpio *gpio, unsigned int offset)
+{
+	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	void __iomem *reg_base	= gpio->regs;
+
+	return bgc->read_reg(reg_base + offset);
+}
+
+static inline void dwapb_write(struct dwapb_gpio *gpio, unsigned int offset,
+			       u32 val)
+{
+	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	void __iomem *reg_base	= gpio->regs;
+
+	bgc->write_reg(reg_base + offset, val);
+}
+
 static int dwapb_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 {
 	struct bgpio_chip *bgc = to_bgpio_chip(gc);
-	struct dwapb_gpio_port *port = container_of(bgc, struct
-						    dwapb_gpio_port, bgc);
+	struct dwapb_gpio_port *port = to_dwapb_gpio_port(bgc);
 	struct dwapb_gpio *gpio = port->gpio;
 
 	return irq_find_mapping(gpio->domain, offset);
@@ -74,21 +117,20 @@ static int dwapb_gpio_to_irq(struct gpio_chip *gc, unsigned offset)
 
 static void dwapb_toggle_trigger(struct dwapb_gpio *gpio, unsigned int offs)
 {
-	u32 v = readl(gpio->regs + GPIO_INT_POLARITY);
+	u32 v = dwapb_read(gpio, GPIO_INT_POLARITY);
 
 	if (gpio_get_value(gpio->ports[0].bgc.gc.base + offs))
 		v &= ~BIT(offs);
 	else
 		v |= BIT(offs);
 
-	writel(v, gpio->regs + GPIO_INT_POLARITY);
+	dwapb_write(gpio, GPIO_INT_POLARITY, v);
 }
 
-static void dwapb_irq_handler(u32 irq, struct irq_desc *desc)
+static u32 dwapb_do_irq(struct dwapb_gpio *gpio)
 {
-	struct dwapb_gpio *gpio = irq_get_handler_data(irq);
-	struct irq_chip *chip = irq_desc_get_chip(desc);
 	u32 irq_status = readl_relaxed(gpio->regs + GPIO_INTSTATUS);
+	u32 ret = irq_status;
 
 	while (irq_status) {
 		int hwirq = fls(irq_status) - 1;
@@ -102,6 +144,16 @@ static void dwapb_irq_handler(u32 irq, struct irq_desc *desc)
 			dwapb_toggle_trigger(gpio, hwirq);
 	}
 
+	return ret;
+}
+
+static void dwapb_irq_handler(u32 irq, struct irq_desc *desc)
+{
+	struct dwapb_gpio *gpio = irq_get_handler_data(irq);
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+
+	dwapb_do_irq(gpio);
+
 	if (chip->irq_eoi)
 		chip->irq_eoi(irq_desc_get_irq_data(desc));
 }
@@ -115,9 +167,9 @@ static void dwapb_irq_enable(struct irq_data *d)
 	u32 val;
 
 	spin_lock_irqsave(&bgc->lock, flags);
-	val = readl(gpio->regs + GPIO_INTEN);
+	val = dwapb_read(gpio, GPIO_INTEN);
 	val |= BIT(d->hwirq);
-	writel(val, gpio->regs + GPIO_INTEN);
+	dwapb_write(gpio, GPIO_INTEN, val);
 	spin_unlock_irqrestore(&bgc->lock, flags);
 }
 
@@ -130,9 +182,9 @@ static void dwapb_irq_disable(struct irq_data *d)
 	u32 val;
 
 	spin_lock_irqsave(&bgc->lock, flags);
-	val = readl(gpio->regs + GPIO_INTEN);
+	val = dwapb_read(gpio, GPIO_INTEN);
 	val &= ~BIT(d->hwirq);
-	writel(val, gpio->regs + GPIO_INTEN);
+	dwapb_write(gpio, GPIO_INTEN, val);
 	spin_unlock_irqrestore(&bgc->lock, flags);
 }
 
@@ -172,8 +224,8 @@ static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 		return -EINVAL;
 
 	spin_lock_irqsave(&bgc->lock, flags);
-	level = readl(gpio->regs + GPIO_INTTYPE_LEVEL);
-	polarity = readl(gpio->regs + GPIO_INT_POLARITY);
+	level = dwapb_read(gpio, GPIO_INTTYPE_LEVEL);
+	polarity = dwapb_read(gpio, GPIO_INT_POLARITY);
 
 	switch (type) {
 	case IRQ_TYPE_EDGE_BOTH:
@@ -200,29 +252,55 @@ static int dwapb_irq_set_type(struct irq_data *d, u32 type)
 
 	irq_setup_alt_chip(d, type);
 
-	writel(level, gpio->regs + GPIO_INTTYPE_LEVEL);
-	writel(polarity, gpio->regs + GPIO_INT_POLARITY);
+	dwapb_write(gpio, GPIO_INTTYPE_LEVEL, level);
+	dwapb_write(gpio, GPIO_INT_POLARITY, polarity);
+	spin_unlock_irqrestore(&bgc->lock, flags);
+
+	return 0;
+}
+
+static int dwapb_gpio_set_debounce(struct gpio_chip *gc,
+				   unsigned offset, unsigned debounce)
+{
+	struct bgpio_chip *bgc = to_bgpio_chip(gc);
+	struct dwapb_gpio_port *port = to_dwapb_gpio_port(bgc);
+	struct dwapb_gpio *gpio = port->gpio;
+	unsigned long flags, val_deb;
+	unsigned long mask = bgc->pin2mask(bgc, offset);
+
+	spin_lock_irqsave(&bgc->lock, flags);
+
+	val_deb = dwapb_read(gpio, GPIO_PORTA_DEBOUNCE);
+	if (debounce)
+		dwapb_write(gpio, GPIO_PORTA_DEBOUNCE, val_deb | mask);
+	else
+		dwapb_write(gpio, GPIO_PORTA_DEBOUNCE, val_deb & ~mask);
+
 	spin_unlock_irqrestore(&bgc->lock, flags);
 
 	return 0;
 }
 
+static irqreturn_t dwapb_irq_handler_mfd(int irq, void *dev_id)
+{
+	u32 worked;
+	struct dwapb_gpio *gpio = dev_id;
+
+	worked = dwapb_do_irq(gpio);
+
+	return worked ? IRQ_HANDLED : IRQ_NONE;
+}
+
 static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
-				 struct dwapb_gpio_port *port)
+				 struct dwapb_gpio_port *port,
+				 struct dwapb_port_property *pp)
 {
 	struct gpio_chip *gc = &port->bgc.gc;
-	struct device_node *node =  gc->of_node;
-	struct irq_chip_generic	*irq_gc;
+	struct device_node *node = pp->node;
+	struct irq_chip_generic	*irq_gc = NULL;
 	unsigned int hwirq, ngpio = gc->ngpio;
 	struct irq_chip_type *ct;
-	int err, irq, i;
-
-	irq = irq_of_parse_and_map(node, 0);
-	if (!irq) {
-		dev_warn(gpio->dev, "no irq for bank %s\n",
-			port->bgc.gc.of_node->full_name);
-		return;
-	}
+	int err, i;
 
 	gpio->domain = irq_domain_add_linear(node, ngpio,
 					     &irq_generic_chip_ops, gpio);
@@ -269,8 +347,24 @@ static void dwapb_configure_irqs(struct dwapb_gpio *gpio,
 	irq_gc->chip_types[1].type = IRQ_TYPE_EDGE_BOTH;
 	irq_gc->chip_types[1].handler = handle_edge_irq;
 
-	irq_set_chained_handler(irq, dwapb_irq_handler);
-	irq_set_handler_data(irq, gpio);
+	if (!pp->irq_shared) {
+		irq_set_chained_handler(pp->irq, dwapb_irq_handler);
+		irq_set_handler_data(pp->irq, gpio);
+	} else {
+		/*
+		 * Request a shared IRQ since where MFD would have devices
+		 * using the same irq pin
+		 */
+		err = devm_request_irq(gpio->dev, pp->irq,
+				       dwapb_irq_handler_mfd,
+				       IRQF_SHARED, "gpio-dwapb-mfd", gpio);
+		if (err) {
+			dev_err(gpio->dev, "error requesting IRQ\n");
+			irq_domain_remove(gpio->domain);
+			gpio->domain = NULL;
+			return;
+		}
+	}
 
 	for (hwirq = 0 ; hwirq < ngpio ; hwirq++)
 		irq_create_mapping(gpio->domain, hwirq);
@@ -296,57 +390,53 @@ static void dwapb_irq_teardown(struct dwapb_gpio *gpio)
 }
 
 static int dwapb_gpio_add_port(struct dwapb_gpio *gpio,
-			       struct device_node *port_np,
+			       struct dwapb_port_property *pp,
 			       unsigned int offs)
 {
 	struct dwapb_gpio_port *port;
-	u32 port_idx, ngpio;
 	void __iomem *dat, *set, *dirout;
 	int err;
 
-	if (of_property_read_u32(port_np, "reg", &port_idx) ||
-		port_idx >= DWAPB_MAX_PORTS) {
-		dev_err(gpio->dev, "missing/invalid port index for %s\n",
-			port_np->full_name);
-		return -EINVAL;
-	}
-
 	port = &gpio->ports[offs];
 	port->gpio = gpio;
+	port->idx = pp->idx;
 
-	if (of_property_read_u32(port_np, "snps,nr-gpios", &ngpio)) {
-		dev_info(gpio->dev, "failed to get number of gpios for %s\n",
-			 port_np->full_name);
-		ngpio = 32;
-	}
+#ifdef CONFIG_PM_SLEEP
+	port->ctx = devm_kzalloc(gpio->dev, sizeof(*port->ctx), GFP_KERNEL);
+	if (!port->ctx)
+		return -ENOMEM;
+#endif
 
-	dat = gpio->regs + GPIO_EXT_PORTA + (port_idx * GPIO_EXT_PORT_SIZE);
-	set = gpio->regs + GPIO_SWPORTA_DR + (port_idx * GPIO_SWPORT_DR_SIZE);
+	dat = gpio->regs + GPIO_EXT_PORTA + (pp->idx * GPIO_EXT_PORT_SIZE);
+	set = gpio->regs + GPIO_SWPORTA_DR + (pp->idx * GPIO_SWPORT_DR_SIZE);
 	dirout = gpio->regs + GPIO_SWPORTA_DDR +
-		(port_idx * GPIO_SWPORT_DDR_SIZE);
+		(pp->idx * GPIO_SWPORT_DDR_SIZE);
 
 	err = bgpio_init(&port->bgc, gpio->dev, 4, dat, set, NULL, dirout,
 			 NULL, false);
 	if (err) {
 		dev_err(gpio->dev, "failed to init gpio chip for %s\n",
-			port_np->full_name);
+			pp->name);
 		return err;
 	}
 
-	port->bgc.gc.ngpio = ngpio;
-	port->bgc.gc.of_node = port_np;
+#ifdef CONFIG_OF_GPIO
+	port->bgc.gc.of_node = pp->node;
+#endif
+	port->bgc.gc.ngpio = pp->ngpio;
+	port->bgc.gc.base = pp->gpio_base;
 
-	/*
-	 * Only port A can provide interrupts in all configurations of the IP.
-	 */
-	if (port_idx == 0 &&
-	    of_property_read_bool(port_np, "interrupt-controller"))
-		dwapb_configure_irqs(gpio, port);
+	/* Only port A support debounce */
+	if (pp->idx == 0)
+		port->bgc.gc.set_debounce = dwapb_gpio_set_debounce;
+
+	if (pp->irq)
+		dwapb_configure_irqs(gpio, port, pp);
 
 	err = gpiochip_add(&port->bgc.gc);
 	if (err)
 		dev_err(gpio->dev, "failed to register gpiochip for %s\n",
-			port_np->full_name);
+			pp->name);
 	else
 		port->is_registered = true;
 
@@ -362,25 +452,116 @@ static void dwapb_gpio_unregister(struct dwapb_gpio *gpio)
 			gpiochip_remove(&gpio->ports[m].bgc.gc);
 }
 
+static struct dwapb_platform_data *
+dwapb_gpio_get_pdata_of(struct device *dev)
+{
+	struct device_node *node, *port_np;
+	struct dwapb_platform_data *pdata;
+	struct dwapb_port_property *pp;
+	int nports;
+	int i;
+
+	node = dev->of_node;
+	if (!IS_ENABLED(CONFIG_OF_GPIO) || !node)
+		return ERR_PTR(-ENODEV);
+
+	nports = of_get_child_count(node);
+	if (nports == 0)
+		return ERR_PTR(-ENODEV);
+
+	pdata = kzalloc(sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	pdata->properties = kcalloc(nports, sizeof(*pp), GFP_KERNEL);
+	if (!pdata->properties) {
+		kfree(pdata);
+		return ERR_PTR(-ENOMEM);
+	}
+
+	pdata->nports = nports;
+
+	i = 0;
+	for_each_child_of_node(node, port_np) {
+		pp = &pdata->properties[i++];
+		pp->node = port_np;
+
+		if (of_property_read_u32(port_np, "reg", &pp->idx) ||
+		    pp->idx >= DWAPB_MAX_PORTS) {
+			dev_err(dev, "missing/invalid port index for %s\n",
+				port_np->full_name);
+			kfree(pdata->properties);
+			kfree(pdata);
+			return ERR_PTR(-EINVAL);
+		}
+
+		if (of_property_read_u32(port_np, "snps,nr-gpios",
+					 &pp->ngpio)) {
+			dev_info(dev, "failed to get number of gpios for %s\n",
+				 port_np->full_name);
+			pp->ngpio = 32;
+		}
+
+		/*
+		 * Only port A can provide interrupts in all configurations of
+		 * the IP.
+		 */
+		if (pp->idx == 0 &&
+		    of_property_read_bool(port_np, "interrupt-controller")) {
+			pp->irq = irq_of_parse_and_map(port_np, 0);
+			if (!pp->irq) {
+				dev_warn(dev, "no irq for bank %s\n",
+					 port_np->full_name);
+			}
+		}
+
+		pp->irq_shared	= false;
+		pp->gpio_base	= -1;
+		pp->name	= port_np->full_name;
+	}
+
+	return pdata;
+}
+
+static inline void dwapb_free_pdata_of(struct dwapb_platform_data *pdata)
+{
+	if (!IS_ENABLED(CONFIG_OF_GPIO) || !pdata)
+		return;
+
+	kfree(pdata->properties);
+	kfree(pdata);
+}
+
 static int dwapb_gpio_probe(struct platform_device *pdev)
 {
+	unsigned int i;
 	struct resource *res;
 	struct dwapb_gpio *gpio;
-	struct device_node *np;
 	int err;
-	unsigned int offs = 0;
+	struct device *dev = &pdev->dev;
+	struct dwapb_platform_data *pdata = dev_get_platdata(dev);
+	bool is_pdata_alloc = !pdata;
+
+	if (is_pdata_alloc) {
+		pdata = dwapb_gpio_get_pdata_of(dev);
+		if (IS_ERR(pdata))
+			return PTR_ERR(pdata);
+	}
 
-	gpio = devm_kzalloc(&pdev->dev, sizeof(*gpio), GFP_KERNEL);
-	if (!gpio)
-		return -ENOMEM;
-	gpio->dev = &pdev->dev;
+	if (!pdata->nports) {
+		err = -ENODEV;
+		goto out_err;
+	}
 
-	gpio->nr_ports = of_get_child_count(pdev->dev.of_node);
-	if (!gpio->nr_ports) {
-		err = -EINVAL;
+	gpio = devm_kzalloc(&pdev->dev, sizeof(*gpio), GFP_KERNEL);
+	if (!gpio) {
+		err = -ENOMEM;
 		goto out_err;
 	}
-	gpio->ports = devm_kzalloc(&pdev->dev, gpio->nr_ports *
+	gpio->dev = &pdev->dev;
+	gpio->nr_ports = pdata->nports;
+
+	gpio->ports = devm_kcalloc(&pdev->dev, gpio->nr_ports,
 				   sizeof(*gpio->ports), GFP_KERNEL);
 	if (!gpio->ports) {
 		err = -ENOMEM;
@@ -394,20 +575,23 @@ static int dwapb_gpio_probe(struct platform_device *pdev)
 		goto out_err;
 	}
 
-	for_each_child_of_node(pdev->dev.of_node, np) {
-		err = dwapb_gpio_add_port(gpio, np, offs++);
+	for (i = 0; i < gpio->nr_ports; i++) {
+		err = dwapb_gpio_add_port(gpio, &pdata->properties[i], i);
 		if (err)
 			goto out_unregister;
 	}
 	platform_set_drvdata(pdev, gpio);
 
-	return 0;
+	goto out_err;
 
 out_unregister:
 	dwapb_gpio_unregister(gpio);
 	dwapb_irq_teardown(gpio);
 
 out_err:
+	if (is_pdata_alloc)
+		dwapb_free_pdata_of(pdata);
+
 	return err;
 }
 
@@ -427,10 +611,100 @@ static const struct of_device_id dwapb_of_match[] = {
 };
 MODULE_DEVICE_TABLE(of, dwapb_of_match);
 
+#ifdef CONFIG_PM_SLEEP
+static int dwapb_gpio_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct dwapb_gpio *gpio = platform_get_drvdata(pdev);
+	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&bgc->lock, flags);
+	for (i = 0; i < gpio->nr_ports; i++) {
+		unsigned int offset;
+		unsigned int idx = gpio->ports[i].idx;
+		struct dwapb_context *ctx = gpio->ports[i].ctx;
+
+		BUG_ON(!ctx);
+
+		offset = GPIO_SWPORTA_DDR + idx * GPIO_SWPORT_DDR_SIZE;
+		ctx->dir = dwapb_read(gpio, offset);
+
+		offset = GPIO_SWPORTA_DR + idx * GPIO_SWPORT_DR_SIZE;
+		ctx->data = dwapb_read(gpio, offset);
+
+		offset = GPIO_EXT_PORTA + idx * GPIO_EXT_PORT_SIZE;
+		ctx->ext = dwapb_read(gpio, offset);
+
+		/* Only port A can provide interrupts */
+		if (idx == 0) {
+			ctx->int_mask	= dwapb_read(gpio, GPIO_INTMASK);
+			ctx->int_en	= dwapb_read(gpio, GPIO_INTEN);
+			ctx->int_pol	= dwapb_read(gpio, GPIO_INT_POLARITY);
+			ctx->int_type	= dwapb_read(gpio, GPIO_INTTYPE_LEVEL);
+			ctx->int_deb	= dwapb_read(gpio, GPIO_PORTA_DEBOUNCE);
+
+			/* Mask out interrupts */
+			dwapb_write(gpio, GPIO_INTMASK, 0xffffffff);
+		}
+	}
+	spin_unlock_irqrestore(&bgc->lock, flags);
+
+	return 0;
+}
+
+static int dwapb_gpio_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct dwapb_gpio *gpio = platform_get_drvdata(pdev);
+	struct bgpio_chip *bgc	= &gpio->ports[0].bgc;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&bgc->lock, flags);
+	for (i = 0; i < gpio->nr_ports; i++) {
+		unsigned int offset;
+		unsigned int idx = gpio->ports[i].idx;
+		struct dwapb_context *ctx = gpio->ports[i].ctx;
+
+		BUG_ON(!ctx);
+
+		offset = GPIO_SWPORTA_DR + idx * GPIO_SWPORT_DR_SIZE;
+		dwapb_write(gpio, offset, ctx->data);
+
+		offset = GPIO_SWPORTA_DDR + idx * GPIO_SWPORT_DDR_SIZE;
+		dwapb_write(gpio, offset, ctx->dir);
+
+		offset = GPIO_EXT_PORTA + idx * GPIO_EXT_PORT_SIZE;
+		dwapb_write(gpio, offset, ctx->ext);
+
+		/* Only port A can provide interrupts */
+		if (idx == 0) {
+			dwapb_write(gpio, GPIO_INTTYPE_LEVEL, ctx->int_type);
+			dwapb_write(gpio, GPIO_INT_POLARITY, ctx->int_pol);
+			dwapb_write(gpio, GPIO_PORTA_DEBOUNCE, ctx->int_deb);
+			dwapb_write(gpio, GPIO_INTEN, ctx->int_en);
+			dwapb_write(gpio, GPIO_INTMASK, ctx->int_mask);
+
+			/* Clear out spurious interrupts */
+			dwapb_write(gpio, GPIO_PORTA_EOI, 0xffffffff);
+		}
+	}
+	spin_unlock_irqrestore(&bgc->lock, flags);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(dwapb_gpio_pm_ops, dwapb_gpio_suspend,
+			 dwapb_gpio_resume);
+
 static struct platform_driver dwapb_gpio_driver = {
 	.driver		= {
 		.name	= "gpio-dwapb",
 		.owner	= THIS_MODULE,
+		.pm	= &dwapb_gpio_pm_ops,
 		.of_match_table = of_match_ptr(dwapb_of_match),
 	},
 	.probe		= dwapb_gpio_probe,
diff --git a/drivers/gpio/gpio-ks8695.c b/drivers/gpio/gpio-ks8695.c
index 464a83de0d6a..cc09b237e88c 100644
--- a/drivers/gpio/gpio-ks8695.c
+++ b/drivers/gpio/gpio-ks8695.c
@@ -265,29 +265,27 @@ static int ks8695_gpio_show(struct seq_file *s, void *unused)
 				seq_printf(s, "EXT%i ", i);
 
 				switch ((ctrl & intmask[i]) >> (4 * i)) {
-					case IOPC_TM_LOW:
-						seq_printf(s, "(Low)");		break;
-					case IOPC_TM_HIGH:
-						seq_printf(s, "(High)");	break;
-					case IOPC_TM_RISING:
-						seq_printf(s, "(Rising)");	break;
-					case IOPC_TM_FALLING:
-						seq_printf(s, "(Falling)");	break;
-					case IOPC_TM_EDGE:
-						seq_printf(s, "(Edges)");	break;
+				case IOPC_TM_LOW:
+					seq_printf(s, "(Low)");		break;
+				case IOPC_TM_HIGH:
+					seq_printf(s, "(High)");	break;
+				case IOPC_TM_RISING:
+					seq_printf(s, "(Rising)");	break;
+				case IOPC_TM_FALLING:
+					seq_printf(s, "(Falling)");	break;
+				case IOPC_TM_EDGE:
+					seq_printf(s, "(Edges)");	break;
 				}
-			}
-			else
+			} else
 				seq_printf(s, "GPIO\t");
-		}
-		else if (i <= KS8695_GPIO_5) {
+		} else if (i <= KS8695_GPIO_5) {
 			if (ctrl & enable[i])
 				seq_printf(s, "TOUT%i\t", i - KS8695_GPIO_4);
 			else
 				seq_printf(s, "GPIO\t");
-		}
-		else
+		} else {
 			seq_printf(s, "GPIO\t");
+		}
 
 		seq_printf(s, "\t");
 
diff --git a/drivers/gpio/gpio-lynxpoint.c b/drivers/gpio/gpio-lynxpoint.c
index ff9eb911b5e4..fa945ec9ccff 100644
--- a/drivers/gpio/gpio-lynxpoint.c
+++ b/drivers/gpio/gpio-lynxpoint.c
@@ -407,9 +407,27 @@ static int lp_gpio_runtime_resume(struct device *dev)
 	return 0;
 }
 
+static int lp_gpio_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct lp_gpio *lg = platform_get_drvdata(pdev);
+	unsigned long reg;
+	int i;
+
+	/* on some hardware suspend clears input sensing, re-enable it here */
+	for (i = 0; i < lg->chip.ngpio; i++) {
+		if (gpiochip_is_requested(&lg->chip, i) != NULL) {
+			reg = lp_gpio_reg(&lg->chip, i, LP_CONFIG2);
+			outl(inl(reg) & ~GPINDIS_BIT, reg);
+		}
+	}
+	return 0;
+}
+
 static const struct dev_pm_ops lp_gpio_pm_ops = {
 	.runtime_suspend = lp_gpio_runtime_suspend,
 	.runtime_resume = lp_gpio_runtime_resume,
+	.resume = lp_gpio_resume,
 };
 
 static const struct acpi_device_id lynxpoint_gpio_acpi_match[] = {
diff --git a/drivers/gpio/gpio-mcp23s08.c b/drivers/gpio/gpio-mcp23s08.c
index 6f183d9b487e..8488e2fd307c 100644
--- a/drivers/gpio/gpio-mcp23s08.c
+++ b/drivers/gpio/gpio-mcp23s08.c
@@ -479,7 +479,7 @@ static int mcp23s08_irq_setup(struct mcp23s08 *mcp)
 
 	mutex_init(&mcp->irq_lock);
 
-	mcp->irq_domain = irq_domain_add_linear(chip->of_node, chip->ngpio,
+	mcp->irq_domain = irq_domain_add_linear(chip->dev->of_node, chip->ngpio,
 						&irq_domain_simple_ops, mcp);
 	if (!mcp->irq_domain)
 		return -ENODEV;
@@ -581,7 +581,7 @@ done:
 
 static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 			      void *data, unsigned addr, unsigned type,
-			      unsigned base, unsigned pullups)
+			      struct mcp23s08_platform_data *pdata, int cs)
 {
 	int status;
 	bool mirror = false;
@@ -635,7 +635,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 		return -EINVAL;
 	}
 
-	mcp->chip.base = base;
+	mcp->chip.base = pdata->base;
 	mcp->chip.can_sleep = true;
 	mcp->chip.dev = dev;
 	mcp->chip.owner = THIS_MODULE;
@@ -648,11 +648,9 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 	if (status < 0)
 		goto fail;
 
-	mcp->irq_controller = of_property_read_bool(mcp->chip.of_node,
-						"interrupt-controller");
+	mcp->irq_controller = pdata->irq_controller;
 	if (mcp->irq && mcp->irq_controller && (type == MCP_TYPE_017))
-		mirror = of_property_read_bool(mcp->chip.of_node,
-						"microchip,irq-mirror");
+		mirror = pdata->mirror;
 
 	if ((status & IOCON_SEQOP) || !(status & IOCON_HAEN) || mirror) {
 		/* mcp23s17 has IOCON twice, make sure they are in sync */
@@ -668,7 +666,7 @@ static int mcp23s08_probe_one(struct mcp23s08 *mcp, struct device *dev,
 	}
 
 	/* configure ~100K pullups */
-	status = mcp->ops->write(mcp, MCP_GPPU, pullups);
+	status = mcp->ops->write(mcp, MCP_GPPU, pdata->chip[cs].pullups);
 	if (status < 0)
 		goto fail;
 
@@ -768,25 +766,29 @@ MODULE_DEVICE_TABLE(of, mcp23s08_i2c_of_match);
 static int mcp230xx_probe(struct i2c_client *client,
 				    const struct i2c_device_id *id)
 {
-	struct mcp23s08_platform_data *pdata;
+	struct mcp23s08_platform_data *pdata, local_pdata;
 	struct mcp23s08 *mcp;
-	int status, base, pullups;
+	int status;
 	const struct of_device_id *match;
 
 	match = of_match_device(of_match_ptr(mcp23s08_i2c_of_match),
 					&client->dev);
-	pdata = dev_get_platdata(&client->dev);
-	if (match || !pdata) {
-		base = -1;
-		pullups = 0;
+	if (match) {
+		pdata = &local_pdata;
+		pdata->base = -1;
+		pdata->chip[0].pullups = 0;
+		pdata->irq_controller =	of_property_read_bool(
+					client->dev.of_node,
+					"interrupt-controller");
+		pdata->mirror = of_property_read_bool(client->dev.of_node,
+						      "microchip,irq-mirror");
 		client->irq = irq_of_parse_and_map(client->dev.of_node, 0);
 	} else {
-		if (!gpio_is_valid(pdata->base)) {
+		pdata = dev_get_platdata(&client->dev);
+		if (!pdata || !gpio_is_valid(pdata->base)) {
 			dev_dbg(&client->dev, "invalid platform data\n");
 			return -EINVAL;
 		}
-		base = pdata->base;
-		pullups = pdata->chip[0].pullups;
 	}
 
 	mcp = kzalloc(sizeof(*mcp), GFP_KERNEL);
@@ -795,7 +797,7 @@ static int mcp230xx_probe(struct i2c_client *client,
 
 	mcp->irq = client->irq;
 	status = mcp23s08_probe_one(mcp, &client->dev, client, client->addr,
-				    id->driver_data, base, pullups);
+				    id->driver_data, pdata, 0);
 	if (status)
 		goto fail;
 
@@ -863,14 +865,12 @@ static void mcp23s08_i2c_exit(void) { }
 
 static int mcp23s08_probe(struct spi_device *spi)
 {
-	struct mcp23s08_platform_data	*pdata;
+	struct mcp23s08_platform_data	*pdata, local_pdata;
 	unsigned			addr;
 	int				chips = 0;
 	struct mcp23s08_driver_data	*data;
 	int				status, type;
-	unsigned			base = -1,
-					ngpio = 0,
-					pullups[ARRAY_SIZE(pdata->chip)];
+	unsigned			ngpio = 0;
 	const struct			of_device_id *match;
 	u32				spi_present_mask = 0;
 
@@ -893,11 +893,18 @@ static int mcp23s08_probe(struct spi_device *spi)
 			return -ENODEV;
 		}
 
+		pdata = &local_pdata;
+		pdata->base = -1;
 		for (addr = 0; addr < ARRAY_SIZE(pdata->chip); addr++) {
-			pullups[addr] = 0;
+			pdata->chip[addr].pullups = 0;
 			if (spi_present_mask & (1 << addr))
 				chips++;
 		}
+		pdata->irq_controller =	of_property_read_bool(
+					spi->dev.of_node,
+					"interrupt-controller");
+		pdata->mirror = of_property_read_bool(spi->dev.of_node,
+						      "microchip,irq-mirror");
 	} else {
 		type = spi_get_device_id(spi)->driver_data;
 		pdata = dev_get_platdata(&spi->dev);
@@ -917,10 +924,7 @@ static int mcp23s08_probe(struct spi_device *spi)
 				return -EINVAL;
 			}
 			spi_present_mask |= 1 << addr;
-			pullups[addr] = pdata->chip[addr].pullups;
 		}
-
-		base = pdata->base;
 	}
 
 	if (!chips)
@@ -938,13 +942,13 @@ static int mcp23s08_probe(struct spi_device *spi)
 		chips--;
 		data->mcp[addr] = &data->chip[chips];
 		status = mcp23s08_probe_one(data->mcp[addr], &spi->dev, spi,
-					    0x40 | (addr << 1), type, base,
-					    pullups[addr]);
+					    0x40 | (addr << 1), type, pdata,
+					    addr);
 		if (status < 0)
 			goto fail;
 
-		if (base != -1)
-			base += (type == MCP_TYPE_S17) ? 16 : 8;
+		if (pdata->base != -1)
+			pdata->base += (type == MCP_TYPE_S17) ? 16 : 8;
 		ngpio += (type == MCP_TYPE_S17) ? 16 : 8;
 	}
 	data->ngpio = ngpio;
diff --git a/drivers/gpio/gpio-omap.c b/drivers/gpio/gpio-omap.c
index 174932165fcb..415682f69214 100644
--- a/drivers/gpio/gpio-omap.c
+++ b/drivers/gpio/gpio-omap.c
@@ -857,16 +857,6 @@ static void omap_gpio_unmask_irq(struct irq_data *d)
 	spin_unlock_irqrestore(&bank->lock, flags);
 }
 
-static struct irq_chip gpio_irq_chip = {
-	.name		= "GPIO",
-	.irq_shutdown	= omap_gpio_irq_shutdown,
-	.irq_ack	= omap_gpio_ack_irq,
-	.irq_mask	= omap_gpio_mask_irq,
-	.irq_unmask	= omap_gpio_unmask_irq,
-	.irq_set_type	= omap_gpio_irq_type,
-	.irq_set_wake	= omap_gpio_wake_enable,
-};
-
 /*---------------------------------------------------------------------*/
 
 static int omap_mpuio_suspend_noirq(struct device *dev)
@@ -1088,7 +1078,7 @@ omap_mpuio_alloc_gc(struct gpio_bank *bank, unsigned int irq_start,
 			       IRQ_NOREQUEST | IRQ_NOPROBE, 0);
 }
 
-static int omap_gpio_chip_init(struct gpio_bank *bank)
+static int omap_gpio_chip_init(struct gpio_bank *bank, struct irq_chip *irqc)
 {
 	int j;
 	static int gpio;
@@ -1137,17 +1127,17 @@ static int omap_gpio_chip_init(struct gpio_bank *bank)
 	}
 #endif
 
-	ret = gpiochip_irqchip_add(&bank->chip, &gpio_irq_chip,
+	ret = gpiochip_irqchip_add(&bank->chip, irqc,
 				   irq_base, omap_gpio_irq_handler,
 				   IRQ_TYPE_NONE);
 
 	if (ret) {
 		dev_err(bank->dev, "Couldn't add irqchip to gpiochip %d\n", ret);
-		ret = gpiochip_remove(&bank->chip);
+		gpiochip_remove(&bank->chip);
 		return -ENODEV;
 	}
 
-	gpiochip_set_chained_irqchip(&bank->chip, &gpio_irq_chip,
+	gpiochip_set_chained_irqchip(&bank->chip, irqc,
 				     bank->irq, omap_gpio_irq_handler);
 
 	for (j = 0; j < bank->width; j++) {
@@ -1172,6 +1162,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 	const struct omap_gpio_platform_data *pdata;
 	struct resource *res;
 	struct gpio_bank *bank;
+	struct irq_chip *irqc;
 	int ret;
 
 	match = of_match_device(of_match_ptr(omap_gpio_match), dev);
@@ -1186,6 +1177,18 @@ static int omap_gpio_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
+	irqc = devm_kzalloc(dev, sizeof(*irqc), GFP_KERNEL);
+	if (!irqc)
+		return -ENOMEM;
+
+	irqc->irq_shutdown = omap_gpio_irq_shutdown,
+	irqc->irq_ack = omap_gpio_ack_irq,
+	irqc->irq_mask = omap_gpio_mask_irq,
+	irqc->irq_unmask = omap_gpio_unmask_irq,
+	irqc->irq_set_type = omap_gpio_irq_type,
+	irqc->irq_set_wake = omap_gpio_wake_enable,
+	irqc->name = dev_name(&pdev->dev);
+
 	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (unlikely(!res)) {
 		dev_err(dev, "Invalid IRQ resource\n");
@@ -1241,7 +1244,7 @@ static int omap_gpio_probe(struct platform_device *pdev)
 
 	omap_gpio_mod_init(bank);
 
-	ret = omap_gpio_chip_init(bank);
+	ret = omap_gpio_chip_init(bank, irqc);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpio/gpio-pca953x.c b/drivers/gpio/gpio-pca953x.c
index f9961eea2120..e2da64abbccd 100644
--- a/drivers/gpio/gpio-pca953x.c
+++ b/drivers/gpio/gpio-pca953x.c
@@ -520,7 +520,7 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 	struct i2c_client *client = chip->client;
 	int ret, i, offset = 0;
 
-	if (irq_base != -1
+	if (client->irq && irq_base != -1
 			&& (id->driver_data & PCA_INT)) {
 
 		switch (chip->chip_type) {
@@ -586,50 +586,6 @@ static int pca953x_irq_setup(struct pca953x_chip *chip,
 }
 #endif
 
-/*
- * Handlers for alternative sources of platform_data
- */
-#ifdef CONFIG_OF_GPIO
-/*
- * Translate OpenFirmware node properties into platform_data
- * WARNING: This is DEPRECATED and will be removed eventually!
- */
-static void
-pca953x_get_alt_pdata(struct i2c_client *client, int *gpio_base, u32 *invert)
-{
-	struct device_node *node;
-	const __be32 *val;
-	int size;
-
-	*gpio_base = -1;
-
-	node = client->dev.of_node;
-	if (node == NULL)
-		return;
-
-	val = of_get_property(node, "linux,gpio-base", &size);
-	WARN(val, "%s: device-tree property 'linux,gpio-base' is deprecated!", __func__);
-	if (val) {
-		if (size != sizeof(*val))
-			dev_warn(&client->dev, "%s: wrong linux,gpio-base\n",
-				 node->full_name);
-		else
-			*gpio_base = be32_to_cpup(val);
-	}
-
-	val = of_get_property(node, "polarity", NULL);
-	WARN(val, "%s: device-tree property 'polarity' is deprecated!", __func__);
-	if (val)
-		*invert = *val;
-}
-#else
-static void
-pca953x_get_alt_pdata(struct i2c_client *client, int *gpio_base, u32 *invert)
-{
-	*gpio_base = -1;
-}
-#endif
-
 static int device_pca953x_init(struct pca953x_chip *chip, u32 invert)
 {
 	int ret;
@@ -704,12 +660,8 @@ static int pca953x_probe(struct i2c_client *client,
 		invert = pdata->invert;
 		chip->names = pdata->names;
 	} else {
-		pca953x_get_alt_pdata(client, &chip->gpio_start, &invert);
-#ifdef CONFIG_OF_GPIO
-		/* If I2C node has no interrupts property, disable GPIO interrupts */
-		if (of_find_property(client->dev.of_node, "interrupts", NULL) == NULL)
-			irq_base = -1;
-#endif
+		chip->gpio_start = -1;
+		irq_base = 0;
 	}
 
 	chip->client = client;
diff --git a/drivers/gpio/gpio-pch.c b/drivers/gpio/gpio-pch.c
index e0ac549dccb5..2d9a950ca2d4 100644
--- a/drivers/gpio/gpio-pch.c
+++ b/drivers/gpio/gpio-pch.c
@@ -171,6 +171,7 @@ static int pch_gpio_direction_input(struct gpio_chip *gpio, unsigned nr)
 	return 0;
 }
 
+#ifdef CONFIG_PM
 /*
  * Save register configuration and disable interrupts.
  */
@@ -206,6 +207,7 @@ static void pch_gpio_restore_reg_conf(struct pch_gpio *chip)
 		iowrite32(chip->pch_gpio_reg.gpio_use_sel_reg,
 			  &chip->reg->gpio_use_sel);
 }
+#endif
 
 static int pch_gpio_to_irq(struct gpio_chip *gpio, unsigned offset)
 {
diff --git a/drivers/gpio/gpio-samsung.c b/drivers/gpio/gpio-samsung.c
index 3810da47043f..7c288ba4dc87 100644
--- a/drivers/gpio/gpio-samsung.c
+++ b/drivers/gpio/gpio-samsung.c
@@ -1309,56 +1309,6 @@ samsung_gpio_pull_t s3c_gpio_getpull(unsigned int pin)
 }
 EXPORT_SYMBOL(s3c_gpio_getpull);
 
-#ifdef CONFIG_S5P_GPIO_DRVSTR
-s5p_gpio_drvstr_t s5p_gpio_get_drvstr(unsigned int pin)
-{
-	struct samsung_gpio_chip *chip = samsung_gpiolib_getchip(pin);
-	unsigned int off;
-	void __iomem *reg;
-	int shift;
-	u32 drvstr;
-
-	if (!chip)
-		return -EINVAL;
-
-	off = pin - chip->chip.base;
-	shift = off * 2;
-	reg = chip->base + 0x0C;
-
-	drvstr = __raw_readl(reg);
-	drvstr = drvstr >> shift;
-	drvstr &= 0x3;
-
-	return (__force s5p_gpio_drvstr_t)drvstr;
-}
-EXPORT_SYMBOL(s5p_gpio_get_drvstr);
-
-int s5p_gpio_set_drvstr(unsigned int pin, s5p_gpio_drvstr_t drvstr)
-{
-	struct samsung_gpio_chip *chip = samsung_gpiolib_getchip(pin);
-	unsigned int off;
-	void __iomem *reg;
-	int shift;
-	u32 tmp;
-
-	if (!chip)
-		return -EINVAL;
-
-	off = pin - chip->chip.base;
-	shift = off * 2;
-	reg = chip->base + 0x0C;
-
-	tmp = __raw_readl(reg);
-	tmp &= ~(0x3 << shift);
-	tmp |= drvstr << shift;
-
-	__raw_writel(tmp, reg);
-
-	return 0;
-}
-EXPORT_SYMBOL(s5p_gpio_set_drvstr);
-#endif	/* CONFIG_S5P_GPIO_DRVSTR */
-
 #ifdef CONFIG_PLAT_S3C24XX
 unsigned int s3c2410_modify_misccr(unsigned int clear, unsigned int change)
 {
diff --git a/drivers/gpio/gpio-stmpe.c b/drivers/gpio/gpio-stmpe.c
index 845025a57240..85c5b1974294 100644
--- a/drivers/gpio/gpio-stmpe.c
+++ b/drivers/gpio/gpio-stmpe.c
@@ -13,6 +13,7 @@
 #include <linux/interrupt.h>
 #include <linux/of.h>
 #include <linux/mfd/stmpe.h>
+#include <linux/seq_file.h>
 
 /*
  * These registers are modified under the irq bus lock and cached to avoid
@@ -127,19 +128,19 @@ static int stmpe_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	int regoffset = offset / 8;
 	int mask = 1 << (offset % 8);
 
-	if (type == IRQ_TYPE_LEVEL_LOW || type == IRQ_TYPE_LEVEL_HIGH)
+	if (type & IRQ_TYPE_LEVEL_LOW || type & IRQ_TYPE_LEVEL_HIGH)
 		return -EINVAL;
 
 	/* STMPE801 doesn't have RE and FE registers */
 	if (stmpe_gpio->stmpe->partnum == STMPE801)
 		return 0;
 
-	if (type == IRQ_TYPE_EDGE_RISING)
+	if (type & IRQ_TYPE_EDGE_RISING)
 		stmpe_gpio->regs[REG_RE][regoffset] |= mask;
 	else
 		stmpe_gpio->regs[REG_RE][regoffset] &= ~mask;
 
-	if (type == IRQ_TYPE_EDGE_FALLING)
+	if (type & IRQ_TYPE_EDGE_FALLING)
 		stmpe_gpio->regs[REG_FE][regoffset] |= mask;
 	else
 		stmpe_gpio->regs[REG_FE][regoffset] &= ~mask;
@@ -211,6 +212,77 @@ static void stmpe_gpio_irq_unmask(struct irq_data *d)
 	stmpe_gpio->regs[REG_IE][regoffset] |= mask;
 }
 
+static void stmpe_dbg_show_one(struct seq_file *s,
+			       struct gpio_chip *gc,
+			       unsigned offset, unsigned gpio)
+{
+	struct stmpe_gpio *stmpe_gpio = to_stmpe_gpio(gc);
+	struct stmpe *stmpe = stmpe_gpio->stmpe;
+	const char *label = gpiochip_is_requested(gc, offset);
+	int num_banks = DIV_ROUND_UP(stmpe->num_gpios, 8);
+	bool val = !!stmpe_gpio_get(gc, offset);
+	u8 dir_reg = stmpe->regs[STMPE_IDX_GPDR_LSB] - (offset / 8);
+	u8 mask = 1 << (offset % 8);
+	int ret;
+	u8 dir;
+
+	ret = stmpe_reg_read(stmpe, dir_reg);
+	if (ret < 0)
+		return;
+	dir = !!(ret & mask);
+
+	if (dir) {
+		seq_printf(s, " gpio-%-3d (%-20.20s) out %s",
+			   gpio, label ?: "(none)",
+			   val ? "hi" : "lo");
+	} else {
+		u8 edge_det_reg = stmpe->regs[STMPE_IDX_GPEDR_MSB] + num_banks - 1 - (offset / 8);
+		u8 rise_reg = stmpe->regs[STMPE_IDX_GPRER_LSB] - (offset / 8);
+		u8 fall_reg = stmpe->regs[STMPE_IDX_GPFER_LSB] - (offset / 8);
+		u8 irqen_reg = stmpe->regs[STMPE_IDX_IEGPIOR_LSB] - (offset / 8);
+		bool edge_det;
+		bool rise;
+		bool fall;
+		bool irqen;
+
+		ret = stmpe_reg_read(stmpe, edge_det_reg);
+		if (ret < 0)
+			return;
+		edge_det = !!(ret & mask);
+		ret = stmpe_reg_read(stmpe, rise_reg);
+		if (ret < 0)
+			return;
+		rise = !!(ret & mask);
+		ret = stmpe_reg_read(stmpe, fall_reg);
+		if (ret < 0)
+			return;
+		fall = !!(ret & mask);
+		ret = stmpe_reg_read(stmpe, irqen_reg);
+		if (ret < 0)
+			return;
+		irqen = !!(ret & mask);
+
+		seq_printf(s, " gpio-%-3d (%-20.20s) in  %s %s %s%s%s",
+			   gpio, label ?: "(none)",
+			   val ? "hi" : "lo",
+			   edge_det ? "edge-asserted" : "edge-inactive",
+			   irqen ? "IRQ-enabled" : "",
+			   rise ? " rising-edge-detection" : "",
+			   fall ? " falling-edge-detection" : "");
+	}
+}
+
+static void stmpe_dbg_show(struct seq_file *s, struct gpio_chip *gc)
+{
+	unsigned i;
+	unsigned gpio = gc->base;
+
+	for (i = 0; i < gc->ngpio; i++, gpio++) {
+		stmpe_dbg_show_one(s, gc, i, gpio);
+		seq_printf(s, "\n");
+	}
+}
+
 static struct irq_chip stmpe_gpio_irq_chip = {
 	.name			= "stmpe-gpio",
 	.irq_bus_lock		= stmpe_gpio_irq_lock,
@@ -293,6 +365,9 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 #endif
 	stmpe_gpio->chip.base = -1;
 
+	if (IS_ENABLED(CONFIG_DEBUG_FS))
+                stmpe_gpio->chip.dbg_show = stmpe_dbg_show;
+
 	if (pdata)
 		stmpe_gpio->norequest_mask = pdata->norequest_mask;
 	else if (np)
@@ -308,6 +383,12 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 	if (ret)
 		goto out_free;
 
+	ret = gpiochip_add(&stmpe_gpio->chip);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
+		goto out_disable;
+	}
+
 	if (irq > 0) {
 		ret = devm_request_threaded_irq(&pdev->dev, irq, NULL,
 				stmpe_gpio_irq, IRQF_ONESHOT,
@@ -324,14 +405,13 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 		if (ret) {
 			dev_err(&pdev->dev,
 				"could not connect irqchip to gpiochip\n");
-			return ret;
+			goto out_disable;
 		}
-	}
 
-	ret = gpiochip_add(&stmpe_gpio->chip);
-	if (ret) {
-		dev_err(&pdev->dev, "unable to add gpiochip: %d\n", ret);
-		goto out_disable;
+		gpiochip_set_chained_irqchip(&stmpe_gpio->chip,
+					     &stmpe_gpio_irq_chip,
+					     irq,
+					     NULL);
 	}
 
 	if (pdata && pdata->setup)
@@ -343,6 +423,7 @@ static int stmpe_gpio_probe(struct platform_device *pdev)
 
 out_disable:
 	stmpe_disable(stmpe, STMPE_BLOCK_GPIO);
+	gpiochip_remove(&stmpe_gpio->chip);
 out_free:
 	kfree(stmpe_gpio);
 	return ret;
diff --git a/drivers/gpio/gpio-stp-xway.c b/drivers/gpio/gpio-stp-xway.c
index 04882a911b65..7e359b7cce1b 100644
--- a/drivers/gpio/gpio-stp-xway.c
+++ b/drivers/gpio/gpio-stp-xway.c
@@ -292,7 +292,7 @@ static struct platform_driver xway_stp_driver = {
 	},
 };
 
-int __init xway_stp_init(void)
+static int __init xway_stp_init(void)
 {
 	return platform_driver_register(&xway_stp_driver);
 }
diff --git a/drivers/gpio/gpio-syscon.c b/drivers/gpio/gpio-syscon.c
index 30884fbc750d..e82fde4b6898 100644
--- a/drivers/gpio/gpio-syscon.c
+++ b/drivers/gpio/gpio-syscon.c
@@ -37,6 +37,8 @@
  * dat_bit_offset:	Offset (in bits) to the first GPIO bit.
  * dir_bit_offset:	Optional offset (in bits) to the first bit to switch
  *			GPIO direction (Used with GPIO_SYSCON_FEAT_DIR flag).
+ * set:		HW specific callback to assigns output value
+ *			for signal "offset"
  */
 
 struct syscon_gpio_data {
@@ -45,12 +47,16 @@ struct syscon_gpio_data {
 	unsigned int	bit_count;
 	unsigned int	dat_bit_offset;
 	unsigned int	dir_bit_offset;
+	void		(*set)(struct gpio_chip *chip,
+			       unsigned offset, int value);
 };
 
 struct syscon_gpio_priv {
 	struct gpio_chip		chip;
 	struct regmap			*syscon;
 	const struct syscon_gpio_data	*data;
+	u32				dreg_offset;
+	u32				dir_reg_offset;
 };
 
 static inline struct syscon_gpio_priv *to_syscon_gpio(struct gpio_chip *chip)
@@ -61,9 +67,11 @@ static inline struct syscon_gpio_priv *to_syscon_gpio(struct gpio_chip *chip)
 static int syscon_gpio_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
-	unsigned int val, offs = priv->data->dat_bit_offset + offset;
+	unsigned int val, offs;
 	int ret;
 
+	offs = priv->dreg_offset + priv->data->dat_bit_offset + offset;
+
 	ret = regmap_read(priv->syscon,
 			  (offs / SYSCON_REG_BITS) * SYSCON_REG_SIZE, &val);
 	if (ret)
@@ -75,7 +83,9 @@ static int syscon_gpio_get(struct gpio_chip *chip, unsigned offset)
 static void syscon_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
 {
 	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
-	unsigned int offs = priv->data->dat_bit_offset + offset;
+	unsigned int offs;
+
+	offs = priv->dreg_offset + priv->data->dat_bit_offset + offset;
 
 	regmap_update_bits(priv->syscon,
 			   (offs / SYSCON_REG_BITS) * SYSCON_REG_SIZE,
@@ -88,7 +98,10 @@ static int syscon_gpio_dir_in(struct gpio_chip *chip, unsigned offset)
 	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
 
 	if (priv->data->flags & GPIO_SYSCON_FEAT_DIR) {
-		unsigned int offs = priv->data->dir_bit_offset + offset;
+		unsigned int offs;
+
+		offs = priv->dir_reg_offset +
+		       priv->data->dir_bit_offset + offset;
 
 		regmap_update_bits(priv->syscon,
 				   (offs / SYSCON_REG_BITS) * SYSCON_REG_SIZE,
@@ -103,7 +116,10 @@ static int syscon_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int val)
 	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
 
 	if (priv->data->flags & GPIO_SYSCON_FEAT_DIR) {
-		unsigned int offs = priv->data->dir_bit_offset + offset;
+		unsigned int offs;
+
+		offs = priv->dir_reg_offset +
+		       priv->data->dir_bit_offset + offset;
 
 		regmap_update_bits(priv->syscon,
 				   (offs / SYSCON_REG_BITS) * SYSCON_REG_SIZE,
@@ -111,7 +127,7 @@ static int syscon_gpio_dir_out(struct gpio_chip *chip, unsigned offset, int val)
 				   BIT(offs % SYSCON_REG_BITS));
 	}
 
-	syscon_gpio_set(chip, offset, val);
+	priv->data->set(chip, offset, val);
 
 	return 0;
 }
@@ -124,11 +140,46 @@ static const struct syscon_gpio_data clps711x_mctrl_gpio = {
 	.dat_bit_offset	= 0x40 * 8 + 8,
 };
 
+#define KEYSTONE_LOCK_BIT BIT(0)
+
+static void keystone_gpio_set(struct gpio_chip *chip, unsigned offset, int val)
+{
+	struct syscon_gpio_priv *priv = to_syscon_gpio(chip);
+	unsigned int offs;
+	int ret;
+
+	offs = priv->dreg_offset + priv->data->dat_bit_offset + offset;
+
+	if (!val)
+		return;
+
+	ret = regmap_update_bits(
+			priv->syscon,
+			(offs / SYSCON_REG_BITS) * SYSCON_REG_SIZE,
+			BIT(offs % SYSCON_REG_BITS) | KEYSTONE_LOCK_BIT,
+			BIT(offs % SYSCON_REG_BITS) | KEYSTONE_LOCK_BIT);
+	if (ret < 0)
+		dev_err(chip->dev, "gpio write failed ret(%d)\n", ret);
+}
+
+static const struct syscon_gpio_data keystone_dsp_gpio = {
+	/* ARM Keystone 2 */
+	.compatible	= NULL,
+	.flags		= GPIO_SYSCON_FEAT_OUT,
+	.bit_count	= 28,
+	.dat_bit_offset	= 4,
+	.set		= keystone_gpio_set,
+};
+
 static const struct of_device_id syscon_gpio_ids[] = {
 	{
 		.compatible	= "cirrus,clps711x-mctrl-gpio",
 		.data		= &clps711x_mctrl_gpio,
 	},
+	{
+		.compatible	= "ti,keystone-dsp-gpio",
+		.data		= &keystone_dsp_gpio,
+	},
 	{ }
 };
 MODULE_DEVICE_TABLE(of, syscon_gpio_ids);
@@ -138,6 +189,8 @@ static int syscon_gpio_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	const struct of_device_id *of_id = of_match_device(syscon_gpio_ids, dev);
 	struct syscon_gpio_priv *priv;
+	struct device_node *np = dev->of_node;
+	int ret;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
@@ -145,10 +198,31 @@ static int syscon_gpio_probe(struct platform_device *pdev)
 
 	priv->data = of_id->data;
 
-	priv->syscon =
-		syscon_regmap_lookup_by_compatible(priv->data->compatible);
-	if (IS_ERR(priv->syscon))
-		return PTR_ERR(priv->syscon);
+	if (priv->data->compatible) {
+		priv->syscon = syscon_regmap_lookup_by_compatible(
+					priv->data->compatible);
+		if (IS_ERR(priv->syscon))
+			return PTR_ERR(priv->syscon);
+	} else {
+		priv->syscon =
+			syscon_regmap_lookup_by_phandle(np, "gpio,syscon-dev");
+		if (IS_ERR(priv->syscon))
+			return PTR_ERR(priv->syscon);
+
+		ret = of_property_read_u32_index(np, "gpio,syscon-dev", 1,
+						 &priv->dreg_offset);
+		if (ret)
+			dev_err(dev, "can't read the data register offset!\n");
+
+		priv->dreg_offset <<= 3;
+
+		ret = of_property_read_u32_index(np, "gpio,syscon-dev", 2,
+						 &priv->dir_reg_offset);
+		if (ret)
+			dev_err(dev, "can't read the dir register offset!\n");
+
+		priv->dir_reg_offset <<= 3;
+	}
 
 	priv->chip.dev = dev;
 	priv->chip.owner = THIS_MODULE;
@@ -159,7 +233,7 @@ static int syscon_gpio_probe(struct platform_device *pdev)
 	if (priv->data->flags & GPIO_SYSCON_FEAT_IN)
 		priv->chip.direction_input = syscon_gpio_dir_in;
 	if (priv->data->flags & GPIO_SYSCON_FEAT_OUT) {
-		priv->chip.set = syscon_gpio_set;
+		priv->chip.set = priv->data->set ? : syscon_gpio_set;
 		priv->chip.direction_output = syscon_gpio_dir_out;
 	}
 
diff --git a/drivers/gpio/gpio-tc3589x.c b/drivers/gpio/gpio-tc3589x.c
index 7324869c38e0..ae0f6466eb09 100644
--- a/drivers/gpio/gpio-tc3589x.c
+++ b/drivers/gpio/gpio-tc3589x.c
@@ -300,6 +300,11 @@ static int tc3589x_gpio_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	gpiochip_set_chained_irqchip(&tc3589x_gpio->chip,
+				     &tc3589x_gpio_irq_chip,
+				     irq,
+				     NULL);
+
 	if (pdata && pdata->setup)
 		pdata->setup(tc3589x, tc3589x_gpio->chip.base);
 
diff --git a/drivers/gpio/gpio-xgene.c b/drivers/gpio/gpio-xgene.c
new file mode 100644
index 000000000000..7d489221dc1f
--- /dev/null
+++ b/drivers/gpio/gpio-xgene.c
@@ -0,0 +1,244 @@
+/*
+ * AppliedMicro X-Gene SoC GPIO Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Author: Feng Kan <fkan@apm.com>.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/driver.h>
+#include <linux/types.h>
+#include <linux/bitops.h>
+
+#define GPIO_SET_DR_OFFSET	0x0C
+#define GPIO_DATA_OFFSET	0x14
+#define GPIO_BANK_STRIDE	0x0C
+
+#define XGENE_GPIOS_PER_BANK	16
+#define XGENE_MAX_GPIO_BANKS	3
+#define XGENE_MAX_GPIOS		(XGENE_GPIOS_PER_BANK * XGENE_MAX_GPIO_BANKS)
+
+#define GPIO_BIT_OFFSET(x)	(x % XGENE_GPIOS_PER_BANK)
+#define GPIO_BANK_OFFSET(x)	((x / XGENE_GPIOS_PER_BANK) * GPIO_BANK_STRIDE)
+
+struct xgene_gpio {
+	struct gpio_chip	chip;
+	void __iomem		*base;
+	spinlock_t		lock;
+#ifdef CONFIG_PM
+	u32			set_dr_val[XGENE_MAX_GPIO_BANKS];
+#endif
+};
+
+static inline struct xgene_gpio *to_xgene_gpio(struct gpio_chip *chip)
+{
+	return container_of(chip, struct xgene_gpio, chip);
+}
+
+static int xgene_gpio_get(struct gpio_chip *gc, unsigned int offset)
+{
+	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	unsigned long bank_offset;
+	u32 bit_offset;
+
+	bank_offset = GPIO_DATA_OFFSET + GPIO_BANK_OFFSET(offset);
+	bit_offset = GPIO_BIT_OFFSET(offset);
+	return !!(ioread32(chip->base + bank_offset) & BIT(bit_offset));
+}
+
+static void __xgene_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
+{
+	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	unsigned long bank_offset;
+	u32 setval, bit_offset;
+
+	bank_offset = GPIO_SET_DR_OFFSET + GPIO_BANK_OFFSET(offset);
+	bit_offset = GPIO_BIT_OFFSET(offset) + XGENE_GPIOS_PER_BANK;
+
+	setval = ioread32(chip->base + bank_offset);
+	if (val)
+		setval |= BIT(bit_offset);
+	else
+		setval &= ~BIT(bit_offset);
+	iowrite32(setval, chip->base + bank_offset);
+}
+
+static void xgene_gpio_set(struct gpio_chip *gc, unsigned int offset, int val)
+{
+	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	unsigned long flags;
+
+	spin_lock_irqsave(&chip->lock, flags);
+	__xgene_gpio_set(gc, offset, val);
+	spin_unlock_irqrestore(&chip->lock, flags);
+}
+
+static int xgene_gpio_dir_in(struct gpio_chip *gc, unsigned int offset)
+{
+	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	unsigned long flags, bank_offset;
+	u32 dirval, bit_offset;
+
+	bank_offset = GPIO_SET_DR_OFFSET + GPIO_BANK_OFFSET(offset);
+	bit_offset = GPIO_BIT_OFFSET(offset);
+
+	spin_lock_irqsave(&chip->lock, flags);
+
+	dirval = ioread32(chip->base + bank_offset);
+	dirval |= BIT(bit_offset);
+	iowrite32(dirval, chip->base + bank_offset);
+
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	return 0;
+}
+
+static int xgene_gpio_dir_out(struct gpio_chip *gc,
+					unsigned int offset, int val)
+{
+	struct xgene_gpio *chip = to_xgene_gpio(gc);
+	unsigned long flags, bank_offset;
+	u32 dirval, bit_offset;
+
+	bank_offset = GPIO_SET_DR_OFFSET + GPIO_BANK_OFFSET(offset);
+	bit_offset = GPIO_BIT_OFFSET(offset);
+
+	spin_lock_irqsave(&chip->lock, flags);
+
+	dirval = ioread32(chip->base + bank_offset);
+	dirval &= ~BIT(bit_offset);
+	iowrite32(dirval, chip->base + bank_offset);
+	__xgene_gpio_set(gc, offset, val);
+
+	spin_unlock_irqrestore(&chip->lock, flags);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int xgene_gpio_suspend(struct device *dev)
+{
+	struct xgene_gpio *gpio = dev_get_drvdata(dev);
+	unsigned long bank_offset;
+	unsigned int bank;
+
+	for (bank = 0; bank < XGENE_MAX_GPIO_BANKS; bank++) {
+		bank_offset = GPIO_SET_DR_OFFSET + bank * GPIO_BANK_STRIDE;
+		gpio->set_dr_val[bank] = ioread32(gpio->base + bank_offset);
+	}
+	return 0;
+}
+
+static int xgene_gpio_resume(struct device *dev)
+{
+	struct xgene_gpio *gpio = dev_get_drvdata(dev);
+	unsigned long bank_offset;
+	unsigned int bank;
+
+	for (bank = 0; bank < XGENE_MAX_GPIO_BANKS; bank++) {
+		bank_offset = GPIO_SET_DR_OFFSET + bank * GPIO_BANK_STRIDE;
+		iowrite32(gpio->set_dr_val[bank], gpio->base + bank_offset);
+	}
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(xgene_gpio_pm, xgene_gpio_suspend, xgene_gpio_resume);
+#define XGENE_GPIO_PM_OPS	(&xgene_gpio_pm)
+#else
+#define XGENE_GPIO_PM_OPS	NULL
+#endif
+
+static int xgene_gpio_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct xgene_gpio *gpio;
+	int err = 0;
+
+	gpio = devm_kzalloc(&pdev->dev, sizeof(*gpio), GFP_KERNEL);
+	if (!gpio) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	gpio->base = devm_ioremap_nocache(&pdev->dev, res->start,
+							resource_size(res));
+	if (!gpio->base) {
+		err = -ENOMEM;
+		goto err;
+	}
+
+	gpio->chip.ngpio = XGENE_MAX_GPIOS;
+
+	spin_lock_init(&gpio->lock);
+	gpio->chip.dev = &pdev->dev;
+	gpio->chip.direction_input = xgene_gpio_dir_in;
+	gpio->chip.direction_output = xgene_gpio_dir_out;
+	gpio->chip.get = xgene_gpio_get;
+	gpio->chip.set = xgene_gpio_set;
+	gpio->chip.label = dev_name(&pdev->dev);
+	gpio->chip.base = -1;
+
+	platform_set_drvdata(pdev, gpio);
+
+	err = gpiochip_add(&gpio->chip);
+	if (err) {
+		dev_err(&pdev->dev,
+			"failed to register gpiochip.\n");
+		goto err;
+	}
+
+	dev_info(&pdev->dev, "X-Gene GPIO driver registered.\n");
+	return 0;
+err:
+	dev_err(&pdev->dev, "X-Gene GPIO driver registration failed.\n");
+	return err;
+}
+
+static int xgene_gpio_remove(struct platform_device *pdev)
+{
+	struct xgene_gpio *gpio = platform_get_drvdata(pdev);
+
+	gpiochip_remove(&gpio->chip);
+	return 0;
+}
+
+static const struct of_device_id xgene_gpio_of_match[] = {
+	{ .compatible = "apm,xgene-gpio", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, xgene_gpio_of_match);
+
+static struct platform_driver xgene_gpio_driver = {
+	.driver = {
+		.name = "xgene-gpio",
+		.owner = THIS_MODULE,
+		.of_match_table = xgene_gpio_of_match,
+		.pm     = XGENE_GPIO_PM_OPS,
+	},
+	.probe = xgene_gpio_probe,
+	.remove = xgene_gpio_remove,
+};
+
+module_platform_driver(xgene_gpio_driver);
+
+MODULE_AUTHOR("Feng Kan <fkan@apm.com>");
+MODULE_DESCRIPTION("APM X-Gene GPIO driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpio/gpio-xilinx.c b/drivers/gpio/gpio-xilinx.c
index 12481867daf1..ba18b06c9a21 100644
--- a/drivers/gpio/gpio-xilinx.c
+++ b/drivers/gpio/gpio-xilinx.c
@@ -197,6 +197,7 @@ static int xgpio_of_probe(struct device_node *np)
 	struct xgpio_instance *chip;
 	int status = 0;
 	const u32 *tree_info;
+	u32 ngpio;
 
 	chip = kzalloc(sizeof(*chip), GFP_KERNEL);
 	if (!chip)
@@ -211,12 +212,13 @@ static int xgpio_of_probe(struct device_node *np)
 	/* Update GPIO direction shadow register with default value */
 	of_property_read_u32(np, "xlnx,tri-default", &chip->gpio_dir);
 
-	/* By default assume full GPIO controller */
-	chip->mmchip.gc.ngpio = 32;
-
-	/* Check device node and parent device node for device width */
-	of_property_read_u32(np, "xlnx,gpio-width",
-			      (u32 *)&chip->mmchip.gc.ngpio);
+	/*
+	 * Check device node and parent device node for device width
+	 * and assume default width of 32
+	 */
+	if (of_property_read_u32(np, "xlnx,gpio-width", &ngpio))
+		ngpio = 32;
+	chip->mmchip.gc.ngpio = (u16)ngpio;
 
 	spin_lock_init(&chip->gpio_lock);
 
@@ -258,12 +260,13 @@ static int xgpio_of_probe(struct device_node *np)
 		/* Update GPIO direction shadow register with default value */
 		of_property_read_u32(np, "xlnx,tri-default-2", &chip->gpio_dir);
 
-		/* By default assume full GPIO controller */
-		chip->mmchip.gc.ngpio = 32;
-
-		/* Check device node and parent device node for device width */
-		of_property_read_u32(np, "xlnx,gpio2-width",
-				     (u32 *)&chip->mmchip.gc.ngpio);
+		/*
+		 * Check device node and parent device node for device width
+		 * and assume default width of 32
+		 */
+		if (of_property_read_u32(np, "xlnx,gpio2-width", &ngpio))
+			ngpio = 32;
+		chip->mmchip.gc.ngpio = (u16)ngpio;
 
 		spin_lock_init(&chip->gpio_lock);
 
diff --git a/drivers/gpio/gpio-zynq.c b/drivers/gpio/gpio-zynq.c
index c3145f91fda3..74cd480bf8de 100644
--- a/drivers/gpio/gpio-zynq.c
+++ b/drivers/gpio/gpio-zynq.c
@@ -88,13 +88,17 @@
  * @chip:	instance of the gpio_chip
  * @base_addr:	base address of the GPIO device
  * @clk:	clock resource for this controller
+ * @irq:	interrupt for the GPIO device
  */
 struct zynq_gpio {
 	struct gpio_chip chip;
 	void __iomem *base_addr;
 	struct clk *clk;
+	int irq;
 };
 
+static struct irq_chip zynq_gpio_level_irqchip;
+static struct irq_chip zynq_gpio_edge_irqchip;
 /**
  * zynq_gpio_get_bank_pin - Get the bank number and pin number within that bank
  * for a given pin in the GPIO device
@@ -135,6 +139,13 @@ static inline void zynq_gpio_get_bank_pin(unsigned int pin_num,
 	}
 }
 
+static const unsigned int zynq_gpio_bank_offset[] = {
+	ZYNQ_GPIO_BANK0_PIN_MIN,
+	ZYNQ_GPIO_BANK1_PIN_MIN,
+	ZYNQ_GPIO_BANK2_PIN_MIN,
+	ZYNQ_GPIO_BANK3_PIN_MIN,
+};
+
 /**
  * zynq_gpio_get_value - Get the state of the specified pin of GPIO device
  * @chip:	gpio_chip instance to be worked on
@@ -410,29 +421,70 @@ static int zynq_gpio_set_irq_type(struct irq_data *irq_data, unsigned int type)
 		       gpio->base_addr + ZYNQ_GPIO_INTPOL_OFFSET(bank_num));
 	writel_relaxed(int_any,
 		       gpio->base_addr + ZYNQ_GPIO_INTANY_OFFSET(bank_num));
+
+	if (type & IRQ_TYPE_LEVEL_MASK) {
+		__irq_set_chip_handler_name_locked(irq_data->irq,
+			&zynq_gpio_level_irqchip, handle_fasteoi_irq, NULL);
+	} else {
+		__irq_set_chip_handler_name_locked(irq_data->irq,
+			&zynq_gpio_edge_irqchip, handle_level_irq, NULL);
+	}
+
 	return 0;
 }
 
 static int zynq_gpio_set_wake(struct irq_data *data, unsigned int on)
 {
-	if (on)
-		zynq_gpio_irq_unmask(data);
-	else
-		zynq_gpio_irq_mask(data);
+	struct zynq_gpio *gpio = irq_data_get_irq_chip_data(data);
+
+	irq_set_irq_wake(gpio->irq, on);
 
 	return 0;
 }
 
 /* irq chip descriptor */
-static struct irq_chip zynq_gpio_irqchip = {
+static struct irq_chip zynq_gpio_level_irqchip = {
 	.name		= DRIVER_NAME,
 	.irq_enable	= zynq_gpio_irq_enable,
+	.irq_eoi	= zynq_gpio_irq_ack,
 	.irq_mask	= zynq_gpio_irq_mask,
 	.irq_unmask	= zynq_gpio_irq_unmask,
 	.irq_set_type	= zynq_gpio_set_irq_type,
 	.irq_set_wake	= zynq_gpio_set_wake,
+	.flags		= IRQCHIP_EOI_THREADED | IRQCHIP_EOI_IF_HANDLED |
+			  IRQCHIP_MASK_ON_SUSPEND,
 };
 
+static struct irq_chip zynq_gpio_edge_irqchip = {
+	.name		= DRIVER_NAME,
+	.irq_enable	= zynq_gpio_irq_enable,
+	.irq_ack	= zynq_gpio_irq_ack,
+	.irq_mask	= zynq_gpio_irq_mask,
+	.irq_unmask	= zynq_gpio_irq_unmask,
+	.irq_set_type	= zynq_gpio_set_irq_type,
+	.irq_set_wake	= zynq_gpio_set_wake,
+	.flags		= IRQCHIP_MASK_ON_SUSPEND,
+};
+
+static void zynq_gpio_handle_bank_irq(struct zynq_gpio *gpio,
+				      unsigned int bank_num,
+				      unsigned long pending)
+{
+	unsigned int bank_offset = zynq_gpio_bank_offset[bank_num];
+	struct irq_domain *irqdomain = gpio->chip.irqdomain;
+	int offset;
+
+	if (!pending)
+		return;
+
+	for_each_set_bit(offset, &pending, 32) {
+		unsigned int gpio_irq;
+
+		gpio_irq = irq_find_mapping(irqdomain, offset + bank_offset);
+		generic_handle_irq(gpio_irq);
+	}
+}
+
 /**
  * zynq_gpio_irqhandler - IRQ handler for the gpio banks of a gpio device
  * @irq:	irq number of the gpio bank where interrupt has occurred
@@ -458,22 +510,7 @@ static void zynq_gpio_irqhandler(unsigned int irq, struct irq_desc *desc)
 					ZYNQ_GPIO_INTSTS_OFFSET(bank_num));
 		int_enb = readl_relaxed(gpio->base_addr +
 					ZYNQ_GPIO_INTMASK_OFFSET(bank_num));
-		int_sts &= ~int_enb;
-		if (int_sts) {
-			int offset;
-			unsigned long pending = int_sts;
-
-			for_each_set_bit(offset, &pending, 32) {
-				unsigned int gpio_irq =
-					irq_find_mapping(gpio->chip.irqdomain,
-							offset);
-				generic_handle_irq(gpio_irq);
-			}
-
-			/* clear IRQ in HW */
-			writel_relaxed(int_sts, gpio->base_addr +
-					ZYNQ_GPIO_INTSTS_OFFSET(bank_num));
-		}
+		zynq_gpio_handle_bank_irq(gpio, bank_num, int_sts & ~int_enb);
 	}
 
 	chained_irq_exit(irqchip, desc);
@@ -481,7 +518,11 @@ static void zynq_gpio_irqhandler(unsigned int irq, struct irq_desc *desc)
 
 static int __maybe_unused zynq_gpio_suspend(struct device *dev)
 {
-	if (!device_may_wakeup(dev))
+	struct platform_device *pdev = to_platform_device(dev);
+	int irq = platform_get_irq(pdev, 0);
+	struct irq_data *data = irq_get_irq_data(irq);
+
+	if (!irqd_is_wakeup_set(data))
 		return pm_runtime_force_suspend(dev);
 
 	return 0;
@@ -489,7 +530,11 @@ static int __maybe_unused zynq_gpio_suspend(struct device *dev)
 
 static int __maybe_unused zynq_gpio_resume(struct device *dev)
 {
-	if (!device_may_wakeup(dev))
+	struct platform_device *pdev = to_platform_device(dev);
+	int irq = platform_get_irq(pdev, 0);
+	struct irq_data *data = irq_get_irq_data(irq);
+
+	if (!irqd_is_wakeup_set(data))
 		return pm_runtime_force_resume(dev);
 
 	return 0;
@@ -550,7 +595,7 @@ static const struct dev_pm_ops zynq_gpio_dev_pm_ops = {
  */
 static int zynq_gpio_probe(struct platform_device *pdev)
 {
-	int ret, bank_num, irq;
+	int ret, bank_num;
 	struct zynq_gpio *gpio;
 	struct gpio_chip *chip;
 	struct resource *res;
@@ -566,10 +611,10 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 	if (IS_ERR(gpio->base_addr))
 		return PTR_ERR(gpio->base_addr);
 
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
+	gpio->irq = platform_get_irq(pdev, 0);
+	if (gpio->irq < 0) {
 		dev_err(&pdev->dev, "invalid IRQ\n");
-		return irq;
+		return gpio->irq;
 	}
 
 	/* configure the gpio chip */
@@ -610,26 +655,23 @@ static int zynq_gpio_probe(struct platform_device *pdev)
 		writel_relaxed(ZYNQ_GPIO_IXR_DISABLE_ALL, gpio->base_addr +
 			       ZYNQ_GPIO_INTDIS_OFFSET(bank_num));
 
-	ret = gpiochip_irqchip_add(chip, &zynq_gpio_irqchip, 0,
-				   handle_simple_irq, IRQ_TYPE_NONE);
+	ret = gpiochip_irqchip_add(chip, &zynq_gpio_edge_irqchip, 0,
+				   handle_level_irq, IRQ_TYPE_NONE);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to add irq chip\n");
 		goto err_rm_gpiochip;
 	}
 
-	gpiochip_set_chained_irqchip(chip, &zynq_gpio_irqchip, irq,
+	gpiochip_set_chained_irqchip(chip, &zynq_gpio_edge_irqchip, gpio->irq,
 				     zynq_gpio_irqhandler);
 
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
-	device_set_wakeup_capable(&pdev->dev, 1);
-
 	return 0;
 
 err_rm_gpiochip:
-	if (gpiochip_remove(chip))
-		dev_err(&pdev->dev, "Failed to remove gpio chip\n");
+	gpiochip_remove(chip);
 err_disable_clk:
 	clk_disable_unprepare(gpio->clk);
 
@@ -644,16 +686,10 @@ err_disable_clk:
  */
 static int zynq_gpio_remove(struct platform_device *pdev)
 {
-	int ret;
 	struct zynq_gpio *gpio = platform_get_drvdata(pdev);
 
 	pm_runtime_get_sync(&pdev->dev);
-
-	ret = gpiochip_remove(&gpio->chip);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to remove gpio chip\n");
-		return ret;
-	}
+	gpiochip_remove(&gpio->chip);
 	clk_disable_unprepare(gpio->clk);
 	device_set_wakeup_capable(&pdev->dev, 0);
 	return 0;
@@ -668,7 +704,6 @@ MODULE_DEVICE_TABLE(of, zynq_gpio_of_match);
 static struct platform_driver zynq_gpio_driver = {
 	.driver	= {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &zynq_gpio_dev_pm_ops,
 		.of_match_table = zynq_gpio_of_match,
 	},
diff --git a/drivers/gpio/gpiolib-acpi.c b/drivers/gpio/gpiolib-acpi.c
index d62eaaa75397..05c6275da224 100644
--- a/drivers/gpio/gpiolib-acpi.c
+++ b/drivers/gpio/gpiolib-acpi.c
@@ -25,10 +25,12 @@ struct acpi_gpio_event {
 	acpi_handle handle;
 	unsigned int pin;
 	unsigned int irq;
+	struct gpio_desc *desc;
 };
 
 struct acpi_gpio_connection {
 	struct list_head node;
+	unsigned int pin;
 	struct gpio_desc *desc;
 };
 
@@ -143,14 +145,8 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 	if (!handler)
 		return AE_BAD_PARAMETER;
 
-	desc = gpiochip_get_desc(chip, pin);
+	desc = gpiochip_request_own_desc(chip, pin, "ACPI:Event");
 	if (IS_ERR(desc)) {
-		dev_err(chip->dev, "Failed to get GPIO descriptor\n");
-		return AE_ERROR;
-	}
-
-	ret = gpiochip_request_own_desc(desc, "ACPI:Event");
-	if (ret) {
 		dev_err(chip->dev, "Failed to request GPIO\n");
 		return AE_ERROR;
 	}
@@ -197,6 +193,7 @@ static acpi_status acpi_gpiochip_request_interrupt(struct acpi_resource *ares,
 	event->handle = evt_handle;
 	event->irq = irq;
 	event->pin = pin;
+	event->desc = desc;
 
 	ret = request_threaded_irq(event->irq, NULL, handler, irqflags,
 				   "ACPI:Event", event);
@@ -280,7 +277,7 @@ void acpi_gpiochip_free_interrupts(struct gpio_chip *chip)
 		struct gpio_desc *desc;
 
 		free_irq(event->irq, event);
-		desc = gpiochip_get_desc(chip, event->pin);
+		desc = event->desc;
 		if (WARN_ON(IS_ERR(desc)))
 			continue;
 		gpio_unlock_as_irq(chip, event->pin);
@@ -377,8 +374,10 @@ acpi_gpio_adr_space_handler(u32 function, acpi_physical_address address,
 	struct gpio_chip *chip = achip->chip;
 	struct acpi_resource_gpio *agpio;
 	struct acpi_resource *ares;
+	int pin_index = (int)address;
 	acpi_status status;
 	bool pull_up;
+	int length;
 	int i;
 
 	status = acpi_buffer_to_resource(achip->conn_info.connection,
@@ -400,32 +399,27 @@ acpi_gpio_adr_space_handler(u32 function, acpi_physical_address address,
 		return AE_BAD_PARAMETER;
 	}
 
-	for (i = 0; i < agpio->pin_table_length; i++) {
+	length = min(agpio->pin_table_length, (u16)(pin_index + bits));
+	for (i = pin_index; i < length; ++i) {
 		unsigned pin = agpio->pin_table[i];
 		struct acpi_gpio_connection *conn;
 		struct gpio_desc *desc;
 		bool found;
 
-		desc = gpiochip_get_desc(chip, pin);
-		if (IS_ERR(desc)) {
-			status = AE_ERROR;
-			goto out;
-		}
-
 		mutex_lock(&achip->conn_lock);
 
 		found = false;
 		list_for_each_entry(conn, &achip->conns, node) {
-			if (conn->desc == desc) {
+			if (conn->pin == pin) {
 				found = true;
+				desc = conn->desc;
 				break;
 			}
 		}
 		if (!found) {
-			int ret;
-
-			ret = gpiochip_request_own_desc(desc, "ACPI:OpRegion");
-			if (ret) {
+			desc = gpiochip_request_own_desc(chip, pin,
+							 "ACPI:OpRegion");
+			if (IS_ERR(desc)) {
 				status = AE_ERROR;
 				mutex_unlock(&achip->conn_lock);
 				goto out;
@@ -462,6 +456,7 @@ acpi_gpio_adr_space_handler(u32 function, acpi_physical_address address,
 				goto out;
 			}
 
+			conn->pin = pin;
 			conn->desc = desc;
 			list_add_tail(&conn->node, &achip->conns);
 		}
diff --git a/drivers/gpio/gpiolib-of.c b/drivers/gpio/gpiolib-of.c
index 7cfdc2278905..604dbe60bdee 100644
--- a/drivers/gpio/gpiolib-of.c
+++ b/drivers/gpio/gpiolib-of.c
@@ -307,7 +307,5 @@ void of_gpiochip_add(struct gpio_chip *chip)
 void of_gpiochip_remove(struct gpio_chip *chip)
 {
 	gpiochip_remove_pin_ranges(chip);
-
-	if (chip->of_node)
-		of_node_put(chip->of_node);
+	of_node_put(chip->of_node);
 }
diff --git a/drivers/gpio/gpiolib.c b/drivers/gpio/gpiolib.c
index 15cc0bb65dda..e8e98ca25ec7 100644
--- a/drivers/gpio/gpiolib.c
+++ b/drivers/gpio/gpiolib.c
@@ -308,10 +308,9 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip);
  *
  * A gpio_chip with any GPIOs still requested may not be removed.
  */
-int gpiochip_remove(struct gpio_chip *chip)
+void gpiochip_remove(struct gpio_chip *chip)
 {
 	unsigned long	flags;
-	int		status = 0;
 	unsigned	id;
 
 	acpi_gpiochip_remove(chip);
@@ -323,24 +322,15 @@ int gpiochip_remove(struct gpio_chip *chip)
 	of_gpiochip_remove(chip);
 
 	for (id = 0; id < chip->ngpio; id++) {
-		if (test_bit(FLAG_REQUESTED, &chip->desc[id].flags)) {
-			status = -EBUSY;
-			break;
-		}
-	}
-	if (status == 0) {
-		for (id = 0; id < chip->ngpio; id++)
-			chip->desc[id].chip = NULL;
-
-		list_del(&chip->list);
+		if (test_bit(FLAG_REQUESTED, &chip->desc[id].flags))
+			dev_crit(chip->dev, "REMOVING GPIOCHIP WITH GPIOS STILL REQUESTED\n");
 	}
+	for (id = 0; id < chip->ngpio; id++)
+		chip->desc[id].chip = NULL;
 
+	list_del(&chip->list);
 	spin_unlock_irqrestore(&gpio_lock, flags);
-
-	if (status == 0)
-		gpiochip_unexport(chip);
-
-	return status;
+	gpiochip_unexport(chip);
 }
 EXPORT_SYMBOL_GPL(gpiochip_remove);
 
@@ -395,30 +385,47 @@ static struct gpio_chip *find_chip_by_name(const char *name)
  */
 
 /**
- * gpiochip_add_chained_irqchip() - adds a chained irqchip to a gpiochip
- * @gpiochip: the gpiochip to add the irqchip to
- * @irqchip: the irqchip to add to the gpiochip
+ * gpiochip_set_chained_irqchip() - sets a chained irqchip to a gpiochip
+ * @gpiochip: the gpiochip to set the irqchip chain to
+ * @irqchip: the irqchip to chain to the gpiochip
  * @parent_irq: the irq number corresponding to the parent IRQ for this
  * chained irqchip
  * @parent_handler: the parent interrupt handler for the accumulated IRQ
- * coming out of the gpiochip
+ * coming out of the gpiochip. If the interrupt is nested rather than
+ * cascaded, pass NULL in this handler argument
  */
 void gpiochip_set_chained_irqchip(struct gpio_chip *gpiochip,
 				  struct irq_chip *irqchip,
 				  int parent_irq,
 				  irq_flow_handler_t parent_handler)
 {
-	if (gpiochip->can_sleep) {
-		chip_err(gpiochip, "you cannot have chained interrupts on a chip that may sleep\n");
+	unsigned int offset;
+
+	if (!gpiochip->irqdomain) {
+		chip_err(gpiochip, "called %s before setting up irqchip\n",
+			 __func__);
 		return;
 	}
 
-	irq_set_chained_handler(parent_irq, parent_handler);
-	/*
-	 * The parent irqchip is already using the chip_data for this
-	 * irqchip, so our callbacks simply use the handler_data.
-	 */
-	irq_set_handler_data(parent_irq, gpiochip);
+	if (parent_handler) {
+		if (gpiochip->can_sleep) {
+			chip_err(gpiochip,
+				 "you cannot have chained interrupts on a "
+				 "chip that may sleep\n");
+			return;
+		}
+		/*
+		 * The parent irqchip is already using the chip_data for this
+		 * irqchip, so our callbacks simply use the handler_data.
+		 */
+		irq_set_handler_data(parent_irq, gpiochip);
+		irq_set_chained_handler(parent_irq, parent_handler);
+	}
+
+	/* Set the parent IRQ for all affected IRQs */
+	for (offset = 0; offset < gpiochip->ngpio; offset++)
+		irq_set_parent(irq_find_mapping(gpiochip->irqdomain, offset),
+			       parent_irq);
 }
 EXPORT_SYMBOL_GPL(gpiochip_set_chained_irqchip);
 
@@ -447,7 +454,7 @@ static int gpiochip_irq_map(struct irq_domain *d, unsigned int irq,
 	irq_set_lockdep_class(irq, &gpiochip_irq_lock_class);
 	irq_set_chip_and_handler(irq, chip->irqchip, chip->irq_handler);
 	/* Chips that can sleep need nested thread handlers */
-	if (chip->can_sleep)
+	if (chip->can_sleep && !chip->irq_not_threaded)
 		irq_set_nested_thread(irq, 1);
 #ifdef CONFIG_ARM
 	set_irq_flags(irq, IRQF_VALID);
@@ -524,7 +531,8 @@ static void gpiochip_irqchip_remove(struct gpio_chip *gpiochip)
 	/* Remove all IRQ mappings and delete the domain */
 	if (gpiochip->irqdomain) {
 		for (offset = 0; offset < gpiochip->ngpio; offset++)
-			irq_dispose_mapping(gpiochip->irq_base + offset);
+			irq_dispose_mapping(
+				irq_find_mapping(gpiochip->irqdomain, offset));
 		irq_domain_remove(gpiochip->irqdomain);
 	}
 
@@ -895,12 +903,22 @@ EXPORT_SYMBOL_GPL(gpiochip_is_requested);
  * allows the GPIO chip module to be unloaded as needed (we assume that the
  * GPIO chip driver handles freeing the GPIOs it has requested).
  */
-int gpiochip_request_own_desc(struct gpio_desc *desc, const char *label)
+struct gpio_desc *gpiochip_request_own_desc(struct gpio_chip *chip, u16 hwnum,
+					    const char *label)
 {
-	if (!desc || !desc->chip)
-		return -EINVAL;
+	struct gpio_desc *desc = gpiochip_get_desc(chip, hwnum);
+	int err;
 
-	return __gpiod_request(desc, label);
+	if (IS_ERR(desc)) {
+		chip_err(chip, "failed to get GPIO descriptor\n");
+		return desc;
+	}
+
+	err = __gpiod_request(desc, label);
+	if (err < 0)
+		return ERR_PTR(err);
+
+	return desc;
 }
 EXPORT_SYMBOL_GPL(gpiochip_request_own_desc);
 
@@ -1652,7 +1670,7 @@ struct gpio_desc *__must_check __gpiod_get_index(struct device *dev,
 	 * a result. In that case, use platform lookup as a fallback.
 	 */
 	if (!desc || desc == ERR_PTR(-ENOENT)) {
-		dev_dbg(dev, "using lookup tables for GPIO lookup");
+		dev_dbg(dev, "using lookup tables for GPIO lookup\n");
 		desc = gpiod_find(dev, con_id, idx, &lookupflags);
 	}
 
@@ -1674,7 +1692,7 @@ struct gpio_desc *__must_check __gpiod_get_index(struct device *dev,
 		set_bit(FLAG_OPEN_SOURCE, &desc->flags);
 
 	/* No particular flag request, return here... */
-	if (flags & GPIOD_FLAGS_BIT_DIR_SET)
+	if (!(flags & GPIOD_FLAGS_BIT_DIR_SET))
 		return desc;
 
 	/* Process flags */
diff --git a/drivers/gpu/drm/Kconfig b/drivers/gpu/drm/Kconfig
index b066bb3ca01a..e3b4b0f02b3d 100644
--- a/drivers/gpu/drm/Kconfig
+++ b/drivers/gpu/drm/Kconfig
@@ -8,6 +8,7 @@ menuconfig DRM
 	tristate "Direct Rendering Manager (XFree86 4.1.0 and higher DRI support)"
 	depends on (AGP || AGP=n) && !EMULATED_CMPXCHG && MMU && HAS_DMA
 	select HDMI
+	select FB_CMDLINE
 	select I2C
 	select I2C_ALGOBIT
 	select DMA_SHARED_BUFFER
@@ -24,12 +25,6 @@ config DRM_MIPI_DSI
 	bool
 	depends on DRM
 
-config DRM_USB
-	tristate
-	depends on DRM
-	depends on USB_SUPPORT && USB_ARCH_HAS_HCD
-	select USB
-
 config DRM_KMS_HELPER
 	tristate
 	depends on DRM
@@ -115,6 +110,7 @@ config DRM_RADEON
 	select HWMON
 	select BACKLIGHT_CLASS_DEVICE
 	select INTERVAL_TREE
+	select MMU_NOTIFIER
 	help
 	  Choose this option if you have an ATI Radeon graphics card.  There
 	  are both PCI and AGP versions.  You don't need to choose this to
diff --git a/drivers/gpu/drm/Makefile b/drivers/gpu/drm/Makefile
index 4a55d59ccd22..9292a761ea6d 100644
--- a/drivers/gpu/drm/Makefile
+++ b/drivers/gpu/drm/Makefile
@@ -4,7 +4,7 @@
 
 ccflags-y := -Iinclude/drm
 
-drm-y       :=	drm_auth.o drm_buffer.o drm_bufs.o drm_cache.o \
+drm-y       :=	drm_auth.o drm_bufs.o drm_cache.o \
 		drm_context.o drm_dma.o \
 		drm_fops.o drm_gem.o drm_ioctl.o drm_irq.o \
 		drm_lock.o drm_memory.o drm_drv.o drm_vm.o \
@@ -22,8 +22,6 @@ drm-$(CONFIG_PCI) += ati_pcigart.o
 drm-$(CONFIG_DRM_PANEL) += drm_panel.o
 drm-$(CONFIG_OF) += drm_of.o
 
-drm-usb-y   := drm_usb.o
-
 drm_kms_helper-y := drm_crtc_helper.o drm_dp_helper.o drm_probe_helper.o \
 		drm_plane_helper.o drm_dp_mst_topology.o
 drm_kms_helper-$(CONFIG_DRM_LOAD_EDID_FIRMWARE) += drm_edid_load.o
@@ -36,7 +34,6 @@ CFLAGS_drm_trace_points.o := -I$(src)
 
 obj-$(CONFIG_DRM)	+= drm.o
 obj-$(CONFIG_DRM_MIPI_DSI) += drm_mipi_dsi.o
-obj-$(CONFIG_DRM_USB)   += drm_usb.o
 obj-$(CONFIG_DRM_TTM)	+= ttm/
 obj-$(CONFIG_DRM_TDFX)	+= tdfx/
 obj-$(CONFIG_DRM_R128)	+= r128/
diff --git a/drivers/gpu/drm/armada/armada_drv.c b/drivers/gpu/drm/armada/armada_drv.c
index e2d5792b140f..f672e6ad8afa 100644
--- a/drivers/gpu/drm/armada/armada_drv.c
+++ b/drivers/gpu/drm/armada/armada_drv.c
@@ -308,6 +308,7 @@ static struct drm_driver armada_drm_driver = {
 	.postclose		= NULL,
 	.lastclose		= armada_drm_lastclose,
 	.unload			= armada_drm_unload,
+	.set_busid		= drm_platform_set_busid,
 	.get_vblank_counter	= drm_vblank_count,
 	.enable_vblank		= armada_drm_enable_vblank,
 	.disable_vblank		= armada_drm_disable_vblank,
diff --git a/drivers/gpu/drm/armada/armada_gem.h b/drivers/gpu/drm/armada/armada_gem.h
index 00b6cd461a03..b000ea3a829a 100644
--- a/drivers/gpu/drm/armada/armada_gem.h
+++ b/drivers/gpu/drm/armada/armada_gem.h
@@ -8,6 +8,8 @@
 #ifndef ARMADA_GEM_H
 #define ARMADA_GEM_H
 
+#include <drm/drm_gem.h>
+
 /* GEM */
 struct armada_gem_object {
 	struct drm_gem_object	obj;
diff --git a/drivers/gpu/drm/ast/ast_dp501.c b/drivers/gpu/drm/ast/ast_dp501.c
index 5da4b62285fa..76f07f38b941 100644
--- a/drivers/gpu/drm/ast/ast_dp501.c
+++ b/drivers/gpu/drm/ast/ast_dp501.c
@@ -379,11 +379,39 @@ static bool ast_init_dvo(struct drm_device *dev)
 	return true;
 }
 
+
+static void ast_init_analog(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+	u32 data;
+
+	/*
+	 * Set DAC source to VGA mode in SCU2C via the P2A
+	 * bridge. First configure the P2U to target the SCU
+	 * in case it isn't at this stage.
+	 */
+	ast_write32(ast, 0xf004, 0x1e6e0000);
+	ast_write32(ast, 0xf000, 0x1);
+
+	/* Then unlock the SCU with the magic password */
+	ast_write32(ast, 0x12000, 0x1688a8a8);
+	ast_write32(ast, 0x12000, 0x1688a8a8);
+	ast_write32(ast, 0x12000, 0x1688a8a8);
+
+	/* Finally, clear bits [17:16] of SCU2c */
+	data = ast_read32(ast, 0x1202c);
+	data &= 0xfffcffff;
+	ast_write32(ast, 0, data);
+
+	/* Disable DVO */
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa3, 0xcf, 0x00);
+}
+
 void ast_init_3rdtx(struct drm_device *dev)
 {
 	struct ast_private *ast = dev->dev_private;
 	u8 jreg;
-	u32 data;
+
 	if (ast->chip == AST2300 || ast->chip == AST2400) {
 		jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd1, 0xff);
 		switch (jreg & 0x0e) {
@@ -399,12 +427,8 @@ void ast_init_3rdtx(struct drm_device *dev)
 		default:
 			if (ast->tx_chip_type == AST_TX_SIL164)
 				ast_init_dvo(dev);
-			else {
-				ast_write32(ast, 0x12000, 0x1688a8a8);
-				data = ast_read32(ast, 0x1202c);
-				data &= 0xfffcffff;
-				ast_write32(ast, 0, data);
-			}
+			else
+				ast_init_analog(dev);
 		}
 	}
 }
diff --git a/drivers/gpu/drm/ast/ast_drv.c b/drivers/gpu/drm/ast/ast_drv.c
index f19682a93c24..9a32d9dfdd26 100644
--- a/drivers/gpu/drm/ast/ast_drv.c
+++ b/drivers/gpu/drm/ast/ast_drv.c
@@ -199,6 +199,7 @@ static struct drm_driver driver = {
 
 	.load = ast_driver_load,
 	.unload = ast_driver_unload,
+	.set_busid = drm_pci_set_busid,
 
 	.fops = &ast_fops,
 	.name = DRIVER_NAME,
diff --git a/drivers/gpu/drm/ast/ast_drv.h b/drivers/gpu/drm/ast/ast_drv.h
index 957d4fabf1e1..86205a28e56b 100644
--- a/drivers/gpu/drm/ast/ast_drv.h
+++ b/drivers/gpu/drm/ast/ast_drv.h
@@ -36,6 +36,8 @@
 #include <drm/ttm/ttm_memory.h>
 #include <drm/ttm/ttm_module.h>
 
+#include <drm/drm_gem.h>
+
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
 
@@ -125,8 +127,9 @@ struct ast_gem_object;
 
 #define AST_IO_AR_PORT_WRITE		(0x40)
 #define AST_IO_MISC_PORT_WRITE		(0x42)
+#define AST_IO_VGA_ENABLE_PORT		(0x43)
 #define AST_IO_SEQ_PORT			(0x44)
-#define AST_DAC_INDEX_READ		(0x3c7)
+#define AST_IO_DAC_INDEX_READ		(0x47)
 #define AST_IO_DAC_INDEX_WRITE		(0x48)
 #define AST_IO_DAC_DATA		        (0x49)
 #define AST_IO_GR_PORT			(0x4E)
@@ -134,6 +137,8 @@ struct ast_gem_object;
 #define AST_IO_INPUT_STATUS1_READ	(0x5A)
 #define AST_IO_MISC_PORT_READ		(0x4C)
 
+#define AST_IO_MM_OFFSET		(0x380)
+
 #define __ast_read(x) \
 static inline u##x ast_read##x(struct ast_private *ast, u32 reg) { \
 u##x val = 0;\
@@ -316,7 +321,7 @@ struct ast_bo {
 	struct ttm_placement placement;
 	struct ttm_bo_kmap_obj kmap;
 	struct drm_gem_object gem;
-	u32 placements[3];
+	struct ttm_place placements[3];
 	int pin_count;
 };
 #define gem_to_ast_bo(gobj) container_of((gobj), struct ast_bo, gem)
@@ -381,6 +386,9 @@ int ast_bo_push_sysram(struct ast_bo *bo);
 int ast_mmap(struct file *filp, struct vm_area_struct *vma);
 
 /* ast post */
+void ast_enable_vga(struct drm_device *dev);
+void ast_enable_mmio(struct drm_device *dev);
+bool ast_is_vga_enabled(struct drm_device *dev);
 void ast_post_gpu(struct drm_device *dev);
 u32 ast_mindwm(struct ast_private *ast, u32 r);
 void ast_moutdwm(struct ast_private *ast, u32 r, u32 v);
diff --git a/drivers/gpu/drm/ast/ast_fb.c b/drivers/gpu/drm/ast/ast_fb.c
index cba45c774552..5c60ae524c45 100644
--- a/drivers/gpu/drm/ast/ast_fb.c
+++ b/drivers/gpu/drm/ast/ast_fb.c
@@ -186,7 +186,8 @@ static int astfb_create_object(struct ast_fbdev *afbdev,
 static int astfb_create(struct drm_fb_helper *helper,
 			struct drm_fb_helper_surface_size *sizes)
 {
-	struct ast_fbdev *afbdev = (struct ast_fbdev *)helper;
+	struct ast_fbdev *afbdev =
+		container_of(helper, struct ast_fbdev, helper);
 	struct drm_device *dev = afbdev->helper.dev;
 	struct drm_mode_fb_cmd2 mode_cmd;
 	struct drm_framebuffer *fb;
diff --git a/drivers/gpu/drm/ast/ast_main.c b/drivers/gpu/drm/ast/ast_main.c
index a2cc6be97983..035dacc93382 100644
--- a/drivers/gpu/drm/ast/ast_main.c
+++ b/drivers/gpu/drm/ast/ast_main.c
@@ -63,10 +63,11 @@ uint8_t ast_get_index_reg_mask(struct ast_private *ast,
 }
 
 
-static int ast_detect_chip(struct drm_device *dev)
+static int ast_detect_chip(struct drm_device *dev, bool *need_post)
 {
 	struct ast_private *ast = dev->dev_private;
 	uint32_t data, jreg;
+	ast_open_key(ast);
 
 	if (dev->pdev->device == PCI_CHIP_AST1180) {
 		ast->chip = AST1100;
@@ -104,11 +105,26 @@ static int ast_detect_chip(struct drm_device *dev)
 			}
 			ast->vga2_clone = false;
 		} else {
-			ast->chip = 2000;
+			ast->chip = AST2000;
 			DRM_INFO("AST 2000 detected\n");
 		}
 	}
 
+	/*
+	 * If VGA isn't enabled, we need to enable now or subsequent
+	 * access to the scratch registers will fail. We also inform
+	 * our caller that it needs to POST the chip
+	 * (Assumption: VGA not enabled -> need to POST)
+	 */
+	if (!ast_is_vga_enabled(dev)) {
+		ast_enable_vga(dev);
+		ast_enable_mmio(dev);
+		DRM_INFO("VGA not enabled on entry, requesting chip POST\n");
+		*need_post = true;
+	} else
+		*need_post = false;
+
+	/* Check if we support wide screen */
 	switch (ast->chip) {
 	case AST1180:
 		ast->support_wide_screen = true;
@@ -124,6 +140,7 @@ static int ast_detect_chip(struct drm_device *dev)
 			ast->support_wide_screen = true;
 		else {
 			ast->support_wide_screen = false;
+			/* Read SCU7c (silicon revision register) */
 			ast_write32(ast, 0xf004, 0x1e6e0000);
 			ast_write32(ast, 0xf000, 0x1);
 			data = ast_read32(ast, 0x1207c);
@@ -136,11 +153,29 @@ static int ast_detect_chip(struct drm_device *dev)
 		break;
 	}
 
+	/* Check 3rd Tx option (digital output afaik) */
 	ast->tx_chip_type = AST_TX_NONE;
-	jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa3, 0xff);
-	if (jreg & 0x80)
-		ast->tx_chip_type = AST_TX_SIL164;
+
+	/*
+	 * VGACRA3 Enhanced Color Mode Register, check if DVO is already
+	 * enabled, in that case, assume we have a SIL164 TMDS transmitter
+	 *
+	 * Don't make that assumption if we the chip wasn't enabled and
+	 * is at power-on reset, otherwise we'll incorrectly "detect" a
+	 * SIL164 when there is none.
+	 */
+	if (!*need_post) {
+		jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa3, 0xff);
+		if (jreg & 0x80)
+			ast->tx_chip_type = AST_TX_SIL164;
+	}
+
 	if ((ast->chip == AST2300) || (ast->chip == AST2400)) {
+		/*
+		 * On AST2300 and 2400, look the configuration set by the SoC in
+		 * the SOC scratch register #1 bits 11:8 (interestingly marked
+		 * as "reserved" in the spec)
+		 */
 		jreg = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xd1, 0xff);
 		switch (jreg) {
 		case 0x04:
@@ -161,6 +196,17 @@ static int ast_detect_chip(struct drm_device *dev)
 		}
 	}
 
+	/* Print stuff for diagnostic purposes */
+	switch(ast->tx_chip_type) {
+	case AST_TX_SIL164:
+		DRM_INFO("Using Sil164 TMDS transmitter\n");
+		break;
+	case AST_TX_DP501:
+		DRM_INFO("Using DP501 DisplayPort transmitter\n");
+		break;
+	default:
+		DRM_INFO("Analog VGA only\n");
+	}
 	return 0;
 }
 
@@ -345,6 +391,7 @@ static u32 ast_get_vram_info(struct drm_device *dev)
 int ast_driver_load(struct drm_device *dev, unsigned long flags)
 {
 	struct ast_private *ast;
+	bool need_post;
 	int ret = 0;
 
 	ast = kzalloc(sizeof(struct ast_private), GFP_KERNEL);
@@ -359,13 +406,27 @@ int ast_driver_load(struct drm_device *dev, unsigned long flags)
 		ret = -EIO;
 		goto out_free;
 	}
-	ast->ioregs = pci_iomap(dev->pdev, 2, 0);
+
+	/*
+	 * If we don't have IO space at all, use MMIO now and
+	 * assume the chip has MMIO enabled by default (rev 0x20
+	 * and higher).
+	 */
+	if (!(pci_resource_flags(dev->pdev, 2) & IORESOURCE_IO)) {
+		DRM_INFO("platform has no IO space, trying MMIO\n");
+		ast->ioregs = ast->regs + AST_IO_MM_OFFSET;
+	}
+
+	/* "map" IO regs if the above hasn't done so already */
 	if (!ast->ioregs) {
-		ret = -EIO;
-		goto out_free;
+		ast->ioregs = pci_iomap(dev->pdev, 2, 0);
+		if (!ast->ioregs) {
+			ret = -EIO;
+			goto out_free;
+		}
 	}
 
-	ast_detect_chip(dev);
+	ast_detect_chip(dev, &need_post);
 
 	if (ast->chip != AST1180) {
 		ast_get_dram_info(dev);
@@ -373,6 +434,9 @@ int ast_driver_load(struct drm_device *dev, unsigned long flags)
 		DRM_INFO("dram %d %d %d %08x\n", ast->mclk, ast->dram_type, ast->dram_bus_width, ast->vram_size);
 	}
 
+	if (need_post)
+		ast_post_gpu(dev);
+
 	ret = ast_mm_init(ast);
 	if (ret)
 		goto out_free;
diff --git a/drivers/gpu/drm/ast/ast_mode.c b/drivers/gpu/drm/ast/ast_mode.c
index 5389350244f2..9dc0fd5c1ea4 100644
--- a/drivers/gpu/drm/ast/ast_mode.c
+++ b/drivers/gpu/drm/ast/ast_mode.c
@@ -80,6 +80,8 @@ static bool ast_get_vbios_mode_info(struct drm_crtc *crtc, struct drm_display_mo
 	struct ast_private *ast = crtc->dev->dev_private;
 	u32 refresh_rate_index = 0, mode_id, color_index, refresh_rate;
 	u32 hborder, vborder;
+	bool check_sync;
+	struct ast_vbios_enhtable *best = NULL;
 
 	switch (crtc->primary->fb->bits_per_pixel) {
 	case 8:
@@ -141,14 +143,34 @@ static bool ast_get_vbios_mode_info(struct drm_crtc *crtc, struct drm_display_mo
 	}
 
 	refresh_rate = drm_mode_vrefresh(mode);
-	while (vbios_mode->enh_table->refresh_rate < refresh_rate) {
-		vbios_mode->enh_table++;
-		if ((vbios_mode->enh_table->refresh_rate > refresh_rate) ||
-		    (vbios_mode->enh_table->refresh_rate == 0xff)) {
-			vbios_mode->enh_table--;
-			break;
+	check_sync = vbios_mode->enh_table->flags & WideScreenMode;
+	do {
+		struct ast_vbios_enhtable *loop = vbios_mode->enh_table;
+
+		while (loop->refresh_rate != 0xff) {
+			if ((check_sync) &&
+			    (((mode->flags & DRM_MODE_FLAG_NVSYNC)  &&
+			      (loop->flags & PVSync))  ||
+			     ((mode->flags & DRM_MODE_FLAG_PVSYNC)  &&
+			      (loop->flags & NVSync))  ||
+			     ((mode->flags & DRM_MODE_FLAG_NHSYNC)  &&
+			      (loop->flags & PHSync))  ||
+			     ((mode->flags & DRM_MODE_FLAG_PHSYNC)  &&
+			      (loop->flags & NHSync)))) {
+				loop++;
+				continue;
+			}
+			if (loop->refresh_rate <= refresh_rate
+			    && (!best || loop->refresh_rate > best->refresh_rate))
+				best = loop;
+			loop++;
 		}
-	}
+		if (best || !check_sync)
+			break;
+		check_sync = 0;
+	} while (1);
+	if (best)
+		vbios_mode->enh_table = best;
 
 	hborder = (vbios_mode->enh_table->flags & HBorder) ? 8 : 0;
 	vborder = (vbios_mode->enh_table->flags & VBorder) ? 8 : 0;
@@ -419,8 +441,10 @@ static void ast_set_sync_reg(struct drm_device *dev, struct drm_display_mode *mo
 	struct ast_private *ast = dev->dev_private;
 	u8 jreg;
 
-	jreg = ast_io_read8(ast, AST_IO_MISC_PORT_READ);
-	jreg |= (vbios_mode->enh_table->flags & SyncNN);
+	jreg  = ast_io_read8(ast, AST_IO_MISC_PORT_READ);
+	jreg &= ~0xC0;
+	if (vbios_mode->enh_table->flags & NVSync) jreg |= 0x80;
+	if (vbios_mode->enh_table->flags & NHSync) jreg |= 0x40;
 	ast_io_write8(ast, AST_IO_MISC_PORT_WRITE, jreg);
 }
 
@@ -1080,8 +1104,8 @@ static u32 copy_cursor_image(u8 *src, u8 *dst, int width, int height)
 			srcdata32[1].ul = *((u32 *)(srcxor + 4)) & 0xf0f0f0f0;
 			data32.b[0] = srcdata32[0].b[1] | (srcdata32[0].b[0] >> 4);
 			data32.b[1] = srcdata32[0].b[3] | (srcdata32[0].b[2] >> 4);
-			data32.b[2] = srcdata32[0].b[1] | (srcdata32[1].b[0] >> 4);
-			data32.b[3] = srcdata32[0].b[3] | (srcdata32[1].b[2] >> 4);
+			data32.b[2] = srcdata32[1].b[1] | (srcdata32[1].b[0] >> 4);
+			data32.b[3] = srcdata32[1].b[3] | (srcdata32[1].b[2] >> 4);
 
 			writel(data32.ul, dstxor);
 			csum += data32.ul;
diff --git a/drivers/gpu/drm/ast/ast_post.c b/drivers/gpu/drm/ast/ast_post.c
index 38d437f3a267..810c51d92b99 100644
--- a/drivers/gpu/drm/ast/ast_post.c
+++ b/drivers/gpu/drm/ast/ast_post.c
@@ -33,18 +33,23 @@
 
 static void ast_init_dram_2300(struct drm_device *dev);
 
-static void
-ast_enable_vga(struct drm_device *dev)
+void ast_enable_vga(struct drm_device *dev)
+{
+	struct ast_private *ast = dev->dev_private;
+
+	ast_io_write8(ast, AST_IO_VGA_ENABLE_PORT, 0x01);
+	ast_io_write8(ast, AST_IO_MISC_PORT_WRITE, 0x01);
+}
+
+void ast_enable_mmio(struct drm_device *dev)
 {
 	struct ast_private *ast = dev->dev_private;
 
-	ast_io_write8(ast, 0x43, 0x01);
-	ast_io_write8(ast, 0x42, 0x01);
+	ast_set_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xa1, 0xff, 0x04);
 }
 
-#if 0 /* will use later */
-static bool
-ast_is_vga_enabled(struct drm_device *dev)
+
+bool ast_is_vga_enabled(struct drm_device *dev)
 {
 	struct ast_private *ast = dev->dev_private;
 	u8 ch;
@@ -52,7 +57,7 @@ ast_is_vga_enabled(struct drm_device *dev)
 	if (ast->chip == AST1180) {
 		/* TODO 1180 */
 	} else {
-		ch = ast_io_read8(ast, 0x43);
+		ch = ast_io_read8(ast, AST_IO_VGA_ENABLE_PORT);
 		if (ch) {
 			ast_open_key(ast);
 			ch = ast_get_index_reg_mask(ast, AST_IO_CRTC_PORT, 0xb6, 0xff);
@@ -61,7 +66,6 @@ ast_is_vga_enabled(struct drm_device *dev)
 	}
 	return 0;
 }
-#endif
 
 static const u8 extreginfo[] = { 0x0f, 0x04, 0x1c, 0xff };
 static const u8 extreginfo_ast2300a0[] = { 0x0f, 0x04, 0x1c, 0xff };
@@ -371,6 +375,7 @@ void ast_post_gpu(struct drm_device *dev)
 	pci_write_config_dword(ast->dev->pdev, 0x04, reg);
 
 	ast_enable_vga(dev);
+	ast_enable_mmio(dev);
 	ast_open_key(ast);
 	ast_set_def_ext_reg(dev);
 
diff --git a/drivers/gpu/drm/ast/ast_tables.h b/drivers/gpu/drm/ast/ast_tables.h
index 4c761dcea972..3608d5aa7451 100644
--- a/drivers/gpu/drm/ast/ast_tables.h
+++ b/drivers/gpu/drm/ast/ast_tables.h
@@ -35,14 +35,18 @@
 #define HalfDCLK                0x00000002
 #define DoubleScanMode          0x00000004
 #define LineCompareOff          0x00000008
-#define SyncPP                  0x00000000
-#define SyncPN                  0x00000040
-#define SyncNP                  0x00000080
-#define SyncNN                  0x000000C0
 #define HBorder                 0x00000020
 #define VBorder                 0x00000010
 #define WideScreenMode		0x00000100
 #define NewModeInfo		0x00000200
+#define NHSync			0x00000400
+#define PHSync			0x00000800
+#define NVSync			0x00001000
+#define PVSync			0x00002000
+#define SyncPP			(PVSync | PHSync)
+#define SyncPN			(PVSync | NHSync)
+#define SyncNP			(NVSync | PHSync)
+#define SyncNN			(NVSync | NHSync)
 
 /* DCLK Index */
 #define VCLK25_175     		0x00
@@ -72,6 +76,7 @@
 #define VCLK119     		0x17
 #define VCLK85_5     		0x18
 #define VCLK97_75     		0x19
+#define VCLK118_25			0x1A
 
 static struct ast_vbios_dclk_info dclk_table[] = {
 	{0x2C, 0xE7, 0x03},					/* 00: VCLK25_175	*/
@@ -99,6 +104,8 @@ static struct ast_vbios_dclk_info dclk_table[] = {
 	{0x25, 0x65, 0x80},					/* 16: VCLK88.75    */
 	{0x77, 0x58, 0x80},					/* 17: VCLK119      */
 	{0x32, 0x67, 0x80},				    /* 18: VCLK85_5     */
+	{0x6a, 0x6d, 0x80},					/* 19: VCLK97_75	*/
+	{0x3b, 0x2c, 0x81},					/* 1A: VCLK118_25	*/
 };
 
 static struct ast_vbios_stdtable vbios_stdtable[] = {
@@ -245,8 +252,10 @@ static struct ast_vbios_enhtable res_1360x768[] = {
 static struct ast_vbios_enhtable res_1600x900[] = {
 	{1760, 1600, 48, 32, 926,  900, 3, 5, VCLK97_75,	/* 60Hz CVT RB */
 	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x3A },
-	{1760, 1600, 48, 32, 926,  900, 3, 5, VCLK97_75,	/* end */
-	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 1, 0x3A }
+	{2112, 1600, 88,168, 934,  900, 3, 5, VCLK118_25,	/* 60Hz CVT */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 2, 0x3A },
+	{2112, 1600, 88,168, 934,  900, 3, 5, VCLK118_25,	/* 60Hz CVT */
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 2, 0x3A },
 };
 
 static struct ast_vbios_enhtable res_1920x1080[] = {
@@ -260,11 +269,11 @@ static struct ast_vbios_enhtable res_1920x1080[] = {
 /* 16:10 */
 static struct ast_vbios_enhtable res_1280x800[] = {
 	{1440, 1280, 48, 32,  823,  800, 3, 6, VCLK71,	/* 60Hz RB */
-	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 35 },
+	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x35 },
 	{1680, 1280, 72,128,  831,  800, 3, 6, VCLK83_5,	/* 60Hz */
-	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x35 },
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 2, 0x35 },
 	{1680, 1280, 72,128,  831,  800, 3, 6, VCLK83_5,	/* 60Hz */
-	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 1, 0x35 },
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 2, 0x35 },
 
 };
 
@@ -272,24 +281,24 @@ static struct ast_vbios_enhtable res_1440x900[] = {
 	{1600, 1440, 48, 32,  926,  900, 3, 6, VCLK88_75,	/* 60Hz RB */
 	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x36 },
 	{1904, 1440, 80,152,  934,  900, 3, 6, VCLK106_5,	/* 60Hz */
-	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x36 },
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 2, 0x36 },
 	{1904, 1440, 80,152,  934,  900, 3, 6, VCLK106_5,	/* 60Hz */
-	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 1, 0x36 },
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 2, 0x36 },
 };
 
 static struct ast_vbios_enhtable res_1680x1050[] = {
 	{1840, 1680, 48, 32, 1080, 1050, 3, 6, VCLK119,	/* 60Hz RB */
 	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x37 },
 	{2240, 1680,104,176, 1089, 1050, 3, 6, VCLK146_25,	/* 60Hz */
-	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x37 },
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 2, 0x37 },
 	{2240, 1680,104,176, 1089, 1050, 3, 6, VCLK146_25,	/* 60Hz */
-	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 1, 0x37 },
+	 (SyncPN | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 2, 0x37 },
 };
 
 static struct ast_vbios_enhtable res_1920x1200[] = {
-	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz */
+	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz RB*/
 	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 60, 1, 0x34 },
-	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz */
+	{2080, 1920, 48, 32, 1235, 1200, 3, 6, VCLK154,	/* 60Hz RB */
 	 (SyncNP | Charx8Dot | LineCompareOff | WideScreenMode | NewModeInfo), 0xFF, 1, 0x34 },
 };
 
diff --git a/drivers/gpu/drm/ast/ast_ttm.c b/drivers/gpu/drm/ast/ast_ttm.c
index b8246227bab0..08f82eae6939 100644
--- a/drivers/gpu/drm/ast/ast_ttm.c
+++ b/drivers/gpu/drm/ast/ast_ttm.c
@@ -293,18 +293,22 @@ void ast_mm_fini(struct ast_private *ast)
 void ast_ttm_placement(struct ast_bo *bo, int domain)
 {
 	u32 c = 0;
-	bo->placement.fpfn = 0;
-	bo->placement.lpfn = 0;
+	unsigned i;
+
 	bo->placement.placement = bo->placements;
 	bo->placement.busy_placement = bo->placements;
 	if (domain & TTM_PL_FLAG_VRAM)
-		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+		bo->placements[c++].flags = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
 	if (domain & TTM_PL_FLAG_SYSTEM)
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_SYSTEM;
 	if (!c)
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_SYSTEM;
 	bo->placement.num_placement = c;
 	bo->placement.num_busy_placement = c;
+	for (i = 0; i < c; ++i) {
+		bo->placements[i].fpfn = 0;
+		bo->placements[i].lpfn = 0;
+	}
 }
 
 int ast_bo_create(struct drm_device *dev, int size, int align,
@@ -335,7 +339,7 @@ int ast_bo_create(struct drm_device *dev, int size, int align,
 	ret = ttm_bo_init(&ast->ttm.bdev, &astbo->bo, size,
 			  ttm_bo_type_device, &astbo->placement,
 			  align >> PAGE_SHIFT, false, NULL, acc_size,
-			  NULL, ast_bo_ttm_destroy);
+			  NULL, NULL, ast_bo_ttm_destroy);
 	if (ret)
 		return ret;
 
@@ -360,7 +364,7 @@ int ast_bo_pin(struct ast_bo *bo, u32 pl_flag, u64 *gpu_addr)
 
 	ast_ttm_placement(bo, pl_flag);
 	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret)
 		return ret;
@@ -383,7 +387,7 @@ int ast_bo_unpin(struct ast_bo *bo)
 		return 0;
 
 	for (i = 0; i < bo->placement.num_placement ; i++)
-		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags &= ~TTM_PL_FLAG_NO_EVICT;
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret)
 		return ret;
@@ -407,7 +411,7 @@ int ast_bo_push_sysram(struct ast_bo *bo)
 
 	ast_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
 	for (i = 0; i < bo->placement.num_placement ; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret) {
@@ -423,7 +427,7 @@ int ast_mmap(struct file *filp, struct vm_area_struct *vma)
 	struct ast_private *ast;
 
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 
 	file_priv = filp->private_data;
 	ast = file_priv->minor->dev->dev_private;
diff --git a/drivers/gpu/drm/ati_pcigart.c b/drivers/gpu/drm/ati_pcigart.c
index c399dea27a3b..6c4d4b6eba80 100644
--- a/drivers/gpu/drm/ati_pcigart.c
+++ b/drivers/gpu/drm/ati_pcigart.c
@@ -34,6 +34,8 @@
 #include <linux/export.h>
 #include <drm/drmP.h>
 
+#include <drm/ati_pcigart.h>
+
 # define ATI_PCIGART_PAGE_SIZE		4096	/**< PCI GART page size */
 
 static int drm_ati_alloc_pcigart_table(struct drm_device *dev,
diff --git a/drivers/gpu/drm/bochs/bochs.h b/drivers/gpu/drm/bochs/bochs.h
index 7eb52dd44b01..71f2687fc3cc 100644
--- a/drivers/gpu/drm/bochs/bochs.h
+++ b/drivers/gpu/drm/bochs/bochs.h
@@ -7,6 +7,8 @@
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_helper.h>
 
+#include <drm/drm_gem.h>
+
 #include <ttm/ttm_bo_driver.h>
 #include <ttm/ttm_page_alloc.h>
 
@@ -99,7 +101,7 @@ struct bochs_bo {
 	struct ttm_placement placement;
 	struct ttm_bo_kmap_obj kmap;
 	struct drm_gem_object gem;
-	u32 placements[3];
+	struct ttm_place placements[3];
 	int pin_count;
 };
 
diff --git a/drivers/gpu/drm/bochs/bochs_drv.c b/drivers/gpu/drm/bochs/bochs_drv.c
index 9738e9b14708..98837bde2d25 100644
--- a/drivers/gpu/drm/bochs/bochs_drv.c
+++ b/drivers/gpu/drm/bochs/bochs_drv.c
@@ -82,6 +82,7 @@ static struct drm_driver bochs_driver = {
 	.driver_features	= DRIVER_GEM | DRIVER_MODESET,
 	.load			= bochs_load,
 	.unload			= bochs_unload,
+	.set_busid		= drm_pci_set_busid,
 	.fops			= &bochs_fops,
 	.name			= "bochs-drm",
 	.desc			= "bochs dispi vga interface (qemu stdvga)",
diff --git a/drivers/gpu/drm/bochs/bochs_kms.c b/drivers/gpu/drm/bochs/bochs_kms.c
index 9d7346b92653..6b7efcf363d6 100644
--- a/drivers/gpu/drm/bochs/bochs_kms.c
+++ b/drivers/gpu/drm/bochs/bochs_kms.c
@@ -250,6 +250,7 @@ static void bochs_connector_init(struct drm_device *dev)
 			   DRM_MODE_CONNECTOR_VIRTUAL);
 	drm_connector_helper_add(connector,
 				 &bochs_connector_connector_helper_funcs);
+	drm_connector_register(connector);
 }
 
 
diff --git a/drivers/gpu/drm/bochs/bochs_mm.c b/drivers/gpu/drm/bochs/bochs_mm.c
index 1728a1b0b813..66286ff518d4 100644
--- a/drivers/gpu/drm/bochs/bochs_mm.c
+++ b/drivers/gpu/drm/bochs/bochs_mm.c
@@ -257,20 +257,26 @@ void bochs_mm_fini(struct bochs_device *bochs)
 
 static void bochs_ttm_placement(struct bochs_bo *bo, int domain)
 {
+	unsigned i;
 	u32 c = 0;
-	bo->placement.fpfn = 0;
-	bo->placement.lpfn = 0;
 	bo->placement.placement = bo->placements;
 	bo->placement.busy_placement = bo->placements;
 	if (domain & TTM_PL_FLAG_VRAM) {
-		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED
+		bo->placements[c++].flags = TTM_PL_FLAG_WC
+			| TTM_PL_FLAG_UNCACHED
 			| TTM_PL_FLAG_VRAM;
 	}
 	if (domain & TTM_PL_FLAG_SYSTEM) {
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_MASK_CACHING
+			| TTM_PL_FLAG_SYSTEM;
 	}
 	if (!c) {
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_MASK_CACHING
+			| TTM_PL_FLAG_SYSTEM;
+	}
+	for (i = 0; i < c; ++i) {
+		bo->placements[i].fpfn = 0;
+		bo->placements[i].lpfn = 0;
 	}
 	bo->placement.num_placement = c;
 	bo->placement.num_busy_placement = c;
@@ -294,7 +300,7 @@ int bochs_bo_pin(struct bochs_bo *bo, u32 pl_flag, u64 *gpu_addr)
 
 	bochs_ttm_placement(bo, pl_flag);
 	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret)
 		return ret;
@@ -319,7 +325,7 @@ int bochs_bo_unpin(struct bochs_bo *bo)
 		return 0;
 
 	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags &= ~TTM_PL_FLAG_NO_EVICT;
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret)
 		return ret;
@@ -333,7 +339,7 @@ int bochs_mmap(struct file *filp, struct vm_area_struct *vma)
 	struct bochs_device *bochs;
 
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 
 	file_priv = filp->private_data;
 	bochs = file_priv->minor->dev->dev_private;
@@ -371,7 +377,7 @@ static int bochs_bo_create(struct drm_device *dev, int size, int align,
 	ret = ttm_bo_init(&bochs->ttm.bdev, &bochsbo->bo, size,
 			  ttm_bo_type_device, &bochsbo->placement,
 			  align >> PAGE_SHIFT, false, NULL, acc_size,
-			  NULL, bochs_bo_ttm_destroy);
+			  NULL, NULL, bochs_bo_ttm_destroy);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.c b/drivers/gpu/drm/cirrus/cirrus_drv.c
index 919c73b94447..c2a1cba1e984 100644
--- a/drivers/gpu/drm/cirrus/cirrus_drv.c
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.c
@@ -32,6 +32,8 @@ static struct drm_driver driver;
 static const struct pci_device_id pciidlist[] = {
 	{ PCI_VENDOR_ID_CIRRUS, PCI_DEVICE_ID_CIRRUS_5446, 0x1af4, 0x1100, 0,
 	  0, 0 },
+	{ PCI_VENDOR_ID_CIRRUS, PCI_DEVICE_ID_CIRRUS_5446, PCI_VENDOR_ID_XEN,
+	  0x0001, 0, 0, 0 },
 	{0,}
 };
 
@@ -128,6 +130,7 @@ static struct drm_driver driver = {
 	.driver_features = DRIVER_MODESET | DRIVER_GEM,
 	.load = cirrus_driver_load,
 	.unload = cirrus_driver_unload,
+	.set_busid = drm_pci_set_busid,
 	.fops = &cirrus_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
diff --git a/drivers/gpu/drm/cirrus/cirrus_drv.h b/drivers/gpu/drm/cirrus/cirrus_drv.h
index 401c890b6c6a..d44e69daa239 100644
--- a/drivers/gpu/drm/cirrus/cirrus_drv.h
+++ b/drivers/gpu/drm/cirrus/cirrus_drv.h
@@ -21,6 +21,8 @@
 #include <drm/ttm/ttm_memory.h>
 #include <drm/ttm/ttm_module.h>
 
+#include <drm/drm_gem.h>
+
 #define DRIVER_AUTHOR		"Matthew Garrett"
 
 #define DRIVER_NAME		"cirrus"
@@ -163,7 +165,7 @@ struct cirrus_bo {
 	struct ttm_placement placement;
 	struct ttm_bo_kmap_obj kmap;
 	struct drm_gem_object gem;
-	u32 placements[3];
+	struct ttm_place placements[3];
 	int pin_count;
 };
 #define gem_to_cirrus_bo(gobj) container_of((gobj), struct cirrus_bo, gem)
diff --git a/drivers/gpu/drm/cirrus/cirrus_fbdev.c b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
index 2a135f253e29..d231b1c317af 100644
--- a/drivers/gpu/drm/cirrus/cirrus_fbdev.c
+++ b/drivers/gpu/drm/cirrus/cirrus_fbdev.c
@@ -160,7 +160,8 @@ static int cirrusfb_create_object(struct cirrus_fbdev *afbdev,
 static int cirrusfb_create(struct drm_fb_helper *helper,
 			   struct drm_fb_helper_surface_size *sizes)
 {
-	struct cirrus_fbdev *gfbdev = (struct cirrus_fbdev *)helper;
+	struct cirrus_fbdev *gfbdev =
+		container_of(helper, struct cirrus_fbdev, helper);
 	struct drm_device *dev = gfbdev->helper.dev;
 	struct cirrus_device *cdev = gfbdev->helper.dev->dev_private;
 	struct fb_info *info;
diff --git a/drivers/gpu/drm/cirrus/cirrus_mode.c b/drivers/gpu/drm/cirrus/cirrus_mode.c
index e1c5c3222129..c7c5a9d91fa0 100644
--- a/drivers/gpu/drm/cirrus/cirrus_mode.c
+++ b/drivers/gpu/drm/cirrus/cirrus_mode.c
@@ -555,6 +555,7 @@ static struct drm_connector *cirrus_vga_init(struct drm_device *dev)
 
 	drm_connector_helper_add(connector, &cirrus_vga_connector_helper_funcs);
 
+	drm_connector_register(connector);
 	return connector;
 }
 
diff --git a/drivers/gpu/drm/cirrus/cirrus_ttm.c b/drivers/gpu/drm/cirrus/cirrus_ttm.c
index 92e6b7786097..dfffd528517a 100644
--- a/drivers/gpu/drm/cirrus/cirrus_ttm.c
+++ b/drivers/gpu/drm/cirrus/cirrus_ttm.c
@@ -298,18 +298,21 @@ void cirrus_mm_fini(struct cirrus_device *cirrus)
 void cirrus_ttm_placement(struct cirrus_bo *bo, int domain)
 {
 	u32 c = 0;
-	bo->placement.fpfn = 0;
-	bo->placement.lpfn = 0;
+	unsigned i;
 	bo->placement.placement = bo->placements;
 	bo->placement.busy_placement = bo->placements;
 	if (domain & TTM_PL_FLAG_VRAM)
-		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+		bo->placements[c++].flags = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
 	if (domain & TTM_PL_FLAG_SYSTEM)
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
 	if (!c)
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
 	bo->placement.num_placement = c;
 	bo->placement.num_busy_placement = c;
+	for (i = 0; i < c; ++i) {
+		bo->placements[i].fpfn = 0;
+		bo->placements[i].lpfn = 0;
+	}
 }
 
 int cirrus_bo_create(struct drm_device *dev, int size, int align,
@@ -340,7 +343,7 @@ int cirrus_bo_create(struct drm_device *dev, int size, int align,
 	ret = ttm_bo_init(&cirrus->ttm.bdev, &cirrusbo->bo, size,
 			  ttm_bo_type_device, &cirrusbo->placement,
 			  align >> PAGE_SHIFT, false, NULL, acc_size,
-			  NULL, cirrus_bo_ttm_destroy);
+			  NULL, NULL, cirrus_bo_ttm_destroy);
 	if (ret)
 		return ret;
 
@@ -365,7 +368,7 @@ int cirrus_bo_pin(struct cirrus_bo *bo, u32 pl_flag, u64 *gpu_addr)
 
 	cirrus_ttm_placement(bo, pl_flag);
 	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret)
 		return ret;
@@ -392,7 +395,7 @@ int cirrus_bo_push_sysram(struct cirrus_bo *bo)
 
 	cirrus_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
 	for (i = 0; i < bo->placement.num_placement ; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret) {
@@ -408,7 +411,7 @@ int cirrus_mmap(struct file *filp, struct vm_area_struct *vma)
 	struct cirrus_device *cirrus;
 
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 
 	file_priv = filp->private_data;
 	cirrus = file_priv->minor->dev->dev_private;
diff --git a/drivers/gpu/drm/drm_agpsupport.c b/drivers/gpu/drm/drm_agpsupport.c
index dde205cef384..4b2b4aa5033b 100644
--- a/drivers/gpu/drm/drm_agpsupport.c
+++ b/drivers/gpu/drm/drm_agpsupport.c
@@ -34,6 +34,7 @@
 #include <drm/drmP.h>
 #include <linux/module.h>
 #include <linux/slab.h>
+#include "drm_legacy.h"
 
 #if __OS_HAS_AGP
 
diff --git a/drivers/gpu/drm/drm_auth.c b/drivers/gpu/drm/drm_auth.c
index 3cedae12b3c1..fc8e8aaa34fb 100644
--- a/drivers/gpu/drm/drm_auth.c
+++ b/drivers/gpu/drm/drm_auth.c
@@ -34,6 +34,13 @@
  */
 
 #include <drm/drmP.h>
+#include "drm_internal.h"
+
+struct drm_magic_entry {
+	struct list_head head;
+	struct drm_hash_item hash_item;
+	struct drm_file *priv;
+};
 
 /**
  * Find the file with the given magic number.
diff --git a/drivers/gpu/drm/drm_bufs.c b/drivers/gpu/drm/drm_bufs.c
index 61acb8f6756d..569064a00693 100644
--- a/drivers/gpu/drm/drm_bufs.c
+++ b/drivers/gpu/drm/drm_bufs.c
@@ -1,18 +1,13 @@
-/**
- * \file drm_bufs.c
- * Generic buffer template
- *
- * \author Rickard E. (Rik) Faith <faith@valinux.com>
- * \author Gareth Hughes <gareth@valinux.com>
- */
-
 /*
- * Created: Thu Nov 23 03:10:50 2000 by gareth@valinux.com
+ * Legacy: Generic DRM Buffer Management
  *
  * Copyright 1999, 2000 Precision Insight, Inc., Cedar Park, Texas.
  * Copyright 2000 VA Linux Systems, Inc., Sunnyvale, California.
  * All Rights Reserved.
  *
+ * Author: Rickard E. (Rik) Faith <faith@valinux.com>
+ * Author: Gareth Hughes <gareth@valinux.com>
+ *
  * Permission is hereby granted, free of charge, to any person obtaining a
  * copy of this software and associated documentation files (the "Software"),
  * to deal in the Software without restriction, including without limitation
@@ -39,6 +34,7 @@
 #include <linux/export.h>
 #include <asm/shmparam.h>
 #include <drm/drmP.h>
+#include "drm_legacy.h"
 
 static struct drm_map_list *drm_find_matching_map(struct drm_device *dev,
 						  struct drm_local_map *map)
@@ -365,9 +361,9 @@ static int drm_addmap_core(struct drm_device * dev, resource_size_t offset,
 	return 0;
 }
 
-int drm_addmap(struct drm_device * dev, resource_size_t offset,
-	       unsigned int size, enum drm_map_type type,
-	       enum drm_map_flags flags, struct drm_local_map ** map_ptr)
+int drm_legacy_addmap(struct drm_device * dev, resource_size_t offset,
+		      unsigned int size, enum drm_map_type type,
+		      enum drm_map_flags flags, struct drm_local_map **map_ptr)
 {
 	struct drm_map_list *list;
 	int rc;
@@ -377,8 +373,7 @@ int drm_addmap(struct drm_device * dev, resource_size_t offset,
 		*map_ptr = list->map;
 	return rc;
 }
-
-EXPORT_SYMBOL(drm_addmap);
+EXPORT_SYMBOL(drm_legacy_addmap);
 
 /**
  * Ioctl to specify a range of memory that is available for mapping by a
@@ -391,8 +386,8 @@ EXPORT_SYMBOL(drm_addmap);
  * \return zero on success or a negative value on error.
  *
  */
-int drm_addmap_ioctl(struct drm_device *dev, void *data,
-		     struct drm_file *file_priv)
+int drm_legacy_addmap_ioctl(struct drm_device *dev, void *data,
+			    struct drm_file *file_priv)
 {
 	struct drm_map *map = data;
 	struct drm_map_list *maplist;
@@ -429,9 +424,9 @@ int drm_addmap_ioctl(struct drm_device *dev, void *data,
  * its being used, and free any associate resource (such as MTRR's) if it's not
  * being on use.
  *
- * \sa drm_addmap
+ * \sa drm_legacy_addmap
  */
-int drm_rmmap_locked(struct drm_device *dev, struct drm_local_map *map)
+int drm_legacy_rmmap_locked(struct drm_device *dev, struct drm_local_map *map)
 {
 	struct drm_map_list *r_list = NULL, *list_t;
 	drm_dma_handle_t dmah;
@@ -478,26 +473,26 @@ int drm_rmmap_locked(struct drm_device *dev, struct drm_local_map *map)
 		dmah.vaddr = map->handle;
 		dmah.busaddr = map->offset;
 		dmah.size = map->size;
-		__drm_pci_free(dev, &dmah);
+		__drm_legacy_pci_free(dev, &dmah);
 		break;
 	}
 	kfree(map);
 
 	return 0;
 }
-EXPORT_SYMBOL(drm_rmmap_locked);
+EXPORT_SYMBOL(drm_legacy_rmmap_locked);
 
-int drm_rmmap(struct drm_device *dev, struct drm_local_map *map)
+int drm_legacy_rmmap(struct drm_device *dev, struct drm_local_map *map)
 {
 	int ret;
 
 	mutex_lock(&dev->struct_mutex);
-	ret = drm_rmmap_locked(dev, map);
+	ret = drm_legacy_rmmap_locked(dev, map);
 	mutex_unlock(&dev->struct_mutex);
 
 	return ret;
 }
-EXPORT_SYMBOL(drm_rmmap);
+EXPORT_SYMBOL(drm_legacy_rmmap);
 
 /* The rmmap ioctl appears to be unnecessary.  All mappings are torn down on
  * the last close of the device, and this is necessary for cleanup when things
@@ -514,8 +509,8 @@ EXPORT_SYMBOL(drm_rmmap);
  * \param arg pointer to a struct drm_map structure.
  * \return zero on success or a negative value on error.
  */
-int drm_rmmap_ioctl(struct drm_device *dev, void *data,
-		    struct drm_file *file_priv)
+int drm_legacy_rmmap_ioctl(struct drm_device *dev, void *data,
+			   struct drm_file *file_priv)
 {
 	struct drm_map *request = data;
 	struct drm_local_map *map = NULL;
@@ -546,7 +541,7 @@ int drm_rmmap_ioctl(struct drm_device *dev, void *data,
 		return 0;
 	}
 
-	ret = drm_rmmap_locked(dev, map);
+	ret = drm_legacy_rmmap_locked(dev, map);
 
 	mutex_unlock(&dev->struct_mutex);
 
@@ -599,7 +594,8 @@ static void drm_cleanup_buf_error(struct drm_device * dev,
  * reallocates the buffer list of the same size order to accommodate the new
  * buffers.
  */
-int drm_addbufs_agp(struct drm_device * dev, struct drm_buf_desc * request)
+int drm_legacy_addbufs_agp(struct drm_device *dev,
+			   struct drm_buf_desc *request)
 {
 	struct drm_device_dma *dma = dev->dma;
 	struct drm_buf_entry *entry;
@@ -759,10 +755,11 @@ int drm_addbufs_agp(struct drm_device * dev, struct drm_buf_desc * request)
 	atomic_dec(&dev->buf_alloc);
 	return 0;
 }
-EXPORT_SYMBOL(drm_addbufs_agp);
+EXPORT_SYMBOL(drm_legacy_addbufs_agp);
 #endif				/* __OS_HAS_AGP */
 
-int drm_addbufs_pci(struct drm_device * dev, struct drm_buf_desc * request)
+int drm_legacy_addbufs_pci(struct drm_device *dev,
+			   struct drm_buf_desc *request)
 {
 	struct drm_device_dma *dma = dev->dma;
 	int count;
@@ -964,9 +961,10 @@ int drm_addbufs_pci(struct drm_device * dev, struct drm_buf_desc * request)
 	return 0;
 
 }
-EXPORT_SYMBOL(drm_addbufs_pci);
+EXPORT_SYMBOL(drm_legacy_addbufs_pci);
 
-static int drm_addbufs_sg(struct drm_device * dev, struct drm_buf_desc * request)
+static int drm_legacy_addbufs_sg(struct drm_device *dev,
+				 struct drm_buf_desc *request)
 {
 	struct drm_device_dma *dma = dev->dma;
 	struct drm_buf_entry *entry;
@@ -1135,8 +1133,8 @@ static int drm_addbufs_sg(struct drm_device * dev, struct drm_buf_desc * request
  * addbufs_sg() or addbufs_pci() for AGP, scatter-gather or consistent
  * PCI memory respectively.
  */
-int drm_addbufs(struct drm_device *dev, void *data,
-		struct drm_file *file_priv)
+int drm_legacy_addbufs(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv)
 {
 	struct drm_buf_desc *request = data;
 	int ret;
@@ -1149,15 +1147,15 @@ int drm_addbufs(struct drm_device *dev, void *data,
 
 #if __OS_HAS_AGP
 	if (request->flags & _DRM_AGP_BUFFER)
-		ret = drm_addbufs_agp(dev, request);
+		ret = drm_legacy_addbufs_agp(dev, request);
 	else
 #endif
 	if (request->flags & _DRM_SG_BUFFER)
-		ret = drm_addbufs_sg(dev, request);
+		ret = drm_legacy_addbufs_sg(dev, request);
 	else if (request->flags & _DRM_FB_BUFFER)
 		ret = -EINVAL;
 	else
-		ret = drm_addbufs_pci(dev, request);
+		ret = drm_legacy_addbufs_pci(dev, request);
 
 	return ret;
 }
@@ -1179,8 +1177,8 @@ int drm_addbufs(struct drm_device *dev, void *data,
  * lock, preventing of allocating more buffers after this call. Information
  * about each requested buffer is then copied into user space.
  */
-int drm_infobufs(struct drm_device *dev, void *data,
-		 struct drm_file *file_priv)
+int drm_legacy_infobufs(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
 	struct drm_buf_info *request = data;
@@ -1260,8 +1258,8 @@ int drm_infobufs(struct drm_device *dev, void *data,
  *
  * \note This ioctl is deprecated and mostly never used.
  */
-int drm_markbufs(struct drm_device *dev, void *data,
-		 struct drm_file *file_priv)
+int drm_legacy_markbufs(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
 	struct drm_buf_desc *request = data;
@@ -1307,8 +1305,8 @@ int drm_markbufs(struct drm_device *dev, void *data,
  * Calls free_buffer() for each used buffer.
  * This function is primarily used for debugging.
  */
-int drm_freebufs(struct drm_device *dev, void *data,
-		 struct drm_file *file_priv)
+int drm_legacy_freebufs(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
 	struct drm_buf_free *request = data;
@@ -1340,7 +1338,7 @@ int drm_freebufs(struct drm_device *dev, void *data,
 				  task_pid_nr(current));
 			return -EINVAL;
 		}
-		drm_free_buffer(dev, buf);
+		drm_legacy_free_buffer(dev, buf);
 	}
 
 	return 0;
@@ -1360,8 +1358,8 @@ int drm_freebufs(struct drm_device *dev, void *data,
  * offset equal to 0, which drm_mmap() interpretes as PCI buffers and calls
  * drm_mmap_dma().
  */
-int drm_mapbufs(struct drm_device *dev, void *data,
-	        struct drm_file *file_priv)
+int drm_legacy_mapbufs(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
 	int retcode = 0;
@@ -1448,7 +1446,7 @@ int drm_mapbufs(struct drm_device *dev, void *data,
 	return retcode;
 }
 
-int drm_dma_ioctl(struct drm_device *dev, void *data,
+int drm_legacy_dma_ioctl(struct drm_device *dev, void *data,
 		  struct drm_file *file_priv)
 {
 	if (drm_core_check_feature(dev, DRIVER_MODESET))
@@ -1460,7 +1458,7 @@ int drm_dma_ioctl(struct drm_device *dev, void *data,
 		return -EINVAL;
 }
 
-struct drm_local_map *drm_getsarea(struct drm_device *dev)
+struct drm_local_map *drm_legacy_getsarea(struct drm_device *dev)
 {
 	struct drm_map_list *entry;
 
@@ -1472,4 +1470,4 @@ struct drm_local_map *drm_getsarea(struct drm_device *dev)
 	}
 	return NULL;
 }
-EXPORT_SYMBOL(drm_getsarea);
+EXPORT_SYMBOL(drm_legacy_getsarea);
diff --git a/drivers/gpu/drm/drm_crtc.c b/drivers/gpu/drm/drm_crtc.c
index fa2be249999c..e79c8d3700d8 100644
--- a/drivers/gpu/drm/drm_crtc.c
+++ b/drivers/gpu/drm/drm_crtc.c
@@ -40,106 +40,12 @@
 #include <drm/drm_modeset_lock.h>
 
 #include "drm_crtc_internal.h"
+#include "drm_internal.h"
 
 static struct drm_framebuffer *add_framebuffer_internal(struct drm_device *dev,
 							struct drm_mode_fb_cmd2 *r,
 							struct drm_file *file_priv);
 
-/**
- * drm_modeset_lock_all - take all modeset locks
- * @dev: drm device
- *
- * This function takes all modeset locks, suitable where a more fine-grained
- * scheme isn't (yet) implemented. Locks must be dropped with
- * drm_modeset_unlock_all.
- */
-void drm_modeset_lock_all(struct drm_device *dev)
-{
-	struct drm_mode_config *config = &dev->mode_config;
-	struct drm_modeset_acquire_ctx *ctx;
-	int ret;
-
-	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
-	if (WARN_ON(!ctx))
-		return;
-
-	mutex_lock(&config->mutex);
-
-	drm_modeset_acquire_init(ctx, 0);
-
-retry:
-	ret = drm_modeset_lock(&config->connection_mutex, ctx);
-	if (ret)
-		goto fail;
-	ret = drm_modeset_lock_all_crtcs(dev, ctx);
-	if (ret)
-		goto fail;
-
-	WARN_ON(config->acquire_ctx);
-
-	/* now we hold the locks, so now that it is safe, stash the
-	 * ctx for drm_modeset_unlock_all():
-	 */
-	config->acquire_ctx = ctx;
-
-	drm_warn_on_modeset_not_all_locked(dev);
-
-	return;
-
-fail:
-	if (ret == -EDEADLK) {
-		drm_modeset_backoff(ctx);
-		goto retry;
-	}
-}
-EXPORT_SYMBOL(drm_modeset_lock_all);
-
-/**
- * drm_modeset_unlock_all - drop all modeset locks
- * @dev: device
- *
- * This function drop all modeset locks taken by drm_modeset_lock_all.
- */
-void drm_modeset_unlock_all(struct drm_device *dev)
-{
-	struct drm_mode_config *config = &dev->mode_config;
-	struct drm_modeset_acquire_ctx *ctx = config->acquire_ctx;
-
-	if (WARN_ON(!ctx))
-		return;
-
-	config->acquire_ctx = NULL;
-	drm_modeset_drop_locks(ctx);
-	drm_modeset_acquire_fini(ctx);
-
-	kfree(ctx);
-
-	mutex_unlock(&dev->mode_config.mutex);
-}
-EXPORT_SYMBOL(drm_modeset_unlock_all);
-
-/**
- * drm_warn_on_modeset_not_all_locked - check that all modeset locks are locked
- * @dev: device
- *
- * Useful as a debug assert.
- */
-void drm_warn_on_modeset_not_all_locked(struct drm_device *dev)
-{
-	struct drm_crtc *crtc;
-
-	/* Locking is currently fubar in the panic handler. */
-	if (oops_in_progress)
-		return;
-
-	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
-		WARN_ON(!drm_modeset_is_locked(&crtc->mutex));
-
-	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
-	WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
-}
-EXPORT_SYMBOL(drm_warn_on_modeset_not_all_locked);
-
 /* Avoid boilerplate.  I'm tired of typing. */
 #define DRM_ENUM_NAME_FN(fnname, list)				\
 	const char *fnname(int val)				\
@@ -515,9 +421,6 @@ int drm_framebuffer_init(struct drm_device *dev, struct drm_framebuffer *fb,
 	if (ret)
 		goto out;
 
-	/* Grab the idr reference. */
-	drm_framebuffer_reference(fb);
-
 	dev->mode_config.num_fb++;
 	list_add(&fb->head, &dev->mode_config.fb_list);
 out:
@@ -527,10 +430,34 @@ out:
 }
 EXPORT_SYMBOL(drm_framebuffer_init);
 
+/* dev->mode_config.fb_lock must be held! */
+static void __drm_framebuffer_unregister(struct drm_device *dev,
+					 struct drm_framebuffer *fb)
+{
+	mutex_lock(&dev->mode_config.idr_mutex);
+	idr_remove(&dev->mode_config.crtc_idr, fb->base.id);
+	mutex_unlock(&dev->mode_config.idr_mutex);
+
+	fb->base.id = 0;
+}
+
 static void drm_framebuffer_free(struct kref *kref)
 {
 	struct drm_framebuffer *fb =
 			container_of(kref, struct drm_framebuffer, refcount);
+	struct drm_device *dev = fb->dev;
+
+	/*
+	 * The lookup idr holds a weak reference, which has not necessarily been
+	 * removed at this point. Check for that.
+	 */
+	mutex_lock(&dev->mode_config.fb_lock);
+	if (fb->base.id) {
+		/* Mark fb as reaped and drop idr ref. */
+		__drm_framebuffer_unregister(dev, fb);
+	}
+	mutex_unlock(&dev->mode_config.fb_lock);
+
 	fb->funcs->destroy(fb);
 }
 
@@ -567,8 +494,10 @@ struct drm_framebuffer *drm_framebuffer_lookup(struct drm_device *dev,
 
 	mutex_lock(&dev->mode_config.fb_lock);
 	fb = __drm_framebuffer_lookup(dev, id);
-	if (fb)
-		drm_framebuffer_reference(fb);
+	if (fb) {
+		if (!kref_get_unless_zero(&fb->refcount))
+			fb = NULL;
+	}
 	mutex_unlock(&dev->mode_config.fb_lock);
 
 	return fb;
@@ -612,19 +541,6 @@ static void __drm_framebuffer_unreference(struct drm_framebuffer *fb)
 	kref_put(&fb->refcount, drm_framebuffer_free_bug);
 }
 
-/* dev->mode_config.fb_lock must be held! */
-static void __drm_framebuffer_unregister(struct drm_device *dev,
-					 struct drm_framebuffer *fb)
-{
-	mutex_lock(&dev->mode_config.idr_mutex);
-	idr_remove(&dev->mode_config.crtc_idr, fb->base.id);
-	mutex_unlock(&dev->mode_config.idr_mutex);
-
-	fb->base.id = 0;
-
-	__drm_framebuffer_unreference(fb);
-}
-
 /**
  * drm_framebuffer_unregister_private - unregister a private fb from the lookup idr
  * @fb: fb to unregister
@@ -764,11 +680,7 @@ int drm_crtc_init_with_planes(struct drm_device *dev, struct drm_crtc *crtc,
 	crtc->funcs = funcs;
 	crtc->invert_dimensions = false;
 
-	drm_modeset_lock_all(dev);
 	drm_modeset_lock_init(&crtc->mutex);
-	/* dropped by _unlock_all(): */
-	drm_modeset_lock(&crtc->mutex, config->acquire_ctx);
-
 	ret = drm_mode_object_get(dev, &crtc->base, DRM_MODE_OBJECT_CRTC);
 	if (ret)
 		goto out;
@@ -786,7 +698,6 @@ int drm_crtc_init_with_planes(struct drm_device *dev, struct drm_crtc *crtc,
 		cursor->possible_crtcs = 1 << drm_crtc_index(crtc);
 
  out:
-	drm_modeset_unlock_all(dev);
 
 	return ret;
 }
@@ -853,6 +764,59 @@ static void drm_mode_remove(struct drm_connector *connector,
 }
 
 /**
+ * drm_connector_get_cmdline_mode - reads the user's cmdline mode
+ * @connector: connector to quwery
+ * @mode: returned mode
+ *
+ * The kernel supports per-connector configration of its consoles through
+ * use of the video= parameter. This function parses that option and
+ * extracts the user's specified mode (or enable/disable status) for a
+ * particular connector. This is typically only used during the early fbdev
+ * setup.
+ */
+static void drm_connector_get_cmdline_mode(struct drm_connector *connector)
+{
+	struct drm_cmdline_mode *mode = &connector->cmdline_mode;
+	char *option = NULL;
+
+	if (fb_get_options(connector->name, &option))
+		return;
+
+	if (!drm_mode_parse_command_line_for_connector(option,
+						       connector,
+						       mode))
+		return;
+
+	if (mode->force) {
+		const char *s;
+
+		switch (mode->force) {
+		case DRM_FORCE_OFF:
+			s = "OFF";
+			break;
+		case DRM_FORCE_ON_DIGITAL:
+			s = "ON - dig";
+			break;
+		default:
+		case DRM_FORCE_ON:
+			s = "ON";
+			break;
+		}
+
+		DRM_INFO("forcing %s connector %s\n", connector->name, s);
+		connector->force = mode->force;
+	}
+
+	DRM_DEBUG_KMS("cmdline mode for connector %s %dx%d@%dHz%s%s%s\n",
+		      connector->name,
+		      mode->xres, mode->yres,
+		      mode->refresh_specified ? mode->refresh : 60,
+		      mode->rb ? " reduced blanking" : "",
+		      mode->margins ? " with margins" : "",
+		      mode->interlace ?  " interlaced" : "");
+}
+
+/**
  * drm_connector_init - Init a preallocated connector
  * @dev: DRM device
  * @connector: the connector to init
@@ -904,6 +868,8 @@ int drm_connector_init(struct drm_device *dev,
 	connector->edid_blob_ptr = NULL;
 	connector->status = connector_status_unknown;
 
+	drm_connector_get_cmdline_mode(connector);
+
 	list_add_tail(&connector->head, &dev->mode_config.connector_list);
 	dev->mode_config.num_connector++;
 
@@ -957,6 +923,29 @@ void drm_connector_cleanup(struct drm_connector *connector)
 EXPORT_SYMBOL(drm_connector_cleanup);
 
 /**
+ * drm_connector_index - find the index of a registered connector
+ * @connector: connector to find index for
+ *
+ * Given a registered connector, return the index of that connector within a DRM
+ * device's list of connectors.
+ */
+unsigned int drm_connector_index(struct drm_connector *connector)
+{
+	unsigned int index = 0;
+	struct drm_connector *tmp;
+
+	list_for_each_entry(tmp, &connector->dev->mode_config.connector_list, head) {
+		if (tmp == connector)
+			return index;
+
+		index++;
+	}
+
+	BUG();
+}
+EXPORT_SYMBOL(drm_connector_index);
+
+/**
  * drm_connector_register - register a connector
  * @connector: the connector to register
  *
@@ -1261,6 +1250,29 @@ void drm_plane_cleanup(struct drm_plane *plane)
 EXPORT_SYMBOL(drm_plane_cleanup);
 
 /**
+ * drm_plane_index - find the index of a registered plane
+ * @plane: plane to find index for
+ *
+ * Given a registered plane, return the index of that CRTC within a DRM
+ * device's list of planes.
+ */
+unsigned int drm_plane_index(struct drm_plane *plane)
+{
+	unsigned int index = 0;
+	struct drm_plane *tmp;
+
+	list_for_each_entry(tmp, &plane->dev->mode_config.plane_list, head) {
+		if (tmp == plane)
+			return index;
+
+		index++;
+	}
+
+	BUG();
+}
+EXPORT_SYMBOL(drm_plane_index);
+
+/**
  * drm_plane_force_disable - Forcibly disable a plane
  * @plane: plane to disable
  *
@@ -1271,19 +1283,21 @@ EXPORT_SYMBOL(drm_plane_cleanup);
  */
 void drm_plane_force_disable(struct drm_plane *plane)
 {
-	struct drm_framebuffer *old_fb = plane->fb;
 	int ret;
 
-	if (!old_fb)
+	if (!plane->fb)
 		return;
 
+	plane->old_fb = plane->fb;
 	ret = plane->funcs->disable_plane(plane);
 	if (ret) {
 		DRM_ERROR("failed to disable plane with busy fb\n");
+		plane->old_fb = NULL;
 		return;
 	}
 	/* disconnect the plane from the fb and crtc: */
-	__drm_framebuffer_unreference(old_fb);
+	__drm_framebuffer_unreference(plane->old_fb);
+	plane->old_fb = NULL;
 	plane->fb = NULL;
 	plane->crtc = NULL;
 }
@@ -2249,33 +2263,29 @@ out:
  *
  * src_{x,y,w,h} are provided in 16.16 fixed point format
  */
-static int setplane_internal(struct drm_plane *plane,
-			     struct drm_crtc *crtc,
-			     struct drm_framebuffer *fb,
-			     int32_t crtc_x, int32_t crtc_y,
-			     uint32_t crtc_w, uint32_t crtc_h,
-			     /* src_{x,y,w,h} values are 16.16 fixed point */
-			     uint32_t src_x, uint32_t src_y,
-			     uint32_t src_w, uint32_t src_h)
+static int __setplane_internal(struct drm_plane *plane,
+			       struct drm_crtc *crtc,
+			       struct drm_framebuffer *fb,
+			       int32_t crtc_x, int32_t crtc_y,
+			       uint32_t crtc_w, uint32_t crtc_h,
+			       /* src_{x,y,w,h} values are 16.16 fixed point */
+			       uint32_t src_x, uint32_t src_y,
+			       uint32_t src_w, uint32_t src_h)
 {
-	struct drm_device *dev = plane->dev;
-	struct drm_framebuffer *old_fb = NULL;
 	int ret = 0;
 	unsigned int fb_width, fb_height;
 	int i;
 
 	/* No fb means shut it down */
 	if (!fb) {
-		drm_modeset_lock_all(dev);
-		old_fb = plane->fb;
+		plane->old_fb = plane->fb;
 		ret = plane->funcs->disable_plane(plane);
 		if (!ret) {
 			plane->crtc = NULL;
 			plane->fb = NULL;
 		} else {
-			old_fb = NULL;
+			plane->old_fb = NULL;
 		}
-		drm_modeset_unlock_all(dev);
 		goto out;
 	}
 
@@ -2315,8 +2325,7 @@ static int setplane_internal(struct drm_plane *plane,
 		goto out;
 	}
 
-	drm_modeset_lock_all(dev);
-	old_fb = plane->fb;
+	plane->old_fb = plane->fb;
 	ret = plane->funcs->update_plane(plane, crtc, fb,
 					 crtc_x, crtc_y, crtc_w, crtc_h,
 					 src_x, src_y, src_w, src_h);
@@ -2325,18 +2334,37 @@ static int setplane_internal(struct drm_plane *plane,
 		plane->fb = fb;
 		fb = NULL;
 	} else {
-		old_fb = NULL;
+		plane->old_fb = NULL;
 	}
-	drm_modeset_unlock_all(dev);
 
 out:
 	if (fb)
 		drm_framebuffer_unreference(fb);
-	if (old_fb)
-		drm_framebuffer_unreference(old_fb);
+	if (plane->old_fb)
+		drm_framebuffer_unreference(plane->old_fb);
+	plane->old_fb = NULL;
 
 	return ret;
+}
+
+static int setplane_internal(struct drm_plane *plane,
+			     struct drm_crtc *crtc,
+			     struct drm_framebuffer *fb,
+			     int32_t crtc_x, int32_t crtc_y,
+			     uint32_t crtc_w, uint32_t crtc_h,
+			     /* src_{x,y,w,h} values are 16.16 fixed point */
+			     uint32_t src_x, uint32_t src_y,
+			     uint32_t src_w, uint32_t src_h)
+{
+	int ret;
+
+	drm_modeset_lock_all(plane->dev);
+	ret = __setplane_internal(plane, crtc, fb,
+				  crtc_x, crtc_y, crtc_w, crtc_h,
+				  src_x, src_y, src_w, src_h);
+	drm_modeset_unlock_all(plane->dev);
 
+	return ret;
 }
 
 /**
@@ -2440,7 +2468,7 @@ int drm_mode_set_config_internal(struct drm_mode_set *set)
 	 * crtcs. Atomic modeset will have saner semantics ...
 	 */
 	list_for_each_entry(tmp, &crtc->dev->mode_config.crtc_list, head)
-		tmp->old_fb = tmp->primary->fb;
+		tmp->primary->old_fb = tmp->primary->fb;
 
 	fb = set->fb;
 
@@ -2453,8 +2481,9 @@ int drm_mode_set_config_internal(struct drm_mode_set *set)
 	list_for_each_entry(tmp, &crtc->dev->mode_config.crtc_list, head) {
 		if (tmp->primary->fb)
 			drm_framebuffer_reference(tmp->primary->fb);
-		if (tmp->old_fb)
-			drm_framebuffer_unreference(tmp->old_fb);
+		if (tmp->primary->old_fb)
+			drm_framebuffer_unreference(tmp->primary->old_fb);
+		tmp->primary->old_fb = NULL;
 	}
 
 	return ret;
@@ -2701,6 +2730,7 @@ static int drm_mode_cursor_universal(struct drm_crtc *crtc,
 	int ret = 0;
 
 	BUG_ON(!crtc->cursor);
+	WARN_ON(crtc->cursor->crtc != crtc && crtc->cursor->crtc != NULL);
 
 	/*
 	 * Obtain fb we'll be using (either new or existing) and take an extra
@@ -2720,11 +2750,9 @@ static int drm_mode_cursor_universal(struct drm_crtc *crtc,
 			fb = NULL;
 		}
 	} else {
-		mutex_lock(&dev->mode_config.mutex);
 		fb = crtc->cursor->fb;
 		if (fb)
 			drm_framebuffer_reference(fb);
-		mutex_unlock(&dev->mode_config.mutex);
 	}
 
 	if (req->flags & DRM_MODE_CURSOR_MOVE) {
@@ -2746,7 +2774,7 @@ static int drm_mode_cursor_universal(struct drm_crtc *crtc,
 	 * setplane_internal will take care of deref'ing either the old or new
 	 * framebuffer depending on success.
 	 */
-	ret = setplane_internal(crtc->cursor, crtc, fb,
+	ret = __setplane_internal(crtc->cursor, crtc, fb,
 				crtc_x, crtc_y, crtc_w, crtc_h,
 				0, 0, src_w, src_h);
 
@@ -2782,10 +2810,12 @@ static int drm_mode_cursor_common(struct drm_device *dev,
 	 * If this crtc has a universal cursor plane, call that plane's update
 	 * handler rather than using legacy cursor handlers.
 	 */
-	if (crtc->cursor)
-		return drm_mode_cursor_universal(crtc, req, file_priv);
+	drm_modeset_lock_crtc(crtc);
+	if (crtc->cursor) {
+		ret = drm_mode_cursor_universal(crtc, req, file_priv);
+		goto out;
+	}
 
-	drm_modeset_lock(&crtc->mutex, NULL);
 	if (req->flags & DRM_MODE_CURSOR_BO) {
 		if (!crtc->funcs->cursor_set && !crtc->funcs->cursor_set2) {
 			ret = -ENXIO;
@@ -2809,7 +2839,7 @@ static int drm_mode_cursor_common(struct drm_device *dev,
 		}
 	}
 out:
-	drm_modeset_unlock(&crtc->mutex);
+	drm_modeset_unlock_crtc(crtc);
 
 	return ret;
 
@@ -3370,7 +3400,16 @@ void drm_fb_release(struct drm_file *priv)
 	struct drm_device *dev = priv->minor->dev;
 	struct drm_framebuffer *fb, *tfb;
 
-	mutex_lock(&priv->fbs_lock);
+	/*
+	 * When the file gets released that means no one else can access the fb
+	 * list any more, so no need to grab fpriv->fbs_lock. And we need to to
+	 * avoid upsetting lockdep since the universal cursor code adds a
+	 * framebuffer while holding mutex locks.
+	 *
+	 * Note that a real deadlock between fpriv->fbs_lock and the modeset
+	 * locks is impossible here since no one else but this function can get
+	 * at it any more.
+	 */
 	list_for_each_entry_safe(fb, tfb, &priv->fbs, filp_head) {
 
 		mutex_lock(&dev->mode_config.fb_lock);
@@ -3383,7 +3422,6 @@ void drm_fb_release(struct drm_file *priv)
 		/* This will also drop the fpriv->fbs reference. */
 		drm_framebuffer_remove(fb);
 	}
-	mutex_unlock(&priv->fbs_lock);
 }
 
 /**
@@ -3495,9 +3533,10 @@ EXPORT_SYMBOL(drm_property_create_enum);
  * @flags: flags specifying the property type
  * @name: name of the property
  * @props: enumeration lists with property bitflags
- * @num_values: number of pre-defined values
+ * @num_props: size of the @props array
+ * @supported_bits: bitmask of all supported enumeration values
  *
- * This creates a new generic drm property which can then be attached to a drm
+ * This creates a new bitmask drm property which can then be attached to a drm
  * object with drm_object_attach_property. The returned property object must be
  * freed with drm_property_destroy.
  *
@@ -4157,12 +4196,25 @@ static int drm_mode_crtc_set_obj_prop(struct drm_mode_object *obj,
 	return ret;
 }
 
-static int drm_mode_plane_set_obj_prop(struct drm_mode_object *obj,
-				      struct drm_property *property,
-				      uint64_t value)
+/**
+ * drm_mode_plane_set_obj_prop - set the value of a property
+ * @plane: drm plane object to set property value for
+ * @property: property to set
+ * @value: value the property should be set to
+ *
+ * This functions sets a given property on a given plane object. This function
+ * calls the driver's ->set_property callback and changes the software state of
+ * the property if the callback succeeds.
+ *
+ * Returns:
+ * Zero on success, error code on failure.
+ */
+int drm_mode_plane_set_obj_prop(struct drm_plane *plane,
+				struct drm_property *property,
+				uint64_t value)
 {
 	int ret = -EINVAL;
-	struct drm_plane *plane = obj_to_plane(obj);
+	struct drm_mode_object *obj = &plane->base;
 
 	if (plane->funcs->set_property)
 		ret = plane->funcs->set_property(plane, property, value);
@@ -4171,6 +4223,7 @@ static int drm_mode_plane_set_obj_prop(struct drm_mode_object *obj,
 
 	return ret;
 }
+EXPORT_SYMBOL(drm_mode_plane_set_obj_prop);
 
 /**
  * drm_mode_getproperty_ioctl - get the current value of a object's property
@@ -4309,7 +4362,8 @@ int drm_mode_obj_set_property_ioctl(struct drm_device *dev, void *data,
 		ret = drm_mode_crtc_set_obj_prop(arg_obj, property, arg->value);
 		break;
 	case DRM_MODE_OBJECT_PLANE:
-		ret = drm_mode_plane_set_obj_prop(arg_obj, property, arg->value);
+		ret = drm_mode_plane_set_obj_prop(obj_to_plane(arg_obj),
+						  property, arg->value);
 		break;
 	}
 
@@ -4529,7 +4583,7 @@ int drm_mode_page_flip_ioctl(struct drm_device *dev,
 {
 	struct drm_mode_crtc_page_flip *page_flip = data;
 	struct drm_crtc *crtc;
-	struct drm_framebuffer *fb = NULL, *old_fb = NULL;
+	struct drm_framebuffer *fb = NULL;
 	struct drm_pending_vblank_event *e = NULL;
 	unsigned long flags;
 	int ret = -EINVAL;
@@ -4545,7 +4599,7 @@ int drm_mode_page_flip_ioctl(struct drm_device *dev,
 	if (!crtc)
 		return -ENOENT;
 
-	drm_modeset_lock(&crtc->mutex, NULL);
+	drm_modeset_lock_crtc(crtc);
 	if (crtc->primary->fb == NULL) {
 		/* The framebuffer is currently unbound, presumably
 		 * due to a hotplug event, that userspace has not
@@ -4601,7 +4655,7 @@ int drm_mode_page_flip_ioctl(struct drm_device *dev,
 			(void (*) (struct drm_pending_event *)) kfree;
 	}
 
-	old_fb = crtc->primary->fb;
+	crtc->primary->old_fb = crtc->primary->fb;
 	ret = crtc->funcs->page_flip(crtc, fb, e, page_flip->flags);
 	if (ret) {
 		if (page_flip->flags & DRM_MODE_PAGE_FLIP_EVENT) {
@@ -4611,7 +4665,7 @@ int drm_mode_page_flip_ioctl(struct drm_device *dev,
 			kfree(e);
 		}
 		/* Keep the old fb, don't unref it. */
-		old_fb = NULL;
+		crtc->primary->old_fb = NULL;
 	} else {
 		/*
 		 * Warn if the driver hasn't properly updated the crtc->fb
@@ -4627,9 +4681,10 @@ int drm_mode_page_flip_ioctl(struct drm_device *dev,
 out:
 	if (fb)
 		drm_framebuffer_unreference(fb);
-	if (old_fb)
-		drm_framebuffer_unreference(old_fb);
-	drm_modeset_unlock(&crtc->mutex);
+	if (crtc->primary->old_fb)
+		drm_framebuffer_unreference(crtc->primary->old_fb);
+	crtc->primary->old_fb = NULL;
+	drm_modeset_unlock_crtc(crtc);
 
 	return ret;
 }
@@ -4645,9 +4700,14 @@ out:
 void drm_mode_config_reset(struct drm_device *dev)
 {
 	struct drm_crtc *crtc;
+	struct drm_plane *plane;
 	struct drm_encoder *encoder;
 	struct drm_connector *connector;
 
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head)
+		if (plane->funcs->reset)
+			plane->funcs->reset(plane);
+
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
 		if (crtc->funcs->reset)
 			crtc->funcs->reset(crtc);
@@ -4696,8 +4756,9 @@ int drm_mode_create_dumb_ioctl(struct drm_device *dev,
 		return -EINVAL;
 
 	/* overflow checks for 32bit size calculations */
+	/* NOTE: DIV_ROUND_UP() can overflow */
 	cpp = DIV_ROUND_UP(args->bpp, 8);
-	if (cpp > 0xffffffffU / args->width)
+	if (!cpp || cpp > 0xffffffffU / args->width)
 		return -EINVAL;
 	stride = cpp * args->width;
 	if (args->height > 0xffffffffU / stride)
diff --git a/drivers/gpu/drm/drm_debugfs.c b/drivers/gpu/drm/drm_debugfs.c
index 13bd42923dd4..3bcf8e6a85b3 100644
--- a/drivers/gpu/drm/drm_debugfs.c
+++ b/drivers/gpu/drm/drm_debugfs.c
@@ -36,6 +36,7 @@
 #include <linux/export.h>
 #include <drm/drmP.h>
 #include <drm/drm_edid.h>
+#include "drm_internal.h"
 
 #if defined(CONFIG_DEBUG_FS)
 
@@ -49,9 +50,7 @@ static const struct drm_info_list drm_debugfs_list[] = {
 	{"clients", drm_clients_info, 0},
 	{"bufs", drm_bufs_info, 0},
 	{"gem_names", drm_gem_name_info, DRIVER_GEM},
-#if DRM_DEBUG_CODE
 	{"vma", drm_vma_info, 0},
-#endif
 };
 #define DRM_DEBUGFS_ENTRIES ARRAY_SIZE(drm_debugfs_list)
 
diff --git a/drivers/gpu/drm/drm_dma.c b/drivers/gpu/drm/drm_dma.c
index 8a140a953754..ea481800ef56 100644
--- a/drivers/gpu/drm/drm_dma.c
+++ b/drivers/gpu/drm/drm_dma.c
@@ -35,6 +35,7 @@
 
 #include <linux/export.h>
 #include <drm/drmP.h>
+#include "drm_legacy.h"
 
 /**
  * Initialize the DMA data.
@@ -124,7 +125,7 @@ void drm_legacy_dma_takedown(struct drm_device *dev)
  *
  * Resets the fields of \p buf.
  */
-void drm_free_buffer(struct drm_device *dev, struct drm_buf * buf)
+void drm_legacy_free_buffer(struct drm_device *dev, struct drm_buf * buf)
 {
 	if (!buf)
 		return;
@@ -142,8 +143,8 @@ void drm_free_buffer(struct drm_device *dev, struct drm_buf * buf)
  *
  * Frees each buffer associated with \p file_priv not already on the hardware.
  */
-void drm_core_reclaim_buffers(struct drm_device *dev,
-			      struct drm_file *file_priv)
+void drm_legacy_reclaim_buffers(struct drm_device *dev,
+				struct drm_file *file_priv)
 {
 	struct drm_device_dma *dma = dev->dma;
 	int i;
@@ -154,7 +155,7 @@ void drm_core_reclaim_buffers(struct drm_device *dev,
 		if (dma->buflist[i]->file_priv == file_priv) {
 			switch (dma->buflist[i]->list) {
 			case DRM_LIST_NONE:
-				drm_free_buffer(dev, dma->buflist[i]);
+				drm_legacy_free_buffer(dev, dma->buflist[i]);
 				break;
 			case DRM_LIST_WAIT:
 				dma->buflist[i]->list = DRM_LIST_RECLAIM;
@@ -166,5 +167,3 @@ void drm_core_reclaim_buffers(struct drm_device *dev,
 		}
 	}
 }
-
-EXPORT_SYMBOL(drm_core_reclaim_buffers);
diff --git a/drivers/gpu/drm/drm_dp_mst_topology.c b/drivers/gpu/drm/drm_dp_mst_topology.c
index ac3c2738db94..070f913d2dba 100644
--- a/drivers/gpu/drm/drm_dp_mst_topology.c
+++ b/drivers/gpu/drm/drm_dp_mst_topology.c
@@ -682,7 +682,7 @@ static int build_allocate_payload(struct drm_dp_sideband_msg_tx *msg, int port_n
 static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
 					struct drm_dp_vcpi *vcpi)
 {
-	int ret;
+	int ret, vcpi_ret;
 
 	mutex_lock(&mgr->payload_lock);
 	ret = find_first_zero_bit(&mgr->payload_mask, mgr->max_payloads + 1);
@@ -692,8 +692,16 @@ static int drm_dp_mst_assign_payload_id(struct drm_dp_mst_topology_mgr *mgr,
 		goto out_unlock;
 	}
 
+	vcpi_ret = find_first_zero_bit(&mgr->vcpi_mask, mgr->max_payloads + 1);
+	if (vcpi_ret > mgr->max_payloads) {
+		ret = -EINVAL;
+		DRM_DEBUG_KMS("out of vcpi ids %d\n", ret);
+		goto out_unlock;
+	}
+
 	set_bit(ret, &mgr->payload_mask);
-	vcpi->vcpi = ret;
+	set_bit(vcpi_ret, &mgr->vcpi_mask);
+	vcpi->vcpi = vcpi_ret + 1;
 	mgr->proposed_vcpis[ret - 1] = vcpi;
 out_unlock:
 	mutex_unlock(&mgr->payload_lock);
@@ -701,15 +709,23 @@ out_unlock:
 }
 
 static void drm_dp_mst_put_payload_id(struct drm_dp_mst_topology_mgr *mgr,
-				      int id)
+				      int vcpi)
 {
-	if (id == 0)
+	int i;
+	if (vcpi == 0)
 		return;
 
 	mutex_lock(&mgr->payload_lock);
-	DRM_DEBUG_KMS("putting payload %d\n", id);
-	clear_bit(id, &mgr->payload_mask);
-	mgr->proposed_vcpis[id - 1] = NULL;
+	DRM_DEBUG_KMS("putting payload %d\n", vcpi);
+	clear_bit(vcpi - 1, &mgr->vcpi_mask);
+
+	for (i = 0; i < mgr->max_payloads; i++) {
+		if (mgr->proposed_vcpis[i])
+			if (mgr->proposed_vcpis[i]->vcpi == vcpi) {
+				mgr->proposed_vcpis[i] = NULL;
+				clear_bit(i + 1, &mgr->payload_mask);
+			}
+	}
 	mutex_unlock(&mgr->payload_lock);
 }
 
@@ -1563,7 +1579,7 @@ static int drm_dp_destroy_payload_step1(struct drm_dp_mst_topology_mgr *mgr,
 	}
 
 	drm_dp_dpcd_write_payload(mgr, id, payload);
-	payload->payload_state = 0;
+	payload->payload_state = DP_PAYLOAD_DELETE_LOCAL;
 	return 0;
 }
 
@@ -1590,7 +1606,7 @@ static int drm_dp_destroy_payload_step2(struct drm_dp_mst_topology_mgr *mgr,
  */
 int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
 {
-	int i;
+	int i, j;
 	int cur_slots = 1;
 	struct drm_dp_payload req_payload;
 	struct drm_dp_mst_port *port;
@@ -1607,26 +1623,46 @@ int drm_dp_update_payload_part1(struct drm_dp_mst_topology_mgr *mgr)
 			port = NULL;
 			req_payload.num_slots = 0;
 		}
+
+		if (mgr->payloads[i].start_slot != req_payload.start_slot) {
+			mgr->payloads[i].start_slot = req_payload.start_slot;
+		}
 		/* work out what is required to happen with this payload */
-		if (mgr->payloads[i].start_slot != req_payload.start_slot ||
-		    mgr->payloads[i].num_slots != req_payload.num_slots) {
+		if (mgr->payloads[i].num_slots != req_payload.num_slots) {
 
 			/* need to push an update for this payload */
 			if (req_payload.num_slots) {
-				drm_dp_create_payload_step1(mgr, i + 1, &req_payload);
+				drm_dp_create_payload_step1(mgr, mgr->proposed_vcpis[i]->vcpi, &req_payload);
 				mgr->payloads[i].num_slots = req_payload.num_slots;
 			} else if (mgr->payloads[i].num_slots) {
 				mgr->payloads[i].num_slots = 0;
-				drm_dp_destroy_payload_step1(mgr, port, i + 1, &mgr->payloads[i]);
+				drm_dp_destroy_payload_step1(mgr, port, port->vcpi.vcpi, &mgr->payloads[i]);
 				req_payload.payload_state = mgr->payloads[i].payload_state;
-			} else
-				req_payload.payload_state = 0;
-
-			mgr->payloads[i].start_slot = req_payload.start_slot;
+				mgr->payloads[i].start_slot = 0;
+			}
 			mgr->payloads[i].payload_state = req_payload.payload_state;
 		}
 		cur_slots += req_payload.num_slots;
 	}
+
+	for (i = 0; i < mgr->max_payloads; i++) {
+		if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
+			DRM_DEBUG_KMS("removing payload %d\n", i);
+			for (j = i; j < mgr->max_payloads - 1; j++) {
+				memcpy(&mgr->payloads[j], &mgr->payloads[j + 1], sizeof(struct drm_dp_payload));
+				mgr->proposed_vcpis[j] = mgr->proposed_vcpis[j + 1];
+				if (mgr->proposed_vcpis[j] && mgr->proposed_vcpis[j]->num_slots) {
+					set_bit(j + 1, &mgr->payload_mask);
+				} else {
+					clear_bit(j + 1, &mgr->payload_mask);
+				}
+			}
+			memset(&mgr->payloads[mgr->max_payloads - 1], 0, sizeof(struct drm_dp_payload));
+			mgr->proposed_vcpis[mgr->max_payloads - 1] = NULL;
+			clear_bit(mgr->max_payloads, &mgr->payload_mask);
+
+		}
+	}
 	mutex_unlock(&mgr->payload_lock);
 
 	return 0;
@@ -1657,9 +1693,9 @@ int drm_dp_update_payload_part2(struct drm_dp_mst_topology_mgr *mgr)
 
 		DRM_DEBUG_KMS("payload %d %d\n", i, mgr->payloads[i].payload_state);
 		if (mgr->payloads[i].payload_state == DP_PAYLOAD_LOCAL) {
-			ret = drm_dp_create_payload_step2(mgr, port, i + 1, &mgr->payloads[i]);
+			ret = drm_dp_create_payload_step2(mgr, port, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
 		} else if (mgr->payloads[i].payload_state == DP_PAYLOAD_DELETE_LOCAL) {
-			ret = drm_dp_destroy_payload_step2(mgr, i + 1, &mgr->payloads[i]);
+			ret = drm_dp_destroy_payload_step2(mgr, mgr->proposed_vcpis[i]->vcpi, &mgr->payloads[i]);
 		}
 		if (ret) {
 			mutex_unlock(&mgr->payload_lock);
@@ -1772,7 +1808,7 @@ static int drm_dp_get_vc_payload_bw(int dp_link_bw, int dp_link_count)
 	case DP_LINK_BW_5_4:
 		return 10 * dp_link_count;
 	}
-	return 0;
+	BUG();
 }
 
 /**
@@ -1861,6 +1897,7 @@ int drm_dp_mst_topology_mgr_set_mst(struct drm_dp_mst_topology_mgr *mgr, bool ms
 		memset(mgr->payloads, 0, mgr->max_payloads * sizeof(struct drm_dp_payload));
 		mgr->payload_mask = 0;
 		set_bit(0, &mgr->payload_mask);
+		mgr->vcpi_mask = 0;
 	}
 
 out_unlock:
@@ -2071,6 +2108,7 @@ static int drm_dp_mst_handle_up_req(struct drm_dp_mst_topology_mgr *mgr)
  * drm_dp_mst_hpd_irq() - MST hotplug IRQ notify
  * @mgr: manager to notify irq for.
  * @esi: 4 bytes from SINK_COUNT_ESI
+ * @handled: whether the hpd interrupt was consumed or not
  *
  * This should be called from the driver when it detects a short IRQ,
  * along with the value of the DEVICE_SERVICE_IRQ_VECTOR_ESI0. The
@@ -2474,7 +2512,7 @@ void drm_dp_mst_dump_topology(struct seq_file *m,
 	mutex_unlock(&mgr->lock);
 
 	mutex_lock(&mgr->payload_lock);
-	seq_printf(m, "vcpi: %lx\n", mgr->payload_mask);
+	seq_printf(m, "vcpi: %lx %lx\n", mgr->payload_mask, mgr->vcpi_mask);
 
 	for (i = 0; i < mgr->max_payloads; i++) {
 		if (mgr->proposed_vcpis[i]) {
diff --git a/drivers/gpu/drm/drm_drv.c b/drivers/gpu/drm/drm_drv.c
index 3242e208c0d0..bc3da32d4585 100644
--- a/drivers/gpu/drm/drm_drv.c
+++ b/drivers/gpu/drm/drm_drv.c
@@ -35,32 +35,20 @@
 #include <drm/drmP.h>
 #include <drm/drm_core.h>
 #include "drm_legacy.h"
+#include "drm_internal.h"
 
 unsigned int drm_debug = 0;	/* 1 to enable debug output */
 EXPORT_SYMBOL(drm_debug);
 
-unsigned int drm_vblank_offdelay = 5000;    /* Default to 5000 msecs. */
-
-unsigned int drm_timestamp_precision = 20;  /* Default to 20 usecs. */
-
-/*
- * Default to use monotonic timestamps for wait-for-vblank and page-flip
- * complete events.
- */
-unsigned int drm_timestamp_monotonic = 1;
-
 MODULE_AUTHOR(CORE_AUTHOR);
 MODULE_DESCRIPTION(CORE_DESC);
 MODULE_LICENSE("GPL and additional rights");
 MODULE_PARM_DESC(debug, "Enable debug output");
-MODULE_PARM_DESC(vblankoffdelay, "Delay until vblank irq auto-disable [msecs]");
+MODULE_PARM_DESC(vblankoffdelay, "Delay until vblank irq auto-disable [msecs] (0: never disable, <0: disable immediately)");
 MODULE_PARM_DESC(timestamp_precision_usec, "Max. error on timestamps [usecs]");
 MODULE_PARM_DESC(timestamp_monotonic, "Use monotonic timestamps");
 
 module_param_named(debug, drm_debug, int, 0600);
-module_param_named(vblankoffdelay, drm_vblank_offdelay, int, 0600);
-module_param_named(timestamp_precision_usec, drm_timestamp_precision, int, 0600);
-module_param_named(timestamp_monotonic, drm_timestamp_monotonic, int, 0600);
 
 static DEFINE_SPINLOCK(drm_minor_lock);
 static struct idr drm_minors_idr;
@@ -68,22 +56,19 @@ static struct idr drm_minors_idr;
 struct class *drm_class;
 static struct dentry *drm_debugfs_root;
 
-int drm_err(const char *func, const char *format, ...)
+void drm_err(const char *func, const char *format, ...)
 {
 	struct va_format vaf;
 	va_list args;
-	int r;
 
 	va_start(args, format);
 
 	vaf.fmt = format;
 	vaf.va = &args;
 
-	r = printk(KERN_ERR "[" DRM_NAME ":%s] *ERROR* %pV", func, &vaf);
+	printk(KERN_ERR "[" DRM_NAME ":%s] *ERROR* %pV", func, &vaf);
 
 	va_end(args);
-
-	return r;
 }
 EXPORT_SYMBOL(drm_err);
 
@@ -102,6 +87,8 @@ void drm_ut_debug_printk(const char *function_name, const char *format, ...)
 }
 EXPORT_SYMBOL(drm_ut_debug_printk);
 
+#define DRM_MAGIC_HASH_ORDER  4  /**< Size of key hash table. Must be power of 2. */
+
 struct drm_master *drm_master_create(struct drm_minor *minor)
 {
 	struct drm_master *master;
@@ -133,7 +120,6 @@ EXPORT_SYMBOL(drm_master_get);
 static void drm_master_destroy(struct kref *kref)
 {
 	struct drm_master *master = container_of(kref, struct drm_master, refcount);
-	struct drm_magic_entry *pt, *next;
 	struct drm_device *dev = master->minor->dev;
 	struct drm_map_list *r_list, *list_temp;
 
@@ -143,7 +129,7 @@ static void drm_master_destroy(struct kref *kref)
 
 	list_for_each_entry_safe(r_list, list_temp, &dev->maplist, head) {
 		if (r_list->master == master) {
-			drm_rmmap_locked(dev, r_list->map);
+			drm_legacy_rmmap_locked(dev, r_list->map);
 			r_list = NULL;
 		}
 	}
@@ -154,12 +140,6 @@ static void drm_master_destroy(struct kref *kref)
 		master->unique_len = 0;
 	}
 
-	list_for_each_entry_safe(pt, next, &master->magicfree, head) {
-		list_del(&pt->head);
-		drm_ht_remove_item(&master->magiclist, &pt->hash_item);
-		kfree(pt);
-	}
-
 	drm_ht_remove(&master->magiclist);
 
 	mutex_unlock(&dev->struct_mutex);
@@ -615,7 +595,7 @@ struct drm_device *drm_dev_alloc(struct drm_driver *driver,
 		goto err_ht;
 	}
 
-	if (driver->driver_features & DRIVER_GEM) {
+	if (drm_core_check_feature(dev, DRIVER_GEM)) {
 		ret = drm_gem_init(dev);
 		if (ret) {
 			DRM_ERROR("Cannot initialize graphics execution manager (GEM)\n");
@@ -645,7 +625,7 @@ static void drm_dev_release(struct kref *ref)
 {
 	struct drm_device *dev = container_of(ref, struct drm_device, ref);
 
-	if (dev->driver->driver_features & DRIVER_GEM)
+	if (drm_core_check_feature(dev, DRIVER_GEM))
 		drm_gem_destroy(dev);
 
 	drm_legacy_ctxbitmap_cleanup(dev);
@@ -779,7 +759,7 @@ void drm_dev_unregister(struct drm_device *dev)
 	drm_vblank_cleanup(dev);
 
 	list_for_each_entry_safe(r_list, list_temp, &dev->maplist, head)
-		drm_rmmap(dev, r_list->map);
+		drm_legacy_rmmap(dev, r_list->map);
 
 	drm_minor_unregister(dev, DRM_MINOR_LEGACY);
 	drm_minor_unregister(dev, DRM_MINOR_RENDER);
diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index 1dbf3bc4c6a3..3bf999134bcc 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -632,27 +632,27 @@ static const struct drm_display_mode edid_cea_modes[] = {
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
 			DRM_MODE_FLAG_INTERLACE),
 	  .vrefresh = 60, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 6 - 1440x480i@60Hz */
-	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
-		   1602, 1716, 0, 480, 488, 494, 525, 0,
+	/* 6 - 720(1440)x480i@60Hz */
+	{ DRM_MODE("720x480i", DRM_MODE_TYPE_DRIVER, 13500, 720, 739,
+		   801, 858, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 60, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 7 - 1440x480i@60Hz */
-	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
-		   1602, 1716, 0, 480, 488, 494, 525, 0,
+	/* 7 - 720(1440)x480i@60Hz */
+	{ DRM_MODE("720x480i", DRM_MODE_TYPE_DRIVER, 13500, 720, 739,
+		   801, 858, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 60, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 8 - 1440x240@60Hz */
-	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
-		   1602, 1716, 0, 240, 244, 247, 262, 0,
+	/* 8 - 720(1440)x240@60Hz */
+	{ DRM_MODE("720x240", DRM_MODE_TYPE_DRIVER, 13500, 720, 739,
+		   801, 858, 0, 240, 244, 247, 262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 60, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 9 - 1440x240@60Hz */
-	{ DRM_MODE("1440x240", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1478,
-		   1602, 1716, 0, 240, 244, 247, 262, 0,
+	/* 9 - 720(1440)x240@60Hz */
+	{ DRM_MODE("720x240", DRM_MODE_TYPE_DRIVER, 13500, 720, 739,
+		   801, 858, 0, 240, 244, 247, 262, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 60, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
@@ -714,27 +714,27 @@ static const struct drm_display_mode edid_cea_modes[] = {
 		   DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC |
 			DRM_MODE_FLAG_INTERLACE),
 	  .vrefresh = 50, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 21 - 1440x576i@50Hz */
-	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
-		   1590, 1728, 0, 576, 580, 586, 625, 0,
+	/* 21 - 720(1440)x576i@50Hz */
+	{ DRM_MODE("720x576i", DRM_MODE_TYPE_DRIVER, 13500, 720, 732,
+		   795, 864, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 50, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 22 - 1440x576i@50Hz */
-	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
-		   1590, 1728, 0, 576, 580, 586, 625, 0,
+	/* 22 - 720(1440)x576i@50Hz */
+	{ DRM_MODE("720x576i", DRM_MODE_TYPE_DRIVER, 13500, 720, 732,
+		   795, 864, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 50, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 23 - 1440x288@50Hz */
-	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
-		   1590, 1728, 0, 288, 290, 293, 312, 0,
+	/* 23 - 720(1440)x288@50Hz */
+	{ DRM_MODE("720x288", DRM_MODE_TYPE_DRIVER, 13500, 720, 732,
+		   795, 864, 0, 288, 290, 293, 312, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 50, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 24 - 1440x288@50Hz */
-	{ DRM_MODE("1440x288", DRM_MODE_TYPE_DRIVER, 27000, 1440, 1464,
-		   1590, 1728, 0, 288, 290, 293, 312, 0,
+	/* 24 - 720(1440)x288@50Hz */
+	{ DRM_MODE("720x288", DRM_MODE_TYPE_DRIVER, 13500, 720, 732,
+		   795, 864, 0, 288, 290, 293, 312, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 50, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
@@ -837,17 +837,17 @@ static const struct drm_display_mode edid_cea_modes[] = {
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
 	  .vrefresh = 100, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 44 - 1440x576i@100Hz */
-	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
-		   1590, 1728, 0, 576, 580, 586, 625, 0,
+	/* 44 - 720(1440)x576i@100Hz */
+	{ DRM_MODE("720x576i", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
+		   795, 864, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK),
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 100, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 45 - 1440x576i@100Hz */
-	{ DRM_MODE("1440x576", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1464,
-		   1590, 1728, 0, 576, 580, 586, 625, 0,
+	/* 45 - 720(1440)x576i@100Hz */
+	{ DRM_MODE("720x576i", DRM_MODE_TYPE_DRIVER, 27000, 720, 732,
+		   795, 864, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
-			DRM_MODE_FLAG_DBLCLK),
+			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 100, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
 	/* 46 - 1920x1080i@120Hz */
 	{ DRM_MODE("1920x1080i", DRM_MODE_TYPE_DRIVER, 148500, 1920, 2008,
@@ -870,15 +870,15 @@ static const struct drm_display_mode edid_cea_modes[] = {
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
 	  .vrefresh = 120, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 50 - 1440x480i@120Hz */
-	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
-		   1602, 1716, 0, 480, 488, 494, 525, 0,
+	/* 50 - 720(1440)x480i@120Hz */
+	{ DRM_MODE("720x480i", DRM_MODE_TYPE_DRIVER, 27000, 720, 739,
+		   801, 858, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 120, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 51 - 1440x480i@120Hz */
-	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 54000, 1440, 1478,
-		   1602, 1716, 0, 480, 488, 494, 525, 0,
+	/* 51 - 720(1440)x480i@120Hz */
+	{ DRM_MODE("720x480i", DRM_MODE_TYPE_DRIVER, 27000, 720, 739,
+		   801, 858, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 120, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
@@ -892,15 +892,15 @@ static const struct drm_display_mode edid_cea_modes[] = {
 		   796, 864, 0, 576, 581, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
 	  .vrefresh = 200, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 54 - 1440x576i@200Hz */
-	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
-		   1590, 1728, 0, 576, 580, 586, 625, 0,
+	/* 54 - 720(1440)x576i@200Hz */
+	{ DRM_MODE("720x576i", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
+		   795, 864, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 200, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 55 - 1440x576i@200Hz */
-	{ DRM_MODE("1440x576i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1464,
-		   1590, 1728, 0, 576, 580, 586, 625, 0,
+	/* 55 - 720(1440)x576i@200Hz */
+	{ DRM_MODE("720x576i", DRM_MODE_TYPE_DRIVER, 54000, 720, 732,
+		   795, 864, 0, 576, 580, 586, 625, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 200, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
@@ -914,15 +914,15 @@ static const struct drm_display_mode edid_cea_modes[] = {
 		   798, 858, 0, 480, 489, 495, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC),
 	  .vrefresh = 240, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
-	/* 58 - 1440x480i@240 */
-	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
-		   1602, 1716, 0, 480, 488, 494, 525, 0,
+	/* 58 - 720(1440)x480i@240 */
+	{ DRM_MODE("720x480i", DRM_MODE_TYPE_DRIVER, 54000, 720, 739,
+		   801, 858, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 240, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_4_3, },
-	/* 59 - 1440x480i@240 */
-	{ DRM_MODE("1440x480i", DRM_MODE_TYPE_DRIVER, 108000, 1440, 1478,
-		   1602, 1716, 0, 480, 488, 494, 525, 0,
+	/* 59 - 720(1440)x480i@240 */
+	{ DRM_MODE("720x480i", DRM_MODE_TYPE_DRIVER, 54000, 720, 739,
+		   801, 858, 0, 480, 488, 494, 525, 0,
 		   DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC |
 			DRM_MODE_FLAG_INTERLACE | DRM_MODE_FLAG_DBLCLK),
 	  .vrefresh = 240, .picture_aspect_ratio = HDMI_PICTURE_ASPECT_16_9, },
@@ -2103,7 +2103,8 @@ static int
 add_inferred_modes(struct drm_connector *connector, struct edid *edid)
 {
 	struct detailed_mode_closure closure = {
-		connector, edid, 0, 0, 0
+		.connector = connector,
+		.edid = edid,
 	};
 
 	if (version_greater(edid, 1, 0))
@@ -2169,7 +2170,8 @@ add_established_modes(struct drm_connector *connector, struct edid *edid)
 		((edid->established_timings.mfg_rsvd & 0x80) << 9);
 	int i, modes = 0;
 	struct detailed_mode_closure closure = {
-		connector, edid, 0, 0, 0
+		.connector = connector,
+		.edid = edid,
 	};
 
 	for (i = 0; i <= EDID_EST_TIMINGS; i++) {
@@ -2227,7 +2229,8 @@ add_standard_modes(struct drm_connector *connector, struct edid *edid)
 {
 	int i, modes = 0;
 	struct detailed_mode_closure closure = {
-		connector, edid, 0, 0, 0
+		.connector = connector,
+		.edid = edid,
 	};
 
 	for (i = 0; i < EDID_STD_TIMINGS; i++) {
@@ -2313,7 +2316,8 @@ static int
 add_cvt_modes(struct drm_connector *connector, struct edid *edid)
 {	
 	struct detailed_mode_closure closure = {
-		connector, edid, 0, 0, 0
+		.connector = connector,
+		.edid = edid,
 	};
 
 	if (version_greater(edid, 1, 2))
@@ -2357,11 +2361,10 @@ add_detailed_modes(struct drm_connector *connector, struct edid *edid,
 		   u32 quirks)
 {
 	struct detailed_mode_closure closure = {
-		connector,
-		edid,
-		1,
-		quirks,
-		0
+		.connector = connector,
+		.edid = edid,
+		.preferred = 1,
+		.quirks = quirks,
 	};
 
 	if (closure.preferred && !version_greater(edid, 1, 3))
@@ -3433,10 +3436,10 @@ EXPORT_SYMBOL(drm_rgb_quant_range_selectable);
 /**
  * drm_assign_hdmi_deep_color_info - detect whether monitor supports
  * hdmi deep color modes and update drm_display_info if so.
- *
  * @edid: monitor EDID information
  * @info: Updated with maximum supported deep color bpc and color format
  *        if deep color supported.
+ * @connector: DRM connector, used only for debug output
  *
  * Parse the CEA extension according to CEA-861-B.
  * Return true if HDMI deep color supported, false if not or unknown.
diff --git a/drivers/gpu/drm/drm_fb_helper.c b/drivers/gpu/drm/drm_fb_helper.c
index 3144db9dc0f1..0c0c39bac23d 100644
--- a/drivers/gpu/drm/drm_fb_helper.c
+++ b/drivers/gpu/drm/drm_fb_helper.c
@@ -126,7 +126,7 @@ int drm_fb_helper_add_one_connector(struct drm_fb_helper *fb_helper, struct drm_
 
 	WARN_ON(!mutex_is_locked(&fb_helper->dev->mode_config.mutex));
 	if (fb_helper->connector_count + 1 > fb_helper->connector_info_alloc_count) {
-		temp = krealloc(fb_helper->connector_info, sizeof(struct drm_fb_helper_connector) * (fb_helper->connector_count + 1), GFP_KERNEL);
+		temp = krealloc(fb_helper->connector_info, sizeof(struct drm_fb_helper_connector *) * (fb_helper->connector_count + 1), GFP_KERNEL);
 		if (!temp)
 			return -ENOMEM;
 
@@ -171,60 +171,6 @@ int drm_fb_helper_remove_one_connector(struct drm_fb_helper *fb_helper,
 }
 EXPORT_SYMBOL(drm_fb_helper_remove_one_connector);
 
-static int drm_fb_helper_parse_command_line(struct drm_fb_helper *fb_helper)
-{
-	struct drm_fb_helper_connector *fb_helper_conn;
-	int i;
-
-	for (i = 0; i < fb_helper->connector_count; i++) {
-		struct drm_cmdline_mode *mode;
-		struct drm_connector *connector;
-		char *option = NULL;
-
-		fb_helper_conn = fb_helper->connector_info[i];
-		connector = fb_helper_conn->connector;
-		mode = &fb_helper_conn->cmdline_mode;
-
-		/* do something on return - turn off connector maybe */
-		if (fb_get_options(connector->name, &option))
-			continue;
-
-		if (drm_mode_parse_command_line_for_connector(option,
-							      connector,
-							      mode)) {
-			if (mode->force) {
-				const char *s;
-				switch (mode->force) {
-				case DRM_FORCE_OFF:
-					s = "OFF";
-					break;
-				case DRM_FORCE_ON_DIGITAL:
-					s = "ON - dig";
-					break;
-				default:
-				case DRM_FORCE_ON:
-					s = "ON";
-					break;
-				}
-
-				DRM_INFO("forcing %s connector %s\n",
-					 connector->name, s);
-				connector->force = mode->force;
-			}
-
-			DRM_DEBUG_KMS("cmdline mode for connector %s %dx%d@%dHz%s%s%s\n",
-				      connector->name,
-				      mode->xres, mode->yres,
-				      mode->refresh_specified ? mode->refresh : 60,
-				      mode->rb ? " reduced blanking" : "",
-				      mode->margins ? " with margins" : "",
-				      mode->interlace ?  " interlaced" : "");
-		}
-
-	}
-	return 0;
-}
-
 static void drm_fb_helper_save_lut_atomic(struct drm_crtc *crtc, struct drm_fb_helper *helper)
 {
 	uint16_t *r_base, *g_base, *b_base;
@@ -345,10 +291,17 @@ static bool restore_fbdev_mode(struct drm_fb_helper *fb_helper)
 
 	drm_warn_on_modeset_not_all_locked(dev);
 
-	list_for_each_entry(plane, &dev->mode_config.plane_list, head)
+	list_for_each_entry(plane, &dev->mode_config.plane_list, head) {
 		if (plane->type != DRM_PLANE_TYPE_PRIMARY)
 			drm_plane_force_disable(plane);
 
+		if (dev->mode_config.rotation_property) {
+			drm_mode_plane_set_obj_prop(plane,
+						    dev->mode_config.rotation_property,
+						    BIT(DRM_ROTATE_0));
+		}
+	}
+
 	for (i = 0; i < fb_helper->crtc_count; i++) {
 		struct drm_mode_set *mode_set = &fb_helper->crtc_info[i].mode_set;
 		struct drm_crtc *crtc = mode_set->crtc;
@@ -419,11 +372,11 @@ static bool drm_fb_helper_force_kernel_mode(void)
 		if (dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 			continue;
 
-		/* NOTE: we use lockless flag below to avoid grabbing other
-		 * modeset locks.  So just trylock the underlying mutex
-		 * directly:
+		/*
+		 * NOTE: Use trylock mode to avoid deadlocks and sleeping in
+		 * panic context.
 		 */
-		if (!mutex_trylock(&dev->mode_config.mutex)) {
+		if (__drm_modeset_lock_all(dev, true) != 0) {
 			error = true;
 			continue;
 		}
@@ -432,7 +385,7 @@ static bool drm_fb_helper_force_kernel_mode(void)
 		if (ret)
 			error = true;
 
-		mutex_unlock(&dev->mode_config.mutex);
+		drm_modeset_unlock_all(dev);
 	}
 	return error;
 }
@@ -1013,7 +966,7 @@ static int drm_fb_helper_single_fb_probe(struct drm_fb_helper *fb_helper,
 		struct drm_fb_helper_connector *fb_helper_conn = fb_helper->connector_info[i];
 		struct drm_cmdline_mode *cmdline_mode;
 
-		cmdline_mode = &fb_helper_conn->cmdline_mode;
+		cmdline_mode = &fb_helper_conn->connector->cmdline_mode;
 
 		if (cmdline_mode->bpp_specified) {
 			switch (cmdline_mode->bpp) {
@@ -1260,9 +1213,7 @@ EXPORT_SYMBOL(drm_has_preferred_mode);
 
 static bool drm_has_cmdline_mode(struct drm_fb_helper_connector *fb_connector)
 {
-	struct drm_cmdline_mode *cmdline_mode;
-	cmdline_mode = &fb_connector->cmdline_mode;
-	return cmdline_mode->specified;
+	return fb_connector->connector->cmdline_mode.specified;
 }
 
 struct drm_display_mode *drm_pick_cmdline_mode(struct drm_fb_helper_connector *fb_helper_conn,
@@ -1272,7 +1223,7 @@ struct drm_display_mode *drm_pick_cmdline_mode(struct drm_fb_helper_connector *f
 	struct drm_display_mode *mode = NULL;
 	bool prefer_non_interlace;
 
-	cmdline_mode = &fb_helper_conn->cmdline_mode;
+	cmdline_mode = &fb_helper_conn->connector->cmdline_mode;
 	if (cmdline_mode->specified == false)
 		return mode;
 
@@ -1657,8 +1608,6 @@ bool drm_fb_helper_initial_config(struct drm_fb_helper *fb_helper, int bpp_sel)
 	struct drm_device *dev = fb_helper->dev;
 	int count = 0;
 
-	drm_fb_helper_parse_command_line(fb_helper);
-
 	mutex_lock(&dev->mode_config.mutex);
 	count = drm_fb_helper_probe_connector_modes(fb_helper,
 						    dev->mode_config.max_width,
diff --git a/drivers/gpu/drm/drm_fops.c b/drivers/gpu/drm/drm_fops.c
index 79d5221c6e41..ed7bc68f7e87 100644
--- a/drivers/gpu/drm/drm_fops.c
+++ b/drivers/gpu/drm/drm_fops.c
@@ -39,10 +39,10 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include "drm_legacy.h"
+#include "drm_internal.h"
 
 /* from BKL pushdown */
 DEFINE_MUTEX(drm_global_mutex);
-EXPORT_SYMBOL(drm_global_mutex);
 
 static int drm_open_helper(struct file *filp, struct drm_minor *minor);
 
@@ -171,7 +171,7 @@ static int drm_open_helper(struct file *filp, struct drm_minor *minor)
 	init_waitqueue_head(&priv->event_wait);
 	priv->event_space = 4096; /* set aside 4k for event buffer */
 
-	if (dev->driver->driver_features & DRIVER_GEM)
+	if (drm_core_check_feature(dev, DRIVER_GEM))
 		drm_gem_open(dev, priv);
 
 	if (drm_core_check_feature(dev, DRIVER_PRIME))
@@ -256,7 +256,7 @@ out_close:
 out_prime_destroy:
 	if (drm_core_check_feature(dev, DRIVER_PRIME))
 		drm_prime_destroy_file_private(&priv->prime);
-	if (dev->driver->driver_features & DRIVER_GEM)
+	if (drm_core_check_feature(dev, DRIVER_GEM))
 		drm_gem_release(dev, priv);
 	put_pid(priv->pid);
 	kfree(priv);
@@ -268,11 +268,11 @@ static void drm_master_release(struct drm_device *dev, struct file *filp)
 {
 	struct drm_file *file_priv = filp->private_data;
 
-	if (drm_i_have_hw_lock(dev, file_priv)) {
+	if (drm_legacy_i_have_hw_lock(dev, file_priv)) {
 		DRM_DEBUG("File %p released, freeing lock for context %d\n",
 			  filp, _DRM_LOCKING_CONTEXT(file_priv->master->lock.hw_lock->lock));
-		drm_lock_free(&file_priv->master->lock,
-			      _DRM_LOCKING_CONTEXT(file_priv->master->lock.hw_lock->lock));
+		drm_legacy_lock_free(&file_priv->master->lock,
+				     _DRM_LOCKING_CONTEXT(file_priv->master->lock.hw_lock->lock));
 	}
 }
 
@@ -330,8 +330,6 @@ static void drm_legacy_dev_reinit(struct drm_device *dev)
  */
 int drm_lastclose(struct drm_device * dev)
 {
-	struct drm_vma_entry *vma, *vma_temp;
-
 	DRM_DEBUG("\n");
 
 	if (dev->driver->lastclose)
@@ -346,13 +344,7 @@ int drm_lastclose(struct drm_device * dev)
 	drm_agp_clear(dev);
 
 	drm_legacy_sg_cleanup(dev);
-
-	/* Clear vma list (only built for debugging) */
-	list_for_each_entry_safe(vma, vma_temp, &dev->vmalist, head) {
-		list_del(&vma->head);
-		kfree(vma);
-	}
-
+	drm_legacy_vma_flush(dev);
 	drm_legacy_dma_takedown(dev);
 
 	mutex_unlock(&dev->struct_mutex);
@@ -412,14 +404,14 @@ int drm_release(struct inode *inode, struct file *filp)
 		drm_master_release(dev, filp);
 
 	if (drm_core_check_feature(dev, DRIVER_HAVE_DMA))
-		drm_core_reclaim_buffers(dev, file_priv);
+		drm_legacy_reclaim_buffers(dev, file_priv);
 
 	drm_events_release(file_priv);
 
-	if (dev->driver->driver_features & DRIVER_MODESET)
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
 		drm_fb_release(file_priv);
 
-	if (dev->driver->driver_features & DRIVER_GEM)
+	if (drm_core_check_feature(dev, DRIVER_GEM))
 		drm_gem_release(dev, file_priv);
 
 	drm_legacy_ctxbitmap_flush(dev, file_priv);
@@ -464,6 +456,8 @@ int drm_release(struct inode *inode, struct file *filp)
 	if (drm_core_check_feature(dev, DRIVER_PRIME))
 		drm_prime_destroy_file_private(&file_priv->prime);
 
+	WARN_ON(!list_empty(&file_priv->event_list));
+
 	put_pid(file_priv->pid);
 	kfree(file_priv);
 
diff --git a/drivers/gpu/drm/drm_gem.c b/drivers/gpu/drm/drm_gem.c
index 6adee4c2afc0..f6ca51259fa3 100644
--- a/drivers/gpu/drm/drm_gem.c
+++ b/drivers/gpu/drm/drm_gem.c
@@ -38,6 +38,8 @@
 #include <linux/dma-buf.h>
 #include <drm/drmP.h>
 #include <drm/drm_vma_manager.h>
+#include <drm/drm_gem.h>
+#include "drm_internal.h"
 
 /** @file drm_gem.c
  *
@@ -146,7 +148,7 @@ int drm_gem_object_init(struct drm_device *dev,
 EXPORT_SYMBOL(drm_gem_object_init);
 
 /**
- * drm_gem_object_init - initialize an allocated private GEM object
+ * drm_gem_private_object_init - initialize an allocated private GEM object
  * @dev: drm_device the object should be initialized for
  * @obj: drm_gem_object to initialize
  * @size: object size
@@ -579,7 +581,7 @@ drm_gem_close_ioctl(struct drm_device *dev, void *data,
 	struct drm_gem_close *args = data;
 	int ret;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
+	if (!drm_core_check_feature(dev, DRIVER_GEM))
 		return -ENODEV;
 
 	ret = drm_gem_handle_delete(file_priv, args->handle);
@@ -606,7 +608,7 @@ drm_gem_flink_ioctl(struct drm_device *dev, void *data,
 	struct drm_gem_object *obj;
 	int ret;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
+	if (!drm_core_check_feature(dev, DRIVER_GEM))
 		return -ENODEV;
 
 	obj = drm_gem_object_lookup(dev, file_priv, args->handle);
@@ -659,7 +661,7 @@ drm_gem_open_ioctl(struct drm_device *dev, void *data,
 	int ret;
 	u32 handle;
 
-	if (!(dev->driver->driver_features & DRIVER_GEM))
+	if (!drm_core_check_feature(dev, DRIVER_GEM))
 		return -ENODEV;
 
 	mutex_lock(&dev->object_name_lock);
@@ -887,7 +889,7 @@ int drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 					   vma_pages(vma));
 	if (!node) {
 		mutex_unlock(&dev->struct_mutex);
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 	} else if (!drm_vma_node_is_allowed(node, filp)) {
 		mutex_unlock(&dev->struct_mutex);
 		return -EACCES;
diff --git a/drivers/gpu/drm/drm_gem_cma_helper.c b/drivers/gpu/drm/drm_gem_cma_helper.c
index e467e67af6e7..0316310e2cc4 100644
--- a/drivers/gpu/drm/drm_gem_cma_helper.c
+++ b/drivers/gpu/drm/drm_gem_cma_helper.c
@@ -316,7 +316,8 @@ out:
 EXPORT_SYMBOL_GPL(drm_gem_cma_prime_get_sg_table);
 
 struct drm_gem_object *
-drm_gem_cma_prime_import_sg_table(struct drm_device *dev, size_t size,
+drm_gem_cma_prime_import_sg_table(struct drm_device *dev,
+				  struct dma_buf_attachment *attach,
 				  struct sg_table *sgt)
 {
 	struct drm_gem_cma_object *cma_obj;
@@ -325,14 +326,14 @@ drm_gem_cma_prime_import_sg_table(struct drm_device *dev, size_t size,
 		return ERR_PTR(-EINVAL);
 
 	/* Create a CMA GEM buffer. */
-	cma_obj = __drm_gem_cma_create(dev, size);
+	cma_obj = __drm_gem_cma_create(dev, attach->dmabuf->size);
 	if (IS_ERR(cma_obj))
 		return ERR_CAST(cma_obj);
 
 	cma_obj->paddr = sg_dma_address(sgt->sgl);
 	cma_obj->sgt = sgt;
 
-	DRM_DEBUG_PRIME("dma_addr = %pad, size = %zu\n", &cma_obj->paddr, size);
+	DRM_DEBUG_PRIME("dma_addr = %pad, size = %zu\n", &cma_obj->paddr, attach->dmabuf->size);
 
 	return &cma_obj->base;
 }
diff --git a/drivers/gpu/drm/drm_info.c b/drivers/gpu/drm/drm_info.c
index ecaf0fa2eec8..51efebd434f3 100644
--- a/drivers/gpu/drm/drm_info.c
+++ b/drivers/gpu/drm/drm_info.c
@@ -35,6 +35,9 @@
 
 #include <linux/seq_file.h>
 #include <drm/drmP.h>
+#include <drm/drm_gem.h>
+
+#include "drm_legacy.h"
 
 /**
  * Called when "/proc/dri/.../name" is read.
@@ -183,15 +186,32 @@ int drm_clients_info(struct seq_file *m, void *data)
 	struct drm_device *dev = node->minor->dev;
 	struct drm_file *priv;
 
+	seq_printf(m,
+		   "%20s %5s %3s master a %5s %10s\n",
+		   "command",
+		   "pid",
+		   "dev",
+		   "uid",
+		   "magic");
+
+	/* dev->filelist is sorted youngest first, but we want to present
+	 * oldest first (i.e. kernel, servers, clients), so walk backwardss.
+	 */
 	mutex_lock(&dev->struct_mutex);
-	seq_printf(m, "a dev	pid    uid	magic\n\n");
-	list_for_each_entry(priv, &dev->filelist, lhead) {
-		seq_printf(m, "%c %3d %5d %5d %10u\n",
-			   priv->authenticated ? 'y' : 'n',
-			   priv->minor->index,
+	list_for_each_entry_reverse(priv, &dev->filelist, lhead) {
+		struct task_struct *task;
+
+		rcu_read_lock(); /* locks pid_task()->comm */
+		task = pid_task(priv->pid, PIDTYPE_PID);
+		seq_printf(m, "%20s %5d %3d   %c    %c %5d %10u\n",
+			   task ? task->comm : "<unknown>",
 			   pid_vnr(priv->pid),
+			   priv->minor->index,
+			   priv->is_master ? 'y' : 'n',
+			   priv->authenticated ? 'y' : 'n',
 			   from_kuid_munged(seq_user_ns(m), priv->uid),
 			   priv->magic);
+		rcu_read_unlock();
 	}
 	mutex_unlock(&dev->struct_mutex);
 	return 0;
@@ -223,62 +243,3 @@ int drm_gem_name_info(struct seq_file *m, void *data)
 
 	return 0;
 }
-
-#if DRM_DEBUG_CODE
-
-int drm_vma_info(struct seq_file *m, void *data)
-{
-	struct drm_info_node *node = (struct drm_info_node *) m->private;
-	struct drm_device *dev = node->minor->dev;
-	struct drm_vma_entry *pt;
-	struct vm_area_struct *vma;
-	unsigned long vma_count = 0;
-#if defined(__i386__)
-	unsigned int pgprot;
-#endif
-
-	mutex_lock(&dev->struct_mutex);
-	list_for_each_entry(pt, &dev->vmalist, head)
-		vma_count++;
-
-	seq_printf(m, "vma use count: %lu, high_memory = %pK, 0x%pK\n",
-		   vma_count, high_memory,
-		   (void *)(unsigned long)virt_to_phys(high_memory));
-
-	list_for_each_entry(pt, &dev->vmalist, head) {
-		vma = pt->vma;
-		if (!vma)
-			continue;
-		seq_printf(m,
-			   "\n%5d 0x%pK-0x%pK %c%c%c%c%c%c 0x%08lx000",
-			   pt->pid,
-			   (void *)vma->vm_start, (void *)vma->vm_end,
-			   vma->vm_flags & VM_READ ? 'r' : '-',
-			   vma->vm_flags & VM_WRITE ? 'w' : '-',
-			   vma->vm_flags & VM_EXEC ? 'x' : '-',
-			   vma->vm_flags & VM_MAYSHARE ? 's' : 'p',
-			   vma->vm_flags & VM_LOCKED ? 'l' : '-',
-			   vma->vm_flags & VM_IO ? 'i' : '-',
-			   vma->vm_pgoff);
-
-#if defined(__i386__)
-		pgprot = pgprot_val(vma->vm_page_prot);
-		seq_printf(m, " %c%c%c%c%c%c%c%c%c",
-			   pgprot & _PAGE_PRESENT ? 'p' : '-',
-			   pgprot & _PAGE_RW ? 'w' : 'r',
-			   pgprot & _PAGE_USER ? 'u' : 's',
-			   pgprot & _PAGE_PWT ? 't' : 'b',
-			   pgprot & _PAGE_PCD ? 'u' : 'c',
-			   pgprot & _PAGE_ACCESSED ? 'a' : '-',
-			   pgprot & _PAGE_DIRTY ? 'd' : '-',
-			   pgprot & _PAGE_PSE ? 'm' : 'k',
-			   pgprot & _PAGE_GLOBAL ? 'g' : 'l');
-#endif
-		seq_printf(m, "\n");
-	}
-	mutex_unlock(&dev->struct_mutex);
-	return 0;
-}
-
-#endif
-
diff --git a/drivers/gpu/drm/drm_internal.h b/drivers/gpu/drm/drm_internal.h
new file mode 100644
index 000000000000..7cc0a3516871
--- /dev/null
+++ b/drivers/gpu/drm/drm_internal.h
@@ -0,0 +1,132 @@
+/*
+ * Copyright © 2014 Intel Corporation
+ *   Daniel Vetter <daniel.vetter@ffwll.ch>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+/* drm_irq.c */
+extern unsigned int drm_timestamp_monotonic;
+
+/* drm_fops.c */
+extern struct mutex drm_global_mutex;
+int drm_lastclose(struct drm_device *dev);
+
+/* drm_pci.c */
+int drm_pci_set_unique(struct drm_device *dev,
+		       struct drm_master *master,
+		       struct drm_unique *u);
+int drm_irq_by_busid(struct drm_device *dev, void *data,
+		     struct drm_file *file_priv);
+
+/* drm_vm.c */
+int drm_vma_info(struct seq_file *m, void *data);
+void drm_vm_open_locked(struct drm_device *dev, struct vm_area_struct *vma);
+void drm_vm_close_locked(struct drm_device *dev, struct vm_area_struct *vma);
+
+/* drm_prime.c */
+int drm_prime_handle_to_fd_ioctl(struct drm_device *dev, void *data,
+				 struct drm_file *file_priv);
+int drm_prime_fd_to_handle_ioctl(struct drm_device *dev, void *data,
+				 struct drm_file *file_priv);
+
+void drm_prime_init_file_private(struct drm_prime_file_private *prime_fpriv);
+void drm_prime_destroy_file_private(struct drm_prime_file_private *prime_fpriv);
+void drm_prime_remove_buf_handle_locked(struct drm_prime_file_private *prime_fpriv,
+					struct dma_buf *dma_buf);
+
+/* drm_info.c */
+int drm_name_info(struct seq_file *m, void *data);
+int drm_vm_info(struct seq_file *m, void *data);
+int drm_bufs_info(struct seq_file *m, void *data);
+int drm_vblank_info(struct seq_file *m, void *data);
+int drm_clients_info(struct seq_file *m, void* data);
+int drm_gem_name_info(struct seq_file *m, void *data);
+
+/* drm_irq.c */
+int drm_control(struct drm_device *dev, void *data,
+		struct drm_file *file_priv);
+int drm_modeset_ctl(struct drm_device *dev, void *data,
+		    struct drm_file *file_priv);
+
+/* drm_auth.c */
+int drm_getmagic(struct drm_device *dev, void *data,
+		 struct drm_file *file_priv);
+int drm_authmagic(struct drm_device *dev, void *data,
+		  struct drm_file *file_priv);
+int drm_remove_magic(struct drm_master *master, drm_magic_t magic);
+
+/* drm_sysfs.c */
+extern struct class *drm_class;
+
+struct class *drm_sysfs_create(struct module *owner, char *name);
+void drm_sysfs_destroy(void);
+struct device *drm_sysfs_minor_alloc(struct drm_minor *minor);
+int drm_sysfs_connector_add(struct drm_connector *connector);
+void drm_sysfs_connector_remove(struct drm_connector *connector);
+
+/* drm_gem.c */
+int drm_gem_init(struct drm_device *dev);
+void drm_gem_destroy(struct drm_device *dev);
+int drm_gem_handle_create_tail(struct drm_file *file_priv,
+			       struct drm_gem_object *obj,
+			       u32 *handlep);
+int drm_gem_close_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv);
+int drm_gem_flink_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv);
+int drm_gem_open_ioctl(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv);
+void drm_gem_open(struct drm_device *dev, struct drm_file *file_private);
+void drm_gem_release(struct drm_device *dev, struct drm_file *file_private);
+
+/* drm_drv.c */
+int drm_setmaster_ioctl(struct drm_device *dev, void *data,
+			struct drm_file *file_priv);
+int drm_dropmaster_ioctl(struct drm_device *dev, void *data,
+			 struct drm_file *file_priv);
+struct drm_master *drm_master_create(struct drm_minor *minor);
+
+/* drm_debugfs.c */
+#if defined(CONFIG_DEBUG_FS)
+int drm_debugfs_init(struct drm_minor *minor, int minor_id,
+		     struct dentry *root);
+int drm_debugfs_cleanup(struct drm_minor *minor);
+int drm_debugfs_connector_add(struct drm_connector *connector);
+void drm_debugfs_connector_remove(struct drm_connector *connector);
+#else
+static inline int drm_debugfs_init(struct drm_minor *minor, int minor_id,
+				   struct dentry *root)
+{
+	return 0;
+}
+
+static inline int drm_debugfs_cleanup(struct drm_minor *minor)
+{
+	return 0;
+}
+
+static inline int drm_debugfs_connector_add(struct drm_connector *connector)
+{
+	return 0;
+}
+static inline void drm_debugfs_connector_remove(struct drm_connector *connector)
+{
+}
+#endif
diff --git a/drivers/gpu/drm/drm_ioctl.c b/drivers/gpu/drm/drm_ioctl.c
index 40be746b7e68..00587a1e3c83 100644
--- a/drivers/gpu/drm/drm_ioctl.c
+++ b/drivers/gpu/drm/drm_ioctl.c
@@ -31,6 +31,7 @@
 #include <drm/drmP.h>
 #include <drm/drm_core.h>
 #include "drm_legacy.h"
+#include "drm_internal.h"
 
 #include <linux/pci.h>
 #include <linux/export.h>
@@ -41,121 +42,6 @@
 static int drm_version(struct drm_device *dev, void *data,
 		       struct drm_file *file_priv);
 
-#define DRM_IOCTL_DEF(ioctl, _func, _flags) \
-	[DRM_IOCTL_NR(ioctl)] = {.cmd = ioctl, .func = _func, .flags = _flags, .cmd_drv = 0, .name = #ioctl}
-
-/** Ioctl table */
-static const struct drm_ioctl_desc drm_ioctls[] = {
-	DRM_IOCTL_DEF(DRM_IOCTL_VERSION, drm_version, DRM_UNLOCKED|DRM_RENDER_ALLOW),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_UNIQUE, drm_getunique, 0),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAGIC, drm_getmagic, 0),
-	DRM_IOCTL_DEF(DRM_IOCTL_IRQ_BUSID, drm_irq_by_busid, DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAP, drm_getmap, DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_CLIENT, drm_getclient, DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_STATS, drm_getstats, DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_CAP, drm_getcap, DRM_UNLOCKED|DRM_RENDER_ALLOW),
-	DRM_IOCTL_DEF(DRM_IOCTL_SET_CLIENT_CAP, drm_setclientcap, 0),
-	DRM_IOCTL_DEF(DRM_IOCTL_SET_VERSION, drm_setversion, DRM_MASTER),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_SET_UNIQUE, drm_setunique, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_BLOCK, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_UNBLOCK, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_AUTH_MAGIC, drm_authmagic, DRM_AUTH|DRM_MASTER),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_ADD_MAP, drm_addmap_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_RM_MAP, drm_rmmap_ioctl, DRM_AUTH),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_SET_SAREA_CTX, drm_legacy_setsareactx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_SAREA_CTX, drm_legacy_getsareactx, DRM_AUTH),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_SET_MASTER, drm_setmaster_ioctl, DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_DROP_MASTER, drm_dropmaster_ioctl, DRM_ROOT_ONLY),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_ADD_CTX, drm_legacy_addctx, DRM_AUTH|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_RM_CTX, drm_legacy_rmctx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_MOD_CTX, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_GET_CTX, drm_legacy_getctx, DRM_AUTH),
-	DRM_IOCTL_DEF(DRM_IOCTL_SWITCH_CTX, drm_legacy_switchctx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_NEW_CTX, drm_legacy_newctx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_RES_CTX, drm_legacy_resctx, DRM_AUTH),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_ADD_DRAW, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_RM_DRAW, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_LOCK, drm_lock, DRM_AUTH),
-	DRM_IOCTL_DEF(DRM_IOCTL_UNLOCK, drm_unlock, DRM_AUTH),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_FINISH, drm_noop, DRM_AUTH),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_ADD_BUFS, drm_addbufs, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_MARK_BUFS, drm_markbufs, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_INFO_BUFS, drm_infobufs, DRM_AUTH),
-	DRM_IOCTL_DEF(DRM_IOCTL_MAP_BUFS, drm_mapbufs, DRM_AUTH),
-	DRM_IOCTL_DEF(DRM_IOCTL_FREE_BUFS, drm_freebufs, DRM_AUTH),
-	DRM_IOCTL_DEF(DRM_IOCTL_DMA, drm_dma_ioctl, DRM_AUTH),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_CONTROL, drm_control, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-
-#if __OS_HAS_AGP
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_ACQUIRE, drm_agp_acquire_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_RELEASE, drm_agp_release_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_ENABLE, drm_agp_enable_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_INFO, drm_agp_info_ioctl, DRM_AUTH),
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_ALLOC, drm_agp_alloc_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_FREE, drm_agp_free_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_BIND, drm_agp_bind_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_AGP_UNBIND, drm_agp_unbind_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-#endif
-
-	DRM_IOCTL_DEF(DRM_IOCTL_SG_ALLOC, drm_sg_alloc, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-	DRM_IOCTL_DEF(DRM_IOCTL_SG_FREE, drm_sg_free, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_WAIT_VBLANK, drm_wait_vblank, DRM_UNLOCKED),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_MODESET_CTL, drm_modeset_ctl, 0),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_UPDATE_DRAW, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_GEM_CLOSE, drm_gem_close_ioctl, DRM_UNLOCKED|DRM_RENDER_ALLOW),
-	DRM_IOCTL_DEF(DRM_IOCTL_GEM_FLINK, drm_gem_flink_ioctl, DRM_AUTH|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_GEM_OPEN, drm_gem_open_ioctl, DRM_AUTH|DRM_UNLOCKED),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETRESOURCES, drm_mode_getresources, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_PRIME_HANDLE_TO_FD, drm_prime_handle_to_fd_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
-	DRM_IOCTL_DEF(DRM_IOCTL_PRIME_FD_TO_HANDLE, drm_prime_fd_to_handle_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
-
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPLANERESOURCES, drm_mode_getplane_res, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETCRTC, drm_mode_getcrtc, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETCRTC, drm_mode_setcrtc, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPLANE, drm_mode_getplane, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETPLANE, drm_mode_setplane, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CURSOR, drm_mode_cursor_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETGAMMA, drm_mode_gamma_get_ioctl, DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETGAMMA, drm_mode_gamma_set_ioctl, DRM_MASTER|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETENCODER, drm_mode_getencoder, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETCONNECTOR, drm_mode_getconnector, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ATTACHMODE, drm_noop, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DETACHMODE, drm_noop, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPROPERTY, drm_mode_getproperty_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETPROPERTY, drm_mode_connector_property_set_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPROPBLOB, drm_mode_getblob_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETFB, drm_mode_getfb, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ADDFB, drm_mode_addfb, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ADDFB2, drm_mode_addfb2, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_RMFB, drm_mode_rmfb, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_PAGE_FLIP, drm_mode_page_flip_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DIRTYFB, drm_mode_dirtyfb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CREATE_DUMB, drm_mode_create_dumb_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_MAP_DUMB, drm_mode_mmap_dumb_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DESTROY_DUMB, drm_mode_destroy_dumb_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_GETPROPERTIES, drm_mode_obj_get_properties_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_SETPROPERTY, drm_mode_obj_set_property_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CURSOR2, drm_mode_cursor2_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
-};
-
-#define DRM_CORE_IOCTL_COUNT	ARRAY_SIZE( drm_ioctls )
-
 /**
  * Get the bus id.
  *
@@ -167,7 +53,7 @@ static const struct drm_ioctl_desc drm_ioctls[] = {
  *
  * Copies the bus id from drm_device::unique into user space.
  */
-int drm_getunique(struct drm_device *dev, void *data,
+static int drm_getunique(struct drm_device *dev, void *data,
 		  struct drm_file *file_priv)
 {
 	struct drm_unique *u = data;
@@ -189,7 +75,6 @@ drm_unset_busid(struct drm_device *dev,
 	kfree(master->unique);
 	master->unique = NULL;
 	master->unique_len = 0;
-	master->unique_size = 0;
 }
 
 /**
@@ -207,7 +92,7 @@ drm_unset_busid(struct drm_device *dev,
  * version 1.1 or greater. Also note that KMS is all version 1.1 and later and
  * UMS was only ever supported on pci devices.
  */
-int drm_setunique(struct drm_device *dev, void *data,
+static int drm_setunique(struct drm_device *dev, void *data,
 		  struct drm_file *file_priv)
 {
 	struct drm_unique *u = data;
@@ -245,15 +130,15 @@ static int drm_set_busid(struct drm_device *dev, struct drm_file *file_priv)
 	if (master->unique != NULL)
 		drm_unset_busid(dev, master);
 
-	if (dev->driver->bus && dev->driver->bus->set_busid) {
-		ret = dev->driver->bus->set_busid(dev, master);
+	if (dev->driver->set_busid) {
+		ret = dev->driver->set_busid(dev, master);
 		if (ret) {
 			drm_unset_busid(dev, master);
 			return ret;
 		}
 	} else {
 		if (WARN(dev->unique == NULL,
-			 "No drm_bus.set_busid() implementation provided by "
+			 "No drm_driver.set_busid() implementation provided by "
 			 "%ps. Use drm_dev_set_unique() to set the unique "
 			 "name explicitly.", dev->driver))
 			return -EINVAL;
@@ -279,7 +164,7 @@ static int drm_set_busid(struct drm_device *dev, struct drm_file *file_priv)
  * Searches for the mapping with the specified offset and copies its information
  * into userspace
  */
-int drm_getmap(struct drm_device *dev, void *data,
+static int drm_getmap(struct drm_device *dev, void *data,
 	       struct drm_file *file_priv)
 {
 	struct drm_map *map = data;
@@ -340,7 +225,7 @@ int drm_getmap(struct drm_device *dev, void *data,
  * Searches for the client with the specified index and copies its information
  * into userspace
  */
-int drm_getclient(struct drm_device *dev, void *data,
+static int drm_getclient(struct drm_device *dev, void *data,
 		  struct drm_file *file_priv)
 {
 	struct drm_client *client = data;
@@ -380,7 +265,7 @@ int drm_getclient(struct drm_device *dev, void *data,
  *
  * \return zero on success or a negative number on failure.
  */
-int drm_getstats(struct drm_device *dev, void *data,
+static int drm_getstats(struct drm_device *dev, void *data,
 		 struct drm_file *file_priv)
 {
 	struct drm_stats *stats = data;
@@ -394,7 +279,7 @@ int drm_getstats(struct drm_device *dev, void *data,
 /**
  * Get device/driver capabilities
  */
-int drm_getcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
+static int drm_getcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
 	struct drm_get_cap *req = data;
 
@@ -444,7 +329,7 @@ int drm_getcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
 /**
  * Set device/driver capabilities
  */
-int
+static int
 drm_setclientcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
 	struct drm_set_client_cap *req = data;
@@ -478,7 +363,7 @@ drm_setclientcap(struct drm_device *dev, void *data, struct drm_file *file_priv)
  *
  * Sets the requested interface version
  */
-int drm_setversion(struct drm_device *dev, void *data, struct drm_file *file_priv)
+static int drm_setversion(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
 	struct drm_set_version *sv = data;
 	int if_version, retcode = 0;
@@ -624,6 +509,121 @@ static int drm_ioctl_permit(u32 flags, struct drm_file *file_priv)
 	return 0;
 }
 
+#define DRM_IOCTL_DEF(ioctl, _func, _flags) \
+	[DRM_IOCTL_NR(ioctl)] = {.cmd = ioctl, .func = _func, .flags = _flags, .cmd_drv = 0, .name = #ioctl}
+
+/** Ioctl table */
+static const struct drm_ioctl_desc drm_ioctls[] = {
+	DRM_IOCTL_DEF(DRM_IOCTL_VERSION, drm_version, DRM_UNLOCKED|DRM_RENDER_ALLOW),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_UNIQUE, drm_getunique, 0),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAGIC, drm_getmagic, 0),
+	DRM_IOCTL_DEF(DRM_IOCTL_IRQ_BUSID, drm_irq_by_busid, DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_MAP, drm_getmap, DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_CLIENT, drm_getclient, DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_STATS, drm_getstats, DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_CAP, drm_getcap, DRM_UNLOCKED|DRM_RENDER_ALLOW),
+	DRM_IOCTL_DEF(DRM_IOCTL_SET_CLIENT_CAP, drm_setclientcap, 0),
+	DRM_IOCTL_DEF(DRM_IOCTL_SET_VERSION, drm_setversion, DRM_MASTER),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_SET_UNIQUE, drm_setunique, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_BLOCK, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_UNBLOCK, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_AUTH_MAGIC, drm_authmagic, DRM_AUTH|DRM_MASTER),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_ADD_MAP, drm_legacy_addmap_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_RM_MAP, drm_legacy_rmmap_ioctl, DRM_AUTH),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_SET_SAREA_CTX, drm_legacy_setsareactx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_SAREA_CTX, drm_legacy_getsareactx, DRM_AUTH),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_SET_MASTER, drm_setmaster_ioctl, DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_DROP_MASTER, drm_dropmaster_ioctl, DRM_ROOT_ONLY),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_ADD_CTX, drm_legacy_addctx, DRM_AUTH|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_RM_CTX, drm_legacy_rmctx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_MOD_CTX, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_GET_CTX, drm_legacy_getctx, DRM_AUTH),
+	DRM_IOCTL_DEF(DRM_IOCTL_SWITCH_CTX, drm_legacy_switchctx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_NEW_CTX, drm_legacy_newctx, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_RES_CTX, drm_legacy_resctx, DRM_AUTH),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_ADD_DRAW, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_RM_DRAW, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_LOCK, drm_legacy_lock, DRM_AUTH),
+	DRM_IOCTL_DEF(DRM_IOCTL_UNLOCK, drm_legacy_unlock, DRM_AUTH),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_FINISH, drm_noop, DRM_AUTH),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_ADD_BUFS, drm_legacy_addbufs, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_MARK_BUFS, drm_legacy_markbufs, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_INFO_BUFS, drm_legacy_infobufs, DRM_AUTH),
+	DRM_IOCTL_DEF(DRM_IOCTL_MAP_BUFS, drm_legacy_mapbufs, DRM_AUTH),
+	DRM_IOCTL_DEF(DRM_IOCTL_FREE_BUFS, drm_legacy_freebufs, DRM_AUTH),
+	DRM_IOCTL_DEF(DRM_IOCTL_DMA, drm_legacy_dma_ioctl, DRM_AUTH),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_CONTROL, drm_control, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+
+#if __OS_HAS_AGP
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_ACQUIRE, drm_agp_acquire_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_RELEASE, drm_agp_release_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_ENABLE, drm_agp_enable_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_INFO, drm_agp_info_ioctl, DRM_AUTH),
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_ALLOC, drm_agp_alloc_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_FREE, drm_agp_free_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_BIND, drm_agp_bind_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_AGP_UNBIND, drm_agp_unbind_ioctl, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+#endif
+
+	DRM_IOCTL_DEF(DRM_IOCTL_SG_ALLOC, drm_legacy_sg_alloc, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+	DRM_IOCTL_DEF(DRM_IOCTL_SG_FREE, drm_legacy_sg_free, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_WAIT_VBLANK, drm_wait_vblank, DRM_UNLOCKED),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_MODESET_CTL, drm_modeset_ctl, 0),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_UPDATE_DRAW, drm_noop, DRM_AUTH|DRM_MASTER|DRM_ROOT_ONLY),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_GEM_CLOSE, drm_gem_close_ioctl, DRM_UNLOCKED|DRM_RENDER_ALLOW),
+	DRM_IOCTL_DEF(DRM_IOCTL_GEM_FLINK, drm_gem_flink_ioctl, DRM_AUTH|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_GEM_OPEN, drm_gem_open_ioctl, DRM_AUTH|DRM_UNLOCKED),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETRESOURCES, drm_mode_getresources, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_PRIME_HANDLE_TO_FD, drm_prime_handle_to_fd_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
+	DRM_IOCTL_DEF(DRM_IOCTL_PRIME_FD_TO_HANDLE, drm_prime_fd_to_handle_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
+
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPLANERESOURCES, drm_mode_getplane_res, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETCRTC, drm_mode_getcrtc, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETCRTC, drm_mode_setcrtc, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPLANE, drm_mode_getplane, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETPLANE, drm_mode_setplane, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CURSOR, drm_mode_cursor_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETGAMMA, drm_mode_gamma_get_ioctl, DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETGAMMA, drm_mode_gamma_set_ioctl, DRM_MASTER|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETENCODER, drm_mode_getencoder, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETCONNECTOR, drm_mode_getconnector, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ATTACHMODE, drm_noop, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DETACHMODE, drm_noop, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPROPERTY, drm_mode_getproperty_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_SETPROPERTY, drm_mode_connector_property_set_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETPROPBLOB, drm_mode_getblob_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_GETFB, drm_mode_getfb, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ADDFB, drm_mode_addfb, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_ADDFB2, drm_mode_addfb2, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_RMFB, drm_mode_rmfb, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_PAGE_FLIP, drm_mode_page_flip_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DIRTYFB, drm_mode_dirtyfb_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CREATE_DUMB, drm_mode_create_dumb_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_MAP_DUMB, drm_mode_mmap_dumb_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_DESTROY_DUMB, drm_mode_destroy_dumb_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_GETPROPERTIES, drm_mode_obj_get_properties_ioctl, DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_OBJ_SETPROPERTY, drm_mode_obj_set_property_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+	DRM_IOCTL_DEF(DRM_IOCTL_MODE_CURSOR2, drm_mode_cursor2_ioctl, DRM_MASTER|DRM_CONTROL_ALLOW|DRM_UNLOCKED),
+};
+
+#define DRM_CORE_IOCTL_COUNT	ARRAY_SIZE( drm_ioctls )
+
 /**
  * Called whenever a process performs an ioctl on /dev/drm.
  *
diff --git a/drivers/gpu/drm/drm_irq.c b/drivers/gpu/drm/drm_irq.c
index 08ba1209228e..5ef03c216a27 100644
--- a/drivers/gpu/drm/drm_irq.c
+++ b/drivers/gpu/drm/drm_irq.c
@@ -34,6 +34,7 @@
 
 #include <drm/drmP.h>
 #include "drm_trace.h"
+#include "drm_internal.h"
 
 #include <linux/interrupt.h>	/* For task queue support */
 #include <linux/slab.h>
@@ -55,12 +56,91 @@
  */
 #define DRM_REDUNDANT_VBLIRQ_THRESH_NS 1000000
 
+static bool
+drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
+			  struct timeval *tvblank, unsigned flags);
+
+static unsigned int drm_timestamp_precision = 20;  /* Default to 20 usecs. */
+
 /*
- * Clear vblank timestamp buffer for a crtc.
+ * Default to use monotonic timestamps for wait-for-vblank and page-flip
+ * complete events.
+ */
+unsigned int drm_timestamp_monotonic = 1;
+
+static int drm_vblank_offdelay = 5000;    /* Default to 5000 msecs. */
+
+module_param_named(vblankoffdelay, drm_vblank_offdelay, int, 0600);
+module_param_named(timestamp_precision_usec, drm_timestamp_precision, int, 0600);
+module_param_named(timestamp_monotonic, drm_timestamp_monotonic, int, 0600);
+
+/**
+ * drm_update_vblank_count - update the master vblank counter
+ * @dev: DRM device
+ * @crtc: counter to update
+ *
+ * Call back into the driver to update the appropriate vblank counter
+ * (specified by @crtc).  Deal with wraparound, if it occurred, and
+ * update the last read value so we can deal with wraparound on the next
+ * call if necessary.
+ *
+ * Only necessary when going from off->on, to account for frames we
+ * didn't get an interrupt for.
+ *
+ * Note: caller must hold dev->vbl_lock since this reads & writes
+ * device vblank fields.
  */
-static void clear_vblank_timestamps(struct drm_device *dev, int crtc)
+static void drm_update_vblank_count(struct drm_device *dev, int crtc)
 {
-	memset(dev->vblank[crtc].time, 0, sizeof(dev->vblank[crtc].time));
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
+	u32 cur_vblank, diff, tslot;
+	bool rc;
+	struct timeval t_vblank;
+
+	/*
+	 * Interrupts were disabled prior to this call, so deal with counter
+	 * wrap if needed.
+	 * NOTE!  It's possible we lost a full dev->max_vblank_count events
+	 * here if the register is small or we had vblank interrupts off for
+	 * a long time.
+	 *
+	 * We repeat the hardware vblank counter & timestamp query until
+	 * we get consistent results. This to prevent races between gpu
+	 * updating its hardware counter while we are retrieving the
+	 * corresponding vblank timestamp.
+	 */
+	do {
+		cur_vblank = dev->driver->get_vblank_counter(dev, crtc);
+		rc = drm_get_last_vbltimestamp(dev, crtc, &t_vblank, 0);
+	} while (cur_vblank != dev->driver->get_vblank_counter(dev, crtc));
+
+	/* Deal with counter wrap */
+	diff = cur_vblank - vblank->last;
+	if (cur_vblank < vblank->last) {
+		diff += dev->max_vblank_count;
+
+		DRM_DEBUG("last_vblank[%d]=0x%x, cur_vblank=0x%x => diff=0x%x\n",
+			  crtc, vblank->last, cur_vblank, diff);
+	}
+
+	DRM_DEBUG("updating vblank count on crtc %d, missed %d\n",
+		  crtc, diff);
+
+	if (diff == 0)
+		return;
+
+	/* Reinitialize corresponding vblank timestamp if high-precision query
+	 * available. Skip this step if query unsupported or failed. Will
+	 * reinitialize delayed at next vblank interrupt in that case.
+	 */
+	if (rc) {
+		tslot = atomic_read(&vblank->count) + diff;
+		vblanktimestamp(dev, crtc, tslot) = t_vblank;
+	}
+
+	smp_mb__before_atomic();
+	atomic_add(diff, &vblank->count);
+	smp_mb__after_atomic();
 }
 
 /*
@@ -71,10 +151,11 @@ static void clear_vblank_timestamps(struct drm_device *dev, int crtc)
  */
 static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	unsigned long irqflags;
 	u32 vblcount;
 	s64 diff_ns;
-	int vblrc;
+	bool vblrc;
 	struct timeval tvblank;
 	int count = DRM_TIMESTAMP_MAXRETRIES;
 
@@ -84,8 +165,28 @@ static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 	 */
 	spin_lock_irqsave(&dev->vblank_time_lock, irqflags);
 
+	/*
+	 * If the vblank interrupt was already disbled update the count
+	 * and timestamp to maintain the appearance that the counter
+	 * has been ticking all along until this time. This makes the
+	 * count account for the entire time between drm_vblank_on() and
+	 * drm_vblank_off().
+	 *
+	 * But only do this if precise vblank timestamps are available.
+	 * Otherwise we might read a totally bogus timestamp since drivers
+	 * lacking precise timestamp support rely upon sampling the system clock
+	 * at vblank interrupt time. Which obviously won't work out well if the
+	 * vblank interrupt is disabled.
+	 */
+	if (!vblank->enabled &&
+	    drm_get_last_vbltimestamp(dev, crtc, &tvblank, 0)) {
+		drm_update_vblank_count(dev, crtc);
+		spin_unlock_irqrestore(&dev->vblank_time_lock, irqflags);
+		return;
+	}
+
 	dev->driver->disable_vblank(dev, crtc);
-	dev->vblank[crtc].enabled = false;
+	vblank->enabled = false;
 
 	/* No further vblank irq's will be processed after
 	 * this point. Get current hardware vblank count and
@@ -100,9 +201,9 @@ static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 	 * delayed gpu counter increment.
 	 */
 	do {
-		dev->vblank[crtc].last = dev->driver->get_vblank_counter(dev, crtc);
+		vblank->last = dev->driver->get_vblank_counter(dev, crtc);
 		vblrc = drm_get_last_vbltimestamp(dev, crtc, &tvblank, 0);
-	} while (dev->vblank[crtc].last != dev->driver->get_vblank_counter(dev, crtc) && (--count) && vblrc);
+	} while (vblank->last != dev->driver->get_vblank_counter(dev, crtc) && (--count) && vblrc);
 
 	if (!count)
 		vblrc = 0;
@@ -110,7 +211,7 @@ static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 	/* Compute time difference to stored timestamp of last vblank
 	 * as updated by last invocation of drm_handle_vblank() in vblank irq.
 	 */
-	vblcount = atomic_read(&dev->vblank[crtc].count);
+	vblcount = atomic_read(&vblank->count);
 	diff_ns = timeval_to_ns(&tvblank) -
 		  timeval_to_ns(&vblanktimestamp(dev, crtc, vblcount));
 
@@ -126,14 +227,18 @@ static void vblank_disable_and_save(struct drm_device *dev, int crtc)
 	 * available. In that case we can't account for this and just
 	 * hope for the best.
 	 */
-	if ((vblrc > 0) && (abs64(diff_ns) > 1000000)) {
-		atomic_inc(&dev->vblank[crtc].count);
+	if (vblrc && (abs64(diff_ns) > 1000000)) {
+		/* Store new timestamp in ringbuffer. */
+		vblanktimestamp(dev, crtc, vblcount + 1) = tvblank;
+
+		/* Increment cooked vblank count. This also atomically commits
+		 * the timestamp computed above.
+		 */
+		smp_mb__before_atomic();
+		atomic_inc(&vblank->count);
 		smp_mb__after_atomic();
 	}
 
-	/* Invalidate all timestamps while vblank irq's are off. */
-	clear_vblank_timestamps(dev, crtc);
-
 	spin_unlock_irqrestore(&dev->vblank_time_lock, irqflags);
 }
 
@@ -164,14 +269,20 @@ static void vblank_disable_fn(unsigned long arg)
 void drm_vblank_cleanup(struct drm_device *dev)
 {
 	int crtc;
+	unsigned long irqflags;
 
 	/* Bail if the driver didn't call drm_vblank_init() */
 	if (dev->num_crtcs == 0)
 		return;
 
 	for (crtc = 0; crtc < dev->num_crtcs; crtc++) {
-		del_timer_sync(&dev->vblank[crtc].disable_timer);
-		vblank_disable_fn((unsigned long)&dev->vblank[crtc]);
+		struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
+
+		del_timer_sync(&vblank->disable_timer);
+
+		spin_lock_irqsave(&dev->vbl_lock, irqflags);
+		vblank_disable_and_save(dev, crtc);
+		spin_unlock_irqrestore(&dev->vbl_lock, irqflags);
 	}
 
 	kfree(dev->vblank);
@@ -204,11 +315,13 @@ int drm_vblank_init(struct drm_device *dev, int num_crtcs)
 		goto err;
 
 	for (i = 0; i < num_crtcs; i++) {
-		dev->vblank[i].dev = dev;
-		dev->vblank[i].crtc = i;
-		init_waitqueue_head(&dev->vblank[i].queue);
-		setup_timer(&dev->vblank[i].disable_timer, vblank_disable_fn,
-			    (unsigned long)&dev->vblank[i]);
+		struct drm_vblank_crtc *vblank = &dev->vblank[i];
+
+		vblank->dev = dev;
+		vblank->crtc = i;
+		init_waitqueue_head(&vblank->queue);
+		setup_timer(&vblank->disable_timer, vblank_disable_fn,
+			    (unsigned long)vblank);
 	}
 
 	DRM_INFO("Supports vblank timestamp caching Rev 2 (21.10.2013).\n");
@@ -224,7 +337,7 @@ int drm_vblank_init(struct drm_device *dev, int num_crtcs)
 	return 0;
 
 err:
-	drm_vblank_cleanup(dev);
+	dev->num_crtcs = 0;
 	return ret;
 }
 EXPORT_SYMBOL(drm_vblank_init);
@@ -360,9 +473,11 @@ int drm_irq_uninstall(struct drm_device *dev)
 	if (dev->num_crtcs) {
 		spin_lock_irqsave(&dev->vbl_lock, irqflags);
 		for (i = 0; i < dev->num_crtcs; i++) {
-			wake_up(&dev->vblank[i].queue);
-			dev->vblank[i].enabled = false;
-			dev->vblank[i].last =
+			struct drm_vblank_crtc *vblank = &dev->vblank[i];
+
+			wake_up(&vblank->queue);
+			vblank->enabled = false;
+			vblank->last =
 				dev->driver->get_vblank_counter(dev, i);
 		}
 		spin_unlock_irqrestore(&dev->vbl_lock, irqflags);
@@ -617,7 +732,7 @@ int drm_calc_vbltimestamp_from_scanoutpos(struct drm_device *dev, int crtc,
 	 * within vblank area, counting down the number of lines until
 	 * start of scanout.
 	 */
-	invbl = vbl_status & DRM_SCANOUTPOS_INVBL;
+	invbl = vbl_status & DRM_SCANOUTPOS_IN_VBLANK;
 
 	/* Convert scanout position into elapsed time at raw_time query
 	 * since start of scanout at first display scanline. delta_ns
@@ -647,7 +762,7 @@ int drm_calc_vbltimestamp_from_scanoutpos(struct drm_device *dev, int crtc,
 
 	vbl_status = DRM_VBLANKTIME_SCANOUTPOS_METHOD;
 	if (invbl)
-		vbl_status |= DRM_VBLANKTIME_INVBL;
+		vbl_status |= DRM_VBLANKTIME_IN_VBLANK;
 
 	return vbl_status;
 }
@@ -679,10 +794,11 @@ static struct timeval get_drm_timestamp(void)
  * call, i.e., it isn't very precisely locked to the true vblank.
  *
  * Returns:
- * Non-zero if timestamp is considered to be very precise, zero otherwise.
+ * True if timestamp is considered to be very precise, false otherwise.
  */
-u32 drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
-			      struct timeval *tvblank, unsigned flags)
+static bool
+drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
+			  struct timeval *tvblank, unsigned flags)
 {
 	int ret;
 
@@ -694,7 +810,7 @@ u32 drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
 		ret = dev->driver->get_vblank_timestamp(dev, crtc, &max_error,
 							tvblank, flags);
 		if (ret > 0)
-			return (u32) ret;
+			return true;
 	}
 
 	/* GPU high precision timestamp query unsupported or failed.
@@ -702,9 +818,8 @@ u32 drm_get_last_vbltimestamp(struct drm_device *dev, int crtc,
 	 */
 	*tvblank = get_drm_timestamp();
 
-	return 0;
+	return false;
 }
-EXPORT_SYMBOL(drm_get_last_vbltimestamp);
 
 /**
  * drm_vblank_count - retrieve "cooked" vblank counter value
@@ -720,7 +835,11 @@ EXPORT_SYMBOL(drm_get_last_vbltimestamp);
  */
 u32 drm_vblank_count(struct drm_device *dev, int crtc)
 {
-	return atomic_read(&dev->vblank[crtc].count);
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
+
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return 0;
+	return atomic_read(&vblank->count);
 }
 EXPORT_SYMBOL(drm_vblank_count);
 
@@ -740,18 +859,22 @@ EXPORT_SYMBOL(drm_vblank_count);
 u32 drm_vblank_count_and_time(struct drm_device *dev, int crtc,
 			      struct timeval *vblanktime)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	u32 cur_vblank;
 
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return 0;
+
 	/* Read timestamp from slot of _vblank_time ringbuffer
 	 * that corresponds to current vblank count. Retry if
 	 * count has incremented during readout. This works like
 	 * a seqlock.
 	 */
 	do {
-		cur_vblank = atomic_read(&dev->vblank[crtc].count);
+		cur_vblank = atomic_read(&vblank->count);
 		*vblanktime = vblanktimestamp(dev, crtc, cur_vblank);
 		smp_rmb();
-	} while (cur_vblank != atomic_read(&dev->vblank[crtc].count));
+	} while (cur_vblank != atomic_read(&vblank->count));
 
 	return cur_vblank;
 }
@@ -800,83 +923,20 @@ void drm_send_vblank_event(struct drm_device *dev, int crtc,
 EXPORT_SYMBOL(drm_send_vblank_event);
 
 /**
- * drm_update_vblank_count - update the master vblank counter
- * @dev: DRM device
- * @crtc: counter to update
- *
- * Call back into the driver to update the appropriate vblank counter
- * (specified by @crtc).  Deal with wraparound, if it occurred, and
- * update the last read value so we can deal with wraparound on the next
- * call if necessary.
- *
- * Only necessary when going from off->on, to account for frames we
- * didn't get an interrupt for.
- *
- * Note: caller must hold dev->vbl_lock since this reads & writes
- * device vblank fields.
- */
-static void drm_update_vblank_count(struct drm_device *dev, int crtc)
-{
-	u32 cur_vblank, diff, tslot, rc;
-	struct timeval t_vblank;
-
-	/*
-	 * Interrupts were disabled prior to this call, so deal with counter
-	 * wrap if needed.
-	 * NOTE!  It's possible we lost a full dev->max_vblank_count events
-	 * here if the register is small or we had vblank interrupts off for
-	 * a long time.
-	 *
-	 * We repeat the hardware vblank counter & timestamp query until
-	 * we get consistent results. This to prevent races between gpu
-	 * updating its hardware counter while we are retrieving the
-	 * corresponding vblank timestamp.
-	 */
-	do {
-		cur_vblank = dev->driver->get_vblank_counter(dev, crtc);
-		rc = drm_get_last_vbltimestamp(dev, crtc, &t_vblank, 0);
-	} while (cur_vblank != dev->driver->get_vblank_counter(dev, crtc));
-
-	/* Deal with counter wrap */
-	diff = cur_vblank - dev->vblank[crtc].last;
-	if (cur_vblank < dev->vblank[crtc].last) {
-		diff += dev->max_vblank_count;
-
-		DRM_DEBUG("last_vblank[%d]=0x%x, cur_vblank=0x%x => diff=0x%x\n",
-			  crtc, dev->vblank[crtc].last, cur_vblank, diff);
-	}
-
-	DRM_DEBUG("enabling vblank interrupts on crtc %d, missed %d\n",
-		  crtc, diff);
-
-	/* Reinitialize corresponding vblank timestamp if high-precision query
-	 * available. Skip this step if query unsupported or failed. Will
-	 * reinitialize delayed at next vblank interrupt in that case.
-	 */
-	if (rc) {
-		tslot = atomic_read(&dev->vblank[crtc].count) + diff;
-		vblanktimestamp(dev, crtc, tslot) = t_vblank;
-	}
-
-	smp_mb__before_atomic();
-	atomic_add(diff, &dev->vblank[crtc].count);
-	smp_mb__after_atomic();
-}
-
-/**
  * drm_vblank_enable - enable the vblank interrupt on a CRTC
  * @dev: DRM device
  * @crtc: CRTC in question
  */
 static int drm_vblank_enable(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	int ret = 0;
 
 	assert_spin_locked(&dev->vbl_lock);
 
 	spin_lock(&dev->vblank_time_lock);
 
-	if (!dev->vblank[crtc].enabled) {
+	if (!vblank->enabled) {
 		/*
 		 * Enable vblank irqs under vblank_time_lock protection.
 		 * All vblank count & timestamp updates are held off
@@ -887,9 +947,9 @@ static int drm_vblank_enable(struct drm_device *dev, int crtc)
 		ret = dev->driver->enable_vblank(dev, crtc);
 		DRM_DEBUG("enabling vblank on crtc %d, ret: %d\n", crtc, ret);
 		if (ret)
-			atomic_dec(&dev->vblank[crtc].refcount);
+			atomic_dec(&vblank->refcount);
 		else {
-			dev->vblank[crtc].enabled = true;
+			vblank->enabled = true;
 			drm_update_vblank_count(dev, crtc);
 		}
 	}
@@ -914,16 +974,20 @@ static int drm_vblank_enable(struct drm_device *dev, int crtc)
  */
 int drm_vblank_get(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	unsigned long irqflags;
 	int ret = 0;
 
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return -EINVAL;
+
 	spin_lock_irqsave(&dev->vbl_lock, irqflags);
 	/* Going from 0->1 means we have to enable interrupts again */
-	if (atomic_add_return(1, &dev->vblank[crtc].refcount) == 1) {
+	if (atomic_add_return(1, &vblank->refcount) == 1) {
 		ret = drm_vblank_enable(dev, crtc);
 	} else {
-		if (!dev->vblank[crtc].enabled) {
-			atomic_dec(&dev->vblank[crtc].refcount);
+		if (!vblank->enabled) {
+			atomic_dec(&vblank->refcount);
 			ret = -EINVAL;
 		}
 	}
@@ -963,13 +1027,23 @@ EXPORT_SYMBOL(drm_crtc_vblank_get);
  */
 void drm_vblank_put(struct drm_device *dev, int crtc)
 {
-	BUG_ON(atomic_read(&dev->vblank[crtc].refcount) == 0);
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
+
+	BUG_ON(atomic_read(&vblank->refcount) == 0);
+
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return;
 
 	/* Last user schedules interrupt disable */
-	if (atomic_dec_and_test(&dev->vblank[crtc].refcount) &&
-	    (drm_vblank_offdelay > 0))
-		mod_timer(&dev->vblank[crtc].disable_timer,
-			  jiffies + ((drm_vblank_offdelay * HZ)/1000));
+	if (atomic_dec_and_test(&vblank->refcount)) {
+		if (drm_vblank_offdelay == 0)
+			return;
+		else if (dev->vblank_disable_immediate || drm_vblank_offdelay < 0)
+			vblank_disable_fn((unsigned long)vblank);
+		else
+			mod_timer(&vblank->disable_timer,
+				  jiffies + ((drm_vblank_offdelay * HZ)/1000));
+	}
 }
 EXPORT_SYMBOL(drm_vblank_put);
 
@@ -989,6 +1063,50 @@ void drm_crtc_vblank_put(struct drm_crtc *crtc)
 EXPORT_SYMBOL(drm_crtc_vblank_put);
 
 /**
+ * drm_wait_one_vblank - wait for one vblank
+ * @dev: DRM device
+ * @crtc: crtc index
+ *
+ * This waits for one vblank to pass on @crtc, using the irq driver interfaces.
+ * It is a failure to call this when the vblank irq for @crtc is disabled, e.g.
+ * due to lack of driver support or because the crtc is off.
+ */
+void drm_wait_one_vblank(struct drm_device *dev, int crtc)
+{
+	int ret;
+	u32 last;
+
+	ret = drm_vblank_get(dev, crtc);
+	if (WARN(ret, "vblank not available on crtc %i, ret=%i\n", crtc, ret))
+		return;
+
+	last = drm_vblank_count(dev, crtc);
+
+	ret = wait_event_timeout(dev->vblank[crtc].queue,
+				 last != drm_vblank_count(dev, crtc),
+				 msecs_to_jiffies(100));
+
+	WARN(ret == 0, "vblank wait timed out on crtc %i\n", crtc);
+
+	drm_vblank_put(dev, crtc);
+}
+EXPORT_SYMBOL(drm_wait_one_vblank);
+
+/**
+ * drm_crtc_wait_one_vblank - wait for one vblank
+ * @crtc: DRM crtc
+ *
+ * This waits for one vblank to pass on @crtc, using the irq driver interfaces.
+ * It is a failure to call this when the vblank irq for @crtc is disabled, e.g.
+ * due to lack of driver support or because the crtc is off.
+ */
+void drm_crtc_wait_one_vblank(struct drm_crtc *crtc)
+{
+	drm_wait_one_vblank(crtc->dev, drm_crtc_index(crtc));
+}
+EXPORT_SYMBOL(drm_crtc_wait_one_vblank);
+
+/**
  * drm_vblank_off - disable vblank events on a CRTC
  * @dev: DRM device
  * @crtc: CRTC in question
@@ -1004,19 +1122,34 @@ EXPORT_SYMBOL(drm_crtc_vblank_put);
  */
 void drm_vblank_off(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	struct drm_pending_vblank_event *e, *t;
 	struct timeval now;
 	unsigned long irqflags;
 	unsigned int seq;
 
-	spin_lock_irqsave(&dev->vbl_lock, irqflags);
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return;
+
+	spin_lock_irqsave(&dev->event_lock, irqflags);
+
+	spin_lock(&dev->vbl_lock);
 	vblank_disable_and_save(dev, crtc);
-	wake_up(&dev->vblank[crtc].queue);
+	wake_up(&vblank->queue);
+
+	/*
+	 * Prevent subsequent drm_vblank_get() from re-enabling
+	 * the vblank interrupt by bumping the refcount.
+	 */
+	if (!vblank->inmodeset) {
+		atomic_inc(&vblank->refcount);
+		vblank->inmodeset = 1;
+	}
+	spin_unlock(&dev->vbl_lock);
 
 	/* Send any queued vblank events, lest the natives grow disquiet */
 	seq = drm_vblank_count_and_time(dev, crtc, &now);
 
-	spin_lock(&dev->event_lock);
 	list_for_each_entry_safe(e, t, &dev->vblank_event_list, base.link) {
 		if (e->pipe != crtc)
 			continue;
@@ -1027,9 +1160,7 @@ void drm_vblank_off(struct drm_device *dev, int crtc)
 		drm_vblank_put(dev, e->pipe);
 		send_vblank_event(dev, e, seq, &now);
 	}
-	spin_unlock(&dev->event_lock);
-
-	spin_unlock_irqrestore(&dev->vbl_lock, irqflags);
+	spin_unlock_irqrestore(&dev->event_lock, irqflags);
 }
 EXPORT_SYMBOL(drm_vblank_off);
 
@@ -1066,11 +1197,35 @@ EXPORT_SYMBOL(drm_crtc_vblank_off);
  */
 void drm_vblank_on(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	unsigned long irqflags;
 
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return;
+
 	spin_lock_irqsave(&dev->vbl_lock, irqflags);
-	/* re-enable interrupts if there's are users left */
-	if (atomic_read(&dev->vblank[crtc].refcount) != 0)
+	/* Drop our private "prevent drm_vblank_get" refcount */
+	if (vblank->inmodeset) {
+		atomic_dec(&vblank->refcount);
+		vblank->inmodeset = 0;
+	}
+
+	/*
+	 * sample the current counter to avoid random jumps
+	 * when drm_vblank_enable() applies the diff
+	 *
+	 * -1 to make sure user will never see the same
+	 * vblank counter value before and after a modeset
+	 */
+	vblank->last =
+		(dev->driver->get_vblank_counter(dev, crtc) - 1) &
+		dev->max_vblank_count;
+	/*
+	 * re-enable interrupts if there are users left, or the
+	 * user wishes vblank interrupts to be enabled all the time.
+	 */
+	if (atomic_read(&vblank->refcount) != 0 ||
+	    (!dev->vblank_disable_immediate && drm_vblank_offdelay == 0))
 		WARN_ON(drm_vblank_enable(dev, crtc));
 	spin_unlock_irqrestore(&dev->vbl_lock, irqflags);
 }
@@ -1118,9 +1273,15 @@ EXPORT_SYMBOL(drm_crtc_vblank_on);
  */
 void drm_vblank_pre_modeset(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
+
 	/* vblank is not initialized (IRQ not installed ?), or has been freed */
 	if (!dev->num_crtcs)
 		return;
+
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return;
+
 	/*
 	 * To avoid all the problems that might happen if interrupts
 	 * were enabled/disabled around or between these calls, we just
@@ -1128,10 +1289,10 @@ void drm_vblank_pre_modeset(struct drm_device *dev, int crtc)
 	 * to avoid corrupting the count if multiple, mismatch calls occur),
 	 * so that interrupts remain enabled in the interim.
 	 */
-	if (!dev->vblank[crtc].inmodeset) {
-		dev->vblank[crtc].inmodeset = 0x1;
+	if (!vblank->inmodeset) {
+		vblank->inmodeset = 0x1;
 		if (drm_vblank_get(dev, crtc) == 0)
-			dev->vblank[crtc].inmodeset |= 0x2;
+			vblank->inmodeset |= 0x2;
 	}
 }
 EXPORT_SYMBOL(drm_vblank_pre_modeset);
@@ -1146,21 +1307,22 @@ EXPORT_SYMBOL(drm_vblank_pre_modeset);
  */
 void drm_vblank_post_modeset(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	unsigned long irqflags;
 
 	/* vblank is not initialized (IRQ not installed ?), or has been freed */
 	if (!dev->num_crtcs)
 		return;
 
-	if (dev->vblank[crtc].inmodeset) {
+	if (vblank->inmodeset) {
 		spin_lock_irqsave(&dev->vbl_lock, irqflags);
 		dev->vblank_disable_allowed = true;
 		spin_unlock_irqrestore(&dev->vbl_lock, irqflags);
 
-		if (dev->vblank[crtc].inmodeset & 0x2)
+		if (vblank->inmodeset & 0x2)
 			drm_vblank_put(dev, crtc);
 
-		dev->vblank[crtc].inmodeset = 0;
+		vblank->inmodeset = 0;
 	}
 }
 EXPORT_SYMBOL(drm_vblank_post_modeset);
@@ -1212,6 +1374,7 @@ static int drm_queue_vblank_event(struct drm_device *dev, int pipe,
 				  union drm_wait_vblank *vblwait,
 				  struct drm_file *file_priv)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[pipe];
 	struct drm_pending_vblank_event *e;
 	struct timeval now;
 	unsigned long flags;
@@ -1235,6 +1398,18 @@ static int drm_queue_vblank_event(struct drm_device *dev, int pipe,
 
 	spin_lock_irqsave(&dev->event_lock, flags);
 
+	/*
+	 * drm_vblank_off() might have been called after we called
+	 * drm_vblank_get(). drm_vblank_off() holds event_lock
+	 * around the vblank disable, so no need for further locking.
+	 * The reference from drm_vblank_get() protects against
+	 * vblank disable from another source.
+	 */
+	if (!vblank->enabled) {
+		ret = -EINVAL;
+		goto err_unlock;
+	}
+
 	if (file_priv->event_space < sizeof e->event) {
 		ret = -EBUSY;
 		goto err_unlock;
@@ -1295,6 +1470,7 @@ err_put:
 int drm_wait_vblank(struct drm_device *dev, void *data,
 		    struct drm_file *file_priv)
 {
+	struct drm_vblank_crtc *vblank;
 	union drm_wait_vblank *vblwait = data;
 	int ret;
 	unsigned int flags, seq, crtc, high_crtc;
@@ -1324,6 +1500,8 @@ int drm_wait_vblank(struct drm_device *dev, void *data,
 	if (crtc >= dev->num_crtcs)
 		return -EINVAL;
 
+	vblank = &dev->vblank[crtc];
+
 	ret = drm_vblank_get(dev, crtc);
 	if (ret) {
 		DRM_DEBUG("failed to acquire vblank counter, %d\n", ret);
@@ -1356,11 +1534,11 @@ int drm_wait_vblank(struct drm_device *dev, void *data,
 
 	DRM_DEBUG("waiting on vblank count %d, crtc %d\n",
 		  vblwait->request.sequence, crtc);
-	dev->vblank[crtc].last_wait = vblwait->request.sequence;
-	DRM_WAIT_ON(ret, dev->vblank[crtc].queue, 3 * HZ,
+	vblank->last_wait = vblwait->request.sequence;
+	DRM_WAIT_ON(ret, vblank->queue, 3 * HZ,
 		    (((drm_vblank_count(dev, crtc) -
 		       vblwait->request.sequence) <= (1 << 23)) ||
-		     !dev->vblank[crtc].enabled ||
+		     !vblank->enabled ||
 		     !dev->irq_enabled));
 
 	if (ret != -EINTR) {
@@ -1385,12 +1563,11 @@ static void drm_handle_vblank_events(struct drm_device *dev, int crtc)
 {
 	struct drm_pending_vblank_event *e, *t;
 	struct timeval now;
-	unsigned long flags;
 	unsigned int seq;
 
-	seq = drm_vblank_count_and_time(dev, crtc, &now);
+	assert_spin_locked(&dev->event_lock);
 
-	spin_lock_irqsave(&dev->event_lock, flags);
+	seq = drm_vblank_count_and_time(dev, crtc, &now);
 
 	list_for_each_entry_safe(e, t, &dev->vblank_event_list, base.link) {
 		if (e->pipe != crtc)
@@ -1406,8 +1583,6 @@ static void drm_handle_vblank_events(struct drm_device *dev, int crtc)
 		send_vblank_event(dev, e, seq, &now);
 	}
 
-	spin_unlock_irqrestore(&dev->event_lock, flags);
-
 	trace_drm_vblank_event(crtc, seq);
 }
 
@@ -1421,6 +1596,7 @@ static void drm_handle_vblank_events(struct drm_device *dev, int crtc)
  */
 bool drm_handle_vblank(struct drm_device *dev, int crtc)
 {
+	struct drm_vblank_crtc *vblank = &dev->vblank[crtc];
 	u32 vblcount;
 	s64 diff_ns;
 	struct timeval tvblank;
@@ -1429,15 +1605,21 @@ bool drm_handle_vblank(struct drm_device *dev, int crtc)
 	if (!dev->num_crtcs)
 		return false;
 
+	if (WARN_ON(crtc >= dev->num_crtcs))
+		return false;
+
+	spin_lock_irqsave(&dev->event_lock, irqflags);
+
 	/* Need timestamp lock to prevent concurrent execution with
 	 * vblank enable/disable, as this would cause inconsistent
 	 * or corrupted timestamps and vblank counts.
 	 */
-	spin_lock_irqsave(&dev->vblank_time_lock, irqflags);
+	spin_lock(&dev->vblank_time_lock);
 
 	/* Vblank irq handling disabled. Nothing to do. */
-	if (!dev->vblank[crtc].enabled) {
-		spin_unlock_irqrestore(&dev->vblank_time_lock, irqflags);
+	if (!vblank->enabled) {
+		spin_unlock(&dev->vblank_time_lock);
+		spin_unlock_irqrestore(&dev->event_lock, irqflags);
 		return false;
 	}
 
@@ -1446,7 +1628,7 @@ bool drm_handle_vblank(struct drm_device *dev, int crtc)
 	 */
 
 	/* Get current timestamp and count. */
-	vblcount = atomic_read(&dev->vblank[crtc].count);
+	vblcount = atomic_read(&vblank->count);
 	drm_get_last_vbltimestamp(dev, crtc, &tvblank, DRM_CALLED_FROM_VBLIRQ);
 
 	/* Compute time difference to timestamp of last vblank */
@@ -1470,17 +1652,20 @@ bool drm_handle_vblank(struct drm_device *dev, int crtc)
 		 * the timestamp computed above.
 		 */
 		smp_mb__before_atomic();
-		atomic_inc(&dev->vblank[crtc].count);
+		atomic_inc(&vblank->count);
 		smp_mb__after_atomic();
 	} else {
 		DRM_DEBUG("crtc %d: Redundant vblirq ignored. diff_ns = %d\n",
 			  crtc, (int) diff_ns);
 	}
 
-	wake_up(&dev->vblank[crtc].queue);
+	spin_unlock(&dev->vblank_time_lock);
+
+	wake_up(&vblank->queue);
 	drm_handle_vblank_events(dev, crtc);
 
-	spin_unlock_irqrestore(&dev->vblank_time_lock, irqflags);
+	spin_unlock_irqrestore(&dev->event_lock, irqflags);
+
 	return true;
 }
 EXPORT_SYMBOL(drm_handle_vblank);
diff --git a/drivers/gpu/drm/drm_legacy.h b/drivers/gpu/drm/drm_legacy.h
index d34f20a79b7c..c1dc61473db5 100644
--- a/drivers/gpu/drm/drm_legacy.h
+++ b/drivers/gpu/drm/drm_legacy.h
@@ -23,6 +23,15 @@
  * OTHER DEALINGS IN THE SOFTWARE.
  */
 
+/*
+ * This file contains legacy interfaces that modern drm drivers
+ * should no longer be using. They cannot be removed as legacy
+ * drivers use them, and removing them are API breaks.
+ */
+#include <linux/list.h>
+#include <drm/drm_legacy.h>
+
+struct agp_memory;
 struct drm_device;
 struct drm_file;
 
@@ -48,4 +57,57 @@ int drm_legacy_rmctx(struct drm_device *d, void *v, struct drm_file *f);
 int drm_legacy_setsareactx(struct drm_device *d, void *v, struct drm_file *f);
 int drm_legacy_getsareactx(struct drm_device *d, void *v, struct drm_file *f);
 
+/*
+ * Generic Buffer Management
+ */
+
+#define DRM_MAP_HASH_OFFSET 0x10000000
+
+int drm_legacy_addmap_ioctl(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_rmmap_ioctl(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_addbufs(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_infobufs(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_markbufs(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_freebufs(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_mapbufs(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_dma_ioctl(struct drm_device *d, void *v, struct drm_file *f);
+
+void drm_legacy_vma_flush(struct drm_device *d);
+
+/*
+ * AGP Support
+ */
+
+struct drm_agp_mem {
+	unsigned long handle;
+	struct agp_memory *memory;
+	unsigned long bound;
+	int pages;
+	struct list_head head;
+};
+
+/*
+ * Generic Userspace Locking-API
+ */
+
+int drm_legacy_i_have_hw_lock(struct drm_device *d, struct drm_file *f);
+int drm_legacy_lock(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_unlock(struct drm_device *d, void *v, struct drm_file *f);
+int drm_legacy_lock_free(struct drm_lock_data *lock, unsigned int ctx);
+
+/* DMA support */
+int drm_legacy_dma_setup(struct drm_device *dev);
+void drm_legacy_dma_takedown(struct drm_device *dev);
+void drm_legacy_free_buffer(struct drm_device *dev,
+			    struct drm_buf * buf);
+void drm_legacy_reclaim_buffers(struct drm_device *dev,
+				struct drm_file *filp);
+
+/* Scatter Gather Support */
+void drm_legacy_sg_cleanup(struct drm_device *dev);
+int drm_legacy_sg_alloc(struct drm_device *dev, void *data,
+			struct drm_file *file_priv);
+int drm_legacy_sg_free(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv);
+
 #endif /* __DRM_LEGACY_H__ */
diff --git a/drivers/gpu/drm/drm_lock.c b/drivers/gpu/drm/drm_lock.c
index e26b59e385ff..f861361a635e 100644
--- a/drivers/gpu/drm/drm_lock.c
+++ b/drivers/gpu/drm/drm_lock.c
@@ -36,6 +36,7 @@
 #include <linux/export.h>
 #include <drm/drmP.h>
 #include "drm_legacy.h"
+#include "drm_internal.h"
 
 static int drm_notifier(void *priv);
 
@@ -52,7 +53,8 @@ static int drm_lock_take(struct drm_lock_data *lock_data, unsigned int context);
  *
  * Add the current task to the lock wait queue, and attempt to take to lock.
  */
-int drm_lock(struct drm_device *dev, void *data, struct drm_file *file_priv)
+int drm_legacy_lock(struct drm_device *dev, void *data,
+		    struct drm_file *file_priv)
 {
 	DECLARE_WAITQUEUE(entry, current);
 	struct drm_lock *lock = data;
@@ -120,7 +122,7 @@ int drm_lock(struct drm_device *dev, void *data, struct drm_file *file_priv)
 		sigaddset(&dev->sigmask, SIGTTOU);
 		dev->sigdata.context = lock->context;
 		dev->sigdata.lock = master->lock.hw_lock;
-		block_all_signals(drm_notifier, &dev->sigdata, &dev->sigmask);
+		block_all_signals(drm_notifier, dev, &dev->sigmask);
 	}
 
 	if (dev->driver->dma_quiescent && (lock->flags & _DRM_LOCK_QUIESCENT))
@@ -146,7 +148,7 @@ int drm_lock(struct drm_device *dev, void *data, struct drm_file *file_priv)
  *
  * Transfer and free the lock.
  */
-int drm_unlock(struct drm_device *dev, void *data, struct drm_file *file_priv)
+int drm_legacy_unlock(struct drm_device *dev, void *data, struct drm_file *file_priv)
 {
 	struct drm_lock *lock = data;
 	struct drm_master *master = file_priv->master;
@@ -157,7 +159,7 @@ int drm_unlock(struct drm_device *dev, void *data, struct drm_file *file_priv)
 		return -EINVAL;
 	}
 
-	if (drm_lock_free(&master->lock, lock->context)) {
+	if (drm_legacy_lock_free(&master->lock, lock->context)) {
 		/* FIXME: Should really bail out here. */
 	}
 
@@ -250,7 +252,7 @@ static int drm_lock_transfer(struct drm_lock_data *lock_data,
  * Marks the lock as not held, via the \p cmpxchg instruction. Wakes any task
  * waiting on the lock queue.
  */
-int drm_lock_free(struct drm_lock_data *lock_data, unsigned int context)
+int drm_legacy_lock_free(struct drm_lock_data *lock_data, unsigned int context)
 {
 	unsigned int old, new, prev;
 	volatile unsigned int *lock = &lock_data->hw_lock->lock;
@@ -286,26 +288,27 @@ int drm_lock_free(struct drm_lock_data *lock_data, unsigned int context)
  * If the lock is not held, then let the signal proceed as usual.  If the lock
  * is held, then set the contended flag and keep the signal blocked.
  *
- * \param priv pointer to a drm_sigdata structure.
+ * \param priv pointer to a drm_device structure.
  * \return one if the signal should be delivered normally, or zero if the
  * signal should be blocked.
  */
 static int drm_notifier(void *priv)
 {
-	struct drm_sigdata *s = (struct drm_sigdata *) priv;
+	struct drm_device *dev = priv;
+	struct drm_hw_lock *lock = dev->sigdata.lock;
 	unsigned int old, new, prev;
 
 	/* Allow signal delivery if lock isn't held */
-	if (!s->lock || !_DRM_LOCK_IS_HELD(s->lock->lock)
-	    || _DRM_LOCKING_CONTEXT(s->lock->lock) != s->context)
+	if (!lock || !_DRM_LOCK_IS_HELD(lock->lock)
+	    || _DRM_LOCKING_CONTEXT(lock->lock) != dev->sigdata.context)
 		return 1;
 
 	/* Otherwise, set flag to force call to
 	   drmUnlock */
 	do {
-		old = s->lock->lock;
+		old = lock->lock;
 		new = old | _DRM_LOCK_CONT;
-		prev = cmpxchg(&s->lock->lock, old, new);
+		prev = cmpxchg(&lock->lock, old, new);
 	} while (prev != old);
 	return 0;
 }
@@ -323,7 +326,7 @@ static int drm_notifier(void *priv)
  * having to worry about starvation.
  */
 
-void drm_idlelock_take(struct drm_lock_data *lock_data)
+void drm_legacy_idlelock_take(struct drm_lock_data *lock_data)
 {
 	int ret;
 
@@ -340,9 +343,9 @@ void drm_idlelock_take(struct drm_lock_data *lock_data)
 	}
 	spin_unlock_bh(&lock_data->spinlock);
 }
-EXPORT_SYMBOL(drm_idlelock_take);
+EXPORT_SYMBOL(drm_legacy_idlelock_take);
 
-void drm_idlelock_release(struct drm_lock_data *lock_data)
+void drm_legacy_idlelock_release(struct drm_lock_data *lock_data)
 {
 	unsigned int old, prev;
 	volatile unsigned int *lock = &lock_data->hw_lock->lock;
@@ -360,9 +363,10 @@ void drm_idlelock_release(struct drm_lock_data *lock_data)
 	}
 	spin_unlock_bh(&lock_data->spinlock);
 }
-EXPORT_SYMBOL(drm_idlelock_release);
+EXPORT_SYMBOL(drm_legacy_idlelock_release);
 
-int drm_i_have_hw_lock(struct drm_device *dev, struct drm_file *file_priv)
+int drm_legacy_i_have_hw_lock(struct drm_device *dev,
+			      struct drm_file *file_priv)
 {
 	struct drm_master *master = file_priv->master;
 	return (file_priv->lock_count && master->lock.hw_lock &&
diff --git a/drivers/gpu/drm/drm_memory.c b/drivers/gpu/drm/drm_memory.c
index 00c67c0f2381..a521ef6ff807 100644
--- a/drivers/gpu/drm/drm_memory.c
+++ b/drivers/gpu/drm/drm_memory.c
@@ -36,8 +36,20 @@
 #include <linux/highmem.h>
 #include <linux/export.h>
 #include <drm/drmP.h>
+#include "drm_legacy.h"
 
 #if __OS_HAS_AGP
+
+#ifdef HAVE_PAGE_AGP
+# include <asm/agp.h>
+#else
+# ifdef __powerpc__
+#  define PAGE_AGP	__pgprot(_PAGE_KERNEL | _PAGE_NO_CACHE)
+# else
+#  define PAGE_AGP	PAGE_KERNEL
+# endif
+#endif
+
 static void *agp_remap(unsigned long offset, unsigned long size,
 		       struct drm_device * dev)
 {
@@ -108,25 +120,25 @@ static inline void *agp_remap(unsigned long offset, unsigned long size,
 
 #endif				/* agp */
 
-void drm_core_ioremap(struct drm_local_map *map, struct drm_device *dev)
+void drm_legacy_ioremap(struct drm_local_map *map, struct drm_device *dev)
 {
 	if (dev->agp && dev->agp->cant_use_aperture && map->type == _DRM_AGP)
 		map->handle = agp_remap(map->offset, map->size, dev);
 	else
 		map->handle = ioremap(map->offset, map->size);
 }
-EXPORT_SYMBOL(drm_core_ioremap);
+EXPORT_SYMBOL(drm_legacy_ioremap);
 
-void drm_core_ioremap_wc(struct drm_local_map *map, struct drm_device *dev)
+void drm_legacy_ioremap_wc(struct drm_local_map *map, struct drm_device *dev)
 {
 	if (dev->agp && dev->agp->cant_use_aperture && map->type == _DRM_AGP)
 		map->handle = agp_remap(map->offset, map->size, dev);
 	else
 		map->handle = ioremap_wc(map->offset, map->size);
 }
-EXPORT_SYMBOL(drm_core_ioremap_wc);
+EXPORT_SYMBOL(drm_legacy_ioremap_wc);
 
-void drm_core_ioremapfree(struct drm_local_map *map, struct drm_device *dev)
+void drm_legacy_ioremapfree(struct drm_local_map *map, struct drm_device *dev)
 {
 	if (!map->handle || !map->size)
 		return;
@@ -136,4 +148,4 @@ void drm_core_ioremapfree(struct drm_local_map *map, struct drm_device *dev)
 	else
 		iounmap(map->handle);
 }
-EXPORT_SYMBOL(drm_core_ioremapfree);
+EXPORT_SYMBOL(drm_legacy_ioremapfree);
diff --git a/drivers/gpu/drm/drm_mipi_dsi.c b/drivers/gpu/drm/drm_mipi_dsi.c
index 6aa6a9e95570..eb6dfe52cab2 100644
--- a/drivers/gpu/drm/drm_mipi_dsi.c
+++ b/drivers/gpu/drm/drm_mipi_dsi.c
@@ -231,6 +231,9 @@ ssize_t mipi_dsi_dcs_write(struct mipi_dsi_device *dsi, const void *data,
 		break;
 	}
 
+	if (dsi->mode_flags & MIPI_DSI_MODE_LPM)
+		msg.flags = MIPI_DSI_MSG_USE_LPM;
+
 	return ops->transfer(dsi->host, &msg);
 }
 EXPORT_SYMBOL(mipi_dsi_dcs_write);
@@ -260,6 +263,9 @@ ssize_t mipi_dsi_dcs_read(struct mipi_dsi_device *dsi, u8 cmd, void *data,
 	if (!ops || !ops->transfer)
 		return -ENOSYS;
 
+	if (dsi->mode_flags & MIPI_DSI_MODE_LPM)
+		msg.flags = MIPI_DSI_MSG_USE_LPM;
+
 	return ops->transfer(dsi->host, &msg);
 }
 EXPORT_SYMBOL(mipi_dsi_dcs_read);
diff --git a/drivers/gpu/drm/drm_modes.c b/drivers/gpu/drm/drm_modes.c
index bedf1894e17e..d1b7d2006529 100644
--- a/drivers/gpu/drm/drm_modes.c
+++ b/drivers/gpu/drm/drm_modes.c
@@ -1259,6 +1259,7 @@ drm_mode_create_from_cmdline_mode(struct drm_device *dev,
 	if (!mode)
 		return NULL;
 
+	mode->type |= DRM_MODE_TYPE_USERDEF;
 	drm_mode_set_crtcinfo(mode, CRTC_INTERLACE_HALVE_V);
 	return mode;
 }
diff --git a/drivers/gpu/drm/drm_modeset_lock.c b/drivers/gpu/drm/drm_modeset_lock.c
index 0dc57d5ecd10..474e4d12a2d8 100644
--- a/drivers/gpu/drm/drm_modeset_lock.c
+++ b/drivers/gpu/drm/drm_modeset_lock.c
@@ -35,7 +35,7 @@
  * of extra utility/tracking out of our acquire-ctx.  This is provided
  * by drm_modeset_lock / drm_modeset_acquire_ctx.
  *
- * For basic principles of ww_mutex, see: Documentation/ww-mutex-design.txt
+ * For basic principles of ww_mutex, see: Documentation/locking/ww-mutex-design.txt
  *
  * The basic usage pattern is to:
  *
@@ -57,6 +57,212 @@
 
 
 /**
+ * __drm_modeset_lock_all - internal helper to grab all modeset locks
+ * @dev: DRM device
+ * @trylock: trylock mode for atomic contexts
+ *
+ * This is a special version of drm_modeset_lock_all() which can also be used in
+ * atomic contexts. Then @trylock must be set to true.
+ *
+ * Returns:
+ * 0 on success or negative error code on failure.
+ */
+int __drm_modeset_lock_all(struct drm_device *dev,
+			   bool trylock)
+{
+	struct drm_mode_config *config = &dev->mode_config;
+	struct drm_modeset_acquire_ctx *ctx;
+	int ret;
+
+	ctx = kzalloc(sizeof(*ctx),
+		      trylock ? GFP_ATOMIC : GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	if (trylock) {
+		if (!mutex_trylock(&config->mutex))
+			return -EBUSY;
+	} else {
+		mutex_lock(&config->mutex);
+	}
+
+	drm_modeset_acquire_init(ctx, 0);
+	ctx->trylock_only = trylock;
+
+retry:
+	ret = drm_modeset_lock(&config->connection_mutex, ctx);
+	if (ret)
+		goto fail;
+	ret = drm_modeset_lock_all_crtcs(dev, ctx);
+	if (ret)
+		goto fail;
+
+	WARN_ON(config->acquire_ctx);
+
+	/* now we hold the locks, so now that it is safe, stash the
+	 * ctx for drm_modeset_unlock_all():
+	 */
+	config->acquire_ctx = ctx;
+
+	drm_warn_on_modeset_not_all_locked(dev);
+
+	return 0;
+
+fail:
+	if (ret == -EDEADLK) {
+		drm_modeset_backoff(ctx);
+		goto retry;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(__drm_modeset_lock_all);
+
+/**
+ * drm_modeset_lock_all - take all modeset locks
+ * @dev: drm device
+ *
+ * This function takes all modeset locks, suitable where a more fine-grained
+ * scheme isn't (yet) implemented. Locks must be dropped with
+ * drm_modeset_unlock_all.
+ */
+void drm_modeset_lock_all(struct drm_device *dev)
+{
+	WARN_ON(__drm_modeset_lock_all(dev, false) != 0);
+}
+EXPORT_SYMBOL(drm_modeset_lock_all);
+
+/**
+ * drm_modeset_unlock_all - drop all modeset locks
+ * @dev: device
+ *
+ * This function drop all modeset locks taken by drm_modeset_lock_all.
+ */
+void drm_modeset_unlock_all(struct drm_device *dev)
+{
+	struct drm_mode_config *config = &dev->mode_config;
+	struct drm_modeset_acquire_ctx *ctx = config->acquire_ctx;
+
+	if (WARN_ON(!ctx))
+		return;
+
+	config->acquire_ctx = NULL;
+	drm_modeset_drop_locks(ctx);
+	drm_modeset_acquire_fini(ctx);
+
+	kfree(ctx);
+
+	mutex_unlock(&dev->mode_config.mutex);
+}
+EXPORT_SYMBOL(drm_modeset_unlock_all);
+
+/**
+ * drm_modeset_lock_crtc - lock crtc with hidden acquire ctx
+ * @crtc: drm crtc
+ *
+ * This function locks the given crtc using a hidden acquire context. This is
+ * necessary so that drivers internally using the atomic interfaces can grab
+ * further locks with the lock acquire context.
+ */
+void drm_modeset_lock_crtc(struct drm_crtc *crtc)
+{
+	struct drm_modeset_acquire_ctx *ctx;
+	int ret;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (WARN_ON(!ctx))
+		return;
+
+	drm_modeset_acquire_init(ctx, 0);
+
+retry:
+	ret = drm_modeset_lock(&crtc->mutex, ctx);
+	if (ret)
+		goto fail;
+
+	WARN_ON(crtc->acquire_ctx);
+
+	/* now we hold the locks, so now that it is safe, stash the
+	 * ctx for drm_modeset_unlock_crtc():
+	 */
+	crtc->acquire_ctx = ctx;
+
+	return;
+
+fail:
+	if (ret == -EDEADLK) {
+		drm_modeset_backoff(ctx);
+		goto retry;
+	}
+}
+EXPORT_SYMBOL(drm_modeset_lock_crtc);
+
+/**
+ * drm_modeset_legacy_acquire_ctx - find acquire ctx for legacy ioctls
+ * @crtc: drm crtc
+ *
+ * Legacy ioctl operations like cursor updates or page flips only have per-crtc
+ * locking, and store the acquire ctx in the corresponding crtc. All other
+ * legacy operations take all locks and use a global acquire context. This
+ * function grabs the right one.
+ */
+struct drm_modeset_acquire_ctx *
+drm_modeset_legacy_acquire_ctx(struct drm_crtc *crtc)
+{
+	if (crtc->acquire_ctx)
+		return crtc->acquire_ctx;
+
+	WARN_ON(!crtc->dev->mode_config.acquire_ctx);
+
+	return crtc->dev->mode_config.acquire_ctx;
+}
+EXPORT_SYMBOL(drm_modeset_legacy_acquire_ctx);
+
+/**
+ * drm_modeset_unlock_crtc - drop crtc lock
+ * @crtc: drm crtc
+ *
+ * This drops the crtc lock acquire with drm_modeset_lock_crtc() and all other
+ * locks acquired through the hidden context.
+ */
+void drm_modeset_unlock_crtc(struct drm_crtc *crtc)
+{
+	struct drm_modeset_acquire_ctx *ctx = crtc->acquire_ctx;
+
+	if (WARN_ON(!ctx))
+		return;
+
+	crtc->acquire_ctx = NULL;
+	drm_modeset_drop_locks(ctx);
+	drm_modeset_acquire_fini(ctx);
+
+	kfree(ctx);
+}
+EXPORT_SYMBOL(drm_modeset_unlock_crtc);
+
+/**
+ * drm_warn_on_modeset_not_all_locked - check that all modeset locks are locked
+ * @dev: device
+ *
+ * Useful as a debug assert.
+ */
+void drm_warn_on_modeset_not_all_locked(struct drm_device *dev)
+{
+	struct drm_crtc *crtc;
+
+	/* Locking is currently fubar in the panic handler. */
+	if (oops_in_progress)
+		return;
+
+	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head)
+		WARN_ON(!drm_modeset_is_locked(&crtc->mutex));
+
+	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
+	WARN_ON(!mutex_is_locked(&dev->mode_config.mutex));
+}
+EXPORT_SYMBOL(drm_warn_on_modeset_not_all_locked);
+
+/**
  * drm_modeset_acquire_init - initialize acquire context
  * @ctx: the acquire context
  * @flags: for future
@@ -108,7 +314,12 @@ static inline int modeset_lock(struct drm_modeset_lock *lock,
 
 	WARN_ON(ctx->contended);
 
-	if (interruptible && slow) {
+	if (ctx->trylock_only) {
+		if (!ww_mutex_trylock(&lock->mutex))
+			return -EBUSY;
+		else
+			return 0;
+	} else if (interruptible && slow) {
 		ret = ww_mutex_lock_slow_interruptible(&lock->mutex, &ctx->ww_ctx);
 	} else if (interruptible) {
 		ret = ww_mutex_lock_interruptible(&lock->mutex, &ctx->ww_ctx);
diff --git a/drivers/gpu/drm/drm_pci.c b/drivers/gpu/drm/drm_pci.c
index 020cfd934854..fd29f03645b8 100644
--- a/drivers/gpu/drm/drm_pci.c
+++ b/drivers/gpu/drm/drm_pci.c
@@ -27,6 +27,7 @@
 #include <linux/dma-mapping.h>
 #include <linux/export.h>
 #include <drm/drmP.h>
+#include "drm_legacy.h"
 
 /**
  * drm_pci_alloc - Allocate a PCI consistent memory block, for DMA.
@@ -81,7 +82,7 @@ EXPORT_SYMBOL(drm_pci_alloc);
  *
  * This function is for internal use in the Linux-specific DRM core code.
  */
-void __drm_pci_free(struct drm_device * dev, drm_dma_handle_t * dmah)
+void __drm_legacy_pci_free(struct drm_device * dev, drm_dma_handle_t * dmah)
 {
 	unsigned long addr;
 	size_t sz;
@@ -105,7 +106,7 @@ void __drm_pci_free(struct drm_device * dev, drm_dma_handle_t * dmah)
  */
 void drm_pci_free(struct drm_device * dev, drm_dma_handle_t * dmah)
 {
-	__drm_pci_free(dev, dmah);
+	__drm_legacy_pci_free(dev, dmah);
 	kfree(dmah);
 }
 
@@ -127,34 +128,20 @@ static int drm_get_pci_domain(struct drm_device *dev)
 	return pci_domain_nr(dev->pdev->bus);
 }
 
-static int drm_pci_set_busid(struct drm_device *dev, struct drm_master *master)
+int drm_pci_set_busid(struct drm_device *dev, struct drm_master *master)
 {
-	int len, ret;
-	master->unique_len = 40;
-	master->unique_size = master->unique_len;
-	master->unique = kmalloc(master->unique_size, GFP_KERNEL);
-	if (master->unique == NULL)
+	master->unique = kasprintf(GFP_KERNEL, "pci:%04x:%02x:%02x.%d",
+					drm_get_pci_domain(dev),
+					dev->pdev->bus->number,
+					PCI_SLOT(dev->pdev->devfn),
+					PCI_FUNC(dev->pdev->devfn));
+	if (!master->unique)
 		return -ENOMEM;
 
-
-	len = snprintf(master->unique, master->unique_len,
-		       "pci:%04x:%02x:%02x.%d",
-		       drm_get_pci_domain(dev),
-		       dev->pdev->bus->number,
-		       PCI_SLOT(dev->pdev->devfn),
-		       PCI_FUNC(dev->pdev->devfn));
-
-	if (len >= master->unique_len) {
-		DRM_ERROR("buffer overflow");
-		ret = -EINVAL;
-		goto err;
-	} else
-		master->unique_len = len;
-
+	master->unique_len = strlen(master->unique);
 	return 0;
-err:
-	return ret;
 }
+EXPORT_SYMBOL(drm_pci_set_busid);
 
 int drm_pci_set_unique(struct drm_device *dev,
 		       struct drm_master *master,
@@ -163,8 +150,7 @@ int drm_pci_set_unique(struct drm_device *dev,
 	int domain, bus, slot, func, ret;
 
 	master->unique_len = u->unique_len;
-	master->unique_size = u->unique_len + 1;
-	master->unique = kmalloc(master->unique_size, GFP_KERNEL);
+	master->unique = kmalloc(master->unique_len + 1, GFP_KERNEL);
 	if (!master->unique) {
 		ret = -ENOMEM;
 		goto err;
@@ -269,10 +255,6 @@ void drm_pci_agp_destroy(struct drm_device *dev)
 	}
 }
 
-static struct drm_bus drm_pci_bus = {
-	.set_busid = drm_pci_set_busid,
-};
-
 /**
  * drm_get_pci_dev - Register a PCI device with the DRM subsystem
  * @pdev: PCI device
@@ -353,8 +335,6 @@ int drm_pci_init(struct drm_driver *driver, struct pci_driver *pdriver)
 
 	DRM_DEBUG("\n");
 
-	driver->bus = &drm_pci_bus;
-
 	if (driver->driver_features & DRIVER_MODESET)
 		return pci_register_driver(pdriver);
 
diff --git a/drivers/gpu/drm/drm_platform.c b/drivers/gpu/drm/drm_platform.c
index d5b76f148c12..5314c9d5fef4 100644
--- a/drivers/gpu/drm/drm_platform.c
+++ b/drivers/gpu/drm/drm_platform.c
@@ -68,42 +68,23 @@ err_free:
 	return ret;
 }
 
-static int drm_platform_set_busid(struct drm_device *dev, struct drm_master *master)
+int drm_platform_set_busid(struct drm_device *dev, struct drm_master *master)
 {
-	int len, ret, id;
-
-	master->unique_len = 13 + strlen(dev->platformdev->name);
-	master->unique_size = master->unique_len;
-	master->unique = kmalloc(master->unique_len + 1, GFP_KERNEL);
-
-	if (master->unique == NULL)
-		return -ENOMEM;
+	int id;
 
 	id = dev->platformdev->id;
-
-	/* if only a single instance of the platform device, id will be
-	 * set to -1.. use 0 instead to avoid a funny looking bus-id:
-	 */
-	if (id == -1)
+	if (id < 0)
 		id = 0;
 
-	len = snprintf(master->unique, master->unique_len,
-			"platform:%s:%02d", dev->platformdev->name, id);
-
-	if (len > master->unique_len) {
-		DRM_ERROR("Unique buffer overflowed\n");
-		ret = -EINVAL;
-		goto err;
-	}
+	master->unique = kasprintf(GFP_KERNEL, "platform:%s:%02d",
+						dev->platformdev->name, id);
+	if (!master->unique)
+		return -ENOMEM;
 
+	master->unique_len = strlen(master->unique);
 	return 0;
-err:
-	return ret;
 }
-
-static struct drm_bus drm_platform_bus = {
-	.set_busid = drm_platform_set_busid,
-};
+EXPORT_SYMBOL(drm_platform_set_busid);
 
 /**
  * drm_platform_init - Register a platform device with the DRM subsystem
@@ -120,7 +101,6 @@ int drm_platform_init(struct drm_driver *driver, struct platform_device *platfor
 {
 	DRM_DEBUG("\n");
 
-	driver->bus = &drm_platform_bus;
 	return drm_get_platform_dev(platform_device, driver);
 }
 EXPORT_SYMBOL(drm_platform_init);
diff --git a/drivers/gpu/drm/drm_prime.c b/drivers/gpu/drm/drm_prime.c
index 99d578bad17e..78ca30808422 100644
--- a/drivers/gpu/drm/drm_prime.c
+++ b/drivers/gpu/drm/drm_prime.c
@@ -29,6 +29,9 @@
 #include <linux/export.h>
 #include <linux/dma-buf.h>
 #include <drm/drmP.h>
+#include <drm/drm_gem.h>
+
+#include "drm_internal.h"
 
 /*
  * DMA-BUF/GEM Object references and lifetime overview:
@@ -522,7 +525,7 @@ struct drm_gem_object *drm_gem_prime_import(struct drm_device *dev,
 		goto fail_detach;
 	}
 
-	obj = dev->driver->gem_prime_import_sg_table(dev, dma_buf->size, sgt);
+	obj = dev->driver->gem_prime_import_sg_table(dev, attach, sgt);
 	if (IS_ERR(obj)) {
 		ret = PTR_ERR(obj);
 		goto fail_unmap;
diff --git a/drivers/gpu/drm/drm_probe_helper.c b/drivers/gpu/drm/drm_probe_helper.c
index db7d250f7ac7..6857e9ad6339 100644
--- a/drivers/gpu/drm/drm_probe_helper.c
+++ b/drivers/gpu/drm/drm_probe_helper.c
@@ -82,6 +82,22 @@ static void drm_mode_validate_flag(struct drm_connector *connector,
 	return;
 }
 
+static int drm_helper_probe_add_cmdline_mode(struct drm_connector *connector)
+{
+	struct drm_display_mode *mode;
+
+	if (!connector->cmdline_mode.specified)
+		return 0;
+
+	mode = drm_mode_create_from_cmdline_mode(connector->dev,
+						 &connector->cmdline_mode);
+	if (mode == NULL)
+		return 0;
+
+	drm_mode_probed_add(connector, mode);
+	return 1;
+}
+
 static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connector *connector,
 							      uint32_t maxX, uint32_t maxY, bool merge_type_bits)
 {
@@ -141,6 +157,7 @@ static int drm_helper_probe_single_connector_modes_merge_bits(struct drm_connect
 
 	if (count == 0 && connector->status == connector_status_connected)
 		count = drm_add_modes_noedid(connector, 1024, 768);
+	count += drm_helper_probe_add_cmdline_mode(connector);
 	if (count == 0)
 		goto prune;
 
diff --git a/drivers/gpu/drm/drm_scatter.c b/drivers/gpu/drm/drm_scatter.c
index 1c78406f6e71..4f0f3b36d537 100644
--- a/drivers/gpu/drm/drm_scatter.c
+++ b/drivers/gpu/drm/drm_scatter.c
@@ -34,6 +34,7 @@
 #include <linux/vmalloc.h>
 #include <linux/slab.h>
 #include <drm/drmP.h>
+#include "drm_legacy.h"
 
 #define DEBUG_SCATTER 0
 
@@ -78,8 +79,8 @@ void drm_legacy_sg_cleanup(struct drm_device *dev)
 # define ScatterHandle(x) (unsigned int)(x)
 #endif
 
-int drm_sg_alloc(struct drm_device *dev, void *data,
-		 struct drm_file *file_priv)
+int drm_legacy_sg_alloc(struct drm_device *dev, void *data,
+			struct drm_file *file_priv)
 {
 	struct drm_scatter_gather *request = data;
 	struct drm_sg_mem *entry;
@@ -194,8 +195,8 @@ int drm_sg_alloc(struct drm_device *dev, void *data,
 	return -ENOMEM;
 }
 
-int drm_sg_free(struct drm_device *dev, void *data,
-		struct drm_file *file_priv)
+int drm_legacy_sg_free(struct drm_device *dev, void *data,
+		       struct drm_file *file_priv)
 {
 	struct drm_scatter_gather *request = data;
 	struct drm_sg_mem *entry;
diff --git a/drivers/gpu/drm/drm_sysfs.c b/drivers/gpu/drm/drm_sysfs.c
index ab1a5f6dde8a..cc3d6d6d67e0 100644
--- a/drivers/gpu/drm/drm_sysfs.c
+++ b/drivers/gpu/drm/drm_sysfs.c
@@ -21,6 +21,7 @@
 #include <drm/drm_sysfs.h>
 #include <drm/drm_core.h>
 #include <drm/drmP.h>
+#include "drm_internal.h"
 
 #define to_drm_minor(d) dev_get_drvdata(d)
 #define to_drm_connector(d) dev_get_drvdata(d)
diff --git a/drivers/gpu/drm/drm_usb.c b/drivers/gpu/drm/drm_usb.c
deleted file mode 100644
index f2fe94aab901..000000000000
--- a/drivers/gpu/drm/drm_usb.c
+++ /dev/null
@@ -1,88 +0,0 @@
-#include <drm/drmP.h>
-#include <drm/drm_usb.h>
-#include <linux/usb.h>
-#include <linux/module.h>
-
-int drm_get_usb_dev(struct usb_interface *interface,
-		    const struct usb_device_id *id,
-		    struct drm_driver *driver)
-{
-	struct drm_device *dev;
-	int ret;
-
-	DRM_DEBUG("\n");
-
-	dev = drm_dev_alloc(driver, &interface->dev);
-	if (!dev)
-		return -ENOMEM;
-
-	dev->usbdev = interface_to_usbdev(interface);
-	usb_set_intfdata(interface, dev);
-
-	ret = drm_dev_register(dev, 0);
-	if (ret)
-		goto err_free;
-
-	DRM_INFO("Initialized %s %d.%d.%d %s on minor %d\n",
-		 driver->name, driver->major, driver->minor, driver->patchlevel,
-		 driver->date, dev->primary->index);
-
-	return 0;
-
-err_free:
-	drm_dev_unref(dev);
-	return ret;
-
-}
-EXPORT_SYMBOL(drm_get_usb_dev);
-
-static int drm_usb_set_busid(struct drm_device *dev,
-			       struct drm_master *master)
-{
-	return 0;
-}
-
-static struct drm_bus drm_usb_bus = {
-	.set_busid = drm_usb_set_busid,
-};
-
-/**
- * drm_usb_init - Register matching USB devices with the DRM subsystem
- * @driver: DRM device driver
- * @udriver: USB device driver
- *
- * Registers one or more devices matched by a USB driver with the DRM
- * subsystem.
- *
- * Return: 0 on success or a negative error code on failure.
- */
-int drm_usb_init(struct drm_driver *driver, struct usb_driver *udriver)
-{
-	int res;
-	DRM_DEBUG("\n");
-
-	driver->bus = &drm_usb_bus;
-
-	res = usb_register(udriver);
-	return res;
-}
-EXPORT_SYMBOL(drm_usb_init);
-
-/**
- * drm_usb_exit - Unregister matching USB devices from the DRM subsystem
- * @driver: DRM device driver
- * @udriver: USB device driver
- *
- * Unregisters one or more devices matched by a USB driver from the DRM
- * subsystem.
- */
-void drm_usb_exit(struct drm_driver *driver,
-		  struct usb_driver *udriver)
-{
-	usb_deregister(udriver);
-}
-EXPORT_SYMBOL(drm_usb_exit);
-
-MODULE_AUTHOR("David Airlie");
-MODULE_DESCRIPTION("USB DRM support");
-MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/gpu/drm/drm_vm.c b/drivers/gpu/drm/drm_vm.c
index 24e045c4f531..4a2c328959e5 100644
--- a/drivers/gpu/drm/drm_vm.c
+++ b/drivers/gpu/drm/drm_vm.c
@@ -35,10 +35,19 @@
 
 #include <drm/drmP.h>
 #include <linux/export.h>
+#include <linux/seq_file.h>
 #if defined(__ia64__)
 #include <linux/efi.h>
 #include <linux/slab.h>
 #endif
+#include <asm/pgtable.h>
+#include "drm_legacy.h"
+
+struct drm_vma_entry {
+	struct list_head head;
+	struct vm_area_struct *vma;
+	pid_t pid;
+};
 
 static void drm_vm_open(struct vm_area_struct *vma);
 static void drm_vm_close(struct vm_area_struct *vma);
@@ -48,15 +57,11 @@ static pgprot_t drm_io_prot(struct drm_local_map *map,
 {
 	pgprot_t tmp = vm_get_page_prot(vma->vm_flags);
 
-#if defined(__i386__) || defined(__x86_64__)
+#if defined(__i386__) || defined(__x86_64__) || defined(__powerpc__)
 	if (map->type == _DRM_REGISTERS && !(map->flags & _DRM_WRITE_COMBINING))
 		tmp = pgprot_noncached(tmp);
 	else
 		tmp = pgprot_writecombine(tmp);
-#elif defined(__powerpc__)
-	pgprot_val(tmp) |= _PAGE_NO_CACHE;
-	if (map->type == _DRM_REGISTERS)
-		pgprot_val(tmp) |= _PAGE_GUARDED;
 #elif defined(__ia64__)
 	if (efi_range_is_wc(vma->vm_start, vma->vm_end -
 				    vma->vm_start))
@@ -263,7 +268,7 @@ static void drm_vm_shm_close(struct vm_area_struct *vma)
 				dmah.vaddr = map->handle;
 				dmah.busaddr = map->offset;
 				dmah.size = map->size;
-				__drm_pci_free(dev, &dmah);
+				__drm_legacy_pci_free(dev, &dmah);
 				break;
 			}
 			kfree(map);
@@ -412,7 +417,6 @@ void drm_vm_open_locked(struct drm_device *dev,
 		list_add(&vma_entry->head, &dev->vmalist);
 	}
 }
-EXPORT_SYMBOL_GPL(drm_vm_open_locked);
 
 static void drm_vm_open(struct vm_area_struct *vma)
 {
@@ -532,7 +536,7 @@ static resource_size_t drm_core_get_reg_ofs(struct drm_device *dev)
  * according to the mapping type and remaps the pages. Finally sets the file
  * pointer and calls vm_open().
  */
-int drm_mmap_locked(struct file *filp, struct vm_area_struct *vma)
+static int drm_mmap_locked(struct file *filp, struct vm_area_struct *vma)
 {
 	struct drm_file *priv = filp->private_data;
 	struct drm_device *dev = priv->minor->dev;
@@ -646,7 +650,7 @@ int drm_mmap_locked(struct file *filp, struct vm_area_struct *vma)
 	return 0;
 }
 
-int drm_mmap(struct file *filp, struct vm_area_struct *vma)
+int drm_legacy_mmap(struct file *filp, struct vm_area_struct *vma)
 {
 	struct drm_file *priv = filp->private_data;
 	struct drm_device *dev = priv->minor->dev;
@@ -661,4 +665,69 @@ int drm_mmap(struct file *filp, struct vm_area_struct *vma)
 
 	return ret;
 }
-EXPORT_SYMBOL(drm_mmap);
+EXPORT_SYMBOL(drm_legacy_mmap);
+
+void drm_legacy_vma_flush(struct drm_device *dev)
+{
+	struct drm_vma_entry *vma, *vma_temp;
+
+	/* Clear vma list (only needed for legacy drivers) */
+	list_for_each_entry_safe(vma, vma_temp, &dev->vmalist, head) {
+		list_del(&vma->head);
+		kfree(vma);
+	}
+}
+
+int drm_vma_info(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_vma_entry *pt;
+	struct vm_area_struct *vma;
+	unsigned long vma_count = 0;
+#if defined(__i386__)
+	unsigned int pgprot;
+#endif
+
+	mutex_lock(&dev->struct_mutex);
+	list_for_each_entry(pt, &dev->vmalist, head)
+		vma_count++;
+
+	seq_printf(m, "vma use count: %lu, high_memory = %pK, 0x%pK\n",
+		   vma_count, high_memory,
+		   (void *)(unsigned long)virt_to_phys(high_memory));
+
+	list_for_each_entry(pt, &dev->vmalist, head) {
+		vma = pt->vma;
+		if (!vma)
+			continue;
+		seq_printf(m,
+			   "\n%5d 0x%pK-0x%pK %c%c%c%c%c%c 0x%08lx000",
+			   pt->pid,
+			   (void *)vma->vm_start, (void *)vma->vm_end,
+			   vma->vm_flags & VM_READ ? 'r' : '-',
+			   vma->vm_flags & VM_WRITE ? 'w' : '-',
+			   vma->vm_flags & VM_EXEC ? 'x' : '-',
+			   vma->vm_flags & VM_MAYSHARE ? 's' : 'p',
+			   vma->vm_flags & VM_LOCKED ? 'l' : '-',
+			   vma->vm_flags & VM_IO ? 'i' : '-',
+			   vma->vm_pgoff);
+
+#if defined(__i386__)
+		pgprot = pgprot_val(vma->vm_page_prot);
+		seq_printf(m, " %c%c%c%c%c%c%c%c%c",
+			   pgprot & _PAGE_PRESENT ? 'p' : '-',
+			   pgprot & _PAGE_RW ? 'w' : 'r',
+			   pgprot & _PAGE_USER ? 'u' : 's',
+			   pgprot & _PAGE_PWT ? 't' : 'b',
+			   pgprot & _PAGE_PCD ? 'u' : 'c',
+			   pgprot & _PAGE_ACCESSED ? 'a' : '-',
+			   pgprot & _PAGE_DIRTY ? 'd' : '-',
+			   pgprot & _PAGE_PSE ? 'm' : 'k',
+			   pgprot & _PAGE_GLOBAL ? 'g' : 'l');
+#endif
+		seq_printf(m, "\n");
+	}
+	mutex_unlock(&dev->struct_mutex);
+	return 0;
+}
diff --git a/drivers/gpu/drm/exynos/exynos_dp_core.c b/drivers/gpu/drm/exynos/exynos_dp_core.c
index 4f3c7eb2d37d..cd50ece31601 100644
--- a/drivers/gpu/drm/exynos/exynos_dp_core.c
+++ b/drivers/gpu/drm/exynos/exynos_dp_core.c
@@ -329,8 +329,8 @@ static int exynos_dp_link_start(struct exynos_dp_device *dp)
 		return retval;
 
 	for (lane = 0; lane < lane_count; lane++)
-		buf[lane] = DP_TRAIN_PRE_EMPHASIS_0 |
-			    DP_TRAIN_VOLTAGE_SWING_400;
+		buf[lane] = DP_TRAIN_PRE_EMPH_LEVEL_0 |
+			    DP_TRAIN_VOLTAGE_SWING_LEVEL_0;
 
 	retval = exynos_dp_write_bytes_to_dpcd(dp, DP_TRAINING_LANE0_SET,
 			lane_count, buf);
@@ -937,6 +937,8 @@ static enum drm_connector_status exynos_dp_detect(
 
 static void exynos_dp_connector_destroy(struct drm_connector *connector)
 {
+	drm_connector_unregister(connector);
+	drm_connector_cleanup(connector);
 }
 
 static struct drm_connector_funcs exynos_dp_connector_funcs = {
@@ -1358,8 +1360,8 @@ static void exynos_dp_unbind(struct device *dev, struct device *master,
 
 	exynos_dp_dpms(display, DRM_MODE_DPMS_OFF);
 
+	exynos_dp_connector_destroy(&dp->connector);
 	encoder->funcs->destroy(encoder);
-	drm_connector_cleanup(&dp->connector);
 }
 
 static const struct component_ops exynos_dp_ops = {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_crtc.c b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
index b68e58f78cd1..8e38e9f8e542 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_crtc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_crtc.c
@@ -32,7 +32,6 @@ enum exynos_crtc_mode {
  * Exynos specific crtc structure.
  *
  * @drm_crtc: crtc object.
- * @drm_plane: pointer of private plane object for this crtc
  * @manager: the manager associated with this crtc
  * @pipe: a crtc index created at load() with a new crtc object creation
  *	and the crtc object would be set to private->crtc array
@@ -46,7 +45,6 @@ enum exynos_crtc_mode {
  */
 struct exynos_drm_crtc {
 	struct drm_crtc			drm_crtc;
-	struct drm_plane		*plane;
 	struct exynos_drm_manager	*manager;
 	unsigned int			pipe;
 	unsigned int			dpms;
@@ -94,12 +92,12 @@ static void exynos_drm_crtc_commit(struct drm_crtc *crtc)
 
 	exynos_drm_crtc_dpms(crtc, DRM_MODE_DPMS_ON);
 
-	exynos_plane_commit(exynos_crtc->plane);
+	exynos_plane_commit(crtc->primary);
 
 	if (manager->ops->commit)
 		manager->ops->commit(manager);
 
-	exynos_plane_dpms(exynos_crtc->plane, DRM_MODE_DPMS_ON);
+	exynos_plane_dpms(crtc->primary, DRM_MODE_DPMS_ON);
 }
 
 static bool
@@ -123,10 +121,9 @@ exynos_drm_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 {
 	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
 	struct exynos_drm_manager *manager = exynos_crtc->manager;
-	struct drm_plane *plane = exynos_crtc->plane;
+	struct drm_framebuffer *fb = crtc->primary->fb;
 	unsigned int crtc_w;
 	unsigned int crtc_h;
-	int ret;
 
 	/*
 	 * copy the mode data adjusted by mode_fixup() into crtc->mode
@@ -134,29 +131,21 @@ exynos_drm_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *mode,
 	 */
 	memcpy(&crtc->mode, adjusted_mode, sizeof(*adjusted_mode));
 
-	crtc_w = crtc->primary->fb->width - x;
-	crtc_h = crtc->primary->fb->height - y;
+	crtc_w = fb->width - x;
+	crtc_h = fb->height - y;
 
 	if (manager->ops->mode_set)
 		manager->ops->mode_set(manager, &crtc->mode);
 
-	ret = exynos_plane_mode_set(plane, crtc, crtc->primary->fb, 0, 0, crtc_w, crtc_h,
-				    x, y, crtc_w, crtc_h);
-	if (ret)
-		return ret;
-
-	plane->crtc = crtc;
-	plane->fb = crtc->primary->fb;
-	drm_framebuffer_reference(plane->fb);
-
-	return 0;
+	return exynos_plane_mode_set(crtc->primary, crtc, fb, 0, 0,
+				     crtc_w, crtc_h, x, y, crtc_w, crtc_h);
 }
 
 static int exynos_drm_crtc_mode_set_commit(struct drm_crtc *crtc, int x, int y,
 					  struct drm_framebuffer *old_fb)
 {
 	struct exynos_drm_crtc *exynos_crtc = to_exynos_crtc(crtc);
-	struct drm_plane *plane = exynos_crtc->plane;
+	struct drm_framebuffer *fb = crtc->primary->fb;
 	unsigned int crtc_w;
 	unsigned int crtc_h;
 	int ret;
@@ -167,11 +156,11 @@ static int exynos_drm_crtc_mode_set_commit(struct drm_crtc *crtc, int x, int y,
 		return -EPERM;
 	}
 
-	crtc_w = crtc->primary->fb->width - x;
-	crtc_h = crtc->primary->fb->height - y;
+	crtc_w = fb->width - x;
+	crtc_h = fb->height - y;
 
-	ret = exynos_plane_mode_set(plane, crtc, crtc->primary->fb, 0, 0, crtc_w, crtc_h,
-				    x, y, crtc_w, crtc_h);
+	ret = exynos_plane_mode_set(crtc->primary, crtc, fb, 0, 0,
+				    crtc_w, crtc_h, x, y, crtc_w, crtc_h);
 	if (ret)
 		return ret;
 
@@ -304,8 +293,7 @@ static int exynos_drm_crtc_set_property(struct drm_crtc *crtc,
 			exynos_drm_crtc_commit(crtc);
 			break;
 		case CRTC_MODE_BLANK:
-			exynos_plane_dpms(exynos_crtc->plane,
-					  DRM_MODE_DPMS_OFF);
+			exynos_plane_dpms(crtc->primary, DRM_MODE_DPMS_OFF);
 			break;
 		default:
 			break;
@@ -351,8 +339,10 @@ static void exynos_drm_crtc_attach_mode_property(struct drm_crtc *crtc)
 int exynos_drm_crtc_create(struct exynos_drm_manager *manager)
 {
 	struct exynos_drm_crtc *exynos_crtc;
+	struct drm_plane *plane;
 	struct exynos_drm_private *private = manager->drm_dev->dev_private;
 	struct drm_crtc *crtc;
+	int ret;
 
 	exynos_crtc = kzalloc(sizeof(*exynos_crtc), GFP_KERNEL);
 	if (!exynos_crtc)
@@ -364,11 +354,11 @@ int exynos_drm_crtc_create(struct exynos_drm_manager *manager)
 	exynos_crtc->dpms = DRM_MODE_DPMS_OFF;
 	exynos_crtc->manager = manager;
 	exynos_crtc->pipe = manager->pipe;
-	exynos_crtc->plane = exynos_plane_init(manager->drm_dev,
-				1 << manager->pipe, true);
-	if (!exynos_crtc->plane) {
-		kfree(exynos_crtc);
-		return -ENOMEM;
+	plane = exynos_plane_init(manager->drm_dev, 1 << manager->pipe,
+				  DRM_PLANE_TYPE_PRIMARY);
+	if (IS_ERR(plane)) {
+		ret = PTR_ERR(plane);
+		goto err_plane;
 	}
 
 	manager->crtc = &exynos_crtc->drm_crtc;
@@ -376,12 +366,22 @@ int exynos_drm_crtc_create(struct exynos_drm_manager *manager)
 
 	private->crtc[manager->pipe] = crtc;
 
-	drm_crtc_init(manager->drm_dev, crtc, &exynos_crtc_funcs);
+	ret = drm_crtc_init_with_planes(manager->drm_dev, crtc, plane, NULL,
+					&exynos_crtc_funcs);
+	if (ret < 0)
+		goto err_crtc;
+
 	drm_crtc_helper_add(crtc, &exynos_crtc_helper_funcs);
 
 	exynos_drm_crtc_attach_mode_property(crtc);
 
 	return 0;
+
+err_crtc:
+	plane->funcs->destroy(plane);
+err_plane:
+	kfree(exynos_crtc);
+	return ret;
 }
 
 int exynos_drm_crtc_enable_vblank(struct drm_device *dev, int pipe)
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dpi.c b/drivers/gpu/drm/exynos/exynos_drm_dpi.c
index fa08f05e3e34..96c87db388fb 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dpi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dpi.c
@@ -342,8 +342,12 @@ int exynos_dpi_remove(struct device *dev)
 	struct exynos_dpi *ctx = exynos_dpi_display.ctx;
 
 	exynos_dpi_dpms(&exynos_dpi_display, DRM_MODE_DPMS_OFF);
+
+	exynos_dpi_connector_destroy(&ctx->connector);
 	encoder->funcs->destroy(encoder);
-	drm_connector_cleanup(&ctx->connector);
+
+	if (ctx->panel)
+		drm_panel_detach(ctx->panel);
 
 	exynos_drm_component_del(dev, EXYNOS_DEVICE_TYPE_CONNECTOR);
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.c b/drivers/gpu/drm/exynos/exynos_drm_drv.c
index 0d74e9b99c4e..443a2069858a 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.c
@@ -15,7 +15,6 @@
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 
-#include <linux/anon_inodes.h>
 #include <linux/component.h>
 
 #include <drm/exynos_drm.h>
@@ -86,8 +85,9 @@ static int exynos_drm_load(struct drm_device *dev, unsigned long flags)
 		struct drm_plane *plane;
 		unsigned long possible_crtcs = (1 << MAX_CRTC) - 1;
 
-		plane = exynos_plane_init(dev, possible_crtcs, false);
-		if (!plane)
+		plane = exynos_plane_init(dev, possible_crtcs,
+					  DRM_PLANE_TYPE_OVERLAY);
+		if (IS_ERR(plane))
 			goto err_mode_config_cleanup;
 	}
 
@@ -116,6 +116,23 @@ static int exynos_drm_load(struct drm_device *dev, unsigned long flags)
 	/* force connectors detection */
 	drm_helper_hpd_irq_event(dev);
 
+	/*
+	 * enable drm irq mode.
+	 * - with irq_enabled = true, we can use the vblank feature.
+	 *
+	 * P.S. note that we wouldn't use drm irq handler but
+	 *	just specific driver own one instead because
+	 *	drm framework supports only one irq handler.
+	 */
+	dev->irq_enabled = true;
+
+	/*
+	 * with vblank_disable_allowed = true, vblank interrupt will be disabled
+	 * by drm timer once a current process gives up ownership of
+	 * vblank event.(after drm_vblank_put function is called)
+	 */
+	dev->vblank_disable_allowed = true;
+
 	return 0;
 
 err_unbind_all:
@@ -136,23 +153,19 @@ static int exynos_drm_unload(struct drm_device *dev)
 	exynos_drm_device_subdrv_remove(dev);
 
 	exynos_drm_fbdev_fini(dev);
-	drm_vblank_cleanup(dev);
 	drm_kms_helper_poll_fini(dev);
-	drm_mode_config_cleanup(dev);
 
+	component_unbind_all(dev->dev, dev);
+	drm_vblank_cleanup(dev);
+	drm_mode_config_cleanup(dev);
 	drm_release_iommu_mapping(dev);
-	kfree(dev->dev_private);
 
-	component_unbind_all(dev->dev, dev);
+	kfree(dev->dev_private);
 	dev->dev_private = NULL;
 
 	return 0;
 }
 
-static const struct file_operations exynos_drm_gem_fops = {
-	.mmap = exynos_drm_gem_mmap_buffer,
-};
-
 static int exynos_drm_suspend(struct drm_device *dev, pm_message_t state)
 {
 	struct drm_connector *connector;
@@ -191,7 +204,6 @@ static int exynos_drm_resume(struct drm_device *dev)
 static int exynos_drm_open(struct drm_device *dev, struct drm_file *file)
 {
 	struct drm_exynos_file_private *file_priv;
-	struct file *anon_filp;
 	int ret;
 
 	file_priv = kzalloc(sizeof(*file_priv), GFP_KERNEL);
@@ -204,21 +216,8 @@ static int exynos_drm_open(struct drm_device *dev, struct drm_file *file)
 	if (ret)
 		goto err_file_priv_free;
 
-	anon_filp = anon_inode_getfile("exynos_gem", &exynos_drm_gem_fops,
-					NULL, 0);
-	if (IS_ERR(anon_filp)) {
-		ret = PTR_ERR(anon_filp);
-		goto err_subdrv_close;
-	}
-
-	anon_filp->f_mode = FMODE_READ | FMODE_WRITE;
-	file_priv->anon_filp = anon_filp;
-
 	return ret;
 
-err_subdrv_close:
-	exynos_drm_subdrv_close(dev, file);
-
 err_file_priv_free:
 	kfree(file_priv);
 	file->driver_priv = NULL;
@@ -234,7 +233,6 @@ static void exynos_drm_preclose(struct drm_device *dev,
 static void exynos_drm_postclose(struct drm_device *dev, struct drm_file *file)
 {
 	struct exynos_drm_private *private = dev->dev_private;
-	struct drm_exynos_file_private *file_priv;
 	struct drm_pending_vblank_event *v, *vt;
 	struct drm_pending_event *e, *et;
 	unsigned long flags;
@@ -260,10 +258,6 @@ static void exynos_drm_postclose(struct drm_device *dev, struct drm_file *file)
 	}
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
-	file_priv = file->driver_priv;
-	if (file_priv->anon_filp)
-		fput(file_priv->anon_filp);
-
 	kfree(file->driver_priv);
 	file->driver_priv = NULL;
 }
@@ -282,11 +276,6 @@ static const struct vm_operations_struct exynos_drm_gem_vm_ops = {
 static const struct drm_ioctl_desc exynos_ioctls[] = {
 	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_CREATE, exynos_drm_gem_create_ioctl,
 			DRM_UNLOCKED | DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_MAP_OFFSET,
-			exynos_drm_gem_map_offset_ioctl, DRM_UNLOCKED |
-			DRM_AUTH),
-	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_MMAP,
-			exynos_drm_gem_mmap_ioctl, DRM_UNLOCKED | DRM_AUTH),
 	DRM_IOCTL_DEF_DRV(EXYNOS_GEM_GET,
 			exynos_drm_gem_get_ioctl, DRM_UNLOCKED),
 	DRM_IOCTL_DEF_DRV(EXYNOS_VIDI_CONNECTION,
@@ -330,6 +319,7 @@ static struct drm_driver exynos_drm_driver = {
 	.preclose		= exynos_drm_preclose,
 	.lastclose		= exynos_drm_lastclose,
 	.postclose		= exynos_drm_postclose,
+	.set_busid		= drm_platform_set_busid,
 	.get_vblank_counter	= drm_vblank_count,
 	.enable_vblank		= exynos_drm_crtc_enable_vblank,
 	.disable_vblank		= exynos_drm_crtc_disable_vblank,
@@ -485,21 +475,20 @@ void exynos_drm_component_del(struct device *dev,
 	mutex_unlock(&drm_component_lock);
 }
 
-static int compare_of(struct device *dev, void *data)
+static int compare_dev(struct device *dev, void *data)
 {
 	return dev == (struct device *)data;
 }
 
-static int exynos_drm_add_components(struct device *dev, struct master *m)
+static struct component_match *exynos_drm_match_add(struct device *dev)
 {
+	struct component_match *match = NULL;
 	struct component_dev *cdev;
 	unsigned int attach_cnt = 0;
 
 	mutex_lock(&drm_component_lock);
 
 	list_for_each_entry(cdev, &drm_component_list, list) {
-		int ret;
-
 		/*
 		 * Add components to master only in case that crtc and
 		 * encoder/connector device objects exist.
@@ -514,16 +503,10 @@ static int exynos_drm_add_components(struct device *dev, struct master *m)
 		/*
 		 * fimd and dpi modules have same device object so add
 		 * only crtc device object in this case.
-		 *
-		 * TODO. if dpi module follows driver-model driver then
-		 * below codes can be removed.
 		 */
 		if (cdev->crtc_dev == cdev->conn_dev) {
-			ret = component_master_add_child(m, compare_of,
-					cdev->crtc_dev);
-			if (ret < 0)
-				return ret;
-
+			component_match_add(dev, &match, compare_dev,
+						cdev->crtc_dev);
 			goto out_lock;
 		}
 
@@ -533,11 +516,8 @@ static int exynos_drm_add_components(struct device *dev, struct master *m)
 		 * connector/encoder need pipe number of crtc when they
 		 * are created.
 		 */
-		ret = component_master_add_child(m, compare_of, cdev->crtc_dev);
-		ret |= component_master_add_child(m, compare_of,
-							cdev->conn_dev);
-		if (ret < 0)
-			return ret;
+		component_match_add(dev, &match, compare_dev, cdev->crtc_dev);
+		component_match_add(dev, &match, compare_dev, cdev->conn_dev);
 
 out_lock:
 		mutex_lock(&drm_component_lock);
@@ -545,7 +525,7 @@ out_lock:
 
 	mutex_unlock(&drm_component_lock);
 
-	return attach_cnt ? 0 : -ENODEV;
+	return attach_cnt ? match : ERR_PTR(-EPROBE_DEFER);
 }
 
 static int exynos_drm_bind(struct device *dev)
@@ -559,13 +539,13 @@ static void exynos_drm_unbind(struct device *dev)
 }
 
 static const struct component_master_ops exynos_drm_ops = {
-	.add_components = exynos_drm_add_components,
 	.bind		= exynos_drm_bind,
 	.unbind		= exynos_drm_unbind,
 };
 
 static int exynos_drm_platform_probe(struct platform_device *pdev)
 {
+	struct component_match *match;
 	int ret;
 
 	pdev->dev.coherent_dma_mask = DMA_BIT_MASK(32);
@@ -632,13 +612,23 @@ static int exynos_drm_platform_probe(struct platform_device *pdev)
 		goto err_unregister_ipp_drv;
 #endif
 
-	ret = component_master_add(&pdev->dev, &exynos_drm_ops);
+	match = exynos_drm_match_add(&pdev->dev);
+	if (IS_ERR(match)) {
+		ret = PTR_ERR(match);
+		goto err_unregister_resources;
+	}
+
+	ret = component_master_add_with_match(&pdev->dev, &exynos_drm_ops,
+						match);
 	if (ret < 0)
-		DRM_DEBUG_KMS("re-tried by last sub driver probed later.\n");
+		goto err_unregister_resources;
 
-	return 0;
+	return ret;
+
+err_unregister_resources:
 
 #ifdef CONFIG_DRM_EXYNOS_IPP
+	exynos_platform_device_ipp_unregister();
 err_unregister_ipp_drv:
 	platform_driver_unregister(&ipp_driver);
 err_unregister_gsc_drv:
diff --git a/drivers/gpu/drm/exynos/exynos_drm_drv.h b/drivers/gpu/drm/exynos/exynos_drm_drv.h
index 69a6fa397d75..d22e640f59a0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_drv.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_drv.h
@@ -240,7 +240,6 @@ struct exynos_drm_g2d_private {
 struct drm_exynos_file_private {
 	struct exynos_drm_g2d_private	*g2d_priv;
 	struct device			*ipp_dev;
-	struct file			*anon_filp;
 };
 
 /*
diff --git a/drivers/gpu/drm/exynos/exynos_drm_dsi.c b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
index 442aa2d00132..24741d8758e8 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_dsi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_dsi.c
@@ -114,6 +114,8 @@
 #define DSIM_SYNC_INFORM		(1 << 27)
 #define DSIM_EOT_DISABLE		(1 << 28)
 #define DSIM_MFLUSH_VS			(1 << 29)
+/* This flag is valid only for exynos3250/3472/4415/5260/5430 */
+#define DSIM_CLKLANE_STOP		(1 << 30)
 
 /* DSIM_ESCMODE */
 #define DSIM_TX_TRIGGER_RST		(1 << 4)
@@ -262,6 +264,7 @@ struct exynos_dsi_driver_data {
 	unsigned int plltmr_reg;
 
 	unsigned int has_freqband:1;
+	unsigned int has_clklane_stop:1;
 };
 
 struct exynos_dsi {
@@ -301,9 +304,16 @@ struct exynos_dsi {
 #define host_to_dsi(host) container_of(host, struct exynos_dsi, dsi_host)
 #define connector_to_dsi(c) container_of(c, struct exynos_dsi, connector)
 
+static struct exynos_dsi_driver_data exynos3_dsi_driver_data = {
+	.plltmr_reg = 0x50,
+	.has_freqband = 1,
+	.has_clklane_stop = 1,
+};
+
 static struct exynos_dsi_driver_data exynos4_dsi_driver_data = {
 	.plltmr_reg = 0x50,
 	.has_freqband = 1,
+	.has_clklane_stop = 1,
 };
 
 static struct exynos_dsi_driver_data exynos5_dsi_driver_data = {
@@ -311,6 +321,8 @@ static struct exynos_dsi_driver_data exynos5_dsi_driver_data = {
 };
 
 static struct of_device_id exynos_dsi_of_match[] = {
+	{ .compatible = "samsung,exynos3250-mipi-dsi",
+	  .data = &exynos3_dsi_driver_data },
 	{ .compatible = "samsung,exynos4210-mipi-dsi",
 	  .data = &exynos4_dsi_driver_data },
 	{ .compatible = "samsung,exynos5410-mipi-dsi",
@@ -421,7 +433,7 @@ static unsigned long exynos_dsi_set_pll(struct exynos_dsi *dsi,
 	if (!fout) {
 		dev_err(dsi->dev,
 			"failed to find PLL PMS for requested frequency\n");
-		return -EFAULT;
+		return 0;
 	}
 	dev_dbg(dsi->dev, "PLL freq %lu, (p %d, m %d, s %d)\n", fout, p, m, s);
 
@@ -453,7 +465,7 @@ static unsigned long exynos_dsi_set_pll(struct exynos_dsi *dsi,
 	do {
 		if (timeout-- == 0) {
 			dev_err(dsi->dev, "PLL failed to stabilize\n");
-			return -EFAULT;
+			return 0;
 		}
 		reg = readl(dsi->reg_base + DSIM_STATUS_REG);
 	} while ((reg & DSIM_PLL_STABLE) == 0);
@@ -569,6 +581,7 @@ static void exynos_dsi_disable_clock(struct exynos_dsi *dsi)
 
 static int exynos_dsi_init_link(struct exynos_dsi *dsi)
 {
+	struct exynos_dsi_driver_data *driver_data = dsi->driver_data;
 	int timeout;
 	u32 reg;
 	u32 lanes_mask;
@@ -650,6 +663,20 @@ static int exynos_dsi_init_link(struct exynos_dsi *dsi)
 	reg |= DSIM_LANE_EN(lanes_mask);
 	writel(reg, dsi->reg_base + DSIM_CONFIG_REG);
 
+	/*
+	 * Use non-continuous clock mode if the periparal wants and
+	 * host controller supports
+	 *
+	 * In non-continous clock mode, host controller will turn off
+	 * the HS clock between high-speed transmissions to reduce
+	 * power consumption.
+	 */
+	if (driver_data->has_clklane_stop &&
+			dsi->mode_flags & MIPI_DSI_CLOCK_NON_CONTINUOUS) {
+		reg |= DSIM_CLKLANE_STOP;
+		writel(reg, dsi->reg_base + DSIM_CONFIG_REG);
+	}
+
 	/* Check clock and data lane state are stop state */
 	timeout = 100;
 	do {
@@ -1414,6 +1441,9 @@ exynos_dsi_detect(struct drm_connector *connector, bool force)
 
 static void exynos_dsi_connector_destroy(struct drm_connector *connector)
 {
+	drm_connector_unregister(connector);
+	drm_connector_cleanup(connector);
+	connector->dev = NULL;
 }
 
 static struct drm_connector_funcs exynos_dsi_connector_funcs = {
@@ -1634,10 +1664,10 @@ static void exynos_dsi_unbind(struct device *dev, struct device *master,
 
 	exynos_dsi_dpms(&exynos_dsi_display, DRM_MODE_DPMS_OFF);
 
-	mipi_dsi_host_unregister(&dsi->dsi_host);
-
+	exynos_dsi_connector_destroy(&dsi->connector);
 	encoder->funcs->destroy(encoder);
-	drm_connector_cleanup(&dsi->connector);
+
+	mipi_dsi_host_unregister(&dsi->dsi_host);
 }
 
 static const struct component_ops exynos_dsi_component_ops = {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fb.c b/drivers/gpu/drm/exynos/exynos_drm_fb.c
index 65a22cad7b36..d346d1e6eda0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fb.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fb.c
@@ -165,6 +165,7 @@ exynos_drm_framebuffer_init(struct drm_device *dev,
 
 	ret = drm_framebuffer_init(dev, &exynos_fb->fb, &exynos_drm_fb_funcs);
 	if (ret) {
+		kfree(exynos_fb);
 		DRM_ERROR("failed to initialize framebuffer\n");
 		return ERR_PTR(ret);
 	}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
index 32e63f60e1d1..e12ea90c6237 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fbdev.c
@@ -123,6 +123,7 @@ static int exynos_drm_fbdev_update(struct drm_fb_helper *helper,
 
 	fbi->screen_base = buffer->kvaddr + offset;
 	fbi->screen_size = size;
+	fbi->fix.smem_len = size;
 
 	return 0;
 }
@@ -353,9 +354,6 @@ void exynos_drm_fbdev_fini(struct drm_device *dev)
 
 	fbdev = to_exynos_fbdev(private->fb_helper);
 
-	if (fbdev->exynos_gem_obj)
-		exynos_drm_gem_destroy(fbdev->exynos_gem_obj);
-
 	exynos_drm_fbdev_destroy(dev, private->fb_helper);
 	kfree(fbdev);
 	private->fb_helper = NULL;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimc.c b/drivers/gpu/drm/exynos/exynos_drm_fimc.c
index ec7cc9ea50df..68d38eb6774d 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimc.c
@@ -336,9 +336,6 @@ static bool fimc_check_ovf(struct fimc_context *ctx)
 		fimc_set_bits(ctx, EXYNOS_CIWDOFST,
 			EXYNOS_CIWDOFST_CLROVFIY | EXYNOS_CIWDOFST_CLROVFICB |
 			EXYNOS_CIWDOFST_CLROVFICR);
-		fimc_clear_bits(ctx, EXYNOS_CIWDOFST,
-			EXYNOS_CIWDOFST_CLROVFIY | EXYNOS_CIWDOFST_CLROVFICB |
-			EXYNOS_CIWDOFST_CLROVFICR);
 
 		dev_err(ippdrv->dev, "occurred overflow at %d, status 0x%x.\n",
 			ctx->id, status);
@@ -718,24 +715,24 @@ static int fimc_src_set_addr(struct device *dev,
 	case IPP_BUF_ENQUEUE:
 		config = &property->config[EXYNOS_DRM_OPS_SRC];
 		fimc_write(ctx, buf_info->base[EXYNOS_DRM_PLANAR_Y],
-			EXYNOS_CIIYSA(buf_id));
+			EXYNOS_CIIYSA0);
 
 		if (config->fmt == DRM_FORMAT_YVU420) {
 			fimc_write(ctx, buf_info->base[EXYNOS_DRM_PLANAR_CR],
-				EXYNOS_CIICBSA(buf_id));
+				EXYNOS_CIICBSA0);
 			fimc_write(ctx, buf_info->base[EXYNOS_DRM_PLANAR_CB],
-				EXYNOS_CIICRSA(buf_id));
+				EXYNOS_CIICRSA0);
 		} else {
 			fimc_write(ctx, buf_info->base[EXYNOS_DRM_PLANAR_CB],
-				EXYNOS_CIICBSA(buf_id));
+				EXYNOS_CIICBSA0);
 			fimc_write(ctx, buf_info->base[EXYNOS_DRM_PLANAR_CR],
-				EXYNOS_CIICRSA(buf_id));
+				EXYNOS_CIICRSA0);
 		}
 		break;
 	case IPP_BUF_DEQUEUE:
-		fimc_write(ctx, 0x0, EXYNOS_CIIYSA(buf_id));
-		fimc_write(ctx, 0x0, EXYNOS_CIICBSA(buf_id));
-		fimc_write(ctx, 0x0, EXYNOS_CIICRSA(buf_id));
+		fimc_write(ctx, 0x0, EXYNOS_CIIYSA0);
+		fimc_write(ctx, 0x0, EXYNOS_CIICBSA0);
+		fimc_write(ctx, 0x0, EXYNOS_CIICRSA0);
 		break;
 	default:
 		/* bypass */
@@ -1122,67 +1119,34 @@ static int fimc_dst_set_size(struct device *dev, int swap,
 	return 0;
 }
 
-static int fimc_dst_get_buf_count(struct fimc_context *ctx)
-{
-	u32 cfg, buf_num;
-
-	cfg = fimc_read(ctx, EXYNOS_CIFCNTSEQ);
-
-	buf_num = hweight32(cfg);
-
-	DRM_DEBUG_KMS("buf_num[%d]\n", buf_num);
-
-	return buf_num;
-}
-
-static int fimc_dst_set_buf_seq(struct fimc_context *ctx, u32 buf_id,
+static void fimc_dst_set_buf_seq(struct fimc_context *ctx, u32 buf_id,
 		enum drm_exynos_ipp_buf_type buf_type)
 {
-	struct exynos_drm_ippdrv *ippdrv = &ctx->ippdrv;
-	bool enable;
-	u32 cfg;
-	u32 mask = 0x00000001 << buf_id;
-	int ret = 0;
 	unsigned long flags;
+	u32 buf_num;
+	u32 cfg;
 
 	DRM_DEBUG_KMS("buf_id[%d]buf_type[%d]\n", buf_id, buf_type);
 
 	spin_lock_irqsave(&ctx->lock, flags);
 
-	/* mask register set */
 	cfg = fimc_read(ctx, EXYNOS_CIFCNTSEQ);
 
-	switch (buf_type) {
-	case IPP_BUF_ENQUEUE:
-		enable = true;
-		break;
-	case IPP_BUF_DEQUEUE:
-		enable = false;
-		break;
-	default:
-		dev_err(ippdrv->dev, "invalid buf ctrl parameter.\n");
-		ret =  -EINVAL;
-		goto err_unlock;
-	}
+	if (buf_type == IPP_BUF_ENQUEUE)
+		cfg |= (1 << buf_id);
+	else
+		cfg &= ~(1 << buf_id);
 
-	/* sequence id */
-	cfg &= ~mask;
-	cfg |= (enable << buf_id);
 	fimc_write(ctx, cfg, EXYNOS_CIFCNTSEQ);
 
-	/* interrupt enable */
-	if (buf_type == IPP_BUF_ENQUEUE &&
-	    fimc_dst_get_buf_count(ctx) >= FIMC_BUF_START)
-		fimc_mask_irq(ctx, true);
+	buf_num = hweight32(cfg);
 
-	/* interrupt disable */
-	if (buf_type == IPP_BUF_DEQUEUE &&
-	    fimc_dst_get_buf_count(ctx) <= FIMC_BUF_STOP)
+	if (buf_type == IPP_BUF_ENQUEUE && buf_num >= FIMC_BUF_START)
+		fimc_mask_irq(ctx, true);
+	else if (buf_type == IPP_BUF_DEQUEUE && buf_num <= FIMC_BUF_STOP)
 		fimc_mask_irq(ctx, false);
 
-err_unlock:
 	spin_unlock_irqrestore(&ctx->lock, flags);
-	return ret;
 }
 
 static int fimc_dst_set_addr(struct device *dev,
@@ -1240,7 +1204,9 @@ static int fimc_dst_set_addr(struct device *dev,
 		break;
 	}
 
-	return fimc_dst_set_buf_seq(ctx, buf_id, buf_type);
+	fimc_dst_set_buf_seq(ctx, buf_id, buf_type);
+
+	return 0;
 }
 
 static struct exynos_drm_ipp_ops fimc_dst_ops = {
@@ -1291,14 +1257,11 @@ static irqreturn_t fimc_irq_handler(int irq, void *dev_id)
 
 	DRM_DEBUG_KMS("buf_id[%d]\n", buf_id);
 
-	if (fimc_dst_set_buf_seq(ctx, buf_id, IPP_BUF_DEQUEUE) < 0) {
-		DRM_ERROR("failed to dequeue.\n");
-		return IRQ_HANDLED;
-	}
+	fimc_dst_set_buf_seq(ctx, buf_id, IPP_BUF_DEQUEUE);
 
 	event_work->ippdrv = ippdrv;
 	event_work->buf_id[EXYNOS_DRM_OPS_DST] = buf_id;
-	queue_work(ippdrv->event_workq, (struct work_struct *)event_work);
+	queue_work(ippdrv->event_workq, &event_work->work);
 
 	return IRQ_HANDLED;
 }
@@ -1590,11 +1553,8 @@ static int fimc_ippdrv_start(struct device *dev, enum drm_exynos_ipp_cmd cmd)
 
 	fimc_clear_bits(ctx, EXYNOS_CIOCTRL, EXYNOS_CIOCTRL_WEAVE_MASK);
 
-	if (cmd == IPP_CMD_M2M) {
-		fimc_set_bits(ctx, EXYNOS_MSCTRL, EXYNOS_MSCTRL_ENVID);
-
+	if (cmd == IPP_CMD_M2M)
 		fimc_set_bits(ctx, EXYNOS_MSCTRL, EXYNOS_MSCTRL_ENVID);
-	}
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_fimd.c b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
index 5d09e33fef87..085b066a9993 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_fimd.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_fimd.c
@@ -104,6 +104,14 @@ static struct fimd_driver_data s3c64xx_fimd_driver_data = {
 	.has_limited_fmt = 1,
 };
 
+static struct fimd_driver_data exynos3_fimd_driver_data = {
+	.timing_base = 0x20000,
+	.lcdblk_offset = 0x210,
+	.lcdblk_bypass_shift = 1,
+	.has_shadowcon = 1,
+	.has_vidoutcon = 1,
+};
+
 static struct fimd_driver_data exynos4_fimd_driver_data = {
 	.timing_base = 0x0,
 	.lcdblk_offset = 0x210,
@@ -168,6 +176,8 @@ struct fimd_context {
 static const struct of_device_id fimd_driver_dt_match[] = {
 	{ .compatible = "samsung,s3c6400-fimd",
 	  .data = &s3c64xx_fimd_driver_data },
+	{ .compatible = "samsung,exynos3250-fimd",
+	  .data = &exynos3_fimd_driver_data },
 	{ .compatible = "samsung,exynos4210-fimd",
 	  .data = &exynos4_fimd_driver_data },
 	{ .compatible = "samsung,exynos5250-fimd",
@@ -204,7 +214,6 @@ static void fimd_wait_for_vblank(struct exynos_drm_manager *mgr)
 		DRM_DEBUG_KMS("vblank wait timed out.\n");
 }
 
-
 static void fimd_clear_channel(struct exynos_drm_manager *mgr)
 {
 	struct fimd_context *ctx = mgr->ctx;
@@ -214,17 +223,31 @@ static void fimd_clear_channel(struct exynos_drm_manager *mgr)
 
 	/* Check if any channel is enabled. */
 	for (win = 0; win < WINDOWS_NR; win++) {
-		u32 val = readl(ctx->regs + SHADOWCON);
-		if (val & SHADOWCON_CHx_ENABLE(win)) {
-			val &= ~SHADOWCON_CHx_ENABLE(win);
-			writel(val, ctx->regs + SHADOWCON);
+		u32 val = readl(ctx->regs + WINCON(win));
+
+		if (val & WINCONx_ENWIN) {
+			/* wincon */
+			val &= ~WINCONx_ENWIN;
+			writel(val, ctx->regs + WINCON(win));
+
+			/* unprotect windows */
+			if (ctx->driver_data->has_shadowcon) {
+				val = readl(ctx->regs + SHADOWCON);
+				val &= ~SHADOWCON_CHx_ENABLE(win);
+				writel(val, ctx->regs + SHADOWCON);
+			}
 			ch_enabled = 1;
 		}
 	}
 
 	/* Wait for vsync, as disable channel takes effect at next vsync */
-	if (ch_enabled)
+	if (ch_enabled) {
+		unsigned int state = ctx->suspended;
+
+		ctx->suspended = 0;
 		fimd_wait_for_vblank(mgr);
+		ctx->suspended = state;
+	}
 }
 
 static int fimd_mgr_initialize(struct exynos_drm_manager *mgr,
@@ -237,23 +260,6 @@ static int fimd_mgr_initialize(struct exynos_drm_manager *mgr,
 	mgr->drm_dev = ctx->drm_dev = drm_dev;
 	mgr->pipe = ctx->pipe = priv->pipe++;
 
-	/*
-	 * enable drm irq mode.
-	 * - with irq_enabled = true, we can use the vblank feature.
-	 *
-	 * P.S. note that we wouldn't use drm irq handler but
-	 *	just specific driver own one instead because
-	 *	drm framework supports only one irq handler.
-	 */
-	drm_dev->irq_enabled = true;
-
-	/*
-	 * with vblank_disable_allowed = true, vblank interrupt will be disabled
-	 * by drm timer once a current process gives up ownership of
-	 * vblank event.(after drm_vblank_put function is called)
-	 */
-	drm_dev->vblank_disable_allowed = true;
-
 	/* attach this sub driver to iommu mapping if supported. */
 	if (is_drm_iommu_supported(ctx->drm_dev)) {
 		/*
@@ -1051,7 +1057,6 @@ static void fimd_unbind(struct device *dev, struct device *master,
 {
 	struct exynos_drm_manager *mgr = dev_get_drvdata(dev);
 	struct fimd_context *ctx = fimd_manager.ctx;
-	struct drm_crtc *crtc = mgr->crtc;
 
 	fimd_dpms(mgr, DRM_MODE_DPMS_OFF);
 
@@ -1059,8 +1064,6 @@ static void fimd_unbind(struct device *dev, struct device *master,
 		exynos_dpi_remove(dev);
 
 	fimd_mgr_remove(mgr);
-
-	crtc->funcs->destroy(crtc);
 }
 
 static const struct component_ops fimd_component_ops = {
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.c b/drivers/gpu/drm/exynos/exynos_drm_gem.c
index 15db80138382..0d5b9698d384 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.c
@@ -318,40 +318,16 @@ void exynos_drm_gem_put_dma_addr(struct drm_device *dev,
 	drm_gem_object_unreference_unlocked(obj);
 }
 
-int exynos_drm_gem_map_offset_ioctl(struct drm_device *dev, void *data,
-				    struct drm_file *file_priv)
-{
-	struct drm_exynos_gem_map_off *args = data;
-
-	DRM_DEBUG_KMS("handle = 0x%x, offset = 0x%lx\n",
-			args->handle, (unsigned long)args->offset);
-
-	if (!(dev->driver->driver_features & DRIVER_GEM)) {
-		DRM_ERROR("does not support GEM.\n");
-		return -ENODEV;
-	}
-
-	return exynos_drm_gem_dumb_map_offset(file_priv, dev, args->handle,
-			&args->offset);
-}
-
-int exynos_drm_gem_mmap_buffer(struct file *filp,
+int exynos_drm_gem_mmap_buffer(struct exynos_drm_gem_obj *exynos_gem_obj,
 				      struct vm_area_struct *vma)
 {
-	struct drm_gem_object *obj = filp->private_data;
-	struct exynos_drm_gem_obj *exynos_gem_obj = to_exynos_gem_obj(obj);
-	struct drm_device *drm_dev = obj->dev;
+	struct drm_device *drm_dev = exynos_gem_obj->base.dev;
 	struct exynos_drm_gem_buf *buffer;
 	unsigned long vm_size;
 	int ret;
 
-	WARN_ON(!mutex_is_locked(&obj->dev->struct_mutex));
-
-	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
-	vma->vm_private_data = obj;
-	vma->vm_ops = drm_dev->driver->gem_vm_ops;
-
-	update_vm_cache_attr(exynos_gem_obj, vma);
+	vma->vm_flags &= ~VM_PFNMAP;
+	vma->vm_pgoff = 0;
 
 	vm_size = vma->vm_end - vma->vm_start;
 
@@ -373,60 +349,6 @@ int exynos_drm_gem_mmap_buffer(struct file *filp,
 		return ret;
 	}
 
-	/*
-	 * take a reference to this mapping of the object. And this reference
-	 * is unreferenced by the corresponding vm_close call.
-	 */
-	drm_gem_object_reference(obj);
-
-	drm_vm_open_locked(drm_dev, vma);
-
-	return 0;
-}
-
-int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
-			      struct drm_file *file_priv)
-{
-	struct drm_exynos_file_private *exynos_file_priv;
-	struct drm_exynos_gem_mmap *args = data;
-	struct drm_gem_object *obj;
-	struct file *anon_filp;
-	unsigned long addr;
-
-	if (!(dev->driver->driver_features & DRIVER_GEM)) {
-		DRM_ERROR("does not support GEM.\n");
-		return -ENODEV;
-	}
-
-	mutex_lock(&dev->struct_mutex);
-
-	obj = drm_gem_object_lookup(dev, file_priv, args->handle);
-	if (!obj) {
-		DRM_ERROR("failed to lookup gem object.\n");
-		mutex_unlock(&dev->struct_mutex);
-		return -EINVAL;
-	}
-
-	exynos_file_priv = file_priv->driver_priv;
-	anon_filp = exynos_file_priv->anon_filp;
-	anon_filp->private_data = obj;
-
-	addr = vm_mmap(anon_filp, 0, args->size, PROT_READ | PROT_WRITE,
-			MAP_SHARED, 0);
-
-	drm_gem_object_unreference(obj);
-
-	if (IS_ERR_VALUE(addr)) {
-		mutex_unlock(&dev->struct_mutex);
-		return (int)addr;
-	}
-
-	mutex_unlock(&dev->struct_mutex);
-
-	args->mapped = addr;
-
-	DRM_DEBUG_KMS("mapped = 0x%lx\n", (unsigned long)args->mapped);
-
 	return 0;
 }
 
@@ -710,16 +632,20 @@ int exynos_drm_gem_mmap(struct file *filp, struct vm_area_struct *vma)
 	exynos_gem_obj = to_exynos_gem_obj(obj);
 
 	ret = check_gem_flags(exynos_gem_obj->flags);
-	if (ret) {
-		drm_gem_vm_close(vma);
-		drm_gem_free_mmap_offset(obj);
-		return ret;
-	}
-
-	vma->vm_flags &= ~VM_PFNMAP;
-	vma->vm_flags |= VM_MIXEDMAP;
+	if (ret)
+		goto err_close_vm;
 
 	update_vm_cache_attr(exynos_gem_obj, vma);
 
+	ret = exynos_drm_gem_mmap_buffer(exynos_gem_obj, vma);
+	if (ret)
+		goto err_close_vm;
+
+	return ret;
+
+err_close_vm:
+	drm_gem_vm_close(vma);
+	drm_gem_free_mmap_offset(obj);
+
 	return ret;
 }
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gem.h b/drivers/gpu/drm/exynos/exynos_drm_gem.h
index 1592c0ba7de8..ec58fe9c40df 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gem.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_gem.h
@@ -12,6 +12,8 @@
 #ifndef _EXYNOS_DRM_GEM_H_
 #define _EXYNOS_DRM_GEM_H_
 
+#include <drm/drm_gem.h>
+
 #define to_exynos_gem_obj(x)	container_of(x,\
 			struct exynos_drm_gem_obj, base)
 
@@ -111,20 +113,6 @@ void exynos_drm_gem_put_dma_addr(struct drm_device *dev,
 					unsigned int gem_handle,
 					struct drm_file *filp);
 
-/* get buffer offset to map to user space. */
-int exynos_drm_gem_map_offset_ioctl(struct drm_device *dev, void *data,
-				    struct drm_file *file_priv);
-
-/*
- * mmap the physically continuous memory that a gem object contains
- * to user space.
- */
-int exynos_drm_gem_mmap_ioctl(struct drm_device *dev, void *data,
-			      struct drm_file *file_priv);
-
-int exynos_drm_gem_mmap_buffer(struct file *filp,
-				      struct vm_area_struct *vma);
-
 /* map user space allocated by malloc to pages. */
 int exynos_drm_gem_userptr_ioctl(struct drm_device *dev, void *data,
 				      struct drm_file *file_priv);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_gsc.c b/drivers/gpu/drm/exynos/exynos_drm_gsc.c
index 9e3ff1672965..c6a013fc321c 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_gsc.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_gsc.c
@@ -1326,8 +1326,7 @@ static irqreturn_t gsc_irq_handler(int irq, void *dev_id)
 			buf_id[EXYNOS_DRM_OPS_SRC];
 		event_work->buf_id[EXYNOS_DRM_OPS_DST] =
 			buf_id[EXYNOS_DRM_OPS_DST];
-		queue_work(ippdrv->event_workq,
-			(struct work_struct *)event_work);
+		queue_work(ippdrv->event_workq, &event_work->work);
 	}
 
 	return IRQ_HANDLED;
diff --git a/drivers/gpu/drm/exynos/exynos_drm_ipp.c b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
index c411399070d6..00d74b18f7cb 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_ipp.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_ipp.c
@@ -75,7 +75,6 @@ struct drm_exynos_ipp_mem_node {
 	u32	prop_id;
 	u32	buf_id;
 	struct drm_exynos_ipp_buf_info	buf_info;
-	struct drm_file		*filp;
 };
 
 /*
@@ -319,44 +318,6 @@ static void ipp_print_property(struct drm_exynos_ipp_property *property,
 		sz->hsize, sz->vsize, config->flip, config->degree);
 }
 
-static int ipp_find_and_set_property(struct drm_exynos_ipp_property *property)
-{
-	struct exynos_drm_ippdrv *ippdrv;
-	struct drm_exynos_ipp_cmd_node *c_node;
-	u32 prop_id = property->prop_id;
-
-	DRM_DEBUG_KMS("prop_id[%d]\n", prop_id);
-
-	ippdrv = ipp_find_drv_by_handle(prop_id);
-	if (IS_ERR(ippdrv)) {
-		DRM_ERROR("failed to get ipp driver.\n");
-		return -EINVAL;
-	}
-
-	/*
-	 * Find command node using command list in ippdrv.
-	 * when we find this command no using prop_id.
-	 * return property information set in this command node.
-	 */
-	mutex_lock(&ippdrv->cmd_lock);
-	list_for_each_entry(c_node, &ippdrv->cmd_list, list) {
-		if ((c_node->property.prop_id == prop_id) &&
-		    (c_node->state == IPP_STATE_STOP)) {
-			mutex_unlock(&ippdrv->cmd_lock);
-			DRM_DEBUG_KMS("found cmd[%d]ippdrv[0x%x]\n",
-				property->cmd, (int)ippdrv);
-
-			c_node->property = *property;
-			return 0;
-		}
-	}
-	mutex_unlock(&ippdrv->cmd_lock);
-
-	DRM_ERROR("failed to search property.\n");
-
-	return -EINVAL;
-}
-
 static struct drm_exynos_ipp_cmd_work *ipp_create_cmd_work(void)
 {
 	struct drm_exynos_ipp_cmd_work *cmd_work;
@@ -392,6 +353,7 @@ int exynos_drm_ipp_set_property(struct drm_device *drm_dev, void *data,
 	struct drm_exynos_ipp_property *property = data;
 	struct exynos_drm_ippdrv *ippdrv;
 	struct drm_exynos_ipp_cmd_node *c_node;
+	u32 prop_id;
 	int ret, i;
 
 	if (!ctx) {
@@ -404,6 +366,8 @@ int exynos_drm_ipp_set_property(struct drm_device *drm_dev, void *data,
 		return -EINVAL;
 	}
 
+	prop_id = property->prop_id;
+
 	/*
 	 * This is log print for user application property.
 	 * user application set various property.
@@ -412,14 +376,24 @@ int exynos_drm_ipp_set_property(struct drm_device *drm_dev, void *data,
 		ipp_print_property(property, i);
 
 	/*
-	 * set property ioctl generated new prop_id.
-	 * but in this case already asigned prop_id using old set property.
-	 * e.g PAUSE state. this case supports find current prop_id and use it
-	 * instead of allocation.
+	 * In case prop_id is not zero try to set existing property.
 	 */
-	if (property->prop_id) {
-		DRM_DEBUG_KMS("prop_id[%d]\n", property->prop_id);
-		return ipp_find_and_set_property(property);
+	if (prop_id) {
+		c_node = ipp_find_obj(&ctx->prop_idr, &ctx->prop_lock, prop_id);
+
+		if (!c_node || c_node->filp != file) {
+			DRM_DEBUG_KMS("prop_id[%d] not found\n", prop_id);
+			return -EINVAL;
+		}
+
+		if (c_node->state != IPP_STATE_STOP) {
+			DRM_DEBUG_KMS("prop_id[%d] not stopped\n", prop_id);
+			return -EINVAL;
+		}
+
+		c_node->property = *property;
+
+		return 0;
 	}
 
 	/* find ipp driver using ipp id */
@@ -445,9 +419,9 @@ int exynos_drm_ipp_set_property(struct drm_device *drm_dev, void *data,
 		property->prop_id, property->cmd, (int)ippdrv);
 
 	/* stored property information and ippdrv in private data */
-	c_node->dev = dev;
 	c_node->property = *property;
 	c_node->state = IPP_STATE_IDLE;
+	c_node->filp = file;
 
 	c_node->start_work = ipp_create_cmd_work();
 	if (IS_ERR(c_node->start_work)) {
@@ -499,105 +473,37 @@ err_clear:
 	return ret;
 }
 
-static void ipp_clean_cmd_node(struct ipp_context *ctx,
-				struct drm_exynos_ipp_cmd_node *c_node)
-{
-	/* delete list */
-	list_del(&c_node->list);
-
-	ipp_remove_id(&ctx->prop_idr, &ctx->prop_lock,
-			c_node->property.prop_id);
-
-	/* destroy mutex */
-	mutex_destroy(&c_node->lock);
-	mutex_destroy(&c_node->mem_lock);
-	mutex_destroy(&c_node->event_lock);
-
-	/* free command node */
-	kfree(c_node->start_work);
-	kfree(c_node->stop_work);
-	kfree(c_node->event_work);
-	kfree(c_node);
-}
-
-static bool ipp_check_mem_list(struct drm_exynos_ipp_cmd_node *c_node)
-{
-	switch (c_node->property.cmd) {
-	case IPP_CMD_WB:
-		return !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_DST]);
-	case IPP_CMD_OUTPUT:
-		return !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_SRC]);
-	case IPP_CMD_M2M:
-	default:
-		return !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_SRC]) &&
-		       !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_DST]);
-	}
-}
-
-static struct drm_exynos_ipp_mem_node
-		*ipp_find_mem_node(struct drm_exynos_ipp_cmd_node *c_node,
-		struct drm_exynos_ipp_queue_buf *qbuf)
-{
-	struct drm_exynos_ipp_mem_node *m_node;
-	struct list_head *head;
-	int count = 0;
-
-	DRM_DEBUG_KMS("buf_id[%d]\n", qbuf->buf_id);
-
-	/* source/destination memory list */
-	head = &c_node->mem_list[qbuf->ops_id];
-
-	/* find memory node from memory list */
-	list_for_each_entry(m_node, head, list) {
-		DRM_DEBUG_KMS("count[%d]m_node[0x%x]\n", count++, (int)m_node);
-
-		/* compare buffer id */
-		if (m_node->buf_id == qbuf->buf_id)
-			return m_node;
-	}
-
-	return NULL;
-}
-
-static int ipp_set_mem_node(struct exynos_drm_ippdrv *ippdrv,
+static int ipp_put_mem_node(struct drm_device *drm_dev,
 		struct drm_exynos_ipp_cmd_node *c_node,
 		struct drm_exynos_ipp_mem_node *m_node)
 {
-	struct exynos_drm_ipp_ops *ops = NULL;
-	int ret = 0;
+	int i;
 
 	DRM_DEBUG_KMS("node[0x%x]\n", (int)m_node);
 
 	if (!m_node) {
-		DRM_ERROR("invalid queue node.\n");
+		DRM_ERROR("invalid dequeue node.\n");
 		return -EFAULT;
 	}
 
 	DRM_DEBUG_KMS("ops_id[%d]\n", m_node->ops_id);
 
-	/* get operations callback */
-	ops = ippdrv->ops[m_node->ops_id];
-	if (!ops) {
-		DRM_ERROR("not support ops.\n");
-		return -EFAULT;
+	/* put gem buffer */
+	for_each_ipp_planar(i) {
+		unsigned long handle = m_node->buf_info.handles[i];
+		if (handle)
+			exynos_drm_gem_put_dma_addr(drm_dev, handle,
+							c_node->filp);
 	}
 
-	/* set address and enable irq */
-	if (ops->set_addr) {
-		ret = ops->set_addr(ippdrv->dev, &m_node->buf_info,
-			m_node->buf_id, IPP_BUF_ENQUEUE);
-		if (ret) {
-			DRM_ERROR("failed to set addr.\n");
-			return ret;
-		}
-	}
+	list_del(&m_node->list);
+	kfree(m_node);
 
-	return ret;
+	return 0;
 }
 
 static struct drm_exynos_ipp_mem_node
 		*ipp_get_mem_node(struct drm_device *drm_dev,
-		struct drm_file *file,
 		struct drm_exynos_ipp_cmd_node *c_node,
 		struct drm_exynos_ipp_queue_buf *qbuf)
 {
@@ -615,6 +521,7 @@ static struct drm_exynos_ipp_mem_node
 	m_node->ops_id = qbuf->ops_id;
 	m_node->prop_id = qbuf->prop_id;
 	m_node->buf_id = qbuf->buf_id;
+	INIT_LIST_HEAD(&m_node->list);
 
 	DRM_DEBUG_KMS("m_node[0x%x]ops_id[%d]\n", (int)m_node, qbuf->ops_id);
 	DRM_DEBUG_KMS("prop_id[%d]buf_id[%d]\n", qbuf->prop_id, m_node->buf_id);
@@ -627,10 +534,11 @@ static struct drm_exynos_ipp_mem_node
 			dma_addr_t *addr;
 
 			addr = exynos_drm_gem_get_dma_addr(drm_dev,
-					qbuf->handle[i], file);
+					qbuf->handle[i], c_node->filp);
 			if (IS_ERR(addr)) {
 				DRM_ERROR("failed to get addr.\n");
-				goto err_clear;
+				ipp_put_mem_node(drm_dev, c_node, m_node);
+				return ERR_PTR(-EFAULT);
 			}
 
 			buf_info->handles[i] = qbuf->handle[i];
@@ -640,46 +548,30 @@ static struct drm_exynos_ipp_mem_node
 		}
 	}
 
-	m_node->filp = file;
 	mutex_lock(&c_node->mem_lock);
 	list_add_tail(&m_node->list, &c_node->mem_list[qbuf->ops_id]);
 	mutex_unlock(&c_node->mem_lock);
 
 	return m_node;
-
-err_clear:
-	kfree(m_node);
-	return ERR_PTR(-EFAULT);
 }
 
-static int ipp_put_mem_node(struct drm_device *drm_dev,
-		struct drm_exynos_ipp_cmd_node *c_node,
-		struct drm_exynos_ipp_mem_node *m_node)
+static void ipp_clean_mem_nodes(struct drm_device *drm_dev,
+			       struct drm_exynos_ipp_cmd_node *c_node, int ops)
 {
-	int i;
-
-	DRM_DEBUG_KMS("node[0x%x]\n", (int)m_node);
+	struct drm_exynos_ipp_mem_node *m_node, *tm_node;
+	struct list_head *head = &c_node->mem_list[ops];
 
-	if (!m_node) {
-		DRM_ERROR("invalid dequeue node.\n");
-		return -EFAULT;
-	}
+	mutex_lock(&c_node->mem_lock);
 
-	DRM_DEBUG_KMS("ops_id[%d]\n", m_node->ops_id);
+	list_for_each_entry_safe(m_node, tm_node, head, list) {
+		int ret;
 
-	/* put gem buffer */
-	for_each_ipp_planar(i) {
-		unsigned long handle = m_node->buf_info.handles[i];
-		if (handle)
-			exynos_drm_gem_put_dma_addr(drm_dev, handle,
-							m_node->filp);
+		ret = ipp_put_mem_node(drm_dev, c_node, m_node);
+		if (ret)
+			DRM_ERROR("failed to put m_node.\n");
 	}
 
-	/* delete list in queue */
-	list_del(&m_node->list);
-	kfree(m_node);
-
-	return 0;
+	mutex_unlock(&c_node->mem_lock);
 }
 
 static void ipp_free_event(struct drm_pending_event *event)
@@ -688,7 +580,6 @@ static void ipp_free_event(struct drm_pending_event *event)
 }
 
 static int ipp_get_event(struct drm_device *drm_dev,
-		struct drm_file *file,
 		struct drm_exynos_ipp_cmd_node *c_node,
 		struct drm_exynos_ipp_queue_buf *qbuf)
 {
@@ -700,7 +591,7 @@ static int ipp_get_event(struct drm_device *drm_dev,
 	e = kzalloc(sizeof(*e), GFP_KERNEL);
 	if (!e) {
 		spin_lock_irqsave(&drm_dev->event_lock, flags);
-		file->event_space += sizeof(e->event);
+		c_node->filp->event_space += sizeof(e->event);
 		spin_unlock_irqrestore(&drm_dev->event_lock, flags);
 		return -ENOMEM;
 	}
@@ -712,7 +603,7 @@ static int ipp_get_event(struct drm_device *drm_dev,
 	e->event.prop_id = qbuf->prop_id;
 	e->event.buf_id[EXYNOS_DRM_OPS_DST] = qbuf->buf_id;
 	e->base.event = &e->event.base;
-	e->base.file_priv = file;
+	e->base.file_priv = c_node->filp;
 	e->base.destroy = ipp_free_event;
 	mutex_lock(&c_node->event_lock);
 	list_add_tail(&e->base.link, &c_node->event_list);
@@ -757,6 +648,115 @@ out_unlock:
 	return;
 }
 
+static void ipp_clean_cmd_node(struct ipp_context *ctx,
+				struct drm_exynos_ipp_cmd_node *c_node)
+{
+	int i;
+
+	/* cancel works */
+	cancel_work_sync(&c_node->start_work->work);
+	cancel_work_sync(&c_node->stop_work->work);
+	cancel_work_sync(&c_node->event_work->work);
+
+	/* put event */
+	ipp_put_event(c_node, NULL);
+
+	for_each_ipp_ops(i)
+		ipp_clean_mem_nodes(ctx->subdrv.drm_dev, c_node, i);
+
+	/* delete list */
+	list_del(&c_node->list);
+
+	ipp_remove_id(&ctx->prop_idr, &ctx->prop_lock,
+			c_node->property.prop_id);
+
+	/* destroy mutex */
+	mutex_destroy(&c_node->lock);
+	mutex_destroy(&c_node->mem_lock);
+	mutex_destroy(&c_node->event_lock);
+
+	/* free command node */
+	kfree(c_node->start_work);
+	kfree(c_node->stop_work);
+	kfree(c_node->event_work);
+	kfree(c_node);
+}
+
+static bool ipp_check_mem_list(struct drm_exynos_ipp_cmd_node *c_node)
+{
+	switch (c_node->property.cmd) {
+	case IPP_CMD_WB:
+		return !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_DST]);
+	case IPP_CMD_OUTPUT:
+		return !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_SRC]);
+	case IPP_CMD_M2M:
+	default:
+		return !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_SRC]) &&
+		       !list_empty(&c_node->mem_list[EXYNOS_DRM_OPS_DST]);
+	}
+}
+
+static struct drm_exynos_ipp_mem_node
+		*ipp_find_mem_node(struct drm_exynos_ipp_cmd_node *c_node,
+		struct drm_exynos_ipp_queue_buf *qbuf)
+{
+	struct drm_exynos_ipp_mem_node *m_node;
+	struct list_head *head;
+	int count = 0;
+
+	DRM_DEBUG_KMS("buf_id[%d]\n", qbuf->buf_id);
+
+	/* source/destination memory list */
+	head = &c_node->mem_list[qbuf->ops_id];
+
+	/* find memory node from memory list */
+	list_for_each_entry(m_node, head, list) {
+		DRM_DEBUG_KMS("count[%d]m_node[0x%x]\n", count++, (int)m_node);
+
+		/* compare buffer id */
+		if (m_node->buf_id == qbuf->buf_id)
+			return m_node;
+	}
+
+	return NULL;
+}
+
+static int ipp_set_mem_node(struct exynos_drm_ippdrv *ippdrv,
+		struct drm_exynos_ipp_cmd_node *c_node,
+		struct drm_exynos_ipp_mem_node *m_node)
+{
+	struct exynos_drm_ipp_ops *ops = NULL;
+	int ret = 0;
+
+	DRM_DEBUG_KMS("node[0x%x]\n", (int)m_node);
+
+	if (!m_node) {
+		DRM_ERROR("invalid queue node.\n");
+		return -EFAULT;
+	}
+
+	DRM_DEBUG_KMS("ops_id[%d]\n", m_node->ops_id);
+
+	/* get operations callback */
+	ops = ippdrv->ops[m_node->ops_id];
+	if (!ops) {
+		DRM_ERROR("not support ops.\n");
+		return -EFAULT;
+	}
+
+	/* set address and enable irq */
+	if (ops->set_addr) {
+		ret = ops->set_addr(ippdrv->dev, &m_node->buf_info,
+			m_node->buf_id, IPP_BUF_ENQUEUE);
+		if (ret) {
+			DRM_ERROR("failed to set addr.\n");
+			return ret;
+		}
+	}
+
+	return ret;
+}
+
 static void ipp_handle_cmd_work(struct device *dev,
 		struct exynos_drm_ippdrv *ippdrv,
 		struct drm_exynos_ipp_cmd_work *cmd_work,
@@ -766,7 +766,7 @@ static void ipp_handle_cmd_work(struct device *dev,
 
 	cmd_work->ippdrv = ippdrv;
 	cmd_work->c_node = c_node;
-	queue_work(ctx->cmd_workq, (struct work_struct *)cmd_work);
+	queue_work(ctx->cmd_workq, &cmd_work->work);
 }
 
 static int ipp_queue_buf_with_run(struct device *dev,
@@ -872,7 +872,7 @@ int exynos_drm_ipp_queue_buf(struct drm_device *drm_dev, void *data,
 	/* find command node */
 	c_node = ipp_find_obj(&ctx->prop_idr, &ctx->prop_lock,
 		qbuf->prop_id);
-	if (!c_node) {
+	if (!c_node || c_node->filp != file) {
 		DRM_ERROR("failed to get command node.\n");
 		return -ENODEV;
 	}
@@ -881,7 +881,7 @@ int exynos_drm_ipp_queue_buf(struct drm_device *drm_dev, void *data,
 	switch (qbuf->buf_type) {
 	case IPP_BUF_ENQUEUE:
 		/* get memory node */
-		m_node = ipp_get_mem_node(drm_dev, file, c_node, qbuf);
+		m_node = ipp_get_mem_node(drm_dev, c_node, qbuf);
 		if (IS_ERR(m_node)) {
 			DRM_ERROR("failed to get m_node.\n");
 			return PTR_ERR(m_node);
@@ -894,7 +894,7 @@ int exynos_drm_ipp_queue_buf(struct drm_device *drm_dev, void *data,
 		 */
 		if (qbuf->ops_id == EXYNOS_DRM_OPS_DST) {
 			/* get event for destination buffer */
-			ret = ipp_get_event(drm_dev, file, c_node, qbuf);
+			ret = ipp_get_event(drm_dev, c_node, qbuf);
 			if (ret) {
 				DRM_ERROR("failed to get event.\n");
 				goto err_clean_node;
@@ -1007,7 +1007,7 @@ int exynos_drm_ipp_cmd_ctrl(struct drm_device *drm_dev, void *data,
 
 	c_node = ipp_find_obj(&ctx->prop_idr, &ctx->prop_lock,
 		cmd_ctrl->prop_id);
-	if (!c_node) {
+	if (!c_node || c_node->filp != file) {
 		DRM_ERROR("invalid command node list.\n");
 		return -ENODEV;
 	}
@@ -1257,80 +1257,39 @@ static int ipp_stop_property(struct drm_device *drm_dev,
 		struct exynos_drm_ippdrv *ippdrv,
 		struct drm_exynos_ipp_cmd_node *c_node)
 {
-	struct drm_exynos_ipp_mem_node *m_node, *tm_node;
 	struct drm_exynos_ipp_property *property = &c_node->property;
-	struct list_head *head;
-	int ret = 0, i;
+	int i;
 
 	DRM_DEBUG_KMS("prop_id[%d]\n", property->prop_id);
 
-	/* put event */
-	ipp_put_event(c_node, NULL);
-
-	mutex_lock(&c_node->mem_lock);
+	/* stop operations */
+	if (ippdrv->stop)
+		ippdrv->stop(ippdrv->dev, property->cmd);
 
 	/* check command */
 	switch (property->cmd) {
 	case IPP_CMD_M2M:
-		for_each_ipp_ops(i) {
-			/* source/destination memory list */
-			head = &c_node->mem_list[i];
-
-			list_for_each_entry_safe(m_node, tm_node,
-				head, list) {
-				ret = ipp_put_mem_node(drm_dev, c_node,
-					m_node);
-				if (ret) {
-					DRM_ERROR("failed to put m_node.\n");
-					goto err_clear;
-				}
-			}
-		}
+		for_each_ipp_ops(i)
+			ipp_clean_mem_nodes(drm_dev, c_node, i);
 		break;
 	case IPP_CMD_WB:
-		/* destination memory list */
-		head = &c_node->mem_list[EXYNOS_DRM_OPS_DST];
-
-		list_for_each_entry_safe(m_node, tm_node, head, list) {
-			ret = ipp_put_mem_node(drm_dev, c_node, m_node);
-			if (ret) {
-				DRM_ERROR("failed to put m_node.\n");
-				goto err_clear;
-			}
-		}
+		ipp_clean_mem_nodes(drm_dev, c_node, EXYNOS_DRM_OPS_DST);
 		break;
 	case IPP_CMD_OUTPUT:
-		/* source memory list */
-		head = &c_node->mem_list[EXYNOS_DRM_OPS_SRC];
-
-		list_for_each_entry_safe(m_node, tm_node, head, list) {
-			ret = ipp_put_mem_node(drm_dev, c_node, m_node);
-			if (ret) {
-				DRM_ERROR("failed to put m_node.\n");
-				goto err_clear;
-			}
-		}
+		ipp_clean_mem_nodes(drm_dev, c_node, EXYNOS_DRM_OPS_SRC);
 		break;
 	default:
 		DRM_ERROR("invalid operations.\n");
-		ret = -EINVAL;
-		goto err_clear;
+		return -EINVAL;
 	}
 
-err_clear:
-	mutex_unlock(&c_node->mem_lock);
-
-	/* stop operations */
-	if (ippdrv->stop)
-		ippdrv->stop(ippdrv->dev, property->cmd);
-
-	return ret;
+	return 0;
 }
 
 void ipp_sched_cmd(struct work_struct *work)
 {
 	struct drm_exynos_ipp_cmd_work *cmd_work =
-		(struct drm_exynos_ipp_cmd_work *)work;
+		container_of(work, struct drm_exynos_ipp_cmd_work, work);
 	struct exynos_drm_ippdrv *ippdrv;
 	struct drm_exynos_ipp_cmd_node *c_node;
 	struct drm_exynos_ipp_property *property;
@@ -1543,7 +1502,7 @@ err_event_unlock:
 void ipp_sched_event(struct work_struct *work)
 {
 	struct drm_exynos_ipp_event_work *event_work =
-		(struct drm_exynos_ipp_event_work *)work;
+		container_of(work, struct drm_exynos_ipp_event_work, work);
 	struct exynos_drm_ippdrv *ippdrv;
 	struct drm_exynos_ipp_cmd_node *c_node;
 	int ret;
@@ -1646,11 +1605,11 @@ err:
 
 static void ipp_subdrv_remove(struct drm_device *drm_dev, struct device *dev)
 {
-	struct exynos_drm_ippdrv *ippdrv;
+	struct exynos_drm_ippdrv *ippdrv, *t;
 	struct ipp_context *ctx = get_ipp_context(dev);
 
 	/* get ipp driver entry */
-	list_for_each_entry(ippdrv, &exynos_drm_ippdrv_list, drv_list) {
+	list_for_each_entry_safe(ippdrv, t, &exynos_drm_ippdrv_list, drv_list) {
 		if (is_drm_iommu_supported(drm_dev))
 			drm_iommu_detach_device(drm_dev, ippdrv->dev);
 
@@ -1677,14 +1636,11 @@ static int ipp_subdrv_open(struct drm_device *drm_dev, struct device *dev,
 static void ipp_subdrv_close(struct drm_device *drm_dev, struct device *dev,
 		struct drm_file *file)
 {
-	struct drm_exynos_file_private *file_priv = file->driver_priv;
 	struct exynos_drm_ippdrv *ippdrv = NULL;
 	struct ipp_context *ctx = get_ipp_context(dev);
 	struct drm_exynos_ipp_cmd_node *c_node, *tc_node;
 	int count = 0;
 
-	DRM_DEBUG_KMS("for priv[0x%x]\n", (int)file_priv->ipp_dev);
-
 	list_for_each_entry(ippdrv, &exynos_drm_ippdrv_list, drv_list) {
 		mutex_lock(&ippdrv->cmd_lock);
 		list_for_each_entry_safe(c_node, tc_node,
@@ -1692,7 +1648,7 @@ static void ipp_subdrv_close(struct drm_device *drm_dev, struct device *dev,
 			DRM_DEBUG_KMS("count[%d]ippdrv[0x%x]\n",
 				count++, (int)ippdrv);
 
-			if (c_node->dev == file_priv->ipp_dev) {
+			if (c_node->filp == file) {
 				/*
 				 * userland goto unnormal state. process killed.
 				 * and close the file.
@@ -1808,63 +1764,12 @@ static int ipp_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static int ipp_power_ctrl(struct ipp_context *ctx, bool enable)
-{
-	DRM_DEBUG_KMS("enable[%d]\n", enable);
-
-	return 0;
-}
-
-#ifdef CONFIG_PM_SLEEP
-static int ipp_suspend(struct device *dev)
-{
-	struct ipp_context *ctx = get_ipp_context(dev);
-
-	if (pm_runtime_suspended(dev))
-		return 0;
-
-	return ipp_power_ctrl(ctx, false);
-}
-
-static int ipp_resume(struct device *dev)
-{
-	struct ipp_context *ctx = get_ipp_context(dev);
-
-	if (!pm_runtime_suspended(dev))
-		return ipp_power_ctrl(ctx, true);
-
-	return 0;
-}
-#endif
-
-#ifdef CONFIG_PM_RUNTIME
-static int ipp_runtime_suspend(struct device *dev)
-{
-	struct ipp_context *ctx = get_ipp_context(dev);
-
-	return ipp_power_ctrl(ctx, false);
-}
-
-static int ipp_runtime_resume(struct device *dev)
-{
-	struct ipp_context *ctx = get_ipp_context(dev);
-
-	return ipp_power_ctrl(ctx, true);
-}
-#endif
-
-static const struct dev_pm_ops ipp_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(ipp_suspend, ipp_resume)
-	SET_RUNTIME_PM_OPS(ipp_runtime_suspend, ipp_runtime_resume, NULL)
-};
-
 struct platform_driver ipp_driver = {
 	.probe		= ipp_probe,
 	.remove		= ipp_remove,
 	.driver		= {
 		.name	= "exynos-drm-ipp",
 		.owner	= THIS_MODULE,
-		.pm	= &ipp_pm_ops,
 	},
 };
 
diff --git a/drivers/gpu/drm/exynos/exynos_drm_ipp.h b/drivers/gpu/drm/exynos/exynos_drm_ipp.h
index 6f48d62aeb30..2a61547a39d0 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_ipp.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_ipp.h
@@ -48,7 +48,6 @@ struct drm_exynos_ipp_cmd_work {
 /*
  * A structure of command node.
  *
- * @dev: IPP device.
  * @list: list head to command queue information.
  * @event_list: list head of event.
  * @mem_list: list head to source,destination memory queue information.
@@ -62,9 +61,9 @@ struct drm_exynos_ipp_cmd_work {
  * @stop_work: stop command work structure.
  * @event_work: event work structure.
  * @state: state of command node.
+ * @filp: associated file pointer.
  */
 struct drm_exynos_ipp_cmd_node {
-	struct device		*dev;
 	struct list_head	list;
 	struct list_head	event_list;
 	struct list_head	mem_list[EXYNOS_DRM_OPS_MAX];
@@ -78,6 +77,7 @@ struct drm_exynos_ipp_cmd_node {
 	struct drm_exynos_ipp_cmd_work *stop_work;
 	struct drm_exynos_ipp_event_work *event_work;
 	enum drm_exynos_ipp_state	state;
+	struct drm_file	*filp;
 };
 
 /*
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.c b/drivers/gpu/drm/exynos/exynos_drm_plane.c
index 8371cbd7631d..c7045a663763 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.c
@@ -139,6 +139,8 @@ int exynos_plane_mode_set(struct drm_plane *plane, struct drm_crtc *crtc,
 			overlay->crtc_x, overlay->crtc_y,
 			overlay->crtc_width, overlay->crtc_height);
 
+	plane->crtc = crtc;
+
 	exynos_drm_crtc_plane_mode_set(crtc, overlay);
 
 	return 0;
@@ -187,8 +189,6 @@ exynos_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	if (ret < 0)
 		return ret;
 
-	plane->crtc = crtc;
-
 	exynos_plane_commit(plane);
 	exynos_plane_dpms(plane, DRM_MODE_DPMS_ON);
 
@@ -254,25 +254,26 @@ static void exynos_plane_attach_zpos_property(struct drm_plane *plane)
 }
 
 struct drm_plane *exynos_plane_init(struct drm_device *dev,
-				    unsigned long possible_crtcs, bool priv)
+				    unsigned long possible_crtcs,
+				    enum drm_plane_type type)
 {
 	struct exynos_plane *exynos_plane;
 	int err;
 
 	exynos_plane = kzalloc(sizeof(struct exynos_plane), GFP_KERNEL);
 	if (!exynos_plane)
-		return NULL;
+		return ERR_PTR(-ENOMEM);
 
-	err = drm_plane_init(dev, &exynos_plane->base, possible_crtcs,
-			      &exynos_plane_funcs, formats, ARRAY_SIZE(formats),
-			      priv);
+	err = drm_universal_plane_init(dev, &exynos_plane->base, possible_crtcs,
+				       &exynos_plane_funcs, formats,
+				       ARRAY_SIZE(formats), type);
 	if (err) {
 		DRM_ERROR("failed to initialize plane\n");
 		kfree(exynos_plane);
-		return NULL;
+		return ERR_PTR(err);
 	}
 
-	if (priv)
+	if (type == DRM_PLANE_TYPE_PRIMARY)
 		exynos_plane->overlay.zpos = DEFAULT_ZPOS;
 	else
 		exynos_plane_attach_zpos_property(&exynos_plane->base);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_plane.h b/drivers/gpu/drm/exynos/exynos_drm_plane.h
index 84d464c90d3d..0d1986b115f8 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_plane.h
+++ b/drivers/gpu/drm/exynos/exynos_drm_plane.h
@@ -17,4 +17,5 @@ int exynos_plane_mode_set(struct drm_plane *plane, struct drm_crtc *crtc,
 void exynos_plane_commit(struct drm_plane *plane);
 void exynos_plane_dpms(struct drm_plane *plane, int mode);
 struct drm_plane *exynos_plane_init(struct drm_device *dev,
-				    unsigned long possible_crtcs, bool priv);
+				    unsigned long possible_crtcs,
+				    enum drm_plane_type type);
diff --git a/drivers/gpu/drm/exynos/exynos_drm_rotator.c b/drivers/gpu/drm/exynos/exynos_drm_rotator.c
index 55af6b41c1df..b6a37d4f5b13 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_rotator.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_rotator.c
@@ -156,8 +156,7 @@ static irqreturn_t rotator_irq_handler(int irq, void *arg)
 		event_work->ippdrv = ippdrv;
 		event_work->buf_id[EXYNOS_DRM_OPS_DST] =
 			rot->cur_buf_id[EXYNOS_DRM_OPS_DST];
-		queue_work(ippdrv->event_workq,
-			(struct work_struct *)event_work);
+		queue_work(ippdrv->event_workq, &event_work->work);
 	} else {
 		DRM_ERROR("the SFR is set illegally\n");
 	}
diff --git a/drivers/gpu/drm/exynos/exynos_drm_vidi.c b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
index 9528d81d8004..d565207040a2 100644
--- a/drivers/gpu/drm/exynos/exynos_drm_vidi.c
+++ b/drivers/gpu/drm/exynos/exynos_drm_vidi.c
@@ -303,23 +303,6 @@ static int vidi_mgr_initialize(struct exynos_drm_manager *mgr,
 	mgr->drm_dev = ctx->drm_dev = drm_dev;
 	mgr->pipe = ctx->pipe = priv->pipe++;
 
-	/*
-	 * enable drm irq mode.
-	 * - with irq_enabled = 1, we can use the vblank feature.
-	 *
-	 * P.S. note that we wouldn't use drm irq handler but
-	 *	just specific driver own one instead because
-	 *	drm framework supports only one irq handler.
-	 */
-	drm_dev->irq_enabled = 1;
-
-	/*
-	 * with vblank_disable_allowed = 1, vblank interrupt will be disabled
-	 * by drm timer once a current process gives up ownership of
-	 * vblank event.(after drm_vblank_put function is called)
-	 */
-	drm_dev->vblank_disable_allowed = 1;
-
 	return 0;
 }
 
@@ -648,7 +631,6 @@ static int vidi_remove(struct platform_device *pdev)
 	struct exynos_drm_manager *mgr = platform_get_drvdata(pdev);
 	struct vidi_context *ctx = mgr->ctx;
 	struct drm_encoder *encoder = ctx->encoder;
-	struct drm_crtc *crtc = mgr->crtc;
 
 	if (ctx->raw_edid != (struct edid *)fake_edid_info) {
 		kfree(ctx->raw_edid);
@@ -657,7 +639,6 @@ static int vidi_remove(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	crtc->funcs->destroy(crtc);
 	encoder->funcs->destroy(encoder);
 	drm_connector_cleanup(&ctx->connector);
 
diff --git a/drivers/gpu/drm/exynos/exynos_hdmi.c b/drivers/gpu/drm/exynos/exynos_hdmi.c
index 562966db2aa1..7910fb37d9bb 100644
--- a/drivers/gpu/drm/exynos/exynos_hdmi.c
+++ b/drivers/gpu/drm/exynos/exynos_hdmi.c
@@ -1040,6 +1040,8 @@ static enum drm_connector_status hdmi_detect(struct drm_connector *connector,
 
 static void hdmi_connector_destroy(struct drm_connector *connector)
 {
+	drm_connector_unregister(connector);
+	drm_connector_cleanup(connector);
 }
 
 static struct drm_connector_funcs hdmi_connector_funcs = {
@@ -2314,8 +2316,8 @@ static void hdmi_unbind(struct device *dev, struct device *master, void *data)
 	struct drm_encoder *encoder = display->encoder;
 	struct hdmi_context *hdata = display->ctx;
 
+	hdmi_connector_destroy(&hdata->connector);
 	encoder->funcs->destroy(encoder);
-	drm_connector_cleanup(&hdata->connector);
 }
 
 static const struct component_ops hdmi_component_ops = {
diff --git a/drivers/gpu/drm/exynos/exynos_mixer.c b/drivers/gpu/drm/exynos/exynos_mixer.c
index e8b4ec84b312..a41c84ee3a2d 100644
--- a/drivers/gpu/drm/exynos/exynos_mixer.c
+++ b/drivers/gpu/drm/exynos/exynos_mixer.c
@@ -1302,15 +1302,12 @@ static int mixer_bind(struct device *dev, struct device *manager, void *data)
 static void mixer_unbind(struct device *dev, struct device *master, void *data)
 {
 	struct exynos_drm_manager *mgr = dev_get_drvdata(dev);
-	struct drm_crtc *crtc = mgr->crtc;
 
 	dev_info(dev, "remove successful\n");
 
 	mixer_mgr_remove(mgr);
 
 	pm_runtime_disable(dev);
-
-	crtc->funcs->destroy(crtc);
 }
 
 static const struct component_ops mixer_component_ops = {
diff --git a/drivers/gpu/drm/gma500/cdv_intel_dp.c b/drivers/gpu/drm/gma500/cdv_intel_dp.c
index a4cc0e60a1be..9f158eab517a 100644
--- a/drivers/gpu/drm/gma500/cdv_intel_dp.c
+++ b/drivers/gpu/drm/gma500/cdv_intel_dp.c
@@ -1089,7 +1089,7 @@ static char	*link_train_names[] = {
 };
 #endif
 
-#define CDV_DP_VOLTAGE_MAX	    DP_TRAIN_VOLTAGE_SWING_1200
+#define CDV_DP_VOLTAGE_MAX	    DP_TRAIN_VOLTAGE_SWING_LEVEL_3
 /*
 static uint8_t
 cdv_intel_dp_pre_emphasis_max(uint8_t voltage_swing)
@@ -1276,7 +1276,7 @@ cdv_intel_dp_set_vswing_premph(struct gma_encoder *encoder, uint8_t signal_level
 		cdv_sb_write(dev, ddi_reg->VSwing2, dp_vswing_premph_table[index]);
 
 	/* ;gfx_dpio_set_reg(0x814c, 0x40802040) */
-	if ((vswing + premph) == DP_TRAIN_VOLTAGE_SWING_1200)
+	if ((vswing + premph) == DP_TRAIN_VOLTAGE_SWING_LEVEL_3)
 		cdv_sb_write(dev, ddi_reg->VSwing3, 0x70802040);
 	else
 		cdv_sb_write(dev, ddi_reg->VSwing3, 0x40802040);
diff --git a/drivers/gpu/drm/gma500/framebuffer.c b/drivers/gpu/drm/gma500/framebuffer.c
index d0dd3bea8aa5..ddd90ddbc200 100644
--- a/drivers/gpu/drm/gma500/framebuffer.c
+++ b/drivers/gpu/drm/gma500/framebuffer.c
@@ -540,7 +540,8 @@ static void psbfb_gamma_get(struct drm_crtc *crtc, u16 *red,
 static int psbfb_probe(struct drm_fb_helper *helper,
 				struct drm_fb_helper_surface_size *sizes)
 {
-	struct psb_fbdev *psb_fbdev = (struct psb_fbdev *)helper;
+	struct psb_fbdev *psb_fbdev =
+		container_of(helper, struct psb_fbdev, psb_fb_helper);
 	struct drm_device *dev = psb_fbdev->psb_fb_helper.dev;
 	struct drm_psb_private *dev_priv = dev->dev_private;
 	int bytespp;
diff --git a/drivers/gpu/drm/gma500/gtt.h b/drivers/gpu/drm/gma500/gtt.h
index f5860a739bd8..cdbb350c9d5d 100644
--- a/drivers/gpu/drm/gma500/gtt.h
+++ b/drivers/gpu/drm/gma500/gtt.h
@@ -21,6 +21,7 @@
 #define _PSB_GTT_H_
 
 #include <drm/drmP.h>
+#include <drm/drm_gem.h>
 
 /* This wants cleaning up with respect to the psb_dev and un-needed stuff */
 struct psb_gtt {
diff --git a/drivers/gpu/drm/gma500/intel_bios.c b/drivers/gpu/drm/gma500/intel_bios.c
index d3497348c4d5..63bde4e86c6a 100644
--- a/drivers/gpu/drm/gma500/intel_bios.c
+++ b/drivers/gpu/drm/gma500/intel_bios.c
@@ -116,30 +116,30 @@ parse_edp(struct drm_psb_private *dev_priv, struct bdb_header *bdb)
 
 	switch (edp_link_params->preemphasis) {
 	case 0:
-		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPHASIS_0;
+		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_0;
 		break;
 	case 1:
-		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPHASIS_3_5;
+		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_1;
 		break;
 	case 2:
-		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPHASIS_6;
+		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_2;
 		break;
 	case 3:
-		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPHASIS_9_5;
+		dev_priv->edp.preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_3;
 		break;
 	}
 	switch (edp_link_params->vswing) {
 	case 0:
-		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_400;
+		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_0;
 		break;
 	case 1:
-		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_600;
+		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_1;
 		break;
 	case 2:
-		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_800;
+		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
 		break;
 	case 3:
-		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_1200;
+		dev_priv->edp.vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 		break;
 	}
 	DRM_DEBUG_KMS("VBT reports EDP: VSwing  %d, Preemph %d\n",
diff --git a/drivers/gpu/drm/gma500/psb_drv.c b/drivers/gpu/drm/gma500/psb_drv.c
index eec993f93b1a..6ec3a905fdd2 100644
--- a/drivers/gpu/drm/gma500/psb_drv.c
+++ b/drivers/gpu/drm/gma500/psb_drv.c
@@ -476,6 +476,7 @@ static struct drm_driver driver = {
 	.unload = psb_driver_unload,
 	.lastclose = psb_driver_lastclose,
 	.preclose = psb_driver_preclose,
+	.set_busid = drm_pci_set_busid,
 
 	.num_ioctls = ARRAY_SIZE(psb_ioctls),
 	.device_is_agp = psb_driver_device_is_agp,
diff --git a/drivers/gpu/drm/i810/i810_dma.c b/drivers/gpu/drm/i810/i810_dma.c
index bae897de9468..d91856779beb 100644
--- a/drivers/gpu/drm/i810/i810_dma.c
+++ b/drivers/gpu/drm/i810/i810_dma.c
@@ -213,7 +213,7 @@ static int i810_dma_cleanup(struct drm_device *dev)
 		    (drm_i810_private_t *) dev->dev_private;
 
 		if (dev_priv->ring.virtual_start)
-			drm_core_ioremapfree(&dev_priv->ring.map, dev);
+			drm_legacy_ioremapfree(&dev_priv->ring.map, dev);
 		if (dev_priv->hw_status_page) {
 			pci_free_consistent(dev->pdev, PAGE_SIZE,
 					    dev_priv->hw_status_page,
@@ -227,7 +227,7 @@ static int i810_dma_cleanup(struct drm_device *dev)
 			drm_i810_buf_priv_t *buf_priv = buf->dev_private;
 
 			if (buf_priv->kernel_virtual && buf->total)
-				drm_core_ioremapfree(&buf_priv->map, dev);
+				drm_legacy_ioremapfree(&buf_priv->map, dev);
 		}
 	}
 	return 0;
@@ -306,7 +306,7 @@ static int i810_freelist_init(struct drm_device *dev, drm_i810_private_t *dev_pr
 		buf_priv->map.flags = 0;
 		buf_priv->map.mtrr = 0;
 
-		drm_core_ioremap(&buf_priv->map, dev);
+		drm_legacy_ioremap(&buf_priv->map, dev);
 		buf_priv->kernel_virtual = buf_priv->map.handle;
 
 	}
@@ -334,7 +334,7 @@ static int i810_dma_initialize(struct drm_device *dev,
 		DRM_ERROR("can not find sarea!\n");
 		return -EINVAL;
 	}
-	dev_priv->mmio_map = drm_core_findmap(dev, init->mmio_offset);
+	dev_priv->mmio_map = drm_legacy_findmap(dev, init->mmio_offset);
 	if (!dev_priv->mmio_map) {
 		dev->dev_private = (void *)dev_priv;
 		i810_dma_cleanup(dev);
@@ -342,7 +342,7 @@ static int i810_dma_initialize(struct drm_device *dev,
 		return -EINVAL;
 	}
 	dev->agp_buffer_token = init->buffers_offset;
-	dev->agp_buffer_map = drm_core_findmap(dev, init->buffers_offset);
+	dev->agp_buffer_map = drm_legacy_findmap(dev, init->buffers_offset);
 	if (!dev->agp_buffer_map) {
 		dev->dev_private = (void *)dev_priv;
 		i810_dma_cleanup(dev);
@@ -363,7 +363,7 @@ static int i810_dma_initialize(struct drm_device *dev,
 	dev_priv->ring.map.flags = 0;
 	dev_priv->ring.map.mtrr = 0;
 
-	drm_core_ioremap(&dev_priv->ring.map, dev);
+	drm_legacy_ioremap(&dev_priv->ring.map, dev);
 
 	if (dev_priv->ring.map.handle == NULL) {
 		dev->dev_private = (void *)dev_priv;
@@ -1215,9 +1215,9 @@ void i810_driver_preclose(struct drm_device *dev, struct drm_file *file_priv)
 	}
 
 	if (file_priv->master && file_priv->master->lock.hw_lock) {
-		drm_idlelock_take(&file_priv->master->lock);
+		drm_legacy_idlelock_take(&file_priv->master->lock);
 		i810_driver_reclaim_buffers(dev, file_priv);
-		drm_idlelock_release(&file_priv->master->lock);
+		drm_legacy_idlelock_release(&file_priv->master->lock);
 	} else {
 		/* master disappeared, clean up stuff anyway and hope nothing
 		 * goes wrong */
diff --git a/drivers/gpu/drm/i810/i810_drv.c b/drivers/gpu/drm/i810/i810_drv.c
index 441ccf8f5bdc..44f4a131c8dd 100644
--- a/drivers/gpu/drm/i810/i810_drv.c
+++ b/drivers/gpu/drm/i810/i810_drv.c
@@ -47,7 +47,7 @@ static const struct file_operations i810_driver_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = drm_compat_ioctl,
@@ -63,6 +63,7 @@ static struct drm_driver driver = {
 	.load = i810_driver_load,
 	.lastclose = i810_driver_lastclose,
 	.preclose = i810_driver_preclose,
+	.set_busid = drm_pci_set_busid,
 	.device_is_agp = i810_driver_device_is_agp,
 	.dma_quiescent = i810_driver_dma_quiescent,
 	.ioctls = i810_ioctls,
diff --git a/drivers/gpu/drm/i810/i810_drv.h b/drivers/gpu/drm/i810/i810_drv.h
index d4d16eddd651..93ec5dc4e7d3 100644
--- a/drivers/gpu/drm/i810/i810_drv.h
+++ b/drivers/gpu/drm/i810/i810_drv.h
@@ -32,6 +32,8 @@
 #ifndef _I810_DRV_H_
 #define _I810_DRV_H_
 
+#include <drm/drm_legacy.h>
+
 /* General customization:
  */
 
diff --git a/drivers/gpu/drm/i915/Makefile b/drivers/gpu/drm/i915/Makefile
index 91bd167e1cb7..c1dd485aeb6c 100644
--- a/drivers/gpu/drm/i915/Makefile
+++ b/drivers/gpu/drm/i915/Makefile
@@ -31,6 +31,7 @@ i915-y += i915_cmd_parser.o \
 	  i915_gpu_error.o \
 	  i915_irq.o \
 	  i915_trace_points.o \
+	  intel_lrc.o \
 	  intel_ringbuffer.o \
 	  intel_uncore.o
 
diff --git a/drivers/gpu/drm/i915/dvo_ns2501.c b/drivers/gpu/drm/i915/dvo_ns2501.c
index 74f2af7c2d3e..441630434d34 100644
--- a/drivers/gpu/drm/i915/dvo_ns2501.c
+++ b/drivers/gpu/drm/i915/dvo_ns2501.c
@@ -60,16 +60,297 @@
 
 #define NS2501_REGC 0x0c
 
+enum {
+	MODE_640x480,
+	MODE_800x600,
+	MODE_1024x768,
+};
+
+struct ns2501_reg {
+	 uint8_t offset;
+	 uint8_t value;
+};
+
+/*
+ * Magic values based on what the BIOS on
+ * Fujitsu-Siemens Lifebook S6010 programs (1024x768 panel).
+ */
+static const struct ns2501_reg regs_1024x768[][86] = {
+	[MODE_640x480] = {
+		[0] = { .offset = 0x0a, .value = 0x81, },
+		[1] = { .offset = 0x18, .value = 0x07, },
+		[2] = { .offset = 0x19, .value = 0x00, },
+		[3] = { .offset = 0x1a, .value = 0x00, },
+		[4] = { .offset = 0x1b, .value = 0x11, },
+		[5] = { .offset = 0x1c, .value = 0x54, },
+		[6] = { .offset = 0x1d, .value = 0x03, },
+		[7] = { .offset = 0x1e, .value = 0x02, },
+		[8] = { .offset = 0xf3, .value = 0x90, },
+		[9] = { .offset = 0xf9, .value = 0x00, },
+		[10] = { .offset = 0xc1, .value = 0x90, },
+		[11] = { .offset = 0xc2, .value = 0x00, },
+		[12] = { .offset = 0xc3, .value = 0x0f, },
+		[13] = { .offset = 0xc4, .value = 0x03, },
+		[14] = { .offset = 0xc5, .value = 0x16, },
+		[15] = { .offset = 0xc6, .value = 0x00, },
+		[16] = { .offset = 0xc7, .value = 0x02, },
+		[17] = { .offset = 0xc8, .value = 0x02, },
+		[18] = { .offset = 0xf4, .value = 0x00, },
+		[19] = { .offset = 0x80, .value = 0xff, },
+		[20] = { .offset = 0x81, .value = 0x07, },
+		[21] = { .offset = 0x82, .value = 0x3d, },
+		[22] = { .offset = 0x83, .value = 0x05, },
+		[23] = { .offset = 0x94, .value = 0x00, },
+		[24] = { .offset = 0x95, .value = 0x00, },
+		[25] = { .offset = 0x96, .value = 0x05, },
+		[26] = { .offset = 0x97, .value = 0x00, },
+		[27] = { .offset = 0x9a, .value = 0x88, },
+		[28] = { .offset = 0x9b, .value = 0x00, },
+		[29] = { .offset = 0x98, .value = 0x00, },
+		[30] = { .offset = 0x99, .value = 0x00, },
+		[31] = { .offset = 0xf7, .value = 0x88, },
+		[32] = { .offset = 0xf8, .value = 0x0a, },
+		[33] = { .offset = 0x9c, .value = 0x24, },
+		[34] = { .offset = 0x9d, .value = 0x00, },
+		[35] = { .offset = 0x9e, .value = 0x25, },
+		[36] = { .offset = 0x9f, .value = 0x03, },
+		[37] = { .offset = 0xa0, .value = 0x28, },
+		[38] = { .offset = 0xa1, .value = 0x01, },
+		[39] = { .offset = 0xa2, .value = 0x28, },
+		[40] = { .offset = 0xa3, .value = 0x05, },
+		[41] = { .offset = 0xb6, .value = 0x09, },
+		[42] = { .offset = 0xb8, .value = 0x00, },
+		[43] = { .offset = 0xb9, .value = 0xa0, },
+		[44] = { .offset = 0xba, .value = 0x00, },
+		[45] = { .offset = 0xbb, .value = 0x20, },
+		[46] = { .offset = 0x10, .value = 0x00, },
+		[47] = { .offset = 0x11, .value = 0xa0, },
+		[48] = { .offset = 0x12, .value = 0x02, },
+		[49] = { .offset = 0x20, .value = 0x00, },
+		[50] = { .offset = 0x22, .value = 0x00, },
+		[51] = { .offset = 0x23, .value = 0x00, },
+		[52] = { .offset = 0x24, .value = 0x00, },
+		[53] = { .offset = 0x25, .value = 0x00, },
+		[54] = { .offset = 0x8c, .value = 0x10, },
+		[55] = { .offset = 0x8d, .value = 0x02, },
+		[56] = { .offset = 0x8e, .value = 0x10, },
+		[57] = { .offset = 0x8f, .value = 0x00, },
+		[58] = { .offset = 0x90, .value = 0xff, },
+		[59] = { .offset = 0x91, .value = 0x07, },
+		[60] = { .offset = 0x92, .value = 0xa0, },
+		[61] = { .offset = 0x93, .value = 0x02, },
+		[62] = { .offset = 0xa5, .value = 0x00, },
+		[63] = { .offset = 0xa6, .value = 0x00, },
+		[64] = { .offset = 0xa7, .value = 0x00, },
+		[65] = { .offset = 0xa8, .value = 0x00, },
+		[66] = { .offset = 0xa9, .value = 0x04, },
+		[67] = { .offset = 0xaa, .value = 0x70, },
+		[68] = { .offset = 0xab, .value = 0x4f, },
+		[69] = { .offset = 0xac, .value = 0x00, },
+		[70] = { .offset = 0xa4, .value = 0x84, },
+		[71] = { .offset = 0x7e, .value = 0x18, },
+		[72] = { .offset = 0x84, .value = 0x00, },
+		[73] = { .offset = 0x85, .value = 0x00, },
+		[74] = { .offset = 0x86, .value = 0x00, },
+		[75] = { .offset = 0x87, .value = 0x00, },
+		[76] = { .offset = 0x88, .value = 0x00, },
+		[77] = { .offset = 0x89, .value = 0x00, },
+		[78] = { .offset = 0x8a, .value = 0x00, },
+		[79] = { .offset = 0x8b, .value = 0x00, },
+		[80] = { .offset = 0x26, .value = 0x00, },
+		[81] = { .offset = 0x27, .value = 0x00, },
+		[82] = { .offset = 0xad, .value = 0x00, },
+		[83] = { .offset = 0x08, .value = 0x30, }, /* 0x31 */
+		[84] = { .offset = 0x41, .value = 0x00, },
+		[85] = { .offset = 0xc0, .value = 0x05, },
+	},
+	[MODE_800x600] = {
+		[0] = { .offset = 0x0a, .value = 0x81, },
+		[1] = { .offset = 0x18, .value = 0x07, },
+		[2] = { .offset = 0x19, .value = 0x00, },
+		[3] = { .offset = 0x1a, .value = 0x00, },
+		[4] = { .offset = 0x1b, .value = 0x19, },
+		[5] = { .offset = 0x1c, .value = 0x64, },
+		[6] = { .offset = 0x1d, .value = 0x02, },
+		[7] = { .offset = 0x1e, .value = 0x02, },
+		[8] = { .offset = 0xf3, .value = 0x90, },
+		[9] = { .offset = 0xf9, .value = 0x00, },
+		[10] = { .offset = 0xc1, .value = 0xd7, },
+		[11] = { .offset = 0xc2, .value = 0x00, },
+		[12] = { .offset = 0xc3, .value = 0xf8, },
+		[13] = { .offset = 0xc4, .value = 0x03, },
+		[14] = { .offset = 0xc5, .value = 0x1a, },
+		[15] = { .offset = 0xc6, .value = 0x00, },
+		[16] = { .offset = 0xc7, .value = 0x73, },
+		[17] = { .offset = 0xc8, .value = 0x02, },
+		[18] = { .offset = 0xf4, .value = 0x00, },
+		[19] = { .offset = 0x80, .value = 0x27, },
+		[20] = { .offset = 0x81, .value = 0x03, },
+		[21] = { .offset = 0x82, .value = 0x41, },
+		[22] = { .offset = 0x83, .value = 0x05, },
+		[23] = { .offset = 0x94, .value = 0x00, },
+		[24] = { .offset = 0x95, .value = 0x00, },
+		[25] = { .offset = 0x96, .value = 0x05, },
+		[26] = { .offset = 0x97, .value = 0x00, },
+		[27] = { .offset = 0x9a, .value = 0x88, },
+		[28] = { .offset = 0x9b, .value = 0x00, },
+		[29] = { .offset = 0x98, .value = 0x00, },
+		[30] = { .offset = 0x99, .value = 0x00, },
+		[31] = { .offset = 0xf7, .value = 0x88, },
+		[32] = { .offset = 0xf8, .value = 0x06, },
+		[33] = { .offset = 0x9c, .value = 0x23, },
+		[34] = { .offset = 0x9d, .value = 0x00, },
+		[35] = { .offset = 0x9e, .value = 0x25, },
+		[36] = { .offset = 0x9f, .value = 0x03, },
+		[37] = { .offset = 0xa0, .value = 0x28, },
+		[38] = { .offset = 0xa1, .value = 0x01, },
+		[39] = { .offset = 0xa2, .value = 0x28, },
+		[40] = { .offset = 0xa3, .value = 0x05, },
+		[41] = { .offset = 0xb6, .value = 0x09, },
+		[42] = { .offset = 0xb8, .value = 0x30, },
+		[43] = { .offset = 0xb9, .value = 0xc8, },
+		[44] = { .offset = 0xba, .value = 0x00, },
+		[45] = { .offset = 0xbb, .value = 0x20, },
+		[46] = { .offset = 0x10, .value = 0x20, },
+		[47] = { .offset = 0x11, .value = 0xc8, },
+		[48] = { .offset = 0x12, .value = 0x02, },
+		[49] = { .offset = 0x20, .value = 0x00, },
+		[50] = { .offset = 0x22, .value = 0x00, },
+		[51] = { .offset = 0x23, .value = 0x00, },
+		[52] = { .offset = 0x24, .value = 0x00, },
+		[53] = { .offset = 0x25, .value = 0x00, },
+		[54] = { .offset = 0x8c, .value = 0x10, },
+		[55] = { .offset = 0x8d, .value = 0x02, },
+		[56] = { .offset = 0x8e, .value = 0x04, },
+		[57] = { .offset = 0x8f, .value = 0x00, },
+		[58] = { .offset = 0x90, .value = 0xff, },
+		[59] = { .offset = 0x91, .value = 0x07, },
+		[60] = { .offset = 0x92, .value = 0xa0, },
+		[61] = { .offset = 0x93, .value = 0x02, },
+		[62] = { .offset = 0xa5, .value = 0x00, },
+		[63] = { .offset = 0xa6, .value = 0x00, },
+		[64] = { .offset = 0xa7, .value = 0x00, },
+		[65] = { .offset = 0xa8, .value = 0x00, },
+		[66] = { .offset = 0xa9, .value = 0x83, },
+		[67] = { .offset = 0xaa, .value = 0x40, },
+		[68] = { .offset = 0xab, .value = 0x32, },
+		[69] = { .offset = 0xac, .value = 0x00, },
+		[70] = { .offset = 0xa4, .value = 0x80, },
+		[71] = { .offset = 0x7e, .value = 0x18, },
+		[72] = { .offset = 0x84, .value = 0x00, },
+		[73] = { .offset = 0x85, .value = 0x00, },
+		[74] = { .offset = 0x86, .value = 0x00, },
+		[75] = { .offset = 0x87, .value = 0x00, },
+		[76] = { .offset = 0x88, .value = 0x00, },
+		[77] = { .offset = 0x89, .value = 0x00, },
+		[78] = { .offset = 0x8a, .value = 0x00, },
+		[79] = { .offset = 0x8b, .value = 0x00, },
+		[80] = { .offset = 0x26, .value = 0x00, },
+		[81] = { .offset = 0x27, .value = 0x00, },
+		[82] = { .offset = 0xad, .value = 0x00, },
+		[83] = { .offset = 0x08, .value = 0x30, }, /* 0x31 */
+		[84] = { .offset = 0x41, .value = 0x00, },
+		[85] = { .offset = 0xc0, .value = 0x07, },
+	},
+	[MODE_1024x768] = {
+		[0] = { .offset = 0x0a, .value = 0x81, },
+		[1] = { .offset = 0x18, .value = 0x07, },
+		[2] = { .offset = 0x19, .value = 0x00, },
+		[3] = { .offset = 0x1a, .value = 0x00, },
+		[4] = { .offset = 0x1b, .value = 0x11, },
+		[5] = { .offset = 0x1c, .value = 0x54, },
+		[6] = { .offset = 0x1d, .value = 0x03, },
+		[7] = { .offset = 0x1e, .value = 0x02, },
+		[8] = { .offset = 0xf3, .value = 0x90, },
+		[9] = { .offset = 0xf9, .value = 0x00, },
+		[10] = { .offset = 0xc1, .value = 0x90, },
+		[11] = { .offset = 0xc2, .value = 0x00, },
+		[12] = { .offset = 0xc3, .value = 0x0f, },
+		[13] = { .offset = 0xc4, .value = 0x03, },
+		[14] = { .offset = 0xc5, .value = 0x16, },
+		[15] = { .offset = 0xc6, .value = 0x00, },
+		[16] = { .offset = 0xc7, .value = 0x02, },
+		[17] = { .offset = 0xc8, .value = 0x02, },
+		[18] = { .offset = 0xf4, .value = 0x00, },
+		[19] = { .offset = 0x80, .value = 0xff, },
+		[20] = { .offset = 0x81, .value = 0x07, },
+		[21] = { .offset = 0x82, .value = 0x3d, },
+		[22] = { .offset = 0x83, .value = 0x05, },
+		[23] = { .offset = 0x94, .value = 0x00, },
+		[24] = { .offset = 0x95, .value = 0x00, },
+		[25] = { .offset = 0x96, .value = 0x05, },
+		[26] = { .offset = 0x97, .value = 0x00, },
+		[27] = { .offset = 0x9a, .value = 0x88, },
+		[28] = { .offset = 0x9b, .value = 0x00, },
+		[29] = { .offset = 0x98, .value = 0x00, },
+		[30] = { .offset = 0x99, .value = 0x00, },
+		[31] = { .offset = 0xf7, .value = 0x88, },
+		[32] = { .offset = 0xf8, .value = 0x0a, },
+		[33] = { .offset = 0x9c, .value = 0x24, },
+		[34] = { .offset = 0x9d, .value = 0x00, },
+		[35] = { .offset = 0x9e, .value = 0x25, },
+		[36] = { .offset = 0x9f, .value = 0x03, },
+		[37] = { .offset = 0xa0, .value = 0x28, },
+		[38] = { .offset = 0xa1, .value = 0x01, },
+		[39] = { .offset = 0xa2, .value = 0x28, },
+		[40] = { .offset = 0xa3, .value = 0x05, },
+		[41] = { .offset = 0xb6, .value = 0x09, },
+		[42] = { .offset = 0xb8, .value = 0x00, },
+		[43] = { .offset = 0xb9, .value = 0xa0, },
+		[44] = { .offset = 0xba, .value = 0x00, },
+		[45] = { .offset = 0xbb, .value = 0x20, },
+		[46] = { .offset = 0x10, .value = 0x00, },
+		[47] = { .offset = 0x11, .value = 0xa0, },
+		[48] = { .offset = 0x12, .value = 0x02, },
+		[49] = { .offset = 0x20, .value = 0x00, },
+		[50] = { .offset = 0x22, .value = 0x00, },
+		[51] = { .offset = 0x23, .value = 0x00, },
+		[52] = { .offset = 0x24, .value = 0x00, },
+		[53] = { .offset = 0x25, .value = 0x00, },
+		[54] = { .offset = 0x8c, .value = 0x10, },
+		[55] = { .offset = 0x8d, .value = 0x02, },
+		[56] = { .offset = 0x8e, .value = 0x10, },
+		[57] = { .offset = 0x8f, .value = 0x00, },
+		[58] = { .offset = 0x90, .value = 0xff, },
+		[59] = { .offset = 0x91, .value = 0x07, },
+		[60] = { .offset = 0x92, .value = 0xa0, },
+		[61] = { .offset = 0x93, .value = 0x02, },
+		[62] = { .offset = 0xa5, .value = 0x00, },
+		[63] = { .offset = 0xa6, .value = 0x00, },
+		[64] = { .offset = 0xa7, .value = 0x00, },
+		[65] = { .offset = 0xa8, .value = 0x00, },
+		[66] = { .offset = 0xa9, .value = 0x04, },
+		[67] = { .offset = 0xaa, .value = 0x70, },
+		[68] = { .offset = 0xab, .value = 0x4f, },
+		[69] = { .offset = 0xac, .value = 0x00, },
+		[70] = { .offset = 0xa4, .value = 0x84, },
+		[71] = { .offset = 0x7e, .value = 0x18, },
+		[72] = { .offset = 0x84, .value = 0x00, },
+		[73] = { .offset = 0x85, .value = 0x00, },
+		[74] = { .offset = 0x86, .value = 0x00, },
+		[75] = { .offset = 0x87, .value = 0x00, },
+		[76] = { .offset = 0x88, .value = 0x00, },
+		[77] = { .offset = 0x89, .value = 0x00, },
+		[78] = { .offset = 0x8a, .value = 0x00, },
+		[79] = { .offset = 0x8b, .value = 0x00, },
+		[80] = { .offset = 0x26, .value = 0x00, },
+		[81] = { .offset = 0x27, .value = 0x00, },
+		[82] = { .offset = 0xad, .value = 0x00, },
+		[83] = { .offset = 0x08, .value = 0x34, }, /* 0x35 */
+		[84] = { .offset = 0x41, .value = 0x00, },
+		[85] = { .offset = 0xc0, .value = 0x01, },
+	},
+};
+
+static const struct ns2501_reg regs_init[] = {
+	[0] = { .offset = 0x35, .value = 0xff, },
+	[1] = { .offset = 0x34, .value = 0x00, },
+	[2] = { .offset = 0x08, .value = 0x30, },
+};
+
 struct ns2501_priv {
-	//I2CDevRec d;
 	bool quiet;
-	int reg_8_shadow;
-	int reg_8_set;
-	// Shadow registers for i915
-	int dvoc;
-	int pll_a;
-	int srcdim;
-	int fw_blc;
+	const struct ns2501_reg *regs;
 };
 
 #define NSPTR(d) ((NS2501Ptr)(d->DriverPrivate.ptr))
@@ -205,11 +486,9 @@ static bool ns2501_init(struct intel_dvo_device *dvo,
 		goto out;
 	}
 	ns->quiet = false;
-	ns->reg_8_set = 0;
-	ns->reg_8_shadow =
-	    NS2501_8_PD | NS2501_8_BPAS | NS2501_8_VEN | NS2501_8_HEN;
 
 	DRM_DEBUG_KMS("init ns2501 dvo controller successfully!\n");
+
 	return true;
 
 out:
@@ -242,9 +521,9 @@ static enum drm_mode_status ns2501_mode_valid(struct intel_dvo_device *dvo,
 	 * of the panel in here so we could always accept it
 	 * by disabling the scaler.
 	 */
-	if ((mode->hdisplay == 800 && mode->vdisplay == 600) ||
-	    (mode->hdisplay == 640 && mode->vdisplay == 480) ||
-	    (mode->hdisplay == 1024 && mode->vdisplay == 768)) {
+	if ((mode->hdisplay == 640 && mode->vdisplay == 480 && mode->clock == 25175) ||
+	    (mode->hdisplay == 800 && mode->vdisplay == 600 && mode->clock == 40000) ||
+	    (mode->hdisplay == 1024 && mode->vdisplay == 768 && mode->clock == 65000)) {
 		return MODE_OK;
 	} else {
 		return MODE_ONE_SIZE;	/* Is this a reasonable error? */
@@ -255,180 +534,30 @@ static void ns2501_mode_set(struct intel_dvo_device *dvo,
 			    struct drm_display_mode *mode,
 			    struct drm_display_mode *adjusted_mode)
 {
-	bool ok;
-	int retries = 10;
 	struct ns2501_priv *ns = (struct ns2501_priv *)(dvo->dev_priv);
+	int mode_idx, i;
 
 	DRM_DEBUG_KMS
 	    ("set mode (hdisplay=%d,htotal=%d,vdisplay=%d,vtotal=%d).\n",
 	     mode->hdisplay, mode->htotal, mode->vdisplay, mode->vtotal);
 
-	/*
-	 * Where do I find the native resolution for which scaling is not required???
-	 *
-	 * First trigger the DVO on as otherwise the chip does not appear on the i2c
-	 * bus.
-	 */
-	do {
-		ok = true;
-
-		if (mode->hdisplay == 800 && mode->vdisplay == 600) {
-			/* mode 277 */
-			ns->reg_8_shadow &= ~NS2501_8_BPAS;
-			DRM_DEBUG_KMS("switching to 800x600\n");
-
-			/*
-			 * No, I do not know where this data comes from.
-			 * It is just what the video bios left in the DVO, so
-			 * I'm just copying it here over.
-			 * This also means that I cannot support any other modes
-			 * except the ones supported by the bios.
-			 */
-			ok &= ns2501_writeb(dvo, 0x11, 0xc8);	// 0xc7 also works.
-			ok &= ns2501_writeb(dvo, 0x1b, 0x19);
-			ok &= ns2501_writeb(dvo, 0x1c, 0x62);	// VBIOS left 0x64 here, but 0x62 works nicer
-			ok &= ns2501_writeb(dvo, 0x1d, 0x02);
-
-			ok &= ns2501_writeb(dvo, 0x34, 0x03);
-			ok &= ns2501_writeb(dvo, 0x35, 0xff);
+	if (mode->hdisplay == 640 && mode->vdisplay == 480)
+		mode_idx = MODE_640x480;
+	else if (mode->hdisplay == 800 && mode->vdisplay == 600)
+		mode_idx = MODE_800x600;
+	else if (mode->hdisplay == 1024 && mode->vdisplay == 768)
+		mode_idx = MODE_1024x768;
+	else
+		return;
 
-			ok &= ns2501_writeb(dvo, 0x80, 0x27);
-			ok &= ns2501_writeb(dvo, 0x81, 0x03);
-			ok &= ns2501_writeb(dvo, 0x82, 0x41);
-			ok &= ns2501_writeb(dvo, 0x83, 0x05);
+	/* Hopefully doing it every time won't hurt... */
+	for (i = 0; i < ARRAY_SIZE(regs_init); i++)
+		ns2501_writeb(dvo, regs_init[i].offset, regs_init[i].value);
 
-			ok &= ns2501_writeb(dvo, 0x8d, 0x02);
-			ok &= ns2501_writeb(dvo, 0x8e, 0x04);
-			ok &= ns2501_writeb(dvo, 0x8f, 0x00);
+	ns->regs = regs_1024x768[mode_idx];
 
-			ok &= ns2501_writeb(dvo, 0x90, 0xfe);	/* vertical. VBIOS left 0xff here, but 0xfe works better */
-			ok &= ns2501_writeb(dvo, 0x91, 0x07);
-			ok &= ns2501_writeb(dvo, 0x94, 0x00);
-			ok &= ns2501_writeb(dvo, 0x95, 0x00);
-
-			ok &= ns2501_writeb(dvo, 0x96, 0x00);
-
-			ok &= ns2501_writeb(dvo, 0x99, 0x00);
-			ok &= ns2501_writeb(dvo, 0x9a, 0x88);
-
-			ok &= ns2501_writeb(dvo, 0x9c, 0x23);	/* Looks like first and last line of the image. */
-			ok &= ns2501_writeb(dvo, 0x9d, 0x00);
-			ok &= ns2501_writeb(dvo, 0x9e, 0x25);
-			ok &= ns2501_writeb(dvo, 0x9f, 0x03);
-
-			ok &= ns2501_writeb(dvo, 0xa4, 0x80);
-
-			ok &= ns2501_writeb(dvo, 0xb6, 0x00);
-
-			ok &= ns2501_writeb(dvo, 0xb9, 0xc8);	/* horizontal? */
-			ok &= ns2501_writeb(dvo, 0xba, 0x00);	/* horizontal? */
-
-			ok &= ns2501_writeb(dvo, 0xc0, 0x05);	/* horizontal? */
-			ok &= ns2501_writeb(dvo, 0xc1, 0xd7);
-
-			ok &= ns2501_writeb(dvo, 0xc2, 0x00);
-			ok &= ns2501_writeb(dvo, 0xc3, 0xf8);
-
-			ok &= ns2501_writeb(dvo, 0xc4, 0x03);
-			ok &= ns2501_writeb(dvo, 0xc5, 0x1a);
-
-			ok &= ns2501_writeb(dvo, 0xc6, 0x00);
-			ok &= ns2501_writeb(dvo, 0xc7, 0x73);
-			ok &= ns2501_writeb(dvo, 0xc8, 0x02);
-
-		} else if (mode->hdisplay == 640 && mode->vdisplay == 480) {
-			/* mode 274 */
-			DRM_DEBUG_KMS("switching to 640x480\n");
-			/*
-			 * No, I do not know where this data comes from.
-			 * It is just what the video bios left in the DVO, so
-			 * I'm just copying it here over.
-			 * This also means that I cannot support any other modes
-			 * except the ones supported by the bios.
-			 */
-			ns->reg_8_shadow &= ~NS2501_8_BPAS;
-
-			ok &= ns2501_writeb(dvo, 0x11, 0xa0);
-			ok &= ns2501_writeb(dvo, 0x1b, 0x11);
-			ok &= ns2501_writeb(dvo, 0x1c, 0x54);
-			ok &= ns2501_writeb(dvo, 0x1d, 0x03);
-
-			ok &= ns2501_writeb(dvo, 0x34, 0x03);
-			ok &= ns2501_writeb(dvo, 0x35, 0xff);
-
-			ok &= ns2501_writeb(dvo, 0x80, 0xff);
-			ok &= ns2501_writeb(dvo, 0x81, 0x07);
-			ok &= ns2501_writeb(dvo, 0x82, 0x3d);
-			ok &= ns2501_writeb(dvo, 0x83, 0x05);
-
-			ok &= ns2501_writeb(dvo, 0x8d, 0x02);
-			ok &= ns2501_writeb(dvo, 0x8e, 0x10);
-			ok &= ns2501_writeb(dvo, 0x8f, 0x00);
-
-			ok &= ns2501_writeb(dvo, 0x90, 0xff);	/* vertical */
-			ok &= ns2501_writeb(dvo, 0x91, 0x07);
-			ok &= ns2501_writeb(dvo, 0x94, 0x00);
-			ok &= ns2501_writeb(dvo, 0x95, 0x00);
-
-			ok &= ns2501_writeb(dvo, 0x96, 0x05);
-
-			ok &= ns2501_writeb(dvo, 0x99, 0x00);
-			ok &= ns2501_writeb(dvo, 0x9a, 0x88);
-
-			ok &= ns2501_writeb(dvo, 0x9c, 0x24);
-			ok &= ns2501_writeb(dvo, 0x9d, 0x00);
-			ok &= ns2501_writeb(dvo, 0x9e, 0x25);
-			ok &= ns2501_writeb(dvo, 0x9f, 0x03);
-
-			ok &= ns2501_writeb(dvo, 0xa4, 0x84);
-
-			ok &= ns2501_writeb(dvo, 0xb6, 0x09);
-
-			ok &= ns2501_writeb(dvo, 0xb9, 0xa0);	/* horizontal? */
-			ok &= ns2501_writeb(dvo, 0xba, 0x00);	/* horizontal? */
-
-			ok &= ns2501_writeb(dvo, 0xc0, 0x05);	/* horizontal? */
-			ok &= ns2501_writeb(dvo, 0xc1, 0x90);
-
-			ok &= ns2501_writeb(dvo, 0xc2, 0x00);
-			ok &= ns2501_writeb(dvo, 0xc3, 0x0f);
-
-			ok &= ns2501_writeb(dvo, 0xc4, 0x03);
-			ok &= ns2501_writeb(dvo, 0xc5, 0x16);
-
-			ok &= ns2501_writeb(dvo, 0xc6, 0x00);
-			ok &= ns2501_writeb(dvo, 0xc7, 0x02);
-			ok &= ns2501_writeb(dvo, 0xc8, 0x02);
-
-		} else if (mode->hdisplay == 1024 && mode->vdisplay == 768) {
-			/* mode 280 */
-			DRM_DEBUG_KMS("switching to 1024x768\n");
-			/*
-			 * This might or might not work, actually. I'm silently
-			 * assuming here that the native panel resolution is
-			 * 1024x768. If not, then this leaves the scaler disabled
-			 * generating a picture that is likely not the expected.
-			 *
-			 * Problem is that I do not know where to take the panel
-			 * dimensions from.
-			 *
-			 * Enable the bypass, scaling not required.
-			 *
-			 * The scaler registers are irrelevant here....
-			 *
-			 */
-			ns->reg_8_shadow |= NS2501_8_BPAS;
-			ok &= ns2501_writeb(dvo, 0x37, 0x44);
-		} else {
-			/*
-			 * Data not known. Bummer!
-			 * Hopefully, the code should not go here
-			 * as mode_OK delivered no other modes.
-			 */
-			ns->reg_8_shadow |= NS2501_8_BPAS;
-		}
-		ok &= ns2501_writeb(dvo, NS2501_REG8, ns->reg_8_shadow);
-	} while (!ok && retries--);
+	for (i = 0; i < 84; i++)
+		ns2501_writeb(dvo, ns->regs[i].offset, ns->regs[i].value);
 }
 
 /* set the NS2501 power state */
@@ -439,60 +568,46 @@ static bool ns2501_get_hw_state(struct intel_dvo_device *dvo)
 	if (!ns2501_readb(dvo, NS2501_REG8, &ch))
 		return false;
 
-	if (ch & NS2501_8_PD)
-		return true;
-	else
-		return false;
+	return ch & NS2501_8_PD;
 }
 
 /* set the NS2501 power state */
 static void ns2501_dpms(struct intel_dvo_device *dvo, bool enable)
 {
-	bool ok;
-	int retries = 10;
 	struct ns2501_priv *ns = (struct ns2501_priv *)(dvo->dev_priv);
-	unsigned char ch;
 
 	DRM_DEBUG_KMS("Trying set the dpms of the DVO to %i\n", enable);
 
-	ch = ns->reg_8_shadow;
+	if (enable) {
+		if (WARN_ON(ns->regs[83].offset != 0x08 ||
+			    ns->regs[84].offset != 0x41 ||
+			    ns->regs[85].offset != 0xc0))
+			return;
 
-	if (enable)
-		ch |= NS2501_8_PD;
-	else
-		ch &= ~NS2501_8_PD;
-
-	if (ns->reg_8_set == 0 || ns->reg_8_shadow != ch) {
-		ns->reg_8_set = 1;
-		ns->reg_8_shadow = ch;
-
-		do {
-			ok = true;
-			ok &= ns2501_writeb(dvo, NS2501_REG8, ch);
-			ok &=
-			    ns2501_writeb(dvo, 0x34,
-					  enable ? 0x03 : 0x00);
-			ok &=
-			    ns2501_writeb(dvo, 0x35,
-					  enable ? 0xff : 0x00);
-		} while (!ok && retries--);
-	}
-}
+		ns2501_writeb(dvo, 0xc0, ns->regs[85].value | 0x08);
 
-static void ns2501_dump_regs(struct intel_dvo_device *dvo)
-{
-	uint8_t val;
-
-	ns2501_readb(dvo, NS2501_FREQ_LO, &val);
-	DRM_DEBUG_KMS("NS2501_FREQ_LO: 0x%02x\n", val);
-	ns2501_readb(dvo, NS2501_FREQ_HI, &val);
-	DRM_DEBUG_KMS("NS2501_FREQ_HI: 0x%02x\n", val);
-	ns2501_readb(dvo, NS2501_REG8, &val);
-	DRM_DEBUG_KMS("NS2501_REG8: 0x%02x\n", val);
-	ns2501_readb(dvo, NS2501_REG9, &val);
-	DRM_DEBUG_KMS("NS2501_REG9: 0x%02x\n", val);
-	ns2501_readb(dvo, NS2501_REGC, &val);
-	DRM_DEBUG_KMS("NS2501_REGC: 0x%02x\n", val);
+		ns2501_writeb(dvo, 0x41, ns->regs[84].value);
+
+		ns2501_writeb(dvo, 0x34, 0x01);
+		msleep(15);
+
+		ns2501_writeb(dvo, 0x08, 0x35);
+		if (!(ns->regs[83].value & NS2501_8_BPAS))
+			ns2501_writeb(dvo, 0x08, 0x31);
+		msleep(200);
+
+		ns2501_writeb(dvo, 0x34, 0x03);
+
+		ns2501_writeb(dvo, 0xc0, ns->regs[85].value);
+	} else {
+		ns2501_writeb(dvo, 0x34, 0x01);
+		msleep(200);
+
+		ns2501_writeb(dvo, 0x08, 0x34);
+		msleep(15);
+
+		ns2501_writeb(dvo, 0x34, 0x00);
+	}
 }
 
 static void ns2501_destroy(struct intel_dvo_device *dvo)
@@ -512,6 +627,5 @@ struct intel_dvo_dev_ops ns2501_ops = {
 	.mode_set = ns2501_mode_set,
 	.dpms = ns2501_dpms,
 	.get_hw_state = ns2501_get_hw_state,
-	.dump_regs = ns2501_dump_regs,
 	.destroy = ns2501_destroy,
 };
diff --git a/drivers/gpu/drm/i915/i915_cmd_parser.c b/drivers/gpu/drm/i915/i915_cmd_parser.c
index dea99d92fb4a..593b657d3e59 100644
--- a/drivers/gpu/drm/i915/i915_cmd_parser.c
+++ b/drivers/gpu/drm/i915/i915_cmd_parser.c
@@ -709,11 +709,13 @@ int i915_cmd_parser_init_ring(struct intel_engine_cs *ring)
 	BUG_ON(!validate_cmds_sorted(ring, cmd_tables, cmd_table_count));
 	BUG_ON(!validate_regs_sorted(ring));
 
-	ret = init_hash_table(ring, cmd_tables, cmd_table_count);
-	if (ret) {
-		DRM_ERROR("CMD: cmd_parser_init failed!\n");
-		fini_hash_table(ring);
-		return ret;
+	if (hash_empty(ring->cmd_hash)) {
+		ret = init_hash_table(ring, cmd_tables, cmd_table_count);
+		if (ret) {
+			DRM_ERROR("CMD: cmd_parser_init failed!\n");
+			fini_hash_table(ring);
+			return ret;
+		}
 	}
 
 	ring->needs_cmd_parser = true;
@@ -842,8 +844,6 @@ finish:
  */
 bool i915_needs_cmd_parser(struct intel_engine_cs *ring)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
-
 	if (!ring->needs_cmd_parser)
 		return false;
 
@@ -852,7 +852,7 @@ bool i915_needs_cmd_parser(struct intel_engine_cs *ring)
 	 * disabled. That will cause all of the parser's PPGTT checks to
 	 * fail. For now, disable parsing when PPGTT is off.
 	 */
-	if (!dev_priv->mm.aliasing_ppgtt)
+	if (USES_PPGTT(ring->dev))
 		return false;
 
 	return (i915.enable_cmd_parser == 1);
diff --git a/drivers/gpu/drm/i915/i915_debugfs.c b/drivers/gpu/drm/i915/i915_debugfs.c
index 9e737b771c40..063b44817e08 100644
--- a/drivers/gpu/drm/i915/i915_debugfs.c
+++ b/drivers/gpu/drm/i915/i915_debugfs.c
@@ -136,7 +136,7 @@ describe_obj(struct seq_file *m, struct drm_i915_gem_object *obj)
 		   obj->last_read_seqno,
 		   obj->last_write_seqno,
 		   obj->last_fenced_seqno,
-		   i915_cache_level_str(obj->cache_level),
+		   i915_cache_level_str(to_i915(obj->base.dev), obj->cache_level),
 		   obj->dirty ? " dirty" : "",
 		   obj->madv == I915_MADV_DONTNEED ? " purgeable" : "");
 	if (obj->base.name)
@@ -333,7 +333,7 @@ static int per_file_stats(int id, void *ptr, void *data)
 			}
 
 			ppgtt = container_of(vma->vm, struct i915_hw_ppgtt, base);
-			if (ppgtt->ctx && ppgtt->ctx->file_priv != stats->file_priv)
+			if (ppgtt->file_priv != stats->file_priv)
 				continue;
 
 			if (obj->ring) /* XXX per-vma statistic */
@@ -515,6 +515,7 @@ static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 {
 	struct drm_info_node *node = m->private;
 	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	unsigned long flags;
 	struct intel_crtc *crtc;
 	int ret;
@@ -534,6 +535,8 @@ static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 			seq_printf(m, "No flip due on pipe %c (plane %c)\n",
 				   pipe, plane);
 		} else {
+			u32 addr;
+
 			if (atomic_read(&work->pending) < INTEL_FLIP_COMPLETE) {
 				seq_printf(m, "Flip queued on pipe %c (plane %c)\n",
 					   pipe, plane);
@@ -541,23 +544,35 @@ static int i915_gem_pageflip_info(struct seq_file *m, void *data)
 				seq_printf(m, "Flip pending (waiting for vsync) on pipe %c (plane %c)\n",
 					   pipe, plane);
 			}
+			if (work->flip_queued_ring) {
+				seq_printf(m, "Flip queued on %s at seqno %u, next seqno %u [current breadcrumb %u], completed? %d\n",
+					   work->flip_queued_ring->name,
+					   work->flip_queued_seqno,
+					   dev_priv->next_seqno,
+					   work->flip_queued_ring->get_seqno(work->flip_queued_ring, true),
+					   i915_seqno_passed(work->flip_queued_ring->get_seqno(work->flip_queued_ring, true),
+							     work->flip_queued_seqno));
+			} else
+				seq_printf(m, "Flip not associated with any ring\n");
+			seq_printf(m, "Flip queued on frame %d, (was ready on frame %d), now %d\n",
+				   work->flip_queued_vblank,
+				   work->flip_ready_vblank,
+				   drm_vblank_count(dev, crtc->pipe));
 			if (work->enable_stall_check)
 				seq_puts(m, "Stall check enabled, ");
 			else
 				seq_puts(m, "Stall check waiting for page flip ioctl, ");
 			seq_printf(m, "%d prepares\n", atomic_read(&work->pending));
 
-			if (work->old_fb_obj) {
-				struct drm_i915_gem_object *obj = work->old_fb_obj;
-				if (obj)
-					seq_printf(m, "Old framebuffer gtt_offset 0x%08lx\n",
-						   i915_gem_obj_ggtt_offset(obj));
-			}
+			if (INTEL_INFO(dev)->gen >= 4)
+				addr = I915_HI_DISPBASE(I915_READ(DSPSURF(crtc->plane)));
+			else
+				addr = I915_READ(DSPADDR(crtc->plane));
+			seq_printf(m, "Current scanout address 0x%08x\n", addr);
+
 			if (work->pending_flip_obj) {
-				struct drm_i915_gem_object *obj = work->pending_flip_obj;
-				if (obj)
-					seq_printf(m, "New framebuffer gtt_offset 0x%08lx\n",
-						   i915_gem_obj_ggtt_offset(obj));
+				seq_printf(m, "New framebuffer address 0x%08lx\n", (long)work->gtt_offset);
+				seq_printf(m, "MMIO update completed? %d\n",  addr == work->gtt_offset);
 			}
 		}
 		spin_unlock_irqrestore(&dev->event_lock, flags);
@@ -650,7 +665,6 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 	intel_runtime_pm_get(dev_priv);
 
 	if (IS_CHERRYVIEW(dev)) {
-		int i;
 		seq_printf(m, "Master Interrupt Control:\t%08x\n",
 			   I915_READ(GEN8_MASTER_IRQ));
 
@@ -662,7 +676,7 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 			   I915_READ(VLV_IIR_RW));
 		seq_printf(m, "Display IMR:\t%08x\n",
 			   I915_READ(VLV_IMR));
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			seq_printf(m, "Pipe %c stat:\t%08x\n",
 				   pipe_name(pipe),
 				   I915_READ(PIPESTAT(pipe)));
@@ -702,7 +716,13 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 				   i, I915_READ(GEN8_GT_IER(i)));
 		}
 
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
+			if (!intel_display_power_enabled(dev_priv,
+						POWER_DOMAIN_PIPE(pipe))) {
+				seq_printf(m, "Pipe %c power disabled\n",
+					   pipe_name(pipe));
+				continue;
+			}
 			seq_printf(m, "Pipe %c IMR:\t%08x\n",
 				   pipe_name(pipe),
 				   I915_READ(GEN8_DE_PIPE_IMR(pipe)));
@@ -743,7 +763,7 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 			   I915_READ(VLV_IIR_RW));
 		seq_printf(m, "Display IMR:\t%08x\n",
 			   I915_READ(VLV_IMR));
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			seq_printf(m, "Pipe %c stat:\t%08x\n",
 				   pipe_name(pipe),
 				   I915_READ(PIPESTAT(pipe)));
@@ -779,7 +799,7 @@ static int i915_interrupt_info(struct seq_file *m, void *data)
 			   I915_READ(IIR));
 		seq_printf(m, "Interrupt mask:      %08x\n",
 			   I915_READ(IMR));
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			seq_printf(m, "Pipe %c stat:         %08x\n",
 				   pipe_name(pipe),
 				   I915_READ(PIPESTAT(pipe)));
@@ -927,7 +947,7 @@ static ssize_t i915_error_state_read(struct file *file, char __user *userbuf,
 	ssize_t ret_count = 0;
 	int ret;
 
-	ret = i915_error_state_buf_init(&error_str, count, *pos);
+	ret = i915_error_state_buf_init(&error_str, to_i915(error_priv->dev), count, *pos);
 	if (ret)
 		return ret;
 
@@ -1024,6 +1044,7 @@ static int i915_frequency_info(struct seq_file *m, void *unused)
 		u32 rpstat, cagf, reqf;
 		u32 rpupei, rpcurup, rpprevup;
 		u32 rpdownei, rpcurdown, rpprevdown;
+		u32 pm_ier, pm_imr, pm_isr, pm_iir, pm_mask;
 		int max_freq;
 
 		/* RPSTAT1 is in the GT power well */
@@ -1061,12 +1082,21 @@ static int i915_frequency_info(struct seq_file *m, void *unused)
 		gen6_gt_force_wake_put(dev_priv, FORCEWAKE_ALL);
 		mutex_unlock(&dev->struct_mutex);
 
+		if (IS_GEN6(dev) || IS_GEN7(dev)) {
+			pm_ier = I915_READ(GEN6_PMIER);
+			pm_imr = I915_READ(GEN6_PMIMR);
+			pm_isr = I915_READ(GEN6_PMISR);
+			pm_iir = I915_READ(GEN6_PMIIR);
+			pm_mask = I915_READ(GEN6_PMINTRMSK);
+		} else {
+			pm_ier = I915_READ(GEN8_GT_IER(2));
+			pm_imr = I915_READ(GEN8_GT_IMR(2));
+			pm_isr = I915_READ(GEN8_GT_ISR(2));
+			pm_iir = I915_READ(GEN8_GT_IIR(2));
+			pm_mask = I915_READ(GEN6_PMINTRMSK);
+		}
 		seq_printf(m, "PM IER=0x%08x IMR=0x%08x ISR=0x%08x IIR=0x%08x, MASK=0x%08x\n",
-			   I915_READ(GEN6_PMIER),
-			   I915_READ(GEN6_PMIMR),
-			   I915_READ(GEN6_PMISR),
-			   I915_READ(GEN6_PMIIR),
-			   I915_READ(GEN6_PMINTRMSK));
+			   pm_ier, pm_imr, pm_isr, pm_iir, pm_mask);
 		seq_printf(m, "GT_PERF_STATUS: 0x%08x\n", gt_perf_status);
 		seq_printf(m, "Render p-state ratio: %d\n",
 			   (gt_perf_status & 0xff00) >> 8);
@@ -1365,7 +1395,7 @@ static int i915_drpc_info(struct seq_file *m, void *unused)
 
 	if (IS_VALLEYVIEW(dev))
 		return vlv_drpc_info(m);
-	else if (IS_GEN6(dev) || IS_GEN7(dev))
+	else if (INTEL_INFO(dev)->gen >= 6)
 		return gen6_drpc_info(m);
 	else
 		return ironlake_drpc_info(m);
@@ -1433,6 +1463,47 @@ static int i915_fbc_status(struct seq_file *m, void *unused)
 	return 0;
 }
 
+static int i915_fbc_fc_get(void *data, u64 *val)
+{
+	struct drm_device *dev = data;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (INTEL_INFO(dev)->gen < 7 || !HAS_FBC(dev))
+		return -ENODEV;
+
+	drm_modeset_lock_all(dev);
+	*val = dev_priv->fbc.false_color;
+	drm_modeset_unlock_all(dev);
+
+	return 0;
+}
+
+static int i915_fbc_fc_set(void *data, u64 val)
+{
+	struct drm_device *dev = data;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 reg;
+
+	if (INTEL_INFO(dev)->gen < 7 || !HAS_FBC(dev))
+		return -ENODEV;
+
+	drm_modeset_lock_all(dev);
+
+	reg = I915_READ(ILK_DPFC_CONTROL);
+	dev_priv->fbc.false_color = val;
+
+	I915_WRITE(ILK_DPFC_CONTROL, val ?
+		   (reg | FBC_CTL_FALSE_COLOR) :
+		   (reg & ~FBC_CTL_FALSE_COLOR));
+
+	drm_modeset_unlock_all(dev);
+	return 0;
+}
+
+DEFINE_SIMPLE_ATTRIBUTE(i915_fbc_fc_fops,
+			i915_fbc_fc_get, i915_fbc_fc_set,
+			"%llu\n");
+
 static int i915_ips_status(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = m->private;
@@ -1630,6 +1701,14 @@ static int i915_gem_framebuffer_info(struct seq_file *m, void *data)
 	return 0;
 }
 
+static void describe_ctx_ringbuf(struct seq_file *m,
+				 struct intel_ringbuffer *ringbuf)
+{
+	seq_printf(m, " (ringbuffer, space: %d, head: %u, tail: %u, last head: %d)",
+		   ringbuf->space, ringbuf->head, ringbuf->tail,
+		   ringbuf->last_retired_head);
+}
+
 static int i915_context_status(struct seq_file *m, void *unused)
 {
 	struct drm_info_node *node = m->private;
@@ -1656,16 +1735,168 @@ static int i915_context_status(struct seq_file *m, void *unused)
 	}
 
 	list_for_each_entry(ctx, &dev_priv->context_list, link) {
-		if (ctx->legacy_hw_ctx.rcs_state == NULL)
+		if (!i915.enable_execlists &&
+		    ctx->legacy_hw_ctx.rcs_state == NULL)
 			continue;
 
 		seq_puts(m, "HW context ");
 		describe_ctx(m, ctx);
-		for_each_ring(ring, dev_priv, i)
+		for_each_ring(ring, dev_priv, i) {
+			if (ring->default_context == ctx)
+				seq_printf(m, "(default context %s) ",
+					   ring->name);
+		}
+
+		if (i915.enable_execlists) {
+			seq_putc(m, '\n');
+			for_each_ring(ring, dev_priv, i) {
+				struct drm_i915_gem_object *ctx_obj =
+					ctx->engine[i].state;
+				struct intel_ringbuffer *ringbuf =
+					ctx->engine[i].ringbuf;
+
+				seq_printf(m, "%s: ", ring->name);
+				if (ctx_obj)
+					describe_obj(m, ctx_obj);
+				if (ringbuf)
+					describe_ctx_ringbuf(m, ringbuf);
+				seq_putc(m, '\n');
+			}
+		} else {
+			describe_obj(m, ctx->legacy_hw_ctx.rcs_state);
+		}
+
+		seq_putc(m, '\n');
+	}
+
+	mutex_unlock(&dev->struct_mutex);
+
+	return 0;
+}
+
+static int i915_dump_lrc(struct seq_file *m, void *unused)
+{
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring;
+	struct intel_context *ctx;
+	int ret, i;
+
+	if (!i915.enable_execlists) {
+		seq_printf(m, "Logical Ring Contexts are disabled\n");
+		return 0;
+	}
+
+	ret = mutex_lock_interruptible(&dev->struct_mutex);
+	if (ret)
+		return ret;
+
+	list_for_each_entry(ctx, &dev_priv->context_list, link) {
+		for_each_ring(ring, dev_priv, i) {
+			struct drm_i915_gem_object *ctx_obj = ctx->engine[i].state;
+
 			if (ring->default_context == ctx)
-				seq_printf(m, "(default context %s) ", ring->name);
+				continue;
+
+			if (ctx_obj) {
+				struct page *page = i915_gem_object_get_page(ctx_obj, 1);
+				uint32_t *reg_state = kmap_atomic(page);
+				int j;
+
+				seq_printf(m, "CONTEXT: %s %u\n", ring->name,
+						intel_execlists_ctx_id(ctx_obj));
+
+				for (j = 0; j < 0x600 / sizeof(u32) / 4; j += 4) {
+					seq_printf(m, "\t[0x%08lx] 0x%08x 0x%08x 0x%08x 0x%08x\n",
+					i915_gem_obj_ggtt_offset(ctx_obj) + 4096 + (j * 4),
+					reg_state[j], reg_state[j + 1],
+					reg_state[j + 2], reg_state[j + 3]);
+				}
+				kunmap_atomic(reg_state);
+
+				seq_putc(m, '\n');
+			}
+		}
+	}
+
+	mutex_unlock(&dev->struct_mutex);
+
+	return 0;
+}
+
+static int i915_execlists(struct seq_file *m, void *data)
+{
+	struct drm_info_node *node = (struct drm_info_node *)m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring;
+	u32 status_pointer;
+	u8 read_pointer;
+	u8 write_pointer;
+	u32 status;
+	u32 ctx_id;
+	struct list_head *cursor;
+	int ring_id, i;
+	int ret;
+
+	if (!i915.enable_execlists) {
+		seq_puts(m, "Logical Ring Contexts are disabled\n");
+		return 0;
+	}
+
+	ret = mutex_lock_interruptible(&dev->struct_mutex);
+	if (ret)
+		return ret;
+
+	for_each_ring(ring, dev_priv, ring_id) {
+		struct intel_ctx_submit_request *head_req = NULL;
+		int count = 0;
+		unsigned long flags;
+
+		seq_printf(m, "%s\n", ring->name);
+
+		status = I915_READ(RING_EXECLIST_STATUS(ring));
+		ctx_id = I915_READ(RING_EXECLIST_STATUS(ring) + 4);
+		seq_printf(m, "\tExeclist status: 0x%08X, context: %u\n",
+			   status, ctx_id);
+
+		status_pointer = I915_READ(RING_CONTEXT_STATUS_PTR(ring));
+		seq_printf(m, "\tStatus pointer: 0x%08X\n", status_pointer);
+
+		read_pointer = ring->next_context_status_buffer;
+		write_pointer = status_pointer & 0x07;
+		if (read_pointer > write_pointer)
+			write_pointer += 6;
+		seq_printf(m, "\tRead pointer: 0x%08X, write pointer 0x%08X\n",
+			   read_pointer, write_pointer);
+
+		for (i = 0; i < 6; i++) {
+			status = I915_READ(RING_CONTEXT_STATUS_BUF(ring) + 8*i);
+			ctx_id = I915_READ(RING_CONTEXT_STATUS_BUF(ring) + 8*i + 4);
+
+			seq_printf(m, "\tStatus buffer %d: 0x%08X, context: %u\n",
+				   i, status, ctx_id);
+		}
+
+		spin_lock_irqsave(&ring->execlist_lock, flags);
+		list_for_each(cursor, &ring->execlist_queue)
+			count++;
+		head_req = list_first_entry_or_null(&ring->execlist_queue,
+				struct intel_ctx_submit_request, execlist_link);
+		spin_unlock_irqrestore(&ring->execlist_lock, flags);
+
+		seq_printf(m, "\t%d requests in queue\n", count);
+		if (head_req) {
+			struct drm_i915_gem_object *ctx_obj;
+
+			ctx_obj = head_req->ctx->engine[ring_id].state;
+			seq_printf(m, "\tHead request id: %u\n",
+				   intel_execlists_ctx_id(ctx_obj));
+			seq_printf(m, "\tHead request tail: %u\n",
+				   head_req->tail);
+		}
 
-		describe_obj(m, ctx->legacy_hw_ctx.rcs_state);
 		seq_putc(m, '\n');
 	}
 
@@ -1774,7 +2005,13 @@ static int per_file_ctx(int id, void *ptr, void *data)
 {
 	struct intel_context *ctx = ptr;
 	struct seq_file *m = data;
-	struct i915_hw_ppgtt *ppgtt = ctx_to_ppgtt(ctx);
+	struct i915_hw_ppgtt *ppgtt = ctx->ppgtt;
+
+	if (!ppgtt) {
+		seq_printf(m, "  no ppgtt for context %d\n",
+			   ctx->user_handle);
+		return 0;
+	}
 
 	if (i915_gem_context_is_default(ctx))
 		seq_puts(m, "  default context:\n");
@@ -1834,8 +2071,7 @@ static void gen6_ppgtt_info(struct seq_file *m, struct drm_device *dev)
 		seq_printf(m, "pd gtt offset: 0x%08x\n", ppgtt->pd_offset);
 
 		ppgtt->debug_dump(ppgtt, m);
-	} else
-		return;
+	}
 
 	list_for_each_entry_reverse(file, &dev->filelist, lhead) {
 		struct drm_i915_file_private *file_priv = file->driver_priv;
@@ -2406,6 +2642,40 @@ static int i915_shared_dplls_info(struct seq_file *m, void *unused)
 	return 0;
 }
 
+static int i915_wa_registers(struct seq_file *m, void *unused)
+{
+	int i;
+	int ret;
+	struct drm_info_node *node = (struct drm_info_node *) m->private;
+	struct drm_device *dev = node->minor->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	ret = mutex_lock_interruptible(&dev->struct_mutex);
+	if (ret)
+		return ret;
+
+	intel_runtime_pm_get(dev_priv);
+
+	seq_printf(m, "Workarounds applied: %d\n", dev_priv->num_wa_regs);
+	for (i = 0; i < dev_priv->num_wa_regs; ++i) {
+		u32 addr, mask;
+
+		addr = dev_priv->intel_wa_regs[i].addr;
+		mask = dev_priv->intel_wa_regs[i].mask;
+		dev_priv->intel_wa_regs[i].value = I915_READ(addr) | mask;
+		if (dev_priv->intel_wa_regs[i].addr)
+			seq_printf(m, "0x%X: 0x%08X, mask: 0x%08X\n",
+				   dev_priv->intel_wa_regs[i].addr,
+				   dev_priv->intel_wa_regs[i].value,
+				   dev_priv->intel_wa_regs[i].mask);
+	}
+
+	intel_runtime_pm_put(dev_priv);
+	mutex_unlock(&dev->struct_mutex);
+
+	return 0;
+}
+
 struct pipe_crc_info {
 	const char *name;
 	struct drm_device *dev;
@@ -2667,8 +2937,7 @@ static int i9xx_pipe_crc_auto_source(struct drm_device *dev, enum pipe pipe,
 	*source = INTEL_PIPE_CRC_SOURCE_PIPE;
 
 	drm_modeset_lock_all(dev);
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		if (!encoder->base.crtc)
 			continue;
 
@@ -3557,9 +3826,6 @@ i915_drop_caches_set(void *data, u64 val)
 {
 	struct drm_device *dev = data;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_i915_gem_object *obj, *next;
-	struct i915_address_space *vm;
-	struct i915_vma *vma, *x;
 	int ret;
 
 	DRM_DEBUG("Dropping caches: 0x%08llx\n", val);
@@ -3579,29 +3845,11 @@ i915_drop_caches_set(void *data, u64 val)
 	if (val & (DROP_RETIRE | DROP_ACTIVE))
 		i915_gem_retire_requests(dev);
 
-	if (val & DROP_BOUND) {
-		list_for_each_entry(vm, &dev_priv->vm_list, global_link) {
-			list_for_each_entry_safe(vma, x, &vm->inactive_list,
-						 mm_list) {
-				if (vma->pin_count)
-					continue;
+	if (val & DROP_BOUND)
+		i915_gem_shrink(dev_priv, LONG_MAX, I915_SHRINK_BOUND);
 
-				ret = i915_vma_unbind(vma);
-				if (ret)
-					goto unlock;
-			}
-		}
-	}
-
-	if (val & DROP_UNBOUND) {
-		list_for_each_entry_safe(obj, next, &dev_priv->mm.unbound_list,
-					 global_list)
-			if (obj->pages_pin_count == 0) {
-				ret = i915_gem_object_put_pages(obj);
-				if (ret)
-					goto unlock;
-			}
-	}
+	if (val & DROP_UNBOUND)
+		i915_gem_shrink(dev_priv, LONG_MAX, I915_SHRINK_UNBOUND);
 
 unlock:
 	mutex_unlock(&dev->struct_mutex);
@@ -3923,6 +4171,8 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_opregion", i915_opregion, 0},
 	{"i915_gem_framebuffer", i915_gem_framebuffer_info, 0},
 	{"i915_context_status", i915_context_status, 0},
+	{"i915_dump_lrc", i915_dump_lrc, 0},
+	{"i915_execlists", i915_execlists, 0},
 	{"i915_gen6_forcewake_count", i915_gen6_forcewake_count_info, 0},
 	{"i915_swizzle_info", i915_swizzle_info, 0},
 	{"i915_ppgtt_info", i915_ppgtt_info, 0},
@@ -3936,6 +4186,7 @@ static const struct drm_info_list i915_debugfs_list[] = {
 	{"i915_semaphore_status", i915_semaphore_status, 0},
 	{"i915_shared_dplls_info", i915_shared_dplls_info, 0},
 	{"i915_dp_mst_info", i915_dp_mst_info, 0},
+	{"i915_wa_registers", i915_wa_registers, 0},
 };
 #define I915_DEBUGFS_ENTRIES ARRAY_SIZE(i915_debugfs_list)
 
@@ -3957,6 +4208,7 @@ static const struct i915_debugfs_files {
 	{"i915_pri_wm_latency", &i915_pri_wm_latency_fops},
 	{"i915_spr_wm_latency", &i915_spr_wm_latency_fops},
 	{"i915_cur_wm_latency", &i915_cur_wm_latency_fops},
+	{"i915_fbc_false_color", &i915_fbc_fc_fops},
 };
 
 void intel_display_crc_init(struct drm_device *dev)
@@ -3964,7 +4216,7 @@ void intel_display_crc_init(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum pipe pipe;
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		struct intel_pipe_crc *pipe_crc = &dev_priv->pipe_crc[pipe];
 
 		pipe_crc->opened = false;
diff --git a/drivers/gpu/drm/i915/i915_dma.c b/drivers/gpu/drm/i915/i915_dma.c
index 2e7f03ad5ee2..1403b01e8216 100644
--- a/drivers/gpu/drm/i915/i915_dma.c
+++ b/drivers/gpu/drm/i915/i915_dma.c
@@ -28,9 +28,11 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/async.h>
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_helper.h>
+#include <drm/drm_legacy.h>
 #include "intel_drv.h"
 #include <drm/i915_drm.h>
 #include "i915_drv.h"
@@ -196,7 +198,7 @@ static int i915_initialize(struct drm_device *dev, drm_i915_init_t *init)
 	struct drm_i915_master_private *master_priv = dev->primary->master->driver_priv;
 	int ret;
 
-	master_priv->sarea = drm_getsarea(dev);
+	master_priv->sarea = drm_legacy_getsarea(dev);
 	if (master_priv->sarea) {
 		master_priv->sarea_priv = (drm_i915_sarea_t *)
 			((u8 *)master_priv->sarea->handle + init->sarea_priv_offset);
@@ -999,7 +1001,7 @@ static int i915_getparam(struct drm_device *dev, void *data,
 		value = HAS_WT(dev);
 		break;
 	case I915_PARAM_HAS_ALIASING_PPGTT:
-		value = dev_priv->mm.aliasing_ppgtt || USES_FULL_PPGTT(dev);
+		value = USES_PPGTT(dev);
 		break;
 	case I915_PARAM_HAS_WAIT_TIMEOUT:
 		value = 1;
@@ -1336,12 +1338,17 @@ static int i915_load_modeset_init(struct drm_device *dev)
 
 	intel_power_domains_init_hw(dev_priv);
 
+	/*
+	 * We enable some interrupt sources in our postinstall hooks, so mark
+	 * interrupts as enabled _before_ actually enabling them to avoid
+	 * special cases in our ordering checks.
+	 */
+	dev_priv->pm._irqs_disabled = false;
+
 	ret = drm_irq_install(dev, dev->pdev->irq);
 	if (ret)
 		goto cleanup_gem_stolen;
 
-	dev_priv->pm._irqs_disabled = false;
-
 	/* Important: The output setup functions called by modeset_init need
 	 * working irqs for e.g. gmbus and dp aux transfers. */
 	intel_modeset_init(dev);
@@ -1350,8 +1357,6 @@ static int i915_load_modeset_init(struct drm_device *dev)
 	if (ret)
 		goto cleanup_irq;
 
-	INIT_WORK(&dev_priv->console_resume_work, intel_console_resume);
-
 	intel_modeset_gem_init(dev);
 
 	/* Always safe in the mode setting case. */
@@ -1377,7 +1382,7 @@ static int i915_load_modeset_init(struct drm_device *dev)
 	 * scanning against hotplug events. Hence do this first and ignore the
 	 * tiny window where we will loose hotplug notifactions.
 	 */
-	intel_fbdev_initial_config(dev);
+	async_schedule(intel_fbdev_initial_config, dev_priv);
 
 	drm_kms_helper_poll_init(dev);
 
@@ -1388,7 +1393,6 @@ cleanup_gem:
 	i915_gem_cleanup_ringbuffer(dev);
 	i915_gem_context_fini(dev);
 	mutex_unlock(&dev->struct_mutex);
-	WARN_ON(dev_priv->mm.aliasing_ppgtt);
 cleanup_irq:
 	drm_irq_uninstall(dev);
 cleanup_gem_stolen:
@@ -1531,10 +1535,10 @@ static void intel_device_info_runtime_init(struct drm_device *dev)
 	info = (struct intel_device_info *)&dev_priv->info;
 
 	if (IS_VALLEYVIEW(dev))
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			info->num_sprites[pipe] = 2;
 	else
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			info->num_sprites[pipe] = 1;
 
 	if (i915.disable_display) {
@@ -1603,9 +1607,10 @@ int i915_driver_load(struct drm_device *dev, unsigned long flags)
 	dev->dev_private = dev_priv;
 	dev_priv->dev = dev;
 
-	/* copy initial configuration to dev_priv->info */
+	/* Setup the write-once "constant" device info */
 	device_info = (struct intel_device_info *)&dev_priv->info;
-	*device_info = *info;
+	memcpy(device_info, info, sizeof(dev_priv->info));
+	device_info->device_id = dev->pdev->device;
 
 	spin_lock_init(&dev_priv->irq_lock);
 	spin_lock_init(&dev_priv->gpu_error.lock);
@@ -1817,7 +1822,7 @@ out_mtrrfree:
 	arch_phys_wc_del(dev_priv->gtt.mtrr);
 	io_mapping_free(dev_priv->gtt.mappable);
 out_gtt:
-	dev_priv->gtt.base.cleanup(&dev_priv->gtt.base);
+	i915_global_gtt_cleanup(dev);
 out_regs:
 	intel_uncore_fini(dev);
 	pci_iounmap(dev->pdev, dev_priv->regs);
@@ -1864,7 +1869,6 @@ int i915_driver_unload(struct drm_device *dev)
 	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
 		intel_fbdev_fini(dev);
 		intel_modeset_cleanup(dev);
-		cancel_work_sync(&dev_priv->console_resume_work);
 
 		/*
 		 * free the memory space allocated for the child device
@@ -1897,7 +1901,6 @@ int i915_driver_unload(struct drm_device *dev)
 		mutex_lock(&dev->struct_mutex);
 		i915_gem_cleanup_ringbuffer(dev);
 		i915_gem_context_fini(dev);
-		WARN_ON(dev_priv->mm.aliasing_ppgtt);
 		mutex_unlock(&dev->struct_mutex);
 		i915_gem_cleanup_stolen(dev);
 
@@ -1905,8 +1908,6 @@ int i915_driver_unload(struct drm_device *dev)
 			i915_free_hws(dev);
 	}
 
-	WARN_ON(!list_empty(&dev_priv->vm_list));
-
 	drm_vblank_cleanup(dev);
 
 	intel_teardown_gmbus(dev);
@@ -1916,7 +1917,7 @@ int i915_driver_unload(struct drm_device *dev)
 	destroy_workqueue(dev_priv->wq);
 	pm_qos_remove_request(&dev_priv->pm_qos);
 
-	dev_priv->gtt.base.cleanup(&dev_priv->gtt.base);
+	i915_global_gtt_cleanup(dev);
 
 	intel_uncore_fini(dev);
 	if (dev_priv->regs != NULL)
@@ -1981,6 +1982,9 @@ void i915_driver_preclose(struct drm_device *dev, struct drm_file *file)
 	i915_gem_context_close(dev, file);
 	i915_gem_release(dev, file);
 	mutex_unlock(&dev->struct_mutex);
+
+	if (drm_core_check_feature(dev, DRIVER_MODESET))
+		intel_modeset_preclose(dev, file);
 }
 
 void i915_driver_postclose(struct drm_device *dev, struct drm_file *file)
diff --git a/drivers/gpu/drm/i915/i915_drv.c b/drivers/gpu/drm/i915/i915_drv.c
index ec96f9a9724c..055d5e7fbf12 100644
--- a/drivers/gpu/drm/i915/i915_drv.c
+++ b/drivers/gpu/drm/i915/i915_drv.c
@@ -481,6 +481,10 @@ bool i915_semaphore_is_enabled(struct drm_device *dev)
 	if (i915.semaphores >= 0)
 		return i915.semaphores;
 
+	/* TODO: make semaphores and Execlists play nicely together */
+	if (i915.enable_execlists)
+		return false;
+
 	/* Until we get further testing... */
 	if (IS_GEN8(dev))
 		return false;
@@ -494,6 +498,40 @@ bool i915_semaphore_is_enabled(struct drm_device *dev)
 	return true;
 }
 
+void intel_hpd_cancel_work(struct drm_i915_private *dev_priv)
+{
+	spin_lock_irq(&dev_priv->irq_lock);
+
+	dev_priv->long_hpd_port_mask = 0;
+	dev_priv->short_hpd_port_mask = 0;
+	dev_priv->hpd_event_bits = 0;
+
+	spin_unlock_irq(&dev_priv->irq_lock);
+
+	cancel_work_sync(&dev_priv->dig_port_work);
+	cancel_work_sync(&dev_priv->hotplug_work);
+	cancel_delayed_work_sync(&dev_priv->hotplug_reenable_work);
+}
+
+static void intel_suspend_encoders(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct drm_encoder *encoder;
+
+	drm_modeset_lock_all(dev);
+	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
+		struct intel_encoder *intel_encoder = to_intel_encoder(encoder);
+
+		if (intel_encoder->suspend)
+			intel_encoder->suspend(intel_encoder);
+	}
+	drm_modeset_unlock_all(dev);
+}
+
+static int intel_suspend_complete(struct drm_i915_private *dev_priv);
+static int intel_resume_prepare(struct drm_i915_private *dev_priv,
+				bool rpm_resume);
+
 static int i915_drm_freeze(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -538,6 +576,9 @@ static int i915_drm_freeze(struct drm_device *dev)
 		flush_delayed_work(&dev_priv->rps.delayed_resume_work);
 
 		intel_runtime_pm_disable_interrupts(dev);
+		intel_hpd_cancel_work(dev_priv);
+
+		intel_suspend_encoders(dev_priv);
 
 		intel_suspend_gt_powersave(dev);
 
@@ -558,9 +599,7 @@ static int i915_drm_freeze(struct drm_device *dev)
 	intel_uncore_forcewake_reset(dev, false);
 	intel_opregion_fini(dev);
 
-	console_lock();
-	intel_fbdev_set_suspend(dev, FBINFO_STATE_SUSPENDED);
-	console_unlock();
+	intel_fbdev_set_suspend(dev, FBINFO_STATE_SUSPENDED, true);
 
 	dev_priv->suspend_count++;
 
@@ -599,30 +638,20 @@ int i915_suspend(struct drm_device *dev, pm_message_t state)
 	return 0;
 }
 
-void intel_console_resume(struct work_struct *work)
-{
-	struct drm_i915_private *dev_priv =
-		container_of(work, struct drm_i915_private,
-			     console_resume_work);
-	struct drm_device *dev = dev_priv->dev;
-
-	console_lock();
-	intel_fbdev_set_suspend(dev, FBINFO_STATE_RUNNING);
-	console_unlock();
-}
-
 static int i915_drm_thaw_early(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret;
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
-		hsw_disable_pc8(dev_priv);
+	ret = intel_resume_prepare(dev_priv, false);
+	if (ret)
+		DRM_ERROR("Resume prepare failed: %d,Continuing resume\n", ret);
 
 	intel_uncore_early_sanitize(dev, true);
 	intel_uncore_sanitize(dev);
 	intel_power_domains_init_hw(dev_priv);
 
-	return 0;
+	return ret;
 }
 
 static int __i915_drm_thaw(struct drm_device *dev, bool restore_gtt_mappings)
@@ -681,17 +710,7 @@ static int __i915_drm_thaw(struct drm_device *dev, bool restore_gtt_mappings)
 
 	intel_opregion_init(dev);
 
-	/*
-	 * The console lock can be pretty contented on resume due
-	 * to all the printk activity.  Try to keep it out of the hot
-	 * path of resume if possible.
-	 */
-	if (console_trylock()) {
-		intel_fbdev_set_suspend(dev, FBINFO_STATE_RUNNING);
-		console_unlock();
-	} else {
-		schedule_work(&dev_priv->console_resume_work);
-	}
+	intel_fbdev_set_suspend(dev, FBINFO_STATE_RUNNING, false);
 
 	mutex_lock(&dev_priv->modeset_restore_lock);
 	dev_priv->modeset_restore = MODESET_DONE;
@@ -825,7 +844,13 @@ int i915_reset(struct drm_device *dev)
 			!dev_priv->ums.mm_suspended) {
 		dev_priv->ums.mm_suspended = 0;
 
+		/* Used to prevent gem_check_wedged returning -EAGAIN during gpu reset */
+		dev_priv->gpu_error.reload_in_reset = true;
+
 		ret = i915_gem_init_hw(dev);
+
+		dev_priv->gpu_error.reload_in_reset = false;
+
 		mutex_unlock(&dev->struct_mutex);
 		if (ret) {
 			DRM_ERROR("Failed hw init on reset %d\n", ret);
@@ -846,8 +871,6 @@ int i915_reset(struct drm_device *dev)
 		 */
 		if (INTEL_INFO(dev)->gen > 5)
 			intel_reset_gt_powersave(dev);
-
-		intel_hpd_init(dev);
 	} else {
 		mutex_unlock(&dev->struct_mutex);
 	}
@@ -908,6 +931,7 @@ static int i915_pm_suspend_late(struct device *dev)
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
 	struct drm_i915_private *dev_priv = drm_dev->dev_private;
+	int ret;
 
 	/*
 	 * We have a suspedn ordering issue with the snd-hda driver also
@@ -921,13 +945,16 @@ static int i915_pm_suspend_late(struct device *dev)
 	if (drm_dev->switch_power_state == DRM_SWITCH_POWER_OFF)
 		return 0;
 
-	if (IS_HASWELL(drm_dev) || IS_BROADWELL(drm_dev))
-		hsw_enable_pc8(dev_priv);
+	ret = intel_suspend_complete(dev_priv);
 
-	pci_disable_device(pdev);
-	pci_set_power_state(pdev, PCI_D3hot);
+	if (ret)
+		DRM_ERROR("Suspend complete failed: %d\n", ret);
+	else {
+		pci_disable_device(pdev);
+		pci_set_power_state(pdev, PCI_D3hot);
+	}
 
-	return 0;
+	return ret;
 }
 
 static int i915_pm_resume_early(struct device *dev)
@@ -983,23 +1010,26 @@ static int i915_pm_poweroff(struct device *dev)
 	return i915_drm_freeze(drm_dev);
 }
 
-static int hsw_runtime_suspend(struct drm_i915_private *dev_priv)
+static int hsw_suspend_complete(struct drm_i915_private *dev_priv)
 {
 	hsw_enable_pc8(dev_priv);
 
 	return 0;
 }
 
-static int snb_runtime_resume(struct drm_i915_private *dev_priv)
+static int snb_resume_prepare(struct drm_i915_private *dev_priv,
+				bool rpm_resume)
 {
 	struct drm_device *dev = dev_priv->dev;
 
-	intel_init_pch_refclk(dev);
+	if (rpm_resume)
+		intel_init_pch_refclk(dev);
 
 	return 0;
 }
 
-static int hsw_runtime_resume(struct drm_i915_private *dev_priv)
+static int hsw_resume_prepare(struct drm_i915_private *dev_priv,
+				bool rpm_resume)
 {
 	hsw_disable_pc8(dev_priv);
 
@@ -1295,7 +1325,7 @@ static void vlv_check_no_gt_access(struct drm_i915_private *dev_priv)
 	I915_WRITE(VLV_GTLC_PW_STATUS, VLV_GTLC_ALLOWWAKEERR);
 }
 
-static int vlv_runtime_suspend(struct drm_i915_private *dev_priv)
+static int vlv_suspend_complete(struct drm_i915_private *dev_priv)
 {
 	u32 mask;
 	int err;
@@ -1335,7 +1365,8 @@ err1:
 	return err;
 }
 
-static int vlv_runtime_resume(struct drm_i915_private *dev_priv)
+static int vlv_resume_prepare(struct drm_i915_private *dev_priv,
+				bool rpm_resume)
 {
 	struct drm_device *dev = dev_priv->dev;
 	int err;
@@ -1360,8 +1391,10 @@ static int vlv_runtime_resume(struct drm_i915_private *dev_priv)
 
 	vlv_check_no_gt_access(dev_priv);
 
-	intel_init_clock_gating(dev);
-	i915_gem_restore_fences(dev);
+	if (rpm_resume) {
+		intel_init_clock_gating(dev);
+		i915_gem_restore_fences(dev);
+	}
 
 	return ret;
 }
@@ -1376,7 +1409,9 @@ static int intel_runtime_suspend(struct device *device)
 	if (WARN_ON_ONCE(!(dev_priv->rps.enabled && intel_enable_rc6(dev))))
 		return -ENODEV;
 
-	WARN_ON(!HAS_RUNTIME_PM(dev));
+	if (WARN_ON_ONCE(!HAS_RUNTIME_PM(dev)))
+		return -ENODEV;
+
 	assert_force_wake_inactive(dev_priv);
 
 	DRM_DEBUG_KMS("Suspending device\n");
@@ -1413,17 +1448,7 @@ static int intel_runtime_suspend(struct device *device)
 	cancel_work_sync(&dev_priv->rps.work);
 	intel_runtime_pm_disable_interrupts(dev);
 
-	if (IS_GEN6(dev)) {
-		ret = 0;
-	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
-		ret = hsw_runtime_suspend(dev_priv);
-	} else if (IS_VALLEYVIEW(dev)) {
-		ret = vlv_runtime_suspend(dev_priv);
-	} else {
-		ret = -ENODEV;
-		WARN_ON(1);
-	}
-
+	ret = intel_suspend_complete(dev_priv);
 	if (ret) {
 		DRM_ERROR("Runtime suspend failed, disabling it (%d)\n", ret);
 		intel_runtime_pm_restore_interrupts(dev);
@@ -1435,13 +1460,29 @@ static int intel_runtime_suspend(struct device *device)
 	dev_priv->pm.suspended = true;
 
 	/*
-	 * current versions of firmware which depend on this opregion
-	 * notification have repurposed the D1 definition to mean
-	 * "runtime suspended" vs. what you would normally expect (D3)
-	 * to distinguish it from notifications that might be sent
-	 * via the suspend path.
+	 * FIXME: We really should find a document that references the arguments
+	 * used below!
 	 */
-	intel_opregion_notify_adapter(dev, PCI_D1);
+	if (IS_HASWELL(dev)) {
+		/*
+		 * current versions of firmware which depend on this opregion
+		 * notification have repurposed the D1 definition to mean
+		 * "runtime suspended" vs. what you would normally expect (D3)
+		 * to distinguish it from notifications that might be sent via
+		 * the suspend path.
+		 */
+		intel_opregion_notify_adapter(dev, PCI_D1);
+	} else {
+		/*
+		 * On Broadwell, if we use PCI_D1 the PCH DDI ports will stop
+		 * being detected, and the call we do at intel_runtime_resume()
+		 * won't be able to restore them. Since PCI_D3hot matches the
+		 * actual specification and appears to be working, use it. Let's
+		 * assume the other non-Haswell platforms will stay the same as
+		 * Broadwell.
+		 */
+		intel_opregion_notify_adapter(dev, PCI_D3hot);
+	}
 
 	DRM_DEBUG_KMS("Device suspended\n");
 	return 0;
@@ -1454,24 +1495,15 @@ static int intel_runtime_resume(struct device *device)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	WARN_ON(!HAS_RUNTIME_PM(dev));
+	if (WARN_ON_ONCE(!HAS_RUNTIME_PM(dev)))
+		return -ENODEV;
 
 	DRM_DEBUG_KMS("Resuming device\n");
 
 	intel_opregion_notify_adapter(dev, PCI_D0);
 	dev_priv->pm.suspended = false;
 
-	if (IS_GEN6(dev)) {
-		ret = snb_runtime_resume(dev_priv);
-	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
-		ret = hsw_runtime_resume(dev_priv);
-	} else if (IS_VALLEYVIEW(dev)) {
-		ret = vlv_runtime_resume(dev_priv);
-	} else {
-		WARN_ON(1);
-		ret = -ENODEV;
-	}
-
+	ret = intel_resume_prepare(dev_priv, true);
 	/*
 	 * No point of rolling back things in case of an error, as the best
 	 * we can do is to hope that things will still work (and disable RPM).
@@ -1490,6 +1522,48 @@ static int intel_runtime_resume(struct device *device)
 	return ret;
 }
 
+/*
+ * This function implements common functionality of runtime and system
+ * suspend sequence.
+ */
+static int intel_suspend_complete(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	int ret;
+
+	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+		ret = hsw_suspend_complete(dev_priv);
+	else if (IS_VALLEYVIEW(dev))
+		ret = vlv_suspend_complete(dev_priv);
+	else
+		ret = 0;
+
+	return ret;
+}
+
+/*
+ * This function implements common functionality of runtime and system
+ * resume sequence. Variable rpm_resume used for implementing different
+ * code paths.
+ */
+static int intel_resume_prepare(struct drm_i915_private *dev_priv,
+				bool rpm_resume)
+{
+	struct drm_device *dev = dev_priv->dev;
+	int ret;
+
+	if (IS_GEN6(dev))
+		ret = snb_resume_prepare(dev_priv, rpm_resume);
+	else if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+		ret = hsw_resume_prepare(dev_priv, rpm_resume);
+	else if (IS_VALLEYVIEW(dev))
+		ret = vlv_resume_prepare(dev_priv, rpm_resume);
+	else
+		ret = 0;
+
+	return ret;
+}
+
 static const struct dev_pm_ops i915_pm_ops = {
 	.suspend = i915_pm_suspend,
 	.suspend_late = i915_pm_suspend_late,
@@ -1539,6 +1613,7 @@ static struct drm_driver driver = {
 	.lastclose = i915_driver_lastclose,
 	.preclose = i915_driver_preclose,
 	.postclose = i915_driver_postclose,
+	.set_busid = drm_pci_set_busid,
 
 	/* Used in place of i915_pm_ops for non-DRIVER_MODESET */
 	.suspend = i915_suspend,
@@ -1630,6 +1705,8 @@ static void __exit i915_exit(void)
 module_init(i915_init);
 module_exit(i915_exit);
 
-MODULE_AUTHOR(DRIVER_AUTHOR);
+MODULE_AUTHOR("Tungsten Graphics, Inc.");
+MODULE_AUTHOR("Intel Corporation");
+
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL and additional rights");
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 4412f6a4383b..16a6f6d187a1 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -35,11 +35,15 @@
 #include "i915_reg.h"
 #include "intel_bios.h"
 #include "intel_ringbuffer.h"
+#include "intel_lrc.h"
 #include "i915_gem_gtt.h"
+#include "i915_gem_render_state.h"
 #include <linux/io-mapping.h>
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
 #include <drm/intel-gtt.h>
+#include <drm/drm_legacy.h> /* for struct drm_dma_handle */
+#include <drm/drm_gem.h>
 #include <linux/backlight.h>
 #include <linux/hashtable.h>
 #include <linux/intel-iommu.h>
@@ -49,11 +53,9 @@
 /* General customization:
  */
 
-#define DRIVER_AUTHOR		"Tungsten Graphics, Inc."
-
 #define DRIVER_NAME		"i915"
 #define DRIVER_DESC		"Intel Graphics"
-#define DRIVER_DATE		"20140725"
+#define DRIVER_DATE		"20140905"
 
 enum pipe {
 	INVALID_PIPE = -1,
@@ -162,7 +164,10 @@ enum hpd_pin {
 	 I915_GEM_DOMAIN_INSTRUCTION | \
 	 I915_GEM_DOMAIN_VERTEX)
 
-#define for_each_pipe(p) for ((p) = 0; (p) < INTEL_INFO(dev)->num_pipes; (p)++)
+#define for_each_pipe(__dev_priv, __p) \
+	for ((__p) = 0; (__p) < INTEL_INFO(__dev_priv)->num_pipes; (__p)++)
+#define for_each_plane(pipe, p) \
+	for ((p) = 0; (p) < INTEL_INFO(dev)->num_sprites[(pipe)] + 1; (p)++)
 #define for_each_sprite(p, s) for ((s) = 0; (s) < INTEL_INFO(dev)->num_sprites[(p)]; (s)++)
 
 #define for_each_crtc(dev, crtc) \
@@ -171,6 +176,11 @@ enum hpd_pin {
 #define for_each_intel_crtc(dev, intel_crtc) \
 	list_for_each_entry(intel_crtc, &dev->mode_config.crtc_list, base.head)
 
+#define for_each_intel_encoder(dev, intel_encoder)		\
+	list_for_each_entry(intel_encoder,			\
+			    &(dev)->mode_config.encoder_list,	\
+			    base.head)
+
 #define for_each_encoder_on_crtc(dev, __crtc, intel_encoder) \
 	list_for_each_entry((intel_encoder), &(dev)->mode_config.encoder_list, base.head) \
 		if ((intel_encoder)->base.crtc == (__crtc))
@@ -184,6 +194,7 @@ enum hpd_pin {
 		if ((1 << (domain)) & (mask))
 
 struct drm_i915_private;
+struct i915_mm_struct;
 struct i915_mmu_object;
 
 enum intel_dpll_id {
@@ -197,10 +208,13 @@ enum intel_dpll_id {
 #define I915_NUM_PLLS 2
 
 struct intel_dpll_hw_state {
+	/* i9xx, pch plls */
 	uint32_t dpll;
 	uint32_t dpll_md;
 	uint32_t fp0;
 	uint32_t fp1;
+
+	/* hsw, bdw */
 	uint32_t wrpll;
 };
 
@@ -276,8 +290,10 @@ struct intel_opregion {
 struct intel_overlay;
 struct intel_overlay_error_state;
 
+struct drm_local_map;
+
 struct drm_i915_master_private {
-	drm_local_map_t *sarea;
+	struct drm_local_map *sarea;
 	struct _drm_i915_sarea *sarea_priv;
 };
 #define I915_FENCE_REG_NONE -1
@@ -387,6 +403,7 @@ struct drm_i915_error_state {
 		pid_t pid;
 		char comm[TASK_COMM_LEN];
 	} ring[I915_NUM_RINGS];
+
 	struct drm_i915_error_buffer {
 		u32 size;
 		u32 name;
@@ -405,6 +422,7 @@ struct drm_i915_error_state {
 	} **active_bo, **pinned_bo;
 
 	u32 *active_bo_count, *pinned_bo_count;
+	u32 vm_count;
 };
 
 struct intel_connector;
@@ -550,6 +568,7 @@ struct intel_uncore {
 
 struct intel_device_info {
 	u32 display_mmio_offset;
+	u16 device_id;
 	u8 num_pipes:3;
 	u8 num_sprites[I915_MAX_PIPES];
 	u8 gen;
@@ -614,13 +633,21 @@ struct intel_context {
 	uint8_t remap_slice;
 	struct drm_i915_file_private *file_priv;
 	struct i915_ctx_hang_stats hang_stats;
-	struct i915_address_space *vm;
+	struct i915_hw_ppgtt *ppgtt;
 
+	/* Legacy ring buffer submission */
 	struct {
 		struct drm_i915_gem_object *rcs_state;
 		bool initialized;
 	} legacy_hw_ctx;
 
+	/* Execlists */
+	bool rcs_initialized;
+	struct {
+		struct drm_i915_gem_object *state;
+		struct intel_ringbuffer *ringbuf;
+	} engine[I915_NUM_RINGS];
+
 	struct list_head link;
 };
 
@@ -634,6 +661,8 @@ struct i915_fbc {
 	struct drm_mm_node compressed_fb;
 	struct drm_mm_node *compressed_llb;
 
+	bool false_color;
+
 	struct intel_fbc_work {
 		struct delayed_work work;
 		struct drm_crtc *crtc;
@@ -687,6 +716,7 @@ enum intel_sbi_destination {
 #define QUIRK_LVDS_SSC_DISABLE (1<<1)
 #define QUIRK_INVERT_BRIGHTNESS (1<<2)
 #define QUIRK_BACKLIGHT_PRESENT (1<<3)
+#define QUIRK_PIPEB_FORCE (1<<4)
 
 struct intel_fbdev;
 struct intel_fbc_work;
@@ -1146,6 +1176,7 @@ struct i915_gem_mm {
 };
 
 struct drm_i915_error_state_buf {
+	struct drm_i915_private *i915;
 	unsigned bytes;
 	unsigned size;
 	int err;
@@ -1218,6 +1249,9 @@ struct i915_gpu_error {
 
 	/* For missed irq/seqno simulation. */
 	unsigned int test_irq_rings;
+
+	/* Used to prevent gem_check_wedged returning -EAGAIN during gpu reset   */
+	bool reload_in_reset;
 };
 
 enum modeset_restore {
@@ -1227,6 +1261,12 @@ enum modeset_restore {
 };
 
 struct ddi_vbt_port_info {
+	/*
+	 * This is an index in the HDMI/DVI DDI buffer translation table.
+	 * The special value HDMI_LEVEL_SHIFT_UNKNOWN means the VBT didn't
+	 * populate this field.
+	 */
+#define HDMI_LEVEL_SHIFT_UNKNOWN	0xff
 	uint8_t hdmi_level_shift;
 
 	uint8_t supports_dvi:1;
@@ -1420,7 +1460,7 @@ struct drm_i915_private {
 	struct drm_i915_gem_object *semaphore_obj;
 	uint32_t last_seqno, next_seqno;
 
-	drm_dma_handle_t *status_page_dmah;
+	struct drm_dma_handle *status_page_dmah;
 	struct resource mch_res;
 
 	/* protects the irq masks */
@@ -1458,7 +1498,7 @@ struct drm_i915_private {
 		} hpd_mark;
 	} hpd_stats[HPD_NUM_PINS];
 	u32 hpd_event_bits;
-	struct timer_list hotplug_reenable_timer;
+	struct delayed_work hotplug_reenable_work;
 
 	struct i915_fbc fbc;
 	struct i915_drrs drrs;
@@ -1474,6 +1514,9 @@ struct drm_i915_private {
 	/* LVDS info */
 	bool no_aux_handshake;
 
+	/* protects panel power sequencer state */
+	struct mutex pps_mutex;
+
 	struct drm_i915_fence_reg fence_regs[I915_MAX_NUM_FENCES]; /* assume 965 */
 	int fence_reg_start; /* 4 if userland hasn't ioctl'd us yet */
 	int num_fence_regs; /* 8 on pre-965, 16 otherwise */
@@ -1506,9 +1549,8 @@ struct drm_i915_private {
 	struct i915_gtt gtt; /* VM representing the global address space */
 
 	struct i915_gem_mm mm;
-#if defined(CONFIG_MMU_NOTIFIER)
-	DECLARE_HASHTABLE(mmu_notifiers, 7);
-#endif
+	DECLARE_HASHTABLE(mm_structs, 7);
+	struct mutex mm_lock;
 
 	/* Kernel Modesetting */
 
@@ -1526,6 +1568,20 @@ struct drm_i915_private {
 	struct intel_shared_dpll shared_dplls[I915_NUM_PLLS];
 	int dpio_phy_iosf_port[I915_NUM_PHYS_VLV];
 
+	/*
+	 * workarounds are currently applied at different places and
+	 * changes are being done to consolidate them so exact count is
+	 * not clear at this point, use a max value for now.
+	 */
+#define I915_MAX_WA_REGS  16
+	struct {
+		u32 addr;
+		u32 value;
+		/* bitmask representing WA bits */
+		u32 mask;
+	} intel_wa_regs[I915_MAX_WA_REGS];
+	u32 num_wa_regs;
+
 	/* Reclocking support */
 	bool render_reclock_avail;
 	bool lvds_downclock_avail;
@@ -1561,14 +1617,9 @@ struct drm_i915_private {
 #ifdef CONFIG_DRM_I915_FBDEV
 	/* list of fbdev register on this device */
 	struct intel_fbdev *fbdev;
+	struct work_struct fbdev_suspend_work;
 #endif
 
-	/*
-	 * The console may be contended at resume, but we don't
-	 * want it to block on it.
-	 */
-	struct work_struct console_resume_work;
-
 	struct drm_property *broadcast_rgb_property;
 	struct drm_property *force_audio_property;
 
@@ -1614,12 +1665,28 @@ struct drm_i915_private {
 	 */
 	struct workqueue_struct *dp_wq;
 
+	uint32_t bios_vgacntr;
+
 	/* Old dri1 support infrastructure, beware the dragons ya fools entering
 	 * here! */
 	struct i915_dri1_state dri1;
 	/* Old ums support infrastructure, same warning applies. */
 	struct i915_ums_state ums;
 
+	/* Abstract the submission mechanism (legacy ringbuffer or execlists) away */
+	struct {
+		int (*do_execbuf)(struct drm_device *dev, struct drm_file *file,
+				  struct intel_engine_cs *ring,
+				  struct intel_context *ctx,
+				  struct drm_i915_gem_execbuffer2 *args,
+				  struct list_head *vmas,
+				  struct drm_i915_gem_object *batch_obj,
+				  u64 exec_start, u32 flags);
+		int (*init_rings)(struct drm_device *dev);
+		void (*cleanup_ring)(struct intel_engine_cs *ring);
+		void (*stop_ring)(struct intel_engine_cs *ring);
+	} gt;
+
 	/*
 	 * NOTE: This is the dri1/ums dungeon, don't add stuff here. Your patch
 	 * will be rejected. Instead look for a better place.
@@ -1761,13 +1828,6 @@ struct drm_i915_gem_object {
 	 * Only honoured if hardware has relevant pte bit
 	 */
 	unsigned long gt_ro:1;
-
-	/*
-	 * Is the GPU currently using a fence to access this buffer,
-	 */
-	unsigned int pending_fenced_gpu_access:1;
-	unsigned int fenced_gpu_access:1;
-
 	unsigned int cache_level:3;
 
 	unsigned int has_aliasing_ppgtt_mapping:1;
@@ -1805,7 +1865,7 @@ struct drm_i915_gem_object {
 	struct drm_file *pin_filp;
 
 	/** for phy allocated objects */
-	drm_dma_handle_t *phys_handle;
+	struct drm_dma_handle *phys_handle;
 
 	union {
 		struct i915_gem_userptr {
@@ -1814,8 +1874,8 @@ struct drm_i915_gem_object {
 			unsigned workers :4;
 #define I915_GEM_USERPTR_MAX_WORKERS 15
 
-			struct mm_struct *mm;
-			struct i915_mmu_object *mn;
+			struct i915_mm_struct *mm;
+			struct i915_mmu_object *mmu_object;
 			struct work_struct *work;
 		} userptr;
 	};
@@ -1971,51 +2031,63 @@ struct drm_i915_cmd_table {
 	int count;
 };
 
-#define INTEL_INFO(dev)	(&to_i915(dev)->info)
-
-#define IS_I830(dev)		((dev)->pdev->device == 0x3577)
-#define IS_845G(dev)		((dev)->pdev->device == 0x2562)
+/* Note that the (struct drm_i915_private *) cast is just to shut up gcc. */
+#define __I915__(p) ({ \
+	struct drm_i915_private *__p; \
+	if (__builtin_types_compatible_p(typeof(*p), struct drm_i915_private)) \
+		__p = (struct drm_i915_private *)p; \
+	else if (__builtin_types_compatible_p(typeof(*p), struct drm_device)) \
+		__p = to_i915((struct drm_device *)p); \
+	else \
+		BUILD_BUG(); \
+	__p; \
+})
+#define INTEL_INFO(p) 	(&__I915__(p)->info)
+#define INTEL_DEVID(p)	(INTEL_INFO(p)->device_id)
+
+#define IS_I830(dev)		(INTEL_DEVID(dev) == 0x3577)
+#define IS_845G(dev)		(INTEL_DEVID(dev) == 0x2562)
 #define IS_I85X(dev)		(INTEL_INFO(dev)->is_i85x)
-#define IS_I865G(dev)		((dev)->pdev->device == 0x2572)
+#define IS_I865G(dev)		(INTEL_DEVID(dev) == 0x2572)
 #define IS_I915G(dev)		(INTEL_INFO(dev)->is_i915g)
-#define IS_I915GM(dev)		((dev)->pdev->device == 0x2592)
-#define IS_I945G(dev)		((dev)->pdev->device == 0x2772)
+#define IS_I915GM(dev)		(INTEL_DEVID(dev) == 0x2592)
+#define IS_I945G(dev)		(INTEL_DEVID(dev) == 0x2772)
 #define IS_I945GM(dev)		(INTEL_INFO(dev)->is_i945gm)
 #define IS_BROADWATER(dev)	(INTEL_INFO(dev)->is_broadwater)
 #define IS_CRESTLINE(dev)	(INTEL_INFO(dev)->is_crestline)
-#define IS_GM45(dev)		((dev)->pdev->device == 0x2A42)
+#define IS_GM45(dev)		(INTEL_DEVID(dev) == 0x2A42)
 #define IS_G4X(dev)		(INTEL_INFO(dev)->is_g4x)
-#define IS_PINEVIEW_G(dev)	((dev)->pdev->device == 0xa001)
-#define IS_PINEVIEW_M(dev)	((dev)->pdev->device == 0xa011)
+#define IS_PINEVIEW_G(dev)	(INTEL_DEVID(dev) == 0xa001)
+#define IS_PINEVIEW_M(dev)	(INTEL_DEVID(dev) == 0xa011)
 #define IS_PINEVIEW(dev)	(INTEL_INFO(dev)->is_pineview)
 #define IS_G33(dev)		(INTEL_INFO(dev)->is_g33)
-#define IS_IRONLAKE_M(dev)	((dev)->pdev->device == 0x0046)
+#define IS_IRONLAKE_M(dev)	(INTEL_DEVID(dev) == 0x0046)
 #define IS_IVYBRIDGE(dev)	(INTEL_INFO(dev)->is_ivybridge)
-#define IS_IVB_GT1(dev)		((dev)->pdev->device == 0x0156 || \
-				 (dev)->pdev->device == 0x0152 || \
-				 (dev)->pdev->device == 0x015a)
-#define IS_SNB_GT1(dev)		((dev)->pdev->device == 0x0102 || \
-				 (dev)->pdev->device == 0x0106 || \
-				 (dev)->pdev->device == 0x010A)
+#define IS_IVB_GT1(dev)		(INTEL_DEVID(dev) == 0x0156 || \
+				 INTEL_DEVID(dev) == 0x0152 || \
+				 INTEL_DEVID(dev) == 0x015a)
+#define IS_SNB_GT1(dev)		(INTEL_DEVID(dev) == 0x0102 || \
+				 INTEL_DEVID(dev) == 0x0106 || \
+				 INTEL_DEVID(dev) == 0x010A)
 #define IS_VALLEYVIEW(dev)	(INTEL_INFO(dev)->is_valleyview)
 #define IS_CHERRYVIEW(dev)	(INTEL_INFO(dev)->is_valleyview && IS_GEN8(dev))
 #define IS_HASWELL(dev)	(INTEL_INFO(dev)->is_haswell)
 #define IS_BROADWELL(dev)	(!INTEL_INFO(dev)->is_valleyview && IS_GEN8(dev))
 #define IS_MOBILE(dev)		(INTEL_INFO(dev)->is_mobile)
 #define IS_HSW_EARLY_SDV(dev)	(IS_HASWELL(dev) && \
-				 ((dev)->pdev->device & 0xFF00) == 0x0C00)
+				 (INTEL_DEVID(dev) & 0xFF00) == 0x0C00)
 #define IS_BDW_ULT(dev)		(IS_BROADWELL(dev) && \
-				 (((dev)->pdev->device & 0xf) == 0x2  || \
-				 ((dev)->pdev->device & 0xf) == 0x6 || \
-				 ((dev)->pdev->device & 0xf) == 0xe))
+				 ((INTEL_DEVID(dev) & 0xf) == 0x2  || \
+				 (INTEL_DEVID(dev) & 0xf) == 0x6 || \
+				 (INTEL_DEVID(dev) & 0xf) == 0xe))
 #define IS_HSW_ULT(dev)		(IS_HASWELL(dev) && \
-				 ((dev)->pdev->device & 0xFF00) == 0x0A00)
+				 (INTEL_DEVID(dev) & 0xFF00) == 0x0A00)
 #define IS_ULT(dev)		(IS_HSW_ULT(dev) || IS_BDW_ULT(dev))
 #define IS_HSW_GT3(dev)		(IS_HASWELL(dev) && \
-				 ((dev)->pdev->device & 0x00F0) == 0x0020)
+				 (INTEL_DEVID(dev) & 0x00F0) == 0x0020)
 /* ULX machines are also considered ULT. */
-#define IS_HSW_ULX(dev)		((dev)->pdev->device == 0x0A0E || \
-				 (dev)->pdev->device == 0x0A1E)
+#define IS_HSW_ULX(dev)		(INTEL_DEVID(dev) == 0x0A0E || \
+				 INTEL_DEVID(dev) == 0x0A1E)
 #define IS_PRELIMINARY_HW(intel_info) ((intel_info)->is_preliminary)
 
 /*
@@ -2047,10 +2119,11 @@ struct drm_i915_cmd_table {
 #define I915_NEED_GFX_HWS(dev)	(INTEL_INFO(dev)->need_gfx_hws)
 
 #define HAS_HW_CONTEXTS(dev)	(INTEL_INFO(dev)->gen >= 6)
+#define HAS_LOGICAL_RING_CONTEXTS(dev)	(INTEL_INFO(dev)->gen >= 8)
 #define HAS_ALIASING_PPGTT(dev)	(INTEL_INFO(dev)->gen >= 6)
 #define HAS_PPGTT(dev)		(INTEL_INFO(dev)->gen >= 7 && !IS_GEN8(dev))
-#define USES_PPGTT(dev)		intel_enable_ppgtt(dev, false)
-#define USES_FULL_PPGTT(dev)	intel_enable_ppgtt(dev, true)
+#define USES_PPGTT(dev)		(i915.enable_ppgtt)
+#define USES_FULL_PPGTT(dev)	(i915.enable_ppgtt == 2)
 
 #define HAS_OVERLAY(dev)		(INTEL_INFO(dev)->has_overlay)
 #define OVERLAY_NEEDS_PHYSICAL(dev)	(INTEL_INFO(dev)->overlay_needs_physical)
@@ -2134,6 +2207,7 @@ struct i915_params {
 	int enable_rc6;
 	int enable_fbc;
 	int enable_ppgtt;
+	int enable_execlists;
 	int enable_psr;
 	unsigned int preliminary_hw_support;
 	int disable_power_well;
@@ -2178,8 +2252,7 @@ extern unsigned long i915_mch_val(struct drm_i915_private *dev_priv);
 extern unsigned long i915_gfx_val(struct drm_i915_private *dev_priv);
 extern void i915_update_gfx_val(struct drm_i915_private *dev_priv);
 int vlv_force_gfx_clock(struct drm_i915_private *dev_priv, bool on);
-
-extern void intel_console_resume(struct work_struct *work);
+void intel_hpd_cancel_work(struct drm_i915_private *dev_priv);
 
 /* i915_irq.c */
 void i915_queue_hangcheck(struct drm_device *dev);
@@ -2228,6 +2301,20 @@ int i915_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv);
 int i915_gem_sw_finish_ioctl(struct drm_device *dev, void *data,
 			     struct drm_file *file_priv);
+void i915_gem_execbuffer_move_to_active(struct list_head *vmas,
+					struct intel_engine_cs *ring);
+void i915_gem_execbuffer_retire_commands(struct drm_device *dev,
+					 struct drm_file *file,
+					 struct intel_engine_cs *ring,
+					 struct drm_i915_gem_object *obj);
+int i915_gem_ringbuffer_submission(struct drm_device *dev,
+				   struct drm_file *file,
+				   struct intel_engine_cs *ring,
+				   struct intel_context *ctx,
+				   struct drm_i915_gem_execbuffer2 *args,
+				   struct list_head *vmas,
+				   struct drm_i915_gem_object *batch_obj,
+				   u64 exec_start, u32 flags);
 int i915_gem_execbuffer(struct drm_device *dev, void *data,
 			struct drm_file *file_priv);
 int i915_gem_execbuffer2(struct drm_device *dev, void *data,
@@ -2262,6 +2349,12 @@ int i915_gem_get_aperture_ioctl(struct drm_device *dev, void *data,
 int i915_gem_wait_ioctl(struct drm_device *dev, void *data,
 			struct drm_file *file_priv);
 void i915_gem_load(struct drm_device *dev);
+unsigned long i915_gem_shrink(struct drm_i915_private *dev_priv,
+			      long target,
+			      unsigned flags);
+#define I915_SHRINK_PURGEABLE 0x1
+#define I915_SHRINK_UNBOUND 0x2
+#define I915_SHRINK_BOUND 0x4
 void *i915_gem_object_alloc(struct drm_device *dev);
 void i915_gem_object_free(struct drm_i915_gem_object *obj);
 void i915_gem_object_init(struct drm_i915_gem_object *obj,
@@ -2380,6 +2473,7 @@ void i915_gem_reset(struct drm_device *dev);
 bool i915_gem_clflush_object(struct drm_i915_gem_object *obj, bool force);
 int __must_check i915_gem_object_finish_gpu(struct drm_i915_gem_object *obj);
 int __must_check i915_gem_init(struct drm_device *dev);
+int i915_gem_init_rings(struct drm_device *dev);
 int __must_check i915_gem_init_hw(struct drm_device *dev);
 int i915_gem_l3_remap(struct intel_engine_cs *ring, int slice);
 void i915_gem_init_swizzling(struct drm_device *dev);
@@ -2450,7 +2544,7 @@ static inline bool i915_gem_obj_is_pinned(struct drm_i915_gem_object *obj) {
 }
 
 /* Some GGTT VM helpers */
-#define obj_to_ggtt(obj) \
+#define i915_obj_to_ggtt(obj) \
 	(&((struct drm_i915_private *)(obj)->base.dev->dev_private)->gtt.base)
 static inline bool i915_is_ggtt(struct i915_address_space *vm)
 {
@@ -2459,21 +2553,30 @@ static inline bool i915_is_ggtt(struct i915_address_space *vm)
 	return vm == ggtt;
 }
 
+static inline struct i915_hw_ppgtt *
+i915_vm_to_ppgtt(struct i915_address_space *vm)
+{
+	WARN_ON(i915_is_ggtt(vm));
+
+	return container_of(vm, struct i915_hw_ppgtt, base);
+}
+
+
 static inline bool i915_gem_obj_ggtt_bound(struct drm_i915_gem_object *obj)
 {
-	return i915_gem_obj_bound(obj, obj_to_ggtt(obj));
+	return i915_gem_obj_bound(obj, i915_obj_to_ggtt(obj));
 }
 
 static inline unsigned long
 i915_gem_obj_ggtt_offset(struct drm_i915_gem_object *obj)
 {
-	return i915_gem_obj_offset(obj, obj_to_ggtt(obj));
+	return i915_gem_obj_offset(obj, i915_obj_to_ggtt(obj));
 }
 
 static inline unsigned long
 i915_gem_obj_ggtt_size(struct drm_i915_gem_object *obj)
 {
-	return i915_gem_obj_size(obj, obj_to_ggtt(obj));
+	return i915_gem_obj_size(obj, i915_obj_to_ggtt(obj));
 }
 
 static inline int __must_check
@@ -2481,7 +2584,8 @@ i915_gem_obj_ggtt_pin(struct drm_i915_gem_object *obj,
 		      uint32_t alignment,
 		      unsigned flags)
 {
-	return i915_gem_object_pin(obj, obj_to_ggtt(obj), alignment, flags | PIN_GLOBAL);
+	return i915_gem_object_pin(obj, i915_obj_to_ggtt(obj),
+				   alignment, flags | PIN_GLOBAL);
 }
 
 static inline int
@@ -2493,7 +2597,6 @@ i915_gem_object_ggtt_unbind(struct drm_i915_gem_object *obj)
 void i915_gem_object_ggtt_unpin(struct drm_i915_gem_object *obj);
 
 /* i915_gem_context.c */
-#define ctx_to_ppgtt(ctx) container_of((ctx)->vm, struct i915_hw_ppgtt, base)
 int __must_check i915_gem_context_init(struct drm_device *dev);
 void i915_gem_context_fini(struct drm_device *dev);
 void i915_gem_context_reset(struct drm_device *dev);
@@ -2505,6 +2608,8 @@ int i915_switch_context(struct intel_engine_cs *ring,
 struct intel_context *
 i915_gem_context_get(struct drm_i915_file_private *file_priv, u32 id);
 void i915_gem_context_free(struct kref *ctx_ref);
+struct drm_i915_gem_object *
+i915_gem_alloc_context_obj(struct drm_device *dev, size_t size);
 static inline void i915_gem_context_reference(struct intel_context *ctx)
 {
 	kref_get(&ctx->ref);
@@ -2525,8 +2630,6 @@ int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
 int i915_gem_context_destroy_ioctl(struct drm_device *dev, void *data,
 				   struct drm_file *file);
 
-/* i915_gem_render_state.c */
-int i915_gem_render_state_init(struct intel_engine_cs *ring);
 /* i915_gem_evict.c */
 int __must_check i915_gem_evict_something(struct drm_device *dev,
 					  struct i915_address_space *vm,
@@ -2594,6 +2697,7 @@ void i915_error_printf(struct drm_i915_error_state_buf *e, const char *f, ...);
 int i915_error_state_to_str(struct drm_i915_error_state_buf *estr,
 			    const struct i915_error_state_file_priv *error);
 int i915_error_state_buf_init(struct drm_i915_error_state_buf *eb,
+			      struct drm_i915_private *i915,
 			      size_t count, loff_t pos);
 static inline void i915_error_state_buf_release(
 	struct drm_i915_error_state_buf *eb)
@@ -2608,7 +2712,7 @@ void i915_error_state_put(struct i915_error_state_file_priv *error_priv);
 void i915_destroy_error_state(struct drm_device *dev);
 
 void i915_get_extra_instdone(struct drm_device *dev, uint32_t *instdone);
-const char *i915_cache_level_str(int type);
+const char *i915_cache_level_str(struct drm_i915_private *i915, int type);
 
 /* i915_cmd_parser.c */
 int i915_cmd_parser_get_version(void);
@@ -2700,6 +2804,7 @@ extern void intel_modeset_setup_hw_state(struct drm_device *dev,
 extern void i915_redisable_vga(struct drm_device *dev);
 extern void i915_redisable_vga_power_on(struct drm_device *dev);
 extern bool intel_fbc_enabled(struct drm_device *dev);
+extern void gen8_fbc_sw_flush(struct drm_device *dev, u32 value);
 extern void intel_disable_fbc(struct drm_device *dev);
 extern bool ironlake_set_drps(struct drm_device *dev, u8 val);
 extern void intel_init_pch_refclk(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/i915_gem.c b/drivers/gpu/drm/i915/i915_gem.c
index ba7f5c6bb50d..28f91df2604d 100644
--- a/drivers/gpu/drm/i915/i915_gem.c
+++ b/drivers/gpu/drm/i915/i915_gem.c
@@ -60,7 +60,6 @@ static unsigned long i915_gem_shrinker_scan(struct shrinker *shrinker,
 static int i915_gem_shrinker_oom(struct notifier_block *nb,
 				 unsigned long event,
 				 void *ptr);
-static unsigned long i915_gem_purge(struct drm_i915_private *dev_priv, long target);
 static unsigned long i915_gem_shrink_all(struct drm_i915_private *dev_priv);
 
 static bool cpu_cache_is_coherent(struct drm_device *dev,
@@ -1085,7 +1084,13 @@ i915_gem_check_wedge(struct i915_gpu_error *error,
 		if (i915_terminally_wedged(error))
 			return -EIO;
 
-		return -EAGAIN;
+		/*
+		 * Check if GPU Reset is in progress - we need intel_ring_begin
+		 * to work properly to reinit the hw state while the gpu is
+		 * still marked as reset-in-progress. Handle this with a flag.
+		 */
+		if (!error->reload_in_reset)
+			return -EAGAIN;
 	}
 
 	return 0;
@@ -1590,10 +1595,13 @@ unlock:
 out:
 	switch (ret) {
 	case -EIO:
-		/* If this -EIO is due to a gpu hang, give the reset code a
-		 * chance to clean up the mess. Otherwise return the proper
-		 * SIGBUS. */
-		if (i915_terminally_wedged(&dev_priv->gpu_error)) {
+		/*
+		 * We eat errors when the gpu is terminally wedged to avoid
+		 * userspace unduly crashing (gl has no provisions for mmaps to
+		 * fail). But any other -EIO isn't ours (e.g. swap in failure)
+		 * and so needs to be reported.
+		 */
+		if (!i915_terminally_wedged(&dev_priv->gpu_error)) {
 			ret = VM_FAULT_SIGBUS;
 			break;
 		}
@@ -1732,7 +1740,11 @@ static int i915_gem_object_create_mmap_offset(struct drm_i915_gem_object *obj)
 	 * offsets on purgeable objects by truncating it and marking it purged,
 	 * which prevents userspace from ever using that object again.
 	 */
-	i915_gem_purge(dev_priv, obj->base.size >> PAGE_SHIFT);
+	i915_gem_shrink(dev_priv,
+			obj->base.size >> PAGE_SHIFT,
+			I915_SHRINK_BOUND |
+			I915_SHRINK_UNBOUND |
+			I915_SHRINK_PURGEABLE);
 	ret = drm_gem_create_mmap_offset(&obj->base);
 	if (ret != -ENOSPC)
 		goto out;
@@ -1929,12 +1941,11 @@ i915_gem_object_put_pages(struct drm_i915_gem_object *obj)
 	return 0;
 }
 
-static unsigned long
-__i915_gem_shrink(struct drm_i915_private *dev_priv, long target,
-		  bool purgeable_only)
+unsigned long
+i915_gem_shrink(struct drm_i915_private *dev_priv,
+		long target, unsigned flags)
 {
-	struct list_head still_in_list;
-	struct drm_i915_gem_object *obj;
+	const bool purgeable_only = flags & I915_SHRINK_PURGEABLE;
 	unsigned long count = 0;
 
 	/*
@@ -1956,62 +1967,68 @@ __i915_gem_shrink(struct drm_i915_private *dev_priv, long target,
 	 * dev->struct_mutex and so we won't ever be able to observe an
 	 * object on the bound_list with a reference count equals 0.
 	 */
-	INIT_LIST_HEAD(&still_in_list);
-	while (count < target && !list_empty(&dev_priv->mm.unbound_list)) {
-		obj = list_first_entry(&dev_priv->mm.unbound_list,
-				       typeof(*obj), global_list);
-		list_move_tail(&obj->global_list, &still_in_list);
+	if (flags & I915_SHRINK_UNBOUND) {
+		struct list_head still_in_list;
 
-		if (!i915_gem_object_is_purgeable(obj) && purgeable_only)
-			continue;
+		INIT_LIST_HEAD(&still_in_list);
+		while (count < target && !list_empty(&dev_priv->mm.unbound_list)) {
+			struct drm_i915_gem_object *obj;
 
-		drm_gem_object_reference(&obj->base);
+			obj = list_first_entry(&dev_priv->mm.unbound_list,
+					       typeof(*obj), global_list);
+			list_move_tail(&obj->global_list, &still_in_list);
 
-		if (i915_gem_object_put_pages(obj) == 0)
-			count += obj->base.size >> PAGE_SHIFT;
+			if (!i915_gem_object_is_purgeable(obj) && purgeable_only)
+				continue;
+
+			drm_gem_object_reference(&obj->base);
 
-		drm_gem_object_unreference(&obj->base);
+			if (i915_gem_object_put_pages(obj) == 0)
+				count += obj->base.size >> PAGE_SHIFT;
+
+			drm_gem_object_unreference(&obj->base);
+		}
+		list_splice(&still_in_list, &dev_priv->mm.unbound_list);
 	}
-	list_splice(&still_in_list, &dev_priv->mm.unbound_list);
 
-	INIT_LIST_HEAD(&still_in_list);
-	while (count < target && !list_empty(&dev_priv->mm.bound_list)) {
-		struct i915_vma *vma, *v;
+	if (flags & I915_SHRINK_BOUND) {
+		struct list_head still_in_list;
 
-		obj = list_first_entry(&dev_priv->mm.bound_list,
-				       typeof(*obj), global_list);
-		list_move_tail(&obj->global_list, &still_in_list);
+		INIT_LIST_HEAD(&still_in_list);
+		while (count < target && !list_empty(&dev_priv->mm.bound_list)) {
+			struct drm_i915_gem_object *obj;
+			struct i915_vma *vma, *v;
 
-		if (!i915_gem_object_is_purgeable(obj) && purgeable_only)
-			continue;
+			obj = list_first_entry(&dev_priv->mm.bound_list,
+					       typeof(*obj), global_list);
+			list_move_tail(&obj->global_list, &still_in_list);
 
-		drm_gem_object_reference(&obj->base);
+			if (!i915_gem_object_is_purgeable(obj) && purgeable_only)
+				continue;
 
-		list_for_each_entry_safe(vma, v, &obj->vma_list, vma_link)
-			if (i915_vma_unbind(vma))
-				break;
+			drm_gem_object_reference(&obj->base);
 
-		if (i915_gem_object_put_pages(obj) == 0)
-			count += obj->base.size >> PAGE_SHIFT;
+			list_for_each_entry_safe(vma, v, &obj->vma_list, vma_link)
+				if (i915_vma_unbind(vma))
+					break;
+
+			if (i915_gem_object_put_pages(obj) == 0)
+				count += obj->base.size >> PAGE_SHIFT;
 
-		drm_gem_object_unreference(&obj->base);
+			drm_gem_object_unreference(&obj->base);
+		}
+		list_splice(&still_in_list, &dev_priv->mm.bound_list);
 	}
-	list_splice(&still_in_list, &dev_priv->mm.bound_list);
 
 	return count;
 }
 
 static unsigned long
-i915_gem_purge(struct drm_i915_private *dev_priv, long target)
-{
-	return __i915_gem_shrink(dev_priv, target, true);
-}
-
-static unsigned long
 i915_gem_shrink_all(struct drm_i915_private *dev_priv)
 {
 	i915_gem_evict_everything(dev_priv->dev);
-	return __i915_gem_shrink(dev_priv, LONG_MAX, false);
+	return i915_gem_shrink(dev_priv, LONG_MAX,
+			       I915_SHRINK_BOUND | I915_SHRINK_UNBOUND);
 }
 
 static int
@@ -2058,7 +2075,11 @@ i915_gem_object_get_pages_gtt(struct drm_i915_gem_object *obj)
 	for (i = 0; i < page_count; i++) {
 		page = shmem_read_mapping_page_gfp(mapping, i, gfp);
 		if (IS_ERR(page)) {
-			i915_gem_purge(dev_priv, page_count);
+			i915_gem_shrink(dev_priv,
+					page_count,
+					I915_SHRINK_BOUND |
+					I915_SHRINK_UNBOUND |
+					I915_SHRINK_PURGEABLE);
 			page = shmem_read_mapping_page_gfp(mapping, i, gfp);
 		}
 		if (IS_ERR(page)) {
@@ -2160,8 +2181,6 @@ static void
 i915_gem_object_move_to_active(struct drm_i915_gem_object *obj,
 			       struct intel_engine_cs *ring)
 {
-	struct drm_device *dev = obj->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 seqno = intel_ring_get_seqno(ring);
 
 	BUG_ON(ring == NULL);
@@ -2180,19 +2199,6 @@ i915_gem_object_move_to_active(struct drm_i915_gem_object *obj,
 	list_move_tail(&obj->ring_list, &ring->active_list);
 
 	obj->last_read_seqno = seqno;
-
-	if (obj->fenced_gpu_access) {
-		obj->last_fenced_seqno = seqno;
-
-		/* Bump MRU to take account of the delayed flush */
-		if (obj->fence_reg != I915_FENCE_REG_NONE) {
-			struct drm_i915_fence_reg *reg;
-
-			reg = &dev_priv->fence_regs[obj->fence_reg];
-			list_move_tail(&reg->lru_list,
-				       &dev_priv->mm.fence_list);
-		}
-	}
 }
 
 void i915_vma_move_to_active(struct i915_vma *vma,
@@ -2228,7 +2234,6 @@ i915_gem_object_move_to_inactive(struct drm_i915_gem_object *obj)
 	obj->base.write_domain = 0;
 
 	obj->last_fenced_seqno = 0;
-	obj->fenced_gpu_access = false;
 
 	obj->active = 0;
 	drm_gem_object_unreference(&obj->base);
@@ -2326,10 +2331,21 @@ int __i915_add_request(struct intel_engine_cs *ring,
 {
 	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 	struct drm_i915_gem_request *request;
+	struct intel_ringbuffer *ringbuf;
 	u32 request_ring_position, request_start;
 	int ret;
 
-	request_start = intel_ring_get_tail(ring->buffer);
+	request = ring->preallocated_lazy_request;
+	if (WARN_ON(request == NULL))
+		return -ENOMEM;
+
+	if (i915.enable_execlists) {
+		struct intel_context *ctx = request->ctx;
+		ringbuf = ctx->engine[ring->id].ringbuf;
+	} else
+		ringbuf = ring->buffer;
+
+	request_start = intel_ring_get_tail(ringbuf);
 	/*
 	 * Emit any outstanding flushes - execbuf can fail to emit the flush
 	 * after having emitted the batchbuffer command. Hence we need to fix
@@ -2337,24 +2353,32 @@ int __i915_add_request(struct intel_engine_cs *ring,
 	 * is that the flush _must_ happen before the next request, no matter
 	 * what.
 	 */
-	ret = intel_ring_flush_all_caches(ring);
-	if (ret)
-		return ret;
-
-	request = ring->preallocated_lazy_request;
-	if (WARN_ON(request == NULL))
-		return -ENOMEM;
+	if (i915.enable_execlists) {
+		ret = logical_ring_flush_all_caches(ringbuf);
+		if (ret)
+			return ret;
+	} else {
+		ret = intel_ring_flush_all_caches(ring);
+		if (ret)
+			return ret;
+	}
 
 	/* Record the position of the start of the request so that
 	 * should we detect the updated seqno part-way through the
 	 * GPU processing the request, we never over-estimate the
 	 * position of the head.
 	 */
-	request_ring_position = intel_ring_get_tail(ring->buffer);
+	request_ring_position = intel_ring_get_tail(ringbuf);
 
-	ret = ring->add_request(ring);
-	if (ret)
-		return ret;
+	if (i915.enable_execlists) {
+		ret = ring->emit_request(ringbuf);
+		if (ret)
+			return ret;
+	} else {
+		ret = ring->add_request(ring);
+		if (ret)
+			return ret;
+	}
 
 	request->seqno = intel_ring_get_seqno(ring);
 	request->ring = ring;
@@ -2369,12 +2393,14 @@ int __i915_add_request(struct intel_engine_cs *ring,
 	 */
 	request->batch_obj = obj;
 
-	/* Hold a reference to the current context so that we can inspect
-	 * it later in case a hangcheck error event fires.
-	 */
-	request->ctx = ring->last_context;
-	if (request->ctx)
-		i915_gem_context_reference(request->ctx);
+	if (!i915.enable_execlists) {
+		/* Hold a reference to the current context so that we can inspect
+		 * it later in case a hangcheck error event fires.
+		 */
+		request->ctx = ring->last_context;
+		if (request->ctx)
+			i915_gem_context_reference(request->ctx);
+	}
 
 	request->emitted_jiffies = jiffies;
 	list_add_tail(&request->list, &ring->request_list);
@@ -2545,6 +2571,18 @@ static void i915_gem_reset_ring_cleanup(struct drm_i915_private *dev_priv,
 		i915_gem_free_request(request);
 	}
 
+	while (!list_empty(&ring->execlist_queue)) {
+		struct intel_ctx_submit_request *submit_req;
+
+		submit_req = list_first_entry(&ring->execlist_queue,
+				struct intel_ctx_submit_request,
+				execlist_link);
+		list_del(&submit_req->execlist_link);
+		intel_runtime_pm_put(dev_priv);
+		i915_gem_context_unreference(submit_req->ctx);
+		kfree(submit_req);
+	}
+
 	/* These may not have been flush before the reset, do so now */
 	kfree(ring->preallocated_lazy_request);
 	ring->preallocated_lazy_request = NULL;
@@ -2629,6 +2667,7 @@ i915_gem_retire_requests_ring(struct intel_engine_cs *ring)
 
 	while (!list_empty(&ring->request_list)) {
 		struct drm_i915_gem_request *request;
+		struct intel_ringbuffer *ringbuf;
 
 		request = list_first_entry(&ring->request_list,
 					   struct drm_i915_gem_request,
@@ -2638,12 +2677,24 @@ i915_gem_retire_requests_ring(struct intel_engine_cs *ring)
 			break;
 
 		trace_i915_gem_request_retire(ring, request->seqno);
+
+		/* This is one of the few common intersection points
+		 * between legacy ringbuffer submission and execlists:
+		 * we need to tell them apart in order to find the correct
+		 * ringbuffer to which the request belongs to.
+		 */
+		if (i915.enable_execlists) {
+			struct intel_context *ctx = request->ctx;
+			ringbuf = ctx->engine[ring->id].ringbuf;
+		} else
+			ringbuf = ring->buffer;
+
 		/* We know the GPU must have read the request to have
 		 * sent us the seqno + interrupt, so use the position
 		 * of tail of the request to update the last known position
 		 * of the GPU head.
 		 */
-		ring->buffer->last_retired_head = request->tail;
+		ringbuf->last_retired_head = request->tail;
 
 		i915_gem_free_request(request);
 	}
@@ -2905,6 +2956,9 @@ int i915_vma_unbind(struct i915_vma *vma)
 	 * cause memory corruption through use-after-free.
 	 */
 
+	/* Throw away the active reference before moving to the unbound list */
+	i915_gem_object_retire(obj);
+
 	if (i915_is_ggtt(vma->vm)) {
 		i915_gem_object_finish_gtt(obj);
 
@@ -2919,9 +2973,8 @@ int i915_vma_unbind(struct i915_vma *vma)
 	vma->unbind_vma(vma);
 
 	list_del_init(&vma->mm_list);
-	/* Avoid an unnecessary call to unbind on rebind. */
 	if (i915_is_ggtt(vma->vm))
-		obj->map_and_fenceable = true;
+		obj->map_and_fenceable = false;
 
 	drm_mm_remove_node(&vma->node);
 	i915_gem_vma_destroy(vma);
@@ -2950,9 +3003,11 @@ int i915_gpu_idle(struct drm_device *dev)
 
 	/* Flush everything onto the inactive list. */
 	for_each_ring(ring, dev_priv, i) {
-		ret = i915_switch_context(ring, ring->default_context);
-		if (ret)
-			return ret;
+		if (!i915.enable_execlists) {
+			ret = i915_switch_context(ring, ring->default_context);
+			if (ret)
+				return ret;
+		}
 
 		ret = intel_ring_idle(ring);
 		if (ret)
@@ -3166,7 +3221,6 @@ i915_gem_object_wait_fence(struct drm_i915_gem_object *obj)
 		obj->last_fenced_seqno = 0;
 	}
 
-	obj->fenced_gpu_access = false;
 	return 0;
 }
 
@@ -3273,6 +3327,9 @@ i915_gem_object_get_fence(struct drm_i915_gem_object *obj)
 			return 0;
 		}
 	} else if (enable) {
+		if (WARN_ON(!obj->map_and_fenceable))
+			return -EINVAL;
+
 		reg = i915_find_fence_reg(dev);
 		if (IS_ERR(reg))
 			return PTR_ERR(reg);
@@ -3294,17 +3351,20 @@ i915_gem_object_get_fence(struct drm_i915_gem_object *obj)
 	return 0;
 }
 
-static bool i915_gem_valid_gtt_space(struct drm_device *dev,
-				     struct drm_mm_node *gtt_space,
+static bool i915_gem_valid_gtt_space(struct i915_vma *vma,
 				     unsigned long cache_level)
 {
+	struct drm_mm_node *gtt_space = &vma->node;
 	struct drm_mm_node *other;
 
-	/* On non-LLC machines we have to be careful when putting differing
-	 * types of snoopable memory together to avoid the prefetcher
-	 * crossing memory domains and dying.
+	/*
+	 * On some machines we have to be careful when putting differing types
+	 * of snoopable memory together to avoid the prefetcher crossing memory
+	 * domains and dying. During vm initialisation, we decide whether or not
+	 * these constraints apply and set the drm_mm.color_adjust
+	 * appropriately.
 	 */
-	if (HAS_LLC(dev))
+	if (vma->vm->mm.color_adjust == NULL)
 		return true;
 
 	if (!drm_mm_node_allocated(gtt_space))
@@ -3442,8 +3502,7 @@ search_free:
 
 		goto err_free_vma;
 	}
-	if (WARN_ON(!i915_gem_valid_gtt_space(dev, &vma->node,
-					      obj->cache_level))) {
+	if (WARN_ON(!i915_gem_valid_gtt_space(vma, obj->cache_level))) {
 		ret = -EINVAL;
 		goto err_remove_node;
 	}
@@ -3583,11 +3642,12 @@ int
 i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write)
 {
 	struct drm_i915_private *dev_priv = obj->base.dev->dev_private;
+	struct i915_vma *vma = i915_gem_obj_to_ggtt(obj);
 	uint32_t old_write_domain, old_read_domains;
 	int ret;
 
 	/* Not valid to be called on unbound objects. */
-	if (!i915_gem_obj_bound_any(obj))
+	if (vma == NULL)
 		return -EINVAL;
 
 	if (obj->base.write_domain == I915_GEM_DOMAIN_GTT)
@@ -3629,13 +3689,9 @@ i915_gem_object_set_to_gtt_domain(struct drm_i915_gem_object *obj, bool write)
 					    old_write_domain);
 
 	/* And bump the LRU for this access */
-	if (i915_gem_object_is_inactive(obj)) {
-		struct i915_vma *vma = i915_gem_obj_to_ggtt(obj);
-		if (vma)
-			list_move_tail(&vma->mm_list,
-				       &dev_priv->gtt.base.inactive_list);
-
-	}
+	if (i915_gem_object_is_inactive(obj))
+		list_move_tail(&vma->mm_list,
+			       &dev_priv->gtt.base.inactive_list);
 
 	return 0;
 }
@@ -3656,7 +3712,7 @@ int i915_gem_object_set_cache_level(struct drm_i915_gem_object *obj,
 	}
 
 	list_for_each_entry_safe(vma, next, &obj->vma_list, vma_link) {
-		if (!i915_gem_valid_gtt_space(dev, &vma->node, cache_level)) {
+		if (!i915_gem_valid_gtt_space(vma, cache_level)) {
 			ret = i915_vma_unbind(vma);
 			if (ret)
 				return ret;
@@ -3799,9 +3855,6 @@ static bool is_pin_display(struct drm_i915_gem_object *obj)
 {
 	struct i915_vma *vma;
 
-	if (list_empty(&obj->vma_list))
-		return false;
-
 	vma = i915_gem_obj_to_ggtt(obj);
 	if (!vma)
 		return false;
@@ -4328,8 +4381,6 @@ void i915_gem_object_init(struct drm_i915_gem_object *obj,
 
 	obj->fence_reg = I915_FENCE_REG_NONE;
 	obj->madv = I915_MADV_WILLNEED;
-	/* Avoid an unnecessary call to unbind on the first bind. */
-	obj->map_and_fenceable = true;
 
 	i915_gem_info_add_obj(obj->base.dev->dev_private, obj->base.size);
 }
@@ -4490,12 +4541,18 @@ struct i915_vma *i915_gem_obj_to_vma(struct drm_i915_gem_object *obj,
 
 void i915_gem_vma_destroy(struct i915_vma *vma)
 {
+	struct i915_address_space *vm = NULL;
 	WARN_ON(vma->node.allocated);
 
 	/* Keep the vma as a placeholder in the execbuffer reservation lists */
 	if (!list_empty(&vma->exec_list))
 		return;
 
+	vm = vma->vm;
+
+	if (!i915_is_ggtt(vm))
+		i915_ppgtt_put(i915_vm_to_ppgtt(vm));
+
 	list_del(&vma->vma_link);
 
 	kfree(vma);
@@ -4509,7 +4566,7 @@ i915_gem_stop_ringbuffers(struct drm_device *dev)
 	int i;
 
 	for_each_ring(ring, dev_priv, i)
-		intel_stop_ring_buffer(ring);
+		dev_priv->gt.stop_ring(ring);
 }
 
 int
@@ -4626,11 +4683,46 @@ intel_enable_blt(struct drm_device *dev)
 	return true;
 }
 
-static int i915_gem_init_rings(struct drm_device *dev)
+static void init_unused_ring(struct drm_device *dev, u32 base)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE(RING_CTL(base), 0);
+	I915_WRITE(RING_HEAD(base), 0);
+	I915_WRITE(RING_TAIL(base), 0);
+	I915_WRITE(RING_START(base), 0);
+}
+
+static void init_unused_rings(struct drm_device *dev)
+{
+	if (IS_I830(dev)) {
+		init_unused_ring(dev, PRB1_BASE);
+		init_unused_ring(dev, SRB0_BASE);
+		init_unused_ring(dev, SRB1_BASE);
+		init_unused_ring(dev, SRB2_BASE);
+		init_unused_ring(dev, SRB3_BASE);
+	} else if (IS_GEN2(dev)) {
+		init_unused_ring(dev, SRB0_BASE);
+		init_unused_ring(dev, SRB1_BASE);
+	} else if (IS_GEN3(dev)) {
+		init_unused_ring(dev, PRB1_BASE);
+		init_unused_ring(dev, PRB2_BASE);
+	}
+}
+
+int i915_gem_init_rings(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
+	/*
+	 * At least 830 can leave some of the unused rings
+	 * "active" (ie. head != tail) after resume which
+	 * will prevent c3 entry. Makes sure all unused rings
+	 * are totally idle.
+	 */
+	init_unused_rings(dev);
+
 	ret = intel_init_render_ring_buffer(dev);
 	if (ret)
 		return ret;
@@ -4709,7 +4801,7 @@ i915_gem_init_hw(struct drm_device *dev)
 
 	i915_gem_init_swizzling(dev);
 
-	ret = i915_gem_init_rings(dev);
+	ret = dev_priv->gt.init_rings(dev);
 	if (ret)
 		return ret;
 
@@ -4727,6 +4819,14 @@ i915_gem_init_hw(struct drm_device *dev)
 	if (ret && ret != -EIO) {
 		DRM_ERROR("Context enable failed %d\n", ret);
 		i915_gem_cleanup_ringbuffer(dev);
+
+		return ret;
+	}
+
+	ret = i915_ppgtt_init_hw(dev);
+	if (ret && ret != -EIO) {
+		DRM_ERROR("PPGTT enable failed %d\n", ret);
+		i915_gem_cleanup_ringbuffer(dev);
 	}
 
 	return ret;
@@ -4737,6 +4837,9 @@ int i915_gem_init(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
+	i915.enable_execlists = intel_sanitize_enable_execlists(dev,
+			i915.enable_execlists);
+
 	mutex_lock(&dev->struct_mutex);
 
 	if (IS_VALLEYVIEW(dev)) {
@@ -4747,7 +4850,24 @@ int i915_gem_init(struct drm_device *dev)
 			DRM_DEBUG_DRIVER("allow wake ack timed out\n");
 	}
 
-	i915_gem_init_userptr(dev);
+	if (!i915.enable_execlists) {
+		dev_priv->gt.do_execbuf = i915_gem_ringbuffer_submission;
+		dev_priv->gt.init_rings = i915_gem_init_rings;
+		dev_priv->gt.cleanup_ring = intel_cleanup_ring_buffer;
+		dev_priv->gt.stop_ring = intel_stop_ring_buffer;
+	} else {
+		dev_priv->gt.do_execbuf = intel_execlists_submission;
+		dev_priv->gt.init_rings = intel_logical_rings_init;
+		dev_priv->gt.cleanup_ring = intel_logical_ring_cleanup;
+		dev_priv->gt.stop_ring = intel_logical_ring_stop;
+	}
+
+	ret = i915_gem_init_userptr(dev);
+	if (ret) {
+		mutex_unlock(&dev->struct_mutex);
+		return ret;
+	}
+
 	i915_gem_init_global_gtt(dev);
 
 	ret = i915_gem_context_init(dev);
@@ -4782,7 +4902,7 @@ i915_gem_cleanup_ringbuffer(struct drm_device *dev)
 	int i;
 
 	for_each_ring(ring, dev_priv, i)
-		intel_cleanup_ring_buffer(ring);
+		dev_priv->gt.cleanup_ring(ring);
 }
 
 int
@@ -5094,9 +5214,7 @@ unsigned long i915_gem_obj_offset(struct drm_i915_gem_object *o,
 	struct drm_i915_private *dev_priv = o->base.dev->dev_private;
 	struct i915_vma *vma;
 
-	if (!dev_priv->mm.aliasing_ppgtt ||
-	    vm == &dev_priv->mm.aliasing_ppgtt->base)
-		vm = &dev_priv->gtt.base;
+	WARN_ON(vm == &dev_priv->mm.aliasing_ppgtt->base);
 
 	list_for_each_entry(vma, &o->vma_list, vma_link) {
 		if (vma->vm == vm)
@@ -5137,9 +5255,7 @@ unsigned long i915_gem_obj_size(struct drm_i915_gem_object *o,
 	struct drm_i915_private *dev_priv = o->base.dev->dev_private;
 	struct i915_vma *vma;
 
-	if (!dev_priv->mm.aliasing_ppgtt ||
-	    vm == &dev_priv->mm.aliasing_ppgtt->base)
-		vm = &dev_priv->gtt.base;
+	WARN_ON(vm == &dev_priv->mm.aliasing_ppgtt->base);
 
 	BUG_ON(list_empty(&o->vma_list));
 
@@ -5162,11 +5278,16 @@ i915_gem_shrinker_scan(struct shrinker *shrinker, struct shrink_control *sc)
 	if (!i915_gem_shrinker_lock(dev, &unlock))
 		return SHRINK_STOP;
 
-	freed = i915_gem_purge(dev_priv, sc->nr_to_scan);
+	freed = i915_gem_shrink(dev_priv,
+				sc->nr_to_scan,
+				I915_SHRINK_BOUND |
+				I915_SHRINK_UNBOUND |
+				I915_SHRINK_PURGEABLE);
 	if (freed < sc->nr_to_scan)
-		freed += __i915_gem_shrink(dev_priv,
-					   sc->nr_to_scan - freed,
-					   false);
+		freed += i915_gem_shrink(dev_priv,
+					 sc->nr_to_scan - freed,
+					 I915_SHRINK_BOUND |
+					 I915_SHRINK_UNBOUND);
 	if (unlock)
 		mutex_unlock(&dev->struct_mutex);
 
@@ -5244,14 +5365,8 @@ struct i915_vma *i915_gem_obj_to_ggtt(struct drm_i915_gem_object *obj)
 {
 	struct i915_vma *vma;
 
-	/* This WARN has probably outlived its usefulness (callers already
-	 * WARN if they don't find the GGTT vma they expect). When removing,
-	 * remember to remove the pre-check in is_pin_display() as well */
-	if (WARN_ON(list_empty(&obj->vma_list)))
-		return NULL;
-
 	vma = list_first_entry(&obj->vma_list, typeof(*vma), vma_link);
-	if (vma->vm != obj_to_ggtt(obj))
+	if (vma->vm != i915_obj_to_ggtt(obj))
 		return NULL;
 
 	return vma;
diff --git a/drivers/gpu/drm/i915/i915_gem_context.c b/drivers/gpu/drm/i915/i915_gem_context.c
index 3b99390e467a..a5221d8f1580 100644
--- a/drivers/gpu/drm/i915/i915_gem_context.c
+++ b/drivers/gpu/drm/i915/i915_gem_context.c
@@ -96,50 +96,6 @@
 #define GEN6_CONTEXT_ALIGN (64<<10)
 #define GEN7_CONTEXT_ALIGN 4096
 
-static void do_ppgtt_cleanup(struct i915_hw_ppgtt *ppgtt)
-{
-	struct drm_device *dev = ppgtt->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct i915_address_space *vm = &ppgtt->base;
-
-	if (ppgtt == dev_priv->mm.aliasing_ppgtt ||
-	    (list_empty(&vm->active_list) && list_empty(&vm->inactive_list))) {
-		ppgtt->base.cleanup(&ppgtt->base);
-		return;
-	}
-
-	/*
-	 * Make sure vmas are unbound before we take down the drm_mm
-	 *
-	 * FIXME: Proper refcounting should take care of this, this shouldn't be
-	 * needed at all.
-	 */
-	if (!list_empty(&vm->active_list)) {
-		struct i915_vma *vma;
-
-		list_for_each_entry(vma, &vm->active_list, mm_list)
-			if (WARN_ON(list_empty(&vma->vma_link) ||
-				    list_is_singular(&vma->vma_link)))
-				break;
-
-		i915_gem_evict_vm(&ppgtt->base, true);
-	} else {
-		i915_gem_retire_requests(dev);
-		i915_gem_evict_vm(&ppgtt->base, false);
-	}
-
-	ppgtt->base.cleanup(&ppgtt->base);
-}
-
-static void ppgtt_release(struct kref *kref)
-{
-	struct i915_hw_ppgtt *ppgtt =
-		container_of(kref, struct i915_hw_ppgtt, ref);
-
-	do_ppgtt_cleanup(ppgtt);
-	kfree(ppgtt);
-}
-
 static size_t get_context_alignment(struct drm_device *dev)
 {
 	if (IS_GEN6(dev))
@@ -179,24 +135,20 @@ static int get_context_size(struct drm_device *dev)
 void i915_gem_context_free(struct kref *ctx_ref)
 {
 	struct intel_context *ctx = container_of(ctx_ref,
-						   typeof(*ctx), ref);
-	struct i915_hw_ppgtt *ppgtt = NULL;
+						 typeof(*ctx), ref);
 
-	if (ctx->legacy_hw_ctx.rcs_state) {
-		/* We refcount even the aliasing PPGTT to keep the code symmetric */
-		if (USES_PPGTT(ctx->legacy_hw_ctx.rcs_state->base.dev))
-			ppgtt = ctx_to_ppgtt(ctx);
-	}
+	if (i915.enable_execlists)
+		intel_lr_context_free(ctx);
+
+	i915_ppgtt_put(ctx->ppgtt);
 
-	if (ppgtt)
-		kref_put(&ppgtt->ref, ppgtt_release);
 	if (ctx->legacy_hw_ctx.rcs_state)
 		drm_gem_object_unreference(&ctx->legacy_hw_ctx.rcs_state->base);
 	list_del(&ctx->link);
 	kfree(ctx);
 }
 
-static struct drm_i915_gem_object *
+struct drm_i915_gem_object *
 i915_gem_alloc_context_obj(struct drm_device *dev, size_t size)
 {
 	struct drm_i915_gem_object *obj;
@@ -226,29 +178,9 @@ i915_gem_alloc_context_obj(struct drm_device *dev, size_t size)
 	return obj;
 }
 
-static struct i915_hw_ppgtt *
-create_vm_for_ctx(struct drm_device *dev, struct intel_context *ctx)
-{
-	struct i915_hw_ppgtt *ppgtt;
-	int ret;
-
-	ppgtt = kzalloc(sizeof(*ppgtt), GFP_KERNEL);
-	if (!ppgtt)
-		return ERR_PTR(-ENOMEM);
-
-	ret = i915_gem_init_ppgtt(dev, ppgtt);
-	if (ret) {
-		kfree(ppgtt);
-		return ERR_PTR(ret);
-	}
-
-	ppgtt->ctx = ctx;
-	return ppgtt;
-}
-
 static struct intel_context *
 __create_hw_context(struct drm_device *dev,
-		  struct drm_i915_file_private *file_priv)
+		    struct drm_i915_file_private *file_priv)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_context *ctx;
@@ -301,11 +233,9 @@ err_out:
  */
 static struct intel_context *
 i915_gem_create_context(struct drm_device *dev,
-			struct drm_i915_file_private *file_priv,
-			bool create_vm)
+			struct drm_i915_file_private *file_priv)
 {
 	const bool is_global_default_ctx = file_priv == NULL;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_context *ctx;
 	int ret = 0;
 
@@ -331,34 +261,18 @@ i915_gem_create_context(struct drm_device *dev,
 		}
 	}
 
-	if (create_vm) {
-		struct i915_hw_ppgtt *ppgtt = create_vm_for_ctx(dev, ctx);
+	if (USES_FULL_PPGTT(dev)) {
+		struct i915_hw_ppgtt *ppgtt = i915_ppgtt_create(dev, file_priv);
 
 		if (IS_ERR_OR_NULL(ppgtt)) {
 			DRM_DEBUG_DRIVER("PPGTT setup failed (%ld)\n",
 					 PTR_ERR(ppgtt));
 			ret = PTR_ERR(ppgtt);
 			goto err_unpin;
-		} else
-			ctx->vm = &ppgtt->base;
-
-		/* This case is reserved for the global default context and
-		 * should only happen once. */
-		if (is_global_default_ctx) {
-			if (WARN_ON(dev_priv->mm.aliasing_ppgtt)) {
-				ret = -EEXIST;
-				goto err_unpin;
-			}
-
-			dev_priv->mm.aliasing_ppgtt = ppgtt;
 		}
-	} else if (USES_PPGTT(dev)) {
-		/* For platforms which only have aliasing PPGTT, we fake the
-		 * address space and refcounting. */
-		ctx->vm = &dev_priv->mm.aliasing_ppgtt->base;
-		kref_get(&dev_priv->mm.aliasing_ppgtt->ref);
-	} else
-		ctx->vm = &dev_priv->gtt.base;
+
+		ctx->ppgtt = ppgtt;
+	}
 
 	return ctx;
 
@@ -375,34 +289,23 @@ void i915_gem_context_reset(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int i;
 
-	/* Prevent the hardware from restoring the last context (which hung) on
-	 * the next switch */
+	/* In execlists mode we will unreference the context when the execlist
+	 * queue is cleared and the requests destroyed.
+	 */
+	if (i915.enable_execlists)
+		return;
+
 	for (i = 0; i < I915_NUM_RINGS; i++) {
 		struct intel_engine_cs *ring = &dev_priv->ring[i];
-		struct intel_context *dctx = ring->default_context;
 		struct intel_context *lctx = ring->last_context;
 
-		/* Do a fake switch to the default context */
-		if (lctx == dctx)
-			continue;
-
-		if (!lctx)
-			continue;
+		if (lctx) {
+			if (lctx->legacy_hw_ctx.rcs_state && i == RCS)
+				i915_gem_object_ggtt_unpin(lctx->legacy_hw_ctx.rcs_state);
 
-		if (dctx->legacy_hw_ctx.rcs_state && i == RCS) {
-			WARN_ON(i915_gem_obj_ggtt_pin(dctx->legacy_hw_ctx.rcs_state,
-						      get_context_alignment(dev), 0));
-			/* Fake a finish/inactive */
-			dctx->legacy_hw_ctx.rcs_state->base.write_domain = 0;
-			dctx->legacy_hw_ctx.rcs_state->active = 0;
+			i915_gem_context_unreference(lctx);
+			ring->last_context = NULL;
 		}
-
-		if (lctx->legacy_hw_ctx.rcs_state && i == RCS)
-			i915_gem_object_ggtt_unpin(lctx->legacy_hw_ctx.rcs_state);
-
-		i915_gem_context_unreference(lctx);
-		i915_gem_context_reference(dctx);
-		ring->last_context = dctx;
 	}
 }
 
@@ -417,7 +320,11 @@ int i915_gem_context_init(struct drm_device *dev)
 	if (WARN_ON(dev_priv->ring[RCS].default_context))
 		return 0;
 
-	if (HAS_HW_CONTEXTS(dev)) {
+	if (i915.enable_execlists) {
+		/* NB: intentionally left blank. We will allocate our own
+		 * backing objects as we need them, thank you very much */
+		dev_priv->hw_context_size = 0;
+	} else if (HAS_HW_CONTEXTS(dev)) {
 		dev_priv->hw_context_size = round_up(get_context_size(dev), 4096);
 		if (dev_priv->hw_context_size > (1<<20)) {
 			DRM_DEBUG_DRIVER("Disabling HW Contexts; invalid size %d\n",
@@ -426,18 +333,23 @@ int i915_gem_context_init(struct drm_device *dev)
 		}
 	}
 
-	ctx = i915_gem_create_context(dev, NULL, USES_PPGTT(dev));
+	ctx = i915_gem_create_context(dev, NULL);
 	if (IS_ERR(ctx)) {
 		DRM_ERROR("Failed to create default global context (error %ld)\n",
 			  PTR_ERR(ctx));
 		return PTR_ERR(ctx);
 	}
 
-	/* NB: RCS will hold a ref for all rings */
-	for (i = 0; i < I915_NUM_RINGS; i++)
-		dev_priv->ring[i].default_context = ctx;
+	for (i = 0; i < I915_NUM_RINGS; i++) {
+		struct intel_engine_cs *ring = &dev_priv->ring[i];
 
-	DRM_DEBUG_DRIVER("%s context support initialized\n", dev_priv->hw_context_size ? "HW" : "fake");
+		/* NB: RCS will hold a ref for all rings */
+		ring->default_context = ctx;
+	}
+
+	DRM_DEBUG_DRIVER("%s context support initialized\n",
+			i915.enable_execlists ? "LR" :
+			dev_priv->hw_context_size ? "HW" : "fake");
 	return 0;
 }
 
@@ -489,19 +401,11 @@ int i915_gem_context_enable(struct drm_i915_private *dev_priv)
 	struct intel_engine_cs *ring;
 	int ret, i;
 
-	/* This is the only place the aliasing PPGTT gets enabled, which means
-	 * it has to happen before we bail on reset */
-	if (dev_priv->mm.aliasing_ppgtt) {
-		struct i915_hw_ppgtt *ppgtt = dev_priv->mm.aliasing_ppgtt;
-		ppgtt->enable(ppgtt);
-	}
+	BUG_ON(!dev_priv->ring[RCS].default_context);
 
-	/* FIXME: We should make this work, even in reset */
-	if (i915_reset_in_progress(&dev_priv->gpu_error))
+	if (i915.enable_execlists)
 		return 0;
 
-	BUG_ON(!dev_priv->ring[RCS].default_context);
-
 	for_each_ring(ring, dev_priv, i) {
 		ret = i915_switch_context(ring, ring->default_context);
 		if (ret)
@@ -527,7 +431,7 @@ int i915_gem_context_open(struct drm_device *dev, struct drm_file *file)
 	idr_init(&file_priv->context_idr);
 
 	mutex_lock(&dev->struct_mutex);
-	ctx = i915_gem_create_context(dev, file_priv, USES_FULL_PPGTT(dev));
+	ctx = i915_gem_create_context(dev, file_priv);
 	mutex_unlock(&dev->struct_mutex);
 
 	if (IS_ERR(ctx)) {
@@ -563,6 +467,7 @@ mi_set_context(struct intel_engine_cs *ring,
 	       struct intel_context *new_context,
 	       u32 hw_flags)
 {
+	u32 flags = hw_flags | MI_MM_SPACE_GTT;
 	int ret;
 
 	/* w/a: If Flush TLB Invalidation Mode is enabled, driver must do a TLB
@@ -576,6 +481,10 @@ mi_set_context(struct intel_engine_cs *ring,
 			return ret;
 	}
 
+	/* These flags are for resource streamer on HSW+ */
+	if (!IS_HASWELL(ring->dev) && INTEL_INFO(ring->dev)->gen < 8)
+		flags |= (MI_SAVE_EXT_STATE_EN | MI_RESTORE_EXT_STATE_EN);
+
 	ret = intel_ring_begin(ring, 6);
 	if (ret)
 		return ret;
@@ -589,10 +498,7 @@ mi_set_context(struct intel_engine_cs *ring,
 	intel_ring_emit(ring, MI_NOOP);
 	intel_ring_emit(ring, MI_SET_CONTEXT);
 	intel_ring_emit(ring, i915_gem_obj_ggtt_offset(new_context->legacy_hw_ctx.rcs_state) |
-			MI_MM_SPACE_GTT |
-			MI_SAVE_EXT_STATE_EN |
-			MI_RESTORE_EXT_STATE_EN |
-			hw_flags);
+			flags);
 	/*
 	 * w/a: MI_SET_CONTEXT must always be followed by MI_NOOP
 	 * WaMiSetContext_Hang:snb,ivb,vlv
@@ -614,7 +520,6 @@ static int do_switch(struct intel_engine_cs *ring,
 {
 	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 	struct intel_context *from = ring->last_context;
-	struct i915_hw_ppgtt *ppgtt = ctx_to_ppgtt(to);
 	u32 hw_flags = 0;
 	bool uninitialized = false;
 	int ret, i;
@@ -642,8 +547,8 @@ static int do_switch(struct intel_engine_cs *ring,
 	 */
 	from = ring->last_context;
 
-	if (USES_FULL_PPGTT(ring->dev)) {
-		ret = ppgtt->switch_mm(ppgtt, ring, false);
+	if (to->ppgtt) {
+		ret = to->ppgtt->switch_mm(to->ppgtt, ring);
 		if (ret)
 			goto unpin_out;
 	}
@@ -723,6 +628,12 @@ done:
 	ring->last_context = to;
 
 	if (uninitialized) {
+		if (ring->init_context) {
+			ret = ring->init_context(ring);
+			if (ret)
+				DRM_ERROR("ring init context: %d\n", ret);
+		}
+
 		ret = i915_gem_render_state_init(ring);
 		if (ret)
 			DRM_ERROR("init render state: %d\n", ret);
@@ -743,14 +654,19 @@ unpin_out:
  *
  * The context life cycle is simple. The context refcount is incremented and
  * decremented by 1 and create and destroy. If the context is in use by the GPU,
- * it will have a refoucnt > 1. This allows us to destroy the context abstract
+ * it will have a refcount > 1. This allows us to destroy the context abstract
  * object while letting the normal object tracking destroy the backing BO.
+ *
+ * This function should not be used in execlists mode.  Instead the context is
+ * switched by writing to the ELSP and requests keep a reference to their
+ * context.
  */
 int i915_switch_context(struct intel_engine_cs *ring,
 			struct intel_context *to)
 {
 	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 
+	WARN_ON(i915.enable_execlists);
 	WARN_ON(!mutex_is_locked(&dev_priv->dev->struct_mutex));
 
 	if (to->legacy_hw_ctx.rcs_state == NULL) { /* We have the fake context */
@@ -766,9 +682,9 @@ int i915_switch_context(struct intel_engine_cs *ring,
 	return do_switch(ring, to);
 }
 
-static bool hw_context_enabled(struct drm_device *dev)
+static bool contexts_enabled(struct drm_device *dev)
 {
-	return to_i915(dev)->hw_context_size;
+	return i915.enable_execlists || to_i915(dev)->hw_context_size;
 }
 
 int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
@@ -779,14 +695,14 @@ int i915_gem_context_create_ioctl(struct drm_device *dev, void *data,
 	struct intel_context *ctx;
 	int ret;
 
-	if (!hw_context_enabled(dev))
+	if (!contexts_enabled(dev))
 		return -ENODEV;
 
 	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
 
-	ctx = i915_gem_create_context(dev, file_priv, USES_FULL_PPGTT(dev));
+	ctx = i915_gem_create_context(dev, file_priv);
 	mutex_unlock(&dev->struct_mutex);
 	if (IS_ERR(ctx))
 		return PTR_ERR(ctx);
diff --git a/drivers/gpu/drm/i915/i915_gem_evict.c b/drivers/gpu/drm/i915/i915_gem_evict.c
index bbf4b12d842e..886ff2ee7a28 100644
--- a/drivers/gpu/drm/i915/i915_gem_evict.c
+++ b/drivers/gpu/drm/i915/i915_gem_evict.c
@@ -243,7 +243,7 @@ int
 i915_gem_evict_everything(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct i915_address_space *vm;
+	struct i915_address_space *vm, *v;
 	bool lists_empty = true;
 	int ret;
 
@@ -270,7 +270,7 @@ i915_gem_evict_everything(struct drm_device *dev)
 	i915_gem_retire_requests(dev);
 
 	/* Having flushed everything, unbind() should never raise an error */
-	list_for_each_entry(vm, &dev_priv->vm_list, global_link)
+	list_for_each_entry_safe(vm, v, &dev_priv->vm_list, global_link)
 		WARN_ON(i915_gem_evict_vm(vm, false));
 
 	return 0;
diff --git a/drivers/gpu/drm/i915/i915_gem_execbuffer.c b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
index 60998fc4e5b2..1a0611bb576b 100644
--- a/drivers/gpu/drm/i915/i915_gem_execbuffer.c
+++ b/drivers/gpu/drm/i915/i915_gem_execbuffer.c
@@ -35,6 +35,7 @@
 
 #define  __EXEC_OBJECT_HAS_PIN (1<<31)
 #define  __EXEC_OBJECT_HAS_FENCE (1<<30)
+#define  __EXEC_OBJECT_NEEDS_MAP (1<<29)
 #define  __EXEC_OBJECT_NEEDS_BIAS (1<<28)
 
 #define BATCH_OFFSET_BIAS (256*1024)
@@ -94,7 +95,6 @@ eb_lookup_vmas(struct eb_vmas *eb,
 	       struct i915_address_space *vm,
 	       struct drm_file *file)
 {
-	struct drm_i915_private *dev_priv = vm->dev->dev_private;
 	struct drm_i915_gem_object *obj;
 	struct list_head objects;
 	int i, ret;
@@ -129,20 +129,6 @@ eb_lookup_vmas(struct eb_vmas *eb,
 	i = 0;
 	while (!list_empty(&objects)) {
 		struct i915_vma *vma;
-		struct i915_address_space *bind_vm = vm;
-
-		if (exec[i].flags & EXEC_OBJECT_NEEDS_GTT &&
-		    USES_FULL_PPGTT(vm->dev)) {
-			ret = -EINVAL;
-			goto err;
-		}
-
-		/* If we have secure dispatch, or the userspace assures us that
-		 * they know what they're doing, use the GGTT VM.
-		 */
-		if (((args->flags & I915_EXEC_SECURE) &&
-		    (i == (args->buffer_count - 1))))
-			bind_vm = &dev_priv->gtt.base;
 
 		obj = list_first_entry(&objects,
 				       struct drm_i915_gem_object,
@@ -156,7 +142,7 @@ eb_lookup_vmas(struct eb_vmas *eb,
 		 * from the (obj, vm) we don't run the risk of creating
 		 * duplicated vmas for the same vm.
 		 */
-		vma = i915_gem_obj_lookup_or_create_vma(obj, bind_vm);
+		vma = i915_gem_obj_lookup_or_create_vma(obj, vm);
 		if (IS_ERR(vma)) {
 			DRM_DEBUG("Failed to lookup VMA\n");
 			ret = PTR_ERR(vma);
@@ -307,7 +293,7 @@ relocate_entry_gtt(struct drm_i915_gem_object *obj,
 	struct drm_device *dev = obj->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	uint64_t delta = reloc->delta + target_offset;
-	uint32_t __iomem *reloc_entry;
+	uint64_t offset;
 	void __iomem *reloc_page;
 	int ret;
 
@@ -320,25 +306,24 @@ relocate_entry_gtt(struct drm_i915_gem_object *obj,
 		return ret;
 
 	/* Map the page containing the relocation we're going to perform.  */
-	reloc->offset += i915_gem_obj_ggtt_offset(obj);
+	offset = i915_gem_obj_ggtt_offset(obj);
+	offset += reloc->offset;
 	reloc_page = io_mapping_map_atomic_wc(dev_priv->gtt.mappable,
-			reloc->offset & PAGE_MASK);
-	reloc_entry = (uint32_t __iomem *)
-		(reloc_page + offset_in_page(reloc->offset));
-	iowrite32(lower_32_bits(delta), reloc_entry);
+					      offset & PAGE_MASK);
+	iowrite32(lower_32_bits(delta), reloc_page + offset_in_page(offset));
 
 	if (INTEL_INFO(dev)->gen >= 8) {
-		reloc_entry += 1;
+		offset += sizeof(uint32_t);
 
-		if (offset_in_page(reloc->offset + sizeof(uint32_t)) == 0) {
+		if (offset_in_page(offset) == 0) {
 			io_mapping_unmap_atomic(reloc_page);
-			reloc_page = io_mapping_map_atomic_wc(
-					dev_priv->gtt.mappable,
-					reloc->offset + sizeof(uint32_t));
-			reloc_entry = reloc_page;
+			reloc_page =
+				io_mapping_map_atomic_wc(dev_priv->gtt.mappable,
+							 offset);
 		}
 
-		iowrite32(upper_32_bits(delta), reloc_entry);
+		iowrite32(upper_32_bits(delta),
+			  reloc_page + offset_in_page(offset));
 	}
 
 	io_mapping_unmap_atomic(reloc_page);
@@ -535,34 +520,18 @@ i915_gem_execbuffer_relocate(struct eb_vmas *eb)
 }
 
 static int
-need_reloc_mappable(struct i915_vma *vma)
-{
-	struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
-	return entry->relocation_count && !use_cpu_reloc(vma->obj) &&
-		i915_is_ggtt(vma->vm);
-}
-
-static int
 i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 				struct intel_engine_cs *ring,
 				bool *need_reloc)
 {
 	struct drm_i915_gem_object *obj = vma->obj;
 	struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
-	bool has_fenced_gpu_access = INTEL_INFO(ring->dev)->gen < 4;
-	bool need_fence;
 	uint64_t flags;
 	int ret;
 
 	flags = 0;
-
-	need_fence =
-		has_fenced_gpu_access &&
-		entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
-		obj->tiling_mode != I915_TILING_NONE;
-	if (need_fence || need_reloc_mappable(vma))
+	if (entry->flags & __EXEC_OBJECT_NEEDS_MAP)
 		flags |= PIN_MAPPABLE;
-
 	if (entry->flags & EXEC_OBJECT_NEEDS_GTT)
 		flags |= PIN_GLOBAL;
 	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS)
@@ -574,17 +543,13 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 
 	entry->flags |= __EXEC_OBJECT_HAS_PIN;
 
-	if (has_fenced_gpu_access) {
-		if (entry->flags & EXEC_OBJECT_NEEDS_FENCE) {
-			ret = i915_gem_object_get_fence(obj);
-			if (ret)
-				return ret;
-
-			if (i915_gem_object_pin_fence(obj))
-				entry->flags |= __EXEC_OBJECT_HAS_FENCE;
+	if (entry->flags & EXEC_OBJECT_NEEDS_FENCE) {
+		ret = i915_gem_object_get_fence(obj);
+		if (ret)
+			return ret;
 
-			obj->pending_fenced_gpu_access = true;
-		}
+		if (i915_gem_object_pin_fence(obj))
+			entry->flags |= __EXEC_OBJECT_HAS_FENCE;
 	}
 
 	if (entry->offset != vma->node.start) {
@@ -601,26 +566,40 @@ i915_gem_execbuffer_reserve_vma(struct i915_vma *vma,
 }
 
 static bool
-eb_vma_misplaced(struct i915_vma *vma, bool has_fenced_gpu_access)
+need_reloc_mappable(struct i915_vma *vma)
 {
 	struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
-	struct drm_i915_gem_object *obj = vma->obj;
-	bool need_fence, need_mappable;
 
-	need_fence =
-		has_fenced_gpu_access &&
-		entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
-		obj->tiling_mode != I915_TILING_NONE;
-	need_mappable = need_fence || need_reloc_mappable(vma);
+	if (entry->relocation_count == 0)
+		return false;
+
+	if (!i915_is_ggtt(vma->vm))
+		return false;
+
+	/* See also use_cpu_reloc() */
+	if (HAS_LLC(vma->obj->base.dev))
+		return false;
 
-	WARN_ON((need_mappable || need_fence) &&
+	if (vma->obj->base.write_domain == I915_GEM_DOMAIN_CPU)
+		return false;
+
+	return true;
+}
+
+static bool
+eb_vma_misplaced(struct i915_vma *vma)
+{
+	struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
+	struct drm_i915_gem_object *obj = vma->obj;
+
+	WARN_ON(entry->flags & __EXEC_OBJECT_NEEDS_MAP &&
 	       !i915_is_ggtt(vma->vm));
 
 	if (entry->alignment &&
 	    vma->node.start & (entry->alignment - 1))
 		return true;
 
-	if (need_mappable && !obj->map_and_fenceable)
+	if (entry->flags & __EXEC_OBJECT_NEEDS_MAP && !obj->map_and_fenceable)
 		return true;
 
 	if (entry->flags & __EXEC_OBJECT_NEEDS_BIAS &&
@@ -642,9 +621,6 @@ i915_gem_execbuffer_reserve(struct intel_engine_cs *ring,
 	bool has_fenced_gpu_access = INTEL_INFO(ring->dev)->gen < 4;
 	int retry;
 
-	if (list_empty(vmas))
-		return 0;
-
 	i915_gem_retire_requests_ring(ring);
 
 	vm = list_first_entry(vmas, struct i915_vma, exec_list)->vm;
@@ -658,20 +634,21 @@ i915_gem_execbuffer_reserve(struct intel_engine_cs *ring,
 		obj = vma->obj;
 		entry = vma->exec_entry;
 
+		if (!has_fenced_gpu_access)
+			entry->flags &= ~EXEC_OBJECT_NEEDS_FENCE;
 		need_fence =
-			has_fenced_gpu_access &&
 			entry->flags & EXEC_OBJECT_NEEDS_FENCE &&
 			obj->tiling_mode != I915_TILING_NONE;
 		need_mappable = need_fence || need_reloc_mappable(vma);
 
-		if (need_mappable)
+		if (need_mappable) {
+			entry->flags |= __EXEC_OBJECT_NEEDS_MAP;
 			list_move(&vma->exec_list, &ordered_vmas);
-		else
+		} else
 			list_move_tail(&vma->exec_list, &ordered_vmas);
 
 		obj->base.pending_read_domains = I915_GEM_GPU_DOMAINS & ~I915_GEM_DOMAIN_COMMAND;
 		obj->base.pending_write_domain = 0;
-		obj->pending_fenced_gpu_access = false;
 	}
 	list_splice(&ordered_vmas, vmas);
 
@@ -696,7 +673,7 @@ i915_gem_execbuffer_reserve(struct intel_engine_cs *ring,
 			if (!drm_mm_node_allocated(&vma->node))
 				continue;
 
-			if (eb_vma_misplaced(vma, has_fenced_gpu_access))
+			if (eb_vma_misplaced(vma))
 				ret = i915_vma_unbind(vma);
 			else
 				ret = i915_gem_execbuffer_reserve_vma(vma, ring, need_relocs);
@@ -744,9 +721,6 @@ i915_gem_execbuffer_relocate_slow(struct drm_device *dev,
 	int i, total, ret;
 	unsigned count = args->buffer_count;
 
-	if (WARN_ON(list_empty(&eb->vmas)))
-		return 0;
-
 	vm = list_first_entry(&eb->vmas, struct i915_vma, exec_list)->vm;
 
 	/* We may process another execbuffer during the unlock... */
@@ -890,18 +864,24 @@ i915_gem_check_execbuffer(struct drm_i915_gem_execbuffer2 *exec)
 }
 
 static int
-validate_exec_list(struct drm_i915_gem_exec_object2 *exec,
+validate_exec_list(struct drm_device *dev,
+		   struct drm_i915_gem_exec_object2 *exec,
 		   int count)
 {
-	int i;
 	unsigned relocs_total = 0;
 	unsigned relocs_max = UINT_MAX / sizeof(struct drm_i915_gem_relocation_entry);
+	unsigned invalid_flags;
+	int i;
+
+	invalid_flags = __EXEC_OBJECT_UNKNOWN_FLAGS;
+	if (USES_FULL_PPGTT(dev))
+		invalid_flags |= EXEC_OBJECT_NEEDS_GTT;
 
 	for (i = 0; i < count; i++) {
 		char __user *ptr = to_user_ptr(exec[i].relocs_ptr);
 		int length; /* limited by fault_in_pages_readable() */
 
-		if (exec[i].flags & __EXEC_OBJECT_UNKNOWN_FLAGS)
+		if (exec[i].flags & invalid_flags)
 			return -EINVAL;
 
 		/* First check for malicious input causing overflow in
@@ -951,16 +931,26 @@ i915_gem_validate_context(struct drm_device *dev, struct drm_file *file,
 		return ERR_PTR(-EIO);
 	}
 
+	if (i915.enable_execlists && !ctx->engine[ring->id].state) {
+		int ret = intel_lr_context_deferred_create(ctx, ring);
+		if (ret) {
+			DRM_DEBUG("Could not create LRC %u: %d\n", ctx_id, ret);
+			return ERR_PTR(ret);
+		}
+	}
+
 	return ctx;
 }
 
-static void
+void
 i915_gem_execbuffer_move_to_active(struct list_head *vmas,
 				   struct intel_engine_cs *ring)
 {
+	u32 seqno = intel_ring_get_seqno(ring);
 	struct i915_vma *vma;
 
 	list_for_each_entry(vma, vmas, exec_list) {
+		struct drm_i915_gem_exec_object2 *entry = vma->exec_entry;
 		struct drm_i915_gem_object *obj = vma->obj;
 		u32 old_read = obj->base.read_domains;
 		u32 old_write = obj->base.write_domain;
@@ -969,24 +959,31 @@ i915_gem_execbuffer_move_to_active(struct list_head *vmas,
 		if (obj->base.write_domain == 0)
 			obj->base.pending_read_domains |= obj->base.read_domains;
 		obj->base.read_domains = obj->base.pending_read_domains;
-		obj->fenced_gpu_access = obj->pending_fenced_gpu_access;
 
 		i915_vma_move_to_active(vma, ring);
 		if (obj->base.write_domain) {
 			obj->dirty = 1;
-			obj->last_write_seqno = intel_ring_get_seqno(ring);
+			obj->last_write_seqno = seqno;
 
 			intel_fb_obj_invalidate(obj, ring);
 
 			/* update for the implicit flush after a batch */
 			obj->base.write_domain &= ~I915_GEM_GPU_DOMAINS;
 		}
+		if (entry->flags & EXEC_OBJECT_NEEDS_FENCE) {
+			obj->last_fenced_seqno = seqno;
+			if (entry->flags & __EXEC_OBJECT_HAS_FENCE) {
+				struct drm_i915_private *dev_priv = to_i915(ring->dev);
+				list_move_tail(&dev_priv->fence_regs[obj->fence_reg].lru_list,
+					       &dev_priv->mm.fence_list);
+			}
+		}
 
 		trace_i915_gem_object_change_domain(obj, old_read, old_write);
 	}
 }
 
-static void
+void
 i915_gem_execbuffer_retire_commands(struct drm_device *dev,
 				    struct drm_file *file,
 				    struct intel_engine_cs *ring,
@@ -1026,14 +1023,14 @@ i915_reset_gen7_sol_offsets(struct drm_device *dev,
 	return 0;
 }
 
-static int
-legacy_ringbuffer_submission(struct drm_device *dev, struct drm_file *file,
-			     struct intel_engine_cs *ring,
-			     struct intel_context *ctx,
-			     struct drm_i915_gem_execbuffer2 *args,
-			     struct list_head *vmas,
-			     struct drm_i915_gem_object *batch_obj,
-			     u64 exec_start, u32 flags)
+int
+i915_gem_ringbuffer_submission(struct drm_device *dev, struct drm_file *file,
+			       struct intel_engine_cs *ring,
+			       struct intel_context *ctx,
+			       struct drm_i915_gem_execbuffer2 *args,
+			       struct list_head *vmas,
+			       struct drm_i915_gem_object *batch_obj,
+			       u64 exec_start, u32 flags)
 {
 	struct drm_clip_rect *cliprects = NULL;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1254,7 +1251,7 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 	if (!i915_gem_check_execbuffer(args))
 		return -EINVAL;
 
-	ret = validate_exec_list(exec, args->buffer_count);
+	ret = validate_exec_list(dev, exec, args->buffer_count);
 	if (ret)
 		return ret;
 
@@ -1318,8 +1315,9 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 
 	i915_gem_context_reference(ctx);
 
-	vm = ctx->vm;
-	if (!USES_FULL_PPGTT(dev))
+	if (ctx->ppgtt)
+		vm = &ctx->ppgtt->base;
+	else
 		vm = &dev_priv->gtt.base;
 
 	eb = eb_create(args);
@@ -1386,25 +1384,36 @@ i915_gem_do_execbuffer(struct drm_device *dev, void *data,
 	/* snb/ivb/vlv conflate the "batch in ppgtt" bit with the "non-secure
 	 * batch" bit. Hence we need to pin secure batches into the global gtt.
 	 * hsw should have this fixed, but bdw mucks it up again. */
-	if (flags & I915_DISPATCH_SECURE &&
-	    !batch_obj->has_global_gtt_mapping) {
-		/* When we have multiple VMs, we'll need to make sure that we
-		 * allocate space first */
-		struct i915_vma *vma = i915_gem_obj_to_ggtt(batch_obj);
-		BUG_ON(!vma);
-		vma->bind_vma(vma, batch_obj->cache_level, GLOBAL_BIND);
-	}
+	if (flags & I915_DISPATCH_SECURE) {
+		/*
+		 * So on first glance it looks freaky that we pin the batch here
+		 * outside of the reservation loop. But:
+		 * - The batch is already pinned into the relevant ppgtt, so we
+		 *   already have the backing storage fully allocated.
+		 * - No other BO uses the global gtt (well contexts, but meh),
+		 *   so we don't really have issues with mutliple objects not
+		 *   fitting due to fragmentation.
+		 * So this is actually safe.
+		 */
+		ret = i915_gem_obj_ggtt_pin(batch_obj, 0, 0);
+		if (ret)
+			goto err;
 
-	if (flags & I915_DISPATCH_SECURE)
 		exec_start += i915_gem_obj_ggtt_offset(batch_obj);
-	else
+	} else
 		exec_start += i915_gem_obj_offset(batch_obj, vm);
 
-	ret = legacy_ringbuffer_submission(dev, file, ring, ctx,
-			args, &eb->vmas, batch_obj, exec_start, flags);
-	if (ret)
-		goto err;
+	ret = dev_priv->gt.do_execbuf(dev, file, ring, ctx, args,
+				      &eb->vmas, batch_obj, exec_start, flags);
 
+	/*
+	 * FIXME: We crucially rely upon the active tracking for the (ppgtt)
+	 * batch vma for correctness. For less ugly and less fragility this
+	 * needs to be adjusted to also track the ggtt batch vma properly as
+	 * active.
+	 */
+	if (flags & I915_DISPATCH_SECURE)
+		i915_gem_object_ggtt_unpin(batch_obj);
 err:
 	/* the request owns the ref now */
 	i915_gem_context_unreference(ctx);
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.c b/drivers/gpu/drm/i915/i915_gem_gtt.c
index 1411613f2174..b672b843fd5e 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.c
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.c
@@ -33,17 +33,6 @@
 static void bdw_setup_private_ppat(struct drm_i915_private *dev_priv);
 static void chv_setup_private_ppat(struct drm_i915_private *dev_priv);
 
-bool intel_enable_ppgtt(struct drm_device *dev, bool full)
-{
-	if (i915.enable_ppgtt == 0)
-		return false;
-
-	if (i915.enable_ppgtt == 1 && full)
-		return false;
-
-	return true;
-}
-
 static int sanitize_enable_ppgtt(struct drm_device *dev, int enable_ppgtt)
 {
 	if (enable_ppgtt == 0 || !HAS_ALIASING_PPGTT(dev))
@@ -78,7 +67,6 @@ static void ppgtt_bind_vma(struct i915_vma *vma,
 			   enum i915_cache_level cache_level,
 			   u32 flags);
 static void ppgtt_unbind_vma(struct i915_vma *vma);
-static int gen8_ppgtt_enable(struct i915_hw_ppgtt *ppgtt);
 
 static inline gen8_gtt_pte_t gen8_pte_encode(dma_addr_t addr,
 					     enum i915_cache_level level,
@@ -216,19 +204,12 @@ static gen6_gtt_pte_t iris_pte_encode(dma_addr_t addr,
 
 /* Broadwell Page Directory Pointer Descriptors */
 static int gen8_write_pdp(struct intel_engine_cs *ring, unsigned entry,
-			   uint64_t val, bool synchronous)
+			   uint64_t val)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 	int ret;
 
 	BUG_ON(entry >= 4);
 
-	if (synchronous) {
-		I915_WRITE(GEN8_RING_PDP_UDW(ring, entry), val >> 32);
-		I915_WRITE(GEN8_RING_PDP_LDW(ring, entry), (u32)val);
-		return 0;
-	}
-
 	ret = intel_ring_begin(ring, 6);
 	if (ret)
 		return ret;
@@ -245,8 +226,7 @@ static int gen8_write_pdp(struct intel_engine_cs *ring, unsigned entry,
 }
 
 static int gen8_mm_switch(struct i915_hw_ppgtt *ppgtt,
-			  struct intel_engine_cs *ring,
-			  bool synchronous)
+			  struct intel_engine_cs *ring)
 {
 	int i, ret;
 
@@ -255,7 +235,7 @@ static int gen8_mm_switch(struct i915_hw_ppgtt *ppgtt,
 
 	for (i = used_pd - 1; i >= 0; i--) {
 		dma_addr_t addr = ppgtt->pd_dma_addr[i];
-		ret = gen8_write_pdp(ring, i, addr, synchronous);
+		ret = gen8_write_pdp(ring, i, addr);
 		if (ret)
 			return ret;
 	}
@@ -403,9 +383,6 @@ static void gen8_ppgtt_cleanup(struct i915_address_space *vm)
 	struct i915_hw_ppgtt *ppgtt =
 		container_of(vm, struct i915_hw_ppgtt, base);
 
-	list_del(&vm->global_link);
-	drm_mm_takedown(&vm->mm);
-
 	gen8_ppgtt_unmap_pages(ppgtt);
 	gen8_ppgtt_free(ppgtt);
 }
@@ -615,7 +592,6 @@ static int gen8_ppgtt_init(struct i915_hw_ppgtt *ppgtt, uint64_t size)
 		kunmap_atomic(pd_vaddr);
 	}
 
-	ppgtt->enable = gen8_ppgtt_enable;
 	ppgtt->switch_mm = gen8_mm_switch;
 	ppgtt->base.clear_range = gen8_ppgtt_clear_range;
 	ppgtt->base.insert_entries = gen8_ppgtt_insert_entries;
@@ -724,29 +700,10 @@ static uint32_t get_pd_offset(struct i915_hw_ppgtt *ppgtt)
 }
 
 static int hsw_mm_switch(struct i915_hw_ppgtt *ppgtt,
-			 struct intel_engine_cs *ring,
-			 bool synchronous)
+			 struct intel_engine_cs *ring)
 {
-	struct drm_device *dev = ppgtt->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	/* If we're in reset, we can assume the GPU is sufficiently idle to
-	 * manually frob these bits. Ideally we could use the ring functions,
-	 * except our error handling makes it quite difficult (can't use
-	 * intel_ring_begin, ring->flush, or intel_ring_advance)
-	 *
-	 * FIXME: We should try not to special case reset
-	 */
-	if (synchronous ||
-	    i915_reset_in_progress(&dev_priv->gpu_error)) {
-		WARN_ON(ppgtt != dev_priv->mm.aliasing_ppgtt);
-		I915_WRITE(RING_PP_DIR_DCLV(ring), PP_DIR_DCLV_2G);
-		I915_WRITE(RING_PP_DIR_BASE(ring), get_pd_offset(ppgtt));
-		POSTING_READ(RING_PP_DIR_BASE(ring));
-		return 0;
-	}
-
 	/* NB: TLBs must be flushed and invalidated before a switch */
 	ret = ring->flush(ring, I915_GEM_GPU_DOMAINS, I915_GEM_GPU_DOMAINS);
 	if (ret)
@@ -768,29 +725,10 @@ static int hsw_mm_switch(struct i915_hw_ppgtt *ppgtt,
 }
 
 static int gen7_mm_switch(struct i915_hw_ppgtt *ppgtt,
-			  struct intel_engine_cs *ring,
-			  bool synchronous)
+			  struct intel_engine_cs *ring)
 {
-	struct drm_device *dev = ppgtt->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	int ret;
 
-	/* If we're in reset, we can assume the GPU is sufficiently idle to
-	 * manually frob these bits. Ideally we could use the ring functions,
-	 * except our error handling makes it quite difficult (can't use
-	 * intel_ring_begin, ring->flush, or intel_ring_advance)
-	 *
-	 * FIXME: We should try not to special case reset
-	 */
-	if (synchronous ||
-	    i915_reset_in_progress(&dev_priv->gpu_error)) {
-		WARN_ON(ppgtt != dev_priv->mm.aliasing_ppgtt);
-		I915_WRITE(RING_PP_DIR_DCLV(ring), PP_DIR_DCLV_2G);
-		I915_WRITE(RING_PP_DIR_BASE(ring), get_pd_offset(ppgtt));
-		POSTING_READ(RING_PP_DIR_BASE(ring));
-		return 0;
-	}
-
 	/* NB: TLBs must be flushed and invalidated before a switch */
 	ret = ring->flush(ring, I915_GEM_GPU_DOMAINS, I915_GEM_GPU_DOMAINS);
 	if (ret)
@@ -819,14 +757,11 @@ static int gen7_mm_switch(struct i915_hw_ppgtt *ppgtt,
 }
 
 static int gen6_mm_switch(struct i915_hw_ppgtt *ppgtt,
-			  struct intel_engine_cs *ring,
-			  bool synchronous)
+			  struct intel_engine_cs *ring)
 {
 	struct drm_device *dev = ppgtt->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (!synchronous)
-		return 0;
 
 	I915_WRITE(RING_PP_DIR_DCLV(ring), PP_DIR_DCLV_2G);
 	I915_WRITE(RING_PP_DIR_BASE(ring), get_pd_offset(ppgtt));
@@ -836,39 +771,20 @@ static int gen6_mm_switch(struct i915_hw_ppgtt *ppgtt,
 	return 0;
 }
 
-static int gen8_ppgtt_enable(struct i915_hw_ppgtt *ppgtt)
+static void gen8_ppgtt_enable(struct drm_device *dev)
 {
-	struct drm_device *dev = ppgtt->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_engine_cs *ring;
-	int j, ret;
+	int j;
 
 	for_each_ring(ring, dev_priv, j) {
 		I915_WRITE(RING_MODE_GEN7(ring),
 			   _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
-
-		/* We promise to do a switch later with FULL PPGTT. If this is
-		 * aliasing, this is the one and only switch we'll do */
-		if (USES_FULL_PPGTT(dev))
-			continue;
-
-		ret = ppgtt->switch_mm(ppgtt, ring, true);
-		if (ret)
-			goto err_out;
 	}
-
-	return 0;
-
-err_out:
-	for_each_ring(ring, dev_priv, j)
-		I915_WRITE(RING_MODE_GEN7(ring),
-			   _MASKED_BIT_DISABLE(GFX_PPGTT_ENABLE));
-	return ret;
 }
 
-static int gen7_ppgtt_enable(struct i915_hw_ppgtt *ppgtt)
+static void gen7_ppgtt_enable(struct drm_device *dev)
 {
-	struct drm_device *dev = ppgtt->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_engine_cs *ring;
 	uint32_t ecochk, ecobits;
@@ -887,31 +803,16 @@ static int gen7_ppgtt_enable(struct i915_hw_ppgtt *ppgtt)
 	I915_WRITE(GAM_ECOCHK, ecochk);
 
 	for_each_ring(ring, dev_priv, i) {
-		int ret;
 		/* GFX_MODE is per-ring on gen7+ */
 		I915_WRITE(RING_MODE_GEN7(ring),
 			   _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
-
-		/* We promise to do a switch later with FULL PPGTT. If this is
-		 * aliasing, this is the one and only switch we'll do */
-		if (USES_FULL_PPGTT(dev))
-			continue;
-
-		ret = ppgtt->switch_mm(ppgtt, ring, true);
-		if (ret)
-			return ret;
 	}
-
-	return 0;
 }
 
-static int gen6_ppgtt_enable(struct i915_hw_ppgtt *ppgtt)
+static void gen6_ppgtt_enable(struct drm_device *dev)
 {
-	struct drm_device *dev = ppgtt->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_engine_cs *ring;
 	uint32_t ecochk, gab_ctl, ecobits;
-	int i;
 
 	ecobits = I915_READ(GAC_ECO_BITS);
 	I915_WRITE(GAC_ECO_BITS, ecobits | ECOBITS_SNB_BIT |
@@ -924,14 +825,6 @@ static int gen6_ppgtt_enable(struct i915_hw_ppgtt *ppgtt)
 	I915_WRITE(GAM_ECOCHK, ecochk | ECOCHK_SNB_BIT | ECOCHK_PPGTT_CACHE64B);
 
 	I915_WRITE(GFX_MODE, _MASKED_BIT_ENABLE(GFX_PPGTT_ENABLE));
-
-	for_each_ring(ring, dev_priv, i) {
-		int ret = ppgtt->switch_mm(ppgtt, ring, true);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
 }
 
 /* PPGTT support for Sandybdrige/Gen6 and later */
@@ -1029,8 +922,6 @@ static void gen6_ppgtt_cleanup(struct i915_address_space *vm)
 	struct i915_hw_ppgtt *ppgtt =
 		container_of(vm, struct i915_hw_ppgtt, base);
 
-	list_del(&vm->global_link);
-	drm_mm_takedown(&ppgtt->base.mm);
 	drm_mm_remove_node(&ppgtt->node);
 
 	gen6_ppgtt_unmap_pages(ppgtt);
@@ -1151,13 +1042,10 @@ static int gen6_ppgtt_init(struct i915_hw_ppgtt *ppgtt)
 
 	ppgtt->base.pte_encode = dev_priv->gtt.base.pte_encode;
 	if (IS_GEN6(dev)) {
-		ppgtt->enable = gen6_ppgtt_enable;
 		ppgtt->switch_mm = gen6_mm_switch;
 	} else if (IS_HASWELL(dev)) {
-		ppgtt->enable = gen7_ppgtt_enable;
 		ppgtt->switch_mm = hsw_mm_switch;
 	} else if (IS_GEN7(dev)) {
-		ppgtt->enable = gen7_ppgtt_enable;
 		ppgtt->switch_mm = gen7_mm_switch;
 	} else
 		BUG();
@@ -1188,39 +1076,114 @@ static int gen6_ppgtt_init(struct i915_hw_ppgtt *ppgtt)
 			 ppgtt->node.size >> 20,
 			 ppgtt->node.start / PAGE_SIZE);
 
+	gen6_write_pdes(ppgtt);
+	DRM_DEBUG("Adding PPGTT at offset %x\n",
+		  ppgtt->pd_offset << 10);
+
 	return 0;
 }
 
-int i915_gem_init_ppgtt(struct drm_device *dev, struct i915_hw_ppgtt *ppgtt)
+static int __hw_ppgtt_init(struct drm_device *dev, struct i915_hw_ppgtt *ppgtt)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret = 0;
 
 	ppgtt->base.dev = dev;
 	ppgtt->base.scratch = dev_priv->gtt.base.scratch;
 
 	if (INTEL_INFO(dev)->gen < 8)
-		ret = gen6_ppgtt_init(ppgtt);
+		return gen6_ppgtt_init(ppgtt);
 	else if (IS_GEN8(dev))
-		ret = gen8_ppgtt_init(ppgtt, dev_priv->gtt.base.total);
+		return gen8_ppgtt_init(ppgtt, dev_priv->gtt.base.total);
 	else
 		BUG();
+}
+int i915_ppgtt_init(struct drm_device *dev, struct i915_hw_ppgtt *ppgtt)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret = 0;
 
-	if (!ret) {
-		struct drm_i915_private *dev_priv = dev->dev_private;
+	ret = __hw_ppgtt_init(dev, ppgtt);
+	if (ret == 0) {
 		kref_init(&ppgtt->ref);
 		drm_mm_init(&ppgtt->base.mm, ppgtt->base.start,
 			    ppgtt->base.total);
 		i915_init_vm(dev_priv, &ppgtt->base);
-		if (INTEL_INFO(dev)->gen < 8) {
-			gen6_write_pdes(ppgtt);
-			DRM_DEBUG("Adding PPGTT at offset %x\n",
-				  ppgtt->pd_offset << 10);
+	}
+
+	return ret;
+}
+
+int i915_ppgtt_init_hw(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring;
+	struct i915_hw_ppgtt *ppgtt = dev_priv->mm.aliasing_ppgtt;
+	int i, ret = 0;
+
+	/* In the case of execlists, PPGTT is enabled by the context descriptor
+	 * and the PDPs are contained within the context itself.  We don't
+	 * need to do anything here. */
+	if (i915.enable_execlists)
+		return 0;
+
+	if (!USES_PPGTT(dev))
+		return 0;
+
+	if (IS_GEN6(dev))
+		gen6_ppgtt_enable(dev);
+	else if (IS_GEN7(dev))
+		gen7_ppgtt_enable(dev);
+	else if (INTEL_INFO(dev)->gen >= 8)
+		gen8_ppgtt_enable(dev);
+	else
+		WARN_ON(1);
+
+	if (ppgtt) {
+		for_each_ring(ring, dev_priv, i) {
+			ret = ppgtt->switch_mm(ppgtt, ring);
+			if (ret != 0)
+				return ret;
 		}
 	}
 
 	return ret;
 }
+struct i915_hw_ppgtt *
+i915_ppgtt_create(struct drm_device *dev, struct drm_i915_file_private *fpriv)
+{
+	struct i915_hw_ppgtt *ppgtt;
+	int ret;
+
+	ppgtt = kzalloc(sizeof(*ppgtt), GFP_KERNEL);
+	if (!ppgtt)
+		return ERR_PTR(-ENOMEM);
+
+	ret = i915_ppgtt_init(dev, ppgtt);
+	if (ret) {
+		kfree(ppgtt);
+		return ERR_PTR(ret);
+	}
+
+	ppgtt->file_priv = fpriv;
+
+	return ppgtt;
+}
+
+void  i915_ppgtt_release(struct kref *kref)
+{
+	struct i915_hw_ppgtt *ppgtt =
+		container_of(kref, struct i915_hw_ppgtt, ref);
+
+	/* vmas should already be unbound */
+	WARN_ON(!list_empty(&ppgtt->base.active_list));
+	WARN_ON(!list_empty(&ppgtt->base.inactive_list));
+
+	list_del(&ppgtt->base.global_link);
+	drm_mm_takedown(&ppgtt->base.mm);
+
+	ppgtt->base.cleanup(&ppgtt->base);
+	kfree(ppgtt);
+}
 
 static void
 ppgtt_bind_vma(struct i915_vma *vma,
@@ -1310,6 +1273,16 @@ void i915_check_and_clear_faults(struct drm_device *dev)
 	POSTING_READ(RING_FAULT_REG(&dev_priv->ring[RCS]));
 }
 
+static void i915_ggtt_flush(struct drm_i915_private *dev_priv)
+{
+	if (INTEL_INFO(dev_priv->dev)->gen < 6) {
+		intel_gtt_chipset_flush();
+	} else {
+		I915_WRITE(GFX_FLSH_CNTL_GEN6, GFX_FLSH_CNTL_EN);
+		POSTING_READ(GFX_FLSH_CNTL_GEN6);
+	}
+}
+
 void i915_gem_suspend_gtt_mappings(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1326,6 +1299,8 @@ void i915_gem_suspend_gtt_mappings(struct drm_device *dev)
 				       dev_priv->gtt.base.start,
 				       dev_priv->gtt.base.total,
 				       true);
+
+	i915_ggtt_flush(dev_priv);
 }
 
 void i915_gem_restore_gtt_mappings(struct drm_device *dev)
@@ -1378,7 +1353,7 @@ void i915_gem_restore_gtt_mappings(struct drm_device *dev)
 		gen6_write_pdes(container_of(vm, struct i915_hw_ppgtt, base));
 	}
 
-	i915_gem_chipset_flush(dev);
+	i915_ggtt_flush(dev_priv);
 }
 
 int i915_gem_gtt_prepare_object(struct drm_i915_gem_object *obj)
@@ -1675,10 +1650,10 @@ static void i915_gtt_color_adjust(struct drm_mm_node *node,
 	}
 }
 
-void i915_gem_setup_global_gtt(struct drm_device *dev,
-			       unsigned long start,
-			       unsigned long mappable_end,
-			       unsigned long end)
+int i915_gem_setup_global_gtt(struct drm_device *dev,
+			      unsigned long start,
+			      unsigned long mappable_end,
+			      unsigned long end)
 {
 	/* Let GEM Manage all of the aperture.
 	 *
@@ -1694,6 +1669,7 @@ void i915_gem_setup_global_gtt(struct drm_device *dev,
 	struct drm_mm_node *entry;
 	struct drm_i915_gem_object *obj;
 	unsigned long hole_start, hole_end;
+	int ret;
 
 	BUG_ON(mappable_end > end);
 
@@ -1705,14 +1681,16 @@ void i915_gem_setup_global_gtt(struct drm_device *dev,
 	/* Mark any preallocated objects as occupied */
 	list_for_each_entry(obj, &dev_priv->mm.bound_list, global_list) {
 		struct i915_vma *vma = i915_gem_obj_to_vma(obj, ggtt_vm);
-		int ret;
+
 		DRM_DEBUG_KMS("reserving preallocated space: %lx + %zx\n",
 			      i915_gem_obj_ggtt_offset(obj), obj->base.size);
 
 		WARN_ON(i915_gem_obj_ggtt_bound(obj));
 		ret = drm_mm_reserve_node(&ggtt_vm->mm, &vma->node);
-		if (ret)
-			DRM_DEBUG_KMS("Reservation failed\n");
+		if (ret) {
+			DRM_DEBUG_KMS("Reservation failed: %i\n", ret);
+			return ret;
+		}
 		obj->has_global_gtt_mapping = 1;
 	}
 
@@ -1729,6 +1707,22 @@ void i915_gem_setup_global_gtt(struct drm_device *dev,
 
 	/* And finally clear the reserved guard page */
 	ggtt_vm->clear_range(ggtt_vm, end - PAGE_SIZE, PAGE_SIZE, true);
+
+	if (USES_PPGTT(dev) && !USES_FULL_PPGTT(dev)) {
+		struct i915_hw_ppgtt *ppgtt;
+
+		ppgtt = kzalloc(sizeof(*ppgtt), GFP_KERNEL);
+		if (!ppgtt)
+			return -ENOMEM;
+
+		ret = __hw_ppgtt_init(dev, ppgtt);
+		if (ret != 0)
+			return ret;
+
+		dev_priv->mm.aliasing_ppgtt = ppgtt;
+	}
+
+	return 0;
 }
 
 void i915_gem_init_global_gtt(struct drm_device *dev)
@@ -1742,6 +1736,25 @@ void i915_gem_init_global_gtt(struct drm_device *dev)
 	i915_gem_setup_global_gtt(dev, 0, mappable_size, gtt_size);
 }
 
+void i915_global_gtt_cleanup(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct i915_address_space *vm = &dev_priv->gtt.base;
+
+	if (dev_priv->mm.aliasing_ppgtt) {
+		struct i915_hw_ppgtt *ppgtt = dev_priv->mm.aliasing_ppgtt;
+
+		ppgtt->base.cleanup(&ppgtt->base);
+	}
+
+	if (drm_mm_initialized(&vm->mm)) {
+		drm_mm_takedown(&vm->mm);
+		list_del(&vm->global_link);
+	}
+
+	vm->cleanup(vm);
+}
+
 static int setup_scratch_page(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -2010,10 +2023,6 @@ static void gen6_gmch_remove(struct i915_address_space *vm)
 
 	struct i915_gtt *gtt = container_of(vm, struct i915_gtt, base);
 
-	if (drm_mm_initialized(&vm->mm)) {
-		drm_mm_takedown(&vm->mm);
-		list_del(&vm->global_link);
-	}
 	iounmap(gtt->gsm);
 	teardown_scratch_page(vm->dev);
 }
@@ -2046,10 +2055,6 @@ static int i915_gmch_probe(struct drm_device *dev,
 
 static void i915_gmch_remove(struct i915_address_space *vm)
 {
-	if (drm_mm_initialized(&vm->mm)) {
-		drm_mm_takedown(&vm->mm);
-		list_del(&vm->global_link);
-	}
 	intel_gmch_remove();
 }
 
@@ -2148,8 +2153,10 @@ static struct i915_vma *__i915_gem_vma_create(struct drm_i915_gem_object *obj,
 	/* Keep GGTT vmas first to make debug easier */
 	if (i915_is_ggtt(vm))
 		list_add(&vma->vma_link, &obj->vma_list);
-	else
+	else {
 		list_add_tail(&vma->vma_link, &obj->vma_list);
+		i915_ppgtt_get(i915_vm_to_ppgtt(vm));
+	}
 
 	return vma;
 }
diff --git a/drivers/gpu/drm/i915/i915_gem_gtt.h b/drivers/gpu/drm/i915/i915_gem_gtt.h
index 8d6f7c18c404..d5c14af51e99 100644
--- a/drivers/gpu/drm/i915/i915_gem_gtt.h
+++ b/drivers/gpu/drm/i915/i915_gem_gtt.h
@@ -34,6 +34,8 @@
 #ifndef __I915_GEM_GTT_H__
 #define __I915_GEM_GTT_H__
 
+struct drm_i915_file_private;
+
 typedef uint32_t gen6_gtt_pte_t;
 typedef uint64_t gen8_gtt_pte_t;
 typedef gen8_gtt_pte_t gen8_ppgtt_pde_t;
@@ -258,22 +260,36 @@ struct i915_hw_ppgtt {
 		dma_addr_t *gen8_pt_dma_addr[4];
 	};
 
-	struct intel_context *ctx;
+	struct drm_i915_file_private *file_priv;
 
 	int (*enable)(struct i915_hw_ppgtt *ppgtt);
 	int (*switch_mm)(struct i915_hw_ppgtt *ppgtt,
-			 struct intel_engine_cs *ring,
-			 bool synchronous);
+			 struct intel_engine_cs *ring);
 	void (*debug_dump)(struct i915_hw_ppgtt *ppgtt, struct seq_file *m);
 };
 
 int i915_gem_gtt_init(struct drm_device *dev);
 void i915_gem_init_global_gtt(struct drm_device *dev);
-void i915_gem_setup_global_gtt(struct drm_device *dev, unsigned long start,
-			       unsigned long mappable_end, unsigned long end);
-
-bool intel_enable_ppgtt(struct drm_device *dev, bool full);
-int i915_gem_init_ppgtt(struct drm_device *dev, struct i915_hw_ppgtt *ppgtt);
+int i915_gem_setup_global_gtt(struct drm_device *dev, unsigned long start,
+			      unsigned long mappable_end, unsigned long end);
+void i915_global_gtt_cleanup(struct drm_device *dev);
+
+
+int i915_ppgtt_init(struct drm_device *dev, struct i915_hw_ppgtt *ppgtt);
+int i915_ppgtt_init_hw(struct drm_device *dev);
+void i915_ppgtt_release(struct kref *kref);
+struct i915_hw_ppgtt *i915_ppgtt_create(struct drm_device *dev,
+					struct drm_i915_file_private *fpriv);
+static inline void i915_ppgtt_get(struct i915_hw_ppgtt *ppgtt)
+{
+	if (ppgtt)
+		kref_get(&ppgtt->ref);
+}
+static inline void i915_ppgtt_put(struct i915_hw_ppgtt *ppgtt)
+{
+	if (ppgtt)
+		kref_put(&ppgtt->ref, i915_ppgtt_release);
+}
 
 void i915_check_and_clear_faults(struct drm_device *dev);
 void i915_gem_suspend_gtt_mappings(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/i915_gem_render_state.c b/drivers/gpu/drm/i915/i915_gem_render_state.c
index e60be3f552a6..a9a62d75aa57 100644
--- a/drivers/gpu/drm/i915/i915_gem_render_state.c
+++ b/drivers/gpu/drm/i915/i915_gem_render_state.c
@@ -28,13 +28,6 @@
 #include "i915_drv.h"
 #include "intel_renderstate.h"
 
-struct render_state {
-	const struct intel_renderstate_rodata *rodata;
-	struct drm_i915_gem_object *obj;
-	u64 ggtt_offset;
-	int gen;
-};
-
 static const struct intel_renderstate_rodata *
 render_state_get_rodata(struct drm_device *dev, const int gen)
 {
@@ -127,30 +120,47 @@ static int render_state_setup(struct render_state *so)
 	return 0;
 }
 
-static void render_state_fini(struct render_state *so)
+void i915_gem_render_state_fini(struct render_state *so)
 {
 	i915_gem_object_ggtt_unpin(so->obj);
 	drm_gem_object_unreference(&so->obj->base);
 }
 
-int i915_gem_render_state_init(struct intel_engine_cs *ring)
+int i915_gem_render_state_prepare(struct intel_engine_cs *ring,
+				  struct render_state *so)
 {
-	struct render_state so;
 	int ret;
 
 	if (WARN_ON(ring->id != RCS))
 		return -ENOENT;
 
-	ret = render_state_init(&so, ring->dev);
+	ret = render_state_init(so, ring->dev);
 	if (ret)
 		return ret;
 
-	if (so.rodata == NULL)
+	if (so->rodata == NULL)
 		return 0;
 
-	ret = render_state_setup(&so);
+	ret = render_state_setup(so);
+	if (ret) {
+		i915_gem_render_state_fini(so);
+		return ret;
+	}
+
+	return 0;
+}
+
+int i915_gem_render_state_init(struct intel_engine_cs *ring)
+{
+	struct render_state so;
+	int ret;
+
+	ret = i915_gem_render_state_prepare(ring, &so);
 	if (ret)
-		goto out;
+		return ret;
+
+	if (so.rodata == NULL)
+		return 0;
 
 	ret = ring->dispatch_execbuffer(ring,
 					so.ggtt_offset,
@@ -164,6 +174,6 @@ int i915_gem_render_state_init(struct intel_engine_cs *ring)
 	ret = __i915_add_request(ring, NULL, so.obj, NULL);
 	/* __i915_add_request moves object to inactive if it fails */
 out:
-	render_state_fini(&so);
+	i915_gem_render_state_fini(&so);
 	return ret;
 }
diff --git a/drivers/gpu/drm/i915/i915_gem_render_state.h b/drivers/gpu/drm/i915/i915_gem_render_state.h
new file mode 100644
index 000000000000..c44961ed3fad
--- /dev/null
+++ b/drivers/gpu/drm/i915/i915_gem_render_state.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright © 2014 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _I915_GEM_RENDER_STATE_H_
+#define _I915_GEM_RENDER_STATE_H_
+
+#include <linux/types.h>
+
+struct intel_renderstate_rodata {
+	const u32 *reloc;
+	const u32 *batch;
+	const u32 batch_items;
+};
+
+struct render_state {
+	const struct intel_renderstate_rodata *rodata;
+	struct drm_i915_gem_object *obj;
+	u64 ggtt_offset;
+	int gen;
+};
+
+int i915_gem_render_state_init(struct intel_engine_cs *ring);
+void i915_gem_render_state_fini(struct render_state *so);
+int i915_gem_render_state_prepare(struct intel_engine_cs *ring,
+				  struct render_state *so);
+
+#endif /* _I915_GEM_RENDER_STATE_H_ */
diff --git a/drivers/gpu/drm/i915/i915_gem_stolen.c b/drivers/gpu/drm/i915/i915_gem_stolen.c
index 21c025a209c0..85fda6b803e4 100644
--- a/drivers/gpu/drm/i915/i915_gem_stolen.c
+++ b/drivers/gpu/drm/i915/i915_gem_stolen.c
@@ -289,6 +289,7 @@ void i915_gem_cleanup_stolen(struct drm_device *dev)
 int i915_gem_init_stolen(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 tmp;
 	int bios_reserved = 0;
 
 #ifdef CONFIG_INTEL_IOMMU
@@ -308,8 +309,16 @@ int i915_gem_init_stolen(struct drm_device *dev)
 	DRM_DEBUG_KMS("found %zd bytes of stolen memory at %08lx\n",
 		      dev_priv->gtt.stolen_size, dev_priv->mm.stolen_base);
 
-	if (IS_VALLEYVIEW(dev))
-		bios_reserved = 1024*1024; /* top 1M on VLV/BYT */
+	if (INTEL_INFO(dev)->gen >= 8) {
+		tmp = I915_READ(GEN7_BIOS_RESERVED);
+		tmp >>= GEN8_BIOS_RESERVED_SHIFT;
+		tmp &= GEN8_BIOS_RESERVED_MASK;
+		bios_reserved = (1024*1024) << tmp;
+	} else if (IS_GEN7(dev)) {
+		tmp = I915_READ(GEN7_BIOS_RESERVED);
+		bios_reserved = tmp & GEN7_BIOS_RESERVED_256K ?
+			256*1024 : 1024*1024;
+	}
 
 	if (WARN_ON(bios_reserved > dev_priv->gtt.stolen_size))
 		return 0;
diff --git a/drivers/gpu/drm/i915/i915_gem_tiling.c b/drivers/gpu/drm/i915/i915_gem_tiling.c
index cb150e8b4336..2cefb597df6d 100644
--- a/drivers/gpu/drm/i915/i915_gem_tiling.c
+++ b/drivers/gpu/drm/i915/i915_gem_tiling.c
@@ -91,7 +91,14 @@ i915_gem_detect_bit_6_swizzle(struct drm_device *dev)
 	uint32_t swizzle_x = I915_BIT_6_SWIZZLE_UNKNOWN;
 	uint32_t swizzle_y = I915_BIT_6_SWIZZLE_UNKNOWN;
 
-	if (IS_VALLEYVIEW(dev)) {
+	if (INTEL_INFO(dev)->gen >= 8 || IS_VALLEYVIEW(dev)) {
+		/*
+		 * On BDW+, swizzling is not used. We leave the CPU memory
+		 * controller in charge of optimizing memory accesses without
+		 * the extra address manipulation GPU side.
+		 *
+		 * VLV and CHV don't have GPU swizzling.
+		 */
 		swizzle_x = I915_BIT_6_SWIZZLE_NONE;
 		swizzle_y = I915_BIT_6_SWIZZLE_NONE;
 	} else if (INTEL_INFO(dev)->gen >= 6) {
@@ -376,7 +383,7 @@ i915_gem_set_tiling(struct drm_device *dev, void *data,
 
 		if (ret == 0) {
 			obj->fence_dirty =
-				obj->fenced_gpu_access ||
+				obj->last_fenced_seqno ||
 				obj->fence_reg != I915_FENCE_REG_NONE;
 
 			obj->tiling_mode = args->tiling_mode;
diff --git a/drivers/gpu/drm/i915/i915_gem_userptr.c b/drivers/gpu/drm/i915/i915_gem_userptr.c
index fe69fc837d9e..d182058383a9 100644
--- a/drivers/gpu/drm/i915/i915_gem_userptr.c
+++ b/drivers/gpu/drm/i915/i915_gem_userptr.c
@@ -32,6 +32,15 @@
 #include <linux/mempolicy.h>
 #include <linux/swap.h>
 
+struct i915_mm_struct {
+	struct mm_struct *mm;
+	struct drm_device *dev;
+	struct i915_mmu_notifier *mn;
+	struct hlist_node node;
+	struct kref kref;
+	struct work_struct work;
+};
+
 #if defined(CONFIG_MMU_NOTIFIER)
 #include <linux/interval_tree.h>
 
@@ -41,16 +50,12 @@ struct i915_mmu_notifier {
 	struct mmu_notifier mn;
 	struct rb_root objects;
 	struct list_head linear;
-	struct drm_device *dev;
-	struct mm_struct *mm;
-	struct work_struct work;
-	unsigned long count;
 	unsigned long serial;
 	bool has_linear;
 };
 
 struct i915_mmu_object {
-	struct i915_mmu_notifier *mmu;
+	struct i915_mmu_notifier *mn;
 	struct interval_tree_node it;
 	struct list_head link;
 	struct drm_i915_gem_object *obj;
@@ -96,18 +101,18 @@ static void *invalidate_range__linear(struct i915_mmu_notifier *mn,
 				      unsigned long start,
 				      unsigned long end)
 {
-	struct i915_mmu_object *mmu;
+	struct i915_mmu_object *mo;
 	unsigned long serial;
 
 restart:
 	serial = mn->serial;
-	list_for_each_entry(mmu, &mn->linear, link) {
+	list_for_each_entry(mo, &mn->linear, link) {
 		struct drm_i915_gem_object *obj;
 
-		if (mmu->it.last < start || mmu->it.start > end)
+		if (mo->it.last < start || mo->it.start > end)
 			continue;
 
-		obj = mmu->obj;
+		obj = mo->obj;
 		drm_gem_object_reference(&obj->base);
 		spin_unlock(&mn->lock);
 
@@ -160,130 +165,47 @@ static const struct mmu_notifier_ops i915_gem_userptr_notifier = {
 };
 
 static struct i915_mmu_notifier *
-__i915_mmu_notifier_lookup(struct drm_device *dev, struct mm_struct *mm)
-{
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct i915_mmu_notifier *mmu;
-
-	/* Protected by dev->struct_mutex */
-	hash_for_each_possible(dev_priv->mmu_notifiers, mmu, node, (unsigned long)mm)
-		if (mmu->mm == mm)
-			return mmu;
-
-	return NULL;
-}
-
-static struct i915_mmu_notifier *
-i915_mmu_notifier_get(struct drm_device *dev, struct mm_struct *mm)
+i915_mmu_notifier_create(struct mm_struct *mm)
 {
-	struct drm_i915_private *dev_priv = to_i915(dev);
-	struct i915_mmu_notifier *mmu;
+	struct i915_mmu_notifier *mn;
 	int ret;
 
-	lockdep_assert_held(&dev->struct_mutex);
-
-	mmu = __i915_mmu_notifier_lookup(dev, mm);
-	if (mmu)
-		return mmu;
-
-	mmu = kmalloc(sizeof(*mmu), GFP_KERNEL);
-	if (mmu == NULL)
+	mn = kmalloc(sizeof(*mn), GFP_KERNEL);
+	if (mn == NULL)
 		return ERR_PTR(-ENOMEM);
 
-	spin_lock_init(&mmu->lock);
-	mmu->dev = dev;
-	mmu->mn.ops = &i915_gem_userptr_notifier;
-	mmu->mm = mm;
-	mmu->objects = RB_ROOT;
-	mmu->count = 0;
-	mmu->serial = 1;
-	INIT_LIST_HEAD(&mmu->linear);
-	mmu->has_linear = false;
-
-	/* Protected by mmap_sem (write-lock) */
-	ret = __mmu_notifier_register(&mmu->mn, mm);
+	spin_lock_init(&mn->lock);
+	mn->mn.ops = &i915_gem_userptr_notifier;
+	mn->objects = RB_ROOT;
+	mn->serial = 1;
+	INIT_LIST_HEAD(&mn->linear);
+	mn->has_linear = false;
+
+	 /* Protected by mmap_sem (write-lock) */
+	ret = __mmu_notifier_register(&mn->mn, mm);
 	if (ret) {
-		kfree(mmu);
+		kfree(mn);
 		return ERR_PTR(ret);
 	}
 
-	/* Protected by dev->struct_mutex */
-	hash_add(dev_priv->mmu_notifiers, &mmu->node, (unsigned long)mm);
-	return mmu;
-}
-
-static void
-__i915_mmu_notifier_destroy_worker(struct work_struct *work)
-{
-	struct i915_mmu_notifier *mmu = container_of(work, typeof(*mmu), work);
-	mmu_notifier_unregister(&mmu->mn, mmu->mm);
-	kfree(mmu);
-}
-
-static void
-__i915_mmu_notifier_destroy(struct i915_mmu_notifier *mmu)
-{
-	lockdep_assert_held(&mmu->dev->struct_mutex);
-
-	/* Protected by dev->struct_mutex */
-	hash_del(&mmu->node);
-
-	/* Our lock ordering is: mmap_sem, mmu_notifier_scru, struct_mutex.
-	 * We enter the function holding struct_mutex, therefore we need
-	 * to drop our mutex prior to calling mmu_notifier_unregister in
-	 * order to prevent lock inversion (and system-wide deadlock)
-	 * between the mmap_sem and struct-mutex. Hence we defer the
-	 * unregistration to a workqueue where we hold no locks.
-	 */
-	INIT_WORK(&mmu->work, __i915_mmu_notifier_destroy_worker);
-	schedule_work(&mmu->work);
-}
-
-static void __i915_mmu_notifier_update_serial(struct i915_mmu_notifier *mmu)
-{
-	if (++mmu->serial == 0)
-		mmu->serial = 1;
+	return mn;
 }
 
-static bool i915_mmu_notifier_has_linear(struct i915_mmu_notifier *mmu)
+static void __i915_mmu_notifier_update_serial(struct i915_mmu_notifier *mn)
 {
-	struct i915_mmu_object *mn;
-
-	list_for_each_entry(mn, &mmu->linear, link)
-		if (mn->is_linear)
-			return true;
-
-	return false;
-}
-
-static void
-i915_mmu_notifier_del(struct i915_mmu_notifier *mmu,
-		      struct i915_mmu_object *mn)
-{
-	lockdep_assert_held(&mmu->dev->struct_mutex);
-
-	spin_lock(&mmu->lock);
-	list_del(&mn->link);
-	if (mn->is_linear)
-		mmu->has_linear = i915_mmu_notifier_has_linear(mmu);
-	else
-		interval_tree_remove(&mn->it, &mmu->objects);
-	__i915_mmu_notifier_update_serial(mmu);
-	spin_unlock(&mmu->lock);
-
-	/* Protected against _add() by dev->struct_mutex */
-	if (--mmu->count == 0)
-		__i915_mmu_notifier_destroy(mmu);
+	if (++mn->serial == 0)
+		mn->serial = 1;
 }
 
 static int
-i915_mmu_notifier_add(struct i915_mmu_notifier *mmu,
-		      struct i915_mmu_object *mn)
+i915_mmu_notifier_add(struct drm_device *dev,
+		      struct i915_mmu_notifier *mn,
+		      struct i915_mmu_object *mo)
 {
 	struct interval_tree_node *it;
 	int ret;
 
-	ret = i915_mutex_lock_interruptible(mmu->dev);
+	ret = i915_mutex_lock_interruptible(dev);
 	if (ret)
 		return ret;
 
@@ -291,11 +213,11 @@ i915_mmu_notifier_add(struct i915_mmu_notifier *mmu,
 	 * remove the objects from the interval tree) before we do
 	 * the check for overlapping objects.
 	 */
-	i915_gem_retire_requests(mmu->dev);
+	i915_gem_retire_requests(dev);
 
-	spin_lock(&mmu->lock);
-	it = interval_tree_iter_first(&mmu->objects,
-				      mn->it.start, mn->it.last);
+	spin_lock(&mn->lock);
+	it = interval_tree_iter_first(&mn->objects,
+				      mo->it.start, mo->it.last);
 	if (it) {
 		struct drm_i915_gem_object *obj;
 
@@ -312,86 +234,130 @@ i915_mmu_notifier_add(struct i915_mmu_notifier *mmu,
 
 		obj = container_of(it, struct i915_mmu_object, it)->obj;
 		if (!obj->userptr.workers)
-			mmu->has_linear = mn->is_linear = true;
+			mn->has_linear = mo->is_linear = true;
 		else
 			ret = -EAGAIN;
 	} else
-		interval_tree_insert(&mn->it, &mmu->objects);
+		interval_tree_insert(&mo->it, &mn->objects);
 
 	if (ret == 0) {
-		list_add(&mn->link, &mmu->linear);
-		__i915_mmu_notifier_update_serial(mmu);
+		list_add(&mo->link, &mn->linear);
+		__i915_mmu_notifier_update_serial(mn);
 	}
-	spin_unlock(&mmu->lock);
-	mutex_unlock(&mmu->dev->struct_mutex);
+	spin_unlock(&mn->lock);
+	mutex_unlock(&dev->struct_mutex);
 
 	return ret;
 }
 
+static bool i915_mmu_notifier_has_linear(struct i915_mmu_notifier *mn)
+{
+	struct i915_mmu_object *mo;
+
+	list_for_each_entry(mo, &mn->linear, link)
+		if (mo->is_linear)
+			return true;
+
+	return false;
+}
+
+static void
+i915_mmu_notifier_del(struct i915_mmu_notifier *mn,
+		      struct i915_mmu_object *mo)
+{
+	spin_lock(&mn->lock);
+	list_del(&mo->link);
+	if (mo->is_linear)
+		mn->has_linear = i915_mmu_notifier_has_linear(mn);
+	else
+		interval_tree_remove(&mo->it, &mn->objects);
+	__i915_mmu_notifier_update_serial(mn);
+	spin_unlock(&mn->lock);
+}
+
 static void
 i915_gem_userptr_release__mmu_notifier(struct drm_i915_gem_object *obj)
 {
-	struct i915_mmu_object *mn;
+	struct i915_mmu_object *mo;
 
-	mn = obj->userptr.mn;
-	if (mn == NULL)
+	mo = obj->userptr.mmu_object;
+	if (mo == NULL)
 		return;
 
-	i915_mmu_notifier_del(mn->mmu, mn);
-	obj->userptr.mn = NULL;
+	i915_mmu_notifier_del(mo->mn, mo);
+	kfree(mo);
+
+	obj->userptr.mmu_object = NULL;
+}
+
+static struct i915_mmu_notifier *
+i915_mmu_notifier_find(struct i915_mm_struct *mm)
+{
+	struct i915_mmu_notifier *mn = mm->mn;
+
+	mn = mm->mn;
+	if (mn)
+		return mn;
+
+	down_write(&mm->mm->mmap_sem);
+	mutex_lock(&to_i915(mm->dev)->mm_lock);
+	if ((mn = mm->mn) == NULL) {
+		mn = i915_mmu_notifier_create(mm->mm);
+		if (!IS_ERR(mn))
+			mm->mn = mn;
+	}
+	mutex_unlock(&to_i915(mm->dev)->mm_lock);
+	up_write(&mm->mm->mmap_sem);
+
+	return mn;
 }
 
 static int
 i915_gem_userptr_init__mmu_notifier(struct drm_i915_gem_object *obj,
 				    unsigned flags)
 {
-	struct i915_mmu_notifier *mmu;
-	struct i915_mmu_object *mn;
+	struct i915_mmu_notifier *mn;
+	struct i915_mmu_object *mo;
 	int ret;
 
 	if (flags & I915_USERPTR_UNSYNCHRONIZED)
 		return capable(CAP_SYS_ADMIN) ? 0 : -EPERM;
 
-	down_write(&obj->userptr.mm->mmap_sem);
-	ret = i915_mutex_lock_interruptible(obj->base.dev);
-	if (ret == 0) {
-		mmu = i915_mmu_notifier_get(obj->base.dev, obj->userptr.mm);
-		if (!IS_ERR(mmu))
-			mmu->count++; /* preemptive add to act as a refcount */
-		else
-			ret = PTR_ERR(mmu);
-		mutex_unlock(&obj->base.dev->struct_mutex);
-	}
-	up_write(&obj->userptr.mm->mmap_sem);
-	if (ret)
-		return ret;
+	if (WARN_ON(obj->userptr.mm == NULL))
+		return -EINVAL;
 
-	mn = kzalloc(sizeof(*mn), GFP_KERNEL);
-	if (mn == NULL) {
-		ret = -ENOMEM;
-		goto destroy_mmu;
-	}
+	mn = i915_mmu_notifier_find(obj->userptr.mm);
+	if (IS_ERR(mn))
+		return PTR_ERR(mn);
 
-	mn->mmu = mmu;
-	mn->it.start = obj->userptr.ptr;
-	mn->it.last = mn->it.start + obj->base.size - 1;
-	mn->obj = obj;
+	mo = kzalloc(sizeof(*mo), GFP_KERNEL);
+	if (mo == NULL)
+		return -ENOMEM;
 
-	ret = i915_mmu_notifier_add(mmu, mn);
-	if (ret)
-		goto free_mn;
+	mo->mn = mn;
+	mo->it.start = obj->userptr.ptr;
+	mo->it.last = mo->it.start + obj->base.size - 1;
+	mo->obj = obj;
 
-	obj->userptr.mn = mn;
+	ret = i915_mmu_notifier_add(obj->base.dev, mn, mo);
+	if (ret) {
+		kfree(mo);
+		return ret;
+	}
+
+	obj->userptr.mmu_object = mo;
 	return 0;
+}
+
+static void
+i915_mmu_notifier_free(struct i915_mmu_notifier *mn,
+		       struct mm_struct *mm)
+{
+	if (mn == NULL)
+		return;
 
-free_mn:
+	mmu_notifier_unregister(&mn->mn, mm);
 	kfree(mn);
-destroy_mmu:
-	mutex_lock(&obj->base.dev->struct_mutex);
-	if (--mmu->count == 0)
-		__i915_mmu_notifier_destroy(mmu);
-	mutex_unlock(&obj->base.dev->struct_mutex);
-	return ret;
 }
 
 #else
@@ -413,15 +379,114 @@ i915_gem_userptr_init__mmu_notifier(struct drm_i915_gem_object *obj,
 
 	return 0;
 }
+
+static void
+i915_mmu_notifier_free(struct i915_mmu_notifier *mn,
+		       struct mm_struct *mm)
+{
+}
+
 #endif
 
+static struct i915_mm_struct *
+__i915_mm_struct_find(struct drm_i915_private *dev_priv, struct mm_struct *real)
+{
+	struct i915_mm_struct *mm;
+
+	/* Protected by dev_priv->mm_lock */
+	hash_for_each_possible(dev_priv->mm_structs, mm, node, (unsigned long)real)
+		if (mm->mm == real)
+			return mm;
+
+	return NULL;
+}
+
+static int
+i915_gem_userptr_init__mm_struct(struct drm_i915_gem_object *obj)
+{
+	struct drm_i915_private *dev_priv = to_i915(obj->base.dev);
+	struct i915_mm_struct *mm;
+	int ret = 0;
+
+	/* During release of the GEM object we hold the struct_mutex. This
+	 * precludes us from calling mmput() at that time as that may be
+	 * the last reference and so call exit_mmap(). exit_mmap() will
+	 * attempt to reap the vma, and if we were holding a GTT mmap
+	 * would then call drm_gem_vm_close() and attempt to reacquire
+	 * the struct mutex. So in order to avoid that recursion, we have
+	 * to defer releasing the mm reference until after we drop the
+	 * struct_mutex, i.e. we need to schedule a worker to do the clean
+	 * up.
+	 */
+	mutex_lock(&dev_priv->mm_lock);
+	mm = __i915_mm_struct_find(dev_priv, current->mm);
+	if (mm == NULL) {
+		mm = kmalloc(sizeof(*mm), GFP_KERNEL);
+		if (mm == NULL) {
+			ret = -ENOMEM;
+			goto out;
+		}
+
+		kref_init(&mm->kref);
+		mm->dev = obj->base.dev;
+
+		mm->mm = current->mm;
+		atomic_inc(&current->mm->mm_count);
+
+		mm->mn = NULL;
+
+		/* Protected by dev_priv->mm_lock */
+		hash_add(dev_priv->mm_structs,
+			 &mm->node, (unsigned long)mm->mm);
+	} else
+		kref_get(&mm->kref);
+
+	obj->userptr.mm = mm;
+out:
+	mutex_unlock(&dev_priv->mm_lock);
+	return ret;
+}
+
+static void
+__i915_mm_struct_free__worker(struct work_struct *work)
+{
+	struct i915_mm_struct *mm = container_of(work, typeof(*mm), work);
+	i915_mmu_notifier_free(mm->mn, mm->mm);
+	mmdrop(mm->mm);
+	kfree(mm);
+}
+
+static void
+__i915_mm_struct_free(struct kref *kref)
+{
+	struct i915_mm_struct *mm = container_of(kref, typeof(*mm), kref);
+
+	/* Protected by dev_priv->mm_lock */
+	hash_del(&mm->node);
+	mutex_unlock(&to_i915(mm->dev)->mm_lock);
+
+	INIT_WORK(&mm->work, __i915_mm_struct_free__worker);
+	schedule_work(&mm->work);
+}
+
+static void
+i915_gem_userptr_release__mm_struct(struct drm_i915_gem_object *obj)
+{
+	if (obj->userptr.mm == NULL)
+		return;
+
+	kref_put_mutex(&obj->userptr.mm->kref,
+		       __i915_mm_struct_free,
+		       &to_i915(obj->base.dev)->mm_lock);
+	obj->userptr.mm = NULL;
+}
+
 struct get_pages_work {
 	struct work_struct work;
 	struct drm_i915_gem_object *obj;
 	struct task_struct *task;
 };
 
-
 #if IS_ENABLED(CONFIG_SWIOTLB)
 #define swiotlb_active() swiotlb_nr_tbl()
 #else
@@ -479,7 +544,7 @@ __i915_gem_userptr_get_pages_worker(struct work_struct *_work)
 	if (pvec == NULL)
 		pvec = drm_malloc_ab(num_pages, sizeof(struct page *));
 	if (pvec != NULL) {
-		struct mm_struct *mm = obj->userptr.mm;
+		struct mm_struct *mm = obj->userptr.mm->mm;
 
 		down_read(&mm->mmap_sem);
 		while (pinned < num_pages) {
@@ -545,7 +610,7 @@ i915_gem_userptr_get_pages(struct drm_i915_gem_object *obj)
 
 	pvec = NULL;
 	pinned = 0;
-	if (obj->userptr.mm == current->mm) {
+	if (obj->userptr.mm->mm == current->mm) {
 		pvec = kmalloc(num_pages*sizeof(struct page *),
 			       GFP_TEMPORARY | __GFP_NOWARN | __GFP_NORETRY);
 		if (pvec == NULL) {
@@ -624,16 +689,15 @@ i915_gem_userptr_get_pages(struct drm_i915_gem_object *obj)
 static void
 i915_gem_userptr_put_pages(struct drm_i915_gem_object *obj)
 {
-	struct scatterlist *sg;
-	int i;
+	struct sg_page_iter sg_iter;
 
 	BUG_ON(obj->userptr.work != NULL);
 
 	if (obj->madv != I915_MADV_WILLNEED)
 		obj->dirty = 0;
 
-	for_each_sg(obj->pages->sgl, sg, obj->pages->nents, i) {
-		struct page *page = sg_page(sg);
+	for_each_sg_page(obj->pages->sgl, &sg_iter, obj->pages->nents, 0) {
+		struct page *page = sg_page_iter_page(&sg_iter);
 
 		if (obj->dirty)
 			set_page_dirty(page);
@@ -651,17 +715,13 @@ static void
 i915_gem_userptr_release(struct drm_i915_gem_object *obj)
 {
 	i915_gem_userptr_release__mmu_notifier(obj);
-
-	if (obj->userptr.mm) {
-		mmput(obj->userptr.mm);
-		obj->userptr.mm = NULL;
-	}
+	i915_gem_userptr_release__mm_struct(obj);
 }
 
 static int
 i915_gem_userptr_dmabuf_export(struct drm_i915_gem_object *obj)
 {
-	if (obj->userptr.mn)
+	if (obj->userptr.mmu_object)
 		return 0;
 
 	return i915_gem_userptr_init__mmu_notifier(obj, 0);
@@ -736,7 +796,6 @@ i915_gem_userptr_ioctl(struct drm_device *dev, void *data, struct drm_file *file
 		return -ENODEV;
 	}
 
-	/* Allocate the new object */
 	obj = i915_gem_object_alloc(dev);
 	if (obj == NULL)
 		return -ENOMEM;
@@ -754,8 +813,8 @@ i915_gem_userptr_ioctl(struct drm_device *dev, void *data, struct drm_file *file
 	 * at binding. This means that we need to hook into the mmu_notifier
 	 * in order to detect if the mmu is destroyed.
 	 */
-	ret = -ENOMEM;
-	if ((obj->userptr.mm = get_task_mm(current)))
+	ret = i915_gem_userptr_init__mm_struct(obj);
+	if (ret == 0)
 		ret = i915_gem_userptr_init__mmu_notifier(obj, args->flags);
 	if (ret == 0)
 		ret = drm_gem_handle_create(file, &obj->base, &handle);
@@ -772,9 +831,8 @@ i915_gem_userptr_ioctl(struct drm_device *dev, void *data, struct drm_file *file
 int
 i915_gem_init_userptr(struct drm_device *dev)
 {
-#if defined(CONFIG_MMU_NOTIFIER)
 	struct drm_i915_private *dev_priv = to_i915(dev);
-	hash_init(dev_priv->mmu_notifiers);
-#endif
+	mutex_init(&dev_priv->mm_lock);
+	hash_init(dev_priv->mm_structs);
 	return 0;
 }
diff --git a/drivers/gpu/drm/i915/i915_gpu_error.c b/drivers/gpu/drm/i915/i915_gpu_error.c
index eab41f9390f8..2c87a797213f 100644
--- a/drivers/gpu/drm/i915/i915_gpu_error.c
+++ b/drivers/gpu/drm/i915/i915_gpu_error.c
@@ -192,10 +192,10 @@ static void print_error_buffers(struct drm_i915_error_state_buf *m,
 				struct drm_i915_error_buffer *err,
 				int count)
 {
-	err_printf(m, "%s [%d]:\n", name, count);
+	err_printf(m, "  %s [%d]:\n", name, count);
 
 	while (count--) {
-		err_printf(m, "  %08x %8u %02x %02x %x %x",
+		err_printf(m, "    %08x %8u %02x %02x %x %x",
 			   err->gtt_offset,
 			   err->size,
 			   err->read_domains,
@@ -208,7 +208,7 @@ static void print_error_buffers(struct drm_i915_error_state_buf *m,
 		err_puts(m, err->userptr ? " userptr" : "");
 		err_puts(m, err->ring != -1 ? " " : "");
 		err_puts(m, ring_str(err->ring));
-		err_puts(m, i915_cache_level_str(err->cache_level));
+		err_puts(m, i915_cache_level_str(m->i915, err->cache_level));
 
 		if (err->name)
 			err_printf(m, " (name: %d)", err->name);
@@ -393,15 +393,17 @@ int i915_error_state_to_str(struct drm_i915_error_state_buf *m,
 		i915_ring_error_state(m, dev, &error->ring[i]);
 	}
 
-	if (error->active_bo)
+	for (i = 0; i < error->vm_count; i++) {
+		err_printf(m, "vm[%d]\n", i);
+
 		print_error_buffers(m, "Active",
-				    error->active_bo[0],
-				    error->active_bo_count[0]);
+				    error->active_bo[i],
+				    error->active_bo_count[i]);
 
-	if (error->pinned_bo)
 		print_error_buffers(m, "Pinned",
-				    error->pinned_bo[0],
-				    error->pinned_bo_count[0]);
+				    error->pinned_bo[i],
+				    error->pinned_bo_count[i]);
+	}
 
 	for (i = 0; i < ARRAY_SIZE(error->ring); i++) {
 		obj = error->ring[i].batchbuffer;
@@ -492,9 +494,11 @@ out:
 }
 
 int i915_error_state_buf_init(struct drm_i915_error_state_buf *ebuf,
+			      struct drm_i915_private *i915,
 			      size_t count, loff_t pos)
 {
 	memset(ebuf, 0, sizeof(*ebuf));
+	ebuf->i915 = i915;
 
 	/* We need to have enough room to store any i915_error_state printf
 	 * so that we can move it to start position.
@@ -556,24 +560,54 @@ static void i915_error_state_free(struct kref *error_ref)
 }
 
 static struct drm_i915_error_object *
-i915_error_object_create_sized(struct drm_i915_private *dev_priv,
-			       struct drm_i915_gem_object *src,
-			       struct i915_address_space *vm,
-			       const int num_pages)
+i915_error_object_create(struct drm_i915_private *dev_priv,
+			 struct drm_i915_gem_object *src,
+			 struct i915_address_space *vm)
 {
 	struct drm_i915_error_object *dst;
-	int i;
+	int num_pages;
+	bool use_ggtt;
+	int i = 0;
 	u32 reloc_offset;
 
 	if (src == NULL || src->pages == NULL)
 		return NULL;
 
+	num_pages = src->base.size >> PAGE_SHIFT;
+
 	dst = kmalloc(sizeof(*dst) + num_pages * sizeof(u32 *), GFP_ATOMIC);
 	if (dst == NULL)
 		return NULL;
 
-	reloc_offset = dst->gtt_offset = i915_gem_obj_offset(src, vm);
-	for (i = 0; i < num_pages; i++) {
+	if (i915_gem_obj_bound(src, vm))
+		dst->gtt_offset = i915_gem_obj_offset(src, vm);
+	else
+		dst->gtt_offset = -1;
+
+	reloc_offset = dst->gtt_offset;
+	use_ggtt = (src->cache_level == I915_CACHE_NONE &&
+		    i915_is_ggtt(vm) &&
+		    src->has_global_gtt_mapping &&
+		    reloc_offset + num_pages * PAGE_SIZE <= dev_priv->gtt.mappable_end);
+
+	/* Cannot access stolen address directly, try to use the aperture */
+	if (src->stolen) {
+		use_ggtt = true;
+
+		if (!src->has_global_gtt_mapping)
+			goto unwind;
+
+		reloc_offset = i915_gem_obj_ggtt_offset(src);
+		if (reloc_offset + num_pages * PAGE_SIZE > dev_priv->gtt.mappable_end)
+			goto unwind;
+	}
+
+	/* Cannot access snooped pages through the aperture */
+	if (use_ggtt && src->cache_level != I915_CACHE_NONE && !HAS_LLC(dev_priv->dev))
+		goto unwind;
+
+	dst->page_count = num_pages;
+	while (num_pages--) {
 		unsigned long flags;
 		void *d;
 
@@ -582,10 +616,7 @@ i915_error_object_create_sized(struct drm_i915_private *dev_priv,
 			goto unwind;
 
 		local_irq_save(flags);
-		if (src->cache_level == I915_CACHE_NONE &&
-		    reloc_offset < dev_priv->gtt.mappable_end &&
-		    src->has_global_gtt_mapping &&
-		    i915_is_ggtt(vm)) {
+		if (use_ggtt) {
 			void __iomem *s;
 
 			/* Simply ignore tiling or any overlapping fence.
@@ -597,14 +628,6 @@ i915_error_object_create_sized(struct drm_i915_private *dev_priv,
 						     reloc_offset);
 			memcpy_fromio(d, s, PAGE_SIZE);
 			io_mapping_unmap_atomic(s);
-		} else if (src->stolen) {
-			unsigned long offset;
-
-			offset = dev_priv->mm.stolen_base;
-			offset += src->stolen->start;
-			offset += i << PAGE_SHIFT;
-
-			memcpy_fromio(d, (void __iomem *) offset, PAGE_SIZE);
 		} else {
 			struct page *page;
 			void *s;
@@ -621,11 +644,9 @@ i915_error_object_create_sized(struct drm_i915_private *dev_priv,
 		}
 		local_irq_restore(flags);
 
-		dst->pages[i] = d;
-
+		dst->pages[i++] = d;
 		reloc_offset += PAGE_SIZE;
 	}
-	dst->page_count = num_pages;
 
 	return dst;
 
@@ -635,22 +656,19 @@ unwind:
 	kfree(dst);
 	return NULL;
 }
-#define i915_error_object_create(dev_priv, src, vm) \
-	i915_error_object_create_sized((dev_priv), (src), (vm), \
-				       (src)->base.size>>PAGE_SHIFT)
-
 #define i915_error_ggtt_object_create(dev_priv, src) \
-	i915_error_object_create_sized((dev_priv), (src), &(dev_priv)->gtt.base, \
-				       (src)->base.size>>PAGE_SHIFT)
+	i915_error_object_create((dev_priv), (src), &(dev_priv)->gtt.base)
 
 static void capture_bo(struct drm_i915_error_buffer *err,
-		       struct drm_i915_gem_object *obj)
+		       struct i915_vma *vma)
 {
+	struct drm_i915_gem_object *obj = vma->obj;
+
 	err->size = obj->base.size;
 	err->name = obj->base.name;
 	err->rseqno = obj->last_read_seqno;
 	err->wseqno = obj->last_write_seqno;
-	err->gtt_offset = i915_gem_obj_ggtt_offset(obj);
+	err->gtt_offset = vma->node.start;
 	err->read_domains = obj->base.read_domains;
 	err->write_domain = obj->base.write_domain;
 	err->fence_reg = obj->fence_reg;
@@ -674,7 +692,7 @@ static u32 capture_active_bo(struct drm_i915_error_buffer *err,
 	int i = 0;
 
 	list_for_each_entry(vma, head, mm_list) {
-		capture_bo(err++, vma->obj);
+		capture_bo(err++, vma);
 		if (++i == count)
 			break;
 	}
@@ -683,21 +701,27 @@ static u32 capture_active_bo(struct drm_i915_error_buffer *err,
 }
 
 static u32 capture_pinned_bo(struct drm_i915_error_buffer *err,
-			     int count, struct list_head *head)
+			     int count, struct list_head *head,
+			     struct i915_address_space *vm)
 {
 	struct drm_i915_gem_object *obj;
-	int i = 0;
+	struct drm_i915_error_buffer * const first = err;
+	struct drm_i915_error_buffer * const last = err + count;
 
 	list_for_each_entry(obj, head, global_list) {
-		if (!i915_gem_obj_is_pinned(obj))
-			continue;
+		struct i915_vma *vma;
 
-		capture_bo(err++, obj);
-		if (++i == count)
+		if (err == last)
 			break;
+
+		list_for_each_entry(vma, &obj->vma_list, vma_link)
+			if (vma->vm == vm && vma->pin_count > 0) {
+				capture_bo(err++, vma);
+				break;
+			}
 	}
 
-	return i;
+	return err - first;
 }
 
 /* Generate a semi-unique error code. The code is not meant to have meaning, The
@@ -890,9 +914,6 @@ static void i915_record_ring_state(struct drm_device *dev,
 		ering->hws = I915_READ(mmio);
 	}
 
-	ering->cpu_ring_head = ring->buffer->head;
-	ering->cpu_ring_tail = ring->buffer->tail;
-
 	ering->hangcheck_score = ring->hangcheck.score;
 	ering->hangcheck_action = ring->hangcheck.action;
 
@@ -955,6 +976,7 @@ static void i915_gem_record_rings(struct drm_device *dev,
 
 	for (i = 0; i < I915_NUM_RINGS; i++) {
 		struct intel_engine_cs *ring = &dev_priv->ring[i];
+		struct intel_ringbuffer *rbuf;
 
 		error->ring[i].pid = -1;
 
@@ -967,6 +989,12 @@ static void i915_gem_record_rings(struct drm_device *dev,
 
 		request = i915_gem_find_active_request(ring);
 		if (request) {
+			struct i915_address_space *vm;
+
+			vm = request->ctx && request->ctx->ppgtt ?
+				&request->ctx->ppgtt->base :
+				&dev_priv->gtt.base;
+
 			/* We need to copy these to an anonymous buffer
 			 * as the simplest method to avoid being overwritten
 			 * by userspace.
@@ -974,12 +1002,9 @@ static void i915_gem_record_rings(struct drm_device *dev,
 			error->ring[i].batchbuffer =
 				i915_error_object_create(dev_priv,
 							 request->batch_obj,
-							 request->ctx ?
-							 request->ctx->vm :
-							 &dev_priv->gtt.base);
+							 vm);
 
-			if (HAS_BROKEN_CS_TLB(dev_priv->dev) &&
-			    ring->scratch.obj)
+			if (HAS_BROKEN_CS_TLB(dev_priv->dev))
 				error->ring[i].wa_batchbuffer =
 					i915_error_ggtt_object_create(dev_priv,
 							     ring->scratch.obj);
@@ -998,12 +1023,27 @@ static void i915_gem_record_rings(struct drm_device *dev,
 			}
 		}
 
+		if (i915.enable_execlists) {
+			/* TODO: This is only a small fix to keep basic error
+			 * capture working, but we need to add more information
+			 * for it to be useful (e.g. dump the context being
+			 * executed).
+			 */
+			if (request)
+				rbuf = request->ctx->engine[ring->id].ringbuf;
+			else
+				rbuf = ring->default_context->engine[ring->id].ringbuf;
+		} else
+			rbuf = ring->buffer;
+
+		error->ring[i].cpu_ring_head = rbuf->head;
+		error->ring[i].cpu_ring_tail = rbuf->tail;
+
 		error->ring[i].ringbuffer =
-			i915_error_ggtt_object_create(dev_priv, ring->buffer->obj);
+			i915_error_ggtt_object_create(dev_priv, rbuf->obj);
 
-		if (ring->status_page.obj)
-			error->ring[i].hws_page =
-				i915_error_ggtt_object_create(dev_priv, ring->status_page.obj);
+		error->ring[i].hws_page =
+			i915_error_ggtt_object_create(dev_priv, ring->status_page.obj);
 
 		i915_gem_record_active_context(ring, error, &error->ring[i]);
 
@@ -1049,9 +1089,14 @@ static void i915_gem_capture_vm(struct drm_i915_private *dev_priv,
 	list_for_each_entry(vma, &vm->active_list, mm_list)
 		i++;
 	error->active_bo_count[ndx] = i;
-	list_for_each_entry(obj, &dev_priv->mm.bound_list, global_list)
-		if (i915_gem_obj_is_pinned(obj))
-			i++;
+
+	list_for_each_entry(obj, &dev_priv->mm.bound_list, global_list) {
+		list_for_each_entry(vma, &obj->vma_list, vma_link)
+			if (vma->vm == vm && vma->pin_count > 0) {
+				i++;
+				break;
+			}
+	}
 	error->pinned_bo_count[ndx] = i - error->active_bo_count[ndx];
 
 	if (i) {
@@ -1070,7 +1115,7 @@ static void i915_gem_capture_vm(struct drm_i915_private *dev_priv,
 		error->pinned_bo_count[ndx] =
 			capture_pinned_bo(pinned_bo,
 					  error->pinned_bo_count[ndx],
-					  &dev_priv->mm.bound_list);
+					  &dev_priv->mm.bound_list, vm);
 	error->active_bo[ndx] = active_bo;
 	error->pinned_bo[ndx] = pinned_bo;
 }
@@ -1091,8 +1136,25 @@ static void i915_gem_capture_buffers(struct drm_i915_private *dev_priv,
 	error->pinned_bo_count = kcalloc(cnt, sizeof(*error->pinned_bo_count),
 					 GFP_ATOMIC);
 
-	list_for_each_entry(vm, &dev_priv->vm_list, global_link)
-		i915_gem_capture_vm(dev_priv, error, vm, i++);
+	if (error->active_bo == NULL ||
+	    error->pinned_bo == NULL ||
+	    error->active_bo_count == NULL ||
+	    error->pinned_bo_count == NULL) {
+		kfree(error->active_bo);
+		kfree(error->active_bo_count);
+		kfree(error->pinned_bo);
+		kfree(error->pinned_bo_count);
+
+		error->active_bo = NULL;
+		error->active_bo_count = NULL;
+		error->pinned_bo = NULL;
+		error->pinned_bo_count = NULL;
+	} else {
+		list_for_each_entry(vm, &dev_priv->vm_list, global_link)
+			i915_gem_capture_vm(dev_priv, error, vm, i++);
+
+		error->vm_count = cnt;
+	}
 }
 
 /* Capture all registers which don't fit into another category. */
@@ -1295,11 +1357,11 @@ void i915_destroy_error_state(struct drm_device *dev)
 		kref_put(&error->ref, i915_error_state_free);
 }
 
-const char *i915_cache_level_str(int type)
+const char *i915_cache_level_str(struct drm_i915_private *i915, int type)
 {
 	switch (type) {
 	case I915_CACHE_NONE: return " uncached";
-	case I915_CACHE_LLC: return " snooped or LLC";
+	case I915_CACHE_LLC: return HAS_LLC(i915) ? " LLC" : " snooped";
 	case I915_CACHE_L3_LLC: return " L3+LLC";
 	case I915_CACHE_WT: return " WT";
 	default: return "";
diff --git a/drivers/gpu/drm/i915/i915_irq.c b/drivers/gpu/drm/i915/i915_irq.c
index 390ccc2a3096..f66392b6e287 100644
--- a/drivers/gpu/drm/i915/i915_irq.c
+++ b/drivers/gpu/drm/i915/i915_irq.c
@@ -151,7 +151,7 @@ ironlake_disable_display_irq(struct drm_i915_private *dev_priv, u32 mask)
 {
 	assert_spin_locked(&dev_priv->irq_lock);
 
-	if (!intel_irqs_enabled(dev_priv))
+	if (WARN_ON(!intel_irqs_enabled(dev_priv)))
 		return;
 
 	if ((dev_priv->irq_mask & mask) != mask) {
@@ -238,7 +238,7 @@ static bool ivb_can_enable_err_int(struct drm_device *dev)
 
 	assert_spin_locked(&dev_priv->irq_lock);
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
 
 		if (crtc->cpu_fifo_underrun_disabled)
@@ -296,7 +296,7 @@ static bool cpt_can_enable_serr_int(struct drm_device *dev)
 
 	assert_spin_locked(&dev_priv->irq_lock);
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
 
 		if (crtc->pch_fifo_underrun_disabled)
@@ -497,7 +497,7 @@ static bool __intel_set_cpu_fifo_underrun_reporting(struct drm_device *dev,
 	old = !intel_crtc->cpu_fifo_underrun_disabled;
 	intel_crtc->cpu_fifo_underrun_disabled = !enable;
 
-	if (INTEL_INFO(dev)->gen < 5 || IS_VALLEYVIEW(dev))
+	if (HAS_GMCH_DISPLAY(dev))
 		i9xx_set_fifo_underrun_reporting(dev, pipe, enable, old);
 	else if (IS_GEN5(dev) || IS_GEN6(dev))
 		ironlake_set_fifo_underrun_reporting(dev, pipe, enable);
@@ -1020,7 +1020,7 @@ static int i915_get_crtc_scanoutpos(struct drm_device *dev, int pipe,
 
 	/* In vblank? */
 	if (in_vbl)
-		ret |= DRM_SCANOUTPOS_INVBL;
+		ret |= DRM_SCANOUTPOS_IN_VBLANK;
 
 	return ret;
 }
@@ -1189,8 +1189,8 @@ static void i915_hotplug_work_func(struct work_struct *work)
 	  * some connectors */
 	if (hpd_disabled) {
 		drm_kms_helper_poll_enable(dev);
-		mod_timer(&dev_priv->hotplug_reenable_timer,
-			  jiffies + msecs_to_jiffies(I915_REENABLE_HOTPLUG_DELAY));
+		mod_delayed_work(system_wq, &dev_priv->hotplug_reenable_work,
+				 msecs_to_jiffies(I915_REENABLE_HOTPLUG_DELAY));
 	}
 
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
@@ -1213,11 +1213,6 @@ static void i915_hotplug_work_func(struct work_struct *work)
 		drm_kms_helper_hotplug_event(dev);
 }
 
-static void intel_hpd_irq_uninstall(struct drm_i915_private *dev_priv)
-{
-	del_timer_sync(&dev_priv->hotplug_reenable_timer);
-}
-
 static void ironlake_rps_change_irq_handler(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1327,10 +1322,10 @@ static u32 vlv_c0_residency(struct drm_i915_private *dev_priv,
  * @dev_priv: DRM device private
  *
  */
-static u32 vlv_calc_delay_from_C0_counters(struct drm_i915_private *dev_priv)
+static int vlv_calc_delay_from_C0_counters(struct drm_i915_private *dev_priv)
 {
 	u32 residency_C0_up = 0, residency_C0_down = 0;
-	u8 new_delay, adj;
+	int new_delay, adj;
 
 	dev_priv->rps.ei_interrupt_count++;
 
@@ -1632,6 +1627,7 @@ static irqreturn_t gen8_gt_irq_handler(struct drm_device *dev,
 				       struct drm_i915_private *dev_priv,
 				       u32 master_ctl)
 {
+	struct intel_engine_cs *ring;
 	u32 rcs, bcs, vcs;
 	uint32_t tmp = 0;
 	irqreturn_t ret = IRQ_NONE;
@@ -1641,12 +1637,20 @@ static irqreturn_t gen8_gt_irq_handler(struct drm_device *dev,
 		if (tmp) {
 			I915_WRITE(GEN8_GT_IIR(0), tmp);
 			ret = IRQ_HANDLED;
+
 			rcs = tmp >> GEN8_RCS_IRQ_SHIFT;
-			bcs = tmp >> GEN8_BCS_IRQ_SHIFT;
+			ring = &dev_priv->ring[RCS];
 			if (rcs & GT_RENDER_USER_INTERRUPT)
-				notify_ring(dev, &dev_priv->ring[RCS]);
+				notify_ring(dev, ring);
+			if (rcs & GT_CONTEXT_SWITCH_INTERRUPT)
+				intel_execlists_handle_ctx_events(ring);
+
+			bcs = tmp >> GEN8_BCS_IRQ_SHIFT;
+			ring = &dev_priv->ring[BCS];
 			if (bcs & GT_RENDER_USER_INTERRUPT)
-				notify_ring(dev, &dev_priv->ring[BCS]);
+				notify_ring(dev, ring);
+			if (bcs & GT_CONTEXT_SWITCH_INTERRUPT)
+				intel_execlists_handle_ctx_events(ring);
 		} else
 			DRM_ERROR("The master control interrupt lied (GT0)!\n");
 	}
@@ -1656,12 +1660,20 @@ static irqreturn_t gen8_gt_irq_handler(struct drm_device *dev,
 		if (tmp) {
 			I915_WRITE(GEN8_GT_IIR(1), tmp);
 			ret = IRQ_HANDLED;
+
 			vcs = tmp >> GEN8_VCS1_IRQ_SHIFT;
+			ring = &dev_priv->ring[VCS];
 			if (vcs & GT_RENDER_USER_INTERRUPT)
-				notify_ring(dev, &dev_priv->ring[VCS]);
+				notify_ring(dev, ring);
+			if (vcs & GT_CONTEXT_SWITCH_INTERRUPT)
+				intel_execlists_handle_ctx_events(ring);
+
 			vcs = tmp >> GEN8_VCS2_IRQ_SHIFT;
+			ring = &dev_priv->ring[VCS2];
 			if (vcs & GT_RENDER_USER_INTERRUPT)
-				notify_ring(dev, &dev_priv->ring[VCS2]);
+				notify_ring(dev, ring);
+			if (vcs & GT_CONTEXT_SWITCH_INTERRUPT)
+				intel_execlists_handle_ctx_events(ring);
 		} else
 			DRM_ERROR("The master control interrupt lied (GT1)!\n");
 	}
@@ -1682,9 +1694,13 @@ static irqreturn_t gen8_gt_irq_handler(struct drm_device *dev,
 		if (tmp) {
 			I915_WRITE(GEN8_GT_IIR(3), tmp);
 			ret = IRQ_HANDLED;
+
 			vcs = tmp >> GEN8_VECS_IRQ_SHIFT;
+			ring = &dev_priv->ring[VECS];
 			if (vcs & GT_RENDER_USER_INTERRUPT)
-				notify_ring(dev, &dev_priv->ring[VECS]);
+				notify_ring(dev, ring);
+			if (vcs & GT_CONTEXT_SWITCH_INTERRUPT)
+				intel_execlists_handle_ctx_events(ring);
 		} else
 			DRM_ERROR("The master control interrupt lied (GT3)!\n");
 	}
@@ -1695,7 +1711,7 @@ static irqreturn_t gen8_gt_irq_handler(struct drm_device *dev,
 #define HPD_STORM_DETECT_PERIOD 1000
 #define HPD_STORM_THRESHOLD 5
 
-static int ilk_port_to_hotplug_shift(enum port port)
+static int pch_port_to_hotplug_shift(enum port port)
 {
 	switch (port) {
 	case PORT_A:
@@ -1711,7 +1727,7 @@ static int ilk_port_to_hotplug_shift(enum port port)
 	}
 }
 
-static int g4x_port_to_hotplug_shift(enum port port)
+static int i915_port_to_hotplug_shift(enum port port)
 {
 	switch (port) {
 	case PORT_A:
@@ -1769,15 +1785,17 @@ static inline void intel_hpd_irq_handler(struct drm_device *dev,
 		if (port && dev_priv->hpd_irq_port[port]) {
 			bool long_hpd;
 
-			if (IS_G4X(dev)) {
-				dig_shift = g4x_port_to_hotplug_shift(port);
-				long_hpd = (hotplug_trigger >> dig_shift) & PORTB_HOTPLUG_LONG_DETECT;
-			} else {
-				dig_shift = ilk_port_to_hotplug_shift(port);
+			if (HAS_PCH_SPLIT(dev)) {
+				dig_shift = pch_port_to_hotplug_shift(port);
 				long_hpd = (dig_hotplug_reg >> dig_shift) & PORTB_HOTPLUG_LONG_DETECT;
+			} else {
+				dig_shift = i915_port_to_hotplug_shift(port);
+				long_hpd = (hotplug_trigger >> dig_shift) & PORTB_HOTPLUG_LONG_DETECT;
 			}
 
-			DRM_DEBUG_DRIVER("digital hpd port %d %d\n", port, long_hpd);
+			DRM_DEBUG_DRIVER("digital hpd port %c - %s\n",
+					 port_name(port),
+					 long_hpd ? "long" : "short");
 			/* for long HPD pulses we want to have the digital queue happen,
 			   but we still want HPD storm detection to function. */
 			if (long_hpd) {
@@ -1989,14 +2007,9 @@ static void gen6_rps_irq_handler(struct drm_i915_private *dev_priv, u32 pm_iir)
 
 static bool intel_pipe_handle_vblank(struct drm_device *dev, enum pipe pipe)
 {
-	struct intel_crtc *crtc;
-
 	if (!drm_handle_vblank(dev, pipe))
 		return false;
 
-	crtc = to_intel_crtc(intel_get_crtc_for_pipe(dev, pipe));
-	wake_up(&crtc->vbl_wait);
-
 	return true;
 }
 
@@ -2007,7 +2020,7 @@ static void valleyview_pipestat_irq_handler(struct drm_device *dev, u32 iir)
 	int pipe;
 
 	spin_lock(&dev_priv->irq_lock);
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		int reg;
 		u32 mask, iir_bit = 0;
 
@@ -2052,9 +2065,10 @@ static void valleyview_pipestat_irq_handler(struct drm_device *dev, u32 iir)
 	}
 	spin_unlock(&dev_priv->irq_lock);
 
-	for_each_pipe(pipe) {
-		if (pipe_stats[pipe] & PIPE_START_VBLANK_INTERRUPT_STATUS)
-			intel_pipe_handle_vblank(dev, pipe);
+	for_each_pipe(dev_priv, pipe) {
+		if (pipe_stats[pipe] & PIPE_START_VBLANK_INTERRUPT_STATUS &&
+		    intel_pipe_handle_vblank(dev, pipe))
+			intel_check_page_flip(dev, pipe);
 
 		if (pipe_stats[pipe] & PLANE_FLIP_DONE_INT_STATUS_VLV) {
 			intel_prepare_page_flip(dev, pipe);
@@ -2221,7 +2235,7 @@ static void ibx_irq_handler(struct drm_device *dev, u32 pch_iir)
 		DRM_ERROR("PCH poison interrupt\n");
 
 	if (pch_iir & SDE_FDI_MASK)
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			DRM_DEBUG_DRIVER("  pipe %c FDI IIR: 0x%08x\n",
 					 pipe_name(pipe),
 					 I915_READ(FDI_RX_IIR(pipe)));
@@ -2252,7 +2266,7 @@ static void ivb_err_int_handler(struct drm_device *dev)
 	if (err_int & ERR_INT_POISON)
 		DRM_ERROR("Poison interrupt\n");
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		if (err_int & ERR_INT_FIFO_UNDERRUN(pipe)) {
 			if (intel_set_cpu_fifo_underrun_reporting(dev, pipe,
 								  false))
@@ -2329,7 +2343,7 @@ static void cpt_irq_handler(struct drm_device *dev, u32 pch_iir)
 		DRM_DEBUG_DRIVER("Audio CP change interrupt\n");
 
 	if (pch_iir & SDE_FDI_MASK_CPT)
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			DRM_DEBUG_DRIVER("  pipe %c FDI IIR: 0x%08x\n",
 					 pipe_name(pipe),
 					 I915_READ(FDI_RX_IIR(pipe)));
@@ -2352,9 +2366,10 @@ static void ilk_display_irq_handler(struct drm_device *dev, u32 de_iir)
 	if (de_iir & DE_POISON)
 		DRM_ERROR("Poison interrupt\n");
 
-	for_each_pipe(pipe) {
-		if (de_iir & DE_PIPE_VBLANK(pipe))
-			intel_pipe_handle_vblank(dev, pipe);
+	for_each_pipe(dev_priv, pipe) {
+		if (de_iir & DE_PIPE_VBLANK(pipe) &&
+		    intel_pipe_handle_vblank(dev, pipe))
+			intel_check_page_flip(dev, pipe);
 
 		if (de_iir & DE_PIPE_FIFO_UNDERRUN(pipe))
 			if (intel_set_cpu_fifo_underrun_reporting(dev, pipe, false))
@@ -2402,9 +2417,10 @@ static void ivb_display_irq_handler(struct drm_device *dev, u32 de_iir)
 	if (de_iir & DE_GSE_IVB)
 		intel_opregion_asle_intr(dev);
 
-	for_each_pipe(pipe) {
-		if (de_iir & (DE_PIPE_VBLANK_IVB(pipe)))
-			intel_pipe_handle_vblank(dev, pipe);
+	for_each_pipe(dev_priv, pipe) {
+		if (de_iir & (DE_PIPE_VBLANK_IVB(pipe)) &&
+		    intel_pipe_handle_vblank(dev, pipe))
+			intel_check_page_flip(dev, pipe);
 
 		/* plane/pipes map 1:1 on ilk+ */
 		if (de_iir & DE_PLANE_FLIP_DONE_IVB(pipe)) {
@@ -2549,7 +2565,7 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 			DRM_ERROR("The master control interrupt lied (DE PORT)!\n");
 	}
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		uint32_t pipe_iir;
 
 		if (!(master_ctl & GEN8_DE_PIPE_IRQ(pipe)))
@@ -2559,8 +2575,9 @@ static irqreturn_t gen8_irq_handler(int irq, void *arg)
 		if (pipe_iir) {
 			ret = IRQ_HANDLED;
 			I915_WRITE(GEN8_DE_PIPE_IIR(pipe), pipe_iir);
-			if (pipe_iir & GEN8_PIPE_VBLANK)
-				intel_pipe_handle_vblank(dev, pipe);
+			if (pipe_iir & GEN8_PIPE_VBLANK &&
+			    intel_pipe_handle_vblank(dev, pipe))
+				intel_check_page_flip(dev, pipe);
 
 			if (pipe_iir & GEN8_PIPE_PRIMARY_FLIP_DONE) {
 				intel_prepare_page_flip(dev, pipe);
@@ -2768,7 +2785,7 @@ static void i915_report_and_clear_eir(struct drm_device *dev)
 
 	if (eir & I915_ERROR_MEMORY_REFRESH) {
 		pr_err("memory refresh error:\n");
-		for_each_pipe(pipe)
+		for_each_pipe(dev_priv, pipe)
 			pr_err("pipe %c stat: 0x%08x\n",
 			       pipe_name(pipe), I915_READ(PIPESTAT(pipe)));
 		/* pipestat has already been acked */
@@ -2865,52 +2882,6 @@ void i915_handle_error(struct drm_device *dev, bool wedged,
 	schedule_work(&dev_priv->gpu_error.work);
 }
 
-static void __always_unused i915_pageflip_stall_check(struct drm_device *dev, int pipe)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct drm_i915_gem_object *obj;
-	struct intel_unpin_work *work;
-	unsigned long flags;
-	bool stall_detected;
-
-	/* Ignore early vblank irqs */
-	if (intel_crtc == NULL)
-		return;
-
-	spin_lock_irqsave(&dev->event_lock, flags);
-	work = intel_crtc->unpin_work;
-
-	if (work == NULL ||
-	    atomic_read(&work->pending) >= INTEL_FLIP_COMPLETE ||
-	    !work->enable_stall_check) {
-		/* Either the pending flip IRQ arrived, or we're too early. Don't check */
-		spin_unlock_irqrestore(&dev->event_lock, flags);
-		return;
-	}
-
-	/* Potential stall - if we see that the flip has happened, assume a missed interrupt */
-	obj = work->pending_flip_obj;
-	if (INTEL_INFO(dev)->gen >= 4) {
-		int dspsurf = DSPSURF(intel_crtc->plane);
-		stall_detected = I915_HI_DISPBASE(I915_READ(dspsurf)) ==
-					i915_gem_obj_ggtt_offset(obj);
-	} else {
-		int dspaddr = DSPADDR(intel_crtc->plane);
-		stall_detected = I915_READ(dspaddr) == (i915_gem_obj_ggtt_offset(obj) +
-							crtc->y * crtc->primary->fb->pitches[0] +
-							crtc->x * crtc->primary->fb->bits_per_pixel/8);
-	}
-
-	spin_unlock_irqrestore(&dev->event_lock, flags);
-
-	if (stall_detected) {
-		DRM_DEBUG_DRIVER("Pageflip stall detected\n");
-		intel_prepare_page_flip(dev, intel_crtc->plane);
-	}
-}
-
 /* Called from drm generic code, passed 'crtc' which
  * we use as a pipe index
  */
@@ -3446,7 +3417,7 @@ static void valleyview_irq_preinstall(struct drm_device *dev)
 
 	I915_WRITE(PORT_HOTPLUG_EN, 0);
 	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0xffff);
 	I915_WRITE(VLV_IIR, 0xffffffff);
 	I915_WRITE(VLV_IMR, 0xffffffff);
@@ -3472,7 +3443,7 @@ static void gen8_irq_reset(struct drm_device *dev)
 
 	gen8_gt_irq_reset(dev_priv);
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		if (intel_display_power_enabled(dev_priv,
 						POWER_DOMAIN_PIPE(pipe)))
 			GEN8_IRQ_RESET_NDX(DE_PIPE, pipe);
@@ -3487,12 +3458,13 @@ static void gen8_irq_reset(struct drm_device *dev)
 void gen8_irq_power_well_post_enable(struct drm_i915_private *dev_priv)
 {
 	unsigned long irqflags;
+	uint32_t extra_ier = GEN8_PIPE_VBLANK | GEN8_PIPE_FIFO_UNDERRUN;
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
 	GEN8_IRQ_INIT_NDX(DE_PIPE, PIPE_B, dev_priv->de_irq_mask[PIPE_B],
-			  ~dev_priv->de_irq_mask[PIPE_B]);
+			  ~dev_priv->de_irq_mask[PIPE_B] | extra_ier);
 	GEN8_IRQ_INIT_NDX(DE_PIPE, PIPE_C, dev_priv->de_irq_mask[PIPE_C],
-			  ~dev_priv->de_irq_mask[PIPE_C]);
+			  ~dev_priv->de_irq_mask[PIPE_C] | extra_ier);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 }
 
@@ -3515,7 +3487,7 @@ static void cherryview_irq_preinstall(struct drm_device *dev)
 	I915_WRITE(PORT_HOTPLUG_EN, 0);
 	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0xffff);
 
 	I915_WRITE(VLV_IMR, 0xffffffff);
@@ -3527,18 +3499,17 @@ static void cherryview_irq_preinstall(struct drm_device *dev)
 static void ibx_hpd_irq_setup(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *intel_encoder;
 	u32 hotplug_irqs, hotplug, enabled_irqs = 0;
 
 	if (HAS_PCH_IBX(dev)) {
 		hotplug_irqs = SDE_HOTPLUG_MASK;
-		list_for_each_entry(intel_encoder, &mode_config->encoder_list, base.head)
+		for_each_intel_encoder(dev, intel_encoder)
 			if (dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark == HPD_ENABLED)
 				enabled_irqs |= hpd_ibx[intel_encoder->hpd_pin];
 	} else {
 		hotplug_irqs = SDE_HOTPLUG_MASK_CPT;
-		list_for_each_entry(intel_encoder, &mode_config->encoder_list, base.head)
+		for_each_intel_encoder(dev, intel_encoder)
 			if (dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark == HPD_ENABLED)
 				enabled_irqs |= hpd_cpt[intel_encoder->hpd_pin];
 	}
@@ -3787,28 +3758,31 @@ static int valleyview_irq_postinstall(struct drm_device *dev)
 
 static void gen8_gt_irq_postinstall(struct drm_i915_private *dev_priv)
 {
-	int i;
-
 	/* These are interrupts we'll toggle with the ring mask register */
 	uint32_t gt_interrupts[] = {
 		GT_RENDER_USER_INTERRUPT << GEN8_RCS_IRQ_SHIFT |
+			GT_CONTEXT_SWITCH_INTERRUPT << GEN8_RCS_IRQ_SHIFT |
 			GT_RENDER_L3_PARITY_ERROR_INTERRUPT |
-			GT_RENDER_USER_INTERRUPT << GEN8_BCS_IRQ_SHIFT,
+			GT_RENDER_USER_INTERRUPT << GEN8_BCS_IRQ_SHIFT |
+			GT_CONTEXT_SWITCH_INTERRUPT << GEN8_BCS_IRQ_SHIFT,
 		GT_RENDER_USER_INTERRUPT << GEN8_VCS1_IRQ_SHIFT |
-			GT_RENDER_USER_INTERRUPT << GEN8_VCS2_IRQ_SHIFT,
+			GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VCS1_IRQ_SHIFT |
+			GT_RENDER_USER_INTERRUPT << GEN8_VCS2_IRQ_SHIFT |
+			GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VCS2_IRQ_SHIFT,
 		0,
-		GT_RENDER_USER_INTERRUPT << GEN8_VECS_IRQ_SHIFT
+		GT_RENDER_USER_INTERRUPT << GEN8_VECS_IRQ_SHIFT |
+			GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VECS_IRQ_SHIFT
 		};
 
-	for (i = 0; i < ARRAY_SIZE(gt_interrupts); i++)
-		GEN8_IRQ_INIT_NDX(GT, i, ~gt_interrupts[i], gt_interrupts[i]);
-
 	dev_priv->pm_irq_mask = 0xffffffff;
+	GEN8_IRQ_INIT_NDX(GT, 0, ~gt_interrupts[0], gt_interrupts[0]);
+	GEN8_IRQ_INIT_NDX(GT, 1, ~gt_interrupts[1], gt_interrupts[1]);
+	GEN8_IRQ_INIT_NDX(GT, 2, dev_priv->pm_irq_mask, dev_priv->pm_rps_events);
+	GEN8_IRQ_INIT_NDX(GT, 3, ~gt_interrupts[3], gt_interrupts[3]);
 }
 
 static void gen8_de_irq_postinstall(struct drm_i915_private *dev_priv)
 {
-	struct drm_device *dev = dev_priv->dev;
 	uint32_t de_pipe_masked = GEN8_PIPE_PRIMARY_FLIP_DONE |
 		GEN8_PIPE_CDCLK_CRC_DONE |
 		GEN8_DE_PIPE_IRQ_FAULT_ERRORS;
@@ -3819,7 +3793,7 @@ static void gen8_de_irq_postinstall(struct drm_i915_private *dev_priv)
 	dev_priv->de_irq_mask[PIPE_B] = ~de_pipe_masked;
 	dev_priv->de_irq_mask[PIPE_C] = ~de_pipe_masked;
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		if (intel_display_power_enabled(dev_priv,
 				POWER_DOMAIN_PIPE(pipe)))
 			GEN8_IRQ_INIT_NDX(DE_PIPE, pipe,
@@ -3864,12 +3838,12 @@ static int cherryview_irq_postinstall(struct drm_device *dev)
 	 */
 	dev_priv->irq_mask = ~enable_mask;
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0xffff);
 
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
 	i915_enable_pipestat(dev_priv, PIPE_A, PIPE_GMBUS_INTERRUPT_STATUS);
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		i915_enable_pipestat(dev_priv, pipe, pipestat_enable);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
 
@@ -3892,8 +3866,6 @@ static void gen8_irq_uninstall(struct drm_device *dev)
 	if (!dev_priv)
 		return;
 
-	intel_hpd_irq_uninstall(dev_priv);
-
 	gen8_irq_reset(dev);
 }
 
@@ -3908,9 +3880,7 @@ static void valleyview_irq_uninstall(struct drm_device *dev)
 
 	I915_WRITE(VLV_MASTER_IER, 0);
 
-	intel_hpd_irq_uninstall(dev_priv);
-
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0xffff);
 
 	I915_WRITE(HWSTAM, 0xffffffff);
@@ -3972,7 +3942,7 @@ do {							\
 	I915_WRITE(PORT_HOTPLUG_EN, 0);
 	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0xffff);
 
 	I915_WRITE(VLV_IMR, 0xffffffff);
@@ -3988,8 +3958,6 @@ static void ironlake_irq_uninstall(struct drm_device *dev)
 	if (!dev_priv)
 		return;
 
-	intel_hpd_irq_uninstall(dev_priv);
-
 	ironlake_irq_reset(dev);
 }
 
@@ -3998,7 +3966,7 @@ static void i8xx_irq_preinstall(struct drm_device * dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe;
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0);
 	I915_WRITE16(IMR, 0xffff);
 	I915_WRITE16(IER, 0x0);
@@ -4052,7 +4020,7 @@ static bool i8xx_handle_vblank(struct drm_device *dev,
 		return false;
 
 	if ((iir & flip_pending) == 0)
-		return false;
+		goto check_page_flip;
 
 	intel_prepare_page_flip(dev, plane);
 
@@ -4063,11 +4031,14 @@ static bool i8xx_handle_vblank(struct drm_device *dev,
 	 * an interrupt per se, we watch for the change at vblank.
 	 */
 	if (I915_READ16(ISR) & flip_pending)
-		return false;
+		goto check_page_flip;
 
 	intel_finish_page_flip(dev, pipe);
-
 	return true;
+
+check_page_flip:
+	intel_check_page_flip(dev, pipe);
+	return false;
 }
 
 static irqreturn_t i8xx_irq_handler(int irq, void *arg)
@@ -4098,7 +4069,7 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 					  "Command parser error, iir 0x%08x",
 					  iir);
 
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
 			int reg = PIPESTAT(pipe);
 			pipe_stats[pipe] = I915_READ(reg);
 
@@ -4118,7 +4089,7 @@ static irqreturn_t i8xx_irq_handler(int irq, void *arg)
 		if (iir & I915_USER_INTERRUPT)
 			notify_ring(dev, &dev_priv->ring[RCS]);
 
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
 			int plane = pipe;
 			if (HAS_FBC(dev))
 				plane = !plane;
@@ -4146,7 +4117,7 @@ static void i8xx_irq_uninstall(struct drm_device * dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe;
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		/* Clear enable bits; then clear status bits */
 		I915_WRITE(PIPESTAT(pipe), 0);
 		I915_WRITE(PIPESTAT(pipe), I915_READ(PIPESTAT(pipe)));
@@ -4167,7 +4138,7 @@ static void i915_irq_preinstall(struct drm_device * dev)
 	}
 
 	I915_WRITE16(HWSTAM, 0xeffe);
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0);
 	I915_WRITE(IMR, 0xffffffff);
 	I915_WRITE(IER, 0x0);
@@ -4237,7 +4208,7 @@ static bool i915_handle_vblank(struct drm_device *dev,
 		return false;
 
 	if ((iir & flip_pending) == 0)
-		return false;
+		goto check_page_flip;
 
 	intel_prepare_page_flip(dev, plane);
 
@@ -4248,11 +4219,14 @@ static bool i915_handle_vblank(struct drm_device *dev,
 	 * an interrupt per se, we watch for the change at vblank.
 	 */
 	if (I915_READ(ISR) & flip_pending)
-		return false;
+		goto check_page_flip;
 
 	intel_finish_page_flip(dev, pipe);
-
 	return true;
+
+check_page_flip:
+	intel_check_page_flip(dev, pipe);
+	return false;
 }
 
 static irqreturn_t i915_irq_handler(int irq, void *arg)
@@ -4282,7 +4256,7 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 					  "Command parser error, iir 0x%08x",
 					  iir);
 
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
 			int reg = PIPESTAT(pipe);
 			pipe_stats[pipe] = I915_READ(reg);
 
@@ -4308,7 +4282,7 @@ static irqreturn_t i915_irq_handler(int irq, void *arg)
 		if (iir & I915_USER_INTERRUPT)
 			notify_ring(dev, &dev_priv->ring[RCS]);
 
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
 			int plane = pipe;
 			if (HAS_FBC(dev))
 				plane = !plane;
@@ -4360,15 +4334,13 @@ static void i915_irq_uninstall(struct drm_device * dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe;
 
-	intel_hpd_irq_uninstall(dev_priv);
-
 	if (I915_HAS_HOTPLUG(dev)) {
 		I915_WRITE(PORT_HOTPLUG_EN, 0);
 		I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 	}
 
 	I915_WRITE16(HWSTAM, 0xffff);
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		/* Clear enable bits; then clear status bits */
 		I915_WRITE(PIPESTAT(pipe), 0);
 		I915_WRITE(PIPESTAT(pipe), I915_READ(PIPESTAT(pipe)));
@@ -4388,7 +4360,7 @@ static void i965_irq_preinstall(struct drm_device * dev)
 	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 
 	I915_WRITE(HWSTAM, 0xeffe);
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0);
 	I915_WRITE(IMR, 0xffffffff);
 	I915_WRITE(IER, 0x0);
@@ -4457,7 +4429,6 @@ static int i965_irq_postinstall(struct drm_device *dev)
 static void i915_hpd_irq_setup(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *intel_encoder;
 	u32 hotplug_en;
 
@@ -4468,7 +4439,7 @@ static void i915_hpd_irq_setup(struct drm_device *dev)
 		hotplug_en &= ~HOTPLUG_INT_EN_MASK;
 		/* Note HDMI and DP share hotplug bits */
 		/* enable bits are the same for all generations */
-		list_for_each_entry(intel_encoder, &mode_config->encoder_list, base.head)
+		for_each_intel_encoder(dev, intel_encoder)
 			if (dev_priv->hpd_stats[intel_encoder->hpd_pin].hpd_mark == HPD_ENABLED)
 				hotplug_en |= hpd_mask_i915[intel_encoder->hpd_pin];
 		/* Programming the CRT detection parameters tends
@@ -4514,7 +4485,7 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 					  "Command parser error, iir 0x%08x",
 					  iir);
 
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
 			int reg = PIPESTAT(pipe);
 			pipe_stats[pipe] = I915_READ(reg);
 
@@ -4545,7 +4516,7 @@ static irqreturn_t i965_irq_handler(int irq, void *arg)
 		if (iir & I915_BSD_USER_INTERRUPT)
 			notify_ring(dev, &dev_priv->ring[VCS]);
 
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
 			if (pipe_stats[pipe] & PIPE_START_VBLANK_INTERRUPT_STATUS &&
 			    i915_handle_vblank(dev, pipe, pipe, iir))
 				flip_mask &= ~DISPLAY_PLANE_FLIP_PENDING(pipe);
@@ -4598,31 +4569,33 @@ static void i965_irq_uninstall(struct drm_device * dev)
 	if (!dev_priv)
 		return;
 
-	intel_hpd_irq_uninstall(dev_priv);
-
 	I915_WRITE(PORT_HOTPLUG_EN, 0);
 	I915_WRITE(PORT_HOTPLUG_STAT, I915_READ(PORT_HOTPLUG_STAT));
 
 	I915_WRITE(HWSTAM, 0xffffffff);
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe), 0);
 	I915_WRITE(IMR, 0xffffffff);
 	I915_WRITE(IER, 0x0);
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		I915_WRITE(PIPESTAT(pipe),
 			   I915_READ(PIPESTAT(pipe)) & 0x8000ffff);
 	I915_WRITE(IIR, I915_READ(IIR));
 }
 
-static void intel_hpd_irq_reenable(unsigned long data)
+static void intel_hpd_irq_reenable(struct work_struct *work)
 {
-	struct drm_i915_private *dev_priv = (struct drm_i915_private *)data;
+	struct drm_i915_private *dev_priv =
+		container_of(work, typeof(*dev_priv),
+			     hotplug_reenable_work.work);
 	struct drm_device *dev = dev_priv->dev;
 	struct drm_mode_config *mode_config = &dev->mode_config;
 	unsigned long irqflags;
 	int i;
 
+	intel_runtime_pm_get(dev_priv);
+
 	spin_lock_irqsave(&dev_priv->irq_lock, irqflags);
 	for (i = (HPD_NONE + 1); i < HPD_NUM_PINS; i++) {
 		struct drm_connector *connector;
@@ -4648,6 +4621,8 @@ static void intel_hpd_irq_reenable(unsigned long data)
 	if (dev_priv->display.hpd_irq_setup)
 		dev_priv->display.hpd_irq_setup(dev);
 	spin_unlock_irqrestore(&dev_priv->irq_lock, irqflags);
+
+	intel_runtime_pm_put(dev_priv);
 }
 
 void intel_irq_init(struct drm_device *dev)
@@ -4661,8 +4636,8 @@ void intel_irq_init(struct drm_device *dev)
 	INIT_WORK(&dev_priv->l3_parity.error_work, ivybridge_parity_work);
 
 	/* Let's track the enabled rps events */
-	if (IS_VALLEYVIEW(dev))
-		/* WaGsvRC0ResidenncyMethod:VLV */
+	if (IS_VALLEYVIEW(dev) && !IS_CHERRYVIEW(dev))
+		/* WaGsvRC0ResidencyMethod:vlv */
 		dev_priv->pm_rps_events = GEN6_PM_RP_UP_EI_EXPIRED;
 	else
 		dev_priv->pm_rps_events = GEN6_PM_RPS_EVENTS;
@@ -4670,8 +4645,8 @@ void intel_irq_init(struct drm_device *dev)
 	setup_timer(&dev_priv->gpu_error.hangcheck_timer,
 		    i915_hangcheck_elapsed,
 		    (unsigned long) dev);
-	setup_timer(&dev_priv->hotplug_reenable_timer, intel_hpd_irq_reenable,
-		    (unsigned long) dev_priv);
+	INIT_DELAYED_WORK(&dev_priv->hotplug_reenable_work,
+			  intel_hpd_irq_reenable);
 
 	pm_qos_add_request(&dev_priv->pm_qos, PM_QOS_CPU_DMA_LATENCY, PM_QOS_DEFAULT_VALUE);
 
@@ -4689,6 +4664,14 @@ void intel_irq_init(struct drm_device *dev)
 		dev->max_vblank_count = 0xffffff; /* only 24 bits of frame count */
 	}
 
+	/*
+	 * Opt out of the vblank disable timer on everything except gen2.
+	 * Gen2 doesn't have a hardware frame counter and so depends on
+	 * vblank interrupts to produce sane vblank seuquence numbers.
+	 */
+	if (!IS_GEN2(dev))
+		dev->vblank_disable_immediate = true;
+
 	if (drm_core_check_feature(dev, DRIVER_MODESET)) {
 		dev->driver->get_vblank_timestamp = i915_get_vblank_timestamp;
 		dev->driver->get_scanout_position = i915_get_crtc_scanoutpos;
diff --git a/drivers/gpu/drm/i915/i915_params.c b/drivers/gpu/drm/i915/i915_params.c
index 7f84dd263ee8..c91cb2033cc5 100644
--- a/drivers/gpu/drm/i915/i915_params.c
+++ b/drivers/gpu/drm/i915/i915_params.c
@@ -35,6 +35,7 @@ struct i915_params i915 __read_mostly = {
 	.vbt_sdvo_panel_type = -1,
 	.enable_rc6 = -1,
 	.enable_fbc = -1,
+	.enable_execlists = 0,
 	.enable_hangcheck = true,
 	.enable_ppgtt = -1,
 	.enable_psr = 0,
@@ -66,12 +67,12 @@ module_param_named(powersave, i915.powersave, int, 0600);
 MODULE_PARM_DESC(powersave,
 	"Enable powersavings, fbc, downclocking, etc. (default: true)");
 
-module_param_named(semaphores, i915.semaphores, int, 0400);
+module_param_named_unsafe(semaphores, i915.semaphores, int, 0400);
 MODULE_PARM_DESC(semaphores,
 	"Use semaphores for inter-ring sync "
 	"(default: -1 (use per-chip defaults))");
 
-module_param_named(enable_rc6, i915.enable_rc6, int, 0400);
+module_param_named_unsafe(enable_rc6, i915.enable_rc6, int, 0400);
 MODULE_PARM_DESC(enable_rc6,
 	"Enable power-saving render C-state 6. "
 	"Different stages can be selected via bitmask values "
@@ -79,7 +80,7 @@ MODULE_PARM_DESC(enable_rc6,
 	"For example, 3 would enable rc6 and deep rc6, and 7 would enable everything. "
 	"default: -1 (use per-chip default)");
 
-module_param_named(enable_fbc, i915.enable_fbc, int, 0600);
+module_param_named_unsafe(enable_fbc, i915.enable_fbc, int, 0600);
 MODULE_PARM_DESC(enable_fbc,
 	"Enable frame buffer compression for power savings "
 	"(default: -1 (use per-chip default))");
@@ -113,11 +114,16 @@ MODULE_PARM_DESC(enable_hangcheck,
 	"WARNING: Disabling this can cause system wide hangs. "
 	"(default: true)");
 
-module_param_named(enable_ppgtt, i915.enable_ppgtt, int, 0400);
+module_param_named_unsafe(enable_ppgtt, i915.enable_ppgtt, int, 0400);
 MODULE_PARM_DESC(enable_ppgtt,
 	"Override PPGTT usage. "
 	"(-1=auto [default], 0=disabled, 1=aliasing, 2=full)");
 
+module_param_named(enable_execlists, i915.enable_execlists, int, 0400);
+MODULE_PARM_DESC(enable_execlists,
+	"Override execlists usage. "
+	"(-1=auto, 0=disabled [default], 1=enabled)");
+
 module_param_named(enable_psr, i915.enable_psr, int, 0600);
 MODULE_PARM_DESC(enable_psr, "Enable PSR (default: false)");
 
diff --git a/drivers/gpu/drm/i915/i915_reg.h b/drivers/gpu/drm/i915/i915_reg.h
index e4d7607da2c4..c01e5f31430e 100644
--- a/drivers/gpu/drm/i915/i915_reg.h
+++ b/drivers/gpu/drm/i915/i915_reg.h
@@ -143,6 +143,14 @@
 #define GAB_CTL				0x24000
 #define   GAB_CTL_CONT_AFTER_PAGEFAULT	(1<<8)
 
+#define GEN7_BIOS_RESERVED		0x1082C0
+#define GEN7_BIOS_RESERVED_1M		(0 << 5)
+#define GEN7_BIOS_RESERVED_256K		(1 << 5)
+#define GEN8_BIOS_RESERVED_SHIFT       7
+#define GEN7_BIOS_RESERVED_MASK        0x1
+#define GEN8_BIOS_RESERVED_MASK        0x3
+
+
 /* VGA stuff */
 
 #define VGA_ST01_MDA 0x3ba
@@ -272,6 +280,7 @@
 #define   MI_SEMAPHORE_POLL		(1<<15)
 #define   MI_SEMAPHORE_SAD_GTE_SDD	(1<<12)
 #define MI_STORE_DWORD_IMM	MI_INSTR(0x20, 1)
+#define MI_STORE_DWORD_IMM_GEN8	MI_INSTR(0x20, 2)
 #define   MI_MEM_VIRTUAL	(1 << 22) /* 965+ only */
 #define MI_STORE_DWORD_INDEX	MI_INSTR(0x21, 1)
 #define   MI_STORE_DWORD_INDEX_SHIFT 2
@@ -282,6 +291,7 @@
  *   address/value pairs. Don't overdue it, though, x <= 2^4 must hold!
  */
 #define MI_LOAD_REGISTER_IMM(x)	MI_INSTR(0x22, 2*(x)-1)
+#define   MI_LRI_FORCE_POSTED		(1<<12)
 #define MI_STORE_REGISTER_MEM(x) MI_INSTR(0x24, 2*(x)-1)
 #define MI_STORE_REGISTER_MEM_GEN8(x) MI_INSTR(0x24, 3*(x)-1)
 #define   MI_SRM_LRM_GLOBAL_GTT		(1<<22)
@@ -334,16 +344,20 @@
 #define GFX_OP_DESTBUFFER_INFO	 ((0x3<<29)|(0x1d<<24)|(0x8e<<16)|1)
 #define GFX_OP_DRAWRECT_INFO     ((0x3<<29)|(0x1d<<24)|(0x80<<16)|(0x3))
 #define GFX_OP_DRAWRECT_INFO_I965  ((0x7900<<16)|0x2)
-#define SRC_COPY_BLT_CMD                ((2<<29)|(0x43<<22)|4)
+
+#define COLOR_BLT_CMD			(2<<29 | 0x40<<22 | (5-2))
+#define SRC_COPY_BLT_CMD		((2<<29)|(0x43<<22)|4)
 #define XY_SRC_COPY_BLT_CMD		((2<<29)|(0x53<<22)|6)
 #define XY_MONO_SRC_COPY_IMM_BLT	((2<<29)|(0x71<<22)|5)
-#define XY_SRC_COPY_BLT_WRITE_ALPHA	(1<<21)
-#define XY_SRC_COPY_BLT_WRITE_RGB	(1<<20)
+#define   BLT_WRITE_A			(2<<20)
+#define   BLT_WRITE_RGB			(1<<20)
+#define   BLT_WRITE_RGBA		(BLT_WRITE_RGB | BLT_WRITE_A)
 #define   BLT_DEPTH_8			(0<<24)
 #define   BLT_DEPTH_16_565		(1<<24)
 #define   BLT_DEPTH_16_1555		(2<<24)
 #define   BLT_DEPTH_32			(3<<24)
-#define   BLT_ROP_GXCOPY		(0xcc<<16)
+#define   BLT_ROP_SRC_COPY		(0xcc<<16)
+#define   BLT_ROP_COLOR_COPY		(0xf0<<16)
 #define XY_SRC_COPY_BLT_SRC_TILED	(1<<15) /* 965+ only */
 #define XY_SRC_COPY_BLT_DST_TILED	(1<<11) /* 965+ only */
 #define CMD_OP_DISPLAYBUFFER_INFO ((0x0<<29)|(0x14<<23)|2)
@@ -497,10 +511,26 @@
 #define BUNIT_REG_BISOC				0x11
 
 #define PUNIT_REG_DSPFREQ			0x36
+#define   DSPFREQSTAT_SHIFT_CHV			24
+#define   DSPFREQSTAT_MASK_CHV			(0x1f << DSPFREQSTAT_SHIFT_CHV)
+#define   DSPFREQGUAR_SHIFT_CHV			8
+#define   DSPFREQGUAR_MASK_CHV			(0x1f << DSPFREQGUAR_SHIFT_CHV)
 #define   DSPFREQSTAT_SHIFT			30
 #define   DSPFREQSTAT_MASK			(0x3 << DSPFREQSTAT_SHIFT)
 #define   DSPFREQGUAR_SHIFT			14
 #define   DSPFREQGUAR_MASK			(0x3 << DSPFREQGUAR_SHIFT)
+#define   _DP_SSC(val, pipe)			((val) << (2 * (pipe)))
+#define   DP_SSC_MASK(pipe)			_DP_SSC(0x3, (pipe))
+#define   DP_SSC_PWR_ON(pipe)			_DP_SSC(0x0, (pipe))
+#define   DP_SSC_CLK_GATE(pipe)			_DP_SSC(0x1, (pipe))
+#define   DP_SSC_RESET(pipe)			_DP_SSC(0x2, (pipe))
+#define   DP_SSC_PWR_GATE(pipe)			_DP_SSC(0x3, (pipe))
+#define   _DP_SSS(val, pipe)			((val) << (2 * (pipe) + 16))
+#define   DP_SSS_MASK(pipe)			_DP_SSS(0x3, (pipe))
+#define   DP_SSS_PWR_ON(pipe)			_DP_SSS(0x0, (pipe))
+#define   DP_SSS_CLK_GATE(pipe)			_DP_SSS(0x1, (pipe))
+#define   DP_SSS_RESET(pipe)			_DP_SSS(0x2, (pipe))
+#define   DP_SSS_PWR_GATE(pipe)			_DP_SSS(0x3, (pipe))
 
 /* See the PUNIT HAS v0.8 for the below bits */
 enum punit_power_well {
@@ -514,6 +544,11 @@ enum punit_power_well {
 	PUNIT_POWER_WELL_DPIO_TX_C_LANES_23	= 9,
 	PUNIT_POWER_WELL_DPIO_RX0		= 10,
 	PUNIT_POWER_WELL_DPIO_RX1		= 11,
+	PUNIT_POWER_WELL_DPIO_CMN_D		= 12,
+	/* FIXME: guesswork below */
+	PUNIT_POWER_WELL_DPIO_TX_D_LANES_01	= 13,
+	PUNIT_POWER_WELL_DPIO_TX_D_LANES_23	= 14,
+	PUNIT_POWER_WELL_DPIO_RX2		= 15,
 
 	PUNIT_POWER_WELL_NUM,
 };
@@ -834,8 +869,8 @@ enum punit_power_well {
 
 #define _VLV_TX_DW2_CH0			0x8288
 #define _VLV_TX_DW2_CH1			0x8488
-#define   DPIO_SWING_MARGIN_SHIFT	16
-#define   DPIO_SWING_MARGIN_MASK	(0xff << DPIO_SWING_MARGIN_SHIFT)
+#define   DPIO_SWING_MARGIN000_SHIFT	16
+#define   DPIO_SWING_MARGIN000_MASK	(0xff << DPIO_SWING_MARGIN000_SHIFT)
 #define   DPIO_UNIQ_TRANS_SCALE_SHIFT	8
 #define VLV_TX_DW2(ch) _PORT(ch, _VLV_TX_DW2_CH0, _VLV_TX_DW2_CH1)
 
@@ -843,12 +878,16 @@ enum punit_power_well {
 #define _VLV_TX_DW3_CH1			0x848c
 /* The following bit for CHV phy */
 #define   DPIO_TX_UNIQ_TRANS_SCALE_EN	(1<<27)
+#define   DPIO_SWING_MARGIN101_SHIFT	16
+#define   DPIO_SWING_MARGIN101_MASK	(0xff << DPIO_SWING_MARGIN101_SHIFT)
 #define VLV_TX_DW3(ch) _PORT(ch, _VLV_TX_DW3_CH0, _VLV_TX_DW3_CH1)
 
 #define _VLV_TX_DW4_CH0			0x8290
 #define _VLV_TX_DW4_CH1			0x8490
 #define   DPIO_SWING_DEEMPH9P5_SHIFT	24
 #define   DPIO_SWING_DEEMPH9P5_MASK	(0xff << DPIO_SWING_DEEMPH9P5_SHIFT)
+#define   DPIO_SWING_DEEMPH6P0_SHIFT	16
+#define   DPIO_SWING_DEEMPH6P0_MASK	(0xff << DPIO_SWING_DEEMPH6P0_SHIFT)
 #define VLV_TX_DW4(ch) _PORT(ch, _VLV_TX_DW4_CH0, _VLV_TX_DW4_CH1)
 
 #define _VLV_TX3_DW4_CH0		0x690
@@ -999,6 +1038,13 @@ enum punit_power_well {
 #define   PGTBL_ADDRESS_LO_MASK	0xfffff000 /* bits [31:12] */
 #define   PGTBL_ADDRESS_HI_MASK	0x000000f0 /* bits [35:32] (gen4) */
 #define PGTBL_ER	0x02024
+#define PRB0_BASE (0x2030-0x30)
+#define PRB1_BASE (0x2040-0x30) /* 830,gen3 */
+#define PRB2_BASE (0x2050-0x30) /* gen3 */
+#define SRB0_BASE (0x2100-0x30) /* gen2 */
+#define SRB1_BASE (0x2110-0x30) /* gen2 */
+#define SRB2_BASE (0x2120-0x30) /* 830 */
+#define SRB3_BASE (0x2130-0x30) /* 830 */
 #define RENDER_RING_BASE	0x02000
 #define BSD_RING_BASE		0x04000
 #define GEN6_BSD_RING_BASE	0x12000
@@ -1060,6 +1106,7 @@ enum punit_power_well {
 #define RING_ACTHD_UDW(base)	((base)+0x5c)
 #define RING_NOPID(base)	((base)+0x94)
 #define RING_IMR(base)		((base)+0xa8)
+#define RING_HWSTAM(base)	((base)+0x98)
 #define RING_TIMESTAMP(base)	((base)+0x358)
 #define   TAIL_ADDR		0x001FFFF8
 #define   HEAD_WRAP_COUNT	0xFFE00000
@@ -1244,6 +1291,10 @@ enum punit_power_well {
 #define   INSTPM_TLB_INVALIDATE	(1<<9)
 #define   INSTPM_SYNC_FLUSH	(1<<5)
 #define ACTHD	        0x020c8
+#define MEM_MODE	0x020cc
+#define   MEM_DISPLAY_B_TRICKLE_FEED_DISABLE (1<<3) /* 830 only */
+#define   MEM_DISPLAY_A_TRICKLE_FEED_DISABLE (1<<2) /* 830/845 only */
+#define   MEM_DISPLAY_TRICKLE_FEED_DISABLE (1<<2) /* 85x only */
 #define FW_BLC		0x020d8
 #define FW_BLC2		0x020dc
 #define FW_BLC_SELF	0x020e0 /* 915+ only */
@@ -1376,6 +1427,7 @@ enum punit_power_well {
 #define GT_BSD_CS_ERROR_INTERRUPT		(1 << 15)
 #define GT_BSD_USER_INTERRUPT			(1 << 12)
 #define GT_RENDER_L3_PARITY_ERROR_INTERRUPT_S1	(1 << 11) /* hsw+; rsvd on snb, ivb, vlv */
+#define GT_CONTEXT_SWITCH_INTERRUPT		(1 <<  8)
 #define GT_RENDER_L3_PARITY_ERROR_INTERRUPT	(1 <<  5) /* !snb */
 #define GT_RENDER_PIPECTL_NOTIFY_INTERRUPT	(1 <<  4)
 #define GT_RENDER_CS_MASTER_ERROR_INTERRUPT	(1 <<  3)
@@ -1515,6 +1567,7 @@ enum punit_power_well {
 /* Framebuffer compression for Ironlake */
 #define ILK_DPFC_CB_BASE	0x43200
 #define ILK_DPFC_CONTROL	0x43208
+#define   FBC_CTL_FALSE_COLOR	(1<<10)
 /* The bit 28-8 is reserved */
 #define   DPFC_RESERVED		(0x1FFFFF00)
 #define ILK_DPFC_RECOMP_CTL	0x4320c
@@ -1671,12 +1724,9 @@ enum punit_power_well {
 #define DPIO_PHY_STATUS			(VLV_DISPLAY_BASE + 0x6240)
 #define   DPLL_PORTD_READY_MASK		(0xf)
 #define DISPLAY_PHY_CONTROL (VLV_DISPLAY_BASE + 0x60100)
-#define   PHY_COM_LANE_RESET_DEASSERT(phy, val) \
-				((phy == DPIO_PHY0) ? (val | 1) : (val | 2))
-#define   PHY_COM_LANE_RESET_ASSERT(phy, val) \
-				((phy == DPIO_PHY0) ? (val & ~1) : (val & ~2))
+#define   PHY_COM_LANE_RESET_DEASSERT(phy) (1 << (phy))
 #define DISPLAY_PHY_STATUS (VLV_DISPLAY_BASE + 0x60104)
-#define   PHY_POWERGOOD(phy)	((phy == DPIO_PHY0) ? (1<<31) : (1<<30))
+#define   PHY_POWERGOOD(phy)	(((phy) == DPIO_PHY0) ? (1<<31) : (1<<30))
 
 /*
  * The i830 generation, in LVDS mode, defines P1 as the bit number set within
@@ -2393,6 +2443,7 @@ enum punit_power_well {
 #define _PIPEASRC	0x6001c
 #define _BCLRPAT_A	0x60020
 #define _VSYNCSHIFT_A	0x60028
+#define _PIPE_MULT_A	0x6002c
 
 /* Pipe B timing regs */
 #define _HTOTAL_B	0x61000
@@ -2404,6 +2455,7 @@ enum punit_power_well {
 #define _PIPEBSRC	0x6101c
 #define _BCLRPAT_B	0x61020
 #define _VSYNCSHIFT_B	0x61028
+#define _PIPE_MULT_B	0x6102c
 
 #define TRANSCODER_A_OFFSET 0x60000
 #define TRANSCODER_B_OFFSET 0x61000
@@ -2424,6 +2476,7 @@ enum punit_power_well {
 #define BCLRPAT(trans) _TRANSCODER2(trans, _BCLRPAT_A)
 #define VSYNCSHIFT(trans) _TRANSCODER2(trans, _VSYNCSHIFT_A)
 #define PIPESRC(trans) _TRANSCODER2(trans, _PIPEASRC)
+#define PIPE_MULT(trans) _TRANSCODER2(trans, _PIPE_MULT_A)
 
 /* HSW+ eDP PSR registers */
 #define EDP_PSR_BASE(dev)                       (IS_HASWELL(dev) ? 0x64800 : 0x6f800)
@@ -3472,6 +3525,8 @@ enum punit_power_well {
 #define   DP_LINK_TRAIN_OFF		(3 << 28)
 #define   DP_LINK_TRAIN_MASK		(3 << 28)
 #define   DP_LINK_TRAIN_SHIFT		28
+#define   DP_LINK_TRAIN_PAT_3_CHV	(1 << 14)
+#define   DP_LINK_TRAIN_MASK_CHV	((3 << 28)|(1<<14))
 
 /* CPT Link training mode */
 #define   DP_LINK_TRAIN_PAT_1_CPT	(0 << 8)
@@ -3728,7 +3783,6 @@ enum punit_power_well {
 #define   PIPE_VSYNC_INTERRUPT_STATUS		(1UL<<9)
 #define   PIPE_DISPLAY_LINE_COMPARE_STATUS	(1UL<<8)
 #define   PIPE_DPST_EVENT_STATUS		(1UL<<7)
-#define   PIPE_LEGACY_BLC_EVENT_STATUS		(1UL<<6)
 #define   PIPE_A_PSR_STATUS_VLV			(1UL<<6)
 #define   PIPE_LEGACY_BLC_EVENT_STATUS		(1UL<<6)
 #define   PIPE_ODD_FIELD_INTERRUPT_STATUS	(1UL<<5)
@@ -3838,73 +3892,151 @@ enum punit_power_well {
 #define   DSPARB_BEND_SHIFT	9 /* on 855 */
 #define   DSPARB_AEND_SHIFT	0
 
+/* pnv/gen4/g4x/vlv/chv */
 #define DSPFW1			(dev_priv->info.display_mmio_offset + 0x70034)
-#define   DSPFW_SR_SHIFT	23
-#define   DSPFW_SR_MASK		(0x1ff<<23)
-#define   DSPFW_CURSORB_SHIFT	16
-#define   DSPFW_CURSORB_MASK	(0x3f<<16)
-#define   DSPFW_PLANEB_SHIFT	8
-#define   DSPFW_PLANEB_MASK	(0x7f<<8)
-#define   DSPFW_PLANEA_MASK	(0x7f)
+#define   DSPFW_SR_SHIFT		23
+#define   DSPFW_SR_MASK			(0x1ff<<23)
+#define   DSPFW_CURSORB_SHIFT		16
+#define   DSPFW_CURSORB_MASK		(0x3f<<16)
+#define   DSPFW_PLANEB_SHIFT		8
+#define   DSPFW_PLANEB_MASK		(0x7f<<8)
+#define   DSPFW_PLANEB_MASK_VLV		(0xff<<8) /* vlv/chv */
+#define   DSPFW_PLANEA_SHIFT		0
+#define   DSPFW_PLANEA_MASK		(0x7f<<0)
+#define   DSPFW_PLANEA_MASK_VLV		(0xff<<0) /* vlv/chv */
 #define DSPFW2			(dev_priv->info.display_mmio_offset + 0x70038)
-#define   DSPFW_CURSORA_MASK	0x00003f00
-#define   DSPFW_CURSORA_SHIFT	8
-#define   DSPFW_PLANEC_MASK	(0x7f)
+#define   DSPFW_FBC_SR_EN		(1<<31)	  /* g4x */
+#define   DSPFW_FBC_SR_SHIFT		28
+#define   DSPFW_FBC_SR_MASK		(0x7<<28) /* g4x */
+#define   DSPFW_FBC_HPLL_SR_SHIFT	24
+#define   DSPFW_FBC_HPLL_SR_MASK	(0xf<<24) /* g4x */
+#define   DSPFW_SPRITEB_SHIFT		(16)
+#define   DSPFW_SPRITEB_MASK		(0x7f<<16) /* g4x */
+#define   DSPFW_SPRITEB_MASK_VLV	(0xff<<16) /* vlv/chv */
+#define   DSPFW_CURSORA_SHIFT		8
+#define   DSPFW_CURSORA_MASK		(0x3f<<8)
+#define   DSPFW_PLANEC_SHIFT_OLD	0
+#define   DSPFW_PLANEC_MASK_OLD		(0x7f<<0) /* pre-gen4 sprite C */
+#define   DSPFW_SPRITEA_SHIFT		0
+#define   DSPFW_SPRITEA_MASK		(0x7f<<0) /* g4x */
+#define   DSPFW_SPRITEA_MASK_VLV	(0xff<<0) /* vlv/chv */
 #define DSPFW3			(dev_priv->info.display_mmio_offset + 0x7003c)
-#define   DSPFW_HPLL_SR_EN	(1<<31)
-#define   DSPFW_CURSOR_SR_SHIFT	24
+#define   DSPFW_HPLL_SR_EN		(1<<31)
 #define   PINEVIEW_SELF_REFRESH_EN	(1<<30)
+#define   DSPFW_CURSOR_SR_SHIFT		24
 #define   DSPFW_CURSOR_SR_MASK		(0x3f<<24)
 #define   DSPFW_HPLL_CURSOR_SHIFT	16
 #define   DSPFW_HPLL_CURSOR_MASK	(0x3f<<16)
-#define   DSPFW_HPLL_SR_MASK		(0x1ff)
-#define DSPFW4			(dev_priv->info.display_mmio_offset + 0x70070)
-#define DSPFW7			(dev_priv->info.display_mmio_offset + 0x7007c)
+#define   DSPFW_HPLL_SR_SHIFT		0
+#define   DSPFW_HPLL_SR_MASK		(0x1ff<<0)
+
+/* vlv/chv */
+#define DSPFW4			(VLV_DISPLAY_BASE + 0x70070)
+#define   DSPFW_SPRITEB_WM1_SHIFT	16
+#define   DSPFW_SPRITEB_WM1_MASK	(0xff<<16)
+#define   DSPFW_CURSORA_WM1_SHIFT	8
+#define   DSPFW_CURSORA_WM1_MASK	(0x3f<<8)
+#define   DSPFW_SPRITEA_WM1_SHIFT	0
+#define   DSPFW_SPRITEA_WM1_MASK	(0xff<<0)
+#define DSPFW5			(VLV_DISPLAY_BASE + 0x70074)
+#define   DSPFW_PLANEB_WM1_SHIFT	24
+#define   DSPFW_PLANEB_WM1_MASK		(0xff<<24)
+#define   DSPFW_PLANEA_WM1_SHIFT	16
+#define   DSPFW_PLANEA_WM1_MASK		(0xff<<16)
+#define   DSPFW_CURSORB_WM1_SHIFT	8
+#define   DSPFW_CURSORB_WM1_MASK	(0x3f<<8)
+#define   DSPFW_CURSOR_SR_WM1_SHIFT	0
+#define   DSPFW_CURSOR_SR_WM1_MASK	(0x3f<<0)
+#define DSPFW6			(VLV_DISPLAY_BASE + 0x70078)
+#define   DSPFW_SR_WM1_SHIFT		0
+#define   DSPFW_SR_WM1_MASK		(0x1ff<<0)
+#define DSPFW7			(VLV_DISPLAY_BASE + 0x7007c)
+#define DSPFW7_CHV		(VLV_DISPLAY_BASE + 0x700b4) /* wtf #1? */
+#define   DSPFW_SPRITED_WM1_SHIFT	24
+#define   DSPFW_SPRITED_WM1_MASK	(0xff<<24)
+#define   DSPFW_SPRITED_SHIFT		16
+#define   DSPFW_SPRITED_MASK		(0xff<<16)
+#define   DSPFW_SPRITEC_WM1_SHIFT	8
+#define   DSPFW_SPRITEC_WM1_MASK	(0xff<<8)
+#define   DSPFW_SPRITEC_SHIFT		0
+#define   DSPFW_SPRITEC_MASK		(0xff<<0)
+#define DSPFW8_CHV		(VLV_DISPLAY_BASE + 0x700b8)
+#define   DSPFW_SPRITEF_WM1_SHIFT	24
+#define   DSPFW_SPRITEF_WM1_MASK	(0xff<<24)
+#define   DSPFW_SPRITEF_SHIFT		16
+#define   DSPFW_SPRITEF_MASK		(0xff<<16)
+#define   DSPFW_SPRITEE_WM1_SHIFT	8
+#define   DSPFW_SPRITEE_WM1_MASK	(0xff<<8)
+#define   DSPFW_SPRITEE_SHIFT		0
+#define   DSPFW_SPRITEE_MASK		(0xff<<0)
+#define DSPFW9_CHV		(VLV_DISPLAY_BASE + 0x7007c) /* wtf #2? */
+#define   DSPFW_PLANEC_WM1_SHIFT	24
+#define   DSPFW_PLANEC_WM1_MASK		(0xff<<24)
+#define   DSPFW_PLANEC_SHIFT		16
+#define   DSPFW_PLANEC_MASK		(0xff<<16)
+#define   DSPFW_CURSORC_WM1_SHIFT	8
+#define   DSPFW_CURSORC_WM1_MASK	(0x3f<<16)
+#define   DSPFW_CURSORC_SHIFT		0
+#define   DSPFW_CURSORC_MASK		(0x3f<<0)
+
+/* vlv/chv high order bits */
+#define DSPHOWM			(VLV_DISPLAY_BASE + 0x70064)
+#define   DSPFW_SR_HI_SHIFT		24
+#define   DSPFW_SR_HI_MASK		(1<<24)
+#define   DSPFW_SPRITEF_HI_SHIFT	23
+#define   DSPFW_SPRITEF_HI_MASK		(1<<23)
+#define   DSPFW_SPRITEE_HI_SHIFT	22
+#define   DSPFW_SPRITEE_HI_MASK		(1<<22)
+#define   DSPFW_PLANEC_HI_SHIFT		21
+#define   DSPFW_PLANEC_HI_MASK		(1<<21)
+#define   DSPFW_SPRITED_HI_SHIFT	20
+#define   DSPFW_SPRITED_HI_MASK		(1<<20)
+#define   DSPFW_SPRITEC_HI_SHIFT	16
+#define   DSPFW_SPRITEC_HI_MASK		(1<<16)
+#define   DSPFW_PLANEB_HI_SHIFT		12
+#define   DSPFW_PLANEB_HI_MASK		(1<<12)
+#define   DSPFW_SPRITEB_HI_SHIFT	8
+#define   DSPFW_SPRITEB_HI_MASK		(1<<8)
+#define   DSPFW_SPRITEA_HI_SHIFT	4
+#define   DSPFW_SPRITEA_HI_MASK		(1<<4)
+#define   DSPFW_PLANEA_HI_SHIFT		0
+#define   DSPFW_PLANEA_HI_MASK		(1<<0)
+#define DSPHOWM1		(VLV_DISPLAY_BASE + 0x70068)
+#define   DSPFW_SR_WM1_HI_SHIFT		24
+#define   DSPFW_SR_WM1_HI_MASK		(1<<24)
+#define   DSPFW_SPRITEF_WM1_HI_SHIFT	23
+#define   DSPFW_SPRITEF_WM1_HI_MASK	(1<<23)
+#define   DSPFW_SPRITEE_WM1_HI_SHIFT	22
+#define   DSPFW_SPRITEE_WM1_HI_MASK	(1<<22)
+#define   DSPFW_PLANEC_WM1_HI_SHIFT	21
+#define   DSPFW_PLANEC_WM1_HI_MASK	(1<<21)
+#define   DSPFW_SPRITED_WM1_HI_SHIFT	20
+#define   DSPFW_SPRITED_WM1_HI_MASK	(1<<20)
+#define   DSPFW_SPRITEC_WM1_HI_SHIFT	16
+#define   DSPFW_SPRITEC_WM1_HI_MASK	(1<<16)
+#define   DSPFW_PLANEB_WM1_HI_SHIFT	12
+#define   DSPFW_PLANEB_WM1_HI_MASK	(1<<12)
+#define   DSPFW_SPRITEB_WM1_HI_SHIFT	8
+#define   DSPFW_SPRITEB_WM1_HI_MASK	(1<<8)
+#define   DSPFW_SPRITEA_WM1_HI_SHIFT	4
+#define   DSPFW_SPRITEA_WM1_HI_MASK	(1<<4)
+#define   DSPFW_PLANEA_WM1_HI_SHIFT	0
+#define   DSPFW_PLANEA_WM1_HI_MASK	(1<<0)
 
 /* drain latency register values*/
 #define DRAIN_LATENCY_PRECISION_32	32
 #define DRAIN_LATENCY_PRECISION_64	64
-#define VLV_DDL1			(VLV_DISPLAY_BASE + 0x70050)
-#define DDL_CURSORA_PRECISION_64	(1<<31)
-#define DDL_CURSORA_PRECISION_32	(0<<31)
-#define DDL_CURSORA_SHIFT		24
-#define DDL_SPRITEB_PRECISION_64	(1<<23)
-#define DDL_SPRITEB_PRECISION_32	(0<<23)
-#define DDL_SPRITEB_SHIFT		16
-#define DDL_SPRITEA_PRECISION_64	(1<<15)
-#define DDL_SPRITEA_PRECISION_32	(0<<15)
-#define DDL_SPRITEA_SHIFT		8
-#define DDL_PLANEA_PRECISION_64		(1<<7)
-#define DDL_PLANEA_PRECISION_32		(0<<7)
-#define DDL_PLANEA_SHIFT		0
-
-#define VLV_DDL2			(VLV_DISPLAY_BASE + 0x70054)
-#define DDL_CURSORB_PRECISION_64	(1<<31)
-#define DDL_CURSORB_PRECISION_32	(0<<31)
-#define DDL_CURSORB_SHIFT		24
-#define DDL_SPRITED_PRECISION_64	(1<<23)
-#define DDL_SPRITED_PRECISION_32	(0<<23)
-#define DDL_SPRITED_SHIFT		16
-#define DDL_SPRITEC_PRECISION_64	(1<<15)
-#define DDL_SPRITEC_PRECISION_32	(0<<15)
-#define DDL_SPRITEC_SHIFT		8
-#define DDL_PLANEB_PRECISION_64		(1<<7)
-#define DDL_PLANEB_PRECISION_32		(0<<7)
-#define DDL_PLANEB_SHIFT		0
-
-#define VLV_DDL3			(VLV_DISPLAY_BASE + 0x70058)
-#define DDL_CURSORC_PRECISION_64	(1<<31)
-#define DDL_CURSORC_PRECISION_32	(0<<31)
-#define DDL_CURSORC_SHIFT		24
-#define DDL_SPRITEF_PRECISION_64	(1<<23)
-#define DDL_SPRITEF_PRECISION_32	(0<<23)
-#define DDL_SPRITEF_SHIFT		16
-#define DDL_SPRITEE_PRECISION_64	(1<<15)
-#define DDL_SPRITEE_PRECISION_32	(0<<15)
-#define DDL_SPRITEE_SHIFT		8
-#define DDL_PLANEC_PRECISION_64		(1<<7)
-#define DDL_PLANEC_PRECISION_32		(0<<7)
-#define DDL_PLANEC_SHIFT		0
+#define VLV_DDL(pipe)			(VLV_DISPLAY_BASE + 0x70050 + 4 * (pipe))
+#define DDL_CURSOR_PRECISION_64		(1<<31)
+#define DDL_CURSOR_PRECISION_32		(0<<31)
+#define DDL_CURSOR_SHIFT		24
+#define DDL_SPRITE_PRECISION_64(sprite)	(1<<(15+8*(sprite)))
+#define DDL_SPRITE_PRECISION_32(sprite)	(0<<(15+8*(sprite)))
+#define DDL_SPRITE_SHIFT(sprite)	(8+8*(sprite))
+#define DDL_PLANE_PRECISION_64		(1<<7)
+#define DDL_PLANE_PRECISION_32		(0<<7)
+#define DDL_PLANE_SHIFT			0
+#define DRAIN_LATENCY_MASK		0x7f
 
 /* FIFO watermark sizes etc */
 #define G4X_FIFO_LINE_SIZE	64
@@ -4022,7 +4154,8 @@ enum punit_power_well {
 /* Old style CUR*CNTR flags (desktop 8xx) */
 #define   CURSOR_ENABLE		0x80000000
 #define   CURSOR_GAMMA_ENABLE	0x40000000
-#define   CURSOR_STRIDE_MASK	0x30000000
+#define   CURSOR_STRIDE_SHIFT	28
+#define   CURSOR_STRIDE(x)	((ffs(x)-9) << CURSOR_STRIDE_SHIFT) /* 256,512,1k,2k */
 #define   CURSOR_PIPE_CSC_ENABLE (1<<24)
 #define   CURSOR_FORMAT_SHIFT	24
 #define   CURSOR_FORMAT_MASK	(0x07 << CURSOR_FORMAT_SHIFT)
@@ -4107,6 +4240,7 @@ enum punit_power_well {
 #define   DISPPLANE_NO_LINE_DOUBLE		0
 #define   DISPPLANE_STEREO_POLARITY_FIRST	0
 #define   DISPPLANE_STEREO_POLARITY_SECOND	(1<<18)
+#define   DISPPLANE_ROTATE_180         (1<<15)
 #define   DISPPLANE_TRICKLE_FEED_DISABLE	(1<<14) /* Ironlake */
 #define   DISPPLANE_TILED			(1<<10)
 #define _DSPAADDR				0x70184
@@ -4191,6 +4325,7 @@ enum punit_power_well {
 #define   DVS_YUV_ORDER_UYVY	(1<<16)
 #define   DVS_YUV_ORDER_YVYU	(2<<16)
 #define   DVS_YUV_ORDER_VYUY	(3<<16)
+#define   DVS_ROTATE_180	(1<<15)
 #define   DVS_DEST_KEY		(1<<2)
 #define   DVS_TRICKLE_FEED_DISABLE (1<<14)
 #define   DVS_TILED		(1<<10)
@@ -4261,6 +4396,7 @@ enum punit_power_well {
 #define   SPRITE_YUV_ORDER_UYVY		(1<<16)
 #define   SPRITE_YUV_ORDER_YVYU		(2<<16)
 #define   SPRITE_YUV_ORDER_VYUY		(3<<16)
+#define   SPRITE_ROTATE_180		(1<<15)
 #define   SPRITE_TRICKLE_FEED_DISABLE	(1<<14)
 #define   SPRITE_INT_GAMMA_ENABLE	(1<<13)
 #define   SPRITE_TILED			(1<<10)
@@ -4334,6 +4470,7 @@ enum punit_power_well {
 #define   SP_YUV_ORDER_UYVY		(1<<16)
 #define   SP_YUV_ORDER_YVYU		(2<<16)
 #define   SP_YUV_ORDER_VYUY		(3<<16)
+#define   SP_ROTATE_180			(1<<15)
 #define   SP_TILED			(1<<10)
 #define _SPALINOFF		(VLV_DISPLAY_BASE + 0x72184)
 #define _SPASTRIDE		(VLV_DISPLAY_BASE + 0x72188)
@@ -5242,8 +5379,7 @@ enum punit_power_well {
 #define PIPEA_PP_STATUS         (VLV_DISPLAY_BASE + 0x61200)
 #define PIPEA_PP_CONTROL        (VLV_DISPLAY_BASE + 0x61204)
 #define PIPEA_PP_ON_DELAYS      (VLV_DISPLAY_BASE + 0x61208)
-#define  PANEL_PORT_SELECT_DPB_VLV	(1 << 30)
-#define  PANEL_PORT_SELECT_DPC_VLV	(2 << 30)
+#define  PANEL_PORT_SELECT_VLV(port)	((port) << 30)
 #define PIPEA_PP_OFF_DELAYS     (VLV_DISPLAY_BASE + 0x6120c)
 #define PIPEA_PP_DIVISOR        (VLV_DISPLAY_BASE + 0x61210)
 
@@ -5403,7 +5539,6 @@ enum punit_power_well {
 #define   VLV_GTLC_ALLOWWAKEERR			(1 << 1)
 #define   VLV_GTLC_PW_MEDIA_STATUS_MASK		(1 << 5)
 #define   VLV_GTLC_PW_RENDER_STATUS_MASK	(1 << 7)
-#define VLV_GTLC_SURVIVABILITY_REG              0x130098
 #define  FORCEWAKE_MT				0xa188 /* multi-threaded */
 #define   FORCEWAKE_KERNEL			0x1
 #define   FORCEWAKE_USER			0x2
@@ -5541,12 +5676,6 @@ enum punit_power_well {
 						 GEN6_PM_RP_DOWN_THRESHOLD | \
 						 GEN6_PM_RP_DOWN_TIMEOUT)
 
-#define CHV_CZ_CLOCK_FREQ_MODE_200			200
-#define CHV_CZ_CLOCK_FREQ_MODE_267			267
-#define CHV_CZ_CLOCK_FREQ_MODE_320			320
-#define CHV_CZ_CLOCK_FREQ_MODE_333			333
-#define CHV_CZ_CLOCK_FREQ_MODE_400			400
-
 #define GEN7_GT_SCRATCH_BASE			0x4F100
 #define GEN7_GT_SCRATCH_REG_NUM			8
 
@@ -5862,15 +5991,7 @@ enum punit_power_well {
 #define DDI_BUF_CTL_B				0x64100
 #define DDI_BUF_CTL(port) _PORT(port, DDI_BUF_CTL_A, DDI_BUF_CTL_B)
 #define  DDI_BUF_CTL_ENABLE			(1<<31)
-#define  DDI_BUF_EMP_400MV_0DB_HSW		(0<<24)   /* Sel0 */
-#define  DDI_BUF_EMP_400MV_3_5DB_HSW		(1<<24)   /* Sel1 */
-#define  DDI_BUF_EMP_400MV_6DB_HSW		(2<<24)   /* Sel2 */
-#define  DDI_BUF_EMP_400MV_9_5DB_HSW		(3<<24)   /* Sel3 */
-#define  DDI_BUF_EMP_600MV_0DB_HSW		(4<<24)   /* Sel4 */
-#define  DDI_BUF_EMP_600MV_3_5DB_HSW		(5<<24)   /* Sel5 */
-#define  DDI_BUF_EMP_600MV_6DB_HSW		(6<<24)   /* Sel6 */
-#define  DDI_BUF_EMP_800MV_0DB_HSW		(7<<24)   /* Sel7 */
-#define  DDI_BUF_EMP_800MV_3_5DB_HSW		(8<<24)   /* Sel8 */
+#define  DDI_BUF_TRANS_SELECT(n)	((n) << 24)
 #define  DDI_BUF_EMP_MASK			(0xf<<24)
 #define  DDI_BUF_PORT_REVERSAL			(1<<16)
 #define  DDI_BUF_IS_IDLE			(1<<7)
diff --git a/drivers/gpu/drm/i915/i915_sysfs.c b/drivers/gpu/drm/i915/i915_sysfs.c
index ae7fd8fc27f0..503847f18fdd 100644
--- a/drivers/gpu/drm/i915/i915_sysfs.c
+++ b/drivers/gpu/drm/i915/i915_sysfs.c
@@ -540,7 +540,7 @@ static ssize_t error_state_read(struct file *filp, struct kobject *kobj,
 
 	memset(&error_priv, 0, sizeof(error_priv));
 
-	ret = i915_error_state_buf_init(&error_str, count, off);
+	ret = i915_error_state_buf_init(&error_str, to_i915(dev), count, off);
 	if (ret)
 		return ret;
 
diff --git a/drivers/gpu/drm/i915/intel_bios.c b/drivers/gpu/drm/i915/intel_bios.c
index a66955037e4e..a4bd90f36a03 100644
--- a/drivers/gpu/drm/i915/intel_bios.c
+++ b/drivers/gpu/drm/i915/intel_bios.c
@@ -627,16 +627,16 @@ parse_edp(struct drm_i915_private *dev_priv, struct bdb_header *bdb)
 
 	switch (edp_link_params->preemphasis) {
 	case EDP_PREEMPHASIS_NONE:
-		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_0;
+		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_0;
 		break;
 	case EDP_PREEMPHASIS_3_5dB:
-		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_3_5;
+		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_1;
 		break;
 	case EDP_PREEMPHASIS_6dB:
-		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_6;
+		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_2;
 		break;
 	case EDP_PREEMPHASIS_9_5dB:
-		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPHASIS_9_5;
+		dev_priv->vbt.edp_preemphasis = DP_TRAIN_PRE_EMPH_LEVEL_3;
 		break;
 	default:
 		DRM_DEBUG_KMS("VBT has unknown eDP pre-emphasis value %u\n",
@@ -646,16 +646,16 @@ parse_edp(struct drm_i915_private *dev_priv, struct bdb_header *bdb)
 
 	switch (edp_link_params->vswing) {
 	case EDP_VSWING_0_4V:
-		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_400;
+		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_0;
 		break;
 	case EDP_VSWING_0_6V:
-		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_600;
+		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_1;
 		break;
 	case EDP_VSWING_0_8V:
-		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_800;
+		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
 		break;
 	case EDP_VSWING_1_2V:
-		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_1200;
+		dev_priv->vbt.edp_vswing = DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 		break;
 	default:
 		DRM_DEBUG_KMS("VBT has unknown eDP voltage swing value %u\n",
@@ -946,7 +946,7 @@ static void parse_ddi_port(struct drm_i915_private *dev_priv, enum port port,
 		DRM_DEBUG_KMS("Analog port %c is also DP or TMDS compatible\n",
 			      port_name(port));
 	if (is_dvi && (port == PORT_A || port == PORT_E))
-		DRM_DEBUG_KMS("Port %c is TMDS compabile\n", port_name(port));
+		DRM_DEBUG_KMS("Port %c is TMDS compatible\n", port_name(port));
 	if (!is_dvi && !is_dp && !is_crt)
 		DRM_DEBUG_KMS("Port %c is not DP/TMDS/CRT compatible\n",
 			      port_name(port));
@@ -976,12 +976,10 @@ static void parse_ddi_port(struct drm_i915_private *dev_priv, enum port port,
 	if (bdb->version >= 158) {
 		/* The VBT HDMI level shift values match the table we have. */
 		hdmi_level_shift = child->raw[7] & 0xF;
-		if (hdmi_level_shift < 0xC) {
-			DRM_DEBUG_KMS("VBT HDMI level shift for port %c: %d\n",
-				      port_name(port),
-				      hdmi_level_shift);
-			info->hdmi_level_shift = hdmi_level_shift;
-		}
+		DRM_DEBUG_KMS("VBT HDMI level shift for port %c: %d\n",
+			      port_name(port),
+			      hdmi_level_shift);
+		info->hdmi_level_shift = hdmi_level_shift;
 	}
 }
 
@@ -1114,8 +1112,7 @@ init_vbt_defaults(struct drm_i915_private *dev_priv)
 		struct ddi_vbt_port_info *info =
 			&dev_priv->vbt.ddi_port_info[port];
 
-		/* Recommended BSpec default: 800mV 0dB. */
-		info->hdmi_level_shift = 6;
+		info->hdmi_level_shift = HDMI_LEVEL_SHIFT_UNKNOWN;
 
 		info->supports_dvi = (port != PORT_A && port != PORT_E);
 		info->supports_hdmi = info->supports_dvi;
@@ -1123,7 +1120,7 @@ init_vbt_defaults(struct drm_i915_private *dev_priv)
 	}
 }
 
-static int __init intel_no_opregion_vbt_callback(const struct dmi_system_id *id)
+static int intel_no_opregion_vbt_callback(const struct dmi_system_id *id)
 {
 	DRM_DEBUG_KMS("Falling back to manually reading VBT from "
 		      "VBIOS ROM for %s\n",
diff --git a/drivers/gpu/drm/i915/intel_bios.h b/drivers/gpu/drm/i915/intel_bios.h
index b98667796337..905999bee2ac 100644
--- a/drivers/gpu/drm/i915/intel_bios.h
+++ b/drivers/gpu/drm/i915/intel_bios.h
@@ -802,7 +802,8 @@ struct mipi_config {
 
 	u16 rsvd4;
 
-	u8 rsvd5[5];
+	u8 rsvd5;
+	u32 target_burst_mode_freq;
 	u32 dsi_ddr_clk;
 	u32 bridge_ref_clk;
 
diff --git a/drivers/gpu/drm/i915/intel_crt.c b/drivers/gpu/drm/i915/intel_crt.c
index 2efaf8e8d9c4..9212e6504e0f 100644
--- a/drivers/gpu/drm/i915/intel_crt.c
+++ b/drivers/gpu/drm/i915/intel_crt.c
@@ -699,16 +699,21 @@ intel_crt_detect(struct drm_connector *connector, bool force)
 		goto out;
 	}
 
+	drm_modeset_acquire_init(&ctx, 0);
+
 	/* for pre-945g platforms use load detect */
 	if (intel_get_load_detect_pipe(connector, NULL, &tmp, &ctx)) {
 		if (intel_crt_detect_ddc(connector))
 			status = connector_status_connected;
 		else
 			status = intel_crt_load_detect(crt);
-		intel_release_load_detect_pipe(connector, &tmp, &ctx);
+		intel_release_load_detect_pipe(connector, &tmp);
 	} else
 		status = connector_status_unknown;
 
+	drm_modeset_drop_locks(&ctx);
+	drm_modeset_acquire_fini(&ctx);
+
 out:
 	intel_display_power_put(dev_priv, power_domain);
 	return status;
@@ -799,7 +804,7 @@ static const struct drm_encoder_funcs intel_crt_enc_funcs = {
 	.destroy = intel_encoder_destroy,
 };
 
-static int __init intel_no_crt_dmi_callback(const struct dmi_system_id *id)
+static int intel_no_crt_dmi_callback(const struct dmi_system_id *id)
 {
 	DRM_INFO("Skipping CRT initialization for %s\n", id->ident);
 	return 1;
diff --git a/drivers/gpu/drm/i915/intel_ddi.c b/drivers/gpu/drm/i915/intel_ddi.c
index 5db0b5552e39..b63d4fa204a3 100644
--- a/drivers/gpu/drm/i915/intel_ddi.c
+++ b/drivers/gpu/drm/i915/intel_ddi.c
@@ -28,87 +28,103 @@
 #include "i915_drv.h"
 #include "intel_drv.h"
 
+struct ddi_buf_trans {
+	u32 trans1;	/* balance leg enable, de-emph level */
+	u32 trans2;	/* vref sel, vswing */
+};
+
 /* HDMI/DVI modes ignore everything but the last 2 items. So we share
  * them for both DP and FDI transports, allowing those ports to
  * automatically adapt to HDMI connections as well
  */
-static const u32 hsw_ddi_translations_dp[] = {
-	0x00FFFFFF, 0x0006000E,		/* DP parameters */
-	0x00D75FFF, 0x0005000A,
-	0x00C30FFF, 0x00040006,
-	0x80AAAFFF, 0x000B0000,
-	0x00FFFFFF, 0x0005000A,
-	0x00D75FFF, 0x000C0004,
-	0x80C30FFF, 0x000B0000,
-	0x00FFFFFF, 0x00040006,
-	0x80D75FFF, 0x000B0000,
+static const struct ddi_buf_trans hsw_ddi_translations_dp[] = {
+	{ 0x00FFFFFF, 0x0006000E },
+	{ 0x00D75FFF, 0x0005000A },
+	{ 0x00C30FFF, 0x00040006 },
+	{ 0x80AAAFFF, 0x000B0000 },
+	{ 0x00FFFFFF, 0x0005000A },
+	{ 0x00D75FFF, 0x000C0004 },
+	{ 0x80C30FFF, 0x000B0000 },
+	{ 0x00FFFFFF, 0x00040006 },
+	{ 0x80D75FFF, 0x000B0000 },
 };
 
-static const u32 hsw_ddi_translations_fdi[] = {
-	0x00FFFFFF, 0x0007000E,		/* FDI parameters */
-	0x00D75FFF, 0x000F000A,
-	0x00C30FFF, 0x00060006,
-	0x00AAAFFF, 0x001E0000,
-	0x00FFFFFF, 0x000F000A,
-	0x00D75FFF, 0x00160004,
-	0x00C30FFF, 0x001E0000,
-	0x00FFFFFF, 0x00060006,
-	0x00D75FFF, 0x001E0000,
+static const struct ddi_buf_trans hsw_ddi_translations_fdi[] = {
+	{ 0x00FFFFFF, 0x0007000E },
+	{ 0x00D75FFF, 0x000F000A },
+	{ 0x00C30FFF, 0x00060006 },
+	{ 0x00AAAFFF, 0x001E0000 },
+	{ 0x00FFFFFF, 0x000F000A },
+	{ 0x00D75FFF, 0x00160004 },
+	{ 0x00C30FFF, 0x001E0000 },
+	{ 0x00FFFFFF, 0x00060006 },
+	{ 0x00D75FFF, 0x001E0000 },
 };
 
-static const u32 hsw_ddi_translations_hdmi[] = {
-				/* Idx	NT mV diff	T mV diff	db  */
-	0x00FFFFFF, 0x0006000E, /* 0:	400		400		0   */
-	0x00E79FFF, 0x000E000C, /* 1:	400		500		2   */
-	0x00D75FFF, 0x0005000A, /* 2:	400		600		3.5 */
-	0x00FFFFFF, 0x0005000A, /* 3:	600		600		0   */
-	0x00E79FFF, 0x001D0007, /* 4:	600		750		2   */
-	0x00D75FFF, 0x000C0004, /* 5:	600		900		3.5 */
-	0x00FFFFFF, 0x00040006, /* 6:	800		800		0   */
-	0x80E79FFF, 0x00030002, /* 7:	800		1000		2   */
-	0x00FFFFFF, 0x00140005, /* 8:	850		850		0   */
-	0x00FFFFFF, 0x000C0004, /* 9:	900		900		0   */
-	0x00FFFFFF, 0x001C0003, /* 10:	950		950		0   */
-	0x80FFFFFF, 0x00030002, /* 11:	1000		1000		0   */
+static const struct ddi_buf_trans hsw_ddi_translations_hdmi[] = {
+					/* Idx	NT mV d	T mV d	db	*/
+	{ 0x00FFFFFF, 0x0006000E },	/* 0:	400	400	0	*/
+	{ 0x00E79FFF, 0x000E000C },	/* 1:	400	500	2	*/
+	{ 0x00D75FFF, 0x0005000A },	/* 2:	400	600	3.5	*/
+	{ 0x00FFFFFF, 0x0005000A },	/* 3:	600	600	0	*/
+	{ 0x00E79FFF, 0x001D0007 },	/* 4:	600	750	2	*/
+	{ 0x00D75FFF, 0x000C0004 },	/* 5:	600	900	3.5	*/
+	{ 0x00FFFFFF, 0x00040006 },	/* 6:	800	800	0	*/
+	{ 0x80E79FFF, 0x00030002 },	/* 7:	800	1000	2	*/
+	{ 0x00FFFFFF, 0x00140005 },	/* 8:	850	850	0	*/
+	{ 0x00FFFFFF, 0x000C0004 },	/* 9:	900	900	0	*/
+	{ 0x00FFFFFF, 0x001C0003 },	/* 10:	950	950	0	*/
+	{ 0x80FFFFFF, 0x00030002 },	/* 11:	1000	1000	0	*/
 };
 
-static const u32 bdw_ddi_translations_edp[] = {
-	0x00FFFFFF, 0x00000012,		/* eDP parameters */
-	0x00EBAFFF, 0x00020011,
-	0x00C71FFF, 0x0006000F,
-	0x00AAAFFF, 0x000E000A,
-	0x00FFFFFF, 0x00020011,
-	0x00DB6FFF, 0x0005000F,
-	0x00BEEFFF, 0x000A000C,
-	0x00FFFFFF, 0x0005000F,
-	0x00DB6FFF, 0x000A000C,
-	0x00FFFFFF, 0x00140006		/* HDMI parameters 800mV 0dB*/
+static const struct ddi_buf_trans bdw_ddi_translations_edp[] = {
+	{ 0x00FFFFFF, 0x00000012 },
+	{ 0x00EBAFFF, 0x00020011 },
+	{ 0x00C71FFF, 0x0006000F },
+	{ 0x00AAAFFF, 0x000E000A },
+	{ 0x00FFFFFF, 0x00020011 },
+	{ 0x00DB6FFF, 0x0005000F },
+	{ 0x00BEEFFF, 0x000A000C },
+	{ 0x00FFFFFF, 0x0005000F },
+	{ 0x00DB6FFF, 0x000A000C },
 };
 
-static const u32 bdw_ddi_translations_dp[] = {
-	0x00FFFFFF, 0x0007000E,		/* DP parameters */
-	0x00D75FFF, 0x000E000A,
-	0x00BEFFFF, 0x00140006,
-	0x80B2CFFF, 0x001B0002,
-	0x00FFFFFF, 0x000E000A,
-	0x00D75FFF, 0x00180004,
-	0x80CB2FFF, 0x001B0002,
-	0x00F7DFFF, 0x00180004,
-	0x80D75FFF, 0x001B0002,
-	0x00FFFFFF, 0x00140006		/* HDMI parameters 800mV 0dB*/
+static const struct ddi_buf_trans bdw_ddi_translations_dp[] = {
+	{ 0x00FFFFFF, 0x0007000E },
+	{ 0x00D75FFF, 0x000E000A },
+	{ 0x00BEFFFF, 0x00140006 },
+	{ 0x80B2CFFF, 0x001B0002 },
+	{ 0x00FFFFFF, 0x000E000A },
+	{ 0x00D75FFF, 0x00180004 },
+	{ 0x80CB2FFF, 0x001B0002 },
+	{ 0x00F7DFFF, 0x00180004 },
+	{ 0x80D75FFF, 0x001B0002 },
 };
 
-static const u32 bdw_ddi_translations_fdi[] = {
-	0x00FFFFFF, 0x0001000E,		/* FDI parameters */
-	0x00D75FFF, 0x0004000A,
-	0x00C30FFF, 0x00070006,
-	0x00AAAFFF, 0x000C0000,
-	0x00FFFFFF, 0x0004000A,
-	0x00D75FFF, 0x00090004,
-	0x00C30FFF, 0x000C0000,
-	0x00FFFFFF, 0x00070006,
-	0x00D75FFF, 0x000C0000,
-	0x00FFFFFF, 0x00140006		/* HDMI parameters 800mV 0dB*/
+static const struct ddi_buf_trans bdw_ddi_translations_fdi[] = {
+	{ 0x00FFFFFF, 0x0001000E },
+	{ 0x00D75FFF, 0x0004000A },
+	{ 0x00C30FFF, 0x00070006 },
+	{ 0x00AAAFFF, 0x000C0000 },
+	{ 0x00FFFFFF, 0x0004000A },
+	{ 0x00D75FFF, 0x00090004 },
+	{ 0x00C30FFF, 0x000C0000 },
+	{ 0x00FFFFFF, 0x00070006 },
+	{ 0x00D75FFF, 0x000C0000 },
+};
+
+static const struct ddi_buf_trans bdw_ddi_translations_hdmi[] = {
+					/* Idx	NT mV d	T mV df	db	*/
+	{ 0x00FFFFFF, 0x0007000E },	/* 0:	400	400	0	*/
+	{ 0x00D75FFF, 0x000E000A },	/* 1:	400	600	3.5	*/
+	{ 0x00BEFFFF, 0x00140006 },	/* 2:	400	800	6	*/
+	{ 0x00FFFFFF, 0x0009000D },	/* 3:	450	450	0	*/
+	{ 0x00FFFFFF, 0x000E000A },	/* 4:	600	600	0	*/
+	{ 0x00D7FFFF, 0x00140006 },	/* 5:	600	800	2.5	*/
+	{ 0x80CB2FFF, 0x001B0002 },	/* 6:	600	1000	4.5	*/
+	{ 0x00FFFFFF, 0x00140006 },	/* 7:	800	800	0	*/
+	{ 0x80E79FFF, 0x001B0002 },	/* 8:	800	1000	2	*/
+	{ 0x80FFFFFF, 0x001B0002 },	/* 9:	1000	1000	0	*/
 };
 
 enum port intel_ddi_get_encoder_port(struct intel_encoder *intel_encoder)
@@ -145,26 +161,36 @@ static void intel_prepare_ddi_buffers(struct drm_device *dev, enum port port)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 reg;
-	int i;
+	int i, n_hdmi_entries, hdmi_800mV_0dB;
 	int hdmi_level = dev_priv->vbt.ddi_port_info[port].hdmi_level_shift;
-	const u32 *ddi_translations_fdi;
-	const u32 *ddi_translations_dp;
-	const u32 *ddi_translations_edp;
-	const u32 *ddi_translations;
+	const struct ddi_buf_trans *ddi_translations_fdi;
+	const struct ddi_buf_trans *ddi_translations_dp;
+	const struct ddi_buf_trans *ddi_translations_edp;
+	const struct ddi_buf_trans *ddi_translations_hdmi;
+	const struct ddi_buf_trans *ddi_translations;
 
 	if (IS_BROADWELL(dev)) {
 		ddi_translations_fdi = bdw_ddi_translations_fdi;
 		ddi_translations_dp = bdw_ddi_translations_dp;
 		ddi_translations_edp = bdw_ddi_translations_edp;
+		ddi_translations_hdmi = bdw_ddi_translations_hdmi;
+		n_hdmi_entries = ARRAY_SIZE(bdw_ddi_translations_hdmi);
+		hdmi_800mV_0dB = 7;
 	} else if (IS_HASWELL(dev)) {
 		ddi_translations_fdi = hsw_ddi_translations_fdi;
 		ddi_translations_dp = hsw_ddi_translations_dp;
 		ddi_translations_edp = hsw_ddi_translations_dp;
+		ddi_translations_hdmi = hsw_ddi_translations_hdmi;
+		n_hdmi_entries = ARRAY_SIZE(hsw_ddi_translations_hdmi);
+		hdmi_800mV_0dB = 6;
 	} else {
 		WARN(1, "ddi translation table missing\n");
 		ddi_translations_edp = bdw_ddi_translations_dp;
 		ddi_translations_fdi = bdw_ddi_translations_fdi;
 		ddi_translations_dp = bdw_ddi_translations_dp;
+		ddi_translations_hdmi = bdw_ddi_translations_hdmi;
+		n_hdmi_entries = ARRAY_SIZE(bdw_ddi_translations_hdmi);
+		hdmi_800mV_0dB = 7;
 	}
 
 	switch (port) {
@@ -190,14 +216,22 @@ static void intel_prepare_ddi_buffers(struct drm_device *dev, enum port port)
 
 	for (i = 0, reg = DDI_BUF_TRANS(port);
 	     i < ARRAY_SIZE(hsw_ddi_translations_fdi); i++) {
-		I915_WRITE(reg, ddi_translations[i]);
+		I915_WRITE(reg, ddi_translations[i].trans1);
 		reg += 4;
-	}
-	/* Entry 9 is for HDMI: */
-	for (i = 0; i < 2; i++) {
-		I915_WRITE(reg, hsw_ddi_translations_hdmi[hdmi_level * 2 + i]);
+		I915_WRITE(reg, ddi_translations[i].trans2);
 		reg += 4;
 	}
+
+	/* Choose a good default if VBT is badly populated */
+	if (hdmi_level == HDMI_LEVEL_SHIFT_UNKNOWN ||
+	    hdmi_level >= n_hdmi_entries)
+		hdmi_level = hdmi_800mV_0dB;
+
+	/* Entry 9 is for HDMI: */
+	I915_WRITE(reg, ddi_translations_hdmi[hdmi_level].trans1);
+	reg += 4;
+	I915_WRITE(reg, ddi_translations_hdmi[hdmi_level].trans2);
+	reg += 4;
 }
 
 /* Program DDI buffers translations for DP. By default, program ports A-D in DP
@@ -214,18 +248,6 @@ void intel_prepare_ddi(struct drm_device *dev)
 		intel_prepare_ddi_buffers(dev, port);
 }
 
-static const long hsw_ddi_buf_ctl_values[] = {
-	DDI_BUF_EMP_400MV_0DB_HSW,
-	DDI_BUF_EMP_400MV_3_5DB_HSW,
-	DDI_BUF_EMP_400MV_6DB_HSW,
-	DDI_BUF_EMP_400MV_9_5DB_HSW,
-	DDI_BUF_EMP_600MV_0DB_HSW,
-	DDI_BUF_EMP_600MV_3_5DB_HSW,
-	DDI_BUF_EMP_600MV_6DB_HSW,
-	DDI_BUF_EMP_800MV_0DB_HSW,
-	DDI_BUF_EMP_800MV_3_5DB_HSW
-};
-
 static void intel_wait_ddi_buf_idle(struct drm_i915_private *dev_priv,
 				    enum port port)
 {
@@ -285,7 +307,7 @@ void hsw_fdi_link_train(struct drm_crtc *crtc)
 
 	/* Start the training iterating through available voltages and emphasis,
 	 * testing each value twice. */
-	for (i = 0; i < ARRAY_SIZE(hsw_ddi_buf_ctl_values) * 2; i++) {
+	for (i = 0; i < ARRAY_SIZE(hsw_ddi_translations_fdi) * 2; i++) {
 		/* Configure DP_TP_CTL with auto-training */
 		I915_WRITE(DP_TP_CTL(PORT_E),
 					DP_TP_CTL_FDI_AUTOTRAIN |
@@ -300,7 +322,7 @@ void hsw_fdi_link_train(struct drm_crtc *crtc)
 		I915_WRITE(DDI_BUF_CTL(PORT_E),
 			   DDI_BUF_CTL_ENABLE |
 			   ((intel_crtc->config.fdi_lanes - 1) << 1) |
-			   hsw_ddi_buf_ctl_values[i / 2]);
+			   DDI_BUF_TRANS_SELECT(i / 2));
 		POSTING_READ(DDI_BUF_CTL(PORT_E));
 
 		udelay(600);
@@ -375,7 +397,7 @@ void intel_ddi_init_dp_buf_reg(struct intel_encoder *encoder)
 		enc_to_dig_port(&encoder->base);
 
 	intel_dp->DP = intel_dig_port->saved_port_bits |
-		DDI_BUF_CTL_ENABLE | DDI_BUF_EMP_400MV_0DB_HSW;
+		DDI_BUF_CTL_ENABLE | DDI_BUF_TRANS_SELECT(0);
 	intel_dp->DP |= DDI_PORT_WIDTH(intel_dp->lane_count);
 
 }
@@ -402,7 +424,7 @@ intel_ddi_get_crtc_encoder(struct drm_crtc *crtc)
 }
 
 #define LC_FREQ 2700
-#define LC_FREQ_2K (LC_FREQ * 2000)
+#define LC_FREQ_2K U64_C(LC_FREQ * 2000)
 
 #define P_MIN 2
 #define P_MAX 64
@@ -414,7 +436,11 @@ intel_ddi_get_crtc_encoder(struct drm_crtc *crtc)
 #define VCO_MIN 2400
 #define VCO_MAX 4800
 
-#define ABS_DIFF(a, b) ((a > b) ? (a - b) : (b - a))
+#define abs_diff(a, b) ({			\
+	typeof(a) __a = (a);			\
+	typeof(b) __b = (b);			\
+	(void) (&__a == &__b);			\
+	__a > __b ? (__a - __b) : (__b - __a); })
 
 struct wrpll_rnp {
 	unsigned p, n2, r2;
@@ -524,9 +550,9 @@ static void wrpll_update_rnp(uint64_t freq2k, unsigned budget,
 	 */
 	a = freq2k * budget * p * r2;
 	b = freq2k * budget * best->p * best->r2;
-	diff = ABS_DIFF((freq2k * p * r2), (LC_FREQ_2K * n2));
-	diff_best = ABS_DIFF((freq2k * best->p * best->r2),
-			     (LC_FREQ_2K * best->n2));
+	diff = abs_diff(freq2k * p * r2, LC_FREQ_2K * n2);
+	diff_best = abs_diff(freq2k * best->p * best->r2,
+			     LC_FREQ_2K * best->n2);
 	c = 1000000 * diff;
 	d = 1000000 * diff_best;
 
@@ -587,8 +613,8 @@ static int intel_ddi_calc_wrpll_link(struct drm_i915_private *dev_priv,
 	return (refclk * n * 100) / (p * r);
 }
 
-void intel_ddi_clock_get(struct intel_encoder *encoder,
-			 struct intel_crtc_config *pipe_config)
+static void hsw_ddi_clock_get(struct intel_encoder *encoder,
+			      struct intel_crtc_config *pipe_config)
 {
 	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
 	int link_clock = 0;
@@ -643,9 +669,15 @@ void intel_ddi_clock_get(struct intel_encoder *encoder,
 		pipe_config->adjusted_mode.crtc_clock = pipe_config->port_clock;
 }
 
+void intel_ddi_clock_get(struct intel_encoder *encoder,
+			 struct intel_crtc_config *pipe_config)
+{
+	hsw_ddi_clock_get(encoder, pipe_config);
+}
+
 static void
-intel_ddi_calculate_wrpll(int clock /* in Hz */,
-			  unsigned *r2_out, unsigned *n2_out, unsigned *p_out)
+hsw_ddi_calculate_wrpll(int clock /* in Hz */,
+			unsigned *r2_out, unsigned *n2_out, unsigned *p_out)
 {
 	uint64_t freq2k;
 	unsigned p, n2, r2;
@@ -708,27 +740,17 @@ intel_ddi_calculate_wrpll(int clock /* in Hz */,
 	*r2_out = best.r2;
 }
 
-/*
- * Tries to find a PLL for the CRTC. If it finds, it increases the refcount and
- * stores it in intel_crtc->ddi_pll_sel, so other mode sets won't be able to
- * steal the selected PLL. You need to call intel_ddi_pll_enable to actually
- * enable the PLL.
- */
-bool intel_ddi_pll_select(struct intel_crtc *intel_crtc)
+static bool
+hsw_ddi_pll_select(struct intel_crtc *intel_crtc,
+		   struct intel_encoder *intel_encoder,
+		   int clock)
 {
-	struct drm_crtc *crtc = &intel_crtc->base;
-	struct intel_encoder *intel_encoder = intel_ddi_get_crtc_encoder(crtc);
-	int type = intel_encoder->type;
-	int clock = intel_crtc->config.port_clock;
-
-	intel_put_shared_dpll(intel_crtc);
-
-	if (type == INTEL_OUTPUT_HDMI) {
+	if (intel_encoder->type == INTEL_OUTPUT_HDMI) {
 		struct intel_shared_dpll *pll;
 		uint32_t val;
 		unsigned p, n2, r2;
 
-		intel_ddi_calculate_wrpll(clock * 1000, &r2, &n2, &p);
+		hsw_ddi_calculate_wrpll(clock * 1000, &r2, &n2, &p);
 
 		val = WRPLL_PLL_ENABLE | WRPLL_PLL_LCPLL |
 		      WRPLL_DIVIDER_REFERENCE(r2) | WRPLL_DIVIDER_FEEDBACK(n2) |
@@ -749,6 +771,25 @@ bool intel_ddi_pll_select(struct intel_crtc *intel_crtc)
 	return true;
 }
 
+
+/*
+ * Tries to find a *shared* PLL for the CRTC and store it in
+ * intel_crtc->ddi_pll_sel.
+ *
+ * For private DPLLs, compute_config() should do the selection for us. This
+ * function should be folded into compute_config() eventually.
+ */
+bool intel_ddi_pll_select(struct intel_crtc *intel_crtc)
+{
+	struct drm_crtc *crtc = &intel_crtc->base;
+	struct intel_encoder *intel_encoder = intel_ddi_get_crtc_encoder(crtc);
+	int clock = intel_crtc->config.port_clock;
+
+	intel_put_shared_dpll(intel_crtc);
+
+	return hsw_ddi_pll_select(intel_crtc, intel_encoder, clock);
+}
+
 void intel_ddi_set_pipe_settings(struct drm_crtc *crtc)
 {
 	struct drm_i915_private *dev_priv = crtc->dev->dev_private;
@@ -1183,31 +1224,52 @@ static void intel_disable_ddi(struct intel_encoder *intel_encoder)
 	}
 }
 
-int intel_ddi_get_cdclk_freq(struct drm_i915_private *dev_priv)
+static int bdw_get_cdclk_freq(struct drm_i915_private *dev_priv)
+{
+	uint32_t lcpll = I915_READ(LCPLL_CTL);
+	uint32_t freq = lcpll & LCPLL_CLK_FREQ_MASK;
+
+	if (lcpll & LCPLL_CD_SOURCE_FCLK)
+		return 800000;
+	else if (I915_READ(FUSE_STRAP) & HSW_CDCLK_LIMIT)
+		return 450000;
+	else if (freq == LCPLL_CLK_FREQ_450)
+		return 450000;
+	else if (freq == LCPLL_CLK_FREQ_54O_BDW)
+		return 540000;
+	else if (freq == LCPLL_CLK_FREQ_337_5_BDW)
+		return 337500;
+	else
+		return 675000;
+}
+
+static int hsw_get_cdclk_freq(struct drm_i915_private *dev_priv)
 {
 	struct drm_device *dev = dev_priv->dev;
 	uint32_t lcpll = I915_READ(LCPLL_CTL);
 	uint32_t freq = lcpll & LCPLL_CLK_FREQ_MASK;
 
-	if (lcpll & LCPLL_CD_SOURCE_FCLK) {
+	if (lcpll & LCPLL_CD_SOURCE_FCLK)
 		return 800000;
-	} else if (I915_READ(FUSE_STRAP) & HSW_CDCLK_LIMIT) {
+	else if (I915_READ(FUSE_STRAP) & HSW_CDCLK_LIMIT)
 		return 450000;
-	} else if (freq == LCPLL_CLK_FREQ_450) {
+	else if (freq == LCPLL_CLK_FREQ_450)
 		return 450000;
-	} else if (IS_HASWELL(dev)) {
-		if (IS_ULT(dev))
-			return 337500;
-		else
-			return 540000;
-	} else {
-		if (freq == LCPLL_CLK_FREQ_54O_BDW)
-			return 540000;
-		else if (freq == LCPLL_CLK_FREQ_337_5_BDW)
-			return 337500;
-		else
-			return 675000;
-	}
+	else if (IS_ULT(dev))
+		return 337500;
+	else
+		return 540000;
+}
+
+int intel_ddi_get_cdclk_freq(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+
+	if (IS_BROADWELL(dev))
+		return bdw_get_cdclk_freq(dev_priv);
+
+	/* Haswell */
+	return hsw_get_cdclk_freq(dev_priv);
 }
 
 static void hsw_ddi_pll_enable(struct drm_i915_private *dev_priv,
@@ -1248,10 +1310,8 @@ static const char * const hsw_ddi_pll_names[] = {
 	"WRPLL 2",
 };
 
-void intel_ddi_pll_init(struct drm_device *dev)
+static void hsw_shared_dplls_init(struct drm_i915_private *dev_priv)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	uint32_t val = I915_READ(LCPLL_CTL);
 	int i;
 
 	dev_priv->num_shared_dpll = 2;
@@ -1264,6 +1324,14 @@ void intel_ddi_pll_init(struct drm_device *dev)
 		dev_priv->shared_dplls[i].get_hw_state =
 			hsw_ddi_pll_get_hw_state;
 	}
+}
+
+void intel_ddi_pll_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t val = I915_READ(LCPLL_CTL);
+
+	hsw_shared_dplls_init(dev_priv);
 
 	/* The LCPLL register should be turned on by the BIOS. For now let's
 	 * just check its state and print errors in case something is wrong.
@@ -1444,7 +1512,7 @@ void intel_ddi_get_config(struct intel_encoder *encoder,
 		dev_priv->vbt.edp_bpp = pipe_config->pipe_bpp;
 	}
 
-	intel_ddi_clock_get(encoder, pipe_config);
+	hsw_ddi_clock_get(encoder, pipe_config);
 }
 
 static void intel_ddi_destroy(struct drm_encoder *encoder)
diff --git a/drivers/gpu/drm/i915/intel_display.c b/drivers/gpu/drm/i915/intel_display.c
index 018fb7222f60..c9e220963a78 100644
--- a/drivers/gpu/drm/i915/intel_display.c
+++ b/drivers/gpu/drm/i915/intel_display.c
@@ -73,9 +73,6 @@ static const uint32_t intel_cursor_formats[] = {
 	DRM_FORMAT_ARGB8888,
 };
 
-#define DIV_ROUND_CLOSEST_ULL(ll, d)	\
-({ unsigned long long _tmp = (ll)+(d)/2; do_div(_tmp, d); _tmp; })
-
 static void intel_increase_pllclock(struct drm_device *dev,
 				    enum pipe pipe);
 static void intel_crtc_update_cursor(struct drm_crtc *crtc, bool on);
@@ -91,15 +88,16 @@ static int intel_framebuffer_init(struct drm_device *dev,
 				  struct intel_framebuffer *ifb,
 				  struct drm_mode_fb_cmd2 *mode_cmd,
 				  struct drm_i915_gem_object *obj);
-static void intel_dp_set_m_n(struct intel_crtc *crtc);
 static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc);
 static void intel_set_pipe_timings(struct intel_crtc *intel_crtc);
 static void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
-					 struct intel_link_m_n *m_n);
+					 struct intel_link_m_n *m_n,
+					 struct intel_link_m_n *m2_n2);
 static void ironlake_set_pipeconf(struct drm_crtc *crtc);
 static void haswell_set_pipeconf(struct drm_crtc *crtc);
 static void intel_set_pipe_csc(struct drm_crtc *crtc);
 static void vlv_prepare_pll(struct intel_crtc *crtc);
+static void chv_prepare_pll(struct intel_crtc *crtc);
 
 static struct intel_encoder *intel_find_encoder(struct intel_connector *connector, int pipe)
 {
@@ -899,7 +897,8 @@ static void g4x_wait_for_vblank(struct drm_device *dev, int pipe)
 	frame = I915_READ(frame_reg);
 
 	if (wait_for(I915_READ_NOTRACE(frame_reg) != frame, 50))
-		WARN(1, "vblank wait timed out\n");
+		WARN(1, "vblank wait on pipe %c timed out\n",
+		     pipe_name(pipe));
 }
 
 /**
@@ -940,7 +939,8 @@ void intel_wait_for_vblank(struct drm_device *dev, int pipe)
 	if (wait_for(I915_READ(pipestat_reg) &
 		     PIPE_VBLANK_INTERRUPT_STATUS,
 		     50))
-		DRM_DEBUG_KMS("vblank wait timed out\n");
+		DRM_DEBUG_KMS("vblank wait on pipe %c timed out\n",
+			      pipe_name(pipe));
 }
 
 static bool pipe_dsl_stopped(struct drm_device *dev, enum pipe pipe)
@@ -964,8 +964,7 @@ static bool pipe_dsl_stopped(struct drm_device *dev, enum pipe pipe)
 
 /*
  * intel_wait_for_pipe_off - wait for pipe to turn off
- * @dev: drm device
- * @pipe: pipe to wait for
+ * @crtc: crtc whose pipe to wait for
  *
  * After disabling a pipe, we can't wait for vblank in the usual way,
  * spinning on the vblank interrupt status bit, since we won't actually
@@ -979,11 +978,12 @@ static bool pipe_dsl_stopped(struct drm_device *dev, enum pipe pipe)
  *   ends up stopping at the start of the next frame).
  *
  */
-void intel_wait_for_pipe_off(struct drm_device *dev, int pipe)
+static void intel_wait_for_pipe_off(struct intel_crtc *crtc)
 {
+	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum transcoder cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv,
-								      pipe);
+	enum transcoder cpu_transcoder = crtc->config.cpu_transcoder;
+	enum pipe pipe = crtc->pipe;
 
 	if (INTEL_INFO(dev)->gen >= 4) {
 		int reg = PIPECONF(cpu_transcoder);
@@ -1192,27 +1192,40 @@ void assert_fdi_rx_pll(struct drm_i915_private *dev_priv,
 static void assert_panel_unlocked(struct drm_i915_private *dev_priv,
 				  enum pipe pipe)
 {
-	int pp_reg, lvds_reg;
+	struct drm_device *dev = dev_priv->dev;
+	int pp_reg;
 	u32 val;
 	enum pipe panel_pipe = PIPE_A;
 	bool locked = true;
 
-	if (HAS_PCH_SPLIT(dev_priv->dev)) {
+	if (WARN_ON(HAS_DDI(dev)))
+		return;
+
+	if (HAS_PCH_SPLIT(dev)) {
+		u32 port_sel;
+
 		pp_reg = PCH_PP_CONTROL;
-		lvds_reg = PCH_LVDS;
+		port_sel = I915_READ(PCH_PP_ON_DELAYS) & PANEL_PORT_SELECT_MASK;
+
+		if (port_sel == PANEL_PORT_SELECT_LVDS &&
+		    I915_READ(PCH_LVDS) & LVDS_PIPEB_SELECT)
+			panel_pipe = PIPE_B;
+		/* XXX: else fix for eDP */
+	} else if (IS_VALLEYVIEW(dev)) {
+		/* presumably write lock depends on pipe, not port select */
+		pp_reg = VLV_PIPE_PP_CONTROL(pipe);
+		panel_pipe = pipe;
 	} else {
 		pp_reg = PP_CONTROL;
-		lvds_reg = LVDS;
+		if (I915_READ(LVDS) & LVDS_PIPEB_SELECT)
+			panel_pipe = PIPE_B;
 	}
 
 	val = I915_READ(pp_reg);
 	if (!(val & PANEL_POWER_ON) ||
-	    ((val & PANEL_UNLOCK_REGS) == PANEL_UNLOCK_REGS))
+	    ((val & PANEL_UNLOCK_MASK) == PANEL_UNLOCK_REGS))
 		locked = false;
 
-	if (I915_READ(lvds_reg) & LVDS_PIPEB_SELECT)
-		panel_pipe = PIPE_B;
-
 	WARN(panel_pipe == pipe && locked,
 	     "panel assertion failure, pipe %c regs locked\n",
 	     pipe_name(pipe));
@@ -1245,8 +1258,9 @@ void assert_pipe(struct drm_i915_private *dev_priv,
 	enum transcoder cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv,
 								      pipe);
 
-	/* if we need the pipe A quirk it must be always on */
-	if (pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE)
+	/* if we need the pipe quirk it must be always on */
+	if ((pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE) ||
+	    (pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE))
 		state = true;
 
 	if (!intel_display_power_enabled(dev_priv,
@@ -1300,7 +1314,7 @@ static void assert_planes_disabled(struct drm_i915_private *dev_priv,
 	}
 
 	/* Need to check both planes against the pipe */
-	for_each_pipe(i) {
+	for_each_pipe(dev_priv, i) {
 		reg = DSPCNTR(i);
 		val = I915_READ(reg);
 		cur_pipe = (val & DISPPLANE_SEL_PIPE_MASK) >>
@@ -1341,6 +1355,12 @@ static void assert_sprites_disabled(struct drm_i915_private *dev_priv,
 	}
 }
 
+static void assert_vblank_disabled(struct drm_crtc *crtc)
+{
+	if (WARN_ON(drm_crtc_vblank_get(crtc) == 0))
+		drm_crtc_vblank_put(crtc);
+}
+
 static void ibx_assert_pch_refclk_enabled(struct drm_i915_private *dev_priv)
 {
 	u32 val;
@@ -1513,34 +1533,6 @@ static void intel_init_dpio(struct drm_device *dev)
 	}
 }
 
-static void intel_reset_dpio(struct drm_device *dev)
-{
-	struct drm_i915_private *dev_priv = dev->dev_private;
-
-	if (IS_CHERRYVIEW(dev)) {
-		enum dpio_phy phy;
-		u32 val;
-
-		for (phy = DPIO_PHY0; phy < I915_NUM_PHYS_VLV; phy++) {
-			/* Poll for phypwrgood signal */
-			if (wait_for(I915_READ(DISPLAY_PHY_STATUS) &
-						PHY_POWERGOOD(phy), 1))
-				DRM_ERROR("Display PHY %d is not power up\n", phy);
-
-			/*
-			 * Deassert common lane reset for PHY.
-			 *
-			 * This should only be done on init and resume from S3
-			 * with both PLLs disabled, or we risk losing DPIO and
-			 * PLL synchronization.
-			 */
-			val = I915_READ(DISPLAY_PHY_CONTROL);
-			I915_WRITE(DISPLAY_PHY_CONTROL,
-				PHY_COM_LANE_RESET_DEASSERT(phy, val));
-		}
-	}
-}
-
 static void vlv_enable_pll(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
@@ -1554,7 +1546,7 @@ static void vlv_enable_pll(struct intel_crtc *crtc)
 	BUG_ON(!IS_VALLEYVIEW(dev_priv->dev));
 
 	/* PLL is protected by panel, make sure we can write it */
-	if (IS_MOBILE(dev_priv->dev) && !IS_I830(dev_priv->dev))
+	if (IS_MOBILE(dev_priv->dev))
 		assert_panel_unlocked(dev_priv, crtc->pipe);
 
 	I915_WRITE(reg, dpll);
@@ -1617,6 +1609,18 @@ static void chv_enable_pll(struct intel_crtc *crtc)
 	mutex_unlock(&dev_priv->dpio_lock);
 }
 
+static int intel_num_dvo_pipes(struct drm_device *dev)
+{
+	struct intel_crtc *crtc;
+	int count = 0;
+
+	for_each_intel_crtc(dev, crtc)
+		count += crtc->active &&
+			intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DVO);
+
+	return count;
+}
+
 static void i9xx_enable_pll(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
@@ -1633,7 +1637,18 @@ static void i9xx_enable_pll(struct intel_crtc *crtc)
 	if (IS_MOBILE(dev) && !IS_I830(dev))
 		assert_panel_unlocked(dev_priv, crtc->pipe);
 
-	I915_WRITE(reg, dpll);
+	/* Enable DVO 2x clock on both PLLs if necessary */
+	if (IS_I830(dev) && intel_num_dvo_pipes(dev) > 0) {
+		/*
+		 * It appears to be important that we don't enable this
+		 * for the current pipe before otherwise configuring the
+		 * PLL. No idea how this should be handled if multiple
+		 * DVO outputs are enabled simultaneosly.
+		 */
+		dpll |= DPLL_DVO_2X_MODE;
+		I915_WRITE(DPLL(!crtc->pipe),
+			   I915_READ(DPLL(!crtc->pipe)) | DPLL_DVO_2X_MODE);
+	}
 
 	/* Wait for the clocks to stabilize. */
 	POSTING_READ(reg);
@@ -1672,10 +1687,25 @@ static void i9xx_enable_pll(struct intel_crtc *crtc)
  *
  * Note!  This is for pre-ILK only.
  */
-static void i9xx_disable_pll(struct drm_i915_private *dev_priv, enum pipe pipe)
+static void i9xx_disable_pll(struct intel_crtc *crtc)
 {
-	/* Don't disable pipe A or pipe A PLLs if needed */
-	if (pipe == PIPE_A && (dev_priv->quirks & QUIRK_PIPEA_FORCE))
+	struct drm_device *dev = crtc->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum pipe pipe = crtc->pipe;
+
+	/* Disable DVO 2x clock on both PLLs if necessary */
+	if (IS_I830(dev) &&
+	    intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DVO) &&
+	    intel_num_dvo_pipes(dev) == 1) {
+		I915_WRITE(DPLL(PIPE_B),
+			   I915_READ(DPLL(PIPE_B)) & ~DPLL_DVO_2X_MODE);
+		I915_WRITE(DPLL(PIPE_A),
+			   I915_READ(DPLL(PIPE_A)) & ~DPLL_DVO_2X_MODE);
+	}
+
+	/* Don't disable pipe or pipe PLLs if needed */
+	if ((pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE) ||
+	    (pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE))
 		return;
 
 	/* Make sure the pipe isn't still relying on us */
@@ -1712,7 +1742,7 @@ static void chv_disable_pll(struct drm_i915_private *dev_priv, enum pipe pipe)
 	assert_pipe_disabled(dev_priv, pipe);
 
 	/* Set PLL en = 0 */
-	val = DPLL_SSC_REF_CLOCK_CHV;
+	val = DPLL_SSC_REF_CLOCK_CHV | DPLL_REFA_CLK_ENABLE_VLV;
 	if (pipe != PIPE_A)
 		val |= DPLL_INTEGRATED_CRI_CLK_VLV;
 	I915_WRITE(DPLL(pipe), val);
@@ -1806,7 +1836,7 @@ static void intel_enable_shared_dpll(struct intel_crtc *crtc)
 	if (WARN_ON(pll->refcount == 0))
 		return;
 
-	DRM_DEBUG_KMS("enable %s (active %d, on? %d)for crtc %d\n",
+	DRM_DEBUG_KMS("enable %s (active %d, on? %d) for crtc %d\n",
 		      pll->name, pll->active, pll->on,
 		      crtc->base.base.id);
 
@@ -1824,7 +1854,7 @@ static void intel_enable_shared_dpll(struct intel_crtc *crtc)
 	pll->on = true;
 }
 
-void intel_disable_shared_dpll(struct intel_crtc *crtc)
+static void intel_disable_shared_dpll(struct intel_crtc *crtc)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1868,7 +1898,7 @@ static void ironlake_enable_pch_transcoder(struct drm_i915_private *dev_priv,
 	uint32_t reg, val, pipeconf_val;
 
 	/* PCH only available on ILK+ */
-	BUG_ON(INTEL_INFO(dev)->gen < 5);
+	BUG_ON(!HAS_PCH_SPLIT(dev));
 
 	/* Make sure PCH DPLL is enabled */
 	assert_shared_dpll_enabled(dev_priv,
@@ -1921,7 +1951,7 @@ static void lpt_enable_pch_transcoder(struct drm_i915_private *dev_priv,
 	u32 val, pipeconf_val;
 
 	/* PCH only available on ILK+ */
-	BUG_ON(INTEL_INFO(dev_priv->dev)->gen < 5);
+	BUG_ON(!HAS_PCH_SPLIT(dev_priv->dev));
 
 	/* FDI must be feeding us bits for PCH ports */
 	assert_fdi_tx_enabled(dev_priv, (enum pipe) cpu_transcoder);
@@ -2043,8 +2073,8 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 	reg = PIPECONF(cpu_transcoder);
 	val = I915_READ(reg);
 	if (val & PIPECONF_ENABLE) {
-		WARN_ON(!(pipe == PIPE_A &&
-			  dev_priv->quirks & QUIRK_PIPEA_FORCE));
+		WARN_ON(!((pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE) ||
+			  (pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE)));
 		return;
 	}
 
@@ -2054,21 +2084,19 @@ static void intel_enable_pipe(struct intel_crtc *crtc)
 
 /**
  * intel_disable_pipe - disable a pipe, asserting requirements
- * @dev_priv: i915 private structure
- * @pipe: pipe to disable
+ * @crtc: crtc whose pipes is to be disabled
  *
- * Disable @pipe, making sure that various hardware specific requirements
- * are met, if applicable, e.g. plane disabled, panel fitter off, etc.
- *
- * @pipe should be %PIPE_A or %PIPE_B.
+ * Disable the pipe of @crtc, making sure that various hardware
+ * specific requirements are met, if applicable, e.g. plane
+ * disabled, panel fitter off, etc.
  *
  * Will wait until the pipe has shut down before returning.
  */
-static void intel_disable_pipe(struct drm_i915_private *dev_priv,
-			       enum pipe pipe)
+static void intel_disable_pipe(struct intel_crtc *crtc)
 {
-	enum transcoder cpu_transcoder = intel_pipe_to_cpu_transcoder(dev_priv,
-								      pipe);
+	struct drm_i915_private *dev_priv = crtc->base.dev->dev_private;
+	enum transcoder cpu_transcoder = crtc->config.cpu_transcoder;
+	enum pipe pipe = crtc->pipe;
 	int reg;
 	u32 val;
 
@@ -2080,17 +2108,26 @@ static void intel_disable_pipe(struct drm_i915_private *dev_priv,
 	assert_cursor_disabled(dev_priv, pipe);
 	assert_sprites_disabled(dev_priv, pipe);
 
-	/* Don't disable pipe A or pipe A PLLs if needed */
-	if (pipe == PIPE_A && (dev_priv->quirks & QUIRK_PIPEA_FORCE))
-		return;
-
 	reg = PIPECONF(cpu_transcoder);
 	val = I915_READ(reg);
 	if ((val & PIPECONF_ENABLE) == 0)
 		return;
 
-	I915_WRITE(reg, val & ~PIPECONF_ENABLE);
-	intel_wait_for_pipe_off(dev_priv->dev, pipe);
+	/*
+	 * Double wide has implications for planes
+	 * so best keep it disabled when not needed.
+	 */
+	if (crtc->config.double_wide)
+		val &= ~PIPECONF_DOUBLE_WIDE;
+
+	/* Don't disable pipe or pipe PLLs if needed */
+	if (!(pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE) &&
+	    !(pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE))
+		val &= ~PIPECONF_ENABLE;
+
+	I915_WRITE(reg, val);
+	if ((val & PIPECONF_ENABLE) == 0)
+		intel_wait_for_pipe_off(crtc);
 }
 
 /*
@@ -2109,35 +2146,28 @@ void intel_flush_primary_plane(struct drm_i915_private *dev_priv,
 
 /**
  * intel_enable_primary_hw_plane - enable the primary plane on a given pipe
- * @dev_priv: i915 private structure
- * @plane: plane to enable
- * @pipe: pipe being fed
+ * @plane:  plane to be enabled
+ * @crtc: crtc for the plane
  *
- * Enable @plane on @pipe, making sure that @pipe is running first.
+ * Enable @plane on @crtc, making sure that the pipe is running first.
  */
-static void intel_enable_primary_hw_plane(struct drm_i915_private *dev_priv,
-					  enum plane plane, enum pipe pipe)
+static void intel_enable_primary_hw_plane(struct drm_plane *plane,
+					  struct drm_crtc *crtc)
 {
-	struct drm_device *dev = dev_priv->dev;
-	struct intel_crtc *intel_crtc =
-		to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-	int reg;
-	u32 val;
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 
 	/* If the pipe isn't enabled, we can't pump pixels and may hang */
-	assert_pipe_enabled(dev_priv, pipe);
+	assert_pipe_enabled(dev_priv, intel_crtc->pipe);
 
 	if (intel_crtc->primary_enabled)
 		return;
 
 	intel_crtc->primary_enabled = true;
 
-	reg = DSPCNTR(plane);
-	val = I915_READ(reg);
-	WARN_ON(val & DISPLAY_PLANE_ENABLE);
-
-	I915_WRITE(reg, val | DISPLAY_PLANE_ENABLE);
-	intel_flush_primary_plane(dev_priv, plane);
+	dev_priv->display.update_primary_plane(crtc, plane->fb,
+					       crtc->x, crtc->y);
 
 	/*
 	 * BDW signals flip done immediately if the plane
@@ -2150,31 +2180,27 @@ static void intel_enable_primary_hw_plane(struct drm_i915_private *dev_priv,
 
 /**
  * intel_disable_primary_hw_plane - disable the primary hardware plane
- * @dev_priv: i915 private structure
- * @plane: plane to disable
- * @pipe: pipe consuming the data
+ * @plane: plane to be disabled
+ * @crtc: crtc for the plane
  *
- * Disable @plane; should be an independent operation.
+ * Disable @plane on @crtc, making sure that the pipe is running first.
  */
-static void intel_disable_primary_hw_plane(struct drm_i915_private *dev_priv,
-					   enum plane plane, enum pipe pipe)
+static void intel_disable_primary_hw_plane(struct drm_plane *plane,
+					   struct drm_crtc *crtc)
 {
-	struct intel_crtc *intel_crtc =
-		to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
-	int reg;
-	u32 val;
+	struct drm_device *dev = plane->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+
+	assert_pipe_enabled(dev_priv, intel_crtc->pipe);
 
 	if (!intel_crtc->primary_enabled)
 		return;
 
 	intel_crtc->primary_enabled = false;
 
-	reg = DSPCNTR(plane);
-	val = I915_READ(reg);
-	WARN_ON((val & DISPLAY_PLANE_ENABLE) == 0);
-
-	I915_WRITE(reg, val & ~DISPLAY_PLANE_ENABLE);
-	intel_flush_primary_plane(dev_priv, plane);
+	dev_priv->display.update_primary_plane(crtc, plane->fb,
+					       crtc->x, crtc->y);
 }
 
 static bool need_vtd_wa(struct drm_device *dev)
@@ -2233,6 +2259,15 @@ intel_pin_and_fence_fb_obj(struct drm_device *dev,
 	if (need_vtd_wa(dev) && alignment < 256 * 1024)
 		alignment = 256 * 1024;
 
+	/*
+	 * Global gtt pte registers are special registers which actually forward
+	 * writes to a chunk of system memory. Which means that there is no risk
+	 * that the register values disappear as soon as we call
+	 * intel_runtime_pm_put(), so it is correct to wrap only the
+	 * pin/unpin/fence and not more.
+	 */
+	intel_runtime_pm_get(dev_priv);
+
 	dev_priv->mm.interruptible = false;
 	ret = i915_gem_object_pin_to_display_plane(obj, alignment, pipelined);
 	if (ret)
@@ -2250,12 +2285,14 @@ intel_pin_and_fence_fb_obj(struct drm_device *dev,
 	i915_gem_object_pin_fence(obj);
 
 	dev_priv->mm.interruptible = true;
+	intel_runtime_pm_put(dev_priv);
 	return 0;
 
 err_unpin:
 	i915_gem_object_unpin_from_display_plane(obj);
 err_interruptible:
 	dev_priv->mm.interruptible = true;
+	intel_runtime_pm_put(dev_priv);
 	return ret;
 }
 
@@ -2411,16 +2448,46 @@ static void i9xx_update_primary_plane(struct drm_crtc *crtc,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	struct drm_i915_gem_object *obj;
 	int plane = intel_crtc->plane;
 	unsigned long linear_offset;
 	u32 dspcntr;
-	u32 reg;
+	u32 reg = DSPCNTR(plane);
+	int pixel_size;
+
+	if (!intel_crtc->primary_enabled) {
+		I915_WRITE(reg, 0);
+		if (INTEL_INFO(dev)->gen >= 4)
+			I915_WRITE(DSPSURF(plane), 0);
+		else
+			I915_WRITE(DSPADDR(plane), 0);
+		POSTING_READ(reg);
+		return;
+	}
+
+	obj = intel_fb_obj(fb);
+	if (WARN_ON(obj == NULL))
+		return;
+
+	pixel_size = drm_format_plane_cpp(fb->pixel_format, 0);
+
+	dspcntr = DISPPLANE_GAMMA_ENABLE;
+
+	dspcntr |= DISPLAY_PLANE_ENABLE;
+
+	if (INTEL_INFO(dev)->gen < 4) {
+		if (intel_crtc->pipe == PIPE_B)
+			dspcntr |= DISPPLANE_SEL_PIPE_B;
+
+		/* pipesrc and dspsize control the size that is scaled from,
+		 * which should always be the user's requested size.
+		 */
+		I915_WRITE(DSPSIZE(plane),
+			   ((intel_crtc->config.pipe_src_h - 1) << 16) |
+			   (intel_crtc->config.pipe_src_w - 1));
+		I915_WRITE(DSPPOS(plane), 0);
+	}
 
-	reg = DSPCNTR(plane);
-	dspcntr = I915_READ(reg);
-	/* Mask out pixel format bits in case we change it */
-	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_C8:
 		dspcntr |= DISPPLANE_8BPP;
@@ -2452,30 +2519,40 @@ static void i9xx_update_primary_plane(struct drm_crtc *crtc,
 		BUG();
 	}
 
-	if (INTEL_INFO(dev)->gen >= 4) {
-		if (obj->tiling_mode != I915_TILING_NONE)
-			dspcntr |= DISPPLANE_TILED;
-		else
-			dspcntr &= ~DISPPLANE_TILED;
-	}
+	if (INTEL_INFO(dev)->gen >= 4 &&
+	    obj->tiling_mode != I915_TILING_NONE)
+		dspcntr |= DISPPLANE_TILED;
 
 	if (IS_G4X(dev))
 		dspcntr |= DISPPLANE_TRICKLE_FEED_DISABLE;
 
-	I915_WRITE(reg, dspcntr);
-
-	linear_offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
+	linear_offset = y * fb->pitches[0] + x * pixel_size;
 
 	if (INTEL_INFO(dev)->gen >= 4) {
 		intel_crtc->dspaddr_offset =
 			intel_gen4_compute_page_offset(&x, &y, obj->tiling_mode,
-						       fb->bits_per_pixel / 8,
+						       pixel_size,
 						       fb->pitches[0]);
 		linear_offset -= intel_crtc->dspaddr_offset;
 	} else {
 		intel_crtc->dspaddr_offset = linear_offset;
 	}
 
+	if (to_intel_plane(crtc->primary)->rotation == BIT(DRM_ROTATE_180)) {
+		dspcntr |= DISPPLANE_ROTATE_180;
+
+		x += (intel_crtc->config.pipe_src_w - 1);
+		y += (intel_crtc->config.pipe_src_h - 1);
+
+		/* Finding the last pixel of the last line of the display
+		data and adding to linear_offset*/
+		linear_offset +=
+			(intel_crtc->config.pipe_src_h - 1) * fb->pitches[0] +
+			(intel_crtc->config.pipe_src_w - 1) * pixel_size;
+	}
+
+	I915_WRITE(reg, dspcntr);
+
 	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
 		      i915_gem_obj_ggtt_offset(obj), linear_offset, x, y,
 		      fb->pitches[0]);
@@ -2497,16 +2574,33 @@ static void ironlake_update_primary_plane(struct drm_crtc *crtc,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
+	struct drm_i915_gem_object *obj;
 	int plane = intel_crtc->plane;
 	unsigned long linear_offset;
 	u32 dspcntr;
-	u32 reg;
+	u32 reg = DSPCNTR(plane);
+	int pixel_size;
+
+	if (!intel_crtc->primary_enabled) {
+		I915_WRITE(reg, 0);
+		I915_WRITE(DSPSURF(plane), 0);
+		POSTING_READ(reg);
+		return;
+	}
+
+	obj = intel_fb_obj(fb);
+	if (WARN_ON(obj == NULL))
+		return;
+
+	pixel_size = drm_format_plane_cpp(fb->pixel_format, 0);
+
+	dspcntr = DISPPLANE_GAMMA_ENABLE;
+
+	dspcntr |= DISPLAY_PLANE_ENABLE;
+
+	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
+		dspcntr |= DISPPLANE_PIPE_CSC_ENABLE;
 
-	reg = DSPCNTR(plane);
-	dspcntr = I915_READ(reg);
-	/* Mask out pixel format bits in case we change it */
-	dspcntr &= ~DISPPLANE_PIXFORMAT_MASK;
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_C8:
 		dspcntr |= DISPPLANE_8BPP;
@@ -2536,22 +2630,32 @@ static void ironlake_update_primary_plane(struct drm_crtc *crtc,
 
 	if (obj->tiling_mode != I915_TILING_NONE)
 		dspcntr |= DISPPLANE_TILED;
-	else
-		dspcntr &= ~DISPPLANE_TILED;
 
-	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
-		dspcntr &= ~DISPPLANE_TRICKLE_FEED_DISABLE;
-	else
+	if (!IS_HASWELL(dev) && !IS_BROADWELL(dev))
 		dspcntr |= DISPPLANE_TRICKLE_FEED_DISABLE;
 
-	I915_WRITE(reg, dspcntr);
-
-	linear_offset = y * fb->pitches[0] + x * (fb->bits_per_pixel / 8);
+	linear_offset = y * fb->pitches[0] + x * pixel_size;
 	intel_crtc->dspaddr_offset =
 		intel_gen4_compute_page_offset(&x, &y, obj->tiling_mode,
-					       fb->bits_per_pixel / 8,
+					       pixel_size,
 					       fb->pitches[0]);
 	linear_offset -= intel_crtc->dspaddr_offset;
+	if (to_intel_plane(crtc->primary)->rotation == BIT(DRM_ROTATE_180)) {
+		dspcntr |= DISPPLANE_ROTATE_180;
+
+		if (!IS_HASWELL(dev) && !IS_BROADWELL(dev)) {
+			x += (intel_crtc->config.pipe_src_w - 1);
+			y += (intel_crtc->config.pipe_src_h - 1);
+
+			/* Finding the last pixel of the last line of the display
+			data and adding to linear_offset*/
+			linear_offset +=
+				(intel_crtc->config.pipe_src_h - 1) * fb->pitches[0] +
+				(intel_crtc->config.pipe_src_w - 1) * pixel_size;
+		}
+	}
+
+	I915_WRITE(reg, dspcntr);
 
 	DRM_DEBUG_KMS("Writing base %08lX %08lX %d %d %d\n",
 		      i915_gem_obj_ggtt_offset(obj), linear_offset, x, y,
@@ -3335,23 +3439,54 @@ bool intel_has_pending_fb_unpin(struct drm_device *dev)
 	return false;
 }
 
+static void page_flip_completed(struct intel_crtc *intel_crtc)
+{
+	struct drm_i915_private *dev_priv = to_i915(intel_crtc->base.dev);
+	struct intel_unpin_work *work = intel_crtc->unpin_work;
+
+	/* ensure that the unpin work is consistent wrt ->pending. */
+	smp_rmb();
+	intel_crtc->unpin_work = NULL;
+
+	if (work->event)
+		drm_send_vblank_event(intel_crtc->base.dev,
+				      intel_crtc->pipe,
+				      work->event);
+
+	drm_crtc_vblank_put(&intel_crtc->base);
+
+	wake_up_all(&dev_priv->pending_flip_queue);
+	queue_work(dev_priv->wq, &work->work);
+
+	trace_i915_flip_complete(intel_crtc->plane,
+				 work->pending_flip_obj);
+}
+
 void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	if (crtc->primary->fb == NULL)
-		return;
-
 	WARN_ON(waitqueue_active(&dev_priv->pending_flip_queue));
+	if (WARN_ON(wait_event_timeout(dev_priv->pending_flip_queue,
+				       !intel_crtc_has_pending_flip(crtc),
+				       60*HZ) == 0)) {
+		struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+		unsigned long flags;
 
-	WARN_ON(wait_event_timeout(dev_priv->pending_flip_queue,
-				   !intel_crtc_has_pending_flip(crtc),
-				   60*HZ) == 0);
+		spin_lock_irqsave(&dev->event_lock, flags);
+		if (intel_crtc->unpin_work) {
+			WARN_ONCE(1, "Removing stuck page flip\n");
+			page_flip_completed(intel_crtc);
+		}
+		spin_unlock_irqrestore(&dev->event_lock, flags);
+	}
 
-	mutex_lock(&dev->struct_mutex);
-	intel_finish_fb(crtc->primary->fb);
-	mutex_unlock(&dev->struct_mutex);
+	if (crtc->primary->fb) {
+		mutex_lock(&dev->struct_mutex);
+		intel_finish_fb(crtc->primary->fb);
+		mutex_unlock(&dev->struct_mutex);
+	}
 }
 
 /* Program iCLKIP clock to the desired frequency */
@@ -3900,14 +4035,14 @@ static void intel_crtc_dpms_overlay(struct intel_crtc *intel_crtc, bool enable)
 static void intel_crtc_enable_planes(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	int pipe = intel_crtc->pipe;
-	int plane = intel_crtc->plane;
+
+	assert_vblank_disabled(crtc);
 
 	drm_vblank_on(dev, pipe);
 
-	intel_enable_primary_hw_plane(dev_priv, plane, pipe);
+	intel_enable_primary_hw_plane(crtc->primary, crtc);
 	intel_enable_planes(crtc);
 	intel_crtc_update_cursor(crtc, true);
 	intel_crtc_dpms_overlay(intel_crtc, true);
@@ -3944,7 +4079,7 @@ static void intel_crtc_disable_planes(struct drm_crtc *crtc)
 	intel_crtc_dpms_overlay(intel_crtc, false);
 	intel_crtc_update_cursor(crtc, false);
 	intel_disable_planes(crtc);
-	intel_disable_primary_hw_plane(dev_priv, plane, pipe);
+	intel_disable_primary_hw_plane(crtc->primary, crtc);
 
 	/*
 	 * FIXME: Once we grow proper nuclear flip support out of this we need
@@ -3954,6 +4089,8 @@ static void intel_crtc_disable_planes(struct drm_crtc *crtc)
 	intel_frontbuffer_flip(dev, INTEL_FRONTBUFFER_ALL_MASK(pipe));
 
 	drm_vblank_off(dev, pipe);
+
+	assert_vblank_disabled(crtc);
 }
 
 static void ironlake_crtc_enable(struct drm_crtc *crtc)
@@ -3963,7 +4100,6 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
-	enum plane plane = intel_crtc->plane;
 
 	WARN_ON(!crtc->enabled);
 
@@ -3980,18 +4116,11 @@ static void ironlake_crtc_enable(struct drm_crtc *crtc)
 
 	if (intel_crtc->config.has_pch_encoder) {
 		intel_cpu_transcoder_set_m_n(intel_crtc,
-					     &intel_crtc->config.fdi_m_n);
+				     &intel_crtc->config.fdi_m_n, NULL);
 	}
 
 	ironlake_set_pipeconf(crtc);
 
-	/* Set up the display plane register */
-	I915_WRITE(DSPCNTR(plane), DISPPLANE_GAMMA_ENABLE);
-	POSTING_READ(DSPCNTR(plane));
-
-	dev_priv->display.update_primary_plane(crtc, crtc->primary->fb,
-					       crtc->x, crtc->y);
-
 	intel_crtc->active = true;
 
 	intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
@@ -4076,7 +4205,6 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
-	enum plane plane = intel_crtc->plane;
 
 	WARN_ON(!crtc->enabled);
 
@@ -4091,22 +4219,20 @@ static void haswell_crtc_enable(struct drm_crtc *crtc)
 
 	intel_set_pipe_timings(intel_crtc);
 
+	if (intel_crtc->config.cpu_transcoder != TRANSCODER_EDP) {
+		I915_WRITE(PIPE_MULT(intel_crtc->config.cpu_transcoder),
+			   intel_crtc->config.pixel_multiplier - 1);
+	}
+
 	if (intel_crtc->config.has_pch_encoder) {
 		intel_cpu_transcoder_set_m_n(intel_crtc,
-					     &intel_crtc->config.fdi_m_n);
+				     &intel_crtc->config.fdi_m_n, NULL);
 	}
 
 	haswell_set_pipeconf(crtc);
 
 	intel_set_pipe_csc(crtc);
 
-	/* Set up the display plane register */
-	I915_WRITE(DSPCNTR(plane), DISPPLANE_GAMMA_ENABLE | DISPPLANE_PIPE_CSC_ENABLE);
-	POSTING_READ(DSPCNTR(plane));
-
-	dev_priv->display.update_primary_plane(crtc, crtc->primary->fb,
-					       crtc->x, crtc->y);
-
 	intel_crtc->active = true;
 
 	intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
@@ -4187,10 +4313,7 @@ static void ironlake_crtc_disable(struct drm_crtc *crtc)
 	if (intel_crtc->config.has_pch_encoder)
 		intel_set_pch_fifo_underrun_reporting(dev, pipe, false);
 
-	intel_disable_pipe(dev_priv, pipe);
-
-	if (intel_crtc->config.dp_encoder_is_mst)
-		intel_ddi_set_vc_payload_alloc(crtc, false);
+	intel_disable_pipe(intel_crtc);
 
 	ironlake_pfit_disable(intel_crtc);
 
@@ -4239,7 +4362,6 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
-	int pipe = intel_crtc->pipe;
 	enum transcoder cpu_transcoder = intel_crtc->config.cpu_transcoder;
 
 	if (!intel_crtc->active)
@@ -4254,7 +4376,10 @@ static void haswell_crtc_disable(struct drm_crtc *crtc)
 
 	if (intel_crtc->config.has_pch_encoder)
 		intel_set_pch_fifo_underrun_reporting(dev, TRANSCODER_A, false);
-	intel_disable_pipe(dev_priv, pipe);
+	intel_disable_pipe(intel_crtc);
+
+	if (intel_crtc->config.dp_encoder_is_mst)
+		intel_ddi_set_vc_payload_alloc(crtc, false);
 
 	intel_ddi_disable_transcoder_func(dev_priv, cpu_transcoder);
 
@@ -4529,12 +4654,57 @@ static void valleyview_set_cdclk(struct drm_device *dev, int cdclk)
 	vlv_update_cdclk(dev);
 }
 
+static void cherryview_set_cdclk(struct drm_device *dev, int cdclk)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 val, cmd;
+
+	WARN_ON(dev_priv->display.get_display_clock_speed(dev) != dev_priv->vlv_cdclk_freq);
+
+	switch (cdclk) {
+	case 400000:
+		cmd = 3;
+		break;
+	case 333333:
+	case 320000:
+		cmd = 2;
+		break;
+	case 266667:
+		cmd = 1;
+		break;
+	case 200000:
+		cmd = 0;
+		break;
+	default:
+		WARN_ON(1);
+		return;
+	}
+
+	mutex_lock(&dev_priv->rps.hw_lock);
+	val = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ);
+	val &= ~DSPFREQGUAR_MASK_CHV;
+	val |= (cmd << DSPFREQGUAR_SHIFT_CHV);
+	vlv_punit_write(dev_priv, PUNIT_REG_DSPFREQ, val);
+	if (wait_for((vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) &
+		      DSPFREQSTAT_MASK_CHV) == (cmd << DSPFREQSTAT_SHIFT_CHV),
+		     50)) {
+		DRM_ERROR("timed out waiting for CDclk change\n");
+	}
+	mutex_unlock(&dev_priv->rps.hw_lock);
+
+	vlv_update_cdclk(dev);
+}
+
 static int valleyview_calc_cdclk(struct drm_i915_private *dev_priv,
 				 int max_pixclk)
 {
 	int vco = valleyview_get_vco(dev_priv);
 	int freq_320 = (vco <<  1) % 320000 != 0 ? 333333 : 320000;
 
+	/* FIXME: Punit isn't quite ready yet */
+	if (IS_CHERRYVIEW(dev_priv->dev))
+		return 400000;
+
 	/*
 	 * Really only a few cases to deal with, as only 4 CDclks are supported:
 	 *   200MHz
@@ -4597,21 +4767,23 @@ static void valleyview_modeset_global_resources(struct drm_device *dev)
 	int max_pixclk = intel_mode_max_pixclk(dev_priv);
 	int req_cdclk = valleyview_calc_cdclk(dev_priv, max_pixclk);
 
-	if (req_cdclk != dev_priv->vlv_cdclk_freq)
-		valleyview_set_cdclk(dev, req_cdclk);
+	if (req_cdclk != dev_priv->vlv_cdclk_freq) {
+		if (IS_CHERRYVIEW(dev))
+			cherryview_set_cdclk(dev, req_cdclk);
+		else
+			valleyview_set_cdclk(dev, req_cdclk);
+	}
+
 	modeset_update_crtc_power_domains(dev);
 }
 
 static void valleyview_crtc_enable(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
-	int plane = intel_crtc->plane;
 	bool is_dsi;
-	u32 dspcntr;
 
 	WARN_ON(!crtc->enabled);
 
@@ -4620,33 +4792,20 @@ static void valleyview_crtc_enable(struct drm_crtc *crtc)
 
 	is_dsi = intel_pipe_has_type(crtc, INTEL_OUTPUT_DSI);
 
-	if (!is_dsi && !IS_CHERRYVIEW(dev))
-		vlv_prepare_pll(intel_crtc);
-
-	/* Set up the display plane register */
-	dspcntr = DISPPLANE_GAMMA_ENABLE;
+	if (!is_dsi) {
+		if (IS_CHERRYVIEW(dev))
+			chv_prepare_pll(intel_crtc);
+		else
+			vlv_prepare_pll(intel_crtc);
+	}
 
 	if (intel_crtc->config.has_dp_encoder)
 		intel_dp_set_m_n(intel_crtc);
 
 	intel_set_pipe_timings(intel_crtc);
 
-	/* pipesrc and dspsize control the size that is scaled from,
-	 * which should always be the user's requested size.
-	 */
-	I915_WRITE(DSPSIZE(plane),
-		   ((intel_crtc->config.pipe_src_h - 1) << 16) |
-		   (intel_crtc->config.pipe_src_w - 1));
-	I915_WRITE(DSPPOS(plane), 0);
-
 	i9xx_set_pipeconf(intel_crtc);
 
-	I915_WRITE(DSPCNTR(plane), dspcntr);
-	POSTING_READ(DSPCNTR(plane));
-
-	dev_priv->display.update_primary_plane(crtc, crtc->primary->fb,
-					       crtc->x, crtc->y);
-
 	intel_crtc->active = true;
 
 	intel_set_cpu_fifo_underrun_reporting(dev, pipe, true);
@@ -4694,12 +4853,9 @@ static void i9xx_set_pll_dividers(struct intel_crtc *crtc)
 static void i9xx_crtc_enable(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_encoder *encoder;
 	int pipe = intel_crtc->pipe;
-	int plane = intel_crtc->plane;
-	u32 dspcntr;
 
 	WARN_ON(!crtc->enabled);
 
@@ -4708,35 +4864,13 @@ static void i9xx_crtc_enable(struct drm_crtc *crtc)
 
 	i9xx_set_pll_dividers(intel_crtc);
 
-	/* Set up the display plane register */
-	dspcntr = DISPPLANE_GAMMA_ENABLE;
-
-	if (pipe == 0)
-		dspcntr &= ~DISPPLANE_SEL_PIPE_MASK;
-	else
-		dspcntr |= DISPPLANE_SEL_PIPE_B;
-
 	if (intel_crtc->config.has_dp_encoder)
 		intel_dp_set_m_n(intel_crtc);
 
 	intel_set_pipe_timings(intel_crtc);
 
-	/* pipesrc and dspsize control the size that is scaled from,
-	 * which should always be the user's requested size.
-	 */
-	I915_WRITE(DSPSIZE(plane),
-		   ((intel_crtc->config.pipe_src_h - 1) << 16) |
-		   (intel_crtc->config.pipe_src_w - 1));
-	I915_WRITE(DSPPOS(plane), 0);
-
 	i9xx_set_pipeconf(intel_crtc);
 
-	I915_WRITE(DSPCNTR(plane), dspcntr);
-	POSTING_READ(DSPCNTR(plane));
-
-	dev_priv->display.update_primary_plane(crtc, crtc->primary->fb,
-					       crtc->x, crtc->y);
-
 	intel_crtc->active = true;
 
 	if (!IS_GEN2(dev))
@@ -4832,7 +4966,7 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 	 */
 	intel_wait_for_vblank(dev, pipe);
 
-	intel_disable_pipe(dev_priv, pipe);
+	intel_disable_pipe(intel_crtc);
 
 	i9xx_pfit_disable(intel_crtc);
 
@@ -4846,7 +4980,7 @@ static void i9xx_crtc_disable(struct drm_crtc *crtc)
 		else if (IS_VALLEYVIEW(dev))
 			vlv_disable_pll(dev_priv, pipe);
 		else
-			i9xx_disable_pll(dev_priv, pipe);
+			i9xx_disable_pll(intel_crtc);
 	}
 
 	if (!IS_GEN2(dev))
@@ -5265,6 +5399,10 @@ static int valleyview_get_display_clock_speed(struct drm_device *dev)
 	u32 val;
 	int divider;
 
+	/* FIXME: Punit isn't quite ready yet */
+	if (IS_CHERRYVIEW(dev))
+		return 400000;
+
 	mutex_lock(&dev_priv->dpio_lock);
 	val = vlv_cck_read(dev_priv, CCK_DISPLAY_CLOCK_CONTROL);
 	mutex_unlock(&dev_priv->dpio_lock);
@@ -5509,7 +5647,8 @@ static void intel_pch_transcoder_set_m_n(struct intel_crtc *crtc,
 }
 
 static void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
-					 struct intel_link_m_n *m_n)
+					 struct intel_link_m_n *m_n,
+					 struct intel_link_m_n *m2_n2)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -5521,6 +5660,18 @@ static void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
 		I915_WRITE(PIPE_DATA_N1(transcoder), m_n->gmch_n);
 		I915_WRITE(PIPE_LINK_M1(transcoder), m_n->link_m);
 		I915_WRITE(PIPE_LINK_N1(transcoder), m_n->link_n);
+		/* M2_N2 registers to be set only for gen < 8 (M2_N2 available
+		 * for gen < 8) and if DRRS is supported (to make sure the
+		 * registers are not unnecessarily accessed).
+		 */
+		if (m2_n2 && INTEL_INFO(dev)->gen < 8 &&
+			crtc->config.has_drrs) {
+			I915_WRITE(PIPE_DATA_M2(transcoder),
+					TU_SIZE(m2_n2->tu) | m2_n2->gmch_m);
+			I915_WRITE(PIPE_DATA_N2(transcoder), m2_n2->gmch_n);
+			I915_WRITE(PIPE_LINK_M2(transcoder), m2_n2->link_m);
+			I915_WRITE(PIPE_LINK_N2(transcoder), m2_n2->link_n);
+		}
 	} else {
 		I915_WRITE(PIPE_DATA_M_G4X(pipe), TU_SIZE(m_n->tu) | m_n->gmch_m);
 		I915_WRITE(PIPE_DATA_N_G4X(pipe), m_n->gmch_n);
@@ -5529,12 +5680,13 @@ static void intel_cpu_transcoder_set_m_n(struct intel_crtc *crtc,
 	}
 }
 
-static void intel_dp_set_m_n(struct intel_crtc *crtc)
+void intel_dp_set_m_n(struct intel_crtc *crtc)
 {
 	if (crtc->config.has_pch_encoder)
 		intel_pch_transcoder_set_m_n(crtc, &crtc->config.dp_m_n);
 	else
-		intel_cpu_transcoder_set_m_n(crtc, &crtc->config.dp_m_n);
+		intel_cpu_transcoder_set_m_n(crtc, &crtc->config.dp_m_n,
+						   &crtc->config.dp_m2_n2);
 }
 
 static void vlv_update_pll(struct intel_crtc *crtc)
@@ -5652,6 +5804,18 @@ static void vlv_prepare_pll(struct intel_crtc *crtc)
 
 static void chv_update_pll(struct intel_crtc *crtc)
 {
+	crtc->config.dpll_hw_state.dpll = DPLL_SSC_REF_CLOCK_CHV |
+		DPLL_REFA_CLK_ENABLE_VLV | DPLL_VGA_MODE_DIS |
+		DPLL_VCO_ENABLE;
+	if (crtc->pipe != PIPE_A)
+		crtc->config.dpll_hw_state.dpll |= DPLL_INTEGRATED_CRI_CLK_VLV;
+
+	crtc->config.dpll_hw_state.dpll_md =
+		(crtc->config.pixel_multiplier - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
+}
+
+static void chv_prepare_pll(struct intel_crtc *crtc)
+{
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe = crtc->pipe;
@@ -5661,15 +5825,6 @@ static void chv_update_pll(struct intel_crtc *crtc)
 	u32 bestn, bestm1, bestm2, bestp1, bestp2, bestm2_frac;
 	int refclk;
 
-	crtc->config.dpll_hw_state.dpll = DPLL_SSC_REF_CLOCK_CHV |
-		DPLL_REFA_CLK_ENABLE_VLV | DPLL_VGA_MODE_DIS |
-		DPLL_VCO_ENABLE;
-	if (pipe != PIPE_A)
-		crtc->config.dpll_hw_state.dpll |= DPLL_INTEGRATED_CRI_CLK_VLV;
-
-	crtc->config.dpll_hw_state.dpll_md =
-		(crtc->config.pixel_multiplier - 1) << DPLL_MD_UDI_MULTIPLIER_SHIFT;
-
 	bestn = crtc->config.dpll.n;
 	bestm2_frac = crtc->config.dpll.m2 & 0x3fffff;
 	bestm1 = crtc->config.dpll.m1;
@@ -5829,7 +5984,7 @@ static void i8xx_update_pll(struct intel_crtc *crtc,
 			dpll |= PLL_P2_DIVIDE_BY_4;
 	}
 
-	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DVO))
+	if (!IS_I830(dev) && intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_DVO))
 		dpll |= DPLL_DVO_2X_MODE;
 
 	if (intel_pipe_has_type(&crtc->base, INTEL_OUTPUT_LVDS) &&
@@ -5980,9 +6135,9 @@ static void i9xx_set_pipeconf(struct intel_crtc *intel_crtc)
 
 	pipeconf = 0;
 
-	if (dev_priv->quirks & QUIRK_PIPEA_FORCE &&
-	    I915_READ(PIPECONF(intel_crtc->pipe)) & PIPECONF_ENABLE)
-		pipeconf |= PIPECONF_ENABLE;
+	if ((intel_crtc->pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE) ||
+	    (intel_crtc->pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE))
+		pipeconf |= I915_READ(PIPECONF(intel_crtc->pipe)) & PIPECONF_ENABLE;
 
 	if (intel_crtc->config.double_wide)
 		pipeconf |= PIPECONF_DOUBLE_WIDE;
@@ -6225,7 +6380,7 @@ static void i9xx_get_plane_config(struct intel_crtc *crtc,
 	crtc->base.primary->fb->height = ((val >> 0) & 0xfff) + 1;
 
 	val = I915_READ(DSPSTRIDE(pipe));
-	crtc->base.primary->fb->pitches[0] = val & 0xffffff80;
+	crtc->base.primary->fb->pitches[0] = val & 0xffffffc0;
 
 	aligned_height = intel_align_height(dev, crtc->base.primary->fb->height,
 					    plane_config->tiled);
@@ -6335,6 +6490,14 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 	}
 	pipe_config->dpll_hw_state.dpll = I915_READ(DPLL(crtc->pipe));
 	if (!IS_VALLEYVIEW(dev)) {
+		/*
+		 * DPLL_DVO_2X_MODE must be enabled for both DPLLs
+		 * on 830. Filter it out here so that we don't
+		 * report errors due to that.
+		 */
+		if (IS_I830(dev))
+			pipe_config->dpll_hw_state.dpll &= ~DPLL_DVO_2X_MODE;
+
 		pipe_config->dpll_hw_state.fp0 = I915_READ(FP0(crtc->pipe));
 		pipe_config->dpll_hw_state.fp1 = I915_READ(FP1(crtc->pipe));
 	} else {
@@ -6357,7 +6520,6 @@ static bool i9xx_get_pipe_config(struct intel_crtc *crtc,
 static void ironlake_init_pch_refclk(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
 	u32 val, final;
 	bool has_lvds = false;
@@ -6367,8 +6529,7 @@ static void ironlake_init_pch_refclk(struct drm_device *dev)
 	bool can_ssc = false;
 
 	/* We need to take the global config into account */
-	list_for_each_entry(encoder, &mode_config->encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		switch (encoder->type) {
 		case INTEL_OUTPUT_LVDS:
 			has_panel = true;
@@ -6675,11 +6836,10 @@ static void lpt_disable_clkout_dp(struct drm_device *dev)
 
 static void lpt_init_pch_refclk(struct drm_device *dev)
 {
-	struct drm_mode_config *mode_config = &dev->mode_config;
 	struct intel_encoder *encoder;
 	bool has_vga = false;
 
-	list_for_each_entry(encoder, &mode_config->encoder_list, base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		switch (encoder->type) {
 		case INTEL_OUTPUT_ANALOG:
 			has_vga = true;
@@ -7135,7 +7295,8 @@ static void intel_pch_transcoder_get_m_n(struct intel_crtc *crtc,
 
 static void intel_cpu_transcoder_get_m_n(struct intel_crtc *crtc,
 					 enum transcoder transcoder,
-					 struct intel_link_m_n *m_n)
+					 struct intel_link_m_n *m_n,
+					 struct intel_link_m_n *m2_n2)
 {
 	struct drm_device *dev = crtc->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -7149,6 +7310,20 @@ static void intel_cpu_transcoder_get_m_n(struct intel_crtc *crtc,
 		m_n->gmch_n = I915_READ(PIPE_DATA_N1(transcoder));
 		m_n->tu = ((I915_READ(PIPE_DATA_M1(transcoder))
 			    & TU_SIZE_MASK) >> TU_SIZE_SHIFT) + 1;
+		/* Read M2_N2 registers only for gen < 8 (M2_N2 available for
+		 * gen < 8) and if DRRS is supported (to make sure the
+		 * registers are not unnecessarily read).
+		 */
+		if (m2_n2 && INTEL_INFO(dev)->gen < 8 &&
+			crtc->config.has_drrs) {
+			m2_n2->link_m = I915_READ(PIPE_LINK_M2(transcoder));
+			m2_n2->link_n =	I915_READ(PIPE_LINK_N2(transcoder));
+			m2_n2->gmch_m =	I915_READ(PIPE_DATA_M2(transcoder))
+					& ~TU_SIZE_MASK;
+			m2_n2->gmch_n =	I915_READ(PIPE_DATA_N2(transcoder));
+			m2_n2->tu = ((I915_READ(PIPE_DATA_M2(transcoder))
+					& TU_SIZE_MASK) >> TU_SIZE_SHIFT) + 1;
+		}
 	} else {
 		m_n->link_m = I915_READ(PIPE_LINK_M_G4X(pipe));
 		m_n->link_n = I915_READ(PIPE_LINK_N_G4X(pipe));
@@ -7167,14 +7342,15 @@ void intel_dp_get_m_n(struct intel_crtc *crtc,
 		intel_pch_transcoder_get_m_n(crtc, &pipe_config->dp_m_n);
 	else
 		intel_cpu_transcoder_get_m_n(crtc, pipe_config->cpu_transcoder,
-					     &pipe_config->dp_m_n);
+					     &pipe_config->dp_m_n,
+					     &pipe_config->dp_m2_n2);
 }
 
 static void ironlake_get_fdi_m_n_config(struct intel_crtc *crtc,
 					struct intel_crtc_config *pipe_config)
 {
 	intel_cpu_transcoder_get_m_n(crtc, pipe_config->cpu_transcoder,
-				     &pipe_config->fdi_m_n);
+				     &pipe_config->fdi_m_n, NULL);
 }
 
 static void ironlake_get_pfit_config(struct intel_crtc *crtc,
@@ -7245,7 +7421,7 @@ static void ironlake_get_plane_config(struct intel_crtc *crtc,
 	crtc->base.primary->fb->height = ((val >> 0) & 0xfff) + 1;
 
 	val = I915_READ(DSPSTRIDE(pipe));
-	crtc->base.primary->fb->pitches[0] = val & 0xffffff80;
+	crtc->base.primary->fb->pitches[0] = val & 0xffffffc0;
 
 	aligned_height = intel_align_height(dev, crtc->base.primary->fb->height,
 					    plane_config->tiled);
@@ -7605,6 +7781,22 @@ static int haswell_crtc_mode_set(struct drm_crtc *crtc,
 	return 0;
 }
 
+static void haswell_get_ddi_pll(struct drm_i915_private *dev_priv,
+				enum port port,
+				struct intel_crtc_config *pipe_config)
+{
+	pipe_config->ddi_pll_sel = I915_READ(PORT_CLK_SEL(port));
+
+	switch (pipe_config->ddi_pll_sel) {
+	case PORT_CLK_SEL_WRPLL1:
+		pipe_config->shared_dpll = DPLL_ID_WRPLL1;
+		break;
+	case PORT_CLK_SEL_WRPLL2:
+		pipe_config->shared_dpll = DPLL_ID_WRPLL2;
+		break;
+	}
+}
+
 static void haswell_get_ddi_port_state(struct intel_crtc *crtc,
 				       struct intel_crtc_config *pipe_config)
 {
@@ -7618,16 +7810,7 @@ static void haswell_get_ddi_port_state(struct intel_crtc *crtc,
 
 	port = (tmp & TRANS_DDI_PORT_MASK) >> TRANS_DDI_PORT_SHIFT;
 
-	pipe_config->ddi_pll_sel = I915_READ(PORT_CLK_SEL(port));
-
-	switch (pipe_config->ddi_pll_sel) {
-	case PORT_CLK_SEL_WRPLL1:
-		pipe_config->shared_dpll = DPLL_ID_WRPLL1;
-		break;
-	case PORT_CLK_SEL_WRPLL2:
-		pipe_config->shared_dpll = DPLL_ID_WRPLL2;
-		break;
-	}
+	haswell_get_ddi_pll(dev_priv, port, pipe_config);
 
 	if (pipe_config->shared_dpll >= 0) {
 		pll = &dev_priv->shared_dplls[pipe_config->shared_dpll];
@@ -7709,7 +7892,12 @@ static bool haswell_get_pipe_config(struct intel_crtc *crtc,
 		pipe_config->ips_enabled = hsw_crtc_supports_ips(crtc) &&
 			(I915_READ(IPS_CTL) & IPS_ENABLE);
 
-	pipe_config->pixel_multiplier = 1;
+	if (pipe_config->cpu_transcoder != TRANSCODER_EDP) {
+		pipe_config->pixel_multiplier =
+			I915_READ(PIPE_MULT(pipe_config->cpu_transcoder)) + 1;
+	} else {
+		pipe_config->pixel_multiplier = 1;
+	}
 
 	return true;
 }
@@ -8027,74 +8215,62 @@ static void i845_update_cursor(struct drm_crtc *crtc, u32 base)
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	uint32_t cntl;
+	uint32_t cntl = 0, size = 0;
 
-	if (base != intel_crtc->cursor_base) {
-		/* On these chipsets we can only modify the base whilst
-		 * the cursor is disabled.
-		 */
-		if (intel_crtc->cursor_cntl) {
-			I915_WRITE(_CURACNTR, 0);
-			POSTING_READ(_CURACNTR);
-			intel_crtc->cursor_cntl = 0;
+	if (base) {
+		unsigned int width = intel_crtc->cursor_width;
+		unsigned int height = intel_crtc->cursor_height;
+		unsigned int stride = roundup_pow_of_two(width) * 4;
+
+		switch (stride) {
+		default:
+			WARN_ONCE(1, "Invalid cursor width/stride, width=%u, stride=%u\n",
+				  width, stride);
+			stride = 256;
+			/* fallthrough */
+		case 256:
+		case 512:
+		case 1024:
+		case 2048:
+			break;
 		}
 
-		I915_WRITE(_CURABASE, base);
-		POSTING_READ(_CURABASE);
+		cntl |= CURSOR_ENABLE |
+			CURSOR_GAMMA_ENABLE |
+			CURSOR_FORMAT_ARGB |
+			CURSOR_STRIDE(stride);
+
+		size = (height << 12) | width;
 	}
 
-	/* XXX width must be 64, stride 256 => 0x00 << 28 */
-	cntl = 0;
-	if (base)
-		cntl = (CURSOR_ENABLE |
-			CURSOR_GAMMA_ENABLE |
-			CURSOR_FORMAT_ARGB);
-	if (intel_crtc->cursor_cntl != cntl) {
-		I915_WRITE(_CURACNTR, cntl);
+	if (intel_crtc->cursor_cntl != 0 &&
+	    (intel_crtc->cursor_base != base ||
+	     intel_crtc->cursor_size != size ||
+	     intel_crtc->cursor_cntl != cntl)) {
+		/* On these chipsets we can only modify the base/size/stride
+		 * whilst the cursor is disabled.
+		 */
+		I915_WRITE(_CURACNTR, 0);
 		POSTING_READ(_CURACNTR);
-		intel_crtc->cursor_cntl = cntl;
+		intel_crtc->cursor_cntl = 0;
 	}
-}
 
-static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base)
-{
-	struct drm_device *dev = crtc->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	int pipe = intel_crtc->pipe;
-	uint32_t cntl;
+	if (intel_crtc->cursor_base != base)
+		I915_WRITE(_CURABASE, base);
 
-	cntl = 0;
-	if (base) {
-		cntl = MCURSOR_GAMMA_ENABLE;
-		switch (intel_crtc->cursor_width) {
-			case 64:
-				cntl |= CURSOR_MODE_64_ARGB_AX;
-				break;
-			case 128:
-				cntl |= CURSOR_MODE_128_ARGB_AX;
-				break;
-			case 256:
-				cntl |= CURSOR_MODE_256_ARGB_AX;
-				break;
-			default:
-				WARN_ON(1);
-				return;
-		}
-		cntl |= pipe << 28; /* Connect to correct pipe */
+	if (intel_crtc->cursor_size != size) {
+		I915_WRITE(CURSIZE, size);
+		intel_crtc->cursor_size = size;
 	}
+
 	if (intel_crtc->cursor_cntl != cntl) {
-		I915_WRITE(CURCNTR(pipe), cntl);
-		POSTING_READ(CURCNTR(pipe));
+		I915_WRITE(_CURACNTR, cntl);
+		POSTING_READ(_CURACNTR);
 		intel_crtc->cursor_cntl = cntl;
 	}
-
-	/* and commit changes on next vblank */
-	I915_WRITE(CURBASE(pipe), base);
-	POSTING_READ(CURBASE(pipe));
 }
 
-static void ivb_update_cursor(struct drm_crtc *crtc, u32 base)
+static void i9xx_update_cursor(struct drm_crtc *crtc, u32 base)
 {
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -8119,6 +8295,7 @@ static void ivb_update_cursor(struct drm_crtc *crtc, u32 base)
 				WARN_ON(1);
 				return;
 		}
+		cntl |= pipe << 28; /* Connect to correct pipe */
 	}
 	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		cntl |= CURSOR_PIPE_CSC_ENABLE;
@@ -8178,15 +8355,50 @@ static void intel_crtc_update_cursor(struct drm_crtc *crtc,
 
 	I915_WRITE(CURPOS(pipe), pos);
 
-	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev) || IS_BROADWELL(dev))
-		ivb_update_cursor(crtc, base);
-	else if (IS_845G(dev) || IS_I865G(dev))
+	if (IS_845G(dev) || IS_I865G(dev))
 		i845_update_cursor(crtc, base);
 	else
 		i9xx_update_cursor(crtc, base);
 	intel_crtc->cursor_base = base;
 }
 
+static bool cursor_size_ok(struct drm_device *dev,
+			   uint32_t width, uint32_t height)
+{
+	if (width == 0 || height == 0)
+		return false;
+
+	/*
+	 * 845g/865g are special in that they are only limited by
+	 * the width of their cursors, the height is arbitrary up to
+	 * the precision of the register. Everything else requires
+	 * square cursors, limited to a few power-of-two sizes.
+	 */
+	if (IS_845G(dev) || IS_I865G(dev)) {
+		if ((width & 63) != 0)
+			return false;
+
+		if (width > (IS_845G(dev) ? 64 : 512))
+			return false;
+
+		if (height > 1023)
+			return false;
+	} else {
+		switch (width | height) {
+		case 256:
+		case 128:
+			if (IS_GEN2(dev))
+				return false;
+		case 64:
+			break;
+		default:
+			return false;
+		}
+	}
+
+	return true;
+}
+
 /*
  * intel_crtc_cursor_set_obj - Set cursor to specified GEM object
  *
@@ -8202,7 +8414,7 @@ static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	enum pipe pipe = intel_crtc->pipe;
-	unsigned old_width;
+	unsigned old_width, stride;
 	uint32_t addr;
 	int ret;
 
@@ -8210,20 +8422,18 @@ static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 	if (!obj) {
 		DRM_DEBUG_KMS("cursor off\n");
 		addr = 0;
-		obj = NULL;
 		mutex_lock(&dev->struct_mutex);
 		goto finish;
 	}
 
 	/* Check for which cursor types we support */
-	if (!((width == 64 && height == 64) ||
-			(width == 128 && height == 128 && !IS_GEN2(dev)) ||
-			(width == 256 && height == 256 && !IS_GEN2(dev)))) {
+	if (!cursor_size_ok(dev, width, height)) {
 		DRM_DEBUG("Cursor dimension not supported\n");
 		return -EINVAL;
 	}
 
-	if (obj->base.size < width * height * 4) {
+	stride = roundup_pow_of_two(width) * 4;
+	if (obj->base.size < stride * height) {
 		DRM_DEBUG_KMS("buffer is too small\n");
 		ret = -ENOMEM;
 		goto fail;
@@ -8240,6 +8450,15 @@ static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 			goto fail_locked;
 		}
 
+		/*
+		 * Global gtt pte registers are special registers which actually
+		 * forward writes to a chunk of system memory. Which means that
+		 * there is no risk that the register values disappear as soon
+		 * as we call intel_runtime_pm_put(), so it is correct to wrap
+		 * only the pin/unpin/fence and not more.
+		 */
+		intel_runtime_pm_get(dev_priv);
+
 		/* Note that the w/a also requires 2 PTE of padding following
 		 * the bo. We currently fill all unused PTE with the shadow
 		 * page and so we should always have valid PTE following the
@@ -8252,16 +8471,20 @@ static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 		ret = i915_gem_object_pin_to_display_plane(obj, alignment, NULL);
 		if (ret) {
 			DRM_DEBUG_KMS("failed to move cursor bo into the GTT\n");
+			intel_runtime_pm_put(dev_priv);
 			goto fail_locked;
 		}
 
 		ret = i915_gem_object_put_fence(obj);
 		if (ret) {
 			DRM_DEBUG_KMS("failed to release fence for cursor");
+			intel_runtime_pm_put(dev_priv);
 			goto fail_unpin;
 		}
 
 		addr = i915_gem_obj_ggtt_offset(obj);
+
+		intel_runtime_pm_put(dev_priv);
 	} else {
 		int align = IS_I830(dev) ? 16 * 1024 : 256;
 		ret = i915_gem_object_attach_phys(obj, align);
@@ -8272,9 +8495,6 @@ static int intel_crtc_cursor_set_obj(struct drm_crtc *crtc,
 		addr = obj->phys_handle->busaddr;
 	}
 
-	if (IS_GEN2(dev))
-		I915_WRITE(CURSIZE, (height << 12) | width);
-
  finish:
 	if (intel_crtc->cursor_bo) {
 		if (!INTEL_INFO(dev)->cursor_needs_physical)
@@ -8462,8 +8682,6 @@ bool intel_get_load_detect_pipe(struct drm_connector *connector,
 		      connector->base.id, connector->name,
 		      encoder->base.id, encoder->name);
 
-	drm_modeset_acquire_init(ctx, 0);
-
 retry:
 	ret = drm_modeset_lock(&config->connection_mutex, ctx);
 	if (ret)
@@ -8502,10 +8720,14 @@ retry:
 		i++;
 		if (!(encoder->possible_crtcs & (1 << i)))
 			continue;
-		if (!possible_crtc->enabled) {
-			crtc = possible_crtc;
-			break;
-		}
+		if (possible_crtc->enabled)
+			continue;
+		/* This can occur when applying the pipe A quirk on resume. */
+		if (to_intel_crtc(possible_crtc)->new_enabled)
+			continue;
+
+		crtc = possible_crtc;
+		break;
 	}
 
 	/*
@@ -8574,15 +8796,11 @@ fail_unlock:
 		goto retry;
 	}
 
-	drm_modeset_drop_locks(ctx);
-	drm_modeset_acquire_fini(ctx);
-
 	return false;
 }
 
 void intel_release_load_detect_pipe(struct drm_connector *connector,
-				    struct intel_load_detect_pipe *old,
-				    struct drm_modeset_acquire_ctx *ctx)
+				    struct intel_load_detect_pipe *old)
 {
 	struct intel_encoder *intel_encoder =
 		intel_attached_encoder(connector);
@@ -8606,17 +8824,12 @@ void intel_release_load_detect_pipe(struct drm_connector *connector,
 			drm_framebuffer_unreference(old->release_fb);
 		}
 
-		goto unlock;
 		return;
 	}
 
 	/* Switch crtc and encoder back off if necessary */
 	if (old->dpms_mode != DRM_MODE_DPMS_ON)
 		connector->funcs->dpms(connector, old->dpms_mode);
-
-unlock:
-	drm_modeset_drop_locks(ctx);
-	drm_modeset_acquire_fini(ctx);
 }
 
 static int i9xx_pll_refclk(struct drm_device *dev,
@@ -8928,12 +9141,13 @@ static void intel_mark_fb_busy(struct drm_device *dev,
 			       unsigned frontbuffer_bits,
 			       struct intel_engine_cs *ring)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum pipe pipe;
 
 	if (!i915.powersave)
 		return;
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		if (!(frontbuffer_bits & INTEL_FRONTBUFFER_ALL_MASK(pipe)))
 			continue;
 
@@ -9003,6 +9217,14 @@ void intel_frontbuffer_flush(struct drm_device *dev,
 	intel_mark_fb_busy(dev, frontbuffer_bits, NULL);
 
 	intel_edp_psr_flush(dev, frontbuffer_bits);
+
+	/*
+	 * FIXME: Unconditional fbc flushing here is a rather gross hack and
+	 * needs to be reworked into a proper frontbuffer tracking scheme like
+	 * psr employs.
+	 */
+	if (IS_BROADWELL(dev))
+		gen8_fbc_sw_flush(dev, FBC_REND_CACHE_CLEAN);
 }
 
 /**
@@ -9135,7 +9357,6 @@ static void intel_unpin_work_fn(struct work_struct *__work)
 static void do_intel_finish_page_flip(struct drm_device *dev,
 				      struct drm_crtc *crtc)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
 	struct intel_unpin_work *work;
 	unsigned long flags;
@@ -9155,23 +9376,9 @@ static void do_intel_finish_page_flip(struct drm_device *dev,
 		return;
 	}
 
-	/* and that the unpin work is consistent wrt ->pending. */
-	smp_rmb();
-
-	intel_crtc->unpin_work = NULL;
-
-	if (work->event)
-		drm_send_vblank_event(dev, intel_crtc->pipe, work->event);
-
-	drm_crtc_vblank_put(crtc);
+	page_flip_completed(intel_crtc);
 
 	spin_unlock_irqrestore(&dev->event_lock, flags);
-
-	wake_up_all(&dev_priv->pending_flip_queue);
-
-	queue_work(dev_priv->wq, &work->work);
-
-	trace_i915_flip_complete(intel_crtc->plane, work->pending_flip_obj);
 }
 
 void intel_finish_page_flip(struct drm_device *dev, int pipe)
@@ -9516,6 +9723,8 @@ static bool use_mmio_flip(struct intel_engine_cs *ring,
 		return false;
 	else if (i915.use_mmio_flip > 0)
 		return true;
+	else if (i915.enable_execlists)
+		return true;
 	else
 		return ring != obj->ring;
 }
@@ -9649,6 +9858,65 @@ static int intel_default_queue_flip(struct drm_device *dev,
 	return -ENODEV;
 }
 
+static bool __intel_pageflip_stall_check(struct drm_device *dev,
+					 struct drm_crtc *crtc)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	struct intel_unpin_work *work = intel_crtc->unpin_work;
+	u32 addr;
+
+	if (atomic_read(&work->pending) >= INTEL_FLIP_COMPLETE)
+		return true;
+
+	if (!work->enable_stall_check)
+		return false;
+
+	if (work->flip_ready_vblank == 0) {
+		if (work->flip_queued_ring &&
+		    !i915_seqno_passed(work->flip_queued_ring->get_seqno(work->flip_queued_ring, true),
+				       work->flip_queued_seqno))
+			return false;
+
+		work->flip_ready_vblank = drm_vblank_count(dev, intel_crtc->pipe);
+	}
+
+	if (drm_vblank_count(dev, intel_crtc->pipe) - work->flip_ready_vblank < 3)
+		return false;
+
+	/* Potential stall - if we see that the flip has happened,
+	 * assume a missed interrupt. */
+	if (INTEL_INFO(dev)->gen >= 4)
+		addr = I915_HI_DISPBASE(I915_READ(DSPSURF(intel_crtc->plane)));
+	else
+		addr = I915_READ(DSPADDR(intel_crtc->plane));
+
+	/* There is a potential issue here with a false positive after a flip
+	 * to the same address. We could address this by checking for a
+	 * non-incrementing frame counter.
+	 */
+	return addr == work->gtt_offset;
+}
+
+void intel_check_page_flip(struct drm_device *dev, int pipe)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_crtc *crtc = dev_priv->pipe_to_crtc_mapping[pipe];
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	unsigned long flags;
+
+	if (crtc == NULL)
+		return;
+
+	spin_lock_irqsave(&dev->event_lock, flags);
+	if (intel_crtc->unpin_work && __intel_pageflip_stall_check(dev, crtc)) {
+		WARN_ONCE(1, "Kicking stuck page flip: queued at %d, now %d\n",
+			 intel_crtc->unpin_work->flip_queued_vblank, drm_vblank_count(dev, pipe));
+		page_flip_completed(intel_crtc);
+	}
+	spin_unlock_irqrestore(&dev->event_lock, flags);
+}
+
 static int intel_crtc_page_flip(struct drm_crtc *crtc,
 				struct drm_framebuffer *fb,
 				struct drm_pending_vblank_event *event,
@@ -9705,12 +9973,20 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	/* We borrow the event spin lock for protecting unpin_work */
 	spin_lock_irqsave(&dev->event_lock, flags);
 	if (intel_crtc->unpin_work) {
-		spin_unlock_irqrestore(&dev->event_lock, flags);
-		kfree(work);
-		drm_crtc_vblank_put(crtc);
+		/* Before declaring the flip queue wedged, check if
+		 * the hardware completed the operation behind our backs.
+		 */
+		if (__intel_pageflip_stall_check(dev, crtc)) {
+			DRM_DEBUG_DRIVER("flip queue: previous flip completed, continuing\n");
+			page_flip_completed(intel_crtc);
+		} else {
+			DRM_DEBUG_DRIVER("flip queue: crtc already busy\n");
+			spin_unlock_irqrestore(&dev->event_lock, flags);
 
-		DRM_DEBUG_DRIVER("flip queue: crtc already busy\n");
-		return -EBUSY;
+			drm_crtc_vblank_put(crtc);
+			kfree(work);
+			return -EBUSY;
+		}
 	}
 	intel_crtc->unpin_work = work;
 	spin_unlock_irqrestore(&dev->event_lock, flags);
@@ -9730,8 +10006,6 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 
 	work->pending_flip_obj = obj;
 
-	work->enable_stall_check = true;
-
 	atomic_inc(&intel_crtc->unpin_work_count);
 	intel_crtc->reset_counter = atomic_read(&dev_priv->gpu_error.reset_counter);
 
@@ -9760,14 +10034,26 @@ static int intel_crtc_page_flip(struct drm_crtc *crtc,
 	work->gtt_offset =
 		i915_gem_obj_ggtt_offset(obj) + intel_crtc->dspaddr_offset;
 
-	if (use_mmio_flip(ring, obj))
+	if (use_mmio_flip(ring, obj)) {
 		ret = intel_queue_mmio_flip(dev, crtc, fb, obj, ring,
 					    page_flip_flags);
-	else
+		if (ret)
+			goto cleanup_unpin;
+
+		work->flip_queued_seqno = obj->last_write_seqno;
+		work->flip_queued_ring = obj->ring;
+	} else {
 		ret = dev_priv->display.queue_flip(dev, crtc, fb, obj, ring,
-				page_flip_flags);
-	if (ret)
-		goto cleanup_unpin;
+						   page_flip_flags);
+		if (ret)
+			goto cleanup_unpin;
+
+		work->flip_queued_seqno = intel_ring_get_seqno(ring);
+		work->flip_queued_ring = ring;
+	}
+
+	work->flip_queued_vblank = drm_vblank_count(dev, intel_crtc->pipe);
+	work->enable_stall_check = true;
 
 	i915_gem_track_fb(work->old_fb_obj, obj,
 			  INTEL_FRONTBUFFER_PRIMARY(pipe));
@@ -9802,8 +10088,11 @@ free_work:
 out_hang:
 		intel_crtc_wait_for_pending_flips(crtc);
 		ret = intel_pipe_set_base(crtc, crtc->x, crtc->y, fb);
-		if (ret == 0 && event)
+		if (ret == 0 && event) {
+			spin_lock_irqsave(&dev->event_lock, flags);
 			drm_send_vblank_event(dev, pipe, event);
+			spin_unlock_irqrestore(&dev->event_lock, flags);
+		}
 	}
 	return ret;
 }
@@ -9831,8 +10120,7 @@ static void intel_modeset_update_staged_output_state(struct drm_device *dev)
 			to_intel_encoder(connector->base.encoder);
 	}
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		encoder->new_crtc =
 			to_intel_crtc(encoder->base.crtc);
 	}
@@ -9863,8 +10151,7 @@ static void intel_modeset_commit_output_state(struct drm_device *dev)
 		connector->base.encoder = &connector->new_encoder->base;
 	}
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		encoder->base.crtc = &encoder->new_crtc->base;
 	}
 
@@ -9991,6 +10278,15 @@ static void intel_dump_pipe_config(struct intel_crtc *crtc,
 		      pipe_config->dp_m_n.gmch_m, pipe_config->dp_m_n.gmch_n,
 		      pipe_config->dp_m_n.link_m, pipe_config->dp_m_n.link_n,
 		      pipe_config->dp_m_n.tu);
+
+	DRM_DEBUG_KMS("dp: %i, gmch_m2: %u, gmch_n2: %u, link_m2: %u, link_n2: %u, tu2: %u\n",
+		      pipe_config->has_dp_encoder,
+		      pipe_config->dp_m2_n2.gmch_m,
+		      pipe_config->dp_m2_n2.gmch_n,
+		      pipe_config->dp_m2_n2.link_m,
+		      pipe_config->dp_m2_n2.link_n,
+		      pipe_config->dp_m2_n2.tu);
+
 	DRM_DEBUG_KMS("requested mode:\n");
 	drm_mode_debug_printmodeline(&pipe_config->requested_mode);
 	DRM_DEBUG_KMS("adjusted mode:\n");
@@ -10025,8 +10321,7 @@ static bool check_single_encoder_cloning(struct intel_crtc *crtc,
 	struct drm_device *dev = crtc->base.dev;
 	struct intel_encoder *source_encoder;
 
-	list_for_each_entry(source_encoder,
-			    &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, source_encoder) {
 		if (source_encoder->new_crtc != crtc)
 			continue;
 
@@ -10042,8 +10337,7 @@ static bool check_encoder_cloning(struct intel_crtc *crtc)
 	struct drm_device *dev = crtc->base.dev;
 	struct intel_encoder *encoder;
 
-	list_for_each_entry(encoder,
-			    &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		if (encoder->new_crtc != crtc)
 			continue;
 
@@ -10127,8 +10421,7 @@ encoder_retry:
 	 * adjust it according to limitations or connector properties, and also
 	 * a chance to reject the mode entirely.
 	 */
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 
 		if (&encoder->new_crtc->base != crtc)
 			continue;
@@ -10206,8 +10499,7 @@ intel_modeset_affected_pipes(struct drm_crtc *crtc, unsigned *modeset_pipes,
 				1 << connector->new_encoder->new_crtc->pipe;
 	}
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		if (encoder->base.crtc == &encoder->new_crtc->base)
 			continue;
 
@@ -10281,8 +10573,7 @@ intel_modeset_update_state(struct drm_device *dev, unsigned prepare_pipes)
 	struct intel_crtc *intel_crtc;
 	struct drm_connector *connector;
 
-	list_for_each_entry(intel_encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, intel_encoder) {
 		if (!intel_encoder->base.crtc)
 			continue;
 
@@ -10371,6 +10662,22 @@ intel_pipe_config_compare(struct drm_device *dev,
 		return false; \
 	}
 
+/* This is required for BDW+ where there is only one set of registers for
+ * switching between high and low RR.
+ * This macro can be used whenever a comparison has to be made between one
+ * hw state and multiple sw state variables.
+ */
+#define PIPE_CONF_CHECK_I_ALT(name, alt_name) \
+	if ((current_config->name != pipe_config->name) && \
+		(current_config->alt_name != pipe_config->name)) { \
+			DRM_ERROR("mismatch in " #name " " \
+				  "(expected %i or %i, found %i)\n", \
+				  current_config->name, \
+				  current_config->alt_name, \
+				  pipe_config->name); \
+			return false; \
+	}
+
 #define PIPE_CONF_CHECK_FLAGS(name, mask)	\
 	if ((current_config->name ^ pipe_config->name) & (mask)) { \
 		DRM_ERROR("mismatch in " #name "(" #mask ") "	   \
@@ -10403,11 +10710,28 @@ intel_pipe_config_compare(struct drm_device *dev,
 	PIPE_CONF_CHECK_I(fdi_m_n.tu);
 
 	PIPE_CONF_CHECK_I(has_dp_encoder);
-	PIPE_CONF_CHECK_I(dp_m_n.gmch_m);
-	PIPE_CONF_CHECK_I(dp_m_n.gmch_n);
-	PIPE_CONF_CHECK_I(dp_m_n.link_m);
-	PIPE_CONF_CHECK_I(dp_m_n.link_n);
-	PIPE_CONF_CHECK_I(dp_m_n.tu);
+
+	if (INTEL_INFO(dev)->gen < 8) {
+		PIPE_CONF_CHECK_I(dp_m_n.gmch_m);
+		PIPE_CONF_CHECK_I(dp_m_n.gmch_n);
+		PIPE_CONF_CHECK_I(dp_m_n.link_m);
+		PIPE_CONF_CHECK_I(dp_m_n.link_n);
+		PIPE_CONF_CHECK_I(dp_m_n.tu);
+
+		if (current_config->has_drrs) {
+			PIPE_CONF_CHECK_I(dp_m2_n2.gmch_m);
+			PIPE_CONF_CHECK_I(dp_m2_n2.gmch_n);
+			PIPE_CONF_CHECK_I(dp_m2_n2.link_m);
+			PIPE_CONF_CHECK_I(dp_m2_n2.link_n);
+			PIPE_CONF_CHECK_I(dp_m2_n2.tu);
+		}
+	} else {
+		PIPE_CONF_CHECK_I_ALT(dp_m_n.gmch_m, dp_m2_n2.gmch_m);
+		PIPE_CONF_CHECK_I_ALT(dp_m_n.gmch_n, dp_m2_n2.gmch_n);
+		PIPE_CONF_CHECK_I_ALT(dp_m_n.link_m, dp_m2_n2.link_m);
+		PIPE_CONF_CHECK_I_ALT(dp_m_n.link_n, dp_m2_n2.link_n);
+		PIPE_CONF_CHECK_I_ALT(dp_m_n.tu, dp_m2_n2.tu);
+	}
 
 	PIPE_CONF_CHECK_I(adjusted_mode.crtc_hdisplay);
 	PIPE_CONF_CHECK_I(adjusted_mode.crtc_htotal);
@@ -10493,6 +10817,7 @@ intel_pipe_config_compare(struct drm_device *dev,
 
 #undef PIPE_CONF_CHECK_X
 #undef PIPE_CONF_CHECK_I
+#undef PIPE_CONF_CHECK_I_ALT
 #undef PIPE_CONF_CHECK_FLAGS
 #undef PIPE_CONF_CHECK_CLOCK_FUZZY
 #undef PIPE_CONF_QUIRK
@@ -10522,8 +10847,7 @@ check_encoder_state(struct drm_device *dev)
 	struct intel_encoder *encoder;
 	struct intel_connector *connector;
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		bool enabled = false;
 		bool active = false;
 		enum pipe pipe, tracked_pipe;
@@ -10602,8 +10926,7 @@ check_crtc_state(struct drm_device *dev)
 		WARN(crtc->active && !crtc->base.enabled,
 		     "active crtc, but not enabled in sw tracking\n");
 
-		list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-				    base.head) {
+		for_each_intel_encoder(dev, encoder) {
 			if (encoder->base.crtc != &crtc->base)
 				continue;
 			enabled = true;
@@ -10621,12 +10944,12 @@ check_crtc_state(struct drm_device *dev)
 		active = dev_priv->display.get_pipe_config(crtc,
 							   &pipe_config);
 
-		/* hw state is inconsistent with the pipe A quirk */
-		if (crtc->pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE)
+		/* hw state is inconsistent with the pipe quirk */
+		if ((crtc->pipe == PIPE_A && dev_priv->quirks & QUIRK_PIPEA_FORCE) ||
+		    (crtc->pipe == PIPE_B && dev_priv->quirks & QUIRK_PIPEB_FORCE))
 			active = crtc->active;
 
-		list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-				    base.head) {
+		for_each_intel_encoder(dev, encoder) {
 			enum pipe pipe;
 			if (encoder->base.crtc != &crtc->base)
 				continue;
@@ -10994,7 +11317,7 @@ static void intel_set_config_restore_state(struct drm_device *dev,
 	}
 
 	count = 0;
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		encoder->new_crtc =
 			to_intel_crtc(config->save_encoder_crtcs[count++]);
 	}
@@ -11153,8 +11476,7 @@ intel_modeset_stage_output_state(struct drm_device *dev,
 	}
 
 	/* Check for any encoders that needs to be disabled. */
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		int num_connectors = 0;
 		list_for_each_entry(connector,
 				    &dev->mode_config.connector_list,
@@ -11187,9 +11509,7 @@ intel_modeset_stage_output_state(struct drm_device *dev,
 	for_each_intel_crtc(dev, crtc) {
 		crtc->new_enabled = false;
 
-		list_for_each_entry(encoder,
-				    &dev->mode_config.encoder_list,
-				    base.head) {
+		for_each_intel_encoder(dev, encoder) {
 			if (encoder->new_crtc == crtc) {
 				crtc->new_enabled = true;
 				break;
@@ -11226,7 +11546,7 @@ static void disable_crtc_nofb(struct intel_crtc *crtc)
 			connector->new_encoder = NULL;
 	}
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		if (encoder->new_crtc == crtc)
 			encoder->new_crtc = NULL;
 	}
@@ -11289,7 +11609,6 @@ static int intel_crtc_set_config(struct drm_mode_set *set)
 		ret = intel_set_mode(set->crtc, set->mode,
 				     set->x, set->y, set->fb);
 	} else if (config->fb_changed) {
-		struct drm_i915_private *dev_priv = dev->dev_private;
 		struct intel_crtc *intel_crtc = to_intel_crtc(set->crtc);
 
 		intel_crtc_wait_for_pending_flips(set->crtc);
@@ -11303,8 +11622,7 @@ static int intel_crtc_set_config(struct drm_mode_set *set)
 		 */
 		if (!intel_crtc->primary_enabled && ret == 0) {
 			WARN_ON(!intel_crtc->active);
-			intel_enable_primary_hw_plane(dev_priv, intel_crtc->plane,
-						      intel_crtc->pipe);
+			intel_enable_primary_hw_plane(set->crtc->primary, set->crtc);
 		}
 
 		/*
@@ -11457,8 +11775,6 @@ static int
 intel_primary_plane_disable(struct drm_plane *plane)
 {
 	struct drm_device *dev = plane->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_plane *intel_plane = to_intel_plane(plane);
 	struct intel_crtc *intel_crtc;
 
 	if (!plane->fb)
@@ -11481,8 +11797,8 @@ intel_primary_plane_disable(struct drm_plane *plane)
 		goto disable_unpin;
 
 	intel_crtc_wait_for_pending_flips(plane->crtc);
-	intel_disable_primary_hw_plane(dev_priv, intel_plane->plane,
-				       intel_plane->pipe);
+	intel_disable_primary_hw_plane(plane, plane->crtc);
+
 disable_unpin:
 	mutex_lock(&dev->struct_mutex);
 	i915_gem_track_fb(intel_fb_obj(plane->fb), NULL,
@@ -11504,7 +11820,6 @@ intel_primary_plane_setplane(struct drm_plane *plane, struct drm_crtc *crtc,
 	struct drm_device *dev = crtc->dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
-	struct intel_plane *intel_plane = to_intel_plane(plane);
 	struct drm_i915_gem_object *obj = intel_fb_obj(fb);
 	struct drm_i915_gem_object *old_obj = intel_fb_obj(plane->fb);
 	struct drm_rect dest = {
@@ -11526,6 +11841,21 @@ intel_primary_plane_setplane(struct drm_plane *plane, struct drm_crtc *crtc,
 		.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0,
 		.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0,
 	};
+	const struct {
+		int crtc_x, crtc_y;
+		unsigned int crtc_w, crtc_h;
+		uint32_t src_x, src_y, src_w, src_h;
+	} orig = {
+		.crtc_x = crtc_x,
+		.crtc_y = crtc_y,
+		.crtc_w = crtc_w,
+		.crtc_h = crtc_h,
+		.src_x = src_x,
+		.src_y = src_y,
+		.src_w = src_w,
+		.src_h = src_h,
+	};
+	struct intel_plane *intel_plane = to_intel_plane(plane);
 	bool visible;
 	int ret;
 
@@ -11591,9 +11921,7 @@ intel_primary_plane_setplane(struct drm_plane *plane, struct drm_crtc *crtc,
 				  INTEL_FRONTBUFFER_PRIMARY(intel_crtc->pipe));
 
 		if (intel_crtc->primary_enabled)
-			intel_disable_primary_hw_plane(dev_priv,
-						       intel_plane->plane,
-						       intel_plane->pipe);
+			intel_disable_primary_hw_plane(plane, crtc);
 
 
 		if (plane->fb != fb)
@@ -11602,16 +11930,42 @@ intel_primary_plane_setplane(struct drm_plane *plane, struct drm_crtc *crtc,
 
 		mutex_unlock(&dev->struct_mutex);
 
-		return 0;
-	}
+	} else {
+		if (intel_crtc && intel_crtc->active &&
+		    intel_crtc->primary_enabled) {
+			/*
+			 * FBC does not work on some platforms for rotated
+			 * planes, so disable it when rotation is not 0 and
+			 * update it when rotation is set back to 0.
+			 *
+			 * FIXME: This is redundant with the fbc update done in
+			 * the primary plane enable function except that that
+			 * one is done too late. We eventually need to unify
+			 * this.
+			 */
+			if (INTEL_INFO(dev)->gen <= 4 && !IS_G4X(dev) &&
+			    dev_priv->fbc.plane == intel_crtc->plane &&
+			    intel_plane->rotation != BIT(DRM_ROTATE_0)) {
+				intel_disable_fbc(dev);
+			}
+		}
+		ret = intel_pipe_set_base(crtc, src.x1, src.y1, fb);
+		if (ret)
+			return ret;
 
-	ret = intel_pipe_set_base(crtc, src.x1, src.y1, fb);
-	if (ret)
-		return ret;
+		if (!intel_crtc->primary_enabled)
+			intel_enable_primary_hw_plane(plane, crtc);
+	}
 
-	if (!intel_crtc->primary_enabled)
-		intel_enable_primary_hw_plane(dev_priv, intel_crtc->plane,
-					      intel_crtc->pipe);
+	intel_plane->crtc_x = orig.crtc_x;
+	intel_plane->crtc_y = orig.crtc_y;
+	intel_plane->crtc_w = orig.crtc_w;
+	intel_plane->crtc_h = orig.crtc_h;
+	intel_plane->src_x = orig.src_x;
+	intel_plane->src_y = orig.src_y;
+	intel_plane->src_w = orig.src_w;
+	intel_plane->src_h = orig.src_h;
+	intel_plane->obj = obj;
 
 	return 0;
 }
@@ -11628,6 +11982,7 @@ static const struct drm_plane_funcs intel_primary_plane_funcs = {
 	.update_plane = intel_primary_plane_setplane,
 	.disable_plane = intel_primary_plane_disable,
 	.destroy = intel_plane_destroy,
+	.set_property = intel_plane_set_property
 };
 
 static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
@@ -11645,6 +12000,7 @@ static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
 	primary->max_downscale = 1;
 	primary->pipe = pipe;
 	primary->plane = pipe;
+	primary->rotation = BIT(DRM_ROTATE_0);
 	if (HAS_FBC(dev) && INTEL_INFO(dev)->gen < 4)
 		primary->plane = !pipe;
 
@@ -11660,6 +12016,19 @@ static struct drm_plane *intel_primary_plane_create(struct drm_device *dev,
 				 &intel_primary_plane_funcs,
 				 intel_primary_formats, num_formats,
 				 DRM_PLANE_TYPE_PRIMARY);
+
+	if (INTEL_INFO(dev)->gen >= 4) {
+		if (!dev->mode_config.rotation_property)
+			dev->mode_config.rotation_property =
+				drm_mode_create_rotation_property(dev,
+							BIT(DRM_ROTATE_0) |
+							BIT(DRM_ROTATE_180));
+		if (dev->mode_config.rotation_property)
+			drm_object_attach_property(&primary->base.base,
+				dev->mode_config.rotation_property,
+				primary->rotation);
+	}
+
 	return &primary->base;
 }
 
@@ -11700,8 +12069,8 @@ intel_cursor_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
 	};
 	const struct drm_rect clip = {
 		/* integer pixels */
-		.x2 = intel_crtc->config.pipe_src_w,
-		.y2 = intel_crtc->config.pipe_src_h,
+		.x2 = intel_crtc->active ? intel_crtc->config.pipe_src_w : 0,
+		.y2 = intel_crtc->active ? intel_crtc->config.pipe_src_h : 0,
 	};
 	bool visible;
 	int ret;
@@ -11720,6 +12089,10 @@ intel_cursor_plane_update(struct drm_plane *plane, struct drm_crtc *crtc,
 		return intel_crtc_cursor_set_obj(crtc, obj, crtc_w, crtc_h);
 	} else {
 		intel_crtc_update_cursor(crtc, visible);
+
+		intel_frontbuffer_flip(crtc->dev,
+				       INTEL_FRONTBUFFER_CURSOR(intel_crtc->pipe));
+
 		return 0;
 	}
 }
@@ -11796,8 +12169,7 @@ static void intel_crtc_init(struct drm_device *dev, int pipe)
 
 	intel_crtc->cursor_base = ~0;
 	intel_crtc->cursor_cntl = ~0;
-
-	init_waitqueue_head(&intel_crtc->vbl_wait);
+	intel_crtc->cursor_size = ~0;
 
 	BUG_ON(pipe >= ARRAY_SIZE(dev_priv->plane_to_crtc_mapping) ||
 	       dev_priv->plane_to_crtc_mapping[intel_crtc->plane] != NULL);
@@ -11860,8 +12232,7 @@ static int intel_encoder_clones(struct intel_encoder *encoder)
 	int index_mask = 0;
 	int entry = 0;
 
-	list_for_each_entry(source_encoder,
-			    &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, source_encoder) {
 		if (encoders_cloneable(encoder, source_encoder))
 			index_mask |= (1 << entry);
 
@@ -11983,27 +12354,36 @@ static void intel_setup_outputs(struct drm_device *dev)
 		if (I915_READ(PCH_DP_D) & DP_DETECTED)
 			intel_dp_init(dev, PCH_DP_D, PORT_D);
 	} else if (IS_VALLEYVIEW(dev)) {
-		if (I915_READ(VLV_DISPLAY_BASE + GEN4_HDMIB) & SDVO_DETECTED) {
+		/*
+		 * The DP_DETECTED bit is the latched state of the DDC
+		 * SDA pin at boot. However since eDP doesn't require DDC
+		 * (no way to plug in a DP->HDMI dongle) the DDC pins for
+		 * eDP ports may have been muxed to an alternate function.
+		 * Thus we can't rely on the DP_DETECTED bit alone to detect
+		 * eDP ports. Consult the VBT as well as DP_DETECTED to
+		 * detect eDP ports.
+		 */
+		if (I915_READ(VLV_DISPLAY_BASE + GEN4_HDMIB) & SDVO_DETECTED)
 			intel_hdmi_init(dev, VLV_DISPLAY_BASE + GEN4_HDMIB,
 					PORT_B);
-			if (I915_READ(VLV_DISPLAY_BASE + DP_B) & DP_DETECTED)
-				intel_dp_init(dev, VLV_DISPLAY_BASE + DP_B, PORT_B);
-		}
+		if (I915_READ(VLV_DISPLAY_BASE + DP_B) & DP_DETECTED ||
+		    intel_dp_is_edp(dev, PORT_B))
+			intel_dp_init(dev, VLV_DISPLAY_BASE + DP_B, PORT_B);
 
-		if (I915_READ(VLV_DISPLAY_BASE + GEN4_HDMIC) & SDVO_DETECTED) {
+		if (I915_READ(VLV_DISPLAY_BASE + GEN4_HDMIC) & SDVO_DETECTED)
 			intel_hdmi_init(dev, VLV_DISPLAY_BASE + GEN4_HDMIC,
 					PORT_C);
-			if (I915_READ(VLV_DISPLAY_BASE + DP_C) & DP_DETECTED)
-				intel_dp_init(dev, VLV_DISPLAY_BASE + DP_C, PORT_C);
-		}
+		if (I915_READ(VLV_DISPLAY_BASE + DP_C) & DP_DETECTED ||
+		    intel_dp_is_edp(dev, PORT_C))
+			intel_dp_init(dev, VLV_DISPLAY_BASE + DP_C, PORT_C);
 
 		if (IS_CHERRYVIEW(dev)) {
-			if (I915_READ(VLV_DISPLAY_BASE + CHV_HDMID) & SDVO_DETECTED) {
+			if (I915_READ(VLV_DISPLAY_BASE + CHV_HDMID) & SDVO_DETECTED)
 				intel_hdmi_init(dev, VLV_DISPLAY_BASE + CHV_HDMID,
 						PORT_D);
-				if (I915_READ(VLV_DISPLAY_BASE + DP_D) & DP_DETECTED)
-					intel_dp_init(dev, VLV_DISPLAY_BASE + DP_D, PORT_D);
-			}
+			/* eDP not supported on port D, so don't check VBT */
+			if (I915_READ(VLV_DISPLAY_BASE + DP_D) & DP_DETECTED)
+				intel_dp_init(dev, VLV_DISPLAY_BASE + DP_D, PORT_D);
 		}
 
 		intel_dsi_init(dev);
@@ -12050,7 +12430,7 @@ static void intel_setup_outputs(struct drm_device *dev)
 
 	intel_edp_psr_init(dev);
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		encoder->base.possible_crtcs = encoder->crtc_mask;
 		encoder->base.possible_clones =
 			intel_encoder_clones(encoder);
@@ -12316,29 +12696,27 @@ static void intel_init_display(struct drm_device *dev)
 		dev_priv->display.get_display_clock_speed =
 			i830_get_display_clock_speed;
 
-	if (HAS_PCH_SPLIT(dev)) {
-		if (IS_GEN5(dev)) {
-			dev_priv->display.fdi_link_train = ironlake_fdi_link_train;
-			dev_priv->display.write_eld = ironlake_write_eld;
-		} else if (IS_GEN6(dev)) {
-			dev_priv->display.fdi_link_train = gen6_fdi_link_train;
-			dev_priv->display.write_eld = ironlake_write_eld;
-			dev_priv->display.modeset_global_resources =
-				snb_modeset_global_resources;
-		} else if (IS_IVYBRIDGE(dev)) {
-			/* FIXME: detect B0+ stepping and use auto training */
-			dev_priv->display.fdi_link_train = ivb_manual_fdi_link_train;
-			dev_priv->display.write_eld = ironlake_write_eld;
-			dev_priv->display.modeset_global_resources =
-				ivb_modeset_global_resources;
-		} else if (IS_HASWELL(dev) || IS_GEN8(dev)) {
-			dev_priv->display.fdi_link_train = hsw_fdi_link_train;
-			dev_priv->display.write_eld = haswell_write_eld;
-			dev_priv->display.modeset_global_resources =
-				haswell_modeset_global_resources;
-		}
-	} else if (IS_G4X(dev)) {
+	if (IS_G4X(dev)) {
 		dev_priv->display.write_eld = g4x_write_eld;
+	} else if (IS_GEN5(dev)) {
+		dev_priv->display.fdi_link_train = ironlake_fdi_link_train;
+		dev_priv->display.write_eld = ironlake_write_eld;
+	} else if (IS_GEN6(dev)) {
+		dev_priv->display.fdi_link_train = gen6_fdi_link_train;
+		dev_priv->display.write_eld = ironlake_write_eld;
+		dev_priv->display.modeset_global_resources =
+			snb_modeset_global_resources;
+	} else if (IS_IVYBRIDGE(dev)) {
+		/* FIXME: detect B0+ stepping and use auto training */
+		dev_priv->display.fdi_link_train = ivb_manual_fdi_link_train;
+		dev_priv->display.write_eld = ironlake_write_eld;
+		dev_priv->display.modeset_global_resources =
+			ivb_modeset_global_resources;
+	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
+		dev_priv->display.fdi_link_train = hsw_fdi_link_train;
+		dev_priv->display.write_eld = haswell_write_eld;
+		dev_priv->display.modeset_global_resources =
+			haswell_modeset_global_resources;
 	} else if (IS_VALLEYVIEW(dev)) {
 		dev_priv->display.modeset_global_resources =
 			valleyview_modeset_global_resources;
@@ -12372,6 +12750,8 @@ static void intel_init_display(struct drm_device *dev)
 	}
 
 	intel_panel_init_backlight_funcs(dev);
+
+	mutex_init(&dev_priv->pps_mutex);
 }
 
 /*
@@ -12387,6 +12767,14 @@ static void quirk_pipea_force(struct drm_device *dev)
 	DRM_INFO("applying pipe a force quirk\n");
 }
 
+static void quirk_pipeb_force(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	dev_priv->quirks |= QUIRK_PIPEB_FORCE;
+	DRM_INFO("applying pipe b force quirk\n");
+}
+
 /*
  * Some machines (Lenovo U160) do not work with SSC on LVDS for some reason
  */
@@ -12461,6 +12849,12 @@ static struct intel_quirk intel_quirks[] = {
 	/* ThinkPad T60 needs pipe A force quirk (bug #16494) */
 	{ 0x2782, 0x17aa, 0x201a, quirk_pipea_force },
 
+	/* 830 needs to leave pipe A & dpll A up */
+	{ 0x3577, PCI_ANY_ID, PCI_ANY_ID, quirk_pipea_force },
+
+	/* 830 needs to leave pipe B & dpll B up */
+	{ 0x3577, PCI_ANY_ID, PCI_ANY_ID, quirk_pipeb_force },
+
 	/* Lenovo U160 cannot use SSC on LVDS */
 	{ 0x0046, 0x17aa, 0x3920, quirk_ssc_force_disable },
 
@@ -12488,6 +12882,9 @@ static struct intel_quirk intel_quirks[] = {
 	/* Acer C720 and C720P Chromebooks (Celeron 2955U) have backlights */
 	{ 0x0a06, 0x1025, 0x0a11, quirk_backlight_present },
 
+	/* Acer C720 Chromebook (Core i3 4005U) */
+	{ 0x0a16, 0x1025, 0x0a11, quirk_backlight_present },
+
 	/* Toshiba CB35 Chromebook (Celeron 2955U) */
 	{ 0x0a06, 0x1179, 0x0a88, quirk_backlight_present },
 
@@ -12531,7 +12928,11 @@ static void i915_disable_vga(struct drm_device *dev)
 	vga_put(dev->pdev, VGA_RSRC_LEGACY_IO);
 	udelay(300);
 
-	I915_WRITE(vga_reg, VGA_DISP_DISABLE);
+	/*
+	 * Fujitsu-Siemens Lifebook S6010 (830) has problems resuming
+	 * from S3 without preserving (some of?) the other bits.
+	 */
+	I915_WRITE(vga_reg, dev_priv->bios_vgacntr | VGA_DISP_DISABLE);
 	POSTING_READ(vga_reg);
 }
 
@@ -12544,8 +12945,6 @@ void intel_modeset_init_hw(struct drm_device *dev)
 
 	intel_init_clock_gating(dev);
 
-	intel_reset_dpio(dev);
-
 	intel_enable_gt_powersave(dev);
 }
 
@@ -12591,7 +12990,10 @@ void intel_modeset_init(struct drm_device *dev)
 		dev->mode_config.max_height = 8192;
 	}
 
-	if (IS_GEN2(dev)) {
+	if (IS_845G(dev) || IS_I865G(dev)) {
+		dev->mode_config.cursor_width = IS_845G(dev) ? 64 : 512;
+		dev->mode_config.cursor_height = 1023;
+	} else if (IS_GEN2(dev)) {
 		dev->mode_config.cursor_width = GEN2_CURSOR_WIDTH;
 		dev->mode_config.cursor_height = GEN2_CURSOR_HEIGHT;
 	} else {
@@ -12605,7 +13007,7 @@ void intel_modeset_init(struct drm_device *dev)
 		      INTEL_INFO(dev)->num_pipes,
 		      INTEL_INFO(dev)->num_pipes > 1 ? "s" : "");
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		intel_crtc_init(dev, pipe);
 		for_each_sprite(pipe, sprite) {
 			ret = intel_plane_init(dev, pipe, sprite);
@@ -12616,10 +13018,11 @@ void intel_modeset_init(struct drm_device *dev)
 	}
 
 	intel_init_dpio(dev);
-	intel_reset_dpio(dev);
 
 	intel_shared_dpll_init(dev);
 
+	/* save the BIOS value before clobbering it */
+	dev_priv->bios_vgacntr = I915_READ(i915_vgacntrl_reg(dev));
 	/* Just disable it once at startup */
 	i915_disable_vga(dev);
 	intel_setup_outputs(dev);
@@ -12659,7 +13062,7 @@ static void intel_enable_pipe_a(struct drm_device *dev)
 	struct intel_connector *connector;
 	struct drm_connector *crt = NULL;
 	struct intel_load_detect_pipe load_detect_temp;
-	struct drm_modeset_acquire_ctx ctx;
+	struct drm_modeset_acquire_ctx *ctx = dev->mode_config.acquire_ctx;
 
 	/* We can't just switch on the pipe A, we need to set things up with a
 	 * proper mode and output configuration. As a gross hack, enable pipe A
@@ -12676,10 +13079,8 @@ static void intel_enable_pipe_a(struct drm_device *dev)
 	if (!crt)
 		return;
 
-	if (intel_get_load_detect_pipe(crt, NULL, &load_detect_temp, &ctx))
-		intel_release_load_detect_pipe(crt, &load_detect_temp, &ctx);
-
-
+	if (intel_get_load_detect_pipe(crt, NULL, &load_detect_temp, ctx))
+		intel_release_load_detect_pipe(crt, &load_detect_temp);
 }
 
 static bool
@@ -12713,9 +13114,10 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 	I915_WRITE(reg, I915_READ(reg) & ~PIPECONF_FRAME_START_DELAY_MASK);
 
 	/* restore vblank interrupts to correct state */
-	if (crtc->active)
+	if (crtc->active) {
+		update_scanline_offset(crtc);
 		drm_vblank_on(dev, crtc->pipe);
-	else
+	} else
 		drm_vblank_off(dev, crtc->pipe);
 
 	/* We need to sanitize the plane -> pipe mapping first because this will
@@ -12798,7 +13200,7 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 		}
 	}
 
-	if (crtc->active || IS_VALLEYVIEW(dev) || INTEL_INFO(dev)->gen < 5) {
+	if (crtc->active || HAS_GMCH_DISPLAY(dev)) {
 		/*
 		 * We start out with underrun reporting disabled to avoid races.
 		 * For correct bookkeeping mark this on active crtcs.
@@ -12814,8 +13216,6 @@ static void intel_sanitize_crtc(struct intel_crtc *crtc)
 		 */
 		crtc->cpu_fifo_underrun_disabled = true;
 		crtc->pch_fifo_underrun_disabled = true;
-
-		update_scanline_offset(crtc);
 	}
 }
 
@@ -12947,8 +13347,7 @@ static void intel_modeset_readout_hw_state(struct drm_device *dev)
 			intel_display_power_get(dev_priv, POWER_DOMAIN_PLLS);
 	}
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		pipe = 0;
 
 		if (encoder->get_hw_state(encoder, &pipe)) {
@@ -13012,12 +13411,11 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 	}
 
 	/* HW state is read out, now we need to sanitize this mess. */
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		intel_sanitize_encoder(encoder);
 	}
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		crtc = to_intel_crtc(dev_priv->pipe_to_crtc_mapping[pipe]);
 		intel_sanitize_crtc(crtc);
 		intel_dump_pipe_config(crtc, &crtc->config, "[setup_hw_state]");
@@ -13045,7 +13443,7 @@ void intel_modeset_setup_hw_state(struct drm_device *dev,
 		 * We need to use raw interfaces for restoring state to avoid
 		 * checking (bogus) intermediate states.
 		 */
-		for_each_pipe(pipe) {
+		for_each_pipe(dev_priv, pipe) {
 			struct drm_crtc *crtc =
 				dev_priv->pipe_to_crtc_mapping[pipe];
 
@@ -13112,7 +13510,7 @@ void intel_modeset_cleanup(struct drm_device *dev)
 	 * experience fancy races otherwise.
 	 */
 	drm_irq_uninstall(dev);
-	cancel_work_sync(&dev_priv->hotplug_work);
+	intel_hpd_cancel_work(dev_priv);
 	dev_priv->pm._irqs_disabled = true;
 
 	/*
@@ -13266,7 +13664,7 @@ intel_display_capture_error_state(struct drm_device *dev)
 	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		error->power_well_driver = I915_READ(HSW_PWR_WELL_DRIVER);
 
-	for_each_pipe(i) {
+	for_each_pipe(dev_priv, i) {
 		error->pipe[i].power_domain_on =
 			intel_display_power_enabled_unlocked(dev_priv,
 							   POWER_DOMAIN_PIPE(i));
@@ -13330,6 +13728,7 @@ intel_display_print_error_state(struct drm_i915_error_state_buf *m,
 				struct drm_device *dev,
 				struct intel_display_error_state *error)
 {
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	int i;
 
 	if (!error)
@@ -13339,7 +13738,7 @@ intel_display_print_error_state(struct drm_i915_error_state_buf *m,
 	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		err_printf(m, "PWR_WELL_CTL2: %08x\n",
 			   error->power_well_driver);
-	for_each_pipe(i) {
+	for_each_pipe(dev_priv, i) {
 		err_printf(m, "Pipe [%d]:\n", i);
 		err_printf(m, "  Power: %s\n",
 			   error->pipe[i].power_domain_on ? "on" : "off");
@@ -13380,3 +13779,25 @@ intel_display_print_error_state(struct drm_i915_error_state_buf *m,
 		err_printf(m, "  VSYNC: %08x\n", error->transcoder[i].vsync);
 	}
 }
+
+void intel_modeset_preclose(struct drm_device *dev, struct drm_file *file)
+{
+	struct intel_crtc *crtc;
+
+	for_each_intel_crtc(dev, crtc) {
+		struct intel_unpin_work *work;
+		unsigned long irqflags;
+
+		spin_lock_irqsave(&dev->event_lock, irqflags);
+
+		work = crtc->unpin_work;
+
+		if (work && work->event &&
+		    work->event->base.file_priv == file) {
+			kfree(work->event);
+			work->event = NULL;
+		}
+
+		spin_unlock_irqrestore(&dev->event_lock, irqflags);
+	}
+}
diff --git a/drivers/gpu/drm/i915/intel_dp.c b/drivers/gpu/drm/i915/intel_dp.c
index ee3942f0b068..f6a3fdd5589e 100644
--- a/drivers/gpu/drm/i915/intel_dp.c
+++ b/drivers/gpu/drm/i915/intel_dp.c
@@ -111,7 +111,7 @@ static struct intel_dp *intel_attached_dp(struct drm_connector *connector)
 }
 
 static void intel_dp_link_down(struct intel_dp *intel_dp);
-static bool _edp_panel_vdd_on(struct intel_dp *intel_dp);
+static bool edp_panel_vdd_on(struct intel_dp *intel_dp);
 static void edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync);
 
 int
@@ -290,32 +290,201 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 					      struct intel_dp *intel_dp,
 					      struct edp_power_seq *out);
 
+static void pps_lock(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct intel_encoder *encoder = &intel_dig_port->base;
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum intel_display_power_domain power_domain;
+
+	/*
+	 * See vlv_power_sequencer_reset() why we need
+	 * a power domain reference here.
+	 */
+	power_domain = intel_display_port_power_domain(encoder);
+	intel_display_power_get(dev_priv, power_domain);
+
+	mutex_lock(&dev_priv->pps_mutex);
+}
+
+static void pps_unlock(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct intel_encoder *encoder = &intel_dig_port->base;
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum intel_display_power_domain power_domain;
+
+	mutex_unlock(&dev_priv->pps_mutex);
+
+	power_domain = intel_display_port_power_domain(encoder);
+	intel_display_power_put(dev_priv, power_domain);
+}
+
 static enum pipe
 vlv_power_sequencer_pipe(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct drm_crtc *crtc = intel_dig_port->base.base.crtc;
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum port port = intel_dig_port->port;
-	enum pipe pipe;
+	struct intel_encoder *encoder;
+	unsigned int pipes = (1 << PIPE_A) | (1 << PIPE_B);
+	struct edp_power_seq power_seq;
+
+	lockdep_assert_held(&dev_priv->pps_mutex);
 
-	/* modeset should have pipe */
-	if (crtc)
-		return to_intel_crtc(crtc)->pipe;
+	if (intel_dp->pps_pipe != INVALID_PIPE)
+		return intel_dp->pps_pipe;
+
+	/*
+	 * We don't have power sequencer currently.
+	 * Pick one that's not used by other ports.
+	 */
+	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
+			    base.head) {
+		struct intel_dp *tmp;
+
+		if (encoder->type != INTEL_OUTPUT_EDP)
+			continue;
+
+		tmp = enc_to_intel_dp(&encoder->base);
+
+		if (tmp->pps_pipe != INVALID_PIPE)
+			pipes &= ~(1 << tmp->pps_pipe);
+	}
+
+	/*
+	 * Didn't find one. This should not happen since there
+	 * are two power sequencers and up to two eDP ports.
+	 */
+	if (WARN_ON(pipes == 0))
+		return PIPE_A;
+
+	intel_dp->pps_pipe = ffs(pipes) - 1;
+
+	DRM_DEBUG_KMS("picked pipe %c power sequencer for port %c\n",
+		      pipe_name(intel_dp->pps_pipe),
+		      port_name(intel_dig_port->port));
+
+	/* init power sequencer on this pipe and port */
+	intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
+	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
+						      &power_seq);
+
+	return intel_dp->pps_pipe;
+}
+
+typedef bool (*vlv_pipe_check)(struct drm_i915_private *dev_priv,
+			       enum pipe pipe);
+
+static bool vlv_pipe_has_pp_on(struct drm_i915_private *dev_priv,
+			       enum pipe pipe)
+{
+	return I915_READ(VLV_PIPE_PP_STATUS(pipe)) & PP_ON;
+}
+
+static bool vlv_pipe_has_vdd_on(struct drm_i915_private *dev_priv,
+				enum pipe pipe)
+{
+	return I915_READ(VLV_PIPE_PP_CONTROL(pipe)) & EDP_FORCE_VDD;
+}
+
+static bool vlv_pipe_any(struct drm_i915_private *dev_priv,
+			 enum pipe pipe)
+{
+	return true;
+}
+
+static enum pipe
+vlv_initial_pps_pipe(struct drm_i915_private *dev_priv,
+		     enum port port,
+		     vlv_pipe_check pipe_check)
+{
+	enum pipe pipe;
 
-	/* init time, try to find a pipe with this port selected */
 	for (pipe = PIPE_A; pipe <= PIPE_B; pipe++) {
 		u32 port_sel = I915_READ(VLV_PIPE_PP_ON_DELAYS(pipe)) &
 			PANEL_PORT_SELECT_MASK;
-		if (port_sel == PANEL_PORT_SELECT_DPB_VLV && port == PORT_B)
-			return pipe;
-		if (port_sel == PANEL_PORT_SELECT_DPC_VLV && port == PORT_C)
-			return pipe;
+
+		if (port_sel != PANEL_PORT_SELECT_VLV(port))
+			continue;
+
+		if (!pipe_check(dev_priv, pipe))
+			continue;
+
+		return pipe;
+	}
+
+	return INVALID_PIPE;
+}
+
+static void
+vlv_initial_power_sequencer_setup(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct edp_power_seq power_seq;
+	enum port port = intel_dig_port->port;
+
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
+	/* try to find a pipe with this port selected */
+	/* first pick one where the panel is on */
+	intel_dp->pps_pipe = vlv_initial_pps_pipe(dev_priv, port,
+						  vlv_pipe_has_pp_on);
+	/* didn't find one? pick one where vdd is on */
+	if (intel_dp->pps_pipe == INVALID_PIPE)
+		intel_dp->pps_pipe = vlv_initial_pps_pipe(dev_priv, port,
+							  vlv_pipe_has_vdd_on);
+	/* didn't find one? pick one with just the correct port */
+	if (intel_dp->pps_pipe == INVALID_PIPE)
+		intel_dp->pps_pipe = vlv_initial_pps_pipe(dev_priv, port,
+							  vlv_pipe_any);
+
+	/* didn't find one? just let vlv_power_sequencer_pipe() pick one when needed */
+	if (intel_dp->pps_pipe == INVALID_PIPE) {
+		DRM_DEBUG_KMS("no initial power sequencer for port %c\n",
+			      port_name(port));
+		return;
 	}
 
-	/* shrug */
-	return PIPE_A;
+	DRM_DEBUG_KMS("initial power sequencer for port %c: pipe %c\n",
+		      port_name(port), pipe_name(intel_dp->pps_pipe));
+
+	intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
+	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
+						      &power_seq);
+}
+
+void vlv_power_sequencer_reset(struct drm_i915_private *dev_priv)
+{
+	struct drm_device *dev = dev_priv->dev;
+	struct intel_encoder *encoder;
+
+	if (WARN_ON(!IS_VALLEYVIEW(dev)))
+		return;
+
+	/*
+	 * We can't grab pps_mutex here due to deadlock with power_domain
+	 * mutex when power_domain functions are called while holding pps_mutex.
+	 * That also means that in order to use pps_pipe the code needs to
+	 * hold both a power domain reference and pps_mutex, and the power domain
+	 * reference get/put must be done while _not_ holding pps_mutex.
+	 * pps_{lock,unlock}() do these steps in the correct order, so one
+	 * should use them always.
+	 */
+
+	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.head) {
+		struct intel_dp *intel_dp;
+
+		if (encoder->type != INTEL_OUTPUT_EDP)
+			continue;
+
+		intel_dp = enc_to_intel_dp(&encoder->base);
+		intel_dp->pps_pipe = INVALID_PIPE;
+	}
 }
 
 static u32 _pp_ctrl_reg(struct intel_dp *intel_dp)
@@ -349,12 +518,15 @@ static int edp_notify_handler(struct notifier_block *this, unsigned long code,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp_div;
 	u32 pp_ctrl_reg, pp_div_reg;
-	enum pipe pipe = vlv_power_sequencer_pipe(intel_dp);
 
 	if (!is_edp(intel_dp) || code != SYS_RESTART)
 		return 0;
 
+	pps_lock(intel_dp);
+
 	if (IS_VALLEYVIEW(dev)) {
+		enum pipe pipe = vlv_power_sequencer_pipe(intel_dp);
+
 		pp_ctrl_reg = VLV_PIPE_PP_CONTROL(pipe);
 		pp_div_reg  = VLV_PIPE_PP_DIVISOR(pipe);
 		pp_div = I915_READ(pp_div_reg);
@@ -366,6 +538,8 @@ static int edp_notify_handler(struct notifier_block *this, unsigned long code,
 		msleep(intel_dp->panel_power_cycle_delay);
 	}
 
+	pps_unlock(intel_dp);
+
 	return 0;
 }
 
@@ -374,6 +548,8 @@ static bool edp_have_panel_power(struct intel_dp *intel_dp)
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	return (I915_READ(_pp_stat_reg(intel_dp)) & PP_ON) != 0;
 }
 
@@ -381,13 +557,10 @@ static bool edp_have_panel_vdd(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct intel_encoder *intel_encoder = &intel_dig_port->base;
-	enum intel_display_power_domain power_domain;
 
-	power_domain = intel_display_port_power_domain(intel_encoder);
-	return intel_display_power_enabled(dev_priv, power_domain) &&
-	       (I915_READ(_pp_ctrl_reg(intel_dp)) & EDP_FORCE_VDD) != 0;
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
+	return I915_READ(_pp_ctrl_reg(intel_dp)) & EDP_FORCE_VDD;
 }
 
 static void
@@ -535,7 +708,15 @@ intel_dp_aux_ch(struct intel_dp *intel_dp,
 	bool has_aux_irq = HAS_AUX_IRQ(dev);
 	bool vdd;
 
-	vdd = _edp_panel_vdd_on(intel_dp);
+	pps_lock(intel_dp);
+
+	/*
+	 * We will be called with VDD already enabled for dpcd/edid/oui reads.
+	 * In such cases we want to leave VDD enabled and it's up to upper layers
+	 * to turn it off. But for eg. i2c-dev access we need to turn it on/off
+	 * ourselves.
+	 */
+	vdd = edp_panel_vdd_on(intel_dp);
 
 	/* dp aux is extremely sensitive to irq latency, hence request the
 	 * lowest possible wakeup latency and so prevent the cpu from going into
@@ -644,6 +825,8 @@ out:
 	if (vdd)
 		edp_panel_vdd_off(intel_dp, false);
 
+	pps_unlock(intel_dp);
+
 	return ret;
 }
 
@@ -828,20 +1011,6 @@ intel_dp_set_clock(struct intel_encoder *encoder,
 	}
 }
 
-static void
-intel_dp_set_m2_n2(struct intel_crtc *crtc, struct intel_link_m_n *m_n)
-{
-	struct drm_device *dev = crtc->base.dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum transcoder transcoder = crtc->config.cpu_transcoder;
-
-	I915_WRITE(PIPE_DATA_M2(transcoder),
-		TU_SIZE(m_n->tu) | m_n->gmch_m);
-	I915_WRITE(PIPE_DATA_N2(transcoder), m_n->gmch_n);
-	I915_WRITE(PIPE_LINK_M2(transcoder), m_n->link_m);
-	I915_WRITE(PIPE_LINK_N2(transcoder), m_n->link_n);
-}
-
 bool
 intel_dp_compute_config(struct intel_encoder *encoder,
 			struct intel_crtc_config *pipe_config)
@@ -867,6 +1036,7 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 		pipe_config->has_pch_encoder = true;
 
 	pipe_config->has_dp_encoder = true;
+	pipe_config->has_drrs = false;
 	pipe_config->has_audio = intel_dp->has_audio;
 
 	if (is_edp(intel_dp) && intel_connector->panel.fixed_mode) {
@@ -898,23 +1068,15 @@ intel_dp_compute_config(struct intel_encoder *encoder,
 			bpp = dev_priv->vbt.edp_bpp;
 		}
 
-		if (IS_BROADWELL(dev)) {
-			/* Yes, it's an ugly hack. */
-			min_lane_count = max_lane_count;
-			DRM_DEBUG_KMS("forcing lane count to max (%u) on BDW\n",
-				      min_lane_count);
-		} else if (dev_priv->vbt.edp_lanes) {
-			min_lane_count = min(dev_priv->vbt.edp_lanes,
-					     max_lane_count);
-			DRM_DEBUG_KMS("using min %u lanes per VBT\n",
-				      min_lane_count);
-		}
-
-		if (dev_priv->vbt.edp_rate) {
-			min_clock = min(dev_priv->vbt.edp_rate >> 3, max_clock);
-			DRM_DEBUG_KMS("using min %02x link bw per VBT\n",
-				      bws[min_clock]);
-		}
+		/*
+		 * Use the maximum clock and number of lanes the eDP panel
+		 * advertizes being capable of. The panels are generally
+		 * designed to support only a single clock and lane
+		 * configuration, and typically these values correspond to the
+		 * native resolution of the panel.
+		 */
+		min_lane_count = max_lane_count;
+		min_clock = max_clock;
 	}
 
 	for (; bpp >= 6*3; bpp -= 2*3) {
@@ -970,13 +1132,14 @@ found:
 
 	if (intel_connector->panel.downclock_mode != NULL &&
 		intel_dp->drrs_state.type == SEAMLESS_DRRS_SUPPORT) {
+			pipe_config->has_drrs = true;
 			intel_link_compute_m_n(bpp, lane_count,
 				intel_connector->panel.downclock_mode->clock,
 				pipe_config->port_clock,
 				&pipe_config->dp_m2_n2);
 	}
 
-	if (HAS_DDI(dev))
+	if (IS_HASWELL(dev) || IS_BROADWELL(dev))
 		hsw_dp_set_ddi_pll_sel(pipe_config, intel_dp->link_bw);
 	else
 		intel_dp_set_clock(encoder, pipe_config, intel_dp->link_bw);
@@ -1110,6 +1273,8 @@ static void wait_panel_status(struct intel_dp *intel_dp,
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 pp_stat_reg, pp_ctrl_reg;
 
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	pp_stat_reg = _pp_stat_reg(intel_dp);
 	pp_ctrl_reg = _pp_ctrl_reg(intel_dp);
 
@@ -1173,13 +1338,20 @@ static  u32 ironlake_get_pp_control(struct intel_dp *intel_dp)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 control;
 
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	control = I915_READ(_pp_ctrl_reg(intel_dp));
 	control &= ~PANEL_UNLOCK_MASK;
 	control |= PANEL_UNLOCK_REGS;
 	return control;
 }
 
-static bool _edp_panel_vdd_on(struct intel_dp *intel_dp)
+/*
+ * Must be paired with edp_panel_vdd_off().
+ * Must hold pps_mutex around the whole on/off sequence.
+ * Can be nested with intel_edp_panel_vdd_{on,off}() calls.
+ */
+static bool edp_panel_vdd_on(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
@@ -1190,6 +1362,8 @@ static bool _edp_panel_vdd_on(struct intel_dp *intel_dp)
 	u32 pp_stat_reg, pp_ctrl_reg;
 	bool need_to_disable = !intel_dp->want_panel_vdd;
 
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	if (!is_edp(intel_dp))
 		return false;
 
@@ -1227,62 +1401,76 @@ static bool _edp_panel_vdd_on(struct intel_dp *intel_dp)
 	return need_to_disable;
 }
 
+/*
+ * Must be paired with intel_edp_panel_vdd_off() or
+ * intel_edp_panel_off().
+ * Nested calls to these functions are not allowed since
+ * we drop the lock. Caller must use some higher level
+ * locking to prevent nested calls from other threads.
+ */
 void intel_edp_panel_vdd_on(struct intel_dp *intel_dp)
 {
-	if (is_edp(intel_dp)) {
-		bool vdd = _edp_panel_vdd_on(intel_dp);
+	bool vdd;
 
-		WARN(!vdd, "eDP VDD already requested on\n");
-	}
+	if (!is_edp(intel_dp))
+		return;
+
+	pps_lock(intel_dp);
+	vdd = edp_panel_vdd_on(intel_dp);
+	pps_unlock(intel_dp);
+
+	WARN(!vdd, "eDP VDD already requested on\n");
 }
 
 static void edp_panel_vdd_off_sync(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_digital_port *intel_dig_port =
+		dp_to_dig_port(intel_dp);
+	struct intel_encoder *intel_encoder = &intel_dig_port->base;
+	enum intel_display_power_domain power_domain;
 	u32 pp;
 	u32 pp_stat_reg, pp_ctrl_reg;
 
-	WARN_ON(!drm_modeset_is_locked(&dev->mode_config.connection_mutex));
+	lockdep_assert_held(&dev_priv->pps_mutex);
 
-	if (!intel_dp->want_panel_vdd && edp_have_panel_vdd(intel_dp)) {
-		struct intel_digital_port *intel_dig_port =
-						dp_to_dig_port(intel_dp);
-		struct intel_encoder *intel_encoder = &intel_dig_port->base;
-		enum intel_display_power_domain power_domain;
+	WARN_ON(intel_dp->want_panel_vdd);
 
-		DRM_DEBUG_KMS("Turning eDP VDD off\n");
+	if (!edp_have_panel_vdd(intel_dp))
+		return;
 
-		pp = ironlake_get_pp_control(intel_dp);
-		pp &= ~EDP_FORCE_VDD;
+	DRM_DEBUG_KMS("Turning eDP VDD off\n");
 
-		pp_ctrl_reg = _pp_ctrl_reg(intel_dp);
-		pp_stat_reg = _pp_stat_reg(intel_dp);
+	pp = ironlake_get_pp_control(intel_dp);
+	pp &= ~EDP_FORCE_VDD;
 
-		I915_WRITE(pp_ctrl_reg, pp);
-		POSTING_READ(pp_ctrl_reg);
+	pp_ctrl_reg = _pp_ctrl_reg(intel_dp);
+	pp_stat_reg = _pp_stat_reg(intel_dp);
 
-		/* Make sure sequencer is idle before allowing subsequent activity */
-		DRM_DEBUG_KMS("PP_STATUS: 0x%08x PP_CONTROL: 0x%08x\n",
-		I915_READ(pp_stat_reg), I915_READ(pp_ctrl_reg));
+	I915_WRITE(pp_ctrl_reg, pp);
+	POSTING_READ(pp_ctrl_reg);
 
-		if ((pp & POWER_TARGET_ON) == 0)
-			intel_dp->last_power_cycle = jiffies;
+	/* Make sure sequencer is idle before allowing subsequent activity */
+	DRM_DEBUG_KMS("PP_STATUS: 0x%08x PP_CONTROL: 0x%08x\n",
+	I915_READ(pp_stat_reg), I915_READ(pp_ctrl_reg));
 
-		power_domain = intel_display_port_power_domain(intel_encoder);
-		intel_display_power_put(dev_priv, power_domain);
-	}
+	if ((pp & POWER_TARGET_ON) == 0)
+		intel_dp->last_power_cycle = jiffies;
+
+	power_domain = intel_display_port_power_domain(intel_encoder);
+	intel_display_power_put(dev_priv, power_domain);
 }
 
 static void edp_panel_vdd_work(struct work_struct *__work)
 {
 	struct intel_dp *intel_dp = container_of(to_delayed_work(__work),
 						 struct intel_dp, panel_vdd_work);
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 
-	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
-	edp_panel_vdd_off_sync(intel_dp);
-	drm_modeset_unlock(&dev->mode_config.connection_mutex);
+	pps_lock(intel_dp);
+	if (!intel_dp->want_panel_vdd)
+		edp_panel_vdd_off_sync(intel_dp);
+	pps_unlock(intel_dp);
 }
 
 static void edp_panel_vdd_schedule_off(struct intel_dp *intel_dp)
@@ -1298,8 +1486,18 @@ static void edp_panel_vdd_schedule_off(struct intel_dp *intel_dp)
 	schedule_delayed_work(&intel_dp->panel_vdd_work, delay);
 }
 
+/*
+ * Must be paired with edp_panel_vdd_on().
+ * Must hold pps_mutex around the whole on/off sequence.
+ * Can be nested with intel_edp_panel_vdd_{on,off}() calls.
+ */
 static void edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync)
 {
+	struct drm_i915_private *dev_priv =
+		intel_dp_to_dev(intel_dp)->dev_private;
+
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	if (!is_edp(intel_dp))
 		return;
 
@@ -1313,6 +1511,22 @@ static void edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync)
 		edp_panel_vdd_schedule_off(intel_dp);
 }
 
+/*
+ * Must be paired with intel_edp_panel_vdd_on().
+ * Nested calls to these functions are not allowed since
+ * we drop the lock. Caller must use some higher level
+ * locking to prevent nested calls from other threads.
+ */
+static void intel_edp_panel_vdd_off(struct intel_dp *intel_dp, bool sync)
+{
+	if (!is_edp(intel_dp))
+		return;
+
+	pps_lock(intel_dp);
+	edp_panel_vdd_off(intel_dp, sync);
+	pps_unlock(intel_dp);
+}
+
 void intel_edp_panel_on(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
@@ -1325,9 +1539,11 @@ void intel_edp_panel_on(struct intel_dp *intel_dp)
 
 	DRM_DEBUG_KMS("Turn eDP power on\n");
 
+	pps_lock(intel_dp);
+
 	if (edp_have_panel_power(intel_dp)) {
 		DRM_DEBUG_KMS("eDP power already on\n");
-		return;
+		goto out;
 	}
 
 	wait_panel_power_cycle(intel_dp);
@@ -1356,6 +1572,9 @@ void intel_edp_panel_on(struct intel_dp *intel_dp)
 		I915_WRITE(pp_ctrl_reg, pp);
 		POSTING_READ(pp_ctrl_reg);
 	}
+
+ out:
+	pps_unlock(intel_dp);
 }
 
 void intel_edp_panel_off(struct intel_dp *intel_dp)
@@ -1373,6 +1592,8 @@ void intel_edp_panel_off(struct intel_dp *intel_dp)
 
 	DRM_DEBUG_KMS("Turn eDP power off\n");
 
+	pps_lock(intel_dp);
+
 	WARN(!intel_dp->want_panel_vdd, "Need VDD to turn off panel\n");
 
 	pp = ironlake_get_pp_control(intel_dp);
@@ -1394,9 +1615,12 @@ void intel_edp_panel_off(struct intel_dp *intel_dp)
 	/* We got a reference when we enabled the VDD. */
 	power_domain = intel_display_port_power_domain(intel_encoder);
 	intel_display_power_put(dev_priv, power_domain);
+
+	pps_unlock(intel_dp);
 }
 
-void intel_edp_backlight_on(struct intel_dp *intel_dp)
+/* Enable backlight in the panel power control. */
+static void _intel_edp_backlight_on(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
@@ -1404,13 +1628,6 @@ void intel_edp_backlight_on(struct intel_dp *intel_dp)
 	u32 pp;
 	u32 pp_ctrl_reg;
 
-	if (!is_edp(intel_dp))
-		return;
-
-	DRM_DEBUG_KMS("\n");
-
-	intel_panel_enable_backlight(intel_dp->attached_connector);
-
 	/*
 	 * If we enable the backlight right away following a panel power
 	 * on, we may see slight flicker as the panel syncs with the eDP
@@ -1418,6 +1635,9 @@ void intel_edp_backlight_on(struct intel_dp *intel_dp)
 	 * allowing it to appear.
 	 */
 	wait_backlight_on(intel_dp);
+
+	pps_lock(intel_dp);
+
 	pp = ironlake_get_pp_control(intel_dp);
 	pp |= EDP_BLC_ENABLE;
 
@@ -1425,9 +1645,24 @@ void intel_edp_backlight_on(struct intel_dp *intel_dp)
 
 	I915_WRITE(pp_ctrl_reg, pp);
 	POSTING_READ(pp_ctrl_reg);
+
+	pps_unlock(intel_dp);
 }
 
-void intel_edp_backlight_off(struct intel_dp *intel_dp)
+/* Enable backlight PWM and backlight PP control. */
+void intel_edp_backlight_on(struct intel_dp *intel_dp)
+{
+	if (!is_edp(intel_dp))
+		return;
+
+	DRM_DEBUG_KMS("\n");
+
+	intel_panel_enable_backlight(intel_dp->attached_connector);
+	_intel_edp_backlight_on(intel_dp);
+}
+
+/* Disable backlight in the panel power control. */
+static void _intel_edp_backlight_off(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -1437,7 +1672,8 @@ void intel_edp_backlight_off(struct intel_dp *intel_dp)
 	if (!is_edp(intel_dp))
 		return;
 
-	DRM_DEBUG_KMS("\n");
+	pps_lock(intel_dp);
+
 	pp = ironlake_get_pp_control(intel_dp);
 	pp &= ~EDP_BLC_ENABLE;
 
@@ -1445,13 +1681,51 @@ void intel_edp_backlight_off(struct intel_dp *intel_dp)
 
 	I915_WRITE(pp_ctrl_reg, pp);
 	POSTING_READ(pp_ctrl_reg);
-	intel_dp->last_backlight_off = jiffies;
 
+	pps_unlock(intel_dp);
+
+	intel_dp->last_backlight_off = jiffies;
 	edp_wait_backlight_off(intel_dp);
+}
+
+/* Disable backlight PP control and backlight PWM. */
+void intel_edp_backlight_off(struct intel_dp *intel_dp)
+{
+	if (!is_edp(intel_dp))
+		return;
 
+	DRM_DEBUG_KMS("\n");
+
+	_intel_edp_backlight_off(intel_dp);
 	intel_panel_disable_backlight(intel_dp->attached_connector);
 }
 
+/*
+ * Hook for controlling the panel power control backlight through the bl_power
+ * sysfs attribute. Take care to handle multiple calls.
+ */
+static void intel_edp_backlight_power(struct intel_connector *connector,
+				      bool enable)
+{
+	struct intel_dp *intel_dp = intel_attached_dp(&connector->base);
+	bool is_enabled;
+
+	pps_lock(intel_dp);
+	is_enabled = ironlake_get_pp_control(intel_dp) & EDP_BLC_ENABLE;
+	pps_unlock(intel_dp);
+
+	if (is_enabled == enable)
+		return;
+
+	DRM_DEBUG_KMS("panel power control backlight %s\n",
+		      enable ? "enable" : "disable");
+
+	if (enable)
+		_intel_edp_backlight_on(intel_dp);
+	else
+		_intel_edp_backlight_off(intel_dp);
+}
+
 static void ironlake_edp_pll_on(struct intel_dp *intel_dp)
 {
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
@@ -1515,8 +1789,6 @@ void intel_dp_sink_dpms(struct intel_dp *intel_dp, int mode)
 	if (mode != DRM_MODE_DPMS_ON) {
 		ret = drm_dp_dpcd_writeb(&intel_dp->aux, DP_SET_POWER,
 					 DP_SET_POWER_D3);
-		if (ret != 1)
-			DRM_DEBUG_DRIVER("failed to write sink power state\n");
 	} else {
 		/*
 		 * When turning on, we need to retry for 1ms to give the sink
@@ -1530,6 +1802,10 @@ void intel_dp_sink_dpms(struct intel_dp *intel_dp, int mode)
 			msleep(1);
 		}
 	}
+
+	if (ret != 1)
+		DRM_DEBUG_KMS("failed to %s sink power state\n",
+			      mode == DRM_MODE_DPMS_ON ? "enable" : "disable");
 }
 
 static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
@@ -1576,7 +1852,7 @@ static bool intel_dp_get_hw_state(struct intel_encoder *encoder,
 			return true;
 		}
 
-		for_each_pipe(i) {
+		for_each_pipe(dev_priv, i) {
 			trans_dp = I915_READ(TRANS_DP_CTL(i));
 			if ((trans_dp & TRANS_DP_PORT_SEL_MASK) == trans_sel) {
 				*pipe = i;
@@ -1631,6 +1907,10 @@ static void intel_dp_get_config(struct intel_encoder *encoder,
 
 	pipe_config->adjusted_mode.flags |= flags;
 
+	if (!HAS_PCH_SPLIT(dev) && !IS_VALLEYVIEW(dev) &&
+	    tmp & DP_COLOR_RANGE_16_235)
+		pipe_config->limited_color_range = true;
+
 	pipe_config->has_dp_encoder = true;
 
 	intel_dp_get_m_n(crtc, pipe_config);
@@ -2032,7 +2312,6 @@ void intel_edp_psr_init(struct drm_device *dev)
 static void intel_disable_dp(struct intel_encoder *encoder)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
-	enum port port = dp_to_dig_port(intel_dp)->port;
 	struct drm_device *dev = encoder->base.dev;
 
 	/* Make sure the panel is off before trying to change the mode. But also
@@ -2042,21 +2321,19 @@ static void intel_disable_dp(struct intel_encoder *encoder)
 	intel_dp_sink_dpms(intel_dp, DRM_MODE_DPMS_OFF);
 	intel_edp_panel_off(intel_dp);
 
-	/* cpu edp my only be disable _after_ the cpu pipe/plane is disabled. */
-	if (!(port == PORT_A || IS_VALLEYVIEW(dev)))
+	/* disable the port before the pipe on g4x */
+	if (INTEL_INFO(dev)->gen < 5)
 		intel_dp_link_down(intel_dp);
 }
 
-static void g4x_post_disable_dp(struct intel_encoder *encoder)
+static void ilk_post_disable_dp(struct intel_encoder *encoder)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
 	enum port port = dp_to_dig_port(intel_dp)->port;
 
-	if (port != PORT_A)
-		return;
-
 	intel_dp_link_down(intel_dp);
-	ironlake_edp_pll_off(intel_dp);
+	if (port == PORT_A)
+		ironlake_edp_pll_off(intel_dp);
 }
 
 static void vlv_post_disable_dp(struct intel_encoder *encoder)
@@ -2102,6 +2379,104 @@ static void chv_post_disable_dp(struct intel_encoder *encoder)
 	mutex_unlock(&dev_priv->dpio_lock);
 }
 
+static void
+_intel_dp_set_link_train(struct intel_dp *intel_dp,
+			 uint32_t *DP,
+			 uint8_t dp_train_pat)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct drm_device *dev = intel_dig_port->base.base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	enum port port = intel_dig_port->port;
+
+	if (HAS_DDI(dev)) {
+		uint32_t temp = I915_READ(DP_TP_CTL(port));
+
+		if (dp_train_pat & DP_LINK_SCRAMBLING_DISABLE)
+			temp |= DP_TP_CTL_SCRAMBLE_DISABLE;
+		else
+			temp &= ~DP_TP_CTL_SCRAMBLE_DISABLE;
+
+		temp &= ~DP_TP_CTL_LINK_TRAIN_MASK;
+		switch (dp_train_pat & DP_TRAINING_PATTERN_MASK) {
+		case DP_TRAINING_PATTERN_DISABLE:
+			temp |= DP_TP_CTL_LINK_TRAIN_NORMAL;
+
+			break;
+		case DP_TRAINING_PATTERN_1:
+			temp |= DP_TP_CTL_LINK_TRAIN_PAT1;
+			break;
+		case DP_TRAINING_PATTERN_2:
+			temp |= DP_TP_CTL_LINK_TRAIN_PAT2;
+			break;
+		case DP_TRAINING_PATTERN_3:
+			temp |= DP_TP_CTL_LINK_TRAIN_PAT3;
+			break;
+		}
+		I915_WRITE(DP_TP_CTL(port), temp);
+
+	} else if (HAS_PCH_CPT(dev) && (IS_GEN7(dev) || port != PORT_A)) {
+		*DP &= ~DP_LINK_TRAIN_MASK_CPT;
+
+		switch (dp_train_pat & DP_TRAINING_PATTERN_MASK) {
+		case DP_TRAINING_PATTERN_DISABLE:
+			*DP |= DP_LINK_TRAIN_OFF_CPT;
+			break;
+		case DP_TRAINING_PATTERN_1:
+			*DP |= DP_LINK_TRAIN_PAT_1_CPT;
+			break;
+		case DP_TRAINING_PATTERN_2:
+			*DP |= DP_LINK_TRAIN_PAT_2_CPT;
+			break;
+		case DP_TRAINING_PATTERN_3:
+			DRM_ERROR("DP training pattern 3 not supported\n");
+			*DP |= DP_LINK_TRAIN_PAT_2_CPT;
+			break;
+		}
+
+	} else {
+		if (IS_CHERRYVIEW(dev))
+			*DP &= ~DP_LINK_TRAIN_MASK_CHV;
+		else
+			*DP &= ~DP_LINK_TRAIN_MASK;
+
+		switch (dp_train_pat & DP_TRAINING_PATTERN_MASK) {
+		case DP_TRAINING_PATTERN_DISABLE:
+			*DP |= DP_LINK_TRAIN_OFF;
+			break;
+		case DP_TRAINING_PATTERN_1:
+			*DP |= DP_LINK_TRAIN_PAT_1;
+			break;
+		case DP_TRAINING_PATTERN_2:
+			*DP |= DP_LINK_TRAIN_PAT_2;
+			break;
+		case DP_TRAINING_PATTERN_3:
+			if (IS_CHERRYVIEW(dev)) {
+				*DP |= DP_LINK_TRAIN_PAT_3_CHV;
+			} else {
+				DRM_ERROR("DP training pattern 3 not supported\n");
+				*DP |= DP_LINK_TRAIN_PAT_2;
+			}
+			break;
+		}
+	}
+}
+
+static void intel_dp_enable_port(struct intel_dp *intel_dp)
+{
+	struct drm_device *dev = intel_dp_to_dev(intel_dp);
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	intel_dp->DP |= DP_PORT_EN;
+
+	/* enable with pattern 1 (as per spec) */
+	_intel_dp_set_link_train(intel_dp, &intel_dp->DP,
+				 DP_TRAINING_PATTERN_1);
+
+	I915_WRITE(intel_dp->output_reg, intel_dp->DP);
+	POSTING_READ(intel_dp->output_reg);
+}
+
 static void intel_enable_dp(struct intel_encoder *encoder)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
@@ -2112,11 +2487,12 @@ static void intel_enable_dp(struct intel_encoder *encoder)
 	if (WARN_ON(dp_reg & DP_PORT_EN))
 		return;
 
+	intel_dp_enable_port(intel_dp);
 	intel_edp_panel_vdd_on(intel_dp);
+	intel_edp_panel_on(intel_dp);
+	intel_edp_panel_vdd_off(intel_dp, true);
 	intel_dp_sink_dpms(intel_dp, DRM_MODE_DPMS_ON);
 	intel_dp_start_link_train(intel_dp);
-	intel_edp_panel_on(intel_dp);
-	edp_panel_vdd_off(intel_dp, true);
 	intel_dp_complete_link_train(intel_dp);
 	intel_dp_stop_link_train(intel_dp);
 }
@@ -2150,6 +2526,78 @@ static void g4x_pre_enable_dp(struct intel_encoder *encoder)
 	}
 }
 
+static void vlv_steal_power_sequencer(struct drm_device *dev,
+				      enum pipe pipe)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_encoder *encoder;
+
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
+	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
+			    base.head) {
+		struct intel_dp *intel_dp;
+		enum port port;
+
+		if (encoder->type != INTEL_OUTPUT_EDP)
+			continue;
+
+		intel_dp = enc_to_intel_dp(&encoder->base);
+		port = dp_to_dig_port(intel_dp)->port;
+
+		if (intel_dp->pps_pipe != pipe)
+			continue;
+
+		DRM_DEBUG_KMS("stealing pipe %c power sequencer from port %c\n",
+			      pipe_name(pipe), port_name(port));
+
+		/* make sure vdd is off before we steal it */
+		edp_panel_vdd_off_sync(intel_dp);
+
+		intel_dp->pps_pipe = INVALID_PIPE;
+	}
+}
+
+static void vlv_init_panel_power_sequencer(struct intel_dp *intel_dp)
+{
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	struct intel_encoder *encoder = &intel_dig_port->base;
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_crtc *crtc = to_intel_crtc(encoder->base.crtc);
+	struct edp_power_seq power_seq;
+
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
+	if (intel_dp->pps_pipe == crtc->pipe)
+		return;
+
+	/*
+	 * If another power sequencer was being used on this
+	 * port previously make sure to turn off vdd there while
+	 * we still have control of it.
+	 */
+	if (intel_dp->pps_pipe != INVALID_PIPE)
+		edp_panel_vdd_off_sync(intel_dp);
+
+	/*
+	 * We may be stealing the power
+	 * sequencer from another port.
+	 */
+	vlv_steal_power_sequencer(dev, crtc->pipe);
+
+	/* now it's all ours */
+	intel_dp->pps_pipe = crtc->pipe;
+
+	DRM_DEBUG_KMS("initializing pipe %c power sequencer for port %c\n",
+		      pipe_name(intel_dp->pps_pipe), port_name(intel_dig_port->port));
+
+	/* init power sequencer on this pipe and port */
+	intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
+	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
+						      &power_seq);
+}
+
 static void vlv_pre_enable_dp(struct intel_encoder *encoder)
 {
 	struct intel_dp *intel_dp = enc_to_intel_dp(&encoder->base);
@@ -2159,7 +2607,6 @@ static void vlv_pre_enable_dp(struct intel_encoder *encoder)
 	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
 	enum dpio_channel port = vlv_dport_to_channel(dport);
 	int pipe = intel_crtc->pipe;
-	struct edp_power_seq power_seq;
 	u32 val;
 
 	mutex_lock(&dev_priv->dpio_lock);
@@ -2178,10 +2625,9 @@ static void vlv_pre_enable_dp(struct intel_encoder *encoder)
 	mutex_unlock(&dev_priv->dpio_lock);
 
 	if (is_edp(intel_dp)) {
-		/* init power sequencer on this pipe and port */
-		intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
-		intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
-							      &power_seq);
+		pps_lock(intel_dp);
+		vlv_init_panel_power_sequencer(intel_dp);
+		pps_unlock(intel_dp);
 	}
 
 	intel_enable_dp(encoder);
@@ -2225,7 +2671,6 @@ static void chv_pre_enable_dp(struct intel_encoder *encoder)
 	struct intel_digital_port *dport = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct edp_power_seq power_seq;
 	struct intel_crtc *intel_crtc =
 		to_intel_crtc(encoder->base.crtc);
 	enum dpio_channel ch = vlv_dport_to_channel(dport);
@@ -2271,10 +2716,9 @@ static void chv_pre_enable_dp(struct intel_encoder *encoder)
 	mutex_unlock(&dev_priv->dpio_lock);
 
 	if (is_edp(intel_dp)) {
-		/* init power sequencer on this pipe and port */
-		intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
-		intel_dp_init_panel_power_sequencer_registers(dev, intel_dp,
-							      &power_seq);
+		pps_lock(intel_dp);
+		vlv_init_panel_power_sequencer(intel_dp);
+		pps_unlock(intel_dp);
 	}
 
 	intel_enable_dp(encoder);
@@ -2293,6 +2737,8 @@ static void chv_dp_pre_pll_enable(struct intel_encoder *encoder)
 	enum pipe pipe = intel_crtc->pipe;
 	u32 val;
 
+	intel_dp_prepare(encoder);
+
 	mutex_lock(&dev_priv->dpio_lock);
 
 	/* program left/right clock distribution */
@@ -2391,13 +2837,13 @@ intel_dp_voltage_max(struct intel_dp *intel_dp)
 	enum port port = dp_to_dig_port(intel_dp)->port;
 
 	if (IS_VALLEYVIEW(dev))
-		return DP_TRAIN_VOLTAGE_SWING_1200;
+		return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 	else if (IS_GEN7(dev) && port == PORT_A)
-		return DP_TRAIN_VOLTAGE_SWING_800;
+		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
 	else if (HAS_PCH_CPT(dev) && port != PORT_A)
-		return DP_TRAIN_VOLTAGE_SWING_1200;
+		return DP_TRAIN_VOLTAGE_SWING_LEVEL_3;
 	else
-		return DP_TRAIN_VOLTAGE_SWING_800;
+		return DP_TRAIN_VOLTAGE_SWING_LEVEL_2;
 }
 
 static uint8_t
@@ -2408,49 +2854,49 @@ intel_dp_pre_emphasis_max(struct intel_dp *intel_dp, uint8_t voltage_swing)
 
 	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
-			return DP_TRAIN_PRE_EMPHASIS_9_5;
-		case DP_TRAIN_VOLTAGE_SWING_600:
-			return DP_TRAIN_PRE_EMPHASIS_6;
-		case DP_TRAIN_VOLTAGE_SWING_800:
-			return DP_TRAIN_PRE_EMPHASIS_3_5;
-		case DP_TRAIN_VOLTAGE_SWING_1200:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
+			return DP_TRAIN_PRE_EMPH_LEVEL_3;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
+			return DP_TRAIN_PRE_EMPH_LEVEL_2;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
+			return DP_TRAIN_PRE_EMPH_LEVEL_1;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_3:
 		default:
-			return DP_TRAIN_PRE_EMPHASIS_0;
+			return DP_TRAIN_PRE_EMPH_LEVEL_0;
 		}
 	} else if (IS_VALLEYVIEW(dev)) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
-			return DP_TRAIN_PRE_EMPHASIS_9_5;
-		case DP_TRAIN_VOLTAGE_SWING_600:
-			return DP_TRAIN_PRE_EMPHASIS_6;
-		case DP_TRAIN_VOLTAGE_SWING_800:
-			return DP_TRAIN_PRE_EMPHASIS_3_5;
-		case DP_TRAIN_VOLTAGE_SWING_1200:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
+			return DP_TRAIN_PRE_EMPH_LEVEL_3;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
+			return DP_TRAIN_PRE_EMPH_LEVEL_2;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
+			return DP_TRAIN_PRE_EMPH_LEVEL_1;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_3:
 		default:
-			return DP_TRAIN_PRE_EMPHASIS_0;
+			return DP_TRAIN_PRE_EMPH_LEVEL_0;
 		}
 	} else if (IS_GEN7(dev) && port == PORT_A) {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
-			return DP_TRAIN_PRE_EMPHASIS_6;
-		case DP_TRAIN_VOLTAGE_SWING_600:
-		case DP_TRAIN_VOLTAGE_SWING_800:
-			return DP_TRAIN_PRE_EMPHASIS_3_5;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
+			return DP_TRAIN_PRE_EMPH_LEVEL_2;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
+			return DP_TRAIN_PRE_EMPH_LEVEL_1;
 		default:
-			return DP_TRAIN_PRE_EMPHASIS_0;
+			return DP_TRAIN_PRE_EMPH_LEVEL_0;
 		}
 	} else {
 		switch (voltage_swing & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
-			return DP_TRAIN_PRE_EMPHASIS_6;
-		case DP_TRAIN_VOLTAGE_SWING_600:
-			return DP_TRAIN_PRE_EMPHASIS_6;
-		case DP_TRAIN_VOLTAGE_SWING_800:
-			return DP_TRAIN_PRE_EMPHASIS_3_5;
-		case DP_TRAIN_VOLTAGE_SWING_1200:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
+			return DP_TRAIN_PRE_EMPH_LEVEL_2;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
+			return DP_TRAIN_PRE_EMPH_LEVEL_2;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
+			return DP_TRAIN_PRE_EMPH_LEVEL_1;
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_3:
 		default:
-			return DP_TRAIN_PRE_EMPHASIS_0;
+			return DP_TRAIN_PRE_EMPH_LEVEL_0;
 		}
 	}
 }
@@ -2469,22 +2915,22 @@ static uint32_t intel_vlv_signal_levels(struct intel_dp *intel_dp)
 	int pipe = intel_crtc->pipe;
 
 	switch (train_set & DP_TRAIN_PRE_EMPHASIS_MASK) {
-	case DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_PRE_EMPH_LEVEL_0:
 		preemph_reg_value = 0x0004000;
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			demph_reg_value = 0x2B405555;
 			uniqtranscale_reg_value = 0x552AB83A;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_600:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
 			demph_reg_value = 0x2B404040;
 			uniqtranscale_reg_value = 0x5548B83A;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_800:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
 			demph_reg_value = 0x2B245555;
 			uniqtranscale_reg_value = 0x5560B83A;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_1200:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_3:
 			demph_reg_value = 0x2B405555;
 			uniqtranscale_reg_value = 0x5598DA3A;
 			break;
@@ -2492,18 +2938,18 @@ static uint32_t intel_vlv_signal_levels(struct intel_dp *intel_dp)
 			return 0;
 		}
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_PRE_EMPH_LEVEL_1:
 		preemph_reg_value = 0x0002000;
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			demph_reg_value = 0x2B404040;
 			uniqtranscale_reg_value = 0x5552B83A;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_600:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
 			demph_reg_value = 0x2B404848;
 			uniqtranscale_reg_value = 0x5580B83A;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_800:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
 			demph_reg_value = 0x2B404040;
 			uniqtranscale_reg_value = 0x55ADDA3A;
 			break;
@@ -2511,14 +2957,14 @@ static uint32_t intel_vlv_signal_levels(struct intel_dp *intel_dp)
 			return 0;
 		}
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_6:
+	case DP_TRAIN_PRE_EMPH_LEVEL_2:
 		preemph_reg_value = 0x0000000;
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			demph_reg_value = 0x2B305555;
 			uniqtranscale_reg_value = 0x5570B83A;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_600:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
 			demph_reg_value = 0x2B2B4040;
 			uniqtranscale_reg_value = 0x55ADDA3A;
 			break;
@@ -2526,10 +2972,10 @@ static uint32_t intel_vlv_signal_levels(struct intel_dp *intel_dp)
 			return 0;
 		}
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_9_5:
+	case DP_TRAIN_PRE_EMPH_LEVEL_3:
 		preemph_reg_value = 0x0006000;
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			demph_reg_value = 0x1B405555;
 			uniqtranscale_reg_value = 0x55ADDA3A;
 			break;
@@ -2568,21 +3014,21 @@ static uint32_t intel_chv_signal_levels(struct intel_dp *intel_dp)
 	int i;
 
 	switch (train_set & DP_TRAIN_PRE_EMPHASIS_MASK) {
-	case DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_PRE_EMPH_LEVEL_0:
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			deemph_reg_value = 128;
 			margin_reg_value = 52;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_600:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
 			deemph_reg_value = 128;
 			margin_reg_value = 77;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_800:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
 			deemph_reg_value = 128;
 			margin_reg_value = 102;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_1200:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_3:
 			deemph_reg_value = 128;
 			margin_reg_value = 154;
 			/* FIXME extra to set for 1200 */
@@ -2591,17 +3037,17 @@ static uint32_t intel_chv_signal_levels(struct intel_dp *intel_dp)
 			return 0;
 		}
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_PRE_EMPH_LEVEL_1:
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			deemph_reg_value = 85;
 			margin_reg_value = 78;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_600:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
 			deemph_reg_value = 85;
 			margin_reg_value = 116;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_800:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
 			deemph_reg_value = 85;
 			margin_reg_value = 154;
 			break;
@@ -2609,13 +3055,13 @@ static uint32_t intel_chv_signal_levels(struct intel_dp *intel_dp)
 			return 0;
 		}
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_6:
+	case DP_TRAIN_PRE_EMPH_LEVEL_2:
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			deemph_reg_value = 64;
 			margin_reg_value = 104;
 			break;
-		case DP_TRAIN_VOLTAGE_SWING_600:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
 			deemph_reg_value = 64;
 			margin_reg_value = 154;
 			break;
@@ -2623,9 +3069,9 @@ static uint32_t intel_chv_signal_levels(struct intel_dp *intel_dp)
 			return 0;
 		}
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_9_5:
+	case DP_TRAIN_PRE_EMPH_LEVEL_3:
 		switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-		case DP_TRAIN_VOLTAGE_SWING_400:
+		case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 			deemph_reg_value = 43;
 			margin_reg_value = 154;
 			break;
@@ -2659,8 +3105,8 @@ static uint32_t intel_chv_signal_levels(struct intel_dp *intel_dp)
 	/* Program swing margin */
 	for (i = 0; i < 4; i++) {
 		val = vlv_dpio_read(dev_priv, pipe, CHV_TX_DW2(ch, i));
-		val &= ~DPIO_SWING_MARGIN_MASK;
-		val |= margin_reg_value << DPIO_SWING_MARGIN_SHIFT;
+		val &= ~DPIO_SWING_MARGIN000_MASK;
+		val |= margin_reg_value << DPIO_SWING_MARGIN000_SHIFT;
 		vlv_dpio_write(dev_priv, pipe, CHV_TX_DW2(ch, i), val);
 	}
 
@@ -2672,9 +3118,9 @@ static uint32_t intel_chv_signal_levels(struct intel_dp *intel_dp)
 	}
 
 	if (((train_set & DP_TRAIN_PRE_EMPHASIS_MASK)
-			== DP_TRAIN_PRE_EMPHASIS_0) &&
+			== DP_TRAIN_PRE_EMPH_LEVEL_0) &&
 		((train_set & DP_TRAIN_VOLTAGE_SWING_MASK)
-			== DP_TRAIN_VOLTAGE_SWING_1200)) {
+			== DP_TRAIN_VOLTAGE_SWING_LEVEL_3)) {
 
 		/*
 		 * The document said it needs to set bit 27 for ch0 and bit 26
@@ -2753,32 +3199,32 @@ intel_gen4_signal_levels(uint8_t train_set)
 	uint32_t	signal_levels = 0;
 
 	switch (train_set & DP_TRAIN_VOLTAGE_SWING_MASK) {
-	case DP_TRAIN_VOLTAGE_SWING_400:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0:
 	default:
 		signal_levels |= DP_VOLTAGE_0_4;
 		break;
-	case DP_TRAIN_VOLTAGE_SWING_600:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1:
 		signal_levels |= DP_VOLTAGE_0_6;
 		break;
-	case DP_TRAIN_VOLTAGE_SWING_800:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_2:
 		signal_levels |= DP_VOLTAGE_0_8;
 		break;
-	case DP_TRAIN_VOLTAGE_SWING_1200:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_3:
 		signal_levels |= DP_VOLTAGE_1_2;
 		break;
 	}
 	switch (train_set & DP_TRAIN_PRE_EMPHASIS_MASK) {
-	case DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_PRE_EMPH_LEVEL_0:
 	default:
 		signal_levels |= DP_PRE_EMPHASIS_0;
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_PRE_EMPH_LEVEL_1:
 		signal_levels |= DP_PRE_EMPHASIS_3_5;
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_6:
+	case DP_TRAIN_PRE_EMPH_LEVEL_2:
 		signal_levels |= DP_PRE_EMPHASIS_6;
 		break;
-	case DP_TRAIN_PRE_EMPHASIS_9_5:
+	case DP_TRAIN_PRE_EMPH_LEVEL_3:
 		signal_levels |= DP_PRE_EMPHASIS_9_5;
 		break;
 	}
@@ -2792,19 +3238,19 @@ intel_gen6_edp_signal_levels(uint8_t train_set)
 	int signal_levels = train_set & (DP_TRAIN_VOLTAGE_SWING_MASK |
 					 DP_TRAIN_PRE_EMPHASIS_MASK);
 	switch (signal_levels) {
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_0:
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_0:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_0:
 		return EDP_LINK_TRAIN_400_600MV_0DB_SNB_B;
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_1:
 		return EDP_LINK_TRAIN_400MV_3_5DB_SNB_B;
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_6:
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_6:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_2:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_2:
 		return EDP_LINK_TRAIN_400_600MV_6DB_SNB_B;
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_3_5:
-	case DP_TRAIN_VOLTAGE_SWING_800 | DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_1:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_2 | DP_TRAIN_PRE_EMPH_LEVEL_1:
 		return EDP_LINK_TRAIN_600_800MV_3_5DB_SNB_B;
-	case DP_TRAIN_VOLTAGE_SWING_800 | DP_TRAIN_PRE_EMPHASIS_0:
-	case DP_TRAIN_VOLTAGE_SWING_1200 | DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_2 | DP_TRAIN_PRE_EMPH_LEVEL_0:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_3 | DP_TRAIN_PRE_EMPH_LEVEL_0:
 		return EDP_LINK_TRAIN_800_1200MV_0DB_SNB_B;
 	default:
 		DRM_DEBUG_KMS("Unsupported voltage swing/pre-emphasis level:"
@@ -2820,21 +3266,21 @@ intel_gen7_edp_signal_levels(uint8_t train_set)
 	int signal_levels = train_set & (DP_TRAIN_VOLTAGE_SWING_MASK |
 					 DP_TRAIN_PRE_EMPHASIS_MASK);
 	switch (signal_levels) {
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_0:
 		return EDP_LINK_TRAIN_400MV_0DB_IVB;
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_1:
 		return EDP_LINK_TRAIN_400MV_3_5DB_IVB;
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_6:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_2:
 		return EDP_LINK_TRAIN_400MV_6DB_IVB;
 
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_0:
 		return EDP_LINK_TRAIN_600MV_0DB_IVB;
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_1:
 		return EDP_LINK_TRAIN_600MV_3_5DB_IVB;
 
-	case DP_TRAIN_VOLTAGE_SWING_800 | DP_TRAIN_PRE_EMPHASIS_0:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_2 | DP_TRAIN_PRE_EMPH_LEVEL_0:
 		return EDP_LINK_TRAIN_800MV_0DB_IVB;
-	case DP_TRAIN_VOLTAGE_SWING_800 | DP_TRAIN_PRE_EMPHASIS_3_5:
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_2 | DP_TRAIN_PRE_EMPH_LEVEL_1:
 		return EDP_LINK_TRAIN_800MV_3_5DB_IVB;
 
 	default:
@@ -2851,30 +3297,30 @@ intel_hsw_signal_levels(uint8_t train_set)
 	int signal_levels = train_set & (DP_TRAIN_VOLTAGE_SWING_MASK |
 					 DP_TRAIN_PRE_EMPHASIS_MASK);
 	switch (signal_levels) {
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_0:
-		return DDI_BUF_EMP_400MV_0DB_HSW;
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_3_5:
-		return DDI_BUF_EMP_400MV_3_5DB_HSW;
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_6:
-		return DDI_BUF_EMP_400MV_6DB_HSW;
-	case DP_TRAIN_VOLTAGE_SWING_400 | DP_TRAIN_PRE_EMPHASIS_9_5:
-		return DDI_BUF_EMP_400MV_9_5DB_HSW;
-
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_0:
-		return DDI_BUF_EMP_600MV_0DB_HSW;
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_3_5:
-		return DDI_BUF_EMP_600MV_3_5DB_HSW;
-	case DP_TRAIN_VOLTAGE_SWING_600 | DP_TRAIN_PRE_EMPHASIS_6:
-		return DDI_BUF_EMP_600MV_6DB_HSW;
-
-	case DP_TRAIN_VOLTAGE_SWING_800 | DP_TRAIN_PRE_EMPHASIS_0:
-		return DDI_BUF_EMP_800MV_0DB_HSW;
-	case DP_TRAIN_VOLTAGE_SWING_800 | DP_TRAIN_PRE_EMPHASIS_3_5:
-		return DDI_BUF_EMP_800MV_3_5DB_HSW;
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_0:
+		return DDI_BUF_TRANS_SELECT(0);
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_1:
+		return DDI_BUF_TRANS_SELECT(1);
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_2:
+		return DDI_BUF_TRANS_SELECT(2);
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_0 | DP_TRAIN_PRE_EMPH_LEVEL_3:
+		return DDI_BUF_TRANS_SELECT(3);
+
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_0:
+		return DDI_BUF_TRANS_SELECT(4);
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_1:
+		return DDI_BUF_TRANS_SELECT(5);
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_1 | DP_TRAIN_PRE_EMPH_LEVEL_2:
+		return DDI_BUF_TRANS_SELECT(6);
+
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_2 | DP_TRAIN_PRE_EMPH_LEVEL_0:
+		return DDI_BUF_TRANS_SELECT(7);
+	case DP_TRAIN_VOLTAGE_SWING_LEVEL_2 | DP_TRAIN_PRE_EMPH_LEVEL_1:
+		return DDI_BUF_TRANS_SELECT(8);
 	default:
 		DRM_DEBUG_KMS("Unsupported voltage swing/pre-emphasis level:"
 			      "0x%x\n", signal_levels);
-		return DDI_BUF_EMP_400MV_0DB_HSW;
+		return DDI_BUF_TRANS_SELECT(0);
 	}
 }
 
@@ -2921,74 +3367,10 @@ intel_dp_set_link_train(struct intel_dp *intel_dp,
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum port port = intel_dig_port->port;
 	uint8_t buf[sizeof(intel_dp->train_set) + 1];
 	int ret, len;
 
-	if (HAS_DDI(dev)) {
-		uint32_t temp = I915_READ(DP_TP_CTL(port));
-
-		if (dp_train_pat & DP_LINK_SCRAMBLING_DISABLE)
-			temp |= DP_TP_CTL_SCRAMBLE_DISABLE;
-		else
-			temp &= ~DP_TP_CTL_SCRAMBLE_DISABLE;
-
-		temp &= ~DP_TP_CTL_LINK_TRAIN_MASK;
-		switch (dp_train_pat & DP_TRAINING_PATTERN_MASK) {
-		case DP_TRAINING_PATTERN_DISABLE:
-			temp |= DP_TP_CTL_LINK_TRAIN_NORMAL;
-
-			break;
-		case DP_TRAINING_PATTERN_1:
-			temp |= DP_TP_CTL_LINK_TRAIN_PAT1;
-			break;
-		case DP_TRAINING_PATTERN_2:
-			temp |= DP_TP_CTL_LINK_TRAIN_PAT2;
-			break;
-		case DP_TRAINING_PATTERN_3:
-			temp |= DP_TP_CTL_LINK_TRAIN_PAT3;
-			break;
-		}
-		I915_WRITE(DP_TP_CTL(port), temp);
-
-	} else if (HAS_PCH_CPT(dev) && (IS_GEN7(dev) || port != PORT_A)) {
-		*DP &= ~DP_LINK_TRAIN_MASK_CPT;
-
-		switch (dp_train_pat & DP_TRAINING_PATTERN_MASK) {
-		case DP_TRAINING_PATTERN_DISABLE:
-			*DP |= DP_LINK_TRAIN_OFF_CPT;
-			break;
-		case DP_TRAINING_PATTERN_1:
-			*DP |= DP_LINK_TRAIN_PAT_1_CPT;
-			break;
-		case DP_TRAINING_PATTERN_2:
-			*DP |= DP_LINK_TRAIN_PAT_2_CPT;
-			break;
-		case DP_TRAINING_PATTERN_3:
-			DRM_ERROR("DP training pattern 3 not supported\n");
-			*DP |= DP_LINK_TRAIN_PAT_2_CPT;
-			break;
-		}
-
-	} else {
-		*DP &= ~DP_LINK_TRAIN_MASK;
-
-		switch (dp_train_pat & DP_TRAINING_PATTERN_MASK) {
-		case DP_TRAINING_PATTERN_DISABLE:
-			*DP |= DP_LINK_TRAIN_OFF;
-			break;
-		case DP_TRAINING_PATTERN_1:
-			*DP |= DP_LINK_TRAIN_PAT_1;
-			break;
-		case DP_TRAINING_PATTERN_2:
-			*DP |= DP_LINK_TRAIN_PAT_2;
-			break;
-		case DP_TRAINING_PATTERN_3:
-			DRM_ERROR("DP training pattern 3 not supported\n");
-			*DP |= DP_LINK_TRAIN_PAT_2;
-			break;
-		}
-	}
+	_intel_dp_set_link_train(intel_dp, DP, dp_train_pat);
 
 	I915_WRITE(intel_dp->output_reg, *DP);
 	POSTING_READ(intel_dp->output_reg);
@@ -3272,7 +3654,10 @@ intel_dp_link_down(struct intel_dp *intel_dp)
 		DP &= ~DP_LINK_TRAIN_MASK_CPT;
 		I915_WRITE(intel_dp->output_reg, DP | DP_LINK_TRAIN_PAT_IDLE_CPT);
 	} else {
-		DP &= ~DP_LINK_TRAIN_MASK;
+		if (IS_CHERRYVIEW(dev))
+			DP &= ~DP_LINK_TRAIN_MASK_CHV;
+		else
+			DP &= ~DP_LINK_TRAIN_MASK;
 		I915_WRITE(intel_dp->output_reg, DP | DP_LINK_TRAIN_PAT_IDLE);
 	}
 	POSTING_READ(intel_dp->output_reg);
@@ -3318,15 +3703,11 @@ intel_dp_get_dpcd(struct intel_dp *intel_dp)
 	struct drm_device *dev = dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
-	char dpcd_hex_dump[sizeof(intel_dp->dpcd) * 3];
-
 	if (intel_dp_dpcd_read_wake(&intel_dp->aux, 0x000, intel_dp->dpcd,
 				    sizeof(intel_dp->dpcd)) < 0)
 		return false; /* aux transfer failed */
 
-	hex_dump_to_buffer(intel_dp->dpcd, sizeof(intel_dp->dpcd),
-			   32, 1, dpcd_hex_dump, sizeof(dpcd_hex_dump), false);
-	DRM_DEBUG_KMS("DPCD: %s\n", dpcd_hex_dump);
+	DRM_DEBUG_KMS("DPCD: %*ph\n", (int) sizeof(intel_dp->dpcd), intel_dp->dpcd);
 
 	if (intel_dp->dpcd[DP_DPCD_REV] == 0)
 		return false; /* DPCD not present */
@@ -3347,7 +3728,7 @@ intel_dp_get_dpcd(struct intel_dp *intel_dp)
 	if (intel_dp->dpcd[DP_DPCD_REV] >= 0x12 &&
 	    intel_dp->dpcd[DP_MAX_LANE_COUNT] & DP_TPS3_SUPPORTED) {
 		intel_dp->use_tps3 = true;
-		DRM_DEBUG_KMS("Displayport TPS3 supported");
+		DRM_DEBUG_KMS("Displayport TPS3 supported\n");
 	} else
 		intel_dp->use_tps3 = false;
 
@@ -3384,7 +3765,7 @@ intel_dp_probe_oui(struct intel_dp *intel_dp)
 		DRM_DEBUG_KMS("Branch OUI: %02hx%02hx%02hx\n",
 			      buf[0], buf[1], buf[2]);
 
-	edp_panel_vdd_off(intel_dp, false);
+	intel_edp_panel_vdd_off(intel_dp, false);
 }
 
 static bool
@@ -3398,7 +3779,7 @@ intel_dp_probe_mst(struct intel_dp *intel_dp)
 	if (intel_dp->dpcd[DP_DPCD_REV] < 0x12)
 		return false;
 
-	_edp_panel_vdd_on(intel_dp);
+	intel_edp_panel_vdd_on(intel_dp);
 	if (intel_dp_dpcd_read_wake(&intel_dp->aux, DP_MSTM_CAP, buf, 1)) {
 		if (buf[0] & DP_MST_CAP) {
 			DRM_DEBUG_KMS("Sink is MST capable\n");
@@ -3408,7 +3789,7 @@ intel_dp_probe_mst(struct intel_dp *intel_dp)
 			intel_dp->is_mst = false;
 		}
 	}
-	edp_panel_vdd_off(intel_dp, false);
+	intel_edp_panel_vdd_off(intel_dp, false);
 
 	drm_dp_mst_topology_mgr_set_mst(&intel_dp->mst_mgr, intel_dp->is_mst);
 	return intel_dp->is_mst;
@@ -3423,21 +3804,21 @@ int intel_dp_sink_crc(struct intel_dp *intel_dp, u8 *crc)
 	u8 buf[1];
 
 	if (drm_dp_dpcd_readb(&intel_dp->aux, DP_TEST_SINK_MISC, buf) < 0)
-		return -EAGAIN;
+		return -EIO;
 
 	if (!(buf[0] & DP_TEST_CRC_SUPPORTED))
 		return -ENOTTY;
 
 	if (drm_dp_dpcd_writeb(&intel_dp->aux, DP_TEST_SINK,
 			       DP_TEST_SINK_START) < 0)
-		return -EAGAIN;
+		return -EIO;
 
 	/* Wait 2 vblanks to be sure we will have the correct CRC value */
 	intel_wait_for_vblank(dev, intel_crtc->pipe);
 	intel_wait_for_vblank(dev, intel_crtc->pipe);
 
 	if (drm_dp_dpcd_read(&intel_dp->aux, DP_TEST_CRC_R_CR, crc, 6) < 0)
-		return -EAGAIN;
+		return -EIO;
 
 	drm_dp_dpcd_writeb(&intel_dp->aux, DP_TEST_SINK, 0);
 	return 0;
@@ -3553,6 +3934,9 @@ intel_dp_check_link_status(struct intel_dp *intel_dp)
 	if (WARN_ON(!intel_encoder->base.crtc))
 		return;
 
+	if (!to_intel_crtc(intel_encoder->base.crtc)->active)
+		return;
+
 	/* Try to read receiver status if the link appears to be up */
 	if (!intel_dp_get_link_status(intel_dp, link_status)) {
 		return;
@@ -3637,20 +4021,24 @@ intel_dp_detect_dpcd(struct intel_dp *intel_dp)
 }
 
 static enum drm_connector_status
+edp_detect(struct intel_dp *intel_dp)
+{
+	struct drm_device *dev = intel_dp_to_dev(intel_dp);
+	enum drm_connector_status status;
+
+	status = intel_panel_detect(dev);
+	if (status == connector_status_unknown)
+		status = connector_status_connected;
+
+	return status;
+}
+
+static enum drm_connector_status
 ironlake_dp_detect(struct intel_dp *intel_dp)
 {
 	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	enum drm_connector_status status;
-
-	/* Can't disconnect eDP, but you can close the lid... */
-	if (is_edp(intel_dp)) {
-		status = intel_panel_detect(dev);
-		if (status == connector_status_unknown)
-			status = connector_status_connected;
-		return status;
-	}
 
 	if (!ibx_digital_port_connected(dev_priv, intel_dig_port))
 		return connector_status_disconnected;
@@ -3658,24 +4046,12 @@ ironlake_dp_detect(struct intel_dp *intel_dp)
 	return intel_dp_detect_dpcd(intel_dp);
 }
 
-static enum drm_connector_status
-g4x_dp_detect(struct intel_dp *intel_dp)
+static int g4x_digital_port_connected(struct drm_device *dev,
+				       struct intel_digital_port *intel_dig_port)
 {
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	uint32_t bit;
 
-	/* Can't disconnect eDP, but you can close the lid... */
-	if (is_edp(intel_dp)) {
-		enum drm_connector_status status;
-
-		status = intel_panel_detect(dev);
-		if (status == connector_status_unknown)
-			status = connector_status_connected;
-		return status;
-	}
-
 	if (IS_VALLEYVIEW(dev)) {
 		switch (intel_dig_port->port) {
 		case PORT_B:
@@ -3688,7 +4064,7 @@ g4x_dp_detect(struct intel_dp *intel_dp)
 			bit = PORTD_HOTPLUG_LIVE_STATUS_VLV;
 			break;
 		default:
-			return connector_status_unknown;
+			return -EINVAL;
 		}
 	} else {
 		switch (intel_dig_port->port) {
@@ -3702,20 +4078,45 @@ g4x_dp_detect(struct intel_dp *intel_dp)
 			bit = PORTD_HOTPLUG_LIVE_STATUS_G4X;
 			break;
 		default:
-			return connector_status_unknown;
+			return -EINVAL;
 		}
 	}
 
 	if ((I915_READ(PORT_HOTPLUG_STAT) & bit) == 0)
+		return 0;
+	return 1;
+}
+
+static enum drm_connector_status
+g4x_dp_detect(struct intel_dp *intel_dp)
+{
+	struct drm_device *dev = intel_dp_to_dev(intel_dp);
+	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
+	int ret;
+
+	/* Can't disconnect eDP, but you can close the lid... */
+	if (is_edp(intel_dp)) {
+		enum drm_connector_status status;
+
+		status = intel_panel_detect(dev);
+		if (status == connector_status_unknown)
+			status = connector_status_connected;
+		return status;
+	}
+
+	ret = g4x_digital_port_connected(dev, intel_dig_port);
+	if (ret == -EINVAL)
+		return connector_status_unknown;
+	else if (ret == 0)
 		return connector_status_disconnected;
 
 	return intel_dp_detect_dpcd(intel_dp);
 }
 
 static struct edid *
-intel_dp_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
+intel_dp_get_edid(struct intel_dp *intel_dp)
 {
-	struct intel_connector *intel_connector = to_intel_connector(connector);
+	struct intel_connector *intel_connector = intel_dp->attached_connector;
 
 	/* use cached edid if we have one */
 	if (intel_connector->edid) {
@@ -3724,27 +4125,55 @@ intel_dp_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 			return NULL;
 
 		return drm_edid_duplicate(intel_connector->edid);
-	}
+	} else
+		return drm_get_edid(&intel_connector->base,
+				    &intel_dp->aux.ddc);
+}
+
+static void
+intel_dp_set_edid(struct intel_dp *intel_dp)
+{
+	struct intel_connector *intel_connector = intel_dp->attached_connector;
+	struct edid *edid;
+
+	edid = intel_dp_get_edid(intel_dp);
+	intel_connector->detect_edid = edid;
 
-	return drm_get_edid(connector, adapter);
+	if (intel_dp->force_audio != HDMI_AUDIO_AUTO)
+		intel_dp->has_audio = intel_dp->force_audio == HDMI_AUDIO_ON;
+	else
+		intel_dp->has_audio = drm_detect_monitor_audio(edid);
 }
 
-static int
-intel_dp_get_edid_modes(struct drm_connector *connector, struct i2c_adapter *adapter)
+static void
+intel_dp_unset_edid(struct intel_dp *intel_dp)
 {
-	struct intel_connector *intel_connector = to_intel_connector(connector);
+	struct intel_connector *intel_connector = intel_dp->attached_connector;
 
-	/* use cached edid if we have one */
-	if (intel_connector->edid) {
-		/* invalid edid */
-		if (IS_ERR(intel_connector->edid))
-			return 0;
+	kfree(intel_connector->detect_edid);
+	intel_connector->detect_edid = NULL;
 
-		return intel_connector_update_modes(connector,
-						    intel_connector->edid);
-	}
+	intel_dp->has_audio = false;
+}
 
-	return intel_ddc_get_modes(connector, adapter);
+static enum intel_display_power_domain
+intel_dp_power_get(struct intel_dp *dp)
+{
+	struct intel_encoder *encoder = &dp_to_dig_port(dp)->base;
+	enum intel_display_power_domain power_domain;
+
+	power_domain = intel_display_port_power_domain(encoder);
+	intel_display_power_get(to_i915(encoder->base.dev), power_domain);
+
+	return power_domain;
+}
+
+static void
+intel_dp_power_put(struct intel_dp *dp,
+		   enum intel_display_power_domain power_domain)
+{
+	struct intel_encoder *encoder = &dp_to_dig_port(dp)->base;
+	intel_display_power_put(to_i915(encoder->base.dev), power_domain);
 }
 
 static enum drm_connector_status
@@ -3754,33 +4183,30 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
 	struct intel_encoder *intel_encoder = &intel_dig_port->base;
 	struct drm_device *dev = connector->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum drm_connector_status status;
 	enum intel_display_power_domain power_domain;
-	struct edid *edid = NULL;
 	bool ret;
 
-	power_domain = intel_display_port_power_domain(intel_encoder);
-	intel_display_power_get(dev_priv, power_domain);
-
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
 		      connector->base.id, connector->name);
+	intel_dp_unset_edid(intel_dp);
 
 	if (intel_dp->is_mst) {
 		/* MST devices are disconnected from a monitor POV */
 		if (intel_encoder->type != INTEL_OUTPUT_EDP)
 			intel_encoder->type = INTEL_OUTPUT_DISPLAYPORT;
-		status = connector_status_disconnected;
-		goto out;
+		return connector_status_disconnected;
 	}
 
-	intel_dp->has_audio = false;
+	power_domain = intel_dp_power_get(intel_dp);
 
-	if (HAS_PCH_SPLIT(dev))
+	/* Can't disconnect eDP, but you can close the lid... */
+	if (is_edp(intel_dp))
+		status = edp_detect(intel_dp);
+	else if (HAS_PCH_SPLIT(dev))
 		status = ironlake_dp_detect(intel_dp);
 	else
 		status = g4x_dp_detect(intel_dp);
-
 	if (status != connector_status_connected)
 		goto out;
 
@@ -3796,82 +4222,78 @@ intel_dp_detect(struct drm_connector *connector, bool force)
 		goto out;
 	}
 
-	if (intel_dp->force_audio != HDMI_AUDIO_AUTO) {
-		intel_dp->has_audio = (intel_dp->force_audio == HDMI_AUDIO_ON);
-	} else {
-		edid = intel_dp_get_edid(connector, &intel_dp->aux.ddc);
-		if (edid) {
-			intel_dp->has_audio = drm_detect_monitor_audio(edid);
-			kfree(edid);
-		}
-	}
+	intel_dp_set_edid(intel_dp);
 
 	if (intel_encoder->type != INTEL_OUTPUT_EDP)
 		intel_encoder->type = INTEL_OUTPUT_DISPLAYPORT;
 	status = connector_status_connected;
 
 out:
-	intel_display_power_put(dev_priv, power_domain);
+	intel_dp_power_put(intel_dp, power_domain);
 	return status;
 }
 
-static int intel_dp_get_modes(struct drm_connector *connector)
+static void
+intel_dp_force(struct drm_connector *connector)
 {
 	struct intel_dp *intel_dp = intel_attached_dp(connector);
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct intel_encoder *intel_encoder = &intel_dig_port->base;
-	struct intel_connector *intel_connector = to_intel_connector(connector);
-	struct drm_device *dev = connector->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_encoder *intel_encoder = &dp_to_dig_port(intel_dp)->base;
 	enum intel_display_power_domain power_domain;
-	int ret;
 
-	/* We should parse the EDID data and find out if it has an audio sink
-	 */
+	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
+		      connector->base.id, connector->name);
+	intel_dp_unset_edid(intel_dp);
 
-	power_domain = intel_display_port_power_domain(intel_encoder);
-	intel_display_power_get(dev_priv, power_domain);
+	if (connector->status != connector_status_connected)
+		return;
 
-	ret = intel_dp_get_edid_modes(connector, &intel_dp->aux.ddc);
-	intel_display_power_put(dev_priv, power_domain);
-	if (ret)
-		return ret;
+	power_domain = intel_dp_power_get(intel_dp);
+
+	intel_dp_set_edid(intel_dp);
+
+	intel_dp_power_put(intel_dp, power_domain);
+
+	if (intel_encoder->type != INTEL_OUTPUT_EDP)
+		intel_encoder->type = INTEL_OUTPUT_DISPLAYPORT;
+}
+
+static int intel_dp_get_modes(struct drm_connector *connector)
+{
+	struct intel_connector *intel_connector = to_intel_connector(connector);
+	struct edid *edid;
+
+	edid = intel_connector->detect_edid;
+	if (edid) {
+		int ret = intel_connector_update_modes(connector, edid);
+		if (ret)
+			return ret;
+	}
 
 	/* if eDP has no EDID, fall back to fixed mode */
-	if (is_edp(intel_dp) && intel_connector->panel.fixed_mode) {
+	if (is_edp(intel_attached_dp(connector)) &&
+	    intel_connector->panel.fixed_mode) {
 		struct drm_display_mode *mode;
-		mode = drm_mode_duplicate(dev,
+
+		mode = drm_mode_duplicate(connector->dev,
 					  intel_connector->panel.fixed_mode);
 		if (mode) {
 			drm_mode_probed_add(connector, mode);
 			return 1;
 		}
 	}
+
 	return 0;
 }
 
 static bool
 intel_dp_detect_audio(struct drm_connector *connector)
 {
-	struct intel_dp *intel_dp = intel_attached_dp(connector);
-	struct intel_digital_port *intel_dig_port = dp_to_dig_port(intel_dp);
-	struct intel_encoder *intel_encoder = &intel_dig_port->base;
-	struct drm_device *dev = connector->dev;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	enum intel_display_power_domain power_domain;
-	struct edid *edid;
 	bool has_audio = false;
+	struct edid *edid;
 
-	power_domain = intel_display_port_power_domain(intel_encoder);
-	intel_display_power_get(dev_priv, power_domain);
-
-	edid = intel_dp_get_edid(connector, &intel_dp->aux.ddc);
-	if (edid) {
+	edid = to_intel_connector(connector)->detect_edid;
+	if (edid)
 		has_audio = drm_detect_monitor_audio(edid);
-		kfree(edid);
-	}
-
-	intel_display_power_put(dev_priv, power_domain);
 
 	return has_audio;
 }
@@ -3969,6 +4391,8 @@ intel_dp_connector_destroy(struct drm_connector *connector)
 {
 	struct intel_connector *intel_connector = to_intel_connector(connector);
 
+	kfree(intel_connector->detect_edid);
+
 	if (!IS_ERR_OR_NULL(intel_connector->edid))
 		kfree(intel_connector->edid);
 
@@ -3985,16 +4409,20 @@ void intel_dp_encoder_destroy(struct drm_encoder *encoder)
 {
 	struct intel_digital_port *intel_dig_port = enc_to_dig_port(encoder);
 	struct intel_dp *intel_dp = &intel_dig_port->dp;
-	struct drm_device *dev = intel_dp_to_dev(intel_dp);
 
 	drm_dp_aux_unregister(&intel_dp->aux);
 	intel_dp_mst_encoder_cleanup(intel_dig_port);
 	drm_encoder_cleanup(encoder);
 	if (is_edp(intel_dp)) {
 		cancel_delayed_work_sync(&intel_dp->panel_vdd_work);
-		drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
+		/*
+		 * vdd might still be enabled do to the delayed vdd off.
+		 * Make sure vdd is actually turned off here.
+		 */
+		pps_lock(intel_dp);
 		edp_panel_vdd_off_sync(intel_dp);
-		drm_modeset_unlock(&dev->mode_config.connection_mutex);
+		pps_unlock(intel_dp);
+
 		if (intel_dp->edp_notifier.notifier_call) {
 			unregister_reboot_notifier(&intel_dp->edp_notifier);
 			intel_dp->edp_notifier.notifier_call = NULL;
@@ -4003,6 +4431,22 @@ void intel_dp_encoder_destroy(struct drm_encoder *encoder)
 	kfree(intel_dig_port);
 }
 
+static void intel_dp_encoder_suspend(struct intel_encoder *intel_encoder)
+{
+	struct intel_dp *intel_dp = enc_to_intel_dp(&intel_encoder->base);
+
+	if (!is_edp(intel_dp))
+		return;
+
+	/*
+	 * vdd might still be enabled do to the delayed vdd off.
+	 * Make sure vdd is actually turned off here.
+	 */
+	pps_lock(intel_dp);
+	edp_panel_vdd_off_sync(intel_dp);
+	pps_unlock(intel_dp);
+}
+
 static void intel_dp_encoder_reset(struct drm_encoder *encoder)
 {
 	intel_edp_panel_vdd_sanitize(to_intel_encoder(encoder));
@@ -4011,6 +4455,7 @@ static void intel_dp_encoder_reset(struct drm_encoder *encoder)
 static const struct drm_connector_funcs intel_dp_connector_funcs = {
 	.dpms = intel_connector_dpms,
 	.detect = intel_dp_detect,
+	.force = intel_dp_force,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = intel_dp_set_property,
 	.destroy = intel_dp_connector_destroy,
@@ -4037,18 +4482,31 @@ bool
 intel_dp_hpd_pulse(struct intel_digital_port *intel_dig_port, bool long_hpd)
 {
 	struct intel_dp *intel_dp = &intel_dig_port->dp;
+	struct intel_encoder *intel_encoder = &intel_dig_port->base;
 	struct drm_device *dev = intel_dig_port->base.base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	int ret;
+	enum intel_display_power_domain power_domain;
+	bool ret = true;
+
 	if (intel_dig_port->base.type != INTEL_OUTPUT_EDP)
 		intel_dig_port->base.type = INTEL_OUTPUT_DISPLAYPORT;
 
-	DRM_DEBUG_KMS("got hpd irq on port %d - %s\n", intel_dig_port->port,
+	DRM_DEBUG_KMS("got hpd irq on port %c - %s\n",
+		      port_name(intel_dig_port->port),
 		      long_hpd ? "long" : "short");
 
+	power_domain = intel_display_port_power_domain(intel_encoder);
+	intel_display_power_get(dev_priv, power_domain);
+
 	if (long_hpd) {
-		if (!ibx_digital_port_connected(dev_priv, intel_dig_port))
-			goto mst_fail;
+
+		if (HAS_PCH_SPLIT(dev)) {
+			if (!ibx_digital_port_connected(dev_priv, intel_dig_port))
+				goto mst_fail;
+		} else {
+			if (g4x_digital_port_connected(dev, intel_dig_port) != 1)
+				goto mst_fail;
+		}
 
 		if (!intel_dp_get_dpcd(intel_dp)) {
 			goto mst_fail;
@@ -4061,8 +4519,7 @@ intel_dp_hpd_pulse(struct intel_digital_port *intel_dig_port, bool long_hpd)
 
 	} else {
 		if (intel_dp->is_mst) {
-			ret = intel_dp_check_mst_status(intel_dp);
-			if (ret == -EINVAL)
+			if (intel_dp_check_mst_status(intel_dp) == -EINVAL)
 				goto mst_fail;
 		}
 
@@ -4076,7 +4533,8 @@ intel_dp_hpd_pulse(struct intel_digital_port *intel_dig_port, bool long_hpd)
 			drm_modeset_unlock(&dev->mode_config.connection_mutex);
 		}
 	}
-	return false;
+	ret = false;
+	goto put_power;
 mst_fail:
 	/* if we were in MST mode, and device is not there get out of MST mode */
 	if (intel_dp->is_mst) {
@@ -4084,7 +4542,10 @@ mst_fail:
 		intel_dp->is_mst = false;
 		drm_dp_mst_topology_mgr_set_mst(&intel_dp->mst_mgr, intel_dp->is_mst);
 	}
-	return true;
+put_power:
+	intel_display_power_put(dev_priv, power_domain);
+
+	return ret;
 }
 
 /* Return which DP Port should be selected for Transcoder DP control */
@@ -4171,6 +4632,8 @@ intel_dp_init_panel_power_sequencer(struct drm_device *dev,
 	u32 pp_on, pp_off, pp_div, pp;
 	int pp_ctrl_reg, pp_on_reg, pp_off_reg, pp_div_reg;
 
+	lockdep_assert_held(&dev_priv->pps_mutex);
+
 	if (HAS_PCH_SPLIT(dev)) {
 		pp_ctrl_reg = PCH_PP_CONTROL;
 		pp_on_reg = PCH_PP_ON_DELAYS;
@@ -4270,6 +4733,9 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 	u32 pp_on, pp_off, pp_div, port_sel = 0;
 	int div = HAS_PCH_SPLIT(dev) ? intel_pch_rawclk(dev) : intel_hrawclk(dev);
 	int pp_on_reg, pp_off_reg, pp_div_reg;
+	enum port port = dp_to_dig_port(intel_dp)->port;
+
+	lockdep_assert_held(&dev_priv->pps_mutex);
 
 	if (HAS_PCH_SPLIT(dev)) {
 		pp_on_reg = PCH_PP_ON_DELAYS;
@@ -4304,12 +4770,9 @@ intel_dp_init_panel_power_sequencer_registers(struct drm_device *dev,
 	/* Haswell doesn't have any port selection bits for the panel
 	 * power sequencer any more. */
 	if (IS_VALLEYVIEW(dev)) {
-		if (dp_to_dig_port(intel_dp)->port == PORT_B)
-			port_sel = PANEL_PORT_SELECT_DPB_VLV;
-		else
-			port_sel = PANEL_PORT_SELECT_DPC_VLV;
+		port_sel = PANEL_PORT_SELECT_VLV(port);
 	} else if (HAS_PCH_IBX(dev) || HAS_PCH_CPT(dev)) {
-		if (dp_to_dig_port(intel_dp)->port == PORT_A)
+		if (port == PORT_A)
 			port_sel = PANEL_PORT_SELECT_DPA;
 		else
 			port_sel = PANEL_PORT_SELECT_DPD;
@@ -4393,7 +4856,7 @@ void intel_dp_set_drrs_state(struct drm_device *dev, int refresh_rate)
 		val = I915_READ(reg);
 		if (index > DRRS_HIGH_RR) {
 			val |= PIPECONF_EDP_RR_MODE_SWITCH;
-			intel_dp_set_m2_n2(intel_crtc, &config->dp_m2_n2);
+			intel_dp_set_m_n(intel_crtc);
 		} else {
 			val &= ~PIPECONF_EDP_RR_MODE_SWITCH;
 		}
@@ -4433,7 +4896,7 @@ intel_dp_drrs_init(struct intel_digital_port *intel_dig_port,
 	}
 
 	if (dev_priv->vbt.drrs_type != SEAMLESS_DRRS_SUPPORT) {
-		DRM_INFO("VBT doesn't support DRRS\n");
+		DRM_DEBUG_KMS("VBT doesn't support DRRS\n");
 		return NULL;
 	}
 
@@ -4441,7 +4904,7 @@ intel_dp_drrs_init(struct intel_digital_port *intel_dig_port,
 					(dev, fixed_mode, connector);
 
 	if (!downclock_mode) {
-		DRM_INFO("DRRS not supported\n");
+		DRM_DEBUG_KMS("DRRS not supported\n");
 		return NULL;
 	}
 
@@ -4452,7 +4915,7 @@ intel_dp_drrs_init(struct intel_digital_port *intel_dig_port,
 	intel_dp->drrs_state.type = dev_priv->vbt.drrs_type;
 
 	intel_dp->drrs_state.refresh_rate_type = DRRS_HIGH_RR;
-	DRM_INFO("seamless DRRS supported for eDP panel.\n");
+	DRM_DEBUG_KMS("seamless DRRS supported for eDP panel.\n");
 	return downclock_mode;
 }
 
@@ -4467,8 +4930,11 @@ void intel_edp_panel_vdd_sanitize(struct intel_encoder *intel_encoder)
 		return;
 
 	intel_dp = enc_to_intel_dp(&intel_encoder->base);
+
+	pps_lock(intel_dp);
+
 	if (!edp_have_panel_vdd(intel_dp))
-		return;
+		goto out;
 	/*
 	 * The VDD bit needs a power domain reference, so if the bit is
 	 * already enabled when we boot or resume, grab this reference and
@@ -4480,6 +4946,8 @@ void intel_edp_panel_vdd_sanitize(struct intel_encoder *intel_encoder)
 	intel_display_power_get(dev_priv, power_domain);
 
 	edp_panel_vdd_schedule_off(intel_dp);
+ out:
+	pps_unlock(intel_dp);
 }
 
 static bool intel_edp_init_connector(struct intel_dp *intel_dp,
@@ -4507,7 +4975,7 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	/* Cache DPCD and EDID for edp. */
 	intel_edp_panel_vdd_on(intel_dp);
 	has_dpcd = intel_dp_get_dpcd(intel_dp);
-	edp_panel_vdd_off(intel_dp, false);
+	intel_edp_panel_vdd_off(intel_dp, false);
 
 	if (has_dpcd) {
 		if (intel_dp->dpcd[DP_DPCD_REV] >= 0x11)
@@ -4521,7 +4989,9 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	}
 
 	/* We now know it's not a ghost, init power sequence regs. */
+	pps_lock(intel_dp);
 	intel_dp_init_panel_power_sequencer_registers(dev, intel_dp, power_seq);
+	pps_unlock(intel_dp);
 
 	mutex_lock(&dev->mode_config.mutex);
 	edid = drm_get_edid(connector, &intel_dp->aux.ddc);
@@ -4565,6 +5035,7 @@ static bool intel_edp_init_connector(struct intel_dp *intel_dp,
 	}
 
 	intel_panel_init(&intel_connector->panel, fixed_mode, downclock_mode);
+	intel_connector->panel.backlight_power = intel_edp_backlight_power;
 	intel_panel_setup_backlight(connector);
 
 	return true;
@@ -4583,6 +5054,8 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	struct edp_power_seq power_seq = { 0 };
 	int type;
 
+	intel_dp->pps_pipe = INVALID_PIPE;
+
 	/* intel_dp vfuncs */
 	if (IS_VALLEYVIEW(dev))
 		intel_dp->get_aux_clock_divider = vlv_get_aux_clock_divider;
@@ -4653,8 +5126,15 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	}
 
 	if (is_edp(intel_dp)) {
-		intel_dp_init_panel_power_timestamps(intel_dp);
-		intel_dp_init_panel_power_sequencer(dev, intel_dp, &power_seq);
+		pps_lock(intel_dp);
+		if (IS_VALLEYVIEW(dev)) {
+			vlv_initial_power_sequencer_setup(intel_dp);
+		} else {
+			intel_dp_init_panel_power_timestamps(intel_dp);
+			intel_dp_init_panel_power_sequencer(dev, intel_dp,
+							    &power_seq);
+		}
+		pps_unlock(intel_dp);
 	}
 
 	intel_dp_aux_init(intel_dp, intel_connector);
@@ -4662,7 +5142,8 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 	/* init MST on ports that can support it */
 	if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
 		if (port == PORT_B || port == PORT_C || port == PORT_D) {
-			intel_dp_mst_encoder_init(intel_dig_port, intel_connector->base.base.id);
+			intel_dp_mst_encoder_init(intel_dig_port,
+						  intel_connector->base.base.id);
 		}
 	}
 
@@ -4670,9 +5151,13 @@ intel_dp_init_connector(struct intel_digital_port *intel_dig_port,
 		drm_dp_aux_unregister(&intel_dp->aux);
 		if (is_edp(intel_dp)) {
 			cancel_delayed_work_sync(&intel_dp->panel_vdd_work);
-			drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
+			/*
+			 * vdd might still be enabled do to the delayed vdd off.
+			 * Make sure vdd is actually turned off here.
+			 */
+			pps_lock(intel_dp);
 			edp_panel_vdd_off_sync(intel_dp);
-			drm_modeset_unlock(&dev->mode_config.connection_mutex);
+			pps_unlock(intel_dp);
 		}
 		drm_connector_unregister(connector);
 		drm_connector_cleanup(connector);
@@ -4722,6 +5207,7 @@ intel_dp_init(struct drm_device *dev, int output_reg, enum port port)
 	intel_encoder->disable = intel_disable_dp;
 	intel_encoder->get_hw_state = intel_dp_get_hw_state;
 	intel_encoder->get_config = intel_dp_get_config;
+	intel_encoder->suspend = intel_dp_encoder_suspend;
 	if (IS_CHERRYVIEW(dev)) {
 		intel_encoder->pre_pll_enable = chv_dp_pre_pll_enable;
 		intel_encoder->pre_enable = chv_pre_enable_dp;
@@ -4735,7 +5221,8 @@ intel_dp_init(struct drm_device *dev, int output_reg, enum port port)
 	} else {
 		intel_encoder->pre_enable = g4x_pre_enable_dp;
 		intel_encoder->enable = g4x_enable_dp;
-		intel_encoder->post_disable = g4x_post_disable_dp;
+		if (INTEL_INFO(dev)->gen >= 5)
+			intel_encoder->post_disable = ilk_post_disable_dp;
 	}
 
 	intel_dig_port->port = port;
diff --git a/drivers/gpu/drm/i915/intel_drv.h b/drivers/gpu/drm/i915/intel_drv.h
index 4b2664bd5b81..ba715229a540 100644
--- a/drivers/gpu/drm/i915/intel_drv.h
+++ b/drivers/gpu/drm/i915/intel_drv.h
@@ -25,6 +25,7 @@
 #ifndef __INTEL_DRV_H__
 #define __INTEL_DRV_H__
 
+#include <linux/async.h>
 #include <linux/i2c.h>
 #include <linux/hdmi.h>
 #include <drm/i915_drm.h>
@@ -34,6 +35,9 @@
 #include <drm/drm_fb_helper.h>
 #include <drm/drm_dp_mst_helper.h>
 
+#define DIV_ROUND_CLOSEST_ULL(ll, d)	\
+({ unsigned long long _tmp = (ll)+(d)/2; do_div(_tmp, d); _tmp; })
+
 /**
  * _wait_for - magic (register) wait macro
  *
@@ -153,6 +157,12 @@ struct intel_encoder {
 	 * be set correctly before calling this function. */
 	void (*get_config)(struct intel_encoder *,
 			   struct intel_crtc_config *pipe_config);
+	/*
+	 * Called during system suspend after all pending requests for the
+	 * encoder are flushed (for example for DP AUX transactions) and
+	 * device interrupts are disabled.
+	 */
+	void (*suspend)(struct intel_encoder *);
 	int crtc_mask;
 	enum hpd_pin hpd_pin;
 };
@@ -173,6 +183,8 @@ struct intel_panel {
 		bool active_low_pwm;
 		struct backlight_device *device;
 	} backlight;
+
+	void (*backlight_power)(struct intel_connector *, bool enable);
 };
 
 struct intel_connector {
@@ -205,6 +217,7 @@ struct intel_connector {
 
 	/* Cached EDID for eDP and LVDS. May hold ERR_PTR for invalid EDID. */
 	struct edid *edid;
+	struct edid *detect_edid;
 
 	/* since POLL and HPD connectors may use the same HPD line keep the native
 	   state of connector->polled in case hotplug storm detection changes it */
@@ -324,6 +337,7 @@ struct intel_crtc_config {
 
 	/* m2_n2 for eDP downclock */
 	struct intel_link_m_n dp_m2_n2;
+	bool has_drrs;
 
 	/*
 	 * Frequence the dpll for the port should run at. Differs from the
@@ -404,6 +418,7 @@ struct intel_crtc {
 	uint32_t cursor_addr;
 	int16_t cursor_width, cursor_height;
 	uint32_t cursor_cntl;
+	uint32_t cursor_size;
 	uint32_t cursor_base;
 
 	struct intel_plane_config plane_config;
@@ -424,8 +439,6 @@ struct intel_crtc {
 		struct intel_pipe_wm active;
 	} wm;
 
-	wait_queue_head_t vbl_wait;
-
 	int scanline_offset;
 	struct intel_mmio_flip mmio_flip;
 };
@@ -449,6 +462,7 @@ struct intel_plane {
 	unsigned int crtc_w, crtc_h;
 	uint32_t src_x, src_y;
 	uint32_t src_w, src_h;
+	unsigned int rotation;
 
 	/* Since we need to change the watermarks before/after
 	 * enabling/disabling the planes, we need to store the parameters here
@@ -559,6 +573,12 @@ struct intel_dp {
 
 	struct notifier_block edp_notifier;
 
+	/*
+	 * Pipe whose power sequencer is currently locked into
+	 * this port. Only relevant on VLV/CHV.
+	 */
+	enum pipe pps_pipe;
+
 	bool use_tps3;
 	bool can_mst; /* this port supports mst */
 	bool is_mst;
@@ -657,6 +677,10 @@ struct intel_unpin_work {
 #define INTEL_FLIP_COMPLETE	2
 	u32 flip_count;
 	u32 gtt_offset;
+	struct intel_engine_cs *flip_queued_ring;
+	u32 flip_queued_seqno;
+	int flip_queued_vblank;
+	int flip_ready_vblank;
 	bool enable_stall_check;
 };
 
@@ -821,7 +845,6 @@ int intel_get_pipe_from_crtc_id(struct drm_device *dev, void *data,
 enum transcoder intel_pipe_to_cpu_transcoder(struct drm_i915_private *dev_priv,
 					     enum pipe pipe);
 void intel_wait_for_vblank(struct drm_device *dev, int pipe);
-void intel_wait_for_pipe_off(struct drm_device *dev, int pipe);
 int ironlake_get_lanes_required(int target_clock, int link_bw, int bpp);
 void vlv_wait_port_ready(struct drm_i915_private *dev_priv,
 			 struct intel_digital_port *dport);
@@ -830,8 +853,7 @@ bool intel_get_load_detect_pipe(struct drm_connector *connector,
 				struct intel_load_detect_pipe *old,
 				struct drm_modeset_acquire_ctx *ctx);
 void intel_release_load_detect_pipe(struct drm_connector *connector,
-				    struct intel_load_detect_pipe *old,
-				    struct drm_modeset_acquire_ctx *ctx);
+				    struct intel_load_detect_pipe *old);
 int intel_pin_and_fence_fb_obj(struct drm_device *dev,
 			       struct drm_i915_gem_object *obj,
 			       struct intel_engine_cs *pipelined);
@@ -843,6 +865,7 @@ __intel_framebuffer_create(struct drm_device *dev,
 void intel_prepare_page_flip(struct drm_device *dev, int plane);
 void intel_finish_page_flip(struct drm_device *dev, int pipe);
 void intel_finish_page_flip_plane(struct drm_device *dev, int plane);
+void intel_check_page_flip(struct drm_device *dev, int pipe);
 
 /* shared dpll functions */
 struct intel_shared_dpll *intel_crtc_to_shared_dpll(struct intel_crtc *crtc);
@@ -877,6 +900,7 @@ void hsw_enable_pc8(struct drm_i915_private *dev_priv);
 void hsw_disable_pc8(struct drm_i915_private *dev_priv);
 void intel_dp_get_m_n(struct intel_crtc *crtc,
 		      struct intel_crtc_config *pipe_config);
+void intel_dp_set_m_n(struct intel_crtc *crtc);
 int intel_dotclock_calculate(int link_freq, const struct intel_link_m_n *m_n);
 void
 ironlake_check_encoder_dotclock(const struct intel_crtc_config *pipe_config,
@@ -891,7 +915,7 @@ void intel_mode_from_pipe_config(struct drm_display_mode *mode,
 				 struct intel_crtc_config *pipe_config);
 int intel_format_to_fourcc(int format);
 void intel_crtc_wait_for_pending_flips(struct drm_crtc *crtc);
-
+void intel_modeset_preclose(struct drm_device *dev, struct drm_file *file);
 
 /* intel_dp.c */
 void intel_dp_init(struct drm_device *dev, int output_reg, enum port port);
@@ -930,6 +954,7 @@ void intel_dp_mst_suspend(struct drm_device *dev);
 void intel_dp_mst_resume(struct drm_device *dev);
 int intel_dp_max_link_bw(struct intel_dp *intel_dp);
 void intel_dp_hot_plug(struct intel_encoder *intel_encoder);
+void vlv_power_sequencer_reset(struct drm_i915_private *dev_priv);
 /* intel_dp_mst.c */
 int intel_dp_mst_encoder_init(struct intel_digital_port *intel_dig_port, int conn_id);
 void intel_dp_mst_encoder_cleanup(struct intel_digital_port *intel_dig_port);
@@ -944,9 +969,9 @@ void intel_dvo_init(struct drm_device *dev);
 /* legacy fbdev emulation in intel_fbdev.c */
 #ifdef CONFIG_DRM_I915_FBDEV
 extern int intel_fbdev_init(struct drm_device *dev);
-extern void intel_fbdev_initial_config(struct drm_device *dev);
+extern void intel_fbdev_initial_config(void *data, async_cookie_t cookie);
 extern void intel_fbdev_fini(struct drm_device *dev);
-extern void intel_fbdev_set_suspend(struct drm_device *dev, int state);
+extern void intel_fbdev_set_suspend(struct drm_device *dev, int state, bool synchronous);
 extern void intel_fbdev_output_poll_changed(struct drm_device *dev);
 extern void intel_fbdev_restore_mode(struct drm_device *dev);
 #else
@@ -955,7 +980,7 @@ static inline int intel_fbdev_init(struct drm_device *dev)
 	return 0;
 }
 
-static inline void intel_fbdev_initial_config(struct drm_device *dev)
+static inline void intel_fbdev_initial_config(void *data, async_cookie_t cookie)
 {
 }
 
@@ -963,7 +988,7 @@ static inline void intel_fbdev_fini(struct drm_device *dev)
 {
 }
 
-static inline void intel_fbdev_set_suspend(struct drm_device *dev, int state)
+static inline void intel_fbdev_set_suspend(struct drm_device *dev, int state, bool synchronous)
 {
 }
 
@@ -1086,7 +1111,10 @@ bool intel_sdvo_init(struct drm_device *dev, uint32_t sdvo_reg, bool is_sdvob);
 int intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane);
 void intel_flush_primary_plane(struct drm_i915_private *dev_priv,
 			       enum plane plane);
-void intel_plane_restore(struct drm_plane *plane);
+int intel_plane_set_property(struct drm_plane *plane,
+			     struct drm_property *prop,
+			     uint64_t val);
+int intel_plane_restore(struct drm_plane *plane);
 void intel_plane_disable(struct drm_plane *plane);
 int intel_sprite_set_colorkey(struct drm_device *dev, void *data,
 			      struct drm_file *file_priv);
diff --git a/drivers/gpu/drm/i915/intel_dsi.c b/drivers/gpu/drm/i915/intel_dsi.c
index 670c29a7b5dd..5bd9e09ad3c5 100644
--- a/drivers/gpu/drm/i915/intel_dsi.c
+++ b/drivers/gpu/drm/i915/intel_dsi.c
@@ -184,7 +184,7 @@ static void intel_dsi_pre_enable(struct intel_encoder *encoder)
 
 	/* update the hw state for DPLL */
 	intel_crtc->config.dpll_hw_state.dpll = DPLL_INTEGRATED_CLOCK_VLV |
-						DPLL_REFA_CLK_ENABLE_VLV;
+		DPLL_REFA_CLK_ENABLE_VLV;
 
 	tmp = I915_READ(DSPCLK_GATE_D);
 	tmp |= DPOUNIT_CLOCK_GATE_DISABLE;
@@ -259,8 +259,8 @@ static void intel_dsi_disable(struct intel_encoder *encoder)
 	temp = I915_READ(MIPI_CTRL(pipe));
 	temp &= ~ESCAPE_CLOCK_DIVIDER_MASK;
 	I915_WRITE(MIPI_CTRL(pipe), temp |
-			intel_dsi->escape_clk_div <<
-			ESCAPE_CLOCK_DIVIDER_SHIFT);
+		   intel_dsi->escape_clk_div <<
+		   ESCAPE_CLOCK_DIVIDER_SHIFT);
 
 	I915_WRITE(MIPI_EOT_DISABLE(pipe), CLOCKSTOP);
 
@@ -297,7 +297,7 @@ static void intel_dsi_clear_device_ready(struct intel_encoder *encoder)
 	usleep_range(2000, 2500);
 
 	if (wait_for(((I915_READ(MIPI_PORT_CTRL(pipe)) & AFE_LATCHOUT)
-					== 0x00000), 30))
+		      == 0x00000), 30))
 		DRM_ERROR("DSI LP not going Low\n");
 
 	val = I915_READ(MIPI_PORT_CTRL(pipe));
@@ -423,9 +423,11 @@ static u16 txclkesc(u32 divider, unsigned int us)
 }
 
 /* return pixels in terms of txbyteclkhs */
-static u16 txbyteclkhs(u16 pixels, int bpp, int lane_count)
+static u16 txbyteclkhs(u16 pixels, int bpp, int lane_count,
+		       u16 burst_mode_ratio)
 {
-	return DIV_ROUND_UP(DIV_ROUND_UP(pixels * bpp, 8), lane_count);
+	return DIV_ROUND_UP(DIV_ROUND_UP(pixels * bpp * burst_mode_ratio,
+					 8 * 100), lane_count);
 }
 
 static void set_dsi_timings(struct drm_encoder *encoder,
@@ -451,10 +453,12 @@ static void set_dsi_timings(struct drm_encoder *encoder,
 	vbp = mode->vtotal - mode->vsync_end;
 
 	/* horizontal values are in terms of high speed byte clock */
-	hactive = txbyteclkhs(hactive, bpp, lane_count);
-	hfp = txbyteclkhs(hfp, bpp, lane_count);
-	hsync = txbyteclkhs(hsync, bpp, lane_count);
-	hbp = txbyteclkhs(hbp, bpp, lane_count);
+	hactive = txbyteclkhs(hactive, bpp, lane_count,
+			      intel_dsi->burst_mode_ratio);
+	hfp = txbyteclkhs(hfp, bpp, lane_count, intel_dsi->burst_mode_ratio);
+	hsync = txbyteclkhs(hsync, bpp, lane_count,
+			    intel_dsi->burst_mode_ratio);
+	hbp = txbyteclkhs(hbp, bpp, lane_count, intel_dsi->burst_mode_ratio);
 
 	I915_WRITE(MIPI_HACTIVE_AREA_COUNT(pipe), hactive);
 	I915_WRITE(MIPI_HFP_COUNT(pipe), hfp);
@@ -541,12 +545,14 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder)
 	    intel_dsi->video_mode_format == VIDEO_MODE_BURST) {
 		I915_WRITE(MIPI_HS_TX_TIMEOUT(pipe),
 			   txbyteclkhs(adjusted_mode->htotal, bpp,
-				       intel_dsi->lane_count) + 1);
+				       intel_dsi->lane_count,
+				       intel_dsi->burst_mode_ratio) + 1);
 	} else {
 		I915_WRITE(MIPI_HS_TX_TIMEOUT(pipe),
 			   txbyteclkhs(adjusted_mode->vtotal *
 				       adjusted_mode->htotal,
-				       bpp, intel_dsi->lane_count) + 1);
+				       bpp, intel_dsi->lane_count,
+				       intel_dsi->burst_mode_ratio) + 1);
 	}
 	I915_WRITE(MIPI_LP_RX_TIMEOUT(pipe), intel_dsi->lp_rx_timeout);
 	I915_WRITE(MIPI_TURN_AROUND_TIMEOUT(pipe), intel_dsi->turn_arnd_val);
@@ -576,7 +582,7 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder)
 	 * XXX: write MIPI_STOP_STATE_STALL?
 	 */
 	I915_WRITE(MIPI_HIGH_LOW_SWITCH_COUNT(pipe),
-						intel_dsi->hs_to_lp_count);
+		   intel_dsi->hs_to_lp_count);
 
 	/* XXX: low power clock equivalence in terms of byte clock. the number
 	 * of byte clocks occupied in one low power clock. based on txbyteclkhs
@@ -601,10 +607,10 @@ static void intel_dsi_prepare(struct intel_encoder *intel_encoder)
 		 * 64 like 1366 x 768. Enable RANDOM resolution support for such
 		 * panels by default */
 		I915_WRITE(MIPI_VIDEO_MODE_FORMAT(pipe),
-				intel_dsi->video_frmt_cfg_bits |
-				intel_dsi->video_mode_format |
-				IP_TG_CONFIG |
-				RANDOM_DPI_DISPLAY_RESOLUTION);
+			   intel_dsi->video_frmt_cfg_bits |
+			   intel_dsi->video_mode_format |
+			   IP_TG_CONFIG |
+			   RANDOM_DPI_DISPLAY_RESOLUTION);
 }
 
 static void intel_dsi_pre_pll_enable(struct intel_encoder *encoder)
diff --git a/drivers/gpu/drm/i915/intel_dsi.h b/drivers/gpu/drm/i915/intel_dsi.h
index fd51867fd0d3..657eb5c1b9d8 100644
--- a/drivers/gpu/drm/i915/intel_dsi.h
+++ b/drivers/gpu/drm/i915/intel_dsi.h
@@ -116,6 +116,8 @@ struct intel_dsi {
 	u16 clk_hs_to_lp_count;
 
 	u16 init_count;
+	u32 pclk;
+	u16 burst_mode_ratio;
 
 	/* all delays in ms */
 	u16 backlight_off_delay;
diff --git a/drivers/gpu/drm/i915/intel_dsi_cmd.c b/drivers/gpu/drm/i915/intel_dsi_cmd.c
index 7f1430ac8543..f4767fd2ebeb 100644
--- a/drivers/gpu/drm/i915/intel_dsi_cmd.c
+++ b/drivers/gpu/drm/i915/intel_dsi_cmd.c
@@ -430,7 +430,7 @@ void wait_for_dsi_fifo_empty(struct intel_dsi *intel_dsi)
 	u32 mask;
 
 	mask = LP_CTRL_FIFO_EMPTY | HS_CTRL_FIFO_EMPTY |
-					LP_DATA_FIFO_EMPTY | HS_DATA_FIFO_EMPTY;
+		LP_DATA_FIFO_EMPTY | HS_DATA_FIFO_EMPTY;
 
 	if (wait_for((I915_READ(MIPI_GEN_FIFO_STAT(pipe)) & mask) == mask, 100))
 		DRM_ERROR("DPI FIFOs are not empty\n");
diff --git a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
index 47c7584a4aa0..f6bdd44069ce 100644
--- a/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
+++ b/drivers/gpu/drm/i915/intel_dsi_panel_vbt.c
@@ -271,6 +271,8 @@ static bool generic_init(struct intel_dsi_device *dsi)
 	u32 ths_prepare_ns, tclk_trail_ns;
 	u32 tclk_prepare_clkzero, ths_prepare_hszero;
 	u32 lp_to_hs_switch, hs_to_lp_switch;
+	u32 pclk, computed_ddr;
+	u16 burst_mode_ratio;
 
 	DRM_DEBUG_KMS("\n");
 
@@ -284,8 +286,6 @@ static bool generic_init(struct intel_dsi_device *dsi)
 	else if (intel_dsi->pixel_format == VID_MODE_FORMAT_RGB565)
 		bits_per_pixel = 16;
 
-	bitrate = (mode->clock * bits_per_pixel) / intel_dsi->lane_count;
-
 	intel_dsi->operation_mode = mipi_config->is_cmd_mode;
 	intel_dsi->video_mode_format = mipi_config->video_transfer_mode;
 	intel_dsi->escape_clk_div = mipi_config->byte_clk_sel;
@@ -297,6 +297,40 @@ static bool generic_init(struct intel_dsi_device *dsi)
 	intel_dsi->video_frmt_cfg_bits =
 		mipi_config->bta_enabled ? DISABLE_VIDEO_BTA : 0;
 
+	pclk = mode->clock;
+
+	/* Burst Mode Ratio
+	 * Target ddr frequency from VBT / non burst ddr freq
+	 * multiply by 100 to preserve remainder
+	 */
+	if (intel_dsi->video_mode_format == VIDEO_MODE_BURST) {
+		if (mipi_config->target_burst_mode_freq) {
+			computed_ddr =
+				(pclk * bits_per_pixel) / intel_dsi->lane_count;
+
+			if (mipi_config->target_burst_mode_freq <
+								computed_ddr) {
+				DRM_ERROR("Burst mode freq is less than computed\n");
+				return false;
+			}
+
+			burst_mode_ratio = DIV_ROUND_UP(
+				mipi_config->target_burst_mode_freq * 100,
+				computed_ddr);
+
+			pclk = DIV_ROUND_UP(pclk * burst_mode_ratio, 100);
+		} else {
+			DRM_ERROR("Burst mode target is not set\n");
+			return false;
+		}
+	} else
+		burst_mode_ratio = 100;
+
+	intel_dsi->burst_mode_ratio = burst_mode_ratio;
+	intel_dsi->pclk = pclk;
+
+	bitrate = (pclk * bits_per_pixel) / intel_dsi->lane_count;
+
 	switch (intel_dsi->escape_clk_div) {
 	case 0:
 		tlpx_ns = 50;
diff --git a/drivers/gpu/drm/i915/intel_dsi_pll.c b/drivers/gpu/drm/i915/intel_dsi_pll.c
index d8bb1ea2f0da..fa7a6ca34cd6 100644
--- a/drivers/gpu/drm/i915/intel_dsi_pll.c
+++ b/drivers/gpu/drm/i915/intel_dsi_pll.c
@@ -134,8 +134,7 @@ static u32 dsi_rr_formula(const struct drm_display_mode *mode,
 #else
 
 /* Get DSI clock from pixel clock */
-static u32 dsi_clk_from_pclk(const struct drm_display_mode *mode,
-			  int pixel_format, int lane_count)
+static u32 dsi_clk_from_pclk(u32 pclk, int pixel_format, int lane_count)
 {
 	u32 dsi_clk_khz;
 	u32 bpp;
@@ -156,7 +155,7 @@ static u32 dsi_clk_from_pclk(const struct drm_display_mode *mode,
 
 	/* DSI data rate = pixel clock * bits per pixel / lane count
 	   pixel clock is converted from KHz to Hz */
-	dsi_clk_khz = DIV_ROUND_CLOSEST(mode->clock * bpp, lane_count);
+	dsi_clk_khz = DIV_ROUND_CLOSEST(pclk * bpp, lane_count);
 
 	return dsi_clk_khz;
 }
@@ -191,7 +190,7 @@ static int dsi_calc_mnp(u32 dsi_clk, struct dsi_mnp *dsi_mnp)
 	for (m = 62; m <= 92; m++) {
 		for (p = 2; p <= 6; p++) {
 			/* Find the optimal m and p divisors
-			with minimal error +/- the required clock */
+			   with minimal error +/- the required clock */
 			calc_dsi_clk = (m * ref_clk) / p;
 			if (calc_dsi_clk == target_dsi_clk) {
 				calc_m = m;
@@ -228,15 +227,13 @@ static int dsi_calc_mnp(u32 dsi_clk, struct dsi_mnp *dsi_mnp)
 static void vlv_configure_dsi_pll(struct intel_encoder *encoder)
 {
 	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
-	struct intel_crtc *intel_crtc = to_intel_crtc(encoder->base.crtc);
-	const struct drm_display_mode *mode = &intel_crtc->config.adjusted_mode;
 	struct intel_dsi *intel_dsi = enc_to_intel_dsi(&encoder->base);
 	int ret;
 	struct dsi_mnp dsi_mnp;
 	u32 dsi_clk;
 
-	dsi_clk = dsi_clk_from_pclk(mode, intel_dsi->pixel_format,
-						intel_dsi->lane_count);
+	dsi_clk = dsi_clk_from_pclk(intel_dsi->pclk, intel_dsi->pixel_format,
+				    intel_dsi->lane_count);
 
 	ret = dsi_calc_mnp(dsi_clk, &dsi_mnp);
 	if (ret) {
@@ -318,8 +315,8 @@ static void assert_bpp_mismatch(int pixel_format, int pipe_bpp)
 	}
 
 	WARN(bpp != pipe_bpp,
-		"bpp match assertion failure (expected %d, current %d)\n",
-		bpp, pipe_bpp);
+	     "bpp match assertion failure (expected %d, current %d)\n",
+	     bpp, pipe_bpp);
 }
 
 u32 vlv_get_dsi_pclk(struct intel_encoder *encoder, int pipe_bpp)
diff --git a/drivers/gpu/drm/i915/intel_dvo.c b/drivers/gpu/drm/i915/intel_dvo.c
index 56b47d2ffaf7..e40e3df33517 100644
--- a/drivers/gpu/drm/i915/intel_dvo.c
+++ b/drivers/gpu/drm/i915/intel_dvo.c
@@ -85,7 +85,7 @@ static const struct intel_dvo_device intel_dvo_devices[] = {
 	{
 	        .type = INTEL_DVO_CHIP_TMDS,
 		.name = "ns2501",
-		.dvo_reg = DVOC,
+		.dvo_reg = DVOB,
 		.slave_addr = NS2501_ADDR,
 		.dev_ops = &ns2501_ops,
        }
@@ -185,12 +185,13 @@ static void intel_enable_dvo(struct intel_encoder *encoder)
 	u32 dvo_reg = intel_dvo->dev.dvo_reg;
 	u32 temp = I915_READ(dvo_reg);
 
-	I915_WRITE(dvo_reg, temp | DVO_ENABLE);
-	I915_READ(dvo_reg);
 	intel_dvo->dev.dev_ops->mode_set(&intel_dvo->dev,
 					 &crtc->config.requested_mode,
 					 &crtc->config.adjusted_mode);
 
+	I915_WRITE(dvo_reg, temp | DVO_ENABLE);
+	I915_READ(dvo_reg);
+
 	intel_dvo->dev.dev_ops->dpms(&intel_dvo->dev, true);
 }
 
@@ -226,10 +227,6 @@ static void intel_dvo_dpms(struct drm_connector *connector, int mode)
 
 		intel_crtc_update_dpms(crtc);
 
-		intel_dvo->dev.dev_ops->mode_set(&intel_dvo->dev,
-						 &config->requested_mode,
-						 &config->adjusted_mode);
-
 		intel_dvo->dev.dev_ops->dpms(&intel_dvo->dev, true);
 	} else {
 		intel_dvo->dev.dev_ops->dpms(&intel_dvo->dev, false);
diff --git a/drivers/gpu/drm/i915/intel_fbdev.c b/drivers/gpu/drm/i915/intel_fbdev.c
index f475414671d8..9b584f3fbb99 100644
--- a/drivers/gpu/drm/i915/intel_fbdev.c
+++ b/drivers/gpu/drm/i915/intel_fbdev.c
@@ -24,8 +24,10 @@
  *     David Airlie
  */
 
+#include <linux/async.h>
 #include <linux/module.h>
 #include <linux/kernel.h>
+#include <linux/console.h>
 #include <linux/errno.h>
 #include <linux/string.h>
 #include <linux/mm.h>
@@ -331,24 +333,6 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 	int num_connectors_enabled = 0;
 	int num_connectors_detected = 0;
 
-	/*
-	 * If the user specified any force options, just bail here
-	 * and use that config.
-	 */
-	for (i = 0; i < fb_helper->connector_count; i++) {
-		struct drm_fb_helper_connector *fb_conn;
-		struct drm_connector *connector;
-
-		fb_conn = fb_helper->connector_info[i];
-		connector = fb_conn->connector;
-
-		if (!enabled[i])
-			continue;
-
-		if (connector->force != DRM_FORCE_UNSPECIFIED)
-			return false;
-	}
-
 	save_enabled = kcalloc(dev->mode_config.num_connector, sizeof(bool),
 			       GFP_KERNEL);
 	if (!save_enabled)
@@ -374,8 +358,18 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 			continue;
 		}
 
+		if (connector->force == DRM_FORCE_OFF) {
+			DRM_DEBUG_KMS("connector %s is disabled by user, skipping\n",
+				      connector->name);
+			enabled[i] = false;
+			continue;
+		}
+
 		encoder = connector->encoder;
 		if (!encoder || WARN_ON(!encoder->crtc)) {
+			if (connector->force > DRM_FORCE_OFF)
+				goto bail;
+
 			DRM_DEBUG_KMS("connector %s has no encoder or crtc, skipping\n",
 				      connector->name);
 			enabled[i] = false;
@@ -394,8 +388,7 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 		for (j = 0; j < fb_helper->connector_count; j++) {
 			if (crtcs[j] == new_crtc) {
 				DRM_DEBUG_KMS("fallback: cloned configuration\n");
-				fallback = true;
-				goto out;
+				goto bail;
 			}
 		}
 
@@ -466,8 +459,8 @@ static bool intel_fb_initial_config(struct drm_fb_helper *fb_helper,
 		fallback = true;
 	}
 
-out:
 	if (fallback) {
+bail:
 		DRM_DEBUG_KMS("Not using firmware configuration\n");
 		memcpy(enabled, save_enabled, dev->mode_config.num_connector);
 		kfree(save_enabled);
@@ -636,6 +629,15 @@ out:
 	return false;
 }
 
+static void intel_fbdev_suspend_worker(struct work_struct *work)
+{
+	intel_fbdev_set_suspend(container_of(work,
+					     struct drm_i915_private,
+					     fbdev_suspend_work)->dev,
+				FBINFO_STATE_RUNNING,
+				true);
+}
+
 int intel_fbdev_init(struct drm_device *dev)
 {
 	struct intel_fbdev *ifbdev;
@@ -662,14 +664,16 @@ int intel_fbdev_init(struct drm_device *dev)
 	}
 
 	dev_priv->fbdev = ifbdev;
+	INIT_WORK(&dev_priv->fbdev_suspend_work, intel_fbdev_suspend_worker);
+
 	drm_fb_helper_single_add_all_connectors(&ifbdev->helper);
 
 	return 0;
 }
 
-void intel_fbdev_initial_config(struct drm_device *dev)
+void intel_fbdev_initial_config(void *data, async_cookie_t cookie)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_private *dev_priv = data;
 	struct intel_fbdev *ifbdev = dev_priv->fbdev;
 
 	/* Due to peculiar init order wrt to hpd handling this is separate. */
@@ -682,12 +686,15 @@ void intel_fbdev_fini(struct drm_device *dev)
 	if (!dev_priv->fbdev)
 		return;
 
+	flush_work(&dev_priv->fbdev_suspend_work);
+
+	async_synchronize_full();
 	intel_fbdev_destroy(dev, dev_priv->fbdev);
 	kfree(dev_priv->fbdev);
 	dev_priv->fbdev = NULL;
 }
 
-void intel_fbdev_set_suspend(struct drm_device *dev, int state)
+void intel_fbdev_set_suspend(struct drm_device *dev, int state, bool synchronous)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_fbdev *ifbdev = dev_priv->fbdev;
@@ -698,6 +705,33 @@ void intel_fbdev_set_suspend(struct drm_device *dev, int state)
 
 	info = ifbdev->helper.fbdev;
 
+	if (synchronous) {
+		/* Flush any pending work to turn the console on, and then
+		 * wait to turn it off. It must be synchronous as we are
+		 * about to suspend or unload the driver.
+		 *
+		 * Note that from within the work-handler, we cannot flush
+		 * ourselves, so only flush outstanding work upon suspend!
+		 */
+		if (state != FBINFO_STATE_RUNNING)
+			flush_work(&dev_priv->fbdev_suspend_work);
+		console_lock();
+	} else {
+		/*
+		 * The console lock can be pretty contented on resume due
+		 * to all the printk activity.  Try to keep it out of the hot
+		 * path of resume if possible.
+		 */
+		WARN_ON(state != FBINFO_STATE_RUNNING);
+		if (!console_trylock()) {
+			/* Don't block our own workqueue as this can
+			 * be run in parallel with other i915.ko tasks.
+			 */
+			schedule_work(&dev_priv->fbdev_suspend_work);
+			return;
+		}
+	}
+
 	/* On resume from hibernation: If the object is shmemfs backed, it has
 	 * been restored from swap. If the object is stolen however, it will be
 	 * full of whatever garbage was left in there.
@@ -706,6 +740,7 @@ void intel_fbdev_set_suspend(struct drm_device *dev, int state)
 		memset_io(info->screen_base, 0, info->screen_size);
 
 	fb_set_suspend(info, state);
+	console_unlock();
 }
 
 void intel_fbdev_output_poll_changed(struct drm_device *dev)
diff --git a/drivers/gpu/drm/i915/intel_hdmi.c b/drivers/gpu/drm/i915/intel_hdmi.c
index f9151f6641d9..29ec1535992d 100644
--- a/drivers/gpu/drm/i915/intel_hdmi.c
+++ b/drivers/gpu/drm/i915/intel_hdmi.c
@@ -712,7 +712,8 @@ static void intel_hdmi_get_config(struct intel_encoder *encoder,
 				  struct intel_crtc_config *pipe_config)
 {
 	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&encoder->base);
-	struct drm_i915_private *dev_priv = encoder->base.dev->dev_private;
+	struct drm_device *dev = encoder->base.dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
 	u32 tmp, flags = 0;
 	int dotclock;
 
@@ -731,9 +732,13 @@ static void intel_hdmi_get_config(struct intel_encoder *encoder,
 	if (tmp & HDMI_MODE_SELECT_HDMI)
 		pipe_config->has_hdmi_sink = true;
 
-	if (tmp & HDMI_MODE_SELECT_HDMI)
+	if (tmp & SDVO_AUDIO_ENABLE)
 		pipe_config->has_audio = true;
 
+	if (!HAS_PCH_SPLIT(dev) &&
+	    tmp & HDMI_COLOR_RANGE_16_235)
+		pipe_config->limited_color_range = true;
+
 	pipe_config->adjusted_mode.flags |= flags;
 
 	if ((tmp & SDVO_COLOR_FORMAT_MASK) == HDMI_COLOR_FORMAT_12bpc)
@@ -864,10 +869,15 @@ static enum drm_mode_status
 intel_hdmi_mode_valid(struct drm_connector *connector,
 		      struct drm_display_mode *mode)
 {
-	if (mode->clock > hdmi_portclock_limit(intel_attached_hdmi(connector),
-					       true))
+	int clock = mode->clock;
+
+	if (mode->flags & DRM_MODE_FLAG_DBLCLK)
+		clock *= 2;
+
+	if (clock > hdmi_portclock_limit(intel_attached_hdmi(connector),
+					 true))
 		return MODE_CLOCK_HIGH;
-	if (mode->clock < 20000)
+	if (clock < 20000)
 		return MODE_CLOCK_LOW;
 
 	if (mode->flags & DRM_MODE_FLAG_DBLSCAN)
@@ -885,7 +895,7 @@ static bool hdmi_12bpc_possible(struct intel_crtc *crtc)
 	if (HAS_GMCH_DISPLAY(dev))
 		return false;
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		if (encoder->new_crtc != crtc)
 			continue;
 
@@ -921,6 +931,10 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 			intel_hdmi->color_range = 0;
 	}
 
+	if (adjusted_mode->flags & DRM_MODE_FLAG_DBLCLK) {
+		pipe_config->pixel_multiplier = 2;
+	}
+
 	if (intel_hdmi->color_range)
 		pipe_config->limited_color_range = true;
 
@@ -962,104 +976,117 @@ bool intel_hdmi_compute_config(struct intel_encoder *encoder,
 	return true;
 }
 
-static enum drm_connector_status
-intel_hdmi_detect(struct drm_connector *connector, bool force)
+static void
+intel_hdmi_unset_edid(struct drm_connector *connector)
 {
-	struct drm_device *dev = connector->dev;
 	struct intel_hdmi *intel_hdmi = intel_attached_hdmi(connector);
-	struct intel_digital_port *intel_dig_port =
-		hdmi_to_dig_port(intel_hdmi);
-	struct intel_encoder *intel_encoder = &intel_dig_port->base;
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	struct edid *edid;
-	enum intel_display_power_domain power_domain;
-	enum drm_connector_status status = connector_status_disconnected;
 
-	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
-		      connector->base.id, connector->name);
+	intel_hdmi->has_hdmi_sink = false;
+	intel_hdmi->has_audio = false;
+	intel_hdmi->rgb_quant_range_selectable = false;
+
+	kfree(to_intel_connector(connector)->detect_edid);
+	to_intel_connector(connector)->detect_edid = NULL;
+}
+
+static bool
+intel_hdmi_set_edid(struct drm_connector *connector)
+{
+	struct drm_i915_private *dev_priv = to_i915(connector->dev);
+	struct intel_hdmi *intel_hdmi = intel_attached_hdmi(connector);
+	struct intel_encoder *intel_encoder =
+		&hdmi_to_dig_port(intel_hdmi)->base;
+	enum intel_display_power_domain power_domain;
+	struct edid *edid;
+	bool connected = false;
 
 	power_domain = intel_display_port_power_domain(intel_encoder);
 	intel_display_power_get(dev_priv, power_domain);
 
-	intel_hdmi->has_hdmi_sink = false;
-	intel_hdmi->has_audio = false;
-	intel_hdmi->rgb_quant_range_selectable = false;
 	edid = drm_get_edid(connector,
 			    intel_gmbus_get_adapter(dev_priv,
 						    intel_hdmi->ddc_bus));
 
-	if (edid) {
-		if (edid->input & DRM_EDID_INPUT_DIGITAL) {
-			status = connector_status_connected;
-			if (intel_hdmi->force_audio != HDMI_AUDIO_OFF_DVI)
-				intel_hdmi->has_hdmi_sink =
-						drm_detect_hdmi_monitor(edid);
-			intel_hdmi->has_audio = drm_detect_monitor_audio(edid);
-			intel_hdmi->rgb_quant_range_selectable =
-				drm_rgb_quant_range_selectable(edid);
-		}
-		kfree(edid);
-	}
+	intel_display_power_put(dev_priv, power_domain);
+
+	to_intel_connector(connector)->detect_edid = edid;
+	if (edid && edid->input & DRM_EDID_INPUT_DIGITAL) {
+		intel_hdmi->rgb_quant_range_selectable =
+			drm_rgb_quant_range_selectable(edid);
 
-	if (status == connector_status_connected) {
+		intel_hdmi->has_audio = drm_detect_monitor_audio(edid);
 		if (intel_hdmi->force_audio != HDMI_AUDIO_AUTO)
 			intel_hdmi->has_audio =
-				(intel_hdmi->force_audio == HDMI_AUDIO_ON);
-		intel_encoder->type = INTEL_OUTPUT_HDMI;
+				intel_hdmi->force_audio == HDMI_AUDIO_ON;
+
+		if (intel_hdmi->force_audio != HDMI_AUDIO_OFF_DVI)
+			intel_hdmi->has_hdmi_sink =
+				drm_detect_hdmi_monitor(edid);
+
+		connected = true;
 	}
 
-	intel_display_power_put(dev_priv, power_domain);
+	return connected;
+}
+
+static enum drm_connector_status
+intel_hdmi_detect(struct drm_connector *connector, bool force)
+{
+	enum drm_connector_status status;
+
+	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
+		      connector->base.id, connector->name);
+
+	intel_hdmi_unset_edid(connector);
+
+	if (intel_hdmi_set_edid(connector)) {
+		struct intel_hdmi *intel_hdmi = intel_attached_hdmi(connector);
+
+		hdmi_to_dig_port(intel_hdmi)->base.type = INTEL_OUTPUT_HDMI;
+		status = connector_status_connected;
+	} else
+		status = connector_status_disconnected;
 
 	return status;
 }
 
-static int intel_hdmi_get_modes(struct drm_connector *connector)
+static void
+intel_hdmi_force(struct drm_connector *connector)
 {
-	struct intel_encoder *intel_encoder = intel_attached_encoder(connector);
-	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&intel_encoder->base);
-	struct drm_i915_private *dev_priv = connector->dev->dev_private;
-	enum intel_display_power_domain power_domain;
-	int ret;
+	struct intel_hdmi *intel_hdmi = intel_attached_hdmi(connector);
 
-	/* We should parse the EDID data and find out if it's an HDMI sink so
-	 * we can send audio to it.
-	 */
+	DRM_DEBUG_KMS("[CONNECTOR:%d:%s]\n",
+		      connector->base.id, connector->name);
 
-	power_domain = intel_display_port_power_domain(intel_encoder);
-	intel_display_power_get(dev_priv, power_domain);
+	intel_hdmi_unset_edid(connector);
 
-	ret = intel_ddc_get_modes(connector,
-				   intel_gmbus_get_adapter(dev_priv,
-							   intel_hdmi->ddc_bus));
+	if (connector->status != connector_status_connected)
+		return;
 
-	intel_display_power_put(dev_priv, power_domain);
+	intel_hdmi_set_edid(connector);
+	hdmi_to_dig_port(intel_hdmi)->base.type = INTEL_OUTPUT_HDMI;
+}
+
+static int intel_hdmi_get_modes(struct drm_connector *connector)
+{
+	struct edid *edid;
 
-	return ret;
+	edid = to_intel_connector(connector)->detect_edid;
+	if (edid == NULL)
+		return 0;
+
+	return intel_connector_update_modes(connector, edid);
 }
 
 static bool
 intel_hdmi_detect_audio(struct drm_connector *connector)
 {
-	struct intel_encoder *intel_encoder = intel_attached_encoder(connector);
-	struct intel_hdmi *intel_hdmi = enc_to_intel_hdmi(&intel_encoder->base);
-	struct drm_i915_private *dev_priv = connector->dev->dev_private;
-	enum intel_display_power_domain power_domain;
-	struct edid *edid;
 	bool has_audio = false;
+	struct edid *edid;
 
-	power_domain = intel_display_port_power_domain(intel_encoder);
-	intel_display_power_get(dev_priv, power_domain);
-
-	edid = drm_get_edid(connector,
-			    intel_gmbus_get_adapter(dev_priv,
-						    intel_hdmi->ddc_bus));
-	if (edid) {
-		if (edid->input & DRM_EDID_INPUT_DIGITAL)
-			has_audio = drm_detect_monitor_audio(edid);
-		kfree(edid);
-	}
-
-	intel_display_power_put(dev_priv, power_domain);
+	edid = to_intel_connector(connector)->detect_edid;
+	if (edid && edid->input & DRM_EDID_INPUT_DIGITAL)
+		has_audio = drm_detect_monitor_audio(edid);
 
 	return has_audio;
 }
@@ -1260,6 +1287,8 @@ static void chv_hdmi_pre_pll_enable(struct intel_encoder *encoder)
 	enum pipe pipe = intel_crtc->pipe;
 	u32 val;
 
+	intel_hdmi_prepare(encoder);
+
 	mutex_lock(&dev_priv->dpio_lock);
 
 	/* program left/right clock distribution */
@@ -1429,8 +1458,8 @@ static void chv_hdmi_pre_enable(struct intel_encoder *encoder)
 
 	for (i = 0; i < 4; i++) {
 		val = vlv_dpio_read(dev_priv, pipe, CHV_TX_DW2(ch, i));
-		val &= ~DPIO_SWING_MARGIN_MASK;
-		val |= 102 << DPIO_SWING_MARGIN_SHIFT;
+		val &= ~DPIO_SWING_MARGIN000_MASK;
+		val |= 102 << DPIO_SWING_MARGIN000_SHIFT;
 		vlv_dpio_write(dev_priv, pipe, CHV_TX_DW2(ch, i), val);
 	}
 
@@ -1477,6 +1506,7 @@ static void chv_hdmi_pre_enable(struct intel_encoder *encoder)
 
 static void intel_hdmi_destroy(struct drm_connector *connector)
 {
+	kfree(to_intel_connector(connector)->detect_edid);
 	drm_connector_cleanup(connector);
 	kfree(connector);
 }
@@ -1484,6 +1514,7 @@ static void intel_hdmi_destroy(struct drm_connector *connector)
 static const struct drm_connector_funcs intel_hdmi_connector_funcs = {
 	.dpms = intel_connector_dpms,
 	.detect = intel_hdmi_detect,
+	.force = intel_hdmi_force,
 	.fill_modes = drm_helper_probe_single_connector_modes,
 	.set_property = intel_hdmi_set_property,
 	.destroy = intel_hdmi_destroy,
diff --git a/drivers/gpu/drm/i915/intel_lrc.c b/drivers/gpu/drm/i915/intel_lrc.c
new file mode 100644
index 000000000000..bafd38b5703e
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_lrc.c
@@ -0,0 +1,1766 @@
+/*
+ * Copyright © 2014 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ *
+ * Authors:
+ *    Ben Widawsky <ben@bwidawsk.net>
+ *    Michel Thierry <michel.thierry@intel.com>
+ *    Thomas Daniel <thomas.daniel@intel.com>
+ *    Oscar Mateo <oscar.mateo@intel.com>
+ *
+ */
+
+/**
+ * DOC: Logical Rings, Logical Ring Contexts and Execlists
+ *
+ * Motivation:
+ * GEN8 brings an expansion of the HW contexts: "Logical Ring Contexts".
+ * These expanded contexts enable a number of new abilities, especially
+ * "Execlists" (also implemented in this file).
+ *
+ * One of the main differences with the legacy HW contexts is that logical
+ * ring contexts incorporate many more things to the context's state, like
+ * PDPs or ringbuffer control registers:
+ *
+ * The reason why PDPs are included in the context is straightforward: as
+ * PPGTTs (per-process GTTs) are actually per-context, having the PDPs
+ * contained there mean you don't need to do a ppgtt->switch_mm yourself,
+ * instead, the GPU will do it for you on the context switch.
+ *
+ * But, what about the ringbuffer control registers (head, tail, etc..)?
+ * shouldn't we just need a set of those per engine command streamer? This is
+ * where the name "Logical Rings" starts to make sense: by virtualizing the
+ * rings, the engine cs shifts to a new "ring buffer" with every context
+ * switch. When you want to submit a workload to the GPU you: A) choose your
+ * context, B) find its appropriate virtualized ring, C) write commands to it
+ * and then, finally, D) tell the GPU to switch to that context.
+ *
+ * Instead of the legacy MI_SET_CONTEXT, the way you tell the GPU to switch
+ * to a contexts is via a context execution list, ergo "Execlists".
+ *
+ * LRC implementation:
+ * Regarding the creation of contexts, we have:
+ *
+ * - One global default context.
+ * - One local default context for each opened fd.
+ * - One local extra context for each context create ioctl call.
+ *
+ * Now that ringbuffers belong per-context (and not per-engine, like before)
+ * and that contexts are uniquely tied to a given engine (and not reusable,
+ * like before) we need:
+ *
+ * - One ringbuffer per-engine inside each context.
+ * - One backing object per-engine inside each context.
+ *
+ * The global default context starts its life with these new objects fully
+ * allocated and populated. The local default context for each opened fd is
+ * more complex, because we don't know at creation time which engine is going
+ * to use them. To handle this, we have implemented a deferred creation of LR
+ * contexts:
+ *
+ * The local context starts its life as a hollow or blank holder, that only
+ * gets populated for a given engine once we receive an execbuffer. If later
+ * on we receive another execbuffer ioctl for the same context but a different
+ * engine, we allocate/populate a new ringbuffer and context backing object and
+ * so on.
+ *
+ * Finally, regarding local contexts created using the ioctl call: as they are
+ * only allowed with the render ring, we can allocate & populate them right
+ * away (no need to defer anything, at least for now).
+ *
+ * Execlists implementation:
+ * Execlists are the new method by which, on gen8+ hardware, workloads are
+ * submitted for execution (as opposed to the legacy, ringbuffer-based, method).
+ * This method works as follows:
+ *
+ * When a request is committed, its commands (the BB start and any leading or
+ * trailing commands, like the seqno breadcrumbs) are placed in the ringbuffer
+ * for the appropriate context. The tail pointer in the hardware context is not
+ * updated at this time, but instead, kept by the driver in the ringbuffer
+ * structure. A structure representing this request is added to a request queue
+ * for the appropriate engine: this structure contains a copy of the context's
+ * tail after the request was written to the ring buffer and a pointer to the
+ * context itself.
+ *
+ * If the engine's request queue was empty before the request was added, the
+ * queue is processed immediately. Otherwise the queue will be processed during
+ * a context switch interrupt. In any case, elements on the queue will get sent
+ * (in pairs) to the GPU's ExecLists Submit Port (ELSP, for short) with a
+ * globally unique 20-bits submission ID.
+ *
+ * When execution of a request completes, the GPU updates the context status
+ * buffer with a context complete event and generates a context switch interrupt.
+ * During the interrupt handling, the driver examines the events in the buffer:
+ * for each context complete event, if the announced ID matches that on the head
+ * of the request queue, then that request is retired and removed from the queue.
+ *
+ * After processing, if any requests were retired and the queue is not empty
+ * then a new execution list can be submitted. The two requests at the front of
+ * the queue are next to be submitted but since a context may not occur twice in
+ * an execution list, if subsequent requests have the same ID as the first then
+ * the two requests must be combined. This is done simply by discarding requests
+ * at the head of the queue until either only one requests is left (in which case
+ * we use a NULL second context) or the first two requests have unique IDs.
+ *
+ * By always executing the first two requests in the queue the driver ensures
+ * that the GPU is kept as busy as possible. In the case where a single context
+ * completes but a second context is still executing, the request for this second
+ * context will be at the head of the queue when we remove the first one. This
+ * request will then be resubmitted along with a new request for a different context,
+ * which will cause the hardware to continue executing the second request and queue
+ * the new request (the GPU detects the condition of a context getting preempted
+ * with the same context and optimizes the context switch flow by not doing
+ * preemption, but just sampling the new tail pointer).
+ *
+ */
+
+#include <drm/drmP.h>
+#include <drm/i915_drm.h>
+#include "i915_drv.h"
+
+#define GEN8_LR_CONTEXT_RENDER_SIZE (20 * PAGE_SIZE)
+#define GEN8_LR_CONTEXT_OTHER_SIZE (2 * PAGE_SIZE)
+
+#define GEN8_LR_CONTEXT_ALIGN 4096
+
+#define RING_EXECLIST_QFULL		(1 << 0x2)
+#define RING_EXECLIST1_VALID		(1 << 0x3)
+#define RING_EXECLIST0_VALID		(1 << 0x4)
+#define RING_EXECLIST_ACTIVE_STATUS	(3 << 0xE)
+#define RING_EXECLIST1_ACTIVE		(1 << 0x11)
+#define RING_EXECLIST0_ACTIVE		(1 << 0x12)
+
+#define GEN8_CTX_STATUS_IDLE_ACTIVE	(1 << 0)
+#define GEN8_CTX_STATUS_PREEMPTED	(1 << 1)
+#define GEN8_CTX_STATUS_ELEMENT_SWITCH	(1 << 2)
+#define GEN8_CTX_STATUS_ACTIVE_IDLE	(1 << 3)
+#define GEN8_CTX_STATUS_COMPLETE	(1 << 4)
+#define GEN8_CTX_STATUS_LITE_RESTORE	(1 << 15)
+
+#define CTX_LRI_HEADER_0		0x01
+#define CTX_CONTEXT_CONTROL		0x02
+#define CTX_RING_HEAD			0x04
+#define CTX_RING_TAIL			0x06
+#define CTX_RING_BUFFER_START		0x08
+#define CTX_RING_BUFFER_CONTROL		0x0a
+#define CTX_BB_HEAD_U			0x0c
+#define CTX_BB_HEAD_L			0x0e
+#define CTX_BB_STATE			0x10
+#define CTX_SECOND_BB_HEAD_U		0x12
+#define CTX_SECOND_BB_HEAD_L		0x14
+#define CTX_SECOND_BB_STATE		0x16
+#define CTX_BB_PER_CTX_PTR		0x18
+#define CTX_RCS_INDIRECT_CTX		0x1a
+#define CTX_RCS_INDIRECT_CTX_OFFSET	0x1c
+#define CTX_LRI_HEADER_1		0x21
+#define CTX_CTX_TIMESTAMP		0x22
+#define CTX_PDP3_UDW			0x24
+#define CTX_PDP3_LDW			0x26
+#define CTX_PDP2_UDW			0x28
+#define CTX_PDP2_LDW			0x2a
+#define CTX_PDP1_UDW			0x2c
+#define CTX_PDP1_LDW			0x2e
+#define CTX_PDP0_UDW			0x30
+#define CTX_PDP0_LDW			0x32
+#define CTX_LRI_HEADER_2		0x41
+#define CTX_R_PWR_CLK_STATE		0x42
+#define CTX_GPGPU_CSR_BASE_ADDRESS	0x44
+
+#define GEN8_CTX_VALID (1<<0)
+#define GEN8_CTX_FORCE_PD_RESTORE (1<<1)
+#define GEN8_CTX_FORCE_RESTORE (1<<2)
+#define GEN8_CTX_L3LLC_COHERENT (1<<5)
+#define GEN8_CTX_PRIVILEGE (1<<8)
+enum {
+	ADVANCED_CONTEXT = 0,
+	LEGACY_CONTEXT,
+	ADVANCED_AD_CONTEXT,
+	LEGACY_64B_CONTEXT
+};
+#define GEN8_CTX_MODE_SHIFT 3
+enum {
+	FAULT_AND_HANG = 0,
+	FAULT_AND_HALT, /* Debug only */
+	FAULT_AND_STREAM,
+	FAULT_AND_CONTINUE /* Unsupported */
+};
+#define GEN8_CTX_ID_SHIFT 32
+
+/**
+ * intel_sanitize_enable_execlists() - sanitize i915.enable_execlists
+ * @dev: DRM device.
+ * @enable_execlists: value of i915.enable_execlists module parameter.
+ *
+ * Only certain platforms support Execlists (the prerequisites being
+ * support for Logical Ring Contexts and Aliasing PPGTT or better),
+ * and only when enabled via module parameter.
+ *
+ * Return: 1 if Execlists is supported and has to be enabled.
+ */
+int intel_sanitize_enable_execlists(struct drm_device *dev, int enable_execlists)
+{
+	WARN_ON(i915.enable_ppgtt == -1);
+
+	if (enable_execlists == 0)
+		return 0;
+
+	if (HAS_LOGICAL_RING_CONTEXTS(dev) && USES_PPGTT(dev) &&
+	    i915.use_mmio_flip >= 0)
+		return 1;
+
+	return 0;
+}
+
+/**
+ * intel_execlists_ctx_id() - get the Execlists Context ID
+ * @ctx_obj: Logical Ring Context backing object.
+ *
+ * Do not confuse with ctx->id! Unfortunately we have a name overload
+ * here: the old context ID we pass to userspace as a handler so that
+ * they can refer to a context, and the new context ID we pass to the
+ * ELSP so that the GPU can inform us of the context status via
+ * interrupts.
+ *
+ * Return: 20-bits globally unique context ID.
+ */
+u32 intel_execlists_ctx_id(struct drm_i915_gem_object *ctx_obj)
+{
+	u32 lrca = i915_gem_obj_ggtt_offset(ctx_obj);
+
+	/* LRCA is required to be 4K aligned so the more significant 20 bits
+	 * are globally unique */
+	return lrca >> 12;
+}
+
+static uint64_t execlists_ctx_descriptor(struct drm_i915_gem_object *ctx_obj)
+{
+	uint64_t desc;
+	uint64_t lrca = i915_gem_obj_ggtt_offset(ctx_obj);
+
+	WARN_ON(lrca & 0xFFFFFFFF00000FFFULL);
+
+	desc = GEN8_CTX_VALID;
+	desc |= LEGACY_CONTEXT << GEN8_CTX_MODE_SHIFT;
+	desc |= GEN8_CTX_L3LLC_COHERENT;
+	desc |= GEN8_CTX_PRIVILEGE;
+	desc |= lrca;
+	desc |= (u64)intel_execlists_ctx_id(ctx_obj) << GEN8_CTX_ID_SHIFT;
+
+	/* TODO: WaDisableLiteRestore when we start using semaphore
+	 * signalling between Command Streamers */
+	/* desc |= GEN8_CTX_FORCE_RESTORE; */
+
+	return desc;
+}
+
+static void execlists_elsp_write(struct intel_engine_cs *ring,
+				 struct drm_i915_gem_object *ctx_obj0,
+				 struct drm_i915_gem_object *ctx_obj1)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	uint64_t temp = 0;
+	uint32_t desc[4];
+	unsigned long flags;
+
+	/* XXX: You must always write both descriptors in the order below. */
+	if (ctx_obj1)
+		temp = execlists_ctx_descriptor(ctx_obj1);
+	else
+		temp = 0;
+	desc[1] = (u32)(temp >> 32);
+	desc[0] = (u32)temp;
+
+	temp = execlists_ctx_descriptor(ctx_obj0);
+	desc[3] = (u32)(temp >> 32);
+	desc[2] = (u32)temp;
+
+	/* Set Force Wakeup bit to prevent GT from entering C6 while ELSP writes
+	 * are in progress.
+	 *
+	 * The other problem is that we can't just call gen6_gt_force_wake_get()
+	 * because that function calls intel_runtime_pm_get(), which might sleep.
+	 * Instead, we do the runtime_pm_get/put when creating/destroying requests.
+	 */
+	spin_lock_irqsave(&dev_priv->uncore.lock, flags);
+	if (IS_CHERRYVIEW(dev_priv->dev)) {
+		if (dev_priv->uncore.fw_rendercount++ == 0)
+			dev_priv->uncore.funcs.force_wake_get(dev_priv,
+							      FORCEWAKE_RENDER);
+		if (dev_priv->uncore.fw_mediacount++ == 0)
+			dev_priv->uncore.funcs.force_wake_get(dev_priv,
+							      FORCEWAKE_MEDIA);
+	} else {
+		if (dev_priv->uncore.forcewake_count++ == 0)
+			dev_priv->uncore.funcs.force_wake_get(dev_priv,
+							      FORCEWAKE_ALL);
+	}
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, flags);
+
+	I915_WRITE(RING_ELSP(ring), desc[1]);
+	I915_WRITE(RING_ELSP(ring), desc[0]);
+	I915_WRITE(RING_ELSP(ring), desc[3]);
+	/* The context is automatically loaded after the following */
+	I915_WRITE(RING_ELSP(ring), desc[2]);
+
+	/* ELSP is a wo register, so use another nearby reg for posting instead */
+	POSTING_READ(RING_EXECLIST_STATUS(ring));
+
+	/* Release Force Wakeup (see the big comment above). */
+	spin_lock_irqsave(&dev_priv->uncore.lock, flags);
+	if (IS_CHERRYVIEW(dev_priv->dev)) {
+		if (--dev_priv->uncore.fw_rendercount == 0)
+			dev_priv->uncore.funcs.force_wake_put(dev_priv,
+							      FORCEWAKE_RENDER);
+		if (--dev_priv->uncore.fw_mediacount == 0)
+			dev_priv->uncore.funcs.force_wake_put(dev_priv,
+							      FORCEWAKE_MEDIA);
+	} else {
+		if (--dev_priv->uncore.forcewake_count == 0)
+			dev_priv->uncore.funcs.force_wake_put(dev_priv,
+							      FORCEWAKE_ALL);
+	}
+
+	spin_unlock_irqrestore(&dev_priv->uncore.lock, flags);
+}
+
+static int execlists_ctx_write_tail(struct drm_i915_gem_object *ctx_obj, u32 tail)
+{
+	struct page *page;
+	uint32_t *reg_state;
+
+	page = i915_gem_object_get_page(ctx_obj, 1);
+	reg_state = kmap_atomic(page);
+
+	reg_state[CTX_RING_TAIL+1] = tail;
+
+	kunmap_atomic(reg_state);
+
+	return 0;
+}
+
+static int execlists_submit_context(struct intel_engine_cs *ring,
+				    struct intel_context *to0, u32 tail0,
+				    struct intel_context *to1, u32 tail1)
+{
+	struct drm_i915_gem_object *ctx_obj0;
+	struct drm_i915_gem_object *ctx_obj1 = NULL;
+
+	ctx_obj0 = to0->engine[ring->id].state;
+	BUG_ON(!ctx_obj0);
+	WARN_ON(!i915_gem_obj_is_pinned(ctx_obj0));
+
+	execlists_ctx_write_tail(ctx_obj0, tail0);
+
+	if (to1) {
+		ctx_obj1 = to1->engine[ring->id].state;
+		BUG_ON(!ctx_obj1);
+		WARN_ON(!i915_gem_obj_is_pinned(ctx_obj1));
+
+		execlists_ctx_write_tail(ctx_obj1, tail1);
+	}
+
+	execlists_elsp_write(ring, ctx_obj0, ctx_obj1);
+
+	return 0;
+}
+
+static void execlists_context_unqueue(struct intel_engine_cs *ring)
+{
+	struct intel_ctx_submit_request *req0 = NULL, *req1 = NULL;
+	struct intel_ctx_submit_request *cursor = NULL, *tmp = NULL;
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+
+	assert_spin_locked(&ring->execlist_lock);
+
+	if (list_empty(&ring->execlist_queue))
+		return;
+
+	/* Try to read in pairs */
+	list_for_each_entry_safe(cursor, tmp, &ring->execlist_queue,
+				 execlist_link) {
+		if (!req0) {
+			req0 = cursor;
+		} else if (req0->ctx == cursor->ctx) {
+			/* Same ctx: ignore first request, as second request
+			 * will update tail past first request's workload */
+			cursor->elsp_submitted = req0->elsp_submitted;
+			list_del(&req0->execlist_link);
+			queue_work(dev_priv->wq, &req0->work);
+			req0 = cursor;
+		} else {
+			req1 = cursor;
+			break;
+		}
+	}
+
+	WARN_ON(req1 && req1->elsp_submitted);
+
+	WARN_ON(execlists_submit_context(ring, req0->ctx, req0->tail,
+					 req1 ? req1->ctx : NULL,
+					 req1 ? req1->tail : 0));
+
+	req0->elsp_submitted++;
+	if (req1)
+		req1->elsp_submitted++;
+}
+
+static bool execlists_check_remove_request(struct intel_engine_cs *ring,
+					   u32 request_id)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	struct intel_ctx_submit_request *head_req;
+
+	assert_spin_locked(&ring->execlist_lock);
+
+	head_req = list_first_entry_or_null(&ring->execlist_queue,
+					    struct intel_ctx_submit_request,
+					    execlist_link);
+
+	if (head_req != NULL) {
+		struct drm_i915_gem_object *ctx_obj =
+				head_req->ctx->engine[ring->id].state;
+		if (intel_execlists_ctx_id(ctx_obj) == request_id) {
+			WARN(head_req->elsp_submitted == 0,
+			     "Never submitted head request\n");
+
+			if (--head_req->elsp_submitted <= 0) {
+				list_del(&head_req->execlist_link);
+				queue_work(dev_priv->wq, &head_req->work);
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+/**
+ * intel_execlists_handle_ctx_events() - handle Context Switch interrupts
+ * @ring: Engine Command Streamer to handle.
+ *
+ * Check the unread Context Status Buffers and manage the submission of new
+ * contexts to the ELSP accordingly.
+ */
+void intel_execlists_handle_ctx_events(struct intel_engine_cs *ring)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	u32 status_pointer;
+	u8 read_pointer;
+	u8 write_pointer;
+	u32 status;
+	u32 status_id;
+	u32 submit_contexts = 0;
+
+	status_pointer = I915_READ(RING_CONTEXT_STATUS_PTR(ring));
+
+	read_pointer = ring->next_context_status_buffer;
+	write_pointer = status_pointer & 0x07;
+	if (read_pointer > write_pointer)
+		write_pointer += 6;
+
+	spin_lock(&ring->execlist_lock);
+
+	while (read_pointer < write_pointer) {
+		read_pointer++;
+		status = I915_READ(RING_CONTEXT_STATUS_BUF(ring) +
+				(read_pointer % 6) * 8);
+		status_id = I915_READ(RING_CONTEXT_STATUS_BUF(ring) +
+				(read_pointer % 6) * 8 + 4);
+
+		if (status & GEN8_CTX_STATUS_PREEMPTED) {
+			if (status & GEN8_CTX_STATUS_LITE_RESTORE) {
+				if (execlists_check_remove_request(ring, status_id))
+					WARN(1, "Lite Restored request removed from queue\n");
+			} else
+				WARN(1, "Preemption without Lite Restore\n");
+		}
+
+		 if ((status & GEN8_CTX_STATUS_ACTIVE_IDLE) ||
+		     (status & GEN8_CTX_STATUS_ELEMENT_SWITCH)) {
+			if (execlists_check_remove_request(ring, status_id))
+				submit_contexts++;
+		}
+	}
+
+	if (submit_contexts != 0)
+		execlists_context_unqueue(ring);
+
+	spin_unlock(&ring->execlist_lock);
+
+	WARN(submit_contexts > 2, "More than two context complete events?\n");
+	ring->next_context_status_buffer = write_pointer % 6;
+
+	I915_WRITE(RING_CONTEXT_STATUS_PTR(ring),
+		   ((u32)ring->next_context_status_buffer & 0x07) << 8);
+}
+
+static void execlists_free_request_task(struct work_struct *work)
+{
+	struct intel_ctx_submit_request *req =
+		container_of(work, struct intel_ctx_submit_request, work);
+	struct drm_device *dev = req->ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	intel_runtime_pm_put(dev_priv);
+
+	mutex_lock(&dev->struct_mutex);
+	i915_gem_context_unreference(req->ctx);
+	mutex_unlock(&dev->struct_mutex);
+
+	kfree(req);
+}
+
+static int execlists_context_queue(struct intel_engine_cs *ring,
+				   struct intel_context *to,
+				   u32 tail)
+{
+	struct intel_ctx_submit_request *req = NULL, *cursor;
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	unsigned long flags;
+	int num_elements = 0;
+
+	req = kzalloc(sizeof(*req), GFP_KERNEL);
+	if (req == NULL)
+		return -ENOMEM;
+	req->ctx = to;
+	i915_gem_context_reference(req->ctx);
+	req->ring = ring;
+	req->tail = tail;
+	INIT_WORK(&req->work, execlists_free_request_task);
+
+	intel_runtime_pm_get(dev_priv);
+
+	spin_lock_irqsave(&ring->execlist_lock, flags);
+
+	list_for_each_entry(cursor, &ring->execlist_queue, execlist_link)
+		if (++num_elements > 2)
+			break;
+
+	if (num_elements > 2) {
+		struct intel_ctx_submit_request *tail_req;
+
+		tail_req = list_last_entry(&ring->execlist_queue,
+					   struct intel_ctx_submit_request,
+					   execlist_link);
+
+		if (to == tail_req->ctx) {
+			WARN(tail_req->elsp_submitted != 0,
+			     "More than 2 already-submitted reqs queued\n");
+			list_del(&tail_req->execlist_link);
+			queue_work(dev_priv->wq, &tail_req->work);
+		}
+	}
+
+	list_add_tail(&req->execlist_link, &ring->execlist_queue);
+	if (num_elements == 0)
+		execlists_context_unqueue(ring);
+
+	spin_unlock_irqrestore(&ring->execlist_lock, flags);
+
+	return 0;
+}
+
+static int logical_ring_invalidate_all_caches(struct intel_ringbuffer *ringbuf)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	uint32_t flush_domains;
+	int ret;
+
+	flush_domains = 0;
+	if (ring->gpu_caches_dirty)
+		flush_domains = I915_GEM_GPU_DOMAINS;
+
+	ret = ring->emit_flush(ringbuf, I915_GEM_GPU_DOMAINS, flush_domains);
+	if (ret)
+		return ret;
+
+	ring->gpu_caches_dirty = false;
+	return 0;
+}
+
+static int execlists_move_to_gpu(struct intel_ringbuffer *ringbuf,
+				 struct list_head *vmas)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	struct i915_vma *vma;
+	uint32_t flush_domains = 0;
+	bool flush_chipset = false;
+	int ret;
+
+	list_for_each_entry(vma, vmas, exec_list) {
+		struct drm_i915_gem_object *obj = vma->obj;
+
+		ret = i915_gem_object_sync(obj, ring);
+		if (ret)
+			return ret;
+
+		if (obj->base.write_domain & I915_GEM_DOMAIN_CPU)
+			flush_chipset |= i915_gem_clflush_object(obj, false);
+
+		flush_domains |= obj->base.write_domain;
+	}
+
+	if (flush_domains & I915_GEM_DOMAIN_GTT)
+		wmb();
+
+	/* Unconditionally invalidate gpu caches and ensure that we do flush
+	 * any residual writes from the previous batch.
+	 */
+	return logical_ring_invalidate_all_caches(ringbuf);
+}
+
+/**
+ * execlists_submission() - submit a batchbuffer for execution, Execlists style
+ * @dev: DRM device.
+ * @file: DRM file.
+ * @ring: Engine Command Streamer to submit to.
+ * @ctx: Context to employ for this submission.
+ * @args: execbuffer call arguments.
+ * @vmas: list of vmas.
+ * @batch_obj: the batchbuffer to submit.
+ * @exec_start: batchbuffer start virtual address pointer.
+ * @flags: translated execbuffer call flags.
+ *
+ * This is the evil twin version of i915_gem_ringbuffer_submission. It abstracts
+ * away the submission details of the execbuffer ioctl call.
+ *
+ * Return: non-zero if the submission fails.
+ */
+int intel_execlists_submission(struct drm_device *dev, struct drm_file *file,
+			       struct intel_engine_cs *ring,
+			       struct intel_context *ctx,
+			       struct drm_i915_gem_execbuffer2 *args,
+			       struct list_head *vmas,
+			       struct drm_i915_gem_object *batch_obj,
+			       u64 exec_start, u32 flags)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_ringbuffer *ringbuf = ctx->engine[ring->id].ringbuf;
+	int instp_mode;
+	u32 instp_mask;
+	int ret;
+
+	instp_mode = args->flags & I915_EXEC_CONSTANTS_MASK;
+	instp_mask = I915_EXEC_CONSTANTS_MASK;
+	switch (instp_mode) {
+	case I915_EXEC_CONSTANTS_REL_GENERAL:
+	case I915_EXEC_CONSTANTS_ABSOLUTE:
+	case I915_EXEC_CONSTANTS_REL_SURFACE:
+		if (instp_mode != 0 && ring != &dev_priv->ring[RCS]) {
+			DRM_DEBUG("non-0 rel constants mode on non-RCS\n");
+			return -EINVAL;
+		}
+
+		if (instp_mode != dev_priv->relative_constants_mode) {
+			if (instp_mode == I915_EXEC_CONSTANTS_REL_SURFACE) {
+				DRM_DEBUG("rel surface constants mode invalid on gen5+\n");
+				return -EINVAL;
+			}
+
+			/* The HW changed the meaning on this bit on gen6 */
+			instp_mask &= ~I915_EXEC_CONSTANTS_REL_SURFACE;
+		}
+		break;
+	default:
+		DRM_DEBUG("execbuf with unknown constants: %d\n", instp_mode);
+		return -EINVAL;
+	}
+
+	if (args->num_cliprects != 0) {
+		DRM_DEBUG("clip rectangles are only valid on pre-gen5\n");
+		return -EINVAL;
+	} else {
+		if (args->DR4 == 0xffffffff) {
+			DRM_DEBUG("UXA submitting garbage DR4, fixing up\n");
+			args->DR4 = 0;
+		}
+
+		if (args->DR1 || args->DR4 || args->cliprects_ptr) {
+			DRM_DEBUG("0 cliprects but dirt in cliprects fields\n");
+			return -EINVAL;
+		}
+	}
+
+	if (args->flags & I915_EXEC_GEN7_SOL_RESET) {
+		DRM_DEBUG("sol reset is gen7 only\n");
+		return -EINVAL;
+	}
+
+	ret = execlists_move_to_gpu(ringbuf, vmas);
+	if (ret)
+		return ret;
+
+	if (ring == &dev_priv->ring[RCS] &&
+	    instp_mode != dev_priv->relative_constants_mode) {
+		ret = intel_logical_ring_begin(ringbuf, 4);
+		if (ret)
+			return ret;
+
+		intel_logical_ring_emit(ringbuf, MI_NOOP);
+		intel_logical_ring_emit(ringbuf, MI_LOAD_REGISTER_IMM(1));
+		intel_logical_ring_emit(ringbuf, INSTPM);
+		intel_logical_ring_emit(ringbuf, instp_mask << 16 | instp_mode);
+		intel_logical_ring_advance(ringbuf);
+
+		dev_priv->relative_constants_mode = instp_mode;
+	}
+
+	ret = ring->emit_bb_start(ringbuf, exec_start, flags);
+	if (ret)
+		return ret;
+
+	i915_gem_execbuffer_move_to_active(vmas, ring);
+	i915_gem_execbuffer_retire_commands(dev, file, ring, batch_obj);
+
+	return 0;
+}
+
+void intel_logical_ring_stop(struct intel_engine_cs *ring)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+	int ret;
+
+	if (!intel_ring_initialized(ring))
+		return;
+
+	ret = intel_ring_idle(ring);
+	if (ret && !i915_reset_in_progress(&to_i915(ring->dev)->gpu_error))
+		DRM_ERROR("failed to quiesce %s whilst cleaning up: %d\n",
+			  ring->name, ret);
+
+	/* TODO: Is this correct with Execlists enabled? */
+	I915_WRITE_MODE(ring, _MASKED_BIT_ENABLE(STOP_RING));
+	if (wait_for_atomic((I915_READ_MODE(ring) & MODE_IDLE) != 0, 1000)) {
+		DRM_ERROR("%s :timed out trying to stop ring\n", ring->name);
+		return;
+	}
+	I915_WRITE_MODE(ring, _MASKED_BIT_DISABLE(STOP_RING));
+}
+
+int logical_ring_flush_all_caches(struct intel_ringbuffer *ringbuf)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	int ret;
+
+	if (!ring->gpu_caches_dirty)
+		return 0;
+
+	ret = ring->emit_flush(ringbuf, 0, I915_GEM_GPU_DOMAINS);
+	if (ret)
+		return ret;
+
+	ring->gpu_caches_dirty = false;
+	return 0;
+}
+
+/**
+ * intel_logical_ring_advance_and_submit() - advance the tail and submit the workload
+ * @ringbuf: Logical Ringbuffer to advance.
+ *
+ * The tail is updated in our logical ringbuffer struct, not in the actual context. What
+ * really happens during submission is that the context and current tail will be placed
+ * on a queue waiting for the ELSP to be ready to accept a new context submission. At that
+ * point, the tail *inside* the context is updated and the ELSP written to.
+ */
+void intel_logical_ring_advance_and_submit(struct intel_ringbuffer *ringbuf)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	struct intel_context *ctx = ringbuf->FIXME_lrc_ctx;
+
+	intel_logical_ring_advance(ringbuf);
+
+	if (intel_ring_stopped(ring))
+		return;
+
+	execlists_context_queue(ring, ctx, ringbuf->tail);
+}
+
+static int logical_ring_alloc_seqno(struct intel_engine_cs *ring,
+				    struct intel_context *ctx)
+{
+	if (ring->outstanding_lazy_seqno)
+		return 0;
+
+	if (ring->preallocated_lazy_request == NULL) {
+		struct drm_i915_gem_request *request;
+
+		request = kmalloc(sizeof(*request), GFP_KERNEL);
+		if (request == NULL)
+			return -ENOMEM;
+
+		/* Hold a reference to the context this request belongs to
+		 * (we will need it when the time comes to emit/retire the
+		 * request).
+		 */
+		request->ctx = ctx;
+		i915_gem_context_reference(request->ctx);
+
+		ring->preallocated_lazy_request = request;
+	}
+
+	return i915_gem_get_seqno(ring->dev, &ring->outstanding_lazy_seqno);
+}
+
+static int logical_ring_wait_request(struct intel_ringbuffer *ringbuf,
+				     int bytes)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	struct drm_i915_gem_request *request;
+	u32 seqno = 0;
+	int ret;
+
+	if (ringbuf->last_retired_head != -1) {
+		ringbuf->head = ringbuf->last_retired_head;
+		ringbuf->last_retired_head = -1;
+
+		ringbuf->space = intel_ring_space(ringbuf);
+		if (ringbuf->space >= bytes)
+			return 0;
+	}
+
+	list_for_each_entry(request, &ring->request_list, list) {
+		if (__intel_ring_space(request->tail, ringbuf->tail,
+				       ringbuf->size) >= bytes) {
+			seqno = request->seqno;
+			break;
+		}
+	}
+
+	if (seqno == 0)
+		return -ENOSPC;
+
+	ret = i915_wait_seqno(ring, seqno);
+	if (ret)
+		return ret;
+
+	i915_gem_retire_requests_ring(ring);
+	ringbuf->head = ringbuf->last_retired_head;
+	ringbuf->last_retired_head = -1;
+
+	ringbuf->space = intel_ring_space(ringbuf);
+	return 0;
+}
+
+static int logical_ring_wait_for_space(struct intel_ringbuffer *ringbuf,
+				       int bytes)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long end;
+	int ret;
+
+	ret = logical_ring_wait_request(ringbuf, bytes);
+	if (ret != -ENOSPC)
+		return ret;
+
+	/* Force the context submission in case we have been skipping it */
+	intel_logical_ring_advance_and_submit(ringbuf);
+
+	/* With GEM the hangcheck timer should kick us out of the loop,
+	 * leaving it early runs the risk of corrupting GEM state (due
+	 * to running on almost untested codepaths). But on resume
+	 * timers don't work yet, so prevent a complete hang in that
+	 * case by choosing an insanely large timeout. */
+	end = jiffies + 60 * HZ;
+
+	do {
+		ringbuf->head = I915_READ_HEAD(ring);
+		ringbuf->space = intel_ring_space(ringbuf);
+		if (ringbuf->space >= bytes) {
+			ret = 0;
+			break;
+		}
+
+		msleep(1);
+
+		if (dev_priv->mm.interruptible && signal_pending(current)) {
+			ret = -ERESTARTSYS;
+			break;
+		}
+
+		ret = i915_gem_check_wedge(&dev_priv->gpu_error,
+					   dev_priv->mm.interruptible);
+		if (ret)
+			break;
+
+		if (time_after(jiffies, end)) {
+			ret = -EBUSY;
+			break;
+		}
+	} while (1);
+
+	return ret;
+}
+
+static int logical_ring_wrap_buffer(struct intel_ringbuffer *ringbuf)
+{
+	uint32_t __iomem *virt;
+	int rem = ringbuf->size - ringbuf->tail;
+
+	if (ringbuf->space < rem) {
+		int ret = logical_ring_wait_for_space(ringbuf, rem);
+
+		if (ret)
+			return ret;
+	}
+
+	virt = ringbuf->virtual_start + ringbuf->tail;
+	rem /= 4;
+	while (rem--)
+		iowrite32(MI_NOOP, virt++);
+
+	ringbuf->tail = 0;
+	ringbuf->space = intel_ring_space(ringbuf);
+
+	return 0;
+}
+
+static int logical_ring_prepare(struct intel_ringbuffer *ringbuf, int bytes)
+{
+	int ret;
+
+	if (unlikely(ringbuf->tail + bytes > ringbuf->effective_size)) {
+		ret = logical_ring_wrap_buffer(ringbuf);
+		if (unlikely(ret))
+			return ret;
+	}
+
+	if (unlikely(ringbuf->space < bytes)) {
+		ret = logical_ring_wait_for_space(ringbuf, bytes);
+		if (unlikely(ret))
+			return ret;
+	}
+
+	return 0;
+}
+
+/**
+ * intel_logical_ring_begin() - prepare the logical ringbuffer to accept some commands
+ *
+ * @ringbuf: Logical ringbuffer.
+ * @num_dwords: number of DWORDs that we plan to write to the ringbuffer.
+ *
+ * The ringbuffer might not be ready to accept the commands right away (maybe it needs to
+ * be wrapped, or wait a bit for the tail to be updated). This function takes care of that
+ * and also preallocates a request (every workload submission is still mediated through
+ * requests, same as it did with legacy ringbuffer submission).
+ *
+ * Return: non-zero if the ringbuffer is not ready to be written to.
+ */
+int intel_logical_ring_begin(struct intel_ringbuffer *ringbuf, int num_dwords)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret;
+
+	ret = i915_gem_check_wedge(&dev_priv->gpu_error,
+				   dev_priv->mm.interruptible);
+	if (ret)
+		return ret;
+
+	ret = logical_ring_prepare(ringbuf, num_dwords * sizeof(uint32_t));
+	if (ret)
+		return ret;
+
+	/* Preallocate the olr before touching the ring */
+	ret = logical_ring_alloc_seqno(ring, ringbuf->FIXME_lrc_ctx);
+	if (ret)
+		return ret;
+
+	ringbuf->space -= num_dwords * sizeof(uint32_t);
+	return 0;
+}
+
+static int gen8_init_common_ring(struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	I915_WRITE_IMR(ring, ~(ring->irq_enable_mask | ring->irq_keep_mask));
+	I915_WRITE(RING_HWSTAM(ring->mmio_base), 0xffffffff);
+
+	I915_WRITE(RING_MODE_GEN7(ring),
+		   _MASKED_BIT_DISABLE(GFX_REPLAY_MODE) |
+		   _MASKED_BIT_ENABLE(GFX_RUN_LIST_ENABLE));
+	POSTING_READ(RING_MODE_GEN7(ring));
+	DRM_DEBUG_DRIVER("Execlists enabled for %s\n", ring->name);
+
+	memset(&ring->hangcheck, 0, sizeof(ring->hangcheck));
+
+	return 0;
+}
+
+static int gen8_init_render_ring(struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret;
+
+	ret = gen8_init_common_ring(ring);
+	if (ret)
+		return ret;
+
+	/* We need to disable the AsyncFlip performance optimisations in order
+	 * to use MI_WAIT_FOR_EVENT within the CS. It should already be
+	 * programmed to '1' on all products.
+	 *
+	 * WaDisableAsyncFlipPerfMode:snb,ivb,hsw,vlv,bdw,chv
+	 */
+	I915_WRITE(MI_MODE, _MASKED_BIT_ENABLE(ASYNC_FLIP_PERF_DISABLE));
+
+	ret = intel_init_pipe_control(ring);
+	if (ret)
+		return ret;
+
+	I915_WRITE(INSTPM, _MASKED_BIT_ENABLE(INSTPM_FORCE_ORDERING));
+
+	return ret;
+}
+
+static int gen8_emit_bb_start(struct intel_ringbuffer *ringbuf,
+			      u64 offset, unsigned flags)
+{
+	bool ppgtt = !(flags & I915_DISPATCH_SECURE);
+	int ret;
+
+	ret = intel_logical_ring_begin(ringbuf, 4);
+	if (ret)
+		return ret;
+
+	/* FIXME(BDW): Address space and security selectors. */
+	intel_logical_ring_emit(ringbuf, MI_BATCH_BUFFER_START_GEN8 | (ppgtt<<8));
+	intel_logical_ring_emit(ringbuf, lower_32_bits(offset));
+	intel_logical_ring_emit(ringbuf, upper_32_bits(offset));
+	intel_logical_ring_emit(ringbuf, MI_NOOP);
+	intel_logical_ring_advance(ringbuf);
+
+	return 0;
+}
+
+static bool gen8_logical_ring_get_irq(struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	if (!dev->irq_enabled)
+		return false;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (ring->irq_refcount++ == 0) {
+		I915_WRITE_IMR(ring, ~(ring->irq_enable_mask | ring->irq_keep_mask));
+		POSTING_READ(RING_IMR(ring->mmio_base));
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+
+	return true;
+}
+
+static void gen8_logical_ring_put_irq(struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev_priv->irq_lock, flags);
+	if (--ring->irq_refcount == 0) {
+		I915_WRITE_IMR(ring, ~ring->irq_keep_mask);
+		POSTING_READ(RING_IMR(ring->mmio_base));
+	}
+	spin_unlock_irqrestore(&dev_priv->irq_lock, flags);
+}
+
+static int gen8_emit_flush(struct intel_ringbuffer *ringbuf,
+			   u32 invalidate_domains,
+			   u32 unused)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	uint32_t cmd;
+	int ret;
+
+	ret = intel_logical_ring_begin(ringbuf, 4);
+	if (ret)
+		return ret;
+
+	cmd = MI_FLUSH_DW + 1;
+
+	if (ring == &dev_priv->ring[VCS]) {
+		if (invalidate_domains & I915_GEM_GPU_DOMAINS)
+			cmd |= MI_INVALIDATE_TLB | MI_INVALIDATE_BSD |
+				MI_FLUSH_DW_STORE_INDEX |
+				MI_FLUSH_DW_OP_STOREDW;
+	} else {
+		if (invalidate_domains & I915_GEM_DOMAIN_RENDER)
+			cmd |= MI_INVALIDATE_TLB | MI_FLUSH_DW_STORE_INDEX |
+				MI_FLUSH_DW_OP_STOREDW;
+	}
+
+	intel_logical_ring_emit(ringbuf, cmd);
+	intel_logical_ring_emit(ringbuf,
+				I915_GEM_HWS_SCRATCH_ADDR |
+				MI_FLUSH_DW_USE_GTT);
+	intel_logical_ring_emit(ringbuf, 0); /* upper addr */
+	intel_logical_ring_emit(ringbuf, 0); /* value */
+	intel_logical_ring_advance(ringbuf);
+
+	return 0;
+}
+
+static int gen8_emit_flush_render(struct intel_ringbuffer *ringbuf,
+				  u32 invalidate_domains,
+				  u32 flush_domains)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	u32 scratch_addr = ring->scratch.gtt_offset + 2 * CACHELINE_BYTES;
+	u32 flags = 0;
+	int ret;
+
+	flags |= PIPE_CONTROL_CS_STALL;
+
+	if (flush_domains) {
+		flags |= PIPE_CONTROL_RENDER_TARGET_CACHE_FLUSH;
+		flags |= PIPE_CONTROL_DEPTH_CACHE_FLUSH;
+	}
+
+	if (invalidate_domains) {
+		flags |= PIPE_CONTROL_TLB_INVALIDATE;
+		flags |= PIPE_CONTROL_INSTRUCTION_CACHE_INVALIDATE;
+		flags |= PIPE_CONTROL_TEXTURE_CACHE_INVALIDATE;
+		flags |= PIPE_CONTROL_VF_CACHE_INVALIDATE;
+		flags |= PIPE_CONTROL_CONST_CACHE_INVALIDATE;
+		flags |= PIPE_CONTROL_STATE_CACHE_INVALIDATE;
+		flags |= PIPE_CONTROL_QW_WRITE;
+		flags |= PIPE_CONTROL_GLOBAL_GTT_IVB;
+	}
+
+	ret = intel_logical_ring_begin(ringbuf, 6);
+	if (ret)
+		return ret;
+
+	intel_logical_ring_emit(ringbuf, GFX_OP_PIPE_CONTROL(6));
+	intel_logical_ring_emit(ringbuf, flags);
+	intel_logical_ring_emit(ringbuf, scratch_addr);
+	intel_logical_ring_emit(ringbuf, 0);
+	intel_logical_ring_emit(ringbuf, 0);
+	intel_logical_ring_emit(ringbuf, 0);
+	intel_logical_ring_advance(ringbuf);
+
+	return 0;
+}
+
+static u32 gen8_get_seqno(struct intel_engine_cs *ring, bool lazy_coherency)
+{
+	return intel_read_status_page(ring, I915_GEM_HWS_INDEX);
+}
+
+static void gen8_set_seqno(struct intel_engine_cs *ring, u32 seqno)
+{
+	intel_write_status_page(ring, I915_GEM_HWS_INDEX, seqno);
+}
+
+static int gen8_emit_request(struct intel_ringbuffer *ringbuf)
+{
+	struct intel_engine_cs *ring = ringbuf->ring;
+	u32 cmd;
+	int ret;
+
+	ret = intel_logical_ring_begin(ringbuf, 6);
+	if (ret)
+		return ret;
+
+	cmd = MI_STORE_DWORD_IMM_GEN8;
+	cmd |= MI_GLOBAL_GTT;
+
+	intel_logical_ring_emit(ringbuf, cmd);
+	intel_logical_ring_emit(ringbuf,
+				(ring->status_page.gfx_addr +
+				(I915_GEM_HWS_INDEX << MI_STORE_DWORD_INDEX_SHIFT)));
+	intel_logical_ring_emit(ringbuf, 0);
+	intel_logical_ring_emit(ringbuf, ring->outstanding_lazy_seqno);
+	intel_logical_ring_emit(ringbuf, MI_USER_INTERRUPT);
+	intel_logical_ring_emit(ringbuf, MI_NOOP);
+	intel_logical_ring_advance_and_submit(ringbuf);
+
+	return 0;
+}
+
+/**
+ * intel_logical_ring_cleanup() - deallocate the Engine Command Streamer
+ *
+ * @ring: Engine Command Streamer.
+ *
+ */
+void intel_logical_ring_cleanup(struct intel_engine_cs *ring)
+{
+	struct drm_i915_private *dev_priv = ring->dev->dev_private;
+
+	if (!intel_ring_initialized(ring))
+		return;
+
+	intel_logical_ring_stop(ring);
+	WARN_ON((I915_READ_MODE(ring) & MODE_IDLE) == 0);
+	ring->preallocated_lazy_request = NULL;
+	ring->outstanding_lazy_seqno = 0;
+
+	if (ring->cleanup)
+		ring->cleanup(ring);
+
+	i915_cmd_parser_fini_ring(ring);
+
+	if (ring->status_page.obj) {
+		kunmap(sg_page(ring->status_page.obj->pages->sgl));
+		ring->status_page.obj = NULL;
+	}
+}
+
+static int logical_ring_init(struct drm_device *dev, struct intel_engine_cs *ring)
+{
+	int ret;
+
+	/* Intentionally left blank. */
+	ring->buffer = NULL;
+
+	ring->dev = dev;
+	INIT_LIST_HEAD(&ring->active_list);
+	INIT_LIST_HEAD(&ring->request_list);
+	init_waitqueue_head(&ring->irq_queue);
+
+	INIT_LIST_HEAD(&ring->execlist_queue);
+	spin_lock_init(&ring->execlist_lock);
+	ring->next_context_status_buffer = 0;
+
+	ret = i915_cmd_parser_init_ring(ring);
+	if (ret)
+		return ret;
+
+	if (ring->init) {
+		ret = ring->init(ring);
+		if (ret)
+			return ret;
+	}
+
+	ret = intel_lr_context_deferred_create(ring->default_context, ring);
+
+	return ret;
+}
+
+static int logical_render_ring_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring = &dev_priv->ring[RCS];
+
+	ring->name = "render ring";
+	ring->id = RCS;
+	ring->mmio_base = RENDER_RING_BASE;
+	ring->irq_enable_mask =
+		GT_RENDER_USER_INTERRUPT << GEN8_RCS_IRQ_SHIFT;
+	ring->irq_keep_mask =
+		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_RCS_IRQ_SHIFT;
+	if (HAS_L3_DPF(dev))
+		ring->irq_keep_mask |= GT_RENDER_L3_PARITY_ERROR_INTERRUPT;
+
+	ring->init = gen8_init_render_ring;
+	ring->cleanup = intel_fini_pipe_control;
+	ring->get_seqno = gen8_get_seqno;
+	ring->set_seqno = gen8_set_seqno;
+	ring->emit_request = gen8_emit_request;
+	ring->emit_flush = gen8_emit_flush_render;
+	ring->irq_get = gen8_logical_ring_get_irq;
+	ring->irq_put = gen8_logical_ring_put_irq;
+	ring->emit_bb_start = gen8_emit_bb_start;
+
+	return logical_ring_init(dev, ring);
+}
+
+static int logical_bsd_ring_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring = &dev_priv->ring[VCS];
+
+	ring->name = "bsd ring";
+	ring->id = VCS;
+	ring->mmio_base = GEN6_BSD_RING_BASE;
+	ring->irq_enable_mask =
+		GT_RENDER_USER_INTERRUPT << GEN8_VCS1_IRQ_SHIFT;
+	ring->irq_keep_mask =
+		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VCS1_IRQ_SHIFT;
+
+	ring->init = gen8_init_common_ring;
+	ring->get_seqno = gen8_get_seqno;
+	ring->set_seqno = gen8_set_seqno;
+	ring->emit_request = gen8_emit_request;
+	ring->emit_flush = gen8_emit_flush;
+	ring->irq_get = gen8_logical_ring_get_irq;
+	ring->irq_put = gen8_logical_ring_put_irq;
+	ring->emit_bb_start = gen8_emit_bb_start;
+
+	return logical_ring_init(dev, ring);
+}
+
+static int logical_bsd2_ring_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring = &dev_priv->ring[VCS2];
+
+	ring->name = "bds2 ring";
+	ring->id = VCS2;
+	ring->mmio_base = GEN8_BSD2_RING_BASE;
+	ring->irq_enable_mask =
+		GT_RENDER_USER_INTERRUPT << GEN8_VCS2_IRQ_SHIFT;
+	ring->irq_keep_mask =
+		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VCS2_IRQ_SHIFT;
+
+	ring->init = gen8_init_common_ring;
+	ring->get_seqno = gen8_get_seqno;
+	ring->set_seqno = gen8_set_seqno;
+	ring->emit_request = gen8_emit_request;
+	ring->emit_flush = gen8_emit_flush;
+	ring->irq_get = gen8_logical_ring_get_irq;
+	ring->irq_put = gen8_logical_ring_put_irq;
+	ring->emit_bb_start = gen8_emit_bb_start;
+
+	return logical_ring_init(dev, ring);
+}
+
+static int logical_blt_ring_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring = &dev_priv->ring[BCS];
+
+	ring->name = "blitter ring";
+	ring->id = BCS;
+	ring->mmio_base = BLT_RING_BASE;
+	ring->irq_enable_mask =
+		GT_RENDER_USER_INTERRUPT << GEN8_BCS_IRQ_SHIFT;
+	ring->irq_keep_mask =
+		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_BCS_IRQ_SHIFT;
+
+	ring->init = gen8_init_common_ring;
+	ring->get_seqno = gen8_get_seqno;
+	ring->set_seqno = gen8_set_seqno;
+	ring->emit_request = gen8_emit_request;
+	ring->emit_flush = gen8_emit_flush;
+	ring->irq_get = gen8_logical_ring_get_irq;
+	ring->irq_put = gen8_logical_ring_put_irq;
+	ring->emit_bb_start = gen8_emit_bb_start;
+
+	return logical_ring_init(dev, ring);
+}
+
+static int logical_vebox_ring_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct intel_engine_cs *ring = &dev_priv->ring[VECS];
+
+	ring->name = "video enhancement ring";
+	ring->id = VECS;
+	ring->mmio_base = VEBOX_RING_BASE;
+	ring->irq_enable_mask =
+		GT_RENDER_USER_INTERRUPT << GEN8_VECS_IRQ_SHIFT;
+	ring->irq_keep_mask =
+		GT_CONTEXT_SWITCH_INTERRUPT << GEN8_VECS_IRQ_SHIFT;
+
+	ring->init = gen8_init_common_ring;
+	ring->get_seqno = gen8_get_seqno;
+	ring->set_seqno = gen8_set_seqno;
+	ring->emit_request = gen8_emit_request;
+	ring->emit_flush = gen8_emit_flush;
+	ring->irq_get = gen8_logical_ring_get_irq;
+	ring->irq_put = gen8_logical_ring_put_irq;
+	ring->emit_bb_start = gen8_emit_bb_start;
+
+	return logical_ring_init(dev, ring);
+}
+
+/**
+ * intel_logical_rings_init() - allocate, populate and init the Engine Command Streamers
+ * @dev: DRM device.
+ *
+ * This function inits the engines for an Execlists submission style (the equivalent in the
+ * legacy ringbuffer submission world would be i915_gem_init_rings). It does it only for
+ * those engines that are present in the hardware.
+ *
+ * Return: non-zero if the initialization failed.
+ */
+int intel_logical_rings_init(struct drm_device *dev)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int ret;
+
+	ret = logical_render_ring_init(dev);
+	if (ret)
+		return ret;
+
+	if (HAS_BSD(dev)) {
+		ret = logical_bsd_ring_init(dev);
+		if (ret)
+			goto cleanup_render_ring;
+	}
+
+	if (HAS_BLT(dev)) {
+		ret = logical_blt_ring_init(dev);
+		if (ret)
+			goto cleanup_bsd_ring;
+	}
+
+	if (HAS_VEBOX(dev)) {
+		ret = logical_vebox_ring_init(dev);
+		if (ret)
+			goto cleanup_blt_ring;
+	}
+
+	if (HAS_BSD2(dev)) {
+		ret = logical_bsd2_ring_init(dev);
+		if (ret)
+			goto cleanup_vebox_ring;
+	}
+
+	ret = i915_gem_set_seqno(dev, ((u32)~0 - 0x1000));
+	if (ret)
+		goto cleanup_bsd2_ring;
+
+	return 0;
+
+cleanup_bsd2_ring:
+	intel_logical_ring_cleanup(&dev_priv->ring[VCS2]);
+cleanup_vebox_ring:
+	intel_logical_ring_cleanup(&dev_priv->ring[VECS]);
+cleanup_blt_ring:
+	intel_logical_ring_cleanup(&dev_priv->ring[BCS]);
+cleanup_bsd_ring:
+	intel_logical_ring_cleanup(&dev_priv->ring[VCS]);
+cleanup_render_ring:
+	intel_logical_ring_cleanup(&dev_priv->ring[RCS]);
+
+	return ret;
+}
+
+int intel_lr_context_render_state_init(struct intel_engine_cs *ring,
+				       struct intel_context *ctx)
+{
+	struct intel_ringbuffer *ringbuf = ctx->engine[ring->id].ringbuf;
+	struct render_state so;
+	struct drm_i915_file_private *file_priv = ctx->file_priv;
+	struct drm_file *file = file_priv ? file_priv->file : NULL;
+	int ret;
+
+	ret = i915_gem_render_state_prepare(ring, &so);
+	if (ret)
+		return ret;
+
+	if (so.rodata == NULL)
+		return 0;
+
+	ret = ring->emit_bb_start(ringbuf,
+			so.ggtt_offset,
+			I915_DISPATCH_SECURE);
+	if (ret)
+		goto out;
+
+	i915_vma_move_to_active(i915_gem_obj_to_ggtt(so.obj), ring);
+
+	ret = __i915_add_request(ring, file, so.obj, NULL);
+	/* intel_logical_ring_add_request moves object to inactive if it
+	 * fails */
+out:
+	i915_gem_render_state_fini(&so);
+	return ret;
+}
+
+static int
+populate_lr_context(struct intel_context *ctx, struct drm_i915_gem_object *ctx_obj,
+		    struct intel_engine_cs *ring, struct intel_ringbuffer *ringbuf)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	struct drm_i915_gem_object *ring_obj = ringbuf->obj;
+	struct i915_hw_ppgtt *ppgtt = ctx->ppgtt;
+	struct page *page;
+	uint32_t *reg_state;
+	int ret;
+
+	if (!ppgtt)
+		ppgtt = dev_priv->mm.aliasing_ppgtt;
+
+	ret = i915_gem_object_set_to_cpu_domain(ctx_obj, true);
+	if (ret) {
+		DRM_DEBUG_DRIVER("Could not set to CPU domain\n");
+		return ret;
+	}
+
+	ret = i915_gem_object_get_pages(ctx_obj);
+	if (ret) {
+		DRM_DEBUG_DRIVER("Could not get object pages\n");
+		return ret;
+	}
+
+	i915_gem_object_pin_pages(ctx_obj);
+
+	/* The second page of the context object contains some fields which must
+	 * be set up prior to the first execution. */
+	page = i915_gem_object_get_page(ctx_obj, 1);
+	reg_state = kmap_atomic(page);
+
+	/* A context is actually a big batch buffer with several MI_LOAD_REGISTER_IMM
+	 * commands followed by (reg, value) pairs. The values we are setting here are
+	 * only for the first context restore: on a subsequent save, the GPU will
+	 * recreate this batchbuffer with new values (including all the missing
+	 * MI_LOAD_REGISTER_IMM commands that we are not initializing here). */
+	if (ring->id == RCS)
+		reg_state[CTX_LRI_HEADER_0] = MI_LOAD_REGISTER_IMM(14);
+	else
+		reg_state[CTX_LRI_HEADER_0] = MI_LOAD_REGISTER_IMM(11);
+	reg_state[CTX_LRI_HEADER_0] |= MI_LRI_FORCE_POSTED;
+	reg_state[CTX_CONTEXT_CONTROL] = RING_CONTEXT_CONTROL(ring);
+	reg_state[CTX_CONTEXT_CONTROL+1] =
+			_MASKED_BIT_ENABLE((1<<3) | MI_RESTORE_INHIBIT);
+	reg_state[CTX_RING_HEAD] = RING_HEAD(ring->mmio_base);
+	reg_state[CTX_RING_HEAD+1] = 0;
+	reg_state[CTX_RING_TAIL] = RING_TAIL(ring->mmio_base);
+	reg_state[CTX_RING_TAIL+1] = 0;
+	reg_state[CTX_RING_BUFFER_START] = RING_START(ring->mmio_base);
+	reg_state[CTX_RING_BUFFER_START+1] = i915_gem_obj_ggtt_offset(ring_obj);
+	reg_state[CTX_RING_BUFFER_CONTROL] = RING_CTL(ring->mmio_base);
+	reg_state[CTX_RING_BUFFER_CONTROL+1] =
+			((ringbuf->size - PAGE_SIZE) & RING_NR_PAGES) | RING_VALID;
+	reg_state[CTX_BB_HEAD_U] = ring->mmio_base + 0x168;
+	reg_state[CTX_BB_HEAD_U+1] = 0;
+	reg_state[CTX_BB_HEAD_L] = ring->mmio_base + 0x140;
+	reg_state[CTX_BB_HEAD_L+1] = 0;
+	reg_state[CTX_BB_STATE] = ring->mmio_base + 0x110;
+	reg_state[CTX_BB_STATE+1] = (1<<5);
+	reg_state[CTX_SECOND_BB_HEAD_U] = ring->mmio_base + 0x11c;
+	reg_state[CTX_SECOND_BB_HEAD_U+1] = 0;
+	reg_state[CTX_SECOND_BB_HEAD_L] = ring->mmio_base + 0x114;
+	reg_state[CTX_SECOND_BB_HEAD_L+1] = 0;
+	reg_state[CTX_SECOND_BB_STATE] = ring->mmio_base + 0x118;
+	reg_state[CTX_SECOND_BB_STATE+1] = 0;
+	if (ring->id == RCS) {
+		/* TODO: according to BSpec, the register state context
+		 * for CHV does not have these. OTOH, these registers do
+		 * exist in CHV. I'm waiting for a clarification */
+		reg_state[CTX_BB_PER_CTX_PTR] = ring->mmio_base + 0x1c0;
+		reg_state[CTX_BB_PER_CTX_PTR+1] = 0;
+		reg_state[CTX_RCS_INDIRECT_CTX] = ring->mmio_base + 0x1c4;
+		reg_state[CTX_RCS_INDIRECT_CTX+1] = 0;
+		reg_state[CTX_RCS_INDIRECT_CTX_OFFSET] = ring->mmio_base + 0x1c8;
+		reg_state[CTX_RCS_INDIRECT_CTX_OFFSET+1] = 0;
+	}
+	reg_state[CTX_LRI_HEADER_1] = MI_LOAD_REGISTER_IMM(9);
+	reg_state[CTX_LRI_HEADER_1] |= MI_LRI_FORCE_POSTED;
+	reg_state[CTX_CTX_TIMESTAMP] = ring->mmio_base + 0x3a8;
+	reg_state[CTX_CTX_TIMESTAMP+1] = 0;
+	reg_state[CTX_PDP3_UDW] = GEN8_RING_PDP_UDW(ring, 3);
+	reg_state[CTX_PDP3_LDW] = GEN8_RING_PDP_LDW(ring, 3);
+	reg_state[CTX_PDP2_UDW] = GEN8_RING_PDP_UDW(ring, 2);
+	reg_state[CTX_PDP2_LDW] = GEN8_RING_PDP_LDW(ring, 2);
+	reg_state[CTX_PDP1_UDW] = GEN8_RING_PDP_UDW(ring, 1);
+	reg_state[CTX_PDP1_LDW] = GEN8_RING_PDP_LDW(ring, 1);
+	reg_state[CTX_PDP0_UDW] = GEN8_RING_PDP_UDW(ring, 0);
+	reg_state[CTX_PDP0_LDW] = GEN8_RING_PDP_LDW(ring, 0);
+	reg_state[CTX_PDP3_UDW+1] = upper_32_bits(ppgtt->pd_dma_addr[3]);
+	reg_state[CTX_PDP3_LDW+1] = lower_32_bits(ppgtt->pd_dma_addr[3]);
+	reg_state[CTX_PDP2_UDW+1] = upper_32_bits(ppgtt->pd_dma_addr[2]);
+	reg_state[CTX_PDP2_LDW+1] = lower_32_bits(ppgtt->pd_dma_addr[2]);
+	reg_state[CTX_PDP1_UDW+1] = upper_32_bits(ppgtt->pd_dma_addr[1]);
+	reg_state[CTX_PDP1_LDW+1] = lower_32_bits(ppgtt->pd_dma_addr[1]);
+	reg_state[CTX_PDP0_UDW+1] = upper_32_bits(ppgtt->pd_dma_addr[0]);
+	reg_state[CTX_PDP0_LDW+1] = lower_32_bits(ppgtt->pd_dma_addr[0]);
+	if (ring->id == RCS) {
+		reg_state[CTX_LRI_HEADER_2] = MI_LOAD_REGISTER_IMM(1);
+		reg_state[CTX_R_PWR_CLK_STATE] = 0x20c8;
+		reg_state[CTX_R_PWR_CLK_STATE+1] = 0;
+	}
+
+	kunmap_atomic(reg_state);
+
+	ctx_obj->dirty = 1;
+	set_page_dirty(page);
+	i915_gem_object_unpin_pages(ctx_obj);
+
+	return 0;
+}
+
+/**
+ * intel_lr_context_free() - free the LRC specific bits of a context
+ * @ctx: the LR context to free.
+ *
+ * The real context freeing is done in i915_gem_context_free: this only
+ * takes care of the bits that are LRC related: the per-engine backing
+ * objects and the logical ringbuffer.
+ */
+void intel_lr_context_free(struct intel_context *ctx)
+{
+	int i;
+
+	for (i = 0; i < I915_NUM_RINGS; i++) {
+		struct drm_i915_gem_object *ctx_obj = ctx->engine[i].state;
+		struct intel_ringbuffer *ringbuf = ctx->engine[i].ringbuf;
+
+		if (ctx_obj) {
+			intel_destroy_ringbuffer_obj(ringbuf);
+			kfree(ringbuf);
+			i915_gem_object_ggtt_unpin(ctx_obj);
+			drm_gem_object_unreference(&ctx_obj->base);
+		}
+	}
+}
+
+static uint32_t get_lr_context_size(struct intel_engine_cs *ring)
+{
+	int ret = 0;
+
+	WARN_ON(INTEL_INFO(ring->dev)->gen != 8);
+
+	switch (ring->id) {
+	case RCS:
+		ret = GEN8_LR_CONTEXT_RENDER_SIZE;
+		break;
+	case VCS:
+	case BCS:
+	case VECS:
+	case VCS2:
+		ret = GEN8_LR_CONTEXT_OTHER_SIZE;
+		break;
+	}
+
+	return ret;
+}
+
+/**
+ * intel_lr_context_deferred_create() - create the LRC specific bits of a context
+ * @ctx: LR context to create.
+ * @ring: engine to be used with the context.
+ *
+ * This function can be called more than once, with different engines, if we plan
+ * to use the context with them. The context backing objects and the ringbuffers
+ * (specially the ringbuffer backing objects) suck a lot of memory up, and that's why
+ * the creation is a deferred call: it's better to make sure first that we need to use
+ * a given ring with the context.
+ *
+ * Return: non-zero on eror.
+ */
+int intel_lr_context_deferred_create(struct intel_context *ctx,
+				     struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_gem_object *ctx_obj;
+	uint32_t context_size;
+	struct intel_ringbuffer *ringbuf;
+	int ret;
+
+	WARN_ON(ctx->legacy_hw_ctx.rcs_state != NULL);
+	if (ctx->engine[ring->id].state)
+		return 0;
+
+	context_size = round_up(get_lr_context_size(ring), 4096);
+
+	ctx_obj = i915_gem_alloc_context_obj(dev, context_size);
+	if (IS_ERR(ctx_obj)) {
+		ret = PTR_ERR(ctx_obj);
+		DRM_DEBUG_DRIVER("Alloc LRC backing obj failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = i915_gem_obj_ggtt_pin(ctx_obj, GEN8_LR_CONTEXT_ALIGN, 0);
+	if (ret) {
+		DRM_DEBUG_DRIVER("Pin LRC backing obj failed: %d\n", ret);
+		drm_gem_object_unreference(&ctx_obj->base);
+		return ret;
+	}
+
+	ringbuf = kzalloc(sizeof(*ringbuf), GFP_KERNEL);
+	if (!ringbuf) {
+		DRM_DEBUG_DRIVER("Failed to allocate ringbuffer %s\n",
+				ring->name);
+		i915_gem_object_ggtt_unpin(ctx_obj);
+		drm_gem_object_unreference(&ctx_obj->base);
+		ret = -ENOMEM;
+		return ret;
+	}
+
+	ringbuf->ring = ring;
+	ringbuf->FIXME_lrc_ctx = ctx;
+
+	ringbuf->size = 32 * PAGE_SIZE;
+	ringbuf->effective_size = ringbuf->size;
+	ringbuf->head = 0;
+	ringbuf->tail = 0;
+	ringbuf->space = ringbuf->size;
+	ringbuf->last_retired_head = -1;
+
+	/* TODO: For now we put this in the mappable region so that we can reuse
+	 * the existing ringbuffer code which ioremaps it. When we start
+	 * creating many contexts, this will no longer work and we must switch
+	 * to a kmapish interface.
+	 */
+	ret = intel_alloc_ringbuffer_obj(dev, ringbuf);
+	if (ret) {
+		DRM_DEBUG_DRIVER("Failed to allocate ringbuffer obj %s: %d\n",
+				ring->name, ret);
+		goto error;
+	}
+
+	ret = populate_lr_context(ctx, ctx_obj, ring, ringbuf);
+	if (ret) {
+		DRM_DEBUG_DRIVER("Failed to populate LRC: %d\n", ret);
+		intel_destroy_ringbuffer_obj(ringbuf);
+		goto error;
+	}
+
+	ctx->engine[ring->id].ringbuf = ringbuf;
+	ctx->engine[ring->id].state = ctx_obj;
+
+	if (ctx == ring->default_context) {
+		/* The status page is offset 0 from the default context object
+		 * in LRC mode. */
+		ring->status_page.gfx_addr = i915_gem_obj_ggtt_offset(ctx_obj);
+		ring->status_page.page_addr =
+				kmap(sg_page(ctx_obj->pages->sgl));
+		if (ring->status_page.page_addr == NULL)
+			return -ENOMEM;
+		ring->status_page.obj = ctx_obj;
+	}
+
+	if (ring->id == RCS && !ctx->rcs_initialized) {
+		ret = intel_lr_context_render_state_init(ring, ctx);
+		if (ret) {
+			DRM_ERROR("Init render state failed: %d\n", ret);
+			ctx->engine[ring->id].ringbuf = NULL;
+			ctx->engine[ring->id].state = NULL;
+			intel_destroy_ringbuffer_obj(ringbuf);
+			goto error;
+		}
+		ctx->rcs_initialized = true;
+	}
+
+	return 0;
+
+error:
+	kfree(ringbuf);
+	i915_gem_object_ggtt_unpin(ctx_obj);
+	drm_gem_object_unreference(&ctx_obj->base);
+	return ret;
+}
diff --git a/drivers/gpu/drm/i915/intel_lrc.h b/drivers/gpu/drm/i915/intel_lrc.h
new file mode 100644
index 000000000000..33c3b4bf28c5
--- /dev/null
+++ b/drivers/gpu/drm/i915/intel_lrc.h
@@ -0,0 +1,114 @@
+/*
+ * Copyright © 2014 Intel Corporation
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the next
+ * paragraph) shall be included in all copies or substantial portions of the
+ * Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+ * DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _INTEL_LRC_H_
+#define _INTEL_LRC_H_
+
+/* Execlists regs */
+#define RING_ELSP(ring)			((ring)->mmio_base+0x230)
+#define RING_EXECLIST_STATUS(ring)	((ring)->mmio_base+0x234)
+#define RING_CONTEXT_CONTROL(ring)	((ring)->mmio_base+0x244)
+#define RING_CONTEXT_STATUS_BUF(ring)	((ring)->mmio_base+0x370)
+#define RING_CONTEXT_STATUS_PTR(ring)	((ring)->mmio_base+0x3a0)
+
+/* Logical Rings */
+void intel_logical_ring_stop(struct intel_engine_cs *ring);
+void intel_logical_ring_cleanup(struct intel_engine_cs *ring);
+int intel_logical_rings_init(struct drm_device *dev);
+
+int logical_ring_flush_all_caches(struct intel_ringbuffer *ringbuf);
+void intel_logical_ring_advance_and_submit(struct intel_ringbuffer *ringbuf);
+/**
+ * intel_logical_ring_advance() - advance the ringbuffer tail
+ * @ringbuf: Ringbuffer to advance.
+ *
+ * The tail is only updated in our logical ringbuffer struct.
+ */
+static inline void intel_logical_ring_advance(struct intel_ringbuffer *ringbuf)
+{
+	ringbuf->tail &= ringbuf->size - 1;
+}
+/**
+ * intel_logical_ring_emit() - write a DWORD to the ringbuffer.
+ * @ringbuf: Ringbuffer to write to.
+ * @data: DWORD to write.
+ */
+static inline void intel_logical_ring_emit(struct intel_ringbuffer *ringbuf,
+					   u32 data)
+{
+	iowrite32(data, ringbuf->virtual_start + ringbuf->tail);
+	ringbuf->tail += 4;
+}
+int intel_logical_ring_begin(struct intel_ringbuffer *ringbuf, int num_dwords);
+
+/* Logical Ring Contexts */
+int intel_lr_context_render_state_init(struct intel_engine_cs *ring,
+				       struct intel_context *ctx);
+void intel_lr_context_free(struct intel_context *ctx);
+int intel_lr_context_deferred_create(struct intel_context *ctx,
+				     struct intel_engine_cs *ring);
+
+/* Execlists */
+int intel_sanitize_enable_execlists(struct drm_device *dev, int enable_execlists);
+int intel_execlists_submission(struct drm_device *dev, struct drm_file *file,
+			       struct intel_engine_cs *ring,
+			       struct intel_context *ctx,
+			       struct drm_i915_gem_execbuffer2 *args,
+			       struct list_head *vmas,
+			       struct drm_i915_gem_object *batch_obj,
+			       u64 exec_start, u32 flags);
+u32 intel_execlists_ctx_id(struct drm_i915_gem_object *ctx_obj);
+
+/**
+ * struct intel_ctx_submit_request - queued context submission request
+ * @ctx: Context to submit to the ELSP.
+ * @ring: Engine to submit it to.
+ * @tail: how far in the context's ringbuffer this request goes to.
+ * @execlist_link: link in the submission queue.
+ * @work: workqueue for processing this request in a bottom half.
+ * @elsp_submitted: no. of times this request has been sent to the ELSP.
+ *
+ * The ELSP only accepts two elements at a time, so we queue context/tail
+ * pairs on a given queue (ring->execlist_queue) until the hardware is
+ * available. The queue serves a double purpose: we also use it to keep track
+ * of the up to 2 contexts currently in the hardware (usually one in execution
+ * and the other queued up by the GPU): We only remove elements from the head
+ * of the queue when the hardware informs us that an element has been
+ * completed.
+ *
+ * All accesses to the queue are mediated by a spinlock (ring->execlist_lock).
+ */
+struct intel_ctx_submit_request {
+	struct intel_context *ctx;
+	struct intel_engine_cs *ring;
+	u32 tail;
+
+	struct list_head execlist_link;
+	struct work_struct work;
+
+	int elsp_submitted;
+};
+
+void intel_execlists_handle_ctx_events(struct intel_engine_cs *ring);
+
+#endif /* _INTEL_LRC_H_ */
diff --git a/drivers/gpu/drm/i915/intel_lvds.c b/drivers/gpu/drm/i915/intel_lvds.c
index 881361c0f27e..a6bd1422e38f 100644
--- a/drivers/gpu/drm/i915/intel_lvds.c
+++ b/drivers/gpu/drm/i915/intel_lvds.c
@@ -538,7 +538,7 @@ static const struct drm_encoder_funcs intel_lvds_enc_funcs = {
 	.destroy = intel_encoder_destroy,
 };
 
-static int __init intel_no_lvds_dmi_callback(const struct dmi_system_id *id)
+static int intel_no_lvds_dmi_callback(const struct dmi_system_id *id)
 {
 	DRM_INFO("Skipping LVDS initialization for %s\n", id->ident);
 	return 1;
@@ -823,8 +823,7 @@ bool intel_is_dual_link_lvds(struct drm_device *dev)
 	struct intel_encoder *encoder;
 	struct intel_lvds_encoder *lvds_encoder;
 
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list,
-			    base.head) {
+	for_each_intel_encoder(dev, encoder) {
 		if (encoder->type == INTEL_OUTPUT_LVDS) {
 			lvds_encoder = to_lvds_encoder(&encoder->base);
 
diff --git a/drivers/gpu/drm/i915/intel_opregion.c b/drivers/gpu/drm/i915/intel_opregion.c
index ca52ad2ae7d1..d8de1d5140a7 100644
--- a/drivers/gpu/drm/i915/intel_opregion.c
+++ b/drivers/gpu/drm/i915/intel_opregion.c
@@ -396,6 +396,16 @@ int intel_opregion_notify_adapter(struct drm_device *dev, pci_power_t state)
 	return -EINVAL;
 }
 
+/*
+ * If the vendor backlight interface is not in use and ACPI backlight interface
+ * is broken, do not bother processing backlight change requests from firmware.
+ */
+static bool should_ignore_backlight_request(void)
+{
+	return acpi_video_backlight_support() &&
+	       !acpi_video_verify_backlight_support();
+}
+
 static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
@@ -404,11 +414,7 @@ static u32 asle_set_backlight(struct drm_device *dev, u32 bclp)
 
 	DRM_DEBUG_DRIVER("bclp = 0x%08x\n", bclp);
 
-	/*
-	 * If the acpi_video interface is not supposed to be used, don't
-	 * bother processing backlight level change requests from firmware.
-	 */
-	if (!acpi_video_verify_backlight_support()) {
+	if (should_ignore_backlight_request()) {
 		DRM_DEBUG_KMS("opregion backlight request ignored\n");
 		return 0;
 	}
diff --git a/drivers/gpu/drm/i915/intel_panel.c b/drivers/gpu/drm/i915/intel_panel.c
index 59b028f0b1e8..0e018cb49147 100644
--- a/drivers/gpu/drm/i915/intel_panel.c
+++ b/drivers/gpu/drm/i915/intel_panel.c
@@ -419,9 +419,8 @@ static uint32_t scale(uint32_t source_val,
 	source_val = clamp(source_val, source_min, source_max);
 
 	/* avoid overflows */
-	target_val = (uint64_t)(source_val - source_min) *
-		(target_max - target_min);
-	do_div(target_val, source_max - source_min);
+	target_val = DIV_ROUND_CLOSEST_ULL((uint64_t)(source_val - source_min) *
+			(target_max - target_min), source_max - source_min);
 	target_val += target_min;
 
 	return target_val;
@@ -751,6 +750,8 @@ void intel_panel_disable_backlight(struct intel_connector *connector)
 
 	spin_lock_irqsave(&dev_priv->backlight_lock, flags);
 
+	if (panel->backlight.device)
+		panel->backlight.device->props.power = FB_BLANK_POWERDOWN;
 	panel->backlight.enabled = false;
 	dev_priv->display.disable_backlight(connector);
 
@@ -801,7 +802,7 @@ static void pch_enable_backlight(struct intel_connector *connector)
 
 	cpu_ctl2 = I915_READ(BLC_PWM_CPU_CTL2);
 	if (cpu_ctl2 & BLM_PWM_ENABLE) {
-		WARN(1, "cpu backlight already enabled\n");
+		DRM_DEBUG_KMS("cpu backlight already enabled\n");
 		cpu_ctl2 &= ~BLM_PWM_ENABLE;
 		I915_WRITE(BLC_PWM_CPU_CTL2, cpu_ctl2);
 	}
@@ -845,7 +846,7 @@ static void i9xx_enable_backlight(struct intel_connector *connector)
 
 	ctl = I915_READ(BLC_PWM_CTL);
 	if (ctl & BACKLIGHT_DUTY_CYCLE_MASK_PNV) {
-		WARN(1, "backlight already enabled\n");
+		DRM_DEBUG_KMS("backlight already enabled\n");
 		I915_WRITE(BLC_PWM_CTL, 0);
 	}
 
@@ -876,7 +877,7 @@ static void i965_enable_backlight(struct intel_connector *connector)
 
 	ctl2 = I915_READ(BLC_PWM_CTL2);
 	if (ctl2 & BLM_PWM_ENABLE) {
-		WARN(1, "backlight already enabled\n");
+		DRM_DEBUG_KMS("backlight already enabled\n");
 		ctl2 &= ~BLM_PWM_ENABLE;
 		I915_WRITE(BLC_PWM_CTL2, ctl2);
 	}
@@ -910,7 +911,7 @@ static void vlv_enable_backlight(struct intel_connector *connector)
 
 	ctl2 = I915_READ(VLV_BLC_PWM_CTL2(pipe));
 	if (ctl2 & BLM_PWM_ENABLE) {
-		WARN(1, "backlight already enabled\n");
+		DRM_DEBUG_KMS("backlight already enabled\n");
 		ctl2 &= ~BLM_PWM_ENABLE;
 		I915_WRITE(VLV_BLC_PWM_CTL2(pipe), ctl2);
 	}
@@ -957,6 +958,8 @@ void intel_panel_enable_backlight(struct intel_connector *connector)
 
 	dev_priv->display.enable_backlight(connector);
 	panel->backlight.enabled = true;
+	if (panel->backlight.device)
+		panel->backlight.device->props.power = FB_BLANK_UNBLANK;
 
 	spin_unlock_irqrestore(&dev_priv->backlight_lock, flags);
 }
@@ -965,6 +968,7 @@ void intel_panel_enable_backlight(struct intel_connector *connector)
 static int intel_backlight_device_update_status(struct backlight_device *bd)
 {
 	struct intel_connector *connector = bl_get_data(bd);
+	struct intel_panel *panel = &connector->panel;
 	struct drm_device *dev = connector->base.dev;
 
 	drm_modeset_lock(&dev->mode_config.connection_mutex, NULL);
@@ -972,6 +976,23 @@ static int intel_backlight_device_update_status(struct backlight_device *bd)
 		      bd->props.brightness, bd->props.max_brightness);
 	intel_panel_set_backlight(connector, bd->props.brightness,
 				  bd->props.max_brightness);
+
+	/*
+	 * Allow flipping bl_power as a sub-state of enabled. Sadly the
+	 * backlight class device does not make it easy to to differentiate
+	 * between callbacks for brightness and bl_power, so our backlight_power
+	 * callback needs to take this into account.
+	 */
+	if (panel->backlight.enabled) {
+		if (panel->backlight_power) {
+			bool enable = bd->props.power == FB_BLANK_UNBLANK &&
+				bd->props.brightness != 0;
+			panel->backlight_power(connector, enable);
+		}
+	} else {
+		bd->props.power = FB_BLANK_POWERDOWN;
+	}
+
 	drm_modeset_unlock(&dev->mode_config.connection_mutex);
 	return 0;
 }
@@ -1023,6 +1044,11 @@ static int intel_backlight_device_register(struct intel_connector *connector)
 					    panel->backlight.level,
 					    props.max_brightness);
 
+	if (panel->backlight.enabled)
+		props.power = FB_BLANK_UNBLANK;
+	else
+		props.power = FB_BLANK_POWERDOWN;
+
 	/*
 	 * Note: using the same name independent of the connector prevents
 	 * registration of multiple backlight devices in the driver.
@@ -1203,7 +1229,7 @@ static int vlv_setup_backlight(struct intel_connector *connector)
 	enum pipe pipe;
 	u32 ctl, ctl2, val;
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		u32 cur_val = I915_READ(VLV_BLC_PWM_CTL(pipe));
 
 		/* Skip if the modulation freq is already set */
diff --git a/drivers/gpu/drm/i915/intel_pm.c b/drivers/gpu/drm/i915/intel_pm.c
index 40c12295c0bd..c27b6140bfd1 100644
--- a/drivers/gpu/drm/i915/intel_pm.c
+++ b/drivers/gpu/drm/i915/intel_pm.c
@@ -309,6 +309,9 @@ static void gen7_enable_fbc(struct drm_crtc *crtc)
 
 	dpfc_ctl |= IVB_DPFC_CTL_FENCE_EN;
 
+	if (dev_priv->fbc.false_color)
+		dpfc_ctl |= FBC_CTL_FALSE_COLOR;
+
 	I915_WRITE(ILK_DPFC_CONTROL, dpfc_ctl | DPFC_CTL_EN);
 
 	if (IS_IVYBRIDGE(dev)) {
@@ -342,6 +345,16 @@ bool intel_fbc_enabled(struct drm_device *dev)
 	return dev_priv->display.fbc_enabled(dev);
 }
 
+void gen8_fbc_sw_flush(struct drm_device *dev, u32 value)
+{
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (!IS_GEN8(dev))
+		return;
+
+	I915_WRITE(MSG_FBC_REND_STATE, value);
+}
+
 static void intel_fbc_work_fn(struct work_struct *__work)
 {
 	struct intel_fbc_work *work =
@@ -578,6 +591,12 @@ void intel_update_fbc(struct drm_device *dev)
 			DRM_DEBUG_KMS("framebuffer not tiled or fenced, disabling compression\n");
 		goto out_disable;
 	}
+	if (INTEL_INFO(dev)->gen <= 4 && !IS_G4X(dev) &&
+	    to_intel_plane(crtc->primary)->rotation != BIT(DRM_ROTATE_0)) {
+		if (set_no_fbc_reason(dev_priv, FBC_UNSUPPORTED_MODE))
+			DRM_DEBUG_KMS("Rotation unsupported, disabling\n");
+		goto out_disable;
+	}
 
 	/* If the kernel debugger is active, always disable compression */
 	if (in_dbg_master())
@@ -853,7 +872,7 @@ void intel_set_memory_cxsr(struct drm_i915_private *dev_priv, bool enable)
  * A value of 5us seems to be a good balance; safe for very low end
  * platforms but not overly aggressive on lower latency configs.
  */
-static const int latency_ns = 5000;
+static const int pessimal_latency_ns = 5000;
 
 static int i9xx_get_fifo_size(struct drm_device *dev, int plane)
 {
@@ -982,13 +1001,20 @@ static const struct intel_watermark_params i915_wm_info = {
 	.guard_size = 2,
 	.cacheline_size = I915_FIFO_LINE_SIZE,
 };
-static const struct intel_watermark_params i830_wm_info = {
+static const struct intel_watermark_params i830_a_wm_info = {
 	.fifo_size = I855GM_FIFO_SIZE,
 	.max_wm = I915_MAX_WM,
 	.default_wm = 1,
 	.guard_size = 2,
 	.cacheline_size = I830_FIFO_LINE_SIZE,
 };
+static const struct intel_watermark_params i830_bc_wm_info = {
+	.fifo_size = I855GM_FIFO_SIZE,
+	.max_wm = I915_MAX_WM/2,
+	.default_wm = 1,
+	.guard_size = 2,
+	.cacheline_size = I830_FIFO_LINE_SIZE,
+};
 static const struct intel_watermark_params i845_wm_info = {
 	.fifo_size = I830_FIFO_SIZE,
 	.max_wm = I915_MAX_WM,
@@ -1044,6 +1070,17 @@ static unsigned long intel_calculate_wm(unsigned long clock_in_khz,
 		wm_size = wm->max_wm;
 	if (wm_size <= 0)
 		wm_size = wm->default_wm;
+
+	/*
+	 * Bspec seems to indicate that the value shouldn't be lower than
+	 * 'burst size + 1'. Certainly 830 is quite unhappy with low values.
+	 * Lets go for 8 which is the burst size since certain platforms
+	 * already use a hardcoded 8 (which is what the spec says should be
+	 * done).
+	 */
+	if (wm_size <= 8)
+		wm_size = 8;
+
 	return wm_size;
 }
 
@@ -1268,33 +1305,27 @@ static bool g4x_compute_srwm(struct drm_device *dev,
 			      display, cursor);
 }
 
-static bool vlv_compute_drain_latency(struct drm_device *dev,
-				     int plane,
-				     int *plane_prec_mult,
-				     int *plane_dl,
-				     int *cursor_prec_mult,
-				     int *cursor_dl)
+static bool vlv_compute_drain_latency(struct drm_crtc *crtc,
+				      int pixel_size,
+				      int *prec_mult,
+				      int *drain_latency)
 {
-	struct drm_crtc *crtc;
-	int clock, pixel_size;
 	int entries;
+	int clock = to_intel_crtc(crtc)->config.adjusted_mode.crtc_clock;
 
-	crtc = intel_get_crtc_for_plane(dev, plane);
-	if (!intel_crtc_active(crtc))
+	if (WARN(clock == 0, "Pixel clock is zero!\n"))
 		return false;
 
-	clock = to_intel_crtc(crtc)->config.adjusted_mode.crtc_clock;
-	pixel_size = crtc->primary->fb->bits_per_pixel / 8;	/* BPP */
+	if (WARN(pixel_size == 0, "Pixel size is zero!\n"))
+		return false;
 
-	entries = (clock / 1000) * pixel_size;
-	*plane_prec_mult = (entries > 128) ?
-		DRAIN_LATENCY_PRECISION_64 : DRAIN_LATENCY_PRECISION_32;
-	*plane_dl = (64 * (*plane_prec_mult) * 4) / entries;
+	entries = DIV_ROUND_UP(clock, 1000) * pixel_size;
+	*prec_mult = (entries > 128) ? DRAIN_LATENCY_PRECISION_64 :
+				       DRAIN_LATENCY_PRECISION_32;
+	*drain_latency = (64 * (*prec_mult) * 4) / entries;
 
-	entries = (clock / 1000) * 4;	/* BPP is always 4 for cursor */
-	*cursor_prec_mult = (entries > 128) ?
-		DRAIN_LATENCY_PRECISION_64 : DRAIN_LATENCY_PRECISION_32;
-	*cursor_dl = (64 * (*cursor_prec_mult) * 4) / entries;
+	if (*drain_latency > DRAIN_LATENCY_MASK)
+		*drain_latency = DRAIN_LATENCY_MASK;
 
 	return true;
 }
@@ -1307,39 +1338,48 @@ static bool vlv_compute_drain_latency(struct drm_device *dev,
  * latency value.
  */
 
-static void vlv_update_drain_latency(struct drm_device *dev)
+static void vlv_update_drain_latency(struct drm_crtc *crtc)
 {
-	struct drm_i915_private *dev_priv = dev->dev_private;
-	int planea_prec, planea_dl, planeb_prec, planeb_dl;
-	int cursora_prec, cursora_dl, cursorb_prec, cursorb_dl;
-	int plane_prec_mult, cursor_prec_mult; /* Precision multiplier is
-							either 16 or 32 */
+	struct drm_i915_private *dev_priv = crtc->dev->dev_private;
+	struct intel_crtc *intel_crtc = to_intel_crtc(crtc);
+	int pixel_size;
+	int drain_latency;
+	enum pipe pipe = intel_crtc->pipe;
+	int plane_prec, prec_mult, plane_dl;
+
+	plane_dl = I915_READ(VLV_DDL(pipe)) & ~(DDL_PLANE_PRECISION_64 |
+		   DRAIN_LATENCY_MASK | DDL_CURSOR_PRECISION_64 |
+		   (DRAIN_LATENCY_MASK << DDL_CURSOR_SHIFT));
 
-	/* For plane A, Cursor A */
-	if (vlv_compute_drain_latency(dev, 0, &plane_prec_mult, &planea_dl,
-				      &cursor_prec_mult, &cursora_dl)) {
-		cursora_prec = (cursor_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
-			DDL_CURSORA_PRECISION_32 : DDL_CURSORA_PRECISION_64;
-		planea_prec = (plane_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
-			DDL_PLANEA_PRECISION_32 : DDL_PLANEA_PRECISION_64;
+	if (!intel_crtc_active(crtc)) {
+		I915_WRITE(VLV_DDL(pipe), plane_dl);
+		return;
+	}
 
-		I915_WRITE(VLV_DDL1, cursora_prec |
-				(cursora_dl << DDL_CURSORA_SHIFT) |
-				planea_prec | planea_dl);
+	/* Primary plane Drain Latency */
+	pixel_size = crtc->primary->fb->bits_per_pixel / 8;	/* BPP */
+	if (vlv_compute_drain_latency(crtc, pixel_size, &prec_mult, &drain_latency)) {
+		plane_prec = (prec_mult == DRAIN_LATENCY_PRECISION_64) ?
+					   DDL_PLANE_PRECISION_64 :
+					   DDL_PLANE_PRECISION_32;
+		plane_dl |= plane_prec | drain_latency;
 	}
 
-	/* For plane B, Cursor B */
-	if (vlv_compute_drain_latency(dev, 1, &plane_prec_mult, &planeb_dl,
-				      &cursor_prec_mult, &cursorb_dl)) {
-		cursorb_prec = (cursor_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
-			DDL_CURSORB_PRECISION_32 : DDL_CURSORB_PRECISION_64;
-		planeb_prec = (plane_prec_mult == DRAIN_LATENCY_PRECISION_32) ?
-			DDL_PLANEB_PRECISION_32 : DDL_PLANEB_PRECISION_64;
+	/* Cursor Drain Latency
+	 * BPP is always 4 for cursor
+	 */
+	pixel_size = 4;
 
-		I915_WRITE(VLV_DDL2, cursorb_prec |
-				(cursorb_dl << DDL_CURSORB_SHIFT) |
-				planeb_prec | planeb_dl);
+	/* Program cursor DL only if it is enabled */
+	if (intel_crtc->cursor_base &&
+	    vlv_compute_drain_latency(crtc, pixel_size, &prec_mult, &drain_latency)) {
+		plane_prec = (prec_mult == DRAIN_LATENCY_PRECISION_64) ?
+					   DDL_CURSOR_PRECISION_64 :
+					   DDL_CURSOR_PRECISION_32;
+		plane_dl |= plane_prec | (drain_latency << DDL_CURSOR_SHIFT);
 	}
+
+	I915_WRITE(VLV_DDL(pipe), plane_dl);
 }
 
 #define single_plane_enabled(mask) is_power_of_2(mask)
@@ -1355,20 +1395,92 @@ static void valleyview_update_wm(struct drm_crtc *crtc)
 	unsigned int enabled = 0;
 	bool cxsr_enabled;
 
-	vlv_update_drain_latency(dev);
+	vlv_update_drain_latency(crtc);
+
+	if (g4x_compute_wm0(dev, PIPE_A,
+			    &valleyview_wm_info, pessimal_latency_ns,
+			    &valleyview_cursor_wm_info, pessimal_latency_ns,
+			    &planea_wm, &cursora_wm))
+		enabled |= 1 << PIPE_A;
+
+	if (g4x_compute_wm0(dev, PIPE_B,
+			    &valleyview_wm_info, pessimal_latency_ns,
+			    &valleyview_cursor_wm_info, pessimal_latency_ns,
+			    &planeb_wm, &cursorb_wm))
+		enabled |= 1 << PIPE_B;
+
+	if (single_plane_enabled(enabled) &&
+	    g4x_compute_srwm(dev, ffs(enabled) - 1,
+			     sr_latency_ns,
+			     &valleyview_wm_info,
+			     &valleyview_cursor_wm_info,
+			     &plane_sr, &ignore_cursor_sr) &&
+	    g4x_compute_srwm(dev, ffs(enabled) - 1,
+			     2*sr_latency_ns,
+			     &valleyview_wm_info,
+			     &valleyview_cursor_wm_info,
+			     &ignore_plane_sr, &cursor_sr)) {
+		cxsr_enabled = true;
+	} else {
+		cxsr_enabled = false;
+		intel_set_memory_cxsr(dev_priv, false);
+		plane_sr = cursor_sr = 0;
+	}
+
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, "
+		      "B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
+		      planea_wm, cursora_wm,
+		      planeb_wm, cursorb_wm,
+		      plane_sr, cursor_sr);
+
+	I915_WRITE(DSPFW1,
+		   (plane_sr << DSPFW_SR_SHIFT) |
+		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
+		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
+		   (planea_wm << DSPFW_PLANEA_SHIFT));
+	I915_WRITE(DSPFW2,
+		   (I915_READ(DSPFW2) & ~DSPFW_CURSORA_MASK) |
+		   (cursora_wm << DSPFW_CURSORA_SHIFT));
+	I915_WRITE(DSPFW3,
+		   (I915_READ(DSPFW3) & ~DSPFW_CURSOR_SR_MASK) |
+		   (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+
+	if (cxsr_enabled)
+		intel_set_memory_cxsr(dev_priv, true);
+}
+
+static void cherryview_update_wm(struct drm_crtc *crtc)
+{
+	struct drm_device *dev = crtc->dev;
+	static const int sr_latency_ns = 12000;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int planea_wm, planeb_wm, planec_wm;
+	int cursora_wm, cursorb_wm, cursorc_wm;
+	int plane_sr, cursor_sr;
+	int ignore_plane_sr, ignore_cursor_sr;
+	unsigned int enabled = 0;
+	bool cxsr_enabled;
+
+	vlv_update_drain_latency(crtc);
 
 	if (g4x_compute_wm0(dev, PIPE_A,
-			    &valleyview_wm_info, latency_ns,
-			    &valleyview_cursor_wm_info, latency_ns,
+			    &valleyview_wm_info, pessimal_latency_ns,
+			    &valleyview_cursor_wm_info, pessimal_latency_ns,
 			    &planea_wm, &cursora_wm))
 		enabled |= 1 << PIPE_A;
 
 	if (g4x_compute_wm0(dev, PIPE_B,
-			    &valleyview_wm_info, latency_ns,
-			    &valleyview_cursor_wm_info, latency_ns,
+			    &valleyview_wm_info, pessimal_latency_ns,
+			    &valleyview_cursor_wm_info, pessimal_latency_ns,
 			    &planeb_wm, &cursorb_wm))
 		enabled |= 1 << PIPE_B;
 
+	if (g4x_compute_wm0(dev, PIPE_C,
+			    &valleyview_wm_info, pessimal_latency_ns,
+			    &valleyview_cursor_wm_info, pessimal_latency_ns,
+			    &planec_wm, &cursorc_wm))
+		enabled |= 1 << PIPE_C;
+
 	if (single_plane_enabled(enabled) &&
 	    g4x_compute_srwm(dev, ffs(enabled) - 1,
 			     sr_latency_ns,
@@ -1387,27 +1499,66 @@ static void valleyview_update_wm(struct drm_crtc *crtc)
 		plane_sr = cursor_sr = 0;
 	}
 
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, "
+		      "B: plane=%d, cursor=%d, C: plane=%d, cursor=%d, "
+		      "SR: plane=%d, cursor=%d\n",
 		      planea_wm, cursora_wm,
 		      planeb_wm, cursorb_wm,
+		      planec_wm, cursorc_wm,
 		      plane_sr, cursor_sr);
 
 	I915_WRITE(DSPFW1,
 		   (plane_sr << DSPFW_SR_SHIFT) |
 		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
 		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
-		   planea_wm);
+		   (planea_wm << DSPFW_PLANEA_SHIFT));
 	I915_WRITE(DSPFW2,
 		   (I915_READ(DSPFW2) & ~DSPFW_CURSORA_MASK) |
 		   (cursora_wm << DSPFW_CURSORA_SHIFT));
 	I915_WRITE(DSPFW3,
 		   (I915_READ(DSPFW3) & ~DSPFW_CURSOR_SR_MASK) |
 		   (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
+	I915_WRITE(DSPFW9_CHV,
+		   (I915_READ(DSPFW9_CHV) & ~(DSPFW_PLANEC_MASK |
+					      DSPFW_CURSORC_MASK)) |
+		   (planec_wm << DSPFW_PLANEC_SHIFT) |
+		   (cursorc_wm << DSPFW_CURSORC_SHIFT));
 
 	if (cxsr_enabled)
 		intel_set_memory_cxsr(dev_priv, true);
 }
 
+static void valleyview_update_sprite_wm(struct drm_plane *plane,
+					struct drm_crtc *crtc,
+					uint32_t sprite_width,
+					uint32_t sprite_height,
+					int pixel_size,
+					bool enabled, bool scaled)
+{
+	struct drm_device *dev = crtc->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+	int pipe = to_intel_plane(plane)->pipe;
+	int sprite = to_intel_plane(plane)->plane;
+	int drain_latency;
+	int plane_prec;
+	int sprite_dl;
+	int prec_mult;
+
+	sprite_dl = I915_READ(VLV_DDL(pipe)) & ~(DDL_SPRITE_PRECISION_64(sprite) |
+		    (DRAIN_LATENCY_MASK << DDL_SPRITE_SHIFT(sprite)));
+
+	if (enabled && vlv_compute_drain_latency(crtc, pixel_size, &prec_mult,
+						 &drain_latency)) {
+		plane_prec = (prec_mult == DRAIN_LATENCY_PRECISION_64) ?
+					   DDL_SPRITE_PRECISION_64(sprite) :
+					   DDL_SPRITE_PRECISION_32(sprite);
+		sprite_dl |= plane_prec |
+			     (drain_latency << DDL_SPRITE_SHIFT(sprite));
+	}
+
+	I915_WRITE(VLV_DDL(pipe), sprite_dl);
+}
+
 static void g4x_update_wm(struct drm_crtc *crtc)
 {
 	struct drm_device *dev = crtc->dev;
@@ -1419,14 +1570,14 @@ static void g4x_update_wm(struct drm_crtc *crtc)
 	bool cxsr_enabled;
 
 	if (g4x_compute_wm0(dev, PIPE_A,
-			    &g4x_wm_info, latency_ns,
-			    &g4x_cursor_wm_info, latency_ns,
+			    &g4x_wm_info, pessimal_latency_ns,
+			    &g4x_cursor_wm_info, pessimal_latency_ns,
 			    &planea_wm, &cursora_wm))
 		enabled |= 1 << PIPE_A;
 
 	if (g4x_compute_wm0(dev, PIPE_B,
-			    &g4x_wm_info, latency_ns,
-			    &g4x_cursor_wm_info, latency_ns,
+			    &g4x_wm_info, pessimal_latency_ns,
+			    &g4x_cursor_wm_info, pessimal_latency_ns,
 			    &planeb_wm, &cursorb_wm))
 		enabled |= 1 << PIPE_B;
 
@@ -1443,7 +1594,8 @@ static void g4x_update_wm(struct drm_crtc *crtc)
 		plane_sr = cursor_sr = 0;
 	}
 
-	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
+	DRM_DEBUG_KMS("Setting FIFO watermarks - A: plane=%d, cursor=%d, "
+		      "B: plane=%d, cursor=%d, SR: plane=%d, cursor=%d\n",
 		      planea_wm, cursora_wm,
 		      planeb_wm, cursorb_wm,
 		      plane_sr, cursor_sr);
@@ -1452,7 +1604,7 @@ static void g4x_update_wm(struct drm_crtc *crtc)
 		   (plane_sr << DSPFW_SR_SHIFT) |
 		   (cursorb_wm << DSPFW_CURSORB_SHIFT) |
 		   (planeb_wm << DSPFW_PLANEB_SHIFT) |
-		   planea_wm);
+		   (planea_wm << DSPFW_PLANEA_SHIFT));
 	I915_WRITE(DSPFW2,
 		   (I915_READ(DSPFW2) & ~DSPFW_CURSORA_MASK) |
 		   (cursora_wm << DSPFW_CURSORA_SHIFT));
@@ -1526,8 +1678,11 @@ static void i965_update_wm(struct drm_crtc *unused_crtc)
 
 	/* 965 has limitations... */
 	I915_WRITE(DSPFW1, (srwm << DSPFW_SR_SHIFT) |
-		   (8 << 16) | (8 << 8) | (8 << 0));
-	I915_WRITE(DSPFW2, (8 << 8) | (8 << 0));
+		   (8 << DSPFW_CURSORB_SHIFT) |
+		   (8 << DSPFW_PLANEB_SHIFT) |
+		   (8 << DSPFW_PLANEA_SHIFT));
+	I915_WRITE(DSPFW2, (8 << DSPFW_CURSORA_SHIFT) |
+		   (8 << DSPFW_PLANEC_SHIFT_OLD));
 	/* update cursor SR watermark */
 	I915_WRITE(DSPFW3, (cursor_sr << DSPFW_CURSOR_SR_SHIFT));
 
@@ -1552,7 +1707,7 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 	else if (!IS_GEN2(dev))
 		wm_info = &i915_wm_info;
 	else
-		wm_info = &i830_wm_info;
+		wm_info = &i830_a_wm_info;
 
 	fifo_size = dev_priv->display.get_fifo_size(dev, 0);
 	crtc = intel_get_crtc_for_plane(dev, 0);
@@ -1565,10 +1720,16 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 		adjusted_mode = &to_intel_crtc(crtc)->config.adjusted_mode;
 		planea_wm = intel_calculate_wm(adjusted_mode->crtc_clock,
 					       wm_info, fifo_size, cpp,
-					       latency_ns);
+					       pessimal_latency_ns);
 		enabled = crtc;
-	} else
+	} else {
 		planea_wm = fifo_size - wm_info->guard_size;
+		if (planea_wm > (long)wm_info->max_wm)
+			planea_wm = wm_info->max_wm;
+	}
+
+	if (IS_GEN2(dev))
+		wm_info = &i830_bc_wm_info;
 
 	fifo_size = dev_priv->display.get_fifo_size(dev, 1);
 	crtc = intel_get_crtc_for_plane(dev, 1);
@@ -1581,13 +1742,16 @@ static void i9xx_update_wm(struct drm_crtc *unused_crtc)
 		adjusted_mode = &to_intel_crtc(crtc)->config.adjusted_mode;
 		planeb_wm = intel_calculate_wm(adjusted_mode->crtc_clock,
 					       wm_info, fifo_size, cpp,
-					       latency_ns);
+					       pessimal_latency_ns);
 		if (enabled == NULL)
 			enabled = crtc;
 		else
 			enabled = NULL;
-	} else
+	} else {
 		planeb_wm = fifo_size - wm_info->guard_size;
+		if (planeb_wm > (long)wm_info->max_wm)
+			planeb_wm = wm_info->max_wm;
+	}
 
 	DRM_DEBUG_KMS("FIFO watermarks - A: %d, B: %d\n", planea_wm, planeb_wm);
 
@@ -1674,7 +1838,7 @@ static void i845_update_wm(struct drm_crtc *unused_crtc)
 	planea_wm = intel_calculate_wm(adjusted_mode->crtc_clock,
 				       &i845_wm_info,
 				       dev_priv->display.get_fifo_size(dev, 0),
-				       4, latency_ns);
+				       4, pessimal_latency_ns);
 	fwater_lo = I915_READ(FW_BLC) & ~0xfff;
 	fwater_lo |= (3<<8) | planea_wm;
 
@@ -2527,7 +2691,7 @@ static struct intel_pipe_wm *ilk_find_best_result(struct drm_device *dev,
 #define WM_DIRTY_FBC (1 << 24)
 #define WM_DIRTY_DDB (1 << 25)
 
-static unsigned int ilk_compute_wm_dirty(struct drm_device *dev,
+static unsigned int ilk_compute_wm_dirty(struct drm_i915_private *dev_priv,
 					 const struct ilk_wm_values *old,
 					 const struct ilk_wm_values *new)
 {
@@ -2535,7 +2699,7 @@ static unsigned int ilk_compute_wm_dirty(struct drm_device *dev,
 	enum pipe pipe;
 	int wm_lp;
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		if (old->wm_linetime[pipe] != new->wm_linetime[pipe]) {
 			dirty |= WM_DIRTY_LINETIME(pipe);
 			/* Must disable LP1+ watermarks too */
@@ -2621,7 +2785,7 @@ static void ilk_write_wm_values(struct drm_i915_private *dev_priv,
 	unsigned int dirty;
 	uint32_t val;
 
-	dirty = ilk_compute_wm_dirty(dev, previous, results);
+	dirty = ilk_compute_wm_dirty(dev_priv, previous, results);
 	if (!dirty)
 		return;
 
@@ -3327,13 +3491,18 @@ void valleyview_set_rps(struct drm_device *dev, u8 val)
 	WARN_ON(val > dev_priv->rps.max_freq_softlimit);
 	WARN_ON(val < dev_priv->rps.min_freq_softlimit);
 
-	DRM_DEBUG_DRIVER("GPU freq request from %d MHz (%u) to %d MHz (%u)\n",
-			 vlv_gpu_freq(dev_priv, dev_priv->rps.cur_freq),
-			 dev_priv->rps.cur_freq,
-			 vlv_gpu_freq(dev_priv, val), val);
+	if (WARN_ONCE(IS_CHERRYVIEW(dev) && (val & 1),
+		      "Odd GPU freq value\n"))
+		val &= ~1;
+
+	if (val != dev_priv->rps.cur_freq) {
+		DRM_DEBUG_DRIVER("GPU freq request from %d MHz (%u) to %d MHz (%u)\n",
+				 vlv_gpu_freq(dev_priv, dev_priv->rps.cur_freq),
+				 dev_priv->rps.cur_freq,
+				 vlv_gpu_freq(dev_priv, val), val);
 
-	if (val != dev_priv->rps.cur_freq)
 		vlv_punit_write(dev_priv, PUNIT_REG_GPU_FREQ_REQ, val);
+	}
 
 	I915_WRITE(GEN6_PMINTRMSK, gen6_rps_pm_mask(dev_priv, val));
 
@@ -3406,8 +3575,14 @@ static void valleyview_disable_rps(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	/* we're doing forcewake before Disabling RC6,
+	 * This what the BIOS expects when going into suspend */
+	gen6_gt_force_wake_get(dev_priv, FORCEWAKE_ALL);
+
 	I915_WRITE(GEN6_RC_CONTROL, 0);
 
+	gen6_gt_force_wake_put(dev_priv, FORCEWAKE_ALL);
+
 	gen6_disable_rps_interrupts(dev);
 }
 
@@ -3598,7 +3773,6 @@ static void gen6_enable_rps(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	struct intel_engine_cs *ring;
 	u32 rp_state_cap;
-	u32 gt_perf_status;
 	u32 rc6vids, pcu_mbox = 0, rc6_mask = 0;
 	u32 gtfifodbg;
 	int rc6_mode;
@@ -3623,7 +3797,6 @@ static void gen6_enable_rps(struct drm_device *dev)
 	gen6_gt_force_wake_get(dev_priv, FORCEWAKE_ALL);
 
 	rp_state_cap = I915_READ(GEN6_RP_STATE_CAP);
-	gt_perf_status = I915_READ(GEN6_GT_PERF_STATUS);
 
 	parse_rp_state_cap(dev_priv, rp_state_cap);
 
@@ -3965,11 +4138,27 @@ static void valleyview_cleanup_pctx(struct drm_device *dev)
 static void valleyview_init_gt_powersave(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 val;
 
 	valleyview_setup_pctx(dev);
 
 	mutex_lock(&dev_priv->rps.hw_lock);
 
+	val = vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS);
+	switch ((val >> 6) & 3) {
+	case 0:
+	case 1:
+		dev_priv->mem_freq = 800;
+		break;
+	case 2:
+		dev_priv->mem_freq = 1066;
+		break;
+	case 3:
+		dev_priv->mem_freq = 1333;
+		break;
+	}
+	DRM_DEBUG_DRIVER("DDR speed: %d MHz", dev_priv->mem_freq);
+
 	dev_priv->rps.max_freq = valleyview_rps_max_freq(dev_priv);
 	dev_priv->rps.rp0_freq = dev_priv->rps.max_freq;
 	DRM_DEBUG_DRIVER("max GPU freq: %d MHz (%u)\n",
@@ -4004,11 +4193,38 @@ static void valleyview_init_gt_powersave(struct drm_device *dev)
 static void cherryview_init_gt_powersave(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
+	u32 val;
 
 	cherryview_setup_pctx(dev);
 
 	mutex_lock(&dev_priv->rps.hw_lock);
 
+	val = vlv_punit_read(dev_priv, CCK_FUSE_REG);
+	switch ((val >> 2) & 0x7) {
+	case 0:
+	case 1:
+		dev_priv->rps.cz_freq = 200;
+		dev_priv->mem_freq = 1600;
+		break;
+	case 2:
+		dev_priv->rps.cz_freq = 267;
+		dev_priv->mem_freq = 1600;
+		break;
+	case 3:
+		dev_priv->rps.cz_freq = 333;
+		dev_priv->mem_freq = 2000;
+		break;
+	case 4:
+		dev_priv->rps.cz_freq = 320;
+		dev_priv->mem_freq = 1600;
+		break;
+	case 5:
+		dev_priv->rps.cz_freq = 400;
+		dev_priv->mem_freq = 1600;
+		break;
+	}
+	DRM_DEBUG_DRIVER("DDR speed: %d MHz", dev_priv->mem_freq);
+
 	dev_priv->rps.max_freq = cherryview_rps_max_freq(dev_priv);
 	dev_priv->rps.rp0_freq = dev_priv->rps.max_freq;
 	DRM_DEBUG_DRIVER("max GPU freq: %d MHz (%u)\n",
@@ -4030,6 +4246,12 @@ static void cherryview_init_gt_powersave(struct drm_device *dev)
 			 vlv_gpu_freq(dev_priv, dev_priv->rps.min_freq),
 			 dev_priv->rps.min_freq);
 
+	WARN_ONCE((dev_priv->rps.max_freq |
+		   dev_priv->rps.efficient_freq |
+		   dev_priv->rps.rp1_freq |
+		   dev_priv->rps.min_freq) & 1,
+		  "Odd GPU freq values\n");
+
 	/* Preserve min/max settings in case of re-init */
 	if (dev_priv->rps.max_freq_softlimit == 0)
 		dev_priv->rps.max_freq_softlimit = dev_priv->rps.max_freq;
@@ -5088,7 +5310,7 @@ static void g4x_disable_trickle_feed(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	int pipe;
 
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		I915_WRITE(DSPCNTR(pipe),
 			   I915_READ(DSPCNTR(pipe)) |
 			   DISPPLANE_TRICKLE_FEED_DISABLE);
@@ -5203,7 +5425,7 @@ static void cpt_init_clock_gating(struct drm_device *dev)
 	/* The below fixes the weird display corruption, a few pixels shifted
 	 * downward, on (only) LVDS of some HP laptops with IVY.
 	 */
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		val = I915_READ(TRANS_CHICKEN2(pipe));
 		val |= TRANS_CHICKEN2_TIMING_OVERRIDE;
 		val &= ~TRANS_CHICKEN2_FDI_POLARITY_REVERSED;
@@ -5215,7 +5437,7 @@ static void cpt_init_clock_gating(struct drm_device *dev)
 		I915_WRITE(TRANS_CHICKEN2(pipe), val);
 	}
 	/* WADP0ClockGatingDisable */
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		I915_WRITE(TRANS_CHICKEN1(pipe),
 			   TRANS_CHICKEN1_DP0UNIT_GC_DISABLE);
 	}
@@ -5383,7 +5605,7 @@ static void lpt_suspend_hw(struct drm_device *dev)
 	}
 }
 
-static void gen8_init_clock_gating(struct drm_device *dev)
+static void broadwell_init_clock_gating(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
 	enum pipe pipe;
@@ -5395,37 +5617,12 @@ static void gen8_init_clock_gating(struct drm_device *dev)
 	/* FIXME(BDW): Check all the w/a, some might only apply to
 	 * pre-production hw. */
 
-	/* WaDisablePartialInstShootdown:bdw */
-	I915_WRITE(GEN8_ROW_CHICKEN,
-		   _MASKED_BIT_ENABLE(PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE));
 
-	/* WaDisableThreadStallDopClockGating:bdw */
-	/* FIXME: Unclear whether we really need this on production bdw. */
-	I915_WRITE(GEN8_ROW_CHICKEN,
-		   _MASKED_BIT_ENABLE(STALL_DOP_GATING_DISABLE));
-
-	/*
-	 * This GEN8_CENTROID_PIXEL_OPT_DIS W/A is only needed for
-	 * pre-production hardware
-	 */
-	I915_WRITE(HALF_SLICE_CHICKEN3,
-		   _MASKED_BIT_ENABLE(GEN8_CENTROID_PIXEL_OPT_DIS));
-	I915_WRITE(HALF_SLICE_CHICKEN3,
-		   _MASKED_BIT_ENABLE(GEN8_SAMPLER_POWER_BYPASS_DIS));
 	I915_WRITE(GAMTARBMODE, _MASKED_BIT_ENABLE(ARB_MODE_BWGTLB_DISABLE));
 
 	I915_WRITE(_3D_CHICKEN3,
 		   _MASKED_BIT_ENABLE(_3D_CHICKEN_SDE_LIMIT_FIFO_POLY_DEPTH(2)));
 
-	I915_WRITE(COMMON_SLICE_CHICKEN2,
-		   _MASKED_BIT_ENABLE(GEN8_CSC2_SBE_VUE_CACHE_CONSERVATIVE));
-
-	I915_WRITE(GEN7_HALF_SLICE_CHICKEN1,
-		   _MASKED_BIT_ENABLE(GEN7_SINGLE_SUBSCAN_DISPATCH_ENABLE));
-
-	/* WaDisableDopClockGating:bdw May not be needed for production */
-	I915_WRITE(GEN7_ROW_CHICKEN2,
-		   _MASKED_BIT_ENABLE(DOP_CLOCK_GATING_DISABLE));
 
 	/* WaSwitchSolVfFArbitrationPriority:bdw */
 	I915_WRITE(GAM_ECOCHK, I915_READ(GAM_ECOCHK) | HSW_ECOCHK_ARB_PRIO_SOL);
@@ -5435,37 +5632,18 @@ static void gen8_init_clock_gating(struct drm_device *dev)
 		   I915_READ(CHICKEN_PAR1_1) | DPA_MASK_VBLANK_SRD);
 
 	/* WaPsrDPRSUnmaskVBlankInSRD:bdw */
-	for_each_pipe(pipe) {
+	for_each_pipe(dev_priv, pipe) {
 		I915_WRITE(CHICKEN_PIPESL_1(pipe),
 			   I915_READ(CHICKEN_PIPESL_1(pipe)) |
 			   BDW_DPRS_MASK_VBLANK_SRD);
 	}
 
-	/* Use Force Non-Coherent whenever executing a 3D context. This is a
-	 * workaround for for a possible hang in the unlikely event a TLB
-	 * invalidation occurs during a PSD flush.
-	 */
-	I915_WRITE(HDC_CHICKEN0,
-		   I915_READ(HDC_CHICKEN0) |
-		   _MASKED_BIT_ENABLE(HDC_FORCE_NON_COHERENT));
-
 	/* WaVSRefCountFullforceMissDisable:bdw */
 	/* WaDSRefCountFullforceMissDisable:bdw */
 	I915_WRITE(GEN7_FF_THREAD_MODE,
 		   I915_READ(GEN7_FF_THREAD_MODE) &
 		   ~(GEN8_FF_DS_REF_CNT_FFME | GEN7_FF_VS_REF_CNT_FFME));
 
-	/*
-	 * BSpec recommends 8x4 when MSAA is used,
-	 * however in practice 16x4 seems fastest.
-	 *
-	 * Note that PS/WM thread counts depend on the WIZ hashing
-	 * disable bit, which we don't touch here, but it's good
-	 * to keep in mind (see 3DSTATE_PS and 3DSTATE_WM).
-	 */
-	I915_WRITE(GEN7_GT_MODE,
-		   GEN6_WIZ_HASHING_MASK | GEN6_WIZ_HASHING_16x4);
-
 	I915_WRITE(GEN6_RC_SLEEP_PSMI_CONTROL,
 		   _MASKED_BIT_ENABLE(GEN8_RC_SEMA_IDLE_MSG_DISABLE));
 
@@ -5473,9 +5651,7 @@ static void gen8_init_clock_gating(struct drm_device *dev)
 	I915_WRITE(GEN8_UCGCTL6, I915_READ(GEN8_UCGCTL6) |
 		   GEN8_SDEUNIT_CLOCK_GATE_DISABLE);
 
-	/* Wa4x4STCOptimizationDisable:bdw */
-	I915_WRITE(CACHE_MODE_1,
-		   _MASKED_BIT_ENABLE(GEN8_4x4_STC_OPTIMIZATION_DISABLE));
+	lpt_init_clock_gating(dev);
 }
 
 static void haswell_init_clock_gating(struct drm_device *dev)
@@ -5631,24 +5807,6 @@ static void ivybridge_init_clock_gating(struct drm_device *dev)
 static void valleyview_init_clock_gating(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 val;
-
-	mutex_lock(&dev_priv->rps.hw_lock);
-	val = vlv_punit_read(dev_priv, PUNIT_REG_GPU_FREQ_STS);
-	mutex_unlock(&dev_priv->rps.hw_lock);
-	switch ((val >> 6) & 3) {
-	case 0:
-	case 1:
-		dev_priv->mem_freq = 800;
-		break;
-	case 2:
-		dev_priv->mem_freq = 1066;
-		break;
-	case 3:
-		dev_priv->mem_freq = 1333;
-		break;
-	}
-	DRM_DEBUG_DRIVER("DDR speed: %d MHz", dev_priv->mem_freq);
 
 	I915_WRITE(DSPCLK_GATE_D, VRHUNIT_CLOCK_GATE_DISABLE);
 
@@ -5724,48 +5882,11 @@ static void valleyview_init_clock_gating(struct drm_device *dev)
 static void cherryview_init_clock_gating(struct drm_device *dev)
 {
 	struct drm_i915_private *dev_priv = dev->dev_private;
-	u32 val;
-
-	mutex_lock(&dev_priv->rps.hw_lock);
-	val = vlv_punit_read(dev_priv, CCK_FUSE_REG);
-	mutex_unlock(&dev_priv->rps.hw_lock);
-	switch ((val >> 2) & 0x7) {
-	case 0:
-	case 1:
-			dev_priv->rps.cz_freq = CHV_CZ_CLOCK_FREQ_MODE_200;
-			dev_priv->mem_freq = 1600;
-			break;
-	case 2:
-			dev_priv->rps.cz_freq = CHV_CZ_CLOCK_FREQ_MODE_267;
-			dev_priv->mem_freq = 1600;
-			break;
-	case 3:
-			dev_priv->rps.cz_freq = CHV_CZ_CLOCK_FREQ_MODE_333;
-			dev_priv->mem_freq = 2000;
-			break;
-	case 4:
-			dev_priv->rps.cz_freq = CHV_CZ_CLOCK_FREQ_MODE_320;
-			dev_priv->mem_freq = 1600;
-			break;
-	case 5:
-			dev_priv->rps.cz_freq = CHV_CZ_CLOCK_FREQ_MODE_400;
-			dev_priv->mem_freq = 1600;
-			break;
-	}
-	DRM_DEBUG_DRIVER("DDR speed: %d MHz", dev_priv->mem_freq);
 
 	I915_WRITE(DSPCLK_GATE_D, VRHUNIT_CLOCK_GATE_DISABLE);
 
 	I915_WRITE(MI_ARB_VLV, MI_ARB_DISPLAY_TRICKLE_FEED_DISABLE);
 
-	/* WaDisablePartialInstShootdown:chv */
-	I915_WRITE(GEN8_ROW_CHICKEN,
-		   _MASKED_BIT_ENABLE(PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE));
-
-	/* WaDisableThreadStallDopClockGating:chv */
-	I915_WRITE(GEN8_ROW_CHICKEN,
-		   _MASKED_BIT_ENABLE(STALL_DOP_GATING_DISABLE));
-
 	/* WaVSRefCountFullforceMissDisable:chv */
 	/* WaDSRefCountFullforceMissDisable:chv */
 	I915_WRITE(GEN7_FF_THREAD_MODE,
@@ -5784,10 +5905,6 @@ static void cherryview_init_clock_gating(struct drm_device *dev)
 	I915_WRITE(GEN8_UCGCTL6, I915_READ(GEN8_UCGCTL6) |
 		   GEN8_SDEUNIT_CLOCK_GATE_DISABLE);
 
-	/* WaDisableSamplerPowerBypass:chv (pre-production hw) */
-	I915_WRITE(HALF_SLICE_CHICKEN3,
-		   _MASKED_BIT_ENABLE(GEN8_SAMPLER_POWER_BYPASS_DIS));
-
 	/* WaDisableGunitClockGating:chv (pre-production hw) */
 	I915_WRITE(VLV_GUNIT_CLOCK_GATE, I915_READ(VLV_GUNIT_CLOCK_GATE) |
 		   GINT_DIS);
@@ -5797,8 +5914,6 @@ static void cherryview_init_clock_gating(struct drm_device *dev)
 		   _MASKED_BIT_ENABLE(GEN8_FF_DOP_CLOCK_GATE_DISABLE));
 
 	/* WaDisableDopClockGating:chv (pre-production hw) */
-	I915_WRITE(GEN7_ROW_CHICKEN2,
-		   _MASKED_BIT_ENABLE(DOP_CLOCK_GATING_DISABLE));
 	I915_WRITE(GEN6_UCGCTL1, I915_READ(GEN6_UCGCTL1) |
 		   GEN6_EU_TCUNIT_CLOCK_GATE_DISABLE);
 }
@@ -5883,6 +5998,9 @@ static void gen3_init_clock_gating(struct drm_device *dev)
 
 	/* On GEN3 we really need to make sure the ARB C3 LP bit is set */
 	I915_WRITE(MI_ARB_STATE, _MASKED_BIT_ENABLE(MI_ARB_C3_LP_WRITE_ENABLE));
+
+	I915_WRITE(MI_ARB_STATE,
+		   _MASKED_BIT_ENABLE(MI_ARB_DISPLAY_TRICKLE_FEED_DISABLE));
 }
 
 static void i85x_init_clock_gating(struct drm_device *dev)
@@ -5894,6 +6012,9 @@ static void i85x_init_clock_gating(struct drm_device *dev)
 	/* interrupts should cause a wake up from C3 */
 	I915_WRITE(MI_STATE, _MASKED_BIT_ENABLE(MI_AGPBUSY_INT_EN) |
 		   _MASKED_BIT_DISABLE(MI_AGPBUSY_830_MODE));
+
+	I915_WRITE(MEM_MODE,
+		   _MASKED_BIT_ENABLE(MEM_DISPLAY_TRICKLE_FEED_DISABLE));
 }
 
 static void i830_init_clock_gating(struct drm_device *dev)
@@ -5901,6 +6022,10 @@ static void i830_init_clock_gating(struct drm_device *dev)
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
 	I915_WRITE(DSPCLK_GATE_D, OVRUNIT_CLOCK_GATE_DISABLE);
+
+	I915_WRITE(MEM_MODE,
+		   _MASKED_BIT_ENABLE(MEM_DISPLAY_A_TRICKLE_FEED_DISABLE) |
+		   _MASKED_BIT_ENABLE(MEM_DISPLAY_B_TRICKLE_FEED_DISABLE));
 }
 
 void intel_init_clock_gating(struct drm_device *dev)
@@ -6203,6 +6328,8 @@ static void vlv_display_power_well_disable(struct drm_i915_private *dev_priv,
 	spin_unlock_irq(&dev_priv->irq_lock);
 
 	vlv_set_power_well(dev_priv, power_well, false);
+
+	vlv_power_sequencer_reset(dev_priv);
 }
 
 static void vlv_dpio_cmn_power_well_enable(struct drm_i915_private *dev_priv,
@@ -6238,12 +6365,11 @@ static void vlv_dpio_cmn_power_well_enable(struct drm_i915_private *dev_priv,
 static void vlv_dpio_cmn_power_well_disable(struct drm_i915_private *dev_priv,
 					    struct i915_power_well *power_well)
 {
-	struct drm_device *dev = dev_priv->dev;
 	enum pipe pipe;
 
 	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC);
 
-	for_each_pipe(pipe)
+	for_each_pipe(dev_priv, pipe)
 		assert_pll_disabled(dev_priv, pipe);
 
 	/* Assert common reset */
@@ -6252,6 +6378,153 @@ static void vlv_dpio_cmn_power_well_disable(struct drm_i915_private *dev_priv,
 	vlv_set_power_well(dev_priv, power_well, false);
 }
 
+static void chv_dpio_cmn_power_well_enable(struct drm_i915_private *dev_priv,
+					   struct i915_power_well *power_well)
+{
+	enum dpio_phy phy;
+
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC &&
+		     power_well->data != PUNIT_POWER_WELL_DPIO_CMN_D);
+
+	/*
+	 * Enable the CRI clock source so we can get at the
+	 * display and the reference clock for VGA
+	 * hotplug / manual detection.
+	 */
+	if (power_well->data == PUNIT_POWER_WELL_DPIO_CMN_BC) {
+		phy = DPIO_PHY0;
+		I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
+			   DPLL_REFA_CLK_ENABLE_VLV);
+		I915_WRITE(DPLL(PIPE_B), I915_READ(DPLL(PIPE_B)) |
+			   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
+	} else {
+		phy = DPIO_PHY1;
+		I915_WRITE(DPLL(PIPE_C), I915_READ(DPLL(PIPE_C)) |
+			   DPLL_REFA_CLK_ENABLE_VLV | DPLL_INTEGRATED_CRI_CLK_VLV);
+	}
+	udelay(1); /* >10ns for cmnreset, >0ns for sidereset */
+	vlv_set_power_well(dev_priv, power_well, true);
+
+	/* Poll for phypwrgood signal */
+	if (wait_for(I915_READ(DISPLAY_PHY_STATUS) & PHY_POWERGOOD(phy), 1))
+		DRM_ERROR("Display PHY %d is not power up\n", phy);
+
+	I915_WRITE(DISPLAY_PHY_CONTROL, I915_READ(DISPLAY_PHY_CONTROL) |
+		   PHY_COM_LANE_RESET_DEASSERT(phy));
+}
+
+static void chv_dpio_cmn_power_well_disable(struct drm_i915_private *dev_priv,
+					    struct i915_power_well *power_well)
+{
+	enum dpio_phy phy;
+
+	WARN_ON_ONCE(power_well->data != PUNIT_POWER_WELL_DPIO_CMN_BC &&
+		     power_well->data != PUNIT_POWER_WELL_DPIO_CMN_D);
+
+	if (power_well->data == PUNIT_POWER_WELL_DPIO_CMN_BC) {
+		phy = DPIO_PHY0;
+		assert_pll_disabled(dev_priv, PIPE_A);
+		assert_pll_disabled(dev_priv, PIPE_B);
+	} else {
+		phy = DPIO_PHY1;
+		assert_pll_disabled(dev_priv, PIPE_C);
+	}
+
+	I915_WRITE(DISPLAY_PHY_CONTROL, I915_READ(DISPLAY_PHY_CONTROL) &
+		   ~PHY_COM_LANE_RESET_DEASSERT(phy));
+
+	vlv_set_power_well(dev_priv, power_well, false);
+}
+
+static bool chv_pipe_power_well_enabled(struct drm_i915_private *dev_priv,
+					struct i915_power_well *power_well)
+{
+	enum pipe pipe = power_well->data;
+	bool enabled;
+	u32 state, ctrl;
+
+	mutex_lock(&dev_priv->rps.hw_lock);
+
+	state = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSS_MASK(pipe);
+	/*
+	 * We only ever set the power-on and power-gate states, anything
+	 * else is unexpected.
+	 */
+	WARN_ON(state != DP_SSS_PWR_ON(pipe) && state != DP_SSS_PWR_GATE(pipe));
+	enabled = state == DP_SSS_PWR_ON(pipe);
+
+	/*
+	 * A transient state at this point would mean some unexpected party
+	 * is poking at the power controls too.
+	 */
+	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSC_MASK(pipe);
+	WARN_ON(ctrl << 16 != state);
+
+	mutex_unlock(&dev_priv->rps.hw_lock);
+
+	return enabled;
+}
+
+static void chv_set_pipe_power_well(struct drm_i915_private *dev_priv,
+				    struct i915_power_well *power_well,
+				    bool enable)
+{
+	enum pipe pipe = power_well->data;
+	u32 state;
+	u32 ctrl;
+
+	state = enable ? DP_SSS_PWR_ON(pipe) : DP_SSS_PWR_GATE(pipe);
+
+	mutex_lock(&dev_priv->rps.hw_lock);
+
+#define COND \
+	((vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ) & DP_SSS_MASK(pipe)) == state)
+
+	if (COND)
+		goto out;
+
+	ctrl = vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ);
+	ctrl &= ~DP_SSC_MASK(pipe);
+	ctrl |= enable ? DP_SSC_PWR_ON(pipe) : DP_SSC_PWR_GATE(pipe);
+	vlv_punit_write(dev_priv, PUNIT_REG_DSPFREQ, ctrl);
+
+	if (wait_for(COND, 100))
+		DRM_ERROR("timout setting power well state %08x (%08x)\n",
+			  state,
+			  vlv_punit_read(dev_priv, PUNIT_REG_DSPFREQ));
+
+#undef COND
+
+out:
+	mutex_unlock(&dev_priv->rps.hw_lock);
+}
+
+static void chv_pipe_power_well_sync_hw(struct drm_i915_private *dev_priv,
+					struct i915_power_well *power_well)
+{
+	chv_set_pipe_power_well(dev_priv, power_well, power_well->count > 0);
+}
+
+static void chv_pipe_power_well_enable(struct drm_i915_private *dev_priv,
+				       struct i915_power_well *power_well)
+{
+	WARN_ON_ONCE(power_well->data != PIPE_A &&
+		     power_well->data != PIPE_B &&
+		     power_well->data != PIPE_C);
+
+	chv_set_pipe_power_well(dev_priv, power_well, true);
+}
+
+static void chv_pipe_power_well_disable(struct drm_i915_private *dev_priv,
+					struct i915_power_well *power_well)
+{
+	WARN_ON_ONCE(power_well->data != PIPE_A &&
+		     power_well->data != PIPE_B &&
+		     power_well->data != PIPE_C);
+
+	chv_set_pipe_power_well(dev_priv, power_well, false);
+}
+
 static void check_power_well_state(struct drm_i915_private *dev_priv,
 				   struct i915_power_well *power_well)
 {
@@ -6443,6 +6716,39 @@ EXPORT_SYMBOL_GPL(i915_get_cdclk_freq);
 	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
 	BIT(POWER_DOMAIN_INIT))
 
+#define CHV_PIPE_A_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PIPE_A) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_PIPE_B_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PIPE_B) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_PIPE_C_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PIPE_C) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_CMN_BC_POWER_DOMAINS (		\
+	BIT(POWER_DOMAIN_PORT_DDI_B_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_B_4_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_C_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_CMN_D_POWER_DOMAINS (		\
+	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_2_LANES) |	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
+#define CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS (	\
+	BIT(POWER_DOMAIN_PORT_DDI_D_4_LANES) |	\
+	BIT(POWER_DOMAIN_INIT))
+
 static const struct i915_power_well_ops i9xx_always_on_power_well_ops = {
 	.sync_hw = i9xx_always_on_power_well_noop,
 	.enable = i9xx_always_on_power_well_noop,
@@ -6450,6 +6756,20 @@ static const struct i915_power_well_ops i9xx_always_on_power_well_ops = {
 	.is_enabled = i9xx_always_on_power_well_enabled,
 };
 
+static const struct i915_power_well_ops chv_pipe_power_well_ops = {
+	.sync_hw = chv_pipe_power_well_sync_hw,
+	.enable = chv_pipe_power_well_enable,
+	.disable = chv_pipe_power_well_disable,
+	.is_enabled = chv_pipe_power_well_enabled,
+};
+
+static const struct i915_power_well_ops chv_dpio_cmn_power_well_ops = {
+	.sync_hw = vlv_power_well_sync_hw,
+	.enable = chv_dpio_cmn_power_well_enable,
+	.disable = chv_dpio_cmn_power_well_disable,
+	.is_enabled = vlv_power_well_enabled,
+};
+
 static struct i915_power_well i9xx_always_on_power_well[] = {
 	{
 		.name = "always-on",
@@ -6572,6 +6892,107 @@ static struct i915_power_well vlv_power_wells[] = {
 	},
 };
 
+static struct i915_power_well chv_power_wells[] = {
+	{
+		.name = "always-on",
+		.always_on = 1,
+		.domains = VLV_ALWAYS_ON_POWER_DOMAINS,
+		.ops = &i9xx_always_on_power_well_ops,
+	},
+#if 0
+	{
+		.name = "display",
+		.domains = VLV_DISPLAY_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DISP2D,
+		.ops = &vlv_display_power_well_ops,
+	},
+	{
+		.name = "pipe-a",
+		.domains = CHV_PIPE_A_POWER_DOMAINS,
+		.data = PIPE_A,
+		.ops = &chv_pipe_power_well_ops,
+	},
+	{
+		.name = "pipe-b",
+		.domains = CHV_PIPE_B_POWER_DOMAINS,
+		.data = PIPE_B,
+		.ops = &chv_pipe_power_well_ops,
+	},
+	{
+		.name = "pipe-c",
+		.domains = CHV_PIPE_C_POWER_DOMAINS,
+		.data = PIPE_C,
+		.ops = &chv_pipe_power_well_ops,
+	},
+#endif
+	{
+		.name = "dpio-common-bc",
+		/*
+		 * XXX: cmnreset for one PHY seems to disturb the other.
+		 * As a workaround keep both powered on at the same
+		 * time for now.
+		 */
+		.domains = CHV_DPIO_CMN_BC_POWER_DOMAINS | CHV_DPIO_CMN_D_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DPIO_CMN_BC,
+		.ops = &chv_dpio_cmn_power_well_ops,
+	},
+	{
+		.name = "dpio-common-d",
+		/*
+		 * XXX: cmnreset for one PHY seems to disturb the other.
+		 * As a workaround keep both powered on at the same
+		 * time for now.
+		 */
+		.domains = CHV_DPIO_CMN_BC_POWER_DOMAINS | CHV_DPIO_CMN_D_POWER_DOMAINS,
+		.data = PUNIT_POWER_WELL_DPIO_CMN_D,
+		.ops = &chv_dpio_cmn_power_well_ops,
+	},
+#if 0
+	{
+		.name = "dpio-tx-b-01",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_01,
+	},
+	{
+		.name = "dpio-tx-b-23",
+		.domains = VLV_DPIO_TX_B_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_B_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_B_LANES_23,
+	},
+	{
+		.name = "dpio-tx-c-01",
+		.domains = VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_01,
+	},
+	{
+		.name = "dpio-tx-c-23",
+		.domains = VLV_DPIO_TX_C_LANES_01_POWER_DOMAINS |
+			   VLV_DPIO_TX_C_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_C_LANES_23,
+	},
+	{
+		.name = "dpio-tx-d-01",
+		.domains = CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS |
+			   CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_D_LANES_01,
+	},
+	{
+		.name = "dpio-tx-d-23",
+		.domains = CHV_DPIO_TX_D_LANES_01_POWER_DOMAINS |
+			   CHV_DPIO_TX_D_LANES_23_POWER_DOMAINS,
+		.ops = &vlv_dpio_power_well_ops,
+		.data = PUNIT_POWER_WELL_DPIO_TX_D_LANES_23,
+	},
+#endif
+};
+
 static struct i915_power_well *lookup_power_well(struct drm_i915_private *dev_priv,
 						 enum punit_power_well power_well_id)
 {
@@ -6608,6 +7029,8 @@ int intel_power_domains_init(struct drm_i915_private *dev_priv)
 	} else if (IS_BROADWELL(dev_priv->dev)) {
 		set_power_wells(power_domains, bdw_power_wells);
 		hsw_pwr = power_domains;
+	} else if (IS_CHERRYVIEW(dev_priv->dev)) {
+		set_power_wells(power_domains, chv_power_wells);
 	} else if (IS_VALLEYVIEW(dev_priv->dev)) {
 		set_power_wells(power_domains, vlv_power_wells);
 	} else {
@@ -6833,13 +7256,15 @@ void intel_init_pm(struct drm_device *dev)
 		else if (IS_HASWELL(dev))
 			dev_priv->display.init_clock_gating = haswell_init_clock_gating;
 		else if (INTEL_INFO(dev)->gen == 8)
-			dev_priv->display.init_clock_gating = gen8_init_clock_gating;
+			dev_priv->display.init_clock_gating = broadwell_init_clock_gating;
 	} else if (IS_CHERRYVIEW(dev)) {
-		dev_priv->display.update_wm = valleyview_update_wm;
+		dev_priv->display.update_wm = cherryview_update_wm;
+		dev_priv->display.update_sprite_wm = valleyview_update_sprite_wm;
 		dev_priv->display.init_clock_gating =
 			cherryview_init_clock_gating;
 	} else if (IS_VALLEYVIEW(dev)) {
 		dev_priv->display.update_wm = valleyview_update_wm;
+		dev_priv->display.update_sprite_wm = valleyview_update_sprite_wm;
 		dev_priv->display.init_clock_gating =
 			valleyview_init_clock_gating;
 	} else if (IS_PINEVIEW(dev)) {
@@ -7025,6 +7450,7 @@ static int chv_freq_opcode(struct drm_i915_private *dev_priv, int val)
 		return -1;
 	}
 
+	/* CHV needs even values */
 	opcode = (DIV_ROUND_CLOSEST((val * 2 * mul), dev_priv->rps.cz_freq) * 2);
 
 	return opcode;
diff --git a/drivers/gpu/drm/i915/intel_renderstate.h b/drivers/gpu/drm/i915/intel_renderstate.h
index fd4f66231d30..6c792d3a9c9c 100644
--- a/drivers/gpu/drm/i915/intel_renderstate.h
+++ b/drivers/gpu/drm/i915/intel_renderstate.h
@@ -24,13 +24,7 @@
 #ifndef _INTEL_RENDERSTATE_H
 #define _INTEL_RENDERSTATE_H
 
-#include <linux/types.h>
-
-struct intel_renderstate_rodata {
-	const u32 *reloc;
-	const u32 *batch;
-	const u32 batch_items;
-};
+#include "i915_drv.h"
 
 extern const struct intel_renderstate_rodata gen6_null_state;
 extern const struct intel_renderstate_rodata gen7_null_state;
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.c b/drivers/gpu/drm/i915/intel_ringbuffer.c
index 16371a444426..0a80e419b589 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.c
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.c
@@ -33,14 +33,24 @@
 #include "i915_trace.h"
 #include "intel_drv.h"
 
-/* Early gen2 devices have a cacheline of just 32 bytes, using 64 is overkill,
- * but keeps the logic simple. Indeed, the whole purpose of this macro is just
- * to give some inclination as to some of the magic values used in the various
- * workarounds!
- */
-#define CACHELINE_BYTES 64
+bool
+intel_ring_initialized(struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+
+	if (!dev)
+		return false;
 
-static inline int __ring_space(int head, int tail, int size)
+	if (i915.enable_execlists) {
+		struct intel_context *dctx = ring->default_context;
+		struct intel_ringbuffer *ringbuf = dctx->engine[ring->id].ringbuf;
+
+		return ringbuf->obj;
+	} else
+		return ring->buffer && ring->buffer->obj;
+}
+
+int __intel_ring_space(int head, int tail, int size)
 {
 	int space = head - (tail + I915_RING_FREE_SPACE);
 	if (space < 0)
@@ -48,12 +58,13 @@ static inline int __ring_space(int head, int tail, int size)
 	return space;
 }
 
-static inline int ring_space(struct intel_ringbuffer *ringbuf)
+int intel_ring_space(struct intel_ringbuffer *ringbuf)
 {
-	return __ring_space(ringbuf->head & HEAD_ADDR, ringbuf->tail, ringbuf->size);
+	return __intel_ring_space(ringbuf->head & HEAD_ADDR,
+				  ringbuf->tail, ringbuf->size);
 }
 
-static bool intel_ring_stopped(struct intel_engine_cs *ring)
+bool intel_ring_stopped(struct intel_engine_cs *ring)
 {
 	struct drm_i915_private *dev_priv = ring->dev->dev_private;
 	return dev_priv->gpu_error.stop_rings & intel_ring_flag(ring);
@@ -433,7 +444,14 @@ gen8_render_ring_flush(struct intel_engine_cs *ring,
 			return ret;
 	}
 
-	return gen8_emit_pipe_control(ring, flags, scratch_addr);
+	ret = gen8_emit_pipe_control(ring, flags, scratch_addr);
+	if (ret)
+		return ret;
+
+	if (!invalidate_domains && flush_domains)
+		return gen7_ring_fbc_flush(ring, FBC_REND_NUKE);
+
+	return 0;
 }
 
 static void ring_write_tail(struct intel_engine_cs *ring,
@@ -476,9 +494,14 @@ static bool stop_ring(struct intel_engine_cs *ring)
 
 	if (!IS_GEN2(ring->dev)) {
 		I915_WRITE_MODE(ring, _MASKED_BIT_ENABLE(STOP_RING));
-		if (wait_for_atomic((I915_READ_MODE(ring) & MODE_IDLE) != 0, 1000)) {
-			DRM_ERROR("%s :timed out trying to stop ring\n", ring->name);
-			return false;
+		if (wait_for((I915_READ_MODE(ring) & MODE_IDLE) != 0, 1000)) {
+			DRM_ERROR("%s : timed out trying to stop ring\n", ring->name);
+			/* Sometimes we observe that the idle flag is not
+			 * set even though the ring is empty. So double
+			 * check before giving up.
+			 */
+			if (I915_READ_HEAD(ring) != I915_READ_TAIL(ring))
+				return false;
 		}
 	}
 
@@ -540,6 +563,14 @@ static int init_ring_common(struct intel_engine_cs *ring)
 	 * also enforces ordering), otherwise the hw might lose the new ring
 	 * register values. */
 	I915_WRITE_START(ring, i915_gem_obj_ggtt_offset(obj));
+
+	/* WaClearRingBufHeadRegAtInit:ctg,elk */
+	if (I915_READ_HEAD(ring))
+		DRM_DEBUG("%s initialization failed [head=%08x], fudging\n",
+			  ring->name, I915_READ_HEAD(ring));
+	I915_WRITE_HEAD(ring, 0);
+	(void)I915_READ_HEAD(ring);
+
 	I915_WRITE_CTL(ring,
 			((ringbuf->size - PAGE_SIZE) & RING_NR_PAGES)
 			| RING_VALID);
@@ -563,7 +594,7 @@ static int init_ring_common(struct intel_engine_cs *ring)
 	else {
 		ringbuf->head = I915_READ_HEAD(ring);
 		ringbuf->tail = I915_READ_TAIL(ring) & TAIL_ADDR;
-		ringbuf->space = ring_space(ringbuf);
+		ringbuf->space = intel_ring_space(ringbuf);
 		ringbuf->last_retired_head = -1;
 	}
 
@@ -575,8 +606,25 @@ out:
 	return ret;
 }
 
-static int
-init_pipe_control(struct intel_engine_cs *ring)
+void
+intel_fini_pipe_control(struct intel_engine_cs *ring)
+{
+	struct drm_device *dev = ring->dev;
+
+	if (ring->scratch.obj == NULL)
+		return;
+
+	if (INTEL_INFO(dev)->gen >= 5) {
+		kunmap(sg_page(ring->scratch.obj->pages->sgl));
+		i915_gem_object_ggtt_unpin(ring->scratch.obj);
+	}
+
+	drm_gem_object_unreference(&ring->scratch.obj->base);
+	ring->scratch.obj = NULL;
+}
+
+int
+intel_init_pipe_control(struct intel_engine_cs *ring)
 {
 	int ret;
 
@@ -617,6 +665,135 @@ err:
 	return ret;
 }
 
+static inline void intel_ring_emit_wa(struct intel_engine_cs *ring,
+				       u32 addr, u32 value)
+{
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	if (WARN_ON(dev_priv->num_wa_regs >= I915_MAX_WA_REGS))
+		return;
+
+	intel_ring_emit(ring, MI_LOAD_REGISTER_IMM(1));
+	intel_ring_emit(ring, addr);
+	intel_ring_emit(ring, value);
+
+	dev_priv->intel_wa_regs[dev_priv->num_wa_regs].addr = addr;
+	dev_priv->intel_wa_regs[dev_priv->num_wa_regs].mask = value & 0xFFFF;
+	/* value is updated with the status of remaining bits of this
+	 * register when it is read from debugfs file
+	 */
+	dev_priv->intel_wa_regs[dev_priv->num_wa_regs].value = value;
+	dev_priv->num_wa_regs++;
+
+	return;
+}
+
+static int bdw_init_workarounds(struct intel_engine_cs *ring)
+{
+	int ret;
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/*
+	 * workarounds applied in this fn are part of register state context,
+	 * they need to be re-initialized followed by gpu reset, suspend/resume,
+	 * module reload.
+	 */
+	dev_priv->num_wa_regs = 0;
+	memset(dev_priv->intel_wa_regs, 0, sizeof(dev_priv->intel_wa_regs));
+
+	/*
+	 * update the number of dwords required based on the
+	 * actual number of workarounds applied
+	 */
+	ret = intel_ring_begin(ring, 18);
+	if (ret)
+		return ret;
+
+	/* WaDisablePartialInstShootdown:bdw */
+	/* WaDisableThreadStallDopClockGating:bdw */
+	/* FIXME: Unclear whether we really need this on production bdw. */
+	intel_ring_emit_wa(ring, GEN8_ROW_CHICKEN,
+			   _MASKED_BIT_ENABLE(PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE
+					     | STALL_DOP_GATING_DISABLE));
+
+	/* WaDisableDopClockGating:bdw May not be needed for production */
+	intel_ring_emit_wa(ring, GEN7_ROW_CHICKEN2,
+			   _MASKED_BIT_ENABLE(DOP_CLOCK_GATING_DISABLE));
+
+	intel_ring_emit_wa(ring, HALF_SLICE_CHICKEN3,
+			   _MASKED_BIT_ENABLE(GEN8_SAMPLER_POWER_BYPASS_DIS));
+
+	/* Use Force Non-Coherent whenever executing a 3D context. This is a
+	 * workaround for for a possible hang in the unlikely event a TLB
+	 * invalidation occurs during a PSD flush.
+	 */
+	intel_ring_emit_wa(ring, HDC_CHICKEN0,
+			   _MASKED_BIT_ENABLE(HDC_FORCE_NON_COHERENT));
+
+	/* Wa4x4STCOptimizationDisable:bdw */
+	intel_ring_emit_wa(ring, CACHE_MODE_1,
+			   _MASKED_BIT_ENABLE(GEN8_4x4_STC_OPTIMIZATION_DISABLE));
+
+	/*
+	 * BSpec recommends 8x4 when MSAA is used,
+	 * however in practice 16x4 seems fastest.
+	 *
+	 * Note that PS/WM thread counts depend on the WIZ hashing
+	 * disable bit, which we don't touch here, but it's good
+	 * to keep in mind (see 3DSTATE_PS and 3DSTATE_WM).
+	 */
+	intel_ring_emit_wa(ring, GEN7_GT_MODE,
+			   GEN6_WIZ_HASHING_MASK | GEN6_WIZ_HASHING_16x4);
+
+	intel_ring_advance(ring);
+
+	DRM_DEBUG_DRIVER("Number of Workarounds applied: %d\n",
+			 dev_priv->num_wa_regs);
+
+	return 0;
+}
+
+static int chv_init_workarounds(struct intel_engine_cs *ring)
+{
+	int ret;
+	struct drm_device *dev = ring->dev;
+	struct drm_i915_private *dev_priv = dev->dev_private;
+
+	/*
+	 * workarounds applied in this fn are part of register state context,
+	 * they need to be re-initialized followed by gpu reset, suspend/resume,
+	 * module reload.
+	 */
+	dev_priv->num_wa_regs = 0;
+	memset(dev_priv->intel_wa_regs, 0, sizeof(dev_priv->intel_wa_regs));
+
+	ret = intel_ring_begin(ring, 12);
+	if (ret)
+		return ret;
+
+	/* WaDisablePartialInstShootdown:chv */
+	intel_ring_emit_wa(ring, GEN8_ROW_CHICKEN,
+			   _MASKED_BIT_ENABLE(PARTIAL_INSTRUCTION_SHOOTDOWN_DISABLE));
+
+	/* WaDisableThreadStallDopClockGating:chv */
+	intel_ring_emit_wa(ring, GEN8_ROW_CHICKEN,
+			   _MASKED_BIT_ENABLE(STALL_DOP_GATING_DISABLE));
+
+	/* WaDisableDopClockGating:chv (pre-production hw) */
+	intel_ring_emit_wa(ring, GEN7_ROW_CHICKEN2,
+			   _MASKED_BIT_ENABLE(DOP_CLOCK_GATING_DISABLE));
+
+	/* WaDisableSamplerPowerBypass:chv (pre-production hw) */
+	intel_ring_emit_wa(ring, HALF_SLICE_CHICKEN3,
+			   _MASKED_BIT_ENABLE(GEN8_SAMPLER_POWER_BYPASS_DIS));
+
+	intel_ring_advance(ring);
+
+	return 0;
+}
+
 static int init_render_ring(struct intel_engine_cs *ring)
 {
 	struct drm_device *dev = ring->dev;
@@ -651,7 +828,7 @@ static int init_render_ring(struct intel_engine_cs *ring)
 			   _MASKED_BIT_ENABLE(GFX_REPLAY_MODE));
 
 	if (INTEL_INFO(dev)->gen >= 5) {
-		ret = init_pipe_control(ring);
+		ret = intel_init_pipe_control(ring);
 		if (ret)
 			return ret;
 	}
@@ -686,16 +863,7 @@ static void render_ring_cleanup(struct intel_engine_cs *ring)
 		dev_priv->semaphore_obj = NULL;
 	}
 
-	if (ring->scratch.obj == NULL)
-		return;
-
-	if (INTEL_INFO(dev)->gen >= 5) {
-		kunmap(sg_page(ring->scratch.obj->pages->sgl));
-		i915_gem_object_ggtt_unpin(ring->scratch.obj);
-	}
-
-	drm_gem_object_unreference(&ring->scratch.obj->base);
-	ring->scratch.obj = NULL;
+	intel_fini_pipe_control(ring);
 }
 
 static int gen8_rcs_signal(struct intel_engine_cs *signaller,
@@ -1363,54 +1531,66 @@ i965_dispatch_execbuffer(struct intel_engine_cs *ring,
 
 /* Just userspace ABI convention to limit the wa batch bo to a resonable size */
 #define I830_BATCH_LIMIT (256*1024)
+#define I830_TLB_ENTRIES (2)
+#define I830_WA_SIZE max(I830_TLB_ENTRIES*4096, I830_BATCH_LIMIT)
 static int
 i830_dispatch_execbuffer(struct intel_engine_cs *ring,
 				u64 offset, u32 len,
 				unsigned flags)
 {
+	u32 cs_offset = ring->scratch.gtt_offset;
 	int ret;
 
-	if (flags & I915_DISPATCH_PINNED) {
-		ret = intel_ring_begin(ring, 4);
-		if (ret)
-			return ret;
+	ret = intel_ring_begin(ring, 6);
+	if (ret)
+		return ret;
 
-		intel_ring_emit(ring, MI_BATCH_BUFFER);
-		intel_ring_emit(ring, offset | (flags & I915_DISPATCH_SECURE ? 0 : MI_BATCH_NON_SECURE));
-		intel_ring_emit(ring, offset + len - 8);
-		intel_ring_emit(ring, MI_NOOP);
-		intel_ring_advance(ring);
-	} else {
-		u32 cs_offset = ring->scratch.gtt_offset;
+	/* Evict the invalid PTE TLBs */
+	intel_ring_emit(ring, COLOR_BLT_CMD | BLT_WRITE_RGBA);
+	intel_ring_emit(ring, BLT_DEPTH_32 | BLT_ROP_COLOR_COPY | 4096);
+	intel_ring_emit(ring, I830_TLB_ENTRIES << 16 | 4); /* load each page */
+	intel_ring_emit(ring, cs_offset);
+	intel_ring_emit(ring, 0xdeadbeef);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_advance(ring);
 
+	if ((flags & I915_DISPATCH_PINNED) == 0) {
 		if (len > I830_BATCH_LIMIT)
 			return -ENOSPC;
 
-		ret = intel_ring_begin(ring, 9+3);
+		ret = intel_ring_begin(ring, 6 + 2);
 		if (ret)
 			return ret;
-		/* Blit the batch (which has now all relocs applied) to the stable batch
-		 * scratch bo area (so that the CS never stumbles over its tlb
-		 * invalidation bug) ... */
-		intel_ring_emit(ring, XY_SRC_COPY_BLT_CMD |
-				XY_SRC_COPY_BLT_WRITE_ALPHA |
-				XY_SRC_COPY_BLT_WRITE_RGB);
-		intel_ring_emit(ring, BLT_DEPTH_32 | BLT_ROP_GXCOPY | 4096);
-		intel_ring_emit(ring, 0);
-		intel_ring_emit(ring, (DIV_ROUND_UP(len, 4096) << 16) | 1024);
+
+		/* Blit the batch (which has now all relocs applied) to the
+		 * stable batch scratch bo area (so that the CS never
+		 * stumbles over its tlb invalidation bug) ...
+		 */
+		intel_ring_emit(ring, SRC_COPY_BLT_CMD | BLT_WRITE_RGBA);
+		intel_ring_emit(ring, BLT_DEPTH_32 | BLT_ROP_SRC_COPY | 4096);
+		intel_ring_emit(ring, DIV_ROUND_UP(len, 4096) << 16 | 4096);
 		intel_ring_emit(ring, cs_offset);
-		intel_ring_emit(ring, 0);
 		intel_ring_emit(ring, 4096);
 		intel_ring_emit(ring, offset);
+
 		intel_ring_emit(ring, MI_FLUSH);
+		intel_ring_emit(ring, MI_NOOP);
+		intel_ring_advance(ring);
 
 		/* ... and execute it. */
-		intel_ring_emit(ring, MI_BATCH_BUFFER);
-		intel_ring_emit(ring, cs_offset | (flags & I915_DISPATCH_SECURE ? 0 : MI_BATCH_NON_SECURE));
-		intel_ring_emit(ring, cs_offset + len - 8);
-		intel_ring_advance(ring);
+		offset = cs_offset;
 	}
 
+	ret = intel_ring_begin(ring, 4);
+	if (ret)
+		return ret;
+
+	intel_ring_emit(ring, MI_BATCH_BUFFER);
+	intel_ring_emit(ring, offset | (flags & I915_DISPATCH_SECURE ? 0 : MI_BATCH_NON_SECURE));
+	intel_ring_emit(ring, offset + len - 8);
+	intel_ring_emit(ring, MI_NOOP);
+	intel_ring_advance(ring);
+
 	return 0;
 }
 
@@ -1514,7 +1694,7 @@ static int init_phys_status_page(struct intel_engine_cs *ring)
 	return 0;
 }
 
-static void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
+void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
 {
 	if (!ringbuf->obj)
 		return;
@@ -1525,8 +1705,8 @@ static void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf)
 	ringbuf->obj = NULL;
 }
 
-static int intel_alloc_ringbuffer_obj(struct drm_device *dev,
-				      struct intel_ringbuffer *ringbuf)
+int intel_alloc_ringbuffer_obj(struct drm_device *dev,
+			       struct intel_ringbuffer *ringbuf)
 {
 	struct drm_i915_private *dev_priv = to_i915(dev);
 	struct drm_i915_gem_object *obj;
@@ -1588,7 +1768,9 @@ static int intel_init_ring_buffer(struct drm_device *dev,
 	ring->dev = dev;
 	INIT_LIST_HEAD(&ring->active_list);
 	INIT_LIST_HEAD(&ring->request_list);
+	INIT_LIST_HEAD(&ring->execlist_queue);
 	ringbuf->size = 32 * PAGE_SIZE;
+	ringbuf->ring = ring;
 	memset(ring->semaphore.sync_seqno, 0, sizeof(ring->semaphore.sync_seqno));
 
 	init_waitqueue_head(&ring->irq_queue);
@@ -1671,13 +1853,14 @@ static int intel_ring_wait_request(struct intel_engine_cs *ring, int n)
 		ringbuf->head = ringbuf->last_retired_head;
 		ringbuf->last_retired_head = -1;
 
-		ringbuf->space = ring_space(ringbuf);
+		ringbuf->space = intel_ring_space(ringbuf);
 		if (ringbuf->space >= n)
 			return 0;
 	}
 
 	list_for_each_entry(request, &ring->request_list, list) {
-		if (__ring_space(request->tail, ringbuf->tail, ringbuf->size) >= n) {
+		if (__intel_ring_space(request->tail, ringbuf->tail,
+				       ringbuf->size) >= n) {
 			seqno = request->seqno;
 			break;
 		}
@@ -1694,7 +1877,7 @@ static int intel_ring_wait_request(struct intel_engine_cs *ring, int n)
 	ringbuf->head = ringbuf->last_retired_head;
 	ringbuf->last_retired_head = -1;
 
-	ringbuf->space = ring_space(ringbuf);
+	ringbuf->space = intel_ring_space(ringbuf);
 	return 0;
 }
 
@@ -1723,7 +1906,7 @@ static int ring_wait_for_space(struct intel_engine_cs *ring, int n)
 	trace_i915_ring_wait_begin(ring);
 	do {
 		ringbuf->head = I915_READ_HEAD(ring);
-		ringbuf->space = ring_space(ringbuf);
+		ringbuf->space = intel_ring_space(ringbuf);
 		if (ringbuf->space >= n) {
 			ret = 0;
 			break;
@@ -1775,7 +1958,7 @@ static int intel_wrap_ring_buffer(struct intel_engine_cs *ring)
 		iowrite32(MI_NOOP, virt++);
 
 	ringbuf->tail = 0;
-	ringbuf->space = ring_space(ringbuf);
+	ringbuf->space = intel_ring_space(ringbuf);
 
 	return 0;
 }
@@ -1980,9 +2163,7 @@ gen8_ring_dispatch_execbuffer(struct intel_engine_cs *ring,
 			      u64 offset, u32 len,
 			      unsigned flags)
 {
-	struct drm_i915_private *dev_priv = ring->dev->dev_private;
-	bool ppgtt = dev_priv->mm.aliasing_ppgtt != NULL &&
-		!(flags & I915_DISPATCH_SECURE);
+	bool ppgtt = USES_PPGTT(ring->dev) && !(flags & I915_DISPATCH_SECURE);
 	int ret;
 
 	ret = intel_ring_begin(ring, 4);
@@ -2011,8 +2192,9 @@ hsw_ring_dispatch_execbuffer(struct intel_engine_cs *ring,
 		return ret;
 
 	intel_ring_emit(ring,
-			MI_BATCH_BUFFER_START | MI_BATCH_PPGTT_HSW |
-			(flags & I915_DISPATCH_SECURE ? 0 : MI_BATCH_NON_SECURE_HSW));
+			MI_BATCH_BUFFER_START |
+			(flags & I915_DISPATCH_SECURE ?
+			 0 : MI_BATCH_PPGTT_HSW | MI_BATCH_NON_SECURE_HSW));
 	/* bit0-7 is the length on GEN6+ */
 	intel_ring_emit(ring, offset);
 	intel_ring_advance(ring);
@@ -2111,6 +2293,10 @@ int intel_init_render_ring_buffer(struct drm_device *dev)
 					dev_priv->semaphore_obj = obj;
 			}
 		}
+		if (IS_CHERRYVIEW(dev))
+			ring->init_context = chv_init_workarounds;
+		else
+			ring->init_context = bdw_init_workarounds;
 		ring->add_request = gen6_add_request;
 		ring->flush = gen8_render_ring_flush;
 		ring->irq_get = gen8_ring_get_irq;
@@ -2200,7 +2386,7 @@ int intel_init_render_ring_buffer(struct drm_device *dev)
 
 	/* Workaround batchbuffer to combat CS tlb bug. */
 	if (HAS_BROKEN_CS_TLB(dev)) {
-		obj = i915_gem_alloc_object(dev, I830_BATCH_LIMIT);
+		obj = i915_gem_alloc_object(dev, I830_WA_SIZE);
 		if (obj == NULL) {
 			DRM_ERROR("Failed to allocate batch bo\n");
 			return -ENOMEM;
diff --git a/drivers/gpu/drm/i915/intel_ringbuffer.h b/drivers/gpu/drm/i915/intel_ringbuffer.h
index 70525d0c2c74..96479c89f4bd 100644
--- a/drivers/gpu/drm/i915/intel_ringbuffer.h
+++ b/drivers/gpu/drm/i915/intel_ringbuffer.h
@@ -5,6 +5,13 @@
 
 #define I915_CMD_HASH_ORDER 9
 
+/* Early gen2 devices have a cacheline of just 32 bytes, using 64 is overkill,
+ * but keeps the logic simple. Indeed, the whole purpose of this macro is just
+ * to give some inclination as to some of the magic values used in the various
+ * workarounds!
+ */
+#define CACHELINE_BYTES 64
+
 /*
  * Gen2 BSpec "1. Programming Environment" / 1.4.4.6 "Ring Buffer Use"
  * Gen3 BSpec "vol1c Memory Interface Functions" / 2.3.4.5 "Ring Buffer Use"
@@ -90,6 +97,15 @@ struct intel_ringbuffer {
 	struct drm_i915_gem_object *obj;
 	void __iomem *virtual_start;
 
+	struct intel_engine_cs *ring;
+
+	/*
+	 * FIXME: This backpointer is an artifact of the history of how the
+	 * execlist patches came into being. It will get removed once the basic
+	 * code has landed.
+	 */
+	struct intel_context *FIXME_lrc_ctx;
+
 	u32 head;
 	u32 tail;
 	int space;
@@ -132,6 +148,8 @@ struct  intel_engine_cs {
 
 	int		(*init)(struct intel_engine_cs *ring);
 
+	int		(*init_context)(struct intel_engine_cs *ring);
+
 	void		(*write_tail)(struct intel_engine_cs *ring,
 				      u32 value);
 	int __must_check (*flush)(struct intel_engine_cs *ring,
@@ -214,6 +232,18 @@ struct  intel_engine_cs {
 				  unsigned int num_dwords);
 	} semaphore;
 
+	/* Execlists */
+	spinlock_t execlist_lock;
+	struct list_head execlist_queue;
+	u8 next_context_status_buffer;
+	u32             irq_keep_mask; /* bitmask for interrupts that should not be masked */
+	int		(*emit_request)(struct intel_ringbuffer *ringbuf);
+	int		(*emit_flush)(struct intel_ringbuffer *ringbuf,
+				      u32 invalidate_domains,
+				      u32 flush_domains);
+	int		(*emit_bb_start)(struct intel_ringbuffer *ringbuf,
+					 u64 offset, unsigned flags);
+
 	/**
 	 * List of objects currently involved in rendering from the
 	 * ringbuffer.
@@ -287,11 +317,7 @@ struct  intel_engine_cs {
 	u32 (*get_cmd_length_mask)(u32 cmd_header);
 };
 
-static inline bool
-intel_ring_initialized(struct intel_engine_cs *ring)
-{
-	return ring->buffer && ring->buffer->obj;
-}
+bool intel_ring_initialized(struct intel_engine_cs *ring);
 
 static inline unsigned
 intel_ring_flag(struct intel_engine_cs *ring)
@@ -355,6 +381,10 @@ intel_write_status_page(struct intel_engine_cs *ring,
 #define I915_GEM_HWS_SCRATCH_INDEX	0x30
 #define I915_GEM_HWS_SCRATCH_ADDR (I915_GEM_HWS_SCRATCH_INDEX << MI_STORE_DWORD_INDEX_SHIFT)
 
+void intel_destroy_ringbuffer_obj(struct intel_ringbuffer *ringbuf);
+int intel_alloc_ringbuffer_obj(struct drm_device *dev,
+			       struct intel_ringbuffer *ringbuf);
+
 void intel_stop_ring_buffer(struct intel_engine_cs *ring);
 void intel_cleanup_ring_buffer(struct intel_engine_cs *ring);
 
@@ -372,6 +402,9 @@ static inline void intel_ring_advance(struct intel_engine_cs *ring)
 	struct intel_ringbuffer *ringbuf = ring->buffer;
 	ringbuf->tail &= ringbuf->size - 1;
 }
+int __intel_ring_space(int head, int tail, int size);
+int intel_ring_space(struct intel_ringbuffer *ringbuf);
+bool intel_ring_stopped(struct intel_engine_cs *ring);
 void __intel_ring_advance(struct intel_engine_cs *ring);
 
 int __must_check intel_ring_idle(struct intel_engine_cs *ring);
@@ -379,6 +412,9 @@ void intel_ring_init_seqno(struct intel_engine_cs *ring, u32 seqno);
 int intel_ring_flush_all_caches(struct intel_engine_cs *ring);
 int intel_ring_invalidate_all_caches(struct intel_engine_cs *ring);
 
+void intel_fini_pipe_control(struct intel_engine_cs *ring);
+int intel_init_pipe_control(struct intel_engine_cs *ring);
+
 int intel_init_render_ring_buffer(struct drm_device *dev);
 int intel_init_bsd_ring_buffer(struct drm_device *dev);
 int intel_init_bsd2_ring_buffer(struct drm_device *dev);
diff --git a/drivers/gpu/drm/i915/intel_sprite.c b/drivers/gpu/drm/i915/intel_sprite.c
index 168c6652cda1..07a74ef589bd 100644
--- a/drivers/gpu/drm/i915/intel_sprite.c
+++ b/drivers/gpu/drm/i915/intel_sprite.c
@@ -53,6 +53,7 @@ static bool intel_pipe_update_start(struct intel_crtc *crtc, uint32_t *start_vbl
 	enum pipe pipe = crtc->pipe;
 	long timeout = msecs_to_jiffies_timeout(1);
 	int scanline, min, max, vblank_start;
+	wait_queue_head_t *wq = drm_crtc_vblank_waitqueue(&crtc->base);
 	DEFINE_WAIT(wait);
 
 	WARN_ON(!drm_modeset_is_locked(&crtc->base.mutex));
@@ -81,7 +82,7 @@ static bool intel_pipe_update_start(struct intel_crtc *crtc, uint32_t *start_vbl
 		 * other CPUs can see the task state update by the time we
 		 * read the scanline.
 		 */
-		prepare_to_wait(&crtc->vbl_wait, &wait, TASK_UNINTERRUPTIBLE);
+		prepare_to_wait(wq, &wait, TASK_UNINTERRUPTIBLE);
 
 		scanline = intel_get_crtc_scanline(crtc);
 		if (scanline < min || scanline > max)
@@ -100,7 +101,7 @@ static bool intel_pipe_update_start(struct intel_crtc *crtc, uint32_t *start_vbl
 		local_irq_disable();
 	}
 
-	finish_wait(&crtc->vbl_wait, &wait);
+	finish_wait(wq, &wait);
 
 	drm_vblank_put(dev, pipe);
 
@@ -163,6 +164,7 @@ vlv_update_plane(struct drm_plane *dplane, struct drm_crtc *crtc,
 	sprctl &= ~SP_PIXFORMAT_MASK;
 	sprctl &= ~SP_YUV_BYTE_ORDER_MASK;
 	sprctl &= ~SP_TILED;
+	sprctl &= ~SP_ROTATE_180;
 
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_YUYV:
@@ -235,6 +237,14 @@ vlv_update_plane(struct drm_plane *dplane, struct drm_crtc *crtc,
 							fb->pitches[0]);
 	linear_offset -= sprsurf_offset;
 
+	if (intel_plane->rotation == BIT(DRM_ROTATE_180)) {
+		sprctl |= SP_ROTATE_180;
+
+		x += src_w;
+		y += src_h;
+		linear_offset += src_h * fb->pitches[0] + src_w * pixel_size;
+	}
+
 	atomic_update = intel_pipe_update_start(intel_crtc, &start_vbl_count);
 
 	intel_update_primary_plane(intel_crtc);
@@ -364,6 +374,7 @@ ivb_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	sprctl &= ~SPRITE_RGB_ORDER_RGBX;
 	sprctl &= ~SPRITE_YUV_BYTE_ORDER_MASK;
 	sprctl &= ~SPRITE_TILED;
+	sprctl &= ~SPRITE_ROTATE_180;
 
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_XBGR8888:
@@ -426,6 +437,18 @@ ivb_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 					       pixel_size, fb->pitches[0]);
 	linear_offset -= sprsurf_offset;
 
+	if (intel_plane->rotation == BIT(DRM_ROTATE_180)) {
+		sprctl |= SPRITE_ROTATE_180;
+
+		/* HSW and BDW does this automagically in hardware */
+		if (!IS_HASWELL(dev) && !IS_BROADWELL(dev)) {
+			x += src_w;
+			y += src_h;
+			linear_offset += src_h * fb->pitches[0] +
+				src_w * pixel_size;
+		}
+	}
+
 	atomic_update = intel_pipe_update_start(intel_crtc, &start_vbl_count);
 
 	intel_update_primary_plane(intel_crtc);
@@ -571,6 +594,7 @@ ilk_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	dvscntr &= ~DVS_RGB_ORDER_XBGR;
 	dvscntr &= ~DVS_YUV_BYTE_ORDER_MASK;
 	dvscntr &= ~DVS_TILED;
+	dvscntr &= ~DVS_ROTATE_180;
 
 	switch (fb->pixel_format) {
 	case DRM_FORMAT_XBGR8888:
@@ -628,6 +652,14 @@ ilk_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 					       pixel_size, fb->pitches[0]);
 	linear_offset -= dvssurf_offset;
 
+	if (intel_plane->rotation == BIT(DRM_ROTATE_180)) {
+		dvscntr |= DVS_ROTATE_180;
+
+		x += src_w;
+		y += src_h;
+		linear_offset += src_h * fb->pitches[0] + src_w * pixel_size;
+	}
+
 	atomic_update = intel_pipe_update_start(intel_crtc, &start_vbl_count);
 
 	intel_update_primary_plane(intel_crtc);
@@ -895,6 +927,9 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 	max_scale = intel_plane->max_downscale << 16;
 	min_scale = intel_plane->can_scale ? 1 : (1 << 16);
 
+	drm_rect_rotate(&src, fb->width << 16, fb->height << 16,
+			intel_plane->rotation);
+
 	hscale = drm_rect_calc_hscale_relaxed(&src, &dst, min_scale, max_scale);
 	BUG_ON(hscale < 0);
 
@@ -933,6 +968,9 @@ intel_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 				     drm_rect_width(&dst) * hscale - drm_rect_width(&src),
 				     drm_rect_height(&dst) * vscale - drm_rect_height(&src));
 
+		drm_rect_rotate_inv(&src, fb->width << 16, fb->height << 16,
+				    intel_plane->rotation);
+
 		/* sanity check to make sure the src viewport wasn't enlarged */
 		WARN_ON(src.x1 < (int) src_x ||
 			src.y1 < (int) src_y ||
@@ -1180,18 +1218,45 @@ out_unlock:
 	return ret;
 }
 
-void intel_plane_restore(struct drm_plane *plane)
+int intel_plane_set_property(struct drm_plane *plane,
+			     struct drm_property *prop,
+			     uint64_t val)
+{
+	struct drm_device *dev = plane->dev;
+	struct intel_plane *intel_plane = to_intel_plane(plane);
+	uint64_t old_val;
+	int ret = -ENOENT;
+
+	if (prop == dev->mode_config.rotation_property) {
+		/* exactly one rotation angle please */
+		if (hweight32(val & 0xf) != 1)
+			return -EINVAL;
+
+		if (intel_plane->rotation == val)
+			return 0;
+
+		old_val = intel_plane->rotation;
+		intel_plane->rotation = val;
+		ret = intel_plane_restore(plane);
+		if (ret)
+			intel_plane->rotation = old_val;
+	}
+
+	return ret;
+}
+
+int intel_plane_restore(struct drm_plane *plane)
 {
 	struct intel_plane *intel_plane = to_intel_plane(plane);
 
 	if (!plane->crtc || !plane->fb)
-		return;
+		return 0;
 
-	intel_update_plane(plane, plane->crtc, plane->fb,
-			   intel_plane->crtc_x, intel_plane->crtc_y,
-			   intel_plane->crtc_w, intel_plane->crtc_h,
-			   intel_plane->src_x, intel_plane->src_y,
-			   intel_plane->src_w, intel_plane->src_h);
+	return plane->funcs->update_plane(plane, plane->crtc, plane->fb,
+				  intel_plane->crtc_x, intel_plane->crtc_y,
+				  intel_plane->crtc_w, intel_plane->crtc_h,
+				  intel_plane->src_x, intel_plane->src_y,
+				  intel_plane->src_w, intel_plane->src_h);
 }
 
 void intel_plane_disable(struct drm_plane *plane)
@@ -1206,6 +1271,7 @@ static const struct drm_plane_funcs intel_plane_funcs = {
 	.update_plane = intel_update_plane,
 	.disable_plane = intel_disable_plane,
 	.destroy = intel_destroy_plane,
+	.set_property = intel_plane_set_property,
 };
 
 static uint32_t ilk_plane_formats[] = {
@@ -1310,13 +1376,28 @@ intel_plane_init(struct drm_device *dev, enum pipe pipe, int plane)
 
 	intel_plane->pipe = pipe;
 	intel_plane->plane = plane;
+	intel_plane->rotation = BIT(DRM_ROTATE_0);
 	possible_crtcs = (1 << pipe);
-	ret = drm_plane_init(dev, &intel_plane->base, possible_crtcs,
-			     &intel_plane_funcs,
-			     plane_formats, num_plane_formats,
-			     false);
-	if (ret)
+	ret = drm_universal_plane_init(dev, &intel_plane->base, possible_crtcs,
+				       &intel_plane_funcs,
+				       plane_formats, num_plane_formats,
+				       DRM_PLANE_TYPE_OVERLAY);
+	if (ret) {
 		kfree(intel_plane);
+		goto out;
+	}
+
+	if (!dev->mode_config.rotation_property)
+		dev->mode_config.rotation_property =
+			drm_mode_create_rotation_property(dev,
+							  BIT(DRM_ROTATE_0) |
+							  BIT(DRM_ROTATE_180));
+
+	if (dev->mode_config.rotation_property)
+		drm_object_attach_property(&intel_plane->base.base,
+					   dev->mode_config.rotation_property,
+					   intel_plane->rotation);
 
+ out:
 	return ret;
 }
diff --git a/drivers/gpu/drm/i915/intel_tv.c b/drivers/gpu/drm/i915/intel_tv.c
index e211eef4b7e4..c14341ca3ef9 100644
--- a/drivers/gpu/drm/i915/intel_tv.c
+++ b/drivers/gpu/drm/i915/intel_tv.c
@@ -854,6 +854,10 @@ intel_enable_tv(struct intel_encoder *encoder)
 	struct drm_device *dev = encoder->base.dev;
 	struct drm_i915_private *dev_priv = dev->dev_private;
 
+	/* Prevents vblank waits from timing out in intel_tv_detect_type() */
+	intel_wait_for_vblank(encoder->base.dev,
+			      to_intel_crtc(encoder->base.crtc)->pipe);
+
 	I915_WRITE(TV_CTL, I915_READ(TV_CTL) | TV_ENC_ENABLE);
 }
 
@@ -1311,6 +1315,7 @@ intel_tv_detect(struct drm_connector *connector, bool force)
 {
 	struct drm_display_mode mode;
 	struct intel_tv *intel_tv = intel_attached_tv(connector);
+	enum drm_connector_status status;
 	int type;
 
 	DRM_DEBUG_KMS("[CONNECTOR:%d:%s] force=%d\n",
@@ -1323,16 +1328,24 @@ intel_tv_detect(struct drm_connector *connector, bool force)
 		struct intel_load_detect_pipe tmp;
 		struct drm_modeset_acquire_ctx ctx;
 
+		drm_modeset_acquire_init(&ctx, 0);
+
 		if (intel_get_load_detect_pipe(connector, &mode, &tmp, &ctx)) {
 			type = intel_tv_detect_type(intel_tv, connector);
-			intel_release_load_detect_pipe(connector, &tmp, &ctx);
+			intel_release_load_detect_pipe(connector, &tmp);
+			status = type < 0 ?
+				connector_status_disconnected :
+				connector_status_connected;
 		} else
-			return connector_status_unknown;
+			status = connector_status_unknown;
+
+		drm_modeset_drop_locks(&ctx);
+		drm_modeset_acquire_fini(&ctx);
 	} else
 		return connector->status;
 
-	if (type < 0)
-		return connector_status_disconnected;
+	if (status != connector_status_connected)
+		return status;
 
 	intel_tv->type = type;
 	intel_tv_find_better_format(connector);
diff --git a/drivers/gpu/drm/i915/intel_uncore.c b/drivers/gpu/drm/i915/intel_uncore.c
index e81bc3bdc533..918b76163965 100644
--- a/drivers/gpu/drm/i915/intel_uncore.c
+++ b/drivers/gpu/drm/i915/intel_uncore.c
@@ -101,7 +101,7 @@ static void __gen7_gt_force_wake_mt_get(struct drm_i915_private *dev_priv,
 {
 	u32 forcewake_ack;
 
-	if (IS_HASWELL(dev_priv->dev) || IS_GEN8(dev_priv->dev))
+	if (IS_HASWELL(dev_priv->dev) || IS_BROADWELL(dev_priv->dev))
 		forcewake_ack = FORCEWAKE_ACK_HSW;
 	else
 		forcewake_ack = FORCEWAKE_MT_ACK;
@@ -334,7 +334,7 @@ void intel_uncore_forcewake_reset(struct drm_device *dev, bool restore)
 	else if (IS_GEN6(dev) || IS_GEN7(dev))
 		__gen6_gt_force_wake_reset(dev_priv);
 
-	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev) || IS_GEN8(dev))
+	if (IS_IVYBRIDGE(dev) || IS_HASWELL(dev) || IS_BROADWELL(dev))
 		__gen7_gt_force_wake_mt_reset(dev_priv);
 
 	if (restore) { /* If reset with a user forcewake, try to restore */
@@ -838,7 +838,7 @@ void intel_uncore_init(struct drm_device *dev)
 	if (IS_VALLEYVIEW(dev)) {
 		dev_priv->uncore.funcs.force_wake_get = __vlv_force_wake_get;
 		dev_priv->uncore.funcs.force_wake_put = __vlv_force_wake_put;
-	} else if (IS_HASWELL(dev) || IS_GEN8(dev)) {
+	} else if (IS_HASWELL(dev) || IS_BROADWELL(dev)) {
 		dev_priv->uncore.funcs.force_wake_get = __gen7_gt_force_wake_mt_get;
 		dev_priv->uncore.funcs.force_wake_put = __gen7_gt_force_wake_mt_put;
 	} else if (IS_IVYBRIDGE(dev)) {
diff --git a/drivers/gpu/drm/mga/mga_dma.c b/drivers/gpu/drm/mga/mga_dma.c
index c3bf059ba720..8cfa9cb74c86 100644
--- a/drivers/gpu/drm/mga/mga_dma.c
+++ b/drivers/gpu/drm/mga/mga_dma.c
@@ -502,31 +502,31 @@ static int mga_do_agp_dma_bootstrap(struct drm_device *dev,
 		return err;
 	}
 
-	/* Make drm_addbufs happy by not trying to create a mapping for less
-	 * than a page.
+	/* Make drm_legacy_addbufs happy by not trying to create a mapping for
+	 * less than a page.
 	 */
 	if (warp_size < PAGE_SIZE)
 		warp_size = PAGE_SIZE;
 
 	offset = 0;
-	err = drm_addmap(dev, offset, warp_size,
-			 _DRM_AGP, _DRM_READ_ONLY, &dev_priv->warp);
+	err = drm_legacy_addmap(dev, offset, warp_size,
+				_DRM_AGP, _DRM_READ_ONLY, &dev_priv->warp);
 	if (err) {
 		DRM_ERROR("Unable to map WARP microcode: %d\n", err);
 		return err;
 	}
 
 	offset += warp_size;
-	err = drm_addmap(dev, offset, dma_bs->primary_size,
-			 _DRM_AGP, _DRM_READ_ONLY, &dev_priv->primary);
+	err = drm_legacy_addmap(dev, offset, dma_bs->primary_size,
+				_DRM_AGP, _DRM_READ_ONLY, &dev_priv->primary);
 	if (err) {
 		DRM_ERROR("Unable to map primary DMA region: %d\n", err);
 		return err;
 	}
 
 	offset += dma_bs->primary_size;
-	err = drm_addmap(dev, offset, secondary_size,
-			 _DRM_AGP, 0, &dev->agp_buffer_map);
+	err = drm_legacy_addmap(dev, offset, secondary_size,
+				_DRM_AGP, 0, &dev->agp_buffer_map);
 	if (err) {
 		DRM_ERROR("Unable to map secondary DMA region: %d\n", err);
 		return err;
@@ -538,7 +538,7 @@ static int mga_do_agp_dma_bootstrap(struct drm_device *dev,
 	req.flags = _DRM_AGP_BUFFER;
 	req.agp_start = offset;
 
-	err = drm_addbufs_agp(dev, &req);
+	err = drm_legacy_addbufs_agp(dev, &req);
 	if (err) {
 		DRM_ERROR("Unable to add secondary DMA buffers: %d\n", err);
 		return err;
@@ -559,16 +559,16 @@ static int mga_do_agp_dma_bootstrap(struct drm_device *dev,
 	}
 
 	offset += secondary_size;
-	err = drm_addmap(dev, offset, agp_size - offset,
-			 _DRM_AGP, 0, &dev_priv->agp_textures);
+	err = drm_legacy_addmap(dev, offset, agp_size - offset,
+				_DRM_AGP, 0, &dev_priv->agp_textures);
 	if (err) {
 		DRM_ERROR("Unable to map AGP texture region %d\n", err);
 		return err;
 	}
 
-	drm_core_ioremap(dev_priv->warp, dev);
-	drm_core_ioremap(dev_priv->primary, dev);
-	drm_core_ioremap(dev->agp_buffer_map, dev);
+	drm_legacy_ioremap(dev_priv->warp, dev);
+	drm_legacy_ioremap(dev_priv->primary, dev);
+	drm_legacy_ioremap(dev->agp_buffer_map, dev);
 
 	if (!dev_priv->warp->handle ||
 	    !dev_priv->primary->handle || !dev->agp_buffer_map->handle) {
@@ -602,7 +602,7 @@ static int mga_do_agp_dma_bootstrap(struct drm_device *dev,
  *
  * \todo
  * Determine whether the maximum address passed to drm_pci_alloc is correct.
- * The same goes for drm_addbufs_pci.
+ * The same goes for drm_legacy_addbufs_pci.
  *
  * \sa mga_do_dma_bootstrap, mga_do_agp_dma_bootstrap
  */
@@ -622,15 +622,15 @@ static int mga_do_pci_dma_bootstrap(struct drm_device *dev,
 		return -EFAULT;
 	}
 
-	/* Make drm_addbufs happy by not trying to create a mapping for less
-	 * than a page.
+	/* Make drm_legacy_addbufs happy by not trying to create a mapping for
+	 * less than a page.
 	 */
 	if (warp_size < PAGE_SIZE)
 		warp_size = PAGE_SIZE;
 
 	/* The proper alignment is 0x100 for this mapping */
-	err = drm_addmap(dev, 0, warp_size, _DRM_CONSISTENT,
-			 _DRM_READ_ONLY, &dev_priv->warp);
+	err = drm_legacy_addmap(dev, 0, warp_size, _DRM_CONSISTENT,
+				_DRM_READ_ONLY, &dev_priv->warp);
 	if (err != 0) {
 		DRM_ERROR("Unable to create mapping for WARP microcode: %d\n",
 			  err);
@@ -645,8 +645,8 @@ static int mga_do_pci_dma_bootstrap(struct drm_device *dev,
 	for (primary_size = dma_bs->primary_size; primary_size != 0;
 	     primary_size >>= 1) {
 		/* The proper alignment for this mapping is 0x04 */
-		err = drm_addmap(dev, 0, primary_size, _DRM_CONSISTENT,
-				 _DRM_READ_ONLY, &dev_priv->primary);
+		err = drm_legacy_addmap(dev, 0, primary_size, _DRM_CONSISTENT,
+					_DRM_READ_ONLY, &dev_priv->primary);
 		if (!err)
 			break;
 	}
@@ -669,7 +669,7 @@ static int mga_do_pci_dma_bootstrap(struct drm_device *dev,
 		req.count = bin_count;
 		req.size = dma_bs->secondary_bin_size;
 
-		err = drm_addbufs_pci(dev, &req);
+		err = drm_legacy_addbufs_pci(dev, &req);
 		if (!err)
 			break;
 	}
@@ -708,15 +708,16 @@ static int mga_do_dma_bootstrap(struct drm_device *dev,
 	/* The first steps are the same for both PCI and AGP based DMA.  Map
 	 * the cards MMIO registers and map a status page.
 	 */
-	err = drm_addmap(dev, dev_priv->mmio_base, dev_priv->mmio_size,
-			 _DRM_REGISTERS, _DRM_READ_ONLY, &dev_priv->mmio);
+	err = drm_legacy_addmap(dev, dev_priv->mmio_base, dev_priv->mmio_size,
+				_DRM_REGISTERS, _DRM_READ_ONLY,
+				&dev_priv->mmio);
 	if (err) {
 		DRM_ERROR("Unable to map MMIO region: %d\n", err);
 		return err;
 	}
 
-	err = drm_addmap(dev, 0, SAREA_MAX, _DRM_SHM,
-			 _DRM_READ_ONLY | _DRM_LOCKED | _DRM_KERNEL,
+	err = drm_legacy_addmap(dev, 0, SAREA_MAX, _DRM_SHM,
+				_DRM_READ_ONLY | _DRM_LOCKED | _DRM_KERNEL,
 			 &dev_priv->status);
 	if (err) {
 		DRM_ERROR("Unable to map status region: %d\n", err);
@@ -809,7 +810,7 @@ static int mga_do_init_dma(struct drm_device *dev, drm_mga_init_t *init)
 	dev_priv->texture_offset = init->texture_offset[0];
 	dev_priv->texture_size = init->texture_size[0];
 
-	dev_priv->sarea = drm_getsarea(dev);
+	dev_priv->sarea = drm_legacy_getsarea(dev);
 	if (!dev_priv->sarea) {
 		DRM_ERROR("failed to find sarea!\n");
 		return -EINVAL;
@@ -820,37 +821,37 @@ static int mga_do_init_dma(struct drm_device *dev, drm_mga_init_t *init)
 		dev_priv->dma_access = MGA_PAGPXFER;
 		dev_priv->wagp_enable = MGA_WAGP_ENABLE;
 
-		dev_priv->status = drm_core_findmap(dev, init->status_offset);
+		dev_priv->status = drm_legacy_findmap(dev, init->status_offset);
 		if (!dev_priv->status) {
 			DRM_ERROR("failed to find status page!\n");
 			return -EINVAL;
 		}
-		dev_priv->mmio = drm_core_findmap(dev, init->mmio_offset);
+		dev_priv->mmio = drm_legacy_findmap(dev, init->mmio_offset);
 		if (!dev_priv->mmio) {
 			DRM_ERROR("failed to find mmio region!\n");
 			return -EINVAL;
 		}
-		dev_priv->warp = drm_core_findmap(dev, init->warp_offset);
+		dev_priv->warp = drm_legacy_findmap(dev, init->warp_offset);
 		if (!dev_priv->warp) {
 			DRM_ERROR("failed to find warp microcode region!\n");
 			return -EINVAL;
 		}
-		dev_priv->primary = drm_core_findmap(dev, init->primary_offset);
+		dev_priv->primary = drm_legacy_findmap(dev, init->primary_offset);
 		if (!dev_priv->primary) {
 			DRM_ERROR("failed to find primary dma region!\n");
 			return -EINVAL;
 		}
 		dev->agp_buffer_token = init->buffers_offset;
 		dev->agp_buffer_map =
-		    drm_core_findmap(dev, init->buffers_offset);
+		    drm_legacy_findmap(dev, init->buffers_offset);
 		if (!dev->agp_buffer_map) {
 			DRM_ERROR("failed to find dma buffer region!\n");
 			return -EINVAL;
 		}
 
-		drm_core_ioremap(dev_priv->warp, dev);
-		drm_core_ioremap(dev_priv->primary, dev);
-		drm_core_ioremap(dev->agp_buffer_map, dev);
+		drm_legacy_ioremap(dev_priv->warp, dev);
+		drm_legacy_ioremap(dev_priv->primary, dev);
+		drm_legacy_ioremap(dev->agp_buffer_map, dev);
 	}
 
 	dev_priv->sarea_priv =
@@ -936,14 +937,14 @@ static int mga_do_cleanup_dma(struct drm_device *dev, int full_cleanup)
 
 		if ((dev_priv->warp != NULL)
 		    && (dev_priv->warp->type != _DRM_CONSISTENT))
-			drm_core_ioremapfree(dev_priv->warp, dev);
+			drm_legacy_ioremapfree(dev_priv->warp, dev);
 
 		if ((dev_priv->primary != NULL)
 		    && (dev_priv->primary->type != _DRM_CONSISTENT))
-			drm_core_ioremapfree(dev_priv->primary, dev);
+			drm_legacy_ioremapfree(dev_priv->primary, dev);
 
 		if (dev->agp_buffer_map != NULL)
-			drm_core_ioremapfree(dev->agp_buffer_map, dev);
+			drm_legacy_ioremapfree(dev->agp_buffer_map, dev);
 
 		if (dev_priv->used_new_dma_init) {
 #if __OS_HAS_AGP
diff --git a/drivers/gpu/drm/mga/mga_drv.c b/drivers/gpu/drm/mga/mga_drv.c
index 6b1a87c8aac5..5e2f131a6a72 100644
--- a/drivers/gpu/drm/mga/mga_drv.c
+++ b/drivers/gpu/drm/mga/mga_drv.c
@@ -48,7 +48,7 @@ static const struct file_operations mga_driver_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = mga_compat_ioctl,
@@ -64,6 +64,7 @@ static struct drm_driver driver = {
 	.load = mga_driver_load,
 	.unload = mga_driver_unload,
 	.lastclose = mga_driver_lastclose,
+	.set_busid = drm_pci_set_busid,
 	.dma_quiescent = mga_driver_dma_quiescent,
 	.device_is_agp = mga_driver_device_is_agp,
 	.get_vblank_counter = mga_get_vblank_counter,
diff --git a/drivers/gpu/drm/mga/mga_drv.h b/drivers/gpu/drm/mga/mga_drv.h
index fe453213600a..b4a2014917e5 100644
--- a/drivers/gpu/drm/mga/mga_drv.h
+++ b/drivers/gpu/drm/mga/mga_drv.h
@@ -31,6 +31,8 @@
 #ifndef __MGA_DRV_H__
 #define __MGA_DRV_H__
 
+#include <drm/drm_legacy.h>
+
 /* General customization:
  */
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.c b/drivers/gpu/drm/mgag200/mgag200_drv.c
index 2d75d6df0789..97745991544d 100644
--- a/drivers/gpu/drm/mgag200/mgag200_drv.c
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.c
@@ -91,6 +91,7 @@ static struct drm_driver driver = {
 	.driver_features = DRIVER_GEM | DRIVER_MODESET,
 	.load = mgag200_driver_load,
 	.unload = mgag200_driver_unload,
+	.set_busid = drm_pci_set_busid,
 	.fops = &mgag200_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
diff --git a/drivers/gpu/drm/mgag200/mgag200_drv.h b/drivers/gpu/drm/mgag200/mgag200_drv.h
index 80de23d9b9c9..e9eea1d4e7c3 100644
--- a/drivers/gpu/drm/mgag200/mgag200_drv.h
+++ b/drivers/gpu/drm/mgag200/mgag200_drv.h
@@ -22,6 +22,8 @@
 #include <drm/ttm/ttm_memory.h>
 #include <drm/ttm/ttm_module.h>
 
+#include <drm/drm_gem.h>
+
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
 
@@ -190,8 +192,6 @@ struct mga_device {
 	resource_size_t			rmmio_size;
 	void __iomem			*rmmio;
 
-	drm_local_map_t			*framebuffer;
-
 	struct mga_mc			mc;
 	struct mga_mode_info		mode_info;
 
@@ -224,7 +224,7 @@ struct mgag200_bo {
 	struct ttm_placement placement;
 	struct ttm_bo_kmap_obj kmap;
 	struct drm_gem_object gem;
-	u32 placements[3];
+	struct ttm_place placements[3];
 	int pin_count;
 };
 #define gem_to_mga_bo(gobj) container_of((gobj), struct mgag200_bo, gem)
diff --git a/drivers/gpu/drm/mgag200/mgag200_fb.c b/drivers/gpu/drm/mgag200/mgag200_fb.c
index 5451dc58eff1..4415af3666ab 100644
--- a/drivers/gpu/drm/mgag200/mgag200_fb.c
+++ b/drivers/gpu/drm/mgag200/mgag200_fb.c
@@ -158,7 +158,8 @@ static int mgag200fb_create_object(struct mga_fbdev *afbdev,
 static int mgag200fb_create(struct drm_fb_helper *helper,
 			   struct drm_fb_helper_surface_size *sizes)
 {
-	struct mga_fbdev *mfbdev = (struct mga_fbdev *)helper;
+	struct mga_fbdev *mfbdev =
+		container_of(helper, struct mga_fbdev, helper);
 	struct drm_device *dev = mfbdev->helper.dev;
 	struct drm_mode_fb_cmd2 mode_cmd;
 	struct mga_device *mdev = dev->dev_private;
diff --git a/drivers/gpu/drm/mgag200/mgag200_mode.c b/drivers/gpu/drm/mgag200/mgag200_mode.c
index 45f04dea0ac2..83485ab81ce8 100644
--- a/drivers/gpu/drm/mgag200/mgag200_mode.c
+++ b/drivers/gpu/drm/mgag200/mgag200_mode.c
@@ -1483,11 +1483,7 @@ static int mga_vga_mode_valid(struct drm_connector *connector,
 {
 	struct drm_device *dev = connector->dev;
 	struct mga_device *mdev = (struct mga_device*)dev->dev_private;
-	struct mga_fbdev *mfbdev = mdev->mfbdev;
-	struct drm_fb_helper *fb_helper = &mfbdev->helper;
-	struct drm_fb_helper_connector *fb_helper_conn = NULL;
 	int bpp = 32;
-	int i = 0;
 
 	if (IS_G200_SE(mdev)) {
 		if (mdev->unique_rev_id == 0x01) {
@@ -1537,21 +1533,14 @@ static int mga_vga_mode_valid(struct drm_connector *connector,
 	}
 
 	/* Validate the mode input by the user */
-	for (i = 0; i < fb_helper->connector_count; i++) {
-		if (fb_helper->connector_info[i]->connector == connector) {
-			/* Found the helper for this connector */
-			fb_helper_conn = fb_helper->connector_info[i];
-			if (fb_helper_conn->cmdline_mode.specified) {
-				if (fb_helper_conn->cmdline_mode.bpp_specified) {
-					bpp = fb_helper_conn->cmdline_mode.bpp;
-				}
-			}
-		}
+	if (connector->cmdline_mode.specified) {
+		if (connector->cmdline_mode.bpp_specified)
+			bpp = connector->cmdline_mode.bpp;
 	}
 
 	if ((mode->hdisplay * mode->vdisplay * (bpp/8)) > mdev->mc.vram_size) {
-		if (fb_helper_conn)
-			fb_helper_conn->cmdline_mode.specified = false;
+		if (connector->cmdline_mode.specified)
+			connector->cmdline_mode.specified = false;
 		return MODE_BAD;
 	}
 
diff --git a/drivers/gpu/drm/mgag200/mgag200_ttm.c b/drivers/gpu/drm/mgag200/mgag200_ttm.c
index 5a00e90696de..d16964ea0ed4 100644
--- a/drivers/gpu/drm/mgag200/mgag200_ttm.c
+++ b/drivers/gpu/drm/mgag200/mgag200_ttm.c
@@ -293,18 +293,22 @@ void mgag200_mm_fini(struct mga_device *mdev)
 void mgag200_ttm_placement(struct mgag200_bo *bo, int domain)
 {
 	u32 c = 0;
-	bo->placement.fpfn = 0;
-	bo->placement.lpfn = 0;
+	unsigned i;
+
 	bo->placement.placement = bo->placements;
 	bo->placement.busy_placement = bo->placements;
 	if (domain & TTM_PL_FLAG_VRAM)
-		bo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
+		bo->placements[c++].flags = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_VRAM;
 	if (domain & TTM_PL_FLAG_SYSTEM)
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
 	if (!c)
-		bo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		bo->placements[c++].flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
 	bo->placement.num_placement = c;
 	bo->placement.num_busy_placement = c;
+	for (i = 0; i < c; ++i) {
+		bo->placements[i].fpfn = 0;
+		bo->placements[i].lpfn = 0;
+	}
 }
 
 int mgag200_bo_create(struct drm_device *dev, int size, int align,
@@ -335,7 +339,7 @@ int mgag200_bo_create(struct drm_device *dev, int size, int align,
 	ret = ttm_bo_init(&mdev->ttm.bdev, &mgabo->bo, size,
 			  ttm_bo_type_device, &mgabo->placement,
 			  align >> PAGE_SHIFT, false, NULL, acc_size,
-			  NULL, mgag200_bo_ttm_destroy);
+			  NULL, NULL, mgag200_bo_ttm_destroy);
 	if (ret)
 		return ret;
 
@@ -361,7 +365,7 @@ int mgag200_bo_pin(struct mgag200_bo *bo, u32 pl_flag, u64 *gpu_addr)
 
 	mgag200_ttm_placement(bo, pl_flag);
 	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret)
 		return ret;
@@ -384,7 +388,7 @@ int mgag200_bo_unpin(struct mgag200_bo *bo)
 		return 0;
 
 	for (i = 0; i < bo->placement.num_placement ; i++)
-		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags &= ~TTM_PL_FLAG_NO_EVICT;
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret)
 		return ret;
@@ -408,7 +412,7 @@ int mgag200_bo_push_sysram(struct mgag200_bo *bo)
 
 	mgag200_ttm_placement(bo, TTM_PL_FLAG_SYSTEM);
 	for (i = 0; i < bo->placement.num_placement ; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 
 	ret = ttm_bo_validate(&bo->bo, &bo->placement, false, false);
 	if (ret) {
@@ -424,7 +428,7 @@ int mgag200_mmap(struct file *filp, struct vm_area_struct *vma)
 	struct mga_device *mdev;
 
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 
 	file_priv = filp->private_data;
 	mdev = file_priv->minor->dev->dev_private;
diff --git a/drivers/gpu/drm/msm/Kconfig b/drivers/gpu/drm/msm/Kconfig
index c99c50de3226..9d907c526c94 100644
--- a/drivers/gpu/drm/msm/Kconfig
+++ b/drivers/gpu/drm/msm/Kconfig
@@ -4,6 +4,7 @@ config DRM_MSM
 	depends on DRM
 	depends on ARCH_QCOM || (ARM && COMPILE_TEST)
 	select DRM_KMS_HELPER
+	select DRM_PANEL
 	select SHMEM
 	select TMPFS
 	default y
diff --git a/drivers/gpu/drm/msm/Makefile b/drivers/gpu/drm/msm/Makefile
index 93ca49c8df44..6283dcb96af5 100644
--- a/drivers/gpu/drm/msm/Makefile
+++ b/drivers/gpu/drm/msm/Makefile
@@ -4,6 +4,7 @@ ifeq (, $(findstring -W,$(EXTRA_CFLAGS)))
 endif
 
 msm-y := \
+	adreno/adreno_device.o \
 	adreno/adreno_gpu.o \
 	adreno/a3xx_gpu.o \
 	hdmi/hdmi.o \
@@ -18,6 +19,8 @@ msm-y := \
 	mdp/mdp_kms.o \
 	mdp/mdp4/mdp4_crtc.o \
 	mdp/mdp4/mdp4_dtv_encoder.o \
+	mdp/mdp4/mdp4_lcdc_encoder.o \
+	mdp/mdp4/mdp4_lvds_connector.o \
 	mdp/mdp4/mdp4_irq.o \
 	mdp/mdp4/mdp4_kms.o \
 	mdp/mdp4/mdp4_plane.o \
@@ -39,5 +42,6 @@ msm-y := \
 	msm_ringbuffer.o
 
 msm-$(CONFIG_DRM_MSM_FBDEV) += msm_fbdev.o
+msm-$(CONFIG_COMMON_CLK) += mdp/mdp4/mdp4_lvds_pll.o
 
 obj-$(CONFIG_DRM_MSM)	+= msm.o
diff --git a/drivers/gpu/drm/msm/adreno/a2xx.xml.h b/drivers/gpu/drm/msm/adreno/a2xx.xml.h
index a8a144b38eaa..a3104598c27f 100644
--- a/drivers/gpu/drm/msm/adreno/a2xx.xml.h
+++ b/drivers/gpu/drm/msm/adreno/a2xx.xml.h
@@ -12,9 +12,9 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14477 bytes, from 2014-05-16 11:51:57)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-06-25 12:57:16)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  26602 bytes, from 2014-06-25 12:57:16)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
diff --git a/drivers/gpu/drm/msm/adreno/a3xx.xml.h b/drivers/gpu/drm/msm/adreno/a3xx.xml.h
index 303e8a9e91a5..82d015279b47 100644
--- a/drivers/gpu/drm/msm/adreno/a3xx.xml.h
+++ b/drivers/gpu/drm/msm/adreno/a3xx.xml.h
@@ -12,9 +12,9 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14477 bytes, from 2014-05-16 11:51:57)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-06-25 12:57:16)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  26602 bytes, from 2014-06-25 12:57:16)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
@@ -654,7 +654,7 @@ static inline uint32_t A3XX_GRAS_SU_POINT_SIZE(float val)
 #define A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__SHIFT		0
 static inline uint32_t A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL(float val)
 {
-	return ((((uint32_t)(val * 40.0))) << A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__MASK;
+	return ((((uint32_t)(val * 28.0))) << A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL__MASK;
 }
 
 #define REG_A3XX_GRAS_SU_POLY_OFFSET_OFFSET			0x0000206d
@@ -662,7 +662,7 @@ static inline uint32_t A3XX_GRAS_SU_POLY_OFFSET_SCALE_VAL(float val)
 #define A3XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT			0
 static inline uint32_t A3XX_GRAS_SU_POLY_OFFSET_OFFSET(float val)
 {
-	return ((((uint32_t)(val * 44.0))) << A3XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_OFFSET__MASK;
+	return ((((uint32_t)(val * 28.0))) << A3XX_GRAS_SU_POLY_OFFSET_OFFSET__SHIFT) & A3XX_GRAS_SU_POLY_OFFSET_OFFSET__MASK;
 }
 
 #define REG_A3XX_GRAS_SU_MODE_CONTROL				0x00002070
@@ -1696,7 +1696,7 @@ static inline uint32_t A3XX_SP_VS_CTRL_REG1_CONSTFOOTPRINT(uint32_t val)
 {
 	return ((val) << A3XX_SP_VS_CTRL_REG1_CONSTFOOTPRINT__SHIFT) & A3XX_SP_VS_CTRL_REG1_CONSTFOOTPRINT__MASK;
 }
-#define A3XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING__MASK		0x3f000000
+#define A3XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING__MASK		0x7f000000
 #define A3XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING__SHIFT		24
 static inline uint32_t A3XX_SP_VS_CTRL_REG1_INITIALOUTSTANDING(uint32_t val)
 {
diff --git a/drivers/gpu/drm/msm/adreno/a3xx_gpu.c b/drivers/gpu/drm/msm/adreno/a3xx_gpu.c
index 2773600c9488..218c5b060398 100644
--- a/drivers/gpu/drm/msm/adreno/a3xx_gpu.c
+++ b/drivers/gpu/drm/msm/adreno/a3xx_gpu.c
@@ -35,10 +35,8 @@
 	 A3XX_INT0_CP_AHB_ERROR_HALT |     \
 	 A3XX_INT0_UCHE_OOB_ACCESS)
 
+extern bool hang_debug;
 
-static bool hang_debug = false;
-MODULE_PARM_DESC(hang_debug, "Dump registers when hang is detected (can be slow!)");
-module_param_named(hang_debug, hang_debug, bool, 0600);
 static void a3xx_dump(struct msm_gpu *gpu);
 
 static void a3xx_me_init(struct msm_gpu *gpu)
@@ -387,58 +385,26 @@ static const unsigned int a3xx_registers[] = {
 	0x2750, 0x2756, 0x2760, 0x2760, 0x300c, 0x300e, 0x301c, 0x301d,
 	0x302a, 0x302a, 0x302c, 0x302d, 0x3030, 0x3031, 0x3034, 0x3036,
 	0x303c, 0x303c, 0x305e, 0x305f,
+	~0   /* sentinel */
 };
 
 #ifdef CONFIG_DEBUG_FS
 static void a3xx_show(struct msm_gpu *gpu, struct seq_file *m)
 {
-	int i;
-
-	adreno_show(gpu, m);
-
 	gpu->funcs->pm_resume(gpu);
-
 	seq_printf(m, "status:   %08x\n",
 			gpu_read(gpu, REG_A3XX_RBBM_STATUS));
-
-	/* dump these out in a form that can be parsed by demsm: */
-	seq_printf(m, "IO:region %s 00000000 00020000\n", gpu->name);
-	for (i = 0; i < ARRAY_SIZE(a3xx_registers); i += 2) {
-		uint32_t start = a3xx_registers[i];
-		uint32_t end   = a3xx_registers[i+1];
-		uint32_t addr;
-
-		for (addr = start; addr <= end; addr++) {
-			uint32_t val = gpu_read(gpu, addr);
-			seq_printf(m, "IO:R %08x %08x\n", addr<<2, val);
-		}
-	}
-
 	gpu->funcs->pm_suspend(gpu);
+	adreno_show(gpu, m);
 }
 #endif
 
 /* would be nice to not have to duplicate the _show() stuff with printk(): */
 static void a3xx_dump(struct msm_gpu *gpu)
 {
-	int i;
-
-	adreno_dump(gpu);
 	printk("status:   %08x\n",
 			gpu_read(gpu, REG_A3XX_RBBM_STATUS));
-
-	/* dump these out in a form that can be parsed by demsm: */
-	printk("IO:region %s 00000000 00020000\n", gpu->name);
-	for (i = 0; i < ARRAY_SIZE(a3xx_registers); i += 2) {
-		uint32_t start = a3xx_registers[i];
-		uint32_t end   = a3xx_registers[i+1];
-		uint32_t addr;
-
-		for (addr = start; addr <= end; addr++) {
-			uint32_t val = gpu_read(gpu, addr);
-			printk("IO:R %08x %08x\n", addr<<2, val);
-		}
-	}
+	adreno_dump(gpu);
 }
 
 static const struct adreno_gpu_funcs funcs = {
@@ -474,7 +440,6 @@ struct msm_gpu *a3xx_gpu_init(struct drm_device *dev)
 	struct msm_gpu *gpu;
 	struct msm_drm_private *priv = dev->dev_private;
 	struct platform_device *pdev = priv->gpu_pdev;
-	struct adreno_platform_config *config;
 	int ret;
 
 	if (!pdev) {
@@ -483,8 +448,6 @@ struct msm_gpu *a3xx_gpu_init(struct drm_device *dev)
 		goto fail;
 	}
 
-	config = pdev->dev.platform_data;
-
 	a3xx_gpu = kzalloc(sizeof(*a3xx_gpu), GFP_KERNEL);
 	if (!a3xx_gpu) {
 		ret = -ENOMEM;
@@ -496,20 +459,12 @@ struct msm_gpu *a3xx_gpu_init(struct drm_device *dev)
 
 	a3xx_gpu->pdev = pdev;
 
-	gpu->fast_rate = config->fast_rate;
-	gpu->slow_rate = config->slow_rate;
-	gpu->bus_freq  = config->bus_freq;
-#ifdef CONFIG_MSM_BUS_SCALING
-	gpu->bus_scale_table = config->bus_scale_table;
-#endif
-
-	DBG("fast_rate=%u, slow_rate=%u, bus_freq=%u",
-			gpu->fast_rate, gpu->slow_rate, gpu->bus_freq);
-
 	gpu->perfcntrs = perfcntrs;
 	gpu->num_perfcntrs = ARRAY_SIZE(perfcntrs);
 
-	ret = adreno_gpu_init(dev, pdev, adreno_gpu, &funcs, config->rev);
+	adreno_gpu->registers = a3xx_registers;
+
+	ret = adreno_gpu_init(dev, pdev, adreno_gpu, &funcs);
 	if (ret)
 		goto fail;
 
@@ -549,158 +504,3 @@ fail:
 
 	return ERR_PTR(ret);
 }
-
-/*
- * The a3xx device:
- */
-
-#if defined(CONFIG_MSM_BUS_SCALING) && !defined(CONFIG_OF)
-#  include <mach/kgsl.h>
-#endif
-
-static void set_gpu_pdev(struct drm_device *dev,
-		struct platform_device *pdev)
-{
-	struct msm_drm_private *priv = dev->dev_private;
-	priv->gpu_pdev = pdev;
-}
-
-static int a3xx_bind(struct device *dev, struct device *master, void *data)
-{
-	static struct adreno_platform_config config = {};
-#ifdef CONFIG_OF
-	struct device_node *child, *node = dev->of_node;
-	u32 val;
-	int ret;
-
-	ret = of_property_read_u32(node, "qcom,chipid", &val);
-	if (ret) {
-		dev_err(dev, "could not find chipid: %d\n", ret);
-		return ret;
-	}
-
-	config.rev = ADRENO_REV((val >> 24) & 0xff,
-			(val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff);
-
-	/* find clock rates: */
-	config.fast_rate = 0;
-	config.slow_rate = ~0;
-	for_each_child_of_node(node, child) {
-		if (of_device_is_compatible(child, "qcom,gpu-pwrlevels")) {
-			struct device_node *pwrlvl;
-			for_each_child_of_node(child, pwrlvl) {
-				ret = of_property_read_u32(pwrlvl, "qcom,gpu-freq", &val);
-				if (ret) {
-					dev_err(dev, "could not find gpu-freq: %d\n", ret);
-					return ret;
-				}
-				config.fast_rate = max(config.fast_rate, val);
-				config.slow_rate = min(config.slow_rate, val);
-			}
-		}
-	}
-
-	if (!config.fast_rate) {
-		dev_err(dev, "could not find clk rates\n");
-		return -ENXIO;
-	}
-
-#else
-	struct kgsl_device_platform_data *pdata = dev->platform_data;
-	uint32_t version = socinfo_get_version();
-	if (cpu_is_apq8064ab()) {
-		config.fast_rate = 450000000;
-		config.slow_rate = 27000000;
-		config.bus_freq  = 4;
-		config.rev = ADRENO_REV(3, 2, 1, 0);
-	} else if (cpu_is_apq8064()) {
-		config.fast_rate = 400000000;
-		config.slow_rate = 27000000;
-		config.bus_freq  = 4;
-
-		if (SOCINFO_VERSION_MAJOR(version) == 2)
-			config.rev = ADRENO_REV(3, 2, 0, 2);
-		else if ((SOCINFO_VERSION_MAJOR(version) == 1) &&
-				(SOCINFO_VERSION_MINOR(version) == 1))
-			config.rev = ADRENO_REV(3, 2, 0, 1);
-		else
-			config.rev = ADRENO_REV(3, 2, 0, 0);
-
-	} else if (cpu_is_msm8960ab()) {
-		config.fast_rate = 400000000;
-		config.slow_rate = 320000000;
-		config.bus_freq  = 4;
-
-		if (SOCINFO_VERSION_MINOR(version) == 0)
-			config.rev = ADRENO_REV(3, 2, 1, 0);
-		else
-			config.rev = ADRENO_REV(3, 2, 1, 1);
-
-	} else if (cpu_is_msm8930()) {
-		config.fast_rate = 400000000;
-		config.slow_rate = 27000000;
-		config.bus_freq  = 3;
-
-		if ((SOCINFO_VERSION_MAJOR(version) == 1) &&
-			(SOCINFO_VERSION_MINOR(version) == 2))
-			config.rev = ADRENO_REV(3, 0, 5, 2);
-		else
-			config.rev = ADRENO_REV(3, 0, 5, 0);
-
-	}
-#  ifdef CONFIG_MSM_BUS_SCALING
-	config.bus_scale_table = pdata->bus_scale_table;
-#  endif
-#endif
-	dev->platform_data = &config;
-	set_gpu_pdev(dev_get_drvdata(master), to_platform_device(dev));
-	return 0;
-}
-
-static void a3xx_unbind(struct device *dev, struct device *master,
-		void *data)
-{
-	set_gpu_pdev(dev_get_drvdata(master), NULL);
-}
-
-static const struct component_ops a3xx_ops = {
-		.bind   = a3xx_bind,
-		.unbind = a3xx_unbind,
-};
-
-static int a3xx_probe(struct platform_device *pdev)
-{
-	return component_add(&pdev->dev, &a3xx_ops);
-}
-
-static int a3xx_remove(struct platform_device *pdev)
-{
-	component_del(&pdev->dev, &a3xx_ops);
-	return 0;
-}
-
-static const struct of_device_id dt_match[] = {
-	{ .compatible = "qcom,adreno-3xx" },
-	/* for backwards compat w/ downstream kgsl DT files: */
-	{ .compatible = "qcom,kgsl-3d0" },
-	{}
-};
-
-static struct platform_driver a3xx_driver = {
-	.probe = a3xx_probe,
-	.remove = a3xx_remove,
-	.driver = {
-		.name = "kgsl-3d0",
-		.of_match_table = dt_match,
-	},
-};
-
-void __init a3xx_register(void)
-{
-	platform_driver_register(&a3xx_driver);
-}
-
-void __exit a3xx_unregister(void)
-{
-	platform_driver_unregister(&a3xx_driver);
-}
diff --git a/drivers/gpu/drm/msm/adreno/adreno_common.xml.h b/drivers/gpu/drm/msm/adreno/adreno_common.xml.h
index 9de19ac2e86c..cc341bc62b51 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_common.xml.h
+++ b/drivers/gpu/drm/msm/adreno/adreno_common.xml.h
@@ -12,9 +12,9 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14477 bytes, from 2014-05-16 11:51:57)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-06-25 12:57:16)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  26602 bytes, from 2014-06-25 12:57:16)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
diff --git a/drivers/gpu/drm/msm/adreno/adreno_device.c b/drivers/gpu/drm/msm/adreno/adreno_device.c
new file mode 100644
index 000000000000..7ab85af3a7db
--- /dev/null
+++ b/drivers/gpu/drm/msm/adreno/adreno_device.c
@@ -0,0 +1,285 @@
+/*
+ * Copyright (C) 2013-2014 Red Hat
+ * Author: Rob Clark <robdclark@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "adreno_gpu.h"
+
+#if defined(CONFIG_MSM_BUS_SCALING) && !defined(CONFIG_OF)
+#  include <mach/kgsl.h>
+#endif
+
+#define ANY_ID 0xff
+
+bool hang_debug = false;
+MODULE_PARM_DESC(hang_debug, "Dump registers when hang is detected (can be slow!)");
+module_param_named(hang_debug, hang_debug, bool, 0600);
+
+struct msm_gpu *a3xx_gpu_init(struct drm_device *dev);
+
+static const struct adreno_info gpulist[] = {
+	{
+		.rev   = ADRENO_REV(3, 0, 5, ANY_ID),
+		.revn  = 305,
+		.name  = "A305",
+		.pm4fw = "a300_pm4.fw",
+		.pfpfw = "a300_pfp.fw",
+		.gmem  = SZ_256K,
+		.init  = a3xx_gpu_init,
+	}, {
+		.rev   = ADRENO_REV(3, 2, ANY_ID, ANY_ID),
+		.revn  = 320,
+		.name  = "A320",
+		.pm4fw = "a300_pm4.fw",
+		.pfpfw = "a300_pfp.fw",
+		.gmem  = SZ_512K,
+		.init  = a3xx_gpu_init,
+	}, {
+		.rev   = ADRENO_REV(3, 3, 0, ANY_ID),
+		.revn  = 330,
+		.name  = "A330",
+		.pm4fw = "a330_pm4.fw",
+		.pfpfw = "a330_pfp.fw",
+		.gmem  = SZ_1M,
+		.init  = a3xx_gpu_init,
+	},
+};
+
+MODULE_FIRMWARE("a300_pm4.fw");
+MODULE_FIRMWARE("a300_pfp.fw");
+MODULE_FIRMWARE("a330_pm4.fw");
+MODULE_FIRMWARE("a330_pfp.fw");
+
+static inline bool _rev_match(uint8_t entry, uint8_t id)
+{
+	return (entry == ANY_ID) || (entry == id);
+}
+
+const struct adreno_info *adreno_info(struct adreno_rev rev)
+{
+	int i;
+
+	/* identify gpu: */
+	for (i = 0; i < ARRAY_SIZE(gpulist); i++) {
+		const struct adreno_info *info = &gpulist[i];
+		if (_rev_match(info->rev.core, rev.core) &&
+				_rev_match(info->rev.major, rev.major) &&
+				_rev_match(info->rev.minor, rev.minor) &&
+				_rev_match(info->rev.patchid, rev.patchid))
+			return info;
+	}
+
+	return NULL;
+}
+
+struct msm_gpu *adreno_load_gpu(struct drm_device *dev)
+{
+	struct msm_drm_private *priv = dev->dev_private;
+	struct platform_device *pdev = priv->gpu_pdev;
+	struct adreno_platform_config *config;
+	struct adreno_rev rev;
+	const struct adreno_info *info;
+	struct msm_gpu *gpu = NULL;
+
+	if (!pdev) {
+		dev_err(dev->dev, "no adreno device\n");
+		return NULL;
+	}
+
+	config = pdev->dev.platform_data;
+	rev = config->rev;
+	info = adreno_info(config->rev);
+
+	if (!info) {
+		dev_warn(dev->dev, "Unknown GPU revision: %u.%u.%u.%u\n",
+				rev.core, rev.major, rev.minor, rev.patchid);
+		return NULL;
+	}
+
+	DBG("Found GPU: %u.%u.%u.%u",  rev.core, rev.major,
+			rev.minor, rev.patchid);
+
+	gpu = info->init(dev);
+	if (IS_ERR(gpu)) {
+		dev_warn(dev->dev, "failed to load adreno gpu\n");
+		gpu = NULL;
+		/* not fatal */
+	}
+
+	if (gpu) {
+		int ret;
+		mutex_lock(&dev->struct_mutex);
+		gpu->funcs->pm_resume(gpu);
+		mutex_unlock(&dev->struct_mutex);
+		ret = gpu->funcs->hw_init(gpu);
+		if (ret) {
+			dev_err(dev->dev, "gpu hw init failed: %d\n", ret);
+			gpu->funcs->destroy(gpu);
+			gpu = NULL;
+		} else {
+			/* give inactive pm a chance to kick in: */
+			msm_gpu_retire(gpu);
+		}
+	}
+
+	return gpu;
+}
+
+static void set_gpu_pdev(struct drm_device *dev,
+		struct platform_device *pdev)
+{
+	struct msm_drm_private *priv = dev->dev_private;
+	priv->gpu_pdev = pdev;
+}
+
+static int adreno_bind(struct device *dev, struct device *master, void *data)
+{
+	static struct adreno_platform_config config = {};
+#ifdef CONFIG_OF
+	struct device_node *child, *node = dev->of_node;
+	u32 val;
+	int ret;
+
+	ret = of_property_read_u32(node, "qcom,chipid", &val);
+	if (ret) {
+		dev_err(dev, "could not find chipid: %d\n", ret);
+		return ret;
+	}
+
+	config.rev = ADRENO_REV((val >> 24) & 0xff,
+			(val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff);
+
+	/* find clock rates: */
+	config.fast_rate = 0;
+	config.slow_rate = ~0;
+	for_each_child_of_node(node, child) {
+		if (of_device_is_compatible(child, "qcom,gpu-pwrlevels")) {
+			struct device_node *pwrlvl;
+			for_each_child_of_node(child, pwrlvl) {
+				ret = of_property_read_u32(pwrlvl, "qcom,gpu-freq", &val);
+				if (ret) {
+					dev_err(dev, "could not find gpu-freq: %d\n", ret);
+					return ret;
+				}
+				config.fast_rate = max(config.fast_rate, val);
+				config.slow_rate = min(config.slow_rate, val);
+			}
+		}
+	}
+
+	if (!config.fast_rate) {
+		dev_err(dev, "could not find clk rates\n");
+		return -ENXIO;
+	}
+
+#else
+	struct kgsl_device_platform_data *pdata = dev->platform_data;
+	uint32_t version = socinfo_get_version();
+	if (cpu_is_apq8064ab()) {
+		config.fast_rate = 450000000;
+		config.slow_rate = 27000000;
+		config.bus_freq  = 4;
+		config.rev = ADRENO_REV(3, 2, 1, 0);
+	} else if (cpu_is_apq8064()) {
+		config.fast_rate = 400000000;
+		config.slow_rate = 27000000;
+		config.bus_freq  = 4;
+
+		if (SOCINFO_VERSION_MAJOR(version) == 2)
+			config.rev = ADRENO_REV(3, 2, 0, 2);
+		else if ((SOCINFO_VERSION_MAJOR(version) == 1) &&
+				(SOCINFO_VERSION_MINOR(version) == 1))
+			config.rev = ADRENO_REV(3, 2, 0, 1);
+		else
+			config.rev = ADRENO_REV(3, 2, 0, 0);
+
+	} else if (cpu_is_msm8960ab()) {
+		config.fast_rate = 400000000;
+		config.slow_rate = 320000000;
+		config.bus_freq  = 4;
+
+		if (SOCINFO_VERSION_MINOR(version) == 0)
+			config.rev = ADRENO_REV(3, 2, 1, 0);
+		else
+			config.rev = ADRENO_REV(3, 2, 1, 1);
+
+	} else if (cpu_is_msm8930()) {
+		config.fast_rate = 400000000;
+		config.slow_rate = 27000000;
+		config.bus_freq  = 3;
+
+		if ((SOCINFO_VERSION_MAJOR(version) == 1) &&
+			(SOCINFO_VERSION_MINOR(version) == 2))
+			config.rev = ADRENO_REV(3, 0, 5, 2);
+		else
+			config.rev = ADRENO_REV(3, 0, 5, 0);
+
+	}
+#  ifdef CONFIG_MSM_BUS_SCALING
+	config.bus_scale_table = pdata->bus_scale_table;
+#  endif
+#endif
+	dev->platform_data = &config;
+	set_gpu_pdev(dev_get_drvdata(master), to_platform_device(dev));
+	return 0;
+}
+
+static void adreno_unbind(struct device *dev, struct device *master,
+		void *data)
+{
+	set_gpu_pdev(dev_get_drvdata(master), NULL);
+}
+
+static const struct component_ops a3xx_ops = {
+		.bind   = adreno_bind,
+		.unbind = adreno_unbind,
+};
+
+static int adreno_probe(struct platform_device *pdev)
+{
+	return component_add(&pdev->dev, &a3xx_ops);
+}
+
+static int adreno_remove(struct platform_device *pdev)
+{
+	component_del(&pdev->dev, &a3xx_ops);
+	return 0;
+}
+
+static const struct of_device_id dt_match[] = {
+	{ .compatible = "qcom,adreno-3xx" },
+	/* for backwards compat w/ downstream kgsl DT files: */
+	{ .compatible = "qcom,kgsl-3d0" },
+	{}
+};
+
+static struct platform_driver adreno_driver = {
+	.probe = adreno_probe,
+	.remove = adreno_remove,
+	.driver = {
+		.name = "adreno",
+		.of_match_table = dt_match,
+	},
+};
+
+void __init adreno_register(void)
+{
+	platform_driver_register(&adreno_driver);
+}
+
+void __exit adreno_unregister(void)
+{
+	platform_driver_unregister(&adreno_driver);
+}
diff --git a/drivers/gpu/drm/msm/adreno/adreno_gpu.c b/drivers/gpu/drm/msm/adreno/adreno_gpu.c
index 655ce5b14ad0..6afa29167fee 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_gpu.c
+++ b/drivers/gpu/drm/msm/adreno/adreno_gpu.c
@@ -19,46 +19,6 @@
 #include "msm_gem.h"
 #include "msm_mmu.h"
 
-struct adreno_info {
-	struct adreno_rev rev;
-	uint32_t revn;
-	const char *name;
-	const char *pm4fw, *pfpfw;
-	uint32_t gmem;
-};
-
-#define ANY_ID 0xff
-
-static const struct adreno_info gpulist[] = {
-	{
-		.rev   = ADRENO_REV(3, 0, 5, ANY_ID),
-		.revn  = 305,
-		.name  = "A305",
-		.pm4fw = "a300_pm4.fw",
-		.pfpfw = "a300_pfp.fw",
-		.gmem  = SZ_256K,
-	}, {
-		.rev   = ADRENO_REV(3, 2, ANY_ID, ANY_ID),
-		.revn  = 320,
-		.name  = "A320",
-		.pm4fw = "a300_pm4.fw",
-		.pfpfw = "a300_pfp.fw",
-		.gmem  = SZ_512K,
-	}, {
-		.rev   = ADRENO_REV(3, 3, 0, ANY_ID),
-		.revn  = 330,
-		.name  = "A330",
-		.pm4fw = "a330_pm4.fw",
-		.pfpfw = "a330_pfp.fw",
-		.gmem  = SZ_1M,
-	},
-};
-
-MODULE_FIRMWARE("a300_pm4.fw");
-MODULE_FIRMWARE("a300_pfp.fw");
-MODULE_FIRMWARE("a330_pm4.fw");
-MODULE_FIRMWARE("a330_pfp.fw");
-
 #define RB_SIZE    SZ_32K
 #define RB_BLKSIZE 16
 
@@ -252,6 +212,7 @@ void adreno_idle(struct msm_gpu *gpu)
 void adreno_show(struct msm_gpu *gpu, struct seq_file *m)
 {
 	struct adreno_gpu *adreno_gpu = to_adreno_gpu(gpu);
+	int i;
 
 	seq_printf(m, "revision: %d (%d.%d.%d.%d)\n",
 			adreno_gpu->info->revn, adreno_gpu->rev.core,
@@ -263,6 +224,23 @@ void adreno_show(struct msm_gpu *gpu, struct seq_file *m)
 	seq_printf(m, "rptr:     %d\n", adreno_gpu->memptrs->rptr);
 	seq_printf(m, "wptr:     %d\n", adreno_gpu->memptrs->wptr);
 	seq_printf(m, "rb wptr:  %d\n", get_wptr(gpu->rb));
+
+	gpu->funcs->pm_resume(gpu);
+
+	/* dump these out in a form that can be parsed by demsm: */
+	seq_printf(m, "IO:region %s 00000000 00020000\n", gpu->name);
+	for (i = 0; adreno_gpu->registers[i] != ~0; i += 2) {
+		uint32_t start = adreno_gpu->registers[i];
+		uint32_t end   = adreno_gpu->registers[i+1];
+		uint32_t addr;
+
+		for (addr = start; addr <= end; addr++) {
+			uint32_t val = gpu_read(gpu, addr);
+			seq_printf(m, "IO:R %08x %08x\n", addr<<2, val);
+		}
+	}
+
+	gpu->funcs->pm_suspend(gpu);
 }
 #endif
 
@@ -270,6 +248,7 @@ void adreno_show(struct msm_gpu *gpu, struct seq_file *m)
 void adreno_dump(struct msm_gpu *gpu)
 {
 	struct adreno_gpu *adreno_gpu = to_adreno_gpu(gpu);
+	int i;
 
 	printk("revision: %d (%d.%d.%d.%d)\n",
 			adreno_gpu->info->revn, adreno_gpu->rev.core,
@@ -282,6 +261,18 @@ void adreno_dump(struct msm_gpu *gpu)
 	printk("wptr:     %d\n", adreno_gpu->memptrs->wptr);
 	printk("rb wptr:  %d\n", get_wptr(gpu->rb));
 
+	/* dump these out in a form that can be parsed by demsm: */
+	printk("IO:region %s 00000000 00020000\n", gpu->name);
+	for (i = 0; adreno_gpu->registers[i] != ~0; i += 2) {
+		uint32_t start = adreno_gpu->registers[i];
+		uint32_t end   = adreno_gpu->registers[i+1];
+		uint32_t addr;
+
+		for (addr = start; addr <= end; addr++) {
+			uint32_t val = gpu_read(gpu, addr);
+			printk("IO:R %08x %08x\n", addr<<2, val);
+		}
+	}
 }
 
 static uint32_t ring_freewords(struct msm_gpu *gpu)
@@ -304,65 +295,51 @@ static const char *iommu_ports[] = {
 		"gfx3d1_user", "gfx3d1_priv",
 };
 
-static inline bool _rev_match(uint8_t entry, uint8_t id)
-{
-	return (entry == ANY_ID) || (entry == id);
-}
-
 int adreno_gpu_init(struct drm_device *drm, struct platform_device *pdev,
-		struct adreno_gpu *gpu, const struct adreno_gpu_funcs *funcs,
-		struct adreno_rev rev)
+		struct adreno_gpu *adreno_gpu, const struct adreno_gpu_funcs *funcs)
 {
+	struct adreno_platform_config *config = pdev->dev.platform_data;
+	struct msm_gpu *gpu = &adreno_gpu->base;
 	struct msm_mmu *mmu;
-	int i, ret;
-
-	/* identify gpu: */
-	for (i = 0; i < ARRAY_SIZE(gpulist); i++) {
-		const struct adreno_info *info = &gpulist[i];
-		if (_rev_match(info->rev.core, rev.core) &&
-				_rev_match(info->rev.major, rev.major) &&
-				_rev_match(info->rev.minor, rev.minor) &&
-				_rev_match(info->rev.patchid, rev.patchid)) {
-			gpu->info = info;
-			gpu->revn = info->revn;
-			break;
-		}
-	}
-
-	if (i == ARRAY_SIZE(gpulist)) {
-		dev_err(drm->dev, "Unknown GPU revision: %u.%u.%u.%u\n",
-				rev.core, rev.major, rev.minor, rev.patchid);
-		return -ENXIO;
-	}
+	int ret;
 
-	DBG("Found GPU: %s (%u.%u.%u.%u)", gpu->info->name,
-			rev.core, rev.major, rev.minor, rev.patchid);
+	adreno_gpu->funcs = funcs;
+	adreno_gpu->info = adreno_info(config->rev);
+	adreno_gpu->gmem = adreno_gpu->info->gmem;
+	adreno_gpu->revn = adreno_gpu->info->revn;
+	adreno_gpu->rev = config->rev;
+
+	gpu->fast_rate = config->fast_rate;
+	gpu->slow_rate = config->slow_rate;
+	gpu->bus_freq  = config->bus_freq;
+#ifdef CONFIG_MSM_BUS_SCALING
+	gpu->bus_scale_table = config->bus_scale_table;
+#endif
 
-	gpu->funcs = funcs;
-	gpu->gmem = gpu->info->gmem;
-	gpu->rev = rev;
+	DBG("fast_rate=%u, slow_rate=%u, bus_freq=%u",
+			gpu->fast_rate, gpu->slow_rate, gpu->bus_freq);
 
-	ret = request_firmware(&gpu->pm4, gpu->info->pm4fw, drm->dev);
+	ret = request_firmware(&adreno_gpu->pm4, adreno_gpu->info->pm4fw, drm->dev);
 	if (ret) {
 		dev_err(drm->dev, "failed to load %s PM4 firmware: %d\n",
-				gpu->info->pm4fw, ret);
+				adreno_gpu->info->pm4fw, ret);
 		return ret;
 	}
 
-	ret = request_firmware(&gpu->pfp, gpu->info->pfpfw, drm->dev);
+	ret = request_firmware(&adreno_gpu->pfp, adreno_gpu->info->pfpfw, drm->dev);
 	if (ret) {
 		dev_err(drm->dev, "failed to load %s PFP firmware: %d\n",
-				gpu->info->pfpfw, ret);
+				adreno_gpu->info->pfpfw, ret);
 		return ret;
 	}
 
-	ret = msm_gpu_init(drm, pdev, &gpu->base, &funcs->base,
-			gpu->info->name, "kgsl_3d0_reg_memory", "kgsl_3d0_irq",
+	ret = msm_gpu_init(drm, pdev, &adreno_gpu->base, &funcs->base,
+			adreno_gpu->info->name, "kgsl_3d0_reg_memory", "kgsl_3d0_irq",
 			RB_SIZE);
 	if (ret)
 		return ret;
 
-	mmu = gpu->base.mmu;
+	mmu = gpu->mmu;
 	if (mmu) {
 		ret = mmu->funcs->attach(mmu, iommu_ports,
 				ARRAY_SIZE(iommu_ports));
@@ -371,24 +348,24 @@ int adreno_gpu_init(struct drm_device *drm, struct platform_device *pdev,
 	}
 
 	mutex_lock(&drm->struct_mutex);
-	gpu->memptrs_bo = msm_gem_new(drm, sizeof(*gpu->memptrs),
+	adreno_gpu->memptrs_bo = msm_gem_new(drm, sizeof(*adreno_gpu->memptrs),
 			MSM_BO_UNCACHED);
 	mutex_unlock(&drm->struct_mutex);
-	if (IS_ERR(gpu->memptrs_bo)) {
-		ret = PTR_ERR(gpu->memptrs_bo);
-		gpu->memptrs_bo = NULL;
+	if (IS_ERR(adreno_gpu->memptrs_bo)) {
+		ret = PTR_ERR(adreno_gpu->memptrs_bo);
+		adreno_gpu->memptrs_bo = NULL;
 		dev_err(drm->dev, "could not allocate memptrs: %d\n", ret);
 		return ret;
 	}
 
-	gpu->memptrs = msm_gem_vaddr(gpu->memptrs_bo);
-	if (!gpu->memptrs) {
+	adreno_gpu->memptrs = msm_gem_vaddr(adreno_gpu->memptrs_bo);
+	if (!adreno_gpu->memptrs) {
 		dev_err(drm->dev, "could not vmap memptrs\n");
 		return -ENOMEM;
 	}
 
-	ret = msm_gem_get_iova(gpu->memptrs_bo, gpu->base.id,
-			&gpu->memptrs_iova);
+	ret = msm_gem_get_iova(adreno_gpu->memptrs_bo, gpu->id,
+			&adreno_gpu->memptrs_iova);
 	if (ret) {
 		dev_err(drm->dev, "could not map memptrs: %d\n", ret);
 		return ret;
diff --git a/drivers/gpu/drm/msm/adreno/adreno_gpu.h b/drivers/gpu/drm/msm/adreno/adreno_gpu.h
index 63c36ce33020..52f051579753 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_gpu.h
+++ b/drivers/gpu/drm/msm/adreno/adreno_gpu.h
@@ -39,7 +39,16 @@ struct adreno_gpu_funcs {
 	struct msm_gpu_funcs base;
 };
 
-struct adreno_info;
+struct adreno_info {
+	struct adreno_rev rev;
+	uint32_t revn;
+	const char *name;
+	const char *pm4fw, *pfpfw;
+	uint32_t gmem;
+	struct msm_gpu *(*init)(struct drm_device *dev);
+};
+
+const struct adreno_info *adreno_info(struct adreno_rev rev);
 
 struct adreno_rbmemptrs {
 	volatile uint32_t rptr;
@@ -55,6 +64,9 @@ struct adreno_gpu {
 	uint32_t revn;  /* numeric revision name */
 	const struct adreno_gpu_funcs *funcs;
 
+	/* interesting register offsets to dump: */
+	const unsigned int *registers;
+
 	/* firmware: */
 	const struct firmware *pm4, *pfp;
 
@@ -131,8 +143,7 @@ void adreno_dump(struct msm_gpu *gpu);
 void adreno_wait_ring(struct msm_gpu *gpu, uint32_t ndwords);
 
 int adreno_gpu_init(struct drm_device *drm, struct platform_device *pdev,
-		struct adreno_gpu *gpu, const struct adreno_gpu_funcs *funcs,
-		struct adreno_rev rev);
+		struct adreno_gpu *gpu, const struct adreno_gpu_funcs *funcs);
 void adreno_gpu_cleanup(struct adreno_gpu *gpu);
 
 
diff --git a/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h b/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h
index 4eee0ec8f069..6ef43f66c30a 100644
--- a/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h
+++ b/drivers/gpu/drm/msm/adreno/adreno_pm4.xml.h
@@ -12,9 +12,9 @@ The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml  (   1453 bytes, from 2013-03-31 16:51:27)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/a2xx.xml          (  32901 bytes, from 2014-06-02 15:21:30)
 - /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_common.xml (   9859 bytes, from 2014-06-02 15:21:30)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14477 bytes, from 2014-05-16 11:51:57)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-06-25 12:57:16)
-- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  26602 bytes, from 2014-06-25 12:57:16)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/adreno_pm4.xml    (  14960 bytes, from 2014-07-27 17:22:13)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a3xx.xml          (  58020 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/adreno/a4xx.xml          (  41068 bytes, from 2014-08-01 12:22:48)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
@@ -163,12 +163,16 @@ enum adreno_pm4_type3_packets {
 	CP_INDIRECT_BUFFER_PFE = 63,
 	CP_SET_BIN = 76,
 	CP_TEST_TWO_MEMS = 113,
+	CP_REG_WR_NO_CTXT = 120,
+	CP_RECORD_PFP_TIMESTAMP = 17,
 	CP_WAIT_FOR_ME = 19,
 	CP_SET_DRAW_STATE = 67,
 	CP_DRAW_INDX_OFFSET = 56,
 	CP_DRAW_INDIRECT = 40,
 	CP_DRAW_INDX_INDIRECT = 41,
 	CP_DRAW_AUTO = 36,
+	CP_UNKNOWN_1A = 26,
+	CP_WIDE_REG_WRITE = 116,
 	IN_IB_PREFETCH_END = 23,
 	IN_SUBBLK_PREFETCH = 31,
 	IN_INSTR_PREFETCH = 32,
diff --git a/drivers/gpu/drm/msm/dsi/dsi.xml.h b/drivers/gpu/drm/msm/dsi/dsi.xml.h
index 0f1f5b9459a5..e965898dfda6 100644
--- a/drivers/gpu/drm/msm/dsi/dsi.xml.h
+++ b/drivers/gpu/drm/msm/dsi/dsi.xml.h
@@ -10,14 +10,14 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  17996 bytes, from 2013-12-01 19:10:31)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2013-11-30 15:00:52)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-06-25 12:55:02)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1544 bytes, from 2013-08-16 19:17:05)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-06-25 12:53:44)
+- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
 Copyright (C) 2013 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
diff --git a/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h b/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h
index d468f86f637c..f2bdda957205 100644
--- a/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h
+++ b/drivers/gpu/drm/msm/dsi/mmss_cc.xml.h
@@ -10,16 +10,16 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  17996 bytes, from 2013-12-01 19:10:31)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2013-11-30 15:00:52)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-06-25 12:55:02)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1544 bytes, from 2013-08-16 19:17:05)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-06-25 12:53:44)
+- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
-Copyright (C) 2013 by the following authors:
+Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
 
 Permission is hereby granted, free of charge, to any person obtaining
@@ -112,5 +112,11 @@ static inline uint32_t MMSS_CC_CLK_NS_VAL(uint32_t val)
 	return ((val) << MMSS_CC_CLK_NS_VAL__SHIFT) & MMSS_CC_CLK_NS_VAL__MASK;
 }
 
+#define REG_MMSS_CC_DSI2_PIXEL_CC				0x00000094
+
+#define REG_MMSS_CC_DSI2_PIXEL_NS				0x000000e4
+
+#define REG_MMSS_CC_DSI2_PIXEL_CC2				0x00000264
+
 
 #endif /* MMSS_CC_XML */
diff --git a/drivers/gpu/drm/msm/dsi/sfpb.xml.h b/drivers/gpu/drm/msm/dsi/sfpb.xml.h
index da8740054cdf..e5b071ffd865 100644
--- a/drivers/gpu/drm/msm/dsi/sfpb.xml.h
+++ b/drivers/gpu/drm/msm/dsi/sfpb.xml.h
@@ -10,14 +10,14 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  17996 bytes, from 2013-12-01 19:10:31)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2013-11-30 15:00:52)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-06-25 12:55:02)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1544 bytes, from 2013-08-16 19:17:05)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-06-25 12:53:44)
+- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
 Copyright (C) 2013 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi.c b/drivers/gpu/drm/msm/hdmi/hdmi.c
index a125a7e32742..9d00dcba6959 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi.c
+++ b/drivers/gpu/drm/msm/hdmi/hdmi.c
@@ -123,7 +123,7 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 	for (i = 0; i < config->hpd_reg_cnt; i++) {
 		struct regulator *reg;
 
-		reg = devm_regulator_get_exclusive(&pdev->dev,
+		reg = devm_regulator_get(&pdev->dev,
 				config->hpd_reg_names[i]);
 		if (IS_ERR(reg)) {
 			ret = PTR_ERR(reg);
@@ -139,7 +139,7 @@ struct hdmi *hdmi_init(struct drm_device *dev, struct drm_encoder *encoder)
 	for (i = 0; i < config->pwr_reg_cnt; i++) {
 		struct regulator *reg;
 
-		reg = devm_regulator_get_exclusive(&pdev->dev,
+		reg = devm_regulator_get(&pdev->dev,
 				config->pwr_reg_names[i]);
 		if (IS_ERR(reg)) {
 			ret = PTR_ERR(reg);
@@ -258,28 +258,30 @@ static void set_hdmi_pdev(struct drm_device *dev,
 	priv->hdmi_pdev = pdev;
 }
 
+#ifdef CONFIG_OF
+static int get_gpio(struct device *dev, struct device_node *of_node, const char *name)
+{
+	int gpio = of_get_named_gpio(of_node, name, 0);
+	if (gpio < 0) {
+		char name2[32];
+		snprintf(name2, sizeof(name2), "%s-gpio", name);
+		gpio = of_get_named_gpio(of_node, name2, 0);
+		if (gpio < 0) {
+			dev_err(dev, "failed to get gpio: %s (%d)\n",
+					name, gpio);
+			gpio = -1;
+		}
+	}
+	return gpio;
+}
+#endif
+
 static int hdmi_bind(struct device *dev, struct device *master, void *data)
 {
 	static struct hdmi_platform_config config = {};
 #ifdef CONFIG_OF
 	struct device_node *of_node = dev->of_node;
 
-	int get_gpio(const char *name)
-	{
-		int gpio = of_get_named_gpio(of_node, name, 0);
-		if (gpio < 0) {
-			char name2[32];
-			snprintf(name2, sizeof(name2), "%s-gpio", name);
-			gpio = of_get_named_gpio(of_node, name2, 0);
-			if (gpio < 0) {
-				dev_err(dev, "failed to get gpio: %s (%d)\n",
-						name, gpio);
-				gpio = -1;
-			}
-		}
-		return gpio;
-	}
-
 	if (of_device_is_compatible(of_node, "qcom,hdmi-tx-8074")) {
 		static const char *hpd_reg_names[] = {"hpd-gdsc", "hpd-5v"};
 		static const char *pwr_reg_names[] = {"core-vdda", "core-vcc"};
@@ -312,12 +314,12 @@ static int hdmi_bind(struct device *dev, struct device *master, void *data)
 	}
 
 	config.mmio_name     = "core_physical";
-	config.ddc_clk_gpio  = get_gpio("qcom,hdmi-tx-ddc-clk");
-	config.ddc_data_gpio = get_gpio("qcom,hdmi-tx-ddc-data");
-	config.hpd_gpio      = get_gpio("qcom,hdmi-tx-hpd");
-	config.mux_en_gpio   = get_gpio("qcom,hdmi-tx-mux-en");
-	config.mux_sel_gpio  = get_gpio("qcom,hdmi-tx-mux-sel");
-	config.mux_lpm_gpio  = get_gpio("qcom,hdmi-tx-mux-lpm");
+	config.ddc_clk_gpio  = get_gpio(dev, of_node, "qcom,hdmi-tx-ddc-clk");
+	config.ddc_data_gpio = get_gpio(dev, of_node, "qcom,hdmi-tx-ddc-data");
+	config.hpd_gpio      = get_gpio(dev, of_node, "qcom,hdmi-tx-hpd");
+	config.mux_en_gpio   = get_gpio(dev, of_node, "qcom,hdmi-tx-mux-en");
+	config.mux_sel_gpio  = get_gpio(dev, of_node, "qcom,hdmi-tx-mux-sel");
+	config.mux_lpm_gpio  = get_gpio(dev, of_node, "qcom,hdmi-tx-mux-lpm");
 
 #else
 	static const char *hpd_clk_names[] = {
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi.xml.h b/drivers/gpu/drm/msm/hdmi/hdmi.xml.h
index e89fe053d375..76fd0cfc6558 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi.xml.h
+++ b/drivers/gpu/drm/msm/hdmi/hdmi.xml.h
@@ -10,14 +10,14 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  17996 bytes, from 2013-12-01 19:10:31)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2013-11-30 15:00:52)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-06-25 12:55:02)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1544 bytes, from 2013-08-16 19:17:05)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-06-25 12:53:44)
+- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
 Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
diff --git a/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c b/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c
index 902d7685d441..f408b69486a8 100644
--- a/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c
+++ b/drivers/gpu/drm/msm/hdmi/hdmi_phy_8960.c
@@ -15,19 +15,25 @@
  * this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#ifdef CONFIG_COMMON_CLK
 #include <linux/clk.h>
 #include <linux/clk-provider.h>
+#endif
 
 #include "hdmi.h"
 
 struct hdmi_phy_8960 {
 	struct hdmi_phy base;
 	struct hdmi *hdmi;
+#ifdef CONFIG_COMMON_CLK
 	struct clk_hw pll_hw;
 	struct clk *pll;
 	unsigned long pixclk;
+#endif
 };
 #define to_hdmi_phy_8960(x) container_of(x, struct hdmi_phy_8960, base)
+
+#ifdef CONFIG_COMMON_CLK
 #define clk_to_phy(x) container_of(x, struct hdmi_phy_8960, pll_hw)
 
 /*
@@ -374,7 +380,7 @@ static struct clk_init_data pll_init = {
 	.parent_names = hdmi_pll_parents,
 	.num_parents = ARRAY_SIZE(hdmi_pll_parents),
 };
-
+#endif
 
 /*
  * HDMI Phy:
@@ -480,12 +486,15 @@ struct hdmi_phy *hdmi_phy_8960_init(struct hdmi *hdmi)
 {
 	struct hdmi_phy_8960 *phy_8960;
 	struct hdmi_phy *phy = NULL;
-	int ret, i;
+	int ret;
+#ifdef CONFIG_COMMON_CLK
+	int i;
 
 	/* sanity check: */
 	for (i = 0; i < (ARRAY_SIZE(freqtbl) - 1); i++)
 		if (WARN_ON(freqtbl[i].rate < freqtbl[i+1].rate))
 			return ERR_PTR(-EINVAL);
+#endif
 
 	phy_8960 = kzalloc(sizeof(*phy_8960), GFP_KERNEL);
 	if (!phy_8960) {
@@ -499,6 +508,7 @@ struct hdmi_phy *hdmi_phy_8960_init(struct hdmi *hdmi)
 
 	phy_8960->hdmi = hdmi;
 
+#ifdef CONFIG_COMMON_CLK
 	phy_8960->pll_hw.init = &pll_init;
 	phy_8960->pll = devm_clk_register(hdmi->dev->dev, &phy_8960->pll_hw);
 	if (IS_ERR(phy_8960->pll)) {
@@ -506,6 +516,7 @@ struct hdmi_phy *hdmi_phy_8960_init(struct hdmi *hdmi)
 		phy_8960->pll = NULL;
 		goto fail;
 	}
+#endif
 
 	return phy;
 
diff --git a/drivers/gpu/drm/msm/hdmi/qfprom.xml.h b/drivers/gpu/drm/msm/hdmi/qfprom.xml.h
index bd81db6a7829..d53c29327df9 100644
--- a/drivers/gpu/drm/msm/hdmi/qfprom.xml.h
+++ b/drivers/gpu/drm/msm/hdmi/qfprom.xml.h
@@ -10,14 +10,14 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  17996 bytes, from 2013-12-01 19:10:31)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2013-11-30 15:00:52)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-06-25 12:55:02)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1544 bytes, from 2013-08-16 19:17:05)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-06-25 12:53:44)
+- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
 Copyright (C) 2013 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h b/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h
index 122208e8a2ee..03c0bd9cd5b9 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4.xml.h
@@ -10,16 +10,16 @@ git clone https://github.com/freedreno/envytools.git
 The rules-ng-ng source files this header was generated from are:
 - /home/robclark/src/freedreno/envytools/rnndb/msm.xml                 (    647 bytes, from 2013-11-30 14:45:35)
 - /home/robclark/src/freedreno/envytools/rnndb/freedreno_copyright.xml (   1453 bytes, from 2013-03-31 16:51:27)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  17996 bytes, from 2013-12-01 19:10:31)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2013-11-30 15:00:52)
-- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-06-25 12:55:02)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp4.xml            (  20457 bytes, from 2014-08-01 12:22:48)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp_common.xml      (   1615 bytes, from 2014-07-17 15:34:33)
+- /home/robclark/src/freedreno/envytools/rnndb/mdp/mdp5.xml            (  22517 bytes, from 2014-07-17 15:34:33)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/dsi.xml             (  11712 bytes, from 2013-08-17 17:13:43)
 - /home/robclark/src/freedreno/envytools/rnndb/dsi/sfpb.xml            (    344 bytes, from 2013-08-11 19:26:32)
-- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1544 bytes, from 2013-08-16 19:17:05)
+- /home/robclark/src/freedreno/envytools/rnndb/dsi/mmss_cc.xml         (   1686 bytes, from 2014-08-01 12:23:53)
 - /home/robclark/src/freedreno/envytools/rnndb/hdmi/qfprom.xml         (    600 bytes, from 2013-07-05 19:21:12)
-- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-06-25 12:53:44)
+- /home/robclark/src/freedreno/envytools/rnndb/hdmi/hdmi.xml           (  23613 bytes, from 2014-07-17 15:33:30)
 
-Copyright (C) 2013 by the following authors:
+Copyright (C) 2013-2014 by the following authors:
 - Rob Clark <robdclark@gmail.com> (robclark)
 
 Permission is hereby granted, free of charge, to any person obtaining
@@ -871,6 +871,101 @@ static inline uint32_t MDP4_LCDC_UNDERFLOW_CLR_COLOR(uint32_t val)
 #define MDP4_LCDC_CTRL_POLARITY_VSYNC_LOW			0x00000002
 #define MDP4_LCDC_CTRL_POLARITY_DATA_EN_LOW			0x00000004
 
+#define REG_MDP4_LCDC_LVDS_INTF_CTL				0x000c2000
+#define MDP4_LCDC_LVDS_INTF_CTL_MODE_SEL			0x00000004
+#define MDP4_LCDC_LVDS_INTF_CTL_RGB_OUT				0x00000008
+#define MDP4_LCDC_LVDS_INTF_CTL_CH_SWAP				0x00000010
+#define MDP4_LCDC_LVDS_INTF_CTL_CH1_RES_BIT			0x00000020
+#define MDP4_LCDC_LVDS_INTF_CTL_CH2_RES_BIT			0x00000040
+#define MDP4_LCDC_LVDS_INTF_CTL_ENABLE				0x00000080
+#define MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE0_EN		0x00000100
+#define MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE1_EN		0x00000200
+#define MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE2_EN		0x00000400
+#define MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE3_EN		0x00000800
+#define MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE0_EN		0x00001000
+#define MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE1_EN		0x00002000
+#define MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE2_EN		0x00004000
+#define MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE3_EN		0x00008000
+#define MDP4_LCDC_LVDS_INTF_CTL_CH1_CLK_LANE_EN			0x00010000
+#define MDP4_LCDC_LVDS_INTF_CTL_CH2_CLK_LANE_EN			0x00020000
+
+static inline uint32_t REG_MDP4_LCDC_LVDS_MUX_CTL(uint32_t i0) { return 0x000c2014 + 0x8*i0; }
+
+static inline uint32_t REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(uint32_t i0) { return 0x000c2014 + 0x8*i0; }
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0__MASK		0x000000ff
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0__SHIFT		0
+static inline uint32_t MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(uint32_t val)
+{
+	return ((val) << MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0__SHIFT) & MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0__MASK;
+}
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1__MASK		0x0000ff00
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1__SHIFT		8
+static inline uint32_t MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(uint32_t val)
+{
+	return ((val) << MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1__SHIFT) & MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1__MASK;
+}
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2__MASK		0x00ff0000
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2__SHIFT		16
+static inline uint32_t MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(uint32_t val)
+{
+	return ((val) << MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2__SHIFT) & MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2__MASK;
+}
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3__MASK		0xff000000
+#define MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3__SHIFT		24
+static inline uint32_t MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(uint32_t val)
+{
+	return ((val) << MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3__SHIFT) & MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3__MASK;
+}
+
+static inline uint32_t REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(uint32_t i0) { return 0x000c2018 + 0x8*i0; }
+#define MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4__MASK		0x000000ff
+#define MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4__SHIFT		0
+static inline uint32_t MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(uint32_t val)
+{
+	return ((val) << MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4__SHIFT) & MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4__MASK;
+}
+#define MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5__MASK		0x0000ff00
+#define MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5__SHIFT		8
+static inline uint32_t MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(uint32_t val)
+{
+	return ((val) << MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5__SHIFT) & MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5__MASK;
+}
+#define MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6__MASK		0x00ff0000
+#define MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6__SHIFT		16
+static inline uint32_t MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(uint32_t val)
+{
+	return ((val) << MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6__SHIFT) & MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6__MASK;
+}
+
+#define REG_MDP4_LCDC_LVDS_PHY_RESET				0x000c2034
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_0				0x000c3000
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_1				0x000c3004
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_2				0x000c3008
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_3				0x000c300c
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_5				0x000c3014
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_6				0x000c3018
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_7				0x000c301c
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_8				0x000c3020
+
+#define REG_MDP4_LVDS_PHY_PLL_CTRL_9				0x000c3024
+
+#define REG_MDP4_LVDS_PHY_PLL_LOCKED				0x000c3080
+
+#define REG_MDP4_LVDS_PHY_CFG2					0x000c3108
+
+#define REG_MDP4_LVDS_PHY_CFG0					0x000c3100
+#define MDP4_LVDS_PHY_CFG0_SERIALIZATION_ENBLE			0x00000010
+#define MDP4_LVDS_PHY_CFG0_CHANNEL0				0x00000040
+#define MDP4_LVDS_PHY_CFG0_CHANNEL1				0x00000080
+
 #define REG_MDP4_DTV						0x000d0000
 
 #define REG_MDP4_DTV_ENABLE					0x000d0000
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
index 74cebb51e8c2..7d00f7fb5773 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_crtc.c
@@ -273,14 +273,17 @@ static void blend_setup(struct drm_crtc *crtc)
 	};
 	bool alpha[4]= { false, false, false, false };
 
+	/* Don't rely on value read back from hw, but instead use our
+	 * own shadowed value.  Possibly disable/reenable looses the
+	 * previous value and goes back to power-on default?
+	 */
+	mixer_cfg = mdp4_kms->mixer_cfg;
+
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_LOW0(ovlp), 0);
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_LOW1(ovlp), 0);
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_HIGH0(ovlp), 0);
 	mdp4_write(mdp4_kms, REG_MDP4_OVLP_TRANSP_HIGH1(ovlp), 0);
 
-	/* TODO single register for all CRTCs, so this won't work properly
-	 * when multiple CRTCs are active..
-	 */
 	for (i = 0; i < ARRAY_SIZE(mdp4_crtc->planes); i++) {
 		struct drm_plane *plane = mdp4_crtc->planes[i];
 		if (plane) {
@@ -291,7 +294,8 @@ static void blend_setup(struct drm_crtc *crtc)
 					to_mdp_format(msm_framebuffer_format(plane->fb));
 				alpha[idx-1] = format->alpha_enable;
 			}
-			mixer_cfg |= mixercfg(mdp4_crtc->mixer, pipe_id, stages[idx]);
+			mixer_cfg = mixercfg(mixer_cfg, mdp4_crtc->mixer,
+					pipe_id, stages[idx]);
 		}
 	}
 
@@ -320,6 +324,7 @@ static void blend_setup(struct drm_crtc *crtc)
 		mdp4_write(mdp4_kms, REG_MDP4_OVLP_STAGE_TRANSP_HIGH1(ovlp, i), 0);
 	}
 
+	mdp4_kms->mixer_cfg = mixer_cfg;
 	mdp4_write(mdp4_kms, REG_MDP4_LAYERMIXER_IN_CFG, mixer_cfg);
 }
 
@@ -397,6 +402,7 @@ static void mdp4_crtc_prepare(struct drm_crtc *crtc)
 	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
 	DBG("%s", mdp4_crtc->name);
 	/* make sure we hold a ref to mdp clks while setting up mode: */
+	drm_crtc_vblank_get(crtc);
 	mdp4_enable(get_kms(crtc));
 	mdp4_crtc_dpms(crtc, DRM_MODE_DPMS_OFF);
 }
@@ -407,6 +413,7 @@ static void mdp4_crtc_commit(struct drm_crtc *crtc)
 	crtc_flush(crtc);
 	/* drop the ref to mdp clk's that we got in prepare: */
 	mdp4_disable(get_kms(crtc));
+	drm_crtc_vblank_put(crtc);
 }
 
 static int mdp4_crtc_mode_set_base(struct drm_crtc *crtc, int x, int y,
@@ -670,7 +677,7 @@ void mdp4_crtc_set_config(struct drm_crtc *crtc, uint32_t config)
 }
 
 /* set interface for routing crtc->encoder: */
-void mdp4_crtc_set_intf(struct drm_crtc *crtc, enum mdp4_intf intf)
+void mdp4_crtc_set_intf(struct drm_crtc *crtc, enum mdp4_intf intf, int mixer)
 {
 	struct mdp4_crtc *mdp4_crtc = to_mdp4_crtc(crtc);
 	struct mdp4_kms *mdp4_kms = get_kms(crtc);
@@ -696,15 +703,13 @@ void mdp4_crtc_set_intf(struct drm_crtc *crtc, enum mdp4_intf intf)
 	if (intf == INTF_DSI_VIDEO) {
 		intf_sel &= ~MDP4_DISP_INTF_SEL_DSI_CMD;
 		intf_sel |= MDP4_DISP_INTF_SEL_DSI_VIDEO;
-		mdp4_crtc->mixer = 0;
 	} else if (intf == INTF_DSI_CMD) {
 		intf_sel &= ~MDP4_DISP_INTF_SEL_DSI_VIDEO;
 		intf_sel |= MDP4_DISP_INTF_SEL_DSI_CMD;
-		mdp4_crtc->mixer = 0;
-	} else if (intf == INTF_LCDC_DTV){
-		mdp4_crtc->mixer = 1;
 	}
 
+	mdp4_crtc->mixer = mixer;
+
 	blend_setup(crtc);
 
 	DBG("%s: intf_sel=%08x", mdp4_crtc->name, intf_sel);
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c
index 067ed03b35fe..c3878420180b 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_dtv_encoder.c
@@ -233,7 +233,7 @@ static void mdp4_dtv_encoder_commit(struct drm_encoder *encoder)
 			MDP4_DMA_CONFIG_G_BPC(BPC8) |
 			MDP4_DMA_CONFIG_B_BPC(BPC8) |
 			MDP4_DMA_CONFIG_PACK(0x21));
-	mdp4_crtc_set_intf(encoder->crtc, INTF_LCDC_DTV);
+	mdp4_crtc_set_intf(encoder->crtc, INTF_LCDC_DTV, 1);
 	mdp4_dtv_encoder_dpms(encoder, DRM_MODE_DPMS_ON);
 }
 
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
index 733646c0d3f8..79d804e61cc4 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.c
@@ -106,6 +106,7 @@ static int mdp4_hw_init(struct msm_kms *kms)
 
 	if (mdp4_kms->rev >= 2)
 		mdp4_write(mdp4_kms, REG_MDP4_LAYERMIXER_IN_CFG_UPDATE_METHOD, 1);
+	mdp4_write(mdp4_kms, REG_MDP4_LAYERMIXER_IN_CFG, 0);
 
 	/* disable CSC matrix / YUV by default: */
 	mdp4_write(mdp4_kms, REG_MDP4_PIPE_OP_MODE(VG1), 0);
@@ -196,6 +197,28 @@ int mdp4_enable(struct mdp4_kms *mdp4_kms)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static struct drm_panel *detect_panel(struct drm_device *dev, const char *name)
+{
+	struct device_node *n;
+	struct drm_panel *panel = NULL;
+
+	n = of_parse_phandle(dev->dev->of_node, name, 0);
+	if (n) {
+		panel = of_drm_find_panel(n);
+		if (!panel)
+			panel = ERR_PTR(-EPROBE_DEFER);
+	}
+
+	return panel;
+}
+#else
+static struct drm_panel *detect_panel(struct drm_device *dev, const char *name)
+{
+	// ??? maybe use a module param to specify which panel is attached?
+}
+#endif
+
 static int modeset_init(struct mdp4_kms *mdp4_kms)
 {
 	struct drm_device *dev = mdp4_kms->dev;
@@ -203,14 +226,11 @@ static int modeset_init(struct mdp4_kms *mdp4_kms)
 	struct drm_plane *plane;
 	struct drm_crtc *crtc;
 	struct drm_encoder *encoder;
+	struct drm_connector *connector;
+	struct drm_panel *panel;
 	struct hdmi *hdmi;
 	int ret;
 
-	/*
-	 *  NOTE: this is a bit simplistic until we add support
-	 * for more than just RGB1->DMA_E->DTV->HDMI
-	 */
-
 	/* construct non-private planes: */
 	plane = mdp4_plane_init(dev, VG1, false);
 	if (IS_ERR(plane)) {
@@ -228,7 +248,57 @@ static int modeset_init(struct mdp4_kms *mdp4_kms)
 	}
 	priv->planes[priv->num_planes++] = plane;
 
-	/* the CRTCs get constructed with a private plane: */
+	/*
+	 * Setup the LCDC/LVDS path: RGB2 -> DMA_P -> LCDC -> LVDS:
+	 */
+
+	panel = detect_panel(dev, "qcom,lvds-panel");
+	if (IS_ERR(panel)) {
+		ret = PTR_ERR(panel);
+		dev_err(dev->dev, "failed to detect LVDS panel: %d\n", ret);
+		goto fail;
+	}
+
+	plane = mdp4_plane_init(dev, RGB2, true);
+	if (IS_ERR(plane)) {
+		dev_err(dev->dev, "failed to construct plane for RGB2\n");
+		ret = PTR_ERR(plane);
+		goto fail;
+	}
+
+	crtc  = mdp4_crtc_init(dev, plane, priv->num_crtcs, 0, DMA_P);
+	if (IS_ERR(crtc)) {
+		dev_err(dev->dev, "failed to construct crtc for DMA_P\n");
+		ret = PTR_ERR(crtc);
+		goto fail;
+	}
+
+	encoder = mdp4_lcdc_encoder_init(dev, panel);
+	if (IS_ERR(encoder)) {
+		dev_err(dev->dev, "failed to construct LCDC encoder\n");
+		ret = PTR_ERR(encoder);
+		goto fail;
+	}
+
+	/* LCDC can be hooked to DMA_P: */
+	encoder->possible_crtcs = 1 << priv->num_crtcs;
+
+	priv->crtcs[priv->num_crtcs++] = crtc;
+	priv->encoders[priv->num_encoders++] = encoder;
+
+	connector = mdp4_lvds_connector_init(dev, panel, encoder);
+	if (IS_ERR(connector)) {
+		ret = PTR_ERR(connector);
+		dev_err(dev->dev, "failed to initialize LVDS connector: %d\n", ret);
+		goto fail;
+	}
+
+	priv->connectors[priv->num_connectors++] = connector;
+
+	/*
+	 * Setup DTV/HDMI path: RGB1 -> DMA_E -> DTV -> HDMI:
+	 */
+
 	plane = mdp4_plane_init(dev, RGB1, true);
 	if (IS_ERR(plane)) {
 		dev_err(dev->dev, "failed to construct plane for RGB1\n");
@@ -242,7 +312,6 @@ static int modeset_init(struct mdp4_kms *mdp4_kms)
 		ret = PTR_ERR(crtc);
 		goto fail;
 	}
-	priv->crtcs[priv->num_crtcs++] = crtc;
 
 	encoder = mdp4_dtv_encoder_init(dev);
 	if (IS_ERR(encoder)) {
@@ -250,7 +319,11 @@ static int modeset_init(struct mdp4_kms *mdp4_kms)
 		ret = PTR_ERR(encoder);
 		goto fail;
 	}
-	encoder->possible_crtcs = 0x1;     /* DTV can be hooked to DMA_E */
+
+	/* DTV can be hooked to DMA_E: */
+	encoder->possible_crtcs = 1 << priv->num_crtcs;
+
+	priv->crtcs[priv->num_crtcs++] = crtc;
 	priv->encoders[priv->num_encoders++] = encoder;
 
 	hdmi = hdmi_init(dev, encoder);
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
index 3225da804c61..9ff6e7ccfe90 100644
--- a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_kms.h
@@ -23,6 +23,8 @@
 #include "mdp/mdp_kms.h"
 #include "mdp4.xml.h"
 
+#include "drm_panel.h"
+
 struct mdp4_kms {
 	struct mdp_kms base;
 
@@ -30,6 +32,13 @@ struct mdp4_kms {
 
 	int rev;
 
+	/* Shadow value for MDP4_LAYERMIXER_IN_CFG.. since setup for all
+	 * crtcs/encoders is in one shared register, we need to update it
+	 * via read/modify/write.  But to avoid getting confused by power-
+	 * on-default values after resume, use this shadow value instead:
+	 */
+	uint32_t mixer_cfg;
+
 	/* mapper-id used to request GEM buffer mapped for scanout: */
 	int id;
 
@@ -74,7 +83,7 @@ static inline uint32_t pipe2flush(enum mdp4_pipe pipe)
 	case VG1:      return MDP4_OVERLAY_FLUSH_VG1;
 	case VG2:      return MDP4_OVERLAY_FLUSH_VG2;
 	case RGB1:     return MDP4_OVERLAY_FLUSH_RGB1;
-	case RGB2:     return MDP4_OVERLAY_FLUSH_RGB1;
+	case RGB2:     return MDP4_OVERLAY_FLUSH_RGB2;
 	default:       return 0;
 	}
 }
@@ -108,38 +117,50 @@ static inline uint32_t dma2err(enum mdp4_dma dma)
 	}
 }
 
-static inline uint32_t mixercfg(int mixer, enum mdp4_pipe pipe,
-		enum mdp_mixer_stage_id stage)
+static inline uint32_t mixercfg(uint32_t mixer_cfg, int mixer,
+		enum mdp4_pipe pipe, enum mdp_mixer_stage_id stage)
 {
-	uint32_t mixer_cfg = 0;
-
 	switch (pipe) {
 	case VG1:
-		mixer_cfg = MDP4_LAYERMIXER_IN_CFG_PIPE0(stage) |
+		mixer_cfg &= ~(MDP4_LAYERMIXER_IN_CFG_PIPE0__MASK |
+				MDP4_LAYERMIXER_IN_CFG_PIPE0_MIXER1);
+		mixer_cfg |= MDP4_LAYERMIXER_IN_CFG_PIPE0(stage) |
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE0_MIXER1);
 		break;
 	case VG2:
-		mixer_cfg = MDP4_LAYERMIXER_IN_CFG_PIPE1(stage) |
+		mixer_cfg &= ~(MDP4_LAYERMIXER_IN_CFG_PIPE1__MASK |
+				MDP4_LAYERMIXER_IN_CFG_PIPE1_MIXER1);
+		mixer_cfg |= MDP4_LAYERMIXER_IN_CFG_PIPE1(stage) |
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE1_MIXER1);
 		break;
 	case RGB1:
-		mixer_cfg = MDP4_LAYERMIXER_IN_CFG_PIPE2(stage) |
+		mixer_cfg &= ~(MDP4_LAYERMIXER_IN_CFG_PIPE2__MASK |
+				MDP4_LAYERMIXER_IN_CFG_PIPE2_MIXER1);
+		mixer_cfg |= MDP4_LAYERMIXER_IN_CFG_PIPE2(stage) |
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE2_MIXER1);
 		break;
 	case RGB2:
-		mixer_cfg = MDP4_LAYERMIXER_IN_CFG_PIPE3(stage) |
+		mixer_cfg &= ~(MDP4_LAYERMIXER_IN_CFG_PIPE3__MASK |
+				MDP4_LAYERMIXER_IN_CFG_PIPE3_MIXER1);
+		mixer_cfg |= MDP4_LAYERMIXER_IN_CFG_PIPE3(stage) |
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE3_MIXER1);
 		break;
 	case RGB3:
-		mixer_cfg = MDP4_LAYERMIXER_IN_CFG_PIPE4(stage) |
+		mixer_cfg &= ~(MDP4_LAYERMIXER_IN_CFG_PIPE4__MASK |
+				MDP4_LAYERMIXER_IN_CFG_PIPE4_MIXER1);
+		mixer_cfg |= MDP4_LAYERMIXER_IN_CFG_PIPE4(stage) |
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE4_MIXER1);
 		break;
 	case VG3:
-		mixer_cfg = MDP4_LAYERMIXER_IN_CFG_PIPE5(stage) |
+		mixer_cfg &= ~(MDP4_LAYERMIXER_IN_CFG_PIPE5__MASK |
+				MDP4_LAYERMIXER_IN_CFG_PIPE5_MIXER1);
+		mixer_cfg |= MDP4_LAYERMIXER_IN_CFG_PIPE5(stage) |
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE5_MIXER1);
 		break;
 	case VG4:
-		mixer_cfg = MDP4_LAYERMIXER_IN_CFG_PIPE6(stage) |
+		mixer_cfg &= ~(MDP4_LAYERMIXER_IN_CFG_PIPE6__MASK |
+				MDP4_LAYERMIXER_IN_CFG_PIPE6_MIXER1);
+		mixer_cfg |= MDP4_LAYERMIXER_IN_CFG_PIPE6(stage) |
 			COND(mixer == 1, MDP4_LAYERMIXER_IN_CFG_PIPE6_MIXER1);
 		break;
 	default:
@@ -188,7 +209,7 @@ struct drm_plane *mdp4_plane_init(struct drm_device *dev,
 uint32_t mdp4_crtc_vblank(struct drm_crtc *crtc);
 void mdp4_crtc_cancel_pending_flip(struct drm_crtc *crtc, struct drm_file *file);
 void mdp4_crtc_set_config(struct drm_crtc *crtc, uint32_t config);
-void mdp4_crtc_set_intf(struct drm_crtc *crtc, enum mdp4_intf intf);
+void mdp4_crtc_set_intf(struct drm_crtc *crtc, enum mdp4_intf intf, int mixer);
 void mdp4_crtc_attach(struct drm_crtc *crtc, struct drm_plane *plane);
 void mdp4_crtc_detach(struct drm_crtc *crtc, struct drm_plane *plane);
 struct drm_crtc *mdp4_crtc_init(struct drm_device *dev,
@@ -198,6 +219,22 @@ struct drm_crtc *mdp4_crtc_init(struct drm_device *dev,
 long mdp4_dtv_round_pixclk(struct drm_encoder *encoder, unsigned long rate);
 struct drm_encoder *mdp4_dtv_encoder_init(struct drm_device *dev);
 
+long mdp4_lcdc_round_pixclk(struct drm_encoder *encoder, unsigned long rate);
+struct drm_encoder *mdp4_lcdc_encoder_init(struct drm_device *dev,
+		struct drm_panel *panel);
+
+struct drm_connector *mdp4_lvds_connector_init(struct drm_device *dev,
+		struct drm_panel *panel, struct drm_encoder *encoder);
+
+#ifdef CONFIG_COMMON_CLK
+struct clk *mpd4_lvds_pll_init(struct drm_device *dev);
+#else
+static inline struct clk *mpd4_lvds_pll_init(struct drm_device *dev)
+{
+	return ERR_PTR(-ENODEV);
+}
+#endif
+
 #ifdef CONFIG_MSM_BUS_SCALING
 static inline int match_dev_name(struct device *dev, void *data)
 {
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lcdc_encoder.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lcdc_encoder.c
new file mode 100644
index 000000000000..41f6436754fc
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lcdc_encoder.c
@@ -0,0 +1,506 @@
+/*
+ * Copyright (C) 2014 Red Hat
+ * Author: Rob Clark <robdclark@gmail.com>
+ * Author: Vinay Simha <vinaysimha@inforcecomputing.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "mdp4_kms.h"
+
+#include "drm_crtc.h"
+#include "drm_crtc_helper.h"
+
+struct mdp4_lcdc_encoder {
+	struct drm_encoder base;
+	struct drm_panel *panel;
+	struct clk *lcdc_clk;
+	unsigned long int pixclock;
+	struct regulator *regs[3];
+	bool enabled;
+	uint32_t bsc;
+};
+#define to_mdp4_lcdc_encoder(x) container_of(x, struct mdp4_lcdc_encoder, base)
+
+static struct mdp4_kms *get_kms(struct drm_encoder *encoder)
+{
+	struct msm_drm_private *priv = encoder->dev->dev_private;
+	return to_mdp4_kms(to_mdp_kms(priv->kms));
+}
+
+#ifdef CONFIG_MSM_BUS_SCALING
+#include <mach/board.h>
+static void bs_init(struct mdp4_lcdc_encoder *mdp4_lcdc_encoder)
+{
+	struct drm_device *dev = mdp4_lcdc_encoder->base.dev;
+	struct lcdc_platform_data *lcdc_pdata = mdp4_find_pdata("lvds.0");
+
+	if (!lcdc_pdata) {
+		dev_err(dev->dev, "could not find lvds pdata\n");
+		return;
+	}
+
+	if (lcdc_pdata->bus_scale_table) {
+		mdp4_lcdc_encoder->bsc = msm_bus_scale_register_client(
+				lcdc_pdata->bus_scale_table);
+		DBG("lvds : bus scale client: %08x", mdp4_lcdc_encoder->bsc);
+	}
+}
+
+static void bs_fini(struct mdp4_lcdc_encoder *mdp4_lcdc_encoder)
+{
+	if (mdp4_lcdc_encoder->bsc) {
+		msm_bus_scale_unregister_client(mdp4_lcdc_encoder->bsc);
+		mdp4_lcdc_encoder->bsc = 0;
+	}
+}
+
+static void bs_set(struct mdp4_lcdc_encoder *mdp4_lcdc_encoder, int idx)
+{
+	if (mdp4_lcdc_encoder->bsc) {
+		DBG("set bus scaling: %d", idx);
+		msm_bus_scale_client_update_request(mdp4_lcdc_encoder->bsc, idx);
+	}
+}
+#else
+static void bs_init(struct mdp4_lcdc_encoder *mdp4_lcdc_encoder) {}
+static void bs_fini(struct mdp4_lcdc_encoder *mdp4_lcdc_encoder) {}
+static void bs_set(struct mdp4_lcdc_encoder *mdp4_lcdc_encoder, int idx) {}
+#endif
+
+static void mdp4_lcdc_encoder_destroy(struct drm_encoder *encoder)
+{
+	struct mdp4_lcdc_encoder *mdp4_lcdc_encoder =
+			to_mdp4_lcdc_encoder(encoder);
+	bs_fini(mdp4_lcdc_encoder);
+	drm_encoder_cleanup(encoder);
+	kfree(mdp4_lcdc_encoder);
+}
+
+static const struct drm_encoder_funcs mdp4_lcdc_encoder_funcs = {
+	.destroy = mdp4_lcdc_encoder_destroy,
+};
+
+/* this should probably be a helper: */
+struct drm_connector *get_connector(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_connector *connector;
+
+	list_for_each_entry(connector, &dev->mode_config.connector_list, head)
+		if (connector->encoder == encoder)
+			return connector;
+
+	return NULL;
+}
+
+static void setup_phy(struct drm_encoder *encoder)
+{
+	struct drm_device *dev = encoder->dev;
+	struct drm_connector *connector = get_connector(encoder);
+	struct mdp4_kms *mdp4_kms = get_kms(encoder);
+	uint32_t lvds_intf = 0, lvds_phy_cfg0 = 0;
+	int bpp, nchan, swap;
+
+	if (!connector)
+		return;
+
+	bpp = 3 * connector->display_info.bpc;
+
+	if (!bpp)
+		bpp = 18;
+
+	/* TODO, these should come from panel somehow: */
+	nchan = 1;
+	swap = 0;
+
+	switch (bpp) {
+	case 24:
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(0),
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(0x08) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(0x05) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(0x04) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(0x03));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(0),
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(0x02) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(0x01) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(0x00));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(1),
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(0x11) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(0x10) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(0x0d) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(0x0c));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(1),
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(0x0b) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(0x0a) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(0x09));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(2),
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(0x1a) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(0x19) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(0x18) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(0x15));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(2),
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(0x14) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(0x13) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(0x12));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(3),
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(0x1b) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(0x17) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(0x16) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(0x0f));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(3),
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(0x0e) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(0x07) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(0x06));
+		if (nchan == 2) {
+			lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE3_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE2_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE1_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE0_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE3_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE2_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE1_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE0_EN;
+		} else {
+			lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE3_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE2_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE1_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE0_EN;
+		}
+		break;
+
+	case 18:
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(0),
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(0x0a) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(0x07) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(0x06) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(0x05));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(0),
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(0x04) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(0x03) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(0x02));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(1),
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(0x13) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(0x12) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(0x0f) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(0x0e));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(1),
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(0x0d) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(0x0c) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(0x0b));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_3_TO_0(2),
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT0(0x1a) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT1(0x19) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT2(0x18) |
+				MDP4_LCDC_LVDS_MUX_CTL_3_TO_0_BIT3(0x17));
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_MUX_CTL_6_TO_4(2),
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT4(0x16) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT5(0x15) |
+				MDP4_LCDC_LVDS_MUX_CTL_6_TO_4_BIT6(0x14));
+		if (nchan == 2) {
+			lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE2_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE1_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH2_DATA_LANE0_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE2_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE1_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE0_EN;
+		} else {
+			lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE2_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE1_EN |
+					MDP4_LCDC_LVDS_INTF_CTL_CH1_DATA_LANE0_EN;
+		}
+		lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_RGB_OUT;
+		break;
+
+	default:
+		dev_err(dev->dev, "unknown bpp: %d\n", bpp);
+		return;
+	}
+
+	switch (nchan) {
+	case 1:
+		lvds_phy_cfg0 = MDP4_LVDS_PHY_CFG0_CHANNEL0;
+		lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_CH1_CLK_LANE_EN |
+				MDP4_LCDC_LVDS_INTF_CTL_MODE_SEL;
+		break;
+	case 2:
+		lvds_phy_cfg0 = MDP4_LVDS_PHY_CFG0_CHANNEL0 |
+				MDP4_LVDS_PHY_CFG0_CHANNEL1;
+		lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_CH2_CLK_LANE_EN |
+				MDP4_LCDC_LVDS_INTF_CTL_CH1_CLK_LANE_EN;
+		break;
+	default:
+		dev_err(dev->dev, "unknown # of channels: %d\n", nchan);
+		return;
+	}
+
+	if (swap)
+		lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_CH_SWAP;
+
+	lvds_intf |= MDP4_LCDC_LVDS_INTF_CTL_ENABLE;
+
+	mdp4_write(mdp4_kms, REG_MDP4_LVDS_PHY_CFG0, lvds_phy_cfg0);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_INTF_CTL, lvds_intf);
+	mdp4_write(mdp4_kms, REG_MDP4_LVDS_PHY_CFG2, 0x30);
+
+	mb();
+	udelay(1);
+	lvds_phy_cfg0 |= MDP4_LVDS_PHY_CFG0_SERIALIZATION_ENBLE;
+	mdp4_write(mdp4_kms, REG_MDP4_LVDS_PHY_CFG0, lvds_phy_cfg0);
+}
+
+static void mdp4_lcdc_encoder_dpms(struct drm_encoder *encoder, int mode)
+{
+	struct drm_device *dev = encoder->dev;
+	struct mdp4_lcdc_encoder *mdp4_lcdc_encoder =
+			to_mdp4_lcdc_encoder(encoder);
+	struct mdp4_kms *mdp4_kms = get_kms(encoder);
+	struct drm_panel *panel = mdp4_lcdc_encoder->panel;
+	bool enabled = (mode == DRM_MODE_DPMS_ON);
+	int i, ret;
+
+	DBG("mode=%d", mode);
+
+	if (enabled == mdp4_lcdc_encoder->enabled)
+		return;
+
+	if (enabled) {
+		unsigned long pc = mdp4_lcdc_encoder->pixclock;
+		int ret;
+
+		bs_set(mdp4_lcdc_encoder, 1);
+
+		for (i = 0; i < ARRAY_SIZE(mdp4_lcdc_encoder->regs); i++) {
+			ret = regulator_enable(mdp4_lcdc_encoder->regs[i]);
+			if (ret)
+				dev_err(dev->dev, "failed to enable regulator: %d\n", ret);
+		}
+
+		DBG("setting lcdc_clk=%lu", pc);
+		ret = clk_set_rate(mdp4_lcdc_encoder->lcdc_clk, pc);
+		if (ret)
+			dev_err(dev->dev, "failed to configure lcdc_clk: %d\n", ret);
+		ret = clk_prepare_enable(mdp4_lcdc_encoder->lcdc_clk);
+		if (ret)
+			dev_err(dev->dev, "failed to enable lcdc_clk: %d\n", ret);
+
+		if (panel)
+			drm_panel_enable(panel);
+
+		setup_phy(encoder);
+
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_ENABLE, 1);
+	} else {
+		mdp4_write(mdp4_kms, REG_MDP4_LCDC_ENABLE, 0);
+
+		if (panel)
+			drm_panel_disable(panel);
+
+		/*
+		 * Wait for a vsync so we know the ENABLE=0 latched before
+		 * the (connector) source of the vsync's gets disabled,
+		 * otherwise we end up in a funny state if we re-enable
+		 * before the disable latches, which results that some of
+		 * the settings changes for the new modeset (like new
+		 * scanout buffer) don't latch properly..
+		 */
+		mdp_irq_wait(&mdp4_kms->base, MDP4_IRQ_PRIMARY_VSYNC);
+
+		clk_disable_unprepare(mdp4_lcdc_encoder->lcdc_clk);
+
+		for (i = 0; i < ARRAY_SIZE(mdp4_lcdc_encoder->regs); i++) {
+			ret = regulator_disable(mdp4_lcdc_encoder->regs[i]);
+			if (ret)
+				dev_err(dev->dev, "failed to disable regulator: %d\n", ret);
+		}
+
+		bs_set(mdp4_lcdc_encoder, 0);
+	}
+
+	mdp4_lcdc_encoder->enabled = enabled;
+}
+
+static bool mdp4_lcdc_encoder_mode_fixup(struct drm_encoder *encoder,
+		const struct drm_display_mode *mode,
+		struct drm_display_mode *adjusted_mode)
+{
+	return true;
+}
+
+static void mdp4_lcdc_encoder_mode_set(struct drm_encoder *encoder,
+		struct drm_display_mode *mode,
+		struct drm_display_mode *adjusted_mode)
+{
+	struct mdp4_lcdc_encoder *mdp4_lcdc_encoder =
+			to_mdp4_lcdc_encoder(encoder);
+	struct mdp4_kms *mdp4_kms = get_kms(encoder);
+	uint32_t lcdc_hsync_skew, vsync_period, vsync_len, ctrl_pol;
+	uint32_t display_v_start, display_v_end;
+	uint32_t hsync_start_x, hsync_end_x;
+
+	mode = adjusted_mode;
+
+	DBG("set mode: %d:\"%s\" %d %d %d %d %d %d %d %d %d %d 0x%x 0x%x",
+			mode->base.id, mode->name,
+			mode->vrefresh, mode->clock,
+			mode->hdisplay, mode->hsync_start,
+			mode->hsync_end, mode->htotal,
+			mode->vdisplay, mode->vsync_start,
+			mode->vsync_end, mode->vtotal,
+			mode->type, mode->flags);
+
+	mdp4_lcdc_encoder->pixclock = mode->clock * 1000;
+
+	DBG("pixclock=%lu", mdp4_lcdc_encoder->pixclock);
+
+	ctrl_pol = 0;
+	if (mode->flags & DRM_MODE_FLAG_NHSYNC)
+		ctrl_pol |= MDP4_LCDC_CTRL_POLARITY_HSYNC_LOW;
+	if (mode->flags & DRM_MODE_FLAG_NVSYNC)
+		ctrl_pol |= MDP4_LCDC_CTRL_POLARITY_VSYNC_LOW;
+	/* probably need to get DATA_EN polarity from panel.. */
+
+	lcdc_hsync_skew = 0;  /* get this from panel? */
+
+	hsync_start_x = (mode->htotal - mode->hsync_start);
+	hsync_end_x = mode->htotal - (mode->hsync_start - mode->hdisplay) - 1;
+
+	vsync_period = mode->vtotal * mode->htotal;
+	vsync_len = (mode->vsync_end - mode->vsync_start) * mode->htotal;
+	display_v_start = (mode->vtotal - mode->vsync_start) * mode->htotal + lcdc_hsync_skew;
+	display_v_end = vsync_period - ((mode->vsync_start - mode->vdisplay) * mode->htotal) + lcdc_hsync_skew - 1;
+
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_HSYNC_CTRL,
+			MDP4_LCDC_HSYNC_CTRL_PULSEW(mode->hsync_end - mode->hsync_start) |
+			MDP4_LCDC_HSYNC_CTRL_PERIOD(mode->htotal));
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_VSYNC_PERIOD, vsync_period);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_VSYNC_LEN, vsync_len);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_DISPLAY_HCTRL,
+			MDP4_LCDC_DISPLAY_HCTRL_START(hsync_start_x) |
+			MDP4_LCDC_DISPLAY_HCTRL_END(hsync_end_x));
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_DISPLAY_VSTART, display_v_start);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_DISPLAY_VEND, display_v_end);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_BORDER_CLR, 0);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_UNDERFLOW_CLR,
+			MDP4_LCDC_UNDERFLOW_CLR_ENABLE_RECOVERY |
+			MDP4_LCDC_UNDERFLOW_CLR_COLOR(0xff));
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_HSYNC_SKEW, lcdc_hsync_skew);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_CTRL_POLARITY, ctrl_pol);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_ACTIVE_HCTL,
+			MDP4_LCDC_ACTIVE_HCTL_START(0) |
+			MDP4_LCDC_ACTIVE_HCTL_END(0));
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_ACTIVE_VSTART, 0);
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_ACTIVE_VEND, 0);
+}
+
+static void mdp4_lcdc_encoder_prepare(struct drm_encoder *encoder)
+{
+	mdp4_lcdc_encoder_dpms(encoder, DRM_MODE_DPMS_OFF);
+}
+
+static void mdp4_lcdc_encoder_commit(struct drm_encoder *encoder)
+{
+	/* TODO: hard-coded for 18bpp: */
+	mdp4_crtc_set_config(encoder->crtc,
+			MDP4_DMA_CONFIG_R_BPC(BPC6) |
+			MDP4_DMA_CONFIG_G_BPC(BPC6) |
+			MDP4_DMA_CONFIG_B_BPC(BPC6) |
+			MDP4_DMA_CONFIG_PACK_ALIGN_MSB |
+			MDP4_DMA_CONFIG_PACK(0x21) |
+			MDP4_DMA_CONFIG_DEFLKR_EN |
+			MDP4_DMA_CONFIG_DITHER_EN);
+	mdp4_crtc_set_intf(encoder->crtc, INTF_LCDC_DTV, 0);
+	mdp4_lcdc_encoder_dpms(encoder, DRM_MODE_DPMS_ON);
+}
+
+static const struct drm_encoder_helper_funcs mdp4_lcdc_encoder_helper_funcs = {
+	.dpms = mdp4_lcdc_encoder_dpms,
+	.mode_fixup = mdp4_lcdc_encoder_mode_fixup,
+	.mode_set = mdp4_lcdc_encoder_mode_set,
+	.prepare = mdp4_lcdc_encoder_prepare,
+	.commit = mdp4_lcdc_encoder_commit,
+};
+
+long mdp4_lcdc_round_pixclk(struct drm_encoder *encoder, unsigned long rate)
+{
+	struct mdp4_lcdc_encoder *mdp4_lcdc_encoder =
+			to_mdp4_lcdc_encoder(encoder);
+	return clk_round_rate(mdp4_lcdc_encoder->lcdc_clk, rate);
+}
+
+/* initialize encoder */
+struct drm_encoder *mdp4_lcdc_encoder_init(struct drm_device *dev,
+		struct drm_panel *panel)
+{
+	struct drm_encoder *encoder = NULL;
+	struct mdp4_lcdc_encoder *mdp4_lcdc_encoder;
+	struct regulator *reg;
+	int ret;
+
+	mdp4_lcdc_encoder = kzalloc(sizeof(*mdp4_lcdc_encoder), GFP_KERNEL);
+	if (!mdp4_lcdc_encoder) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	mdp4_lcdc_encoder->panel = panel;
+
+	encoder = &mdp4_lcdc_encoder->base;
+
+	drm_encoder_init(dev, encoder, &mdp4_lcdc_encoder_funcs,
+			 DRM_MODE_ENCODER_LVDS);
+	drm_encoder_helper_add(encoder, &mdp4_lcdc_encoder_helper_funcs);
+
+	/* TODO: do we need different pll in other cases? */
+	mdp4_lcdc_encoder->lcdc_clk = mpd4_lvds_pll_init(dev);
+	if (IS_ERR(mdp4_lcdc_encoder->lcdc_clk)) {
+		dev_err(dev->dev, "failed to get lvds_clk\n");
+		ret = PTR_ERR(mdp4_lcdc_encoder->lcdc_clk);
+		goto fail;
+	}
+
+	/* TODO: different regulators in other cases? */
+	reg = devm_regulator_get(dev->dev, "lvds-vccs-3p3v");
+	if (IS_ERR(reg)) {
+		ret = PTR_ERR(reg);
+		dev_err(dev->dev, "failed to get lvds-vccs-3p3v: %d\n", ret);
+		goto fail;
+	}
+	mdp4_lcdc_encoder->regs[0] = reg;
+
+	reg = devm_regulator_get(dev->dev, "lvds-pll-vdda");
+	if (IS_ERR(reg)) {
+		ret = PTR_ERR(reg);
+		dev_err(dev->dev, "failed to get lvds-pll-vdda: %d\n", ret);
+		goto fail;
+	}
+	mdp4_lcdc_encoder->regs[1] = reg;
+
+	reg = devm_regulator_get(dev->dev, "lvds-vdda");
+	if (IS_ERR(reg)) {
+		ret = PTR_ERR(reg);
+		dev_err(dev->dev, "failed to get lvds-vdda: %d\n", ret);
+		goto fail;
+	}
+	mdp4_lcdc_encoder->regs[2] = reg;
+
+	bs_init(mdp4_lcdc_encoder);
+
+	return encoder;
+
+fail:
+	if (encoder)
+		mdp4_lcdc_encoder_destroy(encoder);
+
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
new file mode 100644
index 000000000000..310034688c15
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_connector.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2014 Red Hat
+ * Author: Rob Clark <robdclark@gmail.com>
+ * Author: Vinay Simha <vinaysimha@inforcecomputing.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/gpio.h>
+
+#include "mdp4_kms.h"
+
+struct mdp4_lvds_connector {
+	struct drm_connector base;
+	struct drm_encoder *encoder;
+	struct drm_panel *panel;
+};
+#define to_mdp4_lvds_connector(x) container_of(x, struct mdp4_lvds_connector, base)
+
+static enum drm_connector_status mdp4_lvds_connector_detect(
+		struct drm_connector *connector, bool force)
+{
+	struct mdp4_lvds_connector *mdp4_lvds_connector =
+			to_mdp4_lvds_connector(connector);
+
+	return mdp4_lvds_connector->panel ?
+			connector_status_connected :
+			connector_status_disconnected;
+}
+
+static void mdp4_lvds_connector_destroy(struct drm_connector *connector)
+{
+	struct mdp4_lvds_connector *mdp4_lvds_connector =
+			to_mdp4_lvds_connector(connector);
+	struct drm_panel *panel = mdp4_lvds_connector->panel;
+
+	if (panel)
+		drm_panel_detach(panel);
+
+	drm_connector_unregister(connector);
+	drm_connector_cleanup(connector);
+
+	kfree(mdp4_lvds_connector);
+}
+
+static int mdp4_lvds_connector_get_modes(struct drm_connector *connector)
+{
+	struct mdp4_lvds_connector *mdp4_lvds_connector =
+			to_mdp4_lvds_connector(connector);
+	struct drm_panel *panel = mdp4_lvds_connector->panel;
+	int ret = 0;
+
+	if (panel)
+		ret = panel->funcs->get_modes(panel);
+
+	return ret;
+}
+
+static int mdp4_lvds_connector_mode_valid(struct drm_connector *connector,
+				 struct drm_display_mode *mode)
+{
+	struct mdp4_lvds_connector *mdp4_lvds_connector =
+			to_mdp4_lvds_connector(connector);
+	struct drm_encoder *encoder = mdp4_lvds_connector->encoder;
+	long actual, requested;
+
+	requested = 1000 * mode->clock;
+	actual = mdp4_lcdc_round_pixclk(encoder, requested);
+
+	DBG("requested=%ld, actual=%ld", requested, actual);
+
+	if (actual != requested)
+		return MODE_CLOCK_RANGE;
+
+	return MODE_OK;
+}
+
+static struct drm_encoder *
+mdp4_lvds_connector_best_encoder(struct drm_connector *connector)
+{
+	struct mdp4_lvds_connector *mdp4_lvds_connector =
+			to_mdp4_lvds_connector(connector);
+	return mdp4_lvds_connector->encoder;
+}
+
+static const struct drm_connector_funcs mdp4_lvds_connector_funcs = {
+	.dpms = drm_helper_connector_dpms,
+	.detect = mdp4_lvds_connector_detect,
+	.fill_modes = drm_helper_probe_single_connector_modes,
+	.destroy = mdp4_lvds_connector_destroy,
+};
+
+static const struct drm_connector_helper_funcs mdp4_lvds_connector_helper_funcs = {
+	.get_modes = mdp4_lvds_connector_get_modes,
+	.mode_valid = mdp4_lvds_connector_mode_valid,
+	.best_encoder = mdp4_lvds_connector_best_encoder,
+};
+
+/* initialize connector */
+struct drm_connector *mdp4_lvds_connector_init(struct drm_device *dev,
+		struct drm_panel *panel, struct drm_encoder *encoder)
+{
+	struct drm_connector *connector = NULL;
+	struct mdp4_lvds_connector *mdp4_lvds_connector;
+	int ret;
+
+	mdp4_lvds_connector = kzalloc(sizeof(*mdp4_lvds_connector), GFP_KERNEL);
+	if (!mdp4_lvds_connector) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	mdp4_lvds_connector->encoder = encoder;
+	mdp4_lvds_connector->panel = panel;
+
+	connector = &mdp4_lvds_connector->base;
+
+	drm_connector_init(dev, connector, &mdp4_lvds_connector_funcs,
+			DRM_MODE_CONNECTOR_LVDS);
+	drm_connector_helper_add(connector, &mdp4_lvds_connector_helper_funcs);
+
+	connector->polled = 0;
+
+	connector->interlace_allowed = 0;
+	connector->doublescan_allowed = 0;
+
+	drm_connector_register(connector);
+
+	drm_mode_connector_attach_encoder(connector, encoder);
+
+	if (panel)
+		drm_panel_attach(panel, connector);
+
+	return connector;
+
+fail:
+	if (connector)
+		mdp4_lvds_connector_destroy(connector);
+
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_pll.c b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_pll.c
new file mode 100644
index 000000000000..ce4245971673
--- /dev/null
+++ b/drivers/gpu/drm/msm/mdp/mdp4/mdp4_lvds_pll.c
@@ -0,0 +1,172 @@
+/*
+ * Copyright (C) 2014 Red Hat
+ * Author: Rob Clark <robdclark@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+
+#include "mdp4_kms.h"
+
+struct mdp4_lvds_pll {
+	struct clk_hw pll_hw;
+	struct drm_device *dev;
+	unsigned long pixclk;
+};
+#define to_mdp4_lvds_pll(x) container_of(x, struct mdp4_lvds_pll, pll_hw)
+
+static struct mdp4_kms *get_kms(struct mdp4_lvds_pll *lvds_pll)
+{
+	struct msm_drm_private *priv = lvds_pll->dev->dev_private;
+	return to_mdp4_kms(to_mdp_kms(priv->kms));
+}
+
+struct pll_rate {
+	unsigned long rate;
+	struct {
+		uint32_t val;
+		uint32_t reg;
+	} conf[32];
+};
+
+/* NOTE: keep sorted highest freq to lowest: */
+static const struct pll_rate freqtbl[] = {
+	{ 72000000, {
+		{ 0x8f, REG_MDP4_LVDS_PHY_PLL_CTRL_1 },
+		{ 0x30, REG_MDP4_LVDS_PHY_PLL_CTRL_2 },
+		{ 0xc6, REG_MDP4_LVDS_PHY_PLL_CTRL_3 },
+		{ 0x10, REG_MDP4_LVDS_PHY_PLL_CTRL_5 },
+		{ 0x07, REG_MDP4_LVDS_PHY_PLL_CTRL_6 },
+		{ 0x62, REG_MDP4_LVDS_PHY_PLL_CTRL_7 },
+		{ 0x41, REG_MDP4_LVDS_PHY_PLL_CTRL_8 },
+		{ 0x0d, REG_MDP4_LVDS_PHY_PLL_CTRL_9 },
+		{ 0, 0 } }
+	},
+};
+
+static const struct pll_rate *find_rate(unsigned long rate)
+{
+	int i;
+	for (i = 1; i < ARRAY_SIZE(freqtbl); i++)
+		if (rate > freqtbl[i].rate)
+			return &freqtbl[i-1];
+	return &freqtbl[i-1];
+}
+
+static int mpd4_lvds_pll_enable(struct clk_hw *hw)
+{
+	struct mdp4_lvds_pll *lvds_pll = to_mdp4_lvds_pll(hw);
+	struct mdp4_kms *mdp4_kms = get_kms(lvds_pll);
+	const struct pll_rate *pll_rate = find_rate(lvds_pll->pixclk);
+	int i;
+
+	DBG("pixclk=%lu (%lu)", lvds_pll->pixclk, pll_rate->rate);
+
+	if (WARN_ON(!pll_rate))
+		return -EINVAL;
+
+	mdp4_write(mdp4_kms, REG_MDP4_LCDC_LVDS_PHY_RESET, 0x33);
+
+	for (i = 0; pll_rate->conf[i].reg; i++)
+		mdp4_write(mdp4_kms, pll_rate->conf[i].reg, pll_rate->conf[i].val);
+
+	mdp4_write(mdp4_kms, REG_MDP4_LVDS_PHY_PLL_CTRL_0, 0x01);
+
+	/* Wait until LVDS PLL is locked and ready */
+	while (!mdp4_read(mdp4_kms, REG_MDP4_LVDS_PHY_PLL_LOCKED))
+		cpu_relax();
+
+	return 0;
+}
+
+static void mpd4_lvds_pll_disable(struct clk_hw *hw)
+{
+	struct mdp4_lvds_pll *lvds_pll = to_mdp4_lvds_pll(hw);
+	struct mdp4_kms *mdp4_kms = get_kms(lvds_pll);
+
+	DBG("");
+
+	mdp4_write(mdp4_kms, REG_MDP4_LVDS_PHY_CFG0, 0x0);
+	mdp4_write(mdp4_kms, REG_MDP4_LVDS_PHY_PLL_CTRL_0, 0x0);
+}
+
+static unsigned long mpd4_lvds_pll_recalc_rate(struct clk_hw *hw,
+				unsigned long parent_rate)
+{
+	struct mdp4_lvds_pll *lvds_pll = to_mdp4_lvds_pll(hw);
+	return lvds_pll->pixclk;
+}
+
+static long mpd4_lvds_pll_round_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long *parent_rate)
+{
+	const struct pll_rate *pll_rate = find_rate(rate);
+	return pll_rate->rate;
+}
+
+static int mpd4_lvds_pll_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct mdp4_lvds_pll *lvds_pll = to_mdp4_lvds_pll(hw);
+	lvds_pll->pixclk = rate;
+	return 0;
+}
+
+
+static const struct clk_ops mpd4_lvds_pll_ops = {
+	.enable = mpd4_lvds_pll_enable,
+	.disable = mpd4_lvds_pll_disable,
+	.recalc_rate = mpd4_lvds_pll_recalc_rate,
+	.round_rate = mpd4_lvds_pll_round_rate,
+	.set_rate = mpd4_lvds_pll_set_rate,
+};
+
+static const char *mpd4_lvds_pll_parents[] = {
+	"pxo",
+};
+
+static struct clk_init_data pll_init = {
+	.name = "mpd4_lvds_pll",
+	.ops = &mpd4_lvds_pll_ops,
+	.parent_names = mpd4_lvds_pll_parents,
+	.num_parents = ARRAY_SIZE(mpd4_lvds_pll_parents),
+};
+
+struct clk *mpd4_lvds_pll_init(struct drm_device *dev)
+{
+	struct mdp4_lvds_pll *lvds_pll;
+	struct clk *clk;
+	int ret;
+
+	lvds_pll = devm_kzalloc(dev->dev, sizeof(*lvds_pll), GFP_KERNEL);
+	if (!lvds_pll) {
+		ret = -ENOMEM;
+		goto fail;
+	}
+
+	lvds_pll->dev = dev;
+
+	lvds_pll->pll_hw.init = &pll_init;
+	clk = devm_clk_register(dev->dev, &lvds_pll->pll_hw);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		goto fail;
+	}
+
+	return clk;
+
+fail:
+	return ERR_PTR(ret);
+}
diff --git a/drivers/gpu/drm/msm/msm_drv.c b/drivers/gpu/drm/msm/msm_drv.c
index b447c01ad89c..b67ef5985125 100644
--- a/drivers/gpu/drm/msm/msm_drv.c
+++ b/drivers/gpu/drm/msm/msm_drv.c
@@ -52,7 +52,7 @@ module_param(reglog, bool, 0600);
 #define reglog 0
 #endif
 
-static char *vram;
+static char *vram = "16m";
 MODULE_PARM_DESC(vram, "Configure VRAM size (for devices without IOMMU/GPUMMU");
 module_param(vram, charp, 0);
 
@@ -280,7 +280,7 @@ static int msm_load(struct drm_device *dev, unsigned long flags)
 	dev->mode_config.max_height = 2048;
 	dev->mode_config.funcs = &mode_config_funcs;
 
-	ret = drm_vblank_init(dev, 1);
+	ret = drm_vblank_init(dev, priv->num_crtcs);
 	if (ret < 0) {
 		dev_err(dev->dev, "failed to initialize vblank\n");
 		goto fail;
@@ -315,39 +315,12 @@ static void load_gpu(struct drm_device *dev)
 {
 	static DEFINE_MUTEX(init_lock);
 	struct msm_drm_private *priv = dev->dev_private;
-	struct msm_gpu *gpu;
 
 	mutex_lock(&init_lock);
 
-	if (priv->gpu)
-		goto out;
-
-	gpu = a3xx_gpu_init(dev);
-	if (IS_ERR(gpu)) {
-		dev_warn(dev->dev, "failed to load a3xx gpu\n");
-		gpu = NULL;
-		/* not fatal */
-	}
-
-	if (gpu) {
-		int ret;
-		mutex_lock(&dev->struct_mutex);
-		gpu->funcs->pm_resume(gpu);
-		mutex_unlock(&dev->struct_mutex);
-		ret = gpu->funcs->hw_init(gpu);
-		if (ret) {
-			dev_err(dev->dev, "gpu hw init failed: %d\n", ret);
-			gpu->funcs->destroy(gpu);
-			gpu = NULL;
-		} else {
-			/* give inactive pm a chance to kick in: */
-			msm_gpu_retire(gpu);
-		}
-	}
-
-	priv->gpu = gpu;
+	if (!priv->gpu)
+		priv->gpu = adreno_load_gpu(dev);
 
-out:
 	mutex_unlock(&init_lock);
 }
 
@@ -836,6 +809,7 @@ static struct drm_driver msm_driver = {
 	.open               = msm_open,
 	.preclose           = msm_preclose,
 	.lastclose          = msm_lastclose,
+	.set_busid          = drm_platform_set_busid,
 	.irq_handler        = msm_irq,
 	.irq_preinstall     = msm_irq_preinstall,
 	.irq_postinstall    = msm_irq_postinstall,
@@ -974,12 +948,11 @@ static int msm_pdev_probe(struct platform_device *pdev)
 
 	for (i = 0; i < ARRAY_SIZE(devnames); i++) {
 		struct device *dev;
-		int ret;
 
 		dev = bus_find_device_by_name(&platform_bus_type,
 				NULL, devnames[i]);
 		if (!dev) {
-			dev_info(master, "still waiting for %s\n", devnames[i]);
+			dev_info(&pdev->dev, "still waiting for %s\n", devnames[i]);
 			return -EPROBE_DEFER;
 		}
 
@@ -1026,7 +999,7 @@ static int __init msm_drm_register(void)
 {
 	DBG("init");
 	hdmi_register();
-	a3xx_register();
+	adreno_register();
 	return platform_driver_register(&msm_platform_driver);
 }
 
@@ -1035,7 +1008,7 @@ static void __exit msm_drm_unregister(void)
 	DBG("fini");
 	platform_driver_unregister(&msm_platform_driver);
 	hdmi_unregister();
-	a3xx_unregister();
+	adreno_unregister();
 }
 
 module_init(msm_drm_register);
diff --git a/drivers/gpu/drm/msm/msm_drv.h b/drivers/gpu/drm/msm/msm_drv.h
index 8a2c5fd0893e..67f9d0a2332c 100644
--- a/drivers/gpu/drm/msm/msm_drv.h
+++ b/drivers/gpu/drm/msm/msm_drv.h
@@ -51,6 +51,7 @@ static inline struct device *msm_iommu_get_ctx(const char *ctx_name)
 #include <drm/drm_crtc_helper.h>
 #include <drm/drm_fb_helper.h>
 #include <drm/msm_drm.h>
+#include <drm/drm_gem.h>
 
 struct msm_kms;
 struct msm_gpu;
@@ -170,7 +171,7 @@ struct sg_table *msm_gem_prime_get_sg_table(struct drm_gem_object *obj);
 void *msm_gem_prime_vmap(struct drm_gem_object *obj);
 void msm_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr);
 struct drm_gem_object *msm_gem_prime_import_sg_table(struct drm_device *dev,
-		size_t size, struct sg_table *sg);
+		struct dma_buf_attachment *attach, struct sg_table *sg);
 int msm_gem_prime_pin(struct drm_gem_object *obj);
 void msm_gem_prime_unpin(struct drm_gem_object *obj);
 void *msm_gem_vaddr_locked(struct drm_gem_object *obj);
diff --git a/drivers/gpu/drm/msm/msm_fbdev.c b/drivers/gpu/drm/msm/msm_fbdev.c
index 9c5221ce391a..ab5bfd2d0ebf 100644
--- a/drivers/gpu/drm/msm/msm_fbdev.c
+++ b/drivers/gpu/drm/msm/msm_fbdev.c
@@ -143,7 +143,7 @@ static int msm_fbdev_create(struct drm_fb_helper *helper,
 	ret = msm_gem_get_iova_locked(fbdev->bo, 0, &paddr);
 	if (ret) {
 		dev_err(dev->dev, "failed to get buffer obj iova: %d\n", ret);
-		goto fail;
+		goto fail_unlock;
 	}
 
 	fbi = framebuffer_alloc(0, dev->dev);
diff --git a/drivers/gpu/drm/msm/msm_gem_prime.c b/drivers/gpu/drm/msm/msm_gem_prime.c
index d48f9fc5129b..ad772fe36115 100644
--- a/drivers/gpu/drm/msm/msm_gem_prime.c
+++ b/drivers/gpu/drm/msm/msm_gem_prime.c
@@ -18,6 +18,7 @@
 #include "msm_drv.h"
 #include "msm_gem.h"
 
+#include <linux/dma-buf.h>
 
 struct sg_table *msm_gem_prime_get_sg_table(struct drm_gem_object *obj)
 {
@@ -37,9 +38,9 @@ void msm_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
 }
 
 struct drm_gem_object *msm_gem_prime_import_sg_table(struct drm_device *dev,
-		size_t size, struct sg_table *sg)
+		struct dma_buf_attachment *attach, struct sg_table *sg)
 {
-	return msm_gem_import(dev, size, sg);
+	return msm_gem_import(dev, attach->dmabuf->size, sg);
 }
 
 int msm_gem_prime_pin(struct drm_gem_object *obj)
diff --git a/drivers/gpu/drm/msm/msm_gpu.h b/drivers/gpu/drm/msm/msm_gpu.h
index 9b579b792840..fd1e4b4a6d40 100644
--- a/drivers/gpu/drm/msm/msm_gpu.h
+++ b/drivers/gpu/drm/msm/msm_gpu.h
@@ -166,8 +166,8 @@ int msm_gpu_init(struct drm_device *drm, struct platform_device *pdev,
 		const char *name, const char *ioname, const char *irqname, int ringsz);
 void msm_gpu_cleanup(struct msm_gpu *gpu);
 
-struct msm_gpu *a3xx_gpu_init(struct drm_device *dev);
-void __init a3xx_register(void);
-void __exit a3xx_unregister(void);
+struct msm_gpu *adreno_load_gpu(struct drm_device *dev);
+void __init adreno_register(void);
+void __exit adreno_unregister(void);
 
 #endif /* __MSM_GPU_H__ */
diff --git a/drivers/gpu/drm/msm/msm_iommu.c b/drivers/gpu/drm/msm/msm_iommu.c
index 099af483fdf0..7acdaa5688b7 100644
--- a/drivers/gpu/drm/msm/msm_iommu.c
+++ b/drivers/gpu/drm/msm/msm_iommu.c
@@ -27,8 +27,8 @@ struct msm_iommu {
 static int msm_fault_handler(struct iommu_domain *iommu, struct device *dev,
 		unsigned long iova, int flags, void *arg)
 {
-	DBG("*** fault: iova=%08lx, flags=%d", iova, flags);
-	return -ENOSYS;
+	pr_warn_ratelimited("*** fault: iova=%08lx, flags=%d\n", iova, flags);
+	return 0;
 }
 
 static int msm_iommu_attach(struct msm_mmu *mmu, const char **names, int cnt)
diff --git a/drivers/gpu/drm/nouveau/Makefile b/drivers/gpu/drm/nouveau/Makefile
index f5d7f7ce4bc6..12c24c8abf7f 100644
--- a/drivers/gpu/drm/nouveau/Makefile
+++ b/drivers/gpu/drm/nouveau/Makefile
@@ -38,6 +38,7 @@ nouveau-y += core/subdev/bios/dcb.o
 nouveau-y += core/subdev/bios/disp.o
 nouveau-y += core/subdev/bios/dp.o
 nouveau-y += core/subdev/bios/extdev.o
+nouveau-y += core/subdev/bios/fan.o
 nouveau-y += core/subdev/bios/gpio.o
 nouveau-y += core/subdev/bios/i2c.o
 nouveau-y += core/subdev/bios/init.o
@@ -51,6 +52,8 @@ nouveau-y += core/subdev/bios/therm.o
 nouveau-y += core/subdev/bios/vmap.o
 nouveau-y += core/subdev/bios/volt.o
 nouveau-y += core/subdev/bios/xpio.o
+nouveau-y += core/subdev/bios/M0205.o
+nouveau-y += core/subdev/bios/M0209.o
 nouveau-y += core/subdev/bios/P0260.o
 nouveau-y += core/subdev/bus/hwsq.o
 nouveau-y += core/subdev/bus/nv04.o
@@ -124,12 +127,17 @@ nouveau-y += core/subdev/fb/ramnvc0.o
 nouveau-y += core/subdev/fb/ramnve0.o
 nouveau-y += core/subdev/fb/ramgk20a.o
 nouveau-y += core/subdev/fb/ramgm107.o
+nouveau-y += core/subdev/fb/sddr2.o
 nouveau-y += core/subdev/fb/sddr3.o
 nouveau-y += core/subdev/fb/gddr5.o
+nouveau-y += core/subdev/fuse/base.o
+nouveau-y += core/subdev/fuse/g80.o
+nouveau-y += core/subdev/fuse/gf100.o
+nouveau-y += core/subdev/fuse/gm107.o
 nouveau-y += core/subdev/gpio/base.o
 nouveau-y += core/subdev/gpio/nv10.o
 nouveau-y += core/subdev/gpio/nv50.o
-nouveau-y += core/subdev/gpio/nv92.o
+nouveau-y += core/subdev/gpio/nv94.o
 nouveau-y += core/subdev/gpio/nvd0.o
 nouveau-y += core/subdev/gpio/nve0.o
 nouveau-y += core/subdev/i2c/base.o
@@ -190,6 +198,7 @@ nouveau-y += core/subdev/therm/nv50.o
 nouveau-y += core/subdev/therm/nv84.o
 nouveau-y += core/subdev/therm/nva3.o
 nouveau-y += core/subdev/therm/nvd0.o
+nouveau-y += core/subdev/therm/gm107.o
 nouveau-y += core/subdev/timer/base.o
 nouveau-y += core/subdev/timer/nv04.o
 nouveau-y += core/subdev/timer/gk20a.o
@@ -252,6 +261,7 @@ nouveau-y += core/engine/disp/hdanvd0.o
 nouveau-y += core/engine/disp/hdminv84.o
 nouveau-y += core/engine/disp/hdminva3.o
 nouveau-y += core/engine/disp/hdminvd0.o
+nouveau-y += core/engine/disp/hdminve0.o
 nouveau-y += core/engine/disp/piornv50.o
 nouveau-y += core/engine/disp/sornv50.o
 nouveau-y += core/engine/disp/sornv94.o
diff --git a/drivers/gpu/drm/nouveau/core/core/client.c b/drivers/gpu/drm/nouveau/core/core/client.c
index 68bf06768123..e962433294c3 100644
--- a/drivers/gpu/drm/nouveau/core/core/client.c
+++ b/drivers/gpu/drm/nouveau/core/core/client.c
@@ -91,9 +91,10 @@ nvkm_client_notify_del(struct nouveau_client *client, int index)
 }
 
 int
-nvkm_client_notify_new(struct nouveau_client *client,
+nvkm_client_notify_new(struct nouveau_object *object,
 		       struct nvkm_event *event, void *data, u32 size)
 {
+	struct nouveau_client *client = nouveau_client(object);
 	struct nvkm_client_notify *notify;
 	union {
 		struct nvif_notify_req_v0 v0;
@@ -127,8 +128,8 @@ nvkm_client_notify_new(struct nouveau_client *client,
 	}
 
 	if (ret == 0) {
-		ret = nvkm_notify_init(event, nvkm_client_notify, false,
-				       data, size, reply, &notify->n);
+		ret = nvkm_notify_init(object, event, nvkm_client_notify,
+				       false, data, size, reply, &notify->n);
 		if (ret == 0) {
 			client->notify[index] = notify;
 			notify->client = client;
diff --git a/drivers/gpu/drm/nouveau/core/core/event.c b/drivers/gpu/drm/nouveau/core/core/event.c
index 0540a48c5678..ff2b434b3db4 100644
--- a/drivers/gpu/drm/nouveau/core/core/event.c
+++ b/drivers/gpu/drm/nouveau/core/core/event.c
@@ -20,7 +20,7 @@
  * OTHER DEALINGS IN THE SOFTWARE.
  */
 
-#include <core/os.h>
+#include <core/object.h>
 #include <core/event.h>
 
 void
diff --git a/drivers/gpu/drm/nouveau/core/core/gpuobj.c b/drivers/gpu/drm/nouveau/core/core/gpuobj.c
index 560b2214cf1c..daee87702502 100644
--- a/drivers/gpu/drm/nouveau/core/core/gpuobj.c
+++ b/drivers/gpu/drm/nouveau/core/core/gpuobj.c
@@ -115,7 +115,7 @@ nouveau_gpuobj_create_(struct nouveau_object *parent,
 	gpuobj->size = size;
 
 	if (heap) {
-		ret = nouveau_mm_head(heap, 1, size, size,
+		ret = nouveau_mm_head(heap, 0, 1, size, size,
 				      max(align, (u32)1), &gpuobj->node);
 		if (ret)
 			return ret;
diff --git a/drivers/gpu/drm/nouveau/core/core/ioctl.c b/drivers/gpu/drm/nouveau/core/core/ioctl.c
index f7e19bfb489c..692aa92dd850 100644
--- a/drivers/gpu/drm/nouveau/core/core/ioctl.c
+++ b/drivers/gpu/drm/nouveau/core/core/ioctl.c
@@ -349,7 +349,6 @@ nvkm_ioctl_unmap(struct nouveau_handle *handle, void *data, u32 size)
 static int
 nvkm_ioctl_ntfy_new(struct nouveau_handle *handle, void *data, u32 size)
 {
-	struct nouveau_client *client = nouveau_client(handle->object);
 	struct nouveau_object *object = handle->object;
 	struct nouveau_ofuncs *ofuncs = object->oclass->ofuncs;
 	union {
@@ -365,7 +364,7 @@ nvkm_ioctl_ntfy_new(struct nouveau_handle *handle, void *data, u32 size)
 		if (ret = -ENODEV, ofuncs->ntfy)
 			ret = ofuncs->ntfy(object, args->v0.event, &event);
 		if (ret == 0) {
-			ret = nvkm_client_notify_new(client, event, data, size);
+			ret = nvkm_client_notify_new(object, event, data, size);
 			if (ret >= 0) {
 				args->v0.index = ret;
 				ret = 0;
diff --git a/drivers/gpu/drm/nouveau/core/core/mm.c b/drivers/gpu/drm/nouveau/core/core/mm.c
index 7a4e0891c5f8..b4f5db66d5b5 100644
--- a/drivers/gpu/drm/nouveau/core/core/mm.c
+++ b/drivers/gpu/drm/nouveau/core/core/mm.c
@@ -28,6 +28,24 @@
 #define node(root, dir) ((root)->nl_entry.dir == &mm->nodes) ? NULL : \
 	list_entry((root)->nl_entry.dir, struct nouveau_mm_node, nl_entry)
 
+static void
+nouveau_mm_dump(struct nouveau_mm *mm, const char *header)
+{
+	struct nouveau_mm_node *node;
+
+	printk(KERN_ERR "nouveau: %s\n", header);
+	printk(KERN_ERR "nouveau: node list:\n");
+	list_for_each_entry(node, &mm->nodes, nl_entry) {
+		printk(KERN_ERR "nouveau: \t%08x %08x %d\n",
+		       node->offset, node->length, node->type);
+	}
+	printk(KERN_ERR "nouveau: free list:\n");
+	list_for_each_entry(node, &mm->free, fl_entry) {
+		printk(KERN_ERR "nouveau: \t%08x %08x %d\n",
+		       node->offset, node->length, node->type);
+	}
+}
+
 void
 nouveau_mm_free(struct nouveau_mm *mm, struct nouveau_mm_node **pthis)
 {
@@ -37,29 +55,29 @@ nouveau_mm_free(struct nouveau_mm *mm, struct nouveau_mm_node **pthis)
 		struct nouveau_mm_node *prev = node(this, prev);
 		struct nouveau_mm_node *next = node(this, next);
 
-		if (prev && prev->type == 0) {
+		if (prev && prev->type == NVKM_MM_TYPE_NONE) {
 			prev->length += this->length;
 			list_del(&this->nl_entry);
 			kfree(this); this = prev;
 		}
 
-		if (next && next->type == 0) {
+		if (next && next->type == NVKM_MM_TYPE_NONE) {
 			next->offset  = this->offset;
 			next->length += this->length;
-			if (this->type == 0)
+			if (this->type == NVKM_MM_TYPE_NONE)
 				list_del(&this->fl_entry);
 			list_del(&this->nl_entry);
 			kfree(this); this = NULL;
 		}
 
-		if (this && this->type != 0) {
+		if (this && this->type != NVKM_MM_TYPE_NONE) {
 			list_for_each_entry(prev, &mm->free, fl_entry) {
 				if (this->offset < prev->offset)
 					break;
 			}
 
 			list_add_tail(&this->fl_entry, &prev->fl_entry);
-			this->type = 0;
+			this->type = NVKM_MM_TYPE_NONE;
 		}
 	}
 
@@ -80,27 +98,32 @@ region_head(struct nouveau_mm *mm, struct nouveau_mm_node *a, u32 size)
 
 	b->offset = a->offset;
 	b->length = size;
+	b->heap   = a->heap;
 	b->type   = a->type;
 	a->offset += size;
 	a->length -= size;
 	list_add_tail(&b->nl_entry, &a->nl_entry);
-	if (b->type == 0)
+	if (b->type == NVKM_MM_TYPE_NONE)
 		list_add_tail(&b->fl_entry, &a->fl_entry);
 	return b;
 }
 
 int
-nouveau_mm_head(struct nouveau_mm *mm, u8 type, u32 size_max, u32 size_min,
-		u32 align, struct nouveau_mm_node **pnode)
+nouveau_mm_head(struct nouveau_mm *mm, u8 heap, u8 type, u32 size_max,
+		u32 size_min, u32 align, struct nouveau_mm_node **pnode)
 {
 	struct nouveau_mm_node *prev, *this, *next;
 	u32 mask = align - 1;
 	u32 splitoff;
 	u32 s, e;
 
-	BUG_ON(!type);
+	BUG_ON(type == NVKM_MM_TYPE_NONE || type == NVKM_MM_TYPE_HOLE);
 
 	list_for_each_entry(this, &mm->free, fl_entry) {
+		if (unlikely(heap != NVKM_MM_HEAP_ANY)) {
+			if (this->heap != heap)
+				continue;
+		}
 		e = this->offset + this->length;
 		s = this->offset;
 
@@ -149,27 +172,32 @@ region_tail(struct nouveau_mm *mm, struct nouveau_mm_node *a, u32 size)
 	a->length -= size;
 	b->offset  = a->offset + a->length;
 	b->length  = size;
+	b->heap    = a->heap;
 	b->type    = a->type;
 
 	list_add(&b->nl_entry, &a->nl_entry);
-	if (b->type == 0)
+	if (b->type == NVKM_MM_TYPE_NONE)
 		list_add(&b->fl_entry, &a->fl_entry);
 	return b;
 }
 
 int
-nouveau_mm_tail(struct nouveau_mm *mm, u8 type, u32 size_max, u32 size_min,
-		u32 align, struct nouveau_mm_node **pnode)
+nouveau_mm_tail(struct nouveau_mm *mm, u8 heap, u8 type, u32 size_max,
+		u32 size_min, u32 align, struct nouveau_mm_node **pnode)
 {
 	struct nouveau_mm_node *prev, *this, *next;
 	u32 mask = align - 1;
 
-	BUG_ON(!type);
+	BUG_ON(type == NVKM_MM_TYPE_NONE || type == NVKM_MM_TYPE_HOLE);
 
 	list_for_each_entry_reverse(this, &mm->free, fl_entry) {
 		u32 e = this->offset + this->length;
 		u32 s = this->offset;
 		u32 c = 0, a;
+		if (unlikely(heap != NVKM_MM_HEAP_ANY)) {
+			if (this->heap != heap)
+				continue;
+		}
 
 		prev = node(this, prev);
 		if (prev && prev->type != type)
@@ -209,9 +237,23 @@ nouveau_mm_tail(struct nouveau_mm *mm, u8 type, u32 size_max, u32 size_min,
 int
 nouveau_mm_init(struct nouveau_mm *mm, u32 offset, u32 length, u32 block)
 {
-	struct nouveau_mm_node *node;
+	struct nouveau_mm_node *node, *prev;
+	u32 next;
 
-	if (block) {
+	if (nouveau_mm_initialised(mm)) {
+		prev = list_last_entry(&mm->nodes, typeof(*node), nl_entry);
+		next = prev->offset + prev->length;
+		if (next != offset) {
+			BUG_ON(next > offset);
+			if (!(node = kzalloc(sizeof(*node), GFP_KERNEL)))
+				return -ENOMEM;
+			node->type   = NVKM_MM_TYPE_HOLE;
+			node->offset = next;
+			node->length = offset - next;
+			list_add_tail(&node->nl_entry, &mm->nodes);
+		}
+		BUG_ON(block != mm->block_size);
+	} else {
 		INIT_LIST_HEAD(&mm->nodes);
 		INIT_LIST_HEAD(&mm->free);
 		mm->block_size = block;
@@ -230,25 +272,32 @@ nouveau_mm_init(struct nouveau_mm *mm, u32 offset, u32 length, u32 block)
 
 	list_add_tail(&node->nl_entry, &mm->nodes);
 	list_add_tail(&node->fl_entry, &mm->free);
-	mm->heap_nodes++;
+	node->heap = ++mm->heap_nodes;
 	return 0;
 }
 
 int
 nouveau_mm_fini(struct nouveau_mm *mm)
 {
-	if (nouveau_mm_initialised(mm)) {
-		struct nouveau_mm_node *node, *heap =
-			list_first_entry(&mm->nodes, typeof(*heap), nl_entry);
-		int nodes = 0;
+	struct nouveau_mm_node *node, *temp;
+	int nodes = 0;
 
-		list_for_each_entry(node, &mm->nodes, nl_entry) {
-			if (WARN_ON(nodes++ == mm->heap_nodes))
+	if (!nouveau_mm_initialised(mm))
+		return 0;
+
+	list_for_each_entry(node, &mm->nodes, nl_entry) {
+		if (node->type != NVKM_MM_TYPE_HOLE) {
+			if (++nodes > mm->heap_nodes) {
+				nouveau_mm_dump(mm, "mm not clean!");
 				return -EBUSY;
+			}
 		}
-
-		kfree(heap);
 	}
 
+	list_for_each_entry_safe(node, temp, &mm->nodes, nl_entry) {
+		list_del(&node->nl_entry);
+		kfree(node);
+	}
+	mm->heap_nodes = 0;
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/core/notify.c b/drivers/gpu/drm/nouveau/core/core/notify.c
index 76adb81bdea2..d1bcde55e9d7 100644
--- a/drivers/gpu/drm/nouveau/core/core/notify.c
+++ b/drivers/gpu/drm/nouveau/core/core/notify.c
@@ -134,14 +134,15 @@ nvkm_notify_fini(struct nvkm_notify *notify)
 }
 
 int
-nvkm_notify_init(struct nvkm_event *event, int (*func)(struct nvkm_notify *),
-		 bool work, void *data, u32 size, u32 reply,
+nvkm_notify_init(struct nouveau_object *object, struct nvkm_event *event,
+		 int (*func)(struct nvkm_notify *), bool work,
+		 void *data, u32 size, u32 reply,
 		 struct nvkm_notify *notify)
 {
 	unsigned long flags;
 	int ret = -ENODEV;
 	if ((notify->event = event), event->refs) {
-		ret = event->func->ctor(data, size, notify);
+		ret = event->func->ctor(object, data, size, notify);
 		if (ret == 0 && (ret = -EINVAL, notify->size == reply)) {
 			notify->flags = 0;
 			notify->block = 1;
diff --git a/drivers/gpu/drm/nouveau/core/core/parent.c b/drivers/gpu/drm/nouveau/core/core/parent.c
index 8701968a9743..30a2911878f8 100644
--- a/drivers/gpu/drm/nouveau/core/core/parent.c
+++ b/drivers/gpu/drm/nouveau/core/core/parent.c
@@ -86,7 +86,7 @@ nouveau_parent_lclass(struct nouveau_object *parent, u32 *lclass, int size)
 	sclass = nv_parent(parent)->sclass;
 	while (sclass) {
 		if (++nr < size)
-			lclass[nr] = sclass->oclass->handle;
+			lclass[nr] = sclass->oclass->handle & 0xffff;
 		sclass = sclass->sclass;
 	}
 
@@ -96,7 +96,7 @@ nouveau_parent_lclass(struct nouveau_object *parent, u32 *lclass, int size)
 		if (engine && (oclass = engine->sclass)) {
 			while (oclass->ofuncs) {
 				if (++nr < size)
-					lclass[nr] = oclass->handle;
+					lclass[nr] = oclass->handle & 0xffff;
 				oclass++;
 			}
 		}
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/base.c b/drivers/gpu/drm/nouveau/core/engine/device/base.c
index 8928f7981d4a..0ef5a5713182 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/base.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/base.c
@@ -505,7 +505,8 @@ nouveau_device_sclass[] = {
 };
 
 static int
-nouveau_device_event_ctor(void *data, u32 size, struct nvkm_notify *notify)
+nouveau_device_event_ctor(struct nouveau_object *object, void *data, u32 size,
+			  struct nvkm_notify *notify)
 {
 	if (!WARN_ON(size != 0)) {
 		notify->size  = 0;
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/gm100.c b/drivers/gpu/drm/nouveau/core/engine/device/gm100.c
index 377ec0b8851e..6295668e29a5 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/gm100.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/gm100.c
@@ -26,6 +26,7 @@
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
 #include <subdev/i2c.h>
+#include <subdev/fuse.h>
 #include <subdev/clock.h>
 #include <subdev/therm.h>
 #include <subdev/mxm.h>
@@ -62,10 +63,9 @@ gm100_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nvd0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gm107_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
-#if 0
-		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
-#endif
+		device->oclass[NVDEV_SUBDEV_THERM  ] = &gm107_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
 		device->oclass[NVDEV_SUBDEV_DEVINIT] =  gm107_devinit_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] =  gk20a_mc_oclass;
@@ -77,8 +77,9 @@ gm100_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_INSTMEM] =  nv50_instmem_oclass;
 		device->oclass[NVDEV_SUBDEV_VM     ] = &nvc0_vmmgr_oclass;
 		device->oclass[NVDEV_SUBDEV_BAR    ] = &nvc0_bar_oclass;
-#if 0
 		device->oclass[NVDEV_SUBDEV_PWR    ] =  nv108_pwr_oclass;
+
+#if 0
 		device->oclass[NVDEV_SUBDEV_VOLT   ] = &nv40_volt_oclass;
 #endif
 		device->oclass[NVDEV_ENGINE_DMAOBJ ] =  nvd0_dmaeng_oclass;
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/nv50.c b/drivers/gpu/drm/nouveau/core/engine/device/nv50.c
index 932f84fae459..96f568d1321b 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nv50.c
@@ -26,6 +26,7 @@
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
 #include <subdev/i2c.h>
+#include <subdev/fuse.h>
 #include <subdev/clock.h>
 #include <subdev/therm.h>
 #include <subdev/mxm.h>
@@ -62,6 +63,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv50_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv50_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv50_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -87,6 +89,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv50_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv84_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -115,6 +118,7 @@ nv50_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv50_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv84_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -141,8 +145,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0x92:
 		device->cname = "G92";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv50_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv50_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv84_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -169,8 +174,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0x94:
 		device->cname = "G94";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv84_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -197,8 +203,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0x96:
 		device->cname = "G96";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv84_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -225,8 +232,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0x98:
 		device->cname = "G98";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv84_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -253,8 +261,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0xa0:
 		device->cname = "G200";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv50_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nv84_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -281,8 +290,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0xaa:
 		device->cname = "MCP77/MCP78";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nvaa_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -309,8 +319,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0xac:
 		device->cname = "MCP79/MCP7A";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] =  nvaa_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nv84_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -337,8 +348,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0xa3:
 		device->cname = "GT215";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nva3_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -367,8 +379,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0xa5:
 		device->cname = "GT216";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nva3_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -396,8 +409,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0xa8:
 		device->cname = "GT218";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nva3_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -425,8 +439,9 @@ nv50_identify(struct nouveau_device *device)
 	case 0xaf:
 		device->cname = "MCP89";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] =  &g80_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nva3_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/nvc0.c b/drivers/gpu/drm/nouveau/core/engine/device/nvc0.c
index b4a2917ce555..cd05677ad4b7 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nvc0.c
@@ -26,6 +26,7 @@
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
 #include <subdev/i2c.h>
+#include <subdev/fuse.h>
 #include <subdev/clock.h>
 #include <subdev/therm.h>
 #include <subdev/mxm.h>
@@ -60,8 +61,9 @@ nvc0_identify(struct nouveau_device *device)
 	case 0xc0:
 		device->cname = "GF100";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -92,8 +94,9 @@ nvc0_identify(struct nouveau_device *device)
 	case 0xc4:
 		device->cname = "GF104";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -124,8 +127,9 @@ nvc0_identify(struct nouveau_device *device)
 	case 0xc3:
 		device->cname = "GF106";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -155,8 +159,9 @@ nvc0_identify(struct nouveau_device *device)
 	case 0xce:
 		device->cname = "GF114";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -187,8 +192,9 @@ nvc0_identify(struct nouveau_device *device)
 	case 0xcf:
 		device->cname = "GF116";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -219,8 +225,9 @@ nvc0_identify(struct nouveau_device *device)
 	case 0xc1:
 		device->cname = "GF108";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -250,8 +257,9 @@ nvc0_identify(struct nouveau_device *device)
 	case 0xc8:
 		device->cname = "GF110";
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
-		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv92_gpio_oclass;
+		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nv94_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nv94_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nva3_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -284,6 +292,7 @@ nvc0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nvd0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nvd0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -315,6 +324,7 @@ nvc0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nvd0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  gf117_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nvc0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
diff --git a/drivers/gpu/drm/nouveau/core/engine/device/nve0.c b/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
index cdf9147f32a1..b1b2e484ecfa 100644
--- a/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/device/nve0.c
@@ -26,6 +26,7 @@
 #include <subdev/bus.h>
 #include <subdev/gpio.h>
 #include <subdev/i2c.h>
+#include <subdev/fuse.h>
 #include <subdev/clock.h>
 #include <subdev/therm.h>
 #include <subdev/mxm.h>
@@ -62,6 +63,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nve0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -95,6 +97,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nve0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -128,6 +131,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nve0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -161,6 +165,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &gk20a_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_MC     ] =  gk20a_mc_oclass;
 		device->oclass[NVDEV_SUBDEV_BUS    ] =  nvc0_bus_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_TIMER  ] = &gk20a_timer_oclass;
 		device->oclass[NVDEV_SUBDEV_FB     ] =  gk20a_fb_oclass;
 		device->oclass[NVDEV_SUBDEV_LTC    ] =  gk104_ltc_oclass;
@@ -180,6 +185,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nve0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -213,6 +219,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nvd0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
@@ -246,6 +253,7 @@ nve0_identify(struct nouveau_device *device)
 		device->oclass[NVDEV_SUBDEV_VBIOS  ] = &nouveau_bios_oclass;
 		device->oclass[NVDEV_SUBDEV_GPIO   ] =  nve0_gpio_oclass;
 		device->oclass[NVDEV_SUBDEV_I2C    ] =  nve0_i2c_oclass;
+		device->oclass[NVDEV_SUBDEV_FUSE   ] = &gf100_fuse_oclass;
 		device->oclass[NVDEV_SUBDEV_CLOCK  ] = &nve0_clock_oclass;
 		device->oclass[NVDEV_SUBDEV_THERM  ] = &nvd0_therm_oclass;
 		device->oclass[NVDEV_SUBDEV_MXM    ] = &nv50_mxm_oclass;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/base.c b/drivers/gpu/drm/nouveau/core/engine/disp/base.c
index 22d55f6cde50..64b84667f3a5 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/base.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/base.c
@@ -32,7 +32,8 @@
 #include "conn.h"
 
 int
-nouveau_disp_vblank_ctor(void *data, u32 size, struct nvkm_notify *notify)
+nouveau_disp_vblank_ctor(struct nouveau_object *object, void *data, u32 size,
+			 struct nvkm_notify *notify)
 {
 	struct nouveau_disp *disp =
 		container_of(notify->event, typeof(*disp), vblank);
@@ -61,7 +62,8 @@ nouveau_disp_vblank(struct nouveau_disp *disp, int head)
 }
 
 static int
-nouveau_disp_hpd_ctor(void *data, u32 size, struct nvkm_notify *notify)
+nouveau_disp_hpd_ctor(struct nouveau_object *object, void *data, u32 size,
+		      struct nvkm_notify *notify)
 {
 	struct nouveau_disp *disp =
 		container_of(notify->event, typeof(*disp), hpd);
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/conn.c b/drivers/gpu/drm/nouveau/core/engine/disp/conn.c
index 3d1070228977..1496b567dd4a 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/conn.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/conn.c
@@ -126,8 +126,8 @@ nvkm_connector_create_(struct nouveau_object *parent,
 			return 0;
 		}
 
-		ret = nvkm_notify_init(&gpio->event, nvkm_connector_hpd, true,
-				       &(struct nvkm_gpio_ntfy_req) {
+		ret = nvkm_notify_init(NULL, &gpio->event, nvkm_connector_hpd,
+				       true, &(struct nvkm_gpio_ntfy_req) {
 					.mask = NVKM_GPIO_TOGGLED,
 					.line = func.line,
 				       },
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c b/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c
index d54da8b5f87e..b3df3fe2dc09 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/gm107.c
@@ -68,6 +68,10 @@ gm107_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nvd0_disp_chan_uevent, 1, 17, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = gm107_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
@@ -80,7 +84,7 @@ gm107_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	priv->dac.sense = nv50_dac_sense;
 	priv->sor.power = nv50_sor_power;
 	priv->sor.hda_eld = nvd0_hda_eld;
-	priv->sor.hdmi = nvd0_hdmi_ctrl;
+	priv->sor.hdmi = nve0_hdmi_ctrl;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/hdanva3.c b/drivers/gpu/drm/nouveau/core/engine/disp/hdanva3.c
index 8b4e06abe533..fe9ef5894dd4 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/hdanva3.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/hdanva3.c
@@ -26,6 +26,8 @@
 #include <nvif/unpack.h>
 #include <nvif/class.h>
 
+#include <subdev/timer.h>
+
 #include "nv50.h"
 
 int
@@ -46,16 +48,21 @@ nva3_hda_eld(NV50_DISP_MTHD_V1)
 		return ret;
 
 	if (size && args->v0.data[0]) {
+		if (outp->info.type == DCB_OUTPUT_DP) {
+			nv_mask(priv, 0x61c1e0 + soff, 0x8000000d, 0x80000001);
+			nv_wait(priv, 0x61c1e0 + soff, 0x80000000, 0x00000000);
+		}
 		for (i = 0; i < size; i++)
 			nv_wr32(priv, 0x61c440 + soff, (i << 8) | args->v0.data[0]);
 		for (; i < 0x60; i++)
 			nv_wr32(priv, 0x61c440 + soff, (i << 8));
 		nv_mask(priv, 0x61c448 + soff, 0x80000003, 0x80000003);
-	} else
-	if (size) {
-		nv_mask(priv, 0x61c448 + soff, 0x80000003, 0x80000001);
 	} else {
-		nv_mask(priv, 0x61c448 + soff, 0x80000003, 0x80000000);
+		if (outp->info.type == DCB_OUTPUT_DP) {
+			nv_mask(priv, 0x61c1e0 + soff, 0x80000001, 0x80000000);
+			nv_wait(priv, 0x61c1e0 + soff, 0x80000000, 0x00000000);
+		}
+		nv_mask(priv, 0x61c448 + soff, 0x80000003, 0x80000000 | !!size);
 	}
 
 	return 0;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/hdanvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/hdanvd0.c
index baf558fc12fb..1d4e8432d857 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/hdanvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/hdanvd0.c
@@ -26,10 +26,7 @@
 #include <nvif/unpack.h>
 #include <nvif/class.h>
 
-#include <subdev/bios.h>
-#include <subdev/bios/dcb.h>
-#include <subdev/bios/dp.h>
-#include <subdev/bios/init.h>
+#include <subdev/timer.h>
 
 #include "nv50.h"
 
@@ -40,6 +37,7 @@ nvd0_hda_eld(NV50_DISP_MTHD_V1)
 		struct nv50_disp_sor_hda_eld_v0 v0;
 	} *args = data;
 	const u32 soff = outp->or * 0x030;
+	const u32 hoff = head * 0x800;
 	int ret, i;
 
 	nv_ioctl(object, "disp sor hda eld size %d\n", size);
@@ -51,16 +49,22 @@ nvd0_hda_eld(NV50_DISP_MTHD_V1)
 		return ret;
 
 	if (size && args->v0.data[0]) {
+		if (outp->info.type == DCB_OUTPUT_DP) {
+			nv_mask(priv, 0x616618 + hoff, 0x8000000c, 0x80000001);
+			nv_wait(priv, 0x616618 + hoff, 0x80000000, 0x00000000);
+		}
+		nv_mask(priv, 0x616548 + hoff, 0x00000070, 0x00000000);
 		for (i = 0; i < size; i++)
 			nv_wr32(priv, 0x10ec00 + soff, (i << 8) | args->v0.data[i]);
 		for (; i < 0x60; i++)
 			nv_wr32(priv, 0x10ec00 + soff, (i << 8));
 		nv_mask(priv, 0x10ec10 + soff, 0x80000003, 0x80000003);
-	} else
-	if (size) {
-		nv_mask(priv, 0x10ec10 + soff, 0x80000003, 0x80000001);
 	} else {
-		nv_mask(priv, 0x10ec10 + soff, 0x80000003, 0x80000000);
+		if (outp->info.type == DCB_OUTPUT_DP) {
+			nv_mask(priv, 0x616618 + hoff, 0x80000001, 0x80000000);
+			nv_wait(priv, 0x616618 + hoff, 0x80000000, 0x00000000);
+		}
+		nv_mask(priv, 0x10ec10 + soff, 0x80000003, 0x80000000 | !!size);
 	}
 
 	return 0;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/hdminvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/hdminvd0.c
index 3106d295b48d..bac4fc4570f0 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/hdminvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/hdminvd0.c
@@ -75,8 +75,5 @@ nvd0_hdmi_ctrl(NV50_DISP_MTHD_V1)
 
 	/* HDMI_CTRL */
 	nv_mask(priv, 0x616798 + hoff, 0x401f007f, ctrl);
-
-	/* NFI, audio doesn't work without it though.. */
-	nv_mask(priv, 0x616548 + hoff, 0x00000070, 0x00000000);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/hdminve0.c b/drivers/gpu/drm/nouveau/core/engine/disp/hdminve0.c
new file mode 100644
index 000000000000..528d14ec2f7f
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/hdminve0.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2014 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <core/client.h>
+#include <nvif/unpack.h>
+#include <nvif/class.h>
+
+#include "nv50.h"
+
+int
+nve0_hdmi_ctrl(NV50_DISP_MTHD_V1)
+{
+	const u32 hoff = (head * 0x800);
+	const u32 hdmi = (head * 0x400);
+	union {
+		struct nv50_disp_sor_hdmi_pwr_v0 v0;
+	} *args = data;
+	u32 ctrl;
+	int ret;
+
+	nv_ioctl(object, "disp sor hdmi ctrl size %d\n", size);
+	if (nvif_unpack(args->v0, 0, 0, false)) {
+		nv_ioctl(object, "disp sor hdmi ctrl vers %d state %d "
+				 "max_ac_packet %d rekey %d\n",
+			 args->v0.version, args->v0.state,
+			 args->v0.max_ac_packet, args->v0.rekey);
+		if (args->v0.max_ac_packet > 0x1f || args->v0.rekey > 0x7f)
+			return -EINVAL;
+		ctrl  = 0x40000000 * !!args->v0.state;
+		ctrl |= args->v0.max_ac_packet << 16;
+		ctrl |= args->v0.rekey;
+	} else
+		return ret;
+
+	if (!(ctrl & 0x40000000)) {
+		nv_mask(priv, 0x616798 + hoff, 0x40000000, 0x00000000);
+		nv_mask(priv, 0x6900c0 + hdmi, 0x00000001, 0x00000000);
+		nv_mask(priv, 0x690000 + hdmi, 0x00000001, 0x00000000);
+		return 0;
+	}
+
+	/* AVI InfoFrame */
+	nv_mask(priv, 0x690000 + hdmi, 0x00000001, 0x00000000);
+	nv_wr32(priv, 0x690008 + hdmi, 0x000d0282);
+	nv_wr32(priv, 0x69000c + hdmi, 0x0000006f);
+	nv_wr32(priv, 0x690010 + hdmi, 0x00000000);
+	nv_wr32(priv, 0x690014 + hdmi, 0x00000000);
+	nv_wr32(priv, 0x690018 + hdmi, 0x00000000);
+	nv_mask(priv, 0x690000 + hdmi, 0x00000001, 0x00000001);
+
+	/* ??? InfoFrame? */
+	nv_mask(priv, 0x6900c0 + hdmi, 0x00000001, 0x00000000);
+	nv_wr32(priv, 0x6900cc + hdmi, 0x00000010);
+	nv_mask(priv, 0x6900c0 + hdmi, 0x00000001, 0x00000001);
+
+	/* ??? */
+	nv_wr32(priv, 0x690080 + hdmi, 0x82000000);
+
+	/* HDMI_CTRL */
+	nv_mask(priv, 0x616798 + hoff, 0x401f007f, ctrl);
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
index 4b5bb5d58a54..2df3a937037d 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.c
@@ -29,6 +29,7 @@
 #include <core/enum.h>
 #include <nvif/unpack.h>
 #include <nvif/class.h>
+#include <nvif/event.h>
 
 #include <subdev/bios.h>
 #include <subdev/bios/dcb.h>
@@ -82,6 +83,71 @@ nv50_disp_chan_destroy(struct nv50_disp_chan *chan)
 	nouveau_namedb_destroy(&chan->base);
 }
 
+static void
+nv50_disp_chan_uevent_fini(struct nvkm_event *event, int type, int index)
+{
+	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
+	nv_mask(priv, 0x610028, 0x00000001 << index, 0x00000000 << index);
+}
+
+static void
+nv50_disp_chan_uevent_init(struct nvkm_event *event, int types, int index)
+{
+	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
+	nv_mask(priv, 0x610028, 0x00000001 << index, 0x00000001 << index);
+}
+
+void
+nv50_disp_chan_uevent_send(struct nv50_disp_priv *priv, int chid)
+{
+	struct nvif_notify_uevent_rep {
+	} rep;
+
+	nvkm_event_send(&priv->uevent, 1, chid, &rep, sizeof(rep));
+}
+
+int
+nv50_disp_chan_uevent_ctor(struct nouveau_object *object, void *data, u32 size,
+			   struct nvkm_notify *notify)
+{
+	struct nv50_disp_dmac *dmac = (void *)object;
+	union {
+		struct nvif_notify_uevent_req none;
+	} *args = data;
+	int ret;
+
+	if (nvif_unvers(args->none)) {
+		notify->size  = sizeof(struct nvif_notify_uevent_rep);
+		notify->types = 1;
+		notify->index = dmac->base.chid;
+		return 0;
+	}
+
+	return ret;
+}
+
+const struct nvkm_event_func
+nv50_disp_chan_uevent = {
+	.ctor = nv50_disp_chan_uevent_ctor,
+	.init = nv50_disp_chan_uevent_init,
+	.fini = nv50_disp_chan_uevent_fini,
+};
+
+int
+nv50_disp_chan_ntfy(struct nouveau_object *object, u32 type,
+		    struct nvkm_event **pevent)
+{
+	struct nv50_disp_priv *priv = (void *)object->engine;
+	switch (type) {
+	case NV50_DISP_CORE_CHANNEL_DMA_V0_NTFY_UEVENT:
+		*pevent = &priv->uevent;
+		return 0;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
+
 int
 nv50_disp_chan_map(struct nouveau_object *object, u64 *addr, u32 *size)
 {
@@ -195,7 +261,7 @@ nv50_disp_dmac_init(struct nouveau_object *object)
 		return ret;
 
 	/* enable error reporting */
-	nv_mask(priv, 0x610028, 0x00010001 << chid, 0x00010001 << chid);
+	nv_mask(priv, 0x610028, 0x00010000 << chid, 0x00010000 << chid);
 
 	/* initialise channel for dma command submission */
 	nv_wr32(priv, 0x610204 + (chid * 0x0010), dmac->push);
@@ -232,7 +298,7 @@ nv50_disp_dmac_fini(struct nouveau_object *object, bool suspend)
 			return -EBUSY;
 	}
 
-	/* disable error reporting */
+	/* disable error reporting and completion notifications */
 	nv_mask(priv, 0x610028, 0x00010001 << chid, 0x00000000 << chid);
 
 	return nv50_disp_chan_fini(&dmac->base, suspend);
@@ -454,7 +520,7 @@ nv50_disp_mast_init(struct nouveau_object *object)
 		return ret;
 
 	/* enable error reporting */
-	nv_mask(priv, 0x610028, 0x00010001, 0x00010001);
+	nv_mask(priv, 0x610028, 0x00010000, 0x00010000);
 
 	/* attempt to unstick channel from some unknown state */
 	if ((nv_rd32(priv, 0x610200) & 0x009f0000) == 0x00020000)
@@ -494,7 +560,7 @@ nv50_disp_mast_fini(struct nouveau_object *object, bool suspend)
 			return -EBUSY;
 	}
 
-	/* disable error reporting */
+	/* disable error reporting and completion notifications */
 	nv_mask(priv, 0x610028, 0x00010001, 0x00000000);
 
 	return nv50_disp_chan_fini(&mast->base, suspend);
@@ -507,6 +573,7 @@ nv50_disp_mast_ofuncs = {
 	.base.init = nv50_disp_mast_init,
 	.base.fini = nv50_disp_mast_fini,
 	.base.map  = nv50_disp_chan_map,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
 	.chid = 0,
@@ -607,6 +674,7 @@ nv50_disp_sync_ofuncs = {
 	.base.dtor = nv50_disp_dmac_dtor,
 	.base.init = nv50_disp_dmac_init,
 	.base.fini = nv50_disp_dmac_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -696,6 +764,7 @@ nv50_disp_ovly_ofuncs = {
 	.base.dtor = nv50_disp_dmac_dtor,
 	.base.init = nv50_disp_dmac_init,
 	.base.fini = nv50_disp_dmac_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -813,6 +882,7 @@ nv50_disp_oimm_ofuncs = {
 	.base.dtor = nv50_disp_pioc_dtor,
 	.base.init = nv50_disp_pioc_init,
 	.base.fini = nv50_disp_pioc_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -860,6 +930,7 @@ nv50_disp_curs_ofuncs = {
 	.base.dtor = nv50_disp_pioc_dtor,
 	.base.init = nv50_disp_pioc_init,
 	.base.fini = nv50_disp_pioc_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -1559,7 +1630,7 @@ nv50_disp_intr_unk20_1(struct nv50_disp_priv *priv, int head)
 }
 
 static void
-nv50_disp_intr_unk20_2_dp(struct nv50_disp_priv *priv,
+nv50_disp_intr_unk20_2_dp(struct nv50_disp_priv *priv, int head,
 			  struct dcb_output *outp, u32 pclk)
 {
 	const int link = !(outp->sorconf.link & 1);
@@ -1568,24 +1639,36 @@ nv50_disp_intr_unk20_2_dp(struct nv50_disp_priv *priv,
 	const u32 loff = (link * 0x080) + soff;
 	const u32 ctrl = nv_rd32(priv, 0x610794 + (or * 8));
 	const u32 symbol = 100000;
-	u32 dpctrl = nv_rd32(priv, 0x61c10c + loff) & 0x0000f0000;
+	const s32 vactive = nv_rd32(priv, 0x610af8 + (head * 0x540)) & 0xffff;
+	const s32 vblanke = nv_rd32(priv, 0x610ae8 + (head * 0x540)) & 0xffff;
+	const s32 vblanks = nv_rd32(priv, 0x610af0 + (head * 0x540)) & 0xffff;
+	u32 dpctrl = nv_rd32(priv, 0x61c10c + loff);
 	u32 clksor = nv_rd32(priv, 0x614300 + soff);
 	int bestTU = 0, bestVTUi = 0, bestVTUf = 0, bestVTUa = 0;
 	int TU, VTUi, VTUf, VTUa;
 	u64 link_data_rate, link_ratio, unk;
 	u32 best_diff = 64 * symbol;
 	u32 link_nr, link_bw, bits;
-
-	/* calculate packed data rate for each lane */
-	if      (dpctrl > 0x00030000) link_nr = 4;
-	else if (dpctrl > 0x00010000) link_nr = 2;
-	else			      link_nr = 1;
-
-	if (clksor & 0x000c0000)
-		link_bw = 270000;
-	else
-		link_bw = 162000;
-
+	u64 value;
+
+	link_bw = (clksor & 0x000c0000) ? 270000 : 162000;
+	link_nr = hweight32(dpctrl & 0x000f0000);
+
+	/* symbols/hblank - algorithm taken from comments in tegra driver */
+	value = vblanke + vactive - vblanks - 7;
+	value = value * link_bw;
+	do_div(value, pclk);
+	value = value - (3 * !!(dpctrl & 0x00004000)) - (12 / link_nr);
+	nv_mask(priv, 0x61c1e8 + soff, 0x0000ffff, value);
+
+	/* symbols/vblank - algorithm taken from comments in tegra driver */
+	value = vblanks - vblanke - 25;
+	value = value * link_bw;
+	do_div(value, pclk);
+	value = value - ((36 / link_nr) + 3) - 1;
+	nv_mask(priv, 0x61c1ec + soff, 0x00ffffff, value);
+
+	/* watermark / activesym */
 	if      ((ctrl & 0xf0000) == 0x60000) bits = 30;
 	else if ((ctrl & 0xf0000) == 0x50000) bits = 24;
 	else                                  bits = 18;
@@ -1731,7 +1814,7 @@ nv50_disp_intr_unk20_2(struct nv50_disp_priv *priv, int head)
 	} else
 	if (!outp->info.location) {
 		if (outp->info.type == DCB_OUTPUT_DP)
-			nv50_disp_intr_unk20_2_dp(priv, &outp->info, pclk);
+			nv50_disp_intr_unk20_2_dp(priv, head, &outp->info, pclk);
 		oreg = 0x614300 + (ffs(outp->info.or) - 1) * 0x800;
 		oval = (conf & 0x0100) ? 0x00000101 : 0x00000000;
 		hval = 0x00000000;
@@ -1763,9 +1846,10 @@ nv50_disp_intr_unk40_0_tmds(struct nv50_disp_priv *priv, struct dcb_output *outp
 	const int   or = ffs(outp->or) - 1;
 	const u32 loff = (or * 0x800) + (link * 0x80);
 	const u16 mask = (outp->sorconf.link << 6) | outp->or;
+	struct dcb_output match;
 	u8  ver, hdr;
 
-	if (dcb_outp_match(bios, DCB_OUTPUT_DP, mask, &ver, &hdr, outp))
+	if (dcb_outp_match(bios, DCB_OUTPUT_DP, mask, &ver, &hdr, &match))
 		nv_mask(priv, 0x61c10c + loff, 0x00000001, 0x00000000);
 }
 
@@ -1846,6 +1930,12 @@ nv50_disp_intr(struct nouveau_subdev *subdev)
 		intr0 &= ~(0x00010000 << chid);
 	}
 
+	while (intr0 & 0x0000001f) {
+		u32 chid = __ffs(intr0 & 0x0000001f);
+		nv50_disp_chan_uevent_send(priv, chid);
+		intr0 &= ~(0x00000001 << chid);
+	}
+
 	if (intr1 & 0x00000004) {
 		nouveau_disp_vblank(&priv->base, 0);
 		nv_wr32(priv, 0x610024, 0x00000004);
@@ -1880,6 +1970,10 @@ nv50_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nv50_disp_chan_uevent, 1, 9, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nv50_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h
index 8ab14461f70c..5279feefec06 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv50.h
@@ -26,6 +26,8 @@ struct nv50_disp_priv {
 	struct work_struct supervisor;
 	u32 super;
 
+	struct nvkm_event uevent;
+
 	struct {
 		int nr;
 	} head;
@@ -75,6 +77,7 @@ int nvd0_hda_eld(NV50_DISP_MTHD_V1);
 int nv84_hdmi_ctrl(NV50_DISP_MTHD_V1);
 int nva3_hdmi_ctrl(NV50_DISP_MTHD_V1);
 int nvd0_hdmi_ctrl(NV50_DISP_MTHD_V1);
+int nve0_hdmi_ctrl(NV50_DISP_MTHD_V1);
 
 int nv50_sor_power(NV50_DISP_MTHD_V1);
 
@@ -116,9 +119,16 @@ struct nv50_disp_chan {
 	int chid;
 };
 
+int  nv50_disp_chan_ntfy(struct nouveau_object *, u32, struct nvkm_event **);
 int  nv50_disp_chan_map(struct nouveau_object *, u64 *, u32 *);
 u32  nv50_disp_chan_rd32(struct nouveau_object *, u64);
 void nv50_disp_chan_wr32(struct nouveau_object *, u64, u32);
+extern const struct nvkm_event_func nv50_disp_chan_uevent;
+int  nv50_disp_chan_uevent_ctor(struct nouveau_object *, void *, u32,
+				struct nvkm_notify *);
+void nv50_disp_chan_uevent_send(struct nv50_disp_priv *, int);
+
+extern const struct nvkm_event_func nvd0_disp_chan_uevent;
 
 #define nv50_disp_chan_init(a)                                                 \
 	nouveau_namedb_init(&(a)->base)
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c
index 788ced1b6182..d36284715b2a 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv84.c
@@ -236,6 +236,10 @@ nv84_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nv50_disp_chan_uevent, 1, 9, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nv84_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c b/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c
index fa79de906eae..a117064002b1 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nv94.c
@@ -95,6 +95,10 @@ nv94_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nv50_disp_chan_uevent, 1, 9, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nv94_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c
index 7af15f5d48dc..c67e68aadd45 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nva0.c
@@ -112,6 +112,10 @@ nva0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nv50_disp_chan_uevent, 1, 9, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nva0_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c b/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c
index 6bd39448f8da..22969f355aae 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nva3.c
@@ -67,6 +67,10 @@ nva3_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nv50_disp_chan_uevent, 1, 9, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nva3_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nv50_disp_intr;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
index a4bb3c774ee1..747e64bb9c06 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvd0.c
@@ -43,6 +43,31 @@
 #include "nv50.h"
 
 /*******************************************************************************
+ * EVO channel base class
+ ******************************************************************************/
+
+static void
+nvd0_disp_chan_uevent_fini(struct nvkm_event *event, int type, int index)
+{
+	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
+	nv_mask(priv, 0x610090, 0x00000001 << index, 0x00000000 << index);
+}
+
+static void
+nvd0_disp_chan_uevent_init(struct nvkm_event *event, int types, int index)
+{
+	struct nv50_disp_priv *priv = container_of(event, typeof(*priv), uevent);
+	nv_mask(priv, 0x610090, 0x00000001 << index, 0x00000001 << index);
+}
+
+const struct nvkm_event_func
+nvd0_disp_chan_uevent = {
+	.ctor = nv50_disp_chan_uevent_ctor,
+	.init = nvd0_disp_chan_uevent_init,
+	.fini = nvd0_disp_chan_uevent_fini,
+};
+
+/*******************************************************************************
  * EVO DMA channel base class
  ******************************************************************************/
 
@@ -77,7 +102,6 @@ nvd0_disp_dmac_init(struct nouveau_object *object)
 		return ret;
 
 	/* enable error reporting */
-	nv_mask(priv, 0x610090, 0x00000001 << chid, 0x00000001 << chid);
 	nv_mask(priv, 0x6100a0, 0x00000001 << chid, 0x00000001 << chid);
 
 	/* initialise channel for dma command submission */
@@ -115,7 +139,7 @@ nvd0_disp_dmac_fini(struct nouveau_object *object, bool suspend)
 			return -EBUSY;
 	}
 
-	/* disable error reporting */
+	/* disable error reporting and completion notification */
 	nv_mask(priv, 0x610090, 0x00000001 << chid, 0x00000000);
 	nv_mask(priv, 0x6100a0, 0x00000001 << chid, 0x00000000);
 
@@ -278,7 +302,6 @@ nvd0_disp_mast_init(struct nouveau_object *object)
 		return ret;
 
 	/* enable error reporting */
-	nv_mask(priv, 0x610090, 0x00000001, 0x00000001);
 	nv_mask(priv, 0x6100a0, 0x00000001, 0x00000001);
 
 	/* initialise channel for dma command submission */
@@ -313,7 +336,7 @@ nvd0_disp_mast_fini(struct nouveau_object *object, bool suspend)
 			return -EBUSY;
 	}
 
-	/* disable error reporting */
+	/* disable error reporting and completion notification */
 	nv_mask(priv, 0x610090, 0x00000001, 0x00000000);
 	nv_mask(priv, 0x6100a0, 0x00000001, 0x00000000);
 
@@ -326,6 +349,7 @@ nvd0_disp_mast_ofuncs = {
 	.base.dtor = nv50_disp_dmac_dtor,
 	.base.init = nvd0_disp_mast_init,
 	.base.fini = nvd0_disp_mast_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -419,6 +443,7 @@ nvd0_disp_sync_ofuncs = {
 	.base.dtor = nv50_disp_dmac_dtor,
 	.base.init = nvd0_disp_dmac_init,
 	.base.fini = nvd0_disp_dmac_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -499,6 +524,7 @@ nvd0_disp_ovly_ofuncs = {
 	.base.dtor = nv50_disp_dmac_dtor,
 	.base.init = nvd0_disp_dmac_init,
 	.base.fini = nvd0_disp_dmac_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -524,7 +550,6 @@ nvd0_disp_pioc_init(struct nouveau_object *object)
 		return ret;
 
 	/* enable error reporting */
-	nv_mask(priv, 0x610090, 0x00000001 << chid, 0x00000001 << chid);
 	nv_mask(priv, 0x6100a0, 0x00000001 << chid, 0x00000001 << chid);
 
 	/* activate channel */
@@ -553,7 +578,7 @@ nvd0_disp_pioc_fini(struct nouveau_object *object, bool suspend)
 			return -EBUSY;
 	}
 
-	/* disable error reporting */
+	/* disable error reporting and completion notification */
 	nv_mask(priv, 0x610090, 0x00000001 << chid, 0x00000000);
 	nv_mask(priv, 0x6100a0, 0x00000001 << chid, 0x00000000);
 
@@ -570,6 +595,7 @@ nvd0_disp_oimm_ofuncs = {
 	.base.dtor = nv50_disp_pioc_dtor,
 	.base.init = nvd0_disp_pioc_init,
 	.base.fini = nvd0_disp_pioc_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -586,6 +612,7 @@ nvd0_disp_curs_ofuncs = {
 	.base.dtor = nv50_disp_pioc_dtor,
 	.base.init = nvd0_disp_pioc_init,
 	.base.fini = nvd0_disp_pioc_fini,
+	.base.ntfy = nv50_disp_chan_ntfy,
 	.base.map  = nv50_disp_chan_map,
 	.base.rd32 = nv50_disp_chan_rd32,
 	.base.wr32 = nv50_disp_chan_wr32,
@@ -949,6 +976,9 @@ nvd0_disp_intr_unk2_2_tu(struct nv50_disp_priv *priv, int head,
 	const int or = ffs(outp->or) - 1;
 	const u32 ctrl = nv_rd32(priv, 0x660200 + (or   * 0x020));
 	const u32 conf = nv_rd32(priv, 0x660404 + (head * 0x300));
+	const s32 vactive = nv_rd32(priv, 0x660414 + (head * 0x300)) & 0xffff;
+	const s32 vblanke = nv_rd32(priv, 0x66041c + (head * 0x300)) & 0xffff;
+	const s32 vblanks = nv_rd32(priv, 0x660420 + (head * 0x300)) & 0xffff;
 	const u32 pclk = nv_rd32(priv, 0x660450 + (head * 0x300)) / 1000;
 	const u32 link = ((ctrl & 0xf00) == 0x800) ? 0 : 1;
 	const u32 hoff = (head * 0x800);
@@ -956,23 +986,35 @@ nvd0_disp_intr_unk2_2_tu(struct nv50_disp_priv *priv, int head,
 	const u32 loff = (link * 0x080) + soff;
 	const u32 symbol = 100000;
 	const u32 TU = 64;
-	u32 dpctrl = nv_rd32(priv, 0x61c10c + loff) & 0x000f0000;
+	u32 dpctrl = nv_rd32(priv, 0x61c10c + loff);
 	u32 clksor = nv_rd32(priv, 0x612300 + soff);
 	u32 datarate, link_nr, link_bw, bits;
 	u64 ratio, value;
 
+	link_nr  = hweight32(dpctrl & 0x000f0000);
+	link_bw  = (clksor & 0x007c0000) >> 18;
+	link_bw *= 27000;
+
+	/* symbols/hblank - algorithm taken from comments in tegra driver */
+	value = vblanke + vactive - vblanks - 7;
+	value = value * link_bw;
+	do_div(value, pclk);
+	value = value - (3 * !!(dpctrl & 0x00004000)) - (12 / link_nr);
+	nv_mask(priv, 0x616620 + hoff, 0x0000ffff, value);
+
+	/* symbols/vblank - algorithm taken from comments in tegra driver */
+	value = vblanks - vblanke - 25;
+	value = value * link_bw;
+	do_div(value, pclk);
+	value = value - ((36 / link_nr) + 3) - 1;
+	nv_mask(priv, 0x616624 + hoff, 0x00ffffff, value);
+
+	/* watermark */
 	if      ((conf & 0x3c0) == 0x180) bits = 30;
 	else if ((conf & 0x3c0) == 0x140) bits = 24;
 	else                              bits = 18;
 	datarate = (pclk * bits) / 8;
 
-	if      (dpctrl > 0x00030000) link_nr = 4;
-	else if (dpctrl > 0x00010000) link_nr = 2;
-	else			      link_nr = 1;
-
-	link_bw  = (clksor & 0x007c0000) >> 18;
-	link_bw *= 27000;
-
 	ratio  = datarate;
 	ratio *= symbol;
 	do_div(ratio, link_nr * link_bw);
@@ -1153,7 +1195,11 @@ nvd0_disp_intr(struct nouveau_subdev *subdev)
 
 	if (intr & 0x00000001) {
 		u32 stat = nv_rd32(priv, 0x61008c);
-		nv_wr32(priv, 0x61008c, stat);
+		while (stat) {
+			int chid = __ffs(stat); stat &= ~(1 << chid);
+			nv50_disp_chan_uevent_send(priv, chid);
+			nv_wr32(priv, 0x61008c, 1 << chid);
+		}
 		intr &= ~0x00000001;
 	}
 
@@ -1209,6 +1255,10 @@ nvd0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nvd0_disp_chan_uevent, 1, 17, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nvd0_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c
index 47fef1e398c4..db144b2cf06b 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nve0.c
@@ -233,6 +233,10 @@ nve0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nvd0_disp_chan_uevent, 1, 17, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nve0_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
@@ -245,7 +249,7 @@ nve0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	priv->dac.sense = nv50_dac_sense;
 	priv->sor.power = nv50_sor_power;
 	priv->sor.hda_eld = nvd0_hda_eld;
-	priv->sor.hdmi = nvd0_hdmi_ctrl;
+	priv->sor.hdmi = nve0_hdmi_ctrl;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c b/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
index 04bda4ac4ed3..402d7d67d806 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/nvf0.c
@@ -68,6 +68,10 @@ nvf0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	ret = nvkm_event_init(&nvd0_disp_chan_uevent, 1, 17, &priv->uevent);
+	if (ret)
+		return ret;
+
 	nv_engine(priv)->sclass = nvf0_disp_base_oclass;
 	nv_engine(priv)->cclass = &nv50_disp_cclass;
 	nv_subdev(priv)->intr = nvd0_disp_intr;
@@ -80,7 +84,7 @@ nvf0_disp_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	priv->dac.sense = nv50_dac_sense;
 	priv->sor.power = nv50_sor_power;
 	priv->sor.hda_eld = nvd0_hda_eld;
-	priv->sor.hdmi = nvd0_hdmi_ctrl;
+	priv->sor.hdmi = nve0_hdmi_ctrl;
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/outpdp.c b/drivers/gpu/drm/nouveau/core/engine/disp/outpdp.c
index 6f6e2a898270..667a9070e006 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/outpdp.c
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/outpdp.c
@@ -254,7 +254,7 @@ nvkm_output_dp_create_(struct nouveau_object *parent,
 	atomic_set(&outp->lt.done, 0);
 
 	/* link maintenance */
-	ret = nvkm_notify_init(&i2c->event, nvkm_output_dp_irq, true,
+	ret = nvkm_notify_init(NULL, &i2c->event, nvkm_output_dp_irq, true,
 			       &(struct nvkm_i2c_ntfy_req) {
 				.mask = NVKM_I2C_IRQ,
 				.port = outp->base.edid->index,
@@ -268,7 +268,7 @@ nvkm_output_dp_create_(struct nouveau_object *parent,
 	}
 
 	/* hotplug detect, replaces gpio-based mechanism with aux events */
-	ret = nvkm_notify_init(&i2c->event, nvkm_output_dp_hpd, true,
+	ret = nvkm_notify_init(NULL, &i2c->event, nvkm_output_dp_hpd, true,
 			       &(struct nvkm_i2c_ntfy_req) {
 				.mask = NVKM_I2C_PLUG | NVKM_I2C_UNPLUG,
 				.port = outp->base.edid->index,
diff --git a/drivers/gpu/drm/nouveau/core/engine/disp/priv.h b/drivers/gpu/drm/nouveau/core/engine/disp/priv.h
index dbd43ae9df81..6a0511d54ce6 100644
--- a/drivers/gpu/drm/nouveau/core/engine/disp/priv.h
+++ b/drivers/gpu/drm/nouveau/core/engine/disp/priv.h
@@ -40,7 +40,8 @@ int  _nouveau_disp_fini(struct nouveau_object *, bool);
 extern struct nouveau_oclass *nvkm_output_oclass;
 extern struct nouveau_oclass *nvkm_connector_oclass;
 
-int  nouveau_disp_vblank_ctor(void *data, u32 size, struct nvkm_notify *);
+int  nouveau_disp_vblank_ctor(struct nouveau_object *, void *data, u32 size,
+			      struct nvkm_notify *);
 void nouveau_disp_vblank(struct nouveau_disp *, int head);
 int  nouveau_disp_ntfy(struct nouveau_object *, u32, struct nvkm_event **);
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/fifo/base.c b/drivers/gpu/drm/nouveau/core/engine/fifo/base.c
index 0f999fc45ab9..ac8375cf4eef 100644
--- a/drivers/gpu/drm/nouveau/core/engine/fifo/base.c
+++ b/drivers/gpu/drm/nouveau/core/engine/fifo/base.c
@@ -34,7 +34,8 @@
 #include <engine/fifo.h>
 
 static int
-nouveau_fifo_event_ctor(void *data, u32 size, struct nvkm_notify *notify)
+nouveau_fifo_event_ctor(struct nouveau_object *object, void *data, u32 size,
+			struct nvkm_notify *notify)
 {
 	if (size == 0) {
 		notify->size  = 0;
@@ -170,7 +171,8 @@ _nouveau_fifo_channel_wr32(struct nouveau_object *object, u64 addr, u32 data)
 }
 
 int
-nouveau_fifo_uevent_ctor(void *data, u32 size, struct nvkm_notify *notify)
+nouveau_fifo_uevent_ctor(struct nouveau_object *object, void *data, u32 size,
+			 struct nvkm_notify *notify)
 {
 	union {
 		struct nvif_notify_uevent_req none;
diff --git a/drivers/gpu/drm/nouveau/core/engine/graph/ctxnv50.c b/drivers/gpu/drm/nouveau/core/engine/graph/ctxnv50.c
index 552fdbd45ebe..1d0e33fb5f61 100644
--- a/drivers/gpu/drm/nouveau/core/engine/graph/ctxnv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/graph/ctxnv50.c
@@ -113,6 +113,8 @@
 #define IS_NVA3F(x) (((x) > 0xa0 && (x) < 0xaa) || (x) == 0xaf)
 #define IS_NVAAF(x) ((x) >= 0xaa && (x) <= 0xac)
 
+#include <subdev/fb.h>
+
 /*
  * This code deals with PGRAPH contexts on NV50 family cards. Like NV40, it's
  * the GPU itself that does context-switching, but it needs a special
@@ -569,8 +571,12 @@ nv50_graph_construct_mmio(struct nouveau_grctx *ctx)
 			gr_def(ctx, 0x407d08, 0x00010040);
 		else if (device->chipset < 0xa0)
 			gr_def(ctx, 0x407d08, 0x00390040);
-		else
-			gr_def(ctx, 0x407d08, 0x003d0040);
+		else {
+			if (nouveau_fb(device)->ram->type != NV_MEM_TYPE_GDDR5)
+				gr_def(ctx, 0x407d08, 0x003d0040);
+			else
+				gr_def(ctx, 0x407d08, 0x003c0040);
+		}
 		gr_def(ctx, 0x407d0c, 0x00000022);
 	}
 
diff --git a/drivers/gpu/drm/nouveau/core/engine/software/nv50.c b/drivers/gpu/drm/nouveau/core/engine/software/nv50.c
index 4d2994d8cc32..a0fec205f9db 100644
--- a/drivers/gpu/drm/nouveau/core/engine/software/nv50.c
+++ b/drivers/gpu/drm/nouveau/core/engine/software/nv50.c
@@ -175,7 +175,8 @@ nv50_software_context_ctor(struct nouveau_object *parent,
 		return ret;
 
 	for (i = 0; pdisp && i < pdisp->vblank.index_nr; i++) {
-		ret = nvkm_notify_init(&pdisp->vblank, pclass->vblank, false,
+		ret = nvkm_notify_init(NULL, &pdisp->vblank, pclass->vblank,
+				       false,
 				       &(struct nvif_notify_head_req_v0) {
 					.head = i,
 				       },
diff --git a/drivers/gpu/drm/nouveau/core/include/core/client.h b/drivers/gpu/drm/nouveau/core/include/core/client.h
index 1794a05205d8..b0ce9f6680b5 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/client.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/client.h
@@ -48,7 +48,7 @@ int  nouveau_client_init(struct nouveau_client *);
 int  nouveau_client_fini(struct nouveau_client *, bool suspend);
 const char *nouveau_client_name(void *obj);
 
-int nvkm_client_notify_new(struct nouveau_client *, struct nvkm_event *,
+int nvkm_client_notify_new(struct nouveau_object *, struct nvkm_event *,
 			   void *data, u32 size);
 int nvkm_client_notify_del(struct nouveau_client *, int index);
 int nvkm_client_notify_get(struct nouveau_client *, int index);
diff --git a/drivers/gpu/drm/nouveau/core/include/core/device.h b/drivers/gpu/drm/nouveau/core/include/core/device.h
index 8743766454a5..1d9d893929bb 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/device.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/device.h
@@ -24,6 +24,7 @@ enum nv_subdev_type {
 	 * been created, and are allowed to assume any subdevs in the
 	 * list above them exist and have been initialised.
 	 */
+	NVDEV_SUBDEV_FUSE,
 	NVDEV_SUBDEV_MXM,
 	NVDEV_SUBDEV_MC,
 	NVDEV_SUBDEV_BUS,
diff --git a/drivers/gpu/drm/nouveau/core/include/core/event.h b/drivers/gpu/drm/nouveau/core/include/core/event.h
index 51e55d03330a..92876528972f 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/event.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/event.h
@@ -4,7 +4,8 @@
 #include <core/notify.h>
 
 struct nvkm_event_func {
-	int  (*ctor)(void *data, u32 size, struct nvkm_notify *);
+	int  (*ctor)(struct nouveau_object *, void *data, u32 size,
+		     struct nvkm_notify *);
 	void (*send)(void *data, u32 size, struct nvkm_notify *);
 	void (*init)(struct nvkm_event *, int type, int index);
 	void (*fini)(struct nvkm_event *, int type, int index);
diff --git a/drivers/gpu/drm/nouveau/core/include/core/mm.h b/drivers/gpu/drm/nouveau/core/include/core/mm.h
index 2bf7d0e32261..bfe6931544fe 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/mm.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/mm.h
@@ -6,6 +6,10 @@ struct nouveau_mm_node {
 	struct list_head fl_entry;
 	struct list_head rl_entry;
 
+#define NVKM_MM_HEAP_ANY 0x00
+	u8  heap;
+#define NVKM_MM_TYPE_NONE 0x00
+#define NVKM_MM_TYPE_HOLE 0xff
 	u8  type;
 	u32 offset;
 	u32 length;
@@ -27,10 +31,10 @@ nouveau_mm_initialised(struct nouveau_mm *mm)
 
 int  nouveau_mm_init(struct nouveau_mm *, u32 offset, u32 length, u32 block);
 int  nouveau_mm_fini(struct nouveau_mm *);
-int  nouveau_mm_head(struct nouveau_mm *, u8 type, u32 size_max, u32 size_min,
-		     u32 align, struct nouveau_mm_node **);
-int  nouveau_mm_tail(struct nouveau_mm *, u8 type, u32 size_max, u32 size_min,
-		     u32 align, struct nouveau_mm_node **);
+int  nouveau_mm_head(struct nouveau_mm *, u8 heap, u8 type, u32 size_max,
+		     u32 size_min, u32 align, struct nouveau_mm_node **);
+int  nouveau_mm_tail(struct nouveau_mm *, u8 heap, u8 type, u32 size_max,
+		     u32 size_min, u32 align, struct nouveau_mm_node **);
 void nouveau_mm_free(struct nouveau_mm *, struct nouveau_mm_node **);
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/include/core/notify.h b/drivers/gpu/drm/nouveau/core/include/core/notify.h
index 1262d8f020f3..a7c3c5f578cc 100644
--- a/drivers/gpu/drm/nouveau/core/include/core/notify.h
+++ b/drivers/gpu/drm/nouveau/core/include/core/notify.h
@@ -25,8 +25,9 @@ struct nvkm_notify {
 	const void *data;
 };
 
-int  nvkm_notify_init(struct nvkm_event *, int (*func)(struct nvkm_notify *),
-		      bool work, void *data, u32 size, u32 reply,
+int  nvkm_notify_init(struct nouveau_object *, struct nvkm_event *,
+		      int (*func)(struct nvkm_notify *), bool work,
+		      void *data, u32 size, u32 reply,
 		      struct nvkm_notify *);
 void nvkm_notify_fini(struct nvkm_notify *);
 void nvkm_notify_get(struct nvkm_notify *);
diff --git a/drivers/gpu/drm/nouveau/core/include/engine/fifo.h b/drivers/gpu/drm/nouveau/core/include/engine/fifo.h
index e5e4d930b2c2..2007453f6fce 100644
--- a/drivers/gpu/drm/nouveau/core/include/engine/fifo.h
+++ b/drivers/gpu/drm/nouveau/core/include/engine/fifo.h
@@ -116,7 +116,8 @@ extern struct nouveau_oclass *nve0_fifo_oclass;
 extern struct nouveau_oclass *gk20a_fifo_oclass;
 extern struct nouveau_oclass *nv108_fifo_oclass;
 
-int  nouveau_fifo_uevent_ctor(void *, u32, struct nvkm_notify *);
+int  nouveau_fifo_uevent_ctor(struct nouveau_object *, void *, u32,
+			      struct nvkm_notify *);
 void nouveau_fifo_uevent(struct nouveau_fifo *);
 
 void nv04_fifo_intr(struct nouveau_subdev *);
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bar.h b/drivers/gpu/drm/nouveau/core/include/subdev/bar.h
index be037fac534c..257ddf6d36d4 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/bar.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bar.h
@@ -12,7 +12,6 @@ struct nouveau_bar {
 
 	int (*alloc)(struct nouveau_bar *, struct nouveau_object *,
 		     struct nouveau_mem *, struct nouveau_object **);
-	void __iomem *iomem;
 
 	int (*kmap)(struct nouveau_bar *, struct nouveau_mem *,
 		    u32 flags, struct nouveau_vma *);
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0205.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0205.h
new file mode 100644
index 000000000000..e171120cec81
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0205.h
@@ -0,0 +1,32 @@
+#ifndef __NVBIOS_M0205_H__
+#define __NVBIOS_M0205_H__
+
+struct nvbios_M0205T {
+	u16 freq;
+};
+
+u32 nvbios_M0205Te(struct nouveau_bios *,
+		   u8 *ver, u8 *hdr, u8 *cnt, u8 *len, u8 *snr, u8 *ssz);
+u32 nvbios_M0205Tp(struct nouveau_bios *,
+		   u8 *ver, u8 *hdr, u8 *cnt, u8 *len, u8 *snr, u8 *ssz,
+		   struct nvbios_M0205T *);
+
+struct nvbios_M0205E {
+	u8 type;
+};
+
+u32 nvbios_M0205Ee(struct nouveau_bios *, int idx,
+		   u8 *ver, u8 *hdr, u8 *cnt, u8 *len);
+u32 nvbios_M0205Ep(struct nouveau_bios *, int idx,
+		   u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+		   struct nvbios_M0205E *);
+
+struct nvbios_M0205S {
+	u8 data;
+};
+
+u32 nvbios_M0205Se(struct nouveau_bios *, int ent, int idx, u8 *ver, u8 *hdr);
+u32 nvbios_M0205Sp(struct nouveau_bios *, int ent, int idx, u8 *ver, u8 *hdr,
+		   struct nvbios_M0205S *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0209.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0209.h
new file mode 100644
index 000000000000..67dc50d837bc
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/M0209.h
@@ -0,0 +1,30 @@
+#ifndef __NVBIOS_M0209_H__
+#define __NVBIOS_M0209_H__
+
+u32 nvbios_M0209Te(struct nouveau_bios *,
+		   u8 *ver, u8 *hdr, u8 *cnt, u8 *len, u8 *snr, u8 *ssz);
+
+struct nvbios_M0209E {
+	u8 v00_40;
+	u8 bits;
+	u8 modulo;
+	u8 v02_40;
+	u8 v02_07;
+	u8 v03;
+};
+
+u32 nvbios_M0209Ee(struct nouveau_bios *, int idx,
+		   u8 *ver, u8 *hdr, u8 *cnt, u8 *len);
+u32 nvbios_M0209Ep(struct nouveau_bios *, int idx,
+		   u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+		   struct nvbios_M0209E *);
+
+struct nvbios_M0209S {
+	u32 data[0x200];
+};
+
+u32 nvbios_M0209Se(struct nouveau_bios *, int ent, int idx, u8 *ver, u8 *hdr);
+u32 nvbios_M0209Sp(struct nouveau_bios *, int ent, int idx, u8 *ver, u8 *hdr,
+		   struct nvbios_M0209S *);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/fan.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/fan.h
new file mode 100644
index 000000000000..119d0874e041
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/fan.h
@@ -0,0 +1,8 @@
+#ifndef __NVBIOS_FAN_H__
+#define __NVBIOS_FAN_H__
+
+#include <subdev/bios/therm.h>
+
+u16 nvbios_fan_parse(struct nouveau_bios *bios, struct nvbios_therm_fan *fan);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h
index c086ac6d677d..a685bbd04568 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/ramcfg.h
@@ -4,60 +4,118 @@
 struct nouveau_bios;
 
 struct nvbios_ramcfg {
-	unsigned rammap_11_08_01:1;
-	unsigned rammap_11_08_0c:2;
-	unsigned rammap_11_08_10:1;
-	unsigned rammap_11_11_0c:2;
+	unsigned rammap_ver;
+	unsigned rammap_hdr;
+	unsigned rammap_min;
+	unsigned rammap_max;
+	union {
+		struct {
+			unsigned rammap_10_04_02:1;
+			unsigned rammap_10_04_08:1;
+		};
+		struct {
+			unsigned rammap_11_08_01:1;
+			unsigned rammap_11_08_0c:2;
+			unsigned rammap_11_08_10:1;
+			unsigned rammap_11_09_01ff:9;
+			unsigned rammap_11_0a_03fe:9;
+			unsigned rammap_11_0a_0400:1;
+			unsigned rammap_11_0a_0800:1;
+			unsigned rammap_11_0b_01f0:5;
+			unsigned rammap_11_0b_0200:1;
+			unsigned rammap_11_0b_0400:1;
+			unsigned rammap_11_0b_0800:1;
+			unsigned rammap_11_0d:8;
+			unsigned rammap_11_0e:8;
+			unsigned rammap_11_0f:8;
+			unsigned rammap_11_11_0c:2;
+		};
+	};
 
-	unsigned ramcfg_11_01_01:1;
-	unsigned ramcfg_11_01_02:1;
-	unsigned ramcfg_11_01_04:1;
-	unsigned ramcfg_11_01_08:1;
-	unsigned ramcfg_11_01_10:1;
-	unsigned ramcfg_11_01_20:1;
-	unsigned ramcfg_11_01_40:1;
-	unsigned ramcfg_11_01_80:1;
-	unsigned ramcfg_11_02_03:2;
-	unsigned ramcfg_11_02_04:1;
-	unsigned ramcfg_11_02_08:1;
-	unsigned ramcfg_11_02_10:1;
-	unsigned ramcfg_11_02_40:1;
-	unsigned ramcfg_11_02_80:1;
-	unsigned ramcfg_11_03_0f:4;
-	unsigned ramcfg_11_03_30:2;
-	unsigned ramcfg_11_03_c0:2;
-	unsigned ramcfg_11_03_f0:4;
-	unsigned ramcfg_11_04:8;
-	unsigned ramcfg_11_06:8;
-	unsigned ramcfg_11_07_02:1;
-	unsigned ramcfg_11_07_04:1;
-	unsigned ramcfg_11_07_08:1;
-	unsigned ramcfg_11_07_10:1;
-	unsigned ramcfg_11_07_40:1;
-	unsigned ramcfg_11_07_80:1;
-	unsigned ramcfg_11_08_01:1;
-	unsigned ramcfg_11_08_02:1;
-	unsigned ramcfg_11_08_04:1;
-	unsigned ramcfg_11_08_08:1;
-	unsigned ramcfg_11_08_10:1;
-	unsigned ramcfg_11_08_20:1;
-	unsigned ramcfg_11_09:8;
+	unsigned ramcfg_ver;
+	unsigned ramcfg_hdr;
+	unsigned ramcfg_timing;
+	union {
+		struct {
+			unsigned ramcfg_10_02_01:1;
+			unsigned ramcfg_10_02_02:1;
+			unsigned ramcfg_10_02_04:1;
+			unsigned ramcfg_10_02_08:1;
+			unsigned ramcfg_10_02_10:1;
+			unsigned ramcfg_10_02_20:1;
+			unsigned ramcfg_10_02_40:1;
+			unsigned ramcfg_10_03_0f:4;
+			unsigned ramcfg_10_05:8;
+			unsigned ramcfg_10_06:8;
+			unsigned ramcfg_10_07:8;
+			unsigned ramcfg_10_08:8;
+			unsigned ramcfg_10_09_0f:4;
+			unsigned ramcfg_10_09_f0:4;
+		};
+		struct {
+			unsigned ramcfg_11_01_01:1;
+			unsigned ramcfg_11_01_02:1;
+			unsigned ramcfg_11_01_04:1;
+			unsigned ramcfg_11_01_08:1;
+			unsigned ramcfg_11_01_10:1;
+			unsigned ramcfg_11_01_20:1;
+			unsigned ramcfg_11_01_40:1;
+			unsigned ramcfg_11_01_80:1;
+			unsigned ramcfg_11_02_03:2;
+			unsigned ramcfg_11_02_04:1;
+			unsigned ramcfg_11_02_08:1;
+			unsigned ramcfg_11_02_10:1;
+			unsigned ramcfg_11_02_40:1;
+			unsigned ramcfg_11_02_80:1;
+			unsigned ramcfg_11_03_0f:4;
+			unsigned ramcfg_11_03_30:2;
+			unsigned ramcfg_11_03_c0:2;
+			unsigned ramcfg_11_03_f0:4;
+			unsigned ramcfg_11_04:8;
+			unsigned ramcfg_11_06:8;
+			unsigned ramcfg_11_07_02:1;
+			unsigned ramcfg_11_07_04:1;
+			unsigned ramcfg_11_07_08:1;
+			unsigned ramcfg_11_07_10:1;
+			unsigned ramcfg_11_07_40:1;
+			unsigned ramcfg_11_07_80:1;
+			unsigned ramcfg_11_08_01:1;
+			unsigned ramcfg_11_08_02:1;
+			unsigned ramcfg_11_08_04:1;
+			unsigned ramcfg_11_08_08:1;
+			unsigned ramcfg_11_08_10:1;
+			unsigned ramcfg_11_08_20:1;
+			unsigned ramcfg_11_09:8;
+		};
+	};
 
+	unsigned timing_ver;
+	unsigned timing_hdr;
 	unsigned timing[11];
-	unsigned timing_20_2e_03:2;
-	unsigned timing_20_2e_30:2;
-	unsigned timing_20_2e_c0:2;
-	unsigned timing_20_2f_03:2;
-	unsigned timing_20_2c_003f:6;
-	unsigned timing_20_2c_1fc0:7;
-	unsigned timing_20_30_f8:5;
-	unsigned timing_20_30_07:3;
-	unsigned timing_20_31_0007:3;
-	unsigned timing_20_31_0078:4;
-	unsigned timing_20_31_0780:4;
-	unsigned timing_20_31_0800:1;
-	unsigned timing_20_31_7000:3;
-	unsigned timing_20_31_8000:1;
+	union {
+		struct {
+			unsigned timing_10_WR:8;
+			unsigned timing_10_CL:8;
+			unsigned timing_10_ODT:3;
+			unsigned timing_10_CWL:8;
+		};
+		struct {
+			unsigned timing_20_2e_03:2;
+			unsigned timing_20_2e_30:2;
+			unsigned timing_20_2e_c0:2;
+			unsigned timing_20_2f_03:2;
+			unsigned timing_20_2c_003f:6;
+			unsigned timing_20_2c_1fc0:7;
+			unsigned timing_20_30_f8:5;
+			unsigned timing_20_30_07:3;
+			unsigned timing_20_31_0007:3;
+			unsigned timing_20_31_0078:4;
+			unsigned timing_20_31_0780:4;
+			unsigned timing_20_31_0800:1;
+			unsigned timing_20_31_7000:3;
+			unsigned timing_20_31_8000:1;
+		};
+	};
 };
 
 u8 nvbios_ramcfg_count(struct nouveau_bios *);
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/rammap.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/rammap.h
index 5bdf8e4db40a..47e021d3e20d 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/bios/rammap.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/rammap.h
@@ -8,9 +8,10 @@ u32 nvbios_rammapTe(struct nouveau_bios *, u8 *ver, u8 *hdr,
 
 u32 nvbios_rammapEe(struct nouveau_bios *, int idx,
 		    u8 *ver, u8 *hdr, u8 *cnt, u8 *len);
+u32 nvbios_rammapEp(struct nouveau_bios *, int idx,
+		    u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+		    struct nvbios_ramcfg *);
 u32 nvbios_rammapEm(struct nouveau_bios *, u16 mhz,
-		    u8 *ver, u8 *hdr, u8 *cnt, u8 *len);
-u32 nvbios_rammapEp(struct nouveau_bios *, u16 mhz,
 		    u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
 		    struct nvbios_ramcfg *);
 
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/bios/therm.h b/drivers/gpu/drm/nouveau/core/include/subdev/bios/therm.h
index 8dc5051df55d..295d093f3b30 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/bios/therm.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/bios/therm.h
@@ -23,6 +23,12 @@ struct nvbios_therm_sensor {
 	struct nvbios_therm_threshold thrs_shutdown;
 };
 
+enum nvbios_therm_fan_type {
+	NVBIOS_THERM_FAN_UNK = 0,
+	NVBIOS_THERM_FAN_TOGGLE = 1,
+	NVBIOS_THERM_FAN_PWM = 2,
+};
+
 /* no vbios have more than 6 */
 #define NOUVEAU_TEMP_FAN_TRIP_MAX 10
 struct nouveau_therm_trip_point {
@@ -38,7 +44,9 @@ enum nvbios_therm_fan_mode {
 };
 
 struct nvbios_therm_fan {
-	u16 pwm_freq;
+	enum nvbios_therm_fan_type type;
+
+	u32 pwm_freq;
 
 	u8 min_duty;
 	u8 max_duty;
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/clock.h b/drivers/gpu/drm/nouveau/core/include/subdev/clock.h
index a5ca00dd2f61..36ed035d4d42 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/clock.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/clock.h
@@ -29,6 +29,7 @@ enum nv_clk_src {
 	nv_clk_src_mdiv,
 
 	nv_clk_src_core,
+	nv_clk_src_core_intm,
 	nv_clk_src_shader,
 
 	nv_clk_src_mem,
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/fb.h b/drivers/gpu/drm/nouveau/core/include/subdev/fb.h
index 871e73914b24..8d0032f15205 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/fb.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/fb.h
@@ -111,6 +111,7 @@ extern struct nouveau_oclass *gm107_fb_oclass;
 #include <subdev/bios/ramcfg.h>
 
 struct nouveau_ram_data {
+	struct list_head head;
 	struct nvbios_ramcfg bios;
 	u32 freq;
 };
@@ -136,6 +137,7 @@ struct nouveau_ram {
 
 	int ranks;
 	int parts;
+	int part_mask;
 
 	int  (*get)(struct nouveau_fb *, u64 size, u32 align,
 		    u32 size_nc, u32 type, struct nouveau_mem **);
@@ -144,11 +146,6 @@ struct nouveau_ram {
 	int  (*calc)(struct nouveau_fb *, u32 freq);
 	int  (*prog)(struct nouveau_fb *);
 	void (*tidy)(struct nouveau_fb *);
-	struct {
-		u8  version;
-		u32 data;
-		u8  size;
-	} rammap, ramcfg, timing;
 	u32 freq;
 	u32 mr[16];
 	u32 mr1_nuts;
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/fb/regsnv04.h b/drivers/gpu/drm/nouveau/core/include/subdev/fb/regsnv04.h
new file mode 100644
index 000000000000..0f7fc0c52ab2
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/fb/regsnv04.h
@@ -0,0 +1,21 @@
+#ifndef __NOUVEAU_FB_REGS_04_H__
+#define __NOUVEAU_FB_REGS_04_H__
+
+#define NV04_PFB_BOOT_0						0x00100000
+#	define NV04_PFB_BOOT_0_RAM_AMOUNT			0x00000003
+#	define NV04_PFB_BOOT_0_RAM_AMOUNT_32MB			0x00000000
+#	define NV04_PFB_BOOT_0_RAM_AMOUNT_4MB			0x00000001
+#	define NV04_PFB_BOOT_0_RAM_AMOUNT_8MB			0x00000002
+#	define NV04_PFB_BOOT_0_RAM_AMOUNT_16MB			0x00000003
+#	define NV04_PFB_BOOT_0_RAM_WIDTH_128			0x00000004
+#	define NV04_PFB_BOOT_0_RAM_TYPE				0x00000028
+#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_8MBIT		0x00000000
+#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_16MBIT		0x00000008
+#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_16MBIT_4BANK	0x00000010
+#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_16MBIT		0x00000018
+#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_64MBIT		0x00000020
+#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_64MBITX16		0x00000028
+#	define NV04_PFB_BOOT_0_UMA_ENABLE			0x00000100
+#	define NV04_PFB_BOOT_0_UMA_SIZE				0x0000f000
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/fuse.h b/drivers/gpu/drm/nouveau/core/include/subdev/fuse.h
new file mode 100644
index 000000000000..2b1ddb2a9a7d
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/fuse.h
@@ -0,0 +1,30 @@
+#ifndef __NOUVEAU_FUSE_H__
+#define __NOUVEAU_FUSE_H__
+
+#include <core/subdev.h>
+#include <core/device.h>
+
+struct nouveau_fuse {
+	struct nouveau_subdev base;
+};
+
+static inline struct nouveau_fuse *
+nouveau_fuse(void *obj)
+{
+	return (void *)nv_device(obj)->subdev[NVDEV_SUBDEV_FUSE];
+}
+
+#define nouveau_fuse_create(p, e, o, d)                                        \
+	nouveau_fuse_create_((p), (e), (o), sizeof(**d), (void **)d)
+
+int  nouveau_fuse_create_(struct nouveau_object *, struct nouveau_object *,
+			  struct nouveau_oclass *, int, void **);
+void _nouveau_fuse_dtor(struct nouveau_object *);
+int  _nouveau_fuse_init(struct nouveau_object *);
+#define _nouveau_fuse_fini _nouveau_subdev_fini
+
+extern struct nouveau_oclass g80_fuse_oclass;
+extern struct nouveau_oclass gf100_fuse_oclass;
+extern struct nouveau_oclass gm107_fuse_oclass;
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/gpio.h b/drivers/gpu/drm/nouveau/core/include/subdev/gpio.h
index b73733d21cc7..f855140dbcb7 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/gpio.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/gpio.h
@@ -40,7 +40,7 @@ nouveau_gpio(void *obj)
 
 extern struct nouveau_oclass *nv10_gpio_oclass;
 extern struct nouveau_oclass *nv50_gpio_oclass;
-extern struct nouveau_oclass *nv92_gpio_oclass;
+extern struct nouveau_oclass *nv94_gpio_oclass;
 extern struct nouveau_oclass *nvd0_gpio_oclass;
 extern struct nouveau_oclass *nve0_gpio_oclass;
 
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h b/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h
index f73feec151db..bf3d1f611333 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/pwr.h
@@ -47,5 +47,8 @@ void nouveau_memx_wr32(struct nouveau_memx *, u32 addr, u32 data);
 void nouveau_memx_wait(struct nouveau_memx *,
 		       u32 addr, u32 mask, u32 data, u32 nsec);
 void nouveau_memx_nsec(struct nouveau_memx *, u32 nsec);
+void nouveau_memx_wait_vblank(struct nouveau_memx *);
+void nouveau_memx_block(struct nouveau_memx *);
+void nouveau_memx_unblock(struct nouveau_memx *);
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/include/subdev/therm.h b/drivers/gpu/drm/nouveau/core/include/subdev/therm.h
index d4a68179e586..a437597dcafc 100644
--- a/drivers/gpu/drm/nouveau/core/include/subdev/therm.h
+++ b/drivers/gpu/drm/nouveau/core/include/subdev/therm.h
@@ -78,5 +78,6 @@ extern struct nouveau_oclass nv50_therm_oclass;
 extern struct nouveau_oclass nv84_therm_oclass;
 extern struct nouveau_oclass nva3_therm_oclass;
 extern struct nouveau_oclass nvd0_therm_oclass;
+extern struct nouveau_oclass gm107_therm_oclass;
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bar/base.c b/drivers/gpu/drm/nouveau/core/subdev/bar/base.c
index 8bcbdf39cfb2..b1adc69efd88 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bar/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bar/base.c
@@ -38,10 +38,12 @@ struct nouveau_barobj {
 static int
 nouveau_barobj_ctor(struct nouveau_object *parent,
 		    struct nouveau_object *engine,
-		    struct nouveau_oclass *oclass, void *mem, u32 size,
+		    struct nouveau_oclass *oclass, void *data, u32 size,
 		    struct nouveau_object **pobject)
 {
+	struct nouveau_device *device = nv_device(parent);
 	struct nouveau_bar *bar = (void *)engine;
+	struct nouveau_mem *mem = data;
 	struct nouveau_barobj *barobj;
 	int ret;
 
@@ -54,7 +56,13 @@ nouveau_barobj_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	barobj->iomem = bar->iomem + (u32)barobj->vma.offset;
+	barobj->iomem = ioremap(nv_device_resource_start(device, 3) +
+				(u32)barobj->vma.offset, mem->size << 12);
+	if (!barobj->iomem) {
+		nv_warn(bar, "PRAMIN ioremap failed\n");
+		return -ENOMEM;
+	}
+
 	return 0;
 }
 
@@ -63,8 +71,11 @@ nouveau_barobj_dtor(struct nouveau_object *object)
 {
 	struct nouveau_bar *bar = (void *)object->engine;
 	struct nouveau_barobj *barobj = (void *)object;
-	if (barobj->vma.node)
+	if (barobj->vma.node) {
+		if (barobj->iomem)
+			iounmap(barobj->iomem);
 		bar->unmap(bar, &barobj->vma);
+	}
 	nouveau_object_destroy(&barobj->base);
 }
 
@@ -99,12 +110,11 @@ nouveau_bar_alloc(struct nouveau_bar *bar, struct nouveau_object *parent,
 		  struct nouveau_mem *mem, struct nouveau_object **pobject)
 {
 	struct nouveau_object *engine = nv_object(bar);
-	int ret = -ENOMEM;
-	if (bar->iomem) {
-		ret = nouveau_object_ctor(parent, engine,
-					  &nouveau_barobj_oclass,
-					  mem, 0, pobject);
-	}
+	struct nouveau_object *gpuobj;
+	int ret = nouveau_object_ctor(parent, engine, &nouveau_barobj_oclass,
+				      mem, 0, &gpuobj);
+	if (ret == 0)
+		*pobject = gpuobj;
 	return ret;
 }
 
@@ -113,7 +123,6 @@ nouveau_bar_create_(struct nouveau_object *parent,
 		    struct nouveau_object *engine,
 		    struct nouveau_oclass *oclass, int length, void **pobject)
 {
-	struct nouveau_device *device = nv_device(parent);
 	struct nouveau_bar *bar;
 	int ret;
 
@@ -123,21 +132,12 @@ nouveau_bar_create_(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
-	if (nv_device_resource_len(device, 3) != 0) {
-		bar->iomem = ioremap(nv_device_resource_start(device, 3),
-				     nv_device_resource_len(device, 3));
-		if (!bar->iomem)
-			nv_warn(bar, "PRAMIN ioremap failed\n");
-	}
-
 	return 0;
 }
 
 void
 nouveau_bar_destroy(struct nouveau_bar *bar)
 {
-	if (bar->iomem)
-		iounmap(bar->iomem);
 	nouveau_subdev_destroy(&bar->base);
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c
index 0a44459844e3..05a278bab247 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bar/nvc0.c
@@ -200,7 +200,6 @@ nvc0_bar_init(struct nouveau_object *object)
 
 	nv_mask(priv, 0x000200, 0x00000100, 0x00000000);
 	nv_mask(priv, 0x000200, 0x00000100, 0x00000100);
-	nv_mask(priv, 0x100c80, 0x00000001, 0x00000000);
 
 	nv_wr32(priv, 0x001704, 0x80000000 | priv->bar[1].mem->addr >> 12);
 	if (priv->bar[0].mem)
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/M0205.c b/drivers/gpu/drm/nouveau/core/subdev/bios/M0205.c
new file mode 100644
index 000000000000..ac9617c5fc2a
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/M0205.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <subdev/bios.h>
+#include <subdev/bios/bit.h>
+#include <subdev/bios/M0205.h>
+
+u32
+nvbios_M0205Te(struct nouveau_bios *bios,
+	       u8 *ver, u8 *hdr, u8 *cnt, u8 *len, u8 *snr, u8 *ssz)
+{
+	struct bit_entry bit_M;
+	u32 data = 0x00000000;
+
+	if (!bit_entry(bios, 'M', &bit_M)) {
+		if (bit_M.version == 2 && bit_M.length > 0x08)
+			data = nv_ro32(bios, bit_M.offset + 0x05);
+		if (data) {
+			*ver = nv_ro08(bios, data + 0x00);
+			switch (*ver) {
+			case 0x10:
+				*hdr = nv_ro08(bios, data + 0x01);
+				*len = nv_ro08(bios, data + 0x02);
+				*ssz = nv_ro08(bios, data + 0x03);
+				*snr = nv_ro08(bios, data + 0x04);
+				*cnt = nv_ro08(bios, data + 0x05);
+				return data;
+			default:
+				break;
+			}
+		}
+	}
+
+	return 0x00000000;
+}
+
+u32
+nvbios_M0205Tp(struct nouveau_bios *bios,
+	       u8 *ver, u8 *hdr, u8 *cnt, u8 *len, u8 *snr, u8 *ssz,
+	       struct nvbios_M0205T *info)
+{
+	u32 data = nvbios_M0205Te(bios, ver, hdr, cnt, len, snr, ssz);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	case 0x10:
+		info->freq = nv_ro16(bios, data + 0x06);
+		break;
+	default:
+		break;
+	}
+	return data;
+}
+
+u32
+nvbios_M0205Ee(struct nouveau_bios *bios, int idx,
+	       u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
+{
+	u8  snr, ssz;
+	u32 data = nvbios_M0205Te(bios, ver, hdr, cnt, len, &snr, &ssz);
+	if (data && idx < *cnt) {
+		data = data + *hdr + idx * (*len + (snr * ssz));
+		*hdr = *len;
+		*cnt = snr;
+		*len = ssz;
+		return data;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0205Ep(struct nouveau_bios *bios, int idx,
+	       u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+	       struct nvbios_M0205E *info)
+{
+	u32 data = nvbios_M0205Ee(bios, idx, ver, hdr, cnt, len);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	case 0x10:
+		info->type = nv_ro08(bios, data + 0x00) & 0x0f;
+		return data;
+	default:
+		break;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0205Se(struct nouveau_bios *bios, int ent, int idx, u8 *ver, u8 *hdr)
+{
+
+	u8  cnt, len;
+	u32 data = nvbios_M0205Ee(bios, ent, ver, hdr, &cnt, &len);
+	if (data && idx < cnt) {
+		data = data + *hdr + idx * len;
+		*hdr = len;
+		return data;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0205Sp(struct nouveau_bios *bios, int ent, int idx, u8 *ver, u8 *hdr,
+	       struct nvbios_M0205S *info)
+{
+	u32 data = nvbios_M0205Se(bios, ent, idx, ver, hdr);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	case 0x10:
+		info->data = nv_ro08(bios, data + 0x00);
+		return data;
+	default:
+		break;
+	}
+	return 0x00000000;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/M0209.c b/drivers/gpu/drm/nouveau/core/subdev/bios/M0209.c
new file mode 100644
index 000000000000..b142a510e89f
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/M0209.c
@@ -0,0 +1,137 @@
+/*
+ * Copyright 2013 Red Hat Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Ben Skeggs
+ */
+
+#include <subdev/bios.h>
+#include <subdev/bios/bit.h>
+#include <subdev/bios/M0209.h>
+
+u32
+nvbios_M0209Te(struct nouveau_bios *bios,
+	       u8 *ver, u8 *hdr, u8 *cnt, u8 *len, u8 *snr, u8 *ssz)
+{
+	struct bit_entry bit_M;
+	u32 data = 0x00000000;
+
+	if (!bit_entry(bios, 'M', &bit_M)) {
+		if (bit_M.version == 2 && bit_M.length > 0x0c)
+			data = nv_ro32(bios, bit_M.offset + 0x09);
+		if (data) {
+			*ver = nv_ro08(bios, data + 0x00);
+			switch (*ver) {
+			case 0x10:
+				*hdr = nv_ro08(bios, data + 0x01);
+				*len = nv_ro08(bios, data + 0x02);
+				*ssz = nv_ro08(bios, data + 0x03);
+				*snr = 1;
+				*cnt = nv_ro08(bios, data + 0x04);
+				return data;
+			default:
+				break;
+			}
+		}
+	}
+
+	return 0x00000000;
+}
+
+u32
+nvbios_M0209Ee(struct nouveau_bios *bios, int idx,
+	       u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
+{
+	u8  snr, ssz;
+	u32 data = nvbios_M0209Te(bios, ver, hdr, cnt, len, &snr, &ssz);
+	if (data && idx < *cnt) {
+		data = data + *hdr + idx * (*len + (snr * ssz));
+		*hdr = *len;
+		*cnt = snr;
+		*len = ssz;
+		return data;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0209Ep(struct nouveau_bios *bios, int idx,
+	       u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+	       struct nvbios_M0209E *info)
+{
+	u32 data = nvbios_M0209Ee(bios, idx, ver, hdr, cnt, len);
+	memset(info, 0x00, sizeof(*info));
+	switch (!!data * *ver) {
+	case 0x10:
+		info->v00_40 = (nv_ro08(bios, data + 0x00) & 0x40) >> 6;
+		info->bits   =  nv_ro08(bios, data + 0x00) & 0x3f;
+		info->modulo =  nv_ro08(bios, data + 0x01);
+		info->v02_40 = (nv_ro08(bios, data + 0x02) & 0x40) >> 6;
+		info->v02_07 =  nv_ro08(bios, data + 0x02) & 0x07;
+		info->v03    =  nv_ro08(bios, data + 0x03);
+		return data;
+	default:
+		break;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0209Se(struct nouveau_bios *bios, int ent, int idx, u8 *ver, u8 *hdr)
+{
+
+	u8  cnt, len;
+	u32 data = nvbios_M0209Ee(bios, ent, ver, hdr, &cnt, &len);
+	if (data && idx < cnt) {
+		data = data + *hdr + idx * len;
+		*hdr = len;
+		return data;
+	}
+	return 0x00000000;
+}
+
+u32
+nvbios_M0209Sp(struct nouveau_bios *bios, int ent, int idx, u8 *ver, u8 *hdr,
+	       struct nvbios_M0209S *info)
+{
+	struct nvbios_M0209E M0209E;
+	u8  cnt, len;
+	u32 data = nvbios_M0209Ep(bios, ent, ver, hdr, &cnt, &len, &M0209E);
+	if (data) {
+		u32 i, data = nvbios_M0209Se(bios, ent, idx, ver, hdr);
+		memset(info, 0x00, sizeof(*info));
+		switch (!!data * *ver) {
+		case 0x10:
+			for (i = 0; i < ARRAY_SIZE(info->data); i++) {
+				u32 bits = (i % M0209E.modulo) * M0209E.bits;
+				u32 mask = (1ULL << M0209E.bits) - 1;
+				u16  off = bits / 8;
+				u8   mod = bits % 8;
+				info->data[i] = nv_ro32(bios, data + off);
+				info->data[i] = info->data[i] >> mod;
+				info->data[i] = info->data[i] & mask;
+			}
+			return data;
+		default:
+			break;
+		}
+	}
+	return 0x00000000;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c b/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
index 88606bfaf847..bd8d348385b3 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/dcb.c
@@ -124,6 +124,7 @@ dcb_outp_parse(struct nouveau_bios *bios, u8 idx, u8 *ver, u8 *len,
 	       struct dcb_output *outp)
 {
 	u16 dcb = dcb_outp(bios, idx, ver, len);
+	memset(outp, 0x00, sizeof(*outp));
 	if (dcb) {
 		if (*ver >= 0x20) {
 			u32 conn = nv_ro32(bios, dcb + 0x00);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/fan.c b/drivers/gpu/drm/nouveau/core/subdev/bios/fan.c
new file mode 100644
index 000000000000..e419892240f5
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/fan.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright 2014 Martin Peres
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Martin Peres
+ */
+
+#include <subdev/bios.h>
+#include <subdev/bios/bit.h>
+#include <subdev/bios/fan.h>
+
+u16
+nvbios_fan_table(struct nouveau_bios *bios, u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
+{
+	struct bit_entry bit_P;
+	u16 fan = 0x0000;
+
+	if (!bit_entry(bios, 'P', &bit_P)) {
+		if (bit_P.version == 2 && bit_P.length >= 0x5a)
+			fan = nv_ro16(bios, bit_P.offset + 0x58);
+
+		if (fan) {
+			*ver = nv_ro08(bios, fan + 0);
+			switch (*ver) {
+			case 0x10:
+				*hdr = nv_ro08(bios, fan + 1);
+				*len = nv_ro08(bios, fan + 2);
+				*cnt = nv_ro08(bios, fan + 3);
+				return fan;
+			default:
+				break;
+			}
+		}
+	}
+
+	return 0x0000;
+}
+
+u16
+nvbios_fan_entry(struct nouveau_bios *bios, int idx, u8 *ver, u8 *hdr,
+		 u8 *cnt, u8 *len)
+{
+	u16 data = nvbios_fan_table(bios, ver, hdr, cnt, len);
+	if (data && idx < *cnt)
+		return data + *hdr + (idx * (*len));
+	return 0x0000;
+}
+
+u16
+nvbios_fan_parse(struct nouveau_bios *bios, struct nvbios_therm_fan *fan)
+{
+	u8 ver, hdr, cnt, len;
+
+	u16 data = nvbios_fan_entry(bios, 0, &ver, &hdr, &cnt, &len);
+	if (data) {
+		u8 type = nv_ro08(bios, data + 0x00);
+		switch (type) {
+		case 0:
+			fan->type = NVBIOS_THERM_FAN_TOGGLE;
+			break;
+		case 1:
+		case 2:
+			/* TODO: Understand the difference between the two! */
+			fan->type = NVBIOS_THERM_FAN_PWM;
+			break;
+		default:
+			fan->type = NVBIOS_THERM_FAN_UNK;
+		}
+
+		fan->min_duty = nv_ro08(bios, data + 0x02);
+		fan->max_duty = nv_ro08(bios, data + 0x03);
+
+		fan->pwm_freq = nv_ro32(bios, data + 0x0b) & 0xffffff;
+	}
+	return data;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c b/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c
index 1811b2cb0472..585e69331ccc 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/rammap.c
@@ -75,31 +75,39 @@ nvbios_rammapEe(struct nouveau_bios *bios, int idx,
 }
 
 u32
-nvbios_rammapEm(struct nouveau_bios *bios, u16 khz,
-		u8 *ver, u8 *hdr, u8 *cnt, u8 *len)
-{
-	int idx = 0;
-	u32 data;
-	while ((data = nvbios_rammapEe(bios, idx++, ver, hdr, cnt, len))) {
-		if (khz >= nv_ro16(bios, data + 0x00) &&
-		    khz <= nv_ro16(bios, data + 0x02))
-			break;
-	}
-	return data;
-}
-
-u32
-nvbios_rammapEp(struct nouveau_bios *bios, u16 khz,
+nvbios_rammapEp(struct nouveau_bios *bios, int idx,
 		u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
 		struct nvbios_ramcfg *p)
 {
-	u32 data = nvbios_rammapEm(bios, khz, ver, hdr, cnt, len);
+	u32 data = nvbios_rammapEe(bios, idx, ver, hdr, cnt, len), temp;
 	memset(p, 0x00, sizeof(*p));
+	p->rammap_ver = *ver;
+	p->rammap_hdr = *hdr;
 	switch (!!data * *ver) {
+	case 0x10:
+		p->rammap_min      =  nv_ro16(bios, data + 0x00);
+		p->rammap_max      =  nv_ro16(bios, data + 0x02);
+		p->rammap_10_04_02 = (nv_ro08(bios, data + 0x04) & 0x02) >> 1;
+		p->rammap_10_04_08 = (nv_ro08(bios, data + 0x04) & 0x08) >> 3;
+		break;
 	case 0x11:
+		p->rammap_min      =  nv_ro16(bios, data + 0x00);
+		p->rammap_max      =  nv_ro16(bios, data + 0x02);
 		p->rammap_11_08_01 = (nv_ro08(bios, data + 0x08) & 0x01) >> 0;
 		p->rammap_11_08_0c = (nv_ro08(bios, data + 0x08) & 0x0c) >> 2;
 		p->rammap_11_08_10 = (nv_ro08(bios, data + 0x08) & 0x10) >> 4;
+		temp = nv_ro32(bios, data + 0x09);
+		p->rammap_11_09_01ff = (temp & 0x000001ff) >> 0;
+		p->rammap_11_0a_03fe = (temp & 0x0003fe00) >> 9;
+		p->rammap_11_0a_0400 = (temp & 0x00040000) >> 18;
+		p->rammap_11_0a_0800 = (temp & 0x00080000) >> 19;
+		p->rammap_11_0b_01f0 = (temp & 0x01f00000) >> 20;
+		p->rammap_11_0b_0200 = (temp & 0x02000000) >> 25;
+		p->rammap_11_0b_0400 = (temp & 0x04000000) >> 26;
+		p->rammap_11_0b_0800 = (temp & 0x08000000) >> 27;
+		p->rammap_11_0d    =  nv_ro08(bios, data + 0x0d);
+		p->rammap_11_0e    =  nv_ro08(bios, data + 0x0e);
+		p->rammap_11_0f    =  nv_ro08(bios, data + 0x0f);
 		p->rammap_11_11_0c = (nv_ro08(bios, data + 0x11) & 0x0c) >> 2;
 		break;
 	default:
@@ -110,6 +118,20 @@ nvbios_rammapEp(struct nouveau_bios *bios, u16 khz,
 }
 
 u32
+nvbios_rammapEm(struct nouveau_bios *bios, u16 mhz,
+		u8 *ver, u8 *hdr, u8 *cnt, u8 *len,
+		struct nvbios_ramcfg *info)
+{
+	int idx = 0;
+	u32 data;
+	while ((data = nvbios_rammapEp(bios, idx++, ver, hdr, cnt, len, info))) {
+		if (mhz >= info->rammap_min && mhz <= info->rammap_max)
+			break;
+	}
+	return data;
+}
+
+u32
 nvbios_rammapSe(struct nouveau_bios *bios, u32 data,
 		u8 ever, u8 ehdr, u8 ecnt, u8 elen, int idx,
 		u8 *ver, u8 *hdr)
@@ -129,8 +151,28 @@ nvbios_rammapSp(struct nouveau_bios *bios, u32 data,
 		u8 *ver, u8 *hdr, struct nvbios_ramcfg *p)
 {
 	data = nvbios_rammapSe(bios, data, ever, ehdr, ecnt, elen, idx, ver, hdr);
+	p->ramcfg_ver = *ver;
+	p->ramcfg_hdr = *hdr;
 	switch (!!data * *ver) {
+	case 0x10:
+		p->ramcfg_timing   =  nv_ro08(bios, data + 0x01);
+		p->ramcfg_10_02_01 = (nv_ro08(bios, data + 0x02) & 0x01) >> 0;
+		p->ramcfg_10_02_02 = (nv_ro08(bios, data + 0x02) & 0x02) >> 1;
+		p->ramcfg_10_02_04 = (nv_ro08(bios, data + 0x02) & 0x04) >> 2;
+		p->ramcfg_10_02_08 = (nv_ro08(bios, data + 0x02) & 0x08) >> 3;
+		p->ramcfg_10_02_10 = (nv_ro08(bios, data + 0x02) & 0x10) >> 4;
+		p->ramcfg_10_02_20 = (nv_ro08(bios, data + 0x02) & 0x20) >> 5;
+		p->ramcfg_10_02_40 = (nv_ro08(bios, data + 0x02) & 0x40) >> 6;
+		p->ramcfg_10_03_0f = (nv_ro08(bios, data + 0x03) & 0x0f) >> 0;
+		p->ramcfg_10_05    = (nv_ro08(bios, data + 0x05) & 0xff) >> 0;
+		p->ramcfg_10_06    = (nv_ro08(bios, data + 0x06) & 0xff) >> 0;
+		p->ramcfg_10_07    = (nv_ro08(bios, data + 0x07) & 0xff) >> 0;
+		p->ramcfg_10_08    = (nv_ro08(bios, data + 0x08) & 0xff) >> 0;
+		p->ramcfg_10_09_0f = (nv_ro08(bios, data + 0x09) & 0x0f) >> 0;
+		p->ramcfg_10_09_f0 = (nv_ro08(bios, data + 0x09) & 0xf0) >> 4;
+		break;
 	case 0x11:
+		p->ramcfg_timing   =  nv_ro08(bios, data + 0x00);
 		p->ramcfg_11_01_01 = (nv_ro08(bios, data + 0x01) & 0x01) >> 0;
 		p->ramcfg_11_01_02 = (nv_ro08(bios, data + 0x01) & 0x02) >> 1;
 		p->ramcfg_11_01_04 = (nv_ro08(bios, data + 0x01) & 0x04) >> 2;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c b/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c
index 350d44ab2ba2..46d955eb51eb 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/bios/timing.c
@@ -89,7 +89,15 @@ nvbios_timingEp(struct nouveau_bios *bios, int idx,
 		struct nvbios_ramcfg *p)
 {
 	u16 data = nvbios_timingEe(bios, idx, ver, hdr, cnt, len), temp;
+	p->timing_ver = *ver;
+	p->timing_hdr = *hdr;
 	switch (!!data * *ver) {
+	case 0x10:
+		p->timing_10_WR = nv_ro08(bios, data + 0x00);
+		p->timing_10_CL = nv_ro08(bios, data + 0x02);
+		p->timing_10_ODT = nv_ro08(bios, data + 0x0e) & 0x07;
+		p->timing_10_CWL = nv_ro08(bios, data + 0x13);
+		break;
 	case 0x20:
 		p->timing[0] = nv_ro32(bios, data + 0x00);
 		p->timing[1] = nv_ro32(bios, data + 0x04);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/base.c b/drivers/gpu/drm/nouveau/core/subdev/clock/base.c
index a276a711294a..e51b72d47129 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/base.c
@@ -573,7 +573,7 @@ nouveau_clock_create_(struct nouveau_object *parent,
 
 	clk->allow_reclock = allow_reclock;
 
-	ret = nvkm_notify_init(&device->event, nouveau_clock_pwrsrc, true,
+	ret = nvkm_notify_init(NULL, &device->event, nouveau_clock_pwrsrc, true,
 			       NULL, 0, 0, &clk->pwrsrc_ntfy);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c b/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c
index 087012b18956..094551d8ad9b 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.c
@@ -20,8 +20,10 @@
  * OTHER DEALINGS IN THE SOFTWARE.
  *
  * Authors: Ben Skeggs
+ *          Roy Spliet
  */
 
+#include <engine/fifo.h>
 #include <subdev/bios.h>
 #include <subdev/bios/pll.h>
 #include <subdev/timer.h>
@@ -42,9 +44,17 @@ static u32
 read_vco(struct nva3_clock_priv *priv, int clk)
 {
 	u32 sctl = nv_rd32(priv, 0x4120 + (clk * 4));
-	if ((sctl & 0x00000030) != 0x00000030)
+
+	switch (sctl & 0x00000030) {
+	case 0x00000000:
+		return nv_device(priv)->crystal;
+	case 0x00000020:
 		return read_pll(priv, 0x41, 0x00e820);
-	return read_pll(priv, 0x42, 0x00e8a0);
+	case 0x00000030:
+		return read_pll(priv, 0x42, 0x00e8a0);
+	default:
+		return 0;
+	}
 }
 
 static u32
@@ -66,14 +76,25 @@ read_clk(struct nva3_clock_priv *priv, int clk, bool ignore_en)
 	if (!ignore_en && !(sctl & 0x00000100))
 		return 0;
 
+	/* out_alt */
+	if (sctl & 0x00000400)
+		return 108000;
+
+	/* vco_out */
 	switch (sctl & 0x00003000) {
 	case 0x00000000:
-		return nv_device(priv)->crystal;
+		if (!(sctl & 0x00000200))
+			return nv_device(priv)->crystal;
+		return 0;
 	case 0x00002000:
 		if (sctl & 0x00000040)
 			return 108000;
 		return 100000;
 	case 0x00003000:
+		/* vco_enable */
+		if (!(sctl & 0x00000001))
+			return 0;
+
 		sclk = read_vco(priv, clk);
 		sdiv = ((sctl & 0x003f0000) >> 16) + 2;
 		return (sclk * 2) / sdiv;
@@ -95,7 +116,9 @@ read_pll(struct nva3_clock_priv *priv, int clk, u32 pll)
 			N = (coef & 0x0000ff00) >> 8;
 			P = (coef & 0x003f0000) >> 16;
 
-			/* no post-divider on these.. */
+			/* no post-divider on these..
+			 * XXX: it looks more like two post-"dividers" that
+			 * cross each other out in the default RPLL config */
 			if ((pll & 0x00ff00) == 0x00e800)
 				P = 1;
 
@@ -114,13 +137,13 @@ static int
 nva3_clock_read(struct nouveau_clock *clk, enum nv_clk_src src)
 {
 	struct nva3_clock_priv *priv = (void *)clk;
+	u32 hsrc;
 
 	switch (src) {
 	case nv_clk_src_crystal:
 		return nv_device(priv)->crystal;
-	case nv_clk_src_href:
-		return 100000;
 	case nv_clk_src_core:
+	case nv_clk_src_core_intm:
 		return read_pll(priv, 0x00, 0x4200);
 	case nv_clk_src_shader:
 		return read_pll(priv, 0x01, 0x4220);
@@ -132,24 +155,33 @@ nva3_clock_read(struct nouveau_clock *clk, enum nv_clk_src src)
 		return read_clk(priv, 0x21, false);
 	case nv_clk_src_daemon:
 		return read_clk(priv, 0x25, false);
+	case nv_clk_src_host:
+		hsrc = (nv_rd32(priv, 0xc040) & 0x30000000) >> 28;
+		switch (hsrc) {
+		case 0:
+			return read_clk(priv, 0x1d, false);
+		case 2:
+		case 3:
+			return 277000;
+		default:
+			nv_error(clk, "unknown HOST clock source %d\n", hsrc);
+			return -EINVAL;
+		}
 	default:
 		nv_error(clk, "invalid clock source %d\n", src);
 		return -EINVAL;
 	}
+
+	return 0;
 }
 
 int
-nva3_clock_info(struct nouveau_clock *clock, int clk, u32 pll, u32 khz,
+nva3_clk_info(struct nouveau_clock *clock, int clk, u32 khz,
 		struct nva3_clock_info *info)
 {
-	struct nouveau_bios *bios = nouveau_bios(clock);
 	struct nva3_clock_priv *priv = (void *)clock;
-	struct nvbios_pll limits;
-	u32 oclk, sclk, sdiv;
-	int P, N, M, diff;
-	int ret;
+	u32 oclk, sclk, sdiv, diff;
 
-	info->pll = 0;
 	info->clk = 0;
 
 	switch (khz) {
@@ -164,43 +196,69 @@ nva3_clock_info(struct nouveau_clock *clock, int clk, u32 pll, u32 khz,
 		return khz;
 	default:
 		sclk = read_vco(priv, clk);
-		sdiv = min((sclk * 2) / (khz - 2999), (u32)65);
-		/* if the clock has a PLL attached, and we can get a within
-		 * [-2, 3) MHz of a divider, we'll disable the PLL and use
-		 * the divider instead.
-		 *
-		 * divider can go as low as 2, limited here because NVIDIA
+		sdiv = min((sclk * 2) / khz, (u32)65);
+		oclk = (sclk * 2) / sdiv;
+		diff = ((khz + 3000) - oclk);
+
+		/* When imprecise, play it safe and aim for a clock lower than
+		 * desired rather than higher */
+		if (diff < 0) {
+			sdiv++;
+			oclk = (sclk * 2) / sdiv;
+		}
+
+		/* divider can go as low as 2, limited here because NVIDIA
 		 * and the VBIOS on my NVA8 seem to prefer using the PLL
 		 * for 810MHz - is there a good reason?
-		 */
+		 * XXX: PLLs with refclk 810MHz?  */
 		if (sdiv > 4) {
-			oclk = (sclk * 2) / sdiv;
-			diff = khz - oclk;
-			if (!pll || (diff >= -2000 && diff < 3000)) {
-				info->clk = (((sdiv - 2) << 16) | 0x00003100);
-				return oclk;
-			}
+			info->clk = (((sdiv - 2) << 16) | 0x00003100);
+			return oclk;
 		}
 
-		if (!pll)
-			return -ERANGE;
 		break;
 	}
 
+	return -ERANGE;
+}
+
+int
+nva3_pll_info(struct nouveau_clock *clock, int clk, u32 pll, u32 khz,
+		struct nva3_clock_info *info)
+{
+	struct nouveau_bios *bios = nouveau_bios(clock);
+	struct nva3_clock_priv *priv = (void *)clock;
+	struct nvbios_pll limits;
+	int P, N, M, diff;
+	int ret;
+
+	info->pll = 0;
+
+	/* If we can get a within [-2, 3) MHz of a divider, we'll disable the
+	 * PLL and use the divider instead. */
+	ret = nva3_clk_info(clock, clk, khz, info);
+	diff = khz - ret;
+	if (!pll || (diff >= -2000 && diff < 3000)) {
+		goto out;
+	}
+
+	/* Try with PLL */
 	ret = nvbios_pll_parse(bios, pll, &limits);
 	if (ret)
 		return ret;
 
-	limits.refclk = read_clk(priv, clk - 0x10, true);
-	if (!limits.refclk)
+	ret = nva3_clk_info(clock, clk - 0x10, limits.refclk, info);
+	if (ret != limits.refclk)
 		return -EINVAL;
 
 	ret = nva3_pll_calc(nv_subdev(priv), &limits, khz, &N, NULL, &M, &P);
 	if (ret >= 0) {
-		info->clk = nv_rd32(priv, 0x4120 + (clk * 4));
 		info->pll = (P << 16) | (N << 8) | M;
 	}
 
+out:
+	info->fb_delay = max(((khz + 7566) / 15133), (u32) 18);
+
 	return ret ? ret : -ERANGE;
 }
 
@@ -208,13 +266,76 @@ static int
 calc_clk(struct nva3_clock_priv *priv, struct nouveau_cstate *cstate,
 	 int clk, u32 pll, int idx)
 {
-	int ret = nva3_clock_info(&priv->base, clk, pll, cstate->domain[idx],
+	int ret = nva3_pll_info(&priv->base, clk, pll, cstate->domain[idx],
 				  &priv->eng[idx]);
 	if (ret >= 0)
 		return 0;
 	return ret;
 }
 
+static int
+calc_host(struct nva3_clock_priv *priv, struct nouveau_cstate *cstate)
+{
+	int ret = 0;
+	u32 kHz = cstate->domain[nv_clk_src_host];
+	struct nva3_clock_info *info = &priv->eng[nv_clk_src_host];
+
+	if (kHz == 277000) {
+		info->clk = 0;
+		info->host_out = NVA3_HOST_277;
+		return 0;
+	}
+
+	info->host_out = NVA3_HOST_CLK;
+
+	ret = nva3_clk_info(&priv->base, 0x1d, kHz, info);
+	if (ret >= 0)
+		return 0;
+	return ret;
+}
+
+int
+nva3_clock_pre(struct nouveau_clock *clk, unsigned long *flags)
+{
+	struct nouveau_fifo *pfifo = nouveau_fifo(clk);
+
+	/* halt and idle execution engines */
+	nv_mask(clk, 0x020060, 0x00070000, 0x00000000);
+	nv_mask(clk, 0x002504, 0x00000001, 0x00000001);
+	/* Wait until the interrupt handler is finished */
+	if (!nv_wait(clk, 0x000100, 0xffffffff, 0x00000000))
+		return -EBUSY;
+
+	if (pfifo)
+		pfifo->pause(pfifo, flags);
+
+	if (!nv_wait(clk, 0x002504, 0x00000010, 0x00000010))
+		return -EIO;
+	if (!nv_wait(clk, 0x00251c, 0x0000003f, 0x0000003f))
+		return -EIO;
+
+	return 0;
+}
+
+void
+nva3_clock_post(struct nouveau_clock *clk, unsigned long *flags)
+{
+	struct nouveau_fifo *pfifo = nouveau_fifo(clk);
+
+	if (pfifo && flags)
+		pfifo->start(pfifo, flags);
+
+	nv_mask(clk, 0x002504, 0x00000001, 0x00000000);
+	nv_mask(clk, 0x020060, 0x00070000, 0x00040000);
+}
+
+static void
+disable_clk_src(struct nva3_clock_priv *priv, u32 src)
+{
+	nv_mask(priv, src, 0x00000100, 0x00000000);
+	nv_mask(priv, src, 0x00000001, 0x00000000);
+}
+
 static void
 prog_pll(struct nva3_clock_priv *priv, int clk, u32 pll, int idx)
 {
@@ -223,24 +344,35 @@ prog_pll(struct nva3_clock_priv *priv, int clk, u32 pll, int idx)
 	const u32 src1 = 0x004160 + (clk * 4);
 	const u32 ctrl = pll + 0;
 	const u32 coef = pll + 4;
+	u32 bypass;
 
 	if (info->pll) {
-		nv_mask(priv, src0, 0x00000101, 0x00000101);
+		/* Always start from a non-PLL clock */
+		bypass = nv_rd32(priv, ctrl)  & 0x00000008;
+		if (!bypass) {
+			nv_mask(priv, src1, 0x00000101, 0x00000101);
+			nv_mask(priv, ctrl, 0x00000008, 0x00000008);
+			udelay(20);
+		}
+
+		nv_mask(priv, src0, 0x003f3141, 0x00000101 | info->clk);
 		nv_wr32(priv, coef, info->pll);
 		nv_mask(priv, ctrl, 0x00000015, 0x00000015);
 		nv_mask(priv, ctrl, 0x00000010, 0x00000000);
-		nv_wait(priv, ctrl, 0x00020000, 0x00020000);
+		if (!nv_wait(priv, ctrl, 0x00020000, 0x00020000)) {
+			nv_mask(priv, ctrl, 0x00000010, 0x00000010);
+			nv_mask(priv, src0, 0x00000101, 0x00000000);
+			return;
+		}
 		nv_mask(priv, ctrl, 0x00000010, 0x00000010);
 		nv_mask(priv, ctrl, 0x00000008, 0x00000000);
-		nv_mask(priv, src1, 0x00000100, 0x00000000);
-		nv_mask(priv, src1, 0x00000001, 0x00000000);
+		disable_clk_src(priv, src1);
 	} else {
 		nv_mask(priv, src1, 0x003f3141, 0x00000101 | info->clk);
 		nv_mask(priv, ctrl, 0x00000018, 0x00000018);
 		udelay(20);
 		nv_mask(priv, ctrl, 0x00000001, 0x00000000);
-		nv_mask(priv, src0, 0x00000100, 0x00000000);
-		nv_mask(priv, src0, 0x00000001, 0x00000000);
+		disable_clk_src(priv, src0);
 	}
 }
 
@@ -251,18 +383,72 @@ prog_clk(struct nva3_clock_priv *priv, int clk, int idx)
 	nv_mask(priv, 0x004120 + (clk * 4), 0x003f3141, 0x00000101 | info->clk);
 }
 
+static void
+prog_host(struct nva3_clock_priv *priv)
+{
+	struct nva3_clock_info *info = &priv->eng[nv_clk_src_host];
+	u32 hsrc = (nv_rd32(priv, 0xc040));
+
+	switch (info->host_out) {
+	case NVA3_HOST_277:
+		if ((hsrc & 0x30000000) == 0) {
+			nv_wr32(priv, 0xc040, hsrc | 0x20000000);
+			disable_clk_src(priv, 0x4194);
+		}
+		break;
+	case NVA3_HOST_CLK:
+		prog_clk(priv, 0x1d, nv_clk_src_host);
+		if ((hsrc & 0x30000000) >= 0x20000000) {
+			nv_wr32(priv, 0xc040, hsrc & ~0x30000000);
+		}
+		break;
+	default:
+		break;
+	}
+
+	/* This seems to be a clock gating factor on idle, always set to 64 */
+	nv_wr32(priv, 0xc044, 0x3e);
+}
+
+static void
+prog_core(struct nva3_clock_priv *priv, int idx)
+{
+	struct nva3_clock_info *info = &priv->eng[idx];
+	u32 fb_delay = nv_rd32(priv, 0x10002c);
+
+	if (fb_delay < info->fb_delay)
+		nv_wr32(priv, 0x10002c, info->fb_delay);
+
+	prog_pll(priv, 0x00, 0x004200, idx);
+
+	if (fb_delay > info->fb_delay)
+		nv_wr32(priv, 0x10002c, info->fb_delay);
+}
+
 static int
 nva3_clock_calc(struct nouveau_clock *clk, struct nouveau_cstate *cstate)
 {
 	struct nva3_clock_priv *priv = (void *)clk;
+	struct nva3_clock_info *core = &priv->eng[nv_clk_src_core];
 	int ret;
 
 	if ((ret = calc_clk(priv, cstate, 0x10, 0x4200, nv_clk_src_core)) ||
 	    (ret = calc_clk(priv, cstate, 0x11, 0x4220, nv_clk_src_shader)) ||
 	    (ret = calc_clk(priv, cstate, 0x20, 0x0000, nv_clk_src_disp)) ||
-	    (ret = calc_clk(priv, cstate, 0x21, 0x0000, nv_clk_src_vdec)))
+	    (ret = calc_clk(priv, cstate, 0x21, 0x0000, nv_clk_src_vdec)) ||
+	    (ret = calc_host(priv, cstate)))
 		return ret;
 
+	/* XXX: Should be reading the highest bit in the VBIOS clock to decide
+	 * whether to use a PLL or not... but using a PLL defeats the purpose */
+	if (core->pll) {
+		ret = nva3_clk_info(clk, 0x10,
+				cstate->domain[nv_clk_src_core_intm],
+				&priv->eng[nv_clk_src_core_intm]);
+		if (ret < 0)
+			return ret;
+	}
+
 	return 0;
 }
 
@@ -270,11 +456,31 @@ static int
 nva3_clock_prog(struct nouveau_clock *clk)
 {
 	struct nva3_clock_priv *priv = (void *)clk;
-	prog_pll(priv, 0x00, 0x004200, nv_clk_src_core);
+	struct nva3_clock_info *core = &priv->eng[nv_clk_src_core];
+	int ret = 0;
+	unsigned long flags;
+	unsigned long *f = &flags;
+
+	ret = nva3_clock_pre(clk, f);
+	if (ret)
+		goto out;
+
+	if (core->pll)
+		prog_core(priv, nv_clk_src_core_intm);
+
+	prog_core(priv,  nv_clk_src_core);
 	prog_pll(priv, 0x01, 0x004220, nv_clk_src_shader);
 	prog_clk(priv, 0x20, nv_clk_src_disp);
 	prog_clk(priv, 0x21, nv_clk_src_vdec);
-	return 0;
+	prog_host(priv);
+
+out:
+	if (ret == -EBUSY)
+		f = NULL;
+
+	nva3_clock_post(clk, f);
+
+	return ret;
 }
 
 static void
@@ -284,13 +490,14 @@ nva3_clock_tidy(struct nouveau_clock *clk)
 
 static struct nouveau_clocks
 nva3_domain[] = {
-	{ nv_clk_src_crystal, 0xff },
-	{ nv_clk_src_href   , 0xff },
-	{ nv_clk_src_core   , 0x00, 0, "core", 1000 },
-	{ nv_clk_src_shader , 0x01, 0, "shader", 1000 },
-	{ nv_clk_src_mem    , 0x02, 0, "memory", 1000 },
-	{ nv_clk_src_vdec   , 0x03 },
-	{ nv_clk_src_disp   , 0x04 },
+	{ nv_clk_src_crystal  , 0xff },
+	{ nv_clk_src_core     , 0x00, 0, "core", 1000 },
+	{ nv_clk_src_shader   , 0x01, 0, "shader", 1000 },
+	{ nv_clk_src_mem      , 0x02, 0, "memory", 1000 },
+	{ nv_clk_src_vdec     , 0x03 },
+	{ nv_clk_src_disp     , 0x04 },
+	{ nv_clk_src_host     , 0x05 },
+	{ nv_clk_src_core_intm, 0x06 },
 	{ nv_clk_src_max }
 };
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.h b/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.h
index 6229a509b42e..a45a1038b12f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/nva3.h
@@ -6,9 +6,15 @@
 struct nva3_clock_info {
 	u32 clk;
 	u32 pll;
+	enum {
+		NVA3_HOST_277,
+		NVA3_HOST_CLK,
+	} host_out;
+	u32 fb_delay;
 };
 
-int nva3_clock_info(struct nouveau_clock *, int, u32, u32,
+int nva3_pll_info(struct nouveau_clock *, int, u32, u32,
 		    struct nva3_clock_info *);
-
+int nva3_clock_pre(struct nouveau_clock *clk, unsigned long *flags);
+void nva3_clock_post(struct nouveau_clock *clk, unsigned long *flags);
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/clock/nvaa.c b/drivers/gpu/drm/nouveau/core/subdev/clock/nvaa.c
index 74e19731b1b7..54aeab8005a0 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/clock/nvaa.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/clock/nvaa.c
@@ -28,6 +28,7 @@
 #include <subdev/timer.h>
 #include <subdev/clock.h>
 
+#include "nva3.h"
 #include "pll.h"
 
 struct nvaa_clock_priv {
@@ -299,25 +300,14 @@ static int
 nvaa_clock_prog(struct nouveau_clock *clk)
 {
 	struct nvaa_clock_priv *priv = (void *)clk;
-	struct nouveau_fifo *pfifo = nouveau_fifo(clk);
+	u32 pllmask = 0, mast;
 	unsigned long flags;
-	u32 pllmask = 0, mast, ptherm_gate;
-	int ret = -EBUSY;
-
-	/* halt and idle execution engines */
-	ptherm_gate = nv_mask(clk, 0x020060, 0x00070000, 0x00000000);
-	nv_mask(clk, 0x002504, 0x00000001, 0x00000001);
-	/* Wait until the interrupt handler is finished */
-	if (!nv_wait(clk, 0x000100, 0xffffffff, 0x00000000))
-		goto resume;
-
-	if (pfifo)
-		pfifo->pause(pfifo, &flags);
+	unsigned long *f = &flags;
+	int ret = 0;
 
-	if (!nv_wait(clk, 0x002504, 0x00000010, 0x00000010))
-		goto resume;
-	if (!nv_wait(clk, 0x00251c, 0x0000003f, 0x0000003f))
-		goto resume;
+	ret = nva3_clock_pre(clk, f);
+	if (ret)
+		goto out;
 
 	/* First switch to safe clocks: href */
 	mast = nv_mask(clk, 0xc054, 0x03400e70, 0x03400640);
@@ -375,15 +365,8 @@ nvaa_clock_prog(struct nouveau_clock *clk)
 	}
 
 	nv_wr32(clk, 0xc054, mast);
-	ret = 0;
 
 resume:
-	if (pfifo)
-		pfifo->start(pfifo, &flags);
-
-	nv_mask(clk, 0x002504, 0x00000001, 0x00000000);
-	nv_wr32(clk, 0x020060, ptherm_gate);
-
 	/* Disable some PLLs and dividers when unused */
 	if (priv->csrc != nv_clk_src_core) {
 		nv_wr32(clk, 0x4040, 0x00000000);
@@ -395,6 +378,12 @@ resume:
 		nv_mask(clk, 0x4020, 0x80000000, 0x00000000);
 	}
 
+out:
+	if (ret == -EBUSY)
+		f = NULL;
+
+	nva3_clock_post(clk, f);
+
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/devinit/fbmem.h b/drivers/gpu/drm/nouveau/core/subdev/devinit/fbmem.h
index 4fe49cf4c99a..6103484fea72 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/devinit/fbmem.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/devinit/fbmem.h
@@ -26,22 +26,8 @@
 
 #include <core/device.h>
 
-#define NV04_PFB_BOOT_0						0x00100000
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT			0x00000003
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_32MB			0x00000000
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_4MB			0x00000001
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_8MB			0x00000002
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_16MB			0x00000003
-#	define NV04_PFB_BOOT_0_RAM_WIDTH_128			0x00000004
-#	define NV04_PFB_BOOT_0_RAM_TYPE				0x00000028
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_8MBIT		0x00000000
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_16MBIT		0x00000008
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_16MBIT_4BANK	0x00000010
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_16MBIT		0x00000018
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_64MBIT		0x00000020
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_64MBITX16		0x00000028
-#	define NV04_PFB_BOOT_0_UMA_ENABLE			0x00000100
-#	define NV04_PFB_BOOT_0_UMA_SIZE				0x0000f000
+#include <subdev/fb/regsnv04.h>
+
 #define NV04_PFB_DEBUG_0					0x00100080
 #	define NV04_PFB_DEBUG_0_PAGE_MODE			0x00000001
 #	define NV04_PFB_DEBUG_0_REFRESH_OFF			0x00000010
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/gddr5.c b/drivers/gpu/drm/nouveau/core/subdev/fb/gddr5.c
index 66fe959b4f74..7fbbe05d5c60 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/gddr5.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/gddr5.c
@@ -40,7 +40,7 @@ nouveau_gddr5_calc(struct nouveau_ram *ram, bool nuts)
 	int WL, CL, WR, at[2], dt, ds;
 	int rq = ram->freq < 1000000; /* XXX */
 
-	switch (ram->ramcfg.version) {
+	switch (ram->next->bios.ramcfg_ver) {
 	case 0x11:
 		pd =  ram->next->bios.ramcfg_11_01_80;
 		lf =  ram->next->bios.ramcfg_11_01_40;
@@ -54,7 +54,7 @@ nouveau_gddr5_calc(struct nouveau_ram *ram, bool nuts)
 		return -ENOSYS;
 	}
 
-	switch (ram->timing.version) {
+	switch (ram->next->bios.timing_ver) {
 	case 0x20:
 		WL = (ram->next->bios.timing[1] & 0x00000f80) >> 7;
 		CL = (ram->next->bios.timing[1] & 0x0000001f);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nv20.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nv20.c
index f003c1b1893f..2209ade63339 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nv20.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nv20.c
@@ -45,7 +45,7 @@ nv20_fb_tile_comp(struct nouveau_fb *pfb, int i, u32 size, u32 flags,
 {
 	u32 tiles = DIV_ROUND_UP(size, 0x40);
 	u32 tags  = round_up(tiles / pfb->ram->parts, 0x40);
-	if (!nouveau_mm_head(&pfb->tags, 1, tags, tags, 1, &tile->tag)) {
+	if (!nouveau_mm_head(&pfb->tags, 0, 1, tags, tags, 1, &tile->tag)) {
 		if (!(flags & 2)) tile->zcomp = 0x00000000; /* Z16 */
 		else              tile->zcomp = 0x04000000; /* Z24S8 */
 		tile->zcomp |= tile->tag->offset;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nv25.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nv25.c
index f34f4223210b..e2a66c355c50 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nv25.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nv25.c
@@ -32,7 +32,7 @@ nv25_fb_tile_comp(struct nouveau_fb *pfb, int i, u32 size, u32 flags,
 {
 	u32 tiles = DIV_ROUND_UP(size, 0x40);
 	u32 tags  = round_up(tiles / pfb->ram->parts, 0x40);
-	if (!nouveau_mm_head(&pfb->tags, 1, tags, tags, 1, &tile->tag)) {
+	if (!nouveau_mm_head(&pfb->tags, 0, 1, tags, tags, 1, &tile->tag)) {
 		if (!(flags & 2)) tile->zcomp = 0x00100000; /* Z16 */
 		else              tile->zcomp = 0x00200000; /* Z24S8 */
 		tile->zcomp |= tile->tag->offset;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nv30.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nv30.c
index 69093f7151f0..cbec402ba5b9 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nv30.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nv30.c
@@ -51,7 +51,7 @@ nv30_fb_tile_comp(struct nouveau_fb *pfb, int i, u32 size, u32 flags,
 {
 	u32 tiles = DIV_ROUND_UP(size, 0x40);
 	u32 tags  = round_up(tiles / pfb->ram->parts, 0x40);
-	if (!nouveau_mm_head(&pfb->tags, 1, tags, tags, 1, &tile->tag)) {
+	if (!nouveau_mm_head(&pfb->tags, 0, 1, tags, tags, 1, &tile->tag)) {
 		if (flags & 2) tile->zcomp |= 0x01000000; /* Z16 */
 		else           tile->zcomp |= 0x02000000; /* Z24S8 */
 		tile->zcomp |= ((tile->tag->offset           ) >> 6);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nv35.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nv35.c
index 161b06e8fc3f..b2cf8c69fb2e 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nv35.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nv35.c
@@ -32,7 +32,7 @@ nv35_fb_tile_comp(struct nouveau_fb *pfb, int i, u32 size, u32 flags,
 {
 	u32 tiles = DIV_ROUND_UP(size, 0x40);
 	u32 tags  = round_up(tiles / pfb->ram->parts, 0x40);
-	if (!nouveau_mm_head(&pfb->tags, 1, tags, tags, 1, &tile->tag)) {
+	if (!nouveau_mm_head(&pfb->tags, 0, 1, tags, tags, 1, &tile->tag)) {
 		if (flags & 2) tile->zcomp |= 0x04000000; /* Z16 */
 		else           tile->zcomp |= 0x08000000; /* Z24S8 */
 		tile->zcomp |= ((tile->tag->offset           ) >> 6);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nv36.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nv36.c
index 2dd3d0aab6bb..b4cdae2a3b2f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nv36.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nv36.c
@@ -32,7 +32,7 @@ nv36_fb_tile_comp(struct nouveau_fb *pfb, int i, u32 size, u32 flags,
 {
 	u32 tiles = DIV_ROUND_UP(size, 0x40);
 	u32 tags  = round_up(tiles / pfb->ram->parts, 0x40);
-	if (!nouveau_mm_head(&pfb->tags, 1, tags, tags, 1, &tile->tag)) {
+	if (!nouveau_mm_head(&pfb->tags, 0, 1, tags, tags, 1, &tile->tag)) {
 		if (flags & 2) tile->zcomp |= 0x10000000; /* Z16 */
 		else           tile->zcomp |= 0x20000000; /* Z24S8 */
 		tile->zcomp |= ((tile->tag->offset           ) >> 6);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nv40.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nv40.c
index 95a115ab0c86..52814258c212 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nv40.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nv40.c
@@ -33,7 +33,7 @@ nv40_fb_tile_comp(struct nouveau_fb *pfb, int i, u32 size, u32 flags,
 	u32 tiles = DIV_ROUND_UP(size, 0x80);
 	u32 tags  = round_up(tiles / pfb->ram->parts, 0x100);
 	if ( (flags & 2) &&
-	    !nouveau_mm_head(&pfb->tags, 1, tags, tags, 1, &tile->tag)) {
+	    !nouveau_mm_head(&pfb->tags, 0, 1, tags, tags, 1, &tile->tag)) {
 		tile->zcomp  = 0x28000000; /* Z24S8_SPLIT_GRAD */
 		tile->zcomp |= ((tile->tag->offset           ) >> 8);
 		tile->zcomp |= ((tile->tag->offset + tags - 1) >> 8) << 13;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c b/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c
index b19a2b3c1081..32f28dc73ef2 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/nvc0.c
@@ -60,6 +60,7 @@ nvc0_fb_init(struct nouveau_object *object)
 
 	if (priv->r100c10_page)
 		nv_wr32(priv, 0x100c10, priv->r100c10 >> 8);
+	nv_mask(priv, 0x100c80, 0x00000001, 0x00000000); /* 128KiB lpg */
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h b/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h
index 82273f832e42..60322e906dd4 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/priv.h
@@ -35,6 +35,7 @@ extern struct nouveau_oclass nve0_ram_oclass;
 extern struct nouveau_oclass gk20a_ram_oclass;
 extern struct nouveau_oclass gm107_ram_oclass;
 
+int nouveau_sddr2_calc(struct nouveau_ram *ram);
 int nouveau_sddr3_calc(struct nouveau_ram *ram);
 int nouveau_gddr5_calc(struct nouveau_ram *ram, bool nuts);
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h b/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h
index 2af9cfd2c60f..d1fbbe4b00a2 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramfuc.h
@@ -12,16 +12,32 @@ struct ramfuc {
 struct ramfuc_reg {
 	int sequence;
 	bool force;
-	u32 addr[2];
+	u32 addr;
+	u32 stride; /* in bytes */
+	u32 mask;
 	u32 data;
 };
 
 static inline struct ramfuc_reg
+ramfuc_stride(u32 addr, u32 stride, u32 mask)
+{
+	return (struct ramfuc_reg) {
+		.sequence = 0,
+		.addr = addr,
+		.stride = stride,
+		.mask = mask,
+		.data = 0xdeadbeef,
+	};
+}
+
+static inline struct ramfuc_reg
 ramfuc_reg2(u32 addr1, u32 addr2)
 {
 	return (struct ramfuc_reg) {
 		.sequence = 0,
-		.addr = { addr1, addr2 },
+		.addr = addr1,
+		.stride = addr2 - addr1,
+		.mask = 0x3,
 		.data = 0xdeadbeef,
 	};
 }
@@ -29,7 +45,13 @@ ramfuc_reg2(u32 addr1, u32 addr2)
 static noinline struct ramfuc_reg
 ramfuc_reg(u32 addr)
 {
-	return ramfuc_reg2(addr, addr);
+	return (struct ramfuc_reg) {
+		.sequence = 0,
+		.addr = addr,
+		.stride = 0,
+		.mask = 0x1,
+		.data = 0xdeadbeef,
+	};
 }
 
 static inline int
@@ -62,18 +84,25 @@ static inline u32
 ramfuc_rd32(struct ramfuc *ram, struct ramfuc_reg *reg)
 {
 	if (reg->sequence != ram->sequence)
-		reg->data = nv_rd32(ram->pfb, reg->addr[0]);
+		reg->data = nv_rd32(ram->pfb, reg->addr);
 	return reg->data;
 }
 
 static inline void
 ramfuc_wr32(struct ramfuc *ram, struct ramfuc_reg *reg, u32 data)
 {
+	unsigned int mask, off = 0;
+
 	reg->sequence = ram->sequence;
 	reg->data = data;
-	if (reg->addr[0] != reg->addr[1])
-		nouveau_memx_wr32(ram->memx, reg->addr[1], reg->data);
-	nouveau_memx_wr32(ram->memx, reg->addr[0], reg->data);
+
+	for (mask = reg->mask; mask > 0; mask = (mask & ~1) >> 1) {
+		if (mask & 1) {
+			nouveau_memx_wr32(ram->memx, reg->addr+off, reg->data);
+		}
+
+		off += reg->stride;
+	}
 }
 
 static inline void
@@ -105,14 +134,35 @@ ramfuc_nsec(struct ramfuc *ram, u32 nsec)
 	nouveau_memx_nsec(ram->memx, nsec);
 }
 
-#define ram_init(s,p)       ramfuc_init(&(s)->base, (p))
-#define ram_exec(s,e)       ramfuc_exec(&(s)->base, (e))
-#define ram_have(s,r)       ((s)->r_##r.addr[0] != 0x000000)
-#define ram_rd32(s,r)       ramfuc_rd32(&(s)->base, &(s)->r_##r)
-#define ram_wr32(s,r,d)     ramfuc_wr32(&(s)->base, &(s)->r_##r, (d))
-#define ram_nuke(s,r)       ramfuc_nuke(&(s)->base, &(s)->r_##r)
-#define ram_mask(s,r,m,d)   ramfuc_mask(&(s)->base, &(s)->r_##r, (m), (d))
-#define ram_wait(s,r,m,d,n) ramfuc_wait(&(s)->base, (r), (m), (d), (n))
-#define ram_nsec(s,n)       ramfuc_nsec(&(s)->base, (n))
+static inline void
+ramfuc_wait_vblank(struct ramfuc *ram)
+{
+	nouveau_memx_wait_vblank(ram->memx);
+}
+
+static inline void
+ramfuc_block(struct ramfuc *ram)
+{
+	nouveau_memx_block(ram->memx);
+}
+
+static inline void
+ramfuc_unblock(struct ramfuc *ram)
+{
+	nouveau_memx_unblock(ram->memx);
+}
+
+#define ram_init(s,p)        ramfuc_init(&(s)->base, (p))
+#define ram_exec(s,e)        ramfuc_exec(&(s)->base, (e))
+#define ram_have(s,r)        ((s)->r_##r.addr != 0x000000)
+#define ram_rd32(s,r)        ramfuc_rd32(&(s)->base, &(s)->r_##r)
+#define ram_wr32(s,r,d)      ramfuc_wr32(&(s)->base, &(s)->r_##r, (d))
+#define ram_nuke(s,r)        ramfuc_nuke(&(s)->base, &(s)->r_##r)
+#define ram_mask(s,r,m,d)    ramfuc_mask(&(s)->base, &(s)->r_##r, (m), (d))
+#define ram_wait(s,r,m,d,n)  ramfuc_wait(&(s)->base, (r), (m), (d), (n))
+#define ram_nsec(s,n)        ramfuc_nsec(&(s)->base, (n))
+#define ram_wait_vblank(s)   ramfuc_wait_vblank(&(s)->base)
+#define ram_block(s)         ramfuc_block(&(s)->base)
+#define ram_unblock(s)       ramfuc_unblock(&(s)->base)
 
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv04.c b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv04.c
index e781080d3327..1972268d1410 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv04.c
@@ -22,22 +22,7 @@
  * Authors: Ben Skeggs
  */
 
-#define NV04_PFB_BOOT_0						0x00100000
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT			0x00000003
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_32MB			0x00000000
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_4MB			0x00000001
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_8MB			0x00000002
-#	define NV04_PFB_BOOT_0_RAM_AMOUNT_16MB			0x00000003
-#	define NV04_PFB_BOOT_0_RAM_WIDTH_128			0x00000004
-#	define NV04_PFB_BOOT_0_RAM_TYPE				0x00000028
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_8MBIT		0x00000000
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_16MBIT		0x00000008
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SGRAM_16MBIT_4BANK	0x00000010
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_16MBIT		0x00000018
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_64MBIT		0x00000020
-#	define NV04_PFB_BOOT_0_RAM_TYPE_SDRAM_64MBITX16		0x00000028
-#	define NV04_PFB_BOOT_0_UMA_ENABLE			0x00000100
-#	define NV04_PFB_BOOT_0_UMA_SIZE				0x0000f000
+#include <subdev/fb/regsnv04.h>
 
 #include "priv.h"
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv50.c b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv50.c
index e5d12c24cc43..64a983c96625 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv50.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnv50.c
@@ -280,7 +280,7 @@ nv50_ram_get(struct nouveau_fb *pfb, u64 size, u32 align, u32 ncmin,
 		if (align == 16) {
 			int n = (max >> 4) * comp;
 
-			ret = nouveau_mm_head(tags, 1, n, n, 1, &mem->tag);
+			ret = nouveau_mm_head(tags, 0, 1, n, n, 1, &mem->tag);
 			if (ret)
 				mem->tag = NULL;
 		}
@@ -296,9 +296,9 @@ nv50_ram_get(struct nouveau_fb *pfb, u64 size, u32 align, u32 ncmin,
 	type = nv50_fb_memtype[type];
 	do {
 		if (back)
-			ret = nouveau_mm_tail(heap, type, max, min, align, &r);
+			ret = nouveau_mm_tail(heap, 0, type, max, min, align, &r);
 		else
-			ret = nouveau_mm_head(heap, type, max, min, align, &r);
+			ret = nouveau_mm_head(heap, 0, type, max, min, align, &r);
 		if (ret) {
 			mutex_unlock(&pfb->base.mutex);
 			pfb->ram->put(pfb, &mem);
@@ -319,27 +319,22 @@ nv50_ram_get(struct nouveau_fb *pfb, u64 size, u32 align, u32 ncmin,
 static u32
 nv50_fb_vram_rblock(struct nouveau_fb *pfb, struct nouveau_ram *ram)
 {
-	int i, parts, colbits, rowbitsa, rowbitsb, banks;
+	int colbits, rowbitsa, rowbitsb, banks;
 	u64 rowsize, predicted;
-	u32 r0, r4, rt, ru, rblock_size;
+	u32 r0, r4, rt, rblock_size;
 
 	r0 = nv_rd32(pfb, 0x100200);
 	r4 = nv_rd32(pfb, 0x100204);
 	rt = nv_rd32(pfb, 0x100250);
-	ru = nv_rd32(pfb, 0x001540);
-	nv_debug(pfb, "memcfg 0x%08x 0x%08x 0x%08x 0x%08x\n", r0, r4, rt, ru);
-
-	for (i = 0, parts = 0; i < 8; i++) {
-		if (ru & (0x00010000 << i))
-			parts++;
-	}
+	nv_debug(pfb, "memcfg 0x%08x 0x%08x 0x%08x 0x%08x\n", r0, r4, rt,
+			nv_rd32(pfb, 0x001540));
 
 	colbits  =  (r4 & 0x0000f000) >> 12;
 	rowbitsa = ((r4 & 0x000f0000) >> 16) + 8;
 	rowbitsb = ((r4 & 0x00f00000) >> 20) + 8;
 	banks    = 1 << (((r4 & 0x03000000) >> 24) + 2);
 
-	rowsize = parts * banks * (1 << colbits) * 8;
+	rowsize = ram->parts * banks * (1 << colbits) * 8;
 	predicted = rowsize << rowbitsa;
 	if (r0 & 0x00000004)
 		predicted += rowsize << rowbitsb;
@@ -376,6 +371,9 @@ nv50_ram_create_(struct nouveau_object *parent, struct nouveau_object *engine,
 	ram->size = nv_rd32(pfb, 0x10020c);
 	ram->size = (ram->size & 0xffffff00) | ((ram->size & 0x000000ff) << 32);
 
+	ram->part_mask = (nv_rd32(pfb, 0x001540) & 0x00ff0000) >> 16;
+	ram->parts = hweight8(ram->part_mask);
+
 	switch (nv_rd32(pfb, 0x100714) & 0x00000007) {
 	case 0: ram->type = NV_MEM_TYPE_DDR1; break;
 	case 1:
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c
index 8076fb195dd5..3601deca0bd5 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnva3.c
@@ -79,20 +79,27 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 	struct nva3_ram *ram = (void *)pfb->ram;
 	struct nva3_ramfuc *fuc = &ram->fuc;
 	struct nva3_clock_info mclk;
-	u8  ver, cnt, len, strap;
+	struct nouveau_ram_data *next;
+	u8  ver, hdr, cnt, len, strap;
 	u32 data;
-	struct {
-		u32 data;
-		u8  size;
-	} rammap, ramcfg, timing;
 	u32 r004018, r100760, ctrl;
 	u32 unk714, unk718, unk71c;
-	int ret;
+	int ret, i;
+
+	next = &ram->base.target;
+	next->freq = freq;
+	ram->base.next = next;
 
 	/* lookup memory config data relevant to the target frequency */
-	rammap.data = nvbios_rammapEm(bios, freq / 1000, &ver, &rammap.size,
-				     &cnt, &ramcfg.size);
-	if (!rammap.data || ver != 0x10 || rammap.size < 0x0e) {
+	i = 0;
+	while ((data = nvbios_rammapEp(bios, i++, &ver, &hdr, &cnt, &len,
+				      &next->bios))) {
+		if (freq / 1000 >= next->bios.rammap_min &&
+		    freq / 1000 <= next->bios.rammap_max)
+			break;
+	}
+
+	if (!data || ver != 0x10 || hdr < 0x0e) {
 		nv_error(pfb, "invalid/missing rammap entry\n");
 		return -EINVAL;
 	}
@@ -104,26 +111,25 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 		return -EINVAL;
 	}
 
-	ramcfg.data = rammap.data + rammap.size + (strap * ramcfg.size);
-	if (!ramcfg.data || ver != 0x10 || ramcfg.size < 0x0e) {
+	data = nvbios_rammapSp(bios, data, ver, hdr, cnt, len, strap,
+			       &ver, &hdr, &next->bios);
+	if (!data || ver != 0x10 || hdr < 0x0e) {
 		nv_error(pfb, "invalid/missing ramcfg entry\n");
 		return -EINVAL;
 	}
 
 	/* lookup memory timings, if bios says they're present */
-	strap = nv_ro08(bios, ramcfg.data + 0x01);
-	if (strap != 0xff) {
-		timing.data = nvbios_timingEe(bios, strap, &ver, &timing.size,
-					     &cnt, &len);
-		if (!timing.data || ver != 0x10 || timing.size < 0x19) {
+	if (next->bios.ramcfg_timing != 0xff) {
+		data = nvbios_timingEp(bios, next->bios.ramcfg_timing,
+				       &ver, &hdr, &cnt, &len,
+				       &next->bios);
+		if (!data || ver != 0x10 || hdr < 0x19) {
 			nv_error(pfb, "invalid/missing timing entry\n");
 			return -EINVAL;
 		}
-	} else {
-		timing.data = 0;
 	}
 
-	ret = nva3_clock_info(nouveau_clock(pfb), 0x12, 0x4000, freq, &mclk);
+	ret = nva3_pll_info(nouveau_clock(pfb), 0x12, 0x4000, freq, &mclk);
 	if (ret < 0) {
 		nv_error(pfb, "failed mclk calculation\n");
 		return ret;
@@ -163,17 +169,17 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 		ram_mask(fuc, 0x004168, 0x003f3141, ctrl);
 	}
 
-	if ( (nv_ro08(bios, ramcfg.data + 0x02) & 0x10)) {
+	if (next->bios.ramcfg_10_02_10) {
 		ram_mask(fuc, 0x111104, 0x00000600, 0x00000000);
 	} else {
 		ram_mask(fuc, 0x111100, 0x40000000, 0x40000000);
 		ram_mask(fuc, 0x111104, 0x00000180, 0x00000000);
 	}
 
-	if (!(nv_ro08(bios, rammap.data + 0x04) & 0x02))
+	if (!next->bios.rammap_10_04_02)
 		ram_mask(fuc, 0x100200, 0x00000800, 0x00000000);
 	ram_wr32(fuc, 0x611200, 0x00003300);
-	if (!(nv_ro08(bios, ramcfg.data + 0x02) & 0x10))
+	if (!next->bios.ramcfg_10_02_10)
 		ram_wr32(fuc, 0x111100, 0x4c020000); /*XXX*/
 
 	ram_wr32(fuc, 0x1002d4, 0x00000001);
@@ -202,17 +208,16 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 		ram_wr32(fuc, 0x004018, 0x0000d000 | r004018);
 	}
 
-	if ( (nv_ro08(bios, rammap.data + 0x04) & 0x08)) {
-		u32 unk5a0 = (nv_ro16(bios, ramcfg.data + 0x05) << 8) |
-			      nv_ro08(bios, ramcfg.data + 0x05);
-		u32 unk5a4 = (nv_ro16(bios, ramcfg.data + 0x07));
-		u32 unk804 = (nv_ro08(bios, ramcfg.data + 0x09) & 0xf0) << 16 |
-			     (nv_ro08(bios, ramcfg.data + 0x03) & 0x0f) << 16 |
-			     (nv_ro08(bios, ramcfg.data + 0x09) & 0x0f) |
-			     0x80000000;
-		ram_wr32(fuc, 0x1005a0, unk5a0);
-		ram_wr32(fuc, 0x1005a4, unk5a4);
-		ram_wr32(fuc, 0x10f804, unk804);
+	if (next->bios.rammap_10_04_08) {
+		ram_wr32(fuc, 0x1005a0, next->bios.ramcfg_10_06 << 16 |
+					next->bios.ramcfg_10_05 << 8 |
+					next->bios.ramcfg_10_05);
+		ram_wr32(fuc, 0x1005a4, next->bios.ramcfg_10_08 << 8 |
+					next->bios.ramcfg_10_07);
+		ram_wr32(fuc, 0x10f804, next->bios.ramcfg_10_09_f0 << 20 |
+					next->bios.ramcfg_10_03_0f << 16 |
+					next->bios.ramcfg_10_09_0f |
+					0x80000000);
 		ram_mask(fuc, 0x10053c, 0x00001000, 0x00000000);
 	} else {
 		ram_mask(fuc, 0x10053c, 0x00001000, 0x00001000);
@@ -250,27 +255,26 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 	ram_mask(fuc, 0x100220[0], 0x00000000, 0x00000000);
 	ram_mask(fuc, 0x100220[8], 0x00000000, 0x00000000);
 
-	data = (nv_ro08(bios, ramcfg.data + 0x02) & 0x08) ? 0x00000000 : 0x00001000;
-	ram_mask(fuc, 0x100200, 0x00001000, data);
+	ram_mask(fuc, 0x100200, 0x00001000, !next->bios.ramcfg_10_02_08 << 12);
 
 	unk714 = ram_rd32(fuc, 0x100714) & ~0xf0000010;
 	unk718 = ram_rd32(fuc, 0x100718) & ~0x00000100;
 	unk71c = ram_rd32(fuc, 0x10071c) & ~0x00000100;
-	if ( (nv_ro08(bios, ramcfg.data + 0x02) & 0x20))
+	if (next->bios.ramcfg_10_02_20)
 		unk714 |= 0xf0000000;
-	if (!(nv_ro08(bios, ramcfg.data + 0x02) & 0x04))
+	if (!next->bios.ramcfg_10_02_04)
 		unk714 |= 0x00000010;
 	ram_wr32(fuc, 0x100714, unk714);
 
-	if (nv_ro08(bios, ramcfg.data + 0x02) & 0x01)
+	if (next->bios.ramcfg_10_02_01)
 		unk71c |= 0x00000100;
 	ram_wr32(fuc, 0x10071c, unk71c);
 
-	if (nv_ro08(bios, ramcfg.data + 0x02) & 0x02)
+	if (next->bios.ramcfg_10_02_02)
 		unk718 |= 0x00000100;
 	ram_wr32(fuc, 0x100718, unk718);
 
-	if (nv_ro08(bios, ramcfg.data + 0x02) & 0x10)
+	if (next->bios.ramcfg_10_02_10)
 		ram_wr32(fuc, 0x111100, 0x48000000); /*XXX*/
 
 	ram_mask(fuc, mr[0], 0x100, 0x100);
@@ -282,9 +286,9 @@ nva3_ram_calc(struct nouveau_fb *pfb, u32 freq)
 	ram_nsec(fuc, 12000);
 
 	ram_wr32(fuc, 0x611200, 0x00003330);
-	if ( (nv_ro08(bios, rammap.data + 0x04) & 0x02))
+	if (next->bios.rammap_10_04_02)
 		ram_mask(fuc, 0x100200, 0x00000800, 0x00000800);
-	if ( (nv_ro08(bios, ramcfg.data + 0x02) & 0x10)) {
+	if (next->bios.ramcfg_10_02_10) {
 		ram_mask(fuc, 0x111104, 0x00000180, 0x00000180);
 		ram_mask(fuc, 0x111100, 0x40000000, 0x00000000);
 	} else {
@@ -404,11 +408,11 @@ nva3_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	ram->fuc.r_0x100714 = ramfuc_reg(0x100714);
 	ram->fuc.r_0x100718 = ramfuc_reg(0x100718);
 	ram->fuc.r_0x10071c = ramfuc_reg(0x10071c);
-	ram->fuc.r_0x100760 = ramfuc_reg(0x100760);
-	ram->fuc.r_0x1007a0 = ramfuc_reg(0x1007a0);
-	ram->fuc.r_0x1007e0 = ramfuc_reg(0x1007e0);
+	ram->fuc.r_0x100760 = ramfuc_stride(0x100760, 4, ram->base.part_mask);
+	ram->fuc.r_0x1007a0 = ramfuc_stride(0x1007a0, 4, ram->base.part_mask);
+	ram->fuc.r_0x1007e0 = ramfuc_stride(0x1007e0, 4, ram->base.part_mask);
 	ram->fuc.r_0x10f804 = ramfuc_reg(0x10f804);
-	ram->fuc.r_0x1110e0 = ramfuc_reg(0x1110e0);
+	ram->fuc.r_0x1110e0 = ramfuc_stride(0x1110e0, 4, ram->base.part_mask);
 	ram->fuc.r_0x111100 = ramfuc_reg(0x111100);
 	ram->fuc.r_0x111104 = ramfuc_reg(0x111104);
 	ram->fuc.r_0x611200 = ramfuc_reg(0x611200);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnvc0.c b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnvc0.c
index 2b284b192763..735cb9580abe 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnvc0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnvc0.c
@@ -133,6 +133,7 @@ nvc0_ram_calc(struct nouveau_fb *pfb, u32 freq)
 	struct nouveau_bios *bios = nouveau_bios(pfb);
 	struct nvc0_ram *ram = (void *)pfb->ram;
 	struct nvc0_ramfuc *fuc = &ram->fuc;
+	struct nvbios_ramcfg cfg;
 	u8  ver, cnt, len, strap;
 	struct {
 		u32 data;
@@ -145,7 +146,7 @@ nvc0_ram_calc(struct nouveau_fb *pfb, u32 freq)
 
 	/* lookup memory config data relevant to the target frequency */
 	rammap.data = nvbios_rammapEm(bios, freq / 1000, &ver, &rammap.size,
-				     &cnt, &ramcfg.size);
+				     &cnt, &ramcfg.size, &cfg);
 	if (!rammap.data || ver != 0x10 || rammap.size < 0x0e) {
 		nv_error(pfb, "invalid/missing rammap entry\n");
 		return -EINVAL;
@@ -483,9 +484,9 @@ nvc0_ram_get(struct nouveau_fb *pfb, u64 size, u32 align, u32 ncmin,
 
 	do {
 		if (back)
-			ret = nouveau_mm_tail(mm, 1, size, ncmin, align, &r);
+			ret = nouveau_mm_tail(mm, 0, 1, size, ncmin, align, &r);
 		else
-			ret = nouveau_mm_head(mm, 1, size, ncmin, align, &r);
+			ret = nouveau_mm_head(mm, 0, 1, size, ncmin, align, &r);
 		if (ret) {
 			mutex_unlock(&pfb->base.mutex);
 			pfb->ram->put(pfb, &mem);
@@ -562,7 +563,7 @@ nvc0_ram_create_(struct nouveau_object *parent, struct nouveau_object *engine,
 		offset = (0x0200000000ULL >> 12) + (bsize << 8);
 		length = (ram->size >> 12) - ((bsize * parts) << 8) - rsvd_tail;
 
-		ret = nouveau_mm_init(&pfb->vram, offset, length, 0);
+		ret = nouveau_mm_init(&pfb->vram, offset, length, 1);
 		if (ret)
 			nouveau_mm_fini(&pfb->vram);
 	}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnve0.c b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnve0.c
index c5b46e302319..6bae474abb44 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/ramnve0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/ramnve0.c
@@ -29,6 +29,8 @@
 #include <subdev/bios/init.h>
 #include <subdev/bios/rammap.h>
 #include <subdev/bios/timing.h>
+#include <subdev/bios/M0205.h>
+#include <subdev/bios/M0209.h>
 
 #include <subdev/clock.h>
 #include <subdev/clock/pll.h>
@@ -41,14 +43,6 @@
 
 #include "ramfuc.h"
 
-/* binary driver only executes this path if the condition (a) is true
- * for any configuration (combination of rammap+ramcfg+timing) that
- * can be reached on a given card.  for now, we will execute the branch
- * unconditionally in the hope that a "false everywhere" in the bios
- * tables doesn't actually mean "don't touch this".
- */
-#define NOTE00(a) 1
-
 struct nve0_ramfuc {
 	struct ramfuc base;
 
@@ -134,10 +128,12 @@ struct nve0_ram {
 	struct nouveau_ram base;
 	struct nve0_ramfuc fuc;
 
+	struct list_head cfg;
 	u32 parts;
 	u32 pmask;
 	u32 pnuts;
 
+	struct nvbios_ramcfg diff;
 	int from;
 	int mode;
 	int N1, fN1, M1, P1;
@@ -241,7 +237,7 @@ nve0_ram_nuts(struct nve0_ram *ram, struct ramfuc_reg *reg,
 {
 	struct nve0_fb_priv *priv = (void *)nouveau_fb(ram);
 	struct ramfuc *fuc = &ram->fuc.base;
-	u32 addr = 0x110000 + (reg->addr[0] & 0xfff);
+	u32 addr = 0x110000 + (reg->addr & 0xfff);
 	u32 mask = _mask | _copy;
 	u32 data = (_data & _mask) | (reg->data & _copy);
 	u32 i;
@@ -268,6 +264,7 @@ nve0_ram_calc_gddr5(struct nouveau_fb *pfb, u32 freq)
 	u32 mask, data;
 
 	ram_mask(fuc, 0x10f808, 0x40000000, 0x40000000);
+	ram_block(fuc);
 	ram_wr32(fuc, 0x62c000, 0x0f0f0000);
 
 	/* MR1: turn termination on early, for some reason.. */
@@ -478,7 +475,7 @@ nve0_ram_calc_gddr5(struct nouveau_fb *pfb, u32 freq)
 	ram_mask(fuc, 0x10f2e8, 0xffffffff, next->bios.timing[9]);
 
 	data = mask = 0x00000000;
-	if (NOTE00(ramcfg_08_20)) {
+	if (ram->diff.ramcfg_11_08_20) {
 		if (next->bios.ramcfg_11_08_20)
 			data |= 0x01000000;
 		mask |= 0x01000000;
@@ -486,11 +483,11 @@ nve0_ram_calc_gddr5(struct nouveau_fb *pfb, u32 freq)
 	ram_mask(fuc, 0x10f200, mask, data);
 
 	data = mask = 0x00000000;
-	if (NOTE00(ramcfg_02_03 != 0)) {
+	if (ram->diff.ramcfg_11_02_03) {
 		data |= next->bios.ramcfg_11_02_03 << 8;
 		mask |= 0x00000300;
 	}
-	if (NOTE00(ramcfg_01_10)) {
+	if (ram->diff.ramcfg_11_01_10) {
 		if (next->bios.ramcfg_11_01_10)
 			data |= 0x70000000;
 		mask |= 0x70000000;
@@ -498,11 +495,11 @@ nve0_ram_calc_gddr5(struct nouveau_fb *pfb, u32 freq)
 	ram_mask(fuc, 0x10f604, mask, data);
 
 	data = mask = 0x00000000;
-	if (NOTE00(timing_30_07 != 0)) {
+	if (ram->diff.timing_20_30_07) {
 		data |= next->bios.timing_20_30_07 << 28;
 		mask |= 0x70000000;
 	}
-	if (NOTE00(ramcfg_01_01)) {
+	if (ram->diff.ramcfg_11_01_01) {
 		if (next->bios.ramcfg_11_01_01)
 			data |= 0x00000100;
 		mask |= 0x00000100;
@@ -510,11 +507,11 @@ nve0_ram_calc_gddr5(struct nouveau_fb *pfb, u32 freq)
 	ram_mask(fuc, 0x10f614, mask, data);
 
 	data = mask = 0x00000000;
-	if (NOTE00(timing_30_07 != 0)) {
+	if (ram->diff.timing_20_30_07) {
 		data |= next->bios.timing_20_30_07 << 28;
 		mask |= 0x70000000;
 	}
-	if (NOTE00(ramcfg_01_02)) {
+	if (ram->diff.ramcfg_11_01_02) {
 		if (next->bios.ramcfg_11_01_02)
 			data |= 0x00000100;
 		mask |= 0x00000100;
@@ -548,11 +545,11 @@ nve0_ram_calc_gddr5(struct nouveau_fb *pfb, u32 freq)
 	ram_wr32(fuc, 0x10f870, 0x11111111 * next->bios.ramcfg_11_03_0f);
 
 	data = mask = 0x00000000;
-	if (NOTE00(ramcfg_02_03 != 0)) {
+	if (ram->diff.ramcfg_11_02_03) {
 		data |= next->bios.ramcfg_11_02_03;
 		mask |= 0x00000003;
 	}
-	if (NOTE00(ramcfg_01_10)) {
+	if (ram->diff.ramcfg_11_01_10) {
 		if (next->bios.ramcfg_11_01_10)
 			data |= 0x00000004;
 		mask |= 0x00000004;
@@ -666,6 +663,7 @@ nve0_ram_calc_gddr5(struct nouveau_fb *pfb, u32 freq)
 	if (next->bios.ramcfg_11_07_02)
 		nve0_ram_train(fuc, 0x80020000, 0x01000000);
 
+	ram_unblock(fuc);
 	ram_wr32(fuc, 0x62c000, 0x0f0f0f00);
 
 	if (next->bios.rammap_11_08_01)
@@ -695,6 +693,7 @@ nve0_ram_calc_sddr3(struct nouveau_fb *pfb, u32 freq)
 	u32 mask, data;
 
 	ram_mask(fuc, 0x10f808, 0x40000000, 0x40000000);
+	ram_block(fuc);
 	ram_wr32(fuc, 0x62c000, 0x0f0f0000);
 
 	if (vc == 1 && ram_have(fuc, gpio2E)) {
@@ -917,6 +916,7 @@ nve0_ram_calc_sddr3(struct nouveau_fb *pfb, u32 freq)
 	ram_mask(fuc, 0x10f200, 0x80000000, 0x00000000);
 	ram_nsec(fuc, 1000);
 
+	ram_unblock(fuc);
 	ram_wr32(fuc, 0x62c000, 0x0f0f0f00);
 
 	if (next->bios.rammap_11_08_01)
@@ -932,58 +932,24 @@ nve0_ram_calc_sddr3(struct nouveau_fb *pfb, u32 freq)
  ******************************************************************************/
 
 static int
-nve0_ram_calc_data(struct nouveau_fb *pfb, u32 freq,
+nve0_ram_calc_data(struct nouveau_fb *pfb, u32 khz,
 		   struct nouveau_ram_data *data)
 {
-	struct nouveau_bios *bios = nouveau_bios(pfb);
 	struct nve0_ram *ram = (void *)pfb->ram;
-	u8 strap, cnt, len;
-
-	/* lookup memory config data relevant to the target frequency */
-	ram->base.rammap.data = nvbios_rammapEp(bios, freq / 1000,
-					       &ram->base.rammap.version,
-					       &ram->base.rammap.size,
-					       &cnt, &len, &data->bios);
-	if (!ram->base.rammap.data || ram->base.rammap.version != 0x11 ||
-	     ram->base.rammap.size < 0x09) {
-		nv_error(pfb, "invalid/missing rammap entry\n");
-		return -EINVAL;
-	}
-
-	/* locate specific data set for the attached memory */
-	strap = nvbios_ramcfg_index(nv_subdev(pfb));
-	ram->base.ramcfg.data = nvbios_rammapSp(bios, ram->base.rammap.data,
-						ram->base.rammap.version,
-						ram->base.rammap.size,
-						cnt, len, strap,
-						&ram->base.ramcfg.version,
-						&ram->base.ramcfg.size,
-						&data->bios);
-	if (!ram->base.ramcfg.data || ram->base.ramcfg.version != 0x11 ||
-	     ram->base.ramcfg.size < 0x08) {
-		nv_error(pfb, "invalid/missing ramcfg entry\n");
-		return -EINVAL;
-	}
-
-	/* lookup memory timings, if bios says they're present */
-	strap = nv_ro08(bios, ram->base.ramcfg.data + 0x00);
-	if (strap != 0xff) {
-		ram->base.timing.data =
-			nvbios_timingEp(bios, strap, &ram->base.timing.version,
-				       &ram->base.timing.size, &cnt, &len,
-				       &data->bios);
-		if (!ram->base.timing.data ||
-		     ram->base.timing.version != 0x20 ||
-		     ram->base.timing.size < 0x33) {
-			nv_error(pfb, "invalid/missing timing entry\n");
-			return -EINVAL;
+	struct nouveau_ram_data *cfg;
+	u32 mhz = khz / 1000;
+
+	list_for_each_entry(cfg, &ram->cfg, head) {
+		if (mhz >= cfg->bios.rammap_min &&
+		    mhz <= cfg->bios.rammap_max) {
+			*data = *cfg;
+			data->freq = khz;
+			return 0;
 		}
-	} else {
-		ram->base.timing.data = 0;
 	}
 
-	data->freq = freq;
-	return 0;
+	nv_error(ram, "ramcfg data for %dMHz not found\n", mhz);
+	return -EINVAL;
 }
 
 static int
@@ -1106,13 +1072,99 @@ nve0_ram_calc(struct nouveau_fb *pfb, u32 freq)
 	return nve0_ram_calc_xits(pfb, ram->base.next);
 }
 
+static void
+nve0_ram_prog_0(struct nouveau_fb *pfb, u32 freq)
+{
+	struct nve0_ram *ram = (void *)pfb->ram;
+	struct nouveau_ram_data *cfg;
+	u32 mhz = freq / 1000;
+	u32 mask, data;
+
+	list_for_each_entry(cfg, &ram->cfg, head) {
+		if (mhz >= cfg->bios.rammap_min &&
+		    mhz <= cfg->bios.rammap_max)
+			break;
+	}
+
+	if (&cfg->head == &ram->cfg)
+		return;
+
+	if (mask = 0, data = 0, ram->diff.rammap_11_0a_03fe) {
+		data |= cfg->bios.rammap_11_0a_03fe << 12;
+		mask |= 0x001ff000;
+	}
+	if (ram->diff.rammap_11_09_01ff) {
+		data |= cfg->bios.rammap_11_09_01ff;
+		mask |= 0x000001ff;
+	}
+	nv_mask(pfb, 0x10f468, mask, data);
+
+	if (mask = 0, data = 0, ram->diff.rammap_11_0a_0400) {
+		data |= cfg->bios.rammap_11_0a_0400;
+		mask |= 0x00000001;
+	}
+	nv_mask(pfb, 0x10f420, mask, data);
+
+	if (mask = 0, data = 0, ram->diff.rammap_11_0a_0800) {
+		data |= cfg->bios.rammap_11_0a_0800;
+		mask |= 0x00000001;
+	}
+	nv_mask(pfb, 0x10f430, mask, data);
+
+	if (mask = 0, data = 0, ram->diff.rammap_11_0b_01f0) {
+		data |= cfg->bios.rammap_11_0b_01f0;
+		mask |= 0x0000001f;
+	}
+	nv_mask(pfb, 0x10f400, mask, data);
+
+	if (mask = 0, data = 0, ram->diff.rammap_11_0b_0200) {
+		data |= cfg->bios.rammap_11_0b_0200 << 9;
+		mask |= 0x00000200;
+	}
+	nv_mask(pfb, 0x10f410, mask, data);
+
+	if (mask = 0, data = 0, ram->diff.rammap_11_0d) {
+		data |= cfg->bios.rammap_11_0d << 16;
+		mask |= 0x00ff0000;
+	}
+	if (ram->diff.rammap_11_0f) {
+		data |= cfg->bios.rammap_11_0f << 8;
+		mask |= 0x0000ff00;
+	}
+	nv_mask(pfb, 0x10f440, mask, data);
+
+	if (mask = 0, data = 0, ram->diff.rammap_11_0e) {
+		data |= cfg->bios.rammap_11_0e << 8;
+		mask |= 0x0000ff00;
+	}
+	if (ram->diff.rammap_11_0b_0800) {
+		data |= cfg->bios.rammap_11_0b_0800 << 7;
+		mask |= 0x00000080;
+	}
+	if (ram->diff.rammap_11_0b_0400) {
+		data |= cfg->bios.rammap_11_0b_0400 << 5;
+		mask |= 0x00000020;
+	}
+	nv_mask(pfb, 0x10f444, mask, data);
+}
+
 static int
 nve0_ram_prog(struct nouveau_fb *pfb)
 {
 	struct nouveau_device *device = nv_device(pfb);
 	struct nve0_ram *ram = (void *)pfb->ram;
 	struct nve0_ramfuc *fuc = &ram->fuc;
-	ram_exec(fuc, nouveau_boolopt(device->cfgopt, "NvMemExec", true));
+	struct nouveau_ram_data *next = ram->base.next;
+
+	if (!nouveau_boolopt(device->cfgopt, "NvMemExec", true)) {
+		ram_exec(fuc, false);
+		return (ram->base.next == &ram->base.xition);
+	}
+
+	nve0_ram_prog_0(pfb, 1000);
+	ram_exec(fuc, true);
+	nve0_ram_prog_0(pfb, next->freq);
+
 	return (ram->base.next == &ram->base.xition);
 }
 
@@ -1125,24 +1177,147 @@ nve0_ram_tidy(struct nouveau_fb *pfb)
 	ram_exec(fuc, false);
 }
 
+struct nve0_ram_train {
+	u16 mask;
+	struct nvbios_M0209S remap;
+	struct nvbios_M0209S type00;
+	struct nvbios_M0209S type01;
+	struct nvbios_M0209S type04;
+	struct nvbios_M0209S type06;
+	struct nvbios_M0209S type07;
+	struct nvbios_M0209S type08;
+	struct nvbios_M0209S type09;
+};
+
+static int
+nve0_ram_train_type(struct nouveau_fb *pfb, int i, u8 ramcfg,
+		    struct nve0_ram_train *train)
+{
+	struct nouveau_bios *bios = nouveau_bios(pfb);
+	struct nvbios_M0205E M0205E;
+	struct nvbios_M0205S M0205S;
+	struct nvbios_M0209E M0209E;
+	struct nvbios_M0209S *remap = &train->remap;
+	struct nvbios_M0209S *value;
+	u8  ver, hdr, cnt, len;
+	u32 data;
+
+	/* determine type of data for this index */
+	if (!(data = nvbios_M0205Ep(bios, i, &ver, &hdr, &cnt, &len, &M0205E)))
+		return -ENOENT;
+
+	switch (M0205E.type) {
+	case 0x00: value = &train->type00; break;
+	case 0x01: value = &train->type01; break;
+	case 0x04: value = &train->type04; break;
+	case 0x06: value = &train->type06; break;
+	case 0x07: value = &train->type07; break;
+	case 0x08: value = &train->type08; break;
+	case 0x09: value = &train->type09; break;
+	default:
+		return 0;
+	}
+
+	/* training data index determined by ramcfg strap */
+	if (!(data = nvbios_M0205Sp(bios, i, ramcfg, &ver, &hdr, &M0205S)))
+		return -EINVAL;
+	i = M0205S.data;
+
+	/* training data format information */
+	if (!(data = nvbios_M0209Ep(bios, i, &ver, &hdr, &cnt, &len, &M0209E)))
+		return -EINVAL;
+
+	/* ... and the raw data */
+	if (!(data = nvbios_M0209Sp(bios, i, 0, &ver, &hdr, value)))
+		return -EINVAL;
+
+	if (M0209E.v02_07 == 2) {
+		/* of course! why wouldn't we have a pointer to another entry
+		 * in the same table, and use the first one as an array of
+		 * remap indices...
+		 */
+		if (!(data = nvbios_M0209Sp(bios, M0209E.v03, 0, &ver, &hdr,
+					    remap)))
+			return -EINVAL;
+
+		for (i = 0; i < ARRAY_SIZE(value->data); i++)
+			value->data[i] = remap->data[value->data[i]];
+	} else
+	if (M0209E.v02_07 != 1)
+		return -EINVAL;
+
+	train->mask |= 1 << M0205E.type;
+	return 0;
+}
+
+static int
+nve0_ram_train_init_0(struct nouveau_fb *pfb, struct nve0_ram_train *train)
+{
+	int i, j;
+
+	if ((train->mask & 0x03d3) != 0x03d3) {
+		nv_warn(pfb, "missing link training data\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < 0x30; i++) {
+		for (j = 0; j < 8; j += 4) {
+			nv_wr32(pfb, 0x10f968 + j, 0x00000000 | (i << 8));
+			nv_wr32(pfb, 0x10f920 + j, 0x00000000 |
+						   train->type08.data[i] << 4 |
+						   train->type06.data[i]);
+			nv_wr32(pfb, 0x10f918 + j, train->type00.data[i]);
+			nv_wr32(pfb, 0x10f920 + j, 0x00000100 |
+						   train->type09.data[i] << 4 |
+						   train->type07.data[i]);
+			nv_wr32(pfb, 0x10f918 + j, train->type01.data[i]);
+		}
+	}
+
+	for (j = 0; j < 8; j += 4) {
+		for (i = 0; i < 0x100; i++) {
+			nv_wr32(pfb, 0x10f968 + j, i);
+			nv_wr32(pfb, 0x10f900 + j, train->type04.data[i]);
+		}
+	}
+
+	return 0;
+}
+
+static int
+nve0_ram_train_init(struct nouveau_fb *pfb)
+{
+	u8 ramcfg = nvbios_ramcfg_index(nv_subdev(pfb));
+	struct nve0_ram_train *train;
+	int ret = -ENOMEM, i;
+
+	if ((train = kzalloc(sizeof(*train), GFP_KERNEL))) {
+		for (i = 0; i < 0x100; i++) {
+			ret = nve0_ram_train_type(pfb, i, ramcfg, train);
+			if (ret && ret != -ENOENT)
+				break;
+		}
+	}
+
+	switch (pfb->ram->type) {
+	case NV_MEM_TYPE_GDDR5:
+		ret = nve0_ram_train_init_0(pfb, train);
+		break;
+	default:
+		ret = 0;
+		break;
+	}
+
+	kfree(train);
+	return ret;
+}
+
 int
 nve0_ram_init(struct nouveau_object *object)
 {
 	struct nouveau_fb *pfb = (void *)object->parent;
 	struct nve0_ram *ram   = (void *)object;
 	struct nouveau_bios *bios = nouveau_bios(pfb);
-	static const u8  train0[] = {
-		0x00, 0xff, 0xff, 0x00, 0xff, 0x00,
-		0x00, 0xff, 0xff, 0x00, 0xff, 0x00,
-	};
-	static const u32 train1[] = {
-		0x00000000, 0xffffffff,
-		0x55555555, 0xaaaaaaaa,
-		0x33333333, 0xcccccccc,
-		0xf0f0f0f0, 0x0f0f0f0f,
-		0x00ff00ff, 0xff00ff00,
-		0x0000ffff, 0xffff0000,
-	};
 	u8  ver, hdr, cnt, len, snr, ssz;
 	u32 data, save;
 	int ret, i;
@@ -1168,51 +1343,107 @@ nve0_ram_init(struct nouveau_object *object)
 
 	cnt  = nv_ro08(bios, data + 0x14); /* guess at count */
 	data = nv_ro32(bios, data + 0x10); /* guess u32... */
-	save = nv_rd32(pfb, 0x10f65c);
-	for (i = 0; i < cnt; i++) {
-		nv_mask(pfb, 0x10f65c, 0x000000f0, i << 4);
-		nvbios_exec(&(struct nvbios_init) {
-				.subdev = nv_subdev(pfb),
-				.bios = bios,
-				.offset = nv_ro32(bios, data), /* guess u32 */
-				.execute = 1,
-			    });
-		data += 4;
-	}
-	nv_wr32(pfb, 0x10f65c, save);
+	save = nv_rd32(pfb, 0x10f65c) & 0x000000f0;
+	for (i = 0; i < cnt; i++, data += 4) {
+		if (i != save >> 4) {
+			nv_mask(pfb, 0x10f65c, 0x000000f0, i << 4);
+			nvbios_exec(&(struct nvbios_init) {
+					.subdev = nv_subdev(pfb),
+					.bios = bios,
+					.offset = nv_ro32(bios, data),
+					.execute = 1,
+				    });
+		}
+	}
+	nv_mask(pfb, 0x10f65c, 0x000000f0, save);
 	nv_mask(pfb, 0x10f584, 0x11000000, 0x00000000);
+	nv_wr32(pfb, 0x10ecc0, 0xffffffff);
+	nv_mask(pfb, 0x10f160, 0x00000010, 0x00000010);
 
-	switch (ram->base.type) {
-	case NV_MEM_TYPE_GDDR5:
-		for (i = 0; i < 0x30; i++) {
-			nv_wr32(pfb, 0x10f968, 0x00000000 | (i << 8));
-			nv_wr32(pfb, 0x10f920, 0x00000000 | train0[i % 12]);
-			nv_wr32(pfb, 0x10f918,              train1[i % 12]);
-			nv_wr32(pfb, 0x10f920, 0x00000100 | train0[i % 12]);
-			nv_wr32(pfb, 0x10f918,              train1[i % 12]);
-
-			nv_wr32(pfb, 0x10f96c, 0x00000000 | (i << 8));
-			nv_wr32(pfb, 0x10f924, 0x00000000 | train0[i % 12]);
-			nv_wr32(pfb, 0x10f91c,              train1[i % 12]);
-			nv_wr32(pfb, 0x10f924, 0x00000100 | train0[i % 12]);
-			nv_wr32(pfb, 0x10f91c,              train1[i % 12]);
-		}
+	return nve0_ram_train_init(pfb);
+}
 
-		for (i = 0; i < 0x100; i++) {
-			nv_wr32(pfb, 0x10f968, i);
-			nv_wr32(pfb, 0x10f900, train1[2 + (i & 1)]);
-		}
+static int
+nve0_ram_ctor_data(struct nve0_ram *ram, u8 ramcfg, int i)
+{
+	struct nouveau_fb *pfb = (void *)nv_object(ram)->parent;
+	struct nouveau_bios *bios = nouveau_bios(pfb);
+	struct nouveau_ram_data *cfg;
+	struct nvbios_ramcfg *d = &ram->diff;
+	struct nvbios_ramcfg *p, *n;
+	u8  ver, hdr, cnt, len;
+	u32 data;
+	int ret;
 
-		for (i = 0; i < 0x100; i++) {
-			nv_wr32(pfb, 0x10f96c, i);
-			nv_wr32(pfb, 0x10f900, train1[2 + (i & 1)]);
-		}
-		break;
-	default:
-		break;
+	if (!(cfg = kmalloc(sizeof(*cfg), GFP_KERNEL)))
+		return -ENOMEM;
+	p = &list_last_entry(&ram->cfg, typeof(*cfg), head)->bios;
+	n = &cfg->bios;
+
+	/* memory config data for a range of target frequencies */
+	data = nvbios_rammapEp(bios, i, &ver, &hdr, &cnt, &len, &cfg->bios);
+	if (ret = -ENOENT, !data)
+		goto done;
+	if (ret = -ENOSYS, ver != 0x11 || hdr < 0x12)
+		goto done;
+
+	/* ... and a portion specific to the attached memory */
+	data = nvbios_rammapSp(bios, data, ver, hdr, cnt, len, ramcfg,
+			       &ver, &hdr, &cfg->bios);
+	if (ret = -EINVAL, !data)
+		goto done;
+	if (ret = -ENOSYS, ver != 0x11 || hdr < 0x0a)
+		goto done;
+
+	/* lookup memory timings, if bios says they're present */
+	if (cfg->bios.ramcfg_timing != 0xff) {
+		data = nvbios_timingEp(bios, cfg->bios.ramcfg_timing,
+				       &ver, &hdr, &cnt, &len,
+				       &cfg->bios);
+		if (ret = -EINVAL, !data)
+			goto done;
+		if (ret = -ENOSYS, ver != 0x20 || hdr < 0x33)
+			goto done;
 	}
 
-	return 0;
+	list_add_tail(&cfg->head, &ram->cfg);
+	if (ret = 0, i == 0)
+		goto done;
+
+	d->rammap_11_0a_03fe |= p->rammap_11_0a_03fe != n->rammap_11_0a_03fe;
+	d->rammap_11_09_01ff |= p->rammap_11_09_01ff != n->rammap_11_09_01ff;
+	d->rammap_11_0a_0400 |= p->rammap_11_0a_0400 != n->rammap_11_0a_0400;
+	d->rammap_11_0a_0800 |= p->rammap_11_0a_0800 != n->rammap_11_0a_0800;
+	d->rammap_11_0b_01f0 |= p->rammap_11_0b_01f0 != n->rammap_11_0b_01f0;
+	d->rammap_11_0b_0200 |= p->rammap_11_0b_0200 != n->rammap_11_0b_0200;
+	d->rammap_11_0d |= p->rammap_11_0d != n->rammap_11_0d;
+	d->rammap_11_0f |= p->rammap_11_0f != n->rammap_11_0f;
+	d->rammap_11_0e |= p->rammap_11_0e != n->rammap_11_0e;
+	d->rammap_11_0b_0800 |= p->rammap_11_0b_0800 != n->rammap_11_0b_0800;
+	d->rammap_11_0b_0400 |= p->rammap_11_0b_0400 != n->rammap_11_0b_0400;
+	d->ramcfg_11_01_01 |= p->ramcfg_11_01_01 != n->ramcfg_11_01_01;
+	d->ramcfg_11_01_02 |= p->ramcfg_11_01_02 != n->ramcfg_11_01_02;
+	d->ramcfg_11_01_10 |= p->ramcfg_11_01_10 != n->ramcfg_11_01_10;
+	d->ramcfg_11_02_03 |= p->ramcfg_11_02_03 != n->ramcfg_11_02_03;
+	d->ramcfg_11_08_20 |= p->ramcfg_11_08_20 != n->ramcfg_11_08_20;
+	d->timing_20_30_07 |= p->timing_20_30_07 != n->timing_20_30_07;
+done:
+	if (ret)
+		kfree(cfg);
+	return ret;
+}
+
+static void
+nve0_ram_dtor(struct nouveau_object *object)
+{
+	struct nve0_ram *ram = (void *)object;
+	struct nouveau_ram_data *cfg, *tmp;
+
+	list_for_each_entry_safe(cfg, tmp, &ram->cfg, head) {
+		kfree(cfg);
+	}
+
+	nouveau_ram_destroy(&ram->base);
 }
 
 static int
@@ -1226,6 +1457,7 @@ nve0_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	struct dcb_gpio_func func;
 	struct nve0_ram *ram;
 	int ret, i;
+	u8  ramcfg = nvbios_ramcfg_index(nv_subdev(pfb));
 	u32 tmp;
 
 	ret = nvc0_ram_create(parent, engine, oclass, 0x022554, &ram);
@@ -1233,6 +1465,8 @@ nve0_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
+	INIT_LIST_HEAD(&ram->cfg);
+
 	switch (ram->base.type) {
 	case NV_MEM_TYPE_DDR3:
 	case NV_MEM_TYPE_GDDR5:
@@ -1264,7 +1498,26 @@ nve0_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		}
 	}
 
-	// parse bios data for both pll's
+	/* parse bios data for all rammap table entries up-front, and
+	 * build information on whether certain fields differ between
+	 * any of the entries.
+	 *
+	 * the binary driver appears to completely ignore some fields
+	 * when all entries contain the same value.  at first, it was
+	 * hoped that these were mere optimisations and the bios init
+	 * tables had configured as per the values here, but there is
+	 * evidence now to suggest that this isn't the case and we do
+	 * need to treat this condition as a "don't touch" indicator.
+	 */
+	for (i = 0; !ret; i++) {
+		ret = nve0_ram_ctor_data(ram, ramcfg, i);
+		if (ret && ret != -ENOENT) {
+			nv_error(pfb, "failed to parse ramcfg data\n");
+			return ret;
+		}
+	}
+
+	/* parse bios data for both pll's */
 	ret = nvbios_pll_parse(bios, 0x0c, &ram->fuc.refpll);
 	if (ret) {
 		nv_error(pfb, "mclk refpll data not found\n");
@@ -1277,6 +1530,7 @@ nve0_ram_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 		return ret;
 	}
 
+	/* lookup memory voltage gpios */
 	ret = gpio->find(gpio, 0, 0x18, DCB_GPIO_UNUSED, &func);
 	if (ret == 0) {
 		ram->fuc.r_gpioMV = ramfuc_reg(0x00d610 + (func.line * 0x04));
@@ -1385,7 +1639,7 @@ nve0_ram_oclass = {
 	.handle = 0,
 	.ofuncs = &(struct nouveau_ofuncs) {
 		.ctor = nve0_ram_ctor,
-		.dtor = _nouveau_ram_dtor,
+		.dtor = nve0_ram_dtor,
 		.init = nve0_ram_init,
 		.fini = _nouveau_ram_fini,
 	}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/sddr2.c b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr2.c
new file mode 100644
index 000000000000..bb1eb8f3e639
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr2.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright 2014 Roy Spliet
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Roy Spliet <rspliet@eclipso.eu>
+ *          Ben Skeggs
+ */
+
+#include "priv.h"
+
+struct ramxlat {
+	int id;
+	u8 enc;
+};
+
+static inline int
+ramxlat(const struct ramxlat *xlat, int id)
+{
+	while (xlat->id >= 0) {
+		if (xlat->id == id)
+			return xlat->enc;
+		xlat++;
+	}
+	return -EINVAL;
+}
+
+static const struct ramxlat
+ramddr2_cl[] = {
+	{ 2, 2 }, { 3, 3 }, { 4, 4 }, { 5, 5 }, { 6, 6 },
+	/* The following are available in some, but not all DDR2 docs */
+	{ 7, 7 },
+	{ -1 }
+};
+
+static const struct ramxlat
+ramddr2_wr[] = {
+	{ 2, 1 }, { 3, 2 }, { 4, 3 }, { 5, 4 }, { 6, 5 },
+	/* The following are available in some, but not all DDR2 docs */
+	{ 7, 6 },
+	{ -1 }
+};
+
+int
+nouveau_sddr2_calc(struct nouveau_ram *ram)
+{
+	int CL, WR, DLL = 0, ODT = 0;
+
+	switch (ram->next->bios.timing_ver) {
+	case 0x10:
+		CL  = ram->next->bios.timing_10_CL;
+		WR  = ram->next->bios.timing_10_WR;
+		DLL = !ram->next->bios.ramcfg_10_02_40;
+		ODT = ram->next->bios.timing_10_ODT & 3;
+		break;
+	case 0x20:
+		CL  = (ram->next->bios.timing[1] & 0x0000001f);
+		WR  = (ram->next->bios.timing[2] & 0x007f0000) >> 16;
+		break;
+	default:
+		return -ENOSYS;
+	}
+
+	CL  = ramxlat(ramddr2_cl, CL);
+	WR  = ramxlat(ramddr2_wr, WR);
+	if (CL < 0 || WR < 0)
+		return -EINVAL;
+
+	ram->mr[0] &= ~0xf70;
+	ram->mr[0] |= (WR & 0x07) << 9;
+	ram->mr[0] |= (CL & 0x07) << 4;
+
+	ram->mr[1] &= ~0x045;
+	ram->mr[1] |= (ODT & 0x1) << 2;
+	ram->mr[1] |= (ODT & 0x2) << 5;
+	ram->mr[1] |= !DLL;
+	return 0;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c
index ebd4cd9c35d9..83949b11833a 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/fb/sddr3.c
@@ -20,9 +20,9 @@
  * OTHER DEALINGS IN THE SOFTWARE.
  *
  * Authors: Ben Skeggs <bskeggs@redhat.com>
+ * 	    Roy Spliet <rspliet@eclipso.eu>
  */
 
-#include <subdev/bios.h>
 #include "priv.h"
 
 struct ramxlat {
@@ -69,31 +69,52 @@ ramddr3_cwl[] = {
 int
 nouveau_sddr3_calc(struct nouveau_ram *ram)
 {
-	struct nouveau_bios *bios = nouveau_bios(ram);
-	int WL, CL, WR;
+	int CWL, CL, WR, DLL = 0, ODT = 0;
 
-	switch (!!ram->timing.data * ram->timing.version) {
+	switch (ram->next->bios.timing_ver) {
+	case 0x10:
+		if (ram->next->bios.timing_hdr < 0x17) {
+			/* XXX: NV50: Get CWL from the timing register */
+			return -ENOSYS;
+		}
+		CWL = ram->next->bios.timing_10_CWL;
+		CL  = ram->next->bios.timing_10_CL;
+		WR  = ram->next->bios.timing_10_WR;
+		DLL = !ram->next->bios.ramcfg_10_02_40;
+		ODT = ram->next->bios.timing_10_ODT;
+		break;
 	case 0x20:
-		WL = (nv_ro16(bios, ram->timing.data + 0x04) & 0x0f80) >> 7;
-		CL =  nv_ro08(bios, ram->timing.data + 0x04) & 0x1f;
-		WR =  nv_ro08(bios, ram->timing.data + 0x0a) & 0x7f;
+		CWL = (ram->next->bios.timing[1] & 0x00000f80) >> 7;
+		CL  = (ram->next->bios.timing[1] & 0x0000001f) >> 0;
+		WR  = (ram->next->bios.timing[2] & 0x007f0000) >> 16;
+		/* XXX: Get these values from the VBIOS instead */
+		DLL = !(ram->mr[1] & 0x1);
+		ODT =   (ram->mr[1] & 0x004) >> 2 |
+			(ram->mr[1] & 0x040) >> 5 |
+		        (ram->mr[1] & 0x200) >> 7;
 		break;
 	default:
 		return -ENOSYS;
 	}
 
-	WL = ramxlat(ramddr3_cwl, WL);
-	CL = ramxlat(ramddr3_cl, CL);
-	WR = ramxlat(ramddr3_wr, WR);
-	if (WL < 0 || CL < 0 || WR < 0)
+	CWL = ramxlat(ramddr3_cwl, CWL);
+	CL  = ramxlat(ramddr3_cl, CL);
+	WR  = ramxlat(ramddr3_wr, WR);
+	if (CL < 0 || CWL < 0 || WR < 0)
 		return -EINVAL;
 
-	ram->mr[0] &= ~0xe74;
+	ram->mr[0] &= ~0xf74;
 	ram->mr[0] |= (WR & 0x07) << 9;
 	ram->mr[0] |= (CL & 0x0e) << 3;
 	ram->mr[0] |= (CL & 0x01) << 2;
 
+	ram->mr[1] &= ~0x245;
+	ram->mr[1] |= (ODT & 0x1) << 2;
+	ram->mr[1] |= (ODT & 0x2) << 5;
+	ram->mr[1] |= (ODT & 0x4) << 7;
+	ram->mr[1] |= !DLL;
+
 	ram->mr[2] &= ~0x038;
-	ram->mr[2] |= (WL & 0x07) << 3;
+	ram->mr[2] |= (CWL & 0x07) << 3;
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fuse/base.c b/drivers/gpu/drm/nouveau/core/subdev/fuse/base.c
new file mode 100644
index 000000000000..9e8e92127715
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/fuse/base.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright 2014 Martin Peres
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Martin Peres
+ */
+
+#include <subdev/fuse.h>
+
+int
+_nouveau_fuse_init(struct nouveau_object *object)
+{
+	struct nouveau_fuse *fuse = (void *)object;
+	return nouveau_subdev_init(&fuse->base);
+}
+
+void
+_nouveau_fuse_dtor(struct nouveau_object *object)
+{
+	struct nouveau_fuse *fuse = (void *)object;
+	nouveau_subdev_destroy(&fuse->base);
+}
+
+int
+nouveau_fuse_create_(struct nouveau_object *parent,
+		     struct nouveau_object *engine,
+		     struct nouveau_oclass *oclass, int length, void **pobject)
+{
+	struct nouveau_fuse *fuse;
+	int ret;
+
+	ret = nouveau_subdev_create_(parent, engine, oclass, 0, "FUSE",
+				     "fuse", length, pobject);
+	fuse = *pobject;
+
+	return ret;
+}
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fuse/g80.c b/drivers/gpu/drm/nouveau/core/subdev/fuse/g80.c
new file mode 100644
index 000000000000..a374ade485be
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/fuse/g80.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright 2014 Martin Peres
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Martin Peres
+ */
+
+#include "priv.h"
+
+struct g80_fuse_priv {
+	struct nouveau_fuse base;
+
+	spinlock_t fuse_enable_lock;
+};
+
+static u32
+g80_fuse_rd32(struct nouveau_object *object, u64 addr)
+{
+	struct g80_fuse_priv *priv = (void *)object;
+	unsigned long flags;
+	u32 fuse_enable, val;
+
+	spin_lock_irqsave(&priv->fuse_enable_lock, flags);
+
+	/* racy if another part of nouveau start writing to this reg */
+	fuse_enable = nv_mask(priv, 0x1084, 0x800, 0x800);
+	val = nv_rd32(priv, 0x21000 + addr);
+	nv_wr32(priv, 0x1084, fuse_enable);
+
+	spin_unlock_irqrestore(&priv->fuse_enable_lock, flags);
+
+	return val;
+}
+
+
+static int
+g80_fuse_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
+	       struct nouveau_oclass *oclass, void *data, u32 size,
+	       struct nouveau_object **pobject)
+{
+	struct g80_fuse_priv *priv;
+	int ret;
+
+	ret = nouveau_fuse_create(parent, engine, oclass, &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	spin_lock_init(&priv->fuse_enable_lock);
+
+	return 0;
+}
+
+struct nouveau_oclass
+g80_fuse_oclass = {
+	.handle = NV_SUBDEV(FUSE, 0x50),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = g80_fuse_ctor,
+		.dtor = _nouveau_fuse_dtor,
+		.init = _nouveau_fuse_init,
+		.fini = _nouveau_fuse_fini,
+		.rd32 = g80_fuse_rd32,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fuse/gf100.c b/drivers/gpu/drm/nouveau/core/subdev/fuse/gf100.c
new file mode 100644
index 000000000000..5ed03f54b3d4
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/fuse/gf100.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2014 Martin Peres
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Martin Peres
+ */
+
+#include "priv.h"
+
+struct gf100_fuse_priv {
+	struct nouveau_fuse base;
+
+	spinlock_t fuse_enable_lock;
+};
+
+static u32
+gf100_fuse_rd32(struct nouveau_object *object, u64 addr)
+{
+	struct gf100_fuse_priv *priv = (void *)object;
+	unsigned long flags;
+	u32 fuse_enable, unk, val;
+
+	spin_lock_irqsave(&priv->fuse_enable_lock, flags);
+
+	/* racy if another part of nouveau start writing to these regs */
+	fuse_enable = nv_mask(priv, 0x22400, 0x800, 0x800);
+	unk = nv_mask(priv, 0x21000, 0x1, 0x1);
+	val = nv_rd32(priv, 0x21100 + addr);
+	nv_wr32(priv, 0x21000, unk);
+	nv_wr32(priv, 0x22400, fuse_enable);
+
+	spin_unlock_irqrestore(&priv->fuse_enable_lock, flags);
+
+	return val;
+}
+
+
+static int
+gf100_fuse_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
+	       struct nouveau_oclass *oclass, void *data, u32 size,
+	       struct nouveau_object **pobject)
+{
+	struct gf100_fuse_priv *priv;
+	int ret;
+
+	ret = nouveau_fuse_create(parent, engine, oclass, &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	spin_lock_init(&priv->fuse_enable_lock);
+
+	return 0;
+}
+
+struct nouveau_oclass
+gf100_fuse_oclass = {
+	.handle = NV_SUBDEV(FUSE, 0xC0),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = gf100_fuse_ctor,
+		.dtor = _nouveau_fuse_dtor,
+		.init = _nouveau_fuse_init,
+		.fini = _nouveau_fuse_fini,
+		.rd32 = gf100_fuse_rd32,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fuse/gm107.c b/drivers/gpu/drm/nouveau/core/subdev/fuse/gm107.c
new file mode 100644
index 000000000000..4f1a636c6538
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/fuse/gm107.c
@@ -0,0 +1,66 @@
+/*
+ * Copyright 2014 Martin Peres
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Martin Peres
+ */
+
+#include "priv.h"
+
+struct gm107_fuse_priv {
+	struct nouveau_fuse base;
+};
+
+static u32
+gm107_fuse_rd32(struct nouveau_object *object, u64 addr)
+{
+	struct gf100_fuse_priv *priv = (void *)object;
+
+	return nv_rd32(priv, 0x21100 + addr);
+}
+
+
+static int
+gm107_fuse_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
+	       struct nouveau_oclass *oclass, void *data, u32 size,
+	       struct nouveau_object **pobject)
+{
+	struct gm107_fuse_priv *priv;
+	int ret;
+
+	ret = nouveau_fuse_create(parent, engine, oclass, &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+struct nouveau_oclass
+gm107_fuse_oclass = {
+	.handle = NV_SUBDEV(FUSE, 0x117),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = gm107_fuse_ctor,
+		.dtor = _nouveau_fuse_dtor,
+		.init = _nouveau_fuse_init,
+		.fini = _nouveau_fuse_fini,
+		.rd32 = gm107_fuse_rd32,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/fuse/priv.h b/drivers/gpu/drm/nouveau/core/subdev/fuse/priv.h
new file mode 100644
index 000000000000..d2085411a5cb
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/fuse/priv.h
@@ -0,0 +1,9 @@
+#ifndef __NVKM_FUSE_PRIV_H__
+#define __NVKM_FUSE_PRIV_H__
+
+#include <subdev/fuse.h>
+
+int _nouveau_fuse_init(struct nouveau_object *object);
+void _nouveau_fuse_dtor(struct nouveau_object *object);
+
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/gpio/base.c b/drivers/gpu/drm/nouveau/core/subdev/gpio/base.c
index b1e3ed7c8beb..7ad99b763f4c 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/gpio/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/gpio/base.c
@@ -122,7 +122,8 @@ nouveau_gpio_intr_init(struct nvkm_event *event, int type, int index)
 }
 
 static int
-nouveau_gpio_intr_ctor(void *data, u32 size, struct nvkm_notify *notify)
+nouveau_gpio_intr_ctor(struct nouveau_object *object, void *data, u32 size,
+		       struct nvkm_notify *notify)
 {
 	struct nvkm_gpio_ntfy_req *req = data;
 	if (!WARN_ON(size != sizeof(*req))) {
diff --git a/drivers/gpu/drm/nouveau/core/subdev/gpio/nv92.c b/drivers/gpu/drm/nouveau/core/subdev/gpio/nv94.c
index 252083d376f5..cae404ccadac 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/gpio/nv92.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/gpio/nv94.c
@@ -25,7 +25,7 @@
 #include "priv.h"
 
 void
-nv92_gpio_intr_stat(struct nouveau_gpio *gpio, u32 *hi, u32 *lo)
+nv94_gpio_intr_stat(struct nouveau_gpio *gpio, u32 *hi, u32 *lo)
 {
 	u32 intr0 = nv_rd32(gpio, 0x00e054);
 	u32 intr1 = nv_rd32(gpio, 0x00e074);
@@ -38,7 +38,7 @@ nv92_gpio_intr_stat(struct nouveau_gpio *gpio, u32 *hi, u32 *lo)
 }
 
 void
-nv92_gpio_intr_mask(struct nouveau_gpio *gpio, u32 type, u32 mask, u32 data)
+nv94_gpio_intr_mask(struct nouveau_gpio *gpio, u32 type, u32 mask, u32 data)
 {
 	u32 inte0 = nv_rd32(gpio, 0x00e050);
 	u32 inte1 = nv_rd32(gpio, 0x00e070);
@@ -57,8 +57,8 @@ nv92_gpio_intr_mask(struct nouveau_gpio *gpio, u32 type, u32 mask, u32 data)
 }
 
 struct nouveau_oclass *
-nv92_gpio_oclass = &(struct nouveau_gpio_impl) {
-	.base.handle = NV_SUBDEV(GPIO, 0x92),
+nv94_gpio_oclass = &(struct nouveau_gpio_impl) {
+	.base.handle = NV_SUBDEV(GPIO, 0x94),
 	.base.ofuncs = &(struct nouveau_ofuncs) {
 		.ctor = _nouveau_gpio_ctor,
 		.dtor = _nouveau_gpio_dtor,
@@ -66,8 +66,8 @@ nv92_gpio_oclass = &(struct nouveau_gpio_impl) {
 		.fini = _nouveau_gpio_fini,
 	},
 	.lines = 32,
-	.intr_stat = nv92_gpio_intr_stat,
-	.intr_mask = nv92_gpio_intr_mask,
+	.intr_stat = nv94_gpio_intr_stat,
+	.intr_mask = nv94_gpio_intr_mask,
 	.drive = nv50_gpio_drive,
 	.sense = nv50_gpio_sense,
 	.reset = nv50_gpio_reset,
diff --git a/drivers/gpu/drm/nouveau/core/subdev/gpio/nvd0.c b/drivers/gpu/drm/nouveau/core/subdev/gpio/nvd0.c
index a4682b0956ad..480d6d2af770 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/gpio/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/gpio/nvd0.c
@@ -77,8 +77,8 @@ nvd0_gpio_oclass = &(struct nouveau_gpio_impl) {
 		.fini = _nouveau_gpio_fini,
 	},
 	.lines = 32,
-	.intr_stat = nv92_gpio_intr_stat,
-	.intr_mask = nv92_gpio_intr_mask,
+	.intr_stat = nv94_gpio_intr_stat,
+	.intr_mask = nv94_gpio_intr_mask,
 	.drive = nvd0_gpio_drive,
 	.sense = nvd0_gpio_sense,
 	.reset = nvd0_gpio_reset,
diff --git a/drivers/gpu/drm/nouveau/core/subdev/gpio/priv.h b/drivers/gpu/drm/nouveau/core/subdev/gpio/priv.h
index e1724dfc86ae..bff98b86e2b5 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/gpio/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/gpio/priv.h
@@ -56,8 +56,8 @@ void nv50_gpio_reset(struct nouveau_gpio *, u8);
 int  nv50_gpio_drive(struct nouveau_gpio *, int, int, int);
 int  nv50_gpio_sense(struct nouveau_gpio *, int);
 
-void nv92_gpio_intr_stat(struct nouveau_gpio *, u32 *, u32 *);
-void nv92_gpio_intr_mask(struct nouveau_gpio *, u32, u32, u32);
+void nv94_gpio_intr_stat(struct nouveau_gpio *, u32 *, u32 *);
+void nv94_gpio_intr_mask(struct nouveau_gpio *, u32, u32, u32);
 
 void nvd0_gpio_reset(struct nouveau_gpio *, u8);
 int  nvd0_gpio_drive(struct nouveau_gpio *, int, int, int);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c b/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
index a652cafde3d6..2b1bf545e488 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/i2c/base.c
@@ -23,6 +23,7 @@
  */
 
 #include <core/option.h>
+#include <core/object.h>
 #include <core/event.h>
 
 #include <subdev/bios.h>
@@ -346,7 +347,8 @@ nouveau_i2c_intr_init(struct nvkm_event *event, int type, int index)
 }
 
 static int
-nouveau_i2c_intr_ctor(void *data, u32 size, struct nvkm_notify *notify)
+nouveau_i2c_intr_ctor(struct nouveau_object *object, void *data, u32 size,
+		      struct nvkm_notify *notify)
 {
 	struct nvkm_i2c_ntfy_req *req = data;
 	if (!WARN_ON(size != sizeof(*req))) {
diff --git a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c
index 7b64befee48f..e8b1401c59c0 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/instmem/nv04.c
@@ -69,7 +69,7 @@ nv04_instobj_ctor(struct nouveau_object *parent, struct nouveau_object *engine,
 	if (ret)
 		return ret;
 
-	ret = nouveau_mm_head(&priv->heap, 1, args->size, args->size,
+	ret = nouveau_mm_head(&priv->heap, 0, 1, args->size, args->size,
 			      args->align, &node->mem);
 	if (ret)
 		return ret;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/ltc/base.c b/drivers/gpu/drm/nouveau/core/subdev/ltc/base.c
index 32ed442c5913..7fa331516f84 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/ltc/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/ltc/base.c
@@ -31,7 +31,7 @@ nvkm_ltc_tags_alloc(struct nouveau_ltc *ltc, u32 n,
 	struct nvkm_ltc_priv *priv = (void *)ltc;
 	int ret;
 
-	ret = nouveau_mm_head(&priv->tags, 1, n, n, 1, pnode);
+	ret = nouveau_mm_head(&priv->tags, 0, 1, n, n, 1, pnode);
 	if (ret)
 		*pnode = NULL;
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/ltc/gf100.c b/drivers/gpu/drm/nouveau/core/subdev/ltc/gf100.c
index b54b582e72c4..2db0977284f8 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/ltc/gf100.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/ltc/gf100.c
@@ -62,16 +62,38 @@ gf100_ltc_zbc_clear_depth(struct nvkm_ltc_priv *priv, int i, const u32 depth)
 	nv_wr32(priv, 0x17ea58, depth);
 }
 
+static const struct nouveau_bitfield
+gf100_ltc_lts_intr_name[] = {
+	{ 0x00000001, "IDLE_ERROR_IQ" },
+	{ 0x00000002, "IDLE_ERROR_CBC" },
+	{ 0x00000004, "IDLE_ERROR_TSTG" },
+	{ 0x00000008, "IDLE_ERROR_DSTG" },
+	{ 0x00000010, "EVICTED_CB" },
+	{ 0x00000020, "ILLEGAL_COMPSTAT" },
+	{ 0x00000040, "BLOCKLINEAR_CB" },
+	{ 0x00000100, "ECC_SEC_ERROR" },
+	{ 0x00000200, "ECC_DED_ERROR" },
+	{ 0x00000400, "DEBUG" },
+	{ 0x00000800, "ATOMIC_TO_Z" },
+	{ 0x00001000, "ILLEGAL_ATOMIC" },
+	{ 0x00002000, "BLKACTIVITY_ERR" },
+	{}
+};
+
 static void
-gf100_ltc_lts_isr(struct nvkm_ltc_priv *priv, int ltc, int lts)
+gf100_ltc_lts_intr(struct nvkm_ltc_priv *priv, int ltc, int lts)
 {
 	u32 base = 0x141000 + (ltc * 0x2000) + (lts * 0x400);
-	u32 stat = nv_rd32(priv, base + 0x020);
+	u32 intr = nv_rd32(priv, base + 0x020);
+	u32 stat = intr & 0x0000ffff;
 
 	if (stat) {
-		nv_info(priv, "LTC%d_LTS%d: 0x%08x\n", ltc, lts, stat);
-		nv_wr32(priv, base + 0x020, stat);
+		nv_info(priv, "LTC%d_LTS%d:", ltc, lts);
+		nouveau_bitfield_print(gf100_ltc_lts_intr_name, stat);
+		pr_cont("\n");
 	}
+
+	nv_wr32(priv, base + 0x020, intr);
 }
 
 void
@@ -84,20 +106,16 @@ gf100_ltc_intr(struct nouveau_subdev *subdev)
 	while (mask) {
 		u32 lts, ltc = __ffs(mask);
 		for (lts = 0; lts < priv->lts_nr; lts++)
-			gf100_ltc_lts_isr(priv, ltc, lts);
+			gf100_ltc_lts_intr(priv, ltc, lts);
 		mask &= ~(1 << ltc);
 	}
-
-	/* we do something horribly wrong and upset PMFB a lot, so mask off
-	 * interrupts from it after the first one until it's fixed
-	 */
-	nv_mask(priv, 0x000640, 0x02000000, 0x00000000);
 }
 
 static int
 gf100_ltc_init(struct nouveau_object *object)
 {
 	struct nvkm_ltc_priv *priv = (void *)object;
+	u32 lpg128 = !(nv_rd32(priv, 0x100c80) & 0x00000001);
 	int ret;
 
 	ret = nvkm_ltc_init(priv);
@@ -107,6 +125,7 @@ gf100_ltc_init(struct nouveau_object *object)
 	nv_mask(priv, 0x17e820, 0x00100000, 0x00000000); /* INTR_EN &= ~0x10 */
 	nv_wr32(priv, 0x17e8d8, priv->ltc_nr);
 	nv_wr32(priv, 0x17e8d4, priv->tag_base);
+	nv_mask(priv, 0x17e8c0, 0x00000002, lpg128 ? 0x00000002 : 0x00000000);
 	return 0;
 }
 
@@ -151,7 +170,7 @@ gf100_ltc_init_tag_ram(struct nouveau_fb *pfb, struct nvkm_ltc_priv *priv)
 	tag_size += tag_align;
 	tag_size  = (tag_size + 0xfff) >> 12; /* round up */
 
-	ret = nouveau_mm_tail(&pfb->vram, 1, tag_size, tag_size, 1,
+	ret = nouveau_mm_tail(&pfb->vram, 1, 1, tag_size, tag_size, 1,
 	                      &priv->tag_ram);
 	if (ret) {
 		priv->num_tags = 0;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/ltc/gk104.c b/drivers/gpu/drm/nouveau/core/subdev/ltc/gk104.c
index ea716569745d..b39b5d0eb8f9 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/ltc/gk104.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/ltc/gk104.c
@@ -28,6 +28,7 @@ static int
 gk104_ltc_init(struct nouveau_object *object)
 {
 	struct nvkm_ltc_priv *priv = (void *)object;
+	u32 lpg128 = !(nv_rd32(priv, 0x100c80) & 0x00000001);
 	int ret;
 
 	ret = nvkm_ltc_init(priv);
@@ -37,6 +38,7 @@ gk104_ltc_init(struct nouveau_object *object)
 	nv_wr32(priv, 0x17e8d8, priv->ltc_nr);
 	nv_wr32(priv, 0x17e000, priv->ltc_nr);
 	nv_wr32(priv, 0x17e8d4, priv->tag_base);
+	nv_mask(priv, 0x17e8c0, 0x00000002, lpg128 ? 0x00000002 : 0x00000000);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/ltc/gm107.c b/drivers/gpu/drm/nouveau/core/subdev/ltc/gm107.c
index 4761b2e9af00..89fc4238f50c 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/ltc/gm107.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/ltc/gm107.c
@@ -87,17 +87,13 @@ gm107_ltc_intr(struct nouveau_subdev *subdev)
 			gm107_ltc_lts_isr(priv, ltc, lts);
 		mask &= ~(1 << ltc);
 	}
-
-	/* we do something horribly wrong and upset PMFB a lot, so mask off
-	 * interrupts from it after the first one until it's fixed
-	 */
-	nv_mask(priv, 0x000640, 0x02000000, 0x00000000);
 }
 
 static int
 gm107_ltc_init(struct nouveau_object *object)
 {
 	struct nvkm_ltc_priv *priv = (void *)object;
+	u32 lpg128 = !(nv_rd32(priv, 0x100c80) & 0x00000001);
 	int ret;
 
 	ret = nvkm_ltc_init(priv);
@@ -106,6 +102,7 @@ gm107_ltc_init(struct nouveau_object *object)
 
 	nv_wr32(priv, 0x17e27c, priv->ltc_nr);
 	nv_wr32(priv, 0x17e278, priv->tag_base);
+	nv_mask(priv, 0x17e264, 0x00000002, lpg128 ? 0x00000002 : 0x00000000);
 	return 0;
 }
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/ltc/priv.h b/drivers/gpu/drm/nouveau/core/subdev/ltc/priv.h
index 594924f39126..41f179d93da6 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/ltc/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/ltc/priv.h
@@ -4,6 +4,8 @@
 #include <subdev/ltc.h>
 #include <subdev/fb.h>
 
+#include <core/enum.h>
+
 struct nvkm_ltc_priv {
 	struct nouveau_ltc base;
 	u32 ltc_nr;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/base.c b/drivers/gpu/drm/nouveau/core/subdev/pwr/base.c
index 69f1f34f6931..0ab55f27ec45 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/base.c
@@ -203,6 +203,8 @@ _nouveau_pwr_init(struct nouveau_object *object)
 	nv_wait(ppwr, 0x10a04c, 0xffffffff, 0x00000000);
 	nv_mask(ppwr, 0x000200, 0x00002000, 0x00000000);
 	nv_mask(ppwr, 0x000200, 0x00002000, 0x00002000);
+	nv_rd32(ppwr, 0x000200);
+	nv_wait(ppwr, 0x10a10c, 0x00000006, 0x00000000);
 
 	/* upload data segment */
 	nv_wr32(ppwr, 0x10a1c0, 0x01000000);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/arith.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/arith.fuc
new file mode 100644
index 000000000000..214a6d9e088d
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/arith.fuc
@@ -0,0 +1,94 @@
+/*
+ * Copyright 2014 Martin Peres <martin.peres@free.fr>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the folloing conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Martin Peres
+ */
+
+/******************************************************************************
+ * arith data segment
+ *****************************************************************************/
+#ifdef INCLUDE_PROC
+#endif
+
+#ifdef INCLUDE_DATA
+#endif
+
+/******************************************************************************
+ * arith code segment
+ *****************************************************************************/
+#ifdef INCLUDE_CODE
+
+// does a 32x32 -> 64 multiplication
+//
+// A * B = A_lo * B_lo
+//        + ( A_hi * B_lo ) << 16
+//        + ( A_lo * B_hi ) << 16
+//        + ( A_hi * B_hi ) << 32
+//
+// $r15 - current
+// $r14 - A
+// $r13 - B
+// $r12 - mul_lo (return)
+// $r11 - mul_hi (return)
+// $r0  - zero
+mulu32_32_64:
+	push $r1 // A_hi
+	push $r2 // B_hi
+	push $r3 // tmp0
+	push $r4 // tmp1
+
+	shr b32 $r1 $r14 16
+	shr b32 $r2 $r13 16
+
+	clear b32 $r12
+	clear b32 $r11
+
+	// A_lo * B_lo
+	mulu $r12 $r14 $r13
+
+	// ( A_hi * B_lo ) << 16
+	mulu $r3 $r1 $r13 // tmp0 = A_hi * B_lo
+	mov b32 $r4 $r3
+	and $r3 0xffff // tmp0 = tmp0_lo
+	shl b32 $r3 16
+	shr b32 $r4 16 // tmp1 = tmp0_hi
+	add b32 $r12 $r3
+	adc b32 $r11 $r4
+
+	// ( A_lo * B_hi ) << 16
+	mulu $r3 $r14 $r2 // tmp0 = A_lo * B_hi
+	mov b32 $r4 $r3
+	and $r3 0xffff // tmp0 = tmp0_lo
+	shl b32 $r3 16
+	shr b32 $r4 16 // tmp1 = tmp0_hi
+	add b32 $r12 $r3
+	adc b32 $r11 $r4
+
+	// ( A_hi * B_hi ) << 32
+	mulu $r3 $r1 $r2 // tmp0 = A_hi * B_hi
+	add b32 $r11 $r3
+
+	pop $r4
+	pop $r3
+	pop $r2
+	pop $r1
+	ret
+#endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/kernel.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/kernel.fuc
index 8f29badd785f..5cf5be63cbef 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/kernel.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/kernel.fuc
@@ -98,12 +98,16 @@ wr32:
 // $r14 - ns
 // $r0  - zero
 nsec:
+	push $r9
+	push $r8
 	nv_iord($r8, NV_PPWR_TIMER_LOW)
 	nsec_loop:
 		nv_iord($r9, NV_PPWR_TIMER_LOW)
 		sub b32 $r9 $r8
 		cmp b32 $r9 $r14
 		bra l #nsec_loop
+	pop $r8
+	pop $r9
 	ret
 
 // busy-wait for a period of time
@@ -115,6 +119,8 @@ nsec:
 // $r11 - timeout (ns)
 // $r0  - zero
 wait:
+	push $r9
+	push $r8
 	nv_iord($r8, NV_PPWR_TIMER_LOW)
 	wait_loop:
 		nv_rd32($r10, $r14)
@@ -126,6 +132,8 @@ wait:
 		cmp b32 $r9 $r11
 		bra l #wait_loop
 	wait_done:
+	pop $r8
+	pop $r9
 	ret
 
 // $r15 - current (kern)
@@ -242,12 +250,89 @@ intr:
 	bclr $flags $p0
 	iret
 
-// request the current process be sent a message after a timeout expires
+// calculate the number of ticks in the specified nanoseconds delay
+//
+// $r15 - current
+// $r14 - ns
+// $r14 - ticks (return)
+// $r0  - zero
+ticks_from_ns:
+	push $r12
+	push $r11
+
+	/* try not losing precision (multiply then divide) */
+	imm32($r13, HW_TICKS_PER_US)
+	call #mulu32_32_64
+
+	/* use an immeditate, it's ok because HW_TICKS_PER_US < 16 bits */
+	div $r12 $r12 1000
+
+	/* check if there wasn't any overflow */
+	cmpu b32 $r11 0
+	bra e #ticks_from_ns_quit
+
+	/* let's divide then multiply, too bad for the precision! */
+	div $r14 $r14 1000
+	imm32($r13, HW_TICKS_PER_US)
+	call #mulu32_32_64
+
+	/* this cannot overflow as long as HW_TICKS_PER_US < 1000 */
+
+ticks_from_ns_quit:
+	mov b32 $r14 $r12
+	pop $r11
+	pop $r12
+	ret
+
+// calculate the number of ticks in the specified microsecond delay
+//
+// $r15 - current
+// $r14 - us
+// $r14 - ticks (return)
+// $r0  - zero
+ticks_from_us:
+	push $r12
+	push $r11
+
+	/* simply multiply $us by HW_TICKS_PER_US */
+	imm32($r13, HW_TICKS_PER_US)
+	call #mulu32_32_64
+	mov b32 $r14 $r12
+
+	/* check if there wasn't any overflow */
+	cmpu b32 $r11 0
+	bra e #ticks_from_us_quit
+
+	/* Overflow! */
+	clear b32 $r14
+
+ticks_from_us_quit:
+	pop $r11
+	pop $r12
+	ret
+
+// calculate the number of ticks in the specified microsecond delay
 //
 // $r15 - current
 // $r14 - ticks
+// $r14 - us (return)
+// $r0  - zero
+ticks_to_us:
+	/* simply divide $ticks by HW_TICKS_PER_US */
+	imm32($r13, HW_TICKS_PER_US)
+	div $r14 $r14 $r13
+
+	ret
+
+// request the current process be sent a message after a timeout expires
+//
+// $r15 - current
+// $r14 - ticks (make sure it is < 2^31 to avoid any possible overflow)
 // $r0  - zero
 timer:
+	push $r9
+	push $r8
+
 	// interrupts off to prevent racing with timer isr
 	bclr $flags ie0
 
@@ -255,13 +340,22 @@ timer:
 	ld b32 $r8 D[$r15 + #proc_time]
 	cmp b32 $r8 0
 	bra g #timer_done
-	st b32 D[$r15 + #proc_time] $r14
 
-	// halt watchdog timer temporarily and check for a pending
-	// interrupt.  if there's one already pending, we can just
-	// bail since the timer isr will queue the next soonest
-	// right after it's done
+	// halt watchdog timer temporarily
+	clear b32 $r8
 	nv_iowr(NV_PPWR_WATCHDOG_ENABLE, $r8)
+
+	// find out how much time elapsed since the last update
+	// of the watchdog and add this time to the wanted ticks
+	nv_iord($r8, NV_PPWR_WATCHDOG_TIME)
+	ld b32 $r9 D[$r0 + #time_prev]
+	sub b32 $r9 $r8
+	add b32 $r14 $r9
+	st b32 D[$r15 + #proc_time] $r14
+
+	// check for a pending interrupt.  if there's one already
+	// pending, we can just bail since the timer isr will
+	// queue the next soonest right after it's done
 	nv_iord($r8, NV_PPWR_INTR)
 	and $r8 NV_PPWR_INTR_WATCHDOG
 	bra nz #timer_enable
@@ -272,10 +366,10 @@ timer:
 	cmp b32 $r14 $r0
 	bra e #timer_reset
 	cmp b32 $r14 $r8
-	bra l #timer_done
-	timer_reset:
-	nv_iowr(NV_PPWR_WATCHDOG_TIME, $r14)
-	st b32 D[$r0 + #time_prev] $r14
+	bra g #timer_enable
+		timer_reset:
+		nv_iowr(NV_PPWR_WATCHDOG_TIME, $r14)
+		st b32 D[$r0 + #time_prev] $r14
 
 	// re-enable the watchdog timer
 	timer_enable:
@@ -285,6 +379,9 @@ timer:
 	// interrupts back on
 	timer_done:
 	bset $flags ie0
+
+	pop $r8
+	pop $r9
 	ret
 
 // send message to another process
@@ -371,6 +468,9 @@ send:
 // $r14 - process
 // $r0  - zero
 recv:
+	push $r9
+	push $r8
+
 	ld b32 $r8 D[$r14 + #proc_qget]
 	ld b32 $r9 D[$r14 + #proc_qput]
 	bclr $flags $p1
@@ -403,6 +503,8 @@ recv:
 		bset $flags $p1
 		pop $r15
 	recv_done:
+	pop $r8
+	pop $r9
 	ret
 
 init:
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/macros.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/macros.fuc
index 5668e045bac1..96fc984dafdc 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/macros.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/macros.fuc
@@ -250,3 +250,23 @@
 */	st b32 D[$r0] reg /*
 */	clear b32 $r0
 #endif
+
+#define st(size, addr, reg) /*
+*/	movw $r0 addr /*
+*/	st size D[$r0] reg /*
+*/	clear b32 $r0
+
+#define ld(size, reg, addr) /*
+*/	movw $r0 addr /*
+*/	ld size reg D[$r0] /*
+*/	clear b32 $r0
+
+// does a 64+64 -> 64 unsigned addition (C = A + B)
+#define addu64(reg_a_c_hi, reg_a_c_lo, b_hi, b_lo) /*
+*/    add b32 reg_a_c_lo b_lo /*
+*/    adc b32 reg_a_c_hi b_hi
+
+// does a 64+64 -> 64 substraction (C = A - B)
+#define subu64(reg_a_c_hi, reg_a_c_lo, b_hi, b_lo) /*
+*/    sub b32 reg_a_c_lo b_lo /*
+*/    sbb b32 reg_a_c_hi b_hi
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc
index d43741eccb11..e89789a53b80 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/memx.fuc
@@ -43,17 +43,23 @@ process(PROC_MEMX, #memx_init, #memx_recv)
 */	.b32 func
 
 memx_func_head:
-handler(ENTER , 0x0001, 0x0000, #memx_func_enter)
+handler(ENTER , 0x0000, 0x0000, #memx_func_enter)
 memx_func_next:
 handler(LEAVE , 0x0000, 0x0000, #memx_func_leave)
 handler(WR32  , 0x0000, 0x0002, #memx_func_wr32)
 handler(WAIT  , 0x0004, 0x0000, #memx_func_wait)
 handler(DELAY , 0x0001, 0x0000, #memx_func_delay)
+handler(VBLANK, 0x0001, 0x0000, #memx_func_wait_vblank)
 memx_func_tail:
 
 .equ #memx_func_size #memx_func_next - #memx_func_head
 .equ #memx_func_num (#memx_func_tail - #memx_func_head) / #memx_func_size
 
+memx_ts_start:
+.b32 0
+memx_ts_end:
+.b32 0
+
 memx_data_head:
 .skip 0x0800
 memx_data_tail:
@@ -67,19 +73,44 @@ memx_data_tail:
 //
 // $r15 - current (memx)
 // $r4  - packet length
-//	+00: bitmask of heads to wait for vblank on
 // $r3  - opcode desciption
 // $r0  - zero
 memx_func_enter:
+#if NVKM_PPWR_CHIPSET == GT215
+	movw $r8 0x1610
+	nv_rd32($r7, $r8)
+	imm32($r6, 0xfffffffc)
+	and $r7 $r6
+	movw $r6 0x2
+	or $r7 $r6
+	nv_wr32($r8, $r7)
+#else
+	movw $r6 0x001620
+	imm32($r7, ~0x00000aa2);
+	nv_rd32($r8, $r6)
+	and $r8 $r7
+	nv_wr32($r6, $r8)
+
+	imm32($r7, ~0x00000001)
+	nv_rd32($r8, $r6)
+	and $r8 $r7
+	nv_wr32($r6, $r8)
+
+	movw $r6 0x0026f0
+	nv_rd32($r8, $r6)
+	and $r8 $r7
+	nv_wr32($r6, $r8)
+#endif
+
 	mov $r6 NV_PPWR_OUTPUT_SET_FB_PAUSE
 	nv_iowr(NV_PPWR_OUTPUT_SET, $r6)
 	memx_func_enter_wait:
 		nv_iord($r6, NV_PPWR_OUTPUT)
 		and $r6 NV_PPWR_OUTPUT_FB_PAUSE
 		bra z #memx_func_enter_wait
-	//XXX: TODO
-	ld b32 $r6 D[$r1 + 0x00]
-	add b32 $r1 0x04
+
+	nv_iord($r6, NV_PPWR_TIMER_LOW)
+	st b32 D[$r0 + #memx_ts_start] $r6
 	ret
 
 // description
@@ -89,14 +120,93 @@ memx_func_enter:
 // $r3  - opcode desciption
 // $r0  - zero
 memx_func_leave:
+	nv_iord($r6, NV_PPWR_TIMER_LOW)
+	st b32 D[$r0 + #memx_ts_end] $r6
+
 	mov $r6 NV_PPWR_OUTPUT_CLR_FB_PAUSE
 	nv_iowr(NV_PPWR_OUTPUT_CLR, $r6)
 	memx_func_leave_wait:
 		nv_iord($r6, NV_PPWR_OUTPUT)
 		and $r6 NV_PPWR_OUTPUT_FB_PAUSE
 		bra nz #memx_func_leave_wait
+
+#if NVKM_PPWR_CHIPSET == GT215
+	movw $r8 0x1610
+	nv_rd32($r7, $r8)
+	imm32($r6, 0xffffffcc)
+	and $r7 $r6
+	nv_wr32($r8, $r7)
+#else
+	movw $r6 0x0026f0
+	imm32($r7, 0x00000001)
+	nv_rd32($r8, $r6)
+	or $r8 $r7
+	nv_wr32($r6, $r8)
+
+	movw $r6 0x001620
+	nv_rd32($r8, $r6)
+	or $r8 $r7
+	nv_wr32($r6, $r8)
+
+	imm32($r7, 0x00000aa2);
+	nv_rd32($r8, $r6)
+	or $r8 $r7
+	nv_wr32($r6, $r8)
+#endif
+	ret
+
+#if NVKM_PPWR_CHIPSET < GF119
+// description
+//
+// $r15 - current (memx)
+// $r4  - packet length
+//	+00: head to wait for vblank on
+// $r3  - opcode desciption
+// $r0  - zero
+memx_func_wait_vblank:
+	ld b32 $r6 D[$r1 + 0x00]
+	cmp b32 $r6 0x0
+	bra z #memx_func_wait_vblank_head0
+	cmp b32 $r6 0x1
+	bra z #memx_func_wait_vblank_head1
+	bra #memx_func_wait_vblank_fini
+
+	memx_func_wait_vblank_head1:
+	movw $r7 0x20
+	bra #memx_func_wait_vblank_0
+
+	memx_func_wait_vblank_head0:
+	movw $r7 0x8
+
+	memx_func_wait_vblank_0:
+		nv_iord($r6, NV_PPWR_INPUT)
+		and $r6 $r7
+		bra nz #memx_func_wait_vblank_0
+
+	memx_func_wait_vblank_1:
+		nv_iord($r6, NV_PPWR_INPUT)
+		and $r6 $r7
+		bra z #memx_func_wait_vblank_1
+
+	memx_func_wait_vblank_fini:
+	add b32 $r1 0x4
+	ret
+
+#else
+
+// XXX: currently no-op
+//
+// $r15 - current (memx)
+// $r4  - packet length
+//	+00: head to wait for vblank on
+// $r3  - opcode desciption
+// $r0  - zero
+memx_func_wait_vblank:
+	add b32 $r1 0x4
 	ret
 
+#endif
+
 // description
 //
 // $r15 - current (memx)
@@ -160,14 +270,17 @@ memx_exec:
 	push $r13
 	mov b32 $r1 $r12
 	mov b32 $r2 $r11
+
 	memx_exec_next:
-		// fetch the packet header, and locate opcode info
+		// fetch the packet header
 		ld b32 $r3 D[$r1]
 		add b32 $r1 4
-		shr b32 $r4 $r3 16
-		mulu $r3 #memx_func_size
+		extr $r4 $r3 16:31
+		extr $r3 $r3 0:15
 
 		// execute the opcode handler
+		sub b32 $r3 1
+		mulu $r3 #memx_func_size
 		ld b32 $r5 D[$r3 + #memx_func_head + #memx_func]
 		call $r5
 
@@ -176,6 +289,10 @@ memx_exec:
 		bra l #memx_exec_next
 
 	// send completion reply
+	ld b32 $r11 D[$r0 + #memx_ts_start]
+	ld b32 $r12 D[$r0 + #memx_ts_end]
+	sub b32 $r12 $r11
+	nv_iord($r11, NV_PPWR_INPUT)
 	pop $r13
 	pop $r14
 	call(send)
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc
index 17a8a383d91a..b439519ec866 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc
@@ -23,6 +23,7 @@
  */
 
 #define NVKM_PPWR_CHIPSET GK208
+#define HW_TICKS_PER_US 324
 
 #define NVKM_FALCON_PC24
 #define NVKM_FALCON_UNSHIFTED_IO
@@ -34,6 +35,7 @@
 .section #nv108_pwr_data
 #define INCLUDE_PROC
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -44,6 +46,7 @@
 
 #define INCLUDE_DATA
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -56,6 +59,7 @@
 .section #nv108_pwr_code
 #define INCLUDE_CODE
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h
index 986495d533dd..4d278a96b2bb 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nv108.fuc.h
@@ -24,8 +24,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x00000379,
-	0x0000032a,
+	0x00000453,
+	0x00000404,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x00000464,
-	0x00000456,
+	0x0000061c,
+	0x0000060e,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x00000468,
-	0x00000466,
+	0x00000620,
+	0x0000061e,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x0000086c,
-	0x00000713,
+	0x00000a24,
+	0x000008cb,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x0000088d,
-	0x0000086e,
+	0x00000a45,
+	0x00000a26,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000898,
-	0x00000896,
+	0x00000a50,
+	0x00000a4e,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -227,25 +227,31 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 /* 0x0370: memx_func_head */
-	0x00010000,
-	0x00000000,
-	0x000003a9,
-/* 0x037c: memx_func_next */
 	0x00000001,
 	0x00000000,
-	0x000003c7,
+	0x00000483,
+/* 0x037c: memx_func_next */
 	0x00000002,
+	0x00000000,
+	0x00000500,
+	0x00000003,
 	0x00000002,
-	0x000003df,
-	0x00040003,
+	0x00000580,
+	0x00040004,
+	0x00000000,
+	0x0000059d,
+	0x00010005,
+	0x00000000,
+	0x000005b7,
+	0x00010006,
 	0x00000000,
-	0x000003fc,
-	0x00010004,
+	0x0000057b,
+/* 0x03b8: memx_func_tail */
+/* 0x03b8: memx_ts_start */
 	0x00000000,
-	0x00000416,
-/* 0x03ac: memx_func_tail */
-/* 0x03ac: memx_data_head */
+/* 0x03bc: memx_ts_end */
 	0x00000000,
+/* 0x03c0: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -757,8 +763,9 @@ uint32_t nv108_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bac: memx_data_tail */
-/* 0x0bac: i2c_scl_map */
+	0x00000000,
+/* 0x0bc0: memx_data_tail */
+/* 0x0bc0: i2c_scl_map */
 	0x00000400,
 	0x00000800,
 	0x00001000,
@@ -769,7 +776,7 @@ uint32_t nv108_pwr_data[] = {
 	0x00020000,
 	0x00040000,
 	0x00080000,
-/* 0x0bd4: i2c_sda_map */
+/* 0x0be8: i2c_sda_map */
 	0x00100000,
 	0x00200000,
 	0x00400000,
@@ -781,10 +788,69 @@ uint32_t nv108_pwr_data[] = {
 	0x10000000,
 	0x20000000,
 	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 };
 
 uint32_t nv108_pwr_code[] = {
-	0x02910ef5,
+	0x031c0ef5,
 /* 0x0004: rd32 */
 	0xf607a040,
 	0x04bd000e,
@@ -812,15 +878,18 @@ uint32_t nv108_pwr_code[] = {
 	0x7000d4f1,
 	0xf8f61bf4,
 /* 0x005d: nsec */
-	0xcf2c0800,
-/* 0x0062: nsec_loop */
+	0xf990f900,
+	0xcf2c0880,
+/* 0x0066: nsec_loop */
 	0x2c090088,
 	0xbb0099cf,
 	0x9ea60298,
-	0xf8f61ef4,
-/* 0x0071: wait */
-	0xcf2c0800,
-/* 0x0076: wait_loop */
+	0xfcf61ef4,
+	0xf890fc80,
+/* 0x0079: wait */
+	0xf990f900,
+	0xcf2c0880,
+/* 0x0082: wait_loop */
 	0xeeb20088,
 	0x0000047e,
 	0xadfddab2,
@@ -828,28 +897,29 @@ uint32_t nv108_pwr_code[] = {
 	0x2c09100b,
 	0xbb0099cf,
 	0x9ba60298,
-/* 0x0093: wait_done */
-	0xf8e61ef4,
-/* 0x0095: intr_watchdog */
+/* 0x009f: wait_done */
+	0xfce61ef4,
+	0xf890fc80,
+/* 0x00a5: intr_watchdog */
 	0x03e99800,
 	0xf40096b0,
 	0x0a98280b,
 	0x029abb9a,
 	0x0d0e1cf4,
-	0x01de7e01,
+	0x02617e01,
 	0xf494bd00,
-/* 0x00b2: intr_watchdog_next_time */
+/* 0x00c2: intr_watchdog_next_time */
 	0x0a98140e,
 	0x00a6b09b,
 	0xa6080bf4,
 	0x061cf49a,
-/* 0x00c0: intr_watchdog_next_time_set */
-/* 0x00c3: intr_watchdog_next_proc */
+/* 0x00d0: intr_watchdog_next_time_set */
+/* 0x00d3: intr_watchdog_next_proc */
 	0xb59b09b5,
 	0xe0b603e9,
 	0x68e6b158,
 	0xc81bf402,
-/* 0x00d2: intr */
+/* 0x00e2: intr */
 	0x00f900f8,
 	0x80f904bd,
 	0xa0f990f9,
@@ -865,13 +935,13 @@ uint32_t nv108_pwr_code[] = {
 	0xc40088cf,
 	0x0bf40289,
 	0x9b00b51f,
-	0x957e580e,
+	0xa57e580e,
 	0x09980000,
 	0x0096b09b,
 	0x000d0bf4,
 	0x0009f634,
 	0x09b504bd,
-/* 0x0125: intr_skip_watchdog */
+/* 0x0135: intr_skip_watchdog */
 	0x0089e49a,
 	0x360bf408,
 	0xcf068849,
@@ -881,20 +951,20 @@ uint32_t nv108_pwr_code[] = {
 	0xc0f900cc,
 	0xf14f484e,
 	0x0d5453e3,
-	0x023f7e00,
+	0x02c27e00,
 	0x40c0fc00,
 	0x0cf604c0,
-/* 0x0157: intr_subintr_skip_fifo */
+/* 0x0167: intr_subintr_skip_fifo */
 	0x4004bd00,
 	0x09f60688,
-/* 0x015f: intr_skip_subintr */
+/* 0x016f: intr_skip_subintr */
 	0xc404bd00,
 	0x0bf42089,
 	0xbfa4f107,
-/* 0x0169: intr_skip_pause */
+/* 0x0179: intr_skip_pause */
 	0x4089c4ff,
 	0xf1070bf4,
-/* 0x0173: intr_skip_user0 */
+/* 0x0183: intr_skip_user0 */
 	0x00ffbfa4,
 	0x0008f604,
 	0x80fc04bd,
@@ -904,304 +974,417 @@ uint32_t nv108_pwr_code[] = {
 	0xfca0fcb0,
 	0xfc80fc90,
 	0x0032f400,
-/* 0x0196: timer */
-	0x32f401f8,
-	0x03f89810,
-	0xf40086b0,
-	0xfeb53a1c,
-	0xf6380003,
+/* 0x01a6: ticks_from_ns */
+	0xc0f901f8,
+	0xd7f1b0f9,
+	0xd3f00144,
+	0x7721f500,
+	0xe8ccec03,
+	0x00b4b003,
+	0xec120bf4,
+	0xf103e8ee,
+	0xf00144d7,
+	0x21f500d3,
+/* 0x01ce: ticks_from_ns_quit */
+	0xceb20377,
+	0xc0fcb0fc,
+/* 0x01d6: ticks_from_us */
+	0xc0f900f8,
+	0xd7f1b0f9,
+	0xd3f00144,
+	0x7721f500,
+	0xb0ceb203,
+	0x0bf400b4,
+/* 0x01ef: ticks_from_us_quit */
+	0xfce4bd05,
+	0xf8c0fcb0,
+/* 0x01f5: ticks_to_us */
+	0x44d7f100,
+	0x00d3f001,
+	0xf8ecedff,
+/* 0x0201: timer */
+	0xf990f900,
+	0x1032f480,
+	0xb003f898,
+	0x1cf40086,
+	0x0084bd4a,
+	0x0008f638,
+	0x340804bd,
+	0x980088cf,
+	0x98bb9a09,
+	0x00e9bb02,
+	0x0803feb5,
+	0x0088cf08,
+	0xf40284f0,
+	0x34081c1b,
+	0xa60088cf,
+	0x080bf4e0,
+	0x1cf4e8a6,
+/* 0x0245: timer_reset */
+	0xf634000d,
+	0x04bd000e,
+/* 0x024f: timer_enable */
+	0x089a0eb5,
+	0xf6380001,
 	0x04bd0008,
-	0x88cf0808,
-	0x0284f000,
-	0x081c1bf4,
-	0x0088cf34,
-	0x0bf4e0a6,
-	0xf4e8a608,
-/* 0x01c6: timer_reset */
-	0x3400161e,
-	0xbd000ef6,
-	0x9a0eb504,
-/* 0x01d0: timer_enable */
-	0x38000108,
-	0xbd0008f6,
-/* 0x01d9: timer_done */
-	0x1031f404,
-/* 0x01de: send_proc */
-	0x80f900f8,
-	0xe89890f9,
-	0x04e99805,
-	0xa60486f0,
-	0x2a0bf489,
-	0x940398c4,
-	0x80b60488,
-	0x008ebb18,
-	0xb500fa98,
-	0x8db5008a,
-	0x028cb501,
-	0xb6038bb5,
-	0x94f00190,
-	0x04e9b507,
-/* 0x0217: send_done */
-	0xfc0231f4,
-	0xf880fc90,
-/* 0x021d: find */
-	0x0880f900,
-	0x0131f458,
-/* 0x0224: find_loop */
-	0xa6008a98,
-	0x100bf4ae,
-	0xb15880b6,
-	0xf4026886,
-	0x32f4f11b,
-/* 0x0239: find_done */
-	0xfc8eb201,
-/* 0x023f: send */
-	0x7e00f880,
-	0xf400021d,
-	0x00f89b01,
-/* 0x0248: recv */
-	0x9805e898,
-	0x32f404e9,
-	0xf489a601,
-	0x89c43c0b,
-	0x0180b603,
-	0xb50784f0,
-	0xea9805e8,
-	0xfef0f902,
-	0xf0f9018f,
-	0x9994efb2,
-	0x00e9bb04,
-	0x9818e0b6,
-	0xec9803eb,
-	0x01ed9802,
-	0xf900ee98,
-	0xfef0fca5,
-	0x31f400f8,
-/* 0x028f: recv_done */
-	0xf8f0fc01,
-/* 0x0291: init */
-	0x01084100,
-	0xe70011cf,
-	0xb6010911,
-	0x14fe0814,
-	0x00e04100,
-	0x000013f0,
-	0x0001f61c,
-	0xff0104bd,
-	0x01f61400,
-	0x0104bd00,
-	0x0015f102,
-	0xf6100008,
-	0x04bd0001,
-	0xf000d241,
-	0x10fe0013,
-	0x1031f400,
-	0x38000101,
+/* 0x0258: timer_done */
+	0xfc1031f4,
+	0xf890fc80,
+/* 0x0261: send_proc */
+	0xf980f900,
+	0x05e89890,
+	0xf004e998,
+	0x89a60486,
+	0xc42a0bf4,
+	0x88940398,
+	0x1880b604,
+	0x98008ebb,
+	0x8ab500fa,
+	0x018db500,
+	0xb5028cb5,
+	0x90b6038b,
+	0x0794f001,
+	0xf404e9b5,
+/* 0x029a: send_done */
+	0x90fc0231,
+	0x00f880fc,
+/* 0x02a0: find */
+	0x580880f9,
+/* 0x02a7: find_loop */
+	0x980131f4,
+	0xaea6008a,
+	0xb6100bf4,
+	0x86b15880,
+	0x1bf40268,
+	0x0132f4f1,
+/* 0x02bc: find_done */
+	0x80fc8eb2,
+/* 0x02c2: send */
+	0xa07e00f8,
+	0x01f40002,
+/* 0x02cb: recv */
+	0xf900f89b,
+	0x9880f990,
+	0xe99805e8,
+	0x0132f404,
+	0x0bf489a6,
+	0x0389c43c,
+	0xf00180b6,
+	0xe8b50784,
+	0x02ea9805,
+	0x8ffef0f9,
+	0xb2f0f901,
+	0x049994ef,
+	0xb600e9bb,
+	0xeb9818e0,
+	0x02ec9803,
+	0x9801ed98,
+	0xa5f900ee,
+	0xf8fef0fc,
+	0x0131f400,
+/* 0x0316: recv_done */
+	0x80fcf0fc,
+	0x00f890fc,
+/* 0x031c: init */
+	0xcf010841,
+	0x11e70011,
+	0x14b60109,
+	0x0014fe08,
+	0xf000e041,
+	0x1c000013,
 	0xbd0001f6,
-/* 0x02db: init_proc */
-	0x98580f04,
-	0x16b001f1,
-	0xfa0bf400,
-	0xf0b615f9,
-	0xf20ef458,
-/* 0x02ec: host_send */
-	0xcf04b041,
-	0xa0420011,
-	0x0022cf04,
-	0x0bf412a6,
-	0x071ec42e,
-	0xb704ee94,
-	0x980270e0,
-	0xec9803eb,
-	0x01ed9802,
-	0x7e00ee98,
-	0xb600023f,
-	0x1ec40110,
-	0x04b0400f,
-	0xbd000ef6,
-	0xc70ef404,
-/* 0x0328: host_send_done */
-/* 0x032a: host_recv */
-	0x494100f8,
-	0x5413f14e,
-	0xf4e1a652,
-/* 0x0336: host_recv_wait */
-	0xcc41b90b,
-	0x0011cf04,
-	0xcf04c842,
-	0x16f00022,
-	0xf412a608,
-	0x23c4ef0b,
-	0x0434b607,
-	0x02f030b7,
-	0xb5033bb5,
-	0x3db5023c,
-	0x003eb501,
-	0xf00120b6,
-	0xc8400f24,
-	0x0002f604,
-	0x400204bd,
-	0x02f60000,
-	0xf804bd00,
-/* 0x0379: host_init */
-	0x00804100,
-	0xf11014b6,
-	0x40027015,
-	0x01f604d0,
+	0x00ff0104,
+	0x0001f614,
+	0x020104bd,
+	0x080015f1,
+	0x01f61000,
 	0x4104bd00,
+	0x13f000e2,
+	0x0010fe00,
+	0x011031f4,
+	0xf6380001,
+	0x04bd0001,
+/* 0x0366: init_proc */
+	0xf198580f,
+	0x0016b001,
+	0xf9fa0bf4,
+	0x58f0b615,
+/* 0x0377: mulu32_32_64 */
+	0xf9f20ef4,
+	0xf920f910,
+	0x9540f930,
+	0xd29510e1,
+	0xbdc4bd10,
+	0xc0edffb4,
+	0xb2301dff,
+	0xff34f134,
+	0x1034b6ff,
+	0xbb1045b6,
+	0xb4bb00c3,
+	0x30e2ff01,
+	0x34f134b2,
+	0x34b6ffff,
+	0x1045b610,
+	0xbb00c3bb,
+	0x12ff01b4,
+	0x00b3bb30,
+	0x30fc40fc,
+	0x10fc20fc,
+/* 0x03c6: host_send */
+	0xb04100f8,
+	0x0011cf04,
+	0xcf04a042,
+	0x12a60022,
+	0xc42e0bf4,
+	0xee94071e,
+	0x70e0b704,
+	0x03eb9802,
+	0x9802ec98,
+	0xee9801ed,
+	0x02c27e00,
+	0x0110b600,
+	0x400f1ec4,
+	0x0ef604b0,
+	0xf404bd00,
+/* 0x0402: host_send_done */
+	0x00f8c70e,
+/* 0x0404: host_recv */
+	0xf14e4941,
+	0xa6525413,
+	0xb90bf4e1,
+/* 0x0410: host_recv_wait */
+	0xcf04cc41,
+	0xc8420011,
+	0x0022cf04,
+	0xa60816f0,
+	0xef0bf412,
+	0xb60723c4,
+	0x30b70434,
+	0x3bb502f0,
+	0x023cb503,
+	0xb5013db5,
+	0x20b6003e,
+	0x0f24f001,
+	0xf604c840,
+	0x04bd0002,
+	0x00004002,
+	0xbd0002f6,
+/* 0x0453: host_init */
+	0x4100f804,
 	0x14b60080,
-	0xf015f110,
-	0x04dc4002,
+	0x7015f110,
+	0x04d04002,
+	0xbd0001f6,
+	0x00804104,
+	0xf11014b6,
+	0x4002f015,
+	0x01f604dc,
+	0x0104bd00,
+	0x04c44001,
 	0xbd0001f6,
-	0x40010104,
-	0x01f604c4,
-	0xf804bd00,
-/* 0x03a9: memx_func_enter */
-	0x40040600,
-	0x06f607e0,
-/* 0x03b3: memx_func_enter_wait */
-	0x4604bd00,
-	0x66cf07c0,
-	0x0464f000,
-	0x98f70bf4,
-	0x10b60016,
-/* 0x03c7: memx_func_leave */
-	0x0600f804,
-	0x07e44004,
-	0xbd0006f6,
-/* 0x03d1: memx_func_leave_wait */
-	0x07c04604,
-	0xf00066cf,
-	0x1bf40464,
-/* 0x03df: memx_func_wr32 */
-	0x9800f8f7,
-	0x15980016,
-	0x0810b601,
-	0x50f960f9,
+/* 0x0483: memx_func_enter */
+	0xf100f804,
+	0xf1162067,
+	0xf1f55d77,
+	0xb2ffff73,
+	0x00047e6e,
+	0xfdd8b200,
+	0x60f90487,
+	0xd0fc80f9,
+	0x2e7ee0fc,
+	0x77f10000,
+	0x73f1fffe,
+	0x6eb2ffff,
+	0x0000047e,
+	0x87fdd8b2,
+	0xf960f904,
+	0xfcd0fc80,
+	0x002e7ee0,
+	0xf067f100,
+	0x7e6eb226,
+	0xb2000004,
+	0x0487fdd8,
+	0x80f960f9,
 	0xe0fcd0fc,
 	0x00002e7e,
-	0xf40242b6,
-	0x00f8e81b,
-/* 0x03fc: memx_func_wait */
-	0x88cf2c08,
-	0x001e9800,
-	0x98011d98,
-	0x1b98021c,
-	0x1010b603,
-	0x0000717e,
-/* 0x0416: memx_func_delay */
-	0x1e9800f8,
-	0x0410b600,
-	0x00005d7e,
-/* 0x0422: memx_exec */
-	0xe0f900f8,
-	0xc1b2d0f9,
-/* 0x042a: memx_exec_next */
-	0x1398b2b2,
-	0x0410b600,
-	0xf0103495,
-	0x35980c30,
-	0xa655f9de,
-	0xed1ef412,
+	0xe0400406,
+	0x0006f607,
+/* 0x04ea: memx_func_enter_wait */
+	0xc04604bd,
+	0x0066cf07,
+	0xf40464f0,
+	0x2c06f70b,
+	0xb50066cf,
+	0x00f8ee06,
+/* 0x0500: memx_func_leave */
+	0x66cf2c06,
+	0xef06b500,
+	0xe4400406,
+	0x0006f607,
+/* 0x0512: memx_func_leave_wait */
+	0xc04604bd,
+	0x0066cf07,
+	0xf40464f0,
+	0x67f1f71b,
+	0x77f126f0,
+	0x73f00001,
+	0x7e6eb200,
+	0xb2000004,
+	0x0587fdd8,
+	0x80f960f9,
 	0xe0fcd0fc,
-	0x00023f7e,
-/* 0x044a: memx_info */
-	0xac4c00f8,
+	0x00002e7e,
+	0x162067f1,
+	0x047e6eb2,
+	0xd8b20000,
+	0xf90587fd,
+	0xfc80f960,
+	0x7ee0fcd0,
+	0xf100002e,
+	0xf00aa277,
+	0x6eb20073,
+	0x0000047e,
+	0x87fdd8b2,
+	0xf960f905,
+	0xfcd0fc80,
+	0x002e7ee0,
+/* 0x057b: memx_func_wait_vblank */
+	0xb600f800,
+	0x00f80410,
+/* 0x0580: memx_func_wr32 */
+	0x98001698,
+	0x10b60115,
+	0xf960f908,
+	0xfcd0fc50,
+	0x002e7ee0,
+	0x0242b600,
+	0xf8e81bf4,
+/* 0x059d: memx_func_wait */
+	0xcf2c0800,
+	0x1e980088,
+	0x011d9800,
+	0x98021c98,
+	0x10b6031b,
+	0x00797e10,
+/* 0x05b7: memx_func_delay */
+	0x9800f800,
+	0x10b6001e,
+	0x005d7e04,
+/* 0x05c3: memx_exec */
+	0xf900f800,
+	0xb2d0f9e0,
+/* 0x05cb: memx_exec_next */
+	0x98b2b2c1,
+	0x10b60013,
+	0xf034e704,
+	0xe033e701,
+	0x0132b601,
+	0x980c30f0,
+	0x55f9de35,
+	0x1ef412a6,
+	0xee0b98e5,
+	0xbbef0c98,
+	0xc44b02cb,
+	0x00bbcf07,
+	0xe0fcd0fc,
+	0x0002c27e,
+/* 0x0602: memx_info */
+	0xc04c00f8,
 	0x08004b03,
-	0x00023f7e,
-/* 0x0456: memx_recv */
+	0x0002c27e,
+/* 0x060e: memx_recv */
 	0xd6b000f8,
-	0xc90bf401,
+	0xb20bf401,
 	0xf400d6b0,
 	0x00f8eb0b,
-/* 0x0464: memx_init */
-/* 0x0466: perf_recv */
+/* 0x061c: memx_init */
+/* 0x061e: perf_recv */
 	0x00f800f8,
-/* 0x0468: perf_init */
-/* 0x046a: i2c_drive_scl */
+/* 0x0620: perf_init */
+/* 0x0622: i2c_drive_scl */
 	0x36b000f8,
 	0x0d0bf400,
 	0xf607e040,
 	0x04bd0001,
-/* 0x047a: i2c_drive_scl_lo */
+/* 0x0632: i2c_drive_scl_lo */
 	0xe44000f8,
 	0x0001f607,
 	0x00f804bd,
-/* 0x0484: i2c_drive_sda */
+/* 0x063c: i2c_drive_sda */
 	0xf40036b0,
 	0xe0400d0b,
 	0x0002f607,
 	0x00f804bd,
-/* 0x0494: i2c_drive_sda_lo */
+/* 0x064c: i2c_drive_sda_lo */
 	0xf607e440,
 	0x04bd0002,
-/* 0x049e: i2c_sense_scl */
+/* 0x0656: i2c_sense_scl */
 	0x32f400f8,
 	0x07c44301,
 	0xfd0033cf,
 	0x0bf40431,
 	0x0131f406,
-/* 0x04b0: i2c_sense_scl_done */
-/* 0x04b2: i2c_sense_sda */
+/* 0x0668: i2c_sense_scl_done */
+/* 0x066a: i2c_sense_sda */
 	0x32f400f8,
 	0x07c44301,
 	0xfd0033cf,
 	0x0bf40432,
 	0x0131f406,
-/* 0x04c4: i2c_sense_sda_done */
-/* 0x04c6: i2c_raise_scl */
+/* 0x067c: i2c_sense_sda_done */
+/* 0x067e: i2c_raise_scl */
 	0x40f900f8,
 	0x03089844,
-	0x046a7e01,
-/* 0x04d1: i2c_raise_scl_wait */
+	0x06227e01,
+/* 0x0689: i2c_raise_scl_wait */
 	0x03e84e00,
 	0x00005d7e,
-	0x00049e7e,
+	0x0006567e,
 	0xb60901f4,
 	0x1bf40142,
-/* 0x04e5: i2c_raise_scl_done */
+/* 0x069d: i2c_raise_scl_done */
 	0xf840fcef,
-/* 0x04e9: i2c_start */
-	0x049e7e00,
+/* 0x06a1: i2c_start */
+	0x06567e00,
 	0x0d11f400,
-	0x0004b27e,
+	0x00066a7e,
 	0xf40611f4,
-/* 0x04fa: i2c_start_rep */
+/* 0x06b2: i2c_start_rep */
 	0x00032e0e,
-	0x00046a7e,
-	0x847e0103,
-	0x76bb0004,
+	0x0006227e,
+	0x3c7e0103,
+	0x76bb0006,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb60004c6,
+	0xb600067e,
 	0x11f40464,
-/* 0x0525: i2c_start_send */
+/* 0x06dd: i2c_start_send */
 	0x7e00031d,
-	0x4e000484,
+	0x4e00063c,
 	0x5d7e1388,
 	0x00030000,
-	0x00046a7e,
+	0x0006227e,
 	0x7e13884e,
-/* 0x053f: i2c_start_out */
+/* 0x06f7: i2c_start_out */
 	0xf800005d,
-/* 0x0541: i2c_stop */
+/* 0x06f9: i2c_stop */
 	0x7e000300,
-	0x0300046a,
-	0x04847e00,
+	0x03000622,
+	0x063c7e00,
 	0x03e84e00,
 	0x00005d7e,
-	0x6a7e0103,
-	0x884e0004,
+	0x227e0103,
+	0x884e0006,
 	0x005d7e13,
 	0x7e010300,
-	0x4e000484,
+	0x4e00063c,
 	0x5d7e1388,
 	0x00f80000,
-/* 0x0570: i2c_bitw */
-	0x0004847e,
+/* 0x0728: i2c_bitw */
+	0x00063c7e,
 	0x7e03e84e,
 	0xbb00005d,
 	0x65b60076,
@@ -1209,44 +1392,44 @@ uint32_t nv108_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0004c67e,
+	0x00067e7e,
 	0xf40464b6,
 	0x884e1711,
 	0x005d7e13,
 	0x7e000300,
-	0x4e00046a,
+	0x4e000622,
 	0x5d7e1388,
-/* 0x05ae: i2c_bitw_out */
+/* 0x0766: i2c_bitw_out */
 	0x00f80000,
-/* 0x05b0: i2c_bitr */
-	0x847e0103,
-	0xe84e0004,
+/* 0x0768: i2c_bitr */
+	0x3c7e0103,
+	0xe84e0006,
 	0x005d7e03,
 	0x0076bb00,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
-	0xc67e50fc,
-	0x64b60004,
+	0x7e7e50fc,
+	0x64b60006,
 	0x1a11f404,
-	0x0004b27e,
-	0x6a7e0003,
-	0x884e0004,
+	0x00066a7e,
+	0x227e0003,
+	0x884e0006,
 	0x005d7e13,
 	0x013cf000,
-/* 0x05f3: i2c_bitr_done */
+/* 0x07ab: i2c_bitr_done */
 	0xf80131f4,
-/* 0x05f5: i2c_get_byte */
+/* 0x07ad: i2c_get_byte */
 	0x04000500,
-/* 0x05f9: i2c_get_byte_next */
+/* 0x07b1: i2c_get_byte_next */
 	0x0154b608,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x05b07e50,
+	0x07687e50,
 	0x0464b600,
 	0xfd2a11f4,
 	0x42b60553,
@@ -1257,11 +1440,11 @@ uint32_t nv108_pwr_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb6000570,
-/* 0x0642: i2c_get_byte_done */
+	0xb6000728,
+/* 0x07fa: i2c_get_byte_done */
 	0x00f80464,
-/* 0x0644: i2c_put_byte */
-/* 0x0646: i2c_put_byte_next */
+/* 0x07fc: i2c_put_byte */
+/* 0x07fe: i2c_put_byte_next */
 	0x42b60804,
 	0x3854ff01,
 	0xb60076bb,
@@ -1269,7 +1452,7 @@ uint32_t nv108_pwr_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x05707e50,
+	0x07287e50,
 	0x0464b600,
 	0xb03411f4,
 	0x1bf40046,
@@ -1278,21 +1461,21 @@ uint32_t nv108_pwr_code[] = {
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
-	0xb07e50fc,
-	0x64b60005,
+	0x687e50fc,
+	0x64b60007,
 	0x0f11f404,
 	0xb00076bb,
 	0x1bf40136,
 	0x0132f406,
-/* 0x069c: i2c_put_byte_done */
-/* 0x069e: i2c_addr */
+/* 0x0854: i2c_put_byte_done */
+/* 0x0856: i2c_addr */
 	0x76bb00f8,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb60004e9,
+	0xb60006a1,
 	0x11f40464,
 	0x2ec3e729,
 	0x0134b601,
@@ -1302,32 +1485,32 @@ uint32_t nv108_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0006447e,
-/* 0x06e3: i2c_addr_done */
+	0x0007fc7e,
+/* 0x089b: i2c_addr_done */
 	0xf80464b6,
-/* 0x06e5: i2c_acquire_addr */
+/* 0x089d: i2c_acquire_addr */
 	0xf8cec700,
 	0xb705e4b6,
 	0xf8d014e0,
-/* 0x06f1: i2c_acquire */
-	0x06e57e00,
+/* 0x08a9: i2c_acquire */
+	0x089d7e00,
 	0x00047e00,
 	0x03d9f000,
 	0x00002e7e,
-/* 0x0702: i2c_release */
-	0xe57e00f8,
-	0x047e0006,
+/* 0x08ba: i2c_release */
+	0x9d7e00f8,
+	0x047e0008,
 	0xdaf00000,
 	0x002e7e03,
-/* 0x0713: i2c_recv */
+/* 0x08cb: i2c_recv */
 	0xf400f800,
 	0xc1c70132,
 	0x0214b6f8,
 	0xf52816b0,
 	0xb801371f,
-	0x000bd413,
+	0x000be813,
 	0xb8003298,
-	0x000bac13,
+	0x000bc013,
 	0xf4003198,
 	0xd0f90231,
 	0xd0f9e0f9,
@@ -1339,7 +1522,7 @@ uint32_t nv108_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0006f17e,
+	0x0008a97e,
 	0xfc0464b6,
 	0x00d6b0d0,
 	0x00b01bf5,
@@ -1349,7 +1532,7 @@ uint32_t nv108_pwr_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0x7e50fc04,
-	0xb600069e,
+	0xb6000856,
 	0x11f50464,
 	0xc5c700cc,
 	0x0076bbe0,
@@ -1357,8 +1540,8 @@ uint32_t nv108_pwr_code[] = {
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
-	0x447e50fc,
-	0x64b60006,
+	0xfc7e50fc,
+	0x64b60007,
 	0xa911f504,
 	0xbb010500,
 	0x65b60076,
@@ -1366,7 +1549,7 @@ uint32_t nv108_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x00069e7e,
+	0x0008567e,
 	0xf50464b6,
 	0xbb008711,
 	0x65b60076,
@@ -1374,7 +1557,7 @@ uint32_t nv108_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x0005f57e,
+	0x0007ad7e,
 	0xf40464b6,
 	0x5bcb6711,
 	0x0076bbe0,
@@ -1382,37 +1565,37 @@ uint32_t nv108_pwr_code[] = {
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
-	0x417e50fc,
-	0x64b60005,
+	0xf97e50fc,
+	0x64b60006,
 	0xbd5bb204,
 	0x410ef474,
-/* 0x0818: i2c_recv_not_rd08 */
+/* 0x09d0: i2c_recv_not_rd08 */
 	0xf401d6b0,
 	0x00053b1b,
-	0x00069e7e,
+	0x0008567e,
 	0xc73211f4,
-	0x447ee0c5,
-	0x11f40006,
+	0xfc7ee0c5,
+	0x11f40007,
 	0x7e000528,
-	0xf400069e,
+	0xf4000856,
 	0xb5c71f11,
-	0x06447ee0,
+	0x07fc7ee0,
 	0x1511f400,
-	0x0005417e,
+	0x0006f97e,
 	0xc5c774bd,
 	0x091bf408,
 	0xf40232f4,
-/* 0x0856: i2c_recv_not_wr08 */
-/* 0x0856: i2c_recv_done */
+/* 0x0a0e: i2c_recv_not_wr08 */
+/* 0x0a0e: i2c_recv_done */
 	0xcec7030e,
-	0x07027ef8,
+	0x08ba7ef8,
 	0xfce0fc00,
 	0x0912f4d0,
-	0x3f7e7cb2,
-/* 0x086a: i2c_recv_exit */
+	0xc27e7cb2,
+/* 0x0a22: i2c_recv_exit */
 	0x00f80002,
-/* 0x086c: i2c_init */
-/* 0x086e: test_recv */
+/* 0x0a24: i2c_init */
+/* 0x0a26: test_recv */
 	0x584100f8,
 	0x0011cf04,
 	0x400110b6,
@@ -1420,28 +1603,28 @@ uint32_t nv108_pwr_code[] = {
 	0xf104bd00,
 	0xf1d900e7,
 	0x7e134fe3,
-	0xf8000196,
-/* 0x088d: test_init */
+	0xf8000201,
+/* 0x0a45: test_init */
 	0x08004e00,
-	0x0001967e,
-/* 0x0896: idle_recv */
+	0x0002017e,
+/* 0x0a4e: idle_recv */
 	0x00f800f8,
-/* 0x0898: idle */
+/* 0x0a50: idle */
 	0x410031f4,
 	0x11cf0454,
 	0x0110b600,
 	0xf6045440,
 	0x04bd0001,
-/* 0x08ac: idle_loop */
+/* 0x0a64: idle_loop */
 	0x32f45801,
-/* 0x08b1: idle_proc */
-/* 0x08b1: idle_proc_exec */
+/* 0x0a69: idle_proc */
+/* 0x0a69: idle_proc_exec */
 	0xb210f902,
-	0x02487e1e,
+	0x02cb7e1e,
 	0xf410fc00,
 	0x31f40911,
 	0xf00ef402,
-/* 0x08c4: idle_proc_next */
+/* 0x0a7c: idle_proc_next */
 	0xa65810b6,
 	0xe81bf41f,
 	0xf4e002f4,
@@ -1457,4 +1640,22 @@ uint32_t nv108_pwr_code[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc
index 6744fcc06151..daa06c1c655e 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc
@@ -23,6 +23,7 @@
  */
 
 #define NVKM_PPWR_CHIPSET GT215
+#define HW_TICKS_PER_US 203 // should be 202.5
 
 //#define NVKM_FALCON_PC24
 //#define NVKM_FALCON_UNSHIFTED_IO
@@ -34,6 +35,7 @@
 .section #nva3_pwr_data
 #define INCLUDE_PROC
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -44,6 +46,7 @@
 
 #define INCLUDE_DATA
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -56,6 +59,7 @@
 .section #nva3_pwr_code
 #define INCLUDE_CODE
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h
index e087ce3041be..64e97baabc3c 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nva3.fuc.h
@@ -24,8 +24,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x00000430,
-	0x000003cd,
+	0x00000512,
+	0x000004af,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x00000542,
-	0x00000534,
+	0x000006e0,
+	0x000006d2,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x00000546,
-	0x00000544,
+	0x000006e4,
+	0x000006e2,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000976,
-	0x00000819,
+	0x00000b14,
+	0x000009b7,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x0000099f,
-	0x00000978,
+	0x00000b3d,
+	0x00000b16,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x000009ab,
-	0x000009a9,
+	0x00000b49,
+	0x00000b47,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -227,25 +227,31 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 /* 0x0370: memx_func_head */
-	0x00010000,
-	0x00000000,
-	0x0000046f,
-/* 0x037c: memx_func_next */
 	0x00000001,
 	0x00000000,
-	0x00000496,
+	0x00000551,
+/* 0x037c: memx_func_next */
 	0x00000002,
+	0x00000000,
+	0x000005a8,
+	0x00000003,
 	0x00000002,
-	0x000004b7,
-	0x00040003,
+	0x0000063a,
+	0x00040004,
+	0x00000000,
+	0x00000656,
+	0x00010005,
+	0x00000000,
+	0x00000673,
+	0x00010006,
 	0x00000000,
-	0x000004d3,
-	0x00010004,
+	0x000005f8,
+/* 0x03b8: memx_func_tail */
+/* 0x03b8: memx_ts_start */
 	0x00000000,
-	0x000004f0,
-/* 0x03ac: memx_func_tail */
-/* 0x03ac: memx_data_head */
+/* 0x03bc: memx_ts_end */
 	0x00000000,
+/* 0x03c0: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -757,8 +763,9 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bac: memx_data_tail */
-/* 0x0bac: i2c_scl_map */
+	0x00000000,
+/* 0x0bc0: memx_data_tail */
+/* 0x0bc0: i2c_scl_map */
 	0x00001000,
 	0x00004000,
 	0x00010000,
@@ -769,7 +776,7 @@ uint32_t nva3_pwr_data[] = {
 	0x01000000,
 	0x04000000,
 	0x10000000,
-/* 0x0bd4: i2c_sda_map */
+/* 0x0be8: i2c_sda_map */
 	0x00002000,
 	0x00008000,
 	0x00020000,
@@ -780,7 +787,7 @@ uint32_t nva3_pwr_data[] = {
 	0x02000000,
 	0x08000000,
 	0x20000000,
-/* 0x0bfc: i2c_ctrl */
+/* 0x0c10: i2c_ctrl */
 	0x0000e138,
 	0x0000e150,
 	0x0000e168,
@@ -841,15 +848,10 @@ uint32_t nva3_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
 };
 
 uint32_t nva3_pwr_code[] = {
-	0x030d0ef5,
+	0x039e0ef5,
 /* 0x0004: rd32 */
 	0x07a007f1,
 	0xd00604b6,
@@ -885,19 +887,22 @@ uint32_t nva3_pwr_code[] = {
 	0xd4f100dd,
 	0x1bf47000,
 /* 0x007f: nsec */
-	0xf000f8f2,
+	0xf900f8f2,
+	0xf080f990,
 	0x84b62c87,
 	0x0088cf06,
-/* 0x0088: nsec_loop */
+/* 0x008c: nsec_loop */
 	0xb62c97f0,
 	0x99cf0694,
 	0x0298bb00,
 	0xf4069eb8,
-	0x00f8f11e,
-/* 0x009c: wait */
+	0x80fcf11e,
+	0x00f890fc,
+/* 0x00a4: wait */
+	0x80f990f9,
 	0xb62c87f0,
 	0x88cf0684,
-/* 0x00a5: wait_loop */
+/* 0x00b1: wait_loop */
 	0x02eeb900,
 	0xb90421f4,
 	0xadfd02da,
@@ -907,28 +912,29 @@ uint32_t nva3_pwr_code[] = {
 	0x0099cf06,
 	0xb80298bb,
 	0x1ef4069b,
-/* 0x00c9: wait_done */
-/* 0x00cb: intr_watchdog */
-	0x9800f8df,
+/* 0x00d5: wait_done */
+	0xfc80fcdf,
+/* 0x00db: intr_watchdog */
+	0x9800f890,
 	0x96b003e9,
 	0x2a0bf400,
 	0xbb9a0a98,
 	0x1cf4029a,
 	0x01d7f00f,
-	0x025421f5,
+	0x02dd21f5,
 	0x0ef494bd,
-/* 0x00e9: intr_watchdog_next_time */
+/* 0x00f9: intr_watchdog_next_time */
 	0x9b0a9815,
 	0xf400a6b0,
 	0x9ab8090b,
 	0x061cf406,
-/* 0x00f8: intr_watchdog_next_time_set */
-/* 0x00fb: intr_watchdog_next_proc */
+/* 0x0108: intr_watchdog_next_time_set */
+/* 0x010b: intr_watchdog_next_proc */
 	0x809b0980,
 	0xe0b603e9,
 	0x68e6b158,
 	0xc61bf402,
-/* 0x010a: intr */
+/* 0x011a: intr */
 	0x00f900f8,
 	0x80f904bd,
 	0xa0f990f9,
@@ -948,13 +954,13 @@ uint32_t nva3_pwr_code[] = {
 	0xf40289c4,
 	0x0080230b,
 	0x58e7f09b,
-	0x98cb21f4,
+	0x98db21f4,
 	0x96b09b09,
 	0x110bf400,
 	0xb63407f0,
 	0x09d00604,
 	0x8004bd00,
-/* 0x016e: intr_skip_watchdog */
+/* 0x017e: intr_skip_watchdog */
 	0x89e49a09,
 	0x0bf40800,
 	0x8897f148,
@@ -967,22 +973,22 @@ uint32_t nva3_pwr_code[] = {
 	0x48e7f1c0,
 	0x53e3f14f,
 	0x00d7f054,
-	0x02b921f5,
+	0x034221f5,
 	0x07f1c0fc,
 	0x04b604c0,
 	0x000cd006,
-/* 0x01ae: intr_subintr_skip_fifo */
+/* 0x01be: intr_subintr_skip_fifo */
 	0x07f104bd,
 	0x04b60688,
 	0x0009d006,
-/* 0x01ba: intr_skip_subintr */
+/* 0x01ca: intr_skip_subintr */
 	0x89c404bd,
 	0x070bf420,
 	0xffbfa4f1,
-/* 0x01c4: intr_skip_pause */
+/* 0x01d4: intr_skip_pause */
 	0xf44089c4,
 	0xa4f1070b,
-/* 0x01ce: intr_skip_user0 */
+/* 0x01de: intr_skip_user0 */
 	0x07f0ffbf,
 	0x0604b604,
 	0xbd0008d0,
@@ -993,596 +999,732 @@ uint32_t nva3_pwr_code[] = {
 	0x90fca0fc,
 	0x00fc80fc,
 	0xf80032f4,
-/* 0x01f5: timer */
-	0x1032f401,
-	0xb003f898,
-	0x1cf40086,
-	0x03fe8051,
+/* 0x0205: ticks_from_ns */
+	0xf9c0f901,
+	0xcbd7f1b0,
+	0x00d3f000,
+	0x041321f5,
+	0x03e8ccec,
+	0xf400b4b0,
+	0xeeec120b,
+	0xd7f103e8,
+	0xd3f000cb,
+	0x1321f500,
+/* 0x022d: ticks_from_ns_quit */
+	0x02ceb904,
+	0xc0fcb0fc,
+/* 0x0236: ticks_from_us */
+	0xc0f900f8,
+	0xd7f1b0f9,
+	0xd3f000cb,
+	0x1321f500,
+	0x02ceb904,
+	0xf400b4b0,
+	0xe4bd050b,
+/* 0x0250: ticks_from_us_quit */
+	0xc0fcb0fc,
+/* 0x0256: ticks_to_us */
+	0xd7f100f8,
+	0xd3f000cb,
+	0xecedff00,
+/* 0x0262: timer */
+	0x90f900f8,
+	0x32f480f9,
+	0x03f89810,
+	0xf40086b0,
+	0x84bd651c,
 	0xb63807f0,
 	0x08d00604,
 	0xf004bd00,
-	0x84b60887,
+	0x84b63487,
 	0x0088cf06,
-	0xf40284f0,
-	0x87f0261b,
-	0x0684b634,
-	0xb80088cf,
-	0x0bf406e0,
-	0x06e8b809,
-/* 0x0233: timer_reset */
-	0xf01f1ef4,
-	0x04b63407,
-	0x000ed006,
-	0x0e8004bd,
-/* 0x0241: timer_enable */
-	0x0187f09a,
+	0xbb9a0998,
+	0xe9bb0298,
+	0x03fe8000,
+	0xb60887f0,
+	0x88cf0684,
+	0x0284f000,
+	0xf0261bf4,
+	0x84b63487,
+	0x0088cf06,
+	0xf406e0b8,
+	0xe8b8090b,
+	0x111cf406,
+/* 0x02b8: timer_reset */
+	0xb63407f0,
+	0x0ed00604,
+	0x8004bd00,
+/* 0x02c6: timer_enable */
+	0x87f09a0e,
+	0x3807f001,
+	0xd00604b6,
+	0x04bd0008,
+/* 0x02d4: timer_done */
+	0xfc1031f4,
+	0xf890fc80,
+/* 0x02dd: send_proc */
+	0xf980f900,
+	0x05e89890,
+	0xf004e998,
+	0x89b80486,
+	0x2a0bf406,
+	0x940398c4,
+	0x80b60488,
+	0x008ebb18,
+	0x8000fa98,
+	0x8d80008a,
+	0x028c8001,
+	0xb6038b80,
+	0x94f00190,
+	0x04e98007,
+/* 0x0317: send_done */
+	0xfc0231f4,
+	0xf880fc90,
+/* 0x031d: find */
+	0xf080f900,
+	0x31f45887,
+/* 0x0325: find_loop */
+	0x008a9801,
+	0xf406aeb8,
+	0x80b6100b,
+	0x6886b158,
+	0xf01bf402,
+/* 0x033b: find_done */
+	0xb90132f4,
+	0x80fc028e,
+/* 0x0342: send */
+	0x21f500f8,
+	0x01f4031d,
+/* 0x034b: recv */
+	0xf900f897,
+	0x9880f990,
+	0xe99805e8,
+	0x0132f404,
+	0xf40689b8,
+	0x89c43d0b,
+	0x0180b603,
+	0x800784f0,
+	0xea9805e8,
+	0xfef0f902,
+	0xf0f9018f,
+	0x9402efb9,
+	0xe9bb0499,
+	0x18e0b600,
+	0x9803eb98,
+	0xed9802ec,
+	0x00ee9801,
+	0xf0fca5f9,
+	0xf400f8fe,
+	0xf0fc0131,
+/* 0x0398: recv_done */
+	0x90fc80fc,
+/* 0x039e: init */
+	0x17f100f8,
+	0x14b60108,
+	0x0011cf06,
+	0x010911e7,
+	0xfe0814b6,
+	0x17f10014,
+	0x13f000e0,
+	0x1c07f000,
+	0xd00604b6,
+	0x04bd0001,
+	0xf0ff17f0,
+	0x04b61407,
+	0x0001d006,
+	0x17f004bd,
+	0x0015f102,
+	0x1007f008,
+	0xd00604b6,
+	0x04bd0001,
+	0x011a17f1,
+	0xfe0013f0,
+	0x31f40010,
+	0x0117f010,
 	0xb63807f0,
-	0x08d00604,
-/* 0x024f: timer_done */
-	0xf404bd00,
-	0x00f81031,
-/* 0x0254: send_proc */
-	0x90f980f9,
-	0x9805e898,
-	0x86f004e9,
-	0x0689b804,
-	0xc42a0bf4,
-	0x88940398,
-	0x1880b604,
-	0x98008ebb,
-	0x8a8000fa,
-	0x018d8000,
-	0x80028c80,
-	0x90b6038b,
-	0x0794f001,
-	0xf404e980,
-/* 0x028e: send_done */
-	0x90fc0231,
-	0x00f880fc,
-/* 0x0294: find */
-	0x87f080f9,
-	0x0131f458,
-/* 0x029c: find_loop */
-	0xb8008a98,
-	0x0bf406ae,
-	0x5880b610,
-	0x026886b1,
-	0xf4f01bf4,
-/* 0x02b2: find_done */
-	0x8eb90132,
-	0xf880fc02,
-/* 0x02b9: send */
-	0x9421f500,
-	0x9701f402,
-/* 0x02c2: recv */
-	0xe89800f8,
-	0x04e99805,
-	0xb80132f4,
-	0x0bf40689,
-	0x0389c43d,
-	0xf00180b6,
-	0xe8800784,
-	0x02ea9805,
-	0x8ffef0f9,
-	0xb9f0f901,
-	0x999402ef,
-	0x00e9bb04,
-	0x9818e0b6,
-	0xec9803eb,
-	0x01ed9802,
-	0xf900ee98,
-	0xfef0fca5,
-	0x31f400f8,
-/* 0x030b: recv_done */
-	0xf8f0fc01,
-/* 0x030d: init */
-	0x0817f100,
-	0x0614b601,
-	0xe70011cf,
-	0xb6010911,
-	0x14fe0814,
-	0xe017f100,
-	0x0013f000,
-	0xb61c07f0,
 	0x01d00604,
 	0xf004bd00,
-	0x07f0ff17,
-	0x0604b614,
-	0xbd0001d0,
-	0x0217f004,
-	0x080015f1,
-	0xb61007f0,
-	0x01d00604,
-	0xf104bd00,
-	0xf0010a17,
-	0x10fe0013,
-	0x1031f400,
-	0xf00117f0,
-	0x04b63807,
-	0x0001d006,
-	0xf7f004bd,
-/* 0x0371: init_proc */
-	0x01f19858,
-	0xf40016b0,
-	0x15f9fa0b,
-	0xf458f0b6,
-/* 0x0382: host_send */
-	0x17f1f20e,
-	0x14b604b0,
-	0x0011cf06,
-	0x04a027f1,
-	0xcf0624b6,
-	0x12b80022,
-	0x320bf406,
-	0x94071ec4,
-	0xe0b704ee,
-	0xeb980270,
-	0x02ec9803,
-	0x9801ed98,
-	0x21f500ee,
-	0x10b602b9,
-	0x0f1ec401,
-	0x04b007f1,
-	0xd00604b6,
-	0x04bd000e,
-/* 0x03cb: host_send_done */
-	0xf8ba0ef4,
-/* 0x03cd: host_recv */
-	0x4917f100,
-	0x5413f14e,
-	0x06e1b852,
-/* 0x03db: host_recv_wait */
-	0xf1aa0bf4,
-	0xb604cc17,
-	0x11cf0614,
-	0xc827f100,
-	0x0624b604,
-	0xf00022cf,
-	0x12b80816,
-	0xe60bf406,
-	0xb60723c4,
-	0x30b70434,
-	0x3b8002f0,
-	0x023c8003,
-	0x80013d80,
-	0x20b6003e,
-	0x0f24f001,
-	0x04c807f1,
+/* 0x0402: init_proc */
+	0xf19858f7,
+	0x0016b001,
+	0xf9fa0bf4,
+	0x58f0b615,
+/* 0x0413: mulu32_32_64 */
+	0xf9f20ef4,
+	0xf920f910,
+	0x9540f930,
+	0xd29510e1,
+	0xbdc4bd10,
+	0xc0edffb4,
+	0xb9301dff,
+	0x34f10234,
+	0x34b6ffff,
+	0x1045b610,
+	0xbb00c3bb,
+	0xe2ff01b4,
+	0x0234b930,
+	0xffff34f1,
+	0xb61034b6,
+	0xc3bb1045,
+	0x01b4bb00,
+	0xbb3012ff,
+	0x40fc00b3,
+	0x20fc30fc,
+	0x00f810fc,
+/* 0x0464: host_send */
+	0x04b017f1,
+	0xcf0614b6,
+	0x27f10011,
+	0x24b604a0,
+	0x0022cf06,
+	0xf40612b8,
+	0x1ec4320b,
+	0x04ee9407,
+	0x0270e0b7,
+	0x9803eb98,
+	0xed9802ec,
+	0x00ee9801,
+	0x034221f5,
+	0xc40110b6,
+	0x07f10f1e,
+	0x04b604b0,
+	0x000ed006,
+	0x0ef404bd,
+/* 0x04ad: host_send_done */
+/* 0x04af: host_recv */
+	0xf100f8ba,
+	0xf14e4917,
+	0xb8525413,
+	0x0bf406e1,
+/* 0x04bd: host_recv_wait */
+	0xcc17f1aa,
+	0x0614b604,
+	0xf10011cf,
+	0xb604c827,
+	0x22cf0624,
+	0x0816f000,
+	0xf40612b8,
+	0x23c4e60b,
+	0x0434b607,
+	0x02f030b7,
+	0x80033b80,
+	0x3d80023c,
+	0x003e8001,
+	0xf00120b6,
+	0x07f10f24,
+	0x04b604c8,
+	0x0002d006,
+	0x27f004bd,
+	0x0007f040,
 	0xd00604b6,
 	0x04bd0002,
-	0xf04027f0,
-	0x04b60007,
-	0x0002d006,
-	0x00f804bd,
-/* 0x0430: host_init */
-	0x008017f1,
-	0xf11014b6,
-	0xf1027015,
-	0xb604d007,
-	0x01d00604,
-	0xf104bd00,
-	0xb6008017,
-	0x15f11014,
-	0x07f102f0,
-	0x04b604dc,
-	0x0001d006,
-	0x17f004bd,
-	0xc407f101,
+/* 0x0512: host_init */
+	0x17f100f8,
+	0x14b60080,
+	0x7015f110,
+	0xd007f102,
 	0x0604b604,
 	0xbd0001d0,
-/* 0x046f: memx_func_enter */
-	0xf000f804,
+	0x8017f104,
+	0x1014b600,
+	0x02f015f1,
+	0x04dc07f1,
+	0xd00604b6,
+	0x04bd0001,
+	0xf10117f0,
+	0xb604c407,
+	0x01d00604,
+	0xf804bd00,
+/* 0x0551: memx_func_enter */
+	0x1087f100,
+	0x028eb916,
+	0xb90421f4,
+	0x67f102d7,
+	0x63f1fffc,
+	0x76fdffff,
+	0x0267f104,
+	0x0576fd00,
+	0x70f980f9,
+	0xe0fcd0fc,
+	0xf03f21f4,
 	0x07f10467,
 	0x04b607e0,
 	0x0006d006,
-/* 0x047e: memx_func_enter_wait */
+/* 0x058a: memx_func_enter_wait */
 	0x67f104bd,
 	0x64b607c0,
 	0x0066cf06,
 	0xf40464f0,
-	0x1698f30b,
-	0x0410b600,
-/* 0x0496: memx_func_leave */
-	0x67f000f8,
-	0xe407f104,
-	0x0604b607,
-	0xbd0006d0,
-/* 0x04a5: memx_func_leave_wait */
-	0xc067f104,
+	0x67f0f30b,
+	0x0664b62c,
+	0x800066cf,
+	0x00f8ee06,
+/* 0x05a8: memx_func_leave */
+	0xb62c67f0,
+	0x66cf0664,
+	0xef068000,
+	0xf10467f0,
+	0xb607e407,
+	0x06d00604,
+/* 0x05c3: memx_func_leave_wait */
+	0xf104bd00,
+	0xb607c067,
+	0x66cf0664,
+	0x0464f000,
+	0xf1f31bf4,
+	0xb9161087,
+	0x21f4028e,
+	0x02d7b904,
+	0xffcc67f1,
+	0xffff63f1,
+	0xf90476fd,
+	0xfc70f980,
+	0xf4e0fcd0,
+	0x00f83f21,
+/* 0x05f8: memx_func_wait_vblank */
+	0xb0001698,
+	0x0bf40066,
+	0x0166b013,
+	0xf4060bf4,
+/* 0x060a: memx_func_wait_vblank_head1 */
+	0x77f12e0e,
+	0x0ef40020,
+/* 0x0611: memx_func_wait_vblank_head0 */
+	0x0877f107,
+/* 0x0615: memx_func_wait_vblank_0 */
+	0xc467f100,
 	0x0664b607,
-	0xf00066cf,
-	0x1bf40464,
-/* 0x04b7: memx_func_wr32 */
-	0x9800f8f3,
-	0x15980016,
-	0x0810b601,
-	0x50f960f9,
-	0xe0fcd0fc,
-	0xb63f21f4,
-	0x1bf40242,
-/* 0x04d3: memx_func_wait */
-	0xf000f8e9,
-	0x84b62c87,
-	0x0088cf06,
-	0x98001e98,
-	0x1c98011d,
-	0x031b9802,
-	0xf41010b6,
-	0x00f89c21,
-/* 0x04f0: memx_func_delay */
-	0xb6001e98,
-	0x21f40410,
-/* 0x04fb: memx_exec */
-	0xf900f87f,
-	0xb9d0f9e0,
-	0xb2b902c1,
-/* 0x0505: memx_exec_next */
-	0x00139802,
-	0x950410b6,
-	0x30f01034,
-	0xde35980c,
-	0x12b855f9,
-	0xec1ef406,
-	0xe0fcd0fc,
-	0x02b921f5,
-/* 0x0526: memx_info */
-	0xc7f100f8,
-	0xb7f103ac,
-	0x21f50800,
-	0x00f802b9,
-/* 0x0534: memx_recv */
-	0xf401d6b0,
-	0xd6b0c40b,
-	0xe90bf400,
-/* 0x0542: memx_init */
-	0x00f800f8,
-/* 0x0544: perf_recv */
-/* 0x0546: perf_init */
+	0xfd0066cf,
+	0x1bf40467,
+/* 0x0625: memx_func_wait_vblank_1 */
+	0xc467f1f3,
+	0x0664b607,
+	0xfd0066cf,
+	0x0bf40467,
+/* 0x0635: memx_func_wait_vblank_fini */
+	0x0410b6f3,
+/* 0x063a: memx_func_wr32 */
+	0x169800f8,
+	0x01159800,
+	0xf90810b6,
+	0xfc50f960,
+	0xf4e0fcd0,
+	0x42b63f21,
+	0xe91bf402,
+/* 0x0656: memx_func_wait */
+	0x87f000f8,
+	0x0684b62c,
+	0x980088cf,
+	0x1d98001e,
+	0x021c9801,
+	0xb6031b98,
+	0x21f41010,
+/* 0x0673: memx_func_delay */
+	0x9800f8a4,
+	0x10b6001e,
+	0x7f21f404,
+/* 0x067e: memx_exec */
+	0xe0f900f8,
+	0xc1b9d0f9,
+	0x02b2b902,
+/* 0x0688: memx_exec_next */
+	0xb6001398,
+	0x34e70410,
+	0x33e701f0,
+	0x32b601e0,
+	0x0c30f001,
+	0xf9de3598,
+	0x0612b855,
+	0x98e41ef4,
+	0x0c98ee0b,
+	0x02cbbbef,
+	0x07c4b7f1,
+	0xcf06b4b6,
+	0xd0fc00bb,
+	0x21f5e0fc,
+	0x00f80342,
+/* 0x06c4: memx_info */
+	0x03c0c7f1,
+	0x0800b7f1,
+	0x034221f5,
+/* 0x06d2: memx_recv */
+	0xd6b000f8,
+	0xa90bf401,
+	0xf400d6b0,
+	0x00f8e90b,
+/* 0x06e0: memx_init */
+/* 0x06e2: perf_recv */
 	0x00f800f8,
-/* 0x0548: i2c_drive_scl */
-	0xf40036b0,
-	0x07f1110b,
-	0x04b607e0,
-	0x0001d006,
-	0x00f804bd,
-/* 0x055c: i2c_drive_scl_lo */
-	0x07e407f1,
-	0xd00604b6,
-	0x04bd0001,
-/* 0x056a: i2c_drive_sda */
+/* 0x06e4: perf_init */
+/* 0x06e6: i2c_drive_scl */
 	0x36b000f8,
 	0x110bf400,
 	0x07e007f1,
 	0xd00604b6,
-	0x04bd0002,
-/* 0x057e: i2c_drive_sda_lo */
+	0x04bd0001,
+/* 0x06fa: i2c_drive_scl_lo */
 	0x07f100f8,
 	0x04b607e4,
+	0x0001d006,
+	0x00f804bd,
+/* 0x0708: i2c_drive_sda */
+	0xf40036b0,
+	0x07f1110b,
+	0x04b607e0,
 	0x0002d006,
 	0x00f804bd,
-/* 0x058c: i2c_sense_scl */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0431fd00,
-	0xf4060bf4,
-/* 0x05a2: i2c_sense_scl_done */
-	0x00f80131,
-/* 0x05a4: i2c_sense_sda */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0432fd00,
-	0xf4060bf4,
-/* 0x05ba: i2c_sense_sda_done */
-	0x00f80131,
-/* 0x05bc: i2c_raise_scl */
-	0x47f140f9,
-	0x37f00898,
-	0x4821f501,
-/* 0x05c9: i2c_raise_scl_wait */
-	0xe8e7f105,
-	0x7f21f403,
-	0x058c21f5,
-	0xb60901f4,
-	0x1bf40142,
-/* 0x05dd: i2c_raise_scl_done */
-	0xf840fcef,
-/* 0x05e1: i2c_start */
-	0x8c21f500,
-	0x0d11f405,
-	0x05a421f5,
-	0xf40611f4,
-/* 0x05f2: i2c_start_rep */
-	0x37f0300e,
-	0x4821f500,
-	0x0137f005,
-	0x056a21f5,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xbc21f550,
-	0x0464b605,
-/* 0x061f: i2c_start_send */
-	0xf01f11f4,
+/* 0x071c: i2c_drive_sda_lo */
+	0x07e407f1,
+	0xd00604b6,
+	0x04bd0002,
+/* 0x072a: i2c_sense_scl */
+	0x32f400f8,
+	0xc437f101,
+	0x0634b607,
+	0xfd0033cf,
+	0x0bf40431,
+	0x0131f406,
+/* 0x0740: i2c_sense_scl_done */
+/* 0x0742: i2c_sense_sda */
+	0x32f400f8,
+	0xc437f101,
+	0x0634b607,
+	0xfd0033cf,
+	0x0bf40432,
+	0x0131f406,
+/* 0x0758: i2c_sense_sda_done */
+/* 0x075a: i2c_raise_scl */
+	0x40f900f8,
+	0x089847f1,
+	0xf50137f0,
+/* 0x0767: i2c_raise_scl_wait */
+	0xf106e621,
+	0xf403e8e7,
+	0x21f57f21,
+	0x01f4072a,
+	0x0142b609,
+/* 0x077b: i2c_raise_scl_done */
+	0xfcef1bf4,
+/* 0x077f: i2c_start */
+	0xf500f840,
+	0xf4072a21,
+	0x21f50d11,
+	0x11f40742,
+	0x300ef406,
+/* 0x0790: i2c_start_rep */
+	0xf50037f0,
+	0xf006e621,
+	0x21f50137,
+	0x76bb0708,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb6075a21,
+	0x11f40464,
+/* 0x07bd: i2c_start_send */
+	0x0037f01f,
+	0x070821f5,
+	0x1388e7f1,
+	0xf07f21f4,
 	0x21f50037,
-	0xe7f1056a,
+	0xe7f106e6,
 	0x21f41388,
-	0x0037f07f,
-	0x054821f5,
-	0x1388e7f1,
-/* 0x063b: i2c_start_out */
-	0xf87f21f4,
-/* 0x063d: i2c_stop */
-	0x0037f000,
-	0x054821f5,
-	0xf50037f0,
-	0xf1056a21,
-	0xf403e8e7,
-	0x37f07f21,
-	0x4821f501,
-	0x88e7f105,
-	0x7f21f413,
+/* 0x07d9: i2c_start_out */
+/* 0x07db: i2c_stop */
+	0xf000f87f,
+	0x21f50037,
+	0x37f006e6,
+	0x0821f500,
+	0xe8e7f107,
+	0x7f21f403,
 	0xf50137f0,
-	0xf1056a21,
+	0xf106e621,
 	0xf41388e7,
-	0x00f87f21,
-/* 0x0670: i2c_bitw */
-	0x056a21f5,
-	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x05bc21f5,
-	0xf40464b6,
-	0xe7f11811,
-	0x21f41388,
-	0x0037f07f,
-	0x054821f5,
-	0x1388e7f1,
-/* 0x06af: i2c_bitw_out */
-	0xf87f21f4,
-/* 0x06b1: i2c_bitr */
-	0x0137f000,
-	0x056a21f5,
-	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x05bc21f5,
-	0xf40464b6,
-	0x21f51b11,
-	0x37f005a4,
-	0x4821f500,
-	0x88e7f105,
+	0x37f07f21,
+	0x0821f501,
+	0x88e7f107,
 	0x7f21f413,
-	0xf4013cf0,
-/* 0x06f6: i2c_bitr_done */
-	0x00f80131,
-/* 0x06f8: i2c_get_byte */
-	0xf00057f0,
-/* 0x06fe: i2c_get_byte_next */
-	0x54b60847,
-	0x0076bb01,
+/* 0x080e: i2c_bitw */
+	0x21f500f8,
+	0xe7f10708,
+	0x21f403e8,
+	0x0076bb7f,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b606b1,
-	0x2b11f404,
-	0xb60553fd,
-	0x1bf40142,
-	0x0137f0d8,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x7021f550,
-	0x0464b606,
-/* 0x0748: i2c_get_byte_done */
-/* 0x074a: i2c_put_byte */
-	0x47f000f8,
-/* 0x074d: i2c_put_byte_next */
-	0x0142b608,
-	0xbb3854ff,
+	0x64b6075a,
+	0x1811f404,
+	0x1388e7f1,
+	0xf07f21f4,
+	0x21f50037,
+	0xe7f106e6,
+	0x21f41388,
+/* 0x084d: i2c_bitw_out */
+/* 0x084f: i2c_bitr */
+	0xf000f87f,
+	0x21f50137,
+	0xe7f10708,
+	0x21f403e8,
+	0x0076bb7f,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b6075a,
+	0x1b11f404,
+	0x074221f5,
+	0xf50037f0,
+	0xf106e621,
+	0xf41388e7,
+	0x3cf07f21,
+	0x0131f401,
+/* 0x0894: i2c_bitr_done */
+/* 0x0896: i2c_get_byte */
+	0x57f000f8,
+	0x0847f000,
+/* 0x089c: i2c_get_byte_next */
+	0xbb0154b6,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x067021f5,
+	0x084f21f5,
 	0xf40464b6,
-	0x46b03411,
-	0xd81bf400,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xb121f550,
-	0x0464b606,
-	0xbb0f11f4,
-	0x36b00076,
-	0x061bf401,
-/* 0x07a3: i2c_put_byte_done */
-	0xf80132f4,
-/* 0x07a5: i2c_addr */
-	0x0076bb00,
+	0x53fd2b11,
+	0x0142b605,
+	0xf0d81bf4,
+	0x76bb0137,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb6080e21,
+/* 0x08e6: i2c_get_byte_done */
+	0x00f80464,
+/* 0x08e8: i2c_put_byte */
+/* 0x08eb: i2c_put_byte_next */
+	0xb60847f0,
+	0x54ff0142,
+	0x0076bb38,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b605e1,
-	0x2911f404,
-	0x012ec3e7,
-	0xfd0134b6,
-	0x76bb0553,
+	0x64b6080e,
+	0x3411f404,
+	0xf40046b0,
+	0x76bbd81b,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6074a21,
-/* 0x07ea: i2c_addr_done */
-	0x00f80464,
-/* 0x07ec: i2c_acquire_addr */
-	0xb6f8cec7,
-	0xe0b702e4,
-	0xee980bfc,
-/* 0x07fb: i2c_acquire */
-	0xf500f800,
-	0xf407ec21,
-	0xd9f00421,
-	0x3f21f403,
-/* 0x080a: i2c_release */
-	0x21f500f8,
-	0x21f407ec,
-	0x03daf004,
-	0xf83f21f4,
-/* 0x0819: i2c_recv */
-	0x0132f400,
-	0xb6f8c1c7,
-	0x16b00214,
-	0x3a1ff528,
-	0xd413a001,
-	0x0032980b,
-	0x0bac13a0,
-	0xf4003198,
-	0xd0f90231,
-	0xd0f9e0f9,
-	0x000067f1,
-	0x100063f1,
-	0xbb016792,
+	0xb6084f21,
+	0x11f40464,
+	0x0076bb0f,
+	0xf40136b0,
+	0x32f4061b,
+/* 0x0941: i2c_put_byte_done */
+/* 0x0943: i2c_addr */
+	0xbb00f801,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x07fb21f5,
-	0xfc0464b6,
-	0x00d6b0d0,
-	0x00b31bf5,
-	0xbb0057f0,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x07a521f5,
-	0xf50464b6,
-	0xc700d011,
-	0x76bbe0c5,
-	0x0465b600,
-	0x659450f9,
-	0x0256bb04,
-	0x75fd50bd,
-	0xf550fc04,
-	0xb6074a21,
-	0x11f50464,
-	0x57f000ad,
+	0x077f21f5,
+	0xf40464b6,
+	0xc3e72911,
+	0x34b6012e,
+	0x0553fd01,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xe821f550,
+	0x0464b608,
+/* 0x0988: i2c_addr_done */
+/* 0x098a: i2c_acquire_addr */
+	0xcec700f8,
+	0x02e4b6f8,
+	0x0c10e0b7,
+	0xf800ee98,
+/* 0x0999: i2c_acquire */
+	0x8a21f500,
+	0x0421f409,
+	0xf403d9f0,
+	0x00f83f21,
+/* 0x09a8: i2c_release */
+	0x098a21f5,
+	0xf00421f4,
+	0x21f403da,
+/* 0x09b7: i2c_recv */
+	0xf400f83f,
+	0xc1c70132,
+	0x0214b6f8,
+	0xf52816b0,
+	0xa0013a1f,
+	0x980be813,
+	0x13a00032,
+	0x31980bc0,
+	0x0231f400,
+	0xe0f9d0f9,
+	0x67f1d0f9,
+	0x63f10000,
+	0x67921000,
 	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b607a5,
-	0x8a11f504,
+	0x64b60999,
+	0xb0d0fc04,
+	0x1bf500d6,
+	0x57f000b3,
 	0x0076bb00,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b606f8,
-	0x6a11f404,
-	0xbbe05bcb,
+	0x64b60943,
+	0xd011f504,
+	0xe0c5c700,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xe821f550,
+	0x0464b608,
+	0x00ad11f5,
+	0xbb0157f0,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x063d21f5,
-	0xb90464b6,
-	0x74bd025b,
-/* 0x091f: i2c_recv_not_rd08 */
-	0xb0430ef4,
-	0x1bf401d6,
-	0x0057f03d,
-	0x07a521f5,
-	0xc73311f4,
-	0x21f5e0c5,
-	0x11f4074a,
-	0x0057f029,
-	0x07a521f5,
-	0xc71f11f4,
-	0x21f5e0b5,
-	0x11f4074a,
-	0x3d21f515,
-	0xc774bd06,
-	0x1bf408c5,
-	0x0232f409,
-/* 0x095f: i2c_recv_not_wr08 */
-/* 0x095f: i2c_recv_done */
-	0xc7030ef4,
-	0x21f5f8ce,
-	0xe0fc080a,
-	0x12f4d0fc,
-	0x027cb90a,
-	0x02b921f5,
-/* 0x0974: i2c_recv_exit */
-/* 0x0976: i2c_init */
-	0x00f800f8,
-/* 0x0978: test_recv */
-	0x05d817f1,
+	0x094321f5,
+	0xf50464b6,
+	0xbb008a11,
+	0x65b60076,
+	0x9450f904,
+	0x56bb0465,
+	0xfd50bd02,
+	0x50fc0475,
+	0x089621f5,
+	0xf40464b6,
+	0x5bcb6a11,
+	0x0076bbe0,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b607db,
+	0x025bb904,
+	0x0ef474bd,
+/* 0x0abd: i2c_recv_not_rd08 */
+	0x01d6b043,
+	0xf03d1bf4,
+	0x21f50057,
+	0x11f40943,
+	0xe0c5c733,
+	0x08e821f5,
+	0xf02911f4,
+	0x21f50057,
+	0x11f40943,
+	0xe0b5c71f,
+	0x08e821f5,
+	0xf51511f4,
+	0xbd07db21,
+	0x08c5c774,
+	0xf4091bf4,
+	0x0ef40232,
+/* 0x0afd: i2c_recv_not_wr08 */
+/* 0x0afd: i2c_recv_done */
+	0xf8cec703,
+	0x09a821f5,
+	0xd0fce0fc,
+	0xb90a12f4,
+	0x21f5027c,
+/* 0x0b12: i2c_recv_exit */
+	0x00f80342,
+/* 0x0b14: i2c_init */
+/* 0x0b16: test_recv */
+	0x17f100f8,
+	0x14b605d8,
+	0x0011cf06,
+	0xf10110b6,
+	0xb605d807,
+	0x01d00604,
+	0xf104bd00,
+	0xf1d900e7,
+	0xf5134fe3,
+	0xf8026221,
+/* 0x0b3d: test_init */
+	0x00e7f100,
+	0x6221f508,
+/* 0x0b47: idle_recv */
+	0xf800f802,
+/* 0x0b49: idle */
+	0x0031f400,
+	0x05d417f1,
 	0xcf0614b6,
 	0x10b60011,
-	0xd807f101,
+	0xd407f101,
 	0x0604b605,
 	0xbd0001d0,
-	0x00e7f104,
-	0x4fe3f1d9,
-	0xf521f513,
-/* 0x099f: test_init */
-	0xf100f801,
-	0xf50800e7,
-	0xf801f521,
-/* 0x09a9: idle_recv */
-/* 0x09ab: idle */
-	0xf400f800,
-	0x17f10031,
-	0x14b605d4,
-	0x0011cf06,
-	0xf10110b6,
-	0xb605d407,
-	0x01d00604,
-/* 0x09c7: idle_loop */
-	0xf004bd00,
-	0x32f45817,
-/* 0x09cd: idle_proc */
-/* 0x09cd: idle_proc_exec */
-	0xb910f902,
-	0x21f5021e,
-	0x10fc02c2,
-	0xf40911f4,
-	0x0ef40231,
-/* 0x09e1: idle_proc_next */
-	0x5810b6ef,
-	0xf4061fb8,
-	0x02f4e61b,
-	0x0028f4dd,
-	0x00bb0ef4,
+/* 0x0b65: idle_loop */
+	0x5817f004,
+/* 0x0b6b: idle_proc */
+/* 0x0b6b: idle_proc_exec */
+	0xf90232f4,
+	0x021eb910,
+	0x034b21f5,
+	0x11f410fc,
+	0x0231f409,
+/* 0x0b7f: idle_proc_next */
+	0xb6ef0ef4,
+	0x1fb85810,
+	0xe61bf406,
+	0xf4dd02f4,
+	0x0ef40028,
+	0x000000bb,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 	0x00000000,
 	0x00000000,
 	0x00000000,
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc
index 48f79434a449..21bf8cc7618f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc
@@ -23,6 +23,7 @@
  */
 
 #define NVKM_PPWR_CHIPSET GF100
+#define HW_TICKS_PER_US 203 // should be 202.5
 
 //#define NVKM_FALCON_PC24
 //#define NVKM_FALCON_UNSHIFTED_IO
@@ -34,6 +35,7 @@
 .section #nvc0_pwr_data
 #define INCLUDE_PROC
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -44,6 +46,7 @@
 
 #define INCLUDE_DATA
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -56,6 +59,7 @@
 .section #nvc0_pwr_code
 #define INCLUDE_CODE
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h
index 0773ff0e3dc3..ca30fa4011b5 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvc0.fuc.h
@@ -24,8 +24,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x00000430,
-	0x000003cd,
+	0x00000512,
+	0x000004af,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x00000542,
-	0x00000534,
+	0x0000074b,
+	0x0000073d,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x00000546,
-	0x00000544,
+	0x0000074f,
+	0x0000074d,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x00000976,
-	0x00000819,
+	0x00000b7f,
+	0x00000a22,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x0000099f,
-	0x00000978,
+	0x00000ba8,
+	0x00000b81,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x000009ab,
-	0x000009a9,
+	0x00000bb4,
+	0x00000bb2,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -227,25 +227,31 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 /* 0x0370: memx_func_head */
-	0x00010000,
-	0x00000000,
-	0x0000046f,
-/* 0x037c: memx_func_next */
 	0x00000001,
 	0x00000000,
-	0x00000496,
+	0x00000551,
+/* 0x037c: memx_func_next */
 	0x00000002,
+	0x00000000,
+	0x000005db,
+	0x00000003,
 	0x00000002,
-	0x000004b7,
-	0x00040003,
+	0x000006a5,
+	0x00040004,
+	0x00000000,
+	0x000006c1,
+	0x00010005,
+	0x00000000,
+	0x000006de,
+	0x00010006,
 	0x00000000,
-	0x000004d3,
-	0x00010004,
+	0x00000663,
+/* 0x03b8: memx_func_tail */
+/* 0x03b8: memx_ts_start */
 	0x00000000,
-	0x000004f0,
-/* 0x03ac: memx_func_tail */
-/* 0x03ac: memx_data_head */
+/* 0x03bc: memx_ts_end */
 	0x00000000,
+/* 0x03c0: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -757,8 +763,9 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bac: memx_data_tail */
-/* 0x0bac: i2c_scl_map */
+	0x00000000,
+/* 0x0bc0: memx_data_tail */
+/* 0x0bc0: i2c_scl_map */
 	0x00001000,
 	0x00004000,
 	0x00010000,
@@ -769,7 +776,7 @@ uint32_t nvc0_pwr_data[] = {
 	0x01000000,
 	0x04000000,
 	0x10000000,
-/* 0x0bd4: i2c_sda_map */
+/* 0x0be8: i2c_sda_map */
 	0x00002000,
 	0x00008000,
 	0x00020000,
@@ -780,7 +787,7 @@ uint32_t nvc0_pwr_data[] = {
 	0x02000000,
 	0x08000000,
 	0x20000000,
-/* 0x0bfc: i2c_ctrl */
+/* 0x0c10: i2c_ctrl */
 	0x0000e138,
 	0x0000e150,
 	0x0000e168,
@@ -841,15 +848,10 @@ uint32_t nvc0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
-	0x00000000,
 };
 
 uint32_t nvc0_pwr_code[] = {
-	0x030d0ef5,
+	0x039e0ef5,
 /* 0x0004: rd32 */
 	0x07a007f1,
 	0xd00604b6,
@@ -885,19 +887,22 @@ uint32_t nvc0_pwr_code[] = {
 	0xd4f100dd,
 	0x1bf47000,
 /* 0x007f: nsec */
-	0xf000f8f2,
+	0xf900f8f2,
+	0xf080f990,
 	0x84b62c87,
 	0x0088cf06,
-/* 0x0088: nsec_loop */
+/* 0x008c: nsec_loop */
 	0xb62c97f0,
 	0x99cf0694,
 	0x0298bb00,
 	0xf4069eb8,
-	0x00f8f11e,
-/* 0x009c: wait */
+	0x80fcf11e,
+	0x00f890fc,
+/* 0x00a4: wait */
+	0x80f990f9,
 	0xb62c87f0,
 	0x88cf0684,
-/* 0x00a5: wait_loop */
+/* 0x00b1: wait_loop */
 	0x02eeb900,
 	0xb90421f4,
 	0xadfd02da,
@@ -907,28 +912,29 @@ uint32_t nvc0_pwr_code[] = {
 	0x0099cf06,
 	0xb80298bb,
 	0x1ef4069b,
-/* 0x00c9: wait_done */
-/* 0x00cb: intr_watchdog */
-	0x9800f8df,
+/* 0x00d5: wait_done */
+	0xfc80fcdf,
+/* 0x00db: intr_watchdog */
+	0x9800f890,
 	0x96b003e9,
 	0x2a0bf400,
 	0xbb9a0a98,
 	0x1cf4029a,
 	0x01d7f00f,
-	0x025421f5,
+	0x02dd21f5,
 	0x0ef494bd,
-/* 0x00e9: intr_watchdog_next_time */
+/* 0x00f9: intr_watchdog_next_time */
 	0x9b0a9815,
 	0xf400a6b0,
 	0x9ab8090b,
 	0x061cf406,
-/* 0x00f8: intr_watchdog_next_time_set */
-/* 0x00fb: intr_watchdog_next_proc */
+/* 0x0108: intr_watchdog_next_time_set */
+/* 0x010b: intr_watchdog_next_proc */
 	0x809b0980,
 	0xe0b603e9,
 	0x68e6b158,
 	0xc61bf402,
-/* 0x010a: intr */
+/* 0x011a: intr */
 	0x00f900f8,
 	0x80f904bd,
 	0xa0f990f9,
@@ -948,13 +954,13 @@ uint32_t nvc0_pwr_code[] = {
 	0xf40289c4,
 	0x0080230b,
 	0x58e7f09b,
-	0x98cb21f4,
+	0x98db21f4,
 	0x96b09b09,
 	0x110bf400,
 	0xb63407f0,
 	0x09d00604,
 	0x8004bd00,
-/* 0x016e: intr_skip_watchdog */
+/* 0x017e: intr_skip_watchdog */
 	0x89e49a09,
 	0x0bf40800,
 	0x8897f148,
@@ -967,22 +973,22 @@ uint32_t nvc0_pwr_code[] = {
 	0x48e7f1c0,
 	0x53e3f14f,
 	0x00d7f054,
-	0x02b921f5,
+	0x034221f5,
 	0x07f1c0fc,
 	0x04b604c0,
 	0x000cd006,
-/* 0x01ae: intr_subintr_skip_fifo */
+/* 0x01be: intr_subintr_skip_fifo */
 	0x07f104bd,
 	0x04b60688,
 	0x0009d006,
-/* 0x01ba: intr_skip_subintr */
+/* 0x01ca: intr_skip_subintr */
 	0x89c404bd,
 	0x070bf420,
 	0xffbfa4f1,
-/* 0x01c4: intr_skip_pause */
+/* 0x01d4: intr_skip_pause */
 	0xf44089c4,
 	0xa4f1070b,
-/* 0x01ce: intr_skip_user0 */
+/* 0x01de: intr_skip_user0 */
 	0x07f0ffbf,
 	0x0604b604,
 	0xbd0008d0,
@@ -993,597 +999,733 @@ uint32_t nvc0_pwr_code[] = {
 	0x90fca0fc,
 	0x00fc80fc,
 	0xf80032f4,
-/* 0x01f5: timer */
-	0x1032f401,
-	0xb003f898,
-	0x1cf40086,
-	0x03fe8051,
+/* 0x0205: ticks_from_ns */
+	0xf9c0f901,
+	0xcbd7f1b0,
+	0x00d3f000,
+	0x041321f5,
+	0x03e8ccec,
+	0xf400b4b0,
+	0xeeec120b,
+	0xd7f103e8,
+	0xd3f000cb,
+	0x1321f500,
+/* 0x022d: ticks_from_ns_quit */
+	0x02ceb904,
+	0xc0fcb0fc,
+/* 0x0236: ticks_from_us */
+	0xc0f900f8,
+	0xd7f1b0f9,
+	0xd3f000cb,
+	0x1321f500,
+	0x02ceb904,
+	0xf400b4b0,
+	0xe4bd050b,
+/* 0x0250: ticks_from_us_quit */
+	0xc0fcb0fc,
+/* 0x0256: ticks_to_us */
+	0xd7f100f8,
+	0xd3f000cb,
+	0xecedff00,
+/* 0x0262: timer */
+	0x90f900f8,
+	0x32f480f9,
+	0x03f89810,
+	0xf40086b0,
+	0x84bd651c,
 	0xb63807f0,
 	0x08d00604,
 	0xf004bd00,
-	0x84b60887,
+	0x84b63487,
 	0x0088cf06,
-	0xf40284f0,
-	0x87f0261b,
-	0x0684b634,
-	0xb80088cf,
-	0x0bf406e0,
-	0x06e8b809,
-/* 0x0233: timer_reset */
-	0xf01f1ef4,
-	0x04b63407,
-	0x000ed006,
-	0x0e8004bd,
-/* 0x0241: timer_enable */
-	0x0187f09a,
+	0xbb9a0998,
+	0xe9bb0298,
+	0x03fe8000,
+	0xb60887f0,
+	0x88cf0684,
+	0x0284f000,
+	0xf0261bf4,
+	0x84b63487,
+	0x0088cf06,
+	0xf406e0b8,
+	0xe8b8090b,
+	0x111cf406,
+/* 0x02b8: timer_reset */
+	0xb63407f0,
+	0x0ed00604,
+	0x8004bd00,
+/* 0x02c6: timer_enable */
+	0x87f09a0e,
+	0x3807f001,
+	0xd00604b6,
+	0x04bd0008,
+/* 0x02d4: timer_done */
+	0xfc1031f4,
+	0xf890fc80,
+/* 0x02dd: send_proc */
+	0xf980f900,
+	0x05e89890,
+	0xf004e998,
+	0x89b80486,
+	0x2a0bf406,
+	0x940398c4,
+	0x80b60488,
+	0x008ebb18,
+	0x8000fa98,
+	0x8d80008a,
+	0x028c8001,
+	0xb6038b80,
+	0x94f00190,
+	0x04e98007,
+/* 0x0317: send_done */
+	0xfc0231f4,
+	0xf880fc90,
+/* 0x031d: find */
+	0xf080f900,
+	0x31f45887,
+/* 0x0325: find_loop */
+	0x008a9801,
+	0xf406aeb8,
+	0x80b6100b,
+	0x6886b158,
+	0xf01bf402,
+/* 0x033b: find_done */
+	0xb90132f4,
+	0x80fc028e,
+/* 0x0342: send */
+	0x21f500f8,
+	0x01f4031d,
+/* 0x034b: recv */
+	0xf900f897,
+	0x9880f990,
+	0xe99805e8,
+	0x0132f404,
+	0xf40689b8,
+	0x89c43d0b,
+	0x0180b603,
+	0x800784f0,
+	0xea9805e8,
+	0xfef0f902,
+	0xf0f9018f,
+	0x9402efb9,
+	0xe9bb0499,
+	0x18e0b600,
+	0x9803eb98,
+	0xed9802ec,
+	0x00ee9801,
+	0xf0fca5f9,
+	0xf400f8fe,
+	0xf0fc0131,
+/* 0x0398: recv_done */
+	0x90fc80fc,
+/* 0x039e: init */
+	0x17f100f8,
+	0x14b60108,
+	0x0011cf06,
+	0x010911e7,
+	0xfe0814b6,
+	0x17f10014,
+	0x13f000e0,
+	0x1c07f000,
+	0xd00604b6,
+	0x04bd0001,
+	0xf0ff17f0,
+	0x04b61407,
+	0x0001d006,
+	0x17f004bd,
+	0x0015f102,
+	0x1007f008,
+	0xd00604b6,
+	0x04bd0001,
+	0x011a17f1,
+	0xfe0013f0,
+	0x31f40010,
+	0x0117f010,
 	0xb63807f0,
-	0x08d00604,
-/* 0x024f: timer_done */
-	0xf404bd00,
-	0x00f81031,
-/* 0x0254: send_proc */
-	0x90f980f9,
-	0x9805e898,
-	0x86f004e9,
-	0x0689b804,
-	0xc42a0bf4,
-	0x88940398,
-	0x1880b604,
-	0x98008ebb,
-	0x8a8000fa,
-	0x018d8000,
-	0x80028c80,
-	0x90b6038b,
-	0x0794f001,
-	0xf404e980,
-/* 0x028e: send_done */
-	0x90fc0231,
-	0x00f880fc,
-/* 0x0294: find */
-	0x87f080f9,
-	0x0131f458,
-/* 0x029c: find_loop */
-	0xb8008a98,
-	0x0bf406ae,
-	0x5880b610,
-	0x026886b1,
-	0xf4f01bf4,
-/* 0x02b2: find_done */
-	0x8eb90132,
-	0xf880fc02,
-/* 0x02b9: send */
-	0x9421f500,
-	0x9701f402,
-/* 0x02c2: recv */
-	0xe89800f8,
-	0x04e99805,
-	0xb80132f4,
-	0x0bf40689,
-	0x0389c43d,
-	0xf00180b6,
-	0xe8800784,
-	0x02ea9805,
-	0x8ffef0f9,
-	0xb9f0f901,
-	0x999402ef,
-	0x00e9bb04,
-	0x9818e0b6,
-	0xec9803eb,
-	0x01ed9802,
-	0xf900ee98,
-	0xfef0fca5,
-	0x31f400f8,
-/* 0x030b: recv_done */
-	0xf8f0fc01,
-/* 0x030d: init */
-	0x0817f100,
-	0x0614b601,
-	0xe70011cf,
-	0xb6010911,
-	0x14fe0814,
-	0xe017f100,
-	0x0013f000,
-	0xb61c07f0,
 	0x01d00604,
 	0xf004bd00,
-	0x07f0ff17,
-	0x0604b614,
-	0xbd0001d0,
-	0x0217f004,
-	0x080015f1,
-	0xb61007f0,
-	0x01d00604,
-	0xf104bd00,
-	0xf0010a17,
-	0x10fe0013,
-	0x1031f400,
-	0xf00117f0,
-	0x04b63807,
-	0x0001d006,
-	0xf7f004bd,
-/* 0x0371: init_proc */
-	0x01f19858,
-	0xf40016b0,
-	0x15f9fa0b,
-	0xf458f0b6,
-/* 0x0382: host_send */
-	0x17f1f20e,
-	0x14b604b0,
-	0x0011cf06,
-	0x04a027f1,
-	0xcf0624b6,
-	0x12b80022,
-	0x320bf406,
-	0x94071ec4,
-	0xe0b704ee,
-	0xeb980270,
-	0x02ec9803,
-	0x9801ed98,
-	0x21f500ee,
-	0x10b602b9,
-	0x0f1ec401,
-	0x04b007f1,
-	0xd00604b6,
-	0x04bd000e,
-/* 0x03cb: host_send_done */
-	0xf8ba0ef4,
-/* 0x03cd: host_recv */
-	0x4917f100,
-	0x5413f14e,
-	0x06e1b852,
-/* 0x03db: host_recv_wait */
-	0xf1aa0bf4,
-	0xb604cc17,
-	0x11cf0614,
-	0xc827f100,
-	0x0624b604,
-	0xf00022cf,
-	0x12b80816,
-	0xe60bf406,
-	0xb60723c4,
-	0x30b70434,
-	0x3b8002f0,
-	0x023c8003,
-	0x80013d80,
-	0x20b6003e,
-	0x0f24f001,
-	0x04c807f1,
+/* 0x0402: init_proc */
+	0xf19858f7,
+	0x0016b001,
+	0xf9fa0bf4,
+	0x58f0b615,
+/* 0x0413: mulu32_32_64 */
+	0xf9f20ef4,
+	0xf920f910,
+	0x9540f930,
+	0xd29510e1,
+	0xbdc4bd10,
+	0xc0edffb4,
+	0xb9301dff,
+	0x34f10234,
+	0x34b6ffff,
+	0x1045b610,
+	0xbb00c3bb,
+	0xe2ff01b4,
+	0x0234b930,
+	0xffff34f1,
+	0xb61034b6,
+	0xc3bb1045,
+	0x01b4bb00,
+	0xbb3012ff,
+	0x40fc00b3,
+	0x20fc30fc,
+	0x00f810fc,
+/* 0x0464: host_send */
+	0x04b017f1,
+	0xcf0614b6,
+	0x27f10011,
+	0x24b604a0,
+	0x0022cf06,
+	0xf40612b8,
+	0x1ec4320b,
+	0x04ee9407,
+	0x0270e0b7,
+	0x9803eb98,
+	0xed9802ec,
+	0x00ee9801,
+	0x034221f5,
+	0xc40110b6,
+	0x07f10f1e,
+	0x04b604b0,
+	0x000ed006,
+	0x0ef404bd,
+/* 0x04ad: host_send_done */
+/* 0x04af: host_recv */
+	0xf100f8ba,
+	0xf14e4917,
+	0xb8525413,
+	0x0bf406e1,
+/* 0x04bd: host_recv_wait */
+	0xcc17f1aa,
+	0x0614b604,
+	0xf10011cf,
+	0xb604c827,
+	0x22cf0624,
+	0x0816f000,
+	0xf40612b8,
+	0x23c4e60b,
+	0x0434b607,
+	0x02f030b7,
+	0x80033b80,
+	0x3d80023c,
+	0x003e8001,
+	0xf00120b6,
+	0x07f10f24,
+	0x04b604c8,
+	0x0002d006,
+	0x27f004bd,
+	0x0007f040,
 	0xd00604b6,
 	0x04bd0002,
-	0xf04027f0,
-	0x04b60007,
-	0x0002d006,
-	0x00f804bd,
-/* 0x0430: host_init */
-	0x008017f1,
-	0xf11014b6,
-	0xf1027015,
-	0xb604d007,
-	0x01d00604,
-	0xf104bd00,
-	0xb6008017,
-	0x15f11014,
-	0x07f102f0,
-	0x04b604dc,
-	0x0001d006,
-	0x17f004bd,
-	0xc407f101,
+/* 0x0512: host_init */
+	0x17f100f8,
+	0x14b60080,
+	0x7015f110,
+	0xd007f102,
 	0x0604b604,
 	0xbd0001d0,
-/* 0x046f: memx_func_enter */
-	0xf000f804,
+	0x8017f104,
+	0x1014b600,
+	0x02f015f1,
+	0x04dc07f1,
+	0xd00604b6,
+	0x04bd0001,
+	0xf10117f0,
+	0xb604c407,
+	0x01d00604,
+	0xf804bd00,
+/* 0x0551: memx_func_enter */
+	0x2067f100,
+	0x5d77f116,
+	0xff73f1f5,
+	0x026eb9ff,
+	0xb90421f4,
+	0x87fd02d8,
+	0xf960f904,
+	0xfcd0fc80,
+	0x3f21f4e0,
+	0xfffe77f1,
+	0xffff73f1,
+	0xf4026eb9,
+	0xd8b90421,
+	0x0487fd02,
+	0x80f960f9,
+	0xe0fcd0fc,
+	0xf13f21f4,
+	0xb926f067,
+	0x21f4026e,
+	0x02d8b904,
+	0xf90487fd,
+	0xfc80f960,
+	0xf4e0fcd0,
+	0x67f03f21,
+	0xe007f104,
+	0x0604b607,
+	0xbd0006d0,
+/* 0x05bd: memx_func_enter_wait */
+	0xc067f104,
+	0x0664b607,
+	0xf00066cf,
+	0x0bf40464,
+	0x2c67f0f3,
+	0xcf0664b6,
+	0x06800066,
+/* 0x05db: memx_func_leave */
+	0xf000f8ee,
+	0x64b62c67,
+	0x0066cf06,
+	0xf0ef0680,
 	0x07f10467,
-	0x04b607e0,
+	0x04b607e4,
 	0x0006d006,
-/* 0x047e: memx_func_enter_wait */
+/* 0x05f6: memx_func_leave_wait */
 	0x67f104bd,
 	0x64b607c0,
 	0x0066cf06,
 	0xf40464f0,
-	0x1698f30b,
+	0x67f1f31b,
+	0x77f126f0,
+	0x73f00001,
+	0x026eb900,
+	0xb90421f4,
+	0x87fd02d8,
+	0xf960f905,
+	0xfcd0fc80,
+	0x3f21f4e0,
+	0x162067f1,
+	0xf4026eb9,
+	0xd8b90421,
+	0x0587fd02,
+	0x80f960f9,
+	0xe0fcd0fc,
+	0xf13f21f4,
+	0xf00aa277,
+	0x6eb90073,
+	0x0421f402,
+	0xfd02d8b9,
+	0x60f90587,
+	0xd0fc80f9,
+	0x21f4e0fc,
+/* 0x0663: memx_func_wait_vblank */
+	0x9800f83f,
+	0x66b00016,
+	0x130bf400,
+	0xf40166b0,
+	0x0ef4060b,
+/* 0x0675: memx_func_wait_vblank_head1 */
+	0x2077f12e,
+	0x070ef400,
+/* 0x067c: memx_func_wait_vblank_head0 */
+	0x000877f1,
+/* 0x0680: memx_func_wait_vblank_0 */
+	0x07c467f1,
+	0xcf0664b6,
+	0x67fd0066,
+	0xf31bf404,
+/* 0x0690: memx_func_wait_vblank_1 */
+	0x07c467f1,
+	0xcf0664b6,
+	0x67fd0066,
+	0xf30bf404,
+/* 0x06a0: memx_func_wait_vblank_fini */
+	0xf80410b6,
+/* 0x06a5: memx_func_wr32 */
+	0x00169800,
+	0xb6011598,
+	0x60f90810,
+	0xd0fc50f9,
+	0x21f4e0fc,
+	0x0242b63f,
+	0xf8e91bf4,
+/* 0x06c1: memx_func_wait */
+	0x2c87f000,
+	0xcf0684b6,
+	0x1e980088,
+	0x011d9800,
+	0x98021c98,
+	0x10b6031b,
+	0xa421f410,
+/* 0x06de: memx_func_delay */
+	0x1e9800f8,
 	0x0410b600,
-/* 0x0496: memx_func_leave */
-	0x67f000f8,
-	0xe407f104,
+	0xf87f21f4,
+/* 0x06e9: memx_exec */
+	0xf9e0f900,
+	0x02c1b9d0,
+/* 0x06f3: memx_exec_next */
+	0x9802b2b9,
+	0x10b60013,
+	0xf034e704,
+	0xe033e701,
+	0x0132b601,
+	0x980c30f0,
+	0x55f9de35,
+	0xf40612b8,
+	0x0b98e41e,
+	0xef0c98ee,
+	0xf102cbbb,
+	0xb607c4b7,
+	0xbbcf06b4,
+	0xfcd0fc00,
+	0x4221f5e0,
+/* 0x072f: memx_info */
+	0xf100f803,
+	0xf103c0c7,
+	0xf50800b7,
+	0xf8034221,
+/* 0x073d: memx_recv */
+	0x01d6b000,
+	0xb0a90bf4,
+	0x0bf400d6,
+/* 0x074b: memx_init */
+	0xf800f8e9,
+/* 0x074d: perf_recv */
+/* 0x074f: perf_init */
+	0xf800f800,
+/* 0x0751: i2c_drive_scl */
+	0x0036b000,
+	0xf1110bf4,
+	0xb607e007,
+	0x01d00604,
+	0xf804bd00,
+/* 0x0765: i2c_drive_scl_lo */
+	0xe407f100,
 	0x0604b607,
-	0xbd0006d0,
-/* 0x04a5: memx_func_leave_wait */
-	0xc067f104,
-	0x0664b607,
-	0xf00066cf,
-	0x1bf40464,
-/* 0x04b7: memx_func_wr32 */
-	0x9800f8f3,
-	0x15980016,
-	0x0810b601,
-	0x50f960f9,
-	0xe0fcd0fc,
-	0xb63f21f4,
-	0x1bf40242,
-/* 0x04d3: memx_func_wait */
-	0xf000f8e9,
-	0x84b62c87,
-	0x0088cf06,
-	0x98001e98,
-	0x1c98011d,
-	0x031b9802,
-	0xf41010b6,
-	0x00f89c21,
-/* 0x04f0: memx_func_delay */
-	0xb6001e98,
-	0x21f40410,
-/* 0x04fb: memx_exec */
-	0xf900f87f,
-	0xb9d0f9e0,
-	0xb2b902c1,
-/* 0x0505: memx_exec_next */
-	0x00139802,
-	0x950410b6,
-	0x30f01034,
-	0xde35980c,
-	0x12b855f9,
-	0xec1ef406,
-	0xe0fcd0fc,
-	0x02b921f5,
-/* 0x0526: memx_info */
-	0xc7f100f8,
-	0xb7f103ac,
-	0x21f50800,
-	0x00f802b9,
-/* 0x0534: memx_recv */
-	0xf401d6b0,
-	0xd6b0c40b,
-	0xe90bf400,
-/* 0x0542: memx_init */
-	0x00f800f8,
-/* 0x0544: perf_recv */
-/* 0x0546: perf_init */
-	0x00f800f8,
-/* 0x0548: i2c_drive_scl */
-	0xf40036b0,
-	0x07f1110b,
-	0x04b607e0,
-	0x0001d006,
-	0x00f804bd,
-/* 0x055c: i2c_drive_scl_lo */
-	0x07e407f1,
-	0xd00604b6,
-	0x04bd0001,
-/* 0x056a: i2c_drive_sda */
-	0x36b000f8,
-	0x110bf400,
-	0x07e007f1,
-	0xd00604b6,
-	0x04bd0002,
-/* 0x057e: i2c_drive_sda_lo */
-	0x07f100f8,
-	0x04b607e4,
-	0x0002d006,
-	0x00f804bd,
-/* 0x058c: i2c_sense_scl */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0431fd00,
-	0xf4060bf4,
-/* 0x05a2: i2c_sense_scl_done */
-	0x00f80131,
-/* 0x05a4: i2c_sense_sda */
-	0xf10132f4,
-	0xb607c437,
-	0x33cf0634,
-	0x0432fd00,
-	0xf4060bf4,
-/* 0x05ba: i2c_sense_sda_done */
-	0x00f80131,
-/* 0x05bc: i2c_raise_scl */
-	0x47f140f9,
-	0x37f00898,
-	0x4821f501,
-/* 0x05c9: i2c_raise_scl_wait */
-	0xe8e7f105,
-	0x7f21f403,
-	0x058c21f5,
-	0xb60901f4,
-	0x1bf40142,
-/* 0x05dd: i2c_raise_scl_done */
-	0xf840fcef,
-/* 0x05e1: i2c_start */
-	0x8c21f500,
-	0x0d11f405,
-	0x05a421f5,
-	0xf40611f4,
-/* 0x05f2: i2c_start_rep */
-	0x37f0300e,
-	0x4821f500,
-	0x0137f005,
-	0x056a21f5,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0xbc21f550,
-	0x0464b605,
-/* 0x061f: i2c_start_send */
-	0xf01f11f4,
+	0xbd0001d0,
+/* 0x0773: i2c_drive_sda */
+	0xb000f804,
+	0x0bf40036,
+	0xe007f111,
+	0x0604b607,
+	0xbd0002d0,
+/* 0x0787: i2c_drive_sda_lo */
+	0xf100f804,
+	0xb607e407,
+	0x02d00604,
+	0xf804bd00,
+/* 0x0795: i2c_sense_scl */
+	0x0132f400,
+	0x07c437f1,
+	0xcf0634b6,
+	0x31fd0033,
+	0x060bf404,
+/* 0x07ab: i2c_sense_scl_done */
+	0xf80131f4,
+/* 0x07ad: i2c_sense_sda */
+	0x0132f400,
+	0x07c437f1,
+	0xcf0634b6,
+	0x32fd0033,
+	0x060bf404,
+/* 0x07c3: i2c_sense_sda_done */
+	0xf80131f4,
+/* 0x07c5: i2c_raise_scl */
+	0xf140f900,
+	0xf0089847,
+	0x21f50137,
+/* 0x07d2: i2c_raise_scl_wait */
+	0xe7f10751,
+	0x21f403e8,
+	0x9521f57f,
+	0x0901f407,
+	0xf40142b6,
+/* 0x07e6: i2c_raise_scl_done */
+	0x40fcef1b,
+/* 0x07ea: i2c_start */
+	0x21f500f8,
+	0x11f40795,
+	0xad21f50d,
+	0x0611f407,
+/* 0x07fb: i2c_start_rep */
+	0xf0300ef4,
 	0x21f50037,
-	0xe7f1056a,
-	0x21f41388,
-	0x0037f07f,
-	0x054821f5,
-	0x1388e7f1,
-/* 0x063b: i2c_start_out */
-	0xf87f21f4,
-/* 0x063d: i2c_stop */
-	0x0037f000,
-	0x054821f5,
+	0x37f00751,
+	0x7321f501,
+	0x0076bb07,
+	0xf90465b6,
+	0x04659450,
+	0xbd0256bb,
+	0x0475fd50,
+	0x21f550fc,
+	0x64b607c5,
+	0x1f11f404,
+/* 0x0828: i2c_start_send */
 	0xf50037f0,
-	0xf1056a21,
-	0xf403e8e7,
+	0xf1077321,
+	0xf41388e7,
 	0x37f07f21,
-	0x4821f501,
-	0x88e7f105,
+	0x5121f500,
+	0x88e7f107,
 	0x7f21f413,
-	0xf50137f0,
-	0xf1056a21,
-	0xf41388e7,
-	0x00f87f21,
-/* 0x0670: i2c_bitw */
-	0x056a21f5,
+/* 0x0844: i2c_start_out */
+/* 0x0846: i2c_stop */
+	0x37f000f8,
+	0x5121f500,
+	0x0037f007,
+	0x077321f5,
 	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x05bc21f5,
-	0xf40464b6,
-	0xe7f11811,
+	0xf07f21f4,
+	0x21f50137,
+	0xe7f10751,
 	0x21f41388,
-	0x0037f07f,
-	0x054821f5,
+	0x0137f07f,
+	0x077321f5,
 	0x1388e7f1,
-/* 0x06af: i2c_bitw_out */
 	0xf87f21f4,
-/* 0x06b1: i2c_bitr */
-	0x0137f000,
-	0x056a21f5,
-	0x03e8e7f1,
-	0xbb7f21f4,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x05bc21f5,
-	0xf40464b6,
-	0x21f51b11,
-	0x37f005a4,
-	0x4821f500,
-	0x88e7f105,
+/* 0x0879: i2c_bitw */
+	0x7321f500,
+	0xe8e7f107,
+	0x7f21f403,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xc521f550,
+	0x0464b607,
+	0xf11811f4,
+	0xf41388e7,
+	0x37f07f21,
+	0x5121f500,
+	0x88e7f107,
 	0x7f21f413,
-	0xf4013cf0,
-/* 0x06f6: i2c_bitr_done */
-	0x00f80131,
-/* 0x06f8: i2c_get_byte */
-	0xf00057f0,
-/* 0x06fe: i2c_get_byte_next */
-	0x54b60847,
+/* 0x08b8: i2c_bitw_out */
+/* 0x08ba: i2c_bitr */
+	0x37f000f8,
+	0x7321f501,
+	0xe8e7f107,
+	0x7f21f403,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xc521f550,
+	0x0464b607,
+	0xf51b11f4,
+	0xf007ad21,
+	0x21f50037,
+	0xe7f10751,
+	0x21f41388,
+	0x013cf07f,
+/* 0x08ff: i2c_bitr_done */
+	0xf80131f4,
+/* 0x0901: i2c_get_byte */
+	0x0057f000,
+/* 0x0907: i2c_get_byte_next */
+	0xb60847f0,
+	0x76bb0154,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb608ba21,
+	0x11f40464,
+	0x0553fd2b,
+	0xf40142b6,
+	0x37f0d81b,
 	0x0076bb01,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b606b1,
-	0x2b11f404,
-	0xb60553fd,
-	0x1bf40142,
-	0x0137f0d8,
-	0xb60076bb,
-	0x50f90465,
-	0xbb046594,
-	0x50bd0256,
-	0xfc0475fd,
-	0x7021f550,
-	0x0464b606,
-/* 0x0748: i2c_get_byte_done */
-/* 0x074a: i2c_put_byte */
-	0x47f000f8,
-/* 0x074d: i2c_put_byte_next */
-	0x0142b608,
-	0xbb3854ff,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x067021f5,
-	0xf40464b6,
-	0x46b03411,
-	0xd81bf400,
+	0x64b60879,
+/* 0x0951: i2c_get_byte_done */
+/* 0x0953: i2c_put_byte */
+	0xf000f804,
+/* 0x0956: i2c_put_byte_next */
+	0x42b60847,
+	0x3854ff01,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xb121f550,
-	0x0464b606,
-	0xbb0f11f4,
-	0x36b00076,
-	0x061bf401,
-/* 0x07a3: i2c_put_byte_done */
-	0xf80132f4,
-/* 0x07a5: i2c_addr */
-	0x0076bb00,
+	0x7921f550,
+	0x0464b608,
+	0xb03411f4,
+	0x1bf40046,
+	0x0076bbd8,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b605e1,
-	0x2911f404,
-	0x012ec3e7,
-	0xfd0134b6,
-	0x76bb0553,
+	0x64b608ba,
+	0x0f11f404,
+	0xb00076bb,
+	0x1bf40136,
+	0x0132f406,
+/* 0x09ac: i2c_put_byte_done */
+/* 0x09ae: i2c_addr */
+	0x76bb00f8,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6074a21,
-/* 0x07ea: i2c_addr_done */
-	0x00f80464,
-/* 0x07ec: i2c_acquire_addr */
-	0xb6f8cec7,
-	0xe0b702e4,
-	0xee980bfc,
-/* 0x07fb: i2c_acquire */
-	0xf500f800,
-	0xf407ec21,
-	0xd9f00421,
-	0x3f21f403,
-/* 0x080a: i2c_release */
-	0x21f500f8,
-	0x21f407ec,
-	0x03daf004,
-	0xf83f21f4,
-/* 0x0819: i2c_recv */
-	0x0132f400,
-	0xb6f8c1c7,
-	0x16b00214,
-	0x3a1ff528,
-	0xd413a001,
-	0x0032980b,
-	0x0bac13a0,
-	0xf4003198,
-	0xd0f90231,
-	0xd0f9e0f9,
-	0x000067f1,
-	0x100063f1,
-	0xbb016792,
+	0xb607ea21,
+	0x11f40464,
+	0x2ec3e729,
+	0x0134b601,
+	0xbb0553fd,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x07fb21f5,
-	0xfc0464b6,
-	0x00d6b0d0,
-	0x00b31bf5,
-	0xbb0057f0,
+	0x095321f5,
+/* 0x09f3: i2c_addr_done */
+	0xf80464b6,
+/* 0x09f5: i2c_acquire_addr */
+	0xf8cec700,
+	0xb702e4b6,
+	0x980c10e0,
+	0x00f800ee,
+/* 0x0a04: i2c_acquire */
+	0x09f521f5,
+	0xf00421f4,
+	0x21f403d9,
+/* 0x0a13: i2c_release */
+	0xf500f83f,
+	0xf409f521,
+	0xdaf00421,
+	0x3f21f403,
+/* 0x0a22: i2c_recv */
+	0x32f400f8,
+	0xf8c1c701,
+	0xb00214b6,
+	0x1ff52816,
+	0x13a0013a,
+	0x32980be8,
+	0xc013a000,
+	0x0031980b,
+	0xf90231f4,
+	0xf9e0f9d0,
+	0x0067f1d0,
+	0x0063f100,
+	0x01679210,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x0421f550,
+	0x0464b60a,
+	0xd6b0d0fc,
+	0xb31bf500,
+	0x0057f000,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0xae21f550,
+	0x0464b609,
+	0x00d011f5,
+	0xbbe0c5c7,
 	0x65b60076,
 	0x9450f904,
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x07a521f5,
+	0x095321f5,
 	0xf50464b6,
-	0xc700d011,
-	0x76bbe0c5,
+	0xf000ad11,
+	0x76bb0157,
 	0x0465b600,
 	0x659450f9,
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6074a21,
+	0xb609ae21,
 	0x11f50464,
-	0x57f000ad,
-	0x0076bb01,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b607a5,
-	0x8a11f504,
-	0x0076bb00,
-	0xf90465b6,
-	0x04659450,
-	0xbd0256bb,
-	0x0475fd50,
-	0x21f550fc,
-	0x64b606f8,
-	0x6a11f404,
-	0xbbe05bcb,
-	0x65b60076,
-	0x9450f904,
-	0x56bb0465,
-	0xfd50bd02,
-	0x50fc0475,
-	0x063d21f5,
-	0xb90464b6,
-	0x74bd025b,
-/* 0x091f: i2c_recv_not_rd08 */
-	0xb0430ef4,
-	0x1bf401d6,
-	0x0057f03d,
-	0x07a521f5,
-	0xc73311f4,
-	0x21f5e0c5,
-	0x11f4074a,
-	0x0057f029,
-	0x07a521f5,
-	0xc71f11f4,
-	0x21f5e0b5,
-	0x11f4074a,
-	0x3d21f515,
-	0xc774bd06,
-	0x1bf408c5,
-	0x0232f409,
-/* 0x095f: i2c_recv_not_wr08 */
-/* 0x095f: i2c_recv_done */
-	0xc7030ef4,
-	0x21f5f8ce,
-	0xe0fc080a,
-	0x12f4d0fc,
-	0x027cb90a,
-	0x02b921f5,
-/* 0x0974: i2c_recv_exit */
-/* 0x0976: i2c_init */
+	0x76bb008a,
+	0x0465b600,
+	0x659450f9,
+	0x0256bb04,
+	0x75fd50bd,
+	0xf550fc04,
+	0xb6090121,
+	0x11f40464,
+	0xe05bcb6a,
+	0xb60076bb,
+	0x50f90465,
+	0xbb046594,
+	0x50bd0256,
+	0xfc0475fd,
+	0x4621f550,
+	0x0464b608,
+	0xbd025bb9,
+	0x430ef474,
+/* 0x0b28: i2c_recv_not_rd08 */
+	0xf401d6b0,
+	0x57f03d1b,
+	0xae21f500,
+	0x3311f409,
+	0xf5e0c5c7,
+	0xf4095321,
+	0x57f02911,
+	0xae21f500,
+	0x1f11f409,
+	0xf5e0b5c7,
+	0xf4095321,
+	0x21f51511,
+	0x74bd0846,
+	0xf408c5c7,
+	0x32f4091b,
+	0x030ef402,
+/* 0x0b68: i2c_recv_not_wr08 */
+/* 0x0b68: i2c_recv_done */
+	0xf5f8cec7,
+	0xfc0a1321,
+	0xf4d0fce0,
+	0x7cb90a12,
+	0x4221f502,
+/* 0x0b7d: i2c_recv_exit */
+/* 0x0b7f: i2c_init */
+	0xf800f803,
+/* 0x0b81: test_recv */
+	0xd817f100,
+	0x0614b605,
+	0xb60011cf,
+	0x07f10110,
+	0x04b605d8,
+	0x0001d006,
+	0xe7f104bd,
+	0xe3f1d900,
+	0x21f5134f,
+	0x00f80262,
+/* 0x0ba8: test_init */
+	0x0800e7f1,
+	0x026221f5,
+/* 0x0bb2: idle_recv */
 	0x00f800f8,
-/* 0x0978: test_recv */
-	0x05d817f1,
-	0xcf0614b6,
-	0x10b60011,
-	0xd807f101,
-	0x0604b605,
-	0xbd0001d0,
-	0x00e7f104,
-	0x4fe3f1d9,
-	0xf521f513,
-/* 0x099f: test_init */
-	0xf100f801,
-	0xf50800e7,
-	0xf801f521,
-/* 0x09a9: idle_recv */
-/* 0x09ab: idle */
-	0xf400f800,
-	0x17f10031,
-	0x14b605d4,
-	0x0011cf06,
-	0xf10110b6,
-	0xb605d407,
-	0x01d00604,
-/* 0x09c7: idle_loop */
-	0xf004bd00,
-	0x32f45817,
-/* 0x09cd: idle_proc */
-/* 0x09cd: idle_proc_exec */
-	0xb910f902,
-	0x21f5021e,
-	0x10fc02c2,
-	0xf40911f4,
-	0x0ef40231,
-/* 0x09e1: idle_proc_next */
-	0x5810b6ef,
-	0xf4061fb8,
-	0x02f4e61b,
-	0x0028f4dd,
-	0x00bb0ef4,
-	0x00000000,
-	0x00000000,
+/* 0x0bb4: idle */
+	0xf10031f4,
+	0xb605d417,
+	0x11cf0614,
+	0x0110b600,
+	0x05d407f1,
+	0xd00604b6,
+	0x04bd0001,
+/* 0x0bd0: idle_loop */
+	0xf45817f0,
+/* 0x0bd6: idle_proc */
+/* 0x0bd6: idle_proc_exec */
+	0x10f90232,
+	0xf5021eb9,
+	0xfc034b21,
+	0x0911f410,
+	0xf40231f4,
+/* 0x0bea: idle_proc_next */
+	0x10b6ef0e,
+	0x061fb858,
+	0xf4e61bf4,
+	0x28f4dd02,
+	0xbb0ef400,
 	0x00000000,
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc
index 8a89dfe41ce1..b85443261569 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc
@@ -23,6 +23,7 @@
  */
 
 #define NVKM_PPWR_CHIPSET GF119
+#define HW_TICKS_PER_US 324
 
 //#define NVKM_FALCON_PC24
 #define NVKM_FALCON_UNSHIFTED_IO
@@ -34,6 +35,7 @@
 .section #nvd0_pwr_data
 #define INCLUDE_PROC
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -44,6 +46,7 @@
 
 #define INCLUDE_DATA
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
@@ -56,6 +59,7 @@
 .section #nvd0_pwr_code
 #define INCLUDE_CODE
 #include "kernel.fuc"
+#include "arith.fuc"
 #include "host.fuc"
 #include "memx.fuc"
 #include "perf.fuc"
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h
index 8d369b3faaba..12d86f72ad10 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/nvd0.fuc.h
@@ -24,8 +24,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 /* 0x0058: proc_list_head */
 	0x54534f48,
-	0x000003be,
-	0x00000367,
+	0x0000049d,
+	0x00000446,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -46,8 +46,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x584d454d,
-	0x000004b8,
-	0x000004aa,
+	0x00000678,
+	0x0000066a,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -68,8 +68,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x46524550,
-	0x000004bc,
-	0x000004ba,
+	0x0000067c,
+	0x0000067a,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -90,8 +90,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x5f433249,
-	0x000008d7,
-	0x0000077a,
+	0x00000a97,
+	0x0000093a,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -112,8 +112,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x54534554,
-	0x000008fa,
-	0x000008d9,
+	0x00000aba,
+	0x00000a99,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -134,8 +134,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x454c4449,
-	0x00000906,
-	0x00000904,
+	0x00000ac6,
+	0x00000ac4,
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -227,24 +227,31 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 /* 0x0370: memx_func_head */
-	0x00010000,
-	0x00000000,
-	0x000003f4,
-/* 0x037c: memx_func_next */
 	0x00000001,
 	0x00000000,
-	0x00000415,
+	0x000004d3,
+/* 0x037c: memx_func_next */
 	0x00000002,
+	0x00000000,
+	0x00000554,
+	0x00000003,
 	0x00000002,
-	0x00000430,
-	0x00040003,
+	0x000005d8,
+	0x00040004,
+	0x00000000,
+	0x000005f4,
+	0x00010005,
+	0x00000000,
+	0x0000060e,
+	0x00010006,
+	0x00000000,
+	0x000005d3,
+/* 0x03b8: memx_func_tail */
+/* 0x03b8: memx_ts_start */
 	0x00000000,
-	0x0000044c,
-	0x00010004,
+/* 0x03bc: memx_ts_end */
 	0x00000000,
-	0x00000466,
-/* 0x03ac: memx_func_tail */
-/* 0x03ac: memx_data_head */
+/* 0x03c0: memx_data_head */
 	0x00000000,
 	0x00000000,
 	0x00000000,
@@ -757,8 +764,8 @@ uint32_t nvd0_pwr_data[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
-/* 0x0bac: memx_data_tail */
-/* 0x0bac: i2c_scl_map */
+/* 0x0bc0: memx_data_tail */
+/* 0x0bc0: i2c_scl_map */
 	0x00000400,
 	0x00000800,
 	0x00001000,
@@ -769,7 +776,7 @@ uint32_t nvd0_pwr_data[] = {
 	0x00020000,
 	0x00040000,
 	0x00080000,
-/* 0x0bd4: i2c_sda_map */
+/* 0x0be8: i2c_sda_map */
 	0x00100000,
 	0x00200000,
 	0x00400000,
@@ -781,10 +788,69 @@ uint32_t nvd0_pwr_data[] = {
 	0x10000000,
 	0x20000000,
 	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 };
 
 uint32_t nvd0_pwr_code[] = {
-	0x02bf0ef5,
+	0x034d0ef5,
 /* 0x0004: rd32 */
 	0x07a007f1,
 	0xbd000ed0,
@@ -814,17 +880,20 @@ uint32_t nvd0_pwr_code[] = {
 	0xd4f100dd,
 	0x1bf47000,
 /* 0x0067: nsec */
-	0xf000f8f5,
+	0xf900f8f5,
+	0xf080f990,
 	0x88cf2c87,
-/* 0x006d: nsec_loop */
+/* 0x0071: nsec_loop */
 	0x2c97f000,
 	0xbb0099cf,
 	0x9eb80298,
 	0xf41ef406,
-/* 0x007e: wait */
-	0x87f000f8,
+	0x90fc80fc,
+/* 0x0086: wait */
+	0x90f900f8,
+	0x87f080f9,
 	0x0088cf2c,
-/* 0x0084: wait_loop */
+/* 0x0090: wait_loop */
 	0xf402eeb9,
 	0xdab90421,
 	0x04adfd02,
@@ -833,28 +902,29 @@ uint32_t nvd0_pwr_code[] = {
 	0x0099cf2c,
 	0xb80298bb,
 	0x1ef4069b,
-/* 0x00a5: wait_done */
-/* 0x00a7: intr_watchdog */
-	0x9800f8e2,
+/* 0x00b1: wait_done */
+	0xfc80fce2,
+/* 0x00b7: intr_watchdog */
+	0x9800f890,
 	0x96b003e9,
 	0x2a0bf400,
 	0xbb9a0a98,
 	0x1cf4029a,
 	0x01d7f00f,
-	0x020621f5,
+	0x028c21f5,
 	0x0ef494bd,
-/* 0x00c5: intr_watchdog_next_time */
+/* 0x00d5: intr_watchdog_next_time */
 	0x9b0a9815,
 	0xf400a6b0,
 	0x9ab8090b,
 	0x061cf406,
-/* 0x00d4: intr_watchdog_next_time_set */
-/* 0x00d7: intr_watchdog_next_proc */
+/* 0x00e4: intr_watchdog_next_time_set */
+/* 0x00e7: intr_watchdog_next_proc */
 	0x809b0980,
 	0xe0b603e9,
 	0x68e6b158,
 	0xc61bf402,
-/* 0x00e6: intr */
+/* 0x00f6: intr */
 	0x00f900f8,
 	0x80f904bd,
 	0xa0f990f9,
@@ -872,12 +942,12 @@ uint32_t nvd0_pwr_code[] = {
 	0x0bf40289,
 	0x9b008020,
 	0xf458e7f0,
-	0x0998a721,
+	0x0998b721,
 	0x0096b09b,
 	0xf00e0bf4,
 	0x09d03407,
 	0x8004bd00,
-/* 0x013e: intr_skip_watchdog */
+/* 0x014e: intr_skip_watchdog */
 	0x89e49a09,
 	0x0bf40800,
 	0x8897f13c,
@@ -889,20 +959,20 @@ uint32_t nvd0_pwr_code[] = {
 	0xf14f48e7,
 	0xf05453e3,
 	0x21f500d7,
-	0xc0fc026b,
+	0xc0fc02f1,
 	0x04c007f1,
 	0xbd000cd0,
-/* 0x0175: intr_subintr_skip_fifo */
+/* 0x0185: intr_subintr_skip_fifo */
 	0x8807f104,
 	0x0009d006,
-/* 0x017e: intr_skip_subintr */
+/* 0x018e: intr_skip_subintr */
 	0x89c404bd,
 	0x070bf420,
 	0xffbfa4f1,
-/* 0x0188: intr_skip_pause */
+/* 0x0198: intr_skip_pause */
 	0xf44089c4,
 	0xa4f1070b,
-/* 0x0192: intr_skip_user0 */
+/* 0x01a2: intr_skip_user0 */
 	0x07f0ffbf,
 	0x0008d004,
 	0x80fc04bd,
@@ -912,189 +982,298 @@ uint32_t nvd0_pwr_code[] = {
 	0xfca0fcb0,
 	0xfc80fc90,
 	0x0032f400,
-/* 0x01b6: timer */
-	0x32f401f8,
-	0x03f89810,
-	0xf40086b0,
-	0xfe80421c,
-	0x3807f003,
+/* 0x01c6: ticks_from_ns */
+	0xc0f901f8,
+	0xd7f1b0f9,
+	0xd3f00144,
+	0xb321f500,
+	0xe8ccec03,
+	0x00b4b003,
+	0xec120bf4,
+	0xf103e8ee,
+	0xf00144d7,
+	0x21f500d3,
+/* 0x01ee: ticks_from_ns_quit */
+	0xceb903b3,
+	0xfcb0fc02,
+/* 0x01f7: ticks_from_us */
+	0xf900f8c0,
+	0xf1b0f9c0,
+	0xf00144d7,
+	0x21f500d3,
+	0xceb903b3,
+	0x00b4b002,
+	0xbd050bf4,
+/* 0x0211: ticks_from_us_quit */
+	0xfcb0fce4,
+/* 0x0217: ticks_to_us */
+	0xf100f8c0,
+	0xf00144d7,
+	0xedff00d3,
+/* 0x0223: timer */
+	0xf900f8ec,
+	0xf480f990,
+	0xf8981032,
+	0x0086b003,
+	0xbd531cf4,
+	0x3807f084,
 	0xbd0008d0,
-	0x0887f004,
-	0xf00088cf,
-	0x1bf40284,
-	0x3487f020,
-	0xb80088cf,
-	0x0bf406e0,
-	0x06e8b809,
-/* 0x01eb: timer_reset */
-	0xf0191ef4,
-	0x0ed03407,
-	0x8004bd00,
-/* 0x01f6: timer_enable */
-	0x87f09a0e,
-	0x3807f001,
-	0xbd0008d0,
-/* 0x0201: timer_done */
-	0x1031f404,
-/* 0x0206: send_proc */
-	0x80f900f8,
-	0xe89890f9,
+	0x3487f004,
+	0x980088cf,
+	0x98bb9a09,
+	0x00e9bb02,
+	0xf003fe80,
+	0x88cf0887,
+	0x0284f000,
+	0xf0201bf4,
+	0x88cf3487,
+	0x06e0b800,
+	0xb8090bf4,
+	0x1cf406e8,
+/* 0x026d: timer_reset */
+	0x3407f00e,
+	0xbd000ed0,
+	0x9a0e8004,
+/* 0x0278: timer_enable */
+	0xf00187f0,
+	0x08d03807,
+/* 0x0283: timer_done */
+	0xf404bd00,
+	0x80fc1031,
+	0x00f890fc,
+/* 0x028c: send_proc */
+	0x90f980f9,
+	0x9805e898,
+	0x86f004e9,
+	0x0689b804,
+	0xc42a0bf4,
+	0x88940398,
+	0x1880b604,
+	0x98008ebb,
+	0x8a8000fa,
+	0x018d8000,
+	0x80028c80,
+	0x90b6038b,
+	0x0794f001,
+	0xf404e980,
+/* 0x02c6: send_done */
+	0x90fc0231,
+	0x00f880fc,
+/* 0x02cc: find */
+	0x87f080f9,
+	0x0131f458,
+/* 0x02d4: find_loop */
+	0xb8008a98,
+	0x0bf406ae,
+	0x5880b610,
+	0x026886b1,
+	0xf4f01bf4,
+/* 0x02ea: find_done */
+	0x8eb90132,
+	0xf880fc02,
+/* 0x02f1: send */
+	0xcc21f500,
+	0x9701f402,
+/* 0x02fa: recv */
+	0x90f900f8,
+	0xe89880f9,
 	0x04e99805,
-	0xb80486f0,
+	0xb80132f4,
 	0x0bf40689,
-	0x0398c42a,
-	0xb6048894,
-	0x8ebb1880,
-	0x00fa9800,
-	0x80008a80,
-	0x8c80018d,
-	0x038b8002,
-	0xf00190b6,
-	0xe9800794,
-	0x0231f404,
-/* 0x0240: send_done */
-	0x80fc90fc,
-/* 0x0246: find */
-	0x80f900f8,
-	0xf45887f0,
-/* 0x024e: find_loop */
-	0x8a980131,
-	0x06aeb800,
-	0xb6100bf4,
-	0x86b15880,
-	0x1bf40268,
-	0x0132f4f0,
-/* 0x0264: find_done */
-	0xfc028eb9,
-/* 0x026b: send */
-	0xf500f880,
-	0xf4024621,
-	0x00f89701,
-/* 0x0274: recv */
-	0x9805e898,
-	0x32f404e9,
-	0x0689b801,
-	0xc43d0bf4,
-	0x80b60389,
-	0x0784f001,
-	0x9805e880,
-	0xf0f902ea,
-	0xf9018ffe,
-	0x02efb9f0,
-	0xbb049994,
-	0xe0b600e9,
-	0x03eb9818,
-	0x9802ec98,
-	0xee9801ed,
-	0xfca5f900,
-	0x00f8fef0,
-	0xfc0131f4,
-/* 0x02bd: recv_done */
-/* 0x02bf: init */
-	0xf100f8f0,
-	0xcf010817,
-	0x11e70011,
-	0x14b60109,
-	0x0014fe08,
-	0x00e017f1,
-	0xf00013f0,
-	0x01d01c07,
-	0xf004bd00,
-	0x07f0ff17,
-	0x0001d014,
-	0x17f004bd,
-	0x0015f102,
-	0x1007f008,
-	0xbd0001d0,
-	0xe617f104,
-	0x0013f000,
-	0xf40010fe,
-	0x17f01031,
-	0x3807f001,
-	0xbd0001d0,
-	0x58f7f004,
-/* 0x0314: init_proc */
-	0xb001f198,
-	0x0bf40016,
-	0xb615f9fa,
-	0x0ef458f0,
-/* 0x0325: host_send */
-	0xb017f1f2,
-	0x0011cf04,
-	0x04a027f1,
-	0xb80022cf,
-	0x0bf40612,
-	0x071ec42f,
-	0xb704ee94,
-	0x980270e0,
+	0x0389c43d,
+	0xf00180b6,
+	0xe8800784,
+	0x02ea9805,
+	0x8ffef0f9,
+	0xb9f0f901,
+	0x999402ef,
+	0x00e9bb04,
+	0x9818e0b6,
 	0xec9803eb,
 	0x01ed9802,
-	0xf500ee98,
-	0xb6026b21,
-	0x1ec40110,
-	0xb007f10f,
-	0x000ed004,
-	0x0ef404bd,
-/* 0x0365: host_send_done */
-/* 0x0367: host_recv */
-	0xf100f8c3,
-	0xf14e4917,
-	0xb8525413,
-	0x0bf406e1,
-/* 0x0375: host_recv_wait */
-	0xcc17f1b3,
-	0x0011cf04,
-	0x04c827f1,
-	0xf00022cf,
-	0x12b80816,
-	0xec0bf406,
-	0xb60723c4,
-	0x30b70434,
-	0x3b8002f0,
-	0x023c8003,
-	0x80013d80,
-	0x20b6003e,
-	0x0f24f001,
-	0x04c807f1,
-	0xbd0002d0,
-	0x4027f004,
-	0xd00007f0,
-	0x04bd0002,
-/* 0x03be: host_init */
+	0xf900ee98,
+	0xfef0fca5,
+	0x31f400f8,
+/* 0x0347: recv_done */
+	0xfcf0fc01,
+	0xf890fc80,
+/* 0x034d: init */
+	0x0817f100,
+	0x0011cf01,
+	0x010911e7,
+	0xfe0814b6,
+	0x17f10014,
+	0x13f000e0,
+	0x1c07f000,
+	0xbd0001d0,
+	0xff17f004,
+	0xd01407f0,
+	0x04bd0001,
+	0xf10217f0,
+	0xf0080015,
+	0x01d01007,
+	0xf104bd00,
+	0xf000f617,
+	0x10fe0013,
+	0x1031f400,
+	0xf00117f0,
+	0x01d03807,
+	0xf004bd00,
+/* 0x03a2: init_proc */
+	0xf19858f7,
+	0x0016b001,
+	0xf9fa0bf4,
+	0x58f0b615,
+/* 0x03b3: mulu32_32_64 */
+	0xf9f20ef4,
+	0xf920f910,
+	0x9540f930,
+	0xd29510e1,
+	0xbdc4bd10,
+	0xc0edffb4,
+	0xb9301dff,
+	0x34f10234,
+	0x34b6ffff,
+	0x1045b610,
+	0xbb00c3bb,
+	0xe2ff01b4,
+	0x0234b930,
+	0xffff34f1,
+	0xb61034b6,
+	0xc3bb1045,
+	0x01b4bb00,
+	0xbb3012ff,
+	0x40fc00b3,
+	0x20fc30fc,
+	0x00f810fc,
+/* 0x0404: host_send */
+	0x04b017f1,
+	0xf10011cf,
+	0xcf04a027,
+	0x12b80022,
+	0x2f0bf406,
+	0x94071ec4,
+	0xe0b704ee,
+	0xeb980270,
+	0x02ec9803,
+	0x9801ed98,
+	0x21f500ee,
+	0x10b602f1,
+	0x0f1ec401,
+	0x04b007f1,
+	0xbd000ed0,
+	0xc30ef404,
+/* 0x0444: host_send_done */
+/* 0x0446: host_recv */
 	0x17f100f8,
-	0x14b60080,
-	0x7015f110,
-	0xd007f102,
-	0x0001d004,
-	0x17f104bd,
-	0x14b60080,
-	0xf015f110,
-	0xdc07f102,
-	0x0001d004,
-	0x17f004bd,
-	0xc407f101,
-	0x0001d004,
-	0x00f804bd,
-/* 0x03f4: memx_func_enter */
+	0x13f14e49,
+	0xe1b85254,
+	0xb30bf406,
+/* 0x0454: host_recv_wait */
+	0x04cc17f1,
+	0xf10011cf,
+	0xcf04c827,
+	0x16f00022,
+	0x0612b808,
+	0xc4ec0bf4,
+	0x34b60723,
+	0xf030b704,
+	0x033b8002,
+	0x80023c80,
+	0x3e80013d,
+	0x0120b600,
+	0xf10f24f0,
+	0xd004c807,
+	0x04bd0002,
+	0xf04027f0,
+	0x02d00007,
+	0xf804bd00,
+/* 0x049d: host_init */
+	0x8017f100,
+	0x1014b600,
+	0x027015f1,
+	0x04d007f1,
+	0xbd0001d0,
+	0x8017f104,
+	0x1014b600,
+	0x02f015f1,
+	0x04dc07f1,
+	0xbd0001d0,
+	0x0117f004,
+	0x04c407f1,
+	0xbd0001d0,
+/* 0x04d3: memx_func_enter */
+	0xf100f804,
+	0xf1162067,
+	0xf1f55d77,
+	0xb9ffff73,
+	0x21f4026e,
+	0x02d8b904,
+	0xf90487fd,
+	0xfc80f960,
+	0xf4e0fcd0,
+	0x77f13321,
+	0x73f1fffe,
+	0x6eb9ffff,
+	0x0421f402,
+	0xfd02d8b9,
+	0x60f90487,
+	0xd0fc80f9,
+	0x21f4e0fc,
+	0xf067f133,
+	0x026eb926,
+	0xb90421f4,
+	0x87fd02d8,
+	0xf960f904,
+	0xfcd0fc80,
+	0x3321f4e0,
 	0xf10467f0,
 	0xd007e007,
 	0x04bd0006,
-/* 0x0400: memx_func_enter_wait */
+/* 0x053c: memx_func_enter_wait */
 	0x07c067f1,
 	0xf00066cf,
 	0x0bf40464,
-	0x001698f6,
-	0xf80410b6,
-/* 0x0415: memx_func_leave */
-	0x0467f000,
+	0x2c67f0f6,
+	0x800066cf,
+	0x00f8ee06,
+/* 0x0554: memx_func_leave */
+	0xcf2c67f0,
+	0x06800066,
+	0x0467f0ef,
 	0x07e407f1,
 	0xbd0006d0,
-/* 0x0421: memx_func_leave_wait */
+/* 0x0569: memx_func_leave_wait */
 	0xc067f104,
 	0x0066cf07,
 	0xf40464f0,
-	0x00f8f61b,
-/* 0x0430: memx_func_wr32 */
+	0x67f1f61b,
+	0x77f126f0,
+	0x73f00001,
+	0x026eb900,
+	0xb90421f4,
+	0x87fd02d8,
+	0xf960f905,
+	0xfcd0fc80,
+	0x3321f4e0,
+	0x162067f1,
+	0xf4026eb9,
+	0xd8b90421,
+	0x0587fd02,
+	0x80f960f9,
+	0xe0fcd0fc,
+	0xf13321f4,
+	0xf00aa277,
+	0x6eb90073,
+	0x0421f402,
+	0xfd02d8b9,
+	0x60f90587,
+	0xd0fc80f9,
+	0x21f4e0fc,
+/* 0x05d3: memx_func_wait_vblank */
+	0xb600f833,
+	0x00f80410,
+/* 0x05d8: memx_func_wr32 */
 	0x98001698,
 	0x10b60115,
 	0xf960f908,
@@ -1102,131 +1281,137 @@ uint32_t nvd0_pwr_code[] = {
 	0x3321f4e0,
 	0xf40242b6,
 	0x00f8e91b,
-/* 0x044c: memx_func_wait */
+/* 0x05f4: memx_func_wait */
 	0xcf2c87f0,
 	0x1e980088,
 	0x011d9800,
 	0x98021c98,
 	0x10b6031b,
-	0x7e21f410,
-/* 0x0466: memx_func_delay */
+	0x8621f410,
+/* 0x060e: memx_func_delay */
 	0x1e9800f8,
 	0x0410b600,
 	0xf86721f4,
-/* 0x0471: memx_exec */
+/* 0x0619: memx_exec */
 	0xf9e0f900,
 	0x02c1b9d0,
-/* 0x047b: memx_exec_next */
+/* 0x0623: memx_exec_next */
 	0x9802b2b9,
 	0x10b60013,
-	0x10349504,
+	0xf034e704,
+	0xe033e701,
+	0x0132b601,
 	0x980c30f0,
 	0x55f9de35,
 	0xf40612b8,
-	0xd0fcec1e,
+	0x0b98e41e,
+	0xef0c98ee,
+	0xf102cbbb,
+	0xcf07c4b7,
+	0xd0fc00bb,
 	0x21f5e0fc,
-	0x00f8026b,
-/* 0x049c: memx_info */
-	0x03acc7f1,
+	0x00f802f1,
+/* 0x065c: memx_info */
+	0x03c0c7f1,
 	0x0800b7f1,
-	0x026b21f5,
-/* 0x04aa: memx_recv */
+	0x02f121f5,
+/* 0x066a: memx_recv */
 	0xd6b000f8,
-	0xc40bf401,
+	0xac0bf401,
 	0xf400d6b0,
 	0x00f8e90b,
-/* 0x04b8: memx_init */
-/* 0x04ba: perf_recv */
+/* 0x0678: memx_init */
+/* 0x067a: perf_recv */
 	0x00f800f8,
-/* 0x04bc: perf_init */
-/* 0x04be: i2c_drive_scl */
+/* 0x067c: perf_init */
+/* 0x067e: i2c_drive_scl */
 	0x36b000f8,
 	0x0e0bf400,
 	0x07e007f1,
 	0xbd0001d0,
-/* 0x04cf: i2c_drive_scl_lo */
+/* 0x068f: i2c_drive_scl_lo */
 	0xf100f804,
 	0xd007e407,
 	0x04bd0001,
-/* 0x04da: i2c_drive_sda */
+/* 0x069a: i2c_drive_sda */
 	0x36b000f8,
 	0x0e0bf400,
 	0x07e007f1,
 	0xbd0002d0,
-/* 0x04eb: i2c_drive_sda_lo */
+/* 0x06ab: i2c_drive_sda_lo */
 	0xf100f804,
 	0xd007e407,
 	0x04bd0002,
-/* 0x04f6: i2c_sense_scl */
+/* 0x06b6: i2c_sense_scl */
 	0x32f400f8,
 	0xc437f101,
 	0x0033cf07,
 	0xf40431fd,
 	0x31f4060b,
-/* 0x0509: i2c_sense_scl_done */
-/* 0x050b: i2c_sense_sda */
+/* 0x06c9: i2c_sense_scl_done */
+/* 0x06cb: i2c_sense_sda */
 	0xf400f801,
 	0x37f10132,
 	0x33cf07c4,
 	0x0432fd00,
 	0xf4060bf4,
-/* 0x051e: i2c_sense_sda_done */
+/* 0x06de: i2c_sense_sda_done */
 	0x00f80131,
-/* 0x0520: i2c_raise_scl */
+/* 0x06e0: i2c_raise_scl */
 	0x47f140f9,
 	0x37f00898,
-	0xbe21f501,
-/* 0x052d: i2c_raise_scl_wait */
-	0xe8e7f104,
+	0x7e21f501,
+/* 0x06ed: i2c_raise_scl_wait */
+	0xe8e7f106,
 	0x6721f403,
-	0x04f621f5,
+	0x06b621f5,
 	0xb60901f4,
 	0x1bf40142,
-/* 0x0541: i2c_raise_scl_done */
+/* 0x0701: i2c_raise_scl_done */
 	0xf840fcef,
-/* 0x0545: i2c_start */
-	0xf621f500,
-	0x0d11f404,
-	0x050b21f5,
+/* 0x0705: i2c_start */
+	0xb621f500,
+	0x0d11f406,
+	0x06cb21f5,
 	0xf40611f4,
-/* 0x0556: i2c_start_rep */
+/* 0x0716: i2c_start_rep */
 	0x37f0300e,
-	0xbe21f500,
-	0x0137f004,
-	0x04da21f5,
+	0x7e21f500,
+	0x0137f006,
+	0x069a21f5,
 	0xb60076bb,
 	0x50f90465,
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x2021f550,
-	0x0464b605,
-/* 0x0583: i2c_start_send */
+	0xe021f550,
+	0x0464b606,
+/* 0x0743: i2c_start_send */
 	0xf01f11f4,
 	0x21f50037,
-	0xe7f104da,
+	0xe7f1069a,
 	0x21f41388,
 	0x0037f067,
-	0x04be21f5,
+	0x067e21f5,
 	0x1388e7f1,
-/* 0x059f: i2c_start_out */
+/* 0x075f: i2c_start_out */
 	0xf86721f4,
-/* 0x05a1: i2c_stop */
+/* 0x0761: i2c_stop */
 	0x0037f000,
-	0x04be21f5,
+	0x067e21f5,
 	0xf50037f0,
-	0xf104da21,
+	0xf1069a21,
 	0xf403e8e7,
 	0x37f06721,
-	0xbe21f501,
-	0x88e7f104,
+	0x7e21f501,
+	0x88e7f106,
 	0x6721f413,
 	0xf50137f0,
-	0xf104da21,
+	0xf1069a21,
 	0xf41388e7,
 	0x00f86721,
-/* 0x05d4: i2c_bitw */
-	0x04da21f5,
+/* 0x0794: i2c_bitw */
+	0x069a21f5,
 	0x03e8e7f1,
 	0xbb6721f4,
 	0x65b60076,
@@ -1234,18 +1419,18 @@ uint32_t nvd0_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x052021f5,
+	0x06e021f5,
 	0xf40464b6,
 	0xe7f11811,
 	0x21f41388,
 	0x0037f067,
-	0x04be21f5,
+	0x067e21f5,
 	0x1388e7f1,
-/* 0x0613: i2c_bitw_out */
+/* 0x07d3: i2c_bitw_out */
 	0xf86721f4,
-/* 0x0615: i2c_bitr */
+/* 0x07d5: i2c_bitr */
 	0x0137f000,
-	0x04da21f5,
+	0x069a21f5,
 	0x03e8e7f1,
 	0xbb6721f4,
 	0x65b60076,
@@ -1253,19 +1438,19 @@ uint32_t nvd0_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x052021f5,
+	0x06e021f5,
 	0xf40464b6,
 	0x21f51b11,
-	0x37f0050b,
-	0xbe21f500,
-	0x88e7f104,
+	0x37f006cb,
+	0x7e21f500,
+	0x88e7f106,
 	0x6721f413,
 	0xf4013cf0,
-/* 0x065a: i2c_bitr_done */
+/* 0x081a: i2c_bitr_done */
 	0x00f80131,
-/* 0x065c: i2c_get_byte */
+/* 0x081c: i2c_get_byte */
 	0xf00057f0,
-/* 0x0662: i2c_get_byte_next */
+/* 0x0822: i2c_get_byte_next */
 	0x54b60847,
 	0x0076bb01,
 	0xf90465b6,
@@ -1273,7 +1458,7 @@ uint32_t nvd0_pwr_code[] = {
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b60615,
+	0x64b607d5,
 	0x2b11f404,
 	0xb60553fd,
 	0x1bf40142,
@@ -1283,12 +1468,12 @@ uint32_t nvd0_pwr_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xd421f550,
-	0x0464b605,
-/* 0x06ac: i2c_get_byte_done */
-/* 0x06ae: i2c_put_byte */
+	0x9421f550,
+	0x0464b607,
+/* 0x086c: i2c_get_byte_done */
+/* 0x086e: i2c_put_byte */
 	0x47f000f8,
-/* 0x06b1: i2c_put_byte_next */
+/* 0x0871: i2c_put_byte_next */
 	0x0142b608,
 	0xbb3854ff,
 	0x65b60076,
@@ -1296,7 +1481,7 @@ uint32_t nvd0_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x05d421f5,
+	0x079421f5,
 	0xf40464b6,
 	0x46b03411,
 	0xd81bf400,
@@ -1305,21 +1490,21 @@ uint32_t nvd0_pwr_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x1521f550,
-	0x0464b606,
+	0xd521f550,
+	0x0464b607,
 	0xbb0f11f4,
 	0x36b00076,
 	0x061bf401,
-/* 0x0707: i2c_put_byte_done */
+/* 0x08c7: i2c_put_byte_done */
 	0xf80132f4,
-/* 0x0709: i2c_addr */
+/* 0x08c9: i2c_addr */
 	0x0076bb00,
 	0xf90465b6,
 	0x04659450,
 	0xbd0256bb,
 	0x0475fd50,
 	0x21f550fc,
-	0x64b60545,
+	0x64b60705,
 	0x2911f404,
 	0x012ec3e7,
 	0xfd0134b6,
@@ -1329,30 +1514,30 @@ uint32_t nvd0_pwr_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb606ae21,
-/* 0x074e: i2c_addr_done */
+	0xb6086e21,
+/* 0x090e: i2c_addr_done */
 	0x00f80464,
-/* 0x0750: i2c_acquire_addr */
+/* 0x0910: i2c_acquire_addr */
 	0xb6f8cec7,
 	0xe0b705e4,
 	0x00f8d014,
-/* 0x075c: i2c_acquire */
-	0x075021f5,
+/* 0x091c: i2c_acquire */
+	0x091021f5,
 	0xf00421f4,
 	0x21f403d9,
-/* 0x076b: i2c_release */
+/* 0x092b: i2c_release */
 	0xf500f833,
-	0xf4075021,
+	0xf4091021,
 	0xdaf00421,
 	0x3321f403,
-/* 0x077a: i2c_recv */
+/* 0x093a: i2c_recv */
 	0x32f400f8,
 	0xf8c1c701,
 	0xb00214b6,
 	0x1ff52816,
 	0x13a0013a,
-	0x32980bd4,
-	0xac13a000,
+	0x32980be8,
+	0xc013a000,
 	0x0031980b,
 	0xf90231f4,
 	0xf9e0f9d0,
@@ -1364,8 +1549,8 @@ uint32_t nvd0_pwr_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x5c21f550,
-	0x0464b607,
+	0x1c21f550,
+	0x0464b609,
 	0xd6b0d0fc,
 	0xb31bf500,
 	0x0057f000,
@@ -1374,8 +1559,8 @@ uint32_t nvd0_pwr_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0x0921f550,
-	0x0464b607,
+	0xc921f550,
+	0x0464b608,
 	0x00d011f5,
 	0xbbe0c5c7,
 	0x65b60076,
@@ -1383,7 +1568,7 @@ uint32_t nvd0_pwr_code[] = {
 	0x56bb0465,
 	0xfd50bd02,
 	0x50fc0475,
-	0x06ae21f5,
+	0x086e21f5,
 	0xf50464b6,
 	0xf000ad11,
 	0x76bb0157,
@@ -1392,7 +1577,7 @@ uint32_t nvd0_pwr_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6070921,
+	0xb608c921,
 	0x11f50464,
 	0x76bb008a,
 	0x0465b600,
@@ -1400,7 +1585,7 @@ uint32_t nvd0_pwr_code[] = {
 	0x0256bb04,
 	0x75fd50bd,
 	0xf550fc04,
-	0xb6065c21,
+	0xb6081c21,
 	0x11f40464,
 	0xe05bcb6a,
 	0xb60076bb,
@@ -1408,38 +1593,38 @@ uint32_t nvd0_pwr_code[] = {
 	0xbb046594,
 	0x50bd0256,
 	0xfc0475fd,
-	0xa121f550,
-	0x0464b605,
+	0x6121f550,
+	0x0464b607,
 	0xbd025bb9,
 	0x430ef474,
-/* 0x0880: i2c_recv_not_rd08 */
+/* 0x0a40: i2c_recv_not_rd08 */
 	0xf401d6b0,
 	0x57f03d1b,
-	0x0921f500,
-	0x3311f407,
+	0xc921f500,
+	0x3311f408,
 	0xf5e0c5c7,
-	0xf406ae21,
+	0xf4086e21,
 	0x57f02911,
-	0x0921f500,
-	0x1f11f407,
+	0xc921f500,
+	0x1f11f408,
 	0xf5e0b5c7,
-	0xf406ae21,
+	0xf4086e21,
 	0x21f51511,
-	0x74bd05a1,
+	0x74bd0761,
 	0xf408c5c7,
 	0x32f4091b,
 	0x030ef402,
-/* 0x08c0: i2c_recv_not_wr08 */
-/* 0x08c0: i2c_recv_done */
+/* 0x0a80: i2c_recv_not_wr08 */
+/* 0x0a80: i2c_recv_done */
 	0xf5f8cec7,
-	0xfc076b21,
+	0xfc092b21,
 	0xf4d0fce0,
 	0x7cb90a12,
-	0x6b21f502,
-/* 0x08d5: i2c_recv_exit */
-/* 0x08d7: i2c_init */
+	0xf121f502,
+/* 0x0a95: i2c_recv_exit */
+/* 0x0a97: i2c_init */
 	0xf800f802,
-/* 0x08d9: test_recv */
+/* 0x0a99: test_recv */
 	0xd817f100,
 	0x0011cf05,
 	0xf10110b6,
@@ -1447,29 +1632,29 @@ uint32_t nvd0_pwr_code[] = {
 	0x04bd0001,
 	0xd900e7f1,
 	0x134fe3f1,
-	0x01b621f5,
-/* 0x08fa: test_init */
+	0x022321f5,
+/* 0x0aba: test_init */
 	0xe7f100f8,
 	0x21f50800,
-	0x00f801b6,
-/* 0x0904: idle_recv */
-/* 0x0906: idle */
+	0x00f80223,
+/* 0x0ac4: idle_recv */
+/* 0x0ac6: idle */
 	0x31f400f8,
 	0xd417f100,
 	0x0011cf05,
 	0xf10110b6,
 	0xd005d407,
 	0x04bd0001,
-/* 0x091c: idle_loop */
+/* 0x0adc: idle_loop */
 	0xf45817f0,
-/* 0x0922: idle_proc */
-/* 0x0922: idle_proc_exec */
+/* 0x0ae2: idle_proc */
+/* 0x0ae2: idle_proc_exec */
 	0x10f90232,
 	0xf5021eb9,
-	0xfc027421,
+	0xfc02fa21,
 	0x0911f410,
 	0xf40231f4,
-/* 0x0936: idle_proc_next */
+/* 0x0af6: idle_proc_next */
 	0x10b6ef0e,
 	0x061fb858,
 	0xf4e61bf4,
@@ -1521,4 +1706,20 @@ uint32_t nvd0_pwr_code[] = {
 	0x00000000,
 	0x00000000,
 	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
+	0x00000000,
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h
index 574acfa44c8c..522e3079f824 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/fuc/os.h
@@ -19,11 +19,12 @@
 #define MEMX_MSG_EXEC 1
 
 /* MEMX: script opcode definitions */
-#define MEMX_ENTER  0
-#define MEMX_LEAVE  1
-#define MEMX_WR32   2
-#define MEMX_WAIT   3
-#define MEMX_DELAY  4
+#define MEMX_ENTER  1
+#define MEMX_LEAVE  2
+#define MEMX_WR32   3
+#define MEMX_WAIT   4
+#define MEMX_DELAY  5
+#define MEMX_VBLANK 6
 
 /* I2C_: message identifiers */
 #define I2C__MSG_RD08 0
diff --git a/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c b/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c
index def6a9ac68cf..65eaa2546cad 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/pwr/memx.c
@@ -20,10 +20,11 @@ memx_out(struct nouveau_memx *memx)
 	struct nouveau_pwr *ppwr = memx->ppwr;
 	int i;
 
-	if (memx->c.size) {
+	if (memx->c.mthd) {
 		nv_wr32(ppwr, 0x10a1c4, (memx->c.size << 16) | memx->c.mthd);
 		for (i = 0; i < memx->c.size; i++)
 			nv_wr32(ppwr, 0x10a1c4, memx->c.data[i]);
+		memx->c.mthd = 0;
 		memx->c.size = 0;
 	}
 }
@@ -32,7 +33,7 @@ static void
 memx_cmd(struct nouveau_memx *memx, u32 mthd, u32 size, u32 data[])
 {
 	if ((memx->c.size + size >= ARRAY_SIZE(memx->c.data)) ||
-	    (memx->c.size && memx->c.mthd != mthd))
+	    (memx->c.mthd && memx->c.mthd != mthd))
 		memx_out(memx);
 	memcpy(&memx->c.data[memx->c.size], data, size * sizeof(data[0]));
 	memx->c.size += size;
@@ -62,8 +63,7 @@ nouveau_memx_init(struct nouveau_pwr *ppwr, struct nouveau_memx **pmemx)
 		nv_wr32(ppwr, 0x10a580, 0x00000003);
 	} while (nv_rd32(ppwr, 0x10a580) != 0x00000003);
 	nv_wr32(ppwr, 0x10a1c0, 0x01000000 | memx->base);
-	nv_wr32(ppwr, 0x10a1c4, 0x00010000 | MEMX_ENTER);
-	nv_wr32(ppwr, 0x10a1c4, 0x00000000);
+
 	return 0;
 }
 
@@ -78,7 +78,6 @@ nouveau_memx_fini(struct nouveau_memx **pmemx, bool exec)
 	memx_out(memx);
 
 	/* release data segment access */
-	nv_wr32(ppwr, 0x10a1c4, 0x00000000 | MEMX_LEAVE);
 	finish = nv_rd32(ppwr, 0x10a1c0) & 0x00ffffff;
 	nv_wr32(ppwr, 0x10a580, 0x00000000);
 
@@ -88,6 +87,8 @@ nouveau_memx_fini(struct nouveau_memx **pmemx, bool exec)
 				 memx->base, finish);
 	}
 
+	nv_debug(memx->ppwr, "Exec took %uns, PPWR_IN %08x\n",
+		 reply[0], reply[1]);
 	kfree(memx);
 	return 0;
 }
@@ -117,4 +118,51 @@ nouveau_memx_nsec(struct nouveau_memx *memx, u32 nsec)
 	memx_out(memx); /* fuc can't handle multiple */
 }
 
+void
+nouveau_memx_wait_vblank(struct nouveau_memx *memx)
+{
+	struct nouveau_pwr *ppwr = memx->ppwr;
+	u32 heads, x, y, px = 0;
+	int i, head_sync;
+
+	if (nv_device(ppwr)->chipset < 0xd0) {
+		heads = nv_rd32(ppwr, 0x610050);
+		for (i = 0; i < 2; i++) {
+			/* Heuristic: sync to head with biggest resolution */
+			if (heads & (2 << (i << 3))) {
+				x = nv_rd32(ppwr, 0x610b40 + (0x540 * i));
+				y = (x & 0xffff0000) >> 16;
+				x &= 0x0000ffff;
+				if ((x * y) > px) {
+					px = (x * y);
+					head_sync = i;
+				}
+			}
+		}
+	}
+
+	if (px == 0) {
+		nv_debug(memx->ppwr, "WAIT VBLANK !NO ACTIVE HEAD\n");
+		return;
+	}
+
+	nv_debug(memx->ppwr, "WAIT VBLANK HEAD%d\n", head_sync);
+	memx_cmd(memx, MEMX_VBLANK, 1, (u32[]){ head_sync });
+	memx_out(memx); /* fuc can't handle multiple */
+}
+
+void
+nouveau_memx_block(struct nouveau_memx *memx)
+{
+	nv_debug(memx->ppwr, "   HOST BLOCKED\n");
+	memx_cmd(memx, MEMX_ENTER, 0, NULL);
+}
+
+void
+nouveau_memx_unblock(struct nouveau_memx *memx)
+{
+	nv_debug(memx->ppwr, "   HOST UNBLOCKED\n");
+	memx_cmd(memx, MEMX_LEAVE, 0, NULL);
+}
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c b/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c
index 016990a8252c..3656d605168f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/fan.c
@@ -31,6 +31,8 @@
 #include <subdev/gpio.h>
 #include <subdev/timer.h>
 
+#include <subdev/bios/fan.h>
+
 static int
 nouveau_fan_update(struct nouveau_fan *fan, bool immediate, int target)
 {
@@ -275,8 +277,11 @@ nouveau_therm_fan_ctor(struct nouveau_therm *therm)
 	/* other random init... */
 	nouveau_therm_fan_set_defaults(therm);
 	nvbios_perf_fan_parse(bios, &priv->fan->perf);
-	if (nvbios_therm_fan_parse(bios, &priv->fan->bios))
-		nv_error(therm, "parsing the thermal table failed\n");
+	if (!nvbios_fan_parse(bios, &priv->fan->bios)) {
+		nv_debug(therm, "parsing the fan table failed\n");
+		if (nvbios_therm_fan_parse(bios, &priv->fan->bios))
+			nv_error(therm, "parsing both fan tables failed\n");
+	}
 	nouveau_therm_fan_safety_checks(therm);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/fanpwm.c b/drivers/gpu/drm/nouveau/core/subdev/therm/fanpwm.c
index 9a5c07340263..c629d7f2a6a4 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/fanpwm.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/fanpwm.c
@@ -25,6 +25,8 @@
 
 #include <core/option.h>
 #include <subdev/gpio.h>
+#include <subdev/bios.h>
+#include <subdev/bios/fan.h>
 
 #include "priv.h"
 
@@ -86,11 +88,15 @@ nouveau_fanpwm_create(struct nouveau_therm *therm, struct dcb_gpio_func *func)
 {
 	struct nouveau_device *device = nv_device(therm);
 	struct nouveau_therm_priv *tpriv = (void *)therm;
+	struct nouveau_bios *bios = nouveau_bios(therm);
 	struct nouveau_fanpwm_priv *priv;
+	struct nvbios_therm_fan fan;
 	u32 divs, duty;
 
+	nvbios_fan_parse(bios, &fan);
+
 	if (!nouveau_boolopt(device->cfgopt, "NvFanPWM", func->param) ||
-	    !therm->pwm_ctrl ||
+	    !therm->pwm_ctrl || fan.type == NVBIOS_THERM_FAN_TOGGLE ||
 	     therm->pwm_get(therm, func->line, &divs, &duty) == -ENODEV)
 		return -ENODEV;
 
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/gm107.c b/drivers/gpu/drm/nouveau/core/subdev/therm/gm107.c
new file mode 100644
index 000000000000..668cf3322285
--- /dev/null
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/gm107.c
@@ -0,0 +1,93 @@
+/*
+ * Copyright 2014 Martin Peres
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Martin Peres
+ */
+
+#include "priv.h"
+
+struct gm107_therm_priv {
+	struct nouveau_therm_priv base;
+};
+
+static int
+gm107_fan_pwm_ctrl(struct nouveau_therm *therm, int line, bool enable)
+{
+	/* nothing to do, it seems hardwired */
+	return 0;
+}
+
+static int
+gm107_fan_pwm_get(struct nouveau_therm *therm, int line, u32 *divs, u32 *duty)
+{
+	*divs = nv_rd32(therm, 0x10eb20) & 0x1fff;
+	*duty = nv_rd32(therm, 0x10eb24) & 0x1fff;
+	return 0;
+}
+
+static int
+gm107_fan_pwm_set(struct nouveau_therm *therm, int line, u32 divs, u32 duty)
+{
+	nv_mask(therm, 0x10eb10, 0x1fff, divs); /* keep the high bits */
+	nv_wr32(therm, 0x10eb14, duty | 0x80000000);
+	return 0;
+}
+
+static int
+gm107_fan_pwm_clock(struct nouveau_therm *therm, int line)
+{
+	return nv_device(therm)->crystal * 1000;
+}
+
+static int
+gm107_therm_ctor(struct nouveau_object *parent,
+		struct nouveau_object *engine,
+		struct nouveau_oclass *oclass, void *data, u32 size,
+		struct nouveau_object **pobject)
+{
+	struct gm107_therm_priv *priv;
+	int ret;
+
+	ret = nouveau_therm_create(parent, engine, oclass, &priv);
+	*pobject = nv_object(priv);
+	if (ret)
+		return ret;
+
+	priv->base.base.pwm_ctrl = gm107_fan_pwm_ctrl;
+	priv->base.base.pwm_get = gm107_fan_pwm_get;
+	priv->base.base.pwm_set = gm107_fan_pwm_set;
+	priv->base.base.pwm_clock = gm107_fan_pwm_clock;
+	priv->base.base.temp_get = nv84_temp_get;
+	priv->base.base.fan_sense = nva3_therm_fan_sense;
+	priv->base.sensor.program_alarms = nouveau_therm_program_alarms_polling;
+	return nouveau_therm_preinit(&priv->base.base);
+}
+
+struct nouveau_oclass
+gm107_therm_oclass = {
+	.handle = NV_SUBDEV(THERM, 0x117),
+	.ofuncs = &(struct nouveau_ofuncs) {
+		.ctor = gm107_therm_ctor,
+		.dtor = _nouveau_therm_dtor,
+		.init = nvd0_therm_init,
+		.fini = nv84_therm_fini,
+	},
+};
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nv84.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nv84.c
index 1d15c52fad0c..14e2e09bfc24 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nv84.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nv84.c
@@ -24,6 +24,7 @@
  */
 
 #include "priv.h"
+#include <subdev/fuse.h>
 
 struct nv84_therm_priv {
 	struct nouveau_therm_priv base;
@@ -32,7 +33,25 @@ struct nv84_therm_priv {
 int
 nv84_temp_get(struct nouveau_therm *therm)
 {
-	return nv_rd32(therm, 0x20400);
+	struct nouveau_fuse *fuse = nouveau_fuse(therm);
+
+	if (nv_ro32(fuse, 0x1a8) == 1)
+		return nv_rd32(therm, 0x20400);
+	else
+		return -ENODEV;
+}
+
+void
+nv84_sensor_setup(struct nouveau_therm *therm)
+{
+	struct nouveau_fuse *fuse = nouveau_fuse(therm);
+
+	/* enable temperature reading for cards with insane defaults */
+	if (nv_ro32(fuse, 0x1a8) == 1) {
+		nv_mask(therm, 0x20008, 0x80008000, 0x80000000);
+		nv_mask(therm, 0x2000c, 0x80000003, 0x00000000);
+		mdelay(20); /* wait for the temperature to stabilize */
+	}
 }
 
 static void
@@ -171,6 +190,21 @@ nv84_therm_intr(struct nouveau_subdev *subdev)
 }
 
 static int
+nv84_therm_init(struct nouveau_object *object)
+{
+	struct nv84_therm_priv *priv = (void *)object;
+	int ret;
+
+	ret = nouveau_therm_init(&priv->base.base);
+	if (ret)
+		return ret;
+
+	nv84_sensor_setup(&priv->base.base);
+
+	return 0;
+}
+
+static int
 nv84_therm_ctor(struct nouveau_object *parent,
 		struct nouveau_object *engine,
 		struct nouveau_oclass *oclass, void *data, u32 size,
@@ -228,7 +262,7 @@ nv84_therm_oclass = {
 	.ofuncs = &(struct nouveau_ofuncs) {
 		.ctor = nv84_therm_ctor,
 		.dtor = _nouveau_therm_dtor,
-		.init = _nouveau_therm_init,
+		.init = nv84_therm_init,
 		.fini = nv84_therm_fini,
 	},
 };
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c
index 0478b2e3fb1d..7893357a7e9f 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nva3.c
@@ -51,6 +51,8 @@ nva3_therm_init(struct nouveau_object *object)
 	if (ret)
 		return ret;
 
+	nv84_sensor_setup(&priv->base.base);
+
 	/* enable fan tach, count revolutions per-second */
 	nv_mask(priv, 0x00e720, 0x00000003, 0x00000002);
 	if (tach->func != DCB_GPIO_UNUSED) {
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
index bbf117be572f..b70f7cc649b8 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/nvd0.c
@@ -114,7 +114,7 @@ nvd0_fan_pwm_clock(struct nouveau_therm *therm, int line)
 		return nv_device(therm)->crystal * 1000 / 10;
 }
 
-static int
+int
 nvd0_therm_init(struct nouveau_object *object)
 {
 	struct nvd0_therm_priv *priv = (void *)object;
@@ -150,6 +150,8 @@ nvd0_therm_ctor(struct nouveau_object *parent,
 	if (ret)
 		return ret;
 
+	nv84_sensor_setup(&priv->base.base);
+
 	priv->base.base.pwm_ctrl = nvd0_fan_pwm_ctrl;
 	priv->base.base.pwm_get = nvd0_fan_pwm_get;
 	priv->base.base.pwm_set = nvd0_fan_pwm_set;
diff --git a/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h b/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h
index 916fca5c7816..7dba8c281a0b 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h
+++ b/drivers/gpu/drm/nouveau/core/subdev/therm/priv.h
@@ -145,10 +145,13 @@ int nv50_fan_pwm_get(struct nouveau_therm *, int, u32 *, u32 *);
 int nv50_fan_pwm_set(struct nouveau_therm *, int, u32, u32);
 int nv50_fan_pwm_clock(struct nouveau_therm *, int);
 int nv84_temp_get(struct nouveau_therm *therm);
+void nv84_sensor_setup(struct nouveau_therm *therm);
 int nv84_therm_fini(struct nouveau_object *object, bool suspend);
 
 int nva3_therm_fan_sense(struct nouveau_therm *);
 
+int nvd0_therm_init(struct nouveau_object *object);
+
 int nouveau_fanpwm_create(struct nouveau_therm *, struct dcb_gpio_func *);
 int nouveau_fantog_create(struct nouveau_therm *, struct dcb_gpio_func *);
 int nouveau_fannil_create(struct nouveau_therm *);
diff --git a/drivers/gpu/drm/nouveau/core/subdev/vm/base.c b/drivers/gpu/drm/nouveau/core/subdev/vm/base.c
index 7dd680ff2f6f..f75a683bd47a 100644
--- a/drivers/gpu/drm/nouveau/core/subdev/vm/base.c
+++ b/drivers/gpu/drm/nouveau/core/subdev/vm/base.c
@@ -296,7 +296,7 @@ nouveau_vm_get(struct nouveau_vm *vm, u64 size, u32 page_shift,
 	int ret;
 
 	mutex_lock(&nv_subdev(vmm)->mutex);
-	ret = nouveau_mm_head(&vm->mm, page_shift, msize, msize, align,
+	ret = nouveau_mm_head(&vm->mm, 0, page_shift, msize, msize, align,
 			     &vma->node);
 	if (unlikely(ret != 0)) {
 		mutex_unlock(&nv_subdev(vmm)->mutex);
diff --git a/drivers/gpu/drm/nouveau/dispnv04/crtc.c b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
index b90aa5c1f90a..fca6a1f9c20c 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/crtc.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/crtc.c
@@ -1127,7 +1127,7 @@ nv04_crtc_create(struct drm_device *dev, int crtc_num)
 	drm_mode_crtc_set_gamma_size(&nv_crtc->base, 256);
 
 	ret = nouveau_bo_new(dev, 64*64*4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, NULL, &nv_crtc->cursor.nvbo);
+			     0, 0x0000, NULL, NULL, &nv_crtc->cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(nv_crtc->cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret) {
diff --git a/drivers/gpu/drm/nouveau/dispnv04/overlay.c b/drivers/gpu/drm/nouveau/dispnv04/overlay.c
index b36afcbbc83f..1e9056a8df94 100644
--- a/drivers/gpu/drm/nouveau/dispnv04/overlay.c
+++ b/drivers/gpu/drm/nouveau/dispnv04/overlay.c
@@ -97,7 +97,8 @@ nv10_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		  uint32_t src_w, uint32_t src_h)
 {
 	struct nvif_device *dev = &nouveau_drm(plane->dev)->device;
-	struct nouveau_plane *nv_plane = (struct nouveau_plane *)plane;
+	struct nouveau_plane *nv_plane =
+		container_of(plane, struct nouveau_plane, base);
 	struct nouveau_framebuffer *nv_fb = nouveau_framebuffer(fb);
 	struct nouveau_crtc *nv_crtc = nouveau_crtc(crtc);
 	struct nouveau_bo *cur = nv_plane->cur;
@@ -173,7 +174,8 @@ static int
 nv10_disable_plane(struct drm_plane *plane)
 {
 	struct nvif_device *dev = &nouveau_drm(plane->dev)->device;
-	struct nouveau_plane *nv_plane = (struct nouveau_plane *)plane;
+	struct nouveau_plane *nv_plane =
+		container_of(plane, struct nouveau_plane, base);
 
 	nvif_wr32(dev, NV_PVIDEO_STOP, 1);
 	if (nv_plane->cur) {
@@ -224,7 +226,8 @@ nv_set_property(struct drm_plane *plane,
 		struct drm_property *property,
 		uint64_t value)
 {
-	struct nouveau_plane *nv_plane = (struct nouveau_plane *)plane;
+	struct nouveau_plane *nv_plane =
+		container_of(plane, struct nouveau_plane, base);
 
 	if (property == nv_plane->props.colorkey)
 		nv_plane->colorkey = value;
@@ -344,7 +347,8 @@ nv04_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 		  uint32_t src_w, uint32_t src_h)
 {
 	struct nvif_device *dev = &nouveau_drm(plane->dev)->device;
-	struct nouveau_plane *nv_plane = (struct nouveau_plane *)plane;
+	struct nouveau_plane *nv_plane =
+		container_of(plane, struct nouveau_plane, base);
 	struct nouveau_framebuffer *nv_fb = nouveau_framebuffer(fb);
 	struct nouveau_bo *cur = nv_plane->cur;
 	uint32_t overlay = 1;
@@ -423,7 +427,8 @@ static int
 nv04_disable_plane(struct drm_plane *plane)
 {
 	struct nvif_device *dev = &nouveau_drm(plane->dev)->device;
-	struct nouveau_plane *nv_plane = (struct nouveau_plane *)plane;
+	struct nouveau_plane *nv_plane =
+		container_of(plane, struct nouveau_plane, base);
 
 	nvif_mask(dev, NV_PVIDEO_OVERLAY, 1, 0);
 	nvif_wr32(dev, NV_PVIDEO_OE_STATE, 0);
diff --git a/drivers/gpu/drm/nouveau/nouveau_abi16.c b/drivers/gpu/drm/nouveau/nouveau_abi16.c
index 615714c1727d..a24faa5e2a2a 100644
--- a/drivers/gpu/drm/nouveau/nouveau_abi16.c
+++ b/drivers/gpu/drm/nouveau/nouveau_abi16.c
@@ -448,7 +448,7 @@ nouveau_abi16_ioctl_notifierobj_alloc(ABI16_IOCTL_ARGS)
 	list_add(&ntfy->head, &chan->notifiers);
 	ntfy->handle = info->handle;
 
-	ret = nouveau_mm_head(&chan->heap, 1, info->size, info->size, 1,
+	ret = nouveau_mm_head(&chan->heap, 0, 1, info->size, info->size, 1,
 			      &ntfy->node);
 	if (ret)
 		goto done;
diff --git a/drivers/gpu/drm/nouveau/nouveau_acpi.c b/drivers/gpu/drm/nouveau/nouveau_acpi.c
index 279206997e5c..622424692b3b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_acpi.c
+++ b/drivers/gpu/drm/nouveau/nouveau_acpi.c
@@ -46,7 +46,6 @@ static struct nouveau_dsm_priv {
 	bool dsm_detected;
 	bool optimus_detected;
 	acpi_handle dhandle;
-	acpi_handle other_handle;
 	acpi_handle rom_handle;
 } nouveau_dsm_priv;
 
@@ -222,10 +221,9 @@ static int nouveau_dsm_pci_probe(struct pci_dev *pdev)
 	if (!dhandle)
 		return false;
 
-	if (!acpi_has_method(dhandle, "_DSM")) {
-		nouveau_dsm_priv.other_handle = dhandle;
+	if (!acpi_has_method(dhandle, "_DSM"))
 		return false;
-	}
+
 	if (acpi_check_dsm(dhandle, nouveau_dsm_muid, 0x00000102,
 			   1 << NOUVEAU_DSM_POWER))
 		retval |= NOUVEAU_DSM_HAS_MUX;
@@ -301,16 +299,6 @@ static bool nouveau_dsm_detect(void)
 		printk(KERN_INFO "VGA switcheroo: detected DSM switching method %s handle\n",
 			acpi_method_name);
 		nouveau_dsm_priv.dsm_detected = true;
-		/*
-		 * On some systems hotplug events are generated for the device
-		 * being switched off when _DSM is executed.  They cause ACPI
-		 * hotplug to trigger and attempt to remove the device from
-		 * the system, which causes it to break down.  Prevent that from
-		 * happening by setting the no_hotplug flag for the involved
-		 * ACPI device objects.
-		 */
-		acpi_bus_no_hotplug(nouveau_dsm_priv.dhandle);
-		acpi_bus_no_hotplug(nouveau_dsm_priv.other_handle);
 		ret = true;
 	}
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.c b/drivers/gpu/drm/nouveau/nouveau_bo.c
index 01da508625f2..3d474ac03f88 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.c
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.c
@@ -88,13 +88,13 @@ nv10_bo_get_tile_region(struct drm_device *dev, int i)
 
 static void
 nv10_bo_put_tile_region(struct drm_device *dev, struct nouveau_drm_tile *tile,
-			struct nouveau_fence *fence)
+			struct fence *fence)
 {
 	struct nouveau_drm *drm = nouveau_drm(dev);
 
 	if (tile) {
 		spin_lock(&drm->tile.lock);
-		tile->fence = nouveau_fence_ref(fence);
+		tile->fence = (struct nouveau_fence *)fence_get(fence);
 		tile->used = false;
 		spin_unlock(&drm->tile.lock);
 	}
@@ -181,7 +181,7 @@ nouveau_bo_fixup_align(struct nouveau_bo *nvbo, u32 flags,
 int
 nouveau_bo_new(struct drm_device *dev, int size, int align,
 	       uint32_t flags, uint32_t tile_mode, uint32_t tile_flags,
-	       struct sg_table *sg,
+	       struct sg_table *sg, struct reservation_object *robj,
 	       struct nouveau_bo **pnvbo)
 {
 	struct nouveau_drm *drm = nouveau_drm(dev);
@@ -230,7 +230,7 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 	ret = ttm_bo_init(&drm->ttm.bdev, &nvbo->bo, size,
 			  type, &nvbo->placement,
 			  align >> PAGE_SHIFT, false, NULL, acc_size, sg,
-			  nouveau_bo_del_ttm);
+			  robj, nouveau_bo_del_ttm);
 	if (ret) {
 		/* ttm will call nouveau_bo_del_ttm if it fails.. */
 		return ret;
@@ -241,16 +241,16 @@ nouveau_bo_new(struct drm_device *dev, int size, int align,
 }
 
 static void
-set_placement_list(uint32_t *pl, unsigned *n, uint32_t type, uint32_t flags)
+set_placement_list(struct ttm_place *pl, unsigned *n, uint32_t type, uint32_t flags)
 {
 	*n = 0;
 
 	if (type & TTM_PL_FLAG_VRAM)
-		pl[(*n)++] = TTM_PL_FLAG_VRAM | flags;
+		pl[(*n)++].flags = TTM_PL_FLAG_VRAM | flags;
 	if (type & TTM_PL_FLAG_TT)
-		pl[(*n)++] = TTM_PL_FLAG_TT | flags;
+		pl[(*n)++].flags = TTM_PL_FLAG_TT | flags;
 	if (type & TTM_PL_FLAG_SYSTEM)
-		pl[(*n)++] = TTM_PL_FLAG_SYSTEM | flags;
+		pl[(*n)++].flags = TTM_PL_FLAG_SYSTEM | flags;
 }
 
 static void
@@ -258,6 +258,7 @@ set_placement_range(struct nouveau_bo *nvbo, uint32_t type)
 {
 	struct nouveau_drm *drm = nouveau_bdev(nvbo->bo.bdev);
 	u32 vram_pages = drm->device.info.ram_size >> PAGE_SHIFT;
+	unsigned i, fpfn, lpfn;
 
 	if (drm->device.info.family == NV_DEVICE_INFO_V0_CELSIUS &&
 	    nvbo->tile_mode && (type & TTM_PL_FLAG_VRAM) &&
@@ -269,11 +270,19 @@ set_placement_range(struct nouveau_bo *nvbo, uint32_t type)
 		 * at the same time.
 		 */
 		if (nvbo->tile_flags & NOUVEAU_GEM_TILE_ZETA) {
-			nvbo->placement.fpfn = vram_pages / 2;
-			nvbo->placement.lpfn = ~0;
+			fpfn = vram_pages / 2;
+			lpfn = ~0;
 		} else {
-			nvbo->placement.fpfn = 0;
-			nvbo->placement.lpfn = vram_pages / 2;
+			fpfn = 0;
+			lpfn = vram_pages / 2;
+		}
+		for (i = 0; i < nvbo->placement.num_placement; ++i) {
+			nvbo->placements[i].fpfn = fpfn;
+			nvbo->placements[i].lpfn = lpfn;
+		}
+		for (i = 0; i < nvbo->placement.num_busy_placement; ++i) {
+			nvbo->busy_placements[i].fpfn = fpfn;
+			nvbo->busy_placements[i].lpfn = lpfn;
 		}
 	}
 }
@@ -961,13 +970,14 @@ nouveau_bo_move_m2mf(struct ttm_buffer_object *bo, int evict, bool intr,
 	}
 
 	mutex_lock_nested(&cli->mutex, SINGLE_DEPTH_NESTING);
-	ret = nouveau_fence_sync(bo->sync_obj, chan);
+	ret = nouveau_fence_sync(nouveau_bo(bo), chan, true, intr);
 	if (ret == 0) {
 		ret = drm->ttm.move(chan, bo, &bo->mem, new_mem);
 		if (ret == 0) {
 			ret = nouveau_fence_new(chan, false, &fence);
 			if (ret == 0) {
-				ret = ttm_bo_move_accel_cleanup(bo, fence,
+				ret = ttm_bo_move_accel_cleanup(bo,
+								&fence->base,
 								evict,
 								no_wait_gpu,
 								new_mem);
@@ -1041,12 +1051,15 @@ static int
 nouveau_bo_move_flipd(struct ttm_buffer_object *bo, bool evict, bool intr,
 		      bool no_wait_gpu, struct ttm_mem_reg *new_mem)
 {
-	u32 placement_memtype = TTM_PL_FLAG_TT | TTM_PL_MASK_CACHING;
+	struct ttm_place placement_memtype = {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_FLAG_TT | TTM_PL_MASK_CACHING
+	};
 	struct ttm_placement placement;
 	struct ttm_mem_reg tmp_mem;
 	int ret;
 
-	placement.fpfn = placement.lpfn = 0;
 	placement.num_placement = placement.num_busy_placement = 1;
 	placement.placement = placement.busy_placement = &placement_memtype;
 
@@ -1074,12 +1087,15 @@ static int
 nouveau_bo_move_flips(struct ttm_buffer_object *bo, bool evict, bool intr,
 		      bool no_wait_gpu, struct ttm_mem_reg *new_mem)
 {
-	u32 placement_memtype = TTM_PL_FLAG_TT | TTM_PL_MASK_CACHING;
+	struct ttm_place placement_memtype = {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_FLAG_TT | TTM_PL_MASK_CACHING
+	};
 	struct ttm_placement placement;
 	struct ttm_mem_reg tmp_mem;
 	int ret;
 
-	placement.fpfn = placement.lpfn = 0;
 	placement.num_placement = placement.num_busy_placement = 1;
 	placement.placement = placement.busy_placement = &placement_memtype;
 
@@ -1152,8 +1168,9 @@ nouveau_bo_vm_cleanup(struct ttm_buffer_object *bo,
 {
 	struct nouveau_drm *drm = nouveau_bdev(bo->bdev);
 	struct drm_device *dev = drm->dev;
+	struct fence *fence = reservation_object_get_excl(bo->resv);
 
-	nv10_bo_put_tile_region(dev, *old_tile, bo->sync_obj);
+	nv10_bo_put_tile_region(dev, *old_tile, fence);
 	*old_tile = new_tile;
 }
 
@@ -1197,9 +1214,7 @@ nouveau_bo_move(struct ttm_buffer_object *bo, bool evict, bool intr,
 	}
 
 	/* Fallback to software copy. */
-	spin_lock(&bo->bdev->fence_lock);
 	ret = ttm_bo_wait(bo, true, intr, no_wait_gpu);
-	spin_unlock(&bo->bdev->fence_lock);
 	if (ret == 0)
 		ret = ttm_bo_move_memcpy(bo, evict, no_wait_gpu, new_mem);
 
@@ -1294,7 +1309,7 @@ nouveau_ttm_fault_reserve_notify(struct ttm_buffer_object *bo)
 	struct nouveau_bo *nvbo = nouveau_bo(bo);
 	struct nvif_device *device = &drm->device;
 	u32 mappable = nv_device_resource_len(nvkm_device(device), 1) >> PAGE_SHIFT;
-	int ret;
+	int i, ret;
 
 	/* as long as the bo isn't in vram, and isn't tiled, we've got
 	 * nothing to do here.
@@ -1319,9 +1334,16 @@ nouveau_ttm_fault_reserve_notify(struct ttm_buffer_object *bo)
 	    bo->mem.start + bo->mem.num_pages < mappable)
 		return 0;
 
+	for (i = 0; i < nvbo->placement.num_placement; ++i) {
+		nvbo->placements[i].fpfn = 0;
+		nvbo->placements[i].lpfn = mappable;
+	}
+
+	for (i = 0; i < nvbo->placement.num_busy_placement; ++i) {
+		nvbo->busy_placements[i].fpfn = 0;
+		nvbo->busy_placements[i].lpfn = mappable;
+	}
 
-	nvbo->placement.fpfn = 0;
-	nvbo->placement.lpfn = mappable;
 	nouveau_bo_placement_set(nvbo, TTM_PL_FLAG_VRAM, 0);
 	return nouveau_bo_validate(nvbo, false, false);
 }
@@ -1436,47 +1458,14 @@ nouveau_ttm_tt_unpopulate(struct ttm_tt *ttm)
 }
 
 void
-nouveau_bo_fence(struct nouveau_bo *nvbo, struct nouveau_fence *fence)
-{
-	struct nouveau_fence *new_fence = nouveau_fence_ref(fence);
-	struct nouveau_fence *old_fence = NULL;
-
-	spin_lock(&nvbo->bo.bdev->fence_lock);
-	old_fence = nvbo->bo.sync_obj;
-	nvbo->bo.sync_obj = new_fence;
-	spin_unlock(&nvbo->bo.bdev->fence_lock);
-
-	nouveau_fence_unref(&old_fence);
-}
-
-static void
-nouveau_bo_fence_unref(void **sync_obj)
-{
-	nouveau_fence_unref((struct nouveau_fence **)sync_obj);
-}
-
-static void *
-nouveau_bo_fence_ref(void *sync_obj)
+nouveau_bo_fence(struct nouveau_bo *nvbo, struct nouveau_fence *fence, bool exclusive)
 {
-	return nouveau_fence_ref(sync_obj);
-}
+	struct reservation_object *resv = nvbo->bo.resv;
 
-static bool
-nouveau_bo_fence_signalled(void *sync_obj)
-{
-	return nouveau_fence_done(sync_obj);
-}
-
-static int
-nouveau_bo_fence_wait(void *sync_obj, bool lazy, bool intr)
-{
-	return nouveau_fence_wait(sync_obj, lazy, intr);
-}
-
-static int
-nouveau_bo_fence_flush(void *sync_obj)
-{
-	return 0;
+	if (exclusive)
+		reservation_object_add_excl_fence(resv, &fence->base);
+	else if (fence)
+		reservation_object_add_shared_fence(resv, &fence->base);
 }
 
 struct ttm_bo_driver nouveau_bo_driver = {
@@ -1489,11 +1478,6 @@ struct ttm_bo_driver nouveau_bo_driver = {
 	.move_notify = nouveau_bo_move_ntfy,
 	.move = nouveau_bo_move,
 	.verify_access = nouveau_bo_verify_access,
-	.sync_obj_signaled = nouveau_bo_fence_signalled,
-	.sync_obj_wait = nouveau_bo_fence_wait,
-	.sync_obj_flush = nouveau_bo_fence_flush,
-	.sync_obj_unref = nouveau_bo_fence_unref,
-	.sync_obj_ref = nouveau_bo_fence_ref,
 	.fault_reserve_notify = &nouveau_ttm_fault_reserve_notify,
 	.io_mem_reserve = &nouveau_ttm_io_mem_reserve,
 	.io_mem_free = &nouveau_ttm_io_mem_free,
diff --git a/drivers/gpu/drm/nouveau/nouveau_bo.h b/drivers/gpu/drm/nouveau/nouveau_bo.h
index ff17c1f432fc..22d2c764d80b 100644
--- a/drivers/gpu/drm/nouveau/nouveau_bo.h
+++ b/drivers/gpu/drm/nouveau/nouveau_bo.h
@@ -1,6 +1,8 @@
 #ifndef __NOUVEAU_BO_H__
 #define __NOUVEAU_BO_H__
 
+#include <drm/drm_gem.h>
+
 struct nouveau_channel;
 struct nouveau_fence;
 struct nouveau_vma;
@@ -9,8 +11,8 @@ struct nouveau_bo {
 	struct ttm_buffer_object bo;
 	struct ttm_placement placement;
 	u32 valid_domains;
-	u32 placements[3];
-	u32 busy_placements[3];
+	struct ttm_place placements[3];
+	struct ttm_place busy_placements[3];
 	struct ttm_bo_kmap_obj kmap;
 	struct list_head head;
 
@@ -68,6 +70,7 @@ extern struct ttm_bo_driver nouveau_bo_driver;
 void nouveau_bo_move_init(struct nouveau_drm *);
 int  nouveau_bo_new(struct drm_device *, int size, int align, u32 flags,
 		    u32 tile_mode, u32 tile_flags, struct sg_table *sg,
+		    struct reservation_object *robj,
 		    struct nouveau_bo **);
 int  nouveau_bo_pin(struct nouveau_bo *, u32 flags);
 int  nouveau_bo_unpin(struct nouveau_bo *);
@@ -78,7 +81,7 @@ u16  nouveau_bo_rd16(struct nouveau_bo *, unsigned index);
 void nouveau_bo_wr16(struct nouveau_bo *, unsigned index, u16 val);
 u32  nouveau_bo_rd32(struct nouveau_bo *, unsigned index);
 void nouveau_bo_wr32(struct nouveau_bo *, unsigned index, u32 val);
-void nouveau_bo_fence(struct nouveau_bo *, struct nouveau_fence *);
+void nouveau_bo_fence(struct nouveau_bo *, struct nouveau_fence *, bool exclusive);
 int  nouveau_bo_validate(struct nouveau_bo *, bool interruptible,
 			 bool no_wait_gpu);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_chan.c b/drivers/gpu/drm/nouveau/nouveau_chan.c
index 99cd9e4a2aa6..fd3dbd59d73e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_chan.c
+++ b/drivers/gpu/drm/nouveau/nouveau_chan.c
@@ -36,7 +36,7 @@
 #include "nouveau_abi16.h"
 
 MODULE_PARM_DESC(vram_pushbuf, "Create DMA push buffers in VRAM");
-static int nouveau_vram_pushbuf;
+int nouveau_vram_pushbuf;
 module_param_named(vram_pushbuf, nouveau_vram_pushbuf, int, 0400);
 
 int
@@ -106,7 +106,7 @@ nouveau_channel_prep(struct nouveau_drm *drm, struct nvif_device *device,
 	if (nouveau_vram_pushbuf)
 		target = TTM_PL_FLAG_VRAM;
 
-	ret = nouveau_bo_new(drm->dev, size, 0, target, 0, 0, NULL,
+	ret = nouveau_bo_new(drm->dev, size, 0, target, 0, 0, NULL, NULL,
 			    &chan->push.buffer);
 	if (ret == 0) {
 		ret = nouveau_bo_pin(chan->push.buffer, target);
@@ -285,6 +285,7 @@ nouveau_channel_init(struct nouveau_channel *chan, u32 vram, u32 gart)
 	struct nouveau_software_chan *swch;
 	struct nv_dma_v0 args = {};
 	int ret, i;
+	bool save;
 
 	nvif_object_map(chan->object);
 
@@ -386,7 +387,11 @@ nouveau_channel_init(struct nouveau_channel *chan, u32 vram, u32 gart)
 	}
 
 	/* initialise synchronisation */
-	return nouveau_fence(chan->drm)->context_new(chan);
+	save = cli->base.super;
+	cli->base.super = true; /* hack until fencenv50 fixed */
+	ret = nouveau_fence(chan->drm)->context_new(chan);
+	cli->base.super = save;
+	return ret;
 }
 
 int
@@ -395,15 +400,20 @@ nouveau_channel_new(struct nouveau_drm *drm, struct nvif_device *device,
 		    struct nouveau_channel **pchan)
 {
 	struct nouveau_cli *cli = (void *)nvif_client(&device->base);
+	bool super;
 	int ret;
 
+	/* hack until fencenv50 is fixed, and agp access relaxed */
+	super = cli->base.super;
+	cli->base.super = true;
+
 	ret = nouveau_channel_ind(drm, device, handle, arg0, pchan);
 	if (ret) {
 		NV_PRINTK(debug, cli, "ib channel create, %d\n", ret);
 		ret = nouveau_channel_dma(drm, device, handle, pchan);
 		if (ret) {
 			NV_PRINTK(debug, cli, "dma channel create, %d\n", ret);
-			return ret;
+			goto done;
 		}
 	}
 
@@ -411,8 +421,9 @@ nouveau_channel_new(struct nouveau_drm *drm, struct nvif_device *device,
 	if (ret) {
 		NV_PRINTK(error, cli, "channel failed to initialise, %d\n", ret);
 		nouveau_channel_del(pchan);
-		return ret;
 	}
 
-	return 0;
+done:
+	cli->base.super = super;
+	return ret;
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_chan.h b/drivers/gpu/drm/nouveau/nouveau_chan.h
index 20163709d608..8309c24ee698 100644
--- a/drivers/gpu/drm/nouveau/nouveau_chan.h
+++ b/drivers/gpu/drm/nouveau/nouveau_chan.h
@@ -47,4 +47,6 @@ int  nouveau_channel_new(struct nouveau_drm *, struct nvif_device *,
 void nouveau_channel_del(struct nouveau_channel **);
 int  nouveau_channel_idle(struct nouveau_channel *);
 
+extern int nouveau_vram_pushbuf;
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.c b/drivers/gpu/drm/nouveau/nouveau_connector.c
index 1ec44c83e919..c8ac9482cf2e 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.c
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.c
@@ -45,15 +45,15 @@
 #include <nvif/event.h>
 
 MODULE_PARM_DESC(tv_disable, "Disable TV-out detection");
-static int nouveau_tv_disable = 0;
+int nouveau_tv_disable = 0;
 module_param_named(tv_disable, nouveau_tv_disable, int, 0400);
 
 MODULE_PARM_DESC(ignorelid, "Ignore ACPI lid status");
-static int nouveau_ignorelid = 0;
+int nouveau_ignorelid = 0;
 module_param_named(ignorelid, nouveau_ignorelid, int, 0400);
 
 MODULE_PARM_DESC(duallink, "Allow dual-link TMDS (default: enabled)");
-static int nouveau_duallink = 1;
+int nouveau_duallink = 1;
 module_param_named(duallink, nouveau_duallink, int, 0400);
 
 struct nouveau_encoder *
diff --git a/drivers/gpu/drm/nouveau/nouveau_connector.h b/drivers/gpu/drm/nouveau/nouveau_connector.h
index 68029d041dd2..629a380c7085 100644
--- a/drivers/gpu/drm/nouveau/nouveau_connector.h
+++ b/drivers/gpu/drm/nouveau/nouveau_connector.h
@@ -105,4 +105,8 @@ nouveau_crtc_connector_get(struct nouveau_crtc *nv_crtc)
 struct drm_connector *
 nouveau_connector_create(struct drm_device *, int index);
 
+extern int nouveau_tv_disable;
+extern int nouveau_ignorelid;
+extern int nouveau_duallink;
+
 #endif /* __NOUVEAU_CONNECTOR_H__ */
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.c b/drivers/gpu/drm/nouveau/nouveau_display.c
index 65b4fd53dd4e..a88e6927f571 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.c
+++ b/drivers/gpu/drm/nouveau/nouveau_display.c
@@ -126,7 +126,7 @@ nouveau_display_scanoutpos_head(struct drm_crtc *crtc, int *vpos, int *hpos,
 	if (etime) *etime = ns_to_ktime(args.scan.time[1]);
 
 	if (*vpos < 0)
-		ret |= DRM_SCANOUTPOS_INVBL;
+		ret |= DRM_SCANOUTPOS_IN_VBLANK;
 	return ret;
 }
 
@@ -550,14 +550,12 @@ nouveau_display_destroy(struct drm_device *dev)
 }
 
 int
-nouveau_display_suspend(struct drm_device *dev)
+nouveau_display_suspend(struct drm_device *dev, bool runtime)
 {
-	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct drm_crtc *crtc;
 
 	nouveau_display_fini(dev);
 
-	NV_INFO(drm, "unpinning framebuffer(s)...\n");
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		struct nouveau_framebuffer *nouveau_fb;
 
@@ -579,12 +577,13 @@ nouveau_display_suspend(struct drm_device *dev)
 }
 
 void
-nouveau_display_repin(struct drm_device *dev)
+nouveau_display_resume(struct drm_device *dev, bool runtime)
 {
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct drm_crtc *crtc;
-	int ret;
+	int ret, head;
 
+	/* re-pin fb/cursors */
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
 		struct nouveau_framebuffer *nouveau_fb;
 
@@ -606,13 +605,6 @@ nouveau_display_repin(struct drm_device *dev)
 		if (ret)
 			NV_ERROR(drm, "Could not pin/map cursor.\n");
 	}
-}
-
-void
-nouveau_display_resume(struct drm_device *dev)
-{
-	struct drm_crtc *crtc;
-	int head;
 
 	nouveau_display_init(dev);
 
@@ -627,6 +619,13 @@ nouveau_display_resume(struct drm_device *dev)
 	for (head = 0; head < dev->mode_config.num_crtc; head++)
 		drm_vblank_on(dev, head);
 
+	/* This should ensure we don't hit a locking problem when someone
+	 * wakes us up via a connector.  We should never go into suspend
+	 * while the display is on anyways.
+	 */
+	if (runtime)
+		return;
+
 	drm_helper_resume_force_mode(dev);
 
 	list_for_each_entry(crtc, &dev->mode_config.crtc_list, head) {
@@ -658,7 +657,7 @@ nouveau_page_flip_emit(struct nouveau_channel *chan,
 	spin_unlock_irqrestore(&dev->event_lock, flags);
 
 	/* Synchronize with the old framebuffer */
-	ret = nouveau_fence_sync(old_bo->bo.sync_obj, chan);
+	ret = nouveau_fence_sync(old_bo, chan, false, false);
 	if (ret)
 		goto fail;
 
@@ -717,19 +716,24 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	}
 
 	mutex_lock(&cli->mutex);
-
-	/* synchronise rendering channel with the kernel's channel */
-	spin_lock(&new_bo->bo.bdev->fence_lock);
-	fence = nouveau_fence_ref(new_bo->bo.sync_obj);
-	spin_unlock(&new_bo->bo.bdev->fence_lock);
-	ret = nouveau_fence_sync(fence, chan);
-	nouveau_fence_unref(&fence);
+	ret = ttm_bo_reserve(&new_bo->bo, true, false, false, NULL);
 	if (ret)
 		goto fail_unpin;
 
-	ret = ttm_bo_reserve(&old_bo->bo, true, false, false, NULL);
-	if (ret)
+	/* synchronise rendering channel with the kernel's channel */
+	ret = nouveau_fence_sync(new_bo, chan, false, true);
+	if (ret) {
+		ttm_bo_unreserve(&new_bo->bo);
 		goto fail_unpin;
+	}
+
+	if (new_bo != old_bo) {
+		ttm_bo_unreserve(&new_bo->bo);
+
+		ret = ttm_bo_reserve(&old_bo->bo, true, false, false, NULL);
+		if (ret)
+			goto fail_unpin;
+	}
 
 	/* Initialize a page flip struct */
 	*s = (struct nouveau_page_flip_state)
@@ -775,7 +779,7 @@ nouveau_crtc_page_flip(struct drm_crtc *crtc, struct drm_framebuffer *fb,
 	/* Update the crtc struct and cleanup */
 	crtc->primary->fb = fb;
 
-	nouveau_bo_fence(old_bo, fence);
+	nouveau_bo_fence(old_bo, fence, false);
 	ttm_bo_unreserve(&old_bo->bo);
 	if (old_bo != new_bo)
 		nouveau_bo_unpin(old_bo);
diff --git a/drivers/gpu/drm/nouveau/nouveau_display.h b/drivers/gpu/drm/nouveau/nouveau_display.h
index 88ca177cb1c7..be3d5947c6be 100644
--- a/drivers/gpu/drm/nouveau/nouveau_display.h
+++ b/drivers/gpu/drm/nouveau/nouveau_display.h
@@ -63,9 +63,8 @@ int  nouveau_display_create(struct drm_device *dev);
 void nouveau_display_destroy(struct drm_device *dev);
 int  nouveau_display_init(struct drm_device *dev);
 void nouveau_display_fini(struct drm_device *dev);
-int  nouveau_display_suspend(struct drm_device *dev);
-void nouveau_display_repin(struct drm_device *dev);
-void nouveau_display_resume(struct drm_device *dev);
+int  nouveau_display_suspend(struct drm_device *dev, bool runtime);
+void nouveau_display_resume(struct drm_device *dev, bool runtime);
 int  nouveau_display_vblank_enable(struct drm_device *, int);
 void nouveau_display_vblank_disable(struct drm_device *, int);
 int  nouveau_display_scanoutpos(struct drm_device *, int, unsigned int,
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.c b/drivers/gpu/drm/nouveau/nouveau_drm.c
index 250a5e88c751..57238076049f 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.c
@@ -51,6 +51,7 @@
 #include "nouveau_fence.h"
 #include "nouveau_debugfs.h"
 #include "nouveau_usif.h"
+#include "nouveau_connector.h"
 
 MODULE_PARM_DESC(config, "option string to pass to driver core");
 static char *nouveau_config;
@@ -73,7 +74,9 @@ MODULE_PARM_DESC(runpm, "disable (0), force enable (1), optimus only default (-1
 int nouveau_runtime_pm = -1;
 module_param_named(runpm, nouveau_runtime_pm, int, 0400);
 
-static struct drm_driver driver;
+static struct drm_driver driver_stub;
+static struct drm_driver driver_pci;
+static struct drm_driver driver_platform;
 
 static u64
 nouveau_pci_name(struct pci_dev *pdev)
@@ -322,7 +325,7 @@ static int nouveau_drm_probe(struct pci_dev *pdev,
 
 	pci_set_master(pdev);
 
-	ret = drm_get_pci_dev(pdev, pent, &driver);
+	ret = drm_get_pci_dev(pdev, pent, &driver_pci);
 	if (ret) {
 		nouveau_object_ref(NULL, (struct nouveau_object **)&device);
 		return ret;
@@ -547,9 +550,11 @@ nouveau_do_suspend(struct drm_device *dev, bool runtime)
 	struct nouveau_cli *cli;
 	int ret;
 
-	if (dev->mode_config.num_crtc && !runtime) {
+	if (dev->mode_config.num_crtc) {
+		NV_INFO(drm, "suspending console...\n");
+		nouveau_fbcon_set_suspend(dev, 1);
 		NV_INFO(drm, "suspending display...\n");
-		ret = nouveau_display_suspend(dev);
+		ret = nouveau_display_suspend(dev, runtime);
 		if (ret)
 			return ret;
 	}
@@ -603,7 +608,7 @@ fail_client:
 fail_display:
 	if (dev->mode_config.num_crtc) {
 		NV_INFO(drm, "resuming display...\n");
-		nouveau_display_resume(dev);
+		nouveau_display_resume(dev, runtime);
 	}
 	return ret;
 }
@@ -618,21 +623,19 @@ int nouveau_pmops_suspend(struct device *dev)
 	    drm_dev->switch_power_state == DRM_SWITCH_POWER_DYNAMIC_OFF)
 		return 0;
 
-	if (drm_dev->mode_config.num_crtc)
-		nouveau_fbcon_set_suspend(drm_dev, 1);
-
 	ret = nouveau_do_suspend(drm_dev, false);
 	if (ret)
 		return ret;
 
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
+	pci_ignore_hotplug(pdev);
 	pci_set_power_state(pdev, PCI_D3hot);
 	return 0;
 }
 
 static int
-nouveau_do_resume(struct drm_device *dev)
+nouveau_do_resume(struct drm_device *dev, bool runtime)
 {
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct nouveau_cli *cli;
@@ -657,7 +660,9 @@ nouveau_do_resume(struct drm_device *dev)
 
 	if (dev->mode_config.num_crtc) {
 		NV_INFO(drm, "resuming display...\n");
-		nouveau_display_repin(dev);
+		nouveau_display_resume(dev, runtime);
+		NV_INFO(drm, "resuming console...\n");
+		nouveau_fbcon_set_suspend(dev, 0);
 	}
 
 	return 0;
@@ -680,47 +685,21 @@ int nouveau_pmops_resume(struct device *dev)
 		return ret;
 	pci_set_master(pdev);
 
-	ret = nouveau_do_resume(drm_dev);
-	if (ret)
-		return ret;
-
-	if (drm_dev->mode_config.num_crtc) {
-		nouveau_display_resume(drm_dev);
-		nouveau_fbcon_set_suspend(drm_dev, 0);
-	}
-
-	return 0;
+	return nouveau_do_resume(drm_dev, false);
 }
 
 static int nouveau_pmops_freeze(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	int ret;
-
-	if (drm_dev->mode_config.num_crtc)
-		nouveau_fbcon_set_suspend(drm_dev, 1);
-
-	ret = nouveau_do_suspend(drm_dev, false);
-	return ret;
+	return nouveau_do_suspend(drm_dev, false);
 }
 
 static int nouveau_pmops_thaw(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct drm_device *drm_dev = pci_get_drvdata(pdev);
-	int ret;
-
-	ret = nouveau_do_resume(drm_dev);
-	if (ret)
-		return ret;
-
-	if (drm_dev->mode_config.num_crtc) {
-		nouveau_display_resume(drm_dev);
-		nouveau_fbcon_set_suspend(drm_dev, 0);
-	}
-
-	return 0;
+	return nouveau_do_resume(drm_dev, false);
 }
 
 
@@ -855,7 +834,7 @@ nouveau_driver_fops = {
 };
 
 static struct drm_driver
-driver = {
+driver_stub = {
 	.driver_features =
 		DRIVER_USE_AGP |
 		DRIVER_GEM | DRIVER_MODESET | DRIVER_PRIME | DRIVER_RENDER,
@@ -976,7 +955,7 @@ static int nouveau_pmops_runtime_resume(struct device *dev)
 		return ret;
 	pci_set_master(pdev);
 
-	ret = nouveau_do_resume(drm_dev);
+	ret = nouveau_do_resume(drm_dev, true);
 	drm_kms_helper_poll_enable(drm_dev);
 	/* do magic */
 	nvif_mask(device, 0x88488, (1 << 25), (1 << 25));
@@ -1026,6 +1005,23 @@ static int nouveau_pmops_runtime_idle(struct device *dev)
 	return 1;
 }
 
+static void nouveau_display_options(void)
+{
+	DRM_DEBUG_DRIVER("Loading Nouveau with parameters:\n");
+
+	DRM_DEBUG_DRIVER("... tv_disable   : %d\n", nouveau_tv_disable);
+	DRM_DEBUG_DRIVER("... ignorelid    : %d\n", nouveau_ignorelid);
+	DRM_DEBUG_DRIVER("... duallink     : %d\n", nouveau_duallink);
+	DRM_DEBUG_DRIVER("... nofbaccel    : %d\n", nouveau_nofbaccel);
+	DRM_DEBUG_DRIVER("... config       : %s\n", nouveau_config);
+	DRM_DEBUG_DRIVER("... debug        : %s\n", nouveau_debug);
+	DRM_DEBUG_DRIVER("... noaccel      : %d\n", nouveau_noaccel);
+	DRM_DEBUG_DRIVER("... modeset      : %d\n", nouveau_modeset);
+	DRM_DEBUG_DRIVER("... runpm        : %d\n", nouveau_runtime_pm);
+	DRM_DEBUG_DRIVER("... vram_pushbuf : %d\n", nouveau_vram_pushbuf);
+	DRM_DEBUG_DRIVER("... pstate       : %d\n", nouveau_pstate);
+}
+
 static const struct dev_pm_ops nouveau_pm_ops = {
 	.suspend = nouveau_pmops_suspend,
 	.resume = nouveau_pmops_resume,
@@ -1061,7 +1057,7 @@ nouveau_platform_device_create_(struct platform_device *pdev, int size,
 	if (err)
 		return ERR_PTR(err);
 
-	drm = drm_dev_alloc(&driver, &pdev->dev);
+	drm = drm_dev_alloc(&driver_platform, &pdev->dev);
 	if (!drm) {
 		err = -ENOMEM;
 		goto err_free;
@@ -1086,6 +1082,13 @@ EXPORT_SYMBOL(nouveau_platform_device_create_);
 static int __init
 nouveau_drm_init(void)
 {
+	driver_pci = driver_stub;
+	driver_pci.set_busid = drm_pci_set_busid;
+	driver_platform = driver_stub;
+	driver_platform.set_busid = drm_platform_set_busid;
+
+	nouveau_display_options();
+
 	if (nouveau_modeset == -1) {
 #ifdef CONFIG_VGA_CONSOLE
 		if (vgacon_text_force())
@@ -1097,7 +1100,7 @@ nouveau_drm_init(void)
 		return 0;
 
 	nouveau_register_dsm_handler();
-	return drm_pci_init(&driver, &nouveau_drm_pci_driver);
+	return drm_pci_init(&driver_pci, &nouveau_drm_pci_driver);
 }
 
 static void __exit
@@ -1106,7 +1109,7 @@ nouveau_drm_exit(void)
 	if (!nouveau_modeset)
 		return;
 
-	drm_pci_exit(&driver, &nouveau_drm_pci_driver);
+	drm_pci_exit(&driver_pci, &nouveau_drm_pci_driver);
 	nouveau_unregister_dsm_handler();
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_drm.h b/drivers/gpu/drm/nouveau/nouveau_drm.h
index b02b02452c85..8ae36f265fb8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_drm.h
+++ b/drivers/gpu/drm/nouveau/nouveau_drm.h
@@ -10,7 +10,7 @@
 
 #define DRIVER_MAJOR		1
 #define DRIVER_MINOR		2
-#define DRIVER_PATCHLEVEL	0
+#define DRIVER_PATCHLEVEL	1
 
 /*
  * 1.1.1:
@@ -26,6 +26,8 @@
  * 1.2.0:
  * 	- object api exposed to userspace
  * 	- fermi,kepler,maxwell zbc
+ * 1.2.1:
+ *      - allow concurrent access to bo's mapped read/write.
  */
 
 #include <nvif/client.h>
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.c b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
index 8bdd27091db8..593ef8a2a069 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.c
@@ -52,7 +52,7 @@
 #include "nouveau_crtc.h"
 
 MODULE_PARM_DESC(nofbaccel, "Disable fbcon acceleration");
-static int nouveau_nofbaccel = 0;
+int nouveau_nofbaccel = 0;
 module_param_named(nofbaccel, nouveau_nofbaccel, int, 0400);
 
 static void
@@ -308,7 +308,8 @@ static int
 nouveau_fbcon_create(struct drm_fb_helper *helper,
 		     struct drm_fb_helper_surface_size *sizes)
 {
-	struct nouveau_fbdev *fbcon = (struct nouveau_fbdev *)helper;
+	struct nouveau_fbdev *fbcon =
+		container_of(helper, struct nouveau_fbdev, helper);
 	struct drm_device *dev = fbcon->dev;
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	struct nvif_device *device = &drm->device;
@@ -486,6 +487,16 @@ static const struct drm_fb_helper_funcs nouveau_fbcon_helper_funcs = {
 	.fb_probe = nouveau_fbcon_create,
 };
 
+static void
+nouveau_fbcon_set_suspend_work(struct work_struct *work)
+{
+	struct nouveau_fbdev *fbcon = container_of(work, typeof(*fbcon), work);
+	console_lock();
+	nouveau_fbcon_accel_restore(fbcon->dev);
+	nouveau_fbcon_zfill(fbcon->dev, fbcon);
+	fb_set_suspend(fbcon->helper.fbdev, FBINFO_STATE_RUNNING);
+	console_unlock();
+}
 
 int
 nouveau_fbcon_init(struct drm_device *dev)
@@ -503,6 +514,7 @@ nouveau_fbcon_init(struct drm_device *dev)
 	if (!fbcon)
 		return -ENOMEM;
 
+	INIT_WORK(&fbcon->work, nouveau_fbcon_set_suspend_work);
 	fbcon->dev = dev;
 	drm->fbcon = fbcon;
 
@@ -551,14 +563,14 @@ nouveau_fbcon_set_suspend(struct drm_device *dev, int state)
 {
 	struct nouveau_drm *drm = nouveau_drm(dev);
 	if (drm->fbcon) {
-		console_lock();
-		if (state == 0) {
-			nouveau_fbcon_accel_restore(dev);
-			nouveau_fbcon_zfill(dev, drm->fbcon);
+		if (state == FBINFO_STATE_RUNNING) {
+			schedule_work(&drm->fbcon->work);
+			return;
 		}
+		flush_work(&drm->fbcon->work);
+		console_lock();
 		fb_set_suspend(drm->fbcon->helper.fbdev, state);
-		if (state == 1)
-			nouveau_fbcon_accel_save_disable(dev);
+		nouveau_fbcon_accel_save_disable(dev);
 		console_unlock();
 	}
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_fbcon.h b/drivers/gpu/drm/nouveau/nouveau_fbcon.h
index 34658cfa8f5d..6208e70e4a1c 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fbcon.h
+++ b/drivers/gpu/drm/nouveau/nouveau_fbcon.h
@@ -36,6 +36,7 @@ struct nouveau_fbdev {
 	struct nouveau_framebuffer nouveau_fb;
 	struct list_head fbdev_list;
 	struct drm_device *dev;
+	struct work_struct work;
 	unsigned int saved_flags;
 	struct nvif_object surf2d;
 	struct nvif_object clip;
@@ -72,5 +73,8 @@ void nouveau_fbcon_accel_save_disable(struct drm_device *dev);
 void nouveau_fbcon_accel_restore(struct drm_device *dev);
 
 void nouveau_fbcon_output_poll_changed(struct drm_device *dev);
+
+extern int nouveau_nofbaccel;
+
 #endif /* __NV50_FBCON_H__ */
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.c b/drivers/gpu/drm/nouveau/nouveau_fence.c
index 0a93114158cd..515cd9aebb99 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fence.c
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.c
@@ -28,6 +28,7 @@
 
 #include <linux/ktime.h>
 #include <linux/hrtimer.h>
+#include <trace/events/fence.h>
 
 #include <nvif/notify.h>
 #include <nvif/event.h>
@@ -36,123 +37,234 @@
 #include "nouveau_dma.h"
 #include "nouveau_fence.h"
 
-struct fence_work {
-	struct work_struct base;
-	struct list_head head;
-	void (*func)(void *);
-	void *data;
-};
+static const struct fence_ops nouveau_fence_ops_uevent;
+static const struct fence_ops nouveau_fence_ops_legacy;
+
+static inline struct nouveau_fence *
+from_fence(struct fence *fence)
+{
+	return container_of(fence, struct nouveau_fence, base);
+}
+
+static inline struct nouveau_fence_chan *
+nouveau_fctx(struct nouveau_fence *fence)
+{
+	return container_of(fence->base.lock, struct nouveau_fence_chan, lock);
+}
 
 static void
 nouveau_fence_signal(struct nouveau_fence *fence)
 {
-	struct fence_work *work, *temp;
+	fence_signal_locked(&fence->base);
+	list_del(&fence->head);
+
+	if (test_bit(FENCE_FLAG_USER_BITS, &fence->base.flags)) {
+		struct nouveau_fence_chan *fctx = nouveau_fctx(fence);
 
-	list_for_each_entry_safe(work, temp, &fence->work, head) {
-		schedule_work(&work->base);
-		list_del(&work->head);
+		if (!--fctx->notify_ref)
+			nvif_notify_put(&fctx->notify);
 	}
 
-	fence->channel = NULL;
-	list_del(&fence->head);
+	fence_put(&fence->base);
+}
+
+static struct nouveau_fence *
+nouveau_local_fence(struct fence *fence, struct nouveau_drm *drm) {
+	struct nouveau_fence_priv *priv = (void*)drm->fence;
+
+	if (fence->ops != &nouveau_fence_ops_legacy &&
+	    fence->ops != &nouveau_fence_ops_uevent)
+		return NULL;
+
+	if (fence->context < priv->context_base ||
+	    fence->context >= priv->context_base + priv->contexts)
+		return NULL;
+
+	return from_fence(fence);
 }
 
 void
 nouveau_fence_context_del(struct nouveau_fence_chan *fctx)
 {
-	struct nouveau_fence *fence, *fnext;
-	spin_lock(&fctx->lock);
-	list_for_each_entry_safe(fence, fnext, &fctx->pending, head) {
+	struct nouveau_fence *fence;
+
+	nvif_notify_fini(&fctx->notify);
+
+	spin_lock_irq(&fctx->lock);
+	while (!list_empty(&fctx->pending)) {
+		fence = list_entry(fctx->pending.next, typeof(*fence), head);
+
 		nouveau_fence_signal(fence);
+		fence->channel = NULL;
 	}
-	spin_unlock(&fctx->lock);
+	spin_unlock_irq(&fctx->lock);
+}
+
+static void
+nouveau_fence_context_put(struct kref *fence_ref)
+{
+	kfree(container_of(fence_ref, struct nouveau_fence_chan, fence_ref));
 }
 
 void
-nouveau_fence_context_new(struct nouveau_fence_chan *fctx)
+nouveau_fence_context_free(struct nouveau_fence_chan *fctx)
+{
+	kref_put(&fctx->fence_ref, nouveau_fence_context_put);
+}
+
+static void
+nouveau_fence_update(struct nouveau_channel *chan, struct nouveau_fence_chan *fctx)
+{
+	struct nouveau_fence *fence;
+
+	u32 seq = fctx->read(chan);
+
+	while (!list_empty(&fctx->pending)) {
+		fence = list_entry(fctx->pending.next, typeof(*fence), head);
+
+		if ((int)(seq - fence->base.seqno) < 0)
+			return;
+
+		nouveau_fence_signal(fence);
+	}
+}
+
+static int
+nouveau_fence_wait_uevent_handler(struct nvif_notify *notify)
 {
+	struct nouveau_fence_chan *fctx =
+		container_of(notify, typeof(*fctx), notify);
+	unsigned long flags;
+
+	spin_lock_irqsave(&fctx->lock, flags);
+	if (!list_empty(&fctx->pending)) {
+		struct nouveau_fence *fence;
+
+		fence = list_entry(fctx->pending.next, typeof(*fence), head);
+		nouveau_fence_update(fence->channel, fctx);
+	}
+	spin_unlock_irqrestore(&fctx->lock, flags);
+
+	/* Always return keep here. NVIF refcount is handled with nouveau_fence_update */
+	return NVIF_NOTIFY_KEEP;
+}
+
+void
+nouveau_fence_context_new(struct nouveau_channel *chan, struct nouveau_fence_chan *fctx)
+{
+	struct nouveau_fence_priv *priv = (void*)chan->drm->fence;
+	struct nouveau_cli *cli = (void *)nvif_client(chan->object);
+	int ret;
+
 	INIT_LIST_HEAD(&fctx->flip);
 	INIT_LIST_HEAD(&fctx->pending);
 	spin_lock_init(&fctx->lock);
+	fctx->context = priv->context_base + chan->chid;
+
+	if (chan == chan->drm->cechan)
+		strcpy(fctx->name, "copy engine channel");
+	else if (chan == chan->drm->channel)
+		strcpy(fctx->name, "generic kernel channel");
+	else
+		strcpy(fctx->name, nvkm_client(&cli->base)->name);
+
+	kref_init(&fctx->fence_ref);
+	if (!priv->uevent)
+		return;
+
+	ret = nvif_notify_init(chan->object, NULL,
+			 nouveau_fence_wait_uevent_handler, false,
+			 G82_CHANNEL_DMA_V0_NTFY_UEVENT,
+			 &(struct nvif_notify_uevent_req) { },
+			 sizeof(struct nvif_notify_uevent_req),
+			 sizeof(struct nvif_notify_uevent_rep),
+			 &fctx->notify);
+
+	WARN_ON(ret);
 }
 
+struct nouveau_fence_work {
+	struct work_struct work;
+	struct fence_cb cb;
+	void (*func)(void *);
+	void *data;
+};
+
 static void
 nouveau_fence_work_handler(struct work_struct *kwork)
 {
-	struct fence_work *work = container_of(kwork, typeof(*work), base);
+	struct nouveau_fence_work *work = container_of(kwork, typeof(*work), work);
 	work->func(work->data);
 	kfree(work);
 }
 
+static void nouveau_fence_work_cb(struct fence *fence, struct fence_cb *cb)
+{
+	struct nouveau_fence_work *work = container_of(cb, typeof(*work), cb);
+
+	schedule_work(&work->work);
+}
+
 void
-nouveau_fence_work(struct nouveau_fence *fence,
+nouveau_fence_work(struct fence *fence,
 		   void (*func)(void *), void *data)
 {
-	struct nouveau_channel *chan = fence->channel;
-	struct nouveau_fence_chan *fctx;
-	struct fence_work *work = NULL;
+	struct nouveau_fence_work *work;
 
-	if (nouveau_fence_done(fence)) {
-		func(data);
-		return;
-	}
+	if (fence_is_signaled(fence))
+		goto err;
 
-	fctx = chan->fence;
 	work = kmalloc(sizeof(*work), GFP_KERNEL);
 	if (!work) {
-		WARN_ON(nouveau_fence_wait(fence, false, false));
-		func(data);
-		return;
-	}
-
-	spin_lock(&fctx->lock);
-	if (!fence->channel) {
-		spin_unlock(&fctx->lock);
-		kfree(work);
-		func(data);
-		return;
+		/*
+		 * this might not be a nouveau fence any more,
+		 * so force a lazy wait here
+		 */
+		WARN_ON(nouveau_fence_wait((struct nouveau_fence *)fence,
+					   true, false));
+		goto err;
 	}
 
-	INIT_WORK(&work->base, nouveau_fence_work_handler);
+	INIT_WORK(&work->work, nouveau_fence_work_handler);
 	work->func = func;
 	work->data = data;
-	list_add(&work->head, &fence->work);
-	spin_unlock(&fctx->lock);
-}
-
-static void
-nouveau_fence_update(struct nouveau_channel *chan)
-{
-	struct nouveau_fence_chan *fctx = chan->fence;
-	struct nouveau_fence *fence, *fnext;
 
-	spin_lock(&fctx->lock);
-	list_for_each_entry_safe(fence, fnext, &fctx->pending, head) {
-		if (fctx->read(chan) < fence->sequence)
-			break;
+	if (fence_add_callback(fence, &work->cb, nouveau_fence_work_cb) < 0)
+		goto err_free;
+	return;
 
-		nouveau_fence_signal(fence);
-		nouveau_fence_unref(&fence);
-	}
-	spin_unlock(&fctx->lock);
+err_free:
+	kfree(work);
+err:
+	func(data);
 }
 
 int
 nouveau_fence_emit(struct nouveau_fence *fence, struct nouveau_channel *chan)
 {
 	struct nouveau_fence_chan *fctx = chan->fence;
+	struct nouveau_fence_priv *priv = (void*)chan->drm->fence;
 	int ret;
 
 	fence->channel  = chan;
 	fence->timeout  = jiffies + (15 * HZ);
-	fence->sequence = ++fctx->sequence;
 
+	if (priv->uevent)
+		fence_init(&fence->base, &nouveau_fence_ops_uevent,
+			   &fctx->lock, fctx->context, ++fctx->sequence);
+	else
+		fence_init(&fence->base, &nouveau_fence_ops_legacy,
+			   &fctx->lock, fctx->context, ++fctx->sequence);
+	kref_get(&fctx->fence_ref);
+
+	trace_fence_emit(&fence->base);
 	ret = fctx->emit(fence);
 	if (!ret) {
-		kref_get(&fence->kref);
-		spin_lock(&fctx->lock);
+		fence_get(&fence->base);
+		spin_lock_irq(&fctx->lock);
+		nouveau_fence_update(chan, fctx);
 		list_add_tail(&fence->head, &fctx->pending);
-		spin_unlock(&fctx->lock);
+		spin_unlock_irq(&fctx->lock);
 	}
 
 	return ret;
@@ -161,114 +273,70 @@ nouveau_fence_emit(struct nouveau_fence *fence, struct nouveau_channel *chan)
 bool
 nouveau_fence_done(struct nouveau_fence *fence)
 {
-	if (fence->channel)
-		nouveau_fence_update(fence->channel);
-	return !fence->channel;
-}
+	if (fence->base.ops == &nouveau_fence_ops_legacy ||
+	    fence->base.ops == &nouveau_fence_ops_uevent) {
+		struct nouveau_fence_chan *fctx = nouveau_fctx(fence);
+		unsigned long flags;
 
-struct nouveau_fence_wait {
-	struct nouveau_fence_priv *priv;
-	struct nvif_notify notify;
-};
+		if (test_bit(FENCE_FLAG_SIGNALED_BIT, &fence->base.flags))
+			return true;
 
-static int
-nouveau_fence_wait_uevent_handler(struct nvif_notify *notify)
-{
-	struct nouveau_fence_wait *wait =
-		container_of(notify, typeof(*wait), notify);
-	wake_up_all(&wait->priv->waiting);
-	return NVIF_NOTIFY_KEEP;
+		spin_lock_irqsave(&fctx->lock, flags);
+		nouveau_fence_update(fence->channel, fctx);
+		spin_unlock_irqrestore(&fctx->lock, flags);
+	}
+	return fence_is_signaled(&fence->base);
 }
 
-static int
-nouveau_fence_wait_uevent(struct nouveau_fence *fence, bool intr)
-
+static long
+nouveau_fence_wait_legacy(struct fence *f, bool intr, long wait)
 {
-	struct nouveau_channel *chan = fence->channel;
-	struct nouveau_fence_priv *priv = chan->drm->fence;
-	struct nouveau_fence_wait wait = { .priv = priv };
-	int ret = 0;
+	struct nouveau_fence *fence = from_fence(f);
+	unsigned long sleep_time = NSEC_PER_MSEC / 1000;
+	unsigned long t = jiffies, timeout = t + wait;
 
-	ret = nvif_notify_init(chan->object, NULL,
-			       nouveau_fence_wait_uevent_handler, false,
-			       G82_CHANNEL_DMA_V0_NTFY_UEVENT,
-			       &(struct nvif_notify_uevent_req) {
-			       },
-			       sizeof(struct nvif_notify_uevent_req),
-			       sizeof(struct nvif_notify_uevent_rep),
-			       &wait.notify);
-	if (ret)
-		return ret;
+	while (!nouveau_fence_done(fence)) {
+		ktime_t kt;
 
-	nvif_notify_get(&wait.notify);
-
-	if (fence->timeout) {
-		unsigned long timeout = fence->timeout - jiffies;
-
-		if (time_before(jiffies, fence->timeout)) {
-			if (intr) {
-				ret = wait_event_interruptible_timeout(
-						priv->waiting,
-						nouveau_fence_done(fence),
-						timeout);
-			} else {
-				ret = wait_event_timeout(priv->waiting,
-						nouveau_fence_done(fence),
-						timeout);
-			}
-		}
+		t = jiffies;
 
-		if (ret >= 0) {
-			fence->timeout = jiffies + ret;
-			if (time_after_eq(jiffies, fence->timeout))
-				ret = -EBUSY;
-		}
-	} else {
-		if (intr) {
-			ret = wait_event_interruptible(priv->waiting,
-					nouveau_fence_done(fence));
-		} else {
-			wait_event(priv->waiting, nouveau_fence_done(fence));
+		if (wait != MAX_SCHEDULE_TIMEOUT && time_after_eq(t, timeout)) {
+			__set_current_state(TASK_RUNNING);
+			return 0;
 		}
+
+		__set_current_state(intr ? TASK_INTERRUPTIBLE :
+					   TASK_UNINTERRUPTIBLE);
+
+		kt = ktime_set(0, sleep_time);
+		schedule_hrtimeout(&kt, HRTIMER_MODE_REL);
+		sleep_time *= 2;
+		if (sleep_time > NSEC_PER_MSEC)
+			sleep_time = NSEC_PER_MSEC;
+
+		if (intr && signal_pending(current))
+			return -ERESTARTSYS;
 	}
 
-	nvif_notify_fini(&wait.notify);
-	if (unlikely(ret < 0))
-		return ret;
+	__set_current_state(TASK_RUNNING);
 
-	return 0;
+	return timeout - t;
 }
 
-int
-nouveau_fence_wait(struct nouveau_fence *fence, bool lazy, bool intr)
+static int
+nouveau_fence_wait_busy(struct nouveau_fence *fence, bool intr)
 {
-	struct nouveau_channel *chan = fence->channel;
-	struct nouveau_fence_priv *priv = chan ? chan->drm->fence : NULL;
-	unsigned long sleep_time = NSEC_PER_MSEC / 1000;
-	ktime_t t;
 	int ret = 0;
 
-	while (priv && priv->uevent && lazy && !nouveau_fence_done(fence)) {
-		ret = nouveau_fence_wait_uevent(fence, intr);
-		if (ret < 0)
-			return ret;
-	}
-
 	while (!nouveau_fence_done(fence)) {
-		if (fence->timeout && time_after_eq(jiffies, fence->timeout)) {
+		if (time_after_eq(jiffies, fence->timeout)) {
 			ret = -EBUSY;
 			break;
 		}
 
-		__set_current_state(intr ? TASK_INTERRUPTIBLE :
-					   TASK_UNINTERRUPTIBLE);
-		if (lazy) {
-			t = ktime_set(0, sleep_time);
-			schedule_hrtimeout(&t, HRTIMER_MODE_REL);
-			sleep_time *= 2;
-			if (sleep_time > NSEC_PER_MSEC)
-				sleep_time = NSEC_PER_MSEC;
-		}
+		__set_current_state(intr ?
+				    TASK_INTERRUPTIBLE :
+				    TASK_UNINTERRUPTIBLE);
 
 		if (intr && signal_pending(current)) {
 			ret = -ERESTARTSYS;
@@ -281,47 +349,86 @@ nouveau_fence_wait(struct nouveau_fence *fence, bool lazy, bool intr)
 }
 
 int
-nouveau_fence_sync(struct nouveau_fence *fence, struct nouveau_channel *chan)
+nouveau_fence_wait(struct nouveau_fence *fence, bool lazy, bool intr)
 {
-	struct nouveau_fence_chan *fctx = chan->fence;
-	struct nouveau_channel *prev;
-	int ret = 0;
+	long ret;
 
-	prev = fence ? fence->channel : NULL;
-	if (prev) {
-		if (unlikely(prev != chan && !nouveau_fence_done(fence))) {
-			ret = fctx->sync(fence, prev, chan);
-			if (unlikely(ret))
-				ret = nouveau_fence_wait(fence, true, false);
-		}
-	}
+	if (!lazy)
+		return nouveau_fence_wait_busy(fence, intr);
 
-	return ret;
+	ret = fence_wait_timeout(&fence->base, intr, 15 * HZ);
+	if (ret < 0)
+		return ret;
+	else if (!ret)
+		return -EBUSY;
+	else
+		return 0;
 }
 
-static void
-nouveau_fence_del(struct kref *kref)
+int
+nouveau_fence_sync(struct nouveau_bo *nvbo, struct nouveau_channel *chan, bool exclusive, bool intr)
 {
-	struct nouveau_fence *fence = container_of(kref, typeof(*fence), kref);
-	kfree(fence);
+	struct nouveau_fence_chan *fctx = chan->fence;
+	struct fence *fence;
+	struct reservation_object *resv = nvbo->bo.resv;
+	struct reservation_object_list *fobj;
+	struct nouveau_fence *f;
+	int ret = 0, i;
+
+	if (!exclusive) {
+		ret = reservation_object_reserve_shared(resv);
+
+		if (ret)
+			return ret;
+	}
+
+	fobj = reservation_object_get_list(resv);
+	fence = reservation_object_get_excl(resv);
+
+	if (fence && (!exclusive || !fobj || !fobj->shared_count)) {
+		struct nouveau_channel *prev = NULL;
+
+		f = nouveau_local_fence(fence, chan->drm);
+		if (f)
+			prev = f->channel;
+
+		if (!prev || (prev != chan && (ret = fctx->sync(f, prev, chan))))
+			ret = fence_wait(fence, intr);
+
+		return ret;
+	}
+
+	if (!exclusive || !fobj)
+		return ret;
+
+	for (i = 0; i < fobj->shared_count && !ret; ++i) {
+		struct nouveau_channel *prev = NULL;
+
+		fence = rcu_dereference_protected(fobj->shared[i],
+						reservation_object_held(resv));
+
+		f = nouveau_local_fence(fence, chan->drm);
+		if (f)
+			prev = f->channel;
+
+		if (!prev || (prev != chan && (ret = fctx->sync(f, prev, chan))))
+			ret = fence_wait(fence, intr);
+
+		if (ret)
+			break;
+	}
+
+	return ret;
 }
 
 void
 nouveau_fence_unref(struct nouveau_fence **pfence)
 {
 	if (*pfence)
-		kref_put(&(*pfence)->kref, nouveau_fence_del);
+		fence_put(&(*pfence)->base);
 	*pfence = NULL;
 }
 
-struct nouveau_fence *
-nouveau_fence_ref(struct nouveau_fence *fence)
-{
-	if (fence)
-		kref_get(&fence->kref);
-	return fence;
-}
-
 int
 nouveau_fence_new(struct nouveau_channel *chan, bool sysmem,
 		  struct nouveau_fence **pfence)
@@ -336,9 +443,7 @@ nouveau_fence_new(struct nouveau_channel *chan, bool sysmem,
 	if (!fence)
 		return -ENOMEM;
 
-	INIT_LIST_HEAD(&fence->work);
 	fence->sysmem = sysmem;
-	kref_init(&fence->kref);
 
 	ret = nouveau_fence_emit(fence, chan);
 	if (ret)
@@ -347,3 +452,101 @@ nouveau_fence_new(struct nouveau_channel *chan, bool sysmem,
 	*pfence = fence;
 	return ret;
 }
+
+static const char *nouveau_fence_get_get_driver_name(struct fence *fence)
+{
+	return "nouveau";
+}
+
+static const char *nouveau_fence_get_timeline_name(struct fence *f)
+{
+	struct nouveau_fence *fence = from_fence(f);
+	struct nouveau_fence_chan *fctx = nouveau_fctx(fence);
+
+	return fence->channel ? fctx->name : "dead channel";
+}
+
+/*
+ * In an ideal world, read would not assume the channel context is still alive.
+ * This function may be called from another device, running into free memory as a
+ * result. The drm node should still be there, so we can derive the index from
+ * the fence context.
+ */
+static bool nouveau_fence_is_signaled(struct fence *f)
+{
+	struct nouveau_fence *fence = from_fence(f);
+	struct nouveau_fence_chan *fctx = nouveau_fctx(fence);
+	struct nouveau_channel *chan = fence->channel;
+
+	return (int)(fctx->read(chan) - fence->base.seqno) >= 0;
+}
+
+static bool nouveau_fence_no_signaling(struct fence *f)
+{
+	struct nouveau_fence *fence = from_fence(f);
+
+	/*
+	 * caller should have a reference on the fence,
+	 * else fence could get freed here
+	 */
+	WARN_ON(atomic_read(&fence->base.refcount.refcount) <= 1);
+
+	/*
+	 * This needs uevents to work correctly, but fence_add_callback relies on
+	 * being able to enable signaling. It will still get signaled eventually,
+	 * just not right away.
+	 */
+	if (nouveau_fence_is_signaled(f)) {
+		list_del(&fence->head);
+
+		fence_put(&fence->base);
+		return false;
+	}
+
+	return true;
+}
+
+static void nouveau_fence_release(struct fence *f)
+{
+	struct nouveau_fence *fence = from_fence(f);
+	struct nouveau_fence_chan *fctx = nouveau_fctx(fence);
+
+	kref_put(&fctx->fence_ref, nouveau_fence_context_put);
+	fence_free(&fence->base);
+}
+
+static const struct fence_ops nouveau_fence_ops_legacy = {
+	.get_driver_name = nouveau_fence_get_get_driver_name,
+	.get_timeline_name = nouveau_fence_get_timeline_name,
+	.enable_signaling = nouveau_fence_no_signaling,
+	.signaled = nouveau_fence_is_signaled,
+	.wait = nouveau_fence_wait_legacy,
+	.release = nouveau_fence_release
+};
+
+static bool nouveau_fence_enable_signaling(struct fence *f)
+{
+	struct nouveau_fence *fence = from_fence(f);
+	struct nouveau_fence_chan *fctx = nouveau_fctx(fence);
+	bool ret;
+
+	if (!fctx->notify_ref++)
+		nvif_notify_get(&fctx->notify);
+
+	ret = nouveau_fence_no_signaling(f);
+	if (ret)
+		set_bit(FENCE_FLAG_USER_BITS, &fence->base.flags);
+	else if (!--fctx->notify_ref)
+		nvif_notify_put(&fctx->notify);
+
+	return ret;
+}
+
+static const struct fence_ops nouveau_fence_ops_uevent = {
+	.get_driver_name = nouveau_fence_get_get_driver_name,
+	.get_timeline_name = nouveau_fence_get_timeline_name,
+	.enable_signaling = nouveau_fence_enable_signaling,
+	.signaled = nouveau_fence_is_signaled,
+	.wait = fence_default_wait,
+	.release = NULL
+};
diff --git a/drivers/gpu/drm/nouveau/nouveau_fence.h b/drivers/gpu/drm/nouveau/nouveau_fence.h
index c57bb61da58c..943b0b17b1fc 100644
--- a/drivers/gpu/drm/nouveau/nouveau_fence.h
+++ b/drivers/gpu/drm/nouveau/nouveau_fence.h
@@ -1,33 +1,37 @@
 #ifndef __NOUVEAU_FENCE_H__
 #define __NOUVEAU_FENCE_H__
 
+#include <linux/fence.h>
+#include <nvif/notify.h>
+
 struct nouveau_drm;
+struct nouveau_bo;
 
 struct nouveau_fence {
+	struct fence base;
+
 	struct list_head head;
-	struct list_head work;
-	struct kref kref;
 
 	bool sysmem;
 
 	struct nouveau_channel *channel;
 	unsigned long timeout;
-	u32 sequence;
 };
 
 int  nouveau_fence_new(struct nouveau_channel *, bool sysmem,
 		       struct nouveau_fence **);
-struct nouveau_fence *
-nouveau_fence_ref(struct nouveau_fence *);
 void nouveau_fence_unref(struct nouveau_fence **);
 
 int  nouveau_fence_emit(struct nouveau_fence *, struct nouveau_channel *);
 bool nouveau_fence_done(struct nouveau_fence *);
-void nouveau_fence_work(struct nouveau_fence *, void (*)(void *), void *);
+void nouveau_fence_work(struct fence *, void (*)(void *), void *);
 int  nouveau_fence_wait(struct nouveau_fence *, bool lazy, bool intr);
-int  nouveau_fence_sync(struct nouveau_fence *, struct nouveau_channel *);
+int  nouveau_fence_sync(struct nouveau_bo *, struct nouveau_channel *, bool exclusive, bool intr);
 
 struct nouveau_fence_chan {
+	spinlock_t lock;
+	struct kref fence_ref;
+
 	struct list_head pending;
 	struct list_head flip;
 
@@ -38,8 +42,12 @@ struct nouveau_fence_chan {
 	int  (*emit32)(struct nouveau_channel *, u64, u32);
 	int  (*sync32)(struct nouveau_channel *, u64, u32);
 
-	spinlock_t lock;
 	u32 sequence;
+	u32 context;
+	char name[32];
+
+	struct nvif_notify notify;
+	int notify_ref;
 };
 
 struct nouveau_fence_priv {
@@ -49,14 +57,15 @@ struct nouveau_fence_priv {
 	int  (*context_new)(struct nouveau_channel *);
 	void (*context_del)(struct nouveau_channel *);
 
-	wait_queue_head_t waiting;
+	u32 contexts, context_base;
 	bool uevent;
 };
 
 #define nouveau_fence(drm) ((struct nouveau_fence_priv *)(drm)->fence)
 
-void nouveau_fence_context_new(struct nouveau_fence_chan *);
+void nouveau_fence_context_new(struct nouveau_channel *, struct nouveau_fence_chan *);
 void nouveau_fence_context_del(struct nouveau_fence_chan *);
+void nouveau_fence_context_free(struct nouveau_fence_chan *);
 
 int nv04_fence_create(struct nouveau_drm *);
 int nv04_fence_mthd(struct nouveau_channel *, u32, u32, u32);
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.c b/drivers/gpu/drm/nouveau/nouveau_gem.c
index 292a677bfed4..36951ee4b157 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.c
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.c
@@ -98,17 +98,23 @@ static void
 nouveau_gem_object_unmap(struct nouveau_bo *nvbo, struct nouveau_vma *vma)
 {
 	const bool mapped = nvbo->bo.mem.mem_type != TTM_PL_SYSTEM;
-	struct nouveau_fence *fence = NULL;
+	struct reservation_object *resv = nvbo->bo.resv;
+	struct reservation_object_list *fobj;
+	struct fence *fence = NULL;
+
+	fobj = reservation_object_get_list(resv);
 
 	list_del(&vma->head);
 
-	if (mapped) {
-		spin_lock(&nvbo->bo.bdev->fence_lock);
-		fence = nouveau_fence_ref(nvbo->bo.sync_obj);
-		spin_unlock(&nvbo->bo.bdev->fence_lock);
-	}
+	if (fobj && fobj->shared_count > 1)
+		ttm_bo_wait(&nvbo->bo, true, false, false);
+	else if (fobj && fobj->shared_count == 1)
+		fence = rcu_dereference_protected(fobj->shared[0],
+						reservation_object_held(resv));
+	else
+		fence = reservation_object_get_excl(nvbo->bo.resv);
 
-	if (fence) {
+	if (fence && mapped) {
 		nouveau_fence_work(fence, nouveau_gem_object_delete, vma);
 	} else {
 		if (mapped)
@@ -116,7 +122,6 @@ nouveau_gem_object_unmap(struct nouveau_bo *nvbo, struct nouveau_vma *vma)
 		nouveau_vm_put(vma);
 		kfree(vma);
 	}
-	nouveau_fence_unref(&fence);
 }
 
 void
@@ -160,7 +165,7 @@ nouveau_gem_new(struct drm_device *dev, int size, int align, uint32_t domain,
 		flags |= TTM_PL_FLAG_SYSTEM;
 
 	ret = nouveau_bo_new(dev, size, align, flags, tile_mode,
-			     tile_flags, NULL, pnvbo);
+			     tile_flags, NULL, NULL, pnvbo);
 	if (ret)
 		return ret;
 	nvbo = *pnvbo;
@@ -288,24 +293,23 @@ nouveau_gem_set_domain(struct drm_gem_object *gem, uint32_t read_domains,
 }
 
 struct validate_op {
-	struct list_head vram_list;
-	struct list_head gart_list;
-	struct list_head both_list;
+	struct list_head list;
 	struct ww_acquire_ctx ticket;
 };
 
 static void
-validate_fini_list(struct list_head *list, struct nouveau_fence *fence,
-		   struct ww_acquire_ctx *ticket)
+validate_fini_no_ticket(struct validate_op *op, struct nouveau_fence *fence,
+			struct drm_nouveau_gem_pushbuf_bo *pbbo)
 {
-	struct list_head *entry, *tmp;
 	struct nouveau_bo *nvbo;
+	struct drm_nouveau_gem_pushbuf_bo *b;
 
-	list_for_each_safe(entry, tmp, list) {
-		nvbo = list_entry(entry, struct nouveau_bo, entry);
+	while (!list_empty(&op->list)) {
+		nvbo = list_entry(op->list.next, struct nouveau_bo, entry);
+		b = &pbbo[nvbo->pbbo_index];
 
 		if (likely(fence))
-			nouveau_bo_fence(nvbo, fence);
+			nouveau_bo_fence(nvbo, fence, !!b->write_domains);
 
 		if (unlikely(nvbo->validate_mapped)) {
 			ttm_bo_kunmap(&nvbo->kmap);
@@ -314,23 +318,16 @@ validate_fini_list(struct list_head *list, struct nouveau_fence *fence,
 
 		list_del(&nvbo->entry);
 		nvbo->reserved_by = NULL;
-		ttm_bo_unreserve_ticket(&nvbo->bo, ticket);
+		ttm_bo_unreserve_ticket(&nvbo->bo, &op->ticket);
 		drm_gem_object_unreference_unlocked(&nvbo->gem);
 	}
 }
 
 static void
-validate_fini_no_ticket(struct validate_op *op, struct nouveau_fence *fence)
+validate_fini(struct validate_op *op, struct nouveau_fence *fence,
+	      struct drm_nouveau_gem_pushbuf_bo *pbbo)
 {
-	validate_fini_list(&op->vram_list, fence, &op->ticket);
-	validate_fini_list(&op->gart_list, fence, &op->ticket);
-	validate_fini_list(&op->both_list, fence, &op->ticket);
-}
-
-static void
-validate_fini(struct validate_op *op, struct nouveau_fence *fence)
-{
-	validate_fini_no_ticket(op, fence);
+	validate_fini_no_ticket(op, fence, pbbo);
 	ww_acquire_fini(&op->ticket);
 }
 
@@ -344,6 +341,9 @@ validate_init(struct nouveau_channel *chan, struct drm_file *file_priv,
 	int trycnt = 0;
 	int ret, i;
 	struct nouveau_bo *res_bo = NULL;
+	LIST_HEAD(gart_list);
+	LIST_HEAD(vram_list);
+	LIST_HEAD(both_list);
 
 	ww_acquire_init(&op->ticket, &reservation_ww_class);
 retry:
@@ -360,9 +360,8 @@ retry:
 		gem = drm_gem_object_lookup(dev, file_priv, b->handle);
 		if (!gem) {
 			NV_PRINTK(error, cli, "Unknown handle 0x%08x\n", b->handle);
-			ww_acquire_done(&op->ticket);
-			validate_fini(op, NULL);
-			return -ENOENT;
+			ret = -ENOENT;
+			break;
 		}
 		nvbo = nouveau_gem_object(gem);
 		if (nvbo == res_bo) {
@@ -375,14 +374,16 @@ retry:
 			NV_PRINTK(error, cli, "multiple instances of buffer %d on "
 				      "validation list\n", b->handle);
 			drm_gem_object_unreference_unlocked(gem);
-			ww_acquire_done(&op->ticket);
-			validate_fini(op, NULL);
-			return -EINVAL;
+			ret = -EINVAL;
+			break;
 		}
 
 		ret = ttm_bo_reserve(&nvbo->bo, true, false, true, &op->ticket);
 		if (ret) {
-			validate_fini_no_ticket(op, NULL);
+			list_splice_tail_init(&vram_list, &op->list);
+			list_splice_tail_init(&gart_list, &op->list);
+			list_splice_tail_init(&both_list, &op->list);
+			validate_fini_no_ticket(op, NULL, NULL);
 			if (unlikely(ret == -EDEADLK)) {
 				ret = ttm_bo_reserve_slowpath(&nvbo->bo, true,
 							      &op->ticket);
@@ -390,12 +391,9 @@ retry:
 					res_bo = nvbo;
 			}
 			if (unlikely(ret)) {
-				ww_acquire_done(&op->ticket);
-				ww_acquire_fini(&op->ticket);
-				drm_gem_object_unreference_unlocked(gem);
 				if (ret != -ERESTARTSYS)
 					NV_PRINTK(error, cli, "fail reserve\n");
-				return ret;
+				break;
 			}
 		}
 
@@ -404,45 +402,32 @@ retry:
 		nvbo->pbbo_index = i;
 		if ((b->valid_domains & NOUVEAU_GEM_DOMAIN_VRAM) &&
 		    (b->valid_domains & NOUVEAU_GEM_DOMAIN_GART))
-			list_add_tail(&nvbo->entry, &op->both_list);
+			list_add_tail(&nvbo->entry, &both_list);
 		else
 		if (b->valid_domains & NOUVEAU_GEM_DOMAIN_VRAM)
-			list_add_tail(&nvbo->entry, &op->vram_list);
+			list_add_tail(&nvbo->entry, &vram_list);
 		else
 		if (b->valid_domains & NOUVEAU_GEM_DOMAIN_GART)
-			list_add_tail(&nvbo->entry, &op->gart_list);
+			list_add_tail(&nvbo->entry, &gart_list);
 		else {
 			NV_PRINTK(error, cli, "invalid valid domains: 0x%08x\n",
 				 b->valid_domains);
-			list_add_tail(&nvbo->entry, &op->both_list);
-			ww_acquire_done(&op->ticket);
-			validate_fini(op, NULL);
-			return -EINVAL;
+			list_add_tail(&nvbo->entry, &both_list);
+			ret = -EINVAL;
+			break;
 		}
 		if (nvbo == res_bo)
 			goto retry;
 	}
 
 	ww_acquire_done(&op->ticket);
-	return 0;
-}
-
-static int
-validate_sync(struct nouveau_channel *chan, struct nouveau_bo *nvbo)
-{
-	struct nouveau_fence *fence = NULL;
-	int ret = 0;
-
-	spin_lock(&nvbo->bo.bdev->fence_lock);
-	fence = nouveau_fence_ref(nvbo->bo.sync_obj);
-	spin_unlock(&nvbo->bo.bdev->fence_lock);
-
-	if (fence) {
-		ret = nouveau_fence_sync(fence, chan);
-		nouveau_fence_unref(&fence);
-	}
-
+	list_splice_tail(&vram_list, &op->list);
+	list_splice_tail(&gart_list, &op->list);
+	list_splice_tail(&both_list, &op->list);
+	if (ret)
+		validate_fini(op, NULL, NULL);
 	return ret;
+
 }
 
 static int
@@ -474,9 +459,10 @@ validate_list(struct nouveau_channel *chan, struct nouveau_cli *cli,
 			return ret;
 		}
 
-		ret = validate_sync(chan, nvbo);
+		ret = nouveau_fence_sync(nvbo, chan, !!b->write_domains, true);
 		if (unlikely(ret)) {
-			NV_PRINTK(error, cli, "fail post-validate sync\n");
+			if (ret != -ERESTARTSYS)
+				NV_PRINTK(error, cli, "fail post-validate sync\n");
 			return ret;
 		}
 
@@ -513,11 +499,9 @@ nouveau_gem_pushbuf_validate(struct nouveau_channel *chan,
 			     struct validate_op *op, int *apply_relocs)
 {
 	struct nouveau_cli *cli = nouveau_cli(file_priv);
-	int ret, relocs = 0;
+	int ret;
 
-	INIT_LIST_HEAD(&op->vram_list);
-	INIT_LIST_HEAD(&op->gart_list);
-	INIT_LIST_HEAD(&op->both_list);
+	INIT_LIST_HEAD(&op->list);
 
 	if (nr_buffers == 0)
 		return 0;
@@ -529,34 +513,14 @@ nouveau_gem_pushbuf_validate(struct nouveau_channel *chan,
 		return ret;
 	}
 
-	ret = validate_list(chan, cli, &op->vram_list, pbbo, user_buffers);
-	if (unlikely(ret < 0)) {
-		if (ret != -ERESTARTSYS)
-			NV_PRINTK(error, cli, "validate vram_list\n");
-		validate_fini(op, NULL);
-		return ret;
-	}
-	relocs += ret;
-
-	ret = validate_list(chan, cli, &op->gart_list, pbbo, user_buffers);
-	if (unlikely(ret < 0)) {
-		if (ret != -ERESTARTSYS)
-			NV_PRINTK(error, cli, "validate gart_list\n");
-		validate_fini(op, NULL);
-		return ret;
-	}
-	relocs += ret;
-
-	ret = validate_list(chan, cli, &op->both_list, pbbo, user_buffers);
+	ret = validate_list(chan, cli, &op->list, pbbo, user_buffers);
 	if (unlikely(ret < 0)) {
 		if (ret != -ERESTARTSYS)
-			NV_PRINTK(error, cli, "validate both_list\n");
-		validate_fini(op, NULL);
+			NV_PRINTK(error, cli, "validating bo list\n");
+		validate_fini(op, NULL, NULL);
 		return ret;
 	}
-	relocs += ret;
-
-	*apply_relocs = relocs;
+	*apply_relocs = ret;
 	return 0;
 }
 
@@ -659,9 +623,7 @@ nouveau_gem_pushbuf_reloc_apply(struct nouveau_cli *cli,
 				data |= r->vor;
 		}
 
-		spin_lock(&nvbo->bo.bdev->fence_lock);
-		ret = ttm_bo_wait(&nvbo->bo, false, false, false);
-		spin_unlock(&nvbo->bo.bdev->fence_lock);
+		ret = ttm_bo_wait(&nvbo->bo, true, false, false);
 		if (ret) {
 			NV_PRINTK(error, cli, "reloc wait_idle failed: %d\n", ret);
 			break;
@@ -839,7 +801,7 @@ nouveau_gem_ioctl_pushbuf(struct drm_device *dev, void *data,
 	}
 
 out:
-	validate_fini(&op, fence);
+	validate_fini(&op, fence, bo);
 	nouveau_fence_unref(&fence);
 
 out_prevalid:
@@ -884,17 +846,29 @@ nouveau_gem_ioctl_cpu_prep(struct drm_device *dev, void *data,
 	struct drm_gem_object *gem;
 	struct nouveau_bo *nvbo;
 	bool no_wait = !!(req->flags & NOUVEAU_GEM_CPU_PREP_NOWAIT);
-	int ret = -EINVAL;
+	bool write = !!(req->flags & NOUVEAU_GEM_CPU_PREP_WRITE);
+	int ret;
 
 	gem = drm_gem_object_lookup(dev, file_priv, req->handle);
 	if (!gem)
 		return -ENOENT;
 	nvbo = nouveau_gem_object(gem);
 
-	spin_lock(&nvbo->bo.bdev->fence_lock);
-	ret = ttm_bo_wait(&nvbo->bo, true, true, no_wait);
-	spin_unlock(&nvbo->bo.bdev->fence_lock);
+	if (no_wait)
+		ret = reservation_object_test_signaled_rcu(nvbo->bo.resv, write) ? 0 : -EBUSY;
+	else {
+		long lret;
+
+		lret = reservation_object_wait_timeout_rcu(nvbo->bo.resv, write, true, 30 * HZ);
+		if (!lret)
+			ret = -EBUSY;
+		else if (lret > 0)
+			ret = 0;
+		else
+			ret = lret;
+	}
 	drm_gem_object_unreference_unlocked(gem);
+
 	return ret;
 }
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_gem.h b/drivers/gpu/drm/nouveau/nouveau_gem.h
index ddab762d81fe..e4049faca780 100644
--- a/drivers/gpu/drm/nouveau/nouveau_gem.h
+++ b/drivers/gpu/drm/nouveau/nouveau_gem.h
@@ -39,7 +39,7 @@ struct reservation_object *nouveau_gem_prime_res_obj(struct drm_gem_object *);
 extern void nouveau_gem_prime_unpin(struct drm_gem_object *);
 extern struct sg_table *nouveau_gem_prime_get_sg_table(struct drm_gem_object *);
 extern struct drm_gem_object *nouveau_gem_prime_import_sg_table(
-	struct drm_device *, size_t size, struct sg_table *);
+	struct drm_device *, struct dma_buf_attachment *, struct sg_table *);
 extern void *nouveau_gem_prime_vmap(struct drm_gem_object *);
 extern void nouveau_gem_prime_vunmap(struct drm_gem_object *, void *);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_nvif.c b/drivers/gpu/drm/nouveau/nouveau_nvif.c
index 47ca88623753..6544b84f0303 100644
--- a/drivers/gpu/drm/nouveau/nouveau_nvif.c
+++ b/drivers/gpu/drm/nouveau/nouveau_nvif.c
@@ -40,12 +40,12 @@
 #include "nouveau_usif.h"
 
 static void
-nvkm_client_unmap(void *priv, void *ptr, u32 size)
+nvkm_client_unmap(void *priv, void __iomem *ptr, u32 size)
 {
 	iounmap(ptr);
 }
 
-static void *
+static void __iomem *
 nvkm_client_map(void *priv, u64 handle, u32 size)
 {
 	return ioremap(handle, size);
diff --git a/drivers/gpu/drm/nouveau/nouveau_prime.c b/drivers/gpu/drm/nouveau/nouveau_prime.c
index 1f51008e4d26..228226ab27fc 100644
--- a/drivers/gpu/drm/nouveau/nouveau_prime.c
+++ b/drivers/gpu/drm/nouveau/nouveau_prime.c
@@ -23,6 +23,7 @@
  */
 
 #include <drm/drmP.h>
+#include <linux/dma-buf.h>
 
 #include "nouveau_drm.h"
 #include "nouveau_gem.h"
@@ -56,17 +57,20 @@ void nouveau_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
 }
 
 struct drm_gem_object *nouveau_gem_prime_import_sg_table(struct drm_device *dev,
-							 size_t size,
+							 struct dma_buf_attachment *attach,
 							 struct sg_table *sg)
 {
 	struct nouveau_bo *nvbo;
+	struct reservation_object *robj = attach->dmabuf->resv;
 	u32 flags = 0;
 	int ret;
 
 	flags = TTM_PL_FLAG_TT;
 
-	ret = nouveau_bo_new(dev, size, 0, flags, 0, 0,
-			     sg, &nvbo);
+	ww_mutex_lock(&robj->lock, NULL);
+	ret = nouveau_bo_new(dev, attach->dmabuf->size, 0, flags, 0, 0,
+			     sg, robj, &nvbo);
+	ww_mutex_unlock(&robj->lock);
 	if (ret)
 		return ERR_PTR(ret);
 
diff --git a/drivers/gpu/drm/nouveau/nouveau_sysfs.c b/drivers/gpu/drm/nouveau/nouveau_sysfs.c
index 3c6962d15b26..8fbbf3093d86 100644
--- a/drivers/gpu/drm/nouveau/nouveau_sysfs.c
+++ b/drivers/gpu/drm/nouveau/nouveau_sysfs.c
@@ -29,7 +29,7 @@
 #include "nouveau_sysfs.h"
 
 MODULE_PARM_DESC(pstate, "enable sysfs pstate file, which will be moved in the future");
-static int nouveau_pstate;
+int nouveau_pstate;
 module_param_named(pstate, nouveau_pstate, int, 0400);
 
 static inline struct drm_device *
diff --git a/drivers/gpu/drm/nouveau/nouveau_sysfs.h b/drivers/gpu/drm/nouveau/nouveau_sysfs.h
index f973378160f8..4e5ea9241b28 100644
--- a/drivers/gpu/drm/nouveau/nouveau_sysfs.h
+++ b/drivers/gpu/drm/nouveau/nouveau_sysfs.h
@@ -16,4 +16,6 @@ nouveau_sysfs(struct drm_device *dev)
 int  nouveau_sysfs_init(struct drm_device *);
 void nouveau_sysfs_fini(struct drm_device *);
 
+extern int nouveau_pstate;
+
 #endif
diff --git a/drivers/gpu/drm/nouveau/nouveau_ttm.c b/drivers/gpu/drm/nouveau/nouveau_ttm.c
index 53874b76b031..753a6def61e7 100644
--- a/drivers/gpu/drm/nouveau/nouveau_ttm.c
+++ b/drivers/gpu/drm/nouveau/nouveau_ttm.c
@@ -71,8 +71,7 @@ nouveau_vram_manager_del(struct ttm_mem_type_manager *man,
 static int
 nouveau_vram_manager_new(struct ttm_mem_type_manager *man,
 			 struct ttm_buffer_object *bo,
-			 struct ttm_placement *placement,
-			 uint32_t flags,
+			 const struct ttm_place *place,
 			 struct ttm_mem_reg *mem)
 {
 	struct nouveau_drm *drm = nouveau_bdev(man->bdev);
@@ -158,8 +157,7 @@ nouveau_gart_manager_del(struct ttm_mem_type_manager *man,
 static int
 nouveau_gart_manager_new(struct ttm_mem_type_manager *man,
 			 struct ttm_buffer_object *bo,
-			 struct ttm_placement *placement,
-			 uint32_t flags,
+			 const struct ttm_place *place,
 			 struct ttm_mem_reg *mem)
 {
 	struct nouveau_drm *drm = nouveau_bdev(bo->bdev);
@@ -239,8 +237,7 @@ nv04_gart_manager_del(struct ttm_mem_type_manager *man, struct ttm_mem_reg *mem)
 static int
 nv04_gart_manager_new(struct ttm_mem_type_manager *man,
 		      struct ttm_buffer_object *bo,
-		      struct ttm_placement *placement,
-		      uint32_t flags,
+		      const struct ttm_place *place,
 		      struct ttm_mem_reg *mem)
 {
 	struct nouveau_mem *node;
@@ -284,7 +281,7 @@ nouveau_ttm_mmap(struct file *filp, struct vm_area_struct *vma)
 	struct nouveau_drm *drm = nouveau_drm(file_priv->minor->dev);
 
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET))
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 
 	return ttm_bo_mmap(filp, vma, &drm->ttm.bdev);
 }
diff --git a/drivers/gpu/drm/nouveau/nouveau_vga.c b/drivers/gpu/drm/nouveau/nouveau_vga.c
index 18d55d447248..c7592ec8ecb8 100644
--- a/drivers/gpu/drm/nouveau/nouveau_vga.c
+++ b/drivers/gpu/drm/nouveau/nouveau_vga.c
@@ -108,7 +108,16 @@ void
 nouveau_vga_fini(struct nouveau_drm *drm)
 {
 	struct drm_device *dev = drm->dev;
+	bool runtime = false;
+
+	if (nouveau_runtime_pm == 1)
+		runtime = true;
+	if ((nouveau_runtime_pm == -1) && (nouveau_is_optimus() || nouveau_is_v1_dsm()))
+		runtime = true;
+
 	vga_switcheroo_unregister_client(dev->pdev);
+	if (runtime && nouveau_is_v1_dsm() && !nouveau_is_optimus())
+		vga_switcheroo_fini_domain_pm_ops(drm->dev->dev);
 	vga_client_register(dev->pdev, NULL, NULL, NULL);
 }
 
diff --git a/drivers/gpu/drm/nouveau/nv04_fence.c b/drivers/gpu/drm/nouveau/nv04_fence.c
index 239c2c5a9615..f9859deb108a 100644
--- a/drivers/gpu/drm/nouveau/nv04_fence.c
+++ b/drivers/gpu/drm/nouveau/nv04_fence.c
@@ -41,7 +41,7 @@ nv04_fence_emit(struct nouveau_fence *fence)
 	int ret = RING_SPACE(chan, 2);
 	if (ret == 0) {
 		BEGIN_NV04(chan, NvSubSw, 0x0150, 1);
-		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, fence->base.seqno);
 		FIRE_RING (chan);
 	}
 	return ret;
@@ -67,7 +67,7 @@ nv04_fence_context_del(struct nouveau_channel *chan)
 	struct nv04_fence_chan *fctx = chan->fence;
 	nouveau_fence_context_del(&fctx->base);
 	chan->fence = NULL;
-	kfree(fctx);
+	nouveau_fence_context_free(&fctx->base);
 }
 
 static int
@@ -75,7 +75,7 @@ nv04_fence_context_new(struct nouveau_channel *chan)
 {
 	struct nv04_fence_chan *fctx = kzalloc(sizeof(*fctx), GFP_KERNEL);
 	if (fctx) {
-		nouveau_fence_context_new(&fctx->base);
+		nouveau_fence_context_new(chan, &fctx->base);
 		fctx->base.emit = nv04_fence_emit;
 		fctx->base.sync = nv04_fence_sync;
 		fctx->base.read = nv04_fence_read;
@@ -105,5 +105,7 @@ nv04_fence_create(struct nouveau_drm *drm)
 	priv->base.dtor = nv04_fence_destroy;
 	priv->base.context_new = nv04_fence_context_new;
 	priv->base.context_del = nv04_fence_context_del;
+	priv->base.contexts = 15;
+	priv->base.context_base = fence_context_alloc(priv->base.contexts);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv10_fence.c b/drivers/gpu/drm/nouveau/nv10_fence.c
index 4faaf0acf5d7..5e1ea1cdce75 100644
--- a/drivers/gpu/drm/nouveau/nv10_fence.c
+++ b/drivers/gpu/drm/nouveau/nv10_fence.c
@@ -33,7 +33,7 @@ nv10_fence_emit(struct nouveau_fence *fence)
 	int ret = RING_SPACE(chan, 2);
 	if (ret == 0) {
 		BEGIN_NV04(chan, 0, NV10_SUBCHAN_REF_CNT, 1);
-		OUT_RING  (chan, fence->sequence);
+		OUT_RING  (chan, fence->base.seqno);
 		FIRE_RING (chan);
 	}
 	return ret;
@@ -63,7 +63,7 @@ nv10_fence_context_del(struct nouveau_channel *chan)
 		nvif_object_fini(&fctx->head[i]);
 	nvif_object_fini(&fctx->sema);
 	chan->fence = NULL;
-	kfree(fctx);
+	nouveau_fence_context_free(&fctx->base);
 }
 
 int
@@ -75,7 +75,7 @@ nv10_fence_context_new(struct nouveau_channel *chan)
 	if (!fctx)
 		return -ENOMEM;
 
-	nouveau_fence_context_new(&fctx->base);
+	nouveau_fence_context_new(chan, &fctx->base);
 	fctx->base.emit = nv10_fence_emit;
 	fctx->base.read = nv10_fence_read;
 	fctx->base.sync = nv10_fence_sync;
@@ -106,6 +106,8 @@ nv10_fence_create(struct nouveau_drm *drm)
 	priv->base.dtor = nv10_fence_destroy;
 	priv->base.context_new = nv10_fence_context_new;
 	priv->base.context_del = nv10_fence_context_del;
+	priv->base.contexts = 31;
+	priv->base.context_base = fence_context_alloc(priv->base.contexts);
 	spin_lock_init(&priv->lock);
 	return 0;
 }
diff --git a/drivers/gpu/drm/nouveau/nv17_fence.c b/drivers/gpu/drm/nouveau/nv17_fence.c
index ca907479f92f..40b461c7d5c5 100644
--- a/drivers/gpu/drm/nouveau/nv17_fence.c
+++ b/drivers/gpu/drm/nouveau/nv17_fence.c
@@ -84,7 +84,7 @@ nv17_fence_context_new(struct nouveau_channel *chan)
 	if (!fctx)
 		return -ENOMEM;
 
-	nouveau_fence_context_new(&fctx->base);
+	nouveau_fence_context_new(chan, &fctx->base);
 	fctx->base.emit = nv10_fence_emit;
 	fctx->base.read = nv10_fence_read;
 	fctx->base.sync = nv17_fence_sync;
@@ -124,10 +124,12 @@ nv17_fence_create(struct nouveau_drm *drm)
 	priv->base.resume = nv17_fence_resume;
 	priv->base.context_new = nv17_fence_context_new;
 	priv->base.context_del = nv10_fence_context_del;
+	priv->base.contexts = 31;
+	priv->base.context_base = fence_context_alloc(priv->base.contexts);
 	spin_lock_init(&priv->lock);
 
 	ret = nouveau_bo_new(drm->dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, NULL, &priv->bo);
+			     0, 0x0000, NULL, NULL, &priv->bo);
 	if (!ret) {
 		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
 		if (!ret) {
diff --git a/drivers/gpu/drm/nouveau/nv50_display.c b/drivers/gpu/drm/nouveau/nv50_display.c
index 03949eaa629f..ae873d1a8d46 100644
--- a/drivers/gpu/drm/nouveau/nv50_display.c
+++ b/drivers/gpu/drm/nouveau/nv50_display.c
@@ -1066,7 +1066,7 @@ nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
 	u32 vscan = (mode->flags & DRM_MODE_FLAG_DBLSCAN) ? 2 : 1;
 	u32 hactive, hsynce, hbackp, hfrontp, hblanke, hblanks;
 	u32 vactive, vsynce, vbackp, vfrontp, vblanke, vblanks;
-	u32 vblan2e = 0, vblan2s = 1;
+	u32 vblan2e = 0, vblan2s = 1, vblankus = 0;
 	u32 *push;
 	int ret;
 
@@ -1083,6 +1083,11 @@ nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
 	vblanke = vsynce + vbackp;
 	vfrontp = (mode->vsync_start - mode->vdisplay) * vscan / ilace;
 	vblanks = vactive - vfrontp - 1;
+	/* XXX: Safe underestimate, even "0" works */
+	vblankus = (vactive - mode->vdisplay - 2) * hactive;
+	vblankus *= 1000;
+	vblankus /= mode->clock;
+
 	if (mode->flags & DRM_MODE_FLAG_INTERLACE) {
 		vblan2e = vactive + vsynce + vbackp;
 		vblan2s = vblan2e + (mode->vdisplay * vscan / ilace);
@@ -1099,14 +1104,14 @@ nv50_crtc_mode_set(struct drm_crtc *crtc, struct drm_display_mode *umode,
 			evo_mthd(push, 0x0804 + (nv_crtc->index * 0x400), 2);
 			evo_data(push, 0x00800000 | mode->clock);
 			evo_data(push, (ilace == 2) ? 2 : 0);
-			evo_mthd(push, 0x0810 + (nv_crtc->index * 0x400), 6);
+			evo_mthd(push, 0x0810 + (nv_crtc->index * 0x400), 8);
 			evo_data(push, 0x00000000);
 			evo_data(push, (vactive << 16) | hactive);
 			evo_data(push, ( vsynce << 16) | hsynce);
 			evo_data(push, (vblanke << 16) | hblanke);
 			evo_data(push, (vblanks << 16) | hblanks);
 			evo_data(push, (vblan2e << 16) | vblan2s);
-			evo_mthd(push, 0x082c + (nv_crtc->index * 0x400), 1);
+			evo_data(push, vblankus);
 			evo_data(push, 0x00000000);
 			evo_mthd(push, 0x0900 + (nv_crtc->index * 0x400), 2);
 			evo_data(push, 0x00000311);
@@ -1378,7 +1383,7 @@ nv50_crtc_create(struct drm_device *dev, int index)
 	drm_mode_crtc_set_gamma_size(crtc, 256);
 
 	ret = nouveau_bo_new(dev, 8192, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, NULL, &head->base.lut.nvbo);
+			     0, 0x0000, NULL, NULL, &head->base.lut.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(head->base.lut.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret) {
@@ -1401,7 +1406,7 @@ nv50_crtc_create(struct drm_device *dev, int index)
 		goto out;
 
 	ret = nouveau_bo_new(dev, 64 * 64 * 4, 0x100, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, NULL, &head->base.cursor.nvbo);
+			     0, 0x0000, NULL, NULL, &head->base.cursor.nvbo);
 	if (!ret) {
 		ret = nouveau_bo_pin(head->base.cursor.nvbo, TTM_PL_FLAG_VRAM);
 		if (!ret) {
@@ -1651,17 +1656,21 @@ static void
 nv50_audio_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
+	struct nouveau_crtc *nv_crtc = nouveau_crtc(encoder->crtc);
 	struct nouveau_connector *nv_connector;
 	struct nv50_disp *disp = nv50_disp(encoder->dev);
-	struct {
-		struct nv50_disp_mthd_v1 base;
-		struct nv50_disp_sor_hda_eld_v0 eld;
+	struct __packed {
+		struct {
+			struct nv50_disp_mthd_v1 mthd;
+			struct nv50_disp_sor_hda_eld_v0 eld;
+		} base;
 		u8 data[sizeof(nv_connector->base.eld)];
 	} args = {
-		.base.version = 1,
-		.base.method  = NV50_DISP_MTHD_V1_SOR_HDA_ELD,
-		.base.hasht   = nv_encoder->dcb->hasht,
-		.base.hashm   = nv_encoder->dcb->hashm,
+		.base.mthd.version = 1,
+		.base.mthd.method  = NV50_DISP_MTHD_V1_SOR_HDA_ELD,
+		.base.mthd.hasht   = nv_encoder->dcb->hasht,
+		.base.mthd.hashm   = (0xf0ff & nv_encoder->dcb->hashm) |
+				     (0x0100 << nv_crtc->index),
 	};
 
 	nv_connector = nouveau_encoder_connector_get(nv_encoder);
@@ -1671,11 +1680,11 @@ nv50_audio_mode_set(struct drm_encoder *encoder, struct drm_display_mode *mode)
 	drm_edid_to_eld(&nv_connector->base, nv_connector->edid);
 	memcpy(args.data, nv_connector->base.eld, sizeof(args.data));
 
-	nvif_mthd(disp->disp, 0, &args, sizeof(args));
+	nvif_mthd(disp->disp, 0, &args, sizeof(args.base) + args.data[2] * 4);
 }
 
 static void
-nv50_audio_disconnect(struct drm_encoder *encoder)
+nv50_audio_disconnect(struct drm_encoder *encoder, struct nouveau_crtc *nv_crtc)
 {
 	struct nouveau_encoder *nv_encoder = nouveau_encoder(encoder);
 	struct nv50_disp *disp = nv50_disp(encoder->dev);
@@ -1686,7 +1695,8 @@ nv50_audio_disconnect(struct drm_encoder *encoder)
 		.base.version = 1,
 		.base.method  = NV50_DISP_MTHD_V1_SOR_HDA_ELD,
 		.base.hasht   = nv_encoder->dcb->hasht,
-		.base.hashm   = nv_encoder->dcb->hashm,
+		.base.hashm   = (0xf0ff & nv_encoder->dcb->hashm) |
+				(0x0100 << nv_crtc->index),
 	};
 
 	nvif_mthd(disp->disp, 0, &args, sizeof(args));
@@ -1745,8 +1755,6 @@ nv50_hdmi_disconnect(struct drm_encoder *encoder, struct nouveau_crtc *nv_crtc)
 			       (0x0100 << nv_crtc->index),
 	};
 
-	nv50_audio_disconnect(encoder);
-
 	nvif_mthd(disp->disp, 0, &args, sizeof(args));
 }
 
@@ -1855,6 +1863,7 @@ nv50_sor_disconnect(struct drm_encoder *encoder)
 	if (nv_crtc) {
 		nv50_crtc_prepare(&nv_crtc->base);
 		nv50_sor_ctrl(nv_encoder, 1 << nv_crtc->index, 0);
+		nv50_audio_disconnect(encoder, nv_crtc);
 		nv50_hdmi_disconnect(&nv_encoder->base.base, nv_crtc);
 	}
 }
@@ -1954,6 +1963,7 @@ nv50_sor_mode_set(struct drm_encoder *encoder, struct drm_display_mode *umode,
 			proto = 0x8;
 		else
 			proto = 0x9;
+		nv50_audio_mode_set(encoder, mode);
 		break;
 	default:
 		BUG_ON(1);
@@ -2458,7 +2468,7 @@ nv50_display_create(struct drm_device *dev)
 
 	/* small shared memory area we use for notifiers and semaphores */
 	ret = nouveau_bo_new(dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, NULL, &disp->sync);
+			     0, 0x0000, NULL, NULL, &disp->sync);
 	if (!ret) {
 		ret = nouveau_bo_pin(disp->sync, TTM_PL_FLAG_VRAM);
 		if (!ret) {
diff --git a/drivers/gpu/drm/nouveau/nv50_fence.c b/drivers/gpu/drm/nouveau/nv50_fence.c
index 195cf51a7c31..22d242b37962 100644
--- a/drivers/gpu/drm/nouveau/nv50_fence.c
+++ b/drivers/gpu/drm/nouveau/nv50_fence.c
@@ -46,7 +46,7 @@ nv50_fence_context_new(struct nouveau_channel *chan)
 	if (!fctx)
 		return -ENOMEM;
 
-	nouveau_fence_context_new(&fctx->base);
+	nouveau_fence_context_new(chan, &fctx->base);
 	fctx->base.emit = nv10_fence_emit;
 	fctx->base.read = nv10_fence_read;
 	fctx->base.sync = nv17_fence_sync;
@@ -95,10 +95,12 @@ nv50_fence_create(struct nouveau_drm *drm)
 	priv->base.resume = nv17_fence_resume;
 	priv->base.context_new = nv50_fence_context_new;
 	priv->base.context_del = nv10_fence_context_del;
+	priv->base.contexts = 127;
+	priv->base.context_base = fence_context_alloc(priv->base.contexts);
 	spin_lock_init(&priv->lock);
 
 	ret = nouveau_bo_new(drm->dev, 4096, 0x1000, TTM_PL_FLAG_VRAM,
-			     0, 0x0000, NULL, &priv->bo);
+			     0, 0x0000, NULL, NULL, &priv->bo);
 	if (!ret) {
 		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
 		if (!ret) {
diff --git a/drivers/gpu/drm/nouveau/nv84_fence.c b/drivers/gpu/drm/nouveau/nv84_fence.c
index 933a779c93ab..d6c6c87c3f07 100644
--- a/drivers/gpu/drm/nouveau/nv84_fence.c
+++ b/drivers/gpu/drm/nouveau/nv84_fence.c
@@ -82,7 +82,7 @@ nv84_fence_emit(struct nouveau_fence *fence)
 	else
 		addr += fctx->vma.offset;
 
-	return fctx->base.emit32(chan, addr, fence->sequence);
+	return fctx->base.emit32(chan, addr, fence->base.seqno);
 }
 
 static int
@@ -97,7 +97,7 @@ nv84_fence_sync(struct nouveau_fence *fence,
 	else
 		addr += fctx->vma.offset;
 
-	return fctx->base.sync32(chan, addr, fence->sequence);
+	return fctx->base.sync32(chan, addr, fence->base.seqno);
 }
 
 static u32
@@ -120,11 +120,12 @@ nv84_fence_context_del(struct nouveau_channel *chan)
 		nouveau_bo_vma_del(bo, &fctx->dispc_vma[i]);
 	}
 
+	nouveau_bo_wr32(priv->bo, chan->chid * 16 / 4, fctx->base.sequence);
 	nouveau_bo_vma_del(priv->bo, &fctx->vma_gart);
 	nouveau_bo_vma_del(priv->bo, &fctx->vma);
 	nouveau_fence_context_del(&fctx->base);
 	chan->fence = NULL;
-	kfree(fctx);
+	nouveau_fence_context_free(&fctx->base);
 }
 
 int
@@ -139,12 +140,13 @@ nv84_fence_context_new(struct nouveau_channel *chan)
 	if (!fctx)
 		return -ENOMEM;
 
-	nouveau_fence_context_new(&fctx->base);
+	nouveau_fence_context_new(chan, &fctx->base);
 	fctx->base.emit = nv84_fence_emit;
 	fctx->base.sync = nv84_fence_sync;
 	fctx->base.read = nv84_fence_read;
 	fctx->base.emit32 = nv84_fence_emit32;
 	fctx->base.sync32 = nv84_fence_sync32;
+	fctx->base.sequence = nv84_fence_read(chan);
 
 	ret = nouveau_bo_vma_add(priv->bo, cli->vm, &fctx->vma);
 	if (ret == 0) {
@@ -158,8 +160,6 @@ nv84_fence_context_new(struct nouveau_channel *chan)
 		ret = nouveau_bo_vma_add(bo, cli->vm, &fctx->dispc_vma[i]);
 	}
 
-	nouveau_bo_wr32(priv->bo, chan->chid * 16/4, 0x00000000);
-
 	if (ret)
 		nv84_fence_context_del(chan);
 	return ret;
@@ -168,13 +168,12 @@ nv84_fence_context_new(struct nouveau_channel *chan)
 static bool
 nv84_fence_suspend(struct nouveau_drm *drm)
 {
-	struct nouveau_fifo *pfifo = nvkm_fifo(&drm->device);
 	struct nv84_fence_priv *priv = drm->fence;
 	int i;
 
-	priv->suspend = vmalloc((pfifo->max + 1) * sizeof(u32));
+	priv->suspend = vmalloc(priv->base.contexts * sizeof(u32));
 	if (priv->suspend) {
-		for (i = 0; i <= pfifo->max; i++)
+		for (i = 0; i < priv->base.contexts; i++)
 			priv->suspend[i] = nouveau_bo_rd32(priv->bo, i*4);
 	}
 
@@ -184,12 +183,11 @@ nv84_fence_suspend(struct nouveau_drm *drm)
 static void
 nv84_fence_resume(struct nouveau_drm *drm)
 {
-	struct nouveau_fifo *pfifo = nvkm_fifo(&drm->device);
 	struct nv84_fence_priv *priv = drm->fence;
 	int i;
 
 	if (priv->suspend) {
-		for (i = 0; i <= pfifo->max; i++)
+		for (i = 0; i < priv->base.contexts; i++)
 			nouveau_bo_wr32(priv->bo, i*4, priv->suspend[i]);
 		vfree(priv->suspend);
 		priv->suspend = NULL;
@@ -229,11 +227,12 @@ nv84_fence_create(struct nouveau_drm *drm)
 	priv->base.context_new = nv84_fence_context_new;
 	priv->base.context_del = nv84_fence_context_del;
 
-	init_waitqueue_head(&priv->base.waiting);
+	priv->base.contexts = pfifo->max + 1;
+	priv->base.context_base = fence_context_alloc(priv->base.contexts);
 	priv->base.uevent = true;
 
-	ret = nouveau_bo_new(drm->dev, 16 * (pfifo->max + 1), 0,
-			     TTM_PL_FLAG_VRAM, 0, 0, NULL, &priv->bo);
+	ret = nouveau_bo_new(drm->dev, 16 * priv->base.contexts, 0,
+			     TTM_PL_FLAG_VRAM, 0, 0, NULL, NULL, &priv->bo);
 	if (ret == 0) {
 		ret = nouveau_bo_pin(priv->bo, TTM_PL_FLAG_VRAM);
 		if (ret == 0) {
@@ -246,8 +245,8 @@ nv84_fence_create(struct nouveau_drm *drm)
 	}
 
 	if (ret == 0)
-		ret = nouveau_bo_new(drm->dev, 16 * (pfifo->max + 1), 0,
-				     TTM_PL_FLAG_TT, 0, 0, NULL,
+		ret = nouveau_bo_new(drm->dev, 16 * priv->base.contexts, 0,
+				     TTM_PL_FLAG_TT, 0, 0, NULL, NULL,
 				     &priv->bo_gart);
 	if (ret == 0) {
 		ret = nouveau_bo_pin(priv->bo_gart, TTM_PL_FLAG_TT);
diff --git a/drivers/gpu/drm/nouveau/nvif/class.h b/drivers/gpu/drm/nouveau/nvif/class.h
index 573491f84792..e5a27df0672b 100644
--- a/drivers/gpu/drm/nouveau/nvif/class.h
+++ b/drivers/gpu/drm/nouveau/nvif/class.h
@@ -479,6 +479,8 @@ struct nv50_disp_core_channel_dma_v0 {
 	__u32 pushbuf;
 };
 
+#define NV50_DISP_CORE_CHANNEL_DMA_V0_NTFY_UEVENT                          0x00
+
 /* cursor immediate */
 struct nv50_disp_cursor_v0 {
 	__u8  version;
@@ -486,6 +488,8 @@ struct nv50_disp_cursor_v0 {
 	__u8  pad02[6];
 };
 
+#define NV50_DISP_CURSOR_V0_NTFY_UEVENT                                    0x00
+
 /* base */
 struct nv50_disp_base_channel_dma_v0 {
 	__u8  version;
@@ -494,6 +498,8 @@ struct nv50_disp_base_channel_dma_v0 {
 	__u32 pushbuf;
 };
 
+#define NV50_DISP_BASE_CHANNEL_DMA_V0_NTFY_UEVENT                          0x00
+
 /* overlay */
 struct nv50_disp_overlay_channel_dma_v0 {
 	__u8  version;
@@ -502,6 +508,8 @@ struct nv50_disp_overlay_channel_dma_v0 {
 	__u32 pushbuf;
 };
 
+#define NV50_DISP_OVERLAY_CHANNEL_DMA_V0_NTFY_UEVENT                       0x00
+
 /* overlay immediate */
 struct nv50_disp_overlay_v0 {
 	__u8  version;
@@ -509,6 +517,7 @@ struct nv50_disp_overlay_v0 {
 	__u8  pad02[6];
 };
 
+#define NV50_DISP_OVERLAY_V0_NTFY_UEVENT                                   0x00
 
 /*******************************************************************************
  * fermi
diff --git a/drivers/gpu/drm/nouveau/nvif/driver.h b/drivers/gpu/drm/nouveau/nvif/driver.h
index b72a8f0c2758..ac4bdb3ea506 100644
--- a/drivers/gpu/drm/nouveau/nvif/driver.h
+++ b/drivers/gpu/drm/nouveau/nvif/driver.h
@@ -9,8 +9,8 @@ struct nvif_driver {
 	int (*suspend)(void *priv);
 	int (*resume)(void *priv);
 	int (*ioctl)(void *priv, bool super, void *data, u32 size, void **hack);
-	void *(*map)(void *priv, u64 handle, u32 size);
-	void (*unmap)(void *priv, void *ptr, u32 size);
+	void __iomem *(*map)(void *priv, u64 handle, u32 size);
+	void (*unmap)(void *priv, void __iomem *ptr, u32 size);
 	bool keep;
 };
 
diff --git a/drivers/gpu/drm/nouveau/nvif/object.h b/drivers/gpu/drm/nouveau/nvif/object.h
index fac3a3bbec44..fe519179b76c 100644
--- a/drivers/gpu/drm/nouveau/nvif/object.h
+++ b/drivers/gpu/drm/nouveau/nvif/object.h
@@ -14,7 +14,7 @@ struct nvif_object {
 	void *priv; /*XXX: hack */
 	void (*dtor)(struct nvif_object *);
 	struct {
-		void *ptr;
+		void __iomem *ptr;
 		u32 size;
 	} map;
 };
@@ -42,7 +42,7 @@ void nvif_object_unmap(struct nvif_object *);
 	struct nvif_object *_object = nvif_object(a);                          \
 	u32 _data;                                                             \
 	if (likely(_object->map.ptr))                                          \
-		_data = ioread##b##_native((u8 *)_object->map.ptr + (c));      \
+		_data = ioread##b##_native((u8 __iomem *)_object->map.ptr + (c));      \
 	else                                                                   \
 		_data = nvif_object_rd(_object, (b) / 8, (c));                 \
 	_data;                                                                 \
@@ -50,7 +50,7 @@ void nvif_object_unmap(struct nvif_object *);
 #define nvif_wr(a,b,c,d) ({                                                    \
 	struct nvif_object *_object = nvif_object(a);                          \
 	if (likely(_object->map.ptr))                                          \
-		iowrite##b##_native((d), (u8 *)_object->map.ptr + (c));        \
+		iowrite##b##_native((d), (u8 __iomem *)_object->map.ptr + (c));        \
 	else                                                                   \
 		nvif_object_wr(_object, (b) / 8, (c), (d));                    \
 })
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.c b/drivers/gpu/drm/omapdrm/omap_drv.c
index 002b9721e85a..862ba03c236c 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.c
+++ b/drivers/gpu/drm/omapdrm/omap_drv.c
@@ -629,6 +629,7 @@ static struct drm_driver omap_drm_driver = {
 		.lastclose = dev_lastclose,
 		.preclose = dev_preclose,
 		.postclose = dev_postclose,
+		.set_busid = drm_platform_set_busid,
 		.get_vblank_counter = drm_vblank_count,
 		.enable_vblank = omap_irq_enable_vblank,
 		.disable_vblank = omap_irq_disable_vblank,
diff --git a/drivers/gpu/drm/omapdrm/omap_drv.h b/drivers/gpu/drm/omapdrm/omap_drv.h
index 84d73a61b34b..60e47b33c801 100644
--- a/drivers/gpu/drm/omapdrm/omap_drv.h
+++ b/drivers/gpu/drm/omapdrm/omap_drv.h
@@ -26,6 +26,7 @@
 #include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 #include <drm/omap_drm.h>
+#include <drm/drm_gem.h>
 #include <linux/platform_data/omap_drm.h>
 
 
diff --git a/drivers/gpu/drm/panel/panel-simple.c b/drivers/gpu/drm/panel/panel-simple.c
index 4ce1db0a68ff..23de22f8c820 100644
--- a/drivers/gpu/drm/panel/panel-simple.c
+++ b/drivers/gpu/drm/panel/panel-simple.c
@@ -352,6 +352,30 @@ static const struct panel_desc auo_b101aw03 = {
 	},
 };
 
+static const struct drm_display_mode auo_b101xtn01_mode = {
+	.clock = 72000,
+	.hdisplay = 1366,
+	.hsync_start = 1366 + 20,
+	.hsync_end = 1366 + 20 + 70,
+	.htotal = 1366 + 20 + 70,
+	.vdisplay = 768,
+	.vsync_start = 768 + 14,
+	.vsync_end = 768 + 14 + 42,
+	.vtotal = 768 + 14 + 42,
+	.vrefresh = 60,
+	.flags = DRM_MODE_FLAG_NVSYNC | DRM_MODE_FLAG_NHSYNC,
+};
+
+static const struct panel_desc auo_b101xtn01 = {
+	.modes = &auo_b101xtn01_mode,
+	.num_modes = 1,
+	.bpc = 6,
+	.size = {
+		.width = 223,
+		.height = 125,
+	},
+};
+
 static const struct drm_display_mode auo_b133xtn01_mode = {
 	.clock = 69500,
 	.hdisplay = 1366,
@@ -616,6 +640,9 @@ static const struct of_device_id platform_of_match[] = {
 		.compatible = "auo,b101aw03",
 		.data = &auo_b101aw03,
 	}, {
+		.compatible = "auo,b101xtn01",
+		.data = &auo_b101xtn01,
+	}, {
 		.compatible = "auo,b133htn01",
 		.data = &auo_b133htn01,
 	}, {
diff --git a/drivers/gpu/drm/qxl/Makefile b/drivers/gpu/drm/qxl/Makefile
index ea046ba691d2..bacc4aff1201 100644
--- a/drivers/gpu/drm/qxl/Makefile
+++ b/drivers/gpu/drm/qxl/Makefile
@@ -4,6 +4,6 @@
 
 ccflags-y := -Iinclude/drm
 
-qxl-y := qxl_drv.o qxl_kms.o qxl_display.o qxl_ttm.o qxl_fb.o qxl_object.o qxl_gem.o qxl_cmd.o qxl_image.o qxl_draw.o qxl_debugfs.o qxl_irq.o qxl_dumb.o qxl_ioctl.o qxl_fence.o qxl_release.o
+qxl-y := qxl_drv.o qxl_kms.o qxl_display.o qxl_ttm.o qxl_fb.o qxl_object.o qxl_gem.o qxl_cmd.o qxl_image.o qxl_draw.o qxl_debugfs.o qxl_irq.o qxl_dumb.o qxl_ioctl.o qxl_release.o qxl_prime.o
 
 obj-$(CONFIG_DRM_QXL)+= qxl.o
diff --git a/drivers/gpu/drm/qxl/qxl_cmd.c b/drivers/gpu/drm/qxl/qxl_cmd.c
index eb89653a7a17..97823644d347 100644
--- a/drivers/gpu/drm/qxl/qxl_cmd.c
+++ b/drivers/gpu/drm/qxl/qxl_cmd.c
@@ -620,17 +620,10 @@ static int qxl_reap_surf(struct qxl_device *qdev, struct qxl_bo *surf, bool stal
 	if (ret == -EBUSY)
 		return -EBUSY;
 
-	if (surf->fence.num_active_releases > 0 && stall == false) {
-		qxl_bo_unreserve(surf);
-		return -EBUSY;
-	}
-
 	if (stall)
 		mutex_unlock(&qdev->surf_evict_mutex);
 
-	spin_lock(&surf->tbo.bdev->fence_lock);
 	ret = ttm_bo_wait(&surf->tbo, true, true, !stall);
-	spin_unlock(&surf->tbo.bdev->fence_lock);
 
 	if (stall)
 		mutex_lock(&qdev->surf_evict_mutex);
diff --git a/drivers/gpu/drm/qxl/qxl_debugfs.c b/drivers/gpu/drm/qxl/qxl_debugfs.c
index c3c2bbdc6674..6911b8c44492 100644
--- a/drivers/gpu/drm/qxl/qxl_debugfs.c
+++ b/drivers/gpu/drm/qxl/qxl_debugfs.c
@@ -58,9 +58,17 @@ qxl_debugfs_buffers_info(struct seq_file *m, void *data)
 	struct qxl_bo *bo;
 
 	list_for_each_entry(bo, &qdev->gem.objects, list) {
-		seq_printf(m, "size %ld, pc %d, sync obj %p, num releases %d\n",
-			   (unsigned long)bo->gem_base.size, bo->pin_count,
-			   bo->tbo.sync_obj, bo->fence.num_active_releases);
+		struct reservation_object_list *fobj;
+		int rel;
+
+		rcu_read_lock();
+		fobj = rcu_dereference(bo->tbo.resv->fence);
+		rel = fobj ? fobj->shared_count : 0;
+		rcu_read_unlock();
+
+		seq_printf(m, "size %ld, pc %d, num releases %d\n",
+			   (unsigned long)bo->gem_base.size,
+			   bo->pin_count, rel);
 	}
 	return 0;
 }
diff --git a/drivers/gpu/drm/qxl/qxl_display.c b/drivers/gpu/drm/qxl/qxl_display.c
index b8ced08b6291..0d1396266857 100644
--- a/drivers/gpu/drm/qxl/qxl_display.c
+++ b/drivers/gpu/drm/qxl/qxl_display.c
@@ -187,6 +187,54 @@ static void qxl_crtc_destroy(struct drm_crtc *crtc)
 	kfree(qxl_crtc);
 }
 
+static int qxl_crtc_page_flip(struct drm_crtc *crtc,
+                              struct drm_framebuffer *fb,
+                              struct drm_pending_vblank_event *event,
+                              uint32_t page_flip_flags)
+{
+	struct drm_device *dev = crtc->dev;
+	struct qxl_device *qdev = dev->dev_private;
+	struct qxl_crtc *qcrtc = to_qxl_crtc(crtc);
+	struct qxl_framebuffer *qfb_src = to_qxl_framebuffer(fb);
+	struct qxl_framebuffer *qfb_old = to_qxl_framebuffer(crtc->primary->fb);
+	struct qxl_bo *bo_old = gem_to_qxl_bo(qfb_old->obj);
+	struct qxl_bo *bo = gem_to_qxl_bo(qfb_src->obj);
+	unsigned long flags;
+	struct drm_clip_rect norect = {
+	    .x1 = 0,
+	    .y1 = 0,
+	    .x2 = fb->width,
+	    .y2 = fb->height
+	};
+	int inc = 1;
+	int one_clip_rect = 1;
+	int ret = 0;
+
+	crtc->primary->fb = fb;
+	bo_old->is_primary = false;
+	bo->is_primary = true;
+
+	ret = qxl_bo_reserve(bo, false);
+	if (ret)
+		return ret;
+
+	qxl_draw_dirty_fb(qdev, qfb_src, bo, 0, 0,
+			  &norect, one_clip_rect, inc);
+
+	drm_vblank_get(dev, qcrtc->index);
+
+	if (event) {
+		spin_lock_irqsave(&dev->event_lock, flags);
+		drm_send_vblank_event(dev, qcrtc->index, event);
+		spin_unlock_irqrestore(&dev->event_lock, flags);
+	}
+	drm_vblank_put(dev, qcrtc->index);
+
+	qxl_bo_unreserve(bo);
+
+	return 0;
+}
+
 static int
 qxl_hide_cursor(struct qxl_device *qdev)
 {
@@ -374,6 +422,7 @@ static const struct drm_crtc_funcs qxl_crtc_funcs = {
 	.cursor_move = qxl_crtc_cursor_move,
 	.set_config = drm_crtc_helper_set_config,
 	.destroy = qxl_crtc_destroy,
+	.page_flip = qxl_crtc_page_flip,
 };
 
 static void qxl_user_framebuffer_destroy(struct drm_framebuffer *fb)
@@ -523,7 +572,6 @@ static int qxl_crtc_mode_set(struct drm_crtc *crtc,
 	struct qxl_framebuffer *qfb;
 	struct qxl_bo *bo, *old_bo = NULL;
 	struct qxl_crtc *qcrtc = to_qxl_crtc(crtc);
-	uint32_t width, height, base_offset;
 	bool recreate_primary = false;
 	int ret;
 	int surf_id;
@@ -553,9 +601,10 @@ static int qxl_crtc_mode_set(struct drm_crtc *crtc,
 	if (qcrtc->index == 0)
 		recreate_primary = true;
 
-	width = mode->hdisplay;
-	height = mode->vdisplay;
-	base_offset = 0;
+	if (bo->surf.stride * bo->surf.height > qdev->vram_size) {
+		DRM_ERROR("Mode doesn't fit in vram size (vgamem)");
+		return -EINVAL;
+        }
 
 	ret = qxl_bo_reserve(bo, false);
 	if (ret != 0)
@@ -569,10 +618,10 @@ static int qxl_crtc_mode_set(struct drm_crtc *crtc,
 	if (recreate_primary) {
 		qxl_io_destroy_primary(qdev);
 		qxl_io_log(qdev,
-			   "recreate primary: %dx%d (was %dx%d,%d,%d)\n",
-			   width, height, bo->surf.width,
-			   bo->surf.height, bo->surf.stride, bo->surf.format);
-		qxl_io_create_primary(qdev, base_offset, bo);
+			   "recreate primary: %dx%d,%d,%d\n",
+			   bo->surf.width, bo->surf.height,
+			   bo->surf.stride, bo->surf.format);
+		qxl_io_create_primary(qdev, 0, bo);
 		bo->is_primary = true;
 	}
 
diff --git a/drivers/gpu/drm/qxl/qxl_drv.c b/drivers/gpu/drm/qxl/qxl_drv.c
index a3fd92029a14..1d9b80c91a15 100644
--- a/drivers/gpu/drm/qxl/qxl_drv.c
+++ b/drivers/gpu/drm/qxl/qxl_drv.c
@@ -84,6 +84,7 @@ static const struct file_operations qxl_fops = {
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
 	.poll = drm_poll,
+	.read = drm_read,
 	.mmap = qxl_mmap,
 };
 
@@ -195,6 +196,20 @@ static int qxl_pm_restore(struct device *dev)
 	return qxl_drm_resume(drm_dev, false);
 }
 
+static u32 qxl_noop_get_vblank_counter(struct drm_device *dev, int crtc)
+{
+	return dev->vblank[crtc].count.counter;
+}
+
+static int qxl_noop_enable_vblank(struct drm_device *dev, int crtc)
+{
+	return 0;
+}
+
+static void qxl_noop_disable_vblank(struct drm_device *dev, int crtc)
+{
+}
+
 static const struct dev_pm_ops qxl_pm_ops = {
 	.suspend = qxl_pm_suspend,
 	.resume = qxl_pm_resume,
@@ -212,10 +227,15 @@ static struct pci_driver qxl_pci_driver = {
 };
 
 static struct drm_driver qxl_driver = {
-	.driver_features = DRIVER_GEM | DRIVER_MODESET |
+	.driver_features = DRIVER_GEM | DRIVER_MODESET | DRIVER_PRIME |
 			   DRIVER_HAVE_IRQ | DRIVER_IRQ_SHARED,
 	.load = qxl_driver_load,
 	.unload = qxl_driver_unload,
+	.get_vblank_counter = qxl_noop_get_vblank_counter,
+	.enable_vblank = qxl_noop_enable_vblank,
+	.disable_vblank = qxl_noop_disable_vblank,
+
+	.set_busid = drm_pci_set_busid,
 
 	.dumb_create = qxl_mode_dumb_create,
 	.dumb_map_offset = qxl_mode_dumb_mmap,
@@ -224,6 +244,17 @@ static struct drm_driver qxl_driver = {
 	.debugfs_init = qxl_debugfs_init,
 	.debugfs_cleanup = qxl_debugfs_takedown,
 #endif
+	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
+	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
+	.gem_prime_export = drm_gem_prime_export,
+	.gem_prime_import = drm_gem_prime_import,
+	.gem_prime_pin = qxl_gem_prime_pin,
+	.gem_prime_unpin = qxl_gem_prime_unpin,
+	.gem_prime_get_sg_table = qxl_gem_prime_get_sg_table,
+	.gem_prime_import_sg_table = qxl_gem_prime_import_sg_table,
+	.gem_prime_vmap = qxl_gem_prime_vmap,
+	.gem_prime_vunmap = qxl_gem_prime_vunmap,
+	.gem_prime_mmap = qxl_gem_prime_mmap,
 	.gem_free_object = qxl_gem_object_free,
 	.gem_open_object = qxl_gem_object_open,
 	.gem_close_object = qxl_gem_object_close,
diff --git a/drivers/gpu/drm/qxl/qxl_drv.h b/drivers/gpu/drm/qxl/qxl_drv.h
index 36ed40ba773f..7c6cafe21f5f 100644
--- a/drivers/gpu/drm/qxl/qxl_drv.h
+++ b/drivers/gpu/drm/qxl/qxl_drv.h
@@ -31,6 +31,7 @@
  * Definitions taken from spice-protocol, plus kernel driver specific bits.
  */
 
+#include <linux/fence.h>
 #include <linux/workqueue.h>
 #include <linux/firmware.h>
 #include <linux/platform_device.h>
@@ -42,6 +43,8 @@
 #include <ttm/ttm_placement.h>
 #include <ttm/ttm_module.h>
 
+#include <drm/drm_gem.h>
+
 /* just for ttm_validate_buffer */
 #include <ttm/ttm_execbuf_util.h>
 
@@ -95,31 +98,24 @@ enum {
 	QXL_INTERRUPT_IO_CMD |\
 	QXL_INTERRUPT_CLIENT_MONITORS_CONFIG)
 
-struct qxl_fence {
-	struct qxl_device *qdev;
-	uint32_t num_active_releases;
-	uint32_t *release_ids;
-	struct radix_tree_root tree;
-};
-
 struct qxl_bo {
 	/* Protected by gem.mutex */
 	struct list_head		list;
 	/* Protected by tbo.reserved */
-	u32				placements[3];
+	struct ttm_place		placements[3];
 	struct ttm_placement		placement;
 	struct ttm_buffer_object	tbo;
 	struct ttm_bo_kmap_obj		kmap;
 	unsigned			pin_count;
 	void				*kptr;
 	int                             type;
+
 	/* Constant after initialization */
 	struct drm_gem_object		gem_base;
 	bool is_primary; /* is this now a primary surface */
 	bool hw_surf_alloc;
 	struct qxl_surface surf;
 	uint32_t surface_id;
-	struct qxl_fence fence; /* per bo fence  - list of releases */
 	struct qxl_release *surf_create;
 };
 #define gem_to_qxl_bo(gobj) container_of((gobj), struct qxl_bo, gem_base)
@@ -191,6 +187,8 @@ enum {
  * spice-protocol/qxl_dev.h */
 #define QXL_MAX_RES 96
 struct qxl_release {
+	struct fence base;
+
 	int id;
 	int type;
 	uint32_t release_offset;
@@ -284,7 +282,9 @@ struct qxl_device {
 	uint8_t		slot_gen_bits;
 	uint64_t	va_slot_mask;
 
+	spinlock_t	release_lock;
 	struct idr	release_idr;
+	uint32_t	release_seqno;
 	spinlock_t release_idr_lock;
 	struct mutex	async_io_mutex;
 	unsigned int last_sent_io_cmd;
@@ -532,6 +532,18 @@ int qxl_garbage_collect(struct qxl_device *qdev);
 int qxl_debugfs_init(struct drm_minor *minor);
 void qxl_debugfs_takedown(struct drm_minor *minor);
 
+/* qxl_prime.c */
+int qxl_gem_prime_pin(struct drm_gem_object *obj);
+void qxl_gem_prime_unpin(struct drm_gem_object *obj);
+struct sg_table *qxl_gem_prime_get_sg_table(struct drm_gem_object *obj);
+struct drm_gem_object *qxl_gem_prime_import_sg_table(
+	struct drm_device *dev, struct dma_buf_attachment *attach,
+	struct sg_table *sgt);
+void *qxl_gem_prime_vmap(struct drm_gem_object *obj);
+void qxl_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr);
+int qxl_gem_prime_mmap(struct drm_gem_object *obj,
+				struct vm_area_struct *vma);
+
 /* qxl_irq.c */
 int qxl_irq_init(struct qxl_device *qdev);
 irqreturn_t qxl_irq_handler(int irq, void *arg);
@@ -561,10 +573,4 @@ qxl_surface_lookup(struct drm_device *dev, int surface_id);
 void qxl_surface_evict(struct qxl_device *qdev, struct qxl_bo *surf, bool freeing);
 int qxl_update_surface(struct qxl_device *qdev, struct qxl_bo *surf);
 
-/* qxl_fence.c */
-void qxl_fence_add_release_locked(struct qxl_fence *qfence, uint32_t rel_id);
-int qxl_fence_remove_release(struct qxl_fence *qfence, uint32_t rel_id);
-int qxl_fence_init(struct qxl_device *qdev, struct qxl_fence *qfence);
-void qxl_fence_fini(struct qxl_fence *qfence);
-
 #endif
diff --git a/drivers/gpu/drm/qxl/qxl_fb.c b/drivers/gpu/drm/qxl/qxl_fb.c
index df567888bb1e..3d7c1d00a424 100644
--- a/drivers/gpu/drm/qxl/qxl_fb.c
+++ b/drivers/gpu/drm/qxl/qxl_fb.c
@@ -625,7 +625,8 @@ static int qxl_fb_find_or_create_single(
 		struct drm_fb_helper *helper,
 		struct drm_fb_helper_surface_size *sizes)
 {
-	struct qxl_fbdev *qfbdev = (struct qxl_fbdev *)helper;
+	struct qxl_fbdev *qfbdev =
+		container_of(helper, struct qxl_fbdev, helper);
 	int new_fb = 0;
 	int ret;
 
diff --git a/drivers/gpu/drm/qxl/qxl_fence.c b/drivers/gpu/drm/qxl/qxl_fence.c
deleted file mode 100644
index ae59e91cfb9a..000000000000
--- a/drivers/gpu/drm/qxl/qxl_fence.c
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * Copyright 2013 Red Hat Inc.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- *
- * Authors: Dave Airlie
- *          Alon Levy
- */
-
-
-#include "qxl_drv.h"
-
-/* QXL fencing-
-
-   When we submit operations to the GPU we pass a release reference to the GPU
-   with them, the release reference is then added to the release ring when
-   the GPU is finished with that particular operation and has removed it from
-   its tree.
-
-   So we have can have multiple outstanding non linear fences per object.
-
-   From a TTM POV we only care if the object has any outstanding releases on
-   it.
-
-   we wait until all outstanding releases are processeed.
-
-   sync object is just a list of release ids that represent that fence on
-   that buffer.
-
-   we just add new releases onto the sync object attached to the object.
-
-   This currently uses a radix tree to store the list of release ids.
-
-   For some reason every so often qxl hw fails to release, things go wrong.
-*/
-/* must be called with the fence lock held */
-void qxl_fence_add_release_locked(struct qxl_fence *qfence, uint32_t rel_id)
-{
-	radix_tree_insert(&qfence->tree, rel_id, qfence);
-	qfence->num_active_releases++;
-}
-
-int qxl_fence_remove_release(struct qxl_fence *qfence, uint32_t rel_id)
-{
-	void *ret;
-	int retval = 0;
-	struct qxl_bo *bo = container_of(qfence, struct qxl_bo, fence);
-
-	spin_lock(&bo->tbo.bdev->fence_lock);
-
-	ret = radix_tree_delete(&qfence->tree, rel_id);
-	if (ret == qfence)
-		qfence->num_active_releases--;
-	else {
-		DRM_DEBUG("didn't find fence in radix tree for %d\n", rel_id);
-		retval = -ENOENT;
-	}
-	spin_unlock(&bo->tbo.bdev->fence_lock);
-	return retval;
-}
-
-
-int qxl_fence_init(struct qxl_device *qdev, struct qxl_fence *qfence)
-{
-	qfence->qdev = qdev;
-	qfence->num_active_releases = 0;
-	INIT_RADIX_TREE(&qfence->tree, GFP_ATOMIC);
-	return 0;
-}
-
-void qxl_fence_fini(struct qxl_fence *qfence)
-{
-	kfree(qfence->release_ids);
-	qfence->num_active_releases = 0;
-}
diff --git a/drivers/gpu/drm/qxl/qxl_kms.c b/drivers/gpu/drm/qxl/qxl_kms.c
index fd88eb4a3f79..b2977a181935 100644
--- a/drivers/gpu/drm/qxl/qxl_kms.c
+++ b/drivers/gpu/drm/qxl/qxl_kms.c
@@ -223,6 +223,7 @@ static int qxl_device_init(struct qxl_device *qdev,
 
 	idr_init(&qdev->release_idr);
 	spin_lock_init(&qdev->release_idr_lock);
+	spin_lock_init(&qdev->release_lock);
 
 	idr_init(&qdev->surf_id_idr);
 	spin_lock_init(&qdev->surf_id_idr_lock);
@@ -297,6 +298,9 @@ int qxl_driver_unload(struct drm_device *dev)
 
 	if (qdev == NULL)
 		return 0;
+
+	drm_vblank_cleanup(dev);
+
 	qxl_modeset_fini(qdev);
 	qxl_device_fini(qdev);
 
@@ -324,15 +328,20 @@ int qxl_driver_load(struct drm_device *dev, unsigned long flags)
 	if (r)
 		goto out;
 
+	r = drm_vblank_init(dev, 1);
+	if (r)
+		goto unload;
+
 	r = qxl_modeset_init(qdev);
-	if (r) {
-		qxl_driver_unload(dev);
-		goto out;
-	}
+	if (r)
+		goto unload;
 
 	drm_kms_helper_poll_init(qdev->ddev);
 
 	return 0;
+unload:
+	qxl_driver_unload(dev);
+
 out:
 	kfree(qdev);
 	return r;
diff --git a/drivers/gpu/drm/qxl/qxl_object.c b/drivers/gpu/drm/qxl/qxl_object.c
index b95f144f0b49..cdeaf08fdc74 100644
--- a/drivers/gpu/drm/qxl/qxl_object.c
+++ b/drivers/gpu/drm/qxl/qxl_object.c
@@ -36,7 +36,6 @@ static void qxl_ttm_bo_destroy(struct ttm_buffer_object *tbo)
 	qdev = (struct qxl_device *)bo->gem_base.dev->dev_private;
 
 	qxl_surface_evict(qdev, bo, false);
-	qxl_fence_fini(&bo->fence);
 	mutex_lock(&qdev->gem.mutex);
 	list_del_init(&bo->list);
 	mutex_unlock(&qdev->gem.mutex);
@@ -55,21 +54,24 @@ void qxl_ttm_placement_from_domain(struct qxl_bo *qbo, u32 domain, bool pinned)
 {
 	u32 c = 0;
 	u32 pflag = pinned ? TTM_PL_FLAG_NO_EVICT : 0;
+	unsigned i;
 
-	qbo->placement.fpfn = 0;
-	qbo->placement.lpfn = 0;
 	qbo->placement.placement = qbo->placements;
 	qbo->placement.busy_placement = qbo->placements;
 	if (domain == QXL_GEM_DOMAIN_VRAM)
-		qbo->placements[c++] = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_VRAM | pflag;
+		qbo->placements[c++].flags = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_VRAM | pflag;
 	if (domain == QXL_GEM_DOMAIN_SURFACE)
-		qbo->placements[c++] = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_PRIV0 | pflag;
+		qbo->placements[c++].flags = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_PRIV0 | pflag;
 	if (domain == QXL_GEM_DOMAIN_CPU)
-		qbo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM | pflag;
+		qbo->placements[c++].flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM | pflag;
 	if (!c)
-		qbo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		qbo->placements[c++].flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
 	qbo->placement.num_placement = c;
 	qbo->placement.num_busy_placement = c;
+	for (i = 0; i < c; ++i) {
+		qbo->placements[i].fpfn = 0;
+		qbo->placements[i].lpfn = 0;
+	}
 }
 
 
@@ -99,7 +101,6 @@ int qxl_bo_create(struct qxl_device *qdev,
 	bo->type = domain;
 	bo->pin_count = pinned ? 1 : 0;
 	bo->surface_id = 0;
-	qxl_fence_init(qdev, &bo->fence);
 	INIT_LIST_HEAD(&bo->list);
 
 	if (surf)
@@ -109,7 +110,7 @@ int qxl_bo_create(struct qxl_device *qdev,
 
 	r = ttm_bo_init(&qdev->mman.bdev, &bo->tbo, size, type,
 			&bo->placement, 0, !kernel, NULL, size,
-			NULL, &qxl_ttm_bo_destroy);
+			NULL, NULL, &qxl_ttm_bo_destroy);
 	if (unlikely(r != 0)) {
 		if (r != -ERESTARTSYS)
 			dev_err(qdev->dev,
@@ -259,7 +260,7 @@ int qxl_bo_unpin(struct qxl_bo *bo)
 	if (bo->pin_count)
 		return 0;
 	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+		bo->placements[i].flags &= ~TTM_PL_FLAG_NO_EVICT;
 	r = ttm_bo_validate(&bo->tbo, &bo->placement, false, false);
 	if (unlikely(r != 0))
 		dev_err(qdev->dev, "%p validate failed for unpin\n", bo);
diff --git a/drivers/gpu/drm/qxl/qxl_object.h b/drivers/gpu/drm/qxl/qxl_object.h
index 83a423293afd..37af1bc0dd00 100644
--- a/drivers/gpu/drm/qxl/qxl_object.h
+++ b/drivers/gpu/drm/qxl/qxl_object.h
@@ -76,12 +76,10 @@ static inline int qxl_bo_wait(struct qxl_bo *bo, u32 *mem_type,
 		}
 		return r;
 	}
-	spin_lock(&bo->tbo.bdev->fence_lock);
 	if (mem_type)
 		*mem_type = bo->tbo.mem.mem_type;
-	if (bo->tbo.sync_obj)
-		r = ttm_bo_wait(&bo->tbo, true, true, no_wait);
-	spin_unlock(&bo->tbo.bdev->fence_lock);
+
+	r = ttm_bo_wait(&bo->tbo, true, true, no_wait);
 	ttm_bo_unreserve(&bo->tbo);
 	return r;
 }
diff --git a/drivers/gpu/drm/qxl/qxl_prime.c b/drivers/gpu/drm/qxl/qxl_prime.c
new file mode 100644
index 000000000000..3d031b50a8fd
--- /dev/null
+++ b/drivers/gpu/drm/qxl/qxl_prime.c
@@ -0,0 +1,72 @@
+/*
+ * Copyright 2014 Canonical
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
+ * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Authors: Andreas Pokorny
+ */
+
+#include "qxl_drv.h"
+
+/* Empty Implementations as there should not be any other driver for a virtual
+ * device that might share buffers with qxl */
+
+int qxl_gem_prime_pin(struct drm_gem_object *obj)
+{
+	WARN_ONCE(1, "not implemented");
+	return -ENOSYS;
+}
+
+void qxl_gem_prime_unpin(struct drm_gem_object *obj)
+{
+	WARN_ONCE(1, "not implemented");
+}
+
+
+struct sg_table *qxl_gem_prime_get_sg_table(struct drm_gem_object *obj)
+{
+	WARN_ONCE(1, "not implemented");
+	return ERR_PTR(-ENOSYS);
+}
+
+struct drm_gem_object *qxl_gem_prime_import_sg_table(
+	struct drm_device *dev, struct dma_buf_attachment *attach,
+	struct sg_table *table)
+{
+	WARN_ONCE(1, "not implemented");
+	return ERR_PTR(-ENOSYS);
+}
+
+void *qxl_gem_prime_vmap(struct drm_gem_object *obj)
+{
+	WARN_ONCE(1, "not implemented");
+	return ERR_PTR(-ENOSYS);
+}
+
+void qxl_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
+{
+	WARN_ONCE(1, "not implemented");
+}
+
+int qxl_gem_prime_mmap(struct drm_gem_object *obj,
+		       struct vm_area_struct *area)
+{
+	WARN_ONCE(1, "not implemented");
+	return ENOSYS;
+}
diff --git a/drivers/gpu/drm/qxl/qxl_release.c b/drivers/gpu/drm/qxl/qxl_release.c
index 14e776f1d14e..446e71ca36cb 100644
--- a/drivers/gpu/drm/qxl/qxl_release.c
+++ b/drivers/gpu/drm/qxl/qxl_release.c
@@ -21,6 +21,7 @@
  */
 #include "qxl_drv.h"
 #include "qxl_object.h"
+#include <trace/events/fence.h>
 
 /*
  * drawable cmd cache - allocate a bunch of VRAM pages, suballocate
@@ -39,6 +40,88 @@
 static const int release_size_per_bo[] = { RELEASE_SIZE, SURFACE_RELEASE_SIZE, RELEASE_SIZE };
 static const int releases_per_bo[] = { RELEASES_PER_BO, SURFACE_RELEASES_PER_BO, RELEASES_PER_BO };
 
+static const char *qxl_get_driver_name(struct fence *fence)
+{
+	return "qxl";
+}
+
+static const char *qxl_get_timeline_name(struct fence *fence)
+{
+	return "release";
+}
+
+static bool qxl_nop_signaling(struct fence *fence)
+{
+	/* fences are always automatically signaled, so just pretend we did this.. */
+	return true;
+}
+
+static long qxl_fence_wait(struct fence *fence, bool intr, signed long timeout)
+{
+	struct qxl_device *qdev;
+	struct qxl_release *release;
+	int count = 0, sc = 0;
+	bool have_drawable_releases;
+	unsigned long cur, end = jiffies + timeout;
+
+	qdev = container_of(fence->lock, struct qxl_device, release_lock);
+	release = container_of(fence, struct qxl_release, base);
+	have_drawable_releases = release->type == QXL_RELEASE_DRAWABLE;
+
+retry:
+	sc++;
+
+	if (fence_is_signaled(fence))
+		goto signaled;
+
+	qxl_io_notify_oom(qdev);
+
+	for (count = 0; count < 11; count++) {
+		if (!qxl_queue_garbage_collect(qdev, true))
+			break;
+
+		if (fence_is_signaled(fence))
+			goto signaled;
+	}
+
+	if (fence_is_signaled(fence))
+		goto signaled;
+
+	if (have_drawable_releases || sc < 4) {
+		if (sc > 2)
+			/* back off */
+			usleep_range(500, 1000);
+
+		if (time_after(jiffies, end))
+			return 0;
+
+		if (have_drawable_releases && sc > 300) {
+			FENCE_WARN(fence, "failed to wait on release %d "
+					  "after spincount %d\n",
+					  fence->context & ~0xf0000000, sc);
+			goto signaled;
+		}
+		goto retry;
+	}
+	/*
+	 * yeah, original sync_obj_wait gave up after 3 spins when
+	 * have_drawable_releases is not set.
+	 */
+
+signaled:
+	cur = jiffies;
+	if (time_after(cur, end))
+		return 0;
+	return end - cur;
+}
+
+static const struct fence_ops qxl_fence_ops = {
+	.get_driver_name = qxl_get_driver_name,
+	.get_timeline_name = qxl_get_timeline_name,
+	.enable_signaling = qxl_nop_signaling,
+	.wait = qxl_fence_wait,
+};
+
 static uint64_t
 qxl_release_alloc(struct qxl_device *qdev, int type,
 		  struct qxl_release **ret)
@@ -46,13 +129,13 @@ qxl_release_alloc(struct qxl_device *qdev, int type,
 	struct qxl_release *release;
 	int handle;
 	size_t size = sizeof(*release);
-	int idr_ret;
 
 	release = kmalloc(size, GFP_KERNEL);
 	if (!release) {
 		DRM_ERROR("Out of memory\n");
 		return 0;
 	}
+	release->base.ops = NULL;
 	release->type = type;
 	release->release_offset = 0;
 	release->surface_release_id = 0;
@@ -60,44 +143,61 @@ qxl_release_alloc(struct qxl_device *qdev, int type,
 
 	idr_preload(GFP_KERNEL);
 	spin_lock(&qdev->release_idr_lock);
-	idr_ret = idr_alloc(&qdev->release_idr, release, 1, 0, GFP_NOWAIT);
+	handle = idr_alloc(&qdev->release_idr, release, 1, 0, GFP_NOWAIT);
+	release->base.seqno = ++qdev->release_seqno;
 	spin_unlock(&qdev->release_idr_lock);
 	idr_preload_end();
-	handle = idr_ret;
-	if (idr_ret < 0)
-		goto release_fail;
+	if (handle < 0) {
+		kfree(release);
+		*ret = NULL;
+		return handle;
+	}
 	*ret = release;
 	QXL_INFO(qdev, "allocated release %lld\n", handle);
 	release->id = handle;
-release_fail:
-
 	return handle;
 }
 
+static void
+qxl_release_free_list(struct qxl_release *release)
+{
+	while (!list_empty(&release->bos)) {
+		struct qxl_bo_list *entry;
+		struct qxl_bo *bo;
+
+		entry = container_of(release->bos.next,
+				     struct qxl_bo_list, tv.head);
+		bo = to_qxl_bo(entry->tv.bo);
+		qxl_bo_unref(&bo);
+		list_del(&entry->tv.head);
+		kfree(entry);
+	}
+}
+
 void
 qxl_release_free(struct qxl_device *qdev,
 		 struct qxl_release *release)
 {
-	struct qxl_bo_list *entry, *tmp;
 	QXL_INFO(qdev, "release %d, type %d\n", release->id,
 		 release->type);
 
 	if (release->surface_release_id)
 		qxl_surface_id_dealloc(qdev, release->surface_release_id);
 
-	list_for_each_entry_safe(entry, tmp, &release->bos, tv.head) {
-		struct qxl_bo *bo = to_qxl_bo(entry->tv.bo);
-		QXL_INFO(qdev, "release %llx\n",
-			drm_vma_node_offset_addr(&entry->tv.bo->vma_node)
-						- DRM_FILE_OFFSET);
-		qxl_fence_remove_release(&bo->fence, release->id);
-		qxl_bo_unref(&bo);
-		kfree(entry);
-	}
 	spin_lock(&qdev->release_idr_lock);
 	idr_remove(&qdev->release_idr, release->id);
 	spin_unlock(&qdev->release_idr_lock);
-	kfree(release);
+
+	if (release->base.ops) {
+		WARN_ON(list_empty(&release->bos));
+		qxl_release_free_list(release);
+
+		fence_signal(&release->base);
+		fence_put(&release->base);
+	} else {
+		qxl_release_free_list(release);
+		kfree(release);
+	}
 }
 
 static int qxl_release_bo_alloc(struct qxl_device *qdev,
@@ -126,6 +226,7 @@ int qxl_release_list_add(struct qxl_release *release, struct qxl_bo *bo)
 
 	qxl_bo_ref(bo);
 	entry->tv.bo = &bo->tbo;
+	entry->tv.shared = false;
 	list_add_tail(&entry->tv.head, &release->bos);
 	return 0;
 }
@@ -142,6 +243,10 @@ static int qxl_release_validate_bo(struct qxl_bo *bo)
 			return ret;
 	}
 
+	ret = reservation_object_reserve_shared(bo->tbo.resv);
+	if (ret)
+		return ret;
+
 	/* allocate a surface for reserved + validated buffers */
 	ret = qxl_bo_check_id(bo->gem_base.dev->dev_private, bo);
 	if (ret)
@@ -159,7 +264,7 @@ int qxl_release_reserve_list(struct qxl_release *release, bool no_intr)
 	if (list_is_singular(&release->bos))
 		return 0;
 
-	ret = ttm_eu_reserve_buffers(&release->ticket, &release->bos);
+	ret = ttm_eu_reserve_buffers(&release->ticket, &release->bos, !no_intr);
 	if (ret)
 		return ret;
 
@@ -199,6 +304,8 @@ int qxl_alloc_surface_release_reserved(struct qxl_device *qdev,
 
 		/* stash the release after the create command */
 		idr_ret = qxl_release_alloc(qdev, QXL_RELEASE_SURFACE_CMD, release);
+		if (idr_ret < 0)
+			return idr_ret;
 		bo = qxl_bo_ref(to_qxl_bo(entry->tv.bo));
 
 		(*release)->release_offset = create_rel->release_offset + 64;
@@ -239,6 +346,11 @@ int qxl_alloc_release_reserved(struct qxl_device *qdev, unsigned long size,
 	}
 
 	idr_ret = qxl_release_alloc(qdev, type, release);
+	if (idr_ret < 0) {
+		if (rbo)
+			*rbo = NULL;
+		return idr_ret;
+	}
 
 	mutex_lock(&qdev->release_mutex);
 	if (qdev->current_release_bo_offset[cur_idx] + 1 >= releases_per_bo[cur_idx]) {
@@ -319,40 +431,44 @@ void qxl_release_unmap(struct qxl_device *qdev,
 
 void qxl_release_fence_buffer_objects(struct qxl_release *release)
 {
-	struct ttm_validate_buffer *entry;
 	struct ttm_buffer_object *bo;
 	struct ttm_bo_global *glob;
 	struct ttm_bo_device *bdev;
 	struct ttm_bo_driver *driver;
 	struct qxl_bo *qbo;
+	struct ttm_validate_buffer *entry;
+	struct qxl_device *qdev;
 
 	/* if only one object on the release its the release itself
 	   since these objects are pinned no need to reserve */
-	if (list_is_singular(&release->bos))
+	if (list_is_singular(&release->bos) || list_empty(&release->bos))
 		return;
 
 	bo = list_first_entry(&release->bos, struct ttm_validate_buffer, head)->bo;
 	bdev = bo->bdev;
+	qdev = container_of(bdev, struct qxl_device, mman.bdev);
+
+	/*
+	 * Since we never really allocated a context and we don't want to conflict,
+	 * set the highest bits. This will break if we really allow exporting of dma-bufs.
+	 */
+	fence_init(&release->base, &qxl_fence_ops, &qdev->release_lock,
+		   release->id | 0xf0000000, release->base.seqno);
+	trace_fence_emit(&release->base);
+
 	driver = bdev->driver;
 	glob = bo->glob;
 
 	spin_lock(&glob->lru_lock);
-	spin_lock(&bdev->fence_lock);
 
 	list_for_each_entry(entry, &release->bos, head) {
 		bo = entry->bo;
 		qbo = to_qxl_bo(bo);
 
-		if (!entry->bo->sync_obj)
-			entry->bo->sync_obj = &qbo->fence;
-
-		qxl_fence_add_release_locked(&qbo->fence, release->id);
-
+		reservation_object_add_shared_fence(bo->resv, &release->base);
 		ttm_bo_add_to_lru(bo);
 		__ttm_bo_unreserve(bo);
-		entry->reserved = false;
 	}
-	spin_unlock(&bdev->fence_lock);
 	spin_unlock(&glob->lru_lock);
 	ww_acquire_fini(&release->ticket);
 }
diff --git a/drivers/gpu/drm/qxl/qxl_ttm.c b/drivers/gpu/drm/qxl/qxl_ttm.c
index 71a1baeac14e..0cbc4c987164 100644
--- a/drivers/gpu/drm/qxl/qxl_ttm.c
+++ b/drivers/gpu/drm/qxl/qxl_ttm.c
@@ -127,7 +127,7 @@ int qxl_mmap(struct file *filp, struct vm_area_struct *vma)
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET)) {
 		pr_info("%s: vma->vm_pgoff (%ld) < DRM_FILE_PAGE_OFFSET\n",
 			__func__, vma->vm_pgoff);
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 	}
 
 	file_priv = filp->private_data;
@@ -188,11 +188,13 @@ static void qxl_evict_flags(struct ttm_buffer_object *bo,
 				struct ttm_placement *placement)
 {
 	struct qxl_bo *qbo;
-	static u32 placements = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+	static struct ttm_place placements = {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM
+	};
 
 	if (!qxl_ttm_bo_is_qxl_bo(bo)) {
-		placement->fpfn = 0;
-		placement->lpfn = 0;
 		placement->placement = &placements;
 		placement->busy_placement = &placements;
 		placement->num_placement = 1;
@@ -355,92 +357,6 @@ static int qxl_bo_move(struct ttm_buffer_object *bo,
 	return ttm_bo_move_memcpy(bo, evict, no_wait_gpu, new_mem);
 }
 
-
-static int qxl_sync_obj_wait(void *sync_obj,
-			     bool lazy, bool interruptible)
-{
-	struct qxl_fence *qfence = (struct qxl_fence *)sync_obj;
-	int count = 0, sc = 0;
-	struct qxl_bo *bo = container_of(qfence, struct qxl_bo, fence);
-
-	if (qfence->num_active_releases == 0)
-		return 0;
-
-retry:
-	if (sc == 0) {
-		if (bo->type == QXL_GEM_DOMAIN_SURFACE)
-			qxl_update_surface(qfence->qdev, bo);
-	} else if (sc >= 1) {
-		qxl_io_notify_oom(qfence->qdev);
-	}
-
-	sc++;
-
-	for (count = 0; count < 10; count++) {
-		bool ret;
-		ret = qxl_queue_garbage_collect(qfence->qdev, true);
-		if (ret == false)
-			break;
-
-		if (qfence->num_active_releases == 0)
-			return 0;
-	}
-
-	if (qfence->num_active_releases) {
-		bool have_drawable_releases = false;
-		void **slot;
-		struct radix_tree_iter iter;
-		int release_id;
-
-		radix_tree_for_each_slot(slot, &qfence->tree, &iter, 0) {
-			struct qxl_release *release;
-
-			release_id = iter.index;
-			release = qxl_release_from_id_locked(qfence->qdev, release_id);
-			if (release == NULL)
-				continue;
-
-			if (release->type == QXL_RELEASE_DRAWABLE)
-				have_drawable_releases = true;
-		}
-
-		qxl_queue_garbage_collect(qfence->qdev, true);
-
-		if (have_drawable_releases || sc < 4) {
-			if (sc > 2)
-				/* back off */
-				usleep_range(500, 1000);
-			if (have_drawable_releases && sc > 300) {
-				WARN(1, "sync obj %d still has outstanding releases %d %d %d %ld %d\n", sc, bo->surface_id, bo->is_primary, bo->pin_count, (unsigned long)bo->gem_base.size, qfence->num_active_releases);
-				return -EBUSY;
-			}
-			goto retry;
-		}
-	}
-	return 0;
-}
-
-static int qxl_sync_obj_flush(void *sync_obj)
-{
-	return 0;
-}
-
-static void qxl_sync_obj_unref(void **sync_obj)
-{
-	*sync_obj = NULL;
-}
-
-static void *qxl_sync_obj_ref(void *sync_obj)
-{
-	return sync_obj;
-}
-
-static bool qxl_sync_obj_signaled(void *sync_obj)
-{
-	struct qxl_fence *qfence = (struct qxl_fence *)sync_obj;
-	return (qfence->num_active_releases == 0);
-}
-
 static void qxl_bo_move_notify(struct ttm_buffer_object *bo,
 			       struct ttm_mem_reg *new_mem)
 {
@@ -467,16 +383,9 @@ static struct ttm_bo_driver qxl_bo_driver = {
 	.verify_access = &qxl_verify_access,
 	.io_mem_reserve = &qxl_ttm_io_mem_reserve,
 	.io_mem_free = &qxl_ttm_io_mem_free,
-	.sync_obj_signaled = &qxl_sync_obj_signaled,
-	.sync_obj_wait = &qxl_sync_obj_wait,
-	.sync_obj_flush = &qxl_sync_obj_flush,
-	.sync_obj_unref = &qxl_sync_obj_unref,
-	.sync_obj_ref = &qxl_sync_obj_ref,
 	.move_notify = &qxl_bo_move_notify,
 };
 
-
-
 int qxl_ttm_init(struct qxl_device *qdev)
 {
 	int r;
diff --git a/drivers/gpu/drm/r128/r128_cce.c b/drivers/gpu/drm/r128/r128_cce.c
index 59459fe4e8c5..2c45ac9c1dc3 100644
--- a/drivers/gpu/drm/r128/r128_cce.c
+++ b/drivers/gpu/drm/r128/r128_cce.c
@@ -452,7 +452,7 @@ static int r128_do_init_cce(struct drm_device *dev, drm_r128_init_t *init)
 	dev_priv->span_pitch_offset_c = (((dev_priv->depth_pitch / 8) << 21) |
 					 (dev_priv->span_offset >> 5));
 
-	dev_priv->sarea = drm_getsarea(dev);
+	dev_priv->sarea = drm_legacy_getsarea(dev);
 	if (!dev_priv->sarea) {
 		DRM_ERROR("could not find sarea!\n");
 		dev->dev_private = (void *)dev_priv;
@@ -460,21 +460,21 @@ static int r128_do_init_cce(struct drm_device *dev, drm_r128_init_t *init)
 		return -EINVAL;
 	}
 
-	dev_priv->mmio = drm_core_findmap(dev, init->mmio_offset);
+	dev_priv->mmio = drm_legacy_findmap(dev, init->mmio_offset);
 	if (!dev_priv->mmio) {
 		DRM_ERROR("could not find mmio region!\n");
 		dev->dev_private = (void *)dev_priv;
 		r128_do_cleanup_cce(dev);
 		return -EINVAL;
 	}
-	dev_priv->cce_ring = drm_core_findmap(dev, init->ring_offset);
+	dev_priv->cce_ring = drm_legacy_findmap(dev, init->ring_offset);
 	if (!dev_priv->cce_ring) {
 		DRM_ERROR("could not find cce ring region!\n");
 		dev->dev_private = (void *)dev_priv;
 		r128_do_cleanup_cce(dev);
 		return -EINVAL;
 	}
-	dev_priv->ring_rptr = drm_core_findmap(dev, init->ring_rptr_offset);
+	dev_priv->ring_rptr = drm_legacy_findmap(dev, init->ring_rptr_offset);
 	if (!dev_priv->ring_rptr) {
 		DRM_ERROR("could not find ring read pointer!\n");
 		dev->dev_private = (void *)dev_priv;
@@ -482,7 +482,7 @@ static int r128_do_init_cce(struct drm_device *dev, drm_r128_init_t *init)
 		return -EINVAL;
 	}
 	dev->agp_buffer_token = init->buffers_offset;
-	dev->agp_buffer_map = drm_core_findmap(dev, init->buffers_offset);
+	dev->agp_buffer_map = drm_legacy_findmap(dev, init->buffers_offset);
 	if (!dev->agp_buffer_map) {
 		DRM_ERROR("could not find dma buffer region!\n");
 		dev->dev_private = (void *)dev_priv;
@@ -492,7 +492,7 @@ static int r128_do_init_cce(struct drm_device *dev, drm_r128_init_t *init)
 
 	if (!dev_priv->is_pci) {
 		dev_priv->agp_textures =
-		    drm_core_findmap(dev, init->agp_textures_offset);
+		    drm_legacy_findmap(dev, init->agp_textures_offset);
 		if (!dev_priv->agp_textures) {
 			DRM_ERROR("could not find agp texture region!\n");
 			dev->dev_private = (void *)dev_priv;
@@ -507,9 +507,9 @@ static int r128_do_init_cce(struct drm_device *dev, drm_r128_init_t *init)
 
 #if __OS_HAS_AGP
 	if (!dev_priv->is_pci) {
-		drm_core_ioremap_wc(dev_priv->cce_ring, dev);
-		drm_core_ioremap_wc(dev_priv->ring_rptr, dev);
-		drm_core_ioremap_wc(dev->agp_buffer_map, dev);
+		drm_legacy_ioremap_wc(dev_priv->cce_ring, dev);
+		drm_legacy_ioremap_wc(dev_priv->ring_rptr, dev);
+		drm_legacy_ioremap_wc(dev->agp_buffer_map, dev);
 		if (!dev_priv->cce_ring->handle ||
 		    !dev_priv->ring_rptr->handle ||
 		    !dev->agp_buffer_map->handle) {
@@ -603,11 +603,11 @@ int r128_do_cleanup_cce(struct drm_device *dev)
 #if __OS_HAS_AGP
 		if (!dev_priv->is_pci) {
 			if (dev_priv->cce_ring != NULL)
-				drm_core_ioremapfree(dev_priv->cce_ring, dev);
+				drm_legacy_ioremapfree(dev_priv->cce_ring, dev);
 			if (dev_priv->ring_rptr != NULL)
-				drm_core_ioremapfree(dev_priv->ring_rptr, dev);
+				drm_legacy_ioremapfree(dev_priv->ring_rptr, dev);
 			if (dev->agp_buffer_map != NULL) {
-				drm_core_ioremapfree(dev->agp_buffer_map, dev);
+				drm_legacy_ioremapfree(dev->agp_buffer_map, dev);
 				dev->agp_buffer_map = NULL;
 			}
 		} else
diff --git a/drivers/gpu/drm/r128/r128_drv.c b/drivers/gpu/drm/r128/r128_drv.c
index 5bd307cd8da1..c57b4de63caf 100644
--- a/drivers/gpu/drm/r128/r128_drv.c
+++ b/drivers/gpu/drm/r128/r128_drv.c
@@ -46,7 +46,7 @@ static const struct file_operations r128_driver_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = r128_compat_ioctl,
@@ -62,6 +62,7 @@ static struct drm_driver driver = {
 	.load = r128_driver_load,
 	.preclose = r128_driver_preclose,
 	.lastclose = r128_driver_lastclose,
+	.set_busid = drm_pci_set_busid,
 	.get_vblank_counter = r128_get_vblank_counter,
 	.enable_vblank = r128_enable_vblank,
 	.disable_vblank = r128_disable_vblank,
diff --git a/drivers/gpu/drm/r128/r128_drv.h b/drivers/gpu/drm/r128/r128_drv.h
index 5bf3f5ff805d..723e5d6f10a4 100644
--- a/drivers/gpu/drm/r128/r128_drv.h
+++ b/drivers/gpu/drm/r128/r128_drv.h
@@ -35,6 +35,9 @@
 #ifndef __R128_DRV_H__
 #define __R128_DRV_H__
 
+#include <drm/ati_pcigart.h>
+#include <drm/drm_legacy.h>
+
 /* General customization:
  */
 #define DRIVER_AUTHOR		"Gareth Hughes, VA Linux Systems Inc."
diff --git a/drivers/gpu/drm/radeon/Makefile b/drivers/gpu/drm/radeon/Makefile
index 0013ad0db9ef..d01b87991422 100644
--- a/drivers/gpu/drm/radeon/Makefile
+++ b/drivers/gpu/drm/radeon/Makefile
@@ -60,7 +60,7 @@ radeon-y := radeon_drv.o
 
 # add UMS driver
 radeon-$(CONFIG_DRM_RADEON_UMS)+= radeon_cp.o radeon_state.o radeon_mem.o \
-	radeon_irq.o r300_cmdbuf.o r600_cp.o r600_blit.o
+	radeon_irq.o r300_cmdbuf.o r600_cp.o r600_blit.o drm_buffer.o
 
 # add KMS driver
 radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
@@ -72,15 +72,15 @@ radeon-y += radeon_device.o radeon_asic.o radeon_kms.o \
 	radeon_cs.o radeon_bios.o radeon_benchmark.o r100.o r300.o r420.o \
 	rs400.o rs600.o rs690.o rv515.o r520.o r600.o rv770.o radeon_test.o \
 	r200.o radeon_legacy_tv.o r600_cs.o r600_blit_shaders.o \
-	radeon_pm.o atombios_dp.o r600_audio.o r600_hdmi.o dce3_1_afmt.o \
+	radeon_pm.o atombios_dp.o r600_hdmi.o dce3_1_afmt.o \
 	evergreen.o evergreen_cs.o evergreen_blit_shaders.o \
 	evergreen_hdmi.o radeon_trace_points.o ni.o cayman_blit_shaders.o \
 	atombios_encoders.o radeon_semaphore.o radeon_sa.o atombios_i2c.o si.o \
-	si_blit_shaders.o radeon_prime.o radeon_uvd.o cik.o cik_blit_shaders.o \
+	si_blit_shaders.o radeon_prime.o cik.o cik_blit_shaders.o \
 	r600_dpm.o rs780_dpm.o rv6xx_dpm.o rv770_dpm.o rv730_dpm.o rv740_dpm.o \
 	rv770_smc.o cypress_dpm.o btc_dpm.o sumo_dpm.o sumo_smc.o trinity_dpm.o \
 	trinity_smc.o ni_dpm.o si_smc.o si_dpm.o kv_smc.o kv_dpm.o ci_smc.o \
-	ci_dpm.o dce6_afmt.o radeon_vm.o radeon_ucode.o radeon_ib.o
+	ci_dpm.o dce6_afmt.o radeon_vm.o radeon_ucode.o radeon_ib.o radeon_mn.o
 
 # add async DMA block
 radeon-y += \
diff --git a/drivers/gpu/drm/radeon/atombios_dp.c b/drivers/gpu/drm/radeon/atombios_dp.c
index b1e11f8434e2..95d5d4ab3335 100644
--- a/drivers/gpu/drm/radeon/atombios_dp.c
+++ b/drivers/gpu/drm/radeon/atombios_dp.c
@@ -232,8 +232,8 @@ void radeon_dp_aux_init(struct radeon_connector *radeon_connector)
 
 /***** general DP utility functions *****/
 
-#define DP_VOLTAGE_MAX         DP_TRAIN_VOLTAGE_SWING_1200
-#define DP_PRE_EMPHASIS_MAX    DP_TRAIN_PRE_EMPHASIS_9_5
+#define DP_VOLTAGE_MAX         DP_TRAIN_VOLTAGE_SWING_LEVEL_3
+#define DP_PRE_EMPHASIS_MAX    DP_TRAIN_PRE_EMPH_LEVEL_3
 
 static void dp_get_adjust_train(u8 link_status[DP_LINK_STATUS_SIZE],
 				int lane_count,
@@ -405,16 +405,13 @@ bool radeon_dp_getdpcd(struct radeon_connector *radeon_connector)
 	u8 msg[DP_DPCD_SIZE];
 	int ret;
 
-	char dpcd_hex_dump[DP_DPCD_SIZE * 3];
-
 	ret = drm_dp_dpcd_read(&radeon_connector->ddc_bus->aux, DP_DPCD_REV, msg,
 			       DP_DPCD_SIZE);
 	if (ret > 0) {
 		memcpy(dig_connector->dpcd, msg, DP_DPCD_SIZE);
 
-		hex_dump_to_buffer(dig_connector->dpcd, sizeof(dig_connector->dpcd),
-				   32, 1, dpcd_hex_dump, sizeof(dpcd_hex_dump), false);
-		DRM_DEBUG_KMS("DPCD: %s\n", dpcd_hex_dump);
+		DRM_DEBUG_KMS("DPCD: %*ph\n", (int)sizeof(dig_connector->dpcd),
+			      dig_connector->dpcd);
 
 		radeon_dp_probe_oui(radeon_connector);
 
diff --git a/drivers/gpu/drm/radeon/atombios_encoders.c b/drivers/gpu/drm/radeon/atombios_encoders.c
index a7f2ddf09a9d..b8cd7975f797 100644
--- a/drivers/gpu/drm/radeon/atombios_encoders.c
+++ b/drivers/gpu/drm/radeon/atombios_encoders.c
@@ -291,29 +291,6 @@ static void radeon_atom_backlight_exit(struct radeon_encoder *encoder)
 bool radeon_atom_get_tv_timings(struct radeon_device *rdev, int index,
 				struct drm_display_mode *mode);
 
-
-static inline bool radeon_encoder_is_digital(struct drm_encoder *encoder)
-{
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	switch (radeon_encoder->encoder_id) {
-	case ENCODER_OBJECT_ID_INTERNAL_LVDS:
-	case ENCODER_OBJECT_ID_INTERNAL_TMDS1:
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
-	case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
-	case ENCODER_OBJECT_ID_INTERNAL_DVO1:
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
-	case ENCODER_OBJECT_ID_INTERNAL_DDI:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY:
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_LVTMA:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY1:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY2:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY3:
-		return true;
-	default:
-		return false;
-	}
-}
-
 static bool radeon_atom_mode_fixup(struct drm_encoder *encoder,
 				   const struct drm_display_mode *mode,
 				   struct drm_display_mode *adjusted_mode)
diff --git a/drivers/gpu/drm/radeon/btc_dpm.c b/drivers/gpu/drm/radeon/btc_dpm.c
index f81d7ca134db..0b2929de9f41 100644
--- a/drivers/gpu/drm/radeon/btc_dpm.c
+++ b/drivers/gpu/drm/radeon/btc_dpm.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "btcd.h"
 #include "r600_dpm.h"
 #include "cypress_dpm.h"
@@ -2099,7 +2100,6 @@ static void btc_apply_state_adjust_rules(struct radeon_device *rdev,
 	bool disable_mclk_switching;
 	u32 mclk, sclk;
 	u16 vddc, vddci;
-	u32 max_sclk_vddc, max_mclk_vddci, max_mclk_vddc;
 
 	if ((rdev->pm.dpm.new_active_crtc_count > 1) ||
 	    btc_dpm_vblank_too_short(rdev))
@@ -2141,39 +2141,6 @@ static void btc_apply_state_adjust_rules(struct radeon_device *rdev,
 			ps->low.vddci = max_limits->vddci;
 	}
 
-	/* limit clocks to max supported clocks based on voltage dependency tables */
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddc_dependency_on_sclk,
-							&max_sclk_vddc);
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddci_dependency_on_mclk,
-							&max_mclk_vddci);
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddc_dependency_on_mclk,
-							&max_mclk_vddc);
-
-	if (max_sclk_vddc) {
-		if (ps->low.sclk > max_sclk_vddc)
-			ps->low.sclk = max_sclk_vddc;
-		if (ps->medium.sclk > max_sclk_vddc)
-			ps->medium.sclk = max_sclk_vddc;
-		if (ps->high.sclk > max_sclk_vddc)
-			ps->high.sclk = max_sclk_vddc;
-	}
-	if (max_mclk_vddci) {
-		if (ps->low.mclk > max_mclk_vddci)
-			ps->low.mclk = max_mclk_vddci;
-		if (ps->medium.mclk > max_mclk_vddci)
-			ps->medium.mclk = max_mclk_vddci;
-		if (ps->high.mclk > max_mclk_vddci)
-			ps->high.mclk = max_mclk_vddci;
-	}
-	if (max_mclk_vddc) {
-		if (ps->low.mclk > max_mclk_vddc)
-			ps->low.mclk = max_mclk_vddc;
-		if (ps->medium.mclk > max_mclk_vddc)
-			ps->medium.mclk = max_mclk_vddc;
-		if (ps->high.mclk > max_mclk_vddc)
-			ps->high.mclk = max_mclk_vddc;
-	}
-
 	/* XXX validate the min clocks required for display */
 
 	if (disable_mclk_switching) {
diff --git a/drivers/gpu/drm/radeon/ci_dpm.c b/drivers/gpu/drm/radeon/ci_dpm.c
index 022561e28707..11a55e9dad7f 100644
--- a/drivers/gpu/drm/radeon/ci_dpm.c
+++ b/drivers/gpu/drm/radeon/ci_dpm.c
@@ -24,6 +24,7 @@
 #include <linux/firmware.h>
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "radeon_ucode.h"
 #include "cikd.h"
 #include "r600_dpm.h"
@@ -162,8 +163,6 @@ static const struct ci_pt_config_reg didt_config_ci[] =
 };
 
 extern u8 rv770_get_memory_module_index(struct radeon_device *rdev);
-extern void btc_get_max_clock_from_voltage_dependency_table(struct radeon_clock_voltage_dependency_table *table,
-							    u32 *max_clock);
 extern int ni_copy_and_switch_arb_sets(struct radeon_device *rdev,
 				       u32 arb_freq_src, u32 arb_freq_dest);
 extern u8 si_get_ddr3_mclk_frequency_ratio(u32 memory_clock);
@@ -748,7 +747,6 @@ static void ci_apply_state_adjust_rules(struct radeon_device *rdev,
 	struct radeon_clock_and_voltage_limits *max_limits;
 	bool disable_mclk_switching;
 	u32 sclk, mclk;
-	u32 max_sclk_vddc, max_mclk_vddci, max_mclk_vddc;
 	int i;
 
 	if (rps->vce_active) {
@@ -784,29 +782,6 @@ static void ci_apply_state_adjust_rules(struct radeon_device *rdev,
 		}
 	}
 
-	/* limit clocks to max supported clocks based on voltage dependency tables */
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddc_dependency_on_sclk,
-							&max_sclk_vddc);
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddci_dependency_on_mclk,
-							&max_mclk_vddci);
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddc_dependency_on_mclk,
-							&max_mclk_vddc);
-
-	for (i = 0; i < ps->performance_level_count; i++) {
-		if (max_sclk_vddc) {
-			if (ps->performance_levels[i].sclk > max_sclk_vddc)
-				ps->performance_levels[i].sclk = max_sclk_vddc;
-		}
-		if (max_mclk_vddci) {
-			if (ps->performance_levels[i].mclk > max_mclk_vddci)
-				ps->performance_levels[i].mclk = max_mclk_vddci;
-		}
-		if (max_mclk_vddc) {
-			if (ps->performance_levels[i].mclk > max_mclk_vddc)
-				ps->performance_levels[i].mclk = max_mclk_vddc;
-		}
-	}
-
 	/* XXX validate the min clocks required for display */
 
 	if (disable_mclk_switching) {
@@ -869,6 +844,9 @@ static int ci_set_thermal_temperature_range(struct radeon_device *rdev,
 	WREG32_SMC(CG_THERMAL_CTRL, tmp);
 #endif
 
+	rdev->pm.dpm.thermal.min_temp = low_temp;
+	rdev->pm.dpm.thermal.max_temp = high_temp;
+
 	return 0;
 }
 
@@ -5290,9 +5268,13 @@ int ci_dpm_init(struct radeon_device *rdev)
 void ci_dpm_debugfs_print_current_performance_level(struct radeon_device *rdev,
 						    struct seq_file *m)
 {
+	struct ci_power_info *pi = ci_get_pi(rdev);
+	struct radeon_ps *rps = &pi->current_rps;
 	u32 sclk = ci_get_average_sclk_freq(rdev);
 	u32 mclk = ci_get_average_mclk_freq(rdev);
 
+	seq_printf(m, "uvd    %sabled\n", pi->uvd_enabled ? "en" : "dis");
+	seq_printf(m, "vce    %sabled\n", rps->vce_active ? "en" : "dis");
 	seq_printf(m, "power level avg    sclk: %u mclk: %u\n",
 		   sclk, mclk);
 }
diff --git a/drivers/gpu/drm/radeon/cik.c b/drivers/gpu/drm/radeon/cik.c
index b625646bf3e2..377afa504d2b 100644
--- a/drivers/gpu/drm/radeon/cik.c
+++ b/drivers/gpu/drm/radeon/cik.c
@@ -3483,7 +3483,7 @@ static void cik_gpu_init(struct radeon_device *rdev)
 	u32 mc_shared_chmap, mc_arb_ramcfg;
 	u32 hdp_host_path_cntl;
 	u32 tmp;
-	int i, j, k;
+	int i, j;
 
 	switch (rdev->family) {
 	case CHIP_BONAIRE:
@@ -3544,6 +3544,7 @@ static void cik_gpu_init(struct radeon_device *rdev)
 			   (rdev->pdev->device == 0x130B) ||
 			   (rdev->pdev->device == 0x130E) ||
 			   (rdev->pdev->device == 0x1315) ||
+			   (rdev->pdev->device == 0x1318) ||
 			   (rdev->pdev->device == 0x131B)) {
 			rdev->config.cik.max_cu_per_sh = 4;
 			rdev->config.cik.max_backends_per_se = 1;
@@ -3672,12 +3673,11 @@ static void cik_gpu_init(struct radeon_device *rdev)
 		     rdev->config.cik.max_sh_per_se,
 		     rdev->config.cik.max_backends_per_se);
 
+	rdev->config.cik.active_cus = 0;
 	for (i = 0; i < rdev->config.cik.max_shader_engines; i++) {
 		for (j = 0; j < rdev->config.cik.max_sh_per_se; j++) {
-			for (k = 0; k < rdev->config.cik.max_cu_per_sh; k++) {
-				rdev->config.cik.active_cus +=
-					hweight32(cik_get_cu_active_bitmap(rdev, i, j));
-			}
+			rdev->config.cik.active_cus +=
+				hweight32(cik_get_cu_active_bitmap(rdev, i, j));
 		}
 	}
 
@@ -3801,7 +3801,7 @@ int cik_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	radeon_ring_write(ring, PACKET3(PACKET3_SET_UCONFIG_REG, 1));
 	radeon_ring_write(ring, ((scratch - PACKET3_SET_UCONFIG_REG_START) >> 2));
 	radeon_ring_write(ring, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	for (i = 0; i < rdev->usec_timeout; i++) {
 		tmp = RREG32(scratch);
@@ -3920,6 +3920,17 @@ void cik_fence_compute_ring_emit(struct radeon_device *rdev,
 	radeon_ring_write(ring, 0);
 }
 
+/**
+ * cik_semaphore_ring_emit - emit a semaphore on the CP ring
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon ring buffer object
+ * @semaphore: radeon semaphore object
+ * @emit_wait: Is this a sempahore wait?
+ *
+ * Emits a semaphore signal/wait packet to the CP ring and prevents the PFP
+ * from running ahead of semaphore waits.
+ */
 bool cik_semaphore_ring_emit(struct radeon_device *rdev,
 			     struct radeon_ring *ring,
 			     struct radeon_semaphore *semaphore,
@@ -3932,6 +3943,12 @@ bool cik_semaphore_ring_emit(struct radeon_device *rdev,
 	radeon_ring_write(ring, lower_32_bits(addr));
 	radeon_ring_write(ring, (upper_32_bits(addr) & 0xffff) | sel);
 
+	if (emit_wait && ring->idx == RADEON_RING_TYPE_GFX_INDEX) {
+		/* Prevent the PFP from running ahead of the semaphore wait */
+		radeon_ring_write(ring, PACKET3(PACKET3_PFP_SYNC_ME, 0));
+		radeon_ring_write(ring, 0x0);
+	}
+
 	return true;
 }
 
@@ -3942,18 +3959,19 @@ bool cik_semaphore_ring_emit(struct radeon_device *rdev,
  * @src_offset: src GPU address
  * @dst_offset: dst GPU address
  * @num_gpu_pages: number of GPU pages to xfer
- * @fence: radeon fence object
+ * @resv: reservation object to sync to
  *
  * Copy GPU paging using the CP DMA engine (CIK+).
  * Used by the radeon ttm implementation to move pages if
  * registered as the asic copy callback.
  */
-int cik_copy_cpdma(struct radeon_device *rdev,
-		   uint64_t src_offset, uint64_t dst_offset,
-		   unsigned num_gpu_pages,
-		   struct radeon_fence **fence)
+struct radeon_fence *cik_copy_cpdma(struct radeon_device *rdev,
+				    uint64_t src_offset, uint64_t dst_offset,
+				    unsigned num_gpu_pages,
+				    struct reservation_object *resv)
 {
 	struct radeon_semaphore *sem = NULL;
+	struct radeon_fence *fence;
 	int ring_index = rdev->asic->copy.blit_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes, control;
@@ -3963,7 +3981,7 @@ int cik_copy_cpdma(struct radeon_device *rdev,
 	r = radeon_semaphore_create(rdev, &sem);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
@@ -3972,10 +3990,10 @@ int cik_copy_cpdma(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_to(sem, *fence);
+	radeon_semaphore_sync_resv(rdev, sem, resv, false);
 	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
@@ -3997,17 +4015,17 @@ int cik_copy_cpdma(struct radeon_device *rdev,
 		dst_offset += cur_size_in_bytes;
 	}
 
-	r = radeon_fence_emit(rdev, fence, ring->idx);
+	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
-	radeon_semaphore_free(rdev, &sem, *fence);
+	radeon_ring_unlock_commit(rdev, ring, false);
+	radeon_semaphore_free(rdev, &sem, fence);
 
-	return r;
+	return fence;
 }
 
 /*
@@ -4103,7 +4121,7 @@ int cik_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	ib.ptr[1] = ((scratch - PACKET3_SET_UCONFIG_REG_START) >> 2);
 	ib.ptr[2] = 0xDEADBEEF;
 	ib.length_dw = 3;
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 		radeon_scratch_free(rdev, scratch);
 		radeon_ib_free(rdev, &ib);
@@ -4217,7 +4235,7 @@ static int cik_cp_gfx_load_microcode(struct radeon_device *rdev)
 		WREG32(CP_PFP_UCODE_ADDR, 0);
 		for (i = 0; i < fw_size; i++)
 			WREG32(CP_PFP_UCODE_DATA, le32_to_cpup(fw_data++));
-		WREG32(CP_PFP_UCODE_ADDR, 0);
+		WREG32(CP_PFP_UCODE_ADDR, le32_to_cpu(pfp_hdr->header.ucode_version));
 
 		/* CE */
 		fw_data = (const __le32 *)
@@ -4226,7 +4244,7 @@ static int cik_cp_gfx_load_microcode(struct radeon_device *rdev)
 		WREG32(CP_CE_UCODE_ADDR, 0);
 		for (i = 0; i < fw_size; i++)
 			WREG32(CP_CE_UCODE_DATA, le32_to_cpup(fw_data++));
-		WREG32(CP_CE_UCODE_ADDR, 0);
+		WREG32(CP_CE_UCODE_ADDR, le32_to_cpu(ce_hdr->header.ucode_version));
 
 		/* ME */
 		fw_data = (const __be32 *)
@@ -4235,7 +4253,8 @@ static int cik_cp_gfx_load_microcode(struct radeon_device *rdev)
 		WREG32(CP_ME_RAM_WADDR, 0);
 		for (i = 0; i < fw_size; i++)
 			WREG32(CP_ME_RAM_DATA, le32_to_cpup(fw_data++));
-		WREG32(CP_ME_RAM_WADDR, 0);
+		WREG32(CP_ME_RAM_WADDR, le32_to_cpu(me_hdr->header.ucode_version));
+		WREG32(CP_ME_RAM_RADDR, le32_to_cpu(me_hdr->header.ucode_version));
 	} else {
 		const __be32 *fw_data;
 
@@ -4261,10 +4280,6 @@ static int cik_cp_gfx_load_microcode(struct radeon_device *rdev)
 		WREG32(CP_ME_RAM_WADDR, 0);
 	}
 
-	WREG32(CP_PFP_UCODE_ADDR, 0);
-	WREG32(CP_CE_UCODE_ADDR, 0);
-	WREG32(CP_ME_RAM_WADDR, 0);
-	WREG32(CP_ME_RAM_RADDR, 0);
 	return 0;
 }
 
@@ -4324,7 +4339,7 @@ static int cik_cp_gfx_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
 	radeon_ring_write(ring, 0x00000010); /* VGT_OUT_DEALLOC_CNTL */
 
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	return 0;
 }
@@ -4546,7 +4561,7 @@ static int cik_cp_compute_load_microcode(struct radeon_device *rdev)
 		WREG32(CP_MEC_ME1_UCODE_ADDR, 0);
 		for (i = 0; i < fw_size; i++)
 			WREG32(CP_MEC_ME1_UCODE_DATA, le32_to_cpup(fw_data++));
-		WREG32(CP_MEC_ME1_UCODE_ADDR, 0);
+		WREG32(CP_MEC_ME1_UCODE_ADDR, le32_to_cpu(mec_hdr->header.ucode_version));
 
 		/* MEC2 */
 		if (rdev->family == CHIP_KAVERI) {
@@ -4560,7 +4575,7 @@ static int cik_cp_compute_load_microcode(struct radeon_device *rdev)
 			WREG32(CP_MEC_ME2_UCODE_ADDR, 0);
 			for (i = 0; i < fw_size; i++)
 				WREG32(CP_MEC_ME2_UCODE_DATA, le32_to_cpup(fw_data++));
-			WREG32(CP_MEC_ME2_UCODE_ADDR, 0);
+			WREG32(CP_MEC_ME2_UCODE_ADDR, le32_to_cpu(mec2_hdr->header.ucode_version));
 		}
 	} else {
 		const __be32 *fw_data;
@@ -4672,7 +4687,7 @@ static int cik_mec_init(struct radeon_device *rdev)
 		r = radeon_bo_create(rdev,
 				     rdev->mec.num_mec *rdev->mec.num_pipe * MEC_HPD_SIZE * 2,
 				     PAGE_SIZE, true,
-				     RADEON_GEM_DOMAIN_GTT, 0, NULL,
+				     RADEON_GEM_DOMAIN_GTT, 0, NULL, NULL,
 				     &rdev->mec.hpd_eop_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create HDP EOP bo failed\n", r);
@@ -4786,7 +4801,7 @@ struct bonaire_mqd
  */
 static int cik_cp_compute_resume(struct radeon_device *rdev)
 {
-	int r, i, idx;
+	int r, i, j, idx;
 	u32 tmp;
 	bool use_doorbell = true;
 	u64 hqd_gpu_addr;
@@ -4843,7 +4858,7 @@ static int cik_cp_compute_resume(struct radeon_device *rdev)
 					     sizeof(struct bonaire_mqd),
 					     PAGE_SIZE, true,
 					     RADEON_GEM_DOMAIN_GTT, 0, NULL,
-					     &rdev->ring[idx].mqd_obj);
+					     NULL, &rdev->ring[idx].mqd_obj);
 			if (r) {
 				dev_warn(rdev->dev, "(%d) create MQD bo failed\n", r);
 				return r;
@@ -4905,7 +4920,7 @@ static int cik_cp_compute_resume(struct radeon_device *rdev)
 		mqd->queue_state.cp_hqd_pq_wptr= 0;
 		if (RREG32(CP_HQD_ACTIVE) & 1) {
 			WREG32(CP_HQD_DEQUEUE_REQUEST, 1);
-			for (i = 0; i < rdev->usec_timeout; i++) {
+			for (j = 0; j < rdev->usec_timeout; j++) {
 				if (!(RREG32(CP_HQD_ACTIVE) & 1))
 					break;
 				udelay(1);
@@ -5732,20 +5747,17 @@ static int cik_pcie_gart_enable(struct radeon_device *rdev)
 	WREG32(0x15D8, 0);
 	WREG32(0x15DC, 0);
 
-	/* empty context1-15 */
-	/* FIXME start with 4G, once using 2 level pt switch to full
-	 * vm size space
-	 */
+	/* restore context1-15 */
 	/* set vm size, must be a multiple of 4 */
 	WREG32(VM_CONTEXT1_PAGE_TABLE_START_ADDR, 0);
 	WREG32(VM_CONTEXT1_PAGE_TABLE_END_ADDR, rdev->vm_manager.max_pfn);
 	for (i = 1; i < 16; i++) {
 		if (i < 8)
 			WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2),
-			       rdev->gart.table_addr >> 12);
+			       rdev->vm_manager.saved_table_addr[i]);
 		else
 			WREG32(VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((i - 8) << 2),
-			       rdev->gart.table_addr >> 12);
+			       rdev->vm_manager.saved_table_addr[i]);
 	}
 
 	/* enable context1-15 */
@@ -5810,6 +5822,17 @@ static int cik_pcie_gart_enable(struct radeon_device *rdev)
  */
 static void cik_pcie_gart_disable(struct radeon_device *rdev)
 {
+	unsigned i;
+
+	for (i = 1; i < 16; ++i) {
+		uint32_t reg;
+		if (i < 8)
+			reg = VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2);
+		else
+			reg = VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((i - 8) << 2);
+		rdev->vm_manager.saved_table_addr[i] = RREG32(reg);
+	}
+
 	/* Disable all tables */
 	WREG32(VM_CONTEXT0_CNTL, 0);
 	WREG32(VM_CONTEXT1_CNTL, 0);
@@ -5958,14 +5981,14 @@ void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 
 	/* update SH_MEM_* regs */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
-	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
+	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(usepfp) |
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, SRBM_GFX_CNTL >> 2);
 	radeon_ring_write(ring, 0);
 	radeon_ring_write(ring, VMID(vm->id));
 
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 6));
-	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
+	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(usepfp) |
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, SH_MEM_BASES >> 2);
 	radeon_ring_write(ring, 0);
@@ -5976,7 +5999,7 @@ void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 	radeon_ring_write(ring, 0); /* SH_MEM_APE1_LIMIT */
 
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
-	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
+	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(usepfp) |
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, SRBM_GFX_CNTL >> 2);
 	radeon_ring_write(ring, 0);
@@ -5987,7 +6010,7 @@ void cik_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 
 	/* bits 0-15 are the VM contexts0-15 */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
-	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
+	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(usepfp) |
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, VM_INVALIDATE_REQUEST >> 2);
 	radeon_ring_write(ring, 0);
@@ -6201,7 +6224,7 @@ static int cik_rlc_resume(struct radeon_device *rdev)
 		WREG32(RLC_GPM_UCODE_ADDR, 0);
 		for (i = 0; i < size; i++)
 			WREG32(RLC_GPM_UCODE_DATA, le32_to_cpup(fw_data++));
-		WREG32(RLC_GPM_UCODE_ADDR, 0);
+		WREG32(RLC_GPM_UCODE_ADDR, le32_to_cpu(hdr->header.ucode_version));
 	} else {
 		const __be32 *fw_data;
 
@@ -7726,17 +7749,17 @@ static inline u32 cik_get_ih_wptr(struct radeon_device *rdev)
 		wptr = RREG32(IH_RB_WPTR);
 
 	if (wptr & RB_OVERFLOW) {
+		wptr &= ~RB_OVERFLOW;
 		/* When a ring buffer overflow happen start parsing interrupt
 		 * from the last not overwritten vector (wptr + 16). Hopefully
 		 * this should allow us to catchup.
 		 */
-		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, %d, %d)\n",
-			wptr, rdev->ih.rptr, (wptr + 16) + rdev->ih.ptr_mask);
+		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, 0x%08X, 0x%08X)\n",
+			 wptr, rdev->ih.rptr, (wptr + 16) & rdev->ih.ptr_mask);
 		rdev->ih.rptr = (wptr + 16) & rdev->ih.ptr_mask;
 		tmp = RREG32(IH_RB_CNTL);
 		tmp |= IH_WPTR_OVERFLOW_CLEAR;
 		WREG32(IH_RB_CNTL, tmp);
-		wptr &= ~RB_OVERFLOW;
 	}
 	return (wptr & rdev->ih.ptr_mask);
 }
@@ -8226,15 +8249,17 @@ restart_ih:
 		/* wptr/rptr are in bytes! */
 		rptr += 16;
 		rptr &= rdev->ih.ptr_mask;
+		WREG32(IH_RB_RPTR, rptr);
 	}
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
-	if (queue_reset)
-		schedule_work(&rdev->reset_work);
+	if (queue_reset) {
+		rdev->needs_reset = true;
+		wake_up_all(&rdev->fence_queue);
+	}
 	if (queue_thermal)
 		schedule_work(&rdev->pm.dpm.thermal.work);
 	rdev->ih.rptr = rptr;
-	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	atomic_set(&rdev->ih.lock, 0);
 
 	/* make sure wptr hasn't changed while processing */
@@ -9538,6 +9563,9 @@ static void cik_pcie_gen3_enable(struct radeon_device *rdev)
 	int ret, i;
 	u16 tmp16;
 
+	if (pci_is_root_bus(rdev->pdev->bus))
+		return;
+
 	if (radeon_pcie_gen2 == 0)
 		return;
 
@@ -9764,7 +9792,8 @@ static void cik_program_aspm(struct radeon_device *rdev)
 			if (orig != data)
 				WREG32_PCIE_PORT(PCIE_LC_LINK_WIDTH_CNTL, data);
 
-			if (!disable_clkreq) {
+			if (!disable_clkreq &&
+			    !pci_is_root_bus(rdev->pdev->bus)) {
 				struct pci_dev *root = rdev->pdev->bus->self;
 				u32 lnkcap;
 
diff --git a/drivers/gpu/drm/radeon/cik_sdma.c b/drivers/gpu/drm/radeon/cik_sdma.c
index bcf480510ac2..4e8432d07f15 100644
--- a/drivers/gpu/drm/radeon/cik_sdma.c
+++ b/drivers/gpu/drm/radeon/cik_sdma.c
@@ -489,13 +489,6 @@ int cik_sdma_resume(struct radeon_device *rdev)
 {
 	int r;
 
-	/* Reset dma */
-	WREG32(SRBM_SOFT_RESET, SOFT_RESET_SDMA | SOFT_RESET_SDMA1);
-	RREG32(SRBM_SOFT_RESET);
-	udelay(50);
-	WREG32(SRBM_SOFT_RESET, 0);
-	RREG32(SRBM_SOFT_RESET);
-
 	r = cik_sdma_load_microcode(rdev);
 	if (r)
 		return r;
@@ -537,18 +530,19 @@ void cik_sdma_fini(struct radeon_device *rdev)
  * @src_offset: src GPU address
  * @dst_offset: dst GPU address
  * @num_gpu_pages: number of GPU pages to xfer
- * @fence: radeon fence object
+ * @resv: reservation object to sync to
  *
  * Copy GPU paging using the DMA engine (CIK).
  * Used by the radeon ttm implementation to move pages if
  * registered as the asic copy callback.
  */
-int cik_copy_dma(struct radeon_device *rdev,
-		 uint64_t src_offset, uint64_t dst_offset,
-		 unsigned num_gpu_pages,
-		 struct radeon_fence **fence)
+struct radeon_fence *cik_copy_dma(struct radeon_device *rdev,
+				  uint64_t src_offset, uint64_t dst_offset,
+				  unsigned num_gpu_pages,
+				  struct reservation_object *resv)
 {
 	struct radeon_semaphore *sem = NULL;
+	struct radeon_fence *fence;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes;
@@ -558,7 +552,7 @@ int cik_copy_dma(struct radeon_device *rdev,
 	r = radeon_semaphore_create(rdev, &sem);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
@@ -567,10 +561,10 @@ int cik_copy_dma(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_to(sem, *fence);
+	radeon_semaphore_sync_resv(rdev, sem, resv, false);
 	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
@@ -589,17 +583,17 @@ int cik_copy_dma(struct radeon_device *rdev,
 		dst_offset += cur_size_in_bytes;
 	}
 
-	r = radeon_fence_emit(rdev, fence, ring->idx);
+	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
-	radeon_semaphore_free(rdev, &sem, *fence);
+	radeon_ring_unlock_commit(rdev, ring, false);
+	radeon_semaphore_free(rdev, &sem, fence);
 
-	return r;
+	return fence;
 }
 
 /**
@@ -617,16 +611,19 @@ int cik_sdma_ring_test(struct radeon_device *rdev,
 {
 	unsigned i;
 	int r;
-	void __iomem *ptr = (void *)rdev->vram_scratch.ptr;
+	unsigned index;
 	u32 tmp;
+	u64 gpu_addr;
 
-	if (!ptr) {
-		DRM_ERROR("invalid vram scratch pointer\n");
-		return -EINVAL;
-	}
+	if (ring->idx == R600_RING_TYPE_DMA_INDEX)
+		index = R600_WB_DMA_RING_TEST_OFFSET;
+	else
+		index = CAYMAN_WB_DMA1_RING_TEST_OFFSET;
+
+	gpu_addr = rdev->wb.gpu_addr + index;
 
 	tmp = 0xCAFEDEAD;
-	writel(tmp, ptr);
+	rdev->wb.wb[index/4] = cpu_to_le32(tmp);
 
 	r = radeon_ring_lock(rdev, ring, 5);
 	if (r) {
@@ -634,14 +631,14 @@ int cik_sdma_ring_test(struct radeon_device *rdev,
 		return r;
 	}
 	radeon_ring_write(ring, SDMA_PACKET(SDMA_OPCODE_WRITE, SDMA_WRITE_SUB_OPCODE_LINEAR, 0));
-	radeon_ring_write(ring, rdev->vram_scratch.gpu_addr & 0xfffffffc);
-	radeon_ring_write(ring, upper_32_bits(rdev->vram_scratch.gpu_addr));
+	radeon_ring_write(ring, lower_32_bits(gpu_addr));
+	radeon_ring_write(ring, upper_32_bits(gpu_addr));
 	radeon_ring_write(ring, 1); /* number of DWs to follow */
 	radeon_ring_write(ring, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	for (i = 0; i < rdev->usec_timeout; i++) {
-		tmp = readl(ptr);
+		tmp = le32_to_cpu(rdev->wb.wb[index/4]);
 		if (tmp == 0xDEADBEEF)
 			break;
 		DRM_UDELAY(1);
@@ -695,7 +692,7 @@ int cik_sdma_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	ib.ptr[4] = 0xDEADBEEF;
 	ib.length_dw = 5;
 
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 		radeon_ib_free(rdev, &ib);
 		DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
diff --git a/drivers/gpu/drm/radeon/cypress_dpm.c b/drivers/gpu/drm/radeon/cypress_dpm.c
index 47d31e915758..9aad0327e4d1 100644
--- a/drivers/gpu/drm/radeon/cypress_dpm.c
+++ b/drivers/gpu/drm/radeon/cypress_dpm.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "evergreend.h"
 #include "r600_dpm.h"
 #include "cypress_dpm.h"
diff --git a/drivers/gpu/drm/radeon/dce3_1_afmt.c b/drivers/gpu/drm/radeon/dce3_1_afmt.c
index 51800e340a57..2fe8cfc966d9 100644
--- a/drivers/gpu/drm/radeon/dce3_1_afmt.c
+++ b/drivers/gpu/drm/radeon/dce3_1_afmt.c
@@ -32,7 +32,7 @@ static void dce3_2_afmt_write_speaker_allocation(struct drm_encoder *encoder)
 	struct drm_connector *connector;
 	struct radeon_connector *radeon_connector = NULL;
 	u32 tmp;
-	u8 *sadb;
+	u8 *sadb = NULL;
 	int sad_count;
 
 	list_for_each_entry(connector, &encoder->dev->mode_config.connector_list, head) {
@@ -49,8 +49,8 @@ static void dce3_2_afmt_write_speaker_allocation(struct drm_encoder *encoder)
 
 	sad_count = drm_edid_to_speaker_allocation(radeon_connector->edid, &sadb);
 	if (sad_count < 0) {
-		DRM_ERROR("Couldn't read Speaker Allocation Data Block: %d\n", sad_count);
-		return;
+		DRM_DEBUG("Couldn't read Speaker Allocation Data Block: %d\n", sad_count);
+		sad_count = 0;
 	}
 
 	/* program the speaker allocation */
@@ -165,7 +165,7 @@ void dce3_1_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *m
 
 	/* disable audio prior to setting up hw */
 	dig->afmt->pin = r600_audio_get_pin(rdev);
-	r600_audio_enable(rdev, dig->afmt->pin, false);
+	r600_audio_enable(rdev, dig->afmt->pin, 0);
 
 	r600_audio_set_dto(encoder, mode->clock);
 
@@ -240,5 +240,5 @@ void dce3_1_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *m
 	r600_hdmi_audio_workaround(encoder);
 
 	/* enable audio after to setting up hw */
-	r600_audio_enable(rdev, dig->afmt->pin, true);
+	r600_audio_enable(rdev, dig->afmt->pin, 0xf);
 }
diff --git a/drivers/gpu/drm/radeon/dce6_afmt.c b/drivers/gpu/drm/radeon/dce6_afmt.c
index ab29f953a767..f312edf4d50e 100644
--- a/drivers/gpu/drm/radeon/dce6_afmt.c
+++ b/drivers/gpu/drm/radeon/dce6_afmt.c
@@ -155,7 +155,7 @@ void dce6_afmt_write_speaker_allocation(struct drm_encoder *encoder)
 	struct drm_connector *connector;
 	struct radeon_connector *radeon_connector = NULL;
 	u32 offset, tmp;
-	u8 *sadb;
+	u8 *sadb = NULL;
 	int sad_count;
 
 	if (!dig || !dig->afmt || !dig->afmt->pin)
@@ -176,9 +176,9 @@ void dce6_afmt_write_speaker_allocation(struct drm_encoder *encoder)
 	}
 
 	sad_count = drm_edid_to_speaker_allocation(radeon_connector_edid(connector), &sadb);
-	if (sad_count <= 0) {
-		DRM_ERROR("Couldn't read Speaker Allocation Data Block: %d\n", sad_count);
-		return;
+	if (sad_count < 0) {
+		DRM_DEBUG("Couldn't read Speaker Allocation Data Block: %d\n", sad_count);
+		sad_count = 0;
 	}
 
 	/* program the speaker allocation */
@@ -284,13 +284,13 @@ static int dce6_audio_chipset_supported(struct radeon_device *rdev)
 
 void dce6_audio_enable(struct radeon_device *rdev,
 		       struct r600_audio_pin *pin,
-		       bool enable)
+		       u8 enable_mask)
 {
 	if (!pin)
 		return;
 
-	WREG32_ENDPOINT(pin->offset, AZ_F0_CODEC_PIN_CONTROL_HOTPLUG_CONTROL,
-			enable ? AUDIO_ENABLED : 0);
+	WREG32_ENDPOINT(pin->offset, AZ_F0_CODEC_PIN_CONTROL_HOT_PLUG_CONTROL,
+			enable_mask ? AUDIO_ENABLED : 0);
 }
 
 static const u32 pin_offsets[7] =
diff --git a/drivers/gpu/drm/drm_buffer.c b/drivers/gpu/drm/radeon/drm_buffer.c
index 86a4a4a60afc..f4e0f3a3d7b1 100644
--- a/drivers/gpu/drm/drm_buffer.c
+++ b/drivers/gpu/drm/radeon/drm_buffer.c
@@ -33,7 +33,7 @@
  */
 
 #include <linux/export.h>
-#include <drm/drm_buffer.h>
+#include "drm_buffer.h"
 
 /**
  * Allocate the drm buffer object.
@@ -86,7 +86,6 @@ error_out:
 	kfree(*buf);
 	return -ENOMEM;
 }
-EXPORT_SYMBOL(drm_buffer_alloc);
 
 /**
  * Copy the user data to the begin of the buffer and reset the processing
@@ -123,7 +122,6 @@ int drm_buffer_copy_from_user(struct drm_buffer *buf,
 	buf->iterator = 0;
 	return 0;
 }
-EXPORT_SYMBOL(drm_buffer_copy_from_user);
 
 /**
  * Free the drm buffer object
@@ -141,7 +139,6 @@ void drm_buffer_free(struct drm_buffer *buf)
 		kfree(buf);
 	}
 }
-EXPORT_SYMBOL(drm_buffer_free);
 
 /**
  * Read an object from buffer that may be split to multiple parts. If object
@@ -178,4 +175,3 @@ void *drm_buffer_read_object(struct drm_buffer *buf,
 	drm_buffer_advance(buf, objsize);
 	return obj;
 }
-EXPORT_SYMBOL(drm_buffer_read_object);
diff --git a/drivers/gpu/drm/radeon/drm_buffer.h b/drivers/gpu/drm/radeon/drm_buffer.h
new file mode 100644
index 000000000000..c80d3a340b94
--- /dev/null
+++ b/drivers/gpu/drm/radeon/drm_buffer.h
@@ -0,0 +1,148 @@
+/**************************************************************************
+ *
+ * Copyright 2010 Pauli Nieminen.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ *
+ **************************************************************************/
+/*
+ * Multipart buffer for coping data which is larger than the page size.
+ *
+ * Authors:
+ * Pauli Nieminen <suokkos-at-gmail-dot-com>
+ */
+
+#ifndef _DRM_BUFFER_H_
+#define _DRM_BUFFER_H_
+
+#include <drm/drmP.h>
+
+struct drm_buffer {
+	int iterator;
+	int size;
+	char *data[];
+};
+
+
+/**
+ * Return the index of page that buffer is currently pointing at.
+ */
+static inline int drm_buffer_page(struct drm_buffer *buf)
+{
+	return buf->iterator / PAGE_SIZE;
+}
+/**
+ * Return the index of the current byte in the page
+ */
+static inline int drm_buffer_index(struct drm_buffer *buf)
+{
+	return buf->iterator & (PAGE_SIZE - 1);
+}
+/**
+ * Return number of bytes that is left to process
+ */
+static inline int drm_buffer_unprocessed(struct drm_buffer *buf)
+{
+	return buf->size - buf->iterator;
+}
+
+/**
+ * Advance the buffer iterator number of bytes that is given.
+ */
+static inline void drm_buffer_advance(struct drm_buffer *buf, int bytes)
+{
+	buf->iterator += bytes;
+}
+
+/**
+ * Allocate the drm buffer object.
+ *
+ *   buf: A pointer to a pointer where the object is stored.
+ *   size: The number of bytes to allocate.
+ */
+extern int drm_buffer_alloc(struct drm_buffer **buf, int size);
+
+/**
+ * Copy the user data to the begin of the buffer and reset the processing
+ * iterator.
+ *
+ *   user_data: A pointer the data that is copied to the buffer.
+ *   size: The Number of bytes to copy.
+ */
+extern int drm_buffer_copy_from_user(struct drm_buffer *buf,
+		void __user *user_data, int size);
+
+/**
+ * Free the drm buffer object
+ */
+extern void drm_buffer_free(struct drm_buffer *buf);
+
+/**
+ * Read an object from buffer that may be split to multiple parts. If object
+ * is not split function just returns the pointer to object in buffer. But in
+ * case of split object data is copied to given stack object that is suplied
+ * by caller.
+ *
+ * The processing location of the buffer is also advanced to the next byte
+ * after the object.
+ *
+ *   objsize: The size of the objet in bytes.
+ *   stack_obj: A pointer to a memory location where object can be copied.
+ */
+extern void *drm_buffer_read_object(struct drm_buffer *buf,
+		int objsize, void *stack_obj);
+
+/**
+ * Returns the pointer to the dword which is offset number of elements from the
+ * current processing location.
+ *
+ * Caller must make sure that dword is not split in the buffer. This
+ * requirement is easily met if all the sizes of objects in buffer are
+ * multiples of dword and PAGE_SIZE is multiple dword.
+ *
+ * Call to this function doesn't change the processing location.
+ *
+ *   offset: The index of the dword relative to the internat iterator.
+ */
+static inline void *drm_buffer_pointer_to_dword(struct drm_buffer *buffer,
+		int offset)
+{
+	int iter = buffer->iterator + offset * 4;
+	return &buffer->data[iter / PAGE_SIZE][iter & (PAGE_SIZE - 1)];
+}
+/**
+ * Returns the pointer to the dword which is offset number of elements from
+ * the current processing location.
+ *
+ * Call to this function doesn't change the processing location.
+ *
+ *   offset: The index of the byte relative to the internat iterator.
+ */
+static inline void *drm_buffer_pointer_to_byte(struct drm_buffer *buffer,
+		int offset)
+{
+	int iter = buffer->iterator + offset;
+	return &buffer->data[iter / PAGE_SIZE][iter & (PAGE_SIZE - 1)];
+}
+
+#endif
diff --git a/drivers/gpu/drm/radeon/evergreen.c b/drivers/gpu/drm/radeon/evergreen.c
index 4fedd14e670a..a31f1ca40c6a 100644
--- a/drivers/gpu/drm/radeon/evergreen.c
+++ b/drivers/gpu/drm/radeon/evergreen.c
@@ -22,7 +22,6 @@
  * Authors: Alex Deucher
  */
 #include <linux/firmware.h>
-#include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <drm/drmP.h>
 #include "radeon.h"
@@ -2869,7 +2868,7 @@ static int evergreen_cp_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
 	radeon_ring_write(ring, 0);
 	radeon_ring_write(ring, 0);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	cp_me = 0xff;
 	WREG32(CP_ME_CNTL, cp_me);
@@ -2912,7 +2911,7 @@ static int evergreen_cp_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
 	radeon_ring_write(ring, 0x00000010); /*  */
 
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	return 0;
 }
@@ -4023,7 +4022,7 @@ int sumo_rlc_init(struct radeon_device *rdev)
 		if (rdev->rlc.save_restore_obj == NULL) {
 			r = radeon_bo_create(rdev, dws * 4, PAGE_SIZE, true,
 					     RADEON_GEM_DOMAIN_VRAM, 0, NULL,
-					     &rdev->rlc.save_restore_obj);
+					     NULL, &rdev->rlc.save_restore_obj);
 			if (r) {
 				dev_warn(rdev->dev, "(%d) create RLC sr bo failed\n", r);
 				return r;
@@ -4102,7 +4101,7 @@ int sumo_rlc_init(struct radeon_device *rdev)
 		if (rdev->rlc.clear_state_obj == NULL) {
 			r = radeon_bo_create(rdev, dws * 4, PAGE_SIZE, true,
 					     RADEON_GEM_DOMAIN_VRAM, 0, NULL,
-					     &rdev->rlc.clear_state_obj);
+					     NULL, &rdev->rlc.clear_state_obj);
 			if (r) {
 				dev_warn(rdev->dev, "(%d) create RLC c bo failed\n", r);
 				sumo_rlc_fini(rdev);
@@ -4179,7 +4178,7 @@ int sumo_rlc_init(struct radeon_device *rdev)
 			r = radeon_bo_create(rdev, rdev->rlc.cp_table_size,
 					     PAGE_SIZE, true,
 					     RADEON_GEM_DOMAIN_VRAM, 0, NULL,
-					     &rdev->rlc.cp_table_obj);
+					     NULL, &rdev->rlc.cp_table_obj);
 			if (r) {
 				dev_warn(rdev->dev, "(%d) create RLC cp table bo failed\n", r);
 				sumo_rlc_fini(rdev);
@@ -4749,17 +4748,17 @@ static u32 evergreen_get_ih_wptr(struct radeon_device *rdev)
 		wptr = RREG32(IH_RB_WPTR);
 
 	if (wptr & RB_OVERFLOW) {
+		wptr &= ~RB_OVERFLOW;
 		/* When a ring buffer overflow happen start parsing interrupt
 		 * from the last not overwritten vector (wptr + 16). Hopefully
 		 * this should allow us to catchup.
 		 */
-		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, %d, %d)\n",
-			wptr, rdev->ih.rptr, (wptr + 16) + rdev->ih.ptr_mask);
+		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, 0x%08X, 0x%08X)\n",
+			 wptr, rdev->ih.rptr, (wptr + 16) & rdev->ih.ptr_mask);
 		rdev->ih.rptr = (wptr + 16) & rdev->ih.ptr_mask;
 		tmp = RREG32(IH_RB_CNTL);
 		tmp |= IH_WPTR_OVERFLOW_CLEAR;
 		WREG32(IH_RB_CNTL, tmp);
-		wptr &= ~RB_OVERFLOW;
 	}
 	return (wptr & rdev->ih.ptr_mask);
 }
@@ -5137,6 +5136,7 @@ restart_ih:
 		/* wptr/rptr are in bytes! */
 		rptr += 16;
 		rptr &= rdev->ih.ptr_mask;
+		WREG32(IH_RB_RPTR, rptr);
 	}
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
@@ -5145,7 +5145,6 @@ restart_ih:
 	if (queue_thermal && rdev->pm.dpm_enabled)
 		schedule_work(&rdev->pm.dpm.thermal.work);
 	rdev->ih.rptr = rptr;
-	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	atomic_set(&rdev->ih.lock, 0);
 
 	/* make sure wptr hasn't changed while processing */
diff --git a/drivers/gpu/drm/radeon/evergreen_dma.c b/drivers/gpu/drm/radeon/evergreen_dma.c
index 478caefe0fef..66bcfadeedd1 100644
--- a/drivers/gpu/drm/radeon/evergreen_dma.c
+++ b/drivers/gpu/drm/radeon/evergreen_dma.c
@@ -104,12 +104,14 @@ void evergreen_dma_ring_ib_execute(struct radeon_device *rdev,
  * Used by the radeon ttm implementation to move pages if
  * registered as the asic copy callback.
  */
-int evergreen_copy_dma(struct radeon_device *rdev,
-		       uint64_t src_offset, uint64_t dst_offset,
-		       unsigned num_gpu_pages,
-		       struct radeon_fence **fence)
+struct radeon_fence *evergreen_copy_dma(struct radeon_device *rdev,
+					uint64_t src_offset,
+					uint64_t dst_offset,
+					unsigned num_gpu_pages,
+					struct reservation_object *resv)
 {
 	struct radeon_semaphore *sem = NULL;
+	struct radeon_fence *fence;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_dw, cur_size_in_dw;
@@ -119,7 +121,7 @@ int evergreen_copy_dma(struct radeon_device *rdev,
 	r = radeon_semaphore_create(rdev, &sem);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 
 	size_in_dw = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT) / 4;
@@ -128,10 +130,10 @@ int evergreen_copy_dma(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_to(sem, *fence);
+	radeon_semaphore_sync_resv(rdev, sem, resv, false);
 	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
@@ -148,17 +150,17 @@ int evergreen_copy_dma(struct radeon_device *rdev,
 		dst_offset += cur_size_in_dw * 4;
 	}
 
-	r = radeon_fence_emit(rdev, fence, ring->idx);
+	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
-	radeon_semaphore_free(rdev, &sem, *fence);
+	radeon_ring_unlock_commit(rdev, ring, false);
+	radeon_semaphore_free(rdev, &sem, fence);
 
-	return r;
+	return fence;
 }
 
 /**
diff --git a/drivers/gpu/drm/radeon/evergreen_hdmi.c b/drivers/gpu/drm/radeon/evergreen_hdmi.c
index 278c7a139d74..53abd9b17a50 100644
--- a/drivers/gpu/drm/radeon/evergreen_hdmi.c
+++ b/drivers/gpu/drm/radeon/evergreen_hdmi.c
@@ -38,6 +38,37 @@ extern void dce6_afmt_select_pin(struct drm_encoder *encoder);
 extern void dce6_afmt_write_latency_fields(struct drm_encoder *encoder,
 					   struct drm_display_mode *mode);
 
+/* enable the audio stream */
+static void dce4_audio_enable(struct radeon_device *rdev,
+			      struct r600_audio_pin *pin,
+			      u8 enable_mask)
+{
+	u32 tmp = RREG32(AZ_HOT_PLUG_CONTROL);
+
+	if (!pin)
+		return;
+
+	if (enable_mask) {
+		tmp |= AUDIO_ENABLED;
+		if (enable_mask & 1)
+			tmp |= PIN0_AUDIO_ENABLED;
+		if (enable_mask & 2)
+			tmp |= PIN1_AUDIO_ENABLED;
+		if (enable_mask & 4)
+			tmp |= PIN2_AUDIO_ENABLED;
+		if (enable_mask & 8)
+			tmp |= PIN3_AUDIO_ENABLED;
+	} else {
+		tmp &= ~(AUDIO_ENABLED |
+			 PIN0_AUDIO_ENABLED |
+			 PIN1_AUDIO_ENABLED |
+			 PIN2_AUDIO_ENABLED |
+			 PIN3_AUDIO_ENABLED);
+	}
+
+	WREG32(AZ_HOT_PLUG_CONTROL, tmp);
+}
+
 /*
  * update the N and CTS parameters for a given pixel clock rate
  */
@@ -102,7 +133,7 @@ static void dce4_afmt_write_speaker_allocation(struct drm_encoder *encoder)
 	struct drm_connector *connector;
 	struct radeon_connector *radeon_connector = NULL;
 	u32 tmp;
-	u8 *sadb;
+	u8 *sadb = NULL;
 	int sad_count;
 
 	list_for_each_entry(connector, &encoder->dev->mode_config.connector_list, head) {
@@ -118,9 +149,9 @@ static void dce4_afmt_write_speaker_allocation(struct drm_encoder *encoder)
 	}
 
 	sad_count = drm_edid_to_speaker_allocation(radeon_connector_edid(connector), &sadb);
-	if (sad_count <= 0) {
-		DRM_ERROR("Couldn't read Speaker Allocation Data Block: %d\n", sad_count);
-		return;
+	if (sad_count < 0) {
+		DRM_DEBUG("Couldn't read Speaker Allocation Data Block: %d\n", sad_count);
+		sad_count = 0;
 	}
 
 	/* program the speaker allocation */
@@ -318,10 +349,10 @@ void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode
 	/* disable audio prior to setting up hw */
 	if (ASIC_IS_DCE6(rdev)) {
 		dig->afmt->pin = dce6_audio_get_pin(rdev);
-		dce6_audio_enable(rdev, dig->afmt->pin, false);
+		dce6_audio_enable(rdev, dig->afmt->pin, 0);
 	} else {
 		dig->afmt->pin = r600_audio_get_pin(rdev);
-		r600_audio_enable(rdev, dig->afmt->pin, false);
+		dce4_audio_enable(rdev, dig->afmt->pin, 0);
 	}
 
 	evergreen_audio_set_dto(encoder, mode->clock);
@@ -463,13 +494,15 @@ void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode
 
 	/* enable audio after to setting up hw */
 	if (ASIC_IS_DCE6(rdev))
-		dce6_audio_enable(rdev, dig->afmt->pin, true);
+		dce6_audio_enable(rdev, dig->afmt->pin, 1);
 	else
-		r600_audio_enable(rdev, dig->afmt->pin, true);
+		dce4_audio_enable(rdev, dig->afmt->pin, 0xf);
 }
 
 void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable)
 {
+	struct drm_device *dev = encoder->dev;
+	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
 	struct radeon_encoder_atom_dig *dig = radeon_encoder->enc_priv;
 
@@ -482,6 +515,14 @@ void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable)
 	if (!enable && !dig->afmt->enabled)
 		return;
 
+	if (!enable && dig->afmt->pin) {
+		if (ASIC_IS_DCE6(rdev))
+			dce6_audio_enable(rdev, dig->afmt->pin, 0);
+		else
+			dce4_audio_enable(rdev, dig->afmt->pin, 0);
+		dig->afmt->pin = NULL;
+	}
+
 	dig->afmt->enabled = enable;
 
 	DRM_DEBUG("%sabling HDMI interface @ 0x%04X for encoder 0x%x\n",
diff --git a/drivers/gpu/drm/radeon/kv_dpm.c b/drivers/gpu/drm/radeon/kv_dpm.c
index 9ef8c38f2d66..1dd976f447fa 100644
--- a/drivers/gpu/drm/radeon/kv_dpm.c
+++ b/drivers/gpu/drm/radeon/kv_dpm.c
@@ -33,6 +33,8 @@
 #define KV_MINIMUM_ENGINE_CLOCK         800
 #define SMC_RAM_END                     0x40000
 
+static int kv_enable_nb_dpm(struct radeon_device *rdev,
+			    bool enable);
 static void kv_init_graphics_levels(struct radeon_device *rdev);
 static int kv_calculate_ds_divider(struct radeon_device *rdev);
 static int kv_calculate_nbps_level_settings(struct radeon_device *rdev);
@@ -1295,6 +1297,9 @@ void kv_dpm_disable(struct radeon_device *rdev)
 {
 	kv_smc_bapm_enable(rdev, false);
 
+	if (rdev->family == CHIP_MULLINS)
+		kv_enable_nb_dpm(rdev, false);
+
 	/* powerup blocks */
 	kv_dpm_powergate_acp(rdev, false);
 	kv_dpm_powergate_samu(rdev, false);
@@ -1438,14 +1443,14 @@ static int kv_update_uvd_dpm(struct radeon_device *rdev, bool gate)
 	return kv_enable_uvd_dpm(rdev, !gate);
 }
 
-static u8 kv_get_vce_boot_level(struct radeon_device *rdev)
+static u8 kv_get_vce_boot_level(struct radeon_device *rdev, u32 evclk)
 {
 	u8 i;
 	struct radeon_vce_clock_voltage_dependency_table *table =
 		&rdev->pm.dpm.dyn_state.vce_clock_voltage_dependency_table;
 
 	for (i = 0; i < table->count; i++) {
-		if (table->entries[i].evclk >= 0) /* XXX */
+		if (table->entries[i].evclk >= evclk)
 			break;
 	}
 
@@ -1468,7 +1473,7 @@ static int kv_update_vce_dpm(struct radeon_device *rdev,
 		if (pi->caps_stable_p_state)
 			pi->vce_boot_level = table->count - 1;
 		else
-			pi->vce_boot_level = kv_get_vce_boot_level(rdev);
+			pi->vce_boot_level = kv_get_vce_boot_level(rdev, radeon_new_state->evclk);
 
 		ret = kv_copy_bytes_to_smc(rdev,
 					   pi->dpm_table_start +
@@ -1769,15 +1774,24 @@ static int kv_update_dfs_bypass_settings(struct radeon_device *rdev,
 	return ret;
 }
 
-static int kv_enable_nb_dpm(struct radeon_device *rdev)
+static int kv_enable_nb_dpm(struct radeon_device *rdev,
+			    bool enable)
 {
 	struct kv_power_info *pi = kv_get_pi(rdev);
 	int ret = 0;
 
-	if (pi->enable_nb_dpm && !pi->nb_dpm_enabled) {
-		ret = kv_notify_message_to_smu(rdev, PPSMC_MSG_NBDPM_Enable);
-		if (ret == 0)
-			pi->nb_dpm_enabled = true;
+	if (enable) {
+		if (pi->enable_nb_dpm && !pi->nb_dpm_enabled) {
+			ret = kv_notify_message_to_smu(rdev, PPSMC_MSG_NBDPM_Enable);
+			if (ret == 0)
+				pi->nb_dpm_enabled = true;
+		}
+	} else {
+		if (pi->enable_nb_dpm && pi->nb_dpm_enabled) {
+			ret = kv_notify_message_to_smu(rdev, PPSMC_MSG_NBDPM_Disable);
+			if (ret == 0)
+				pi->nb_dpm_enabled = false;
+		}
 	}
 
 	return ret;
@@ -1864,7 +1878,7 @@ int kv_dpm_set_power_state(struct radeon_device *rdev)
 			}
 			kv_update_sclk_t(rdev);
 			if (rdev->family == CHIP_MULLINS)
-				kv_enable_nb_dpm(rdev);
+				kv_enable_nb_dpm(rdev, true);
 		}
 	} else {
 		if (pi->enable_dpm) {
@@ -1889,7 +1903,7 @@ int kv_dpm_set_power_state(struct radeon_device *rdev)
 			}
 			kv_update_acp_boot_level(rdev);
 			kv_update_sclk_t(rdev);
-			kv_enable_nb_dpm(rdev);
+			kv_enable_nb_dpm(rdev, true);
 		}
 	}
 
@@ -2726,7 +2740,10 @@ int kv_dpm_init(struct radeon_device *rdev)
 	pi->caps_sclk_ds = true;
 	pi->enable_auto_thermal_throttling = true;
 	pi->disable_nb_ps3_in_battery = false;
-	pi->bapm_enable = true;
+	if (radeon_bapm == 0)
+		pi->bapm_enable = false;
+	else
+		pi->bapm_enable = true;
 	pi->voltage_drop_t = 0;
 	pi->caps_sclk_throttle_low_notification = false;
 	pi->caps_fps = false; /* true? */
@@ -2770,6 +2787,8 @@ void kv_dpm_debugfs_print_current_performance_level(struct radeon_device *rdev,
 		tmp = (RREG32_SMC(SMU_VOLTAGE_STATUS) & SMU_VOLTAGE_CURRENT_LEVEL_MASK) >>
 			SMU_VOLTAGE_CURRENT_LEVEL_SHIFT;
 		vddc = kv_convert_8bit_index_to_voltage(rdev, (u16)tmp);
+		seq_printf(m, "uvd    %sabled\n", pi->uvd_power_gated ? "dis" : "en");
+		seq_printf(m, "vce    %sabled\n", pi->vce_power_gated ? "dis" : "en");
 		seq_printf(m, "power level %d    sclk: %u vddc: %u\n",
 			   current_index, sclk, vddc);
 	}
diff --git a/drivers/gpu/drm/radeon/ni.c b/drivers/gpu/drm/radeon/ni.c
index 327b85f7fd0d..3faee58946dd 100644
--- a/drivers/gpu/drm/radeon/ni.c
+++ b/drivers/gpu/drm/radeon/ni.c
@@ -1271,7 +1271,7 @@ static int cayman_pcie_gart_enable(struct radeon_device *rdev)
 		WREG32(VM_CONTEXT0_PAGE_TABLE_START_ADDR + (i << 2), 0);
 		WREG32(VM_CONTEXT0_PAGE_TABLE_END_ADDR + (i << 2), rdev->vm_manager.max_pfn);
 		WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2),
-			rdev->gart.table_addr >> 12);
+		       rdev->vm_manager.saved_table_addr[i]);
 	}
 
 	/* enable context1-7 */
@@ -1303,6 +1303,13 @@ static int cayman_pcie_gart_enable(struct radeon_device *rdev)
 
 static void cayman_pcie_gart_disable(struct radeon_device *rdev)
 {
+	unsigned i;
+
+	for (i = 1; i < 8; ++i) {
+		rdev->vm_manager.saved_table_addr[i] = RREG32(
+			VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2));
+	}
+
 	/* Disable all tables */
 	WREG32(VM_CONTEXT0_CNTL, 0);
 	WREG32(VM_CONTEXT1_CNTL, 0);
@@ -1505,7 +1512,7 @@ static int cayman_cp_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
 	radeon_ring_write(ring, 0);
 	radeon_ring_write(ring, 0);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	cayman_cp_enable(rdev, true);
 
@@ -1547,7 +1554,7 @@ static int cayman_cp_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
 	radeon_ring_write(ring, 0x00000010); /*  */
 
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	/* XXX init other rings */
 
diff --git a/drivers/gpu/drm/radeon/ni_dma.c b/drivers/gpu/drm/radeon/ni_dma.c
index 8a3e6221cece..f26f0a9fb522 100644
--- a/drivers/gpu/drm/radeon/ni_dma.c
+++ b/drivers/gpu/drm/radeon/ni_dma.c
@@ -191,12 +191,6 @@ int cayman_dma_resume(struct radeon_device *rdev)
 	u32 reg_offset, wb_offset;
 	int i, r;
 
-	/* Reset dma */
-	WREG32(SRBM_SOFT_RESET, SOFT_RESET_DMA | SOFT_RESET_DMA1);
-	RREG32(SRBM_SOFT_RESET);
-	udelay(50);
-	WREG32(SRBM_SOFT_RESET, 0);
-
 	for (i = 0; i < 2; i++) {
 		if (i == 0) {
 			ring = &rdev->ring[R600_RING_TYPE_DMA_INDEX];
diff --git a/drivers/gpu/drm/radeon/ni_dpm.c b/drivers/gpu/drm/radeon/ni_dpm.c
index 01fc4888e6fe..6d2f16cf2c1c 100644
--- a/drivers/gpu/drm/radeon/ni_dpm.c
+++ b/drivers/gpu/drm/radeon/ni_dpm.c
@@ -23,6 +23,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "nid.h"
 #include "r600_dpm.h"
 #include "ni_dpm.h"
@@ -789,7 +790,6 @@ static void ni_apply_state_adjust_rules(struct radeon_device *rdev,
 	bool disable_mclk_switching;
 	u32 mclk;
 	u16 vddci;
-	u32 max_sclk_vddc, max_mclk_vddci, max_mclk_vddc;
 	int i;
 
 	if ((rdev->pm.dpm.new_active_crtc_count > 1) ||
@@ -816,29 +816,6 @@ static void ni_apply_state_adjust_rules(struct radeon_device *rdev,
 		}
 	}
 
-	/* limit clocks to max supported clocks based on voltage dependency tables */
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddc_dependency_on_sclk,
-							&max_sclk_vddc);
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddci_dependency_on_mclk,
-							&max_mclk_vddci);
-	btc_get_max_clock_from_voltage_dependency_table(&rdev->pm.dpm.dyn_state.vddc_dependency_on_mclk,
-							&max_mclk_vddc);
-
-	for (i = 0; i < ps->performance_level_count; i++) {
-		if (max_sclk_vddc) {
-			if (ps->performance_levels[i].sclk > max_sclk_vddc)
-				ps->performance_levels[i].sclk = max_sclk_vddc;
-		}
-		if (max_mclk_vddci) {
-			if (ps->performance_levels[i].mclk > max_mclk_vddci)
-				ps->performance_levels[i].mclk = max_mclk_vddci;
-		}
-		if (max_mclk_vddc) {
-			if (ps->performance_levels[i].mclk > max_mclk_vddc)
-				ps->performance_levels[i].mclk = max_mclk_vddc;
-		}
-	}
-
 	/* XXX validate the min clocks required for display */
 
 	/* adjust low state */
diff --git a/drivers/gpu/drm/radeon/r100.c b/drivers/gpu/drm/radeon/r100.c
index 04b5940b8923..10f8be0ee173 100644
--- a/drivers/gpu/drm/radeon/r100.c
+++ b/drivers/gpu/drm/radeon/r100.c
@@ -821,6 +821,20 @@ u32 r100_get_vblank_counter(struct radeon_device *rdev, int crtc)
 		return RREG32(RADEON_CRTC2_CRNT_FRAME);
 }
 
+/**
+ * r100_ring_hdp_flush - flush Host Data Path via the ring buffer
+ * rdev: radeon device structure
+ * ring: ring buffer struct for emitting packets
+ */
+static void r100_ring_hdp_flush(struct radeon_device *rdev, struct radeon_ring *ring)
+{
+	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
+	radeon_ring_write(ring, rdev->config.r100.hdp_cntl |
+				RADEON_HDP_READ_BUFFER_INVALIDATE);
+	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
+	radeon_ring_write(ring, rdev->config.r100.hdp_cntl);
+}
+
 /* Who ever call radeon_fence_emit should call ring_lock and ask
  * for enough space (today caller are ib schedule and buffer move) */
 void r100_fence_ring_emit(struct radeon_device *rdev,
@@ -855,13 +869,14 @@ bool r100_semaphore_ring_emit(struct radeon_device *rdev,
 	return false;
 }
 
-int r100_copy_blit(struct radeon_device *rdev,
-		   uint64_t src_offset,
-		   uint64_t dst_offset,
-		   unsigned num_gpu_pages,
-		   struct radeon_fence **fence)
+struct radeon_fence *r100_copy_blit(struct radeon_device *rdev,
+				    uint64_t src_offset,
+				    uint64_t dst_offset,
+				    unsigned num_gpu_pages,
+				    struct reservation_object *resv)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	struct radeon_fence *fence;
 	uint32_t cur_pages;
 	uint32_t stride_bytes = RADEON_GPU_PAGE_SIZE;
 	uint32_t pitch;
@@ -882,7 +897,7 @@ int r100_copy_blit(struct radeon_device *rdev,
 	r = radeon_ring_lock(rdev, ring, ndw);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d) asking for %u dw.\n", r, ndw);
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 	}
 	while (num_gpu_pages > 0) {
 		cur_pages = num_gpu_pages;
@@ -922,11 +937,13 @@ int r100_copy_blit(struct radeon_device *rdev,
 			  RADEON_WAIT_2D_IDLECLEAN |
 			  RADEON_WAIT_HOST_IDLECLEAN |
 			  RADEON_WAIT_DMA_GUI_IDLE);
-	if (fence) {
-		r = radeon_fence_emit(rdev, fence, RADEON_RING_TYPE_GFX_INDEX);
+	r = radeon_fence_emit(rdev, &fence, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		radeon_ring_unlock_undo(rdev, ring);
+		return ERR_PTR(r);
 	}
-	radeon_ring_unlock_commit(rdev, ring);
-	return r;
+	radeon_ring_unlock_commit(rdev, ring, false);
+	return fence;
 }
 
 static int r100_cp_wait_for_idle(struct radeon_device *rdev)
@@ -958,7 +975,7 @@ void r100_ring_start(struct radeon_device *rdev, struct radeon_ring *ring)
 			  RADEON_ISYNC_ANY3D_IDLE2D |
 			  RADEON_ISYNC_WAIT_IDLEGUI |
 			  RADEON_ISYNC_CPSCRATCH_IDLEGUI);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 }
 
 
@@ -1056,20 +1073,6 @@ void r100_gfx_set_wptr(struct radeon_device *rdev,
 	(void)RREG32(RADEON_CP_RB_WPTR);
 }
 
-/**
- * r100_ring_hdp_flush - flush Host Data Path via the ring buffer
- * rdev: radeon device structure
- * ring: ring buffer struct for emitting packets
- */
-void r100_ring_hdp_flush(struct radeon_device *rdev, struct radeon_ring *ring)
-{
-	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
-	radeon_ring_write(ring, rdev->config.r100.hdp_cntl |
-				RADEON_HDP_READ_BUFFER_INVALIDATE);
-	radeon_ring_write(ring, PACKET0(RADEON_HOST_PATH_CNTL, 0));
-	radeon_ring_write(ring, rdev->config.r100.hdp_cntl);
-}
-
 static void r100_cp_load_microcode(struct radeon_device *rdev)
 {
 	const __be32 *fw_data;
@@ -3638,7 +3641,7 @@ int r100_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	}
 	radeon_ring_write(ring, PACKET0(scratch, 0));
 	radeon_ring_write(ring, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 	for (i = 0; i < rdev->usec_timeout; i++) {
 		tmp = RREG32(scratch);
 		if (tmp == 0xDEADBEEF) {
@@ -3700,7 +3703,7 @@ int r100_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	ib.ptr[6] = PACKET2(0);
 	ib.ptr[7] = PACKET2(0);
 	ib.length_dw = 8;
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 		DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
 		goto free_ib;
diff --git a/drivers/gpu/drm/radeon/r200.c b/drivers/gpu/drm/radeon/r200.c
index 58f0473aa73f..732d4938aab7 100644
--- a/drivers/gpu/drm/radeon/r200.c
+++ b/drivers/gpu/drm/radeon/r200.c
@@ -80,13 +80,14 @@ static int r200_get_vtx_size_0(uint32_t vtx_fmt_0)
 	return vtx_size;
 }
 
-int r200_copy_dma(struct radeon_device *rdev,
-		  uint64_t src_offset,
-		  uint64_t dst_offset,
-		  unsigned num_gpu_pages,
-		  struct radeon_fence **fence)
+struct radeon_fence *r200_copy_dma(struct radeon_device *rdev,
+				   uint64_t src_offset,
+				   uint64_t dst_offset,
+				   unsigned num_gpu_pages,
+				   struct reservation_object *resv)
 {
 	struct radeon_ring *ring = &rdev->ring[RADEON_RING_TYPE_GFX_INDEX];
+	struct radeon_fence *fence;
 	uint32_t size;
 	uint32_t cur_size;
 	int i, num_loops;
@@ -98,7 +99,7 @@ int r200_copy_dma(struct radeon_device *rdev,
 	r = radeon_ring_lock(rdev, ring, num_loops * 4 + 64);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 	/* Must wait for 2D idle & clean before DMA or hangs might happen */
 	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
@@ -118,11 +119,13 @@ int r200_copy_dma(struct radeon_device *rdev,
 	}
 	radeon_ring_write(ring, PACKET0(RADEON_WAIT_UNTIL, 0));
 	radeon_ring_write(ring, RADEON_WAIT_DMA_GUI_IDLE);
-	if (fence) {
-		r = radeon_fence_emit(rdev, fence, RADEON_RING_TYPE_GFX_INDEX);
+	r = radeon_fence_emit(rdev, &fence, RADEON_RING_TYPE_GFX_INDEX);
+	if (r) {
+		radeon_ring_unlock_undo(rdev, ring);
+		return ERR_PTR(r);
 	}
-	radeon_ring_unlock_commit(rdev, ring);
-	return r;
+	radeon_ring_unlock_commit(rdev, ring, false);
+	return fence;
 }
 
 
diff --git a/drivers/gpu/drm/radeon/r300.c b/drivers/gpu/drm/radeon/r300.c
index 75b30338c226..1bc4704034ce 100644
--- a/drivers/gpu/drm/radeon/r300.c
+++ b/drivers/gpu/drm/radeon/r300.c
@@ -295,7 +295,7 @@ void r300_ring_start(struct radeon_device *rdev, struct radeon_ring *ring)
 	radeon_ring_write(ring,
 			  R300_GEOMETRY_ROUND_NEAREST |
 			  R300_COLOR_ROUND_NEAREST);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 }
 
 static void r300_errata(struct radeon_device *rdev)
diff --git a/drivers/gpu/drm/radeon/r300_cmdbuf.c b/drivers/gpu/drm/radeon/r300_cmdbuf.c
index 84b1d5367a11..9418e388b045 100644
--- a/drivers/gpu/drm/radeon/r300_cmdbuf.c
+++ b/drivers/gpu/drm/radeon/r300_cmdbuf.c
@@ -34,10 +34,10 @@
  */
 
 #include <drm/drmP.h>
-#include <drm/drm_buffer.h>
 #include <drm/radeon_drm.h>
 #include "radeon_drv.h"
 #include "r300_reg.h"
+#include "drm_buffer.h"
 
 #include <asm/unaligned.h>
 
diff --git a/drivers/gpu/drm/radeon/r420.c b/drivers/gpu/drm/radeon/r420.c
index 802b19220a21..2828605aef3f 100644
--- a/drivers/gpu/drm/radeon/r420.c
+++ b/drivers/gpu/drm/radeon/r420.c
@@ -219,7 +219,7 @@ static void r420_cp_errata_init(struct radeon_device *rdev)
 	radeon_ring_write(ring, PACKET0(R300_CP_RESYNC_ADDR, 1));
 	radeon_ring_write(ring, rdev->config.r300.resync_scratch);
 	radeon_ring_write(ring, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 }
 
 static void r420_cp_errata_fini(struct radeon_device *rdev)
@@ -232,7 +232,7 @@ static void r420_cp_errata_fini(struct radeon_device *rdev)
 	radeon_ring_lock(rdev, ring, 8);
 	radeon_ring_write(ring, PACKET0(R300_RB3D_DSTCACHE_CTLSTAT, 0));
 	radeon_ring_write(ring, R300_RB3D_DC_FINISH);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 	radeon_scratch_free(rdev, rdev->config.r300.resync_scratch);
 }
 
diff --git a/drivers/gpu/drm/radeon/r600.c b/drivers/gpu/drm/radeon/r600.c
index c70a504d96af..56b02927cd3d 100644
--- a/drivers/gpu/drm/radeon/r600.c
+++ b/drivers/gpu/drm/radeon/r600.c
@@ -122,6 +122,94 @@ u32 r600_get_xclk(struct radeon_device *rdev)
 
 int r600_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk)
 {
+	unsigned fb_div = 0, ref_div, vclk_div = 0, dclk_div = 0;
+	int r;
+
+	/* bypass vclk and dclk with bclk */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		 VCLK_SRC_SEL(1) | DCLK_SRC_SEL(1),
+		 ~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	/* assert BYPASS_EN, deassert UPLL_RESET, UPLL_SLEEP and UPLL_CTLREQ */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_BYPASS_EN_MASK, ~(
+		 UPLL_RESET_MASK | UPLL_SLEEP_MASK | UPLL_CTLREQ_MASK));
+
+	if (rdev->family >= CHIP_RS780)
+		WREG32_P(GFX_MACRO_BYPASS_CNTL, UPLL_BYPASS_CNTL,
+			 ~UPLL_BYPASS_CNTL);
+
+	if (!vclk || !dclk) {
+		/* keep the Bypass mode, put PLL to sleep */
+		WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_SLEEP_MASK, ~UPLL_SLEEP_MASK);
+		return 0;
+	}
+
+	if (rdev->clock.spll.reference_freq == 10000)
+		ref_div = 34;
+	else
+		ref_div = 4;
+
+	r = radeon_uvd_calc_upll_dividers(rdev, vclk, dclk, 50000, 160000,
+					  ref_div + 1, 0xFFF, 2, 30, ~0,
+					  &fb_div, &vclk_div, &dclk_div);
+	if (r)
+		return r;
+
+	if (rdev->family >= CHIP_RV670 && rdev->family < CHIP_RS780)
+		fb_div >>= 1;
+	else
+		fb_div |= 1;
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+        if (r)
+                return r;
+
+	/* assert PLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_RESET_MASK, ~UPLL_RESET_MASK);
+
+	/* For RS780 we have to choose ref clk */
+	if (rdev->family >= CHIP_RS780)
+		WREG32_P(CG_UPLL_FUNC_CNTL, UPLL_REFCLK_SRC_SEL_MASK,
+			 ~UPLL_REFCLK_SRC_SEL_MASK);
+
+	/* set the required fb, ref and post divder values */
+	WREG32_P(CG_UPLL_FUNC_CNTL,
+		 UPLL_FB_DIV(fb_div) |
+		 UPLL_REF_DIV(ref_div),
+		 ~(UPLL_FB_DIV_MASK | UPLL_REF_DIV_MASK));
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		 UPLL_SW_HILEN(vclk_div >> 1) |
+		 UPLL_SW_LOLEN((vclk_div >> 1) + (vclk_div & 1)) |
+		 UPLL_SW_HILEN2(dclk_div >> 1) |
+		 UPLL_SW_LOLEN2((dclk_div >> 1) + (dclk_div & 1)) |
+		 UPLL_DIVEN_MASK | UPLL_DIVEN2_MASK,
+		 ~UPLL_SW_MASK);
+
+	/* give the PLL some time to settle */
+	mdelay(15);
+
+	/* deassert PLL_RESET */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_RESET_MASK);
+
+	mdelay(15);
+
+	/* deassert BYPASS EN */
+	WREG32_P(CG_UPLL_FUNC_CNTL, 0, ~UPLL_BYPASS_EN_MASK);
+
+	if (rdev->family >= CHIP_RS780)
+		WREG32_P(GFX_MACRO_BYPASS_CNTL, 0, ~UPLL_BYPASS_CNTL);
+
+	r = radeon_uvd_send_upll_ctlreq(rdev, CG_UPLL_FUNC_CNTL);
+	if (r)
+		return r;
+
+	/* switch VCLK and DCLK selection */
+	WREG32_P(CG_UPLL_FUNC_CNTL_2,
+		 VCLK_SRC_SEL(2) | DCLK_SRC_SEL(2),
+		 ~(VCLK_SRC_SEL_MASK | DCLK_SRC_SEL_MASK));
+
+	mdelay(100);
+
 	return 0;
 }
 
@@ -992,6 +1080,8 @@ static int r600_pcie_gart_enable(struct radeon_device *rdev)
 	WREG32(MC_VM_L1_TLB_MCB_WR_GFX_CNTL, tmp);
 	WREG32(MC_VM_L1_TLB_MCB_RD_PDMA_CNTL, tmp);
 	WREG32(MC_VM_L1_TLB_MCB_WR_PDMA_CNTL, tmp);
+	WREG32(MC_VM_L1_TLB_MCB_RD_UVD_CNTL, tmp);
+	WREG32(MC_VM_L1_TLB_MCB_WR_UVD_CNTL, tmp);
 	WREG32(MC_VM_L1_TLB_MCB_RD_SEM_CNTL, tmp | ENABLE_SEMAPHORE_MODE);
 	WREG32(MC_VM_L1_TLB_MCB_WR_SEM_CNTL, tmp | ENABLE_SEMAPHORE_MODE);
 	WREG32(VM_CONTEXT0_PAGE_TABLE_START_ADDR, rdev->mc.gtt_start >> 12);
@@ -1042,6 +1132,8 @@ static void r600_pcie_gart_disable(struct radeon_device *rdev)
 	WREG32(MC_VM_L1_TLB_MCB_WR_SYS_CNTL, tmp);
 	WREG32(MC_VM_L1_TLB_MCB_RD_HDP_CNTL, tmp);
 	WREG32(MC_VM_L1_TLB_MCB_WR_HDP_CNTL, tmp);
+	WREG32(MC_VM_L1_TLB_MCB_RD_UVD_CNTL, tmp);
+	WREG32(MC_VM_L1_TLB_MCB_WR_UVD_CNTL, tmp);
 	radeon_gart_table_vram_unpin(rdev);
 }
 
@@ -1338,7 +1430,7 @@ int r600_vram_scratch_init(struct radeon_device *rdev)
 	if (rdev->vram_scratch.robj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				     0, NULL, &rdev->vram_scratch.robj);
+				     0, NULL, NULL, &rdev->vram_scratch.robj);
 		if (r) {
 			return r;
 		}
@@ -1812,7 +1904,6 @@ static void r600_gpu_init(struct radeon_device *rdev)
 {
 	u32 tiling_config;
 	u32 ramcfg;
-	u32 cc_rb_backend_disable;
 	u32 cc_gc_shader_pipe_config;
 	u32 tmp;
 	int i, j;
@@ -1939,29 +2030,20 @@ static void r600_gpu_init(struct radeon_device *rdev)
 	}
 	tiling_config |= BANK_SWAPS(1);
 
-	cc_rb_backend_disable = RREG32(CC_RB_BACKEND_DISABLE) & 0x00ff0000;
-	tmp = R6XX_MAX_BACKENDS -
-		r600_count_pipe_bits((cc_rb_backend_disable >> 16) & R6XX_MAX_BACKENDS_MASK);
-	if (tmp < rdev->config.r600.max_backends) {
-		rdev->config.r600.max_backends = tmp;
-	}
-
 	cc_gc_shader_pipe_config = RREG32(CC_GC_SHADER_PIPE_CONFIG) & 0x00ffff00;
-	tmp = R6XX_MAX_PIPES -
-		r600_count_pipe_bits((cc_gc_shader_pipe_config >> 8) & R6XX_MAX_PIPES_MASK);
-	if (tmp < rdev->config.r600.max_pipes) {
-		rdev->config.r600.max_pipes = tmp;
-	}
-	tmp = R6XX_MAX_SIMDS -
-		r600_count_pipe_bits((cc_gc_shader_pipe_config >> 16) & R6XX_MAX_SIMDS_MASK);
-	if (tmp < rdev->config.r600.max_simds) {
-		rdev->config.r600.max_simds = tmp;
-	}
 	tmp = rdev->config.r600.max_simds -
 		r600_count_pipe_bits((cc_gc_shader_pipe_config >> 16) & R6XX_MAX_SIMDS_MASK);
 	rdev->config.r600.active_simds = tmp;
 
 	disabled_rb_mask = (RREG32(CC_RB_BACKEND_DISABLE) >> 16) & R6XX_MAX_BACKENDS_MASK;
+	tmp = 0;
+	for (i = 0; i < rdev->config.r600.max_backends; i++)
+		tmp |= (1 << i);
+	/* if all the backends are disabled, fix it up here */
+	if ((disabled_rb_mask & tmp) == tmp) {
+		for (i = 0; i < rdev->config.r600.max_backends; i++)
+			disabled_rb_mask &= ~(1 << i);
+	}
 	tmp = (tiling_config & PIPE_TILING__MASK) >> PIPE_TILING__SHIFT;
 	tmp = r6xx_remap_render_backend(rdev, tmp, rdev->config.r600.max_backends,
 					R6XX_MAX_BACKENDS, disabled_rb_mask);
@@ -2547,7 +2629,7 @@ int r600_cp_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, PACKET3_ME_INITIALIZE_DEVICE_ID(1));
 	radeon_ring_write(ring, 0);
 	radeon_ring_write(ring, 0);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	cp_me = 0xff;
 	WREG32(R_0086D8_CP_ME_CNTL, cp_me);
@@ -2683,7 +2765,7 @@ int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
 	radeon_ring_write(ring, ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2));
 	radeon_ring_write(ring, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 	for (i = 0; i < rdev->usec_timeout; i++) {
 		tmp = RREG32(scratch);
 		if (tmp == 0xDEADBEEF)
@@ -2753,6 +2835,17 @@ void r600_fence_ring_emit(struct radeon_device *rdev,
 	}
 }
 
+/**
+ * r600_semaphore_ring_emit - emit a semaphore on the CP ring
+ *
+ * @rdev: radeon_device pointer
+ * @ring: radeon ring buffer object
+ * @semaphore: radeon semaphore object
+ * @emit_wait: Is this a sempahore wait?
+ *
+ * Emits a semaphore signal/wait packet to the CP ring and prevents the PFP
+ * from running ahead of semaphore waits.
+ */
 bool r600_semaphore_ring_emit(struct radeon_device *rdev,
 			      struct radeon_ring *ring,
 			      struct radeon_semaphore *semaphore,
@@ -2768,6 +2861,13 @@ bool r600_semaphore_ring_emit(struct radeon_device *rdev,
 	radeon_ring_write(ring, lower_32_bits(addr));
 	radeon_ring_write(ring, (upper_32_bits(addr) & 0xff) | sel);
 
+	/* PFP_SYNC_ME packet only exists on 7xx+, only enable it on eg+ */
+	if (emit_wait && (rdev->family >= CHIP_CEDAR)) {
+		/* Prevent the PFP from running ahead of the semaphore wait */
+		radeon_ring_write(ring, PACKET3(PACKET3_PFP_SYNC_ME, 0));
+		radeon_ring_write(ring, 0x0);
+	}
+
 	return true;
 }
 
@@ -2784,12 +2884,13 @@ bool r600_semaphore_ring_emit(struct radeon_device *rdev,
  * Used by the radeon ttm implementation to move pages if
  * registered as the asic copy callback.
  */
-int r600_copy_cpdma(struct radeon_device *rdev,
-		    uint64_t src_offset, uint64_t dst_offset,
-		    unsigned num_gpu_pages,
-		    struct radeon_fence **fence)
+struct radeon_fence *r600_copy_cpdma(struct radeon_device *rdev,
+				     uint64_t src_offset, uint64_t dst_offset,
+				     unsigned num_gpu_pages,
+				     struct reservation_object *resv)
 {
 	struct radeon_semaphore *sem = NULL;
+	struct radeon_fence *fence;
 	int ring_index = rdev->asic->copy.blit_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes, tmp;
@@ -2799,7 +2900,7 @@ int r600_copy_cpdma(struct radeon_device *rdev,
 	r = radeon_semaphore_create(rdev, &sem);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
@@ -2808,10 +2909,10 @@ int r600_copy_cpdma(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_to(sem, *fence);
+	radeon_semaphore_sync_resv(rdev, sem, resv, false);
 	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
 
 	radeon_ring_write(ring, PACKET3(PACKET3_SET_CONFIG_REG, 1));
@@ -2838,17 +2939,17 @@ int r600_copy_cpdma(struct radeon_device *rdev,
 	radeon_ring_write(ring, (WAIT_UNTIL - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
 	radeon_ring_write(ring, WAIT_CP_DMA_IDLE_bit);
 
-	r = radeon_fence_emit(rdev, fence, ring->idx);
+	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
-	radeon_semaphore_free(rdev, &sem, *fence);
+	radeon_ring_unlock_commit(rdev, ring, false);
+	radeon_semaphore_free(rdev, &sem, fence);
 
-	return r;
+	return fence;
 }
 
 int r600_set_surface_reg(struct radeon_device *rdev, int reg,
@@ -2899,6 +3000,18 @@ static int r600_startup(struct radeon_device *rdev)
 		return r;
 	}
 
+	if (rdev->has_uvd) {
+		r = uvd_v1_0_resume(rdev);
+		if (!r) {
+			r = radeon_fence_driver_start_ring(rdev, R600_RING_TYPE_UVD_INDEX);
+			if (r) {
+				dev_err(rdev->dev, "failed initializing UVD fences (%d).\n", r);
+			}
+		}
+		if (r)
+			rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_size = 0;
+	}
+
 	/* Enable IRQ */
 	if (!rdev->irq.installed) {
 		r = radeon_irq_kms_init(rdev);
@@ -2927,6 +3040,18 @@ static int r600_startup(struct radeon_device *rdev)
 	if (r)
 		return r;
 
+	if (rdev->has_uvd) {
+		ring = &rdev->ring[R600_RING_TYPE_UVD_INDEX];
+		if (ring->ring_size) {
+			r = radeon_ring_init(rdev, ring, ring->ring_size, 0,
+					     RADEON_CP_PACKET2);
+			if (!r)
+				r = uvd_v1_0_init(rdev);
+			if (r)
+				DRM_ERROR("radeon: failed initializing UVD (%d).\n", r);
+		}
+	}
+
 	r = radeon_ib_pool_init(rdev);
 	if (r) {
 		dev_err(rdev->dev, "IB initialization failed (%d).\n", r);
@@ -2986,6 +3111,10 @@ int r600_suspend(struct radeon_device *rdev)
 	radeon_pm_suspend(rdev);
 	r600_audio_fini(rdev);
 	r600_cp_stop(rdev);
+	if (rdev->has_uvd) {
+		uvd_v1_0_fini(rdev);
+		radeon_uvd_suspend(rdev);
+	}
 	r600_irq_suspend(rdev);
 	radeon_wb_disable(rdev);
 	r600_pcie_gart_disable(rdev);
@@ -3065,6 +3194,14 @@ int r600_init(struct radeon_device *rdev)
 	rdev->ring[RADEON_RING_TYPE_GFX_INDEX].ring_obj = NULL;
 	r600_ring_init(rdev, &rdev->ring[RADEON_RING_TYPE_GFX_INDEX], 1024 * 1024);
 
+	if (rdev->has_uvd) {
+		r = radeon_uvd_init(rdev);
+		if (!r) {
+			rdev->ring[R600_RING_TYPE_UVD_INDEX].ring_obj = NULL;
+			r600_ring_init(rdev, &rdev->ring[R600_RING_TYPE_UVD_INDEX], 4096);
+		}
+	}
+
 	rdev->ih.ring_obj = NULL;
 	r600_ih_ring_init(rdev, 64 * 1024);
 
@@ -3094,6 +3231,10 @@ void r600_fini(struct radeon_device *rdev)
 	r600_audio_fini(rdev);
 	r600_cp_fini(rdev);
 	r600_irq_fini(rdev);
+	if (rdev->has_uvd) {
+		uvd_v1_0_fini(rdev);
+		radeon_uvd_fini(rdev);
+	}
 	radeon_wb_fini(rdev);
 	radeon_ib_pool_fini(rdev);
 	radeon_irq_kms_fini(rdev);
@@ -3165,7 +3306,7 @@ int r600_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	ib.ptr[1] = ((scratch - PACKET3_SET_CONFIG_REG_OFFSET) >> 2);
 	ib.ptr[2] = 0xDEADBEEF;
 	ib.length_dw = 3;
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 		DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
 		goto free_ib;
@@ -3227,7 +3368,7 @@ int r600_ih_ring_alloc(struct radeon_device *rdev)
 		r = radeon_bo_create(rdev, rdev->ih.ring_size,
 				     PAGE_SIZE, true,
 				     RADEON_GEM_DOMAIN_GTT, 0,
-				     NULL, &rdev->ih.ring_obj);
+				     NULL, NULL, &rdev->ih.ring_obj);
 		if (r) {
 			DRM_ERROR("radeon: failed to create ih ring buffer (%d).\n", r);
 			return r;
@@ -3784,17 +3925,17 @@ static u32 r600_get_ih_wptr(struct radeon_device *rdev)
 		wptr = RREG32(IH_RB_WPTR);
 
 	if (wptr & RB_OVERFLOW) {
+		wptr &= ~RB_OVERFLOW;
 		/* When a ring buffer overflow happen start parsing interrupt
 		 * from the last not overwritten vector (wptr + 16). Hopefully
 		 * this should allow us to catchup.
 		 */
-		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, %d, %d)\n",
-			wptr, rdev->ih.rptr, (wptr + 16) + rdev->ih.ptr_mask);
+		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, 0x%08X, 0x%08X)\n",
+			 wptr, rdev->ih.rptr, (wptr + 16) & rdev->ih.ptr_mask);
 		rdev->ih.rptr = (wptr + 16) & rdev->ih.ptr_mask;
 		tmp = RREG32(IH_RB_CNTL);
 		tmp |= IH_WPTR_OVERFLOW_CLEAR;
 		WREG32(IH_RB_CNTL, tmp);
-		wptr &= ~RB_OVERFLOW;
 	}
 	return (wptr & rdev->ih.ptr_mask);
 }
@@ -4040,6 +4181,7 @@ restart_ih:
 		/* wptr/rptr are in bytes! */
 		rptr += 16;
 		rptr &= rdev->ih.ptr_mask;
+		WREG32(IH_RB_RPTR, rptr);
 	}
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
@@ -4048,7 +4190,6 @@ restart_ih:
 	if (queue_thermal && rdev->pm.dpm_enabled)
 		schedule_work(&rdev->pm.dpm.thermal.work);
 	rdev->ih.rptr = rptr;
-	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	atomic_set(&rdev->ih.lock, 0);
 
 	/* make sure wptr hasn't changed while processing */
diff --git a/drivers/gpu/drm/radeon/r600_audio.c b/drivers/gpu/drm/radeon/r600_audio.c
deleted file mode 100644
index bffac10c4296..000000000000
--- a/drivers/gpu/drm/radeon/r600_audio.c
+++ /dev/null
@@ -1,207 +0,0 @@
-/*
- * Copyright 2008 Advanced Micro Devices, Inc.
- * Copyright 2008 Red Hat Inc.
- * Copyright 2009 Christian König.
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
- * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- *
- * Authors: Christian König
- */
-#include <drm/drmP.h>
-#include "radeon.h"
-#include "radeon_reg.h"
-#include "radeon_asic.h"
-#include "atom.h"
-
-/*
- * check if enc_priv stores radeon_encoder_atom_dig
- */
-static bool radeon_dig_encoder(struct drm_encoder *encoder)
-{
-	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
-	switch (radeon_encoder->encoder_id) {
-	case ENCODER_OBJECT_ID_INTERNAL_LVDS:
-	case ENCODER_OBJECT_ID_INTERNAL_TMDS1:
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
-	case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
-	case ENCODER_OBJECT_ID_INTERNAL_DVO1:
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
-	case ENCODER_OBJECT_ID_INTERNAL_DDI:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY:
-	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_LVTMA:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY1:
-	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY2:
-		return true;
-	}
-	return false;
-}
-
-/*
- * check if the chipset is supported
- */
-static int r600_audio_chipset_supported(struct radeon_device *rdev)
-{
-	return ASIC_IS_DCE2(rdev) && !ASIC_IS_NODCE(rdev);
-}
-
-struct r600_audio_pin r600_audio_status(struct radeon_device *rdev)
-{
-	struct r600_audio_pin status;
-	uint32_t value;
-
-	value = RREG32(R600_AUDIO_RATE_BPS_CHANNEL);
-
-	/* number of channels */
-	status.channels = (value & 0x7) + 1;
-
-	/* bits per sample */
-	switch ((value & 0xF0) >> 4) {
-	case 0x0:
-		status.bits_per_sample = 8;
-		break;
-	case 0x1:
-		status.bits_per_sample = 16;
-		break;
-	case 0x2:
-		status.bits_per_sample = 20;
-		break;
-	case 0x3:
-		status.bits_per_sample = 24;
-		break;
-	case 0x4:
-		status.bits_per_sample = 32;
-		break;
-	default:
-		dev_err(rdev->dev, "Unknown bits per sample 0x%x, using 16\n",
-			(int)value);
-		status.bits_per_sample = 16;
-	}
-
-	/* current sampling rate in HZ */
-	if (value & 0x4000)
-		status.rate = 44100;
-	else
-		status.rate = 48000;
-	status.rate *= ((value >> 11) & 0x7) + 1;
-	status.rate /= ((value >> 8) & 0x7) + 1;
-
-	value = RREG32(R600_AUDIO_STATUS_BITS);
-
-	/* iec 60958 status bits */
-	status.status_bits = value & 0xff;
-
-	/* iec 60958 category code */
-	status.category_code = (value >> 8) & 0xff;
-
-	return status;
-}
-
-/*
- * update all hdmi interfaces with current audio parameters
- */
-void r600_audio_update_hdmi(struct work_struct *work)
-{
-	struct radeon_device *rdev = container_of(work, struct radeon_device,
-						  audio_work);
-	struct drm_device *dev = rdev->ddev;
-	struct r600_audio_pin audio_status = r600_audio_status(rdev);
-	struct drm_encoder *encoder;
-	bool changed = false;
-
-	if (rdev->audio.pin[0].channels != audio_status.channels ||
-	    rdev->audio.pin[0].rate != audio_status.rate ||
-	    rdev->audio.pin[0].bits_per_sample != audio_status.bits_per_sample ||
-	    rdev->audio.pin[0].status_bits != audio_status.status_bits ||
-	    rdev->audio.pin[0].category_code != audio_status.category_code) {
-		rdev->audio.pin[0] = audio_status;
-		changed = true;
-	}
-
-	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
-		if (!radeon_dig_encoder(encoder))
-			continue;
-		if (changed || r600_hdmi_buffer_status_changed(encoder))
-			r600_hdmi_update_audio_settings(encoder);
-	}
-}
-
-/* enable the audio stream */
-void r600_audio_enable(struct radeon_device *rdev,
-		       struct r600_audio_pin *pin,
-		       bool enable)
-{
-	u32 value = 0;
-
-	if (!pin)
-		return;
-
-	if (ASIC_IS_DCE4(rdev)) {
-		if (enable) {
-			value |= 0x81000000; /* Required to enable audio */
-			value |= 0x0e1000f0; /* fglrx sets that too */
-		}
-		WREG32(EVERGREEN_AUDIO_ENABLE, value);
-	} else {
-		WREG32_P(R600_AUDIO_ENABLE,
-			 enable ? 0x81000000 : 0x0, ~0x81000000);
-	}
-}
-
-/*
- * initialize the audio vars
- */
-int r600_audio_init(struct radeon_device *rdev)
-{
-	if (!radeon_audio || !r600_audio_chipset_supported(rdev))
-		return 0;
-
-	rdev->audio.enabled = true;
-
-	rdev->audio.num_pins = 1;
-	rdev->audio.pin[0].channels = -1;
-	rdev->audio.pin[0].rate = -1;
-	rdev->audio.pin[0].bits_per_sample = -1;
-	rdev->audio.pin[0].status_bits = 0;
-	rdev->audio.pin[0].category_code = 0;
-	rdev->audio.pin[0].id = 0;
-	/* disable audio.  it will be set up later */
-	r600_audio_enable(rdev, &rdev->audio.pin[0], false);
-
-	return 0;
-}
-
-/*
- * release the audio timer
- * TODO: How to do this correctly on SMP systems?
- */
-void r600_audio_fini(struct radeon_device *rdev)
-{
-	if (!rdev->audio.enabled)
-		return;
-
-	r600_audio_enable(rdev, &rdev->audio.pin[0], false);
-
-	rdev->audio.enabled = false;
-}
-
-struct r600_audio_pin *r600_audio_get_pin(struct radeon_device *rdev)
-{
-	/* only one pin on 6xx-NI */
-	return &rdev->audio.pin[0];
-}
diff --git a/drivers/gpu/drm/radeon/r600_cp.c b/drivers/gpu/drm/radeon/r600_cp.c
index 8c9b7e26533c..09e3f39925fa 100644
--- a/drivers/gpu/drm/radeon/r600_cp.c
+++ b/drivers/gpu/drm/radeon/r600_cp.c
@@ -1949,15 +1949,15 @@ int r600_do_cleanup_cp(struct drm_device *dev)
 #if __OS_HAS_AGP
 	if (dev_priv->flags & RADEON_IS_AGP) {
 		if (dev_priv->cp_ring != NULL) {
-			drm_core_ioremapfree(dev_priv->cp_ring, dev);
+			drm_legacy_ioremapfree(dev_priv->cp_ring, dev);
 			dev_priv->cp_ring = NULL;
 		}
 		if (dev_priv->ring_rptr != NULL) {
-			drm_core_ioremapfree(dev_priv->ring_rptr, dev);
+			drm_legacy_ioremapfree(dev_priv->ring_rptr, dev);
 			dev_priv->ring_rptr = NULL;
 		}
 		if (dev->agp_buffer_map != NULL) {
-			drm_core_ioremapfree(dev->agp_buffer_map, dev);
+			drm_legacy_ioremapfree(dev->agp_buffer_map, dev);
 			dev->agp_buffer_map = NULL;
 		}
 	} else
@@ -1968,7 +1968,7 @@ int r600_do_cleanup_cp(struct drm_device *dev)
 			r600_page_table_cleanup(dev, &dev_priv->gart_info);
 
 		if (dev_priv->gart_info.gart_table_location == DRM_ATI_GART_FB) {
-			drm_core_ioremapfree(&dev_priv->gart_info.mapping, dev);
+			drm_legacy_ioremapfree(&dev_priv->gart_info.mapping, dev);
 			dev_priv->gart_info.addr = NULL;
 		}
 	}
@@ -2052,27 +2052,27 @@ int r600_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 	dev_priv->buffers_offset = init->buffers_offset;
 	dev_priv->gart_textures_offset = init->gart_textures_offset;
 
-	master_priv->sarea = drm_getsarea(dev);
+	master_priv->sarea = drm_legacy_getsarea(dev);
 	if (!master_priv->sarea) {
 		DRM_ERROR("could not find sarea!\n");
 		r600_do_cleanup_cp(dev);
 		return -EINVAL;
 	}
 
-	dev_priv->cp_ring = drm_core_findmap(dev, init->ring_offset);
+	dev_priv->cp_ring = drm_legacy_findmap(dev, init->ring_offset);
 	if (!dev_priv->cp_ring) {
 		DRM_ERROR("could not find cp ring region!\n");
 		r600_do_cleanup_cp(dev);
 		return -EINVAL;
 	}
-	dev_priv->ring_rptr = drm_core_findmap(dev, init->ring_rptr_offset);
+	dev_priv->ring_rptr = drm_legacy_findmap(dev, init->ring_rptr_offset);
 	if (!dev_priv->ring_rptr) {
 		DRM_ERROR("could not find ring read pointer!\n");
 		r600_do_cleanup_cp(dev);
 		return -EINVAL;
 	}
 	dev->agp_buffer_token = init->buffers_offset;
-	dev->agp_buffer_map = drm_core_findmap(dev, init->buffers_offset);
+	dev->agp_buffer_map = drm_legacy_findmap(dev, init->buffers_offset);
 	if (!dev->agp_buffer_map) {
 		DRM_ERROR("could not find dma buffer region!\n");
 		r600_do_cleanup_cp(dev);
@@ -2081,7 +2081,7 @@ int r600_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 
 	if (init->gart_textures_offset) {
 		dev_priv->gart_textures =
-		    drm_core_findmap(dev, init->gart_textures_offset);
+		    drm_legacy_findmap(dev, init->gart_textures_offset);
 		if (!dev_priv->gart_textures) {
 			DRM_ERROR("could not find GART texture region!\n");
 			r600_do_cleanup_cp(dev);
@@ -2092,9 +2092,9 @@ int r600_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 #if __OS_HAS_AGP
 	/* XXX */
 	if (dev_priv->flags & RADEON_IS_AGP) {
-		drm_core_ioremap_wc(dev_priv->cp_ring, dev);
-		drm_core_ioremap_wc(dev_priv->ring_rptr, dev);
-		drm_core_ioremap_wc(dev->agp_buffer_map, dev);
+		drm_legacy_ioremap_wc(dev_priv->cp_ring, dev);
+		drm_legacy_ioremap_wc(dev_priv->ring_rptr, dev);
+		drm_legacy_ioremap_wc(dev->agp_buffer_map, dev);
 		if (!dev_priv->cp_ring->handle ||
 		    !dev_priv->ring_rptr->handle ||
 		    !dev->agp_buffer_map->handle) {
@@ -2235,7 +2235,7 @@ int r600_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 		dev_priv->gart_info.mapping.size =
 			dev_priv->gart_info.table_size;
 
-		drm_core_ioremap_wc(&dev_priv->gart_info.mapping, dev);
+		drm_legacy_ioremap_wc(&dev_priv->gart_info.mapping, dev);
 		if (!dev_priv->gart_info.mapping.handle) {
 			DRM_ERROR("ioremap failed.\n");
 			r600_do_cleanup_cp(dev);
diff --git a/drivers/gpu/drm/radeon/r600_dma.c b/drivers/gpu/drm/radeon/r600_dma.c
index 4969cef44a19..aabc343b9a8f 100644
--- a/drivers/gpu/drm/radeon/r600_dma.c
+++ b/drivers/gpu/drm/radeon/r600_dma.c
@@ -124,15 +124,6 @@ int r600_dma_resume(struct radeon_device *rdev)
 	u32 rb_bufsz;
 	int r;
 
-	/* Reset dma */
-	if (rdev->family >= CHIP_RV770)
-		WREG32(SRBM_SOFT_RESET, RV770_SOFT_RESET_DMA);
-	else
-		WREG32(SRBM_SOFT_RESET, SOFT_RESET_DMA);
-	RREG32(SRBM_SOFT_RESET);
-	udelay(50);
-	WREG32(SRBM_SOFT_RESET, 0);
-
 	WREG32(DMA_SEM_INCOMPLETE_TIMER_CNTL, 0);
 	WREG32(DMA_SEM_WAIT_FAIL_TIMER_CNTL, 0);
 
@@ -241,16 +232,19 @@ int r600_dma_ring_test(struct radeon_device *rdev,
 {
 	unsigned i;
 	int r;
-	void __iomem *ptr = (void *)rdev->vram_scratch.ptr;
+	unsigned index;
 	u32 tmp;
+	u64 gpu_addr;
 
-	if (!ptr) {
-		DRM_ERROR("invalid vram scratch pointer\n");
-		return -EINVAL;
-	}
+	if (ring->idx == R600_RING_TYPE_DMA_INDEX)
+		index = R600_WB_DMA_RING_TEST_OFFSET;
+	else
+		index = CAYMAN_WB_DMA1_RING_TEST_OFFSET;
+
+	gpu_addr = rdev->wb.gpu_addr + index;
 
 	tmp = 0xCAFEDEAD;
-	writel(tmp, ptr);
+	rdev->wb.wb[index/4] = cpu_to_le32(tmp);
 
 	r = radeon_ring_lock(rdev, ring, 4);
 	if (r) {
@@ -258,13 +252,13 @@ int r600_dma_ring_test(struct radeon_device *rdev,
 		return r;
 	}
 	radeon_ring_write(ring, DMA_PACKET(DMA_PACKET_WRITE, 0, 0, 1));
-	radeon_ring_write(ring, rdev->vram_scratch.gpu_addr & 0xfffffffc);
-	radeon_ring_write(ring, upper_32_bits(rdev->vram_scratch.gpu_addr) & 0xff);
+	radeon_ring_write(ring, lower_32_bits(gpu_addr));
+	radeon_ring_write(ring, upper_32_bits(gpu_addr) & 0xff);
 	radeon_ring_write(ring, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	for (i = 0; i < rdev->usec_timeout; i++) {
-		tmp = readl(ptr);
+		tmp = le32_to_cpu(rdev->wb.wb[index/4]);
 		if (tmp == 0xDEADBEEF)
 			break;
 		DRM_UDELAY(1);
@@ -368,7 +362,7 @@ int r600_dma_ib_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	ib.ptr[3] = 0xDEADBEEF;
 	ib.length_dw = 4;
 
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 		radeon_ib_free(rdev, &ib);
 		DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
@@ -436,18 +430,19 @@ void r600_dma_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib)
  * @src_offset: src GPU address
  * @dst_offset: dst GPU address
  * @num_gpu_pages: number of GPU pages to xfer
- * @fence: radeon fence object
+ * @resv: reservation object to sync to
  *
  * Copy GPU paging using the DMA engine (r6xx).
  * Used by the radeon ttm implementation to move pages if
  * registered as the asic copy callback.
  */
-int r600_copy_dma(struct radeon_device *rdev,
-		  uint64_t src_offset, uint64_t dst_offset,
-		  unsigned num_gpu_pages,
-		  struct radeon_fence **fence)
+struct radeon_fence *r600_copy_dma(struct radeon_device *rdev,
+				   uint64_t src_offset, uint64_t dst_offset,
+				   unsigned num_gpu_pages,
+				   struct reservation_object *resv)
 {
 	struct radeon_semaphore *sem = NULL;
+	struct radeon_fence *fence;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_dw, cur_size_in_dw;
@@ -457,7 +452,7 @@ int r600_copy_dma(struct radeon_device *rdev,
 	r = radeon_semaphore_create(rdev, &sem);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 
 	size_in_dw = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT) / 4;
@@ -466,10 +461,10 @@ int r600_copy_dma(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_to(sem, *fence);
+	radeon_semaphore_sync_resv(rdev, sem, resv, false);
 	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
@@ -486,15 +481,15 @@ int r600_copy_dma(struct radeon_device *rdev,
 		dst_offset += cur_size_in_dw * 4;
 	}
 
-	r = radeon_fence_emit(rdev, fence, ring->idx);
+	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
-	radeon_semaphore_free(rdev, &sem, *fence);
+	radeon_ring_unlock_commit(rdev, ring, false);
+	radeon_semaphore_free(rdev, &sem, fence);
 
-	return r;
+	return fence;
 }
diff --git a/drivers/gpu/drm/radeon/r600_dpm.c b/drivers/gpu/drm/radeon/r600_dpm.c
index 9c61b74ef441..f6309bd23e01 100644
--- a/drivers/gpu/drm/radeon/r600_dpm.c
+++ b/drivers/gpu/drm/radeon/r600_dpm.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "r600d.h"
 #include "r600_dpm.h"
 #include "atom.h"
diff --git a/drivers/gpu/drm/radeon/r600_hdmi.c b/drivers/gpu/drm/radeon/r600_hdmi.c
index 26ef8ced6f89..b90dc0eb08e6 100644
--- a/drivers/gpu/drm/radeon/r600_hdmi.c
+++ b/drivers/gpu/drm/radeon/r600_hdmi.c
@@ -72,6 +72,169 @@ static const struct radeon_hdmi_acr r600_hdmi_predefined_acr[] = {
 
 
 /*
+ * check if the chipset is supported
+ */
+static int r600_audio_chipset_supported(struct radeon_device *rdev)
+{
+	return ASIC_IS_DCE2(rdev) && !ASIC_IS_NODCE(rdev);
+}
+
+static struct r600_audio_pin r600_audio_status(struct radeon_device *rdev)
+{
+	struct r600_audio_pin status;
+	uint32_t value;
+
+	value = RREG32(R600_AUDIO_RATE_BPS_CHANNEL);
+
+	/* number of channels */
+	status.channels = (value & 0x7) + 1;
+
+	/* bits per sample */
+	switch ((value & 0xF0) >> 4) {
+	case 0x0:
+		status.bits_per_sample = 8;
+		break;
+	case 0x1:
+		status.bits_per_sample = 16;
+		break;
+	case 0x2:
+		status.bits_per_sample = 20;
+		break;
+	case 0x3:
+		status.bits_per_sample = 24;
+		break;
+	case 0x4:
+		status.bits_per_sample = 32;
+		break;
+	default:
+		dev_err(rdev->dev, "Unknown bits per sample 0x%x, using 16\n",
+			(int)value);
+		status.bits_per_sample = 16;
+	}
+
+	/* current sampling rate in HZ */
+	if (value & 0x4000)
+		status.rate = 44100;
+	else
+		status.rate = 48000;
+	status.rate *= ((value >> 11) & 0x7) + 1;
+	status.rate /= ((value >> 8) & 0x7) + 1;
+
+	value = RREG32(R600_AUDIO_STATUS_BITS);
+
+	/* iec 60958 status bits */
+	status.status_bits = value & 0xff;
+
+	/* iec 60958 category code */
+	status.category_code = (value >> 8) & 0xff;
+
+	return status;
+}
+
+/*
+ * update all hdmi interfaces with current audio parameters
+ */
+void r600_audio_update_hdmi(struct work_struct *work)
+{
+	struct radeon_device *rdev = container_of(work, struct radeon_device,
+						  audio_work);
+	struct drm_device *dev = rdev->ddev;
+	struct r600_audio_pin audio_status = r600_audio_status(rdev);
+	struct drm_encoder *encoder;
+	bool changed = false;
+
+	if (rdev->audio.pin[0].channels != audio_status.channels ||
+	    rdev->audio.pin[0].rate != audio_status.rate ||
+	    rdev->audio.pin[0].bits_per_sample != audio_status.bits_per_sample ||
+	    rdev->audio.pin[0].status_bits != audio_status.status_bits ||
+	    rdev->audio.pin[0].category_code != audio_status.category_code) {
+		rdev->audio.pin[0] = audio_status;
+		changed = true;
+	}
+
+	list_for_each_entry(encoder, &dev->mode_config.encoder_list, head) {
+		if (!radeon_encoder_is_digital(encoder))
+			continue;
+		if (changed || r600_hdmi_buffer_status_changed(encoder))
+			r600_hdmi_update_audio_settings(encoder);
+	}
+}
+
+/* enable the audio stream */
+void r600_audio_enable(struct radeon_device *rdev,
+		       struct r600_audio_pin *pin,
+		       u8 enable_mask)
+{
+	u32 tmp = RREG32(AZ_HOT_PLUG_CONTROL);
+
+	if (!pin)
+		return;
+
+	if (enable_mask) {
+		tmp |= AUDIO_ENABLED;
+		if (enable_mask & 1)
+			tmp |= PIN0_AUDIO_ENABLED;
+		if (enable_mask & 2)
+			tmp |= PIN1_AUDIO_ENABLED;
+		if (enable_mask & 4)
+			tmp |= PIN2_AUDIO_ENABLED;
+		if (enable_mask & 8)
+			tmp |= PIN3_AUDIO_ENABLED;
+	} else {
+		tmp &= ~(AUDIO_ENABLED |
+			 PIN0_AUDIO_ENABLED |
+			 PIN1_AUDIO_ENABLED |
+			 PIN2_AUDIO_ENABLED |
+			 PIN3_AUDIO_ENABLED);
+	}
+
+	WREG32(AZ_HOT_PLUG_CONTROL, tmp);
+}
+
+/*
+ * initialize the audio vars
+ */
+int r600_audio_init(struct radeon_device *rdev)
+{
+	if (!radeon_audio || !r600_audio_chipset_supported(rdev))
+		return 0;
+
+	rdev->audio.enabled = true;
+
+	rdev->audio.num_pins = 1;
+	rdev->audio.pin[0].channels = -1;
+	rdev->audio.pin[0].rate = -1;
+	rdev->audio.pin[0].bits_per_sample = -1;
+	rdev->audio.pin[0].status_bits = 0;
+	rdev->audio.pin[0].category_code = 0;
+	rdev->audio.pin[0].id = 0;
+	/* disable audio.  it will be set up later */
+	r600_audio_enable(rdev, &rdev->audio.pin[0], 0);
+
+	return 0;
+}
+
+/*
+ * release the audio timer
+ * TODO: How to do this correctly on SMP systems?
+ */
+void r600_audio_fini(struct radeon_device *rdev)
+{
+	if (!rdev->audio.enabled)
+		return;
+
+	r600_audio_enable(rdev, &rdev->audio.pin[0], 0);
+
+	rdev->audio.enabled = false;
+}
+
+struct r600_audio_pin *r600_audio_get_pin(struct radeon_device *rdev)
+{
+	/* only one pin on 6xx-NI */
+	return &rdev->audio.pin[0];
+}
+
+/*
  * calculate CTS and N values if they are not found in the table
  */
 static void r600_hdmi_calc_cts(uint32_t clock, int *CTS, int *N, int freq)
@@ -357,7 +520,7 @@ void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mod
 
 	/* disable audio prior to setting up hw */
 	dig->afmt->pin = r600_audio_get_pin(rdev);
-	r600_audio_enable(rdev, dig->afmt->pin, false);
+	r600_audio_enable(rdev, dig->afmt->pin, 0xf);
 
 	r600_audio_set_dto(encoder, mode->clock);
 
@@ -443,7 +606,7 @@ void r600_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mod
 	WREG32(HDMI0_RAMP_CONTROL3 + offset, 0x00000001);
 
 	/* enable audio after to setting up hw */
-	r600_audio_enable(rdev, dig->afmt->pin, true);
+	r600_audio_enable(rdev, dig->afmt->pin, 0xf);
 }
 
 /**
@@ -528,6 +691,11 @@ void r600_hdmi_enable(struct drm_encoder *encoder, bool enable)
 	if (!enable && !dig->afmt->enabled)
 		return;
 
+	if (!enable && dig->afmt->pin) {
+		r600_audio_enable(rdev, dig->afmt->pin, 0);
+		dig->afmt->pin = NULL;
+	}
+
 	/* Older chipsets require setting HDMI and routing manually */
 	if (!ASIC_IS_DCE3(rdev)) {
 		if (enable)
diff --git a/drivers/gpu/drm/radeon/r600d.h b/drivers/gpu/drm/radeon/r600d.h
index f94e7a9afe75..1e8495cca41e 100644
--- a/drivers/gpu/drm/radeon/r600d.h
+++ b/drivers/gpu/drm/radeon/r600d.h
@@ -44,13 +44,6 @@
 #define R6XX_MAX_PIPES				8
 #define R6XX_MAX_PIPES_MASK			0xff
 
-/* PTE flags */
-#define PTE_VALID				(1 << 0)
-#define PTE_SYSTEM				(1 << 1)
-#define PTE_SNOOPED				(1 << 2)
-#define PTE_READABLE				(1 << 5)
-#define PTE_WRITEABLE				(1 << 6)
-
 /* tiling bits */
 #define     ARRAY_LINEAR_GENERAL              0x00000000
 #define     ARRAY_LINEAR_ALIGNED              0x00000001
@@ -330,11 +323,12 @@
 #define	HDP_TILING_CONFIG				0x2F3C
 #define HDP_DEBUG1                                      0x2F34
 
+#define MC_CONFIG					0x2000
 #define MC_VM_AGP_TOP					0x2184
 #define MC_VM_AGP_BOT					0x2188
 #define	MC_VM_AGP_BASE					0x218C
 #define MC_VM_FB_LOCATION				0x2180
-#define MC_VM_L1_TLB_MCD_RD_A_CNTL			0x219C
+#define MC_VM_L1_TLB_MCB_RD_UVD_CNTL			0x2124
 #define 	ENABLE_L1_TLB					(1 << 0)
 #define		ENABLE_L1_FRAGMENT_PROCESSING			(1 << 1)
 #define		ENABLE_L1_STRICT_ORDERING			(1 << 2)
@@ -354,12 +348,14 @@
 #define		EFFECTIVE_L1_QUEUE_SIZE(x)			(((x) & 7) << 15)
 #define		EFFECTIVE_L1_QUEUE_SIZE_MASK			0x00038000
 #define		EFFECTIVE_L1_QUEUE_SIZE_SHIFT			15
+#define MC_VM_L1_TLB_MCD_RD_A_CNTL			0x219C
 #define MC_VM_L1_TLB_MCD_RD_B_CNTL			0x21A0
 #define MC_VM_L1_TLB_MCB_RD_GFX_CNTL			0x21FC
 #define MC_VM_L1_TLB_MCB_RD_HDP_CNTL			0x2204
 #define MC_VM_L1_TLB_MCB_RD_PDMA_CNTL			0x2208
 #define MC_VM_L1_TLB_MCB_RD_SEM_CNTL			0x220C
 #define	MC_VM_L1_TLB_MCB_RD_SYS_CNTL			0x2200
+#define MC_VM_L1_TLB_MCB_WR_UVD_CNTL			0x212c
 #define MC_VM_L1_TLB_MCD_WR_A_CNTL			0x21A4
 #define MC_VM_L1_TLB_MCD_WR_B_CNTL			0x21A8
 #define MC_VM_L1_TLB_MCB_WR_GFX_CNTL			0x2210
@@ -373,6 +369,8 @@
 #define MC_VM_SYSTEM_APERTURE_HIGH_ADDR			0x2194
 #define MC_VM_SYSTEM_APERTURE_DEFAULT_ADDR		0x2198
 
+#define RS_DQ_RD_RET_CONF				0x2348
+
 #define	PA_CL_ENHANCE					0x8A14
 #define		CLIP_VTX_REORDER_ENA				(1 << 0)
 #define		NUM_CLIP_SEQ(x)					((x) << 1)
@@ -929,6 +927,23 @@
 #       define TARGET_LINK_SPEED_MASK                     (0xf << 0)
 #       define SELECTABLE_DEEMPHASIS                      (1 << 6)
 
+/* Audio */
+#define AZ_HOT_PLUG_CONTROL               0x7300
+#       define AZ_FORCE_CODEC_WAKE        (1 << 0)
+#       define JACK_DETECTION_ENABLE      (1 << 4)
+#       define UNSOLICITED_RESPONSE_ENABLE (1 << 8)
+#       define CODEC_HOT_PLUG_ENABLE      (1 << 12)
+#       define AUDIO_ENABLED              (1 << 31)
+/* DCE3 adds */
+#       define PIN0_JACK_DETECTION_ENABLE (1 << 4)
+#       define PIN1_JACK_DETECTION_ENABLE (1 << 5)
+#       define PIN2_JACK_DETECTION_ENABLE (1 << 6)
+#       define PIN3_JACK_DETECTION_ENABLE (1 << 7)
+#       define PIN0_AUDIO_ENABLED         (1 << 24)
+#       define PIN1_AUDIO_ENABLED         (1 << 25)
+#       define PIN2_AUDIO_ENABLED         (1 << 26)
+#       define PIN3_AUDIO_ENABLED         (1 << 27)
+
 /* Audio clocks DCE 2.0/3.0 */
 #define AUDIO_DTO                         0x7340
 #       define AUDIO_DTO_PHASE(x)         (((x) & 0xffff) << 0)
@@ -1483,6 +1498,7 @@
 #define UVD_CGC_GATE					0xf4a8
 #define UVD_LMI_CTRL2					0xf4f4
 #define UVD_MASTINT_EN					0xf500
+#define UVD_FW_START					0xf51C
 #define UVD_LMI_ADDR_EXT				0xf594
 #define UVD_LMI_CTRL					0xf598
 #define UVD_LMI_SWAP_CNTL				0xf5b4
@@ -1495,6 +1511,13 @@
 #define UVD_MPC_SET_MUX					0xf5f4
 #define UVD_MPC_SET_ALU					0xf5f8
 
+#define UVD_VCPU_CACHE_OFFSET0				0xf608
+#define UVD_VCPU_CACHE_SIZE0				0xf60c
+#define UVD_VCPU_CACHE_OFFSET1				0xf610
+#define UVD_VCPU_CACHE_SIZE1				0xf614
+#define UVD_VCPU_CACHE_OFFSET2				0xf618
+#define UVD_VCPU_CACHE_SIZE2				0xf61c
+
 #define UVD_VCPU_CNTL					0xf660
 #define UVD_SOFT_RESET					0xf680
 #define		RBC_SOFT_RESET					(1<<0)
@@ -1524,9 +1547,35 @@
 
 #define UVD_CONTEXT_ID					0xf6f4
 
+/* rs780 only */
+#define	GFX_MACRO_BYPASS_CNTL				0x30c0
+#define		SPLL_BYPASS_CNTL			(1 << 0)
+#define		UPLL_BYPASS_CNTL			(1 << 1)
+
+#define CG_UPLL_FUNC_CNTL				0x7e0
+#	define UPLL_RESET_MASK				0x00000001
+#	define UPLL_SLEEP_MASK				0x00000002
+#	define UPLL_BYPASS_EN_MASK			0x00000004
 #	define UPLL_CTLREQ_MASK				0x00000008
+#	define UPLL_FB_DIV(x)				((x) << 4)
+#	define UPLL_FB_DIV_MASK				0x0000FFF0
+#	define UPLL_REF_DIV(x)				((x) << 16)
+#	define UPLL_REF_DIV_MASK			0x003F0000
+#	define UPLL_REFCLK_SRC_SEL_MASK			0x20000000
 #	define UPLL_CTLACK_MASK				0x40000000
 #	define UPLL_CTLACK2_MASK			0x80000000
+#define CG_UPLL_FUNC_CNTL_2				0x7e4
+#	define UPLL_SW_HILEN(x)				((x) << 0)
+#	define UPLL_SW_LOLEN(x)				((x) << 4)
+#	define UPLL_SW_HILEN2(x)			((x) << 8)
+#	define UPLL_SW_LOLEN2(x)			((x) << 12)
+#	define UPLL_DIVEN_MASK				0x00010000
+#	define UPLL_DIVEN2_MASK				0x00020000
+#	define UPLL_SW_MASK				0x0003FFFF
+#	define VCLK_SRC_SEL(x)				((x) << 20)
+#	define VCLK_SRC_SEL_MASK			0x01F00000
+#	define DCLK_SRC_SEL(x)				((x) << 25)
+#	define DCLK_SRC_SEL_MASK			0x3E000000
 
 /*
  * PM4
@@ -1597,6 +1646,7 @@
 		 */
 #              define PACKET3_CP_DMA_CMD_SAIC      (1 << 28)
 #              define PACKET3_CP_DMA_CMD_DAIC      (1 << 29)
+#define	PACKET3_PFP_SYNC_ME				0x42 /* r7xx+ only */
 #define	PACKET3_SURFACE_SYNC				0x43
 #              define PACKET3_CB0_DEST_BASE_ENA    (1 << 6)
 #              define PACKET3_FULL_CACHE_ENA       (1 << 20) /* r7xx+ only */
diff --git a/drivers/gpu/drm/radeon/radeon.h b/drivers/gpu/drm/radeon/radeon.h
index 9e1732eb402c..a9717b3fbf1b 100644
--- a/drivers/gpu/drm/radeon/radeon.h
+++ b/drivers/gpu/drm/radeon/radeon.h
@@ -65,6 +65,8 @@
 #include <linux/list.h>
 #include <linux/kref.h>
 #include <linux/interval_tree.h>
+#include <linux/hashtable.h>
+#include <linux/fence.h>
 
 #include <ttm/ttm_bo_api.h>
 #include <ttm/ttm_bo_driver.h>
@@ -72,6 +74,8 @@
 #include <ttm/ttm_module.h>
 #include <ttm/ttm_execbuf_util.h>
 
+#include <drm/drm_gem.h>
+
 #include "radeon_family.h"
 #include "radeon_mode.h"
 #include "radeon_reg.h"
@@ -105,6 +109,8 @@ extern int radeon_vm_size;
 extern int radeon_vm_block_size;
 extern int radeon_deep_color;
 extern int radeon_use_pflipirq;
+extern int radeon_bapm;
+extern int radeon_backlight;
 
 /*
  * Copy from radeon_drv.h so we don't have to include both and have conflicting
@@ -118,9 +124,6 @@ extern int radeon_use_pflipirq;
 #define RADEONFB_CONN_LIMIT			4
 #define RADEON_BIOS_NUM_SCRATCH			8
 
-/* fence seq are set to this number when signaled */
-#define RADEON_FENCE_SIGNALED_SEQ		0LL
-
 /* internal ring indices */
 /* r1xx+ has gfx CP ring */
 #define RADEON_RING_TYPE_GFX_INDEX		0
@@ -348,28 +351,32 @@ extern void evergreen_tiling_fields(unsigned tiling_flags, unsigned *bankw,
  * Fences.
  */
 struct radeon_fence_driver {
+	struct radeon_device		*rdev;
 	uint32_t			scratch_reg;
 	uint64_t			gpu_addr;
 	volatile uint32_t		*cpu_addr;
 	/* sync_seq is protected by ring emission lock */
 	uint64_t			sync_seq[RADEON_NUM_RINGS];
 	atomic64_t			last_seq;
-	bool				initialized;
+	bool				initialized, delayed_irq;
+	struct delayed_work		lockup_work;
 };
 
 struct radeon_fence {
+	struct fence base;
+
 	struct radeon_device		*rdev;
-	struct kref			kref;
-	/* protected by radeon_fence.lock */
 	uint64_t			seq;
 	/* RB, DMA, etc. */
 	unsigned			ring;
+
+	wait_queue_t			fence_wake;
 };
 
 int radeon_fence_driver_start_ring(struct radeon_device *rdev, int ring);
 int radeon_fence_driver_init(struct radeon_device *rdev);
 void radeon_fence_driver_fini(struct radeon_device *rdev);
-void radeon_fence_driver_force_completion(struct radeon_device *rdev);
+void radeon_fence_driver_force_completion(struct radeon_device *rdev, int ring);
 int radeon_fence_emit(struct radeon_device *rdev, struct radeon_fence **fence, int ring);
 void radeon_fence_process(struct radeon_device *rdev, int ring);
 bool radeon_fence_signaled(struct radeon_fence *fence);
@@ -467,7 +474,7 @@ struct radeon_bo {
 	struct list_head		list;
 	/* Protected by tbo.reserved */
 	u32				initial_domain;
-	u32				placements[3];
+	struct ttm_place		placements[3];
 	struct ttm_placement		placement;
 	struct ttm_buffer_object	tbo;
 	struct ttm_bo_kmap_obj		kmap;
@@ -487,6 +494,9 @@ struct radeon_bo {
 
 	struct ttm_bo_kmap_obj		dma_buf_vmap;
 	pid_t				pid;
+
+	struct radeon_mn		*mn;
+	struct interval_tree_node	mn_it;
 };
 #define gem_to_radeon_bo(gobj) container_of((gobj), struct radeon_bo, gem_base)
 
@@ -578,8 +588,12 @@ bool radeon_semaphore_emit_signal(struct radeon_device *rdev, int ring,
 				  struct radeon_semaphore *semaphore);
 bool radeon_semaphore_emit_wait(struct radeon_device *rdev, int ring,
 				struct radeon_semaphore *semaphore);
-void radeon_semaphore_sync_to(struct radeon_semaphore *semaphore,
-			      struct radeon_fence *fence);
+void radeon_semaphore_sync_fence(struct radeon_semaphore *semaphore,
+				 struct radeon_fence *fence);
+int radeon_semaphore_sync_resv(struct radeon_device *rdev,
+			       struct radeon_semaphore *semaphore,
+			       struct reservation_object *resv,
+			       bool shared);
 int radeon_semaphore_sync_rings(struct radeon_device *rdev,
 				struct radeon_semaphore *semaphore,
 				int waiting_ring);
@@ -700,7 +714,7 @@ struct radeon_flip_work {
 	uint64_t			base;
 	struct drm_pending_vblank_event *event;
 	struct radeon_bo		*old_rbo;
-	struct radeon_fence		*fence;
+	struct fence			*fence;
 };
 
 struct r500_irq_stat_regs {
@@ -778,6 +792,7 @@ struct radeon_irq {
 int radeon_irq_kms_init(struct radeon_device *rdev);
 void radeon_irq_kms_fini(struct radeon_device *rdev);
 void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev, int ring);
+bool radeon_irq_kms_sw_irq_get_delayed(struct radeon_device *rdev, int ring);
 void radeon_irq_kms_sw_irq_put(struct radeon_device *rdev, int ring);
 void radeon_irq_kms_pflip_irq_get(struct radeon_device *rdev, int crtc);
 void radeon_irq_kms_pflip_irq_put(struct radeon_device *rdev, int crtc);
@@ -914,6 +929,8 @@ struct radeon_vm_manager {
 	u64				vram_base_offset;
 	/* is vm enabled? */
 	bool				enabled;
+	/* for hw to save the PD addr on suspend/resume */
+	uint32_t			saved_table_addr[RADEON_NUM_VM];
 };
 
 /*
@@ -967,7 +984,7 @@ int radeon_ib_get(struct radeon_device *rdev, int ring,
 		  unsigned size);
 void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib);
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
-		       struct radeon_ib *const_ib);
+		       struct radeon_ib *const_ib, bool hdp_flush);
 int radeon_ib_pool_init(struct radeon_device *rdev);
 void radeon_ib_pool_fini(struct radeon_device *rdev);
 int radeon_ib_ring_tests(struct radeon_device *rdev);
@@ -977,8 +994,10 @@ bool radeon_ring_supports_scratch_reg(struct radeon_device *rdev,
 void radeon_ring_free_size(struct radeon_device *rdev, struct radeon_ring *cp);
 int radeon_ring_alloc(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
 int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *cp, unsigned ndw);
-void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *cp);
-void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *cp);
+void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *cp,
+			bool hdp_flush);
+void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *cp,
+			       bool hdp_flush);
 void radeon_ring_undo(struct radeon_ring *ring);
 void radeon_ring_unlock_undo(struct radeon_device *rdev, struct radeon_ring *cp);
 int radeon_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
@@ -1114,6 +1133,8 @@ struct radeon_wb {
 #define R600_WB_EVENT_OFFSET     3072
 #define CIK_WB_CP1_WPTR_OFFSET     3328
 #define CIK_WB_CP2_WPTR_OFFSET     3584
+#define R600_WB_DMA_RING_TEST_OFFSET 3588
+#define CAYMAN_WB_DMA1_RING_TEST_OFFSET 3592
 
 /**
  * struct radeon_pm - power management datas
@@ -1636,7 +1657,8 @@ int radeon_uvd_get_create_msg(struct radeon_device *rdev, int ring,
 			      uint32_t handle, struct radeon_fence **fence);
 int radeon_uvd_get_destroy_msg(struct radeon_device *rdev, int ring,
 			       uint32_t handle, struct radeon_fence **fence);
-void radeon_uvd_force_into_uvd_segment(struct radeon_bo *rbo);
+void radeon_uvd_force_into_uvd_segment(struct radeon_bo *rbo,
+				       uint32_t allowed_domains);
 void radeon_uvd_free_handles(struct radeon_device *rdev,
 			     struct drm_file *filp);
 int radeon_uvd_cs_parse(struct radeon_cs_parser *parser);
@@ -1725,6 +1747,11 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 			   struct radeon_ring *cpB);
 void radeon_test_syncing(struct radeon_device *rdev);
 
+/*
+ * MMU Notifier
+ */
+int radeon_mn_register(struct radeon_bo *bo, unsigned long addr);
+void radeon_mn_unregister(struct radeon_bo *bo);
 
 /*
  * Debugfs
@@ -1839,24 +1866,24 @@ struct radeon_asic {
 	} display;
 	/* copy functions for bo handling */
 	struct {
-		int (*blit)(struct radeon_device *rdev,
-			    uint64_t src_offset,
-			    uint64_t dst_offset,
-			    unsigned num_gpu_pages,
-			    struct radeon_fence **fence);
+		struct radeon_fence *(*blit)(struct radeon_device *rdev,
+					     uint64_t src_offset,
+					     uint64_t dst_offset,
+					     unsigned num_gpu_pages,
+					     struct reservation_object *resv);
 		u32 blit_ring_index;
-		int (*dma)(struct radeon_device *rdev,
-			   uint64_t src_offset,
-			   uint64_t dst_offset,
-			   unsigned num_gpu_pages,
-			   struct radeon_fence **fence);
+		struct radeon_fence *(*dma)(struct radeon_device *rdev,
+					    uint64_t src_offset,
+					    uint64_t dst_offset,
+					    unsigned num_gpu_pages,
+					    struct reservation_object *resv);
 		u32 dma_ring_index;
 		/* method used for bo copy */
-		int (*copy)(struct radeon_device *rdev,
-			    uint64_t src_offset,
-			    uint64_t dst_offset,
-			    unsigned num_gpu_pages,
-			    struct radeon_fence **fence);
+		struct radeon_fence *(*copy)(struct radeon_device *rdev,
+					     uint64_t src_offset,
+					     uint64_t dst_offset,
+					     unsigned num_gpu_pages,
+					     struct reservation_object *resv);
 		/* ring used for bo copies */
 		u32 copy_ring_index;
 	} copy;
@@ -2138,6 +2165,8 @@ int radeon_gem_info_ioctl(struct drm_device *dev, void *data,
 			  struct drm_file *filp);
 int radeon_gem_create_ioctl(struct drm_device *dev, void *data,
 			    struct drm_file *filp);
+int radeon_gem_userptr_ioctl(struct drm_device *dev, void *data,
+			     struct drm_file *filp);
 int radeon_gem_pin_ioctl(struct drm_device *dev, void *data,
 			 struct drm_file *file_priv);
 int radeon_gem_unpin_ioctl(struct drm_device *dev, void *data,
@@ -2294,6 +2323,7 @@ struct radeon_device {
 	struct radeon_mman		mman;
 	struct radeon_fence_driver	fence_drv[RADEON_NUM_RINGS];
 	wait_queue_head_t		fence_queue;
+	unsigned			fence_context;
 	struct mutex			ring_lock;
 	struct radeon_ring		ring[RADEON_NUM_RINGS];
 	bool				ib_pool_ready;
@@ -2312,7 +2342,7 @@ struct radeon_device {
 	bool				need_dma32;
 	bool				accel_working;
 	bool				fastfb_working; /* IGP feature*/
-	bool				needs_reset;
+	bool				needs_reset, in_reset;
 	struct radeon_surface_reg surface_regs[RADEON_GEM_MAX_SURFACES];
 	const struct firmware *me_fw;	/* all family ME firmware */
 	const struct firmware *pfp_fw;	/* r6/700 PFP firmware */
@@ -2333,7 +2363,6 @@ struct radeon_device {
 	struct radeon_mec mec;
 	struct work_struct hotplug_work;
 	struct work_struct audio_work;
-	struct work_struct reset_work;
 	int num_crtc; /* number of crtcs */
 	struct mutex dc_hw_i2c_mutex; /* display controller hw i2c mutex */
 	bool has_uvd;
@@ -2370,6 +2399,9 @@ struct radeon_device {
 	/* tracking pinned memory */
 	u64 vram_pin_size;
 	u64 gart_pin_size;
+
+	struct mutex	mn_lock;
+	DECLARE_HASHTABLE(mn_hash, 7);
 };
 
 bool radeon_is_px(struct drm_device *dev);
@@ -2425,7 +2457,17 @@ void cik_mm_wdoorbell(struct radeon_device *rdev, u32 index, u32 v);
 /*
  * Cast helper
  */
-#define to_radeon_fence(p) ((struct radeon_fence *)(p))
+extern const struct fence_ops radeon_fence_ops;
+
+static inline struct radeon_fence *to_radeon_fence(struct fence *f)
+{
+	struct radeon_fence *__f = container_of(f, struct radeon_fence, base);
+
+	if (__f->base.ops == &radeon_fence_ops)
+		return __f;
+
+	return NULL;
+}
 
 /*
  * Registers read & write functions.
@@ -2745,18 +2787,25 @@ void radeon_atombios_fini(struct radeon_device *rdev);
 /*
  * RING helpers.
  */
-#if DRM_DEBUG_CODE == 0
+
+/**
+ * radeon_ring_write - write a value to the ring
+ *
+ * @ring: radeon_ring structure holding ring information
+ * @v: dword (dw) value to write
+ *
+ * Write a value to the requested ring buffer (all asics).
+ */
 static inline void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
 {
+	if (ring->count_dw <= 0)
+		DRM_ERROR("radeon: writing more dwords to the ring than expected!\n");
+
 	ring->ring[ring->wptr++] = v;
 	ring->wptr &= ring->ptr_mask;
 	ring->count_dw--;
 	ring->ring_free_dw--;
 }
-#else
-/* With debugging this is just too big to inline */
-void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
-#endif
 
 /*
  * ASICs macro.
@@ -2795,9 +2844,9 @@ void radeon_ring_write(struct radeon_ring *ring, uint32_t v);
 #define radeon_hdmi_setmode(rdev, e, m) (rdev)->asic->display.hdmi_setmode((e), (m))
 #define radeon_fence_ring_emit(rdev, r, fence) (rdev)->asic->ring[(r)]->emit_fence((rdev), (fence))
 #define radeon_semaphore_ring_emit(rdev, r, cp, semaphore, emit_wait) (rdev)->asic->ring[(r)]->emit_semaphore((rdev), (cp), (semaphore), (emit_wait))
-#define radeon_copy_blit(rdev, s, d, np, f) (rdev)->asic->copy.blit((rdev), (s), (d), (np), (f))
-#define radeon_copy_dma(rdev, s, d, np, f) (rdev)->asic->copy.dma((rdev), (s), (d), (np), (f))
-#define radeon_copy(rdev, s, d, np, f) (rdev)->asic->copy.copy((rdev), (s), (d), (np), (f))
+#define radeon_copy_blit(rdev, s, d, np, resv) (rdev)->asic->copy.blit((rdev), (s), (d), (np), (resv))
+#define radeon_copy_dma(rdev, s, d, np, resv) (rdev)->asic->copy.dma((rdev), (s), (d), (np), (resv))
+#define radeon_copy(rdev, s, d, np, resv) (rdev)->asic->copy.copy((rdev), (s), (d), (np), (resv))
 #define radeon_copy_blit_ring_index(rdev) (rdev)->asic->copy.blit_ring_index
 #define radeon_copy_dma_ring_index(rdev) (rdev)->asic->copy.dma_ring_index
 #define radeon_copy_ring_index(rdev) (rdev)->asic->copy.copy_ring_index
@@ -2871,6 +2920,10 @@ extern void radeon_legacy_set_clock_gating(struct radeon_device *rdev, int enabl
 extern void radeon_atom_set_clock_gating(struct radeon_device *rdev, int enable);
 extern void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain);
 extern bool radeon_ttm_bo_is_radeon_bo(struct ttm_buffer_object *bo);
+extern int radeon_ttm_tt_set_userptr(struct ttm_tt *ttm, uint64_t addr,
+				     uint32_t flags);
+extern bool radeon_ttm_tt_has_userptr(struct ttm_tt *ttm);
+extern bool radeon_ttm_tt_is_readonly(struct ttm_tt *ttm);
 extern void radeon_vram_location(struct radeon_device *rdev, struct radeon_mc *mc, u64 base);
 extern void radeon_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc);
 extern int radeon_resume_kms(struct drm_device *dev, bool resume, bool fbcon);
@@ -2928,10 +2981,10 @@ struct r600_audio_pin *r600_audio_get_pin(struct radeon_device *rdev);
 struct r600_audio_pin *dce6_audio_get_pin(struct radeon_device *rdev);
 void r600_audio_enable(struct radeon_device *rdev,
 		       struct r600_audio_pin *pin,
-		       bool enable);
+		       u8 enable_mask);
 void dce6_audio_enable(struct radeon_device *rdev,
 		       struct r600_audio_pin *pin,
-		       bool enable);
+		       u8 enable_mask);
 
 /*
  * R600 vram scratch functions
diff --git a/drivers/gpu/drm/radeon/radeon_asic.c b/drivers/gpu/drm/radeon/radeon_asic.c
index eeeeabe09758..850de57069be 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.c
+++ b/drivers/gpu/drm/radeon/radeon_asic.c
@@ -185,7 +185,6 @@ static struct radeon_asic_ring r100_gfx_ring = {
 	.get_rptr = &r100_gfx_get_rptr,
 	.get_wptr = &r100_gfx_get_wptr,
 	.set_wptr = &r100_gfx_set_wptr,
-	.hdp_flush = &r100_ring_hdp_flush,
 };
 
 static struct radeon_asic r100_asic = {
@@ -332,7 +331,6 @@ static struct radeon_asic_ring r300_gfx_ring = {
 	.get_rptr = &r100_gfx_get_rptr,
 	.get_wptr = &r100_gfx_get_wptr,
 	.set_wptr = &r100_gfx_set_wptr,
-	.hdp_flush = &r100_ring_hdp_flush,
 };
 
 static struct radeon_asic r300_asic = {
@@ -965,6 +963,19 @@ static struct radeon_asic r600_asic = {
 	},
 };
 
+static struct radeon_asic_ring rv6xx_uvd_ring = {
+	.ib_execute = &uvd_v1_0_ib_execute,
+	.emit_fence = &uvd_v1_0_fence_emit,
+	.emit_semaphore = &uvd_v1_0_semaphore_emit,
+	.cs_parse = &radeon_uvd_cs_parse,
+	.ring_test = &uvd_v1_0_ring_test,
+	.ib_test = &uvd_v1_0_ib_test,
+	.is_lockup = &radeon_ring_test_lockup,
+	.get_rptr = &uvd_v1_0_get_rptr,
+	.get_wptr = &uvd_v1_0_get_wptr,
+	.set_wptr = &uvd_v1_0_set_wptr,
+};
+
 static struct radeon_asic rv6xx_asic = {
 	.init = &r600_init,
 	.fini = &r600_fini,
@@ -984,6 +995,7 @@ static struct radeon_asic rv6xx_asic = {
 	.ring = {
 		[RADEON_RING_TYPE_GFX_INDEX] = &r600_gfx_ring,
 		[R600_RING_TYPE_DMA_INDEX] = &r600_dma_ring,
+		[R600_RING_TYPE_UVD_INDEX] = &rv6xx_uvd_ring,
 	},
 	.irq = {
 		.set = &r600_irq_set,
@@ -1074,6 +1086,7 @@ static struct radeon_asic rs780_asic = {
 	.ring = {
 		[RADEON_RING_TYPE_GFX_INDEX] = &r600_gfx_ring,
 		[R600_RING_TYPE_DMA_INDEX] = &r600_dma_ring,
+		[R600_RING_TYPE_UVD_INDEX] = &rv6xx_uvd_ring,
 	},
 	.irq = {
 		.set = &r600_irq_set,
@@ -2298,7 +2311,15 @@ int radeon_asic_init(struct radeon_device *rdev)
 	case CHIP_RS780:
 	case CHIP_RS880:
 		rdev->asic = &rs780_asic;
-		rdev->has_uvd = true;
+		/* 760G/780V/880V don't have UVD */
+		if ((rdev->pdev->device == 0x9616)||
+		    (rdev->pdev->device == 0x9611)||
+		    (rdev->pdev->device == 0x9613)||
+		    (rdev->pdev->device == 0x9711)||
+		    (rdev->pdev->device == 0x9713))
+			rdev->has_uvd = false;
+		else
+			rdev->has_uvd = true;
 		break;
 	case CHIP_RV770:
 	case CHIP_RV730:
diff --git a/drivers/gpu/drm/radeon/radeon_asic.h b/drivers/gpu/drm/radeon/radeon_asic.h
index 275a5dc01780..d8ace5b28a5b 100644
--- a/drivers/gpu/drm/radeon/radeon_asic.h
+++ b/drivers/gpu/drm/radeon/radeon_asic.h
@@ -81,11 +81,11 @@ bool r100_semaphore_ring_emit(struct radeon_device *rdev,
 int r100_cs_parse(struct radeon_cs_parser *p);
 void r100_pll_wreg(struct radeon_device *rdev, uint32_t reg, uint32_t v);
 uint32_t r100_pll_rreg(struct radeon_device *rdev, uint32_t reg);
-int r100_copy_blit(struct radeon_device *rdev,
-		   uint64_t src_offset,
-		   uint64_t dst_offset,
-		   unsigned num_gpu_pages,
-		   struct radeon_fence **fence);
+struct radeon_fence *r100_copy_blit(struct radeon_device *rdev,
+				    uint64_t src_offset,
+				    uint64_t dst_offset,
+				    unsigned num_gpu_pages,
+				    struct reservation_object *resv);
 int r100_set_surface_reg(struct radeon_device *rdev, int reg,
 			 uint32_t tiling_flags, uint32_t pitch,
 			 uint32_t offset, uint32_t obj_size);
@@ -148,16 +148,15 @@ u32 r100_gfx_get_wptr(struct radeon_device *rdev,
 		      struct radeon_ring *ring);
 void r100_gfx_set_wptr(struct radeon_device *rdev,
 		       struct radeon_ring *ring);
-void r100_ring_hdp_flush(struct radeon_device *rdev,
-			 struct radeon_ring *ring);
+
 /*
  * r200,rv250,rs300,rv280
  */
-extern int r200_copy_dma(struct radeon_device *rdev,
-			 uint64_t src_offset,
-			 uint64_t dst_offset,
-			 unsigned num_gpu_pages,
-			 struct radeon_fence **fence);
+struct radeon_fence *r200_copy_dma(struct radeon_device *rdev,
+				   uint64_t src_offset,
+				   uint64_t dst_offset,
+				   unsigned num_gpu_pages,
+				   struct reservation_object *resv);
 void r200_set_safe_registers(struct radeon_device *rdev);
 
 /*
@@ -341,12 +340,14 @@ int r600_dma_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
 void r600_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
 int r600_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
 int r600_dma_ring_test(struct radeon_device *rdev, struct radeon_ring *cp);
-int r600_copy_cpdma(struct radeon_device *rdev,
-		    uint64_t src_offset, uint64_t dst_offset,
-		    unsigned num_gpu_pages, struct radeon_fence **fence);
-int r600_copy_dma(struct radeon_device *rdev,
-		  uint64_t src_offset, uint64_t dst_offset,
-		  unsigned num_gpu_pages, struct radeon_fence **fence);
+struct radeon_fence *r600_copy_cpdma(struct radeon_device *rdev,
+				     uint64_t src_offset, uint64_t dst_offset,
+				     unsigned num_gpu_pages,
+				     struct reservation_object *resv);
+struct radeon_fence *r600_copy_dma(struct radeon_device *rdev,
+				   uint64_t src_offset, uint64_t dst_offset,
+				   unsigned num_gpu_pages,
+				   struct reservation_object *resv);
 void r600_hpd_init(struct radeon_device *rdev);
 void r600_hpd_fini(struct radeon_device *rdev);
 bool r600_hpd_sense(struct radeon_device *rdev, enum radeon_hpd_id hpd);
@@ -390,7 +391,6 @@ void r600_disable_interrupts(struct radeon_device *rdev);
 void r600_rlc_stop(struct radeon_device *rdev);
 /* r600 audio */
 int r600_audio_init(struct radeon_device *rdev);
-struct r600_audio_pin r600_audio_status(struct radeon_device *rdev);
 void r600_audio_fini(struct radeon_device *rdev);
 void r600_audio_set_dto(struct drm_encoder *encoder, u32 clock);
 void r600_hdmi_update_avi_infoframe(struct drm_encoder *encoder, void *buffer,
@@ -462,10 +462,10 @@ bool rv770_page_flip_pending(struct radeon_device *rdev, int crtc);
 void r700_vram_gtt_location(struct radeon_device *rdev, struct radeon_mc *mc);
 void r700_cp_stop(struct radeon_device *rdev);
 void r700_cp_fini(struct radeon_device *rdev);
-int rv770_copy_dma(struct radeon_device *rdev,
-		  uint64_t src_offset, uint64_t dst_offset,
-		  unsigned num_gpu_pages,
-		   struct radeon_fence **fence);
+struct radeon_fence *rv770_copy_dma(struct radeon_device *rdev,
+				    uint64_t src_offset, uint64_t dst_offset,
+				    unsigned num_gpu_pages,
+				    struct reservation_object *resv);
 u32 rv770_get_xclk(struct radeon_device *rdev);
 int rv770_set_uvd_clocks(struct radeon_device *rdev, u32 vclk, u32 dclk);
 int rv770_get_temp(struct radeon_device *rdev);
@@ -536,10 +536,10 @@ void evergreen_dma_fence_ring_emit(struct radeon_device *rdev,
 				   struct radeon_fence *fence);
 void evergreen_dma_ring_ib_execute(struct radeon_device *rdev,
 				   struct radeon_ib *ib);
-int evergreen_copy_dma(struct radeon_device *rdev,
-		       uint64_t src_offset, uint64_t dst_offset,
-		       unsigned num_gpu_pages,
-		       struct radeon_fence **fence);
+struct radeon_fence *evergreen_copy_dma(struct radeon_device *rdev,
+					uint64_t src_offset, uint64_t dst_offset,
+					unsigned num_gpu_pages,
+					struct reservation_object *resv);
 void evergreen_hdmi_enable(struct drm_encoder *encoder, bool enable);
 void evergreen_hdmi_setmode(struct drm_encoder *encoder, struct drm_display_mode *mode);
 int evergreen_get_temp(struct radeon_device *rdev);
@@ -701,10 +701,10 @@ int si_vm_init(struct radeon_device *rdev);
 void si_vm_fini(struct radeon_device *rdev);
 void si_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm);
 int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib);
-int si_copy_dma(struct radeon_device *rdev,
-		uint64_t src_offset, uint64_t dst_offset,
-		unsigned num_gpu_pages,
-		struct radeon_fence **fence);
+struct radeon_fence *si_copy_dma(struct radeon_device *rdev,
+				 uint64_t src_offset, uint64_t dst_offset,
+				 unsigned num_gpu_pages,
+				 struct reservation_object *resv);
 
 void si_dma_vm_copy_pages(struct radeon_device *rdev,
 			  struct radeon_ib *ib,
@@ -760,14 +760,14 @@ bool cik_sdma_semaphore_ring_emit(struct radeon_device *rdev,
 				  struct radeon_semaphore *semaphore,
 				  bool emit_wait);
 void cik_sdma_ring_ib_execute(struct radeon_device *rdev, struct radeon_ib *ib);
-int cik_copy_dma(struct radeon_device *rdev,
-		 uint64_t src_offset, uint64_t dst_offset,
-		 unsigned num_gpu_pages,
-		 struct radeon_fence **fence);
-int cik_copy_cpdma(struct radeon_device *rdev,
-		   uint64_t src_offset, uint64_t dst_offset,
-		   unsigned num_gpu_pages,
-		   struct radeon_fence **fence);
+struct radeon_fence *cik_copy_dma(struct radeon_device *rdev,
+				  uint64_t src_offset, uint64_t dst_offset,
+				  unsigned num_gpu_pages,
+				  struct reservation_object *resv);
+struct radeon_fence *cik_copy_cpdma(struct radeon_device *rdev,
+				    uint64_t src_offset, uint64_t dst_offset,
+				    unsigned num_gpu_pages,
+				    struct reservation_object *resv);
 int cik_sdma_ring_test(struct radeon_device *rdev, struct radeon_ring *ring);
 int cik_sdma_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
 bool cik_sdma_is_lockup(struct radeon_device *rdev, struct radeon_ring *ring);
@@ -883,6 +883,7 @@ uint32_t uvd_v1_0_get_wptr(struct radeon_device *rdev,
                            struct radeon_ring *ring);
 void uvd_v1_0_set_wptr(struct radeon_device *rdev,
                        struct radeon_ring *ring);
+int uvd_v1_0_resume(struct radeon_device *rdev);
 
 int uvd_v1_0_init(struct radeon_device *rdev);
 void uvd_v1_0_fini(struct radeon_device *rdev);
@@ -890,6 +891,8 @@ int uvd_v1_0_start(struct radeon_device *rdev);
 void uvd_v1_0_stop(struct radeon_device *rdev);
 
 int uvd_v1_0_ring_test(struct radeon_device *rdev, struct radeon_ring *ring);
+void uvd_v1_0_fence_emit(struct radeon_device *rdev,
+			 struct radeon_fence *fence);
 int uvd_v1_0_ib_test(struct radeon_device *rdev, struct radeon_ring *ring);
 bool uvd_v1_0_semaphore_emit(struct radeon_device *rdev,
 			     struct radeon_ring *ring,
diff --git a/drivers/gpu/drm/radeon/radeon_atombios.c b/drivers/gpu/drm/radeon/radeon_atombios.c
index 92b2d8dd4735..df69b92ba164 100644
--- a/drivers/gpu/drm/radeon/radeon_atombios.c
+++ b/drivers/gpu/drm/radeon/radeon_atombios.c
@@ -447,11 +447,18 @@ static bool radeon_atom_apply_quirks(struct drm_device *dev,
 		}
 	}
 
+	/* Fujitsu D3003-S2 board lists DVI-I as DVI-I and VGA */
+	if ((dev->pdev->device == 0x9805) &&
+	    (dev->pdev->subsystem_vendor == 0x1734) &&
+	    (dev->pdev->subsystem_device == 0x11bd)) {
+		if (*connector_type == DRM_MODE_CONNECTOR_VGA)
+			return false;
+	}
 
 	return true;
 }
 
-const int supported_devices_connector_convert[] = {
+static const int supported_devices_connector_convert[] = {
 	DRM_MODE_CONNECTOR_Unknown,
 	DRM_MODE_CONNECTOR_VGA,
 	DRM_MODE_CONNECTOR_DVII,
@@ -470,7 +477,7 @@ const int supported_devices_connector_convert[] = {
 	DRM_MODE_CONNECTOR_DisplayPort
 };
 
-const uint16_t supported_devices_connector_object_id_convert[] = {
+static const uint16_t supported_devices_connector_object_id_convert[] = {
 	CONNECTOR_OBJECT_ID_NONE,
 	CONNECTOR_OBJECT_ID_VGA,
 	CONNECTOR_OBJECT_ID_DUAL_LINK_DVI_I, /* not all boards support DL */
@@ -487,7 +494,7 @@ const uint16_t supported_devices_connector_object_id_convert[] = {
 	CONNECTOR_OBJECT_ID_SVIDEO
 };
 
-const int object_connector_convert[] = {
+static const int object_connector_convert[] = {
 	DRM_MODE_CONNECTOR_Unknown,
 	DRM_MODE_CONNECTOR_DVII,
 	DRM_MODE_CONNECTOR_DVII,
@@ -2281,19 +2288,31 @@ static void radeon_atombios_add_pplib_thermal_controller(struct radeon_device *r
 				 (controller->ucFanParameters &
 				  ATOM_PP_FANPARAMETERS_NOFAN) ? "without" : "with");
 			rdev->pm.int_thermal_type = THERMAL_TYPE_KV;
-		} else if ((controller->ucType ==
-			    ATOM_PP_THERMALCONTROLLER_EXTERNAL_GPIO) ||
-			   (controller->ucType ==
-			    ATOM_PP_THERMALCONTROLLER_ADT7473_WITH_INTERNAL) ||
-			   (controller->ucType ==
-			    ATOM_PP_THERMALCONTROLLER_EMC2103_WITH_INTERNAL)) {
-			DRM_INFO("Special thermal controller config\n");
+		} else if (controller->ucType ==
+			   ATOM_PP_THERMALCONTROLLER_EXTERNAL_GPIO) {
+			DRM_INFO("External GPIO thermal controller %s fan control\n",
+				 (controller->ucFanParameters &
+				  ATOM_PP_FANPARAMETERS_NOFAN) ? "without" : "with");
+			rdev->pm.int_thermal_type = THERMAL_TYPE_EXTERNAL_GPIO;
+		} else if (controller->ucType ==
+			   ATOM_PP_THERMALCONTROLLER_ADT7473_WITH_INTERNAL) {
+			DRM_INFO("ADT7473 with internal thermal controller %s fan control\n",
+				 (controller->ucFanParameters &
+				  ATOM_PP_FANPARAMETERS_NOFAN) ? "without" : "with");
+			rdev->pm.int_thermal_type = THERMAL_TYPE_ADT7473_WITH_INTERNAL;
+		} else if (controller->ucType ==
+			   ATOM_PP_THERMALCONTROLLER_EMC2103_WITH_INTERNAL) {
+			DRM_INFO("EMC2103 with internal thermal controller %s fan control\n",
+				 (controller->ucFanParameters &
+				  ATOM_PP_FANPARAMETERS_NOFAN) ? "without" : "with");
+			rdev->pm.int_thermal_type = THERMAL_TYPE_EMC2103_WITH_INTERNAL;
 		} else if (controller->ucType < ARRAY_SIZE(pp_lib_thermal_controller_names)) {
 			DRM_INFO("Possible %s thermal controller at 0x%02x %s fan control\n",
 				 pp_lib_thermal_controller_names[controller->ucType],
 				 controller->ucI2cAddress >> 1,
 				 (controller->ucFanParameters &
 				  ATOM_PP_FANPARAMETERS_NOFAN) ? "without" : "with");
+			rdev->pm.int_thermal_type = THERMAL_TYPE_EXTERNAL;
 			i2c_bus = radeon_lookup_i2c_gpio(rdev, controller->ucI2cLine);
 			rdev->pm.i2c_bus = radeon_i2c_lookup(rdev, &i2c_bus);
 			if (rdev->pm.i2c_bus) {
diff --git a/drivers/gpu/drm/radeon/radeon_atpx_handler.c b/drivers/gpu/drm/radeon/radeon_atpx_handler.c
index a9fb0d016d38..8bc7d0bbd3c8 100644
--- a/drivers/gpu/drm/radeon/radeon_atpx_handler.c
+++ b/drivers/gpu/drm/radeon/radeon_atpx_handler.c
@@ -33,7 +33,6 @@ static struct radeon_atpx_priv {
 	bool atpx_detected;
 	/* handle for device - and atpx */
 	acpi_handle dhandle;
-	acpi_handle other_handle;
 	struct radeon_atpx atpx;
 } radeon_atpx_priv;
 
@@ -453,10 +452,9 @@ static bool radeon_atpx_pci_probe_handle(struct pci_dev *pdev)
 		return false;
 
 	status = acpi_get_handle(dhandle, "ATPX", &atpx_handle);
-	if (ACPI_FAILURE(status)) {
-		radeon_atpx_priv.other_handle = dhandle;
+	if (ACPI_FAILURE(status))
 		return false;
-	}
+
 	radeon_atpx_priv.dhandle = dhandle;
 	radeon_atpx_priv.atpx.handle = atpx_handle;
 	return true;
@@ -540,16 +538,6 @@ static bool radeon_atpx_detect(void)
 		printk(KERN_INFO "VGA switcheroo: detected switching method %s handle\n",
 		       acpi_method_name);
 		radeon_atpx_priv.atpx_detected = true;
-		/*
-		 * On some systems hotplug events are generated for the device
-		 * being switched off when ATPX is executed.  They cause ACPI
-		 * hotplug to trigger and attempt to remove the device from
-		 * the system, which causes it to break down.  Prevent that from
-		 * happening by setting the no_hotplug flag for the involved
-		 * ACPI device objects.
-		 */
-		acpi_bus_no_hotplug(radeon_atpx_priv.dhandle);
-		acpi_bus_no_hotplug(radeon_atpx_priv.other_handle);
 		return true;
 	}
 	return false;
diff --git a/drivers/gpu/drm/radeon/radeon_benchmark.c b/drivers/gpu/drm/radeon/radeon_benchmark.c
index 69f5695bdab9..9e7f23dd14bd 100644
--- a/drivers/gpu/drm/radeon/radeon_benchmark.c
+++ b/drivers/gpu/drm/radeon/radeon_benchmark.c
@@ -45,33 +45,29 @@ static int radeon_benchmark_do_move(struct radeon_device *rdev, unsigned size,
 	for (i = 0; i < n; i++) {
 		switch (flag) {
 		case RADEON_BENCHMARK_COPY_DMA:
-			r = radeon_copy_dma(rdev, saddr, daddr,
-					    size / RADEON_GPU_PAGE_SIZE,
-					    &fence);
+			fence = radeon_copy_dma(rdev, saddr, daddr,
+						size / RADEON_GPU_PAGE_SIZE,
+						NULL);
 			break;
 		case RADEON_BENCHMARK_COPY_BLIT:
-			r = radeon_copy_blit(rdev, saddr, daddr,
-					     size / RADEON_GPU_PAGE_SIZE,
-					     &fence);
+			fence = radeon_copy_blit(rdev, saddr, daddr,
+						 size / RADEON_GPU_PAGE_SIZE,
+						 NULL);
 			break;
 		default:
 			DRM_ERROR("Unknown copy method\n");
-			r = -EINVAL;
+			return -EINVAL;
 		}
-		if (r)
-			goto exit_do_move;
+		if (IS_ERR(fence))
+			return PTR_ERR(fence);
+
 		r = radeon_fence_wait(fence, false);
-		if (r)
-			goto exit_do_move;
 		radeon_fence_unref(&fence);
+		if (r)
+			return r;
 	}
 	end_jiffies = jiffies;
-	r = jiffies_to_msecs(end_jiffies - start_jiffies);
-
-exit_do_move:
-	if (fence)
-		radeon_fence_unref(&fence);
-	return r;
+	return jiffies_to_msecs(end_jiffies - start_jiffies);
 }
 
 
@@ -97,7 +93,7 @@ static void radeon_benchmark_move(struct radeon_device *rdev, unsigned size,
 	int time;
 
 	n = RADEON_BENCHMARK_ITERATIONS;
-	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, sdomain, 0, NULL, &sobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, sdomain, 0, NULL, NULL, &sobj);
 	if (r) {
 		goto out_cleanup;
 	}
@@ -109,7 +105,7 @@ static void radeon_benchmark_move(struct radeon_device *rdev, unsigned size,
 	if (r) {
 		goto out_cleanup;
 	}
-	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, ddomain, 0, NULL, &dobj);
+	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, ddomain, 0, NULL, NULL, &dobj);
 	if (r) {
 		goto out_cleanup;
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_combios.c b/drivers/gpu/drm/radeon/radeon_combios.c
index 6651177110f0..3e5f6b71f3ad 100644
--- a/drivers/gpu/drm/radeon/radeon_combios.c
+++ b/drivers/gpu/drm/radeon/radeon_combios.c
@@ -116,7 +116,7 @@ enum radeon_combios_connector {
 	CONNECTOR_UNSUPPORTED_LEGACY
 };
 
-const int legacy_connector_convert[] = {
+static const int legacy_connector_convert[] = {
 	DRM_MODE_CONNECTOR_Unknown,
 	DRM_MODE_CONNECTOR_DVID,
 	DRM_MODE_CONNECTOR_VGA,
diff --git a/drivers/gpu/drm/radeon/radeon_cp.c b/drivers/gpu/drm/radeon/radeon_cp.c
index bb0d5c3a8311..ea134a7d51a5 100644
--- a/drivers/gpu/drm/radeon/radeon_cp.c
+++ b/drivers/gpu/drm/radeon/radeon_cp.c
@@ -1298,27 +1298,27 @@ static int radeon_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 	dev_priv->buffers_offset = init->buffers_offset;
 	dev_priv->gart_textures_offset = init->gart_textures_offset;
 
-	master_priv->sarea = drm_getsarea(dev);
+	master_priv->sarea = drm_legacy_getsarea(dev);
 	if (!master_priv->sarea) {
 		DRM_ERROR("could not find sarea!\n");
 		radeon_do_cleanup_cp(dev);
 		return -EINVAL;
 	}
 
-	dev_priv->cp_ring = drm_core_findmap(dev, init->ring_offset);
+	dev_priv->cp_ring = drm_legacy_findmap(dev, init->ring_offset);
 	if (!dev_priv->cp_ring) {
 		DRM_ERROR("could not find cp ring region!\n");
 		radeon_do_cleanup_cp(dev);
 		return -EINVAL;
 	}
-	dev_priv->ring_rptr = drm_core_findmap(dev, init->ring_rptr_offset);
+	dev_priv->ring_rptr = drm_legacy_findmap(dev, init->ring_rptr_offset);
 	if (!dev_priv->ring_rptr) {
 		DRM_ERROR("could not find ring read pointer!\n");
 		radeon_do_cleanup_cp(dev);
 		return -EINVAL;
 	}
 	dev->agp_buffer_token = init->buffers_offset;
-	dev->agp_buffer_map = drm_core_findmap(dev, init->buffers_offset);
+	dev->agp_buffer_map = drm_legacy_findmap(dev, init->buffers_offset);
 	if (!dev->agp_buffer_map) {
 		DRM_ERROR("could not find dma buffer region!\n");
 		radeon_do_cleanup_cp(dev);
@@ -1327,7 +1327,7 @@ static int radeon_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 
 	if (init->gart_textures_offset) {
 		dev_priv->gart_textures =
-		    drm_core_findmap(dev, init->gart_textures_offset);
+		    drm_legacy_findmap(dev, init->gart_textures_offset);
 		if (!dev_priv->gart_textures) {
 			DRM_ERROR("could not find GART texture region!\n");
 			radeon_do_cleanup_cp(dev);
@@ -1337,9 +1337,9 @@ static int radeon_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 
 #if __OS_HAS_AGP
 	if (dev_priv->flags & RADEON_IS_AGP) {
-		drm_core_ioremap_wc(dev_priv->cp_ring, dev);
-		drm_core_ioremap_wc(dev_priv->ring_rptr, dev);
-		drm_core_ioremap_wc(dev->agp_buffer_map, dev);
+		drm_legacy_ioremap_wc(dev_priv->cp_ring, dev);
+		drm_legacy_ioremap_wc(dev_priv->ring_rptr, dev);
+		drm_legacy_ioremap_wc(dev->agp_buffer_map, dev);
 		if (!dev_priv->cp_ring->handle ||
 		    !dev_priv->ring_rptr->handle ||
 		    !dev->agp_buffer_map->handle) {
@@ -1475,7 +1475,7 @@ static int radeon_do_init_cp(struct drm_device *dev, drm_radeon_init_t *init,
 			dev_priv->gart_info.mapping.size =
 			    dev_priv->gart_info.table_size;
 
-			drm_core_ioremap_wc(&dev_priv->gart_info.mapping, dev);
+			drm_legacy_ioremap_wc(&dev_priv->gart_info.mapping, dev);
 			dev_priv->gart_info.addr =
 			    dev_priv->gart_info.mapping.handle;
 
@@ -1569,15 +1569,15 @@ static int radeon_do_cleanup_cp(struct drm_device * dev)
 #if __OS_HAS_AGP
 	if (dev_priv->flags & RADEON_IS_AGP) {
 		if (dev_priv->cp_ring != NULL) {
-			drm_core_ioremapfree(dev_priv->cp_ring, dev);
+			drm_legacy_ioremapfree(dev_priv->cp_ring, dev);
 			dev_priv->cp_ring = NULL;
 		}
 		if (dev_priv->ring_rptr != NULL) {
-			drm_core_ioremapfree(dev_priv->ring_rptr, dev);
+			drm_legacy_ioremapfree(dev_priv->ring_rptr, dev);
 			dev_priv->ring_rptr = NULL;
 		}
 		if (dev->agp_buffer_map != NULL) {
-			drm_core_ioremapfree(dev->agp_buffer_map, dev);
+			drm_legacy_ioremapfree(dev->agp_buffer_map, dev);
 			dev->agp_buffer_map = NULL;
 		}
 	} else
@@ -1597,7 +1597,7 @@ static int radeon_do_cleanup_cp(struct drm_device * dev)
 
 		if (dev_priv->gart_info.gart_table_location == DRM_ATI_GART_FB)
 		{
-			drm_core_ioremapfree(&dev_priv->gart_info.mapping, dev);
+			drm_legacy_ioremapfree(&dev_priv->gart_info.mapping, dev);
 			dev_priv->gart_info.addr = NULL;
 		}
 	}
@@ -2106,9 +2106,9 @@ int radeon_driver_load(struct drm_device *dev, unsigned long flags)
 	else
 		dev_priv->flags |= RADEON_IS_PCI;
 
-	ret = drm_addmap(dev, pci_resource_start(dev->pdev, 2),
-			 pci_resource_len(dev->pdev, 2), _DRM_REGISTERS,
-			 _DRM_READ_ONLY | _DRM_DRIVER, &dev_priv->mmio);
+	ret = drm_legacy_addmap(dev, pci_resource_start(dev->pdev, 2),
+				pci_resource_len(dev->pdev, 2), _DRM_REGISTERS,
+				_DRM_READ_ONLY | _DRM_DRIVER, &dev_priv->mmio);
 	if (ret != 0)
 		return ret;
 
@@ -2135,8 +2135,8 @@ int radeon_master_create(struct drm_device *dev, struct drm_master *master)
 
 	/* prebuild the SAREA */
 	sareapage = max_t(unsigned long, SAREA_MAX, PAGE_SIZE);
-	ret = drm_addmap(dev, 0, sareapage, _DRM_SHM, _DRM_CONTAINS_LOCK,
-			 &master_priv->sarea);
+	ret = drm_legacy_addmap(dev, 0, sareapage, _DRM_SHM, _DRM_CONTAINS_LOCK,
+				&master_priv->sarea);
 	if (ret) {
 		DRM_ERROR("SAREA setup failed\n");
 		kfree(master_priv);
@@ -2162,7 +2162,7 @@ void radeon_master_destroy(struct drm_device *dev, struct drm_master *master)
 
 	master_priv->sarea_priv = NULL;
 	if (master_priv->sarea)
-		drm_rmmap_locked(dev, master_priv->sarea);
+		drm_legacy_rmmap_locked(dev, master_priv->sarea);
 
 	kfree(master_priv);
 
@@ -2181,9 +2181,9 @@ int radeon_driver_firstopen(struct drm_device *dev)
 	dev_priv->gart_info.table_size = RADEON_PCIGART_TABLE_SIZE;
 
 	dev_priv->fb_aper_offset = pci_resource_start(dev->pdev, 0);
-	ret = drm_addmap(dev, dev_priv->fb_aper_offset,
-			 pci_resource_len(dev->pdev, 0), _DRM_FRAME_BUFFER,
-			 _DRM_WRITE_COMBINING, &map);
+	ret = drm_legacy_addmap(dev, dev_priv->fb_aper_offset,
+				pci_resource_len(dev->pdev, 0),
+				_DRM_FRAME_BUFFER, _DRM_WRITE_COMBINING, &map);
 	if (ret != 0)
 		return ret;
 
@@ -2196,7 +2196,7 @@ int radeon_driver_unload(struct drm_device *dev)
 
 	DRM_DEBUG("\n");
 
-	drm_rmmap(dev, dev_priv->mmio);
+	drm_legacy_rmmap(dev, dev_priv->mmio);
 
 	kfree(dev_priv);
 
diff --git a/drivers/gpu/drm/radeon/radeon_cs.c b/drivers/gpu/drm/radeon/radeon_cs.c
index ee712c199b25..1c893447d7cd 100644
--- a/drivers/gpu/drm/radeon/radeon_cs.c
+++ b/drivers/gpu/drm/radeon/radeon_cs.c
@@ -78,7 +78,8 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 	struct radeon_cs_chunk *chunk;
 	struct radeon_cs_buckets buckets;
 	unsigned i, j;
-	bool duplicate;
+	bool duplicate, need_mmap_lock = false;
+	int r;
 
 	if (p->chunk_relocs_idx == -1) {
 		return 0;
@@ -132,13 +133,17 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 		 * the buffers used for read only, which doubles the range
 		 * to 0 to 31. 32 is reserved for the kernel driver.
 		 */
-		priority = (r->flags & 0xf) * 2 + !!r->write_domain;
+		priority = (r->flags & RADEON_RELOC_PRIO_MASK) * 2
+			   + !!r->write_domain;
 
 		/* the first reloc of an UVD job is the msg and that must be in
-		   VRAM, also but everything into VRAM on AGP cards to avoid
-		   image corruptions */
+		   VRAM, also but everything into VRAM on AGP cards and older
+		   IGP chips to avoid image corruptions */
 		if (p->ring == R600_RING_TYPE_UVD_INDEX &&
-		    (i == 0 || drm_pci_device_is_agp(p->rdev->ddev))) {
+		    (i == 0 || drm_pci_device_is_agp(p->rdev->ddev) ||
+		     p->rdev->family == CHIP_RS780 ||
+		     p->rdev->family == CHIP_RS880)) {
+
 			/* TODO: is this still needed for NI+ ? */
 			p->relocs[i].prefered_domains =
 				RADEON_GEM_DOMAIN_VRAM;
@@ -164,7 +169,21 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 			p->relocs[i].allowed_domains = domain;
 		}
 
+		if (radeon_ttm_tt_has_userptr(p->relocs[i].robj->tbo.ttm)) {
+			uint32_t domain = p->relocs[i].prefered_domains;
+			if (!(domain & RADEON_GEM_DOMAIN_GTT)) {
+				DRM_ERROR("Only RADEON_GEM_DOMAIN_GTT is "
+					  "allowed for userptr BOs\n");
+				return -EINVAL;
+			}
+			need_mmap_lock = true;
+			domain = RADEON_GEM_DOMAIN_GTT;
+			p->relocs[i].prefered_domains = domain;
+			p->relocs[i].allowed_domains = domain;
+		}
+
 		p->relocs[i].tv.bo = &p->relocs[i].robj->tbo;
+		p->relocs[i].tv.shared = !r->write_domain;
 		p->relocs[i].handle = r->handle;
 
 		radeon_cs_buckets_add(&buckets, &p->relocs[i].tv.head,
@@ -176,8 +195,15 @@ static int radeon_cs_parser_relocs(struct radeon_cs_parser *p)
 	if (p->cs_flags & RADEON_CS_USE_VM)
 		p->vm_bos = radeon_vm_get_bos(p->rdev, p->ib.vm,
 					      &p->validated);
+	if (need_mmap_lock)
+		down_read(&current->mm->mmap_sem);
 
-	return radeon_bo_list_validate(p->rdev, &p->ticket, &p->validated, p->ring);
+	r = radeon_bo_list_validate(p->rdev, &p->ticket, &p->validated, p->ring);
+
+	if (need_mmap_lock)
+		up_read(&current->mm->mmap_sem);
+
+	return r;
 }
 
 static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority)
@@ -223,17 +249,24 @@ static int radeon_cs_get_ring(struct radeon_cs_parser *p, u32 ring, s32 priority
 	return 0;
 }
 
-static void radeon_cs_sync_rings(struct radeon_cs_parser *p)
+static int radeon_cs_sync_rings(struct radeon_cs_parser *p)
 {
-	int i;
+	int i, r = 0;
 
 	for (i = 0; i < p->nrelocs; i++) {
+		struct reservation_object *resv;
+
 		if (!p->relocs[i].robj)
 			continue;
 
-		radeon_semaphore_sync_to(p->ib.semaphore,
-					 p->relocs[i].robj->tbo.sync_obj);
+		resv = p->relocs[i].robj->tbo.resv;
+		r = radeon_semaphore_sync_resv(p->rdev, p->ib.semaphore, resv,
+					       p->relocs[i].tv.shared);
+
+		if (r)
+			break;
 	}
+	return r;
 }
 
 /* XXX: note that this is called from the legacy UMS CS ioctl as well */
@@ -402,7 +435,7 @@ static void radeon_cs_parser_fini(struct radeon_cs_parser *parser, int error, bo
 
 		ttm_eu_fence_buffer_objects(&parser->ticket,
 					    &parser->validated,
-					    parser->ib.fence);
+					    &parser->ib.fence->base);
 	} else if (backoff) {
 		ttm_eu_backoff_reservation(&parser->ticket,
 					   &parser->validated);
@@ -443,14 +476,20 @@ static int radeon_cs_ib_chunk(struct radeon_device *rdev,
 		return r;
 	}
 
+	r = radeon_cs_sync_rings(parser);
+	if (r) {
+		if (r != -ERESTARTSYS)
+			DRM_ERROR("Failed to sync rings: %i\n", r);
+		return r;
+	}
+
 	if (parser->ring == R600_RING_TYPE_UVD_INDEX)
 		radeon_uvd_note_usage(rdev);
 	else if ((parser->ring == TN_RING_TYPE_VCE1_INDEX) ||
 		 (parser->ring == TN_RING_TYPE_VCE2_INDEX))
 		radeon_vce_note_usage(rdev);
 
-	radeon_cs_sync_rings(parser);
-	r = radeon_ib_schedule(rdev, &parser->ib, NULL);
+	r = radeon_ib_schedule(rdev, &parser->ib, NULL, true);
 	if (r) {
 		DRM_ERROR("Failed to schedule IB !\n");
 	}
@@ -536,14 +575,20 @@ static int radeon_cs_ib_vm_chunk(struct radeon_device *rdev,
 	if (r) {
 		goto out;
 	}
-	radeon_cs_sync_rings(parser);
-	radeon_semaphore_sync_to(parser->ib.semaphore, vm->fence);
+
+	r = radeon_cs_sync_rings(parser);
+	if (r) {
+		if (r != -ERESTARTSYS)
+			DRM_ERROR("Failed to sync rings: %i\n", r);
+		goto out;
+	}
+	radeon_semaphore_sync_fence(parser->ib.semaphore, vm->fence);
 
 	if ((rdev->family >= CHIP_TAHITI) &&
 	    (parser->chunk_const_ib_idx != -1)) {
-		r = radeon_ib_schedule(rdev, &parser->ib, &parser->const_ib);
+		r = radeon_ib_schedule(rdev, &parser->ib, &parser->const_ib, true);
 	} else {
-		r = radeon_ib_schedule(rdev, &parser->ib, NULL);
+		r = radeon_ib_schedule(rdev, &parser->ib, NULL, true);
 	}
 
 out:
@@ -628,6 +673,13 @@ int radeon_cs_ioctl(struct drm_device *dev, void *data, struct drm_file *filp)
 		up_read(&rdev->exclusive_lock);
 		return -EBUSY;
 	}
+	if (rdev->in_reset) {
+		up_read(&rdev->exclusive_lock);
+		r = radeon_gpu_reset(rdev);
+		if (!r)
+			r = -EAGAIN;
+		return r;
+	}
 	/* initialize parser */
 	memset(&parser, 0, sizeof(struct radeon_cs_parser));
 	parser.filp = filp;
diff --git a/drivers/gpu/drm/radeon/radeon_device.c b/drivers/gpu/drm/radeon/radeon_device.c
index c8ea050c8fa4..ea2676954dde 100644
--- a/drivers/gpu/drm/radeon/radeon_device.c
+++ b/drivers/gpu/drm/radeon/radeon_device.c
@@ -123,6 +123,10 @@ static struct radeon_px_quirk radeon_px_quirk_list[] = {
 	 * https://bugzilla.kernel.org/show_bug.cgi?id=51381
 	 */
 	{ PCI_VENDOR_ID_ATI, 0x6741, 0x1043, 0x108c, RADEON_PX_QUIRK_DISABLE_PX },
+	/* Asus K53TK laptop with AMD A6-3420M APU and Radeon 7670m GPU
+	 * https://bugzilla.kernel.org/show_bug.cgi?id=51381
+	 */
+	{ PCI_VENDOR_ID_ATI, 0x6840, 0x1043, 0x2122, RADEON_PX_QUIRK_DISABLE_PX },
 	/* macbook pro 8.2 */
 	{ PCI_VENDOR_ID_ATI, 0x6741, PCI_VENDOR_ID_APPLE, 0x00e2, RADEON_PX_QUIRK_LONG_WAKEUP },
 	{ 0, 0, 0, 0, 0 },
@@ -430,7 +434,7 @@ int radeon_wb_init(struct radeon_device *rdev)
 
 	if (rdev->wb.wb_obj == NULL) {
 		r = radeon_bo_create(rdev, RADEON_GPU_PAGE_SIZE, PAGE_SIZE, true,
-				     RADEON_GEM_DOMAIN_GTT, 0, NULL,
+				     RADEON_GEM_DOMAIN_GTT, 0, NULL, NULL,
 				     &rdev->wb.wb_obj);
 		if (r) {
 			dev_warn(rdev->dev, "(%d) create WB bo failed\n", r);
@@ -1126,7 +1130,7 @@ static void radeon_check_arguments(struct radeon_device *rdev)
 	if (radeon_vm_block_size == -1) {
 
 		/* Total bits covered by PD + PTs */
-		unsigned bits = ilog2(radeon_vm_size) + 17;
+		unsigned bits = ilog2(radeon_vm_size) + 18;
 
 		/* Make sure the PD is 4K in size up to 8GB address space.
 		   Above that split equal between PD and PTs */
@@ -1253,6 +1257,7 @@ int radeon_device_init(struct radeon_device *rdev,
 	for (i = 0; i < RADEON_NUM_RINGS; i++) {
 		rdev->ring[i].idx = i;
 	}
+	rdev->fence_context = fence_context_alloc(RADEON_NUM_RINGS);
 
 	DRM_INFO("initializing kernel modesetting (%s 0x%04X:0x%04X 0x%04X:0x%04X).\n",
 		radeon_family_name[rdev->family], pdev->vendor, pdev->device,
@@ -1270,6 +1275,8 @@ int radeon_device_init(struct radeon_device *rdev,
 	init_rwsem(&rdev->pm.mclk_lock);
 	init_rwsem(&rdev->exclusive_lock);
 	init_waitqueue_head(&rdev->irq.vblank_queue);
+	mutex_init(&rdev->mn_lock);
+	hash_init(rdev->mn_hash);
 	r = radeon_gem_init(rdev);
 	if (r)
 		return r;
@@ -1393,11 +1400,7 @@ int radeon_device_init(struct radeon_device *rdev,
 
 	r = radeon_init(rdev);
 	if (r)
-		return r;
-
-	r = radeon_ib_ring_tests(rdev);
-	if (r)
-		DRM_ERROR("ib ring test failed (%d).\n", r);
+		goto failed;
 
 	r = radeon_gem_debugfs_init(rdev);
 	if (r) {
@@ -1413,9 +1416,13 @@ int radeon_device_init(struct radeon_device *rdev,
 		radeon_agp_disable(rdev);
 		r = radeon_init(rdev);
 		if (r)
-			return r;
+			goto failed;
 	}
 
+	r = radeon_ib_ring_tests(rdev);
+	if (r)
+		DRM_ERROR("ib ring test failed (%d).\n", r);
+
 	if ((radeon_testing & 1)) {
 		if (rdev->accel_working)
 			radeon_test_moves(rdev);
@@ -1435,6 +1442,11 @@ int radeon_device_init(struct radeon_device *rdev,
 			DRM_INFO("radeon: acceleration disabled, skipping benchmarks\n");
 	}
 	return 0;
+
+failed:
+	if (runtime)
+		vga_switcheroo_fini_domain_pm_ops(rdev->dev);
+	return r;
 }
 
 static void radeon_debugfs_remove_files(struct radeon_device *rdev);
@@ -1455,6 +1467,8 @@ void radeon_device_fini(struct radeon_device *rdev)
 	radeon_bo_evict_vram(rdev);
 	radeon_fini(rdev);
 	vga_switcheroo_unregister_client(rdev->pdev);
+	if (rdev->flags & RADEON_IS_PX)
+		vga_switcheroo_fini_domain_pm_ops(rdev->dev);
 	vga_client_register(rdev->pdev, NULL, NULL, NULL);
 	if (rdev->rio_mem)
 		pci_iounmap(rdev->pdev, rdev->rio_mem);
@@ -1486,7 +1500,6 @@ int radeon_suspend_kms(struct drm_device *dev, bool suspend, bool fbcon)
 	struct drm_crtc *crtc;
 	struct drm_connector *connector;
 	int i, r;
-	bool force_completion = false;
 
 	if (dev == NULL || dev->dev_private == NULL) {
 		return -ENODEV;
@@ -1530,12 +1543,9 @@ int radeon_suspend_kms(struct drm_device *dev, bool suspend, bool fbcon)
 		r = radeon_fence_wait_empty(rdev, i);
 		if (r) {
 			/* delay GPU reset to resume */
-			force_completion = true;
+			radeon_fence_driver_force_completion(rdev, i);
 		}
 	}
-	if (force_completion) {
-		radeon_fence_driver_force_completion(rdev);
-	}
 
 	radeon_save_bios_scratch_regs(rdev);
 
@@ -1675,13 +1685,11 @@ int radeon_gpu_reset(struct radeon_device *rdev)
 		return 0;
 	}
 
-	rdev->needs_reset = false;
-
 	radeon_save_bios_scratch_regs(rdev);
 	/* block TTM */
 	resched = ttm_bo_lock_delayed_workqueue(&rdev->mman.bdev);
-	radeon_pm_suspend(rdev);
 	radeon_suspend(rdev);
+	radeon_hpd_fini(rdev);
 
 	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
 		ring_sizes[i] = radeon_ring_backup(rdev, &rdev->ring[i],
@@ -1693,7 +1701,6 @@ int radeon_gpu_reset(struct radeon_device *rdev)
 		}
 	}
 
-retry:
 	r = radeon_asic_reset(rdev);
 	if (!r) {
 		dev_info(rdev->dev, "GPU reset succeeded, trying to resume\n");
@@ -1702,40 +1709,69 @@ retry:
 
 	radeon_restore_bios_scratch_regs(rdev);
 
-	if (!r) {
-		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (!r && ring_data[i]) {
 			radeon_ring_restore(rdev, &rdev->ring[i],
 					    ring_sizes[i], ring_data[i]);
-			ring_sizes[i] = 0;
-			ring_data[i] = NULL;
+		} else {
+			radeon_fence_driver_force_completion(rdev, i);
+			kfree(ring_data[i]);
 		}
+	}
 
-		r = radeon_ib_ring_tests(rdev);
+	if ((rdev->pm.pm_method == PM_METHOD_DPM) && rdev->pm.dpm_enabled) {
+		/* do dpm late init */
+		r = radeon_pm_late_init(rdev);
 		if (r) {
-			dev_err(rdev->dev, "ib ring test failed (%d).\n", r);
-			if (saved) {
-				saved = false;
-				radeon_suspend(rdev);
-				goto retry;
-			}
+			rdev->pm.dpm_enabled = false;
+			DRM_ERROR("radeon_pm_late_init failed, disabling dpm\n");
 		}
 	} else {
-		radeon_fence_driver_force_completion(rdev);
-		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-			kfree(ring_data[i]);
+		/* resume old pm late */
+		radeon_pm_resume(rdev);
+	}
+
+	/* init dig PHYs, disp eng pll */
+	if (rdev->is_atom_bios) {
+		radeon_atom_encoder_init(rdev);
+		radeon_atom_disp_eng_pll_init(rdev);
+		/* turn on the BL */
+		if (rdev->mode_info.bl_encoder) {
+			u8 bl_level = radeon_get_backlight_level(rdev,
+								 rdev->mode_info.bl_encoder);
+			radeon_set_backlight_level(rdev, rdev->mode_info.bl_encoder,
+						   bl_level);
 		}
 	}
+	/* reset hpd state */
+	radeon_hpd_init(rdev);
+
+	ttm_bo_unlock_delayed_workqueue(&rdev->mman.bdev, resched);
+
+	rdev->in_reset = true;
+	rdev->needs_reset = false;
+
+	downgrade_write(&rdev->exclusive_lock);
 
-	radeon_pm_resume(rdev);
 	drm_helper_resume_force_mode(rdev->ddev);
 
-	ttm_bo_unlock_delayed_workqueue(&rdev->mman.bdev, resched);
-	if (r) {
+	/* set the power state here in case we are a PX system or headless */
+	if ((rdev->pm.pm_method == PM_METHOD_DPM) && rdev->pm.dpm_enabled)
+		radeon_pm_compute_clocks(rdev);
+
+	if (!r) {
+		r = radeon_ib_ring_tests(rdev);
+		if (r && saved)
+			r = -EAGAIN;
+	} else {
 		/* bad news, how to tell it to userspace ? */
 		dev_info(rdev->dev, "GPU reset failed\n");
 	}
 
-	up_write(&rdev->exclusive_lock);
+	rdev->needs_reset = r == -EAGAIN;
+	rdev->in_reset = false;
+
+	up_read(&rdev->exclusive_lock);
 	return r;
 }
 
diff --git a/drivers/gpu/drm/radeon/radeon_display.c b/drivers/gpu/drm/radeon/radeon_display.c
index 3fdf87318069..00ead8c2758a 100644
--- a/drivers/gpu/drm/radeon/radeon_display.c
+++ b/drivers/gpu/drm/radeon/radeon_display.c
@@ -402,12 +402,21 @@ static void radeon_flip_work_func(struct work_struct *__work)
 
         down_read(&rdev->exclusive_lock);
 	if (work->fence) {
-		r = radeon_fence_wait(work->fence, false);
-		if (r == -EDEADLK) {
-			up_read(&rdev->exclusive_lock);
-			r = radeon_gpu_reset(rdev);
-			down_read(&rdev->exclusive_lock);
-		}
+		struct radeon_fence *fence;
+
+		fence = to_radeon_fence(work->fence);
+		if (fence && fence->rdev == rdev) {
+			r = radeon_fence_wait(fence, false);
+			if (r == -EDEADLK) {
+				up_read(&rdev->exclusive_lock);
+				do {
+					r = radeon_gpu_reset(rdev);
+				} while (r == -EAGAIN);
+				down_read(&rdev->exclusive_lock);
+			}
+		} else
+			r = fence_wait(work->fence, false);
+
 		if (r)
 			DRM_ERROR("failed to wait on page flip fence (%d)!\n", r);
 
@@ -416,7 +425,8 @@ static void radeon_flip_work_func(struct work_struct *__work)
 		 * confused about which BO the CRTC is scanning out
 		 */
 
-		radeon_fence_unref(&work->fence);
+		fence_put(work->fence);
+		work->fence = NULL;
 	}
 
 	/* We borrow the event spin lock for protecting flip_status */
@@ -474,11 +484,6 @@ static int radeon_crtc_page_flip(struct drm_crtc *crtc,
 	obj = new_radeon_fb->obj;
 	new_rbo = gem_to_radeon_bo(obj);
 
-	spin_lock(&new_rbo->tbo.bdev->fence_lock);
-	if (new_rbo->tbo.sync_obj)
-		work->fence = radeon_fence_ref(new_rbo->tbo.sync_obj);
-	spin_unlock(&new_rbo->tbo.bdev->fence_lock);
-
 	/* pin the new buffer */
 	DRM_DEBUG_DRIVER("flip-ioctl() cur_rbo = %p, new_rbo = %p\n",
 			 work->old_rbo, new_rbo);
@@ -497,6 +502,7 @@ static int radeon_crtc_page_flip(struct drm_crtc *crtc,
 		DRM_ERROR("failed to pin new rbo buffer before flip\n");
 		goto cleanup;
 	}
+	work->fence = fence_get(reservation_object_get_excl(new_rbo->tbo.resv));
 	radeon_bo_get_tiling_flags(new_rbo, &tiling_flags, NULL);
 	radeon_bo_unreserve(new_rbo);
 
@@ -578,9 +584,8 @@ pflip_cleanup:
 
 cleanup:
 	drm_gem_object_unreference_unlocked(&work->old_rbo->gem_base);
-	radeon_fence_unref(&work->fence);
+	fence_put(work->fence);
 	kfree(work);
-
 	return r;
 }
 
@@ -1917,7 +1922,7 @@ int radeon_get_crtc_scanoutpos(struct drm_device *dev, int crtc, unsigned int fl
 
 	/* In vblank? */
 	if (in_vbl)
-		ret |= DRM_SCANOUTPOS_INVBL;
+		ret |= DRM_SCANOUTPOS_IN_VBLANK;
 
 	/* Is vpos outside nominal vblank area, but less than
 	 * 1/100 of a frame height away from start of vblank?
diff --git a/drivers/gpu/drm/radeon/radeon_drv.c b/drivers/gpu/drm/radeon/radeon_drv.c
index 092d067f93e1..dcffa30ee2db 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.c
+++ b/drivers/gpu/drm/radeon/radeon_drv.c
@@ -38,6 +38,8 @@
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
 #include <linux/vga_switcheroo.h>
+#include <drm/drm_gem.h>
+
 #include "drm_crtc_helper.h"
 /*
  * KMS wrapper.
@@ -83,7 +85,7 @@
  *            CIK: 1D and linear tiling modes contain valid PIPE_CONFIG
  *   2.39.0 - Add INFO query for number of active CUs
  *   2.40.0 - Add RADEON_GEM_GTT_WC/UC, flush HDP cache before submitting
- *            CS to GPU
+ *            CS to GPU on >= r600
  */
 #define KMS_DRIVER_MAJOR	2
 #define KMS_DRIVER_MINOR	40
@@ -114,6 +116,9 @@ int radeon_gem_object_open(struct drm_gem_object *obj,
 				struct drm_file *file_priv);
 void radeon_gem_object_close(struct drm_gem_object *obj,
 				struct drm_file *file_priv);
+struct dma_buf *radeon_gem_prime_export(struct drm_device *dev,
+					struct drm_gem_object *gobj,
+					int flags);
 extern int radeon_get_crtc_scanoutpos(struct drm_device *dev, int crtc,
 				      unsigned int flags,
 				      int *vpos, int *hpos, ktime_t *stime,
@@ -130,7 +135,7 @@ int radeon_mode_dumb_create(struct drm_file *file_priv,
 			    struct drm_mode_create_dumb *args);
 struct sg_table *radeon_gem_prime_get_sg_table(struct drm_gem_object *obj);
 struct drm_gem_object *radeon_gem_prime_import_sg_table(struct drm_device *dev,
-							size_t size,
+							struct dma_buf_attachment *,
 							struct sg_table *sg);
 int radeon_gem_prime_pin(struct drm_gem_object *obj);
 void radeon_gem_prime_unpin(struct drm_gem_object *obj);
@@ -180,6 +185,8 @@ int radeon_vm_size = 8;
 int radeon_vm_block_size = -1;
 int radeon_deep_color = 0;
 int radeon_use_pflipirq = 2;
+int radeon_bapm = -1;
+int radeon_backlight = -1;
 
 MODULE_PARM_DESC(no_wb, "Disable AGP writeback for scratch registers");
 module_param_named(no_wb, radeon_no_wb, int, 0444);
@@ -259,6 +266,12 @@ module_param_named(deep_color, radeon_deep_color, int, 0444);
 MODULE_PARM_DESC(use_pflipirq, "Pflip irqs for pageflip completion (0 = disable, 1 = as fallback, 2 = exclusive (default))");
 module_param_named(use_pflipirq, radeon_use_pflipirq, int, 0444);
 
+MODULE_PARM_DESC(bapm, "BAPM support (1 = enable, 0 = disable, -1 = auto)");
+module_param_named(bapm, radeon_bapm, int, 0444);
+
+MODULE_PARM_DESC(backlight, "backlight support (1 = enable, 0 = disable, -1 = auto)");
+module_param_named(backlight, radeon_backlight, int, 0444);
+
 static struct pci_device_id pciidlist[] = {
 	radeon_PCI_IDS
 };
@@ -301,7 +314,7 @@ static const struct file_operations radeon_driver_old_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 	.read = drm_read,
 #ifdef CONFIG_COMPAT
@@ -321,6 +334,7 @@ static struct drm_driver driver_old = {
 	.preclose = radeon_driver_preclose,
 	.postclose = radeon_driver_postclose,
 	.lastclose = radeon_driver_lastclose,
+	.set_busid = drm_pci_set_busid,
 	.unload = radeon_driver_unload,
 	.suspend = radeon_suspend,
 	.resume = radeon_resume,
@@ -436,6 +450,7 @@ static int radeon_pmops_runtime_suspend(struct device *dev)
 	ret = radeon_suspend_kms(drm_dev, false, false);
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
+	pci_ignore_hotplug(pdev);
 	pci_set_power_state(pdev, PCI_D3cold);
 	drm_dev->switch_power_state = DRM_SWITCH_POWER_DYNAMIC_OFF;
 
@@ -544,6 +559,7 @@ static struct drm_driver kms_driver = {
 	.preclose = radeon_driver_preclose_kms,
 	.postclose = radeon_driver_postclose_kms,
 	.lastclose = radeon_driver_lastclose_kms,
+	.set_busid = drm_pci_set_busid,
 	.unload = radeon_driver_unload_kms,
 	.get_vblank_counter = radeon_get_vblank_counter_kms,
 	.enable_vblank = radeon_enable_vblank_kms,
@@ -569,7 +585,7 @@ static struct drm_driver kms_driver = {
 
 	.prime_handle_to_fd = drm_gem_prime_handle_to_fd,
 	.prime_fd_to_handle = drm_gem_prime_fd_to_handle,
-	.gem_prime_export = drm_gem_prime_export,
+	.gem_prime_export = radeon_gem_prime_export,
 	.gem_prime_import = drm_gem_prime_import,
 	.gem_prime_pin = radeon_gem_prime_pin,
 	.gem_prime_unpin = radeon_gem_prime_unpin,
diff --git a/drivers/gpu/drm/radeon/radeon_drv.h b/drivers/gpu/drm/radeon/radeon_drv.h
index dafd812e4571..46bd3938282c 100644
--- a/drivers/gpu/drm/radeon/radeon_drv.h
+++ b/drivers/gpu/drm/radeon/radeon_drv.h
@@ -33,7 +33,9 @@
 
 #include <linux/firmware.h>
 #include <linux/platform_device.h>
+#include <drm/drm_legacy.h>
 
+#include <drm/ati_pcigart.h>
 #include "radeon_family.h"
 
 /* General customization:
diff --git a/drivers/gpu/drm/radeon/radeon_encoders.c b/drivers/gpu/drm/radeon/radeon_encoders.c
index 3c2094c25b53..9a19e52cc655 100644
--- a/drivers/gpu/drm/radeon/radeon_encoders.c
+++ b/drivers/gpu/drm/radeon/radeon_encoders.c
@@ -158,10 +158,43 @@ radeon_get_encoder_enum(struct drm_device *dev, uint32_t supported_device, uint8
 	return ret;
 }
 
+static void radeon_encoder_add_backlight(struct radeon_encoder *radeon_encoder,
+					 struct drm_connector *connector)
+{
+	struct drm_device *dev = radeon_encoder->base.dev;
+	struct radeon_device *rdev = dev->dev_private;
+	bool use_bl = false;
+
+	if (!(radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)))
+		return;
+
+	if (radeon_backlight == 0) {
+		return;
+	} else if (radeon_backlight == 1) {
+		use_bl = true;
+	} else if (radeon_backlight == -1) {
+		/* Quirks */
+		/* Amilo Xi 2550 only works with acpi bl */
+		if ((rdev->pdev->device == 0x9583) &&
+		    (rdev->pdev->subsystem_vendor == 0x1734) &&
+		    (rdev->pdev->subsystem_device == 0x1107))
+			use_bl = false;
+		else
+			use_bl = true;
+	}
+
+	if (use_bl) {
+		if (rdev->is_atom_bios)
+			radeon_atom_backlight_init(radeon_encoder, connector);
+		else
+			radeon_legacy_backlight_init(radeon_encoder, connector);
+		rdev->mode_info.bl_encoder = radeon_encoder;
+	}
+}
+
 void
 radeon_link_encoder_connector(struct drm_device *dev)
 {
-	struct radeon_device *rdev = dev->dev_private;
 	struct drm_connector *connector;
 	struct radeon_connector *radeon_connector;
 	struct drm_encoder *encoder;
@@ -174,13 +207,8 @@ radeon_link_encoder_connector(struct drm_device *dev)
 			radeon_encoder = to_radeon_encoder(encoder);
 			if (radeon_encoder->devices & radeon_connector->devices) {
 				drm_mode_connector_attach_encoder(connector, encoder);
-				if (radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT)) {
-					if (rdev->is_atom_bios)
-						radeon_atom_backlight_init(radeon_encoder, connector);
-					else
-						radeon_legacy_backlight_init(radeon_encoder, connector);
-					rdev->mode_info.bl_encoder = radeon_encoder;
-				}
+				if (radeon_encoder->devices & (ATOM_DEVICE_LCD_SUPPORT))
+					radeon_encoder_add_backlight(radeon_encoder, connector);
 			}
 		}
 	}
@@ -382,3 +410,24 @@ bool radeon_dig_monitor_is_duallink(struct drm_encoder *encoder,
 	}
 }
 
+bool radeon_encoder_is_digital(struct drm_encoder *encoder)
+{
+	struct radeon_encoder *radeon_encoder = to_radeon_encoder(encoder);
+	switch (radeon_encoder->encoder_id) {
+	case ENCODER_OBJECT_ID_INTERNAL_LVDS:
+	case ENCODER_OBJECT_ID_INTERNAL_TMDS1:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_TMDS1:
+	case ENCODER_OBJECT_ID_INTERNAL_LVTM1:
+	case ENCODER_OBJECT_ID_INTERNAL_DVO1:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_DVO1:
+	case ENCODER_OBJECT_ID_INTERNAL_DDI:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY:
+	case ENCODER_OBJECT_ID_INTERNAL_KLDSCP_LVTMA:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY1:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY2:
+	case ENCODER_OBJECT_ID_INTERNAL_UNIPHY3:
+		return true;
+	default:
+		return false;
+	}
+}
diff --git a/drivers/gpu/drm/radeon/radeon_fb.c b/drivers/gpu/drm/radeon/radeon_fb.c
index 94b0f2aa3d7c..0ea1db83d573 100644
--- a/drivers/gpu/drm/radeon/radeon_fb.c
+++ b/drivers/gpu/drm/radeon/radeon_fb.c
@@ -189,7 +189,8 @@ out_unref:
 static int radeonfb_create(struct drm_fb_helper *helper,
 			   struct drm_fb_helper_surface_size *sizes)
 {
-	struct radeon_fbdev *rfbdev = (struct radeon_fbdev *)helper;
+	struct radeon_fbdev *rfbdev =
+		container_of(helper, struct radeon_fbdev, helper);
 	struct radeon_device *rdev = rfbdev->rdev;
 	struct fb_info *info;
 	struct drm_framebuffer *fb = NULL;
diff --git a/drivers/gpu/drm/radeon/radeon_fence.c b/drivers/gpu/drm/radeon/radeon_fence.c
index 913787085dfa..995167025282 100644
--- a/drivers/gpu/drm/radeon/radeon_fence.c
+++ b/drivers/gpu/drm/radeon/radeon_fence.c
@@ -98,6 +98,25 @@ static u32 radeon_fence_read(struct radeon_device *rdev, int ring)
 }
 
 /**
+ * radeon_fence_schedule_check - schedule lockup check
+ *
+ * @rdev: radeon_device pointer
+ * @ring: ring index we should work with
+ *
+ * Queues a delayed work item to check for lockups.
+ */
+static void radeon_fence_schedule_check(struct radeon_device *rdev, int ring)
+{
+	/*
+	 * Do not reset the timer here with mod_delayed_work,
+	 * this can livelock in an interaction with TTM delayed destroy.
+	 */
+	queue_delayed_work(system_power_efficient_wq,
+			   &rdev->fence_drv[ring].lockup_work,
+			   RADEON_FENCE_JIFFIES_TIMEOUT);
+}
+
+/**
  * radeon_fence_emit - emit a fence on the requested ring
  *
  * @rdev: radeon_device pointer
@@ -111,30 +130,70 @@ int radeon_fence_emit(struct radeon_device *rdev,
 		      struct radeon_fence **fence,
 		      int ring)
 {
+	u64 seq = ++rdev->fence_drv[ring].sync_seq[ring];
+
 	/* we are protected by the ring emission mutex */
 	*fence = kmalloc(sizeof(struct radeon_fence), GFP_KERNEL);
 	if ((*fence) == NULL) {
 		return -ENOMEM;
 	}
-	kref_init(&((*fence)->kref));
 	(*fence)->rdev = rdev;
-	(*fence)->seq = ++rdev->fence_drv[ring].sync_seq[ring];
+	(*fence)->seq = seq;
 	(*fence)->ring = ring;
+	fence_init(&(*fence)->base, &radeon_fence_ops,
+		   &rdev->fence_queue.lock, rdev->fence_context + ring, seq);
 	radeon_fence_ring_emit(rdev, ring, *fence);
 	trace_radeon_fence_emit(rdev->ddev, ring, (*fence)->seq);
+	radeon_fence_schedule_check(rdev, ring);
 	return 0;
 }
 
 /**
- * radeon_fence_process - process a fence
+ * radeon_fence_check_signaled - callback from fence_queue
+ *
+ * this function is called with fence_queue lock held, which is also used
+ * for the fence locking itself, so unlocked variants are used for
+ * fence_signal, and remove_wait_queue.
+ */
+static int radeon_fence_check_signaled(wait_queue_t *wait, unsigned mode, int flags, void *key)
+{
+	struct radeon_fence *fence;
+	u64 seq;
+
+	fence = container_of(wait, struct radeon_fence, fence_wake);
+
+	/*
+	 * We cannot use radeon_fence_process here because we're already
+	 * in the waitqueue, in a call from wake_up_all.
+	 */
+	seq = atomic64_read(&fence->rdev->fence_drv[fence->ring].last_seq);
+	if (seq >= fence->seq) {
+		int ret = fence_signal_locked(&fence->base);
+
+		if (!ret)
+			FENCE_TRACE(&fence->base, "signaled from irq context\n");
+		else
+			FENCE_TRACE(&fence->base, "was already signaled\n");
+
+		radeon_irq_kms_sw_irq_put(fence->rdev, fence->ring);
+		__remove_wait_queue(&fence->rdev->fence_queue, &fence->fence_wake);
+		fence_put(&fence->base);
+	} else
+		FENCE_TRACE(&fence->base, "pending\n");
+	return 0;
+}
+
+/**
+ * radeon_fence_activity - check for fence activity
  *
  * @rdev: radeon_device pointer
  * @ring: ring index the fence is associated with
  *
- * Checks the current fence value and wakes the fence queue
- * if the sequence number has increased (all asics).
+ * Checks the current fence value and calculates the last
+ * signalled fence value. Returns true if activity occured
+ * on the ring, and the fence_queue should be waken up.
  */
-void radeon_fence_process(struct radeon_device *rdev, int ring)
+static bool radeon_fence_activity(struct radeon_device *rdev, int ring)
 {
 	uint64_t seq, last_seq, last_emitted;
 	unsigned count_loop = 0;
@@ -190,23 +249,77 @@ void radeon_fence_process(struct radeon_device *rdev, int ring)
 		}
 	} while (atomic64_xchg(&rdev->fence_drv[ring].last_seq, seq) > seq);
 
-	if (wake)
-		wake_up_all(&rdev->fence_queue);
+	if (seq < last_emitted)
+		radeon_fence_schedule_check(rdev, ring);
+
+	return wake;
 }
 
 /**
- * radeon_fence_destroy - destroy a fence
+ * radeon_fence_check_lockup - check for hardware lockup
  *
- * @kref: fence kref
+ * @work: delayed work item
  *
- * Frees the fence object (all asics).
+ * Checks for fence activity and if there is none probe
+ * the hardware if a lockup occured.
  */
-static void radeon_fence_destroy(struct kref *kref)
+static void radeon_fence_check_lockup(struct work_struct *work)
 {
-	struct radeon_fence *fence;
+	struct radeon_fence_driver *fence_drv;
+	struct radeon_device *rdev;
+	int ring;
+
+	fence_drv = container_of(work, struct radeon_fence_driver,
+				 lockup_work.work);
+	rdev = fence_drv->rdev;
+	ring = fence_drv - &rdev->fence_drv[0];
+
+	if (!down_read_trylock(&rdev->exclusive_lock)) {
+		/* just reschedule the check if a reset is going on */
+		radeon_fence_schedule_check(rdev, ring);
+		return;
+	}
+
+	if (fence_drv->delayed_irq && rdev->ddev->irq_enabled) {
+		unsigned long irqflags;
+
+		fence_drv->delayed_irq = false;
+		spin_lock_irqsave(&rdev->irq.lock, irqflags);
+		radeon_irq_set(rdev);
+		spin_unlock_irqrestore(&rdev->irq.lock, irqflags);
+	}
+
+	if (radeon_fence_activity(rdev, ring))
+		wake_up_all(&rdev->fence_queue);
 
-	fence = container_of(kref, struct radeon_fence, kref);
-	kfree(fence);
+	else if (radeon_ring_is_lockup(rdev, ring, &rdev->ring[ring])) {
+
+		/* good news we believe it's a lockup */
+		dev_warn(rdev->dev, "GPU lockup (current fence id "
+			 "0x%016llx last fence id 0x%016llx on ring %d)\n",
+			 (uint64_t)atomic64_read(&fence_drv->last_seq),
+			 fence_drv->sync_seq[ring], ring);
+
+		/* remember that we need an reset */
+		rdev->needs_reset = true;
+		wake_up_all(&rdev->fence_queue);
+	}
+	up_read(&rdev->exclusive_lock);
+}
+
+/**
+ * radeon_fence_process - process a fence
+ *
+ * @rdev: radeon_device pointer
+ * @ring: ring index the fence is associated with
+ *
+ * Checks the current fence value and wakes the fence queue
+ * if the sequence number has increased (all asics).
+ */
+void radeon_fence_process(struct radeon_device *rdev, int ring)
+{
+	if (radeon_fence_activity(rdev, ring))
+		wake_up_all(&rdev->fence_queue);
 }
 
 /**
@@ -237,6 +350,75 @@ static bool radeon_fence_seq_signaled(struct radeon_device *rdev,
 	return false;
 }
 
+static bool radeon_fence_is_signaled(struct fence *f)
+{
+	struct radeon_fence *fence = to_radeon_fence(f);
+	struct radeon_device *rdev = fence->rdev;
+	unsigned ring = fence->ring;
+	u64 seq = fence->seq;
+
+	if (atomic64_read(&rdev->fence_drv[ring].last_seq) >= seq) {
+		return true;
+	}
+
+	if (down_read_trylock(&rdev->exclusive_lock)) {
+		radeon_fence_process(rdev, ring);
+		up_read(&rdev->exclusive_lock);
+
+		if (atomic64_read(&rdev->fence_drv[ring].last_seq) >= seq) {
+			return true;
+		}
+	}
+	return false;
+}
+
+/**
+ * radeon_fence_enable_signaling - enable signalling on fence
+ * @fence: fence
+ *
+ * This function is called with fence_queue lock held, and adds a callback
+ * to fence_queue that checks if this fence is signaled, and if so it
+ * signals the fence and removes itself.
+ */
+static bool radeon_fence_enable_signaling(struct fence *f)
+{
+	struct radeon_fence *fence = to_radeon_fence(f);
+	struct radeon_device *rdev = fence->rdev;
+
+	if (atomic64_read(&rdev->fence_drv[fence->ring].last_seq) >= fence->seq)
+		return false;
+
+	if (down_read_trylock(&rdev->exclusive_lock)) {
+		radeon_irq_kms_sw_irq_get(rdev, fence->ring);
+
+		if (radeon_fence_activity(rdev, fence->ring))
+			wake_up_all_locked(&rdev->fence_queue);
+
+		/* did fence get signaled after we enabled the sw irq? */
+		if (atomic64_read(&rdev->fence_drv[fence->ring].last_seq) >= fence->seq) {
+			radeon_irq_kms_sw_irq_put(rdev, fence->ring);
+			up_read(&rdev->exclusive_lock);
+			return false;
+		}
+
+		up_read(&rdev->exclusive_lock);
+	} else {
+		/* we're probably in a lockup, lets not fiddle too much */
+		if (radeon_irq_kms_sw_irq_get_delayed(rdev, fence->ring))
+			rdev->fence_drv[fence->ring].delayed_irq = true;
+		radeon_fence_schedule_check(rdev, fence->ring);
+	}
+
+	fence->fence_wake.flags = 0;
+	fence->fence_wake.private = NULL;
+	fence->fence_wake.func = radeon_fence_check_signaled;
+	__add_wait_queue(&rdev->fence_queue, &fence->fence_wake);
+	fence_get(f);
+
+	FENCE_TRACE(&fence->base, "armed on ring %i!\n", fence->ring);
+	return true;
+}
+
 /**
  * radeon_fence_signaled - check if a fence has signaled
  *
@@ -247,14 +429,15 @@ static bool radeon_fence_seq_signaled(struct radeon_device *rdev,
  */
 bool radeon_fence_signaled(struct radeon_fence *fence)
 {
-	if (!fence) {
+	if (!fence)
 		return true;
-	}
-	if (fence->seq == RADEON_FENCE_SIGNALED_SEQ) {
-		return true;
-	}
+
 	if (radeon_fence_seq_signaled(fence->rdev, fence->seq, fence->ring)) {
-		fence->seq = RADEON_FENCE_SIGNALED_SEQ;
+		int ret;
+
+		ret = fence_signal(&fence->base);
+		if (!ret)
+			FENCE_TRACE(&fence->base, "signaled from radeon_fence_signaled\n");
 		return true;
 	}
 	return false;
@@ -283,110 +466,70 @@ static bool radeon_fence_any_seq_signaled(struct radeon_device *rdev, u64 *seq)
 }
 
 /**
- * radeon_fence_wait_seq - wait for a specific sequence numbers
+ * radeon_fence_wait_seq_timeout - wait for a specific sequence numbers
  *
  * @rdev: radeon device pointer
  * @target_seq: sequence number(s) we want to wait for
  * @intr: use interruptable sleep
+ * @timeout: maximum time to wait, or MAX_SCHEDULE_TIMEOUT for infinite wait
  *
  * Wait for the requested sequence number(s) to be written by any ring
  * (all asics).  Sequnce number array is indexed by ring id.
  * @intr selects whether to use interruptable (true) or non-interruptable
  * (false) sleep when waiting for the sequence number.  Helper function
  * for radeon_fence_wait_*().
- * Returns 0 if the sequence number has passed, error for all other cases.
+ * Returns remaining time if the sequence number has passed, 0 when
+ * the wait timeout, or an error for all other cases.
  * -EDEADLK is returned when a GPU lockup has been detected.
  */
-static int radeon_fence_wait_seq(struct radeon_device *rdev, u64 *target_seq,
-				 bool intr)
+static long radeon_fence_wait_seq_timeout(struct radeon_device *rdev,
+					  u64 *target_seq, bool intr,
+					  long timeout)
 {
-	uint64_t last_seq[RADEON_NUM_RINGS];
-	bool signaled;
-	int i, r;
-
-	while (!radeon_fence_any_seq_signaled(rdev, target_seq)) {
+	long r;
+	int i;
 
-		/* Save current sequence values, used to check for GPU lockups */
-		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-			if (!target_seq[i])
-				continue;
+	if (radeon_fence_any_seq_signaled(rdev, target_seq))
+		return timeout;
 
-			last_seq[i] = atomic64_read(&rdev->fence_drv[i].last_seq);
-			trace_radeon_fence_wait_begin(rdev->ddev, i, target_seq[i]);
-			radeon_irq_kms_sw_irq_get(rdev, i);
-		}
+	/* enable IRQs and tracing */
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (!target_seq[i])
+			continue;
 
-		if (intr) {
-			r = wait_event_interruptible_timeout(rdev->fence_queue, (
-				(signaled = radeon_fence_any_seq_signaled(rdev, target_seq))
-				 || rdev->needs_reset), RADEON_FENCE_JIFFIES_TIMEOUT);
-		} else {
-			r = wait_event_timeout(rdev->fence_queue, (
-				(signaled = radeon_fence_any_seq_signaled(rdev, target_seq))
-				 || rdev->needs_reset), RADEON_FENCE_JIFFIES_TIMEOUT);
-		}
+		trace_radeon_fence_wait_begin(rdev->ddev, i, target_seq[i]);
+		radeon_irq_kms_sw_irq_get(rdev, i);
+	}
 
-		for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-			if (!target_seq[i])
-				continue;
+	if (intr) {
+		r = wait_event_interruptible_timeout(rdev->fence_queue, (
+			radeon_fence_any_seq_signaled(rdev, target_seq)
+			 || rdev->needs_reset), timeout);
+	} else {
+		r = wait_event_timeout(rdev->fence_queue, (
+			radeon_fence_any_seq_signaled(rdev, target_seq)
+			 || rdev->needs_reset), timeout);
+	}
 
-			radeon_irq_kms_sw_irq_put(rdev, i);
-			trace_radeon_fence_wait_end(rdev->ddev, i, target_seq[i]);
-		}
+	if (rdev->needs_reset)
+		r = -EDEADLK;
 
-		if (unlikely(r < 0))
-			return r;
+	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
+		if (!target_seq[i])
+			continue;
 
-		if (unlikely(!signaled)) {
-			if (rdev->needs_reset)
-				return -EDEADLK;
-
-			/* we were interrupted for some reason and fence
-			 * isn't signaled yet, resume waiting */
-			if (r)
-				continue;
-
-			for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-				if (!target_seq[i])
-					continue;
-
-				if (last_seq[i] != atomic64_read(&rdev->fence_drv[i].last_seq))
-					break;
-			}
-
-			if (i != RADEON_NUM_RINGS)
-				continue;
-
-			for (i = 0; i < RADEON_NUM_RINGS; ++i) {
-				if (!target_seq[i])
-					continue;
-
-				if (radeon_ring_is_lockup(rdev, i, &rdev->ring[i]))
-					break;
-			}
-
-			if (i < RADEON_NUM_RINGS) {
-				/* good news we believe it's a lockup */
-				dev_warn(rdev->dev, "GPU lockup (waiting for "
-					 "0x%016llx last fence id 0x%016llx on"
-					 " ring %d)\n",
-					 target_seq[i], last_seq[i], i);
-
-				/* remember that we need an reset */
-				rdev->needs_reset = true;
-				wake_up_all(&rdev->fence_queue);
-				return -EDEADLK;
-			}
-		}
+		radeon_irq_kms_sw_irq_put(rdev, i);
+		trace_radeon_fence_wait_end(rdev->ddev, i, target_seq[i]);
 	}
-	return 0;
+
+	return r;
 }
 
 /**
  * radeon_fence_wait - wait for a fence to signal
  *
  * @fence: radeon fence object
- * @intr: use interruptable sleep
+ * @intr: use interruptible sleep
  *
  * Wait for the requested fence to signal (all asics).
  * @intr selects whether to use interruptable (true) or non-interruptable
@@ -396,22 +539,26 @@ static int radeon_fence_wait_seq(struct radeon_device *rdev, u64 *target_seq,
 int radeon_fence_wait(struct radeon_fence *fence, bool intr)
 {
 	uint64_t seq[RADEON_NUM_RINGS] = {};
-	int r;
+	long r;
 
-	if (fence == NULL) {
-		WARN(1, "Querying an invalid fence : %p !\n", fence);
-		return -EINVAL;
-	}
+	/*
+	 * This function should not be called on !radeon fences.
+	 * If this is the case, it would mean this function can
+	 * also be called on radeon fences belonging to another card.
+	 * exclusive_lock is not held in that case.
+	 */
+	if (WARN_ON_ONCE(!to_radeon_fence(&fence->base)))
+		return fence_wait(&fence->base, intr);
 
 	seq[fence->ring] = fence->seq;
-	if (seq[fence->ring] == RADEON_FENCE_SIGNALED_SEQ)
-		return 0;
-
-	r = radeon_fence_wait_seq(fence->rdev, seq, intr);
-	if (r)
+	r = radeon_fence_wait_seq_timeout(fence->rdev, seq, intr, MAX_SCHEDULE_TIMEOUT);
+	if (r < 0) {
 		return r;
+	}
 
-	fence->seq = RADEON_FENCE_SIGNALED_SEQ;
+	r = fence_signal(&fence->base);
+	if (!r)
+		FENCE_TRACE(&fence->base, "signaled from fence_wait\n");
 	return 0;
 }
 
@@ -434,7 +581,7 @@ int radeon_fence_wait_any(struct radeon_device *rdev,
 {
 	uint64_t seq[RADEON_NUM_RINGS];
 	unsigned i, num_rings = 0;
-	int r;
+	long r;
 
 	for (i = 0; i < RADEON_NUM_RINGS; ++i) {
 		seq[i] = 0;
@@ -445,18 +592,14 @@ int radeon_fence_wait_any(struct radeon_device *rdev,
 
 		seq[i] = fences[i]->seq;
 		++num_rings;
-
-		/* test if something was allready signaled */
-		if (seq[i] == RADEON_FENCE_SIGNALED_SEQ)
-			return 0;
 	}
 
 	/* nothing to wait for ? */
 	if (num_rings == 0)
 		return -ENOENT;
 
-	r = radeon_fence_wait_seq(rdev, seq, intr);
-	if (r) {
+	r = radeon_fence_wait_seq_timeout(rdev, seq, intr, MAX_SCHEDULE_TIMEOUT);
+	if (r < 0) {
 		return r;
 	}
 	return 0;
@@ -475,6 +618,7 @@ int radeon_fence_wait_any(struct radeon_device *rdev,
 int radeon_fence_wait_next(struct radeon_device *rdev, int ring)
 {
 	uint64_t seq[RADEON_NUM_RINGS] = {};
+	long r;
 
 	seq[ring] = atomic64_read(&rdev->fence_drv[ring].last_seq) + 1ULL;
 	if (seq[ring] >= rdev->fence_drv[ring].sync_seq[ring]) {
@@ -482,7 +626,10 @@ int radeon_fence_wait_next(struct radeon_device *rdev, int ring)
 		   already the last emited fence */
 		return -ENOENT;
 	}
-	return radeon_fence_wait_seq(rdev, seq, false);
+	r = radeon_fence_wait_seq_timeout(rdev, seq, false, MAX_SCHEDULE_TIMEOUT);
+	if (r < 0)
+		return r;
+	return 0;
 }
 
 /**
@@ -498,18 +645,18 @@ int radeon_fence_wait_next(struct radeon_device *rdev, int ring)
 int radeon_fence_wait_empty(struct radeon_device *rdev, int ring)
 {
 	uint64_t seq[RADEON_NUM_RINGS] = {};
-	int r;
+	long r;
 
 	seq[ring] = rdev->fence_drv[ring].sync_seq[ring];
 	if (!seq[ring])
 		return 0;
 
-	r = radeon_fence_wait_seq(rdev, seq, false);
-	if (r) {
+	r = radeon_fence_wait_seq_timeout(rdev, seq, false, MAX_SCHEDULE_TIMEOUT);
+	if (r < 0) {
 		if (r == -EDEADLK)
 			return -EDEADLK;
 
-		dev_err(rdev->dev, "error waiting for ring[%d] to become idle (%d)\n",
+		dev_err(rdev->dev, "error waiting for ring[%d] to become idle (%ld)\n",
 			ring, r);
 	}
 	return 0;
@@ -525,7 +672,7 @@ int radeon_fence_wait_empty(struct radeon_device *rdev, int ring)
  */
 struct radeon_fence *radeon_fence_ref(struct radeon_fence *fence)
 {
-	kref_get(&fence->kref);
+	fence_get(&fence->base);
 	return fence;
 }
 
@@ -542,7 +689,7 @@ void radeon_fence_unref(struct radeon_fence **fence)
 
 	*fence = NULL;
 	if (tmp) {
-		kref_put(&tmp->kref, radeon_fence_destroy);
+		fence_put(&tmp->base);
 	}
 }
 
@@ -711,6 +858,9 @@ static void radeon_fence_driver_init_ring(struct radeon_device *rdev, int ring)
 		rdev->fence_drv[ring].sync_seq[i] = 0;
 	atomic64_set(&rdev->fence_drv[ring].last_seq, 0);
 	rdev->fence_drv[ring].initialized = false;
+	INIT_DELAYED_WORK(&rdev->fence_drv[ring].lockup_work,
+			  radeon_fence_check_lockup);
+	rdev->fence_drv[ring].rdev = rdev;
 }
 
 /**
@@ -758,8 +908,9 @@ void radeon_fence_driver_fini(struct radeon_device *rdev)
 		r = radeon_fence_wait_empty(rdev, ring);
 		if (r) {
 			/* no need to trigger GPU reset as we are unloading */
-			radeon_fence_driver_force_completion(rdev);
+			radeon_fence_driver_force_completion(rdev, ring);
 		}
+		cancel_delayed_work_sync(&rdev->fence_drv[ring].lockup_work);
 		wake_up_all(&rdev->fence_queue);
 		radeon_scratch_free(rdev, rdev->fence_drv[ring].scratch_reg);
 		rdev->fence_drv[ring].initialized = false;
@@ -771,18 +922,16 @@ void radeon_fence_driver_fini(struct radeon_device *rdev)
  * radeon_fence_driver_force_completion - force all fence waiter to complete
  *
  * @rdev: radeon device pointer
+ * @ring: the ring to complete
  *
  * In case of GPU reset failure make sure no process keep waiting on fence
  * that will never complete.
  */
-void radeon_fence_driver_force_completion(struct radeon_device *rdev)
+void radeon_fence_driver_force_completion(struct radeon_device *rdev, int ring)
 {
-	int ring;
-
-	for (ring = 0; ring < RADEON_NUM_RINGS; ring++) {
-		if (!rdev->fence_drv[ring].initialized)
-			continue;
+	if (rdev->fence_drv[ring].initialized) {
 		radeon_fence_write(rdev, rdev->fence_drv[ring].sync_seq[ring], ring);
+		cancel_delayed_work_sync(&rdev->fence_drv[ring].lockup_work);
 	}
 }
 
@@ -833,6 +982,7 @@ static int radeon_debugfs_gpu_reset(struct seq_file *m, void *data)
 	down_read(&rdev->exclusive_lock);
 	seq_printf(m, "%d\n", rdev->needs_reset);
 	rdev->needs_reset = true;
+	wake_up_all(&rdev->fence_queue);
 	up_read(&rdev->exclusive_lock);
 
 	return 0;
@@ -852,3 +1002,72 @@ int radeon_debugfs_fence_init(struct radeon_device *rdev)
 	return 0;
 #endif
 }
+
+static const char *radeon_fence_get_driver_name(struct fence *fence)
+{
+	return "radeon";
+}
+
+static const char *radeon_fence_get_timeline_name(struct fence *f)
+{
+	struct radeon_fence *fence = to_radeon_fence(f);
+	switch (fence->ring) {
+	case RADEON_RING_TYPE_GFX_INDEX: return "radeon.gfx";
+	case CAYMAN_RING_TYPE_CP1_INDEX: return "radeon.cp1";
+	case CAYMAN_RING_TYPE_CP2_INDEX: return "radeon.cp2";
+	case R600_RING_TYPE_DMA_INDEX: return "radeon.dma";
+	case CAYMAN_RING_TYPE_DMA1_INDEX: return "radeon.dma1";
+	case R600_RING_TYPE_UVD_INDEX: return "radeon.uvd";
+	case TN_RING_TYPE_VCE1_INDEX: return "radeon.vce1";
+	case TN_RING_TYPE_VCE2_INDEX: return "radeon.vce2";
+	default: WARN_ON_ONCE(1); return "radeon.unk";
+	}
+}
+
+static inline bool radeon_test_signaled(struct radeon_fence *fence)
+{
+	return test_bit(FENCE_FLAG_SIGNALED_BIT, &fence->base.flags);
+}
+
+static signed long radeon_fence_default_wait(struct fence *f, bool intr,
+					     signed long t)
+{
+	struct radeon_fence *fence = to_radeon_fence(f);
+	struct radeon_device *rdev = fence->rdev;
+	bool signaled;
+
+	fence_enable_sw_signaling(&fence->base);
+
+	/*
+	 * This function has to return -EDEADLK, but cannot hold
+	 * exclusive_lock during the wait because some callers
+	 * may already hold it. This means checking needs_reset without
+	 * lock, and not fiddling with any gpu internals.
+	 *
+	 * The callback installed with fence_enable_sw_signaling will
+	 * run before our wait_event_*timeout call, so we will see
+	 * both the signaled fence and the changes to needs_reset.
+	 */
+
+	if (intr)
+		t = wait_event_interruptible_timeout(rdev->fence_queue,
+			((signaled = radeon_test_signaled(fence)) ||
+			 rdev->needs_reset), t);
+	else
+		t = wait_event_timeout(rdev->fence_queue,
+			((signaled = radeon_test_signaled(fence)) ||
+			 rdev->needs_reset), t);
+
+	if (t > 0 && !signaled)
+		return -EDEADLK;
+	return t;
+}
+
+const struct fence_ops radeon_fence_ops = {
+	.get_driver_name = radeon_fence_get_driver_name,
+	.get_timeline_name = radeon_fence_get_timeline_name,
+	.enable_signaling = radeon_fence_enable_signaling,
+	.signaled = radeon_fence_is_signaled,
+	.wait = radeon_fence_default_wait,
+	.release = NULL,
+};
diff --git a/drivers/gpu/drm/radeon/radeon_gart.c b/drivers/gpu/drm/radeon/radeon_gart.c
index a053a0779aac..84146d5901aa 100644
--- a/drivers/gpu/drm/radeon/radeon_gart.c
+++ b/drivers/gpu/drm/radeon/radeon_gart.c
@@ -128,7 +128,7 @@ int radeon_gart_table_vram_alloc(struct radeon_device *rdev)
 	if (rdev->gart.robj == NULL) {
 		r = radeon_bo_create(rdev, rdev->gart.table_size,
 				     PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-				     0, NULL, &rdev->gart.robj);
+				     0, NULL, NULL, &rdev->gart.robj);
 		if (r) {
 			return r;
 		}
diff --git a/drivers/gpu/drm/radeon/radeon_gem.c b/drivers/gpu/drm/radeon/radeon_gem.c
index bfd7e1b0ff3f..c194497aa586 100644
--- a/drivers/gpu/drm/radeon/radeon_gem.c
+++ b/drivers/gpu/drm/radeon/radeon_gem.c
@@ -67,7 +67,7 @@ int radeon_gem_object_create(struct radeon_device *rdev, unsigned long size,
 
 retry:
 	r = radeon_bo_create(rdev, size, alignment, kernel, initial_domain,
-			     flags, NULL, &robj);
+			     flags, NULL, NULL, &robj);
 	if (r) {
 		if (r != -ERESTARTSYS) {
 			if (initial_domain == RADEON_GEM_DOMAIN_VRAM) {
@@ -94,7 +94,7 @@ static int radeon_gem_set_domain(struct drm_gem_object *gobj,
 {
 	struct radeon_bo *robj;
 	uint32_t domain;
-	int r;
+	long r;
 
 	/* FIXME: reeimplement */
 	robj = gem_to_radeon_bo(gobj);
@@ -110,9 +110,12 @@ static int radeon_gem_set_domain(struct drm_gem_object *gobj,
 	}
 	if (domain == RADEON_GEM_DOMAIN_CPU) {
 		/* Asking for cpu access wait for object idle */
-		r = radeon_bo_wait(robj, NULL, false);
-		if (r) {
-			printk(KERN_ERR "Failed to wait for object !\n");
+		r = reservation_object_wait_timeout_rcu(robj->tbo.resv, true, true, 30 * HZ);
+		if (!r)
+			r = -EBUSY;
+
+		if (r < 0 && r != -EINTR) {
+			printk(KERN_ERR "Failed to wait for object: %li\n", r);
 			return r;
 		}
 	}
@@ -272,6 +275,94 @@ int radeon_gem_create_ioctl(struct drm_device *dev, void *data,
 	return 0;
 }
 
+int radeon_gem_userptr_ioctl(struct drm_device *dev, void *data,
+			     struct drm_file *filp)
+{
+	struct radeon_device *rdev = dev->dev_private;
+	struct drm_radeon_gem_userptr *args = data;
+	struct drm_gem_object *gobj;
+	struct radeon_bo *bo;
+	uint32_t handle;
+	int r;
+
+	if (offset_in_page(args->addr | args->size))
+		return -EINVAL;
+
+	/* reject unknown flag values */
+	if (args->flags & ~(RADEON_GEM_USERPTR_READONLY |
+	    RADEON_GEM_USERPTR_ANONONLY | RADEON_GEM_USERPTR_VALIDATE |
+	    RADEON_GEM_USERPTR_REGISTER))
+		return -EINVAL;
+
+	if (args->flags & RADEON_GEM_USERPTR_READONLY) {
+		/* readonly pages not tested on older hardware */
+		if (rdev->family < CHIP_R600)
+			return -EINVAL;
+
+	} else if (!(args->flags & RADEON_GEM_USERPTR_ANONONLY) ||
+		   !(args->flags & RADEON_GEM_USERPTR_REGISTER)) {
+
+		/* if we want to write to it we must require anonymous
+		   memory and install a MMU notifier */
+		return -EACCES;
+	}
+
+	down_read(&rdev->exclusive_lock);
+
+	/* create a gem object to contain this object in */
+	r = radeon_gem_object_create(rdev, args->size, 0,
+				     RADEON_GEM_DOMAIN_CPU, 0,
+				     false, &gobj);
+	if (r)
+		goto handle_lockup;
+
+	bo = gem_to_radeon_bo(gobj);
+	r = radeon_ttm_tt_set_userptr(bo->tbo.ttm, args->addr, args->flags);
+	if (r)
+		goto release_object;
+
+	if (args->flags & RADEON_GEM_USERPTR_REGISTER) {
+		r = radeon_mn_register(bo, args->addr);
+		if (r)
+			goto release_object;
+	}
+
+	if (args->flags & RADEON_GEM_USERPTR_VALIDATE) {
+		down_read(&current->mm->mmap_sem);
+		r = radeon_bo_reserve(bo, true);
+		if (r) {
+			up_read(&current->mm->mmap_sem);
+			goto release_object;
+		}
+
+		radeon_ttm_placement_from_domain(bo, RADEON_GEM_DOMAIN_GTT);
+		r = ttm_bo_validate(&bo->tbo, &bo->placement, true, false);
+		radeon_bo_unreserve(bo);
+		up_read(&current->mm->mmap_sem);
+		if (r)
+			goto release_object;
+	}
+
+	r = drm_gem_handle_create(filp, gobj, &handle);
+	/* drop reference from allocate - handle holds it now */
+	drm_gem_object_unreference_unlocked(gobj);
+	if (r)
+		goto handle_lockup;
+
+	args->handle = handle;
+	up_read(&rdev->exclusive_lock);
+	return 0;
+
+release_object:
+	drm_gem_object_unreference_unlocked(gobj);
+
+handle_lockup:
+	up_read(&rdev->exclusive_lock);
+	r = radeon_gem_handle_lockup(rdev, r);
+
+	return r;
+}
+
 int radeon_gem_set_domain_ioctl(struct drm_device *dev, void *data,
 				struct drm_file *filp)
 {
@@ -315,6 +406,10 @@ int radeon_mode_dumb_mmap(struct drm_file *filp,
 		return -ENOENT;
 	}
 	robj = gem_to_radeon_bo(gobj);
+	if (radeon_ttm_tt_has_userptr(robj->tbo.ttm)) {
+		drm_gem_object_unreference_unlocked(gobj);
+		return -EPERM;
+	}
 	*offset_p = radeon_bo_mmap_offset(robj);
 	drm_gem_object_unreference_unlocked(gobj);
 	return 0;
@@ -357,15 +452,22 @@ int radeon_gem_wait_idle_ioctl(struct drm_device *dev, void *data,
 	struct drm_radeon_gem_wait_idle *args = data;
 	struct drm_gem_object *gobj;
 	struct radeon_bo *robj;
-	int r;
+	int r = 0;
 	uint32_t cur_placement = 0;
+	long ret;
 
 	gobj = drm_gem_object_lookup(dev, filp, args->handle);
 	if (gobj == NULL) {
 		return -ENOENT;
 	}
 	robj = gem_to_radeon_bo(gobj);
-	r = radeon_bo_wait(robj, &cur_placement, false);
+
+	ret = reservation_object_wait_timeout_rcu(robj->tbo.resv, true, true, 30 * HZ);
+	if (ret == 0)
+		r = -EBUSY;
+	else if (ret < 0)
+		r = ret;
+
 	/* Flush HDP cache via MMIO if necessary */
 	if (rdev->asic->mmio_hdp_flush &&
 	    radeon_mem_type_to_domain(cur_placement) == RADEON_GEM_DOMAIN_VRAM)
@@ -532,6 +634,11 @@ int radeon_gem_op_ioctl(struct drm_device *dev, void *data,
 		return -ENOENT;
 	}
 	robj = gem_to_radeon_bo(gobj);
+
+	r = -EPERM;
+	if (radeon_ttm_tt_has_userptr(robj->tbo.ttm))
+		goto out;
+
 	r = radeon_bo_reserve(robj, false);
 	if (unlikely(r))
 		goto out;
diff --git a/drivers/gpu/drm/radeon/radeon_ib.c b/drivers/gpu/drm/radeon/radeon_ib.c
index 65b0c213488d..3f39fcca4d07 100644
--- a/drivers/gpu/drm/radeon/radeon_ib.c
+++ b/drivers/gpu/drm/radeon/radeon_ib.c
@@ -107,6 +107,7 @@ void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib)
  * @rdev: radeon_device pointer
  * @ib: IB object to schedule
  * @const_ib: Const IB to schedule (SI only)
+ * @hdp_flush: Whether or not to perform an HDP cache flush
  *
  * Schedule an IB on the associated ring (all asics).
  * Returns 0 on success, error on failure.
@@ -122,7 +123,7 @@ void radeon_ib_free(struct radeon_device *rdev, struct radeon_ib *ib)
  * to SI there was just a DE IB.
  */
 int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
-		       struct radeon_ib *const_ib)
+		       struct radeon_ib *const_ib, bool hdp_flush)
 {
 	struct radeon_ring *ring = &rdev->ring[ib->ring];
 	int r = 0;
@@ -144,7 +145,7 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
 	if (ib->vm) {
 		struct radeon_fence *vm_id_fence;
 		vm_id_fence = radeon_vm_grab_id(rdev, ib->vm, ib->ring);
-        	radeon_semaphore_sync_to(ib->semaphore, vm_id_fence);
+		radeon_semaphore_sync_fence(ib->semaphore, vm_id_fence);
 	}
 
 	/* sync with other rings */
@@ -176,7 +177,7 @@ int radeon_ib_schedule(struct radeon_device *rdev, struct radeon_ib *ib,
 	if (ib->vm)
 		radeon_vm_fence(rdev, ib->vm, ib->fence);
 
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, hdp_flush);
 	return 0;
 }
 
@@ -268,6 +269,7 @@ int radeon_ib_ring_tests(struct radeon_device *rdev)
 
 		r = radeon_ib_test(rdev, i, ring);
 		if (r) {
+			radeon_fence_driver_force_completion(rdev, i);
 			ring->ready = false;
 			rdev->needs_reset = false;
 
diff --git a/drivers/gpu/drm/radeon/radeon_irq_kms.c b/drivers/gpu/drm/radeon/radeon_irq_kms.c
index 16807afab362..7784911d78ef 100644
--- a/drivers/gpu/drm/radeon/radeon_irq_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_irq_kms.c
@@ -88,23 +88,6 @@ static void radeon_hotplug_work_func(struct work_struct *work)
 }
 
 /**
- * radeon_irq_reset_work_func - execute gpu reset
- *
- * @work: work struct
- *
- * Execute scheduled gpu reset (cayman+).
- * This function is called when the irq handler
- * thinks we need a gpu reset.
- */
-static void radeon_irq_reset_work_func(struct work_struct *work)
-{
-	struct radeon_device *rdev = container_of(work, struct radeon_device,
-						  reset_work);
-
-	radeon_gpu_reset(rdev);
-}
-
-/**
  * radeon_driver_irq_preinstall_kms - drm irq preinstall callback
  *
  * @dev: drm dev pointer
@@ -284,7 +267,6 @@ int radeon_irq_kms_init(struct radeon_device *rdev)
 
 	INIT_WORK(&rdev->hotplug_work, radeon_hotplug_work_func);
 	INIT_WORK(&rdev->audio_work, r600_audio_update_hdmi);
-	INIT_WORK(&rdev->reset_work, radeon_irq_reset_work_func);
 
 	rdev->irq.installed = true;
 	r = drm_irq_install(rdev->ddev, rdev->ddev->pdev->irq);
@@ -342,6 +324,21 @@ void radeon_irq_kms_sw_irq_get(struct radeon_device *rdev, int ring)
 }
 
 /**
+ * radeon_irq_kms_sw_irq_get_delayed - enable software interrupt
+ *
+ * @rdev: radeon device pointer
+ * @ring: ring whose interrupt you want to enable
+ *
+ * Enables the software interrupt for a specific ring (all asics).
+ * The software interrupt is generally used to signal a fence on
+ * a particular ring.
+ */
+bool radeon_irq_kms_sw_irq_get_delayed(struct radeon_device *rdev, int ring)
+{
+	return atomic_inc_return(&rdev->irq.ring_int[ring]) == 1;
+}
+
+/**
  * radeon_irq_kms_sw_irq_put - disable software interrupt
  *
  * @rdev: radeon device pointer
diff --git a/drivers/gpu/drm/radeon/radeon_kms.c b/drivers/gpu/drm/radeon/radeon_kms.c
index eb7164d07985..8309b11e674d 100644
--- a/drivers/gpu/drm/radeon/radeon_kms.c
+++ b/drivers/gpu/drm/radeon/radeon_kms.c
@@ -885,5 +885,6 @@ const struct drm_ioctl_desc radeon_ioctls_kms[] = {
 	DRM_IOCTL_DEF_DRV(RADEON_GEM_BUSY, radeon_gem_busy_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(RADEON_GEM_VA, radeon_gem_va_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
 	DRM_IOCTL_DEF_DRV(RADEON_GEM_OP, radeon_gem_op_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
+	DRM_IOCTL_DEF_DRV(RADEON_GEM_USERPTR, radeon_gem_userptr_ioctl, DRM_AUTH|DRM_UNLOCKED|DRM_RENDER_ALLOW),
 };
 int radeon_max_kms_ioctl = ARRAY_SIZE(radeon_ioctls_kms);
diff --git a/drivers/gpu/drm/radeon/radeon_mn.c b/drivers/gpu/drm/radeon/radeon_mn.c
new file mode 100644
index 000000000000..a69bd441dd2d
--- /dev/null
+++ b/drivers/gpu/drm/radeon/radeon_mn.c
@@ -0,0 +1,274 @@
+/*
+ * Copyright 2014 Advanced Micro Devices, Inc.
+ * All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the
+ * "Software"), to deal in the Software without restriction, including
+ * without limitation the rights to use, copy, modify, merge, publish,
+ * distribute, sub license, and/or sell copies of the Software, and to
+ * permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT. IN NO EVENT SHALL
+ * THE COPYRIGHT HOLDERS, AUTHORS AND/OR ITS SUPPLIERS BE LIABLE FOR ANY CLAIM,
+ * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
+ * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
+ * USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * The above copyright notice and this permission notice (including the
+ * next paragraph) shall be included in all copies or substantial portions
+ * of the Software.
+ *
+ */
+/*
+ * Authors:
+ *    Christian König <christian.koenig@amd.com>
+ */
+
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <linux/mmu_notifier.h>
+#include <drm/drmP.h>
+#include <drm/drm.h>
+
+#include "radeon.h"
+
+struct radeon_mn {
+	/* constant after initialisation */
+	struct radeon_device	*rdev;
+	struct mm_struct	*mm;
+	struct mmu_notifier	mn;
+
+	/* only used on destruction */
+	struct work_struct	work;
+
+	/* protected by rdev->mn_lock */
+	struct hlist_node	node;
+
+	/* objects protected by lock */
+	struct mutex		lock;
+	struct rb_root		objects;
+};
+
+/**
+ * radeon_mn_destroy - destroy the rmn
+ *
+ * @work: previously sheduled work item
+ *
+ * Lazy destroys the notifier from a work item
+ */
+static void radeon_mn_destroy(struct work_struct *work)
+{
+	struct radeon_mn *rmn = container_of(work, struct radeon_mn, work);
+	struct radeon_device *rdev = rmn->rdev;
+	struct radeon_bo *bo, *next;
+
+	mutex_lock(&rdev->mn_lock);
+	mutex_lock(&rmn->lock);
+	hash_del(&rmn->node);
+	rbtree_postorder_for_each_entry_safe(bo, next, &rmn->objects, mn_it.rb) {
+		interval_tree_remove(&bo->mn_it, &rmn->objects);
+		bo->mn = NULL;
+	}
+	mutex_unlock(&rmn->lock);
+	mutex_unlock(&rdev->mn_lock);
+	mmu_notifier_unregister(&rmn->mn, rmn->mm);
+	kfree(rmn);
+}
+
+/**
+ * radeon_mn_release - callback to notify about mm destruction
+ *
+ * @mn: our notifier
+ * @mn: the mm this callback is about
+ *
+ * Shedule a work item to lazy destroy our notifier.
+ */
+static void radeon_mn_release(struct mmu_notifier *mn,
+			      struct mm_struct *mm)
+{
+	struct radeon_mn *rmn = container_of(mn, struct radeon_mn, mn);
+	INIT_WORK(&rmn->work, radeon_mn_destroy);
+	schedule_work(&rmn->work);
+}
+
+/**
+ * radeon_mn_invalidate_range_start - callback to notify about mm change
+ *
+ * @mn: our notifier
+ * @mn: the mm this callback is about
+ * @start: start of updated range
+ * @end: end of updated range
+ *
+ * We block for all BOs between start and end to be idle and
+ * unmap them by move them into system domain again.
+ */
+static void radeon_mn_invalidate_range_start(struct mmu_notifier *mn,
+					     struct mm_struct *mm,
+					     unsigned long start,
+					     unsigned long end)
+{
+	struct radeon_mn *rmn = container_of(mn, struct radeon_mn, mn);
+	struct interval_tree_node *it;
+
+	/* notification is exclusive, but interval is inclusive */
+	end -= 1;
+
+	mutex_lock(&rmn->lock);
+
+	it = interval_tree_iter_first(&rmn->objects, start, end);
+	while (it) {
+		struct radeon_bo *bo;
+		struct fence *fence;
+		int r;
+
+		bo = container_of(it, struct radeon_bo, mn_it);
+		it = interval_tree_iter_next(it, start, end);
+
+		r = radeon_bo_reserve(bo, true);
+		if (r) {
+			DRM_ERROR("(%d) failed to reserve user bo\n", r);
+			continue;
+		}
+
+		fence = reservation_object_get_excl(bo->tbo.resv);
+		if (fence) {
+			r = radeon_fence_wait((struct radeon_fence *)fence, false);
+			if (r)
+				DRM_ERROR("(%d) failed to wait for user bo\n", r);
+		}
+
+		radeon_ttm_placement_from_domain(bo, RADEON_GEM_DOMAIN_CPU);
+		r = ttm_bo_validate(&bo->tbo, &bo->placement, false, false);
+		if (r)
+			DRM_ERROR("(%d) failed to validate user bo\n", r);
+
+		radeon_bo_unreserve(bo);
+	}
+	
+	mutex_unlock(&rmn->lock);
+}
+
+static const struct mmu_notifier_ops radeon_mn_ops = {
+	.release = radeon_mn_release,
+	.invalidate_range_start = radeon_mn_invalidate_range_start,
+};
+
+/**
+ * radeon_mn_get - create notifier context
+ *
+ * @rdev: radeon device pointer
+ *
+ * Creates a notifier context for current->mm.
+ */
+static struct radeon_mn *radeon_mn_get(struct radeon_device *rdev)
+{
+	struct mm_struct *mm = current->mm;
+	struct radeon_mn *rmn;
+	int r;
+
+	down_write(&mm->mmap_sem);
+	mutex_lock(&rdev->mn_lock);
+
+	hash_for_each_possible(rdev->mn_hash, rmn, node, (unsigned long)mm)
+		if (rmn->mm == mm)
+			goto release_locks;
+
+	rmn = kzalloc(sizeof(*rmn), GFP_KERNEL);
+	if (!rmn) {
+		rmn = ERR_PTR(-ENOMEM);
+		goto release_locks;
+	}
+
+	rmn->rdev = rdev;
+	rmn->mm = mm;
+	rmn->mn.ops = &radeon_mn_ops;
+	mutex_init(&rmn->lock);
+	rmn->objects = RB_ROOT;
+	
+	r = __mmu_notifier_register(&rmn->mn, mm);
+	if (r)
+		goto free_rmn;
+
+	hash_add(rdev->mn_hash, &rmn->node, (unsigned long)mm);
+
+release_locks:
+	mutex_unlock(&rdev->mn_lock);
+	up_write(&mm->mmap_sem);
+
+	return rmn;
+
+free_rmn:
+	mutex_unlock(&rdev->mn_lock);
+	up_write(&mm->mmap_sem);
+	kfree(rmn);
+
+	return ERR_PTR(r);
+}
+
+/**
+ * radeon_mn_register - register a BO for notifier updates
+ *
+ * @bo: radeon buffer object
+ * @addr: userptr addr we should monitor
+ *
+ * Registers an MMU notifier for the given BO at the specified address.
+ * Returns 0 on success, -ERRNO if anything goes wrong.
+ */
+int radeon_mn_register(struct radeon_bo *bo, unsigned long addr)
+{
+	unsigned long end = addr + radeon_bo_size(bo) - 1;
+	struct radeon_device *rdev = bo->rdev;
+	struct radeon_mn *rmn;
+	struct interval_tree_node *it;
+
+	rmn = radeon_mn_get(rdev);
+	if (IS_ERR(rmn))
+		return PTR_ERR(rmn);
+
+	mutex_lock(&rmn->lock);
+
+	it = interval_tree_iter_first(&rmn->objects, addr, end);
+	if (it) {
+		mutex_unlock(&rmn->lock);
+		return -EEXIST;
+	}
+
+	bo->mn = rmn;
+	bo->mn_it.start = addr;
+	bo->mn_it.last = end;
+	interval_tree_insert(&bo->mn_it, &rmn->objects);
+
+	mutex_unlock(&rmn->lock);
+
+	return 0;
+}
+
+/**
+ * radeon_mn_unregister - unregister a BO for notifier updates
+ *
+ * @bo: radeon buffer object
+ *
+ * Remove any registration of MMU notifier updates from the buffer object.
+ */
+void radeon_mn_unregister(struct radeon_bo *bo)
+{
+	struct radeon_device *rdev = bo->rdev;
+	struct radeon_mn *rmn;
+
+	mutex_lock(&rdev->mn_lock);
+	rmn = bo->mn;
+	if (rmn == NULL) {
+		mutex_unlock(&rdev->mn_lock);
+		return;
+	}
+
+	mutex_lock(&rmn->lock);
+	interval_tree_remove(&bo->mn_it, &rmn->objects);
+	bo->mn = NULL;
+	mutex_unlock(&rmn->lock);
+	mutex_unlock(&rdev->mn_lock);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_mode.h b/drivers/gpu/drm/radeon/radeon_mode.h
index e27608c29c11..04db2fdd8692 100644
--- a/drivers/gpu/drm/radeon/radeon_mode.h
+++ b/drivers/gpu/drm/radeon/radeon_mode.h
@@ -777,6 +777,7 @@ extern void atombios_digital_setup(struct drm_encoder *encoder, int action);
 extern int atombios_get_encoder_mode(struct drm_encoder *encoder);
 extern bool atombios_set_edp_panel_power(struct drm_connector *connector, int action);
 extern void radeon_encoder_set_active_device(struct drm_encoder *encoder);
+extern bool radeon_encoder_is_digital(struct drm_encoder *encoder);
 
 extern void radeon_crtc_load_lut(struct drm_crtc *crtc);
 extern int atombios_crtc_set_base(struct drm_crtc *crtc, int x, int y,
diff --git a/drivers/gpu/drm/radeon/radeon_object.c b/drivers/gpu/drm/radeon/radeon_object.c
index 480c87d8edc5..99a960a4f302 100644
--- a/drivers/gpu/drm/radeon/radeon_object.c
+++ b/drivers/gpu/drm/radeon/radeon_object.c
@@ -75,6 +75,7 @@ static void radeon_ttm_bo_destroy(struct ttm_buffer_object *tbo)
 	bo = container_of(tbo, struct radeon_bo, tbo);
 
 	radeon_update_memory_usage(bo, bo->tbo.mem.mem_type, -1);
+	radeon_mn_unregister(bo);
 
 	mutex_lock(&bo->rdev->gem.mutex);
 	list_del_init(&bo->list);
@@ -96,55 +97,80 @@ void radeon_ttm_placement_from_domain(struct radeon_bo *rbo, u32 domain)
 {
 	u32 c = 0, i;
 
-	rbo->placement.fpfn = 0;
-	rbo->placement.lpfn = 0;
 	rbo->placement.placement = rbo->placements;
 	rbo->placement.busy_placement = rbo->placements;
 	if (domain & RADEON_GEM_DOMAIN_VRAM)
-		rbo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED |
-					TTM_PL_FLAG_VRAM;
+		rbo->placements[c++].flags = TTM_PL_FLAG_WC |
+					     TTM_PL_FLAG_UNCACHED |
+					     TTM_PL_FLAG_VRAM;
+
 	if (domain & RADEON_GEM_DOMAIN_GTT) {
 		if (rbo->flags & RADEON_GEM_GTT_UC) {
-			rbo->placements[c++] = TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_TT;
+			rbo->placements[c++].flags = TTM_PL_FLAG_UNCACHED |
+				TTM_PL_FLAG_TT;
+
 		} else if ((rbo->flags & RADEON_GEM_GTT_WC) ||
 			   (rbo->rdev->flags & RADEON_IS_AGP)) {
-			rbo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED |
+			rbo->placements[c++].flags = TTM_PL_FLAG_WC |
+				TTM_PL_FLAG_UNCACHED |
 				TTM_PL_FLAG_TT;
 		} else {
-			rbo->placements[c++] = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_TT;
+			rbo->placements[c++].flags = TTM_PL_FLAG_CACHED |
+						     TTM_PL_FLAG_TT;
 		}
 	}
+
 	if (domain & RADEON_GEM_DOMAIN_CPU) {
 		if (rbo->flags & RADEON_GEM_GTT_UC) {
-			rbo->placements[c++] = TTM_PL_FLAG_UNCACHED | TTM_PL_FLAG_SYSTEM;
+			rbo->placements[c++].flags = TTM_PL_FLAG_UNCACHED |
+				TTM_PL_FLAG_SYSTEM;
+
 		} else if ((rbo->flags & RADEON_GEM_GTT_WC) ||
 		    rbo->rdev->flags & RADEON_IS_AGP) {
-			rbo->placements[c++] = TTM_PL_FLAG_WC | TTM_PL_FLAG_UNCACHED |
+			rbo->placements[c++].flags = TTM_PL_FLAG_WC |
+				TTM_PL_FLAG_UNCACHED |
 				TTM_PL_FLAG_SYSTEM;
 		} else {
-			rbo->placements[c++] = TTM_PL_FLAG_CACHED | TTM_PL_FLAG_SYSTEM;
+			rbo->placements[c++].flags = TTM_PL_FLAG_CACHED |
+						     TTM_PL_FLAG_SYSTEM;
 		}
 	}
 	if (!c)
-		rbo->placements[c++] = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
+		rbo->placements[c++].flags = TTM_PL_MASK_CACHING |
+					     TTM_PL_FLAG_SYSTEM;
+
 	rbo->placement.num_placement = c;
 	rbo->placement.num_busy_placement = c;
 
+	for (i = 0; i < c; ++i) {
+		rbo->placements[i].fpfn = 0;
+		if ((rbo->flags & RADEON_GEM_CPU_ACCESS) &&
+		    (rbo->placements[i].flags & TTM_PL_FLAG_VRAM))
+			rbo->placements[i].lpfn =
+				rbo->rdev->mc.visible_vram_size >> PAGE_SHIFT;
+		else
+			rbo->placements[i].lpfn = 0;
+	}
+
 	/*
 	 * Use two-ended allocation depending on the buffer size to
 	 * improve fragmentation quality.
 	 * 512kb was measured as the most optimal number.
 	 */
-	if (rbo->tbo.mem.size > 512 * 1024) {
+	if (!((rbo->flags & RADEON_GEM_CPU_ACCESS) &&
+	      (rbo->placements[i].flags & TTM_PL_FLAG_VRAM)) &&
+	    rbo->tbo.mem.size > 512 * 1024) {
 		for (i = 0; i < c; i++) {
-			rbo->placements[i] |= TTM_PL_FLAG_TOPDOWN;
+			rbo->placements[i].flags |= TTM_PL_FLAG_TOPDOWN;
 		}
 	}
 }
 
 int radeon_bo_create(struct radeon_device *rdev,
-		     unsigned long size, int byte_align, bool kernel, u32 domain,
-		     u32 flags, struct sg_table *sg, struct radeon_bo **bo_ptr)
+		     unsigned long size, int byte_align, bool kernel,
+		     u32 domain, u32 flags, struct sg_table *sg,
+		     struct reservation_object *resv,
+		     struct radeon_bo **bo_ptr)
 {
 	struct radeon_bo *bo;
 	enum ttm_bo_type type;
@@ -192,7 +218,7 @@ int radeon_bo_create(struct radeon_device *rdev,
 	down_read(&rdev->pm.mclk_lock);
 	r = ttm_bo_init(&rdev->mman.bdev, &bo->tbo, size, type,
 			&bo->placement, page_align, !kernel, NULL,
-			acc_size, sg, &radeon_ttm_bo_destroy);
+			acc_size, sg, resv, &radeon_ttm_bo_destroy);
 	up_read(&rdev->pm.mclk_lock);
 	if (unlikely(r != 0)) {
 		return r;
@@ -264,6 +290,9 @@ int radeon_bo_pin_restricted(struct radeon_bo *bo, u32 domain, u64 max_offset,
 {
 	int r, i;
 
+	if (radeon_ttm_tt_has_userptr(bo->tbo.ttm))
+		return -EPERM;
+
 	if (bo->pin_count) {
 		bo->pin_count++;
 		if (gpu_addr)
@@ -283,21 +312,19 @@ int radeon_bo_pin_restricted(struct radeon_bo *bo, u32 domain, u64 max_offset,
 		return 0;
 	}
 	radeon_ttm_placement_from_domain(bo, domain);
-	if (domain == RADEON_GEM_DOMAIN_VRAM) {
+	for (i = 0; i < bo->placement.num_placement; i++) {
 		/* force to pin into visible video ram */
-		bo->placement.lpfn = bo->rdev->mc.visible_vram_size >> PAGE_SHIFT;
-	}
-	if (max_offset) {
-		u64 lpfn = max_offset >> PAGE_SHIFT;
-
-		if (!bo->placement.lpfn)
-			bo->placement.lpfn = bo->rdev->mc.gtt_size >> PAGE_SHIFT;
+		if ((bo->placements[i].flags & TTM_PL_FLAG_VRAM) &&
+		    !(bo->flags & RADEON_GEM_NO_CPU_ACCESS) &&
+		    (!max_offset || max_offset > bo->rdev->mc.visible_vram_size))
+			bo->placements[i].lpfn =
+				bo->rdev->mc.visible_vram_size >> PAGE_SHIFT;
+		else
+			bo->placements[i].lpfn = max_offset >> PAGE_SHIFT;
 
-		if (lpfn < bo->placement.lpfn)
-			bo->placement.lpfn = lpfn;
+		bo->placements[i].flags |= TTM_PL_FLAG_NO_EVICT;
 	}
-	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] |= TTM_PL_FLAG_NO_EVICT;
+
 	r = ttm_bo_validate(&bo->tbo, &bo->placement, false, false);
 	if (likely(r == 0)) {
 		bo->pin_count = 1;
@@ -329,8 +356,10 @@ int radeon_bo_unpin(struct radeon_bo *bo)
 	bo->pin_count--;
 	if (bo->pin_count)
 		return 0;
-	for (i = 0; i < bo->placement.num_placement; i++)
-		bo->placements[i] &= ~TTM_PL_FLAG_NO_EVICT;
+	for (i = 0; i < bo->placement.num_placement; i++) {
+		bo->placements[i].lpfn = 0;
+		bo->placements[i].flags &= ~TTM_PL_FLAG_NO_EVICT;
+	}
 	r = ttm_bo_validate(&bo->tbo, &bo->placement, false, false);
 	if (likely(r == 0)) {
 		if (bo->tbo.mem.mem_type == TTM_PL_VRAM)
@@ -459,7 +488,7 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 	u64 bytes_moved = 0, initial_bytes_moved;
 	u64 bytes_moved_threshold = radeon_bo_get_threshold_for_moves(rdev);
 
-	r = ttm_eu_reserve_buffers(ticket, head);
+	r = ttm_eu_reserve_buffers(ticket, head, true);
 	if (unlikely(r != 0)) {
 		return r;
 	}
@@ -468,6 +497,7 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 		bo = lobj->robj;
 		if (!bo->pin_count) {
 			u32 domain = lobj->prefered_domains;
+			u32 allowed = lobj->allowed_domains;
 			u32 current_domain =
 				radeon_mem_type_to_domain(bo->tbo.mem.mem_type);
 
@@ -479,7 +509,7 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 			 * into account. We don't want to disallow buffer moves
 			 * completely.
 			 */
-			if ((lobj->allowed_domains & current_domain) != 0 &&
+			if ((allowed & current_domain) != 0 &&
 			    (domain & current_domain) == 0 && /* will be moved */
 			    bytes_moved > bytes_moved_threshold) {
 				/* don't move it */
@@ -489,7 +519,7 @@ int radeon_bo_list_validate(struct radeon_device *rdev,
 		retry:
 			radeon_ttm_placement_from_domain(bo, domain);
 			if (ring == R600_RING_TYPE_UVD_INDEX)
-				radeon_uvd_force_into_uvd_segment(bo);
+				radeon_uvd_force_into_uvd_segment(bo, allowed);
 
 			initial_bytes_moved = atomic64_read(&rdev->num_bytes_moved);
 			r = ttm_bo_validate(&bo->tbo, &bo->placement, true, false);
@@ -731,7 +761,7 @@ int radeon_bo_fault_reserve_notify(struct ttm_buffer_object *bo)
 
 	/* hurrah the memory is not visible ! */
 	radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_VRAM);
-	rbo->placement.lpfn = rdev->mc.visible_vram_size >> PAGE_SHIFT;
+	rbo->placements[0].lpfn = rdev->mc.visible_vram_size >> PAGE_SHIFT;
 	r = ttm_bo_validate(bo, &rbo->placement, false, false);
 	if (unlikely(r == -ENOMEM)) {
 		radeon_ttm_placement_from_domain(rbo, RADEON_GEM_DOMAIN_GTT);
@@ -755,12 +785,10 @@ int radeon_bo_wait(struct radeon_bo *bo, u32 *mem_type, bool no_wait)
 	r = ttm_bo_reserve(&bo->tbo, true, no_wait, false, NULL);
 	if (unlikely(r != 0))
 		return r;
-	spin_lock(&bo->tbo.bdev->fence_lock);
 	if (mem_type)
 		*mem_type = bo->tbo.mem.mem_type;
-	if (bo->tbo.sync_obj)
-		r = ttm_bo_wait(&bo->tbo, true, true, no_wait);
-	spin_unlock(&bo->tbo.bdev->fence_lock);
+
+	r = ttm_bo_wait(&bo->tbo, true, true, no_wait);
 	ttm_bo_unreserve(&bo->tbo);
 	return r;
 }
diff --git a/drivers/gpu/drm/radeon/radeon_object.h b/drivers/gpu/drm/radeon/radeon_object.h
index 98a47fdf3625..1b8ec7917154 100644
--- a/drivers/gpu/drm/radeon/radeon_object.h
+++ b/drivers/gpu/drm/radeon/radeon_object.h
@@ -126,6 +126,7 @@ extern int radeon_bo_create(struct radeon_device *rdev,
 			    unsigned long size, int byte_align,
 			    bool kernel, u32 domain, u32 flags,
 			    struct sg_table *sg,
+			    struct reservation_object *resv,
 			    struct radeon_bo **bo_ptr);
 extern int radeon_bo_kmap(struct radeon_bo *bo, void **ptr);
 extern void radeon_bo_kunmap(struct radeon_bo *bo);
diff --git a/drivers/gpu/drm/radeon/radeon_pm.c b/drivers/gpu/drm/radeon/radeon_pm.c
index 23314be49480..32522cc940a1 100644
--- a/drivers/gpu/drm/radeon/radeon_pm.c
+++ b/drivers/gpu/drm/radeon/radeon_pm.c
@@ -460,10 +460,6 @@ static ssize_t radeon_get_dpm_state(struct device *dev,
 	struct radeon_device *rdev = ddev->dev_private;
 	enum radeon_pm_state_type pm = rdev->pm.dpm.user_state;
 
-	if  ((rdev->flags & RADEON_IS_PX) &&
-	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
-		return snprintf(buf, PAGE_SIZE, "off\n");
-
 	return snprintf(buf, PAGE_SIZE, "%s\n",
 			(pm == POWER_STATE_TYPE_BATTERY) ? "battery" :
 			(pm == POWER_STATE_TYPE_BALANCED) ? "balanced" : "performance");
@@ -477,11 +473,6 @@ static ssize_t radeon_set_dpm_state(struct device *dev,
 	struct drm_device *ddev = dev_get_drvdata(dev);
 	struct radeon_device *rdev = ddev->dev_private;
 
-	/* Can't set dpm state when the card is off */
-	if  ((rdev->flags & RADEON_IS_PX) &&
-	     (ddev->switch_power_state != DRM_SWITCH_POWER_ON))
-		return -EINVAL;
-
 	mutex_lock(&rdev->pm.mutex);
 	if (strncmp("battery", buf, strlen("battery")) == 0)
 		rdev->pm.dpm.user_state = POWER_STATE_TYPE_BATTERY;
@@ -495,7 +486,12 @@ static ssize_t radeon_set_dpm_state(struct device *dev,
 		goto fail;
 	}
 	mutex_unlock(&rdev->pm.mutex);
-	radeon_pm_compute_clocks(rdev);
+
+	/* Can't set dpm state when the card is off */
+	if (!(rdev->flags & RADEON_IS_PX) ||
+	    (ddev->switch_power_state == DRM_SWITCH_POWER_ON))
+		radeon_pm_compute_clocks(rdev);
+
 fail:
 	return count;
 }
@@ -1560,7 +1556,7 @@ static bool radeon_pm_in_vbl(struct radeon_device *rdev)
 		if (rdev->pm.active_crtcs & (1 << crtc)) {
 			vbl_status = radeon_get_crtc_scanoutpos(rdev->ddev, crtc, 0, &vpos, &hpos, NULL, NULL);
 			if ((vbl_status & DRM_SCANOUTPOS_VALID) &&
-			    !(vbl_status & DRM_SCANOUTPOS_INVBL))
+			    !(vbl_status & DRM_SCANOUTPOS_IN_VBLANK))
 				in_vbl = false;
 		}
 	}
diff --git a/drivers/gpu/drm/radeon/radeon_prime.c b/drivers/gpu/drm/radeon/radeon_prime.c
index 0b16f2cbcf17..f3609c97496b 100644
--- a/drivers/gpu/drm/radeon/radeon_prime.c
+++ b/drivers/gpu/drm/radeon/radeon_prime.c
@@ -27,6 +27,7 @@
 
 #include "radeon.h"
 #include <drm/radeon_drm.h>
+#include <linux/dma-buf.h>
 
 struct sg_table *radeon_gem_prime_get_sg_table(struct drm_gem_object *obj)
 {
@@ -57,15 +58,18 @@ void radeon_gem_prime_vunmap(struct drm_gem_object *obj, void *vaddr)
 }
 
 struct drm_gem_object *radeon_gem_prime_import_sg_table(struct drm_device *dev,
-							size_t size,
+							struct dma_buf_attachment *attach,
 							struct sg_table *sg)
 {
+	struct reservation_object *resv = attach->dmabuf->resv;
 	struct radeon_device *rdev = dev->dev_private;
 	struct radeon_bo *bo;
 	int ret;
 
-	ret = radeon_bo_create(rdev, size, PAGE_SIZE, false,
-			       RADEON_GEM_DOMAIN_GTT, 0, sg, &bo);
+	ww_mutex_lock(&resv->lock, NULL);
+	ret = radeon_bo_create(rdev, attach->dmabuf->size, PAGE_SIZE, false,
+			       RADEON_GEM_DOMAIN_GTT, 0, sg, resv, &bo);
+	ww_mutex_unlock(&resv->lock);
 	if (ret)
 		return ERR_PTR(ret);
 
@@ -111,3 +115,13 @@ struct reservation_object *radeon_gem_prime_res_obj(struct drm_gem_object *obj)
 
 	return bo->tbo.resv;
 }
+
+struct dma_buf *radeon_gem_prime_export(struct drm_device *dev,
+					struct drm_gem_object *gobj,
+					int flags)
+{
+	struct radeon_bo *bo = gem_to_radeon_bo(gobj);
+	if (radeon_ttm_tt_has_userptr(bo->tbo.ttm))
+		return ERR_PTR(-EPERM);
+	return drm_gem_prime_export(dev, gobj, flags);
+}
diff --git a/drivers/gpu/drm/radeon/radeon_ring.c b/drivers/gpu/drm/radeon/radeon_ring.c
index 5b4e0cf231a0..3d17af34afa7 100644
--- a/drivers/gpu/drm/radeon/radeon_ring.c
+++ b/drivers/gpu/drm/radeon/radeon_ring.c
@@ -45,27 +45,6 @@
 static int radeon_debugfs_ring_init(struct radeon_device *rdev, struct radeon_ring *ring);
 
 /**
- * radeon_ring_write - write a value to the ring
- *
- * @ring: radeon_ring structure holding ring information
- * @v: dword (dw) value to write
- *
- * Write a value to the requested ring buffer (all asics).
- */
-void radeon_ring_write(struct radeon_ring *ring, uint32_t v)
-{
-#if DRM_DEBUG_CODE
-	if (ring->count_dw <= 0) {
-		DRM_ERROR("radeon: writing more dwords to the ring than expected!\n");
-	}
-#endif
-	ring->ring[ring->wptr++] = v;
-	ring->wptr &= ring->ptr_mask;
-	ring->count_dw--;
-	ring->ring_free_dw--;
-}
-
-/**
  * radeon_ring_supports_scratch_reg - check if the ring supports
  * writing to scratch registers
  *
@@ -177,16 +156,18 @@ int radeon_ring_lock(struct radeon_device *rdev, struct radeon_ring *ring, unsig
  *
  * @rdev: radeon_device pointer
  * @ring: radeon_ring structure holding ring information
+ * @hdp_flush: Whether or not to perform an HDP cache flush
  *
  * Update the wptr (write pointer) to tell the GPU to
  * execute new commands on the ring buffer (all asics).
  */
-void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
+void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring,
+			bool hdp_flush)
 {
 	/* If we are emitting the HDP flush via the ring buffer, we need to
 	 * do it before padding.
 	 */
-	if (rdev->asic->ring[ring->idx]->hdp_flush)
+	if (hdp_flush && rdev->asic->ring[ring->idx]->hdp_flush)
 		rdev->asic->ring[ring->idx]->hdp_flush(rdev, ring);
 	/* We pad to match fetch size */
 	while (ring->wptr & ring->align_mask) {
@@ -196,7 +177,7 @@ void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
 	/* If we are emitting the HDP flush via MMIO, we need to do it after
 	 * all CPU writes to VRAM finished.
 	 */
-	if (rdev->asic->mmio_hdp_flush)
+	if (hdp_flush && rdev->asic->mmio_hdp_flush)
 		rdev->asic->mmio_hdp_flush(rdev);
 	radeon_ring_set_wptr(rdev, ring);
 }
@@ -207,12 +188,14 @@ void radeon_ring_commit(struct radeon_device *rdev, struct radeon_ring *ring)
  *
  * @rdev: radeon_device pointer
  * @ring: radeon_ring structure holding ring information
+ * @hdp_flush: Whether or not to perform an HDP cache flush
  *
  * Call radeon_ring_commit() then unlock the ring (all asics).
  */
-void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *ring)
+void radeon_ring_unlock_commit(struct radeon_device *rdev, struct radeon_ring *ring,
+			       bool hdp_flush)
 {
-	radeon_ring_commit(rdev, ring);
+	radeon_ring_commit(rdev, ring, hdp_flush);
 	mutex_unlock(&rdev->ring_lock);
 }
 
@@ -372,7 +355,7 @@ int radeon_ring_restore(struct radeon_device *rdev, struct radeon_ring *ring,
 		radeon_ring_write(ring, data[i]);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 	kfree(data);
 	return 0;
 }
@@ -400,9 +383,7 @@ int radeon_ring_init(struct radeon_device *rdev, struct radeon_ring *ring, unsig
 	/* Allocate ring buffer */
 	if (ring->ring_obj == NULL) {
 		r = radeon_bo_create(rdev, ring->ring_size, PAGE_SIZE, true,
-				     RADEON_GEM_DOMAIN_GTT,
-				     (rdev->flags & RADEON_IS_PCIE) ?
-				     RADEON_GEM_GTT_WC : 0,
+				     RADEON_GEM_DOMAIN_GTT, 0, NULL,
 				     NULL, &ring->ring_obj);
 		if (r) {
 			dev_err(rdev->dev, "(%d) ring create failed\n", r);
diff --git a/drivers/gpu/drm/radeon/radeon_sa.c b/drivers/gpu/drm/radeon/radeon_sa.c
index b84f97c8718c..c507896aca45 100644
--- a/drivers/gpu/drm/radeon/radeon_sa.c
+++ b/drivers/gpu/drm/radeon/radeon_sa.c
@@ -65,7 +65,7 @@ int radeon_sa_bo_manager_init(struct radeon_device *rdev,
 	}
 
 	r = radeon_bo_create(rdev, size, align, true,
-			     domain, flags, NULL, &sa_manager->bo);
+			     domain, flags, NULL, NULL, &sa_manager->bo);
 	if (r) {
 		dev_err(rdev->dev, "(%d) failed to allocate bo for manager\n", r);
 		return r;
diff --git a/drivers/gpu/drm/radeon/radeon_semaphore.c b/drivers/gpu/drm/radeon/radeon_semaphore.c
index dbd6bcde92de..6deb08f045b7 100644
--- a/drivers/gpu/drm/radeon/radeon_semaphore.c
+++ b/drivers/gpu/drm/radeon/radeon_semaphore.c
@@ -34,7 +34,7 @@
 int radeon_semaphore_create(struct radeon_device *rdev,
 			    struct radeon_semaphore **semaphore)
 {
-	uint32_t *cpu_addr;
+	uint64_t *cpu_addr;
 	int i, r;
 
 	*semaphore = kmalloc(sizeof(struct radeon_semaphore), GFP_KERNEL);
@@ -96,15 +96,15 @@ bool radeon_semaphore_emit_wait(struct radeon_device *rdev, int ridx,
 }
 
 /**
- * radeon_semaphore_sync_to - use the semaphore to sync to a fence
+ * radeon_semaphore_sync_fence - use the semaphore to sync to a fence
  *
  * @semaphore: semaphore object to add fence to
  * @fence: fence to sync to
  *
  * Sync to the fence using this semaphore object
  */
-void radeon_semaphore_sync_to(struct radeon_semaphore *semaphore,
-			      struct radeon_fence *fence)
+void radeon_semaphore_sync_fence(struct radeon_semaphore *semaphore,
+				 struct radeon_fence *fence)
 {
         struct radeon_fence *other;
 
@@ -116,6 +116,53 @@ void radeon_semaphore_sync_to(struct radeon_semaphore *semaphore,
 }
 
 /**
+ * radeon_semaphore_sync_to - use the semaphore to sync to a reservation object
+ *
+ * @sema: semaphore object to add fence from reservation object to
+ * @resv: reservation object with embedded fence
+ * @shared: true if we should onyl sync to the exclusive fence
+ *
+ * Sync to the fence using this semaphore object
+ */
+int radeon_semaphore_sync_resv(struct radeon_device *rdev,
+			       struct radeon_semaphore *sema,
+			       struct reservation_object *resv,
+			       bool shared)
+{
+	struct reservation_object_list *flist;
+	struct fence *f;
+	struct radeon_fence *fence;
+	unsigned i;
+	int r = 0;
+
+	/* always sync to the exclusive fence */
+	f = reservation_object_get_excl(resv);
+	fence = f ? to_radeon_fence(f) : NULL;
+	if (fence && fence->rdev == rdev)
+		radeon_semaphore_sync_fence(sema, fence);
+	else if (f)
+		r = fence_wait(f, true);
+
+	flist = reservation_object_get_list(resv);
+	if (shared || !flist || r)
+		return r;
+
+	for (i = 0; i < flist->shared_count; ++i) {
+		f = rcu_dereference_protected(flist->shared[i],
+					      reservation_object_held(resv));
+		fence = to_radeon_fence(f);
+		if (fence && fence->rdev == rdev)
+			radeon_semaphore_sync_fence(sema, fence);
+		else
+			r = fence_wait(f, true);
+
+		if (r)
+			break;
+	}
+	return r;
+}
+
+/**
  * radeon_semaphore_sync_rings - sync ring to all registered fences
  *
  * @rdev: radeon_device pointer
@@ -179,7 +226,7 @@ int radeon_semaphore_sync_rings(struct radeon_device *rdev,
 			continue;
 		}
 
-		radeon_ring_commit(rdev, &rdev->ring[i]);
+		radeon_ring_commit(rdev, &rdev->ring[i], false);
 		radeon_fence_note_sync(fence, ring);
 
 		semaphore->gpu_addr += 8;
diff --git a/drivers/gpu/drm/radeon/radeon_state.c b/drivers/gpu/drm/radeon/radeon_state.c
index 23bb64fd775f..535403e0c8a2 100644
--- a/drivers/gpu/drm/radeon/radeon_state.c
+++ b/drivers/gpu/drm/radeon/radeon_state.c
@@ -30,9 +30,9 @@
  */
 
 #include <drm/drmP.h>
-#include <drm/drm_buffer.h>
 #include <drm/radeon_drm.h>
 #include "radeon_drv.h"
+#include "drm_buffer.h"
 
 /* ================================================================
  * Helper functions for client state checking and fixup
diff --git a/drivers/gpu/drm/radeon/radeon_test.c b/drivers/gpu/drm/radeon/radeon_test.c
index 5adf4207453d..07b506b41008 100644
--- a/drivers/gpu/drm/radeon/radeon_test.c
+++ b/drivers/gpu/drm/radeon/radeon_test.c
@@ -67,7 +67,7 @@ static void radeon_do_test_moves(struct radeon_device *rdev, int flag)
 	}
 
 	r = radeon_bo_create(rdev, size, PAGE_SIZE, true, RADEON_GEM_DOMAIN_VRAM,
-			     0, NULL, &vram_obj);
+			     0, NULL, NULL, &vram_obj);
 	if (r) {
 		DRM_ERROR("Failed to create VRAM object\n");
 		goto out_cleanup;
@@ -87,7 +87,8 @@ static void radeon_do_test_moves(struct radeon_device *rdev, int flag)
 		struct radeon_fence *fence = NULL;
 
 		r = radeon_bo_create(rdev, size, PAGE_SIZE, true,
-				     RADEON_GEM_DOMAIN_GTT, 0, NULL, gtt_obj + i);
+				     RADEON_GEM_DOMAIN_GTT, 0, NULL, NULL,
+				     gtt_obj + i);
 		if (r) {
 			DRM_ERROR("Failed to create GTT object %d\n", i);
 			goto out_lclean;
@@ -116,11 +117,16 @@ static void radeon_do_test_moves(struct radeon_device *rdev, int flag)
 		radeon_bo_kunmap(gtt_obj[i]);
 
 		if (ring == R600_RING_TYPE_DMA_INDEX)
-			r = radeon_copy_dma(rdev, gtt_addr, vram_addr, size / RADEON_GPU_PAGE_SIZE, &fence);
+			fence = radeon_copy_dma(rdev, gtt_addr, vram_addr,
+						size / RADEON_GPU_PAGE_SIZE,
+						NULL);
 		else
-			r = radeon_copy_blit(rdev, gtt_addr, vram_addr, size / RADEON_GPU_PAGE_SIZE, &fence);
-		if (r) {
+			fence = radeon_copy_blit(rdev, gtt_addr, vram_addr,
+						 size / RADEON_GPU_PAGE_SIZE,
+						 NULL);
+		if (IS_ERR(fence)) {
 			DRM_ERROR("Failed GTT->VRAM copy %d\n", i);
+			r = PTR_ERR(fence);
 			goto out_lclean_unpin;
 		}
 
@@ -162,11 +168,16 @@ static void radeon_do_test_moves(struct radeon_device *rdev, int flag)
 		radeon_bo_kunmap(vram_obj);
 
 		if (ring == R600_RING_TYPE_DMA_INDEX)
-			r = radeon_copy_dma(rdev, vram_addr, gtt_addr, size / RADEON_GPU_PAGE_SIZE, &fence);
+			fence = radeon_copy_dma(rdev, vram_addr, gtt_addr,
+						size / RADEON_GPU_PAGE_SIZE,
+						NULL);
 		else
-			r = radeon_copy_blit(rdev, vram_addr, gtt_addr, size / RADEON_GPU_PAGE_SIZE, &fence);
-		if (r) {
+			fence = radeon_copy_blit(rdev, vram_addr, gtt_addr,
+						 size / RADEON_GPU_PAGE_SIZE,
+						 NULL);
+		if (IS_ERR(fence)) {
 			DRM_ERROR("Failed VRAM->GTT copy %d\n", i);
+			r = PTR_ERR(fence);
 			goto out_lclean_unpin;
 		}
 
@@ -222,7 +233,7 @@ out_lclean:
 			radeon_bo_unreserve(gtt_obj[i]);
 			radeon_bo_unref(&gtt_obj[i]);
 		}
-		if (fence)
+		if (fence && !IS_ERR(fence))
 			radeon_fence_unref(&fence);
 		break;
 	}
@@ -288,7 +299,7 @@ static int radeon_test_create_and_emit_fence(struct radeon_device *rdev,
 			return r;
 		}
 		radeon_fence_emit(rdev, fence, ring->idx);
-		radeon_ring_unlock_commit(rdev, ring);
+		radeon_ring_unlock_commit(rdev, ring, false);
 	}
 	return 0;
 }
@@ -313,7 +324,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringA);
+	radeon_ring_unlock_commit(rdev, ringA, false);
 
 	r = radeon_test_create_and_emit_fence(rdev, ringA, &fence1);
 	if (r)
@@ -325,7 +336,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringA);
+	radeon_ring_unlock_commit(rdev, ringA, false);
 
 	r = radeon_test_create_and_emit_fence(rdev, ringA, &fence2);
 	if (r)
@@ -344,7 +355,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_signal(rdev, ringB->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringB);
+	radeon_ring_unlock_commit(rdev, ringB, false);
 
 	r = radeon_fence_wait(fence1, false);
 	if (r) {
@@ -365,7 +376,7 @@ void radeon_test_ring_sync(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_signal(rdev, ringB->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringB);
+	radeon_ring_unlock_commit(rdev, ringB, false);
 
 	r = radeon_fence_wait(fence2, false);
 	if (r) {
@@ -408,7 +419,7 @@ static void radeon_test_ring_sync2(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_wait(rdev, ringA->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringA);
+	radeon_ring_unlock_commit(rdev, ringA, false);
 
 	r = radeon_test_create_and_emit_fence(rdev, ringA, &fenceA);
 	if (r)
@@ -420,7 +431,7 @@ static void radeon_test_ring_sync2(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_wait(rdev, ringB->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringB);
+	radeon_ring_unlock_commit(rdev, ringB, false);
 	r = radeon_test_create_and_emit_fence(rdev, ringB, &fenceB);
 	if (r)
 		goto out_cleanup;
@@ -442,7 +453,7 @@ static void radeon_test_ring_sync2(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_signal(rdev, ringC->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringC);
+	radeon_ring_unlock_commit(rdev, ringC, false);
 
 	for (i = 0; i < 30; ++i) {
 		mdelay(100);
@@ -468,7 +479,7 @@ static void radeon_test_ring_sync2(struct radeon_device *rdev,
 		goto out_cleanup;
 	}
 	radeon_semaphore_emit_signal(rdev, ringC->idx, semaphore);
-	radeon_ring_unlock_commit(rdev, ringC);
+	radeon_ring_unlock_commit(rdev, ringC, false);
 
 	mdelay(1000);
 
diff --git a/drivers/gpu/drm/radeon/radeon_ttm.c b/drivers/gpu/drm/radeon/radeon_ttm.c
index 72afe82a95c9..8624979afb65 100644
--- a/drivers/gpu/drm/radeon/radeon_ttm.c
+++ b/drivers/gpu/drm/radeon/radeon_ttm.c
@@ -39,6 +39,8 @@
 #include <linux/seq_file.h>
 #include <linux/slab.h>
 #include <linux/swiotlb.h>
+#include <linux/swap.h>
+#include <linux/pagemap.h>
 #include <linux/debugfs.h>
 #include "radeon_reg.h"
 #include "radeon.h"
@@ -176,12 +178,15 @@ static int radeon_init_mem_type(struct ttm_bo_device *bdev, uint32_t type,
 static void radeon_evict_flags(struct ttm_buffer_object *bo,
 				struct ttm_placement *placement)
 {
+	static struct ttm_place placements = {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM
+	};
+
 	struct radeon_bo *rbo;
-	static u32 placements = TTM_PL_MASK_CACHING | TTM_PL_FLAG_SYSTEM;
 
 	if (!radeon_ttm_bo_is_radeon_bo(bo)) {
-		placement->fpfn = 0;
-		placement->lpfn = 0;
 		placement->placement = &placements;
 		placement->busy_placement = &placements;
 		placement->num_placement = 1;
@@ -228,6 +233,7 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 	struct radeon_device *rdev;
 	uint64_t old_start, new_start;
 	struct radeon_fence *fence;
+	unsigned num_pages;
 	int r, ridx;
 
 	rdev = radeon_get_rdev(bo->bdev);
@@ -264,13 +270,12 @@ static int radeon_move_blit(struct ttm_buffer_object *bo,
 
 	BUILD_BUG_ON((PAGE_SIZE % RADEON_GPU_PAGE_SIZE) != 0);
 
-	/* sync other rings */
-	fence = bo->sync_obj;
-	r = radeon_copy(rdev, old_start, new_start,
-			new_mem->num_pages * (PAGE_SIZE / RADEON_GPU_PAGE_SIZE), /* GPU pages */
-			&fence);
-	/* FIXME: handle copy error */
-	r = ttm_bo_move_accel_cleanup(bo, (void *)fence,
+	num_pages = new_mem->num_pages * (PAGE_SIZE / RADEON_GPU_PAGE_SIZE);
+	fence = radeon_copy(rdev, old_start, new_start, num_pages, bo->resv);
+	if (IS_ERR(fence))
+		return PTR_ERR(fence);
+
+	r = ttm_bo_move_accel_cleanup(bo, &fence->base,
 				      evict, no_wait_gpu, new_mem);
 	radeon_fence_unref(&fence);
 	return r;
@@ -284,20 +289,20 @@ static int radeon_move_vram_ram(struct ttm_buffer_object *bo,
 	struct radeon_device *rdev;
 	struct ttm_mem_reg *old_mem = &bo->mem;
 	struct ttm_mem_reg tmp_mem;
-	u32 placements;
+	struct ttm_place placements;
 	struct ttm_placement placement;
 	int r;
 
 	rdev = radeon_get_rdev(bo->bdev);
 	tmp_mem = *new_mem;
 	tmp_mem.mm_node = NULL;
-	placement.fpfn = 0;
-	placement.lpfn = 0;
 	placement.num_placement = 1;
 	placement.placement = &placements;
 	placement.num_busy_placement = 1;
 	placement.busy_placement = &placements;
-	placements = TTM_PL_MASK_CACHING | TTM_PL_FLAG_TT;
+	placements.fpfn = 0;
+	placements.lpfn = 0;
+	placements.flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_TT;
 	r = ttm_bo_mem_space(bo, &placement, &tmp_mem,
 			     interruptible, no_wait_gpu);
 	if (unlikely(r)) {
@@ -332,19 +337,19 @@ static int radeon_move_ram_vram(struct ttm_buffer_object *bo,
 	struct ttm_mem_reg *old_mem = &bo->mem;
 	struct ttm_mem_reg tmp_mem;
 	struct ttm_placement placement;
-	u32 placements;
+	struct ttm_place placements;
 	int r;
 
 	rdev = radeon_get_rdev(bo->bdev);
 	tmp_mem = *new_mem;
 	tmp_mem.mm_node = NULL;
-	placement.fpfn = 0;
-	placement.lpfn = 0;
 	placement.num_placement = 1;
 	placement.placement = &placements;
 	placement.num_busy_placement = 1;
 	placement.busy_placement = &placements;
-	placements = TTM_PL_MASK_CACHING | TTM_PL_FLAG_TT;
+	placements.fpfn = 0;
+	placements.lpfn = 0;
+	placements.flags = TTM_PL_MASK_CACHING | TTM_PL_FLAG_TT;
 	r = ttm_bo_mem_space(bo, &placement, &tmp_mem,
 			     interruptible, no_wait_gpu);
 	if (unlikely(r)) {
@@ -483,39 +488,108 @@ static void radeon_ttm_io_mem_free(struct ttm_bo_device *bdev, struct ttm_mem_re
 {
 }
 
-static int radeon_sync_obj_wait(void *sync_obj, bool lazy, bool interruptible)
-{
-	return radeon_fence_wait((struct radeon_fence *)sync_obj, interruptible);
-}
+/*
+ * TTM backend functions.
+ */
+struct radeon_ttm_tt {
+	struct ttm_dma_tt		ttm;
+	struct radeon_device		*rdev;
+	u64				offset;
 
-static int radeon_sync_obj_flush(void *sync_obj)
+	uint64_t			userptr;
+	struct mm_struct		*usermm;
+	uint32_t			userflags;
+};
+
+/* prepare the sg table with the user pages */
+static int radeon_ttm_tt_pin_userptr(struct ttm_tt *ttm)
 {
+	struct radeon_device *rdev = radeon_get_rdev(ttm->bdev);
+	struct radeon_ttm_tt *gtt = (void *)ttm;
+	unsigned pinned = 0, nents;
+	int r;
+
+	int write = !(gtt->userflags & RADEON_GEM_USERPTR_READONLY);
+	enum dma_data_direction direction = write ?
+		DMA_BIDIRECTIONAL : DMA_TO_DEVICE;
+
+	if (current->mm != gtt->usermm)
+		return -EPERM;
+
+	if (gtt->userflags & RADEON_GEM_USERPTR_ANONONLY) {
+		/* check that we only pin down anonymous memory
+		   to prevent problems with writeback */
+		unsigned long end = gtt->userptr + ttm->num_pages * PAGE_SIZE;
+		struct vm_area_struct *vma;
+		vma = find_vma(gtt->usermm, gtt->userptr);
+		if (!vma || vma->vm_file || vma->vm_end < end)
+			return -EPERM;
+	}
+
+	do {
+		unsigned num_pages = ttm->num_pages - pinned;
+		uint64_t userptr = gtt->userptr + pinned * PAGE_SIZE;
+		struct page **pages = ttm->pages + pinned;
+
+		r = get_user_pages(current, current->mm, userptr, num_pages,
+				   write, 0, pages, NULL);
+		if (r < 0)
+			goto release_pages;
+
+		pinned += r;
+
+	} while (pinned < ttm->num_pages);
+
+	r = sg_alloc_table_from_pages(ttm->sg, ttm->pages, ttm->num_pages, 0,
+				      ttm->num_pages << PAGE_SHIFT,
+				      GFP_KERNEL);
+	if (r)
+		goto release_sg;
+
+	r = -ENOMEM;
+	nents = dma_map_sg(rdev->dev, ttm->sg->sgl, ttm->sg->nents, direction);
+	if (nents != ttm->sg->nents)
+		goto release_sg;
+
+	drm_prime_sg_to_page_addr_arrays(ttm->sg, ttm->pages,
+					 gtt->ttm.dma_address, ttm->num_pages);
+
 	return 0;
-}
 
-static void radeon_sync_obj_unref(void **sync_obj)
-{
-	radeon_fence_unref((struct radeon_fence **)sync_obj);
-}
+release_sg:
+	kfree(ttm->sg);
 
-static void *radeon_sync_obj_ref(void *sync_obj)
-{
-	return radeon_fence_ref((struct radeon_fence *)sync_obj);
+release_pages:
+	release_pages(ttm->pages, pinned, 0);
+	return r;
 }
 
-static bool radeon_sync_obj_signaled(void *sync_obj)
+static void radeon_ttm_tt_unpin_userptr(struct ttm_tt *ttm)
 {
-	return radeon_fence_signaled((struct radeon_fence *)sync_obj);
-}
+	struct radeon_device *rdev = radeon_get_rdev(ttm->bdev);
+	struct radeon_ttm_tt *gtt = (void *)ttm;
+	struct scatterlist *sg;
+	int i;
 
-/*
- * TTM backend functions.
- */
-struct radeon_ttm_tt {
-	struct ttm_dma_tt		ttm;
-	struct radeon_device		*rdev;
-	u64				offset;
-};
+	int write = !(gtt->userflags & RADEON_GEM_USERPTR_READONLY);
+	enum dma_data_direction direction = write ?
+		DMA_BIDIRECTIONAL : DMA_TO_DEVICE;
+
+	/* free the sg table and pages again */
+	dma_unmap_sg(rdev->dev, ttm->sg->sgl, ttm->sg->nents, direction);
+
+	for_each_sg(ttm->sg->sgl, sg, ttm->sg->nents, i) {
+		struct page *page = sg_page(sg);
+
+		if (!(gtt->userflags & RADEON_GEM_USERPTR_READONLY))
+			set_page_dirty(page);
+
+		mark_page_accessed(page);
+		page_cache_release(page);
+	}
+
+	sg_free_table(ttm->sg);
+}
 
 static int radeon_ttm_backend_bind(struct ttm_tt *ttm,
 				   struct ttm_mem_reg *bo_mem)
@@ -525,6 +599,11 @@ static int radeon_ttm_backend_bind(struct ttm_tt *ttm,
 		RADEON_GART_PAGE_WRITE;
 	int r;
 
+	if (gtt->userptr) {
+		radeon_ttm_tt_pin_userptr(ttm);
+		flags &= ~RADEON_GART_PAGE_WRITE;
+	}
+
 	gtt->offset = (unsigned long)(bo_mem->start << PAGE_SHIFT);
 	if (!ttm->num_pages) {
 		WARN(1, "nothing to bind %lu pages for mreg %p back %p!\n",
@@ -547,6 +626,10 @@ static int radeon_ttm_backend_unbind(struct ttm_tt *ttm)
 	struct radeon_ttm_tt *gtt = (void *)ttm;
 
 	radeon_gart_unbind(gtt->rdev, gtt->offset, ttm->num_pages);
+
+	if (gtt->userptr)
+		radeon_ttm_tt_unpin_userptr(ttm);
+
 	return 0;
 }
 
@@ -592,10 +675,17 @@ static struct ttm_tt *radeon_ttm_tt_create(struct ttm_bo_device *bdev,
 	return &gtt->ttm.ttm;
 }
 
+static struct radeon_ttm_tt *radeon_ttm_tt_to_gtt(struct ttm_tt *ttm)
+{
+	if (!ttm || ttm->func != &radeon_backend_func)
+		return NULL;
+	return (struct radeon_ttm_tt *)ttm;
+}
+
 static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
 {
+	struct radeon_ttm_tt *gtt = radeon_ttm_tt_to_gtt(ttm);
 	struct radeon_device *rdev;
-	struct radeon_ttm_tt *gtt = (void *)ttm;
 	unsigned i;
 	int r;
 	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
@@ -603,6 +693,16 @@ static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
 	if (ttm->state != tt_unpopulated)
 		return 0;
 
+	if (gtt && gtt->userptr) {
+		ttm->sg = kcalloc(1, sizeof(struct sg_table), GFP_KERNEL);
+		if (!ttm->sg)
+			return -ENOMEM;
+
+		ttm->page_flags |= TTM_PAGE_FLAG_SG;
+		ttm->state = tt_unbound;
+		return 0;
+	}
+
 	if (slave && ttm->sg) {
 		drm_prime_sg_to_page_addr_arrays(ttm->sg, ttm->pages,
 						 gtt->ttm.dma_address, ttm->num_pages);
@@ -648,10 +748,16 @@ static int radeon_ttm_tt_populate(struct ttm_tt *ttm)
 static void radeon_ttm_tt_unpopulate(struct ttm_tt *ttm)
 {
 	struct radeon_device *rdev;
-	struct radeon_ttm_tt *gtt = (void *)ttm;
+	struct radeon_ttm_tt *gtt = radeon_ttm_tt_to_gtt(ttm);
 	unsigned i;
 	bool slave = !!(ttm->page_flags & TTM_PAGE_FLAG_SG);
 
+	if (gtt && gtt->userptr) {
+		kfree(ttm->sg);
+		ttm->page_flags &= ~TTM_PAGE_FLAG_SG;
+		return;
+	}
+
 	if (slave)
 		return;
 
@@ -680,6 +786,40 @@ static void radeon_ttm_tt_unpopulate(struct ttm_tt *ttm)
 	ttm_pool_unpopulate(ttm);
 }
 
+int radeon_ttm_tt_set_userptr(struct ttm_tt *ttm, uint64_t addr,
+			      uint32_t flags)
+{
+	struct radeon_ttm_tt *gtt = radeon_ttm_tt_to_gtt(ttm);
+
+	if (gtt == NULL)
+		return -EINVAL;
+
+	gtt->userptr = addr;
+	gtt->usermm = current->mm;
+	gtt->userflags = flags;
+	return 0;
+}
+
+bool radeon_ttm_tt_has_userptr(struct ttm_tt *ttm)
+{
+	struct radeon_ttm_tt *gtt = radeon_ttm_tt_to_gtt(ttm);
+
+	if (gtt == NULL)
+		return false;
+
+	return !!gtt->userptr;
+}
+
+bool radeon_ttm_tt_is_readonly(struct ttm_tt *ttm)
+{
+	struct radeon_ttm_tt *gtt = radeon_ttm_tt_to_gtt(ttm);
+
+	if (gtt == NULL)
+		return false;
+
+	return !!(gtt->userflags & RADEON_GEM_USERPTR_READONLY);
+}
+
 static struct ttm_bo_driver radeon_bo_driver = {
 	.ttm_tt_create = &radeon_ttm_tt_create,
 	.ttm_tt_populate = &radeon_ttm_tt_populate,
@@ -689,11 +829,6 @@ static struct ttm_bo_driver radeon_bo_driver = {
 	.evict_flags = &radeon_evict_flags,
 	.move = &radeon_bo_move,
 	.verify_access = &radeon_verify_access,
-	.sync_obj_signaled = &radeon_sync_obj_signaled,
-	.sync_obj_wait = &radeon_sync_obj_wait,
-	.sync_obj_flush = &radeon_sync_obj_flush,
-	.sync_obj_unref = &radeon_sync_obj_unref,
-	.sync_obj_ref = &radeon_sync_obj_ref,
 	.move_notify = &radeon_bo_move_notify,
 	.fault_reserve_notify = &radeon_bo_fault_reserve_notify,
 	.io_mem_reserve = &radeon_ttm_io_mem_reserve,
@@ -730,7 +865,7 @@ int radeon_ttm_init(struct radeon_device *rdev)
 	radeon_ttm_set_active_vram_size(rdev, rdev->mc.visible_vram_size);
 
 	r = radeon_bo_create(rdev, 256 * 1024, PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_VRAM, 0,
+			     RADEON_GEM_DOMAIN_VRAM, 0, NULL,
 			     NULL, &rdev->stollen_vga_memory);
 	if (r) {
 		return r;
@@ -828,7 +963,7 @@ int radeon_mmap(struct file *filp, struct vm_area_struct *vma)
 	int r;
 
 	if (unlikely(vma->vm_pgoff < DRM_FILE_PAGE_OFFSET)) {
-		return drm_mmap(filp, vma);
+		return -EINVAL;
 	}
 
 	file_priv = filp->private_data;
diff --git a/drivers/gpu/drm/radeon/radeon_uvd.c b/drivers/gpu/drm/radeon/radeon_uvd.c
index 6bf55ec85b62..11b662469253 100644
--- a/drivers/gpu/drm/radeon/radeon_uvd.c
+++ b/drivers/gpu/drm/radeon/radeon_uvd.c
@@ -40,12 +40,18 @@
 #define UVD_IDLE_TIMEOUT_MS	1000
 
 /* Firmware Names */
+#define FIRMWARE_R600		"radeon/R600_uvd.bin"
+#define FIRMWARE_RS780		"radeon/RS780_uvd.bin"
+#define FIRMWARE_RV770		"radeon/RV770_uvd.bin"
 #define FIRMWARE_RV710		"radeon/RV710_uvd.bin"
 #define FIRMWARE_CYPRESS	"radeon/CYPRESS_uvd.bin"
 #define FIRMWARE_SUMO		"radeon/SUMO_uvd.bin"
 #define FIRMWARE_TAHITI		"radeon/TAHITI_uvd.bin"
 #define FIRMWARE_BONAIRE	"radeon/BONAIRE_uvd.bin"
 
+MODULE_FIRMWARE(FIRMWARE_R600);
+MODULE_FIRMWARE(FIRMWARE_RS780);
+MODULE_FIRMWARE(FIRMWARE_RV770);
 MODULE_FIRMWARE(FIRMWARE_RV710);
 MODULE_FIRMWARE(FIRMWARE_CYPRESS);
 MODULE_FIRMWARE(FIRMWARE_SUMO);
@@ -63,6 +69,23 @@ int radeon_uvd_init(struct radeon_device *rdev)
 	INIT_DELAYED_WORK(&rdev->uvd.idle_work, radeon_uvd_idle_work_handler);
 
 	switch (rdev->family) {
+	case CHIP_RV610:
+	case CHIP_RV630:
+	case CHIP_RV670:
+	case CHIP_RV620:
+	case CHIP_RV635:
+		fw_name = FIRMWARE_R600;
+		break;
+
+	case CHIP_RS780:
+	case CHIP_RS880:
+		fw_name = FIRMWARE_RS780;
+		break;
+
+	case CHIP_RV770:
+		fw_name = FIRMWARE_RV770;
+		break;
+
 	case CHIP_RV710:
 	case CHIP_RV730:
 	case CHIP_RV740:
@@ -115,9 +138,11 @@ int radeon_uvd_init(struct radeon_device *rdev)
 	}
 
 	bo_size = RADEON_GPU_PAGE_ALIGN(rdev->uvd_fw->size + 8) +
-		  RADEON_UVD_STACK_SIZE + RADEON_UVD_HEAP_SIZE;
+		  RADEON_UVD_STACK_SIZE + RADEON_UVD_HEAP_SIZE +
+		  RADEON_GPU_PAGE_SIZE;
 	r = radeon_bo_create(rdev, bo_size, PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_VRAM, 0, NULL, &rdev->uvd.vcpu_bo);
+			     RADEON_GEM_DOMAIN_VRAM, 0, NULL,
+			     NULL, &rdev->uvd.vcpu_bo);
 	if (r) {
 		dev_err(rdev->dev, "(%d) failed to allocate UVD bo\n", r);
 		return r;
@@ -231,10 +256,30 @@ int radeon_uvd_resume(struct radeon_device *rdev)
 	return 0;
 }
 
-void radeon_uvd_force_into_uvd_segment(struct radeon_bo *rbo)
+void radeon_uvd_force_into_uvd_segment(struct radeon_bo *rbo,
+				       uint32_t allowed_domains)
 {
-	rbo->placement.fpfn = 0 >> PAGE_SHIFT;
-	rbo->placement.lpfn = (256 * 1024 * 1024) >> PAGE_SHIFT;
+	int i;
+
+	for (i = 0; i < rbo->placement.num_placement; ++i) {
+		rbo->placements[i].fpfn = 0 >> PAGE_SHIFT;
+		rbo->placements[i].lpfn = (256 * 1024 * 1024) >> PAGE_SHIFT;
+	}
+
+	/* If it must be in VRAM it must be in the first segment as well */
+	if (allowed_domains == RADEON_GEM_DOMAIN_VRAM)
+		return;
+
+	/* abort if we already have more than one placement */
+	if (rbo->placement.num_placement > 1)
+		return;
+
+	/* add another 256MB segment */
+	rbo->placements[1] = rbo->placements[0];
+	rbo->placements[1].fpfn += (256 * 1024 * 1024) >> PAGE_SHIFT;
+	rbo->placements[1].lpfn += (256 * 1024 * 1024) >> PAGE_SHIFT;
+	rbo->placement.num_placement++;
+	rbo->placement.num_busy_placement++;
 }
 
 void radeon_uvd_free_handles(struct radeon_device *rdev, struct drm_file *filp)
@@ -356,6 +401,7 @@ static int radeon_uvd_cs_msg(struct radeon_cs_parser *p, struct radeon_bo *bo,
 {
 	int32_t *msg, msg_type, handle;
 	unsigned img_size = 0;
+	struct fence *f;
 	void *ptr;
 
 	int i, r;
@@ -365,8 +411,9 @@ static int radeon_uvd_cs_msg(struct radeon_cs_parser *p, struct radeon_bo *bo,
 		return -EINVAL;
 	}
 
-	if (bo->tbo.sync_obj) {
-		r = radeon_fence_wait(bo->tbo.sync_obj, false);
+	f = reservation_object_get_excl(bo->tbo.resv);
+	if (f) {
+		r = radeon_fence_wait((struct radeon_fence *)f, false);
 		if (r) {
 			DRM_ERROR("Failed waiting for UVD message (%d)!\n", r);
 			return r;
@@ -604,38 +651,16 @@ int radeon_uvd_cs_parse(struct radeon_cs_parser *p)
 }
 
 static int radeon_uvd_send_msg(struct radeon_device *rdev,
-			       int ring, struct radeon_bo *bo,
+			       int ring, uint64_t addr,
 			       struct radeon_fence **fence)
 {
-	struct ttm_validate_buffer tv;
-	struct ww_acquire_ctx ticket;
-	struct list_head head;
 	struct radeon_ib ib;
-	uint64_t addr;
 	int i, r;
 
-	memset(&tv, 0, sizeof(tv));
-	tv.bo = &bo->tbo;
-
-	INIT_LIST_HEAD(&head);
-	list_add(&tv.head, &head);
-
-	r = ttm_eu_reserve_buffers(&ticket, &head);
-	if (r)
-		return r;
-
-	radeon_ttm_placement_from_domain(bo, RADEON_GEM_DOMAIN_VRAM);
-	radeon_uvd_force_into_uvd_segment(bo);
-
-	r = ttm_bo_validate(&bo->tbo, &bo->placement, true, false);
-	if (r) 
-		goto err;
-
 	r = radeon_ib_get(rdev, ring, &ib, NULL, 64);
 	if (r)
-		goto err;
+		return r;
 
-	addr = radeon_bo_gpu_offset(bo);
 	ib.ptr[0] = PACKET0(UVD_GPCOM_VCPU_DATA0, 0);
 	ib.ptr[1] = addr;
 	ib.ptr[2] = PACKET0(UVD_GPCOM_VCPU_DATA1, 0);
@@ -646,20 +671,12 @@ static int radeon_uvd_send_msg(struct radeon_device *rdev,
 		ib.ptr[i] = PACKET2(0);
 	ib.length_dw = 16;
 
-	r = radeon_ib_schedule(rdev, &ib, NULL);
-	if (r)
-		goto err;
-	ttm_eu_fence_buffer_objects(&ticket, &head, ib.fence);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 
 	if (fence)
 		*fence = radeon_fence_ref(ib.fence);
 
 	radeon_ib_free(rdev, &ib);
-	radeon_bo_unref(&bo);
-	return 0;
-
-err:
-	ttm_eu_backoff_reservation(&ticket, &head);
 	return r;
 }
 
@@ -669,27 +686,18 @@ err:
 int radeon_uvd_get_create_msg(struct radeon_device *rdev, int ring,
 			      uint32_t handle, struct radeon_fence **fence)
 {
-	struct radeon_bo *bo;
-	uint32_t *msg;
-	int r, i;
+	/* we use the last page of the vcpu bo for the UVD message */
+	uint64_t offs = radeon_bo_size(rdev->uvd.vcpu_bo) -
+		RADEON_GPU_PAGE_SIZE;
 
-	r = radeon_bo_create(rdev, 1024, PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_VRAM, 0, NULL, &bo);
-	if (r)
-		return r;
+	uint32_t *msg = rdev->uvd.cpu_addr + offs;
+	uint64_t addr = rdev->uvd.gpu_addr + offs;
 
-	r = radeon_bo_reserve(bo, false);
-	if (r) {
-		radeon_bo_unref(&bo);
-		return r;
-	}
+	int r, i;
 
-	r = radeon_bo_kmap(bo, (void **)&msg);
-	if (r) {
-		radeon_bo_unreserve(bo);
-		radeon_bo_unref(&bo);
+	r = radeon_bo_reserve(rdev->uvd.vcpu_bo, true);
+	if (r)
 		return r;
-	}
 
 	/* stitch together an UVD create msg */
 	msg[0] = cpu_to_le32(0x00000de4);
@@ -706,36 +714,26 @@ int radeon_uvd_get_create_msg(struct radeon_device *rdev, int ring,
 	for (i = 11; i < 1024; ++i)
 		msg[i] = cpu_to_le32(0x0);
 
-	radeon_bo_kunmap(bo);
-	radeon_bo_unreserve(bo);
-
-	return radeon_uvd_send_msg(rdev, ring, bo, fence);
+	r = radeon_uvd_send_msg(rdev, ring, addr, fence);
+	radeon_bo_unreserve(rdev->uvd.vcpu_bo);
+	return r;
 }
 
 int radeon_uvd_get_destroy_msg(struct radeon_device *rdev, int ring,
 			       uint32_t handle, struct radeon_fence **fence)
 {
-	struct radeon_bo *bo;
-	uint32_t *msg;
-	int r, i;
+	/* we use the last page of the vcpu bo for the UVD message */
+	uint64_t offs = radeon_bo_size(rdev->uvd.vcpu_bo) -
+		RADEON_GPU_PAGE_SIZE;
 
-	r = radeon_bo_create(rdev, 1024, PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_VRAM, 0, NULL, &bo);
-	if (r)
-		return r;
+	uint32_t *msg = rdev->uvd.cpu_addr + offs;
+	uint64_t addr = rdev->uvd.gpu_addr + offs;
 
-	r = radeon_bo_reserve(bo, false);
-	if (r) {
-		radeon_bo_unref(&bo);
-		return r;
-	}
+	int r, i;
 
-	r = radeon_bo_kmap(bo, (void **)&msg);
-	if (r) {
-		radeon_bo_unreserve(bo);
-		radeon_bo_unref(&bo);
+	r = radeon_bo_reserve(rdev->uvd.vcpu_bo, true);
+	if (r)
 		return r;
-	}
 
 	/* stitch together an UVD destroy msg */
 	msg[0] = cpu_to_le32(0x00000de4);
@@ -745,10 +743,9 @@ int radeon_uvd_get_destroy_msg(struct radeon_device *rdev, int ring,
 	for (i = 4; i < 1024; ++i)
 		msg[i] = cpu_to_le32(0x0);
 
-	radeon_bo_kunmap(bo);
-	radeon_bo_unreserve(bo);
-
-	return radeon_uvd_send_msg(rdev, ring, bo, fence);
+	r = radeon_uvd_send_msg(rdev, ring, addr, fence);
+	radeon_bo_unreserve(rdev->uvd.vcpu_bo);
+	return r;
 }
 
 /**
diff --git a/drivers/gpu/drm/radeon/radeon_vce.c b/drivers/gpu/drm/radeon/radeon_vce.c
index f9b70a43aa52..9e85757d5599 100644
--- a/drivers/gpu/drm/radeon/radeon_vce.c
+++ b/drivers/gpu/drm/radeon/radeon_vce.c
@@ -126,7 +126,8 @@ int radeon_vce_init(struct radeon_device *rdev)
 	size = RADEON_GPU_PAGE_ALIGN(rdev->vce_fw->size) +
 	       RADEON_VCE_STACK_SIZE + RADEON_VCE_HEAP_SIZE;
 	r = radeon_bo_create(rdev, size, PAGE_SIZE, true,
-			     RADEON_GEM_DOMAIN_VRAM, 0, NULL, &rdev->vce.vcpu_bo);
+			     RADEON_GEM_DOMAIN_VRAM, 0, NULL, NULL,
+			     &rdev->vce.vcpu_bo);
 	if (r) {
 		dev_err(rdev->dev, "(%d) failed to allocate VCE bo\n", r);
 		return r;
@@ -368,7 +369,7 @@ int radeon_vce_get_create_msg(struct radeon_device *rdev, int ring,
 	for (i = ib.length_dw; i < ib_size_dw; ++i)
 		ib.ptr[i] = 0x0;
 
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 	        DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
 	}
@@ -425,7 +426,7 @@ int radeon_vce_get_destroy_msg(struct radeon_device *rdev, int ring,
 	for (i = ib.length_dw; i < ib_size_dw; ++i)
 		ib.ptr[i] = 0x0;
 
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 	        DRM_ERROR("radeon: failed to schedule ib (%d).\n", r);
 	}
@@ -715,7 +716,7 @@ int radeon_vce_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 		return r;
 	}
 	radeon_ring_write(ring, VCE_CMD_END);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	for (i = 0; i < rdev->usec_timeout; i++) {
 	        if (vce_v1_0_get_rptr(rdev, ring) != rptr)
diff --git a/drivers/gpu/drm/radeon/radeon_vm.c b/drivers/gpu/drm/radeon/radeon_vm.c
index ccae4d9dc3de..4532cc76a0a6 100644
--- a/drivers/gpu/drm/radeon/radeon_vm.c
+++ b/drivers/gpu/drm/radeon/radeon_vm.c
@@ -143,6 +143,7 @@ struct radeon_cs_reloc *radeon_vm_get_bos(struct radeon_device *rdev,
 	list[0].prefered_domains = RADEON_GEM_DOMAIN_VRAM;
 	list[0].allowed_domains = RADEON_GEM_DOMAIN_VRAM;
 	list[0].tv.bo = &vm->page_directory->tbo;
+	list[0].tv.shared = false;
 	list[0].tiling_flags = 0;
 	list[0].handle = 0;
 	list_add(&list[0].tv.head, head);
@@ -156,6 +157,7 @@ struct radeon_cs_reloc *radeon_vm_get_bos(struct radeon_device *rdev,
 		list[idx].prefered_domains = RADEON_GEM_DOMAIN_VRAM;
 		list[idx].allowed_domains = RADEON_GEM_DOMAIN_VRAM;
 		list[idx].tv.bo = &list[idx].robj->tbo;
+		list[idx].tv.shared = false;
 		list[idx].tiling_flags = 0;
 		list[idx].handle = 0;
 		list_add(&list[idx++].tv.head, head);
@@ -395,11 +397,12 @@ static int radeon_vm_clear_bo(struct radeon_device *rdev,
 
         memset(&tv, 0, sizeof(tv));
         tv.bo = &bo->tbo;
+	tv.shared = false;
 
         INIT_LIST_HEAD(&head);
         list_add(&tv.head, &head);
 
-        r = ttm_eu_reserve_buffers(&ticket, &head);
+        r = ttm_eu_reserve_buffers(&ticket, &head, true);
         if (r)
 		return r;
 
@@ -420,11 +423,11 @@ static int radeon_vm_clear_bo(struct radeon_device *rdev,
 	radeon_asic_vm_pad_ib(rdev, &ib);
 	WARN_ON(ib.length_dw > 64);
 
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r)
                 goto error;
 
-	ttm_eu_fence_buffer_objects(&ticket, &head, ib.fence);
+	ttm_eu_fence_buffer_objects(&ticket, &head, &ib.fence->base);
 	radeon_ib_free(rdev, &ib);
 
 	return 0;
@@ -483,6 +486,10 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 			/* add a clone of the bo_va to clear the old address */
 			struct radeon_bo_va *tmp;
 			tmp = kzalloc(sizeof(struct radeon_bo_va), GFP_KERNEL);
+			if (!tmp) {
+				mutex_unlock(&vm->mutex);
+				return -ENOMEM;
+			}
 			tmp->it.start = bo_va->it.start;
 			tmp->it.last = bo_va->it.last;
 			tmp->vm = vm;
@@ -541,7 +548,8 @@ int radeon_vm_bo_set_addr(struct radeon_device *rdev,
 
 		r = radeon_bo_create(rdev, RADEON_VM_PTE_COUNT * 8,
 				     RADEON_GPU_PAGE_SIZE, true,
-				     RADEON_GEM_DOMAIN_VRAM, 0, NULL, &pt);
+				     RADEON_GEM_DOMAIN_VRAM, 0,
+				     NULL, NULL, &pt);
 		if (r)
 			return r;
 
@@ -690,10 +698,11 @@ int radeon_vm_update_page_directory(struct radeon_device *rdev,
 
 	if (ib.length_dw != 0) {
 		radeon_asic_vm_pad_ib(rdev, &ib);
-		radeon_semaphore_sync_to(ib.semaphore, pd->tbo.sync_obj);
-		radeon_semaphore_sync_to(ib.semaphore, vm->last_id_use);
+
+		radeon_semaphore_sync_resv(rdev, ib.semaphore, pd->tbo.resv, false);
+		radeon_semaphore_sync_fence(ib.semaphore, vm->last_id_use);
 		WARN_ON(ib.length_dw > ndw);
-		r = radeon_ib_schedule(rdev, &ib, NULL);
+		r = radeon_ib_schedule(rdev, &ib, NULL, false);
 		if (r) {
 			radeon_ib_free(rdev, &ib);
 			return r;
@@ -817,7 +826,7 @@ static void radeon_vm_update_ptes(struct radeon_device *rdev,
 		unsigned nptes;
 		uint64_t pte;
 
-		radeon_semaphore_sync_to(ib->semaphore, pt->tbo.sync_obj);
+		radeon_semaphore_sync_resv(rdev, ib->semaphore, pt->tbo.resv, false);
 
 		if ((addr & ~mask) == (end & ~mask))
 			nptes = end - addr;
@@ -888,6 +897,9 @@ int radeon_vm_bo_update(struct radeon_device *rdev,
 	bo_va->flags &= ~RADEON_VM_PAGE_VALID;
 	bo_va->flags &= ~RADEON_VM_PAGE_SYSTEM;
 	bo_va->flags &= ~RADEON_VM_PAGE_SNOOPED;
+	if (bo_va->bo && radeon_ttm_tt_is_readonly(bo_va->bo->tbo.ttm))
+		bo_va->flags &= ~RADEON_VM_PAGE_WRITEABLE;
+
 	if (mem) {
 		addr = mem->start << PAGE_SHIFT;
 		if (mem->mem_type != TTM_PL_SYSTEM) {
@@ -956,8 +968,8 @@ int radeon_vm_bo_update(struct radeon_device *rdev,
 	radeon_asic_vm_pad_ib(rdev, &ib);
 	WARN_ON(ib.length_dw > ndw);
 
-	radeon_semaphore_sync_to(ib.semaphore, vm->fence);
-	r = radeon_ib_schedule(rdev, &ib, NULL);
+	radeon_semaphore_sync_fence(ib.semaphore, vm->fence);
+	r = radeon_ib_schedule(rdev, &ib, NULL, false);
 	if (r) {
 		radeon_ib_free(rdev, &ib);
 		return r;
@@ -1116,7 +1128,7 @@ int radeon_vm_init(struct radeon_device *rdev, struct radeon_vm *vm)
 
 	r = radeon_bo_create(rdev, pd_size, align, true,
 			     RADEON_GEM_DOMAIN_VRAM, 0, NULL,
-			     &vm->page_directory);
+			     NULL, &vm->page_directory);
 	if (r)
 		return r;
 
diff --git a/drivers/gpu/drm/radeon/rs400.c b/drivers/gpu/drm/radeon/rs400.c
index 6c1fc339d228..c5799f16aa4b 100644
--- a/drivers/gpu/drm/radeon/rs400.c
+++ b/drivers/gpu/drm/radeon/rs400.c
@@ -221,9 +221,9 @@ void rs400_gart_set_page(struct radeon_device *rdev, unsigned i,
 	entry = (lower_32_bits(addr) & PAGE_MASK) |
 		((upper_32_bits(addr) & 0xff) << 4);
 	if (flags & RADEON_GART_PAGE_READ)
-		addr |= RS400_PTE_READABLE;
+		entry |= RS400_PTE_READABLE;
 	if (flags & RADEON_GART_PAGE_WRITE)
-		addr |= RS400_PTE_WRITEABLE;
+		entry |= RS400_PTE_WRITEABLE;
 	if (!(flags & RADEON_GART_PAGE_SNOOP))
 		entry |= RS400_PTE_UNSNOOPED;
 	entry = cpu_to_le32(entry);
diff --git a/drivers/gpu/drm/radeon/rs780_dpm.c b/drivers/gpu/drm/radeon/rs780_dpm.c
index 02f7710de470..9031f4b69824 100644
--- a/drivers/gpu/drm/radeon/rs780_dpm.c
+++ b/drivers/gpu/drm/radeon/rs780_dpm.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "rs780d.h"
 #include "r600_dpm.h"
 #include "rs780_dpm.h"
diff --git a/drivers/gpu/drm/radeon/rv515.c b/drivers/gpu/drm/radeon/rv515.c
index 3e21e869015f..8a477bf1fdb3 100644
--- a/drivers/gpu/drm/radeon/rv515.c
+++ b/drivers/gpu/drm/radeon/rv515.c
@@ -124,7 +124,7 @@ void rv515_ring_start(struct radeon_device *rdev, struct radeon_ring *ring)
 	radeon_ring_write(ring, GEOMETRY_ROUND_NEAREST | COLOR_ROUND_NEAREST);
 	radeon_ring_write(ring, PACKET0(0x20C8, 0));
 	radeon_ring_write(ring, 0);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 }
 
 int rv515_mc_wait_for_idle(struct radeon_device *rdev)
diff --git a/drivers/gpu/drm/radeon/rv6xx_dpm.c b/drivers/gpu/drm/radeon/rv6xx_dpm.c
index e7045b085715..6a5c233361e9 100644
--- a/drivers/gpu/drm/radeon/rv6xx_dpm.c
+++ b/drivers/gpu/drm/radeon/rv6xx_dpm.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "rv6xxd.h"
 #include "r600_dpm.h"
 #include "rv6xx_dpm.h"
diff --git a/drivers/gpu/drm/radeon/rv770.c b/drivers/gpu/drm/radeon/rv770.c
index 2983f17ea1b3..372016e266d0 100644
--- a/drivers/gpu/drm/radeon/rv770.c
+++ b/drivers/gpu/drm/radeon/rv770.c
@@ -26,7 +26,6 @@
  *          Jerome Glisse
  */
 #include <linux/firmware.h>
-#include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <drm/drmP.h>
 #include "radeon.h"
@@ -1177,7 +1176,6 @@ static void rv770_gpu_init(struct radeon_device *rdev)
 	u32 hdp_host_path_cntl;
 	u32 sq_dyn_gpr_size_simd_ab_0;
 	u32 gb_tiling_config = 0;
-	u32 cc_rb_backend_disable = 0;
 	u32 cc_gc_shader_pipe_config = 0;
 	u32 mc_arb_ramcfg;
 	u32 db_debug4, tmp;
@@ -1311,21 +1309,7 @@ static void rv770_gpu_init(struct radeon_device *rdev)
 		WREG32(SPI_CONFIG_CNTL, 0);
 	}
 
-	cc_rb_backend_disable = RREG32(CC_RB_BACKEND_DISABLE) & 0x00ff0000;
-	tmp = R7XX_MAX_BACKENDS - r600_count_pipe_bits(cc_rb_backend_disable >> 16);
-	if (tmp < rdev->config.rv770.max_backends) {
-		rdev->config.rv770.max_backends = tmp;
-	}
-
 	cc_gc_shader_pipe_config = RREG32(CC_GC_SHADER_PIPE_CONFIG) & 0xffffff00;
-	tmp = R7XX_MAX_PIPES - r600_count_pipe_bits((cc_gc_shader_pipe_config >> 8) & R7XX_MAX_PIPES_MASK);
-	if (tmp < rdev->config.rv770.max_pipes) {
-		rdev->config.rv770.max_pipes = tmp;
-	}
-	tmp = R7XX_MAX_SIMDS - r600_count_pipe_bits((cc_gc_shader_pipe_config >> 16) & R7XX_MAX_SIMDS_MASK);
-	if (tmp < rdev->config.rv770.max_simds) {
-		rdev->config.rv770.max_simds = tmp;
-	}
 	tmp = rdev->config.rv770.max_simds -
 		r600_count_pipe_bits((cc_gc_shader_pipe_config >> 16) & R7XX_MAX_SIMDS_MASK);
 	rdev->config.rv770.active_simds = tmp;
@@ -1348,6 +1332,14 @@ static void rv770_gpu_init(struct radeon_device *rdev)
 	rdev->config.rv770.tiling_npipes = rdev->config.rv770.max_tile_pipes;
 
 	disabled_rb_mask = (RREG32(CC_RB_BACKEND_DISABLE) >> 16) & R7XX_MAX_BACKENDS_MASK;
+	tmp = 0;
+	for (i = 0; i < rdev->config.rv770.max_backends; i++)
+		tmp |= (1 << i);
+	/* if all the backends are disabled, fix it up here */
+	if ((disabled_rb_mask & tmp) == tmp) {
+		for (i = 0; i < rdev->config.rv770.max_backends; i++)
+			disabled_rb_mask &= ~(1 << i);
+	}
 	tmp = (gb_tiling_config & PIPE_TILING__MASK) >> PIPE_TILING__SHIFT;
 	tmp = r6xx_remap_render_backend(rdev, tmp, rdev->config.rv770.max_backends,
 					R7XX_MAX_BACKENDS, disabled_rb_mask);
diff --git a/drivers/gpu/drm/radeon/rv770_dma.c b/drivers/gpu/drm/radeon/rv770_dma.c
index bbf2e076ee45..7f34bad2e724 100644
--- a/drivers/gpu/drm/radeon/rv770_dma.c
+++ b/drivers/gpu/drm/radeon/rv770_dma.c
@@ -33,18 +33,19 @@
  * @src_offset: src GPU address
  * @dst_offset: dst GPU address
  * @num_gpu_pages: number of GPU pages to xfer
- * @fence: radeon fence object
+ * @resv: reservation object to sync to
  *
  * Copy GPU paging using the DMA engine (r7xx).
  * Used by the radeon ttm implementation to move pages if
  * registered as the asic copy callback.
  */
-int rv770_copy_dma(struct radeon_device *rdev,
-		  uint64_t src_offset, uint64_t dst_offset,
-		  unsigned num_gpu_pages,
-		  struct radeon_fence **fence)
+struct radeon_fence *rv770_copy_dma(struct radeon_device *rdev,
+				    uint64_t src_offset, uint64_t dst_offset,
+				    unsigned num_gpu_pages,
+				    struct reservation_object *resv)
 {
 	struct radeon_semaphore *sem = NULL;
+	struct radeon_fence *fence;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_dw, cur_size_in_dw;
@@ -54,7 +55,7 @@ int rv770_copy_dma(struct radeon_device *rdev,
 	r = radeon_semaphore_create(rdev, &sem);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 
 	size_in_dw = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT) / 4;
@@ -63,10 +64,10 @@ int rv770_copy_dma(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_to(sem, *fence);
+	radeon_semaphore_sync_resv(rdev, sem, resv, false);
 	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
@@ -83,15 +84,15 @@ int rv770_copy_dma(struct radeon_device *rdev,
 		dst_offset += cur_size_in_dw * 4;
 	}
 
-	r = radeon_fence_emit(rdev, fence, ring->idx);
+	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
-	radeon_semaphore_free(rdev, &sem, *fence);
+	radeon_ring_unlock_commit(rdev, ring, false);
+	radeon_semaphore_free(rdev, &sem, fence);
 
-	return r;
+	return fence;
 }
diff --git a/drivers/gpu/drm/radeon/rv770_dpm.c b/drivers/gpu/drm/radeon/rv770_dpm.c
index 3c76e1dcdf04..755a8f96fe46 100644
--- a/drivers/gpu/drm/radeon/rv770_dpm.c
+++ b/drivers/gpu/drm/radeon/rv770_dpm.c
@@ -24,6 +24,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "rv770d.h"
 #include "r600_dpm.h"
 #include "rv770_dpm.h"
diff --git a/drivers/gpu/drm/radeon/si.c b/drivers/gpu/drm/radeon/si.c
index 011779bd2b3d..eeea5b6a1775 100644
--- a/drivers/gpu/drm/radeon/si.c
+++ b/drivers/gpu/drm/radeon/si.c
@@ -3057,7 +3057,7 @@ static void si_gpu_init(struct radeon_device *rdev)
 	u32 sx_debug_1;
 	u32 hdp_host_path_cntl;
 	u32 tmp;
-	int i, j, k;
+	int i, j;
 
 	switch (rdev->family) {
 	case CHIP_TAHITI:
@@ -3255,12 +3255,11 @@ static void si_gpu_init(struct radeon_device *rdev)
 		     rdev->config.si.max_sh_per_se,
 		     rdev->config.si.max_cu_per_sh);
 
+	rdev->config.si.active_cus = 0;
 	for (i = 0; i < rdev->config.si.max_shader_engines; i++) {
 		for (j = 0; j < rdev->config.si.max_sh_per_se; j++) {
-			for (k = 0; k < rdev->config.si.max_cu_per_sh; k++) {
-				rdev->config.si.active_cus +=
-					hweight32(si_get_cu_active_bitmap(rdev, i, j));
-			}
+			rdev->config.si.active_cus +=
+				hweight32(si_get_cu_active_bitmap(rdev, i, j));
 		}
 	}
 
@@ -3541,7 +3540,7 @@ static int si_cp_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, PACKET3_BASE_INDEX(CE_PARTITION_BASE));
 	radeon_ring_write(ring, 0xc000);
 	radeon_ring_write(ring, 0xe000);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	si_cp_enable(rdev, true);
 
@@ -3570,7 +3569,7 @@ static int si_cp_start(struct radeon_device *rdev)
 	radeon_ring_write(ring, 0x0000000e); /* VGT_VERTEX_REUSE_BLOCK_CNTL */
 	radeon_ring_write(ring, 0x00000010); /* VGT_OUT_DEALLOC_CNTL */
 
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 	for (i = RADEON_RING_TYPE_GFX_INDEX; i <= CAYMAN_RING_TYPE_CP2_INDEX; ++i) {
 		ring = &rdev->ring[i];
@@ -3580,7 +3579,7 @@ static int si_cp_start(struct radeon_device *rdev)
 		radeon_ring_write(ring, PACKET3_COMPUTE(PACKET3_CLEAR_STATE, 0));
 		radeon_ring_write(ring, 0);
 
-		radeon_ring_unlock_commit(rdev, ring);
+		radeon_ring_unlock_commit(rdev, ring, false);
 	}
 
 	return 0;
@@ -4291,10 +4290,10 @@ static int si_pcie_gart_enable(struct radeon_device *rdev)
 	for (i = 1; i < 16; i++) {
 		if (i < 8)
 			WREG32(VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2),
-			       rdev->gart.table_addr >> 12);
+			       rdev->vm_manager.saved_table_addr[i]);
 		else
 			WREG32(VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((i - 8) << 2),
-			       rdev->gart.table_addr >> 12);
+			       rdev->vm_manager.saved_table_addr[i]);
 	}
 
 	/* enable context1-15 */
@@ -4326,6 +4325,17 @@ static int si_pcie_gart_enable(struct radeon_device *rdev)
 
 static void si_pcie_gart_disable(struct radeon_device *rdev)
 {
+	unsigned i;
+
+	for (i = 1; i < 16; ++i) {
+		uint32_t reg;
+		if (i < 8)
+			reg = VM_CONTEXT0_PAGE_TABLE_BASE_ADDR + (i << 2);
+		else
+			reg = VM_CONTEXT8_PAGE_TABLE_BASE_ADDR + ((i - 8) << 2);
+		rdev->vm_manager.saved_table_addr[i] = RREG32(reg);
+	}
+
 	/* Disable all tables */
 	WREG32(VM_CONTEXT0_CNTL, 0);
 	WREG32(VM_CONTEXT1_CNTL, 0);
@@ -4674,7 +4684,7 @@ static int si_vm_packet3_compute_check(struct radeon_device *rdev,
 int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
 {
 	int ret = 0;
-	u32 idx = 0;
+	u32 idx = 0, i;
 	struct radeon_cs_packet pkt;
 
 	do {
@@ -4685,6 +4695,12 @@ int si_ib_parse(struct radeon_device *rdev, struct radeon_ib *ib)
 		switch (pkt.type) {
 		case RADEON_PACKET_TYPE0:
 			dev_err(rdev->dev, "Packet0 not allowed!\n");
+			for (i = 0; i < ib->length_dw; i++) {
+				if (i == idx)
+					printk("\t0x%08x <---\n", ib->ptr[i]);
+				else
+					printk("\t0x%08x\n", ib->ptr[i]);
+			}
 			ret = -EINVAL;
 			break;
 		case RADEON_PACKET_TYPE2:
@@ -5028,7 +5044,7 @@ void si_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 
 	/* flush hdp cache */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
-	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
+	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(1) |
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, HDP_MEM_COHERENCY_FLUSH_CNTL >> 2);
 	radeon_ring_write(ring, 0);
@@ -5036,7 +5052,7 @@ void si_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
 
 	/* bits 0-15 are the VM contexts0-15 */
 	radeon_ring_write(ring, PACKET3(PACKET3_WRITE_DATA, 3));
-	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(0) |
+	radeon_ring_write(ring, (WRITE_DATA_ENGINE_SEL(1) |
 				 WRITE_DATA_DST_SEL(0)));
 	radeon_ring_write(ring, VM_INVALIDATE_REQUEST >> 2);
 	radeon_ring_write(ring, 0);
@@ -6306,17 +6322,17 @@ static inline u32 si_get_ih_wptr(struct radeon_device *rdev)
 		wptr = RREG32(IH_RB_WPTR);
 
 	if (wptr & RB_OVERFLOW) {
+		wptr &= ~RB_OVERFLOW;
 		/* When a ring buffer overflow happen start parsing interrupt
 		 * from the last not overwritten vector (wptr + 16). Hopefully
 		 * this should allow us to catchup.
 		 */
-		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, %d, %d)\n",
-			wptr, rdev->ih.rptr, (wptr + 16) + rdev->ih.ptr_mask);
+		dev_warn(rdev->dev, "IH ring buffer overflow (0x%08X, 0x%08X, 0x%08X)\n",
+			 wptr, rdev->ih.rptr, (wptr + 16) & rdev->ih.ptr_mask);
 		rdev->ih.rptr = (wptr + 16) & rdev->ih.ptr_mask;
 		tmp = RREG32(IH_RB_CNTL);
 		tmp |= IH_WPTR_OVERFLOW_CLEAR;
 		WREG32(IH_RB_CNTL, tmp);
-		wptr &= ~RB_OVERFLOW;
 	}
 	return (wptr & rdev->ih.ptr_mask);
 }
@@ -6654,13 +6670,13 @@ restart_ih:
 		/* wptr/rptr are in bytes! */
 		rptr += 16;
 		rptr &= rdev->ih.ptr_mask;
+		WREG32(IH_RB_RPTR, rptr);
 	}
 	if (queue_hotplug)
 		schedule_work(&rdev->hotplug_work);
 	if (queue_thermal && rdev->pm.dpm_enabled)
 		schedule_work(&rdev->pm.dpm.thermal.work);
 	rdev->ih.rptr = rptr;
-	WREG32(IH_RB_RPTR, rdev->ih.rptr);
 	atomic_set(&rdev->ih.lock, 0);
 
 	/* make sure wptr hasn't changed while processing */
@@ -7178,6 +7194,9 @@ static void si_pcie_gen3_enable(struct radeon_device *rdev)
 	int ret, i;
 	u16 tmp16;
 
+	if (pci_is_root_bus(rdev->pdev->bus))
+		return;
+
 	if (radeon_pcie_gen2 == 0)
 		return;
 
@@ -7455,7 +7474,8 @@ static void si_program_aspm(struct radeon_device *rdev)
 			if (orig != data)
 				WREG32_PIF_PHY1(PB1_PIF_CNTL, data);
 
-			if (!disable_clkreq) {
+			if (!disable_clkreq &&
+			    !pci_is_root_bus(rdev->pdev->bus)) {
 				struct pci_dev *root = rdev->pdev->bus->self;
 				u32 lnkcap;
 
diff --git a/drivers/gpu/drm/radeon/si_dma.c b/drivers/gpu/drm/radeon/si_dma.c
index 716505129450..b58f12b762d7 100644
--- a/drivers/gpu/drm/radeon/si_dma.c
+++ b/drivers/gpu/drm/radeon/si_dma.c
@@ -218,18 +218,19 @@ void si_dma_vm_flush(struct radeon_device *rdev, int ridx, struct radeon_vm *vm)
  * @src_offset: src GPU address
  * @dst_offset: dst GPU address
  * @num_gpu_pages: number of GPU pages to xfer
- * @fence: radeon fence object
+ * @resv: reservation object to sync to
  *
  * Copy GPU paging using the DMA engine (SI).
  * Used by the radeon ttm implementation to move pages if
  * registered as the asic copy callback.
  */
-int si_copy_dma(struct radeon_device *rdev,
-		uint64_t src_offset, uint64_t dst_offset,
-		unsigned num_gpu_pages,
-		struct radeon_fence **fence)
+struct radeon_fence *si_copy_dma(struct radeon_device *rdev,
+				 uint64_t src_offset, uint64_t dst_offset,
+				 unsigned num_gpu_pages,
+				 struct reservation_object *resv)
 {
 	struct radeon_semaphore *sem = NULL;
+	struct radeon_fence *fence;
 	int ring_index = rdev->asic->copy.dma_ring_index;
 	struct radeon_ring *ring = &rdev->ring[ring_index];
 	u32 size_in_bytes, cur_size_in_bytes;
@@ -239,7 +240,7 @@ int si_copy_dma(struct radeon_device *rdev,
 	r = radeon_semaphore_create(rdev, &sem);
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
-		return r;
+		return ERR_PTR(r);
 	}
 
 	size_in_bytes = (num_gpu_pages << RADEON_GPU_PAGE_SHIFT);
@@ -248,10 +249,10 @@ int si_copy_dma(struct radeon_device *rdev,
 	if (r) {
 		DRM_ERROR("radeon: moving bo (%d).\n", r);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_semaphore_sync_to(sem, *fence);
+	radeon_semaphore_sync_resv(rdev, sem, resv, false);
 	radeon_semaphore_sync_rings(rdev, sem, ring->idx);
 
 	for (i = 0; i < num_loops; i++) {
@@ -268,16 +269,16 @@ int si_copy_dma(struct radeon_device *rdev,
 		dst_offset += cur_size_in_bytes;
 	}
 
-	r = radeon_fence_emit(rdev, fence, ring->idx);
+	r = radeon_fence_emit(rdev, &fence, ring->idx);
 	if (r) {
 		radeon_ring_unlock_undo(rdev, ring);
 		radeon_semaphore_free(rdev, &sem, NULL);
-		return r;
+		return ERR_PTR(r);
 	}
 
-	radeon_ring_unlock_commit(rdev, ring);
-	radeon_semaphore_free(rdev, &sem, *fence);
+	radeon_ring_unlock_commit(rdev, ring, false);
+	radeon_semaphore_free(rdev, &sem, fence);
 
-	return r;
+	return fence;
 }
 
diff --git a/drivers/gpu/drm/radeon/si_dpm.c b/drivers/gpu/drm/radeon/si_dpm.c
index 70e61ffeace2..a53c2e79d9cb 100644
--- a/drivers/gpu/drm/radeon/si_dpm.c
+++ b/drivers/gpu/drm/radeon/si_dpm.c
@@ -23,6 +23,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "sid.h"
 #include "r600_dpm.h"
 #include "si_dpm.h"
diff --git a/drivers/gpu/drm/radeon/sid.h b/drivers/gpu/drm/radeon/sid.h
index fd414d34d885..6635da9ec986 100644
--- a/drivers/gpu/drm/radeon/sid.h
+++ b/drivers/gpu/drm/radeon/sid.h
@@ -736,7 +736,7 @@
 #       define DESCRIPTION16(x)                          (((x) & 0xff) << 0)
 #       define DESCRIPTION17(x)                          (((x) & 0xff) << 8)
 
-#define AZ_F0_CODEC_PIN_CONTROL_HOTPLUG_CONTROL          0x54
+#define AZ_F0_CODEC_PIN_CONTROL_HOT_PLUG_CONTROL         0x54
 #       define AUDIO_ENABLED                             (1 << 31)
 
 #define AZ_F0_CODEC_PIN_CONTROL_RESPONSE_CONFIGURATION_DEFAULT  0x56
diff --git a/drivers/gpu/drm/radeon/sumo_dpm.c b/drivers/gpu/drm/radeon/sumo_dpm.c
index 3f0e8d7b8dbe..1f8a8833e1be 100644
--- a/drivers/gpu/drm/radeon/sumo_dpm.c
+++ b/drivers/gpu/drm/radeon/sumo_dpm.c
@@ -23,6 +23,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "sumod.h"
 #include "r600_dpm.h"
 #include "cypress_dpm.h"
diff --git a/drivers/gpu/drm/radeon/trinity_dpm.c b/drivers/gpu/drm/radeon/trinity_dpm.c
index 32e50be9c4ac..b4ec5c4e7969 100644
--- a/drivers/gpu/drm/radeon/trinity_dpm.c
+++ b/drivers/gpu/drm/radeon/trinity_dpm.c
@@ -23,6 +23,7 @@
 
 #include "drmP.h"
 #include "radeon.h"
+#include "radeon_asic.h"
 #include "trinityd.h"
 #include "r600_dpm.h"
 #include "trinity_dpm.h"
@@ -1874,16 +1875,22 @@ int trinity_dpm_init(struct radeon_device *rdev)
 	for (i = 0; i < SUMO_MAX_HARDWARE_POWERLEVELS; i++)
 		pi->at[i] = TRINITY_AT_DFLT;
 
-	/* There are stability issues reported on with
-	 * bapm enabled when switching between AC and battery
-	 * power.  At the same time, some MSI boards hang
-	 * if it's not enabled and dpm is enabled.  Just enable
-	 * it for MSI boards right now.
-	 */
-	if (rdev->pdev->subsystem_vendor == 0x1462)
-		pi->enable_bapm = true;
-	else
+	if (radeon_bapm == -1) {
+		/* There are stability issues reported on with
+		 * bapm enabled when switching between AC and battery
+		 * power.  At the same time, some MSI boards hang
+		 * if it's not enabled and dpm is enabled.  Just enable
+		 * it for MSI boards right now.
+		 */
+		if (rdev->pdev->subsystem_vendor == 0x1462)
+			pi->enable_bapm = true;
+		else
+			pi->enable_bapm = false;
+	} else if (radeon_bapm == 0) {
 		pi->enable_bapm = false;
+	} else {
+		pi->enable_bapm = true;
+	}
 	pi->enable_nbps_policy = true;
 	pi->enable_sclk_ds = true;
 	pi->enable_gfx_power_gating = true;
diff --git a/drivers/gpu/drm/radeon/uvd_v1_0.c b/drivers/gpu/drm/radeon/uvd_v1_0.c
index be42c8125203..e72b3cb59358 100644
--- a/drivers/gpu/drm/radeon/uvd_v1_0.c
+++ b/drivers/gpu/drm/radeon/uvd_v1_0.c
@@ -22,6 +22,7 @@
  * Authors: Christian König <christian.koenig@amd.com>
  */
 
+#include <linux/firmware.h>
 #include <drm/drmP.h>
 #include "radeon.h"
 #include "radeon_asic.h"
@@ -70,6 +71,82 @@ void uvd_v1_0_set_wptr(struct radeon_device *rdev,
 }
 
 /**
+ * uvd_v1_0_fence_emit - emit an fence & trap command
+ *
+ * @rdev: radeon_device pointer
+ * @fence: fence to emit
+ *
+ * Write a fence and a trap command to the ring.
+ */
+void uvd_v1_0_fence_emit(struct radeon_device *rdev,
+			 struct radeon_fence *fence)
+{
+	struct radeon_ring *ring = &rdev->ring[fence->ring];
+	uint64_t addr = rdev->fence_drv[fence->ring].gpu_addr;
+
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA0, 0));
+	radeon_ring_write(ring, addr & 0xffffffff);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA1, 0));
+	radeon_ring_write(ring, fence->seq);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_CMD, 0));
+	radeon_ring_write(ring, 0);
+
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA0, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_DATA1, 0));
+	radeon_ring_write(ring, 0);
+	radeon_ring_write(ring, PACKET0(UVD_GPCOM_VCPU_CMD, 0));
+	radeon_ring_write(ring, 2);
+	return;
+}
+
+/**
+ * uvd_v1_0_resume - memory controller programming
+ *
+ * @rdev: radeon_device pointer
+ *
+ * Let the UVD memory controller know it's offsets
+ */
+int uvd_v1_0_resume(struct radeon_device *rdev)
+{
+	uint64_t addr;
+	uint32_t size;
+	int r;
+
+	r = radeon_uvd_resume(rdev);
+	if (r)
+		return r;
+
+	/* programm the VCPU memory controller bits 0-27 */
+	addr = (rdev->uvd.gpu_addr >> 3) + 16;
+	size = RADEON_GPU_PAGE_ALIGN(rdev->uvd_fw->size) >> 3;
+	WREG32(UVD_VCPU_CACHE_OFFSET0, addr);
+	WREG32(UVD_VCPU_CACHE_SIZE0, size);
+
+	addr += size;
+	size = RADEON_UVD_STACK_SIZE >> 3;
+	WREG32(UVD_VCPU_CACHE_OFFSET1, addr);
+	WREG32(UVD_VCPU_CACHE_SIZE1, size);
+
+	addr += size;
+	size = RADEON_UVD_HEAP_SIZE >> 3;
+	WREG32(UVD_VCPU_CACHE_OFFSET2, addr);
+	WREG32(UVD_VCPU_CACHE_SIZE2, size);
+
+	/* bits 28-31 */
+	addr = (rdev->uvd.gpu_addr >> 28) & 0xF;
+	WREG32(UVD_LMI_ADDR_EXT, (addr << 12) | (addr << 0));
+
+	/* bits 32-39 */
+	addr = (rdev->uvd.gpu_addr >> 32) & 0xFF;
+	WREG32(UVD_LMI_EXT40_ADDR, addr | (0x9 << 16) | (0x1 << 31));
+
+	WREG32(UVD_FW_START, *((uint32_t*)rdev->uvd.cpu_addr));
+
+	return 0;
+}
+
+/**
  * uvd_v1_0_init - start and test UVD block
  *
  * @rdev: radeon_device pointer
@@ -124,14 +201,38 @@ int uvd_v1_0_init(struct radeon_device *rdev)
 	radeon_ring_write(ring, PACKET0(UVD_SEMA_CNTL, 0));
 	radeon_ring_write(ring, 3);
 
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 
 done:
 	/* lower clocks again */
 	radeon_set_uvd_clocks(rdev, 0, 0);
 
-	if (!r)
+	if (!r) {
+		switch (rdev->family) {
+		case CHIP_RV610:
+		case CHIP_RV630:
+		case CHIP_RV620:
+			/* 64byte granularity workaround */
+			WREG32(MC_CONFIG, 0);
+			WREG32(MC_CONFIG, 1 << 4);
+			WREG32(RS_DQ_RD_RET_CONF, 0x3f);
+			WREG32(MC_CONFIG, 0x1f);
+
+			/* fall through */
+		case CHIP_RV670:
+		case CHIP_RV635:
+
+			/* write clean workaround */
+			WREG32_P(UVD_VCPU_CNTL, 0x10, ~0x10);
+			break;
+
+		default:
+			/* TODO: Do we need more? */
+			break;
+		}
+
 		DRM_INFO("UVD initialized successfully.\n");
+	}
 
 	return r;
 }
@@ -218,12 +319,12 @@ int uvd_v1_0_start(struct radeon_device *rdev)
 	/* enable UMC */
 	WREG32_P(UVD_LMI_CTRL2, 0, ~(1 << 8));
 
+	WREG32_P(UVD_RB_ARB_CTRL, 0, ~(1 << 3));
+
 	/* boot up the VCPU */
 	WREG32(UVD_SOFT_RESET, 0);
 	mdelay(10);
 
-	WREG32_P(UVD_RB_ARB_CTRL, 0, ~(1 << 3));
-
 	for (i = 0; i < 10; ++i) {
 		uint32_t status;
 		for (j = 0; j < 100; ++j) {
@@ -331,7 +432,7 @@ int uvd_v1_0_ring_test(struct radeon_device *rdev, struct radeon_ring *ring)
 	}
 	radeon_ring_write(ring, PACKET0(UVD_CONTEXT_ID, 0));
 	radeon_ring_write(ring, 0xDEADBEEF);
-	radeon_ring_unlock_commit(rdev, ring);
+	radeon_ring_unlock_commit(rdev, ring, false);
 	for (i = 0; i < rdev->usec_timeout; i++) {
 		tmp = RREG32(UVD_CONTEXT_ID);
 		if (tmp == 0xDEADBEEF)
diff --git a/drivers/gpu/drm/radeon/uvd_v2_2.c b/drivers/gpu/drm/radeon/uvd_v2_2.c
index 8bfdadd56598..89193519f8a1 100644
--- a/drivers/gpu/drm/radeon/uvd_v2_2.c
+++ b/drivers/gpu/drm/radeon/uvd_v2_2.c
@@ -72,6 +72,10 @@ int uvd_v2_2_resume(struct radeon_device *rdev)
 	uint32_t chip_id, size;
 	int r;
 
+	/* RV770 uses V1.0 MC */
+	if (rdev->family == CHIP_RV770)
+		return uvd_v1_0_resume(rdev);
+
 	r = radeon_uvd_resume(rdev);
 	if (r)
 		return r;
diff --git a/drivers/gpu/drm/rcar-du/Kconfig b/drivers/gpu/drm/rcar-du/Kconfig
index 2e3d7b5b0ad7..c96f6089f8bf 100644
--- a/drivers/gpu/drm/rcar-du/Kconfig
+++ b/drivers/gpu/drm/rcar-du/Kconfig
@@ -6,6 +6,7 @@ config DRM_RCAR_DU
 	select DRM_KMS_CMA_HELPER
 	select DRM_GEM_CMA_HELPER
 	select DRM_KMS_FB_HELPER
+	select VIDEOMODE_HELPERS
 	help
 	  Choose this option if you have an R-Car chipset.
 	  If M is selected the module will be called rcar-du-drm.
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_crtc.c b/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
index 299267db2898..148b50589181 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_crtc.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_crtc.c  --  R-Car Display Unit CRTCs
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_crtc.h b/drivers/gpu/drm/rcar-du/rcar_du_crtc.h
index 43e7575c700c..e97ae502dec5 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_crtc.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_crtc.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_crtc.h  --  R-Car Display Unit CRTCs
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_drv.c b/drivers/gpu/drm/rcar-du/rcar_du_drv.c
index fda64b7b73e8..d212efa6a495 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_drv.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_drv.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_drv.c  --  R-Car Display Unit DRM driver
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -15,6 +15,7 @@
 #include <linux/io.h>
 #include <linux/mm.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/pm.h>
 #include <linux/slab.h>
@@ -30,6 +31,97 @@
 #include "rcar_du_regs.h"
 
 /* -----------------------------------------------------------------------------
+ * Device Information
+ */
+
+static const struct rcar_du_device_info rcar_du_r8a7779_info = {
+	.features = 0,
+	.num_crtcs = 2,
+	.routes = {
+		/* R8A7779 has two RGB outputs and one (currently unsupported)
+		 * TCON output.
+		 */
+		[RCAR_DU_OUTPUT_DPAD0] = {
+			.possible_crtcs = BIT(0),
+			.encoder_type = DRM_MODE_ENCODER_NONE,
+			.port = 0,
+		},
+		[RCAR_DU_OUTPUT_DPAD1] = {
+			.possible_crtcs = BIT(1) | BIT(0),
+			.encoder_type = DRM_MODE_ENCODER_NONE,
+			.port = 1,
+		},
+	},
+	.num_lvds = 0,
+};
+
+static const struct rcar_du_device_info rcar_du_r8a7790_info = {
+	.features = RCAR_DU_FEATURE_CRTC_IRQ_CLOCK | RCAR_DU_FEATURE_DEFR8,
+	.quirks = RCAR_DU_QUIRK_ALIGN_128B | RCAR_DU_QUIRK_LVDS_LANES,
+	.num_crtcs = 3,
+	.routes = {
+		/* R8A7790 has one RGB output, two LVDS outputs and one
+		 * (currently unsupported) TCON output.
+		 */
+		[RCAR_DU_OUTPUT_DPAD0] = {
+			.possible_crtcs = BIT(2) | BIT(1) | BIT(0),
+			.encoder_type = DRM_MODE_ENCODER_NONE,
+			.port = 0,
+		},
+		[RCAR_DU_OUTPUT_LVDS0] = {
+			.possible_crtcs = BIT(0),
+			.encoder_type = DRM_MODE_ENCODER_LVDS,
+			.port = 1,
+		},
+		[RCAR_DU_OUTPUT_LVDS1] = {
+			.possible_crtcs = BIT(2) | BIT(1),
+			.encoder_type = DRM_MODE_ENCODER_LVDS,
+			.port = 2,
+		},
+	},
+	.num_lvds = 2,
+};
+
+static const struct rcar_du_device_info rcar_du_r8a7791_info = {
+	.features = RCAR_DU_FEATURE_CRTC_IRQ_CLOCK | RCAR_DU_FEATURE_DEFR8,
+	.num_crtcs = 2,
+	.routes = {
+		/* R8A7791 has one RGB output, one LVDS output and one
+		 * (currently unsupported) TCON output.
+		 */
+		[RCAR_DU_OUTPUT_DPAD0] = {
+			.possible_crtcs = BIT(1),
+			.encoder_type = DRM_MODE_ENCODER_NONE,
+			.port = 0,
+		},
+		[RCAR_DU_OUTPUT_LVDS0] = {
+			.possible_crtcs = BIT(0),
+			.encoder_type = DRM_MODE_ENCODER_LVDS,
+			.port = 1,
+		},
+	},
+	.num_lvds = 1,
+};
+
+static const struct platform_device_id rcar_du_id_table[] = {
+	{ "rcar-du-r8a7779", (kernel_ulong_t)&rcar_du_r8a7779_info },
+	{ "rcar-du-r8a7790", (kernel_ulong_t)&rcar_du_r8a7790_info },
+	{ "rcar-du-r8a7791", (kernel_ulong_t)&rcar_du_r8a7791_info },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(platform, rcar_du_id_table);
+
+static const struct of_device_id rcar_du_of_table[] = {
+	{ .compatible = "renesas,du-r8a7779", .data = &rcar_du_r8a7779_info },
+	{ .compatible = "renesas,du-r8a7790", .data = &rcar_du_r8a7790_info },
+	{ .compatible = "renesas,du-r8a7791", .data = &rcar_du_r8a7791_info },
+	{ }
+};
+
+MODULE_DEVICE_TABLE(of, rcar_du_of_table);
+
+/* -----------------------------------------------------------------------------
  * DRM operations
  */
 
@@ -53,12 +145,13 @@ static int rcar_du_unload(struct drm_device *dev)
 static int rcar_du_load(struct drm_device *dev, unsigned long flags)
 {
 	struct platform_device *pdev = dev->platformdev;
+	struct device_node *np = pdev->dev.of_node;
 	struct rcar_du_platform_data *pdata = pdev->dev.platform_data;
 	struct rcar_du_device *rcdu;
 	struct resource *mem;
 	int ret;
 
-	if (pdata == NULL) {
+	if (pdata == NULL && np == NULL) {
 		dev_err(dev->dev, "no platform data\n");
 		return -ENODEV;
 	}
@@ -71,7 +164,8 @@ static int rcar_du_load(struct drm_device *dev, unsigned long flags)
 
 	rcdu->dev = &pdev->dev;
 	rcdu->pdata = pdata;
-	rcdu->info = (struct rcar_du_device_info *)pdev->id_entry->driver_data;
+	rcdu->info = np ? of_match_device(rcar_du_of_table, rcdu->dev)->data
+		   : (void *)platform_get_device_id(pdev)->driver_data;
 	rcdu->ddev = dev;
 	dev->dev_private = rcdu;
 
@@ -158,6 +252,7 @@ static struct drm_driver rcar_du_driver = {
 	.unload			= rcar_du_unload,
 	.preclose		= rcar_du_preclose,
 	.lastclose		= rcar_du_lastclose,
+	.set_busid		= drm_platform_set_busid,
 	.get_vblank_counter	= drm_vblank_count,
 	.enable_vblank		= rcar_du_enable_vblank,
 	.disable_vblank		= rcar_du_disable_vblank,
@@ -231,77 +326,6 @@ static int rcar_du_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct rcar_du_device_info rcar_du_r8a7779_info = {
-	.features = 0,
-	.num_crtcs = 2,
-	.routes = {
-		/* R8A7779 has two RGB outputs and one (currently unsupported)
-		 * TCON output.
-		 */
-		[RCAR_DU_OUTPUT_DPAD0] = {
-			.possible_crtcs = BIT(0),
-			.encoder_type = DRM_MODE_ENCODER_NONE,
-		},
-		[RCAR_DU_OUTPUT_DPAD1] = {
-			.possible_crtcs = BIT(1) | BIT(0),
-			.encoder_type = DRM_MODE_ENCODER_NONE,
-		},
-	},
-	.num_lvds = 0,
-};
-
-static const struct rcar_du_device_info rcar_du_r8a7790_info = {
-	.features = RCAR_DU_FEATURE_CRTC_IRQ_CLOCK | RCAR_DU_FEATURE_DEFR8,
-	.quirks = RCAR_DU_QUIRK_ALIGN_128B | RCAR_DU_QUIRK_LVDS_LANES,
-	.num_crtcs = 3,
-	.routes = {
-		/* R8A7790 has one RGB output, two LVDS outputs and one
-		 * (currently unsupported) TCON output.
-		 */
-		[RCAR_DU_OUTPUT_DPAD0] = {
-			.possible_crtcs = BIT(2) | BIT(1) | BIT(0),
-			.encoder_type = DRM_MODE_ENCODER_NONE,
-		},
-		[RCAR_DU_OUTPUT_LVDS0] = {
-			.possible_crtcs = BIT(0),
-			.encoder_type = DRM_MODE_ENCODER_LVDS,
-		},
-		[RCAR_DU_OUTPUT_LVDS1] = {
-			.possible_crtcs = BIT(2) | BIT(1),
-			.encoder_type = DRM_MODE_ENCODER_LVDS,
-		},
-	},
-	.num_lvds = 2,
-};
-
-static const struct rcar_du_device_info rcar_du_r8a7791_info = {
-	.features = RCAR_DU_FEATURE_CRTC_IRQ_CLOCK | RCAR_DU_FEATURE_DEFR8,
-	.num_crtcs = 2,
-	.routes = {
-		/* R8A7791 has one RGB output, one LVDS output and one
-		 * (currently unsupported) TCON output.
-		 */
-		[RCAR_DU_OUTPUT_DPAD0] = {
-			.possible_crtcs = BIT(1),
-			.encoder_type = DRM_MODE_ENCODER_NONE,
-		},
-		[RCAR_DU_OUTPUT_LVDS0] = {
-			.possible_crtcs = BIT(0),
-			.encoder_type = DRM_MODE_ENCODER_LVDS,
-		},
-	},
-	.num_lvds = 1,
-};
-
-static const struct platform_device_id rcar_du_id_table[] = {
-	{ "rcar-du-r8a7779", (kernel_ulong_t)&rcar_du_r8a7779_info },
-	{ "rcar-du-r8a7790", (kernel_ulong_t)&rcar_du_r8a7790_info },
-	{ "rcar-du-r8a7791", (kernel_ulong_t)&rcar_du_r8a7791_info },
-	{ }
-};
-
-MODULE_DEVICE_TABLE(platform, rcar_du_id_table);
-
 static struct platform_driver rcar_du_platform_driver = {
 	.probe		= rcar_du_probe,
 	.remove		= rcar_du_remove,
@@ -309,6 +333,7 @@ static struct platform_driver rcar_du_platform_driver = {
 		.owner	= THIS_MODULE,
 		.name	= "rcar-du",
 		.pm	= &rcar_du_pm_ops,
+		.of_match_table = rcar_du_of_table,
 	},
 	.id_table	= rcar_du_id_table,
 };
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_drv.h b/drivers/gpu/drm/rcar-du/rcar_du_drv.h
index e31b735d3f25..8e494633c3b3 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_drv.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_drv.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_drv.h  --  R-Car Display Unit DRM driver
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -37,6 +37,7 @@ struct rcar_du_lvdsenc;
  * struct rcar_du_output_routing - Output routing specification
  * @possible_crtcs: bitmask of possible CRTCs for the output
  * @encoder_type: DRM type of the internal encoder associated with the output
+ * @port: device tree port number corresponding to this output route
  *
  * The DU has 5 possible outputs (DPAD0/1, LVDS0/1, TCON). Output routing data
  * specify the valid SoC outputs, which CRTCs can drive the output, and the type
@@ -45,6 +46,7 @@ struct rcar_du_lvdsenc;
 struct rcar_du_output_routing {
 	unsigned int possible_crtcs;
 	unsigned int encoder_type;
+	unsigned int port;
 };
 
 /*
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_encoder.c b/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
index 3daa7a168dc6..7c0ec95915ef 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_encoder.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_encoder.c  --  R-Car Display Unit Encoder
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -142,7 +142,8 @@ static const struct drm_encoder_funcs encoder_funcs = {
 int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 			 enum rcar_du_encoder_type type,
 			 enum rcar_du_output output,
-			 const struct rcar_du_encoder_data *data)
+			 const struct rcar_du_encoder_data *data,
+			 struct device_node *np)
 {
 	struct rcar_du_encoder *renc;
 	unsigned int encoder_type;
@@ -189,9 +190,11 @@ int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 	drm_encoder_helper_add(&renc->encoder, &encoder_helper_funcs);
 
 	switch (encoder_type) {
-	case DRM_MODE_ENCODER_LVDS:
-		return rcar_du_lvds_connector_init(rcdu, renc,
-						   &data->connector.lvds.panel);
+	case DRM_MODE_ENCODER_LVDS: {
+		const struct rcar_du_panel_data *pdata =
+			data ? &data->connector.lvds.panel : NULL;
+		return rcar_du_lvds_connector_init(rcdu, renc, pdata, np);
+	}
 
 	case DRM_MODE_ENCODER_DAC:
 		return rcar_du_vga_connector_init(rcdu, renc);
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_encoder.h b/drivers/gpu/drm/rcar-du/rcar_du_encoder.h
index 0e5a65e45d0e..bd624135ef1f 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_encoder.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_encoder.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_encoder.h  --  R-Car Display Unit Encoder
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -44,6 +44,7 @@ rcar_du_connector_best_encoder(struct drm_connector *connector);
 int rcar_du_encoder_init(struct rcar_du_device *rcdu,
 			 enum rcar_du_encoder_type type,
 			 enum rcar_du_output output,
-			 const struct rcar_du_encoder_data *data);
+			 const struct rcar_du_encoder_data *data,
+			 struct device_node *np);
 
 #endif /* __RCAR_DU_ENCODER_H__ */
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_group.c b/drivers/gpu/drm/rcar-du/rcar_du_group.c
index eb53cd97e8c6..4e7614b145db 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_group.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_group.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_group.c  --  R-Car Display Unit Channels Pair
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_group.h b/drivers/gpu/drm/rcar-du/rcar_du_group.h
index 5025930972ec..0c38cdcda4ca 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_group.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_group.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_group.c  --  R-Car Display Unit Planes and CRTCs Group
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_kms.c b/drivers/gpu/drm/rcar-du/rcar_du_kms.c
index 76026104d000..6c24ad7d03ef 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_kms.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_kms.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_kms.c  --  R-Car Display Unit Mode Setting
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -17,6 +17,8 @@
 #include <drm/drm_fb_cma_helper.h>
 #include <drm/drm_gem_cma_helper.h>
 
+#include <linux/of_graph.h>
+
 #include "rcar_du_crtc.h"
 #include "rcar_du_drv.h"
 #include "rcar_du_encoder.h"
@@ -188,6 +190,205 @@ static const struct drm_mode_config_funcs rcar_du_mode_config_funcs = {
 	.output_poll_changed = rcar_du_output_poll_changed,
 };
 
+static int rcar_du_encoders_init_pdata(struct rcar_du_device *rcdu)
+{
+	unsigned int num_encoders = 0;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; i < rcdu->pdata->num_encoders; ++i) {
+		const struct rcar_du_encoder_data *pdata =
+			&rcdu->pdata->encoders[i];
+		const struct rcar_du_output_routing *route =
+			&rcdu->info->routes[pdata->output];
+
+		if (pdata->type == RCAR_DU_ENCODER_UNUSED)
+			continue;
+
+		if (pdata->output >= RCAR_DU_OUTPUT_MAX ||
+		    route->possible_crtcs == 0) {
+			dev_warn(rcdu->dev,
+				 "encoder %u references unexisting output %u, skipping\n",
+				 i, pdata->output);
+			continue;
+		}
+
+		ret = rcar_du_encoder_init(rcdu, pdata->type, pdata->output,
+					   pdata, NULL);
+		if (ret < 0)
+			return ret;
+
+		num_encoders++;
+	}
+
+	return num_encoders;
+}
+
+static int rcar_du_encoders_init_dt_one(struct rcar_du_device *rcdu,
+					enum rcar_du_output output,
+					struct of_endpoint *ep)
+{
+	static const struct {
+		const char *compatible;
+		enum rcar_du_encoder_type type;
+	} encoders[] = {
+		{ "adi,adv7123", RCAR_DU_ENCODER_VGA },
+		{ "thine,thc63lvdm83d", RCAR_DU_ENCODER_LVDS },
+	};
+
+	enum rcar_du_encoder_type enc_type = RCAR_DU_ENCODER_NONE;
+	struct device_node *connector = NULL;
+	struct device_node *encoder = NULL;
+	struct device_node *prev = NULL;
+	struct device_node *entity_ep_node;
+	struct device_node *entity;
+	int ret;
+
+	/*
+	 * Locate the connected entity and infer its type from the number of
+	 * endpoints.
+	 */
+	entity = of_graph_get_remote_port_parent(ep->local_node);
+	if (!entity) {
+		dev_dbg(rcdu->dev, "unconnected endpoint %s, skipping\n",
+			ep->local_node->full_name);
+		return 0;
+	}
+
+	entity_ep_node = of_parse_phandle(ep->local_node, "remote-endpoint", 0);
+
+	while (1) {
+		struct device_node *ep_node;
+
+		ep_node = of_graph_get_next_endpoint(entity, prev);
+		of_node_put(prev);
+		prev = ep_node;
+
+		if (!ep_node)
+			break;
+
+		if (ep_node == entity_ep_node)
+			continue;
+
+		/*
+		 * We've found one endpoint other than the input, this must
+		 * be an encoder. Locate the connector.
+		 */
+		encoder = entity;
+		connector = of_graph_get_remote_port_parent(ep_node);
+		of_node_put(ep_node);
+
+		if (!connector) {
+			dev_warn(rcdu->dev,
+				 "no connector for encoder %s, skipping\n",
+				 encoder->full_name);
+			of_node_put(entity_ep_node);
+			of_node_put(encoder);
+			return 0;
+		}
+
+		break;
+	}
+
+	of_node_put(entity_ep_node);
+
+	if (encoder) {
+		/*
+		 * If an encoder has been found, get its type based on its
+		 * compatible string.
+		 */
+		unsigned int i;
+
+		for (i = 0; i < ARRAY_SIZE(encoders); ++i) {
+			if (of_device_is_compatible(encoder,
+						    encoders[i].compatible)) {
+				enc_type = encoders[i].type;
+				break;
+			}
+		}
+
+		if (i == ARRAY_SIZE(encoders)) {
+			dev_warn(rcdu->dev,
+				 "unknown encoder type for %s, skipping\n",
+				 encoder->full_name);
+			of_node_put(encoder);
+			of_node_put(connector);
+			return 0;
+		}
+	} else {
+		/*
+		 * If no encoder has been found the entity must be the
+		 * connector.
+		 */
+		connector = entity;
+	}
+
+	ret = rcar_du_encoder_init(rcdu, enc_type, output, NULL, connector);
+	of_node_put(encoder);
+	of_node_put(connector);
+
+	return ret < 0 ? ret : 1;
+}
+
+static int rcar_du_encoders_init_dt(struct rcar_du_device *rcdu)
+{
+	struct device_node *np = rcdu->dev->of_node;
+	struct device_node *prev = NULL;
+	unsigned int num_encoders = 0;
+
+	/*
+	 * Iterate over the endpoints and create one encoder for each output
+	 * pipeline.
+	 */
+	while (1) {
+		struct device_node *ep_node;
+		enum rcar_du_output output;
+		struct of_endpoint ep;
+		unsigned int i;
+		int ret;
+
+		ep_node = of_graph_get_next_endpoint(np, prev);
+		of_node_put(prev);
+		prev = ep_node;
+
+		if (ep_node == NULL)
+			break;
+
+		ret = of_graph_parse_endpoint(ep_node, &ep);
+		if (ret < 0) {
+			of_node_put(ep_node);
+			return ret;
+		}
+
+		/* Find the output route corresponding to the port number. */
+		for (i = 0; i < RCAR_DU_OUTPUT_MAX; ++i) {
+			if (rcdu->info->routes[i].possible_crtcs &&
+			    rcdu->info->routes[i].port == ep.port) {
+				output = i;
+				break;
+			}
+		}
+
+		if (i == RCAR_DU_OUTPUT_MAX) {
+			dev_warn(rcdu->dev,
+				 "port %u references unexisting output, skipping\n",
+				 ep.port);
+			continue;
+		}
+
+		/* Process the output pipeline. */
+		ret = rcar_du_encoders_init_dt_one(rcdu, output, &ep);
+		if (ret < 0) {
+			of_node_put(ep_node);
+			return ret;
+		}
+
+		num_encoders += ret;
+	}
+
+	return num_encoders;
+}
+
 int rcar_du_modeset_init(struct rcar_du_device *rcdu)
 {
 	static const unsigned int mmio_offsets[] = {
@@ -197,6 +398,7 @@ int rcar_du_modeset_init(struct rcar_du_device *rcdu)
 	struct drm_device *dev = rcdu->ddev;
 	struct drm_encoder *encoder;
 	struct drm_fbdev_cma *fbdev;
+	unsigned int num_encoders;
 	unsigned int num_groups;
 	unsigned int i;
 	int ret;
@@ -240,28 +442,15 @@ int rcar_du_modeset_init(struct rcar_du_device *rcdu)
 	if (ret < 0)
 		return ret;
 
-	for (i = 0; i < rcdu->pdata->num_encoders; ++i) {
-		const struct rcar_du_encoder_data *pdata =
-			&rcdu->pdata->encoders[i];
-		const struct rcar_du_output_routing *route =
-			&rcdu->info->routes[pdata->output];
-
-		if (pdata->type == RCAR_DU_ENCODER_UNUSED)
-			continue;
+	if (rcdu->pdata)
+		ret = rcar_du_encoders_init_pdata(rcdu);
+	else
+		ret = rcar_du_encoders_init_dt(rcdu);
 
-		if (pdata->output >= RCAR_DU_OUTPUT_MAX ||
-		    route->possible_crtcs == 0) {
-			dev_warn(rcdu->dev,
-				 "encoder %u references unexisting output %u, skipping\n",
-				 i, pdata->output);
-			continue;
-		}
+	if (ret < 0)
+		return ret;
 
-		ret = rcar_du_encoder_init(rcdu, pdata->type, pdata->output,
-					   pdata);
-		if (ret < 0)
-			return ret;
-	}
+	num_encoders = ret;
 
 	/* Set the possible CRTCs and possible clones. There's always at least
 	 * one way for all encoders to clone each other, set all bits in the
@@ -273,7 +462,7 @@ int rcar_du_modeset_init(struct rcar_du_device *rcdu)
 			&rcdu->info->routes[renc->output];
 
 		encoder->possible_crtcs = route->possible_crtcs;
-		encoder->possible_clones = (1 << rcdu->pdata->num_encoders) - 1;
+		encoder->possible_clones = (1 << num_encoders) - 1;
 	}
 
 	/* Now that the CRTCs have been initialized register the planes. */
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_kms.h b/drivers/gpu/drm/rcar-du/rcar_du_kms.h
index 5750e6af5655..07951d5fe38b 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_kms.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_kms.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_kms.h  --  R-Car Display Unit Mode Setting
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c
index 21426bd234eb..115eed20db12 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_lvdscon.c  --  R-Car Display Unit LVDS Connector
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -15,6 +15,10 @@
 #include <drm/drm_crtc.h>
 #include <drm/drm_crtc_helper.h>
 
+#include <video/display_timing.h>
+#include <video/of_display_timing.h>
+#include <video/videomode.h>
+
 #include "rcar_du_drv.h"
 #include "rcar_du_encoder.h"
 #include "rcar_du_kms.h"
@@ -23,7 +27,7 @@
 struct rcar_du_lvds_connector {
 	struct rcar_du_connector connector;
 
-	const struct rcar_du_panel_data *panel;
+	struct rcar_du_panel_data panel;
 };
 
 #define to_rcar_lvds_connector(c) \
@@ -40,18 +44,9 @@ static int rcar_du_lvds_connector_get_modes(struct drm_connector *connector)
 		return 0;
 
 	mode->type = DRM_MODE_TYPE_PREFERRED | DRM_MODE_TYPE_DRIVER;
-	mode->clock = lvdscon->panel->mode.clock;
-	mode->hdisplay = lvdscon->panel->mode.hdisplay;
-	mode->hsync_start = lvdscon->panel->mode.hsync_start;
-	mode->hsync_end = lvdscon->panel->mode.hsync_end;
-	mode->htotal = lvdscon->panel->mode.htotal;
-	mode->vdisplay = lvdscon->panel->mode.vdisplay;
-	mode->vsync_start = lvdscon->panel->mode.vsync_start;
-	mode->vsync_end = lvdscon->panel->mode.vsync_end;
-	mode->vtotal = lvdscon->panel->mode.vtotal;
-	mode->flags = lvdscon->panel->mode.flags;
-
-	drm_mode_set_name(mode);
+
+	drm_display_mode_from_videomode(&lvdscon->panel.mode, mode);
+
 	drm_mode_probed_add(connector, mode);
 
 	return 1;
@@ -83,7 +78,8 @@ static const struct drm_connector_funcs connector_funcs = {
 
 int rcar_du_lvds_connector_init(struct rcar_du_device *rcdu,
 				struct rcar_du_encoder *renc,
-				const struct rcar_du_panel_data *panel)
+				const struct rcar_du_panel_data *panel,
+				/* TODO const */ struct device_node *np)
 {
 	struct rcar_du_lvds_connector *lvdscon;
 	struct drm_connector *connector;
@@ -93,11 +89,24 @@ int rcar_du_lvds_connector_init(struct rcar_du_device *rcdu,
 	if (lvdscon == NULL)
 		return -ENOMEM;
 
-	lvdscon->panel = panel;
+	if (panel) {
+		lvdscon->panel = *panel;
+	} else {
+		struct display_timing timing;
+
+		ret = of_get_display_timing(np, "panel-timing", &timing);
+		if (ret < 0)
+			return ret;
+
+		videomode_from_timing(&timing, &lvdscon->panel.mode);
+
+		of_property_read_u32(np, "width-mm", &lvdscon->panel.width_mm);
+		of_property_read_u32(np, "height-mm", &lvdscon->panel.height_mm);
+	}
 
 	connector = &lvdscon->connector.connector;
-	connector->display_info.width_mm = panel->width_mm;
-	connector->display_info.height_mm = panel->height_mm;
+	connector->display_info.width_mm = lvdscon->panel.width_mm;
+	connector->display_info.height_mm = lvdscon->panel.height_mm;
 
 	ret = drm_connector_init(rcdu->ddev, connector, &connector_funcs,
 				 DRM_MODE_CONNECTOR_LVDS);
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h
index bff8683699ca..d11424d537f9 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_lvdscon.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_lvdscon.h  --  R-Car Display Unit LVDS Connector
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -20,6 +20,7 @@ struct rcar_du_panel_data;
 
 int rcar_du_lvds_connector_init(struct rcar_du_device *rcdu,
 				struct rcar_du_encoder *renc,
-				const struct rcar_du_panel_data *panel);
+				const struct rcar_du_panel_data *panel,
+				struct device_node *np);
 
 #endif /* __RCAR_DU_LVDSCON_H__ */
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.c b/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.c
index df30a075d793..7cfb48ce1791 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_lvdsenc.c  --  R-Car Display Unit LVDS Encoder
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h b/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h
index 7051c6de19ae..3303a55cec79 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_lvdsenc.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_lvdsenc.h  --  R-Car Display Unit LVDS Encoder
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_plane.c b/drivers/gpu/drm/rcar-du/rcar_du_plane.c
index 3fb69d9ae61b..72a7cb47bd9f 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_plane.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_plane.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_plane.c  --  R-Car Display Unit Planes
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_plane.h b/drivers/gpu/drm/rcar-du/rcar_du_plane.h
index f94f9ce84998..3021288b1a89 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_plane.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_plane.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_plane.h  --  R-Car Display Unit Planes
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c b/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c
index 8af3944d31b9..564a723ede03 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c
+++ b/drivers/gpu/drm/rcar-du/rcar_du_vgacon.c
@@ -1,7 +1,7 @@
 /*
  * rcar_du_vgacon.c  --  R-Car Display Unit VGA Connector
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/rcar-du/rcar_du_vgacon.h b/drivers/gpu/drm/rcar-du/rcar_du_vgacon.h
index b12b0cf7f117..112f50316e01 100644
--- a/drivers/gpu/drm/rcar-du/rcar_du_vgacon.h
+++ b/drivers/gpu/drm/rcar-du/rcar_du_vgacon.h
@@ -1,7 +1,7 @@
 /*
  * rcar_du_vgacon.h  --  R-Car Display Unit VGA Connector
  *
- * Copyright (C) 2013 Renesas Corporation
+ * Copyright (C) 2013-2014 Renesas Electronics Corporation
  *
  * Contact: Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/savage/savage_bci.c b/drivers/gpu/drm/savage/savage_bci.c
index c97cdc9ab239..d47dff95fe52 100644
--- a/drivers/gpu/drm/savage/savage_bci.c
+++ b/drivers/gpu/drm/savage/savage_bci.c
@@ -556,7 +556,7 @@ int savage_driver_load(struct drm_device *dev, unsigned long chipset)
 /*
  * Initialize mappings. On Savage4 and SavageIX the alignment
  * and size of the aperture is not suitable for automatic MTRR setup
- * in drm_addmap. Therefore we add them manually before the maps are
+ * in drm_legacy_addmap. Therefore we add them manually before the maps are
  * initialized, and tear them down on last close.
  */
 int savage_driver_firstopen(struct drm_device *dev)
@@ -624,19 +624,20 @@ int savage_driver_firstopen(struct drm_device *dev)
 		/* Automatic MTRR setup will do the right thing. */
 	}
 
-	ret = drm_addmap(dev, mmio_base, SAVAGE_MMIO_SIZE, _DRM_REGISTERS,
-			 _DRM_READ_ONLY, &dev_priv->mmio);
+	ret = drm_legacy_addmap(dev, mmio_base, SAVAGE_MMIO_SIZE,
+				_DRM_REGISTERS, _DRM_READ_ONLY,
+				&dev_priv->mmio);
 	if (ret)
 		return ret;
 
-	ret = drm_addmap(dev, fb_base, fb_size, _DRM_FRAME_BUFFER,
-			 _DRM_WRITE_COMBINING, &dev_priv->fb);
+	ret = drm_legacy_addmap(dev, fb_base, fb_size, _DRM_FRAME_BUFFER,
+				_DRM_WRITE_COMBINING, &dev_priv->fb);
 	if (ret)
 		return ret;
 
-	ret = drm_addmap(dev, aperture_base, SAVAGE_APERTURE_SIZE,
-			 _DRM_FRAME_BUFFER, _DRM_WRITE_COMBINING,
-			 &dev_priv->aperture);
+	ret = drm_legacy_addmap(dev, aperture_base, SAVAGE_APERTURE_SIZE,
+				_DRM_FRAME_BUFFER, _DRM_WRITE_COMBINING,
+				&dev_priv->aperture);
 	return ret;
 }
 
@@ -698,14 +699,14 @@ static int savage_do_init_bci(struct drm_device * dev, drm_savage_init_t * init)
 	dev_priv->texture_offset = init->texture_offset;
 	dev_priv->texture_size = init->texture_size;
 
-	dev_priv->sarea = drm_getsarea(dev);
+	dev_priv->sarea = drm_legacy_getsarea(dev);
 	if (!dev_priv->sarea) {
 		DRM_ERROR("could not find sarea!\n");
 		savage_do_cleanup_bci(dev);
 		return -EINVAL;
 	}
 	if (init->status_offset != 0) {
-		dev_priv->status = drm_core_findmap(dev, init->status_offset);
+		dev_priv->status = drm_legacy_findmap(dev, init->status_offset);
 		if (!dev_priv->status) {
 			DRM_ERROR("could not find shadow status region!\n");
 			savage_do_cleanup_bci(dev);
@@ -716,14 +717,14 @@ static int savage_do_init_bci(struct drm_device * dev, drm_savage_init_t * init)
 	}
 	if (dev_priv->dma_type == SAVAGE_DMA_AGP && init->buffers_offset) {
 		dev->agp_buffer_token = init->buffers_offset;
-		dev->agp_buffer_map = drm_core_findmap(dev,
+		dev->agp_buffer_map = drm_legacy_findmap(dev,
 						       init->buffers_offset);
 		if (!dev->agp_buffer_map) {
 			DRM_ERROR("could not find DMA buffer region!\n");
 			savage_do_cleanup_bci(dev);
 			return -EINVAL;
 		}
-		drm_core_ioremap(dev->agp_buffer_map, dev);
+		drm_legacy_ioremap(dev->agp_buffer_map, dev);
 		if (!dev->agp_buffer_map->handle) {
 			DRM_ERROR("failed to ioremap DMA buffer region!\n");
 			savage_do_cleanup_bci(dev);
@@ -732,7 +733,7 @@ static int savage_do_init_bci(struct drm_device * dev, drm_savage_init_t * init)
 	}
 	if (init->agp_textures_offset) {
 		dev_priv->agp_textures =
-		    drm_core_findmap(dev, init->agp_textures_offset);
+		    drm_legacy_findmap(dev, init->agp_textures_offset);
 		if (!dev_priv->agp_textures) {
 			DRM_ERROR("could not find agp texture region!\n");
 			savage_do_cleanup_bci(dev);
@@ -755,7 +756,7 @@ static int savage_do_init_bci(struct drm_device * dev, drm_savage_init_t * init)
 			savage_do_cleanup_bci(dev);
 			return -EINVAL;
 		}
-		dev_priv->cmd_dma = drm_core_findmap(dev, init->cmd_dma_offset);
+		dev_priv->cmd_dma = drm_legacy_findmap(dev, init->cmd_dma_offset);
 		if (!dev_priv->cmd_dma) {
 			DRM_ERROR("could not find command DMA region!\n");
 			savage_do_cleanup_bci(dev);
@@ -768,7 +769,7 @@ static int savage_do_init_bci(struct drm_device * dev, drm_savage_init_t * init)
 				savage_do_cleanup_bci(dev);
 				return -EINVAL;
 			}
-			drm_core_ioremap(dev_priv->cmd_dma, dev);
+			drm_legacy_ioremap(dev_priv->cmd_dma, dev);
 			if (!dev_priv->cmd_dma->handle) {
 				DRM_ERROR("failed to ioremap command "
 					  "DMA region!\n");
@@ -894,11 +895,11 @@ static int savage_do_cleanup_bci(struct drm_device * dev)
 	} else if (dev_priv->cmd_dma && dev_priv->cmd_dma->handle &&
 		   dev_priv->cmd_dma->type == _DRM_AGP &&
 		   dev_priv->dma_type == SAVAGE_DMA_AGP)
-		drm_core_ioremapfree(dev_priv->cmd_dma, dev);
+		drm_legacy_ioremapfree(dev_priv->cmd_dma, dev);
 
 	if (dev_priv->dma_type == SAVAGE_DMA_AGP &&
 	    dev->agp_buffer_map && dev->agp_buffer_map->handle) {
-		drm_core_ioremapfree(dev->agp_buffer_map, dev);
+		drm_legacy_ioremapfree(dev->agp_buffer_map, dev);
 		/* make sure the next instance (which may be running
 		 * in PCI mode) doesn't try to use an old
 		 * agp_buffer_map. */
@@ -1050,7 +1051,7 @@ void savage_reclaim_buffers(struct drm_device *dev, struct drm_file *file_priv)
 		return;
 
 	if (file_priv->master && file_priv->master->lock.hw_lock) {
-		drm_idlelock_take(&file_priv->master->lock);
+		drm_legacy_idlelock_take(&file_priv->master->lock);
 		release_idlelock = 1;
 	}
 
@@ -1069,7 +1070,7 @@ void savage_reclaim_buffers(struct drm_device *dev, struct drm_file *file_priv)
 	}
 
 	if (release_idlelock)
-		drm_idlelock_release(&file_priv->master->lock);
+		drm_legacy_idlelock_release(&file_priv->master->lock);
 }
 
 const struct drm_ioctl_desc savage_ioctls[] = {
diff --git a/drivers/gpu/drm/savage/savage_drv.c b/drivers/gpu/drm/savage/savage_drv.c
index 3c030216e888..21aed1febeb4 100644
--- a/drivers/gpu/drm/savage/savage_drv.c
+++ b/drivers/gpu/drm/savage/savage_drv.c
@@ -40,7 +40,7 @@ static const struct file_operations savage_driver_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = drm_compat_ioctl,
@@ -57,6 +57,7 @@ static struct drm_driver driver = {
 	.preclose = savage_reclaim_buffers,
 	.lastclose = savage_driver_lastclose,
 	.unload = savage_driver_unload,
+	.set_busid = drm_pci_set_busid,
 	.ioctls = savage_ioctls,
 	.dma_ioctl = savage_bci_buffers,
 	.fops = &savage_driver_fops,
diff --git a/drivers/gpu/drm/savage/savage_drv.h b/drivers/gpu/drm/savage/savage_drv.h
index 335f8fcf1041..37b699571ad0 100644
--- a/drivers/gpu/drm/savage/savage_drv.h
+++ b/drivers/gpu/drm/savage/savage_drv.h
@@ -26,6 +26,8 @@
 #ifndef __SAVAGE_DRV_H__
 #define __SAVAGE_DRV_H__
 
+#include <drm/drm_legacy.h>
+
 #define DRIVER_AUTHOR	"Felix Kuehling"
 
 #define DRIVER_NAME	"savage"
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_backlight.c b/drivers/gpu/drm/shmobile/shmob_drm_backlight.c
index 463aee18f774..33dd41afea0e 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_backlight.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_backlight.c
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_backlight.c  --  SH Mobile DRM Backlight
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_backlight.h b/drivers/gpu/drm/shmobile/shmob_drm_backlight.h
index 9477595d2ff3..bac719ecc301 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_backlight.h
+++ b/drivers/gpu/drm/shmobile/shmob_drm_backlight.h
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_backlight.h  --  SH Mobile DRM Backlight
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
index 47875de89010..0ddce4d046d9 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_crtc.c
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_crtc.c  --  SH Mobile DRM CRTCs
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_crtc.h b/drivers/gpu/drm/shmobile/shmob_drm_crtc.h
index e5bd109c4c38..eddad6dcc88a 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_crtc.h
+++ b/drivers/gpu/drm/shmobile/shmob_drm_crtc.h
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_crtc.h  --  SH Mobile DRM CRTCs
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_drv.c b/drivers/gpu/drm/shmobile/shmob_drm_drv.c
index ff4ba483b602..e62cbde81e50 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_drv.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_drv.c
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_drv.c  --  SH Mobile DRM driver
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
@@ -267,6 +267,7 @@ static struct drm_driver shmob_drm_driver = {
 	.load			= shmob_drm_load,
 	.unload			= shmob_drm_unload,
 	.preclose		= shmob_drm_preclose,
+	.set_busid		= drm_platform_set_busid,
 	.irq_handler		= shmob_drm_irq,
 	.get_vblank_counter	= drm_vblank_count,
 	.enable_vblank		= shmob_drm_enable_vblank,
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_drv.h b/drivers/gpu/drm/shmobile/shmob_drm_drv.h
index 4d46b811b5a7..02ea315ba69a 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_drv.h
+++ b/drivers/gpu/drm/shmobile/shmob_drm_drv.h
@@ -1,7 +1,7 @@
 /*
  * shmob_drm.h  --  SH Mobile DRM driver
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_kms.c b/drivers/gpu/drm/shmobile/shmob_drm_kms.c
index fc0ef0ca7d04..aaf98ace4a90 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_kms.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_kms.c
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_kms.c  --  SH Mobile DRM Mode Setting
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_kms.h b/drivers/gpu/drm/shmobile/shmob_drm_kms.h
index 9495c9111308..06d5b7caa026 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_kms.h
+++ b/drivers/gpu/drm/shmobile/shmob_drm_kms.h
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_kms.h  --  SH Mobile DRM Mode Setting
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_plane.c b/drivers/gpu/drm/shmobile/shmob_drm_plane.c
index 060ae03e5f9b..1805bb23b113 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_plane.c
+++ b/drivers/gpu/drm/shmobile/shmob_drm_plane.c
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_plane.c  --  SH Mobile DRM Planes
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_plane.h b/drivers/gpu/drm/shmobile/shmob_drm_plane.h
index 99623d05e3b0..a58cc1fc3240 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_plane.h
+++ b/drivers/gpu/drm/shmobile/shmob_drm_plane.h
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_plane.h  --  SH Mobile DRM Planes
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/shmobile/shmob_drm_regs.h b/drivers/gpu/drm/shmobile/shmob_drm_regs.h
index 7923cdd6368e..ea17d4415b9e 100644
--- a/drivers/gpu/drm/shmobile/shmob_drm_regs.h
+++ b/drivers/gpu/drm/shmobile/shmob_drm_regs.h
@@ -1,7 +1,7 @@
 /*
  * shmob_drm_regs.h  --  SH Mobile DRM registers
  *
- * Copyright (C) 2012 Renesas Corporation
+ * Copyright (C) 2012 Renesas Electronics Corporation
  *
  * Laurent Pinchart (laurent.pinchart@ideasonboard.com)
  *
diff --git a/drivers/gpu/drm/sis/sis_drv.c b/drivers/gpu/drm/sis/sis_drv.c
index 756f787b7143..79bce76cb8f7 100644
--- a/drivers/gpu/drm/sis/sis_drv.c
+++ b/drivers/gpu/drm/sis/sis_drv.c
@@ -70,7 +70,7 @@ static const struct file_operations sis_driver_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = drm_compat_ioctl,
@@ -108,6 +108,7 @@ static struct drm_driver driver = {
 	.open = sis_driver_open,
 	.preclose = sis_reclaim_buffers_locked,
 	.postclose = sis_driver_postclose,
+	.set_busid = drm_pci_set_busid,
 	.dma_quiescent = sis_idle,
 	.lastclose = sis_lastclose,
 	.ioctls = sis_ioctls,
diff --git a/drivers/gpu/drm/sis/sis_drv.h b/drivers/gpu/drm/sis/sis_drv.h
index c31c0253054d..16f972b2a76a 100644
--- a/drivers/gpu/drm/sis/sis_drv.h
+++ b/drivers/gpu/drm/sis/sis_drv.h
@@ -28,6 +28,8 @@
 #ifndef _SIS_DRV_H_
 #define _SIS_DRV_H_
 
+#include <drm/drm_legacy.h>
+
 /* General customization:
  */
 
diff --git a/drivers/gpu/drm/sis/sis_mm.c b/drivers/gpu/drm/sis/sis_mm.c
index 77f288e4a0a6..93ad8a5704d1 100644
--- a/drivers/gpu/drm/sis/sis_mm.c
+++ b/drivers/gpu/drm/sis/sis_mm.c
@@ -319,12 +319,12 @@ void sis_reclaim_buffers_locked(struct drm_device *dev,
 	if (!(file->minor->master && file->master->lock.hw_lock))
 		return;
 
-	drm_idlelock_take(&file->master->lock);
+	drm_legacy_idlelock_take(&file->master->lock);
 
 	mutex_lock(&dev->struct_mutex);
 	if (list_empty(&file_priv->obj_list)) {
 		mutex_unlock(&dev->struct_mutex);
-		drm_idlelock_release(&file->master->lock);
+		drm_legacy_idlelock_release(&file->master->lock);
 
 		return;
 	}
@@ -345,7 +345,7 @@ void sis_reclaim_buffers_locked(struct drm_device *dev,
 	}
 	mutex_unlock(&dev->struct_mutex);
 
-	drm_idlelock_release(&file->master->lock);
+	drm_legacy_idlelock_release(&file->master->lock);
 
 	return;
 }
diff --git a/drivers/gpu/drm/sti/Kconfig b/drivers/gpu/drm/sti/Kconfig
index 2d9d4252d598..ae8850f3e63b 100644
--- a/drivers/gpu/drm/sti/Kconfig
+++ b/drivers/gpu/drm/sti/Kconfig
@@ -1,6 +1,7 @@
 config DRM_STI
 	tristate "DRM Support for STMicroelectronics SoC stiH41x Series"
 	depends on DRM && (SOC_STIH415 || SOC_STIH416 || ARCH_MULTIPLATFORM)
+	select RESET_CONTROLLER
 	select DRM_KMS_HELPER
 	select DRM_GEM_CMA_HELPER
 	select DRM_KMS_CMA_HELPER
diff --git a/drivers/gpu/drm/sti/sti_drm_drv.c b/drivers/gpu/drm/sti/sti_drm_drv.c
index a7cc24917a96..223d93c3a05d 100644
--- a/drivers/gpu/drm/sti/sti_drm_drv.c
+++ b/drivers/gpu/drm/sti/sti_drm_drv.c
@@ -201,8 +201,8 @@ static int sti_drm_platform_probe(struct platform_device *pdev)
 	master = platform_device_register_resndata(dev,
 			DRIVER_NAME "__master", -1,
 			NULL, 0, NULL, 0);
-	if (!master)
-		return -EINVAL;
+	if (IS_ERR(master))
+               return PTR_ERR(master);
 
 	platform_set_drvdata(pdev, master);
 	return 0;
diff --git a/drivers/gpu/drm/sti/sti_hda.c b/drivers/gpu/drm/sti/sti_hda.c
index 72d957f81c05..2ae9a9b73666 100644
--- a/drivers/gpu/drm/sti/sti_hda.c
+++ b/drivers/gpu/drm/sti/sti_hda.c
@@ -730,16 +730,16 @@ static int sti_hda_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 	hda->regs = devm_ioremap_nocache(dev, res->start, resource_size(res));
-	if (IS_ERR(hda->regs))
-		return PTR_ERR(hda->regs);
+	if (!hda->regs)
+		return -ENOMEM;
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
 			"video-dacs-ctrl");
 	if (res) {
 		hda->video_dacs_ctrl = devm_ioremap_nocache(dev, res->start,
 				resource_size(res));
-		if (IS_ERR(hda->video_dacs_ctrl))
-			return PTR_ERR(hda->video_dacs_ctrl);
+		if (!hda->video_dacs_ctrl)
+			return -ENOMEM;
 	} else {
 		/* If no existing video-dacs-ctrl resource continue the probe */
 		DRM_DEBUG_DRIVER("No video-dacs-ctrl resource\n");
@@ -770,7 +770,7 @@ static int sti_hda_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id hda_of_match[] = {
+static const struct of_device_id hda_of_match[] = {
 	{ .compatible = "st,stih416-hda", },
 	{ .compatible = "st,stih407-hda", },
 	{ /* end node */ }
diff --git a/drivers/gpu/drm/sti/sti_hdmi.c b/drivers/gpu/drm/sti/sti_hdmi.c
index 284e541d970d..b22968c08d1f 100644
--- a/drivers/gpu/drm/sti/sti_hdmi.c
+++ b/drivers/gpu/drm/sti/sti_hdmi.c
@@ -298,7 +298,6 @@ static int hdmi_avi_infoframe_config(struct sti_hdmi *hdmi)
 	hdmi_write(hdmi, val, HDMI_SW_DI_N_PKT_WORD2(HDMI_IFRAME_SLOT_AVI));
 
 	val = frame[0xC];
-	val |= frame[0xD] << 8;
 	hdmi_write(hdmi, val, HDMI_SW_DI_N_PKT_WORD3(HDMI_IFRAME_SLOT_AVI));
 
 	/* Enable transmission slot for AVI infoframe
@@ -677,7 +676,7 @@ static const struct component_ops sti_hdmi_ops = {
 	.unbind = sti_hdmi_unbind,
 };
 
-static struct of_device_id hdmi_of_match[] = {
+static const struct of_device_id hdmi_of_match[] = {
 	{
 		.compatible = "st,stih416-hdmi",
 		.data = &tx3g0c55phy_ops,
@@ -713,8 +712,8 @@ static int sti_hdmi_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 	hdmi->regs = devm_ioremap_nocache(dev, res->start, resource_size(res));
-	if (IS_ERR(hdmi->regs))
-		return PTR_ERR(hdmi->regs);
+	if (!hdmi->regs)
+		return -ENOMEM;
 
 	if (of_device_is_compatible(np, "st,stih416-hdmi")) {
 		res = platform_get_resource_byname(pdev, IORESOURCE_MEM,
@@ -725,8 +724,8 @@ static int sti_hdmi_probe(struct platform_device *pdev)
 		}
 		hdmi->syscfg = devm_ioremap_nocache(dev, res->start,
 						    resource_size(res));
-		if (IS_ERR(hdmi->syscfg))
-			return PTR_ERR(hdmi->syscfg);
+		if (!hdmi->syscfg)
+			return -ENOMEM;
 
 	}
 
diff --git a/drivers/gpu/drm/sti/sti_tvout.c b/drivers/gpu/drm/sti/sti_tvout.c
index b69e26fee76e..b8afe490356a 100644
--- a/drivers/gpu/drm/sti/sti_tvout.c
+++ b/drivers/gpu/drm/sti/sti_tvout.c
@@ -591,8 +591,8 @@ static int sti_tvout_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 	tvout->regs = devm_ioremap_nocache(dev, res->start, resource_size(res));
-	if (IS_ERR(tvout->regs))
-		return PTR_ERR(tvout->regs);
+	if (!tvout->regs)
+		return -ENOMEM;
 
 	/* get reset resources */
 	tvout->reset = devm_reset_control_get(dev, "tvout");
@@ -624,7 +624,7 @@ static int sti_tvout_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id tvout_of_match[] = {
+static const struct of_device_id tvout_of_match[] = {
 	{ .compatible = "st,stih416-tvout", },
 	{ .compatible = "st,stih407-tvout", },
 	{ /* end node */ }
diff --git a/drivers/gpu/drm/sti/sti_vtac.c b/drivers/gpu/drm/sti/sti_vtac.c
index 82a51d488434..97bcdac23ae1 100644
--- a/drivers/gpu/drm/sti/sti_vtac.c
+++ b/drivers/gpu/drm/sti/sti_vtac.c
@@ -56,8 +56,16 @@ struct sti_vtac_mode {
 	u32 phyts_per_pixel;
 };
 
-static const struct sti_vtac_mode vtac_mode_main = {0x2, 0x2, VTAC_5_PPP};
-static const struct sti_vtac_mode vtac_mode_aux = {0x1, 0x0, VTAC_17_PPP};
+static const struct sti_vtac_mode vtac_mode_main = {
+	.vid_in_width = 0x2,
+	.phyts_width = 0x2,
+	.phyts_per_pixel = VTAC_5_PPP,
+};
+static const struct sti_vtac_mode vtac_mode_aux = {
+	.vid_in_width = 0x1,
+	.phyts_width = 0x0,
+	.phyts_per_pixel = VTAC_17_PPP,
+};
 
 /**
  * VTAC structure
diff --git a/drivers/gpu/drm/tdfx/tdfx_drv.c b/drivers/gpu/drm/tdfx/tdfx_drv.c
index 3492ca5c46d3..fab5ebcb0fef 100644
--- a/drivers/gpu/drm/tdfx/tdfx_drv.c
+++ b/drivers/gpu/drm/tdfx/tdfx_drv.c
@@ -36,6 +36,7 @@
 #include "tdfx_drv.h"
 
 #include <drm/drm_pciids.h>
+#include <drm/drm_legacy.h>
 
 static struct pci_device_id pciidlist[] = {
 	tdfx_PCI_IDS
@@ -46,7 +47,7 @@ static const struct file_operations tdfx_driver_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = drm_compat_ioctl,
@@ -55,6 +56,7 @@ static const struct file_operations tdfx_driver_fops = {
 };
 
 static struct drm_driver driver = {
+	.set_busid = drm_pci_set_busid,
 	.fops = &tdfx_driver_fops,
 	.name = DRIVER_NAME,
 	.desc = DRIVER_DESC,
diff --git a/drivers/gpu/drm/tegra/dpaux.c b/drivers/gpu/drm/tegra/dpaux.c
index 708f783ead47..d6b55e3e3716 100644
--- a/drivers/gpu/drm/tegra/dpaux.c
+++ b/drivers/gpu/drm/tegra/dpaux.c
@@ -533,9 +533,9 @@ int tegra_dpaux_train(struct tegra_dpaux *dpaux, struct drm_dp_link *link,
 
 	for (i = 0; i < link->num_lanes; i++)
 		values[i] = DP_TRAIN_MAX_PRE_EMPHASIS_REACHED |
-			    DP_TRAIN_PRE_EMPHASIS_0 |
+			    DP_TRAIN_PRE_EMPH_LEVEL_0 |
 			    DP_TRAIN_MAX_SWING_REACHED |
-			    DP_TRAIN_VOLTAGE_SWING_400;
+			    DP_TRAIN_VOLTAGE_SWING_LEVEL_0;
 
 	err = drm_dp_dpcd_write(&dpaux->aux, DP_TRAINING_LANE0_SET, values,
 				link->num_lanes);
diff --git a/drivers/gpu/drm/tegra/gem.h b/drivers/gpu/drm/tegra/gem.h
index 43a25c853357..6538b56780c2 100644
--- a/drivers/gpu/drm/tegra/gem.h
+++ b/drivers/gpu/drm/tegra/gem.h
@@ -15,6 +15,7 @@
 
 #include <drm/drm.h>
 #include <drm/drmP.h>
+#include <drm/drm_gem.h>
 
 #define TEGRA_BO_BOTTOM_UP (1 << 0)
 
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_drv.c b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
index 6be623b4a86f..79a34cbd29f5 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_drv.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_drv.c
@@ -84,6 +84,7 @@ static int modeset_init(struct drm_device *dev)
 	if ((priv->num_encoders == 0) || (priv->num_connectors == 0)) {
 		/* oh nos! */
 		dev_err(dev->dev, "no encoders/connectors found\n");
+		drm_mode_config_cleanup(dev);
 		return -ENXIO;
 	}
 
@@ -172,33 +173,37 @@ static int tilcdc_load(struct drm_device *dev, unsigned long flags)
 	dev->dev_private = priv;
 
 	priv->wq = alloc_ordered_workqueue("tilcdc", 0);
+	if (!priv->wq) {
+		ret = -ENOMEM;
+		goto fail_free_priv;
+	}
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!res) {
 		dev_err(dev->dev, "failed to get memory resource\n");
 		ret = -EINVAL;
-		goto fail;
+		goto fail_free_wq;
 	}
 
 	priv->mmio = ioremap_nocache(res->start, resource_size(res));
 	if (!priv->mmio) {
 		dev_err(dev->dev, "failed to ioremap\n");
 		ret = -ENOMEM;
-		goto fail;
+		goto fail_free_wq;
 	}
 
 	priv->clk = clk_get(dev->dev, "fck");
 	if (IS_ERR(priv->clk)) {
 		dev_err(dev->dev, "failed to get functional clock\n");
 		ret = -ENODEV;
-		goto fail;
+		goto fail_iounmap;
 	}
 
 	priv->disp_clk = clk_get(dev->dev, "dpll_disp_ck");
 	if (IS_ERR(priv->clk)) {
 		dev_err(dev->dev, "failed to get display clock\n");
 		ret = -ENODEV;
-		goto fail;
+		goto fail_put_clk;
 	}
 
 #ifdef CONFIG_CPU_FREQ
@@ -208,7 +213,7 @@ static int tilcdc_load(struct drm_device *dev, unsigned long flags)
 			CPUFREQ_TRANSITION_NOTIFIER);
 	if (ret) {
 		dev_err(dev->dev, "failed to register cpufreq notifier\n");
-		goto fail;
+		goto fail_put_disp_clk;
 	}
 #endif
 
@@ -253,13 +258,13 @@ static int tilcdc_load(struct drm_device *dev, unsigned long flags)
 	ret = modeset_init(dev);
 	if (ret < 0) {
 		dev_err(dev->dev, "failed to initialize mode setting\n");
-		goto fail;
+		goto fail_cpufreq_unregister;
 	}
 
 	ret = drm_vblank_init(dev, 1);
 	if (ret < 0) {
 		dev_err(dev->dev, "failed to initialize vblank\n");
-		goto fail;
+		goto fail_mode_config_cleanup;
 	}
 
 	pm_runtime_get_sync(dev->dev);
@@ -267,7 +272,7 @@ static int tilcdc_load(struct drm_device *dev, unsigned long flags)
 	pm_runtime_put_sync(dev->dev);
 	if (ret < 0) {
 		dev_err(dev->dev, "failed to install IRQ handler\n");
-		goto fail;
+		goto fail_vblank_cleanup;
 	}
 
 	platform_set_drvdata(pdev, dev);
@@ -283,13 +288,48 @@ static int tilcdc_load(struct drm_device *dev, unsigned long flags)
 	priv->fbdev = drm_fbdev_cma_init(dev, bpp,
 			dev->mode_config.num_crtc,
 			dev->mode_config.num_connector);
+	if (IS_ERR(priv->fbdev)) {
+		ret = PTR_ERR(priv->fbdev);
+		goto fail_irq_uninstall;
+	}
 
 	drm_kms_helper_poll_init(dev);
 
 	return 0;
 
-fail:
-	tilcdc_unload(dev);
+fail_irq_uninstall:
+	pm_runtime_get_sync(dev->dev);
+	drm_irq_uninstall(dev);
+	pm_runtime_put_sync(dev->dev);
+
+fail_vblank_cleanup:
+	drm_vblank_cleanup(dev);
+
+fail_mode_config_cleanup:
+	drm_mode_config_cleanup(dev);
+
+fail_cpufreq_unregister:
+	pm_runtime_disable(dev->dev);
+#ifdef CONFIG_CPU_FREQ
+	cpufreq_unregister_notifier(&priv->freq_transition,
+			CPUFREQ_TRANSITION_NOTIFIER);
+fail_put_disp_clk:
+	clk_put(priv->disp_clk);
+#endif
+
+fail_put_clk:
+	clk_put(priv->clk);
+
+fail_iounmap:
+	iounmap(priv->mmio);
+
+fail_free_wq:
+	flush_workqueue(priv->wq);
+	destroy_workqueue(priv->wq);
+
+fail_free_priv:
+	dev->dev_private = NULL;
+	kfree(priv);
 	return ret;
 }
 
@@ -502,6 +542,7 @@ static struct drm_driver tilcdc_driver = {
 	.unload             = tilcdc_unload,
 	.preclose           = tilcdc_preclose,
 	.lastclose          = tilcdc_lastclose,
+	.set_busid          = drm_platform_set_busid,
 	.irq_handler        = tilcdc_irq,
 	.irq_preinstall     = tilcdc_irq_preinstall,
 	.irq_postinstall    = tilcdc_irq_postinstall,
diff --git a/drivers/gpu/drm/tilcdc/tilcdc_panel.c b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
index 4c7aa1d8134f..7a0315855e90 100644
--- a/drivers/gpu/drm/tilcdc/tilcdc_panel.c
+++ b/drivers/gpu/drm/tilcdc/tilcdc_panel.c
@@ -18,6 +18,7 @@
 #include <linux/pinctrl/pinmux.h>
 #include <linux/pinctrl/consumer.h>
 #include <linux/backlight.h>
+#include <linux/gpio/consumer.h>
 #include <video/display_timing.h>
 #include <video/of_display_timing.h>
 #include <video/videomode.h>
@@ -29,6 +30,7 @@ struct panel_module {
 	struct tilcdc_panel_info *info;
 	struct display_timings *timings;
 	struct backlight_device *backlight;
+	struct gpio_desc *enable_gpio;
 };
 #define to_panel_module(x) container_of(x, struct panel_module, base)
 
@@ -55,13 +57,17 @@ static void panel_encoder_dpms(struct drm_encoder *encoder, int mode)
 {
 	struct panel_encoder *panel_encoder = to_panel_encoder(encoder);
 	struct backlight_device *backlight = panel_encoder->mod->backlight;
+	struct gpio_desc *gpio = panel_encoder->mod->enable_gpio;
 
-	if (!backlight)
-		return;
+	if (backlight) {
+		backlight->props.power = mode == DRM_MODE_DPMS_ON ?
+					 FB_BLANK_UNBLANK : FB_BLANK_POWERDOWN;
+		backlight_update_status(backlight);
+	}
 
-	backlight->props.power = mode == DRM_MODE_DPMS_ON
-				     ? FB_BLANK_UNBLANK : FB_BLANK_POWERDOWN;
-	backlight_update_status(backlight);
+	if (gpio)
+		gpiod_set_value_cansleep(gpio,
+					 mode == DRM_MODE_DPMS_ON ? 1 : 0);
 }
 
 static bool panel_encoder_mode_fixup(struct drm_encoder *encoder,
@@ -311,6 +317,7 @@ static struct tilcdc_panel_info *of_get_panel_info(struct device_node *np)
 	info = kzalloc(sizeof(*info), GFP_KERNEL);
 	if (!info) {
 		pr_err("%s: allocation failed\n", __func__);
+		of_node_put(info_np);
 		return NULL;
 	}
 
@@ -331,22 +338,21 @@ static struct tilcdc_panel_info *of_get_panel_info(struct device_node *np)
 	if (ret) {
 		pr_err("%s: error reading panel-info properties\n", __func__);
 		kfree(info);
+		of_node_put(info_np);
 		return NULL;
 	}
+	of_node_put(info_np);
 
 	return info;
 }
 
-static struct of_device_id panel_of_match[];
-
 static int panel_probe(struct platform_device *pdev)
 {
-	struct device_node *node = pdev->dev.of_node;
+	struct device_node *bl_node, *node = pdev->dev.of_node;
 	struct panel_module *panel_mod;
 	struct tilcdc_module *mod;
 	struct pinctrl *pinctrl;
-	int ret = -EINVAL;
-
+	int ret;
 
 	/* bail out early if no DT data: */
 	if (!node) {
@@ -354,10 +360,40 @@ static int panel_probe(struct platform_device *pdev)
 		return -ENXIO;
 	}
 
-	panel_mod = kzalloc(sizeof(*panel_mod), GFP_KERNEL);
+	panel_mod = devm_kzalloc(&pdev->dev, sizeof(*panel_mod), GFP_KERNEL);
 	if (!panel_mod)
 		return -ENOMEM;
 
+	bl_node = of_parse_phandle(node, "backlight", 0);
+	if (bl_node) {
+		panel_mod->backlight = of_find_backlight_by_node(bl_node);
+		of_node_put(bl_node);
+
+		if (!panel_mod->backlight)
+			return -EPROBE_DEFER;
+
+		dev_info(&pdev->dev, "found backlight\n");
+	}
+
+	panel_mod->enable_gpio = devm_gpiod_get(&pdev->dev, "enable");
+	if (IS_ERR(panel_mod->enable_gpio)) {
+		ret = PTR_ERR(panel_mod->enable_gpio);
+		if (ret != -ENOENT) {
+			dev_err(&pdev->dev, "failed to request enable GPIO\n");
+			goto fail_backlight;
+		}
+
+		/* Optional GPIO is not here, continue silently. */
+		panel_mod->enable_gpio = NULL;
+	} else {
+		ret = gpiod_direction_output(panel_mod->enable_gpio, 0);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to setup GPIO\n");
+			goto fail_backlight;
+		}
+		dev_info(&pdev->dev, "found enable GPIO\n");
+	}
+
 	mod = &panel_mod->base;
 	pdev->dev.platform_data = mod;
 
@@ -370,29 +406,30 @@ static int panel_probe(struct platform_device *pdev)
 	panel_mod->timings = of_get_display_timings(node);
 	if (!panel_mod->timings) {
 		dev_err(&pdev->dev, "could not get panel timings\n");
+		ret = -EINVAL;
 		goto fail_free;
 	}
 
 	panel_mod->info = of_get_panel_info(node);
 	if (!panel_mod->info) {
 		dev_err(&pdev->dev, "could not get panel info\n");
+		ret = -EINVAL;
 		goto fail_timings;
 	}
 
 	mod->preferred_bpp = panel_mod->info->bpp;
 
-	panel_mod->backlight = of_find_backlight_by_node(node);
-	if (panel_mod->backlight)
-		dev_info(&pdev->dev, "found backlight\n");
-
 	return 0;
 
 fail_timings:
 	display_timings_release(panel_mod->timings);
 
 fail_free:
-	kfree(panel_mod);
 	tilcdc_module_cleanup(mod);
+
+fail_backlight:
+	if (panel_mod->backlight)
+		put_device(&panel_mod->backlight->dev);
 	return ret;
 }
 
@@ -400,12 +437,15 @@ static int panel_remove(struct platform_device *pdev)
 {
 	struct tilcdc_module *mod = dev_get_platdata(&pdev->dev);
 	struct panel_module *panel_mod = to_panel_module(mod);
+	struct backlight_device *backlight = panel_mod->backlight;
+
+	if (backlight)
+		put_device(&backlight->dev);
 
 	display_timings_release(panel_mod->timings);
 
 	tilcdc_module_cleanup(mod);
 	kfree(panel_mod->info);
-	kfree(panel_mod);
 
 	return 0;
 }
diff --git a/drivers/gpu/drm/ttm/ttm_bo.c b/drivers/gpu/drm/ttm/ttm_bo.c
index 3da89d5dab60..d395b0bef73b 100644
--- a/drivers/gpu/drm/ttm/ttm_bo.c
+++ b/drivers/gpu/drm/ttm/ttm_bo.c
@@ -40,6 +40,7 @@
 #include <linux/file.h>
 #include <linux/module.h>
 #include <linux/atomic.h>
+#include <linux/reservation.h>
 
 #define TTM_ASSERT_LOCKED(param)
 #define TTM_DEBUG(fmt, arg...)
@@ -53,12 +54,13 @@ static struct attribute ttm_bo_count = {
 	.mode = S_IRUGO
 };
 
-static inline int ttm_mem_type_from_flags(uint32_t flags, uint32_t *mem_type)
+static inline int ttm_mem_type_from_place(const struct ttm_place *place,
+					  uint32_t *mem_type)
 {
 	int i;
 
 	for (i = 0; i <= TTM_PL_PRIV5; i++)
-		if (flags & (1 << i)) {
+		if (place->flags & (1 << i)) {
 			*mem_type = i;
 			return 0;
 		}
@@ -89,12 +91,12 @@ static void ttm_bo_mem_space_debug(struct ttm_buffer_object *bo,
 	       bo, bo->mem.num_pages, bo->mem.size >> 10,
 	       bo->mem.size >> 20);
 	for (i = 0; i < placement->num_placement; i++) {
-		ret = ttm_mem_type_from_flags(placement->placement[i],
+		ret = ttm_mem_type_from_place(&placement->placement[i],
 						&mem_type);
 		if (ret)
 			return;
 		pr_err("  placement[%d]=0x%08X (%d)\n",
-		       i, placement->placement[i], mem_type);
+		       i, placement->placement[i].flags, mem_type);
 		ttm_mem_type_debug(bo->bdev, mem_type);
 	}
 }
@@ -141,7 +143,6 @@ static void ttm_bo_release_list(struct kref *list_kref)
 	BUG_ON(atomic_read(&bo->list_kref.refcount));
 	BUG_ON(atomic_read(&bo->kref.refcount));
 	BUG_ON(atomic_read(&bo->cpu_writers));
-	BUG_ON(bo->sync_obj != NULL);
 	BUG_ON(bo->mem.mm_node != NULL);
 	BUG_ON(!list_empty(&bo->lru));
 	BUG_ON(!list_empty(&bo->ddestroy));
@@ -402,36 +403,48 @@ static void ttm_bo_cleanup_memtype_use(struct ttm_buffer_object *bo)
 	ww_mutex_unlock (&bo->resv->lock);
 }
 
+static void ttm_bo_flush_all_fences(struct ttm_buffer_object *bo)
+{
+	struct reservation_object_list *fobj;
+	struct fence *fence;
+	int i;
+
+	fobj = reservation_object_get_list(bo->resv);
+	fence = reservation_object_get_excl(bo->resv);
+	if (fence && !fence->ops->signaled)
+		fence_enable_sw_signaling(fence);
+
+	for (i = 0; fobj && i < fobj->shared_count; ++i) {
+		fence = rcu_dereference_protected(fobj->shared[i],
+					reservation_object_held(bo->resv));
+
+		if (!fence->ops->signaled)
+			fence_enable_sw_signaling(fence);
+	}
+}
+
 static void ttm_bo_cleanup_refs_or_queue(struct ttm_buffer_object *bo)
 {
 	struct ttm_bo_device *bdev = bo->bdev;
 	struct ttm_bo_global *glob = bo->glob;
-	struct ttm_bo_driver *driver = bdev->driver;
-	void *sync_obj = NULL;
 	int put_count;
 	int ret;
 
 	spin_lock(&glob->lru_lock);
 	ret = __ttm_bo_reserve(bo, false, true, false, NULL);
 
-	spin_lock(&bdev->fence_lock);
-	(void) ttm_bo_wait(bo, false, false, true);
-	if (!ret && !bo->sync_obj) {
-		spin_unlock(&bdev->fence_lock);
-		put_count = ttm_bo_del_from_lru(bo);
-
-		spin_unlock(&glob->lru_lock);
-		ttm_bo_cleanup_memtype_use(bo);
+	if (!ret) {
+		if (!ttm_bo_wait(bo, false, false, true)) {
+			put_count = ttm_bo_del_from_lru(bo);
 
-		ttm_bo_list_ref_sub(bo, put_count, true);
+			spin_unlock(&glob->lru_lock);
+			ttm_bo_cleanup_memtype_use(bo);
 
-		return;
-	}
-	if (bo->sync_obj)
-		sync_obj = driver->sync_obj_ref(bo->sync_obj);
-	spin_unlock(&bdev->fence_lock);
+			ttm_bo_list_ref_sub(bo, put_count, true);
 
-	if (!ret) {
+			return;
+		} else
+			ttm_bo_flush_all_fences(bo);
 
 		/*
 		 * Make NO_EVICT bos immediately available to
@@ -450,10 +463,6 @@ static void ttm_bo_cleanup_refs_or_queue(struct ttm_buffer_object *bo)
 	list_add_tail(&bo->ddestroy, &bdev->ddestroy);
 	spin_unlock(&glob->lru_lock);
 
-	if (sync_obj) {
-		driver->sync_obj_flush(sync_obj);
-		driver->sync_obj_unref(&sync_obj);
-	}
 	schedule_delayed_work(&bdev->wq,
 			      ((HZ / 100) < 1) ? 1 : HZ / 100);
 }
@@ -474,44 +483,26 @@ static int ttm_bo_cleanup_refs_and_unlock(struct ttm_buffer_object *bo,
 					  bool interruptible,
 					  bool no_wait_gpu)
 {
-	struct ttm_bo_device *bdev = bo->bdev;
-	struct ttm_bo_driver *driver = bdev->driver;
 	struct ttm_bo_global *glob = bo->glob;
 	int put_count;
 	int ret;
 
-	spin_lock(&bdev->fence_lock);
 	ret = ttm_bo_wait(bo, false, false, true);
 
 	if (ret && !no_wait_gpu) {
-		void *sync_obj;
-
-		/*
-		 * Take a reference to the fence and unreserve,
-		 * at this point the buffer should be dead, so
-		 * no new sync objects can be attached.
-		 */
-		sync_obj = driver->sync_obj_ref(bo->sync_obj);
-		spin_unlock(&bdev->fence_lock);
-
-		__ttm_bo_unreserve(bo);
+		long lret;
+		ww_mutex_unlock(&bo->resv->lock);
 		spin_unlock(&glob->lru_lock);
 
-		ret = driver->sync_obj_wait(sync_obj, false, interruptible);
-		driver->sync_obj_unref(&sync_obj);
-		if (ret)
-			return ret;
+		lret = reservation_object_wait_timeout_rcu(bo->resv,
+							   true,
+							   interruptible,
+							   30 * HZ);
 
-		/*
-		 * remove sync_obj with ttm_bo_wait, the wait should be
-		 * finished, and no new wait object should have been added.
-		 */
-		spin_lock(&bdev->fence_lock);
-		ret = ttm_bo_wait(bo, false, false, true);
-		WARN_ON(ret);
-		spin_unlock(&bdev->fence_lock);
-		if (ret)
-			return ret;
+		if (lret < 0)
+			return lret;
+		else if (lret == 0)
+			return -EBUSY;
 
 		spin_lock(&glob->lru_lock);
 		ret = __ttm_bo_reserve(bo, false, true, false, NULL);
@@ -528,8 +519,14 @@ static int ttm_bo_cleanup_refs_and_unlock(struct ttm_buffer_object *bo,
 			spin_unlock(&glob->lru_lock);
 			return 0;
 		}
-	} else
-		spin_unlock(&bdev->fence_lock);
+
+		/*
+		 * remove sync_obj with ttm_bo_wait, the wait should be
+		 * finished, and no new wait object should have been added.
+		 */
+		ret = ttm_bo_wait(bo, false, false, true);
+		WARN_ON(ret);
+	}
 
 	if (ret || unlikely(list_empty(&bo->ddestroy))) {
 		__ttm_bo_unreserve(bo);
@@ -667,9 +664,7 @@ static int ttm_bo_evict(struct ttm_buffer_object *bo, bool interruptible,
 	struct ttm_placement placement;
 	int ret = 0;
 
-	spin_lock(&bdev->fence_lock);
 	ret = ttm_bo_wait(bo, false, interruptible, no_wait_gpu);
-	spin_unlock(&bdev->fence_lock);
 
 	if (unlikely(ret != 0)) {
 		if (ret != -ERESTARTSYS) {
@@ -685,8 +680,6 @@ static int ttm_bo_evict(struct ttm_buffer_object *bo, bool interruptible,
 	evict_mem.bus.io_reserved_vm = false;
 	evict_mem.bus.io_reserved_count = 0;
 
-	placement.fpfn = 0;
-	placement.lpfn = 0;
 	placement.num_placement = 0;
 	placement.num_busy_placement = 0;
 	bdev->driver->evict_flags(bo, &placement);
@@ -716,6 +709,7 @@ out:
 
 static int ttm_mem_evict_first(struct ttm_bo_device *bdev,
 				uint32_t mem_type,
+				const struct ttm_place *place,
 				bool interruptible,
 				bool no_wait_gpu)
 {
@@ -727,8 +721,21 @@ static int ttm_mem_evict_first(struct ttm_bo_device *bdev,
 	spin_lock(&glob->lru_lock);
 	list_for_each_entry(bo, &man->lru, lru) {
 		ret = __ttm_bo_reserve(bo, false, true, false, NULL);
-		if (!ret)
+		if (!ret) {
+			if (place && (place->fpfn || place->lpfn)) {
+				/* Don't evict this BO if it's outside of the
+				 * requested placement range
+				 */
+				if (place->fpfn >= (bo->mem.start + bo->mem.size) ||
+				    (place->lpfn && place->lpfn <= bo->mem.start)) {
+					__ttm_bo_unreserve(bo);
+					ret = -EBUSY;
+					continue;
+				}
+			}
+
 			break;
+		}
 	}
 
 	if (ret) {
@@ -774,7 +781,7 @@ EXPORT_SYMBOL(ttm_bo_mem_put);
  */
 static int ttm_bo_mem_force_space(struct ttm_buffer_object *bo,
 					uint32_t mem_type,
-					struct ttm_placement *placement,
+					const struct ttm_place *place,
 					struct ttm_mem_reg *mem,
 					bool interruptible,
 					bool no_wait_gpu)
@@ -784,12 +791,12 @@ static int ttm_bo_mem_force_space(struct ttm_buffer_object *bo,
 	int ret;
 
 	do {
-		ret = (*man->func->get_node)(man, bo, placement, 0, mem);
+		ret = (*man->func->get_node)(man, bo, place, mem);
 		if (unlikely(ret != 0))
 			return ret;
 		if (mem->mm_node)
 			break;
-		ret = ttm_mem_evict_first(bdev, mem_type,
+		ret = ttm_mem_evict_first(bdev, mem_type, place,
 					  interruptible, no_wait_gpu);
 		if (unlikely(ret != 0))
 			return ret;
@@ -827,18 +834,18 @@ static uint32_t ttm_bo_select_caching(struct ttm_mem_type_manager *man,
 
 static bool ttm_bo_mt_compatible(struct ttm_mem_type_manager *man,
 				 uint32_t mem_type,
-				 uint32_t proposed_placement,
+				 const struct ttm_place *place,
 				 uint32_t *masked_placement)
 {
 	uint32_t cur_flags = ttm_bo_type_flags(mem_type);
 
-	if ((cur_flags & proposed_placement & TTM_PL_MASK_MEM) == 0)
+	if ((cur_flags & place->flags & TTM_PL_MASK_MEM) == 0)
 		return false;
 
-	if ((proposed_placement & man->available_caching) == 0)
+	if ((place->flags & man->available_caching) == 0)
 		return false;
 
-	cur_flags |= (proposed_placement & man->available_caching);
+	cur_flags |= (place->flags & man->available_caching);
 
 	*masked_placement = cur_flags;
 	return true;
@@ -869,15 +876,14 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 
 	mem->mm_node = NULL;
 	for (i = 0; i < placement->num_placement; ++i) {
-		ret = ttm_mem_type_from_flags(placement->placement[i],
-						&mem_type);
+		const struct ttm_place *place = &placement->placement[i];
+
+		ret = ttm_mem_type_from_place(place, &mem_type);
 		if (ret)
 			return ret;
 		man = &bdev->man[mem_type];
 
-		type_ok = ttm_bo_mt_compatible(man,
-						mem_type,
-						placement->placement[i],
+		type_ok = ttm_bo_mt_compatible(man, mem_type, place,
 						&cur_flags);
 
 		if (!type_ok)
@@ -889,7 +895,7 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 		 * Use the access and other non-mapping-related flag bits from
 		 * the memory placement flags to the current flags
 		 */
-		ttm_flag_masked(&cur_flags, placement->placement[i],
+		ttm_flag_masked(&cur_flags, place->flags,
 				~TTM_PL_MASK_MEMTYPE);
 
 		if (mem_type == TTM_PL_SYSTEM)
@@ -897,8 +903,7 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 
 		if (man->has_type && man->use_type) {
 			type_found = true;
-			ret = (*man->func->get_node)(man, bo, placement,
-						     cur_flags, mem);
+			ret = (*man->func->get_node)(man, bo, place, mem);
 			if (unlikely(ret))
 				return ret;
 		}
@@ -916,17 +921,15 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 		return -EINVAL;
 
 	for (i = 0; i < placement->num_busy_placement; ++i) {
-		ret = ttm_mem_type_from_flags(placement->busy_placement[i],
-						&mem_type);
+		const struct ttm_place *place = &placement->busy_placement[i];
+
+		ret = ttm_mem_type_from_place(place, &mem_type);
 		if (ret)
 			return ret;
 		man = &bdev->man[mem_type];
 		if (!man->has_type)
 			continue;
-		if (!ttm_bo_mt_compatible(man,
-						mem_type,
-						placement->busy_placement[i],
-						&cur_flags))
+		if (!ttm_bo_mt_compatible(man, mem_type, place, &cur_flags))
 			continue;
 
 		cur_flags = ttm_bo_select_caching(man, bo->mem.placement,
@@ -935,7 +938,7 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 		 * Use the access and other non-mapping-related flag bits from
 		 * the memory placement flags to the current flags
 		 */
-		ttm_flag_masked(&cur_flags, placement->busy_placement[i],
+		ttm_flag_masked(&cur_flags, place->flags,
 				~TTM_PL_MASK_MEMTYPE);
 
 		if (mem_type == TTM_PL_SYSTEM) {
@@ -945,7 +948,7 @@ int ttm_bo_mem_space(struct ttm_buffer_object *bo,
 			return 0;
 		}
 
-		ret = ttm_bo_mem_force_space(bo, mem_type, placement, mem,
+		ret = ttm_bo_mem_force_space(bo, mem_type, place, mem,
 						interruptible, no_wait_gpu);
 		if (ret == 0 && mem->mm_node) {
 			mem->placement = cur_flags;
@@ -966,7 +969,6 @@ static int ttm_bo_move_buffer(struct ttm_buffer_object *bo,
 {
 	int ret = 0;
 	struct ttm_mem_reg mem;
-	struct ttm_bo_device *bdev = bo->bdev;
 
 	lockdep_assert_held(&bo->resv->lock.base);
 
@@ -975,9 +977,7 @@ static int ttm_bo_move_buffer(struct ttm_buffer_object *bo,
 	 * Have the driver move function wait for idle when necessary,
 	 * instead of doing it here.
 	 */
-	spin_lock(&bdev->fence_lock);
 	ret = ttm_bo_wait(bo, false, interruptible, no_wait_gpu);
-	spin_unlock(&bdev->fence_lock);
 	if (ret)
 		return ret;
 	mem.num_pages = bo->num_pages;
@@ -1006,20 +1006,27 @@ static bool ttm_bo_mem_compat(struct ttm_placement *placement,
 {
 	int i;
 
-	if (mem->mm_node && placement->lpfn != 0 &&
-	    (mem->start < placement->fpfn ||
-	     mem->start + mem->num_pages > placement->lpfn))
-		return false;
-
 	for (i = 0; i < placement->num_placement; i++) {
-		*new_flags = placement->placement[i];
+		const struct ttm_place *heap = &placement->placement[i];
+		if (mem->mm_node &&
+		    (mem->start < heap->fpfn ||
+		     (heap->lpfn != 0 && (mem->start + mem->num_pages) > heap->lpfn)))
+			continue;
+
+		*new_flags = heap->flags;
 		if ((*new_flags & mem->placement & TTM_PL_MASK_CACHING) &&
 		    (*new_flags & mem->placement & TTM_PL_MASK_MEM))
 			return true;
 	}
 
 	for (i = 0; i < placement->num_busy_placement; i++) {
-		*new_flags = placement->busy_placement[i];
+		const struct ttm_place *heap = &placement->busy_placement[i];
+		if (mem->mm_node &&
+		    (mem->start < heap->fpfn ||
+		     (heap->lpfn != 0 && (mem->start + mem->num_pages) > heap->lpfn)))
+			continue;
+
+		*new_flags = heap->flags;
 		if ((*new_flags & mem->placement & TTM_PL_MASK_CACHING) &&
 		    (*new_flags & mem->placement & TTM_PL_MASK_MEM))
 			return true;
@@ -1037,11 +1044,6 @@ int ttm_bo_validate(struct ttm_buffer_object *bo,
 	uint32_t new_flags;
 
 	lockdep_assert_held(&bo->resv->lock.base);
-	/* Check that range is valid */
-	if (placement->lpfn || placement->fpfn)
-		if (placement->fpfn > placement->lpfn ||
-			(placement->lpfn - placement->fpfn) < bo->num_pages)
-			return -EINVAL;
 	/*
 	 * Check whether we need to move buffer.
 	 */
@@ -1070,15 +1072,6 @@ int ttm_bo_validate(struct ttm_buffer_object *bo,
 }
 EXPORT_SYMBOL(ttm_bo_validate);
 
-int ttm_bo_check_placement(struct ttm_buffer_object *bo,
-				struct ttm_placement *placement)
-{
-	BUG_ON((placement->fpfn || placement->lpfn) &&
-	       (bo->mem.num_pages > (placement->lpfn - placement->fpfn)));
-
-	return 0;
-}
-
 int ttm_bo_init(struct ttm_bo_device *bdev,
 		struct ttm_buffer_object *bo,
 		unsigned long size,
@@ -1089,6 +1082,7 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 		struct file *persistent_swap_storage,
 		size_t acc_size,
 		struct sg_table *sg,
+		struct reservation_object *resv,
 		void (*destroy) (struct ttm_buffer_object *))
 {
 	int ret = 0;
@@ -1142,30 +1136,38 @@ int ttm_bo_init(struct ttm_bo_device *bdev,
 	bo->persistent_swap_storage = persistent_swap_storage;
 	bo->acc_size = acc_size;
 	bo->sg = sg;
-	bo->resv = &bo->ttm_resv;
-	reservation_object_init(bo->resv);
+	if (resv) {
+		bo->resv = resv;
+		lockdep_assert_held(&bo->resv->lock.base);
+	} else {
+		bo->resv = &bo->ttm_resv;
+		reservation_object_init(&bo->ttm_resv);
+	}
 	atomic_inc(&bo->glob->bo_count);
 	drm_vma_node_reset(&bo->vma_node);
 
-	ret = ttm_bo_check_placement(bo, placement);
-
 	/*
 	 * For ttm_bo_type_device buffers, allocate
 	 * address space from the device.
 	 */
-	if (likely(!ret) &&
-	    (bo->type == ttm_bo_type_device ||
-	     bo->type == ttm_bo_type_sg))
+	if (bo->type == ttm_bo_type_device ||
+	    bo->type == ttm_bo_type_sg)
 		ret = drm_vma_offset_add(&bdev->vma_manager, &bo->vma_node,
 					 bo->mem.num_pages);
 
-	locked = ww_mutex_trylock(&bo->resv->lock);
-	WARN_ON(!locked);
+	/* passed reservation objects should already be locked,
+	 * since otherwise lockdep will be angered in radeon.
+	 */
+	if (!resv) {
+		locked = ww_mutex_trylock(&bo->resv->lock);
+		WARN_ON(!locked);
+	}
 
 	if (likely(!ret))
 		ret = ttm_bo_validate(bo, placement, interruptible, false);
 
-	ttm_bo_unreserve(bo);
+	if (!resv)
+		ttm_bo_unreserve(bo);
 
 	if (unlikely(ret))
 		ttm_bo_unref(&bo);
@@ -1223,7 +1225,7 @@ int ttm_bo_create(struct ttm_bo_device *bdev,
 	acc_size = ttm_bo_acc_size(bdev, size, sizeof(struct ttm_buffer_object));
 	ret = ttm_bo_init(bdev, bo, size, type, placement, page_alignment,
 			  interruptible, persistent_swap_storage, acc_size,
-			  NULL, NULL);
+			  NULL, NULL, NULL);
 	if (likely(ret == 0))
 		*p_bo = bo;
 
@@ -1245,7 +1247,7 @@ static int ttm_bo_force_list_clean(struct ttm_bo_device *bdev,
 	spin_lock(&glob->lru_lock);
 	while (!list_empty(&man->lru)) {
 		spin_unlock(&glob->lru_lock);
-		ret = ttm_mem_evict_first(bdev, mem_type, false, false);
+		ret = ttm_mem_evict_first(bdev, mem_type, NULL, false, false);
 		if (ret) {
 			if (allow_errors) {
 				return ret;
@@ -1477,7 +1479,6 @@ int ttm_bo_device_init(struct ttm_bo_device *bdev,
 	bdev->glob = glob;
 	bdev->need_dma32 = need_dma32;
 	bdev->val_seq = 0;
-	spin_lock_init(&bdev->fence_lock);
 	mutex_lock(&glob->device_list_mutex);
 	list_add_tail(&bdev->device_list, &glob->device_list);
 	mutex_unlock(&glob->device_list_mutex);
@@ -1530,65 +1531,56 @@ void ttm_bo_unmap_virtual(struct ttm_buffer_object *bo)
 
 EXPORT_SYMBOL(ttm_bo_unmap_virtual);
 
-
 int ttm_bo_wait(struct ttm_buffer_object *bo,
 		bool lazy, bool interruptible, bool no_wait)
 {
-	struct ttm_bo_driver *driver = bo->bdev->driver;
-	struct ttm_bo_device *bdev = bo->bdev;
-	void *sync_obj;
-	int ret = 0;
-
-	if (likely(bo->sync_obj == NULL))
-		return 0;
+	struct reservation_object_list *fobj;
+	struct reservation_object *resv;
+	struct fence *excl;
+	long timeout = 15 * HZ;
+	int i;
 
-	while (bo->sync_obj) {
+	resv = bo->resv;
+	fobj = reservation_object_get_list(resv);
+	excl = reservation_object_get_excl(resv);
+	if (excl) {
+		if (!fence_is_signaled(excl)) {
+			if (no_wait)
+				return -EBUSY;
 
-		if (driver->sync_obj_signaled(bo->sync_obj)) {
-			void *tmp_obj = bo->sync_obj;
-			bo->sync_obj = NULL;
-			clear_bit(TTM_BO_PRIV_FLAG_MOVING, &bo->priv_flags);
-			spin_unlock(&bdev->fence_lock);
-			driver->sync_obj_unref(&tmp_obj);
-			spin_lock(&bdev->fence_lock);
-			continue;
+			timeout = fence_wait_timeout(excl,
+						     interruptible, timeout);
 		}
+	}
 
-		if (no_wait)
-			return -EBUSY;
+	for (i = 0; fobj && timeout > 0 && i < fobj->shared_count; ++i) {
+		struct fence *fence;
+		fence = rcu_dereference_protected(fobj->shared[i],
+						reservation_object_held(resv));
 
-		sync_obj = driver->sync_obj_ref(bo->sync_obj);
-		spin_unlock(&bdev->fence_lock);
-		ret = driver->sync_obj_wait(sync_obj,
-					    lazy, interruptible);
-		if (unlikely(ret != 0)) {
-			driver->sync_obj_unref(&sync_obj);
-			spin_lock(&bdev->fence_lock);
-			return ret;
-		}
-		spin_lock(&bdev->fence_lock);
-		if (likely(bo->sync_obj == sync_obj)) {
-			void *tmp_obj = bo->sync_obj;
-			bo->sync_obj = NULL;
-			clear_bit(TTM_BO_PRIV_FLAG_MOVING,
-				  &bo->priv_flags);
-			spin_unlock(&bdev->fence_lock);
-			driver->sync_obj_unref(&sync_obj);
-			driver->sync_obj_unref(&tmp_obj);
-			spin_lock(&bdev->fence_lock);
-		} else {
-			spin_unlock(&bdev->fence_lock);
-			driver->sync_obj_unref(&sync_obj);
-			spin_lock(&bdev->fence_lock);
+		if (!fence_is_signaled(fence)) {
+			if (no_wait)
+				return -EBUSY;
+
+			timeout = fence_wait_timeout(fence,
+						     interruptible, timeout);
 		}
 	}
+
+	if (timeout < 0)
+		return timeout;
+
+	if (timeout == 0)
+		return -EBUSY;
+
+	reservation_object_add_excl_fence(resv, NULL);
+	clear_bit(TTM_BO_PRIV_FLAG_MOVING, &bo->priv_flags);
 	return 0;
 }
 EXPORT_SYMBOL(ttm_bo_wait);
 
 int ttm_bo_synccpu_write_grab(struct ttm_buffer_object *bo, bool no_wait)
 {
-	struct ttm_bo_device *bdev = bo->bdev;
 	int ret = 0;
 
 	/*
@@ -1598,9 +1590,7 @@ int ttm_bo_synccpu_write_grab(struct ttm_buffer_object *bo, bool no_wait)
 	ret = ttm_bo_reserve(bo, true, no_wait, false, NULL);
 	if (unlikely(ret != 0))
 		return ret;
-	spin_lock(&bdev->fence_lock);
 	ret = ttm_bo_wait(bo, false, true, no_wait);
-	spin_unlock(&bdev->fence_lock);
 	if (likely(ret == 0))
 		atomic_inc(&bo->cpu_writers);
 	ttm_bo_unreserve(bo);
@@ -1657,9 +1647,7 @@ static int ttm_bo_swapout(struct ttm_mem_shrink *shrink)
 	 * Wait for GPU, then move to system cached.
 	 */
 
-	spin_lock(&bo->bdev->fence_lock);
 	ret = ttm_bo_wait(bo, false, false, false);
-	spin_unlock(&bo->bdev->fence_lock);
 
 	if (unlikely(ret != 0))
 		goto out;
diff --git a/drivers/gpu/drm/ttm/ttm_bo_manager.c b/drivers/gpu/drm/ttm/ttm_bo_manager.c
index 9e103a4875c8..964387fc5c8f 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_manager.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_manager.c
@@ -49,8 +49,7 @@ struct ttm_range_manager {
 
 static int ttm_bo_man_get_node(struct ttm_mem_type_manager *man,
 			       struct ttm_buffer_object *bo,
-			       struct ttm_placement *placement,
-			       uint32_t flags,
+			       const struct ttm_place *place,
 			       struct ttm_mem_reg *mem)
 {
 	struct ttm_range_manager *rman = (struct ttm_range_manager *) man->priv;
@@ -60,7 +59,7 @@ static int ttm_bo_man_get_node(struct ttm_mem_type_manager *man,
 	unsigned long lpfn;
 	int ret;
 
-	lpfn = placement->lpfn;
+	lpfn = place->lpfn;
 	if (!lpfn)
 		lpfn = man->size;
 
@@ -68,13 +67,13 @@ static int ttm_bo_man_get_node(struct ttm_mem_type_manager *man,
 	if (!node)
 		return -ENOMEM;
 
-	if (flags & TTM_PL_FLAG_TOPDOWN)
+	if (place->flags & TTM_PL_FLAG_TOPDOWN)
 		aflags = DRM_MM_CREATE_TOP;
 
 	spin_lock(&rman->lock);
 	ret = drm_mm_insert_node_in_range_generic(mm, node, mem->num_pages,
 					  mem->page_alignment, 0,
-					  placement->fpfn, lpfn,
+					  place->fpfn, lpfn,
 					  DRM_MM_SEARCH_BEST,
 					  aflags);
 	spin_unlock(&rman->lock);
diff --git a/drivers/gpu/drm/ttm/ttm_bo_util.c b/drivers/gpu/drm/ttm/ttm_bo_util.c
index 30e5d90cb7bc..882cccdad272 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_util.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_util.c
@@ -37,6 +37,7 @@
 #include <linux/slab.h>
 #include <linux/vmalloc.h>
 #include <linux/module.h>
+#include <linux/reservation.h>
 
 void ttm_bo_free_old_node(struct ttm_buffer_object *bo)
 {
@@ -444,8 +445,6 @@ static int ttm_buffer_object_transfer(struct ttm_buffer_object *bo,
 				      struct ttm_buffer_object **new_obj)
 {
 	struct ttm_buffer_object *fbo;
-	struct ttm_bo_device *bdev = bo->bdev;
-	struct ttm_bo_driver *driver = bdev->driver;
 	int ret;
 
 	fbo = kmalloc(sizeof(*fbo), GFP_KERNEL);
@@ -466,12 +465,6 @@ static int ttm_buffer_object_transfer(struct ttm_buffer_object *bo,
 	drm_vma_node_reset(&fbo->vma_node);
 	atomic_set(&fbo->cpu_writers, 0);
 
-	spin_lock(&bdev->fence_lock);
-	if (bo->sync_obj)
-		fbo->sync_obj = driver->sync_obj_ref(bo->sync_obj);
-	else
-		fbo->sync_obj = NULL;
-	spin_unlock(&bdev->fence_lock);
 	kref_init(&fbo->list_kref);
 	kref_init(&fbo->kref);
 	fbo->destroy = &ttm_transfered_destroy;
@@ -487,28 +480,24 @@ static int ttm_buffer_object_transfer(struct ttm_buffer_object *bo,
 
 pgprot_t ttm_io_prot(uint32_t caching_flags, pgprot_t tmp)
 {
+	/* Cached mappings need no adjustment */
+	if (caching_flags & TTM_PL_FLAG_CACHED)
+		return tmp;
+
 #if defined(__i386__) || defined(__x86_64__)
 	if (caching_flags & TTM_PL_FLAG_WC)
 		tmp = pgprot_writecombine(tmp);
 	else if (boot_cpu_data.x86 > 3)
 		tmp = pgprot_noncached(tmp);
-
-#elif defined(__powerpc__)
-	if (!(caching_flags & TTM_PL_FLAG_CACHED)) {
-		pgprot_val(tmp) |= _PAGE_NO_CACHE;
-		if (caching_flags & TTM_PL_FLAG_UNCACHED)
-			pgprot_val(tmp) |= _PAGE_GUARDED;
-	}
 #endif
-#if defined(__ia64__) || defined(__arm__)
+#if defined(__ia64__) || defined(__arm__) || defined(__powerpc__)
 	if (caching_flags & TTM_PL_FLAG_WC)
 		tmp = pgprot_writecombine(tmp);
 	else
 		tmp = pgprot_noncached(tmp);
 #endif
 #if defined(__sparc__) || defined(__mips__)
-	if (!(caching_flags & TTM_PL_FLAG_CACHED))
-		tmp = pgprot_noncached(tmp);
+	tmp = pgprot_noncached(tmp);
 #endif
 	return tmp;
 }
@@ -567,9 +556,7 @@ static int ttm_bo_kmap_ttm(struct ttm_buffer_object *bo,
 		 * We need to use vmap to get the desired page protection
 		 * or to make the buffer object look contiguous.
 		 */
-		prot = (mem->placement & TTM_PL_FLAG_CACHED) ?
-			PAGE_KERNEL :
-			ttm_io_prot(mem->placement, PAGE_KERNEL);
+		prot = ttm_io_prot(mem->placement, PAGE_KERNEL);
 		map->bo_kmap_type = ttm_bo_map_vmap;
 		map->virtual = vmap(ttm->pages + start_page, num_pages,
 				    0, prot);
@@ -644,30 +631,20 @@ void ttm_bo_kunmap(struct ttm_bo_kmap_obj *map)
 EXPORT_SYMBOL(ttm_bo_kunmap);
 
 int ttm_bo_move_accel_cleanup(struct ttm_buffer_object *bo,
-			      void *sync_obj,
+			      struct fence *fence,
 			      bool evict,
 			      bool no_wait_gpu,
 			      struct ttm_mem_reg *new_mem)
 {
 	struct ttm_bo_device *bdev = bo->bdev;
-	struct ttm_bo_driver *driver = bdev->driver;
 	struct ttm_mem_type_manager *man = &bdev->man[new_mem->mem_type];
 	struct ttm_mem_reg *old_mem = &bo->mem;
 	int ret;
 	struct ttm_buffer_object *ghost_obj;
-	void *tmp_obj = NULL;
 
-	spin_lock(&bdev->fence_lock);
-	if (bo->sync_obj) {
-		tmp_obj = bo->sync_obj;
-		bo->sync_obj = NULL;
-	}
-	bo->sync_obj = driver->sync_obj_ref(sync_obj);
+	reservation_object_add_excl_fence(bo->resv, fence);
 	if (evict) {
 		ret = ttm_bo_wait(bo, false, false, false);
-		spin_unlock(&bdev->fence_lock);
-		if (tmp_obj)
-			driver->sync_obj_unref(&tmp_obj);
 		if (ret)
 			return ret;
 
@@ -688,14 +665,13 @@ int ttm_bo_move_accel_cleanup(struct ttm_buffer_object *bo,
 		 */
 
 		set_bit(TTM_BO_PRIV_FLAG_MOVING, &bo->priv_flags);
-		spin_unlock(&bdev->fence_lock);
-		if (tmp_obj)
-			driver->sync_obj_unref(&tmp_obj);
 
 		ret = ttm_buffer_object_transfer(bo, &ghost_obj);
 		if (ret)
 			return ret;
 
+		reservation_object_add_excl_fence(ghost_obj->resv, fence);
+
 		/**
 		 * If we're not moving to fixed memory, the TTM object
 		 * needs to stay alive. Otherwhise hang it on the ghost
diff --git a/drivers/gpu/drm/ttm/ttm_bo_vm.c b/drivers/gpu/drm/ttm/ttm_bo_vm.c
index 0ce48e5a9cb4..8fb7213277cc 100644
--- a/drivers/gpu/drm/ttm/ttm_bo_vm.c
+++ b/drivers/gpu/drm/ttm/ttm_bo_vm.c
@@ -45,10 +45,8 @@ static int ttm_bo_vm_fault_idle(struct ttm_buffer_object *bo,
 				struct vm_area_struct *vma,
 				struct vm_fault *vmf)
 {
-	struct ttm_bo_device *bdev = bo->bdev;
 	int ret = 0;
 
-	spin_lock(&bdev->fence_lock);
 	if (likely(!test_bit(TTM_BO_PRIV_FLAG_MOVING, &bo->priv_flags)))
 		goto out_unlock;
 
@@ -82,7 +80,6 @@ static int ttm_bo_vm_fault_idle(struct ttm_buffer_object *bo,
 			VM_FAULT_NOPAGE;
 
 out_unlock:
-	spin_unlock(&bdev->fence_lock);
 	return ret;
 }
 
@@ -200,9 +197,8 @@ static int ttm_bo_vm_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
 						cvma.vm_page_prot);
 	} else {
 		ttm = bo->ttm;
-		if (!(bo->mem.placement & TTM_PL_FLAG_CACHED))
-			cvma.vm_page_prot = ttm_io_prot(bo->mem.placement,
-							cvma.vm_page_prot);
+		cvma.vm_page_prot = ttm_io_prot(bo->mem.placement,
+						cvma.vm_page_prot);
 
 		/* Allocate all page at once, most common usage */
 		if (ttm->bdev->driver->ttm_tt_populate(ttm)) {
diff --git a/drivers/gpu/drm/ttm/ttm_execbuf_util.c b/drivers/gpu/drm/ttm/ttm_execbuf_util.c
index e8dac8758528..8ce508e76208 100644
--- a/drivers/gpu/drm/ttm/ttm_execbuf_util.c
+++ b/drivers/gpu/drm/ttm/ttm_execbuf_util.c
@@ -32,20 +32,12 @@
 #include <linux/sched.h>
 #include <linux/module.h>
 
-static void ttm_eu_backoff_reservation_locked(struct list_head *list)
+static void ttm_eu_backoff_reservation_reverse(struct list_head *list,
+					      struct ttm_validate_buffer *entry)
 {
-	struct ttm_validate_buffer *entry;
-
-	list_for_each_entry(entry, list, head) {
+	list_for_each_entry_continue_reverse(entry, list, head) {
 		struct ttm_buffer_object *bo = entry->bo;
-		if (!entry->reserved)
-			continue;
 
-		entry->reserved = false;
-		if (entry->removed) {
-			ttm_bo_add_to_lru(bo);
-			entry->removed = false;
-		}
 		__ttm_bo_unreserve(bo);
 	}
 }
@@ -56,27 +48,9 @@ static void ttm_eu_del_from_lru_locked(struct list_head *list)
 
 	list_for_each_entry(entry, list, head) {
 		struct ttm_buffer_object *bo = entry->bo;
-		if (!entry->reserved)
-			continue;
-
-		if (!entry->removed) {
-			entry->put_count = ttm_bo_del_from_lru(bo);
-			entry->removed = true;
-		}
-	}
-}
-
-static void ttm_eu_list_ref_sub(struct list_head *list)
-{
-	struct ttm_validate_buffer *entry;
-
-	list_for_each_entry(entry, list, head) {
-		struct ttm_buffer_object *bo = entry->bo;
+		unsigned put_count = ttm_bo_del_from_lru(bo);
 
-		if (entry->put_count) {
-			ttm_bo_list_ref_sub(bo, entry->put_count, true);
-			entry->put_count = 0;
-		}
+		ttm_bo_list_ref_sub(bo, put_count, true);
 	}
 }
 
@@ -91,11 +65,18 @@ void ttm_eu_backoff_reservation(struct ww_acquire_ctx *ticket,
 
 	entry = list_first_entry(list, struct ttm_validate_buffer, head);
 	glob = entry->bo->glob;
+
 	spin_lock(&glob->lru_lock);
-	ttm_eu_backoff_reservation_locked(list);
+	list_for_each_entry(entry, list, head) {
+		struct ttm_buffer_object *bo = entry->bo;
+
+		ttm_bo_add_to_lru(bo);
+		__ttm_bo_unreserve(bo);
+	}
+	spin_unlock(&glob->lru_lock);
+
 	if (ticket)
 		ww_acquire_fini(ticket);
-	spin_unlock(&glob->lru_lock);
 }
 EXPORT_SYMBOL(ttm_eu_backoff_reservation);
 
@@ -112,7 +93,7 @@ EXPORT_SYMBOL(ttm_eu_backoff_reservation);
  */
 
 int ttm_eu_reserve_buffers(struct ww_acquire_ctx *ticket,
-			   struct list_head *list)
+			   struct list_head *list, bool intr)
 {
 	struct ttm_bo_global *glob;
 	struct ttm_validate_buffer *entry;
@@ -121,60 +102,64 @@ int ttm_eu_reserve_buffers(struct ww_acquire_ctx *ticket,
 	if (list_empty(list))
 		return 0;
 
-	list_for_each_entry(entry, list, head) {
-		entry->reserved = false;
-		entry->put_count = 0;
-		entry->removed = false;
-	}
-
 	entry = list_first_entry(list, struct ttm_validate_buffer, head);
 	glob = entry->bo->glob;
 
 	if (ticket)
 		ww_acquire_init(ticket, &reservation_ww_class);
-retry:
+
 	list_for_each_entry(entry, list, head) {
 		struct ttm_buffer_object *bo = entry->bo;
 
-		/* already slowpath reserved? */
-		if (entry->reserved)
-			continue;
-
-		ret = __ttm_bo_reserve(bo, true, (ticket == NULL), true,
+		ret = __ttm_bo_reserve(bo, intr, (ticket == NULL), true,
 				       ticket);
+		if (!ret && unlikely(atomic_read(&bo->cpu_writers) > 0)) {
+			__ttm_bo_unreserve(bo);
+
+			ret = -EBUSY;
+		}
 
-		if (ret == -EDEADLK) {
-			/* uh oh, we lost out, drop every reservation and try
-			 * to only reserve this buffer, then start over if
-			 * this succeeds.
-			 */
-			BUG_ON(ticket == NULL);
-			spin_lock(&glob->lru_lock);
-			ttm_eu_backoff_reservation_locked(list);
-			spin_unlock(&glob->lru_lock);
-			ttm_eu_list_ref_sub(list);
+		if (!ret) {
+			if (!entry->shared)
+				continue;
+
+			ret = reservation_object_reserve_shared(bo->resv);
+			if (!ret)
+				continue;
+		}
+
+		/* uh oh, we lost out, drop every reservation and try
+		 * to only reserve this buffer, then start over if
+		 * this succeeds.
+		 */
+		ttm_eu_backoff_reservation_reverse(list, entry);
+
+		if (ret == -EDEADLK && intr) {
 			ret = ww_mutex_lock_slow_interruptible(&bo->resv->lock,
 							       ticket);
-			if (unlikely(ret != 0)) {
-				if (ret == -EINTR)
-					ret = -ERESTARTSYS;
-				goto err_fini;
-			}
+		} else if (ret == -EDEADLK) {
+			ww_mutex_lock_slow(&bo->resv->lock, ticket);
+			ret = 0;
+		}
 
-			entry->reserved = true;
-			if (unlikely(atomic_read(&bo->cpu_writers) > 0)) {
-				ret = -EBUSY;
-				goto err;
-			}
-			goto retry;
-		} else if (ret)
-			goto err;
+		if (!ret && entry->shared)
+			ret = reservation_object_reserve_shared(bo->resv);
 
-		entry->reserved = true;
-		if (unlikely(atomic_read(&bo->cpu_writers) > 0)) {
-			ret = -EBUSY;
-			goto err;
+		if (unlikely(ret != 0)) {
+			if (ret == -EINTR)
+				ret = -ERESTARTSYS;
+			if (ticket) {
+				ww_acquire_done(ticket);
+				ww_acquire_fini(ticket);
+			}
+			return ret;
 		}
+
+		/* move this item to the front of the list,
+		 * forces correct iteration of the loop without keeping track
+		 */
+		list_del(&entry->head);
+		list_add(&entry->head, list);
 	}
 
 	if (ticket)
@@ -182,25 +167,12 @@ retry:
 	spin_lock(&glob->lru_lock);
 	ttm_eu_del_from_lru_locked(list);
 	spin_unlock(&glob->lru_lock);
-	ttm_eu_list_ref_sub(list);
 	return 0;
-
-err:
-	spin_lock(&glob->lru_lock);
-	ttm_eu_backoff_reservation_locked(list);
-	spin_unlock(&glob->lru_lock);
-	ttm_eu_list_ref_sub(list);
-err_fini:
-	if (ticket) {
-		ww_acquire_done(ticket);
-		ww_acquire_fini(ticket);
-	}
-	return ret;
 }
 EXPORT_SYMBOL(ttm_eu_reserve_buffers);
 
 void ttm_eu_fence_buffer_objects(struct ww_acquire_ctx *ticket,
-				 struct list_head *list, void *sync_obj)
+				 struct list_head *list, struct fence *fence)
 {
 	struct ttm_validate_buffer *entry;
 	struct ttm_buffer_object *bo;
@@ -217,24 +189,18 @@ void ttm_eu_fence_buffer_objects(struct ww_acquire_ctx *ticket,
 	glob = bo->glob;
 
 	spin_lock(&glob->lru_lock);
-	spin_lock(&bdev->fence_lock);
 
 	list_for_each_entry(entry, list, head) {
 		bo = entry->bo;
-		entry->old_sync_obj = bo->sync_obj;
-		bo->sync_obj = driver->sync_obj_ref(sync_obj);
+		if (entry->shared)
+			reservation_object_add_shared_fence(bo->resv, fence);
+		else
+			reservation_object_add_excl_fence(bo->resv, fence);
 		ttm_bo_add_to_lru(bo);
 		__ttm_bo_unreserve(bo);
-		entry->reserved = false;
 	}
-	spin_unlock(&bdev->fence_lock);
 	spin_unlock(&glob->lru_lock);
 	if (ticket)
 		ww_acquire_fini(ticket);
-
-	list_for_each_entry(entry, list, head) {
-		if (entry->old_sync_obj)
-			driver->sync_obj_unref(&entry->old_sync_obj);
-	}
 }
 EXPORT_SYMBOL(ttm_eu_fence_buffer_objects);
diff --git a/drivers/gpu/drm/ttm/ttm_memory.c b/drivers/gpu/drm/ttm/ttm_memory.c
index dbc2def887cd..a1803fbcc898 100644
--- a/drivers/gpu/drm/ttm/ttm_memory.c
+++ b/drivers/gpu/drm/ttm/ttm_memory.c
@@ -300,7 +300,8 @@ static int ttm_mem_init_highmem_zone(struct ttm_mem_global *glob,
 	zone->glob = glob;
 	glob->zone_highmem = zone;
 	ret = kobject_init_and_add(
-		&zone->kobj, &ttm_mem_zone_kobj_type, &glob->kobj, zone->name);
+		&zone->kobj, &ttm_mem_zone_kobj_type, &glob->kobj, "%s",
+		zone->name);
 	if (unlikely(ret != 0)) {
 		kobject_put(&zone->kobj);
 		return ret;
diff --git a/drivers/gpu/drm/udl/Kconfig b/drivers/gpu/drm/udl/Kconfig
index f02528686cd5..613ab0622d6e 100644
--- a/drivers/gpu/drm/udl/Kconfig
+++ b/drivers/gpu/drm/udl/Kconfig
@@ -1,8 +1,9 @@
 config DRM_UDL
 	tristate "DisplayLink"
 	depends on DRM
+	depends on USB_SUPPORT
 	depends on USB_ARCH_HAS_HCD
-	select DRM_USB
+	select USB
 	select FB_SYS_FILLRECT
 	select FB_SYS_COPYAREA
 	select FB_SYS_IMAGEBLIT
diff --git a/drivers/gpu/drm/udl/udl_connector.c b/drivers/gpu/drm/udl/udl_connector.c
index e026a9e2942a..0110d95522f3 100644
--- a/drivers/gpu/drm/udl/udl_connector.c
+++ b/drivers/gpu/drm/udl/udl_connector.c
@@ -34,8 +34,8 @@ static u8 *udl_get_edid(struct udl_device *udl)
 		goto error;
 
 	for (i = 0; i < EDID_LENGTH; i++) {
-		ret = usb_control_msg(udl->ddev->usbdev,
-				      usb_rcvctrlpipe(udl->ddev->usbdev, 0), (0x02),
+		ret = usb_control_msg(udl->udev,
+				      usb_rcvctrlpipe(udl->udev, 0), (0x02),
 				      (0x80 | (0x02 << 5)), i << 8, 0xA1, rbuf, 2,
 				      HZ);
 		if (ret < 1) {
diff --git a/drivers/gpu/drm/udl/udl_drv.c b/drivers/gpu/drm/udl/udl_drv.c
index 3ddd6cd98ac1..8607e9e513db 100644
--- a/drivers/gpu/drm/udl/udl_drv.c
+++ b/drivers/gpu/drm/udl/udl_drv.c
@@ -7,48 +7,13 @@
  */
 
 #include <linux/module.h>
-#include <drm/drm_usb.h>
+#include <drm/drmP.h>
 #include <drm/drm_crtc_helper.h>
 #include "udl_drv.h"
 
-static struct drm_driver driver;
-
-/*
- * There are many DisplayLink-based graphics products, all with unique PIDs.
- * So we match on DisplayLink's VID + Vendor-Defined Interface Class (0xff)
- * We also require a match on SubClass (0x00) and Protocol (0x00),
- * which is compatible with all known USB 2.0 era graphics chips and firmware,
- * but allows DisplayLink to increment those for any future incompatible chips
- */
-static struct usb_device_id id_table[] = {
-	{.idVendor = 0x17e9, .bInterfaceClass = 0xff,
-	 .bInterfaceSubClass = 0x00,
-	 .bInterfaceProtocol = 0x00,
-	 .match_flags = USB_DEVICE_ID_MATCH_VENDOR |
-			USB_DEVICE_ID_MATCH_INT_CLASS |
-			USB_DEVICE_ID_MATCH_INT_SUBCLASS |
-			USB_DEVICE_ID_MATCH_INT_PROTOCOL,},
-	{},
-};
-MODULE_DEVICE_TABLE(usb, id_table);
-
-MODULE_LICENSE("GPL");
-
-static int udl_usb_probe(struct usb_interface *interface,
-			 const struct usb_device_id *id)
+static int udl_driver_set_busid(struct drm_device *d, struct drm_master *m)
 {
-	return drm_get_usb_dev(interface, id, &driver);
-}
-
-static void udl_usb_disconnect(struct usb_interface *interface)
-{
-	struct drm_device *dev = usb_get_intfdata(interface);
-
-	drm_kms_helper_poll_disable(dev);
-	drm_connector_unplug_all(dev);
-	udl_fbdev_unplug(dev);
-	udl_drop_usb(dev);
-	drm_unplug_dev(dev);
+	return 0;
 }
 
 static const struct vm_operations_struct udl_gem_vm_ops = {
@@ -75,6 +40,7 @@ static struct drm_driver driver = {
 	.driver_features = DRIVER_MODESET | DRIVER_GEM | DRIVER_PRIME,
 	.load = udl_driver_load,
 	.unload = udl_driver_unload,
+	.set_busid = udl_driver_set_busid,
 
 	/* gem hooks */
 	.gem_free_object = udl_gem_free_object,
@@ -96,6 +62,61 @@ static struct drm_driver driver = {
 	.patchlevel = DRIVER_PATCHLEVEL,
 };
 
+static int udl_usb_probe(struct usb_interface *interface,
+			 const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(interface);
+	struct drm_device *dev;
+	int r;
+
+	dev = drm_dev_alloc(&driver, &interface->dev);
+	if (!dev)
+		return -ENOMEM;
+
+	r = drm_dev_register(dev, (unsigned long)udev);
+	if (r)
+		goto err_free;
+
+	usb_set_intfdata(interface, dev);
+	DRM_INFO("Initialized udl on minor %d\n", dev->primary->index);
+
+	return 0;
+
+err_free:
+	drm_dev_unref(dev);
+	return r;
+}
+
+static void udl_usb_disconnect(struct usb_interface *interface)
+{
+	struct drm_device *dev = usb_get_intfdata(interface);
+
+	drm_kms_helper_poll_disable(dev);
+	drm_connector_unplug_all(dev);
+	udl_fbdev_unplug(dev);
+	udl_drop_usb(dev);
+	drm_unplug_dev(dev);
+}
+
+/*
+ * There are many DisplayLink-based graphics products, all with unique PIDs.
+ * So we match on DisplayLink's VID + Vendor-Defined Interface Class (0xff)
+ * We also require a match on SubClass (0x00) and Protocol (0x00),
+ * which is compatible with all known USB 2.0 era graphics chips and firmware,
+ * but allows DisplayLink to increment those for any future incompatible chips
+ */
+static struct usb_device_id id_table[] = {
+	{.idVendor = 0x17e9, .bInterfaceClass = 0xff,
+	 .bInterfaceSubClass = 0x00,
+	 .bInterfaceProtocol = 0x00,
+	 .match_flags = USB_DEVICE_ID_MATCH_VENDOR |
+			USB_DEVICE_ID_MATCH_INT_CLASS |
+			USB_DEVICE_ID_MATCH_INT_SUBCLASS |
+			USB_DEVICE_ID_MATCH_INT_PROTOCOL,},
+	{},
+};
+MODULE_DEVICE_TABLE(usb, id_table);
+
 static struct usb_driver udl_driver = {
 	.name = "udl",
 	.probe = udl_usb_probe,
@@ -105,13 +126,14 @@ static struct usb_driver udl_driver = {
 
 static int __init udl_init(void)
 {
-	return drm_usb_init(&driver, &udl_driver);
+	return usb_register(&udl_driver);
 }
 
 static void __exit udl_exit(void)
 {
-	drm_usb_exit(&driver, &udl_driver);
+	usb_deregister(&udl_driver);
 }
 
 module_init(udl_init);
 module_exit(udl_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/gpu/drm/udl/udl_drv.h b/drivers/gpu/drm/udl/udl_drv.h
index 1fbf7b357f16..c7490a2489a7 100644
--- a/drivers/gpu/drm/udl/udl_drv.h
+++ b/drivers/gpu/drm/udl/udl_drv.h
@@ -15,6 +15,7 @@
 #define UDL_DRV_H
 
 #include <linux/usb.h>
+#include <drm/drm_gem.h>
 
 #define DRIVER_NAME		"udl"
 #define DRIVER_DESC		"DisplayLink"
@@ -47,6 +48,7 @@ struct udl_fbdev;
 struct udl_device {
 	struct device *dev;
 	struct drm_device *ddev;
+	struct usb_device *udev;
 
 	int sku_pixel_limit;
 
diff --git a/drivers/gpu/drm/udl/udl_fb.c b/drivers/gpu/drm/udl/udl_fb.c
index d1da339843ca..8cbcb4589bd3 100644
--- a/drivers/gpu/drm/udl/udl_fb.c
+++ b/drivers/gpu/drm/udl/udl_fb.c
@@ -472,7 +472,8 @@ udl_framebuffer_init(struct drm_device *dev,
 static int udlfb_create(struct drm_fb_helper *helper,
 			struct drm_fb_helper_surface_size *sizes)
 {
-	struct udl_fbdev *ufbdev = (struct udl_fbdev *)helper;
+	struct udl_fbdev *ufbdev =
+		container_of(helper, struct udl_fbdev, helper);
 	struct drm_device *dev = ufbdev->helper.dev;
 	struct fb_info *info;
 	struct device *device = dev->dev;
diff --git a/drivers/gpu/drm/udl/udl_main.c b/drivers/gpu/drm/udl/udl_main.c
index 42795674bc07..33dbfb2c4748 100644
--- a/drivers/gpu/drm/udl/udl_main.c
+++ b/drivers/gpu/drm/udl/udl_main.c
@@ -202,7 +202,7 @@ static int udl_alloc_urb_list(struct drm_device *dev, int count, size_t size)
 		}
 		unode->urb = urb;
 
-		buf = usb_alloc_coherent(udl->ddev->usbdev, MAX_TRANSFER, GFP_KERNEL,
+		buf = usb_alloc_coherent(udl->udev, MAX_TRANSFER, GFP_KERNEL,
 					 &urb->transfer_dma);
 		if (!buf) {
 			kfree(unode);
@@ -211,7 +211,7 @@ static int udl_alloc_urb_list(struct drm_device *dev, int count, size_t size)
 		}
 
 		/* urb->transfer_buffer_length set to actual before submit */
-		usb_fill_bulk_urb(urb, udl->ddev->usbdev, usb_sndbulkpipe(udl->ddev->usbdev, 1),
+		usb_fill_bulk_urb(urb, udl->udev, usb_sndbulkpipe(udl->udev, 1),
 			buf, size, udl_urb_completion, unode);
 		urb->transfer_flags |= URB_NO_TRANSFER_DMA_MAP;
 
@@ -282,6 +282,7 @@ int udl_submit_urb(struct drm_device *dev, struct urb *urb, size_t len)
 
 int udl_driver_load(struct drm_device *dev, unsigned long flags)
 {
+	struct usb_device *udev = (void*)flags;
 	struct udl_device *udl;
 	int ret = -ENOMEM;
 
@@ -290,10 +291,11 @@ int udl_driver_load(struct drm_device *dev, unsigned long flags)
 	if (!udl)
 		return -ENOMEM;
 
+	udl->udev = udev;
 	udl->ddev = dev;
 	dev->dev_private = udl;
 
-	if (!udl_parse_vendor_descriptor(dev, dev->usbdev)) {
+	if (!udl_parse_vendor_descriptor(dev, udl->udev)) {
 		ret = -ENODEV;
 		DRM_ERROR("firmware not recognized. Assume incompatible device\n");
 		goto err;
diff --git a/drivers/gpu/drm/via/via_dma.c b/drivers/gpu/drm/via/via_dma.c
index 6fc0648dd37f..d17d8f245c1a 100644
--- a/drivers/gpu/drm/via/via_dma.c
+++ b/drivers/gpu/drm/via/via_dma.c
@@ -161,7 +161,7 @@ int via_dma_cleanup(struct drm_device *dev)
 		if (dev_priv->ring.virtual_start) {
 			via_cmdbuf_reset(dev_priv);
 
-			drm_core_ioremapfree(&dev_priv->ring.map, dev);
+			drm_legacy_ioremapfree(&dev_priv->ring.map, dev);
 			dev_priv->ring.virtual_start = NULL;
 		}
 
@@ -200,7 +200,7 @@ static int via_initialize(struct drm_device *dev,
 	dev_priv->ring.map.flags = 0;
 	dev_priv->ring.map.mtrr = 0;
 
-	drm_core_ioremap(&dev_priv->ring.map, dev);
+	drm_legacy_ioremap(&dev_priv->ring.map, dev);
 
 	if (dev_priv->ring.map.handle == NULL) {
 		via_dma_cleanup(dev);
diff --git a/drivers/gpu/drm/via/via_drv.c b/drivers/gpu/drm/via/via_drv.c
index 50abc2adfaee..ed8aa8ff861a 100644
--- a/drivers/gpu/drm/via/via_drv.c
+++ b/drivers/gpu/drm/via/via_drv.c
@@ -62,7 +62,7 @@ static const struct file_operations via_driver_fops = {
 	.open = drm_open,
 	.release = drm_release,
 	.unlocked_ioctl = drm_ioctl,
-	.mmap = drm_mmap,
+	.mmap = drm_legacy_mmap,
 	.poll = drm_poll,
 #ifdef CONFIG_COMPAT
 	.compat_ioctl = drm_compat_ioctl,
@@ -79,6 +79,7 @@ static struct drm_driver driver = {
 	.open = via_driver_open,
 	.preclose = via_reclaim_buffers_locked,
 	.postclose = via_driver_postclose,
+	.set_busid = drm_pci_set_busid,
 	.context_dtor = via_final_context,
 	.get_vblank_counter = via_get_vblank_counter,
 	.enable_vblank = via_enable_vblank,
diff --git a/drivers/gpu/drm/via/via_drv.h b/drivers/gpu/drm/via/via_drv.h
index ad0273256beb..ef8c500b4a00 100644
--- a/drivers/gpu/drm/via/via_drv.h
+++ b/drivers/gpu/drm/via/via_drv.h
@@ -25,6 +25,8 @@
 #define _VIA_DRV_H_
 
 #include <drm/drm_mm.h>
+#include <drm/drm_legacy.h>
+
 #define DRIVER_AUTHOR	"Various"
 
 #define DRIVER_NAME		"via"
diff --git a/drivers/gpu/drm/via/via_map.c b/drivers/gpu/drm/via/via_map.c
index d0ab3fb32acd..0b3522dba6e8 100644
--- a/drivers/gpu/drm/via/via_map.c
+++ b/drivers/gpu/drm/via/via_map.c
@@ -31,7 +31,7 @@ static int via_do_init_map(struct drm_device *dev, drm_via_init_t *init)
 
 	DRM_DEBUG("\n");
 
-	dev_priv->sarea = drm_getsarea(dev);
+	dev_priv->sarea = drm_legacy_getsarea(dev);
 	if (!dev_priv->sarea) {
 		DRM_ERROR("could not find sarea!\n");
 		dev->dev_private = (void *)dev_priv;
@@ -39,14 +39,14 @@ static int via_do_init_map(struct drm_device *dev, drm_via_init_t *init)
 		return -EINVAL;
 	}
 
-	dev_priv->fb = drm_core_findmap(dev, init->fb_offset);
+	dev_priv->fb = drm_legacy_findmap(dev, init->fb_offset);
 	if (!dev_priv->fb) {
 		DRM_ERROR("could not find framebuffer!\n");
 		dev->dev_private = (void *)dev_priv;
 		via_do_cleanup_map(dev);
 		return -EINVAL;
 	}
-	dev_priv->mmio = drm_core_findmap(dev, init->mmio_offset);
+	dev_priv->mmio = drm_legacy_findmap(dev, init->mmio_offset);
 	if (!dev_priv->mmio) {
 		DRM_ERROR("could not find mmio region!\n");
 		dev->dev_private = (void *)dev_priv;
diff --git a/drivers/gpu/drm/via/via_mm.c b/drivers/gpu/drm/via/via_mm.c
index d70b1e1544bf..4f20742e7788 100644
--- a/drivers/gpu/drm/via/via_mm.c
+++ b/drivers/gpu/drm/via/via_mm.c
@@ -211,12 +211,12 @@ void via_reclaim_buffers_locked(struct drm_device *dev,
 	if (!(file->minor->master && file->master->lock.hw_lock))
 		return;
 
-	drm_idlelock_take(&file->master->lock);
+	drm_legacy_idlelock_take(&file->master->lock);
 
 	mutex_lock(&dev->struct_mutex);
 	if (list_empty(&file_priv->obj_list)) {
 		mutex_unlock(&dev->struct_mutex);
-		drm_idlelock_release(&file->master->lock);
+		drm_legacy_idlelock_release(&file->master->lock);
 
 		return;
 	}
@@ -231,7 +231,7 @@ void via_reclaim_buffers_locked(struct drm_device *dev,
 	}
 	mutex_unlock(&dev->struct_mutex);
 
-	drm_idlelock_release(&file->master->lock);
+	drm_legacy_idlelock_release(&file->master->lock);
 
 	return;
 }
diff --git a/drivers/gpu/drm/via/via_verifier.c b/drivers/gpu/drm/via/via_verifier.c
index 9dbc92bd1512..0677bbf4ec7e 100644
--- a/drivers/gpu/drm/via/via_verifier.c
+++ b/drivers/gpu/drm/via/via_verifier.c
@@ -31,6 +31,7 @@
 #include "via_3d_reg.h"
 #include <drm/drmP.h>
 #include <drm/via_drm.h>
+#include <drm/drm_legacy.h>
 #include "via_verifier.h"
 #include "via_drv.h"
 
diff --git a/drivers/gpu/drm/vmwgfx/svga_reg.h b/drivers/gpu/drm/vmwgfx/svga_reg.h
index 11323dd5196f..e4259c2c1acc 100644
--- a/drivers/gpu/drm/vmwgfx/svga_reg.h
+++ b/drivers/gpu/drm/vmwgfx/svga_reg.h
@@ -35,7 +35,6 @@
 /*
  * PCI device IDs.
  */
-#define PCI_VENDOR_ID_VMWARE            0x15AD
 #define PCI_DEVICE_ID_VMWARE_SVGA2      0x0405
 
 /*
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c b/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
index 6327cfc36805..cff2bf9db9d2 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_buffer.c
@@ -30,66 +30,101 @@
 #include <drm/ttm/ttm_placement.h>
 #include <drm/ttm/ttm_page_alloc.h>
 
-static uint32_t vram_placement_flags = TTM_PL_FLAG_VRAM |
-	TTM_PL_FLAG_CACHED;
-
-static uint32_t vram_ne_placement_flags = TTM_PL_FLAG_VRAM |
-	TTM_PL_FLAG_CACHED |
-	TTM_PL_FLAG_NO_EVICT;
+static struct ttm_place vram_placement_flags = {
+	.fpfn = 0,
+	.lpfn = 0,
+	.flags = TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED
+};
 
-static uint32_t sys_placement_flags = TTM_PL_FLAG_SYSTEM |
-	TTM_PL_FLAG_CACHED;
+static struct ttm_place vram_ne_placement_flags = {
+	.fpfn = 0,
+	.lpfn = 0,
+	.flags = TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED | TTM_PL_FLAG_NO_EVICT
+};
 
-static uint32_t sys_ne_placement_flags = TTM_PL_FLAG_SYSTEM |
-	TTM_PL_FLAG_CACHED |
-	TTM_PL_FLAG_NO_EVICT;
+static struct ttm_place sys_placement_flags = {
+	.fpfn = 0,
+	.lpfn = 0,
+	.flags = TTM_PL_FLAG_SYSTEM | TTM_PL_FLAG_CACHED
+};
 
-static uint32_t gmr_placement_flags = VMW_PL_FLAG_GMR |
-	TTM_PL_FLAG_CACHED;
+static struct ttm_place sys_ne_placement_flags = {
+	.fpfn = 0,
+	.lpfn = 0,
+	.flags = TTM_PL_FLAG_SYSTEM | TTM_PL_FLAG_CACHED | TTM_PL_FLAG_NO_EVICT
+};
 
-static uint32_t gmr_ne_placement_flags = VMW_PL_FLAG_GMR |
-	TTM_PL_FLAG_CACHED |
-	TTM_PL_FLAG_NO_EVICT;
+static struct ttm_place gmr_placement_flags = {
+	.fpfn = 0,
+	.lpfn = 0,
+	.flags = VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED
+};
 
-static uint32_t mob_placement_flags = VMW_PL_FLAG_MOB |
-	TTM_PL_FLAG_CACHED;
+static struct ttm_place gmr_ne_placement_flags = {
+	.fpfn = 0,
+	.lpfn = 0,
+	.flags = VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED | TTM_PL_FLAG_NO_EVICT
+};
 
-struct ttm_placement vmw_vram_placement = {
+static struct ttm_place mob_placement_flags = {
 	.fpfn = 0,
 	.lpfn = 0,
+	.flags = VMW_PL_FLAG_MOB | TTM_PL_FLAG_CACHED
+};
+
+struct ttm_placement vmw_vram_placement = {
 	.num_placement = 1,
 	.placement = &vram_placement_flags,
 	.num_busy_placement = 1,
 	.busy_placement = &vram_placement_flags
 };
 
-static uint32_t vram_gmr_placement_flags[] = {
-	TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED,
-	VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED
+static struct ttm_place vram_gmr_placement_flags[] = {
+	{
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED
+	}, {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED
+	}
 };
 
-static uint32_t gmr_vram_placement_flags[] = {
-	VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED,
-	TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED
+static struct ttm_place gmr_vram_placement_flags[] = {
+	{
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED
+	}, {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED
+	}
 };
 
 struct ttm_placement vmw_vram_gmr_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 2,
 	.placement = vram_gmr_placement_flags,
 	.num_busy_placement = 1,
 	.busy_placement = &gmr_placement_flags
 };
 
-static uint32_t vram_gmr_ne_placement_flags[] = {
-	TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED | TTM_PL_FLAG_NO_EVICT,
-	VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED | TTM_PL_FLAG_NO_EVICT
+static struct ttm_place vram_gmr_ne_placement_flags[] = {
+	{
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED |
+			 TTM_PL_FLAG_NO_EVICT
+	}, {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED |
+			 TTM_PL_FLAG_NO_EVICT
+	}
 };
 
 struct ttm_placement vmw_vram_gmr_ne_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 2,
 	.placement = vram_gmr_ne_placement_flags,
 	.num_busy_placement = 1,
@@ -97,8 +132,6 @@ struct ttm_placement vmw_vram_gmr_ne_placement = {
 };
 
 struct ttm_placement vmw_vram_sys_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 1,
 	.placement = &vram_placement_flags,
 	.num_busy_placement = 1,
@@ -106,8 +139,6 @@ struct ttm_placement vmw_vram_sys_placement = {
 };
 
 struct ttm_placement vmw_vram_ne_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 1,
 	.placement = &vram_ne_placement_flags,
 	.num_busy_placement = 1,
@@ -115,8 +146,6 @@ struct ttm_placement vmw_vram_ne_placement = {
 };
 
 struct ttm_placement vmw_sys_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 1,
 	.placement = &sys_placement_flags,
 	.num_busy_placement = 1,
@@ -124,24 +153,33 @@ struct ttm_placement vmw_sys_placement = {
 };
 
 struct ttm_placement vmw_sys_ne_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 1,
 	.placement = &sys_ne_placement_flags,
 	.num_busy_placement = 1,
 	.busy_placement = &sys_ne_placement_flags
 };
 
-static uint32_t evictable_placement_flags[] = {
-	TTM_PL_FLAG_SYSTEM | TTM_PL_FLAG_CACHED,
-	TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED,
-	VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED,
-	VMW_PL_FLAG_MOB | TTM_PL_FLAG_CACHED
+static struct ttm_place evictable_placement_flags[] = {
+	{
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_FLAG_SYSTEM | TTM_PL_FLAG_CACHED
+	}, {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = TTM_PL_FLAG_VRAM | TTM_PL_FLAG_CACHED
+	}, {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = VMW_PL_FLAG_GMR | TTM_PL_FLAG_CACHED
+	}, {
+		.fpfn = 0,
+		.lpfn = 0,
+		.flags = VMW_PL_FLAG_MOB | TTM_PL_FLAG_CACHED
+	}
 };
 
 struct ttm_placement vmw_evictable_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 4,
 	.placement = evictable_placement_flags,
 	.num_busy_placement = 1,
@@ -149,8 +187,6 @@ struct ttm_placement vmw_evictable_placement = {
 };
 
 struct ttm_placement vmw_srf_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 1,
 	.num_busy_placement = 2,
 	.placement = &gmr_placement_flags,
@@ -158,8 +194,6 @@ struct ttm_placement vmw_srf_placement = {
 };
 
 struct ttm_placement vmw_mob_placement = {
-	.fpfn = 0,
-	.lpfn = 0,
 	.num_placement = 1,
 	.num_busy_placement = 1,
 	.placement = &mob_placement_flags,
@@ -768,44 +802,6 @@ static int vmw_ttm_fault_reserve_notify(struct ttm_buffer_object *bo)
 }
 
 /**
- * FIXME: We're using the old vmware polling method to sync.
- * Do this with fences instead.
- */
-
-static void *vmw_sync_obj_ref(void *sync_obj)
-{
-
-	return (void *)
-		vmw_fence_obj_reference((struct vmw_fence_obj *) sync_obj);
-}
-
-static void vmw_sync_obj_unref(void **sync_obj)
-{
-	vmw_fence_obj_unreference((struct vmw_fence_obj **) sync_obj);
-}
-
-static int vmw_sync_obj_flush(void *sync_obj)
-{
-	vmw_fence_obj_flush((struct vmw_fence_obj *) sync_obj);
-	return 0;
-}
-
-static bool vmw_sync_obj_signaled(void *sync_obj)
-{
-	return	vmw_fence_obj_signaled((struct vmw_fence_obj *) sync_obj,
-				       DRM_VMW_FENCE_FLAG_EXEC);
-
-}
-
-static int vmw_sync_obj_wait(void *sync_obj, bool lazy, bool interruptible)
-{
-	return vmw_fence_obj_wait((struct vmw_fence_obj *) sync_obj,
-				  DRM_VMW_FENCE_FLAG_EXEC,
-				  lazy, interruptible,
-				  VMW_FENCE_WAIT_TIMEOUT);
-}
-
-/**
  * vmw_move_notify - TTM move_notify_callback
  *
  * @bo:             The TTM buffer object about to move.
@@ -829,11 +825,7 @@ static void vmw_move_notify(struct ttm_buffer_object *bo,
  */
 static void vmw_swap_notify(struct ttm_buffer_object *bo)
 {
-	struct ttm_bo_device *bdev = bo->bdev;
-
-	spin_lock(&bdev->fence_lock);
 	ttm_bo_wait(bo, false, false, false);
-	spin_unlock(&bdev->fence_lock);
 }
 
 
@@ -846,11 +838,6 @@ struct ttm_bo_driver vmw_bo_driver = {
 	.evict_flags = vmw_evict_flags,
 	.move = NULL,
 	.verify_access = vmw_verify_access,
-	.sync_obj_signaled = vmw_sync_obj_signaled,
-	.sync_obj_wait = vmw_sync_obj_wait,
-	.sync_obj_flush = vmw_sync_obj_flush,
-	.sync_obj_unref = vmw_sync_obj_unref,
-	.sync_obj_ref = vmw_sync_obj_ref,
 	.move_notify = vmw_move_notify,
 	.swap_notify = vmw_swap_notify,
 	.fault_reserve_notify = &vmw_ttm_fault_reserve_notify,
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_dmabuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_dmabuf.c
index ed1d51006ab1..914b375763dc 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_dmabuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_dmabuf.c
@@ -198,13 +198,19 @@ int vmw_dmabuf_to_start_of_vram(struct vmw_private *dev_priv,
 {
 	struct ttm_buffer_object *bo = &buf->base;
 	struct ttm_placement placement;
+	struct ttm_place place;
 	int ret = 0;
 
 	if (pin)
-		placement = vmw_vram_ne_placement;
+		place = vmw_vram_ne_placement.placement[0];
 	else
-		placement = vmw_vram_placement;
-	placement.lpfn = bo->num_pages;
+		place = vmw_vram_placement.placement[0];
+	place.lpfn = bo->num_pages;
+
+	placement.num_placement = 1;
+	placement.placement = &place;
+	placement.num_busy_placement = 1;
+	placement.busy_placement = &place;
 
 	ret = ttm_write_lock(&dev_priv->reservation_sem, interruptible);
 	if (unlikely(ret != 0))
@@ -293,21 +299,23 @@ void vmw_bo_get_guest_ptr(const struct ttm_buffer_object *bo,
  */
 void vmw_bo_pin(struct ttm_buffer_object *bo, bool pin)
 {
-	uint32_t pl_flags;
+	struct ttm_place pl;
 	struct ttm_placement placement;
 	uint32_t old_mem_type = bo->mem.mem_type;
 	int ret;
 
 	lockdep_assert_held(&bo->resv->lock.base);
 
-	pl_flags = TTM_PL_FLAG_VRAM | VMW_PL_FLAG_GMR | VMW_PL_FLAG_MOB
+	pl.fpfn = 0;
+	pl.lpfn = 0;
+	pl.flags = TTM_PL_FLAG_VRAM | VMW_PL_FLAG_GMR | VMW_PL_FLAG_MOB
 		| TTM_PL_FLAG_SYSTEM | TTM_PL_FLAG_CACHED;
 	if (pin)
-		pl_flags |= TTM_PL_FLAG_NO_EVICT;
+		pl.flags |= TTM_PL_FLAG_NO_EVICT;
 
 	memset(&placement, 0, sizeof(placement));
 	placement.num_placement = 1;
-	placement.placement = &pl_flags;
+	placement.placement = &pl;
 
 	ret = ttm_bo_validate(bo, &placement, false, true);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
index 18b54acacfbb..7197af157313 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.c
@@ -1418,6 +1418,7 @@ static struct drm_driver driver = {
 	.open = vmw_driver_open,
 	.preclose = vmw_preclose,
 	.postclose = vmw_postclose,
+	.set_busid = drm_pci_set_busid,
 
 	.dumb_create = vmw_dumb_create,
 	.dumb_map_offset = vmw_dumb_map_offset,
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
index 99f731757c4b..4ee799b43d5d 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_drv.h
@@ -342,7 +342,6 @@ struct vmw_sw_context{
 	uint32_t *cmd_bounce;
 	uint32_t cmd_bounce_size;
 	struct list_head resource_list;
-	uint32_t fence_flags;
 	struct ttm_buffer_object *cur_query_bo;
 	struct list_head res_relocations;
 	uint32_t *buf_start;
@@ -704,6 +703,7 @@ extern void *vmw_fifo_reserve(struct vmw_private *dev_priv, uint32_t bytes);
 extern void vmw_fifo_commit(struct vmw_private *dev_priv, uint32_t bytes);
 extern int vmw_fifo_send_fence(struct vmw_private *dev_priv,
 			       uint32_t *seqno);
+extern void vmw_fifo_ping_host_locked(struct vmw_private *, uint32_t reason);
 extern void vmw_fifo_ping_host(struct vmw_private *dev_priv, uint32_t reason);
 extern bool vmw_fifo_have_3d(struct vmw_private *dev_priv);
 extern bool vmw_fifo_have_pitchlock(struct vmw_private *dev_priv);
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
index 7bfdaa163a33..596cd6dafd33 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_execbuf.c
@@ -346,13 +346,11 @@ static int vmw_bo_to_validate_list(struct vmw_sw_context *sw_context,
 		++sw_context->cur_val_buf;
 		val_buf = &vval_buf->base;
 		val_buf->bo = ttm_bo_reference(bo);
-		val_buf->reserved = false;
+		val_buf->shared = false;
 		list_add_tail(&val_buf->head, &sw_context->validate_nodes);
 		vval_buf->validate_as_mob = validate_as_mob;
 	}
 
-	sw_context->fence_flags |= DRM_VMW_FENCE_FLAG_EXEC;
-
 	if (p_val_node)
 		*p_val_node = val_node;
 
@@ -450,11 +448,11 @@ static int vmw_cmd_res_reloc_add(struct vmw_private *dev_priv,
 					  res,
 					  id_loc - sw_context->buf_start);
 	if (unlikely(ret != 0))
-		goto out_err;
+		return ret;
 
 	ret = vmw_resource_val_add(sw_context, res, &node);
 	if (unlikely(ret != 0))
-		goto out_err;
+		return ret;
 
 	if (res_type == vmw_res_context && dev_priv->has_mob &&
 	    node->first_usage) {
@@ -468,13 +466,13 @@ static int vmw_cmd_res_reloc_add(struct vmw_private *dev_priv,
 
 		ret = vmw_resource_context_res_add(dev_priv, sw_context, res);
 		if (unlikely(ret != 0))
-			goto out_err;
+			return ret;
 		node->staged_bindings =
 			kzalloc(sizeof(*node->staged_bindings), GFP_KERNEL);
 		if (node->staged_bindings == NULL) {
 			DRM_ERROR("Failed to allocate context binding "
 				  "information.\n");
-			goto out_err;
+			return -ENOMEM;
 		}
 		INIT_LIST_HEAD(&node->staged_bindings->list);
 	}
@@ -482,8 +480,7 @@ static int vmw_cmd_res_reloc_add(struct vmw_private *dev_priv,
 	if (p_val)
 		*p_val = node;
 
-out_err:
-	return ret;
+	return 0;
 }
 
 
@@ -2338,13 +2335,9 @@ int vmw_execbuf_fence_commands(struct drm_file *file_priv,
 
 	if (p_handle != NULL)
 		ret = vmw_user_fence_create(file_priv, dev_priv->fman,
-					    sequence,
-					    DRM_VMW_FENCE_FLAG_EXEC,
-					    p_fence, p_handle);
+					    sequence, p_fence, p_handle);
 	else
-		ret = vmw_fence_create(dev_priv->fman, sequence,
-				       DRM_VMW_FENCE_FLAG_EXEC,
-				       p_fence);
+		ret = vmw_fence_create(dev_priv->fman, sequence, p_fence);
 
 	if (unlikely(ret != 0 && !synced)) {
 		(void) vmw_fallback_wait(dev_priv, false, false,
@@ -2396,7 +2389,7 @@ vmw_execbuf_copy_fence_user(struct vmw_private *dev_priv,
 		BUG_ON(fence == NULL);
 
 		fence_rep.handle = fence_handle;
-		fence_rep.seqno = fence->seqno;
+		fence_rep.seqno = fence->base.seqno;
 		vmw_update_seqno(dev_priv, &dev_priv->fifo);
 		fence_rep.passed_seqno = dev_priv->last_read_seqno;
 	}
@@ -2417,8 +2410,7 @@ vmw_execbuf_copy_fence_user(struct vmw_private *dev_priv,
 		ttm_ref_object_base_unref(vmw_fp->tfile,
 					  fence_handle, TTM_REF_USAGE);
 		DRM_ERROR("Fence copy error. Syncing.\n");
-		(void) vmw_fence_obj_wait(fence, fence->signal_mask,
-					  false, false,
+		(void) vmw_fence_obj_wait(fence, false, false,
 					  VMW_FENCE_WAIT_TIMEOUT);
 	}
 }
@@ -2470,7 +2462,6 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 	sw_context->fp = vmw_fpriv(file_priv);
 	sw_context->cur_reloc = 0;
 	sw_context->cur_val_buf = 0;
-	sw_context->fence_flags = 0;
 	INIT_LIST_HEAD(&sw_context->resource_list);
 	sw_context->cur_query_bo = dev_priv->pinned_bo;
 	sw_context->last_query_ctx = NULL;
@@ -2496,7 +2487,7 @@ int vmw_execbuf_process(struct drm_file *file_priv,
 	if (unlikely(ret != 0))
 		goto out_err_nores;
 
-	ret = ttm_eu_reserve_buffers(&ticket, &sw_context->validate_nodes);
+	ret = ttm_eu_reserve_buffers(&ticket, &sw_context->validate_nodes, true);
 	if (unlikely(ret != 0))
 		goto out_err;
 
@@ -2679,15 +2670,14 @@ void __vmw_execbuf_release_pinned_bo(struct vmw_private *dev_priv,
 	INIT_LIST_HEAD(&validate_list);
 
 	pinned_val.bo = ttm_bo_reference(dev_priv->pinned_bo);
+	pinned_val.shared = false;
 	list_add_tail(&pinned_val.head, &validate_list);
 
 	query_val.bo = ttm_bo_reference(dev_priv->dummy_query_bo);
+	query_val.shared = false;
 	list_add_tail(&query_val.head, &validate_list);
 
-	do {
-		ret = ttm_eu_reserve_buffers(&ticket, &validate_list);
-	} while (ret == -ERESTARTSYS);
-
+	ret = ttm_eu_reserve_buffers(&ticket, &validate_list, false);
 	if (unlikely(ret != 0)) {
 		vmw_execbuf_unpin_panic(dev_priv);
 		goto out_no_reserve;
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c
index b031b48dbb3c..0a474f391fad 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fb.c
@@ -374,10 +374,16 @@ static int vmw_fb_create_bo(struct vmw_private *vmw_priv,
 			    size_t size, struct vmw_dma_buffer **out)
 {
 	struct vmw_dma_buffer *vmw_bo;
-	struct ttm_placement ne_placement = vmw_vram_ne_placement;
+	struct ttm_place ne_place = vmw_vram_ne_placement.placement[0];
+	struct ttm_placement ne_placement;
 	int ret;
 
-	ne_placement.lpfn = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
+	ne_placement.num_placement = 1;
+	ne_placement.placement = &ne_place;
+	ne_placement.num_busy_placement = 1;
+	ne_placement.busy_placement = &ne_place;
+
+	ne_place.lpfn = (size + PAGE_SIZE - 1) >> PAGE_SHIFT;
 
 	(void) ttm_write_lock(&vmw_priv->reservation_sem, false);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
index 436b013b4231..197164fd7803 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.c
@@ -35,7 +35,7 @@ struct vmw_fence_manager {
 	struct vmw_private *dev_priv;
 	spinlock_t lock;
 	struct list_head fence_list;
-	struct work_struct work;
+	struct work_struct work, ping_work;
 	u32 user_fence_size;
 	u32 fence_size;
 	u32 event_fence_action_size;
@@ -46,6 +46,7 @@ struct vmw_fence_manager {
 	bool goal_irq_on; /* Protected by @goal_irq_mutex */
 	bool seqno_valid; /* Protected by @lock, and may not be set to true
 			     without the @goal_irq_mutex held. */
+	unsigned ctx;
 };
 
 struct vmw_user_fence {
@@ -80,6 +81,12 @@ struct vmw_event_fence_action {
 	uint32_t *tv_usec;
 };
 
+static struct vmw_fence_manager *
+fman_from_fence(struct vmw_fence_obj *fence)
+{
+	return container_of(fence->base.lock, struct vmw_fence_manager, lock);
+}
+
 /**
  * Note on fencing subsystem usage of irqs:
  * Typically the vmw_fences_update function is called
@@ -102,25 +109,143 @@ struct vmw_event_fence_action {
  * objects with actions attached to them.
  */
 
-static void vmw_fence_obj_destroy_locked(struct kref *kref)
+static void vmw_fence_obj_destroy(struct fence *f)
 {
 	struct vmw_fence_obj *fence =
-		container_of(kref, struct vmw_fence_obj, kref);
+		container_of(f, struct vmw_fence_obj, base);
 
-	struct vmw_fence_manager *fman = fence->fman;
-	unsigned int num_fences;
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
+	unsigned long irq_flags;
 
+	spin_lock_irqsave(&fman->lock, irq_flags);
 	list_del_init(&fence->head);
-	num_fences = --fman->num_fence_objects;
-	spin_unlock_irq(&fman->lock);
-	if (fence->destroy)
-		fence->destroy(fence);
-	else
-		kfree(fence);
+	--fman->num_fence_objects;
+	spin_unlock_irqrestore(&fman->lock, irq_flags);
+	fence->destroy(fence);
+}
 
-	spin_lock_irq(&fman->lock);
+static const char *vmw_fence_get_driver_name(struct fence *f)
+{
+	return "vmwgfx";
+}
+
+static const char *vmw_fence_get_timeline_name(struct fence *f)
+{
+	return "svga";
+}
+
+static void vmw_fence_ping_func(struct work_struct *work)
+{
+	struct vmw_fence_manager *fman =
+		container_of(work, struct vmw_fence_manager, ping_work);
+
+	vmw_fifo_ping_host(fman->dev_priv, SVGA_SYNC_GENERIC);
+}
+
+static bool vmw_fence_enable_signaling(struct fence *f)
+{
+	struct vmw_fence_obj *fence =
+		container_of(f, struct vmw_fence_obj, base);
+
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
+	struct vmw_private *dev_priv = fman->dev_priv;
+
+	__le32 __iomem *fifo_mem = dev_priv->mmio_virt;
+	u32 seqno = ioread32(fifo_mem + SVGA_FIFO_FENCE);
+	if (seqno - fence->base.seqno < VMW_FENCE_WRAP)
+		return false;
+
+	if (mutex_trylock(&dev_priv->hw_mutex)) {
+		vmw_fifo_ping_host_locked(dev_priv, SVGA_SYNC_GENERIC);
+		mutex_unlock(&dev_priv->hw_mutex);
+	} else
+		schedule_work(&fman->ping_work);
+
+	return true;
+}
+
+struct vmwgfx_wait_cb {
+	struct fence_cb base;
+	struct task_struct *task;
+};
+
+static void
+vmwgfx_wait_cb(struct fence *fence, struct fence_cb *cb)
+{
+	struct vmwgfx_wait_cb *wait =
+		container_of(cb, struct vmwgfx_wait_cb, base);
+
+	wake_up_process(wait->task);
+}
+
+static void __vmw_fences_update(struct vmw_fence_manager *fman);
+
+static long vmw_fence_wait(struct fence *f, bool intr, signed long timeout)
+{
+	struct vmw_fence_obj *fence =
+		container_of(f, struct vmw_fence_obj, base);
+
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
+	struct vmw_private *dev_priv = fman->dev_priv;
+	struct vmwgfx_wait_cb cb;
+	long ret = timeout;
+	unsigned long irq_flags;
+
+	if (likely(vmw_fence_obj_signaled(fence)))
+		return timeout;
+
+	vmw_fifo_ping_host(dev_priv, SVGA_SYNC_GENERIC);
+	vmw_seqno_waiter_add(dev_priv);
+
+	spin_lock_irqsave(f->lock, irq_flags);
+
+	if (intr && signal_pending(current)) {
+		ret = -ERESTARTSYS;
+		goto out;
+	}
+
+	cb.base.func = vmwgfx_wait_cb;
+	cb.task = current;
+	list_add(&cb.base.node, &f->cb_list);
+
+	while (ret > 0) {
+		__vmw_fences_update(fman);
+		if (test_bit(FENCE_FLAG_SIGNALED_BIT, &f->flags))
+			break;
+
+		if (intr)
+			__set_current_state(TASK_INTERRUPTIBLE);
+		else
+			__set_current_state(TASK_UNINTERRUPTIBLE);
+		spin_unlock_irqrestore(f->lock, irq_flags);
+
+		ret = schedule_timeout(ret);
+
+		spin_lock_irqsave(f->lock, irq_flags);
+		if (ret > 0 && intr && signal_pending(current))
+			ret = -ERESTARTSYS;
+	}
+
+	if (!list_empty(&cb.base.node))
+		list_del(&cb.base.node);
+	__set_current_state(TASK_RUNNING);
+
+out:
+	spin_unlock_irqrestore(f->lock, irq_flags);
+
+	vmw_seqno_waiter_remove(dev_priv);
+
+	return ret;
 }
 
+static struct fence_ops vmw_fence_ops = {
+	.get_driver_name = vmw_fence_get_driver_name,
+	.get_timeline_name = vmw_fence_get_timeline_name,
+	.enable_signaling = vmw_fence_enable_signaling,
+	.wait = vmw_fence_wait,
+	.release = vmw_fence_obj_destroy,
+};
+
 
 /**
  * Execute signal actions on fences recently signaled.
@@ -180,12 +305,14 @@ struct vmw_fence_manager *vmw_fence_manager_init(struct vmw_private *dev_priv)
 	INIT_LIST_HEAD(&fman->fence_list);
 	INIT_LIST_HEAD(&fman->cleanup_list);
 	INIT_WORK(&fman->work, &vmw_fence_work_func);
+	INIT_WORK(&fman->ping_work, &vmw_fence_ping_func);
 	fman->fifo_down = true;
 	fman->user_fence_size = ttm_round_pot(sizeof(struct vmw_user_fence));
 	fman->fence_size = ttm_round_pot(sizeof(struct vmw_fence_obj));
 	fman->event_fence_action_size =
 		ttm_round_pot(sizeof(struct vmw_event_fence_action));
 	mutex_init(&fman->goal_irq_mutex);
+	fman->ctx = fence_context_alloc(1);
 
 	return fman;
 }
@@ -196,6 +323,7 @@ void vmw_fence_manager_takedown(struct vmw_fence_manager *fman)
 	bool lists_empty;
 
 	(void) cancel_work_sync(&fman->work);
+	(void) cancel_work_sync(&fman->ping_work);
 
 	spin_lock_irqsave(&fman->lock, irq_flags);
 	lists_empty = list_empty(&fman->fence_list) &&
@@ -207,23 +335,16 @@ void vmw_fence_manager_takedown(struct vmw_fence_manager *fman)
 }
 
 static int vmw_fence_obj_init(struct vmw_fence_manager *fman,
-			      struct vmw_fence_obj *fence,
-			      u32 seqno,
-			      uint32_t mask,
+			      struct vmw_fence_obj *fence, u32 seqno,
 			      void (*destroy) (struct vmw_fence_obj *fence))
 {
 	unsigned long irq_flags;
-	unsigned int num_fences;
 	int ret = 0;
 
-	fence->seqno = seqno;
+	fence_init(&fence->base, &vmw_fence_ops, &fman->lock,
+		   fman->ctx, seqno);
 	INIT_LIST_HEAD(&fence->seq_passed_actions);
-	fence->fman = fman;
-	fence->signaled = 0;
-	fence->signal_mask = mask;
-	kref_init(&fence->kref);
 	fence->destroy = destroy;
-	init_waitqueue_head(&fence->queue);
 
 	spin_lock_irqsave(&fman->lock, irq_flags);
 	if (unlikely(fman->fifo_down)) {
@@ -231,7 +352,7 @@ static int vmw_fence_obj_init(struct vmw_fence_manager *fman,
 		goto out_unlock;
 	}
 	list_add_tail(&fence->head, &fman->fence_list);
-	num_fences = ++fman->num_fence_objects;
+	++fman->num_fence_objects;
 
 out_unlock:
 	spin_unlock_irqrestore(&fman->lock, irq_flags);
@@ -239,38 +360,6 @@ out_unlock:
 
 }
 
-struct vmw_fence_obj *vmw_fence_obj_reference(struct vmw_fence_obj *fence)
-{
-	if (unlikely(fence == NULL))
-		return NULL;
-
-	kref_get(&fence->kref);
-	return fence;
-}
-
-/**
- * vmw_fence_obj_unreference
- *
- * Note that this function may not be entered with disabled irqs since
- * it may re-enable them in the destroy function.
- *
- */
-void vmw_fence_obj_unreference(struct vmw_fence_obj **fence_p)
-{
-	struct vmw_fence_obj *fence = *fence_p;
-	struct vmw_fence_manager *fman;
-
-	if (unlikely(fence == NULL))
-		return;
-
-	fman = fence->fman;
-	*fence_p = NULL;
-	spin_lock_irq(&fman->lock);
-	BUG_ON(atomic_read(&fence->kref.refcount) == 0);
-	kref_put(&fence->kref, vmw_fence_obj_destroy_locked);
-	spin_unlock_irq(&fman->lock);
-}
-
 static void vmw_fences_perform_actions(struct vmw_fence_manager *fman,
 				struct list_head *list)
 {
@@ -326,7 +415,7 @@ static bool vmw_fence_goal_new_locked(struct vmw_fence_manager *fman,
 	list_for_each_entry(fence, &fman->fence_list, head) {
 		if (!list_empty(&fence->seq_passed_actions)) {
 			fman->seqno_valid = true;
-			iowrite32(fence->seqno,
+			iowrite32(fence->base.seqno,
 				  fifo_mem + SVGA_FIFO_FENCE_GOAL);
 			break;
 		}
@@ -353,27 +442,27 @@ static bool vmw_fence_goal_new_locked(struct vmw_fence_manager *fman,
  */
 static bool vmw_fence_goal_check_locked(struct vmw_fence_obj *fence)
 {
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
 	u32 goal_seqno;
 	__le32 __iomem *fifo_mem;
 
-	if (fence->signaled & DRM_VMW_FENCE_FLAG_EXEC)
+	if (fence_is_signaled_locked(&fence->base))
 		return false;
 
-	fifo_mem = fence->fman->dev_priv->mmio_virt;
+	fifo_mem = fman->dev_priv->mmio_virt;
 	goal_seqno = ioread32(fifo_mem + SVGA_FIFO_FENCE_GOAL);
-	if (likely(fence->fman->seqno_valid &&
-		   goal_seqno - fence->seqno < VMW_FENCE_WRAP))
+	if (likely(fman->seqno_valid &&
+		   goal_seqno - fence->base.seqno < VMW_FENCE_WRAP))
 		return false;
 
-	iowrite32(fence->seqno, fifo_mem + SVGA_FIFO_FENCE_GOAL);
-	fence->fman->seqno_valid = true;
+	iowrite32(fence->base.seqno, fifo_mem + SVGA_FIFO_FENCE_GOAL);
+	fman->seqno_valid = true;
 
 	return true;
 }
 
-void vmw_fences_update(struct vmw_fence_manager *fman)
+static void __vmw_fences_update(struct vmw_fence_manager *fman)
 {
-	unsigned long flags;
 	struct vmw_fence_obj *fence, *next_fence;
 	struct list_head action_list;
 	bool needs_rerun;
@@ -382,32 +471,25 @@ void vmw_fences_update(struct vmw_fence_manager *fman)
 
 	seqno = ioread32(fifo_mem + SVGA_FIFO_FENCE);
 rerun:
-	spin_lock_irqsave(&fman->lock, flags);
 	list_for_each_entry_safe(fence, next_fence, &fman->fence_list, head) {
-		if (seqno - fence->seqno < VMW_FENCE_WRAP) {
+		if (seqno - fence->base.seqno < VMW_FENCE_WRAP) {
 			list_del_init(&fence->head);
-			fence->signaled |= DRM_VMW_FENCE_FLAG_EXEC;
+			fence_signal_locked(&fence->base);
 			INIT_LIST_HEAD(&action_list);
 			list_splice_init(&fence->seq_passed_actions,
 					 &action_list);
 			vmw_fences_perform_actions(fman, &action_list);
-			wake_up_all(&fence->queue);
 		} else
 			break;
 	}
 
-	needs_rerun = vmw_fence_goal_new_locked(fman, seqno);
-
-	if (!list_empty(&fman->cleanup_list))
-		(void) schedule_work(&fman->work);
-	spin_unlock_irqrestore(&fman->lock, flags);
-
 	/*
 	 * Rerun if the fence goal seqno was updated, and the
 	 * hardware might have raced with that update, so that
 	 * we missed a fence_goal irq.
 	 */
 
+	needs_rerun = vmw_fence_goal_new_locked(fman, seqno);
 	if (unlikely(needs_rerun)) {
 		new_seqno = ioread32(fifo_mem + SVGA_FIFO_FENCE);
 		if (new_seqno != seqno) {
@@ -415,79 +497,58 @@ rerun:
 			goto rerun;
 		}
 	}
+
+	if (!list_empty(&fman->cleanup_list))
+		(void) schedule_work(&fman->work);
 }
 
-bool vmw_fence_obj_signaled(struct vmw_fence_obj *fence,
-			    uint32_t flags)
+void vmw_fences_update(struct vmw_fence_manager *fman)
 {
-	struct vmw_fence_manager *fman = fence->fman;
 	unsigned long irq_flags;
-	uint32_t signaled;
 
 	spin_lock_irqsave(&fman->lock, irq_flags);
-	signaled = fence->signaled;
+	__vmw_fences_update(fman);
 	spin_unlock_irqrestore(&fman->lock, irq_flags);
+}
 
-	flags &= fence->signal_mask;
-	if ((signaled & flags) == flags)
-		return 1;
+bool vmw_fence_obj_signaled(struct vmw_fence_obj *fence)
+{
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
 
-	if ((signaled & DRM_VMW_FENCE_FLAG_EXEC) == 0)
-		vmw_fences_update(fman);
+	if (test_bit(FENCE_FLAG_SIGNALED_BIT, &fence->base.flags))
+		return 1;
 
-	spin_lock_irqsave(&fman->lock, irq_flags);
-	signaled = fence->signaled;
-	spin_unlock_irqrestore(&fman->lock, irq_flags);
+	vmw_fences_update(fman);
 
-	return ((signaled & flags) == flags);
+	return fence_is_signaled(&fence->base);
 }
 
-int vmw_fence_obj_wait(struct vmw_fence_obj *fence,
-		       uint32_t flags, bool lazy,
+int vmw_fence_obj_wait(struct vmw_fence_obj *fence, bool lazy,
 		       bool interruptible, unsigned long timeout)
 {
-	struct vmw_private *dev_priv = fence->fman->dev_priv;
-	long ret;
+	long ret = fence_wait_timeout(&fence->base, interruptible, timeout);
 
-	if (likely(vmw_fence_obj_signaled(fence, flags)))
+	if (likely(ret > 0))
 		return 0;
-
-	vmw_fifo_ping_host(dev_priv, SVGA_SYNC_GENERIC);
-	vmw_seqno_waiter_add(dev_priv);
-
-	if (interruptible)
-		ret = wait_event_interruptible_timeout
-			(fence->queue,
-			 vmw_fence_obj_signaled(fence, flags),
-			 timeout);
+	else if (ret == 0)
+		return -EBUSY;
 	else
-		ret = wait_event_timeout
-			(fence->queue,
-			 vmw_fence_obj_signaled(fence, flags),
-			 timeout);
-
-	vmw_seqno_waiter_remove(dev_priv);
-
-	if (unlikely(ret == 0))
-		ret = -EBUSY;
-	else if (likely(ret > 0))
-		ret = 0;
-
-	return ret;
+		return ret;
 }
 
 void vmw_fence_obj_flush(struct vmw_fence_obj *fence)
 {
-	struct vmw_private *dev_priv = fence->fman->dev_priv;
+	struct vmw_private *dev_priv = fman_from_fence(fence)->dev_priv;
 
 	vmw_fifo_ping_host(dev_priv, SVGA_SYNC_GENERIC);
 }
 
 static void vmw_fence_destroy(struct vmw_fence_obj *fence)
 {
-	struct vmw_fence_manager *fman = fence->fman;
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
+
+	fence_free(&fence->base);
 
-	kfree(fence);
 	/*
 	 * Free kernel space accounting.
 	 */
@@ -497,7 +558,6 @@ static void vmw_fence_destroy(struct vmw_fence_obj *fence)
 
 int vmw_fence_create(struct vmw_fence_manager *fman,
 		     uint32_t seqno,
-		     uint32_t mask,
 		     struct vmw_fence_obj **p_fence)
 {
 	struct ttm_mem_global *mem_glob = vmw_mem_glob(fman->dev_priv);
@@ -515,7 +575,7 @@ int vmw_fence_create(struct vmw_fence_manager *fman,
 		goto out_no_object;
 	}
 
-	ret = vmw_fence_obj_init(fman, fence, seqno, mask,
+	ret = vmw_fence_obj_init(fman, fence, seqno,
 				 vmw_fence_destroy);
 	if (unlikely(ret != 0))
 		goto out_err_init;
@@ -535,7 +595,7 @@ static void vmw_user_fence_destroy(struct vmw_fence_obj *fence)
 {
 	struct vmw_user_fence *ufence =
 		container_of(fence, struct vmw_user_fence, fence);
-	struct vmw_fence_manager *fman = fence->fman;
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
 
 	ttm_base_object_kfree(ufence, base);
 	/*
@@ -559,7 +619,6 @@ static void vmw_user_fence_base_release(struct ttm_base_object **p_base)
 int vmw_user_fence_create(struct drm_file *file_priv,
 			  struct vmw_fence_manager *fman,
 			  uint32_t seqno,
-			  uint32_t mask,
 			  struct vmw_fence_obj **p_fence,
 			  uint32_t *p_handle)
 {
@@ -586,7 +645,7 @@ int vmw_user_fence_create(struct drm_file *file_priv,
 	}
 
 	ret = vmw_fence_obj_init(fman, &ufence->fence, seqno,
-				 mask, vmw_user_fence_destroy);
+				 vmw_user_fence_destroy);
 	if (unlikely(ret != 0)) {
 		kfree(ufence);
 		goto out_no_object;
@@ -629,7 +688,6 @@ out_no_object:
 
 void vmw_fence_fifo_down(struct vmw_fence_manager *fman)
 {
-	unsigned long irq_flags;
 	struct list_head action_list;
 	int ret;
 
@@ -638,35 +696,32 @@ void vmw_fence_fifo_down(struct vmw_fence_manager *fman)
 	 * restart when we've released the fman->lock.
 	 */
 
-	spin_lock_irqsave(&fman->lock, irq_flags);
+	spin_lock_irq(&fman->lock);
 	fman->fifo_down = true;
 	while (!list_empty(&fman->fence_list)) {
 		struct vmw_fence_obj *fence =
 			list_entry(fman->fence_list.prev, struct vmw_fence_obj,
 				   head);
-		kref_get(&fence->kref);
+		fence_get(&fence->base);
 		spin_unlock_irq(&fman->lock);
 
-		ret = vmw_fence_obj_wait(fence, fence->signal_mask,
-					 false, false,
+		ret = vmw_fence_obj_wait(fence, false, false,
 					 VMW_FENCE_WAIT_TIMEOUT);
 
 		if (unlikely(ret != 0)) {
 			list_del_init(&fence->head);
-			fence->signaled |= DRM_VMW_FENCE_FLAG_EXEC;
+			fence_signal(&fence->base);
 			INIT_LIST_HEAD(&action_list);
 			list_splice_init(&fence->seq_passed_actions,
 					 &action_list);
 			vmw_fences_perform_actions(fman, &action_list);
-			wake_up_all(&fence->queue);
 		}
 
-		spin_lock_irq(&fman->lock);
-
 		BUG_ON(!list_empty(&fence->head));
-		kref_put(&fence->kref, vmw_fence_obj_destroy_locked);
+		fence_put(&fence->base);
+		spin_lock_irq(&fman->lock);
 	}
-	spin_unlock_irqrestore(&fman->lock, irq_flags);
+	spin_unlock_irq(&fman->lock);
 }
 
 void vmw_fence_fifo_up(struct vmw_fence_manager *fman)
@@ -716,14 +771,14 @@ int vmw_fence_obj_wait_ioctl(struct drm_device *dev, void *data,
 
 	timeout = jiffies;
 	if (time_after_eq(timeout, (unsigned long)arg->kernel_cookie)) {
-		ret = ((vmw_fence_obj_signaled(fence, arg->flags)) ?
+		ret = ((vmw_fence_obj_signaled(fence)) ?
 		       0 : -EBUSY);
 		goto out;
 	}
 
 	timeout = (unsigned long)arg->kernel_cookie - timeout;
 
-	ret = vmw_fence_obj_wait(fence, arg->flags, arg->lazy, true, timeout);
+	ret = vmw_fence_obj_wait(fence, arg->lazy, true, timeout);
 
 out:
 	ttm_base_object_unref(&base);
@@ -758,12 +813,12 @@ int vmw_fence_obj_signaled_ioctl(struct drm_device *dev, void *data,
 	}
 
 	fence = &(container_of(base, struct vmw_user_fence, base)->fence);
-	fman = fence->fman;
+	fman = fman_from_fence(fence);
 
-	arg->signaled = vmw_fence_obj_signaled(fence, arg->flags);
-	spin_lock_irq(&fman->lock);
+	arg->signaled = vmw_fence_obj_signaled(fence);
 
-	arg->signaled_flags = fence->signaled;
+	arg->signaled_flags = arg->flags;
+	spin_lock_irq(&fman->lock);
 	arg->passed_seqno = dev_priv->last_read_seqno;
 	spin_unlock_irq(&fman->lock);
 
@@ -876,7 +931,7 @@ static void vmw_event_fence_action_cleanup(struct vmw_fence_action *action)
 {
 	struct vmw_event_fence_action *eaction =
 		container_of(action, struct vmw_event_fence_action, action);
-	struct vmw_fence_manager *fman = eaction->fence->fman;
+	struct vmw_fence_manager *fman = fman_from_fence(eaction->fence);
 	unsigned long irq_flags;
 
 	spin_lock_irqsave(&fman->lock, irq_flags);
@@ -900,7 +955,7 @@ static void vmw_event_fence_action_cleanup(struct vmw_fence_action *action)
 static void vmw_fence_obj_add_action(struct vmw_fence_obj *fence,
 			      struct vmw_fence_action *action)
 {
-	struct vmw_fence_manager *fman = fence->fman;
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
 	unsigned long irq_flags;
 	bool run_update = false;
 
@@ -908,7 +963,7 @@ static void vmw_fence_obj_add_action(struct vmw_fence_obj *fence,
 	spin_lock_irqsave(&fman->lock, irq_flags);
 
 	fman->pending_actions[action->type]++;
-	if (fence->signaled & DRM_VMW_FENCE_FLAG_EXEC) {
+	if (fence_is_signaled_locked(&fence->base)) {
 		struct list_head action_list;
 
 		INIT_LIST_HEAD(&action_list);
@@ -960,7 +1015,7 @@ int vmw_event_fence_action_queue(struct drm_file *file_priv,
 				 bool interruptible)
 {
 	struct vmw_event_fence_action *eaction;
-	struct vmw_fence_manager *fman = fence->fman;
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
 	struct vmw_fpriv *vmw_fp = vmw_fpriv(file_priv);
 	unsigned long irq_flags;
 
@@ -1000,7 +1055,8 @@ static int vmw_event_fence_action_create(struct drm_file *file_priv,
 				  bool interruptible)
 {
 	struct vmw_event_fence_pending *event;
-	struct drm_device *dev = fence->fman->dev_priv->dev;
+	struct vmw_fence_manager *fman = fman_from_fence(fence);
+	struct drm_device *dev = fman->dev_priv->dev;
 	unsigned long irq_flags;
 	int ret;
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.h b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.h
index faf2e7873860..26a4add39208 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fence.h
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fence.h
@@ -27,6 +27,8 @@
 
 #ifndef _VMWGFX_FENCE_H_
 
+#include <linux/fence.h>
+
 #define VMW_FENCE_WAIT_TIMEOUT (5*HZ)
 
 struct vmw_private;
@@ -50,16 +52,11 @@ struct vmw_fence_action {
 };
 
 struct vmw_fence_obj {
-	struct kref kref;
-	u32 seqno;
+	struct fence base;
 
-	struct vmw_fence_manager *fman;
 	struct list_head head;
-	uint32_t signaled;
-	uint32_t signal_mask;
 	struct list_head seq_passed_actions;
 	void (*destroy)(struct vmw_fence_obj *fence);
-	wait_queue_head_t queue;
 };
 
 extern struct vmw_fence_manager *
@@ -67,17 +64,29 @@ vmw_fence_manager_init(struct vmw_private *dev_priv);
 
 extern void vmw_fence_manager_takedown(struct vmw_fence_manager *fman);
 
-extern void vmw_fence_obj_unreference(struct vmw_fence_obj **fence_p);
+static inline void
+vmw_fence_obj_unreference(struct vmw_fence_obj **fence_p)
+{
+	struct vmw_fence_obj *fence = *fence_p;
+
+	*fence_p = NULL;
+	if (fence)
+		fence_put(&fence->base);
+}
 
-extern struct vmw_fence_obj *
-vmw_fence_obj_reference(struct vmw_fence_obj *fence);
+static inline struct vmw_fence_obj *
+vmw_fence_obj_reference(struct vmw_fence_obj *fence)
+{
+	if (fence)
+		fence_get(&fence->base);
+	return fence;
+}
 
 extern void vmw_fences_update(struct vmw_fence_manager *fman);
 
-extern bool vmw_fence_obj_signaled(struct vmw_fence_obj *fence,
-				   uint32_t flags);
+extern bool vmw_fence_obj_signaled(struct vmw_fence_obj *fence);
 
-extern int vmw_fence_obj_wait(struct vmw_fence_obj *fence, uint32_t flags,
+extern int vmw_fence_obj_wait(struct vmw_fence_obj *fence,
 			      bool lazy,
 			      bool interruptible, unsigned long timeout);
 
@@ -85,13 +94,11 @@ extern void vmw_fence_obj_flush(struct vmw_fence_obj *fence);
 
 extern int vmw_fence_create(struct vmw_fence_manager *fman,
 			    uint32_t seqno,
-			    uint32_t mask,
 			    struct vmw_fence_obj **p_fence);
 
 extern int vmw_user_fence_create(struct drm_file *file_priv,
 				 struct vmw_fence_manager *fman,
 				 uint32_t sequence,
-				 uint32_t mask,
 				 struct vmw_fence_obj **p_fence,
 				 uint32_t *p_handle);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c b/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c
index 6ccd993e26bf..09e10aefcd8e 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_fifo.c
@@ -160,16 +160,21 @@ int vmw_fifo_init(struct vmw_private *dev_priv, struct vmw_fifo_state *fifo)
 	return vmw_fifo_send_fence(dev_priv, &dummy);
 }
 
-void vmw_fifo_ping_host(struct vmw_private *dev_priv, uint32_t reason)
+void vmw_fifo_ping_host_locked(struct vmw_private *dev_priv, uint32_t reason)
 {
 	__le32 __iomem *fifo_mem = dev_priv->mmio_virt;
 
-	mutex_lock(&dev_priv->hw_mutex);
-
 	if (unlikely(ioread32(fifo_mem + SVGA_FIFO_BUSY) == 0)) {
 		iowrite32(1, fifo_mem + SVGA_FIFO_BUSY);
 		vmw_write(dev_priv, SVGA_REG_SYNC, reason);
 	}
+}
+
+void vmw_fifo_ping_host(struct vmw_private *dev_priv, uint32_t reason)
+{
+	mutex_lock(&dev_priv->hw_mutex);
+
+	vmw_fifo_ping_host_locked(dev_priv, reason);
 
 	mutex_unlock(&dev_priv->hw_mutex);
 }
@@ -180,8 +185,9 @@ void vmw_fifo_release(struct vmw_private *dev_priv, struct vmw_fifo_state *fifo)
 
 	mutex_lock(&dev_priv->hw_mutex);
 
+	vmw_write(dev_priv, SVGA_REG_SYNC, SVGA_SYNC_GENERIC);
 	while (vmw_read(dev_priv, SVGA_REG_BUSY) != 0)
-		vmw_write(dev_priv, SVGA_REG_SYNC, SVGA_SYNC_GENERIC);
+		;
 
 	dev_priv->last_read_seqno = ioread32(fifo_mem + SVGA_FIFO_FENCE);
 
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_gmrid_manager.c b/drivers/gpu/drm/vmwgfx/vmwgfx_gmrid_manager.c
index 26f8bdde3529..170b61be1e4e 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_gmrid_manager.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_gmrid_manager.c
@@ -46,8 +46,7 @@ struct vmwgfx_gmrid_man {
 
 static int vmw_gmrid_man_get_node(struct ttm_mem_type_manager *man,
 				  struct ttm_buffer_object *bo,
-				  struct ttm_placement *placement,
-				  uint32_t flags,
+				  const struct ttm_place *place,
 				  struct ttm_mem_reg *mem)
 {
 	struct vmwgfx_gmrid_man *gman =
diff --git a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
index a432c0db257c..026de7cea0f6 100644
--- a/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
+++ b/drivers/gpu/drm/vmwgfx/vmwgfx_resource.c
@@ -133,6 +133,7 @@ static void vmw_resource_release(struct kref *kref)
 			struct ttm_validate_buffer val_buf;
 
 			val_buf.bo = bo;
+			val_buf.shared = false;
 			res->func->unbind(res, false, &val_buf);
 		}
 		res->backup_dirty = false;
@@ -429,7 +430,7 @@ int vmw_dmabuf_init(struct vmw_private *dev_priv,
 	ret = ttm_bo_init(bdev, &vmw_bo->base, size,
 			  ttm_bo_type_device, placement,
 			  0, interruptible,
-			  NULL, acc_size, NULL, bo_free);
+			  NULL, acc_size, NULL, NULL, bo_free);
 	return ret;
 }
 
@@ -567,13 +568,18 @@ static int vmw_user_dmabuf_synccpu_grab(struct vmw_user_dma_buffer *user_bo,
 	int ret;
 
 	if (flags & drm_vmw_synccpu_allow_cs) {
-		struct ttm_bo_device *bdev = bo->bdev;
+		bool nonblock = !!(flags & drm_vmw_synccpu_dontblock);
+		long lret;
 
-		spin_lock(&bdev->fence_lock);
-		ret = ttm_bo_wait(bo, false, true,
-				  !!(flags & drm_vmw_synccpu_dontblock));
-		spin_unlock(&bdev->fence_lock);
-		return ret;
+		if (nonblock)
+			return reservation_object_test_signaled_rcu(bo->resv, true) ? 0 : -EBUSY;
+
+		lret = reservation_object_wait_timeout_rcu(bo->resv, true, true, MAX_SCHEDULE_TIMEOUT);
+		if (!lret)
+			return -EBUSY;
+		else if (lret < 0)
+			return lret;
+		return 0;
 	}
 
 	ret = ttm_bo_synccpu_write_grab
@@ -1214,8 +1220,9 @@ vmw_resource_check_buffer(struct vmw_resource *res,
 
 	INIT_LIST_HEAD(&val_list);
 	val_buf->bo = ttm_bo_reference(&res->backup->base);
+	val_buf->shared = false;
 	list_add_tail(&val_buf->head, &val_list);
-	ret = ttm_eu_reserve_buffers(NULL, &val_list);
+	ret = ttm_eu_reserve_buffers(NULL, &val_list, interruptible);
 	if (unlikely(ret != 0))
 		goto out_no_reserve;
 
@@ -1307,6 +1314,7 @@ int vmw_resource_do_evict(struct vmw_resource *res, bool interruptible)
 	BUG_ON(!func->may_evict);
 
 	val_buf.bo = NULL;
+	val_buf.shared = false;
 	ret = vmw_resource_check_buffer(res, interruptible, &val_buf);
 	if (unlikely(ret != 0))
 		return ret;
@@ -1352,6 +1360,7 @@ int vmw_resource_validate(struct vmw_resource *res)
 		return 0;
 
 	val_buf.bo = NULL;
+	val_buf.shared = false;
 	if (res->backup)
 		val_buf.bo = &res->backup->base;
 	do {
@@ -1419,25 +1428,16 @@ void vmw_fence_single_bo(struct ttm_buffer_object *bo,
 			 struct vmw_fence_obj *fence)
 {
 	struct ttm_bo_device *bdev = bo->bdev;
-	struct ttm_bo_driver *driver = bdev->driver;
-	struct vmw_fence_obj *old_fence_obj;
+
 	struct vmw_private *dev_priv =
 		container_of(bdev, struct vmw_private, bdev);
 
-	if (fence == NULL)
+	if (fence == NULL) {
 		vmw_execbuf_fence_commands(NULL, dev_priv, &fence, NULL);
-	else
-		driver->sync_obj_ref(fence);
-
-	spin_lock(&bdev->fence_lock);
-
-	old_fence_obj = bo->sync_obj;
-	bo->sync_obj = fence;
-
-	spin_unlock(&bdev->fence_lock);
-
-	if (old_fence_obj)
-		vmw_fence_obj_unreference(&old_fence_obj);
+		reservation_object_add_excl_fence(bo->resv, &fence->base);
+		fence_put(&fence->base);
+	} else
+		reservation_object_add_excl_fence(bo->resv, &fence->base);
 }
 
 /**
@@ -1475,10 +1475,10 @@ void vmw_resource_move_notify(struct ttm_buffer_object *bo,
 
 	if (mem->mem_type != VMW_PL_MOB) {
 		struct vmw_resource *res, *n;
-		struct ttm_bo_device *bdev = bo->bdev;
 		struct ttm_validate_buffer val_buf;
 
 		val_buf.bo = bo;
+		val_buf.shared = false;
 
 		list_for_each_entry_safe(res, n, &dma_buf->res_list, mob_head) {
 
@@ -1491,9 +1491,7 @@ void vmw_resource_move_notify(struct ttm_buffer_object *bo,
 			list_del_init(&res->mob_head);
 		}
 
-		spin_lock(&bdev->fence_lock);
 		(void) ttm_bo_wait(bo, false, false, false);
-		spin_unlock(&bdev->fence_lock);
 	}
 }
 
diff --git a/drivers/gpu/ipu-v3/Kconfig b/drivers/gpu/ipu-v3/Kconfig
index 2f228a2f2a48..aefdff95356d 100644
--- a/drivers/gpu/ipu-v3/Kconfig
+++ b/drivers/gpu/ipu-v3/Kconfig
@@ -1,7 +1,8 @@
 config IMX_IPUV3_CORE
 	tristate "IPUv3 core support"
-	depends on SOC_IMX5 || SOC_IMX6Q || SOC_IMX6SL || ARCH_MULTIPLATFORM
+	depends on SOC_IMX5 || SOC_IMX6Q || ARCH_MULTIPLATFORM
 	depends on RESET_CONTROLLER
+	select GENERIC_IRQ_CHIP
 	help
 	  Choose this if you have a i.MX5/6 system and want to use the Image
 	  Processing Unit. This option only enables IPU base support.
diff --git a/drivers/gpu/ipu-v3/Makefile b/drivers/gpu/ipu-v3/Makefile
index 1887972b4ac2..107ec236a4a6 100644
--- a/drivers/gpu/ipu-v3/Makefile
+++ b/drivers/gpu/ipu-v3/Makefile
@@ -1,3 +1,4 @@
 obj-$(CONFIG_IMX_IPUV3_CORE) += imx-ipu-v3.o
 
-imx-ipu-v3-objs := ipu-common.o ipu-dc.o ipu-di.o ipu-dp.o ipu-dmfc.o ipu-smfc.o
+imx-ipu-v3-objs := ipu-common.o ipu-cpmem.o ipu-csi.o ipu-dc.o ipu-di.o \
+		ipu-dp.o ipu-dmfc.o ipu-ic.o ipu-smfc.o
diff --git a/drivers/gpu/ipu-v3/ipu-common.c b/drivers/gpu/ipu-v3/ipu-common.c
index 04e7b2eafbdd..f707d25ae78f 100644
--- a/drivers/gpu/ipu-v3/ipu-common.c
+++ b/drivers/gpu/ipu-v3/ipu-common.c
@@ -44,17 +44,6 @@ static inline void ipu_cm_write(struct ipu_soc *ipu, u32 value, unsigned offset)
 	writel(value, ipu->cm_reg + offset);
 }
 
-static inline u32 ipu_idmac_read(struct ipu_soc *ipu, unsigned offset)
-{
-	return readl(ipu->idmac_reg + offset);
-}
-
-static inline void ipu_idmac_write(struct ipu_soc *ipu, u32 value,
-		unsigned offset)
-{
-	writel(value, ipu->idmac_reg + offset);
-}
-
 void ipu_srm_dp_sync_update(struct ipu_soc *ipu)
 {
 	u32 val;
@@ -65,457 +54,184 @@ void ipu_srm_dp_sync_update(struct ipu_soc *ipu)
 }
 EXPORT_SYMBOL_GPL(ipu_srm_dp_sync_update);
 
-struct ipu_ch_param __iomem *ipu_get_cpmem(struct ipuv3_channel *channel)
-{
-	struct ipu_soc *ipu = channel->ipu;
-
-	return ipu->cpmem_base + channel->num;
-}
-EXPORT_SYMBOL_GPL(ipu_get_cpmem);
-
-void ipu_cpmem_set_high_priority(struct ipuv3_channel *channel)
-{
-	struct ipu_soc *ipu = channel->ipu;
-	struct ipu_ch_param __iomem *p = ipu_get_cpmem(channel);
-	u32 val;
-
-	if (ipu->ipu_type == IPUV3EX)
-		ipu_ch_param_write_field(p, IPU_FIELD_ID, 1);
-
-	val = ipu_idmac_read(ipu, IDMAC_CHA_PRI(channel->num));
-	val |= 1 << (channel->num % 32);
-	ipu_idmac_write(ipu, val, IDMAC_CHA_PRI(channel->num));
-};
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_high_priority);
-
-void ipu_ch_param_write_field(struct ipu_ch_param __iomem *base, u32 wbs, u32 v)
-{
-	u32 bit = (wbs >> 8) % 160;
-	u32 size = wbs & 0xff;
-	u32 word = (wbs >> 8) / 160;
-	u32 i = bit / 32;
-	u32 ofs = bit % 32;
-	u32 mask = (1 << size) - 1;
-	u32 val;
-
-	pr_debug("%s %d %d %d\n", __func__, word, bit , size);
-
-	val = readl(&base->word[word].data[i]);
-	val &= ~(mask << ofs);
-	val |= v << ofs;
-	writel(val, &base->word[word].data[i]);
-
-	if ((bit + size - 1) / 32 > i) {
-		val = readl(&base->word[word].data[i + 1]);
-		val &= ~(mask >> (ofs ? (32 - ofs) : 0));
-		val |= v >> (ofs ? (32 - ofs) : 0);
-		writel(val, &base->word[word].data[i + 1]);
-	}
-}
-EXPORT_SYMBOL_GPL(ipu_ch_param_write_field);
-
-u32 ipu_ch_param_read_field(struct ipu_ch_param __iomem *base, u32 wbs)
-{
-	u32 bit = (wbs >> 8) % 160;
-	u32 size = wbs & 0xff;
-	u32 word = (wbs >> 8) / 160;
-	u32 i = bit / 32;
-	u32 ofs = bit % 32;
-	u32 mask = (1 << size) - 1;
-	u32 val = 0;
-
-	pr_debug("%s %d %d %d\n", __func__, word, bit , size);
-
-	val = (readl(&base->word[word].data[i]) >> ofs) & mask;
-
-	if ((bit + size - 1) / 32 > i) {
-		u32 tmp;
-		tmp = readl(&base->word[word].data[i + 1]);
-		tmp &= mask >> (ofs ? (32 - ofs) : 0);
-		val |= tmp << (ofs ? (32 - ofs) : 0);
-	}
-
-	return val;
-}
-EXPORT_SYMBOL_GPL(ipu_ch_param_read_field);
-
-int ipu_cpmem_set_format_rgb(struct ipu_ch_param __iomem *p,
-		const struct ipu_rgb *rgb)
-{
-	int bpp = 0, npb = 0, ro, go, bo, to;
-
-	ro = rgb->bits_per_pixel - rgb->red.length - rgb->red.offset;
-	go = rgb->bits_per_pixel - rgb->green.length - rgb->green.offset;
-	bo = rgb->bits_per_pixel - rgb->blue.length - rgb->blue.offset;
-	to = rgb->bits_per_pixel - rgb->transp.length - rgb->transp.offset;
-
-	ipu_ch_param_write_field(p, IPU_FIELD_WID0, rgb->red.length - 1);
-	ipu_ch_param_write_field(p, IPU_FIELD_OFS0, ro);
-	ipu_ch_param_write_field(p, IPU_FIELD_WID1, rgb->green.length - 1);
-	ipu_ch_param_write_field(p, IPU_FIELD_OFS1, go);
-	ipu_ch_param_write_field(p, IPU_FIELD_WID2, rgb->blue.length - 1);
-	ipu_ch_param_write_field(p, IPU_FIELD_OFS2, bo);
-
-	if (rgb->transp.length) {
-		ipu_ch_param_write_field(p, IPU_FIELD_WID3,
-				rgb->transp.length - 1);
-		ipu_ch_param_write_field(p, IPU_FIELD_OFS3, to);
-	} else {
-		ipu_ch_param_write_field(p, IPU_FIELD_WID3, 7);
-		ipu_ch_param_write_field(p, IPU_FIELD_OFS3,
-				rgb->bits_per_pixel);
-	}
-
-	switch (rgb->bits_per_pixel) {
-	case 32:
-		bpp = 0;
-		npb = 15;
-		break;
-	case 24:
-		bpp = 1;
-		npb = 19;
-		break;
-	case 16:
-		bpp = 3;
-		npb = 31;
-		break;
-	case 8:
-		bpp = 5;
-		npb = 63;
-		break;
-	default:
-		return -EINVAL;
-	}
-	ipu_ch_param_write_field(p, IPU_FIELD_BPP, bpp);
-	ipu_ch_param_write_field(p, IPU_FIELD_NPB, npb);
-	ipu_ch_param_write_field(p, IPU_FIELD_PFS, 7); /* rgb mode */
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_format_rgb);
-
-int ipu_cpmem_set_format_passthrough(struct ipu_ch_param __iomem *p,
-		int width)
+enum ipu_color_space ipu_drm_fourcc_to_colorspace(u32 drm_fourcc)
 {
-	int bpp = 0, npb = 0;
-
-	switch (width) {
-	case 32:
-		bpp = 0;
-		npb = 15;
-		break;
-	case 24:
-		bpp = 1;
-		npb = 19;
-		break;
-	case 16:
-		bpp = 3;
-		npb = 31;
-		break;
-	case 8:
-		bpp = 5;
-		npb = 63;
-		break;
+	switch (drm_fourcc) {
+	case DRM_FORMAT_RGB565:
+	case DRM_FORMAT_BGR565:
+	case DRM_FORMAT_RGB888:
+	case DRM_FORMAT_BGR888:
+	case DRM_FORMAT_XRGB8888:
+	case DRM_FORMAT_XBGR8888:
+	case DRM_FORMAT_RGBX8888:
+	case DRM_FORMAT_BGRX8888:
+	case DRM_FORMAT_ARGB8888:
+	case DRM_FORMAT_ABGR8888:
+	case DRM_FORMAT_RGBA8888:
+	case DRM_FORMAT_BGRA8888:
+		return IPUV3_COLORSPACE_RGB;
+	case DRM_FORMAT_YUYV:
+	case DRM_FORMAT_UYVY:
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+	case DRM_FORMAT_NV12:
+	case DRM_FORMAT_NV21:
+	case DRM_FORMAT_NV16:
+	case DRM_FORMAT_NV61:
+		return IPUV3_COLORSPACE_YUV;
 	default:
-		return -EINVAL;
+		return IPUV3_COLORSPACE_UNKNOWN;
 	}
-
-	ipu_ch_param_write_field(p, IPU_FIELD_BPP, bpp);
-	ipu_ch_param_write_field(p, IPU_FIELD_NPB, npb);
-	ipu_ch_param_write_field(p, IPU_FIELD_PFS, 6); /* raw mode */
-
-	return 0;
 }
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_format_passthrough);
+EXPORT_SYMBOL_GPL(ipu_drm_fourcc_to_colorspace);
 
-void ipu_cpmem_set_yuv_interleaved(struct ipu_ch_param __iomem *p,
-				   u32 pixel_format)
+enum ipu_color_space ipu_pixelformat_to_colorspace(u32 pixelformat)
 {
-	switch (pixel_format) {
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_YUV422P:
 	case V4L2_PIX_FMT_UYVY:
-		ipu_ch_param_write_field(p, IPU_FIELD_BPP, 3);    /* bits/pixel */
-		ipu_ch_param_write_field(p, IPU_FIELD_PFS, 0xA);  /* pix format */
-		ipu_ch_param_write_field(p, IPU_FIELD_NPB, 31);   /* burst size */
-		break;
 	case V4L2_PIX_FMT_YUYV:
-		ipu_ch_param_write_field(p, IPU_FIELD_BPP, 3);    /* bits/pixel */
-		ipu_ch_param_write_field(p, IPU_FIELD_PFS, 0x8);  /* pix format */
-		ipu_ch_param_write_field(p, IPU_FIELD_NPB, 31);   /* burst size */
-		break;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		return IPUV3_COLORSPACE_YUV;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB565:
+		return IPUV3_COLORSPACE_RGB;
+	default:
+		return IPUV3_COLORSPACE_UNKNOWN;
 	}
 }
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_interleaved);
+EXPORT_SYMBOL_GPL(ipu_pixelformat_to_colorspace);
 
-void ipu_cpmem_set_yuv_planar_full(struct ipu_ch_param __iomem *p,
-		u32 pixel_format, int stride, int u_offset, int v_offset)
+bool ipu_pixelformat_is_planar(u32 pixelformat)
 {
-	switch (pixel_format) {
+	switch (pixelformat) {
 	case V4L2_PIX_FMT_YUV420:
-		ipu_ch_param_write_field(p, IPU_FIELD_SLUV, (stride / 2) - 1);
-		ipu_ch_param_write_field(p, IPU_FIELD_UBO, u_offset / 8);
-		ipu_ch_param_write_field(p, IPU_FIELD_VBO, v_offset / 8);
-		break;
 	case V4L2_PIX_FMT_YVU420:
-		ipu_ch_param_write_field(p, IPU_FIELD_SLUV, (stride / 2) - 1);
-		ipu_ch_param_write_field(p, IPU_FIELD_UBO, v_offset / 8);
-		ipu_ch_param_write_field(p, IPU_FIELD_VBO, u_offset / 8);
-		break;
+	case V4L2_PIX_FMT_YUV422P:
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
+		return true;
 	}
-}
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_planar_full);
-
-void ipu_cpmem_set_yuv_planar(struct ipu_ch_param __iomem *p, u32 pixel_format,
-		int stride, int height)
-{
-	int u_offset, v_offset;
-	int uv_stride = 0;
 
-	switch (pixel_format) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-		uv_stride = stride / 2;
-		u_offset = stride * height;
-		v_offset = u_offset + (uv_stride * height / 2);
-		ipu_cpmem_set_yuv_planar_full(p, pixel_format, stride,
-				u_offset, v_offset);
-		break;
-	}
+	return false;
 }
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_planar);
-
-static const struct ipu_rgb def_rgb_32 = {
-	.red	= { .offset = 16, .length = 8, },
-	.green	= { .offset =  8, .length = 8, },
-	.blue	= { .offset =  0, .length = 8, },
-	.transp = { .offset = 24, .length = 8, },
-	.bits_per_pixel = 32,
-};
-
-static const struct ipu_rgb def_bgr_32 = {
-	.red	= { .offset =  0, .length = 8, },
-	.green	= { .offset =  8, .length = 8, },
-	.blue	= { .offset = 16, .length = 8, },
-	.transp = { .offset = 24, .length = 8, },
-	.bits_per_pixel = 32,
-};
-
-static const struct ipu_rgb def_rgb_24 = {
-	.red	= { .offset = 16, .length = 8, },
-	.green	= { .offset =  8, .length = 8, },
-	.blue	= { .offset =  0, .length = 8, },
-	.transp = { .offset =  0, .length = 0, },
-	.bits_per_pixel = 24,
-};
-
-static const struct ipu_rgb def_bgr_24 = {
-	.red	= { .offset =  0, .length = 8, },
-	.green	= { .offset =  8, .length = 8, },
-	.blue	= { .offset = 16, .length = 8, },
-	.transp = { .offset =  0, .length = 0, },
-	.bits_per_pixel = 24,
-};
-
-static const struct ipu_rgb def_rgb_16 = {
-	.red	= { .offset = 11, .length = 5, },
-	.green	= { .offset =  5, .length = 6, },
-	.blue	= { .offset =  0, .length = 5, },
-	.transp = { .offset =  0, .length = 0, },
-	.bits_per_pixel = 16,
-};
-
-static const struct ipu_rgb def_bgr_16 = {
-	.red	= { .offset =  0, .length = 5, },
-	.green	= { .offset =  5, .length = 6, },
-	.blue	= { .offset = 11, .length = 5, },
-	.transp = { .offset =  0, .length = 0, },
-	.bits_per_pixel = 16,
-};
-
-#define Y_OFFSET(pix, x, y)	((x) + pix->width * (y))
-#define U_OFFSET(pix, x, y)	((pix->width * pix->height) + \
-					(pix->width * (y) / 4) + (x) / 2)
-#define V_OFFSET(pix, x, y)	((pix->width * pix->height) + \
-					(pix->width * pix->height / 4) + \
-					(pix->width * (y) / 4) + (x) / 2)
+EXPORT_SYMBOL_GPL(ipu_pixelformat_is_planar);
 
-int ipu_cpmem_set_fmt(struct ipu_ch_param __iomem *cpmem, u32 drm_fourcc)
+enum ipu_color_space ipu_mbus_code_to_colorspace(u32 mbus_code)
 {
-	switch (drm_fourcc) {
-	case DRM_FORMAT_YUV420:
-	case DRM_FORMAT_YVU420:
-		/* pix format */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_PFS, 2);
-		/* burst size */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_NPB, 63);
-		break;
-	case DRM_FORMAT_UYVY:
-		/* bits/pixel */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_BPP, 3);
-		/* pix format */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_PFS, 0xA);
-		/* burst size */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_NPB, 31);
-		break;
-	case DRM_FORMAT_YUYV:
-		/* bits/pixel */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_BPP, 3);
-		/* pix format */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_PFS, 0x8);
-		/* burst size */
-		ipu_ch_param_write_field(cpmem, IPU_FIELD_NPB, 31);
-		break;
-	case DRM_FORMAT_ABGR8888:
-	case DRM_FORMAT_XBGR8888:
-		ipu_cpmem_set_format_rgb(cpmem, &def_bgr_32);
-		break;
-	case DRM_FORMAT_ARGB8888:
-	case DRM_FORMAT_XRGB8888:
-		ipu_cpmem_set_format_rgb(cpmem, &def_rgb_32);
-		break;
-	case DRM_FORMAT_BGR888:
-		ipu_cpmem_set_format_rgb(cpmem, &def_bgr_24);
-		break;
-	case DRM_FORMAT_RGB888:
-		ipu_cpmem_set_format_rgb(cpmem, &def_rgb_24);
-		break;
-	case DRM_FORMAT_RGB565:
-		ipu_cpmem_set_format_rgb(cpmem, &def_rgb_16);
-		break;
-	case DRM_FORMAT_BGR565:
-		ipu_cpmem_set_format_rgb(cpmem, &def_bgr_16);
-		break;
+	switch (mbus_code & 0xf000) {
+	case 0x1000:
+		return IPUV3_COLORSPACE_RGB;
+	case 0x2000:
+		return IPUV3_COLORSPACE_YUV;
 	default:
-		return -EINVAL;
+		return IPUV3_COLORSPACE_UNKNOWN;
 	}
-
-	return 0;
 }
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_fmt);
+EXPORT_SYMBOL_GPL(ipu_mbus_code_to_colorspace);
 
-/*
- * The V4L2 spec defines packed RGB formats in memory byte order, which from
- * point of view of the IPU corresponds to little-endian words with the first
- * component in the least significant bits.
- * The DRM pixel formats and IPU internal representation are ordered the other
- * way around, with the first named component ordered at the most significant
- * bits. Further, V4L2 formats are not well defined:
- *     http://linuxtv.org/downloads/v4l-dvb-apis/packed-rgb.html
- * We choose the interpretation which matches GStreamer behavior.
- */
-static int v4l2_pix_fmt_to_drm_fourcc(u32 pixelformat)
+int ipu_stride_to_bytes(u32 pixel_stride, u32 pixelformat)
 {
 	switch (pixelformat) {
-	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_YUV422P:
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV21:
+	case V4L2_PIX_FMT_NV16:
+	case V4L2_PIX_FMT_NV61:
 		/*
-		 * Here we choose the 'corrected' interpretation of RGBP, a
-		 * little-endian 16-bit word with the red component at the most
-		 * significant bits:
-		 * g[2:0]b[4:0] r[4:0]g[5:3] <=> [16:0] R:G:B
+		 * for the planar YUV formats, the stride passed to
+		 * cpmem must be the stride in bytes of the Y plane.
+		 * And all the planar YUV formats have an 8-bit
+		 * Y component.
 		 */
-		return DRM_FORMAT_RGB565;
+		return (8 * pixel_stride) >> 3;
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+		return (16 * pixel_stride) >> 3;
 	case V4L2_PIX_FMT_BGR24:
-		/* B G R <=> [24:0] R:G:B */
-		return DRM_FORMAT_RGB888;
 	case V4L2_PIX_FMT_RGB24:
-		/* R G B <=> [24:0] B:G:R */
-		return DRM_FORMAT_BGR888;
+		return (24 * pixel_stride) >> 3;
 	case V4L2_PIX_FMT_BGR32:
-		/* B G R A <=> [32:0] A:B:G:R */
-		return DRM_FORMAT_XRGB8888;
 	case V4L2_PIX_FMT_RGB32:
-		/* R G B A <=> [32:0] A:B:G:R */
-		return DRM_FORMAT_XBGR8888;
-	case V4L2_PIX_FMT_UYVY:
-		return DRM_FORMAT_UYVY;
-	case V4L2_PIX_FMT_YUYV:
-		return DRM_FORMAT_YUYV;
-	case V4L2_PIX_FMT_YUV420:
-		return DRM_FORMAT_YUV420;
-	case V4L2_PIX_FMT_YVU420:
-		return DRM_FORMAT_YVU420;
+		return (32 * pixel_stride) >> 3;
+	default:
+		break;
 	}
 
 	return -EINVAL;
 }
+EXPORT_SYMBOL_GPL(ipu_stride_to_bytes);
 
-enum ipu_color_space ipu_drm_fourcc_to_colorspace(u32 drm_fourcc)
+int ipu_degrees_to_rot_mode(enum ipu_rotate_mode *mode, int degrees,
+			    bool hflip, bool vflip)
 {
-	switch (drm_fourcc) {
-	case DRM_FORMAT_RGB565:
-	case DRM_FORMAT_BGR565:
-	case DRM_FORMAT_RGB888:
-	case DRM_FORMAT_BGR888:
-	case DRM_FORMAT_XRGB8888:
-	case DRM_FORMAT_XBGR8888:
-	case DRM_FORMAT_RGBX8888:
-	case DRM_FORMAT_BGRX8888:
-	case DRM_FORMAT_ARGB8888:
-	case DRM_FORMAT_ABGR8888:
-	case DRM_FORMAT_RGBA8888:
-	case DRM_FORMAT_BGRA8888:
-		return IPUV3_COLORSPACE_RGB;
-	case DRM_FORMAT_YUYV:
-	case DRM_FORMAT_UYVY:
-	case DRM_FORMAT_YUV420:
-	case DRM_FORMAT_YVU420:
-		return IPUV3_COLORSPACE_YUV;
+	u32 r90, vf, hf;
+
+	switch (degrees) {
+	case 0:
+		vf = hf = r90 = 0;
+		break;
+	case 90:
+		vf = hf = 0;
+		r90 = 1;
+		break;
+	case 180:
+		vf = hf = 1;
+		r90 = 0;
+		break;
+	case 270:
+		vf = hf = r90 = 1;
+		break;
 	default:
-		return IPUV3_COLORSPACE_UNKNOWN;
+		return -EINVAL;
 	}
-}
-EXPORT_SYMBOL_GPL(ipu_drm_fourcc_to_colorspace);
 
-int ipu_cpmem_set_image(struct ipu_ch_param __iomem *cpmem,
-		struct ipu_image *image)
-{
-	struct v4l2_pix_format *pix = &image->pix;
-	int y_offset, u_offset, v_offset;
+	hf ^= (u32)hflip;
+	vf ^= (u32)vflip;
 
-	pr_debug("%s: resolution: %dx%d stride: %d\n",
-			__func__, pix->width, pix->height,
-			pix->bytesperline);
+	*mode = (enum ipu_rotate_mode)((r90 << 2) | (hf << 1) | vf);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_degrees_to_rot_mode);
 
-	ipu_cpmem_set_resolution(cpmem, image->rect.width,
-			image->rect.height);
-	ipu_cpmem_set_stride(cpmem, pix->bytesperline);
+int ipu_rot_mode_to_degrees(int *degrees, enum ipu_rotate_mode mode,
+			    bool hflip, bool vflip)
+{
+	u32 r90, vf, hf;
 
-	ipu_cpmem_set_fmt(cpmem, v4l2_pix_fmt_to_drm_fourcc(pix->pixelformat));
+	r90 = ((u32)mode >> 2) & 0x1;
+	hf = ((u32)mode >> 1) & 0x1;
+	vf = ((u32)mode >> 0) & 0x1;
+	hf ^= (u32)hflip;
+	vf ^= (u32)vflip;
 
-	switch (pix->pixelformat) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-		y_offset = Y_OFFSET(pix, image->rect.left, image->rect.top);
-		u_offset = U_OFFSET(pix, image->rect.left,
-				image->rect.top) - y_offset;
-		v_offset = V_OFFSET(pix, image->rect.left,
-				image->rect.top) - y_offset;
-
-		ipu_cpmem_set_yuv_planar_full(cpmem, pix->pixelformat,
-				pix->bytesperline, u_offset, v_offset);
-		ipu_cpmem_set_buffer(cpmem, 0, image->phys + y_offset);
+	switch ((enum ipu_rotate_mode)((r90 << 2) | (hf << 1) | vf)) {
+	case IPU_ROTATE_NONE:
+		*degrees = 0;
 		break;
-	case V4L2_PIX_FMT_UYVY:
-	case V4L2_PIX_FMT_YUYV:
-		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
-				image->rect.left * 2 +
-				image->rect.top * image->pix.bytesperline);
+	case IPU_ROTATE_90_RIGHT:
+		*degrees = 90;
 		break;
-	case V4L2_PIX_FMT_RGB32:
-	case V4L2_PIX_FMT_BGR32:
-		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
-				image->rect.left * 4 +
-				image->rect.top * image->pix.bytesperline);
+	case IPU_ROTATE_180:
+		*degrees = 180;
 		break;
-	case V4L2_PIX_FMT_RGB565:
-		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
-				image->rect.left * 2 +
-				image->rect.top * image->pix.bytesperline);
-		break;
-	case V4L2_PIX_FMT_RGB24:
-	case V4L2_PIX_FMT_BGR24:
-		ipu_cpmem_set_buffer(cpmem, 0, image->phys +
-				image->rect.left * 3 +
-				image->rect.top * image->pix.bytesperline);
+	case IPU_ROTATE_90_LEFT:
+		*degrees = 270;
 		break;
 	default:
 		return -EINVAL;
@@ -523,27 +239,7 @@ int ipu_cpmem_set_image(struct ipu_ch_param __iomem *cpmem,
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(ipu_cpmem_set_image);
-
-enum ipu_color_space ipu_pixelformat_to_colorspace(u32 pixelformat)
-{
-	switch (pixelformat) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-	case V4L2_PIX_FMT_UYVY:
-	case V4L2_PIX_FMT_YUYV:
-		return IPUV3_COLORSPACE_YUV;
-	case V4L2_PIX_FMT_RGB32:
-	case V4L2_PIX_FMT_BGR32:
-	case V4L2_PIX_FMT_RGB24:
-	case V4L2_PIX_FMT_BGR24:
-	case V4L2_PIX_FMT_RGB565:
-		return IPUV3_COLORSPACE_RGB;
-	default:
-		return IPUV3_COLORSPACE_UNKNOWN;
-	}
-}
-EXPORT_SYMBOL_GPL(ipu_pixelformat_to_colorspace);
+EXPORT_SYMBOL_GPL(ipu_rot_mode_to_degrees);
 
 struct ipuv3_channel *ipu_idmac_get(struct ipu_soc *ipu, unsigned num)
 {
@@ -587,7 +283,26 @@ void ipu_idmac_put(struct ipuv3_channel *channel)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_put);
 
-#define idma_mask(ch)			(1 << (ch & 0x1f))
+#define idma_mask(ch)			(1 << ((ch) & 0x1f))
+
+/*
+ * This is an undocumented feature, a write one to a channel bit in
+ * IPU_CHA_CUR_BUF and IPU_CHA_TRIPLE_CUR_BUF will reset the channel's
+ * internal current buffer pointer so that transfers start from buffer
+ * 0 on the next channel enable (that's the theory anyway, the imx6 TRM
+ * only says these are read-only registers). This operation is required
+ * for channel linking to work correctly, for instance video capture
+ * pipelines that carry out image rotations will fail after the first
+ * streaming unless this function is called for each channel before
+ * re-enabling the channels.
+ */
+static void __ipu_idmac_reset_current_buffer(struct ipuv3_channel *channel)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	unsigned int chno = channel->num;
+
+	ipu_cm_write(ipu, idma_mask(chno), IPU_CHA_CUR_BUF(chno));
+}
 
 void ipu_idmac_set_double_buffer(struct ipuv3_channel *channel,
 		bool doublebuffer)
@@ -605,10 +320,81 @@ void ipu_idmac_set_double_buffer(struct ipuv3_channel *channel,
 		reg &= ~idma_mask(channel->num);
 	ipu_cm_write(ipu, reg, IPU_CHA_DB_MODE_SEL(channel->num));
 
+	__ipu_idmac_reset_current_buffer(channel);
+
 	spin_unlock_irqrestore(&ipu->lock, flags);
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_set_double_buffer);
 
+static const struct {
+	int chnum;
+	u32 reg;
+	int shift;
+} idmac_lock_en_info[] = {
+	{ .chnum =  5, .reg = IDMAC_CH_LOCK_EN_1, .shift =  0, },
+	{ .chnum = 11, .reg = IDMAC_CH_LOCK_EN_1, .shift =  2, },
+	{ .chnum = 12, .reg = IDMAC_CH_LOCK_EN_1, .shift =  4, },
+	{ .chnum = 14, .reg = IDMAC_CH_LOCK_EN_1, .shift =  6, },
+	{ .chnum = 15, .reg = IDMAC_CH_LOCK_EN_1, .shift =  8, },
+	{ .chnum = 20, .reg = IDMAC_CH_LOCK_EN_1, .shift = 10, },
+	{ .chnum = 21, .reg = IDMAC_CH_LOCK_EN_1, .shift = 12, },
+	{ .chnum = 22, .reg = IDMAC_CH_LOCK_EN_1, .shift = 14, },
+	{ .chnum = 23, .reg = IDMAC_CH_LOCK_EN_1, .shift = 16, },
+	{ .chnum = 27, .reg = IDMAC_CH_LOCK_EN_1, .shift = 18, },
+	{ .chnum = 28, .reg = IDMAC_CH_LOCK_EN_1, .shift = 20, },
+	{ .chnum = 45, .reg = IDMAC_CH_LOCK_EN_2, .shift =  0, },
+	{ .chnum = 46, .reg = IDMAC_CH_LOCK_EN_2, .shift =  2, },
+	{ .chnum = 47, .reg = IDMAC_CH_LOCK_EN_2, .shift =  4, },
+	{ .chnum = 48, .reg = IDMAC_CH_LOCK_EN_2, .shift =  6, },
+	{ .chnum = 49, .reg = IDMAC_CH_LOCK_EN_2, .shift =  8, },
+	{ .chnum = 50, .reg = IDMAC_CH_LOCK_EN_2, .shift = 10, },
+};
+
+int ipu_idmac_lock_enable(struct ipuv3_channel *channel, int num_bursts)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	unsigned long flags;
+	u32 bursts, regval;
+	int i;
+
+	switch (num_bursts) {
+	case 0:
+	case 1:
+		bursts = 0x00; /* locking disabled */
+		break;
+	case 2:
+		bursts = 0x01;
+		break;
+	case 4:
+		bursts = 0x02;
+		break;
+	case 8:
+		bursts = 0x03;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(idmac_lock_en_info); i++) {
+		if (channel->num == idmac_lock_en_info[i].chnum)
+			break;
+	}
+	if (i >= ARRAY_SIZE(idmac_lock_en_info))
+		return -EINVAL;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	regval = ipu_idmac_read(ipu, idmac_lock_en_info[i].reg);
+	regval &= ~(0x03 << idmac_lock_en_info[i].shift);
+	regval |= (bursts << idmac_lock_en_info[i].shift);
+	ipu_idmac_write(ipu, regval, idmac_lock_en_info[i].reg);
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_lock_enable);
+
 int ipu_module_enable(struct ipu_soc *ipu, u32 mask)
 {
 	unsigned long lock_flags;
@@ -661,30 +447,6 @@ int ipu_module_disable(struct ipu_soc *ipu, u32 mask)
 }
 EXPORT_SYMBOL_GPL(ipu_module_disable);
 
-int ipu_csi_enable(struct ipu_soc *ipu, int csi)
-{
-	return ipu_module_enable(ipu, csi ? IPU_CONF_CSI1_EN : IPU_CONF_CSI0_EN);
-}
-EXPORT_SYMBOL_GPL(ipu_csi_enable);
-
-int ipu_csi_disable(struct ipu_soc *ipu, int csi)
-{
-	return ipu_module_disable(ipu, csi ? IPU_CONF_CSI1_EN : IPU_CONF_CSI0_EN);
-}
-EXPORT_SYMBOL_GPL(ipu_csi_disable);
-
-int ipu_smfc_enable(struct ipu_soc *ipu)
-{
-	return ipu_module_enable(ipu, IPU_CONF_SMFC_EN);
-}
-EXPORT_SYMBOL_GPL(ipu_smfc_enable);
-
-int ipu_smfc_disable(struct ipu_soc *ipu)
-{
-	return ipu_module_disable(ipu, IPU_CONF_SMFC_EN);
-}
-EXPORT_SYMBOL_GPL(ipu_smfc_disable);
-
 int ipu_idmac_get_current_buffer(struct ipuv3_channel *channel)
 {
 	struct ipu_soc *ipu = channel->ipu;
@@ -694,6 +456,30 @@ int ipu_idmac_get_current_buffer(struct ipuv3_channel *channel)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_get_current_buffer);
 
+bool ipu_idmac_buffer_is_ready(struct ipuv3_channel *channel, u32 buf_num)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	unsigned long flags;
+	u32 reg = 0;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+	switch (buf_num) {
+	case 0:
+		reg = ipu_cm_read(ipu, IPU_CHA_BUF0_RDY(channel->num));
+		break;
+	case 1:
+		reg = ipu_cm_read(ipu, IPU_CHA_BUF1_RDY(channel->num));
+		break;
+	case 2:
+		reg = ipu_cm_read(ipu, IPU_CHA_BUF2_RDY(channel->num));
+		break;
+	}
+	spin_unlock_irqrestore(&ipu->lock, flags);
+
+	return ((reg & idma_mask(channel->num)) != 0);
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_buffer_is_ready);
+
 void ipu_idmac_select_buffer(struct ipuv3_channel *channel, u32 buf_num)
 {
 	struct ipu_soc *ipu = channel->ipu;
@@ -712,6 +498,34 @@ void ipu_idmac_select_buffer(struct ipuv3_channel *channel, u32 buf_num)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_select_buffer);
 
+void ipu_idmac_clear_buffer(struct ipuv3_channel *channel, u32 buf_num)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	unsigned int chno = channel->num;
+	unsigned long flags;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	ipu_cm_write(ipu, 0xF0300000, IPU_GPR); /* write one to clear */
+	switch (buf_num) {
+	case 0:
+		ipu_cm_write(ipu, idma_mask(chno), IPU_CHA_BUF0_RDY(chno));
+		break;
+	case 1:
+		ipu_cm_write(ipu, idma_mask(chno), IPU_CHA_BUF1_RDY(chno));
+		break;
+	case 2:
+		ipu_cm_write(ipu, idma_mask(chno), IPU_CHA_BUF2_RDY(chno));
+		break;
+	default:
+		break;
+	}
+	ipu_cm_write(ipu, 0x0, IPU_GPR); /* write one to set */
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_clear_buffer);
+
 int ipu_idmac_enable_channel(struct ipuv3_channel *channel)
 {
 	struct ipu_soc *ipu = channel->ipu;
@@ -782,6 +596,8 @@ int ipu_idmac_disable_channel(struct ipuv3_channel *channel)
 	val &= ~idma_mask(channel->num);
 	ipu_idmac_write(ipu, val, IDMAC_CHA_EN(channel->num));
 
+	__ipu_idmac_reset_current_buffer(channel);
+
 	/* Set channel buffers NOT to be ready */
 	ipu_cm_write(ipu, 0xf0000000, IPU_GPR); /* write one to clear */
 
@@ -810,6 +626,31 @@ int ipu_idmac_disable_channel(struct ipuv3_channel *channel)
 }
 EXPORT_SYMBOL_GPL(ipu_idmac_disable_channel);
 
+/*
+ * The imx6 rev. D TRM says that enabling the WM feature will increase
+ * a channel's priority. Refer to Table 36-8 Calculated priority value.
+ * The sub-module that is the sink or source for the channel must enable
+ * watermark signal for this to take effect (SMFC_WM for instance).
+ */
+void ipu_idmac_enable_watermark(struct ipuv3_channel *channel, bool enable)
+{
+	struct ipu_soc *ipu = channel->ipu;
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	val = ipu_idmac_read(ipu, IDMAC_WM_EN(channel->num));
+	if (enable)
+		val |= 1 << (channel->num % 32);
+	else
+		val &= ~(1 << (channel->num % 32));
+	ipu_idmac_write(ipu, val, IDMAC_WM_EN(channel->num));
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_idmac_enable_watermark);
+
 static int ipu_memory_reset(struct ipu_soc *ipu)
 {
 	unsigned long timeout;
@@ -826,12 +667,66 @@ static int ipu_memory_reset(struct ipu_soc *ipu)
 	return 0;
 }
 
+/*
+ * Set the source mux for the given CSI. Selects either parallel or
+ * MIPI CSI2 sources.
+ */
+void ipu_set_csi_src_mux(struct ipu_soc *ipu, int csi_id, bool mipi_csi2)
+{
+	unsigned long flags;
+	u32 val, mask;
+
+	mask = (csi_id == 1) ? IPU_CONF_CSI1_DATA_SOURCE :
+		IPU_CONF_CSI0_DATA_SOURCE;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	val = ipu_cm_read(ipu, IPU_CONF);
+	if (mipi_csi2)
+		val |= mask;
+	else
+		val &= ~mask;
+	ipu_cm_write(ipu, val, IPU_CONF);
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_set_csi_src_mux);
+
+/*
+ * Set the source mux for the IC. Selects either CSI[01] or the VDI.
+ */
+void ipu_set_ic_src_mux(struct ipu_soc *ipu, int csi_id, bool vdi)
+{
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&ipu->lock, flags);
+
+	val = ipu_cm_read(ipu, IPU_CONF);
+	if (vdi) {
+		val |= IPU_CONF_IC_INPUT;
+	} else {
+		val &= ~IPU_CONF_IC_INPUT;
+		if (csi_id == 1)
+			val |= IPU_CONF_CSI_SEL;
+		else
+			val &= ~IPU_CONF_CSI_SEL;
+	}
+	ipu_cm_write(ipu, val, IPU_CONF);
+
+	spin_unlock_irqrestore(&ipu->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_set_ic_src_mux);
+
 struct ipu_devtype {
 	const char *name;
 	unsigned long cm_ofs;
 	unsigned long cpmem_ofs;
 	unsigned long srm_ofs;
 	unsigned long tpm_ofs;
+	unsigned long csi0_ofs;
+	unsigned long csi1_ofs;
+	unsigned long ic_ofs;
 	unsigned long disp0_ofs;
 	unsigned long disp1_ofs;
 	unsigned long dc_tmpl_ofs;
@@ -845,6 +740,9 @@ static struct ipu_devtype ipu_type_imx51 = {
 	.cpmem_ofs = 0x1f000000,
 	.srm_ofs = 0x1f040000,
 	.tpm_ofs = 0x1f060000,
+	.csi0_ofs = 0x1f030000,
+	.csi1_ofs = 0x1f038000,
+	.ic_ofs = 0x1f020000,
 	.disp0_ofs = 0x1e040000,
 	.disp1_ofs = 0x1e048000,
 	.dc_tmpl_ofs = 0x1f080000,
@@ -858,6 +756,9 @@ static struct ipu_devtype ipu_type_imx53 = {
 	.cpmem_ofs = 0x07000000,
 	.srm_ofs = 0x07040000,
 	.tpm_ofs = 0x07060000,
+	.csi0_ofs = 0x07030000,
+	.csi1_ofs = 0x07038000,
+	.ic_ofs = 0x07020000,
 	.disp0_ofs = 0x06040000,
 	.disp1_ofs = 0x06048000,
 	.dc_tmpl_ofs = 0x07080000,
@@ -871,6 +772,9 @@ static struct ipu_devtype ipu_type_imx6q = {
 	.cpmem_ofs = 0x00300000,
 	.srm_ofs = 0x00340000,
 	.tpm_ofs = 0x00360000,
+	.csi0_ofs = 0x00230000,
+	.csi1_ofs = 0x00238000,
+	.ic_ofs = 0x00220000,
 	.disp0_ofs = 0x00240000,
 	.disp1_ofs = 0x00248000,
 	.dc_tmpl_ofs = 0x00380000,
@@ -895,8 +799,36 @@ static int ipu_submodules_init(struct ipu_soc *ipu,
 	struct device *dev = &pdev->dev;
 	const struct ipu_devtype *devtype = ipu->devtype;
 
+	ret = ipu_cpmem_init(ipu, dev, ipu_base + devtype->cpmem_ofs);
+	if (ret) {
+		unit = "cpmem";
+		goto err_cpmem;
+	}
+
+	ret = ipu_csi_init(ipu, dev, 0, ipu_base + devtype->csi0_ofs,
+			   IPU_CONF_CSI0_EN, ipu_clk);
+	if (ret) {
+		unit = "csi0";
+		goto err_csi_0;
+	}
+
+	ret = ipu_csi_init(ipu, dev, 1, ipu_base + devtype->csi1_ofs,
+			   IPU_CONF_CSI1_EN, ipu_clk);
+	if (ret) {
+		unit = "csi1";
+		goto err_csi_1;
+	}
+
+	ret = ipu_ic_init(ipu, dev,
+			  ipu_base + devtype->ic_ofs,
+			  ipu_base + devtype->tpm_ofs);
+	if (ret) {
+		unit = "ic";
+		goto err_ic;
+	}
+
 	ret = ipu_di_init(ipu, dev, 0, ipu_base + devtype->disp0_ofs,
-			IPU_CONF_DI0_EN, ipu_clk);
+			  IPU_CONF_DI0_EN, ipu_clk);
 	if (ret) {
 		unit = "di0";
 		goto err_di_0;
@@ -949,6 +881,14 @@ err_dc:
 err_di_1:
 	ipu_di_exit(ipu, 0);
 err_di_0:
+	ipu_ic_exit(ipu);
+err_ic:
+	ipu_csi_exit(ipu, 1);
+err_csi_1:
+	ipu_csi_exit(ipu, 0);
+err_csi_0:
+	ipu_cpmem_exit(ipu);
+err_cpmem:
 	dev_err(&pdev->dev, "init %s failed with %d\n", unit, ret);
 	return ret;
 }
@@ -1025,6 +965,10 @@ static void ipu_submodules_exit(struct ipu_soc *ipu)
 	ipu_dc_exit(ipu);
 	ipu_di_exit(ipu, 1);
 	ipu_di_exit(ipu, 0);
+	ipu_ic_exit(ipu);
+	ipu_csi_exit(ipu, 1);
+	ipu_csi_exit(ipu, 0);
+	ipu_cpmem_exit(ipu);
 }
 
 static int platform_remove_devices_fn(struct device *dev, void *unused)
@@ -1116,8 +1060,10 @@ static int ipu_add_client_devices(struct ipu_soc *ipu, unsigned long ipu_base)
 				id++, &reg->pdata, sizeof(reg->pdata));
 		}
 
-		if (IS_ERR(pdev))
+		if (IS_ERR(pdev)) {
+			ret = PTR_ERR(pdev);
 			goto err_register;
+		}
 	}
 
 	return 0;
@@ -1201,6 +1147,44 @@ static void ipu_irq_exit(struct ipu_soc *ipu)
 	irq_domain_remove(ipu->domain);
 }
 
+void ipu_dump(struct ipu_soc *ipu)
+{
+	int i;
+
+	dev_dbg(ipu->dev, "IPU_CONF = \t0x%08X\n",
+		ipu_cm_read(ipu, IPU_CONF));
+	dev_dbg(ipu->dev, "IDMAC_CONF = \t0x%08X\n",
+		ipu_idmac_read(ipu, IDMAC_CONF));
+	dev_dbg(ipu->dev, "IDMAC_CHA_EN1 = \t0x%08X\n",
+		ipu_idmac_read(ipu, IDMAC_CHA_EN(0)));
+	dev_dbg(ipu->dev, "IDMAC_CHA_EN2 = \t0x%08X\n",
+		ipu_idmac_read(ipu, IDMAC_CHA_EN(32)));
+	dev_dbg(ipu->dev, "IDMAC_CHA_PRI1 = \t0x%08X\n",
+		ipu_idmac_read(ipu, IDMAC_CHA_PRI(0)));
+	dev_dbg(ipu->dev, "IDMAC_CHA_PRI2 = \t0x%08X\n",
+		ipu_idmac_read(ipu, IDMAC_CHA_PRI(32)));
+	dev_dbg(ipu->dev, "IDMAC_BAND_EN1 = \t0x%08X\n",
+		ipu_idmac_read(ipu, IDMAC_BAND_EN(0)));
+	dev_dbg(ipu->dev, "IDMAC_BAND_EN2 = \t0x%08X\n",
+		ipu_idmac_read(ipu, IDMAC_BAND_EN(32)));
+	dev_dbg(ipu->dev, "IPU_CHA_DB_MODE_SEL0 = \t0x%08X\n",
+		ipu_cm_read(ipu, IPU_CHA_DB_MODE_SEL(0)));
+	dev_dbg(ipu->dev, "IPU_CHA_DB_MODE_SEL1 = \t0x%08X\n",
+		ipu_cm_read(ipu, IPU_CHA_DB_MODE_SEL(32)));
+	dev_dbg(ipu->dev, "IPU_FS_PROC_FLOW1 = \t0x%08X\n",
+		ipu_cm_read(ipu, IPU_FS_PROC_FLOW1));
+	dev_dbg(ipu->dev, "IPU_FS_PROC_FLOW2 = \t0x%08X\n",
+		ipu_cm_read(ipu, IPU_FS_PROC_FLOW2));
+	dev_dbg(ipu->dev, "IPU_FS_PROC_FLOW3 = \t0x%08X\n",
+		ipu_cm_read(ipu, IPU_FS_PROC_FLOW3));
+	dev_dbg(ipu->dev, "IPU_FS_DISP_FLOW1 = \t0x%08X\n",
+		ipu_cm_read(ipu, IPU_FS_DISP_FLOW1));
+	for (i = 0; i < 15; i++)
+		dev_dbg(ipu->dev, "IPU_INT_CTRL(%d) = \t%08X\n", i,
+			ipu_cm_read(ipu, IPU_INT_CTRL(i)));
+}
+EXPORT_SYMBOL_GPL(ipu_dump);
+
 static int ipu_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *of_id =
@@ -1243,6 +1227,12 @@ static int ipu_probe(struct platform_device *pdev)
 			ipu_base + devtype->cm_ofs + IPU_CM_IDMAC_REG_OFS);
 	dev_dbg(&pdev->dev, "cpmem:    0x%08lx\n",
 			ipu_base + devtype->cpmem_ofs);
+	dev_dbg(&pdev->dev, "csi0:    0x%08lx\n",
+			ipu_base + devtype->csi0_ofs);
+	dev_dbg(&pdev->dev, "csi1:    0x%08lx\n",
+			ipu_base + devtype->csi1_ofs);
+	dev_dbg(&pdev->dev, "ic:      0x%08lx\n",
+			ipu_base + devtype->ic_ofs);
 	dev_dbg(&pdev->dev, "disp0:    0x%08lx\n",
 			ipu_base + devtype->disp0_ofs);
 	dev_dbg(&pdev->dev, "disp1:    0x%08lx\n",
@@ -1265,10 +1255,8 @@ static int ipu_probe(struct platform_device *pdev)
 	ipu->idmac_reg = devm_ioremap(&pdev->dev,
 			ipu_base + devtype->cm_ofs + IPU_CM_IDMAC_REG_OFS,
 			PAGE_SIZE);
-	ipu->cpmem_base = devm_ioremap(&pdev->dev,
-			ipu_base + devtype->cpmem_ofs, PAGE_SIZE);
 
-	if (!ipu->cm_reg || !ipu->idmac_reg || !ipu->cpmem_base)
+	if (!ipu->cm_reg || !ipu->idmac_reg)
 		return -ENOMEM;
 
 	ipu->clk = devm_clk_get(&pdev->dev, "bus");
diff --git a/drivers/gpu/ipu-v3/ipu-cpmem.c b/drivers/gpu/ipu-v3/ipu-cpmem.c
new file mode 100644
index 000000000000..3bf05bc4ab67
--- /dev/null
+++ b/drivers/gpu/ipu-v3/ipu-cpmem.c
@@ -0,0 +1,764 @@
+/*
+ * Copyright (C) 2012 Mentor Graphics Inc.
+ * Copyright 2005-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/types.h>
+#include <linux/bitrev.h>
+#include <linux/io.h>
+#include <drm/drm_fourcc.h>
+#include "ipu-prv.h"
+
+struct ipu_cpmem_word {
+	u32 data[5];
+	u32 res[3];
+};
+
+struct ipu_ch_param {
+	struct ipu_cpmem_word word[2];
+};
+
+struct ipu_cpmem {
+	struct ipu_ch_param __iomem *base;
+	u32 module;
+	spinlock_t lock;
+	int use_count;
+	struct ipu_soc *ipu;
+};
+
+#define IPU_CPMEM_WORD(word, ofs, size) ((((word) * 160 + (ofs)) << 8) | (size))
+
+#define IPU_FIELD_UBO		IPU_CPMEM_WORD(0, 46, 22)
+#define IPU_FIELD_VBO		IPU_CPMEM_WORD(0, 68, 22)
+#define IPU_FIELD_IOX		IPU_CPMEM_WORD(0, 90, 4)
+#define IPU_FIELD_RDRW		IPU_CPMEM_WORD(0, 94, 1)
+#define IPU_FIELD_SO		IPU_CPMEM_WORD(0, 113, 1)
+#define IPU_FIELD_SLY		IPU_CPMEM_WORD(1, 102, 14)
+#define IPU_FIELD_SLUV		IPU_CPMEM_WORD(1, 128, 14)
+
+#define IPU_FIELD_XV		IPU_CPMEM_WORD(0, 0, 10)
+#define IPU_FIELD_YV		IPU_CPMEM_WORD(0, 10, 9)
+#define IPU_FIELD_XB		IPU_CPMEM_WORD(0, 19, 13)
+#define IPU_FIELD_YB		IPU_CPMEM_WORD(0, 32, 12)
+#define IPU_FIELD_NSB_B		IPU_CPMEM_WORD(0, 44, 1)
+#define IPU_FIELD_CF		IPU_CPMEM_WORD(0, 45, 1)
+#define IPU_FIELD_SX		IPU_CPMEM_WORD(0, 46, 12)
+#define IPU_FIELD_SY		IPU_CPMEM_WORD(0, 58, 11)
+#define IPU_FIELD_NS		IPU_CPMEM_WORD(0, 69, 10)
+#define IPU_FIELD_SDX		IPU_CPMEM_WORD(0, 79, 7)
+#define IPU_FIELD_SM		IPU_CPMEM_WORD(0, 86, 10)
+#define IPU_FIELD_SCC		IPU_CPMEM_WORD(0, 96, 1)
+#define IPU_FIELD_SCE		IPU_CPMEM_WORD(0, 97, 1)
+#define IPU_FIELD_SDY		IPU_CPMEM_WORD(0, 98, 7)
+#define IPU_FIELD_SDRX		IPU_CPMEM_WORD(0, 105, 1)
+#define IPU_FIELD_SDRY		IPU_CPMEM_WORD(0, 106, 1)
+#define IPU_FIELD_BPP		IPU_CPMEM_WORD(0, 107, 3)
+#define IPU_FIELD_DEC_SEL	IPU_CPMEM_WORD(0, 110, 2)
+#define IPU_FIELD_DIM		IPU_CPMEM_WORD(0, 112, 1)
+#define IPU_FIELD_BNDM		IPU_CPMEM_WORD(0, 114, 3)
+#define IPU_FIELD_BM		IPU_CPMEM_WORD(0, 117, 2)
+#define IPU_FIELD_ROT		IPU_CPMEM_WORD(0, 119, 1)
+#define IPU_FIELD_ROT_HF_VF	IPU_CPMEM_WORD(0, 119, 3)
+#define IPU_FIELD_HF		IPU_CPMEM_WORD(0, 120, 1)
+#define IPU_FIELD_VF		IPU_CPMEM_WORD(0, 121, 1)
+#define IPU_FIELD_THE		IPU_CPMEM_WORD(0, 122, 1)
+#define IPU_FIELD_CAP		IPU_CPMEM_WORD(0, 123, 1)
+#define IPU_FIELD_CAE		IPU_CPMEM_WORD(0, 124, 1)
+#define IPU_FIELD_FW		IPU_CPMEM_WORD(0, 125, 13)
+#define IPU_FIELD_FH		IPU_CPMEM_WORD(0, 138, 12)
+#define IPU_FIELD_EBA0		IPU_CPMEM_WORD(1, 0, 29)
+#define IPU_FIELD_EBA1		IPU_CPMEM_WORD(1, 29, 29)
+#define IPU_FIELD_ILO		IPU_CPMEM_WORD(1, 58, 20)
+#define IPU_FIELD_NPB		IPU_CPMEM_WORD(1, 78, 7)
+#define IPU_FIELD_PFS		IPU_CPMEM_WORD(1, 85, 4)
+#define IPU_FIELD_ALU		IPU_CPMEM_WORD(1, 89, 1)
+#define IPU_FIELD_ALBM		IPU_CPMEM_WORD(1, 90, 3)
+#define IPU_FIELD_ID		IPU_CPMEM_WORD(1, 93, 2)
+#define IPU_FIELD_TH		IPU_CPMEM_WORD(1, 95, 7)
+#define IPU_FIELD_SL		IPU_CPMEM_WORD(1, 102, 14)
+#define IPU_FIELD_WID0		IPU_CPMEM_WORD(1, 116, 3)
+#define IPU_FIELD_WID1		IPU_CPMEM_WORD(1, 119, 3)
+#define IPU_FIELD_WID2		IPU_CPMEM_WORD(1, 122, 3)
+#define IPU_FIELD_WID3		IPU_CPMEM_WORD(1, 125, 3)
+#define IPU_FIELD_OFS0		IPU_CPMEM_WORD(1, 128, 5)
+#define IPU_FIELD_OFS1		IPU_CPMEM_WORD(1, 133, 5)
+#define IPU_FIELD_OFS2		IPU_CPMEM_WORD(1, 138, 5)
+#define IPU_FIELD_OFS3		IPU_CPMEM_WORD(1, 143, 5)
+#define IPU_FIELD_SXYS		IPU_CPMEM_WORD(1, 148, 1)
+#define IPU_FIELD_CRE		IPU_CPMEM_WORD(1, 149, 1)
+#define IPU_FIELD_DEC_SEL2	IPU_CPMEM_WORD(1, 150, 1)
+
+static inline struct ipu_ch_param __iomem *
+ipu_get_cpmem(struct ipuv3_channel *ch)
+{
+	struct ipu_cpmem *cpmem = ch->ipu->cpmem_priv;
+
+	return cpmem->base + ch->num;
+}
+
+static void ipu_ch_param_write_field(struct ipuv3_channel *ch, u32 wbs, u32 v)
+{
+	struct ipu_ch_param __iomem *base = ipu_get_cpmem(ch);
+	u32 bit = (wbs >> 8) % 160;
+	u32 size = wbs & 0xff;
+	u32 word = (wbs >> 8) / 160;
+	u32 i = bit / 32;
+	u32 ofs = bit % 32;
+	u32 mask = (1 << size) - 1;
+	u32 val;
+
+	pr_debug("%s %d %d %d\n", __func__, word, bit , size);
+
+	val = readl(&base->word[word].data[i]);
+	val &= ~(mask << ofs);
+	val |= v << ofs;
+	writel(val, &base->word[word].data[i]);
+
+	if ((bit + size - 1) / 32 > i) {
+		val = readl(&base->word[word].data[i + 1]);
+		val &= ~(mask >> (ofs ? (32 - ofs) : 0));
+		val |= v >> (ofs ? (32 - ofs) : 0);
+		writel(val, &base->word[word].data[i + 1]);
+	}
+}
+
+static u32 ipu_ch_param_read_field(struct ipuv3_channel *ch, u32 wbs)
+{
+	struct ipu_ch_param __iomem *base = ipu_get_cpmem(ch);
+	u32 bit = (wbs >> 8) % 160;
+	u32 size = wbs & 0xff;
+	u32 word = (wbs >> 8) / 160;
+	u32 i = bit / 32;
+	u32 ofs = bit % 32;
+	u32 mask = (1 << size) - 1;
+	u32 val = 0;
+
+	pr_debug("%s %d %d %d\n", __func__, word, bit , size);
+
+	val = (readl(&base->word[word].data[i]) >> ofs) & mask;
+
+	if ((bit + size - 1) / 32 > i) {
+		u32 tmp;
+
+		tmp = readl(&base->word[word].data[i + 1]);
+		tmp &= mask >> (ofs ? (32 - ofs) : 0);
+		val |= tmp << (ofs ? (32 - ofs) : 0);
+	}
+
+	return val;
+}
+
+/*
+ * The V4L2 spec defines packed RGB formats in memory byte order, which from
+ * point of view of the IPU corresponds to little-endian words with the first
+ * component in the least significant bits.
+ * The DRM pixel formats and IPU internal representation are ordered the other
+ * way around, with the first named component ordered at the most significant
+ * bits. Further, V4L2 formats are not well defined:
+ *     http://linuxtv.org/downloads/v4l-dvb-apis/packed-rgb.html
+ * We choose the interpretation which matches GStreamer behavior.
+ */
+static int v4l2_pix_fmt_to_drm_fourcc(u32 pixelformat)
+{
+	switch (pixelformat) {
+	case V4L2_PIX_FMT_RGB565:
+		/*
+		 * Here we choose the 'corrected' interpretation of RGBP, a
+		 * little-endian 16-bit word with the red component at the most
+		 * significant bits:
+		 * g[2:0]b[4:0] r[4:0]g[5:3] <=> [16:0] R:G:B
+		 */
+		return DRM_FORMAT_RGB565;
+	case V4L2_PIX_FMT_BGR24:
+		/* B G R <=> [24:0] R:G:B */
+		return DRM_FORMAT_RGB888;
+	case V4L2_PIX_FMT_RGB24:
+		/* R G B <=> [24:0] B:G:R */
+		return DRM_FORMAT_BGR888;
+	case V4L2_PIX_FMT_BGR32:
+		/* B G R A <=> [32:0] A:B:G:R */
+		return DRM_FORMAT_XRGB8888;
+	case V4L2_PIX_FMT_RGB32:
+		/* R G B A <=> [32:0] A:B:G:R */
+		return DRM_FORMAT_XBGR8888;
+	case V4L2_PIX_FMT_UYVY:
+		return DRM_FORMAT_UYVY;
+	case V4L2_PIX_FMT_YUYV:
+		return DRM_FORMAT_YUYV;
+	case V4L2_PIX_FMT_YUV420:
+		return DRM_FORMAT_YUV420;
+	case V4L2_PIX_FMT_YUV422P:
+		return DRM_FORMAT_YUV422;
+	case V4L2_PIX_FMT_YVU420:
+		return DRM_FORMAT_YVU420;
+	case V4L2_PIX_FMT_NV12:
+		return DRM_FORMAT_NV12;
+	case V4L2_PIX_FMT_NV16:
+		return DRM_FORMAT_NV16;
+	}
+
+	return -EINVAL;
+}
+
+void ipu_cpmem_zero(struct ipuv3_channel *ch)
+{
+	struct ipu_ch_param __iomem *p = ipu_get_cpmem(ch);
+	void __iomem *base = p;
+	int i;
+
+	for (i = 0; i < sizeof(*p) / sizeof(u32); i++)
+		writel(0, base + i * sizeof(u32));
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_zero);
+
+void ipu_cpmem_set_resolution(struct ipuv3_channel *ch, int xres, int yres)
+{
+	ipu_ch_param_write_field(ch, IPU_FIELD_FW, xres - 1);
+	ipu_ch_param_write_field(ch, IPU_FIELD_FH, yres - 1);
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_resolution);
+
+void ipu_cpmem_set_stride(struct ipuv3_channel *ch, int stride)
+{
+	ipu_ch_param_write_field(ch, IPU_FIELD_SLY, stride - 1);
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_stride);
+
+void ipu_cpmem_set_high_priority(struct ipuv3_channel *ch)
+{
+	struct ipu_soc *ipu = ch->ipu;
+	u32 val;
+
+	if (ipu->ipu_type == IPUV3EX)
+		ipu_ch_param_write_field(ch, IPU_FIELD_ID, 1);
+
+	val = ipu_idmac_read(ipu, IDMAC_CHA_PRI(ch->num));
+	val |= 1 << (ch->num % 32);
+	ipu_idmac_write(ipu, val, IDMAC_CHA_PRI(ch->num));
+};
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_high_priority);
+
+void ipu_cpmem_set_buffer(struct ipuv3_channel *ch, int bufnum, dma_addr_t buf)
+{
+	if (bufnum)
+		ipu_ch_param_write_field(ch, IPU_FIELD_EBA1, buf >> 3);
+	else
+		ipu_ch_param_write_field(ch, IPU_FIELD_EBA0, buf >> 3);
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_buffer);
+
+void ipu_cpmem_interlaced_scan(struct ipuv3_channel *ch, int stride)
+{
+	ipu_ch_param_write_field(ch, IPU_FIELD_SO, 1);
+	ipu_ch_param_write_field(ch, IPU_FIELD_ILO, stride / 8);
+	ipu_ch_param_write_field(ch, IPU_FIELD_SLY, (stride * 2) - 1);
+};
+EXPORT_SYMBOL_GPL(ipu_cpmem_interlaced_scan);
+
+void ipu_cpmem_set_axi_id(struct ipuv3_channel *ch, u32 id)
+{
+	id &= 0x3;
+	ipu_ch_param_write_field(ch, IPU_FIELD_ID, id);
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_axi_id);
+
+void ipu_cpmem_set_burstsize(struct ipuv3_channel *ch, int burstsize)
+{
+	ipu_ch_param_write_field(ch, IPU_FIELD_NPB, burstsize - 1);
+};
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_burstsize);
+
+void ipu_cpmem_set_block_mode(struct ipuv3_channel *ch)
+{
+	ipu_ch_param_write_field(ch, IPU_FIELD_BM, 1);
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_block_mode);
+
+void ipu_cpmem_set_rotation(struct ipuv3_channel *ch,
+			    enum ipu_rotate_mode rot)
+{
+	u32 temp_rot = bitrev8(rot) >> 5;
+
+	ipu_ch_param_write_field(ch, IPU_FIELD_ROT_HF_VF, temp_rot);
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_rotation);
+
+int ipu_cpmem_set_format_rgb(struct ipuv3_channel *ch,
+			     const struct ipu_rgb *rgb)
+{
+	int bpp = 0, npb = 0, ro, go, bo, to;
+
+	ro = rgb->bits_per_pixel - rgb->red.length - rgb->red.offset;
+	go = rgb->bits_per_pixel - rgb->green.length - rgb->green.offset;
+	bo = rgb->bits_per_pixel - rgb->blue.length - rgb->blue.offset;
+	to = rgb->bits_per_pixel - rgb->transp.length - rgb->transp.offset;
+
+	ipu_ch_param_write_field(ch, IPU_FIELD_WID0, rgb->red.length - 1);
+	ipu_ch_param_write_field(ch, IPU_FIELD_OFS0, ro);
+	ipu_ch_param_write_field(ch, IPU_FIELD_WID1, rgb->green.length - 1);
+	ipu_ch_param_write_field(ch, IPU_FIELD_OFS1, go);
+	ipu_ch_param_write_field(ch, IPU_FIELD_WID2, rgb->blue.length - 1);
+	ipu_ch_param_write_field(ch, IPU_FIELD_OFS2, bo);
+
+	if (rgb->transp.length) {
+		ipu_ch_param_write_field(ch, IPU_FIELD_WID3,
+				rgb->transp.length - 1);
+		ipu_ch_param_write_field(ch, IPU_FIELD_OFS3, to);
+	} else {
+		ipu_ch_param_write_field(ch, IPU_FIELD_WID3, 7);
+		ipu_ch_param_write_field(ch, IPU_FIELD_OFS3,
+				rgb->bits_per_pixel);
+	}
+
+	switch (rgb->bits_per_pixel) {
+	case 32:
+		bpp = 0;
+		npb = 15;
+		break;
+	case 24:
+		bpp = 1;
+		npb = 19;
+		break;
+	case 16:
+		bpp = 3;
+		npb = 31;
+		break;
+	case 8:
+		bpp = 5;
+		npb = 63;
+		break;
+	default:
+		return -EINVAL;
+	}
+	ipu_ch_param_write_field(ch, IPU_FIELD_BPP, bpp);
+	ipu_ch_param_write_field(ch, IPU_FIELD_NPB, npb);
+	ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 7); /* rgb mode */
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_format_rgb);
+
+int ipu_cpmem_set_format_passthrough(struct ipuv3_channel *ch, int width)
+{
+	int bpp = 0, npb = 0;
+
+	switch (width) {
+	case 32:
+		bpp = 0;
+		npb = 15;
+		break;
+	case 24:
+		bpp = 1;
+		npb = 19;
+		break;
+	case 16:
+		bpp = 3;
+		npb = 31;
+		break;
+	case 8:
+		bpp = 5;
+		npb = 63;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ipu_ch_param_write_field(ch, IPU_FIELD_BPP, bpp);
+	ipu_ch_param_write_field(ch, IPU_FIELD_NPB, npb);
+	ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 6); /* raw mode */
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_format_passthrough);
+
+void ipu_cpmem_set_yuv_interleaved(struct ipuv3_channel *ch, u32 pixel_format)
+{
+	switch (pixel_format) {
+	case V4L2_PIX_FMT_UYVY:
+		ipu_ch_param_write_field(ch, IPU_FIELD_BPP, 3); /* bits/pixel */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 0xA);/* pix fmt */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);/* burst size */
+		break;
+	case V4L2_PIX_FMT_YUYV:
+		ipu_ch_param_write_field(ch, IPU_FIELD_BPP, 3); /* bits/pixel */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 0x8);/* pix fmt */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);/* burst size */
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_interleaved);
+
+void ipu_cpmem_set_yuv_planar_full(struct ipuv3_channel *ch,
+				   u32 pixel_format, int stride,
+				   int u_offset, int v_offset)
+{
+	switch (pixel_format) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YUV422P:
+		ipu_ch_param_write_field(ch, IPU_FIELD_SLUV, (stride / 2) - 1);
+		ipu_ch_param_write_field(ch, IPU_FIELD_UBO, u_offset / 8);
+		ipu_ch_param_write_field(ch, IPU_FIELD_VBO, v_offset / 8);
+		break;
+	case V4L2_PIX_FMT_YVU420:
+		ipu_ch_param_write_field(ch, IPU_FIELD_SLUV, (stride / 2) - 1);
+		ipu_ch_param_write_field(ch, IPU_FIELD_UBO, v_offset / 8);
+		ipu_ch_param_write_field(ch, IPU_FIELD_VBO, u_offset / 8);
+		break;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV16:
+		ipu_ch_param_write_field(ch, IPU_FIELD_SLUV, stride - 1);
+		ipu_ch_param_write_field(ch, IPU_FIELD_UBO, u_offset / 8);
+		ipu_ch_param_write_field(ch, IPU_FIELD_VBO, u_offset / 8);
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_planar_full);
+
+void ipu_cpmem_set_yuv_planar(struct ipuv3_channel *ch,
+			      u32 pixel_format, int stride, int height)
+{
+	int u_offset, v_offset;
+	int uv_stride = 0;
+
+	switch (pixel_format) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		uv_stride = stride / 2;
+		u_offset = stride * height;
+		v_offset = u_offset + (uv_stride * height / 2);
+		ipu_cpmem_set_yuv_planar_full(ch, pixel_format, stride,
+					      u_offset, v_offset);
+		break;
+	case V4L2_PIX_FMT_YUV422P:
+		uv_stride = stride / 2;
+		u_offset = stride * height;
+		v_offset = u_offset + (uv_stride * height);
+		ipu_cpmem_set_yuv_planar_full(ch, pixel_format, stride,
+					      u_offset, v_offset);
+		break;
+	case V4L2_PIX_FMT_NV12:
+	case V4L2_PIX_FMT_NV16:
+		u_offset = stride * height;
+		ipu_cpmem_set_yuv_planar_full(ch, pixel_format, stride,
+					      u_offset, 0);
+		break;
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_yuv_planar);
+
+static const struct ipu_rgb def_rgb_32 = {
+	.red	= { .offset = 16, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset =  0, .length = 8, },
+	.transp = { .offset = 24, .length = 8, },
+	.bits_per_pixel = 32,
+};
+
+static const struct ipu_rgb def_bgr_32 = {
+	.red	= { .offset =  0, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset = 16, .length = 8, },
+	.transp = { .offset = 24, .length = 8, },
+	.bits_per_pixel = 32,
+};
+
+static const struct ipu_rgb def_rgb_24 = {
+	.red	= { .offset = 16, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset =  0, .length = 8, },
+	.transp = { .offset =  0, .length = 0, },
+	.bits_per_pixel = 24,
+};
+
+static const struct ipu_rgb def_bgr_24 = {
+	.red	= { .offset =  0, .length = 8, },
+	.green	= { .offset =  8, .length = 8, },
+	.blue	= { .offset = 16, .length = 8, },
+	.transp = { .offset =  0, .length = 0, },
+	.bits_per_pixel = 24,
+};
+
+static const struct ipu_rgb def_rgb_16 = {
+	.red	= { .offset = 11, .length = 5, },
+	.green	= { .offset =  5, .length = 6, },
+	.blue	= { .offset =  0, .length = 5, },
+	.transp = { .offset =  0, .length = 0, },
+	.bits_per_pixel = 16,
+};
+
+static const struct ipu_rgb def_bgr_16 = {
+	.red	= { .offset =  0, .length = 5, },
+	.green	= { .offset =  5, .length = 6, },
+	.blue	= { .offset = 11, .length = 5, },
+	.transp = { .offset =  0, .length = 0, },
+	.bits_per_pixel = 16,
+};
+
+#define Y_OFFSET(pix, x, y)	((x) + pix->width * (y))
+#define U_OFFSET(pix, x, y)	((pix->width * pix->height) +		\
+				 (pix->width * (y) / 4) + (x) / 2)
+#define V_OFFSET(pix, x, y)	((pix->width * pix->height) +		\
+				 (pix->width * pix->height / 4) +	\
+				 (pix->width * (y) / 4) + (x) / 2)
+#define U2_OFFSET(pix, x, y)	((pix->width * pix->height) +		\
+				 (pix->width * (y) / 2) + (x) / 2)
+#define V2_OFFSET(pix, x, y)	((pix->width * pix->height) +		\
+				 (pix->width * pix->height / 2) +	\
+				 (pix->width * (y) / 2) + (x) / 2)
+#define UV_OFFSET(pix, x, y)	((pix->width * pix->height) +	\
+				 (pix->width * (y) / 2) + (x))
+#define UV2_OFFSET(pix, x, y)	((pix->width * pix->height) +	\
+				 (pix->width * y) + (x))
+
+int ipu_cpmem_set_fmt(struct ipuv3_channel *ch, u32 drm_fourcc)
+{
+	switch (drm_fourcc) {
+	case DRM_FORMAT_YUV420:
+	case DRM_FORMAT_YVU420:
+		/* pix format */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 2);
+		/* burst size */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);
+		break;
+	case DRM_FORMAT_YUV422:
+	case DRM_FORMAT_YVU422:
+		/* pix format */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 1);
+		/* burst size */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);
+		break;
+	case DRM_FORMAT_NV12:
+		/* pix format */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 4);
+		/* burst size */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);
+		break;
+	case DRM_FORMAT_NV16:
+		/* pix format */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 3);
+		/* burst size */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);
+		break;
+	case DRM_FORMAT_UYVY:
+		/* bits/pixel */
+		ipu_ch_param_write_field(ch, IPU_FIELD_BPP, 3);
+		/* pix format */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 0xA);
+		/* burst size */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);
+		break;
+	case DRM_FORMAT_YUYV:
+		/* bits/pixel */
+		ipu_ch_param_write_field(ch, IPU_FIELD_BPP, 3);
+		/* pix format */
+		ipu_ch_param_write_field(ch, IPU_FIELD_PFS, 0x8);
+		/* burst size */
+		ipu_ch_param_write_field(ch, IPU_FIELD_NPB, 31);
+		break;
+	case DRM_FORMAT_ABGR8888:
+	case DRM_FORMAT_XBGR8888:
+		ipu_cpmem_set_format_rgb(ch, &def_bgr_32);
+		break;
+	case DRM_FORMAT_ARGB8888:
+	case DRM_FORMAT_XRGB8888:
+		ipu_cpmem_set_format_rgb(ch, &def_rgb_32);
+		break;
+	case DRM_FORMAT_BGR888:
+		ipu_cpmem_set_format_rgb(ch, &def_bgr_24);
+		break;
+	case DRM_FORMAT_RGB888:
+		ipu_cpmem_set_format_rgb(ch, &def_rgb_24);
+		break;
+	case DRM_FORMAT_RGB565:
+		ipu_cpmem_set_format_rgb(ch, &def_rgb_16);
+		break;
+	case DRM_FORMAT_BGR565:
+		ipu_cpmem_set_format_rgb(ch, &def_bgr_16);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_fmt);
+
+int ipu_cpmem_set_image(struct ipuv3_channel *ch, struct ipu_image *image)
+{
+	struct v4l2_pix_format *pix = &image->pix;
+	int offset, u_offset, v_offset;
+
+	pr_debug("%s: resolution: %dx%d stride: %d\n",
+		 __func__, pix->width, pix->height,
+		 pix->bytesperline);
+
+	ipu_cpmem_set_resolution(ch, image->rect.width, image->rect.height);
+	ipu_cpmem_set_stride(ch, pix->bytesperline);
+
+	ipu_cpmem_set_fmt(ch, v4l2_pix_fmt_to_drm_fourcc(pix->pixelformat));
+
+	switch (pix->pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		offset = Y_OFFSET(pix, image->rect.left, image->rect.top);
+		u_offset = U_OFFSET(pix, image->rect.left,
+				    image->rect.top) - offset;
+		v_offset = V_OFFSET(pix, image->rect.left,
+				    image->rect.top) - offset;
+
+		ipu_cpmem_set_yuv_planar_full(ch, pix->pixelformat,
+					      pix->bytesperline,
+					      u_offset, v_offset);
+		break;
+	case V4L2_PIX_FMT_YUV422P:
+		offset = Y_OFFSET(pix, image->rect.left, image->rect.top);
+		u_offset = U2_OFFSET(pix, image->rect.left,
+				     image->rect.top) - offset;
+		v_offset = V2_OFFSET(pix, image->rect.left,
+				     image->rect.top) - offset;
+
+		ipu_cpmem_set_yuv_planar_full(ch, pix->pixelformat,
+					      pix->bytesperline,
+					      u_offset, v_offset);
+		break;
+	case V4L2_PIX_FMT_NV12:
+		offset = Y_OFFSET(pix, image->rect.left, image->rect.top);
+		u_offset = UV_OFFSET(pix, image->rect.left,
+				     image->rect.top) - offset;
+		v_offset = 0;
+
+		ipu_cpmem_set_yuv_planar_full(ch, pix->pixelformat,
+					      pix->bytesperline,
+					      u_offset, v_offset);
+		break;
+	case V4L2_PIX_FMT_NV16:
+		offset = Y_OFFSET(pix, image->rect.left, image->rect.top);
+		u_offset = UV2_OFFSET(pix, image->rect.left,
+				      image->rect.top) - offset;
+		v_offset = 0;
+
+		ipu_cpmem_set_yuv_planar_full(ch, pix->pixelformat,
+					      pix->bytesperline,
+					      u_offset, v_offset);
+		break;
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_RGB565:
+		offset = image->rect.left * 2 +
+			image->rect.top * pix->bytesperline;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+		offset = image->rect.left * 4 +
+			image->rect.top * pix->bytesperline;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+		offset = image->rect.left * 3 +
+			image->rect.top * pix->bytesperline;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	ipu_cpmem_set_buffer(ch, 0, image->phys0 + offset);
+	ipu_cpmem_set_buffer(ch, 1, image->phys1 + offset);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_set_image);
+
+void ipu_cpmem_dump(struct ipuv3_channel *ch)
+{
+	struct ipu_ch_param __iomem *p = ipu_get_cpmem(ch);
+	struct ipu_soc *ipu = ch->ipu;
+	int chno = ch->num;
+
+	dev_dbg(ipu->dev, "ch %d word 0 - %08X %08X %08X %08X %08X\n", chno,
+		readl(&p->word[0].data[0]),
+		readl(&p->word[0].data[1]),
+		readl(&p->word[0].data[2]),
+		readl(&p->word[0].data[3]),
+		readl(&p->word[0].data[4]));
+	dev_dbg(ipu->dev, "ch %d word 1 - %08X %08X %08X %08X %08X\n", chno,
+		readl(&p->word[1].data[0]),
+		readl(&p->word[1].data[1]),
+		readl(&p->word[1].data[2]),
+		readl(&p->word[1].data[3]),
+		readl(&p->word[1].data[4]));
+	dev_dbg(ipu->dev, "PFS 0x%x, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_PFS));
+	dev_dbg(ipu->dev, "BPP 0x%x, ",
+		ipu_ch_param_read_field(ch, IPU_FIELD_BPP));
+	dev_dbg(ipu->dev, "NPB 0x%x\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_NPB));
+
+	dev_dbg(ipu->dev, "FW %d, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_FW));
+	dev_dbg(ipu->dev, "FH %d, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_FH));
+	dev_dbg(ipu->dev, "EBA0 0x%x\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_EBA0) << 3);
+	dev_dbg(ipu->dev, "EBA1 0x%x\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_EBA1) << 3);
+	dev_dbg(ipu->dev, "Stride %d\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_SL));
+	dev_dbg(ipu->dev, "scan_order %d\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_SO));
+	dev_dbg(ipu->dev, "uv_stride %d\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_SLUV));
+	dev_dbg(ipu->dev, "u_offset 0x%x\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_UBO) << 3);
+	dev_dbg(ipu->dev, "v_offset 0x%x\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_VBO) << 3);
+
+	dev_dbg(ipu->dev, "Width0 %d+1, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_WID0));
+	dev_dbg(ipu->dev, "Width1 %d+1, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_WID1));
+	dev_dbg(ipu->dev, "Width2 %d+1, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_WID2));
+	dev_dbg(ipu->dev, "Width3 %d+1, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_WID3));
+	dev_dbg(ipu->dev, "Offset0 %d, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_OFS0));
+	dev_dbg(ipu->dev, "Offset1 %d, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_OFS1));
+	dev_dbg(ipu->dev, "Offset2 %d, ",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_OFS2));
+	dev_dbg(ipu->dev, "Offset3 %d\n",
+		 ipu_ch_param_read_field(ch, IPU_FIELD_OFS3));
+}
+EXPORT_SYMBOL_GPL(ipu_cpmem_dump);
+
+int ipu_cpmem_init(struct ipu_soc *ipu, struct device *dev, unsigned long base)
+{
+	struct ipu_cpmem *cpmem;
+
+	cpmem = devm_kzalloc(dev, sizeof(*cpmem), GFP_KERNEL);
+	if (!cpmem)
+		return -ENOMEM;
+
+	ipu->cpmem_priv = cpmem;
+
+	spin_lock_init(&cpmem->lock);
+	cpmem->base = devm_ioremap(dev, base, SZ_128K);
+	if (!cpmem->base)
+		return -ENOMEM;
+
+	dev_dbg(dev, "CPMEM base: 0x%08lx remapped to %p\n",
+		base, cpmem->base);
+	cpmem->ipu = ipu;
+
+	return 0;
+}
+
+void ipu_cpmem_exit(struct ipu_soc *ipu)
+{
+}
diff --git a/drivers/gpu/ipu-v3/ipu-csi.c b/drivers/gpu/ipu-v3/ipu-csi.c
new file mode 100644
index 000000000000..d6f56471bd2a
--- /dev/null
+++ b/drivers/gpu/ipu-v3/ipu-csi.c
@@ -0,0 +1,741 @@
+/*
+ * Copyright (C) 2012-2014 Mentor Graphics Inc.
+ * Copyright (C) 2005-2009 Freescale Semiconductor, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ */
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/videodev2.h>
+#include <uapi/linux/v4l2-mediabus.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+#include <linux/clkdev.h>
+
+#include "ipu-prv.h"
+
+struct ipu_csi {
+	void __iomem *base;
+	int id;
+	u32 module;
+	struct clk *clk_ipu;	/* IPU bus clock */
+	spinlock_t lock;
+	bool inuse;
+	struct ipu_soc *ipu;
+};
+
+/* CSI Register Offsets */
+#define CSI_SENS_CONF		0x0000
+#define CSI_SENS_FRM_SIZE	0x0004
+#define CSI_ACT_FRM_SIZE	0x0008
+#define CSI_OUT_FRM_CTRL	0x000c
+#define CSI_TST_CTRL		0x0010
+#define CSI_CCIR_CODE_1		0x0014
+#define CSI_CCIR_CODE_2		0x0018
+#define CSI_CCIR_CODE_3		0x001c
+#define CSI_MIPI_DI		0x0020
+#define CSI_SKIP		0x0024
+#define CSI_CPD_CTRL		0x0028
+#define CSI_CPD_RC(n)		(0x002c + ((n)*4))
+#define CSI_CPD_RS(n)		(0x004c + ((n)*4))
+#define CSI_CPD_GRC(n)		(0x005c + ((n)*4))
+#define CSI_CPD_GRS(n)		(0x007c + ((n)*4))
+#define CSI_CPD_GBC(n)		(0x008c + ((n)*4))
+#define CSI_CPD_GBS(n)		(0x00Ac + ((n)*4))
+#define CSI_CPD_BC(n)		(0x00Bc + ((n)*4))
+#define CSI_CPD_BS(n)		(0x00Dc + ((n)*4))
+#define CSI_CPD_OFFSET1		0x00ec
+#define CSI_CPD_OFFSET2		0x00f0
+
+/* CSI Register Fields */
+#define CSI_SENS_CONF_DATA_FMT_SHIFT		8
+#define CSI_SENS_CONF_DATA_FMT_MASK		0x00000700
+#define CSI_SENS_CONF_DATA_FMT_RGB_YUV444	0L
+#define CSI_SENS_CONF_DATA_FMT_YUV422_YUYV	1L
+#define CSI_SENS_CONF_DATA_FMT_YUV422_UYVY	2L
+#define CSI_SENS_CONF_DATA_FMT_BAYER		3L
+#define CSI_SENS_CONF_DATA_FMT_RGB565		4L
+#define CSI_SENS_CONF_DATA_FMT_RGB555		5L
+#define CSI_SENS_CONF_DATA_FMT_RGB444		6L
+#define CSI_SENS_CONF_DATA_FMT_JPEG		7L
+
+#define CSI_SENS_CONF_VSYNC_POL_SHIFT		0
+#define CSI_SENS_CONF_HSYNC_POL_SHIFT		1
+#define CSI_SENS_CONF_DATA_POL_SHIFT		2
+#define CSI_SENS_CONF_PIX_CLK_POL_SHIFT		3
+#define CSI_SENS_CONF_SENS_PRTCL_MASK		0x00000070
+#define CSI_SENS_CONF_SENS_PRTCL_SHIFT		4
+#define CSI_SENS_CONF_PACK_TIGHT_SHIFT		7
+#define CSI_SENS_CONF_DATA_WIDTH_SHIFT		11
+#define CSI_SENS_CONF_EXT_VSYNC_SHIFT		15
+#define CSI_SENS_CONF_DIVRATIO_SHIFT		16
+
+#define CSI_SENS_CONF_DIVRATIO_MASK		0x00ff0000
+#define CSI_SENS_CONF_DATA_DEST_SHIFT		24
+#define CSI_SENS_CONF_DATA_DEST_MASK		0x07000000
+#define CSI_SENS_CONF_JPEG8_EN_SHIFT		27
+#define CSI_SENS_CONF_JPEG_EN_SHIFT		28
+#define CSI_SENS_CONF_FORCE_EOF_SHIFT		29
+#define CSI_SENS_CONF_DATA_EN_POL_SHIFT		31
+
+#define CSI_DATA_DEST_IC			2
+#define CSI_DATA_DEST_IDMAC			4
+
+#define CSI_CCIR_ERR_DET_EN			0x01000000
+#define CSI_HORI_DOWNSIZE_EN			0x80000000
+#define CSI_VERT_DOWNSIZE_EN			0x40000000
+#define CSI_TEST_GEN_MODE_EN			0x01000000
+
+#define CSI_HSC_MASK				0x1fff0000
+#define CSI_HSC_SHIFT				16
+#define CSI_VSC_MASK				0x00000fff
+#define CSI_VSC_SHIFT				0
+
+#define CSI_TEST_GEN_R_MASK			0x000000ff
+#define CSI_TEST_GEN_R_SHIFT			0
+#define CSI_TEST_GEN_G_MASK			0x0000ff00
+#define CSI_TEST_GEN_G_SHIFT			8
+#define CSI_TEST_GEN_B_MASK			0x00ff0000
+#define CSI_TEST_GEN_B_SHIFT			16
+
+#define CSI_MAX_RATIO_SKIP_SMFC_MASK		0x00000007
+#define CSI_MAX_RATIO_SKIP_SMFC_SHIFT		0
+#define CSI_SKIP_SMFC_MASK			0x000000f8
+#define CSI_SKIP_SMFC_SHIFT			3
+#define CSI_ID_2_SKIP_MASK			0x00000300
+#define CSI_ID_2_SKIP_SHIFT			8
+
+#define CSI_COLOR_FIRST_ROW_MASK		0x00000002
+#define CSI_COLOR_FIRST_COMP_MASK		0x00000001
+
+/* MIPI CSI-2 data types */
+#define MIPI_DT_YUV420		0x18 /* YYY.../UYVY.... */
+#define MIPI_DT_YUV420_LEGACY	0x1a /* UYY.../VYY...   */
+#define MIPI_DT_YUV422		0x1e /* UYVY...         */
+#define MIPI_DT_RGB444		0x20
+#define MIPI_DT_RGB555		0x21
+#define MIPI_DT_RGB565		0x22
+#define MIPI_DT_RGB666		0x23
+#define MIPI_DT_RGB888		0x24
+#define MIPI_DT_RAW6		0x28
+#define MIPI_DT_RAW7		0x29
+#define MIPI_DT_RAW8		0x2a
+#define MIPI_DT_RAW10		0x2b
+#define MIPI_DT_RAW12		0x2c
+#define MIPI_DT_RAW14		0x2d
+
+/*
+ * Bitfield of CSI bus signal polarities and modes.
+ */
+struct ipu_csi_bus_config {
+	unsigned data_width:4;
+	unsigned clk_mode:3;
+	unsigned ext_vsync:1;
+	unsigned vsync_pol:1;
+	unsigned hsync_pol:1;
+	unsigned pixclk_pol:1;
+	unsigned data_pol:1;
+	unsigned sens_clksrc:1;
+	unsigned pack_tight:1;
+	unsigned force_eof:1;
+	unsigned data_en_pol:1;
+
+	unsigned data_fmt;
+	unsigned mipi_dt;
+};
+
+/*
+ * Enumeration of CSI data bus widths.
+ */
+enum ipu_csi_data_width {
+	IPU_CSI_DATA_WIDTH_4   = 0,
+	IPU_CSI_DATA_WIDTH_8   = 1,
+	IPU_CSI_DATA_WIDTH_10  = 3,
+	IPU_CSI_DATA_WIDTH_12  = 5,
+	IPU_CSI_DATA_WIDTH_16  = 9,
+};
+
+/*
+ * Enumeration of CSI clock modes.
+ */
+enum ipu_csi_clk_mode {
+	IPU_CSI_CLK_MODE_GATED_CLK,
+	IPU_CSI_CLK_MODE_NONGATED_CLK,
+	IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE,
+	IPU_CSI_CLK_MODE_CCIR656_INTERLACED,
+	IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_DDR,
+	IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_SDR,
+	IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_DDR,
+	IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_SDR,
+};
+
+static inline u32 ipu_csi_read(struct ipu_csi *csi, unsigned offset)
+{
+	return readl(csi->base + offset);
+}
+
+static inline void ipu_csi_write(struct ipu_csi *csi, u32 value,
+				 unsigned offset)
+{
+	writel(value, csi->base + offset);
+}
+
+/*
+ * Set mclk division ratio for generating test mode mclk. Only used
+ * for test generator.
+ */
+static int ipu_csi_set_testgen_mclk(struct ipu_csi *csi, u32 pixel_clk,
+					u32 ipu_clk)
+{
+	u32 temp;
+	u32 div_ratio;
+
+	div_ratio = (ipu_clk / pixel_clk) - 1;
+
+	if (div_ratio > 0xFF || div_ratio < 0) {
+		dev_err(csi->ipu->dev,
+			"value of pixel_clk extends normal range\n");
+		return -EINVAL;
+	}
+
+	temp = ipu_csi_read(csi, CSI_SENS_CONF);
+	temp &= ~CSI_SENS_CONF_DIVRATIO_MASK;
+	ipu_csi_write(csi, temp | (div_ratio << CSI_SENS_CONF_DIVRATIO_SHIFT),
+			  CSI_SENS_CONF);
+
+	return 0;
+}
+
+/*
+ * Find the CSI data format and data width for the given V4L2 media
+ * bus pixel format code.
+ */
+static int mbus_code_to_bus_cfg(struct ipu_csi_bus_config *cfg, u32 mbus_code)
+{
+	switch (mbus_code) {
+	case V4L2_MBUS_FMT_BGR565_2X8_BE:
+	case V4L2_MBUS_FMT_BGR565_2X8_LE:
+	case V4L2_MBUS_FMT_RGB565_2X8_BE:
+	case V4L2_MBUS_FMT_RGB565_2X8_LE:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_RGB565;
+		cfg->mipi_dt = MIPI_DT_RGB565;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	case V4L2_MBUS_FMT_RGB444_2X8_PADHI_BE:
+	case V4L2_MBUS_FMT_RGB444_2X8_PADHI_LE:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_RGB444;
+		cfg->mipi_dt = MIPI_DT_RGB444;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_BE:
+	case V4L2_MBUS_FMT_RGB555_2X8_PADHI_LE:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_RGB555;
+		cfg->mipi_dt = MIPI_DT_RGB555;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	case V4L2_MBUS_FMT_UYVY8_2X8:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_YUV422_UYVY;
+		cfg->mipi_dt = MIPI_DT_YUV422;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	case V4L2_MBUS_FMT_YUYV8_2X8:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_YUV422_YUYV;
+		cfg->mipi_dt = MIPI_DT_YUV422;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	case V4L2_MBUS_FMT_UYVY8_1X16:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_YUV422_UYVY;
+		cfg->mipi_dt = MIPI_DT_YUV422;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_16;
+		break;
+	case V4L2_MBUS_FMT_YUYV8_1X16:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_YUV422_YUYV;
+		cfg->mipi_dt = MIPI_DT_YUV422;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_16;
+		break;
+	case V4L2_MBUS_FMT_SBGGR8_1X8:
+	case V4L2_MBUS_FMT_SGBRG8_1X8:
+	case V4L2_MBUS_FMT_SGRBG8_1X8:
+	case V4L2_MBUS_FMT_SRGGB8_1X8:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_BAYER;
+		cfg->mipi_dt = MIPI_DT_RAW8;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	case V4L2_MBUS_FMT_SBGGR10_DPCM8_1X8:
+	case V4L2_MBUS_FMT_SGBRG10_DPCM8_1X8:
+	case V4L2_MBUS_FMT_SGRBG10_DPCM8_1X8:
+	case V4L2_MBUS_FMT_SRGGB10_DPCM8_1X8:
+	case V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_BE:
+	case V4L2_MBUS_FMT_SBGGR10_2X8_PADHI_LE:
+	case V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_BE:
+	case V4L2_MBUS_FMT_SBGGR10_2X8_PADLO_LE:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_BAYER;
+		cfg->mipi_dt = MIPI_DT_RAW10;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	case V4L2_MBUS_FMT_SBGGR10_1X10:
+	case V4L2_MBUS_FMT_SGBRG10_1X10:
+	case V4L2_MBUS_FMT_SGRBG10_1X10:
+	case V4L2_MBUS_FMT_SRGGB10_1X10:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_BAYER;
+		cfg->mipi_dt = MIPI_DT_RAW10;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_10;
+		break;
+	case V4L2_MBUS_FMT_SBGGR12_1X12:
+	case V4L2_MBUS_FMT_SGBRG12_1X12:
+	case V4L2_MBUS_FMT_SGRBG12_1X12:
+	case V4L2_MBUS_FMT_SRGGB12_1X12:
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_BAYER;
+		cfg->mipi_dt = MIPI_DT_RAW12;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_12;
+		break;
+	case V4L2_MBUS_FMT_JPEG_1X8:
+		/* TODO */
+		cfg->data_fmt = CSI_SENS_CONF_DATA_FMT_JPEG;
+		cfg->mipi_dt = MIPI_DT_RAW8;
+		cfg->data_width = IPU_CSI_DATA_WIDTH_8;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/*
+ * Fill a CSI bus config struct from mbus_config and mbus_framefmt.
+ */
+static void fill_csi_bus_cfg(struct ipu_csi_bus_config *csicfg,
+				 struct v4l2_mbus_config *mbus_cfg,
+				 struct v4l2_mbus_framefmt *mbus_fmt)
+{
+	memset(csicfg, 0, sizeof(*csicfg));
+
+	mbus_code_to_bus_cfg(csicfg, mbus_fmt->code);
+
+	switch (mbus_cfg->type) {
+	case V4L2_MBUS_PARALLEL:
+		csicfg->ext_vsync = 1;
+		csicfg->vsync_pol = (mbus_cfg->flags &
+				     V4L2_MBUS_VSYNC_ACTIVE_LOW) ? 1 : 0;
+		csicfg->hsync_pol = (mbus_cfg->flags &
+				     V4L2_MBUS_HSYNC_ACTIVE_LOW) ? 1 : 0;
+		csicfg->pixclk_pol = (mbus_cfg->flags &
+				      V4L2_MBUS_PCLK_SAMPLE_FALLING) ? 1 : 0;
+		csicfg->clk_mode = IPU_CSI_CLK_MODE_GATED_CLK;
+		break;
+	case V4L2_MBUS_BT656:
+		csicfg->ext_vsync = 0;
+		if (V4L2_FIELD_HAS_BOTH(mbus_fmt->field))
+			csicfg->clk_mode = IPU_CSI_CLK_MODE_CCIR656_INTERLACED;
+		else
+			csicfg->clk_mode = IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE;
+		break;
+	case V4L2_MBUS_CSI2:
+		/*
+		 * MIPI CSI-2 requires non gated clock mode, all other
+		 * parameters are not applicable for MIPI CSI-2 bus.
+		 */
+		csicfg->clk_mode = IPU_CSI_CLK_MODE_NONGATED_CLK;
+		break;
+	default:
+		/* will never get here, keep compiler quiet */
+		break;
+	}
+}
+
+int ipu_csi_init_interface(struct ipu_csi *csi,
+			   struct v4l2_mbus_config *mbus_cfg,
+			   struct v4l2_mbus_framefmt *mbus_fmt)
+{
+	struct ipu_csi_bus_config cfg;
+	unsigned long flags;
+	u32 data = 0;
+
+	fill_csi_bus_cfg(&cfg, mbus_cfg, mbus_fmt);
+
+	/* Set the CSI_SENS_CONF register remaining fields */
+	data |= cfg.data_width << CSI_SENS_CONF_DATA_WIDTH_SHIFT |
+		cfg.data_fmt << CSI_SENS_CONF_DATA_FMT_SHIFT |
+		cfg.data_pol << CSI_SENS_CONF_DATA_POL_SHIFT |
+		cfg.vsync_pol << CSI_SENS_CONF_VSYNC_POL_SHIFT |
+		cfg.hsync_pol << CSI_SENS_CONF_HSYNC_POL_SHIFT |
+		cfg.pixclk_pol << CSI_SENS_CONF_PIX_CLK_POL_SHIFT |
+		cfg.ext_vsync << CSI_SENS_CONF_EXT_VSYNC_SHIFT |
+		cfg.clk_mode << CSI_SENS_CONF_SENS_PRTCL_SHIFT |
+		cfg.pack_tight << CSI_SENS_CONF_PACK_TIGHT_SHIFT |
+		cfg.force_eof << CSI_SENS_CONF_FORCE_EOF_SHIFT |
+		cfg.data_en_pol << CSI_SENS_CONF_DATA_EN_POL_SHIFT;
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	ipu_csi_write(csi, data, CSI_SENS_CONF);
+
+	/* Setup sensor frame size */
+	ipu_csi_write(csi,
+		      (mbus_fmt->width - 1) | ((mbus_fmt->height - 1) << 16),
+		      CSI_SENS_FRM_SIZE);
+
+	/* Set CCIR registers */
+
+	switch (cfg.clk_mode) {
+	case IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE:
+		ipu_csi_write(csi, 0x40030, CSI_CCIR_CODE_1);
+		ipu_csi_write(csi, 0xFF0000, CSI_CCIR_CODE_3);
+		break;
+	case IPU_CSI_CLK_MODE_CCIR656_INTERLACED:
+		if (mbus_fmt->width == 720 && mbus_fmt->height == 576) {
+			/*
+			 * PAL case
+			 *
+			 * Field0BlankEnd = 0x6, Field0BlankStart = 0x2,
+			 * Field0ActiveEnd = 0x4, Field0ActiveStart = 0
+			 * Field1BlankEnd = 0x7, Field1BlankStart = 0x3,
+			 * Field1ActiveEnd = 0x5, Field1ActiveStart = 0x1
+			 */
+			ipu_csi_write(csi, 0x40596 | CSI_CCIR_ERR_DET_EN,
+					  CSI_CCIR_CODE_1);
+			ipu_csi_write(csi, 0xD07DF, CSI_CCIR_CODE_2);
+			ipu_csi_write(csi, 0xFF0000, CSI_CCIR_CODE_3);
+
+		} else if (mbus_fmt->width == 720 && mbus_fmt->height == 480) {
+			/*
+			 * NTSC case
+			 *
+			 * Field0BlankEnd = 0x7, Field0BlankStart = 0x3,
+			 * Field0ActiveEnd = 0x5, Field0ActiveStart = 0x1
+			 * Field1BlankEnd = 0x6, Field1BlankStart = 0x2,
+			 * Field1ActiveEnd = 0x4, Field1ActiveStart = 0
+			 */
+			ipu_csi_write(csi, 0xD07DF | CSI_CCIR_ERR_DET_EN,
+					  CSI_CCIR_CODE_1);
+			ipu_csi_write(csi, 0x40596, CSI_CCIR_CODE_2);
+			ipu_csi_write(csi, 0xFF0000, CSI_CCIR_CODE_3);
+		} else {
+			dev_err(csi->ipu->dev,
+				"Unsupported CCIR656 interlaced video mode\n");
+			spin_unlock_irqrestore(&csi->lock, flags);
+			return -EINVAL;
+		}
+		break;
+	case IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_DDR:
+	case IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_SDR:
+	case IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_DDR:
+	case IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_SDR:
+		ipu_csi_write(csi, 0x40030 | CSI_CCIR_ERR_DET_EN,
+				   CSI_CCIR_CODE_1);
+		ipu_csi_write(csi, 0xFF0000, CSI_CCIR_CODE_3);
+		break;
+	case IPU_CSI_CLK_MODE_GATED_CLK:
+	case IPU_CSI_CLK_MODE_NONGATED_CLK:
+		ipu_csi_write(csi, 0, CSI_CCIR_CODE_1);
+		break;
+	}
+
+	dev_dbg(csi->ipu->dev, "CSI_SENS_CONF = 0x%08X\n",
+		ipu_csi_read(csi, CSI_SENS_CONF));
+	dev_dbg(csi->ipu->dev, "CSI_ACT_FRM_SIZE = 0x%08X\n",
+		ipu_csi_read(csi, CSI_ACT_FRM_SIZE));
+
+	spin_unlock_irqrestore(&csi->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_csi_init_interface);
+
+bool ipu_csi_is_interlaced(struct ipu_csi *csi)
+{
+	unsigned long flags;
+	u32 sensor_protocol;
+
+	spin_lock_irqsave(&csi->lock, flags);
+	sensor_protocol =
+		(ipu_csi_read(csi, CSI_SENS_CONF) &
+		 CSI_SENS_CONF_SENS_PRTCL_MASK) >>
+		CSI_SENS_CONF_SENS_PRTCL_SHIFT;
+	spin_unlock_irqrestore(&csi->lock, flags);
+
+	switch (sensor_protocol) {
+	case IPU_CSI_CLK_MODE_GATED_CLK:
+	case IPU_CSI_CLK_MODE_NONGATED_CLK:
+	case IPU_CSI_CLK_MODE_CCIR656_PROGRESSIVE:
+	case IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_DDR:
+	case IPU_CSI_CLK_MODE_CCIR1120_PROGRESSIVE_SDR:
+		return false;
+	case IPU_CSI_CLK_MODE_CCIR656_INTERLACED:
+	case IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_DDR:
+	case IPU_CSI_CLK_MODE_CCIR1120_INTERLACED_SDR:
+		return true;
+	default:
+		dev_err(csi->ipu->dev,
+			"CSI %d sensor protocol unsupported\n", csi->id);
+		return false;
+	}
+}
+EXPORT_SYMBOL_GPL(ipu_csi_is_interlaced);
+
+void ipu_csi_get_window(struct ipu_csi *csi, struct v4l2_rect *w)
+{
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	reg = ipu_csi_read(csi, CSI_ACT_FRM_SIZE);
+	w->width = (reg & 0xFFFF) + 1;
+	w->height = (reg >> 16 & 0xFFFF) + 1;
+
+	reg = ipu_csi_read(csi, CSI_OUT_FRM_CTRL);
+	w->left = (reg & CSI_HSC_MASK) >> CSI_HSC_SHIFT;
+	w->top = (reg & CSI_VSC_MASK) >> CSI_VSC_SHIFT;
+
+	spin_unlock_irqrestore(&csi->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_csi_get_window);
+
+void ipu_csi_set_window(struct ipu_csi *csi, struct v4l2_rect *w)
+{
+	unsigned long flags;
+	u32 reg;
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	ipu_csi_write(csi, (w->width - 1) | ((w->height - 1) << 16),
+			  CSI_ACT_FRM_SIZE);
+
+	reg = ipu_csi_read(csi, CSI_OUT_FRM_CTRL);
+	reg &= ~(CSI_HSC_MASK | CSI_VSC_MASK);
+	reg |= ((w->top << CSI_VSC_SHIFT) | (w->left << CSI_HSC_SHIFT));
+	ipu_csi_write(csi, reg, CSI_OUT_FRM_CTRL);
+
+	spin_unlock_irqrestore(&csi->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_csi_set_window);
+
+void ipu_csi_set_test_generator(struct ipu_csi *csi, bool active,
+				u32 r_value, u32 g_value, u32 b_value,
+				u32 pix_clk)
+{
+	unsigned long flags;
+	u32 ipu_clk = clk_get_rate(csi->clk_ipu);
+	u32 temp;
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	temp = ipu_csi_read(csi, CSI_TST_CTRL);
+
+	if (active == false) {
+		temp &= ~CSI_TEST_GEN_MODE_EN;
+		ipu_csi_write(csi, temp, CSI_TST_CTRL);
+	} else {
+		/* Set sensb_mclk div_ratio */
+		ipu_csi_set_testgen_mclk(csi, pix_clk, ipu_clk);
+
+		temp &= ~(CSI_TEST_GEN_R_MASK | CSI_TEST_GEN_G_MASK |
+			  CSI_TEST_GEN_B_MASK);
+		temp |= CSI_TEST_GEN_MODE_EN;
+		temp |= (r_value << CSI_TEST_GEN_R_SHIFT) |
+			(g_value << CSI_TEST_GEN_G_SHIFT) |
+			(b_value << CSI_TEST_GEN_B_SHIFT);
+		ipu_csi_write(csi, temp, CSI_TST_CTRL);
+	}
+
+	spin_unlock_irqrestore(&csi->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_csi_set_test_generator);
+
+int ipu_csi_set_mipi_datatype(struct ipu_csi *csi, u32 vc,
+			      struct v4l2_mbus_framefmt *mbus_fmt)
+{
+	struct ipu_csi_bus_config cfg;
+	unsigned long flags;
+	u32 temp;
+
+	if (vc > 3)
+		return -EINVAL;
+
+	mbus_code_to_bus_cfg(&cfg, mbus_fmt->code);
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	temp = ipu_csi_read(csi, CSI_MIPI_DI);
+	temp &= ~(0xff << (vc * 8));
+	temp |= (cfg.mipi_dt << (vc * 8));
+	ipu_csi_write(csi, temp, CSI_MIPI_DI);
+
+	spin_unlock_irqrestore(&csi->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_csi_set_mipi_datatype);
+
+int ipu_csi_set_skip_smfc(struct ipu_csi *csi, u32 skip,
+			  u32 max_ratio, u32 id)
+{
+	unsigned long flags;
+	u32 temp;
+
+	if (max_ratio > 5 || id > 3)
+		return -EINVAL;
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	temp = ipu_csi_read(csi, CSI_SKIP);
+	temp &= ~(CSI_MAX_RATIO_SKIP_SMFC_MASK | CSI_ID_2_SKIP_MASK |
+		  CSI_SKIP_SMFC_MASK);
+	temp |= (max_ratio << CSI_MAX_RATIO_SKIP_SMFC_SHIFT) |
+		(id << CSI_ID_2_SKIP_SHIFT) |
+		(skip << CSI_SKIP_SMFC_SHIFT);
+	ipu_csi_write(csi, temp, CSI_SKIP);
+
+	spin_unlock_irqrestore(&csi->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_csi_set_skip_smfc);
+
+int ipu_csi_set_dest(struct ipu_csi *csi, enum ipu_csi_dest csi_dest)
+{
+	unsigned long flags;
+	u32 csi_sens_conf, dest;
+
+	if (csi_dest == IPU_CSI_DEST_IDMAC)
+		dest = CSI_DATA_DEST_IDMAC;
+	else
+		dest = CSI_DATA_DEST_IC; /* IC or VDIC */
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	csi_sens_conf = ipu_csi_read(csi, CSI_SENS_CONF);
+	csi_sens_conf &= ~CSI_SENS_CONF_DATA_DEST_MASK;
+	csi_sens_conf |= (dest << CSI_SENS_CONF_DATA_DEST_SHIFT);
+	ipu_csi_write(csi, csi_sens_conf, CSI_SENS_CONF);
+
+	spin_unlock_irqrestore(&csi->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_csi_set_dest);
+
+int ipu_csi_enable(struct ipu_csi *csi)
+{
+	ipu_module_enable(csi->ipu, csi->module);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_csi_enable);
+
+int ipu_csi_disable(struct ipu_csi *csi)
+{
+	ipu_module_disable(csi->ipu, csi->module);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_csi_disable);
+
+struct ipu_csi *ipu_csi_get(struct ipu_soc *ipu, int id)
+{
+	unsigned long flags;
+	struct ipu_csi *csi, *ret;
+
+	if (id > 1)
+		return ERR_PTR(-EINVAL);
+
+	csi = ipu->csi_priv[id];
+	ret = csi;
+
+	spin_lock_irqsave(&csi->lock, flags);
+
+	if (csi->inuse) {
+		ret = ERR_PTR(-EBUSY);
+		goto unlock;
+	}
+
+	csi->inuse = true;
+unlock:
+	spin_unlock_irqrestore(&csi->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ipu_csi_get);
+
+void ipu_csi_put(struct ipu_csi *csi)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&csi->lock, flags);
+	csi->inuse = false;
+	spin_unlock_irqrestore(&csi->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_csi_put);
+
+int ipu_csi_init(struct ipu_soc *ipu, struct device *dev, int id,
+		 unsigned long base, u32 module, struct clk *clk_ipu)
+{
+	struct ipu_csi *csi;
+
+	if (id > 1)
+		return -ENODEV;
+
+	csi = devm_kzalloc(dev, sizeof(*csi), GFP_KERNEL);
+	if (!csi)
+		return -ENOMEM;
+
+	ipu->csi_priv[id] = csi;
+
+	spin_lock_init(&csi->lock);
+	csi->module = module;
+	csi->id = id;
+	csi->clk_ipu = clk_ipu;
+	csi->base = devm_ioremap(dev, base, PAGE_SIZE);
+	if (!csi->base)
+		return -ENOMEM;
+
+	dev_dbg(dev, "CSI%d base: 0x%08lx remapped to %p\n",
+		id, base, csi->base);
+	csi->ipu = ipu;
+
+	return 0;
+}
+
+void ipu_csi_exit(struct ipu_soc *ipu, int id)
+{
+}
+
+void ipu_csi_dump(struct ipu_csi *csi)
+{
+	dev_dbg(csi->ipu->dev, "CSI_SENS_CONF:     %08x\n",
+		ipu_csi_read(csi, CSI_SENS_CONF));
+	dev_dbg(csi->ipu->dev, "CSI_SENS_FRM_SIZE: %08x\n",
+		ipu_csi_read(csi, CSI_SENS_FRM_SIZE));
+	dev_dbg(csi->ipu->dev, "CSI_ACT_FRM_SIZE:  %08x\n",
+		ipu_csi_read(csi, CSI_ACT_FRM_SIZE));
+	dev_dbg(csi->ipu->dev, "CSI_OUT_FRM_CTRL:  %08x\n",
+		ipu_csi_read(csi, CSI_OUT_FRM_CTRL));
+	dev_dbg(csi->ipu->dev, "CSI_TST_CTRL:      %08x\n",
+		ipu_csi_read(csi, CSI_TST_CTRL));
+	dev_dbg(csi->ipu->dev, "CSI_CCIR_CODE_1:   %08x\n",
+		ipu_csi_read(csi, CSI_CCIR_CODE_1));
+	dev_dbg(csi->ipu->dev, "CSI_CCIR_CODE_2:   %08x\n",
+		ipu_csi_read(csi, CSI_CCIR_CODE_2));
+	dev_dbg(csi->ipu->dev, "CSI_CCIR_CODE_3:   %08x\n",
+		ipu_csi_read(csi, CSI_CCIR_CODE_3));
+	dev_dbg(csi->ipu->dev, "CSI_MIPI_DI:       %08x\n",
+		ipu_csi_read(csi, CSI_MIPI_DI));
+	dev_dbg(csi->ipu->dev, "CSI_SKIP:          %08x\n",
+		ipu_csi_read(csi, CSI_SKIP));
+}
+EXPORT_SYMBOL_GPL(ipu_csi_dump);
diff --git a/drivers/gpu/ipu-v3/ipu-ic.c b/drivers/gpu/ipu-v3/ipu-ic.c
new file mode 100644
index 000000000000..ad75588e1629
--- /dev/null
+++ b/drivers/gpu/ipu-v3/ipu-ic.c
@@ -0,0 +1,778 @@
+/*
+ * Copyright (C) 2012-2014 Mentor Graphics Inc.
+ * Copyright 2005-2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+
+#include <linux/types.h>
+#include <linux/init.h>
+#include <linux/errno.h>
+#include <linux/spinlock.h>
+#include <linux/bitrev.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include "ipu-prv.h"
+
+/* IC Register Offsets */
+#define IC_CONF                 0x0000
+#define IC_PRP_ENC_RSC          0x0004
+#define IC_PRP_VF_RSC           0x0008
+#define IC_PP_RSC               0x000C
+#define IC_CMBP_1               0x0010
+#define IC_CMBP_2               0x0014
+#define IC_IDMAC_1              0x0018
+#define IC_IDMAC_2              0x001C
+#define IC_IDMAC_3              0x0020
+#define IC_IDMAC_4              0x0024
+
+/* IC Register Fields */
+#define IC_CONF_PRPENC_EN       (1 << 0)
+#define IC_CONF_PRPENC_CSC1     (1 << 1)
+#define IC_CONF_PRPENC_ROT_EN   (1 << 2)
+#define IC_CONF_PRPVF_EN        (1 << 8)
+#define IC_CONF_PRPVF_CSC1      (1 << 9)
+#define IC_CONF_PRPVF_CSC2      (1 << 10)
+#define IC_CONF_PRPVF_CMB       (1 << 11)
+#define IC_CONF_PRPVF_ROT_EN    (1 << 12)
+#define IC_CONF_PP_EN           (1 << 16)
+#define IC_CONF_PP_CSC1         (1 << 17)
+#define IC_CONF_PP_CSC2         (1 << 18)
+#define IC_CONF_PP_CMB          (1 << 19)
+#define IC_CONF_PP_ROT_EN       (1 << 20)
+#define IC_CONF_IC_GLB_LOC_A    (1 << 28)
+#define IC_CONF_KEY_COLOR_EN    (1 << 29)
+#define IC_CONF_RWS_EN          (1 << 30)
+#define IC_CONF_CSI_MEM_WR_EN   (1 << 31)
+
+#define IC_IDMAC_1_CB0_BURST_16         (1 << 0)
+#define IC_IDMAC_1_CB1_BURST_16         (1 << 1)
+#define IC_IDMAC_1_CB2_BURST_16         (1 << 2)
+#define IC_IDMAC_1_CB3_BURST_16         (1 << 3)
+#define IC_IDMAC_1_CB4_BURST_16         (1 << 4)
+#define IC_IDMAC_1_CB5_BURST_16         (1 << 5)
+#define IC_IDMAC_1_CB6_BURST_16         (1 << 6)
+#define IC_IDMAC_1_CB7_BURST_16         (1 << 7)
+#define IC_IDMAC_1_PRPENC_ROT_MASK      (0x7 << 11)
+#define IC_IDMAC_1_PRPENC_ROT_OFFSET    11
+#define IC_IDMAC_1_PRPVF_ROT_MASK       (0x7 << 14)
+#define IC_IDMAC_1_PRPVF_ROT_OFFSET     14
+#define IC_IDMAC_1_PP_ROT_MASK          (0x7 << 17)
+#define IC_IDMAC_1_PP_ROT_OFFSET        17
+#define IC_IDMAC_1_PP_FLIP_RS           (1 << 22)
+#define IC_IDMAC_1_PRPVF_FLIP_RS        (1 << 21)
+#define IC_IDMAC_1_PRPENC_FLIP_RS       (1 << 20)
+
+#define IC_IDMAC_2_PRPENC_HEIGHT_MASK   (0x3ff << 0)
+#define IC_IDMAC_2_PRPENC_HEIGHT_OFFSET 0
+#define IC_IDMAC_2_PRPVF_HEIGHT_MASK    (0x3ff << 10)
+#define IC_IDMAC_2_PRPVF_HEIGHT_OFFSET  10
+#define IC_IDMAC_2_PP_HEIGHT_MASK       (0x3ff << 20)
+#define IC_IDMAC_2_PP_HEIGHT_OFFSET     20
+
+#define IC_IDMAC_3_PRPENC_WIDTH_MASK    (0x3ff << 0)
+#define IC_IDMAC_3_PRPENC_WIDTH_OFFSET  0
+#define IC_IDMAC_3_PRPVF_WIDTH_MASK     (0x3ff << 10)
+#define IC_IDMAC_3_PRPVF_WIDTH_OFFSET   10
+#define IC_IDMAC_3_PP_WIDTH_MASK        (0x3ff << 20)
+#define IC_IDMAC_3_PP_WIDTH_OFFSET      20
+
+struct ic_task_regoffs {
+	u32 rsc;
+	u32 tpmem_csc[2];
+};
+
+struct ic_task_bitfields {
+	u32 ic_conf_en;
+	u32 ic_conf_rot_en;
+	u32 ic_conf_cmb_en;
+	u32 ic_conf_csc1_en;
+	u32 ic_conf_csc2_en;
+	u32 ic_cmb_galpha_bit;
+};
+
+static const struct ic_task_regoffs ic_task_reg[IC_NUM_TASKS] = {
+	[IC_TASK_ENCODER] = {
+		.rsc = IC_PRP_ENC_RSC,
+		.tpmem_csc = {0x2008, 0},
+	},
+	[IC_TASK_VIEWFINDER] = {
+		.rsc = IC_PRP_VF_RSC,
+		.tpmem_csc = {0x4028, 0x4040},
+	},
+	[IC_TASK_POST_PROCESSOR] = {
+		.rsc = IC_PP_RSC,
+		.tpmem_csc = {0x6060, 0x6078},
+	},
+};
+
+static const struct ic_task_bitfields ic_task_bit[IC_NUM_TASKS] = {
+	[IC_TASK_ENCODER] = {
+		.ic_conf_en = IC_CONF_PRPENC_EN,
+		.ic_conf_rot_en = IC_CONF_PRPENC_ROT_EN,
+		.ic_conf_cmb_en = 0,    /* NA */
+		.ic_conf_csc1_en = IC_CONF_PRPENC_CSC1,
+		.ic_conf_csc2_en = 0,   /* NA */
+		.ic_cmb_galpha_bit = 0, /* NA */
+	},
+	[IC_TASK_VIEWFINDER] = {
+		.ic_conf_en = IC_CONF_PRPVF_EN,
+		.ic_conf_rot_en = IC_CONF_PRPVF_ROT_EN,
+		.ic_conf_cmb_en = IC_CONF_PRPVF_CMB,
+		.ic_conf_csc1_en = IC_CONF_PRPVF_CSC1,
+		.ic_conf_csc2_en = IC_CONF_PRPVF_CSC2,
+		.ic_cmb_galpha_bit = 0,
+	},
+	[IC_TASK_POST_PROCESSOR] = {
+		.ic_conf_en = IC_CONF_PP_EN,
+		.ic_conf_rot_en = IC_CONF_PP_ROT_EN,
+		.ic_conf_cmb_en = IC_CONF_PP_CMB,
+		.ic_conf_csc1_en = IC_CONF_PP_CSC1,
+		.ic_conf_csc2_en = IC_CONF_PP_CSC2,
+		.ic_cmb_galpha_bit = 8,
+	},
+};
+
+struct ipu_ic_priv;
+
+struct ipu_ic {
+	enum ipu_ic_task task;
+	const struct ic_task_regoffs *reg;
+	const struct ic_task_bitfields *bit;
+
+	enum ipu_color_space in_cs, g_in_cs;
+	enum ipu_color_space out_cs;
+	bool graphics;
+	bool rotation;
+	bool in_use;
+
+	struct ipu_ic_priv *priv;
+};
+
+struct ipu_ic_priv {
+	void __iomem *base;
+	void __iomem *tpmem_base;
+	spinlock_t lock;
+	struct ipu_soc *ipu;
+	int use_count;
+	struct ipu_ic task[IC_NUM_TASKS];
+};
+
+static inline u32 ipu_ic_read(struct ipu_ic *ic, unsigned offset)
+{
+	return readl(ic->priv->base + offset);
+}
+
+static inline void ipu_ic_write(struct ipu_ic *ic, u32 value, unsigned offset)
+{
+	writel(value, ic->priv->base + offset);
+}
+
+struct ic_csc_params {
+	s16 coeff[3][3];	/* signed 9-bit integer coefficients */
+	s16 offset[3];		/* signed 11+2-bit fixed point offset */
+	u8 scale:2;		/* scale coefficients * 2^(scale-1) */
+	bool sat:1;		/* saturate to (16, 235(Y) / 240(U, V)) */
+};
+
+/*
+ * Y = R *  .299 + G *  .587 + B *  .114;
+ * U = R * -.169 + G * -.332 + B *  .500 + 128.;
+ * V = R *  .500 + G * -.419 + B * -.0813 + 128.;
+ */
+static const struct ic_csc_params ic_csc_rgb2ycbcr = {
+	.coeff = {
+		{ 77, 150, 29 },
+		{ 469, 427, 128 },
+		{ 128, 405, 491 },
+	},
+	.offset = { 0, 512, 512 },
+	.scale = 1,
+};
+
+/* transparent RGB->RGB matrix for graphics combining */
+static const struct ic_csc_params ic_csc_rgb2rgb = {
+	.coeff = {
+		{ 128, 0, 0 },
+		{ 0, 128, 0 },
+		{ 0, 0, 128 },
+	},
+	.scale = 2,
+};
+
+/*
+ * R = (1.164 * (Y - 16)) + (1.596 * (Cr - 128));
+ * G = (1.164 * (Y - 16)) - (0.392 * (Cb - 128)) - (0.813 * (Cr - 128));
+ * B = (1.164 * (Y - 16)) + (2.017 * (Cb - 128);
+ */
+static const struct ic_csc_params ic_csc_ycbcr2rgb = {
+	.coeff = {
+		{ 149, 0, 204 },
+		{ 149, 462, 408 },
+		{ 149, 255, 0 },
+	},
+	.offset = { -446, 266, -554 },
+	.scale = 2,
+};
+
+static int init_csc(struct ipu_ic *ic,
+		    enum ipu_color_space inf,
+		    enum ipu_color_space outf,
+		    int csc_index)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	const struct ic_csc_params *params;
+	u32 __iomem *base;
+	const u16 (*c)[3];
+	const u16 *a;
+	u32 param;
+
+	base = (u32 __iomem *)
+		(priv->tpmem_base + ic->reg->tpmem_csc[csc_index]);
+
+	if (inf == IPUV3_COLORSPACE_YUV && outf == IPUV3_COLORSPACE_RGB)
+		params = &ic_csc_ycbcr2rgb;
+	else if (inf == IPUV3_COLORSPACE_RGB && outf == IPUV3_COLORSPACE_YUV)
+		params = &ic_csc_rgb2ycbcr;
+	else if (inf == IPUV3_COLORSPACE_RGB && outf == IPUV3_COLORSPACE_RGB)
+		params = &ic_csc_rgb2rgb;
+	else {
+		dev_err(priv->ipu->dev, "Unsupported color space conversion\n");
+		return -EINVAL;
+	}
+
+	/* Cast to unsigned */
+	c = (const u16 (*)[3])params->coeff;
+	a = (const u16 *)params->offset;
+
+	param = ((a[0] & 0x1f) << 27) | ((c[0][0] & 0x1ff) << 18) |
+		((c[1][1] & 0x1ff) << 9) | (c[2][2] & 0x1ff);
+	writel(param, base++);
+
+	param = ((a[0] & 0x1fe0) >> 5) | (params->scale << 8) |
+		(params->sat << 9);
+	writel(param, base++);
+
+	param = ((a[1] & 0x1f) << 27) | ((c[0][1] & 0x1ff) << 18) |
+		((c[1][0] & 0x1ff) << 9) | (c[2][0] & 0x1ff);
+	writel(param, base++);
+
+	param = ((a[1] & 0x1fe0) >> 5);
+	writel(param, base++);
+
+	param = ((a[2] & 0x1f) << 27) | ((c[0][2] & 0x1ff) << 18) |
+		((c[1][2] & 0x1ff) << 9) | (c[2][1] & 0x1ff);
+	writel(param, base++);
+
+	param = ((a[2] & 0x1fe0) >> 5);
+	writel(param, base++);
+
+	return 0;
+}
+
+static int calc_resize_coeffs(struct ipu_ic *ic,
+			      u32 in_size, u32 out_size,
+			      u32 *resize_coeff,
+			      u32 *downsize_coeff)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	struct ipu_soc *ipu = priv->ipu;
+	u32 temp_size, temp_downsize;
+
+	/*
+	 * Input size cannot be more than 4096, and output size cannot
+	 * be more than 1024
+	 */
+	if (in_size > 4096) {
+		dev_err(ipu->dev, "Unsupported resize (in_size > 4096)\n");
+		return -EINVAL;
+	}
+	if (out_size > 1024) {
+		dev_err(ipu->dev, "Unsupported resize (out_size > 1024)\n");
+		return -EINVAL;
+	}
+
+	/* Cannot downsize more than 8:1 */
+	if ((out_size << 3) < in_size) {
+		dev_err(ipu->dev, "Unsupported downsize\n");
+		return -EINVAL;
+	}
+
+	/* Compute downsizing coefficient */
+	temp_downsize = 0;
+	temp_size = in_size;
+	while (((temp_size > 1024) || (temp_size >= out_size * 2)) &&
+	       (temp_downsize < 2)) {
+		temp_size >>= 1;
+		temp_downsize++;
+	}
+	*downsize_coeff = temp_downsize;
+
+	/*
+	 * compute resizing coefficient using the following equation:
+	 * resize_coeff = M * (SI - 1) / (SO - 1)
+	 * where M = 2^13, SI = input size, SO = output size
+	 */
+	*resize_coeff = (8192L * (temp_size - 1)) / (out_size - 1);
+	if (*resize_coeff >= 16384L) {
+		dev_err(ipu->dev, "Warning! Overflow on resize coeff.\n");
+		*resize_coeff = 0x3FFF;
+	}
+
+	return 0;
+}
+
+void ipu_ic_task_enable(struct ipu_ic *ic)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	unsigned long flags;
+	u32 ic_conf;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ic_conf = ipu_ic_read(ic, IC_CONF);
+
+	ic_conf |= ic->bit->ic_conf_en;
+
+	if (ic->rotation)
+		ic_conf |= ic->bit->ic_conf_rot_en;
+
+	if (ic->in_cs != ic->out_cs)
+		ic_conf |= ic->bit->ic_conf_csc1_en;
+
+	if (ic->graphics) {
+		ic_conf |= ic->bit->ic_conf_cmb_en;
+		ic_conf |= ic->bit->ic_conf_csc1_en;
+
+		if (ic->g_in_cs != ic->out_cs)
+			ic_conf |= ic->bit->ic_conf_csc2_en;
+	}
+
+	ipu_ic_write(ic, ic_conf, IC_CONF);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_ic_task_enable);
+
+void ipu_ic_task_disable(struct ipu_ic *ic)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	unsigned long flags;
+	u32 ic_conf;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ic_conf = ipu_ic_read(ic, IC_CONF);
+
+	ic_conf &= ~(ic->bit->ic_conf_en |
+		     ic->bit->ic_conf_csc1_en |
+		     ic->bit->ic_conf_rot_en);
+	if (ic->bit->ic_conf_csc2_en)
+		ic_conf &= ~ic->bit->ic_conf_csc2_en;
+	if (ic->bit->ic_conf_cmb_en)
+		ic_conf &= ~ic->bit->ic_conf_cmb_en;
+
+	ipu_ic_write(ic, ic_conf, IC_CONF);
+
+	ic->rotation = ic->graphics = false;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_ic_task_disable);
+
+int ipu_ic_task_graphics_init(struct ipu_ic *ic,
+			      enum ipu_color_space in_g_cs,
+			      bool galpha_en, u32 galpha,
+			      bool colorkey_en, u32 colorkey)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	unsigned long flags;
+	u32 reg, ic_conf;
+	int ret = 0;
+
+	if (ic->task == IC_TASK_ENCODER)
+		return -EINVAL;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ic_conf = ipu_ic_read(ic, IC_CONF);
+
+	if (!(ic_conf & ic->bit->ic_conf_csc1_en)) {
+		/* need transparent CSC1 conversion */
+		ret = init_csc(ic, IPUV3_COLORSPACE_RGB,
+			       IPUV3_COLORSPACE_RGB, 0);
+		if (ret)
+			goto unlock;
+	}
+
+	ic->g_in_cs = in_g_cs;
+
+	if (ic->g_in_cs != ic->out_cs) {
+		ret = init_csc(ic, ic->g_in_cs, ic->out_cs, 1);
+		if (ret)
+			goto unlock;
+	}
+
+	if (galpha_en) {
+		ic_conf |= IC_CONF_IC_GLB_LOC_A;
+		reg = ipu_ic_read(ic, IC_CMBP_1);
+		reg &= ~(0xff << ic->bit->ic_cmb_galpha_bit);
+		reg |= (galpha << ic->bit->ic_cmb_galpha_bit);
+		ipu_ic_write(ic, reg, IC_CMBP_1);
+	} else
+		ic_conf &= ~IC_CONF_IC_GLB_LOC_A;
+
+	if (colorkey_en) {
+		ic_conf |= IC_CONF_KEY_COLOR_EN;
+		ipu_ic_write(ic, colorkey, IC_CMBP_2);
+	} else
+		ic_conf &= ~IC_CONF_KEY_COLOR_EN;
+
+	ipu_ic_write(ic, ic_conf, IC_CONF);
+
+	ic->graphics = true;
+unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ipu_ic_task_graphics_init);
+
+int ipu_ic_task_init(struct ipu_ic *ic,
+		     int in_width, int in_height,
+		     int out_width, int out_height,
+		     enum ipu_color_space in_cs,
+		     enum ipu_color_space out_cs)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	u32 reg, downsize_coeff, resize_coeff;
+	unsigned long flags;
+	int ret = 0;
+
+	/* Setup vertical resizing */
+	ret = calc_resize_coeffs(ic, in_height, out_height,
+				 &resize_coeff, &downsize_coeff);
+	if (ret)
+		return ret;
+
+	reg = (downsize_coeff << 30) | (resize_coeff << 16);
+
+	/* Setup horizontal resizing */
+	ret = calc_resize_coeffs(ic, in_width, out_width,
+				 &resize_coeff, &downsize_coeff);
+	if (ret)
+		return ret;
+
+	reg |= (downsize_coeff << 14) | resize_coeff;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ipu_ic_write(ic, reg, ic->reg->rsc);
+
+	/* Setup color space conversion */
+	ic->in_cs = in_cs;
+	ic->out_cs = out_cs;
+
+	if (ic->in_cs != ic->out_cs) {
+		ret = init_csc(ic, ic->in_cs, ic->out_cs, 0);
+		if (ret)
+			goto unlock;
+	}
+
+unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ipu_ic_task_init);
+
+int ipu_ic_task_idma_init(struct ipu_ic *ic, struct ipuv3_channel *channel,
+			  u32 width, u32 height, int burst_size,
+			  enum ipu_rotate_mode rot)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	struct ipu_soc *ipu = priv->ipu;
+	u32 ic_idmac_1, ic_idmac_2, ic_idmac_3;
+	u32 temp_rot = bitrev8(rot) >> 5;
+	bool need_hor_flip = false;
+	unsigned long flags;
+	int ret = 0;
+
+	if ((burst_size != 8) && (burst_size != 16)) {
+		dev_err(ipu->dev, "Illegal burst length for IC\n");
+		return -EINVAL;
+	}
+
+	width--;
+	height--;
+
+	if (temp_rot & 0x2)	/* Need horizontal flip */
+		need_hor_flip = true;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ic_idmac_1 = ipu_ic_read(ic, IC_IDMAC_1);
+	ic_idmac_2 = ipu_ic_read(ic, IC_IDMAC_2);
+	ic_idmac_3 = ipu_ic_read(ic, IC_IDMAC_3);
+
+	switch (channel->num) {
+	case IPUV3_CHANNEL_IC_PP_MEM:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB2_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB2_BURST_16;
+
+		if (need_hor_flip)
+			ic_idmac_1 |= IC_IDMAC_1_PP_FLIP_RS;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_PP_FLIP_RS;
+
+		ic_idmac_2 &= ~IC_IDMAC_2_PP_HEIGHT_MASK;
+		ic_idmac_2 |= height << IC_IDMAC_2_PP_HEIGHT_OFFSET;
+
+		ic_idmac_3 &= ~IC_IDMAC_3_PP_WIDTH_MASK;
+		ic_idmac_3 |= width << IC_IDMAC_3_PP_WIDTH_OFFSET;
+		break;
+	case IPUV3_CHANNEL_MEM_IC_PP:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB5_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB5_BURST_16;
+		break;
+	case IPUV3_CHANNEL_MEM_ROT_PP:
+		ic_idmac_1 &= ~IC_IDMAC_1_PP_ROT_MASK;
+		ic_idmac_1 |= temp_rot << IC_IDMAC_1_PP_ROT_OFFSET;
+		break;
+	case IPUV3_CHANNEL_MEM_IC_PRP_VF:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB6_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB6_BURST_16;
+		break;
+	case IPUV3_CHANNEL_IC_PRP_ENC_MEM:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB0_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB0_BURST_16;
+
+		if (need_hor_flip)
+			ic_idmac_1 |= IC_IDMAC_1_PRPENC_FLIP_RS;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_PRPENC_FLIP_RS;
+
+		ic_idmac_2 &= ~IC_IDMAC_2_PRPENC_HEIGHT_MASK;
+		ic_idmac_2 |= height << IC_IDMAC_2_PRPENC_HEIGHT_OFFSET;
+
+		ic_idmac_3 &= ~IC_IDMAC_3_PRPENC_WIDTH_MASK;
+		ic_idmac_3 |= width << IC_IDMAC_3_PRPENC_WIDTH_OFFSET;
+		break;
+	case IPUV3_CHANNEL_MEM_ROT_ENC:
+		ic_idmac_1 &= ~IC_IDMAC_1_PRPENC_ROT_MASK;
+		ic_idmac_1 |= temp_rot << IC_IDMAC_1_PRPENC_ROT_OFFSET;
+		break;
+	case IPUV3_CHANNEL_IC_PRP_VF_MEM:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB1_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB1_BURST_16;
+
+		if (need_hor_flip)
+			ic_idmac_1 |= IC_IDMAC_1_PRPVF_FLIP_RS;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_PRPVF_FLIP_RS;
+
+		ic_idmac_2 &= ~IC_IDMAC_2_PRPVF_HEIGHT_MASK;
+		ic_idmac_2 |= height << IC_IDMAC_2_PRPVF_HEIGHT_OFFSET;
+
+		ic_idmac_3 &= ~IC_IDMAC_3_PRPVF_WIDTH_MASK;
+		ic_idmac_3 |= width << IC_IDMAC_3_PRPVF_WIDTH_OFFSET;
+		break;
+	case IPUV3_CHANNEL_MEM_ROT_VF:
+		ic_idmac_1 &= ~IC_IDMAC_1_PRPVF_ROT_MASK;
+		ic_idmac_1 |= temp_rot << IC_IDMAC_1_PRPVF_ROT_OFFSET;
+		break;
+	case IPUV3_CHANNEL_G_MEM_IC_PRP_VF:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB3_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB3_BURST_16;
+		break;
+	case IPUV3_CHANNEL_G_MEM_IC_PP:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB4_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB4_BURST_16;
+		break;
+	case IPUV3_CHANNEL_VDI_MEM_IC_VF:
+		if (burst_size == 16)
+			ic_idmac_1 |= IC_IDMAC_1_CB7_BURST_16;
+		else
+			ic_idmac_1 &= ~IC_IDMAC_1_CB7_BURST_16;
+		break;
+	default:
+		goto unlock;
+	}
+
+	ipu_ic_write(ic, ic_idmac_1, IC_IDMAC_1);
+	ipu_ic_write(ic, ic_idmac_2, IC_IDMAC_2);
+	ipu_ic_write(ic, ic_idmac_3, IC_IDMAC_3);
+
+	if (rot >= IPU_ROTATE_90_RIGHT)
+		ic->rotation = true;
+
+unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ipu_ic_task_idma_init);
+
+int ipu_ic_enable(struct ipu_ic *ic)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	unsigned long flags;
+	u32 module = IPU_CONF_IC_EN;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (ic->rotation)
+		module |= IPU_CONF_ROT_EN;
+
+	if (!priv->use_count)
+		ipu_module_enable(priv->ipu, module);
+
+	priv->use_count++;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_ic_enable);
+
+int ipu_ic_disable(struct ipu_ic *ic)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	unsigned long flags;
+	u32 module = IPU_CONF_IC_EN | IPU_CONF_ROT_EN;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	priv->use_count--;
+
+	if (!priv->use_count)
+		ipu_module_disable(priv->ipu, module);
+
+	if (priv->use_count < 0)
+		priv->use_count = 0;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_ic_disable);
+
+struct ipu_ic *ipu_ic_get(struct ipu_soc *ipu, enum ipu_ic_task task)
+{
+	struct ipu_ic_priv *priv = ipu->ic_priv;
+	unsigned long flags;
+	struct ipu_ic *ic, *ret;
+
+	if (task >= IC_NUM_TASKS)
+		return ERR_PTR(-EINVAL);
+
+	ic = &priv->task[task];
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (ic->in_use) {
+		ret = ERR_PTR(-EBUSY);
+		goto unlock;
+	}
+
+	ic->in_use = true;
+	ret = ic;
+
+unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ipu_ic_get);
+
+void ipu_ic_put(struct ipu_ic *ic)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ic->in_use = false;
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_ic_put);
+
+int ipu_ic_init(struct ipu_soc *ipu, struct device *dev,
+		unsigned long base, unsigned long tpmem_base)
+{
+	struct ipu_ic_priv *priv;
+	int i;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	ipu->ic_priv = priv;
+
+	spin_lock_init(&priv->lock);
+	priv->base = devm_ioremap(dev, base, PAGE_SIZE);
+	if (!priv->base)
+		return -ENOMEM;
+	priv->tpmem_base = devm_ioremap(dev, tpmem_base, SZ_64K);
+	if (!priv->tpmem_base)
+		return -ENOMEM;
+
+	dev_dbg(dev, "IC base: 0x%08lx remapped to %p\n", base, priv->base);
+
+	priv->ipu = ipu;
+
+	for (i = 0; i < IC_NUM_TASKS; i++) {
+		priv->task[i].task = i;
+		priv->task[i].priv = priv;
+		priv->task[i].reg = &ic_task_reg[i];
+		priv->task[i].bit = &ic_task_bit[i];
+	}
+
+	return 0;
+}
+
+void ipu_ic_exit(struct ipu_soc *ipu)
+{
+}
+
+void ipu_ic_dump(struct ipu_ic *ic)
+{
+	struct ipu_ic_priv *priv = ic->priv;
+	struct ipu_soc *ipu = priv->ipu;
+
+	dev_dbg(ipu->dev, "IC_CONF = \t0x%08X\n",
+		ipu_ic_read(ic, IC_CONF));
+	dev_dbg(ipu->dev, "IC_PRP_ENC_RSC = \t0x%08X\n",
+		ipu_ic_read(ic, IC_PRP_ENC_RSC));
+	dev_dbg(ipu->dev, "IC_PRP_VF_RSC = \t0x%08X\n",
+		ipu_ic_read(ic, IC_PRP_VF_RSC));
+	dev_dbg(ipu->dev, "IC_PP_RSC = \t0x%08X\n",
+		ipu_ic_read(ic, IC_PP_RSC));
+	dev_dbg(ipu->dev, "IC_CMBP_1 = \t0x%08X\n",
+		ipu_ic_read(ic, IC_CMBP_1));
+	dev_dbg(ipu->dev, "IC_CMBP_2 = \t0x%08X\n",
+		ipu_ic_read(ic, IC_CMBP_2));
+	dev_dbg(ipu->dev, "IC_IDMAC_1 = \t0x%08X\n",
+		ipu_ic_read(ic, IC_IDMAC_1));
+	dev_dbg(ipu->dev, "IC_IDMAC_2 = \t0x%08X\n",
+		ipu_ic_read(ic, IC_IDMAC_2));
+	dev_dbg(ipu->dev, "IC_IDMAC_3 = \t0x%08X\n",
+		ipu_ic_read(ic, IC_IDMAC_3));
+	dev_dbg(ipu->dev, "IC_IDMAC_4 = \t0x%08X\n",
+		ipu_ic_read(ic, IC_IDMAC_4));
+}
+EXPORT_SYMBOL_GPL(ipu_ic_dump);
diff --git a/drivers/gpu/ipu-v3/ipu-prv.h b/drivers/gpu/ipu-v3/ipu-prv.h
index c93f50ec04f7..bfb1e8a4483f 100644
--- a/drivers/gpu/ipu-v3/ipu-prv.h
+++ b/drivers/gpu/ipu-v3/ipu-prv.h
@@ -24,23 +24,6 @@ struct ipu_soc;
 
 #include <video/imx-ipu-v3.h>
 
-#define IPUV3_CHANNEL_CSI0			 0
-#define IPUV3_CHANNEL_CSI1			 1
-#define IPUV3_CHANNEL_CSI2			 2
-#define IPUV3_CHANNEL_CSI3			 3
-#define IPUV3_CHANNEL_MEM_BG_SYNC		23
-#define IPUV3_CHANNEL_MEM_FG_SYNC		27
-#define IPUV3_CHANNEL_MEM_DC_SYNC		28
-#define IPUV3_CHANNEL_MEM_FG_SYNC_ALPHA		31
-#define IPUV3_CHANNEL_MEM_DC_ASYNC		41
-#define IPUV3_CHANNEL_ROT_ENC_MEM		45
-#define IPUV3_CHANNEL_ROT_VF_MEM		46
-#define IPUV3_CHANNEL_ROT_PP_MEM		47
-#define IPUV3_CHANNEL_ROT_ENC_MEM_OUT		48
-#define IPUV3_CHANNEL_ROT_VF_MEM_OUT		49
-#define IPUV3_CHANNEL_ROT_PP_MEM_OUT		50
-#define IPUV3_CHANNEL_MEM_BG_SYNC_ALPHA		51
-
 #define IPU_MCU_T_DEFAULT	8
 #define IPU_CM_IDMAC_REG_OFS	0x00008000
 #define IPU_CM_IC_REG_OFS	0x00020000
@@ -85,6 +68,7 @@ struct ipu_soc;
 #define IPU_DISP_TASK_STAT		IPU_CM_REG(0x0254)
 #define IPU_CHA_BUF0_RDY(ch)		IPU_CM_REG(0x0268 + 4 * ((ch) / 32))
 #define IPU_CHA_BUF1_RDY(ch)		IPU_CM_REG(0x0270 + 4 * ((ch) / 32))
+#define IPU_CHA_BUF2_RDY(ch)		IPU_CM_REG(0x0288 + 4 * ((ch) / 32))
 #define IPU_ALT_CHA_BUF0_RDY(ch)	IPU_CM_REG(0x0278 + 4 * ((ch) / 32))
 #define IPU_ALT_CHA_BUF1_RDY(ch)	IPU_CM_REG(0x0280 + 4 * ((ch) / 32))
 
@@ -148,9 +132,12 @@ struct ipuv3_channel {
 	struct ipu_soc *ipu;
 };
 
+struct ipu_cpmem;
+struct ipu_csi;
 struct ipu_dc_priv;
 struct ipu_dmfc_priv;
 struct ipu_di;
+struct ipu_ic_priv;
 struct ipu_smfc_priv;
 
 struct ipu_devtype;
@@ -164,7 +151,6 @@ struct ipu_soc {
 
 	void __iomem		*cm_reg;
 	void __iomem		*idmac_reg;
-	struct ipu_ch_param __iomem	*cpmem_base;
 
 	int			usecount;
 
@@ -176,13 +162,27 @@ struct ipu_soc {
 	int			irq_err;
 	struct irq_domain	*domain;
 
+	struct ipu_cpmem	*cpmem_priv;
 	struct ipu_dc_priv	*dc_priv;
 	struct ipu_dp_priv	*dp_priv;
 	struct ipu_dmfc_priv	*dmfc_priv;
 	struct ipu_di		*di_priv[2];
+	struct ipu_csi		*csi_priv[2];
+	struct ipu_ic_priv	*ic_priv;
 	struct ipu_smfc_priv	*smfc_priv;
 };
 
+static inline u32 ipu_idmac_read(struct ipu_soc *ipu, unsigned offset)
+{
+	return readl(ipu->idmac_reg + offset);
+}
+
+static inline void ipu_idmac_write(struct ipu_soc *ipu, u32 value,
+				   unsigned offset)
+{
+	writel(value, ipu->idmac_reg + offset);
+}
+
 void ipu_srm_dp_sync_update(struct ipu_soc *ipu);
 
 int ipu_module_enable(struct ipu_soc *ipu, u32 mask);
@@ -191,6 +191,14 @@ int ipu_module_disable(struct ipu_soc *ipu, u32 mask);
 bool ipu_idmac_channel_busy(struct ipu_soc *ipu, unsigned int chno);
 int ipu_wait_interrupt(struct ipu_soc *ipu, int irq, int ms);
 
+int ipu_csi_init(struct ipu_soc *ipu, struct device *dev, int id,
+		 unsigned long base, u32 module, struct clk *clk_ipu);
+void ipu_csi_exit(struct ipu_soc *ipu, int id);
+
+int ipu_ic_init(struct ipu_soc *ipu, struct device *dev,
+		unsigned long base, unsigned long tpmem_base);
+void ipu_ic_exit(struct ipu_soc *ipu);
+
 int ipu_di_init(struct ipu_soc *ipu, struct device *dev, int id,
 		unsigned long base, u32 module, struct clk *ipu_clk);
 void ipu_di_exit(struct ipu_soc *ipu, int id);
diff --git a/drivers/gpu/ipu-v3/ipu-smfc.c b/drivers/gpu/ipu-v3/ipu-smfc.c
index e4f85ad286fc..4ef910991413 100644
--- a/drivers/gpu/ipu-v3/ipu-smfc.c
+++ b/drivers/gpu/ipu-v3/ipu-smfc.c
@@ -8,7 +8,6 @@
  * http://www.opensource.org/licenses/gpl-license.html
  * http://www.gnu.org/copyleft/gpl.html
  */
-#define DEBUG
 #include <linux/export.h>
 #include <linux/types.h>
 #include <linux/init.h>
@@ -21,9 +20,18 @@
 
 #include "ipu-prv.h"
 
+struct ipu_smfc {
+	struct ipu_smfc_priv *priv;
+	int chno;
+	bool inuse;
+};
+
 struct ipu_smfc_priv {
 	void __iomem *base;
 	spinlock_t lock;
+	struct ipu_soc *ipu;
+	struct ipu_smfc channel[4];
+	int use_count;
 };
 
 /*SMFC Registers */
@@ -31,63 +39,166 @@ struct ipu_smfc_priv {
 #define SMFC_WMC	0x0004
 #define SMFC_BS		0x0008
 
-int ipu_smfc_set_burstsize(struct ipu_soc *ipu, int channel, int burstsize)
+int ipu_smfc_set_burstsize(struct ipu_smfc *smfc, int burstsize)
 {
-	struct ipu_smfc_priv *smfc = ipu->smfc_priv;
+	struct ipu_smfc_priv *priv = smfc->priv;
 	unsigned long flags;
 	u32 val, shift;
 
-	spin_lock_irqsave(&smfc->lock, flags);
+	spin_lock_irqsave(&priv->lock, flags);
 
-	shift = channel * 4;
-	val = readl(smfc->base + SMFC_BS);
+	shift = smfc->chno * 4;
+	val = readl(priv->base + SMFC_BS);
 	val &= ~(0xf << shift);
 	val |= burstsize << shift;
-	writel(val, smfc->base + SMFC_BS);
+	writel(val, priv->base + SMFC_BS);
 
-	spin_unlock_irqrestore(&smfc->lock, flags);
+	spin_unlock_irqrestore(&priv->lock, flags);
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(ipu_smfc_set_burstsize);
 
-int ipu_smfc_map_channel(struct ipu_soc *ipu, int channel, int csi_id, int mipi_id)
+int ipu_smfc_map_channel(struct ipu_smfc *smfc, int csi_id, int mipi_id)
 {
-	struct ipu_smfc_priv *smfc = ipu->smfc_priv;
+	struct ipu_smfc_priv *priv = smfc->priv;
 	unsigned long flags;
 	u32 val, shift;
 
-	spin_lock_irqsave(&smfc->lock, flags);
+	spin_lock_irqsave(&priv->lock, flags);
 
-	shift = channel * 3;
-	val = readl(smfc->base + SMFC_MAP);
+	shift = smfc->chno * 3;
+	val = readl(priv->base + SMFC_MAP);
 	val &= ~(0x7 << shift);
 	val |= ((csi_id << 2) | mipi_id) << shift;
-	writel(val, smfc->base + SMFC_MAP);
+	writel(val, priv->base + SMFC_MAP);
 
-	spin_unlock_irqrestore(&smfc->lock, flags);
+	spin_unlock_irqrestore(&priv->lock, flags);
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(ipu_smfc_map_channel);
 
+int ipu_smfc_set_watermark(struct ipu_smfc *smfc, u32 set_level, u32 clr_level)
+{
+	struct ipu_smfc_priv *priv = smfc->priv;
+	unsigned long flags;
+	u32 val, shift;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	shift = smfc->chno * 6 + (smfc->chno > 1 ? 4 : 0);
+	val = readl(priv->base + SMFC_WMC);
+	val &= ~(0x3f << shift);
+	val |= ((clr_level << 3) | set_level) << shift;
+	writel(val, priv->base + SMFC_WMC);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_smfc_set_watermark);
+
+int ipu_smfc_enable(struct ipu_smfc *smfc)
+{
+	struct ipu_smfc_priv *priv = smfc->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (!priv->use_count)
+		ipu_module_enable(priv->ipu, IPU_CONF_SMFC_EN);
+
+	priv->use_count++;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_smfc_enable);
+
+int ipu_smfc_disable(struct ipu_smfc *smfc)
+{
+	struct ipu_smfc_priv *priv = smfc->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	priv->use_count--;
+
+	if (!priv->use_count)
+		ipu_module_disable(priv->ipu, IPU_CONF_SMFC_EN);
+
+	if (priv->use_count < 0)
+		priv->use_count = 0;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(ipu_smfc_disable);
+
+struct ipu_smfc *ipu_smfc_get(struct ipu_soc *ipu, unsigned int chno)
+{
+	struct ipu_smfc_priv *priv = ipu->smfc_priv;
+	struct ipu_smfc *smfc, *ret;
+	unsigned long flags;
+
+	if (chno >= 4)
+		return ERR_PTR(-EINVAL);
+
+	smfc = &priv->channel[chno];
+	ret = smfc;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (smfc->inuse) {
+		ret = ERR_PTR(-EBUSY);
+		goto unlock;
+	}
+
+	smfc->inuse = true;
+unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(ipu_smfc_get);
+
+void ipu_smfc_put(struct ipu_smfc *smfc)
+{
+	struct ipu_smfc_priv *priv = smfc->priv;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	smfc->inuse = false;
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+EXPORT_SYMBOL_GPL(ipu_smfc_put);
+
 int ipu_smfc_init(struct ipu_soc *ipu, struct device *dev,
 		  unsigned long base)
 {
-	struct ipu_smfc_priv *smfc;
+	struct ipu_smfc_priv *priv;
+	int i;
 
-	smfc = devm_kzalloc(dev, sizeof(*smfc), GFP_KERNEL);
-	if (!smfc)
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
 		return -ENOMEM;
 
-	ipu->smfc_priv = smfc;
-	spin_lock_init(&smfc->lock);
+	ipu->smfc_priv = priv;
+	spin_lock_init(&priv->lock);
+	priv->ipu = ipu;
 
-	smfc->base = devm_ioremap(dev, base, PAGE_SIZE);
-	if (!smfc->base)
+	priv->base = devm_ioremap(dev, base, PAGE_SIZE);
+	if (!priv->base)
 		return -ENOMEM;
 
-	pr_debug("%s: ioremap 0x%08lx -> %p\n", __func__, base, smfc->base);
+	for (i = 0; i < 4; i++) {
+		priv->channel[i].priv = priv;
+		priv->channel[i].chno = i;
+	}
+
+	pr_debug("%s: ioremap 0x%08lx -> %p\n", __func__, base, priv->base);
 
 	return 0;
 }
diff --git a/drivers/gpu/vga/vga_switcheroo.c b/drivers/gpu/vga/vga_switcheroo.c
index 6866448083b2..37ac7b5dbd06 100644
--- a/drivers/gpu/vga/vga_switcheroo.c
+++ b/drivers/gpu/vga/vga_switcheroo.c
@@ -660,6 +660,12 @@ int vga_switcheroo_init_domain_pm_ops(struct device *dev, struct dev_pm_domain *
 }
 EXPORT_SYMBOL(vga_switcheroo_init_domain_pm_ops);
 
+void vga_switcheroo_fini_domain_pm_ops(struct device *dev)
+{
+	dev->pm_domain = NULL;
+}
+EXPORT_SYMBOL(vga_switcheroo_fini_domain_pm_ops);
+
 static int vga_switcheroo_runtime_resume_hdmi_audio(struct device *dev)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
diff --git a/drivers/gpu/vga/vgaarb.c b/drivers/gpu/vga/vgaarb.c
index d2077f040f3e..7bcbf863656e 100644
--- a/drivers/gpu/vga/vgaarb.c
+++ b/drivers/gpu/vga/vgaarb.c
@@ -41,6 +41,7 @@
 #include <linux/poll.h>
 #include <linux/miscdevice.h>
 #include <linux/slab.h>
+#include <linux/screen_info.h>
 
 #include <linux/uaccess.h>
 
@@ -112,10 +113,8 @@ both:
 	return 1;
 }
 
-#ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
 /* this is only used a cookie - it should not be dereferenced */
 static struct pci_dev *vga_default;
-#endif
 
 static void vga_arb_device_card_gone(struct pci_dev *pdev);
 
@@ -131,7 +130,6 @@ static struct vga_device *vgadev_find(struct pci_dev *pdev)
 }
 
 /* Returns the default VGA device (vgacon's babe) */
-#ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
 struct pci_dev *vga_default_device(void)
 {
 	return vga_default;
@@ -147,7 +145,6 @@ void vga_set_default_device(struct pci_dev *pdev)
 	pci_dev_put(vga_default);
 	vga_default = pci_dev_get(pdev);
 }
-#endif
 
 static inline void vga_irq_set_state(struct vga_device *vgadev, bool state)
 {
@@ -403,7 +400,6 @@ int vga_get(struct pci_dev *pdev, unsigned int rsrc, int interruptible)
 		}
 		schedule();
 		remove_wait_queue(&vga_wait_queue, &wait);
-		set_current_state(TASK_RUNNING);
 	}
 	return rc;
 }
@@ -583,11 +579,12 @@ static bool vga_arbiter_add_pci_device(struct pci_dev *pdev)
 	/* Deal with VGA default device. Use first enabled one
 	 * by default if arch doesn't have it's own hook
 	 */
-#ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
 	if (vga_default == NULL &&
-	    ((vgadev->owns & VGA_RSRC_LEGACY_MASK) == VGA_RSRC_LEGACY_MASK))
+	    ((vgadev->owns & VGA_RSRC_LEGACY_MASK) == VGA_RSRC_LEGACY_MASK)) {
+		pr_info("vgaarb: setting as boot device: PCI:%s\n",
+			pci_name(pdev));
 		vga_set_default_device(pdev);
-#endif
+	}
 
 	vga_arbiter_check_bridge_sharing(vgadev);
 
@@ -621,10 +618,8 @@ static bool vga_arbiter_del_pci_device(struct pci_dev *pdev)
 		goto bail;
 	}
 
-#ifndef __ARCH_HAS_VGA_DEFAULT_DEVICE
 	if (vga_default == pdev)
 		vga_set_default_device(NULL);
-#endif
 
 	if (vgadev->decodes & (VGA_RSRC_LEGACY_IO | VGA_RSRC_LEGACY_MEM))
 		vga_decode_count--;
@@ -1320,6 +1315,38 @@ static int __init vga_arb_device_init(void)
 	pr_info("vgaarb: loaded\n");
 
 	list_for_each_entry(vgadev, &vga_list, list) {
+#if defined(CONFIG_X86) || defined(CONFIG_IA64)
+		/* Override I/O based detection done by vga_arbiter_add_pci_device()
+		 * as it may take the wrong device (e.g. on Apple system under EFI).
+		 *
+		 * Select the device owning the boot framebuffer if there is one.
+		 */
+		resource_size_t start, end;
+		int i;
+
+		/* Does firmware framebuffer belong to us? */
+		for (i = 0; i < DEVICE_COUNT_RESOURCE; i++) {
+			if (!(pci_resource_flags(vgadev->pdev, i) & IORESOURCE_MEM))
+				continue;
+
+			start = pci_resource_start(vgadev->pdev, i);
+			end  = pci_resource_end(vgadev->pdev, i);
+
+			if (!start || !end)
+				continue;
+
+			if (screen_info.lfb_base < start ||
+			    (screen_info.lfb_base + screen_info.lfb_size) >= end)
+				continue;
+			if (!vga_default_device())
+				pr_info("vgaarb: setting as boot device: PCI:%s\n",
+					pci_name(vgadev->pdev));
+			else if (vgadev->pdev != vga_default_device())
+				pr_info("vgaarb: overriding boot device: PCI:%s\n",
+					pci_name(vgadev->pdev));
+			vga_set_default_device(vgadev->pdev);
+		}
+#endif
 		if (vgadev->bridge_has_one_vga)
 			pr_info("vgaarb: bridge control possible %s\n", pci_name(vgadev->pdev));
 		else
diff --git a/drivers/hid/Kconfig b/drivers/hid/Kconfig
index c18d5d71062d..f42df4dd58d2 100644
--- a/drivers/hid/Kconfig
+++ b/drivers/hid/Kconfig
@@ -530,6 +530,17 @@ config PANTHERLORD_FF
 	  Say Y here if you have a PantherLord/GreenAsia based game controller
 	  or adapter and want to enable force feedback support for it.
 
+config HID_PENMOUNT
+	tristate "Penmount touch device"
+	depends on USB_HID
+	---help---
+	  This selects a driver for the PenMount 6000 touch controller.
+
+	  The driver works around a problem in the report descript allowing
+	  the userspace to touch events instead of mouse events.
+
+	  Say Y here if you have a Penmount based touch controller.
+
 config HID_PETALYNX
 	tristate "Petalynx Maxter remote control"
 	depends on HID
diff --git a/drivers/hid/Makefile b/drivers/hid/Makefile
index 4dbac7f8530c..e2850d8af9ca 100644
--- a/drivers/hid/Makefile
+++ b/drivers/hid/Makefile
@@ -71,6 +71,7 @@ obj-$(CONFIG_HID_NTRIG)		+= hid-ntrig.o
 obj-$(CONFIG_HID_ORTEK)		+= hid-ortek.o
 obj-$(CONFIG_HID_PRODIKEYS)	+= hid-prodikeys.o
 obj-$(CONFIG_HID_PANTHERLORD)	+= hid-pl.o
+obj-$(CONFIG_HID_PENMOUNT)	+= hid-penmount.o
 obj-$(CONFIG_HID_PETALYNX)	+= hid-petalynx.o
 obj-$(CONFIG_HID_PICOLCD)	+= hid-picolcd.o
 hid-picolcd-y			+= hid-picolcd_core.o
diff --git a/drivers/hid/hid-cherry.c b/drivers/hid/hid-cherry.c
index 1bdcccc54a1d..f745d2c1325e 100644
--- a/drivers/hid/hid-cherry.c
+++ b/drivers/hid/hid-cherry.c
@@ -28,7 +28,7 @@
 static __u8 *ch_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	if (*rsize >= 17 && rdesc[11] == 0x3c && rdesc[12] == 0x02) {
+	if (*rsize >= 18 && rdesc[11] == 0x3c && rdesc[12] == 0x02) {
 		hid_info(hdev, "fixing up Cherry Cymotion report descriptor\n");
 		rdesc[11] = rdesc[16] = 0xff;
 		rdesc[12] = rdesc[17] = 0x03;
diff --git a/drivers/hid/hid-core.c b/drivers/hid/hid-core.c
index 12b6e67d9de0..73bd9e2e42bc 100644
--- a/drivers/hid/hid-core.c
+++ b/drivers/hid/hid-core.c
@@ -52,7 +52,7 @@ EXPORT_SYMBOL_GPL(hid_debug);
 
 static int hid_ignore_special_drivers = 0;
 module_param_named(ignore_special_drivers, hid_ignore_special_drivers, int, 0600);
-MODULE_PARM_DESC(debug, "Ignore any special drivers and handle all devices by generic driver");
+MODULE_PARM_DESC(ignore_special_drivers, "Ignore any special drivers and handle all devices by generic driver");
 
 /*
  * Register a new report for a device.
@@ -1591,6 +1591,9 @@ int hid_connect(struct hid_device *hdev, unsigned int connect_mask)
 	if ((connect_mask & HID_CONNECT_HIDRAW) && !hidraw_connect(hdev))
 		hdev->claimed |= HID_CLAIMED_HIDRAW;
 
+	if (connect_mask & HID_CONNECT_DRIVER)
+		hdev->claimed |= HID_CLAIMED_DRIVER;
+
 	/* Drivers with the ->raw_event callback set are not required to connect
 	 * to any other listener. */
 	if (!hdev->claimed && !hdev->driver->raw_event) {
@@ -1793,6 +1796,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A070) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A072) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A081) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT, USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A0C2) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HUION, USB_DEVICE_ID_HUION_TABLET) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_JESS2, USB_DEVICE_ID_JESS2_COLOR_RUMBLE_PAD) },
 	{ HID_BLUETOOTH_DEVICE(USB_VENDOR_ID_ION, USB_DEVICE_ID_ICADE) },
@@ -1880,6 +1884,7 @@ static const struct hid_device_id hid_have_special_driver[] = {
 	{ HID_USB_DEVICE(USB_VENDOR_ID_NTRIG, USB_DEVICE_ID_NTRIG_TOUCH_SCREEN_18) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_PKB1700) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_ORTEK, USB_DEVICE_ID_ORTEK_WKB2000) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_6000) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PETALYNX, USB_DEVICE_ID_PETALYNX_MAXTER_REMOTE) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_PRIMAX, USB_DEVICE_ID_PRIMAX_KEYBOARD) },
 #if IS_ENABLED(CONFIG_HID_ROCCAT)
diff --git a/drivers/hid/hid-cp2112.c b/drivers/hid/hid-cp2112.c
index a822db5a8338..3318de690e00 100644
--- a/drivers/hid/hid-cp2112.c
+++ b/drivers/hid/hid-cp2112.c
@@ -1069,8 +1069,7 @@ static int cp2112_probe(struct hid_device *hdev, const struct hid_device_id *id)
 	return ret;
 
 err_gpiochip_remove:
-	if (gpiochip_remove(&dev->gc) < 0)
-		hid_err(hdev, "error removing gpio chip\n");
+	gpiochip_remove(&dev->gc);
 err_free_i2c:
 	i2c_del_adapter(&dev->adap);
 err_free_dev:
@@ -1089,8 +1088,7 @@ static void cp2112_remove(struct hid_device *hdev)
 	struct cp2112_device *dev = hid_get_drvdata(hdev);
 
 	sysfs_remove_group(&hdev->dev.kobj, &cp2112_attr_group);
-	if (gpiochip_remove(&dev->gc))
-		hid_err(hdev, "unable to remove gpio chip\n");
+	gpiochip_remove(&dev->gc);
 	i2c_del_adapter(&dev->adap);
 	/* i2c_del_adapter has finished removing all i2c devices from our
 	 * adapter. Well behaved devices should no longer call our cp2112_xfer
diff --git a/drivers/hid/hid-holtek-mouse.c b/drivers/hid/hid-holtek-mouse.c
index d60fbd0adc0c..78b3a0c76775 100644
--- a/drivers/hid/hid-holtek-mouse.c
+++ b/drivers/hid/hid-holtek-mouse.c
@@ -29,6 +29,7 @@
  *   and Zalman ZM-GM1
  * - USB ID 04d9:a081, sold as SHARKOON DarkGlider Gaming mouse
  * - USB ID 04d9:a072, sold as LEETGION Hellion Gaming Mouse
+ * - USB ID 04d9:a0c2, sold as ETEKCITY Scroll T-140 Gaming Mouse
  */
 
 static __u8 *holtek_mouse_report_fixup(struct hid_device *hdev, __u8 *rdesc,
@@ -42,6 +43,7 @@ static __u8 *holtek_mouse_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		switch (hdev->product) {
 		case USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A067:
 		case USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A072:
+		case USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A0C2:
 			if (*rsize >= 122 && rdesc[115] == 0xff && rdesc[116] == 0x7f
 					&& rdesc[120] == 0xff && rdesc[121] == 0x7f) {
 				hid_info(hdev, "Fixing up report descriptor\n");
@@ -74,6 +76,8 @@ static const struct hid_device_id holtek_mouse_devices[] = {
 			USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A072) },
 	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT,
 			USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A081) },
+	{ HID_USB_DEVICE(USB_VENDOR_ID_HOLTEK_ALT,
+			USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A0C2) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, holtek_mouse_devices);
diff --git a/drivers/hid/hid-huion.c b/drivers/hid/hid-huion.c
index 60f44cd1b0ed..61b68ca27790 100644
--- a/drivers/hid/hid-huion.c
+++ b/drivers/hid/hid-huion.c
@@ -84,6 +84,15 @@ static const __u8 huion_tablet_rdesc_template[] = {
 	0xC0                    /*  End Collection                          */
 };
 
+/* Parameter indices */
+enum huion_prm {
+	HUION_PRM_X_LM		= 1,
+	HUION_PRM_Y_LM		= 2,
+	HUION_PRM_PRESSURE_LM	= 4,
+	HUION_PRM_RESOLUTION	= 5,
+	HUION_PRM_NUM
+};
+
 /* Driver data */
 struct huion_drvdata {
 	__u8 *rdesc;
@@ -115,7 +124,12 @@ static int huion_tablet_enable(struct hid_device *hdev)
 	int rc;
 	struct usb_device *usb_dev = hid_to_usb_dev(hdev);
 	struct huion_drvdata *drvdata = hid_get_drvdata(hdev);
-	__le16 buf[6];
+	__le16 *buf = NULL;
+	size_t len;
+	s32 params[HUION_PH_ID_NUM];
+	s32 resolution;
+	__u8 *p;
+	s32 v;
 
 	/*
 	 * Read string descriptor containing tablet parameters. The specific
@@ -123,65 +137,79 @@ static int huion_tablet_enable(struct hid_device *hdev)
 	 * driver traffic.
 	 * NOTE: This enables fully-functional tablet mode.
 	 */
+	len = HUION_PRM_NUM * sizeof(*buf);
+	buf = kmalloc(len, GFP_KERNEL);
+	if (buf == NULL) {
+		hid_err(hdev, "failed to allocate parameter buffer\n");
+		rc = -ENOMEM;
+		goto cleanup;
+	}
 	rc = usb_control_msg(usb_dev, usb_rcvctrlpipe(usb_dev, 0),
 				USB_REQ_GET_DESCRIPTOR, USB_DIR_IN,
 				(USB_DT_STRING << 8) + 0x64,
-				0x0409, buf, sizeof(buf),
+				0x0409, buf, len,
 				USB_CTRL_GET_TIMEOUT);
-	if (rc == -EPIPE)
-		hid_warn(hdev, "device parameters not found\n");
-	else if (rc < 0)
-		hid_warn(hdev, "failed to get device parameters: %d\n", rc);
-	else if (rc != sizeof(buf))
-		hid_warn(hdev, "invalid device parameters\n");
-	else {
-		s32 params[HUION_PH_ID_NUM];
-		s32 resolution;
-		__u8 *p;
-		s32 v;
+	if (rc == -EPIPE) {
+		hid_err(hdev, "device parameters not found\n");
+		rc = -ENODEV;
+		goto cleanup;
+	} else if (rc < 0) {
+		hid_err(hdev, "failed to get device parameters: %d\n", rc);
+		rc = -ENODEV;
+		goto cleanup;
+	} else if (rc != len) {
+		hid_err(hdev, "invalid device parameters\n");
+		rc = -ENODEV;
+		goto cleanup;
+	}
 
-		/* Extract device parameters */
-		params[HUION_PH_ID_X_LM] = le16_to_cpu(buf[1]);
-		params[HUION_PH_ID_Y_LM] = le16_to_cpu(buf[2]);
-		params[HUION_PH_ID_PRESSURE_LM] = le16_to_cpu(buf[4]);
-		resolution = le16_to_cpu(buf[5]);
-		if (resolution == 0) {
-			params[HUION_PH_ID_X_PM] = 0;
-			params[HUION_PH_ID_Y_PM] = 0;
-		} else {
-			params[HUION_PH_ID_X_PM] = params[HUION_PH_ID_X_LM] *
-							1000 / resolution;
-			params[HUION_PH_ID_Y_PM] = params[HUION_PH_ID_Y_LM] *
-							1000 / resolution;
-		}
+	/* Extract device parameters */
+	params[HUION_PH_ID_X_LM] = le16_to_cpu(buf[HUION_PRM_X_LM]);
+	params[HUION_PH_ID_Y_LM] = le16_to_cpu(buf[HUION_PRM_Y_LM]);
+	params[HUION_PH_ID_PRESSURE_LM] =
+		le16_to_cpu(buf[HUION_PRM_PRESSURE_LM]);
+	resolution = le16_to_cpu(buf[HUION_PRM_RESOLUTION]);
+	if (resolution == 0) {
+		params[HUION_PH_ID_X_PM] = 0;
+		params[HUION_PH_ID_Y_PM] = 0;
+	} else {
+		params[HUION_PH_ID_X_PM] = params[HUION_PH_ID_X_LM] *
+						1000 / resolution;
+		params[HUION_PH_ID_Y_PM] = params[HUION_PH_ID_Y_LM] *
+						1000 / resolution;
+	}
 
-		/* Allocate fixed report descriptor */
-		drvdata->rdesc = devm_kmalloc(&hdev->dev,
-					sizeof(huion_tablet_rdesc_template),
-					GFP_KERNEL);
-		if (drvdata->rdesc == NULL) {
-			hid_err(hdev, "failed to allocate fixed rdesc\n");
-			return -ENOMEM;
-		}
-		drvdata->rsize = sizeof(huion_tablet_rdesc_template);
+	/* Allocate fixed report descriptor */
+	drvdata->rdesc = devm_kmalloc(&hdev->dev,
+				sizeof(huion_tablet_rdesc_template),
+				GFP_KERNEL);
+	if (drvdata->rdesc == NULL) {
+		hid_err(hdev, "failed to allocate fixed rdesc\n");
+		rc = -ENOMEM;
+		goto cleanup;
+	}
+	drvdata->rsize = sizeof(huion_tablet_rdesc_template);
 
-		/* Format fixed report descriptor */
-		memcpy(drvdata->rdesc, huion_tablet_rdesc_template,
-			drvdata->rsize);
-		for (p = drvdata->rdesc;
-		     p <= drvdata->rdesc + drvdata->rsize - 4;) {
-			if (p[0] == 0xFE && p[1] == 0xED && p[2] == 0x1D &&
-			    p[3] < sizeof(params)) {
-				v = params[p[3]];
-				put_unaligned(cpu_to_le32(v), (s32 *)p);
-				p += 4;
-			} else {
-				p++;
-			}
+	/* Format fixed report descriptor */
+	memcpy(drvdata->rdesc, huion_tablet_rdesc_template,
+		drvdata->rsize);
+	for (p = drvdata->rdesc;
+	     p <= drvdata->rdesc + drvdata->rsize - 4;) {
+		if (p[0] == 0xFE && p[1] == 0xED && p[2] == 0x1D &&
+		    p[3] < sizeof(params)) {
+			v = params[p[3]];
+			put_unaligned(cpu_to_le32(v), (s32 *)p);
+			p += 4;
+		} else {
+			p++;
 		}
 	}
 
-	return 0;
+	rc = 0;
+
+cleanup:
+	kfree(buf);
+	return rc;
 }
 
 static int huion_probe(struct hid_device *hdev, const struct hid_device_id *id)
diff --git a/drivers/hid/hid-ids.h b/drivers/hid/hid-ids.h
index 25cd674d6064..cd9c9e96cf0e 100644
--- a/drivers/hid/hid-ids.h
+++ b/drivers/hid/hid-ids.h
@@ -296,6 +296,9 @@
 #define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_73F7	0x73f7
 #define USB_DEVICE_ID_DWAV_EGALAX_MULTITOUCH_A001	0xa001
 
+#define USB_VENDOR_ID_ELAN		0x04f3
+#define USB_DEVICE_ID_ELAN_TOUCHSCREEN	0x0089
+
 #define USB_VENDOR_ID_ELECOM		0x056e
 #define USB_DEVICE_ID_ELECOM_BM084	0x0061
 
@@ -479,6 +482,7 @@
 #define USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A070	0xa070
 #define USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A072	0xa072
 #define USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A081	0xa081
+#define USB_DEVICE_ID_HOLTEK_ALT_MOUSE_A0C2	0xa0c2
 #define USB_DEVICE_ID_HOLTEK_ALT_KEYBOARD_A096	0xa096
 
 #define USB_VENDOR_ID_IMATION		0x0718
@@ -722,6 +726,7 @@
 #define USB_DEVICE_ID_PENMOUNT_PCI	0x3500
 #define USB_DEVICE_ID_PENMOUNT_1610	0x1610
 #define USB_DEVICE_ID_PENMOUNT_1640	0x1640
+#define USB_DEVICE_ID_PENMOUNT_6000	0x6000
 
 #define USB_VENDOR_ID_PETALYNX		0x18b1
 #define USB_DEVICE_ID_PETALYNX_MAXTER_REMOTE	0x0037
@@ -733,6 +738,8 @@
 #define USB_DEVICE_ID_PI_ENGINEERING_VEC_USB_FOOTPEDAL	0xff
 
 #define USB_VENDOR_ID_PIXART				0x093a
+#define USB_DEVICE_ID_PIXART_USB_OPTICAL_MOUSE_ID2	0x0137
+#define USB_DEVICE_ID_PIXART_USB_OPTICAL_MOUSE		0x2510
 #define USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN	0x8001
 #define USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1	0x8002
 #define USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2	0x8003
diff --git a/drivers/hid/hid-input.c b/drivers/hid/hid-input.c
index 2619f7f4517a..2df7fddbd119 100644
--- a/drivers/hid/hid-input.c
+++ b/drivers/hid/hid-input.c
@@ -599,6 +599,12 @@ static void hidinput_configure_usage(struct hid_input *hidinput, struct hid_fiel
 		/* These usage IDs map directly to the usage codes. */
 		case HID_GD_X: case HID_GD_Y: case HID_GD_Z:
 		case HID_GD_RX: case HID_GD_RY: case HID_GD_RZ:
+			if (field->flags & HID_MAIN_ITEM_RELATIVE)
+				map_rel(usage->hid & 0xf);
+			else
+				map_abs_clear(usage->hid & 0xf);
+			break;
+
 		case HID_GD_SLIDER: case HID_GD_DIAL: case HID_GD_WHEEL:
 			if (field->flags & HID_MAIN_ITEM_RELATIVE)
 				map_rel(usage->hid & 0xf);
diff --git a/drivers/hid/hid-kye.c b/drivers/hid/hid-kye.c
index e77696367591..b92bf01a1ae8 100644
--- a/drivers/hid/hid-kye.c
+++ b/drivers/hid/hid-kye.c
@@ -300,7 +300,7 @@ static __u8 *kye_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		 *   - change the button usage range to 4-7 for the extra
 		 *     buttons
 		 */
-		if (*rsize >= 74 &&
+		if (*rsize >= 75 &&
 			rdesc[61] == 0x05 && rdesc[62] == 0x08 &&
 			rdesc[63] == 0x19 && rdesc[64] == 0x08 &&
 			rdesc[65] == 0x29 && rdesc[66] == 0x0f &&
diff --git a/drivers/hid/hid-lg.c b/drivers/hid/hid-lg.c
index a976f48263f6..f91ff145db9a 100644
--- a/drivers/hid/hid-lg.c
+++ b/drivers/hid/hid-lg.c
@@ -345,14 +345,14 @@ static __u8 *lg_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 	struct usb_device_descriptor *udesc;
 	__u16 bcdDevice, rev_maj, rev_min;
 
-	if ((drv_data->quirks & LG_RDESC) && *rsize >= 90 && rdesc[83] == 0x26 &&
+	if ((drv_data->quirks & LG_RDESC) && *rsize >= 91 && rdesc[83] == 0x26 &&
 			rdesc[84] == 0x8c && rdesc[85] == 0x02) {
 		hid_info(hdev,
 			 "fixing up Logitech keyboard report descriptor\n");
 		rdesc[84] = rdesc[89] = 0x4d;
 		rdesc[85] = rdesc[90] = 0x10;
 	}
-	if ((drv_data->quirks & LG_RDESC_REL_ABS) && *rsize >= 50 &&
+	if ((drv_data->quirks & LG_RDESC_REL_ABS) && *rsize >= 51 &&
 			rdesc[32] == 0x81 && rdesc[33] == 0x06 &&
 			rdesc[49] == 0x81 && rdesc[50] == 0x06) {
 		hid_info(hdev,
diff --git a/drivers/hid/hid-lg4ff.c b/drivers/hid/hid-lg4ff.c
index cc2bd2022198..7835717bc020 100644
--- a/drivers/hid/hid-lg4ff.c
+++ b/drivers/hid/hid-lg4ff.c
@@ -451,13 +451,13 @@ static ssize_t lg4ff_range_store(struct device *dev, struct device_attribute *at
 	drv_data = hid_get_drvdata(hid);
 	if (!drv_data) {
 		hid_err(hid, "Private driver data not found!\n");
-		return 0;
+		return -EINVAL;
 	}
 
 	entry = drv_data->device_props;
 	if (!entry) {
 		hid_err(hid, "Device properties not found!\n");
-		return 0;
+		return -EINVAL;
 	}
 
 	if (range == 0)
diff --git a/drivers/hid/hid-logitech-dj.c b/drivers/hid/hid-logitech-dj.c
index 486dbde2ba2d..71f569292cab 100644
--- a/drivers/hid/hid-logitech-dj.c
+++ b/drivers/hid/hid-logitech-dj.c
@@ -238,13 +238,6 @@ static void logi_dj_recv_add_djhid_device(struct dj_receiver_dev *djrcv_dev,
 		return;
 	}
 
-	if ((dj_report->device_index < DJ_DEVICE_INDEX_MIN) ||
-	    (dj_report->device_index > DJ_DEVICE_INDEX_MAX)) {
-		dev_err(&djrcv_hdev->dev, "%s: invalid device index:%d\n",
-			__func__, dj_report->device_index);
-		return;
-	}
-
 	if (djrcv_dev->paired_dj_devices[dj_report->device_index]) {
 		/* The device is already known. No need to reallocate it. */
 		dbg_hid("%s: device is already known\n", __func__);
@@ -392,18 +385,6 @@ static void logi_dj_recv_forward_null_report(struct dj_receiver_dev *djrcv_dev,
 
 	djdev = djrcv_dev->paired_dj_devices[dj_report->device_index];
 
-	if (!djdev) {
-		dbg_hid("djrcv_dev->paired_dj_devices[dj_report->device_index]"
-			" is NULL, index %d\n", dj_report->device_index);
-		kfifo_in(&djrcv_dev->notif_fifo, dj_report, sizeof(struct dj_report));
-
-		if (schedule_work(&djrcv_dev->work) == 0) {
-			dbg_hid("%s: did not schedule the work item, was already "
-			"queued\n", __func__);
-		}
-		return;
-	}
-
 	memset(reportbuffer, 0, sizeof(reportbuffer));
 
 	for (i = 0; i < NUMBER_OF_HID_REPORTS; i++) {
@@ -428,18 +409,6 @@ static void logi_dj_recv_forward_report(struct dj_receiver_dev *djrcv_dev,
 
 	dj_device = djrcv_dev->paired_dj_devices[dj_report->device_index];
 
-	if (dj_device == NULL) {
-		dbg_hid("djrcv_dev->paired_dj_devices[dj_report->device_index]"
-			" is NULL, index %d\n", dj_report->device_index);
-		kfifo_in(&djrcv_dev->notif_fifo, dj_report, sizeof(struct dj_report));
-
-		if (schedule_work(&djrcv_dev->work) == 0) {
-			dbg_hid("%s: did not schedule the work item, was already "
-			"queued\n", __func__);
-		}
-		return;
-	}
-
 	if ((dj_report->report_type > ARRAY_SIZE(hid_reportid_size_map) - 1) ||
 	    (hid_reportid_size_map[dj_report->report_type] == 0)) {
 		dbg_hid("invalid report type:%x\n", dj_report->report_type);
@@ -557,7 +526,7 @@ static int logi_dj_ll_raw_request(struct hid_device *hid,
 	if (!out_buf)
 		return -ENOMEM;
 
-	if (count < DJREPORT_SHORT_LENGTH - 2)
+	if (count > DJREPORT_SHORT_LENGTH - 2)
 		count = DJREPORT_SHORT_LENGTH - 2;
 
 	out_buf[0] = REPORT_ID_DJ_SHORT;
@@ -663,7 +632,6 @@ static int logi_dj_raw_event(struct hid_device *hdev,
 	struct dj_receiver_dev *djrcv_dev = hid_get_drvdata(hdev);
 	struct dj_report *dj_report = (struct dj_report *) data;
 	unsigned long flags;
-	bool report_processed = false;
 
 	dbg_hid("%s, size:%d\n", __func__, size);
 
@@ -691,27 +659,52 @@ static int logi_dj_raw_event(struct hid_device *hdev,
 	 * anything else with it.
 	 */
 
+	/* case 1) */
+	if (data[0] != REPORT_ID_DJ_SHORT)
+		return false;
+
+	if ((dj_report->device_index < DJ_DEVICE_INDEX_MIN) ||
+	    (dj_report->device_index > DJ_DEVICE_INDEX_MAX)) {
+		/*
+		 * Device index is wrong, bail out.
+		 * This driver can ignore safely the receiver notifications,
+		 * so ignore those reports too.
+		 */
+		if (dj_report->device_index != DJ_RECEIVER_INDEX)
+			dev_err(&hdev->dev, "%s: invalid device index:%d\n",
+				__func__, dj_report->device_index);
+		return false;
+	}
+
 	spin_lock_irqsave(&djrcv_dev->lock, flags);
-	if (dj_report->report_id == REPORT_ID_DJ_SHORT) {
-		switch (dj_report->report_type) {
-		case REPORT_TYPE_NOTIF_DEVICE_PAIRED:
-		case REPORT_TYPE_NOTIF_DEVICE_UNPAIRED:
-			logi_dj_recv_queue_notification(djrcv_dev, dj_report);
-			break;
-		case REPORT_TYPE_NOTIF_CONNECTION_STATUS:
-			if (dj_report->report_params[CONNECTION_STATUS_PARAM_STATUS] ==
-			    STATUS_LINKLOSS) {
-				logi_dj_recv_forward_null_report(djrcv_dev, dj_report);
-			}
-			break;
-		default:
-			logi_dj_recv_forward_report(djrcv_dev, dj_report);
+
+	if (!djrcv_dev->paired_dj_devices[dj_report->device_index]) {
+		/* received an event for an unknown device, bail out */
+		logi_dj_recv_queue_notification(djrcv_dev, dj_report);
+		goto out;
+	}
+
+	switch (dj_report->report_type) {
+	case REPORT_TYPE_NOTIF_DEVICE_PAIRED:
+		/* pairing notifications are handled above the switch */
+		break;
+	case REPORT_TYPE_NOTIF_DEVICE_UNPAIRED:
+		logi_dj_recv_queue_notification(djrcv_dev, dj_report);
+		break;
+	case REPORT_TYPE_NOTIF_CONNECTION_STATUS:
+		if (dj_report->report_params[CONNECTION_STATUS_PARAM_STATUS] ==
+		    STATUS_LINKLOSS) {
+			logi_dj_recv_forward_null_report(djrcv_dev, dj_report);
 		}
-		report_processed = true;
+		break;
+	default:
+		logi_dj_recv_forward_report(djrcv_dev, dj_report);
 	}
+
+out:
 	spin_unlock_irqrestore(&djrcv_dev->lock, flags);
 
-	return report_processed;
+	return true;
 }
 
 static int logi_dj_probe(struct hid_device *hdev,
diff --git a/drivers/hid/hid-logitech-dj.h b/drivers/hid/hid-logitech-dj.h
index 4a4000340ce1..daeb0aa4bee9 100644
--- a/drivers/hid/hid-logitech-dj.h
+++ b/drivers/hid/hid-logitech-dj.h
@@ -27,6 +27,7 @@
 
 #define DJ_MAX_PAIRED_DEVICES			6
 #define DJ_MAX_NUMBER_NOTIFICATIONS		8
+#define DJ_RECEIVER_INDEX			0
 #define DJ_DEVICE_INDEX_MIN 			1
 #define DJ_DEVICE_INDEX_MAX 			6
 
diff --git a/drivers/hid/hid-magicmouse.c b/drivers/hid/hid-magicmouse.c
index ecc2cbf300cc..29a74c1efcb8 100644
--- a/drivers/hid/hid-magicmouse.c
+++ b/drivers/hid/hid-magicmouse.c
@@ -290,6 +290,11 @@ static int magicmouse_raw_event(struct hid_device *hdev,
 		if (size < 4 || ((size - 4) % 9) != 0)
 			return 0;
 		npoints = (size - 4) / 9;
+		if (npoints > 15) {
+			hid_warn(hdev, "invalid size value (%d) for TRACKPAD_REPORT_ID\n",
+					size);
+			return 0;
+		}
 		msc->ntouches = 0;
 		for (ii = 0; ii < npoints; ii++)
 			magicmouse_emit_touch(msc, ii, data + ii * 9 + 4);
@@ -307,6 +312,11 @@ static int magicmouse_raw_event(struct hid_device *hdev,
 		if (size < 6 || ((size - 6) % 8) != 0)
 			return 0;
 		npoints = (size - 6) / 8;
+		if (npoints > 15) {
+			hid_warn(hdev, "invalid size value (%d) for MOUSE_REPORT_ID\n",
+					size);
+			return 0;
+		}
 		msc->ntouches = 0;
 		for (ii = 0; ii < npoints; ii++)
 			magicmouse_emit_touch(msc, ii, data + ii * 8 + 6);
diff --git a/drivers/hid/hid-monterey.c b/drivers/hid/hid-monterey.c
index 9e14c00eb1b6..25daf28b26bd 100644
--- a/drivers/hid/hid-monterey.c
+++ b/drivers/hid/hid-monterey.c
@@ -24,7 +24,7 @@
 static __u8 *mr_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	if (*rsize >= 30 && rdesc[29] == 0x05 && rdesc[30] == 0x09) {
+	if (*rsize >= 31 && rdesc[29] == 0x05 && rdesc[30] == 0x09) {
 		hid_info(hdev, "fixing up button/consumer in HID report descriptor\n");
 		rdesc[30] = 0x0c;
 	}
diff --git a/drivers/hid/hid-penmount.c b/drivers/hid/hid-penmount.c
new file mode 100644
index 000000000000..c11dce85cd18
--- /dev/null
+++ b/drivers/hid/hid-penmount.c
@@ -0,0 +1,49 @@
+/*
+ *  HID driver for PenMount touchscreens
+ *
+ *  Copyright (c) 2014 Christian Gmeiner <christian.gmeiner <at> gmail.com>
+ *
+ *  based on hid-penmount copyrighted by
+ *    PenMount Touch Solutions <penmount <at> seed.net.tw>
+ */
+
+/*
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the Free
+ * Software Foundation; either version 2 of the License, or (at your option)
+ * any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/hid.h>
+#include "hid-ids.h"
+
+static int penmount_input_mapping(struct hid_device *hdev,
+		struct hid_input *hi, struct hid_field *field,
+		struct hid_usage *usage, unsigned long **bit, int *max)
+{
+	if ((usage->hid & HID_USAGE_PAGE) == HID_UP_BUTTON) {
+		hid_map_usage(hi, usage, bit, max, EV_KEY, BTN_TOUCH);
+		return 1;
+	}
+
+	return 0;
+}
+
+static const struct hid_device_id penmount_devices[] = {
+	{ HID_USB_DEVICE(USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_6000) },
+	{ }
+};
+MODULE_DEVICE_TABLE(hid, penmount_devices);
+
+static struct hid_driver penmount_driver = {
+	.name = "hid-penmount",
+	.id_table = penmount_devices,
+	.input_mapping = penmount_input_mapping,
+};
+
+module_hid_driver(penmount_driver);
+
+MODULE_AUTHOR("Christian Gmeiner <christian.gmeiner@gmail.com>");
+MODULE_DESCRIPTION("PenMount HID TouchScreen driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/hid/hid-petalynx.c b/drivers/hid/hid-petalynx.c
index 736b2502df4f..6aca4f2554bf 100644
--- a/drivers/hid/hid-petalynx.c
+++ b/drivers/hid/hid-petalynx.c
@@ -25,7 +25,7 @@
 static __u8 *pl_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	if (*rsize >= 60 && rdesc[39] == 0x2a && rdesc[40] == 0xf5 &&
+	if (*rsize >= 62 && rdesc[39] == 0x2a && rdesc[40] == 0xf5 &&
 			rdesc[41] == 0x00 && rdesc[59] == 0x26 &&
 			rdesc[60] == 0xf9 && rdesc[61] == 0x00) {
 		hid_info(hdev, "fixing up Petalynx Maxter Remote report descriptor\n");
diff --git a/drivers/hid/hid-picolcd_core.c b/drivers/hid/hid-picolcd_core.c
index acbb021065ec..c1b29a9eb41a 100644
--- a/drivers/hid/hid-picolcd_core.c
+++ b/drivers/hid/hid-picolcd_core.c
@@ -350,6 +350,12 @@ static int picolcd_raw_event(struct hid_device *hdev,
 	if (!data)
 		return 1;
 
+	if (size > 64) {
+		hid_warn(hdev, "invalid size value (%d) for picolcd raw event (%d)\n",
+				size, report->id);
+		return 0;
+	}
+
 	if (report->id == REPORT_KEY_STATE) {
 		if (data->input_keys)
 			ret = picolcd_raw_keypad(data, report, raw_data+1, size-1);
diff --git a/drivers/hid/hid-rmi.c b/drivers/hid/hid-rmi.c
index 0dc25142f451..3cccff73b9b9 100644
--- a/drivers/hid/hid-rmi.c
+++ b/drivers/hid/hid-rmi.c
@@ -320,10 +320,7 @@ static int rmi_f11_input_event(struct hid_device *hdev, u8 irq, u8 *data,
 	int offset;
 	int i;
 
-	if (size < hdata->f11.report_size)
-		return 0;
-
-	if (!(irq & hdata->f11.irq_mask))
+	if (!(irq & hdata->f11.irq_mask) || size <= 0)
 		return 0;
 
 	offset = (hdata->max_fingers >> 2) + 1;
@@ -332,9 +329,19 @@ static int rmi_f11_input_event(struct hid_device *hdev, u8 irq, u8 *data,
 		int fs_bit_position = (i & 0x3) << 1;
 		int finger_state = (data[fs_byte_position] >> fs_bit_position) &
 					0x03;
+		int position = offset + 5 * i;
+
+		if (position + 5 > size) {
+			/* partial report, go on with what we received */
+			printk_once(KERN_WARNING
+				"%s %s: Detected incomplete finger report. Finger reports may occasionally get dropped on this platform.\n",
+				 dev_driver_string(&hdev->dev),
+				 dev_name(&hdev->dev));
+			hid_dbg(hdev, "Incomplete finger report\n");
+			break;
+		}
 
-		rmi_f11_process_touch(hdata, i, finger_state,
-				&data[offset + 5 * i]);
+		rmi_f11_process_touch(hdata, i, finger_state, &data[position]);
 	}
 	input_mt_sync_frame(hdata->input);
 	input_sync(hdata->input);
@@ -352,6 +359,11 @@ static int rmi_f30_input_event(struct hid_device *hdev, u8 irq, u8 *data,
 	if (!(irq & hdata->f30.irq_mask))
 		return 0;
 
+	if (size < (int)hdata->f30.report_size) {
+		hid_warn(hdev, "Click Button pressed, but the click data is missing\n");
+		return 0;
+	}
+
 	for (i = 0; i < hdata->gpio_led_count; i++) {
 		if (test_bit(i, &hdata->button_mask)) {
 			value = (data[i / 8] >> (i & 0x07)) & BIT(0);
@@ -412,9 +424,29 @@ static int rmi_read_data_event(struct hid_device *hdev, u8 *data, int size)
 	return 1;
 }
 
+static int rmi_check_sanity(struct hid_device *hdev, u8 *data, int size)
+{
+	int valid_size = size;
+	/*
+	 * On the Dell XPS 13 9333, the bus sometimes get confused and fills
+	 * the report with a sentinel value "ff". Synaptics told us that such
+	 * behavior does not comes from the touchpad itself, so we filter out
+	 * such reports here.
+	 */
+
+	while ((data[valid_size - 1] == 0xff) && valid_size > 0)
+		valid_size--;
+
+	return valid_size;
+}
+
 static int rmi_raw_event(struct hid_device *hdev,
 		struct hid_report *report, u8 *data, int size)
 {
+	size = rmi_check_sanity(hdev, data, size);
+	if (size < 2)
+		return 0;
+
 	switch (data[0]) {
 	case RMI_READ_DATA_REPORT_ID:
 		return rmi_read_data_event(hdev, data, size);
@@ -909,10 +941,15 @@ static int rmi_probe(struct hid_device *hdev, const struct hid_device_id *id)
 		return ret;
 	}
 
-	if (!test_bit(RMI_STARTED, &data->flags)) {
-		hid_hw_stop(hdev);
-		return -EIO;
-	}
+	if (!test_bit(RMI_STARTED, &data->flags))
+		/*
+		 * The device maybe in the bootloader if rmi_input_configured
+		 * failed to find F11 in the PDT. Print an error, but don't
+		 * return an error from rmi_probe so that hidraw will be
+		 * accessible from userspace. That way a userspace tool
+		 * can be used to reload working firmware on the touchpad.
+		 */
+		hid_err(hdev, "Device failed to be properly configured\n");
 
 	return 0;
 }
diff --git a/drivers/hid/hid-sensor-hub.c b/drivers/hid/hid-sensor-hub.c
index e244e449cbba..e6d8e18dae97 100644
--- a/drivers/hid/hid-sensor-hub.c
+++ b/drivers/hid/hid-sensor-hub.c
@@ -604,9 +604,9 @@ static int sensor_hub_probe(struct hid_device *hdev,
 		ret = -EINVAL;
 		goto err_stop_hw;
 	}
-	sd->hid_sensor_hub_client_devs = kzalloc(dev_cnt *
-						sizeof(struct mfd_cell),
-						GFP_KERNEL);
+	sd->hid_sensor_hub_client_devs = devm_kzalloc(&hdev->dev, dev_cnt *
+						      sizeof(struct mfd_cell),
+						      GFP_KERNEL);
 	if (sd->hid_sensor_hub_client_devs == NULL) {
 		hid_err(hdev, "Failed to allocate memory for mfd cells\n");
 			ret = -ENOMEM;
@@ -618,11 +618,12 @@ static int sensor_hub_probe(struct hid_device *hdev,
 
 		if (collection->type == HID_COLLECTION_PHYSICAL) {
 
-			hsdev = kzalloc(sizeof(*hsdev), GFP_KERNEL);
+			hsdev = devm_kzalloc(&hdev->dev, sizeof(*hsdev),
+					     GFP_KERNEL);
 			if (!hsdev) {
 				hid_err(hdev, "cannot allocate hid_sensor_hub_device\n");
 				ret = -ENOMEM;
-				goto err_no_mem;
+				goto err_stop_hw;
 			}
 			hsdev->hdev = hdev;
 			hsdev->vendor_id = hdev->vendor;
@@ -631,13 +632,13 @@ static int sensor_hub_probe(struct hid_device *hdev,
 			if (last_hsdev)
 				last_hsdev->end_collection_index = i;
 			last_hsdev = hsdev;
-			name = kasprintf(GFP_KERNEL, "HID-SENSOR-%x",
-					collection->usage);
+			name = devm_kasprintf(&hdev->dev, GFP_KERNEL,
+					      "HID-SENSOR-%x",
+					      collection->usage);
 			if (name == NULL) {
 				hid_err(hdev, "Failed MFD device name\n");
 					ret = -ENOMEM;
-					kfree(hsdev);
-					goto err_no_mem;
+					goto err_stop_hw;
 			}
 			sd->hid_sensor_hub_client_devs[
 				sd->hid_sensor_client_cnt].id =
@@ -661,16 +662,10 @@ static int sensor_hub_probe(struct hid_device *hdev,
 	ret = mfd_add_devices(&hdev->dev, 0, sd->hid_sensor_hub_client_devs,
 		sd->hid_sensor_client_cnt, NULL, 0, NULL);
 	if (ret < 0)
-		goto err_no_mem;
+		goto err_stop_hw;
 
 	return ret;
 
-err_no_mem:
-	for (i = 0; i < sd->hid_sensor_client_cnt; ++i) {
-		kfree(sd->hid_sensor_hub_client_devs[i].name);
-		kfree(sd->hid_sensor_hub_client_devs[i].platform_data);
-	}
-	kfree(sd->hid_sensor_hub_client_devs);
 err_stop_hw:
 	hid_hw_stop(hdev);
 
@@ -681,7 +676,6 @@ static void sensor_hub_remove(struct hid_device *hdev)
 {
 	struct sensor_hub_data *data = hid_get_drvdata(hdev);
 	unsigned long flags;
-	int i;
 
 	hid_dbg(hdev, " hardware removed\n");
 	hid_hw_close(hdev);
@@ -691,11 +685,6 @@ static void sensor_hub_remove(struct hid_device *hdev)
 		complete(&data->pending.ready);
 	spin_unlock_irqrestore(&data->lock, flags);
 	mfd_remove_devices(&hdev->dev);
-	for (i = 0; i < data->hid_sensor_client_cnt; ++i) {
-		kfree(data->hid_sensor_hub_client_devs[i].name);
-		kfree(data->hid_sensor_hub_client_devs[i].platform_data);
-	}
-	kfree(data->hid_sensor_hub_client_devs);
 	hid_set_drvdata(hdev, NULL);
 	mutex_destroy(&data->mutex);
 }
@@ -720,6 +709,9 @@ static const struct hid_device_id sensor_hub_devices[] = {
 			USB_DEVICE_ID_MS_TYPE_COVER_2),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_STM_0,
+			USB_DEVICE_ID_STM_HID_SENSOR),
+			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
+	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_STM_0,
 			USB_DEVICE_ID_STM_HID_SENSOR_1),
 			.driver_data = HID_SENSOR_HUB_ENUM_QUIRK},
 	{ HID_DEVICE(HID_BUS_ANY, HID_GROUP_SENSOR_HUB, USB_VENDOR_ID_TEXAS_INSTRUMENTS,
diff --git a/drivers/hid/hid-sony.c b/drivers/hid/hid-sony.c
index c372368e438c..bc4269e559f1 100644
--- a/drivers/hid/hid-sony.c
+++ b/drivers/hid/hid-sony.c
@@ -1,5 +1,5 @@
 /*
- *  HID driver for Sony / PS2 / PS3 BD devices.
+ *  HID driver for Sony / PS2 / PS3 / PS4 BD devices.
  *
  *  Copyright (c) 1999 Andreas Gal
  *  Copyright (c) 2000-2005 Vojtech Pavlik <vojtech@suse.cz>
@@ -8,6 +8,7 @@
  *  Copyright (c) 2012 David Dillow <dave@thedillows.org>
  *  Copyright (c) 2006-2013 Jiri Kosina
  *  Copyright (c) 2013 Colin Leitner <colin.leitner@gmail.com>
+ *  Copyright (c) 2014 Frank Praznik <frank.praznik@gmail.com>
  */
 
 /*
@@ -176,7 +177,7 @@ static u8 dualshock4_usb_rdesc[] = {
 	0x75, 0x06,         /*      Report Size (6),                */
 	0x95, 0x01,         /*      Report Count (1),               */
 	0x15, 0x00,         /*      Logical Minimum (0),            */
-	0x25, 0x7F,         /*      Logical Maximum (127),          */
+	0x25, 0x3F,         /*      Logical Maximum (63),           */
 	0x81, 0x02,         /*      Input (Variable),               */
 	0x05, 0x01,         /*      Usage Page (Desktop),           */
 	0x09, 0x33,         /*      Usage (Rx),                     */
@@ -200,14 +201,14 @@ static u8 dualshock4_usb_rdesc[] = {
 	0x81, 0x02,         /*      Input (Variable),               */
 	0x19, 0x43,         /*      Usage Minimum (43h),            */
 	0x29, 0x45,         /*      Usage Maximum (45h),            */
-	0x16, 0xFF, 0xBF,   /*      Logical Minimum (-16385),       */
-	0x26, 0x00, 0x40,   /*      Logical Maximum (16384),        */
+	0x16, 0x00, 0xE0,   /*      Logical Minimum (-8192),        */
+	0x26, 0xFF, 0x1F,   /*      Logical Maximum (8191),         */
 	0x95, 0x03,         /*      Report Count (3),               */
 	0x81, 0x02,         /*      Input (Variable),               */
 	0x06, 0x00, 0xFF,   /*      Usage Page (FF00h),             */
 	0x09, 0x21,         /*      Usage (21h),                    */
 	0x15, 0x00,         /*      Logical Minimum (0),            */
-	0x25, 0xFF,         /*      Logical Maximum (255),          */
+	0x26, 0xFF, 0x00,   /*      Logical Maximum (255),          */
 	0x75, 0x08,         /*      Report Size (8),                */
 	0x95, 0x27,         /*      Report Count (39),              */
 	0x81, 0x02,         /*      Input (Variable),               */
@@ -395,11 +396,11 @@ static u8 dualshock4_usb_rdesc[] = {
 
 /*
  * The default behavior of the Dualshock 4 is to send reports using report
- * type 1 when running over Bluetooth. However, as soon as it receives a
- * report of type 17 to set the LEDs or rumble it starts returning it's state
- * in report 17 instead of 1.  Since report 17 is undefined in the default HID
+ * type 1 when running over Bluetooth. However, when feature report 2 is
+ * requested during the controller initialization it starts sending input
+ * reports in report 17.  Since report 17 is undefined in the default HID
  * descriptor the button and axis definitions must be moved to report 17 or
- * the HID layer won't process the received input once a report is sent.
+ * the HID layer won't process the received input.
  */
 static u8 dualshock4_bt_rdesc[] = {
 	0x05, 0x01,         /*  Usage Page (Desktop),               */
@@ -509,8 +510,8 @@ static u8 dualshock4_bt_rdesc[] = {
 	0x81, 0x02,         /*      Input (Variable),               */
 	0x19, 0x43,         /*      Usage Minimum (43h),            */
 	0x29, 0x45,         /*      Usage Maximum (45h),            */
-	0x16, 0xFF, 0xBF,   /*      Logical Minimum (-16385),       */
-	0x26, 0x00, 0x40,   /*      Logical Maximum (16384),        */
+	0x16, 0x00, 0xE0,   /*      Logical Minimum (-8192),        */
+	0x26, 0xFF, 0x1F,   /*      Logical Maximum (8191),         */
 	0x95, 0x03,         /*      Report Count (3),               */
 	0x81, 0x02,         /*      Input (Variable),               */
 	0x06, 0x00, 0xFF,   /*      Usage Page (FF00h),             */
@@ -935,12 +936,13 @@ static void sixaxis_parse_report(struct sony_sc *sc, __u8 *rd, int size)
 	if (rd[30] >= 0xee) {
 		battery_capacity = 100;
 		battery_charging = !(rd[30] & 0x01);
+		cable_state = 1;
 	} else {
 		__u8 index = rd[30] <= 5 ? rd[30] : 5;
 		battery_capacity = sixaxis_battery_capacity[index];
 		battery_charging = 0;
+		cable_state = 0;
 	}
-	cable_state = !(rd[31] & 0x04);
 
 	spin_lock_irqsave(&sc->lock, flags);
 	sc->cable_state = cable_state;
@@ -1082,6 +1084,38 @@ static int sony_mapping(struct hid_device *hdev, struct hid_input *hi,
 	return 0;
 }
 
+static int sony_register_touchpad(struct hid_input *hi, int touch_count,
+					int w, int h)
+{
+	struct input_dev *input_dev = hi->input;
+	int ret;
+
+	ret = input_mt_init_slots(input_dev, touch_count, 0);
+	if (ret < 0)
+		return ret;
+
+	input_set_abs_params(input_dev, ABS_MT_POSITION_X, 0, w, 0, 0);
+	input_set_abs_params(input_dev, ABS_MT_POSITION_Y, 0, h, 0, 0);
+
+	return 0;
+}
+
+static void sony_input_configured(struct hid_device *hdev,
+					struct hid_input *hidinput)
+{
+	struct sony_sc *sc = hid_get_drvdata(hdev);
+
+	/*
+	 * The Dualshock 4 touchpad supports 2 touches and has a
+	 * resolution of 1920x942 (44.86 dots/mm).
+	 */
+	if (sc->quirks & DUALSHOCK4_CONTROLLER) {
+		if (sony_register_touchpad(hidinput, 2, 1920, 942) != 0)
+			hid_err(sc->hdev,
+				"Unable to initialize multi-touch slots\n");
+	}
+}
+
 /*
  * Sending HID_REQ_GET_REPORT changes the operation mode of the ps3 controller
  * to "operational".  Without this, the ps3 controller will not report any
@@ -1654,26 +1688,6 @@ static void sony_battery_remove(struct sony_sc *sc)
 	sc->battery.name = NULL;
 }
 
-static int sony_register_touchpad(struct sony_sc *sc, int touch_count,
-					int w, int h)
-{
-	struct hid_input *hidinput = list_entry(sc->hdev->inputs.next,
-						struct hid_input, list);
-	struct input_dev *input_dev = hidinput->input;
-	int ret;
-
-	ret = input_mt_init_slots(input_dev, touch_count, 0);
-	if (ret < 0) {
-		hid_err(sc->hdev, "Unable to initialize multi-touch slots\n");
-		return ret;
-	}
-
-	input_set_abs_params(input_dev, ABS_MT_POSITION_X, 0, w, 0, 0);
-	input_set_abs_params(input_dev, ABS_MT_POSITION_Y, 0, h, 0, 0);
-
-	return 0;
-}
-
 /*
  * If a controller is plugged in via USB while already connected via Bluetooth
  * it will show up as two devices. A global list of connected controllers and
@@ -1923,13 +1937,6 @@ static int sony_probe(struct hid_device *hdev, const struct hid_device_id *id)
 				goto err_stop;
 			}
 		}
-		/*
-		 * The Dualshock 4 touchpad supports 2 touches and has a
-		 * resolution of 1920x940.
-		 */
-		ret = sony_register_touchpad(sc, 2, 1920, 940);
-		if (ret < 0)
-			goto err_stop;
 
 		sony_init_work(sc, dualshock4_state_worker);
 	} else {
@@ -2037,13 +2044,14 @@ static const struct hid_device_id sony_devices[] = {
 MODULE_DEVICE_TABLE(hid, sony_devices);
 
 static struct hid_driver sony_driver = {
-	.name          = "sony",
-	.id_table      = sony_devices,
-	.input_mapping = sony_mapping,
-	.probe         = sony_probe,
-	.remove        = sony_remove,
-	.report_fixup  = sony_report_fixup,
-	.raw_event     = sony_raw_event
+	.name             = "sony",
+	.id_table         = sony_devices,
+	.input_mapping    = sony_mapping,
+	.input_configured = sony_input_configured,
+	.probe            = sony_probe,
+	.remove           = sony_remove,
+	.report_fixup     = sony_report_fixup,
+	.raw_event        = sony_raw_event
 };
 
 static int __init sony_init(void)
diff --git a/drivers/hid/hid-sunplus.c b/drivers/hid/hid-sunplus.c
index 87fc91e1c8de..91072fa54663 100644
--- a/drivers/hid/hid-sunplus.c
+++ b/drivers/hid/hid-sunplus.c
@@ -24,7 +24,7 @@
 static __u8 *sp_report_fixup(struct hid_device *hdev, __u8 *rdesc,
 		unsigned int *rsize)
 {
-	if (*rsize >= 107 && rdesc[104] == 0x26 && rdesc[105] == 0x80 &&
+	if (*rsize >= 112 && rdesc[104] == 0x26 && rdesc[105] == 0x80 &&
 			rdesc[106] == 0x03) {
 		hid_info(hdev, "fixing up Sunplus Wireless Desktop report descriptor\n");
 		rdesc[105] = rdesc[110] = 0x03;
diff --git a/drivers/hid/hid-thingm.c b/drivers/hid/hid-thingm.c
index 134be89b15ea..b95d3978c272 100644
--- a/drivers/hid/hid-thingm.c
+++ b/drivers/hid/hid-thingm.c
@@ -208,10 +208,10 @@ unregister_red:
 
 static void thingm_remove_rgb(struct thingm_rgb *rgb)
 {
-	flush_work(&rgb->work);
 	led_classdev_unregister(&rgb->red.ldev);
 	led_classdev_unregister(&rgb->green.ldev);
 	led_classdev_unregister(&rgb->blue.ldev);
+	flush_work(&rgb->work);
 }
 
 static int thingm_probe(struct hid_device *hdev, const struct hid_device_id *id)
@@ -250,6 +250,7 @@ static int thingm_probe(struct hid_device *hdev, const struct hid_device_id *id)
 
 	if (!tdev->fwinfo) {
 		hid_err(hdev, "unsupported firmware %c\n", tdev->version.major);
+		err = -ENODEV;
 		goto stop;
 	}
 
@@ -286,10 +287,10 @@ static void thingm_remove(struct hid_device *hdev)
 	struct thingm_device *tdev = hid_get_drvdata(hdev);
 	int i;
 
+	hid_hw_stop(hdev);
+
 	for (i = 0; i < tdev->fwinfo->numrgb; ++i)
 		thingm_remove_rgb(tdev->rgb + i);
-
-	hid_hw_stop(hdev);
 }
 
 static const struct hid_device_id thingm_table[] = {
diff --git a/drivers/hid/uhid.c b/drivers/hid/uhid.c
index 0cb92e347258..e094c572b86e 100644
--- a/drivers/hid/uhid.c
+++ b/drivers/hid/uhid.c
@@ -44,10 +44,12 @@ struct uhid_device {
 	__u8 tail;
 	struct uhid_event *outq[UHID_BUFSIZE];
 
+	/* blocking GET_REPORT support; state changes protected by qlock */
 	struct mutex report_lock;
 	wait_queue_head_t report_wait;
-	atomic_t report_done;
-	atomic_t report_id;
+	bool report_running;
+	u32 report_id;
+	u32 report_type;
 	struct uhid_event report_buf;
 };
 
@@ -90,8 +92,27 @@ static int uhid_queue_event(struct uhid_device *uhid, __u32 event)
 static int uhid_hid_start(struct hid_device *hid)
 {
 	struct uhid_device *uhid = hid->driver_data;
+	struct uhid_event *ev;
+	unsigned long flags;
+
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev)
+		return -ENOMEM;
+
+	ev->type = UHID_START;
 
-	return uhid_queue_event(uhid, UHID_START);
+	if (hid->report_enum[HID_FEATURE_REPORT].numbered)
+		ev->u.start.dev_flags |= UHID_DEV_NUMBERED_FEATURE_REPORTS;
+	if (hid->report_enum[HID_OUTPUT_REPORT].numbered)
+		ev->u.start.dev_flags |= UHID_DEV_NUMBERED_OUTPUT_REPORTS;
+	if (hid->report_enum[HID_INPUT_REPORT].numbered)
+		ev->u.start.dev_flags |= UHID_DEV_NUMBERED_INPUT_REPORTS;
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+	uhid_queue(uhid, ev);
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+
+	return 0;
 }
 
 static void uhid_hid_stop(struct hid_device *hid)
@@ -123,87 +144,169 @@ static int uhid_hid_parse(struct hid_device *hid)
 	return hid_parse_report(hid, uhid->rd_data, uhid->rd_size);
 }
 
-static int uhid_hid_get_raw(struct hid_device *hid, unsigned char rnum,
-			    __u8 *buf, size_t count, unsigned char rtype)
+/* must be called with report_lock held */
+static int __uhid_report_queue_and_wait(struct uhid_device *uhid,
+					struct uhid_event *ev,
+					__u32 *report_id)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+	*report_id = ++uhid->report_id;
+	uhid->report_type = ev->type + 1;
+	uhid->report_running = true;
+	uhid_queue(uhid, ev);
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+
+	ret = wait_event_interruptible_timeout(uhid->report_wait,
+				!uhid->report_running || !uhid->running,
+				5 * HZ);
+	if (!ret || !uhid->running || uhid->report_running)
+		ret = -EIO;
+	else if (ret < 0)
+		ret = -ERESTARTSYS;
+	else
+		ret = 0;
+
+	uhid->report_running = false;
+
+	return ret;
+}
+
+static void uhid_report_wake_up(struct uhid_device *uhid, u32 id,
+				const struct uhid_event *ev)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&uhid->qlock, flags);
+
+	/* id for old report; drop it silently */
+	if (uhid->report_type != ev->type || uhid->report_id != id)
+		goto unlock;
+	if (!uhid->report_running)
+		goto unlock;
+
+	memcpy(&uhid->report_buf, ev, sizeof(*ev));
+	uhid->report_running = false;
+	wake_up_interruptible(&uhid->report_wait);
+
+unlock:
+	spin_unlock_irqrestore(&uhid->qlock, flags);
+}
+
+static int uhid_hid_get_report(struct hid_device *hid, unsigned char rnum,
+			       u8 *buf, size_t count, u8 rtype)
 {
 	struct uhid_device *uhid = hid->driver_data;
-	__u8 report_type;
+	struct uhid_get_report_reply_req *req;
 	struct uhid_event *ev;
-	unsigned long flags;
 	int ret;
-	size_t uninitialized_var(len);
-	struct uhid_feature_answer_req *req;
 
 	if (!uhid->running)
 		return -EIO;
 
-	switch (rtype) {
-	case HID_FEATURE_REPORT:
-		report_type = UHID_FEATURE_REPORT;
-		break;
-	case HID_OUTPUT_REPORT:
-		report_type = UHID_OUTPUT_REPORT;
-		break;
-	case HID_INPUT_REPORT:
-		report_type = UHID_INPUT_REPORT;
-		break;
-	default:
-		return -EINVAL;
-	}
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev)
+		return -ENOMEM;
+
+	ev->type = UHID_GET_REPORT;
+	ev->u.get_report.rnum = rnum;
+	ev->u.get_report.rtype = rtype;
 
 	ret = mutex_lock_interruptible(&uhid->report_lock);
-	if (ret)
+	if (ret) {
+		kfree(ev);
 		return ret;
+	}
 
-	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
-	if (!ev) {
-		ret = -ENOMEM;
+	/* this _always_ takes ownership of @ev */
+	ret = __uhid_report_queue_and_wait(uhid, ev, &ev->u.get_report.id);
+	if (ret)
 		goto unlock;
+
+	req = &uhid->report_buf.u.get_report_reply;
+	if (req->err) {
+		ret = -EIO;
+	} else {
+		ret = min3(count, (size_t)req->size, (size_t)UHID_DATA_MAX);
+		memcpy(buf, req->data, ret);
 	}
 
-	spin_lock_irqsave(&uhid->qlock, flags);
-	ev->type = UHID_FEATURE;
-	ev->u.feature.id = atomic_inc_return(&uhid->report_id);
-	ev->u.feature.rnum = rnum;
-	ev->u.feature.rtype = report_type;
+unlock:
+	mutex_unlock(&uhid->report_lock);
+	return ret;
+}
 
-	atomic_set(&uhid->report_done, 0);
-	uhid_queue(uhid, ev);
-	spin_unlock_irqrestore(&uhid->qlock, flags);
+static int uhid_hid_set_report(struct hid_device *hid, unsigned char rnum,
+			       const u8 *buf, size_t count, u8 rtype)
+{
+	struct uhid_device *uhid = hid->driver_data;
+	struct uhid_event *ev;
+	int ret;
 
-	ret = wait_event_interruptible_timeout(uhid->report_wait,
-				atomic_read(&uhid->report_done), 5 * HZ);
-
-	/*
-	 * Make sure "uhid->running" is cleared on shutdown before
-	 * "uhid->report_done" is set.
-	 */
-	smp_rmb();
-	if (!ret || !uhid->running) {
-		ret = -EIO;
-	} else if (ret < 0) {
-		ret = -ERESTARTSYS;
-	} else {
-		spin_lock_irqsave(&uhid->qlock, flags);
-		req = &uhid->report_buf.u.feature_answer;
+	if (!uhid->running || count > UHID_DATA_MAX)
+		return -EIO;
 
-		if (req->err) {
-			ret = -EIO;
-		} else {
-			ret = 0;
-			len = min(count,
-				min_t(size_t, req->size, UHID_DATA_MAX));
-			memcpy(buf, req->data, len);
-		}
+	ev = kzalloc(sizeof(*ev), GFP_KERNEL);
+	if (!ev)
+		return -ENOMEM;
+
+	ev->type = UHID_SET_REPORT;
+	ev->u.set_report.rnum = rnum;
+	ev->u.set_report.rtype = rtype;
+	ev->u.set_report.size = count;
+	memcpy(ev->u.set_report.data, buf, count);
 
-		spin_unlock_irqrestore(&uhid->qlock, flags);
+	ret = mutex_lock_interruptible(&uhid->report_lock);
+	if (ret) {
+		kfree(ev);
+		return ret;
 	}
 
-	atomic_set(&uhid->report_done, 1);
+	/* this _always_ takes ownership of @ev */
+	ret = __uhid_report_queue_and_wait(uhid, ev, &ev->u.set_report.id);
+	if (ret)
+		goto unlock;
+
+	if (uhid->report_buf.u.set_report_reply.err)
+		ret = -EIO;
+	else
+		ret = count;
 
 unlock:
 	mutex_unlock(&uhid->report_lock);
-	return ret ? ret : len;
+	return ret;
+}
+
+static int uhid_hid_raw_request(struct hid_device *hid, unsigned char reportnum,
+				__u8 *buf, size_t len, unsigned char rtype,
+				int reqtype)
+{
+	u8 u_rtype;
+
+	switch (rtype) {
+	case HID_FEATURE_REPORT:
+		u_rtype = UHID_FEATURE_REPORT;
+		break;
+	case HID_OUTPUT_REPORT:
+		u_rtype = UHID_OUTPUT_REPORT;
+		break;
+	case HID_INPUT_REPORT:
+		u_rtype = UHID_INPUT_REPORT;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	switch (reqtype) {
+	case HID_REQ_GET_REPORT:
+		return uhid_hid_get_report(hid, reportnum, buf, len, u_rtype);
+	case HID_REQ_SET_REPORT:
+		return uhid_hid_set_report(hid, reportnum, buf, len, u_rtype);
+	default:
+		return -EIO;
+	}
 }
 
 static int uhid_hid_output_raw(struct hid_device *hid, __u8 *buf, size_t count,
@@ -250,29 +353,14 @@ static int uhid_hid_output_report(struct hid_device *hid, __u8 *buf,
 	return uhid_hid_output_raw(hid, buf, count, HID_OUTPUT_REPORT);
 }
 
-static int uhid_raw_request(struct hid_device *hid, unsigned char reportnum,
-			    __u8 *buf, size_t len, unsigned char rtype,
-			    int reqtype)
-{
-	switch (reqtype) {
-	case HID_REQ_GET_REPORT:
-		return uhid_hid_get_raw(hid, reportnum, buf, len, rtype);
-	case HID_REQ_SET_REPORT:
-		/* TODO: implement proper SET_REPORT functionality */
-		return -ENOSYS;
-	default:
-		return -EIO;
-	}
-}
-
 static struct hid_ll_driver uhid_hid_driver = {
 	.start = uhid_hid_start,
 	.stop = uhid_hid_stop,
 	.open = uhid_hid_open,
 	.close = uhid_hid_close,
 	.parse = uhid_hid_parse,
+	.raw_request = uhid_hid_raw_request,
 	.output_report = uhid_hid_output_report,
-	.raw_request = uhid_raw_request,
 };
 
 #ifdef CONFIG_COMPAT
@@ -363,28 +451,27 @@ static int uhid_event_from_user(const char __user *buffer, size_t len,
 }
 #endif
 
-static int uhid_dev_create(struct uhid_device *uhid,
-			   const struct uhid_event *ev)
+static int uhid_dev_create2(struct uhid_device *uhid,
+			    const struct uhid_event *ev)
 {
 	struct hid_device *hid;
+	size_t rd_size, len;
+	void *rd_data;
 	int ret;
 
 	if (uhid->running)
 		return -EALREADY;
 
-	uhid->rd_size = ev->u.create.rd_size;
-	if (uhid->rd_size <= 0 || uhid->rd_size > HID_MAX_DESCRIPTOR_SIZE)
+	rd_size = ev->u.create2.rd_size;
+	if (rd_size <= 0 || rd_size > HID_MAX_DESCRIPTOR_SIZE)
 		return -EINVAL;
 
-	uhid->rd_data = kmalloc(uhid->rd_size, GFP_KERNEL);
-	if (!uhid->rd_data)
+	rd_data = kmemdup(ev->u.create2.rd_data, rd_size, GFP_KERNEL);
+	if (!rd_data)
 		return -ENOMEM;
 
-	if (copy_from_user(uhid->rd_data, ev->u.create.rd_data,
-			   uhid->rd_size)) {
-		ret = -EFAULT;
-		goto err_free;
-	}
+	uhid->rd_size = rd_size;
+	uhid->rd_data = rd_data;
 
 	hid = hid_allocate_device();
 	if (IS_ERR(hid)) {
@@ -392,19 +479,19 @@ static int uhid_dev_create(struct uhid_device *uhid,
 		goto err_free;
 	}
 
-	strncpy(hid->name, ev->u.create.name, 127);
-	hid->name[127] = 0;
-	strncpy(hid->phys, ev->u.create.phys, 63);
-	hid->phys[63] = 0;
-	strncpy(hid->uniq, ev->u.create.uniq, 63);
-	hid->uniq[63] = 0;
+	len = min(sizeof(hid->name), sizeof(ev->u.create2.name)) - 1;
+	strncpy(hid->name, ev->u.create2.name, len);
+	len = min(sizeof(hid->phys), sizeof(ev->u.create2.phys)) - 1;
+	strncpy(hid->phys, ev->u.create2.phys, len);
+	len = min(sizeof(hid->uniq), sizeof(ev->u.create2.uniq)) - 1;
+	strncpy(hid->uniq, ev->u.create2.uniq, len);
 
 	hid->ll_driver = &uhid_hid_driver;
-	hid->bus = ev->u.create.bus;
-	hid->vendor = ev->u.create.vendor;
-	hid->product = ev->u.create.product;
-	hid->version = ev->u.create.version;
-	hid->country = ev->u.create.country;
+	hid->bus = ev->u.create2.bus;
+	hid->vendor = ev->u.create2.vendor;
+	hid->product = ev->u.create2.product;
+	hid->version = ev->u.create2.version;
+	hid->country = ev->u.create2.country;
 	hid->driver_data = uhid;
 	hid->dev.parent = uhid_misc.this_device;
 
@@ -425,67 +512,34 @@ err_hid:
 	uhid->running = false;
 err_free:
 	kfree(uhid->rd_data);
+	uhid->rd_data = NULL;
+	uhid->rd_size = 0;
 	return ret;
 }
 
-static int uhid_dev_create2(struct uhid_device *uhid,
-			    const struct uhid_event *ev)
+static int uhid_dev_create(struct uhid_device *uhid,
+			   struct uhid_event *ev)
 {
-	struct hid_device *hid;
-	int ret;
+	struct uhid_create_req orig;
 
-	if (uhid->running)
-		return -EALREADY;
+	orig = ev->u.create;
 
-	uhid->rd_size = ev->u.create2.rd_size;
-	if (uhid->rd_size <= 0 || uhid->rd_size > HID_MAX_DESCRIPTOR_SIZE)
+	if (orig.rd_size <= 0 || orig.rd_size > HID_MAX_DESCRIPTOR_SIZE)
 		return -EINVAL;
+	if (copy_from_user(&ev->u.create2.rd_data, orig.rd_data, orig.rd_size))
+		return -EFAULT;
 
-	uhid->rd_data = kmemdup(ev->u.create2.rd_data, uhid->rd_size,
-				GFP_KERNEL);
-	if (!uhid->rd_data)
-		return -ENOMEM;
-
-	hid = hid_allocate_device();
-	if (IS_ERR(hid)) {
-		ret = PTR_ERR(hid);
-		goto err_free;
-	}
-
-	strncpy(hid->name, ev->u.create2.name, 127);
-	hid->name[127] = 0;
-	strncpy(hid->phys, ev->u.create2.phys, 63);
-	hid->phys[63] = 0;
-	strncpy(hid->uniq, ev->u.create2.uniq, 63);
-	hid->uniq[63] = 0;
-
-	hid->ll_driver = &uhid_hid_driver;
-	hid->bus = ev->u.create2.bus;
-	hid->vendor = ev->u.create2.vendor;
-	hid->product = ev->u.create2.product;
-	hid->version = ev->u.create2.version;
-	hid->country = ev->u.create2.country;
-	hid->driver_data = uhid;
-	hid->dev.parent = uhid_misc.this_device;
-
-	uhid->hid = hid;
-	uhid->running = true;
-
-	ret = hid_add_device(hid);
-	if (ret) {
-		hid_err(hid, "Cannot register HID device\n");
-		goto err_hid;
-	}
-
-	return 0;
-
-err_hid:
-	hid_destroy_device(hid);
-	uhid->hid = NULL;
-	uhid->running = false;
-err_free:
-	kfree(uhid->rd_data);
-	return ret;
+	memcpy(ev->u.create2.name, orig.name, sizeof(orig.name));
+	memcpy(ev->u.create2.phys, orig.phys, sizeof(orig.phys));
+	memcpy(ev->u.create2.uniq, orig.uniq, sizeof(orig.uniq));
+	ev->u.create2.rd_size = orig.rd_size;
+	ev->u.create2.bus = orig.bus;
+	ev->u.create2.vendor = orig.vendor;
+	ev->u.create2.product = orig.product;
+	ev->u.create2.version = orig.version;
+	ev->u.create2.country = orig.country;
+
+	return uhid_dev_create2(uhid, ev);
 }
 
 static int uhid_dev_destroy(struct uhid_device *uhid)
@@ -493,10 +547,7 @@ static int uhid_dev_destroy(struct uhid_device *uhid)
 	if (!uhid->running)
 		return -EINVAL;
 
-	/* clear "running" before setting "report_done" */
 	uhid->running = false;
-	smp_wmb();
-	atomic_set(&uhid->report_done, 1);
 	wake_up_interruptible(&uhid->report_wait);
 
 	hid_destroy_device(uhid->hid);
@@ -527,28 +578,23 @@ static int uhid_dev_input2(struct uhid_device *uhid, struct uhid_event *ev)
 	return 0;
 }
 
-static int uhid_dev_feature_answer(struct uhid_device *uhid,
-				   struct uhid_event *ev)
+static int uhid_dev_get_report_reply(struct uhid_device *uhid,
+				     struct uhid_event *ev)
 {
-	unsigned long flags;
-
 	if (!uhid->running)
 		return -EINVAL;
 
-	spin_lock_irqsave(&uhid->qlock, flags);
-
-	/* id for old report; drop it silently */
-	if (atomic_read(&uhid->report_id) != ev->u.feature_answer.id)
-		goto unlock;
-	if (atomic_read(&uhid->report_done))
-		goto unlock;
+	uhid_report_wake_up(uhid, ev->u.get_report_reply.id, ev);
+	return 0;
+}
 
-	memcpy(&uhid->report_buf, ev, sizeof(*ev));
-	atomic_set(&uhid->report_done, 1);
-	wake_up_interruptible(&uhid->report_wait);
+static int uhid_dev_set_report_reply(struct uhid_device *uhid,
+				     struct uhid_event *ev)
+{
+	if (!uhid->running)
+		return -EINVAL;
 
-unlock:
-	spin_unlock_irqrestore(&uhid->qlock, flags);
+	uhid_report_wake_up(uhid, ev->u.set_report_reply.id, ev);
 	return 0;
 }
 
@@ -566,7 +612,6 @@ static int uhid_char_open(struct inode *inode, struct file *file)
 	init_waitqueue_head(&uhid->waitq);
 	init_waitqueue_head(&uhid->report_wait);
 	uhid->running = false;
-	atomic_set(&uhid->report_done, 1);
 
 	file->private_data = uhid;
 	nonseekable_open(inode, file);
@@ -675,8 +720,11 @@ static ssize_t uhid_char_write(struct file *file, const char __user *buffer,
 	case UHID_INPUT2:
 		ret = uhid_dev_input2(uhid, &uhid->input_buf);
 		break;
-	case UHID_FEATURE_ANSWER:
-		ret = uhid_dev_feature_answer(uhid, &uhid->input_buf);
+	case UHID_GET_REPORT_REPLY:
+		ret = uhid_dev_get_report_reply(uhid, &uhid->input_buf);
+		break;
+	case UHID_SET_REPORT_REPLY:
+		ret = uhid_dev_set_report_reply(uhid, &uhid->input_buf);
 		break;
 	default:
 		ret = -EOPNOTSUPP;
diff --git a/drivers/hid/usbhid/hid-core.c b/drivers/hid/usbhid/hid-core.c
index 79cf503e37bf..ca6849a0121e 100644
--- a/drivers/hid/usbhid/hid-core.c
+++ b/drivers/hid/usbhid/hid-core.c
@@ -82,7 +82,7 @@ static int hid_start_in(struct hid_device *hid)
 	struct usbhid_device *usbhid = hid->driver_data;
 
 	spin_lock_irqsave(&usbhid->lock, flags);
-	if (hid->open > 0 &&
+	if ((hid->open > 0 || hid->quirks & HID_QUIRK_ALWAYS_POLL) &&
 			!test_bit(HID_DISCONNECTED, &usbhid->iofl) &&
 			!test_bit(HID_SUSPENDED, &usbhid->iofl) &&
 			!test_and_set_bit(HID_IN_RUNNING, &usbhid->iofl)) {
@@ -116,40 +116,24 @@ static void hid_reset(struct work_struct *work)
 	struct usbhid_device *usbhid =
 		container_of(work, struct usbhid_device, reset_work);
 	struct hid_device *hid = usbhid->hid;
-	int rc = 0;
+	int rc;
 
 	if (test_bit(HID_CLEAR_HALT, &usbhid->iofl)) {
 		dev_dbg(&usbhid->intf->dev, "clear halt\n");
 		rc = usb_clear_halt(hid_to_usb_dev(hid), usbhid->urbin->pipe);
 		clear_bit(HID_CLEAR_HALT, &usbhid->iofl);
-		hid_start_in(hid);
-	}
-
-	else if (test_bit(HID_RESET_PENDING, &usbhid->iofl)) {
-		dev_dbg(&usbhid->intf->dev, "resetting device\n");
-		rc = usb_lock_device_for_reset(hid_to_usb_dev(hid), usbhid->intf);
 		if (rc == 0) {
-			rc = usb_reset_device(hid_to_usb_dev(hid));
-			usb_unlock_device(hid_to_usb_dev(hid));
+			hid_start_in(hid);
+		} else {
+			dev_dbg(&usbhid->intf->dev,
+					"clear-halt failed: %d\n", rc);
+			set_bit(HID_RESET_PENDING, &usbhid->iofl);
 		}
-		clear_bit(HID_RESET_PENDING, &usbhid->iofl);
 	}
 
-	switch (rc) {
-	case 0:
-		if (!test_bit(HID_IN_RUNNING, &usbhid->iofl))
-			hid_io_error(hid);
-		break;
-	default:
-		hid_err(hid, "can't reset device, %s-%s/input%d, status %d\n",
-			hid_to_usb_dev(hid)->bus->bus_name,
-			hid_to_usb_dev(hid)->devpath,
-			usbhid->ifnum, rc);
-		/* FALLTHROUGH */
-	case -EHOSTUNREACH:
-	case -ENODEV:
-	case -EINTR:
-		break;
+	if (test_bit(HID_RESET_PENDING, &usbhid->iofl)) {
+		dev_dbg(&usbhid->intf->dev, "resetting device\n");
+		usb_queue_reset_device(usbhid->intf);
 	}
 }
 
@@ -292,6 +276,8 @@ static void hid_irq_in(struct urb *urb)
 	case 0:			/* success */
 		usbhid_mark_busy(usbhid);
 		usbhid->retry_delay = 0;
+		if ((hid->quirks & HID_QUIRK_ALWAYS_POLL) && !hid->open)
+			break;
 		hid_input_report(urb->context, HID_INPUT_REPORT,
 				 urb->transfer_buffer,
 				 urb->actual_length, 1);
@@ -735,8 +721,10 @@ void usbhid_close(struct hid_device *hid)
 	if (!--hid->open) {
 		spin_unlock_irq(&usbhid->lock);
 		hid_cancel_delayed_stuff(usbhid);
-		usb_kill_urb(usbhid->urbin);
-		usbhid->intf->needs_remote_wakeup = 0;
+		if (!(hid->quirks & HID_QUIRK_ALWAYS_POLL)) {
+			usb_kill_urb(usbhid->urbin);
+			usbhid->intf->needs_remote_wakeup = 0;
+		}
 	} else {
 		spin_unlock_irq(&usbhid->lock);
 	}
@@ -1134,6 +1122,19 @@ static int usbhid_start(struct hid_device *hid)
 
 	set_bit(HID_STARTED, &usbhid->iofl);
 
+	if (hid->quirks & HID_QUIRK_ALWAYS_POLL) {
+		ret = usb_autopm_get_interface(usbhid->intf);
+		if (ret)
+			goto fail;
+		usbhid->intf->needs_remote_wakeup = 1;
+		ret = hid_start_in(hid);
+		if (ret) {
+			dev_err(&hid->dev,
+				"failed to start in urb: %d\n", ret);
+		}
+		usb_autopm_put_interface(usbhid->intf);
+	}
+
 	/* Some keyboards don't work until their LEDs have been set.
 	 * Since BIOSes do set the LEDs, it must be safe for any device
 	 * that supports the keyboard boot protocol.
@@ -1166,6 +1167,9 @@ static void usbhid_stop(struct hid_device *hid)
 	if (WARN_ON(!usbhid))
 		return;
 
+	if (hid->quirks & HID_QUIRK_ALWAYS_POLL)
+		usbhid->intf->needs_remote_wakeup = 0;
+
 	clear_bit(HID_STARTED, &usbhid->iofl);
 	spin_lock_irq(&usbhid->lock);	/* Sync with error and led handlers */
 	set_bit(HID_DISCONNECTED, &usbhid->iofl);
diff --git a/drivers/hid/usbhid/hid-quirks.c b/drivers/hid/usbhid/hid-quirks.c
index 15225f3eaed1..f3cb5b0a4345 100644
--- a/drivers/hid/usbhid/hid-quirks.c
+++ b/drivers/hid/usbhid/hid-quirks.c
@@ -70,6 +70,7 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_CH, USB_DEVICE_ID_CH_3AXIS_5BUTTON_STICK, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_CH, USB_DEVICE_ID_CH_AXIS_295, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_DMI, USB_DEVICE_ID_DMI_ENC, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_ELAN, USB_DEVICE_ID_ELAN_TOUCHSCREEN, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_ELO, USB_DEVICE_ID_ELO_TS2700, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_FORMOSA, USB_DEVICE_ID_FORMOSA_IR_RECEIVER, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_FREESCALE, USB_DEVICE_ID_FREESCALE_MX28, HID_QUIRK_NOGET },
@@ -79,6 +80,8 @@ static const struct hid_blacklist {
 	{ USB_VENDOR_ID_NOVATEK, USB_DEVICE_ID_NOVATEK_MOUSE, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_1610, HID_QUIRK_NOGET },
 	{ USB_VENDOR_ID_PENMOUNT, USB_DEVICE_ID_PENMOUNT_1640, HID_QUIRK_NOGET },
+	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_USB_OPTICAL_MOUSE, HID_QUIRK_ALWAYS_POLL },
+	{ USB_VENDOR_ID_KYE, USB_DEVICE_ID_PIXART_USB_OPTICAL_MOUSE_ID2, HID_QUIRK_ALWAYS_POLL },
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN1, HID_QUIRK_NO_INIT_REPORTS },
 	{ USB_VENDOR_ID_PIXART, USB_DEVICE_ID_PIXART_OPTICAL_TOUCH_SCREEN2, HID_QUIRK_NO_INIT_REPORTS },
diff --git a/drivers/hid/wacom.h b/drivers/hid/wacom.h
index 64bc1b296d91..0cc53440543a 100644
--- a/drivers/hid/wacom.h
+++ b/drivers/hid/wacom.h
@@ -89,6 +89,7 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/mod_devicetable.h>
+#include <linux/hid.h>
 #include <linux/usb/input.h>
 #include <linux/power_supply.h>
 #include <asm/unaligned.h>
@@ -143,4 +144,9 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 				   struct wacom_wac *wacom_wac);
 int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 				       struct wacom_wac *wacom_wac);
+void wacom_wac_usage_mapping(struct hid_device *hdev,
+		struct hid_field *field, struct hid_usage *usage);
+int wacom_wac_event(struct hid_device *hdev, struct hid_field *field,
+		struct hid_usage *usage, __s32 value);
+void wacom_wac_report(struct hid_device *hdev, struct hid_report *report);
 #endif
diff --git a/drivers/hid/wacom_sys.c b/drivers/hid/wacom_sys.c
index f0db7eca9023..8593047bb726 100644
--- a/drivers/hid/wacom_sys.c
+++ b/drivers/hid/wacom_sys.c
@@ -13,23 +13,26 @@
 
 #include "wacom_wac.h"
 #include "wacom.h"
-#include <linux/hid.h>
 
 #define WAC_MSG_RETRIES		5
 
+#define WAC_CMD_WL_LED_CONTROL	0x03
 #define WAC_CMD_LED_CONTROL	0x20
 #define WAC_CMD_ICON_START	0x21
 #define WAC_CMD_ICON_XFER	0x23
 #define WAC_CMD_ICON_BT_XFER	0x26
 #define WAC_CMD_RETRIES		10
 
-static int wacom_get_report(struct hid_device *hdev, u8 type, u8 id,
-			    void *buf, size_t size, unsigned int retries)
+#define DEV_ATTR_RW_PERM (S_IRUGO | S_IWUSR | S_IWGRP)
+#define DEV_ATTR_WO_PERM (S_IWUSR | S_IWGRP)
+
+static int wacom_get_report(struct hid_device *hdev, u8 type, u8 *buf,
+			    size_t size, unsigned int retries)
 {
 	int retval;
 
 	do {
-		retval = hid_hw_raw_request(hdev, id, buf, size, type,
+		retval = hid_hw_raw_request(hdev, buf[0], buf, size, type,
 				HID_REQ_GET_REPORT);
 	} while ((retval == -ETIMEDOUT || retval == -EPIPE) && --retries);
 
@@ -106,12 +109,35 @@ static void wacom_feature_mapping(struct hid_device *hdev,
 {
 	struct wacom *wacom = hid_get_drvdata(hdev);
 	struct wacom_features *features = &wacom->wacom_wac.features;
+	struct hid_data *hid_data = &wacom->wacom_wac.hid_data;
+	u8 *data;
+	int ret;
 
 	switch (usage->hid) {
 	case HID_DG_CONTACTMAX:
 		/* leave touch_max as is if predefined */
-		if (!features->touch_max)
-			features->touch_max = field->value[0];
+		if (!features->touch_max) {
+			/* read manually */
+			data = kzalloc(2, GFP_KERNEL);
+			if (!data)
+				break;
+			data[0] = field->report->id;
+			ret = wacom_get_report(hdev, HID_FEATURE_REPORT,
+						data, 2, 0);
+			if (ret == 2)
+				features->touch_max = data[1];
+			kfree(data);
+		}
+		break;
+	case HID_DG_INPUTMODE:
+		/* Ignore if value index is out of bounds. */
+		if (usage->usage_index >= field->report_count) {
+			dev_err(&hdev->dev, "HID_DG_INPUTMODE out of range\n");
+			break;
+		}
+
+		hid_data->inputmode = field->report->id;
+		hid_data->inputmode_index = usage->usage_index;
 		break;
 	}
 }
@@ -199,6 +225,9 @@ static void wacom_usage_mapping(struct hid_device *hdev,
 			features->pressure_max = field->logical_maximum;
 		break;
 	}
+
+	if (features->type == HID_GENERIC)
+		wacom_wac_usage_mapping(hdev, field, usage);
 }
 
 static void wacom_parse_hid(struct hid_device *hdev,
@@ -237,6 +266,25 @@ static void wacom_parse_hid(struct hid_device *hdev,
 	}
 }
 
+static int wacom_hid_set_device_mode(struct hid_device *hdev)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct hid_data *hid_data = &wacom->wacom_wac.hid_data;
+	struct hid_report *r;
+	struct hid_report_enum *re;
+
+	if (hid_data->inputmode < 0)
+		return 0;
+
+	re = &(hdev->report_enum[HID_FEATURE_REPORT]);
+	r = re->report_id_hash[hid_data->inputmode];
+	if (r) {
+		r->field[0]->value[hid_data->inputmode_index] = 2;
+		hid_hw_request(hdev, r, HID_REQ_SET_REPORT);
+	}
+	return 0;
+}
+
 static int wacom_set_device_mode(struct hid_device *hdev, int report_id,
 		int length, int mode)
 {
@@ -255,7 +303,7 @@ static int wacom_set_device_mode(struct hid_device *hdev, int report_id,
 					 length, 1);
 		if (error >= 0)
 			error = wacom_get_report(hdev, HID_FEATURE_REPORT,
-			                         report_id, rep_data, length, 1);
+			                         rep_data, length, 1);
 	} while ((error < 0 || rep_data[1] != mode) && limit++ < WAC_MSG_RETRIES);
 
 	kfree(rep_data);
@@ -329,6 +377,9 @@ static int wacom_query_tablet_data(struct hid_device *hdev,
 	if (hdev->bus == BUS_BLUETOOTH)
 		return wacom_bt_query_tablet_data(hdev, 1, features);
 
+	if (features->type == HID_GENERIC)
+		return wacom_hid_set_device_mode(hdev);
+
 	if (features->device_type == BTN_TOOL_FINGER) {
 		if (features->type > TABLETPC) {
 			/* MT Tablet PC touch */
@@ -487,8 +538,14 @@ static int wacom_led_control(struct wacom *wacom)
 {
 	unsigned char *buf;
 	int retval;
+	unsigned char report_id = WAC_CMD_LED_CONTROL;
+	int buf_size = 9;
 
-	buf = kzalloc(9, GFP_KERNEL);
+	if (wacom->wacom_wac.pid) { /* wireless connected */
+		report_id = WAC_CMD_WL_LED_CONTROL;
+		buf_size = 13;
+	}
+	buf = kzalloc(buf_size, GFP_KERNEL);
 	if (!buf)
 		return -ENOMEM;
 
@@ -502,9 +559,16 @@ static int wacom_led_control(struct wacom *wacom)
 		int ring_led = wacom->led.select[0] & 0x03;
 		int ring_lum = (((wacom->led.llv & 0x60) >> 5) - 1) & 0x03;
 		int crop_lum = 0;
-
-		buf[0] = WAC_CMD_LED_CONTROL;
-		buf[1] = (crop_lum << 4) | (ring_lum << 2) | (ring_led);
+		unsigned char led_bits = (crop_lum << 4) | (ring_lum << 2) | (ring_led);
+
+		buf[0] = report_id;
+		if (wacom->wacom_wac.pid) {
+			wacom_get_report(wacom->hdev, HID_FEATURE_REPORT,
+					 buf, buf_size, WAC_CMD_RETRIES);
+			buf[0] = report_id;
+			buf[4] = led_bits;
+		} else
+			buf[1] = led_bits;
 	}
 	else {
 		int led = wacom->led.select[0] | 0x4;
@@ -513,14 +577,14 @@ static int wacom_led_control(struct wacom *wacom)
 		    wacom->wacom_wac.features.type == WACOM_24HD)
 			led |= (wacom->led.select[1] << 4) | 0x40;
 
-		buf[0] = WAC_CMD_LED_CONTROL;
+		buf[0] = report_id;
 		buf[1] = led;
 		buf[2] = wacom->led.llv;
 		buf[3] = wacom->led.hlv;
 		buf[4] = wacom->led.img_lum;
 	}
 
-	retval = wacom_set_report(wacom->hdev, HID_FEATURE_REPORT, buf, 9,
+	retval = wacom_set_report(wacom->hdev, HID_FEATURE_REPORT, buf, buf_size,
 				  WAC_CMD_RETRIES);
 	kfree(buf);
 
@@ -602,9 +666,10 @@ static ssize_t wacom_led##SET_ID##_select_show(struct device *dev,	\
 {									\
 	struct hid_device *hdev = container_of(dev, struct hid_device, dev);\
 	struct wacom *wacom = hid_get_drvdata(hdev);			\
-	return snprintf(buf, 2, "%d\n", wacom->led.select[SET_ID]);	\
+	return scnprintf(buf, PAGE_SIZE, "%d\n",			\
+			 wacom->led.select[SET_ID]);			\
 }									\
-static DEVICE_ATTR(status_led##SET_ID##_select, S_IWUSR | S_IRUSR,	\
+static DEVICE_ATTR(status_led##SET_ID##_select, DEV_ATTR_RW_PERM,	\
 		    wacom_led##SET_ID##_select_show,			\
 		    wacom_led##SET_ID##_select_store)
 
@@ -641,8 +706,15 @@ static ssize_t wacom_##name##_luminance_store(struct device *dev,	\
 	return wacom_luminance_store(wacom, &wacom->led.field,		\
 				     buf, count);			\
 }									\
-static DEVICE_ATTR(name##_luminance, S_IWUSR,				\
-		   NULL, wacom_##name##_luminance_store)
+static ssize_t wacom_##name##_luminance_show(struct device *dev,	\
+	struct device_attribute *attr, char *buf)			\
+{									\
+	struct wacom *wacom = dev_get_drvdata(dev);			\
+	return scnprintf(buf, PAGE_SIZE, "%d\n", wacom->led.field);	\
+}									\
+static DEVICE_ATTR(name##_luminance, DEV_ATTR_RW_PERM,			\
+		   wacom_##name##_luminance_show,			\
+		   wacom_##name##_luminance_store)
 
 DEVICE_LUMINANCE_ATTR(status0, llv);
 DEVICE_LUMINANCE_ATTR(status1, hlv);
@@ -683,7 +755,7 @@ static ssize_t wacom_btnimg##BUTTON_ID##_store(struct device *dev,	\
 {									\
 	return wacom_button_image_store(dev, BUTTON_ID, buf, count);	\
 }									\
-static DEVICE_ATTR(button##BUTTON_ID##_rawimg, S_IWUSR,			\
+static DEVICE_ATTR(button##BUTTON_ID##_rawimg, DEV_ATTR_WO_PERM,	\
 		   NULL, wacom_btnimg##BUTTON_ID##_store)
 
 DEVICE_BTNIMG_ATTR(0);
@@ -989,7 +1061,7 @@ static ssize_t wacom_store_speed(struct device *dev,
 	return count;
 }
 
-static DEVICE_ATTR(speed, S_IRUGO | S_IWUSR | S_IWGRP,
+static DEVICE_ATTR(speed, DEV_ATTR_RW_PERM,
 		wacom_show_speed, wacom_store_speed);
 
 static struct input_dev *wacom_allocate_input(struct wacom *wacom)
@@ -1010,47 +1082,82 @@ static struct input_dev *wacom_allocate_input(struct wacom *wacom)
 	input_dev->uniq = hdev->uniq;
 	input_dev->id.bustype = hdev->bus;
 	input_dev->id.vendor  = hdev->vendor;
-	input_dev->id.product = hdev->product;
+	input_dev->id.product = wacom_wac->pid ? wacom_wac->pid : hdev->product;
 	input_dev->id.version = hdev->version;
 	input_set_drvdata(input_dev, wacom);
 
 	return input_dev;
 }
 
-static void wacom_unregister_inputs(struct wacom *wacom)
+static void wacom_free_inputs(struct wacom *wacom)
 {
-	if (wacom->wacom_wac.input)
-		input_unregister_device(wacom->wacom_wac.input);
-	if (wacom->wacom_wac.pad_input)
-		input_unregister_device(wacom->wacom_wac.pad_input);
-	wacom->wacom_wac.input = NULL;
-	wacom->wacom_wac.pad_input = NULL;
+	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
+
+	if (wacom_wac->input)
+		input_free_device(wacom_wac->input);
+	if (wacom_wac->pad_input)
+		input_free_device(wacom_wac->pad_input);
+	wacom_wac->input = NULL;
+	wacom_wac->pad_input = NULL;
 }
 
-static int wacom_register_inputs(struct wacom *wacom)
+static int wacom_allocate_inputs(struct wacom *wacom)
 {
 	struct input_dev *input_dev, *pad_input_dev;
 	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
-	int error;
 
 	input_dev = wacom_allocate_input(wacom);
 	pad_input_dev = wacom_allocate_input(wacom);
 	if (!input_dev || !pad_input_dev) {
-		error = -ENOMEM;
-		goto fail1;
+		wacom_free_inputs(wacom);
+		return -ENOMEM;
 	}
 
 	wacom_wac->input = input_dev;
 	wacom_wac->pad_input = pad_input_dev;
 	wacom_wac->pad_input->name = wacom_wac->pad_name;
 
+	return 0;
+}
+
+static void wacom_clean_inputs(struct wacom *wacom)
+{
+	if (wacom->wacom_wac.input) {
+		if (wacom->wacom_wac.input_registered)
+			input_unregister_device(wacom->wacom_wac.input);
+		else
+			input_free_device(wacom->wacom_wac.input);
+	}
+	if (wacom->wacom_wac.pad_input) {
+		if (wacom->wacom_wac.input_registered)
+			input_unregister_device(wacom->wacom_wac.pad_input);
+		else
+			input_free_device(wacom->wacom_wac.pad_input);
+	}
+	wacom->wacom_wac.input = NULL;
+	wacom->wacom_wac.pad_input = NULL;
+	wacom_destroy_leds(wacom);
+}
+
+static int wacom_register_inputs(struct wacom *wacom)
+{
+	struct input_dev *input_dev, *pad_input_dev;
+	struct wacom_wac *wacom_wac = &(wacom->wacom_wac);
+	int error;
+
+	input_dev = wacom_wac->input;
+	pad_input_dev = wacom_wac->pad_input;
+
+	if (!input_dev || !pad_input_dev)
+		return -EINVAL;
+
 	error = wacom_setup_input_capabilities(input_dev, wacom_wac);
 	if (error)
-		goto fail2;
+		return error;
 
 	error = input_register_device(input_dev);
 	if (error)
-		goto fail2;
+		return error;
 
 	error = wacom_setup_pad_input_capabilities(pad_input_dev, wacom_wac);
 	if (error) {
@@ -1061,22 +1168,23 @@ static int wacom_register_inputs(struct wacom *wacom)
 	} else {
 		error = input_register_device(pad_input_dev);
 		if (error)
-			goto fail3;
+			goto fail_register_pad_input;
+
+		error = wacom_initialize_leds(wacom);
+		if (error)
+			goto fail_leds;
 	}
 
+	wacom_wac->input_registered = true;
+
 	return 0;
 
-fail3:
+fail_leds:
+	input_unregister_device(pad_input_dev);
+	pad_input_dev = NULL;
+fail_register_pad_input:
 	input_unregister_device(input_dev);
-	input_dev = NULL;
-fail2:
 	wacom_wac->input = NULL;
-	wacom_wac->pad_input = NULL;
-fail1:
-	if (input_dev)
-		input_free_device(input_dev);
-	if (pad_input_dev)
-		input_free_device(pad_input_dev);
 	return error;
 }
 
@@ -1101,13 +1209,13 @@ static void wacom_wireless_work(struct work_struct *work)
 	hdev1 = usb_get_intfdata(usbdev->config->interface[1]);
 	wacom1 = hid_get_drvdata(hdev1);
 	wacom_wac1 = &(wacom1->wacom_wac);
-	wacom_unregister_inputs(wacom1);
+	wacom_clean_inputs(wacom1);
 
 	/* Touch interface */
 	hdev2 = usb_get_intfdata(usbdev->config->interface[2]);
 	wacom2 = hid_get_drvdata(hdev2);
 	wacom_wac2 = &(wacom2->wacom_wac);
-	wacom_unregister_inputs(wacom2);
+	wacom_clean_inputs(wacom2);
 
 	if (wacom_wac->pid == 0) {
 		hid_info(wacom->hdev, "wireless tablet disconnected\n");
@@ -1140,7 +1248,9 @@ static void wacom_wireless_work(struct work_struct *work)
 			 wacom_wac1->features.name);
 		wacom_wac1->shared->touch_max = wacom_wac1->features.touch_max;
 		wacom_wac1->shared->type = wacom_wac1->features.type;
-		error = wacom_register_inputs(wacom1);
+		wacom_wac1->pid = wacom_wac->pid;
+		error = wacom_allocate_inputs(wacom1) ||
+			wacom_register_inputs(wacom1);
 		if (error)
 			goto fail;
 
@@ -1160,7 +1270,9 @@ static void wacom_wireless_work(struct work_struct *work)
 					 "%s (WL) Pad",wacom_wac2->features.name);
 			snprintf(wacom_wac2->pad_name, WACOM_NAME_MAX,
 				 "%s (WL) Pad", wacom_wac2->features.name);
-			error = wacom_register_inputs(wacom2);
+			wacom_wac2->pid = wacom_wac->pid;
+			error = wacom_allocate_inputs(wacom2) ||
+				wacom_register_inputs(wacom2);
 			if (error)
 				goto fail;
 
@@ -1177,8 +1289,8 @@ static void wacom_wireless_work(struct work_struct *work)
 	return;
 
 fail:
-	wacom_unregister_inputs(wacom1);
-	wacom_unregister_inputs(wacom2);
+	wacom_clean_inputs(wacom1);
+	wacom_clean_inputs(wacom2);
 	return;
 }
 
@@ -1241,10 +1353,13 @@ static int wacom_probe(struct hid_device *hdev,
 	struct wacom_wac *wacom_wac;
 	struct wacom_features *features;
 	int error;
+	unsigned int connect_mask = HID_CONNECT_HIDRAW;
 
 	if (!id->driver_data)
 		return -EINVAL;
 
+	hdev->quirks |= HID_QUIRK_NO_INIT_REPORTS;
+
 	wacom = kzalloc(sizeof(struct wacom), GFP_KERNEL);
 	if (!wacom)
 		return -ENOMEM;
@@ -1256,7 +1371,7 @@ static int wacom_probe(struct hid_device *hdev,
 	error = hid_parse(hdev);
 	if (error) {
 		hid_err(hdev, "parse failed\n");
-		goto fail1;
+		goto fail_parse;
 	}
 
 	wacom_wac = &wacom->wacom_wac;
@@ -1265,12 +1380,12 @@ static int wacom_probe(struct hid_device *hdev,
 	features->pktlen = wacom_compute_pktlen(hdev);
 	if (features->pktlen > WACOM_PKGLEN_MAX) {
 		error = -EINVAL;
-		goto fail1;
+		goto fail_pktlen;
 	}
 
 	if (features->check_for_hid_type && features->hid_type != hdev->type) {
 		error = -ENODEV;
-		goto fail1;
+		goto fail_type;
 	}
 
 	wacom->usbdev = dev;
@@ -1278,6 +1393,12 @@ static int wacom_probe(struct hid_device *hdev,
 	mutex_init(&wacom->lock);
 	INIT_WORK(&wacom->work, wacom_wireless_work);
 
+	if (!(features->quirks & WACOM_QUIRK_NO_INPUT)) {
+		error = wacom_allocate_inputs(wacom);
+		if (error)
+			goto fail_allocate_inputs;
+	}
+
 	/* set the default size in case we do not get them from hid */
 	wacom_set_default_phy(features);
 
@@ -1339,24 +1460,20 @@ static int wacom_probe(struct hid_device *hdev,
 
 		error = wacom_add_shared_data(hdev);
 		if (error)
-			goto fail1;
+			goto fail_shared_data;
 	}
 
-	error = wacom_initialize_leds(wacom);
-	if (error)
-		goto fail2;
-
 	if (!(features->quirks & WACOM_QUIRK_MONITOR) &&
 	     (features->quirks & WACOM_QUIRK_BATTERY)) {
 		error = wacom_initialize_battery(wacom);
 		if (error)
-			goto fail3;
+			goto fail_battery;
 	}
 
 	if (!(features->quirks & WACOM_QUIRK_NO_INPUT)) {
 		error = wacom_register_inputs(wacom);
 		if (error)
-			goto fail4;
+			goto fail_register_inputs;
 	}
 
 	if (hdev->bus == BUS_BLUETOOTH) {
@@ -1367,16 +1484,19 @@ static int wacom_probe(struct hid_device *hdev,
 				 error);
 	}
 
-	/* Note that if query fails it is not a hard failure */
-	wacom_query_tablet_data(hdev, features);
+	if (features->type == HID_GENERIC)
+		connect_mask |= HID_CONNECT_DRIVER;
 
 	/* Regular HID work starts now */
-	error = hid_hw_start(hdev, HID_CONNECT_HIDRAW);
+	error = hid_hw_start(hdev, connect_mask);
 	if (error) {
 		hid_err(hdev, "hw start failed\n");
-		goto fail5;
+		goto fail_hw_start;
 	}
 
+	/* Note that if query fails it is not a hard failure */
+	wacom_query_tablet_data(hdev, features);
+
 	if (features->quirks & WACOM_QUIRK_MONITOR)
 		error = hid_hw_open(hdev);
 
@@ -1387,13 +1507,21 @@ static int wacom_probe(struct hid_device *hdev,
 
 	return 0;
 
- fail5:	if (hdev->bus == BUS_BLUETOOTH)
+fail_hw_start:
+	if (hdev->bus == BUS_BLUETOOTH)
 		device_remove_file(&hdev->dev, &dev_attr_speed);
-	wacom_unregister_inputs(wacom);
- fail4:	wacom_destroy_battery(wacom);
- fail3:	wacom_destroy_leds(wacom);
- fail2:	wacom_remove_shared_data(wacom_wac);
- fail1:	kfree(wacom);
+fail_register_inputs:
+	wacom_clean_inputs(wacom);
+	wacom_destroy_battery(wacom);
+fail_battery:
+	wacom_remove_shared_data(wacom_wac);
+fail_shared_data:
+	wacom_clean_inputs(wacom);
+fail_allocate_inputs:
+fail_type:
+fail_pktlen:
+fail_parse:
+	kfree(wacom);
 	hid_set_drvdata(hdev, NULL);
 	return error;
 }
@@ -1405,11 +1533,10 @@ static void wacom_remove(struct hid_device *hdev)
 	hid_hw_stop(hdev);
 
 	cancel_work_sync(&wacom->work);
-	wacom_unregister_inputs(wacom);
+	wacom_clean_inputs(wacom);
 	if (hdev->bus == BUS_BLUETOOTH)
 		device_remove_file(&hdev->dev, &dev_attr_speed);
 	wacom_destroy_battery(wacom);
-	wacom_destroy_leds(wacom);
 	wacom_remove_shared_data(&wacom->wacom_wac);
 
 	hid_set_drvdata(hdev, NULL);
@@ -1444,6 +1571,8 @@ static struct hid_driver wacom_driver = {
 	.id_table =	wacom_ids,
 	.probe =	wacom_probe,
 	.remove =	wacom_remove,
+	.event =	wacom_wac_event,
+	.report =	wacom_wac_report,
 #ifdef CONFIG_PM
 	.resume =	wacom_resume,
 	.reset_resume =	wacom_reset_resume,
diff --git a/drivers/hid/wacom_wac.c b/drivers/hid/wacom_wac.c
index aa6a08eb7ad6..586b2405b0d4 100644
--- a/drivers/hid/wacom_wac.c
+++ b/drivers/hid/wacom_wac.c
@@ -1248,6 +1248,296 @@ static int wacom_tpc_irq(struct wacom_wac *wacom, size_t len)
 	return 0;
 }
 
+static void wacom_map_usage(struct wacom *wacom, struct hid_usage *usage,
+		struct hid_field *field, __u8 type, __u16 code, int fuzz)
+{
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->input;
+	int fmin = field->logical_minimum;
+	int fmax = field->logical_maximum;
+
+	usage->type = type;
+	usage->code = code;
+
+	set_bit(type, input->evbit);
+
+	switch (type) {
+	case EV_ABS:
+		input_set_abs_params(input, code, fmin, fmax, fuzz, 0);
+		input_abs_set_res(input, code,
+				  hidinput_calc_abs_res(field, code));
+		break;
+	case EV_KEY:
+		input_set_capability(input, EV_KEY, code);
+		break;
+	case EV_MSC:
+		input_set_capability(input, EV_MSC, code);
+		break;
+	}
+}
+
+static void wacom_wac_pen_usage_mapping(struct hid_device *hdev,
+		struct hid_field *field, struct hid_usage *usage)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+
+	switch (usage->hid) {
+	case HID_GD_X:
+		wacom_map_usage(wacom, usage, field, EV_ABS, ABS_X, 4);
+		break;
+	case HID_GD_Y:
+		wacom_map_usage(wacom, usage, field, EV_ABS, ABS_Y, 4);
+		break;
+	case HID_DG_TIPPRESSURE:
+		wacom_map_usage(wacom, usage, field, EV_ABS, ABS_PRESSURE, 0);
+		break;
+	case HID_DG_INRANGE:
+		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_TOOL_PEN, 0);
+		break;
+	case HID_DG_INVERT:
+		wacom_map_usage(wacom, usage, field, EV_KEY,
+				BTN_TOOL_RUBBER, 0);
+		break;
+	case HID_DG_ERASER:
+	case HID_DG_TIPSWITCH:
+		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_TOUCH, 0);
+		break;
+	case HID_DG_BARRELSWITCH:
+		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_STYLUS, 0);
+		break;
+	case HID_DG_BARRELSWITCH2:
+		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_STYLUS2, 0);
+		break;
+	case HID_DG_TOOLSERIALNUMBER:
+		wacom_map_usage(wacom, usage, field, EV_MSC, MSC_SERIAL, 0);
+		break;
+	}
+}
+
+static int wacom_wac_pen_event(struct hid_device *hdev, struct hid_field *field,
+		struct hid_usage *usage, __s32 value)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->input;
+
+	/* checking which Tool / tip switch to send */
+	switch (usage->hid) {
+	case HID_DG_INRANGE:
+		wacom_wac->hid_data.inrange_state = value;
+		return 0;
+	case HID_DG_INVERT:
+		wacom_wac->hid_data.invert_state = value;
+		return 0;
+	case HID_DG_ERASER:
+	case HID_DG_TIPSWITCH:
+		wacom_wac->hid_data.tipswitch |= value;
+		return 0;
+	}
+
+	/* send pen events only when touch is up or forced out */
+	if (!usage->type || wacom_wac->shared->touch_down)
+		return 0;
+
+	input_event(input, usage->type, usage->code, value);
+
+	return 0;
+}
+
+static void wacom_wac_pen_report(struct hid_device *hdev,
+		struct hid_report *report)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->input;
+	bool prox = wacom_wac->hid_data.inrange_state;
+
+	if (!wacom_wac->shared->stylus_in_proximity) /* first in prox */
+		/* Going into proximity select tool */
+		wacom_wac->tool[0] = wacom_wac->hid_data.invert_state ?
+						BTN_TOOL_RUBBER : BTN_TOOL_PEN;
+
+	/* keep pen state for touch events */
+	wacom_wac->shared->stylus_in_proximity = prox;
+
+	/* send pen events only when touch is up or forced out */
+	if (!wacom_wac->shared->touch_down) {
+		input_report_key(input, BTN_TOUCH,
+				wacom_wac->hid_data.tipswitch);
+		input_report_key(input, wacom_wac->tool[0], prox);
+
+		wacom_wac->hid_data.tipswitch = false;
+
+		input_sync(input);
+	}
+}
+
+static void wacom_wac_finger_usage_mapping(struct hid_device *hdev,
+		struct hid_field *field, struct hid_usage *usage)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->input;
+	unsigned touch_max = wacom_wac->features.touch_max;
+
+	switch (usage->hid) {
+	case HID_GD_X:
+		if (touch_max == 1)
+			wacom_map_usage(wacom, usage, field, EV_ABS, ABS_X, 4);
+		else
+			wacom_map_usage(wacom, usage, field, EV_ABS,
+					ABS_MT_POSITION_X, 4);
+		break;
+	case HID_GD_Y:
+		if (touch_max == 1)
+			wacom_map_usage(wacom, usage, field, EV_ABS, ABS_Y, 4);
+		else
+			wacom_map_usage(wacom, usage, field, EV_ABS,
+					ABS_MT_POSITION_Y, 4);
+		break;
+	case HID_DG_CONTACTID:
+		input_mt_init_slots(input, wacom_wac->features.touch_max,
+			INPUT_MT_DIRECT);
+		break;
+	case HID_DG_INRANGE:
+		break;
+	case HID_DG_INVERT:
+		break;
+	case HID_DG_TIPSWITCH:
+		wacom_map_usage(wacom, usage, field, EV_KEY, BTN_TOUCH, 0);
+		break;
+	}
+}
+
+static int wacom_wac_finger_event(struct hid_device *hdev,
+		struct hid_field *field, struct hid_usage *usage, __s32 value)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+
+	switch (usage->hid) {
+	case HID_GD_X:
+		wacom_wac->hid_data.x = value;
+		break;
+	case HID_GD_Y:
+		wacom_wac->hid_data.y = value;
+		break;
+	case HID_DG_CONTACTID:
+		wacom_wac->hid_data.id = value;
+		break;
+	case HID_DG_TIPSWITCH:
+		wacom_wac->hid_data.tipswitch = value;
+		break;
+	}
+
+
+	return 0;
+}
+
+static void wacom_wac_finger_mt_report(struct wacom_wac *wacom_wac,
+		struct input_dev *input, bool touch)
+{
+	int slot;
+	struct hid_data *hid_data = &wacom_wac->hid_data;
+
+	slot = input_mt_get_slot_by_key(input, hid_data->id);
+
+	input_mt_slot(input, slot);
+	input_mt_report_slot_state(input, MT_TOOL_FINGER, touch);
+	if (touch) {
+		input_report_abs(input, ABS_MT_POSITION_X, hid_data->x);
+		input_report_abs(input, ABS_MT_POSITION_Y, hid_data->y);
+	}
+	input_mt_sync_frame(input);
+}
+
+static void wacom_wac_finger_single_touch_report(struct wacom_wac *wacom_wac,
+		struct input_dev *input, bool touch)
+{
+	struct hid_data *hid_data = &wacom_wac->hid_data;
+
+	if (touch) {
+		input_report_abs(input, ABS_X, hid_data->x);
+		input_report_abs(input, ABS_Y, hid_data->y);
+	}
+	input_report_key(input, BTN_TOUCH, touch);
+}
+
+static void wacom_wac_finger_report(struct hid_device *hdev,
+		struct hid_report *report)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->input;
+	bool touch = wacom_wac->hid_data.tipswitch &&
+		     !wacom_wac->shared->stylus_in_proximity;
+	unsigned touch_max = wacom_wac->features.touch_max;
+
+	if (touch_max > 1)
+		wacom_wac_finger_mt_report(wacom_wac, input, touch);
+	else
+		wacom_wac_finger_single_touch_report(wacom_wac, input, touch);
+	input_sync(input);
+
+	/* keep touch state for pen event */
+	wacom_wac->shared->touch_down = touch;
+}
+
+#define WACOM_PEN_FIELD(f)	(((f)->logical == HID_DG_STYLUS) || \
+				 ((f)->physical == HID_DG_STYLUS))
+#define WACOM_FINGER_FIELD(f)	(((f)->logical == HID_DG_FINGER) || \
+				 ((f)->physical == HID_DG_FINGER))
+
+void wacom_wac_usage_mapping(struct hid_device *hdev,
+		struct hid_field *field, struct hid_usage *usage)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct input_dev *input = wacom_wac->input;
+
+	/* currently, only direct devices have proper hid report descriptors */
+	__set_bit(INPUT_PROP_DIRECT, input->propbit);
+
+	if (WACOM_PEN_FIELD(field))
+		return wacom_wac_pen_usage_mapping(hdev, field, usage);
+
+	if (WACOM_FINGER_FIELD(field))
+		return wacom_wac_finger_usage_mapping(hdev, field, usage);
+}
+
+int wacom_wac_event(struct hid_device *hdev, struct hid_field *field,
+		struct hid_usage *usage, __s32 value)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+
+	if (wacom->wacom_wac.features.type != HID_GENERIC)
+		return 0;
+
+	if (WACOM_PEN_FIELD(field))
+		return wacom_wac_pen_event(hdev, field, usage, value);
+
+	if (WACOM_FINGER_FIELD(field))
+		return wacom_wac_finger_event(hdev, field, usage, value);
+
+	return 0;
+}
+
+void wacom_wac_report(struct hid_device *hdev, struct hid_report *report)
+{
+	struct wacom *wacom = hid_get_drvdata(hdev);
+	struct wacom_wac *wacom_wac = &wacom->wacom_wac;
+	struct hid_field *field = report->field[0];
+
+	if (wacom_wac->features.type != HID_GENERIC)
+		return;
+
+	if (WACOM_PEN_FIELD(field))
+		return wacom_wac_pen_report(hdev, report);
+
+	if (WACOM_FINGER_FIELD(field))
+		return wacom_wac_finger_report(hdev, report);
+}
+
 static int wacom_bpt_touch(struct wacom_wac *wacom)
 {
 	struct wacom_features *features = &wacom->features;
@@ -1746,6 +2036,10 @@ int wacom_setup_input_capabilities(struct input_dev *input_dev,
 
 	input_dev->evbit[0] |= BIT_MASK(EV_KEY) | BIT_MASK(EV_ABS);
 
+	if (features->type == HID_GENERIC)
+		/* setup has already been done */
+		return 0;
+
 	__set_bit(BTN_TOUCH, input_dev->keybit);
 	__set_bit(ABS_MISC, input_dev->absbit);
 
@@ -1990,6 +2284,9 @@ int wacom_setup_pad_input_capabilities(struct input_dev *input_dev,
 	input_set_abs_params(input_dev, ABS_X, 0, 1, 0, 0);
 	input_set_abs_params(input_dev, ABS_Y, 0, 1, 0, 0);
 
+	/* kept for making udev and libwacom accepting the pad */
+	__set_bit(BTN_STYLUS, input_dev->keybit);
+
 	switch (features->type) {
 	case GRAPHIRE_BT:
 		__set_bit(BTN_0, input_dev->keybit);
@@ -2573,6 +2870,17 @@ static const struct wacom_features wacom_features_0x309 =
 	{ "Wacom ISDv5 309", .type = WACOM_24HDT, /* Touch */
 	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x0307, .touch_max = 10,
 	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
+static const struct wacom_features wacom_features_0x30A =
+	{ "Wacom ISDv5 30A", 59352, 33648, 2047, 63,
+	  CINTIQ_HYBRID, WACOM_INTUOS3_RES, WACOM_INTUOS3_RES, 200, 200,
+	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x30C };
+static const struct wacom_features wacom_features_0x30C =
+	{ "Wacom ISDv5 30C", .type = WACOM_24HDT, /* Touch */
+	  .oVid = USB_VENDOR_ID_WACOM, .oPid = 0x30A, .touch_max = 10,
+	  .check_for_hid_type = true, .hid_type = HID_TYPE_USBNONE };
+
+static const struct wacom_features wacom_features_HID_ANY_ID =
+	{ "Wacom HID", .type = HID_GENERIC };
 
 #define USB_DEVICE_WACOM(prod)						\
 	HID_DEVICE(BUS_USB, HID_GROUP_WACOM, USB_VENDOR_ID_WACOM, prod),\
@@ -2708,6 +3016,8 @@ const struct hid_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x304) },
 	{ USB_DEVICE_WACOM(0x307) },
 	{ USB_DEVICE_WACOM(0x309) },
+	{ USB_DEVICE_WACOM(0x30A) },
+	{ USB_DEVICE_WACOM(0x30C) },
 	{ USB_DEVICE_WACOM(0x30E) },
 	{ USB_DEVICE_WACOM(0x314) },
 	{ USB_DEVICE_WACOM(0x315) },
@@ -2716,6 +3026,8 @@ const struct hid_device_id wacom_ids[] = {
 	{ USB_DEVICE_WACOM(0x4004) },
 	{ USB_DEVICE_WACOM(0x5000) },
 	{ USB_DEVICE_WACOM(0x5002) },
+
+	{ USB_DEVICE_WACOM(HID_ANY_ID) },
 	{ }
 };
 MODULE_DEVICE_TABLE(hid, wacom_ids);
diff --git a/drivers/hid/wacom_wac.h b/drivers/hid/wacom_wac.h
index 339ab5d81a2d..0f0b85ec1322 100644
--- a/drivers/hid/wacom_wac.h
+++ b/drivers/hid/wacom_wac.h
@@ -113,6 +113,7 @@ enum {
 	MTSCREEN,
 	MTTPC,
 	MTTPC_B,
+	HID_GENERIC,
 	MAX_TYPE
 };
 
@@ -154,6 +155,20 @@ struct wacom_shared {
 	struct input_dev *touch_input;
 };
 
+struct hid_data {
+	__s16 inputmode;	/* InputMode HID feature, -1 if non-existent */
+	__s16 inputmode_index;	/* InputMode HID feature index in the report */
+	bool inrange_state;
+	bool invert_state;
+	bool tipswitch;
+	int x;
+	int y;
+	int pressure;
+	int width;
+	int height;
+	int id;
+};
+
 struct wacom_wac {
 	char name[WACOM_NAME_MAX];
 	char pad_name[WACOM_NAME_MAX];
@@ -167,6 +182,7 @@ struct wacom_wac {
 	struct wacom_shared *shared;
 	struct input_dev *input;
 	struct input_dev *pad_input;
+	bool input_registered;
 	int pid;
 	int battery_capacity;
 	int num_contacts_left;
@@ -174,6 +190,7 @@ struct wacom_wac {
 	int ps_connected;
 	u8 bt_features;
 	u8 bt_high_speed;
+	struct hid_data hid_data;
 };
 
 #endif
diff --git a/drivers/hv/channel.c b/drivers/hv/channel.c
index 531a593912ec..433f72a1c006 100644
--- a/drivers/hv/channel.c
+++ b/drivers/hv/channel.c
@@ -165,8 +165,10 @@ int vmbus_open(struct vmbus_channel *newchannel, u32 send_ringbuffer_size,
 	ret = vmbus_post_msg(open_msg,
 			       sizeof(struct vmbus_channel_open_channel));
 
-	if (ret != 0)
+	if (ret != 0) {
+		err = ret;
 		goto error1;
+	}
 
 	t = wait_for_completion_timeout(&open_info->waitevent, 5*HZ);
 	if (t == 0) {
@@ -363,7 +365,6 @@ int vmbus_establish_gpadl(struct vmbus_channel *channel, void *kbuffer,
 	u32 next_gpadl_handle;
 	unsigned long flags;
 	int ret = 0;
-	int t;
 
 	next_gpadl_handle = atomic_read(&vmbus_connection.next_gpadl_handle);
 	atomic_inc(&vmbus_connection.next_gpadl_handle);
@@ -410,9 +411,7 @@ int vmbus_establish_gpadl(struct vmbus_channel *channel, void *kbuffer,
 
 		}
 	}
-	t = wait_for_completion_timeout(&msginfo->waitevent, 5*HZ);
-	BUG_ON(t == 0);
-
+	wait_for_completion(&msginfo->waitevent);
 
 	/* At this point, we received the gpadl created msg */
 	*gpadl_handle = gpadlmsg->gpadl;
@@ -435,7 +434,7 @@ int vmbus_teardown_gpadl(struct vmbus_channel *channel, u32 gpadl_handle)
 	struct vmbus_channel_gpadl_teardown *msg;
 	struct vmbus_channel_msginfo *info;
 	unsigned long flags;
-	int ret, t;
+	int ret;
 
 	info = kmalloc(sizeof(*info) +
 		       sizeof(struct vmbus_channel_gpadl_teardown), GFP_KERNEL);
@@ -457,11 +456,12 @@ int vmbus_teardown_gpadl(struct vmbus_channel *channel, u32 gpadl_handle)
 	ret = vmbus_post_msg(msg,
 			       sizeof(struct vmbus_channel_gpadl_teardown));
 
-	BUG_ON(ret != 0);
-	t = wait_for_completion_timeout(&info->waitevent, 5*HZ);
-	BUG_ON(t == 0);
+	if (ret)
+		goto post_msg_err;
+
+	wait_for_completion(&info->waitevent);
 
-	/* Received a torndown response */
+post_msg_err:
 	spin_lock_irqsave(&vmbus_connection.channelmsg_lock, flags);
 	list_del(&info->msglistentry);
 	spin_unlock_irqrestore(&vmbus_connection.channelmsg_lock, flags);
@@ -478,7 +478,7 @@ static void reset_channel_cb(void *arg)
 	channel->onchannel_callback = NULL;
 }
 
-static void vmbus_close_internal(struct vmbus_channel *channel)
+static int vmbus_close_internal(struct vmbus_channel *channel)
 {
 	struct vmbus_channel_close_channel *msg;
 	int ret;
@@ -486,11 +486,14 @@ static void vmbus_close_internal(struct vmbus_channel *channel)
 	channel->state = CHANNEL_OPEN_STATE;
 	channel->sc_creation_callback = NULL;
 	/* Stop callback and cancel the timer asap */
-	if (channel->target_cpu != smp_processor_id())
+	if (channel->target_cpu != get_cpu()) {
+		put_cpu();
 		smp_call_function_single(channel->target_cpu, reset_channel_cb,
 					 channel, true);
-	else
+	} else {
 		reset_channel_cb(channel);
+		put_cpu();
+	}
 
 	/* Send a closing message */
 
@@ -501,11 +504,28 @@ static void vmbus_close_internal(struct vmbus_channel *channel)
 
 	ret = vmbus_post_msg(msg, sizeof(struct vmbus_channel_close_channel));
 
-	BUG_ON(ret != 0);
+	if (ret) {
+		pr_err("Close failed: close post msg return is %d\n", ret);
+		/*
+		 * If we failed to post the close msg,
+		 * it is perhaps better to leak memory.
+		 */
+		return ret;
+	}
+
 	/* Tear down the gpadl for the channel's ring buffer */
-	if (channel->ringbuffer_gpadlhandle)
-		vmbus_teardown_gpadl(channel,
-					  channel->ringbuffer_gpadlhandle);
+	if (channel->ringbuffer_gpadlhandle) {
+		ret = vmbus_teardown_gpadl(channel,
+					   channel->ringbuffer_gpadlhandle);
+		if (ret) {
+			pr_err("Close failed: teardown gpadl return %d\n", ret);
+			/*
+			 * If we failed to teardown gpadl,
+			 * it is perhaps better to leak memory.
+			 */
+			return ret;
+		}
+	}
 
 	/* Cleanup the ring buffers for this channel */
 	hv_ringbuffer_cleanup(&channel->outbound);
@@ -514,7 +534,7 @@ static void vmbus_close_internal(struct vmbus_channel *channel)
 	free_pages((unsigned long)channel->ringbuffer_pages,
 		get_order(channel->ringbuffer_pagecount * PAGE_SIZE));
 
-
+	return ret;
 }
 
 /*
diff --git a/drivers/hv/channel_mgmt.c b/drivers/hv/channel_mgmt.c
index ed9350d42764..a2d1a9612c86 100644
--- a/drivers/hv/channel_mgmt.c
+++ b/drivers/hv/channel_mgmt.c
@@ -224,11 +224,14 @@ static void vmbus_process_rescind_offer(struct work_struct *work)
 	msg.header.msgtype = CHANNELMSG_RELID_RELEASED;
 	vmbus_post_msg(&msg, sizeof(struct vmbus_channel_relid_released));
 
-	if (channel->target_cpu != smp_processor_id())
+	if (channel->target_cpu != get_cpu()) {
+		put_cpu();
 		smp_call_function_single(channel->target_cpu,
 					 percpu_channel_deq, channel, true);
-	else
+	} else {
 		percpu_channel_deq(channel);
+		put_cpu();
+	}
 
 	if (channel->primary_channel == NULL) {
 		spin_lock_irqsave(&vmbus_connection.channel_lock, flags);
@@ -294,12 +297,15 @@ static void vmbus_process_offer(struct work_struct *work)
 	spin_unlock_irqrestore(&vmbus_connection.channel_lock, flags);
 
 	if (enq) {
-		if (newchannel->target_cpu != smp_processor_id())
+		if (newchannel->target_cpu != get_cpu()) {
+			put_cpu();
 			smp_call_function_single(newchannel->target_cpu,
 						 percpu_channel_enq,
 						 newchannel, true);
-		else
+		} else {
 			percpu_channel_enq(newchannel);
+			put_cpu();
+		}
 	}
 	if (!fnew) {
 		/*
@@ -314,12 +320,15 @@ static void vmbus_process_offer(struct work_struct *work)
 			list_add_tail(&newchannel->sc_list, &channel->sc_list);
 			spin_unlock_irqrestore(&channel->sc_lock, flags);
 
-			if (newchannel->target_cpu != smp_processor_id())
+			if (newchannel->target_cpu != get_cpu()) {
+				put_cpu();
 				smp_call_function_single(newchannel->target_cpu,
 							 percpu_channel_enq,
 							 newchannel, true);
-			else
+			} else {
 				percpu_channel_enq(newchannel);
+				put_cpu();
+			}
 
 			newchannel->state = CHANNEL_OPEN_STATE;
 			if (channel->sc_creation_callback != NULL)
diff --git a/drivers/hv/connection.c b/drivers/hv/connection.c
index ae22e3c1fc4c..e206619b946e 100644
--- a/drivers/hv/connection.c
+++ b/drivers/hv/connection.c
@@ -427,10 +427,21 @@ int vmbus_post_msg(void *buffer, size_t buflen)
 	 * insufficient resources. Retry the operation a couple of
 	 * times before giving up.
 	 */
-	while (retries < 3) {
-		ret =  hv_post_message(conn_id, 1, buffer, buflen);
-		if (ret != HV_STATUS_INSUFFICIENT_BUFFERS)
+	while (retries < 10) {
+		ret = hv_post_message(conn_id, 1, buffer, buflen);
+
+		switch (ret) {
+		case HV_STATUS_INSUFFICIENT_BUFFERS:
+			ret = -ENOMEM;
+		case -ENOMEM:
+			break;
+		case HV_STATUS_SUCCESS:
 			return ret;
+		default:
+			pr_err("hv_post_msg() failed; error code:%d\n", ret);
+			return -EINVAL;
+		}
+
 		retries++;
 		msleep(100);
 	}
diff --git a/drivers/hv/hv.c b/drivers/hv/hv.c
index edfc8488cb03..3e4235c7a47f 100644
--- a/drivers/hv/hv.c
+++ b/drivers/hv/hv.c
@@ -138,6 +138,8 @@ int hv_init(void)
 	memset(hv_context.synic_event_page, 0, sizeof(void *) * NR_CPUS);
 	memset(hv_context.synic_message_page, 0,
 	       sizeof(void *) * NR_CPUS);
+	memset(hv_context.post_msg_page, 0,
+	       sizeof(void *) * NR_CPUS);
 	memset(hv_context.vp_index, 0,
 	       sizeof(int) * NR_CPUS);
 	memset(hv_context.event_dpc, 0,
@@ -217,26 +219,18 @@ int hv_post_message(union hv_connection_id connection_id,
 		  enum hv_message_type message_type,
 		  void *payload, size_t payload_size)
 {
-	struct aligned_input {
-		u64 alignment8;
-		struct hv_input_post_message msg;
-	};
 
 	struct hv_input_post_message *aligned_msg;
 	u16 status;
-	unsigned long addr;
 
 	if (payload_size > HV_MESSAGE_PAYLOAD_BYTE_COUNT)
 		return -EMSGSIZE;
 
-	addr = (unsigned long)kmalloc(sizeof(struct aligned_input), GFP_ATOMIC);
-	if (!addr)
-		return -ENOMEM;
-
 	aligned_msg = (struct hv_input_post_message *)
-			(ALIGN(addr, HV_HYPERCALL_PARAM_ALIGN));
+			hv_context.post_msg_page[get_cpu()];
 
 	aligned_msg->connectionid = connection_id;
+	aligned_msg->reserved = 0;
 	aligned_msg->message_type = message_type;
 	aligned_msg->payload_size = payload_size;
 	memcpy((void *)aligned_msg->payload, payload, payload_size);
@@ -244,8 +238,7 @@ int hv_post_message(union hv_connection_id connection_id,
 	status = do_hypercall(HVCALL_POST_MESSAGE, aligned_msg, NULL)
 		& 0xFFFF;
 
-	kfree((void *)addr);
-
+	put_cpu();
 	return status;
 }
 
@@ -294,6 +287,14 @@ int hv_synic_alloc(void)
 			pr_err("Unable to allocate SYNIC event page\n");
 			goto err;
 		}
+
+		hv_context.post_msg_page[cpu] =
+			(void *)get_zeroed_page(GFP_ATOMIC);
+
+		if (hv_context.post_msg_page[cpu] == NULL) {
+			pr_err("Unable to allocate post msg page\n");
+			goto err;
+		}
 	}
 
 	return 0;
@@ -308,6 +309,8 @@ static void hv_synic_free_cpu(int cpu)
 		free_page((unsigned long)hv_context.synic_event_page[cpu]);
 	if (hv_context.synic_message_page[cpu])
 		free_page((unsigned long)hv_context.synic_message_page[cpu]);
+	if (hv_context.post_msg_page[cpu])
+		free_page((unsigned long)hv_context.post_msg_page[cpu]);
 }
 
 void hv_synic_free(void)
diff --git a/drivers/hv/hyperv_vmbus.h b/drivers/hv/hyperv_vmbus.h
index 22b750749a39..c386d8dc7223 100644
--- a/drivers/hv/hyperv_vmbus.h
+++ b/drivers/hv/hyperv_vmbus.h
@@ -515,6 +515,10 @@ struct hv_context {
 	 * per-cpu list of the channels based on their CPU affinity.
 	 */
 	struct list_head percpu_list[NR_CPUS];
+	/*
+	 * buffer to post messages to the host.
+	 */
+	void *post_msg_page[NR_CPUS];
 };
 
 extern struct hv_context hv_context;
diff --git a/drivers/hv/ring_buffer.c b/drivers/hv/ring_buffer.c
index 15db66b74141..6361d124f67d 100644
--- a/drivers/hv/ring_buffer.c
+++ b/drivers/hv/ring_buffer.c
@@ -361,6 +361,11 @@ int hv_ringbuffer_init(struct hv_ring_buffer_info *ring_info,
 	ring_info->ring_buffer->read_index =
 		ring_info->ring_buffer->write_index = 0;
 
+	/*
+	 * Set the feature bit for enabling flow control.
+	 */
+	ring_info->ring_buffer->feature_bits.value = 1;
+
 	ring_info->ring_size = buflen;
 	ring_info->ring_datasize = buflen - sizeof(struct hv_ring_buffer);
 
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index f00d048aa583..5286d7ce1f9e 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -280,8 +280,8 @@ config SENSORS_K10TEMP
 	  If you say yes here you get support for the temperature
 	  sensor(s) inside your CPU. Supported are later revisions of
 	  the AMD Family 10h and all revisions of the AMD Family 11h,
-	  12h (Llano), 14h (Brazos), 15h (Bulldozer/Trinity/Kaveri) and
-	  16h (Kabini/Mullins) microarchitectures.
+	  12h (Llano), 14h (Brazos), 15h (Bulldozer/Trinity/Kaveri/Carrizo)
+	  and 16h (Kabini/Mullins) microarchitectures.
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called k10temp.
@@ -839,6 +839,16 @@ config SENSORS_MCP3021
 	  This driver can also be built as a module.  If so, the module
 	  will be called mcp3021.
 
+config SENSORS_MENF21BMC_HWMON
+	tristate "MEN 14F021P00 BMC Hardware Monitoring"
+	depends on MFD_MENF21BMC
+	help
+	  Say Y here to include support for the MEN 14F021P00 BMC
+	  hardware monitoring.
+
+	  This driver can also be built as a module. If so the module
+	  will be called menf21bmc_hwmon.
+
 config SENSORS_ADCXX
 	tristate "National Semiconductor ADCxxxSxxx"
 	depends on SPI_MASTER
@@ -1077,6 +1087,7 @@ config SENSORS_PC87427
 config SENSORS_NTC_THERMISTOR
 	tristate "NTC thermistor support from Murata"
 	depends on !OF || IIO=n || IIO
+	depends on THERMAL || !THERMAL_OF
 	help
 	  This driver supports NTC thermistors sensor reading and its
 	  interpretation. The driver can also monitor the temperature and
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index be28152c9848..c90a7611efaa 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -115,6 +115,7 @@ obj-$(CONFIG_SENSORS_MAX6650)	+= max6650.o
 obj-$(CONFIG_SENSORS_MAX6697)	+= max6697.o
 obj-$(CONFIG_SENSORS_MC13783_ADC)+= mc13783-adc.o
 obj-$(CONFIG_SENSORS_MCP3021)	+= mcp3021.o
+obj-$(CONFIG_SENSORS_MENF21BMC_HWMON) += menf21bmc_hwmon.o
 obj-$(CONFIG_SENSORS_NCT6683)	+= nct6683.o
 obj-$(CONFIG_SENSORS_NCT6775)	+= nct6775.o
 obj-$(CONFIG_SENSORS_NTC_THERMISTOR)	+= ntc_thermistor.o
diff --git a/drivers/hwmon/ab8500.c b/drivers/hwmon/ab8500.c
index d844dc806853..8b6a4f4c8774 100644
--- a/drivers/hwmon/ab8500.c
+++ b/drivers/hwmon/ab8500.c
@@ -6,7 +6,7 @@
  *
  * When the AB8500 thermal warning temperature is reached (threshold cannot
  * be changed by SW), an interrupt is set, and if no further action is taken
- * within a certain time frame, pm_power off will be called.
+ * within a certain time frame, kernel_power_off will be called.
  *
  * When AB8500 thermal shutdown temperature is reached a hardware shutdown of
  * the AB8500 will occur.
@@ -21,6 +21,7 @@
 #include <linux/module.h>
 #include <linux/platform_device.h>
 #include <linux/power/ab8500.h>
+#include <linux/reboot.h>
 #include <linux/slab.h>
 #include <linux/sysfs.h>
 #include "abx500.h"
@@ -106,7 +107,7 @@ static void ab8500_thermal_power_off(struct work_struct *work)
 
 	dev_warn(&abx500_data->pdev->dev, "Power off due to critical temp\n");
 
-	pm_power_off();
+	kernel_power_off();
 }
 
 static ssize_t ab8500_show_name(struct device *dev,
diff --git a/drivers/hwmon/ads1015.c b/drivers/hwmon/ads1015.c
index 126516414c11..f155b8380481 100644
--- a/drivers/hwmon/ads1015.c
+++ b/drivers/hwmon/ads1015.c
@@ -184,20 +184,18 @@ static int ads1015_get_channels_config_of(struct i2c_client *client)
 		return -EINVAL;
 
 	for_each_child_of_node(client->dev.of_node, node) {
-		const __be32 *property;
-		int len;
+		u32 pval;
 		unsigned int channel;
 		unsigned int pga = ADS1015_DEFAULT_PGA;
 		unsigned int data_rate = ADS1015_DEFAULT_DATA_RATE;
 
-		property = of_get_property(node, "reg", &len);
-		if (!property || len != sizeof(int)) {
+		if (of_property_read_u32(node, "reg", &pval)) {
 			dev_err(&client->dev, "invalid reg on %s\n",
 				node->full_name);
 			continue;
 		}
 
-		channel = be32_to_cpup(property);
+		channel = pval;
 		if (channel >= ADS1015_CHANNELS) {
 			dev_err(&client->dev,
 				"invalid channel index %d on %s\n",
@@ -205,20 +203,17 @@ static int ads1015_get_channels_config_of(struct i2c_client *client)
 			continue;
 		}
 
-		property = of_get_property(node, "ti,gain", &len);
-		if (property && len == sizeof(int)) {
-			pga = be32_to_cpup(property);
+		if (!of_property_read_u32(node, "ti,gain", &pval)) {
+			pga = pval;
 			if (pga > 6) {
-				dev_err(&client->dev,
-					"invalid gain on %s\n",
+				dev_err(&client->dev, "invalid gain on %s\n",
 					node->full_name);
 				return -EINVAL;
 			}
 		}
 
-		property = of_get_property(node, "ti,datarate", &len);
-		if (property && len == sizeof(int)) {
-			data_rate = be32_to_cpup(property);
+		if (!of_property_read_u32(node, "ti,datarate", &pval)) {
+			data_rate = pval;
 			if (data_rate > 7) {
 				dev_err(&client->dev,
 					"invalid data_rate on %s\n",
diff --git a/drivers/hwmon/da9052-hwmon.c b/drivers/hwmon/da9052-hwmon.c
index d14ab3c45daa..692b3f34d88c 100644
--- a/drivers/hwmon/da9052-hwmon.c
+++ b/drivers/hwmon/da9052-hwmon.c
@@ -26,7 +26,6 @@
 
 struct da9052_hwmon {
 	struct da9052	*da9052;
-	struct device	*class_device;
 	struct mutex	hwmon_lock;
 };
 
@@ -190,13 +189,6 @@ static ssize_t da9052_read_vbbat(struct device *dev,
 	return sprintf(buf, "%d\n", vbbat_reg_to_mv(ret));
 }
 
-static ssize_t da9052_hwmon_show_name(struct device *dev,
-				      struct device_attribute *devattr,
-				      char *buf)
-{
-	return sprintf(buf, "da9052\n");
-}
-
 static ssize_t show_label(struct device *dev,
 			  struct device_attribute *devattr, char *buf)
 {
@@ -243,10 +235,7 @@ static SENSOR_DEVICE_ATTR(temp8_input, S_IRUGO, da9052_read_tjunc, NULL,
 static SENSOR_DEVICE_ATTR(temp8_label, S_IRUGO, show_label, NULL,
 			  DA9052_ADC_TJUNC);
 
-static DEVICE_ATTR(name, S_IRUGO, da9052_hwmon_show_name, NULL);
-
-static struct attribute *da9052_attr[] = {
-	&dev_attr_name.attr,
+static struct attribute *da9052_attrs[] = {
 	&sensor_dev_attr_in0_input.dev_attr.attr,
 	&sensor_dev_attr_in0_label.dev_attr.attr,
 	&sensor_dev_attr_in3_input.dev_attr.attr,
@@ -268,54 +257,29 @@ static struct attribute *da9052_attr[] = {
 	NULL
 };
 
-static const struct attribute_group da9052_attr_group = {.attrs = da9052_attr};
+ATTRIBUTE_GROUPS(da9052);
 
 static int da9052_hwmon_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct da9052_hwmon *hwmon;
-	int ret;
+	struct device *hwmon_dev;
 
-	hwmon = devm_kzalloc(&pdev->dev, sizeof(struct da9052_hwmon),
-			     GFP_KERNEL);
+	hwmon = devm_kzalloc(dev, sizeof(struct da9052_hwmon), GFP_KERNEL);
 	if (!hwmon)
 		return -ENOMEM;
 
 	mutex_init(&hwmon->hwmon_lock);
 	hwmon->da9052 = dev_get_drvdata(pdev->dev.parent);
 
-	platform_set_drvdata(pdev, hwmon);
-
-	ret = sysfs_create_group(&pdev->dev.kobj, &da9052_attr_group);
-	if (ret)
-		goto err_mem;
-
-	hwmon->class_device = hwmon_device_register(&pdev->dev);
-	if (IS_ERR(hwmon->class_device)) {
-		ret = PTR_ERR(hwmon->class_device);
-		goto err_sysfs;
-	}
-
-	return 0;
-
-err_sysfs:
-	sysfs_remove_group(&pdev->dev.kobj, &da9052_attr_group);
-err_mem:
-	return ret;
-}
-
-static int da9052_hwmon_remove(struct platform_device *pdev)
-{
-	struct da9052_hwmon *hwmon = platform_get_drvdata(pdev);
-
-	hwmon_device_unregister(hwmon->class_device);
-	sysfs_remove_group(&pdev->dev.kobj, &da9052_attr_group);
-
-	return 0;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, "da9052",
+							   hwmon,
+							   da9052_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 static struct platform_driver da9052_hwmon_driver = {
 	.probe = da9052_hwmon_probe,
-	.remove = da9052_hwmon_remove,
 	.driver = {
 		.name = "da9052-hwmon",
 		.owner = THIS_MODULE,
diff --git a/drivers/hwmon/da9055-hwmon.c b/drivers/hwmon/da9055-hwmon.c
index 35eb7738d711..9916a3fb4bb9 100644
--- a/drivers/hwmon/da9055-hwmon.c
+++ b/drivers/hwmon/da9055-hwmon.c
@@ -36,7 +36,6 @@
 
 struct da9055_hwmon {
 	struct da9055	*da9055;
-	struct device	*class_device;
 	struct mutex	hwmon_lock;
 	struct mutex	irq_lock;
 	struct completion done;
@@ -200,13 +199,6 @@ static ssize_t da9055_read_tjunc(struct device *dev,
 							+ 3076332, 10000));
 }
 
-static ssize_t da9055_hwmon_show_name(struct device *dev,
-				      struct device_attribute *devattr,
-				      char *buf)
-{
-	return sprintf(buf, "da9055\n");
-}
-
 static ssize_t show_label(struct device *dev,
 			  struct device_attribute *devattr, char *buf)
 {
@@ -236,10 +228,7 @@ static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, da9055_read_tjunc, NULL,
 static SENSOR_DEVICE_ATTR(temp1_label, S_IRUGO, show_label, NULL,
 			  DA9055_ADC_TJUNC);
 
-static DEVICE_ATTR(name, S_IRUGO, da9055_hwmon_show_name, NULL);
-
-static struct attribute *da9055_attr[] = {
-	&dev_attr_name.attr,
+static struct attribute *da9055_attrs[] = {
 	&sensor_dev_attr_in0_input.dev_attr.attr,
 	&sensor_dev_attr_in0_label.dev_attr.attr,
 	&sensor_dev_attr_in1_input.dev_attr.attr,
@@ -254,15 +243,16 @@ static struct attribute *da9055_attr[] = {
 	NULL
 };
 
-static const struct attribute_group da9055_attr_group = {.attrs = da9055_attr};
+ATTRIBUTE_GROUPS(da9055);
 
 static int da9055_hwmon_probe(struct platform_device *pdev)
 {
+	struct device *dev = &pdev->dev;
 	struct da9055_hwmon *hwmon;
+	struct device *hwmon_dev;
 	int hwmon_irq, ret;
 
-	hwmon = devm_kzalloc(&pdev->dev, sizeof(struct da9055_hwmon),
-			     GFP_KERNEL);
+	hwmon = devm_kzalloc(dev, sizeof(struct da9055_hwmon), GFP_KERNEL);
 	if (!hwmon)
 		return -ENOMEM;
 
@@ -272,8 +262,6 @@ static int da9055_hwmon_probe(struct platform_device *pdev)
 	init_completion(&hwmon->done);
 	hwmon->da9055 = dev_get_drvdata(pdev->dev.parent);
 
-	platform_set_drvdata(pdev, hwmon);
-
 	hwmon_irq = platform_get_irq_byname(pdev, "HWMON");
 	if (hwmon_irq < 0)
 		return hwmon_irq;
@@ -288,36 +276,14 @@ static int da9055_hwmon_probe(struct platform_device *pdev)
 		return ret;
 	}
 
-	ret = sysfs_create_group(&pdev->dev.kobj, &da9055_attr_group);
-	if (ret)
-		return ret;
-
-	hwmon->class_device = hwmon_device_register(&pdev->dev);
-	if (IS_ERR(hwmon->class_device)) {
-		ret = PTR_ERR(hwmon->class_device);
-		goto err;
-	}
-
-	return 0;
-
-err:
-	sysfs_remove_group(&pdev->dev.kobj, &da9055_attr_group);
-	return ret;
-}
-
-static int da9055_hwmon_remove(struct platform_device *pdev)
-{
-	struct da9055_hwmon *hwmon = platform_get_drvdata(pdev);
-
-	sysfs_remove_group(&pdev->dev.kobj, &da9055_attr_group);
-	hwmon_device_unregister(hwmon->class_device);
-
-	return 0;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, "da9055",
+							   hwmon,
+							   da9055_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 static struct platform_driver da9055_hwmon_driver = {
 	.probe = da9055_hwmon_probe,
-	.remove = da9055_hwmon_remove,
 	.driver = {
 		.name = "da9055-hwmon",
 		.owner = THIS_MODULE,
diff --git a/drivers/hwmon/ds1621.c b/drivers/hwmon/ds1621.c
index fc6f5d54e7f7..8890870309e4 100644
--- a/drivers/hwmon/ds1621.c
+++ b/drivers/hwmon/ds1621.c
@@ -309,6 +309,7 @@ static ssize_t set_convrate(struct device *dev, struct device_attribute *da,
 	data->conf |= (resol << DS1621_REG_CONFIG_RESOL_SHIFT);
 	i2c_smbus_write_byte_data(client, DS1621_REG_CONF, data->conf);
 	data->update_interval = ds1721_convrates[resol];
+	data->zbits = 7 - resol;
 	mutex_unlock(&data->update_lock);
 
 	return count;
diff --git a/drivers/hwmon/fam15h_power.c b/drivers/hwmon/fam15h_power.c
index 4a7cbfad1d74..fcdbde4ec692 100644
--- a/drivers/hwmon/fam15h_power.c
+++ b/drivers/hwmon/fam15h_power.c
@@ -93,13 +93,29 @@ static ssize_t show_power_crit(struct device *dev,
 }
 static DEVICE_ATTR(power1_crit, S_IRUGO, show_power_crit, NULL);
 
+static umode_t fam15h_power_is_visible(struct kobject *kobj,
+				       struct attribute *attr,
+				       int index)
+{
+	/* power1_input is only reported for Fam15h, Models 00h-0fh */
+	if (attr == &dev_attr_power1_input.attr &&
+	   (boot_cpu_data.x86 != 0x15 || boot_cpu_data.x86_model > 0xf))
+		return 0;
+
+	return attr->mode;
+}
+
 static struct attribute *fam15h_power_attrs[] = {
 	&dev_attr_power1_input.attr,
 	&dev_attr_power1_crit.attr,
 	NULL
 };
 
-ATTRIBUTE_GROUPS(fam15h_power);
+static const struct attribute_group fam15h_power_group = {
+	.attrs = fam15h_power_attrs,
+	.is_visible = fam15h_power_is_visible,
+};
+__ATTRIBUTE_GROUPS(fam15h_power);
 
 static bool fam15h_power_is_internal_node0(struct pci_dev *f4)
 {
@@ -216,7 +232,9 @@ static int fam15h_power_probe(struct pci_dev *pdev,
 
 static const struct pci_device_id fam15h_power_id_table[] = {
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_NB_F4) },
+	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M30H_NB_F4) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_16H_NB_F4) },
+	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_16H_M30H_NB_F3) },
 	{}
 };
 MODULE_DEVICE_TABLE(pci, fam15h_power_id_table);
diff --git a/drivers/hwmon/k10temp.c b/drivers/hwmon/k10temp.c
index f7b46f68ef43..1e7bdcdcb295 100644
--- a/drivers/hwmon/k10temp.c
+++ b/drivers/hwmon/k10temp.c
@@ -33,6 +33,9 @@ static bool force;
 module_param(force, bool, 0444);
 MODULE_PARM_DESC(force, "force loading on processors with erratum 319");
 
+/* Provide lock for writing to NB_SMU_IND_ADDR */
+static DEFINE_MUTEX(nb_smu_ind_mutex);
+
 /* CPUID function 0x80000001, ebx */
 #define CPUID_PKGTYPE_MASK	0xf0000000
 #define CPUID_PKGTYPE_F		0x00000000
@@ -51,13 +54,38 @@ MODULE_PARM_DESC(force, "force loading on processors with erratum 319");
 #define REG_NORTHBRIDGE_CAPABILITIES	0xe8
 #define  NB_CAP_HTC			0x00000400
 
+/*
+ * For F15h M60h, functionality of REG_REPORTED_TEMPERATURE
+ * has been moved to D0F0xBC_xD820_0CA4 [Reported Temperature
+ * Control]
+ */
+#define F15H_M60H_REPORTED_TEMP_CTRL_OFFSET	0xd8200ca4
+#define PCI_DEVICE_ID_AMD_15H_M60H_NB_F3	0x1573
+
+static void amd_nb_smu_index_read(struct pci_dev *pdev, unsigned int devfn,
+				  int offset, u32 *val)
+{
+	mutex_lock(&nb_smu_ind_mutex);
+	pci_bus_write_config_dword(pdev->bus, devfn,
+				   0xb8, offset);
+	pci_bus_read_config_dword(pdev->bus, devfn,
+				  0xbc, val);
+	mutex_unlock(&nb_smu_ind_mutex);
+}
+
 static ssize_t show_temp(struct device *dev,
 			 struct device_attribute *attr, char *buf)
 {
 	u32 regval;
-
-	pci_read_config_dword(to_pci_dev(dev),
-			      REG_REPORTED_TEMPERATURE, &regval);
+	struct pci_dev *pdev = dev_get_drvdata(dev);
+
+	if (boot_cpu_data.x86 == 0x15 && boot_cpu_data.x86_model == 0x60) {
+		amd_nb_smu_index_read(pdev, PCI_DEVFN(0, 0),
+				      F15H_M60H_REPORTED_TEMP_CTRL_OFFSET,
+				      &regval);
+	} else {
+		pci_read_config_dword(pdev, REG_REPORTED_TEMPERATURE, &regval);
+	}
 	return sprintf(buf, "%u\n", (regval >> 21) * 125);
 }
 
@@ -75,7 +103,7 @@ static ssize_t show_temp_crit(struct device *dev,
 	u32 regval;
 	int value;
 
-	pci_read_config_dword(to_pci_dev(dev),
+	pci_read_config_dword(dev_get_drvdata(dev),
 			      REG_HARDWARE_THERMAL_CONTROL, &regval);
 	value = ((regval >> 16) & 0x7f) * 500 + 52000;
 	if (show_hyst)
@@ -83,17 +111,43 @@ static ssize_t show_temp_crit(struct device *dev,
 	return sprintf(buf, "%d\n", value);
 }
 
-static ssize_t show_name(struct device *dev,
-			 struct device_attribute *attr, char *buf)
-{
-	return sprintf(buf, "k10temp\n");
-}
-
 static DEVICE_ATTR(temp1_input, S_IRUGO, show_temp, NULL);
 static DEVICE_ATTR(temp1_max, S_IRUGO, show_temp_max, NULL);
 static SENSOR_DEVICE_ATTR(temp1_crit, S_IRUGO, show_temp_crit, NULL, 0);
 static SENSOR_DEVICE_ATTR(temp1_crit_hyst, S_IRUGO, show_temp_crit, NULL, 1);
-static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
+
+static umode_t k10temp_is_visible(struct kobject *kobj,
+				  struct attribute *attr, int index)
+{
+	struct device *dev = container_of(kobj, struct device, kobj);
+	struct pci_dev *pdev = dev_get_drvdata(dev);
+
+	if (index >= 2) {
+		u32 reg_caps, reg_htc;
+
+		pci_read_config_dword(pdev, REG_NORTHBRIDGE_CAPABILITIES,
+				      &reg_caps);
+		pci_read_config_dword(pdev, REG_HARDWARE_THERMAL_CONTROL,
+				      &reg_htc);
+		if (!(reg_caps & NB_CAP_HTC) || !(reg_htc & HTC_ENABLE))
+			return 0;
+	}
+	return attr->mode;
+}
+
+static struct attribute *k10temp_attrs[] = {
+	&dev_attr_temp1_input.attr,
+	&dev_attr_temp1_max.attr,
+	&sensor_dev_attr_temp1_crit.dev_attr.attr,
+	&sensor_dev_attr_temp1_crit_hyst.dev_attr.attr,
+	NULL
+};
+
+static const struct attribute_group k10temp_group = {
+	.attrs = k10temp_attrs,
+	.is_visible = k10temp_is_visible,
+};
+__ATTRIBUTE_GROUPS(k10temp);
 
 static bool has_erratum_319(struct pci_dev *pdev)
 {
@@ -132,76 +186,23 @@ static bool has_erratum_319(struct pci_dev *pdev)
 static int k10temp_probe(struct pci_dev *pdev,
 				   const struct pci_device_id *id)
 {
-	struct device *hwmon_dev;
-	u32 reg_caps, reg_htc;
 	int unreliable = has_erratum_319(pdev);
-	int err;
-
-	if (unreliable && !force) {
-		dev_err(&pdev->dev,
-			"unreliable CPU thermal sensor; monitoring disabled\n");
-		err = -ENODEV;
-		goto exit;
-	}
-
-	err = device_create_file(&pdev->dev, &dev_attr_temp1_input);
-	if (err)
-		goto exit;
-	err = device_create_file(&pdev->dev, &dev_attr_temp1_max);
-	if (err)
-		goto exit_remove;
-
-	pci_read_config_dword(pdev, REG_NORTHBRIDGE_CAPABILITIES, &reg_caps);
-	pci_read_config_dword(pdev, REG_HARDWARE_THERMAL_CONTROL, &reg_htc);
-	if ((reg_caps & NB_CAP_HTC) && (reg_htc & HTC_ENABLE)) {
-		err = device_create_file(&pdev->dev,
-				&sensor_dev_attr_temp1_crit.dev_attr);
-		if (err)
-			goto exit_remove;
-		err = device_create_file(&pdev->dev,
-				&sensor_dev_attr_temp1_crit_hyst.dev_attr);
-		if (err)
-			goto exit_remove;
-	}
-
-	err = device_create_file(&pdev->dev, &dev_attr_name);
-	if (err)
-		goto exit_remove;
-
-	hwmon_dev = hwmon_device_register(&pdev->dev);
-	if (IS_ERR(hwmon_dev)) {
-		err = PTR_ERR(hwmon_dev);
-		goto exit_remove;
-	}
-	pci_set_drvdata(pdev, hwmon_dev);
+	struct device *dev = &pdev->dev;
+	struct device *hwmon_dev;
 
-	if (unreliable && force)
-		dev_warn(&pdev->dev,
+	if (unreliable) {
+		if (!force) {
+			dev_err(dev,
+				"unreliable CPU thermal sensor; monitoring disabled\n");
+			return -ENODEV;
+		}
+		dev_warn(dev,
 			 "unreliable CPU thermal sensor; check erratum 319\n");
-	return 0;
-
-exit_remove:
-	device_remove_file(&pdev->dev, &dev_attr_name);
-	device_remove_file(&pdev->dev, &dev_attr_temp1_input);
-	device_remove_file(&pdev->dev, &dev_attr_temp1_max);
-	device_remove_file(&pdev->dev,
-			   &sensor_dev_attr_temp1_crit.dev_attr);
-	device_remove_file(&pdev->dev,
-			   &sensor_dev_attr_temp1_crit_hyst.dev_attr);
-exit:
-	return err;
-}
+	}
 
-static void k10temp_remove(struct pci_dev *pdev)
-{
-	hwmon_device_unregister(pci_get_drvdata(pdev));
-	device_remove_file(&pdev->dev, &dev_attr_name);
-	device_remove_file(&pdev->dev, &dev_attr_temp1_input);
-	device_remove_file(&pdev->dev, &dev_attr_temp1_max);
-	device_remove_file(&pdev->dev,
-			   &sensor_dev_attr_temp1_crit.dev_attr);
-	device_remove_file(&pdev->dev,
-			   &sensor_dev_attr_temp1_crit_hyst.dev_attr);
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, "k10temp", pdev,
+							   k10temp_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 static const struct pci_device_id k10temp_id_table[] = {
@@ -211,6 +212,7 @@ static const struct pci_device_id k10temp_id_table[] = {
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_NB_F3) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M10H_F3) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M30H_NB_F3) },
+	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_15H_M60H_NB_F3) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_16H_NB_F3) },
 	{ PCI_VDEVICE(AMD, PCI_DEVICE_ID_AMD_16H_M30H_NB_F3) },
 	{}
@@ -221,7 +223,6 @@ static struct pci_driver k10temp_driver = {
 	.name = "k10temp",
 	.id_table = k10temp_id_table,
 	.probe = k10temp_probe,
-	.remove = k10temp_remove,
 };
 
 module_pci_driver(k10temp_driver);
diff --git a/drivers/hwmon/menf21bmc_hwmon.c b/drivers/hwmon/menf21bmc_hwmon.c
new file mode 100644
index 000000000000..afc6b58eaa62
--- /dev/null
+++ b/drivers/hwmon/menf21bmc_hwmon.c
@@ -0,0 +1,231 @@
+/*
+ *  MEN 14F021P00 Board Management Controller (BMC) hwmon driver.
+ *
+ *  This is the core hwmon driver of the MEN 14F021P00 BMC.
+ *  The BMC monitors the board voltages which can be access with this
+ *  driver through sysfs.
+ *
+ *  Copyright (C) 2014 MEN Mikro Elektronik Nuernberg GmbH
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/jiffies.h>
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/err.h>
+
+#define DRV_NAME  "menf21bmc_hwmon"
+
+#define BMC_VOLT_COUNT	5
+#define MENF21BMC_V33	0
+#define MENF21BMC_V5	1
+#define MENF21BMC_V12	2
+#define MENF21BMC_V5_SB	3
+#define MENF21BMC_VBAT	4
+
+#define IDX_TO_VOLT_MIN_CMD(idx) (0x40 + idx)
+#define IDX_TO_VOLT_MAX_CMD(idx) (0x50 + idx)
+#define IDX_TO_VOLT_INP_CMD(idx) (0x60 + idx)
+
+struct menf21bmc_hwmon {
+	bool valid;
+	struct i2c_client *i2c_client;
+	unsigned long last_update;
+	int in_val[BMC_VOLT_COUNT];
+	int in_min[BMC_VOLT_COUNT];
+	int in_max[BMC_VOLT_COUNT];
+};
+
+static const char *const input_names[] = {
+	[MENF21BMC_V33]		= "MON_3_3V",
+	[MENF21BMC_V5]		= "MON_5V",
+	[MENF21BMC_V12]		= "MON_12V",
+	[MENF21BMC_V5_SB]	= "5V_STANDBY",
+	[MENF21BMC_VBAT]	= "VBAT"
+};
+
+static struct menf21bmc_hwmon *menf21bmc_hwmon_update(struct device *dev)
+{
+	int i;
+	int val;
+	struct menf21bmc_hwmon *drv_data = dev_get_drvdata(dev);
+	struct menf21bmc_hwmon *data_ret = drv_data;
+
+	if (time_after(jiffies, drv_data->last_update + HZ)
+	    || !drv_data->valid) {
+		for (i = 0; i < BMC_VOLT_COUNT; i++) {
+			val = i2c_smbus_read_word_data(drv_data->i2c_client,
+						       IDX_TO_VOLT_INP_CMD(i));
+			if (val < 0) {
+				data_ret = ERR_PTR(val);
+				goto abort;
+			}
+			drv_data->in_val[i] = val;
+		}
+		drv_data->last_update = jiffies;
+		drv_data->valid = true;
+	}
+abort:
+	return data_ret;
+}
+
+static int menf21bmc_hwmon_get_volt_limits(struct menf21bmc_hwmon *drv_data)
+{
+	int i, val;
+
+	for (i = 0; i < BMC_VOLT_COUNT; i++) {
+		val = i2c_smbus_read_word_data(drv_data->i2c_client,
+					       IDX_TO_VOLT_MIN_CMD(i));
+		if (val < 0)
+			return val;
+
+		drv_data->in_min[i] = val;
+
+		val = i2c_smbus_read_word_data(drv_data->i2c_client,
+					       IDX_TO_VOLT_MAX_CMD(i));
+		if (val < 0)
+			return val;
+
+		drv_data->in_max[i] = val;
+	}
+	return 0;
+}
+
+static ssize_t
+show_label(struct device *dev, struct device_attribute *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+
+	return sprintf(buf, "%s\n", input_names[attr->index]);
+}
+
+static ssize_t
+show_in(struct device *dev, struct device_attribute *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct menf21bmc_hwmon *drv_data = menf21bmc_hwmon_update(dev);
+
+	if (IS_ERR(drv_data))
+		return PTR_ERR(drv_data);
+
+	return sprintf(buf, "%d\n", drv_data->in_val[attr->index]);
+}
+
+static ssize_t
+show_min(struct device *dev, struct device_attribute *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct menf21bmc_hwmon *drv_data = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", drv_data->in_min[attr->index]);
+}
+
+static ssize_t
+show_max(struct device *dev, struct device_attribute *devattr, char *buf)
+{
+	struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+	struct menf21bmc_hwmon *drv_data = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", drv_data->in_max[attr->index]);
+}
+
+#define create_voltage_sysfs(idx)			\
+static SENSOR_DEVICE_ATTR(in##idx##_input, S_IRUGO,	\
+			show_in, NULL, idx);		\
+static SENSOR_DEVICE_ATTR(in##idx##_min, S_IRUGO,	\
+			show_min, NULL, idx);		\
+static SENSOR_DEVICE_ATTR(in##idx##_max, S_IRUGO,	\
+			show_max, NULL, idx);		\
+static SENSOR_DEVICE_ATTR(in##idx##_label, S_IRUGO,	\
+			show_label, NULL, idx);
+
+create_voltage_sysfs(0);
+create_voltage_sysfs(1);
+create_voltage_sysfs(2);
+create_voltage_sysfs(3);
+create_voltage_sysfs(4);
+
+static struct attribute *menf21bmc_hwmon_attrs[] = {
+	&sensor_dev_attr_in0_input.dev_attr.attr,
+	&sensor_dev_attr_in0_min.dev_attr.attr,
+	&sensor_dev_attr_in0_max.dev_attr.attr,
+	&sensor_dev_attr_in0_label.dev_attr.attr,
+
+	&sensor_dev_attr_in1_input.dev_attr.attr,
+	&sensor_dev_attr_in1_min.dev_attr.attr,
+	&sensor_dev_attr_in1_max.dev_attr.attr,
+	&sensor_dev_attr_in1_label.dev_attr.attr,
+
+	&sensor_dev_attr_in2_input.dev_attr.attr,
+	&sensor_dev_attr_in2_min.dev_attr.attr,
+	&sensor_dev_attr_in2_max.dev_attr.attr,
+	&sensor_dev_attr_in2_label.dev_attr.attr,
+
+	&sensor_dev_attr_in3_input.dev_attr.attr,
+	&sensor_dev_attr_in3_min.dev_attr.attr,
+	&sensor_dev_attr_in3_max.dev_attr.attr,
+	&sensor_dev_attr_in3_label.dev_attr.attr,
+
+	&sensor_dev_attr_in4_input.dev_attr.attr,
+	&sensor_dev_attr_in4_min.dev_attr.attr,
+	&sensor_dev_attr_in4_max.dev_attr.attr,
+	&sensor_dev_attr_in4_label.dev_attr.attr,
+	NULL
+};
+
+ATTRIBUTE_GROUPS(menf21bmc_hwmon);
+
+static int menf21bmc_hwmon_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct menf21bmc_hwmon *drv_data;
+	struct i2c_client *i2c_client = to_i2c_client(pdev->dev.parent);
+	struct device *hwmon_dev;
+
+	drv_data = devm_kzalloc(&pdev->dev, sizeof(struct menf21bmc_hwmon),
+				GFP_KERNEL);
+	if (!drv_data)
+		return -ENOMEM;
+
+	drv_data->i2c_client = i2c_client;
+
+	ret = menf21bmc_hwmon_get_volt_limits(drv_data);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to read sensor limits");
+		return ret;
+	}
+
+	hwmon_dev = devm_hwmon_device_register_with_groups(&pdev->dev,
+						   "menf21bmc", drv_data,
+						   menf21bmc_hwmon_groups);
+	if (IS_ERR(hwmon_dev))
+		return PTR_ERR(hwmon_dev);
+
+	dev_info(&pdev->dev, "MEN 14F021P00 BMC hwmon device enabled");
+
+	return 0;
+}
+
+static struct platform_driver menf21bmc_hwmon = {
+	.probe		= menf21bmc_hwmon_probe,
+	.driver		= {
+		.name		= DRV_NAME,
+		.owner		= THIS_MODULE,
+	},
+};
+
+module_platform_driver(menf21bmc_hwmon);
+
+MODULE_AUTHOR("Andreas Werner <andreas.werner@men.de>");
+MODULE_DESCRIPTION("MEN 14F021P00 BMC hwmon");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:menf21bmc_hwmon");
diff --git a/drivers/hwmon/ntc_thermistor.c b/drivers/hwmon/ntc_thermistor.c
index bd410722cd4b..4ff89b2482e4 100644
--- a/drivers/hwmon/ntc_thermistor.c
+++ b/drivers/hwmon/ntc_thermistor.c
@@ -38,6 +38,7 @@
 
 #include <linux/hwmon.h>
 #include <linux/hwmon-sysfs.h>
+#include <linux/thermal.h>
 
 struct ntc_compensation {
 	int		temp_c;
@@ -182,6 +183,7 @@ struct ntc_data {
 	struct device *dev;
 	int n_comp;
 	char name[PLATFORM_NAME_SIZE];
+	struct thermal_zone_device *tz;
 };
 
 #if defined(CONFIG_OF) && IS_ENABLED(CONFIG_IIO)
@@ -428,6 +430,20 @@ static int ntc_thermistor_get_ohm(struct ntc_data *data)
 	return -EINVAL;
 }
 
+static int ntc_read_temp(void *dev, long *temp)
+{
+	struct ntc_data *data = dev_get_drvdata(dev);
+	int ohm;
+
+	ohm = ntc_thermistor_get_ohm(data);
+	if (ohm < 0)
+		return ohm;
+
+	*temp = get_temp_mc(data, ohm);
+
+	return 0;
+}
+
 static ssize_t ntc_show_name(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -562,6 +578,13 @@ static int ntc_thermistor_probe(struct platform_device *pdev)
 	dev_info(&pdev->dev, "Thermistor type: %s successfully probed.\n",
 								pdev_id->name);
 
+	data->tz = thermal_zone_of_sensor_register(data->dev, 0, data->dev,
+						ntc_read_temp, NULL);
+	if (IS_ERR(data->tz)) {
+		dev_dbg(&pdev->dev, "Failed to register to thermal fw.\n");
+		data->tz = NULL;
+	}
+
 	return 0;
 err_after_sysfs:
 	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
@@ -578,6 +601,8 @@ static int ntc_thermistor_remove(struct platform_device *pdev)
 	sysfs_remove_group(&data->dev->kobj, &ntc_attr_group);
 	ntc_iio_channel_release(pdata);
 
+	thermal_zone_of_sensor_unregister(data->dev, data->tz);
+
 	return 0;
 }
 
diff --git a/drivers/hwmon/smsc47b397.c b/drivers/hwmon/smsc47b397.c
index bd89e87bd6ae..221f0931bf1c 100644
--- a/drivers/hwmon/smsc47b397.c
+++ b/drivers/hwmon/smsc47b397.c
@@ -100,8 +100,6 @@ static u8 smsc47b397_reg_temp[] = {0x25, 0x26, 0x27, 0x80};
 
 struct smsc47b397_data {
 	unsigned short addr;
-	const char *name;
-	struct device *hwmon_dev;
 	struct mutex lock;
 
 	struct mutex update_lock;
@@ -202,15 +200,7 @@ static SENSOR_DEVICE_ATTR(fan2_input, S_IRUGO, show_fan, NULL, 1);
 static SENSOR_DEVICE_ATTR(fan3_input, S_IRUGO, show_fan, NULL, 2);
 static SENSOR_DEVICE_ATTR(fan4_input, S_IRUGO, show_fan, NULL, 3);
 
-static ssize_t show_name(struct device *dev, struct device_attribute
-			 *devattr, char *buf)
-{
-	struct smsc47b397_data *data = dev_get_drvdata(dev);
-	return sprintf(buf, "%s\n", data->name);
-}
-static DEVICE_ATTR(name, S_IRUGO, show_name, NULL);
-
-static struct attribute *smsc47b397_attributes[] = {
+static struct attribute *smsc47b397_attrs[] = {
 	&sensor_dev_attr_temp1_input.dev_attr.attr,
 	&sensor_dev_attr_temp2_input.dev_attr.attr,
 	&sensor_dev_attr_temp3_input.dev_attr.attr,
@@ -220,23 +210,10 @@ static struct attribute *smsc47b397_attributes[] = {
 	&sensor_dev_attr_fan3_input.dev_attr.attr,
 	&sensor_dev_attr_fan4_input.dev_attr.attr,
 
-	&dev_attr_name.attr,
 	NULL
 };
 
-static const struct attribute_group smsc47b397_group = {
-	.attrs = smsc47b397_attributes,
-};
-
-static int smsc47b397_remove(struct platform_device *pdev)
-{
-	struct smsc47b397_data *data = platform_get_drvdata(pdev);
-
-	hwmon_device_unregister(data->hwmon_dev);
-	sysfs_remove_group(&pdev->dev.kobj, &smsc47b397_group);
-
-	return 0;
-}
+ATTRIBUTE_GROUPS(smsc47b397);
 
 static int smsc47b397_probe(struct platform_device *pdev);
 
@@ -246,15 +223,14 @@ static struct platform_driver smsc47b397_driver = {
 		.name	= DRVNAME,
 	},
 	.probe		= smsc47b397_probe,
-	.remove		= smsc47b397_remove,
 };
 
 static int smsc47b397_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
 	struct smsc47b397_data *data;
+	struct device *hwmon_dev;
 	struct resource *res;
-	int err = 0;
 
 	res = platform_get_resource(pdev, IORESOURCE_IO, 0);
 	if (!devm_request_region(dev, res->start, SMSC_EXTENT,
@@ -270,26 +246,13 @@ static int smsc47b397_probe(struct platform_device *pdev)
 		return -ENOMEM;
 
 	data->addr = res->start;
-	data->name = "smsc47b397";
 	mutex_init(&data->lock);
 	mutex_init(&data->update_lock);
-	platform_set_drvdata(pdev, data);
-
-	err = sysfs_create_group(&dev->kobj, &smsc47b397_group);
-	if (err)
-		return err;
 
-	data->hwmon_dev = hwmon_device_register(dev);
-	if (IS_ERR(data->hwmon_dev)) {
-		err = PTR_ERR(data->hwmon_dev);
-		goto error_remove;
-	}
-
-	return 0;
-
-error_remove:
-	sysfs_remove_group(&dev->kobj, &smsc47b397_group);
-	return err;
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, "smsc47b397",
+							   data,
+							   smsc47b397_groups);
+	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
 
 static int __init smsc47b397_device_add(unsigned short address)
diff --git a/drivers/hwmon/tmp103.c b/drivers/hwmon/tmp103.c
index e42964f07f67..ad571ec795a3 100644
--- a/drivers/hwmon/tmp103.c
+++ b/drivers/hwmon/tmp103.c
@@ -145,7 +145,7 @@ static int tmp103_probe(struct i2c_client *client,
 	}
 
 	i2c_set_clientdata(client, regmap);
-	hwmon_dev = hwmon_device_register_with_groups(dev, client->name,
+	hwmon_dev = devm_hwmon_device_register_with_groups(dev, client->name,
 						      regmap, tmp103_groups);
 	return PTR_ERR_OR_ZERO(hwmon_dev);
 }
diff --git a/drivers/i2c/Kconfig b/drivers/i2c/Kconfig
index 3e3b680dc007..b51a402752c4 100644
--- a/drivers/i2c/Kconfig
+++ b/drivers/i2c/Kconfig
@@ -23,17 +23,14 @@ config I2C
 	  This I2C support can also be built as a module.  If so, the module
 	  will be called i2c-core.
 
-config I2C_ACPI
-	bool "I2C ACPI support"
-	select I2C
-	depends on ACPI
+config ACPI_I2C_OPREGION
+	bool "ACPI I2C Operation region support"
+	depends on I2C=y && ACPI
 	default y
 	help
-	  Say Y here if you want to enable ACPI I2C support. This includes support
-	  for automatic enumeration of I2C slave devices and support for ACPI I2C
-	  Operation Regions. Operation Regions allow firmware (BIOS) code to
-	  access I2C slave devices, such as smart batteries through an I2C host
-	  controller driver.
+	  Say Y here if you want to enable ACPI I2C operation region support.
+	  Operation Regions allow firmware (BIOS) code to access I2C slave devices,
+	  such as smart batteries through an I2C host controller driver.
 
 if I2C
 
diff --git a/drivers/i2c/Makefile b/drivers/i2c/Makefile
index a1f590cbb435..1722f50f2473 100644
--- a/drivers/i2c/Makefile
+++ b/drivers/i2c/Makefile
@@ -2,11 +2,8 @@
 # Makefile for the i2c core.
 #
 
-i2ccore-y := i2c-core.o
-i2ccore-$(CONFIG_I2C_ACPI)	+= i2c-acpi.o
-
 obj-$(CONFIG_I2C_BOARDINFO)	+= i2c-boardinfo.o
-obj-$(CONFIG_I2C)		+= i2ccore.o
+obj-$(CONFIG_I2C)		+= i2c-core.o
 obj-$(CONFIG_I2C_SMBUS)		+= i2c-smbus.o
 obj-$(CONFIG_I2C_CHARDEV)	+= i2c-dev.o
 obj-$(CONFIG_I2C_MUX)		+= i2c-mux.o
diff --git a/drivers/i2c/busses/Kconfig b/drivers/i2c/busses/Kconfig
index 2ac87fa3058d..917c3585f45b 100644
--- a/drivers/i2c/busses/Kconfig
+++ b/drivers/i2c/busses/Kconfig
@@ -77,6 +77,16 @@ config I2C_AMD8111
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-amd8111.
 
+config I2C_HIX5HD2
+	tristate "Hix5hd2 high-speed I2C driver"
+	depends on ARCH_HIX5HD2
+	help
+	  Say Y here to include support for high-speed I2C controller in the
+	  Hisilicon based hix5hd2 SoCs.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called i2c-hix5hd2.
+
 config I2C_I801
 	tristate "Intel 82801 (ICH/PCH)"
 	depends on PCI
@@ -112,6 +122,7 @@ config I2C_I801
 	    Wildcat Point (PCH)
 	    Wildcat Point-LP (PCH)
 	    BayTrail (SOC)
+	    Sunrise Point-H (PCH)
 
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-i801.
@@ -337,6 +348,17 @@ config I2C_AU1550
 	  This driver can also be built as a module.  If so, the module
 	  will be called i2c-au1550.
 
+config I2C_AXXIA
+	tristate "Axxia I2C controller"
+	depends on ARCH_AXXIA || COMPILE_TEST
+	default ARCH_AXXIA
+	help
+	  Say yes if you want to support the I2C bus on Axxia platforms.
+
+	  Please note that this controller is limited to transfers of maximum
+	  255 bytes in length. Any attempt to to a larger transfer will return
+	  an error.
+
 config I2C_BCM2835
 	tristate "Broadcom BCM2835 I2C controller"
 	depends on ARCH_BCM2835
@@ -357,7 +379,7 @@ config I2C_BCM_KONA
 	  If you say yes to this option, support will be included for the
 	  I2C interface on the Broadcom Kona family of processors.
 
-	  If you do not need KONA I2C inteface, say N.
+	  If you do not need KONA I2C interface, say N.
 
 config I2C_BLACKFIN_TWI
 	tristate "Blackfin TWI I2C support"
@@ -423,6 +445,7 @@ config I2C_DESIGNWARE_CORE
 config I2C_DESIGNWARE_PLATFORM
 	tristate "Synopsys DesignWare Platform"
 	select I2C_DESIGNWARE_CORE
+	depends on (ACPI && COMMON_CLK) || !ACPI
 	help
 	  If you say yes to this option, support will be included for the
 	  Synopsys DesignWare I2C adapter. Only master mode is supported.
@@ -465,7 +488,7 @@ config I2C_EG20T
 
 config I2C_EXYNOS5
 	tristate "Exynos5 high-speed I2C driver"
-	depends on ARCH_EXYNOS5 && OF
+	depends on ARCH_EXYNOS && OF
 	default y
 	help
 	  High-speed I2C controller on Exynos5 based Samsung SoCs.
diff --git a/drivers/i2c/busses/Makefile b/drivers/i2c/busses/Makefile
index 49bf07e5ef4d..78d56c54ba2b 100644
--- a/drivers/i2c/busses/Makefile
+++ b/drivers/i2c/busses/Makefile
@@ -31,6 +31,7 @@ obj-$(CONFIG_I2C_POWERMAC)	+= i2c-powermac.o
 # Embedded system I2C/SMBus host controller drivers
 obj-$(CONFIG_I2C_AT91)		+= i2c-at91.o
 obj-$(CONFIG_I2C_AU1550)	+= i2c-au1550.o
+obj-$(CONFIG_I2C_AXXIA)		+= i2c-axxia.o
 obj-$(CONFIG_I2C_BCM2835)	+= i2c-bcm2835.o
 obj-$(CONFIG_I2C_BLACKFIN_TWI)	+= i2c-bfin-twi.o
 obj-$(CONFIG_I2C_CADENCE)	+= i2c-cadence.o
@@ -47,6 +48,7 @@ obj-$(CONFIG_I2C_EG20T)		+= i2c-eg20t.o
 obj-$(CONFIG_I2C_EXYNOS5)	+= i2c-exynos5.o
 obj-$(CONFIG_I2C_GPIO)		+= i2c-gpio.o
 obj-$(CONFIG_I2C_HIGHLANDER)	+= i2c-highlander.o
+obj-$(CONFIG_I2C_HIX5HD2)	+= i2c-hix5hd2.o
 obj-$(CONFIG_I2C_IBM_IIC)	+= i2c-ibm_iic.o
 obj-$(CONFIG_I2C_IMX)		+= i2c-imx.o
 obj-$(CONFIG_I2C_IOP3XX)	+= i2c-iop3xx.o
diff --git a/drivers/i2c/busses/i2c-at91.c b/drivers/i2c/busses/i2c-at91.c
index 79a68999a696..917d54588d95 100644
--- a/drivers/i2c/busses/i2c-at91.c
+++ b/drivers/i2c/busses/i2c-at91.c
@@ -101,6 +101,7 @@ struct at91_twi_dev {
 	unsigned twi_cwgr_reg;
 	struct at91_twi_pdata *pdata;
 	bool use_dma;
+	bool recv_len_abort;
 	struct at91_twi_dma dma;
 };
 
@@ -267,12 +268,24 @@ static void at91_twi_read_next_byte(struct at91_twi_dev *dev)
 	*dev->buf = at91_twi_read(dev, AT91_TWI_RHR) & 0xff;
 	--dev->buf_len;
 
+	/* return if aborting, we only needed to read RHR to clear RXRDY*/
+	if (dev->recv_len_abort)
+		return;
+
 	/* handle I2C_SMBUS_BLOCK_DATA */
 	if (unlikely(dev->msg->flags & I2C_M_RECV_LEN)) {
-		dev->msg->flags &= ~I2C_M_RECV_LEN;
-		dev->buf_len += *dev->buf;
-		dev->msg->len = dev->buf_len + 1;
-		dev_dbg(dev->dev, "received block length %d\n", dev->buf_len);
+		/* ensure length byte is a valid value */
+		if (*dev->buf <= I2C_SMBUS_BLOCK_MAX && *dev->buf > 0) {
+			dev->msg->flags &= ~I2C_M_RECV_LEN;
+			dev->buf_len += *dev->buf;
+			dev->msg->len = dev->buf_len + 1;
+			dev_dbg(dev->dev, "received block length %d\n",
+					 dev->buf_len);
+		} else {
+			/* abort and send the stop by reading one more byte */
+			dev->recv_len_abort = true;
+			dev->buf_len = 1;
+		}
 	}
 
 	/* send stop if second but last byte has been read */
@@ -421,8 +434,8 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 		}
 	}
 
-	ret = wait_for_completion_interruptible_timeout(&dev->cmd_complete,
-							dev->adapter.timeout);
+	ret = wait_for_completion_io_timeout(&dev->cmd_complete,
+					     dev->adapter.timeout);
 	if (ret == 0) {
 		dev_err(dev->dev, "controller timed out\n");
 		at91_init_twi_bus(dev);
@@ -444,6 +457,12 @@ static int at91_do_twi_transfer(struct at91_twi_dev *dev)
 		ret = -EIO;
 		goto error;
 	}
+	if (dev->recv_len_abort) {
+		dev_err(dev->dev, "invalid smbus block length recvd\n");
+		ret = -EPROTO;
+		goto error;
+	}
+
 	dev_dbg(dev->dev, "transfer complete\n");
 
 	return 0;
@@ -500,6 +519,7 @@ static int at91_twi_xfer(struct i2c_adapter *adap, struct i2c_msg *msg, int num)
 	dev->buf_len = m_start->len;
 	dev->buf = m_start->buf;
 	dev->msg = m_start;
+	dev->recv_len_abort = false;
 
 	ret = at91_do_twi_transfer(dev);
 
diff --git a/drivers/i2c/busses/i2c-axxia.c b/drivers/i2c/busses/i2c-axxia.c
new file mode 100644
index 000000000000..768a598d8d03
--- /dev/null
+++ b/drivers/i2c/busses/i2c-axxia.c
@@ -0,0 +1,559 @@
+/*
+ * This driver implements I2C master functionality using the LSI API2C
+ * controller.
+ *
+ * NOTE: The controller has a limitation in that it can only do transfers of
+ * maximum 255 bytes at a time. If a larger transfer is attempted, error code
+ * (-EINVAL) is returned.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ */
+#include <linux/clk.h>
+#include <linux/clkdev.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+
+#define SCL_WAIT_TIMEOUT_NS 25000000
+#define I2C_XFER_TIMEOUT    (msecs_to_jiffies(250))
+#define I2C_STOP_TIMEOUT    (msecs_to_jiffies(100))
+#define FIFO_SIZE           8
+
+#define GLOBAL_CONTROL		0x00
+#define   GLOBAL_MST_EN         BIT(0)
+#define   GLOBAL_SLV_EN         BIT(1)
+#define   GLOBAL_IBML_EN        BIT(2)
+#define INTERRUPT_STATUS	0x04
+#define INTERRUPT_ENABLE	0x08
+#define   INT_SLV               BIT(1)
+#define   INT_MST               BIT(0)
+#define WAIT_TIMER_CONTROL	0x0c
+#define   WT_EN			BIT(15)
+#define   WT_VALUE(_x)		((_x) & 0x7fff)
+#define IBML_TIMEOUT		0x10
+#define IBML_LOW_MEXT		0x14
+#define IBML_LOW_SEXT		0x18
+#define TIMER_CLOCK_DIV		0x1c
+#define I2C_BUS_MONITOR		0x20
+#define SOFT_RESET		0x24
+#define MST_COMMAND		0x28
+#define   CMD_BUSY		(1<<3)
+#define   CMD_MANUAL		(0x00 | CMD_BUSY)
+#define   CMD_AUTO		(0x01 | CMD_BUSY)
+#define MST_RX_XFER		0x2c
+#define MST_TX_XFER		0x30
+#define MST_ADDR_1		0x34
+#define MST_ADDR_2		0x38
+#define MST_DATA		0x3c
+#define MST_TX_FIFO		0x40
+#define MST_RX_FIFO		0x44
+#define MST_INT_ENABLE		0x48
+#define MST_INT_STATUS		0x4c
+#define   MST_STATUS_RFL	(1 << 13) /* RX FIFO serivce */
+#define   MST_STATUS_TFL	(1 << 12) /* TX FIFO service */
+#define   MST_STATUS_SNS	(1 << 11) /* Manual mode done */
+#define   MST_STATUS_SS		(1 << 10) /* Automatic mode done */
+#define   MST_STATUS_SCC	(1 << 9)  /* Stop complete */
+#define   MST_STATUS_IP		(1 << 8)  /* Invalid parameter */
+#define   MST_STATUS_TSS	(1 << 7)  /* Timeout */
+#define   MST_STATUS_AL		(1 << 6)  /* Arbitration lost */
+#define   MST_STATUS_ND		(1 << 5)  /* NAK on data phase */
+#define   MST_STATUS_NA		(1 << 4)  /* NAK on address phase */
+#define   MST_STATUS_NAK	(MST_STATUS_NA | \
+				 MST_STATUS_ND)
+#define   MST_STATUS_ERR	(MST_STATUS_NAK | \
+				 MST_STATUS_AL  | \
+				 MST_STATUS_IP  | \
+				 MST_STATUS_TSS)
+#define MST_TX_BYTES_XFRD	0x50
+#define MST_RX_BYTES_XFRD	0x54
+#define SCL_HIGH_PERIOD		0x80
+#define SCL_LOW_PERIOD		0x84
+#define SPIKE_FLTR_LEN		0x88
+#define SDA_SETUP_TIME		0x8c
+#define SDA_HOLD_TIME		0x90
+
+/**
+ * axxia_i2c_dev - I2C device context
+ * @base: pointer to register struct
+ * @msg: pointer to current message
+ * @msg_xfrd: number of bytes transferred in msg
+ * @msg_err: error code for completed message
+ * @msg_complete: xfer completion object
+ * @dev: device reference
+ * @adapter: core i2c abstraction
+ * @i2c_clk: clock reference for i2c input clock
+ * @bus_clk_rate: current i2c bus clock rate
+ */
+struct axxia_i2c_dev {
+	void __iomem *base;
+	struct i2c_msg *msg;
+	size_t msg_xfrd;
+	int msg_err;
+	struct completion msg_complete;
+	struct device *dev;
+	struct i2c_adapter adapter;
+	struct clk *i2c_clk;
+	u32 bus_clk_rate;
+};
+
+static void i2c_int_disable(struct axxia_i2c_dev *idev, u32 mask)
+{
+	u32 int_en;
+
+	int_en = readl(idev->base + MST_INT_ENABLE);
+	writel(int_en & ~mask, idev->base + MST_INT_ENABLE);
+}
+
+static void i2c_int_enable(struct axxia_i2c_dev *idev, u32 mask)
+{
+	u32 int_en;
+
+	int_en = readl(idev->base + MST_INT_ENABLE);
+	writel(int_en | mask, idev->base + MST_INT_ENABLE);
+}
+
+/**
+ * ns_to_clk - Convert time (ns) to clock cycles for the given clock frequency.
+ */
+static u32 ns_to_clk(u64 ns, u32 clk_mhz)
+{
+	return div_u64(ns * clk_mhz, 1000);
+}
+
+static int axxia_i2c_init(struct axxia_i2c_dev *idev)
+{
+	u32 divisor = clk_get_rate(idev->i2c_clk) / idev->bus_clk_rate;
+	u32 clk_mhz = clk_get_rate(idev->i2c_clk) / 1000000;
+	u32 t_setup;
+	u32 t_high, t_low;
+	u32 tmo_clk;
+	u32 prescale;
+	unsigned long timeout;
+
+	dev_dbg(idev->dev, "rate=%uHz per_clk=%uMHz -> ratio=1:%u\n",
+		idev->bus_clk_rate, clk_mhz, divisor);
+
+	/* Reset controller */
+	writel(0x01, idev->base + SOFT_RESET);
+	timeout = jiffies + msecs_to_jiffies(100);
+	while (readl(idev->base + SOFT_RESET) & 1) {
+		if (time_after(jiffies, timeout)) {
+			dev_warn(idev->dev, "Soft reset failed\n");
+			break;
+		}
+	}
+
+	/* Enable Master Mode */
+	writel(0x1, idev->base + GLOBAL_CONTROL);
+
+	if (idev->bus_clk_rate <= 100000) {
+		/* Standard mode SCL 50/50, tSU:DAT = 250 ns */
+		t_high = divisor * 1 / 2;
+		t_low = divisor * 1 / 2;
+		t_setup = ns_to_clk(250, clk_mhz);
+	} else {
+		/* Fast mode SCL 33/66, tSU:DAT = 100 ns */
+		t_high = divisor * 1 / 3;
+		t_low = divisor * 2 / 3;
+		t_setup = ns_to_clk(100, clk_mhz);
+	}
+
+	/* SCL High Time */
+	writel(t_high, idev->base + SCL_HIGH_PERIOD);
+	/* SCL Low Time */
+	writel(t_low, idev->base + SCL_LOW_PERIOD);
+	/* SDA Setup Time */
+	writel(t_setup, idev->base + SDA_SETUP_TIME);
+	/* SDA Hold Time, 300ns */
+	writel(ns_to_clk(300, clk_mhz), idev->base + SDA_HOLD_TIME);
+	/* Filter <50ns spikes */
+	writel(ns_to_clk(50, clk_mhz), idev->base + SPIKE_FLTR_LEN);
+
+	/* Configure Time-Out Registers */
+	tmo_clk = ns_to_clk(SCL_WAIT_TIMEOUT_NS, clk_mhz);
+
+	/* Find prescaler value that makes tmo_clk fit in 15-bits counter. */
+	for (prescale = 0; prescale < 15; ++prescale) {
+		if (tmo_clk <= 0x7fff)
+			break;
+		tmo_clk >>= 1;
+	}
+	if (tmo_clk > 0x7fff)
+		tmo_clk = 0x7fff;
+
+	/* Prescale divider (log2) */
+	writel(prescale, idev->base + TIMER_CLOCK_DIV);
+	/* Timeout in divided clocks */
+	writel(WT_EN | WT_VALUE(tmo_clk), idev->base + WAIT_TIMER_CONTROL);
+
+	/* Mask all master interrupt bits */
+	i2c_int_disable(idev, ~0);
+
+	/* Interrupt enable */
+	writel(0x01, idev->base + INTERRUPT_ENABLE);
+
+	return 0;
+}
+
+static int i2c_m_rd(const struct i2c_msg *msg)
+{
+	return (msg->flags & I2C_M_RD) != 0;
+}
+
+static int i2c_m_ten(const struct i2c_msg *msg)
+{
+	return (msg->flags & I2C_M_TEN) != 0;
+}
+
+static int i2c_m_recv_len(const struct i2c_msg *msg)
+{
+	return (msg->flags & I2C_M_RECV_LEN) != 0;
+}
+
+/**
+ * axxia_i2c_empty_rx_fifo - Fetch data from RX FIFO and update SMBus block
+ * transfer length if this is the first byte of such a transfer.
+ */
+static int axxia_i2c_empty_rx_fifo(struct axxia_i2c_dev *idev)
+{
+	struct i2c_msg *msg = idev->msg;
+	size_t rx_fifo_avail = readl(idev->base + MST_RX_FIFO);
+	int bytes_to_transfer = min(rx_fifo_avail, msg->len - idev->msg_xfrd);
+
+	while (bytes_to_transfer-- > 0) {
+		int c = readl(idev->base + MST_DATA);
+
+		if (idev->msg_xfrd == 0 && i2c_m_recv_len(msg)) {
+			/*
+			 * Check length byte for SMBus block read
+			 */
+			if (c <= 0 || c > I2C_SMBUS_BLOCK_MAX) {
+				idev->msg_err = -EPROTO;
+				i2c_int_disable(idev, ~0);
+				complete(&idev->msg_complete);
+				break;
+			}
+			msg->len = 1 + c;
+			writel(msg->len, idev->base + MST_RX_XFER);
+		}
+		msg->buf[idev->msg_xfrd++] = c;
+	}
+
+	return 0;
+}
+
+/**
+ * axxia_i2c_fill_tx_fifo - Fill TX FIFO from current message buffer.
+ * @return: Number of bytes left to transfer.
+ */
+static int axxia_i2c_fill_tx_fifo(struct axxia_i2c_dev *idev)
+{
+	struct i2c_msg *msg = idev->msg;
+	size_t tx_fifo_avail = FIFO_SIZE - readl(idev->base + MST_TX_FIFO);
+	int bytes_to_transfer = min(tx_fifo_avail, msg->len - idev->msg_xfrd);
+	int ret = msg->len - idev->msg_xfrd - bytes_to_transfer;
+
+	while (bytes_to_transfer-- > 0)
+		writel(msg->buf[idev->msg_xfrd++], idev->base + MST_DATA);
+
+	return ret;
+}
+
+static irqreturn_t axxia_i2c_isr(int irq, void *_dev)
+{
+	struct axxia_i2c_dev *idev = _dev;
+	u32 status;
+
+	if (!(readl(idev->base + INTERRUPT_STATUS) & INT_MST))
+		return IRQ_NONE;
+
+	/* Read interrupt status bits */
+	status = readl(idev->base + MST_INT_STATUS);
+
+	if (!idev->msg) {
+		dev_warn(idev->dev, "unexpected interrupt\n");
+		goto out;
+	}
+
+	/* RX FIFO needs service? */
+	if (i2c_m_rd(idev->msg) && (status & MST_STATUS_RFL))
+		axxia_i2c_empty_rx_fifo(idev);
+
+	/* TX FIFO needs service? */
+	if (!i2c_m_rd(idev->msg) && (status & MST_STATUS_TFL)) {
+		if (axxia_i2c_fill_tx_fifo(idev) == 0)
+			i2c_int_disable(idev, MST_STATUS_TFL);
+	}
+
+	if (status & MST_STATUS_SCC) {
+		/* Stop completed */
+		i2c_int_disable(idev, ~0);
+		complete(&idev->msg_complete);
+	} else if (status & MST_STATUS_SNS) {
+		/* Transfer done */
+		i2c_int_disable(idev, ~0);
+		if (i2c_m_rd(idev->msg) && idev->msg_xfrd < idev->msg->len)
+			axxia_i2c_empty_rx_fifo(idev);
+		complete(&idev->msg_complete);
+	} else if (unlikely(status & MST_STATUS_ERR)) {
+		/* Transfer error */
+		i2c_int_disable(idev, ~0);
+		if (status & MST_STATUS_AL)
+			idev->msg_err = -EAGAIN;
+		else if (status & MST_STATUS_NAK)
+			idev->msg_err = -ENXIO;
+		else
+			idev->msg_err = -EIO;
+		dev_dbg(idev->dev, "error %#x, addr=%#x rx=%u/%u tx=%u/%u\n",
+			status,
+			idev->msg->addr,
+			readl(idev->base + MST_RX_BYTES_XFRD),
+			readl(idev->base + MST_RX_XFER),
+			readl(idev->base + MST_TX_BYTES_XFRD),
+			readl(idev->base + MST_TX_XFER));
+		complete(&idev->msg_complete);
+	}
+
+out:
+	/* Clear interrupt */
+	writel(INT_MST, idev->base + INTERRUPT_STATUS);
+
+	return IRQ_HANDLED;
+}
+
+static int axxia_i2c_xfer_msg(struct axxia_i2c_dev *idev, struct i2c_msg *msg)
+{
+	u32 int_mask = MST_STATUS_ERR | MST_STATUS_SNS;
+	u32 rx_xfer, tx_xfer;
+	u32 addr_1, addr_2;
+	int ret;
+
+	if (msg->len > 255) {
+		dev_warn(idev->dev, "unsupported length %u\n", msg->len);
+		return -EINVAL;
+	}
+
+	idev->msg = msg;
+	idev->msg_xfrd = 0;
+	idev->msg_err = 0;
+	reinit_completion(&idev->msg_complete);
+
+	if (i2c_m_ten(msg)) {
+		/* 10-bit address
+		 *   addr_1: 5'b11110 | addr[9:8] | (R/nW)
+		 *   addr_2: addr[7:0]
+		 */
+		addr_1 = 0xF0 | ((msg->addr >> 7) & 0x06);
+		addr_2 = msg->addr & 0xFF;
+	} else {
+		/* 7-bit address
+		 *   addr_1: addr[6:0] | (R/nW)
+		 *   addr_2: dont care
+		 */
+		addr_1 = (msg->addr << 1) & 0xFF;
+		addr_2 = 0;
+	}
+
+	if (i2c_m_rd(msg)) {
+		/* I2C read transfer */
+		rx_xfer = i2c_m_recv_len(msg) ? I2C_SMBUS_BLOCK_MAX : msg->len;
+		tx_xfer = 0;
+		addr_1 |= 1;	/* Set the R/nW bit of the address */
+	} else {
+		/* I2C write transfer */
+		rx_xfer = 0;
+		tx_xfer = msg->len;
+	}
+
+	writel(rx_xfer, idev->base + MST_RX_XFER);
+	writel(tx_xfer, idev->base + MST_TX_XFER);
+	writel(addr_1, idev->base + MST_ADDR_1);
+	writel(addr_2, idev->base + MST_ADDR_2);
+
+	if (i2c_m_rd(msg))
+		int_mask |= MST_STATUS_RFL;
+	else if (axxia_i2c_fill_tx_fifo(idev) != 0)
+		int_mask |= MST_STATUS_TFL;
+
+	/* Start manual mode */
+	writel(CMD_MANUAL, idev->base + MST_COMMAND);
+
+	i2c_int_enable(idev, int_mask);
+
+	ret = wait_for_completion_timeout(&idev->msg_complete,
+					  I2C_XFER_TIMEOUT);
+
+	i2c_int_disable(idev, int_mask);
+
+	if (readl(idev->base + MST_COMMAND) & CMD_BUSY)
+		dev_warn(idev->dev, "busy after xfer\n");
+
+	if (ret == 0)
+		idev->msg_err = -ETIMEDOUT;
+
+	if (unlikely(idev->msg_err) && idev->msg_err != -ENXIO)
+		axxia_i2c_init(idev);
+
+	return idev->msg_err;
+}
+
+static int axxia_i2c_stop(struct axxia_i2c_dev *idev)
+{
+	u32 int_mask = MST_STATUS_ERR | MST_STATUS_SCC;
+	int ret;
+
+	reinit_completion(&idev->msg_complete);
+
+	/* Issue stop */
+	writel(0xb, idev->base + MST_COMMAND);
+	i2c_int_enable(idev, int_mask);
+	ret = wait_for_completion_timeout(&idev->msg_complete,
+					  I2C_STOP_TIMEOUT);
+	i2c_int_disable(idev, int_mask);
+	if (ret == 0)
+		return -ETIMEDOUT;
+
+	if (readl(idev->base + MST_COMMAND) & CMD_BUSY)
+		dev_warn(idev->dev, "busy after stop\n");
+
+	return 0;
+}
+
+static int
+axxia_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msgs[], int num)
+{
+	struct axxia_i2c_dev *idev = i2c_get_adapdata(adap);
+	int i;
+	int ret = 0;
+
+	for (i = 0; ret == 0 && i < num; ++i)
+		ret = axxia_i2c_xfer_msg(idev, &msgs[i]);
+
+	axxia_i2c_stop(idev);
+
+	return ret ? : i;
+}
+
+static u32 axxia_i2c_func(struct i2c_adapter *adap)
+{
+	u32 caps = (I2C_FUNC_I2C | I2C_FUNC_10BIT_ADDR |
+		    I2C_FUNC_SMBUS_EMUL | I2C_FUNC_SMBUS_BLOCK_DATA);
+	return caps;
+}
+
+static const struct i2c_algorithm axxia_i2c_algo = {
+	.master_xfer = axxia_i2c_xfer,
+	.functionality = axxia_i2c_func,
+};
+
+static int axxia_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct axxia_i2c_dev *idev = NULL;
+	struct resource *res;
+	void __iomem *base;
+	int irq;
+	int ret = 0;
+
+	idev = devm_kzalloc(&pdev->dev, sizeof(*idev), GFP_KERNEL);
+	if (!idev)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "missing interrupt resource\n");
+		return irq;
+	}
+
+	idev->i2c_clk = devm_clk_get(&pdev->dev, "i2c");
+	if (IS_ERR(idev->i2c_clk)) {
+		dev_err(&pdev->dev, "missing clock\n");
+		return PTR_ERR(idev->i2c_clk);
+	}
+
+	idev->base = base;
+	idev->dev = &pdev->dev;
+	init_completion(&idev->msg_complete);
+
+	of_property_read_u32(np, "clock-frequency", &idev->bus_clk_rate);
+	if (idev->bus_clk_rate == 0)
+		idev->bus_clk_rate = 100000;	/* default clock rate */
+
+	ret = axxia_i2c_init(idev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize\n");
+		return ret;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, axxia_i2c_isr, 0,
+			       pdev->name, idev);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to claim IRQ%d\n", irq);
+		return ret;
+	}
+
+	clk_prepare_enable(idev->i2c_clk);
+
+	i2c_set_adapdata(&idev->adapter, idev);
+	strlcpy(idev->adapter.name, pdev->name, sizeof(idev->adapter.name));
+	idev->adapter.owner = THIS_MODULE;
+	idev->adapter.algo = &axxia_i2c_algo;
+	idev->adapter.dev.parent = &pdev->dev;
+	idev->adapter.dev.of_node = pdev->dev.of_node;
+
+	platform_set_drvdata(pdev, idev);
+
+	ret = i2c_add_adapter(&idev->adapter);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to add adapter\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int axxia_i2c_remove(struct platform_device *pdev)
+{
+	struct axxia_i2c_dev *idev = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(idev->i2c_clk);
+	i2c_del_adapter(&idev->adapter);
+
+	return 0;
+}
+
+/* Match table for of_platform binding */
+static const struct of_device_id axxia_i2c_of_match[] = {
+	{ .compatible = "lsi,api2c", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, axxia_i2c_of_match);
+
+static struct platform_driver axxia_i2c_driver = {
+	.probe = axxia_i2c_probe,
+	.remove = axxia_i2c_remove,
+	.driver = {
+		.name = "axxia-i2c",
+		.of_match_table = axxia_i2c_of_match,
+	},
+};
+
+module_platform_driver(axxia_i2c_driver);
+
+MODULE_DESCRIPTION("Axxia I2C Bus driver");
+MODULE_AUTHOR("Anders Berg <anders.berg@lsi.com>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/i2c/busses/i2c-cros-ec-tunnel.c b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
index 05e033c98115..875c22ae5400 100644
--- a/drivers/i2c/busses/i2c-cros-ec-tunnel.c
+++ b/drivers/i2c/busses/i2c-cros-ec-tunnel.c
@@ -16,6 +16,8 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 
+#define I2C_MAX_RETRIES 3
+
 /**
  * struct ec_i2c_device - Driver data for I2C tunnel
  *
@@ -94,7 +96,7 @@ static int ec_i2c_construct_message(u8 *buf, const struct i2c_msg i2c_msgs[],
 		msg->addr_flags = i2c_msg->addr;
 
 		if (i2c_msg->flags & I2C_M_TEN)
-			msg->addr_flags |= EC_I2C_FLAG_10BIT;
+			return -EINVAL;
 
 		if (i2c_msg->flags & I2C_M_RD) {
 			msg->addr_flags |= EC_I2C_FLAG_READ;
@@ -218,7 +220,9 @@ static int ec_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg i2c_msgs[],
 		}
 	}
 
-	ec_i2c_construct_message(request, i2c_msgs, num, bus_num);
+	result = ec_i2c_construct_message(request, i2c_msgs, num, bus_num);
+	if (result)
+		goto exit;
 
 	msg.version = 0;
 	msg.command = EC_CMD_I2C_PASSTHRU;
@@ -227,7 +231,7 @@ static int ec_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg i2c_msgs[],
 	msg.indata = response;
 	msg.insize = response_len;
 
-	result = bus->ec->cmd_xfer(bus->ec, &msg);
+	result = cros_ec_cmd_xfer(bus->ec, &msg);
 	if (result < 0)
 		goto exit;
 
@@ -290,6 +294,7 @@ static int ec_i2c_probe(struct platform_device *pdev)
 	bus->adap.algo_data = bus;
 	bus->adap.dev.parent = &pdev->dev;
 	bus->adap.dev.of_node = np;
+	bus->adap.retries = I2C_MAX_RETRIES;
 
 	err = i2c_add_adapter(&bus->adap);
 	if (err) {
@@ -310,11 +315,20 @@ static int ec_i2c_remove(struct platform_device *dev)
 	return 0;
 }
 
+#ifdef CONFIG_OF
+static const struct of_device_id cros_ec_i2c_of_match[] = {
+	{ .compatible = "google,cros-ec-i2c-tunnel" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, cros_ec_i2c_of_match);
+#endif
+
 static struct platform_driver ec_i2c_tunnel_driver = {
 	.probe = ec_i2c_probe,
 	.remove = ec_i2c_remove,
 	.driver = {
 		.name = "cros-ec-i2c-tunnel",
+		.of_match_table = of_match_ptr(cros_ec_i2c_of_match),
 	},
 };
 
diff --git a/drivers/i2c/busses/i2c-designware-platdrv.c b/drivers/i2c/busses/i2c-designware-platdrv.c
index bc8773333155..a7431150acf7 100644
--- a/drivers/i2c/busses/i2c-designware-platdrv.c
+++ b/drivers/i2c/busses/i2c-designware-platdrv.c
@@ -30,6 +30,7 @@
 #include <linux/delay.h>
 #include <linux/i2c.h>
 #include <linux/clk.h>
+#include <linux/clk-provider.h>
 #include <linux/errno.h>
 #include <linux/sched.h>
 #include <linux/err.h>
@@ -41,6 +42,7 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/acpi.h>
+#include <linux/platform_data/i2c-designware.h>
 #include "i2c-designware-core.h"
 
 static struct i2c_algorithm i2c_dw_algo = {
@@ -79,10 +81,7 @@ static void dw_i2c_acpi_params(struct platform_device *pdev, char method[],
 static int dw_i2c_acpi_configure(struct platform_device *pdev)
 {
 	struct dw_i2c_dev *dev = platform_get_drvdata(pdev);
-	bool fs_mode = dev->master_cfg & DW_IC_CON_SPEED_FAST;
-
-	if (!ACPI_HANDLE(&pdev->dev))
-		return -ENODEV;
+	const struct acpi_device_id *id;
 
 	dev->adapter.nr = -1;
 	dev->tx_fifo_depth = 32;
@@ -92,14 +91,33 @@ static int dw_i2c_acpi_configure(struct platform_device *pdev)
 	 * Try to get SDA hold time and *CNT values from an ACPI method if
 	 * it exists for both supported speed modes.
 	 */
-	dw_i2c_acpi_params(pdev, "SSCN", &dev->ss_hcnt, &dev->ss_lcnt,
-			   fs_mode ? NULL : &dev->sda_hold_time);
+	dw_i2c_acpi_params(pdev, "SSCN", &dev->ss_hcnt, &dev->ss_lcnt, NULL);
 	dw_i2c_acpi_params(pdev, "FMCN", &dev->fs_hcnt, &dev->fs_lcnt,
-			   fs_mode ? &dev->sda_hold_time : NULL);
+			   &dev->sda_hold_time);
+
+	/*
+	 * Provide a way for Designware I2C host controllers that are not
+	 * based on Intel LPSS to specify their input clock frequency via
+	 * id->driver_data.
+	 */
+	id = acpi_match_device(pdev->dev.driver->acpi_match_table, &pdev->dev);
+	if (id && id->driver_data)
+		clk_register_fixed_rate(&pdev->dev, dev_name(&pdev->dev), NULL,
+					CLK_IS_ROOT, id->driver_data);
 
 	return 0;
 }
 
+static void dw_i2c_acpi_unconfigure(struct platform_device *pdev)
+{
+	struct dw_i2c_dev *dev = platform_get_drvdata(pdev);
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(pdev->dev.driver->acpi_match_table, &pdev->dev);
+	if (id && id->driver_data)
+		clk_unregister(dev->clk);
+}
+
 static const struct acpi_device_id dw_i2c_acpi_match[] = {
 	{ "INT33C2", 0 },
 	{ "INT33C3", 0 },
@@ -107,6 +125,7 @@ static const struct acpi_device_id dw_i2c_acpi_match[] = {
 	{ "INT3433", 0 },
 	{ "80860F41", 0 },
 	{ "808622C1", 0 },
+	{ "AMD0010", 133 * 1000 * 1000 },
 	{ }
 };
 MODULE_DEVICE_TABLE(acpi, dw_i2c_acpi_match);
@@ -115,6 +134,7 @@ static inline int dw_i2c_acpi_configure(struct platform_device *pdev)
 {
 	return -ENODEV;
 }
+static inline void dw_i2c_acpi_unconfigure(struct platform_device *pdev) { }
 #endif
 
 static int dw_i2c_probe(struct platform_device *pdev)
@@ -122,7 +142,9 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	struct dw_i2c_dev *dev;
 	struct i2c_adapter *adap;
 	struct resource *mem;
+	struct dw_i2c_platform_data *pdata;
 	int irq, r;
+	u32 clk_freq, ht = 0;
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
@@ -145,21 +167,14 @@ static int dw_i2c_probe(struct platform_device *pdev)
 	dev->irq = irq;
 	platform_set_drvdata(pdev, dev);
 
-	dev->clk = devm_clk_get(&pdev->dev, NULL);
-	dev->get_clk_rate_khz = i2c_dw_get_clk_rate_khz;
-
-	if (IS_ERR(dev->clk))
-		return PTR_ERR(dev->clk);
-	clk_prepare_enable(dev->clk);
-
-	if (pdev->dev.of_node) {
-		u32 ht = 0;
-		u32 ic_clk = dev->get_clk_rate_khz(dev);
+	/* fast mode by default because of legacy reasons */
+	clk_freq = 400000;
 
+	if (ACPI_COMPANION(&pdev->dev)) {
+		dw_i2c_acpi_configure(pdev);
+	} else if (pdev->dev.of_node) {
 		of_property_read_u32(pdev->dev.of_node,
 					"i2c-sda-hold-time-ns", &ht);
-		dev->sda_hold_time = div_u64((u64)ic_clk * ht + 500000,
-					     1000000);
 
 		of_property_read_u32(pdev->dev.of_node,
 				     "i2c-sda-falling-time-ns",
@@ -167,6 +182,21 @@ static int dw_i2c_probe(struct platform_device *pdev)
 		of_property_read_u32(pdev->dev.of_node,
 				     "i2c-scl-falling-time-ns",
 				     &dev->scl_falling_time);
+
+		of_property_read_u32(pdev->dev.of_node, "clock-frequency",
+				     &clk_freq);
+
+		/* Only standard mode at 100kHz and fast mode at 400kHz
+		 * are supported.
+		 */
+		if (clk_freq != 100000 && clk_freq != 400000) {
+			dev_err(&pdev->dev, "Only 100kHz and 400kHz supported");
+			return -EINVAL;
+		}
+	} else {
+		pdata = dev_get_platdata(&pdev->dev);
+		if (pdata)
+			clk_freq = pdata->i2c_scl_freq;
 	}
 
 	dev->functionality =
@@ -176,12 +206,27 @@ static int dw_i2c_probe(struct platform_device *pdev)
 		I2C_FUNC_SMBUS_BYTE_DATA |
 		I2C_FUNC_SMBUS_WORD_DATA |
 		I2C_FUNC_SMBUS_I2C_BLOCK;
-	dev->master_cfg =  DW_IC_CON_MASTER | DW_IC_CON_SLAVE_DISABLE |
-		DW_IC_CON_RESTART_EN | DW_IC_CON_SPEED_FAST;
+	if (clk_freq == 100000)
+		dev->master_cfg =  DW_IC_CON_MASTER | DW_IC_CON_SLAVE_DISABLE |
+			DW_IC_CON_RESTART_EN | DW_IC_CON_SPEED_STD;
+	else
+		dev->master_cfg =  DW_IC_CON_MASTER | DW_IC_CON_SLAVE_DISABLE |
+			DW_IC_CON_RESTART_EN | DW_IC_CON_SPEED_FAST;
 
-	/* Try first if we can configure the device from ACPI */
-	r = dw_i2c_acpi_configure(pdev);
-	if (r) {
+	dev->clk = devm_clk_get(&pdev->dev, NULL);
+	dev->get_clk_rate_khz = i2c_dw_get_clk_rate_khz;
+	if (IS_ERR(dev->clk))
+		return PTR_ERR(dev->clk);
+	clk_prepare_enable(dev->clk);
+
+	if (!dev->sda_hold_time && ht) {
+		u32 ic_clk = dev->get_clk_rate_khz(dev);
+
+		dev->sda_hold_time = div_u64((u64)ic_clk * ht + 500000,
+					     1000000);
+	}
+
+	if (!dev->tx_fifo_depth) {
 		u32 param1 = i2c_dw_read_comp_param(dev);
 
 		dev->tx_fifo_depth = ((param1 >> 16) & 0xff) + 1;
@@ -237,6 +282,9 @@ static int dw_i2c_remove(struct platform_device *pdev)
 	pm_runtime_put(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
+	if (ACPI_COMPANION(&pdev->dev))
+		dw_i2c_acpi_unconfigure(pdev);
+
 	return 0;
 }
 
diff --git a/drivers/i2c/busses/i2c-exynos5.c b/drivers/i2c/busses/i2c-exynos5.c
index 28073f1d6d47..81e6263cd7da 100644
--- a/drivers/i2c/busses/i2c-exynos5.c
+++ b/drivers/i2c/busses/i2c-exynos5.c
@@ -83,7 +83,6 @@
 #define HSI2C_INT_TX_ALMOSTEMPTY_EN		(1u << 0)
 #define HSI2C_INT_RX_ALMOSTFULL_EN		(1u << 1)
 #define HSI2C_INT_TRAILING_EN			(1u << 6)
-#define HSI2C_INT_I2C_EN			(1u << 9)
 
 /* I2C_INT_STAT Register bits */
 #define HSI2C_INT_TX_ALMOSTEMPTY		(1u << 0)
@@ -95,6 +94,17 @@
 #define HSI2C_INT_TRAILING			(1u << 6)
 #define HSI2C_INT_I2C				(1u << 9)
 
+#define HSI2C_INT_TRANS_DONE			(1u << 7)
+#define HSI2C_INT_TRANS_ABORT			(1u << 8)
+#define HSI2C_INT_NO_DEV_ACK			(1u << 9)
+#define HSI2C_INT_NO_DEV			(1u << 10)
+#define HSI2C_INT_TIMEOUT			(1u << 11)
+#define HSI2C_INT_I2C_TRANS			(HSI2C_INT_TRANS_DONE |	\
+						HSI2C_INT_TRANS_ABORT |	\
+						HSI2C_INT_NO_DEV_ACK |	\
+						HSI2C_INT_NO_DEV |	\
+						HSI2C_INT_TIMEOUT)
+
 /* I2C_FIFO_STAT Register bits */
 #define HSI2C_RX_FIFO_EMPTY			(1u << 24)
 #define HSI2C_RX_FIFO_FULL			(1u << 23)
@@ -143,6 +153,8 @@
 
 #define EXYNOS5_I2C_TIMEOUT (msecs_to_jiffies(1000))
 
+#define HSI2C_EXYNOS7	BIT(0)
+
 struct exynos5_i2c {
 	struct i2c_adapter	adap;
 	unsigned int		suspended:1;
@@ -192,6 +204,7 @@ struct exynos5_i2c {
  */
 struct exynos_hsi2c_variant {
 	unsigned int	fifo_depth;
+	unsigned int	hw;
 };
 
 static const struct exynos_hsi2c_variant exynos5250_hsi2c_data = {
@@ -202,6 +215,11 @@ static const struct exynos_hsi2c_variant exynos5260_hsi2c_data = {
 	.fifo_depth	= 16,
 };
 
+static const struct exynos_hsi2c_variant exynos7_hsi2c_data = {
+	.fifo_depth	= 16,
+	.hw		= HSI2C_EXYNOS7,
+};
+
 static const struct of_device_id exynos5_i2c_match[] = {
 	{
 		.compatible = "samsung,exynos5-hsi2c",
@@ -212,6 +230,9 @@ static const struct of_device_id exynos5_i2c_match[] = {
 	}, {
 		.compatible = "samsung,exynos5260-hsi2c",
 		.data = &exynos5260_hsi2c_data
+	}, {
+		.compatible = "samsung,exynos7-hsi2c",
+		.data = &exynos7_hsi2c_data
 	}, {},
 };
 MODULE_DEVICE_TABLE(of, exynos5_i2c_match);
@@ -256,13 +277,24 @@ static int exynos5_i2c_set_timing(struct exynos5_i2c *i2c, int mode)
 				i2c->hs_clock : i2c->fs_clock;
 
 	/*
+	 * In case of HSI2C controller in Exynos5 series
 	 * FPCLK / FI2C =
 	 * (CLK_DIV + 1) * (TSCLK_L + TSCLK_H + 2) + 8 + 2 * FLT_CYCLE
+	 *
+	 * In case of HSI2C controllers in Exynos7 series
+	 * FPCLK / FI2C =
+	 * (CLK_DIV + 1) * (TSCLK_L + TSCLK_H + 2) + 8 + FLT_CYCLE
+	 *
 	 * utemp0 = (CLK_DIV + 1) * (TSCLK_L + TSCLK_H + 2)
 	 * utemp1 = (TSCLK_L + TSCLK_H + 2)
 	 */
 	t_ftl_cycle = (readl(i2c->regs + HSI2C_CONF) >> 16) & 0x7;
-	utemp0 = (clkin / op_clk) - 8 - 2 * t_ftl_cycle;
+	utemp0 = (clkin / op_clk) - 8;
+
+	if (i2c->variant->hw == HSI2C_EXYNOS7)
+		utemp0 -= t_ftl_cycle;
+	else
+		utemp0 -= 2 * t_ftl_cycle;
 
 	/* CLK_DIV max is 256 */
 	for (div = 0; div < 256; div++) {
@@ -407,7 +439,28 @@ static irqreturn_t exynos5_i2c_irq(int irqno, void *dev_id)
 	writel(int_status, i2c->regs + HSI2C_INT_STATUS);
 
 	/* handle interrupt related to the transfer status */
-	if (int_status & HSI2C_INT_I2C) {
+	if (i2c->variant->hw == HSI2C_EXYNOS7) {
+		if (int_status & HSI2C_INT_TRANS_DONE) {
+			i2c->trans_done = 1;
+			i2c->state = 0;
+		} else if (int_status & HSI2C_INT_TRANS_ABORT) {
+			dev_dbg(i2c->dev, "Deal with arbitration lose\n");
+			i2c->state = -EAGAIN;
+			goto stop;
+		} else if (int_status & HSI2C_INT_NO_DEV_ACK) {
+			dev_dbg(i2c->dev, "No ACK from device\n");
+			i2c->state = -ENXIO;
+			goto stop;
+		} else if (int_status & HSI2C_INT_NO_DEV) {
+			dev_dbg(i2c->dev, "No device\n");
+			i2c->state = -ENXIO;
+			goto stop;
+		} else if (int_status & HSI2C_INT_TIMEOUT) {
+			dev_dbg(i2c->dev, "Accessing device timed out\n");
+			i2c->state = -EAGAIN;
+			goto stop;
+		}
+	} else if (int_status & HSI2C_INT_I2C) {
 		trans_status = readl(i2c->regs + HSI2C_TRANS_STATUS);
 		if (trans_status & HSI2C_NO_DEV_ACK) {
 			dev_dbg(i2c->dev, "No ACK from device\n");
@@ -512,12 +565,17 @@ static int exynos5_i2c_wait_bus_idle(struct exynos5_i2c *i2c)
 static void exynos5_i2c_message_start(struct exynos5_i2c *i2c, int stop)
 {
 	u32 i2c_ctl;
-	u32 int_en = HSI2C_INT_I2C_EN;
+	u32 int_en = 0;
 	u32 i2c_auto_conf = 0;
 	u32 fifo_ctl;
 	unsigned long flags;
 	unsigned short trig_lvl;
 
+	if (i2c->variant->hw == HSI2C_EXYNOS7)
+		int_en |= HSI2C_INT_I2C_TRANS;
+	else
+		int_en |= HSI2C_INT_I2C;
+
 	i2c_ctl = readl(i2c->regs + HSI2C_CTL);
 	i2c_ctl &= ~(HSI2C_TXCHON | HSI2C_RXCHON);
 	fifo_ctl = HSI2C_RXFIFO_EN | HSI2C_TXFIFO_EN;
@@ -724,12 +782,13 @@ static int exynos5_i2c_probe(struct platform_device *pdev)
 		goto err_clk;
 	}
 
+	/* Need to check the variant before setting up. */
+	i2c->variant = exynos5_i2c_get_variant(pdev);
+
 	ret = exynos5_hsi2c_clock_setup(i2c);
 	if (ret)
 		goto err_clk;
 
-	i2c->variant = exynos5_i2c_get_variant(pdev);
-
 	exynos5_i2c_reset(i2c);
 
 	ret = i2c_add_adapter(&i2c->adap);
diff --git a/drivers/i2c/busses/i2c-hix5hd2.c b/drivers/i2c/busses/i2c-hix5hd2.c
new file mode 100644
index 000000000000..9490d0f4255c
--- /dev/null
+++ b/drivers/i2c/busses/i2c-hix5hd2.c
@@ -0,0 +1,557 @@
+/*
+ * Copyright (c) 2014 Linaro Ltd.
+ * Copyright (c) 2014 Hisilicon Limited.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Now only support 7 bit address.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+
+/* Register Map */
+#define HIX5I2C_CTRL		0x00
+#define HIX5I2C_COM		0x04
+#define HIX5I2C_ICR		0x08
+#define HIX5I2C_SR		0x0c
+#define HIX5I2C_SCL_H		0x10
+#define HIX5I2C_SCL_L		0x14
+#define HIX5I2C_TXR		0x18
+#define HIX5I2C_RXR		0x1c
+
+/* I2C_CTRL_REG */
+#define I2C_ENABLE		BIT(8)
+#define I2C_UNMASK_TOTAL	BIT(7)
+#define I2C_UNMASK_START	BIT(6)
+#define I2C_UNMASK_END		BIT(5)
+#define I2C_UNMASK_SEND		BIT(4)
+#define I2C_UNMASK_RECEIVE	BIT(3)
+#define I2C_UNMASK_ACK		BIT(2)
+#define I2C_UNMASK_ARBITRATE	BIT(1)
+#define I2C_UNMASK_OVER		BIT(0)
+#define I2C_UNMASK_ALL		(I2C_UNMASK_ACK | I2C_UNMASK_OVER)
+
+/* I2C_COM_REG */
+#define I2C_NO_ACK		BIT(4)
+#define I2C_START		BIT(3)
+#define I2C_READ		BIT(2)
+#define I2C_WRITE		BIT(1)
+#define I2C_STOP		BIT(0)
+
+/* I2C_ICR_REG */
+#define I2C_CLEAR_START		BIT(6)
+#define I2C_CLEAR_END		BIT(5)
+#define I2C_CLEAR_SEND		BIT(4)
+#define I2C_CLEAR_RECEIVE	BIT(3)
+#define I2C_CLEAR_ACK		BIT(2)
+#define I2C_CLEAR_ARBITRATE	BIT(1)
+#define I2C_CLEAR_OVER		BIT(0)
+#define I2C_CLEAR_ALL		(I2C_CLEAR_START | I2C_CLEAR_END | \
+				I2C_CLEAR_SEND | I2C_CLEAR_RECEIVE | \
+				I2C_CLEAR_ACK | I2C_CLEAR_ARBITRATE | \
+				I2C_CLEAR_OVER)
+
+/* I2C_SR_REG */
+#define I2C_BUSY		BIT(7)
+#define I2C_START_INTR		BIT(6)
+#define I2C_END_INTR		BIT(5)
+#define I2C_SEND_INTR		BIT(4)
+#define I2C_RECEIVE_INTR	BIT(3)
+#define I2C_ACK_INTR		BIT(2)
+#define I2C_ARBITRATE_INTR	BIT(1)
+#define I2C_OVER_INTR		BIT(0)
+
+#define HIX5I2C_MAX_FREQ	400000		/* 400k */
+#define HIX5I2C_READ_OPERATION	0x01
+
+enum hix5hd2_i2c_state {
+	HIX5I2C_STAT_RW_ERR = -1,
+	HIX5I2C_STAT_INIT,
+	HIX5I2C_STAT_RW,
+	HIX5I2C_STAT_SND_STOP,
+	HIX5I2C_STAT_RW_SUCCESS,
+};
+
+struct hix5hd2_i2c_priv {
+	struct i2c_adapter adap;
+	struct i2c_msg *msg;
+	struct completion msg_complete;
+	unsigned int msg_idx;
+	unsigned int msg_len;
+	int stop;
+	void __iomem *regs;
+	struct clk *clk;
+	struct device *dev;
+	spinlock_t lock;	/* IRQ synchronization */
+	int err;
+	unsigned int freq;
+	enum hix5hd2_i2c_state state;
+};
+
+static u32 hix5hd2_i2c_clr_pend_irq(struct hix5hd2_i2c_priv *priv)
+{
+	u32 val = readl_relaxed(priv->regs + HIX5I2C_SR);
+
+	writel_relaxed(val, priv->regs + HIX5I2C_ICR);
+
+	return val;
+}
+
+static void hix5hd2_i2c_clr_all_irq(struct hix5hd2_i2c_priv *priv)
+{
+	writel_relaxed(I2C_CLEAR_ALL, priv->regs + HIX5I2C_ICR);
+}
+
+static void hix5hd2_i2c_disable_irq(struct hix5hd2_i2c_priv *priv)
+{
+	writel_relaxed(0, priv->regs + HIX5I2C_CTRL);
+}
+
+static void hix5hd2_i2c_enable_irq(struct hix5hd2_i2c_priv *priv)
+{
+	writel_relaxed(I2C_ENABLE | I2C_UNMASK_TOTAL | I2C_UNMASK_ALL,
+		       priv->regs + HIX5I2C_CTRL);
+}
+
+static void hix5hd2_i2c_drv_setrate(struct hix5hd2_i2c_priv *priv)
+{
+	u32 rate, val;
+	u32 scl, sysclock;
+
+	/* close all i2c interrupt */
+	val = readl_relaxed(priv->regs + HIX5I2C_CTRL);
+	writel_relaxed(val & (~I2C_UNMASK_TOTAL), priv->regs + HIX5I2C_CTRL);
+
+	rate = priv->freq;
+	sysclock = clk_get_rate(priv->clk);
+	scl = (sysclock / (rate * 2)) / 2 - 1;
+	writel_relaxed(scl, priv->regs + HIX5I2C_SCL_H);
+	writel_relaxed(scl, priv->regs + HIX5I2C_SCL_L);
+
+	/* restore original interrupt*/
+	writel_relaxed(val, priv->regs + HIX5I2C_CTRL);
+
+	dev_dbg(priv->dev, "%s: sysclock=%d, rate=%d, scl=%d\n",
+		__func__, sysclock, rate, scl);
+}
+
+static void hix5hd2_i2c_init(struct hix5hd2_i2c_priv *priv)
+{
+	hix5hd2_i2c_disable_irq(priv);
+	hix5hd2_i2c_drv_setrate(priv);
+	hix5hd2_i2c_clr_all_irq(priv);
+	hix5hd2_i2c_enable_irq(priv);
+}
+
+static void hix5hd2_i2c_reset(struct hix5hd2_i2c_priv *priv)
+{
+	clk_disable_unprepare(priv->clk);
+	msleep(20);
+	clk_prepare_enable(priv->clk);
+	hix5hd2_i2c_init(priv);
+}
+
+static int hix5hd2_i2c_wait_bus_idle(struct hix5hd2_i2c_priv *priv)
+{
+	unsigned long stop_time;
+	u32 int_status;
+
+	/* wait for 100 milli seconds for the bus to be idle */
+	stop_time = jiffies + msecs_to_jiffies(100);
+	do {
+		int_status = hix5hd2_i2c_clr_pend_irq(priv);
+		if (!(int_status & I2C_BUSY))
+			return 0;
+
+		usleep_range(50, 200);
+	} while (time_before(jiffies, stop_time));
+
+	return -EBUSY;
+}
+
+static void hix5hd2_rw_over(struct hix5hd2_i2c_priv *priv)
+{
+	if (priv->state == HIX5I2C_STAT_SND_STOP)
+		dev_dbg(priv->dev, "%s: rw and send stop over\n", __func__);
+	else
+		dev_dbg(priv->dev, "%s: have not data to send\n", __func__);
+
+	priv->state = HIX5I2C_STAT_RW_SUCCESS;
+	priv->err = 0;
+}
+
+static void hix5hd2_rw_handle_stop(struct hix5hd2_i2c_priv *priv)
+{
+	if (priv->stop) {
+		priv->state = HIX5I2C_STAT_SND_STOP;
+		writel_relaxed(I2C_STOP, priv->regs + HIX5I2C_COM);
+	} else {
+		hix5hd2_rw_over(priv);
+	}
+}
+
+static void hix5hd2_read_handle(struct hix5hd2_i2c_priv *priv)
+{
+	if (priv->msg_len == 1) {
+		/* the last byte don't need send ACK */
+		writel_relaxed(I2C_READ | I2C_NO_ACK, priv->regs + HIX5I2C_COM);
+	} else if (priv->msg_len > 1) {
+		/* if i2c master receive data will send ACK */
+		writel_relaxed(I2C_READ, priv->regs + HIX5I2C_COM);
+	} else {
+		hix5hd2_rw_handle_stop(priv);
+	}
+}
+
+static void hix5hd2_write_handle(struct hix5hd2_i2c_priv *priv)
+{
+	u8 data;
+
+	if (priv->msg_len > 0) {
+		data = priv->msg->buf[priv->msg_idx++];
+		writel_relaxed(data, priv->regs + HIX5I2C_TXR);
+		writel_relaxed(I2C_WRITE, priv->regs + HIX5I2C_COM);
+	} else {
+		hix5hd2_rw_handle_stop(priv);
+	}
+}
+
+static int hix5hd2_rw_preprocess(struct hix5hd2_i2c_priv *priv)
+{
+	u8 data;
+
+	if (priv->state == HIX5I2C_STAT_INIT) {
+		priv->state = HIX5I2C_STAT_RW;
+	} else if (priv->state == HIX5I2C_STAT_RW) {
+		if (priv->msg->flags & I2C_M_RD) {
+			data = readl_relaxed(priv->regs + HIX5I2C_RXR);
+			priv->msg->buf[priv->msg_idx++] = data;
+		}
+		priv->msg_len--;
+	} else {
+		dev_dbg(priv->dev, "%s: error: priv->state = %d, msg_len = %d\n",
+			__func__, priv->state, priv->msg_len);
+		return -EAGAIN;
+	}
+	return 0;
+}
+
+static irqreturn_t hix5hd2_i2c_irq(int irqno, void *dev_id)
+{
+	struct hix5hd2_i2c_priv *priv = dev_id;
+	u32 int_status;
+	int ret;
+
+	spin_lock(&priv->lock);
+
+	int_status = hix5hd2_i2c_clr_pend_irq(priv);
+
+	/* handle error */
+	if (int_status & I2C_ARBITRATE_INTR) {
+		/* bus error */
+		dev_dbg(priv->dev, "ARB bus loss\n");
+		priv->err = -EAGAIN;
+		priv->state = HIX5I2C_STAT_RW_ERR;
+		goto stop;
+	} else if (int_status & I2C_ACK_INTR) {
+		/* ack error */
+		dev_dbg(priv->dev, "No ACK from device\n");
+		priv->err = -ENXIO;
+		priv->state = HIX5I2C_STAT_RW_ERR;
+		goto stop;
+	}
+
+	if (int_status & I2C_OVER_INTR) {
+		if (priv->msg_len > 0) {
+			ret = hix5hd2_rw_preprocess(priv);
+			if (ret) {
+				priv->err = ret;
+				priv->state = HIX5I2C_STAT_RW_ERR;
+				goto stop;
+			}
+			if (priv->msg->flags & I2C_M_RD)
+				hix5hd2_read_handle(priv);
+			else
+				hix5hd2_write_handle(priv);
+		} else {
+			hix5hd2_rw_over(priv);
+		}
+	}
+
+stop:
+	if ((priv->state == HIX5I2C_STAT_RW_SUCCESS &&
+	     priv->msg->len == priv->msg_idx) ||
+	    (priv->state == HIX5I2C_STAT_RW_ERR)) {
+		hix5hd2_i2c_disable_irq(priv);
+		hix5hd2_i2c_clr_pend_irq(priv);
+		complete(&priv->msg_complete);
+	}
+
+	spin_unlock(&priv->lock);
+
+	return IRQ_HANDLED;
+}
+
+static void hix5hd2_i2c_message_start(struct hix5hd2_i2c_priv *priv, int stop)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	hix5hd2_i2c_clr_all_irq(priv);
+	hix5hd2_i2c_enable_irq(priv);
+
+	if (priv->msg->flags & I2C_M_RD)
+		writel_relaxed((priv->msg->addr << 1) | HIX5I2C_READ_OPERATION,
+			       priv->regs + HIX5I2C_TXR);
+	else
+		writel_relaxed(priv->msg->addr << 1,
+			       priv->regs + HIX5I2C_TXR);
+
+	writel_relaxed(I2C_WRITE | I2C_START, priv->regs + HIX5I2C_COM);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static int hix5hd2_i2c_xfer_msg(struct hix5hd2_i2c_priv *priv,
+				struct i2c_msg *msgs, int stop)
+{
+	unsigned long timeout;
+	int ret;
+
+	priv->msg = msgs;
+	priv->msg_idx = 0;
+	priv->msg_len = priv->msg->len;
+	priv->stop = stop;
+	priv->err = 0;
+	priv->state = HIX5I2C_STAT_INIT;
+
+	reinit_completion(&priv->msg_complete);
+	hix5hd2_i2c_message_start(priv, stop);
+
+	timeout = wait_for_completion_timeout(&priv->msg_complete,
+					      priv->adap.timeout);
+	if (timeout == 0) {
+		priv->state = HIX5I2C_STAT_RW_ERR;
+		priv->err = -ETIMEDOUT;
+		dev_warn(priv->dev, "%s timeout=%d\n",
+			 msgs->flags & I2C_M_RD ? "rx" : "tx",
+			 priv->adap.timeout);
+	}
+	ret = priv->state;
+
+	/*
+	 * If this is the last message to be transfered (stop == 1)
+	 * Then check if the bus can be brought back to idle.
+	 */
+	if (priv->state == HIX5I2C_STAT_RW_SUCCESS && stop)
+		ret = hix5hd2_i2c_wait_bus_idle(priv);
+
+	if (ret < 0)
+		hix5hd2_i2c_reset(priv);
+
+	return priv->err;
+}
+
+static int hix5hd2_i2c_xfer(struct i2c_adapter *adap,
+			    struct i2c_msg *msgs, int num)
+{
+	struct hix5hd2_i2c_priv *priv = i2c_get_adapdata(adap);
+	int i, ret, stop;
+
+	pm_runtime_get_sync(priv->dev);
+
+	for (i = 0; i < num; i++, msgs++) {
+		stop = (i == num - 1);
+		ret = hix5hd2_i2c_xfer_msg(priv, msgs, stop);
+		if (ret < 0)
+			goto out;
+	}
+
+	if (i == num) {
+		ret = num;
+	} else {
+		/* Only one message, cannot access the device */
+		if (i == 1)
+			ret = -EREMOTEIO;
+		else
+			ret = i;
+
+		dev_warn(priv->dev, "xfer message failed\n");
+	}
+
+out:
+	pm_runtime_mark_last_busy(priv->dev);
+	pm_runtime_put_autosuspend(priv->dev);
+	return ret;
+}
+
+static u32 hix5hd2_i2c_func(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C | (I2C_FUNC_SMBUS_EMUL & ~I2C_FUNC_SMBUS_QUICK);
+}
+
+static const struct i2c_algorithm hix5hd2_i2c_algorithm = {
+	.master_xfer		= hix5hd2_i2c_xfer,
+	.functionality		= hix5hd2_i2c_func,
+};
+
+static int hix5hd2_i2c_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct hix5hd2_i2c_priv *priv;
+	struct resource *mem;
+	unsigned int freq;
+	int irq, ret;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	if (of_property_read_u32(np, "clock-frequency", &freq)) {
+		/* use 100k as default value */
+		priv->freq = 100000;
+	} else {
+		if (freq > HIX5I2C_MAX_FREQ) {
+			priv->freq = HIX5I2C_MAX_FREQ;
+			dev_warn(priv->dev, "use max freq %d instead\n",
+				 HIX5I2C_MAX_FREQ);
+		} else {
+			priv->freq = freq;
+		}
+	}
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->regs = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(priv->regs))
+		return PTR_ERR(priv->regs);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq <= 0) {
+		dev_err(&pdev->dev, "cannot find HS-I2C IRQ\n");
+		return irq;
+	}
+
+	priv->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(priv->clk)) {
+		dev_err(&pdev->dev, "cannot get clock\n");
+		return PTR_ERR(priv->clk);
+	}
+	clk_prepare_enable(priv->clk);
+
+	strlcpy(priv->adap.name, "hix5hd2-i2c", sizeof(priv->adap.name));
+	priv->dev = &pdev->dev;
+	priv->adap.owner = THIS_MODULE;
+	priv->adap.algo = &hix5hd2_i2c_algorithm;
+	priv->adap.retries = 3;
+	priv->adap.dev.of_node = np;
+	priv->adap.algo_data = priv;
+	priv->adap.dev.parent = &pdev->dev;
+	i2c_set_adapdata(&priv->adap, priv);
+	platform_set_drvdata(pdev, priv);
+	spin_lock_init(&priv->lock);
+	init_completion(&priv->msg_complete);
+
+	hix5hd2_i2c_init(priv);
+
+	ret = devm_request_irq(&pdev->dev, irq, hix5hd2_i2c_irq,
+			       IRQF_NO_SUSPEND | IRQF_ONESHOT,
+			       dev_name(&pdev->dev), priv);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "cannot request HS-I2C IRQ %d\n", irq);
+		goto err_clk;
+	}
+
+	pm_suspend_ignore_children(&pdev->dev, true);
+	pm_runtime_set_autosuspend_delay(priv->dev, MSEC_PER_SEC);
+	pm_runtime_use_autosuspend(priv->dev);
+	pm_runtime_set_active(priv->dev);
+	pm_runtime_enable(priv->dev);
+
+	ret = i2c_add_adapter(&priv->adap);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to add bus to i2c core\n");
+		goto err_runtime;
+	}
+
+	return ret;
+
+err_runtime:
+	pm_runtime_disable(priv->dev);
+	pm_runtime_set_suspended(priv->dev);
+err_clk:
+	clk_disable_unprepare(priv->clk);
+	return ret;
+}
+
+static int hix5hd2_i2c_remove(struct platform_device *pdev)
+{
+	struct hix5hd2_i2c_priv *priv = platform_get_drvdata(pdev);
+
+	i2c_del_adapter(&priv->adap);
+	pm_runtime_disable(priv->dev);
+	pm_runtime_set_suspended(priv->dev);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int hix5hd2_i2c_runtime_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct hix5hd2_i2c_priv *priv = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(priv->clk);
+
+	return 0;
+}
+
+static int hix5hd2_i2c_runtime_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct hix5hd2_i2c_priv *priv = platform_get_drvdata(pdev);
+
+	clk_prepare_enable(priv->clk);
+	hix5hd2_i2c_init(priv);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops hix5hd2_i2c_pm_ops = {
+	SET_PM_RUNTIME_PM_OPS(hix5hd2_i2c_runtime_suspend,
+			      hix5hd2_i2c_runtime_resume,
+			      NULL)
+};
+
+static const struct of_device_id hix5hd2_i2c_match[] = {
+	{ .compatible = "hisilicon,hix5hd2-i2c" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, hix5hd2_i2c_match);
+
+static struct platform_driver hix5hd2_i2c_driver = {
+	.probe		= hix5hd2_i2c_probe,
+	.remove		= hix5hd2_i2c_remove,
+	.driver		= {
+		.name	= "hix5hd2-i2c",
+		.pm	= &hix5hd2_i2c_pm_ops,
+		.of_match_table = hix5hd2_i2c_match,
+	},
+};
+
+module_platform_driver(hix5hd2_i2c_driver);
+
+MODULE_DESCRIPTION("Hix5hd2 I2C Bus driver");
+MODULE_AUTHOR("Wei Yan <sledge.yanwei@huawei.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:i2c-hix5hd2");
diff --git a/drivers/i2c/busses/i2c-i801.c b/drivers/i2c/busses/i2c-i801.c
index 2994690b26e9..7cfc183b3d63 100644
--- a/drivers/i2c/busses/i2c-i801.c
+++ b/drivers/i2c/busses/i2c-i801.c
@@ -62,6 +62,7 @@
  * Wildcat Point (PCH)		0x8ca2	32	hard	yes	yes	yes
  * Wildcat Point-LP (PCH)	0x9ca2	32	hard	yes	yes	yes
  * BayTrail (SOC)		0x0f12	32	hard	yes	yes	yes
+ * Sunrise Point-H (PCH) 	0xa123  32	hard	yes	yes	yes
  *
  * Features supported by this driver:
  * Software PEC				no
@@ -164,6 +165,7 @@
 
 /* Older devices have their ID defined in <linux/pci_ids.h> */
 #define PCI_DEVICE_ID_INTEL_BAYTRAIL_SMBUS		0x0f12
+#define PCI_DEVICE_ID_INTEL_BRASWELL_SMBUS		0x2292
 #define PCI_DEVICE_ID_INTEL_COUGARPOINT_SMBUS		0x1c22
 #define PCI_DEVICE_ID_INTEL_PATSBURG_SMBUS		0x1d22
 /* Patsburg also has three 'Integrated Device Function' SMBus controllers */
@@ -183,6 +185,7 @@
 #define PCI_DEVICE_ID_INTEL_WELLSBURG_SMBUS_MS2		0x8d7f
 #define PCI_DEVICE_ID_INTEL_LYNXPOINT_LP_SMBUS		0x9c22
 #define PCI_DEVICE_ID_INTEL_WILDCATPOINT_LP_SMBUS	0x9ca2
+#define PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_SMBUS	0xa123
 
 struct i801_mux_config {
 	char *gpio_chip;
@@ -828,6 +831,8 @@ static const struct pci_device_id i801_ids[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_WILDCATPOINT_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_WILDCATPOINT_LP_SMBUS) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BAYTRAIL_SMBUS) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BRASWELL_SMBUS) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SUNRISEPOINT_H_SMBUS) },
 	{ 0, }
 };
 
diff --git a/drivers/i2c/busses/i2c-imx.c b/drivers/i2c/busses/i2c-imx.c
index 613069bc561a..c48e46af670a 100644
--- a/drivers/i2c/busses/i2c-imx.c
+++ b/drivers/i2c/busses/i2c-imx.c
@@ -268,6 +268,14 @@ static int i2c_imx_bus_busy(struct imx_i2c_struct *i2c_imx, int for_busy)
 
 	while (1) {
 		temp = imx_i2c_read_reg(i2c_imx, IMX_I2C_I2SR);
+
+		/* check for arbitration lost */
+		if (temp & I2SR_IAL) {
+			temp &= ~I2SR_IAL;
+			imx_i2c_write_reg(temp, i2c_imx, IMX_I2C_I2SR);
+			return -EAGAIN;
+		}
+
 		if (for_busy && (temp & I2SR_IBB))
 			break;
 		if (!for_busy && !(temp & I2SR_IBB))
@@ -702,7 +710,7 @@ static int i2c_imx_probe(struct platform_device *pdev)
 				pdev->name, i2c_imx);
 	if (ret) {
 		dev_err(&pdev->dev, "can't claim irq %d\n", irq);
-		return ret;
+		goto clk_disable;
 	}
 
 	/* Init queue */
@@ -727,7 +735,7 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	ret = i2c_add_numbered_adapter(&i2c_imx->adapter);
 	if (ret < 0) {
 		dev_err(&pdev->dev, "registration failed\n");
-		return ret;
+		goto clk_disable;
 	}
 
 	/* Set up platform driver data */
@@ -741,6 +749,10 @@ static int i2c_imx_probe(struct platform_device *pdev)
 	dev_info(&i2c_imx->adapter.dev, "IMX I2C adapter registered\n");
 
 	return 0;   /* Return OK */
+
+clk_disable:
+	clk_disable_unprepare(i2c_imx->clk);
+	return ret;
 }
 
 static int i2c_imx_remove(struct platform_device *pdev)
diff --git a/drivers/i2c/busses/i2c-ismt.c b/drivers/i2c/busses/i2c-ismt.c
index 984492553e95..3f6ecbfb9a56 100644
--- a/drivers/i2c/busses/i2c-ismt.c
+++ b/drivers/i2c/busses/i2c-ismt.c
@@ -81,7 +81,7 @@
 #define PCI_DEVICE_ID_INTEL_S1200_SMT1	0x0c5a
 #define PCI_DEVICE_ID_INTEL_AVOTON_SMT	0x1f15
 
-#define ISMT_DESC_ENTRIES	32	/* number of descriptor entries */
+#define ISMT_DESC_ENTRIES	2	/* number of descriptor entries */
 #define ISMT_MAX_RETRIES	3	/* number of SMBus retries to attempt */
 
 /* Hardware Descriptor Constants - Control Field */
@@ -497,7 +497,7 @@ static int ismt_access(struct i2c_adapter *adap, u16 addr,
 			desc->wr_len_cmd = dma_size;
 			desc->control |= ISMT_DESC_BLK;
 			priv->dma_buffer[0] = command;
-			memcpy(&priv->dma_buffer[1], &data->block[1], dma_size);
+			memcpy(&priv->dma_buffer[1], &data->block[1], dma_size - 1);
 		} else {
 			/* Block Read */
 			dev_dbg(dev, "I2C_SMBUS_BLOCK_DATA:  READ\n");
@@ -525,7 +525,7 @@ static int ismt_access(struct i2c_adapter *adap, u16 addr,
 			desc->wr_len_cmd = dma_size;
 			desc->control |= ISMT_DESC_I2C;
 			priv->dma_buffer[0] = command;
-			memcpy(&priv->dma_buffer[1], &data->block[1], dma_size);
+			memcpy(&priv->dma_buffer[1], &data->block[1], dma_size - 1);
 		} else {
 			/* i2c Block Read */
 			dev_dbg(dev, "I2C_SMBUS_I2C_BLOCK_DATA:  READ\n");
diff --git a/drivers/i2c/busses/i2c-mv64xxx.c b/drivers/i2c/busses/i2c-mv64xxx.c
index 6dc5ded86f62..2f64273d3f2b 100644
--- a/drivers/i2c/busses/i2c-mv64xxx.c
+++ b/drivers/i2c/busses/i2c-mv64xxx.c
@@ -746,8 +746,7 @@ mv64xxx_of_config(struct mv64xxx_i2c_data *drv_data,
 	}
 	tclk = clk_get_rate(drv_data->clk);
 
-	rc = of_property_read_u32(np, "clock-frequency", &bus_freq);
-	if (rc)
+	if (of_property_read_u32(np, "clock-frequency", &bus_freq))
 		bus_freq = 100000; /* 100kHz by default */
 
 	if (!mv64xxx_find_baud_factors(bus_freq, tclk,
diff --git a/drivers/i2c/busses/i2c-mxs.c b/drivers/i2c/busses/i2c-mxs.c
index 7170fc892829..07e1be6f8992 100644
--- a/drivers/i2c/busses/i2c-mxs.c
+++ b/drivers/i2c/busses/i2c-mxs.c
@@ -307,6 +307,9 @@ static int mxs_i2c_pio_wait_xfer_end(struct mxs_i2c_dev *i2c)
 	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
 
 	while (readl(i2c->regs + MXS_I2C_CTRL0) & MXS_I2C_CTRL0_RUN) {
+		if (readl(i2c->regs + MXS_I2C_CTRL1) &
+				MXS_I2C_CTRL1_NO_SLAVE_ACK_IRQ)
+			return -ENXIO;
 		if (time_after(jiffies, timeout))
 			return -ETIMEDOUT;
 		cond_resched();
@@ -429,7 +432,7 @@ static int mxs_i2c_pio_setup_xfer(struct i2c_adapter *adap,
 		ret = mxs_i2c_pio_wait_xfer_end(i2c);
 		if (ret) {
 			dev_err(i2c->dev,
-				"PIO: Failed to send SELECT command!\n");
+				"PIO: Failed to send READ command!\n");
 			goto cleanup;
 		}
 
diff --git a/drivers/i2c/busses/i2c-qup.c b/drivers/i2c/busses/i2c-qup.c
index 3a4d64e1dfb1..092d89bd3224 100644
--- a/drivers/i2c/busses/i2c-qup.c
+++ b/drivers/i2c/busses/i2c-qup.c
@@ -674,16 +674,20 @@ static int qup_i2c_probe(struct platform_device *pdev)
 	qup->adap.dev.of_node = pdev->dev.of_node;
 	strlcpy(qup->adap.name, "QUP I2C adapter", sizeof(qup->adap.name));
 
-	ret = i2c_add_adapter(&qup->adap);
-	if (ret)
-		goto fail;
-
 	pm_runtime_set_autosuspend_delay(qup->dev, MSEC_PER_SEC);
 	pm_runtime_use_autosuspend(qup->dev);
 	pm_runtime_set_active(qup->dev);
 	pm_runtime_enable(qup->dev);
+
+	ret = i2c_add_adapter(&qup->adap);
+	if (ret)
+		goto fail_runtime;
+
 	return 0;
 
+fail_runtime:
+	pm_runtime_disable(qup->dev);
+	pm_runtime_set_suspended(qup->dev);
 fail:
 	qup_i2c_disable_clocks(qup);
 	return ret;
diff --git a/drivers/i2c/busses/i2c-rcar.c b/drivers/i2c/busses/i2c-rcar.c
index f3c7139dfa25..d826e82dd997 100644
--- a/drivers/i2c/busses/i2c-rcar.c
+++ b/drivers/i2c/busses/i2c-rcar.c
@@ -34,6 +34,7 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
+#include <linux/spinlock.h>
 
 /* register offsets */
 #define ICSCR	0x00	/* slave ctrl */
@@ -75,8 +76,8 @@
 #define RCAR_IRQ_RECV	(MNR | MAL | MST | MAT | MDR)
 #define RCAR_IRQ_STOP	(MST)
 
-#define RCAR_IRQ_ACK_SEND	(~(MAT | MDE))
-#define RCAR_IRQ_ACK_RECV	(~(MAT | MDR))
+#define RCAR_IRQ_ACK_SEND	(~(MAT | MDE) & 0xFF)
+#define RCAR_IRQ_ACK_RECV	(~(MAT | MDR) & 0xFF)
 
 #define ID_LAST_MSG	(1 << 0)
 #define ID_IOERROR	(1 << 1)
@@ -95,6 +96,7 @@ struct rcar_i2c_priv {
 	struct i2c_msg	*msg;
 	struct clk *clk;
 
+	spinlock_t lock;
 	wait_queue_head_t wait;
 
 	int pos;
@@ -193,7 +195,7 @@ static int rcar_i2c_clock_calculate(struct rcar_i2c_priv *priv,
 	 */
 	rate = clk_get_rate(priv->clk);
 	cdf = rate / 20000000;
-	if (cdf >= 1 << cdf_width) {
+	if (cdf >= 1U << cdf_width) {
 		dev_err(dev, "Input clock %lu too high\n", rate);
 		return -EIO;
 	}
@@ -243,7 +245,7 @@ scgd_find:
 	return 0;
 }
 
-static int rcar_i2c_prepare_msg(struct rcar_i2c_priv *priv)
+static void rcar_i2c_prepare_msg(struct rcar_i2c_priv *priv)
 {
 	int read = !!rcar_i2c_is_recv(priv);
 
@@ -251,8 +253,6 @@ static int rcar_i2c_prepare_msg(struct rcar_i2c_priv *priv)
 	rcar_i2c_write(priv, ICMSR, 0);
 	rcar_i2c_write(priv, ICMCR, RCAR_BUS_PHASE_START);
 	rcar_i2c_write(priv, ICMIER, read ? RCAR_IRQ_RECV : RCAR_IRQ_SEND);
-
-	return 0;
 }
 
 /*
@@ -363,22 +363,27 @@ static int rcar_i2c_irq_recv(struct rcar_i2c_priv *priv, u32 msr)
 static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
 {
 	struct rcar_i2c_priv *priv = ptr;
+	irqreturn_t result = IRQ_HANDLED;
 	u32 msr;
 
+	/*-------------- spin lock -----------------*/
+	spin_lock(&priv->lock);
+
 	msr = rcar_i2c_read(priv, ICMSR);
 
+	/* Only handle interrupts that are currently enabled */
+	msr &= rcar_i2c_read(priv, ICMIER);
+	if (!msr) {
+		result = IRQ_NONE;
+		goto exit;
+	}
+
 	/* Arbitration lost */
 	if (msr & MAL) {
 		rcar_i2c_flags_set(priv, (ID_DONE | ID_ARBLOST));
 		goto out;
 	}
 
-	/* Stop */
-	if (msr & MST) {
-		rcar_i2c_flags_set(priv, ID_DONE);
-		goto out;
-	}
-
 	/* Nack */
 	if (msr & MNR) {
 		/* go to stop phase */
@@ -388,6 +393,12 @@ static irqreturn_t rcar_i2c_irq(int irq, void *ptr)
 		goto out;
 	}
 
+	/* Stop */
+	if (msr & MST) {
+		rcar_i2c_flags_set(priv, ID_DONE);
+		goto out;
+	}
+
 	if (rcar_i2c_is_recv(priv))
 		rcar_i2c_flags_set(priv, rcar_i2c_irq_recv(priv, msr));
 	else
@@ -400,7 +411,11 @@ out:
 		wake_up(&priv->wait);
 	}
 
-	return IRQ_HANDLED;
+exit:
+	spin_unlock(&priv->lock);
+	/*-------------- spin unlock -----------------*/
+
+	return result;
 }
 
 static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
@@ -409,14 +424,21 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 {
 	struct rcar_i2c_priv *priv = i2c_get_adapdata(adap);
 	struct device *dev = rcar_i2c_priv_to_dev(priv);
+	unsigned long flags;
 	int i, ret, timeout;
 
 	pm_runtime_get_sync(dev);
 
+	/*-------------- spin lock -----------------*/
+	spin_lock_irqsave(&priv->lock, flags);
+
 	rcar_i2c_init(priv);
 	/* start clock */
 	rcar_i2c_write(priv, ICCCR, priv->icccr);
 
+	spin_unlock_irqrestore(&priv->lock, flags);
+	/*-------------- spin unlock -----------------*/
+
 	ret = rcar_i2c_bus_barrier(priv);
 	if (ret < 0)
 		goto out;
@@ -428,17 +450,20 @@ static int rcar_i2c_master_xfer(struct i2c_adapter *adap,
 			break;
 		}
 
+		/*-------------- spin lock -----------------*/
+		spin_lock_irqsave(&priv->lock, flags);
+
 		/* init each data */
 		priv->msg	= &msgs[i];
 		priv->pos	= 0;
 		priv->flags	= 0;
-		if (priv->msg == &msgs[num - 1])
+		if (i == num - 1)
 			rcar_i2c_flags_set(priv, ID_LAST_MSG);
 
-		ret = rcar_i2c_prepare_msg(priv);
+		rcar_i2c_prepare_msg(priv);
 
-		if (ret < 0)
-			break;
+		spin_unlock_irqrestore(&priv->lock, flags);
+		/*-------------- spin unlock -----------------*/
 
 		timeout = wait_event_timeout(priv->wait,
 					     rcar_i2c_flags_has(priv, ID_DONE),
@@ -540,6 +565,7 @@ static int rcar_i2c_probe(struct platform_device *pdev)
 
 	irq = platform_get_irq(pdev, 0);
 	init_waitqueue_head(&priv->wait);
+	spin_lock_init(&priv->lock);
 
 	adap = &priv->adap;
 	adap->nr = pdev->id;
diff --git a/drivers/i2c/busses/i2c-rk3x.c b/drivers/i2c/busses/i2c-rk3x.c
index 69e11853e8bf..f486d0eac4d0 100644
--- a/drivers/i2c/busses/i2c-rk3x.c
+++ b/drivers/i2c/busses/i2c-rk3x.c
@@ -208,7 +208,7 @@ static void rk3x_i2c_prepare_read(struct rk3x_i2c *i2c)
 	 * The hw can read up to 32 bytes at a time. If we need more than one
 	 * chunk, send an ACK after the last byte of the current chunk.
 	 */
-	if (unlikely(len > 32)) {
+	if (len > 32) {
 		len = 32;
 		con &= ~REG_CON_LASTACK;
 	} else {
@@ -238,7 +238,7 @@ static void rk3x_i2c_fill_transmit_buf(struct rk3x_i2c *i2c)
 	for (i = 0; i < 8; ++i) {
 		val = 0;
 		for (j = 0; j < 4; ++j) {
-			if (i2c->processed == i2c->msg->len)
+			if ((i2c->processed == i2c->msg->len) && (cnt != 0))
 				break;
 
 			if (i2c->processed == 0 && cnt == 0)
@@ -323,6 +323,10 @@ static void rk3x_i2c_handle_read(struct rk3x_i2c *i2c, unsigned int ipd)
 	/* ack interrupt */
 	i2c_writel(i2c, REG_INT_MBRF, REG_IPD);
 
+	/* Can only handle a maximum of 32 bytes at a time */
+	if (len > 32)
+		len = 32;
+
 	/* read the data from receive buffer */
 	for (i = 0; i < len; ++i) {
 		if (i % 4 == 0)
@@ -399,7 +403,7 @@ static irqreturn_t rk3x_i2c_irq(int irqno, void *dev_id)
 	}
 
 	/* is there anything left to handle? */
-	if (unlikely((ipd & REG_INT_ALL) == 0))
+	if ((ipd & REG_INT_ALL) == 0)
 		goto out;
 
 	switch (i2c->state) {
@@ -429,12 +433,11 @@ static void rk3x_i2c_set_scl_rate(struct rk3x_i2c *i2c, unsigned long scl_rate)
 	unsigned long i2c_rate = clk_get_rate(i2c->clk);
 	unsigned int div;
 
-	/* SCL rate = (clk rate) / (8 * DIV) */
-	div = DIV_ROUND_UP(i2c_rate, scl_rate * 8);
-
-	/* The lower and upper half of the CLKDIV reg describe the length of
-	 * SCL low & high periods. */
-	div = DIV_ROUND_UP(div, 2);
+	/* set DIV = DIVH = DIVL
+	 * SCL rate = (clk rate) / (8 * (DIVH + 1 + DIVL + 1))
+	 *          = (clk rate) / (16 * (DIV + 1))
+	 */
+	div = DIV_ROUND_UP(i2c_rate, scl_rate * 16) - 1;
 
 	i2c_writel(i2c, (div << 16) | (div & 0xffff), REG_CLKDIV);
 }
diff --git a/drivers/i2c/busses/i2c-s3c2410.c b/drivers/i2c/busses/i2c-s3c2410.c
index e086fb075f2b..e3b0337faeb7 100644
--- a/drivers/i2c/busses/i2c-s3c2410.c
+++ b/drivers/i2c/busses/i2c-s3c2410.c
@@ -244,7 +244,7 @@ static bool is_ack(struct s3c24xx_i2c *i2c)
 		}
 		usleep_range(1000, 2000);
 	}
-	dev_err(i2c->dev, "ack was not recieved\n");
+	dev_err(i2c->dev, "ack was not received\n");
 	return false;
 }
 
diff --git a/drivers/i2c/busses/i2c-tegra.c b/drivers/i2c/busses/i2c-tegra.c
index 87d0371cebb7..efba1ebe16ba 100644
--- a/drivers/i2c/busses/i2c-tegra.c
+++ b/drivers/i2c/busses/i2c-tegra.c
@@ -380,34 +380,33 @@ static inline int tegra_i2c_clock_enable(struct tegra_i2c_dev *i2c_dev)
 {
 	int ret;
 	if (!i2c_dev->hw->has_single_clk_source) {
-		ret = clk_prepare_enable(i2c_dev->fast_clk);
+		ret = clk_enable(i2c_dev->fast_clk);
 		if (ret < 0) {
 			dev_err(i2c_dev->dev,
 				"Enabling fast clk failed, err %d\n", ret);
 			return ret;
 		}
 	}
-	ret = clk_prepare_enable(i2c_dev->div_clk);
+	ret = clk_enable(i2c_dev->div_clk);
 	if (ret < 0) {
 		dev_err(i2c_dev->dev,
 			"Enabling div clk failed, err %d\n", ret);
-		clk_disable_unprepare(i2c_dev->fast_clk);
+		clk_disable(i2c_dev->fast_clk);
 	}
 	return ret;
 }
 
 static inline void tegra_i2c_clock_disable(struct tegra_i2c_dev *i2c_dev)
 {
-	clk_disable_unprepare(i2c_dev->div_clk);
+	clk_disable(i2c_dev->div_clk);
 	if (!i2c_dev->hw->has_single_clk_source)
-		clk_disable_unprepare(i2c_dev->fast_clk);
+		clk_disable(i2c_dev->fast_clk);
 }
 
 static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 {
 	u32 val;
 	int err = 0;
-	int clk_multiplier = I2C_CLK_MULTIPLIER_STD_FAST_MODE;
 	u32 clk_divisor;
 
 	err = tegra_i2c_clock_enable(i2c_dev);
@@ -428,9 +427,6 @@ static int tegra_i2c_init(struct tegra_i2c_dev *i2c_dev)
 	i2c_writel(i2c_dev, val, I2C_CNFG);
 	i2c_writel(i2c_dev, 0, I2C_INT_MASK);
 
-	clk_multiplier *= (i2c_dev->hw->clk_divisor_std_fast_mode + 1);
-	clk_set_rate(i2c_dev->div_clk, i2c_dev->bus_clk_rate * clk_multiplier);
-
 	/* Make sure clock divisor programmed correctly */
 	clk_divisor = i2c_dev->hw->clk_divisor_hs_mode;
 	clk_divisor |= i2c_dev->hw->clk_divisor_std_fast_mode <<
@@ -712,6 +708,7 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 	void __iomem *base;
 	int irq;
 	int ret = 0;
+	int clk_multiplier = I2C_CLK_MULTIPLIER_STD_FAST_MODE;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	base = devm_ioremap_resource(&pdev->dev, res);
@@ -777,17 +774,39 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, i2c_dev);
 
+	if (!i2c_dev->hw->has_single_clk_source) {
+		ret = clk_prepare(i2c_dev->fast_clk);
+		if (ret < 0) {
+			dev_err(i2c_dev->dev, "Clock prepare failed %d\n", ret);
+			return ret;
+		}
+	}
+
+	clk_multiplier *= (i2c_dev->hw->clk_divisor_std_fast_mode + 1);
+	ret = clk_set_rate(i2c_dev->div_clk,
+			   i2c_dev->bus_clk_rate * clk_multiplier);
+	if (ret) {
+		dev_err(i2c_dev->dev, "Clock rate change failed %d\n", ret);
+		goto unprepare_fast_clk;
+	}
+
+	ret = clk_prepare(i2c_dev->div_clk);
+	if (ret < 0) {
+		dev_err(i2c_dev->dev, "Clock prepare failed %d\n", ret);
+		goto unprepare_fast_clk;
+	}
+
 	ret = tegra_i2c_init(i2c_dev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to initialize i2c controller");
-		return ret;
+		goto unprepare_div_clk;
 	}
 
 	ret = devm_request_irq(&pdev->dev, i2c_dev->irq,
 			tegra_i2c_isr, 0, dev_name(&pdev->dev), i2c_dev);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to request irq %i\n", i2c_dev->irq);
-		return ret;
+		goto unprepare_div_clk;
 	}
 
 	i2c_set_adapdata(&i2c_dev->adapter, i2c_dev);
@@ -803,16 +822,30 @@ static int tegra_i2c_probe(struct platform_device *pdev)
 	ret = i2c_add_numbered_adapter(&i2c_dev->adapter);
 	if (ret) {
 		dev_err(&pdev->dev, "Failed to add I2C adapter\n");
-		return ret;
+		goto unprepare_div_clk;
 	}
 
 	return 0;
+
+unprepare_div_clk:
+	clk_unprepare(i2c_dev->div_clk);
+
+unprepare_fast_clk:
+	if (!i2c_dev->hw->has_single_clk_source)
+		clk_unprepare(i2c_dev->fast_clk);
+
+	return ret;
 }
 
 static int tegra_i2c_remove(struct platform_device *pdev)
 {
 	struct tegra_i2c_dev *i2c_dev = platform_get_drvdata(pdev);
 	i2c_del_adapter(&i2c_dev->adapter);
+
+	clk_unprepare(i2c_dev->div_clk);
+	if (!i2c_dev->hw->has_single_clk_source)
+		clk_unprepare(i2c_dev->fast_clk);
+
 	return 0;
 }
 
diff --git a/drivers/i2c/i2c-acpi.c b/drivers/i2c/i2c-acpi.c
deleted file mode 100644
index e8b61967334b..000000000000
--- a/drivers/i2c/i2c-acpi.c
+++ /dev/null
@@ -1,362 +0,0 @@
-/*
- * I2C ACPI code
- *
- * Copyright (C) 2014 Intel Corp
- *
- * Author: Lan Tianyu <tianyu.lan@intel.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
- * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
- * for more details.
- */
-#define pr_fmt(fmt) "I2C/ACPI : " fmt
-
-#include <linux/kernel.h>
-#include <linux/errno.h>
-#include <linux/err.h>
-#include <linux/i2c.h>
-#include <linux/acpi.h>
-
-struct acpi_i2c_handler_data {
-	struct acpi_connection_info info;
-	struct i2c_adapter *adapter;
-};
-
-struct gsb_buffer {
-	u8	status;
-	u8	len;
-	union {
-		u16	wdata;
-		u8	bdata;
-		u8	data[0];
-	};
-} __packed;
-
-static int acpi_i2c_add_resource(struct acpi_resource *ares, void *data)
-{
-	struct i2c_board_info *info = data;
-
-	if (ares->type == ACPI_RESOURCE_TYPE_SERIAL_BUS) {
-		struct acpi_resource_i2c_serialbus *sb;
-
-		sb = &ares->data.i2c_serial_bus;
-		if (sb->type == ACPI_RESOURCE_SERIAL_TYPE_I2C) {
-			info->addr = sb->slave_address;
-			if (sb->access_mode == ACPI_I2C_10BIT_MODE)
-				info->flags |= I2C_CLIENT_TEN;
-		}
-	} else if (info->irq < 0) {
-		struct resource r;
-
-		if (acpi_dev_resource_interrupt(ares, 0, &r))
-			info->irq = r.start;
-	}
-
-	/* Tell the ACPI core to skip this resource */
-	return 1;
-}
-
-static acpi_status acpi_i2c_add_device(acpi_handle handle, u32 level,
-				       void *data, void **return_value)
-{
-	struct i2c_adapter *adapter = data;
-	struct list_head resource_list;
-	struct i2c_board_info info;
-	struct acpi_device *adev;
-	int ret;
-
-	if (acpi_bus_get_device(handle, &adev))
-		return AE_OK;
-	if (acpi_bus_get_status(adev) || !adev->status.present)
-		return AE_OK;
-
-	memset(&info, 0, sizeof(info));
-	info.acpi_node.companion = adev;
-	info.irq = -1;
-
-	INIT_LIST_HEAD(&resource_list);
-	ret = acpi_dev_get_resources(adev, &resource_list,
-				     acpi_i2c_add_resource, &info);
-	acpi_dev_free_resource_list(&resource_list);
-
-	if (ret < 0 || !info.addr)
-		return AE_OK;
-
-	adev->power.flags.ignore_parent = true;
-	strlcpy(info.type, dev_name(&adev->dev), sizeof(info.type));
-	if (!i2c_new_device(adapter, &info)) {
-		adev->power.flags.ignore_parent = false;
-		dev_err(&adapter->dev,
-			"failed to add I2C device %s from ACPI\n",
-			dev_name(&adev->dev));
-	}
-
-	return AE_OK;
-}
-
-/**
- * acpi_i2c_register_devices - enumerate I2C slave devices behind adapter
- * @adap: pointer to adapter
- *
- * Enumerate all I2C slave devices behind this adapter by walking the ACPI
- * namespace. When a device is found it will be added to the Linux device
- * model and bound to the corresponding ACPI handle.
- */
-void acpi_i2c_register_devices(struct i2c_adapter *adap)
-{
-	acpi_handle handle;
-	acpi_status status;
-
-	if (!adap->dev.parent)
-		return;
-
-	handle = ACPI_HANDLE(adap->dev.parent);
-	if (!handle)
-		return;
-
-	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, 1,
-				     acpi_i2c_add_device, NULL,
-				     adap, NULL);
-	if (ACPI_FAILURE(status))
-		dev_warn(&adap->dev, "failed to enumerate I2C slaves\n");
-}
-
-static int acpi_gsb_i2c_read_bytes(struct i2c_client *client,
-		u8 cmd, u8 *data, u8 data_len)
-{
-
-	struct i2c_msg msgs[2];
-	int ret;
-	u8 *buffer;
-
-	buffer = kzalloc(data_len, GFP_KERNEL);
-	if (!buffer)
-		return AE_NO_MEMORY;
-
-	msgs[0].addr = client->addr;
-	msgs[0].flags = client->flags;
-	msgs[0].len = 1;
-	msgs[0].buf = &cmd;
-
-	msgs[1].addr = client->addr;
-	msgs[1].flags = client->flags | I2C_M_RD;
-	msgs[1].len = data_len;
-	msgs[1].buf = buffer;
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret < 0)
-		dev_err(&client->adapter->dev, "i2c read failed\n");
-	else
-		memcpy(data, buffer, data_len);
-
-	kfree(buffer);
-	return ret;
-}
-
-static int acpi_gsb_i2c_write_bytes(struct i2c_client *client,
-		u8 cmd, u8 *data, u8 data_len)
-{
-
-	struct i2c_msg msgs[1];
-	u8 *buffer;
-	int ret = AE_OK;
-
-	buffer = kzalloc(data_len + 1, GFP_KERNEL);
-	if (!buffer)
-		return AE_NO_MEMORY;
-
-	buffer[0] = cmd;
-	memcpy(buffer + 1, data, data_len);
-
-	msgs[0].addr = client->addr;
-	msgs[0].flags = client->flags;
-	msgs[0].len = data_len + 1;
-	msgs[0].buf = buffer;
-
-	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
-	if (ret < 0)
-		dev_err(&client->adapter->dev, "i2c write failed\n");
-
-	kfree(buffer);
-	return ret;
-}
-
-static acpi_status
-acpi_i2c_space_handler(u32 function, acpi_physical_address command,
-			u32 bits, u64 *value64,
-			void *handler_context, void *region_context)
-{
-	struct gsb_buffer *gsb = (struct gsb_buffer *)value64;
-	struct acpi_i2c_handler_data *data = handler_context;
-	struct acpi_connection_info *info = &data->info;
-	struct acpi_resource_i2c_serialbus *sb;
-	struct i2c_adapter *adapter = data->adapter;
-	struct i2c_client client;
-	struct acpi_resource *ares;
-	u32 accessor_type = function >> 16;
-	u8 action = function & ACPI_IO_MASK;
-	acpi_status ret = AE_OK;
-	int status;
-
-	ret = acpi_buffer_to_resource(info->connection, info->length, &ares);
-	if (ACPI_FAILURE(ret))
-		return ret;
-
-	if (!value64 || ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS) {
-		ret = AE_BAD_PARAMETER;
-		goto err;
-	}
-
-	sb = &ares->data.i2c_serial_bus;
-	if (sb->type != ACPI_RESOURCE_SERIAL_TYPE_I2C) {
-		ret = AE_BAD_PARAMETER;
-		goto err;
-	}
-
-	memset(&client, 0, sizeof(client));
-	client.adapter = adapter;
-	client.addr = sb->slave_address;
-	client.flags = 0;
-
-	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
-		client.flags |= I2C_CLIENT_TEN;
-
-	switch (accessor_type) {
-	case ACPI_GSB_ACCESS_ATTRIB_SEND_RCV:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_byte(&client);
-			if (status >= 0) {
-				gsb->bdata = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_byte(&client, gsb->bdata);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_BYTE:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_byte_data(&client, command);
-			if (status >= 0) {
-				gsb->bdata = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_byte_data(&client, command,
-					gsb->bdata);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_WORD:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_word_data(&client, command);
-			if (status >= 0) {
-				gsb->wdata = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_word_data(&client, command,
-					gsb->wdata);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_BLOCK:
-		if (action == ACPI_READ) {
-			status = i2c_smbus_read_block_data(&client, command,
-					gsb->data);
-			if (status >= 0) {
-				gsb->len = status;
-				status = 0;
-			}
-		} else {
-			status = i2c_smbus_write_block_data(&client, command,
-					gsb->len, gsb->data);
-		}
-		break;
-
-	case ACPI_GSB_ACCESS_ATTRIB_MULTIBYTE:
-		if (action == ACPI_READ) {
-			status = acpi_gsb_i2c_read_bytes(&client, command,
-					gsb->data, info->access_length);
-			if (status > 0)
-				status = 0;
-		} else {
-			status = acpi_gsb_i2c_write_bytes(&client, command,
-					gsb->data, info->access_length);
-		}
-		break;
-
-	default:
-		pr_info("protocol(0x%02x) is not supported.\n", accessor_type);
-		ret = AE_BAD_PARAMETER;
-		goto err;
-	}
-
-	gsb->status = status;
-
- err:
-	ACPI_FREE(ares);
-	return ret;
-}
-
-
-int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
-{
-	acpi_handle handle = ACPI_HANDLE(adapter->dev.parent);
-	struct acpi_i2c_handler_data *data;
-	acpi_status status;
-
-	if (!handle)
-		return -ENODEV;
-
-	data = kzalloc(sizeof(struct acpi_i2c_handler_data),
-			    GFP_KERNEL);
-	if (!data)
-		return -ENOMEM;
-
-	data->adapter = adapter;
-	status = acpi_bus_attach_private_data(handle, (void *)data);
-	if (ACPI_FAILURE(status)) {
-		kfree(data);
-		return -ENOMEM;
-	}
-
-	status = acpi_install_address_space_handler(handle,
-				ACPI_ADR_SPACE_GSBUS,
-				&acpi_i2c_space_handler,
-				NULL,
-				data);
-	if (ACPI_FAILURE(status)) {
-		dev_err(&adapter->dev, "Error installing i2c space handler\n");
-		acpi_bus_detach_private_data(handle);
-		kfree(data);
-		return -ENOMEM;
-	}
-
-	return 0;
-}
-
-void acpi_i2c_remove_space_handler(struct i2c_adapter *adapter)
-{
-	acpi_handle handle = ACPI_HANDLE(adapter->dev.parent);
-	struct acpi_i2c_handler_data *data;
-	acpi_status status;
-
-	if (!handle)
-		return;
-
-	acpi_remove_address_space_handler(handle,
-				ACPI_ADR_SPACE_GSBUS,
-				&acpi_i2c_space_handler);
-
-	status = acpi_bus_get_private_data(handle, (void **)&data);
-	if (ACPI_SUCCESS(status))
-		kfree(data);
-
-	acpi_bus_detach_private_data(handle);
-}
diff --git a/drivers/i2c/i2c-core.c b/drivers/i2c/i2c-core.c
index 632057a44615..2f90ac6a7f79 100644
--- a/drivers/i2c/i2c-core.c
+++ b/drivers/i2c/i2c-core.c
@@ -27,6 +27,8 @@
    OF support is copyright (c) 2008 Jochen Friedrich <jochen@scram.de>
    (based on a previous patch from Jon Smirl <jonsmirl@gmail.com>) and
    (c) 2013  Wolfram Sang <wsa@the-dreams.de>
+   I2C ACPI code Copyright (C) 2014 Intel Corp
+   Author: Lan Tianyu <tianyu.lan@intel.com>
  */
 
 #include <linux/module.h>
@@ -48,6 +50,7 @@
 #include <linux/irqflags.h>
 #include <linux/rwsem.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_domain.h>
 #include <linux/acpi.h>
 #include <linux/jump_label.h>
 #include <asm/uaccess.h>
@@ -78,6 +81,368 @@ void i2c_transfer_trace_unreg(void)
 	static_key_slow_dec(&i2c_trace_msg);
 }
 
+#if defined(CONFIG_ACPI)
+struct acpi_i2c_handler_data {
+	struct acpi_connection_info info;
+	struct i2c_adapter *adapter;
+};
+
+struct gsb_buffer {
+	u8	status;
+	u8	len;
+	union {
+		u16	wdata;
+		u8	bdata;
+		u8	data[0];
+	};
+} __packed;
+
+static int acpi_i2c_add_resource(struct acpi_resource *ares, void *data)
+{
+	struct i2c_board_info *info = data;
+
+	if (ares->type == ACPI_RESOURCE_TYPE_SERIAL_BUS) {
+		struct acpi_resource_i2c_serialbus *sb;
+
+		sb = &ares->data.i2c_serial_bus;
+		if (sb->type == ACPI_RESOURCE_SERIAL_TYPE_I2C) {
+			info->addr = sb->slave_address;
+			if (sb->access_mode == ACPI_I2C_10BIT_MODE)
+				info->flags |= I2C_CLIENT_TEN;
+		}
+	} else if (info->irq < 0) {
+		struct resource r;
+
+		if (acpi_dev_resource_interrupt(ares, 0, &r))
+			info->irq = r.start;
+	}
+
+	/* Tell the ACPI core to skip this resource */
+	return 1;
+}
+
+static acpi_status acpi_i2c_add_device(acpi_handle handle, u32 level,
+				       void *data, void **return_value)
+{
+	struct i2c_adapter *adapter = data;
+	struct list_head resource_list;
+	struct i2c_board_info info;
+	struct acpi_device *adev;
+	int ret;
+
+	if (acpi_bus_get_device(handle, &adev))
+		return AE_OK;
+	if (acpi_bus_get_status(adev) || !adev->status.present)
+		return AE_OK;
+
+	memset(&info, 0, sizeof(info));
+	info.acpi_node.companion = adev;
+	info.irq = -1;
+
+	INIT_LIST_HEAD(&resource_list);
+	ret = acpi_dev_get_resources(adev, &resource_list,
+				     acpi_i2c_add_resource, &info);
+	acpi_dev_free_resource_list(&resource_list);
+
+	if (ret < 0 || !info.addr)
+		return AE_OK;
+
+	adev->power.flags.ignore_parent = true;
+	strlcpy(info.type, dev_name(&adev->dev), sizeof(info.type));
+	if (!i2c_new_device(adapter, &info)) {
+		adev->power.flags.ignore_parent = false;
+		dev_err(&adapter->dev,
+			"failed to add I2C device %s from ACPI\n",
+			dev_name(&adev->dev));
+	}
+
+	return AE_OK;
+}
+
+/**
+ * acpi_i2c_register_devices - enumerate I2C slave devices behind adapter
+ * @adap: pointer to adapter
+ *
+ * Enumerate all I2C slave devices behind this adapter by walking the ACPI
+ * namespace. When a device is found it will be added to the Linux device
+ * model and bound to the corresponding ACPI handle.
+ */
+static void acpi_i2c_register_devices(struct i2c_adapter *adap)
+{
+	acpi_handle handle;
+	acpi_status status;
+
+	if (!adap->dev.parent)
+		return;
+
+	handle = ACPI_HANDLE(adap->dev.parent);
+	if (!handle)
+		return;
+
+	status = acpi_walk_namespace(ACPI_TYPE_DEVICE, handle, 1,
+				     acpi_i2c_add_device, NULL,
+				     adap, NULL);
+	if (ACPI_FAILURE(status))
+		dev_warn(&adap->dev, "failed to enumerate I2C slaves\n");
+}
+
+#else /* CONFIG_ACPI */
+static inline void acpi_i2c_register_devices(struct i2c_adapter *adap) { }
+#endif /* CONFIG_ACPI */
+
+#ifdef CONFIG_ACPI_I2C_OPREGION
+static int acpi_gsb_i2c_read_bytes(struct i2c_client *client,
+		u8 cmd, u8 *data, u8 data_len)
+{
+
+	struct i2c_msg msgs[2];
+	int ret;
+	u8 *buffer;
+
+	buffer = kzalloc(data_len, GFP_KERNEL);
+	if (!buffer)
+		return AE_NO_MEMORY;
+
+	msgs[0].addr = client->addr;
+	msgs[0].flags = client->flags;
+	msgs[0].len = 1;
+	msgs[0].buf = &cmd;
+
+	msgs[1].addr = client->addr;
+	msgs[1].flags = client->flags | I2C_M_RD;
+	msgs[1].len = data_len;
+	msgs[1].buf = buffer;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		dev_err(&client->adapter->dev, "i2c read failed\n");
+	else
+		memcpy(data, buffer, data_len);
+
+	kfree(buffer);
+	return ret;
+}
+
+static int acpi_gsb_i2c_write_bytes(struct i2c_client *client,
+		u8 cmd, u8 *data, u8 data_len)
+{
+
+	struct i2c_msg msgs[1];
+	u8 *buffer;
+	int ret = AE_OK;
+
+	buffer = kzalloc(data_len + 1, GFP_KERNEL);
+	if (!buffer)
+		return AE_NO_MEMORY;
+
+	buffer[0] = cmd;
+	memcpy(buffer + 1, data, data_len);
+
+	msgs[0].addr = client->addr;
+	msgs[0].flags = client->flags;
+	msgs[0].len = data_len + 1;
+	msgs[0].buf = buffer;
+
+	ret = i2c_transfer(client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret < 0)
+		dev_err(&client->adapter->dev, "i2c write failed\n");
+
+	kfree(buffer);
+	return ret;
+}
+
+static acpi_status
+acpi_i2c_space_handler(u32 function, acpi_physical_address command,
+			u32 bits, u64 *value64,
+			void *handler_context, void *region_context)
+{
+	struct gsb_buffer *gsb = (struct gsb_buffer *)value64;
+	struct acpi_i2c_handler_data *data = handler_context;
+	struct acpi_connection_info *info = &data->info;
+	struct acpi_resource_i2c_serialbus *sb;
+	struct i2c_adapter *adapter = data->adapter;
+	struct i2c_client client;
+	struct acpi_resource *ares;
+	u32 accessor_type = function >> 16;
+	u8 action = function & ACPI_IO_MASK;
+	acpi_status ret = AE_OK;
+	int status;
+
+	ret = acpi_buffer_to_resource(info->connection, info->length, &ares);
+	if (ACPI_FAILURE(ret))
+		return ret;
+
+	if (!value64 || ares->type != ACPI_RESOURCE_TYPE_SERIAL_BUS) {
+		ret = AE_BAD_PARAMETER;
+		goto err;
+	}
+
+	sb = &ares->data.i2c_serial_bus;
+	if (sb->type != ACPI_RESOURCE_SERIAL_TYPE_I2C) {
+		ret = AE_BAD_PARAMETER;
+		goto err;
+	}
+
+	memset(&client, 0, sizeof(client));
+	client.adapter = adapter;
+	client.addr = sb->slave_address;
+	client.flags = 0;
+
+	if (sb->access_mode == ACPI_I2C_10BIT_MODE)
+		client.flags |= I2C_CLIENT_TEN;
+
+	switch (accessor_type) {
+	case ACPI_GSB_ACCESS_ATTRIB_SEND_RCV:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_byte(&client);
+			if (status >= 0) {
+				gsb->bdata = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_byte(&client, gsb->bdata);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_BYTE:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_byte_data(&client, command);
+			if (status >= 0) {
+				gsb->bdata = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_byte_data(&client, command,
+					gsb->bdata);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_WORD:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_word_data(&client, command);
+			if (status >= 0) {
+				gsb->wdata = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_word_data(&client, command,
+					gsb->wdata);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_BLOCK:
+		if (action == ACPI_READ) {
+			status = i2c_smbus_read_block_data(&client, command,
+					gsb->data);
+			if (status >= 0) {
+				gsb->len = status;
+				status = 0;
+			}
+		} else {
+			status = i2c_smbus_write_block_data(&client, command,
+					gsb->len, gsb->data);
+		}
+		break;
+
+	case ACPI_GSB_ACCESS_ATTRIB_MULTIBYTE:
+		if (action == ACPI_READ) {
+			status = acpi_gsb_i2c_read_bytes(&client, command,
+					gsb->data, info->access_length);
+			if (status > 0)
+				status = 0;
+		} else {
+			status = acpi_gsb_i2c_write_bytes(&client, command,
+					gsb->data, info->access_length);
+		}
+		break;
+
+	default:
+		pr_info("protocol(0x%02x) is not supported.\n", accessor_type);
+		ret = AE_BAD_PARAMETER;
+		goto err;
+	}
+
+	gsb->status = status;
+
+ err:
+	ACPI_FREE(ares);
+	return ret;
+}
+
+
+static int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
+{
+	acpi_handle handle;
+	struct acpi_i2c_handler_data *data;
+	acpi_status status;
+
+	if (!adapter->dev.parent)
+		return -ENODEV;
+
+	handle = ACPI_HANDLE(adapter->dev.parent);
+
+	if (!handle)
+		return -ENODEV;
+
+	data = kzalloc(sizeof(struct acpi_i2c_handler_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->adapter = adapter;
+	status = acpi_bus_attach_private_data(handle, (void *)data);
+	if (ACPI_FAILURE(status)) {
+		kfree(data);
+		return -ENOMEM;
+	}
+
+	status = acpi_install_address_space_handler(handle,
+				ACPI_ADR_SPACE_GSBUS,
+				&acpi_i2c_space_handler,
+				NULL,
+				data);
+	if (ACPI_FAILURE(status)) {
+		dev_err(&adapter->dev, "Error installing i2c space handler\n");
+		acpi_bus_detach_private_data(handle);
+		kfree(data);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void acpi_i2c_remove_space_handler(struct i2c_adapter *adapter)
+{
+	acpi_handle handle;
+	struct acpi_i2c_handler_data *data;
+	acpi_status status;
+
+	if (!adapter->dev.parent)
+		return;
+
+	handle = ACPI_HANDLE(adapter->dev.parent);
+
+	if (!handle)
+		return;
+
+	acpi_remove_address_space_handler(handle,
+				ACPI_ADR_SPACE_GSBUS,
+				&acpi_i2c_space_handler);
+
+	status = acpi_bus_get_private_data(handle, (void **)&data);
+	if (ACPI_SUCCESS(status))
+		kfree(data);
+
+	acpi_bus_detach_private_data(handle);
+}
+#else /* CONFIG_ACPI_I2C_OPREGION */
+static inline void acpi_i2c_remove_space_handler(struct i2c_adapter *adapter)
+{ }
+
+static inline int acpi_i2c_install_space_handler(struct i2c_adapter *adapter)
+{ return 0; }
+#endif /* CONFIG_ACPI_I2C_OPREGION */
+
 /* ------------------------------------------------------------------------- */
 
 static const struct i2c_device_id *i2c_match_id(const struct i2c_device_id *id,
@@ -279,10 +644,13 @@ static int i2c_device_probe(struct device *dev)
 	if (status < 0)
 		return status;
 
-	acpi_dev_pm_attach(&client->dev, true);
-	status = driver->probe(client, i2c_match_id(driver->id_table, client));
-	if (status)
-		acpi_dev_pm_detach(&client->dev, true);
+	status = dev_pm_domain_attach(&client->dev, true);
+	if (status != -EPROBE_DEFER) {
+		status = driver->probe(client, i2c_match_id(driver->id_table,
+					client));
+		if (status)
+			dev_pm_domain_detach(&client->dev, true);
+	}
 
 	return status;
 }
@@ -302,7 +670,7 @@ static int i2c_device_remove(struct device *dev)
 		status = driver->remove(client);
 	}
 
-	acpi_dev_pm_detach(&client->dev, true);
+	dev_pm_domain_detach(&client->dev, true);
 	return status;
 }
 
diff --git a/drivers/ide/atiixp.c b/drivers/ide/atiixp.c
index dbd0f242ec18..76650e92db41 100644
--- a/drivers/ide/atiixp.c
+++ b/drivers/ide/atiixp.c
@@ -19,12 +19,12 @@
 #define ATIIXP_IDE_UDMA_CONTROL		0x54
 #define ATIIXP_IDE_UDMA_MODE		0x56
 
-typedef struct {
+struct atiixp_ide_timing {
 	u8 command_width;
 	u8 recover_width;
-} atiixp_ide_timing;
+};
 
-static atiixp_ide_timing pio_timing[] = {
+static struct atiixp_ide_timing pio_timing[] = {
 	{ 0x05, 0x0d },
 	{ 0x04, 0x07 },
 	{ 0x03, 0x04 },
@@ -32,7 +32,7 @@ static atiixp_ide_timing pio_timing[] = {
 	{ 0x02, 0x00 },
 };
 
-static atiixp_ide_timing mdma_timing[] = {
+static struct atiixp_ide_timing mdma_timing[] = {
 	{ 0x07, 0x07 },
 	{ 0x02, 0x01 },
 	{ 0x02, 0x00 },
diff --git a/drivers/ide/ide-disk.c b/drivers/ide/ide-disk.c
index ee880382e3bc..56b9708894a5 100644
--- a/drivers/ide/ide-disk.c
+++ b/drivers/ide/ide-disk.c
@@ -685,8 +685,10 @@ static void ide_disk_setup(ide_drive_t *drive)
 	printk(KERN_INFO "%s: max request size: %dKiB\n", drive->name,
 	       queue_max_sectors(q) / 2);
 
-	if (ata_id_is_ssd(id))
+	if (ata_id_is_ssd(id)) {
 		queue_flag_set_unlocked(QUEUE_FLAG_NONROT, q);
+		queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, q);
+	}
 
 	/* calculate drive capacity, and select LBA if possible */
 	ide_disk_get_capacity(drive);
diff --git a/drivers/ide/ide-park.c b/drivers/ide/ide-park.c
index f41558a0bcd1..ca958604cda2 100644
--- a/drivers/ide/ide-park.c
+++ b/drivers/ide/ide-park.c
@@ -46,7 +46,7 @@ static void issue_park_cmd(ide_drive_t *drive, unsigned long timeout)
 	 * timeout has expired, so power management will be reenabled.
 	 */
 	rq = blk_get_request(q, READ, GFP_NOWAIT);
-	if (unlikely(!rq))
+	if (IS_ERR(rq))
 		goto out;
 
 	rq->cmd[0] = REQ_UNPARK_HEADS;
diff --git a/drivers/iio/accel/Kconfig b/drivers/iio/accel/Kconfig
index 12addf272a61..9b9be8725e9d 100644
--- a/drivers/iio/accel/Kconfig
+++ b/drivers/iio/accel/Kconfig
@@ -6,17 +6,32 @@
 menu "Accelerometers"
 
 config BMA180
-	tristate "Bosch BMA180 3-Axis Accelerometer Driver"
+	tristate "Bosch BMA180/BMA250 3-Axis Accelerometer Driver"
 	depends on I2C
 	select IIO_BUFFER
 	select IIO_TRIGGERED_BUFFER
 	help
-	  Say Y here if you want to build a driver for the Bosch BMA180
-	  triaxial acceleration sensor.
+	  Say Y here if you want to build a driver for the Bosch BMA180 or
+	  BMA250 triaxial acceleration sensor.
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called bma180.
 
+config BMC150_ACCEL
+	tristate "Bosch BMC150 Accelerometer Driver"
+	depends on I2C
+	select IIO_BUFFER
+	select IIO_TRIGGERED_BUFFER
+	help
+	  Say yes here to build support for the following Bosch accelerometers:
+	  BMC150, BMI055, BMA250E, BMA222E, BMA255, BMA280.
+
+	  Currently this only supports the device via an i2c interface.
+
+	  This is a combo module with both accelerometer and magnetometer.
+	  This driver is only implementing accelerometer part, which has
+	  its own address and register map.
+
 config HID_SENSOR_ACCEL_3D
 	depends on HID_SENSOR_HUB
 	select IIO_BUFFER
@@ -84,7 +99,8 @@ config KXCJK1013
 	select IIO_TRIGGERED_BUFFER
 	help
 	  Say Y here if you want to build a driver for the Kionix KXCJK-1013
-	  triaxial acceleration sensor.
+	  triaxial acceleration sensor. This driver also supports KXCJ9-1008
+	  and KXTJ2-1009.
 
 	  To compile this driver as a module, choose M here: the module will
 	  be called kxcjk-1013.
diff --git a/drivers/iio/accel/Makefile b/drivers/iio/accel/Makefile
index 6578ca1a8e09..a593996c6539 100644
--- a/drivers/iio/accel/Makefile
+++ b/drivers/iio/accel/Makefile
@@ -4,6 +4,7 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_BMA180) += bma180.o
+obj-$(CONFIG_BMC150_ACCEL) += bmc150-accel.o
 obj-$(CONFIG_HID_SENSOR_ACCEL_3D) += hid-sensor-accel-3d.o
 obj-$(CONFIG_KXCJK1013) += kxcjk-1013.o
 obj-$(CONFIG_KXSD9)	+= kxsd9.o
diff --git a/drivers/iio/accel/bma180.c b/drivers/iio/accel/bma180.c
index a077cc86421b..1096da327130 100644
--- a/drivers/iio/accel/bma180.c
+++ b/drivers/iio/accel/bma180.c
@@ -3,9 +3,15 @@
  *
  * Copyright 2013 Oleksandr Kravchenko <x0199363@ti.com>
  *
+ * Support for BMA250 (c) Peter Meerwald <pmeerw@pmeerw.net>
+ *
  * This file is subject to the terms and conditions of version 2 of
  * the GNU General Public License.  See the file COPYING in the main
  * directory of this archive for more details.
+ *
+ * SPI is not supported by driver
+ * BMA180: 7-bit I2C slave address 0x40 or 0x41
+ * BMA250: 7-bit I2C slave address 0x18 or 0x19
  */
 
 #include <linux/module.h>
@@ -26,9 +32,37 @@
 #define BMA180_DRV_NAME "bma180"
 #define BMA180_IRQ_NAME "bma180_event"
 
+enum {
+	BMA180,
+	BMA250,
+};
+
+struct bma180_data;
+
+struct bma180_part_info {
+	const struct iio_chan_spec *channels;
+	unsigned num_channels;
+	const int *scale_table;
+	unsigned num_scales;
+	const int *bw_table;
+	unsigned num_bw;
+
+	u8 int_reset_reg, int_reset_mask;
+	u8 sleep_reg, sleep_mask;
+	u8 bw_reg, bw_mask;
+	u8 scale_reg, scale_mask;
+	u8 power_reg, power_mask, lowpower_val;
+	u8 int_enable_reg, int_enable_mask;
+	u8 softreset_reg;
+
+	int (*chip_config)(struct bma180_data *data);
+	void (*chip_disable)(struct bma180_data *data);
+};
+
 /* Register set */
 #define BMA180_CHIP_ID		0x00 /* Need to distinguish BMA180 from other */
 #define BMA180_ACC_X_LSB	0x02 /* First of 6 registers of accel data */
+#define BMA180_TEMP		0x08
 #define BMA180_CTRL_REG0	0x0d
 #define BMA180_RESET		0x10
 #define BMA180_BW_TCS		0x20
@@ -49,65 +83,81 @@
 #define BMA180_SMP_SKIP		BIT(0)
 
 /* Bit masks for registers bit fields */
-#define BMA180_RANGE		0x0e /* Range of measured accel values*/
+#define BMA180_RANGE		0x0e /* Range of measured accel values */
 #define BMA180_BW		0xf0 /* Accel bandwidth */
 #define BMA180_MODE_CONFIG	0x03 /* Config operation modes */
 
 /* We have to write this value in reset register to do soft reset */
 #define BMA180_RESET_VAL	0xb6
 
-#define BMA_180_ID_REG_VAL	0x03
+#define BMA180_ID_REG_VAL	0x03
 
 /* Chip power modes */
-#define BMA180_LOW_NOISE	0x00
 #define BMA180_LOW_POWER	0x03
 
-#define BMA180_LOW_NOISE_STR	"low_noise"
-#define BMA180_LOW_POWER_STR	"low_power"
-
-/* Defaults values */
-#define BMA180_DEF_PMODE	0
-#define BMA180_DEF_BW		20
-#define BMA180_DEF_SCALE	2452
-
-/* Available values for sysfs */
-#define BMA180_FLP_FREQ_AVAILABLE \
-	"10 20 40 75 150 300"
-#define BMA180_SCALE_AVAILABLE \
-	"0.001275 0.001863 0.002452 0.003727 0.004903 0.009709 0.019417"
+#define BMA250_RANGE_REG	0x0f
+#define BMA250_BW_REG		0x10
+#define BMA250_POWER_REG	0x11
+#define BMA250_RESET_REG	0x14
+#define BMA250_INT_ENABLE_REG	0x17
+#define BMA250_INT_MAP_REG	0x1a
+#define BMA250_INT_RESET_REG	0x21
+
+#define BMA250_RANGE_MASK	GENMASK(3, 0) /* Range of accel values */
+#define BMA250_BW_MASK		GENMASK(4, 0) /* Accel bandwidth */
+#define BMA250_SUSPEND_MASK	BIT(7) /* chip will sleep */
+#define BMA250_LOWPOWER_MASK	BIT(6)
+#define BMA250_DATA_INTEN_MASK	BIT(4)
+#define BMA250_INT1_DATA_MASK	BIT(0)
+#define BMA250_INT_RESET_MASK	BIT(7) /* Reset pending interrupts */
 
 struct bma180_data {
 	struct i2c_client *client;
 	struct iio_trigger *trig;
+	const struct bma180_part_info *part_info;
 	struct mutex mutex;
-	int sleep_state;
+	bool sleep_state;
 	int scale;
 	int bw;
-	int pmode;
-	char *buff;
+	bool pmode;
+	u8 buff[16]; /* 3x 16-bit + 8-bit + padding + timestamp */
 };
 
-enum bma180_axis {
+enum bma180_chan {
 	AXIS_X,
 	AXIS_Y,
 	AXIS_Z,
+	TEMP
 };
 
-static int bw_table[] = { 10, 20, 40, 75, 150, 300 }; /* Hz */
-static int scale_table[] = { 1275, 1863, 2452, 3727, 4903, 9709, 19417 };
+static int bma180_bw_table[] = { 10, 20, 40, 75, 150, 300 }; /* Hz */
+static int bma180_scale_table[] = { 1275, 1863, 2452, 3727, 4903, 9709, 19417 };
+
+static int bma250_bw_table[] = { 8, 16, 31, 63, 125, 250 }; /* Hz */
+static int bma250_scale_table[] = { 0, 0, 0, 38344, 0, 76590, 0, 0, 153180, 0,
+	0, 0, 306458 };
 
-static int bma180_get_acc_reg(struct bma180_data *data, enum bma180_axis axis)
+static int bma180_get_data_reg(struct bma180_data *data, enum bma180_chan chan)
 {
-	u8 reg = BMA180_ACC_X_LSB + axis * 2;
 	int ret;
 
 	if (data->sleep_state)
 		return -EBUSY;
 
-	ret = i2c_smbus_read_word_data(data->client, reg);
-	if (ret < 0)
-		dev_err(&data->client->dev,
-			"failed to read accel_%c registers\n", 'x' + axis);
+	switch (chan) {
+	case TEMP:
+		ret = i2c_smbus_read_byte_data(data->client, BMA180_TEMP);
+		if (ret < 0)
+			dev_err(&data->client->dev, "failed to read temp register\n");
+		break;
+	default:
+		ret = i2c_smbus_read_word_data(data->client,
+			BMA180_ACC_X_LSB + chan * 2);
+		if (ret < 0)
+			dev_err(&data->client->dev,
+				"failed to read accel_%c register\n",
+				'x' + chan);
+	}
 
 	return ret;
 }
@@ -125,7 +175,8 @@ static int bma180_set_bits(struct bma180_data *data, u8 reg, u8 mask, u8 val)
 
 static int bma180_reset_intr(struct bma180_data *data)
 {
-	int ret = bma180_set_bits(data, BMA180_CTRL_REG0, BMA180_RESET_INT, 1);
+	int ret = bma180_set_bits(data, data->part_info->int_reset_reg,
+		data->part_info->int_reset_mask, 1);
 
 	if (ret)
 		dev_err(&data->client->dev, "failed to reset interrupt\n");
@@ -133,12 +184,10 @@ static int bma180_reset_intr(struct bma180_data *data)
 	return ret;
 }
 
-static int bma180_set_new_data_intr_state(struct bma180_data *data, int state)
+static int bma180_set_new_data_intr_state(struct bma180_data *data, bool state)
 {
-	u8 reg_val = state ? BMA180_NEW_DATA_INT : 0x00;
-	int ret = i2c_smbus_write_byte_data(data->client, BMA180_CTRL_REG3,
-			reg_val);
-
+	int ret = bma180_set_bits(data, data->part_info->int_enable_reg,
+			data->part_info->int_enable_mask, state);
 	if (ret)
 		goto err;
 	ret = bma180_reset_intr(data);
@@ -153,9 +202,10 @@ err:
 	return ret;
 }
 
-static int bma180_set_sleep_state(struct bma180_data *data, int state)
+static int bma180_set_sleep_state(struct bma180_data *data, bool state)
 {
-	int ret = bma180_set_bits(data, BMA180_CTRL_REG0, BMA180_SLEEP, state);
+	int ret = bma180_set_bits(data, data->part_info->sleep_reg,
+		data->part_info->sleep_mask, state);
 
 	if (ret) {
 		dev_err(&data->client->dev,
@@ -167,7 +217,7 @@ static int bma180_set_sleep_state(struct bma180_data *data, int state)
 	return 0;
 }
 
-static int bma180_set_ee_writing_state(struct bma180_data *data, int state)
+static int bma180_set_ee_writing_state(struct bma180_data *data, bool state)
 {
 	int ret = bma180_set_bits(data, BMA180_CTRL_REG0, BMA180_EE_W, state);
 
@@ -185,10 +235,10 @@ static int bma180_set_bw(struct bma180_data *data, int val)
 	if (data->sleep_state)
 		return -EBUSY;
 
-	for (i = 0; i < ARRAY_SIZE(bw_table); ++i) {
-		if (bw_table[i] == val) {
-			ret = bma180_set_bits(data,
-					BMA180_BW_TCS, BMA180_BW, i);
+	for (i = 0; i < data->part_info->num_bw; ++i) {
+		if (data->part_info->bw_table[i] == val) {
+			ret = bma180_set_bits(data, data->part_info->bw_reg,
+				data->part_info->bw_mask, i);
 			if (ret) {
 				dev_err(&data->client->dev,
 					"failed to set bandwidth\n");
@@ -209,10 +259,10 @@ static int bma180_set_scale(struct bma180_data *data, int val)
 	if (data->sleep_state)
 		return -EBUSY;
 
-	for (i = 0; i < ARRAY_SIZE(scale_table); ++i)
-		if (scale_table[i] == val) {
-			ret = bma180_set_bits(data,
-					BMA180_OFFSET_LSB1, BMA180_RANGE, i);
+	for (i = 0; i < data->part_info->num_scales; ++i)
+		if (data->part_info->scale_table[i] == val) {
+			ret = bma180_set_bits(data, data->part_info->scale_reg,
+				data->part_info->scale_mask, i);
 			if (ret) {
 				dev_err(&data->client->dev,
 					"failed to set scale\n");
@@ -225,11 +275,11 @@ static int bma180_set_scale(struct bma180_data *data, int val)
 	return -EINVAL;
 }
 
-static int bma180_set_pmode(struct bma180_data *data, int mode)
+static int bma180_set_pmode(struct bma180_data *data, bool mode)
 {
-	u8 reg_val = mode ? BMA180_LOW_POWER : BMA180_LOW_NOISE;
-	int ret = bma180_set_bits(data, BMA180_TCO_Z, BMA180_MODE_CONFIG,
-			reg_val);
+	u8 reg_val = mode ? data->part_info->lowpower_val : 0;
+	int ret = bma180_set_bits(data, data->part_info->power_reg,
+		data->part_info->power_mask, reg_val);
 
 	if (ret) {
 		dev_err(&data->client->dev, "failed to set power mode\n");
@@ -243,7 +293,7 @@ static int bma180_set_pmode(struct bma180_data *data, int mode)
 static int bma180_soft_reset(struct bma180_data *data)
 {
 	int ret = i2c_smbus_write_byte_data(data->client,
-			BMA180_RESET, BMA180_RESET_VAL);
+		data->part_info->softreset_reg, BMA180_RESET_VAL);
 
 	if (ret)
 		dev_err(&data->client->dev, "failed to reset the chip\n");
@@ -257,57 +307,99 @@ static int bma180_chip_init(struct bma180_data *data)
 	int ret = i2c_smbus_read_byte_data(data->client, BMA180_CHIP_ID);
 
 	if (ret < 0)
-		goto err;
-	if (ret != BMA_180_ID_REG_VAL) {
-		ret = -ENODEV;
-		goto err;
-	}
+		return ret;
+	if (ret != BMA180_ID_REG_VAL)
+		return -ENODEV;
 
 	ret = bma180_soft_reset(data);
 	if (ret)
-		goto err;
+		return ret;
 	/*
 	 * No serial transaction should occur within minimum 10 us
 	 * after soft_reset command
 	 */
 	msleep(20);
 
-	ret = bma180_set_bits(data, BMA180_CTRL_REG0, BMA180_DIS_WAKE_UP, 1);
+	ret = bma180_set_new_data_intr_state(data, false);
+	if (ret)
+		return ret;
+
+	return bma180_set_pmode(data, false);
+}
+
+static int bma180_chip_config(struct bma180_data *data)
+{
+	int ret = bma180_chip_init(data);
+
 	if (ret)
 		goto err;
-	ret = bma180_set_ee_writing_state(data, 1);
+	ret = bma180_set_bits(data, BMA180_CTRL_REG0, BMA180_DIS_WAKE_UP, 1);
 	if (ret)
 		goto err;
-	ret = bma180_set_new_data_intr_state(data, 0);
+	ret = bma180_set_ee_writing_state(data, true);
 	if (ret)
 		goto err;
 	ret = bma180_set_bits(data, BMA180_OFFSET_LSB1, BMA180_SMP_SKIP, 1);
 	if (ret)
 		goto err;
-	ret = bma180_set_pmode(data, BMA180_DEF_PMODE);
+	ret = bma180_set_bw(data, 20); /* 20 Hz */
 	if (ret)
 		goto err;
-	ret = bma180_set_bw(data, BMA180_DEF_BW);
+	ret = bma180_set_scale(data, 2452); /* 2 G */
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	dev_err(&data->client->dev, "failed to config the chip\n");
+	return ret;
+}
+
+static int bma250_chip_config(struct bma180_data *data)
+{
+	int ret = bma180_chip_init(data);
+
 	if (ret)
 		goto err;
-	ret = bma180_set_scale(data, BMA180_DEF_SCALE);
+	ret = bma180_set_bw(data, 16); /* 16 Hz */
+	if (ret)
+		goto err;
+	ret = bma180_set_scale(data, 38344); /* 2 G */
+	if (ret)
+		goto err;
+	ret = bma180_set_bits(data, BMA250_INT_MAP_REG,
+		BMA250_INT1_DATA_MASK, 1);
 	if (ret)
 		goto err;
 
 	return 0;
 
 err:
-	dev_err(&data->client->dev, "failed to init the chip\n");
+	dev_err(&data->client->dev, "failed to config the chip\n");
 	return ret;
 }
 
 static void bma180_chip_disable(struct bma180_data *data)
 {
-	if (bma180_set_new_data_intr_state(data, 0))
+	if (bma180_set_new_data_intr_state(data, false))
 		goto err;
-	if (bma180_set_ee_writing_state(data, 0))
+	if (bma180_set_ee_writing_state(data, false))
+		goto err;
+	if (bma180_set_sleep_state(data, true))
+		goto err;
+
+	return;
+
+err:
+	dev_err(&data->client->dev, "failed to disable the chip\n");
+}
+
+static void bma250_chip_disable(struct bma180_data *data)
+{
+	if (bma180_set_new_data_intr_state(data, false))
 		goto err;
-	if (bma180_set_sleep_state(data, 1))
+	if (bma180_set_sleep_state(data, true))
 		goto err;
 
 	return;
@@ -316,13 +408,51 @@ err:
 	dev_err(&data->client->dev, "failed to disable the chip\n");
 }
 
-static IIO_CONST_ATTR(in_accel_filter_low_pass_3db_frequency_available,
-		BMA180_FLP_FREQ_AVAILABLE);
-static IIO_CONST_ATTR(in_accel_scale_available, BMA180_SCALE_AVAILABLE);
+static ssize_t bma180_show_avail(char *buf, const int *vals, unsigned n,
+				 bool micros)
+{
+	size_t len = 0;
+	int i;
+
+	for (i = 0; i < n; i++) {
+		if (!vals[i])
+			continue;
+		len += scnprintf(buf + len, PAGE_SIZE - len,
+			micros ? "0.%06d " : "%d ", vals[i]);
+	}
+	buf[len - 1] = '\n';
+
+	return len;
+}
+
+static ssize_t bma180_show_filter_freq_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct bma180_data *data = iio_priv(dev_to_iio_dev(dev));
+
+	return bma180_show_avail(buf, data->part_info->bw_table,
+		data->part_info->num_bw, false);
+}
+
+static ssize_t bma180_show_scale_avail(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct bma180_data *data = iio_priv(dev_to_iio_dev(dev));
+
+	return bma180_show_avail(buf, data->part_info->scale_table,
+		data->part_info->num_scales, true);
+}
+
+static IIO_DEVICE_ATTR(in_accel_filter_low_pass_3db_frequency_available,
+	S_IRUGO, bma180_show_filter_freq_avail, NULL, 0);
+
+static IIO_DEVICE_ATTR(in_accel_scale_available,
+	S_IRUGO, bma180_show_scale_avail, NULL, 0);
 
 static struct attribute *bma180_attributes[] = {
-	&iio_const_attr_in_accel_filter_low_pass_3db_frequency_available.dev_attr.attr,
-	&iio_const_attr_in_accel_scale_available.dev_attr.attr,
+	&iio_dev_attr_in_accel_filter_low_pass_3db_frequency_available.
+		dev_attr.attr,
+	&iio_dev_attr_in_accel_scale_available.dev_attr.attr,
 	NULL,
 };
 
@@ -340,22 +470,35 @@ static int bma180_read_raw(struct iio_dev *indio_dev,
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&data->mutex);
-		if (iio_buffer_enabled(indio_dev))
-			ret = -EBUSY;
-		else
-			ret = bma180_get_acc_reg(data, chan->scan_index);
+		if (iio_buffer_enabled(indio_dev)) {
+			mutex_unlock(&data->mutex);
+			return -EBUSY;
+		}
+		ret = bma180_get_data_reg(data, chan->scan_index);
 		mutex_unlock(&data->mutex);
 		if (ret < 0)
 			return ret;
-		*val = (s16)ret >> chan->scan_type.shift;
+		*val = sign_extend32(ret >> chan->scan_type.shift,
+			chan->scan_type.realbits - 1);
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_LOW_PASS_FILTER_3DB_FREQUENCY:
 		*val = data->bw;
 		return IIO_VAL_INT;
 	case IIO_CHAN_INFO_SCALE:
-		*val = 0;
-		*val2 = data->scale;
-		return IIO_VAL_INT_PLUS_MICRO;
+		switch (chan->type) {
+		case IIO_ACCEL:
+			*val = 0;
+			*val2 = data->scale;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_TEMP:
+			*val = 500;
+			return IIO_VAL_INT;
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_OFFSET:
+		*val = 48; /* 0 LSB @ 24 degree C */
+		return IIO_VAL_INT;
 	default:
 		return -EINVAL;
 	}
@@ -387,33 +530,14 @@ static int bma180_write_raw(struct iio_dev *indio_dev,
 	}
 }
 
-static int bma180_update_scan_mode(struct iio_dev *indio_dev,
-		const unsigned long *scan_mask)
-{
-	struct bma180_data *data = iio_priv(indio_dev);
-
-	if (data->buff)
-		devm_kfree(&indio_dev->dev, data->buff);
-	data->buff = devm_kzalloc(&indio_dev->dev,
-			indio_dev->scan_bytes, GFP_KERNEL);
-	if (!data->buff)
-		return -ENOMEM;
-
-	return 0;
-}
-
 static const struct iio_info bma180_info = {
 	.attrs			= &bma180_attrs_group,
 	.read_raw		= bma180_read_raw,
 	.write_raw		= bma180_write_raw,
-	.update_scan_mode	= bma180_update_scan_mode,
 	.driver_module		= THIS_MODULE,
 };
 
-static const char * const bma180_power_modes[] = {
-	BMA180_LOW_NOISE_STR,
-	BMA180_LOW_POWER_STR,
-};
+static const char * const bma180_power_modes[] = { "low_noise", "low_power" };
 
 static int bma180_get_power_mode(struct iio_dev *indio_dev,
 		const struct iio_chan_spec *chan)
@@ -449,7 +573,7 @@ static const struct iio_chan_spec_ext_info bma180_ext_info[] = {
 	{ },
 };
 
-#define BMA180_CHANNEL(_axis) {					\
+#define BMA180_ACC_CHANNEL(_axis, _bits) {				\
 	.type = IIO_ACCEL,						\
 	.modified = 1,							\
 	.channel2 = IIO_MOD_##_axis,					\
@@ -459,18 +583,70 @@ static const struct iio_chan_spec_ext_info bma180_ext_info[] = {
 	.scan_index = AXIS_##_axis,					\
 	.scan_type = {							\
 		.sign = 's',						\
-		.realbits = 14,						\
+		.realbits = _bits,					\
 		.storagebits = 16,					\
-		.shift = 2,						\
+		.shift = 16 - _bits,					\
 	},								\
 	.ext_info = bma180_ext_info,					\
 }
 
+#define BMA180_TEMP_CHANNEL {						\
+	.type = IIO_TEMP,						\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |			\
+		BIT(IIO_CHAN_INFO_SCALE) | BIT(IIO_CHAN_INFO_OFFSET),	\
+	.scan_index = TEMP,						\
+	.scan_type = {							\
+		.sign = 's',						\
+		.realbits = 8,						\
+		.storagebits = 16,					\
+	},								\
+}
+
 static const struct iio_chan_spec bma180_channels[] = {
-	BMA180_CHANNEL(X),
-	BMA180_CHANNEL(Y),
-	BMA180_CHANNEL(Z),
-	IIO_CHAN_SOFT_TIMESTAMP(3),
+	BMA180_ACC_CHANNEL(X, 14),
+	BMA180_ACC_CHANNEL(Y, 14),
+	BMA180_ACC_CHANNEL(Z, 14),
+	BMA180_TEMP_CHANNEL,
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+static const struct iio_chan_spec bma250_channels[] = {
+	BMA180_ACC_CHANNEL(X, 10),
+	BMA180_ACC_CHANNEL(Y, 10),
+	BMA180_ACC_CHANNEL(Z, 10),
+	BMA180_TEMP_CHANNEL,
+	IIO_CHAN_SOFT_TIMESTAMP(4),
+};
+
+static const struct bma180_part_info bma180_part_info[] = {
+	[BMA180] = {
+		bma180_channels, ARRAY_SIZE(bma180_channels),
+		bma180_scale_table, ARRAY_SIZE(bma180_scale_table),
+		bma180_bw_table, ARRAY_SIZE(bma180_bw_table),
+		BMA180_CTRL_REG0, BMA180_RESET_INT,
+		BMA180_CTRL_REG0, BMA180_SLEEP,
+		BMA180_BW_TCS, BMA180_BW,
+		BMA180_OFFSET_LSB1, BMA180_RANGE,
+		BMA180_TCO_Z, BMA180_MODE_CONFIG, BMA180_LOW_POWER,
+		BMA180_CTRL_REG3, BMA180_NEW_DATA_INT,
+		BMA180_RESET,
+		bma180_chip_config,
+		bma180_chip_disable,
+	},
+	[BMA250] = {
+		bma250_channels, ARRAY_SIZE(bma250_channels),
+		bma250_scale_table, ARRAY_SIZE(bma250_scale_table),
+		bma250_bw_table, ARRAY_SIZE(bma250_bw_table),
+		BMA250_INT_RESET_REG, BMA250_INT_RESET_MASK,
+		BMA250_POWER_REG, BMA250_SUSPEND_MASK,
+		BMA250_BW_REG, BMA250_BW_MASK,
+		BMA250_RANGE_REG, BMA250_RANGE_MASK,
+		BMA250_POWER_REG, BMA250_LOWPOWER_MASK, 1,
+		BMA250_INT_ENABLE_REG, BMA250_DATA_INTEN_MASK,
+		BMA250_RESET_REG,
+		bma250_chip_config,
+		bma250_chip_disable,
+	},
 };
 
 static irqreturn_t bma180_trigger_handler(int irq, void *p)
@@ -485,13 +661,14 @@ static irqreturn_t bma180_trigger_handler(int irq, void *p)
 
 	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
 			 indio_dev->masklength) {
-		ret = bma180_get_acc_reg(data, bit);
+		ret = bma180_get_data_reg(data, bit);
 		if (ret < 0) {
 			mutex_unlock(&data->mutex);
 			goto err;
 		}
 		((s16 *)data->buff)[i++] = ret;
 	}
+
 	mutex_unlock(&data->mutex);
 
 	iio_push_to_buffers_with_timestamp(indio_dev, data->buff, time_ns);
@@ -529,7 +706,6 @@ static int bma180_probe(struct i2c_client *client,
 {
 	struct bma180_data *data;
 	struct iio_dev *indio_dev;
-	struct iio_trigger *trig;
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
@@ -539,43 +715,45 @@ static int bma180_probe(struct i2c_client *client,
 	data = iio_priv(indio_dev);
 	i2c_set_clientdata(client, indio_dev);
 	data->client = client;
+	data->part_info = &bma180_part_info[id->driver_data];
 
-	ret = bma180_chip_init(data);
+	ret = data->part_info->chip_config(data);
 	if (ret < 0)
 		goto err_chip_disable;
 
 	mutex_init(&data->mutex);
-
 	indio_dev->dev.parent = &client->dev;
-	indio_dev->channels = bma180_channels;
-	indio_dev->num_channels = ARRAY_SIZE(bma180_channels);
-	indio_dev->name = BMA180_DRV_NAME;
+	indio_dev->channels = data->part_info->channels;
+	indio_dev->num_channels = data->part_info->num_channels;
+	indio_dev->name = id->name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &bma180_info;
 
-	trig = iio_trigger_alloc("%s-dev%d", indio_dev->name, indio_dev->id);
-	if (!trig) {
-		ret = -ENOMEM;
-		goto err_chip_disable;
-	}
+	if (client->irq > 0) {
+		data->trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
+			indio_dev->id);
+		if (!data->trig) {
+			ret = -ENOMEM;
+			goto err_chip_disable;
+		}
 
-	ret = devm_request_irq(&client->dev, client->irq,
-			iio_trigger_generic_data_rdy_poll,
-			IRQF_TRIGGER_RISING, BMA180_IRQ_NAME, trig);
-	if (ret) {
-		dev_err(&client->dev, "unable to request IRQ\n");
-		goto err_trigger_free;
-	}
+		ret = devm_request_irq(&client->dev, client->irq,
+			iio_trigger_generic_data_rdy_poll, IRQF_TRIGGER_RISING,
+			"bma180_event", data->trig);
+		if (ret) {
+			dev_err(&client->dev, "unable to request IRQ\n");
+			goto err_trigger_free;
+		}
 
-	trig->dev.parent = &client->dev;
-	trig->ops = &bma180_trigger_ops;
-	iio_trigger_set_drvdata(trig, indio_dev);
-	data->trig = trig;
-	indio_dev->trig = trig;
+		data->trig->dev.parent = &client->dev;
+		data->trig->ops = &bma180_trigger_ops;
+		iio_trigger_set_drvdata(data->trig, indio_dev);
+		indio_dev->trig = iio_trigger_get(data->trig);
 
-	ret = iio_trigger_register(trig);
-	if (ret)
-		goto err_trigger_free;
+		ret = iio_trigger_register(data->trig);
+		if (ret)
+			goto err_trigger_free;
+	}
 
 	ret = iio_triggered_buffer_setup(indio_dev, NULL,
 			bma180_trigger_handler, NULL);
@@ -595,11 +773,12 @@ static int bma180_probe(struct i2c_client *client,
 err_buffer_cleanup:
 	iio_triggered_buffer_cleanup(indio_dev);
 err_trigger_unregister:
-	iio_trigger_unregister(trig);
+	if (data->trig)
+		iio_trigger_unregister(data->trig);
 err_trigger_free:
-	iio_trigger_free(trig);
+	iio_trigger_free(data->trig);
 err_chip_disable:
-	bma180_chip_disable(data);
+	data->part_info->chip_disable(data);
 
 	return ret;
 }
@@ -611,11 +790,13 @@ static int bma180_remove(struct i2c_client *client)
 
 	iio_device_unregister(indio_dev);
 	iio_triggered_buffer_cleanup(indio_dev);
-	iio_trigger_unregister(data->trig);
-	iio_trigger_free(data->trig);
+	if (data->trig) {
+		iio_trigger_unregister(data->trig);
+		iio_trigger_free(data->trig);
+	}
 
 	mutex_lock(&data->mutex);
-	bma180_chip_disable(data);
+	data->part_info->chip_disable(data);
 	mutex_unlock(&data->mutex);
 
 	return 0;
@@ -629,7 +810,7 @@ static int bma180_suspend(struct device *dev)
 	int ret;
 
 	mutex_lock(&data->mutex);
-	ret = bma180_set_sleep_state(data, 1);
+	ret = bma180_set_sleep_state(data, true);
 	mutex_unlock(&data->mutex);
 
 	return ret;
@@ -642,7 +823,7 @@ static int bma180_resume(struct device *dev)
 	int ret;
 
 	mutex_lock(&data->mutex);
-	ret = bma180_set_sleep_state(data, 0);
+	ret = bma180_set_sleep_state(data, false);
 	mutex_unlock(&data->mutex);
 
 	return ret;
@@ -654,27 +835,28 @@ static SIMPLE_DEV_PM_OPS(bma180_pm_ops, bma180_suspend, bma180_resume);
 #define BMA180_PM_OPS NULL
 #endif
 
-static struct i2c_device_id bma180_id[] = {
-	{ BMA180_DRV_NAME, 0 },
+static struct i2c_device_id bma180_ids[] = {
+	{ "bma180", BMA180 },
+	{ "bma250", BMA250 },
 	{ }
 };
 
-MODULE_DEVICE_TABLE(i2c, bma180_id);
+MODULE_DEVICE_TABLE(i2c, bma180_ids);
 
 static struct i2c_driver bma180_driver = {
 	.driver = {
-		.name	= BMA180_DRV_NAME,
+		.name	= "bma180",
 		.owner	= THIS_MODULE,
 		.pm	= BMA180_PM_OPS,
 	},
 	.probe		= bma180_probe,
 	.remove		= bma180_remove,
-	.id_table	= bma180_id,
+	.id_table	= bma180_ids,
 };
 
 module_i2c_driver(bma180_driver);
 
 MODULE_AUTHOR("Kravchenko Oleksandr <x0199363@ti.com>");
 MODULE_AUTHOR("Texas Instruments, Inc.");
-MODULE_DESCRIPTION("Bosch BMA180 triaxial acceleration sensor");
+MODULE_DESCRIPTION("Bosch BMA180/BMA250 triaxial acceleration sensor");
 MODULE_LICENSE("GPL");
diff --git a/drivers/iio/accel/bmc150-accel.c b/drivers/iio/accel/bmc150-accel.c
new file mode 100644
index 000000000000..22c096ce39ad
--- /dev/null
+++ b/drivers/iio/accel/bmc150-accel.c
@@ -0,0 +1,1430 @@
+/*
+ * 3-axis accelerometer driver supporting following Bosch-Sensortec chips:
+ *  - BMC150
+ *  - BMI055
+ *  - BMA255
+ *  - BMA250E
+ *  - BMA222E
+ *  - BMA280
+ *
+ * Copyright (c) 2014, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/gpio/consumer.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/events.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#define BMC150_ACCEL_DRV_NAME			"bmc150_accel"
+#define BMC150_ACCEL_IRQ_NAME			"bmc150_accel_event"
+#define BMC150_ACCEL_GPIO_NAME			"bmc150_accel_int"
+
+#define BMC150_ACCEL_REG_CHIP_ID		0x00
+
+#define BMC150_ACCEL_REG_INT_STATUS_2		0x0B
+#define BMC150_ACCEL_ANY_MOTION_MASK		0x07
+#define BMC150_ACCEL_ANY_MOTION_BIT_SIGN	BIT(3)
+
+#define BMC150_ACCEL_REG_PMU_LPW		0x11
+#define BMC150_ACCEL_PMU_MODE_MASK		0xE0
+#define BMC150_ACCEL_PMU_MODE_SHIFT		5
+#define BMC150_ACCEL_PMU_BIT_SLEEP_DUR_MASK	0x17
+#define BMC150_ACCEL_PMU_BIT_SLEEP_DUR_SHIFT	1
+
+#define BMC150_ACCEL_REG_PMU_RANGE		0x0F
+
+#define BMC150_ACCEL_DEF_RANGE_2G		0x03
+#define BMC150_ACCEL_DEF_RANGE_4G		0x05
+#define BMC150_ACCEL_DEF_RANGE_8G		0x08
+#define BMC150_ACCEL_DEF_RANGE_16G		0x0C
+
+/* Default BW: 125Hz */
+#define BMC150_ACCEL_REG_PMU_BW		0x10
+#define BMC150_ACCEL_DEF_BW			125
+
+#define BMC150_ACCEL_REG_INT_MAP_0		0x19
+#define BMC150_ACCEL_INT_MAP_0_BIT_SLOPE	BIT(2)
+
+#define BMC150_ACCEL_REG_INT_MAP_1		0x1A
+#define BMC150_ACCEL_INT_MAP_1_BIT_DATA	BIT(0)
+
+#define BMC150_ACCEL_REG_INT_RST_LATCH		0x21
+#define BMC150_ACCEL_INT_MODE_LATCH_RESET	0x80
+#define BMC150_ACCEL_INT_MODE_LATCH_INT	0x0F
+#define BMC150_ACCEL_INT_MODE_NON_LATCH_INT	0x00
+
+#define BMC150_ACCEL_REG_INT_EN_0		0x16
+#define BMC150_ACCEL_INT_EN_BIT_SLP_X		BIT(0)
+#define BMC150_ACCEL_INT_EN_BIT_SLP_Y		BIT(1)
+#define BMC150_ACCEL_INT_EN_BIT_SLP_Z		BIT(2)
+
+#define BMC150_ACCEL_REG_INT_EN_1		0x17
+#define BMC150_ACCEL_INT_EN_BIT_DATA_EN	BIT(4)
+
+#define BMC150_ACCEL_REG_INT_OUT_CTRL		0x20
+#define BMC150_ACCEL_INT_OUT_CTRL_INT1_LVL	BIT(0)
+
+#define BMC150_ACCEL_REG_INT_5			0x27
+#define BMC150_ACCEL_SLOPE_DUR_MASK		0x03
+
+#define BMC150_ACCEL_REG_INT_6			0x28
+#define BMC150_ACCEL_SLOPE_THRES_MASK		0xFF
+
+/* Slope duration in terms of number of samples */
+#define BMC150_ACCEL_DEF_SLOPE_DURATION	2
+/* in terms of multiples of g's/LSB, based on range */
+#define BMC150_ACCEL_DEF_SLOPE_THRESHOLD	5
+
+#define BMC150_ACCEL_REG_XOUT_L		0x02
+
+#define BMC150_ACCEL_MAX_STARTUP_TIME_MS	100
+
+/* Sleep Duration values */
+#define BMC150_ACCEL_SLEEP_500_MICRO		0x05
+#define BMC150_ACCEL_SLEEP_1_MS		0x06
+#define BMC150_ACCEL_SLEEP_2_MS		0x07
+#define BMC150_ACCEL_SLEEP_4_MS		0x08
+#define BMC150_ACCEL_SLEEP_6_MS		0x09
+#define BMC150_ACCEL_SLEEP_10_MS		0x0A
+#define BMC150_ACCEL_SLEEP_25_MS		0x0B
+#define BMC150_ACCEL_SLEEP_50_MS		0x0C
+#define BMC150_ACCEL_SLEEP_100_MS		0x0D
+#define BMC150_ACCEL_SLEEP_500_MS		0x0E
+#define BMC150_ACCEL_SLEEP_1_SEC		0x0F
+
+#define BMC150_ACCEL_REG_TEMP			0x08
+#define BMC150_ACCEL_TEMP_CENTER_VAL		24
+
+#define BMC150_ACCEL_AXIS_TO_REG(axis)	(BMC150_ACCEL_REG_XOUT_L + (axis * 2))
+#define BMC150_AUTO_SUSPEND_DELAY_MS		2000
+
+enum bmc150_accel_axis {
+	AXIS_X,
+	AXIS_Y,
+	AXIS_Z,
+};
+
+enum bmc150_power_modes {
+	BMC150_ACCEL_SLEEP_MODE_NORMAL,
+	BMC150_ACCEL_SLEEP_MODE_DEEP_SUSPEND,
+	BMC150_ACCEL_SLEEP_MODE_LPM,
+	BMC150_ACCEL_SLEEP_MODE_SUSPEND = 0x04,
+};
+
+struct bmc150_scale_info {
+	int scale;
+	u8 reg_range;
+};
+
+struct bmc150_accel_chip_info {
+	u8 chip_id;
+	const struct iio_chan_spec *channels;
+	int num_channels;
+	const struct bmc150_scale_info scale_table[4];
+};
+
+struct bmc150_accel_data {
+	struct i2c_client *client;
+	struct iio_trigger *dready_trig;
+	struct iio_trigger *motion_trig;
+	struct mutex mutex;
+	s16 buffer[8];
+	u8 bw_bits;
+	u32 slope_dur;
+	u32 slope_thres;
+	u32 range;
+	int ev_enable_state;
+	bool dready_trigger_on;
+	bool motion_trigger_on;
+	int64_t timestamp;
+	const struct bmc150_accel_chip_info *chip_info;
+};
+
+static const struct {
+	int val;
+	int val2;
+	u8 bw_bits;
+} bmc150_accel_samp_freq_table[] = { {7, 810000, 0x08},
+				     {15, 630000, 0x09},
+				     {31, 250000, 0x0A},
+				     {62, 500000, 0x0B},
+				     {125, 0, 0x0C},
+				     {250, 0, 0x0D},
+				     {500, 0, 0x0E},
+				     {1000, 0, 0x0F} };
+
+static const struct {
+	int bw_bits;
+	int msec;
+} bmc150_accel_sample_upd_time[] = { {0x08, 64},
+				     {0x09, 32},
+				     {0x0A, 16},
+				     {0x0B, 8},
+				     {0x0C, 4},
+				     {0x0D, 2},
+				     {0x0E, 1},
+				     {0x0F, 1} };
+
+static const struct {
+	int sleep_dur;
+	u8 reg_value;
+} bmc150_accel_sleep_value_table[] = { {0, 0},
+				       {500, BMC150_ACCEL_SLEEP_500_MICRO},
+				       {1000, BMC150_ACCEL_SLEEP_1_MS},
+				       {2000, BMC150_ACCEL_SLEEP_2_MS},
+				       {4000, BMC150_ACCEL_SLEEP_4_MS},
+				       {6000, BMC150_ACCEL_SLEEP_6_MS},
+				       {10000, BMC150_ACCEL_SLEEP_10_MS},
+				       {25000, BMC150_ACCEL_SLEEP_25_MS},
+				       {50000, BMC150_ACCEL_SLEEP_50_MS},
+				       {100000, BMC150_ACCEL_SLEEP_100_MS},
+				       {500000, BMC150_ACCEL_SLEEP_500_MS},
+				       {1000000, BMC150_ACCEL_SLEEP_1_SEC} };
+
+
+static int bmc150_accel_set_mode(struct bmc150_accel_data *data,
+				 enum bmc150_power_modes mode,
+				 int dur_us)
+{
+	int i;
+	int ret;
+	u8 lpw_bits;
+	int dur_val = -1;
+
+	if (dur_us > 0) {
+		for (i = 0; i < ARRAY_SIZE(bmc150_accel_sleep_value_table);
+									 ++i) {
+			if (bmc150_accel_sleep_value_table[i].sleep_dur ==
+									dur_us)
+				dur_val =
+				bmc150_accel_sleep_value_table[i].reg_value;
+		}
+	} else
+		dur_val = 0;
+
+	if (dur_val < 0)
+		return -EINVAL;
+
+	lpw_bits = mode << BMC150_ACCEL_PMU_MODE_SHIFT;
+	lpw_bits |= (dur_val << BMC150_ACCEL_PMU_BIT_SLEEP_DUR_SHIFT);
+
+	dev_dbg(&data->client->dev, "Set Mode bits %x\n", lpw_bits);
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_PMU_LPW, lpw_bits);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_pmu_lpw\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmc150_accel_set_bw(struct bmc150_accel_data *data, int val,
+			       int val2)
+{
+	int i;
+	int ret;
+
+	for (i = 0; i < ARRAY_SIZE(bmc150_accel_samp_freq_table); ++i) {
+		if (bmc150_accel_samp_freq_table[i].val == val &&
+				bmc150_accel_samp_freq_table[i].val2 == val2) {
+			ret = i2c_smbus_write_byte_data(
+				data->client,
+				BMC150_ACCEL_REG_PMU_BW,
+				bmc150_accel_samp_freq_table[i].bw_bits);
+			if (ret < 0)
+				return ret;
+
+			data->bw_bits =
+				bmc150_accel_samp_freq_table[i].bw_bits;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int bmc150_accel_chip_init(struct bmc150_accel_data *data)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, BMC150_ACCEL_REG_CHIP_ID);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Error: Reading chip id\n");
+		return ret;
+	}
+
+	dev_dbg(&data->client->dev, "Chip Id %x\n", ret);
+	if (ret != data->chip_info->chip_id) {
+		dev_err(&data->client->dev, "Invalid chip %x\n", ret);
+		return -ENODEV;
+	}
+
+	ret = bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_NORMAL, 0);
+	if (ret < 0)
+		return ret;
+
+	/* Set Bandwidth */
+	ret = bmc150_accel_set_bw(data, BMC150_ACCEL_DEF_BW, 0);
+	if (ret < 0)
+		return ret;
+
+	/* Set Default Range */
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_PMU_RANGE,
+					BMC150_ACCEL_DEF_RANGE_4G);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+					"Error writing reg_pmu_range\n");
+		return ret;
+	}
+
+	data->range = BMC150_ACCEL_DEF_RANGE_4G;
+
+	/* Set default slope duration */
+	ret = i2c_smbus_read_byte_data(data->client, BMC150_ACCEL_REG_INT_5);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_5\n");
+		return ret;
+	}
+	data->slope_dur |= BMC150_ACCEL_DEF_SLOPE_DURATION;
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_5,
+					data->slope_dur);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_5\n");
+		return ret;
+	}
+	dev_dbg(&data->client->dev, "slope_dur %x\n", data->slope_dur);
+
+	/* Set default slope thresholds */
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_6,
+					BMC150_ACCEL_DEF_SLOPE_THRESHOLD);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_6\n");
+		return ret;
+	}
+	data->slope_thres = BMC150_ACCEL_DEF_SLOPE_THRESHOLD;
+	dev_dbg(&data->client->dev, "slope_thres %x\n", data->slope_thres);
+
+	/* Set default as latched interrupts */
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_RST_LATCH,
+					BMC150_ACCEL_INT_MODE_LATCH_INT |
+					BMC150_ACCEL_INT_MODE_LATCH_RESET);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Error writing reg_int_rst_latch\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmc150_accel_setup_any_motion_interrupt(
+					struct bmc150_accel_data *data,
+					bool status)
+{
+	int ret;
+
+	/* Enable/Disable INT1 mapping */
+	ret = i2c_smbus_read_byte_data(data->client,
+				       BMC150_ACCEL_REG_INT_MAP_0);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_map_0\n");
+		return ret;
+	}
+	if (status)
+		ret |= BMC150_ACCEL_INT_MAP_0_BIT_SLOPE;
+	else
+		ret &= ~BMC150_ACCEL_INT_MAP_0_BIT_SLOPE;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_MAP_0,
+					ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_map_0\n");
+		return ret;
+	}
+
+	if (status) {
+		/* Set slope duration (no of samples) */
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMC150_ACCEL_REG_INT_5,
+						data->slope_dur);
+		if (ret < 0) {
+			dev_err(&data->client->dev, "Error write reg_int_5\n");
+			return ret;
+		}
+
+		/* Set slope thresholds */
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMC150_ACCEL_REG_INT_6,
+						data->slope_thres);
+		if (ret < 0) {
+			dev_err(&data->client->dev, "Error write reg_int_6\n");
+			return ret;
+		}
+
+		/*
+		 * New data interrupt is always non-latched,
+		 * which will have higher priority, so no need
+		 * to set latched mode, we will be flooded anyway with INTR
+		 */
+		if (!data->dready_trigger_on) {
+			ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_RST_LATCH,
+					BMC150_ACCEL_INT_MODE_LATCH_INT |
+					BMC150_ACCEL_INT_MODE_LATCH_RESET);
+			if (ret < 0) {
+				dev_err(&data->client->dev,
+					"Error writing reg_int_rst_latch\n");
+				return ret;
+			}
+		}
+
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMC150_ACCEL_REG_INT_EN_0,
+						BMC150_ACCEL_INT_EN_BIT_SLP_X |
+						BMC150_ACCEL_INT_EN_BIT_SLP_Y |
+						BMC150_ACCEL_INT_EN_BIT_SLP_Z);
+	} else
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMC150_ACCEL_REG_INT_EN_0,
+						0);
+
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_en_0\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmc150_accel_setup_new_data_interrupt(struct bmc150_accel_data *data,
+					   bool status)
+{
+	int ret;
+
+	/* Enable/Disable INT1 mapping */
+	ret = i2c_smbus_read_byte_data(data->client,
+				       BMC150_ACCEL_REG_INT_MAP_1);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_map_1\n");
+		return ret;
+	}
+	if (status)
+		ret |= BMC150_ACCEL_INT_MAP_1_BIT_DATA;
+	else
+		ret &= ~BMC150_ACCEL_INT_MAP_1_BIT_DATA;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_MAP_1,
+					ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_map_1\n");
+		return ret;
+	}
+
+	if (status) {
+		/*
+		 * Set non latched mode interrupt and clear any latched
+		 * interrupt
+		 */
+		ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_RST_LATCH,
+					BMC150_ACCEL_INT_MODE_NON_LATCH_INT |
+					BMC150_ACCEL_INT_MODE_LATCH_RESET);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error writing reg_int_rst_latch\n");
+			return ret;
+		}
+
+		ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_EN_1,
+					BMC150_ACCEL_INT_EN_BIT_DATA_EN);
+
+	} else {
+		/* Restore default interrupt mode */
+		ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_RST_LATCH,
+					BMC150_ACCEL_INT_MODE_LATCH_INT |
+					BMC150_ACCEL_INT_MODE_LATCH_RESET);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error writing reg_int_rst_latch\n");
+			return ret;
+		}
+
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMC150_ACCEL_REG_INT_EN_1,
+						0);
+	}
+
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_en_1\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmc150_accel_get_bw(struct bmc150_accel_data *data, int *val,
+			       int *val2)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bmc150_accel_samp_freq_table); ++i) {
+		if (bmc150_accel_samp_freq_table[i].bw_bits == data->bw_bits) {
+			*val = bmc150_accel_samp_freq_table[i].val;
+			*val2 = bmc150_accel_samp_freq_table[i].val2;
+			return IIO_VAL_INT_PLUS_MICRO;
+		}
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int bmc150_accel_get_startup_times(struct bmc150_accel_data *data)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bmc150_accel_sample_upd_time); ++i) {
+		if (bmc150_accel_sample_upd_time[i].bw_bits == data->bw_bits)
+			return bmc150_accel_sample_upd_time[i].msec;
+	}
+
+	return BMC150_ACCEL_MAX_STARTUP_TIME_MS;
+}
+
+static int bmc150_accel_set_power_state(struct bmc150_accel_data *data, bool on)
+{
+	int ret;
+
+	if (on)
+		ret = pm_runtime_get_sync(&data->client->dev);
+	else {
+		pm_runtime_mark_last_busy(&data->client->dev);
+		ret = pm_runtime_put_autosuspend(&data->client->dev);
+	}
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Failed: bmc150_accel_set_power_state for %d\n", on);
+		return ret;
+	}
+
+	return 0;
+}
+#else
+static int bmc150_accel_set_power_state(struct bmc150_accel_data *data, bool on)
+{
+	return 0;
+}
+#endif
+
+static int bmc150_accel_set_scale(struct bmc150_accel_data *data, int val)
+{
+	int ret, i;
+
+	for (i = 0; i < ARRAY_SIZE(data->chip_info->scale_table); ++i) {
+		if (data->chip_info->scale_table[i].scale == val) {
+			ret = i2c_smbus_write_byte_data(
+				     data->client,
+				     BMC150_ACCEL_REG_PMU_RANGE,
+				     data->chip_info->scale_table[i].reg_range);
+			if (ret < 0) {
+				dev_err(&data->client->dev,
+					"Error writing pmu_range\n");
+				return ret;
+			}
+
+			data->range = data->chip_info->scale_table[i].reg_range;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int bmc150_accel_get_temp(struct bmc150_accel_data *data, int *val)
+{
+	int ret;
+
+	mutex_lock(&data->mutex);
+
+	ret = i2c_smbus_read_byte_data(data->client, BMC150_ACCEL_REG_TEMP);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_temp\n");
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	*val = sign_extend32(ret, 7);
+
+	mutex_unlock(&data->mutex);
+
+	return IIO_VAL_INT;
+}
+
+static int bmc150_accel_get_axis(struct bmc150_accel_data *data,
+				 struct iio_chan_spec const *chan,
+				 int *val)
+{
+	int ret;
+	int axis = chan->scan_index;
+
+	mutex_lock(&data->mutex);
+	ret = bmc150_accel_set_power_state(data, true);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	ret = i2c_smbus_read_word_data(data->client,
+				       BMC150_ACCEL_AXIS_TO_REG(axis));
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading axis %d\n", axis);
+		bmc150_accel_set_power_state(data, false);
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	*val = sign_extend32(ret >> chan->scan_type.shift,
+			     chan->scan_type.realbits - 1);
+	ret = bmc150_accel_set_power_state(data, false);
+	mutex_unlock(&data->mutex);
+	if (ret < 0)
+		return ret;
+
+	return IIO_VAL_INT;
+}
+
+static int bmc150_accel_read_raw(struct iio_dev *indio_dev,
+				 struct iio_chan_spec const *chan,
+				 int *val, int *val2, long mask)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		switch (chan->type) {
+		case IIO_TEMP:
+			return bmc150_accel_get_temp(data, val);
+		case IIO_ACCEL:
+			if (iio_buffer_enabled(indio_dev))
+				return -EBUSY;
+			else
+				return bmc150_accel_get_axis(data, chan, val);
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_OFFSET:
+		if (chan->type == IIO_TEMP) {
+			*val = BMC150_ACCEL_TEMP_CENTER_VAL;
+			return IIO_VAL_INT;
+		} else
+			return -EINVAL;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		switch (chan->type) {
+		case IIO_TEMP:
+			*val2 = 500000;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_ACCEL:
+		{
+			int i;
+			const struct bmc150_scale_info *si;
+			int st_size = ARRAY_SIZE(data->chip_info->scale_table);
+
+			for (i = 0; i < st_size; ++i) {
+				si = &data->chip_info->scale_table[i];
+				if (si->reg_range == data->range) {
+					*val2 = si->scale;
+					return IIO_VAL_INT_PLUS_MICRO;
+				}
+			}
+			return -EINVAL;
+		}
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		mutex_lock(&data->mutex);
+		ret = bmc150_accel_get_bw(data, val, val2);
+		mutex_unlock(&data->mutex);
+		return ret;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bmc150_accel_write_raw(struct iio_dev *indio_dev,
+				  struct iio_chan_spec const *chan,
+				  int val, int val2, long mask)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		mutex_lock(&data->mutex);
+		ret = bmc150_accel_set_bw(data, val, val2);
+		mutex_unlock(&data->mutex);
+		break;
+	case IIO_CHAN_INFO_SCALE:
+		if (val)
+			return -EINVAL;
+
+		mutex_lock(&data->mutex);
+		ret = bmc150_accel_set_scale(data, val2);
+		mutex_unlock(&data->mutex);
+		return ret;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int bmc150_accel_read_event(struct iio_dev *indio_dev,
+				   const struct iio_chan_spec *chan,
+				   enum iio_event_type type,
+				   enum iio_event_direction dir,
+				   enum iio_event_info info,
+				   int *val, int *val2)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	*val2 = 0;
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		*val = data->slope_thres;
+		break;
+	case IIO_EV_INFO_PERIOD:
+		*val = data->slope_dur & BMC150_ACCEL_SLOPE_DUR_MASK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static int bmc150_accel_write_event(struct iio_dev *indio_dev,
+				    const struct iio_chan_spec *chan,
+				    enum iio_event_type type,
+				    enum iio_event_direction dir,
+				    enum iio_event_info info,
+				    int val, int val2)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	if (data->ev_enable_state)
+		return -EBUSY;
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		data->slope_thres = val;
+		break;
+	case IIO_EV_INFO_PERIOD:
+		data->slope_dur &= ~BMC150_ACCEL_SLOPE_DUR_MASK;
+		data->slope_dur |= val & BMC150_ACCEL_SLOPE_DUR_MASK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int bmc150_accel_read_event_config(struct iio_dev *indio_dev,
+					  const struct iio_chan_spec *chan,
+					  enum iio_event_type type,
+					  enum iio_event_direction dir)
+{
+
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	return data->ev_enable_state;
+}
+
+static int bmc150_accel_write_event_config(struct iio_dev *indio_dev,
+					   const struct iio_chan_spec *chan,
+					   enum iio_event_type type,
+					   enum iio_event_direction dir,
+					   int state)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (state && data->ev_enable_state)
+		return 0;
+
+	mutex_lock(&data->mutex);
+
+	if (!state && data->motion_trigger_on) {
+		data->ev_enable_state = 0;
+		mutex_unlock(&data->mutex);
+		return 0;
+	}
+
+	/*
+	 * We will expect the enable and disable to do operation in
+	 * in reverse order. This will happen here anyway as our
+	 * resume operation uses sync mode runtime pm calls, the
+	 * suspend operation will be delayed by autosuspend delay
+	 * So the disable operation will still happen in reverse of
+	 * enable operation. When runtime pm is disabled the mode
+	 * is always on so sequence doesn't matter
+	 */
+
+	ret = bmc150_accel_set_power_state(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	ret =  bmc150_accel_setup_any_motion_interrupt(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	data->ev_enable_state = state;
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+static int bmc150_accel_validate_trigger(struct iio_dev *indio_dev,
+				   struct iio_trigger *trig)
+{
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	if (data->dready_trig != trig && data->motion_trig != trig)
+		return -EINVAL;
+
+	return 0;
+}
+
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
+		"7.810000 15.630000 31.250000 62.500000 125 250 500 1000");
+
+static struct attribute *bmc150_accel_attributes[] = {
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group bmc150_accel_attrs_group = {
+	.attrs = bmc150_accel_attributes,
+};
+
+static const struct iio_event_spec bmc150_accel_event = {
+		.type = IIO_EV_TYPE_ROC,
+		.dir = IIO_EV_DIR_RISING | IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+				 BIT(IIO_EV_INFO_ENABLE) |
+				 BIT(IIO_EV_INFO_PERIOD)
+};
+
+#define BMC150_ACCEL_CHANNEL(_axis, bits) {				\
+	.type = IIO_ACCEL,						\
+	.modified = 1,							\
+	.channel2 = IIO_MOD_##_axis,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |		\
+				BIT(IIO_CHAN_INFO_SAMP_FREQ),		\
+	.scan_index = AXIS_##_axis,					\
+	.scan_type = {							\
+		.sign = 's',						\
+		.realbits = (bits),					\
+		.storagebits = 16,					\
+		.shift = 16 - (bits),					\
+	},								\
+	.event_spec = &bmc150_accel_event,				\
+	.num_event_specs = 1						\
+}
+
+#define BMC150_ACCEL_CHANNELS(bits) {					\
+	{								\
+		.type = IIO_TEMP,					\
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |		\
+				      BIT(IIO_CHAN_INFO_SCALE) |	\
+				      BIT(IIO_CHAN_INFO_OFFSET),	\
+		.scan_index = -1,					\
+	},								\
+	BMC150_ACCEL_CHANNEL(X, bits),					\
+	BMC150_ACCEL_CHANNEL(Y, bits),					\
+	BMC150_ACCEL_CHANNEL(Z, bits),					\
+	IIO_CHAN_SOFT_TIMESTAMP(3),					\
+}
+
+static const struct iio_chan_spec bma222e_accel_channels[] =
+	BMC150_ACCEL_CHANNELS(8);
+static const struct iio_chan_spec bma250e_accel_channels[] =
+	BMC150_ACCEL_CHANNELS(10);
+static const struct iio_chan_spec bmc150_accel_channels[] =
+	BMC150_ACCEL_CHANNELS(12);
+static const struct iio_chan_spec bma280_accel_channels[] =
+	BMC150_ACCEL_CHANNELS(14);
+
+enum {
+	bmc150,
+	bmi055,
+	bma255,
+	bma250e,
+	bma222e,
+	bma280,
+};
+
+static const struct bmc150_accel_chip_info bmc150_accel_chip_info_tbl[] = {
+	[bmc150] = {
+		.chip_id = 0xFA,
+		.channels = bmc150_accel_channels,
+		.num_channels = ARRAY_SIZE(bmc150_accel_channels),
+		.scale_table = { {9610, BMC150_ACCEL_DEF_RANGE_2G},
+				 {19122, BMC150_ACCEL_DEF_RANGE_4G},
+				 {38344, BMC150_ACCEL_DEF_RANGE_8G},
+				 {76590, BMC150_ACCEL_DEF_RANGE_16G} },
+	},
+	[bmi055] = {
+		.chip_id = 0xFA,
+		.channels = bmc150_accel_channels,
+		.num_channels = ARRAY_SIZE(bmc150_accel_channels),
+		.scale_table = { {9610, BMC150_ACCEL_DEF_RANGE_2G},
+				 {19122, BMC150_ACCEL_DEF_RANGE_4G},
+				 {38344, BMC150_ACCEL_DEF_RANGE_8G},
+				 {76590, BMC150_ACCEL_DEF_RANGE_16G} },
+	},
+	[bma255] = {
+		.chip_id = 0xFA,
+		.channels = bmc150_accel_channels,
+		.num_channels = ARRAY_SIZE(bmc150_accel_channels),
+		.scale_table = { {9610, BMC150_ACCEL_DEF_RANGE_2G},
+				 {19122, BMC150_ACCEL_DEF_RANGE_4G},
+				 {38344, BMC150_ACCEL_DEF_RANGE_8G},
+				 {76590, BMC150_ACCEL_DEF_RANGE_16G} },
+	},
+	[bma250e] = {
+		.chip_id = 0xF9,
+		.channels = bma250e_accel_channels,
+		.num_channels = ARRAY_SIZE(bma250e_accel_channels),
+		.scale_table = { {38344, BMC150_ACCEL_DEF_RANGE_2G},
+				 {76590, BMC150_ACCEL_DEF_RANGE_4G},
+				 {153277, BMC150_ACCEL_DEF_RANGE_8G},
+				 {306457, BMC150_ACCEL_DEF_RANGE_16G} },
+	},
+	[bma222e] = {
+		.chip_id = 0xF8,
+		.channels = bma222e_accel_channels,
+		.num_channels = ARRAY_SIZE(bma222e_accel_channels),
+		.scale_table = { {153277, BMC150_ACCEL_DEF_RANGE_2G},
+				 {306457, BMC150_ACCEL_DEF_RANGE_4G},
+				 {612915, BMC150_ACCEL_DEF_RANGE_8G},
+				 {1225831, BMC150_ACCEL_DEF_RANGE_16G} },
+	},
+	[bma280] = {
+		.chip_id = 0xFB,
+		.channels = bma280_accel_channels,
+		.num_channels = ARRAY_SIZE(bma280_accel_channels),
+		.scale_table = { {2392, BMC150_ACCEL_DEF_RANGE_2G},
+				 {4785, BMC150_ACCEL_DEF_RANGE_4G},
+				 {9581, BMC150_ACCEL_DEF_RANGE_8G},
+				 {19152, BMC150_ACCEL_DEF_RANGE_16G} },
+	},
+};
+
+static const struct iio_info bmc150_accel_info = {
+	.attrs			= &bmc150_accel_attrs_group,
+	.read_raw		= bmc150_accel_read_raw,
+	.write_raw		= bmc150_accel_write_raw,
+	.read_event_value	= bmc150_accel_read_event,
+	.write_event_value	= bmc150_accel_write_event,
+	.write_event_config	= bmc150_accel_write_event_config,
+	.read_event_config	= bmc150_accel_read_event_config,
+	.validate_trigger	= bmc150_accel_validate_trigger,
+	.driver_module		= THIS_MODULE,
+};
+
+static irqreturn_t bmc150_accel_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int bit, ret, i = 0;
+
+	mutex_lock(&data->mutex);
+	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+			 indio_dev->masklength) {
+		ret = i2c_smbus_read_word_data(data->client,
+					       BMC150_ACCEL_AXIS_TO_REG(bit));
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			goto err_read;
+		}
+		data->buffer[i++] = ret;
+	}
+	mutex_unlock(&data->mutex);
+
+	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
+					   data->timestamp);
+err_read:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int bmc150_accel_trig_try_reen(struct iio_trigger *trig)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+
+	/* new data interrupts don't need ack */
+	if (data->dready_trigger_on)
+		return 0;
+
+	mutex_lock(&data->mutex);
+	/* clear any latched interrupt */
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_RST_LATCH,
+					BMC150_ACCEL_INT_MODE_LATCH_INT |
+					BMC150_ACCEL_INT_MODE_LATCH_RESET);
+	mutex_unlock(&data->mutex);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Error writing reg_int_rst_latch\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmc150_accel_data_rdy_trigger_set_state(struct iio_trigger *trig,
+						   bool state)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+
+	if (!state && data->ev_enable_state && data->motion_trigger_on) {
+		data->motion_trigger_on = false;
+		mutex_unlock(&data->mutex);
+		return 0;
+	}
+
+	/*
+	 * Refer to comment in bmc150_accel_write_event_config for
+	 * enable/disable operation order
+	 */
+	ret = bmc150_accel_set_power_state(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	if (data->motion_trig == trig)
+		ret =  bmc150_accel_setup_any_motion_interrupt(data, state);
+	else
+		ret = bmc150_accel_setup_new_data_interrupt(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	if (data->motion_trig == trig)
+		data->motion_trigger_on = state;
+	else
+		data->dready_trigger_on = state;
+
+	mutex_unlock(&data->mutex);
+
+	return ret;
+}
+
+static const struct iio_trigger_ops bmc150_accel_trigger_ops = {
+	.set_trigger_state = bmc150_accel_data_rdy_trigger_set_state,
+	.try_reenable = bmc150_accel_trig_try_reen,
+	.owner = THIS_MODULE,
+};
+
+static irqreturn_t bmc150_accel_event_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+	int dir;
+
+	ret = i2c_smbus_read_byte_data(data->client,
+				       BMC150_ACCEL_REG_INT_STATUS_2);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_status_2\n");
+		goto ack_intr_status;
+	}
+
+	if (ret & BMC150_ACCEL_ANY_MOTION_BIT_SIGN)
+		dir = IIO_EV_DIR_FALLING;
+	else
+		dir = IIO_EV_DIR_RISING;
+
+	if (ret & BMC150_ACCEL_ANY_MOTION_MASK)
+		iio_push_event(indio_dev, IIO_MOD_EVENT_CODE(IIO_ACCEL,
+							0,
+							IIO_MOD_X_OR_Y_OR_Z,
+							IIO_EV_TYPE_ROC,
+							IIO_EV_DIR_EITHER),
+							data->timestamp);
+ack_intr_status:
+	if (!data->dready_trigger_on)
+		ret = i2c_smbus_write_byte_data(data->client,
+					BMC150_ACCEL_REG_INT_RST_LATCH,
+					BMC150_ACCEL_INT_MODE_LATCH_INT |
+					BMC150_ACCEL_INT_MODE_LATCH_RESET);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t bmc150_accel_data_rdy_trig_poll(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	data->timestamp = iio_get_time_ns();
+
+	if (data->dready_trigger_on)
+		iio_trigger_poll(data->dready_trig);
+	else if (data->motion_trigger_on)
+		iio_trigger_poll(data->motion_trig);
+
+	if (data->ev_enable_state)
+		return IRQ_WAKE_THREAD;
+	else
+		return IRQ_HANDLED;
+}
+
+static const char *bmc150_accel_match_acpi_device(struct device *dev, int *data)
+{
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+
+	if (!id)
+		return NULL;
+
+	*data = (int) id->driver_data;
+
+	return dev_name(dev);
+}
+
+static int bmc150_accel_gpio_probe(struct i2c_client *client,
+					struct bmc150_accel_data *data)
+{
+	struct device *dev;
+	struct gpio_desc *gpio;
+	int ret;
+
+	if (!client)
+		return -EINVAL;
+
+	dev = &client->dev;
+
+	/* data ready gpio interrupt pin */
+	gpio = devm_gpiod_get_index(dev, BMC150_ACCEL_GPIO_NAME, 0);
+	if (IS_ERR(gpio)) {
+		dev_err(dev, "Failed: gpio get index\n");
+		return PTR_ERR(gpio);
+	}
+
+	ret = gpiod_direction_input(gpio);
+	if (ret)
+		return ret;
+
+	ret = gpiod_to_irq(gpio);
+
+	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
+
+	return ret;
+}
+
+static int bmc150_accel_probe(struct i2c_client *client,
+			      const struct i2c_device_id *id)
+{
+	struct bmc150_accel_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+	const char *name = NULL;
+	int chip_id = 0;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	if (id) {
+		name = id->name;
+		chip_id = id->driver_data;
+	}
+
+	if (ACPI_HANDLE(&client->dev))
+		name = bmc150_accel_match_acpi_device(&client->dev, &chip_id);
+
+	data->chip_info = &bmc150_accel_chip_info_tbl[chip_id];
+
+	ret = bmc150_accel_chip_init(data);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&data->mutex);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->channels = data->chip_info->channels;
+	indio_dev->num_channels = data->chip_info->num_channels;
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &bmc150_accel_info;
+
+	if (client->irq < 0)
+		client->irq = bmc150_accel_gpio_probe(client, data);
+
+	if (client->irq >= 0) {
+		ret = devm_request_threaded_irq(
+						&client->dev, client->irq,
+						bmc150_accel_data_rdy_trig_poll,
+						bmc150_accel_event_handler,
+						IRQF_TRIGGER_RISING,
+						BMC150_ACCEL_IRQ_NAME,
+						indio_dev);
+		if (ret)
+			return ret;
+
+		data->dready_trig = devm_iio_trigger_alloc(&client->dev,
+							   "%s-dev%d",
+							   indio_dev->name,
+							   indio_dev->id);
+		if (!data->dready_trig)
+			return -ENOMEM;
+
+		data->motion_trig = devm_iio_trigger_alloc(&client->dev,
+							  "%s-any-motion-dev%d",
+							  indio_dev->name,
+							  indio_dev->id);
+		if (!data->motion_trig)
+			return -ENOMEM;
+
+		data->dready_trig->dev.parent = &client->dev;
+		data->dready_trig->ops = &bmc150_accel_trigger_ops;
+		iio_trigger_set_drvdata(data->dready_trig, indio_dev);
+		ret = iio_trigger_register(data->dready_trig);
+		if (ret)
+			return ret;
+
+		data->motion_trig->dev.parent = &client->dev;
+		data->motion_trig->ops = &bmc150_accel_trigger_ops;
+		iio_trigger_set_drvdata(data->motion_trig, indio_dev);
+		ret = iio_trigger_register(data->motion_trig);
+		if (ret) {
+			data->motion_trig = NULL;
+			goto err_trigger_unregister;
+		}
+
+		ret = iio_triggered_buffer_setup(indio_dev,
+						 &iio_pollfunc_store_time,
+						 bmc150_accel_trigger_handler,
+						 NULL);
+		if (ret < 0) {
+			dev_err(&client->dev,
+				"Failed: iio triggered buffer setup\n");
+			goto err_trigger_unregister;
+		}
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "Unable to register iio device\n");
+		goto err_buffer_cleanup;
+	}
+
+	ret = pm_runtime_set_active(&client->dev);
+	if (ret)
+		goto err_iio_unregister;
+
+	pm_runtime_enable(&client->dev);
+	pm_runtime_set_autosuspend_delay(&client->dev,
+					 BMC150_AUTO_SUSPEND_DELAY_MS);
+	pm_runtime_use_autosuspend(&client->dev);
+
+	return 0;
+
+err_iio_unregister:
+	iio_device_unregister(indio_dev);
+err_buffer_cleanup:
+	if (data->dready_trig)
+		iio_triggered_buffer_cleanup(indio_dev);
+err_trigger_unregister:
+	if (data->dready_trig)
+		iio_trigger_unregister(data->dready_trig);
+	if (data->motion_trig)
+		iio_trigger_unregister(data->motion_trig);
+
+	return ret;
+}
+
+static int bmc150_accel_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (data->dready_trig) {
+		iio_triggered_buffer_cleanup(indio_dev);
+		iio_trigger_unregister(data->dready_trig);
+		iio_trigger_unregister(data->motion_trig);
+	}
+
+	mutex_lock(&data->mutex);
+	bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_DEEP_SUSPEND, 0);
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int bmc150_accel_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	mutex_lock(&data->mutex);
+	bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_SUSPEND, 0);
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+static int bmc150_accel_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	mutex_lock(&data->mutex);
+	if (data->dready_trigger_on || data->motion_trigger_on ||
+							data->ev_enable_state)
+		bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_NORMAL, 0);
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_PM_RUNTIME
+static int bmc150_accel_runtime_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+
+	dev_dbg(&data->client->dev,  __func__);
+
+	return bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_SUSPEND, 0);
+}
+
+static int bmc150_accel_runtime_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmc150_accel_data *data = iio_priv(indio_dev);
+	int ret;
+	int sleep_val;
+
+	dev_dbg(&data->client->dev,  __func__);
+
+	ret = bmc150_accel_set_mode(data, BMC150_ACCEL_SLEEP_MODE_NORMAL, 0);
+	if (ret < 0)
+		return ret;
+
+	sleep_val = bmc150_accel_get_startup_times(data);
+	if (sleep_val < 20)
+		usleep_range(sleep_val * 1000, 20000);
+	else
+		msleep_interruptible(sleep_val);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops bmc150_accel_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(bmc150_accel_suspend, bmc150_accel_resume)
+	SET_RUNTIME_PM_OPS(bmc150_accel_runtime_suspend,
+			   bmc150_accel_runtime_resume, NULL)
+};
+
+static const struct acpi_device_id bmc150_accel_acpi_match[] = {
+	{"BSBA0150",	bmc150},
+	{"BMC150A",	bmc150},
+	{"BMI055A",	bmi055},
+	{"BMA0255",	bma255},
+	{"BMA250E",	bma250e},
+	{"BMA222E",	bma222e},
+	{"BMA0280",	bma280},
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, bmc150_accel_acpi_match);
+
+static const struct i2c_device_id bmc150_accel_id[] = {
+	{"bmc150_accel",	bmc150},
+	{"bmi055_accel",	bmi055},
+	{"bma255",		bma255},
+	{"bma250e",		bma250e},
+	{"bma222e",		bma222e},
+	{"bma280",		bma280},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, bmc150_accel_id);
+
+static struct i2c_driver bmc150_accel_driver = {
+	.driver = {
+		.name	= BMC150_ACCEL_DRV_NAME,
+		.acpi_match_table = ACPI_PTR(bmc150_accel_acpi_match),
+		.pm	= &bmc150_accel_pm_ops,
+	},
+	.probe		= bmc150_accel_probe,
+	.remove		= bmc150_accel_remove,
+	.id_table	= bmc150_accel_id,
+};
+module_i2c_driver(bmc150_accel_driver);
+
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("BMC150 accelerometer driver");
diff --git a/drivers/iio/accel/hid-sensor-accel-3d.c b/drivers/iio/accel/hid-sensor-accel-3d.c
index 54e464e4bb72..d5d95317003a 100644
--- a/drivers/iio/accel/hid-sensor-accel-3d.c
+++ b/drivers/iio/accel/hid-sensor-accel-3d.c
@@ -419,7 +419,6 @@ static struct platform_driver hid_accel_3d_platform_driver = {
 	.id_table = hid_accel_3d_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_accel_3d_probe,
 	.remove		= hid_accel_3d_remove,
diff --git a/drivers/iio/accel/kxcjk-1013.c b/drivers/iio/accel/kxcjk-1013.c
index 7941cf2d31ee..98909a9e284e 100644
--- a/drivers/iio/accel/kxcjk-1013.c
+++ b/drivers/iio/accel/kxcjk-1013.c
@@ -21,10 +21,13 @@
 #include <linux/string.h>
 #include <linux/acpi.h>
 #include <linux/gpio/consumer.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
 #include <linux/iio/buffer.h>
 #include <linux/iio/trigger.h>
+#include <linux/iio/events.h>
 #include <linux/iio/trigger_consumer.h>
 #include <linux/iio/triggered_buffer.h>
 #include <linux/iio/accel/kxcjk_1013.h>
@@ -71,15 +74,40 @@
 #define KXCJK1013_DATA_MASK_12_BIT	0x0FFF
 #define KXCJK1013_MAX_STARTUP_TIME_US	100000
 
+#define KXCJK1013_SLEEP_DELAY_MS	2000
+
+#define KXCJK1013_REG_INT_SRC2_BIT_ZP	BIT(0)
+#define KXCJK1013_REG_INT_SRC2_BIT_ZN	BIT(1)
+#define KXCJK1013_REG_INT_SRC2_BIT_YP	BIT(2)
+#define KXCJK1013_REG_INT_SRC2_BIT_YN	BIT(3)
+#define KXCJK1013_REG_INT_SRC2_BIT_XP	BIT(4)
+#define KXCJK1013_REG_INT_SRC2_BIT_XN	BIT(5)
+
+#define KXCJK1013_DEFAULT_WAKE_THRES	1
+
+enum kx_chipset {
+	KXCJK1013,
+	KXCJ91008,
+	KXTJ21009,
+	KX_MAX_CHIPS /* this must be last */
+};
+
 struct kxcjk1013_data {
 	struct i2c_client *client;
-	struct iio_trigger *trig;
-	bool trig_mode;
+	struct iio_trigger *dready_trig;
+	struct iio_trigger *motion_trig;
 	struct mutex mutex;
 	s16 buffer[8];
-	int power_state;
 	u8 odr_bits;
+	u8 range;
+	int wake_thres;
+	int wake_dur;
 	bool active_high_intr;
+	bool dready_trigger_on;
+	int ev_enable_state;
+	bool motion_trigger_on;
+	int64_t timestamp;
+	enum kx_chipset chipset;
 };
 
 enum kxcjk1013_axis {
@@ -93,6 +121,12 @@ enum kxcjk1013_mode {
 	OPERATION,
 };
 
+enum kxcjk1013_range {
+	KXCJK1013_RANGE_2G,
+	KXCJK1013_RANGE_4G,
+	KXCJK1013_RANGE_8G,
+};
+
 static const struct {
 	int val;
 	int val2;
@@ -107,10 +141,78 @@ static const struct {
 static const struct {
 	int odr_bits;
 	int usec;
-} odr_start_up_times[] = { {0x08, 100000}, {0x09, 100000}, {0x0A, 100000},
-			   {0x0B, 100000}, { 0, 80000}, {0x01, 41000},
-			   {0x02, 21000}, {0x03, 11000}, {0x04, 6400},
-			   {0x05, 3900}, {0x06, 2700}, {0x07, 2100} };
+} odr_start_up_times[KX_MAX_CHIPS][12] = {
+	/* KXCJK-1013 */
+	{
+		{0x08, 100000},
+		{0x09, 100000},
+		{0x0A, 100000},
+		{0x0B, 100000},
+		{0, 80000},
+		{0x01, 41000},
+		{0x02, 21000},
+		{0x03, 11000},
+		{0x04, 6400},
+		{0x05, 3900},
+		{0x06, 2700},
+		{0x07, 2100},
+	},
+	/* KXCJ9-1008 */
+	{
+		{0x08, 100000},
+		{0x09, 100000},
+		{0x0A, 100000},
+		{0x0B, 100000},
+		{0, 80000},
+		{0x01, 41000},
+		{0x02, 21000},
+		{0x03, 11000},
+		{0x04, 6400},
+		{0x05, 3900},
+		{0x06, 2700},
+		{0x07, 2100},
+	},
+	/* KXCTJ2-1009 */
+	{
+		{0x08, 1240000},
+		{0x09, 621000},
+		{0x0A, 309000},
+		{0x0B, 151000},
+		{0, 80000},
+		{0x01, 41000},
+		{0x02, 21000},
+		{0x03, 11000},
+		{0x04, 6000},
+		{0x05, 4000},
+		{0x06, 3000},
+		{0x07, 2000},
+	},
+};
+
+static const struct {
+	u16 scale;
+	u8 gsel_0;
+	u8 gsel_1;
+} KXCJK1013_scale_table[] = { {9582, 0, 0},
+			      {19163, 1, 0},
+			      {38326, 0, 1} };
+
+static const struct {
+	int val;
+	int val2;
+	int odr_bits;
+} wake_odr_data_rate_table[] = { {0, 781000, 0x00},
+				 {1, 563000, 0x01},
+				 {3, 125000, 0x02},
+				 {6, 250000, 0x03},
+				 {12, 500000, 0x04},
+				 {25, 0, 0x05},
+				 {50, 0, 0x06},
+				 {100, 0, 0x06},
+				 {200, 0, 0x06},
+				 {400, 0, 0x06},
+				 {800, 0, 0x06},
+				 {1600, 0, 0x06} };
 
 static int kxcjk1013_set_mode(struct kxcjk1013_data *data,
 			      enum kxcjk1013_mode mode)
@@ -138,6 +240,51 @@ static int kxcjk1013_set_mode(struct kxcjk1013_data *data,
 	return 0;
 }
 
+static int kxcjk1013_get_mode(struct kxcjk1013_data *data,
+			      enum kxcjk1013_mode *mode)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, KXCJK1013_REG_CTRL1);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
+		return ret;
+	}
+
+	if (ret & KXCJK1013_REG_CTRL1_BIT_PC1)
+		*mode = OPERATION;
+	else
+		*mode = STANDBY;
+
+	return 0;
+}
+
+static int kxcjk1013_set_range(struct kxcjk1013_data *data, int range_index)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, KXCJK1013_REG_CTRL1);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
+		return ret;
+	}
+
+	ret |= (KXCJK1013_scale_table[range_index].gsel_0 << 3);
+	ret |= (KXCJK1013_scale_table[range_index].gsel_1 << 4);
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					KXCJK1013_REG_CTRL1,
+					ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_ctrl1\n");
+		return ret;
+	}
+
+	data->range = range_index;
+
+	return 0;
+}
+
 static int kxcjk1013_chip_init(struct kxcjk1013_data *data)
 {
 	int ret;
@@ -160,10 +307,6 @@ static int kxcjk1013_chip_init(struct kxcjk1013_data *data)
 		return ret;
 	}
 
-	/* Setting range to 4G */
-	ret |= KXCJK1013_REG_CTRL1_BIT_GSEL0;
-	ret &= ~KXCJK1013_REG_CTRL1_BIT_GSEL1;
-
 	/* Set 12 bit mode */
 	ret |= KXCJK1013_REG_CTRL1_BIT_RES;
 
@@ -174,6 +317,11 @@ static int kxcjk1013_chip_init(struct kxcjk1013_data *data)
 		return ret;
 	}
 
+	/* Setting range to 4G */
+	ret = kxcjk1013_set_range(data, KXCJK1013_RANGE_4G);
+	if (ret < 0)
+		return ret;
+
 	ret = i2c_smbus_read_byte_data(data->client, KXCJK1013_REG_DATA_CTRL);
 	if (ret < 0) {
 		dev_err(&data->client->dev, "Error reading reg_data_ctrl\n");
@@ -201,14 +349,147 @@ static int kxcjk1013_chip_init(struct kxcjk1013_data *data)
 		return ret;
 	}
 
+	ret = kxcjk1013_set_mode(data, OPERATION);
+	if (ret < 0)
+		return ret;
+
+	data->wake_thres = KXCJK1013_DEFAULT_WAKE_THRES;
+
 	return 0;
 }
 
-static int kxcjk1013_chip_setup_interrupt(struct kxcjk1013_data *data,
-					  bool status)
+#ifdef CONFIG_PM_RUNTIME
+static int kxcjk1013_get_startup_times(struct kxcjk1013_data *data)
+{
+	int i;
+	int idx = data->chipset;
+
+	for (i = 0; i < ARRAY_SIZE(odr_start_up_times[idx]); ++i) {
+		if (odr_start_up_times[idx][i].odr_bits == data->odr_bits)
+			return odr_start_up_times[idx][i].usec;
+	}
+
+	return KXCJK1013_MAX_STARTUP_TIME_US;
+}
+#endif
+
+static int kxcjk1013_set_power_state(struct kxcjk1013_data *data, bool on)
 {
 	int ret;
 
+	if (on)
+		ret = pm_runtime_get_sync(&data->client->dev);
+	else {
+		pm_runtime_mark_last_busy(&data->client->dev);
+		ret = pm_runtime_put_autosuspend(&data->client->dev);
+	}
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Failed: kxcjk1013_set_power_state for %d\n", on);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int kxcjk1013_chip_update_thresholds(struct kxcjk1013_data *data)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					KXCJK1013_REG_WAKE_TIMER,
+					data->wake_dur);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Error writing reg_wake_timer\n");
+		return ret;
+	}
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					KXCJK1013_REG_WAKE_THRES,
+					data->wake_thres);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_wake_thres\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int kxcjk1013_setup_any_motion_interrupt(struct kxcjk1013_data *data,
+						bool status)
+{
+	int ret;
+	enum kxcjk1013_mode store_mode;
+
+	ret = kxcjk1013_get_mode(data, &store_mode);
+	if (ret < 0)
+		return ret;
+
+	/* This is requirement by spec to change state to STANDBY */
+	ret = kxcjk1013_set_mode(data, STANDBY);
+	if (ret < 0)
+		return ret;
+
+	ret = kxcjk1013_chip_update_thresholds(data);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, KXCJK1013_REG_INT_CTRL1);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_ctrl1\n");
+		return ret;
+	}
+
+	if (status)
+		ret |= KXCJK1013_REG_INT_REG1_BIT_IEN;
+	else
+		ret &= ~KXCJK1013_REG_INT_REG1_BIT_IEN;
+
+	ret = i2c_smbus_write_byte_data(data->client, KXCJK1013_REG_INT_CTRL1,
+					ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_ctrl1\n");
+		return ret;
+	}
+
+	ret = i2c_smbus_read_byte_data(data->client, KXCJK1013_REG_CTRL1);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_ctrl1\n");
+		return ret;
+	}
+
+	if (status)
+		ret |= KXCJK1013_REG_CTRL1_BIT_WUFE;
+	else
+		ret &= ~KXCJK1013_REG_CTRL1_BIT_WUFE;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					KXCJK1013_REG_CTRL1, ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_ctrl1\n");
+		return ret;
+	}
+
+	if (store_mode == OPERATION) {
+		ret = kxcjk1013_set_mode(data, OPERATION);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int kxcjk1013_setup_new_data_interrupt(struct kxcjk1013_data *data,
+					      bool status)
+{
+	int ret;
+	enum kxcjk1013_mode store_mode;
+
+	ret = kxcjk1013_get_mode(data, &store_mode);
+	if (ret < 0)
+		return ret;
+
 	/* This is requirement by spec to change state to STANDBY */
 	ret = kxcjk1013_set_mode(data, STANDBY);
 	if (ret < 0)
@@ -250,7 +531,13 @@ static int kxcjk1013_chip_setup_interrupt(struct kxcjk1013_data *data,
 		return ret;
 	}
 
-	return ret;
+	if (store_mode == OPERATION) {
+		ret = kxcjk1013_set_mode(data, OPERATION);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
 }
 
 static int kxcjk1013_convert_freq_to_bit(int val, int val2)
@@ -267,10 +554,29 @@ static int kxcjk1013_convert_freq_to_bit(int val, int val2)
 	return -EINVAL;
 }
 
+static int kxcjk1013_convert_wake_odr_to_bit(int val, int val2)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(wake_odr_data_rate_table); ++i) {
+		if (wake_odr_data_rate_table[i].val == val &&
+			wake_odr_data_rate_table[i].val2 == val2) {
+			return wake_odr_data_rate_table[i].odr_bits;
+		}
+	}
+
+	return -EINVAL;
+}
+
 static int kxcjk1013_set_odr(struct kxcjk1013_data *data, int val, int val2)
 {
 	int ret;
 	int odr_bits;
+	enum kxcjk1013_mode store_mode;
+
+	ret = kxcjk1013_get_mode(data, &store_mode);
+	if (ret < 0)
+		return ret;
 
 	odr_bits = kxcjk1013_convert_freq_to_bit(val, val2);
 	if (odr_bits < 0)
@@ -290,9 +596,18 @@ static int kxcjk1013_set_odr(struct kxcjk1013_data *data, int val, int val2)
 
 	data->odr_bits = odr_bits;
 
-	/* Check, if the ODR is changed after data enable */
-	if (data->power_state) {
-		/* Set the state back to operation */
+	odr_bits = kxcjk1013_convert_wake_odr_to_bit(val, val2);
+	if (odr_bits < 0)
+		return odr_bits;
+
+	ret = i2c_smbus_write_byte_data(data->client, KXCJK1013_REG_CTRL2,
+					odr_bits);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_ctrl2\n");
+		return ret;
+	}
+
+	if (store_mode == OPERATION) {
 		ret = kxcjk1013_set_mode(data, OPERATION);
 		if (ret < 0)
 			return ret;
@@ -331,16 +646,38 @@ static int kxcjk1013_get_acc_reg(struct kxcjk1013_data *data, int axis)
 	return ret;
 }
 
-static int kxcjk1013_get_startup_times(struct kxcjk1013_data *data)
+static int kxcjk1013_set_scale(struct kxcjk1013_data *data, int val)
 {
-	int i;
+	int ret, i;
+	enum kxcjk1013_mode store_mode;
+
+
+	for (i = 0; i < ARRAY_SIZE(KXCJK1013_scale_table); ++i) {
+		if (KXCJK1013_scale_table[i].scale == val) {
+
+			ret = kxcjk1013_get_mode(data, &store_mode);
+			if (ret < 0)
+				return ret;
+
+			ret = kxcjk1013_set_mode(data, STANDBY);
+			if (ret < 0)
+				return ret;
+
+			ret = kxcjk1013_set_range(data, i);
+			if (ret < 0)
+				return ret;
 
-	for (i = 0; i < ARRAY_SIZE(odr_start_up_times); ++i) {
-		if (odr_start_up_times[i].odr_bits == data->odr_bits)
-			return odr_start_up_times[i].usec;
+			if (store_mode == OPERATION) {
+				ret = kxcjk1013_set_mode(data, OPERATION);
+				if (ret)
+					return ret;
+			}
+
+			return 0;
+		}
 	}
 
-	return KXCJK1013_MAX_STARTUP_TIME_US;
+	return -EINVAL;
 }
 
 static int kxcjk1013_read_raw(struct iio_dev *indio_dev,
@@ -356,34 +693,30 @@ static int kxcjk1013_read_raw(struct iio_dev *indio_dev,
 		if (iio_buffer_enabled(indio_dev))
 			ret = -EBUSY;
 		else {
-			int sleep_val;
-
-			ret = kxcjk1013_set_mode(data, OPERATION);
+			ret = kxcjk1013_set_power_state(data, true);
 			if (ret < 0) {
 				mutex_unlock(&data->mutex);
 				return ret;
 			}
-			++data->power_state;
-			sleep_val = kxcjk1013_get_startup_times(data);
-			if (sleep_val < 20000)
-				usleep_range(sleep_val, 20000);
-			else
-				msleep_interruptible(sleep_val/1000);
 			ret = kxcjk1013_get_acc_reg(data, chan->scan_index);
-			if (--data->power_state == 0)
-				kxcjk1013_set_mode(data, STANDBY);
+			if (ret < 0) {
+				kxcjk1013_set_power_state(data, false);
+				mutex_unlock(&data->mutex);
+				return ret;
+			}
+			*val = sign_extend32(ret >> 4, 11);
+			ret = kxcjk1013_set_power_state(data, false);
 		}
 		mutex_unlock(&data->mutex);
 
 		if (ret < 0)
 			return ret;
 
-		*val = sign_extend32(ret >> 4, 11);
 		return IIO_VAL_INT;
 
 	case IIO_CHAN_INFO_SCALE:
 		*val = 0;
-		*val2 = 19163; /* range +-4g (4/2047*9.806650) */
+		*val2 = KXCJK1013_scale_table[data->range].scale;
 		return IIO_VAL_INT_PLUS_MICRO;
 
 	case IIO_CHAN_INFO_SAMP_FREQ:
@@ -410,6 +743,14 @@ static int kxcjk1013_write_raw(struct iio_dev *indio_dev,
 		ret = kxcjk1013_set_odr(data, val, val2);
 		mutex_unlock(&data->mutex);
 		break;
+	case IIO_CHAN_INFO_SCALE:
+		if (val)
+			return -EINVAL;
+
+		mutex_lock(&data->mutex);
+		ret = kxcjk1013_set_scale(data, val2);
+		mutex_unlock(&data->mutex);
+		break;
 	default:
 		ret = -EINVAL;
 	}
@@ -417,12 +758,120 @@ static int kxcjk1013_write_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
+static int kxcjk1013_read_event(struct iio_dev *indio_dev,
+				   const struct iio_chan_spec *chan,
+				   enum iio_event_type type,
+				   enum iio_event_direction dir,
+				   enum iio_event_info info,
+				   int *val, int *val2)
+{
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+
+	*val2 = 0;
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		*val = data->wake_thres;
+		break;
+	case IIO_EV_INFO_PERIOD:
+		*val = data->wake_dur;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static int kxcjk1013_write_event(struct iio_dev *indio_dev,
+				    const struct iio_chan_spec *chan,
+				    enum iio_event_type type,
+				    enum iio_event_direction dir,
+				    enum iio_event_info info,
+				    int val, int val2)
+{
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+
+	if (data->ev_enable_state)
+		return -EBUSY;
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		data->wake_thres = val;
+		break;
+	case IIO_EV_INFO_PERIOD:
+		data->wake_dur = val;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int kxcjk1013_read_event_config(struct iio_dev *indio_dev,
+					  const struct iio_chan_spec *chan,
+					  enum iio_event_type type,
+					  enum iio_event_direction dir)
+{
+
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+
+	return data->ev_enable_state;
+}
+
+static int kxcjk1013_write_event_config(struct iio_dev *indio_dev,
+					   const struct iio_chan_spec *chan,
+					   enum iio_event_type type,
+					   enum iio_event_direction dir,
+					   int state)
+{
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (state && data->ev_enable_state)
+		return 0;
+
+	mutex_lock(&data->mutex);
+
+	if (!state && data->motion_trigger_on) {
+		data->ev_enable_state = 0;
+		mutex_unlock(&data->mutex);
+		return 0;
+	}
+
+	/*
+	 * We will expect the enable and disable to do operation in
+	 * in reverse order. This will happen here anyway as our
+	 * resume operation uses sync mode runtime pm calls, the
+	 * suspend operation will be delayed by autosuspend delay
+	 * So the disable operation will still happen in reverse of
+	 * enable operation. When runtime pm is disabled the mode
+	 * is always on so sequence doesn't matter
+	 */
+	ret = kxcjk1013_set_power_state(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	ret =  kxcjk1013_setup_any_motion_interrupt(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	data->ev_enable_state = state;
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
 static int kxcjk1013_validate_trigger(struct iio_dev *indio_dev,
 				      struct iio_trigger *trig)
 {
 	struct kxcjk1013_data *data = iio_priv(indio_dev);
 
-	if (data->trig != trig)
+	if (data->dready_trig != trig && data->motion_trig != trig)
 		return -EINVAL;
 
 	return 0;
@@ -431,8 +880,11 @@ static int kxcjk1013_validate_trigger(struct iio_dev *indio_dev,
 static IIO_CONST_ATTR_SAMP_FREQ_AVAIL(
 	"0.781000 1.563000 3.125000 6.250000 12.500000 25 50 100 200 400 800 1600");
 
+static IIO_CONST_ATTR(in_accel_scale_available, "0.009582 0.019163 0.038326");
+
 static struct attribute *kxcjk1013_attributes[] = {
 	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_const_attr_in_accel_scale_available.dev_attr.attr,
 	NULL,
 };
 
@@ -440,6 +892,14 @@ static const struct attribute_group kxcjk1013_attrs_group = {
 	.attrs = kxcjk1013_attributes,
 };
 
+static const struct iio_event_spec kxcjk1013_event = {
+		.type = IIO_EV_TYPE_THRESH,
+		.dir = IIO_EV_DIR_RISING | IIO_EV_DIR_FALLING,
+		.mask_separate = BIT(IIO_EV_INFO_VALUE) |
+				 BIT(IIO_EV_INFO_ENABLE) |
+				 BIT(IIO_EV_INFO_PERIOD)
+};
+
 #define KXCJK1013_CHANNEL(_axis) {					\
 	.type = IIO_ACCEL,						\
 	.modified = 1,							\
@@ -455,6 +915,8 @@ static const struct attribute_group kxcjk1013_attrs_group = {
 		.shift = 4,						\
 		.endianness = IIO_CPU,					\
 	},								\
+	.event_spec = &kxcjk1013_event,				\
+	.num_event_specs = 1						\
 }
 
 static const struct iio_chan_spec kxcjk1013_channels[] = {
@@ -468,6 +930,10 @@ static const struct iio_info kxcjk1013_info = {
 	.attrs			= &kxcjk1013_attrs_group,
 	.read_raw		= kxcjk1013_read_raw,
 	.write_raw		= kxcjk1013_write_raw,
+	.read_event_value	= kxcjk1013_read_event,
+	.write_event_value	= kxcjk1013_write_event,
+	.write_event_config	= kxcjk1013_write_event_config,
+	.read_event_config	= kxcjk1013_read_event_config,
 	.validate_trigger	= kxcjk1013_validate_trigger,
 	.driver_module		= THIS_MODULE,
 };
@@ -493,7 +959,7 @@ static irqreturn_t kxcjk1013_trigger_handler(int irq, void *p)
 	mutex_unlock(&data->mutex);
 
 	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
-					   pf->timestamp);
+					   data->timestamp);
 err:
 	iio_trigger_notify_done(indio_dev->trig);
 
@@ -520,20 +986,34 @@ static int kxcjk1013_data_rdy_trigger_set_state(struct iio_trigger *trig,
 {
 	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 	struct kxcjk1013_data *data = iio_priv(indio_dev);
+	int ret;
 
 	mutex_lock(&data->mutex);
-	if (state) {
-		kxcjk1013_chip_setup_interrupt(data, true);
-		kxcjk1013_set_mode(data, OPERATION);
-		++data->power_state;
-	} else {
-		if (--data->power_state) {
-			mutex_unlock(&data->mutex);
-			return 0;
-		}
-		kxcjk1013_chip_setup_interrupt(data, false);
-		kxcjk1013_set_mode(data, STANDBY);
+
+	if (!state && data->ev_enable_state && data->motion_trigger_on) {
+		data->motion_trigger_on = false;
+		mutex_unlock(&data->mutex);
+		return 0;
 	}
+
+	ret = kxcjk1013_set_power_state(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	if (data->motion_trig == trig)
+		ret = kxcjk1013_setup_any_motion_interrupt(data, state);
+	else
+		ret = kxcjk1013_setup_new_data_interrupt(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	if (data->motion_trig == trig)
+		data->motion_trigger_on = state;
+	else
+		data->dready_trigger_on = state;
+
 	mutex_unlock(&data->mutex);
 
 	return 0;
@@ -545,10 +1025,124 @@ static const struct iio_trigger_ops kxcjk1013_trigger_ops = {
 	.owner = THIS_MODULE,
 };
 
-static int kxcjk1013_acpi_gpio_probe(struct i2c_client *client,
-				     struct kxcjk1013_data *data)
+static irqreturn_t kxcjk1013_event_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, KXCJK1013_REG_INT_SRC1);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_src1\n");
+		goto ack_intr;
+	}
+
+	if (ret & 0x02) {
+		ret = i2c_smbus_read_byte_data(data->client,
+					       KXCJK1013_REG_INT_SRC2);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error reading reg_int_src2\n");
+			goto ack_intr;
+		}
+
+		if (ret & KXCJK1013_REG_INT_SRC2_BIT_XN)
+			iio_push_event(indio_dev,
+				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
+				       0,
+				       IIO_MOD_X,
+				       IIO_EV_TYPE_THRESH,
+				       IIO_EV_DIR_FALLING),
+				       data->timestamp);
+		if (ret & KXCJK1013_REG_INT_SRC2_BIT_XP)
+			iio_push_event(indio_dev,
+				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
+				       0,
+				       IIO_MOD_X,
+				       IIO_EV_TYPE_THRESH,
+				       IIO_EV_DIR_RISING),
+				       data->timestamp);
+
+
+		if (ret & KXCJK1013_REG_INT_SRC2_BIT_YN)
+			iio_push_event(indio_dev,
+				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
+				       0,
+				       IIO_MOD_Y,
+				       IIO_EV_TYPE_THRESH,
+				       IIO_EV_DIR_FALLING),
+				       data->timestamp);
+		if (ret & KXCJK1013_REG_INT_SRC2_BIT_YP)
+			iio_push_event(indio_dev,
+				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
+				       0,
+				       IIO_MOD_Y,
+				       IIO_EV_TYPE_THRESH,
+				       IIO_EV_DIR_RISING),
+				       data->timestamp);
+
+		if (ret & KXCJK1013_REG_INT_SRC2_BIT_ZN)
+			iio_push_event(indio_dev,
+				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
+				       0,
+				       IIO_MOD_Z,
+				       IIO_EV_TYPE_THRESH,
+				       IIO_EV_DIR_FALLING),
+				       data->timestamp);
+		if (ret & KXCJK1013_REG_INT_SRC2_BIT_ZP)
+			iio_push_event(indio_dev,
+				       IIO_MOD_EVENT_CODE(IIO_ACCEL,
+				       0,
+				       IIO_MOD_Z,
+				       IIO_EV_TYPE_THRESH,
+				       IIO_EV_DIR_RISING),
+				       data->timestamp);
+	}
+
+ack_intr:
+	if (data->dready_trigger_on)
+		return IRQ_HANDLED;
+
+	ret = i2c_smbus_read_byte_data(data->client, KXCJK1013_REG_INT_REL);
+	if (ret < 0)
+		dev_err(&data->client->dev, "Error reading reg_int_rel\n");
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t kxcjk1013_data_rdy_trig_poll(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+
+	data->timestamp = iio_get_time_ns();
+
+	if (data->dready_trigger_on)
+		iio_trigger_poll(data->dready_trig);
+	else if (data->motion_trigger_on)
+		iio_trigger_poll(data->motion_trig);
+
+	if (data->ev_enable_state)
+		return IRQ_WAKE_THREAD;
+	else
+		return IRQ_HANDLED;
+}
+
+static const char *kxcjk1013_match_acpi_device(struct device *dev,
+					       enum kx_chipset *chipset)
 {
 	const struct acpi_device_id *id;
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return NULL;
+	*chipset = (enum kx_chipset)id->driver_data;
+
+	return dev_name(dev);
+}
+
+static int kxcjk1013_gpio_probe(struct i2c_client *client,
+				struct kxcjk1013_data *data)
+{
 	struct device *dev;
 	struct gpio_desc *gpio;
 	int ret;
@@ -557,12 +1151,6 @@ static int kxcjk1013_acpi_gpio_probe(struct i2c_client *client,
 		return -EINVAL;
 
 	dev = &client->dev;
-	if (!ACPI_HANDLE(dev))
-		return -ENODEV;
-
-	id = acpi_match_device(dev->driver->acpi_match_table, dev);
-	if (!id)
-		return -ENODEV;
 
 	/* data ready gpio interrupt pin */
 	gpio = devm_gpiod_get_index(dev, "kxcjk1013_int", 0);
@@ -587,8 +1175,8 @@ static int kxcjk1013_probe(struct i2c_client *client,
 {
 	struct kxcjk1013_data *data;
 	struct iio_dev *indio_dev;
-	struct iio_trigger *trig = NULL;
 	struct kxcjk_1013_platform_data *pdata;
+	const char *name;
 	int ret;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
@@ -605,6 +1193,15 @@ static int kxcjk1013_probe(struct i2c_client *client,
 	else
 		data->active_high_intr = true; /* default polarity */
 
+	if (id) {
+		data->chipset = (enum kx_chipset)(id->driver_data);
+		name = id->name;
+	} else if (ACPI_HANDLE(&client->dev)) {
+		name = kxcjk1013_match_acpi_device(&client->dev,
+						   &data->chipset);
+	} else
+		return -ENODEV;
+
 	ret = kxcjk1013_chip_init(data);
 	if (ret < 0)
 		return ret;
@@ -614,41 +1211,54 @@ static int kxcjk1013_probe(struct i2c_client *client,
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->channels = kxcjk1013_channels;
 	indio_dev->num_channels = ARRAY_SIZE(kxcjk1013_channels);
-	indio_dev->name = KXCJK1013_DRV_NAME;
+	indio_dev->name = name;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	indio_dev->info = &kxcjk1013_info;
 
 	if (client->irq < 0)
-		client->irq = kxcjk1013_acpi_gpio_probe(client, data);
+		client->irq = kxcjk1013_gpio_probe(client, data);
 
 	if (client->irq >= 0) {
-		trig = iio_trigger_alloc("%s-dev%d", indio_dev->name,
-					 indio_dev->id);
-		if (!trig)
-			return -ENOMEM;
+		ret = devm_request_threaded_irq(&client->dev, client->irq,
+						kxcjk1013_data_rdy_trig_poll,
+						kxcjk1013_event_handler,
+						IRQF_TRIGGER_RISING,
+						KXCJK1013_IRQ_NAME,
+						indio_dev);
+		if (ret)
+			return ret;
 
-		data->trig_mode = true;
+		data->dready_trig = devm_iio_trigger_alloc(&client->dev,
+							   "%s-dev%d",
+							   indio_dev->name,
+							   indio_dev->id);
+		if (!data->dready_trig)
+			return -ENOMEM;
 
-		ret = devm_request_irq(&client->dev, client->irq,
-					iio_trigger_generic_data_rdy_poll,
-					IRQF_TRIGGER_RISING,
-					KXCJK1013_IRQ_NAME,
-					trig);
-		if (ret) {
-			dev_err(&client->dev, "unable to request IRQ\n");
-			goto err_trigger_free;
-		}
+		data->motion_trig = devm_iio_trigger_alloc(&client->dev,
+							  "%s-any-motion-dev%d",
+							  indio_dev->name,
+							  indio_dev->id);
+		if (!data->motion_trig)
+			return -ENOMEM;
 
-		trig->dev.parent = &client->dev;
-		trig->ops = &kxcjk1013_trigger_ops;
-		iio_trigger_set_drvdata(trig, indio_dev);
-		data->trig = trig;
-		indio_dev->trig = trig;
+		data->dready_trig->dev.parent = &client->dev;
+		data->dready_trig->ops = &kxcjk1013_trigger_ops;
+		iio_trigger_set_drvdata(data->dready_trig, indio_dev);
+		indio_dev->trig = data->dready_trig;
 		iio_trigger_get(indio_dev->trig);
-
-		ret = iio_trigger_register(trig);
+		ret = iio_trigger_register(data->dready_trig);
 		if (ret)
-			goto err_trigger_free;
+			return ret;
+
+		data->motion_trig->dev.parent = &client->dev;
+		data->motion_trig->ops = &kxcjk1013_trigger_ops;
+		iio_trigger_set_drvdata(data->motion_trig, indio_dev);
+		ret = iio_trigger_register(data->motion_trig);
+		if (ret) {
+			data->motion_trig = NULL;
+			goto err_trigger_unregister;
+		}
 
 		ret = iio_triggered_buffer_setup(indio_dev,
 						&iio_pollfunc_store_time,
@@ -661,23 +1271,33 @@ static int kxcjk1013_probe(struct i2c_client *client,
 		}
 	}
 
-	ret = devm_iio_device_register(&client->dev, indio_dev);
+	ret = iio_device_register(indio_dev);
 	if (ret < 0) {
 		dev_err(&client->dev, "unable to register iio device\n");
 		goto err_buffer_cleanup;
 	}
 
+	ret = pm_runtime_set_active(&client->dev);
+	if (ret)
+		goto err_iio_unregister;
+
+	pm_runtime_enable(&client->dev);
+	pm_runtime_set_autosuspend_delay(&client->dev,
+					 KXCJK1013_SLEEP_DELAY_MS);
+	pm_runtime_use_autosuspend(&client->dev);
+
 	return 0;
 
+err_iio_unregister:
+	iio_device_unregister(indio_dev);
 err_buffer_cleanup:
-	if (data->trig_mode)
+	if (data->dready_trig)
 		iio_triggered_buffer_cleanup(indio_dev);
 err_trigger_unregister:
-	if (data->trig_mode)
-		iio_trigger_unregister(trig);
-err_trigger_free:
-	if (data->trig_mode)
-		iio_trigger_free(trig);
+	if (data->dready_trig)
+		iio_trigger_unregister(data->dready_trig);
+	if (data->motion_trig)
+		iio_trigger_unregister(data->motion_trig);
 
 	return ret;
 }
@@ -687,10 +1307,16 @@ static int kxcjk1013_remove(struct i2c_client *client)
 	struct iio_dev *indio_dev = i2c_get_clientdata(client);
 	struct kxcjk1013_data *data = iio_priv(indio_dev);
 
-	if (data->trig_mode) {
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (data->dready_trig) {
 		iio_triggered_buffer_cleanup(indio_dev);
-		iio_trigger_unregister(data->trig);
-		iio_trigger_free(data->trig);
+		iio_trigger_unregister(data->dready_trig);
+		iio_trigger_unregister(data->motion_trig);
 	}
 
 	mutex_lock(&data->mutex);
@@ -705,43 +1331,80 @@ static int kxcjk1013_suspend(struct device *dev)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
 	struct kxcjk1013_data *data = iio_priv(indio_dev);
+	int ret;
 
 	mutex_lock(&data->mutex);
-	kxcjk1013_set_mode(data, STANDBY);
+	ret = kxcjk1013_set_mode(data, STANDBY);
 	mutex_unlock(&data->mutex);
 
-	return 0;
+	return ret;
 }
 
 static int kxcjk1013_resume(struct device *dev)
 {
 	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
 	struct kxcjk1013_data *data = iio_priv(indio_dev);
+	int ret = 0;
 
 	mutex_lock(&data->mutex);
+	/* Check, if the suspend occured while active */
+	if (data->dready_trigger_on || data->motion_trigger_on ||
+							data->ev_enable_state)
+		ret = kxcjk1013_set_mode(data, OPERATION);
+	mutex_unlock(&data->mutex);
 
-	if (data->power_state)
-		kxcjk1013_set_mode(data, OPERATION);
+	return ret;
+}
+#endif
 
-	mutex_unlock(&data->mutex);
+#ifdef CONFIG_PM_RUNTIME
+static int kxcjk1013_runtime_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
 
-	return 0;
+	return kxcjk1013_set_mode(data, STANDBY);
 }
 
-static SIMPLE_DEV_PM_OPS(kxcjk1013_pm_ops, kxcjk1013_suspend, kxcjk1013_resume);
-#define KXCJK1013_PM_OPS (&kxcjk1013_pm_ops)
-#else
-#define KXCJK1013_PM_OPS NULL
+static int kxcjk1013_runtime_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct kxcjk1013_data *data = iio_priv(indio_dev);
+	int ret;
+	int sleep_val;
+
+	ret = kxcjk1013_set_mode(data, OPERATION);
+	if (ret < 0)
+		return ret;
+
+	sleep_val = kxcjk1013_get_startup_times(data);
+	if (sleep_val < 20000)
+		usleep_range(sleep_val, 20000);
+	else
+		msleep_interruptible(sleep_val/1000);
+
+	return 0;
+}
 #endif
 
+static const struct dev_pm_ops kxcjk1013_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(kxcjk1013_suspend, kxcjk1013_resume)
+	SET_RUNTIME_PM_OPS(kxcjk1013_runtime_suspend,
+			   kxcjk1013_runtime_resume, NULL)
+};
+
 static const struct acpi_device_id kx_acpi_match[] = {
-	{"KXCJ1013", 0},
+	{"KXCJ1013", KXCJK1013},
+	{"KXCJ1008", KXCJ91008},
+	{"KXTJ1009", KXTJ21009},
 	{ },
 };
 MODULE_DEVICE_TABLE(acpi, kx_acpi_match);
 
 static const struct i2c_device_id kxcjk1013_id[] = {
-	{"kxcjk1013", 0},
+	{"kxcjk1013", KXCJK1013},
+	{"kxcj91008", KXCJ91008},
+	{"kxtj21009", KXTJ21009},
 	{}
 };
 
@@ -751,7 +1414,7 @@ static struct i2c_driver kxcjk1013_driver = {
 	.driver = {
 		.name	= KXCJK1013_DRV_NAME,
 		.acpi_match_table = ACPI_PTR(kx_acpi_match),
-		.pm	= KXCJK1013_PM_OPS,
+		.pm	= &kxcjk1013_pm_ops,
 	},
 	.probe		= kxcjk1013_probe,
 	.remove		= kxcjk1013_remove,
diff --git a/drivers/iio/adc/Kconfig b/drivers/iio/adc/Kconfig
index 11b048a59fde..88bdc8f612e2 100644
--- a/drivers/iio/adc/Kconfig
+++ b/drivers/iio/adc/Kconfig
@@ -129,7 +129,7 @@ config AT91_ADC
 
 config EXYNOS_ADC
 	tristate "Exynos ADC driver support"
-	depends on ARCH_EXYNOS || (OF && COMPILE_TEST)
+	depends on ARCH_EXYNOS || ARCH_S3C24XX || ARCH_S3C64XX || (OF && COMPILE_TEST)
 	help
 	  Core support for the ADC block found in the Samsung EXYNOS series
 	  of SoCs for drivers such as the touchscreen and hwmon to use to share
@@ -206,6 +206,16 @@ config NAU7802
 	  To compile this driver as a module, choose M here: the
 	  module will be called nau7802.
 
+config ROCKCHIP_SARADC
+	tristate "Rockchip SARADC driver"
+	depends on ARCH_ROCKCHIP || (ARM && COMPILE_TEST)
+	help
+	  Say yes here to build support for the SARADC found in SoCs from
+	  Rockchip.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called rockchip_saradc.
+
 config TI_ADC081C
 	tristate "Texas Instruments ADC081C021/027"
 	depends on I2C
@@ -216,6 +226,16 @@ config TI_ADC081C
 	  This driver can also be built as a module. If so, the module will be
 	  called ti-adc081c.
 
+config TI_ADC128S052
+	tristate "Texas Instruments ADC128S052"
+	depends on SPI
+	help
+	  If you say yes here you get support for Texas Instruments ADC128S052
+	  chip.
+
+	  This driver can also be built as a module. If so, the module will be
+	  called ti-adc128s052.
+
 config TI_AM335X_ADC
 	tristate "TI's AM335X ADC driver"
 	depends on MFD_TI_AM335X_TSCADC
diff --git a/drivers/iio/adc/Makefile b/drivers/iio/adc/Makefile
index ad81b512aa3d..cb88a6a23b8f 100644
--- a/drivers/iio/adc/Makefile
+++ b/drivers/iio/adc/Makefile
@@ -22,7 +22,9 @@ obj-$(CONFIG_MCP320X) += mcp320x.o
 obj-$(CONFIG_MCP3422) += mcp3422.o
 obj-$(CONFIG_MEN_Z188_ADC) += men_z188_adc.o
 obj-$(CONFIG_NAU7802) += nau7802.o
+obj-$(CONFIG_ROCKCHIP_SARADC) += rockchip_saradc.o
 obj-$(CONFIG_TI_ADC081C) += ti-adc081c.o
+obj-$(CONFIG_TI_ADC128S052) += ti-adc128s052.o
 obj-$(CONFIG_TI_AM335X_ADC) += ti_am335x_adc.o
 obj-$(CONFIG_TWL4030_MADC) += twl4030-madc.o
 obj-$(CONFIG_TWL6030_GPADC) += twl6030-gpadc.o
diff --git a/drivers/iio/adc/ad_sigma_delta.c b/drivers/iio/adc/ad_sigma_delta.c
index c55b81f7f970..d10bd0c97233 100644
--- a/drivers/iio/adc/ad_sigma_delta.c
+++ b/drivers/iio/adc/ad_sigma_delta.c
@@ -472,7 +472,7 @@ static int ad_sd_probe_trigger(struct iio_dev *indio_dev)
 		goto error_free_irq;
 
 	/* select default trigger */
-	indio_dev->trig = sigma_delta->trig;
+	indio_dev->trig = iio_trigger_get(sigma_delta->trig);
 
 	return 0;
 
diff --git a/drivers/iio/adc/at91_adc.c b/drivers/iio/adc/at91_adc.c
index 772e869c280e..ff61ae55dd3f 100644
--- a/drivers/iio/adc/at91_adc.c
+++ b/drivers/iio/adc/at91_adc.c
@@ -196,6 +196,7 @@ struct at91_adc_state {
 	bool			done;
 	int			irq;
 	u16			last_value;
+	int			chnb;
 	struct mutex		lock;
 	u8			num_channels;
 	void __iomem		*reg_base;
@@ -266,7 +267,7 @@ static irqreturn_t at91_adc_trigger_handler(int irq, void *p)
 }
 
 /* Handler for classic adc channel eoc trigger */
-void handle_adc_eoc_trigger(int irq, struct iio_dev *idev)
+static void handle_adc_eoc_trigger(int irq, struct iio_dev *idev)
 {
 	struct at91_adc_state *st = iio_priv(idev);
 
@@ -274,7 +275,7 @@ void handle_adc_eoc_trigger(int irq, struct iio_dev *idev)
 		disable_irq_nosync(irq);
 		iio_trigger_poll(idev->trig);
 	} else {
-		st->last_value = at91_adc_readl(st, AT91_ADC_LCDR);
+		st->last_value = at91_adc_readl(st, AT91_ADC_CHAN(st, st->chnb));
 		st->done = true;
 		wake_up_interruptible(&st->wq_data_avail);
 	}
@@ -351,7 +352,7 @@ static irqreturn_t at91_adc_rl_interrupt(int irq, void *private)
 	unsigned int reg;
 
 	status &= at91_adc_readl(st, AT91_ADC_IMR);
-	if (status & st->registers->drdy_mask)
+	if (status & GENMASK(st->num_channels - 1, 0))
 		handle_adc_eoc_trigger(irq, idev);
 
 	if (status & AT91RL_ADC_IER_PEN) {
@@ -418,7 +419,7 @@ static irqreturn_t at91_adc_9x5_interrupt(int irq, void *private)
 		AT91_ADC_IER_YRDY |
 		AT91_ADC_IER_PRDY;
 
-	if (status & st->registers->drdy_mask)
+	if (status & GENMASK(st->num_channels - 1, 0))
 		handle_adc_eoc_trigger(irq, idev);
 
 	if (status & AT91_ADC_IER_PEN) {
@@ -689,9 +690,10 @@ static int at91_adc_read_raw(struct iio_dev *idev,
 	case IIO_CHAN_INFO_RAW:
 		mutex_lock(&st->lock);
 
+		st->chnb = chan->channel;
 		at91_adc_writel(st, AT91_ADC_CHER,
 				AT91_ADC_CH(chan->channel));
-		at91_adc_writel(st, AT91_ADC_IER, st->registers->drdy_mask);
+		at91_adc_writel(st, AT91_ADC_IER, BIT(chan->channel));
 		at91_adc_writel(st, AT91_ADC_CR, AT91_ADC_START);
 
 		ret = wait_event_interruptible_timeout(st->wq_data_avail,
@@ -708,7 +710,7 @@ static int at91_adc_read_raw(struct iio_dev *idev,
 
 		at91_adc_writel(st, AT91_ADC_CHDR,
 				AT91_ADC_CH(chan->channel));
-		at91_adc_writel(st, AT91_ADC_IDR, st->registers->drdy_mask);
+		at91_adc_writel(st, AT91_ADC_IDR, BIT(chan->channel));
 
 		st->last_value = 0;
 		st->done = false;
diff --git a/drivers/iio/adc/exynos_adc.c b/drivers/iio/adc/exynos_adc.c
index fc9dfc23ecb7..43620fd4c66a 100644
--- a/drivers/iio/adc/exynos_adc.c
+++ b/drivers/iio/adc/exynos_adc.c
@@ -40,13 +40,16 @@
 #include <linux/iio/machine.h>
 #include <linux/iio/driver.h>
 
-/* EXYNOS4412/5250 ADC_V1 registers definitions */
+/* S3C/EXYNOS4412/5250 ADC_V1 registers definitions */
 #define ADC_V1_CON(x)		((x) + 0x00)
 #define ADC_V1_DLY(x)		((x) + 0x08)
 #define ADC_V1_DATX(x)		((x) + 0x0C)
 #define ADC_V1_INTCLR(x)	((x) + 0x18)
 #define ADC_V1_MUX(x)		((x) + 0x1c)
 
+/* S3C2410 ADC registers definitions */
+#define ADC_S3C2410_MUX(x)	((x) + 0x18)
+
 /* Future ADC_V2 registers definitions */
 #define ADC_V2_CON1(x)		((x) + 0x00)
 #define ADC_V2_CON2(x)		((x) + 0x04)
@@ -61,6 +64,11 @@
 #define ADC_V1_CON_PRSCLV(x)	(((x) & 0xFF) << 6)
 #define ADC_V1_CON_STANDBY	(1u << 2)
 
+/* Bit definitions for S3C2410 ADC */
+#define ADC_S3C2410_CON_SELMUX(x) (((x) & 7) << 3)
+#define ADC_S3C2410_DATX_MASK	0x3FF
+#define ADC_S3C2416_CON_RES_SEL	(1u << 3)
+
 /* Bit definitions for ADC_V2 */
 #define ADC_V2_CON1_SOFT_RESET	(1u << 2)
 
@@ -77,6 +85,7 @@
 
 /* Bit definitions common for ADC_V1 and ADC_V2 */
 #define ADC_CON_EN_START	(1u << 0)
+#define ADC_CON_EN_START_MASK	(0x3 << 0)
 #define ADC_DATX_MASK		0xFFF
 
 #define EXYNOS_ADC_TIMEOUT	(msecs_to_jiffies(100))
@@ -100,6 +109,8 @@ struct exynos_adc {
 struct exynos_adc_data {
 	int num_channels;
 	bool needs_sclk;
+	bool needs_adc_phy;
+	u32 mask;
 
 	void (*init_hw)(struct exynos_adc *info);
 	void (*exit_hw)(struct exynos_adc *info);
@@ -171,7 +182,8 @@ static void exynos_adc_v1_init_hw(struct exynos_adc *info)
 {
 	u32 con1;
 
-	writel(1, info->enable_reg);
+	if (info->data->needs_adc_phy)
+		writel(1, info->enable_reg);
 
 	/* set default prescaler values and Enable prescaler */
 	con1 =  ADC_V1_CON_PRSCLV(49) | ADC_V1_CON_PRSCEN;
@@ -185,7 +197,8 @@ static void exynos_adc_v1_exit_hw(struct exynos_adc *info)
 {
 	u32 con;
 
-	writel(0, info->enable_reg);
+	if (info->data->needs_adc_phy)
+		writel(0, info->enable_reg);
 
 	con = readl(ADC_V1_CON(info->regs));
 	con |= ADC_V1_CON_STANDBY;
@@ -210,6 +223,8 @@ static void exynos_adc_v1_start_conv(struct exynos_adc *info,
 
 static const struct exynos_adc_data exynos_adc_v1_data = {
 	.num_channels	= MAX_ADC_V1_CHANNELS,
+	.mask		= ADC_DATX_MASK,	/* 12 bit ADC resolution */
+	.needs_adc_phy	= true,
 
 	.init_hw	= exynos_adc_v1_init_hw,
 	.exit_hw	= exynos_adc_v1_exit_hw,
@@ -217,11 +232,89 @@ static const struct exynos_adc_data exynos_adc_v1_data = {
 	.start_conv	= exynos_adc_v1_start_conv,
 };
 
+static void exynos_adc_s3c2416_start_conv(struct exynos_adc *info,
+					  unsigned long addr)
+{
+	u32 con1;
+
+	/* Enable 12 bit ADC resolution */
+	con1 = readl(ADC_V1_CON(info->regs));
+	con1 |= ADC_S3C2416_CON_RES_SEL;
+	writel(con1, ADC_V1_CON(info->regs));
+
+	/* Select channel for S3C2416 */
+	writel(addr, ADC_S3C2410_MUX(info->regs));
+
+	con1 = readl(ADC_V1_CON(info->regs));
+	writel(con1 | ADC_CON_EN_START, ADC_V1_CON(info->regs));
+}
+
+static struct exynos_adc_data const exynos_adc_s3c2416_data = {
+	.num_channels	= MAX_ADC_V1_CHANNELS,
+	.mask		= ADC_DATX_MASK,	/* 12 bit ADC resolution */
+
+	.init_hw	= exynos_adc_v1_init_hw,
+	.exit_hw	= exynos_adc_v1_exit_hw,
+	.start_conv	= exynos_adc_s3c2416_start_conv,
+};
+
+static void exynos_adc_s3c2443_start_conv(struct exynos_adc *info,
+					  unsigned long addr)
+{
+	u32 con1;
+
+	/* Select channel for S3C2433 */
+	writel(addr, ADC_S3C2410_MUX(info->regs));
+
+	con1 = readl(ADC_V1_CON(info->regs));
+	writel(con1 | ADC_CON_EN_START, ADC_V1_CON(info->regs));
+}
+
+static struct exynos_adc_data const exynos_adc_s3c2443_data = {
+	.num_channels	= MAX_ADC_V1_CHANNELS,
+	.mask		= ADC_S3C2410_DATX_MASK, /* 10 bit ADC resolution */
+
+	.init_hw	= exynos_adc_v1_init_hw,
+	.exit_hw	= exynos_adc_v1_exit_hw,
+	.start_conv	= exynos_adc_s3c2443_start_conv,
+};
+
+static void exynos_adc_s3c64xx_start_conv(struct exynos_adc *info,
+					  unsigned long addr)
+{
+	u32 con1;
+
+	con1 = readl(ADC_V1_CON(info->regs));
+	con1 &= ~ADC_S3C2410_CON_SELMUX(0x7);
+	con1 |= ADC_S3C2410_CON_SELMUX(addr);
+	writel(con1 | ADC_CON_EN_START, ADC_V1_CON(info->regs));
+}
+
+static struct exynos_adc_data const exynos_adc_s3c24xx_data = {
+	.num_channels	= MAX_ADC_V1_CHANNELS,
+	.mask		= ADC_S3C2410_DATX_MASK, /* 10 bit ADC resolution */
+
+	.init_hw	= exynos_adc_v1_init_hw,
+	.exit_hw	= exynos_adc_v1_exit_hw,
+	.start_conv	= exynos_adc_s3c64xx_start_conv,
+};
+
+static struct exynos_adc_data const exynos_adc_s3c64xx_data = {
+	.num_channels	= MAX_ADC_V1_CHANNELS,
+	.mask		= ADC_DATX_MASK,	/* 12 bit ADC resolution */
+
+	.init_hw	= exynos_adc_v1_init_hw,
+	.exit_hw	= exynos_adc_v1_exit_hw,
+	.clear_irq	= exynos_adc_v1_clear_irq,
+	.start_conv	= exynos_adc_s3c64xx_start_conv,
+};
+
 static void exynos_adc_v2_init_hw(struct exynos_adc *info)
 {
 	u32 con1, con2;
 
-	writel(1, info->enable_reg);
+	if (info->data->needs_adc_phy)
+		writel(1, info->enable_reg);
 
 	con1 = ADC_V2_CON1_SOFT_RESET;
 	writel(con1, ADC_V2_CON1(info->regs));
@@ -238,7 +331,8 @@ static void exynos_adc_v2_exit_hw(struct exynos_adc *info)
 {
 	u32 con;
 
-	writel(0, info->enable_reg);
+	if (info->data->needs_adc_phy)
+		writel(0, info->enable_reg);
 
 	con = readl(ADC_V2_CON1(info->regs));
 	con &= ~ADC_CON_EN_START;
@@ -266,6 +360,8 @@ static void exynos_adc_v2_start_conv(struct exynos_adc *info,
 
 static const struct exynos_adc_data exynos_adc_v2_data = {
 	.num_channels	= MAX_ADC_V2_CHANNELS,
+	.mask		= ADC_DATX_MASK, /* 12 bit ADC resolution */
+	.needs_adc_phy	= true,
 
 	.init_hw	= exynos_adc_v2_init_hw,
 	.exit_hw	= exynos_adc_v2_exit_hw,
@@ -275,7 +371,9 @@ static const struct exynos_adc_data exynos_adc_v2_data = {
 
 static const struct exynos_adc_data exynos3250_adc_data = {
 	.num_channels	= MAX_EXYNOS3250_ADC_CHANNELS,
+	.mask		= ADC_DATX_MASK, /* 12 bit ADC resolution */
 	.needs_sclk	= true,
+	.needs_adc_phy	= true,
 
 	.init_hw	= exynos_adc_v2_init_hw,
 	.exit_hw	= exynos_adc_v2_exit_hw,
@@ -285,6 +383,21 @@ static const struct exynos_adc_data exynos3250_adc_data = {
 
 static const struct of_device_id exynos_adc_match[] = {
 	{
+		.compatible = "samsung,s3c2410-adc",
+		.data = &exynos_adc_s3c24xx_data,
+	}, {
+		.compatible = "samsung,s3c2416-adc",
+		.data = &exynos_adc_s3c2416_data,
+	}, {
+		.compatible = "samsung,s3c2440-adc",
+		.data = &exynos_adc_s3c24xx_data,
+	}, {
+		.compatible = "samsung,s3c2443-adc",
+		.data = &exynos_adc_s3c2443_data,
+	}, {
+		.compatible = "samsung,s3c6410-adc",
+		.data = &exynos_adc_s3c64xx_data,
+	}, {
 		.compatible = "samsung,exynos-adc-v1",
 		.data = &exynos_adc_v1_data,
 	}, {
@@ -347,9 +460,10 @@ static int exynos_read_raw(struct iio_dev *indio_dev,
 static irqreturn_t exynos_adc_isr(int irq, void *dev_id)
 {
 	struct exynos_adc *info = (struct exynos_adc *)dev_id;
+	u32 mask = info->data->mask;
 
 	/* Read value */
-	info->value = readl(ADC_V1_DATX(info->regs)) & ADC_DATX_MASK;
+	info->value = readl(ADC_V1_DATX(info->regs)) & mask;
 
 	/* clear irq */
 	if (info->data->clear_irq)
@@ -442,10 +556,13 @@ static int exynos_adc_probe(struct platform_device *pdev)
 	if (IS_ERR(info->regs))
 		return PTR_ERR(info->regs);
 
-	mem = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	info->enable_reg = devm_ioremap_resource(&pdev->dev, mem);
-	if (IS_ERR(info->enable_reg))
-		return PTR_ERR(info->enable_reg);
+
+	if (info->data->needs_adc_phy) {
+		mem = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+		info->enable_reg = devm_ioremap_resource(&pdev->dev, mem);
+		if (IS_ERR(info->enable_reg))
+			return PTR_ERR(info->enable_reg);
+	}
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
@@ -606,7 +723,6 @@ static struct platform_driver exynos_adc_driver = {
 	.remove		= exynos_adc_remove,
 	.driver		= {
 		.name	= "exynos-adc",
-		.owner	= THIS_MODULE,
 		.of_match_table = exynos_adc_match,
 		.pm	= &exynos_adc_pm_ops,
 	},
diff --git a/drivers/iio/adc/lp8788_adc.c b/drivers/iio/adc/lp8788_adc.c
index 5c8c91595f47..152cfc8e1c7b 100644
--- a/drivers/iio/adc/lp8788_adc.c
+++ b/drivers/iio/adc/lp8788_adc.c
@@ -244,7 +244,6 @@ static struct platform_driver lp8788_adc_driver = {
 	.remove = lp8788_adc_remove,
 	.driver = {
 		.name = LP8788_DEV_ADC,
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(lp8788_adc_driver);
diff --git a/drivers/iio/adc/rockchip_saradc.c b/drivers/iio/adc/rockchip_saradc.c
new file mode 100644
index 000000000000..e074a0b03f28
--- /dev/null
+++ b/drivers/iio/adc/rockchip_saradc.c
@@ -0,0 +1,316 @@
+/*
+ * Rockchip Successive Approximation Register (SAR) A/D Converter
+ * Copyright (C) 2014 ROCKCHIP, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/clk.h>
+#include <linux/completion.h>
+#include <linux/regulator/consumer.h>
+#include <linux/iio/iio.h>
+
+#define SARADC_DATA			0x00
+#define SARADC_DATA_MASK		0x3ff
+
+#define SARADC_STAS			0x04
+#define SARADC_STAS_BUSY		BIT(0)
+
+#define SARADC_CTRL			0x08
+#define SARADC_CTRL_IRQ_STATUS		BIT(6)
+#define SARADC_CTRL_IRQ_ENABLE		BIT(5)
+#define SARADC_CTRL_POWER_CTRL		BIT(3)
+#define SARADC_CTRL_CHN_MASK		0x7
+
+#define SARADC_DLY_PU_SOC		0x0c
+#define SARADC_DLY_PU_SOC_MASK		0x3f
+
+#define SARADC_BITS			10
+#define SARADC_TIMEOUT			msecs_to_jiffies(100)
+
+struct rockchip_saradc {
+	void __iomem		*regs;
+	struct clk		*pclk;
+	struct clk		*clk;
+	struct completion	completion;
+	struct regulator	*vref;
+	u16			last_val;
+};
+
+static int rockchip_saradc_read_raw(struct iio_dev *indio_dev,
+				    struct iio_chan_spec const *chan,
+				    int *val, int *val2, long mask)
+{
+	struct rockchip_saradc *info = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		mutex_lock(&indio_dev->mlock);
+
+		reinit_completion(&info->completion);
+
+		/* 8 clock periods as delay between power up and start cmd */
+		writel_relaxed(8, info->regs + SARADC_DLY_PU_SOC);
+
+		/* Select the channel to be used and trigger conversion */
+		writel(SARADC_CTRL_POWER_CTRL
+				| (chan->channel & SARADC_CTRL_CHN_MASK)
+				| SARADC_CTRL_IRQ_ENABLE,
+		       info->regs + SARADC_CTRL);
+
+		if (!wait_for_completion_timeout(&info->completion,
+						 SARADC_TIMEOUT)) {
+			writel_relaxed(0, info->regs + SARADC_CTRL);
+			mutex_unlock(&indio_dev->mlock);
+			return -ETIMEDOUT;
+		}
+
+		*val = info->last_val;
+		mutex_unlock(&indio_dev->mlock);
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		ret = regulator_get_voltage(info->vref);
+		if (ret < 0) {
+			dev_err(&indio_dev->dev, "failed to get voltage\n");
+			return ret;
+		}
+
+		*val = ret / 1000;
+		*val2 = SARADC_BITS;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	default:
+		return -EINVAL;
+	}
+}
+
+static irqreturn_t rockchip_saradc_isr(int irq, void *dev_id)
+{
+	struct rockchip_saradc *info = (struct rockchip_saradc *)dev_id;
+
+	/* Read value */
+	info->last_val = readl_relaxed(info->regs + SARADC_DATA);
+	info->last_val &= SARADC_DATA_MASK;
+
+	/* Clear irq & power down adc */
+	writel_relaxed(0, info->regs + SARADC_CTRL);
+
+	complete(&info->completion);
+
+	return IRQ_HANDLED;
+}
+
+static const struct iio_info rockchip_saradc_iio_info = {
+	.read_raw = rockchip_saradc_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+#define ADC_CHANNEL(_index, _id) {				\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.channel = _index,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE),	\
+	.datasheet_name = _id,					\
+}
+
+static const struct iio_chan_spec rockchip_saradc_iio_channels[] = {
+	ADC_CHANNEL(0, "adc0"),
+	ADC_CHANNEL(1, "adc1"),
+	ADC_CHANNEL(2, "adc2"),
+};
+
+static int rockchip_saradc_probe(struct platform_device *pdev)
+{
+	struct rockchip_saradc *info = NULL;
+	struct device_node *np = pdev->dev.of_node;
+	struct iio_dev *indio_dev = NULL;
+	struct resource	*mem;
+	int ret;
+	int irq;
+
+	if (!np)
+		return -ENODEV;
+
+	indio_dev = devm_iio_device_alloc(&pdev->dev, sizeof(*info));
+	if (!indio_dev) {
+		dev_err(&pdev->dev, "failed allocating iio device\n");
+		return -ENOMEM;
+	}
+	info = iio_priv(indio_dev);
+
+	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	info->regs = devm_ioremap_resource(&pdev->dev, mem);
+	if (IS_ERR(info->regs))
+		return PTR_ERR(info->regs);
+
+	init_completion(&info->completion);
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "no irq resource?\n");
+		return irq;
+	}
+
+	ret = devm_request_irq(&pdev->dev, irq, rockchip_saradc_isr,
+			       0, dev_name(&pdev->dev), info);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed requesting irq %d\n", irq);
+		return ret;
+	}
+
+	info->pclk = devm_clk_get(&pdev->dev, "apb_pclk");
+	if (IS_ERR(info->pclk)) {
+		dev_err(&pdev->dev, "failed to get pclk\n");
+		return PTR_ERR(info->pclk);
+	}
+
+	info->clk = devm_clk_get(&pdev->dev, "saradc");
+	if (IS_ERR(info->clk)) {
+		dev_err(&pdev->dev, "failed to get adc clock\n");
+		return PTR_ERR(info->clk);
+	}
+
+	info->vref = devm_regulator_get(&pdev->dev, "vref");
+	if (IS_ERR(info->vref)) {
+		dev_err(&pdev->dev, "failed to get regulator, %ld\n",
+			PTR_ERR(info->vref));
+		return PTR_ERR(info->vref);
+	}
+
+	/*
+	 * Use a default of 1MHz for the converter clock.
+	 * This may become user-configurable in the future.
+	 */
+	ret = clk_set_rate(info->clk, 1000000);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to set adc clk rate, %d\n", ret);
+		return ret;
+	}
+
+	ret = regulator_enable(info->vref);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to enable vref regulator\n");
+		return ret;
+	}
+
+	ret = clk_prepare_enable(info->pclk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to enable pclk\n");
+		goto err_reg_voltage;
+	}
+
+	ret = clk_prepare_enable(info->clk);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to enable converter clock\n");
+		goto err_pclk;
+	}
+
+	platform_set_drvdata(pdev, indio_dev);
+
+	indio_dev->name = dev_name(&pdev->dev);
+	indio_dev->dev.parent = &pdev->dev;
+	indio_dev->dev.of_node = pdev->dev.of_node;
+	indio_dev->info = &rockchip_saradc_iio_info;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	indio_dev->channels = rockchip_saradc_iio_channels;
+	indio_dev->num_channels = ARRAY_SIZE(rockchip_saradc_iio_channels);
+
+	ret = iio_device_register(indio_dev);
+	if (ret)
+		goto err_clk;
+
+	return 0;
+
+err_clk:
+	clk_disable_unprepare(info->clk);
+err_pclk:
+	clk_disable_unprepare(info->pclk);
+err_reg_voltage:
+	regulator_disable(info->vref);
+	return ret;
+}
+
+static int rockchip_saradc_remove(struct platform_device *pdev)
+{
+	struct iio_dev *indio_dev = platform_get_drvdata(pdev);
+	struct rockchip_saradc *info = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	clk_disable_unprepare(info->clk);
+	clk_disable_unprepare(info->pclk);
+	regulator_disable(info->vref);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int rockchip_saradc_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct rockchip_saradc *info = iio_priv(indio_dev);
+
+	clk_disable_unprepare(info->clk);
+	clk_disable_unprepare(info->pclk);
+	regulator_disable(info->vref);
+
+	return 0;
+}
+
+static int rockchip_saradc_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = dev_get_drvdata(dev);
+	struct rockchip_saradc *info = iio_priv(indio_dev);
+	int ret;
+
+	ret = regulator_enable(info->vref);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(info->pclk);
+	if (ret)
+		return ret;
+
+	ret = clk_prepare_enable(info->clk);
+	if (ret)
+		return ret;
+
+	return ret;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(rockchip_saradc_pm_ops,
+			 rockchip_saradc_suspend, rockchip_saradc_resume);
+
+static const struct of_device_id rockchip_saradc_match[] = {
+	{ .compatible = "rockchip,saradc" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, rockchip_saradc_match);
+
+static struct platform_driver rockchip_saradc_driver = {
+	.probe		= rockchip_saradc_probe,
+	.remove		= rockchip_saradc_remove,
+	.driver		= {
+		.name	= "rockchip-saradc",
+		.owner	= THIS_MODULE,
+		.of_match_table = rockchip_saradc_match,
+		.pm	= &rockchip_saradc_pm_ops,
+	},
+};
+
+module_platform_driver(rockchip_saradc_driver);
diff --git a/drivers/iio/adc/ti-adc128s052.c b/drivers/iio/adc/ti-adc128s052.c
new file mode 100644
index 000000000000..655cb564ec54
--- /dev/null
+++ b/drivers/iio/adc/ti-adc128s052.c
@@ -0,0 +1,179 @@
+/*
+ * Copyright (C) 2014 Angelo Compagnucci <angelo.compagnucci@gmail.com>
+ *
+ * Driver for Texas Instruments' ADC128S052 ADC chip.
+ * Datasheet can be found here:
+ * http://www.ti.com/lit/ds/symlink/adc128s052.pdf
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/err.h>
+#include <linux/spi/spi.h>
+#include <linux/module.h>
+#include <linux/iio/iio.h>
+#include <linux/regulator/consumer.h>
+
+struct adc128 {
+	struct spi_device *spi;
+
+	struct regulator *reg;
+	struct mutex lock;
+
+	u8 buffer[2] ____cacheline_aligned;
+};
+
+static int adc128_adc_conversion(struct adc128 *adc, u8 channel)
+{
+	int ret;
+
+	mutex_lock(&adc->lock);
+
+	adc->buffer[0] = channel << 3;
+	adc->buffer[1] = 0;
+
+	ret = spi_write(adc->spi, &adc->buffer, 2);
+	if (ret < 0) {
+		mutex_unlock(&adc->lock);
+		return ret;
+	}
+
+	ret = spi_read(adc->spi, &adc->buffer, 2);
+
+	mutex_unlock(&adc->lock);
+
+	if (ret < 0)
+		return ret;
+
+	return ((adc->buffer[0] << 8 | adc->buffer[1]) & 0xFFF);
+}
+
+static int adc128_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *channel, int *val,
+			   int *val2, long mask)
+{
+	struct adc128 *adc = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+
+		ret = adc128_adc_conversion(adc, channel->channel);
+		if (ret < 0)
+			return ret;
+
+		*val = ret;
+		return IIO_VAL_INT;
+
+	case IIO_CHAN_INFO_SCALE:
+
+		ret = regulator_get_voltage(adc->reg);
+		if (ret < 0)
+			return ret;
+
+		*val = ret / 1000;
+		*val2 = 12;
+		return IIO_VAL_FRACTIONAL_LOG2;
+
+	default:
+		return -EINVAL;
+	}
+
+}
+
+#define ADC128_VOLTAGE_CHANNEL(num)	\
+	{ \
+		.type = IIO_VOLTAGE, \
+		.indexed = 1, \
+		.channel = (num), \
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW), \
+		.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) \
+	}
+
+static const struct iio_chan_spec adc128_channels[] = {
+	ADC128_VOLTAGE_CHANNEL(0),
+	ADC128_VOLTAGE_CHANNEL(1),
+	ADC128_VOLTAGE_CHANNEL(2),
+	ADC128_VOLTAGE_CHANNEL(3),
+	ADC128_VOLTAGE_CHANNEL(4),
+	ADC128_VOLTAGE_CHANNEL(5),
+	ADC128_VOLTAGE_CHANNEL(6),
+	ADC128_VOLTAGE_CHANNEL(7),
+};
+
+static const struct iio_info adc128_info = {
+	.read_raw = adc128_read_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int adc128_probe(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev;
+	struct adc128 *adc;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&spi->dev, sizeof(*adc));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	adc = iio_priv(indio_dev);
+	adc->spi = spi;
+
+	spi_set_drvdata(spi, indio_dev);
+
+	indio_dev->dev.parent = &spi->dev;
+	indio_dev->name = spi_get_device_id(spi)->name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &adc128_info;
+
+	indio_dev->channels = adc128_channels;
+	indio_dev->num_channels = ARRAY_SIZE(adc128_channels);
+
+	adc->reg = devm_regulator_get(&spi->dev, "vref");
+	if (IS_ERR(adc->reg))
+		return PTR_ERR(adc->reg);
+
+	ret = regulator_enable(adc->reg);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&adc->lock);
+
+	ret = iio_device_register(indio_dev);
+
+	return ret;
+}
+
+static int adc128_remove(struct spi_device *spi)
+{
+	struct iio_dev *indio_dev = spi_get_drvdata(spi);
+	struct adc128 *adc = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	regulator_disable(adc->reg);
+
+	return 0;
+}
+
+static const struct spi_device_id adc128_id[] = {
+	{ "adc128s052", 0},
+	{ }
+};
+MODULE_DEVICE_TABLE(spi, adc128_id);
+
+static struct spi_driver adc128_driver = {
+	.driver = {
+		.name = "adc128s052",
+		.owner = THIS_MODULE,
+	},
+	.probe = adc128_probe,
+	.remove = adc128_remove,
+	.id_table = adc128_id,
+};
+module_spi_driver(adc128_driver);
+
+MODULE_AUTHOR("Angelo Compagnucci <angelo.compagnucci@gmail.com>");
+MODULE_DESCRIPTION("Texas Instruments ADC128S052");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/adc/ti_am335x_adc.c b/drivers/iio/adc/ti_am335x_adc.c
index d5dc4c6ce86c..b730864731e8 100644
--- a/drivers/iio/adc/ti_am335x_adc.c
+++ b/drivers/iio/adc/ti_am335x_adc.c
@@ -545,7 +545,6 @@ MODULE_DEVICE_TABLE(of, ti_adc_dt_ids);
 static struct platform_driver tiadc_driver = {
 	.driver = {
 		.name   = "TI-am335x-adc",
-		.owner	= THIS_MODULE,
 		.pm	= TIADC_PM_OPS,
 		.of_match_table = ti_adc_dt_ids,
 	},
diff --git a/drivers/iio/adc/twl4030-madc.c b/drivers/iio/adc/twl4030-madc.c
index eb86786e698e..94c5f05b4bc1 100644
--- a/drivers/iio/adc/twl4030-madc.c
+++ b/drivers/iio/adc/twl4030-madc.c
@@ -883,7 +883,6 @@ static struct platform_driver twl4030_madc_driver = {
 	.remove = twl4030_madc_remove,
 	.driver = {
 		   .name = "twl4030_madc",
-		   .owner = THIS_MODULE,
 		   .of_match_table = of_match_ptr(twl_madc_of_match),
 	},
 };
diff --git a/drivers/iio/adc/twl6030-gpadc.c b/drivers/iio/adc/twl6030-gpadc.c
index 15282f148b3b..89d8aa1d2818 100644
--- a/drivers/iio/adc/twl6030-gpadc.c
+++ b/drivers/iio/adc/twl6030-gpadc.c
@@ -994,7 +994,6 @@ static struct platform_driver twl6030_gpadc_driver = {
 	.remove		= twl6030_gpadc_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.pm	= &twl6030_gpadc_pm_ops,
 		.of_match_table = of_twl6030_match_tbl,
 	},
diff --git a/drivers/iio/adc/vf610_adc.c b/drivers/iio/adc/vf610_adc.c
index 44799eb5930e..4a10ae97dbf2 100644
--- a/drivers/iio/adc/vf610_adc.c
+++ b/drivers/iio/adc/vf610_adc.c
@@ -698,7 +698,6 @@ static struct platform_driver vf610_adc_driver = {
 	.remove         = vf610_adc_remove,
 	.driver         = {
 		.name   = DRIVER_NAME,
-		.owner  = THIS_MODULE,
 		.of_match_table = vf610_adc_match,
 		.pm     = &vf610_adc_pm_ops,
 	},
diff --git a/drivers/iio/adc/viperboard_adc.c b/drivers/iio/adc/viperboard_adc.c
index 9acf6b6d705b..3be2e35721cc 100644
--- a/drivers/iio/adc/viperboard_adc.c
+++ b/drivers/iio/adc/viperboard_adc.c
@@ -145,7 +145,6 @@ static int vprbrd_adc_probe(struct platform_device *pdev)
 static struct platform_driver vprbrd_adc_driver = {
 	.driver = {
 		.name	= "viperboard-adc",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= vprbrd_adc_probe,
 };
diff --git a/drivers/iio/adc/xilinx-xadc-core.c b/drivers/iio/adc/xilinx-xadc-core.c
index fd2745c62943..a221f7329b79 100644
--- a/drivers/iio/adc/xilinx-xadc-core.c
+++ b/drivers/iio/adc/xilinx-xadc-core.c
@@ -1126,7 +1126,7 @@ static int xadc_parse_dt(struct iio_dev *indio_dev, struct device_node *np,
 				chan->address = XADC_REG_VPVN;
 			} else {
 				chan->scan_index = 15 + reg;
-				chan->scan_index = XADC_REG_VAUX(reg - 1);
+				chan->address = XADC_REG_VAUX(reg - 1);
 			}
 			num_channels++;
 			chan++;
@@ -1201,12 +1201,16 @@ static int xadc_probe(struct platform_device *pdev)
 			goto err_device_free;
 
 		xadc->convst_trigger = xadc_alloc_trigger(indio_dev, "convst");
-		if (IS_ERR(xadc->convst_trigger))
+		if (IS_ERR(xadc->convst_trigger)) {
+			ret = PTR_ERR(xadc->convst_trigger);
 			goto err_triggered_buffer_cleanup;
+		}
 		xadc->samplerate_trigger = xadc_alloc_trigger(indio_dev,
 			"samplerate");
-		if (IS_ERR(xadc->samplerate_trigger))
+		if (IS_ERR(xadc->samplerate_trigger)) {
+			ret = PTR_ERR(xadc->samplerate_trigger);
 			goto err_free_convst_trigger;
+		}
 	}
 
 	xadc->clk = devm_clk_get(&pdev->dev, NULL);
@@ -1322,7 +1326,6 @@ static struct platform_driver xadc_driver = {
 	.remove = xadc_remove,
 	.driver = {
 		.name = "xadc",
-		.owner = THIS_MODULE,
 		.of_match_table = xadc_of_match_table,
 	},
 };
diff --git a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
index a3109a6f4d86..92068cdbf8c7 100644
--- a/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
+++ b/drivers/iio/common/hid-sensors/hid-sensor-trigger.c
@@ -122,7 +122,8 @@ int hid_sensor_setup_trigger(struct iio_dev *indio_dev, const char *name,
 		dev_err(&indio_dev->dev, "Trigger Register Failed\n");
 		goto error_free_trig;
 	}
-	indio_dev->trig = attrb->trigger = trig;
+	attrb->trigger = trig;
+	indio_dev->trig = iio_trigger_get(trig);
 
 	return ret;
 
diff --git a/drivers/iio/common/st_sensors/st_sensors_core.c b/drivers/iio/common/st_sensors/st_sensors_core.c
index 8a4ec00a91a0..24cfe4e044f9 100644
--- a/drivers/iio/common/st_sensors/st_sensors_core.c
+++ b/drivers/iio/common/st_sensors/st_sensors_core.c
@@ -306,8 +306,11 @@ int st_sensors_init_sensor(struct iio_dev *indio_dev,
 	if (of_pdata)
 		pdata = of_pdata;
 
-	if (pdata)
+	if (pdata) {
 		err = st_sensors_set_drdy_int_pin(indio_dev, pdata);
+		if (err < 0)
+			return err;
+	}
 
 	err = st_sensors_set_enable(indio_dev, false);
 	if (err < 0)
diff --git a/drivers/iio/common/st_sensors/st_sensors_trigger.c b/drivers/iio/common/st_sensors/st_sensors_trigger.c
index 8fc3a97eb266..8d8ca6f1e16a 100644
--- a/drivers/iio/common/st_sensors/st_sensors_trigger.c
+++ b/drivers/iio/common/st_sensors/st_sensors_trigger.c
@@ -49,7 +49,7 @@ int st_sensors_allocate_trigger(struct iio_dev *indio_dev,
 		dev_err(&indio_dev->dev, "failed to register iio trigger.\n");
 		goto iio_trigger_register_error;
 	}
-	indio_dev->trig = sdata->trig;
+	indio_dev->trig = iio_trigger_get(sdata->trig);
 
 	return 0;
 
diff --git a/drivers/iio/dac/Kconfig b/drivers/iio/dac/Kconfig
index f278eff42a4c..2236ea22f98a 100644
--- a/drivers/iio/dac/Kconfig
+++ b/drivers/iio/dac/Kconfig
@@ -152,6 +152,14 @@ config MAX517
 	  This driver can also be built as a module.  If so, the module
 	  will be called max517.
 
+config MAX5821
+	tristate "Maxim MAX5821 DAC driver"
+	depends on I2C
+	depends on OF
+	help
+	  Say yes here to build support for Maxim MAX5821
+	  10 bits DAC.
+
 config MCP4725
 	tristate "MCP4725 DAC driver"
 	depends on I2C
diff --git a/drivers/iio/dac/Makefile b/drivers/iio/dac/Makefile
index 10107640bb46..52be7e1acf16 100644
--- a/drivers/iio/dac/Makefile
+++ b/drivers/iio/dac/Makefile
@@ -17,5 +17,6 @@ obj-$(CONFIG_AD5791) += ad5791.o
 obj-$(CONFIG_AD5686) += ad5686.o
 obj-$(CONFIG_AD7303) += ad7303.o
 obj-$(CONFIG_MAX517) += max517.o
+obj-$(CONFIG_MAX5821) += max5821.o
 obj-$(CONFIG_MCP4725) += mcp4725.o
 obj-$(CONFIG_MCP4922) += mcp4922.o
diff --git a/drivers/iio/dac/max5821.c b/drivers/iio/dac/max5821.c
new file mode 100644
index 000000000000..6e914495b346
--- /dev/null
+++ b/drivers/iio/dac/max5821.c
@@ -0,0 +1,405 @@
+ /*
+  * iio/dac/max5821.c
+  * Copyright (C) 2014 Philippe Reynes
+  *
+  * This program is free software; you can redistribute it and/or modify
+  * it under the terms of the GNU General Public License version 2 as
+  * published by the Free Software Foundation.
+  */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/iio/iio.h>
+#include <linux/regulator/consumer.h>
+
+#define MAX5821_MAX_DAC_CHANNELS		2
+
+/* command bytes */
+#define MAX5821_LOAD_DAC_A_IN_REG_B		0x00
+#define MAX5821_LOAD_DAC_B_IN_REG_A		0x10
+#define MAX5821_EXTENDED_COMMAND_MODE		0xf0
+#define MAX5821_READ_DAC_A_COMMAND		0xf1
+#define MAX5821_READ_DAC_B_COMMAND		0xf2
+
+#define MAX5821_EXTENDED_POWER_UP		0x00
+#define MAX5821_EXTENDED_POWER_DOWN_MODE0	0x01
+#define MAX5821_EXTENDED_POWER_DOWN_MODE1	0x02
+#define MAX5821_EXTENDED_POWER_DOWN_MODE2	0x03
+#define MAX5821_EXTENDED_DAC_A			0x04
+#define MAX5821_EXTENDED_DAC_B			0x08
+
+enum max5821_device_ids {
+	ID_MAX5821,
+};
+
+struct max5821_data {
+	struct i2c_client	*client;
+	struct regulator	*vref_reg;
+	unsigned short		vref_mv;
+	bool			powerdown[MAX5821_MAX_DAC_CHANNELS];
+	u8			powerdown_mode[MAX5821_MAX_DAC_CHANNELS];
+	struct mutex		lock;
+};
+
+static const char * const max5821_powerdown_modes[] = {
+	"three_state",
+	"1kohm_to_gnd",
+	"100kohm_to_gnd",
+};
+
+enum {
+	MAX5821_THREE_STATE,
+	MAX5821_1KOHM_TO_GND,
+	MAX5821_100KOHM_TO_GND
+};
+
+static int max5821_get_powerdown_mode(struct iio_dev *indio_dev,
+				      const struct iio_chan_spec *chan)
+{
+	struct max5821_data *st = iio_priv(indio_dev);
+
+	return st->powerdown_mode[chan->channel];
+}
+
+static int max5821_set_powerdown_mode(struct iio_dev *indio_dev,
+				      const struct iio_chan_spec *chan,
+				      unsigned int mode)
+{
+	struct max5821_data *st = iio_priv(indio_dev);
+
+	st->powerdown_mode[chan->channel] = mode;
+
+	return 0;
+}
+
+static const struct iio_enum max5821_powerdown_mode_enum = {
+	.items = max5821_powerdown_modes,
+	.num_items = ARRAY_SIZE(max5821_powerdown_modes),
+	.get = max5821_get_powerdown_mode,
+	.set = max5821_set_powerdown_mode,
+};
+
+static ssize_t max5821_read_dac_powerdown(struct iio_dev *indio_dev,
+					  uintptr_t private,
+					  const struct iio_chan_spec *chan,
+					  char *buf)
+{
+	struct max5821_data *st = iio_priv(indio_dev);
+
+	return sprintf(buf, "%d\n", st->powerdown[chan->channel]);
+}
+
+static int max5821_sync_powerdown_mode(struct max5821_data *data,
+				       const struct iio_chan_spec *chan)
+{
+	u8 outbuf[2];
+
+	outbuf[0] = MAX5821_EXTENDED_COMMAND_MODE;
+
+	if (chan->channel == 0)
+		outbuf[1] = MAX5821_EXTENDED_DAC_A;
+	else
+		outbuf[1] = MAX5821_EXTENDED_DAC_B;
+
+	if (data->powerdown[chan->channel])
+		outbuf[1] |= data->powerdown_mode[chan->channel] + 1;
+	else
+		outbuf[1] |= MAX5821_EXTENDED_POWER_UP;
+
+	return i2c_master_send(data->client, outbuf, 2);
+}
+
+static ssize_t max5821_write_dac_powerdown(struct iio_dev *indio_dev,
+					   uintptr_t private,
+					   const struct iio_chan_spec *chan,
+					   const char *buf, size_t len)
+{
+	struct max5821_data *data = iio_priv(indio_dev);
+	bool powerdown;
+	int ret;
+
+	ret = strtobool(buf, &powerdown);
+	if (ret)
+		return ret;
+
+	data->powerdown[chan->channel] = powerdown;
+
+	ret = max5821_sync_powerdown_mode(data, chan);
+	if (ret < 0)
+		return ret;
+
+	return len;
+}
+
+static const struct iio_chan_spec_ext_info max5821_ext_info[] = {
+	{
+		.name = "powerdown",
+		.read = max5821_read_dac_powerdown,
+		.write = max5821_write_dac_powerdown,
+		.shared = IIO_SEPARATE,
+	},
+	IIO_ENUM("powerdown_mode", IIO_SEPARATE, &max5821_powerdown_mode_enum),
+	IIO_ENUM_AVAILABLE("powerdown_mode", &max5821_powerdown_mode_enum),
+	{ },
+};
+
+#define MAX5821_CHANNEL(chan) {					\
+	.type = IIO_VOLTAGE,					\
+	.indexed = 1,						\
+	.output = 1,						\
+	.channel = (chan),					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),		\
+	.info_mask_shared_by_all = BIT(IIO_CHAN_INFO_SCALE),	\
+	.ext_info = max5821_ext_info,				\
+}
+
+static const struct iio_chan_spec max5821_channels[] = {
+	MAX5821_CHANNEL(0),
+	MAX5821_CHANNEL(1)
+};
+
+static const u8 max5821_read_dac_command[] = {
+	MAX5821_READ_DAC_A_COMMAND,
+	MAX5821_READ_DAC_B_COMMAND
+};
+
+static const u8 max5821_load_dac_command[] = {
+	MAX5821_LOAD_DAC_A_IN_REG_B,
+	MAX5821_LOAD_DAC_B_IN_REG_A
+};
+
+static int max5821_get_value(struct iio_dev *indio_dev,
+			     int *val, int channel)
+{
+	struct max5821_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+	u8 outbuf[1];
+	u8 inbuf[2];
+	int ret;
+
+	if ((channel != 0) && (channel != 1))
+		return -EINVAL;
+
+	outbuf[0] = max5821_read_dac_command[channel];
+
+	mutex_lock(&data->lock);
+
+	ret = i2c_master_send(client, outbuf, 1);
+	if (ret < 0) {
+		mutex_unlock(&data->lock);
+		return ret;
+	} else if (ret != 1) {
+		mutex_unlock(&data->lock);
+		return -EIO;
+	}
+
+	ret = i2c_master_recv(client, inbuf, 2);
+	if (ret < 0) {
+		mutex_unlock(&data->lock);
+		return ret;
+	} else if (ret != 2) {
+		mutex_unlock(&data->lock);
+		return -EIO;
+	}
+
+	mutex_unlock(&data->lock);
+
+	*val = ((inbuf[0] & 0x0f) << 6) | (inbuf[1] >> 2);
+
+	return IIO_VAL_INT;
+}
+
+static int max5821_set_value(struct iio_dev *indio_dev,
+			     int val, int channel)
+{
+	struct max5821_data *data = iio_priv(indio_dev);
+	struct i2c_client *client = data->client;
+	u8 outbuf[2];
+	int ret;
+
+	if ((val < 0) || (val > 1023))
+		return -EINVAL;
+
+	if ((channel != 0) && (channel != 1))
+		return -EINVAL;
+
+	outbuf[0] = max5821_load_dac_command[channel];
+	outbuf[0] |= val >> 6;
+	outbuf[1] = (val & 0x3f) << 2;
+
+	ret = i2c_master_send(client, outbuf, 2);
+	if (ret < 0)
+		return ret;
+	else if (ret != 2)
+		return -EIO;
+	else
+		return 0;
+}
+
+static int max5821_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	struct max5821_data *data = iio_priv(indio_dev);
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		return max5821_get_value(indio_dev, val, chan->channel);
+	case IIO_CHAN_INFO_SCALE:
+		*val = data->vref_mv;
+		*val2 = 10;
+		return IIO_VAL_FRACTIONAL_LOG2;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int max5821_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan,
+			     int val, int val2, long mask)
+{
+	if (val2 != 0)
+		return -EINVAL;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		return max5821_set_value(indio_dev, val, chan->channel);
+	default:
+		return -EINVAL;
+	}
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int max5821_suspend(struct device *dev)
+{
+	u8 outbuf[2] = { MAX5821_EXTENDED_COMMAND_MODE,
+			 MAX5821_EXTENDED_DAC_A |
+			 MAX5821_EXTENDED_DAC_B |
+			 MAX5821_EXTENDED_POWER_DOWN_MODE2 };
+
+	return i2c_master_send(to_i2c_client(dev), outbuf, 2);
+}
+
+static int max5821_resume(struct device *dev)
+{
+	u8 outbuf[2] = { MAX5821_EXTENDED_COMMAND_MODE,
+			 MAX5821_EXTENDED_DAC_A |
+			 MAX5821_EXTENDED_DAC_B |
+			 MAX5821_EXTENDED_POWER_UP };
+
+	return i2c_master_send(to_i2c_client(dev), outbuf, 2);
+}
+
+static SIMPLE_DEV_PM_OPS(max5821_pm_ops, max5821_suspend, max5821_resume);
+#define MAX5821_PM_OPS (&max5821_pm_ops)
+#else
+#define MAX5821_PM_OPS NULL
+#endif /* CONFIG_PM_SLEEP */
+
+static const struct iio_info max5821_info = {
+	.read_raw = max5821_read_raw,
+	.write_raw = max5821_write_raw,
+	.driver_module = THIS_MODULE,
+};
+
+static int max5821_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct max5821_data *data;
+	struct iio_dev *indio_dev;
+	u32 tmp;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+	mutex_init(&data->lock);
+
+	/* max5821 start in powerdown mode 100Kohm to ground */
+	for (tmp = 0; tmp < MAX5821_MAX_DAC_CHANNELS; tmp++) {
+		data->powerdown[tmp] = true;
+		data->powerdown_mode[tmp] = MAX5821_100KOHM_TO_GND;
+	}
+
+	data->vref_reg = devm_regulator_get(&client->dev, "vref");
+	if (IS_ERR(data->vref_reg)) {
+		ret = PTR_ERR(data->vref_reg);
+		dev_err(&client->dev,
+			"Failed to get vref regulator: %d\n", ret);
+		goto error_free_reg;
+	}
+
+	ret = regulator_enable(data->vref_reg);
+	if (ret) {
+		dev_err(&client->dev,
+			"Failed to enable vref regulator: %d\n", ret);
+		goto error_free_reg;
+	}
+
+	ret = regulator_get_voltage(data->vref_reg);
+	if (ret < 0) {
+		dev_err(&client->dev,
+			"Failed to get voltage on regulator: %d\n", ret);
+		goto error_disable_reg;
+	}
+
+	data->vref_mv = ret / 1000;
+
+	indio_dev->name = id->name;
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->num_channels = ARRAY_SIZE(max5821_channels);
+	indio_dev->channels = max5821_channels;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &max5821_info;
+
+	return iio_device_register(indio_dev);
+
+error_disable_reg:
+	regulator_disable(data->vref_reg);
+
+error_free_reg:
+
+	return ret;
+}
+
+static int max5821_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct max5821_data *data = iio_priv(indio_dev);
+
+	iio_device_unregister(indio_dev);
+	regulator_disable(data->vref_reg);
+
+	return 0;
+}
+
+static const struct i2c_device_id max5821_id[] = {
+	{ "max5821", ID_MAX5821 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, max5821_id);
+
+static const struct of_device_id max5821_of_match[] = {
+	{ .compatible = "maxim,max5821" },
+	{ }
+};
+
+static struct i2c_driver max5821_driver = {
+	.driver = {
+		.name	= "max5821",
+		.pm     = MAX5821_PM_OPS,
+		.owner	= THIS_MODULE,
+	},
+	.probe		= max5821_probe,
+	.remove		= max5821_remove,
+	.id_table	= max5821_id,
+};
+module_i2c_driver(max5821_driver);
+
+MODULE_AUTHOR("Philippe Reynes <tremyfr@yahoo.fr>");
+MODULE_DESCRIPTION("MAX5821 DAC");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/gyro/Kconfig b/drivers/iio/gyro/Kconfig
index ac2d69e34c8c..b3d0e94f72eb 100644
--- a/drivers/iio/gyro/Kconfig
+++ b/drivers/iio/gyro/Kconfig
@@ -50,6 +50,17 @@ config ADXRS450
 	  This driver can also be built as a module.  If so, the module
 	  will be called adxrs450.
 
+config BMG160
+	tristate "BOSCH BMG160 Gyro Sensor"
+	depends on I2C
+	select IIO_TRIGGERED_BUFFER if IIO_BUFFER
+	help
+	  Say yes here to build support for Bosch BMG160 Tri-axis Gyro Sensor
+	  driver. This driver also supports BMI055 gyroscope.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called bmg160.
+
 config HID_SENSOR_GYRO_3D
 	depends on HID_SENSOR_HUB
 	select IIO_BUFFER
diff --git a/drivers/iio/gyro/Makefile b/drivers/iio/gyro/Makefile
index 2f2752a4ea83..36a38776f739 100644
--- a/drivers/iio/gyro/Makefile
+++ b/drivers/iio/gyro/Makefile
@@ -8,6 +8,7 @@ obj-$(CONFIG_ADIS16130) += adis16130.o
 obj-$(CONFIG_ADIS16136) += adis16136.o
 obj-$(CONFIG_ADIS16260) += adis16260.o
 obj-$(CONFIG_ADXRS450) += adxrs450.o
+obj-$(CONFIG_BMG160) += bmg160.o
 
 obj-$(CONFIG_HID_SENSOR_GYRO_3D) += hid-sensor-gyro-3d.o
 
diff --git a/drivers/iio/gyro/bmg160.c b/drivers/iio/gyro/bmg160.c
new file mode 100644
index 000000000000..1f967e0d688e
--- /dev/null
+++ b/drivers/iio/gyro/bmg160.c
@@ -0,0 +1,1228 @@
+/*
+ * BMG160 Gyro Sensor driver
+ * Copyright (c) 2014, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/acpi.h>
+#include <linux/gpio/consumer.h>
+#include <linux/pm.h>
+#include <linux/pm_runtime.h>
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+#include <linux/iio/buffer.h>
+#include <linux/iio/trigger.h>
+#include <linux/iio/events.h>
+#include <linux/iio/trigger_consumer.h>
+#include <linux/iio/triggered_buffer.h>
+
+#define BMG160_DRV_NAME		"bmg160"
+#define BMG160_IRQ_NAME		"bmg160_event"
+#define BMG160_GPIO_NAME		"gpio_int"
+
+#define BMG160_REG_CHIP_ID		0x00
+#define BMG160_CHIP_ID_VAL		0x0F
+
+#define BMG160_REG_PMU_LPW		0x11
+#define BMG160_MODE_NORMAL		0x00
+#define BMG160_MODE_DEEP_SUSPEND	0x20
+#define BMG160_MODE_SUSPEND		0x80
+
+#define BMG160_REG_RANGE		0x0F
+
+#define BMG160_RANGE_2000DPS		0
+#define BMG160_RANGE_1000DPS		1
+#define BMG160_RANGE_500DPS		2
+#define BMG160_RANGE_250DPS		3
+#define BMG160_RANGE_125DPS		4
+
+#define BMG160_REG_PMU_BW		0x10
+#define BMG160_NO_FILTER		0
+#define BMG160_DEF_BW			100
+
+#define BMG160_REG_INT_MAP_0		0x17
+#define BMG160_INT_MAP_0_BIT_ANY	BIT(1)
+
+#define BMG160_REG_INT_MAP_1		0x18
+#define BMG160_INT_MAP_1_BIT_NEW_DATA	BIT(0)
+
+#define BMG160_REG_INT_RST_LATCH	0x21
+#define BMG160_INT_MODE_LATCH_RESET	0x80
+#define BMG160_INT_MODE_LATCH_INT	0x0F
+#define BMG160_INT_MODE_NON_LATCH_INT	0x00
+
+#define BMG160_REG_INT_EN_0		0x15
+#define BMG160_DATA_ENABLE_INT		BIT(7)
+
+#define BMG160_REG_XOUT_L		0x02
+#define BMG160_AXIS_TO_REG(axis)	(BMG160_REG_XOUT_L + (axis * 2))
+
+#define BMG160_REG_SLOPE_THRES		0x1B
+#define BMG160_SLOPE_THRES_MASK	0x0F
+
+#define BMG160_REG_MOTION_INTR		0x1C
+#define BMG160_INT_MOTION_X		BIT(0)
+#define BMG160_INT_MOTION_Y		BIT(1)
+#define BMG160_INT_MOTION_Z		BIT(2)
+#define BMG160_ANY_DUR_MASK		0x30
+#define BMG160_ANY_DUR_SHIFT		4
+
+#define BMG160_REG_INT_STATUS_2	0x0B
+#define BMG160_ANY_MOTION_MASK		0x07
+
+#define BMG160_REG_TEMP		0x08
+#define BMG160_TEMP_CENTER_VAL		23
+
+#define BMG160_MAX_STARTUP_TIME_MS	80
+
+#define BMG160_AUTO_SUSPEND_DELAY_MS	2000
+
+struct bmg160_data {
+	struct i2c_client *client;
+	struct iio_trigger *dready_trig;
+	struct iio_trigger *motion_trig;
+	struct mutex mutex;
+	s16 buffer[8];
+	u8 bw_bits;
+	u32 dps_range;
+	int ev_enable_state;
+	int slope_thres;
+	bool dready_trigger_on;
+	bool motion_trigger_on;
+	int64_t timestamp;
+};
+
+enum bmg160_axis {
+	AXIS_X,
+	AXIS_Y,
+	AXIS_Z,
+};
+
+static const struct {
+	int val;
+	int bw_bits;
+} bmg160_samp_freq_table[] = { {100, 0x07},
+			       {200, 0x06},
+			       {400, 0x03},
+			       {1000, 0x02},
+			       {2000, 0x01} };
+
+static const struct {
+	int scale;
+	int dps_range;
+} bmg160_scale_table[] = { { 1065, BMG160_RANGE_2000DPS},
+			   { 532, BMG160_RANGE_1000DPS},
+			   { 266, BMG160_RANGE_500DPS},
+			   { 133, BMG160_RANGE_250DPS},
+			   { 66, BMG160_RANGE_125DPS} };
+
+static int bmg160_set_mode(struct bmg160_data *data, u8 mode)
+{
+	int ret;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMG160_REG_PMU_LPW, mode);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_pmu_lpw\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmg160_convert_freq_to_bit(int val)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bmg160_samp_freq_table); ++i) {
+		if (bmg160_samp_freq_table[i].val == val)
+			return bmg160_samp_freq_table[i].bw_bits;
+	}
+
+	return -EINVAL;
+}
+
+static int bmg160_set_bw(struct bmg160_data *data, int val)
+{
+	int ret;
+	int bw_bits;
+
+	bw_bits = bmg160_convert_freq_to_bit(val);
+	if (bw_bits < 0)
+		return bw_bits;
+
+	ret = i2c_smbus_write_byte_data(data->client, BMG160_REG_PMU_BW,
+					bw_bits);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_pmu_bw\n");
+		return ret;
+	}
+
+	data->bw_bits = bw_bits;
+
+	return 0;
+}
+
+static int bmg160_chip_init(struct bmg160_data *data)
+{
+	int ret;
+
+	ret = i2c_smbus_read_byte_data(data->client, BMG160_REG_CHIP_ID);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_chip_id\n");
+		return ret;
+	}
+
+	dev_dbg(&data->client->dev, "Chip Id %x\n", ret);
+	if (ret != BMG160_CHIP_ID_VAL) {
+		dev_err(&data->client->dev, "invalid chip %x\n", ret);
+		return -ENODEV;
+	}
+
+	ret = bmg160_set_mode(data, BMG160_MODE_NORMAL);
+	if (ret < 0)
+		return ret;
+
+	/* Wait upto 500 ms to be ready after changing mode */
+	usleep_range(500, 1000);
+
+	/* Set Bandwidth */
+	ret = bmg160_set_bw(data, BMG160_DEF_BW);
+	if (ret < 0)
+		return ret;
+
+	/* Set Default Range */
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMG160_REG_RANGE,
+					BMG160_RANGE_500DPS);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_range\n");
+		return ret;
+	}
+	data->dps_range = BMG160_RANGE_500DPS;
+
+	ret = i2c_smbus_read_byte_data(data->client, BMG160_REG_SLOPE_THRES);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_slope_thres\n");
+		return ret;
+	}
+	data->slope_thres = ret;
+
+	/* Set default interrupt mode */
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMG160_REG_INT_RST_LATCH,
+					BMG160_INT_MODE_LATCH_INT |
+					BMG160_INT_MODE_LATCH_RESET);
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Error writing reg_motion_intr\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmg160_set_power_state(struct bmg160_data *data, bool on)
+{
+#ifdef CONFIG_PM_RUNTIME
+	int ret;
+
+	if (on)
+		ret = pm_runtime_get_sync(&data->client->dev);
+	else {
+		pm_runtime_mark_last_busy(&data->client->dev);
+		ret = pm_runtime_put_autosuspend(&data->client->dev);
+	}
+
+	if (ret < 0) {
+		dev_err(&data->client->dev,
+			"Failed: bmg160_set_power_state for %d\n", on);
+		return ret;
+	}
+#endif
+
+	return 0;
+}
+
+static int bmg160_setup_any_motion_interrupt(struct bmg160_data *data,
+					     bool status)
+{
+	int ret;
+
+	/* Enable/Disable INT_MAP0 mapping */
+	ret = i2c_smbus_read_byte_data(data->client,  BMG160_REG_INT_MAP_0);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_map0\n");
+		return ret;
+	}
+	if (status)
+		ret |= BMG160_INT_MAP_0_BIT_ANY;
+	else
+		ret &= ~BMG160_INT_MAP_0_BIT_ANY;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMG160_REG_INT_MAP_0,
+					ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_map0\n");
+		return ret;
+	}
+
+	/* Enable/Disable slope interrupts */
+	if (status) {
+		/* Update slope thres */
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_SLOPE_THRES,
+						data->slope_thres);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error writing reg_slope_thres\n");
+			return ret;
+		}
+
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_MOTION_INTR,
+						BMG160_INT_MOTION_X |
+						BMG160_INT_MOTION_Y |
+						BMG160_INT_MOTION_Z);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error writing reg_motion_intr\n");
+			return ret;
+		}
+
+		/*
+		 * New data interrupt is always non-latched,
+		 * which will have higher priority, so no need
+		 * to set latched mode, we will be flooded anyway with INTR
+		 */
+		if (!data->dready_trigger_on) {
+			ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_INT_RST_LATCH,
+						BMG160_INT_MODE_LATCH_INT |
+						BMG160_INT_MODE_LATCH_RESET);
+			if (ret < 0) {
+				dev_err(&data->client->dev,
+					"Error writing reg_rst_latch\n");
+				return ret;
+			}
+		}
+
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_INT_EN_0,
+						BMG160_DATA_ENABLE_INT);
+
+	} else
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_INT_EN_0,
+						0);
+
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_en0\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmg160_setup_new_data_interrupt(struct bmg160_data *data,
+					   bool status)
+{
+	int ret;
+
+	/* Enable/Disable INT_MAP1 mapping */
+	ret = i2c_smbus_read_byte_data(data->client, BMG160_REG_INT_MAP_1);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_map1\n");
+		return ret;
+	}
+
+	if (status)
+		ret |= BMG160_INT_MAP_1_BIT_NEW_DATA;
+	else
+		ret &= ~BMG160_INT_MAP_1_BIT_NEW_DATA;
+
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMG160_REG_INT_MAP_1,
+					ret);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_map1\n");
+		return ret;
+	}
+
+	if (status) {
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_INT_RST_LATCH,
+						BMG160_INT_MODE_NON_LATCH_INT |
+						BMG160_INT_MODE_LATCH_RESET);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error writing reg_rst_latch\n");
+				return ret;
+		}
+
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_INT_EN_0,
+						BMG160_DATA_ENABLE_INT);
+
+	} else {
+		/* Restore interrupt mode */
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_INT_RST_LATCH,
+						BMG160_INT_MODE_LATCH_INT |
+						BMG160_INT_MODE_LATCH_RESET);
+		if (ret < 0) {
+			dev_err(&data->client->dev,
+				"Error writing reg_rst_latch\n");
+				return ret;
+		}
+
+		ret = i2c_smbus_write_byte_data(data->client,
+						BMG160_REG_INT_EN_0,
+						0);
+	}
+
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_int_en0\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmg160_get_bw(struct bmg160_data *data, int *val)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(bmg160_samp_freq_table); ++i) {
+		if (bmg160_samp_freq_table[i].bw_bits == data->bw_bits) {
+			*val = bmg160_samp_freq_table[i].val;
+			return IIO_VAL_INT;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int bmg160_set_scale(struct bmg160_data *data, int val)
+{
+	int ret, i;
+
+	for (i = 0; i < ARRAY_SIZE(bmg160_scale_table); ++i) {
+		if (bmg160_scale_table[i].scale == val) {
+			ret = i2c_smbus_write_byte_data(
+					data->client,
+					BMG160_REG_RANGE,
+					bmg160_scale_table[i].dps_range);
+			if (ret < 0) {
+				dev_err(&data->client->dev,
+					"Error writing reg_range\n");
+				return ret;
+			}
+			data->dps_range = bmg160_scale_table[i].dps_range;
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+static int bmg160_get_temp(struct bmg160_data *data, int *val)
+{
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = bmg160_set_power_state(data, true);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	ret = i2c_smbus_read_byte_data(data->client, BMG160_REG_TEMP);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_temp\n");
+		bmg160_set_power_state(data, false);
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	*val = sign_extend32(ret, 7);
+	ret = bmg160_set_power_state(data, false);
+	mutex_unlock(&data->mutex);
+	if (ret < 0)
+		return ret;
+
+	return IIO_VAL_INT;
+}
+
+static int bmg160_get_axis(struct bmg160_data *data, int axis, int *val)
+{
+	int ret;
+
+	mutex_lock(&data->mutex);
+	ret = bmg160_set_power_state(data, true);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	ret = i2c_smbus_read_word_data(data->client, BMG160_AXIS_TO_REG(axis));
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading axis %d\n", axis);
+		bmg160_set_power_state(data, false);
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	*val = sign_extend32(ret, 15);
+	ret = bmg160_set_power_state(data, false);
+	mutex_unlock(&data->mutex);
+	if (ret < 0)
+		return ret;
+
+	return IIO_VAL_INT;
+}
+
+static int bmg160_read_raw(struct iio_dev *indio_dev,
+			   struct iio_chan_spec const *chan,
+			   int *val, int *val2, long mask)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		switch (chan->type) {
+		case IIO_TEMP:
+			return bmg160_get_temp(data, val);
+		case IIO_ANGL_VEL:
+			if (iio_buffer_enabled(indio_dev))
+				return -EBUSY;
+			else
+				return bmg160_get_axis(data, chan->scan_index,
+						       val);
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_OFFSET:
+		if (chan->type == IIO_TEMP) {
+			*val = BMG160_TEMP_CENTER_VAL;
+			return IIO_VAL_INT;
+		} else
+			return -EINVAL;
+	case IIO_CHAN_INFO_SCALE:
+		*val = 0;
+		switch (chan->type) {
+		case IIO_TEMP:
+			*val2 = 500000;
+			return IIO_VAL_INT_PLUS_MICRO;
+		case IIO_ANGL_VEL:
+		{
+			int i;
+
+			for (i = 0; i < ARRAY_SIZE(bmg160_scale_table); ++i) {
+				if (bmg160_scale_table[i].dps_range ==
+							data->dps_range) {
+					*val2 = bmg160_scale_table[i].scale;
+					return IIO_VAL_INT_PLUS_MICRO;
+				}
+			}
+			return -EINVAL;
+		}
+		default:
+			return -EINVAL;
+		}
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		*val2 = 0;
+		mutex_lock(&data->mutex);
+		ret = bmg160_get_bw(data, val);
+		mutex_unlock(&data->mutex);
+		return ret;
+	default:
+		return -EINVAL;
+	}
+}
+
+static int bmg160_write_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan,
+			    int val, int val2, long mask)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SAMP_FREQ:
+		mutex_lock(&data->mutex);
+		/*
+		 * Section 4.2 of spec
+		 * In suspend mode, the only supported operations are reading
+		 * registers as well as writing to the (0x14) softreset
+		 * register. Since we will be in suspend mode by default, change
+		 * mode to power on for other writes.
+		 */
+		ret = bmg160_set_power_state(data, true);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+		ret = bmg160_set_bw(data, val);
+		if (ret < 0) {
+			bmg160_set_power_state(data, false);
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+		ret = bmg160_set_power_state(data, false);
+		mutex_unlock(&data->mutex);
+		return ret;
+	case IIO_CHAN_INFO_SCALE:
+		if (val)
+			return -EINVAL;
+
+		mutex_lock(&data->mutex);
+		/* Refer to comments above for the suspend mode ops */
+		ret = bmg160_set_power_state(data, true);
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+		ret = bmg160_set_scale(data, val2);
+		if (ret < 0) {
+			bmg160_set_power_state(data, false);
+			mutex_unlock(&data->mutex);
+			return ret;
+		}
+		ret = bmg160_set_power_state(data, false);
+		mutex_unlock(&data->mutex);
+		return ret;
+	default:
+		return -EINVAL;
+	}
+
+	return -EINVAL;
+}
+
+static int bmg160_read_event(struct iio_dev *indio_dev,
+			     const struct iio_chan_spec *chan,
+			     enum iio_event_type type,
+			     enum iio_event_direction dir,
+			     enum iio_event_info info,
+			     int *val, int *val2)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	*val2 = 0;
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		*val = data->slope_thres & BMG160_SLOPE_THRES_MASK;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return IIO_VAL_INT;
+}
+
+static int bmg160_write_event(struct iio_dev *indio_dev,
+			      const struct iio_chan_spec *chan,
+			      enum iio_event_type type,
+			      enum iio_event_direction dir,
+			      enum iio_event_info info,
+			      int val, int val2)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	switch (info) {
+	case IIO_EV_INFO_VALUE:
+		if (data->ev_enable_state)
+			return -EBUSY;
+		data->slope_thres &= ~BMG160_SLOPE_THRES_MASK;
+		data->slope_thres |= (val & BMG160_SLOPE_THRES_MASK);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int bmg160_read_event_config(struct iio_dev *indio_dev,
+				    const struct iio_chan_spec *chan,
+				    enum iio_event_type type,
+				    enum iio_event_direction dir)
+{
+
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	return data->ev_enable_state;
+}
+
+static int bmg160_write_event_config(struct iio_dev *indio_dev,
+				     const struct iio_chan_spec *chan,
+				     enum iio_event_type type,
+				     enum iio_event_direction dir,
+				     int state)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int ret;
+
+	if (state && data->ev_enable_state)
+		return 0;
+
+	mutex_lock(&data->mutex);
+
+	if (!state && data->motion_trigger_on) {
+		data->ev_enable_state = 0;
+		mutex_unlock(&data->mutex);
+		return 0;
+	}
+	/*
+	 * We will expect the enable and disable to do operation in
+	 * in reverse order. This will happen here anyway as our
+	 * resume operation uses sync mode runtime pm calls, the
+	 * suspend operation will be delayed by autosuspend delay
+	 * So the disable operation will still happen in reverse of
+	 * enable operation. When runtime pm is disabled the mode
+	 * is always on so sequence doesn't matter
+	 */
+	ret = bmg160_set_power_state(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	ret =  bmg160_setup_any_motion_interrupt(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+
+	data->ev_enable_state = state;
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+static int bmg160_validate_trigger(struct iio_dev *indio_dev,
+				   struct iio_trigger *trig)
+{
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	if (data->dready_trig != trig && data->motion_trig != trig)
+		return -EINVAL;
+
+	return 0;
+}
+
+static IIO_CONST_ATTR_SAMP_FREQ_AVAIL("100 200 400 1000 2000");
+
+static IIO_CONST_ATTR(in_anglvel_scale_available,
+		      "0.001065 0.000532 0.000266 0.000133 0.000066");
+
+static struct attribute *bmg160_attributes[] = {
+	&iio_const_attr_sampling_frequency_available.dev_attr.attr,
+	&iio_const_attr_in_anglvel_scale_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group bmg160_attrs_group = {
+	.attrs = bmg160_attributes,
+};
+
+static const struct iio_event_spec bmg160_event = {
+		.type = IIO_EV_TYPE_ROC,
+		.dir = IIO_EV_DIR_RISING | IIO_EV_DIR_FALLING,
+		.mask_shared_by_type = BIT(IIO_EV_INFO_VALUE) |
+				       BIT(IIO_EV_INFO_ENABLE)
+};
+
+#define BMG160_CHANNEL(_axis) {					\
+	.type = IIO_ANGL_VEL,						\
+	.modified = 1,							\
+	.channel2 = IIO_MOD_##_axis,					\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+	.info_mask_shared_by_type = BIT(IIO_CHAN_INFO_SCALE) |		\
+				    BIT(IIO_CHAN_INFO_SAMP_FREQ),	\
+	.scan_index = AXIS_##_axis,					\
+	.scan_type = {							\
+		.sign = 's',						\
+		.realbits = 16,					\
+		.storagebits = 16,					\
+	},								\
+	.event_spec = &bmg160_event,					\
+	.num_event_specs = 1						\
+}
+
+static const struct iio_chan_spec bmg160_channels[] = {
+	{
+		.type = IIO_TEMP,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				      BIT(IIO_CHAN_INFO_SCALE) |
+				      BIT(IIO_CHAN_INFO_OFFSET),
+		.scan_index = -1,
+	},
+	BMG160_CHANNEL(X),
+	BMG160_CHANNEL(Y),
+	BMG160_CHANNEL(Z),
+	IIO_CHAN_SOFT_TIMESTAMP(3),
+};
+
+static const struct iio_info bmg160_info = {
+	.attrs			= &bmg160_attrs_group,
+	.read_raw		= bmg160_read_raw,
+	.write_raw		= bmg160_write_raw,
+	.read_event_value	= bmg160_read_event,
+	.write_event_value	= bmg160_write_event,
+	.write_event_config	= bmg160_write_event_config,
+	.read_event_config	= bmg160_read_event_config,
+	.validate_trigger	= bmg160_validate_trigger,
+	.driver_module		= THIS_MODULE,
+};
+
+static irqreturn_t bmg160_trigger_handler(int irq, void *p)
+{
+	struct iio_poll_func *pf = p;
+	struct iio_dev *indio_dev = pf->indio_dev;
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int bit, ret, i = 0;
+
+	mutex_lock(&data->mutex);
+	for_each_set_bit(bit, indio_dev->buffer->scan_mask,
+			 indio_dev->masklength) {
+		ret = i2c_smbus_read_word_data(data->client,
+					       BMG160_AXIS_TO_REG(bit));
+		if (ret < 0) {
+			mutex_unlock(&data->mutex);
+			goto err;
+		}
+		data->buffer[i++] = ret;
+	}
+	mutex_unlock(&data->mutex);
+
+	iio_push_to_buffers_with_timestamp(indio_dev, data->buffer,
+					   data->timestamp);
+err:
+	iio_trigger_notify_done(indio_dev->trig);
+
+	return IRQ_HANDLED;
+}
+
+static int bmg160_trig_try_reen(struct iio_trigger *trig)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int ret;
+
+	/* new data interrupts don't need ack */
+	if (data->dready_trigger_on)
+		return 0;
+
+	/* Set latched mode interrupt and clear any latched interrupt */
+	ret = i2c_smbus_write_byte_data(data->client,
+					BMG160_REG_INT_RST_LATCH,
+					BMG160_INT_MODE_LATCH_INT |
+					BMG160_INT_MODE_LATCH_RESET);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error writing reg_rst_latch\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int bmg160_data_rdy_trigger_set_state(struct iio_trigger *trig,
+					     bool state)
+{
+	struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int ret;
+
+	mutex_lock(&data->mutex);
+
+	if (!state && data->ev_enable_state && data->motion_trigger_on) {
+		data->motion_trigger_on = false;
+		mutex_unlock(&data->mutex);
+		return 0;
+	}
+
+	/*
+	 * Refer to comment in bmg160_write_event_config for
+	 * enable/disable operation order
+	 */
+	ret = bmg160_set_power_state(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	if (data->motion_trig == trig)
+		ret =  bmg160_setup_any_motion_interrupt(data, state);
+	else
+		ret = bmg160_setup_new_data_interrupt(data, state);
+	if (ret < 0) {
+		mutex_unlock(&data->mutex);
+		return ret;
+	}
+	if (data->motion_trig == trig)
+		data->motion_trigger_on = state;
+	else
+		data->dready_trigger_on = state;
+
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+static const struct iio_trigger_ops bmg160_trigger_ops = {
+	.set_trigger_state = bmg160_data_rdy_trigger_set_state,
+	.try_reenable = bmg160_trig_try_reen,
+	.owner = THIS_MODULE,
+};
+
+static irqreturn_t bmg160_event_handler(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int ret;
+	int dir;
+
+	ret = i2c_smbus_read_byte_data(data->client, BMG160_REG_INT_STATUS_2);
+	if (ret < 0) {
+		dev_err(&data->client->dev, "Error reading reg_int_status2\n");
+		goto ack_intr_status;
+	}
+
+	if (ret & 0x08)
+		dir = IIO_EV_DIR_RISING;
+	else
+		dir = IIO_EV_DIR_FALLING;
+
+	if (ret & BMG160_ANY_MOTION_MASK)
+		iio_push_event(indio_dev, IIO_MOD_EVENT_CODE(IIO_ANGL_VEL,
+							0,
+							IIO_MOD_X_OR_Y_OR_Z,
+							IIO_EV_TYPE_ROC,
+							dir),
+							data->timestamp);
+
+ack_intr_status:
+	if (!data->dready_trigger_on) {
+		ret = i2c_smbus_write_byte_data(data->client,
+					BMG160_REG_INT_RST_LATCH,
+					BMG160_INT_MODE_LATCH_INT |
+					BMG160_INT_MODE_LATCH_RESET);
+		if (ret < 0)
+			dev_err(&data->client->dev,
+				"Error writing reg_rst_latch\n");
+	}
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t bmg160_data_rdy_trig_poll(int irq, void *private)
+{
+	struct iio_dev *indio_dev = private;
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	data->timestamp = iio_get_time_ns();
+
+	if (data->dready_trigger_on)
+		iio_trigger_poll(data->dready_trig);
+	else if (data->motion_trigger_on)
+		iio_trigger_poll(data->motion_trig);
+
+	if (data->ev_enable_state)
+		return IRQ_WAKE_THREAD;
+	else
+		return IRQ_HANDLED;
+
+}
+
+static int bmg160_gpio_probe(struct i2c_client *client,
+			     struct bmg160_data *data)
+
+{
+	struct device *dev;
+	struct gpio_desc *gpio;
+	int ret;
+
+	if (!client)
+		return -EINVAL;
+
+	dev = &client->dev;
+
+	/* data ready gpio interrupt pin */
+	gpio = devm_gpiod_get_index(dev, BMG160_GPIO_NAME, 0);
+	if (IS_ERR(gpio)) {
+		dev_err(dev, "acpi gpio get index failed\n");
+		return PTR_ERR(gpio);
+	}
+
+	ret = gpiod_direction_input(gpio);
+	if (ret)
+		return ret;
+
+	ret = gpiod_to_irq(gpio);
+
+	dev_dbg(dev, "GPIO resource, no:%d irq:%d\n", desc_to_gpio(gpio), ret);
+
+	return ret;
+}
+
+static const char *bmg160_match_acpi_device(struct device *dev)
+{
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return NULL;
+
+	return dev_name(dev);
+}
+
+static int bmg160_probe(struct i2c_client *client,
+			const struct i2c_device_id *id)
+{
+	struct bmg160_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+	const char *name = NULL;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	ret = bmg160_chip_init(data);
+	if (ret < 0)
+		return ret;
+
+	mutex_init(&data->mutex);
+
+	if (id)
+		name = id->name;
+
+	if (ACPI_HANDLE(&client->dev))
+		name = bmg160_match_acpi_device(&client->dev);
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->channels = bmg160_channels;
+	indio_dev->num_channels = ARRAY_SIZE(bmg160_channels);
+	indio_dev->name = name;
+	indio_dev->modes = INDIO_DIRECT_MODE;
+	indio_dev->info = &bmg160_info;
+
+	if (client->irq <= 0)
+		client->irq = bmg160_gpio_probe(client, data);
+
+	if (client->irq > 0) {
+		ret = devm_request_threaded_irq(&client->dev,
+						client->irq,
+						bmg160_data_rdy_trig_poll,
+						bmg160_event_handler,
+						IRQF_TRIGGER_RISING,
+						BMG160_IRQ_NAME,
+						indio_dev);
+		if (ret)
+			return ret;
+
+		data->dready_trig = devm_iio_trigger_alloc(&client->dev,
+							   "%s-dev%d",
+							   indio_dev->name,
+							   indio_dev->id);
+		if (!data->dready_trig)
+			return -ENOMEM;
+
+		data->motion_trig = devm_iio_trigger_alloc(&client->dev,
+							  "%s-any-motion-dev%d",
+							  indio_dev->name,
+							  indio_dev->id);
+		if (!data->motion_trig)
+			return -ENOMEM;
+
+		data->dready_trig->dev.parent = &client->dev;
+		data->dready_trig->ops = &bmg160_trigger_ops;
+		iio_trigger_set_drvdata(data->dready_trig, indio_dev);
+		ret = iio_trigger_register(data->dready_trig);
+		if (ret)
+			return ret;
+
+		data->motion_trig->dev.parent = &client->dev;
+		data->motion_trig->ops = &bmg160_trigger_ops;
+		iio_trigger_set_drvdata(data->motion_trig, indio_dev);
+		ret = iio_trigger_register(data->motion_trig);
+		if (ret) {
+			data->motion_trig = NULL;
+			goto err_trigger_unregister;
+		}
+
+		ret = iio_triggered_buffer_setup(indio_dev,
+						 NULL,
+						 bmg160_trigger_handler,
+						 NULL);
+		if (ret < 0) {
+			dev_err(&client->dev,
+				"iio triggered buffer setup failed\n");
+			goto err_trigger_unregister;
+		}
+	}
+
+	ret = iio_device_register(indio_dev);
+	if (ret < 0) {
+		dev_err(&client->dev, "unable to register iio device\n");
+		goto err_buffer_cleanup;
+	}
+
+	ret = pm_runtime_set_active(&client->dev);
+	if (ret)
+		goto err_iio_unregister;
+
+	pm_runtime_enable(&client->dev);
+	pm_runtime_set_autosuspend_delay(&client->dev,
+					 BMG160_AUTO_SUSPEND_DELAY_MS);
+	pm_runtime_use_autosuspend(&client->dev);
+
+	return 0;
+
+err_iio_unregister:
+	iio_device_unregister(indio_dev);
+err_buffer_cleanup:
+	if (data->dready_trig)
+		iio_triggered_buffer_cleanup(indio_dev);
+err_trigger_unregister:
+	if (data->dready_trig)
+		iio_trigger_unregister(data->dready_trig);
+	if (data->motion_trig)
+		iio_trigger_unregister(data->motion_trig);
+
+	return ret;
+}
+
+static int bmg160_remove(struct i2c_client *client)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(client);
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	pm_runtime_disable(&client->dev);
+	pm_runtime_set_suspended(&client->dev);
+	pm_runtime_put_noidle(&client->dev);
+
+	iio_device_unregister(indio_dev);
+
+	if (data->dready_trig) {
+		iio_triggered_buffer_cleanup(indio_dev);
+		iio_trigger_unregister(data->dready_trig);
+		iio_trigger_unregister(data->motion_trig);
+	}
+
+	mutex_lock(&data->mutex);
+	bmg160_set_mode(data, BMG160_MODE_DEEP_SUSPEND);
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int bmg160_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	mutex_lock(&data->mutex);
+	bmg160_set_mode(data, BMG160_MODE_SUSPEND);
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+
+static int bmg160_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	mutex_lock(&data->mutex);
+	if (data->dready_trigger_on || data->motion_trigger_on ||
+							data->ev_enable_state)
+		bmg160_set_mode(data, BMG160_MODE_NORMAL);
+	mutex_unlock(&data->mutex);
+
+	return 0;
+}
+#endif
+
+#ifdef CONFIG_PM_RUNTIME
+static int bmg160_runtime_suspend(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmg160_data *data = iio_priv(indio_dev);
+
+	return bmg160_set_mode(data, BMG160_MODE_SUSPEND);
+}
+
+static int bmg160_runtime_resume(struct device *dev)
+{
+	struct iio_dev *indio_dev = i2c_get_clientdata(to_i2c_client(dev));
+	struct bmg160_data *data = iio_priv(indio_dev);
+	int ret;
+
+	ret = bmg160_set_mode(data, BMG160_MODE_NORMAL);
+	if (ret < 0)
+		return ret;
+
+	msleep_interruptible(BMG160_MAX_STARTUP_TIME_MS);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops bmg160_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(bmg160_suspend, bmg160_resume)
+	SET_RUNTIME_PM_OPS(bmg160_runtime_suspend,
+			   bmg160_runtime_resume, NULL)
+};
+
+static const struct acpi_device_id bmg160_acpi_match[] = {
+	{"BMG0160", 0},
+	{"BMI055B", 0},
+	{},
+};
+
+MODULE_DEVICE_TABLE(acpi, bmg160_acpi_match);
+
+static const struct i2c_device_id bmg160_id[] = {
+	{"bmg160", 0},
+	{"bmi055_gyro", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(i2c, bmg160_id);
+
+static struct i2c_driver bmg160_driver = {
+	.driver = {
+		.name	= BMG160_DRV_NAME,
+		.acpi_match_table = ACPI_PTR(bmg160_acpi_match),
+		.pm	= &bmg160_pm_ops,
+	},
+	.probe		= bmg160_probe,
+	.remove		= bmg160_remove,
+	.id_table	= bmg160_id,
+};
+module_i2c_driver(bmg160_driver);
+
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("BMG160 Gyro driver");
diff --git a/drivers/iio/gyro/hid-sensor-gyro-3d.c b/drivers/iio/gyro/hid-sensor-gyro-3d.c
index fa034a3dad78..a3ea1e8785d7 100644
--- a/drivers/iio/gyro/hid-sensor-gyro-3d.c
+++ b/drivers/iio/gyro/hid-sensor-gyro-3d.c
@@ -416,7 +416,6 @@ static struct platform_driver hid_gyro_3d_platform_driver = {
 	.id_table = hid_gyro_3d_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_gyro_3d_probe,
 	.remove		= hid_gyro_3d_remove,
diff --git a/drivers/iio/gyro/itg3200_buffer.c b/drivers/iio/gyro/itg3200_buffer.c
index e3b3c5084070..eef50e91f17c 100644
--- a/drivers/iio/gyro/itg3200_buffer.c
+++ b/drivers/iio/gyro/itg3200_buffer.c
@@ -132,7 +132,7 @@ int itg3200_probe_trigger(struct iio_dev *indio_dev)
 		goto error_free_irq;
 
 	/* select default trigger */
-	indio_dev->trig = st->trig;
+	indio_dev->trig = iio_trigger_get(st->trig);
 
 	return 0;
 
diff --git a/drivers/iio/humidity/dht11.c b/drivers/iio/humidity/dht11.c
index d8771f546bf2..623c145d8a97 100644
--- a/drivers/iio/humidity/dht11.c
+++ b/drivers/iio/humidity/dht11.c
@@ -281,7 +281,6 @@ static int dht11_probe(struct platform_device *pdev)
 static struct platform_driver dht11_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = dht11_dt_ids,
 	},
 	.probe  = dht11_probe,
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
index 0c6517c94a9d..b75519deac1a 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c
@@ -673,8 +673,7 @@ static int inv_mpu_probe(struct i2c_client *client,
 
 	st = iio_priv(indio_dev);
 	st->client = client;
-	pdata = (struct inv_mpu6050_platform_data
-			*)dev_get_platdata(&client->dev);
+	pdata = dev_get_platdata(&client->dev);
 	if (pdata)
 		st->plat_data = *pdata;
 	/* power is turned on inside check chip type*/
diff --git a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
index 03b9372c1212..926fccea8de0 100644
--- a/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
+++ b/drivers/iio/imu/inv_mpu6050/inv_mpu_trigger.c
@@ -135,7 +135,7 @@ int inv_mpu6050_probe_trigger(struct iio_dev *indio_dev)
 	ret = iio_trigger_register(st->trig);
 	if (ret)
 		goto error_free_irq;
-	indio_dev->trig = st->trig;
+	indio_dev->trig = iio_trigger_get(st->trig);
 
 	return 0;
 
diff --git a/drivers/iio/industrialio-buffer.c b/drivers/iio/industrialio-buffer.c
index 0472ee268271..f971f79103ec 100644
--- a/drivers/iio/industrialio-buffer.c
+++ b/drivers/iio/industrialio-buffer.c
@@ -942,13 +942,34 @@ int iio_push_to_buffers(struct iio_dev *indio_dev, const void *data)
 }
 EXPORT_SYMBOL_GPL(iio_push_to_buffers);
 
+static int iio_buffer_add_demux(struct iio_buffer *buffer,
+	struct iio_demux_table **p, unsigned int in_loc, unsigned int out_loc,
+	unsigned int length)
+{
+
+	if (*p && (*p)->from + (*p)->length == in_loc &&
+		(*p)->to + (*p)->length == out_loc) {
+		(*p)->length += length;
+	} else {
+		*p = kmalloc(sizeof(**p), GFP_KERNEL);
+		if (*p == NULL)
+			return -ENOMEM;
+		(*p)->from = in_loc;
+		(*p)->to = out_loc;
+		(*p)->length = length;
+		list_add_tail(&(*p)->l, &buffer->demux_list);
+	}
+
+	return 0;
+}
+
 static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 				   struct iio_buffer *buffer)
 {
 	const struct iio_chan_spec *ch;
 	int ret, in_ind = -1, out_ind, length;
 	unsigned in_loc = 0, out_loc = 0;
-	struct iio_demux_table *p;
+	struct iio_demux_table *p = NULL;
 
 	/* Clear out any old demux */
 	iio_buffer_demux_free(buffer);
@@ -979,14 +1000,7 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 			else
 				length = ch->scan_type.storagebits / 8;
 			/* Make sure we are aligned */
-			in_loc += length;
-			if (in_loc % length)
-				in_loc += length - in_loc % length;
-		}
-		p = kmalloc(sizeof(*p), GFP_KERNEL);
-		if (p == NULL) {
-			ret = -ENOMEM;
-			goto error_clear_mux_table;
+			in_loc = roundup(in_loc, length) + length;
 		}
 		ch = iio_find_channel_from_si(indio_dev, in_ind);
 		if (ch->scan_type.repeat > 1)
@@ -994,24 +1008,16 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 				ch->scan_type.repeat;
 		else
 			length = ch->scan_type.storagebits / 8;
-		if (out_loc % length)
-			out_loc += length - out_loc % length;
-		if (in_loc % length)
-			in_loc += length - in_loc % length;
-		p->from = in_loc;
-		p->to = out_loc;
-		p->length = length;
-		list_add_tail(&p->l, &buffer->demux_list);
+		out_loc = roundup(out_loc, length);
+		in_loc = roundup(in_loc, length);
+		ret = iio_buffer_add_demux(buffer, &p, in_loc, out_loc, length);
+		if (ret)
+			goto error_clear_mux_table;
 		out_loc += length;
 		in_loc += length;
 	}
 	/* Relies on scan_timestamp being last */
 	if (buffer->scan_timestamp) {
-		p = kmalloc(sizeof(*p), GFP_KERNEL);
-		if (p == NULL) {
-			ret = -ENOMEM;
-			goto error_clear_mux_table;
-		}
 		ch = iio_find_channel_from_si(indio_dev,
 			indio_dev->scan_index_timestamp);
 		if (ch->scan_type.repeat > 1)
@@ -1019,14 +1025,11 @@ static int iio_buffer_update_demux(struct iio_dev *indio_dev,
 				ch->scan_type.repeat;
 		else
 			length = ch->scan_type.storagebits / 8;
-		if (out_loc % length)
-			out_loc += length - out_loc % length;
-		if (in_loc % length)
-			in_loc += length - in_loc % length;
-		p->from = in_loc;
-		p->to = out_loc;
-		p->length = length;
-		list_add_tail(&p->l, &buffer->demux_list);
+		out_loc = roundup(out_loc, length);
+		in_loc = roundup(in_loc, length);
+		ret = iio_buffer_add_demux(buffer, &p, in_loc, out_loc, length);
+		if (ret)
+			goto error_clear_mux_table;
 		out_loc += length;
 		in_loc += length;
 	}
diff --git a/drivers/iio/inkern.c b/drivers/iio/inkern.c
index c7497009d60a..f0846108d006 100644
--- a/drivers/iio/inkern.c
+++ b/drivers/iio/inkern.c
@@ -178,7 +178,7 @@ static struct iio_channel *of_iio_channel_get_by_name(struct device_node *np,
 			index = of_property_match_string(np, "io-channel-names",
 							 name);
 		chan = of_iio_channel_get(np, index);
-		if (!IS_ERR(chan))
+		if (!IS_ERR(chan) || PTR_ERR(chan) == -EPROBE_DEFER)
 			break;
 		else if (name && index >= 0) {
 			pr_err("ERROR: could not get IIO channel %s:%s(%i)\n",
diff --git a/drivers/iio/light/Kconfig b/drivers/iio/light/Kconfig
index bf05ca5b0a57..5bea821adcae 100644
--- a/drivers/iio/light/Kconfig
+++ b/drivers/iio/light/Kconfig
@@ -17,6 +17,16 @@ config ADJD_S311
 	 This driver can also be built as a module.  If so, the module
 	 will be called adjd_s311.
 
+config AL3320A
+	tristate "AL3320A ambient light sensor"
+	depends on I2C
+	help
+	 Say Y here if you want to build a driver for the Dyna Image AL3320A
+	 ambient light sensor.
+
+	 To compile this driver as a module, choose M here: the
+	 module will be called al3320a.
+
 config APDS9300
 	tristate "APDS9300 ambient light sensor"
 	depends on I2C
diff --git a/drivers/iio/light/Makefile b/drivers/iio/light/Makefile
index 8b8c09f9c1f8..47877a36cc12 100644
--- a/drivers/iio/light/Makefile
+++ b/drivers/iio/light/Makefile
@@ -4,6 +4,7 @@
 
 # When adding new entries keep the list in alphabetical order
 obj-$(CONFIG_ADJD_S311)		+= adjd_s311.o
+obj-$(CONFIG_AL3320A)		+= al3320a.o
 obj-$(CONFIG_APDS9300)		+= apds9300.o
 obj-$(CONFIG_CM32181)		+= cm32181.o
 obj-$(CONFIG_CM36651)		+= cm36651.o
diff --git a/drivers/iio/light/al3320a.c b/drivers/iio/light/al3320a.c
new file mode 100644
index 000000000000..6aac6513fd41
--- /dev/null
+++ b/drivers/iio/light/al3320a.c
@@ -0,0 +1,232 @@
+/*
+ * AL3320A - Dyna Image Ambient Light Sensor
+ *
+ * Copyright (c) 2014, Intel Corporation.
+ *
+ * This file is subject to the terms and conditions of version 2 of
+ * the GNU General Public License.  See the file COPYING in the main
+ * directory of this archive for more details.
+ *
+ * IIO driver for AL3320A (7-bit I2C slave address 0x1C).
+ *
+ * TODO: interrupt support, thresholds
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+
+#include <linux/iio/iio.h>
+#include <linux/iio/sysfs.h>
+
+#define AL3320A_DRV_NAME "al3320a"
+
+#define AL3320A_REG_CONFIG		0x00
+#define AL3320A_REG_STATUS		0x01
+#define AL3320A_REG_INT			0x02
+#define AL3320A_REG_WAIT		0x06
+#define AL3320A_REG_CONFIG_RANGE	0x07
+#define AL3320A_REG_PERSIST		0x08
+#define AL3320A_REG_MEAN_TIME		0x09
+#define AL3320A_REG_ADUMMY		0x0A
+#define AL3320A_REG_DATA_LOW		0x22
+
+#define AL3320A_REG_LOW_THRESH_LOW	0x30
+#define AL3320A_REG_LOW_THRESH_HIGH	0x31
+#define AL3320A_REG_HIGH_THRESH_LOW	0x32
+#define AL3320A_REG_HIGH_THRESH_HIGH	0x33
+
+#define AL3320A_CONFIG_DISABLE		0x00
+#define AL3320A_CONFIG_ENABLE		0x01
+
+#define AL3320A_GAIN_SHIFT		1
+#define AL3320A_GAIN_MASK		(BIT(2) | BIT(1))
+
+/* chip params default values */
+#define AL3320A_DEFAULT_MEAN_TIME	4
+#define AL3320A_DEFAULT_WAIT_TIME	0 /* no waiting */
+
+#define AL3320A_SCALE_AVAILABLE "0.512 0.128 0.032 0.01"
+
+enum al3320a_range {
+	AL3320A_RANGE_1, /* 33.28 Klx */
+	AL3320A_RANGE_2, /* 8.32 Klx  */
+	AL3320A_RANGE_3, /* 2.08 Klx  */
+	AL3320A_RANGE_4  /* 0.65 Klx  */
+};
+
+static const int al3320a_scales[][2] = {
+	{0, 512000}, {0, 128000}, {0, 32000}, {0, 10000}
+};
+
+struct al3320a_data {
+	struct i2c_client *client;
+};
+
+static const struct iio_chan_spec al3320a_channels[] = {
+	{
+		.type	= IIO_LIGHT,
+		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW) |
+				      BIT(IIO_CHAN_INFO_SCALE),
+	}
+};
+
+static IIO_CONST_ATTR(in_illuminance_scale_available, AL3320A_SCALE_AVAILABLE);
+
+static struct attribute *al3320a_attributes[] = {
+	&iio_const_attr_in_illuminance_scale_available.dev_attr.attr,
+	NULL,
+};
+
+static const struct attribute_group al3320a_attribute_group = {
+	.attrs = al3320a_attributes,
+};
+
+static int al3320a_init(struct al3320a_data *data)
+{
+	int ret;
+
+	/* power on */
+	ret = i2c_smbus_write_byte_data(data->client, AL3320A_REG_CONFIG,
+					AL3320A_CONFIG_ENABLE);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(data->client, AL3320A_REG_CONFIG_RANGE,
+					AL3320A_RANGE_3 << AL3320A_GAIN_SHIFT);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(data->client, AL3320A_REG_MEAN_TIME,
+					AL3320A_DEFAULT_MEAN_TIME);
+	if (ret < 0)
+		return ret;
+
+	ret = i2c_smbus_write_byte_data(data->client, AL3320A_REG_WAIT,
+					AL3320A_DEFAULT_WAIT_TIME);
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
+static int al3320a_read_raw(struct iio_dev *indio_dev,
+			    struct iio_chan_spec const *chan, int *val,
+			    int *val2, long mask)
+{
+	struct al3320a_data *data = iio_priv(indio_dev);
+	int ret;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_RAW:
+		/*
+		 * ALS ADC value is stored in two adjacent registers:
+		 * - low byte of output is stored at AL3320A_REG_DATA_LOW
+		 * - high byte of output is stored at AL3320A_REG_DATA_LOW + 1
+		 */
+		ret = i2c_smbus_read_word_data(data->client,
+					       AL3320A_REG_DATA_LOW);
+		if (ret < 0)
+			return ret;
+		*val = ret;
+		return IIO_VAL_INT;
+	case IIO_CHAN_INFO_SCALE:
+		ret = i2c_smbus_read_byte_data(data->client,
+					       AL3320A_REG_CONFIG_RANGE);
+		if (ret < 0)
+			return ret;
+
+		ret = (ret & AL3320A_GAIN_MASK) >> AL3320A_GAIN_SHIFT;
+		*val = al3320a_scales[ret][0];
+		*val2 = al3320a_scales[ret][1];
+
+		return IIO_VAL_INT_PLUS_MICRO;
+	}
+	return -EINVAL;
+}
+
+static int al3320a_write_raw(struct iio_dev *indio_dev,
+			     struct iio_chan_spec const *chan, int val,
+			     int val2, long mask)
+{
+	struct al3320a_data *data = iio_priv(indio_dev);
+	int i;
+
+	switch (mask) {
+	case IIO_CHAN_INFO_SCALE:
+		for (i = 0; i < ARRAY_SIZE(al3320a_scales); i++) {
+			if (val == al3320a_scales[i][0] &&
+			    val2 == al3320a_scales[i][1])
+				return i2c_smbus_write_byte_data(data->client,
+					AL3320A_REG_CONFIG_RANGE,
+					i << AL3320A_GAIN_SHIFT);
+		}
+		break;
+	}
+	return -EINVAL;
+}
+
+static const struct iio_info al3320a_info = {
+	.driver_module	= THIS_MODULE,
+	.read_raw	= al3320a_read_raw,
+	.write_raw	= al3320a_write_raw,
+	.attrs		= &al3320a_attribute_group,
+};
+
+static int al3320a_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct al3320a_data *data;
+	struct iio_dev *indio_dev;
+	int ret;
+
+	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*data));
+	if (!indio_dev)
+		return -ENOMEM;
+
+	data = iio_priv(indio_dev);
+	i2c_set_clientdata(client, indio_dev);
+	data->client = client;
+
+	indio_dev->dev.parent = &client->dev;
+	indio_dev->info = &al3320a_info;
+	indio_dev->name = AL3320A_DRV_NAME;
+	indio_dev->channels = al3320a_channels;
+	indio_dev->num_channels = ARRAY_SIZE(al3320a_channels);
+	indio_dev->modes = INDIO_DIRECT_MODE;
+
+	ret = al3320a_init(data);
+	if (ret < 0) {
+		dev_err(&client->dev, "al3320a chip init failed\n");
+		return ret;
+	}
+	return devm_iio_device_register(&client->dev, indio_dev);
+}
+
+static int al3320a_remove(struct i2c_client *client)
+{
+	return i2c_smbus_write_byte_data(client, AL3320A_REG_CONFIG,
+					 AL3320A_CONFIG_DISABLE);
+}
+
+static const struct i2c_device_id al3320a_id[] = {
+	{"al3320a", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, al3320a_id);
+
+static struct i2c_driver al3320a_driver = {
+	.driver = {
+		.name = AL3320A_DRV_NAME,
+	},
+	.probe		= al3320a_probe,
+	.remove		= al3320a_remove,
+	.id_table	= al3320a_id,
+};
+
+module_i2c_driver(al3320a_driver);
+
+MODULE_AUTHOR("Daniel Baluta <daniel.baluta@intel.com>");
+MODULE_DESCRIPTION("AL3320A Ambient Light Sensor driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/iio/light/hid-sensor-als.c b/drivers/iio/light/hid-sensor-als.c
index 96e71e103ea7..a5283d75c096 100644
--- a/drivers/iio/light/hid-sensor-als.c
+++ b/drivers/iio/light/hid-sensor-als.c
@@ -381,7 +381,6 @@ static struct platform_driver hid_als_platform_driver = {
 	.id_table = hid_als_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_als_probe,
 	.remove		= hid_als_remove,
diff --git a/drivers/iio/light/hid-sensor-prox.c b/drivers/iio/light/hid-sensor-prox.c
index 412bae86d6ae..f5a514698fd8 100644
--- a/drivers/iio/light/hid-sensor-prox.c
+++ b/drivers/iio/light/hid-sensor-prox.c
@@ -373,7 +373,6 @@ static struct platform_driver hid_prox_platform_driver = {
 	.id_table = hid_prox_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_prox_probe,
 	.remove		= hid_prox_remove,
diff --git a/drivers/iio/light/lm3533-als.c b/drivers/iio/light/lm3533-als.c
index c1aadc6b865a..ae3c71bdd6c6 100644
--- a/drivers/iio/light/lm3533-als.c
+++ b/drivers/iio/light/lm3533-als.c
@@ -915,7 +915,6 @@ static int lm3533_als_remove(struct platform_device *pdev)
 static struct platform_driver lm3533_als_driver = {
 	.driver	= {
 		.name	= "lm3533-als",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= lm3533_als_probe,
 	.remove		= lm3533_als_remove,
diff --git a/drivers/iio/magnetometer/ak8975.c b/drivers/iio/magnetometer/ak8975.c
index a2357921d761..bf5ef077e791 100644
--- a/drivers/iio/magnetometer/ak8975.c
+++ b/drivers/iio/magnetometer/ak8975.c
@@ -477,8 +477,8 @@ static const struct acpi_device_id ak_acpi_match[] = {
 };
 MODULE_DEVICE_TABLE(acpi, ak_acpi_match);
 
-static char *ak8975_match_acpi_device(struct device *dev,
-				enum asahi_compass_chipset *chipset)
+static const char *ak8975_match_acpi_device(struct device *dev,
+					    enum asahi_compass_chipset *chipset)
 {
 	const struct acpi_device_id *id;
 
@@ -487,7 +487,7 @@ static char *ak8975_match_acpi_device(struct device *dev,
 		return NULL;
 	*chipset = (int)id->driver_data;
 
-	return (char *)dev_name(dev);
+	return dev_name(dev);
 }
 
 static int ak8975_probe(struct i2c_client *client,
@@ -497,7 +497,7 @@ static int ak8975_probe(struct i2c_client *client,
 	struct iio_dev *indio_dev;
 	int eoc_gpio;
 	int err;
-	char *name = NULL;
+	const char *name = NULL;
 
 	/* Grab and set up the supplied GPIO. */
 	if (client->dev.platform_data)
@@ -539,7 +539,7 @@ static int ak8975_probe(struct i2c_client *client,
 	if (id) {
 		data->chipset =
 			(enum asahi_compass_chipset)(id->driver_data);
-		name = (char *) id->name;
+		name = id->name;
 	} else if (ACPI_HANDLE(&client->dev))
 		name = ak8975_match_acpi_device(&client->dev, &data->chipset);
 	else
diff --git a/drivers/iio/magnetometer/hid-sensor-magn-3d.c b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
index 3ec777a8f64e..6294575d2777 100644
--- a/drivers/iio/magnetometer/hid-sensor-magn-3d.c
+++ b/drivers/iio/magnetometer/hid-sensor-magn-3d.c
@@ -246,8 +246,7 @@ static const struct iio_info magn_3d_info = {
 };
 
 /* Function to push data to buffer */
-static void hid_sensor_push_data(struct iio_dev *indio_dev, const void *data,
-	int len)
+static void hid_sensor_push_data(struct iio_dev *indio_dev, const void *data)
 {
 	dev_dbg(&indio_dev->dev, "hid_sensor_push_data\n");
 	iio_push_to_buffers(indio_dev, data);
@@ -263,9 +262,7 @@ static int magn_3d_proc_event(struct hid_sensor_hub_device *hsdev,
 
 	dev_dbg(&indio_dev->dev, "magn_3d_proc_event\n");
 	if (atomic_read(&magn_state->common_attributes.data_ready))
-		hid_sensor_push_data(indio_dev,
-				magn_state->iio_vals,
-				sizeof(magn_state->iio_vals));
+		hid_sensor_push_data(indio_dev, magn_state->iio_vals);
 
 	return 0;
 }
@@ -533,7 +530,6 @@ static struct platform_driver hid_magn_3d_platform_driver = {
 	.id_table = hid_magn_3d_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_magn_3d_probe,
 	.remove		= hid_magn_3d_remove,
diff --git a/drivers/iio/magnetometer/st_magn_core.c b/drivers/iio/magnetometer/st_magn_core.c
index a4b64130ac2f..68cae86dbd29 100644
--- a/drivers/iio/magnetometer/st_magn_core.c
+++ b/drivers/iio/magnetometer/st_magn_core.c
@@ -42,7 +42,8 @@
 #define ST_MAGN_FS_AVL_5600MG			5600
 #define ST_MAGN_FS_AVL_8000MG			8000
 #define ST_MAGN_FS_AVL_8100MG			8100
-#define ST_MAGN_FS_AVL_10000MG			10000
+#define ST_MAGN_FS_AVL_12000MG			12000
+#define ST_MAGN_FS_AVL_16000MG			16000
 
 /* CUSTOM VALUES FOR SENSOR 1 */
 #define ST_MAGN_1_WAI_EXP			0x3c
@@ -69,20 +70,20 @@
 #define ST_MAGN_1_FS_AVL_4700_VAL		0x05
 #define ST_MAGN_1_FS_AVL_5600_VAL		0x06
 #define ST_MAGN_1_FS_AVL_8100_VAL		0x07
-#define ST_MAGN_1_FS_AVL_1300_GAIN_XY		1100
-#define ST_MAGN_1_FS_AVL_1900_GAIN_XY		855
-#define ST_MAGN_1_FS_AVL_2500_GAIN_XY		670
-#define ST_MAGN_1_FS_AVL_4000_GAIN_XY		450
-#define ST_MAGN_1_FS_AVL_4700_GAIN_XY		400
-#define ST_MAGN_1_FS_AVL_5600_GAIN_XY		330
-#define ST_MAGN_1_FS_AVL_8100_GAIN_XY		230
-#define ST_MAGN_1_FS_AVL_1300_GAIN_Z		980
-#define ST_MAGN_1_FS_AVL_1900_GAIN_Z		760
-#define ST_MAGN_1_FS_AVL_2500_GAIN_Z		600
-#define ST_MAGN_1_FS_AVL_4000_GAIN_Z		400
-#define ST_MAGN_1_FS_AVL_4700_GAIN_Z		355
-#define ST_MAGN_1_FS_AVL_5600_GAIN_Z		295
-#define ST_MAGN_1_FS_AVL_8100_GAIN_Z		205
+#define ST_MAGN_1_FS_AVL_1300_GAIN_XY		909
+#define ST_MAGN_1_FS_AVL_1900_GAIN_XY		1169
+#define ST_MAGN_1_FS_AVL_2500_GAIN_XY		1492
+#define ST_MAGN_1_FS_AVL_4000_GAIN_XY		2222
+#define ST_MAGN_1_FS_AVL_4700_GAIN_XY		2500
+#define ST_MAGN_1_FS_AVL_5600_GAIN_XY		3030
+#define ST_MAGN_1_FS_AVL_8100_GAIN_XY		4347
+#define ST_MAGN_1_FS_AVL_1300_GAIN_Z		1020
+#define ST_MAGN_1_FS_AVL_1900_GAIN_Z		1315
+#define ST_MAGN_1_FS_AVL_2500_GAIN_Z		1666
+#define ST_MAGN_1_FS_AVL_4000_GAIN_Z		2500
+#define ST_MAGN_1_FS_AVL_4700_GAIN_Z		2816
+#define ST_MAGN_1_FS_AVL_5600_GAIN_Z		3389
+#define ST_MAGN_1_FS_AVL_8100_GAIN_Z		4878
 #define ST_MAGN_1_MULTIREAD_BIT			false
 
 /* CUSTOM VALUES FOR SENSOR 2 */
@@ -105,10 +106,12 @@
 #define ST_MAGN_2_FS_MASK			0x60
 #define ST_MAGN_2_FS_AVL_4000_VAL		0x00
 #define ST_MAGN_2_FS_AVL_8000_VAL		0x01
-#define ST_MAGN_2_FS_AVL_10000_VAL		0x02
-#define ST_MAGN_2_FS_AVL_4000_GAIN		430
-#define ST_MAGN_2_FS_AVL_8000_GAIN		230
-#define ST_MAGN_2_FS_AVL_10000_GAIN		230
+#define ST_MAGN_2_FS_AVL_12000_VAL		0x02
+#define ST_MAGN_2_FS_AVL_16000_VAL		0x03
+#define ST_MAGN_2_FS_AVL_4000_GAIN		146
+#define ST_MAGN_2_FS_AVL_8000_GAIN		292
+#define ST_MAGN_2_FS_AVL_12000_GAIN		438
+#define ST_MAGN_2_FS_AVL_16000_GAIN		584
 #define ST_MAGN_2_MULTIREAD_BIT			false
 #define ST_MAGN_2_OUT_X_L_ADDR			0x28
 #define ST_MAGN_2_OUT_Y_L_ADDR			0x2a
@@ -266,9 +269,14 @@ static const struct st_sensors st_magn_sensors[] = {
 					.gain = ST_MAGN_2_FS_AVL_8000_GAIN,
 				},
 				[2] = {
-					.num = ST_MAGN_FS_AVL_10000MG,
-					.value = ST_MAGN_2_FS_AVL_10000_VAL,
-					.gain = ST_MAGN_2_FS_AVL_10000_GAIN,
+					.num = ST_MAGN_FS_AVL_12000MG,
+					.value = ST_MAGN_2_FS_AVL_12000_VAL,
+					.gain = ST_MAGN_2_FS_AVL_12000_GAIN,
+				},
+				[3] = {
+					.num = ST_MAGN_FS_AVL_16000MG,
+					.value = ST_MAGN_2_FS_AVL_16000_VAL,
+					.gain = ST_MAGN_2_FS_AVL_16000_GAIN,
 				},
 			},
 		},
diff --git a/drivers/iio/orientation/hid-sensor-incl-3d.c b/drivers/iio/orientation/hid-sensor-incl-3d.c
index 2478f6c2ef25..1ff181bbbcef 100644
--- a/drivers/iio/orientation/hid-sensor-incl-3d.c
+++ b/drivers/iio/orientation/hid-sensor-incl-3d.c
@@ -437,7 +437,6 @@ static struct platform_driver hid_incl_3d_platform_driver = {
 	.id_table = hid_incl_3d_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_incl_3d_probe,
 	.remove		= hid_incl_3d_remove,
diff --git a/drivers/iio/orientation/hid-sensor-rotation.c b/drivers/iio/orientation/hid-sensor-rotation.c
index dccf848e8b0f..4afb6c79ccbc 100644
--- a/drivers/iio/orientation/hid-sensor-rotation.c
+++ b/drivers/iio/orientation/hid-sensor-rotation.c
@@ -334,7 +334,6 @@ static struct platform_driver hid_dev_rot_platform_driver = {
 	.id_table = hid_dev_rot_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_dev_rot_probe,
 	.remove		= hid_dev_rot_remove,
diff --git a/drivers/iio/pressure/hid-sensor-press.c b/drivers/iio/pressure/hid-sensor-press.c
index 2c0d2a4fed8c..764928682df2 100644
--- a/drivers/iio/pressure/hid-sensor-press.c
+++ b/drivers/iio/pressure/hid-sensor-press.c
@@ -382,7 +382,6 @@ static struct platform_driver hid_press_platform_driver = {
 	.id_table = hid_press_ids,
 	.driver = {
 		.name	= KBUILD_MODNAME,
-		.owner	= THIS_MODULE,
 	},
 	.probe		= hid_press_probe,
 	.remove		= hid_press_remove,
diff --git a/drivers/iio/trigger/iio-trig-interrupt.c b/drivers/iio/trigger/iio-trig-interrupt.c
index 7a149a7822bc..572bc6f02ca8 100644
--- a/drivers/iio/trigger/iio-trig-interrupt.c
+++ b/drivers/iio/trigger/iio-trig-interrupt.c
@@ -109,7 +109,6 @@ static struct platform_driver iio_interrupt_trigger_driver = {
 	.remove = iio_interrupt_trigger_remove,
 	.driver = {
 		.name = "iio_interrupt_trigger",
-		.owner = THIS_MODULE,
 	},
 };
 
diff --git a/drivers/infiniband/core/umem.c b/drivers/infiniband/core/umem.c
index a3a2e9c1639b..df0c4f605a21 100644
--- a/drivers/infiniband/core/umem.c
+++ b/drivers/infiniband/core/umem.c
@@ -105,6 +105,7 @@ struct ib_umem *ib_umem_get(struct ib_ucontext *context, unsigned long addr,
 	umem->length    = size;
 	umem->offset    = addr & ~PAGE_MASK;
 	umem->page_size = PAGE_SIZE;
+	umem->pid       = get_task_pid(current, PIDTYPE_PID);
 	/*
 	 * We ask for writable memory if any access flags other than
 	 * "remote read" are set.  "Local write" and "remote write"
@@ -198,6 +199,7 @@ out:
 	if (ret < 0) {
 		if (need_release)
 			__ib_umem_release(context->device, umem, 0);
+		put_pid(umem->pid);
 		kfree(umem);
 	} else
 		current->mm->pinned_vm = locked;
@@ -230,15 +232,19 @@ void ib_umem_release(struct ib_umem *umem)
 {
 	struct ib_ucontext *context = umem->context;
 	struct mm_struct *mm;
+	struct task_struct *task;
 	unsigned long diff;
 
 	__ib_umem_release(umem->context->device, umem, 1);
 
-	mm = get_task_mm(current);
-	if (!mm) {
-		kfree(umem);
-		return;
-	}
+	task = get_pid_task(umem->pid, PIDTYPE_PID);
+	put_pid(umem->pid);
+	if (!task)
+		goto out;
+	mm = get_task_mm(task);
+	put_task_struct(task);
+	if (!mm)
+		goto out;
 
 	diff = PAGE_ALIGN(umem->length + umem->offset) >> PAGE_SHIFT;
 
@@ -262,9 +268,10 @@ void ib_umem_release(struct ib_umem *umem)
 	} else
 		down_write(&mm->mmap_sem);
 
-	current->mm->pinned_vm -= diff;
+	mm->pinned_vm -= diff;
 	up_write(&mm->mmap_sem);
 	mmput(mm);
+out:
 	kfree(umem);
 }
 EXPORT_SYMBOL(ib_umem_release);
diff --git a/drivers/infiniband/core/uverbs_cmd.c b/drivers/infiniband/core/uverbs_cmd.c
index 0600c50e6215..5ba2a86aab6a 100644
--- a/drivers/infiniband/core/uverbs_cmd.c
+++ b/drivers/infiniband/core/uverbs_cmd.c
@@ -2518,6 +2518,8 @@ ssize_t ib_uverbs_create_ah(struct ib_uverbs_file *file,
 	attr.grh.sgid_index    = cmd.attr.grh.sgid_index;
 	attr.grh.hop_limit     = cmd.attr.grh.hop_limit;
 	attr.grh.traffic_class = cmd.attr.grh.traffic_class;
+	attr.vlan_id           = 0;
+	memset(&attr.dmac, 0, sizeof(attr.dmac));
 	memcpy(attr.grh.dgid.raw, cmd.attr.grh.dgid, 16);
 
 	ah = ib_create_ah(pd, &attr);
diff --git a/drivers/infiniband/core/uverbs_main.c b/drivers/infiniband/core/uverbs_main.c
index c73b22a257fe..71ab83fde472 100644
--- a/drivers/infiniband/core/uverbs_main.c
+++ b/drivers/infiniband/core/uverbs_main.c
@@ -477,6 +477,7 @@ static void ib_uverbs_async_handler(struct ib_uverbs_file *file,
 
 	entry->desc.async.element    = element;
 	entry->desc.async.event_type = event;
+	entry->desc.async.reserved   = 0;
 	entry->counter               = counter;
 
 	list_add_tail(&entry->list, &file->async_file->event_list);
@@ -502,6 +503,10 @@ void ib_uverbs_qp_event_handler(struct ib_event *event, void *context_ptr)
 {
 	struct ib_uevent_object *uobj;
 
+	/* for XRC target qp's, check that qp is live */
+	if (!event->element.qp->uobject || !event->element.qp->uobject->live)
+		return;
+
 	uobj = container_of(event->element.qp->uobject,
 			    struct ib_uevent_object, uobject);
 
diff --git a/drivers/infiniband/core/uverbs_marshall.c b/drivers/infiniband/core/uverbs_marshall.c
index e7bee46868d1..abd97247443e 100644
--- a/drivers/infiniband/core/uverbs_marshall.c
+++ b/drivers/infiniband/core/uverbs_marshall.c
@@ -140,5 +140,9 @@ void ib_copy_path_rec_from_user(struct ib_sa_path_rec *dst,
 	dst->packet_life_time	= src->packet_life_time;
 	dst->preference		= src->preference;
 	dst->packet_life_time_selector = src->packet_life_time_selector;
+
+	memset(dst->smac, 0, sizeof(dst->smac));
+	memset(dst->dmac, 0, sizeof(dst->dmac));
+	dst->vlan_id = 0xffff;
 }
 EXPORT_SYMBOL(ib_copy_path_rec_from_user);
diff --git a/drivers/infiniband/hw/cxgb4/cm.c b/drivers/infiniband/hw/cxgb4/cm.c
index c2fb71c182a8..fb61f6685809 100644
--- a/drivers/infiniband/hw/cxgb4/cm.c
+++ b/drivers/infiniband/hw/cxgb4/cm.c
@@ -236,10 +236,12 @@ static void release_tid(struct c4iw_rdev *rdev, u32 hwtid, struct sk_buff *skb)
 static void set_emss(struct c4iw_ep *ep, u16 opt)
 {
 	ep->emss = ep->com.dev->rdev.lldi.mtus[GET_TCPOPT_MSS(opt)] -
-		   sizeof(struct iphdr) - sizeof(struct tcphdr);
+		   ((AF_INET == ep->com.remote_addr.ss_family) ?
+		    sizeof(struct iphdr) : sizeof(struct ipv6hdr)) -
+		   sizeof(struct tcphdr);
 	ep->mss = ep->emss;
 	if (GET_TCPOPT_TSTAMP(opt))
-		ep->emss -= 12;
+		ep->emss -= round_up(TCPOLEN_TIMESTAMP, 4);
 	if (ep->emss < 128)
 		ep->emss = 128;
 	if (ep->emss & 7)
@@ -415,6 +417,7 @@ static struct dst_entry *find_route(struct c4iw_dev *dev, __be32 local_ip,
 		return NULL;
 	if (!our_interface(dev, n->dev) &&
 	    !(n->dev->flags & IFF_LOOPBACK)) {
+		neigh_release(n);
 		dst_release(&rt->dst);
 		return NULL;
 	}
@@ -581,11 +584,14 @@ static void c4iw_record_pm_msg(struct c4iw_ep *ep,
 }
 
 static void best_mtu(const unsigned short *mtus, unsigned short mtu,
-		     unsigned int *idx, int use_ts)
+		     unsigned int *idx, int use_ts, int ipv6)
 {
-	unsigned short hdr_size = sizeof(struct iphdr) +
+	unsigned short hdr_size = (ipv6 ?
+				   sizeof(struct ipv6hdr) :
+				   sizeof(struct iphdr)) +
 				  sizeof(struct tcphdr) +
-				  (use_ts ? 12 : 0);
+				  (use_ts ?
+				   round_up(TCPOLEN_TIMESTAMP, 4) : 0);
 	unsigned short data_size = mtu - hdr_size;
 
 	cxgb4_best_aligned_mtu(mtus, hdr_size, data_size, 8, idx);
@@ -634,7 +640,8 @@ static int send_connect(struct c4iw_ep *ep)
 	set_wr_txq(skb, CPL_PRIORITY_SETUP, ep->ctrlq_idx);
 
 	best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
-		 enable_tcp_timestamps);
+		 enable_tcp_timestamps,
+		 (AF_INET == ep->com.remote_addr.ss_family) ? 0 : 1);
 	wscale = compute_wscale(rcv_win);
 
 	/*
@@ -668,6 +675,7 @@ static int send_connect(struct c4iw_ep *ep)
 	if (is_t5(ep->com.dev->rdev.lldi.adapter_type)) {
 		opt2 |= T5_OPT_2_VALID;
 		opt2 |= V_CONG_CNTRL(CONG_ALG_TAHOE);
+		opt2 |= CONG_CNTRL_VALID; /* OPT_2_ISS for T5 */
 	}
 	t4_set_arp_err_handler(skb, ep, act_open_req_arp_failure);
 
@@ -713,8 +721,6 @@ static int send_connect(struct c4iw_ep *ep)
 	} else {
 		u32 isn = (prandom_u32() & ~7UL) - 1;
 
-		opt2 |= T5_OPT_2_VALID;
-		opt2 |= CONG_CNTRL_VALID; /* OPT_2_ISS for T5 */
 		if (peer2peer)
 			isn += 4;
 
@@ -756,10 +762,10 @@ static int send_connect(struct c4iw_ep *ep)
 			t5_req6->peer_ip_lo = *((__be64 *)
 						(ra6->sin6_addr.s6_addr + 8));
 			t5_req6->opt0 = cpu_to_be64(opt0);
-			t5_req6->params = (__force __be64)cpu_to_be32(
+			t5_req6->params = cpu_to_be64(V_FILTER_TUPLE(
 							cxgb4_select_ntuple(
 						ep->com.dev->rdev.lldi.ports[0],
-						ep->l2t));
+						ep->l2t)));
 			t5_req6->rsvd = cpu_to_be32(isn);
 			PDBG("%s snd_isn %u\n", __func__,
 			     be32_to_cpu(t5_req6->rsvd));
@@ -1763,7 +1769,8 @@ static void send_fw_act_open_req(struct c4iw_ep *ep, unsigned int atid)
 	req->tcb.tx_max = (__force __be32) jiffies;
 	req->tcb.rcv_adv = htons(1);
 	best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
-		 enable_tcp_timestamps);
+		 enable_tcp_timestamps,
+		 (AF_INET == ep->com.remote_addr.ss_family) ? 0 : 1);
 	wscale = compute_wscale(rcv_win);
 
 	/*
@@ -2162,7 +2169,8 @@ static void accept_cr(struct c4iw_ep *ep, struct sk_buff *skb,
 						    ep->hwtid));
 
 	best_mtu(ep->com.dev->rdev.lldi.mtus, ep->mtu, &mtu_idx,
-		 enable_tcp_timestamps && req->tcpopt.tstamp);
+		 enable_tcp_timestamps && req->tcpopt.tstamp,
+		 (AF_INET == ep->com.remote_addr.ss_family) ? 0 : 1);
 	wscale = compute_wscale(rcv_win);
 
 	/*
diff --git a/drivers/infiniband/hw/cxgb4/device.c b/drivers/infiniband/hw/cxgb4/device.c
index f25df5276c22..72f1f052e88c 100644
--- a/drivers/infiniband/hw/cxgb4/device.c
+++ b/drivers/infiniband/hw/cxgb4/device.c
@@ -60,7 +60,7 @@ int c4iw_wr_log = 0;
 module_param(c4iw_wr_log, int, 0444);
 MODULE_PARM_DESC(c4iw_wr_log, "Enables logging of work request timing data.");
 
-int c4iw_wr_log_size_order = 12;
+static int c4iw_wr_log_size_order = 12;
 module_param(c4iw_wr_log_size_order, int, 0444);
 MODULE_PARM_DESC(c4iw_wr_log_size_order,
 		 "Number of entries (log2) in the work request timing log.");
diff --git a/drivers/infiniband/hw/ipath/ipath_fs.c b/drivers/infiniband/hw/ipath/ipath_fs.c
index e0c404bdc4a8..4977082e081f 100644
--- a/drivers/infiniband/hw/ipath/ipath_fs.c
+++ b/drivers/infiniband/hw/ipath/ipath_fs.c
@@ -82,7 +82,6 @@ static int create_file(const char *name, umode_t mode,
 {
 	int error;
 
-	*dentry = NULL;
 	mutex_lock(&parent->d_inode->i_mutex);
 	*dentry = lookup_one_len(name, parent, strlen(name));
 	if (!IS_ERR(*dentry))
diff --git a/drivers/infiniband/hw/ipath/ipath_user_pages.c b/drivers/infiniband/hw/ipath/ipath_user_pages.c
index dc66c4506916..1da1252dcdb3 100644
--- a/drivers/infiniband/hw/ipath/ipath_user_pages.c
+++ b/drivers/infiniband/hw/ipath/ipath_user_pages.c
@@ -54,7 +54,7 @@ static void __ipath_release_user_pages(struct page **p, size_t num_pages,
 
 /* call with current->mm->mmap_sem held */
 static int __ipath_get_user_pages(unsigned long start_page, size_t num_pages,
-				  struct page **p, struct vm_area_struct **vma)
+				  struct page **p)
 {
 	unsigned long lock_limit;
 	size_t got;
@@ -74,7 +74,7 @@ static int __ipath_get_user_pages(unsigned long start_page, size_t num_pages,
 		ret = get_user_pages(current, current->mm,
 				     start_page + got * PAGE_SIZE,
 				     num_pages - got, 1, 1,
-				     p + got, vma);
+				     p + got, NULL);
 		if (ret < 0)
 			goto bail_release;
 	}
@@ -165,7 +165,7 @@ int ipath_get_user_pages(unsigned long start_page, size_t num_pages,
 
 	down_write(&current->mm->mmap_sem);
 
-	ret = __ipath_get_user_pages(start_page, num_pages, p, NULL);
+	ret = __ipath_get_user_pages(start_page, num_pages, p);
 
 	up_write(&current->mm->mmap_sem);
 
diff --git a/drivers/infiniband/hw/mlx4/main.c b/drivers/infiniband/hw/mlx4/main.c
index e1e558a3d692..bda5994ceb68 100644
--- a/drivers/infiniband/hw/mlx4/main.c
+++ b/drivers/infiniband/hw/mlx4/main.c
@@ -59,6 +59,7 @@
 
 #define MLX4_IB_FLOW_MAX_PRIO 0xFFF
 #define MLX4_IB_FLOW_QPN_MASK 0xFFFFFF
+#define MLX4_IB_CARD_REV_A0   0xA0
 
 MODULE_AUTHOR("Roland Dreier");
 MODULE_DESCRIPTION("Mellanox ConnectX HCA InfiniBand driver");
@@ -119,6 +120,17 @@ static int check_flow_steering_support(struct mlx4_dev *dev)
 	return dmfs;
 }
 
+static int num_ib_ports(struct mlx4_dev *dev)
+{
+	int ib_ports = 0;
+	int i;
+
+	mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB)
+		ib_ports++;
+
+	return ib_ports;
+}
+
 static int mlx4_ib_query_device(struct ib_device *ibdev,
 				struct ib_device_attr *props)
 {
@@ -126,6 +138,7 @@ static int mlx4_ib_query_device(struct ib_device *ibdev,
 	struct ib_smp *in_mad  = NULL;
 	struct ib_smp *out_mad = NULL;
 	int err = -ENOMEM;
+	int have_ib_ports;
 
 	in_mad  = kzalloc(sizeof *in_mad, GFP_KERNEL);
 	out_mad = kmalloc(sizeof *out_mad, GFP_KERNEL);
@@ -142,6 +155,8 @@ static int mlx4_ib_query_device(struct ib_device *ibdev,
 
 	memset(props, 0, sizeof *props);
 
+	have_ib_ports = num_ib_ports(dev->dev);
+
 	props->fw_ver = dev->dev->caps.fw_ver;
 	props->device_cap_flags    = IB_DEVICE_CHANGE_PHY_PORT |
 		IB_DEVICE_PORT_ACTIVE_EVENT		|
@@ -152,13 +167,15 @@ static int mlx4_ib_query_device(struct ib_device *ibdev,
 		props->device_cap_flags |= IB_DEVICE_BAD_PKEY_CNTR;
 	if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_BAD_QKEY_CNTR)
 		props->device_cap_flags |= IB_DEVICE_BAD_QKEY_CNTR;
-	if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_APM)
+	if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_APM && have_ib_ports)
 		props->device_cap_flags |= IB_DEVICE_AUTO_PATH_MIG;
 	if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_UD_AV_PORT)
 		props->device_cap_flags |= IB_DEVICE_UD_AV_PORT_ENFORCE;
 	if (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_IPOIB_CSUM)
 		props->device_cap_flags |= IB_DEVICE_UD_IP_CSUM;
-	if (dev->dev->caps.max_gso_sz && dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_BLH)
+	if (dev->dev->caps.max_gso_sz &&
+	    (dev->dev->rev_id != MLX4_IB_CARD_REV_A0) &&
+	    (dev->dev->caps.flags & MLX4_DEV_CAP_FLAG_BLH))
 		props->device_cap_flags |= IB_DEVICE_UD_TSO;
 	if (dev->dev->caps.bmme_flags & MLX4_BMME_FLAG_RESERVED_LKEY)
 		props->device_cap_flags |= IB_DEVICE_LOCAL_DMA_LKEY;
@@ -357,7 +374,7 @@ static int eth_link_query_port(struct ib_device *ibdev, u8 port,
 	props->state		= IB_PORT_DOWN;
 	props->phys_state	= state_to_phys_state(props->state);
 	props->active_mtu	= IB_MTU_256;
-	spin_lock(&iboe->lock);
+	spin_lock_bh(&iboe->lock);
 	ndev = iboe->netdevs[port - 1];
 	if (!ndev)
 		goto out_unlock;
@@ -369,7 +386,7 @@ static int eth_link_query_port(struct ib_device *ibdev, u8 port,
 					IB_PORT_ACTIVE : IB_PORT_DOWN;
 	props->phys_state	= state_to_phys_state(props->state);
 out_unlock:
-	spin_unlock(&iboe->lock);
+	spin_unlock_bh(&iboe->lock);
 out:
 	mlx4_free_cmd_mailbox(mdev->dev, mailbox);
 	return err;
@@ -811,11 +828,11 @@ int mlx4_ib_add_mc(struct mlx4_ib_dev *mdev, struct mlx4_ib_qp *mqp,
 	if (!mqp->port)
 		return 0;
 
-	spin_lock(&mdev->iboe.lock);
+	spin_lock_bh(&mdev->iboe.lock);
 	ndev = mdev->iboe.netdevs[mqp->port - 1];
 	if (ndev)
 		dev_hold(ndev);
-	spin_unlock(&mdev->iboe.lock);
+	spin_unlock_bh(&mdev->iboe.lock);
 
 	if (ndev) {
 		ret = 1;
@@ -1089,6 +1106,30 @@ static int __mlx4_ib_destroy_flow(struct mlx4_dev *dev, u64 reg_id)
 	return err;
 }
 
+static int mlx4_ib_tunnel_steer_add(struct ib_qp *qp, struct ib_flow_attr *flow_attr,
+				    u64 *reg_id)
+{
+	void *ib_flow;
+	union ib_flow_spec *ib_spec;
+	struct mlx4_dev	*dev = to_mdev(qp->device)->dev;
+	int err = 0;
+
+	if (dev->caps.tunnel_offload_mode != MLX4_TUNNEL_OFFLOAD_MODE_VXLAN)
+		return 0; /* do nothing */
+
+	ib_flow = flow_attr + 1;
+	ib_spec = (union ib_flow_spec *)ib_flow;
+
+	if (ib_spec->type !=  IB_FLOW_SPEC_ETH || flow_attr->num_of_specs != 1)
+		return 0; /* do nothing */
+
+	err = mlx4_tunnel_steer_add(to_mdev(qp->device)->dev, ib_spec->eth.val.dst_mac,
+				    flow_attr->port, qp->qp_num,
+				    MLX4_DOMAIN_UVERBS | (flow_attr->priority & 0xff),
+				    reg_id);
+	return err;
+}
+
 static struct ib_flow *mlx4_ib_create_flow(struct ib_qp *qp,
 				    struct ib_flow_attr *flow_attr,
 				    int domain)
@@ -1136,6 +1177,12 @@ static struct ib_flow *mlx4_ib_create_flow(struct ib_qp *qp,
 		i++;
 	}
 
+	if (i < ARRAY_SIZE(type) && flow_attr->type == IB_FLOW_ATTR_NORMAL) {
+		err = mlx4_ib_tunnel_steer_add(qp, flow_attr, &mflow->reg_id[i]);
+		if (err)
+			goto err_free;
+	}
+
 	return &mflow->ibflow;
 
 err_free:
@@ -1262,11 +1309,11 @@ static int mlx4_ib_mcg_detach(struct ib_qp *ibqp, union ib_gid *gid, u16 lid)
 	mutex_lock(&mqp->mutex);
 	ge = find_gid_entry(mqp, gid->raw);
 	if (ge) {
-		spin_lock(&mdev->iboe.lock);
+		spin_lock_bh(&mdev->iboe.lock);
 		ndev = ge->added ? mdev->iboe.netdevs[ge->port - 1] : NULL;
 		if (ndev)
 			dev_hold(ndev);
-		spin_unlock(&mdev->iboe.lock);
+		spin_unlock_bh(&mdev->iboe.lock);
 		if (ndev)
 			dev_put(ndev);
 		list_del(&ge->list);
@@ -1387,6 +1434,9 @@ static void update_gids_task(struct work_struct *work)
 	int err;
 	struct mlx4_dev	*dev = gw->dev->dev;
 
+	if (!gw->dev->ib_active)
+		return;
+
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox)) {
 		pr_warn("update gid table failed %ld\n", PTR_ERR(mailbox));
@@ -1417,6 +1467,9 @@ static void reset_gids_task(struct work_struct *work)
 	int err;
 	struct mlx4_dev	*dev = gw->dev->dev;
 
+	if (!gw->dev->ib_active)
+		return;
+
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox)) {
 		pr_warn("reset gid table failed\n");
@@ -1551,7 +1604,7 @@ static int mlx4_ib_addr_event(int event, struct net_device *event_netdev,
 		return 0;
 
 	iboe = &ibdev->iboe;
-	spin_lock(&iboe->lock);
+	spin_lock_bh(&iboe->lock);
 
 	for (port = 1; port <= ibdev->dev->caps.num_ports; ++port)
 		if ((netif_is_bond_master(real_dev) &&
@@ -1561,7 +1614,7 @@ static int mlx4_ib_addr_event(int event, struct net_device *event_netdev,
 			update_gid_table(ibdev, port, gid,
 					 event == NETDEV_DOWN, 0);
 
-	spin_unlock(&iboe->lock);
+	spin_unlock_bh(&iboe->lock);
 	return 0;
 
 }
@@ -1634,13 +1687,21 @@ static void mlx4_ib_update_qps(struct mlx4_ib_dev *ibdev,
 	new_smac = mlx4_mac_to_u64(dev->dev_addr);
 	read_unlock(&dev_base_lock);
 
+	atomic64_set(&ibdev->iboe.mac[port - 1], new_smac);
+
+	/* no need for update QP1 and mac registration in non-SRIOV */
+	if (!mlx4_is_mfunc(ibdev->dev))
+		return;
+
 	mutex_lock(&ibdev->qp1_proxy_lock[port - 1]);
 	qp = ibdev->qp1_proxy[port - 1];
 	if (qp) {
 		int new_smac_index;
-		u64 old_smac = qp->pri.smac;
+		u64 old_smac;
 		struct mlx4_update_qp_params update_params;
 
+		mutex_lock(&qp->mutex);
+		old_smac = qp->pri.smac;
 		if (new_smac == old_smac)
 			goto unlock;
 
@@ -1650,22 +1711,25 @@ static void mlx4_ib_update_qps(struct mlx4_ib_dev *ibdev,
 			goto unlock;
 
 		update_params.smac_index = new_smac_index;
-		if (mlx4_update_qp(ibdev->dev, &qp->mqp, MLX4_UPDATE_QP_SMAC,
+		if (mlx4_update_qp(ibdev->dev, qp->mqp.qpn, MLX4_UPDATE_QP_SMAC,
 				   &update_params)) {
 			release_mac = new_smac;
 			goto unlock;
 		}
-
+		/* if old port was zero, no mac was yet registered for this QP */
+		if (qp->pri.smac_port)
+			release_mac = old_smac;
 		qp->pri.smac = new_smac;
+		qp->pri.smac_port = port;
 		qp->pri.smac_index = new_smac_index;
-
-		release_mac = old_smac;
 	}
 
 unlock:
-	mutex_unlock(&ibdev->qp1_proxy_lock[port - 1]);
 	if (release_mac != MLX4_IB_INVALID_MAC)
 		mlx4_unregister_mac(ibdev->dev, port, release_mac);
+	if (qp)
+		mutex_unlock(&qp->mutex);
+	mutex_unlock(&ibdev->qp1_proxy_lock[port - 1]);
 }
 
 static void mlx4_ib_get_dev_addr(struct net_device *dev,
@@ -1676,6 +1740,7 @@ static void mlx4_ib_get_dev_addr(struct net_device *dev,
 	struct inet6_dev *in6_dev;
 	union ib_gid  *pgid;
 	struct inet6_ifaddr *ifp;
+	union ib_gid default_gid;
 #endif
 	union ib_gid gid;
 
@@ -1696,12 +1761,15 @@ static void mlx4_ib_get_dev_addr(struct net_device *dev,
 		in_dev_put(in_dev);
 	}
 #if IS_ENABLED(CONFIG_IPV6)
+	mlx4_make_default_gid(dev, &default_gid);
 	/* IPv6 gids */
 	in6_dev = in6_dev_get(dev);
 	if (in6_dev) {
 		read_lock_bh(&in6_dev->lock);
 		list_for_each_entry(ifp, &in6_dev->addr_list, if_list) {
 			pgid = (union ib_gid *)&ifp->addr;
+			if (!memcmp(pgid, &default_gid, sizeof(*pgid)))
+				continue;
 			update_gid_table(ibdev, port, pgid, 0, 0);
 		}
 		read_unlock_bh(&in6_dev->lock);
@@ -1723,24 +1791,33 @@ static int mlx4_ib_init_gid_table(struct mlx4_ib_dev *ibdev)
 	struct	net_device *dev;
 	struct mlx4_ib_iboe *iboe = &ibdev->iboe;
 	int i;
+	int err = 0;
 
-	for (i = 1; i <= ibdev->num_ports; ++i)
-		if (reset_gid_table(ibdev, i))
-			return -1;
+	for (i = 1; i <= ibdev->num_ports; ++i) {
+		if (rdma_port_get_link_layer(&ibdev->ib_dev, i) ==
+		    IB_LINK_LAYER_ETHERNET) {
+			err = reset_gid_table(ibdev, i);
+			if (err)
+				goto out;
+		}
+	}
 
 	read_lock(&dev_base_lock);
-	spin_lock(&iboe->lock);
+	spin_lock_bh(&iboe->lock);
 
 	for_each_netdev(&init_net, dev) {
 		u8 port = mlx4_ib_get_dev_port(dev, ibdev);
-		if (port)
+		/* port will be non-zero only for ETH ports */
+		if (port) {
+			mlx4_ib_set_default_gid(ibdev, dev, port);
 			mlx4_ib_get_dev_addr(dev, ibdev, port);
+		}
 	}
 
-	spin_unlock(&iboe->lock);
+	spin_unlock_bh(&iboe->lock);
 	read_unlock(&dev_base_lock);
-
-	return 0;
+out:
+	return err;
 }
 
 static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev,
@@ -1754,7 +1831,7 @@ static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev,
 
 	iboe = &ibdev->iboe;
 
-	spin_lock(&iboe->lock);
+	spin_lock_bh(&iboe->lock);
 	mlx4_foreach_ib_transport_port(port, ibdev->dev) {
 		enum ib_port_state	port_state = IB_PORT_NOP;
 		struct net_device *old_master = iboe->masters[port - 1];
@@ -1786,35 +1863,47 @@ static void mlx4_ib_scan_netdevs(struct mlx4_ib_dev *ibdev,
 			port_state = (netif_running(curr_netdev) && netif_carrier_ok(curr_netdev)) ?
 						IB_PORT_ACTIVE : IB_PORT_DOWN;
 			mlx4_ib_set_default_gid(ibdev, curr_netdev, port);
-		} else {
-			reset_gid_table(ibdev, port);
-		}
-		/* if using bonding/team and a slave port is down, we don't the bond IP
-		 * based gids in the table since flows that select port by gid may get
-		 * the down port.
-		 */
-		if (curr_master && (port_state == IB_PORT_DOWN)) {
-			reset_gid_table(ibdev, port);
-			mlx4_ib_set_default_gid(ibdev, curr_netdev, port);
-		}
-		/* if bonding is used it is possible that we add it to masters
-		 * only after IP address is assigned to the net bonding
-		 * interface.
-		*/
-		if (curr_master && (old_master != curr_master)) {
-			reset_gid_table(ibdev, port);
-			mlx4_ib_set_default_gid(ibdev, curr_netdev, port);
-			mlx4_ib_get_dev_addr(curr_master, ibdev, port);
-		}
+			if (curr_master) {
+				/* if using bonding/team and a slave port is down, we
+				 * don't want the bond IP based gids in the table since
+				 * flows that select port by gid may get the down port.
+				*/
+				if (port_state == IB_PORT_DOWN) {
+					reset_gid_table(ibdev, port);
+					mlx4_ib_set_default_gid(ibdev,
+								curr_netdev,
+								port);
+				} else {
+					/* gids from the upper dev (bond/team)
+					 * should appear in port's gid table
+					*/
+					mlx4_ib_get_dev_addr(curr_master,
+							     ibdev, port);
+				}
+			}
+			/* if bonding is used it is possible that we add it to
+			 * masters only after IP address is assigned to the
+			 * net bonding interface.
+			*/
+			if (curr_master && (old_master != curr_master)) {
+				reset_gid_table(ibdev, port);
+				mlx4_ib_set_default_gid(ibdev,
+							curr_netdev, port);
+				mlx4_ib_get_dev_addr(curr_master, ibdev, port);
+			}
 
-		if (!curr_master && (old_master != curr_master)) {
+			if (!curr_master && (old_master != curr_master)) {
+				reset_gid_table(ibdev, port);
+				mlx4_ib_set_default_gid(ibdev,
+							curr_netdev, port);
+				mlx4_ib_get_dev_addr(curr_netdev, ibdev, port);
+			}
+		} else {
 			reset_gid_table(ibdev, port);
-			mlx4_ib_set_default_gid(ibdev, curr_netdev, port);
-			mlx4_ib_get_dev_addr(curr_netdev, ibdev, port);
 		}
 	}
 
-	spin_unlock(&iboe->lock);
+	spin_unlock_bh(&iboe->lock);
 
 	if (update_qps_port > 0)
 		mlx4_ib_update_qps(ibdev, dev, update_qps_port);
@@ -2156,6 +2245,9 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 			goto err_steer_free_bitmap;
 	}
 
+	for (j = 1; j <= ibdev->dev->caps.num_ports; j++)
+		atomic64_set(&iboe->mac[j - 1], ibdev->dev->caps.def_mac[j]);
+
 	if (ib_register_device(&ibdev->ib_dev, NULL))
 		goto err_steer_free_bitmap;
 
@@ -2192,12 +2284,8 @@ static void *mlx4_ib_add(struct mlx4_dev *dev)
 			}
 		}
 #endif
-		for (i = 1 ; i <= ibdev->num_ports ; ++i)
-			reset_gid_table(ibdev, i);
-		rtnl_lock();
-		mlx4_ib_scan_netdevs(ibdev, NULL, 0);
-		rtnl_unlock();
-		mlx4_ib_init_gid_table(ibdev);
+		if (mlx4_ib_init_gid_table(ibdev))
+			goto err_notif;
 	}
 
 	for (j = 0; j < ARRAY_SIZE(mlx4_class_attributes); ++j) {
@@ -2345,6 +2433,9 @@ static void mlx4_ib_remove(struct mlx4_dev *dev, void *ibdev_ptr)
 	struct mlx4_ib_dev *ibdev = ibdev_ptr;
 	int p;
 
+	ibdev->ib_active = false;
+	flush_workqueue(wq);
+
 	mlx4_ib_close_sriov(ibdev);
 	mlx4_ib_mad_cleanup(ibdev);
 	ib_unregister_device(&ibdev->ib_dev);
diff --git a/drivers/infiniband/hw/mlx4/mlx4_ib.h b/drivers/infiniband/hw/mlx4/mlx4_ib.h
index e8cad3926bfc..6eb743f65f6f 100644
--- a/drivers/infiniband/hw/mlx4/mlx4_ib.h
+++ b/drivers/infiniband/hw/mlx4/mlx4_ib.h
@@ -451,6 +451,7 @@ struct mlx4_ib_iboe {
 	spinlock_t		lock;
 	struct net_device      *netdevs[MLX4_MAX_PORTS];
 	struct net_device      *masters[MLX4_MAX_PORTS];
+	atomic64_t		mac[MLX4_MAX_PORTS];
 	struct notifier_block 	nb;
 	struct notifier_block	nb_inet;
 	struct notifier_block	nb_inet6;
diff --git a/drivers/infiniband/hw/mlx4/mr.c b/drivers/infiniband/hw/mlx4/mr.c
index 9b0e80e59b08..8f9325cfc85d 100644
--- a/drivers/infiniband/hw/mlx4/mr.c
+++ b/drivers/infiniband/hw/mlx4/mr.c
@@ -234,14 +234,13 @@ int mlx4_ib_rereg_user_mr(struct ib_mr *mr, int flags,
 					0);
 		if (IS_ERR(mmr->umem)) {
 			err = PTR_ERR(mmr->umem);
+			/* Prevent mlx4_ib_dereg_mr from free'ing invalid pointer */
 			mmr->umem = NULL;
 			goto release_mpt_entry;
 		}
 		n = ib_umem_page_count(mmr->umem);
 		shift = ilog2(mmr->umem->page_size);
 
-		mmr->mmr.iova       = virt_addr;
-		mmr->mmr.size       = length;
 		err = mlx4_mr_rereg_mem_write(dev->dev, &mmr->mmr,
 					      virt_addr, length, n, shift,
 					      *pmpt_entry);
@@ -249,6 +248,8 @@ int mlx4_ib_rereg_user_mr(struct ib_mr *mr, int flags,
 			ib_umem_release(mmr->umem);
 			goto release_mpt_entry;
 		}
+		mmr->mmr.iova       = virt_addr;
+		mmr->mmr.size       = length;
 
 		err = mlx4_ib_umem_write_mtt(dev, &mmr->mmr.mtt, mmr->umem);
 		if (err) {
@@ -262,6 +263,8 @@ int mlx4_ib_rereg_user_mr(struct ib_mr *mr, int flags,
 	 * return a failure. But dereg_mr will free the resources.
 	 */
 	err = mlx4_mr_hw_write_mpt(dev->dev, &mmr->mmr, pmpt_entry);
+	if (!err && flags & IB_MR_REREG_ACCESS)
+		mmr->mmr.access = mr_access_flags;
 
 release_mpt_entry:
 	mlx4_mr_hw_put_mpt(dev->dev, pmpt_entry);
diff --git a/drivers/infiniband/hw/mlx4/qp.c b/drivers/infiniband/hw/mlx4/qp.c
index 67780452f0cf..9c5150c3cb31 100644
--- a/drivers/infiniband/hw/mlx4/qp.c
+++ b/drivers/infiniband/hw/mlx4/qp.c
@@ -964,9 +964,10 @@ static void destroy_qp_common(struct mlx4_ib_dev *dev, struct mlx4_ib_qp *qp,
 				   MLX4_QP_STATE_RST, NULL, 0, 0, &qp->mqp))
 			pr_warn("modify QP %06x to RESET failed.\n",
 			       qp->mqp.qpn);
-		if (qp->pri.smac) {
+		if (qp->pri.smac || (!qp->pri.smac && qp->pri.smac_port)) {
 			mlx4_unregister_mac(dev->dev, qp->pri.smac_port, qp->pri.smac);
 			qp->pri.smac = 0;
+			qp->pri.smac_port = 0;
 		}
 		if (qp->alt.smac) {
 			mlx4_unregister_mac(dev->dev, qp->alt.smac_port, qp->alt.smac);
@@ -1325,7 +1326,8 @@ static int _mlx4_set_path(struct mlx4_ib_dev *dev, const struct ib_ah_attr *ah,
 		 * If one was already assigned, but the new mac differs,
 		 * unregister the old one and register the new one.
 		*/
-		if (!smac_info->smac || smac_info->smac != smac) {
+		if ((!smac_info->smac && !smac_info->smac_port) ||
+		    smac_info->smac != smac) {
 			/* register candidate now, unreg if needed, after success */
 			smac_index = mlx4_register_mac(dev->dev, port, smac);
 			if (smac_index >= 0) {
@@ -1390,21 +1392,13 @@ static void update_mcg_macs(struct mlx4_ib_dev *dev, struct mlx4_ib_qp *qp)
 static int handle_eth_ud_smac_index(struct mlx4_ib_dev *dev, struct mlx4_ib_qp *qp, u8 *smac,
 				    struct mlx4_qp_context *context)
 {
-	struct net_device *ndev;
 	u64 u64_mac;
 	int smac_index;
 
-
-	ndev = dev->iboe.netdevs[qp->port - 1];
-	if (ndev) {
-		smac = ndev->dev_addr;
-		u64_mac = mlx4_mac_to_u64(smac);
-	} else {
-		u64_mac = dev->dev->caps.def_mac[qp->port];
-	}
+	u64_mac = atomic64_read(&dev->iboe.mac[qp->port - 1]);
 
 	context->pri_path.sched_queue = MLX4_IB_DEFAULT_SCHED_QUEUE | ((qp->port - 1) << 6);
-	if (!qp->pri.smac) {
+	if (!qp->pri.smac && !qp->pri.smac_port) {
 		smac_index = mlx4_register_mac(dev->dev, qp->port, u64_mac);
 		if (smac_index >= 0) {
 			qp->pri.candidate_smac_index = smac_index;
@@ -1432,6 +1426,12 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 	int steer_qp = 0;
 	int err = -EINVAL;
 
+	/* APM is not supported under RoCE */
+	if (attr_mask & IB_QP_ALT_PATH &&
+	    rdma_port_get_link_layer(&dev->ib_dev, qp->port) ==
+	    IB_LINK_LAYER_ETHERNET)
+		return -ENOTSUPP;
+
 	context = kzalloc(sizeof *context, GFP_KERNEL);
 	if (!context)
 		return -ENOMEM;
@@ -1677,9 +1677,15 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 		}
 	}
 
-	if (qp->ibqp.qp_type == IB_QPT_RAW_PACKET)
+	if (qp->ibqp.qp_type == IB_QPT_RAW_PACKET) {
 		context->pri_path.ackto = (context->pri_path.ackto & 0xf8) |
 					MLX4_IB_LINK_TYPE_ETH;
+		if (dev->dev->caps.tunnel_offload_mode ==  MLX4_TUNNEL_OFFLOAD_MODE_VXLAN) {
+			/* set QP to receive both tunneled & non-tunneled packets */
+			if (!(context->flags & cpu_to_be32(1 << MLX4_RSS_QPC_FLAG_OFFSET)))
+				context->srqn = cpu_to_be32(7 << 28);
+		}
+	}
 
 	if (ibqp->qp_type == IB_QPT_UD && (new_state == IB_QPS_RTR)) {
 		int is_eth = rdma_port_get_link_layer(
@@ -1780,9 +1786,10 @@ static int __mlx4_ib_modify_qp(struct ib_qp *ibqp,
 			if (qp->flags & MLX4_IB_QP_NETIF)
 				mlx4_ib_steer_qp_reg(dev, qp, 0);
 		}
-		if (qp->pri.smac) {
+		if (qp->pri.smac || (!qp->pri.smac && qp->pri.smac_port)) {
 			mlx4_unregister_mac(dev->dev, qp->pri.smac_port, qp->pri.smac);
 			qp->pri.smac = 0;
+			qp->pri.smac_port = 0;
 		}
 		if (qp->alt.smac) {
 			mlx4_unregister_mac(dev->dev, qp->alt.smac_port, qp->alt.smac);
@@ -1806,11 +1813,12 @@ out:
 	if (err && steer_qp)
 		mlx4_ib_steer_qp_reg(dev, qp, 0);
 	kfree(context);
-	if (qp->pri.candidate_smac) {
+	if (qp->pri.candidate_smac ||
+	    (!qp->pri.candidate_smac && qp->pri.candidate_smac_port)) {
 		if (err) {
 			mlx4_unregister_mac(dev->dev, qp->pri.candidate_smac_port, qp->pri.candidate_smac);
 		} else {
-			if (qp->pri.smac)
+			if (qp->pri.smac || (!qp->pri.smac && qp->pri.smac_port))
 				mlx4_unregister_mac(dev->dev, qp->pri.smac_port, qp->pri.smac);
 			qp->pri.smac = qp->pri.candidate_smac;
 			qp->pri.smac_index = qp->pri.candidate_smac_index;
@@ -2083,6 +2091,16 @@ static int build_sriov_qp0_header(struct mlx4_ib_sqp *sqp,
 	return 0;
 }
 
+static void mlx4_u64_to_smac(u8 *dst_mac, u64 src_mac)
+{
+	int i;
+
+	for (i = ETH_ALEN; i; i--) {
+		dst_mac[i - 1] = src_mac & 0xff;
+		src_mac >>= 8;
+	}
+}
+
 static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_send_wr *wr,
 			    void *wqe, unsigned *mlx_seg_len)
 {
@@ -2197,7 +2215,6 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_send_wr *wr,
 	}
 
 	if (is_eth) {
-		u8 *smac;
 		struct in6_addr in6;
 
 		u16 pcp = (be32_to_cpu(ah->av.ib.sl_tclass_flowlabel) >> 29) << 13;
@@ -2210,12 +2227,17 @@ static int build_mlx_header(struct mlx4_ib_sqp *sqp, struct ib_send_wr *wr,
 		memcpy(&ctrl->imm, ah->av.eth.mac + 2, 4);
 		memcpy(&in6, sgid.raw, sizeof(in6));
 
-		if (!mlx4_is_mfunc(to_mdev(ib_dev)->dev))
-			smac = to_mdev(sqp->qp.ibqp.device)->
-				iboe.netdevs[sqp->qp.port - 1]->dev_addr;
-		else	/* use the src mac of the tunnel */
-			smac = ah->av.eth.s_mac;
-		memcpy(sqp->ud_header.eth.smac_h, smac, 6);
+		if (!mlx4_is_mfunc(to_mdev(ib_dev)->dev)) {
+			u64 mac = atomic64_read(&to_mdev(ib_dev)->iboe.mac[sqp->qp.port - 1]);
+			u8 smac[ETH_ALEN];
+
+			mlx4_u64_to_smac(smac, mac);
+			memcpy(sqp->ud_header.eth.smac_h, smac, ETH_ALEN);
+		} else {
+			/* use the src mac of the tunnel */
+			memcpy(sqp->ud_header.eth.smac_h, ah->av.eth.s_mac, ETH_ALEN);
+		}
+
 		if (!memcmp(sqp->ud_header.eth.smac_h, sqp->ud_header.eth.dmac_h, 6))
 			mlx->flags |= cpu_to_be32(MLX4_WQE_CTRL_FORCE_LOOPBACK);
 		if (!is_vlan) {
diff --git a/drivers/infiniband/hw/mlx5/cq.c b/drivers/infiniband/hw/mlx5/cq.c
index e4056279166d..10cfce5119a9 100644
--- a/drivers/infiniband/hw/mlx5/cq.c
+++ b/drivers/infiniband/hw/mlx5/cq.c
@@ -752,7 +752,7 @@ struct ib_cq *mlx5_ib_create_cq(struct ib_device *ibdev, int entries,
 		return ERR_PTR(-EINVAL);
 
 	entries = roundup_pow_of_two(entries + 1);
-	if (entries > dev->mdev->caps.max_cqes)
+	if (entries > dev->mdev->caps.gen.max_cqes)
 		return ERR_PTR(-EINVAL);
 
 	cq = kzalloc(sizeof(*cq), GFP_KERNEL);
@@ -919,7 +919,7 @@ int mlx5_ib_modify_cq(struct ib_cq *cq, u16 cq_count, u16 cq_period)
 	int err;
 	u32 fsel;
 
-	if (!(dev->mdev->caps.flags & MLX5_DEV_CAP_FLAG_CQ_MODER))
+	if (!(dev->mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_CQ_MODER))
 		return -ENOSYS;
 
 	in = kzalloc(sizeof(*in), GFP_KERNEL);
@@ -1074,7 +1074,7 @@ int mlx5_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 	int uninitialized_var(cqe_size);
 	unsigned long flags;
 
-	if (!(dev->mdev->caps.flags & MLX5_DEV_CAP_FLAG_RESIZE_CQ)) {
+	if (!(dev->mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_RESIZE_CQ)) {
 		pr_info("Firmware does not support resize CQ\n");
 		return -ENOSYS;
 	}
@@ -1083,7 +1083,7 @@ int mlx5_ib_resize_cq(struct ib_cq *ibcq, int entries, struct ib_udata *udata)
 		return -EINVAL;
 
 	entries = roundup_pow_of_two(entries + 1);
-	if (entries > dev->mdev->caps.max_cqes + 1)
+	if (entries > dev->mdev->caps.gen.max_cqes + 1)
 		return -EINVAL;
 
 	if (entries == ibcq->cqe + 1)
diff --git a/drivers/infiniband/hw/mlx5/mad.c b/drivers/infiniband/hw/mlx5/mad.c
index b514bbb5610f..657af9a1167c 100644
--- a/drivers/infiniband/hw/mlx5/mad.c
+++ b/drivers/infiniband/hw/mlx5/mad.c
@@ -129,7 +129,7 @@ int mlx5_query_ext_port_caps(struct mlx5_ib_dev *dev, u8 port)
 
 	packet_error = be16_to_cpu(out_mad->status);
 
-	dev->mdev->caps.ext_port_cap[port - 1] = (!err && !packet_error) ?
+	dev->mdev->caps.gen.ext_port_cap[port - 1] = (!err && !packet_error) ?
 		MLX_EXT_PORT_CAP_FLAG_EXTENDED_PORT_INFO : 0;
 
 out:
diff --git a/drivers/infiniband/hw/mlx5/main.c b/drivers/infiniband/hw/mlx5/main.c
index d8907b20522a..1ba6c42e4df8 100644
--- a/drivers/infiniband/hw/mlx5/main.c
+++ b/drivers/infiniband/hw/mlx5/main.c
@@ -157,11 +157,13 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
 	struct ib_smp *in_mad  = NULL;
 	struct ib_smp *out_mad = NULL;
+	struct mlx5_general_caps *gen;
 	int err = -ENOMEM;
 	int max_rq_sg;
 	int max_sq_sg;
 	u64 flags;
 
+	gen = &dev->mdev->caps.gen;
 	in_mad  = kzalloc(sizeof(*in_mad), GFP_KERNEL);
 	out_mad = kmalloc(sizeof(*out_mad), GFP_KERNEL);
 	if (!in_mad || !out_mad)
@@ -183,7 +185,7 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 		IB_DEVICE_PORT_ACTIVE_EVENT		|
 		IB_DEVICE_SYS_IMAGE_GUID		|
 		IB_DEVICE_RC_RNR_NAK_GEN;
-	flags = dev->mdev->caps.flags;
+	flags = gen->flags;
 	if (flags & MLX5_DEV_CAP_FLAG_BAD_PKEY_CNTR)
 		props->device_cap_flags |= IB_DEVICE_BAD_PKEY_CNTR;
 	if (flags & MLX5_DEV_CAP_FLAG_BAD_QKEY_CNTR)
@@ -213,30 +215,31 @@ static int mlx5_ib_query_device(struct ib_device *ibdev,
 	memcpy(&props->sys_image_guid, out_mad->data +	4, 8);
 
 	props->max_mr_size	   = ~0ull;
-	props->page_size_cap	   = dev->mdev->caps.min_page_sz;
-	props->max_qp		   = 1 << dev->mdev->caps.log_max_qp;
-	props->max_qp_wr	   = dev->mdev->caps.max_wqes;
-	max_rq_sg = dev->mdev->caps.max_rq_desc_sz / sizeof(struct mlx5_wqe_data_seg);
-	max_sq_sg = (dev->mdev->caps.max_sq_desc_sz - sizeof(struct mlx5_wqe_ctrl_seg)) /
+	props->page_size_cap	   = gen->min_page_sz;
+	props->max_qp		   = 1 << gen->log_max_qp;
+	props->max_qp_wr	   = gen->max_wqes;
+	max_rq_sg = gen->max_rq_desc_sz / sizeof(struct mlx5_wqe_data_seg);
+	max_sq_sg = (gen->max_sq_desc_sz - sizeof(struct mlx5_wqe_ctrl_seg)) /
 		sizeof(struct mlx5_wqe_data_seg);
 	props->max_sge = min(max_rq_sg, max_sq_sg);
-	props->max_cq		   = 1 << dev->mdev->caps.log_max_cq;
-	props->max_cqe		   = dev->mdev->caps.max_cqes - 1;
-	props->max_mr		   = 1 << dev->mdev->caps.log_max_mkey;
-	props->max_pd		   = 1 << dev->mdev->caps.log_max_pd;
-	props->max_qp_rd_atom	   = dev->mdev->caps.max_ra_req_qp;
-	props->max_qp_init_rd_atom = dev->mdev->caps.max_ra_res_qp;
+	props->max_cq		   = 1 << gen->log_max_cq;
+	props->max_cqe		   = gen->max_cqes - 1;
+	props->max_mr		   = 1 << gen->log_max_mkey;
+	props->max_pd		   = 1 << gen->log_max_pd;
+	props->max_qp_rd_atom	   = 1 << gen->log_max_ra_req_qp;
+	props->max_qp_init_rd_atom = 1 << gen->log_max_ra_res_qp;
+	props->max_srq		   = 1 << gen->log_max_srq;
+	props->max_srq_wr	   = gen->max_srq_wqes - 1;
+	props->local_ca_ack_delay  = gen->local_ca_ack_delay;
 	props->max_res_rd_atom	   = props->max_qp_rd_atom * props->max_qp;
-	props->max_srq		   = 1 << dev->mdev->caps.log_max_srq;
-	props->max_srq_wr	   = dev->mdev->caps.max_srq_wqes - 1;
 	props->max_srq_sge	   = max_rq_sg - 1;
 	props->max_fast_reg_page_list_len = (unsigned int)-1;
-	props->local_ca_ack_delay  = dev->mdev->caps.local_ca_ack_delay;
+	props->local_ca_ack_delay  = gen->local_ca_ack_delay;
 	props->atomic_cap	   = IB_ATOMIC_NONE;
 	props->masked_atomic_cap   = IB_ATOMIC_NONE;
 	props->max_pkeys	   = be16_to_cpup((__be16 *)(out_mad->data + 28));
-	props->max_mcast_grp	   = 1 << dev->mdev->caps.log_max_mcg;
-	props->max_mcast_qp_attach = dev->mdev->caps.max_qp_mcg;
+	props->max_mcast_grp	   = 1 << gen->log_max_mcg;
+	props->max_mcast_qp_attach = gen->max_qp_mcg;
 	props->max_total_mcast_qp_attach = props->max_mcast_qp_attach *
 					   props->max_mcast_grp;
 	props->max_map_per_fmr = INT_MAX; /* no limit in ConnectIB */
@@ -254,10 +257,12 @@ int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
 	struct ib_smp *in_mad  = NULL;
 	struct ib_smp *out_mad = NULL;
+	struct mlx5_general_caps *gen;
 	int ext_active_speed;
 	int err = -ENOMEM;
 
-	if (port < 1 || port > dev->mdev->caps.num_ports) {
+	gen = &dev->mdev->caps.gen;
+	if (port < 1 || port > gen->num_ports) {
 		mlx5_ib_warn(dev, "invalid port number %d\n", port);
 		return -EINVAL;
 	}
@@ -288,8 +293,8 @@ int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
 	props->phys_state	= out_mad->data[33] >> 4;
 	props->port_cap_flags	= be32_to_cpup((__be32 *)(out_mad->data + 20));
 	props->gid_tbl_len	= out_mad->data[50];
-	props->max_msg_sz	= 1 << to_mdev(ibdev)->mdev->caps.log_max_msg;
-	props->pkey_tbl_len	= to_mdev(ibdev)->mdev->caps.port[port - 1].pkey_table_len;
+	props->max_msg_sz	= 1 << gen->log_max_msg;
+	props->pkey_tbl_len	= gen->port[port - 1].pkey_table_len;
 	props->bad_pkey_cntr	= be16_to_cpup((__be16 *)(out_mad->data + 46));
 	props->qkey_viol_cntr	= be16_to_cpup((__be16 *)(out_mad->data + 48));
 	props->active_width	= out_mad->data[31] & 0xf;
@@ -316,7 +321,7 @@ int mlx5_ib_query_port(struct ib_device *ibdev, u8 port,
 
 	/* If reported active speed is QDR, check if is FDR-10 */
 	if (props->active_speed == 4) {
-		if (dev->mdev->caps.ext_port_cap[port - 1] &
+		if (gen->ext_port_cap[port - 1] &
 		    MLX_EXT_PORT_CAP_FLAG_EXTENDED_PORT_INFO) {
 			init_query_mad(in_mad);
 			in_mad->attr_id = MLX5_ATTR_EXTENDED_PORT_INFO;
@@ -470,6 +475,7 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	struct mlx5_ib_alloc_ucontext_req_v2 req;
 	struct mlx5_ib_alloc_ucontext_resp resp;
 	struct mlx5_ib_ucontext *context;
+	struct mlx5_general_caps *gen;
 	struct mlx5_uuar_info *uuari;
 	struct mlx5_uar *uars;
 	int gross_uuars;
@@ -480,6 +486,7 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	int i;
 	size_t reqlen;
 
+	gen = &dev->mdev->caps.gen;
 	if (!dev->ib_active)
 		return ERR_PTR(-EAGAIN);
 
@@ -512,14 +519,14 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 
 	num_uars = req.total_num_uuars / MLX5_NON_FP_BF_REGS_PER_PAGE;
 	gross_uuars = num_uars * MLX5_BF_REGS_PER_PAGE;
-	resp.qp_tab_size      = 1 << dev->mdev->caps.log_max_qp;
-	resp.bf_reg_size      = dev->mdev->caps.bf_reg_size;
+	resp.qp_tab_size      = 1 << gen->log_max_qp;
+	resp.bf_reg_size      = gen->bf_reg_size;
 	resp.cache_line_size  = L1_CACHE_BYTES;
-	resp.max_sq_desc_sz = dev->mdev->caps.max_sq_desc_sz;
-	resp.max_rq_desc_sz = dev->mdev->caps.max_rq_desc_sz;
-	resp.max_send_wqebb = dev->mdev->caps.max_wqes;
-	resp.max_recv_wr = dev->mdev->caps.max_wqes;
-	resp.max_srq_recv_wr = dev->mdev->caps.max_srq_wqes;
+	resp.max_sq_desc_sz = gen->max_sq_desc_sz;
+	resp.max_rq_desc_sz = gen->max_rq_desc_sz;
+	resp.max_send_wqebb = gen->max_wqes;
+	resp.max_recv_wr = gen->max_wqes;
+	resp.max_srq_recv_wr = gen->max_srq_wqes;
 
 	context = kzalloc(sizeof(*context), GFP_KERNEL);
 	if (!context)
@@ -565,7 +572,7 @@ static struct ib_ucontext *mlx5_ib_alloc_ucontext(struct ib_device *ibdev,
 	mutex_init(&context->db_page_mutex);
 
 	resp.tot_uuars = req.total_num_uuars;
-	resp.num_ports = dev->mdev->caps.num_ports;
+	resp.num_ports = gen->num_ports;
 	err = ib_copy_to_udata(udata, &resp,
 			       sizeof(resp) - sizeof(resp.reserved));
 	if (err)
@@ -650,13 +657,13 @@ static int mlx5_ib_mmap(struct ib_ucontext *ibcontext, struct vm_area_struct *vm
 			return -EINVAL;
 
 		idx = get_index(vma->vm_pgoff);
+		if (idx >= uuari->num_uars)
+			return -EINVAL;
+
 		pfn = uar_index2pfn(dev, uuari->uars[idx].index);
 		mlx5_ib_dbg(dev, "uar idx 0x%lx, pfn 0x%llx\n", idx,
 			    (unsigned long long)pfn);
 
-		if (idx >= uuari->num_uars)
-			return -EINVAL;
-
 		vma->vm_page_prot = pgprot_writecombine(vma->vm_page_prot);
 		if (io_remap_pfn_range(vma, vma->vm_start, pfn,
 				       PAGE_SIZE, vma->vm_page_prot))
@@ -967,9 +974,11 @@ static void mlx5_ib_event(struct mlx5_core_dev *dev, void *context,
 
 static void get_ext_port_caps(struct mlx5_ib_dev *dev)
 {
+	struct mlx5_general_caps *gen;
 	int port;
 
-	for (port = 1; port <= dev->mdev->caps.num_ports; port++)
+	gen = &dev->mdev->caps.gen;
+	for (port = 1; port <= gen->num_ports; port++)
 		mlx5_query_ext_port_caps(dev, port);
 }
 
@@ -977,9 +986,11 @@ static int get_port_caps(struct mlx5_ib_dev *dev)
 {
 	struct ib_device_attr *dprops = NULL;
 	struct ib_port_attr *pprops = NULL;
+	struct mlx5_general_caps *gen;
 	int err = 0;
 	int port;
 
+	gen = &dev->mdev->caps.gen;
 	pprops = kmalloc(sizeof(*pprops), GFP_KERNEL);
 	if (!pprops)
 		goto out;
@@ -994,14 +1005,14 @@ static int get_port_caps(struct mlx5_ib_dev *dev)
 		goto out;
 	}
 
-	for (port = 1; port <= dev->mdev->caps.num_ports; port++) {
+	for (port = 1; port <= gen->num_ports; port++) {
 		err = mlx5_ib_query_port(&dev->ib_dev, port, pprops);
 		if (err) {
 			mlx5_ib_warn(dev, "query_port %d failed %d\n", port, err);
 			break;
 		}
-		dev->mdev->caps.port[port - 1].pkey_table_len = dprops->max_pkeys;
-		dev->mdev->caps.port[port - 1].gid_table_len = pprops->gid_tbl_len;
+		gen->port[port - 1].pkey_table_len = dprops->max_pkeys;
+		gen->port[port - 1].gid_table_len = pprops->gid_tbl_len;
 		mlx5_ib_dbg(dev, "pkey_table_len %d, gid_table_len %d\n",
 			    dprops->max_pkeys, pprops->gid_tbl_len);
 	}
@@ -1279,8 +1290,8 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	strlcpy(dev->ib_dev.name, "mlx5_%d", IB_DEVICE_NAME_MAX);
 	dev->ib_dev.owner		= THIS_MODULE;
 	dev->ib_dev.node_type		= RDMA_NODE_IB_CA;
-	dev->ib_dev.local_dma_lkey	= mdev->caps.reserved_lkey;
-	dev->num_ports		= mdev->caps.num_ports;
+	dev->ib_dev.local_dma_lkey	= mdev->caps.gen.reserved_lkey;
+	dev->num_ports		= mdev->caps.gen.num_ports;
 	dev->ib_dev.phys_port_cnt     = dev->num_ports;
 	dev->ib_dev.num_comp_vectors	= dev->num_comp_vectors;
 	dev->ib_dev.dma_device	= &mdev->pdev->dev;
@@ -1355,7 +1366,7 @@ static void *mlx5_ib_add(struct mlx5_core_dev *mdev)
 	dev->ib_dev.free_fast_reg_page_list  = mlx5_ib_free_fast_reg_page_list;
 	dev->ib_dev.check_mr_status	= mlx5_ib_check_mr_status;
 
-	if (mdev->caps.flags & MLX5_DEV_CAP_FLAG_XRC) {
+	if (mdev->caps.gen.flags & MLX5_DEV_CAP_FLAG_XRC) {
 		dev->ib_dev.alloc_xrcd = mlx5_ib_alloc_xrcd;
 		dev->ib_dev.dealloc_xrcd = mlx5_ib_dealloc_xrcd;
 		dev->ib_dev.uverbs_cmd_mask |=
@@ -1414,8 +1425,8 @@ err_dealloc:
 static void mlx5_ib_remove(struct mlx5_core_dev *mdev, void *context)
 {
 	struct mlx5_ib_dev *dev = context;
-	destroy_umrc_res(dev);
 	ib_unregister_device(&dev->ib_dev);
+	destroy_umrc_res(dev);
 	destroy_dev_resources(&dev->devr);
 	free_comp_eqs(dev);
 	ib_dealloc_device(&dev->ib_dev);
diff --git a/drivers/infiniband/hw/mlx5/mem.c b/drivers/infiniband/hw/mlx5/mem.c
index a3e81444c825..dae07eae9507 100644
--- a/drivers/infiniband/hw/mlx5/mem.c
+++ b/drivers/infiniband/hw/mlx5/mem.c
@@ -55,16 +55,17 @@ void mlx5_ib_cont_pages(struct ib_umem *umem, u64 addr, int *count, int *shift,
 	u64 pfn;
 	struct scatterlist *sg;
 	int entry;
+	unsigned long page_shift = ilog2(umem->page_size);
 
-	addr = addr >> PAGE_SHIFT;
+	addr = addr >> page_shift;
 	tmp = (unsigned long)addr;
 	m = find_first_bit(&tmp, sizeof(tmp));
 	skip = 1 << m;
 	mask = skip - 1;
 	i = 0;
 	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
-		len = sg_dma_len(sg) >> PAGE_SHIFT;
-		pfn = sg_dma_address(sg) >> PAGE_SHIFT;
+		len = sg_dma_len(sg) >> page_shift;
+		pfn = sg_dma_address(sg) >> page_shift;
 		for (k = 0; k < len; k++) {
 			if (!(i & mask)) {
 				tmp = (unsigned long)pfn;
@@ -103,14 +104,15 @@ void mlx5_ib_cont_pages(struct ib_umem *umem, u64 addr, int *count, int *shift,
 
 		*ncont = 0;
 	}
-	*shift = PAGE_SHIFT + m;
+	*shift = page_shift + m;
 	*count = i;
 }
 
 void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
 			  int page_shift, __be64 *pas, int umr)
 {
-	int shift = page_shift - PAGE_SHIFT;
+	unsigned long umem_page_shift = ilog2(umem->page_size);
+	int shift = page_shift - umem_page_shift;
 	int mask = (1 << shift) - 1;
 	int i, k;
 	u64 cur = 0;
@@ -121,11 +123,11 @@ void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
 
 	i = 0;
 	for_each_sg(umem->sg_head.sgl, sg, umem->nmap, entry) {
-		len = sg_dma_len(sg) >> PAGE_SHIFT;
+		len = sg_dma_len(sg) >> umem_page_shift;
 		base = sg_dma_address(sg);
 		for (k = 0; k < len; k++) {
 			if (!(i & mask)) {
-				cur = base + (k << PAGE_SHIFT);
+				cur = base + (k << umem_page_shift);
 				if (umr)
 					cur |= 3;
 
@@ -134,7 +136,7 @@ void mlx5_ib_populate_pas(struct mlx5_ib_dev *dev, struct ib_umem *umem,
 					    i >> shift, be64_to_cpu(pas[i >> shift]));
 			}  else
 				mlx5_ib_dbg(dev, "=====> 0x%llx\n",
-					    base + (k << PAGE_SHIFT));
+					    base + (k << umem_page_shift));
 			i++;
 		}
 	}
diff --git a/drivers/infiniband/hw/mlx5/mr.c b/drivers/infiniband/hw/mlx5/mr.c
index 80b3c63eab5d..8ee7cb46e059 100644
--- a/drivers/infiniband/hw/mlx5/mr.c
+++ b/drivers/infiniband/hw/mlx5/mr.c
@@ -881,12 +881,12 @@ struct ib_mr *mlx5_ib_reg_user_mr(struct ib_pd *pd, u64 start, u64 length,
 	int order;
 	int err;
 
-	mlx5_ib_dbg(dev, "start 0x%llx, virt_addr 0x%llx, length 0x%llx\n",
-		    start, virt_addr, length);
+	mlx5_ib_dbg(dev, "start 0x%llx, virt_addr 0x%llx, length 0x%llx, access_flags 0x%x\n",
+		    start, virt_addr, length, access_flags);
 	umem = ib_umem_get(pd->uobject->context, start, length, access_flags,
 			   0);
 	if (IS_ERR(umem)) {
-		mlx5_ib_dbg(dev, "umem get failed\n");
+		mlx5_ib_dbg(dev, "umem get failed (%ld)\n", PTR_ERR(umem));
 		return (void *)umem;
 	}
 
diff --git a/drivers/infiniband/hw/mlx5/qp.c b/drivers/infiniband/hw/mlx5/qp.c
index 8c574b63d77b..e261a53f9a02 100644
--- a/drivers/infiniband/hw/mlx5/qp.c
+++ b/drivers/infiniband/hw/mlx5/qp.c
@@ -158,11 +158,13 @@ static void mlx5_ib_qp_event(struct mlx5_core_qp *qp, int type)
 static int set_rq_size(struct mlx5_ib_dev *dev, struct ib_qp_cap *cap,
 		       int has_rq, struct mlx5_ib_qp *qp, struct mlx5_ib_create_qp *ucmd)
 {
+	struct mlx5_general_caps *gen;
 	int wqe_size;
 	int wq_size;
 
+	gen = &dev->mdev->caps.gen;
 	/* Sanity check RQ size before proceeding */
-	if (cap->max_recv_wr  > dev->mdev->caps.max_wqes)
+	if (cap->max_recv_wr  > gen->max_wqes)
 		return -EINVAL;
 
 	if (!has_rq) {
@@ -182,10 +184,10 @@ static int set_rq_size(struct mlx5_ib_dev *dev, struct ib_qp_cap *cap,
 			wq_size = roundup_pow_of_two(cap->max_recv_wr) * wqe_size;
 			wq_size = max_t(int, wq_size, MLX5_SEND_WQE_BB);
 			qp->rq.wqe_cnt = wq_size / wqe_size;
-			if (wqe_size > dev->mdev->caps.max_rq_desc_sz) {
+			if (wqe_size > gen->max_rq_desc_sz) {
 				mlx5_ib_dbg(dev, "wqe_size %d, max %d\n",
 					    wqe_size,
-					    dev->mdev->caps.max_rq_desc_sz);
+					    gen->max_rq_desc_sz);
 				return -EINVAL;
 			}
 			qp->rq.wqe_shift = ilog2(wqe_size);
@@ -266,9 +268,11 @@ static int calc_send_wqe(struct ib_qp_init_attr *attr)
 static int calc_sq_size(struct mlx5_ib_dev *dev, struct ib_qp_init_attr *attr,
 			struct mlx5_ib_qp *qp)
 {
+	struct mlx5_general_caps *gen;
 	int wqe_size;
 	int wq_size;
 
+	gen = &dev->mdev->caps.gen;
 	if (!attr->cap.max_send_wr)
 		return 0;
 
@@ -277,9 +281,9 @@ static int calc_sq_size(struct mlx5_ib_dev *dev, struct ib_qp_init_attr *attr,
 	if (wqe_size < 0)
 		return wqe_size;
 
-	if (wqe_size > dev->mdev->caps.max_sq_desc_sz) {
+	if (wqe_size > gen->max_sq_desc_sz) {
 		mlx5_ib_dbg(dev, "wqe_size(%d) > max_sq_desc_sz(%d)\n",
-			    wqe_size, dev->mdev->caps.max_sq_desc_sz);
+			    wqe_size, gen->max_sq_desc_sz);
 		return -EINVAL;
 	}
 
@@ -292,9 +296,9 @@ static int calc_sq_size(struct mlx5_ib_dev *dev, struct ib_qp_init_attr *attr,
 
 	wq_size = roundup_pow_of_two(attr->cap.max_send_wr * wqe_size);
 	qp->sq.wqe_cnt = wq_size / MLX5_SEND_WQE_BB;
-	if (qp->sq.wqe_cnt > dev->mdev->caps.max_wqes) {
+	if (qp->sq.wqe_cnt > gen->max_wqes) {
 		mlx5_ib_dbg(dev, "wqe count(%d) exceeds limits(%d)\n",
-			    qp->sq.wqe_cnt, dev->mdev->caps.max_wqes);
+			    qp->sq.wqe_cnt, gen->max_wqes);
 		return -ENOMEM;
 	}
 	qp->sq.wqe_shift = ilog2(MLX5_SEND_WQE_BB);
@@ -309,11 +313,13 @@ static int set_user_buf_size(struct mlx5_ib_dev *dev,
 			    struct mlx5_ib_qp *qp,
 			    struct mlx5_ib_create_qp *ucmd)
 {
+	struct mlx5_general_caps *gen;
 	int desc_sz = 1 << qp->sq.wqe_shift;
 
-	if (desc_sz > dev->mdev->caps.max_sq_desc_sz) {
+	gen = &dev->mdev->caps.gen;
+	if (desc_sz > gen->max_sq_desc_sz) {
 		mlx5_ib_warn(dev, "desc_sz %d, max_sq_desc_sz %d\n",
-			     desc_sz, dev->mdev->caps.max_sq_desc_sz);
+			     desc_sz, gen->max_sq_desc_sz);
 		return -EINVAL;
 	}
 
@@ -325,9 +331,9 @@ static int set_user_buf_size(struct mlx5_ib_dev *dev,
 
 	qp->sq.wqe_cnt = ucmd->sq_wqe_count;
 
-	if (qp->sq.wqe_cnt > dev->mdev->caps.max_wqes) {
+	if (qp->sq.wqe_cnt > gen->max_wqes) {
 		mlx5_ib_warn(dev, "wqe_cnt %d, max_wqes %d\n",
-			     qp->sq.wqe_cnt, dev->mdev->caps.max_wqes);
+			     qp->sq.wqe_cnt, gen->max_wqes);
 		return -EINVAL;
 	}
 
@@ -803,16 +809,18 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 	struct mlx5_ib_resources *devr = &dev->devr;
 	struct mlx5_ib_create_qp_resp resp;
 	struct mlx5_create_qp_mbox_in *in;
+	struct mlx5_general_caps *gen;
 	struct mlx5_ib_create_qp ucmd;
 	int inlen = sizeof(*in);
 	int err;
 
+	gen = &dev->mdev->caps.gen;
 	mutex_init(&qp->mutex);
 	spin_lock_init(&qp->sq.lock);
 	spin_lock_init(&qp->rq.lock);
 
 	if (init_attr->create_flags & IB_QP_CREATE_BLOCK_MULTICAST_LOOPBACK) {
-		if (!(dev->mdev->caps.flags & MLX5_DEV_CAP_FLAG_BLOCK_MCAST)) {
+		if (!(gen->flags & MLX5_DEV_CAP_FLAG_BLOCK_MCAST)) {
 			mlx5_ib_dbg(dev, "block multicast loopback isn't supported\n");
 			return -EINVAL;
 		} else {
@@ -851,9 +859,9 @@ static int create_qp_common(struct mlx5_ib_dev *dev, struct ib_pd *pd,
 				mlx5_ib_dbg(dev, "invalid rq params\n");
 				return -EINVAL;
 			}
-			if (ucmd.sq_wqe_count > dev->mdev->caps.max_wqes) {
+			if (ucmd.sq_wqe_count > gen->max_wqes) {
 				mlx5_ib_dbg(dev, "requested sq_wqe_count (%d) > max allowed (%d)\n",
-					    ucmd.sq_wqe_count, dev->mdev->caps.max_wqes);
+					    ucmd.sq_wqe_count, gen->max_wqes);
 				return -EINVAL;
 			}
 			err = create_user_qp(dev, pd, qp, udata, &in, &resp, &inlen);
@@ -1144,6 +1152,7 @@ struct ib_qp *mlx5_ib_create_qp(struct ib_pd *pd,
 				struct ib_qp_init_attr *init_attr,
 				struct ib_udata *udata)
 {
+	struct mlx5_general_caps *gen;
 	struct mlx5_ib_dev *dev;
 	struct mlx5_ib_qp *qp;
 	u16 xrcdn = 0;
@@ -1161,11 +1170,12 @@ struct ib_qp *mlx5_ib_create_qp(struct ib_pd *pd,
 		}
 		dev = to_mdev(to_mxrcd(init_attr->xrcd)->ibxrcd.device);
 	}
+	gen = &dev->mdev->caps.gen;
 
 	switch (init_attr->qp_type) {
 	case IB_QPT_XRC_TGT:
 	case IB_QPT_XRC_INI:
-		if (!(dev->mdev->caps.flags & MLX5_DEV_CAP_FLAG_XRC)) {
+		if (!(gen->flags & MLX5_DEV_CAP_FLAG_XRC)) {
 			mlx5_ib_dbg(dev, "XRC not supported\n");
 			return ERR_PTR(-ENOSYS);
 		}
@@ -1272,6 +1282,9 @@ enum {
 
 static int ib_rate_to_mlx5(struct mlx5_ib_dev *dev, u8 rate)
 {
+	struct mlx5_general_caps *gen;
+
+	gen = &dev->mdev->caps.gen;
 	if (rate == IB_RATE_PORT_CURRENT) {
 		return 0;
 	} else if (rate < IB_RATE_2_5_GBPS || rate > IB_RATE_300_GBPS) {
@@ -1279,7 +1292,7 @@ static int ib_rate_to_mlx5(struct mlx5_ib_dev *dev, u8 rate)
 	} else {
 		while (rate != IB_RATE_2_5_GBPS &&
 		       !(1 << (rate + MLX5_STAT_RATE_OFFSET) &
-			 dev->mdev->caps.stat_rate_support))
+			 gen->stat_rate_support))
 			--rate;
 	}
 
@@ -1290,8 +1303,10 @@ static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
 			 struct mlx5_qp_path *path, u8 port, int attr_mask,
 			 u32 path_flags, const struct ib_qp_attr *attr)
 {
+	struct mlx5_general_caps *gen;
 	int err;
 
+	gen = &dev->mdev->caps.gen;
 	path->fl = (path_flags & MLX5_PATH_FLAG_FL) ? 0x80 : 0;
 	path->free_ar = (path_flags & MLX5_PATH_FLAG_FREE_AR) ? 0x80 : 0;
 
@@ -1302,6 +1317,11 @@ static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
 	path->rlid	= cpu_to_be16(ah->dlid);
 
 	if (ah->ah_flags & IB_AH_GRH) {
+		if (ah->grh.sgid_index >= gen->port[port - 1].gid_table_len) {
+			pr_err(KERN_ERR "sgid_index (%u) too large. max is %d\n",
+			       ah->grh.sgid_index, gen->port[port - 1].gid_table_len);
+			return -EINVAL;
+		}
 		path->grh_mlid |= 1 << 7;
 		path->mgid_index = ah->grh.sgid_index;
 		path->hop_limit  = ah->grh.hop_limit;
@@ -1317,22 +1337,6 @@ static int mlx5_set_path(struct mlx5_ib_dev *dev, const struct ib_ah_attr *ah,
 	path->static_rate = err;
 	path->port = port;
 
-	if (ah->ah_flags & IB_AH_GRH) {
-		if (ah->grh.sgid_index >= dev->mdev->caps.port[port - 1].gid_table_len) {
-			pr_err(KERN_ERR "sgid_index (%u) too large. max is %d\n",
-			       ah->grh.sgid_index, dev->mdev->caps.port[port - 1].gid_table_len);
-			return -EINVAL;
-		}
-
-		path->grh_mlid |= 1 << 7;
-		path->mgid_index = ah->grh.sgid_index;
-		path->hop_limit  = ah->grh.hop_limit;
-		path->tclass_flowlabel =
-			cpu_to_be32((ah->grh.traffic_class << 20) |
-				    (ah->grh.flow_label));
-		memcpy(path->rgid, ah->grh.dgid.raw, 16);
-	}
-
 	if (attr_mask & IB_QP_TIMEOUT)
 		path->ackto_lt = attr->timeout << 3;
 
@@ -1492,6 +1496,7 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	struct mlx5_ib_qp *qp = to_mqp(ibqp);
 	struct mlx5_ib_cq *send_cq, *recv_cq;
 	struct mlx5_qp_context *context;
+	struct mlx5_general_caps *gen;
 	struct mlx5_modify_qp_mbox_in *in;
 	struct mlx5_ib_pd *pd;
 	enum mlx5_qp_state mlx5_cur, mlx5_new;
@@ -1500,6 +1505,7 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 	int mlx5_st;
 	int err;
 
+	gen = &dev->mdev->caps.gen;
 	in = kzalloc(sizeof(*in), GFP_KERNEL);
 	if (!in)
 		return -ENOMEM;
@@ -1539,7 +1545,7 @@ static int __mlx5_ib_modify_qp(struct ib_qp *ibqp,
 			err = -EINVAL;
 			goto out;
 		}
-		context->mtu_msgmax = (attr->path_mtu << 5) | dev->mdev->caps.log_max_msg;
+		context->mtu_msgmax = (attr->path_mtu << 5) | gen->log_max_msg;
 	}
 
 	if (attr_mask & IB_QP_DEST_QPN)
@@ -1685,9 +1691,11 @@ int mlx5_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 	struct mlx5_ib_dev *dev = to_mdev(ibqp->device);
 	struct mlx5_ib_qp *qp = to_mqp(ibqp);
 	enum ib_qp_state cur_state, new_state;
+	struct mlx5_general_caps *gen;
 	int err = -EINVAL;
 	int port;
 
+	gen = &dev->mdev->caps.gen;
 	mutex_lock(&qp->mutex);
 
 	cur_state = attr_mask & IB_QP_CUR_STATE ? attr->cur_qp_state : qp->state;
@@ -1699,21 +1707,21 @@ int mlx5_ib_modify_qp(struct ib_qp *ibqp, struct ib_qp_attr *attr,
 		goto out;
 
 	if ((attr_mask & IB_QP_PORT) &&
-	    (attr->port_num == 0 || attr->port_num > dev->mdev->caps.num_ports))
+	    (attr->port_num == 0 || attr->port_num > gen->num_ports))
 		goto out;
 
 	if (attr_mask & IB_QP_PKEY_INDEX) {
 		port = attr_mask & IB_QP_PORT ? attr->port_num : qp->port;
-		if (attr->pkey_index >= dev->mdev->caps.port[port - 1].pkey_table_len)
+		if (attr->pkey_index >= gen->port[port - 1].pkey_table_len)
 			goto out;
 	}
 
 	if (attr_mask & IB_QP_MAX_QP_RD_ATOMIC &&
-	    attr->max_rd_atomic > dev->mdev->caps.max_ra_res_qp)
+	    attr->max_rd_atomic > (1 << gen->log_max_ra_res_qp))
 		goto out;
 
 	if (attr_mask & IB_QP_MAX_DEST_RD_ATOMIC &&
-	    attr->max_dest_rd_atomic > dev->mdev->caps.max_ra_req_qp)
+	    attr->max_dest_rd_atomic > (1 << gen->log_max_ra_req_qp))
 		goto out;
 
 	if (cur_state == new_state && cur_state == IB_QPS_RESET) {
@@ -2020,56 +2028,31 @@ static u8 bs_selector(int block_size)
 	}
 }
 
-static int format_selector(struct ib_sig_attrs *attr,
-			   struct ib_sig_domain *domain,
-			   int *selector)
+static void mlx5_fill_inl_bsf(struct ib_sig_domain *domain,
+			      struct mlx5_bsf_inl *inl)
 {
+	/* Valid inline section and allow BSF refresh */
+	inl->vld_refresh = cpu_to_be16(MLX5_BSF_INL_VALID |
+				       MLX5_BSF_REFRESH_DIF);
+	inl->dif_apptag = cpu_to_be16(domain->sig.dif.app_tag);
+	inl->dif_reftag = cpu_to_be32(domain->sig.dif.ref_tag);
+	/* repeating block */
+	inl->rp_inv_seed = MLX5_BSF_REPEAT_BLOCK;
+	inl->sig_type = domain->sig.dif.bg_type == IB_T10DIF_CRC ?
+			MLX5_DIF_CRC : MLX5_DIF_IPCS;
 
-#define FORMAT_DIF_NONE		0
-#define FORMAT_DIF_CRC_INC	8
-#define FORMAT_DIF_CRC_NO_INC	12
-#define FORMAT_DIF_CSUM_INC	13
-#define FORMAT_DIF_CSUM_NO_INC	14
+	if (domain->sig.dif.ref_remap)
+		inl->dif_inc_ref_guard_check |= MLX5_BSF_INC_REFTAG;
 
-	switch (domain->sig.dif.type) {
-	case IB_T10DIF_NONE:
-		/* No DIF */
-		*selector = FORMAT_DIF_NONE;
-		break;
-	case IB_T10DIF_TYPE1: /* Fall through */
-	case IB_T10DIF_TYPE2:
-		switch (domain->sig.dif.bg_type) {
-		case IB_T10DIF_CRC:
-			*selector = FORMAT_DIF_CRC_INC;
-			break;
-		case IB_T10DIF_CSUM:
-			*selector = FORMAT_DIF_CSUM_INC;
-			break;
-		default:
-			return 1;
-		}
-		break;
-	case IB_T10DIF_TYPE3:
-		switch (domain->sig.dif.bg_type) {
-		case IB_T10DIF_CRC:
-			*selector = domain->sig.dif.type3_inc_reftag ?
-					   FORMAT_DIF_CRC_INC :
-					   FORMAT_DIF_CRC_NO_INC;
-			break;
-		case IB_T10DIF_CSUM:
-			*selector = domain->sig.dif.type3_inc_reftag ?
-					   FORMAT_DIF_CSUM_INC :
-					   FORMAT_DIF_CSUM_NO_INC;
-			break;
-		default:
-			return 1;
-		}
-		break;
-	default:
-		return 1;
+	if (domain->sig.dif.app_escape) {
+		if (domain->sig.dif.ref_escape)
+			inl->dif_inc_ref_guard_check |= MLX5_BSF_APPREF_ESCAPE;
+		else
+			inl->dif_inc_ref_guard_check |= MLX5_BSF_APPTAG_ESCAPE;
 	}
 
-	return 0;
+	inl->dif_app_bitmask_check =
+		cpu_to_be16(domain->sig.dif.apptag_check_mask);
 }
 
 static int mlx5_set_bsf(struct ib_mr *sig_mr,
@@ -2080,45 +2063,49 @@ static int mlx5_set_bsf(struct ib_mr *sig_mr,
 	struct mlx5_bsf_basic *basic = &bsf->basic;
 	struct ib_sig_domain *mem = &sig_attrs->mem;
 	struct ib_sig_domain *wire = &sig_attrs->wire;
-	int ret, selector;
 
 	memset(bsf, 0, sizeof(*bsf));
+
+	/* Basic + Extended + Inline */
+	basic->bsf_size_sbs = 1 << 7;
+	/* Input domain check byte mask */
+	basic->check_byte_mask = sig_attrs->check_mask;
+	basic->raw_data_size = cpu_to_be32(data_size);
+
+	/* Memory domain */
 	switch (sig_attrs->mem.sig_type) {
+	case IB_SIG_TYPE_NONE:
+		break;
 	case IB_SIG_TYPE_T10_DIF:
-		if (sig_attrs->wire.sig_type != IB_SIG_TYPE_T10_DIF)
-			return -EINVAL;
+		basic->mem.bs_selector = bs_selector(mem->sig.dif.pi_interval);
+		basic->m_bfs_psv = cpu_to_be32(msig->psv_memory.psv_idx);
+		mlx5_fill_inl_bsf(mem, &bsf->m_inl);
+		break;
+	default:
+		return -EINVAL;
+	}
 
-		/* Input domain check byte mask */
-		basic->check_byte_mask = sig_attrs->check_mask;
+	/* Wire domain */
+	switch (sig_attrs->wire.sig_type) {
+	case IB_SIG_TYPE_NONE:
+		break;
+	case IB_SIG_TYPE_T10_DIF:
 		if (mem->sig.dif.pi_interval == wire->sig.dif.pi_interval &&
-		    mem->sig.dif.type == wire->sig.dif.type) {
+		    mem->sig_type == wire->sig_type) {
 			/* Same block structure */
-			basic->bsf_size_sbs = 1 << 4;
+			basic->bsf_size_sbs |= 1 << 4;
 			if (mem->sig.dif.bg_type == wire->sig.dif.bg_type)
-				basic->wire.copy_byte_mask |= 0xc0;
+				basic->wire.copy_byte_mask |= MLX5_CPY_GRD_MASK;
 			if (mem->sig.dif.app_tag == wire->sig.dif.app_tag)
-				basic->wire.copy_byte_mask |= 0x30;
+				basic->wire.copy_byte_mask |= MLX5_CPY_APP_MASK;
 			if (mem->sig.dif.ref_tag == wire->sig.dif.ref_tag)
-				basic->wire.copy_byte_mask |= 0x0f;
+				basic->wire.copy_byte_mask |= MLX5_CPY_REF_MASK;
 		} else
 			basic->wire.bs_selector = bs_selector(wire->sig.dif.pi_interval);
 
-		basic->mem.bs_selector = bs_selector(mem->sig.dif.pi_interval);
-		basic->raw_data_size = cpu_to_be32(data_size);
-
-		ret = format_selector(sig_attrs, mem, &selector);
-		if (ret)
-			return -EINVAL;
-		basic->m_bfs_psv = cpu_to_be32(selector << 24 |
-					       msig->psv_memory.psv_idx);
-
-		ret = format_selector(sig_attrs, wire, &selector);
-		if (ret)
-			return -EINVAL;
-		basic->w_bfs_psv = cpu_to_be32(selector << 24 |
-					       msig->psv_wire.psv_idx);
+		basic->w_bfs_psv = cpu_to_be32(msig->psv_wire.psv_idx);
+		mlx5_fill_inl_bsf(wire, &bsf->w_inl);
 		break;
-
 	default:
 		return -EINVAL;
 	}
@@ -2317,20 +2304,21 @@ static int set_psv_wr(struct ib_sig_domain *domain,
 	memset(psv_seg, 0, sizeof(*psv_seg));
 	psv_seg->psv_num = cpu_to_be32(psv_idx);
 	switch (domain->sig_type) {
+	case IB_SIG_TYPE_NONE:
+		break;
 	case IB_SIG_TYPE_T10_DIF:
 		psv_seg->transient_sig = cpu_to_be32(domain->sig.dif.bg << 16 |
 						     domain->sig.dif.app_tag);
 		psv_seg->ref_tag = cpu_to_be32(domain->sig.dif.ref_tag);
-
-		*seg += sizeof(*psv_seg);
-		*size += sizeof(*psv_seg) / 16;
 		break;
-
 	default:
 		pr_err("Bad signature type given.\n");
 		return 1;
 	}
 
+	*seg += sizeof(*psv_seg);
+	*size += sizeof(*psv_seg) / 16;
+
 	return 0;
 }
 
@@ -2893,7 +2881,8 @@ static void to_ib_ah_attr(struct mlx5_ib_dev *ibdev, struct ib_ah_attr *ib_ah_at
 	memset(ib_ah_attr, 0, sizeof(*ib_ah_attr));
 	ib_ah_attr->port_num	  = path->port;
 
-	if (ib_ah_attr->port_num == 0 || ib_ah_attr->port_num > dev->caps.num_ports)
+	if (ib_ah_attr->port_num == 0 ||
+	    ib_ah_attr->port_num > dev->caps.gen.num_ports)
 		return;
 
 	ib_ah_attr->sl = path->sl & 0xf;
@@ -3011,10 +3000,12 @@ struct ib_xrcd *mlx5_ib_alloc_xrcd(struct ib_device *ibdev,
 					  struct ib_udata *udata)
 {
 	struct mlx5_ib_dev *dev = to_mdev(ibdev);
+	struct mlx5_general_caps *gen;
 	struct mlx5_ib_xrcd *xrcd;
 	int err;
 
-	if (!(dev->mdev->caps.flags & MLX5_DEV_CAP_FLAG_XRC))
+	gen = &dev->mdev->caps.gen;
+	if (!(gen->flags & MLX5_DEV_CAP_FLAG_XRC))
 		return ERR_PTR(-ENOSYS);
 
 	xrcd = kmalloc(sizeof(*xrcd), GFP_KERNEL);
diff --git a/drivers/infiniband/hw/mlx5/srq.c b/drivers/infiniband/hw/mlx5/srq.c
index 70bd131ba646..97cc1baaa8e3 100644
--- a/drivers/infiniband/hw/mlx5/srq.c
+++ b/drivers/infiniband/hw/mlx5/srq.c
@@ -238,6 +238,7 @@ struct ib_srq *mlx5_ib_create_srq(struct ib_pd *pd,
 				  struct ib_udata *udata)
 {
 	struct mlx5_ib_dev *dev = to_mdev(pd->device);
+	struct mlx5_general_caps *gen;
 	struct mlx5_ib_srq *srq;
 	int desc_size;
 	int buf_size;
@@ -247,11 +248,12 @@ struct ib_srq *mlx5_ib_create_srq(struct ib_pd *pd,
 	int is_xrc;
 	u32 flgs, xrcdn;
 
+	gen = &dev->mdev->caps.gen;
 	/* Sanity check SRQ size before proceeding */
-	if (init_attr->attr.max_wr >= dev->mdev->caps.max_srq_wqes) {
+	if (init_attr->attr.max_wr >= gen->max_srq_wqes) {
 		mlx5_ib_dbg(dev, "max_wr %d, cap %d\n",
 			    init_attr->attr.max_wr,
-			    dev->mdev->caps.max_srq_wqes);
+			    gen->max_srq_wqes);
 		return ERR_PTR(-EINVAL);
 	}
 
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
index 40f8536c10b0..ac02ce4e8040 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_ah.c
@@ -38,7 +38,7 @@
 #define OCRDMA_VID_PCP_SHIFT	0xD
 
 static inline int set_av_attr(struct ocrdma_dev *dev, struct ocrdma_ah *ah,
-				struct ib_ah_attr *attr, int pdid)
+			struct ib_ah_attr *attr, union ib_gid *sgid, int pdid)
 {
 	int status = 0;
 	u16 vlan_tag; bool vlan_enabled = false;
@@ -49,8 +49,7 @@ static inline int set_av_attr(struct ocrdma_dev *dev, struct ocrdma_ah *ah,
 	memset(&eth, 0, sizeof(eth));
 	memset(&grh, 0, sizeof(grh));
 
-	ah->sgid_index = attr->grh.sgid_index;
-
+	/* VLAN */
 	vlan_tag = attr->vlan_id;
 	if (!vlan_tag || (vlan_tag > 0xFFF))
 		vlan_tag = dev->pvid;
@@ -65,15 +64,14 @@ static inline int set_av_attr(struct ocrdma_dev *dev, struct ocrdma_ah *ah,
 		eth.eth_type = cpu_to_be16(OCRDMA_ROCE_ETH_TYPE);
 		eth_sz = sizeof(struct ocrdma_eth_basic);
 	}
+	/* MAC */
 	memcpy(&eth.smac[0], &dev->nic_info.mac_addr[0], ETH_ALEN);
-	memcpy(&eth.dmac[0], attr->dmac, ETH_ALEN);
 	status = ocrdma_resolve_dmac(dev, attr, &eth.dmac[0]);
 	if (status)
 		return status;
-	status = ocrdma_query_gid(&dev->ibdev, 1, attr->grh.sgid_index,
-			(union ib_gid *)&grh.sgid[0]);
-	if (status)
-		return status;
+	ah->sgid_index = attr->grh.sgid_index;
+	memcpy(&grh.sgid[0], sgid->raw, sizeof(union ib_gid));
+	memcpy(&grh.dgid[0], attr->grh.dgid.raw, sizeof(attr->grh.dgid.raw));
 
 	grh.tclass_flow = cpu_to_be32((6 << 28) |
 			(attr->grh.traffic_class << 24) |
@@ -81,8 +79,7 @@ static inline int set_av_attr(struct ocrdma_dev *dev, struct ocrdma_ah *ah,
 	/* 0x1b is next header value in GRH */
 	grh.pdid_hoplimit = cpu_to_be32((pdid << 16) |
 			(0x1b << 8) | attr->grh.hop_limit);
-
-	memcpy(&grh.dgid[0], attr->grh.dgid.raw, sizeof(attr->grh.dgid.raw));
+	/* Eth HDR */
 	memcpy(&ah->av->eth_hdr, &eth, eth_sz);
 	memcpy((u8 *)ah->av + eth_sz, &grh, sizeof(struct ocrdma_grh));
 	if (vlan_enabled)
@@ -98,6 +95,8 @@ struct ib_ah *ocrdma_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr)
 	struct ocrdma_ah *ah;
 	struct ocrdma_pd *pd = get_ocrdma_pd(ibpd);
 	struct ocrdma_dev *dev = get_ocrdma_dev(ibpd->device);
+	union ib_gid sgid;
+	u8 zmac[ETH_ALEN];
 
 	if (!(attr->ah_flags & IB_AH_GRH))
 		return ERR_PTR(-EINVAL);
@@ -111,7 +110,27 @@ struct ib_ah *ocrdma_create_ah(struct ib_pd *ibpd, struct ib_ah_attr *attr)
 	status = ocrdma_alloc_av(dev, ah);
 	if (status)
 		goto av_err;
-	status = set_av_attr(dev, ah, attr, pd->id);
+
+	status = ocrdma_query_gid(&dev->ibdev, 1, attr->grh.sgid_index, &sgid);
+	if (status) {
+		pr_err("%s(): Failed to query sgid, status = %d\n",
+		      __func__, status);
+		goto av_conf_err;
+	}
+
+	memset(&zmac, 0, ETH_ALEN);
+	if (pd->uctx &&
+	    memcmp(attr->dmac, &zmac, ETH_ALEN)) {
+		status = rdma_addr_find_dmac_by_grh(&sgid, &attr->grh.dgid,
+                                        attr->dmac, &attr->vlan_id);
+		if (status) {
+			pr_err("%s(): Failed to resolve dmac from gid." 
+				"status = %d\n", __func__, status);
+			goto av_conf_err;
+		}
+	}
+
+	status = set_av_attr(dev, ah, attr, &sgid, pd->id);
 	if (status)
 		goto av_conf_err;
 
@@ -145,7 +164,7 @@ int ocrdma_query_ah(struct ib_ah *ibah, struct ib_ah_attr *attr)
 	struct ocrdma_av *av = ah->av;
 	struct ocrdma_grh *grh;
 	attr->ah_flags |= IB_AH_GRH;
-	if (ah->av->valid & Bit(1)) {
+	if (ah->av->valid & OCRDMA_AV_VALID) {
 		grh = (struct ocrdma_grh *)((u8 *)ah->av +
 				sizeof(struct ocrdma_eth_vlan));
 		attr->sl = be16_to_cpu(av->eth_hdr.vlan_tag) >> 13;
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_hw.c b/drivers/infiniband/hw/ocrdma/ocrdma_hw.c
index dd35ae558ae1..638bff1ffc6c 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_hw.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_hw.c
@@ -348,11 +348,6 @@ static void *ocrdma_init_emb_mqe(u8 opcode, u32 cmd_len)
 	return mqe;
 }
 
-static void *ocrdma_alloc_mqe(void)
-{
-	return kzalloc(sizeof(struct ocrdma_mqe), GFP_KERNEL);
-}
-
 static void ocrdma_free_q(struct ocrdma_dev *dev, struct ocrdma_queue_info *q)
 {
 	dma_free_coherent(&dev->nic_info.pdev->dev, q->size, q->va, q->dma);
@@ -566,8 +561,8 @@ static int ocrdma_mbx_create_mq(struct ocrdma_dev *dev,
 	cmd->cqid_pages |= (cq->id << OCRDMA_CREATE_MQ_CQ_ID_SHIFT);
 	cmd->async_cqid_valid = OCRDMA_CREATE_MQ_ASYNC_CQ_VALID;
 
-	cmd->async_event_bitmap = Bit(OCRDMA_ASYNC_GRP5_EVE_CODE);
-	cmd->async_event_bitmap |= Bit(OCRDMA_ASYNC_RDMA_EVE_CODE);
+	cmd->async_event_bitmap = BIT(OCRDMA_ASYNC_GRP5_EVE_CODE);
+	cmd->async_event_bitmap |= BIT(OCRDMA_ASYNC_RDMA_EVE_CODE);
 
 	cmd->async_cqid_ringsize = cq->id;
 	cmd->async_cqid_ringsize |= (ocrdma_encoded_q_len(mq->len) <<
@@ -1189,10 +1184,10 @@ int ocrdma_mbx_rdma_stats(struct ocrdma_dev *dev, bool reset)
 {
 	struct ocrdma_rdma_stats_req *req = dev->stats_mem.va;
 	struct ocrdma_mqe *mqe = &dev->stats_mem.mqe;
-	struct ocrdma_rdma_stats_resp *old_stats = NULL;
+	struct ocrdma_rdma_stats_resp *old_stats;
 	int status;
 
-	old_stats = kzalloc(sizeof(*old_stats), GFP_KERNEL);
+	old_stats = kmalloc(sizeof(*old_stats), GFP_KERNEL);
 	if (old_stats == NULL)
 		return -ENOMEM;
 
@@ -1235,10 +1230,9 @@ static int ocrdma_mbx_get_ctrl_attribs(struct ocrdma_dev *dev)
 	struct ocrdma_get_ctrl_attribs_rsp *ctrl_attr_rsp;
 	struct mgmt_hba_attribs *hba_attribs;
 
-	mqe = ocrdma_alloc_mqe();
+	mqe = kzalloc(sizeof(struct ocrdma_mqe), GFP_KERNEL);
 	if (!mqe)
 		return status;
-	memset(mqe, 0, sizeof(*mqe));
 
 	dma.size = sizeof(struct ocrdma_get_ctrl_attribs_rsp);
 	dma.va	 = dma_alloc_coherent(&dev->nic_info.pdev->dev,
@@ -2279,7 +2273,8 @@ mbx_err:
 
 static int ocrdma_set_av_params(struct ocrdma_qp *qp,
 				struct ocrdma_modify_qp *cmd,
-				struct ib_qp_attr *attrs)
+				struct ib_qp_attr *attrs,
+				int attr_mask)
 {
 	int status;
 	struct ib_ah_attr *ah_attr = &attrs->ah_attr;
@@ -2319,8 +2314,8 @@ static int ocrdma_set_av_params(struct ocrdma_qp *qp,
 	ocrdma_cpu_to_le32(&cmd->params.dgid[0], sizeof(cmd->params.dgid));
 	ocrdma_cpu_to_le32(&cmd->params.sgid[0], sizeof(cmd->params.sgid));
 	cmd->params.vlan_dmac_b4_to_b5 = mac_addr[4] | (mac_addr[5] << 8);
-	vlan_id = ah_attr->vlan_id;
-	if (vlan_id && (vlan_id < 0x1000)) {
+	if (attr_mask & IB_QP_VID) {
+		vlan_id = attrs->vlan_id;
 		cmd->params.vlan_dmac_b4_to_b5 |=
 		    vlan_id << OCRDMA_QP_PARAMS_VLAN_SHIFT;
 		cmd->flags |= OCRDMA_QP_PARA_VLAN_EN_VALID;
@@ -2347,7 +2342,7 @@ static int ocrdma_set_qp_params(struct ocrdma_qp *qp,
 		cmd->flags |= OCRDMA_QP_PARA_QKEY_VALID;
 	}
 	if (attr_mask & IB_QP_AV) {
-		status = ocrdma_set_av_params(qp, cmd, attrs);
+		status = ocrdma_set_av_params(qp, cmd, attrs, attr_mask);
 		if (status)
 			return status;
 	} else if (qp->qp_type == IB_QPT_GSI || qp->qp_type == IB_QPT_UD) {
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_main.c b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
index 256a06bc0b68..b0b2257b8e04 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_main.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_main.c
@@ -388,6 +388,15 @@ static void ocrdma_remove_sysfiles(struct ocrdma_dev *dev)
 		device_remove_file(&dev->ibdev.dev, ocrdma_attributes[i]);
 }
 
+static void ocrdma_add_default_sgid(struct ocrdma_dev *dev)
+{
+	/* GID Index 0 - Invariant manufacturer-assigned EUI-64 */
+	union ib_gid *sgid = &dev->sgid_tbl[0];
+
+	sgid->global.subnet_prefix = cpu_to_be64(0xfe80000000000000LL);
+	ocrdma_get_guid(dev, &sgid->raw[8]);
+}
+
 static void ocrdma_init_ipv4_gids(struct ocrdma_dev *dev,
 				  struct net_device *net)
 {
@@ -434,6 +443,7 @@ static void ocrdma_init_gid_table(struct ocrdma_dev *dev)
 				rdma_vlan_dev_real_dev(net_dev) : net_dev;
 
 		if (real_dev == dev->nic_info.netdev) {
+			ocrdma_add_default_sgid(dev);
 			ocrdma_init_ipv4_gids(dev, net_dev);
 			ocrdma_init_ipv6_gids(dev, net_dev);
 		}
@@ -646,8 +656,10 @@ static int __init ocrdma_init_module(void)
 	return 0;
 
 err_be_reg:
+#if IS_ENABLED(CONFIG_IPV6)
 	ocrdma_unregister_inet6addr_notifier();
 err_notifier6:
+#endif
 	ocrdma_unregister_inetaddr_notifier();
 	return status;
 }
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_sli.h b/drivers/infiniband/hw/ocrdma/ocrdma_sli.h
index 904989ec5eaa..4e036480c1a8 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_sli.h
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_sli.h
@@ -28,8 +28,6 @@
 #ifndef __OCRDMA_SLI_H__
 #define __OCRDMA_SLI_H__
 
-#define Bit(_b) (1 << (_b))
-
 enum {
 	OCRDMA_ASIC_GEN_SKH_R = 0x04,
 	OCRDMA_ASIC_GEN_LANCER = 0x0B
@@ -103,7 +101,7 @@ enum {
 	QTYPE_MCCQ	= 3
 };
 
-#define OCRDMA_MAX_SGID (8)
+#define OCRDMA_MAX_SGID		8
 
 #define OCRDMA_MAX_QP    2048
 #define OCRDMA_MAX_CQ    2048
@@ -128,33 +126,33 @@ enum {
 #define OCRDMA_DB_CQ_RING_ID_EXT_MASK  0x0C00	/* bits 10-11 of qid at 12-11 */
 /* qid #2 msbits at 12-11 */
 #define OCRDMA_DB_CQ_RING_ID_EXT_MASK_SHIFT  0x1
-#define OCRDMA_DB_CQ_NUM_POPPED_SHIFT       (16)	/* bits 16 - 28 */
+#define OCRDMA_DB_CQ_NUM_POPPED_SHIFT	16	/* bits 16 - 28 */
 /* Rearm bit */
-#define OCRDMA_DB_CQ_REARM_SHIFT        (29)	/* bit 29 */
+#define OCRDMA_DB_CQ_REARM_SHIFT	29	/* bit 29 */
 /* solicited bit */
-#define OCRDMA_DB_CQ_SOLICIT_SHIFT   (31)	/* bit 31 */
+#define OCRDMA_DB_CQ_SOLICIT_SHIFT	31	/* bit 31 */
 
 #define OCRDMA_EQ_ID_MASK		0x1FF	/* bits 0 - 8 */
 #define OCRDMA_EQ_ID_EXT_MASK		0x3e00	/* bits 9-13 */
-#define OCRDMA_EQ_ID_EXT_MASK_SHIFT	(2)	/* qid bits 9-13 at 11-15 */
+#define OCRDMA_EQ_ID_EXT_MASK_SHIFT	2	/* qid bits 9-13 at 11-15 */
 
 /* Clear the interrupt for this eq */
-#define OCRDMA_EQ_CLR_SHIFT			(9)	/* bit 9 */
+#define OCRDMA_EQ_CLR_SHIFT		9	/* bit 9 */
 /* Must be 1 */
-#define OCRDMA_EQ_TYPE_SHIFT		(10)	/* bit 10 */
+#define OCRDMA_EQ_TYPE_SHIFT		10	/* bit 10 */
 /* Number of event entries processed */
-#define OCRDMA_NUM_EQE_SHIFT		(16)	/* bits 16 - 28 */
+#define OCRDMA_NUM_EQE_SHIFT		16	/* bits 16 - 28 */
 /* Rearm bit */
-#define OCRDMA_REARM_SHIFT		(29)	/* bit 29 */
+#define OCRDMA_REARM_SHIFT		29	/* bit 29 */
 
 #define OCRDMA_MQ_ID_MASK		0x7FF	/* bits 0 - 10 */
 /* Number of entries posted */
-#define OCRDMA_MQ_NUM_MQE_SHIFT	(16)	/* bits 16 - 29 */
+#define OCRDMA_MQ_NUM_MQE_SHIFT	16	/* bits 16 - 29 */
 
-#define OCRDMA_MIN_HPAGE_SIZE (4096)
+#define OCRDMA_MIN_HPAGE_SIZE	4096
 
-#define OCRDMA_MIN_Q_PAGE_SIZE (4096)
-#define OCRDMA_MAX_Q_PAGES     (8)
+#define OCRDMA_MIN_Q_PAGE_SIZE	4096
+#define OCRDMA_MAX_Q_PAGES	8
 
 #define OCRDMA_SLI_ASIC_ID_OFFSET	0x9C
 #define OCRDMA_SLI_ASIC_REV_MASK	0x000000FF
@@ -170,14 +168,14 @@ enum {
 # 6: 256K Bytes
 # 7: 512K Bytes
 */
-#define OCRDMA_MAX_Q_PAGE_SIZE_CNT (8)
+#define OCRDMA_MAX_Q_PAGE_SIZE_CNT	8
 #define OCRDMA_Q_PAGE_BASE_SIZE (OCRDMA_MIN_Q_PAGE_SIZE * OCRDMA_MAX_Q_PAGES)
 
-#define MAX_OCRDMA_QP_PAGES      (8)
+#define MAX_OCRDMA_QP_PAGES		8
 #define OCRDMA_MAX_WQE_MEM_SIZE (MAX_OCRDMA_QP_PAGES * OCRDMA_MIN_HQ_PAGE_SIZE)
 
-#define OCRDMA_CREATE_CQ_MAX_PAGES (4)
-#define OCRDMA_DPP_CQE_SIZE (4)
+#define OCRDMA_CREATE_CQ_MAX_PAGES	4
+#define OCRDMA_DPP_CQE_SIZE		4
 
 #define OCRDMA_GEN2_MAX_CQE 1024
 #define OCRDMA_GEN2_CQ_PAGE_SIZE 4096
@@ -238,7 +236,7 @@ struct ocrdma_mqe_sge {
 
 enum {
 	OCRDMA_MQE_HDR_EMB_SHIFT	= 0,
-	OCRDMA_MQE_HDR_EMB_MASK		= Bit(0),
+	OCRDMA_MQE_HDR_EMB_MASK		= BIT(0),
 	OCRDMA_MQE_HDR_SGE_CNT_SHIFT	= 3,
 	OCRDMA_MQE_HDR_SGE_CNT_MASK	= 0x1F << OCRDMA_MQE_HDR_SGE_CNT_SHIFT,
 	OCRDMA_MQE_HDR_SPECIAL_SHIFT	= 24,
@@ -292,7 +290,7 @@ struct ocrdma_pa {
 	u32 hi;
 };
 
-#define MAX_OCRDMA_EQ_PAGES (8)
+#define MAX_OCRDMA_EQ_PAGES	8
 struct ocrdma_create_eq_req {
 	struct ocrdma_mbx_hdr req;
 	u32 num_pages;
@@ -304,7 +302,7 @@ struct ocrdma_create_eq_req {
 };
 
 enum {
-	OCRDMA_CREATE_EQ_VALID	= Bit(29),
+	OCRDMA_CREATE_EQ_VALID	= BIT(29),
 	OCRDMA_CREATE_EQ_CNT_SHIFT	= 26,
 	OCRDMA_CREATE_CQ_DELAY_SHIFT	= 13,
 };
@@ -314,7 +312,7 @@ struct ocrdma_create_eq_rsp {
 	u32 vector_eqid;
 };
 
-#define OCRDMA_EQ_MINOR_OTHER (0x1)
+#define OCRDMA_EQ_MINOR_OTHER	0x1
 
 enum {
 	OCRDMA_MCQE_STATUS_SHIFT	= 0,
@@ -322,13 +320,13 @@ enum {
 	OCRDMA_MCQE_ESTATUS_SHIFT	= 16,
 	OCRDMA_MCQE_ESTATUS_MASK	= 0xFFFF << OCRDMA_MCQE_ESTATUS_SHIFT,
 	OCRDMA_MCQE_CONS_SHIFT		= 27,
-	OCRDMA_MCQE_CONS_MASK		= Bit(27),
+	OCRDMA_MCQE_CONS_MASK		= BIT(27),
 	OCRDMA_MCQE_CMPL_SHIFT		= 28,
-	OCRDMA_MCQE_CMPL_MASK		= Bit(28),
+	OCRDMA_MCQE_CMPL_MASK		= BIT(28),
 	OCRDMA_MCQE_AE_SHIFT		= 30,
-	OCRDMA_MCQE_AE_MASK		= Bit(30),
+	OCRDMA_MCQE_AE_MASK		= BIT(30),
 	OCRDMA_MCQE_VALID_SHIFT		= 31,
-	OCRDMA_MCQE_VALID_MASK		= Bit(31)
+	OCRDMA_MCQE_VALID_MASK		= BIT(31)
 };
 
 struct ocrdma_mcqe {
@@ -339,13 +337,13 @@ struct ocrdma_mcqe {
 };
 
 enum {
-	OCRDMA_AE_MCQE_QPVALID		= Bit(31),
+	OCRDMA_AE_MCQE_QPVALID		= BIT(31),
 	OCRDMA_AE_MCQE_QPID_MASK	= 0xFFFF,
 
-	OCRDMA_AE_MCQE_CQVALID		= Bit(31),
+	OCRDMA_AE_MCQE_CQVALID		= BIT(31),
 	OCRDMA_AE_MCQE_CQID_MASK	= 0xFFFF,
-	OCRDMA_AE_MCQE_VALID		= Bit(31),
-	OCRDMA_AE_MCQE_AE		= Bit(30),
+	OCRDMA_AE_MCQE_VALID		= BIT(31),
+	OCRDMA_AE_MCQE_AE		= BIT(30),
 	OCRDMA_AE_MCQE_EVENT_TYPE_SHIFT	= 16,
 	OCRDMA_AE_MCQE_EVENT_TYPE_MASK	=
 					0xFF << OCRDMA_AE_MCQE_EVENT_TYPE_SHIFT,
@@ -386,9 +384,9 @@ enum {
 	OCRDMA_AE_MPA_MCQE_EVENT_TYPE_MASK	= 0xFF <<
 					OCRDMA_AE_MPA_MCQE_EVENT_TYPE_SHIFT,
 	OCRDMA_AE_MPA_MCQE_EVENT_AE_SHIFT	= 30,
-	OCRDMA_AE_MPA_MCQE_EVENT_AE_MASK	= Bit(30),
+	OCRDMA_AE_MPA_MCQE_EVENT_AE_MASK	= BIT(30),
 	OCRDMA_AE_MPA_MCQE_EVENT_VALID_SHIFT	= 31,
-	OCRDMA_AE_MPA_MCQE_EVENT_VALID_MASK	= Bit(31)
+	OCRDMA_AE_MPA_MCQE_EVENT_VALID_MASK	= BIT(31)
 };
 
 struct ocrdma_ae_mpa_mcqe {
@@ -412,9 +410,9 @@ enum {
 	OCRDMA_AE_QP_MCQE_EVENT_TYPE_MASK	= 0xFF <<
 				OCRDMA_AE_QP_MCQE_EVENT_TYPE_SHIFT,
 	OCRDMA_AE_QP_MCQE_EVENT_AE_SHIFT	= 30,
-	OCRDMA_AE_QP_MCQE_EVENT_AE_MASK		= Bit(30),
+	OCRDMA_AE_QP_MCQE_EVENT_AE_MASK		= BIT(30),
 	OCRDMA_AE_QP_MCQE_EVENT_VALID_SHIFT	= 31,
-	OCRDMA_AE_QP_MCQE_EVENT_VALID_MASK	= Bit(31)
+	OCRDMA_AE_QP_MCQE_EVENT_VALID_MASK	= BIT(31)
 };
 
 struct ocrdma_ae_qp_mcqe {
@@ -449,9 +447,9 @@ enum OCRDMA_ASYNC_EVENT_TYPE {
 /* mailbox command request and responses */
 enum {
 	OCRDMA_MBX_QUERY_CFG_CQ_OVERFLOW_SHIFT		= 2,
-	OCRDMA_MBX_QUERY_CFG_CQ_OVERFLOW_MASK		= Bit(2),
+	OCRDMA_MBX_QUERY_CFG_CQ_OVERFLOW_MASK		= BIT(2),
 	OCRDMA_MBX_QUERY_CFG_SRQ_SUPPORTED_SHIFT	= 3,
-	OCRDMA_MBX_QUERY_CFG_SRQ_SUPPORTED_MASK		= Bit(3),
+	OCRDMA_MBX_QUERY_CFG_SRQ_SUPPORTED_MASK		= BIT(3),
 	OCRDMA_MBX_QUERY_CFG_MAX_QP_SHIFT		= 8,
 	OCRDMA_MBX_QUERY_CFG_MAX_QP_MASK		= 0xFFFFFF <<
 				OCRDMA_MBX_QUERY_CFG_MAX_QP_SHIFT,
@@ -672,9 +670,9 @@ enum {
 	OCRDMA_CREATE_CQ_PAGE_SIZE_MASK		= 0xFF,
 
 	OCRDMA_CREATE_CQ_COALESCWM_SHIFT	= 12,
-	OCRDMA_CREATE_CQ_COALESCWM_MASK		= Bit(13) | Bit(12),
-	OCRDMA_CREATE_CQ_FLAGS_NODELAY		= Bit(14),
-	OCRDMA_CREATE_CQ_FLAGS_AUTO_VALID	= Bit(15),
+	OCRDMA_CREATE_CQ_COALESCWM_MASK		= BIT(13) | BIT(12),
+	OCRDMA_CREATE_CQ_FLAGS_NODELAY		= BIT(14),
+	OCRDMA_CREATE_CQ_FLAGS_AUTO_VALID	= BIT(15),
 
 	OCRDMA_CREATE_CQ_EQ_ID_MASK		= 0xFFFF,
 	OCRDMA_CREATE_CQ_CQE_COUNT_MASK		= 0xFFFF
@@ -687,8 +685,8 @@ enum {
 	OCRDMA_CREATE_CQ_EQID_SHIFT		= 22,
 
 	OCRDMA_CREATE_CQ_CNT_SHIFT		= 27,
-	OCRDMA_CREATE_CQ_FLAGS_VALID		= Bit(29),
-	OCRDMA_CREATE_CQ_FLAGS_EVENTABLE	= Bit(31),
+	OCRDMA_CREATE_CQ_FLAGS_VALID		= BIT(29),
+	OCRDMA_CREATE_CQ_FLAGS_EVENTABLE	= BIT(31),
 	OCRDMA_CREATE_CQ_DEF_FLAGS		= OCRDMA_CREATE_CQ_FLAGS_VALID |
 					OCRDMA_CREATE_CQ_FLAGS_EVENTABLE |
 					OCRDMA_CREATE_CQ_FLAGS_NODELAY
@@ -731,8 +729,8 @@ enum {
 	OCRDMA_CREATE_MQ_V0_CQ_ID_SHIFT		= 22,
 	OCRDMA_CREATE_MQ_CQ_ID_SHIFT		= 16,
 	OCRDMA_CREATE_MQ_RING_SIZE_SHIFT	= 16,
-	OCRDMA_CREATE_MQ_VALID			= Bit(31),
-	OCRDMA_CREATE_MQ_ASYNC_CQ_VALID		= Bit(0)
+	OCRDMA_CREATE_MQ_VALID			= BIT(31),
+	OCRDMA_CREATE_MQ_ASYNC_CQ_VALID		= BIT(0)
 };
 
 struct ocrdma_create_mq_req {
@@ -783,7 +781,7 @@ enum {
 	OCRDMA_CREATE_QP_REQ_SQ_PAGE_SIZE_SHIFT	= 16,
 	OCRDMA_CREATE_QP_REQ_RQ_PAGE_SIZE_SHIFT	= 19,
 	OCRDMA_CREATE_QP_REQ_QPT_SHIFT		= 29,
-	OCRDMA_CREATE_QP_REQ_QPT_MASK		= Bit(31) | Bit(30) | Bit(29),
+	OCRDMA_CREATE_QP_REQ_QPT_MASK		= BIT(31) | BIT(30) | BIT(29),
 
 	OCRDMA_CREATE_QP_REQ_MAX_RQE_SHIFT	= 0,
 	OCRDMA_CREATE_QP_REQ_MAX_RQE_MASK	= 0xFFFF,
@@ -798,23 +796,23 @@ enum {
 					OCRDMA_CREATE_QP_REQ_MAX_SGE_SEND_SHIFT,
 
 	OCRDMA_CREATE_QP_REQ_FMR_EN_SHIFT		= 0,
-	OCRDMA_CREATE_QP_REQ_FMR_EN_MASK		= Bit(0),
+	OCRDMA_CREATE_QP_REQ_FMR_EN_MASK		= BIT(0),
 	OCRDMA_CREATE_QP_REQ_ZERO_LKEYEN_SHIFT		= 1,
-	OCRDMA_CREATE_QP_REQ_ZERO_LKEYEN_MASK		= Bit(1),
+	OCRDMA_CREATE_QP_REQ_ZERO_LKEYEN_MASK		= BIT(1),
 	OCRDMA_CREATE_QP_REQ_BIND_MEMWIN_SHIFT		= 2,
-	OCRDMA_CREATE_QP_REQ_BIND_MEMWIN_MASK		= Bit(2),
+	OCRDMA_CREATE_QP_REQ_BIND_MEMWIN_MASK		= BIT(2),
 	OCRDMA_CREATE_QP_REQ_INB_WREN_SHIFT		= 3,
-	OCRDMA_CREATE_QP_REQ_INB_WREN_MASK		= Bit(3),
+	OCRDMA_CREATE_QP_REQ_INB_WREN_MASK		= BIT(3),
 	OCRDMA_CREATE_QP_REQ_INB_RDEN_SHIFT		= 4,
-	OCRDMA_CREATE_QP_REQ_INB_RDEN_MASK		= Bit(4),
+	OCRDMA_CREATE_QP_REQ_INB_RDEN_MASK		= BIT(4),
 	OCRDMA_CREATE_QP_REQ_USE_SRQ_SHIFT		= 5,
-	OCRDMA_CREATE_QP_REQ_USE_SRQ_MASK		= Bit(5),
+	OCRDMA_CREATE_QP_REQ_USE_SRQ_MASK		= BIT(5),
 	OCRDMA_CREATE_QP_REQ_ENABLE_RPIR_SHIFT		= 6,
-	OCRDMA_CREATE_QP_REQ_ENABLE_RPIR_MASK		= Bit(6),
+	OCRDMA_CREATE_QP_REQ_ENABLE_RPIR_MASK		= BIT(6),
 	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_SHIFT		= 7,
-	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_MASK		= Bit(7),
+	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_MASK		= BIT(7),
 	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_CQ_SHIFT	= 8,
-	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_CQ_MASK		= Bit(8),
+	OCRDMA_CREATE_QP_REQ_ENABLE_DPP_CQ_MASK		= BIT(8),
 	OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_SHIFT		= 16,
 	OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_MASK		= 0xFFFF <<
 				OCRDMA_CREATE_QP_REQ_MAX_SGE_RECV_SHIFT,
@@ -927,7 +925,7 @@ enum {
 	OCRDMA_CREATE_QP_RSP_SQ_ID_MASK			= 0xFFFF <<
 				OCRDMA_CREATE_QP_RSP_SQ_ID_SHIFT,
 
-	OCRDMA_CREATE_QP_RSP_DPP_ENABLED_MASK		= Bit(0),
+	OCRDMA_CREATE_QP_RSP_DPP_ENABLED_MASK		= BIT(0),
 	OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_SHIFT	= 1,
 	OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_MASK	= 0x7FFF <<
 				OCRDMA_CREATE_QP_RSP_DPP_PAGE_OFFSET_SHIFT,
@@ -964,38 +962,38 @@ enum {
 	OCRDMA_MODIFY_QP_ID_SHIFT	= 0,
 	OCRDMA_MODIFY_QP_ID_MASK	= 0xFFFF,
 
-	OCRDMA_QP_PARA_QPS_VALID	= Bit(0),
-	OCRDMA_QP_PARA_SQD_ASYNC_VALID	= Bit(1),
-	OCRDMA_QP_PARA_PKEY_VALID	= Bit(2),
-	OCRDMA_QP_PARA_QKEY_VALID	= Bit(3),
-	OCRDMA_QP_PARA_PMTU_VALID	= Bit(4),
-	OCRDMA_QP_PARA_ACK_TO_VALID	= Bit(5),
-	OCRDMA_QP_PARA_RETRY_CNT_VALID	= Bit(6),
-	OCRDMA_QP_PARA_RRC_VALID	= Bit(7),
-	OCRDMA_QP_PARA_RQPSN_VALID	= Bit(8),
-	OCRDMA_QP_PARA_MAX_IRD_VALID	= Bit(9),
-	OCRDMA_QP_PARA_MAX_ORD_VALID	= Bit(10),
-	OCRDMA_QP_PARA_RNT_VALID	= Bit(11),
-	OCRDMA_QP_PARA_SQPSN_VALID	= Bit(12),
-	OCRDMA_QP_PARA_DST_QPN_VALID	= Bit(13),
-	OCRDMA_QP_PARA_MAX_WQE_VALID	= Bit(14),
-	OCRDMA_QP_PARA_MAX_RQE_VALID	= Bit(15),
-	OCRDMA_QP_PARA_SGE_SEND_VALID	= Bit(16),
-	OCRDMA_QP_PARA_SGE_RECV_VALID	= Bit(17),
-	OCRDMA_QP_PARA_SGE_WR_VALID	= Bit(18),
-	OCRDMA_QP_PARA_INB_RDEN_VALID	= Bit(19),
-	OCRDMA_QP_PARA_INB_WREN_VALID	= Bit(20),
-	OCRDMA_QP_PARA_FLOW_LBL_VALID	= Bit(21),
-	OCRDMA_QP_PARA_BIND_EN_VALID	= Bit(22),
-	OCRDMA_QP_PARA_ZLKEY_EN_VALID	= Bit(23),
-	OCRDMA_QP_PARA_FMR_EN_VALID	= Bit(24),
-	OCRDMA_QP_PARA_INBAT_EN_VALID	= Bit(25),
-	OCRDMA_QP_PARA_VLAN_EN_VALID	= Bit(26),
-
-	OCRDMA_MODIFY_QP_FLAGS_RD	= Bit(0),
-	OCRDMA_MODIFY_QP_FLAGS_WR	= Bit(1),
-	OCRDMA_MODIFY_QP_FLAGS_SEND	= Bit(2),
-	OCRDMA_MODIFY_QP_FLAGS_ATOMIC	= Bit(3)
+	OCRDMA_QP_PARA_QPS_VALID	= BIT(0),
+	OCRDMA_QP_PARA_SQD_ASYNC_VALID	= BIT(1),
+	OCRDMA_QP_PARA_PKEY_VALID	= BIT(2),
+	OCRDMA_QP_PARA_QKEY_VALID	= BIT(3),
+	OCRDMA_QP_PARA_PMTU_VALID	= BIT(4),
+	OCRDMA_QP_PARA_ACK_TO_VALID	= BIT(5),
+	OCRDMA_QP_PARA_RETRY_CNT_VALID	= BIT(6),
+	OCRDMA_QP_PARA_RRC_VALID	= BIT(7),
+	OCRDMA_QP_PARA_RQPSN_VALID	= BIT(8),
+	OCRDMA_QP_PARA_MAX_IRD_VALID	= BIT(9),
+	OCRDMA_QP_PARA_MAX_ORD_VALID	= BIT(10),
+	OCRDMA_QP_PARA_RNT_VALID	= BIT(11),
+	OCRDMA_QP_PARA_SQPSN_VALID	= BIT(12),
+	OCRDMA_QP_PARA_DST_QPN_VALID	= BIT(13),
+	OCRDMA_QP_PARA_MAX_WQE_VALID	= BIT(14),
+	OCRDMA_QP_PARA_MAX_RQE_VALID	= BIT(15),
+	OCRDMA_QP_PARA_SGE_SEND_VALID	= BIT(16),
+	OCRDMA_QP_PARA_SGE_RECV_VALID	= BIT(17),
+	OCRDMA_QP_PARA_SGE_WR_VALID	= BIT(18),
+	OCRDMA_QP_PARA_INB_RDEN_VALID	= BIT(19),
+	OCRDMA_QP_PARA_INB_WREN_VALID	= BIT(20),
+	OCRDMA_QP_PARA_FLOW_LBL_VALID	= BIT(21),
+	OCRDMA_QP_PARA_BIND_EN_VALID	= BIT(22),
+	OCRDMA_QP_PARA_ZLKEY_EN_VALID	= BIT(23),
+	OCRDMA_QP_PARA_FMR_EN_VALID	= BIT(24),
+	OCRDMA_QP_PARA_INBAT_EN_VALID	= BIT(25),
+	OCRDMA_QP_PARA_VLAN_EN_VALID	= BIT(26),
+
+	OCRDMA_MODIFY_QP_FLAGS_RD	= BIT(0),
+	OCRDMA_MODIFY_QP_FLAGS_WR	= BIT(1),
+	OCRDMA_MODIFY_QP_FLAGS_SEND	= BIT(2),
+	OCRDMA_MODIFY_QP_FLAGS_ATOMIC	= BIT(3)
 };
 
 enum {
@@ -1014,15 +1012,15 @@ enum {
 	OCRDMA_QP_PARAMS_MAX_SGE_SEND_MASK	= 0xFFFF <<
 					OCRDMA_QP_PARAMS_MAX_SGE_SEND_SHIFT,
 
-	OCRDMA_QP_PARAMS_FLAGS_FMR_EN		= Bit(0),
-	OCRDMA_QP_PARAMS_FLAGS_LKEY_0_EN	= Bit(1),
-	OCRDMA_QP_PARAMS_FLAGS_BIND_MW_EN	= Bit(2),
-	OCRDMA_QP_PARAMS_FLAGS_INBWR_EN		= Bit(3),
-	OCRDMA_QP_PARAMS_FLAGS_INBRD_EN		= Bit(4),
+	OCRDMA_QP_PARAMS_FLAGS_FMR_EN		= BIT(0),
+	OCRDMA_QP_PARAMS_FLAGS_LKEY_0_EN	= BIT(1),
+	OCRDMA_QP_PARAMS_FLAGS_BIND_MW_EN	= BIT(2),
+	OCRDMA_QP_PARAMS_FLAGS_INBWR_EN		= BIT(3),
+	OCRDMA_QP_PARAMS_FLAGS_INBRD_EN		= BIT(4),
 	OCRDMA_QP_PARAMS_STATE_SHIFT		= 5,
-	OCRDMA_QP_PARAMS_STATE_MASK		= Bit(5) | Bit(6) | Bit(7),
-	OCRDMA_QP_PARAMS_FLAGS_SQD_ASYNC	= Bit(8),
-	OCRDMA_QP_PARAMS_FLAGS_INB_ATEN		= Bit(9),
+	OCRDMA_QP_PARAMS_STATE_MASK		= BIT(5) | BIT(6) | BIT(7),
+	OCRDMA_QP_PARAMS_FLAGS_SQD_ASYNC	= BIT(8),
+	OCRDMA_QP_PARAMS_FLAGS_INB_ATEN		= BIT(9),
 	OCRDMA_QP_PARAMS_MAX_SGE_RECV_SHIFT	= 16,
 	OCRDMA_QP_PARAMS_MAX_SGE_RECV_MASK	= 0xFFFF <<
 					OCRDMA_QP_PARAMS_MAX_SGE_RECV_SHIFT,
@@ -1277,7 +1275,7 @@ struct ocrdma_alloc_pd {
 };
 
 enum {
-	OCRDMA_ALLOC_PD_RSP_DPP			= Bit(16),
+	OCRDMA_ALLOC_PD_RSP_DPP			= BIT(16),
 	OCRDMA_ALLOC_PD_RSP_DPP_PAGE_SHIFT	= 20,
 	OCRDMA_ALLOC_PD_RSP_PDID_MASK		= 0xFFFF,
 };
@@ -1309,18 +1307,18 @@ enum {
 	OCRDMA_ALLOC_LKEY_PD_ID_MASK		= 0xFFFF,
 
 	OCRDMA_ALLOC_LKEY_ADDR_CHECK_SHIFT	= 0,
-	OCRDMA_ALLOC_LKEY_ADDR_CHECK_MASK	= Bit(0),
+	OCRDMA_ALLOC_LKEY_ADDR_CHECK_MASK	= BIT(0),
 	OCRDMA_ALLOC_LKEY_FMR_SHIFT		= 1,
-	OCRDMA_ALLOC_LKEY_FMR_MASK		= Bit(1),
+	OCRDMA_ALLOC_LKEY_FMR_MASK		= BIT(1),
 	OCRDMA_ALLOC_LKEY_REMOTE_INV_SHIFT	= 2,
-	OCRDMA_ALLOC_LKEY_REMOTE_INV_MASK	= Bit(2),
+	OCRDMA_ALLOC_LKEY_REMOTE_INV_MASK	= BIT(2),
 	OCRDMA_ALLOC_LKEY_REMOTE_WR_SHIFT	= 3,
-	OCRDMA_ALLOC_LKEY_REMOTE_WR_MASK	= Bit(3),
+	OCRDMA_ALLOC_LKEY_REMOTE_WR_MASK	= BIT(3),
 	OCRDMA_ALLOC_LKEY_REMOTE_RD_SHIFT	= 4,
-	OCRDMA_ALLOC_LKEY_REMOTE_RD_MASK	= Bit(4),
+	OCRDMA_ALLOC_LKEY_REMOTE_RD_MASK	= BIT(4),
 	OCRDMA_ALLOC_LKEY_LOCAL_WR_SHIFT	= 5,
-	OCRDMA_ALLOC_LKEY_LOCAL_WR_MASK		= Bit(5),
-	OCRDMA_ALLOC_LKEY_REMOTE_ATOMIC_MASK	= Bit(6),
+	OCRDMA_ALLOC_LKEY_LOCAL_WR_MASK		= BIT(5),
+	OCRDMA_ALLOC_LKEY_REMOTE_ATOMIC_MASK	= BIT(6),
 	OCRDMA_ALLOC_LKEY_REMOTE_ATOMIC_SHIFT	= 6,
 	OCRDMA_ALLOC_LKEY_PBL_SIZE_SHIFT	= 16,
 	OCRDMA_ALLOC_LKEY_PBL_SIZE_MASK		= 0xFFFF <<
@@ -1379,21 +1377,21 @@ enum {
 	OCRDMA_REG_NSMR_HPAGE_SIZE_MASK		= 0xFF <<
 					OCRDMA_REG_NSMR_HPAGE_SIZE_SHIFT,
 	OCRDMA_REG_NSMR_BIND_MEMWIN_SHIFT	= 24,
-	OCRDMA_REG_NSMR_BIND_MEMWIN_MASK	= Bit(24),
+	OCRDMA_REG_NSMR_BIND_MEMWIN_MASK	= BIT(24),
 	OCRDMA_REG_NSMR_ZB_SHIFT		= 25,
-	OCRDMA_REG_NSMR_ZB_SHIFT_MASK		= Bit(25),
+	OCRDMA_REG_NSMR_ZB_SHIFT_MASK		= BIT(25),
 	OCRDMA_REG_NSMR_REMOTE_INV_SHIFT	= 26,
-	OCRDMA_REG_NSMR_REMOTE_INV_MASK		= Bit(26),
+	OCRDMA_REG_NSMR_REMOTE_INV_MASK		= BIT(26),
 	OCRDMA_REG_NSMR_REMOTE_WR_SHIFT		= 27,
-	OCRDMA_REG_NSMR_REMOTE_WR_MASK		= Bit(27),
+	OCRDMA_REG_NSMR_REMOTE_WR_MASK		= BIT(27),
 	OCRDMA_REG_NSMR_REMOTE_RD_SHIFT		= 28,
-	OCRDMA_REG_NSMR_REMOTE_RD_MASK		= Bit(28),
+	OCRDMA_REG_NSMR_REMOTE_RD_MASK		= BIT(28),
 	OCRDMA_REG_NSMR_LOCAL_WR_SHIFT		= 29,
-	OCRDMA_REG_NSMR_LOCAL_WR_MASK		= Bit(29),
+	OCRDMA_REG_NSMR_LOCAL_WR_MASK		= BIT(29),
 	OCRDMA_REG_NSMR_REMOTE_ATOMIC_SHIFT	= 30,
-	OCRDMA_REG_NSMR_REMOTE_ATOMIC_MASK	= Bit(30),
+	OCRDMA_REG_NSMR_REMOTE_ATOMIC_MASK	= BIT(30),
 	OCRDMA_REG_NSMR_LAST_SHIFT		= 31,
-	OCRDMA_REG_NSMR_LAST_MASK		= Bit(31)
+	OCRDMA_REG_NSMR_LAST_MASK		= BIT(31)
 };
 
 struct ocrdma_reg_nsmr {
@@ -1420,7 +1418,7 @@ enum {
 					OCRDMA_REG_NSMR_CONT_NUM_PBL_SHIFT,
 
 	OCRDMA_REG_NSMR_CONT_LAST_SHIFT		= 31,
-	OCRDMA_REG_NSMR_CONT_LAST_MASK		= Bit(31)
+	OCRDMA_REG_NSMR_CONT_LAST_MASK		= BIT(31)
 };
 
 struct ocrdma_reg_nsmr_cont {
@@ -1566,7 +1564,7 @@ struct ocrdma_delete_ah_tbl_rsp {
 
 enum {
 	OCRDMA_EQE_VALID_SHIFT		= 0,
-	OCRDMA_EQE_VALID_MASK		= Bit(0),
+	OCRDMA_EQE_VALID_MASK		= BIT(0),
 	OCRDMA_EQE_FOR_CQE_MASK		= 0xFFFE,
 	OCRDMA_EQE_RESOURCE_ID_SHIFT	= 16,
 	OCRDMA_EQE_RESOURCE_ID_MASK	= 0xFFFF <<
@@ -1624,11 +1622,11 @@ enum {
 	OCRDMA_CQE_UD_STATUS_MASK	= 0x7 << OCRDMA_CQE_UD_STATUS_SHIFT,
 	OCRDMA_CQE_STATUS_SHIFT		= 16,
 	OCRDMA_CQE_STATUS_MASK		= 0xFF << OCRDMA_CQE_STATUS_SHIFT,
-	OCRDMA_CQE_VALID		= Bit(31),
-	OCRDMA_CQE_INVALIDATE		= Bit(30),
-	OCRDMA_CQE_QTYPE		= Bit(29),
-	OCRDMA_CQE_IMM			= Bit(28),
-	OCRDMA_CQE_WRITE_IMM		= Bit(27),
+	OCRDMA_CQE_VALID		= BIT(31),
+	OCRDMA_CQE_INVALIDATE		= BIT(30),
+	OCRDMA_CQE_QTYPE		= BIT(29),
+	OCRDMA_CQE_IMM			= BIT(28),
+	OCRDMA_CQE_WRITE_IMM		= BIT(27),
 	OCRDMA_CQE_QTYPE_SQ		= 0,
 	OCRDMA_CQE_QTYPE_RQ		= 1,
 	OCRDMA_CQE_SRCQP_MASK		= 0xFFFFFF
@@ -1772,8 +1770,8 @@ struct ocrdma_grh {
 	u16	rsvd;
 } __packed;
 
-#define OCRDMA_AV_VALID		Bit(7)
-#define OCRDMA_AV_VLAN_VALID	Bit(1)
+#define OCRDMA_AV_VALID		BIT(7)
+#define OCRDMA_AV_VLAN_VALID	BIT(1)
 
 struct ocrdma_av {
 	struct ocrdma_eth_vlan eth_hdr;
diff --git a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
index acb434d16903..4c68305ee781 100644
--- a/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
+++ b/drivers/infiniband/hw/ocrdma/ocrdma_verbs.c
@@ -101,7 +101,7 @@ int ocrdma_query_device(struct ib_device *ibdev, struct ib_device_attr *attr)
 	attr->max_srq_sge = dev->attr.max_srq_sge;
 	attr->max_srq_wr = dev->attr.max_rqe;
 	attr->local_ca_ack_delay = dev->attr.local_ca_ack_delay;
-	attr->max_fast_reg_page_list_len = 0;
+	attr->max_fast_reg_page_list_len = dev->attr.max_pages_per_frmr;
 	attr->max_pkeys = 1;
 	return 0;
 }
@@ -388,7 +388,7 @@ struct ib_ucontext *ocrdma_alloc_ucontext(struct ib_device *ibdev,
 
 	memset(&resp, 0, sizeof(resp));
 	resp.ah_tbl_len = ctx->ah_tbl.len;
-	resp.ah_tbl_page = ctx->ah_tbl.pa;
+	resp.ah_tbl_page = virt_to_phys(ctx->ah_tbl.va);
 
 	status = ocrdma_add_mmap(ctx, resp.ah_tbl_page, resp.ah_tbl_len);
 	if (status)
@@ -870,7 +870,7 @@ static int ocrdma_copy_cq_uresp(struct ocrdma_dev *dev, struct ocrdma_cq *cq,
 	uresp.page_size = PAGE_ALIGN(cq->len);
 	uresp.num_pages = 1;
 	uresp.max_hw_cqe = cq->max_hw_cqe;
-	uresp.page_addr[0] = cq->pa;
+	uresp.page_addr[0] = virt_to_phys(cq->va);
 	uresp.db_page_addr =  ocrdma_get_db_addr(dev, uctx->cntxt_pd->id);
 	uresp.db_page_size = dev->nic_info.db_page_size;
 	uresp.phase_change = cq->phase_change ? 1 : 0;
@@ -1123,13 +1123,13 @@ static int ocrdma_copy_qp_uresp(struct ocrdma_qp *qp,
 	uresp.sq_dbid = qp->sq.dbid;
 	uresp.num_sq_pages = 1;
 	uresp.sq_page_size = PAGE_ALIGN(qp->sq.len);
-	uresp.sq_page_addr[0] = qp->sq.pa;
+	uresp.sq_page_addr[0] = virt_to_phys(qp->sq.va);
 	uresp.num_wqe_allocated = qp->sq.max_cnt;
 	if (!srq) {
 		uresp.rq_dbid = qp->rq.dbid;
 		uresp.num_rq_pages = 1;
 		uresp.rq_page_size = PAGE_ALIGN(qp->rq.len);
-		uresp.rq_page_addr[0] = qp->rq.pa;
+		uresp.rq_page_addr[0] = virt_to_phys(qp->rq.va);
 		uresp.num_rqe_allocated = qp->rq.max_cnt;
 	}
 	uresp.db_page_addr = usr_db;
@@ -1680,7 +1680,7 @@ static int ocrdma_copy_srq_uresp(struct ocrdma_dev *dev, struct ocrdma_srq *srq,
 	memset(&uresp, 0, sizeof(uresp));
 	uresp.rq_dbid = srq->rq.dbid;
 	uresp.num_rq_pages = 1;
-	uresp.rq_page_addr[0] = srq->rq.pa;
+	uresp.rq_page_addr[0] = virt_to_phys(srq->rq.va);
 	uresp.rq_page_size = srq->rq.len;
 	uresp.db_page_addr = dev->nic_info.unmapped_db +
 	    (srq->pd->id * dev->nic_info.db_page_size);
@@ -1870,7 +1870,7 @@ static int ocrdma_build_inline_sges(struct ocrdma_qp *qp,
 		hdr->total_len = ocrdma_sglist_len(wr->sg_list, wr->num_sge);
 		if (unlikely(hdr->total_len > qp->max_inline_data)) {
 			pr_err("%s() supported_len=0x%x,\n"
-			       " unspported len req=0x%x\n", __func__,
+			       " unsupported len req=0x%x\n", __func__,
 				qp->max_inline_data, hdr->total_len);
 			return -EINVAL;
 		}
@@ -2846,11 +2846,9 @@ int ocrdma_arm_cq(struct ib_cq *ibcq, enum ib_cq_notify_flags cq_flags)
 	if (cq->first_arm) {
 		ocrdma_ring_cq_db(dev, cq_id, arm_needed, sol_needed, 0);
 		cq->first_arm = false;
-		goto skip_defer;
 	}
-	cq->deferred_arm = true;
 
-skip_defer:
+	cq->deferred_arm = true;
 	cq->deferred_sol = sol_needed;
 	spin_unlock_irqrestore(&cq->cq_lock, flags);
 
diff --git a/drivers/infiniband/hw/qib/qib_debugfs.c b/drivers/infiniband/hw/qib/qib_debugfs.c
index 799a0c3bffc4..6abd3ed3cd51 100644
--- a/drivers/infiniband/hw/qib/qib_debugfs.c
+++ b/drivers/infiniband/hw/qib/qib_debugfs.c
@@ -193,6 +193,7 @@ static void *_qp_stats_seq_start(struct seq_file *s, loff_t *pos)
 	struct qib_qp_iter *iter;
 	loff_t n = *pos;
 
+	rcu_read_lock();
 	iter = qib_qp_iter_init(s->private);
 	if (!iter)
 		return NULL;
@@ -224,7 +225,7 @@ static void *_qp_stats_seq_next(struct seq_file *s, void *iter_ptr,
 
 static void _qp_stats_seq_stop(struct seq_file *s, void *iter_ptr)
 {
-	/* nothing for now */
+	rcu_read_unlock();
 }
 
 static int _qp_stats_seq_show(struct seq_file *s, void *iter_ptr)
diff --git a/drivers/infiniband/hw/qib/qib_fs.c b/drivers/infiniband/hw/qib/qib_fs.c
index cab610ccd50e..81854586c081 100644
--- a/drivers/infiniband/hw/qib/qib_fs.c
+++ b/drivers/infiniband/hw/qib/qib_fs.c
@@ -89,7 +89,6 @@ static int create_file(const char *name, umode_t mode,
 {
 	int error;
 
-	*dentry = NULL;
 	mutex_lock(&parent->d_inode->i_mutex);
 	*dentry = lookup_one_len(name, parent, strlen(name));
 	if (!IS_ERR(*dentry))
diff --git a/drivers/infiniband/hw/qib/qib_qp.c b/drivers/infiniband/hw/qib/qib_qp.c
index 7fcc150d603c..6ddc0264aad2 100644
--- a/drivers/infiniband/hw/qib/qib_qp.c
+++ b/drivers/infiniband/hw/qib/qib_qp.c
@@ -1325,7 +1325,6 @@ int qib_qp_iter_next(struct qib_qp_iter *iter)
 	struct qib_qp *pqp = iter->qp;
 	struct qib_qp *qp;
 
-	rcu_read_lock();
 	for (; n < dev->qp_table_size; n++) {
 		if (pqp)
 			qp = rcu_dereference(pqp->next);
@@ -1333,18 +1332,11 @@ int qib_qp_iter_next(struct qib_qp_iter *iter)
 			qp = rcu_dereference(dev->qp_table[n]);
 		pqp = qp;
 		if (qp) {
-			if (iter->qp)
-				atomic_dec(&iter->qp->refcount);
-			atomic_inc(&qp->refcount);
-			rcu_read_unlock();
 			iter->qp = qp;
 			iter->n = n;
 			return 0;
 		}
 	}
-	rcu_read_unlock();
-	if (iter->qp)
-		atomic_dec(&iter->qp->refcount);
 	return ret;
 }
 
diff --git a/drivers/infiniband/hw/qib/qib_user_pages.c b/drivers/infiniband/hw/qib/qib_user_pages.c
index 2bc1d2b96298..74f90b2619f6 100644
--- a/drivers/infiniband/hw/qib/qib_user_pages.c
+++ b/drivers/infiniband/hw/qib/qib_user_pages.c
@@ -52,7 +52,7 @@ static void __qib_release_user_pages(struct page **p, size_t num_pages,
  * Call with current->mm->mmap_sem held.
  */
 static int __qib_get_user_pages(unsigned long start_page, size_t num_pages,
-				struct page **p, struct vm_area_struct **vma)
+				struct page **p)
 {
 	unsigned long lock_limit;
 	size_t got;
@@ -69,7 +69,7 @@ static int __qib_get_user_pages(unsigned long start_page, size_t num_pages,
 		ret = get_user_pages(current, current->mm,
 				     start_page + got * PAGE_SIZE,
 				     num_pages - got, 1, 1,
-				     p + got, vma);
+				     p + got, NULL);
 		if (ret < 0)
 			goto bail_release;
 	}
@@ -136,7 +136,7 @@ int qib_get_user_pages(unsigned long start_page, size_t num_pages,
 
 	down_write(&current->mm->mmap_sem);
 
-	ret = __qib_get_user_pages(start_page, num_pages, p, NULL);
+	ret = __qib_get_user_pages(start_page, num_pages, p);
 
 	up_write(&current->mm->mmap_sem);
 
diff --git a/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c b/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c
index f8dfd76be89f..db3588df3546 100644
--- a/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c
+++ b/drivers/infiniband/hw/usnic/usnic_ib_qp_grp.c
@@ -511,7 +511,7 @@ int usnic_ib_qp_grp_modify(struct usnic_ib_qp_grp *qp_grp,
 		usnic_ib_qp_grp_state_to_string(old_state),
 		usnic_ib_qp_grp_state_to_string(new_state));
 	} else {
-		usnic_err("Failed to transistion %u from %s to %s",
+		usnic_err("Failed to transition %u from %s to %s",
 		qp_grp->grp_id,
 		usnic_ib_qp_grp_state_to_string(old_state),
 		usnic_ib_qp_grp_state_to_string(new_state));
diff --git a/drivers/infiniband/hw/usnic/usnic_uiom.c b/drivers/infiniband/hw/usnic/usnic_uiom.c
index 801a1d6937e4..417de1f32960 100644
--- a/drivers/infiniband/hw/usnic/usnic_uiom.c
+++ b/drivers/infiniband/hw/usnic/usnic_uiom.c
@@ -507,7 +507,7 @@ int usnic_uiom_attach_dev_to_pd(struct usnic_uiom_pd *pd, struct device *dev)
 	if (err)
 		goto out_free_dev;
 
-	if (!iommu_domain_has_cap(pd->domain, IOMMU_CAP_CACHE_COHERENCY)) {
+	if (!iommu_capable(dev->bus, IOMMU_CAP_CACHE_COHERENCY)) {
 		usnic_err("IOMMU of %s does not support cache coherency\n",
 				dev_name(dev));
 		err = -EINVAL;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib.h b/drivers/infiniband/ulp/ipoib/ipoib.h
index 3edce617c31b..d7562beb5423 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib.h
+++ b/drivers/infiniband/ulp/ipoib/ipoib.h
@@ -131,6 +131,12 @@ struct ipoib_cb {
 	u8			hwaddr[INFINIBAND_ALEN];
 };
 
+static inline struct ipoib_cb *ipoib_skb_cb(const struct sk_buff *skb)
+{
+	BUILD_BUG_ON(sizeof(skb->cb) < sizeof(struct ipoib_cb));
+	return (struct ipoib_cb *)skb->cb;
+}
+
 /* Used for all multicast joins (broadcast, IPv4 mcast and IPv6 mcast) */
 struct ipoib_mcast {
 	struct ib_sa_mcmember_rec mcmember;
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_main.c b/drivers/infiniband/ulp/ipoib/ipoib_main.c
index 1310acf6bf92..58b5aa3b6f2d 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_main.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_main.c
@@ -716,7 +716,7 @@ static int ipoib_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct ipoib_dev_priv *priv = netdev_priv(dev);
 	struct ipoib_neigh *neigh;
-	struct ipoib_cb *cb = (struct ipoib_cb *) skb->cb;
+	struct ipoib_cb *cb = ipoib_skb_cb(skb);
 	struct ipoib_header *header;
 	unsigned long flags;
 
@@ -813,7 +813,7 @@ static int ipoib_hard_header(struct sk_buff *skb,
 			     const void *daddr, const void *saddr, unsigned len)
 {
 	struct ipoib_header *header;
-	struct ipoib_cb *cb = (struct ipoib_cb *) skb->cb;
+	struct ipoib_cb *cb = ipoib_skb_cb(skb);
 
 	header = (struct ipoib_header *) skb_push(skb, sizeof *header);
 
@@ -1364,7 +1364,7 @@ void ipoib_setup(struct net_device *dev)
 	dev->tx_queue_len	 = ipoib_sendq_size * 2;
 	dev->features		 = (NETIF_F_VLAN_CHALLENGED	|
 				    NETIF_F_HIGHDMA);
-	dev->priv_flags		&= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(dev);
 
 	memcpy(dev->broadcast, ipv4_bcast_addr, INFINIBAND_ALEN);
 
diff --git a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
index d4e005720d01..ffb83b5f7e80 100644
--- a/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
+++ b/drivers/infiniband/ulp/ipoib/ipoib_multicast.c
@@ -529,21 +529,13 @@ void ipoib_mcast_join_task(struct work_struct *work)
 			  port_attr.state);
 		return;
 	}
+	priv->local_lid = port_attr.lid;
 
 	if (ib_query_gid(priv->ca, priv->port, 0, &priv->local_gid))
 		ipoib_warn(priv, "ib_query_gid() failed\n");
 	else
 		memcpy(priv->dev->dev_addr + 4, priv->local_gid.raw, sizeof (union ib_gid));
 
-	{
-		struct ib_port_attr attr;
-
-		if (!ib_query_port(priv->ca, priv->port, &attr))
-			priv->local_lid = attr.lid;
-		else
-			ipoib_warn(priv, "ib_query_port failed\n");
-	}
-
 	if (!priv->broadcast) {
 		struct ipoib_mcast *broadcast;
 
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.c b/drivers/infiniband/ulp/iser/iscsi_iser.c
index 61ee91d88380..f42ab14105ac 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.c
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.c
@@ -83,7 +83,7 @@ module_param_named(max_lun, iscsi_max_lun, uint, S_IRUGO);
 
 int iser_debug_level = 0;
 bool iser_pi_enable = false;
-int iser_pi_guard = 0;
+int iser_pi_guard = 1;
 
 MODULE_DESCRIPTION("iSER (iSCSI Extensions for RDMA) Datamover");
 MODULE_LICENSE("Dual BSD/GPL");
@@ -97,14 +97,24 @@ module_param_named(pi_enable, iser_pi_enable, bool, 0644);
 MODULE_PARM_DESC(pi_enable, "Enable T10-PI offload support (default:disabled)");
 
 module_param_named(pi_guard, iser_pi_guard, int, 0644);
-MODULE_PARM_DESC(pi_guard, "T10-PI guard_type, 0:CRC|1:IP_CSUM (default:CRC)");
+MODULE_PARM_DESC(pi_guard, "T10-PI guard_type, 0:CRC|1:IP_CSUM (default:IP_CSUM)");
 
 static struct workqueue_struct *release_wq;
 struct iser_global ig;
 
+/*
+ * iscsi_iser_recv() - Process a successfull recv completion
+ * @conn:         iscsi connection
+ * @hdr:          iscsi header
+ * @rx_data:      buffer containing receive data payload
+ * @rx_data_len:  length of rx_data
+ *
+ * Notes: In case of data length errors or iscsi PDU completion failures
+ *        this routine will signal iscsi layer of connection failure.
+ */
 void
-iscsi_iser_recv(struct iscsi_conn *conn,
-		struct iscsi_hdr *hdr, char *rx_data, int rx_data_len)
+iscsi_iser_recv(struct iscsi_conn *conn, struct iscsi_hdr *hdr,
+		char *rx_data, int rx_data_len)
 {
 	int rc = 0;
 	int datalen;
@@ -135,20 +145,30 @@ error:
 	iscsi_conn_failure(conn, rc);
 }
 
-static int iscsi_iser_pdu_alloc(struct iscsi_task *task, uint8_t opcode)
+/**
+ * iscsi_iser_pdu_alloc() - allocate an iscsi-iser PDU
+ * @task:     iscsi task
+ * @opcode:   iscsi command opcode
+ *
+ * Netes: This routine can't fail, just assign iscsi task
+ *        hdr and max hdr size.
+ */
+static int
+iscsi_iser_pdu_alloc(struct iscsi_task *task, uint8_t opcode)
 {
 	struct iscsi_iser_task *iser_task = task->dd_data;
 
 	task->hdr = (struct iscsi_hdr *)&iser_task->desc.iscsi_header;
 	task->hdr_max = sizeof(iser_task->desc.iscsi_header);
+
 	return 0;
 }
 
 int iser_initialize_task_headers(struct iscsi_task *task,
 						struct iser_tx_desc *tx_desc)
 {
-	struct iser_conn       *ib_conn   = task->conn->dd_data;
-	struct iser_device     *device    = ib_conn->device;
+	struct iser_conn       *iser_conn   = task->conn->dd_data;
+	struct iser_device *device = iser_conn->ib_conn.device;
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	u64 dma_addr;
 
@@ -162,14 +182,18 @@ int iser_initialize_task_headers(struct iscsi_task *task,
 	tx_desc->tx_sg[0].length = ISER_HEADERS_LEN;
 	tx_desc->tx_sg[0].lkey   = device->mr->lkey;
 
-	iser_task->ib_conn = ib_conn;
+	iser_task->iser_conn = iser_conn;
 	return 0;
 }
+
 /**
- * iscsi_iser_task_init - Initialize task
+ * iscsi_iser_task_init() - Initialize iscsi-iser task
  * @task: iscsi task
  *
  * Initialize the task for the scsi command or mgmt command.
+ *
+ * Return: Returns zero on success or -ENOMEM when failing
+ *         to init task headers (dma mapping error).
  */
 static int
 iscsi_iser_task_init(struct iscsi_task *task)
@@ -191,7 +215,7 @@ iscsi_iser_task_init(struct iscsi_task *task)
 }
 
 /**
- * iscsi_iser_mtask_xmit - xmit management(immediate) task
+ * iscsi_iser_mtask_xmit() - xmit management (immediate) task
  * @conn: iscsi connection
  * @task: task management task
  *
@@ -249,6 +273,12 @@ iscsi_iser_task_xmit_unsol_data_exit:
 	return error;
 }
 
+/**
+ * iscsi_iser_task_xmit() - xmit iscsi-iser task
+ * @task: iscsi task
+ *
+ * Return: zero on success or escalates $error on failure.
+ */
 static int
 iscsi_iser_task_xmit(struct iscsi_task *task)
 {
@@ -286,12 +316,24 @@ iscsi_iser_task_xmit(struct iscsi_task *task)
 	return error;
 }
 
+/**
+ * iscsi_iser_cleanup_task() - cleanup an iscsi-iser task
+ * @task: iscsi task
+ *
+ * Notes: In case the RDMA device is already NULL (might have
+ *        been removed in DEVICE_REMOVAL CM event it will bail-out
+ *        without doing dma unmapping.
+ */
 static void iscsi_iser_cleanup_task(struct iscsi_task *task)
 {
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	struct iser_tx_desc    *tx_desc   = &iser_task->desc;
-	struct iser_conn       *ib_conn	  = task->conn->dd_data;
-	struct iser_device     *device	  = ib_conn->device;
+	struct iser_conn       *iser_conn	  = task->conn->dd_data;
+	struct iser_device *device = iser_conn->ib_conn.device;
+
+	/* DEVICE_REMOVAL event might have already released the device */
+	if (!device)
+		return;
 
 	ib_dma_unmap_single(device->ib_device,
 		tx_desc->dma_addr, ISER_HEADERS_LEN, DMA_TO_DEVICE);
@@ -306,7 +348,20 @@ static void iscsi_iser_cleanup_task(struct iscsi_task *task)
 	}
 }
 
-static u8 iscsi_iser_check_protection(struct iscsi_task *task, sector_t *sector)
+/**
+ * iscsi_iser_check_protection() - check protection information status of task.
+ * @task:     iscsi task
+ * @sector:   error sector if exsists (output)
+ *
+ * Return: zero if no data-integrity errors have occured
+ *         0x1: data-integrity error occured in the guard-block
+ *         0x2: data-integrity error occured in the reference tag
+ *         0x3: data-integrity error occured in the application tag
+ *
+ *         In addition the error sector is marked.
+ */
+static u8
+iscsi_iser_check_protection(struct iscsi_task *task, sector_t *sector)
 {
 	struct iscsi_iser_task *iser_task = task->dd_data;
 
@@ -318,8 +373,17 @@ static u8 iscsi_iser_check_protection(struct iscsi_task *task, sector_t *sector)
 						 sector);
 }
 
+/**
+ * iscsi_iser_conn_create() - create a new iscsi-iser connection
+ * @cls_session: iscsi class connection
+ * @conn_idx:    connection index within the session (for MCS)
+ *
+ * Return: iscsi_cls_conn when iscsi_conn_setup succeeds or NULL
+ *         otherwise.
+ */
 static struct iscsi_cls_conn *
-iscsi_iser_conn_create(struct iscsi_cls_session *cls_session, uint32_t conn_idx)
+iscsi_iser_conn_create(struct iscsi_cls_session *cls_session,
+		       uint32_t conn_idx)
 {
 	struct iscsi_conn *conn;
 	struct iscsi_cls_conn *cls_conn;
@@ -338,14 +402,25 @@ iscsi_iser_conn_create(struct iscsi_cls_session *cls_session, uint32_t conn_idx)
 	return cls_conn;
 }
 
+/**
+ * iscsi_iser_conn_bind() - bind iscsi and iser connection structures
+ * @cls_session:     iscsi class session
+ * @cls_conn:        iscsi class connection
+ * @transport_eph:   transport end-point handle
+ * @is_leading:      indicate if this is the session leading connection (MCS)
+ *
+ * Return: zero on success, $error if iscsi_conn_bind fails and
+ *         -EINVAL in case end-point doesn't exsits anymore or iser connection
+ *         state is not UP (teardown already started).
+ */
 static int
 iscsi_iser_conn_bind(struct iscsi_cls_session *cls_session,
-		     struct iscsi_cls_conn *cls_conn, uint64_t transport_eph,
+		     struct iscsi_cls_conn *cls_conn,
+		     uint64_t transport_eph,
 		     int is_leading)
 {
 	struct iscsi_conn *conn = cls_conn->dd_data;
-	struct iscsi_session *session;
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
 	struct iscsi_endpoint *ep;
 	int error;
 
@@ -361,56 +436,100 @@ iscsi_iser_conn_bind(struct iscsi_cls_session *cls_session,
 			 (unsigned long long)transport_eph);
 		return -EINVAL;
 	}
-	ib_conn = ep->dd_data;
+	iser_conn = ep->dd_data;
+
+	mutex_lock(&iser_conn->state_mutex);
+	if (iser_conn->state != ISER_CONN_UP) {
+		error = -EINVAL;
+		iser_err("iser_conn %p state is %d, teardown started\n",
+			 iser_conn, iser_conn->state);
+		goto out;
+	}
 
-	session = conn->session;
-	if (iser_alloc_rx_descriptors(ib_conn, session))
-		return -ENOMEM;
+	error = iser_alloc_rx_descriptors(iser_conn, conn->session);
+	if (error)
+		goto out;
 
 	/* binds the iSER connection retrieved from the previously
 	 * connected ep_handle to the iSCSI layer connection. exchanges
 	 * connection pointers */
-	iser_info("binding iscsi conn %p to ib_conn %p\n", conn, ib_conn);
+	iser_info("binding iscsi conn %p to iser_conn %p\n", conn, iser_conn);
 
-	conn->dd_data = ib_conn;
-	ib_conn->iscsi_conn = conn;
+	conn->dd_data = iser_conn;
+	iser_conn->iscsi_conn = conn;
 
-	return 0;
+out:
+	mutex_unlock(&iser_conn->state_mutex);
+	return error;
 }
 
+/**
+ * iscsi_iser_conn_start() - start iscsi-iser connection
+ * @cls_conn: iscsi class connection
+ *
+ * Notes: Here iser intialize (or re-initialize) stop_completion as
+ *        from this point iscsi must call conn_stop in session/connection
+ *        teardown so iser transport must wait for it.
+ */
 static int
 iscsi_iser_conn_start(struct iscsi_cls_conn *cls_conn)
 {
 	struct iscsi_conn *iscsi_conn;
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
 
 	iscsi_conn = cls_conn->dd_data;
-	ib_conn = iscsi_conn->dd_data;
-	reinit_completion(&ib_conn->stop_completion);
+	iser_conn = iscsi_conn->dd_data;
+	reinit_completion(&iser_conn->stop_completion);
 
 	return iscsi_conn_start(cls_conn);
 }
 
+/**
+ * iscsi_iser_conn_stop() - stop iscsi-iser connection
+ * @cls_conn:  iscsi class connection
+ * @flag:      indicate if recover or terminate (passed as is)
+ *
+ * Notes: Calling iscsi_conn_stop might theoretically race with
+ *        DEVICE_REMOVAL event and dereference a previously freed RDMA device
+ *        handle, so we call it under iser the state lock to protect against
+ *        this kind of race.
+ */
 static void
 iscsi_iser_conn_stop(struct iscsi_cls_conn *cls_conn, int flag)
 {
 	struct iscsi_conn *conn = cls_conn->dd_data;
-	struct iser_conn *ib_conn = conn->dd_data;
+	struct iser_conn *iser_conn = conn->dd_data;
 
-	iser_dbg("stopping iscsi_conn: %p, ib_conn: %p\n", conn, ib_conn);
-	iscsi_conn_stop(cls_conn, flag);
+	iser_info("stopping iscsi_conn: %p, iser_conn: %p\n", conn, iser_conn);
 
 	/*
 	 * Userspace may have goofed up and not bound the connection or
 	 * might have only partially setup the connection.
 	 */
-	if (ib_conn) {
+	if (iser_conn) {
+		mutex_lock(&iser_conn->state_mutex);
+		iscsi_conn_stop(cls_conn, flag);
+		iser_conn_terminate(iser_conn);
+
+		/* unbind */
+		iser_conn->iscsi_conn = NULL;
 		conn->dd_data = NULL;
-		complete(&ib_conn->stop_completion);
+
+		complete(&iser_conn->stop_completion);
+		mutex_unlock(&iser_conn->state_mutex);
+	} else {
+		iscsi_conn_stop(cls_conn, flag);
 	}
 }
 
-static void iscsi_iser_session_destroy(struct iscsi_cls_session *cls_session)
+/**
+ * iscsi_iser_session_destroy() - destroy iscsi-iser session
+ * @cls_session: iscsi class session
+ *
+ * Removes and free iscsi host.
+ */
+static void
+iscsi_iser_session_destroy(struct iscsi_cls_session *cls_session)
 {
 	struct Scsi_Host *shost = iscsi_session_to_shost(cls_session);
 
@@ -430,6 +549,16 @@ iser_dif_prot_caps(int prot_caps)
 						      SHOST_DIX_TYPE3_PROTECTION : 0);
 }
 
+/**
+ * iscsi_iser_session_create() - create an iscsi-iser session
+ * @ep:             iscsi end-point handle
+ * @cmds_max:       maximum commands in this session
+ * @qdepth:         session command queue depth
+ * @initial_cmdsn:  initiator command sequnce number
+ *
+ * Allocates and adds a scsi host, expose DIF supprot if
+ * exists, and sets up an iscsi session.
+ */
 static struct iscsi_cls_session *
 iscsi_iser_session_create(struct iscsi_endpoint *ep,
 			  uint16_t cmds_max, uint16_t qdepth,
@@ -438,7 +567,8 @@ iscsi_iser_session_create(struct iscsi_endpoint *ep,
 	struct iscsi_cls_session *cls_session;
 	struct iscsi_session *session;
 	struct Scsi_Host *shost;
-	struct iser_conn *ib_conn = NULL;
+	struct iser_conn *iser_conn = NULL;
+	struct ib_conn *ib_conn;
 
 	shost = iscsi_host_alloc(&iscsi_iser_sht, 0, 0);
 	if (!shost)
@@ -455,7 +585,8 @@ iscsi_iser_session_create(struct iscsi_endpoint *ep,
 	 * the leading conn's ep so this will be NULL;
 	 */
 	if (ep) {
-		ib_conn = ep->dd_data;
+		iser_conn = ep->dd_data;
+		ib_conn = &iser_conn->ib_conn;
 		if (ib_conn->pi_support) {
 			u32 sig_caps = ib_conn->device->dev_attr.sig_prot_cap;
 
@@ -467,8 +598,8 @@ iscsi_iser_session_create(struct iscsi_endpoint *ep,
 		}
 	}
 
-	if (iscsi_host_add(shost,
-			   ep ? ib_conn->device->ib_device->dma_device : NULL))
+	if (iscsi_host_add(shost, ep ?
+			   ib_conn->device->ib_device->dma_device : NULL))
 		goto free_host;
 
 	if (cmds_max > ISER_DEF_XMIT_CMDS_MAX) {
@@ -540,6 +671,13 @@ iscsi_iser_set_param(struct iscsi_cls_conn *cls_conn,
 	return 0;
 }
 
+/**
+ * iscsi_iser_set_param() - set class connection parameter
+ * @cls_conn:    iscsi class connection
+ * @stats:       iscsi stats to output
+ *
+ * Output connection statistics.
+ */
 static void
 iscsi_iser_conn_get_stats(struct iscsi_cls_conn *cls_conn, struct iscsi_stats *stats)
 {
@@ -568,18 +706,18 @@ iscsi_iser_conn_get_stats(struct iscsi_cls_conn *cls_conn, struct iscsi_stats *s
 static int iscsi_iser_get_ep_param(struct iscsi_endpoint *ep,
 				   enum iscsi_param param, char *buf)
 {
-	struct iser_conn *ib_conn = ep->dd_data;
+	struct iser_conn *iser_conn = ep->dd_data;
 	int len;
 
 	switch (param) {
 	case ISCSI_PARAM_CONN_PORT:
 	case ISCSI_PARAM_CONN_ADDRESS:
-		if (!ib_conn || !ib_conn->cma_id)
+		if (!iser_conn || !iser_conn->ib_conn.cma_id)
 			return -ENOTCONN;
 
 		return iscsi_conn_get_addr_param((struct sockaddr_storage *)
-					&ib_conn->cma_id->route.addr.dst_addr,
-					param, buf);
+				&iser_conn->ib_conn.cma_id->route.addr.dst_addr,
+				param, buf);
 		break;
 	default:
 		return -ENOSYS;
@@ -588,29 +726,44 @@ static int iscsi_iser_get_ep_param(struct iscsi_endpoint *ep,
 	return len;
 }
 
+/**
+ * iscsi_iser_ep_connect() - Initiate iSER connection establishment
+ * @shost:          scsi_host
+ * @dst_addr:       destination address
+ * @non-blocking:   indicate if routine can block
+ *
+ * Allocate an iscsi endpoint, an iser_conn structure and bind them.
+ * After that start RDMA connection establishment via rdma_cm. We
+ * don't allocate iser_conn embedded in iscsi_endpoint since in teardown
+ * the endpoint will be destroyed at ep_disconnect while iser_conn will
+ * cleanup its resources asynchronuously.
+ *
+ * Return: iscsi_endpoint created by iscsi layer or ERR_PTR(error)
+ *         if fails.
+ */
 static struct iscsi_endpoint *
 iscsi_iser_ep_connect(struct Scsi_Host *shost, struct sockaddr *dst_addr,
 		      int non_blocking)
 {
 	int err;
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
 	struct iscsi_endpoint *ep;
 
 	ep = iscsi_create_endpoint(0);
 	if (!ep)
 		return ERR_PTR(-ENOMEM);
 
-	ib_conn = kzalloc(sizeof(*ib_conn), GFP_KERNEL);
-	if (!ib_conn) {
+	iser_conn = kzalloc(sizeof(*iser_conn), GFP_KERNEL);
+	if (!iser_conn) {
 		err = -ENOMEM;
 		goto failure;
 	}
 
-	ep->dd_data = ib_conn;
-	ib_conn->ep = ep;
-	iser_conn_init(ib_conn);
+	ep->dd_data = iser_conn;
+	iser_conn->ep = ep;
+	iser_conn_init(iser_conn);
 
-	err = iser_connect(ib_conn, NULL, dst_addr, non_blocking);
+	err = iser_connect(iser_conn, NULL, dst_addr, non_blocking);
 	if (err)
 		goto failure;
 
@@ -620,25 +773,38 @@ failure:
 	return ERR_PTR(err);
 }
 
+/**
+ * iscsi_iser_ep_poll() - poll for iser connection establishment to complete
+ * @ep:            iscsi endpoint (created at ep_connect)
+ * @timeout_ms:    polling timeout allowed in ms.
+ *
+ * This routine boils down to waiting for up_completion signaling
+ * that cma_id got CONNECTED event.
+ *
+ * Return: 1 if succeeded in connection establishment, 0 if timeout expired
+ *         (libiscsi will retry will kick in) or -1 if interrupted by signal
+ *         or more likely iser connection state transitioned to TEMINATING or
+ *         DOWN during the wait period.
+ */
 static int
 iscsi_iser_ep_poll(struct iscsi_endpoint *ep, int timeout_ms)
 {
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
 	int rc;
 
-	ib_conn = ep->dd_data;
-	rc = wait_for_completion_interruptible_timeout(&ib_conn->up_completion,
+	iser_conn = ep->dd_data;
+	rc = wait_for_completion_interruptible_timeout(&iser_conn->up_completion,
 						       msecs_to_jiffies(timeout_ms));
 	/* if conn establishment failed, return error code to iscsi */
 	if (rc == 0) {
-		mutex_lock(&ib_conn->state_mutex);
-		if (ib_conn->state == ISER_CONN_TERMINATING ||
-		    ib_conn->state == ISER_CONN_DOWN)
+		mutex_lock(&iser_conn->state_mutex);
+		if (iser_conn->state == ISER_CONN_TERMINATING ||
+		    iser_conn->state == ISER_CONN_DOWN)
 			rc = -1;
-		mutex_unlock(&ib_conn->state_mutex);
+		mutex_unlock(&iser_conn->state_mutex);
 	}
 
-	iser_info("ib conn %p rc = %d\n", ib_conn, rc);
+	iser_info("ib conn %p rc = %d\n", iser_conn, rc);
 
 	if (rc > 0)
 		return 1; /* success, this is the equivalent of POLLOUT */
@@ -648,15 +814,26 @@ iscsi_iser_ep_poll(struct iscsi_endpoint *ep, int timeout_ms)
 		return rc; /* signal */
 }
 
+/**
+ * iscsi_iser_ep_disconnect() - Initiate connection teardown process
+ * @ep:    iscsi endpoint handle
+ *
+ * This routine is not blocked by iser and RDMA termination process
+ * completion as we queue a deffered work for iser/RDMA destruction
+ * and cleanup or actually call it immediately in case we didn't pass
+ * iscsi conn bind/start stage, thus it is safe.
+ */
 static void
 iscsi_iser_ep_disconnect(struct iscsi_endpoint *ep)
 {
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
+
+	iser_conn = ep->dd_data;
+	iser_info("ep %p iser conn %p state %d\n",
+		  ep, iser_conn, iser_conn->state);
 
-	ib_conn = ep->dd_data;
-	iser_info("ep %p ib conn %p state %d\n", ep, ib_conn, ib_conn->state);
-	mutex_lock(&ib_conn->state_mutex);
-	iser_conn_terminate(ib_conn);
+	mutex_lock(&iser_conn->state_mutex);
+	iser_conn_terminate(iser_conn);
 
 	/*
 	 * if iser_conn and iscsi_conn are bound, we must wait for
@@ -664,14 +841,14 @@ iscsi_iser_ep_disconnect(struct iscsi_endpoint *ep)
 	 * the iser resources. Otherwise we are safe to free resources
 	 * immediately.
 	 */
-	if (ib_conn->iscsi_conn) {
-		INIT_WORK(&ib_conn->release_work, iser_release_work);
-		queue_work(release_wq, &ib_conn->release_work);
-		mutex_unlock(&ib_conn->state_mutex);
+	if (iser_conn->iscsi_conn) {
+		INIT_WORK(&iser_conn->release_work, iser_release_work);
+		queue_work(release_wq, &iser_conn->release_work);
+		mutex_unlock(&iser_conn->state_mutex);
 	} else {
-		ib_conn->state = ISER_CONN_DOWN;
-		mutex_unlock(&ib_conn->state_mutex);
-		iser_conn_release(ib_conn);
+		iser_conn->state = ISER_CONN_DOWN;
+		mutex_unlock(&iser_conn->state_mutex);
+		iser_conn_release(iser_conn);
 	}
 	iscsi_destroy_endpoint(ep);
 }
@@ -834,7 +1011,7 @@ register_transport_failure:
 
 static void __exit iser_exit(void)
 {
-	struct iser_conn *ib_conn, *n;
+	struct iser_conn *iser_conn, *n;
 	int connlist_empty;
 
 	iser_dbg("Removing iSER datamover...\n");
@@ -847,8 +1024,9 @@ static void __exit iser_exit(void)
 	if (!connlist_empty) {
 		iser_err("Error cleanup stage completed but we still have iser "
 			 "connections, destroying them anyway.\n");
-		list_for_each_entry_safe(ib_conn, n, &ig.connlist, conn_list) {
-			iser_conn_release(ib_conn);
+		list_for_each_entry_safe(iser_conn, n, &ig.connlist,
+					 conn_list) {
+			iser_conn_release(iser_conn);
 		}
 	}
 
diff --git a/drivers/infiniband/ulp/iser/iscsi_iser.h b/drivers/infiniband/ulp/iser/iscsi_iser.h
index c877dad381cb..cd4174ca9a76 100644
--- a/drivers/infiniband/ulp/iser/iscsi_iser.h
+++ b/drivers/infiniband/ulp/iser/iscsi_iser.h
@@ -69,39 +69,38 @@
 
 #define DRV_NAME	"iser"
 #define PFX		DRV_NAME ": "
-#define DRV_VER		"1.4"
+#define DRV_VER		"1.4.8"
 
-#define iser_dbg(fmt, arg...)				\
-	do {						\
-		if (iser_debug_level > 2)		\
-			printk(KERN_DEBUG PFX "%s:" fmt,\
-				__func__ , ## arg);	\
+#define iser_dbg(fmt, arg...)				 \
+	do {						 \
+		if (iser_debug_level > 2)		 \
+			printk(KERN_DEBUG PFX "%s: " fmt,\
+				__func__ , ## arg);	 \
 	} while (0)
 
 #define iser_warn(fmt, arg...)				\
 	do {						\
 		if (iser_debug_level > 0)		\
-			pr_warn(PFX "%s:" fmt,          \
+			pr_warn(PFX "%s: " fmt,		\
 				__func__ , ## arg);	\
 	} while (0)
 
 #define iser_info(fmt, arg...)				\
 	do {						\
 		if (iser_debug_level > 1)		\
-			pr_info(PFX "%s:" fmt,          \
+			pr_info(PFX "%s: " fmt,		\
 				__func__ , ## arg);	\
 	} while (0)
 
 #define iser_err(fmt, arg...)				\
 	do {						\
-		printk(KERN_ERR PFX "%s:" fmt,          \
+		printk(KERN_ERR PFX "%s: " fmt,		\
 		       __func__ , ## arg);		\
 	} while (0)
 
 #define SHIFT_4K	12
 #define SIZE_4K	(1ULL << SHIFT_4K)
 #define MASK_4K	(~(SIZE_4K-1))
-
 					/* support up to 512KB in one RDMA */
 #define ISCSI_ISER_SG_TABLESIZE         (0x80000 >> SHIFT_4K)
 #define ISER_DEF_XMIT_CMDS_DEFAULT		512
@@ -145,18 +144,32 @@
 					ISER_MAX_TX_MISC_PDUS         + \
 					ISER_MAX_RX_MISC_PDUS)
 
+#define ISER_WC_BATCH_COUNT   16
+#define ISER_SIGNAL_CMD_COUNT 32
+
 #define ISER_VER			0x10
 #define ISER_WSV			0x08
 #define ISER_RSV			0x04
 
 #define ISER_FASTREG_LI_WRID		0xffffffffffffffffULL
+#define ISER_BEACON_WRID		0xfffffffffffffffeULL
 
+/**
+ * struct iser_hdr - iSER header
+ *
+ * @flags:        flags support (zbva, remote_inv)
+ * @rsvd:         reserved
+ * @write_stag:   write rkey
+ * @write_va:     write virtual address
+ * @reaf_stag:    read rkey
+ * @read_va:      read virtual address
+ */
 struct iser_hdr {
 	u8      flags;
 	u8      rsvd[3];
-	__be32  write_stag; /* write rkey */
+	__be32  write_stag;
 	__be64  write_va;
-	__be32  read_stag;  /* read rkey */
+	__be32  read_stag;
 	__be64  read_va;
 } __attribute__((packed));
 
@@ -179,7 +192,7 @@ struct iser_cm_hdr {
 /* Length of an object name string */
 #define ISER_OBJECT_NAME_SIZE		    64
 
-enum iser_ib_conn_state {
+enum iser_conn_state {
 	ISER_CONN_INIT,		   /* descriptor allocd, no conn          */
 	ISER_CONN_PENDING,	   /* in the process of being established */
 	ISER_CONN_UP,		   /* up and running                      */
@@ -200,23 +213,42 @@ enum iser_data_dir {
 	ISER_DIRS_NUM
 };
 
+/**
+ * struct iser_data_buf - iSER data buffer
+ *
+ * @buf:          pointer to the sg list
+ * @size:         num entries of this sg
+ * @data_len:     total beffer byte len
+ * @dma_nents:    returned by dma_map_sg
+ * @copy_buf:     allocated copy buf for SGs unaligned
+ *                for rdma which are copied
+ * @sg_single:    SG-ified clone of a non SG SC or
+ *                unaligned SG
+ */
 struct iser_data_buf {
-	void               *buf;      /* pointer to the sg list               */
-	unsigned int       size;      /* num entries of this sg               */
-	unsigned long      data_len;  /* total data len                       */
-	unsigned int       dma_nents; /* returned by dma_map_sg               */
-	char       	   *copy_buf; /* allocated copy buf for SGs unaligned *
-	                               * for rdma which are copied            */
-	struct scatterlist sg_single; /* SG-ified clone of a non SG SC or     *
-				       * unaligned SG                         */
+	void               *buf;
+	unsigned int       size;
+	unsigned long      data_len;
+	unsigned int       dma_nents;
+	char               *copy_buf;
+	struct scatterlist sg_single;
   };
 
 /* fwd declarations */
 struct iser_device;
-struct iser_cq_desc;
 struct iscsi_iser_task;
 struct iscsi_endpoint;
 
+/**
+ * struct iser_mem_reg - iSER memory registration info
+ *
+ * @lkey:         MR local key
+ * @rkey:         MR remote key
+ * @va:           MR start address (buffer va)
+ * @len:          MR length
+ * @mem_h:        pointer to registration context (FMR/Fastreg)
+ * @is_mr:        indicates weather we registered the buffer
+ */
 struct iser_mem_reg {
 	u32  lkey;
 	u32  rkey;
@@ -226,11 +258,20 @@ struct iser_mem_reg {
 	int  is_mr;
 };
 
+/**
+ * struct iser_regd_buf - iSER buffer registration desc
+ *
+ * @reg:          memory registration info
+ * @virt_addr:    virtual address of buffer
+ * @device:       reference to iser device
+ * @direction:    dma direction (for dma_unmap)
+ * @data_size:    data buffer size in bytes
+ */
 struct iser_regd_buf {
-	struct iser_mem_reg     reg;        /* memory registration info        */
+	struct iser_mem_reg     reg;
 	void                    *virt_addr;
-	struct iser_device      *device;    /* device->device for dma_unmap    */
-	enum dma_data_direction direction;  /* direction for dma_unmap	       */
+	struct iser_device      *device;
+	enum dma_data_direction direction;
 	unsigned int            data_size;
 };
 
@@ -240,19 +281,39 @@ enum iser_desc_type {
 	ISCSI_TX_DATAOUT
 };
 
+/**
+ * struct iser_tx_desc - iSER TX descriptor (for send wr_id)
+ *
+ * @iser_header:   iser header
+ * @iscsi_header:  iscsi header
+ * @type:          command/control/dataout
+ * @dam_addr:      header buffer dma_address
+ * @tx_sg:         sg[0] points to iser/iscsi headers
+ *                 sg[1] optionally points to either of immediate data
+ *                 unsolicited data-out or control
+ * @num_sge:       number sges used on this TX task
+ */
 struct iser_tx_desc {
 	struct iser_hdr              iser_header;
 	struct iscsi_hdr             iscsi_header;
 	enum   iser_desc_type        type;
 	u64		             dma_addr;
-	/* sg[0] points to iser/iscsi headers, sg[1] optionally points to either
-	of immediate data, unsolicited data-out or control (login,text) */
 	struct ib_sge		     tx_sg[2];
 	int                          num_sge;
 };
 
 #define ISER_RX_PAD_SIZE	(256 - (ISER_RX_PAYLOAD_SIZE + \
 					sizeof(u64) + sizeof(struct ib_sge)))
+/**
+ * struct iser_rx_desc - iSER RX descriptor (for recv wr_id)
+ *
+ * @iser_header:   iser header
+ * @iscsi_header:  iscsi header
+ * @data:          received data segment
+ * @dma_addr:      receive buffer dma address
+ * @rx_sg:         ib_sge of receive buffer
+ * @pad:           for sense data TODO: Modify to maximum sense length supported
+ */
 struct iser_rx_desc {
 	struct iser_hdr              iser_header;
 	struct iscsi_hdr             iscsi_header;
@@ -265,25 +326,59 @@ struct iser_rx_desc {
 #define ISER_MAX_CQ 4
 
 struct iser_conn;
+struct ib_conn;
 struct iscsi_iser_task;
 
+/**
+ * struct iser_comp - iSER completion context
+ *
+ * @device:     pointer to device handle
+ * @cq:         completion queue
+ * @wcs:        work completion array
+ * @tasklet:    Tasklet handle
+ * @active_qps: Number of active QPs attached
+ *              to completion context
+ */
+struct iser_comp {
+	struct iser_device      *device;
+	struct ib_cq		*cq;
+	struct ib_wc		 wcs[ISER_WC_BATCH_COUNT];
+	struct tasklet_struct	 tasklet;
+	int                      active_qps;
+};
+
+/**
+ * struct iser_device - iSER device handle
+ *
+ * @ib_device:     RDMA device
+ * @pd:            Protection Domain for this device
+ * @dev_attr:      Device attributes container
+ * @mr:            Global DMA memory region
+ * @event_handler: IB events handle routine
+ * @ig_list:	   entry in devices list
+ * @refcount:      Reference counter, dominated by open iser connections
+ * @comps_used:    Number of completion contexts used, Min between online
+ *                 cpus and device max completion vectors
+ * @comps:         Dinamically allocated array of completion handlers
+ * Memory registration pool Function pointers (FMR or Fastreg):
+ *     @iser_alloc_rdma_reg_res: Allocation of memory regions pool
+ *     @iser_free_rdma_reg_res:  Free of memory regions pool
+ *     @iser_reg_rdma_mem:       Memory registration routine
+ *     @iser_unreg_rdma_mem:     Memory deregistration routine
+ */
 struct iser_device {
 	struct ib_device             *ib_device;
 	struct ib_pd	             *pd;
 	struct ib_device_attr	     dev_attr;
-	struct ib_cq	             *rx_cq[ISER_MAX_CQ];
-	struct ib_cq	             *tx_cq[ISER_MAX_CQ];
 	struct ib_mr	             *mr;
-	struct tasklet_struct	     cq_tasklet[ISER_MAX_CQ];
 	struct ib_event_handler      event_handler;
-	struct list_head             ig_list; /* entry in ig devices list */
+	struct list_head             ig_list;
 	int                          refcount;
-	int                          cq_active_qps[ISER_MAX_CQ];
-	int			     cqs_used;
-	struct iser_cq_desc	     *cq_desc;
-	int                          (*iser_alloc_rdma_reg_res)(struct iser_conn *ib_conn,
+	int			     comps_used;
+	struct iser_comp	     comps[ISER_MAX_CQ];
+	int                          (*iser_alloc_rdma_reg_res)(struct ib_conn *ib_conn,
 								unsigned cmds_max);
-	void                         (*iser_free_rdma_reg_res)(struct iser_conn *ib_conn);
+	void                         (*iser_free_rdma_reg_res)(struct ib_conn *ib_conn);
 	int                          (*iser_reg_rdma_mem)(struct iscsi_iser_task *iser_task,
 							  enum iser_data_dir cmd_dir);
 	void                         (*iser_unreg_rdma_mem)(struct iscsi_iser_task *iser_task,
@@ -301,78 +396,160 @@ enum iser_reg_indicator {
 	ISER_FASTREG_PROTECTED	= 1 << 3,
 };
 
+/**
+ * struct iser_pi_context - Protection information context
+ *
+ * @prot_mr:        protection memory region
+ * @prot_frpl:      protection fastreg page list
+ * @sig_mr:         signature feature enabled memory region
+ */
 struct iser_pi_context {
 	struct ib_mr                   *prot_mr;
 	struct ib_fast_reg_page_list   *prot_frpl;
 	struct ib_mr                   *sig_mr;
 };
 
+/**
+ * struct fast_reg_descriptor - Fast registration descriptor
+ *
+ * @list:           entry in connection fastreg pool
+ * @data_mr:        data memory region
+ * @data_frpl:      data fastreg page list
+ * @pi_ctx:         protection information context
+ * @reg_indicators: fast registration indicators
+ */
 struct fast_reg_descriptor {
 	struct list_head		  list;
-	/* For fast registration - FRWR */
 	struct ib_mr			 *data_mr;
 	struct ib_fast_reg_page_list     *data_frpl;
 	struct iser_pi_context		 *pi_ctx;
-	/* registration indicators container */
 	u8				  reg_indicators;
 };
 
+/**
+ * struct ib_conn - Infiniband related objects
+ *
+ * @cma_id:              rdma_cm connection maneger handle
+ * @qp:                  Connection Queue-pair
+ * @post_recv_buf_count: post receive counter
+ * @rx_wr:               receive work request for batch posts
+ * @device:              reference to iser device
+ * @comp:                iser completion context
+ * @pi_support:          Indicate device T10-PI support
+ * @beacon:              beacon send wr to signal all flush errors were drained
+ * @flush_comp:          completes when all connection completions consumed
+ * @lock:                protects fmr/fastreg pool
+ * @union.fmr:
+ *     @pool:            FMR pool for fast registrations
+ *     @page_vec:        page vector to hold mapped commands pages
+ *                       used for registration
+ * @union.fastreg:
+ *     @pool:            Fast registration descriptors pool for fast
+ *                       registrations
+ *     @pool_size:       Size of pool
+ */
+struct ib_conn {
+	struct rdma_cm_id           *cma_id;
+	struct ib_qp	            *qp;
+	int                          post_recv_buf_count;
+	struct ib_recv_wr	     rx_wr[ISER_MIN_POSTED_RX];
+	struct iser_device          *device;
+	struct iser_comp	    *comp;
+	bool			     pi_support;
+	struct ib_send_wr	     beacon;
+	struct completion	     flush_comp;
+	spinlock_t		     lock;
+	union {
+		struct {
+			struct ib_fmr_pool      *pool;
+			struct iser_page_vec	*page_vec;
+		} fmr;
+		struct {
+			struct list_head	 pool;
+			int			 pool_size;
+		} fastreg;
+	};
+};
+
+/**
+ * struct iser_conn - iSER connection context
+ *
+ * @ib_conn:          connection RDMA resources
+ * @iscsi_conn:       link to matching iscsi connection
+ * @ep:               transport handle
+ * @state:            connection logical state
+ * @qp_max_recv_dtos: maximum number of data outs, corresponds
+ *                    to max number of post recvs
+ * @qp_max_recv_dtos_mask: (qp_max_recv_dtos - 1)
+ * @min_posted_rx:    (qp_max_recv_dtos >> 2)
+ * @name:             connection peer portal
+ * @release_work:     deffered work for release job
+ * @state_mutex:      protects iser onnection state
+ * @stop_completion:  conn_stop completion
+ * @ib_completion:    RDMA cleanup completion
+ * @up_completion:    connection establishment completed
+ *                    (state is ISER_CONN_UP)
+ * @conn_list:        entry in ig conn list
+ * @login_buf:        login data buffer (stores login parameters)
+ * @login_req_buf:    login request buffer
+ * @login_req_dma:    login request buffer dma address
+ * @login_resp_buf:   login response buffer
+ * @login_resp_dma:   login response buffer dma address
+ * @rx_desc_head:     head of rx_descs cyclic buffer
+ * @rx_descs:         rx buffers array (cyclic buffer)
+ * @num_rx_descs:     number of rx descriptors
+ */
 struct iser_conn {
+	struct ib_conn		     ib_conn;
 	struct iscsi_conn	     *iscsi_conn;
 	struct iscsi_endpoint	     *ep;
-	enum iser_ib_conn_state	     state;	    /* rdma connection state   */
-	atomic_t		     refcount;
-	spinlock_t		     lock;	    /* used for state changes  */
-	struct iser_device           *device;       /* device context          */
-	struct rdma_cm_id            *cma_id;       /* CMA ID		       */
-	struct ib_qp	             *qp;           /* QP 		       */
-	unsigned		     qp_max_recv_dtos; /* num of rx buffers */
-	unsigned		     qp_max_recv_dtos_mask; /* above minus 1 */
-	unsigned		     min_posted_rx; /* qp_max_recv_dtos >> 2 */
-	int                          post_recv_buf_count; /* posted rx count  */
-	atomic_t                     post_send_buf_count; /* posted tx count   */
+	enum iser_conn_state	     state;
+	unsigned		     qp_max_recv_dtos;
+	unsigned		     qp_max_recv_dtos_mask;
+	unsigned		     min_posted_rx;
 	char 			     name[ISER_OBJECT_NAME_SIZE];
 	struct work_struct	     release_work;
-	struct completion	     stop_completion;
 	struct mutex		     state_mutex;
-	struct completion	     flush_completion;
+	struct completion	     stop_completion;
+	struct completion	     ib_completion;
 	struct completion	     up_completion;
-	struct list_head	     conn_list;       /* entry in ig conn list */
+	struct list_head	     conn_list;
 
 	char  			     *login_buf;
 	char			     *login_req_buf, *login_resp_buf;
 	u64			     login_req_dma, login_resp_dma;
 	unsigned int 		     rx_desc_head;
 	struct iser_rx_desc	     *rx_descs;
-	struct ib_recv_wr	     rx_wr[ISER_MIN_POSTED_RX];
-	bool			     pi_support;
-
-	/* Connection memory registration pool */
-	union {
-		struct {
-			struct ib_fmr_pool      *pool;	   /* pool of IB FMRs         */
-			struct iser_page_vec	*page_vec; /* represents SG to fmr maps*
-							    * maps serialized as tx is*/
-		} fmr;
-		struct {
-			struct list_head	pool;
-			int			pool_size;
-		} fastreg;
-	};
+	u32                          num_rx_descs;
 };
 
+/**
+ * struct iscsi_iser_task - iser task context
+ *
+ * @desc:     TX descriptor
+ * @iser_conn:        link to iser connection
+ * @status:           current task status
+ * @sc:               link to scsi command
+ * @command_sent:     indicate if command was sent
+ * @dir:              iser data direction
+ * @rdma_regd:        task rdma registration desc
+ * @data:             iser data buffer desc
+ * @data_copy:        iser data copy buffer desc (bounce buffer)
+ * @prot:             iser protection buffer desc
+ * @prot_copy:        iser protection copy buffer desc (bounce buffer)
+ */
 struct iscsi_iser_task {
 	struct iser_tx_desc          desc;
-	struct iser_conn	     *ib_conn;
+	struct iser_conn	     *iser_conn;
 	enum iser_task_status 	     status;
 	struct scsi_cmnd	     *sc;
-	int                          command_sent;  /* set if command  sent  */
-	int                          dir[ISER_DIRS_NUM];      /* set if dir use*/
-	struct iser_regd_buf         rdma_regd[ISER_DIRS_NUM];/* regd rdma buf */
-	struct iser_data_buf         data[ISER_DIRS_NUM];     /* orig. data des*/
-	struct iser_data_buf         data_copy[ISER_DIRS_NUM];/* contig. copy  */
-	struct iser_data_buf         prot[ISER_DIRS_NUM];     /* prot desc     */
-	struct iser_data_buf         prot_copy[ISER_DIRS_NUM];/* prot copy     */
+	int                          command_sent;
+	int                          dir[ISER_DIRS_NUM];
+	struct iser_regd_buf         rdma_regd[ISER_DIRS_NUM];
+	struct iser_data_buf         data[ISER_DIRS_NUM];
+	struct iser_data_buf         data_copy[ISER_DIRS_NUM];
+	struct iser_data_buf         prot[ISER_DIRS_NUM];
+	struct iser_data_buf         prot_copy[ISER_DIRS_NUM];
 };
 
 struct iser_page_vec {
@@ -382,17 +559,20 @@ struct iser_page_vec {
 	int data_size;
 };
 
-struct iser_cq_desc {
-	struct iser_device           *device;
-	int                          cq_index;
-};
-
+/**
+ * struct iser_global: iSER global context
+ *
+ * @device_list_mutex:    protects device_list
+ * @device_list:          iser devices global list
+ * @connlist_mutex:       protects connlist
+ * @connlist:             iser connections global list
+ * @desc_cache:           kmem cache for tx dataout
+ */
 struct iser_global {
-	struct mutex      device_list_mutex;/*                   */
-	struct list_head  device_list;	     /* all iSER devices */
+	struct mutex      device_list_mutex;
+	struct list_head  device_list;
 	struct mutex      connlist_mutex;
-	struct list_head  connlist;		/* all iSER IB connections */
-
+	struct list_head  connlist;
 	struct kmem_cache *desc_cache;
 };
 
@@ -401,9 +581,6 @@ extern int iser_debug_level;
 extern bool iser_pi_enable;
 extern int iser_pi_guard;
 
-/* allocate connection resources needed for rdma functionality */
-int iser_conn_set_full_featured_mode(struct iscsi_conn *conn);
-
 int iser_send_control(struct iscsi_conn *conn,
 		      struct iscsi_task *task);
 
@@ -415,29 +592,30 @@ int iser_send_data_out(struct iscsi_conn *conn,
 		       struct iscsi_data *hdr);
 
 void iscsi_iser_recv(struct iscsi_conn *conn,
-		     struct iscsi_hdr       *hdr,
-		     char                   *rx_data,
-		     int                    rx_data_len);
+		     struct iscsi_hdr *hdr,
+		     char *rx_data,
+		     int rx_data_len);
 
-void iser_conn_init(struct iser_conn *ib_conn);
+void iser_conn_init(struct iser_conn *iser_conn);
 
-void iser_conn_release(struct iser_conn *ib_conn);
+void iser_conn_release(struct iser_conn *iser_conn);
 
-void iser_conn_terminate(struct iser_conn *ib_conn);
+int iser_conn_terminate(struct iser_conn *iser_conn);
 
 void iser_release_work(struct work_struct *work);
 
 void iser_rcv_completion(struct iser_rx_desc *desc,
-			 unsigned long    dto_xfer_len,
-			struct iser_conn *ib_conn);
+			 unsigned long dto_xfer_len,
+			 struct ib_conn *ib_conn);
 
-void iser_snd_completion(struct iser_tx_desc *desc, struct iser_conn *ib_conn);
+void iser_snd_completion(struct iser_tx_desc *desc,
+			 struct ib_conn *ib_conn);
 
 void iser_task_rdma_init(struct iscsi_iser_task *task);
 
 void iser_task_rdma_finalize(struct iscsi_iser_task *task);
 
-void iser_free_rx_descriptors(struct iser_conn *ib_conn);
+void iser_free_rx_descriptors(struct iser_conn *iser_conn);
 
 void iser_finalize_rdma_unaligned_sg(struct iscsi_iser_task *iser_task,
 				     struct iser_data_buf *mem,
@@ -449,38 +627,40 @@ int  iser_reg_rdma_mem_fmr(struct iscsi_iser_task *task,
 int  iser_reg_rdma_mem_fastreg(struct iscsi_iser_task *task,
 			       enum iser_data_dir cmd_dir);
 
-int  iser_connect(struct iser_conn   *ib_conn,
-		  struct sockaddr    *src_addr,
-		  struct sockaddr    *dst_addr,
-		  int                non_blocking);
+int  iser_connect(struct iser_conn *iser_conn,
+		  struct sockaddr *src_addr,
+		  struct sockaddr *dst_addr,
+		  int non_blocking);
 
-int  iser_reg_page_vec(struct iser_conn     *ib_conn,
+int  iser_reg_page_vec(struct ib_conn *ib_conn,
 		       struct iser_page_vec *page_vec,
-		       struct iser_mem_reg  *mem_reg);
+		       struct iser_mem_reg *mem_reg);
 
 void iser_unreg_mem_fmr(struct iscsi_iser_task *iser_task,
 			enum iser_data_dir cmd_dir);
 void iser_unreg_mem_fastreg(struct iscsi_iser_task *iser_task,
 			    enum iser_data_dir cmd_dir);
 
-int  iser_post_recvl(struct iser_conn *ib_conn);
-int  iser_post_recvm(struct iser_conn *ib_conn, int count);
-int  iser_post_send(struct iser_conn *ib_conn, struct iser_tx_desc *tx_desc);
+int  iser_post_recvl(struct iser_conn *iser_conn);
+int  iser_post_recvm(struct iser_conn *iser_conn, int count);
+int  iser_post_send(struct ib_conn *ib_conn, struct iser_tx_desc *tx_desc,
+		    bool signal);
 
 int iser_dma_map_task_data(struct iscsi_iser_task *iser_task,
-			    struct iser_data_buf       *data,
-			    enum   iser_data_dir       iser_dir,
-			    enum   dma_data_direction  dma_dir);
+			   struct iser_data_buf *data,
+			   enum iser_data_dir iser_dir,
+			   enum dma_data_direction dma_dir);
 
 void iser_dma_unmap_task_data(struct iscsi_iser_task *iser_task,
 			      struct iser_data_buf *data);
 int  iser_initialize_task_headers(struct iscsi_task *task,
 			struct iser_tx_desc *tx_desc);
-int iser_alloc_rx_descriptors(struct iser_conn *ib_conn, struct iscsi_session *session);
-int iser_create_fmr_pool(struct iser_conn *ib_conn, unsigned cmds_max);
-void iser_free_fmr_pool(struct iser_conn *ib_conn);
-int iser_create_fastreg_pool(struct iser_conn *ib_conn, unsigned cmds_max);
-void iser_free_fastreg_pool(struct iser_conn *ib_conn);
+int iser_alloc_rx_descriptors(struct iser_conn *iser_conn,
+			      struct iscsi_session *session);
+int iser_create_fmr_pool(struct ib_conn *ib_conn, unsigned cmds_max);
+void iser_free_fmr_pool(struct ib_conn *ib_conn);
+int iser_create_fastreg_pool(struct ib_conn *ib_conn, unsigned cmds_max);
+void iser_free_fastreg_pool(struct ib_conn *ib_conn);
 u8 iser_check_task_pi_status(struct iscsi_iser_task *iser_task,
 			     enum iser_data_dir cmd_dir, sector_t *sector);
 #endif
diff --git a/drivers/infiniband/ulp/iser/iser_initiator.c b/drivers/infiniband/ulp/iser/iser_initiator.c
index 8d44a4060634..5a489ea63732 100644
--- a/drivers/infiniband/ulp/iser/iser_initiator.c
+++ b/drivers/infiniband/ulp/iser/iser_initiator.c
@@ -49,7 +49,7 @@ static int iser_prepare_read_cmd(struct iscsi_task *task)
 
 {
 	struct iscsi_iser_task *iser_task = task->dd_data;
-	struct iser_device  *device = iser_task->ib_conn->device;
+	struct iser_device  *device = iser_task->iser_conn->ib_conn.device;
 	struct iser_regd_buf *regd_buf;
 	int err;
 	struct iser_hdr *hdr = &iser_task->desc.iser_header;
@@ -103,7 +103,7 @@ iser_prepare_write_cmd(struct iscsi_task *task,
 		       unsigned int edtl)
 {
 	struct iscsi_iser_task *iser_task = task->dd_data;
-	struct iser_device  *device = iser_task->ib_conn->device;
+	struct iser_device  *device = iser_task->iser_conn->ib_conn.device;
 	struct iser_regd_buf *regd_buf;
 	int err;
 	struct iser_hdr *hdr = &iser_task->desc.iser_header;
@@ -160,10 +160,10 @@ iser_prepare_write_cmd(struct iscsi_task *task,
 }
 
 /* creates a new tx descriptor and adds header regd buffer */
-static void iser_create_send_desc(struct iser_conn	*ib_conn,
+static void iser_create_send_desc(struct iser_conn	*iser_conn,
 				  struct iser_tx_desc	*tx_desc)
 {
-	struct iser_device *device = ib_conn->device;
+	struct iser_device *device = iser_conn->ib_conn.device;
 
 	ib_dma_sync_single_for_cpu(device->ib_device,
 		tx_desc->dma_addr, ISER_HEADERS_LEN, DMA_TO_DEVICE);
@@ -179,103 +179,108 @@ static void iser_create_send_desc(struct iser_conn	*ib_conn,
 	}
 }
 
-static void iser_free_login_buf(struct iser_conn *ib_conn)
+static void iser_free_login_buf(struct iser_conn *iser_conn)
 {
-	if (!ib_conn->login_buf)
+	struct iser_device *device = iser_conn->ib_conn.device;
+
+	if (!iser_conn->login_buf)
 		return;
 
-	if (ib_conn->login_req_dma)
-		ib_dma_unmap_single(ib_conn->device->ib_device,
-				    ib_conn->login_req_dma,
+	if (iser_conn->login_req_dma)
+		ib_dma_unmap_single(device->ib_device,
+				    iser_conn->login_req_dma,
 				    ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_TO_DEVICE);
 
-	if (ib_conn->login_resp_dma)
-		ib_dma_unmap_single(ib_conn->device->ib_device,
-				    ib_conn->login_resp_dma,
+	if (iser_conn->login_resp_dma)
+		ib_dma_unmap_single(device->ib_device,
+				    iser_conn->login_resp_dma,
 				    ISER_RX_LOGIN_SIZE, DMA_FROM_DEVICE);
 
-	kfree(ib_conn->login_buf);
+	kfree(iser_conn->login_buf);
 
 	/* make sure we never redo any unmapping */
-	ib_conn->login_req_dma = 0;
-	ib_conn->login_resp_dma = 0;
-	ib_conn->login_buf = NULL;
+	iser_conn->login_req_dma = 0;
+	iser_conn->login_resp_dma = 0;
+	iser_conn->login_buf = NULL;
 }
 
-static int iser_alloc_login_buf(struct iser_conn *ib_conn)
+static int iser_alloc_login_buf(struct iser_conn *iser_conn)
 {
-	struct iser_device	*device;
+	struct iser_device *device = iser_conn->ib_conn.device;
 	int			req_err, resp_err;
 
-	BUG_ON(ib_conn->device == NULL);
+	BUG_ON(device == NULL);
 
-	device = ib_conn->device;
-
-	ib_conn->login_buf = kmalloc(ISCSI_DEF_MAX_RECV_SEG_LEN +
+	iser_conn->login_buf = kmalloc(ISCSI_DEF_MAX_RECV_SEG_LEN +
 				     ISER_RX_LOGIN_SIZE, GFP_KERNEL);
-	if (!ib_conn->login_buf)
+	if (!iser_conn->login_buf)
 		goto out_err;
 
-	ib_conn->login_req_buf  = ib_conn->login_buf;
-	ib_conn->login_resp_buf = ib_conn->login_buf +
+	iser_conn->login_req_buf  = iser_conn->login_buf;
+	iser_conn->login_resp_buf = iser_conn->login_buf +
 						ISCSI_DEF_MAX_RECV_SEG_LEN;
 
-	ib_conn->login_req_dma = ib_dma_map_single(ib_conn->device->ib_device,
-				(void *)ib_conn->login_req_buf,
-				ISCSI_DEF_MAX_RECV_SEG_LEN, DMA_TO_DEVICE);
+	iser_conn->login_req_dma = ib_dma_map_single(device->ib_device,
+						     iser_conn->login_req_buf,
+						     ISCSI_DEF_MAX_RECV_SEG_LEN,
+						     DMA_TO_DEVICE);
 
-	ib_conn->login_resp_dma = ib_dma_map_single(ib_conn->device->ib_device,
-				(void *)ib_conn->login_resp_buf,
-				ISER_RX_LOGIN_SIZE, DMA_FROM_DEVICE);
+	iser_conn->login_resp_dma = ib_dma_map_single(device->ib_device,
+						      iser_conn->login_resp_buf,
+						      ISER_RX_LOGIN_SIZE,
+						      DMA_FROM_DEVICE);
 
 	req_err  = ib_dma_mapping_error(device->ib_device,
-					ib_conn->login_req_dma);
+					iser_conn->login_req_dma);
 	resp_err = ib_dma_mapping_error(device->ib_device,
-					ib_conn->login_resp_dma);
+					iser_conn->login_resp_dma);
 
 	if (req_err || resp_err) {
 		if (req_err)
-			ib_conn->login_req_dma = 0;
+			iser_conn->login_req_dma = 0;
 		if (resp_err)
-			ib_conn->login_resp_dma = 0;
+			iser_conn->login_resp_dma = 0;
 		goto free_login_buf;
 	}
 	return 0;
 
 free_login_buf:
-	iser_free_login_buf(ib_conn);
+	iser_free_login_buf(iser_conn);
 
 out_err:
 	iser_err("unable to alloc or map login buf\n");
 	return -ENOMEM;
 }
 
-int iser_alloc_rx_descriptors(struct iser_conn *ib_conn, struct iscsi_session *session)
+int iser_alloc_rx_descriptors(struct iser_conn *iser_conn,
+			      struct iscsi_session *session)
 {
 	int i, j;
 	u64 dma_addr;
 	struct iser_rx_desc *rx_desc;
 	struct ib_sge       *rx_sg;
-	struct iser_device  *device = ib_conn->device;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
+	struct iser_device *device = ib_conn->device;
 
-	ib_conn->qp_max_recv_dtos = session->cmds_max;
-	ib_conn->qp_max_recv_dtos_mask = session->cmds_max - 1; /* cmds_max is 2^N */
-	ib_conn->min_posted_rx = ib_conn->qp_max_recv_dtos >> 2;
+	iser_conn->qp_max_recv_dtos = session->cmds_max;
+	iser_conn->qp_max_recv_dtos_mask = session->cmds_max - 1; /* cmds_max is 2^N */
+	iser_conn->min_posted_rx = iser_conn->qp_max_recv_dtos >> 2;
 
 	if (device->iser_alloc_rdma_reg_res(ib_conn, session->scsi_cmds_max))
 		goto create_rdma_reg_res_failed;
 
-	if (iser_alloc_login_buf(ib_conn))
+	if (iser_alloc_login_buf(iser_conn))
 		goto alloc_login_buf_fail;
 
-	ib_conn->rx_descs = kmalloc(session->cmds_max *
+	iser_conn->num_rx_descs = session->cmds_max;
+	iser_conn->rx_descs = kmalloc(iser_conn->num_rx_descs *
 				sizeof(struct iser_rx_desc), GFP_KERNEL);
-	if (!ib_conn->rx_descs)
+	if (!iser_conn->rx_descs)
 		goto rx_desc_alloc_fail;
 
-	rx_desc = ib_conn->rx_descs;
+	rx_desc = iser_conn->rx_descs;
 
-	for (i = 0; i < ib_conn->qp_max_recv_dtos; i++, rx_desc++)  {
+	for (i = 0; i < iser_conn->qp_max_recv_dtos; i++, rx_desc++)  {
 		dma_addr = ib_dma_map_single(device->ib_device, (void *)rx_desc,
 					ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
 		if (ib_dma_mapping_error(device->ib_device, dma_addr))
@@ -289,18 +294,18 @@ int iser_alloc_rx_descriptors(struct iser_conn *ib_conn, struct iscsi_session *s
 		rx_sg->lkey   = device->mr->lkey;
 	}
 
-	ib_conn->rx_desc_head = 0;
+	iser_conn->rx_desc_head = 0;
 	return 0;
 
 rx_desc_dma_map_failed:
-	rx_desc = ib_conn->rx_descs;
+	rx_desc = iser_conn->rx_descs;
 	for (j = 0; j < i; j++, rx_desc++)
 		ib_dma_unmap_single(device->ib_device, rx_desc->dma_addr,
 				    ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
-	kfree(ib_conn->rx_descs);
-	ib_conn->rx_descs = NULL;
+	kfree(iser_conn->rx_descs);
+	iser_conn->rx_descs = NULL;
 rx_desc_alloc_fail:
-	iser_free_login_buf(ib_conn);
+	iser_free_login_buf(iser_conn);
 alloc_login_buf_fail:
 	device->iser_free_rdma_reg_res(ib_conn);
 create_rdma_reg_res_failed:
@@ -308,33 +313,35 @@ create_rdma_reg_res_failed:
 	return -ENOMEM;
 }
 
-void iser_free_rx_descriptors(struct iser_conn *ib_conn)
+void iser_free_rx_descriptors(struct iser_conn *iser_conn)
 {
 	int i;
 	struct iser_rx_desc *rx_desc;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	struct iser_device *device = ib_conn->device;
 
-	if (!ib_conn->rx_descs)
+	if (!iser_conn->rx_descs)
 		goto free_login_buf;
 
 	if (device->iser_free_rdma_reg_res)
 		device->iser_free_rdma_reg_res(ib_conn);
 
-	rx_desc = ib_conn->rx_descs;
-	for (i = 0; i < ib_conn->qp_max_recv_dtos; i++, rx_desc++)
+	rx_desc = iser_conn->rx_descs;
+	for (i = 0; i < iser_conn->qp_max_recv_dtos; i++, rx_desc++)
 		ib_dma_unmap_single(device->ib_device, rx_desc->dma_addr,
 				    ISER_RX_PAYLOAD_SIZE, DMA_FROM_DEVICE);
-	kfree(ib_conn->rx_descs);
+	kfree(iser_conn->rx_descs);
 	/* make sure we never redo any unmapping */
-	ib_conn->rx_descs = NULL;
+	iser_conn->rx_descs = NULL;
 
 free_login_buf:
-	iser_free_login_buf(ib_conn);
+	iser_free_login_buf(iser_conn);
 }
 
 static int iser_post_rx_bufs(struct iscsi_conn *conn, struct iscsi_hdr *req)
 {
-	struct iser_conn *ib_conn = conn->dd_data;
+	struct iser_conn *iser_conn = conn->dd_data;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	struct iscsi_session *session = conn->session;
 
 	iser_dbg("req op %x flags %x\n", req->opcode, req->flags);
@@ -343,34 +350,37 @@ static int iser_post_rx_bufs(struct iscsi_conn *conn, struct iscsi_hdr *req)
 		return 0;
 
 	/*
-	 * Check that there is one posted recv buffer (for the last login
-	 * response) and no posted send buffers left - they must have been
-	 * consumed during previous login phases.
+	 * Check that there is one posted recv buffer
+	 * (for the last login response).
 	 */
 	WARN_ON(ib_conn->post_recv_buf_count != 1);
-	WARN_ON(atomic_read(&ib_conn->post_send_buf_count) != 0);
 
 	if (session->discovery_sess) {
 		iser_info("Discovery session, re-using login RX buffer\n");
 		return 0;
 	} else
 		iser_info("Normal session, posting batch of RX %d buffers\n",
-			  ib_conn->min_posted_rx);
+			  iser_conn->min_posted_rx);
 
 	/* Initial post receive buffers */
-	if (iser_post_recvm(ib_conn, ib_conn->min_posted_rx))
+	if (iser_post_recvm(iser_conn, iser_conn->min_posted_rx))
 		return -ENOMEM;
 
 	return 0;
 }
 
+static inline bool iser_signal_comp(int sig_count)
+{
+	return ((sig_count % ISER_SIGNAL_CMD_COUNT) == 0);
+}
+
 /**
  * iser_send_command - send command PDU
  */
 int iser_send_command(struct iscsi_conn *conn,
 		      struct iscsi_task *task)
 {
-	struct iser_conn *ib_conn = conn->dd_data;
+	struct iser_conn *iser_conn = conn->dd_data;
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	unsigned long edtl;
 	int err;
@@ -378,12 +388,13 @@ int iser_send_command(struct iscsi_conn *conn,
 	struct iscsi_scsi_req *hdr = (struct iscsi_scsi_req *)task->hdr;
 	struct scsi_cmnd *sc  =  task->sc;
 	struct iser_tx_desc *tx_desc = &iser_task->desc;
+	static unsigned sig_count;
 
 	edtl = ntohl(hdr->data_length);
 
 	/* build the tx desc regd header and add it to the tx desc dto */
 	tx_desc->type = ISCSI_TX_SCSI_COMMAND;
-	iser_create_send_desc(ib_conn, tx_desc);
+	iser_create_send_desc(iser_conn, tx_desc);
 
 	if (hdr->flags & ISCSI_FLAG_CMD_READ) {
 		data_buf = &iser_task->data[ISER_DIR_IN];
@@ -423,7 +434,8 @@ int iser_send_command(struct iscsi_conn *conn,
 
 	iser_task->status = ISER_TASK_STATUS_STARTED;
 
-	err = iser_post_send(ib_conn, tx_desc);
+	err = iser_post_send(&iser_conn->ib_conn, tx_desc,
+			     iser_signal_comp(++sig_count));
 	if (!err)
 		return 0;
 
@@ -439,7 +451,7 @@ int iser_send_data_out(struct iscsi_conn *conn,
 		       struct iscsi_task *task,
 		       struct iscsi_data *hdr)
 {
-	struct iser_conn *ib_conn = conn->dd_data;
+	struct iser_conn *iser_conn = conn->dd_data;
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	struct iser_tx_desc *tx_desc = NULL;
 	struct iser_regd_buf *regd_buf;
@@ -488,7 +500,7 @@ int iser_send_data_out(struct iscsi_conn *conn,
 		 itt, buf_offset, data_seg_len);
 
 
-	err = iser_post_send(ib_conn, tx_desc);
+	err = iser_post_send(&iser_conn->ib_conn, tx_desc, true);
 	if (!err)
 		return 0;
 
@@ -501,7 +513,7 @@ send_data_out_error:
 int iser_send_control(struct iscsi_conn *conn,
 		      struct iscsi_task *task)
 {
-	struct iser_conn *ib_conn = conn->dd_data;
+	struct iser_conn *iser_conn = conn->dd_data;
 	struct iscsi_iser_task *iser_task = task->dd_data;
 	struct iser_tx_desc *mdesc = &iser_task->desc;
 	unsigned long data_seg_len;
@@ -510,9 +522,9 @@ int iser_send_control(struct iscsi_conn *conn,
 
 	/* build the tx desc regd header and add it to the tx desc dto */
 	mdesc->type = ISCSI_TX_CONTROL;
-	iser_create_send_desc(ib_conn, mdesc);
+	iser_create_send_desc(iser_conn, mdesc);
 
-	device = ib_conn->device;
+	device = iser_conn->ib_conn.device;
 
 	data_seg_len = ntoh24(task->hdr->dlength);
 
@@ -524,16 +536,16 @@ int iser_send_control(struct iscsi_conn *conn,
 		}
 
 		ib_dma_sync_single_for_cpu(device->ib_device,
-			ib_conn->login_req_dma, task->data_count,
+			iser_conn->login_req_dma, task->data_count,
 			DMA_TO_DEVICE);
 
-		memcpy(ib_conn->login_req_buf, task->data, task->data_count);
+		memcpy(iser_conn->login_req_buf, task->data, task->data_count);
 
 		ib_dma_sync_single_for_device(device->ib_device,
-			ib_conn->login_req_dma, task->data_count,
+			iser_conn->login_req_dma, task->data_count,
 			DMA_TO_DEVICE);
 
-		tx_dsg->addr    = ib_conn->login_req_dma;
+		tx_dsg->addr    = iser_conn->login_req_dma;
 		tx_dsg->length  = task->data_count;
 		tx_dsg->lkey    = device->mr->lkey;
 		mdesc->num_sge = 2;
@@ -542,7 +554,7 @@ int iser_send_control(struct iscsi_conn *conn,
 	if (task == conn->login_task) {
 		iser_dbg("op %x dsl %lx, posting login rx buffer\n",
 			 task->hdr->opcode, data_seg_len);
-		err = iser_post_recvl(ib_conn);
+		err = iser_post_recvl(iser_conn);
 		if (err)
 			goto send_control_error;
 		err = iser_post_rx_bufs(conn, task->hdr);
@@ -550,7 +562,7 @@ int iser_send_control(struct iscsi_conn *conn,
 			goto send_control_error;
 	}
 
-	err = iser_post_send(ib_conn, mdesc);
+	err = iser_post_send(&iser_conn->ib_conn, mdesc, true);
 	if (!err)
 		return 0;
 
@@ -564,15 +576,17 @@ send_control_error:
  */
 void iser_rcv_completion(struct iser_rx_desc *rx_desc,
 			 unsigned long rx_xfer_len,
-			 struct iser_conn *ib_conn)
+			 struct ib_conn *ib_conn)
 {
+	struct iser_conn *iser_conn = container_of(ib_conn, struct iser_conn,
+						   ib_conn);
 	struct iscsi_hdr *hdr;
 	u64 rx_dma;
 	int rx_buflen, outstanding, count, err;
 
 	/* differentiate between login to all other PDUs */
-	if ((char *)rx_desc == ib_conn->login_resp_buf) {
-		rx_dma = ib_conn->login_resp_dma;
+	if ((char *)rx_desc == iser_conn->login_resp_buf) {
+		rx_dma = iser_conn->login_resp_dma;
 		rx_buflen = ISER_RX_LOGIN_SIZE;
 	} else {
 		rx_dma = rx_desc->dma_addr;
@@ -580,14 +594,14 @@ void iser_rcv_completion(struct iser_rx_desc *rx_desc,
 	}
 
 	ib_dma_sync_single_for_cpu(ib_conn->device->ib_device, rx_dma,
-			rx_buflen, DMA_FROM_DEVICE);
+				   rx_buflen, DMA_FROM_DEVICE);
 
 	hdr = &rx_desc->iscsi_header;
 
 	iser_dbg("op 0x%x itt 0x%x dlen %d\n", hdr->opcode,
 			hdr->itt, (int)(rx_xfer_len - ISER_HEADERS_LEN));
 
-	iscsi_iser_recv(ib_conn->iscsi_conn, hdr, rx_desc->data,
+	iscsi_iser_recv(iser_conn->iscsi_conn, hdr, rx_desc->data,
 			rx_xfer_len - ISER_HEADERS_LEN);
 
 	ib_dma_sync_single_for_device(ib_conn->device->ib_device, rx_dma,
@@ -599,21 +613,21 @@ void iser_rcv_completion(struct iser_rx_desc *rx_desc,
 	 * for the posted rx bufs refcount to become zero handles everything   */
 	ib_conn->post_recv_buf_count--;
 
-	if (rx_dma == ib_conn->login_resp_dma)
+	if (rx_dma == iser_conn->login_resp_dma)
 		return;
 
 	outstanding = ib_conn->post_recv_buf_count;
-	if (outstanding + ib_conn->min_posted_rx <= ib_conn->qp_max_recv_dtos) {
-		count = min(ib_conn->qp_max_recv_dtos - outstanding,
-						ib_conn->min_posted_rx);
-		err = iser_post_recvm(ib_conn, count);
+	if (outstanding + iser_conn->min_posted_rx <= iser_conn->qp_max_recv_dtos) {
+		count = min(iser_conn->qp_max_recv_dtos - outstanding,
+			    iser_conn->min_posted_rx);
+		err = iser_post_recvm(iser_conn, count);
 		if (err)
 			iser_err("posting %d rx bufs err %d\n", count, err);
 	}
 }
 
 void iser_snd_completion(struct iser_tx_desc *tx_desc,
-			struct iser_conn *ib_conn)
+			struct ib_conn *ib_conn)
 {
 	struct iscsi_task *task;
 	struct iser_device *device = ib_conn->device;
@@ -625,8 +639,6 @@ void iser_snd_completion(struct iser_tx_desc *tx_desc,
 		tx_desc = NULL;
 	}
 
-	atomic_dec(&ib_conn->post_send_buf_count);
-
 	if (tx_desc && tx_desc->type == ISCSI_TX_CONTROL) {
 		/* this arithmetic is legal by libiscsi dd_data allocation */
 		task = (void *) ((long)(void *)tx_desc -
@@ -658,7 +670,7 @@ void iser_task_rdma_init(struct iscsi_iser_task *iser_task)
 
 void iser_task_rdma_finalize(struct iscsi_iser_task *iser_task)
 {
-	struct iser_device *device = iser_task->ib_conn->device;
+	struct iser_device *device = iser_task->iser_conn->ib_conn.device;
 	int is_rdma_data_aligned = 1;
 	int is_rdma_prot_aligned = 1;
 	int prot_count = scsi_prot_sg_count(iser_task->sc);
diff --git a/drivers/infiniband/ulp/iser/iser_memory.c b/drivers/infiniband/ulp/iser/iser_memory.c
index 47acd3ad3a17..6c5ce357fba6 100644
--- a/drivers/infiniband/ulp/iser/iser_memory.c
+++ b/drivers/infiniband/ulp/iser/iser_memory.c
@@ -49,7 +49,7 @@ static int iser_start_rdma_unaligned_sg(struct iscsi_iser_task *iser_task,
 					struct iser_data_buf *data_copy,
 					enum iser_data_dir cmd_dir)
 {
-	struct ib_device *dev = iser_task->ib_conn->device->ib_device;
+	struct ib_device *dev = iser_task->iser_conn->ib_conn.device->ib_device;
 	struct scatterlist *sgl = (struct scatterlist *)data->buf;
 	struct scatterlist *sg;
 	char *mem = NULL;
@@ -116,7 +116,7 @@ void iser_finalize_rdma_unaligned_sg(struct iscsi_iser_task *iser_task,
 	struct ib_device *dev;
 	unsigned long  cmd_data_len;
 
-	dev = iser_task->ib_conn->device->ib_device;
+	dev = iser_task->iser_conn->ib_conn.device->ib_device;
 
 	ib_dma_unmap_sg(dev, &data_copy->sg_single, 1,
 			(cmd_dir == ISER_DIR_OUT) ?
@@ -322,7 +322,7 @@ int iser_dma_map_task_data(struct iscsi_iser_task *iser_task,
 	struct ib_device *dev;
 
 	iser_task->dir[iser_dir] = 1;
-	dev = iser_task->ib_conn->device->ib_device;
+	dev = iser_task->iser_conn->ib_conn.device->ib_device;
 
 	data->dma_nents = ib_dma_map_sg(dev, data->buf, data->size, dma_dir);
 	if (data->dma_nents == 0) {
@@ -337,7 +337,7 @@ void iser_dma_unmap_task_data(struct iscsi_iser_task *iser_task,
 {
 	struct ib_device *dev;
 
-	dev = iser_task->ib_conn->device->ib_device;
+	dev = iser_task->iser_conn->ib_conn.device->ib_device;
 	ib_dma_unmap_sg(dev, data->buf, data->size, DMA_FROM_DEVICE);
 }
 
@@ -348,7 +348,7 @@ static int fall_to_bounce_buf(struct iscsi_iser_task *iser_task,
 			      enum iser_data_dir cmd_dir,
 			      int aligned_len)
 {
-	struct iscsi_conn    *iscsi_conn = iser_task->ib_conn->iscsi_conn;
+	struct iscsi_conn    *iscsi_conn = iser_task->iser_conn->iscsi_conn;
 
 	iscsi_conn->fmr_unalign_cnt++;
 	iser_warn("rdma alignment violation (%d/%d aligned) or FMR not supported\n",
@@ -377,7 +377,7 @@ static int fall_to_bounce_buf(struct iscsi_iser_task *iser_task,
 int iser_reg_rdma_mem_fmr(struct iscsi_iser_task *iser_task,
 			  enum iser_data_dir cmd_dir)
 {
-	struct iser_conn     *ib_conn = iser_task->ib_conn;
+	struct ib_conn *ib_conn = &iser_task->iser_conn->ib_conn;
 	struct iser_device   *device = ib_conn->device;
 	struct ib_device     *ibdev = device->ib_device;
 	struct iser_data_buf *mem = &iser_task->data[cmd_dir];
@@ -432,7 +432,7 @@ int iser_reg_rdma_mem_fmr(struct iscsi_iser_task *iser_task,
 				 ib_conn->fmr.page_vec->offset);
 			for (i = 0; i < ib_conn->fmr.page_vec->length; i++)
 				iser_err("page_vec[%d] = 0x%llx\n", i,
-					 (unsigned long long) ib_conn->fmr.page_vec->pages[i]);
+					 (unsigned long long)ib_conn->fmr.page_vec->pages[i]);
 		}
 		if (err)
 			return err;
@@ -440,77 +440,74 @@ int iser_reg_rdma_mem_fmr(struct iscsi_iser_task *iser_task,
 	return 0;
 }
 
-static inline enum ib_t10_dif_type
-scsi2ib_prot_type(unsigned char prot_type)
+static inline void
+iser_set_dif_domain(struct scsi_cmnd *sc, struct ib_sig_attrs *sig_attrs,
+		    struct ib_sig_domain *domain)
 {
-	switch (prot_type) {
-	case SCSI_PROT_DIF_TYPE0:
-		return IB_T10DIF_NONE;
-	case SCSI_PROT_DIF_TYPE1:
-		return IB_T10DIF_TYPE1;
-	case SCSI_PROT_DIF_TYPE2:
-		return IB_T10DIF_TYPE2;
-	case SCSI_PROT_DIF_TYPE3:
-		return IB_T10DIF_TYPE3;
-	default:
-		return IB_T10DIF_NONE;
-	}
-}
-
+	domain->sig_type = IB_SIG_TYPE_T10_DIF;
+	domain->sig.dif.pi_interval = sc->device->sector_size;
+	domain->sig.dif.ref_tag = scsi_get_lba(sc) & 0xffffffff;
+	/*
+	 * At the moment we hard code those, but in the future
+	 * we will take them from sc.
+	 */
+	domain->sig.dif.apptag_check_mask = 0xffff;
+	domain->sig.dif.app_escape = true;
+	domain->sig.dif.ref_escape = true;
+	if (scsi_get_prot_type(sc) == SCSI_PROT_DIF_TYPE1 ||
+	    scsi_get_prot_type(sc) == SCSI_PROT_DIF_TYPE2)
+		domain->sig.dif.ref_remap = true;
+};
 
 static int
 iser_set_sig_attrs(struct scsi_cmnd *sc, struct ib_sig_attrs *sig_attrs)
 {
-	unsigned char scsi_ptype = scsi_get_prot_type(sc);
-
-	sig_attrs->mem.sig_type = IB_SIG_TYPE_T10_DIF;
-	sig_attrs->wire.sig_type = IB_SIG_TYPE_T10_DIF;
-	sig_attrs->mem.sig.dif.pi_interval = sc->device->sector_size;
-	sig_attrs->wire.sig.dif.pi_interval = sc->device->sector_size;
-
 	switch (scsi_get_prot_op(sc)) {
 	case SCSI_PROT_WRITE_INSERT:
 	case SCSI_PROT_READ_STRIP:
-		sig_attrs->mem.sig.dif.type = IB_T10DIF_NONE;
-		sig_attrs->wire.sig.dif.type = scsi2ib_prot_type(scsi_ptype);
+		sig_attrs->mem.sig_type = IB_SIG_TYPE_NONE;
+		iser_set_dif_domain(sc, sig_attrs, &sig_attrs->wire);
 		sig_attrs->wire.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->wire.sig.dif.ref_tag = scsi_get_lba(sc) &
-						  0xffffffff;
 		break;
 	case SCSI_PROT_READ_INSERT:
 	case SCSI_PROT_WRITE_STRIP:
-		sig_attrs->mem.sig.dif.type = scsi2ib_prot_type(scsi_ptype);
-		sig_attrs->mem.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->mem.sig.dif.ref_tag = scsi_get_lba(sc) &
-						 0xffffffff;
-		sig_attrs->wire.sig.dif.type = IB_T10DIF_NONE;
+		sig_attrs->wire.sig_type = IB_SIG_TYPE_NONE;
+		iser_set_dif_domain(sc, sig_attrs, &sig_attrs->mem);
+		/*
+		 * At the moment we use this modparam to tell what is
+		 * the memory bg_type, in the future we will take it
+		 * from sc.
+		 */
+		sig_attrs->mem.sig.dif.bg_type = iser_pi_guard ? IB_T10DIF_CSUM :
+						 IB_T10DIF_CRC;
 		break;
 	case SCSI_PROT_READ_PASS:
 	case SCSI_PROT_WRITE_PASS:
-		sig_attrs->mem.sig.dif.type = scsi2ib_prot_type(scsi_ptype);
-		sig_attrs->mem.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->mem.sig.dif.ref_tag = scsi_get_lba(sc) &
-						 0xffffffff;
-		sig_attrs->wire.sig.dif.type = scsi2ib_prot_type(scsi_ptype);
+		iser_set_dif_domain(sc, sig_attrs, &sig_attrs->wire);
 		sig_attrs->wire.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->wire.sig.dif.ref_tag = scsi_get_lba(sc) &
-						  0xffffffff;
+		iser_set_dif_domain(sc, sig_attrs, &sig_attrs->mem);
+		/*
+		 * At the moment we use this modparam to tell what is
+		 * the memory bg_type, in the future we will take it
+		 * from sc.
+		 */
+		sig_attrs->mem.sig.dif.bg_type = iser_pi_guard ? IB_T10DIF_CSUM :
+						 IB_T10DIF_CRC;
 		break;
 	default:
 		iser_err("Unsupported PI operation %d\n",
 			 scsi_get_prot_op(sc));
 		return -EINVAL;
 	}
+
 	return 0;
 }
 
-
 static int
 iser_set_prot_checks(struct scsi_cmnd *sc, u8 *mask)
 {
 	switch (scsi_get_prot_type(sc)) {
 	case SCSI_PROT_DIF_TYPE0:
-		*mask = 0x0;
 		break;
 	case SCSI_PROT_DIF_TYPE1:
 	case SCSI_PROT_DIF_TYPE2:
@@ -533,7 +530,7 @@ iser_reg_sig_mr(struct iscsi_iser_task *iser_task,
 		struct fast_reg_descriptor *desc, struct ib_sge *data_sge,
 		struct ib_sge *prot_sge, struct ib_sge *sig_sge)
 {
-	struct iser_conn *ib_conn = iser_task->ib_conn;
+	struct ib_conn *ib_conn = &iser_task->iser_conn->ib_conn;
 	struct iser_pi_context *pi_ctx = desc->pi_ctx;
 	struct ib_send_wr sig_wr, inv_wr;
 	struct ib_send_wr *bad_wr, *wr = NULL;
@@ -609,7 +606,7 @@ static int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 			    struct ib_sge *sge)
 {
 	struct fast_reg_descriptor *desc = regd_buf->reg.mem_h;
-	struct iser_conn *ib_conn = iser_task->ib_conn;
+	struct ib_conn *ib_conn = &iser_task->iser_conn->ib_conn;
 	struct iser_device *device = ib_conn->device;
 	struct ib_device *ibdev = device->ib_device;
 	struct ib_mr *mr;
@@ -700,7 +697,7 @@ static int iser_fast_reg_mr(struct iscsi_iser_task *iser_task,
 int iser_reg_rdma_mem_fastreg(struct iscsi_iser_task *iser_task,
 			      enum iser_data_dir cmd_dir)
 {
-	struct iser_conn *ib_conn = iser_task->ib_conn;
+	struct ib_conn *ib_conn = &iser_task->iser_conn->ib_conn;
 	struct iser_device *device = ib_conn->device;
 	struct ib_device *ibdev = device->ib_device;
 	struct iser_data_buf *mem = &iser_task->data[cmd_dir];
diff --git a/drivers/infiniband/ulp/iser/iser_verbs.c b/drivers/infiniband/ulp/iser/iser_verbs.c
index 3ef167f97d6f..67225bb82bb5 100644
--- a/drivers/infiniband/ulp/iser/iser_verbs.c
+++ b/drivers/infiniband/ulp/iser/iser_verbs.c
@@ -39,8 +39,12 @@
 #include "iscsi_iser.h"
 
 #define ISCSI_ISER_MAX_CONN	8
-#define ISER_MAX_RX_CQ_LEN	(ISER_QP_MAX_RECV_DTOS * ISCSI_ISER_MAX_CONN)
-#define ISER_MAX_TX_CQ_LEN	(ISER_QP_MAX_REQ_DTOS  * ISCSI_ISER_MAX_CONN)
+#define ISER_MAX_RX_LEN		(ISER_QP_MAX_RECV_DTOS * ISCSI_ISER_MAX_CONN)
+#define ISER_MAX_TX_LEN		(ISER_QP_MAX_REQ_DTOS  * ISCSI_ISER_MAX_CONN)
+#define ISER_MAX_CQ_LEN		(ISER_MAX_RX_LEN + ISER_MAX_TX_LEN + \
+				 ISCSI_ISER_MAX_CONN)
+
+static int iser_cq_poll_limit = 512;
 
 static void iser_cq_tasklet_fn(unsigned long data);
 static void iser_cq_callback(struct ib_cq *cq, void *cq_context);
@@ -71,9 +75,8 @@ static void iser_event_handler(struct ib_event_handler *handler,
  */
 static int iser_create_device_ib_res(struct iser_device *device)
 {
-	struct iser_cq_desc *cq_desc;
 	struct ib_device_attr *dev_attr = &device->dev_attr;
-	int ret, i, j;
+	int ret, i;
 
 	ret = ib_query_device(device->ib_device, dev_attr);
 	if (ret) {
@@ -101,47 +104,35 @@ static int iser_create_device_ib_res(struct iser_device *device)
 		return -1;
 	}
 
-	device->cqs_used = min(ISER_MAX_CQ, device->ib_device->num_comp_vectors);
+	device->comps_used = min(ISER_MAX_CQ,
+				 device->ib_device->num_comp_vectors);
 	iser_info("using %d CQs, device %s supports %d vectors\n",
-		  device->cqs_used, device->ib_device->name,
+		  device->comps_used, device->ib_device->name,
 		  device->ib_device->num_comp_vectors);
 
-	device->cq_desc = kmalloc(sizeof(struct iser_cq_desc) * device->cqs_used,
-				  GFP_KERNEL);
-	if (device->cq_desc == NULL)
-		goto cq_desc_err;
-	cq_desc = device->cq_desc;
-
 	device->pd = ib_alloc_pd(device->ib_device);
 	if (IS_ERR(device->pd))
 		goto pd_err;
 
-	for (i = 0; i < device->cqs_used; i++) {
-		cq_desc[i].device   = device;
-		cq_desc[i].cq_index = i;
-
-		device->rx_cq[i] = ib_create_cq(device->ib_device,
-					  iser_cq_callback,
-					  iser_cq_event_callback,
-					  (void *)&cq_desc[i],
-					  ISER_MAX_RX_CQ_LEN, i);
-		if (IS_ERR(device->rx_cq[i]))
-			goto cq_err;
-
-		device->tx_cq[i] = ib_create_cq(device->ib_device,
-					  NULL, iser_cq_event_callback,
-					  (void *)&cq_desc[i],
-					  ISER_MAX_TX_CQ_LEN, i);
-
-		if (IS_ERR(device->tx_cq[i]))
+	for (i = 0; i < device->comps_used; i++) {
+		struct iser_comp *comp = &device->comps[i];
+
+		comp->device = device;
+		comp->cq = ib_create_cq(device->ib_device,
+					iser_cq_callback,
+					iser_cq_event_callback,
+					(void *)comp,
+					ISER_MAX_CQ_LEN, i);
+		if (IS_ERR(comp->cq)) {
+			comp->cq = NULL;
 			goto cq_err;
+		}
 
-		if (ib_req_notify_cq(device->rx_cq[i], IB_CQ_NEXT_COMP))
+		if (ib_req_notify_cq(comp->cq, IB_CQ_NEXT_COMP))
 			goto cq_err;
 
-		tasklet_init(&device->cq_tasklet[i],
-			     iser_cq_tasklet_fn,
-			(unsigned long)&cq_desc[i]);
+		tasklet_init(&comp->tasklet, iser_cq_tasklet_fn,
+			     (unsigned long)comp);
 	}
 
 	device->mr = ib_get_dma_mr(device->pd, IB_ACCESS_LOCAL_WRITE |
@@ -160,19 +151,17 @@ static int iser_create_device_ib_res(struct iser_device *device)
 handler_err:
 	ib_dereg_mr(device->mr);
 dma_mr_err:
-	for (j = 0; j < device->cqs_used; j++)
-		tasklet_kill(&device->cq_tasklet[j]);
+	for (i = 0; i < device->comps_used; i++)
+		tasklet_kill(&device->comps[i].tasklet);
 cq_err:
-	for (j = 0; j < i; j++) {
-		if (device->tx_cq[j])
-			ib_destroy_cq(device->tx_cq[j]);
-		if (device->rx_cq[j])
-			ib_destroy_cq(device->rx_cq[j]);
+	for (i = 0; i < device->comps_used; i++) {
+		struct iser_comp *comp = &device->comps[i];
+
+		if (comp->cq)
+			ib_destroy_cq(comp->cq);
 	}
 	ib_dealloc_pd(device->pd);
 pd_err:
-	kfree(device->cq_desc);
-cq_desc_err:
 	iser_err("failed to allocate an IB resource\n");
 	return -1;
 }
@@ -186,20 +175,18 @@ static void iser_free_device_ib_res(struct iser_device *device)
 	int i;
 	BUG_ON(device->mr == NULL);
 
-	for (i = 0; i < device->cqs_used; i++) {
-		tasklet_kill(&device->cq_tasklet[i]);
-		(void)ib_destroy_cq(device->tx_cq[i]);
-		(void)ib_destroy_cq(device->rx_cq[i]);
-		device->tx_cq[i] = NULL;
-		device->rx_cq[i] = NULL;
+	for (i = 0; i < device->comps_used; i++) {
+		struct iser_comp *comp = &device->comps[i];
+
+		tasklet_kill(&comp->tasklet);
+		ib_destroy_cq(comp->cq);
+		comp->cq = NULL;
 	}
 
 	(void)ib_unregister_event_handler(&device->event_handler);
 	(void)ib_dereg_mr(device->mr);
 	(void)ib_dealloc_pd(device->pd);
 
-	kfree(device->cq_desc);
-
 	device->mr = NULL;
 	device->pd = NULL;
 }
@@ -209,7 +196,7 @@ static void iser_free_device_ib_res(struct iser_device *device)
  *
  * returns 0 on success, or errno code on failure
  */
-int iser_create_fmr_pool(struct iser_conn *ib_conn, unsigned cmds_max)
+int iser_create_fmr_pool(struct ib_conn *ib_conn, unsigned cmds_max)
 {
 	struct iser_device *device = ib_conn->device;
 	struct ib_fmr_pool_param params;
@@ -259,7 +246,7 @@ int iser_create_fmr_pool(struct iser_conn *ib_conn, unsigned cmds_max)
 /**
  * iser_free_fmr_pool - releases the FMR pool and page vec
  */
-void iser_free_fmr_pool(struct iser_conn *ib_conn)
+void iser_free_fmr_pool(struct ib_conn *ib_conn)
 {
 	iser_info("freeing conn %p fmr pool %p\n",
 		  ib_conn, ib_conn->fmr.pool);
@@ -363,10 +350,10 @@ fast_reg_mr_failure:
  * for fast registration work requests.
  * returns 0 on success, or errno code on failure
  */
-int iser_create_fastreg_pool(struct iser_conn *ib_conn, unsigned cmds_max)
+int iser_create_fastreg_pool(struct ib_conn *ib_conn, unsigned cmds_max)
 {
-	struct iser_device	*device = ib_conn->device;
-	struct fast_reg_descriptor	*desc;
+	struct iser_device *device = ib_conn->device;
+	struct fast_reg_descriptor *desc;
 	int i, ret;
 
 	INIT_LIST_HEAD(&ib_conn->fastreg.pool);
@@ -402,7 +389,7 @@ err:
 /**
  * iser_free_fastreg_pool - releases the pool of fast_reg descriptors
  */
-void iser_free_fastreg_pool(struct iser_conn *ib_conn)
+void iser_free_fastreg_pool(struct ib_conn *ib_conn)
 {
 	struct fast_reg_descriptor *desc, *tmp;
 	int i = 0;
@@ -436,7 +423,7 @@ void iser_free_fastreg_pool(struct iser_conn *ib_conn)
  *
  * returns 0 on success, -1 on failure
  */
-static int iser_create_ib_conn_res(struct iser_conn *ib_conn)
+static int iser_create_ib_conn_res(struct ib_conn *ib_conn)
 {
 	struct iser_device	*device;
 	struct ib_qp_init_attr	init_attr;
@@ -451,28 +438,30 @@ static int iser_create_ib_conn_res(struct iser_conn *ib_conn)
 
 	mutex_lock(&ig.connlist_mutex);
 	/* select the CQ with the minimal number of usages */
-	for (index = 0; index < device->cqs_used; index++)
-		if (device->cq_active_qps[index] <
-		    device->cq_active_qps[min_index])
+	for (index = 0; index < device->comps_used; index++) {
+		if (device->comps[index].active_qps <
+		    device->comps[min_index].active_qps)
 			min_index = index;
-	device->cq_active_qps[min_index]++;
+	}
+	ib_conn->comp = &device->comps[min_index];
+	ib_conn->comp->active_qps++;
 	mutex_unlock(&ig.connlist_mutex);
 	iser_info("cq index %d used for ib_conn %p\n", min_index, ib_conn);
 
 	init_attr.event_handler = iser_qp_event_callback;
 	init_attr.qp_context	= (void *)ib_conn;
-	init_attr.send_cq	= device->tx_cq[min_index];
-	init_attr.recv_cq	= device->rx_cq[min_index];
+	init_attr.send_cq	= ib_conn->comp->cq;
+	init_attr.recv_cq	= ib_conn->comp->cq;
 	init_attr.cap.max_recv_wr  = ISER_QP_MAX_RECV_DTOS;
 	init_attr.cap.max_send_sge = 2;
 	init_attr.cap.max_recv_sge = 1;
 	init_attr.sq_sig_type	= IB_SIGNAL_REQ_WR;
 	init_attr.qp_type	= IB_QPT_RC;
 	if (ib_conn->pi_support) {
-		init_attr.cap.max_send_wr = ISER_QP_SIG_MAX_REQ_DTOS;
+		init_attr.cap.max_send_wr = ISER_QP_SIG_MAX_REQ_DTOS + 1;
 		init_attr.create_flags |= IB_QP_CREATE_SIGNATURE_EN;
 	} else {
-		init_attr.cap.max_send_wr  = ISER_QP_MAX_REQ_DTOS;
+		init_attr.cap.max_send_wr  = ISER_QP_MAX_REQ_DTOS + 1;
 	}
 
 	ret = rdma_create_qp(ib_conn->cma_id, device->pd, &init_attr);
@@ -491,30 +480,6 @@ out_err:
 }
 
 /**
- * releases the QP object
- */
-static void iser_free_ib_conn_res(struct iser_conn *ib_conn)
-{
-	int cq_index;
-	BUG_ON(ib_conn == NULL);
-
-	iser_info("freeing conn %p cma_id %p qp %p\n",
-		  ib_conn, ib_conn->cma_id,
-		  ib_conn->qp);
-
-	/* qp is created only once both addr & route are resolved */
-
-	if (ib_conn->qp != NULL) {
-		cq_index = ((struct iser_cq_desc *)ib_conn->qp->recv_cq->cq_context)->cq_index;
-		ib_conn->device->cq_active_qps[cq_index]--;
-
-		rdma_destroy_qp(ib_conn->cma_id);
-	}
-
-	ib_conn->qp	  = NULL;
-}
-
-/**
  * based on the resolved device node GUID see if there already allocated
  * device for this device. If there's no such, create one.
  */
@@ -568,88 +533,142 @@ static void iser_device_try_release(struct iser_device *device)
 /**
  * Called with state mutex held
  **/
-static int iser_conn_state_comp_exch(struct iser_conn *ib_conn,
-				     enum iser_ib_conn_state comp,
-				     enum iser_ib_conn_state exch)
+static int iser_conn_state_comp_exch(struct iser_conn *iser_conn,
+				     enum iser_conn_state comp,
+				     enum iser_conn_state exch)
 {
 	int ret;
 
-	if ((ret = (ib_conn->state == comp)))
-		ib_conn->state = exch;
+	ret = (iser_conn->state == comp);
+	if (ret)
+		iser_conn->state = exch;
+
 	return ret;
 }
 
 void iser_release_work(struct work_struct *work)
 {
-	struct iser_conn *ib_conn;
-	int rc;
+	struct iser_conn *iser_conn;
+
+	iser_conn = container_of(work, struct iser_conn, release_work);
 
-	ib_conn = container_of(work, struct iser_conn, release_work);
+	/* Wait for conn_stop to complete */
+	wait_for_completion(&iser_conn->stop_completion);
+	/* Wait for IB resouces cleanup to complete */
+	wait_for_completion(&iser_conn->ib_completion);
 
-	/* wait for .conn_stop callback */
-	rc = wait_for_completion_timeout(&ib_conn->stop_completion, 30 * HZ);
-	WARN_ON(rc == 0);
+	mutex_lock(&iser_conn->state_mutex);
+	iser_conn->state = ISER_CONN_DOWN;
+	mutex_unlock(&iser_conn->state_mutex);
+
+	iser_conn_release(iser_conn);
+}
+
+/**
+ * iser_free_ib_conn_res - release IB related resources
+ * @iser_conn: iser connection struct
+ * @destroy_device: indicator if we need to try to release
+ *     the iser device (only iscsi shutdown and DEVICE_REMOVAL
+ *     will use this.
+ *
+ * This routine is called with the iser state mutex held
+ * so the cm_id removal is out of here. It is Safe to
+ * be invoked multiple times.
+ */
+static void iser_free_ib_conn_res(struct iser_conn *iser_conn,
+				  bool destroy_device)
+{
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
+	struct iser_device *device = ib_conn->device;
 
-	/* wait for the qp`s post send and post receive buffers to empty */
-	rc = wait_for_completion_timeout(&ib_conn->flush_completion, 30 * HZ);
-	WARN_ON(rc == 0);
+	iser_info("freeing conn %p cma_id %p qp %p\n",
+		  iser_conn, ib_conn->cma_id, ib_conn->qp);
 
-	ib_conn->state = ISER_CONN_DOWN;
+	iser_free_rx_descriptors(iser_conn);
 
-	mutex_lock(&ib_conn->state_mutex);
-	ib_conn->state = ISER_CONN_DOWN;
-	mutex_unlock(&ib_conn->state_mutex);
+	if (ib_conn->qp != NULL) {
+		ib_conn->comp->active_qps--;
+		rdma_destroy_qp(ib_conn->cma_id);
+		ib_conn->qp = NULL;
+	}
 
-	iser_conn_release(ib_conn);
+	if (destroy_device && device != NULL) {
+		iser_device_try_release(device);
+		ib_conn->device = NULL;
+	}
 }
 
 /**
  * Frees all conn objects and deallocs conn descriptor
  */
-void iser_conn_release(struct iser_conn *ib_conn)
+void iser_conn_release(struct iser_conn *iser_conn)
 {
-	struct iser_device  *device = ib_conn->device;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 
 	mutex_lock(&ig.connlist_mutex);
-	list_del(&ib_conn->conn_list);
+	list_del(&iser_conn->conn_list);
 	mutex_unlock(&ig.connlist_mutex);
 
-	mutex_lock(&ib_conn->state_mutex);
-	BUG_ON(ib_conn->state != ISER_CONN_DOWN);
-
-	iser_free_rx_descriptors(ib_conn);
-	iser_free_ib_conn_res(ib_conn);
-	ib_conn->device = NULL;
-	/* on EVENT_ADDR_ERROR there's no device yet for this conn */
-	if (device != NULL)
-		iser_device_try_release(device);
-	mutex_unlock(&ib_conn->state_mutex);
+	mutex_lock(&iser_conn->state_mutex);
+	if (iser_conn->state != ISER_CONN_DOWN)
+		iser_warn("iser conn %p state %d, expected state down.\n",
+			  iser_conn, iser_conn->state);
+	/*
+	 * In case we never got to bind stage, we still need to
+	 * release IB resources (which is safe to call more than once).
+	 */
+	iser_free_ib_conn_res(iser_conn, true);
+	mutex_unlock(&iser_conn->state_mutex);
 
-	/* if cma handler context, the caller actually destroy the id */
 	if (ib_conn->cma_id != NULL) {
 		rdma_destroy_id(ib_conn->cma_id);
 		ib_conn->cma_id = NULL;
 	}
-	kfree(ib_conn);
+
+	kfree(iser_conn);
 }
 
 /**
  * triggers start of the disconnect procedures and wait for them to be done
+ * Called with state mutex held
  */
-void iser_conn_terminate(struct iser_conn *ib_conn)
+int iser_conn_terminate(struct iser_conn *iser_conn)
 {
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
+	struct ib_send_wr *bad_wr;
 	int err = 0;
 
-	/* change the ib conn state only if the conn is UP, however always call
-	 * rdma_disconnect since this is the only way to cause the CMA to change
-	 * the QP state to ERROR
+	/* terminate the iser conn only if the conn state is UP */
+	if (!iser_conn_state_comp_exch(iser_conn, ISER_CONN_UP,
+				       ISER_CONN_TERMINATING))
+		return 0;
+
+	iser_info("iser_conn %p state %d\n", iser_conn, iser_conn->state);
+
+	/* suspend queuing of new iscsi commands */
+	if (iser_conn->iscsi_conn)
+		iscsi_suspend_queue(iser_conn->iscsi_conn);
+
+	/*
+	 * In case we didn't already clean up the cma_id (peer initiated
+	 * a disconnection), we need to Cause the CMA to change the QP
+	 * state to ERROR.
 	 */
+	if (ib_conn->cma_id) {
+		err = rdma_disconnect(ib_conn->cma_id);
+		if (err)
+			iser_err("Failed to disconnect, conn: 0x%p err %d\n",
+				 iser_conn, err);
+
+		/* post an indication that all flush errors were consumed */
+		err = ib_post_send(ib_conn->qp, &ib_conn->beacon, &bad_wr);
+		if (err)
+			iser_err("conn %p failed to post beacon", ib_conn);
+
+		wait_for_completion(&ib_conn->flush_comp);
+	}
 
-	iser_conn_state_comp_exch(ib_conn, ISER_CONN_UP, ISER_CONN_TERMINATING);
-	err = rdma_disconnect(ib_conn->cma_id);
-	if (err)
-		iser_err("Failed to disconnect, conn: 0x%p err %d\n",
-			 ib_conn,err);
+	return 1;
 }
 
 /**
@@ -657,10 +676,10 @@ void iser_conn_terminate(struct iser_conn *ib_conn)
  **/
 static void iser_connect_error(struct rdma_cm_id *cma_id)
 {
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
 
-	ib_conn = (struct iser_conn *)cma_id->context;
-	ib_conn->state = ISER_CONN_DOWN;
+	iser_conn = (struct iser_conn *)cma_id->context;
+	iser_conn->state = ISER_CONN_DOWN;
 }
 
 /**
@@ -669,14 +688,16 @@ static void iser_connect_error(struct rdma_cm_id *cma_id)
 static void iser_addr_handler(struct rdma_cm_id *cma_id)
 {
 	struct iser_device *device;
-	struct iser_conn   *ib_conn;
+	struct iser_conn   *iser_conn;
+	struct ib_conn   *ib_conn;
 	int    ret;
 
-	ib_conn = (struct iser_conn *)cma_id->context;
-	if (ib_conn->state != ISER_CONN_PENDING)
+	iser_conn = (struct iser_conn *)cma_id->context;
+	if (iser_conn->state != ISER_CONN_PENDING)
 		/* bailout */
 		return;
 
+	ib_conn = &iser_conn->ib_conn;
 	device = iser_device_find_by_ib_device(cma_id);
 	if (!device) {
 		iser_err("device lookup/creation failed\n");
@@ -715,14 +736,15 @@ static void iser_route_handler(struct rdma_cm_id *cma_id)
 	struct rdma_conn_param conn_param;
 	int    ret;
 	struct iser_cm_hdr req_hdr;
-	struct iser_conn *ib_conn = (struct iser_conn *)cma_id->context;
+	struct iser_conn *iser_conn = (struct iser_conn *)cma_id->context;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	struct iser_device *device = ib_conn->device;
 
-	if (ib_conn->state != ISER_CONN_PENDING)
+	if (iser_conn->state != ISER_CONN_PENDING)
 		/* bailout */
 		return;
 
-	ret = iser_create_ib_conn_res((struct iser_conn *)cma_id->context);
+	ret = iser_create_ib_conn_res(ib_conn);
 	if (ret)
 		goto failure;
 
@@ -751,57 +773,60 @@ failure:
 
 static void iser_connected_handler(struct rdma_cm_id *cma_id)
 {
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
 	struct ib_qp_attr attr;
 	struct ib_qp_init_attr init_attr;
 
-	ib_conn = (struct iser_conn *)cma_id->context;
-	if (ib_conn->state != ISER_CONN_PENDING)
+	iser_conn = (struct iser_conn *)cma_id->context;
+	if (iser_conn->state != ISER_CONN_PENDING)
 		/* bailout */
 		return;
 
 	(void)ib_query_qp(cma_id->qp, &attr, ~0, &init_attr);
 	iser_info("remote qpn:%x my qpn:%x\n", attr.dest_qp_num, cma_id->qp->qp_num);
 
-	ib_conn->state = ISER_CONN_UP;
-	complete(&ib_conn->up_completion);
+	iser_conn->state = ISER_CONN_UP;
+	complete(&iser_conn->up_completion);
 }
 
 static void iser_disconnected_handler(struct rdma_cm_id *cma_id)
 {
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn = (struct iser_conn *)cma_id->context;
 
-	ib_conn = (struct iser_conn *)cma_id->context;
-
-	/* getting here when the state is UP means that the conn is being *
-	 * terminated asynchronously from the iSCSI layer's perspective.  */
-	if (iser_conn_state_comp_exch(ib_conn, ISER_CONN_UP,
-					ISER_CONN_TERMINATING)){
-		if (ib_conn->iscsi_conn)
-			iscsi_conn_failure(ib_conn->iscsi_conn, ISCSI_ERR_CONN_FAILED);
+	if (iser_conn_terminate(iser_conn)) {
+		if (iser_conn->iscsi_conn)
+			iscsi_conn_failure(iser_conn->iscsi_conn,
+					   ISCSI_ERR_CONN_FAILED);
 		else
 			iser_err("iscsi_iser connection isn't bound\n");
 	}
+}
 
-	/* Complete the termination process if no posts are pending. This code
-	 * block also exists in iser_handle_comp_error(), but it is needed here
-	 * for cases of no flushes at all, e.g. discovery over rdma.
+static void iser_cleanup_handler(struct rdma_cm_id *cma_id,
+				 bool destroy_device)
+{
+	struct iser_conn *iser_conn = (struct iser_conn *)cma_id->context;
+
+	/*
+	 * We are not guaranteed that we visited disconnected_handler
+	 * by now, call it here to be safe that we handle CM drep
+	 * and flush errors.
 	 */
-	if (ib_conn->post_recv_buf_count == 0 &&
-	    (atomic_read(&ib_conn->post_send_buf_count) == 0)) {
-		complete(&ib_conn->flush_completion);
-	}
-}
+	iser_disconnected_handler(cma_id);
+	iser_free_ib_conn_res(iser_conn, destroy_device);
+	complete(&iser_conn->ib_completion);
+};
 
 static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 {
-	struct iser_conn *ib_conn;
+	struct iser_conn *iser_conn;
+	int ret = 0;
 
-	ib_conn = (struct iser_conn *)cma_id->context;
+	iser_conn = (struct iser_conn *)cma_id->context;
 	iser_info("event %d status %d conn %p id %p\n",
 		  event->event, event->status, cma_id->context, cma_id);
 
-	mutex_lock(&ib_conn->state_mutex);
+	mutex_lock(&iser_conn->state_mutex);
 	switch (event->event) {
 	case RDMA_CM_EVENT_ADDR_RESOLVED:
 		iser_addr_handler(cma_id);
@@ -820,57 +845,73 @@ static int iser_cma_handler(struct rdma_cm_id *cma_id, struct rdma_cm_event *eve
 		iser_connect_error(cma_id);
 		break;
 	case RDMA_CM_EVENT_DISCONNECTED:
-	case RDMA_CM_EVENT_DEVICE_REMOVAL:
 	case RDMA_CM_EVENT_ADDR_CHANGE:
-	case RDMA_CM_EVENT_TIMEWAIT_EXIT:
 		iser_disconnected_handler(cma_id);
 		break;
+	case RDMA_CM_EVENT_DEVICE_REMOVAL:
+		/*
+		 * we *must* destroy the device as we cannot rely
+		 * on iscsid to be around to initiate error handling.
+		 * also implicitly destroy the cma_id.
+		 */
+		iser_cleanup_handler(cma_id, true);
+		iser_conn->ib_conn.cma_id = NULL;
+		ret = 1;
+		break;
+	case RDMA_CM_EVENT_TIMEWAIT_EXIT:
+		iser_cleanup_handler(cma_id, false);
+		break;
 	default:
 		iser_err("Unexpected RDMA CM event (%d)\n", event->event);
 		break;
 	}
-	mutex_unlock(&ib_conn->state_mutex);
-	return 0;
+	mutex_unlock(&iser_conn->state_mutex);
+
+	return ret;
 }
 
-void iser_conn_init(struct iser_conn *ib_conn)
+void iser_conn_init(struct iser_conn *iser_conn)
 {
-	ib_conn->state = ISER_CONN_INIT;
-	ib_conn->post_recv_buf_count = 0;
-	atomic_set(&ib_conn->post_send_buf_count, 0);
-	init_completion(&ib_conn->stop_completion);
-	init_completion(&ib_conn->flush_completion);
-	init_completion(&ib_conn->up_completion);
-	INIT_LIST_HEAD(&ib_conn->conn_list);
-	spin_lock_init(&ib_conn->lock);
-	mutex_init(&ib_conn->state_mutex);
+	iser_conn->state = ISER_CONN_INIT;
+	iser_conn->ib_conn.post_recv_buf_count = 0;
+	init_completion(&iser_conn->ib_conn.flush_comp);
+	init_completion(&iser_conn->stop_completion);
+	init_completion(&iser_conn->ib_completion);
+	init_completion(&iser_conn->up_completion);
+	INIT_LIST_HEAD(&iser_conn->conn_list);
+	spin_lock_init(&iser_conn->ib_conn.lock);
+	mutex_init(&iser_conn->state_mutex);
 }
 
  /**
  * starts the process of connecting to the target
  * sleeps until the connection is established or rejected
  */
-int iser_connect(struct iser_conn   *ib_conn,
+int iser_connect(struct iser_conn   *iser_conn,
 		 struct sockaddr    *src_addr,
 		 struct sockaddr    *dst_addr,
 		 int                 non_blocking)
 {
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	int err = 0;
 
-	mutex_lock(&ib_conn->state_mutex);
+	mutex_lock(&iser_conn->state_mutex);
 
-	sprintf(ib_conn->name, "%pISp", dst_addr);
+	sprintf(iser_conn->name, "%pISp", dst_addr);
 
-	iser_info("connecting to: %s\n", ib_conn->name);
+	iser_info("connecting to: %s\n", iser_conn->name);
 
 	/* the device is known only --after-- address resolution */
 	ib_conn->device = NULL;
 
-	ib_conn->state = ISER_CONN_PENDING;
+	iser_conn->state = ISER_CONN_PENDING;
+
+	ib_conn->beacon.wr_id = ISER_BEACON_WRID;
+	ib_conn->beacon.opcode = IB_WR_SEND;
 
 	ib_conn->cma_id = rdma_create_id(iser_cma_handler,
-					     (void *)ib_conn,
-					     RDMA_PS_TCP, IB_QPT_RC);
+					 (void *)iser_conn,
+					 RDMA_PS_TCP, IB_QPT_RC);
 	if (IS_ERR(ib_conn->cma_id)) {
 		err = PTR_ERR(ib_conn->cma_id);
 		iser_err("rdma_create_id failed: %d\n", err);
@@ -884,27 +925,27 @@ int iser_connect(struct iser_conn   *ib_conn,
 	}
 
 	if (!non_blocking) {
-		wait_for_completion_interruptible(&ib_conn->up_completion);
+		wait_for_completion_interruptible(&iser_conn->up_completion);
 
-		if (ib_conn->state != ISER_CONN_UP) {
+		if (iser_conn->state != ISER_CONN_UP) {
 			err =  -EIO;
 			goto connect_failure;
 		}
 	}
-	mutex_unlock(&ib_conn->state_mutex);
+	mutex_unlock(&iser_conn->state_mutex);
 
 	mutex_lock(&ig.connlist_mutex);
-	list_add(&ib_conn->conn_list, &ig.connlist);
+	list_add(&iser_conn->conn_list, &ig.connlist);
 	mutex_unlock(&ig.connlist_mutex);
 	return 0;
 
 id_failure:
 	ib_conn->cma_id = NULL;
 addr_failure:
-	ib_conn->state = ISER_CONN_DOWN;
+	iser_conn->state = ISER_CONN_DOWN;
 connect_failure:
-	mutex_unlock(&ib_conn->state_mutex);
-	iser_conn_release(ib_conn);
+	mutex_unlock(&iser_conn->state_mutex);
+	iser_conn_release(iser_conn);
 	return err;
 }
 
@@ -913,7 +954,7 @@ connect_failure:
  *
  * returns: 0 on success, errno code on failure
  */
-int iser_reg_page_vec(struct iser_conn     *ib_conn,
+int iser_reg_page_vec(struct ib_conn *ib_conn,
 		      struct iser_page_vec *page_vec,
 		      struct iser_mem_reg  *mem_reg)
 {
@@ -983,7 +1024,8 @@ void iser_unreg_mem_fastreg(struct iscsi_iser_task *iser_task,
 			    enum iser_data_dir cmd_dir)
 {
 	struct iser_mem_reg *reg = &iser_task->rdma_regd[cmd_dir].reg;
-	struct iser_conn *ib_conn = iser_task->ib_conn;
+	struct iser_conn *iser_conn = iser_task->iser_conn;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	struct fast_reg_descriptor *desc = reg->mem_h;
 
 	if (!reg->is_mr)
@@ -996,17 +1038,18 @@ void iser_unreg_mem_fastreg(struct iscsi_iser_task *iser_task,
 	spin_unlock_bh(&ib_conn->lock);
 }
 
-int iser_post_recvl(struct iser_conn *ib_conn)
+int iser_post_recvl(struct iser_conn *iser_conn)
 {
 	struct ib_recv_wr rx_wr, *rx_wr_failed;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
 	struct ib_sge	  sge;
 	int ib_ret;
 
-	sge.addr   = ib_conn->login_resp_dma;
+	sge.addr   = iser_conn->login_resp_dma;
 	sge.length = ISER_RX_LOGIN_SIZE;
 	sge.lkey   = ib_conn->device->mr->lkey;
 
-	rx_wr.wr_id   = (unsigned long)ib_conn->login_resp_buf;
+	rx_wr.wr_id   = (unsigned long)iser_conn->login_resp_buf;
 	rx_wr.sg_list = &sge;
 	rx_wr.num_sge = 1;
 	rx_wr.next    = NULL;
@@ -1020,20 +1063,21 @@ int iser_post_recvl(struct iser_conn *ib_conn)
 	return ib_ret;
 }
 
-int iser_post_recvm(struct iser_conn *ib_conn, int count)
+int iser_post_recvm(struct iser_conn *iser_conn, int count)
 {
 	struct ib_recv_wr *rx_wr, *rx_wr_failed;
 	int i, ib_ret;
-	unsigned int my_rx_head = ib_conn->rx_desc_head;
+	struct ib_conn *ib_conn = &iser_conn->ib_conn;
+	unsigned int my_rx_head = iser_conn->rx_desc_head;
 	struct iser_rx_desc *rx_desc;
 
 	for (rx_wr = ib_conn->rx_wr, i = 0; i < count; i++, rx_wr++) {
-		rx_desc		= &ib_conn->rx_descs[my_rx_head];
+		rx_desc		= &iser_conn->rx_descs[my_rx_head];
 		rx_wr->wr_id	= (unsigned long)rx_desc;
 		rx_wr->sg_list	= &rx_desc->rx_sg;
 		rx_wr->num_sge	= 1;
 		rx_wr->next	= rx_wr + 1;
-		my_rx_head = (my_rx_head + 1) & ib_conn->qp_max_recv_dtos_mask;
+		my_rx_head = (my_rx_head + 1) & iser_conn->qp_max_recv_dtos_mask;
 	}
 
 	rx_wr--;
@@ -1045,7 +1089,7 @@ int iser_post_recvm(struct iser_conn *ib_conn, int count)
 		iser_err("ib_post_recv failed ret=%d\n", ib_ret);
 		ib_conn->post_recv_buf_count -= count;
 	} else
-		ib_conn->rx_desc_head = my_rx_head;
+		iser_conn->rx_desc_head = my_rx_head;
 	return ib_ret;
 }
 
@@ -1055,139 +1099,166 @@ int iser_post_recvm(struct iser_conn *ib_conn, int count)
  *
  * returns 0 on success, -1 on failure
  */
-int iser_post_send(struct iser_conn *ib_conn, struct iser_tx_desc *tx_desc)
+int iser_post_send(struct ib_conn *ib_conn, struct iser_tx_desc *tx_desc,
+		   bool signal)
 {
 	int		  ib_ret;
 	struct ib_send_wr send_wr, *send_wr_failed;
 
 	ib_dma_sync_single_for_device(ib_conn->device->ib_device,
-		tx_desc->dma_addr, ISER_HEADERS_LEN, DMA_TO_DEVICE);
+				      tx_desc->dma_addr, ISER_HEADERS_LEN,
+				      DMA_TO_DEVICE);
 
 	send_wr.next	   = NULL;
 	send_wr.wr_id	   = (unsigned long)tx_desc;
 	send_wr.sg_list	   = tx_desc->tx_sg;
 	send_wr.num_sge	   = tx_desc->num_sge;
 	send_wr.opcode	   = IB_WR_SEND;
-	send_wr.send_flags = IB_SEND_SIGNALED;
-
-	atomic_inc(&ib_conn->post_send_buf_count);
+	send_wr.send_flags = signal ? IB_SEND_SIGNALED : 0;
 
 	ib_ret = ib_post_send(ib_conn->qp, &send_wr, &send_wr_failed);
-	if (ib_ret) {
+	if (ib_ret)
 		iser_err("ib_post_send failed, ret:%d\n", ib_ret);
-		atomic_dec(&ib_conn->post_send_buf_count);
-	}
+
 	return ib_ret;
 }
 
-static void iser_handle_comp_error(struct iser_tx_desc *desc,
-				struct iser_conn *ib_conn)
+/**
+ * is_iser_tx_desc - Indicate if the completion wr_id
+ *     is a TX descriptor or not.
+ * @iser_conn: iser connection
+ * @wr_id: completion WR identifier
+ *
+ * Since we cannot rely on wc opcode in FLUSH errors
+ * we must work around it by checking if the wr_id address
+ * falls in the iser connection rx_descs buffer. If so
+ * it is an RX descriptor, otherwize it is a TX.
+ */
+static inline bool
+is_iser_tx_desc(struct iser_conn *iser_conn, void *wr_id)
 {
-	if (desc && desc->type == ISCSI_TX_DATAOUT)
-		kmem_cache_free(ig.desc_cache, desc);
-
-	if (ib_conn->post_recv_buf_count == 0 &&
-	    atomic_read(&ib_conn->post_send_buf_count) == 0) {
-		/**
-		 * getting here when the state is UP means that the conn is
-		 * being terminated asynchronously from the iSCSI layer's
-		 * perspective. It is safe to peek at the connection state
-		 * since iscsi_conn_failure is allowed to be called twice.
-		 **/
-		if (ib_conn->state == ISER_CONN_UP)
-			iscsi_conn_failure(ib_conn->iscsi_conn,
+	void *start = iser_conn->rx_descs;
+	int len = iser_conn->num_rx_descs * sizeof(*iser_conn->rx_descs);
+
+	if (wr_id >= start && wr_id < start + len)
+		return false;
+
+	return true;
+}
+
+/**
+ * iser_handle_comp_error() - Handle error completion
+ * @ib_conn:   connection RDMA resources
+ * @wc:        work completion
+ *
+ * Notes: We may handle a FLUSH error completion and in this case
+ *        we only cleanup in case TX type was DATAOUT. For non-FLUSH
+ *        error completion we should also notify iscsi layer that
+ *        connection is failed (in case we passed bind stage).
+ */
+static void
+iser_handle_comp_error(struct ib_conn *ib_conn,
+		       struct ib_wc *wc)
+{
+	struct iser_conn *iser_conn = container_of(ib_conn, struct iser_conn,
+						   ib_conn);
+
+	if (wc->status != IB_WC_WR_FLUSH_ERR)
+		if (iser_conn->iscsi_conn)
+			iscsi_conn_failure(iser_conn->iscsi_conn,
 					   ISCSI_ERR_CONN_FAILED);
 
-		/* no more non completed posts to the QP, complete the
-		 * termination process w.o worrying on disconnect event */
-		complete(&ib_conn->flush_completion);
+	if (is_iser_tx_desc(iser_conn, (void *)wc->wr_id)) {
+		struct iser_tx_desc *desc = (struct iser_tx_desc *)wc->wr_id;
+
+		if (desc->type == ISCSI_TX_DATAOUT)
+			kmem_cache_free(ig.desc_cache, desc);
+	} else {
+		ib_conn->post_recv_buf_count--;
 	}
 }
 
-static int iser_drain_tx_cq(struct iser_device  *device, int cq_index)
+/**
+ * iser_handle_wc - handle a single work completion
+ * @wc: work completion
+ *
+ * Soft-IRQ context, work completion can be either
+ * SEND or RECV, and can turn out successful or
+ * with error (or flush error).
+ */
+static void iser_handle_wc(struct ib_wc *wc)
 {
-	struct ib_cq  *cq = device->tx_cq[cq_index];
-	struct ib_wc  wc;
+	struct ib_conn *ib_conn;
 	struct iser_tx_desc *tx_desc;
-	struct iser_conn *ib_conn;
-	int completed_tx = 0;
-
-	while (ib_poll_cq(cq, 1, &wc) == 1) {
-		tx_desc	= (struct iser_tx_desc *) (unsigned long) wc.wr_id;
-		ib_conn = wc.qp->qp_context;
-		if (wc.status == IB_WC_SUCCESS) {
-			if (wc.opcode == IB_WC_SEND)
-				iser_snd_completion(tx_desc, ib_conn);
-			else
-				iser_err("expected opcode %d got %d\n",
-					IB_WC_SEND, wc.opcode);
+	struct iser_rx_desc *rx_desc;
+
+	ib_conn = wc->qp->qp_context;
+	if (wc->status == IB_WC_SUCCESS) {
+		if (wc->opcode == IB_WC_RECV) {
+			rx_desc = (struct iser_rx_desc *)wc->wr_id;
+			iser_rcv_completion(rx_desc, wc->byte_len,
+					    ib_conn);
+		} else
+		if (wc->opcode == IB_WC_SEND) {
+			tx_desc = (struct iser_tx_desc *)wc->wr_id;
+			iser_snd_completion(tx_desc, ib_conn);
 		} else {
-			iser_err("tx id %llx status %d vend_err %x\n",
-				 wc.wr_id, wc.status, wc.vendor_err);
-			if (wc.wr_id != ISER_FASTREG_LI_WRID) {
-				atomic_dec(&ib_conn->post_send_buf_count);
-				iser_handle_comp_error(tx_desc, ib_conn);
-			}
+			iser_err("Unknown wc opcode %d\n", wc->opcode);
 		}
-		completed_tx++;
+	} else {
+		if (wc->status != IB_WC_WR_FLUSH_ERR)
+			iser_err("wr id %llx status %d vend_err %x\n",
+				 wc->wr_id, wc->status, wc->vendor_err);
+		else
+			iser_dbg("flush error: wr id %llx\n", wc->wr_id);
+
+		if (wc->wr_id != ISER_FASTREG_LI_WRID &&
+		    wc->wr_id != ISER_BEACON_WRID)
+			iser_handle_comp_error(ib_conn, wc);
+
+		/* complete in case all flush errors were consumed */
+		if (wc->wr_id == ISER_BEACON_WRID)
+			complete(&ib_conn->flush_comp);
 	}
-	return completed_tx;
 }
 
-
+/**
+ * iser_cq_tasklet_fn - iSER completion polling loop
+ * @data: iSER completion context
+ *
+ * Soft-IRQ context, polling connection CQ until
+ * either CQ was empty or we exausted polling budget
+ */
 static void iser_cq_tasklet_fn(unsigned long data)
 {
-	struct iser_cq_desc *cq_desc = (struct iser_cq_desc *)data;
-	struct iser_device  *device = cq_desc->device;
-	int cq_index = cq_desc->cq_index;
-	struct ib_cq	     *cq = device->rx_cq[cq_index];
-	 struct ib_wc	     wc;
-	 struct iser_rx_desc *desc;
-	 unsigned long	     xfer_len;
-	struct iser_conn *ib_conn;
-	int completed_tx, completed_rx = 0;
-
-	/* First do tx drain, so in a case where we have rx flushes and a successful
-	 * tx completion we will still go through completion error handling.
-	 */
-	completed_tx = iser_drain_tx_cq(device, cq_index);
-
-	while (ib_poll_cq(cq, 1, &wc) == 1) {
-		desc	 = (struct iser_rx_desc *) (unsigned long) wc.wr_id;
-		BUG_ON(desc == NULL);
-		ib_conn = wc.qp->qp_context;
-		if (wc.status == IB_WC_SUCCESS) {
-			if (wc.opcode == IB_WC_RECV) {
-				xfer_len = (unsigned long)wc.byte_len;
-				iser_rcv_completion(desc, xfer_len, ib_conn);
-			} else
-				iser_err("expected opcode %d got %d\n",
-					IB_WC_RECV, wc.opcode);
-		} else {
-			if (wc.status != IB_WC_WR_FLUSH_ERR)
-				iser_err("rx id %llx status %d vend_err %x\n",
-					wc.wr_id, wc.status, wc.vendor_err);
-			ib_conn->post_recv_buf_count--;
-			iser_handle_comp_error(NULL, ib_conn);
-		}
-		completed_rx++;
-		if (!(completed_rx & 63))
-			completed_tx += iser_drain_tx_cq(device, cq_index);
+	struct iser_comp *comp = (struct iser_comp *)data;
+	struct ib_cq *cq = comp->cq;
+	struct ib_wc *const wcs = comp->wcs;
+	int i, n, completed = 0;
+
+	while ((n = ib_poll_cq(cq, ARRAY_SIZE(comp->wcs), wcs)) > 0) {
+		for (i = 0; i < n; i++)
+			iser_handle_wc(&wcs[i]);
+
+		completed += n;
+		if (completed >= iser_cq_poll_limit)
+			break;
 	}
-	/* #warning "it is assumed here that arming CQ only once its empty" *
-	 * " would not cause interrupts to be missed"                       */
+
+	/*
+	 * It is assumed here that arming CQ only once its empty
+	 * would not cause interrupts to be missed.
+	 */
 	ib_req_notify_cq(cq, IB_CQ_NEXT_COMP);
 
-	iser_dbg("got %d rx %d tx completions\n", completed_rx, completed_tx);
+	iser_dbg("got %d completions\n", completed);
 }
 
 static void iser_cq_callback(struct ib_cq *cq, void *cq_context)
 {
-	struct iser_cq_desc *cq_desc = (struct iser_cq_desc *)cq_context;
-	struct iser_device  *device = cq_desc->device;
-	int cq_index = cq_desc->cq_index;
+	struct iser_comp *comp = cq_context;
 
-	tasklet_schedule(&device->cq_tasklet[cq_index]);
+	tasklet_schedule(&comp->tasklet);
 }
 
 u8 iser_check_task_pi_status(struct iscsi_iser_task *iser_task,
diff --git a/drivers/infiniband/ulp/isert/ib_isert.c b/drivers/infiniband/ulp/isert/ib_isert.c
index d4c7928a0f36..3effa931fce2 100644
--- a/drivers/infiniband/ulp/isert/ib_isert.c
+++ b/drivers/infiniband/ulp/isert/ib_isert.c
@@ -586,17 +586,12 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 	init_completion(&isert_conn->conn_wait);
 	init_completion(&isert_conn->conn_wait_comp_err);
 	kref_init(&isert_conn->conn_kref);
-	kref_get(&isert_conn->conn_kref);
 	mutex_init(&isert_conn->conn_mutex);
 	spin_lock_init(&isert_conn->conn_lock);
 	INIT_LIST_HEAD(&isert_conn->conn_fr_pool);
 
 	cma_id->context = isert_conn;
 	isert_conn->conn_cm_id = cma_id;
-	isert_conn->responder_resources = event->param.conn.responder_resources;
-	isert_conn->initiator_depth = event->param.conn.initiator_depth;
-	pr_debug("Using responder_resources: %u initiator_depth: %u\n",
-		 isert_conn->responder_resources, isert_conn->initiator_depth);
 
 	isert_conn->login_buf = kzalloc(ISCSI_DEF_MAX_RECV_SEG_LEN +
 					ISER_RX_LOGIN_SIZE, GFP_KERNEL);
@@ -643,6 +638,12 @@ isert_connect_request(struct rdma_cm_id *cma_id, struct rdma_cm_event *event)
 		goto out_rsp_dma_map;
 	}
 
+	/* Set max inflight RDMA READ requests */
+	isert_conn->initiator_depth = min_t(u8,
+				event->param.conn.initiator_depth,
+				device->dev_attr.max_qp_init_rd_atom);
+	pr_debug("Using initiator_depth: %u\n", isert_conn->initiator_depth);
+
 	isert_conn->conn_device = device;
 	isert_conn->conn_pd = ib_alloc_pd(isert_conn->conn_device->ib_device);
 	if (IS_ERR(isert_conn->conn_pd)) {
@@ -746,7 +747,9 @@ isert_connect_release(struct isert_conn *isert_conn)
 static void
 isert_connected_handler(struct rdma_cm_id *cma_id)
 {
-	return;
+	struct isert_conn *isert_conn = cma_id->context;
+
+	kref_get(&isert_conn->conn_kref);
 }
 
 static void
@@ -798,7 +801,6 @@ isert_disconnect_work(struct work_struct *work)
 
 wake_up:
 	complete(&isert_conn->conn_wait);
-	isert_put_conn(isert_conn);
 }
 
 static void
@@ -2183,7 +2185,7 @@ isert_put_response(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
 		isert_cmd->tx_desc.num_sge = 2;
 	}
 
-	isert_init_send_wr(isert_conn, isert_cmd, send_wr, true);
+	isert_init_send_wr(isert_conn, isert_cmd, send_wr, false);
 
 	pr_debug("Posting SCSI Response IB_WR_SEND >>>>>>>>>>>>>>>>>>>>>>\n");
 
@@ -2607,58 +2609,45 @@ isert_fast_reg_mr(struct isert_conn *isert_conn,
 	return ret;
 }
 
-static inline enum ib_t10_dif_type
-se2ib_prot_type(enum target_prot_type prot_type)
+static inline void
+isert_set_dif_domain(struct se_cmd *se_cmd, struct ib_sig_attrs *sig_attrs,
+		     struct ib_sig_domain *domain)
 {
-	switch (prot_type) {
-	case TARGET_DIF_TYPE0_PROT:
-		return IB_T10DIF_NONE;
-	case TARGET_DIF_TYPE1_PROT:
-		return IB_T10DIF_TYPE1;
-	case TARGET_DIF_TYPE2_PROT:
-		return IB_T10DIF_TYPE2;
-	case TARGET_DIF_TYPE3_PROT:
-		return IB_T10DIF_TYPE3;
-	default:
-		return IB_T10DIF_NONE;
-	}
-}
+	domain->sig_type = IB_SIG_TYPE_T10_DIF;
+	domain->sig.dif.bg_type = IB_T10DIF_CRC;
+	domain->sig.dif.pi_interval = se_cmd->se_dev->dev_attrib.block_size;
+	domain->sig.dif.ref_tag = se_cmd->reftag_seed;
+	/*
+	 * At the moment we hard code those, but if in the future
+	 * the target core would like to use it, we will take it
+	 * from se_cmd.
+	 */
+	domain->sig.dif.apptag_check_mask = 0xffff;
+	domain->sig.dif.app_escape = true;
+	domain->sig.dif.ref_escape = true;
+	if (se_cmd->prot_type == TARGET_DIF_TYPE1_PROT ||
+	    se_cmd->prot_type == TARGET_DIF_TYPE2_PROT)
+		domain->sig.dif.ref_remap = true;
+};
 
 static int
 isert_set_sig_attrs(struct se_cmd *se_cmd, struct ib_sig_attrs *sig_attrs)
 {
-	enum ib_t10_dif_type ib_prot_type = se2ib_prot_type(se_cmd->prot_type);
-
-	sig_attrs->mem.sig_type = IB_SIG_TYPE_T10_DIF;
-	sig_attrs->wire.sig_type = IB_SIG_TYPE_T10_DIF;
-	sig_attrs->mem.sig.dif.pi_interval =
-				se_cmd->se_dev->dev_attrib.block_size;
-	sig_attrs->wire.sig.dif.pi_interval =
-				se_cmd->se_dev->dev_attrib.block_size;
-
 	switch (se_cmd->prot_op) {
 	case TARGET_PROT_DIN_INSERT:
 	case TARGET_PROT_DOUT_STRIP:
-		sig_attrs->mem.sig.dif.type = IB_T10DIF_NONE;
-		sig_attrs->wire.sig.dif.type = ib_prot_type;
-		sig_attrs->wire.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->wire.sig.dif.ref_tag = se_cmd->reftag_seed;
+		sig_attrs->mem.sig_type = IB_SIG_TYPE_NONE;
+		isert_set_dif_domain(se_cmd, sig_attrs, &sig_attrs->wire);
 		break;
 	case TARGET_PROT_DOUT_INSERT:
 	case TARGET_PROT_DIN_STRIP:
-		sig_attrs->mem.sig.dif.type = ib_prot_type;
-		sig_attrs->mem.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->mem.sig.dif.ref_tag = se_cmd->reftag_seed;
-		sig_attrs->wire.sig.dif.type = IB_T10DIF_NONE;
+		sig_attrs->wire.sig_type = IB_SIG_TYPE_NONE;
+		isert_set_dif_domain(se_cmd, sig_attrs, &sig_attrs->mem);
 		break;
 	case TARGET_PROT_DIN_PASS:
 	case TARGET_PROT_DOUT_PASS:
-		sig_attrs->mem.sig.dif.type = ib_prot_type;
-		sig_attrs->mem.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->mem.sig.dif.ref_tag = se_cmd->reftag_seed;
-		sig_attrs->wire.sig.dif.type = ib_prot_type;
-		sig_attrs->wire.sig.dif.bg_type = IB_T10DIF_CRC;
-		sig_attrs->wire.sig.dif.ref_tag = se_cmd->reftag_seed;
+		isert_set_dif_domain(se_cmd, sig_attrs, &sig_attrs->wire);
+		isert_set_dif_domain(se_cmd, sig_attrs, &sig_attrs->mem);
 		break;
 	default:
 		pr_err("Unsupported PI operation %d\n", se_cmd->prot_op);
@@ -2882,7 +2871,7 @@ isert_put_datain(struct iscsi_conn *conn, struct iscsi_cmd *cmd)
 				     &isert_cmd->tx_desc.iscsi_header);
 		isert_init_tx_hdrs(isert_conn, &isert_cmd->tx_desc);
 		isert_init_send_wr(isert_conn, isert_cmd,
-				   &isert_cmd->tx_desc.send_wr, true);
+				   &isert_cmd->tx_desc.send_wr, false);
 		isert_cmd->rdma_wr.s_send_wr.next = &isert_cmd->tx_desc.send_wr;
 		wr->send_wr_num += 1;
 	}
@@ -3067,7 +3056,6 @@ isert_rdma_accept(struct isert_conn *isert_conn)
 	int ret;
 
 	memset(&cp, 0, sizeof(struct rdma_conn_param));
-	cp.responder_resources = isert_conn->responder_resources;
 	cp.initiator_depth = isert_conn->initiator_depth;
 	cp.retry_count = 7;
 	cp.rnr_retry_count = 7;
@@ -3152,7 +3140,7 @@ isert_accept_np(struct iscsi_np *np, struct iscsi_conn *conn)
 
 accept_wait:
 	ret = down_interruptible(&isert_np->np_sem);
-	if (max_accept > 5)
+	if (ret || max_accept > 5)
 		return -ENODEV;
 
 	spin_lock_bh(&np->np_thread_lock);
@@ -3215,7 +3203,7 @@ static void isert_wait_conn(struct iscsi_conn *conn)
 	pr_debug("isert_wait_conn: Starting \n");
 
 	mutex_lock(&isert_conn->conn_mutex);
-	if (isert_conn->conn_cm_id) {
+	if (isert_conn->conn_cm_id && !isert_conn->disconnect) {
 		pr_debug("Calling rdma_disconnect from isert_wait_conn\n");
 		rdma_disconnect(isert_conn->conn_cm_id);
 	}
@@ -3234,6 +3222,7 @@ static void isert_wait_conn(struct iscsi_conn *conn)
 	wait_for_completion(&isert_conn->conn_wait_comp_err);
 
 	wait_for_completion(&isert_conn->conn_wait);
+	isert_put_conn(isert_conn);
 }
 
 static void isert_free_conn(struct iscsi_conn *conn)
diff --git a/drivers/infiniband/ulp/srpt/ib_srpt.c b/drivers/infiniband/ulp/srpt/ib_srpt.c
index d28a8c284da9..7206547c13ce 100644
--- a/drivers/infiniband/ulp/srpt/ib_srpt.c
+++ b/drivers/infiniband/ulp/srpt/ib_srpt.c
@@ -3574,7 +3574,7 @@ static int srpt_parse_i_port_id(u8 i_port_id[16], const char *name)
 	int ret, rc;
 
 	p = name;
-	if (strnicmp(p, "0x", 2) == 0)
+	if (strncasecmp(p, "0x", 2) == 0)
 		p += 2;
 	ret = -EINVAL;
 	len = strlen(p);
diff --git a/drivers/input/evdev.c b/drivers/input/evdev.c
index de055451d1af..bc203485716d 100644
--- a/drivers/input/evdev.c
+++ b/drivers/input/evdev.c
@@ -738,20 +738,23 @@ static int evdev_handle_set_keycode_v2(struct input_dev *dev, void __user *p)
  */
 static int evdev_handle_get_val(struct evdev_client *client,
 				struct input_dev *dev, unsigned int type,
-				unsigned long *bits, unsigned int max,
-				unsigned int size, void __user *p, int compat)
+				unsigned long *bits, unsigned int maxbit,
+				unsigned int maxlen, void __user *p,
+				int compat)
 {
 	int ret;
 	unsigned long *mem;
+	size_t len;
 
-	mem = kmalloc(sizeof(unsigned long) * max, GFP_KERNEL);
+	len = BITS_TO_LONGS(maxbit) * sizeof(unsigned long);
+	mem = kmalloc(len, GFP_KERNEL);
 	if (!mem)
 		return -ENOMEM;
 
 	spin_lock_irq(&dev->event_lock);
 	spin_lock(&client->buffer_lock);
 
-	memcpy(mem, bits, sizeof(unsigned long) * max);
+	memcpy(mem, bits, len);
 
 	spin_unlock(&dev->event_lock);
 
@@ -759,7 +762,7 @@ static int evdev_handle_get_val(struct evdev_client *client,
 
 	spin_unlock_irq(&client->buffer_lock);
 
-	ret = bits_to_user(mem, max, size, p, compat);
+	ret = bits_to_user(mem, maxbit, maxlen, p, compat);
 	if (ret < 0)
 		evdev_queue_syn_dropped(client);
 
diff --git a/drivers/input/gameport/gameport.c b/drivers/input/gameport/gameport.c
index 24c41ba7d4e0..e29c04e2aff4 100644
--- a/drivers/input/gameport/gameport.c
+++ b/drivers/input/gameport/gameport.c
@@ -23,6 +23,7 @@
 #include <linux/workqueue.h>
 #include <linux/sched.h>	/* HZ */
 #include <linux/mutex.h>
+#include <linux/timekeeping.h>
 
 /*#include <asm/io.h>*/
 
@@ -30,6 +31,10 @@ MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION("Generic gameport layer");
 MODULE_LICENSE("GPL");
 
+static bool use_ktime = true;
+module_param(use_ktime, bool, 0400);
+MODULE_PARM_DESC(use_ktime, "Use ktime for measuring I/O speed");
+
 /*
  * gameport_mutex protects entire gameport subsystem and is taken
  * every time gameport port or driver registrered or unregistered.
@@ -76,6 +81,38 @@ static unsigned int get_time_pit(void)
 
 static int gameport_measure_speed(struct gameport *gameport)
 {
+	unsigned int i, t, tx;
+	u64 t1, t2, t3;
+	unsigned long flags;
+
+	if (gameport_open(gameport, NULL, GAMEPORT_MODE_RAW))
+		return 0;
+
+	tx = ~0;
+
+	for (i = 0; i < 50; i++) {
+		local_irq_save(flags);
+		t1 = ktime_get_ns();
+		for (t = 0; t < 50; t++)
+			gameport_read(gameport);
+		t2 = ktime_get_ns();
+		t3 = ktime_get_ns();
+		local_irq_restore(flags);
+		udelay(i * 10);
+		t = (t2 - t1) - (t3 - t2);
+		if (t < tx)
+			tx = t;
+	}
+
+	gameport_close(gameport);
+	t = 1000000 * 50;
+	if (tx)
+		t /= tx;
+	return t;
+}
+
+static int old_gameport_measure_speed(struct gameport *gameport)
+{
 #if defined(__i386__)
 
 	unsigned int i, t, t1, t2, t3, tx;
@@ -521,7 +558,9 @@ static void gameport_add_port(struct gameport *gameport)
 	if (gameport->parent)
 		gameport->parent->child = gameport;
 
-	gameport->speed = gameport_measure_speed(gameport);
+	gameport->speed = use_ktime ?
+		gameport_measure_speed(gameport) :
+		old_gameport_measure_speed(gameport);
 
 	list_add_tail(&gameport->node, &gameport_list);
 
diff --git a/drivers/input/input-mt.c b/drivers/input/input-mt.c
index c30204f2fa30..fbe29fcb15c5 100644
--- a/drivers/input/input-mt.c
+++ b/drivers/input/input-mt.c
@@ -236,6 +236,18 @@ void input_mt_report_pointer_emulation(struct input_dev *dev, bool use_count)
 }
 EXPORT_SYMBOL(input_mt_report_pointer_emulation);
 
+static void __input_mt_drop_unused(struct input_dev *dev, struct input_mt *mt)
+{
+	int i;
+
+	for (i = 0; i < mt->num_slots; i++) {
+		if (!input_mt_is_used(mt, &mt->slots[i])) {
+			input_mt_slot(dev, i);
+			input_event(dev, EV_ABS, ABS_MT_TRACKING_ID, -1);
+		}
+	}
+}
+
 /**
  * input_mt_drop_unused() - Inactivate slots not seen in this frame
  * @dev: input device with allocated MT slots
@@ -245,19 +257,11 @@ EXPORT_SYMBOL(input_mt_report_pointer_emulation);
 void input_mt_drop_unused(struct input_dev *dev)
 {
 	struct input_mt *mt = dev->mt;
-	int i;
 
-	if (!mt)
-		return;
-
-	for (i = 0; i < mt->num_slots; i++) {
-		if (!input_mt_is_used(mt, &mt->slots[i])) {
-			input_mt_slot(dev, i);
-			input_event(dev, EV_ABS, ABS_MT_TRACKING_ID, -1);
-		}
+	if (mt) {
+		__input_mt_drop_unused(dev, mt);
+		mt->frame++;
 	}
-
-	mt->frame++;
 }
 EXPORT_SYMBOL(input_mt_drop_unused);
 
@@ -278,12 +282,14 @@ void input_mt_sync_frame(struct input_dev *dev)
 		return;
 
 	if (mt->flags & INPUT_MT_DROP_UNUSED)
-		input_mt_drop_unused(dev);
+		__input_mt_drop_unused(dev, mt);
 
 	if ((mt->flags & INPUT_MT_POINTER) && !(mt->flags & INPUT_MT_SEMI_MT))
 		use_count = true;
 
 	input_mt_report_pointer_emulation(dev, use_count);
+
+	mt->frame++;
 }
 EXPORT_SYMBOL(input_mt_sync_frame);
 
diff --git a/drivers/input/input.c b/drivers/input/input.c
index 29ca0bb4f561..0f175f55782b 100644
--- a/drivers/input/input.c
+++ b/drivers/input/input.c
@@ -498,7 +498,8 @@ void input_set_abs_params(struct input_dev *dev, unsigned int axis,
 	absinfo->fuzz = fuzz;
 	absinfo->flat = flat;
 
-	dev->absbit[BIT_WORD(axis)] |= BIT_MASK(axis);
+	__set_bit(EV_ABS, dev->evbit);
+	__set_bit(axis, dev->absbit);
 }
 EXPORT_SYMBOL(input_set_abs_params);
 
@@ -1788,7 +1789,7 @@ struct input_dev *input_allocate_device(void)
 		INIT_LIST_HEAD(&dev->h_list);
 		INIT_LIST_HEAD(&dev->node);
 
-		dev_set_name(&dev->dev, "input%ld",
+		dev_set_name(&dev->dev, "input%lu",
 			     (unsigned long) atomic_inc_return(&input_no) - 1);
 
 		__module_get(THIS_MODULE);
diff --git a/drivers/input/joystick/analog.c b/drivers/input/joystick/analog.c
index ab0fdcd36e18..4284080e481d 100644
--- a/drivers/input/joystick/analog.c
+++ b/drivers/input/joystick/analog.c
@@ -36,6 +36,7 @@
 #include <linux/gameport.h>
 #include <linux/jiffies.h>
 #include <linux/timex.h>
+#include <linux/timekeeping.h>
 
 #define DRIVER_DESC	"Analog joystick and gamepad driver"
 
@@ -43,6 +44,10 @@ MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 
+static bool use_ktime = true;
+module_param(use_ktime, bool, 0400);
+MODULE_PARM_DESC(use_ktime, "Use ktime for measuring I/O speed");
+
 /*
  * Option parsing.
  */
@@ -171,6 +176,25 @@ static unsigned long analog_faketime = 0;
 #warning Precise timer not defined for this architecture.
 #endif
 
+static inline u64 get_time(void)
+{
+	if (use_ktime) {
+		return ktime_get_ns();
+	} else {
+		unsigned int x;
+		GET_TIME(x);
+		return x;
+	}
+}
+
+static inline unsigned int delta(u64 x, u64 y)
+{
+	if (use_ktime)
+		return y - x;
+	else
+		return DELTA((unsigned int)x, (unsigned int)y);
+}
+
 /*
  * analog_decode() decodes analog joystick data and reports input events.
  */
@@ -226,7 +250,8 @@ static void analog_decode(struct analog *analog, int *axes, int *initial, int bu
 static int analog_cooked_read(struct analog_port *port)
 {
 	struct gameport *gameport = port->gameport;
-	unsigned int time[4], start, loop, now, loopout, timeout;
+	u64 time[4], start, loop, now;
+	unsigned int loopout, timeout;
 	unsigned char data[4], this, last;
 	unsigned long flags;
 	int i, j;
@@ -236,7 +261,7 @@ static int analog_cooked_read(struct analog_port *port)
 
 	local_irq_save(flags);
 	gameport_trigger(gameport);
-	GET_TIME(now);
+	now = get_time();
 	local_irq_restore(flags);
 
 	start = now;
@@ -249,16 +274,16 @@ static int analog_cooked_read(struct analog_port *port)
 
 		local_irq_disable();
 		this = gameport_read(gameport) & port->mask;
-		GET_TIME(now);
+		now = get_time();
 		local_irq_restore(flags);
 
-		if ((last ^ this) && (DELTA(loop, now) < loopout)) {
+		if ((last ^ this) && (delta(loop, now) < loopout)) {
 			data[i] = last ^ this;
 			time[i] = now;
 			i++;
 		}
 
-	} while (this && (i < 4) && (DELTA(start, now) < timeout));
+	} while (this && (i < 4) && (delta(start, now) < timeout));
 
 	this <<= 4;
 
@@ -266,7 +291,7 @@ static int analog_cooked_read(struct analog_port *port)
 		this |= data[i];
 		for (j = 0; j < 4; j++)
 			if (data[i] & (1 << j))
-				port->axes[j] = (DELTA(start, time[i]) << ANALOG_FUZZ_BITS) / port->loop;
+				port->axes[j] = (delta(start, time[i]) << ANALOG_FUZZ_BITS) / port->loop;
 	}
 
 	return -(this != port->mask);
@@ -365,31 +390,39 @@ static void analog_close(struct input_dev *dev)
 static void analog_calibrate_timer(struct analog_port *port)
 {
 	struct gameport *gameport = port->gameport;
-	unsigned int i, t, tx, t1, t2, t3;
+	unsigned int i, t, tx;
+	u64 t1, t2, t3;
 	unsigned long flags;
 
-	local_irq_save(flags);
-	GET_TIME(t1);
+	if (use_ktime) {
+		port->speed = 1000000;
+	} else {
+		local_irq_save(flags);
+		t1 = get_time();
 #ifdef FAKE_TIME
-	analog_faketime += 830;
+		analog_faketime += 830;
 #endif
-	mdelay(1);
-	GET_TIME(t2);
-	GET_TIME(t3);
-	local_irq_restore(flags);
+		mdelay(1);
+		t2 = get_time();
+		t3 = get_time();
+		local_irq_restore(flags);
 
-	port->speed = DELTA(t1, t2) - DELTA(t2, t3);
+		port->speed = delta(t1, t2) - delta(t2, t3);
+	}
 
 	tx = ~0;
 
 	for (i = 0; i < 50; i++) {
 		local_irq_save(flags);
-		GET_TIME(t1);
-		for (t = 0; t < 50; t++) { gameport_read(gameport); GET_TIME(t2); }
-		GET_TIME(t3);
+		t1 = get_time();
+		for (t = 0; t < 50; t++) {
+			gameport_read(gameport);
+			t2 = get_time();
+		}
+		t3 = get_time();
 		local_irq_restore(flags);
 		udelay(i);
-		t = DELTA(t1, t2) - DELTA(t2, t3);
+		t = delta(t1, t2) - delta(t2, t3);
 		if (t < tx) tx = t;
 	}
 
diff --git a/drivers/input/joystick/xpad.c b/drivers/input/joystick/xpad.c
index 177602cf7079..2ed7905a068f 100644
--- a/drivers/input/joystick/xpad.c
+++ b/drivers/input/joystick/xpad.c
@@ -126,7 +126,9 @@ static const struct xpad_device {
 	{ 0x045e, 0x0291, "Xbox 360 Wireless Receiver (XBOX)", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX360W },
 	{ 0x045e, 0x0719, "Xbox 360 Wireless Receiver", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX360W },
 	{ 0x044f, 0x0f07, "Thrustmaster, Inc. Controller", 0, XTYPE_XBOX },
+	{ 0x044f, 0xb326, "Thrustmaster Gamepad GP XID", 0, XTYPE_XBOX360 },
 	{ 0x046d, 0xc21d, "Logitech Gamepad F310", 0, XTYPE_XBOX360 },
+	{ 0x046d, 0xc21e, "Logitech Gamepad F510", 0, XTYPE_XBOX360 },
 	{ 0x046d, 0xc21f, "Logitech Gamepad F710", 0, XTYPE_XBOX360 },
 	{ 0x046d, 0xc242, "Logitech Chillstream Controller", 0, XTYPE_XBOX360 },
 	{ 0x046d, 0xca84, "Logitech Xbox Cordless Controller", 0, XTYPE_XBOX },
@@ -140,10 +142,17 @@ static const struct xpad_device {
 	{ 0x0738, 0x4540, "Mad Catz Beat Pad", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX },
 	{ 0x0738, 0x4556, "Mad Catz Lynx Wireless Controller", 0, XTYPE_XBOX },
 	{ 0x0738, 0x4716, "Mad Catz Wired Xbox 360 Controller", 0, XTYPE_XBOX360 },
+	{ 0x0738, 0x4718, "Mad Catz Street Fighter IV FightStick SE", 0, XTYPE_XBOX360 },
+	{ 0x0738, 0x4726, "Mad Catz Xbox 360 Controller", 0, XTYPE_XBOX360 },
 	{ 0x0738, 0x4728, "Mad Catz Street Fighter IV FightPad", MAP_TRIGGERS_TO_BUTTONS, XTYPE_XBOX360 },
 	{ 0x0738, 0x4738, "Mad Catz Wired Xbox 360 Controller (SFIV)", MAP_TRIGGERS_TO_BUTTONS, XTYPE_XBOX360 },
+	{ 0x0738, 0x4740, "Mad Catz Beat Pad", 0, XTYPE_XBOX360 },
 	{ 0x0738, 0x6040, "Mad Catz Beat Pad Pro", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX },
+	{ 0x0738, 0xb726, "Mad Catz Xbox controller - MW2", 0, XTYPE_XBOX360 },
 	{ 0x0738, 0xbeef, "Mad Catz JOYTECH NEO SE Advanced GamePad", XTYPE_XBOX360 },
+	{ 0x0738, 0xcb02, "Saitek Cyborg Rumble Pad - PC/Xbox 360", 0, XTYPE_XBOX360 },
+	{ 0x0738, 0xcb03, "Saitek P3200 Rumble Pad - PC/Xbox 360", 0, XTYPE_XBOX360 },
+	{ 0x0738, 0xf738, "Super SFIV FightStick TE S", 0, XTYPE_XBOX360 },
 	{ 0x0c12, 0x8802, "Zeroplus Xbox Controller", 0, XTYPE_XBOX },
 	{ 0x0c12, 0x8809, "RedOctane Xbox Dance Pad", DANCEPAD_MAP_CONFIG, XTYPE_XBOX },
 	{ 0x0c12, 0x880a, "Pelican Eclipse PL-2023", 0, XTYPE_XBOX },
@@ -156,28 +165,51 @@ static const struct xpad_device {
 	{ 0x0e6f, 0x0005, "Eclipse wireless Controller", 0, XTYPE_XBOX },
 	{ 0x0e6f, 0x0006, "Edge wireless Controller", 0, XTYPE_XBOX },
 	{ 0x0e6f, 0x0105, "HSM3 Xbox360 dancepad", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX360 },
+	{ 0x0e6f, 0x0113, "Afterglow AX.1 Gamepad for Xbox 360", 0, XTYPE_XBOX360 },
 	{ 0x0e6f, 0x0201, "Pelican PL-3601 'TSZ' Wired Xbox 360 Controller", 0, XTYPE_XBOX360 },
 	{ 0x0e6f, 0x0213, "Afterglow Gamepad for Xbox 360", 0, XTYPE_XBOX360 },
+	{ 0x0e6f, 0x021f, "Rock Candy Gamepad for Xbox 360", 0, XTYPE_XBOX360 },
+	{ 0x0e6f, 0x0301, "Logic3 Controller", 0, XTYPE_XBOX360 },
+	{ 0x0e6f, 0x0401, "Logic3 Controller", 0, XTYPE_XBOX360 },
 	{ 0x0e8f, 0x0201, "SmartJoy Frag Xpad/PS2 adaptor", 0, XTYPE_XBOX },
+	{ 0x0e8f, 0x3008, "Generic xbox control (dealextreme)", 0, XTYPE_XBOX },
+	{ 0x0f0d, 0x000a, "Hori Co. DOA4 FightStick", 0, XTYPE_XBOX360 },
 	{ 0x0f0d, 0x000d, "Hori Fighting Stick EX2", MAP_TRIGGERS_TO_BUTTONS, XTYPE_XBOX360 },
 	{ 0x0f0d, 0x0016, "Hori Real Arcade Pro.EX", MAP_TRIGGERS_TO_BUTTONS, XTYPE_XBOX360 },
 	{ 0x0f30, 0x0202, "Joytech Advanced Controller", 0, XTYPE_XBOX },
 	{ 0x0f30, 0x8888, "BigBen XBMiniPad Controller", 0, XTYPE_XBOX },
 	{ 0x102c, 0xff0c, "Joytech Wireless Advanced Controller", 0, XTYPE_XBOX },
 	{ 0x12ab, 0x0004, "Honey Bee Xbox360 dancepad", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX360 },
+	{ 0x12ab, 0x0301, "PDP AFTERGLOW AX.1", 0, XTYPE_XBOX360 },
 	{ 0x12ab, 0x8809, "Xbox DDR dancepad", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX },
 	{ 0x1430, 0x4748, "RedOctane Guitar Hero X-plorer", 0, XTYPE_XBOX360 },
 	{ 0x1430, 0x8888, "TX6500+ Dance Pad (first generation)", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX },
 	{ 0x146b, 0x0601, "BigBen Interactive XBOX 360 Controller", 0, XTYPE_XBOX360 },
+	{ 0x1532, 0x0037, "Razer Sabertooth", 0, XTYPE_XBOX360 },
+	{ 0x15e4, 0x3f00, "Power A Mini Pro Elite", 0, XTYPE_XBOX360 },
+	{ 0x15e4, 0x3f0a, "Xbox Airflo wired controller", 0, XTYPE_XBOX360 },
+	{ 0x15e4, 0x3f10, "Batarang Xbox 360 controller", 0, XTYPE_XBOX360 },
+	{ 0x162e, 0xbeef, "Joytech Neo-Se Take2", 0, XTYPE_XBOX360 },
 	{ 0x1689, 0xfd00, "Razer Onza Tournament Edition", 0, XTYPE_XBOX360 },
 	{ 0x1689, 0xfd01, "Razer Onza Classic Edition", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5d04, "Razer Sabertooth", 0, XTYPE_XBOX360 },
 	{ 0x1bad, 0x0002, "Harmonix Rock Band Guitar", 0, XTYPE_XBOX360 },
 	{ 0x1bad, 0x0003, "Harmonix Rock Band Drumkit", MAP_DPAD_TO_BUTTONS, XTYPE_XBOX360 },
 	{ 0x1bad, 0xf016, "Mad Catz Xbox 360 Controller", 0, XTYPE_XBOX360 },
+	{ 0x1bad, 0xf023, "MLG Pro Circuit Controller (Xbox)", 0, XTYPE_XBOX360 },
 	{ 0x1bad, 0xf028, "Street Fighter IV FightPad", 0, XTYPE_XBOX360 },
+	{ 0x1bad, 0xf038, "Street Fighter IV FightStick TE", 0, XTYPE_XBOX360 },
+	{ 0x1bad, 0xf900, "Harmonix Xbox 360 Controller", 0, XTYPE_XBOX360 },
 	{ 0x1bad, 0xf901, "Gamestop Xbox 360 Controller", 0, XTYPE_XBOX360 },
 	{ 0x1bad, 0xf903, "Tron Xbox 360 controller", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5000, "Razer Atrox Arcade Stick", 0, XTYPE_XBOX360 },
 	{ 0x24c6, 0x5300, "PowerA MINI PROEX Controller", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5303, "Xbox Airflo wired controller", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5500, "Hori XBOX 360 EX 2 with Turbo", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5501, "Hori Real Arcade Pro VX-SA", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5506, "Hori SOULCALIBUR V Stick", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5b02, "Thrustmaster, Inc. GPX Controller", 0, XTYPE_XBOX360 },
+	{ 0x24c6, 0x5b03, "Thrustmaster Ferrari 458 Racing Wheel", 0, XTYPE_XBOX360 },
 	{ 0xffff, 0xffff, "Chinese-made Xbox Controller", 0, XTYPE_XBOX },
 	{ 0x0000, 0x0000, "Generic X-Box pad", 0, XTYPE_UNKNOWN }
 };
@@ -261,6 +293,7 @@ static const signed short xpad_abs_triggers[] = {
 
 static struct usb_device_id xpad_table[] = {
 	{ USB_INTERFACE_INFO('X', 'B', 0) },	/* X-Box USB-IF not approved class */
+	XPAD_XBOX360_VENDOR(0x044f),		/* Thrustmaster X-Box 360 controllers */
 	XPAD_XBOX360_VENDOR(0x045e),		/* Microsoft X-Box 360 controllers */
 	XPAD_XBOXONE_VENDOR(0x045e),		/* Microsoft X-Box One controllers */
 	XPAD_XBOX360_VENDOR(0x046d),		/* Logitech X-Box 360 style controllers */
@@ -274,6 +307,9 @@ static struct usb_device_id xpad_table[] = {
 	XPAD_XBOX360_VENDOR(0x0f0d),		/* Hori Controllers */
 	XPAD_XBOX360_VENDOR(0x1689),		/* Razer Onza */
 	XPAD_XBOX360_VENDOR(0x24c6),		/* PowerA Controllers */
+	XPAD_XBOX360_VENDOR(0x1532),		/* Razer Sabertooth */
+	XPAD_XBOX360_VENDOR(0x15e4),		/* Numark X-Box 360 controllers */
+	XPAD_XBOX360_VENDOR(0x162e),		/* Joytech X-Box 360 controllers */
 	{ }
 };
 
diff --git a/drivers/input/keyboard/adp5588-keys.c b/drivers/input/keyboard/adp5588-keys.c
index 5ef7fcf0e250..21a62d0fa764 100644
--- a/drivers/input/keyboard/adp5588-keys.c
+++ b/drivers/input/keyboard/adp5588-keys.c
@@ -251,9 +251,7 @@ static void adp5588_gpio_remove(struct adp5588_kpad *kpad)
 			dev_warn(dev, "teardown failed %d\n", error);
 	}
 
-	error = gpiochip_remove(&kpad->gc);
-	if (error)
-		dev_warn(dev, "gpiochip_remove failed %d\n", error);
+	gpiochip_remove(&kpad->gc);
 }
 #else
 static inline int adp5588_gpio_add(struct adp5588_kpad *kpad)
@@ -589,6 +587,7 @@ static int adp5588_probe(struct i2c_client *client,
 
  err_free_irq:
 	free_irq(client->irq, kpad);
+	cancel_delayed_work_sync(&kpad->work);
  err_unreg_dev:
 	input_unregister_device(input);
 	input = NULL;
diff --git a/drivers/input/keyboard/adp5589-keys.c b/drivers/input/keyboard/adp5589-keys.c
index 6329549bf6ad..a45267729dfc 100644
--- a/drivers/input/keyboard/adp5589-keys.c
+++ b/drivers/input/keyboard/adp5589-keys.c
@@ -567,9 +567,7 @@ static void adp5589_gpio_remove(struct adp5589_kpad *kpad)
 			dev_warn(dev, "teardown failed %d\n", error);
 	}
 
-	error = gpiochip_remove(&kpad->gc);
-	if (error)
-		dev_warn(dev, "gpiochip_remove failed %d\n", error);
+	gpiochip_remove(&kpad->gc);
 }
 #else
 static inline int adp5589_gpio_add(struct adp5589_kpad *kpad)
diff --git a/drivers/input/keyboard/atkbd.c b/drivers/input/keyboard/atkbd.c
index 2dd1d0dd4f7d..6f5d79569136 100644
--- a/drivers/input/keyboard/atkbd.c
+++ b/drivers/input/keyboard/atkbd.c
@@ -1791,14 +1791,6 @@ static const struct dmi_system_id atkbd_dmi_quirk_table[] __initconst = {
 	{
 		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "LG Electronics"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "LW25-B7HV"),
-		},
-		.callback = atkbd_deactivate_fixup,
-	},
-	{
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LG Electronics"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "P1-J273B"),
 		},
 		.callback = atkbd_deactivate_fixup,
 	},
diff --git a/drivers/input/keyboard/cap1106.c b/drivers/input/keyboard/cap1106.c
index 180b184ab90f..d70b65a14ced 100644
--- a/drivers/input/keyboard/cap1106.c
+++ b/drivers/input/keyboard/cap1106.c
@@ -33,8 +33,8 @@
 #define CAP1106_REG_SENSOR_CONFIG	0x22
 #define CAP1106_REG_SENSOR_CONFIG2	0x23
 #define CAP1106_REG_SAMPLING_CONFIG	0x24
-#define CAP1106_REG_CALIBRATION		0x25
-#define CAP1106_REG_INT_ENABLE		0x26
+#define CAP1106_REG_CALIBRATION		0x26
+#define CAP1106_REG_INT_ENABLE		0x27
 #define CAP1106_REG_REPEAT_RATE		0x28
 #define CAP1106_REG_MT_CONFIG		0x2a
 #define CAP1106_REG_MT_PATTERN_CONFIG	0x2b
diff --git a/drivers/input/keyboard/cros_ec_keyb.c b/drivers/input/keyboard/cros_ec_keyb.c
index 791781ade4e7..ffa989f2c785 100644
--- a/drivers/input/keyboard/cros_ec_keyb.c
+++ b/drivers/input/keyboard/cros_ec_keyb.c
@@ -22,6 +22,7 @@
  */
 
 #include <linux/module.h>
+#include <linux/bitops.h>
 #include <linux/i2c.h>
 #include <linux/input.h>
 #include <linux/interrupt.h>
@@ -38,6 +39,7 @@
  * @row_shift: log2 or number of rows, rounded up
  * @keymap_data: Matrix keymap data used to convert to keyscan values
  * @ghost_filter: true to enable the matrix key-ghosting filter
+ * @valid_keys: bitmap of existing keys for each matrix column
  * @old_kb_state: bitmap of keys pressed last scan
  * @dev: Device pointer
  * @idev: Input device
@@ -49,6 +51,7 @@ struct cros_ec_keyb {
 	int row_shift;
 	const struct matrix_keymap_data *keymap_data;
 	bool ghost_filter;
+	uint8_t *valid_keys;
 	uint8_t *old_kb_state;
 
 	struct device *dev;
@@ -57,39 +60,15 @@ struct cros_ec_keyb {
 };
 
 
-static bool cros_ec_keyb_row_has_ghosting(struct cros_ec_keyb *ckdev,
-					  uint8_t *buf, int row)
-{
-	int pressed_in_row = 0;
-	int row_has_teeth = 0;
-	int col, mask;
-
-	mask = 1 << row;
-	for (col = 0; col < ckdev->cols; col++) {
-		if (buf[col] & mask) {
-			pressed_in_row++;
-			row_has_teeth |= buf[col] & ~mask;
-			if (pressed_in_row > 1 && row_has_teeth) {
-				/* ghosting */
-				dev_dbg(ckdev->dev,
-					"ghost found at: r%d c%d, pressed %d, teeth 0x%x\n",
-					row, col, pressed_in_row,
-					row_has_teeth);
-				return true;
-			}
-		}
-	}
-
-	return false;
-}
-
 /*
  * Returns true when there is at least one combination of pressed keys that
  * results in ghosting.
  */
 static bool cros_ec_keyb_has_ghosting(struct cros_ec_keyb *ckdev, uint8_t *buf)
 {
-	int row;
+	int col1, col2, buf1, buf2;
+	struct device *dev = ckdev->dev;
+	uint8_t *valid_keys = ckdev->valid_keys;
 
 	/*
 	 * Ghosting happens if for any pressed key X there are other keys
@@ -103,27 +82,23 @@ static bool cros_ec_keyb_has_ghosting(struct cros_ec_keyb *ckdev, uint8_t *buf)
 	 *
 	 * In this case only X, Y, and Z are pressed, but g appears to be
 	 * pressed too (see Wikipedia).
-	 *
-	 * We can detect ghosting in a single pass (*) over the keyboard state
-	 * by maintaining two arrays.  pressed_in_row counts how many pressed
-	 * keys we have found in a row.  row_has_teeth is true if any of the
-	 * pressed keys for this row has other pressed keys in its column.  If
-	 * at any point of the scan we find that a row has multiple pressed
-	 * keys, and at least one of them is at the intersection with a column
-	 * with multiple pressed keys, we're sure there is ghosting.
-	 * Conversely, if there is ghosting, we will detect such situation for
-	 * at least one key during the pass.
-	 *
-	 * (*) This looks linear in the number of keys, but it's not.  We can
-	 * cheat because the number of rows is small.
 	 */
-	for (row = 0; row < ckdev->rows; row++)
-		if (cros_ec_keyb_row_has_ghosting(ckdev, buf, row))
-			return true;
+	for (col1 = 0; col1 < ckdev->cols; col1++) {
+		buf1 = buf[col1] & valid_keys[col1];
+		for (col2 = col1 + 1; col2 < ckdev->cols; col2++) {
+			buf2 = buf[col2] & valid_keys[col2];
+			if (hweight8(buf1 & buf2) > 1) {
+				dev_dbg(dev, "ghost found at: B[%02d]:0x%02x & B[%02d]:0x%02x",
+					col1, buf1, col2, buf2);
+				return true;
+			}
+		}
+	}
 
 	return false;
 }
 
+
 /*
  * Compares the new keyboard state to the old one and produces key
  * press/release events accordingly.  The keyboard state is 13 bytes (one byte
@@ -182,7 +157,7 @@ static int cros_ec_keyb_get_state(struct cros_ec_keyb *ckdev, uint8_t *kb_state)
 		.insize = ckdev->cols,
 	};
 
-	return ckdev->ec->cmd_xfer(ckdev->ec, &msg);
+	return cros_ec_cmd_xfer(ckdev->ec, &msg);
 }
 
 static irqreturn_t cros_ec_keyb_irq(int irq, void *data)
@@ -222,6 +197,30 @@ static void cros_ec_keyb_close(struct input_dev *dev)
 	free_irq(ec->irq, ckdev);
 }
 
+/*
+ * Walks keycodes flipping bit in buffer COLUMNS deep where bit is ROW.  Used by
+ * ghosting logic to ignore NULL or virtual keys.
+ */
+static void cros_ec_keyb_compute_valid_keys(struct cros_ec_keyb *ckdev)
+{
+	int row, col;
+	int row_shift = ckdev->row_shift;
+	unsigned short *keymap = ckdev->idev->keycode;
+	unsigned short code;
+
+	BUG_ON(ckdev->idev->keycodesize != sizeof(*keymap));
+
+	for (col = 0; col < ckdev->cols; col++) {
+		for (row = 0; row < ckdev->rows; row++) {
+			code = keymap[MATRIX_SCAN_CODE(row, col, row_shift)];
+			if (code && (code != KEY_BATTERY))
+				ckdev->valid_keys[col] |= 1 << row;
+		}
+		dev_dbg(ckdev->dev, "valid_keys[%02d] = 0x%02x\n",
+			col, ckdev->valid_keys[col]);
+	}
+}
+
 static int cros_ec_keyb_probe(struct platform_device *pdev)
 {
 	struct cros_ec_device *ec = dev_get_drvdata(pdev->dev.parent);
@@ -242,6 +241,11 @@ static int cros_ec_keyb_probe(struct platform_device *pdev)
 					    &ckdev->cols);
 	if (err)
 		return err;
+
+	ckdev->valid_keys = devm_kzalloc(&pdev->dev, ckdev->cols, GFP_KERNEL);
+	if (!ckdev->valid_keys)
+		return -ENOMEM;
+
 	ckdev->old_kb_state = devm_kzalloc(&pdev->dev, ckdev->cols, GFP_KERNEL);
 	if (!ckdev->old_kb_state)
 		return -ENOMEM;
@@ -285,6 +289,8 @@ static int cros_ec_keyb_probe(struct platform_device *pdev)
 	input_set_capability(idev, EV_MSC, MSC_SCAN);
 	input_set_drvdata(idev, ckdev);
 	ckdev->idev = idev;
+	cros_ec_keyb_compute_valid_keys(ckdev);
+
 	err = input_register_device(ckdev->idev);
 	if (err) {
 		dev_err(dev, "cannot register input device\n");
@@ -342,10 +348,19 @@ static int cros_ec_keyb_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(cros_ec_keyb_pm_ops, NULL, cros_ec_keyb_resume);
 
+#ifdef CONFIG_OF
+static const struct of_device_id cros_ec_keyb_of_match[] = {
+	{ .compatible = "google,cros-ec-keyb" },
+	{},
+};
+MODULE_DEVICE_TABLE(of, cros_ec_keyb_of_match);
+#endif
+
 static struct platform_driver cros_ec_keyb_driver = {
 	.probe = cros_ec_keyb_probe,
 	.driver = {
 		.name = "cros-ec-keyb",
+		.of_match_table = of_match_ptr(cros_ec_keyb_of_match),
 		.pm	= &cros_ec_keyb_pm_ops,
 	},
 };
diff --git a/drivers/input/keyboard/matrix_keypad.c b/drivers/input/keyboard/matrix_keypad.c
index 8d2e19e81e1e..e651fa692afe 100644
--- a/drivers/input/keyboard/matrix_keypad.c
+++ b/drivers/input/keyboard/matrix_keypad.c
@@ -332,23 +332,24 @@ static int matrix_keypad_init_gpio(struct platform_device *pdev,
 	}
 
 	if (pdata->clustered_irq > 0) {
-		err = request_irq(pdata->clustered_irq,
+		err = request_any_context_irq(pdata->clustered_irq,
 				matrix_keypad_interrupt,
 				pdata->clustered_irq_flags,
 				"matrix-keypad", keypad);
-		if (err) {
+		if (err < 0) {
 			dev_err(&pdev->dev,
 				"Unable to acquire clustered interrupt\n");
 			goto err_free_rows;
 		}
 	} else {
 		for (i = 0; i < pdata->num_row_gpios; i++) {
-			err = request_irq(gpio_to_irq(pdata->row_gpios[i]),
+			err = request_any_context_irq(
+					gpio_to_irq(pdata->row_gpios[i]),
 					matrix_keypad_interrupt,
 					IRQF_TRIGGER_RISING |
 					IRQF_TRIGGER_FALLING,
 					"matrix-keypad", keypad);
-			if (err) {
+			if (err < 0) {
 				dev_err(&pdev->dev,
 					"Unable to acquire interrupt for GPIO line %i\n",
 					pdata->row_gpios[i]);
diff --git a/drivers/input/keyboard/opencores-kbd.c b/drivers/input/keyboard/opencores-kbd.c
index 7b9b44158ad1..62abe2c16670 100644
--- a/drivers/input/keyboard/opencores-kbd.c
+++ b/drivers/input/keyboard/opencores-kbd.c
@@ -18,7 +18,6 @@
 
 struct opencores_kbd {
 	struct input_dev *input;
-	struct resource *addr_res;
 	void __iomem *addr;
 	int irq;
 	unsigned short keycodes[128];
@@ -56,35 +55,25 @@ static int opencores_kbd_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
-	opencores_kbd = kzalloc(sizeof(*opencores_kbd), GFP_KERNEL);
-	input = input_allocate_device();
-	if (!opencores_kbd || !input) {
-		dev_err(&pdev->dev, "failed to allocate device structures\n");
-		error = -ENOMEM;
-		goto err_free_mem;
-	}
-
-	opencores_kbd->addr_res = res;
-	res = request_mem_region(res->start, resource_size(res), pdev->name);
-	if (!res) {
-		dev_err(&pdev->dev, "failed to request I/O memory\n");
-		error = -EBUSY;
-		goto err_free_mem;
-	}
+	opencores_kbd = devm_kzalloc(&pdev->dev, sizeof(*opencores_kbd),
+				     GFP_KERNEL);
+	if (!opencores_kbd)
+		return -ENOMEM;
 
-	opencores_kbd->addr = ioremap(res->start, resource_size(res));
-	if (!opencores_kbd->addr) {
-		dev_err(&pdev->dev, "failed to remap I/O memory\n");
-		error = -ENXIO;
-		goto err_rel_mem;
+	input = devm_input_allocate_device(&pdev->dev);
+	if (!input) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		return -ENOMEM;
 	}
 
 	opencores_kbd->input = input;
-	opencores_kbd->irq = irq;
+
+	opencores_kbd->addr = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(opencores_kbd->addr))
+		error = PTR_ERR(opencores_kbd->addr);
 
 	input->name = pdev->name;
 	input->phys = "opencores-kbd/input0";
-	input->dev.parent = &pdev->dev;
 
 	input_set_drvdata(input, opencores_kbd);
 
@@ -109,54 +98,27 @@ static int opencores_kbd_probe(struct platform_device *pdev)
 	}
 	__clear_bit(KEY_RESERVED, input->keybit);
 
-	error = request_irq(irq, &opencores_kbd_isr,
-			    IRQF_TRIGGER_RISING, pdev->name, opencores_kbd);
+	error = devm_request_irq(&pdev->dev, irq, &opencores_kbd_isr,
+				 IRQF_TRIGGER_RISING,
+				 pdev->name, opencores_kbd);
 	if (error) {
 		dev_err(&pdev->dev, "unable to claim irq %d\n", irq);
-		goto err_unmap_mem;
+		return error;
 	}
 
 	error = input_register_device(input);
 	if (error) {
 		dev_err(&pdev->dev, "unable to register input device\n");
-		goto err_free_irq;
+		return error;
 	}
 
 	platform_set_drvdata(pdev, opencores_kbd);
 
 	return 0;
-
- err_free_irq:
-	free_irq(irq, opencores_kbd);
- err_unmap_mem:
-	iounmap(opencores_kbd->addr);
- err_rel_mem:
-	release_mem_region(res->start, resource_size(res));
- err_free_mem:
-	input_free_device(input);
-	kfree(opencores_kbd);
-
-	return error;
-}
-
-static int opencores_kbd_remove(struct platform_device *pdev)
-{
-	struct opencores_kbd *opencores_kbd = platform_get_drvdata(pdev);
-
-	free_irq(opencores_kbd->irq, opencores_kbd);
-
-	iounmap(opencores_kbd->addr);
-	release_mem_region(opencores_kbd->addr_res->start,
-		resource_size(opencores_kbd->addr_res));
-	input_unregister_device(opencores_kbd->input);
-	kfree(opencores_kbd);
-
-	return 0;
 }
 
 static struct platform_driver opencores_kbd_device_driver = {
 	.probe    = opencores_kbd_probe,
-	.remove   = opencores_kbd_remove,
 	.driver   = {
 		.name = "opencores-kbd",
 	},
diff --git a/drivers/input/misc/Kconfig b/drivers/input/misc/Kconfig
index 2ff4425a893b..23297ab6163f 100644
--- a/drivers/input/misc/Kconfig
+++ b/drivers/input/misc/Kconfig
@@ -144,6 +144,17 @@ config INPUT_M68K_BEEP
 	tristate "M68k Beeper support"
 	depends on M68K
 
+config INPUT_MAX77693_HAPTIC
+	tristate "MAXIM MAX77693 haptic controller support"
+	depends on MFD_MAX77693 && PWM
+	select INPUT_FF_MEMLESS
+	help
+	  This option enables support for the haptic controller on
+	  MAXIM MAX77693 chip.
+
+	  To compile this driver as module, choose M here: the
+	  module will be called max77693-haptic.
+
 config INPUT_MAX8925_ONKEY
 	tristate "MAX8925 ONKEY support"
 	depends on MFD_MAX8925
@@ -451,6 +462,16 @@ config HP_SDC_RTC
 	  Say Y here if you want to support the built-in real time clock
 	  of the HP SDC controller.
 
+config INPUT_PALMAS_PWRBUTTON
+	tristate "Palmas Power button Driver"
+	depends on MFD_PALMAS
+	help
+	  Say Y here if you want to enable power key reporting via the
+	  Palmas family of PMICs.
+
+	  To compile this driver as a module, choose M here. The module will
+	  be called palmas_pwrbutton.
+
 config INPUT_PCF50633_PMU
 	tristate "PCF50633 PMU events"
 	depends on MFD_PCF50633
@@ -676,4 +697,26 @@ config INPUT_SOC_BUTTON_ARRAY
 	  To compile this driver as a module, choose M here: the
 	  module will be called soc_button_array.
 
+config INPUT_DRV260X_HAPTICS
+	tristate "TI DRV260X haptics support"
+	depends on INPUT && I2C && GPIOLIB
+	select INPUT_FF_MEMLESS
+	select REGMAP_I2C
+	help
+	  Say Y to enable support for the TI DRV260X haptics driver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called drv260x-haptics.
+
+config INPUT_DRV2667_HAPTICS
+	tristate "TI DRV2667 haptics support"
+	depends on INPUT && I2C
+	select INPUT_FF_MEMLESS
+	select REGMAP_I2C
+	help
+	  Say Y to enable support for the TI DRV2667 haptics driver.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called drv260x-haptics.
+
 endif
diff --git a/drivers/input/misc/Makefile b/drivers/input/misc/Makefile
index 4955ad322a01..19c760361f80 100644
--- a/drivers/input/misc/Makefile
+++ b/drivers/input/misc/Makefile
@@ -26,6 +26,8 @@ obj-$(CONFIG_INPUT_COBALT_BTNS)		+= cobalt_btns.o
 obj-$(CONFIG_INPUT_DA9052_ONKEY)	+= da9052_onkey.o
 obj-$(CONFIG_INPUT_DA9055_ONKEY)	+= da9055_onkey.o
 obj-$(CONFIG_INPUT_DM355EVM)		+= dm355evm_keys.o
+obj-$(CONFIG_INPUT_DRV260X_HAPTICS)	+= drv260x.o
+obj-$(CONFIG_INPUT_DRV2667_HAPTICS)	+= drv2667.o
 obj-$(CONFIG_INPUT_GP2A)		+= gp2ap002a00f.o
 obj-$(CONFIG_INPUT_GPIO_BEEPER)		+= gpio-beeper.o
 obj-$(CONFIG_INPUT_GPIO_TILT_POLLED)	+= gpio_tilt_polled.o
@@ -35,11 +37,13 @@ obj-$(CONFIG_INPUT_IXP4XX_BEEPER)	+= ixp4xx-beeper.o
 obj-$(CONFIG_INPUT_KEYSPAN_REMOTE)	+= keyspan_remote.o
 obj-$(CONFIG_INPUT_KXTJ9)		+= kxtj9.o
 obj-$(CONFIG_INPUT_M68K_BEEP)		+= m68kspkr.o
+obj-$(CONFIG_INPUT_MAX77693_HAPTIC)	+= max77693-haptic.o
 obj-$(CONFIG_INPUT_MAX8925_ONKEY)	+= max8925_onkey.o
 obj-$(CONFIG_INPUT_MAX8997_HAPTIC)	+= max8997_haptic.o
 obj-$(CONFIG_INPUT_MC13783_PWRBUTTON)	+= mc13783-pwrbutton.o
 obj-$(CONFIG_INPUT_MMA8450)		+= mma8450.o
 obj-$(CONFIG_INPUT_MPU3050)		+= mpu3050.o
+obj-$(CONFIG_INPUT_PALMAS_PWRBUTTON)	+= palmas-pwrbutton.o
 obj-$(CONFIG_INPUT_PCAP)		+= pcap_keys.o
 obj-$(CONFIG_INPUT_PCF50633_PMU)	+= pcf50633-input.o
 obj-$(CONFIG_INPUT_PCF8574)		+= pcf8574_keypad.o
diff --git a/drivers/input/misc/drv260x.c b/drivers/input/misc/drv260x.c
new file mode 100644
index 000000000000..cab87f5ce6d3
--- /dev/null
+++ b/drivers/input/misc/drv260x.c
@@ -0,0 +1,741 @@
+/*
+ * DRV260X haptics driver family
+ *
+ * Author: Dan Murphy <dmurphy@ti.com>
+ *
+ * Copyright:   (C) 2014 Texas Instruments, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/of_gpio.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/gpio/consumer.h>
+#include <linux/regulator/consumer.h>
+
+#include <dt-bindings/input/ti-drv260x.h>
+#include <linux/platform_data/drv260x-pdata.h>
+
+#define DRV260X_STATUS		0x0
+#define DRV260X_MODE		0x1
+#define DRV260X_RT_PB_IN	0x2
+#define DRV260X_LIB_SEL		0x3
+#define DRV260X_WV_SEQ_1	0x4
+#define DRV260X_WV_SEQ_2	0x5
+#define DRV260X_WV_SEQ_3	0x6
+#define DRV260X_WV_SEQ_4	0x7
+#define DRV260X_WV_SEQ_5	0x8
+#define DRV260X_WV_SEQ_6	0x9
+#define DRV260X_WV_SEQ_7	0xa
+#define DRV260X_WV_SEQ_8	0xb
+#define DRV260X_GO				0xc
+#define DRV260X_OVERDRIVE_OFF	0xd
+#define DRV260X_SUSTAIN_P_OFF	0xe
+#define DRV260X_SUSTAIN_N_OFF	0xf
+#define DRV260X_BRAKE_OFF		0x10
+#define DRV260X_A_TO_V_CTRL		0x11
+#define DRV260X_A_TO_V_MIN_INPUT	0x12
+#define DRV260X_A_TO_V_MAX_INPUT	0x13
+#define DRV260X_A_TO_V_MIN_OUT	0x14
+#define DRV260X_A_TO_V_MAX_OUT	0x15
+#define DRV260X_RATED_VOLT		0x16
+#define DRV260X_OD_CLAMP_VOLT	0x17
+#define DRV260X_CAL_COMP		0x18
+#define DRV260X_CAL_BACK_EMF	0x19
+#define DRV260X_FEEDBACK_CTRL	0x1a
+#define DRV260X_CTRL1			0x1b
+#define DRV260X_CTRL2			0x1c
+#define DRV260X_CTRL3			0x1d
+#define DRV260X_CTRL4			0x1e
+#define DRV260X_CTRL5			0x1f
+#define DRV260X_LRA_LOOP_PERIOD	0x20
+#define DRV260X_VBAT_MON		0x21
+#define DRV260X_LRA_RES_PERIOD	0x22
+#define DRV260X_MAX_REG			0x23
+
+#define DRV260X_GO_BIT				0x01
+
+/* Library Selection */
+#define DRV260X_LIB_SEL_MASK		0x07
+#define DRV260X_LIB_SEL_RAM			0x0
+#define DRV260X_LIB_SEL_OD			0x1
+#define DRV260X_LIB_SEL_40_60		0x2
+#define DRV260X_LIB_SEL_60_80		0x3
+#define DRV260X_LIB_SEL_100_140		0x4
+#define DRV260X_LIB_SEL_140_PLUS	0x5
+
+#define DRV260X_LIB_SEL_HIZ_MASK	0x10
+#define DRV260X_LIB_SEL_HIZ_EN		0x01
+#define DRV260X_LIB_SEL_HIZ_DIS		0
+
+/* Mode register */
+#define DRV260X_STANDBY				(1 << 6)
+#define DRV260X_STANDBY_MASK		0x40
+#define DRV260X_INTERNAL_TRIGGER	0x00
+#define DRV260X_EXT_TRIGGER_EDGE	0x01
+#define DRV260X_EXT_TRIGGER_LEVEL	0x02
+#define DRV260X_PWM_ANALOG_IN		0x03
+#define DRV260X_AUDIOHAPTIC			0x04
+#define DRV260X_RT_PLAYBACK			0x05
+#define DRV260X_DIAGNOSTICS			0x06
+#define DRV260X_AUTO_CAL			0x07
+
+/* Audio to Haptics Control */
+#define DRV260X_AUDIO_HAPTICS_PEAK_10MS		(0 << 2)
+#define DRV260X_AUDIO_HAPTICS_PEAK_20MS		(1 << 2)
+#define DRV260X_AUDIO_HAPTICS_PEAK_30MS		(2 << 2)
+#define DRV260X_AUDIO_HAPTICS_PEAK_40MS		(3 << 2)
+
+#define DRV260X_AUDIO_HAPTICS_FILTER_100HZ	0x00
+#define DRV260X_AUDIO_HAPTICS_FILTER_125HZ	0x01
+#define DRV260X_AUDIO_HAPTICS_FILTER_150HZ	0x02
+#define DRV260X_AUDIO_HAPTICS_FILTER_200HZ	0x03
+
+/* Min/Max Input/Output Voltages */
+#define DRV260X_AUDIO_HAPTICS_MIN_IN_VOLT	0x19
+#define DRV260X_AUDIO_HAPTICS_MAX_IN_VOLT	0x64
+#define DRV260X_AUDIO_HAPTICS_MIN_OUT_VOLT	0x19
+#define DRV260X_AUDIO_HAPTICS_MAX_OUT_VOLT	0xFF
+
+/* Feedback register */
+#define DRV260X_FB_REG_ERM_MODE			0x7f
+#define DRV260X_FB_REG_LRA_MODE			(1 << 7)
+
+#define DRV260X_BRAKE_FACTOR_MASK	0x1f
+#define DRV260X_BRAKE_FACTOR_2X		(1 << 0)
+#define DRV260X_BRAKE_FACTOR_3X		(2 << 4)
+#define DRV260X_BRAKE_FACTOR_4X		(3 << 4)
+#define DRV260X_BRAKE_FACTOR_6X		(4 << 4)
+#define DRV260X_BRAKE_FACTOR_8X		(5 << 4)
+#define DRV260X_BRAKE_FACTOR_16		(6 << 4)
+#define DRV260X_BRAKE_FACTOR_DIS	(7 << 4)
+
+#define DRV260X_LOOP_GAIN_LOW		0xf3
+#define DRV260X_LOOP_GAIN_MED		(1 << 2)
+#define DRV260X_LOOP_GAIN_HIGH		(2 << 2)
+#define DRV260X_LOOP_GAIN_VERY_HIGH	(3 << 2)
+
+#define DRV260X_BEMF_GAIN_0			0xfc
+#define DRV260X_BEMF_GAIN_1		(1 << 0)
+#define DRV260X_BEMF_GAIN_2		(2 << 0)
+#define DRV260X_BEMF_GAIN_3		(3 << 0)
+
+/* Control 1 register */
+#define DRV260X_AC_CPLE_EN			(1 << 5)
+#define DRV260X_STARTUP_BOOST		(1 << 7)
+
+/* Control 2 register */
+
+#define DRV260X_IDISS_TIME_45		0
+#define DRV260X_IDISS_TIME_75		(1 << 0)
+#define DRV260X_IDISS_TIME_150		(1 << 1)
+#define DRV260X_IDISS_TIME_225		0x03
+
+#define DRV260X_BLANK_TIME_45	(0 << 2)
+#define DRV260X_BLANK_TIME_75	(1 << 2)
+#define DRV260X_BLANK_TIME_150	(2 << 2)
+#define DRV260X_BLANK_TIME_225	(3 << 2)
+
+#define DRV260X_SAMP_TIME_150	(0 << 4)
+#define DRV260X_SAMP_TIME_200	(1 << 4)
+#define DRV260X_SAMP_TIME_250	(2 << 4)
+#define DRV260X_SAMP_TIME_300	(3 << 4)
+
+#define DRV260X_BRAKE_STABILIZER	(1 << 6)
+#define DRV260X_UNIDIR_IN			(0 << 7)
+#define DRV260X_BIDIR_IN			(1 << 7)
+
+/* Control 3 Register */
+#define DRV260X_LRA_OPEN_LOOP		(1 << 0)
+#define DRV260X_ANANLOG_IN			(1 << 1)
+#define DRV260X_LRA_DRV_MODE		(1 << 2)
+#define DRV260X_RTP_UNSIGNED_DATA	(1 << 3)
+#define DRV260X_SUPPLY_COMP_DIS		(1 << 4)
+#define DRV260X_ERM_OPEN_LOOP		(1 << 5)
+#define DRV260X_NG_THRESH_0			(0 << 6)
+#define DRV260X_NG_THRESH_2			(1 << 6)
+#define DRV260X_NG_THRESH_4			(2 << 6)
+#define DRV260X_NG_THRESH_8			(3 << 6)
+
+/* Control 4 Register */
+#define DRV260X_AUTOCAL_TIME_150MS		(0 << 4)
+#define DRV260X_AUTOCAL_TIME_250MS		(1 << 4)
+#define DRV260X_AUTOCAL_TIME_500MS		(2 << 4)
+#define DRV260X_AUTOCAL_TIME_1000MS		(3 << 4)
+
+/**
+ * struct drv260x_data -
+ * @input_dev - Pointer to the input device
+ * @client - Pointer to the I2C client
+ * @regmap - Register map of the device
+ * @work - Work item used to off load the enable/disable of the vibration
+ * @enable_gpio - Pointer to the gpio used for enable/disabling
+ * @regulator - Pointer to the regulator for the IC
+ * @magnitude - Magnitude of the vibration event
+ * @mode - The operating mode of the IC (LRA_NO_CAL, ERM or LRA)
+ * @library - The vibration library to be used
+ * @rated_voltage - The rated_voltage of the actuator
+ * @overdriver_voltage - The over drive voltage of the actuator
+**/
+struct drv260x_data {
+	struct input_dev *input_dev;
+	struct i2c_client *client;
+	struct regmap *regmap;
+	struct work_struct work;
+	struct gpio_desc *enable_gpio;
+	struct regulator *regulator;
+	u32 magnitude;
+	u32 mode;
+	u32 library;
+	int rated_voltage;
+	int overdrive_voltage;
+};
+
+static struct reg_default drv260x_reg_defs[] = {
+	{ DRV260X_STATUS, 0xe0 },
+	{ DRV260X_MODE, 0x40 },
+	{ DRV260X_RT_PB_IN, 0x00 },
+	{ DRV260X_LIB_SEL, 0x00 },
+	{ DRV260X_WV_SEQ_1, 0x01 },
+	{ DRV260X_WV_SEQ_2, 0x00 },
+	{ DRV260X_WV_SEQ_3, 0x00 },
+	{ DRV260X_WV_SEQ_4, 0x00 },
+	{ DRV260X_WV_SEQ_5, 0x00 },
+	{ DRV260X_WV_SEQ_6, 0x00 },
+	{ DRV260X_WV_SEQ_7, 0x00 },
+	{ DRV260X_WV_SEQ_8, 0x00 },
+	{ DRV260X_GO, 0x00 },
+	{ DRV260X_OVERDRIVE_OFF, 0x00 },
+	{ DRV260X_SUSTAIN_P_OFF, 0x00 },
+	{ DRV260X_SUSTAIN_N_OFF, 0x00 },
+	{ DRV260X_BRAKE_OFF, 0x00 },
+	{ DRV260X_A_TO_V_CTRL, 0x05 },
+	{ DRV260X_A_TO_V_MIN_INPUT, 0x19 },
+	{ DRV260X_A_TO_V_MAX_INPUT, 0xff },
+	{ DRV260X_A_TO_V_MIN_OUT, 0x19 },
+	{ DRV260X_A_TO_V_MAX_OUT, 0xff },
+	{ DRV260X_RATED_VOLT, 0x3e },
+	{ DRV260X_OD_CLAMP_VOLT, 0x8c },
+	{ DRV260X_CAL_COMP, 0x0c },
+	{ DRV260X_CAL_BACK_EMF, 0x6c },
+	{ DRV260X_FEEDBACK_CTRL, 0x36 },
+	{ DRV260X_CTRL1, 0x93 },
+	{ DRV260X_CTRL2, 0xfa },
+	{ DRV260X_CTRL3, 0xa0 },
+	{ DRV260X_CTRL4, 0x20 },
+	{ DRV260X_CTRL5, 0x80 },
+	{ DRV260X_LRA_LOOP_PERIOD, 0x33 },
+	{ DRV260X_VBAT_MON, 0x00 },
+	{ DRV260X_LRA_RES_PERIOD, 0x00 },
+};
+
+#define DRV260X_DEF_RATED_VOLT		0x90
+#define DRV260X_DEF_OD_CLAMP_VOLT	0x90
+
+/**
+ * Rated and Overdriver Voltages:
+ * Calculated using the formula r = v * 255 / 5.6
+ * where r is what will be written to the register
+ * and v is the rated or overdriver voltage of the actuator
+ **/
+static int drv260x_calculate_voltage(unsigned int voltage)
+{
+	return (voltage * 255 / 5600);
+}
+
+static void drv260x_worker(struct work_struct *work)
+{
+	struct drv260x_data *haptics = container_of(work, struct drv260x_data, work);
+	int error;
+
+	gpiod_set_value(haptics->enable_gpio, 1);
+	/* Data sheet says to wait 250us before trying to communicate */
+	udelay(250);
+
+	error = regmap_write(haptics->regmap,
+			     DRV260X_MODE, DRV260X_RT_PLAYBACK);
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to write set mode: %d\n", error);
+	} else {
+		error = regmap_write(haptics->regmap,
+				     DRV260X_RT_PB_IN, haptics->magnitude);
+		if (error)
+			dev_err(&haptics->client->dev,
+				"Failed to set magnitude: %d\n", error);
+	}
+}
+
+static int drv260x_haptics_play(struct input_dev *input, void *data,
+				struct ff_effect *effect)
+{
+	struct drv260x_data *haptics = input_get_drvdata(input);
+
+	haptics->mode = DRV260X_LRA_NO_CAL_MODE;
+
+	if (effect->u.rumble.strong_magnitude > 0)
+		haptics->magnitude = effect->u.rumble.strong_magnitude;
+	else if (effect->u.rumble.weak_magnitude > 0)
+		haptics->magnitude = effect->u.rumble.weak_magnitude;
+	else
+		haptics->magnitude = 0;
+
+	schedule_work(&haptics->work);
+
+	return 0;
+}
+
+static void drv260x_close(struct input_dev *input)
+{
+	struct drv260x_data *haptics = input_get_drvdata(input);
+	int error;
+
+	cancel_work_sync(&haptics->work);
+
+	error = regmap_write(haptics->regmap, DRV260X_MODE, DRV260X_STANDBY);
+	if (error)
+		dev_err(&haptics->client->dev,
+			"Failed to enter standby mode: %d\n", error);
+
+	gpiod_set_value(haptics->enable_gpio, 0);
+}
+
+static const struct reg_default drv260x_lra_cal_regs[] = {
+	{ DRV260X_MODE, DRV260X_AUTO_CAL },
+	{ DRV260X_CTRL3, DRV260X_NG_THRESH_2 },
+	{ DRV260X_FEEDBACK_CTRL, DRV260X_FB_REG_LRA_MODE |
+		DRV260X_BRAKE_FACTOR_4X | DRV260X_LOOP_GAIN_HIGH },
+};
+
+static const struct reg_default drv260x_lra_init_regs[] = {
+	{ DRV260X_MODE, DRV260X_RT_PLAYBACK },
+	{ DRV260X_A_TO_V_CTRL, DRV260X_AUDIO_HAPTICS_PEAK_20MS |
+		DRV260X_AUDIO_HAPTICS_FILTER_125HZ },
+	{ DRV260X_A_TO_V_MIN_INPUT, DRV260X_AUDIO_HAPTICS_MIN_IN_VOLT },
+	{ DRV260X_A_TO_V_MAX_INPUT, DRV260X_AUDIO_HAPTICS_MAX_IN_VOLT },
+	{ DRV260X_A_TO_V_MIN_OUT, DRV260X_AUDIO_HAPTICS_MIN_OUT_VOLT },
+	{ DRV260X_A_TO_V_MAX_OUT, DRV260X_AUDIO_HAPTICS_MAX_OUT_VOLT },
+	{ DRV260X_FEEDBACK_CTRL, DRV260X_FB_REG_LRA_MODE |
+		DRV260X_BRAKE_FACTOR_2X | DRV260X_LOOP_GAIN_MED |
+		DRV260X_BEMF_GAIN_3 },
+	{ DRV260X_CTRL1, DRV260X_STARTUP_BOOST },
+	{ DRV260X_CTRL2, DRV260X_SAMP_TIME_250 },
+	{ DRV260X_CTRL3, DRV260X_NG_THRESH_2 | DRV260X_ANANLOG_IN },
+	{ DRV260X_CTRL4, DRV260X_AUTOCAL_TIME_500MS },
+};
+
+static const struct reg_default drv260x_erm_cal_regs[] = {
+	{ DRV260X_MODE, DRV260X_AUTO_CAL },
+	{ DRV260X_A_TO_V_MIN_INPUT, DRV260X_AUDIO_HAPTICS_MIN_IN_VOLT },
+	{ DRV260X_A_TO_V_MAX_INPUT, DRV260X_AUDIO_HAPTICS_MAX_IN_VOLT },
+	{ DRV260X_A_TO_V_MIN_OUT, DRV260X_AUDIO_HAPTICS_MIN_OUT_VOLT },
+	{ DRV260X_A_TO_V_MAX_OUT, DRV260X_AUDIO_HAPTICS_MAX_OUT_VOLT },
+	{ DRV260X_FEEDBACK_CTRL, DRV260X_BRAKE_FACTOR_3X |
+		DRV260X_LOOP_GAIN_MED | DRV260X_BEMF_GAIN_2 },
+	{ DRV260X_CTRL1, DRV260X_STARTUP_BOOST },
+	{ DRV260X_CTRL2, DRV260X_SAMP_TIME_250 | DRV260X_BLANK_TIME_75 |
+		DRV260X_IDISS_TIME_75 },
+	{ DRV260X_CTRL3, DRV260X_NG_THRESH_2 | DRV260X_ERM_OPEN_LOOP },
+	{ DRV260X_CTRL4, DRV260X_AUTOCAL_TIME_500MS },
+};
+
+static int drv260x_init(struct drv260x_data *haptics)
+{
+	int error;
+	unsigned int cal_buf;
+
+	error = regmap_write(haptics->regmap,
+			     DRV260X_RATED_VOLT, haptics->rated_voltage);
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to write DRV260X_RATED_VOLT register: %d\n",
+			error);
+		return error;
+	}
+
+	error = regmap_write(haptics->regmap,
+			     DRV260X_OD_CLAMP_VOLT, haptics->overdrive_voltage);
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to write DRV260X_OD_CLAMP_VOLT register: %d\n",
+			error);
+		return error;
+	}
+
+	switch (haptics->mode) {
+	case DRV260X_LRA_MODE:
+		error = regmap_register_patch(haptics->regmap,
+					      drv260x_lra_cal_regs,
+					      ARRAY_SIZE(drv260x_lra_cal_regs));
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to write LRA calibration registers: %d\n",
+				error);
+			return error;
+		}
+
+		break;
+
+	case DRV260X_ERM_MODE:
+		error = regmap_register_patch(haptics->regmap,
+					      drv260x_erm_cal_regs,
+					      ARRAY_SIZE(drv260x_erm_cal_regs));
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to write ERM calibration registers: %d\n",
+				error);
+			return error;
+		}
+
+		error = regmap_update_bits(haptics->regmap, DRV260X_LIB_SEL,
+					   DRV260X_LIB_SEL_MASK,
+					   haptics->library);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to write DRV260X_LIB_SEL register: %d\n",
+				error);
+			return error;
+		}
+
+		break;
+
+	default:
+		error = regmap_register_patch(haptics->regmap,
+					      drv260x_lra_init_regs,
+					      ARRAY_SIZE(drv260x_lra_init_regs));
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to write LRA init registers: %d\n",
+				error);
+			return error;
+		}
+
+		error = regmap_update_bits(haptics->regmap, DRV260X_LIB_SEL,
+					   DRV260X_LIB_SEL_MASK,
+					   haptics->library);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to write DRV260X_LIB_SEL register: %d\n",
+				error);
+			return error;
+		}
+
+		/* No need to set GO bit here */
+		return 0;
+	}
+
+	error = regmap_write(haptics->regmap, DRV260X_GO, DRV260X_GO_BIT);
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to write GO register: %d\n",
+			error);
+		return error;
+	}
+
+	do {
+		error = regmap_read(haptics->regmap, DRV260X_GO, &cal_buf);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to read GO register: %d\n",
+				error);
+			return error;
+		}
+	} while (cal_buf == DRV260X_GO_BIT);
+
+	return 0;
+}
+
+static const struct regmap_config drv260x_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = DRV260X_MAX_REG,
+	.reg_defaults = drv260x_reg_defs,
+	.num_reg_defaults = ARRAY_SIZE(drv260x_reg_defs),
+	.cache_type = REGCACHE_NONE,
+};
+
+#ifdef CONFIG_OF
+static int drv260x_parse_dt(struct device *dev,
+			    struct drv260x_data *haptics)
+{
+	struct device_node *np = dev->of_node;
+	unsigned int voltage;
+	int error;
+
+	error = of_property_read_u32(np, "mode", &haptics->mode);
+	if (error) {
+		dev_err(dev, "%s: No entry for mode\n", __func__);
+		return error;
+	}
+
+	error = of_property_read_u32(np, "library-sel", &haptics->library);
+	if (error) {
+		dev_err(dev, "%s: No entry for library selection\n",
+			__func__);
+		return error;
+	}
+
+	error = of_property_read_u32(np, "vib-rated-mv", &voltage);
+	if (!error)
+		haptics->rated_voltage = drv260x_calculate_voltage(voltage);
+
+
+	error = of_property_read_u32(np, "vib-overdrive-mv", &voltage);
+	if (!error)
+		haptics->overdrive_voltage = drv260x_calculate_voltage(voltage);
+
+	return 0;
+}
+#else
+static inline int drv260x_parse_dt(struct device *dev,
+				   struct drv260x_data *haptics)
+{
+	dev_err(dev, "no platform data defined\n");
+
+	return -EINVAL;
+}
+#endif
+
+static int drv260x_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	const struct drv260x_platform_data *pdata = dev_get_platdata(&client->dev);
+	struct drv260x_data *haptics;
+	int error;
+
+	haptics = devm_kzalloc(&client->dev, sizeof(*haptics), GFP_KERNEL);
+	if (!haptics)
+		return -ENOMEM;
+
+	haptics->rated_voltage = DRV260X_DEF_OD_CLAMP_VOLT;
+	haptics->rated_voltage = DRV260X_DEF_RATED_VOLT;
+
+	if (pdata) {
+		haptics->mode = pdata->mode;
+		haptics->library = pdata->library_selection;
+		if (pdata->vib_overdrive_voltage)
+			haptics->overdrive_voltage = drv260x_calculate_voltage(pdata->vib_overdrive_voltage);
+		if (pdata->vib_rated_voltage)
+			haptics->rated_voltage = drv260x_calculate_voltage(pdata->vib_rated_voltage);
+	} else if (client->dev.of_node) {
+		error = drv260x_parse_dt(&client->dev, haptics);
+		if (error)
+			return error;
+	} else {
+		dev_err(&client->dev, "Platform data not set\n");
+		return -ENODEV;
+	}
+
+
+	if (haptics->mode < DRV260X_LRA_MODE ||
+	    haptics->mode > DRV260X_ERM_MODE) {
+		dev_err(&client->dev,
+			"Vibrator mode is invalid: %i\n",
+			haptics->mode);
+		return -EINVAL;
+	}
+
+	if (haptics->library < DRV260X_LIB_EMPTY ||
+	    haptics->library > DRV260X_ERM_LIB_F) {
+		dev_err(&client->dev,
+			"Library value is invalid: %i\n", haptics->library);
+		return -EINVAL;
+	}
+
+	if (haptics->mode == DRV260X_LRA_MODE &&
+	    haptics->library != DRV260X_LIB_EMPTY &&
+	    haptics->library != DRV260X_LIB_LRA) {
+		dev_err(&client->dev,
+			"LRA Mode with ERM Library mismatch\n");
+		return -EINVAL;
+	}
+
+	if (haptics->mode == DRV260X_ERM_MODE &&
+	    (haptics->library == DRV260X_LIB_EMPTY ||
+	     haptics->library == DRV260X_LIB_LRA)) {
+		dev_err(&client->dev,
+			"ERM Mode with LRA Library mismatch\n");
+		return -EINVAL;
+	}
+
+	haptics->regulator = devm_regulator_get(&client->dev, "vbat");
+	if (IS_ERR(haptics->regulator)) {
+		error = PTR_ERR(haptics->regulator);
+		dev_err(&client->dev,
+			"unable to get regulator, error: %d\n", error);
+		return error;
+	}
+
+	haptics->enable_gpio = devm_gpiod_get(&client->dev, "enable");
+	if (IS_ERR(haptics->enable_gpio)) {
+		error = PTR_ERR(haptics->enable_gpio);
+		if (error != -ENOENT && error != -ENOSYS)
+			return error;
+		haptics->enable_gpio = NULL;
+	} else {
+		gpiod_direction_output(haptics->enable_gpio, 1);
+	}
+
+	haptics->input_dev = devm_input_allocate_device(&client->dev);
+	if (!haptics->input_dev) {
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	haptics->input_dev->name = "drv260x:haptics";
+	haptics->input_dev->dev.parent = client->dev.parent;
+	haptics->input_dev->close = drv260x_close;
+	input_set_drvdata(haptics->input_dev, haptics);
+	input_set_capability(haptics->input_dev, EV_FF, FF_RUMBLE);
+
+	error = input_ff_create_memless(haptics->input_dev, NULL,
+					drv260x_haptics_play);
+	if (error) {
+		dev_err(&client->dev, "input_ff_create() failed: %d\n",
+			error);
+		return error;
+	}
+
+	INIT_WORK(&haptics->work, drv260x_worker);
+
+	haptics->client = client;
+	i2c_set_clientdata(client, haptics);
+
+	haptics->regmap = devm_regmap_init_i2c(client, &drv260x_regmap_config);
+	if (IS_ERR(haptics->regmap)) {
+		error = PTR_ERR(haptics->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			error);
+		return error;
+	}
+
+	error = drv260x_init(haptics);
+	if (error) {
+		dev_err(&client->dev, "Device init failed: %d\n", error);
+		return error;
+	}
+
+	error = input_register_device(haptics->input_dev);
+	if (error) {
+		dev_err(&client->dev, "couldn't register input device: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int drv260x_suspend(struct device *dev)
+{
+	struct drv260x_data *haptics = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&haptics->input_dev->mutex);
+
+	if (haptics->input_dev->users) {
+		ret = regmap_update_bits(haptics->regmap,
+					 DRV260X_MODE,
+					 DRV260X_STANDBY_MASK,
+					 DRV260X_STANDBY);
+		if (ret) {
+			dev_err(dev, "Failed to set standby mode\n");
+			goto out;
+		}
+
+		gpiod_set_value(haptics->enable_gpio, 0);
+
+		ret = regulator_disable(haptics->regulator);
+		if (ret) {
+			dev_err(dev, "Failed to disable regulator\n");
+			regmap_update_bits(haptics->regmap,
+					   DRV260X_MODE,
+					   DRV260X_STANDBY_MASK, 0);
+		}
+	}
+out:
+	mutex_unlock(&haptics->input_dev->mutex);
+	return ret;
+}
+
+static int drv260x_resume(struct device *dev)
+{
+	struct drv260x_data *haptics = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&haptics->input_dev->mutex);
+
+	if (haptics->input_dev->users) {
+		ret = regulator_enable(haptics->regulator);
+		if (ret) {
+			dev_err(dev, "Failed to enable regulator\n");
+			goto out;
+		}
+
+		ret = regmap_update_bits(haptics->regmap,
+					 DRV260X_MODE,
+					 DRV260X_STANDBY_MASK, 0);
+		if (ret) {
+			dev_err(dev, "Failed to unset standby mode\n");
+			regulator_disable(haptics->regulator);
+			goto out;
+		}
+
+		gpiod_set_value(haptics->enable_gpio, 1);
+	}
+
+out:
+	mutex_unlock(&haptics->input_dev->mutex);
+	return ret;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(drv260x_pm_ops, drv260x_suspend, drv260x_resume);
+
+static const struct i2c_device_id drv260x_id[] = {
+	{ "drv2605l", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, drv260x_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id drv260x_of_match[] = {
+	{ .compatible = "ti,drv2604", },
+	{ .compatible = "ti,drv2604l", },
+	{ .compatible = "ti,drv2605", },
+	{ .compatible = "ti,drv2605l", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, drv260x_of_match);
+#endif
+
+static struct i2c_driver drv260x_driver = {
+	.probe		= drv260x_probe,
+	.driver		= {
+		.name	= "drv260x-haptics",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(drv260x_of_match),
+		.pm	= &drv260x_pm_ops,
+	},
+	.id_table = drv260x_id,
+};
+module_i2c_driver(drv260x_driver);
+
+MODULE_ALIAS("platform:drv260x-haptics");
+MODULE_DESCRIPTION("TI DRV260x haptics driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dan Murphy <dmurphy@ti.com>");
diff --git a/drivers/input/misc/drv2667.c b/drivers/input/misc/drv2667.c
new file mode 100644
index 000000000000..0f437581cc04
--- /dev/null
+++ b/drivers/input/misc/drv2667.c
@@ -0,0 +1,500 @@
+/*
+ * DRV2667 haptics driver family
+ *
+ * Author: Dan Murphy <dmurphy@ti.com>
+ *
+ * Copyright: (C) 2014 Texas Instruments, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/regulator/consumer.h>
+
+/* Contol registers */
+#define DRV2667_STATUS	0x00
+#define DRV2667_CTRL_1	0x01
+#define DRV2667_CTRL_2	0x02
+/* Waveform sequencer */
+#define DRV2667_WV_SEQ_0	0x03
+#define DRV2667_WV_SEQ_1	0x04
+#define DRV2667_WV_SEQ_2	0x05
+#define DRV2667_WV_SEQ_3	0x06
+#define DRV2667_WV_SEQ_4	0x07
+#define DRV2667_WV_SEQ_5	0x08
+#define DRV2667_WV_SEQ_6	0x09
+#define DRV2667_WV_SEQ_7	0x0A
+#define DRV2667_FIFO		0x0B
+#define DRV2667_PAGE		0xFF
+#define DRV2667_MAX_REG		DRV2667_PAGE
+
+#define DRV2667_PAGE_0		0x00
+#define DRV2667_PAGE_1		0x01
+#define DRV2667_PAGE_2		0x02
+#define DRV2667_PAGE_3		0x03
+#define DRV2667_PAGE_4		0x04
+#define DRV2667_PAGE_5		0x05
+#define DRV2667_PAGE_6		0x06
+#define DRV2667_PAGE_7		0x07
+#define DRV2667_PAGE_8		0x08
+
+/* RAM fields */
+#define DRV2667_RAM_HDR_SZ	0x0
+/* RAM Header addresses */
+#define DRV2667_RAM_START_HI	0x01
+#define DRV2667_RAM_START_LO	0x02
+#define DRV2667_RAM_STOP_HI		0x03
+#define DRV2667_RAM_STOP_LO		0x04
+#define DRV2667_RAM_REPEAT_CT	0x05
+/* RAM data addresses */
+#define DRV2667_RAM_AMP		0x06
+#define DRV2667_RAM_FREQ	0x07
+#define DRV2667_RAM_DURATION	0x08
+#define DRV2667_RAM_ENVELOPE	0x09
+
+/* Control 1 Register */
+#define DRV2667_25_VPP_GAIN		0x00
+#define DRV2667_50_VPP_GAIN		0x01
+#define DRV2667_75_VPP_GAIN		0x02
+#define DRV2667_100_VPP_GAIN	0x03
+#define DRV2667_DIGITAL_IN		0xfc
+#define DRV2667_ANALOG_IN		(1 << 2)
+
+/* Control 2 Register */
+#define DRV2667_GO			(1 << 0)
+#define DRV2667_STANDBY		(1 << 6)
+#define DRV2667_DEV_RST		(1 << 7)
+
+/* RAM Envelope settings */
+#define DRV2667_NO_ENV			0x00
+#define DRV2667_32_MS_ENV		0x01
+#define DRV2667_64_MS_ENV		0x02
+#define DRV2667_96_MS_ENV		0x03
+#define DRV2667_128_MS_ENV		0x04
+#define DRV2667_160_MS_ENV		0x05
+#define DRV2667_192_MS_ENV		0x06
+#define DRV2667_224_MS_ENV		0x07
+#define DRV2667_256_MS_ENV		0x08
+#define DRV2667_512_MS_ENV		0x09
+#define DRV2667_768_MS_ENV		0x0a
+#define DRV2667_1024_MS_ENV		0x0b
+#define DRV2667_1280_MS_ENV		0x0c
+#define DRV2667_1536_MS_ENV		0x0d
+#define DRV2667_1792_MS_ENV		0x0e
+#define DRV2667_2048_MS_ENV		0x0f
+
+/**
+ * struct drv2667_data -
+ * @input_dev - Pointer to the input device
+ * @client - Pointer to the I2C client
+ * @regmap - Register map of the device
+ * @work - Work item used to off load the enable/disable of the vibration
+ * @regulator - Pointer to the regulator for the IC
+ * @magnitude - Magnitude of the vibration event
+**/
+struct drv2667_data {
+	struct input_dev *input_dev;
+	struct i2c_client *client;
+	struct regmap *regmap;
+	struct work_struct work;
+	struct regulator *regulator;
+	u32 page;
+	u32 magnitude;
+	u32 frequency;
+};
+
+static struct reg_default drv2667_reg_defs[] = {
+	{ DRV2667_STATUS, 0x02 },
+	{ DRV2667_CTRL_1, 0x28 },
+	{ DRV2667_CTRL_2, 0x40 },
+	{ DRV2667_WV_SEQ_0, 0x00 },
+	{ DRV2667_WV_SEQ_1, 0x00 },
+	{ DRV2667_WV_SEQ_2, 0x00 },
+	{ DRV2667_WV_SEQ_3, 0x00 },
+	{ DRV2667_WV_SEQ_4, 0x00 },
+	{ DRV2667_WV_SEQ_5, 0x00 },
+	{ DRV2667_WV_SEQ_6, 0x00 },
+	{ DRV2667_WV_SEQ_7, 0x00 },
+	{ DRV2667_FIFO, 0x00 },
+	{ DRV2667_PAGE, 0x00 },
+};
+
+static int drv2667_set_waveform_freq(struct drv2667_data *haptics)
+{
+	unsigned int read_buf;
+	int freq;
+	int error;
+
+	/* Per the data sheet:
+	 * Sinusoid Frequency (Hz) = 7.8125 x Frequency
+	 */
+	freq = (haptics->frequency * 1000) / 78125;
+	if (freq <= 0) {
+		dev_err(&haptics->client->dev,
+			"ERROR: Frequency calculated to %i\n", freq);
+		return -EINVAL;
+	}
+
+	error = regmap_read(haptics->regmap, DRV2667_PAGE, &read_buf);
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to read the page number: %d\n", error);
+		return -EIO;
+	}
+
+	if (read_buf == DRV2667_PAGE_0 ||
+		haptics->page != read_buf) {
+		error = regmap_write(haptics->regmap,
+				DRV2667_PAGE, haptics->page);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to set the page: %d\n", error);
+			return -EIO;
+		}
+	}
+
+	error = regmap_write(haptics->regmap, DRV2667_RAM_FREQ,	freq);
+	if (error)
+		dev_err(&haptics->client->dev,
+				"Failed to set the frequency: %d\n", error);
+
+	/* Reset back to original page */
+	if (read_buf == DRV2667_PAGE_0 ||
+		haptics->page != read_buf) {
+		error = regmap_write(haptics->regmap, DRV2667_PAGE, read_buf);
+		if (error) {
+			dev_err(&haptics->client->dev,
+					"Failed to set the page: %d\n", error);
+				return -EIO;
+			}
+	}
+
+	return error;
+}
+
+static void drv2667_worker(struct work_struct *work)
+{
+	struct drv2667_data *haptics = container_of(work, struct drv2667_data, work);
+	int error;
+
+	if (haptics->magnitude) {
+		error = regmap_write(haptics->regmap,
+				DRV2667_PAGE, haptics->page);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to set the page: %d\n", error);
+			return;
+		}
+
+		error = regmap_write(haptics->regmap, DRV2667_RAM_AMP,
+				haptics->magnitude);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to set the amplitude: %d\n", error);
+			return;
+		}
+
+		error = regmap_write(haptics->regmap,
+				DRV2667_PAGE, DRV2667_PAGE_0);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to set the page: %d\n", error);
+			return;
+		}
+
+		error = regmap_write(haptics->regmap,
+				DRV2667_CTRL_2, DRV2667_GO);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to set the GO bit: %d\n", error);
+		}
+	} else {
+		error = regmap_update_bits(haptics->regmap, DRV2667_CTRL_2,
+				DRV2667_GO, 0);
+		if (error) {
+			dev_err(&haptics->client->dev,
+				"Failed to unset the GO bit: %d\n", error);
+		}
+	}
+}
+
+static int drv2667_haptics_play(struct input_dev *input, void *data,
+				struct ff_effect *effect)
+{
+	struct drv2667_data *haptics = input_get_drvdata(input);
+
+	if (effect->u.rumble.strong_magnitude > 0)
+		haptics->magnitude = effect->u.rumble.strong_magnitude;
+	else if (effect->u.rumble.weak_magnitude > 0)
+		haptics->magnitude = effect->u.rumble.weak_magnitude;
+	else
+		haptics->magnitude = 0;
+
+	schedule_work(&haptics->work);
+
+	return 0;
+}
+
+static void drv2667_close(struct input_dev *input)
+{
+	struct drv2667_data *haptics = input_get_drvdata(input);
+	int error;
+
+	cancel_work_sync(&haptics->work);
+
+	error = regmap_update_bits(haptics->regmap, DRV2667_CTRL_2,
+				DRV2667_STANDBY, 1);
+	if (error)
+		dev_err(&haptics->client->dev,
+			"Failed to enter standby mode: %d\n", error);
+}
+
+static const struct reg_default drv2667_init_regs[] = {
+	{ DRV2667_CTRL_2, 0 },
+	{ DRV2667_CTRL_1, DRV2667_25_VPP_GAIN },
+	{ DRV2667_WV_SEQ_0, 1 },
+	{ DRV2667_WV_SEQ_1, 0 }
+};
+
+static const struct reg_default drv2667_page1_init[] = {
+	{ DRV2667_RAM_HDR_SZ, 0x05 },
+	{ DRV2667_RAM_START_HI, 0x80 },
+	{ DRV2667_RAM_START_LO, 0x06 },
+	{ DRV2667_RAM_STOP_HI, 0x00 },
+	{ DRV2667_RAM_STOP_LO, 0x09 },
+	{ DRV2667_RAM_REPEAT_CT, 0 },
+	{ DRV2667_RAM_DURATION, 0x05 },
+	{ DRV2667_RAM_ENVELOPE, DRV2667_NO_ENV },
+	{ DRV2667_RAM_AMP, 0x60 },
+};
+
+static int drv2667_init(struct drv2667_data *haptics)
+{
+	int error;
+
+	/* Set default haptic frequency to 195Hz on Page 1*/
+	haptics->frequency = 195;
+	haptics->page = DRV2667_PAGE_1;
+
+	error = regmap_register_patch(haptics->regmap,
+				      drv2667_init_regs,
+				      ARRAY_SIZE(drv2667_init_regs));
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to write init registers: %d\n",
+			error);
+		return error;
+	}
+
+	error = regmap_write(haptics->regmap, DRV2667_PAGE, haptics->page);
+	if (error) {
+		dev_err(&haptics->client->dev, "Failed to set page: %d\n",
+			error);
+		goto error_out;
+	}
+
+	error = drv2667_set_waveform_freq(haptics);
+	if (error)
+		goto error_page;
+
+	error = regmap_register_patch(haptics->regmap,
+				      drv2667_page1_init,
+				      ARRAY_SIZE(drv2667_page1_init));
+	if (error) {
+		dev_err(&haptics->client->dev,
+			"Failed to write page registers: %d\n",
+			error);
+		return error;
+	}
+
+	error = regmap_write(haptics->regmap, DRV2667_PAGE, DRV2667_PAGE_0);
+	return error;
+
+error_page:
+	regmap_write(haptics->regmap, DRV2667_PAGE, DRV2667_PAGE_0);
+error_out:
+	return error;
+}
+
+static const struct regmap_config drv2667_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = DRV2667_MAX_REG,
+	.reg_defaults = drv2667_reg_defs,
+	.num_reg_defaults = ARRAY_SIZE(drv2667_reg_defs),
+	.cache_type = REGCACHE_NONE,
+};
+
+static int drv2667_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct drv2667_data *haptics;
+	int error;
+
+	haptics = devm_kzalloc(&client->dev, sizeof(*haptics), GFP_KERNEL);
+	if (!haptics)
+		return -ENOMEM;
+
+	haptics->regulator = devm_regulator_get(&client->dev, "vbat");
+	if (IS_ERR(haptics->regulator)) {
+		error = PTR_ERR(haptics->regulator);
+		dev_err(&client->dev,
+			"unable to get regulator, error: %d\n", error);
+		return error;
+	}
+
+	haptics->input_dev = devm_input_allocate_device(&client->dev);
+	if (!haptics->input_dev) {
+		dev_err(&client->dev, "Failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	haptics->input_dev->name = "drv2667:haptics";
+	haptics->input_dev->dev.parent = client->dev.parent;
+	haptics->input_dev->close = drv2667_close;
+	input_set_drvdata(haptics->input_dev, haptics);
+	input_set_capability(haptics->input_dev, EV_FF, FF_RUMBLE);
+
+	error = input_ff_create_memless(haptics->input_dev, NULL,
+					drv2667_haptics_play);
+	if (error) {
+		dev_err(&client->dev, "input_ff_create() failed: %d\n",
+			error);
+		return error;
+	}
+
+	INIT_WORK(&haptics->work, drv2667_worker);
+
+	haptics->client = client;
+	i2c_set_clientdata(client, haptics);
+
+	haptics->regmap = devm_regmap_init_i2c(client, &drv2667_regmap_config);
+	if (IS_ERR(haptics->regmap)) {
+		error = PTR_ERR(haptics->regmap);
+		dev_err(&client->dev, "Failed to allocate register map: %d\n",
+			error);
+		return error;
+	}
+
+	error = drv2667_init(haptics);
+	if (error) {
+		dev_err(&client->dev, "Device init failed: %d\n", error);
+		return error;
+	}
+
+	error = input_register_device(haptics->input_dev);
+	if (error) {
+		dev_err(&client->dev, "couldn't register input device: %d\n",
+			error);
+		return error;
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int drv2667_suspend(struct device *dev)
+{
+	struct drv2667_data *haptics = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&haptics->input_dev->mutex);
+
+	if (haptics->input_dev->users) {
+		ret = regmap_update_bits(haptics->regmap, DRV2667_CTRL_2,
+				DRV2667_STANDBY, 1);
+		if (ret) {
+			dev_err(dev, "Failed to set standby mode\n");
+			regulator_disable(haptics->regulator);
+			goto out;
+		}
+
+		ret = regulator_disable(haptics->regulator);
+		if (ret) {
+			dev_err(dev, "Failed to disable regulator\n");
+			regmap_update_bits(haptics->regmap,
+					   DRV2667_CTRL_2,
+					   DRV2667_STANDBY, 0);
+		}
+	}
+out:
+	mutex_unlock(&haptics->input_dev->mutex);
+	return ret;
+}
+
+static int drv2667_resume(struct device *dev)
+{
+	struct drv2667_data *haptics = dev_get_drvdata(dev);
+	int ret = 0;
+
+	mutex_lock(&haptics->input_dev->mutex);
+
+	if (haptics->input_dev->users) {
+		ret = regulator_enable(haptics->regulator);
+		if (ret) {
+			dev_err(dev, "Failed to enable regulator\n");
+			goto out;
+		}
+
+		ret = regmap_update_bits(haptics->regmap, DRV2667_CTRL_2,
+					 DRV2667_STANDBY, 0);
+		if (ret) {
+			dev_err(dev, "Failed to unset standby mode\n");
+			regulator_disable(haptics->regulator);
+			goto out;
+		}
+
+	}
+
+out:
+	mutex_unlock(&haptics->input_dev->mutex);
+	return ret;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(drv2667_pm_ops, drv2667_suspend, drv2667_resume);
+
+static const struct i2c_device_id drv2667_id[] = {
+	{ "drv2667", 0 },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, drv2667_id);
+
+#ifdef CONFIG_OF
+static const struct of_device_id drv2667_of_match[] = {
+	{ .compatible = "ti,drv2667", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, drv2667_of_match);
+#endif
+
+static struct i2c_driver drv2667_driver = {
+	.probe		= drv2667_probe,
+	.driver		= {
+		.name	= "drv2667-haptics",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(drv2667_of_match),
+		.pm	= &drv2667_pm_ops,
+	},
+	.id_table = drv2667_id,
+};
+module_i2c_driver(drv2667_driver);
+
+MODULE_ALIAS("platform:drv2667-haptics");
+MODULE_DESCRIPTION("TI DRV2667 haptics driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Dan Murphy <dmurphy@ti.com>");
diff --git a/drivers/input/misc/max77693-haptic.c b/drivers/input/misc/max77693-haptic.c
new file mode 100644
index 000000000000..7b1fde93799e
--- /dev/null
+++ b/drivers/input/misc/max77693-haptic.c
@@ -0,0 +1,357 @@
+/*
+ * MAXIM MAX77693 Haptic device driver
+ *
+ * Copyright (C) 2014 Samsung Electronics
+ * Jaewon Kim <jaewon02.kim@samsung.com>
+ *
+ * This program is not provided / owned by Maxim Integrated Products.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/regmap.h>
+#include <linux/input.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/pwm.h>
+#include <linux/slab.h>
+#include <linux/workqueue.h>
+#include <linux/regulator/consumer.h>
+#include <linux/mfd/max77693.h>
+#include <linux/mfd/max77693-private.h>
+
+#define MAX_MAGNITUDE_SHIFT	16
+
+enum max77693_haptic_motor_type {
+	MAX77693_HAPTIC_ERM = 0,
+	MAX77693_HAPTIC_LRA,
+};
+
+enum max77693_haptic_pulse_mode {
+	MAX77693_HAPTIC_EXTERNAL_MODE = 0,
+	MAX77693_HAPTIC_INTERNAL_MODE,
+};
+
+enum max77693_haptic_pwm_divisor {
+	MAX77693_HAPTIC_PWM_DIVISOR_32 = 0,
+	MAX77693_HAPTIC_PWM_DIVISOR_64,
+	MAX77693_HAPTIC_PWM_DIVISOR_128,
+	MAX77693_HAPTIC_PWM_DIVISOR_256,
+};
+
+struct max77693_haptic {
+	struct regmap *regmap_pmic;
+	struct regmap *regmap_haptic;
+	struct device *dev;
+	struct input_dev *input_dev;
+	struct pwm_device *pwm_dev;
+	struct regulator *motor_reg;
+
+	bool enabled;
+	bool suspend_state;
+	unsigned int magnitude;
+	unsigned int pwm_duty;
+	enum max77693_haptic_motor_type type;
+	enum max77693_haptic_pulse_mode mode;
+	enum max77693_haptic_pwm_divisor pwm_divisor;
+
+	struct work_struct work;
+};
+
+static int max77693_haptic_set_duty_cycle(struct max77693_haptic *haptic)
+{
+	int delta = (haptic->pwm_dev->period + haptic->pwm_duty) / 2;
+	int error;
+
+	error = pwm_config(haptic->pwm_dev, delta, haptic->pwm_dev->period);
+	if (error) {
+		dev_err(haptic->dev, "failed to configure pwm: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int max77693_haptic_configure(struct max77693_haptic *haptic,
+				     bool enable)
+{
+	unsigned int value;
+	int error;
+
+	value = ((haptic->type << MAX77693_CONFIG2_MODE) |
+		(enable << MAX77693_CONFIG2_MEN) |
+		(haptic->mode << MAX77693_CONFIG2_HTYP) |
+		(haptic->pwm_divisor));
+
+	error = regmap_write(haptic->regmap_haptic,
+			     MAX77693_HAPTIC_REG_CONFIG2, value);
+	if (error) {
+		dev_err(haptic->dev,
+			"failed to update haptic config: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static int max77693_haptic_lowsys(struct max77693_haptic *haptic, bool enable)
+{
+	int error;
+
+	error = regmap_update_bits(haptic->regmap_pmic,
+				   MAX77693_PMIC_REG_LSCNFG,
+				   MAX77693_PMIC_LOW_SYS_MASK,
+				   enable << MAX77693_PMIC_LOW_SYS_SHIFT);
+	if (error) {
+		dev_err(haptic->dev, "cannot update pmic regmap: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static void max77693_haptic_enable(struct max77693_haptic *haptic)
+{
+	int error;
+
+	if (haptic->enabled)
+		return;
+
+	error = pwm_enable(haptic->pwm_dev);
+	if (error) {
+		dev_err(haptic->dev,
+			"failed to enable haptic pwm device: %d\n", error);
+		return;
+	}
+
+	error = max77693_haptic_lowsys(haptic, true);
+	if (error)
+		goto err_enable_lowsys;
+
+	error = max77693_haptic_configure(haptic, true);
+	if (error)
+		goto err_enable_config;
+
+	haptic->enabled = true;
+
+	return;
+
+err_enable_config:
+	max77693_haptic_lowsys(haptic, false);
+err_enable_lowsys:
+	pwm_disable(haptic->pwm_dev);
+}
+
+static void max77693_haptic_disable(struct max77693_haptic *haptic)
+{
+	int error;
+
+	if (!haptic->enabled)
+		return;
+
+	error = max77693_haptic_configure(haptic, false);
+	if (error)
+		return;
+
+	error = max77693_haptic_lowsys(haptic, false);
+	if (error)
+		goto err_disable_lowsys;
+
+	pwm_disable(haptic->pwm_dev);
+	haptic->enabled = false;
+
+	return;
+
+err_disable_lowsys:
+	max77693_haptic_configure(haptic, true);
+}
+
+static void max77693_haptic_play_work(struct work_struct *work)
+{
+	struct max77693_haptic *haptic =
+			container_of(work, struct max77693_haptic, work);
+	int error;
+
+	error = max77693_haptic_set_duty_cycle(haptic);
+	if (error) {
+		dev_err(haptic->dev, "failed to set duty cycle: %d\n", error);
+		return;
+	}
+
+	if (haptic->magnitude)
+		max77693_haptic_enable(haptic);
+	else
+		max77693_haptic_disable(haptic);
+}
+
+static int max77693_haptic_play_effect(struct input_dev *dev, void *data,
+				       struct ff_effect *effect)
+{
+	struct max77693_haptic *haptic = input_get_drvdata(dev);
+	uint64_t period_mag_multi;
+
+	haptic->magnitude = effect->u.rumble.strong_magnitude;
+	if (!haptic->magnitude)
+		haptic->magnitude = effect->u.rumble.weak_magnitude;
+
+	/*
+	 * The magnitude comes from force-feedback interface.
+	 * The formula to convert magnitude to pwm_duty as follows:
+	 * - pwm_duty = (magnitude * pwm_period) / MAX_MAGNITUDE(0xFFFF)
+	 */
+	period_mag_multi = (int64_t)(haptic->pwm_dev->period *
+						haptic->magnitude);
+	haptic->pwm_duty = (unsigned int)(period_mag_multi >>
+						MAX_MAGNITUDE_SHIFT);
+
+	schedule_work(&haptic->work);
+
+	return 0;
+}
+
+static int max77693_haptic_open(struct input_dev *dev)
+{
+	struct max77693_haptic *haptic = input_get_drvdata(dev);
+	int error;
+
+	error = regulator_enable(haptic->motor_reg);
+	if (error) {
+		dev_err(haptic->dev,
+			"failed to enable regulator: %d\n", error);
+		return error;
+	}
+
+	return 0;
+}
+
+static void max77693_haptic_close(struct input_dev *dev)
+{
+	struct max77693_haptic *haptic = input_get_drvdata(dev);
+	int error;
+
+	cancel_work_sync(&haptic->work);
+	max77693_haptic_disable(haptic);
+
+	error = regulator_disable(haptic->motor_reg);
+	if (error)
+		dev_err(haptic->dev,
+			"failed to disable regulator: %d\n", error);
+}
+
+static int max77693_haptic_probe(struct platform_device *pdev)
+{
+	struct max77693_dev *max77693 = dev_get_drvdata(pdev->dev.parent);
+	struct max77693_haptic *haptic;
+	int error;
+
+	haptic = devm_kzalloc(&pdev->dev, sizeof(*haptic), GFP_KERNEL);
+	if (!haptic)
+		return -ENOMEM;
+
+	haptic->regmap_pmic = max77693->regmap;
+	haptic->regmap_haptic = max77693->regmap_haptic;
+	haptic->dev = &pdev->dev;
+	haptic->type = MAX77693_HAPTIC_LRA;
+	haptic->mode = MAX77693_HAPTIC_EXTERNAL_MODE;
+	haptic->pwm_divisor = MAX77693_HAPTIC_PWM_DIVISOR_128;
+	haptic->suspend_state = false;
+
+	INIT_WORK(&haptic->work, max77693_haptic_play_work);
+
+	/* Get pwm and regulatot for haptic device */
+	haptic->pwm_dev = devm_pwm_get(&pdev->dev, NULL);
+	if (IS_ERR(haptic->pwm_dev)) {
+		dev_err(&pdev->dev, "failed to get pwm device\n");
+		return PTR_ERR(haptic->pwm_dev);
+	}
+
+	haptic->motor_reg = devm_regulator_get(&pdev->dev, "haptic");
+	if (IS_ERR(haptic->motor_reg)) {
+		dev_err(&pdev->dev, "failed to get regulator\n");
+		return PTR_ERR(haptic->motor_reg);
+	}
+
+	/* Initialize input device for haptic device */
+	haptic->input_dev = devm_input_allocate_device(&pdev->dev);
+	if (!haptic->input_dev) {
+		dev_err(&pdev->dev, "failed to allocate input device\n");
+		return -ENOMEM;
+	}
+
+	haptic->input_dev->name = "max77693-haptic";
+	haptic->input_dev->id.version = 1;
+	haptic->input_dev->dev.parent = &pdev->dev;
+	haptic->input_dev->open = max77693_haptic_open;
+	haptic->input_dev->close = max77693_haptic_close;
+	input_set_drvdata(haptic->input_dev, haptic);
+	input_set_capability(haptic->input_dev, EV_FF, FF_RUMBLE);
+
+	error = input_ff_create_memless(haptic->input_dev, NULL,
+				max77693_haptic_play_effect);
+	if (error) {
+		dev_err(&pdev->dev, "failed to create force-feedback\n");
+		return error;
+	}
+
+	error = input_register_device(haptic->input_dev);
+	if (error) {
+		dev_err(&pdev->dev, "failed to register input device\n");
+		return error;
+	}
+
+	platform_set_drvdata(pdev, haptic);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int max77693_haptic_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct max77693_haptic *haptic = platform_get_drvdata(pdev);
+
+	if (haptic->enabled) {
+		max77693_haptic_disable(haptic);
+		haptic->suspend_state = true;
+	}
+
+	return 0;
+}
+
+static int max77693_haptic_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct max77693_haptic *haptic = platform_get_drvdata(pdev);
+
+	if (haptic->suspend_state) {
+		max77693_haptic_enable(haptic);
+		haptic->suspend_state = false;
+	}
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(max77693_haptic_pm_ops,
+			 max77693_haptic_suspend, max77693_haptic_resume);
+
+static struct platform_driver max77693_haptic_driver = {
+	.driver		= {
+		.name	= "max77693-haptic",
+		.owner	= THIS_MODULE,
+		.pm	= &max77693_haptic_pm_ops,
+	},
+	.probe		= max77693_haptic_probe,
+};
+module_platform_driver(max77693_haptic_driver);
+
+MODULE_AUTHOR("Jaewon Kim <jaewon02.kim@samsung.com>");
+MODULE_DESCRIPTION("MAXIM MAX77693 Haptic driver");
+MODULE_ALIAS("platform:max77693-haptic");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/palmas-pwrbutton.c b/drivers/input/misc/palmas-pwrbutton.c
new file mode 100644
index 000000000000..f505ac3a8d87
--- /dev/null
+++ b/drivers/input/misc/palmas-pwrbutton.c
@@ -0,0 +1,332 @@
+/*
+ * Texas Instruments' Palmas Power Button Input Driver
+ *
+ * Copyright (C) 2012-2014 Texas Instruments Incorporated - http://www.ti.com/
+ *	Girish S Ghongdemath
+ *	Nishanth Menon
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/input.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/mfd/palmas.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define PALMAS_LPK_TIME_MASK		0x0c
+#define PALMAS_PWRON_DEBOUNCE_MASK	0x03
+#define PALMAS_PWR_KEY_Q_TIME_MS	20
+
+/**
+ * struct palmas_pwron - Palmas power on data
+ * @palmas:		pointer to palmas device
+ * @input_dev:		pointer to input device
+ * @input_work:		work for detecting release of key
+ * @irq:		irq that we are hooked on to
+ */
+struct palmas_pwron {
+	struct palmas *palmas;
+	struct input_dev *input_dev;
+	struct delayed_work input_work;
+	int irq;
+};
+
+/**
+ * struct palmas_pwron_config - configuration of palmas power on
+ * @long_press_time_val:	value for long press h/w shutdown event
+ * @pwron_debounce_val:		value for debounce of power button
+ */
+struct palmas_pwron_config {
+	u8 long_press_time_val;
+	u8 pwron_debounce_val;
+};
+
+/**
+ * palmas_power_button_work() - Detects the button release event
+ * @work:	work item to detect button release
+ */
+static void palmas_power_button_work(struct work_struct *work)
+{
+	struct palmas_pwron *pwron = container_of(work,
+						  struct palmas_pwron,
+						  input_work.work);
+	struct input_dev *input_dev = pwron->input_dev;
+	unsigned int reg;
+	int error;
+
+	error = palmas_read(pwron->palmas, PALMAS_INTERRUPT_BASE,
+			    PALMAS_INT1_LINE_STATE, &reg);
+	if (error) {
+		dev_err(input_dev->dev.parent,
+			"Cannot read palmas PWRON status: %d\n", error);
+	} else if (reg & BIT(1)) {
+		/* The button is released, report event. */
+		input_report_key(input_dev, KEY_POWER, 0);
+		input_sync(input_dev);
+	} else {
+		/* The button is still depressed, keep checking. */
+		schedule_delayed_work(&pwron->input_work,
+				msecs_to_jiffies(PALMAS_PWR_KEY_Q_TIME_MS));
+	}
+}
+
+/**
+ * pwron_irq() - button press isr
+ * @irq:		irq
+ * @palmas_pwron:	pwron struct
+ *
+ * Return: IRQ_HANDLED
+ */
+static irqreturn_t pwron_irq(int irq, void *palmas_pwron)
+{
+	struct palmas_pwron *pwron = palmas_pwron;
+	struct input_dev *input_dev = pwron->input_dev;
+
+	input_report_key(input_dev, KEY_POWER, 1);
+	pm_wakeup_event(input_dev->dev.parent, 0);
+	input_sync(input_dev);
+
+	mod_delayed_work(system_wq, &pwron->input_work,
+			 msecs_to_jiffies(PALMAS_PWR_KEY_Q_TIME_MS));
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * palmas_pwron_params_ofinit() - device tree parameter parser
+ * @dev:	palmas button device
+ * @config:	configuration params that this fills up
+ */
+static void palmas_pwron_params_ofinit(struct device *dev,
+				       struct palmas_pwron_config *config)
+{
+	struct device_node *np;
+	u32 val;
+	int i, error;
+	u8 lpk_times[] = { 6, 8, 10, 12 };
+	int pwr_on_deb_ms[] = { 15, 100, 500, 1000 };
+
+	memset(config, 0, sizeof(*config));
+
+	/* Default config parameters */
+	config->long_press_time_val = ARRAY_SIZE(lpk_times) - 1;
+
+	np = dev->of_node;
+	if (!np)
+		return;
+
+	error = of_property_read_u32(np, "ti,palmas-long-press-seconds", &val);
+	if (!error) {
+		for (i = 0; i < ARRAY_SIZE(lpk_times); i++) {
+			if (val <= lpk_times[i]) {
+				config->long_press_time_val = i;
+				break;
+			}
+		}
+	}
+
+	error = of_property_read_u32(np,
+				     "ti,palmas-pwron-debounce-milli-seconds",
+				     &val);
+	if (!error) {
+		for (i = 0; i < ARRAY_SIZE(pwr_on_deb_ms); i++) {
+			if (val <= pwr_on_deb_ms[i]) {
+				config->pwron_debounce_val = i;
+				break;
+			}
+		}
+	}
+
+	dev_info(dev, "h/w controlled shutdown duration=%d seconds\n",
+		 lpk_times[config->long_press_time_val]);
+}
+
+/**
+ * palmas_pwron_probe() - probe
+ * @pdev:	platform device for the button
+ *
+ * Return: 0 for successful probe else appropriate error
+ */
+static int palmas_pwron_probe(struct platform_device *pdev)
+{
+	struct palmas *palmas = dev_get_drvdata(pdev->dev.parent);
+	struct device *dev = &pdev->dev;
+	struct input_dev *input_dev;
+	struct palmas_pwron *pwron;
+	struct palmas_pwron_config config;
+	int val;
+	int error;
+
+	palmas_pwron_params_ofinit(dev, &config);
+
+	pwron = kzalloc(sizeof(*pwron), GFP_KERNEL);
+	if (!pwron)
+		return -ENOMEM;
+
+	input_dev = input_allocate_device();
+	if (!input_dev) {
+		dev_err(dev, "Can't allocate power button\n");
+		error = -ENOMEM;
+		goto err_free_mem;
+	}
+
+	input_dev->name = "palmas_pwron";
+	input_dev->phys = "palmas_pwron/input0";
+	input_dev->dev.parent = dev;
+
+	input_set_capability(input_dev, EV_KEY, KEY_POWER);
+
+	/*
+	 * Setup default hardware shutdown option (long key press)
+	 * and debounce.
+	 */
+	val = config.long_press_time_val << __ffs(PALMAS_LPK_TIME_MASK);
+	val |= config.pwron_debounce_val << __ffs(PALMAS_PWRON_DEBOUNCE_MASK);
+	error = palmas_update_bits(palmas, PALMAS_PMU_CONTROL_BASE,
+				   PALMAS_LONG_PRESS_KEY,
+				   PALMAS_LPK_TIME_MASK |
+					PALMAS_PWRON_DEBOUNCE_MASK,
+				   val);
+	if (error) {
+		dev_err(dev, "LONG_PRESS_KEY_UPDATE failed: %d\n", error);
+		goto err_free_input;
+	}
+
+	pwron->palmas = palmas;
+	pwron->input_dev = input_dev;
+
+	INIT_DELAYED_WORK(&pwron->input_work, palmas_power_button_work);
+
+	pwron->irq = platform_get_irq(pdev, 0);
+	error = request_threaded_irq(pwron->irq, NULL, pwron_irq,
+				     IRQF_TRIGGER_HIGH |
+					IRQF_TRIGGER_LOW |
+					IRQF_ONESHOT,
+				     dev_name(dev), pwron);
+	if (error) {
+		dev_err(dev, "Can't get IRQ for pwron: %d\n", error);
+		goto err_free_input;
+	}
+
+	error = input_register_device(input_dev);
+	if (error) {
+		dev_err(dev, "Can't register power button: %d\n", error);
+		goto err_free_irq;
+	}
+
+	platform_set_drvdata(pdev, pwron);
+	device_init_wakeup(dev, true);
+
+	return 0;
+
+err_free_irq:
+	cancel_delayed_work_sync(&pwron->input_work);
+	free_irq(pwron->irq, pwron);
+err_free_input:
+	input_free_device(input_dev);
+err_free_mem:
+	kfree(pwron);
+	return error;
+}
+
+/**
+ * palmas_pwron_remove() - Cleanup on removal
+ * @pdev:	platform device for the button
+ *
+ * Return: 0
+ */
+static int palmas_pwron_remove(struct platform_device *pdev)
+{
+	struct palmas_pwron *pwron = platform_get_drvdata(pdev);
+
+	free_irq(pwron->irq, pwron);
+	cancel_delayed_work_sync(&pwron->input_work);
+
+	input_unregister_device(pwron->input_dev);
+	kfree(pwron);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+/**
+ * palmas_pwron_suspend() - suspend handler
+ * @dev:	power button device
+ *
+ * Cancel all pending work items for the power button, setup irq for wakeup
+ *
+ * Return: 0
+ */
+static int palmas_pwron_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct palmas_pwron *pwron = platform_get_drvdata(pdev);
+
+	cancel_delayed_work_sync(&pwron->input_work);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(pwron->irq);
+
+	return 0;
+}
+
+/**
+ * palmas_pwron_resume() - resume handler
+ * @dev:	power button device
+ *
+ * Just disable the wakeup capability of irq here.
+ *
+ * Return: 0
+ */
+static int palmas_pwron_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct palmas_pwron *pwron = platform_get_drvdata(pdev);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(pwron->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(palmas_pwron_pm,
+			 palmas_pwron_suspend, palmas_pwron_resume);
+
+#ifdef CONFIG_OF
+static struct of_device_id of_palmas_pwr_match[] = {
+	{ .compatible = "ti,palmas-pwrbutton" },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(of, of_palmas_pwr_match);
+#endif
+
+static struct platform_driver palmas_pwron_driver = {
+	.probe	= palmas_pwron_probe,
+	.remove	= palmas_pwron_remove,
+	.driver	= {
+		.name	= "palmas_pwrbutton",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(of_palmas_pwr_match),
+		.pm	= &palmas_pwron_pm,
+	},
+};
+module_platform_driver(palmas_pwron_driver);
+
+MODULE_ALIAS("platform:palmas-pwrbutton");
+MODULE_DESCRIPTION("Palmas Power Button");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Texas Instruments Inc.");
diff --git a/drivers/input/misc/soc_button_array.c b/drivers/input/misc/soc_button_array.c
index e34dfc29beb3..735604753568 100644
--- a/drivers/input/misc/soc_button_array.c
+++ b/drivers/input/misc/soc_button_array.c
@@ -18,7 +18,7 @@
 #include <linux/gpio/consumer.h>
 #include <linux/gpio_keys.h>
 #include <linux/platform_device.h>
-#include <linux/pnp.h>
+#include <linux/acpi.h>
 
 /*
  * Definition of buttons on the tablet. The ACPI index of each button
@@ -67,7 +67,7 @@ static int soc_button_lookup_gpio(struct device *dev, int acpi_index)
 }
 
 static struct platform_device *
-soc_button_device_create(struct pnp_dev *pdev,
+soc_button_device_create(struct platform_device *pdev,
 			 const struct soc_button_info *button_info,
 			 bool autorepeat)
 {
@@ -138,30 +138,40 @@ err_free_mem:
 	return ERR_PTR(error);
 }
 
-static void soc_button_remove(struct pnp_dev *pdev)
+static int soc_button_remove(struct platform_device *pdev)
 {
-	struct soc_button_data *priv = pnp_get_drvdata(pdev);
+	struct soc_button_data *priv = platform_get_drvdata(pdev);
+
 	int i;
 
 	for (i = 0; i < BUTTON_TYPES; i++)
 		if (priv->children[i])
 			platform_device_unregister(priv->children[i]);
+
+	return 0;
 }
 
-static int soc_button_pnp_probe(struct pnp_dev *pdev,
-				const struct pnp_device_id *id)
+static int soc_button_probe(struct platform_device *pdev)
 {
-	const struct soc_button_info *button_info = (void *)id->driver_data;
+	struct device *dev = &pdev->dev;
+	const struct acpi_device_id *id;
+	struct soc_button_info *button_info;
 	struct soc_button_data *priv;
 	struct platform_device *pd;
 	int i;
 	int error;
 
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+	if (!id)
+		return -ENODEV;
+
+	button_info = (struct soc_button_info *)id->driver_data;
+
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
-	pnp_set_drvdata(pdev, priv);
+	platform_set_drvdata(pdev, priv);
 
 	for (i = 0; i < BUTTON_TYPES; i++) {
 		pd = soc_button_device_create(pdev, button_info, i == 0);
@@ -192,30 +202,22 @@ static struct soc_button_info soc_button_PNP0C40[] = {
 	{ }
 };
 
-static const struct pnp_device_id soc_button_pnp_match[] = {
-	{ .id = "PNP0C40", .driver_data = (long)soc_button_PNP0C40 },
-	{ .id = "" }
+static const struct acpi_device_id soc_button_acpi_match[] = {
+	{ "PNP0C40", (unsigned long)soc_button_PNP0C40 },
+	{ }
 };
-MODULE_DEVICE_TABLE(pnp, soc_button_pnp_match);
 
-static struct pnp_driver soc_button_pnp_driver = {
-	.name		= KBUILD_MODNAME,
-	.id_table	= soc_button_pnp_match,
-	.probe          = soc_button_pnp_probe,
+MODULE_DEVICE_TABLE(acpi, soc_button_acpi_match);
+
+static struct platform_driver soc_button_driver = {
+	.probe          = soc_button_probe,
 	.remove		= soc_button_remove,
+	.driver		= {
+		.name = KBUILD_MODNAME,
+		.owner = THIS_MODULE,
+		.acpi_match_table = ACPI_PTR(soc_button_acpi_match),
+	},
 };
-
-static int __init soc_button_init(void)
-{
-	return pnp_register_driver(&soc_button_pnp_driver);
-}
-
-static void __exit soc_button_exit(void)
-{
-	pnp_unregister_driver(&soc_button_pnp_driver);
-}
-
-module_init(soc_button_init);
-module_exit(soc_button_exit);
+module_platform_driver(soc_button_driver);
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/input/misc/xen-kbdfront.c b/drivers/input/misc/xen-kbdfront.c
index fbfdc10573be..95599e478e19 100644
--- a/drivers/input/misc/xen-kbdfront.c
+++ b/drivers/input/misc/xen-kbdfront.c
@@ -285,7 +285,7 @@ static int xenkbd_connect_backend(struct xenbus_device *dev,
  error_evtchan:
 	xenbus_free_evtchn(dev, evtchn);
  error_grant:
-	gnttab_end_foreign_access_ref(info->gref, 0);
+	gnttab_end_foreign_access(info->gref, 0, 0UL);
 	info->gref = -1;
 	return ret;
 }
@@ -296,7 +296,7 @@ static void xenkbd_disconnect_backend(struct xenkbd_info *info)
 		unbind_from_irqhandler(info->irq, info);
 	info->irq = -1;
 	if (info->gref >= 0)
-		gnttab_end_foreign_access_ref(info->gref, 0);
+		gnttab_end_foreign_access(info->gref, 0, 0UL);
 	info->gref = -1;
 }
 
@@ -365,12 +365,13 @@ static const struct xenbus_device_id xenkbd_ids[] = {
 	{ "" }
 };
 
-static DEFINE_XENBUS_DRIVER(xenkbd, ,
+static struct xenbus_driver xenkbd_driver = {
+	.ids = xenkbd_ids,
 	.probe = xenkbd_probe,
 	.remove = xenkbd_remove,
 	.resume = xenkbd_resume,
 	.otherend_changed = xenkbd_backend_changed,
-);
+};
 
 static int __init xenkbd_init(void)
 {
diff --git a/drivers/input/mouse/Makefile b/drivers/input/mouse/Makefile
index c25efdb3f288..dda507f8b3a2 100644
--- a/drivers/input/mouse/Makefile
+++ b/drivers/input/mouse/Makefile
@@ -23,7 +23,7 @@ obj-$(CONFIG_MOUSE_SYNAPTICS_I2C)	+= synaptics_i2c.o
 obj-$(CONFIG_MOUSE_SYNAPTICS_USB)	+= synaptics_usb.o
 obj-$(CONFIG_MOUSE_VSXXXAA)		+= vsxxxaa.o
 
-psmouse-objs := psmouse-base.o synaptics.o
+psmouse-objs := psmouse-base.o synaptics.o focaltech.o
 
 psmouse-$(CONFIG_MOUSE_PS2_ALPS)	+= alps.o
 psmouse-$(CONFIG_MOUSE_PS2_ELANTECH)	+= elantech.o
diff --git a/drivers/input/mouse/alps.c b/drivers/input/mouse/alps.c
index a59a1a64b674..2b0ae8cc8e51 100644
--- a/drivers/input/mouse/alps.c
+++ b/drivers/input/mouse/alps.c
@@ -835,8 +835,8 @@ static void alps_process_packet_v4(struct psmouse *psmouse)
 		f->fingers = alps_process_bitmap(priv, f);
 	}
 
-	f->left = packet[4] & 0x01;
-	f->right = packet[4] & 0x02;
+	f->left = !!(packet[4] & 0x01);
+	f->right = !!(packet[4] & 0x02);
 
 	f->st.x = ((packet[1] & 0x7f) << 4) | ((packet[3] & 0x30) >> 2) |
 		  ((packet[0] & 0x30) >> 4);
@@ -2234,8 +2234,8 @@ static int alps_identify(struct psmouse *psmouse, struct alps_data *priv)
 		return 0;
 	}
 
-	psmouse_info(psmouse,
-		     "Unknown ALPS touchpad: E7=%3ph, EC=%3ph\n", e7, ec);
+	psmouse_dbg(psmouse,
+		    "Likely not an ALPS touchpad: E7=%3ph, EC=%3ph\n", e7, ec);
 
 	return -EINVAL;
 }
@@ -2373,6 +2373,10 @@ int alps_init(struct psmouse *psmouse)
 	dev2->keybit[BIT_WORD(BTN_LEFT)] =
 		BIT_MASK(BTN_LEFT) | BIT_MASK(BTN_MIDDLE) | BIT_MASK(BTN_RIGHT);
 
+	__set_bit(INPUT_PROP_POINTER, dev2->propbit);
+	if (priv->flags & ALPS_DUALPOINT)
+		__set_bit(INPUT_PROP_POINTING_STICK, dev2->propbit);
+
 	if (input_register_device(priv->dev2))
 		goto init_fail;
 
diff --git a/drivers/input/mouse/elantech.c b/drivers/input/mouse/elantech.c
index ee2a04d90d20..06fc6e76ffbe 100644
--- a/drivers/input/mouse/elantech.c
+++ b/drivers/input/mouse/elantech.c
@@ -18,6 +18,7 @@
 #include <linux/input/mt.h>
 #include <linux/serio.h>
 #include <linux/libps2.h>
+#include <asm/unaligned.h>
 #include "psmouse.h"
 #include "elantech.h"
 
@@ -403,6 +404,68 @@ static void elantech_report_absolute_v2(struct psmouse *psmouse)
 	input_sync(dev);
 }
 
+static void elantech_report_trackpoint(struct psmouse *psmouse,
+				       int packet_type)
+{
+	/*
+	 * byte 0:  0   0  sx  sy   0   M   R   L
+	 * byte 1:~sx   0   0   0   0   0   0   0
+	 * byte 2:~sy   0   0   0   0   0   0   0
+	 * byte 3:  0   0 ~sy ~sx   0   1   1   0
+	 * byte 4: x7  x6  x5  x4  x3  x2  x1  x0
+	 * byte 5: y7  y6  y5  y4  y3  y2  y1  y0
+	 *
+	 * x and y are written in two's complement spread
+	 * over 9 bits with sx/sy the relative top bit and
+	 * x7..x0 and y7..y0 the lower bits.
+	 * The sign of y is opposite to what the input driver
+	 * expects for a relative movement
+	 */
+
+	struct elantech_data *etd = psmouse->private;
+	struct input_dev *tp_dev = etd->tp_dev;
+	unsigned char *packet = psmouse->packet;
+	int x, y;
+	u32 t;
+
+	if (dev_WARN_ONCE(&psmouse->ps2dev.serio->dev,
+			  !tp_dev,
+			  psmouse_fmt("Unexpected trackpoint message\n"))) {
+		if (etd->debug == 1)
+			elantech_packet_dump(psmouse);
+		return;
+	}
+
+	t = get_unaligned_le32(&packet[0]);
+
+	switch (t & ~7U) {
+	case 0x06000030U:
+	case 0x16008020U:
+	case 0x26800010U:
+	case 0x36808000U:
+		x = packet[4] - (int)((packet[1]^0x80) << 1);
+		y = (int)((packet[2]^0x80) << 1) - packet[5];
+
+		input_report_key(tp_dev, BTN_LEFT, packet[0] & 0x01);
+		input_report_key(tp_dev, BTN_RIGHT, packet[0] & 0x02);
+		input_report_key(tp_dev, BTN_MIDDLE, packet[0] & 0x04);
+
+		input_report_rel(tp_dev, REL_X, x);
+		input_report_rel(tp_dev, REL_Y, y);
+
+		input_sync(tp_dev);
+
+		break;
+
+	default:
+		/* Dump unexpected packet sequences if debug=1 (default) */
+		if (etd->debug == 1)
+			elantech_packet_dump(psmouse);
+
+		break;
+	}
+}
+
 /*
  * Interpret complete data packets and report absolute mode input events for
  * hardware version 3. (12 byte packets for two fingers)
@@ -715,6 +778,8 @@ static int elantech_packet_check_v3(struct psmouse *psmouse)
 
 		if ((packet[0] & 0x0c) == 0x0c && (packet[3] & 0xce) == 0x0c)
 			return PACKET_V3_TAIL;
+		if ((packet[3] & 0x0f) == 0x06)
+			return PACKET_TRACKPOINT;
 	}
 
 	return PACKET_UNKNOWN;
@@ -791,14 +856,23 @@ static psmouse_ret_t elantech_process_byte(struct psmouse *psmouse)
 
 	case 3:
 		packet_type = elantech_packet_check_v3(psmouse);
-		/* ignore debounce */
-		if (packet_type == PACKET_DEBOUNCE)
-			return PSMOUSE_FULL_PACKET;
-
-		if (packet_type == PACKET_UNKNOWN)
+		switch (packet_type) {
+		case PACKET_UNKNOWN:
 			return PSMOUSE_BAD_DATA;
 
-		elantech_report_absolute_v3(psmouse, packet_type);
+		case PACKET_DEBOUNCE:
+			/* ignore debounce */
+			break;
+
+		case PACKET_TRACKPOINT:
+			elantech_report_trackpoint(psmouse, packet_type);
+			break;
+
+		default:
+			elantech_report_absolute_v3(psmouse, packet_type);
+			break;
+		}
+
 		break;
 
 	case 4:
@@ -1018,8 +1092,10 @@ static int elantech_get_resolution_v4(struct psmouse *psmouse,
  * Asus UX31               0x361f00        20, 15, 0e      clickpad
  * Asus UX32VD             0x361f02        00, 15, 0e      clickpad
  * Avatar AVIU-145A2       0x361f00        ?               clickpad
+ * Fujitsu H730            0x570f00        c0, 14, 0c      3 hw buttons (**)
  * Gigabyte U2442          0x450f01        58, 17, 0c      2 hw buttons
  * Lenovo L430             0x350f02        b9, 15, 0c      2 hw buttons (*)
+ * Lenovo L530             0x350f02        b9, 15, 0c      2 hw buttons (*)
  * Samsung NF210           0x150b00        78, 14, 0a      2 hw buttons
  * Samsung NP770Z5E        0x575f01        10, 15, 0f      clickpad
  * Samsung NP700Z5B        0x361f06        21, 15, 0f      clickpad
@@ -1029,6 +1105,8 @@ static int elantech_get_resolution_v4(struct psmouse *psmouse,
  * Samsung RF710           0x450f00        ?               2 hw buttons
  * System76 Pangolin       0x250f01        ?               2 hw buttons
  * (*) + 3 trackpoint buttons
+ * (**) + 0 trackpoint buttons
+ * Note: Lenovo L430 and Lenovo L430 have the same fw_version/caps
  */
 static void elantech_set_buttonpad_prop(struct psmouse *psmouse)
 {
@@ -1253,6 +1331,13 @@ static bool elantech_is_signature_valid(const unsigned char *param)
 	if (param[1] == 0)
 		return true;
 
+	/*
+	 * Some models have a revision higher then 20. Meaning param[2] may
+	 * be 10 or 20, skip the rates check for these.
+	 */
+	if (param[0] == 0x46 && (param[1] & 0xef) == 0x0f && param[2] < 40)
+		return true;
+
 	for (i = 0; i < ARRAY_SIZE(rates); i++)
 		if (param[2] == rates[i])
 			return false;
@@ -1324,6 +1409,10 @@ int elantech_detect(struct psmouse *psmouse, bool set_properties)
  */
 static void elantech_disconnect(struct psmouse *psmouse)
 {
+	struct elantech_data *etd = psmouse->private;
+
+	if (etd->tp_dev)
+		input_unregister_device(etd->tp_dev);
 	sysfs_remove_group(&psmouse->ps2dev.serio->dev.kobj,
 			   &elantech_attr_group);
 	kfree(psmouse->private);
@@ -1438,8 +1527,10 @@ static int elantech_set_properties(struct elantech_data *etd)
 int elantech_init(struct psmouse *psmouse)
 {
 	struct elantech_data *etd;
-	int i, error;
+	int i;
+	int error = -EINVAL;
 	unsigned char param[3];
+	struct input_dev *tp_dev;
 
 	psmouse->private = etd = kzalloc(sizeof(struct elantech_data), GFP_KERNEL);
 	if (!etd)
@@ -1498,14 +1589,53 @@ int elantech_init(struct psmouse *psmouse)
 		goto init_fail;
 	}
 
+	/* The MSB indicates the presence of the trackpoint */
+	if ((etd->capabilities[0] & 0x80) == 0x80) {
+		tp_dev = input_allocate_device();
+
+		if (!tp_dev) {
+			error = -ENOMEM;
+			goto init_fail_tp_alloc;
+		}
+
+		etd->tp_dev = tp_dev;
+		snprintf(etd->tp_phys, sizeof(etd->tp_phys), "%s/input1",
+			psmouse->ps2dev.serio->phys);
+		tp_dev->phys = etd->tp_phys;
+		tp_dev->name = "Elantech PS/2 TrackPoint";
+		tp_dev->id.bustype = BUS_I8042;
+		tp_dev->id.vendor  = 0x0002;
+		tp_dev->id.product = PSMOUSE_ELANTECH;
+		tp_dev->id.version = 0x0000;
+		tp_dev->dev.parent = &psmouse->ps2dev.serio->dev;
+		tp_dev->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REL);
+		tp_dev->relbit[BIT_WORD(REL_X)] =
+			BIT_MASK(REL_X) | BIT_MASK(REL_Y);
+		tp_dev->keybit[BIT_WORD(BTN_LEFT)] =
+			BIT_MASK(BTN_LEFT) | BIT_MASK(BTN_MIDDLE) |
+			BIT_MASK(BTN_RIGHT);
+
+		__set_bit(INPUT_PROP_POINTER, tp_dev->propbit);
+		__set_bit(INPUT_PROP_POINTING_STICK, tp_dev->propbit);
+
+		error = input_register_device(etd->tp_dev);
+		if (error < 0)
+			goto init_fail_tp_reg;
+	}
+
 	psmouse->protocol_handler = elantech_process_byte;
 	psmouse->disconnect = elantech_disconnect;
 	psmouse->reconnect = elantech_reconnect;
 	psmouse->pktsize = etd->hw_version > 1 ? 6 : 4;
 
 	return 0;
-
+ init_fail_tp_reg:
+	input_free_device(tp_dev);
+ init_fail_tp_alloc:
+	sysfs_remove_group(&psmouse->ps2dev.serio->dev.kobj,
+			   &elantech_attr_group);
  init_fail:
+	psmouse_reset(psmouse);
 	kfree(etd);
-	return -1;
+	return error;
 }
diff --git a/drivers/input/mouse/elantech.h b/drivers/input/mouse/elantech.h
index 9e0e2a1f340d..6f3afec02f03 100644
--- a/drivers/input/mouse/elantech.h
+++ b/drivers/input/mouse/elantech.h
@@ -94,6 +94,7 @@
 #define PACKET_V4_HEAD			0x05
 #define PACKET_V4_MOTION		0x06
 #define PACKET_V4_STATUS		0x07
+#define PACKET_TRACKPOINT		0x08
 
 /*
  * track up to 5 fingers for v4 hardware
@@ -114,6 +115,8 @@ struct finger_pos {
 };
 
 struct elantech_data {
+	struct input_dev *tp_dev;	/* Relative device for trackpoint */
+	char tp_phys[32];
 	unsigned char reg_07;
 	unsigned char reg_10;
 	unsigned char reg_11;
diff --git a/drivers/input/mouse/focaltech.c b/drivers/input/mouse/focaltech.c
new file mode 100644
index 000000000000..f4d657ee1cc0
--- /dev/null
+++ b/drivers/input/mouse/focaltech.c
@@ -0,0 +1,52 @@
+/*
+ * Focaltech TouchPad PS/2 mouse driver
+ *
+ * Copyright (c) 2014 Red Hat Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Red Hat authors:
+ *
+ * Hans de Goede <hdegoede@redhat.com>
+ */
+
+/*
+ * The Focaltech PS/2 touchpad protocol is unknown. This drivers deals with
+ * detection only, to avoid further detection attempts confusing the touchpad
+ * this way it at least works in PS/2 mouse compatibility mode.
+ */
+
+#include <linux/device.h>
+#include <linux/libps2.h>
+#include "psmouse.h"
+
+static const char * const focaltech_pnp_ids[] = {
+	"FLT0101",
+	"FLT0102",
+	"FLT0103",
+	NULL
+};
+
+int focaltech_detect(struct psmouse *psmouse, bool set_properties)
+{
+	if (!psmouse_matches_pnp_id(psmouse, focaltech_pnp_ids))
+		return -ENODEV;
+
+	if (set_properties) {
+		psmouse->vendor = "FocalTech";
+		psmouse->name = "FocalTech Touchpad in mouse emulation mode";
+	}
+
+	return 0;
+}
+
+int focaltech_init(struct psmouse *psmouse)
+{
+	ps2_command(&psmouse->ps2dev, NULL, PSMOUSE_CMD_RESET_DIS);
+	psmouse_reset(psmouse);
+
+	return 0;
+}
diff --git a/drivers/input/mouse/focaltech.h b/drivers/input/mouse/focaltech.h
new file mode 100644
index 000000000000..498650c61e28
--- /dev/null
+++ b/drivers/input/mouse/focaltech.h
@@ -0,0 +1,22 @@
+/*
+ * Focaltech TouchPad PS/2 mouse driver
+ *
+ * Copyright (c) 2014 Red Hat Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Red Hat authors:
+ *
+ * Hans de Goede <hdegoede@redhat.com>
+ */
+
+#ifndef _FOCALTECH_H
+#define _FOCALTECH_H
+
+int focaltech_detect(struct psmouse *psmouse, bool set_properties);
+int focaltech_init(struct psmouse *psmouse);
+
+#endif
diff --git a/drivers/input/mouse/psmouse-base.c b/drivers/input/mouse/psmouse-base.c
index cff065f6261c..26994f6a2b2a 100644
--- a/drivers/input/mouse/psmouse-base.c
+++ b/drivers/input/mouse/psmouse-base.c
@@ -35,6 +35,7 @@
 #include "elantech.h"
 #include "sentelic.h"
 #include "cypress_ps2.h"
+#include "focaltech.h"
 
 #define DRIVER_DESC	"PS/2 mouse driver"
 
@@ -462,6 +463,20 @@ static int psmouse_poll(struct psmouse *psmouse)
 			   PSMOUSE_CMD_POLL | (psmouse->pktsize << 8));
 }
 
+/*
+ * psmouse_matches_pnp_id - check if psmouse matches one of the passed in ids.
+ */
+bool psmouse_matches_pnp_id(struct psmouse *psmouse, const char * const ids[])
+{
+	int i;
+
+	if (!strncmp(psmouse->ps2dev.serio->firmware_id, "PNP:", 4))
+		for (i = 0; ids[i]; i++)
+			if (strstr(psmouse->ps2dev.serio->firmware_id, ids[i]))
+				return true;
+
+	return false;
+}
 
 /*
  * Genius NetMouse magic init.
@@ -670,6 +685,8 @@ static void psmouse_apply_defaults(struct psmouse *psmouse)
 	__set_bit(REL_X, input_dev->relbit);
 	__set_bit(REL_Y, input_dev->relbit);
 
+	__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+
 	psmouse->set_rate = psmouse_set_rate;
 	psmouse->set_resolution = psmouse_set_resolution;
 	psmouse->poll = psmouse_poll;
@@ -706,6 +723,21 @@ static int psmouse_extensions(struct psmouse *psmouse,
 {
 	bool synaptics_hardware = false;
 
+/* Always check for focaltech, this is safe as it uses pnp-id matching */
+	if (psmouse_do_detect(focaltech_detect, psmouse, set_properties) == 0) {
+		if (!set_properties || focaltech_init(psmouse) == 0) {
+			/*
+			 * Not supported yet, use bare protocol.
+			 * Note that we need to also restrict
+			 * psmouse_max_proto so that psmouse_initialize()
+			 * does not try to reset rate and resolution,
+			 * because even that upsets the device.
+			 */
+			psmouse_max_proto = PSMOUSE_PS2;
+			return PSMOUSE_PS2;
+		}
+	}
+
 /*
  * We always check for lifebook because it does not disturb mouse
  * (it only checks DMI information).
diff --git a/drivers/input/mouse/psmouse.h b/drivers/input/mouse/psmouse.h
index 2f0b39d59a9b..f4cf664c7db3 100644
--- a/drivers/input/mouse/psmouse.h
+++ b/drivers/input/mouse/psmouse.h
@@ -108,6 +108,7 @@ void psmouse_set_resolution(struct psmouse *psmouse, unsigned int resolution);
 psmouse_ret_t psmouse_process_byte(struct psmouse *psmouse);
 int psmouse_activate(struct psmouse *psmouse);
 int psmouse_deactivate(struct psmouse *psmouse);
+bool psmouse_matches_pnp_id(struct psmouse *psmouse, const char * const ids[]);
 
 struct psmouse_attribute {
 	struct device_attribute dattr;
diff --git a/drivers/input/mouse/synaptics.c b/drivers/input/mouse/synaptics.c
index e8573c68f77e..9031a0a28ea4 100644
--- a/drivers/input/mouse/synaptics.c
+++ b/drivers/input/mouse/synaptics.c
@@ -185,18 +185,6 @@ static const char * const topbuttonpad_pnp_ids[] = {
 	NULL
 };
 
-static bool matches_pnp_id(struct psmouse *psmouse, const char * const ids[])
-{
-	int i;
-
-	if (!strncmp(psmouse->ps2dev.serio->firmware_id, "PNP:", 4))
-		for (i = 0; ids[i]; i++)
-			if (strstr(psmouse->ps2dev.serio->firmware_id, ids[i]))
-				return true;
-
-	return false;
-}
-
 /*****************************************************************************
  *	Synaptics communications functions
  ****************************************************************************/
@@ -362,7 +350,8 @@ static int synaptics_resolution(struct psmouse *psmouse)
 	}
 
 	for (i = 0; min_max_pnpid_table[i].pnp_ids; i++) {
-		if (matches_pnp_id(psmouse, min_max_pnpid_table[i].pnp_ids)) {
+		if (psmouse_matches_pnp_id(psmouse,
+					   min_max_pnpid_table[i].pnp_ids)) {
 			priv->x_min = min_max_pnpid_table[i].x_min;
 			priv->x_max = min_max_pnpid_table[i].x_max;
 			priv->y_min = min_max_pnpid_table[i].y_min;
@@ -618,6 +607,8 @@ static void synaptics_parse_agm(const unsigned char buf[],
 	priv->agm_pending = true;
 }
 
+static bool is_forcepad;
+
 static int synaptics_parse_hw_state(const unsigned char buf[],
 				    struct synaptics_data *priv,
 				    struct synaptics_hw_state *hw)
@@ -629,10 +620,61 @@ static int synaptics_parse_hw_state(const unsigned char buf[],
 			 ((buf[0] & 0x04) >> 1) |
 			 ((buf[3] & 0x04) >> 2));
 
+		if ((SYN_CAP_ADV_GESTURE(priv->ext_cap_0c) ||
+			SYN_CAP_IMAGE_SENSOR(priv->ext_cap_0c)) &&
+		    hw->w == 2) {
+			synaptics_parse_agm(buf, priv, hw);
+			return 1;
+		}
+
+		hw->x = (((buf[3] & 0x10) << 8) |
+			 ((buf[1] & 0x0f) << 8) |
+			 buf[4]);
+		hw->y = (((buf[3] & 0x20) << 7) |
+			 ((buf[1] & 0xf0) << 4) |
+			 buf[5]);
+		hw->z = buf[2];
+
 		hw->left  = (buf[0] & 0x01) ? 1 : 0;
 		hw->right = (buf[0] & 0x02) ? 1 : 0;
 
-		if (SYN_CAP_CLICKPAD(priv->ext_cap_0c)) {
+		if (is_forcepad) {
+			/*
+			 * ForcePads, like Clickpads, use middle button
+			 * bits to report primary button clicks.
+			 * Unfortunately they report primary button not
+			 * only when user presses on the pad above certain
+			 * threshold, but also when there are more than one
+			 * finger on the touchpad, which interferes with
+			 * out multi-finger gestures.
+			 */
+			if (hw->z == 0) {
+				/* No contacts */
+				priv->press = priv->report_press = false;
+			} else if (hw->w >= 4 && ((buf[0] ^ buf[3]) & 0x01)) {
+				/*
+				 * Single-finger touch with pressure above
+				 * the threshold. If pressure stays long
+				 * enough, we'll start reporting primary
+				 * button. We rely on the device continuing
+				 * sending data even if finger does not
+				 * move.
+				 */
+				if  (!priv->press) {
+					priv->press_start = jiffies;
+					priv->press = true;
+				} else if (time_after(jiffies,
+						priv->press_start +
+							msecs_to_jiffies(50))) {
+					priv->report_press = true;
+				}
+			} else {
+				priv->press = false;
+			}
+
+			hw->left = priv->report_press;
+
+		} else if (SYN_CAP_CLICKPAD(priv->ext_cap_0c)) {
 			/*
 			 * Clickpad's button is transmitted as middle button,
 			 * however, since it is primary button, we will report
@@ -651,21 +693,6 @@ static int synaptics_parse_hw_state(const unsigned char buf[],
 			hw->down = ((buf[0] ^ buf[3]) & 0x02) ? 1 : 0;
 		}
 
-		if ((SYN_CAP_ADV_GESTURE(priv->ext_cap_0c) ||
-			SYN_CAP_IMAGE_SENSOR(priv->ext_cap_0c)) &&
-		    hw->w == 2) {
-			synaptics_parse_agm(buf, priv, hw);
-			return 1;
-		}
-
-		hw->x = (((buf[3] & 0x10) << 8) |
-			 ((buf[1] & 0x0f) << 8) |
-			 buf[4]);
-		hw->y = (((buf[3] & 0x20) << 7) |
-			 ((buf[1] & 0xf0) << 4) |
-			 buf[5]);
-		hw->z = buf[2];
-
 		if (SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap) &&
 		    ((buf[0] ^ buf[3]) & 0x02)) {
 			switch (SYN_CAP_MULTI_BUTTON_NO(priv->ext_cap) & ~0x01) {
@@ -1456,7 +1483,7 @@ static void set_input_params(struct psmouse *psmouse,
 
 	if (SYN_CAP_CLICKPAD(priv->ext_cap_0c)) {
 		__set_bit(INPUT_PROP_BUTTONPAD, dev->propbit);
-		if (matches_pnp_id(psmouse, topbuttonpad_pnp_ids))
+		if (psmouse_matches_pnp_id(psmouse, topbuttonpad_pnp_ids))
 			__set_bit(INPUT_PROP_TOPBUTTONPAD, dev->propbit);
 		/* Clickpads report only left button */
 		__clear_bit(BTN_RIGHT, dev->keybit);
@@ -1642,11 +1669,29 @@ static const struct dmi_system_id __initconst cr48_dmi_table[] = {
 	{ }
 };
 
+static const struct dmi_system_id forcepad_dmi_table[] __initconst = {
+#if defined(CONFIG_DMI) && defined(CONFIG_X86)
+	{
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "HP EliteBook Folio 1040 G1"),
+		},
+	},
+#endif
+	{ }
+};
+
 void __init synaptics_module_init(void)
 {
 	impaired_toshiba_kbc = dmi_check_system(toshiba_dmi_table);
 	broken_olpc_ec = dmi_check_system(olpc_dmi_table);
 	cr48_profile_sensor = dmi_check_system(cr48_dmi_table);
+
+	/*
+	 * Unfortunately ForcePad capability is not exported over PS/2,
+	 * so we have to resort to checking DMI.
+	 */
+	is_forcepad = dmi_check_system(forcepad_dmi_table);
 }
 
 static int __synaptics_init(struct psmouse *psmouse, bool absolute_mode)
diff --git a/drivers/input/mouse/synaptics.h b/drivers/input/mouse/synaptics.h
index e594af0b264b..1bd01f21783b 100644
--- a/drivers/input/mouse/synaptics.h
+++ b/drivers/input/mouse/synaptics.h
@@ -77,6 +77,8 @@
  *					for noise.
  * 2	0x08	image sensor		image sensor tracks 5 fingers, but only
  *					reports 2.
+ * 2	0x01	uniform clickpad	whole clickpad moves instead of being
+ *					hinged at the top.
  * 2	0x20	report min		query 0x0f gives min coord reported
  */
 #define SYN_CAP_CLICKPAD(ex0c)		((ex0c) & 0x100000) /* 1-button ClickPad */
@@ -177,6 +179,11 @@ struct synaptics_data {
 	 */
 	struct synaptics_hw_state agm;
 	bool agm_pending;			/* new AGM packet received */
+
+	/* ForcePad handling */
+	unsigned long				press_start;
+	bool					press;
+	bool					report_press;
 };
 
 void synaptics_module_init(void);
diff --git a/drivers/input/mouse/synaptics_usb.c b/drivers/input/mouse/synaptics_usb.c
index e122bda16aab..6bcc0189c1c9 100644
--- a/drivers/input/mouse/synaptics_usb.c
+++ b/drivers/input/mouse/synaptics_usb.c
@@ -387,6 +387,7 @@ static int synusb_probe(struct usb_interface *intf,
 		__set_bit(EV_REL, input_dev->evbit);
 		__set_bit(REL_X, input_dev->relbit);
 		__set_bit(REL_Y, input_dev->relbit);
+		__set_bit(INPUT_PROP_POINTING_STICK, input_dev->propbit);
 		input_set_abs_params(input_dev, ABS_PRESSURE, 0, 127, 0, 0);
 	} else {
 		input_set_abs_params(input_dev, ABS_X,
@@ -401,6 +402,11 @@ static int synusb_probe(struct usb_interface *intf,
 		__set_bit(BTN_TOOL_TRIPLETAP, input_dev->keybit);
 	}
 
+	if (synusb->flags & SYNUSB_TOUCHSCREEN)
+		__set_bit(INPUT_PROP_DIRECT, input_dev->propbit);
+	else
+		__set_bit(INPUT_PROP_POINTER, input_dev->propbit);
+
 	__set_bit(BTN_LEFT, input_dev->keybit);
 	__set_bit(BTN_RIGHT, input_dev->keybit);
 	__set_bit(BTN_MIDDLE, input_dev->keybit);
diff --git a/drivers/input/mouse/trackpoint.c b/drivers/input/mouse/trackpoint.c
index ca843b6cf6bd..30c8b6998808 100644
--- a/drivers/input/mouse/trackpoint.c
+++ b/drivers/input/mouse/trackpoint.c
@@ -393,6 +393,9 @@ int trackpoint_detect(struct psmouse *psmouse, bool set_properties)
 	if ((button_info & 0x0f) >= 3)
 		__set_bit(BTN_MIDDLE, psmouse->dev->keybit);
 
+	__set_bit(INPUT_PROP_POINTER, psmouse->dev->propbit);
+	__set_bit(INPUT_PROP_POINTING_STICK, psmouse->dev->propbit);
+
 	trackpoint_defaults(psmouse->private);
 
 	error = trackpoint_power_on_reset(&psmouse->ps2dev);
diff --git a/drivers/input/serio/i8042-sparcio.h b/drivers/input/serio/i8042-sparcio.h
index d6aa4c67dbb6..93cb7912703c 100644
--- a/drivers/input/serio/i8042-sparcio.h
+++ b/drivers/input/serio/i8042-sparcio.h
@@ -17,7 +17,6 @@ static int i8042_aux_irq = -1;
 #define I8042_MUX_PHYS_DESC "sparcps2/serio%d"
 
 static void __iomem *kbd_iobase;
-static struct resource *kbd_res;
 
 #define I8042_COMMAND_REG	(kbd_iobase + 0x64UL)
 #define I8042_DATA_REG		(kbd_iobase + 0x60UL)
@@ -44,6 +43,8 @@ static inline void i8042_write_command(int val)
 
 #ifdef CONFIG_PCI
 
+static struct resource *kbd_res;
+
 #define OBP_PS2KBD_NAME1	"kb_ps2"
 #define OBP_PS2KBD_NAME2	"keyboard"
 #define OBP_PS2MS_NAME1		"kdmouse"
diff --git a/drivers/input/serio/i8042-x86ia64io.h b/drivers/input/serio/i8042-x86ia64io.h
index 136b7b204f56..a0bcbb64d06d 100644
--- a/drivers/input/serio/i8042-x86ia64io.h
+++ b/drivers/input/serio/i8042-x86ia64io.h
@@ -101,6 +101,12 @@ static const struct dmi_system_id __initconst i8042_dmi_noloop_table[] = {
 	},
 	{
 		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "ASUSTeK COMPUTER INC."),
+			DMI_MATCH(DMI_PRODUCT_NAME, "X750LN"),
+		},
+	},
+	{
+		.matches = {
 			DMI_MATCH(DMI_SYS_VENDOR, "Compaq"),
 			DMI_MATCH(DMI_PRODUCT_NAME , "ProLiant"),
 			DMI_MATCH(DMI_PRODUCT_VERSION, "8500"),
@@ -201,268 +207,17 @@ static const struct dmi_system_id __initconst i8042_dmi_noloop_table[] = {
 };
 
 /*
- * Some Fujitsu notebooks are having trouble with touchpads if
- * active multiplexing mode is activated. Luckily they don't have
- * external PS/2 ports so we can safely disable it.
- * ... apparently some Toshibas don't like MUX mode either and
- * die horrible death on reboot.
+ * Some laptops do implement active multiplexing mode correctly;
+ * unfortunately they are in minority.
  */
-static const struct dmi_system_id __initconst i8042_dmi_nomux_table[] = {
-	{
-		/* Fujitsu Lifebook P7010/P7010D */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "P7010"),
-		},
-	},
-	{
-		/* Fujitsu Lifebook P7010 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU SIEMENS"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "0000000000"),
-		},
-	},
-	{
-		/* Fujitsu Lifebook P5020D */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "LifeBook P Series"),
-		},
-	},
-	{
-		/* Fujitsu Lifebook S2000 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "LifeBook S Series"),
-		},
-	},
-	{
-		/* Fujitsu Lifebook S6230 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "LifeBook S6230"),
-		},
-	},
-	{
-		/* Fujitsu T70H */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "FMVLT70H"),
-		},
-	},
-	{
-		/* Fujitsu-Siemens Lifebook T3010 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU SIEMENS"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK T3010"),
-		},
-	},
-	{
-		/* Fujitsu-Siemens Lifebook E4010 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU SIEMENS"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK E4010"),
-		},
-	},
-	{
-		/* Fujitsu-Siemens Amilo Pro 2010 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU SIEMENS"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "AMILO Pro V2010"),
-		},
-	},
-	{
-		/* Fujitsu-Siemens Amilo Pro 2030 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU SIEMENS"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "AMILO PRO V2030"),
-		},
-	},
-	{
-		/*
-		 * No data is coming from the touchscreen unless KBC
-		 * is in legacy mode.
-		 */
-		/* Panasonic CF-29 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Matsushita"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "CF-29"),
-		},
-	},
+static const struct dmi_system_id __initconst i8042_dmi_mux_table[] = {
 	{
 		/*
-		 * HP Pavilion DV4017EA -
-		 * errors on MUX ports are reported without raising AUXDATA
-		 * causing "spurious NAK" messages.
+		 * Panasonic CF-18 needs to be in MUX mode since the
+		 * touchscreen is on serio3 and it also has touchpad.
 		 */
 		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Pavilion dv4000 (EA032EA#ABF)"),
-		},
-	},
-	{
-		/*
-		 * HP Pavilion ZT1000 -
-		 * like DV4017EA does not raise AUXERR for errors on MUX ports.
-		 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "HP Pavilion Notebook PC"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "HP Pavilion Notebook ZT1000"),
-		},
-	},
-	{
-		/*
-		 * HP Pavilion DV4270ca -
-		 * like DV4017EA does not raise AUXERR for errors on MUX ports.
-		 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Pavilion dv4000 (EH476UA#ABL)"),
-		},
-	},
-	{
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Satellite P10"),
-		},
-	},
-	{
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "EQUIUM A110"),
-		},
-	},
-	{
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "TOSHIBA"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "SATELLITE C850D"),
-		},
-	},
-	{
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "ALIENWARE"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Sentia"),
-		},
-	},
-	{
-		/* Sharp Actius MM20 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "SHARP"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "PC-MM20 Series"),
-		},
-	},
-	{
-		/* Sony Vaio FS-115b */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Sony Corporation"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "VGN-FS115B"),
-		},
-	},
-	{
-		/*
-		 * Sony Vaio FZ-240E -
-		 * reset and GET ID commands issued via KBD port are
-		 * sometimes being delivered to AUX3.
-		 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Sony Corporation"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "VGN-FZ240E"),
-		},
-	},
-	{
-		/*
-		 * Most (all?) VAIOs do not have external PS/2 ports nor
-		 * they implement active multiplexing properly, and
-		 * MUX discovery usually messes up keyboard/touchpad.
-		 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Sony Corporation"),
-			DMI_MATCH(DMI_BOARD_NAME, "VAIO"),
-		},
-	},
-	{
-		/* Amoi M636/A737 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Amoi Electronics CO.,LTD."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "M636/A737 platform"),
-		},
-	},
-	{
-		/* Lenovo 3000 n100 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "076804U"),
-		},
-	},
-	{
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 1360"),
-		},
-	},
-	{
-		/* Acer Aspire 5710 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 5710"),
-		},
-	},
-	{
-		/* Gericom Bellagio */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Gericom"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "N34AS6"),
-		},
-	},
-	{
-		/* IBM 2656 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "IBM"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "2656"),
-		},
-	},
-	{
-		/* Dell XPS M1530 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "XPS M1530"),
-		},
-	},
-	{
-		/* Compal HEL80I */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "COMPAL"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "HEL80I"),
-		},
-	},
-	{
-		/* Dell Vostro 1510 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Vostro1510"),
-		},
-	},
-	{
-		/* Acer Aspire 5536 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Acer"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Aspire 5536"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "0100"),
-		},
-	},
-	{
-		/* Dell Vostro V13 */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Dell Inc."),
-			DMI_MATCH(DMI_PRODUCT_NAME, "Vostro V13"),
-		},
-	},
-	{
-		/* Newer HP Pavilion dv4 models */
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "Hewlett-Packard"),
-			DMI_MATCH(DMI_PRODUCT_NAME, "HP Pavilion dv4 Notebook PC"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "CF-18"),
 		},
 	},
 	{ }
@@ -608,6 +363,14 @@ static const struct dmi_system_id __initconst i8042_dmi_notimeout_table[] = {
 			DMI_MATCH(DMI_PRODUCT_NAME, "HP Pavilion dv4 Notebook PC"),
 		},
 	},
+	{
+		/* Fujitsu U574 laptop */
+		/* https://bugzilla.kernel.org/show_bug.cgi?id=69731 */
+		.matches = {
+			DMI_MATCH(DMI_SYS_VENDOR, "FUJITSU"),
+			DMI_MATCH(DMI_PRODUCT_NAME, "LIFEBOOK U574"),
+		},
+	},
 	{ }
 };
 
@@ -977,8 +740,8 @@ static int __init i8042_platform_init(void)
 	if (dmi_check_system(i8042_dmi_noloop_table))
 		i8042_noloop = true;
 
-	if (dmi_check_system(i8042_dmi_nomux_table))
-		i8042_nomux = true;
+	if (dmi_check_system(i8042_dmi_mux_table))
+		i8042_nomux = false;
 
 	if (dmi_check_system(i8042_dmi_notimeout_table))
 		i8042_notimeout = true;
diff --git a/drivers/input/serio/i8042.c b/drivers/input/serio/i8042.c
index 3807c3e971cc..9a97c2b10926 100644
--- a/drivers/input/serio/i8042.c
+++ b/drivers/input/serio/i8042.c
@@ -39,7 +39,7 @@ static bool i8042_noaux;
 module_param_named(noaux, i8042_noaux, bool, 0);
 MODULE_PARM_DESC(noaux, "Do not probe or use AUX (mouse) port.");
 
-static bool i8042_nomux;
+static bool i8042_nomux = true;
 module_param_named(nomux, i8042_nomux, bool, 0);
 MODULE_PARM_DESC(nomux, "Do not check whether an active multiplexing controller is present.");
 
@@ -1254,6 +1254,8 @@ static int __init i8042_create_aux_port(int idx)
 	} else {
 		snprintf(serio->name, sizeof(serio->name), "i8042 AUX%d port", idx);
 		snprintf(serio->phys, sizeof(serio->phys), I8042_MUX_PHYS_DESC, idx + 1);
+		strlcpy(serio->firmware_id, i8042_aux_firmware_id,
+			sizeof(serio->firmware_id));
 	}
 
 	port->serio = serio;
diff --git a/drivers/input/serio/serio.c b/drivers/input/serio/serio.c
index b29134de983b..d399b8b0f000 100644
--- a/drivers/input/serio/serio.c
+++ b/drivers/input/serio/serio.c
@@ -524,8 +524,8 @@ static void serio_init_port(struct serio *serio)
 	spin_lock_init(&serio->lock);
 	mutex_init(&serio->drv_mutex);
 	device_initialize(&serio->dev);
-	dev_set_name(&serio->dev, "serio%ld",
-			(long)atomic_inc_return(&serio_no) - 1);
+	dev_set_name(&serio->dev, "serio%lu",
+		     (unsigned long)atomic_inc_return(&serio_no) - 1);
 	serio->dev.bus = &serio_bus;
 	serio->dev.release = serio_release_port;
 	serio->dev.groups = serio_device_attr_groups;
diff --git a/drivers/input/serio/serport.c b/drivers/input/serio/serport.c
index 0cb7ef59071b..69175b825346 100644
--- a/drivers/input/serio/serport.c
+++ b/drivers/input/serio/serport.c
@@ -21,6 +21,7 @@
 #include <linux/init.h>
 #include <linux/serio.h>
 #include <linux/tty.h>
+#include <linux/compat.h>
 
 MODULE_AUTHOR("Vojtech Pavlik <vojtech@ucw.cz>");
 MODULE_DESCRIPTION("Input device TTY line discipline");
@@ -198,28 +199,55 @@ static ssize_t serport_ldisc_read(struct tty_struct * tty, struct file * file, u
 	return 0;
 }
 
+static void serport_set_type(struct tty_struct *tty, unsigned long type)
+{
+	struct serport *serport = tty->disc_data;
+
+	serport->id.proto = type & 0x000000ff;
+	serport->id.id    = (type & 0x0000ff00) >> 8;
+	serport->id.extra = (type & 0x00ff0000) >> 16;
+}
+
 /*
  * serport_ldisc_ioctl() allows to set the port protocol, and device ID
  */
 
-static int serport_ldisc_ioctl(struct tty_struct * tty, struct file * file, unsigned int cmd, unsigned long arg)
+static int serport_ldisc_ioctl(struct tty_struct *tty, struct file *file,
+			       unsigned int cmd, unsigned long arg)
 {
-	struct serport *serport = (struct serport*) tty->disc_data;
-	unsigned long type;
-
 	if (cmd == SPIOCSTYPE) {
+		unsigned long type;
+
 		if (get_user(type, (unsigned long __user *) arg))
 			return -EFAULT;
 
-		serport->id.proto = type & 0x000000ff;
-		serport->id.id	  = (type & 0x0000ff00) >> 8;
-		serport->id.extra = (type & 0x00ff0000) >> 16;
+		serport_set_type(tty, type);
+		return 0;
+	}
+
+	return -EINVAL;
+}
+
+#ifdef CONFIG_COMPAT
+#define COMPAT_SPIOCSTYPE	_IOW('q', 0x01, compat_ulong_t)
+static long serport_ldisc_compat_ioctl(struct tty_struct *tty,
+				       struct file *file,
+				       unsigned int cmd, unsigned long arg)
+{
+	if (cmd == COMPAT_SPIOCSTYPE) {
+		void __user *uarg = compat_ptr(arg);
+		compat_ulong_t compat_type;
+
+		if (get_user(compat_type, (compat_ulong_t __user *)uarg))
+			return -EFAULT;
 
+		serport_set_type(tty, compat_type);
 		return 0;
 	}
 
 	return -EINVAL;
 }
+#endif
 
 static void serport_ldisc_write_wakeup(struct tty_struct * tty)
 {
@@ -243,6 +271,9 @@ static struct tty_ldisc_ops serport_ldisc = {
 	.close =	serport_ldisc_close,
 	.read =		serport_ldisc_read,
 	.ioctl =	serport_ldisc_ioctl,
+#ifdef CONFIG_COMPAT
+	.compat_ioctl =	serport_ldisc_compat_ioctl,
+#endif
 	.receive_buf =	serport_ldisc_receive,
 	.write_wakeup =	serport_ldisc_write_wakeup
 };
diff --git a/drivers/input/touchscreen/Kconfig b/drivers/input/touchscreen/Kconfig
index 6bb9a7dd23b6..e1d8003d01f8 100644
--- a/drivers/input/touchscreen/Kconfig
+++ b/drivers/input/touchscreen/Kconfig
@@ -90,6 +90,18 @@ config TOUCHSCREEN_AD7879_SPI
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad7879-spi.
 
+config TOUCHSCREEN_AR1021_I2C
+	tristate "Microchip AR1021 i2c touchscreen"
+	depends on I2C && OF
+	help
+	  Say Y here if you have the Microchip AR1021 touchscreen controller
+	  chip in your system.
+
+	  If unsure, say N.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called ar1021_i2c.
+
 config TOUCHSCREEN_ATMEL_MXT
 	tristate "Atmel mXT I2C Touchscreen"
 	depends on I2C
diff --git a/drivers/input/touchscreen/Makefile b/drivers/input/touchscreen/Makefile
index 4be94fce41af..090e61cc9171 100644
--- a/drivers/input/touchscreen/Makefile
+++ b/drivers/input/touchscreen/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_TOUCHSCREEN_AD7879)	+= ad7879.o
 obj-$(CONFIG_TOUCHSCREEN_AD7879_I2C)	+= ad7879-i2c.o
 obj-$(CONFIG_TOUCHSCREEN_AD7879_SPI)	+= ad7879-spi.o
 obj-$(CONFIG_TOUCHSCREEN_ADS7846)	+= ads7846.o
+obj-$(CONFIG_TOUCHSCREEN_AR1021_I2C)	+= ar1021_i2c.o
 obj-$(CONFIG_TOUCHSCREEN_ATMEL_MXT)	+= atmel_mxt_ts.o
 obj-$(CONFIG_TOUCHSCREEN_AUO_PIXCIR)	+= auo-pixcir-ts.o
 obj-$(CONFIG_TOUCHSCREEN_BU21013)	+= bu21013_ts.o
diff --git a/drivers/input/touchscreen/ad7879.c b/drivers/input/touchscreen/ad7879.c
index fce590677b7b..1eb9d3c20886 100644
--- a/drivers/input/touchscreen/ad7879.c
+++ b/drivers/input/touchscreen/ad7879.c
@@ -470,14 +470,10 @@ static int ad7879_gpio_add(struct ad7879 *ts,
 static void ad7879_gpio_remove(struct ad7879 *ts)
 {
 	const struct ad7879_platform_data *pdata = dev_get_platdata(ts->dev);
-	int ret;
 
-	if (pdata->gpio_export) {
-		ret = gpiochip_remove(&ts->gc);
-		if (ret)
-			dev_err(ts->dev, "failed to remove gpio %d\n",
-				ts->gc.base);
-	}
+	if (pdata->gpio_export)
+		gpiochip_remove(&ts->gc);
+
 }
 #else
 static inline int ad7879_gpio_add(struct ad7879 *ts,
diff --git a/drivers/input/touchscreen/ar1021_i2c.c b/drivers/input/touchscreen/ar1021_i2c.c
new file mode 100644
index 000000000000..ba30578e296e
--- /dev/null
+++ b/drivers/input/touchscreen/ar1021_i2c.c
@@ -0,0 +1,181 @@
+/*
+ * Microchip AR1021 driver for I2C
+ *
+ * Author: Christian Gmeiner <christian.gmeiner@gmail.com>
+ *
+ * License: GPLv2 as published by the FSF.
+ */
+
+#include <linux/module.h>
+#include <linux/input.h>
+#include <linux/of.h>
+#include <linux/i2c.h>
+#include <linux/irq.h>
+#include <linux/interrupt.h>
+
+#define AR1021_TOCUH_PKG_SIZE	5
+
+#define AR1021_MAX_X	4095
+#define AR1021_MAX_Y	4095
+
+struct ar1021_i2c {
+	struct i2c_client *client;
+	struct input_dev *input;
+	u8 data[AR1021_TOCUH_PKG_SIZE];
+};
+
+static irqreturn_t ar1021_i2c_irq(int irq, void *dev_id)
+{
+	struct ar1021_i2c *ar1021 = dev_id;
+	struct input_dev *input = ar1021->input;
+	u8 *data = ar1021->data;
+	unsigned int x, y, button;
+	int retval;
+
+	retval = i2c_master_recv(ar1021->client,
+				ar1021->data, sizeof(ar1021->data));
+	if (retval != sizeof(ar1021->data))
+		goto out;
+
+	/* sync bit set ? */
+	if ((data[0] & 0x80) == 0)
+		goto out;
+
+	button = data[0] & BIT(0);
+	x = ((data[2] & 0x1f) << 7) | (data[1] & 0x7f);
+	y = ((data[4] & 0x1f) << 7) | (data[3] & 0x7f);
+
+	input_report_abs(input, ABS_X, x);
+	input_report_abs(input, ABS_Y, y);
+	input_report_key(input, BTN_TOUCH, button);
+	input_sync(input);
+
+out:
+	return IRQ_HANDLED;
+}
+
+static int ar1021_i2c_open(struct input_dev *dev)
+{
+	struct ar1021_i2c *ar1021 = input_get_drvdata(dev);
+	struct i2c_client *client = ar1021->client;
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static void ar1021_i2c_close(struct input_dev *dev)
+{
+	struct ar1021_i2c *ar1021 = input_get_drvdata(dev);
+	struct i2c_client *client = ar1021->client;
+
+	disable_irq(client->irq);
+}
+
+static int ar1021_i2c_probe(struct i2c_client *client,
+				     const struct i2c_device_id *id)
+{
+	struct ar1021_i2c *ar1021;
+	struct input_dev *input;
+	int error;
+
+	if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) {
+		dev_err(&client->dev, "i2c_check_functionality error\n");
+		return -ENXIO;
+	}
+
+	ar1021 = devm_kzalloc(&client->dev, sizeof(*ar1021), GFP_KERNEL);
+	if (!ar1021)
+		return -ENOMEM;
+
+	input = devm_input_allocate_device(&client->dev);
+	if (!input)
+		return -ENOMEM;
+
+	ar1021->client = client;
+	ar1021->input = input;
+
+	input->name = "ar1021 I2C Touchscreen";
+	input->id.bustype = BUS_I2C;
+	input->dev.parent = &client->dev;
+	input->open = ar1021_i2c_open;
+	input->close = ar1021_i2c_close;
+
+	input_set_capability(input, EV_KEY, BTN_TOUCH);
+	input_set_abs_params(input, ABS_X, 0, AR1021_MAX_X, 0, 0);
+	input_set_abs_params(input, ABS_Y, 0, AR1021_MAX_Y, 0, 0);
+
+	input_set_drvdata(input, ar1021);
+
+	error = devm_request_threaded_irq(&client->dev, client->irq,
+					  NULL, ar1021_i2c_irq,
+					  IRQF_TRIGGER_RISING | IRQF_ONESHOT,
+					  "ar1021_i2c", ar1021);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to enable IRQ, error: %d\n", error);
+		return error;
+	}
+
+	/* Disable the IRQ, we'll enable it in ar1021_i2c_open() */
+	disable_irq(client->irq);
+
+	error = input_register_device(ar1021->input);
+	if (error) {
+		dev_err(&client->dev,
+			"Failed to register input device, error: %d\n", error);
+		return error;
+	}
+
+	i2c_set_clientdata(client, ar1021);
+	return 0;
+}
+
+static int __maybe_unused ar1021_i2c_suspend(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	disable_irq(client->irq);
+
+	return 0;
+}
+
+static int __maybe_unused ar1021_i2c_resume(struct device *dev)
+{
+	struct i2c_client *client = to_i2c_client(dev);
+
+	enable_irq(client->irq);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(ar1021_i2c_pm, ar1021_i2c_suspend, ar1021_i2c_resume);
+
+static const struct i2c_device_id ar1021_i2c_id[] = {
+	{ "MICROCHIP_AR1021_I2C", 0 },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, ar1021_i2c_id);
+
+static struct of_device_id ar1021_i2c_of_match[] = {
+	{ .compatible = "microchip,ar1021-i2c", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, ar1021_i2c_of_match);
+
+static struct i2c_driver ar1021_i2c_driver = {
+	.driver	= {
+		.name	= "ar1021_i2c",
+		.owner	= THIS_MODULE,
+		.pm	= &ar1021_i2c_pm,
+		.of_match_table = ar1021_i2c_of_match,
+	},
+
+	.probe		= ar1021_i2c_probe,
+	.id_table	= ar1021_i2c_id,
+};
+module_i2c_driver(ar1021_i2c_driver);
+
+MODULE_AUTHOR("Christian Gmeiner <christian.gmeiner@gmail.com>");
+MODULE_DESCRIPTION("Microchip AR1021 I2C Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/input/touchscreen/atmel_mxt_ts.c b/drivers/input/touchscreen/atmel_mxt_ts.c
index db178ed2b47e..aaacf8bfa61f 100644
--- a/drivers/input/touchscreen/atmel_mxt_ts.c
+++ b/drivers/input/touchscreen/atmel_mxt_ts.c
@@ -837,7 +837,12 @@ static irqreturn_t mxt_process_messages_t44(struct mxt_data *data)
 	count = data->msg_buf[0];
 
 	if (count == 0) {
-		dev_warn(dev, "Interrupt triggered but zero messages\n");
+		/*
+		 * This condition is caused by the CHG line being configured
+		 * in Mode 0. It results in unnecessary I2C operations but it
+		 * is benign.
+		 */
+		dev_dbg(dev, "Interrupt triggered but zero messages\n");
 		return IRQ_NONE;
 	} else if (count > data->max_reportid) {
 		dev_err(dev, "T44 count %d exceeded max report id\n", count);
@@ -1374,11 +1379,16 @@ static int mxt_get_info(struct mxt_data *data)
 	return 0;
 }
 
-static void mxt_free_object_table(struct mxt_data *data)
+static void mxt_free_input_device(struct mxt_data *data)
 {
-	input_unregister_device(data->input_dev);
-	data->input_dev = NULL;
+	if (data->input_dev) {
+		input_unregister_device(data->input_dev);
+		data->input_dev = NULL;
+	}
+}
 
+static void mxt_free_object_table(struct mxt_data *data)
+{
 	kfree(data->object_table);
 	data->object_table = NULL;
 	kfree(data->msg_buf);
@@ -1957,11 +1967,13 @@ static int mxt_load_fw(struct device *dev, const char *fn)
 		ret = mxt_lookup_bootloader_address(data, 0);
 		if (ret)
 			goto release_firmware;
+
+		mxt_free_input_device(data);
+		mxt_free_object_table(data);
 	} else {
 		enable_irq(data->irq);
 	}
 
-	mxt_free_object_table(data);
 	reinit_completion(&data->bl_completion);
 
 	ret = mxt_check_bootloader(data, MXT_WAITING_BOOTLOAD_CMD, false);
@@ -2210,6 +2222,7 @@ static int mxt_probe(struct i2c_client *client, const struct i2c_device_id *id)
 	return 0;
 
 err_free_object:
+	mxt_free_input_device(data);
 	mxt_free_object_table(data);
 err_free_irq:
 	free_irq(client->irq, data);
@@ -2224,7 +2237,7 @@ static int mxt_remove(struct i2c_client *client)
 
 	sysfs_remove_group(&client->dev.kobj, &mxt_attr_group);
 	free_irq(data->irq, data);
-	input_unregister_device(data->input_dev);
+	mxt_free_input_device(data);
 	mxt_free_object_table(data);
 	kfree(data);
 
diff --git a/drivers/input/touchscreen/edt-ft5x06.c b/drivers/input/touchscreen/edt-ft5x06.c
index 8857d5b9be71..ee3434f1e949 100644
--- a/drivers/input/touchscreen/edt-ft5x06.c
+++ b/drivers/input/touchscreen/edt-ft5x06.c
@@ -812,7 +812,7 @@ static int edt_ft5x06_ts_identify(struct i2c_client *client,
 	/* if we find something consistent, stay with that assumption
 	 * at least M09 won't send 3 bytes here
 	 */
-	if (!(strnicmp(rdbuf + 1, "EP0", 3))) {
+	if (!(strncasecmp(rdbuf + 1, "EP0", 3))) {
 		tsdata->version = M06;
 
 		/* remove last '$' end marker */
diff --git a/drivers/input/touchscreen/wm9712.c b/drivers/input/touchscreen/wm9712.c
index 16b52115c27f..705ffa1e064a 100644
--- a/drivers/input/touchscreen/wm9712.c
+++ b/drivers/input/touchscreen/wm9712.c
@@ -41,7 +41,7 @@
  */
 static int rpu = 8;
 module_param(rpu, int, 0);
-MODULE_PARM_DESC(rpu, "Set internal pull up resitor for pen detect.");
+MODULE_PARM_DESC(rpu, "Set internal pull up resistor for pen detect.");
 
 /*
  * Set current used for pressure measurement.
diff --git a/drivers/input/touchscreen/wm9713.c b/drivers/input/touchscreen/wm9713.c
index 7405353199d7..572a5a64face 100644
--- a/drivers/input/touchscreen/wm9713.c
+++ b/drivers/input/touchscreen/wm9713.c
@@ -41,7 +41,7 @@
  */
 static int rpu = 8;
 module_param(rpu, int, 0);
-MODULE_PARM_DESC(rpu, "Set internal pull up resitor for pen detect.");
+MODULE_PARM_DESC(rpu, "Set internal pull up resistor for pen detect.");
 
 /*
  * Set current used for pressure measurement.
diff --git a/drivers/iommu/amd_iommu.c b/drivers/iommu/amd_iommu.c
index 18405314168b..505a9adac2d5 100644
--- a/drivers/iommu/amd_iommu.c
+++ b/drivers/iommu/amd_iommu.c
@@ -88,6 +88,27 @@ int amd_iommu_max_glx_val = -1;
 static struct dma_map_ops amd_iommu_dma_ops;
 
 /*
+ * This struct contains device specific data for the IOMMU
+ */
+struct iommu_dev_data {
+	struct list_head list;		  /* For domain->dev_list */
+	struct list_head dev_data_list;	  /* For global dev_data_list */
+	struct list_head alias_list;      /* Link alias-groups together */
+	struct iommu_dev_data *alias_data;/* The alias dev_data */
+	struct protection_domain *domain; /* Domain the device is bound to */
+	u16 devid;			  /* PCI Device ID */
+	bool iommu_v2;			  /* Device can make use of IOMMUv2 */
+	bool passthrough;		  /* Default for device is pt_domain */
+	struct {
+		bool enabled;
+		int qdep;
+	} ats;				  /* ATS state */
+	bool pri_tlp;			  /* PASID TLB required for
+					     PPR completions */
+	u32 errata;			  /* Bitmap for errata to apply */
+};
+
+/*
  * general struct to manage commands send to an IOMMU
  */
 struct iommu_cmd {
@@ -114,8 +135,9 @@ static struct iommu_dev_data *alloc_dev_data(u16 devid)
 	if (!dev_data)
 		return NULL;
 
+	INIT_LIST_HEAD(&dev_data->alias_list);
+
 	dev_data->devid = devid;
-	atomic_set(&dev_data->bind, 0);
 
 	spin_lock_irqsave(&dev_data_list_lock, flags);
 	list_add_tail(&dev_data->dev_data_list, &dev_data_list);
@@ -260,17 +282,13 @@ static bool check_device(struct device *dev)
 	return true;
 }
 
-static int init_iommu_group(struct device *dev)
+static void init_iommu_group(struct device *dev)
 {
 	struct iommu_group *group;
 
 	group = iommu_group_get_for_dev(dev);
-
-	if (IS_ERR(group))
-		return PTR_ERR(group);
-
-	iommu_group_put(group);
-	return 0;
+	if (!IS_ERR(group))
+		iommu_group_put(group);
 }
 
 static int __last_alias(struct pci_dev *pdev, u16 alias, void *data)
@@ -340,7 +358,6 @@ static int iommu_init_device(struct device *dev)
 	struct pci_dev *pdev = to_pci_dev(dev);
 	struct iommu_dev_data *dev_data;
 	u16 alias;
-	int ret;
 
 	if (dev->archdata.iommu)
 		return 0;
@@ -362,12 +379,9 @@ static int iommu_init_device(struct device *dev)
 			return -ENOTSUPP;
 		}
 		dev_data->alias_data = alias_data;
-	}
 
-	ret = init_iommu_group(dev);
-	if (ret) {
-		free_dev_data(dev_data);
-		return ret;
+		/* Add device to the alias_list */
+		list_add(&dev_data->alias_list, &alias_data->alias_list);
 	}
 
 	if (pci_iommuv2_capable(pdev)) {
@@ -455,6 +469,15 @@ int __init amd_iommu_init_devices(void)
 			goto out_free;
 	}
 
+	/*
+	 * Initialize IOMMU groups only after iommu_init_device() has
+	 * had a chance to populate any IVRS defined aliases.
+	 */
+	for_each_pci_dev(pdev) {
+		if (check_device(&pdev->dev))
+			init_iommu_group(&pdev->dev);
+	}
+
 	return 0;
 
 out_free:
@@ -1368,6 +1391,9 @@ static int iommu_map_page(struct protection_domain *dom,
 	count     = PAGE_SIZE_PTE_COUNT(page_size);
 	pte       = alloc_pte(dom, bus_addr, page_size, NULL, GFP_KERNEL);
 
+	if (!pte)
+		return -ENOMEM;
+
 	for (i = 0; i < count; ++i)
 		if (IOMMU_PTE_PRESENT(pte[i]))
 			return -EBUSY;
@@ -2122,35 +2148,29 @@ static void do_detach(struct iommu_dev_data *dev_data)
 static int __attach_device(struct iommu_dev_data *dev_data,
 			   struct protection_domain *domain)
 {
+	struct iommu_dev_data *head, *entry;
 	int ret;
 
 	/* lock domain */
 	spin_lock(&domain->lock);
 
-	if (dev_data->alias_data != NULL) {
-		struct iommu_dev_data *alias_data = dev_data->alias_data;
+	head = dev_data;
 
-		/* Some sanity checks */
-		ret = -EBUSY;
-		if (alias_data->domain != NULL &&
-				alias_data->domain != domain)
-			goto out_unlock;
+	if (head->alias_data != NULL)
+		head = head->alias_data;
 
-		if (dev_data->domain != NULL &&
-				dev_data->domain != domain)
-			goto out_unlock;
+	/* Now we have the root of the alias group, if any */
 
-		/* Do real assignment */
-		if (alias_data->domain == NULL)
-			do_attach(alias_data, domain);
-
-		atomic_inc(&alias_data->bind);
-	}
+	ret = -EBUSY;
+	if (head->domain != NULL)
+		goto out_unlock;
 
-	if (dev_data->domain == NULL)
-		do_attach(dev_data, domain);
+	/* Attach alias group root */
+	do_attach(head, domain);
 
-	atomic_inc(&dev_data->bind);
+	/* Attach other devices in the alias group */
+	list_for_each_entry(entry, &head->alias_list, alias_list)
+		do_attach(entry, domain);
 
 	ret = 0;
 
@@ -2298,6 +2318,7 @@ static int attach_device(struct device *dev,
  */
 static void __detach_device(struct iommu_dev_data *dev_data)
 {
+	struct iommu_dev_data *head, *entry;
 	struct protection_domain *domain;
 	unsigned long flags;
 
@@ -2307,15 +2328,14 @@ static void __detach_device(struct iommu_dev_data *dev_data)
 
 	spin_lock_irqsave(&domain->lock, flags);
 
-	if (dev_data->alias_data != NULL) {
-		struct iommu_dev_data *alias_data = dev_data->alias_data;
+	head = dev_data;
+	if (head->alias_data != NULL)
+		head = head->alias_data;
 
-		if (atomic_dec_and_test(&alias_data->bind))
-			do_detach(alias_data);
-	}
+	list_for_each_entry(entry, &head->alias_list, alias_list)
+		do_detach(entry);
 
-	if (atomic_dec_and_test(&dev_data->bind))
-		do_detach(dev_data);
+	do_detach(head);
 
 	spin_unlock_irqrestore(&domain->lock, flags);
 
@@ -2415,6 +2435,7 @@ static int device_change_notifier(struct notifier_block *nb,
 	case BUS_NOTIFY_ADD_DEVICE:
 
 		iommu_init_device(dev);
+		init_iommu_group(dev);
 
 		/*
 		 * dev_data is still NULL and
@@ -3149,14 +3170,15 @@ free_domains:
 
 static void cleanup_domain(struct protection_domain *domain)
 {
-	struct iommu_dev_data *dev_data, *next;
+	struct iommu_dev_data *entry;
 	unsigned long flags;
 
 	write_lock_irqsave(&amd_iommu_devtable_lock, flags);
 
-	list_for_each_entry_safe(dev_data, next, &domain->dev_list, list) {
-		__detach_device(dev_data);
-		atomic_set(&dev_data->bind, 0);
+	while (!list_empty(&domain->dev_list)) {
+		entry = list_first_entry(&domain->dev_list,
+					 struct iommu_dev_data, list);
+		__detach_device(entry);
 	}
 
 	write_unlock_irqrestore(&amd_iommu_devtable_lock, flags);
@@ -3382,20 +3404,20 @@ static phys_addr_t amd_iommu_iova_to_phys(struct iommu_domain *dom,
 	return paddr;
 }
 
-static int amd_iommu_domain_has_cap(struct iommu_domain *domain,
-				    unsigned long cap)
+static bool amd_iommu_capable(enum iommu_cap cap)
 {
 	switch (cap) {
 	case IOMMU_CAP_CACHE_COHERENCY:
-		return 1;
+		return true;
 	case IOMMU_CAP_INTR_REMAP:
-		return irq_remapping_enabled;
+		return (irq_remapping_enabled == 1);
 	}
 
-	return 0;
+	return false;
 }
 
 static const struct iommu_ops amd_iommu_ops = {
+	.capable = amd_iommu_capable,
 	.domain_init = amd_iommu_domain_init,
 	.domain_destroy = amd_iommu_domain_destroy,
 	.attach_dev = amd_iommu_attach_device,
@@ -3403,7 +3425,6 @@ static const struct iommu_ops amd_iommu_ops = {
 	.map = amd_iommu_map,
 	.unmap = amd_iommu_unmap,
 	.iova_to_phys = amd_iommu_iova_to_phys,
-	.domain_has_cap = amd_iommu_domain_has_cap,
 	.pgsize_bitmap	= AMD_IOMMU_PGSIZES,
 };
 
@@ -4233,7 +4254,7 @@ static int msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
 	return 0;
 }
 
-static int setup_hpet_msi(unsigned int irq, unsigned int id)
+static int alloc_hpet_msi(unsigned int irq, unsigned int id)
 {
 	struct irq_2_irte *irte_info;
 	struct irq_cfg *cfg;
@@ -4272,6 +4293,6 @@ struct irq_remap_ops amd_iommu_irq_ops = {
 	.compose_msi_msg	= compose_msi_msg,
 	.msi_alloc_irq		= msi_alloc_irq,
 	.msi_setup_irq		= msi_setup_irq,
-	.setup_hpet_msi		= setup_hpet_msi,
+	.alloc_hpet_msi		= alloc_hpet_msi,
 };
 #endif
diff --git a/drivers/iommu/amd_iommu_init.c b/drivers/iommu/amd_iommu_init.c
index 3783e0b44df6..b0522f15730f 100644
--- a/drivers/iommu/amd_iommu_init.c
+++ b/drivers/iommu/amd_iommu_init.c
@@ -712,7 +712,7 @@ static void __init set_dev_entry_from_acpi(struct amd_iommu *iommu,
 	set_iommu_for_device(iommu, devid);
 }
 
-static int __init add_special_device(u8 type, u8 id, u16 devid, bool cmd_line)
+static int __init add_special_device(u8 type, u8 id, u16 *devid, bool cmd_line)
 {
 	struct devid_map *entry;
 	struct list_head *list;
@@ -731,6 +731,8 @@ static int __init add_special_device(u8 type, u8 id, u16 devid, bool cmd_line)
 		pr_info("AMD-Vi: Command-line override present for %s id %d - ignoring\n",
 			type == IVHD_SPECIAL_IOAPIC ? "IOAPIC" : "HPET", id);
 
+		*devid = entry->devid;
+
 		return 0;
 	}
 
@@ -739,7 +741,7 @@ static int __init add_special_device(u8 type, u8 id, u16 devid, bool cmd_line)
 		return -ENOMEM;
 
 	entry->id	= id;
-	entry->devid	= devid;
+	entry->devid	= *devid;
 	entry->cmd_line	= cmd_line;
 
 	list_add_tail(&entry->list, list);
@@ -754,7 +756,7 @@ static int __init add_early_maps(void)
 	for (i = 0; i < early_ioapic_map_size; ++i) {
 		ret = add_special_device(IVHD_SPECIAL_IOAPIC,
 					 early_ioapic_map[i].id,
-					 early_ioapic_map[i].devid,
+					 &early_ioapic_map[i].devid,
 					 early_ioapic_map[i].cmd_line);
 		if (ret)
 			return ret;
@@ -763,7 +765,7 @@ static int __init add_early_maps(void)
 	for (i = 0; i < early_hpet_map_size; ++i) {
 		ret = add_special_device(IVHD_SPECIAL_HPET,
 					 early_hpet_map[i].id,
-					 early_hpet_map[i].devid,
+					 &early_hpet_map[i].devid,
 					 early_hpet_map[i].cmd_line);
 		if (ret)
 			return ret;
@@ -978,10 +980,17 @@ static int __init init_iommu_from_acpi(struct amd_iommu *iommu,
 				    PCI_SLOT(devid),
 				    PCI_FUNC(devid));
 
-			set_dev_entry_from_acpi(iommu, devid, e->flags, 0);
-			ret = add_special_device(type, handle, devid, false);
+			ret = add_special_device(type, handle, &devid, false);
 			if (ret)
 				return ret;
+
+			/*
+			 * add_special_device might update the devid in case a
+			 * command-line override is present. So call
+			 * set_dev_entry_from_acpi after add_special_device.
+			 */
+			set_dev_entry_from_acpi(iommu, devid, e->flags, 0);
+
 			break;
 		}
 		default:
diff --git a/drivers/iommu/amd_iommu_types.h b/drivers/iommu/amd_iommu_types.h
index 8e43b7cba133..cec51a8ba844 100644
--- a/drivers/iommu/amd_iommu_types.h
+++ b/drivers/iommu/amd_iommu_types.h
@@ -418,27 +418,6 @@ struct protection_domain {
 };
 
 /*
- * This struct contains device specific data for the IOMMU
- */
-struct iommu_dev_data {
-	struct list_head list;		  /* For domain->dev_list */
-	struct list_head dev_data_list;	  /* For global dev_data_list */
-	struct iommu_dev_data *alias_data;/* The alias dev_data */
-	struct protection_domain *domain; /* Domain the device is bound to */
-	atomic_t bind;			  /* Domain attach reference count */
-	u16 devid;			  /* PCI Device ID */
-	bool iommu_v2;			  /* Device can make use of IOMMUv2 */
-	bool passthrough;		  /* Default for device is pt_domain */
-	struct {
-		bool enabled;
-		int qdep;
-	} ats;				  /* ATS state */
-	bool pri_tlp;			  /* PASID TLB required for
-					     PPR completions */
-	u32 errata;			  /* Bitmap for errata to apply */
-};
-
-/*
  * For dynamic growth the aperture size is split into ranges of 128MB of
  * DMA address space each. This struct represents one such range.
  */
diff --git a/drivers/iommu/amd_iommu_v2.c b/drivers/iommu/amd_iommu_v2.c
index 5f578e850fc5..90d734bbf467 100644
--- a/drivers/iommu/amd_iommu_v2.c
+++ b/drivers/iommu/amd_iommu_v2.c
@@ -402,9 +402,11 @@ static void __mn_flush_page(struct mmu_notifier *mn,
 
 static int mn_clear_flush_young(struct mmu_notifier *mn,
 				struct mm_struct *mm,
-				unsigned long address)
+				unsigned long start,
+				unsigned long end)
 {
-	__mn_flush_page(mn, address);
+	for (; start < end; start += PAGE_SIZE)
+		__mn_flush_page(mn, start);
 
 	return 0;
 }
diff --git a/drivers/iommu/arm-smmu.c b/drivers/iommu/arm-smmu.c
index ca18d6d42a9b..60558f794922 100644
--- a/drivers/iommu/arm-smmu.c
+++ b/drivers/iommu/arm-smmu.c
@@ -24,7 +24,7 @@
  *	- v7/v8 long-descriptor format
  *	- Non-secure access to the SMMU
  *	- 4k and 64k pages, with contiguous pte hints.
- *	- Up to 42-bit addressing (dependent on VA_BITS)
+ *	- Up to 48-bit addressing (dependent on VA_BITS)
  *	- Context fault reporting
  */
 
@@ -59,7 +59,7 @@
 
 /* SMMU global address space */
 #define ARM_SMMU_GR0(smmu)		((smmu)->base)
-#define ARM_SMMU_GR1(smmu)		((smmu)->base + (smmu)->pagesize)
+#define ARM_SMMU_GR1(smmu)		((smmu)->base + (1 << (smmu)->pgshift))
 
 /*
  * SMMU global address space with conditional offset to access secure
@@ -146,6 +146,8 @@
 #define ID0_CTTW			(1 << 14)
 #define ID0_NUMIRPT_SHIFT		16
 #define ID0_NUMIRPT_MASK		0xff
+#define ID0_NUMSIDB_SHIFT		9
+#define ID0_NUMSIDB_MASK		0xf
 #define ID0_NUMSMRG_SHIFT		0
 #define ID0_NUMSMRG_MASK		0xff
 
@@ -222,7 +224,7 @@
 
 /* Translation context bank */
 #define ARM_SMMU_CB_BASE(smmu)		((smmu)->base + ((smmu)->size >> 1))
-#define ARM_SMMU_CB(smmu, n)		((n) * (smmu)->pagesize)
+#define ARM_SMMU_CB(smmu, n)		((n) * (1 << (smmu)->pgshift))
 
 #define ARM_SMMU_CB_SCTLR		0x0
 #define ARM_SMMU_CB_RESUME		0x8
@@ -324,6 +326,16 @@
 
 #define FSYNR0_WNR			(1 << 4)
 
+static int force_stage;
+module_param_named(force_stage, force_stage, int, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(force_stage,
+	"Force SMMU mappings to be installed at a particular stage of translation. A value of '1' or '2' forces the corresponding stage. All other values are ignored (i.e. no stage is forced). Note that selecting a specific stage will disable support for nested translation.");
+
+enum arm_smmu_arch_version {
+	ARM_SMMU_V1 = 1,
+	ARM_SMMU_V2,
+};
+
 struct arm_smmu_smr {
 	u8				idx;
 	u16				mask;
@@ -347,7 +359,7 @@ struct arm_smmu_device {
 
 	void __iomem			*base;
 	unsigned long			size;
-	unsigned long			pagesize;
+	unsigned long			pgshift;
 
 #define ARM_SMMU_FEAT_COHERENT_WALK	(1 << 0)
 #define ARM_SMMU_FEAT_STREAM_MATCH	(1 << 1)
@@ -358,7 +370,7 @@ struct arm_smmu_device {
 
 #define ARM_SMMU_OPT_SECURE_CFG_ACCESS (1 << 0)
 	u32				options;
-	int				version;
+	enum arm_smmu_arch_version	version;
 
 	u32				num_context_banks;
 	u32				num_s2_context_banks;
@@ -368,8 +380,9 @@ struct arm_smmu_device {
 	u32				num_mapping_groups;
 	DECLARE_BITMAP(smr_map, ARM_SMMU_MAX_SMRS);
 
-	unsigned long			input_size;
+	unsigned long			s1_input_size;
 	unsigned long			s1_output_size;
+	unsigned long			s2_input_size;
 	unsigned long			s2_output_size;
 
 	u32				num_global_irqs;
@@ -424,17 +437,17 @@ static void parse_driver_options(struct arm_smmu_device *smmu)
 	} while (arm_smmu_options[++i].opt);
 }
 
-static struct device *dev_get_master_dev(struct device *dev)
+static struct device_node *dev_get_dev_node(struct device *dev)
 {
 	if (dev_is_pci(dev)) {
 		struct pci_bus *bus = to_pci_dev(dev)->bus;
 
 		while (!pci_is_root_bus(bus))
 			bus = bus->parent;
-		return bus->bridge->parent;
+		return bus->bridge->parent->of_node;
 	}
 
-	return dev;
+	return dev->of_node;
 }
 
 static struct arm_smmu_master *find_smmu_master(struct arm_smmu_device *smmu,
@@ -459,15 +472,17 @@ static struct arm_smmu_master *find_smmu_master(struct arm_smmu_device *smmu,
 }
 
 static struct arm_smmu_master_cfg *
-find_smmu_master_cfg(struct arm_smmu_device *smmu, struct device *dev)
+find_smmu_master_cfg(struct device *dev)
 {
-	struct arm_smmu_master *master;
+	struct arm_smmu_master_cfg *cfg = NULL;
+	struct iommu_group *group = iommu_group_get(dev);
 
-	if (dev_is_pci(dev))
-		return dev->archdata.iommu;
+	if (group) {
+		cfg = iommu_group_get_iommudata(group);
+		iommu_group_put(group);
+	}
 
-	master = find_smmu_master(smmu, dev->of_node);
-	return master ? &master->cfg : NULL;
+	return cfg;
 }
 
 static int insert_smmu_master(struct arm_smmu_device *smmu,
@@ -524,9 +539,18 @@ static int register_smmu_master(struct arm_smmu_device *smmu,
 	master->of_node			= masterspec->np;
 	master->cfg.num_streamids	= masterspec->args_count;
 
-	for (i = 0; i < master->cfg.num_streamids; ++i)
-		master->cfg.streamids[i] = masterspec->args[i];
+	for (i = 0; i < master->cfg.num_streamids; ++i) {
+		u16 streamid = masterspec->args[i];
 
+		if (!(smmu->features & ARM_SMMU_FEAT_STREAM_MATCH) &&
+		     (streamid >= smmu->num_mapping_groups)) {
+			dev_err(dev,
+				"stream ID for master device %s greater than maximum allowed (%d)\n",
+				masterspec->np->name, smmu->num_mapping_groups);
+			return -ERANGE;
+		}
+		master->cfg.streamids[i] = streamid;
+	}
 	return insert_smmu_master(smmu, master);
 }
 
@@ -534,7 +558,7 @@ static struct arm_smmu_device *find_smmu_for_device(struct device *dev)
 {
 	struct arm_smmu_device *smmu;
 	struct arm_smmu_master *master = NULL;
-	struct device_node *dev_node = dev_get_master_dev(dev)->of_node;
+	struct device_node *dev_node = dev_get_dev_node(dev);
 
 	spin_lock(&arm_smmu_devices_lock);
 	list_for_each_entry(smmu, &arm_smmu_devices, list) {
@@ -623,7 +647,7 @@ static irqreturn_t arm_smmu_context_fault(int irq, void *dev)
 
 	if (fsr & FSR_IGN)
 		dev_err_ratelimited(smmu->dev,
-				    "Unexpected context fault (fsr 0x%u)\n",
+				    "Unexpected context fault (fsr 0x%x)\n",
 				    fsr);
 
 	fsynr = readl_relaxed(cb_base + ARM_SMMU_CB_FSYNR0);
@@ -718,7 +742,7 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 
 	/* CBAR */
 	reg = cfg->cbar;
-	if (smmu->version == 1)
+	if (smmu->version == ARM_SMMU_V1)
 		reg |= cfg->irptndx << CBAR_IRPTNDX_SHIFT;
 
 	/*
@@ -733,7 +757,7 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 	}
 	writel_relaxed(reg, gr1_base + ARM_SMMU_GR1_CBAR(cfg->cbndx));
 
-	if (smmu->version > 1) {
+	if (smmu->version > ARM_SMMU_V1) {
 		/* CBA2R */
 #ifdef CONFIG_64BIT
 		reg = CBA2R_RW64_64BIT;
@@ -744,7 +768,7 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 			       gr1_base + ARM_SMMU_GR1_CBA2R(cfg->cbndx));
 
 		/* TTBCR2 */
-		switch (smmu->input_size) {
+		switch (smmu->s1_input_size) {
 		case 32:
 			reg = (TTBCR2_ADDR_32 << TTBCR2_SEP_SHIFT);
 			break;
@@ -752,6 +776,7 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 			reg = (TTBCR2_ADDR_36 << TTBCR2_SEP_SHIFT);
 			break;
 		case 39:
+		case 40:
 			reg = (TTBCR2_ADDR_40 << TTBCR2_SEP_SHIFT);
 			break;
 		case 42:
@@ -773,6 +798,7 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 			reg |= (TTBCR2_ADDR_36 << TTBCR2_PASIZE_SHIFT);
 			break;
 		case 39:
+		case 40:
 			reg |= (TTBCR2_ADDR_40 << TTBCR2_PASIZE_SHIFT);
 			break;
 		case 42:
@@ -804,14 +830,14 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 	 * TTBCR
 	 * We use long descriptor, with inner-shareable WBWA tables in TTBR0.
 	 */
-	if (smmu->version > 1) {
+	if (smmu->version > ARM_SMMU_V1) {
 		if (PAGE_SIZE == SZ_4K)
 			reg = TTBCR_TG0_4K;
 		else
 			reg = TTBCR_TG0_64K;
 
 		if (!stage1) {
-			reg |= (64 - smmu->s1_output_size) << TTBCR_T0SZ_SHIFT;
+			reg |= (64 - smmu->s2_input_size) << TTBCR_T0SZ_SHIFT;
 
 			switch (smmu->s2_output_size) {
 			case 32:
@@ -834,7 +860,7 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 				break;
 			}
 		} else {
-			reg |= (64 - smmu->input_size) << TTBCR_T0SZ_SHIFT;
+			reg |= (64 - smmu->s1_input_size) << TTBCR_T0SZ_SHIFT;
 		}
 	} else {
 		reg = 0;
@@ -843,8 +869,11 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 	reg |= TTBCR_EAE |
 	      (TTBCR_SH_IS << TTBCR_SH0_SHIFT) |
 	      (TTBCR_RGN_WBWA << TTBCR_ORGN0_SHIFT) |
-	      (TTBCR_RGN_WBWA << TTBCR_IRGN0_SHIFT) |
-	      (TTBCR_SL0_LVL_1 << TTBCR_SL0_SHIFT);
+	      (TTBCR_RGN_WBWA << TTBCR_IRGN0_SHIFT);
+
+	if (!stage1)
+		reg |= (TTBCR_SL0_LVL_1 << TTBCR_SL0_SHIFT);
+
 	writel_relaxed(reg, cb_base + ARM_SMMU_CB_TTBCR);
 
 	/* MAIR0 (stage-1 only) */
@@ -868,10 +897,15 @@ static void arm_smmu_init_context_bank(struct arm_smmu_domain *smmu_domain)
 static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 					struct arm_smmu_device *smmu)
 {
-	int irq, ret, start;
+	int irq, start, ret = 0;
+	unsigned long flags;
 	struct arm_smmu_domain *smmu_domain = domain->priv;
 	struct arm_smmu_cfg *cfg = &smmu_domain->cfg;
 
+	spin_lock_irqsave(&smmu_domain->lock, flags);
+	if (smmu_domain->smmu)
+		goto out_unlock;
+
 	if (smmu->features & ARM_SMMU_FEAT_TRANS_NESTED) {
 		/*
 		 * We will likely want to change this if/when KVM gets
@@ -890,16 +924,20 @@ static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 	ret = __arm_smmu_alloc_bitmap(smmu->context_map, start,
 				      smmu->num_context_banks);
 	if (IS_ERR_VALUE(ret))
-		return ret;
+		goto out_unlock;
 
 	cfg->cbndx = ret;
-	if (smmu->version == 1) {
+	if (smmu->version == ARM_SMMU_V1) {
 		cfg->irptndx = atomic_inc_return(&smmu->irptndx);
 		cfg->irptndx %= smmu->num_context_irqs;
 	} else {
 		cfg->irptndx = cfg->cbndx;
 	}
 
+	ACCESS_ONCE(smmu_domain->smmu) = smmu;
+	arm_smmu_init_context_bank(smmu_domain);
+	spin_unlock_irqrestore(&smmu_domain->lock, flags);
+
 	irq = smmu->irqs[smmu->num_global_irqs + cfg->irptndx];
 	ret = request_irq(irq, arm_smmu_context_fault, IRQF_SHARED,
 			  "arm-smmu-context-fault", domain);
@@ -907,15 +945,12 @@ static int arm_smmu_init_domain_context(struct iommu_domain *domain,
 		dev_err(smmu->dev, "failed to request context IRQ %d (%u)\n",
 			cfg->irptndx, irq);
 		cfg->irptndx = INVALID_IRPTNDX;
-		goto out_free_context;
 	}
 
-	smmu_domain->smmu = smmu;
-	arm_smmu_init_context_bank(smmu_domain);
 	return 0;
 
-out_free_context:
-	__arm_smmu_free_bitmap(smmu->context_map, cfg->cbndx);
+out_unlock:
+	spin_unlock_irqrestore(&smmu_domain->lock, flags);
 	return ret;
 }
 
@@ -975,7 +1010,6 @@ static void arm_smmu_free_ptes(pmd_t *pmd)
 {
 	pgtable_t table = pmd_pgtable(*pmd);
 
-	pgtable_page_dtor(table);
 	__free_page(table);
 }
 
@@ -1108,6 +1142,9 @@ static void arm_smmu_master_free_smrs(struct arm_smmu_device *smmu,
 	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
 	struct arm_smmu_smr *smrs = cfg->smrs;
 
+	if (!smrs)
+		return;
+
 	/* Invalidate the SMRs before freeing back to the allocator */
 	for (i = 0; i < cfg->num_streamids; ++i) {
 		u8 idx = smrs[i].idx;
@@ -1120,20 +1157,6 @@ static void arm_smmu_master_free_smrs(struct arm_smmu_device *smmu,
 	kfree(smrs);
 }
 
-static void arm_smmu_bypass_stream_mapping(struct arm_smmu_device *smmu,
-					   struct arm_smmu_master_cfg *cfg)
-{
-	int i;
-	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
-
-	for (i = 0; i < cfg->num_streamids; ++i) {
-		u16 sid = cfg->streamids[i];
-
-		writel_relaxed(S2CR_TYPE_BYPASS,
-			       gr0_base + ARM_SMMU_GR0_S2CR(sid));
-	}
-}
-
 static int arm_smmu_domain_add_master(struct arm_smmu_domain *smmu_domain,
 				      struct arm_smmu_master_cfg *cfg)
 {
@@ -1141,9 +1164,10 @@ static int arm_smmu_domain_add_master(struct arm_smmu_domain *smmu_domain,
 	struct arm_smmu_device *smmu = smmu_domain->smmu;
 	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
 
+	/* Devices in an IOMMU group may already be configured */
 	ret = arm_smmu_master_configure_smrs(smmu, cfg);
 	if (ret)
-		return ret;
+		return ret == -EEXIST ? 0 : ret;
 
 	for (i = 0; i < cfg->num_streamids; ++i) {
 		u32 idx, s2cr;
@@ -1160,58 +1184,75 @@ static int arm_smmu_domain_add_master(struct arm_smmu_domain *smmu_domain,
 static void arm_smmu_domain_remove_master(struct arm_smmu_domain *smmu_domain,
 					  struct arm_smmu_master_cfg *cfg)
 {
+	int i;
 	struct arm_smmu_device *smmu = smmu_domain->smmu;
+	void __iomem *gr0_base = ARM_SMMU_GR0(smmu);
+
+	/* An IOMMU group is torn down by the first device to be removed */
+	if ((smmu->features & ARM_SMMU_FEAT_STREAM_MATCH) && !cfg->smrs)
+		return;
 
 	/*
 	 * We *must* clear the S2CR first, because freeing the SMR means
 	 * that it can be re-allocated immediately.
 	 */
-	arm_smmu_bypass_stream_mapping(smmu, cfg);
+	for (i = 0; i < cfg->num_streamids; ++i) {
+		u32 idx = cfg->smrs ? cfg->smrs[i].idx : cfg->streamids[i];
+
+		writel_relaxed(S2CR_TYPE_BYPASS,
+			       gr0_base + ARM_SMMU_GR0_S2CR(idx));
+	}
+
 	arm_smmu_master_free_smrs(smmu, cfg);
 }
 
 static int arm_smmu_attach_dev(struct iommu_domain *domain, struct device *dev)
 {
-	int ret = -EINVAL;
+	int ret;
 	struct arm_smmu_domain *smmu_domain = domain->priv;
-	struct arm_smmu_device *smmu;
+	struct arm_smmu_device *smmu, *dom_smmu;
 	struct arm_smmu_master_cfg *cfg;
-	unsigned long flags;
 
-	smmu = dev_get_master_dev(dev)->archdata.iommu;
+	smmu = find_smmu_for_device(dev);
 	if (!smmu) {
 		dev_err(dev, "cannot attach to SMMU, is it on the same bus?\n");
 		return -ENXIO;
 	}
 
+	if (dev->archdata.iommu) {
+		dev_err(dev, "already attached to IOMMU domain\n");
+		return -EEXIST;
+	}
+
 	/*
 	 * Sanity check the domain. We don't support domains across
 	 * different SMMUs.
 	 */
-	spin_lock_irqsave(&smmu_domain->lock, flags);
-	if (!smmu_domain->smmu) {
+	dom_smmu = ACCESS_ONCE(smmu_domain->smmu);
+	if (!dom_smmu) {
 		/* Now that we have a master, we can finalise the domain */
 		ret = arm_smmu_init_domain_context(domain, smmu);
 		if (IS_ERR_VALUE(ret))
-			goto err_unlock;
-	} else if (smmu_domain->smmu != smmu) {
+			return ret;
+
+		dom_smmu = smmu_domain->smmu;
+	}
+
+	if (dom_smmu != smmu) {
 		dev_err(dev,
 			"cannot attach to SMMU %s whilst already attached to domain on SMMU %s\n",
-			dev_name(smmu_domain->smmu->dev),
-			dev_name(smmu->dev));
-		goto err_unlock;
+			dev_name(smmu_domain->smmu->dev), dev_name(smmu->dev));
+		return -EINVAL;
 	}
-	spin_unlock_irqrestore(&smmu_domain->lock, flags);
 
 	/* Looks ok, so add the device to the domain */
-	cfg = find_smmu_master_cfg(smmu_domain->smmu, dev);
+	cfg = find_smmu_master_cfg(dev);
 	if (!cfg)
 		return -ENODEV;
 
-	return arm_smmu_domain_add_master(smmu_domain, cfg);
-
-err_unlock:
-	spin_unlock_irqrestore(&smmu_domain->lock, flags);
+	ret = arm_smmu_domain_add_master(smmu_domain, cfg);
+	if (!ret)
+		dev->archdata.iommu = domain;
 	return ret;
 }
 
@@ -1220,9 +1261,12 @@ static void arm_smmu_detach_dev(struct iommu_domain *domain, struct device *dev)
 	struct arm_smmu_domain *smmu_domain = domain->priv;
 	struct arm_smmu_master_cfg *cfg;
 
-	cfg = find_smmu_master_cfg(smmu_domain->smmu, dev);
-	if (cfg)
-		arm_smmu_domain_remove_master(smmu_domain, cfg);
+	cfg = find_smmu_master_cfg(dev);
+	if (!cfg)
+		return;
+
+	dev->archdata.iommu = NULL;
+	arm_smmu_domain_remove_master(smmu_domain, cfg);
 }
 
 static bool arm_smmu_pte_is_contiguous_range(unsigned long addr,
@@ -1247,10 +1291,6 @@ static int arm_smmu_alloc_init_pte(struct arm_smmu_device *smmu, pmd_t *pmd,
 			return -ENOMEM;
 
 		arm_smmu_flush_pgtable(smmu, page_address(table), PAGE_SIZE);
-		if (!pgtable_page_ctor(table)) {
-			__free_page(table);
-			return -ENOMEM;
-		}
 		pmd_populate(NULL, pmd, table);
 		arm_smmu_flush_pgtable(smmu, pmd, sizeof(*pmd));
 	}
@@ -1368,6 +1408,7 @@ static int arm_smmu_alloc_init_pmd(struct arm_smmu_device *smmu, pud_t *pud,
 		ret = arm_smmu_alloc_init_pte(smmu, pmd, addr, next, pfn,
 					      prot, stage);
 		phys += next - addr;
+		pfn = __phys_to_pfn(phys);
 	} while (pmd++, addr = next, addr < end);
 
 	return ret;
@@ -1420,9 +1461,11 @@ static int arm_smmu_handle_mapping(struct arm_smmu_domain *smmu_domain,
 
 	if (cfg->cbar == CBAR_TYPE_S2_TRANS) {
 		stage = 2;
+		input_mask = (1ULL << smmu->s2_input_size) - 1;
 		output_mask = (1ULL << smmu->s2_output_size) - 1;
 	} else {
 		stage = 1;
+		input_mask = (1ULL << smmu->s1_input_size) - 1;
 		output_mask = (1ULL << smmu->s1_output_size) - 1;
 	}
 
@@ -1432,7 +1475,6 @@ static int arm_smmu_handle_mapping(struct arm_smmu_domain *smmu_domain,
 	if (size & ~PAGE_MASK)
 		return -EINVAL;
 
-	input_mask = (1ULL << smmu->input_size) - 1;
 	if ((phys_addr_t)iova & ~input_mask)
 		return -ERANGE;
 
@@ -1515,20 +1557,19 @@ static phys_addr_t arm_smmu_iova_to_phys(struct iommu_domain *domain,
 	return __pfn_to_phys(pte_pfn(pte)) | (iova & ~PAGE_MASK);
 }
 
-static int arm_smmu_domain_has_cap(struct iommu_domain *domain,
-				   unsigned long cap)
+static bool arm_smmu_capable(enum iommu_cap cap)
 {
-	struct arm_smmu_domain *smmu_domain = domain->priv;
-	struct arm_smmu_device *smmu = smmu_domain->smmu;
-	u32 features = smmu ? smmu->features : 0;
-
 	switch (cap) {
 	case IOMMU_CAP_CACHE_COHERENCY:
-		return features & ARM_SMMU_FEAT_COHERENT_WALK;
+		/*
+		 * Return true here as the SMMU can always send out coherent
+		 * requests.
+		 */
+		return true;
 	case IOMMU_CAP_INTR_REMAP:
-		return 1; /* MSIs are just memory writes */
+		return true; /* MSIs are just memory writes */
 	default:
-		return 0;
+		return false;
 	}
 }
 
@@ -1538,17 +1579,19 @@ static int __arm_smmu_get_pci_sid(struct pci_dev *pdev, u16 alias, void *data)
 	return 0; /* Continue walking */
 }
 
+static void __arm_smmu_release_pci_iommudata(void *data)
+{
+	kfree(data);
+}
+
 static int arm_smmu_add_device(struct device *dev)
 {
 	struct arm_smmu_device *smmu;
+	struct arm_smmu_master_cfg *cfg;
 	struct iommu_group *group;
+	void (*releasefn)(void *) = NULL;
 	int ret;
 
-	if (dev->archdata.iommu) {
-		dev_warn(dev, "IOMMU driver already assigned to device\n");
-		return -EINVAL;
-	}
-
 	smmu = find_smmu_for_device(dev);
 	if (!smmu)
 		return -ENODEV;
@@ -1560,7 +1603,6 @@ static int arm_smmu_add_device(struct device *dev)
 	}
 
 	if (dev_is_pci(dev)) {
-		struct arm_smmu_master_cfg *cfg;
 		struct pci_dev *pdev = to_pci_dev(dev);
 
 		cfg = kzalloc(sizeof(*cfg), GFP_KERNEL);
@@ -1576,11 +1618,20 @@ static int arm_smmu_add_device(struct device *dev)
 		 */
 		pci_for_each_dma_alias(pdev, __arm_smmu_get_pci_sid,
 				       &cfg->streamids[0]);
-		dev->archdata.iommu = cfg;
+		releasefn = __arm_smmu_release_pci_iommudata;
 	} else {
-		dev->archdata.iommu = smmu;
+		struct arm_smmu_master *master;
+
+		master = find_smmu_master(smmu, dev->of_node);
+		if (!master) {
+			ret = -ENODEV;
+			goto out_put_group;
+		}
+
+		cfg = &master->cfg;
 	}
 
+	iommu_group_set_iommudata(group, cfg, releasefn);
 	ret = iommu_group_add_device(group, dev);
 
 out_put_group:
@@ -1590,14 +1641,11 @@ out_put_group:
 
 static void arm_smmu_remove_device(struct device *dev)
 {
-	if (dev_is_pci(dev))
-		kfree(dev->archdata.iommu);
-
-	dev->archdata.iommu = NULL;
 	iommu_group_remove_device(dev);
 }
 
 static const struct iommu_ops arm_smmu_ops = {
+	.capable	= arm_smmu_capable,
 	.domain_init	= arm_smmu_domain_init,
 	.domain_destroy	= arm_smmu_domain_destroy,
 	.attach_dev	= arm_smmu_attach_dev,
@@ -1605,7 +1653,6 @@ static const struct iommu_ops arm_smmu_ops = {
 	.map		= arm_smmu_map,
 	.unmap		= arm_smmu_unmap,
 	.iova_to_phys	= arm_smmu_iova_to_phys,
-	.domain_has_cap	= arm_smmu_domain_has_cap,
 	.add_device	= arm_smmu_add_device,
 	.remove_device	= arm_smmu_remove_device,
 	.pgsize_bitmap	= (SECTION_SIZE |
@@ -1626,7 +1673,7 @@ static void arm_smmu_device_reset(struct arm_smmu_device *smmu)
 
 	/* Mark all SMRn as invalid and all S2CRn as bypass */
 	for (i = 0; i < smmu->num_mapping_groups; ++i) {
-		writel_relaxed(~SMR_VALID, gr0_base + ARM_SMMU_GR0_SMR(i));
+		writel_relaxed(0, gr0_base + ARM_SMMU_GR0_SMR(i));
 		writel_relaxed(S2CR_TYPE_BYPASS,
 			gr0_base + ARM_SMMU_GR0_S2CR(i));
 	}
@@ -1691,10 +1738,6 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 	u32 id;
 
 	dev_notice(smmu->dev, "probing hardware configuration...\n");
-
-	/* Primecell ID */
-	id = readl_relaxed(gr0_base + ARM_SMMU_GR0_PIDR2);
-	smmu->version = ((id >> PIDR2_ARCH_SHIFT) & PIDR2_ARCH_MASK) + 1;
 	dev_notice(smmu->dev, "SMMUv%d with:\n", smmu->version);
 
 	/* ID0 */
@@ -1705,6 +1748,13 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 		return -ENODEV;
 	}
 #endif
+
+	/* Restrict available stages based on module parameter */
+	if (force_stage == 1)
+		id &= ~(ID0_S2TS | ID0_NTS);
+	else if (force_stage == 2)
+		id &= ~(ID0_S1TS | ID0_NTS);
+
 	if (id & ID0_S1TS) {
 		smmu->features |= ARM_SMMU_FEAT_TRANS_S1;
 		dev_notice(smmu->dev, "\tstage 1 translation\n");
@@ -1721,8 +1771,7 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 	}
 
 	if (!(smmu->features &
-		(ARM_SMMU_FEAT_TRANS_S1 | ARM_SMMU_FEAT_TRANS_S2 |
-		 ARM_SMMU_FEAT_TRANS_NESTED))) {
+		(ARM_SMMU_FEAT_TRANS_S1 | ARM_SMMU_FEAT_TRANS_S2))) {
 		dev_err(smmu->dev, "\tno translation support!\n");
 		return -ENODEV;
 	}
@@ -1761,16 +1810,19 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 		dev_notice(smmu->dev,
 			   "\tstream matching with %u register groups, mask 0x%x",
 			   smmu->num_mapping_groups, mask);
+	} else {
+		smmu->num_mapping_groups = (id >> ID0_NUMSIDB_SHIFT) &
+					   ID0_NUMSIDB_MASK;
 	}
 
 	/* ID1 */
 	id = readl_relaxed(gr0_base + ARM_SMMU_GR0_ID1);
-	smmu->pagesize = (id & ID1_PAGESIZE) ? SZ_64K : SZ_4K;
+	smmu->pgshift = (id & ID1_PAGESIZE) ? 16 : 12;
 
 	/* Check for size mismatch of SMMU address space from mapped region */
 	size = 1 <<
 		(((id >> ID1_NUMPAGENDXB_SHIFT) & ID1_NUMPAGENDXB_MASK) + 1);
-	size *= (smmu->pagesize << 1);
+	size *= 2 << smmu->pgshift;
 	if (smmu->size != size)
 		dev_warn(smmu->dev,
 			"SMMU address space size (0x%lx) differs from mapped region size (0x%lx)!\n",
@@ -1789,23 +1841,21 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 	/* ID2 */
 	id = readl_relaxed(gr0_base + ARM_SMMU_GR0_ID2);
 	size = arm_smmu_id_size_to_bits((id >> ID2_IAS_SHIFT) & ID2_IAS_MASK);
+	smmu->s1_output_size = min_t(unsigned long, PHYS_MASK_SHIFT, size);
 
-	/*
-	 * Stage-1 output limited by stage-2 input size due to pgd
-	 * allocation (PTRS_PER_PGD).
-	 */
+	/* Stage-2 input size limited due to pgd allocation (PTRS_PER_PGD) */
 #ifdef CONFIG_64BIT
-	smmu->s1_output_size = min_t(unsigned long, VA_BITS, size);
+	smmu->s2_input_size = min_t(unsigned long, VA_BITS, size);
 #else
-	smmu->s1_output_size = min(32UL, size);
+	smmu->s2_input_size = min(32UL, size);
 #endif
 
 	/* The stage-2 output mask is also applied for bypass */
 	size = arm_smmu_id_size_to_bits((id >> ID2_OAS_SHIFT) & ID2_OAS_MASK);
 	smmu->s2_output_size = min_t(unsigned long, PHYS_MASK_SHIFT, size);
 
-	if (smmu->version == 1) {
-		smmu->input_size = 32;
+	if (smmu->version == ARM_SMMU_V1) {
+		smmu->s1_input_size = 32;
 	} else {
 #ifdef CONFIG_64BIT
 		size = (id >> ID2_UBS_SHIFT) & ID2_UBS_MASK;
@@ -1813,7 +1863,7 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 #else
 		size = 32;
 #endif
-		smmu->input_size = size;
+		smmu->s1_input_size = size;
 
 		if ((PAGE_SIZE == SZ_4K && !(id & ID2_PTFS_4K)) ||
 		    (PAGE_SIZE == SZ_64K && !(id & ID2_PTFS_64K)) ||
@@ -1824,15 +1874,30 @@ static int arm_smmu_device_cfg_probe(struct arm_smmu_device *smmu)
 		}
 	}
 
-	dev_notice(smmu->dev,
-		   "\t%lu-bit VA, %lu-bit IPA, %lu-bit PA\n",
-		   smmu->input_size, smmu->s1_output_size,
-		   smmu->s2_output_size);
+	if (smmu->features & ARM_SMMU_FEAT_TRANS_S1)
+		dev_notice(smmu->dev, "\tStage-1: %lu-bit VA -> %lu-bit IPA\n",
+			   smmu->s1_input_size, smmu->s1_output_size);
+
+	if (smmu->features & ARM_SMMU_FEAT_TRANS_S2)
+		dev_notice(smmu->dev, "\tStage-2: %lu-bit IPA -> %lu-bit PA\n",
+			   smmu->s2_input_size, smmu->s2_output_size);
+
 	return 0;
 }
 
+static const struct of_device_id arm_smmu_of_match[] = {
+	{ .compatible = "arm,smmu-v1", .data = (void *)ARM_SMMU_V1 },
+	{ .compatible = "arm,smmu-v2", .data = (void *)ARM_SMMU_V2 },
+	{ .compatible = "arm,mmu-400", .data = (void *)ARM_SMMU_V1 },
+	{ .compatible = "arm,mmu-401", .data = (void *)ARM_SMMU_V1 },
+	{ .compatible = "arm,mmu-500", .data = (void *)ARM_SMMU_V2 },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, arm_smmu_of_match);
+
 static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id;
 	struct resource *res;
 	struct arm_smmu_device *smmu;
 	struct device *dev = &pdev->dev;
@@ -1847,6 +1912,9 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 	}
 	smmu->dev = dev;
 
+	of_id = of_match_node(arm_smmu_of_match, dev->of_node);
+	smmu->version = (enum arm_smmu_arch_version)of_id->data;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	smmu->base = devm_ioremap_resource(dev, res);
 	if (IS_ERR(smmu->base))
@@ -1889,6 +1957,10 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 		smmu->irqs[i] = irq;
 	}
 
+	err = arm_smmu_device_cfg_probe(smmu);
+	if (err)
+		return err;
+
 	i = 0;
 	smmu->masters = RB_ROOT;
 	while (!of_parse_phandle_with_args(dev->of_node, "mmu-masters",
@@ -1905,13 +1977,9 @@ static int arm_smmu_device_dt_probe(struct platform_device *pdev)
 	}
 	dev_notice(dev, "registered %d master devices\n", i);
 
-	err = arm_smmu_device_cfg_probe(smmu);
-	if (err)
-		goto out_put_masters;
-
 	parse_driver_options(smmu);
 
-	if (smmu->version > 1 &&
+	if (smmu->version > ARM_SMMU_V1 &&
 	    smmu->num_context_banks != smmu->num_context_irqs) {
 		dev_err(dev,
 			"found only %d context interrupt(s) but %d required\n",
@@ -1992,17 +2060,6 @@ static int arm_smmu_device_remove(struct platform_device *pdev)
 	return 0;
 }
 
-#ifdef CONFIG_OF
-static struct of_device_id arm_smmu_of_match[] = {
-	{ .compatible = "arm,smmu-v1", },
-	{ .compatible = "arm,smmu-v2", },
-	{ .compatible = "arm,mmu-400", },
-	{ .compatible = "arm,mmu-500", },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, arm_smmu_of_match);
-#endif
-
 static struct platform_driver arm_smmu_driver = {
 	.driver	= {
 		.owner		= THIS_MODULE,
diff --git a/drivers/iommu/dmar.c b/drivers/iommu/dmar.c
index 60ab474bfff3..c5c61cabd6e3 100644
--- a/drivers/iommu/dmar.c
+++ b/drivers/iommu/dmar.c
@@ -155,6 +155,7 @@ dmar_alloc_pci_notify_info(struct pci_dev *dev, unsigned long event)
 	if (event == BUS_NOTIFY_ADD_DEVICE) {
 		for (tmp = dev; tmp; tmp = tmp->bus->self) {
 			level--;
+			info->path[level].bus = tmp->bus->number;
 			info->path[level].device = PCI_SLOT(tmp->devfn);
 			info->path[level].function = PCI_FUNC(tmp->devfn);
 			if (pci_is_root_bus(tmp->bus))
@@ -177,17 +178,33 @@ static bool dmar_match_pci_path(struct dmar_pci_notify_info *info, int bus,
 	int i;
 
 	if (info->bus != bus)
-		return false;
+		goto fallback;
 	if (info->level != count)
-		return false;
+		goto fallback;
 
 	for (i = 0; i < count; i++) {
 		if (path[i].device != info->path[i].device ||
 		    path[i].function != info->path[i].function)
-			return false;
+			goto fallback;
 	}
 
 	return true;
+
+fallback:
+
+	if (count != 1)
+		return false;
+
+	i = info->level - 1;
+	if (bus              == info->path[i].bus &&
+	    path[0].device   == info->path[i].device &&
+	    path[0].function == info->path[i].function) {
+		pr_info(FW_BUG "RMRR entry for device %02x:%02x.%x is broken - applying workaround\n",
+			bus, path[0].device, path[0].function);
+		return true;
+	}
+
+	return false;
 }
 
 /* Return: > 0 if match found, 0 if no match found, < 0 if error happens */
@@ -247,7 +264,7 @@ int dmar_remove_dev_scope(struct dmar_pci_notify_info *info, u16 segment,
 
 	for_each_active_dev_scope(devices, count, index, tmp)
 		if (tmp == &info->dev->dev) {
-			rcu_assign_pointer(devices[index].dev, NULL);
+			RCU_INIT_POINTER(devices[index].dev, NULL);
 			synchronize_rcu();
 			put_device(tmp);
 			return 1;
@@ -678,8 +695,7 @@ static int __init dmar_acpi_dev_scope_init(void)
 				       andd->device_name);
 				continue;
 			}
-			acpi_bus_get_device(h, &adev);
-			if (!adev) {
+			if (acpi_bus_get_device(h, &adev)) {
 				pr_err("Failed to get device for ACPI object %s\n",
 				       andd->device_name);
 				continue;
diff --git a/drivers/iommu/exynos-iommu.c b/drivers/iommu/exynos-iommu.c
index d037e87a1fe5..74233186f6f7 100644
--- a/drivers/iommu/exynos-iommu.c
+++ b/drivers/iommu/exynos-iommu.c
@@ -32,7 +32,7 @@
 typedef u32 sysmmu_iova_t;
 typedef u32 sysmmu_pte_t;
 
-/* We does not consider super section mapping (16MB) */
+/* We do not consider super section mapping (16MB) */
 #define SECT_ORDER 20
 #define LPAGE_ORDER 16
 #define SPAGE_ORDER 12
@@ -307,7 +307,7 @@ static void show_fault_information(const char *name,
 
 static irqreturn_t exynos_sysmmu_irq(int irq, void *dev_id)
 {
-	/* SYSMMU is in blocked when interrupt occurred. */
+	/* SYSMMU is in blocked state when interrupt occurred. */
 	struct sysmmu_drvdata *data = dev_id;
 	enum exynos_sysmmu_inttype itype;
 	sysmmu_iova_t addr = -1;
@@ -567,8 +567,8 @@ static void sysmmu_tlb_invalidate_entry(struct device *dev, sysmmu_iova_t iova,
 		/*
 		 * L2TLB invalidation required
 		 * 4KB page: 1 invalidation
-		 * 64KB page: 16 invalidation
-		 * 1MB page: 64 invalidation
+		 * 64KB page: 16 invalidations
+		 * 1MB page: 64 invalidations
 		 * because it is set-associative TLB
 		 * with 8-way and 64 sets.
 		 * 1MB page can be cached in one of all sets.
@@ -714,7 +714,7 @@ static int exynos_iommu_domain_init(struct iommu_domain *domain)
 	if (!priv->lv2entcnt)
 		goto err_counter;
 
-	/* w/a of System MMU v3.3 to prevent caching 1MiB mapping */
+	/* Workaround for System MMU v3.3 to prevent caching 1MiB mapping */
 	for (i = 0; i < NUM_LV1ENTRIES; i += 8) {
 		priv->pgtable[i + 0] = ZERO_LV2LINK;
 		priv->pgtable[i + 1] = ZERO_LV2LINK;
@@ -861,14 +861,14 @@ static sysmmu_pte_t *alloc_lv2entry(struct exynos_iommu_domain *priv,
 		pgtable_flush(sent, sent + 1);
 
 		/*
-		 * If pretched SLPD is a fault SLPD in zero_l2_table, FLPD cache
-		 * may caches the address of zero_l2_table. This function
-		 * replaces the zero_l2_table with new L2 page table to write
-		 * valid mappings.
+		 * If pre-fetched SLPD is a faulty SLPD in zero_l2_table,
+		 * FLPD cache may cache the address of zero_l2_table. This
+		 * function replaces the zero_l2_table with new L2 page table
+		 * to write valid mappings.
 		 * Accessing the valid area may cause page fault since FLPD
-		 * cache may still caches zero_l2_table for the valid area
-		 * instead of new L2 page table that have the mapping
-		 * information of the valid area
+		 * cache may still cache zero_l2_table for the valid area
+		 * instead of new L2 page table that has the mapping
+		 * information of the valid area.
 		 * Thus any replacement of zero_l2_table with other valid L2
 		 * page table must involve FLPD cache invalidation for System
 		 * MMU v3.3.
@@ -963,27 +963,27 @@ static int lv2set_page(sysmmu_pte_t *pent, phys_addr_t paddr, size_t size,
 /*
  * *CAUTION* to the I/O virtual memory managers that support exynos-iommu:
  *
- * System MMU v3.x have an advanced logic to improve address translation
+ * System MMU v3.x has advanced logic to improve address translation
  * performance with caching more page table entries by a page table walk.
- * However, the logic has a bug that caching fault page table entries and System
- * MMU reports page fault if the cached fault entry is hit even though the fault
- * entry is updated to a valid entry after the entry is cached.
- * To prevent caching fault page table entries which may be updated to valid
- * entries later, the virtual memory manager should care about the w/a about the
- * problem. The followings describe w/a.
+ * However, the logic has a bug that while caching faulty page table entries,
+ * System MMU reports page fault if the cached fault entry is hit even though
+ * the fault entry is updated to a valid entry after the entry is cached.
+ * To prevent caching faulty page table entries which may be updated to valid
+ * entries later, the virtual memory manager should care about the workaround
+ * for the problem. The following describes the workaround.
  *
  * Any two consecutive I/O virtual address regions must have a hole of 128KiB
- * in maximum to prevent misbehavior of System MMU 3.x. (w/a of h/w bug)
+ * at maximum to prevent misbehavior of System MMU 3.x (workaround for h/w bug).
  *
- * Precisely, any start address of I/O virtual region must be aligned by
+ * Precisely, any start address of I/O virtual region must be aligned with
  * the following sizes for System MMU v3.1 and v3.2.
  * System MMU v3.1: 128KiB
  * System MMU v3.2: 256KiB
  *
  * Because System MMU v3.3 caches page table entries more aggressively, it needs
- * more w/a.
- * - Any two consecutive I/O virtual regions must be have a hole of larger size
- *   than or equal size to 128KiB.
+ * more workarounds.
+ * - Any two consecutive I/O virtual regions must have a hole of size larger
+ *   than or equal to 128KiB.
  * - Start address of an I/O virtual region must be aligned by 128KiB.
  */
 static int exynos_iommu_map(struct iommu_domain *domain, unsigned long l_iova,
@@ -1061,7 +1061,8 @@ static size_t exynos_iommu_unmap(struct iommu_domain *domain,
 			goto err;
 		}
 
-		*ent = ZERO_LV2LINK; /* w/a for h/w bug in Sysmem MMU v3.3 */
+		/* workaround for h/w bug in System MMU v3.3 */
+		*ent = ZERO_LV2LINK;
 		pgtable_flush(ent, ent + 1);
 		size = SECT_SIZE;
 		goto done;
diff --git a/drivers/iommu/fsl_pamu_domain.c b/drivers/iommu/fsl_pamu_domain.c
index 61d1dafa242d..c828f80d48b0 100644
--- a/drivers/iommu/fsl_pamu_domain.c
+++ b/drivers/iommu/fsl_pamu_domain.c
@@ -411,8 +411,7 @@ static phys_addr_t fsl_pamu_iova_to_phys(struct iommu_domain *domain,
 	return get_phys_addr(dma_domain, iova);
 }
 
-static int fsl_pamu_domain_has_cap(struct iommu_domain *domain,
-				      unsigned long cap)
+static bool fsl_pamu_capable(enum iommu_cap cap)
 {
 	return cap == IOMMU_CAP_CACHE_COHERENCY;
 }
@@ -984,7 +983,7 @@ static int fsl_pamu_add_device(struct device *dev)
 	struct iommu_group *group = ERR_PTR(-ENODEV);
 	struct pci_dev *pdev;
 	const u32 *prop;
-	int ret, len;
+	int ret = 0, len;
 
 	/*
 	 * For platform devices we allocate a separate group for
@@ -1007,7 +1006,13 @@ static int fsl_pamu_add_device(struct device *dev)
 	if (IS_ERR(group))
 		return PTR_ERR(group);
 
-	ret = iommu_group_add_device(group, dev);
+	/*
+	 * Check if device has already been added to an iommu group.
+	 * Group could have already been created for a PCI device in
+	 * the iommu_group_get_for_dev path.
+	 */
+	if (!dev->iommu_group)
+		ret = iommu_group_add_device(group, dev);
 
 	iommu_group_put(group);
 	return ret;
@@ -1074,6 +1079,7 @@ static u32 fsl_pamu_get_windows(struct iommu_domain *domain)
 }
 
 static const struct iommu_ops fsl_pamu_ops = {
+	.capable	= fsl_pamu_capable,
 	.domain_init	= fsl_pamu_domain_init,
 	.domain_destroy = fsl_pamu_domain_destroy,
 	.attach_dev	= fsl_pamu_attach_device,
@@ -1083,7 +1089,6 @@ static const struct iommu_ops fsl_pamu_ops = {
 	.domain_get_windows = fsl_pamu_get_windows,
 	.domain_set_windows = fsl_pamu_set_windows,
 	.iova_to_phys	= fsl_pamu_iova_to_phys,
-	.domain_has_cap = fsl_pamu_domain_has_cap,
 	.domain_set_attr = fsl_pamu_set_domain_attr,
 	.domain_get_attr = fsl_pamu_get_domain_attr,
 	.add_device	= fsl_pamu_add_device,
diff --git a/drivers/iommu/intel-iommu.c b/drivers/iommu/intel-iommu.c
index d1f5caad04f9..a27d6cb1a793 100644
--- a/drivers/iommu/intel-iommu.c
+++ b/drivers/iommu/intel-iommu.c
@@ -3865,8 +3865,15 @@ static int device_notifier(struct notifier_block *nb,
 	if (iommu_dummy(dev))
 		return 0;
 
-	if (action != BUS_NOTIFY_UNBOUND_DRIVER &&
-	    action != BUS_NOTIFY_DEL_DEVICE)
+	if (action != BUS_NOTIFY_REMOVED_DEVICE)
+		return 0;
+
+	/*
+	 * If the device is still attached to a device driver we can't
+	 * tear down the domain yet as DMA mappings may still be in use.
+	 * Wait for the BUS_NOTIFY_UNBOUND_DRIVER event to do that.
+	 */
+	if (action == BUS_NOTIFY_DEL_DEVICE && dev->driver != NULL)
 		return 0;
 
 	domain = find_domain(dev);
@@ -4407,17 +4414,14 @@ static phys_addr_t intel_iommu_iova_to_phys(struct iommu_domain *domain,
 	return phys;
 }
 
-static int intel_iommu_domain_has_cap(struct iommu_domain *domain,
-				      unsigned long cap)
+static bool intel_iommu_capable(enum iommu_cap cap)
 {
-	struct dmar_domain *dmar_domain = domain->priv;
-
 	if (cap == IOMMU_CAP_CACHE_COHERENCY)
-		return dmar_domain->iommu_snooping;
+		return domain_update_iommu_snooping(NULL) == 1;
 	if (cap == IOMMU_CAP_INTR_REMAP)
-		return irq_remapping_enabled;
+		return irq_remapping_enabled == 1;
 
-	return 0;
+	return false;
 }
 
 static int intel_iommu_add_device(struct device *dev)
@@ -4456,6 +4460,7 @@ static void intel_iommu_remove_device(struct device *dev)
 }
 
 static const struct iommu_ops intel_iommu_ops = {
+	.capable	= intel_iommu_capable,
 	.domain_init	= intel_iommu_domain_init,
 	.domain_destroy = intel_iommu_domain_destroy,
 	.attach_dev	= intel_iommu_attach_device,
@@ -4463,7 +4468,6 @@ static const struct iommu_ops intel_iommu_ops = {
 	.map		= intel_iommu_map,
 	.unmap		= intel_iommu_unmap,
 	.iova_to_phys	= intel_iommu_iova_to_phys,
-	.domain_has_cap = intel_iommu_domain_has_cap,
 	.add_device	= intel_iommu_add_device,
 	.remove_device	= intel_iommu_remove_device,
 	.pgsize_bitmap	= INTEL_IOMMU_PGSIZES,
diff --git a/drivers/iommu/intel_irq_remapping.c b/drivers/iommu/intel_irq_remapping.c
index 0df41f6264f5..7c80661b35c1 100644
--- a/drivers/iommu/intel_irq_remapping.c
+++ b/drivers/iommu/intel_irq_remapping.c
@@ -438,8 +438,7 @@ static void iommu_set_irq_remapping(struct intel_iommu *iommu, int mode)
 		    (addr) | IR_X2APIC_MODE(mode) | INTR_REMAP_TABLE_REG_SIZE);
 
 	/* Set interrupt-remapping table pointer */
-	iommu->gcmd |= DMA_GCMD_SIRTP;
-	writel(iommu->gcmd, iommu->reg + DMAR_GCMD_REG);
+	writel(iommu->gcmd | DMA_GCMD_SIRTP, iommu->reg + DMAR_GCMD_REG);
 
 	IOMMU_WAIT_OP(iommu, DMAR_GSTS_REG,
 		      readl, (sts & DMA_GSTS_IRTPS), sts);
@@ -1139,7 +1138,7 @@ static int intel_msi_setup_irq(struct pci_dev *pdev, unsigned int irq,
 	return ret;
 }
 
-static int intel_setup_hpet_msi(unsigned int irq, unsigned int id)
+static int intel_alloc_hpet_msi(unsigned int irq, unsigned int id)
 {
 	int ret = -1;
 	struct intel_iommu *iommu;
@@ -1170,5 +1169,5 @@ struct irq_remap_ops intel_irq_remap_ops = {
 	.compose_msi_msg	= intel_compose_msi_msg,
 	.msi_alloc_irq		= intel_msi_alloc_irq,
 	.msi_setup_irq		= intel_msi_setup_irq,
-	.setup_hpet_msi		= intel_setup_hpet_msi,
+	.alloc_hpet_msi		= intel_alloc_hpet_msi,
 };
diff --git a/drivers/iommu/iommu.c b/drivers/iommu/iommu.c
index 169836020208..ed8b04867b1f 100644
--- a/drivers/iommu/iommu.c
+++ b/drivers/iommu/iommu.c
@@ -30,6 +30,7 @@
 #include <linux/notifier.h>
 #include <linux/err.h>
 #include <linux/pci.h>
+#include <linux/bitops.h>
 #include <trace/events/iommu.h>
 
 static struct kset *iommu_group_kset;
@@ -519,6 +520,9 @@ int iommu_group_id(struct iommu_group *group)
 }
 EXPORT_SYMBOL_GPL(iommu_group_id);
 
+static struct iommu_group *get_pci_alias_group(struct pci_dev *pdev,
+					       unsigned long *devfns);
+
 /*
  * To consider a PCI device isolated, we require ACS to support Source
  * Validation, Request Redirection, Completer Redirection, and Upstream
@@ -529,6 +533,86 @@ EXPORT_SYMBOL_GPL(iommu_group_id);
  */
 #define REQ_ACS_FLAGS   (PCI_ACS_SV | PCI_ACS_RR | PCI_ACS_CR | PCI_ACS_UF)
 
+/*
+ * For multifunction devices which are not isolated from each other, find
+ * all the other non-isolated functions and look for existing groups.  For
+ * each function, we also need to look for aliases to or from other devices
+ * that may already have a group.
+ */
+static struct iommu_group *get_pci_function_alias_group(struct pci_dev *pdev,
+							unsigned long *devfns)
+{
+	struct pci_dev *tmp = NULL;
+	struct iommu_group *group;
+
+	if (!pdev->multifunction || pci_acs_enabled(pdev, REQ_ACS_FLAGS))
+		return NULL;
+
+	for_each_pci_dev(tmp) {
+		if (tmp == pdev || tmp->bus != pdev->bus ||
+		    PCI_SLOT(tmp->devfn) != PCI_SLOT(pdev->devfn) ||
+		    pci_acs_enabled(tmp, REQ_ACS_FLAGS))
+			continue;
+
+		group = get_pci_alias_group(tmp, devfns);
+		if (group) {
+			pci_dev_put(tmp);
+			return group;
+		}
+	}
+
+	return NULL;
+}
+
+/*
+ * Look for aliases to or from the given device for exisiting groups.  The
+ * dma_alias_devfn only supports aliases on the same bus, therefore the search
+ * space is quite small (especially since we're really only looking at pcie
+ * device, and therefore only expect multiple slots on the root complex or
+ * downstream switch ports).  It's conceivable though that a pair of
+ * multifunction devices could have aliases between them that would cause a
+ * loop.  To prevent this, we use a bitmap to track where we've been.
+ */
+static struct iommu_group *get_pci_alias_group(struct pci_dev *pdev,
+					       unsigned long *devfns)
+{
+	struct pci_dev *tmp = NULL;
+	struct iommu_group *group;
+
+	if (test_and_set_bit(pdev->devfn & 0xff, devfns))
+		return NULL;
+
+	group = iommu_group_get(&pdev->dev);
+	if (group)
+		return group;
+
+	for_each_pci_dev(tmp) {
+		if (tmp == pdev || tmp->bus != pdev->bus)
+			continue;
+
+		/* We alias them or they alias us */
+		if (((pdev->dev_flags & PCI_DEV_FLAGS_DMA_ALIAS_DEVFN) &&
+		     pdev->dma_alias_devfn == tmp->devfn) ||
+		    ((tmp->dev_flags & PCI_DEV_FLAGS_DMA_ALIAS_DEVFN) &&
+		     tmp->dma_alias_devfn == pdev->devfn)) {
+
+			group = get_pci_alias_group(tmp, devfns);
+			if (group) {
+				pci_dev_put(tmp);
+				return group;
+			}
+
+			group = get_pci_function_alias_group(tmp, devfns);
+			if (group) {
+				pci_dev_put(tmp);
+				return group;
+			}
+		}
+	}
+
+	return NULL;
+}
+
 struct group_for_pci_data {
 	struct pci_dev *pdev;
 	struct iommu_group *group;
@@ -557,7 +641,7 @@ static struct iommu_group *iommu_group_get_for_pci_dev(struct pci_dev *pdev)
 	struct group_for_pci_data data;
 	struct pci_bus *bus;
 	struct iommu_group *group = NULL;
-	struct pci_dev *tmp;
+	u64 devfns[4] = { 0 };
 
 	/*
 	 * Find the upstream DMA alias for the device.  A device must not
@@ -591,76 +675,21 @@ static struct iommu_group *iommu_group_get_for_pci_dev(struct pci_dev *pdev)
 	}
 
 	/*
-	 * Next we need to consider DMA alias quirks.  If one device aliases
-	 * to another, they should be grouped together.  It's theoretically
-	 * possible that aliases could create chains of devices where each
-	 * device aliases another device.  If we then factor in multifunction
-	 * ACS grouping requirements, each alias could incorporate a new slot
-	 * with multiple functions, each with aliases.  This is all extremely
-	 * unlikely as DMA alias quirks are typically only used for PCIe
-	 * devices where we usually have a single slot per bus.  Furthermore,
-	 * the alias quirk is usually to another function within the slot
-	 * (and ACS multifunction is not supported) or to a different slot
-	 * that doesn't physically exist.  The likely scenario is therefore
-	 * that everything on the bus gets grouped together.  To reduce the
-	 * problem space, share the IOMMU group for all devices on the bus
-	 * if a DMA alias quirk is present on the bus.
-	 */
-	tmp = NULL;
-	for_each_pci_dev(tmp) {
-		if (tmp->bus != pdev->bus ||
-		    !(tmp->dev_flags & PCI_DEV_FLAGS_DMA_ALIAS_DEVFN))
-			continue;
-
-		pci_dev_put(tmp);
-		tmp = NULL;
-
-		/* We have an alias quirk, search for an existing group */
-		for_each_pci_dev(tmp) {
-			struct iommu_group *group_tmp;
-
-			if (tmp->bus != pdev->bus)
-				continue;
-
-			group_tmp = iommu_group_get(&tmp->dev);
-			if (!group) {
-				group = group_tmp;
-				continue;
-			}
-
-			if (group_tmp) {
-				WARN_ON(group != group_tmp);
-				iommu_group_put(group_tmp);
-			}
-		}
-
-		return group ? group : iommu_group_alloc();
-	}
-
-	/*
-	 * Non-multifunction devices or multifunction devices supporting
-	 * ACS get their own group.
+	 * Look for existing groups on device aliases.  If we alias another
+	 * device or another device aliases us, use the same group.
 	 */
-	if (!pdev->multifunction || pci_acs_enabled(pdev, REQ_ACS_FLAGS))
-		return iommu_group_alloc();
+	group = get_pci_alias_group(pdev, (unsigned long *)devfns);
+	if (group)
+		return group;
 
 	/*
-	 * Multifunction devices not supporting ACS share a group with other
-	 * similar devices in the same slot.
+	 * Look for existing groups on non-isolated functions on the same
+	 * slot and aliases of those funcions, if any.  No need to clear
+	 * the search bitmap, the tested devfns are still valid.
 	 */
-	tmp = NULL;
-	for_each_pci_dev(tmp) {
-		if (tmp == pdev || tmp->bus != pdev->bus ||
-		    PCI_SLOT(tmp->devfn) !=  PCI_SLOT(pdev->devfn) ||
-		    pci_acs_enabled(tmp, REQ_ACS_FLAGS))
-			continue;
-
-		group = iommu_group_get(&tmp->dev);
-		if (group) {
-			pci_dev_put(tmp);
-			return group;
-		}
-	}
+	group = get_pci_function_alias_group(pdev, (unsigned long *)devfns);
+	if (group)
+		return group;
 
 	/* No shared group found, allocate new */
 	return iommu_group_alloc();
@@ -678,15 +707,17 @@ static struct iommu_group *iommu_group_get_for_pci_dev(struct pci_dev *pdev)
  */
 struct iommu_group *iommu_group_get_for_dev(struct device *dev)
 {
-	struct iommu_group *group = ERR_PTR(-EIO);
+	struct iommu_group *group;
 	int ret;
 
 	group = iommu_group_get(dev);
 	if (group)
 		return group;
 
-	if (dev_is_pci(dev))
-		group = iommu_group_get_for_pci_dev(to_pci_dev(dev));
+	if (!dev_is_pci(dev))
+		return ERR_PTR(-EINVAL);
+
+	group = iommu_group_get_for_pci_dev(to_pci_dev(dev));
 
 	if (IS_ERR(group))
 		return group;
@@ -768,18 +799,26 @@ static int iommu_bus_notifier(struct notifier_block *nb,
 	return 0;
 }
 
-static struct notifier_block iommu_bus_nb = {
-	.notifier_call = iommu_bus_notifier,
-};
-
-static void iommu_bus_init(struct bus_type *bus, const struct iommu_ops *ops)
+static int iommu_bus_init(struct bus_type *bus, const struct iommu_ops *ops)
 {
+	int err;
+	struct notifier_block *nb;
 	struct iommu_callback_data cb = {
 		.ops = ops,
 	};
 
-	bus_register_notifier(bus, &iommu_bus_nb);
-	bus_for_each_dev(bus, NULL, &cb, add_iommu_group);
+	nb = kzalloc(sizeof(struct notifier_block), GFP_KERNEL);
+	if (!nb)
+		return -ENOMEM;
+
+	nb->notifier_call = iommu_bus_notifier;
+
+	err = bus_register_notifier(bus, nb);
+	if (err) {
+		kfree(nb);
+		return err;
+	}
+	return bus_for_each_dev(bus, NULL, &cb, add_iommu_group);
 }
 
 /**
@@ -803,9 +842,7 @@ int bus_set_iommu(struct bus_type *bus, const struct iommu_ops *ops)
 	bus->iommu_ops = ops;
 
 	/* Do IOMMU specific setup for this bus-type */
-	iommu_bus_init(bus, ops);
-
-	return 0;
+	return iommu_bus_init(bus, ops);
 }
 EXPORT_SYMBOL_GPL(bus_set_iommu);
 
@@ -815,6 +852,15 @@ bool iommu_present(struct bus_type *bus)
 }
 EXPORT_SYMBOL_GPL(iommu_present);
 
+bool iommu_capable(struct bus_type *bus, enum iommu_cap cap)
+{
+	if (!bus->iommu_ops || !bus->iommu_ops->capable)
+		return false;
+
+	return bus->iommu_ops->capable(cap);
+}
+EXPORT_SYMBOL_GPL(iommu_capable);
+
 /**
  * iommu_set_fault_handler() - set a fault handler for an iommu domain
  * @domain: iommu domain
@@ -945,16 +991,6 @@ phys_addr_t iommu_iova_to_phys(struct iommu_domain *domain, dma_addr_t iova)
 }
 EXPORT_SYMBOL_GPL(iommu_iova_to_phys);
 
-int iommu_domain_has_cap(struct iommu_domain *domain,
-			 unsigned long cap)
-{
-	if (unlikely(domain->ops->domain_has_cap == NULL))
-		return 0;
-
-	return domain->ops->domain_has_cap(domain, cap);
-}
-EXPORT_SYMBOL_GPL(iommu_domain_has_cap);
-
 static size_t iommu_pgsize(struct iommu_domain *domain,
 			   unsigned long addr_merge, size_t size)
 {
@@ -995,7 +1031,7 @@ int iommu_map(struct iommu_domain *domain, unsigned long iova,
 	size_t orig_size = size;
 	int ret = 0;
 
-	if (unlikely(domain->ops->unmap == NULL ||
+	if (unlikely(domain->ops->map == NULL ||
 		     domain->ops->pgsize_bitmap == 0UL))
 		return -ENODEV;
 
diff --git a/drivers/iommu/irq_remapping.c b/drivers/iommu/irq_remapping.c
index 33c439524080..74a1767c89b5 100644
--- a/drivers/iommu/irq_remapping.c
+++ b/drivers/iommu/irq_remapping.c
@@ -12,6 +12,7 @@
 #include <asm/processor.h>
 #include <asm/x86_init.h>
 #include <asm/apic.h>
+#include <asm/hpet.h>
 
 #include "irq_remapping.h"
 
@@ -345,10 +346,16 @@ static int msi_setup_remapped_irq(struct pci_dev *pdev, unsigned int irq,
 
 int setup_hpet_msi_remapped(unsigned int irq, unsigned int id)
 {
-	if (!remap_ops || !remap_ops->setup_hpet_msi)
+	int ret;
+
+	if (!remap_ops || !remap_ops->alloc_hpet_msi)
 		return -ENODEV;
 
-	return remap_ops->setup_hpet_msi(irq, id);
+	ret = remap_ops->alloc_hpet_msi(irq, id);
+	if (ret)
+		return -EINVAL;
+
+	return default_setup_hpet_msi(irq, id);
 }
 
 void panic_if_irq_remap(const char *msg)
diff --git a/drivers/iommu/irq_remapping.h b/drivers/iommu/irq_remapping.h
index 90c4dae5a46b..fde250f86e60 100644
--- a/drivers/iommu/irq_remapping.h
+++ b/drivers/iommu/irq_remapping.h
@@ -80,7 +80,7 @@ struct irq_remap_ops {
 	int (*msi_setup_irq)(struct pci_dev *, unsigned int, int, int);
 
 	/* Setup interrupt remapping for an HPET MSI */
-	int (*setup_hpet_msi)(unsigned int, unsigned int);
+	int (*alloc_hpet_msi)(unsigned int, unsigned int);
 };
 
 extern struct irq_remap_ops intel_irq_remap_ops;
diff --git a/drivers/iommu/msm_iommu.c b/drivers/iommu/msm_iommu.c
index 49f41d6e02f1..6e3dcc289d59 100644
--- a/drivers/iommu/msm_iommu.c
+++ b/drivers/iommu/msm_iommu.c
@@ -603,10 +603,9 @@ fail:
 	return ret;
 }
 
-static int msm_iommu_domain_has_cap(struct iommu_domain *domain,
-				    unsigned long cap)
+static bool msm_iommu_capable(enum iommu_cap cap)
 {
-	return 0;
+	return false;
 }
 
 static void print_ctx_regs(void __iomem *base, int ctx)
@@ -675,6 +674,7 @@ fail:
 }
 
 static const struct iommu_ops msm_iommu_ops = {
+	.capable = msm_iommu_capable,
 	.domain_init = msm_iommu_domain_init,
 	.domain_destroy = msm_iommu_domain_destroy,
 	.attach_dev = msm_iommu_attach_dev,
@@ -682,7 +682,6 @@ static const struct iommu_ops msm_iommu_ops = {
 	.map = msm_iommu_map,
 	.unmap = msm_iommu_unmap,
 	.iova_to_phys = msm_iommu_iova_to_phys,
-	.domain_has_cap = msm_iommu_domain_has_cap,
 	.pgsize_bitmap = MSM_IOMMU_PGSIZES,
 };
 
diff --git a/drivers/iommu/omap-iommu.c b/drivers/iommu/omap-iommu.c
index e202b0c24120..36278870e84a 100644
--- a/drivers/iommu/omap-iommu.c
+++ b/drivers/iommu/omap-iommu.c
@@ -26,6 +26,7 @@
 #include <linux/of.h>
 #include <linux/of_iommu.h>
 #include <linux/of_irq.h>
+#include <linux/of_platform.h>
 
 #include <asm/cacheflush.h>
 
@@ -892,19 +893,11 @@ static struct omap_iommu *omap_iommu_attach(const char *name, u32 *iopgd)
 		goto err_enable;
 	flush_iotlb_all(obj);
 
-	if (!try_module_get(obj->owner)) {
-		err = -ENODEV;
-		goto err_module;
-	}
-
 	spin_unlock(&obj->iommu_lock);
 
 	dev_dbg(obj->dev, "%s: %s\n", __func__, obj->name);
 	return obj;
 
-err_module:
-	if (obj->refcount == 1)
-		iommu_disable(obj);
 err_enable:
 	obj->refcount--;
 	spin_unlock(&obj->iommu_lock);
@@ -925,8 +918,6 @@ static void omap_iommu_detach(struct omap_iommu *obj)
 	if (--obj->refcount == 0)
 		iommu_disable(obj);
 
-	module_put(obj->owner);
-
 	obj->iopgd = NULL;
 
 	spin_unlock(&obj->iommu_lock);
@@ -1006,7 +997,7 @@ static int omap_iommu_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static struct of_device_id omap_iommu_of_match[] = {
+static const struct of_device_id omap_iommu_of_match[] = {
 	{ .compatible = "ti,omap2-iommu" },
 	{ .compatible = "ti,omap4-iommu" },
 	{ .compatible = "ti,dra7-iommu"	},
@@ -1091,6 +1082,11 @@ omap_iommu_attach_dev(struct iommu_domain *domain, struct device *dev)
 	struct omap_iommu_arch_data *arch_data = dev->archdata.iommu;
 	int ret = 0;
 
+	if (!arch_data || !arch_data->name) {
+		dev_err(dev, "device doesn't have an associated iommu\n");
+		return -EINVAL;
+	}
+
 	spin_lock(&omap_domain->lock);
 
 	/* only a single device is supported per domain for now */
@@ -1239,6 +1235,7 @@ static int omap_iommu_add_device(struct device *dev)
 {
 	struct omap_iommu_arch_data *arch_data;
 	struct device_node *np;
+	struct platform_device *pdev;
 
 	/*
 	 * Allocate the archdata iommu structure for DT-based devices.
@@ -1253,13 +1250,19 @@ static int omap_iommu_add_device(struct device *dev)
 	if (!np)
 		return 0;
 
+	pdev = of_find_device_by_node(np);
+	if (WARN_ON(!pdev)) {
+		of_node_put(np);
+		return -EINVAL;
+	}
+
 	arch_data = kzalloc(sizeof(*arch_data), GFP_KERNEL);
 	if (!arch_data) {
 		of_node_put(np);
 		return -ENOMEM;
 	}
 
-	arch_data->name = kstrdup(dev_name(dev), GFP_KERNEL);
+	arch_data->name = kstrdup(dev_name(&pdev->dev), GFP_KERNEL);
 	dev->archdata.iommu = arch_data;
 
 	of_node_put(np);
diff --git a/drivers/iommu/omap-iommu.h b/drivers/iommu/omap-iommu.h
index 1275a822934b..4f1b68c08c15 100644
--- a/drivers/iommu/omap-iommu.h
+++ b/drivers/iommu/omap-iommu.h
@@ -28,7 +28,6 @@ struct iotlb_entry {
 
 struct omap_iommu {
 	const char	*name;
-	struct module	*owner;
 	void __iomem	*regbase;
 	struct device	*dev;
 	void		*isr_priv;
diff --git a/drivers/iommu/tegra-gart.c b/drivers/iommu/tegra-gart.c
index b10a8ecede8e..a6d76abf2c06 100644
--- a/drivers/iommu/tegra-gart.c
+++ b/drivers/iommu/tegra-gart.c
@@ -303,13 +303,13 @@ static phys_addr_t gart_iommu_iova_to_phys(struct iommu_domain *domain,
 	return pa;
 }
 
-static int gart_iommu_domain_has_cap(struct iommu_domain *domain,
-				     unsigned long cap)
+static bool gart_iommu_capable(enum iommu_cap cap)
 {
-	return 0;
+	return false;
 }
 
 static const struct iommu_ops gart_iommu_ops = {
+	.capable	= gart_iommu_capable,
 	.domain_init	= gart_iommu_domain_init,
 	.domain_destroy	= gart_iommu_domain_destroy,
 	.attach_dev	= gart_iommu_attach_dev,
@@ -317,7 +317,6 @@ static const struct iommu_ops gart_iommu_ops = {
 	.map		= gart_iommu_map,
 	.unmap		= gart_iommu_unmap,
 	.iova_to_phys	= gart_iommu_iova_to_phys,
-	.domain_has_cap	= gart_iommu_domain_has_cap,
 	.pgsize_bitmap	= GART_IOMMU_PGSIZES,
 };
 
@@ -416,7 +415,7 @@ static const struct dev_pm_ops tegra_gart_pm_ops = {
 	.resume		= tegra_gart_resume,
 };
 
-static struct of_device_id tegra_gart_of_match[] = {
+static const struct of_device_id tegra_gart_of_match[] = {
 	{ .compatible = "nvidia,tegra20-gart", },
 	{ },
 };
diff --git a/drivers/iommu/tegra-smmu.c b/drivers/iommu/tegra-smmu.c
index 3ded3894623c..3afdf43f732a 100644
--- a/drivers/iommu/tegra-smmu.c
+++ b/drivers/iommu/tegra-smmu.c
@@ -780,10 +780,9 @@ static phys_addr_t smmu_iommu_iova_to_phys(struct iommu_domain *domain,
 	return PFN_PHYS(pfn);
 }
 
-static int smmu_iommu_domain_has_cap(struct iommu_domain *domain,
-				     unsigned long cap)
+static bool smmu_iommu_capable(enum iommu_cap cap)
 {
-	return 0;
+	return false;
 }
 
 static int smmu_iommu_attach_dev(struct iommu_domain *domain,
@@ -949,6 +948,7 @@ static void smmu_iommu_domain_destroy(struct iommu_domain *domain)
 }
 
 static const struct iommu_ops smmu_iommu_ops = {
+	.capable	= smmu_iommu_capable,
 	.domain_init	= smmu_iommu_domain_init,
 	.domain_destroy	= smmu_iommu_domain_destroy,
 	.attach_dev	= smmu_iommu_attach_dev,
@@ -956,7 +956,6 @@ static const struct iommu_ops smmu_iommu_ops = {
 	.map		= smmu_iommu_map,
 	.unmap		= smmu_iommu_unmap,
 	.iova_to_phys	= smmu_iommu_iova_to_phys,
-	.domain_has_cap	= smmu_iommu_domain_has_cap,
 	.pgsize_bitmap	= SMMU_IOMMU_PGSIZES,
 };
 
@@ -1260,7 +1259,7 @@ static const struct dev_pm_ops tegra_smmu_pm_ops = {
 	.resume		= tegra_smmu_resume,
 };
 
-static struct of_device_id tegra_smmu_of_match[] = {
+static const struct of_device_id tegra_smmu_of_match[] = {
 	{ .compatible = "nvidia,tegra30-smmu", },
 	{ },
 };
diff --git a/drivers/ipack/carriers/tpci200.c b/drivers/ipack/carriers/tpci200.c
index de5e32151a1e..9b23843dcad4 100644
--- a/drivers/ipack/carriers/tpci200.c
+++ b/drivers/ipack/carriers/tpci200.c
@@ -572,7 +572,8 @@ static int tpci200_pci_probe(struct pci_dev *pdev,
 	/* Register the carrier in the industry pack bus driver */
 	tpci200->info->ipack_bus = ipack_bus_register(&pdev->dev,
 						      TPCI200_NB_SLOT,
-						      &tpci200_bus_ops);
+						      &tpci200_bus_ops,
+						      THIS_MODULE);
 	if (!tpci200->info->ipack_bus) {
 		dev_err(&pdev->dev,
 			"error registering the carrier on ipack driver\n");
diff --git a/drivers/ipack/devices/ipoctal.c b/drivers/ipack/devices/ipoctal.c
index e41bef048c23..035d5449227e 100644
--- a/drivers/ipack/devices/ipoctal.c
+++ b/drivers/ipack/devices/ipoctal.c
@@ -55,6 +55,22 @@ struct ipoctal {
 	u8 __iomem			*int_space;
 };
 
+static inline struct ipoctal *chan_to_ipoctal(struct ipoctal_channel *chan,
+					      unsigned int index)
+{
+	return container_of(chan, struct ipoctal, channel[index]);
+}
+
+static void ipoctal_reset_channel(struct ipoctal_channel *channel)
+{
+	iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
+	channel->rx_enable = 0;
+	iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_ERR_STATUS, &channel->regs->w.cr);
+	iowrite8(CR_CMD_RESET_MR, &channel->regs->w.cr);
+}
+
 static int ipoctal_port_activate(struct tty_port *port, struct tty_struct *tty)
 {
 	struct ipoctal_channel *channel;
@@ -72,12 +88,20 @@ static int ipoctal_port_activate(struct tty_port *port, struct tty_struct *tty)
 
 static int ipoctal_open(struct tty_struct *tty, struct file *file)
 {
-	struct ipoctal_channel *channel;
+	struct ipoctal_channel *channel = dev_get_drvdata(tty->dev);
+	struct ipoctal *ipoctal = chan_to_ipoctal(channel, tty->index);
+	int err;
 
-	channel = dev_get_drvdata(tty->dev);
 	tty->driver_data = channel;
 
-	return tty_port_open(&channel->tty_port, tty, file);
+	if (!ipack_get_carrier(ipoctal->dev))
+		return -EBUSY;
+
+	err = tty_port_open(&channel->tty_port, tty, file);
+	if (err)
+		ipack_put_carrier(ipoctal->dev);
+
+	return err;
 }
 
 static void ipoctal_reset_stats(struct ipoctal_stats *stats)
@@ -151,7 +175,6 @@ static void ipoctal_irq_rx(struct ipoctal_channel *channel, u8 sr)
 				flag = TTY_FRAME;
 			}
 			if (sr & SR_RECEIVED_BREAK) {
-				iowrite8(CR_CMD_RESET_BREAK_CHANGE, &channel->regs->w.cr);
 				channel->stats.rcv_break++;
 				flag = TTY_BREAK;
 			}
@@ -196,6 +219,9 @@ static void ipoctal_irq_channel(struct ipoctal_channel *channel)
 	isr = ioread8(&channel->block_regs->r.isr);
 	sr = ioread8(&channel->regs->r.sr);
 
+	if (isr & (IMR_DELTA_BREAK_A | IMR_DELTA_BREAK_B))
+		iowrite8(CR_CMD_RESET_BREAK_CHANGE, &channel->regs->w.cr);
+
 	if ((sr & SR_TX_EMPTY) && (channel->nb_bytes == 0)) {
 		iowrite8(CR_DISABLE_TX, &channel->regs->w.cr);
 		/* In case of RS-485, change from TX to RX when finishing TX.
@@ -304,10 +330,7 @@ static int ipoctal_inst_slot(struct ipoctal *ipoctal, unsigned int bus_nr,
 			channel->isr_rx_rdy_mask = ISR_RxRDY_FFULL_A;
 		}
 
-		iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
-		channel->rx_enable = 0;
-		iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
-		iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
+		ipoctal_reset_channel(channel);
 		iowrite8(MR1_CHRL_8_BITS | MR1_ERROR_CHAR | MR1_RxINT_RxRDY,
 			 &channel->regs->w.mr); /* mr1 */
 		iowrite8(0, &channel->regs->w.mr); /* mr2 */
@@ -467,11 +490,7 @@ static void ipoctal_set_termios(struct tty_struct *tty,
 	cflag = tty->termios.c_cflag;
 
 	/* Disable and reset everything before change the setup */
-	iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_ERR_STATUS, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_MR, &channel->regs->w.cr);
+	ipoctal_reset_channel(channel);
 
 	/* Set Bits per chars */
 	switch (cflag & CSIZE) {
@@ -609,12 +628,7 @@ static void ipoctal_hangup(struct tty_struct *tty)
 
 	tty_port_hangup(&channel->tty_port);
 
-	iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
-	channel->rx_enable = 0;
-	iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_ERR_STATUS, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_MR, &channel->regs->w.cr);
+	ipoctal_reset_channel(channel);
 
 	clear_bit(ASYNCB_INITIALIZED, &channel->tty_port.flags);
 	wake_up_interruptible(&channel->tty_port.open_wait);
@@ -627,15 +641,19 @@ static void ipoctal_shutdown(struct tty_struct *tty)
 	if (channel == NULL)
 		return;
 
-	iowrite8(CR_DISABLE_RX | CR_DISABLE_TX, &channel->regs->w.cr);
-	channel->rx_enable = 0;
-	iowrite8(CR_CMD_RESET_RX, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_TX, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_ERR_STATUS, &channel->regs->w.cr);
-	iowrite8(CR_CMD_RESET_MR, &channel->regs->w.cr);
+	ipoctal_reset_channel(channel);
 	clear_bit(ASYNCB_INITIALIZED, &channel->tty_port.flags);
 }
 
+static void ipoctal_cleanup(struct tty_struct *tty)
+{
+	struct ipoctal_channel *channel = tty->driver_data;
+	struct ipoctal *ipoctal = chan_to_ipoctal(channel, tty->index);
+
+	/* release the carrier driver */
+	ipack_put_carrier(ipoctal->dev);
+}
+
 static const struct tty_operations ipoctal_fops = {
 	.ioctl =		NULL,
 	.open =			ipoctal_open,
@@ -647,6 +665,7 @@ static const struct tty_operations ipoctal_fops = {
 	.get_icount =		ipoctal_get_icount,
 	.hangup =		ipoctal_hangup,
 	.shutdown =		ipoctal_shutdown,
+	.cleanup =              ipoctal_cleanup,
 };
 
 static int ipoctal_probe(struct ipack_device *dev)
diff --git a/drivers/ipack/devices/ipoctal.h b/drivers/ipack/devices/ipoctal.h
index 28f1c4233154..7fede0eb6a0c 100644
--- a/drivers/ipack/devices/ipoctal.h
+++ b/drivers/ipack/devices/ipoctal.h
@@ -12,7 +12,7 @@
  * Software Foundation; version 2 of the License.
  */
 
-#ifndef _IPOCTAL_H
+#ifndef _IPOCTAL_H_
 #define _IPOCTAL_H_
 
 #define NR_CHANNELS		8
diff --git a/drivers/ipack/ipack.c b/drivers/ipack/ipack.c
index d0016ba469ed..c0e7b624ce54 100644
--- a/drivers/ipack/ipack.c
+++ b/drivers/ipack/ipack.c
@@ -206,7 +206,8 @@ static struct bus_type ipack_bus_type = {
 };
 
 struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
-					    const struct ipack_bus_ops *ops)
+					    const struct ipack_bus_ops *ops,
+					    struct module *owner)
 {
 	int bus_nr;
 	struct ipack_bus_device *bus;
@@ -225,6 +226,7 @@ struct ipack_bus_device *ipack_bus_register(struct device *parent, int slots,
 	bus->parent = parent;
 	bus->slots = slots;
 	bus->ops = ops;
+	bus->owner = owner;
 	return bus;
 }
 EXPORT_SYMBOL_GPL(ipack_bus_register);
diff --git a/drivers/irqchip/Kconfig b/drivers/irqchip/Kconfig
index b8632bf9a7f3..b21f12f1766d 100644
--- a/drivers/irqchip/Kconfig
+++ b/drivers/irqchip/Kconfig
@@ -75,6 +75,11 @@ config OR1K_PIC
 	bool
 	select IRQ_DOMAIN
 
+config OMAP_IRQCHIP
+	bool
+	select GENERIC_IRQ_CHIP
+	select IRQ_DOMAIN
+
 config ORION_IRQCHIP
 	bool
 	select IRQ_DOMAIN
@@ -109,7 +114,14 @@ config XTENSA_MX
 config IRQ_CROSSBAR
 	bool
 	help
-	  Support for a CROSSBAR ip that preceeds the main interrupt controller.
+	  Support for a CROSSBAR ip that precedes the main interrupt controller.
 	  The primary irqchip invokes the crossbar's callback which inturn allocates
 	  a free irq and configures the IP. Thus the peripheral interrupts are
 	  routed to one of the free irqchip interrupt lines.
+
+config KEYSTONE_IRQ
+	tristate "Keystone 2 IRQ controller IP"
+	depends on ARCH_KEYSTONE
+	help
+		Support for Texas Instruments Keystone 2 IRQ controller IP which
+		is part of the Keystone 2 IPC mechanism
diff --git a/drivers/irqchip/Makefile b/drivers/irqchip/Makefile
index 73052ba9ca62..173bb5fa2cc9 100644
--- a/drivers/irqchip/Makefile
+++ b/drivers/irqchip/Makefile
@@ -2,6 +2,7 @@ obj-$(CONFIG_IRQCHIP)			+= irqchip.o
 
 obj-$(CONFIG_ARCH_BCM2835)		+= irq-bcm2835.o
 obj-$(CONFIG_ARCH_EXYNOS)		+= exynos-combiner.o
+obj-$(CONFIG_ARCH_HIP04)		+= irq-hip04.o
 obj-$(CONFIG_ARCH_MMP)			+= irq-mmp.o
 obj-$(CONFIG_ARCH_MVEBU)		+= irq-armada-370-xp.o
 obj-$(CONFIG_ARCH_MXS)			+= irq-mxs.o
@@ -13,6 +14,7 @@ obj-$(CONFIG_ARCH_MOXART)		+= irq-moxart.o
 obj-$(CONFIG_CLPS711X_IRQCHIP)		+= irq-clps711x.o
 obj-$(CONFIG_OR1K_PIC)			+= irq-or1k-pic.o
 obj-$(CONFIG_ORION_IRQCHIP)		+= irq-orion.o
+obj-$(CONFIG_OMAP_IRQCHIP)		+= irq-omap-intc.o
 obj-$(CONFIG_ARCH_SUNXI)		+= irq-sun4i.o
 obj-$(CONFIG_ARCH_SUNXI)		+= irq-sunxi-nmi.o
 obj-$(CONFIG_ARCH_SPEAR3XX)		+= spear-shirq.o
@@ -33,4 +35,6 @@ obj-$(CONFIG_TB10X_IRQC)		+= irq-tb10x.o
 obj-$(CONFIG_XTENSA)			+= irq-xtensa-pic.o
 obj-$(CONFIG_XTENSA_MX)			+= irq-xtensa-mx.o
 obj-$(CONFIG_IRQ_CROSSBAR)		+= irq-crossbar.o
-obj-$(CONFIG_BRCMSTB_L2_IRQ)		+= irq-brcmstb-l2.o
+obj-$(CONFIG_BRCMSTB_L2_IRQ)		+= irq-brcmstb-l2.o \
+					   irq-bcm7120-l2.o
+obj-$(CONFIG_KEYSTONE_IRQ)		+= irq-keystone.o
diff --git a/drivers/irqchip/exynos-combiner.c b/drivers/irqchip/exynos-combiner.c
index f8636a650cf6..5945223b73fa 100644
--- a/drivers/irqchip/exynos-combiner.c
+++ b/drivers/irqchip/exynos-combiner.c
@@ -15,6 +15,7 @@
 #include <linux/slab.h>
 #include <linux/irqdomain.h>
 #include <linux/irqchip/chained_irq.h>
+#include <linux/interrupt.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index 574aba0eba4e..3e238cd049e6 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -136,6 +136,10 @@ static int armada_370_xp_setup_msi_irq(struct msi_chip *chip,
 	struct msi_msg msg;
 	int virq, hwirq;
 
+	/* We support MSI, but not MSI-X */
+	if (desc->msi_attrib.is_msix)
+		return -EINVAL;
+
 	hwirq = armada_370_xp_alloc_msi();
 	if (hwirq < 0)
 		return hwirq;
@@ -166,15 +170,6 @@ static void armada_370_xp_teardown_msi_irq(struct msi_chip *chip,
 	armada_370_xp_free_msi(hwirq);
 }
 
-static int armada_370_xp_check_msi_device(struct msi_chip *chip, struct pci_dev *dev,
-					  int nvec, int type)
-{
-	/* We support MSI, but not MSI-X */
-	if (type == PCI_CAP_ID_MSI)
-		return 0;
-	return -EINVAL;
-}
-
 static struct irq_chip armada_370_xp_msi_irq_chip = {
 	.name = "armada_370_xp_msi_irq",
 	.irq_enable = unmask_msi_irq,
@@ -213,7 +208,6 @@ static int armada_370_xp_msi_init(struct device_node *node,
 
 	msi_chip->setup_irq = armada_370_xp_setup_msi_irq;
 	msi_chip->teardown_irq = armada_370_xp_teardown_msi_irq;
-	msi_chip->check_device = armada_370_xp_check_msi_device;
 	msi_chip->of_node = node;
 
 	armada_370_xp_msi_domain =
@@ -393,13 +387,15 @@ static void armada_370_xp_handle_msi_irq(struct pt_regs *regs, bool is_chained)
 		if (!(msimask & BIT(msinr)))
 			continue;
 
-		irq = irq_find_mapping(armada_370_xp_msi_domain,
-				       msinr - 16);
-
-		if (is_chained)
+		if (is_chained) {
+			irq = irq_find_mapping(armada_370_xp_msi_domain,
+					       msinr - 16);
 			generic_handle_irq(irq);
-		else
-			handle_IRQ(irq, regs);
+		} else {
+			irq = msinr - 16;
+			handle_domain_irq(armada_370_xp_msi_domain,
+					  irq, regs);
+		}
 	}
 }
 #else
@@ -444,9 +440,8 @@ armada_370_xp_handle_irq(struct pt_regs *regs)
 			break;
 
 		if (irqnr > 1) {
-			irqnr =	irq_find_mapping(armada_370_xp_mpic_domain,
-					irqnr);
-			handle_IRQ(irqnr, regs);
+			handle_domain_irq(armada_370_xp_mpic_domain,
+					  irqnr, regs);
 			continue;
 		}
 
diff --git a/drivers/irqchip/irq-atmel-aic.c b/drivers/irqchip/irq-atmel-aic.c
index a82869e9fb26..9a2cf3c1a3a5 100644
--- a/drivers/irqchip/irq-atmel-aic.c
+++ b/drivers/irqchip/irq-atmel-aic.c
@@ -68,12 +68,10 @@ aic_handle(struct pt_regs *regs)
 	irqnr = irq_reg_readl(gc->reg_base + AT91_AIC_IVR);
 	irqstat = irq_reg_readl(gc->reg_base + AT91_AIC_ISR);
 
-	irqnr = irq_find_mapping(aic_domain, irqnr);
-
 	if (!irqstat)
 		irq_reg_writel(0, gc->reg_base + AT91_AIC_EOICR);
 	else
-		handle_IRQ(irqnr, regs);
+		handle_domain_irq(aic_domain, irqnr, regs);
 }
 
 static int aic_retrigger(struct irq_data *d)
diff --git a/drivers/irqchip/irq-atmel-aic5.c b/drivers/irqchip/irq-atmel-aic5.c
index edb227081524..a11aae8fb006 100644
--- a/drivers/irqchip/irq-atmel-aic5.c
+++ b/drivers/irqchip/irq-atmel-aic5.c
@@ -78,12 +78,10 @@ aic5_handle(struct pt_regs *regs)
 	irqnr = irq_reg_readl(gc->reg_base + AT91_AIC5_IVR);
 	irqstat = irq_reg_readl(gc->reg_base + AT91_AIC5_ISR);
 
-	irqnr = irq_find_mapping(aic5_domain, irqnr);
-
 	if (!irqstat)
 		irq_reg_writel(0, gc->reg_base + AT91_AIC5_EOICR);
 	else
-		handle_IRQ(irqnr, regs);
+		handle_domain_irq(aic5_domain, irqnr, regs);
 }
 
 static void aic5_mask(struct irq_data *d)
@@ -297,6 +295,7 @@ static void __init sama5d3_aic_irq_fixup(struct device_node *root)
 
 static const struct of_device_id __initdata aic5_irq_fixups[] = {
 	{ .compatible = "atmel,sama5d3", .data = sama5d3_aic_irq_fixup },
+	{ .compatible = "atmel,sama5d4", .data = sama5d3_aic_irq_fixup },
 	{ /* sentinel */ },
 };
 
@@ -343,7 +342,7 @@ static int __init aic5_of_init(struct device_node *node,
 	return 0;
 }
 
-#define NR_SAMA5D3_IRQS		50
+#define NR_SAMA5D3_IRQS		48
 
 static int __init sama5d3_aic5_of_init(struct device_node *node,
 				       struct device_node *parent)
@@ -351,3 +350,12 @@ static int __init sama5d3_aic5_of_init(struct device_node *node,
 	return aic5_of_init(node, parent, NR_SAMA5D3_IRQS);
 }
 IRQCHIP_DECLARE(sama5d3_aic5, "atmel,sama5d3-aic", sama5d3_aic5_of_init);
+
+#define NR_SAMA5D4_IRQS		68
+
+static int __init sama5d4_aic5_of_init(struct device_node *node,
+				       struct device_node *parent)
+{
+	return aic5_of_init(node, parent, NR_SAMA5D4_IRQS);
+}
+IRQCHIP_DECLARE(sama5d4_aic5, "atmel,sama5d4-aic", sama5d4_aic5_of_init);
diff --git a/drivers/irqchip/irq-bcm7120-l2.c b/drivers/irqchip/irq-bcm7120-l2.c
new file mode 100644
index 000000000000..b9f4fb808e49
--- /dev/null
+++ b/drivers/irqchip/irq-bcm7120-l2.c
@@ -0,0 +1,219 @@
+/*
+ * Broadcom BCM7120 style Level 2 interrupt controller driver
+ *
+ * Copyright (C) 2014 Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME	": " fmt
+
+#include <linux/init.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <linux/of_platform.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/irqdomain.h>
+#include <linux/reboot.h>
+#include <linux/irqchip/chained_irq.h>
+
+#include "irqchip.h"
+
+#include <asm/mach/irq.h>
+
+/* Register offset in the L2 interrupt controller */
+#define IRQEN		0x00
+#define IRQSTAT		0x04
+
+struct bcm7120_l2_intc_data {
+	void __iomem *base;
+	struct irq_domain *domain;
+	bool can_wake;
+	u32 irq_fwd_mask;
+	u32 irq_map_mask;
+	u32 saved_mask;
+};
+
+static void bcm7120_l2_intc_irq_handle(unsigned int irq, struct irq_desc *desc)
+{
+	struct bcm7120_l2_intc_data *b = irq_desc_get_handler_data(desc);
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+	u32 status;
+
+	chained_irq_enter(chip, desc);
+
+	status = __raw_readl(b->base + IRQSTAT);
+
+	if (status == 0) {
+		do_bad_IRQ(irq, desc);
+		goto out;
+	}
+
+	do {
+		irq = ffs(status) - 1;
+		status &= ~(1 << irq);
+		generic_handle_irq(irq_find_mapping(b->domain, irq));
+	} while (status);
+
+out:
+	chained_irq_exit(chip, desc);
+}
+
+static void bcm7120_l2_intc_suspend(struct irq_data *d)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	struct bcm7120_l2_intc_data *b = gc->private;
+	u32 reg;
+
+	irq_gc_lock(gc);
+	/* Save the current mask and the interrupt forward mask */
+	b->saved_mask = __raw_readl(b->base) | b->irq_fwd_mask;
+	if (b->can_wake) {
+		reg = b->saved_mask | gc->wake_active;
+		__raw_writel(reg, b->base);
+	}
+	irq_gc_unlock(gc);
+}
+
+static void bcm7120_l2_intc_resume(struct irq_data *d)
+{
+	struct irq_chip_generic *gc = irq_data_get_irq_chip_data(d);
+	struct bcm7120_l2_intc_data *b = gc->private;
+
+	/* Restore the saved mask */
+	irq_gc_lock(gc);
+	__raw_writel(b->saved_mask, b->base);
+	irq_gc_unlock(gc);
+}
+
+static int bcm7120_l2_intc_init_one(struct device_node *dn,
+					struct bcm7120_l2_intc_data *data,
+					int irq, const __be32 *map_mask)
+{
+	int parent_irq;
+
+	parent_irq = irq_of_parse_and_map(dn, irq);
+	if (parent_irq < 0) {
+		pr_err("failed to map interrupt %d\n", irq);
+		return parent_irq;
+	}
+
+	data->irq_map_mask |= be32_to_cpup(map_mask + irq);
+
+	irq_set_handler_data(parent_irq, data);
+	irq_set_chained_handler(parent_irq, bcm7120_l2_intc_irq_handle);
+
+	return 0;
+}
+
+int __init bcm7120_l2_intc_of_init(struct device_node *dn,
+					struct device_node *parent)
+{
+	unsigned int clr = IRQ_NOREQUEST | IRQ_NOPROBE | IRQ_NOAUTOEN;
+	struct bcm7120_l2_intc_data *data;
+	struct irq_chip_generic *gc;
+	struct irq_chip_type *ct;
+	const __be32 *map_mask;
+	int num_parent_irqs;
+	int ret = 0, len, irq;
+
+	data = kzalloc(sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	data->base = of_iomap(dn, 0);
+	if (!data->base) {
+		pr_err("failed to remap intc L2 registers\n");
+		ret = -ENOMEM;
+		goto out_free;
+	}
+
+	if (of_property_read_u32(dn, "brcm,int-fwd-mask", &data->irq_fwd_mask))
+		data->irq_fwd_mask = 0;
+
+	/* Enable all interrupt specified in the interrupt forward mask and have
+	 * the other disabled
+	 */
+	__raw_writel(data->irq_fwd_mask, data->base + IRQEN);
+
+	num_parent_irqs = of_irq_count(dn);
+	if (num_parent_irqs <= 0) {
+		pr_err("invalid number of parent interrupts\n");
+		ret = -ENOMEM;
+		goto out_unmap;
+	}
+
+	map_mask = of_get_property(dn, "brcm,int-map-mask", &len);
+	if (!map_mask || (len != (sizeof(*map_mask) * num_parent_irqs))) {
+		pr_err("invalid brcm,int-map-mask property\n");
+		ret = -EINVAL;
+		goto out_unmap;
+	}
+
+	for (irq = 0; irq < num_parent_irqs; irq++) {
+		ret = bcm7120_l2_intc_init_one(dn, data, irq, map_mask);
+		if (ret)
+			goto out_unmap;
+	}
+
+	data->domain = irq_domain_add_linear(dn, 32,
+					&irq_generic_chip_ops, NULL);
+	if (!data->domain) {
+		ret = -ENOMEM;
+		goto out_unmap;
+	}
+
+	ret = irq_alloc_domain_generic_chips(data->domain, 32, 1,
+				dn->full_name, handle_level_irq, clr, 0,
+				IRQ_GC_INIT_MASK_CACHE);
+	if (ret) {
+		pr_err("failed to allocate generic irq chip\n");
+		goto out_free_domain;
+	}
+
+	gc = irq_get_domain_generic_chip(data->domain, 0);
+	gc->unused = 0xfffffff & ~data->irq_map_mask;
+	gc->reg_base = data->base;
+	gc->private = data;
+	ct = gc->chip_types;
+
+	ct->regs.mask = IRQEN;
+	ct->chip.irq_mask = irq_gc_mask_clr_bit;
+	ct->chip.irq_unmask = irq_gc_mask_set_bit;
+	ct->chip.irq_ack = irq_gc_noop;
+	ct->chip.irq_suspend = bcm7120_l2_intc_suspend;
+	ct->chip.irq_resume = bcm7120_l2_intc_resume;
+
+	if (of_property_read_bool(dn, "brcm,irq-can-wake")) {
+		data->can_wake = true;
+		/* This IRQ chip can wake the system, set all relevant child
+		 * interupts in wake_enabled mask
+		 */
+		gc->wake_enabled = 0xffffffff;
+		gc->wake_enabled &= ~gc->unused;
+		ct->chip.irq_set_wake = irq_gc_set_wake;
+	}
+
+	pr_info("registered BCM7120 L2 intc (mem: 0x%p, parent IRQ(s): %d)\n",
+			data->base, num_parent_irqs);
+
+	return 0;
+
+out_free_domain:
+	irq_domain_remove(data->domain);
+out_unmap:
+	iounmap(data->base);
+out_free:
+	kfree(data);
+	return ret;
+}
+IRQCHIP_DECLARE(brcmstb_l2_intc, "brcm,bcm7120-l2-intc",
+		bcm7120_l2_intc_of_init);
diff --git a/drivers/irqchip/irq-clps711x.c b/drivers/irqchip/irq-clps711x.c
index 33340dc97d1d..33127f131d78 100644
--- a/drivers/irqchip/irq-clps711x.c
+++ b/drivers/irqchip/irq-clps711x.c
@@ -76,24 +76,20 @@ static struct {
 
 static asmlinkage void __exception_irq_entry clps711x_irqh(struct pt_regs *regs)
 {
-	u32 irqnr, irqstat;
+	u32 irqstat;
 
 	do {
 		irqstat = readw_relaxed(clps711x_intc->intmr[0]) &
 			  readw_relaxed(clps711x_intc->intsr[0]);
-		if (irqstat) {
-			irqnr =	irq_find_mapping(clps711x_intc->domain,
-						 fls(irqstat) - 1);
-			handle_IRQ(irqnr, regs);
-		}
+		if (irqstat)
+			handle_domain_irq(clps711x_intc->domain,
+					  fls(irqstat) - 1, regs);
 
 		irqstat = readw_relaxed(clps711x_intc->intmr[1]) &
 			  readw_relaxed(clps711x_intc->intsr[1]);
-		if (irqstat) {
-			irqnr =	irq_find_mapping(clps711x_intc->domain,
-						 fls(irqstat) - 1 + 16);
-			handle_IRQ(irqnr, regs);
-		}
+		if (irqstat)
+			handle_domain_irq(clps711x_intc->domain,
+					  fls(irqstat) - 1 + 16, regs);
 	} while (irqstat);
 }
 
diff --git a/drivers/irqchip/irq-crossbar.c b/drivers/irqchip/irq-crossbar.c
index 85c2985d8bcb..bbbaf5de65d2 100644
--- a/drivers/irqchip/irq-crossbar.c
+++ b/drivers/irqchip/irq-crossbar.c
@@ -220,7 +220,7 @@ static int __init crossbar_of_init(struct device_node *node)
 			of_property_read_u32_index(node,
 						   "ti,irqs-reserved",
 						   i, &entry);
-			if (entry > max) {
+			if (entry >= max) {
 				pr_err("Invalid reserved entry\n");
 				ret = -EINVAL;
 				goto err_irq_map;
@@ -238,7 +238,7 @@ static int __init crossbar_of_init(struct device_node *node)
 			of_property_read_u32_index(node,
 						   "ti,irqs-skip",
 						   i, &entry);
-			if (entry > max) {
+			if (entry >= max) {
 				pr_err("Invalid skip entry\n");
 				ret = -EINVAL;
 				goto err_irq_map;
diff --git a/drivers/irqchip/irq-gic-common.c b/drivers/irqchip/irq-gic-common.c
index 60ac704d2090..61541ff24397 100644
--- a/drivers/irqchip/irq-gic-common.c
+++ b/drivers/irqchip/irq-gic-common.c
@@ -74,20 +74,22 @@ void __init gic_dist_config(void __iomem *base, int gic_irqs,
 	 * Set all global interrupts to be level triggered, active low.
 	 */
 	for (i = 32; i < gic_irqs; i += 16)
-		writel_relaxed(0, base + GIC_DIST_CONFIG + i / 4);
+		writel_relaxed(GICD_INT_ACTLOW_LVLTRIG,
+					base + GIC_DIST_CONFIG + i / 4);
 
 	/*
 	 * Set priority on all global interrupts.
 	 */
 	for (i = 32; i < gic_irqs; i += 4)
-		writel_relaxed(0xa0a0a0a0, base + GIC_DIST_PRI + i);
+		writel_relaxed(GICD_INT_DEF_PRI_X4, base + GIC_DIST_PRI + i);
 
 	/*
 	 * Disable all interrupts.  Leave the PPI and SGIs alone
 	 * as they are enabled by redistributor registers.
 	 */
 	for (i = 32; i < gic_irqs; i += 32)
-		writel_relaxed(0xffffffff, base + GIC_DIST_ENABLE_CLEAR + i / 8);
+		writel_relaxed(GICD_INT_EN_CLR_X32,
+					base + GIC_DIST_ENABLE_CLEAR + i / 8);
 
 	if (sync_access)
 		sync_access();
@@ -101,14 +103,15 @@ void gic_cpu_config(void __iomem *base, void (*sync_access)(void))
 	 * Deal with the banked PPI and SGI interrupts - disable all
 	 * PPI interrupts, ensure all SGI interrupts are enabled.
 	 */
-	writel_relaxed(0xffff0000, base + GIC_DIST_ENABLE_CLEAR);
-	writel_relaxed(0x0000ffff, base + GIC_DIST_ENABLE_SET);
+	writel_relaxed(GICD_INT_EN_CLR_PPI, base + GIC_DIST_ENABLE_CLEAR);
+	writel_relaxed(GICD_INT_EN_SET_SGI, base + GIC_DIST_ENABLE_SET);
 
 	/*
 	 * Set priority on PPI and SGI interrupts
 	 */
 	for (i = 0; i < 32; i += 4)
-		writel_relaxed(0xa0a0a0a0, base + GIC_DIST_PRI + i * 4 / 4);
+		writel_relaxed(GICD_INT_DEF_PRI_X4,
+					base + GIC_DIST_PRI + i * 4 / 4);
 
 	if (sync_access)
 		sync_access();
diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index 57eaa5a0b1e3..aa17ae805a70 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -16,6 +16,7 @@
  */
 
 #include <linux/cpu.h>
+#include <linux/cpu_pm.h>
 #include <linux/delay.h>
 #include <linux/interrupt.h>
 #include <linux/of.h>
@@ -36,7 +37,7 @@
 struct gic_chip_data {
 	void __iomem		*dist_base;
 	void __iomem		**redist_base;
-	void __percpu __iomem	**rdist;
+	void __iomem * __percpu	*rdist;
 	struct irq_domain	*domain;
 	u64			redist_stride;
 	u32			redist_regions;
@@ -104,7 +105,7 @@ static void gic_redist_wait_for_rwp(void)
 }
 
 /* Low level accessors */
-static u64 gic_read_iar(void)
+static u64 __maybe_unused gic_read_iar(void)
 {
 	u64 irqstat;
 
@@ -112,24 +113,24 @@ static u64 gic_read_iar(void)
 	return irqstat;
 }
 
-static void gic_write_pmr(u64 val)
+static void __maybe_unused gic_write_pmr(u64 val)
 {
 	asm volatile("msr_s " __stringify(ICC_PMR_EL1) ", %0" : : "r" (val));
 }
 
-static void gic_write_ctlr(u64 val)
+static void __maybe_unused gic_write_ctlr(u64 val)
 {
 	asm volatile("msr_s " __stringify(ICC_CTLR_EL1) ", %0" : : "r" (val));
 	isb();
 }
 
-static void gic_write_grpen1(u64 val)
+static void __maybe_unused gic_write_grpen1(u64 val)
 {
 	asm volatile("msr_s " __stringify(ICC_GRPEN1_EL1) ", %0" : : "r" (val));
 	isb();
 }
 
-static void gic_write_sgi1r(u64 val)
+static void __maybe_unused gic_write_sgi1r(u64 val)
 {
 	asm volatile("msr_s " __stringify(ICC_SGI1R_EL1) ", %0" : : "r" (val));
 }
@@ -155,7 +156,7 @@ static void gic_enable_sre(void)
 		pr_err("GIC: unable to set SRE (disabled at EL2), panic ahead\n");
 }
 
-static void gic_enable_redist(void)
+static void gic_enable_redist(bool enable)
 {
 	void __iomem *rbase;
 	u32 count = 1000000;	/* 1s! */
@@ -163,20 +164,30 @@ static void gic_enable_redist(void)
 
 	rbase = gic_data_rdist_rd_base();
 
-	/* Wake up this CPU redistributor */
 	val = readl_relaxed(rbase + GICR_WAKER);
-	val &= ~GICR_WAKER_ProcessorSleep;
+	if (enable)
+		/* Wake up this CPU redistributor */
+		val &= ~GICR_WAKER_ProcessorSleep;
+	else
+		val |= GICR_WAKER_ProcessorSleep;
 	writel_relaxed(val, rbase + GICR_WAKER);
 
-	while (readl_relaxed(rbase + GICR_WAKER) & GICR_WAKER_ChildrenAsleep) {
-		count--;
-		if (!count) {
-			pr_err_ratelimited("redist didn't wake up...\n");
-			return;
-		}
+	if (!enable) {		/* Check that GICR_WAKER is writeable */
+		val = readl_relaxed(rbase + GICR_WAKER);
+		if (!(val & GICR_WAKER_ProcessorSleep))
+			return;	/* No PM support in this redistributor */
+	}
+
+	while (count--) {
+		val = readl_relaxed(rbase + GICR_WAKER);
+		if (enable ^ (val & GICR_WAKER_ChildrenAsleep))
+			break;
 		cpu_relax();
 		udelay(1);
 	};
+	if (!count)
+		pr_err_ratelimited("redistributor failed to %s...\n",
+				   enable ? "wakeup" : "sleep");
 }
 
 /*
@@ -200,19 +211,6 @@ static void gic_poke_irq(struct irq_data *d, u32 offset)
 	rwp_wait();
 }
 
-static int gic_peek_irq(struct irq_data *d, u32 offset)
-{
-	u32 mask = 1 << (gic_irq(d) % 32);
-	void __iomem *base;
-
-	if (gic_irq_in_rdist(d))
-		base = gic_data_rdist_sgi_base();
-	else
-		base = gic_data.dist_base;
-
-	return !!(readl_relaxed(base + offset + (gic_irq(d) / 32) * 4) & mask);
-}
-
 static void gic_mask_irq(struct irq_data *d)
 {
 	gic_poke_irq(d, GICD_ICENABLER);
@@ -274,14 +272,13 @@ static asmlinkage void __exception_irq_entry gic_handle_irq(struct pt_regs *regs
 		irqnr = gic_read_iar();
 
 		if (likely(irqnr > 15 && irqnr < 1020)) {
-			u64 irq = irq_find_mapping(gic_data.domain, irqnr);
-			if (likely(irq)) {
-				handle_IRQ(irq, regs);
-				continue;
+			int err;
+			err = handle_domain_irq(gic_data.domain, irqnr, regs);
+			if (err) {
+				WARN_ONCE(true, "Unexpected SPI received!\n");
+				gic_write_eoir(irqnr);
 			}
-
-			WARN_ONCE(true, "Unexpected SPI received!\n");
-			gic_write_eoir(irqnr);
+			continue;
 		}
 		if (irqnr < 16) {
 			gic_write_eoir(irqnr);
@@ -373,6 +370,21 @@ static int gic_populate_rdist(void)
 	return -ENODEV;
 }
 
+static void gic_cpu_sys_reg_init(void)
+{
+	/* Enable system registers */
+	gic_enable_sre();
+
+	/* Set priority mask register */
+	gic_write_pmr(DEFAULT_PMR_VALUE);
+
+	/* EOI deactivates interrupt too (mode 0) */
+	gic_write_ctlr(ICC_CTLR_EL1_EOImode_drop_dir);
+
+	/* ... and let's hit the road... */
+	gic_write_grpen1(1);
+}
+
 static void gic_cpu_init(void)
 {
 	void __iomem *rbase;
@@ -381,26 +393,30 @@ static void gic_cpu_init(void)
 	if (gic_populate_rdist())
 		return;
 
-	gic_enable_redist();
+	gic_enable_redist(true);
 
 	rbase = gic_data_rdist_sgi_base();
 
 	gic_cpu_config(rbase, gic_redist_wait_for_rwp);
 
-	/* Enable system registers */
-	gic_enable_sre();
+	/* initialise system registers */
+	gic_cpu_sys_reg_init();
+}
 
-	/* Set priority mask register */
-	gic_write_pmr(DEFAULT_PMR_VALUE);
+#ifdef CONFIG_SMP
+static int gic_peek_irq(struct irq_data *d, u32 offset)
+{
+	u32 mask = 1 << (gic_irq(d) % 32);
+	void __iomem *base;
 
-	/* EOI deactivates interrupt too (mode 0) */
-	gic_write_ctlr(ICC_CTLR_EL1_EOImode_drop_dir);
+	if (gic_irq_in_rdist(d))
+		base = gic_data_rdist_sgi_base();
+	else
+		base = gic_data.dist_base;
 
-	/* ... and let's hit the road... */
-	gic_write_grpen1(1);
+	return !!(readl_relaxed(base + offset + (gic_irq(d) / 32) * 4) & mask);
 }
 
-#ifdef CONFIG_SMP
 static int gic_secondary_init(struct notifier_block *nfb,
 			      unsigned long action, void *hcpu)
 {
@@ -533,6 +549,33 @@ static int gic_set_affinity(struct irq_data *d, const struct cpumask *mask_val,
 #define gic_smp_init()		do { } while(0)
 #endif
 
+#ifdef CONFIG_CPU_PM
+static int gic_cpu_pm_notifier(struct notifier_block *self,
+			       unsigned long cmd, void *v)
+{
+	if (cmd == CPU_PM_EXIT) {
+		gic_enable_redist(true);
+		gic_cpu_sys_reg_init();
+	} else if (cmd == CPU_PM_ENTER) {
+		gic_write_grpen1(0);
+		gic_enable_redist(false);
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block gic_cpu_pm_notifier_block = {
+	.notifier_call = gic_cpu_pm_notifier,
+};
+
+static void gic_cpu_pm_init(void)
+{
+	cpu_pm_register_notifier(&gic_cpu_pm_notifier_block);
+}
+
+#else
+static inline void gic_cpu_pm_init(void) { }
+#endif /* CONFIG_CPU_PM */
+
 static struct irq_chip gic_chip = {
 	.name			= "GICv3",
 	.irq_mask		= gic_mask_irq,
@@ -672,6 +715,7 @@ static int __init gic_of_init(struct device_node *node, struct device_node *pare
 	gic_smp_init();
 	gic_dist_init();
 	gic_cpu_init();
+	gic_cpu_pm_init();
 
 	return 0;
 
diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 4b959e606fe8..38493ff28fa5 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -102,7 +102,7 @@ static struct gic_chip_data gic_data[MAX_GIC_NR] __read_mostly;
 #ifdef CONFIG_GIC_NON_BANKED
 static void __iomem *gic_get_percpu_base(union gic_base *base)
 {
-	return *__this_cpu_ptr(base->percpu_base);
+	return raw_cpu_read(*base->percpu_base);
 }
 
 static void __iomem *gic_get_common_base(union gic_base *base)
@@ -270,8 +270,7 @@ static void __exception_irq_entry gic_handle_irq(struct pt_regs *regs)
 		irqnr = irqstat & GICC_IAR_INT_ID_MASK;
 
 		if (likely(irqnr > 15 && irqnr < 1021)) {
-			irqnr = irq_find_mapping(gic->domain, irqnr);
-			handle_IRQ(irqnr, regs);
+			handle_domain_irq(gic->domain, irqnr, regs);
 			continue;
 		}
 		if (irqnr < 16) {
@@ -298,8 +297,8 @@ static void gic_handle_cascade_irq(unsigned int irq, struct irq_desc *desc)
 	status = readl_relaxed(gic_data_cpu_base(chip_data) + GIC_CPU_INTACK);
 	raw_spin_unlock(&irq_controller_lock);
 
-	gic_irq = (status & 0x3ff);
-	if (gic_irq == 1023)
+	gic_irq = (status & GICC_IAR_INT_ID_MASK);
+	if (gic_irq == GICC_INT_SPURIOUS)
 		goto out;
 
 	cascade_irq = irq_find_mapping(chip_data->domain, gic_irq);
@@ -353,6 +352,21 @@ static u8 gic_get_cpumask(struct gic_chip_data *gic)
 	return mask;
 }
 
+static void gic_cpu_if_up(void)
+{
+	void __iomem *cpu_base = gic_data_cpu_base(&gic_data[0]);
+	u32 bypass = 0;
+
+	/*
+	* Preserve bypass disable bits to be written back later
+	*/
+	bypass = readl(cpu_base + GIC_CPU_CTRL);
+	bypass &= GICC_DIS_BYPASS_MASK;
+
+	writel_relaxed(bypass | GICC_ENABLE, cpu_base + GIC_CPU_CTRL);
+}
+
+
 static void __init gic_dist_init(struct gic_chip_data *gic)
 {
 	unsigned int i;
@@ -360,7 +374,7 @@ static void __init gic_dist_init(struct gic_chip_data *gic)
 	unsigned int gic_irqs = gic->gic_irqs;
 	void __iomem *base = gic_data_dist_base(gic);
 
-	writel_relaxed(0, base + GIC_DIST_CTRL);
+	writel_relaxed(GICD_DISABLE, base + GIC_DIST_CTRL);
 
 	/*
 	 * Set all global interrupts to this CPU only.
@@ -373,7 +387,7 @@ static void __init gic_dist_init(struct gic_chip_data *gic)
 
 	gic_dist_config(base, gic_irqs, NULL);
 
-	writel_relaxed(1, base + GIC_DIST_CTRL);
+	writel_relaxed(GICD_ENABLE, base + GIC_DIST_CTRL);
 }
 
 static void gic_cpu_init(struct gic_chip_data *gic)
@@ -400,14 +414,18 @@ static void gic_cpu_init(struct gic_chip_data *gic)
 
 	gic_cpu_config(dist_base, NULL);
 
-	writel_relaxed(0xf0, base + GIC_CPU_PRIMASK);
-	writel_relaxed(1, base + GIC_CPU_CTRL);
+	writel_relaxed(GICC_INT_PRI_THRESHOLD, base + GIC_CPU_PRIMASK);
+	gic_cpu_if_up();
 }
 
 void gic_cpu_if_down(void)
 {
 	void __iomem *cpu_base = gic_data_cpu_base(&gic_data[0]);
-	writel_relaxed(0, cpu_base + GIC_CPU_CTRL);
+	u32 val = 0;
+
+	val = readl(cpu_base + GIC_CPU_CTRL);
+	val &= ~GICC_ENABLE;
+	writel_relaxed(val, cpu_base + GIC_CPU_CTRL);
 }
 
 #ifdef CONFIG_CPU_PM
@@ -467,14 +485,14 @@ static void gic_dist_restore(unsigned int gic_nr)
 	if (!dist_base)
 		return;
 
-	writel_relaxed(0, dist_base + GIC_DIST_CTRL);
+	writel_relaxed(GICD_DISABLE, dist_base + GIC_DIST_CTRL);
 
 	for (i = 0; i < DIV_ROUND_UP(gic_irqs, 16); i++)
 		writel_relaxed(gic_data[gic_nr].saved_spi_conf[i],
 			dist_base + GIC_DIST_CONFIG + i * 4);
 
 	for (i = 0; i < DIV_ROUND_UP(gic_irqs, 4); i++)
-		writel_relaxed(0xa0a0a0a0,
+		writel_relaxed(GICD_INT_DEF_PRI_X4,
 			dist_base + GIC_DIST_PRI + i * 4);
 
 	for (i = 0; i < DIV_ROUND_UP(gic_irqs, 4); i++)
@@ -485,7 +503,7 @@ static void gic_dist_restore(unsigned int gic_nr)
 		writel_relaxed(gic_data[gic_nr].saved_spi_enable[i],
 			dist_base + GIC_DIST_ENABLE_SET + i * 4);
 
-	writel_relaxed(1, dist_base + GIC_DIST_CTRL);
+	writel_relaxed(GICD_ENABLE, dist_base + GIC_DIST_CTRL);
 }
 
 static void gic_cpu_save(unsigned int gic_nr)
@@ -504,11 +522,11 @@ static void gic_cpu_save(unsigned int gic_nr)
 	if (!dist_base || !cpu_base)
 		return;
 
-	ptr = __this_cpu_ptr(gic_data[gic_nr].saved_ppi_enable);
+	ptr = raw_cpu_ptr(gic_data[gic_nr].saved_ppi_enable);
 	for (i = 0; i < DIV_ROUND_UP(32, 32); i++)
 		ptr[i] = readl_relaxed(dist_base + GIC_DIST_ENABLE_SET + i * 4);
 
-	ptr = __this_cpu_ptr(gic_data[gic_nr].saved_ppi_conf);
+	ptr = raw_cpu_ptr(gic_data[gic_nr].saved_ppi_conf);
 	for (i = 0; i < DIV_ROUND_UP(32, 16); i++)
 		ptr[i] = readl_relaxed(dist_base + GIC_DIST_CONFIG + i * 4);
 
@@ -530,19 +548,20 @@ static void gic_cpu_restore(unsigned int gic_nr)
 	if (!dist_base || !cpu_base)
 		return;
 
-	ptr = __this_cpu_ptr(gic_data[gic_nr].saved_ppi_enable);
+	ptr = raw_cpu_ptr(gic_data[gic_nr].saved_ppi_enable);
 	for (i = 0; i < DIV_ROUND_UP(32, 32); i++)
 		writel_relaxed(ptr[i], dist_base + GIC_DIST_ENABLE_SET + i * 4);
 
-	ptr = __this_cpu_ptr(gic_data[gic_nr].saved_ppi_conf);
+	ptr = raw_cpu_ptr(gic_data[gic_nr].saved_ppi_conf);
 	for (i = 0; i < DIV_ROUND_UP(32, 16); i++)
 		writel_relaxed(ptr[i], dist_base + GIC_DIST_CONFIG + i * 4);
 
 	for (i = 0; i < DIV_ROUND_UP(32, 4); i++)
-		writel_relaxed(0xa0a0a0a0, dist_base + GIC_DIST_PRI + i * 4);
+		writel_relaxed(GICD_INT_DEF_PRI_X4,
+					dist_base + GIC_DIST_PRI + i * 4);
 
-	writel_relaxed(0xf0, cpu_base + GIC_CPU_PRIMASK);
-	writel_relaxed(1, cpu_base + GIC_CPU_CTRL);
+	writel_relaxed(GICC_INT_PRI_THRESHOLD, cpu_base + GIC_CPU_PRIMASK);
+	gic_cpu_if_up();
 }
 
 static int gic_notifier(struct notifier_block *self, unsigned long cmd,	void *v)
@@ -867,7 +886,7 @@ static int gic_routable_irq_domain_xlate(struct irq_domain *d,
 	return 0;
 }
 
-const struct irq_domain_ops gic_default_routable_irq_domain_ops = {
+static const struct irq_domain_ops gic_default_routable_irq_domain_ops = {
 	.map = gic_routable_irq_domain_map,
 	.unmap = gic_routable_irq_domain_unmap,
 	.xlate = gic_routable_irq_domain_xlate,
diff --git a/drivers/irqchip/irq-hip04.c b/drivers/irqchip/irq-hip04.c
new file mode 100644
index 000000000000..9c8f833522e6
--- /dev/null
+++ b/drivers/irqchip/irq-hip04.c
@@ -0,0 +1,424 @@
+/*
+ * Hisilicon HiP04 INTC
+ *
+ * Copyright (C) 2002-2014 ARM Limited.
+ * Copyright (c) 2013-2014 Hisilicon Ltd.
+ * Copyright (c) 2013-2014 Linaro Ltd.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * Interrupt architecture for the HIP04 INTC:
+ *
+ * o There is one Interrupt Distributor, which receives interrupts
+ *   from system devices and sends them to the Interrupt Controllers.
+ *
+ * o There is one CPU Interface per CPU, which sends interrupts sent
+ *   by the Distributor, and interrupts generated locally, to the
+ *   associated CPU. The base address of the CPU interface is usually
+ *   aliased so that the same address points to different chips depending
+ *   on the CPU it is accessed from.
+ *
+ * Note that IRQs 0-31 are special - they are local to each CPU.
+ * As such, the enable set/clear, pending set/clear and active bit
+ * registers are banked per-cpu for these sources.
+ */
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/list.h>
+#include <linux/smp.h>
+#include <linux/cpu.h>
+#include <linux/cpu_pm.h>
+#include <linux/cpumask.h>
+#include <linux/io.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/irqdomain.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/irqchip/arm-gic.h>
+
+#include <asm/irq.h>
+#include <asm/exception.h>
+#include <asm/smp_plat.h>
+
+#include "irq-gic-common.h"
+#include "irqchip.h"
+
+#define HIP04_MAX_IRQS		510
+
+struct hip04_irq_data {
+	void __iomem *dist_base;
+	void __iomem *cpu_base;
+	struct irq_domain *domain;
+	unsigned int nr_irqs;
+};
+
+static DEFINE_RAW_SPINLOCK(irq_controller_lock);
+
+/*
+ * The GIC mapping of CPU interfaces does not necessarily match
+ * the logical CPU numbering.  Let's use a mapping as returned
+ * by the GIC itself.
+ */
+#define NR_HIP04_CPU_IF 16
+static u16 hip04_cpu_map[NR_HIP04_CPU_IF] __read_mostly;
+
+static struct hip04_irq_data hip04_data __read_mostly;
+
+static inline void __iomem *hip04_dist_base(struct irq_data *d)
+{
+	struct hip04_irq_data *hip04_data = irq_data_get_irq_chip_data(d);
+	return hip04_data->dist_base;
+}
+
+static inline void __iomem *hip04_cpu_base(struct irq_data *d)
+{
+	struct hip04_irq_data *hip04_data = irq_data_get_irq_chip_data(d);
+	return hip04_data->cpu_base;
+}
+
+static inline unsigned int hip04_irq(struct irq_data *d)
+{
+	return d->hwirq;
+}
+
+/*
+ * Routines to acknowledge, disable and enable interrupts
+ */
+static void hip04_mask_irq(struct irq_data *d)
+{
+	u32 mask = 1 << (hip04_irq(d) % 32);
+
+	raw_spin_lock(&irq_controller_lock);
+	writel_relaxed(mask, hip04_dist_base(d) + GIC_DIST_ENABLE_CLEAR +
+		       (hip04_irq(d) / 32) * 4);
+	raw_spin_unlock(&irq_controller_lock);
+}
+
+static void hip04_unmask_irq(struct irq_data *d)
+{
+	u32 mask = 1 << (hip04_irq(d) % 32);
+
+	raw_spin_lock(&irq_controller_lock);
+	writel_relaxed(mask, hip04_dist_base(d) + GIC_DIST_ENABLE_SET +
+		       (hip04_irq(d) / 32) * 4);
+	raw_spin_unlock(&irq_controller_lock);
+}
+
+static void hip04_eoi_irq(struct irq_data *d)
+{
+	writel_relaxed(hip04_irq(d), hip04_cpu_base(d) + GIC_CPU_EOI);
+}
+
+static int hip04_irq_set_type(struct irq_data *d, unsigned int type)
+{
+	void __iomem *base = hip04_dist_base(d);
+	unsigned int irq = hip04_irq(d);
+
+	/* Interrupt configuration for SGIs can't be changed */
+	if (irq < 16)
+		return -EINVAL;
+
+	if (type != IRQ_TYPE_LEVEL_HIGH && type != IRQ_TYPE_EDGE_RISING)
+		return -EINVAL;
+
+	raw_spin_lock(&irq_controller_lock);
+
+	gic_configure_irq(irq, type, base, NULL);
+
+	raw_spin_unlock(&irq_controller_lock);
+
+	return 0;
+}
+
+#ifdef CONFIG_SMP
+static int hip04_irq_set_affinity(struct irq_data *d,
+				  const struct cpumask *mask_val,
+				  bool force)
+{
+	void __iomem *reg;
+	unsigned int cpu, shift = (hip04_irq(d) % 2) * 16;
+	u32 val, mask, bit;
+
+	if (!force)
+		cpu = cpumask_any_and(mask_val, cpu_online_mask);
+	else
+		cpu = cpumask_first(mask_val);
+
+	if (cpu >= NR_HIP04_CPU_IF || cpu >= nr_cpu_ids)
+		return -EINVAL;
+
+	raw_spin_lock(&irq_controller_lock);
+	reg = hip04_dist_base(d) + GIC_DIST_TARGET + ((hip04_irq(d) * 2) & ~3);
+	mask = 0xffff << shift;
+	bit = hip04_cpu_map[cpu] << shift;
+	val = readl_relaxed(reg) & ~mask;
+	writel_relaxed(val | bit, reg);
+	raw_spin_unlock(&irq_controller_lock);
+
+	return IRQ_SET_MASK_OK;
+}
+#endif
+
+static void __exception_irq_entry hip04_handle_irq(struct pt_regs *regs)
+{
+	u32 irqstat, irqnr;
+	void __iomem *cpu_base = hip04_data.cpu_base;
+
+	do {
+		irqstat = readl_relaxed(cpu_base + GIC_CPU_INTACK);
+		irqnr = irqstat & GICC_IAR_INT_ID_MASK;
+
+		if (likely(irqnr > 15 && irqnr <= HIP04_MAX_IRQS)) {
+			irqnr = irq_find_mapping(hip04_data.domain, irqnr);
+			handle_IRQ(irqnr, regs);
+			continue;
+		}
+		if (irqnr < 16) {
+			writel_relaxed(irqstat, cpu_base + GIC_CPU_EOI);
+#ifdef CONFIG_SMP
+			handle_IPI(irqnr, regs);
+#endif
+			continue;
+		}
+		break;
+	} while (1);
+}
+
+static struct irq_chip hip04_irq_chip = {
+	.name			= "HIP04 INTC",
+	.irq_mask		= hip04_mask_irq,
+	.irq_unmask		= hip04_unmask_irq,
+	.irq_eoi		= hip04_eoi_irq,
+	.irq_set_type		= hip04_irq_set_type,
+#ifdef CONFIG_SMP
+	.irq_set_affinity	= hip04_irq_set_affinity,
+#endif
+};
+
+static u16 hip04_get_cpumask(struct hip04_irq_data *intc)
+{
+	void __iomem *base = intc->dist_base;
+	u32 mask, i;
+
+	for (i = mask = 0; i < 32; i += 2) {
+		mask = readl_relaxed(base + GIC_DIST_TARGET + i * 2);
+		mask |= mask >> 16;
+		if (mask)
+			break;
+	}
+
+	if (!mask)
+		pr_crit("GIC CPU mask not found - kernel will fail to boot.\n");
+
+	return mask;
+}
+
+static void __init hip04_irq_dist_init(struct hip04_irq_data *intc)
+{
+	unsigned int i;
+	u32 cpumask;
+	unsigned int nr_irqs = intc->nr_irqs;
+	void __iomem *base = intc->dist_base;
+
+	writel_relaxed(0, base + GIC_DIST_CTRL);
+
+	/*
+	 * Set all global interrupts to this CPU only.
+	 */
+	cpumask = hip04_get_cpumask(intc);
+	cpumask |= cpumask << 16;
+	for (i = 32; i < nr_irqs; i += 2)
+		writel_relaxed(cpumask, base + GIC_DIST_TARGET + ((i * 2) & ~3));
+
+	gic_dist_config(base, nr_irqs, NULL);
+
+	writel_relaxed(1, base + GIC_DIST_CTRL);
+}
+
+static void hip04_irq_cpu_init(struct hip04_irq_data *intc)
+{
+	void __iomem *dist_base = intc->dist_base;
+	void __iomem *base = intc->cpu_base;
+	unsigned int cpu_mask, cpu = smp_processor_id();
+	int i;
+
+	/*
+	 * Get what the GIC says our CPU mask is.
+	 */
+	BUG_ON(cpu >= NR_HIP04_CPU_IF);
+	cpu_mask = hip04_get_cpumask(intc);
+	hip04_cpu_map[cpu] = cpu_mask;
+
+	/*
+	 * Clear our mask from the other map entries in case they're
+	 * still undefined.
+	 */
+	for (i = 0; i < NR_HIP04_CPU_IF; i++)
+		if (i != cpu)
+			hip04_cpu_map[i] &= ~cpu_mask;
+
+	gic_cpu_config(dist_base, NULL);
+
+	writel_relaxed(0xf0, base + GIC_CPU_PRIMASK);
+	writel_relaxed(1, base + GIC_CPU_CTRL);
+}
+
+#ifdef CONFIG_SMP
+static void hip04_raise_softirq(const struct cpumask *mask, unsigned int irq)
+{
+	int cpu;
+	unsigned long flags, map = 0;
+
+	raw_spin_lock_irqsave(&irq_controller_lock, flags);
+
+	/* Convert our logical CPU mask into a physical one. */
+	for_each_cpu(cpu, mask)
+		map |= hip04_cpu_map[cpu];
+
+	/*
+	 * Ensure that stores to Normal memory are visible to the
+	 * other CPUs before they observe us issuing the IPI.
+	 */
+	dmb(ishst);
+
+	/* this always happens on GIC0 */
+	writel_relaxed(map << 8 | irq, hip04_data.dist_base + GIC_DIST_SOFTINT);
+
+	raw_spin_unlock_irqrestore(&irq_controller_lock, flags);
+}
+#endif
+
+static int hip04_irq_domain_map(struct irq_domain *d, unsigned int irq,
+				irq_hw_number_t hw)
+{
+	if (hw < 32) {
+		irq_set_percpu_devid(irq);
+		irq_set_chip_and_handler(irq, &hip04_irq_chip,
+					 handle_percpu_devid_irq);
+		set_irq_flags(irq, IRQF_VALID | IRQF_NOAUTOEN);
+	} else {
+		irq_set_chip_and_handler(irq, &hip04_irq_chip,
+					 handle_fasteoi_irq);
+		set_irq_flags(irq, IRQF_VALID | IRQF_PROBE);
+	}
+	irq_set_chip_data(irq, d->host_data);
+	return 0;
+}
+
+static int hip04_irq_domain_xlate(struct irq_domain *d,
+				  struct device_node *controller,
+				  const u32 *intspec, unsigned int intsize,
+				  unsigned long *out_hwirq,
+				  unsigned int *out_type)
+{
+	unsigned long ret = 0;
+
+	if (d->of_node != controller)
+		return -EINVAL;
+	if (intsize < 3)
+		return -EINVAL;
+
+	/* Get the interrupt number and add 16 to skip over SGIs */
+	*out_hwirq = intspec[1] + 16;
+
+	/* For SPIs, we need to add 16 more to get the irq ID number */
+	if (!intspec[0])
+		*out_hwirq += 16;
+
+	*out_type = intspec[2] & IRQ_TYPE_SENSE_MASK;
+
+	return ret;
+}
+
+#ifdef CONFIG_SMP
+static int hip04_irq_secondary_init(struct notifier_block *nfb,
+				    unsigned long action,
+				    void *hcpu)
+{
+	if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+		hip04_irq_cpu_init(&hip04_data);
+	return NOTIFY_OK;
+}
+
+/*
+ * Notifier for enabling the INTC CPU interface. Set an arbitrarily high
+ * priority because the GIC needs to be up before the ARM generic timers.
+ */
+static struct notifier_block hip04_irq_cpu_notifier = {
+	.notifier_call	= hip04_irq_secondary_init,
+	.priority	= 100,
+};
+#endif
+
+static const struct irq_domain_ops hip04_irq_domain_ops = {
+	.map	= hip04_irq_domain_map,
+	.xlate	= hip04_irq_domain_xlate,
+};
+
+static int __init
+hip04_of_init(struct device_node *node, struct device_node *parent)
+{
+	irq_hw_number_t hwirq_base = 16;
+	int nr_irqs, irq_base, i;
+
+	if (WARN_ON(!node))
+		return -ENODEV;
+
+	hip04_data.dist_base = of_iomap(node, 0);
+	WARN(!hip04_data.dist_base, "fail to map hip04 intc dist registers\n");
+
+	hip04_data.cpu_base = of_iomap(node, 1);
+	WARN(!hip04_data.cpu_base, "unable to map hip04 intc cpu registers\n");
+
+	/*
+	 * Initialize the CPU interface map to all CPUs.
+	 * It will be refined as each CPU probes its ID.
+	 */
+	for (i = 0; i < NR_HIP04_CPU_IF; i++)
+		hip04_cpu_map[i] = 0xff;
+
+	/*
+	 * Find out how many interrupts are supported.
+	 * The HIP04 INTC only supports up to 510 interrupt sources.
+	 */
+	nr_irqs = readl_relaxed(hip04_data.dist_base + GIC_DIST_CTR) & 0x1f;
+	nr_irqs = (nr_irqs + 1) * 32;
+	if (nr_irqs > HIP04_MAX_IRQS)
+		nr_irqs = HIP04_MAX_IRQS;
+	hip04_data.nr_irqs = nr_irqs;
+
+	nr_irqs -= hwirq_base; /* calculate # of irqs to allocate */
+
+	irq_base = irq_alloc_descs(-1, hwirq_base, nr_irqs, numa_node_id());
+	if (IS_ERR_VALUE(irq_base)) {
+		pr_err("failed to allocate IRQ numbers\n");
+		return -EINVAL;
+	}
+
+	hip04_data.domain = irq_domain_add_legacy(node, nr_irqs, irq_base,
+						  hwirq_base,
+						  &hip04_irq_domain_ops,
+						  &hip04_data);
+
+	if (WARN_ON(!hip04_data.domain))
+		return -EINVAL;
+
+#ifdef CONFIG_SMP
+	set_smp_cross_call(hip04_raise_softirq);
+	register_cpu_notifier(&hip04_irq_cpu_notifier);
+#endif
+	set_handle_irq(hip04_handle_irq);
+
+	hip04_irq_dist_init(&hip04_data);
+	hip04_irq_cpu_init(&hip04_data);
+
+	return 0;
+}
+IRQCHIP_DECLARE(hip04_intc, "hisilicon,hip04-intc", hip04_of_init);
diff --git a/drivers/irqchip/irq-keystone.c b/drivers/irqchip/irq-keystone.c
new file mode 100644
index 000000000000..608abf9c9283
--- /dev/null
+++ b/drivers/irqchip/irq-keystone.c
@@ -0,0 +1,232 @@
+/*
+ * Texas Instruments Keystone IRQ controller IP driver
+ *
+ * Copyright (C) 2014 Texas Instruments, Inc.
+ * Author: Sajesh Kumar Saran <sajesh@ti.com>
+ *	   Grygorii Strashko <grygorii.strashko@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/irq.h>
+#include <linux/bitops.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/irqdomain.h>
+#include <linux/irqchip/chained_irq.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include "irqchip.h"
+
+
+/* The source ID bits start from 4 to 31 (total 28 bits)*/
+#define BIT_OFS			4
+#define KEYSTONE_N_IRQ		(32 - BIT_OFS)
+
+struct keystone_irq_device {
+	struct device		*dev;
+	struct irq_chip		 chip;
+	u32			 mask;
+	int			 irq;
+	struct irq_domain	*irqd;
+	struct regmap		*devctrl_regs;
+	u32			devctrl_offset;
+};
+
+static inline u32 keystone_irq_readl(struct keystone_irq_device *kirq)
+{
+	int ret;
+	u32 val = 0;
+
+	ret = regmap_read(kirq->devctrl_regs, kirq->devctrl_offset, &val);
+	if (ret < 0)
+		dev_dbg(kirq->dev, "irq read failed ret(%d)\n", ret);
+	return val;
+}
+
+static inline void
+keystone_irq_writel(struct keystone_irq_device *kirq, u32 value)
+{
+	int ret;
+
+	ret = regmap_write(kirq->devctrl_regs, kirq->devctrl_offset, value);
+	if (ret < 0)
+		dev_dbg(kirq->dev, "irq write failed ret(%d)\n", ret);
+}
+
+static void keystone_irq_setmask(struct irq_data *d)
+{
+	struct keystone_irq_device *kirq = irq_data_get_irq_chip_data(d);
+
+	kirq->mask |= BIT(d->hwirq);
+	dev_dbg(kirq->dev, "mask %lu [%x]\n", d->hwirq, kirq->mask);
+}
+
+static void keystone_irq_unmask(struct irq_data *d)
+{
+	struct keystone_irq_device *kirq = irq_data_get_irq_chip_data(d);
+
+	kirq->mask &= ~BIT(d->hwirq);
+	dev_dbg(kirq->dev, "unmask %lu [%x]\n", d->hwirq, kirq->mask);
+}
+
+static void keystone_irq_ack(struct irq_data *d)
+{
+	/* nothing to do here */
+}
+
+static void keystone_irq_handler(unsigned irq, struct irq_desc *desc)
+{
+	struct keystone_irq_device *kirq = irq_desc_get_handler_data(desc);
+	unsigned long pending;
+	int src, virq;
+
+	dev_dbg(kirq->dev, "start irq %d\n", irq);
+
+	chained_irq_enter(irq_desc_get_chip(desc), desc);
+
+	pending = keystone_irq_readl(kirq);
+	keystone_irq_writel(kirq, pending);
+
+	dev_dbg(kirq->dev, "pending 0x%lx, mask 0x%x\n", pending, kirq->mask);
+
+	pending = (pending >> BIT_OFS) & ~kirq->mask;
+
+	dev_dbg(kirq->dev, "pending after mask 0x%lx\n", pending);
+
+	for (src = 0; src < KEYSTONE_N_IRQ; src++) {
+		if (BIT(src) & pending) {
+			virq = irq_find_mapping(kirq->irqd, src);
+			dev_dbg(kirq->dev, "dispatch bit %d, virq %d\n",
+				src, virq);
+			if (!virq)
+				dev_warn(kirq->dev, "sporious irq detected hwirq %d, virq %d\n",
+					 src, virq);
+			generic_handle_irq(virq);
+		}
+	}
+
+	chained_irq_exit(irq_desc_get_chip(desc), desc);
+
+	dev_dbg(kirq->dev, "end irq %d\n", irq);
+}
+
+static int keystone_irq_map(struct irq_domain *h, unsigned int virq,
+				irq_hw_number_t hw)
+{
+	struct keystone_irq_device *kirq = h->host_data;
+
+	irq_set_chip_data(virq, kirq);
+	irq_set_chip_and_handler(virq, &kirq->chip, handle_level_irq);
+	set_irq_flags(virq, IRQF_VALID | IRQF_PROBE);
+	return 0;
+}
+
+static struct irq_domain_ops keystone_irq_ops = {
+	.map	= keystone_irq_map,
+	.xlate	= irq_domain_xlate_onecell,
+};
+
+static int keystone_irq_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct keystone_irq_device *kirq;
+	int ret;
+
+	if (np == NULL)
+		return -EINVAL;
+
+	kirq = devm_kzalloc(dev, sizeof(*kirq), GFP_KERNEL);
+	if (!kirq)
+		return -ENOMEM;
+
+	kirq->devctrl_regs =
+		syscon_regmap_lookup_by_phandle(np, "ti,syscon-dev");
+	if (IS_ERR(kirq->devctrl_regs))
+		return PTR_ERR(kirq->devctrl_regs);
+
+	ret = of_property_read_u32_index(np, "ti,syscon-dev", 1,
+					 &kirq->devctrl_offset);
+	if (ret) {
+		dev_err(dev, "couldn't read the devctrl_offset offset!\n");
+		return ret;
+	}
+
+	kirq->irq = platform_get_irq(pdev, 0);
+	if (kirq->irq < 0) {
+		dev_err(dev, "no irq resource %d\n", kirq->irq);
+		return kirq->irq;
+	}
+
+	kirq->dev = dev;
+	kirq->mask = ~0x0;
+	kirq->chip.name		= "keystone-irq";
+	kirq->chip.irq_ack	= keystone_irq_ack;
+	kirq->chip.irq_mask	= keystone_irq_setmask;
+	kirq->chip.irq_unmask	= keystone_irq_unmask;
+
+	kirq->irqd = irq_domain_add_linear(np, KEYSTONE_N_IRQ,
+					   &keystone_irq_ops, kirq);
+	if (!kirq->irqd) {
+		dev_err(dev, "IRQ domain registration failed\n");
+		return -ENODEV;
+	}
+
+	platform_set_drvdata(pdev, kirq);
+
+	irq_set_chained_handler(kirq->irq, keystone_irq_handler);
+	irq_set_handler_data(kirq->irq, kirq);
+
+	/* clear all source bits */
+	keystone_irq_writel(kirq, ~0x0);
+
+	dev_info(dev, "irqchip registered, nr_irqs %u\n", KEYSTONE_N_IRQ);
+
+	return 0;
+}
+
+static int keystone_irq_remove(struct platform_device *pdev)
+{
+	struct keystone_irq_device *kirq = platform_get_drvdata(pdev);
+	int hwirq;
+
+	for (hwirq = 0; hwirq < KEYSTONE_N_IRQ; hwirq++)
+		irq_dispose_mapping(irq_find_mapping(kirq->irqd, hwirq));
+
+	irq_domain_remove(kirq->irqd);
+	return 0;
+}
+
+static const struct of_device_id keystone_irq_dt_ids[] = {
+	{ .compatible = "ti,keystone-irq", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, keystone_irq_dt_ids);
+
+static struct platform_driver keystone_irq_device_driver = {
+	.probe		= keystone_irq_probe,
+	.remove		= keystone_irq_remove,
+	.driver		= {
+		.name	= "keystone_irq",
+		.owner	= THIS_MODULE,
+		.of_match_table	= of_match_ptr(keystone_irq_dt_ids),
+	}
+};
+
+module_platform_driver(keystone_irq_device_driver);
+
+MODULE_AUTHOR("Texas Instruments");
+MODULE_AUTHOR("Sajesh Kumar Saran");
+MODULE_AUTHOR("Grygorii Strashko");
+MODULE_DESCRIPTION("Keystone IRQ chip");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/irqchip/irq-mmp.c b/drivers/irqchip/irq-mmp.c
index 1c3e2c9b46ba..c0da57bdb89d 100644
--- a/drivers/irqchip/irq-mmp.c
+++ b/drivers/irqchip/irq-mmp.c
@@ -196,26 +196,24 @@ static struct mmp_intc_conf mmp2_conf = {
 
 static void __exception_irq_entry mmp_handle_irq(struct pt_regs *regs)
 {
-	int irq, hwirq;
+	int hwirq;
 
 	hwirq = readl_relaxed(mmp_icu_base + PJ1_INT_SEL);
 	if (!(hwirq & SEL_INT_PENDING))
 		return;
 	hwirq &= SEL_INT_NUM_MASK;
-	irq = irq_find_mapping(icu_data[0].domain, hwirq);
-	handle_IRQ(irq, regs);
+	handle_domain_irq(icu_data[0].domain, hwirq, regs);
 }
 
 static void __exception_irq_entry mmp2_handle_irq(struct pt_regs *regs)
 {
-	int irq, hwirq;
+	int hwirq;
 
 	hwirq = readl_relaxed(mmp_icu_base + PJ4_INT_SEL);
 	if (!(hwirq & SEL_INT_PENDING))
 		return;
 	hwirq &= SEL_INT_NUM_MASK;
-	irq = irq_find_mapping(icu_data[0].domain, hwirq);
-	handle_IRQ(irq, regs);
+	handle_domain_irq(icu_data[0].domain, hwirq, regs);
 }
 
 /* MMP (ARMv5) */
diff --git a/drivers/irqchip/irq-mxs.c b/drivers/irqchip/irq-mxs.c
index 4044ff287663..e4acf1e3f8e3 100644
--- a/drivers/irqchip/irq-mxs.c
+++ b/drivers/irqchip/irq-mxs.c
@@ -78,8 +78,7 @@ asmlinkage void __exception_irq_entry icoll_handle_irq(struct pt_regs *regs)
 
 	irqnr = __raw_readl(icoll_base + HW_ICOLL_STAT_OFFSET);
 	__raw_writel(irqnr, icoll_base + HW_ICOLL_VECTOR);
-	irqnr = irq_find_mapping(icoll_domain, irqnr);
-	handle_IRQ(irqnr, regs);
+	handle_domain_irq(icoll_domain, irqnr, regs);
 }
 
 static int icoll_irq_domain_map(struct irq_domain *d, unsigned int virq,
diff --git a/drivers/irqchip/irq-omap-intc.c b/drivers/irqchip/irq-omap-intc.c
new file mode 100644
index 000000000000..28718d3e8281
--- /dev/null
+++ b/drivers/irqchip/irq-omap-intc.c
@@ -0,0 +1,402 @@
+/*
+ * linux/arch/arm/mach-omap2/irq.c
+ *
+ * Interrupt handler for OMAP2 boards.
+ *
+ * Copyright (C) 2005 Nokia Corporation
+ * Author: Paul Mundt <paul.mundt@nokia.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+
+#include <asm/exception.h>
+#include <linux/irqdomain.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+
+#include "irqchip.h"
+
+/* Define these here for now until we drop all board-files */
+#define OMAP24XX_IC_BASE	0x480fe000
+#define OMAP34XX_IC_BASE	0x48200000
+
+/* selected INTC register offsets */
+
+#define INTC_REVISION		0x0000
+#define INTC_SYSCONFIG		0x0010
+#define INTC_SYSSTATUS		0x0014
+#define INTC_SIR		0x0040
+#define INTC_CONTROL		0x0048
+#define INTC_PROTECTION		0x004C
+#define INTC_IDLE		0x0050
+#define INTC_THRESHOLD		0x0068
+#define INTC_MIR0		0x0084
+#define INTC_MIR_CLEAR0		0x0088
+#define INTC_MIR_SET0		0x008c
+#define INTC_PENDING_IRQ0	0x0098
+#define INTC_PENDING_IRQ1	0x00b8
+#define INTC_PENDING_IRQ2	0x00d8
+#define INTC_PENDING_IRQ3	0x00f8
+#define INTC_ILR0		0x0100
+
+#define ACTIVEIRQ_MASK		0x7f	/* omap2/3 active interrupt bits */
+#define INTCPS_NR_ILR_REGS	128
+#define INTCPS_NR_MIR_REGS	4
+
+#define INTC_IDLE_FUNCIDLE	(1 << 0)
+#define INTC_IDLE_TURBO		(1 << 1)
+
+#define INTC_PROTECTION_ENABLE	(1 << 0)
+
+struct omap_intc_regs {
+	u32 sysconfig;
+	u32 protection;
+	u32 idle;
+	u32 threshold;
+	u32 ilr[INTCPS_NR_ILR_REGS];
+	u32 mir[INTCPS_NR_MIR_REGS];
+};
+static struct omap_intc_regs intc_context;
+
+static struct irq_domain *domain;
+static void __iomem *omap_irq_base;
+static int omap_nr_pending = 3;
+static int omap_nr_irqs = 96;
+
+static void intc_writel(u32 reg, u32 val)
+{
+	writel_relaxed(val, omap_irq_base + reg);
+}
+
+static u32 intc_readl(u32 reg)
+{
+	return readl_relaxed(omap_irq_base + reg);
+}
+
+void omap_intc_save_context(void)
+{
+	int i;
+
+	intc_context.sysconfig =
+		intc_readl(INTC_SYSCONFIG);
+	intc_context.protection =
+		intc_readl(INTC_PROTECTION);
+	intc_context.idle =
+		intc_readl(INTC_IDLE);
+	intc_context.threshold =
+		intc_readl(INTC_THRESHOLD);
+
+	for (i = 0; i < omap_nr_irqs; i++)
+		intc_context.ilr[i] =
+			intc_readl((INTC_ILR0 + 0x4 * i));
+	for (i = 0; i < INTCPS_NR_MIR_REGS; i++)
+		intc_context.mir[i] =
+			intc_readl(INTC_MIR0 + (0x20 * i));
+}
+
+void omap_intc_restore_context(void)
+{
+	int i;
+
+	intc_writel(INTC_SYSCONFIG, intc_context.sysconfig);
+	intc_writel(INTC_PROTECTION, intc_context.protection);
+	intc_writel(INTC_IDLE, intc_context.idle);
+	intc_writel(INTC_THRESHOLD, intc_context.threshold);
+
+	for (i = 0; i < omap_nr_irqs; i++)
+		intc_writel(INTC_ILR0 + 0x4 * i,
+				intc_context.ilr[i]);
+
+	for (i = 0; i < INTCPS_NR_MIR_REGS; i++)
+		intc_writel(INTC_MIR0 + 0x20 * i,
+			intc_context.mir[i]);
+	/* MIRs are saved and restore with other PRCM registers */
+}
+
+void omap3_intc_prepare_idle(void)
+{
+	/*
+	 * Disable autoidle as it can stall interrupt controller,
+	 * cf. errata ID i540 for 3430 (all revisions up to 3.1.x)
+	 */
+	intc_writel(INTC_SYSCONFIG, 0);
+	intc_writel(INTC_IDLE, INTC_IDLE_TURBO);
+}
+
+void omap3_intc_resume_idle(void)
+{
+	/* Re-enable autoidle */
+	intc_writel(INTC_SYSCONFIG, 1);
+	intc_writel(INTC_IDLE, 0);
+}
+
+/* XXX: FIQ and additional INTC support (only MPU at the moment) */
+static void omap_ack_irq(struct irq_data *d)
+{
+	intc_writel(INTC_CONTROL, 0x1);
+}
+
+static void omap_mask_ack_irq(struct irq_data *d)
+{
+	irq_gc_mask_disable_reg(d);
+	omap_ack_irq(d);
+}
+
+static void __init omap_irq_soft_reset(void)
+{
+	unsigned long tmp;
+
+	tmp = intc_readl(INTC_REVISION) & 0xff;
+
+	pr_info("IRQ: Found an INTC at 0x%p (revision %ld.%ld) with %d interrupts\n",
+		omap_irq_base, tmp >> 4, tmp & 0xf, omap_nr_irqs);
+
+	tmp = intc_readl(INTC_SYSCONFIG);
+	tmp |= 1 << 1;	/* soft reset */
+	intc_writel(INTC_SYSCONFIG, tmp);
+
+	while (!(intc_readl(INTC_SYSSTATUS) & 0x1))
+		/* Wait for reset to complete */;
+
+	/* Enable autoidle */
+	intc_writel(INTC_SYSCONFIG, 1 << 0);
+}
+
+int omap_irq_pending(void)
+{
+	int i;
+
+	for (i = 0; i < omap_nr_pending; i++)
+		if (intc_readl(INTC_PENDING_IRQ0 + (0x20 * i)))
+			return 1;
+	return 0;
+}
+
+void omap3_intc_suspend(void)
+{
+	/* A pending interrupt would prevent OMAP from entering suspend */
+	omap_ack_irq(NULL);
+}
+
+static int __init omap_alloc_gc_of(struct irq_domain *d, void __iomem *base)
+{
+	int ret;
+	int i;
+
+	ret = irq_alloc_domain_generic_chips(d, 32, 1, "INTC",
+			handle_level_irq, IRQ_NOREQUEST | IRQ_NOPROBE,
+			IRQ_LEVEL, 0);
+	if (ret) {
+		pr_warn("Failed to allocate irq chips\n");
+		return ret;
+	}
+
+	for (i = 0; i < omap_nr_pending; i++) {
+		struct irq_chip_generic *gc;
+		struct irq_chip_type *ct;
+
+		gc = irq_get_domain_generic_chip(d, 32 * i);
+		gc->reg_base = base;
+		ct = gc->chip_types;
+
+		ct->type = IRQ_TYPE_LEVEL_MASK;
+		ct->handler = handle_level_irq;
+
+		ct->chip.irq_ack = omap_mask_ack_irq;
+		ct->chip.irq_mask = irq_gc_mask_disable_reg;
+		ct->chip.irq_unmask = irq_gc_unmask_enable_reg;
+
+		ct->chip.flags |= IRQCHIP_SKIP_SET_WAKE;
+
+		ct->regs.enable = INTC_MIR_CLEAR0 + 32 * i;
+		ct->regs.disable = INTC_MIR_SET0 + 32 * i;
+	}
+
+	return 0;
+}
+
+static void __init omap_alloc_gc_legacy(void __iomem *base,
+		unsigned int irq_start, unsigned int num)
+{
+	struct irq_chip_generic *gc;
+	struct irq_chip_type *ct;
+
+	gc = irq_alloc_generic_chip("INTC", 1, irq_start, base,
+			handle_level_irq);
+	ct = gc->chip_types;
+	ct->chip.irq_ack = omap_mask_ack_irq;
+	ct->chip.irq_mask = irq_gc_mask_disable_reg;
+	ct->chip.irq_unmask = irq_gc_unmask_enable_reg;
+	ct->chip.flags |= IRQCHIP_SKIP_SET_WAKE;
+
+	ct->regs.enable = INTC_MIR_CLEAR0;
+	ct->regs.disable = INTC_MIR_SET0;
+	irq_setup_generic_chip(gc, IRQ_MSK(num), IRQ_GC_INIT_MASK_CACHE,
+			IRQ_NOREQUEST | IRQ_NOPROBE, 0);
+}
+
+static int __init omap_init_irq_of(struct device_node *node)
+{
+	int ret;
+
+	omap_irq_base = of_iomap(node, 0);
+	if (WARN_ON(!omap_irq_base))
+		return -ENOMEM;
+
+	domain = irq_domain_add_linear(node, omap_nr_irqs,
+			&irq_generic_chip_ops, NULL);
+
+	omap_irq_soft_reset();
+
+	ret = omap_alloc_gc_of(domain, omap_irq_base);
+	if (ret < 0)
+		irq_domain_remove(domain);
+
+	return ret;
+}
+
+static int __init omap_init_irq_legacy(u32 base)
+{
+	int j, irq_base;
+
+	omap_irq_base = ioremap(base, SZ_4K);
+	if (WARN_ON(!omap_irq_base))
+		return -ENOMEM;
+
+	irq_base = irq_alloc_descs(-1, 0, omap_nr_irqs, 0);
+	if (irq_base < 0) {
+		pr_warn("Couldn't allocate IRQ numbers\n");
+		irq_base = 0;
+	}
+
+	domain = irq_domain_add_legacy(NULL, omap_nr_irqs, irq_base, 0,
+			&irq_domain_simple_ops, NULL);
+
+	omap_irq_soft_reset();
+
+	for (j = 0; j < omap_nr_irqs; j += 32)
+		omap_alloc_gc_legacy(omap_irq_base + j, j + irq_base, 32);
+
+	return 0;
+}
+
+static void __init omap_irq_enable_protection(void)
+{
+	u32 reg;
+
+	reg = intc_readl(INTC_PROTECTION);
+	reg |= INTC_PROTECTION_ENABLE;
+	intc_writel(INTC_PROTECTION, reg);
+}
+
+static int __init omap_init_irq(u32 base, struct device_node *node)
+{
+	int ret;
+
+	if (node)
+		ret = omap_init_irq_of(node);
+	else
+		ret = omap_init_irq_legacy(base);
+
+	if (ret == 0)
+		omap_irq_enable_protection();
+
+	return ret;
+}
+
+static asmlinkage void __exception_irq_entry
+omap_intc_handle_irq(struct pt_regs *regs)
+{
+	u32 irqnr = 0;
+	int handled_irq = 0;
+	int i;
+
+	do {
+		for (i = 0; i < omap_nr_pending; i++) {
+			irqnr = intc_readl(INTC_PENDING_IRQ0 + (0x20 * i));
+			if (irqnr)
+				goto out;
+		}
+
+out:
+		if (!irqnr)
+			break;
+
+		irqnr = intc_readl(INTC_SIR);
+		irqnr &= ACTIVEIRQ_MASK;
+
+		if (irqnr) {
+			handle_domain_irq(domain, irqnr, regs);
+			handled_irq = 1;
+		}
+	} while (irqnr);
+
+	/*
+	 * If an irq is masked or deasserted while active, we will
+	 * keep ending up here with no irq handled. So remove it from
+	 * the INTC with an ack.
+	 */
+	if (!handled_irq)
+		omap_ack_irq(NULL);
+}
+
+void __init omap2_init_irq(void)
+{
+	omap_nr_irqs = 96;
+	omap_nr_pending = 3;
+	omap_init_irq(OMAP24XX_IC_BASE, NULL);
+	set_handle_irq(omap_intc_handle_irq);
+}
+
+void __init omap3_init_irq(void)
+{
+	omap_nr_irqs = 96;
+	omap_nr_pending = 3;
+	omap_init_irq(OMAP34XX_IC_BASE, NULL);
+	set_handle_irq(omap_intc_handle_irq);
+}
+
+void __init ti81xx_init_irq(void)
+{
+	omap_nr_irqs = 96;
+	omap_nr_pending = 4;
+	omap_init_irq(OMAP34XX_IC_BASE, NULL);
+	set_handle_irq(omap_intc_handle_irq);
+}
+
+static int __init intc_of_init(struct device_node *node,
+			     struct device_node *parent)
+{
+	int ret;
+
+	omap_nr_pending = 3;
+	omap_nr_irqs = 96;
+
+	if (WARN_ON(!node))
+		return -ENODEV;
+
+	if (of_device_is_compatible(node, "ti,am33xx-intc")) {
+		omap_nr_irqs = 128;
+		omap_nr_pending = 4;
+	}
+
+	ret = omap_init_irq(-1, of_node_get(node));
+	if (ret < 0)
+		return ret;
+
+	set_handle_irq(omap_intc_handle_irq);
+
+	return 0;
+}
+
+IRQCHIP_DECLARE(omap2_intc, "ti,omap2-intc", intc_of_init);
+IRQCHIP_DECLARE(omap3_intc, "ti,omap3-intc", intc_of_init);
+IRQCHIP_DECLARE(am33xx_intc, "ti,am33xx-intc", intc_of_init);
diff --git a/drivers/irqchip/irq-or1k-pic.c b/drivers/irqchip/irq-or1k-pic.c
index 17ff033d9925..e93d079fe069 100644
--- a/drivers/irqchip/irq-or1k-pic.c
+++ b/drivers/irqchip/irq-or1k-pic.c
@@ -113,7 +113,7 @@ static inline int pic_get_irq(int first)
 	else
 		hwirq = hwirq + first - 1;
 
-	return irq_find_mapping(root_domain, hwirq);
+	return hwirq;
 }
 
 static void or1k_pic_handle_irq(struct pt_regs *regs)
@@ -121,7 +121,7 @@ static void or1k_pic_handle_irq(struct pt_regs *regs)
 	int irq = -1;
 
 	while ((irq = pic_get_irq(irq + 1)) != NO_IRQ)
-		handle_IRQ(irq, regs);
+		handle_domain_irq(root_domain, irq, regs);
 }
 
 static int or1k_map(struct irq_domain *d, unsigned int irq, irq_hw_number_t hw)
diff --git a/drivers/irqchip/irq-orion.c b/drivers/irqchip/irq-orion.c
index 34d18b48bb78..ad0c0f6f1d65 100644
--- a/drivers/irqchip/irq-orion.c
+++ b/drivers/irqchip/irq-orion.c
@@ -43,9 +43,8 @@ __exception_irq_entry orion_handle_irq(struct pt_regs *regs)
 			gc->mask_cache;
 		while (stat) {
 			u32 hwirq = __fls(stat);
-			u32 irq = irq_find_mapping(orion_irq_domain,
-						   gc->irq_base + hwirq);
-			handle_IRQ(irq, regs);
+			handle_domain_irq(orion_irq_domain,
+					  gc->irq_base + hwirq, regs);
 			stat &= ~(1 << hwirq);
 		}
 	}
diff --git a/drivers/irqchip/irq-renesas-intc-irqpin.c b/drivers/irqchip/irq-renesas-intc-irqpin.c
index 3ee78f02e5d7..542e850f4946 100644
--- a/drivers/irqchip/irq-renesas-intc-irqpin.c
+++ b/drivers/irqchip/irq-renesas-intc-irqpin.c
@@ -17,6 +17,7 @@
  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  */
 
+#include <linux/clk.h>
 #include <linux/init.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
@@ -30,6 +31,7 @@
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/platform_data/irq-renesas-intc-irqpin.h>
+#include <linux/pm_runtime.h>
 
 #define INTC_IRQPIN_MAX 8 /* maximum 8 interrupts per driver instance */
 
@@ -75,6 +77,7 @@ struct intc_irqpin_priv {
 	struct platform_device *pdev;
 	struct irq_chip irq_chip;
 	struct irq_domain *irq_domain;
+	struct clk *clk;
 	bool shared_irqs;
 	u8 shared_irq_mask;
 };
@@ -270,6 +273,21 @@ static int intc_irqpin_irq_set_type(struct irq_data *d, unsigned int type)
 				     value ^ INTC_IRQ_SENSE_VALID);
 }
 
+static int intc_irqpin_irq_set_wake(struct irq_data *d, unsigned int on)
+{
+	struct intc_irqpin_priv *p = irq_data_get_irq_chip_data(d);
+
+	if (!p->clk)
+		return 0;
+
+	if (on)
+		clk_enable(p->clk);
+	else
+		clk_disable(p->clk);
+
+	return 0;
+}
+
 static irqreturn_t intc_irqpin_irq_handler(int irq, void *dev_id)
 {
 	struct intc_irqpin_irq *i = dev_id;
@@ -329,7 +347,8 @@ static struct irq_domain_ops intc_irqpin_irq_domain_ops = {
 
 static int intc_irqpin_probe(struct platform_device *pdev)
 {
-	struct renesas_intc_irqpin_config *pdata = pdev->dev.platform_data;
+	struct device *dev = &pdev->dev;
+	struct renesas_intc_irqpin_config *pdata = dev->platform_data;
 	struct intc_irqpin_priv *p;
 	struct intc_irqpin_iomem *i;
 	struct resource *io[INTC_IRQPIN_REG_NR];
@@ -337,25 +356,24 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	struct irq_chip *irq_chip;
 	void (*enable_fn)(struct irq_data *d);
 	void (*disable_fn)(struct irq_data *d);
-	const char *name = dev_name(&pdev->dev);
+	const char *name = dev_name(dev);
 	int ref_irq;
 	int ret;
 	int k;
 
-	p = devm_kzalloc(&pdev->dev, sizeof(*p), GFP_KERNEL);
+	p = devm_kzalloc(dev, sizeof(*p), GFP_KERNEL);
 	if (!p) {
-		dev_err(&pdev->dev, "failed to allocate driver data\n");
-		ret = -ENOMEM;
-		goto err0;
+		dev_err(dev, "failed to allocate driver data\n");
+		return -ENOMEM;
 	}
 
 	/* deal with driver instance configuration */
 	if (pdata) {
 		memcpy(&p->config, pdata, sizeof(*pdata));
 	} else {
-		of_property_read_u32(pdev->dev.of_node, "sense-bitfield-width",
+		of_property_read_u32(dev->of_node, "sense-bitfield-width",
 				     &p->config.sense_bitfield_width);
-		p->config.control_parent = of_property_read_bool(pdev->dev.of_node,
+		p->config.control_parent = of_property_read_bool(dev->of_node,
 								 "control-parent");
 	}
 	if (!p->config.sense_bitfield_width)
@@ -364,11 +382,20 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	p->pdev = pdev;
 	platform_set_drvdata(pdev, p);
 
+	p->clk = devm_clk_get(dev, NULL);
+	if (IS_ERR(p->clk)) {
+		dev_warn(dev, "unable to get clock\n");
+		p->clk = NULL;
+	}
+
+	pm_runtime_enable(dev);
+	pm_runtime_get_sync(dev);
+
 	/* get hold of manadatory IOMEM */
 	for (k = 0; k < INTC_IRQPIN_REG_NR; k++) {
 		io[k] = platform_get_resource(pdev, IORESOURCE_MEM, k);
 		if (!io[k]) {
-			dev_err(&pdev->dev, "not enough IOMEM resources\n");
+			dev_err(dev, "not enough IOMEM resources\n");
 			ret = -EINVAL;
 			goto err0;
 		}
@@ -386,7 +413,7 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 
 	p->number_of_irqs = k;
 	if (p->number_of_irqs < 1) {
-		dev_err(&pdev->dev, "not enough IRQ resources\n");
+		dev_err(dev, "not enough IRQ resources\n");
 		ret = -EINVAL;
 		goto err0;
 	}
@@ -407,15 +434,15 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 			i->write = intc_irqpin_write32;
 			break;
 		default:
-			dev_err(&pdev->dev, "IOMEM size mismatch\n");
+			dev_err(dev, "IOMEM size mismatch\n");
 			ret = -EINVAL;
 			goto err0;
 		}
 
-		i->iomem = devm_ioremap_nocache(&pdev->dev, io[k]->start,
+		i->iomem = devm_ioremap_nocache(dev, io[k]->start,
 						resource_size(io[k]));
 		if (!i->iomem) {
-			dev_err(&pdev->dev, "failed to remap IOMEM\n");
+			dev_err(dev, "failed to remap IOMEM\n");
 			ret = -ENXIO;
 			goto err0;
 		}
@@ -454,39 +481,36 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	irq_chip->name = name;
 	irq_chip->irq_mask = disable_fn;
 	irq_chip->irq_unmask = enable_fn;
-	irq_chip->irq_enable = enable_fn;
-	irq_chip->irq_disable = disable_fn;
 	irq_chip->irq_set_type = intc_irqpin_irq_set_type;
-	irq_chip->flags	= IRQCHIP_SKIP_SET_WAKE;
+	irq_chip->irq_set_wake = intc_irqpin_irq_set_wake;
+	irq_chip->flags	= IRQCHIP_MASK_ON_SUSPEND;
 
-	p->irq_domain = irq_domain_add_simple(pdev->dev.of_node,
+	p->irq_domain = irq_domain_add_simple(dev->of_node,
 					      p->number_of_irqs,
 					      p->config.irq_base,
 					      &intc_irqpin_irq_domain_ops, p);
 	if (!p->irq_domain) {
 		ret = -ENXIO;
-		dev_err(&pdev->dev, "cannot initialize irq domain\n");
+		dev_err(dev, "cannot initialize irq domain\n");
 		goto err0;
 	}
 
 	if (p->shared_irqs) {
 		/* request one shared interrupt */
-		if (devm_request_irq(&pdev->dev, p->irq[0].requested_irq,
+		if (devm_request_irq(dev, p->irq[0].requested_irq,
 				intc_irqpin_shared_irq_handler,
 				IRQF_SHARED, name, p)) {
-			dev_err(&pdev->dev, "failed to request low IRQ\n");
+			dev_err(dev, "failed to request low IRQ\n");
 			ret = -ENOENT;
 			goto err1;
 		}
 	} else {
 		/* request interrupts one by one */
 		for (k = 0; k < p->number_of_irqs; k++) {
-			if (devm_request_irq(&pdev->dev,
-					p->irq[k].requested_irq,
-					intc_irqpin_irq_handler,
-					0, name, &p->irq[k])) {
-				dev_err(&pdev->dev,
-					"failed to request low IRQ\n");
+			if (devm_request_irq(dev, p->irq[k].requested_irq,
+					     intc_irqpin_irq_handler, 0, name,
+					     &p->irq[k])) {
+				dev_err(dev, "failed to request low IRQ\n");
 				ret = -ENOENT;
 				goto err1;
 			}
@@ -497,12 +521,12 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 	for (k = 0; k < p->number_of_irqs; k++)
 		intc_irqpin_mask_unmask_prio(p, k, 0);
 
-	dev_info(&pdev->dev, "driving %d irqs\n", p->number_of_irqs);
+	dev_info(dev, "driving %d irqs\n", p->number_of_irqs);
 
 	/* warn in case of mismatch if irq base is specified */
 	if (p->config.irq_base) {
 		if (p->config.irq_base != p->irq[0].domain_irq)
-			dev_warn(&pdev->dev, "irq base mismatch (%d/%d)\n",
+			dev_warn(dev, "irq base mismatch (%d/%d)\n",
 				 p->config.irq_base, p->irq[0].domain_irq);
 	}
 
@@ -511,6 +535,8 @@ static int intc_irqpin_probe(struct platform_device *pdev)
 err1:
 	irq_domain_remove(p->irq_domain);
 err0:
+	pm_runtime_put(dev);
+	pm_runtime_disable(dev);
 	return ret;
 }
 
@@ -519,7 +545,8 @@ static int intc_irqpin_remove(struct platform_device *pdev)
 	struct intc_irqpin_priv *p = platform_get_drvdata(pdev);
 
 	irq_domain_remove(p->irq_domain);
-
+	pm_runtime_put(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
 	return 0;
 }
 
diff --git a/drivers/irqchip/irq-s3c24xx.c b/drivers/irqchip/irq-s3c24xx.c
index 78a6accd205f..c8d373fcd823 100644
--- a/drivers/irqchip/irq-s3c24xx.c
+++ b/drivers/irqchip/irq-s3c24xx.c
@@ -339,7 +339,6 @@ static inline int s3c24xx_handle_intc(struct s3c_irq_intc *intc,
 {
 	int pnd;
 	int offset;
-	int irq;
 
 	pnd = __raw_readl(intc->reg_intpnd);
 	if (!pnd)
@@ -365,8 +364,7 @@ static inline int s3c24xx_handle_intc(struct s3c_irq_intc *intc,
 	if (!(pnd & (1 << offset)))
 		offset =  __ffs(pnd);
 
-	irq = irq_find_mapping(intc->domain, intc_offset + offset);
-	handle_IRQ(irq, regs);
+	handle_domain_irq(intc->domain, intc_offset + offset, regs);
 	return true;
 }
 
diff --git a/drivers/irqchip/irq-sirfsoc.c b/drivers/irqchip/irq-sirfsoc.c
index 5e54f6d71e77..a469355df352 100644
--- a/drivers/irqchip/irq-sirfsoc.c
+++ b/drivers/irqchip/irq-sirfsoc.c
@@ -50,12 +50,10 @@ sirfsoc_alloc_gc(void __iomem *base, unsigned int irq_start, unsigned int num)
 static void __exception_irq_entry sirfsoc_handle_irq(struct pt_regs *regs)
 {
 	void __iomem *base = sirfsoc_irqdomain->host_data;
-	u32 irqstat, irqnr;
+	u32 irqstat;
 
 	irqstat = readl_relaxed(base + SIRFSOC_INIT_IRQ_ID);
-	irqnr = irq_find_mapping(sirfsoc_irqdomain, irqstat & 0xff);
-
-	handle_IRQ(irqnr, regs);
+	handle_domain_irq(sirfsoc_irqdomain, irqstat & 0xff, regs);
 }
 
 static int __init sirfsoc_irq_init(struct device_node *np,
diff --git a/drivers/irqchip/irq-sun4i.c b/drivers/irqchip/irq-sun4i.c
index 6fcef4a95a18..64155b686081 100644
--- a/drivers/irqchip/irq-sun4i.c
+++ b/drivers/irqchip/irq-sun4i.c
@@ -136,7 +136,7 @@ IRQCHIP_DECLARE(allwinner_sun4i_ic, "allwinner,sun4i-a10-ic", sun4i_of_init);
 
 static void __exception_irq_entry sun4i_handle_irq(struct pt_regs *regs)
 {
-	u32 irq, hwirq;
+	u32 hwirq;
 
 	/*
 	 * hwirq == 0 can mean one of 3 things:
@@ -154,8 +154,7 @@ static void __exception_irq_entry sun4i_handle_irq(struct pt_regs *regs)
 		return;
 
 	do {
-		irq = irq_find_mapping(sun4i_irq_domain, hwirq);
-		handle_IRQ(irq, regs);
+		handle_domain_irq(sun4i_irq_domain, hwirq, regs);
 		hwirq = readl(sun4i_irq_base + SUN4I_IRQ_VECTOR_REG) >> 2;
 	} while (hwirq != 0);
 }
diff --git a/drivers/irqchip/irq-versatile-fpga.c b/drivers/irqchip/irq-versatile-fpga.c
index ccf58548b161..1ab451729a5c 100644
--- a/drivers/irqchip/irq-versatile-fpga.c
+++ b/drivers/irqchip/irq-versatile-fpga.c
@@ -96,7 +96,7 @@ static int handle_one_fpga(struct fpga_irq_data *f, struct pt_regs *regs)
 
 	while ((status  = readl(f->base + IRQ_STATUS))) {
 		irq = ffs(status) - 1;
-		handle_IRQ(irq_find_mapping(f->domain, irq), regs);
+		handle_domain_irq(f->domain, irq, regs);
 		handled = 1;
 	}
 
diff --git a/drivers/irqchip/irq-vic.c b/drivers/irqchip/irq-vic.c
index 7d35287f9e90..54089debf2dc 100644
--- a/drivers/irqchip/irq-vic.c
+++ b/drivers/irqchip/irq-vic.c
@@ -219,7 +219,7 @@ static int handle_one_vic(struct vic_device *vic, struct pt_regs *regs)
 
 	while ((stat = readl_relaxed(vic->base + VIC_IRQ_STATUS))) {
 		irq = ffs(stat) - 1;
-		handle_IRQ(irq_find_mapping(vic->domain, irq), regs);
+		handle_domain_irq(vic->domain, irq, regs);
 		handled = 1;
 	}
 
diff --git a/drivers/irqchip/irq-vt8500.c b/drivers/irqchip/irq-vt8500.c
index eb6e91efdec8..b7af816f2769 100644
--- a/drivers/irqchip/irq-vt8500.c
+++ b/drivers/irqchip/irq-vt8500.c
@@ -181,7 +181,7 @@ static struct irq_domain_ops vt8500_irq_domain_ops = {
 static void __exception_irq_entry vt8500_handle_irq(struct pt_regs *regs)
 {
 	u32 stat, i;
-	int irqnr, virq;
+	int irqnr;
 	void __iomem *base;
 
 	/* Loop through each active controller */
@@ -198,8 +198,7 @@ static void __exception_irq_entry vt8500_handle_irq(struct pt_regs *regs)
 				continue;
 		}
 
-		virq = irq_find_mapping(intc[i].domain, irqnr);
-		handle_IRQ(virq, regs);
+		handle_domain_irq(intc[i].domain, irqnr, regs);
 	}
 }
 
diff --git a/drivers/irqchip/irq-zevio.c b/drivers/irqchip/irq-zevio.c
index ceb3a4318f73..e4ef74ed454a 100644
--- a/drivers/irqchip/irq-zevio.c
+++ b/drivers/irqchip/irq-zevio.c
@@ -56,8 +56,7 @@ static void __exception_irq_entry zevio_handle_irq(struct pt_regs *regs)
 
 	while (readl(zevio_irq_io + IO_STATUS)) {
 		irqnr = readl(zevio_irq_io + IO_CURRENT);
-		irqnr = irq_find_mapping(zevio_irq_domain, irqnr);
-		handle_IRQ(irqnr, regs);
+		handle_domain_irq(zevio_irq_domain, irqnr, regs);
 	};
 }
 
diff --git a/drivers/isdn/capi/capidrv.c b/drivers/isdn/capi/capidrv.c
index fd6d28f3fc36..1cc6ca8bfbda 100644
--- a/drivers/isdn/capi/capidrv.c
+++ b/drivers/isdn/capi/capidrv.c
@@ -506,7 +506,10 @@ static void send_message(capidrv_contr *card, _cmsg *cmsg)
 	struct sk_buff *skb;
 	size_t len;
 
-	capi_cmsg2message(cmsg, cmsg->buf);
+	if (capi_cmsg2message(cmsg, cmsg->buf)) {
+		printk(KERN_ERR "capidrv::send_message: parser failure\n");
+		return;
+	}
 	len = CAPIMSG_LEN(cmsg->buf);
 	skb = alloc_skb(len, GFP_ATOMIC);
 	if (!skb) {
@@ -1578,7 +1581,12 @@ static _cmsg s_cmsg;
 
 static void capidrv_recv_message(struct capi20_appl *ap, struct sk_buff *skb)
 {
-	capi_message2cmsg(&s_cmsg, skb->data);
+	if (capi_message2cmsg(&s_cmsg, skb->data)) {
+		printk(KERN_ERR "capidrv: applid=%d: received invalid message\n",
+		       ap->applid);
+		kfree_skb(skb);
+		return;
+	}
 	if (debugmode > 3) {
 		_cdebbuf *cdb = capi_cmsg2str(&s_cmsg);
 
@@ -1903,7 +1911,11 @@ static int capidrv_command(isdn_ctrl *c, capidrv_contr *card)
 				       NULL,	/* Useruserdata */
 				       NULL	/* Facilitydataarray */
 			);
-		capi_cmsg2message(&cmdcmsg, cmdcmsg.buf);
+		if (capi_cmsg2message(&cmdcmsg, cmdcmsg.buf)) {
+			printk(KERN_ERR "capidrv-%d: capidrv_command: parser failure\n",
+			       card->contrnr);
+			return -EINVAL;
+		}
 		plci_change_state(card, bchan->plcip, EV_PLCI_CONNECT_RESP);
 		send_message(card, &cmdcmsg);
 		return 0;
@@ -2090,7 +2102,11 @@ static int if_sendbuf(int id, int channel, int doack, struct sk_buff *skb)
 	if (capidrv_add_ack(nccip, datahandle, doack ? (int)skb->len : -1) < 0)
 		return 0;
 
-	capi_cmsg2message(&sendcmsg, sendcmsg.buf);
+	if (capi_cmsg2message(&sendcmsg, sendcmsg.buf)) {
+		printk(KERN_ERR "capidrv-%d: if_sendbuf: parser failure\n",
+		       card->contrnr);
+		return -EINVAL;
+	}
 	msglen = CAPIMSG_LEN(sendcmsg.buf);
 	if (skb_headroom(skb) < msglen) {
 		struct sk_buff *nskb = skb_realloc_headroom(skb, msglen);
diff --git a/drivers/isdn/capi/capiutil.c b/drivers/isdn/capi/capiutil.c
index 6e797e502cfa..36c1b37cea0a 100644
--- a/drivers/isdn/capi/capiutil.c
+++ b/drivers/isdn/capi/capiutil.c
@@ -205,14 +205,26 @@ static unsigned command_2_index(unsigned c, unsigned sc)
 {
 	if (c & 0x80)
 		c = 0x9 + (c & 0x0f);
-	else if (c <= 0x0f);
 	else if (c == 0x41)
 		c = 0x9 + 0x1;
-	else if (c == 0xff)
+	if (c > 0x18)
 		c = 0x00;
 	return (sc & 3) * (0x9 + 0x9) + c;
 }
 
+/**
+ * capi_cmd2par() - find parameter string for CAPI 2.0 command/subcommand
+ * @cmd:	command number
+ * @subcmd:	subcommand number
+ *
+ * Return value: static string, NULL if command/subcommand unknown
+ */
+
+static unsigned char *capi_cmd2par(u8 cmd, u8 subcmd)
+{
+	return cpars[command_2_index(cmd, subcmd)];
+}
+
 /*-------------------------------------------------------*/
 #define TYP (cdef[cmsg->par[cmsg->p]].typ)
 #define OFF (((u8 *)cmsg) + cdef[cmsg->par[cmsg->p]].off)
@@ -305,7 +317,9 @@ unsigned capi_cmsg2message(_cmsg *cmsg, u8 *msg)
 	cmsg->m = msg;
 	cmsg->l = 8;
 	cmsg->p = 0;
-	cmsg->par = cpars[command_2_index(cmsg->Command, cmsg->Subcommand)];
+	cmsg->par = capi_cmd2par(cmsg->Command, cmsg->Subcommand);
+	if (!cmsg->par)
+		return 1;	/* invalid command/subcommand */
 
 	pars_2_message(cmsg);
 
@@ -378,7 +392,9 @@ unsigned capi_message2cmsg(_cmsg *cmsg, u8 *msg)
 	cmsg->p = 0;
 	byteTRcpy(cmsg->m + 4, &cmsg->Command);
 	byteTRcpy(cmsg->m + 5, &cmsg->Subcommand);
-	cmsg->par = cpars[command_2_index(cmsg->Command, cmsg->Subcommand)];
+	cmsg->par = capi_cmd2par(cmsg->Command, cmsg->Subcommand);
+	if (!cmsg->par)
+		return 1;	/* invalid command/subcommand */
 
 	message_2_pars(cmsg);
 
@@ -473,12 +489,17 @@ static char *mnames[] =
  * @cmd:	command number
  * @subcmd:	subcommand number
  *
- * Return value: static string, NULL if command/subcommand unknown
+ * Return value: static string
  */
 
 char *capi_cmd2str(u8 cmd, u8 subcmd)
 {
-	return mnames[command_2_index(cmd, subcmd)];
+	char *result;
+
+	result = mnames[command_2_index(cmd, subcmd)];
+	if (result == NULL)
+		result = "INVALID_COMMAND";
+	return result;
 }
 
 
@@ -628,6 +649,9 @@ static _cdebbuf *printstruct(_cdebbuf *cdb, u8 *m)
 
 static _cdebbuf *protocol_message_2_pars(_cdebbuf *cdb, _cmsg *cmsg, int level)
 {
+	if (!cmsg->par)
+		return NULL;	/* invalid command/subcommand */
+
 	for (; TYP != _CEND; cmsg->p++) {
 		int slen = 29 + 3 - level;
 		int i;
@@ -762,10 +786,10 @@ _cdebbuf *capi_message2str(u8 *msg)
 	cmsg->p = 0;
 	byteTRcpy(cmsg->m + 4, &cmsg->Command);
 	byteTRcpy(cmsg->m + 5, &cmsg->Subcommand);
-	cmsg->par = cpars[command_2_index(cmsg->Command, cmsg->Subcommand)];
+	cmsg->par = capi_cmd2par(cmsg->Command, cmsg->Subcommand);
 
 	cdb = bufprint(cdb, "%-26s ID=%03d #0x%04x LEN=%04d\n",
-		       mnames[command_2_index(cmsg->Command, cmsg->Subcommand)],
+		       capi_cmd2str(cmsg->Command, cmsg->Subcommand),
 		       ((unsigned short *) msg)[1],
 		       ((unsigned short *) msg)[3],
 		       ((unsigned short *) msg)[0]);
@@ -799,7 +823,7 @@ _cdebbuf *capi_cmsg2str(_cmsg *cmsg)
 	cmsg->l = 8;
 	cmsg->p = 0;
 	cdb = bufprint(cdb, "%s ID=%03d #0x%04x LEN=%04d\n",
-		       mnames[command_2_index(cmsg->Command, cmsg->Subcommand)],
+		       capi_cmd2str(cmsg->Command, cmsg->Subcommand),
 		       ((u16 *) cmsg->m)[1],
 		       ((u16 *) cmsg->m)[3],
 		       ((u16 *) cmsg->m)[0]);
diff --git a/drivers/isdn/capi/kcapi.c b/drivers/isdn/capi/kcapi.c
index c123709acf82..823f6985b260 100644
--- a/drivers/isdn/capi/kcapi.c
+++ b/drivers/isdn/capi/kcapi.c
@@ -1184,7 +1184,7 @@ static int old_capi_manufacturer(unsigned int cmd, void __user *data)
  * Return value: CAPI result code
  */
 
-int capi20_manufacturer(unsigned int cmd, void __user *data)
+int capi20_manufacturer(unsigned long cmd, void __user *data)
 {
 	struct capi_ctr *ctr;
 	int retval;
@@ -1259,7 +1259,7 @@ int capi20_manufacturer(unsigned int cmd, void __user *data)
 	}
 
 	default:
-		printk(KERN_ERR "kcapi: manufacturer command %d unknown.\n",
+		printk(KERN_ERR "kcapi: manufacturer command %lu unknown.\n",
 		       cmd);
 		break;
 
diff --git a/drivers/isdn/gigaset/bas-gigaset.c b/drivers/isdn/gigaset/bas-gigaset.c
index b7ae0a0dd5b6..aecec6d32463 100644
--- a/drivers/isdn/gigaset/bas-gigaset.c
+++ b/drivers/isdn/gigaset/bas-gigaset.c
@@ -2365,7 +2365,7 @@ static int gigaset_probe(struct usb_interface *interface,
 	endpoint = &hostif->endpoint[0].desc;
 	usb_fill_int_urb(ucs->urb_int_in, udev,
 			 usb_rcvintpipe(udev,
-					(endpoint->bEndpointAddress) & 0x0f),
+					usb_endpoint_num(endpoint)),
 			 ucs->int_in_buf, IP_MSGSIZE, read_int_callback, cs,
 			 endpoint->bInterval);
 	rc = usb_submit_urb(ucs->urb_int_in, GFP_KERNEL);
diff --git a/drivers/isdn/gigaset/capi.c b/drivers/isdn/gigaset/capi.c
index 3286903a95d2..ccec7778cad2 100644
--- a/drivers/isdn/gigaset/capi.c
+++ b/drivers/isdn/gigaset/capi.c
@@ -250,6 +250,8 @@ static inline void dump_rawmsg(enum debuglevel level, const char *tag,
 	l -= 12;
 	if (l <= 0)
 		return;
+	if (l > 64)
+		l = 64; /* arbitrary limit */
 	dbgline = kmalloc(3 * l, GFP_ATOMIC);
 	if (!dbgline)
 		return;
@@ -645,7 +647,13 @@ int gigaset_isdn_icall(struct at_state_t *at_state)
 					__func__);
 				break;
 			}
-			capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize));
+			if (capi_cmsg2message(&iif->hcmsg,
+					      __skb_put(skb, msgsize))) {
+				dev_err(cs->dev, "%s: message parser failure\n",
+					__func__);
+				dev_kfree_skb_any(skb);
+				break;
+			}
 			dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
 
 			/* add to listeners on this B channel, update state */
@@ -691,7 +699,12 @@ static void send_disconnect_ind(struct bc_state *bcs,
 		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		return;
 	}
-	capi_cmsg2message(&iif->hcmsg, __skb_put(skb, CAPI_DISCONNECT_IND_LEN));
+	if (capi_cmsg2message(&iif->hcmsg,
+			      __skb_put(skb, CAPI_DISCONNECT_IND_LEN))) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
 	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
 }
@@ -721,8 +734,12 @@ static void send_disconnect_b3_ind(struct bc_state *bcs,
 		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		return;
 	}
-	capi_cmsg2message(&iif->hcmsg,
-			  __skb_put(skb, CAPI_DISCONNECT_B3_IND_BASELEN));
+	if (capi_cmsg2message(&iif->hcmsg,
+			  __skb_put(skb, CAPI_DISCONNECT_B3_IND_BASELEN))) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
 	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
 }
@@ -787,7 +804,11 @@ void gigaset_isdn_connD(struct bc_state *bcs)
 		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		return;
 	}
-	capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize));
+	if (capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize))) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
 	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
 }
@@ -887,7 +908,11 @@ void gigaset_isdn_connB(struct bc_state *bcs)
 		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		return;
 	}
-	capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize));
+	if (capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize))) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
 	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
 }
@@ -1094,13 +1119,19 @@ static void send_conf(struct gigaset_capi_ctr *iif,
 		      struct sk_buff *skb,
 		      u16 info)
 {
+	struct cardstate *cs = iif->ctr.driverdata;
+
 	/*
 	 * _CONF replies always only have NCCI and Info parameters
 	 * so they'll fit into the _REQ message skb
 	 */
 	capi_cmsg_answer(&iif->acmsg);
 	iif->acmsg.Info = info;
-	capi_cmsg2message(&iif->acmsg, skb->data);
+	if (capi_cmsg2message(&iif->acmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	__skb_trim(skb, CAPI_STDCONF_LEN);
 	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
@@ -1122,7 +1153,11 @@ static void do_facility_req(struct gigaset_capi_ctr *iif,
 	static u8 confparam[10];	/* max. 9 octets + length byte */
 
 	/* decode message */
-	capi_message2cmsg(cmsg, skb->data);
+	if (capi_message2cmsg(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 
 	/*
@@ -1180,6 +1215,7 @@ static void do_facility_req(struct gigaset_capi_ctr *iif,
 				confparam[3] = 2;	/* length */
 				capimsg_setu16(confparam, 4,
 					       CapiSupplementaryServiceNotSupported);
+				break;
 			}
 			info = CapiSuccess;
 			confparam[3] = 2;	/* length */
@@ -1220,6 +1256,7 @@ static void do_facility_req(struct gigaset_capi_ctr *iif,
 	}
 
 	/* send FACILITY_CONF with given Info and confirmation parameter */
+	dev_kfree_skb_any(skb);
 	capi_cmsg_answer(cmsg);
 	cmsg->Info = info;
 	cmsg->FacilityConfirmationParameter = confparam;
@@ -1229,7 +1266,11 @@ static void do_facility_req(struct gigaset_capi_ctr *iif,
 		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		return;
 	}
-	capi_cmsg2message(cmsg, __skb_put(cskb, msgsize));
+	if (capi_cmsg2message(cmsg, __skb_put(cskb, msgsize))) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(cskb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 	capi_ctr_handle_message(&iif->ctr, ap->id, cskb);
 }
@@ -1243,8 +1284,14 @@ static void do_listen_req(struct gigaset_capi_ctr *iif,
 			  struct gigaset_capi_appl *ap,
 			  struct sk_buff *skb)
 {
+	struct cardstate *cs = iif->ctr.driverdata;
+
 	/* decode message */
-	capi_message2cmsg(&iif->acmsg, skb->data);
+	if (capi_message2cmsg(&iif->acmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 
 	/* store listening parameters */
@@ -1261,8 +1308,14 @@ static void do_alert_req(struct gigaset_capi_ctr *iif,
 			 struct gigaset_capi_appl *ap,
 			 struct sk_buff *skb)
 {
+	struct cardstate *cs = iif->ctr.driverdata;
+
 	/* decode message */
-	capi_message2cmsg(&iif->acmsg, skb->data);
+	if (capi_message2cmsg(&iif->acmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 	send_conf(iif, ap, skb, CapiAlertAlreadySent);
 }
@@ -1287,7 +1340,11 @@ static void do_connect_req(struct gigaset_capi_ctr *iif,
 	u16 info;
 
 	/* decode message */
-	capi_message2cmsg(cmsg, skb->data);
+	if (capi_message2cmsg(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 
 	/* get free B channel & construct PLCI */
@@ -1574,7 +1631,11 @@ static void do_connect_resp(struct gigaset_capi_ctr *iif,
 	int channel;
 
 	/* decode message */
-	capi_message2cmsg(cmsg, skb->data);
+	if (capi_message2cmsg(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 	dev_kfree_skb_any(skb);
 
@@ -1740,7 +1801,11 @@ static void do_connect_b3_req(struct gigaset_capi_ctr *iif,
 	int channel;
 
 	/* decode message */
-	capi_message2cmsg(cmsg, skb->data);
+	if (capi_message2cmsg(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 
 	/* extract and check channel number from PLCI */
@@ -1785,7 +1850,11 @@ static void do_connect_b3_resp(struct gigaset_capi_ctr *iif,
 	u8 command;
 
 	/* decode message */
-	capi_message2cmsg(cmsg, skb->data);
+	if (capi_message2cmsg(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 
 	/* extract and check channel number and NCCI */
@@ -1825,7 +1894,11 @@ static void do_connect_b3_resp(struct gigaset_capi_ctr *iif,
 	capi_cmsg_header(cmsg, ap->id, command, CAPI_IND,
 			 ap->nextMessageNumber++, cmsg->adr.adrNCCI);
 	__skb_trim(skb, msgsize);
-	capi_cmsg2message(cmsg, skb->data);
+	if (capi_cmsg2message(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
 }
@@ -1847,7 +1920,11 @@ static void do_disconnect_req(struct gigaset_capi_ctr *iif,
 	int channel;
 
 	/* decode message */
-	capi_message2cmsg(cmsg, skb->data);
+	if (capi_message2cmsg(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 
 	/* extract and check channel number from PLCI */
@@ -1903,8 +1980,14 @@ static void do_disconnect_req(struct gigaset_capi_ctr *iif,
 			kfree(b3cmsg);
 			return;
 		}
-		capi_cmsg2message(b3cmsg,
-				  __skb_put(b3skb, CAPI_DISCONNECT_B3_IND_BASELEN));
+		if (capi_cmsg2message(b3cmsg,
+				      __skb_put(b3skb, CAPI_DISCONNECT_B3_IND_BASELEN))) {
+			dev_err(cs->dev, "%s: message parser failure\n",
+				__func__);
+			kfree(b3cmsg);
+			dev_kfree_skb_any(b3skb);
+			return;
+		}
 		dump_cmsg(DEBUG_CMD, __func__, b3cmsg);
 		kfree(b3cmsg);
 		capi_ctr_handle_message(&iif->ctr, ap->id, b3skb);
@@ -1935,7 +2018,11 @@ static void do_disconnect_b3_req(struct gigaset_capi_ctr *iif,
 	int channel;
 
 	/* decode message */
-	capi_message2cmsg(cmsg, skb->data);
+	if (capi_message2cmsg(cmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, cmsg);
 
 	/* extract and check channel number and NCCI */
@@ -2052,8 +2139,14 @@ static void do_reset_b3_req(struct gigaset_capi_ctr *iif,
 			    struct gigaset_capi_appl *ap,
 			    struct sk_buff *skb)
 {
+	struct cardstate *cs = iif->ctr.driverdata;
+
 	/* decode message */
-	capi_message2cmsg(&iif->acmsg, skb->data);
+	if (capi_message2cmsg(&iif->acmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 	send_conf(iif, ap, skb,
 		  CapiResetProcedureNotSupportedByCurrentProtocol);
@@ -2066,8 +2159,14 @@ static void do_unsupported(struct gigaset_capi_ctr *iif,
 			   struct gigaset_capi_appl *ap,
 			   struct sk_buff *skb)
 {
+	struct cardstate *cs = iif->ctr.driverdata;
+
 	/* decode message */
-	capi_message2cmsg(&iif->acmsg, skb->data);
+	if (capi_message2cmsg(&iif->acmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 	send_conf(iif, ap, skb, CapiMessageNotSupportedInCurrentState);
 }
@@ -2079,8 +2178,14 @@ static void do_nothing(struct gigaset_capi_ctr *iif,
 		       struct gigaset_capi_appl *ap,
 		       struct sk_buff *skb)
 {
+	struct cardstate *cs = iif->ctr.driverdata;
+
 	/* decode message */
-	capi_message2cmsg(&iif->acmsg, skb->data);
+	if (capi_message2cmsg(&iif->acmsg, skb->data)) {
+		dev_err(cs->dev, "%s: message parser failure\n", __func__);
+		dev_kfree_skb_any(skb);
+		return;
+	}
 	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
 	dev_kfree_skb_any(skb);
 }
@@ -2357,7 +2462,7 @@ int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 	struct gigaset_capi_ctr *iif;
 	int rc;
 
-	iif = kmalloc(sizeof(*iif), GFP_KERNEL);
+	iif = kzalloc(sizeof(*iif), GFP_KERNEL);
 	if (!iif) {
 		pr_err("%s: out of memory\n", __func__);
 		return -ENOMEM;
@@ -2366,7 +2471,7 @@ int gigaset_isdn_regdev(struct cardstate *cs, const char *isdnid)
 	/* prepare controller structure */
 	iif->ctr.owner         = THIS_MODULE;
 	iif->ctr.driverdata    = cs;
-	strncpy(iif->ctr.name, isdnid, sizeof(iif->ctr.name));
+	strncpy(iif->ctr.name, isdnid, sizeof(iif->ctr.name) - 1);
 	iif->ctr.driver_name   = "gigaset";
 	iif->ctr.load_firmware = NULL;
 	iif->ctr.reset_ctr     = NULL;
diff --git a/drivers/isdn/gigaset/ev-layer.c b/drivers/isdn/gigaset/ev-layer.c
index 7459b127ddd5..c8ced12fa452 100644
--- a/drivers/isdn/gigaset/ev-layer.c
+++ b/drivers/isdn/gigaset/ev-layer.c
@@ -604,14 +604,14 @@ void gigaset_handle_modem_response(struct cardstate *cs)
 }
 EXPORT_SYMBOL_GPL(gigaset_handle_modem_response);
 
-/* disconnect
+/* disconnect_nobc
  * process closing of connection associated with given AT state structure
+ * without B channel
  */
-static void disconnect(struct at_state_t **at_state_p)
+static void disconnect_nobc(struct at_state_t **at_state_p,
+			    struct cardstate *cs)
 {
 	unsigned long flags;
-	struct bc_state *bcs = (*at_state_p)->bcs;
-	struct cardstate *cs = (*at_state_p)->cs;
 
 	spin_lock_irqsave(&cs->lock, flags);
 	++(*at_state_p)->seq_index;
@@ -622,23 +622,44 @@ static void disconnect(struct at_state_t **at_state_p)
 		gig_dbg(DEBUG_EVENT, "Scheduling PC_UMMODE");
 		cs->commands_pending = 1;
 	}
-	spin_unlock_irqrestore(&cs->lock, flags);
 
-	if (bcs) {
-		/* B channel assigned: invoke hardware specific handler */
-		cs->ops->close_bchannel(bcs);
-		/* notify LL */
-		if (bcs->chstate & (CHS_D_UP | CHS_NOTIFY_LL)) {
-			bcs->chstate &= ~(CHS_D_UP | CHS_NOTIFY_LL);
-			gigaset_isdn_hupD(bcs);
-		}
-	} else {
-		/* no B channel assigned: just deallocate */
-		spin_lock_irqsave(&cs->lock, flags);
+	/* check for and deallocate temporary AT state */
+	if (!list_empty(&(*at_state_p)->list)) {
 		list_del(&(*at_state_p)->list);
 		kfree(*at_state_p);
 		*at_state_p = NULL;
-		spin_unlock_irqrestore(&cs->lock, flags);
+	}
+
+	spin_unlock_irqrestore(&cs->lock, flags);
+}
+
+/* disconnect_bc
+ * process closing of connection associated with given AT state structure
+ * and B channel
+ */
+static void disconnect_bc(struct at_state_t *at_state,
+			  struct cardstate *cs, struct bc_state *bcs)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&cs->lock, flags);
+	++at_state->seq_index;
+
+	/* revert to selected idle mode */
+	if (!cs->cidmode) {
+		cs->at_state.pending_commands |= PC_UMMODE;
+		gig_dbg(DEBUG_EVENT, "Scheduling PC_UMMODE");
+		cs->commands_pending = 1;
+	}
+	spin_unlock_irqrestore(&cs->lock, flags);
+
+	/* invoke hardware specific handler */
+	cs->ops->close_bchannel(bcs);
+
+	/* notify LL */
+	if (bcs->chstate & (CHS_D_UP | CHS_NOTIFY_LL)) {
+		bcs->chstate &= ~(CHS_D_UP | CHS_NOTIFY_LL);
+		gigaset_isdn_hupD(bcs);
 	}
 }
 
@@ -646,7 +667,7 @@ static void disconnect(struct at_state_t **at_state_p)
  * get a free AT state structure: either one of those associated with the
  * B channels of the Gigaset device, or if none of those is available,
  * a newly allocated one with bcs=NULL
- * The structure should be freed by calling disconnect() after use.
+ * The structure should be freed by calling disconnect_nobc() after use.
  */
 static inline struct at_state_t *get_free_channel(struct cardstate *cs,
 						  int cid)
@@ -1057,7 +1078,7 @@ static void do_action(int action, struct cardstate *cs,
 		      struct event_t *ev)
 {
 	struct at_state_t *at_state = *p_at_state;
-	struct at_state_t *at_state2;
+	struct bc_state *bcs2;
 	unsigned long flags;
 
 	int channel;
@@ -1156,8 +1177,8 @@ static void do_action(int action, struct cardstate *cs,
 		break;
 	case ACT_RING:
 		/* get fresh AT state structure for new CID */
-		at_state2 = get_free_channel(cs, ev->parameter);
-		if (!at_state2) {
+		at_state = get_free_channel(cs, ev->parameter);
+		if (!at_state) {
 			dev_warn(cs->dev,
 				 "RING ignored: could not allocate channel structure\n");
 			break;
@@ -1166,16 +1187,16 @@ static void do_action(int action, struct cardstate *cs,
 		/* initialize AT state structure
 		 * note that bcs may be NULL if no B channel is free
 		 */
-		at_state2->ConState = 700;
+		at_state->ConState = 700;
 		for (i = 0; i < STR_NUM; ++i) {
-			kfree(at_state2->str_var[i]);
-			at_state2->str_var[i] = NULL;
+			kfree(at_state->str_var[i]);
+			at_state->str_var[i] = NULL;
 		}
-		at_state2->int_var[VAR_ZCTP] = -1;
+		at_state->int_var[VAR_ZCTP] = -1;
 
 		spin_lock_irqsave(&cs->lock, flags);
-		at_state2->timer_expires = RING_TIMEOUT;
-		at_state2->timer_active = 1;
+		at_state->timer_expires = RING_TIMEOUT;
+		at_state->timer_active = 1;
 		spin_unlock_irqrestore(&cs->lock, flags);
 		break;
 	case ACT_ICALL:
@@ -1213,14 +1234,17 @@ static void do_action(int action, struct cardstate *cs,
 	case ACT_DISCONNECT:
 		cs->cur_at_seq = SEQ_NONE;
 		at_state->cid = -1;
-		if (bcs && cs->onechannel && cs->dle) {
+		if (!bcs) {
+			disconnect_nobc(p_at_state, cs);
+		} else if (cs->onechannel && cs->dle) {
 			/* Check for other open channels not needed:
 			 * DLE only used for M10x with one B channel.
 			 */
 			at_state->pending_commands |= PC_DLE0;
 			cs->commands_pending = 1;
-		} else
-			disconnect(p_at_state);
+		} else {
+			disconnect_bc(at_state, cs, bcs);
+		}
 		break;
 	case ACT_FAKEDLE0:
 		at_state->int_var[VAR_ZDLE] = 0;
@@ -1228,24 +1252,27 @@ static void do_action(int action, struct cardstate *cs,
 		/* fall through */
 	case ACT_DLE0:
 		cs->cur_at_seq = SEQ_NONE;
-		at_state2 = &cs->bcs[cs->curchannel].at_state;
-		disconnect(&at_state2);
+		bcs2 = cs->bcs + cs->curchannel;
+		disconnect_bc(&bcs2->at_state, cs, bcs2);
 		break;
 	case ACT_ABORTHUP:
 		cs->cur_at_seq = SEQ_NONE;
 		dev_warn(cs->dev, "Could not hang up.\n");
 		at_state->cid = -1;
-		if (bcs && cs->onechannel)
+		if (!bcs)
+			disconnect_nobc(p_at_state, cs);
+		else if (cs->onechannel)
 			at_state->pending_commands |= PC_DLE0;
 		else
-			disconnect(p_at_state);
+			disconnect_bc(at_state, cs, bcs);
 		schedule_init(cs, MS_RECOVER);
 		break;
 	case ACT_FAILDLE0:
 		cs->cur_at_seq = SEQ_NONE;
-		dev_warn(cs->dev, "Could not leave DLE mode.\n");
-		at_state2 = &cs->bcs[cs->curchannel].at_state;
-		disconnect(&at_state2);
+		dev_warn(cs->dev, "Error leaving DLE mode.\n");
+		cs->dle = 0;
+		bcs2 = cs->bcs + cs->curchannel;
+		disconnect_bc(&bcs2->at_state, cs, bcs2);
 		schedule_init(cs, MS_RECOVER);
 		break;
 	case ACT_FAILDLE1:
@@ -1274,14 +1301,14 @@ static void do_action(int action, struct cardstate *cs,
 		if (reinit_and_retry(cs, channel) < 0) {
 			dev_warn(cs->dev,
 				 "Could not get a call ID. Cannot dial.\n");
-			at_state2 = &cs->bcs[channel].at_state;
-			disconnect(&at_state2);
+			bcs2 = cs->bcs + channel;
+			disconnect_bc(&bcs2->at_state, cs, bcs2);
 		}
 		break;
 	case ACT_ABORTCID:
 		cs->cur_at_seq = SEQ_NONE;
-		at_state2 = &cs->bcs[cs->curchannel].at_state;
-		disconnect(&at_state2);
+		bcs2 = cs->bcs + cs->curchannel;
+		disconnect_bc(&bcs2->at_state, cs, bcs2);
 		break;
 
 	case ACT_DIALING:
@@ -1290,7 +1317,10 @@ static void do_action(int action, struct cardstate *cs,
 		break;
 
 	case ACT_ABORTACCEPT:	/* hangup/error/timeout during ICALL procssng */
-		disconnect(p_at_state);
+		if (bcs)
+			disconnect_bc(at_state, cs, bcs);
+		else
+			disconnect_nobc(p_at_state, cs);
 		break;
 
 	case ACT_ABORTDIAL:	/* error/timeout during dial preparation */
@@ -1379,6 +1409,11 @@ static void do_action(int action, struct cardstate *cs,
 	/* events from the LL */
 
 	case ACT_DIAL:
+		if (!ev->ptr) {
+			*p_genresp = 1;
+			*p_resp_code = RSP_ERROR;
+			break;
+		}
 		start_dial(at_state, ev->ptr, ev->parameter);
 		break;
 	case ACT_ACCEPT:
diff --git a/drivers/isdn/gigaset/usb-gigaset.c b/drivers/isdn/gigaset/usb-gigaset.c
index d0a41cb0cf62..a8e652dac54d 100644
--- a/drivers/isdn/gigaset/usb-gigaset.c
+++ b/drivers/isdn/gigaset/usb-gigaset.c
@@ -135,14 +135,13 @@ struct usb_cardstate {
 	/* Output buffer */
 	unsigned char		*bulk_out_buffer;
 	int			bulk_out_size;
-	__u8			bulk_out_endpointAddr;
+	int			bulk_out_epnum;
 	struct urb		*bulk_out_urb;
 
 	/* Input buffer */
 	unsigned char		*rcvbuf;
 	int			rcvbuf_size;
 	struct urb		*read_urb;
-	__u8			int_in_endpointAddr;
 
 	char			bchars[6];		/* for request 0x19 */
 };
@@ -466,7 +465,7 @@ static int send_cb(struct cardstate *cs, struct cmdbuf_t *cb)
 
 			usb_fill_bulk_urb(ucs->bulk_out_urb, ucs->udev,
 					  usb_sndbulkpipe(ucs->udev,
-							  ucs->bulk_out_endpointAddr & 0x0f),
+							  ucs->bulk_out_epnum),
 					  cb->buf + cb->offset, count,
 					  gigaset_write_bulk_callback, cs);
 
@@ -498,6 +497,7 @@ static int send_cb(struct cardstate *cs, struct cmdbuf_t *cb)
 static int gigaset_write_cmd(struct cardstate *cs, struct cmdbuf_t *cb)
 {
 	unsigned long flags;
+	int len;
 
 	gigaset_dbg_buffer(cs->mstate != MS_LOCKED ?
 			   DEBUG_TRANSCMD : DEBUG_LOCKCMD,
@@ -516,10 +516,11 @@ static int gigaset_write_cmd(struct cardstate *cs, struct cmdbuf_t *cb)
 	spin_unlock_irqrestore(&cs->cmdlock, flags);
 
 	spin_lock_irqsave(&cs->lock, flags);
+	len = cb->len;
 	if (cs->connected)
 		tasklet_schedule(&cs->write_tasklet);
 	spin_unlock_irqrestore(&cs->lock, flags);
-	return cb->len;
+	return len;
 }
 
 static int gigaset_write_room(struct cardstate *cs)
@@ -628,8 +629,7 @@ static int write_modem(struct cardstate *cs)
 	if (cs->connected) {
 		usb_fill_bulk_urb(ucs->bulk_out_urb, ucs->udev,
 				  usb_sndbulkpipe(ucs->udev,
-						  ucs->bulk_out_endpointAddr &
-						  0x0f),
+						  ucs->bulk_out_epnum),
 				  ucs->bulk_out_buffer, count,
 				  gigaset_write_bulk_callback, cs);
 		ret = usb_submit_urb(ucs->bulk_out_urb, GFP_ATOMIC);
@@ -714,7 +714,7 @@ static int gigaset_probe(struct usb_interface *interface,
 
 	buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
 	ucs->bulk_out_size = buffer_size;
-	ucs->bulk_out_endpointAddr = endpoint->bEndpointAddress;
+	ucs->bulk_out_epnum = usb_endpoint_num(endpoint);
 	ucs->bulk_out_buffer = kmalloc(buffer_size, GFP_KERNEL);
 	if (!ucs->bulk_out_buffer) {
 		dev_err(cs->dev, "Couldn't allocate bulk_out_buffer\n");
@@ -741,7 +741,6 @@ static int gigaset_probe(struct usb_interface *interface,
 	}
 	buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
 	ucs->rcvbuf_size = buffer_size;
-	ucs->int_in_endpointAddr = endpoint->bEndpointAddress;
 	ucs->rcvbuf = kmalloc(buffer_size, GFP_KERNEL);
 	if (!ucs->rcvbuf) {
 		dev_err(cs->dev, "Couldn't allocate rcvbuf\n");
@@ -750,8 +749,7 @@ static int gigaset_probe(struct usb_interface *interface,
 	}
 	/* Fill the interrupt urb and send it to the core */
 	usb_fill_int_urb(ucs->read_urb, udev,
-			 usb_rcvintpipe(udev,
-					endpoint->bEndpointAddress & 0x0f),
+			 usb_rcvintpipe(udev, usb_endpoint_num(endpoint)),
 			 ucs->rcvbuf, buffer_size,
 			 gigaset_read_int_callback,
 			 cs, endpoint->bInterval);
diff --git a/drivers/isdn/hardware/eicon/xdi_msg.h b/drivers/isdn/hardware/eicon/xdi_msg.h
index 58368f7b5cba..2498c349a32e 100644
--- a/drivers/isdn/hardware/eicon/xdi_msg.h
+++ b/drivers/isdn/hardware/eicon/xdi_msg.h
@@ -1,6 +1,6 @@
 /* $Id: xdi_msg.h,v 1.1.2.2 2001/02/16 08:40:36 armin Exp $ */
 
-#ifndef __DIVA_XDI_UM_CFG_MESSSGE_H__
+#ifndef __DIVA_XDI_UM_CFG_MESSAGE_H__
 #define __DIVA_XDI_UM_CFG_MESSAGE_H__
 
 /*
diff --git a/drivers/isdn/i4l/isdn_tty.c b/drivers/isdn/i4l/isdn_tty.c
index 3c5f2491a16f..bc912611fe09 100644
--- a/drivers/isdn/i4l/isdn_tty.c
+++ b/drivers/isdn/i4l/isdn_tty.c
@@ -1043,11 +1043,6 @@ isdn_tty_change_speed(modem_info *info)
 	if (!(cflag & PARODD))
 		cval |= UART_LCR_EPAR;
 
-	/* CTS flow control flag and modem status interrupts */
-	if (cflag & CRTSCTS) {
-		port->flags |= ASYNC_CTS_FLOW;
-	} else
-		port->flags &= ~ASYNC_CTS_FLOW;
 	if (cflag & CLOCAL)
 		port->flags &= ~ASYNC_CHECK_CD;
 	else {
diff --git a/drivers/isdn/mISDN/dsp_cmx.c b/drivers/isdn/mISDN/dsp_cmx.c
index a4f05c54c32b..87f7dff20ff6 100644
--- a/drivers/isdn/mISDN/dsp_cmx.c
+++ b/drivers/isdn/mISDN/dsp_cmx.c
@@ -1454,66 +1454,63 @@ dsp_cmx_send_member(struct dsp *dsp, int len, s32 *c, int members)
 #ifdef CMX_CONF_DEBUG
 	if (0) {
 #else
-		if (members == 2) {
+	if (members == 2) {
 #endif
-			/* "other" becomes other party */
-			other = (list_entry(conf->mlist.next,
-					    struct dsp_conf_member, list))->dsp;
-			if (other == member)
-				other = (list_entry(conf->mlist.prev,
-						    struct dsp_conf_member, list))->dsp;
-			o_q = other->rx_buff; /* received data */
-			o_rr = (other->rx_R + len) & CMX_BUFF_MASK;
-			/* end of rx-pointer */
-			o_r = (o_rr - rr + r) & CMX_BUFF_MASK;
-			/* start rx-pointer at current read position*/
-			/* -> if echo is NOT enabled */
-			if (!dsp->echo.software) {
-				/*
-				 * -> copy other member's rx-data,
-				 * if tx-data is available, mix
-				 */
-				while (o_r != o_rr && t != tt) {
-					*d++ = dsp_audio_mix_law[(p[t] << 8) | o_q[o_r]];
-					t = (t + 1) & CMX_BUFF_MASK;
-					o_r = (o_r + 1) & CMX_BUFF_MASK;
-				}
-				while (o_r != o_rr) {
-					*d++ = o_q[o_r];
-					o_r = (o_r + 1) & CMX_BUFF_MASK;
-				}
-				/* -> if echo is enabled */
-			} else {
-				/*
-				 * -> mix other member's rx-data with echo,
-				 * if tx-data is available, mix
-				 */
-				while (r != rr && t != tt) {
-					sample = dsp_audio_law_to_s32[p[t]] +
-						dsp_audio_law_to_s32[q[r]] +
-						dsp_audio_law_to_s32[o_q[o_r]];
-					if (sample < -32768)
-						sample = -32768;
-					else if (sample > 32767)
-						sample = 32767;
-					*d++ = dsp_audio_s16_to_law[sample & 0xffff];
-					/* tx-data + rx_data + echo */
-					t = (t + 1) & CMX_BUFF_MASK;
-					r = (r + 1) & CMX_BUFF_MASK;
-					o_r = (o_r + 1) & CMX_BUFF_MASK;
-				}
-				while (r != rr) {
-					*d++ = dsp_audio_mix_law[(q[r] << 8) | o_q[o_r]];
-					r = (r + 1) & CMX_BUFF_MASK;
-					o_r = (o_r + 1) & CMX_BUFF_MASK;
-				}
+		/* "other" becomes other party */
+		other = (list_entry(conf->mlist.next,
+				    struct dsp_conf_member, list))->dsp;
+		if (other == member)
+			other = (list_entry(conf->mlist.prev,
+				    struct dsp_conf_member, list))->dsp;
+		o_q = other->rx_buff; /* received data */
+		o_rr = (other->rx_R + len) & CMX_BUFF_MASK;
+		/* end of rx-pointer */
+		o_r = (o_rr - rr + r) & CMX_BUFF_MASK;
+		/* start rx-pointer at current read position*/
+		/* -> if echo is NOT enabled */
+		if (!dsp->echo.software) {
+			/*
+			 * -> copy other member's rx-data,
+			 * if tx-data is available, mix
+			 */
+			while (o_r != o_rr && t != tt) {
+				*d++ = dsp_audio_mix_law[(p[t] << 8) | o_q[o_r]];
+				t = (t + 1) & CMX_BUFF_MASK;
+				o_r = (o_r + 1) & CMX_BUFF_MASK;
+			}
+			while (o_r != o_rr) {
+				*d++ = o_q[o_r];
+				o_r = (o_r + 1) & CMX_BUFF_MASK;
+			}
+			/* -> if echo is enabled */
+		} else {
+			/*
+			 * -> mix other member's rx-data with echo,
+			 * if tx-data is available, mix
+			 */
+			while (r != rr && t != tt) {
+				sample = dsp_audio_law_to_s32[p[t]] +
+					dsp_audio_law_to_s32[q[r]] +
+					dsp_audio_law_to_s32[o_q[o_r]];
+				if (sample < -32768)
+					sample = -32768;
+				else if (sample > 32767)
+					sample = 32767;
+				*d++ = dsp_audio_s16_to_law[sample & 0xffff];
+				/* tx-data + rx_data + echo */
+				t = (t + 1) & CMX_BUFF_MASK;
+				r = (r + 1) & CMX_BUFF_MASK;
+				o_r = (o_r + 1) & CMX_BUFF_MASK;
+			}
+			while (r != rr) {
+				*d++ = dsp_audio_mix_law[(q[r] << 8) | o_q[o_r]];
+				r = (r + 1) & CMX_BUFF_MASK;
+				o_r = (o_r + 1) & CMX_BUFF_MASK;
 			}
-			dsp->tx_R = t;
-			goto send_packet;
 		}
-#ifdef DSP_NEVER_DEFINED
+		dsp->tx_R = t;
+		goto send_packet;
 	}
-#endif
 	/* PROCESS DATA (three or more members) */
 	/* -> if echo is NOT enabled */
 	if (!dsp->echo.software) {
diff --git a/drivers/leds/Kconfig b/drivers/leds/Kconfig
index 8c96e2ddf43b..a210338cfeb1 100644
--- a/drivers/leds/Kconfig
+++ b/drivers/leds/Kconfig
@@ -410,7 +410,7 @@ config LEDS_MC13783
 config LEDS_NS2
 	tristate "LED support for Network Space v2 GPIO LEDs"
 	depends on LEDS_CLASS
-	depends on ARCH_KIRKWOOD || MACH_KIRKWOOD
+	depends on MACH_KIRKWOOD
 	default y
 	help
 	  This option enable support for the dual-GPIO LED found on the
@@ -420,7 +420,7 @@ config LEDS_NS2
 config LEDS_NETXBIG
 	tristate "LED support for Big Network series LEDs"
 	depends on LEDS_CLASS
-	depends on ARCH_KIRKWOOD || MACH_KIRKWOOD
+	depends on MACH_KIRKWOOD
 	default y
 	help
 	  This option enable support for LEDs found on the LaCie 2Big
@@ -468,6 +468,15 @@ config LEDS_OT200
 	  This option enables support for the LEDs on the Bachmann OT200.
 	  Say Y to enable LEDs on the Bachmann OT200.
 
+config LEDS_MENF21BMC
+	tristate "LED support for the MEN 14F021P00 BMC"
+	depends on LEDS_CLASS && MFD_MENF21BMC
+	help
+	  Say Y here to include support for the MEN 14F021P00 BMC LEDs.
+
+	  This driver can also be built as a module. If so the module
+	  will be called leds-menf21bmc.
+
 comment "LED driver for blink(1) USB RGB LED is under Special HID drivers (HID_THINGM)"
 
 config LEDS_BLINKM
@@ -478,6 +487,16 @@ config LEDS_BLINKM
 	  This option enables support for the BlinkM RGB LED connected
 	  through I2C. Say Y to enable support for the BlinkM LED.
 
+config LEDS_SYSCON
+	bool "LED support for LEDs on system controllers"
+	depends on LEDS_CLASS=y
+	depends on MFD_SYSCON
+	depends on OF
+	help
+	  This option enabled support for the LEDs on syscon type
+	  devices. This will only work with device tree enabled
+	  devices.
+
 config LEDS_VERSATILE
 	tristate "LED support for the ARM Versatile and RealView"
 	depends on ARCH_REALVIEW || ARCH_VERSATILE
diff --git a/drivers/leds/Makefile b/drivers/leds/Makefile
index d8cc5f2777de..a2b164741465 100644
--- a/drivers/leds/Makefile
+++ b/drivers/leds/Makefile
@@ -53,7 +53,9 @@ obj-$(CONFIG_LEDS_ASIC3)		+= leds-asic3.o
 obj-$(CONFIG_LEDS_MAX8997)		+= leds-max8997.o
 obj-$(CONFIG_LEDS_LM355x)		+= leds-lm355x.o
 obj-$(CONFIG_LEDS_BLINKM)		+= leds-blinkm.o
+obj-$(CONFIG_LEDS_SYSCON)		+= leds-syscon.o
 obj-$(CONFIG_LEDS_VERSATILE)		+= leds-versatile.o
+obj-$(CONFIG_LEDS_MENF21BMC)		+= leds-menf21bmc.o
 
 # LED SPI Drivers
 obj-$(CONFIG_LEDS_DAC124S085)		+= leds-dac124s085.o
diff --git a/drivers/leds/led-class.c b/drivers/leds/led-class.c
index 129729d35478..7440c58b8e6f 100644
--- a/drivers/leds/led-class.c
+++ b/drivers/leds/led-class.c
@@ -9,26 +9,21 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/list.h>
-#include <linux/spinlock.h>
+#include <linux/ctype.h>
 #include <linux/device.h>
 #include <linux/err.h>
-#include <linux/ctype.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
 #include <linux/leds.h>
-#include <linux/workqueue.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/timer.h>
 #include "leds.h"
 
 static struct class *leds_class;
 
-static void led_update_brightness(struct led_classdev *led_cdev)
-{
-	if (led_cdev->brightness_get)
-		led_cdev->brightness = led_cdev->brightness_get(led_cdev);
-}
-
 static ssize_t brightness_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
@@ -59,14 +54,14 @@ static ssize_t brightness_store(struct device *dev,
 }
 static DEVICE_ATTR_RW(brightness);
 
-static ssize_t led_max_brightness_show(struct device *dev,
+static ssize_t max_brightness_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
 	struct led_classdev *led_cdev = dev_get_drvdata(dev);
 
 	return sprintf(buf, "%u\n", led_cdev->max_brightness);
 }
-static DEVICE_ATTR(max_brightness, 0444, led_max_brightness_show, NULL);
+static DEVICE_ATTR_RO(max_brightness);
 
 #ifdef CONFIG_LEDS_TRIGGERS
 static DEVICE_ATTR(trigger, 0644, led_trigger_show, led_trigger_store);
@@ -97,10 +92,9 @@ static const struct attribute_group *led_groups[] = {
 	NULL,
 };
 
-static void led_work_function(struct work_struct *ws)
+static void led_timer_function(unsigned long data)
 {
-	struct led_classdev *led_cdev =
-		container_of(ws, struct led_classdev, blink_work.work);
+	struct led_classdev *led_cdev = (void *)data;
 	unsigned long brightness;
 	unsigned long delay;
 
@@ -144,8 +138,7 @@ static void led_work_function(struct work_struct *ws)
 		}
 	}
 
-	queue_delayed_work(system_wq, &led_cdev->blink_work,
-			   msecs_to_jiffies(delay));
+	mod_timer(&led_cdev->blink_timer, jiffies + msecs_to_jiffies(delay));
 }
 
 static void set_brightness_delayed(struct work_struct *ws)
@@ -233,7 +226,9 @@ int led_classdev_register(struct device *parent, struct led_classdev *led_cdev)
 
 	INIT_WORK(&led_cdev->set_brightness_work, set_brightness_delayed);
 
-	INIT_DELAYED_WORK(&led_cdev->blink_work, led_work_function);
+	init_timer(&led_cdev->blink_timer);
+	led_cdev->blink_timer.function = led_timer_function;
+	led_cdev->blink_timer.data = (unsigned long)led_cdev;
 
 #ifdef CONFIG_LEDS_TRIGGERS
 	led_trigger_set_default(led_cdev);
diff --git a/drivers/leds/led-core.c b/drivers/leds/led-core.c
index 4bb116867b88..aaa8eba9099f 100644
--- a/drivers/leds/led-core.c
+++ b/drivers/leds/led-core.c
@@ -12,11 +12,11 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/leds.h>
 #include <linux/list.h>
 #include <linux/module.h>
+#include <linux/mutex.h>
 #include <linux/rwsem.h>
-#include <linux/leds.h>
-#include <linux/workqueue.h>
 #include "leds.h"
 
 DECLARE_RWSEM(leds_list_lock);
@@ -52,7 +52,7 @@ static void led_set_software_blink(struct led_classdev *led_cdev,
 		return;
 	}
 
-	queue_delayed_work(system_wq, &led_cdev->blink_work, 1);
+	mod_timer(&led_cdev->blink_timer, jiffies + 1);
 }
 
 
@@ -76,7 +76,7 @@ void led_blink_set(struct led_classdev *led_cdev,
 		   unsigned long *delay_on,
 		   unsigned long *delay_off)
 {
-	cancel_delayed_work_sync(&led_cdev->blink_work);
+	del_timer_sync(&led_cdev->blink_timer);
 
 	led_cdev->flags &= ~LED_BLINK_ONESHOT;
 	led_cdev->flags &= ~LED_BLINK_ONESHOT_STOP;
@@ -91,7 +91,7 @@ void led_blink_set_oneshot(struct led_classdev *led_cdev,
 			   int invert)
 {
 	if ((led_cdev->flags & LED_BLINK_ONESHOT) &&
-	     delayed_work_pending(&led_cdev->blink_work))
+	     timer_pending(&led_cdev->blink_timer))
 		return;
 
 	led_cdev->flags |= LED_BLINK_ONESHOT;
@@ -108,7 +108,7 @@ EXPORT_SYMBOL(led_blink_set_oneshot);
 
 void led_stop_software_blink(struct led_classdev *led_cdev)
 {
-	cancel_delayed_work_sync(&led_cdev->blink_work);
+	del_timer_sync(&led_cdev->blink_timer);
 	led_cdev->blink_delay_on = 0;
 	led_cdev->blink_delay_off = 0;
 }
@@ -117,7 +117,7 @@ EXPORT_SYMBOL_GPL(led_stop_software_blink);
 void led_set_brightness(struct led_classdev *led_cdev,
 			enum led_brightness brightness)
 {
-	/* delay brightness setting if need to stop soft-blink work */
+	/* delay brightness setting if need to stop soft-blink timer */
 	if (led_cdev->blink_delay_on || led_cdev->blink_delay_off) {
 		led_cdev->delayed_set_value = brightness;
 		schedule_work(&led_cdev->set_brightness_work);
@@ -127,3 +127,19 @@ void led_set_brightness(struct led_classdev *led_cdev,
 	__led_set_brightness(led_cdev, brightness);
 }
 EXPORT_SYMBOL(led_set_brightness);
+
+int led_update_brightness(struct led_classdev *led_cdev)
+{
+	int ret = 0;
+
+	if (led_cdev->brightness_get) {
+		ret = led_cdev->brightness_get(led_cdev);
+		if (ret >= 0) {
+			led_cdev->brightness = ret;
+			return 0;
+		}
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(led_update_brightness);
diff --git a/drivers/leds/leds-gpio-register.c b/drivers/leds/leds-gpio-register.c
index 1c4ed5510f35..75717ba68ae0 100644
--- a/drivers/leds/leds-gpio-register.c
+++ b/drivers/leds/leds-gpio-register.c
@@ -7,9 +7,9 @@
  * Free Software Foundation.
  */
 #include <linux/err.h>
+#include <linux/leds.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include <linux/leds.h>
 
 /**
  * gpio_led_register_device - register a gpio-led device
@@ -28,6 +28,9 @@ struct platform_device *__init gpio_led_register_device(
 	struct platform_device *ret;
 	struct gpio_led_platform_data _pdata = *pdata;
 
+	if (!pdata->num_leds)
+		return ERR_PTR(-EINVAL);
+
 	_pdata.leds = kmemdup(pdata->leds,
 			pdata->num_leds * sizeof(*pdata->leds), GFP_KERNEL);
 	if (!_pdata.leds)
diff --git a/drivers/leds/leds-gpio.c b/drivers/leds/leds-gpio.c
index 57ff20fecf57..b4518c8751c8 100644
--- a/drivers/leds/leds-gpio.c
+++ b/drivers/leds/leds-gpio.c
@@ -10,17 +10,17 @@
  * published by the Free Software Foundation.
  *
  */
-#include <linux/kernel.h>
-#include <linux/platform_device.h>
+#include <linux/err.h>
 #include <linux/gpio.h>
+#include <linux/kernel.h>
 #include <linux/leds.h>
+#include <linux/module.h>
 #include <linux/of.h>
-#include <linux/of_platform.h>
 #include <linux/of_gpio.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/workqueue.h>
-#include <linux/module.h>
-#include <linux/err.h>
 
 struct gpio_led_data {
 	struct led_classdev cdev;
@@ -36,7 +36,7 @@ struct gpio_led_data {
 
 static void gpio_led_work(struct work_struct *work)
 {
-	struct gpio_led_data	*led_dat =
+	struct gpio_led_data *led_dat =
 		container_of(work, struct gpio_led_data, work);
 
 	if (led_dat->blinking) {
@@ -235,14 +235,12 @@ static struct gpio_leds_priv *gpio_leds_create_of(struct platform_device *pdev)
 }
 #endif /* CONFIG_OF_GPIO */
 
-
 static int gpio_led_probe(struct platform_device *pdev)
 {
 	struct gpio_led_platform_data *pdata = dev_get_platdata(&pdev->dev);
 	struct gpio_leds_priv *priv;
 	int i, ret = 0;
 
-
 	if (pdata && pdata->num_leds) {
 		priv = devm_kzalloc(&pdev->dev,
 				sizeof_gpio_leds_priv(pdata->num_leds),
diff --git a/drivers/leds/leds-lp3944.c b/drivers/leds/leds-lp3944.c
index 8e1abdcd4c9d..53144fb96167 100644
--- a/drivers/leds/leds-lp3944.c
+++ b/drivers/leds/leds-lp3944.c
@@ -335,7 +335,8 @@ static int lp3944_configure(struct i2c_client *client,
 			}
 
 			/* to expose the default value to userspace */
-			led->ldev.brightness = led->status;
+			led->ldev.brightness =
+					(enum led_brightness) led->status;
 
 			/* Set the default led status */
 			err = lp3944_led_set(led, led->status);
diff --git a/drivers/leds/leds-menf21bmc.c b/drivers/leds/leds-menf21bmc.c
new file mode 100644
index 000000000000..89dd57769e3b
--- /dev/null
+++ b/drivers/leds/leds-menf21bmc.c
@@ -0,0 +1,131 @@
+/*
+ *  MEN 14F021P00 Board Management Controller (BMC) LEDs Driver.
+ *
+ *  This is the core LED driver of the MEN 14F021P00 BMC.
+ *  There are four LEDs available which can be switched on and off.
+ *  STATUS LED, HOT SWAP LED, USER LED 1, USER LED 2
+ *
+ *  Copyright (C) 2014 MEN Mikro Elektronik Nuernberg GmbH
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/leds.h>
+#include <linux/i2c.h>
+
+#define BMC_CMD_LED_GET_SET	0xA0
+#define BMC_BIT_LED_STATUS	BIT(0)
+#define BMC_BIT_LED_HOTSWAP	BIT(1)
+#define BMC_BIT_LED_USER1	BIT(2)
+#define BMC_BIT_LED_USER2	BIT(3)
+
+struct menf21bmc_led {
+	struct led_classdev cdev;
+	u8 led_bit;
+	const char *name;
+	struct i2c_client *i2c_client;
+};
+
+static struct menf21bmc_led leds[] = {
+	{
+		.name = "menf21bmc:led_status",
+		.led_bit = BMC_BIT_LED_STATUS,
+	},
+	{
+		.name = "menf21bmc:led_hotswap",
+		.led_bit = BMC_BIT_LED_HOTSWAP,
+	},
+	{
+		.name = "menf21bmc:led_user1",
+		.led_bit = BMC_BIT_LED_USER1,
+	},
+	{
+		.name = "menf21bmc:led_user2",
+		.led_bit = BMC_BIT_LED_USER2,
+	}
+};
+
+static DEFINE_MUTEX(led_lock);
+
+static void
+menf21bmc_led_set(struct led_classdev *led_cdev, enum led_brightness value)
+{
+	int led_val;
+	struct menf21bmc_led *led = container_of(led_cdev,
+					struct menf21bmc_led, cdev);
+
+	mutex_lock(&led_lock);
+	led_val = i2c_smbus_read_byte_data(led->i2c_client,
+					   BMC_CMD_LED_GET_SET);
+	if (led_val < 0)
+		goto err_out;
+
+	if (value == LED_OFF)
+		led_val &= ~led->led_bit;
+	else
+		led_val |= led->led_bit;
+
+	i2c_smbus_write_byte_data(led->i2c_client,
+				  BMC_CMD_LED_GET_SET, led_val);
+err_out:
+	mutex_unlock(&led_lock);
+}
+
+static int menf21bmc_led_probe(struct platform_device *pdev)
+{
+	int i;
+	int ret;
+	struct i2c_client *i2c_client = to_i2c_client(pdev->dev.parent);
+
+	for (i = 0; i < ARRAY_SIZE(leds); i++) {
+		leds[i].cdev.name = leds[i].name;
+		leds[i].cdev.brightness_set = menf21bmc_led_set;
+		leds[i].i2c_client = i2c_client;
+		ret = led_classdev_register(&pdev->dev, &leds[i].cdev);
+		if (ret < 0)
+			goto err_free_leds;
+	}
+	dev_info(&pdev->dev, "MEN 140F21P00 BMC LED device enabled\n");
+
+	return 0;
+
+err_free_leds:
+	dev_err(&pdev->dev, "failed to register LED device\n");
+
+	for (i = i - 1; i >= 0; i--)
+		led_classdev_unregister(&leds[i].cdev);
+
+	return ret;
+}
+
+static int menf21bmc_led_remove(struct platform_device *pdev)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(leds); i++)
+		led_classdev_unregister(&leds[i].cdev);
+
+	return 0;
+}
+
+static struct platform_driver menf21bmc_led = {
+	.probe		= menf21bmc_led_probe,
+	.remove		= menf21bmc_led_remove,
+	.driver		= {
+		.name		= "menf21bmc_led",
+		.owner		= THIS_MODULE,
+	},
+};
+
+module_platform_driver(menf21bmc_led);
+
+MODULE_AUTHOR("Andreas Werner <andreas.werner@men.de>");
+MODULE_DESCRIPTION("MEN 14F021P00 BMC led driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:menf21bmc_led");
diff --git a/drivers/leds/leds-pca9532.c b/drivers/leds/leds-pca9532.c
index 4a0e786b7832..5a6363d161a2 100644
--- a/drivers/leds/leds-pca9532.c
+++ b/drivers/leds/leds-pca9532.c
@@ -319,14 +319,8 @@ static int pca9532_destroy_devices(struct pca9532_data *data, int n_devs)
 	}
 
 #ifdef CONFIG_LEDS_PCA9532_GPIO
-	if (data->gpio.dev) {
-		int err = gpiochip_remove(&data->gpio);
-		if (err) {
-			dev_err(&data->client->dev, "%s failed, %d\n",
-						"gpiochip_remove()", err);
-			return err;
-		}
-	}
+	if (data->gpio.dev)
+		gpiochip_remove(&data->gpio);
 #endif
 
 	return 0;
diff --git a/drivers/leds/leds-syscon.c b/drivers/leds/leds-syscon.c
new file mode 100644
index 000000000000..3afec79c43f4
--- /dev/null
+++ b/drivers/leds/leds-syscon.c
@@ -0,0 +1,166 @@
+/*
+ * Generic Syscon LEDs Driver
+ *
+ * Copyright (c) 2014, Linaro Limited
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ *
+ * This driver provides system reboot functionality for APM X-Gene SoC.
+ * For system shutdown, this is board specify. If a board designer
+ * implements GPIO shutdown, use the gpio-poweroff.c driver.
+ */
+#include <linux/io.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/stat.h>
+#include <linux/slab.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/leds.h>
+
+/**
+ * struct syscon_led - state container for syscon based LEDs
+ * @cdev: LED class device for this LED
+ * @map: regmap to access the syscon device backing this LED
+ * @offset: the offset into the syscon regmap for the LED register
+ * @mask: the bit in the register corresponding to the LED
+ * @state: current state of the LED
+ */
+struct syscon_led {
+	struct led_classdev cdev;
+	struct regmap *map;
+	u32 offset;
+	u32 mask;
+	bool state;
+};
+
+static void syscon_led_set(struct led_classdev *led_cdev,
+	enum led_brightness value)
+{
+	struct syscon_led *sled =
+		container_of(led_cdev, struct syscon_led, cdev);
+	u32 val;
+	int ret;
+
+	if (value == LED_OFF) {
+		val = 0;
+		sled->state = false;
+	} else {
+		val = sled->mask;
+		sled->state = true;
+	}
+
+	ret = regmap_update_bits(sled->map, sled->offset, sled->mask, val);
+	if (ret < 0)
+		dev_err(sled->cdev.dev, "error updating LED status\n");
+}
+
+static const struct of_device_id syscon_match[] = {
+	{ .compatible = "syscon", },
+	{},
+};
+
+static int __init syscon_leds_init(void)
+{
+	const struct of_device_id *devid;
+	struct device_node *np;
+	struct device_node *child;
+	struct regmap *map;
+	struct platform_device *pdev;
+	struct device *dev;
+	int ret;
+
+	np = of_find_matching_node_and_match(NULL, syscon_match,
+					     &devid);
+	if (!np)
+		return -ENODEV;
+
+	map = syscon_node_to_regmap(np);
+	if (IS_ERR(map))
+		return PTR_ERR(map);
+
+	/*
+	 * If the map is there, the device should be there, we allocate
+	 * memory on the syscon device's behalf here.
+	 */
+	pdev = of_find_device_by_node(np);
+	if (!pdev)
+		return -ENODEV;
+	dev = &pdev->dev;
+
+	for_each_available_child_of_node(np, child) {
+		struct syscon_led *sled;
+		const char *state;
+
+		/* Only check for register-bit-leds */
+		if (of_property_match_string(child, "compatible",
+					     "register-bit-led") < 0)
+			continue;
+
+		sled = devm_kzalloc(dev, sizeof(*sled), GFP_KERNEL);
+		if (!sled)
+			return -ENOMEM;
+
+		sled->map = map;
+
+		if (of_property_read_u32(child, "offset", &sled->offset))
+			return -EINVAL;
+		if (of_property_read_u32(child, "mask", &sled->mask))
+			return -EINVAL;
+		sled->cdev.name =
+			of_get_property(child, "label", NULL) ? : child->name;
+		sled->cdev.default_trigger =
+			of_get_property(child, "linux,default-trigger", NULL);
+
+		state = of_get_property(child, "default-state", NULL);
+		if (state) {
+			if (!strcmp(state, "keep")) {
+				u32 val;
+
+				ret = regmap_read(map, sled->offset, &val);
+				if (ret < 0)
+					return ret;
+				sled->state = !!(val & sled->mask);
+			} else if (!strcmp(state, "on")) {
+				sled->state = true;
+				ret = regmap_update_bits(map, sled->offset,
+							 sled->mask,
+							 sled->mask);
+				if (ret < 0)
+					return ret;
+			} else {
+				sled->state = false;
+				ret = regmap_update_bits(map, sled->offset,
+							 sled->mask, 0);
+				if (ret < 0)
+					return ret;
+			}
+
+		}
+		sled->cdev.brightness_set = syscon_led_set;
+
+		ret = led_classdev_register(dev, &sled->cdev);
+		if (ret < 0)
+			return ret;
+
+		dev_info(dev, "registered LED %s\n", sled->cdev.name);
+	}
+
+       return 0;
+}
+device_initcall(syscon_leds_init);
diff --git a/drivers/leds/leds-tca6507.c b/drivers/leds/leds-tca6507.c
index 3d9e267a56c4..20fa8e77f186 100644
--- a/drivers/leds/leds-tca6507.c
+++ b/drivers/leds/leds-tca6507.c
@@ -667,11 +667,8 @@ static int tca6507_probe_gpios(struct i2c_client *client,
 
 static void tca6507_remove_gpio(struct tca6507_chip *tca)
 {
-	if (tca->gpio.ngpio) {
-		int err = gpiochip_remove(&tca->gpio);
-		dev_err(&tca->client->dev, "%s failed, %d\n",
-			"gpiochip_remove()", err);
-	}
+	if (tca->gpio.ngpio)
+		gpiochip_remove(&tca->gpio);
 }
 #else /* CONFIG_GPIOLIB */
 static int tca6507_probe_gpios(struct i2c_client *client,
diff --git a/drivers/leds/trigger/ledtrig-gpio.c b/drivers/leds/trigger/ledtrig-gpio.c
index 35812e3a37f2..c86c41826476 100644
--- a/drivers/leds/trigger/ledtrig-gpio.c
+++ b/drivers/leds/trigger/ledtrig-gpio.c
@@ -48,7 +48,7 @@ static void gpio_trig_work(struct work_struct *work)
 	if (!gpio_data->gpio)
 		return;
 
-	tmp = gpio_get_value(gpio_data->gpio);
+	tmp = gpio_get_value_cansleep(gpio_data->gpio);
 	if (gpio_data->inverted)
 		tmp = !tmp;
 
diff --git a/drivers/macintosh/adb.c b/drivers/macintosh/adb.c
index 9e9c56758a08..226179b975a0 100644
--- a/drivers/macintosh/adb.c
+++ b/drivers/macintosh/adb.c
@@ -411,6 +411,7 @@ adb_poll(void)
 		return;
 	adb_controller->poll();
 }
+EXPORT_SYMBOL(adb_poll);
 
 static void adb_sync_req_done(struct adb_request *req)
 {
@@ -460,6 +461,7 @@ adb_request(struct adb_request *req, void (*done)(struct adb_request *),
 
 	return rc;
 }
+EXPORT_SYMBOL(adb_request);
 
  /* Ultimately this should return the number of devices with
     the given default id.
@@ -495,6 +497,7 @@ adb_register(int default_id, int handler_id, struct adb_ids *ids,
 	mutex_unlock(&adb_handler_mutex);
 	return ids->nids;
 }
+EXPORT_SYMBOL(adb_register);
 
 int
 adb_unregister(int index)
@@ -516,6 +519,7 @@ adb_unregister(int index)
 	mutex_unlock(&adb_handler_mutex);
 	return ret;
 }
+EXPORT_SYMBOL(adb_unregister);
 
 void
 adb_input(unsigned char *buf, int nb, int autopoll)
@@ -582,6 +586,7 @@ adb_try_handler_change(int address, int new_id)
 	mutex_unlock(&adb_handler_mutex);
 	return ret;
 }
+EXPORT_SYMBOL(adb_try_handler_change);
 
 int
 adb_get_infos(int address, int *original_address, int *handler_id)
diff --git a/drivers/macintosh/via-cuda.c b/drivers/macintosh/via-cuda.c
index d61f271d2207..bad18130f125 100644
--- a/drivers/macintosh/via-cuda.c
+++ b/drivers/macintosh/via-cuda.c
@@ -379,6 +379,7 @@ cuda_request(struct adb_request *req, void (*done)(struct adb_request *),
     req->reply_expected = 1;
     return cuda_write(req);
 }
+EXPORT_SYMBOL(cuda_request);
 
 static int
 cuda_write(struct adb_request *req)
@@ -441,6 +442,7 @@ cuda_poll(void)
     if (cuda_irq)
 	enable_irq(cuda_irq);
 }
+EXPORT_SYMBOL(cuda_poll);
 
 static irqreturn_t
 cuda_interrupt(int irq, void *arg)
diff --git a/drivers/mailbox/Makefile b/drivers/mailbox/Makefile
index 6d184dbcaca8..94ed7cefb14d 100644
--- a/drivers/mailbox/Makefile
+++ b/drivers/mailbox/Makefile
@@ -1,3 +1,7 @@
+# Generic MAILBOX API
+
+obj-$(CONFIG_MAILBOX)		+= mailbox.o
+
 obj-$(CONFIG_PL320_MBOX)	+= pl320-ipc.o
 
 obj-$(CONFIG_OMAP2PLUS_MBOX)	+= omap-mailbox.o
diff --git a/drivers/mailbox/mailbox.c b/drivers/mailbox/mailbox.c
new file mode 100644
index 000000000000..afcb430508ec
--- /dev/null
+++ b/drivers/mailbox/mailbox.c
@@ -0,0 +1,465 @@
+/*
+ * Mailbox: Common code for Mailbox controllers and users
+ *
+ * Copyright (C) 2013-2014 Linaro Ltd.
+ * Author: Jassi Brar <jassisinghbrar@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/bitops.h>
+#include <linux/mailbox_client.h>
+#include <linux/mailbox_controller.h>
+
+#define TXDONE_BY_IRQ	BIT(0) /* controller has remote RTR irq */
+#define TXDONE_BY_POLL	BIT(1) /* controller can read status of last TX */
+#define TXDONE_BY_ACK	BIT(2) /* S/W ACK recevied by Client ticks the TX */
+
+static LIST_HEAD(mbox_cons);
+static DEFINE_MUTEX(con_mutex);
+
+static int add_to_rbuf(struct mbox_chan *chan, void *mssg)
+{
+	int idx;
+	unsigned long flags;
+
+	spin_lock_irqsave(&chan->lock, flags);
+
+	/* See if there is any space left */
+	if (chan->msg_count == MBOX_TX_QUEUE_LEN) {
+		spin_unlock_irqrestore(&chan->lock, flags);
+		return -ENOBUFS;
+	}
+
+	idx = chan->msg_free;
+	chan->msg_data[idx] = mssg;
+	chan->msg_count++;
+
+	if (idx == MBOX_TX_QUEUE_LEN - 1)
+		chan->msg_free = 0;
+	else
+		chan->msg_free++;
+
+	spin_unlock_irqrestore(&chan->lock, flags);
+
+	return idx;
+}
+
+static void msg_submit(struct mbox_chan *chan)
+{
+	unsigned count, idx;
+	unsigned long flags;
+	void *data;
+	int err;
+
+	spin_lock_irqsave(&chan->lock, flags);
+
+	if (!chan->msg_count || chan->active_req)
+		goto exit;
+
+	count = chan->msg_count;
+	idx = chan->msg_free;
+	if (idx >= count)
+		idx -= count;
+	else
+		idx += MBOX_TX_QUEUE_LEN - count;
+
+	data = chan->msg_data[idx];
+
+	/* Try to submit a message to the MBOX controller */
+	err = chan->mbox->ops->send_data(chan, data);
+	if (!err) {
+		chan->active_req = data;
+		chan->msg_count--;
+	}
+exit:
+	spin_unlock_irqrestore(&chan->lock, flags);
+}
+
+static void tx_tick(struct mbox_chan *chan, int r)
+{
+	unsigned long flags;
+	void *mssg;
+
+	spin_lock_irqsave(&chan->lock, flags);
+	mssg = chan->active_req;
+	chan->active_req = NULL;
+	spin_unlock_irqrestore(&chan->lock, flags);
+
+	/* Submit next message */
+	msg_submit(chan);
+
+	/* Notify the client */
+	if (mssg && chan->cl->tx_done)
+		chan->cl->tx_done(chan->cl, mssg, r);
+
+	if (chan->cl->tx_block)
+		complete(&chan->tx_complete);
+}
+
+static void poll_txdone(unsigned long data)
+{
+	struct mbox_controller *mbox = (struct mbox_controller *)data;
+	bool txdone, resched = false;
+	int i;
+
+	for (i = 0; i < mbox->num_chans; i++) {
+		struct mbox_chan *chan = &mbox->chans[i];
+
+		if (chan->active_req && chan->cl) {
+			resched = true;
+			txdone = chan->mbox->ops->last_tx_done(chan);
+			if (txdone)
+				tx_tick(chan, 0);
+		}
+	}
+
+	if (resched)
+		mod_timer(&mbox->poll, jiffies +
+				msecs_to_jiffies(mbox->txpoll_period));
+}
+
+/**
+ * mbox_chan_received_data - A way for controller driver to push data
+ *				received from remote to the upper layer.
+ * @chan: Pointer to the mailbox channel on which RX happened.
+ * @mssg: Client specific message typecasted as void *
+ *
+ * After startup and before shutdown any data received on the chan
+ * is passed on to the API via atomic mbox_chan_received_data().
+ * The controller should ACK the RX only after this call returns.
+ */
+void mbox_chan_received_data(struct mbox_chan *chan, void *mssg)
+{
+	/* No buffering the received data */
+	if (chan->cl->rx_callback)
+		chan->cl->rx_callback(chan->cl, mssg);
+}
+EXPORT_SYMBOL_GPL(mbox_chan_received_data);
+
+/**
+ * mbox_chan_txdone - A way for controller driver to notify the
+ *			framework that the last TX has completed.
+ * @chan: Pointer to the mailbox chan on which TX happened.
+ * @r: Status of last TX - OK or ERROR
+ *
+ * The controller that has IRQ for TX ACK calls this atomic API
+ * to tick the TX state machine. It works only if txdone_irq
+ * is set by the controller.
+ */
+void mbox_chan_txdone(struct mbox_chan *chan, int r)
+{
+	if (unlikely(!(chan->txdone_method & TXDONE_BY_IRQ))) {
+		dev_err(chan->mbox->dev,
+		       "Controller can't run the TX ticker\n");
+		return;
+	}
+
+	tx_tick(chan, r);
+}
+EXPORT_SYMBOL_GPL(mbox_chan_txdone);
+
+/**
+ * mbox_client_txdone - The way for a client to run the TX state machine.
+ * @chan: Mailbox channel assigned to this client.
+ * @r: Success status of last transmission.
+ *
+ * The client/protocol had received some 'ACK' packet and it notifies
+ * the API that the last packet was sent successfully. This only works
+ * if the controller can't sense TX-Done.
+ */
+void mbox_client_txdone(struct mbox_chan *chan, int r)
+{
+	if (unlikely(!(chan->txdone_method & TXDONE_BY_ACK))) {
+		dev_err(chan->mbox->dev, "Client can't run the TX ticker\n");
+		return;
+	}
+
+	tx_tick(chan, r);
+}
+EXPORT_SYMBOL_GPL(mbox_client_txdone);
+
+/**
+ * mbox_client_peek_data - A way for client driver to pull data
+ *			received from remote by the controller.
+ * @chan: Mailbox channel assigned to this client.
+ *
+ * A poke to controller driver for any received data.
+ * The data is actually passed onto client via the
+ * mbox_chan_received_data()
+ * The call can be made from atomic context, so the controller's
+ * implementation of peek_data() must not sleep.
+ *
+ * Return: True, if controller has, and is going to push after this,
+ *          some data.
+ *         False, if controller doesn't have any data to be read.
+ */
+bool mbox_client_peek_data(struct mbox_chan *chan)
+{
+	if (chan->mbox->ops->peek_data)
+		return chan->mbox->ops->peek_data(chan);
+
+	return false;
+}
+EXPORT_SYMBOL_GPL(mbox_client_peek_data);
+
+/**
+ * mbox_send_message -	For client to submit a message to be
+ *				sent to the remote.
+ * @chan: Mailbox channel assigned to this client.
+ * @mssg: Client specific message typecasted.
+ *
+ * For client to submit data to the controller destined for a remote
+ * processor. If the client had set 'tx_block', the call will return
+ * either when the remote receives the data or when 'tx_tout' millisecs
+ * run out.
+ *  In non-blocking mode, the requests are buffered by the API and a
+ * non-negative token is returned for each queued request. If the request
+ * is not queued, a negative token is returned. Upon failure or successful
+ * TX, the API calls 'tx_done' from atomic context, from which the client
+ * could submit yet another request.
+ * The pointer to message should be preserved until it is sent
+ * over the chan, i.e, tx_done() is made.
+ * This function could be called from atomic context as it simply
+ * queues the data and returns a token against the request.
+ *
+ * Return: Non-negative integer for successful submission (non-blocking mode)
+ *	or transmission over chan (blocking mode).
+ *	Negative value denotes failure.
+ */
+int mbox_send_message(struct mbox_chan *chan, void *mssg)
+{
+	int t;
+
+	if (!chan || !chan->cl)
+		return -EINVAL;
+
+	t = add_to_rbuf(chan, mssg);
+	if (t < 0) {
+		dev_err(chan->mbox->dev, "Try increasing MBOX_TX_QUEUE_LEN\n");
+		return t;
+	}
+
+	msg_submit(chan);
+
+	if (chan->txdone_method	== TXDONE_BY_POLL)
+		poll_txdone((unsigned long)chan->mbox);
+
+	if (chan->cl->tx_block && chan->active_req) {
+		unsigned long wait;
+		int ret;
+
+		if (!chan->cl->tx_tout) /* wait forever */
+			wait = msecs_to_jiffies(3600000);
+		else
+			wait = msecs_to_jiffies(chan->cl->tx_tout);
+
+		ret = wait_for_completion_timeout(&chan->tx_complete, wait);
+		if (ret == 0) {
+			t = -EIO;
+			tx_tick(chan, -EIO);
+		}
+	}
+
+	return t;
+}
+EXPORT_SYMBOL_GPL(mbox_send_message);
+
+/**
+ * mbox_request_channel - Request a mailbox channel.
+ * @cl: Identity of the client requesting the channel.
+ * @index: Index of mailbox specifier in 'mboxes' property.
+ *
+ * The Client specifies its requirements and capabilities while asking for
+ * a mailbox channel. It can't be called from atomic context.
+ * The channel is exclusively allocated and can't be used by another
+ * client before the owner calls mbox_free_channel.
+ * After assignment, any packet received on this channel will be
+ * handed over to the client via the 'rx_callback'.
+ * The framework holds reference to the client, so the mbox_client
+ * structure shouldn't be modified until the mbox_free_channel returns.
+ *
+ * Return: Pointer to the channel assigned to the client if successful.
+ *		ERR_PTR for request failure.
+ */
+struct mbox_chan *mbox_request_channel(struct mbox_client *cl, int index)
+{
+	struct device *dev = cl->dev;
+	struct mbox_controller *mbox;
+	struct of_phandle_args spec;
+	struct mbox_chan *chan;
+	unsigned long flags;
+	int ret;
+
+	if (!dev || !dev->of_node) {
+		pr_debug("%s: No owner device node\n", __func__);
+		return ERR_PTR(-ENODEV);
+	}
+
+	mutex_lock(&con_mutex);
+
+	if (of_parse_phandle_with_args(dev->of_node, "mboxes",
+				       "#mbox-cells", index, &spec)) {
+		dev_dbg(dev, "%s: can't parse \"mboxes\" property\n", __func__);
+		mutex_unlock(&con_mutex);
+		return ERR_PTR(-ENODEV);
+	}
+
+	chan = NULL;
+	list_for_each_entry(mbox, &mbox_cons, node)
+		if (mbox->dev->of_node == spec.np) {
+			chan = mbox->of_xlate(mbox, &spec);
+			break;
+		}
+
+	of_node_put(spec.np);
+
+	if (!chan || chan->cl || !try_module_get(mbox->dev->driver->owner)) {
+		dev_dbg(dev, "%s: mailbox not free\n", __func__);
+		mutex_unlock(&con_mutex);
+		return ERR_PTR(-EBUSY);
+	}
+
+	spin_lock_irqsave(&chan->lock, flags);
+	chan->msg_free = 0;
+	chan->msg_count = 0;
+	chan->active_req = NULL;
+	chan->cl = cl;
+	init_completion(&chan->tx_complete);
+
+	if (chan->txdone_method	== TXDONE_BY_POLL && cl->knows_txdone)
+		chan->txdone_method |= TXDONE_BY_ACK;
+
+	spin_unlock_irqrestore(&chan->lock, flags);
+
+	ret = chan->mbox->ops->startup(chan);
+	if (ret) {
+		dev_err(dev, "Unable to startup the chan (%d)\n", ret);
+		mbox_free_channel(chan);
+		chan = ERR_PTR(ret);
+	}
+
+	mutex_unlock(&con_mutex);
+	return chan;
+}
+EXPORT_SYMBOL_GPL(mbox_request_channel);
+
+/**
+ * mbox_free_channel - The client relinquishes control of a mailbox
+ *			channel by this call.
+ * @chan: The mailbox channel to be freed.
+ */
+void mbox_free_channel(struct mbox_chan *chan)
+{
+	unsigned long flags;
+
+	if (!chan || !chan->cl)
+		return;
+
+	chan->mbox->ops->shutdown(chan);
+
+	/* The queued TX requests are simply aborted, no callbacks are made */
+	spin_lock_irqsave(&chan->lock, flags);
+	chan->cl = NULL;
+	chan->active_req = NULL;
+	if (chan->txdone_method == (TXDONE_BY_POLL | TXDONE_BY_ACK))
+		chan->txdone_method = TXDONE_BY_POLL;
+
+	module_put(chan->mbox->dev->driver->owner);
+	spin_unlock_irqrestore(&chan->lock, flags);
+}
+EXPORT_SYMBOL_GPL(mbox_free_channel);
+
+static struct mbox_chan *
+of_mbox_index_xlate(struct mbox_controller *mbox,
+		    const struct of_phandle_args *sp)
+{
+	int ind = sp->args[0];
+
+	if (ind >= mbox->num_chans)
+		return NULL;
+
+	return &mbox->chans[ind];
+}
+
+/**
+ * mbox_controller_register - Register the mailbox controller
+ * @mbox:	Pointer to the mailbox controller.
+ *
+ * The controller driver registers its communication channels
+ */
+int mbox_controller_register(struct mbox_controller *mbox)
+{
+	int i, txdone;
+
+	/* Sanity check */
+	if (!mbox || !mbox->dev || !mbox->ops || !mbox->num_chans)
+		return -EINVAL;
+
+	if (mbox->txdone_irq)
+		txdone = TXDONE_BY_IRQ;
+	else if (mbox->txdone_poll)
+		txdone = TXDONE_BY_POLL;
+	else /* It has to be ACK then */
+		txdone = TXDONE_BY_ACK;
+
+	if (txdone == TXDONE_BY_POLL) {
+		mbox->poll.function = &poll_txdone;
+		mbox->poll.data = (unsigned long)mbox;
+		init_timer(&mbox->poll);
+	}
+
+	for (i = 0; i < mbox->num_chans; i++) {
+		struct mbox_chan *chan = &mbox->chans[i];
+
+		chan->cl = NULL;
+		chan->mbox = mbox;
+		chan->txdone_method = txdone;
+		spin_lock_init(&chan->lock);
+	}
+
+	if (!mbox->of_xlate)
+		mbox->of_xlate = of_mbox_index_xlate;
+
+	mutex_lock(&con_mutex);
+	list_add_tail(&mbox->node, &mbox_cons);
+	mutex_unlock(&con_mutex);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(mbox_controller_register);
+
+/**
+ * mbox_controller_unregister - Unregister the mailbox controller
+ * @mbox:	Pointer to the mailbox controller.
+ */
+void mbox_controller_unregister(struct mbox_controller *mbox)
+{
+	int i;
+
+	if (!mbox)
+		return;
+
+	mutex_lock(&con_mutex);
+
+	list_del(&mbox->node);
+
+	for (i = 0; i < mbox->num_chans; i++)
+		mbox_free_channel(&mbox->chans[i]);
+
+	if (mbox->txdone_poll)
+		del_timer_sync(&mbox->poll);
+
+	mutex_unlock(&con_mutex);
+}
+EXPORT_SYMBOL_GPL(mbox_controller_unregister);
diff --git a/drivers/mailbox/omap-mailbox.c b/drivers/mailbox/omap-mailbox.c
index a27e00e63a8a..bcc7ee129276 100644
--- a/drivers/mailbox/omap-mailbox.c
+++ b/drivers/mailbox/omap-mailbox.c
@@ -31,6 +31,7 @@
 #include <linux/err.h>
 #include <linux/notifier.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/platform_data/mailbox-omap.h>
@@ -94,6 +95,18 @@ struct omap_mbox_device {
 	struct list_head elem;
 };
 
+struct omap_mbox_fifo_info {
+	int tx_id;
+	int tx_usr;
+	int tx_irq;
+
+	int rx_id;
+	int rx_usr;
+	int rx_irq;
+
+	const char *name;
+};
+
 struct omap_mbox {
 	const char		*name;
 	int			irq;
@@ -587,24 +600,118 @@ static int omap_mbox_unregister(struct omap_mbox_device *mdev)
 	return 0;
 }
 
+static const struct of_device_id omap_mailbox_of_match[] = {
+	{
+		.compatible	= "ti,omap2-mailbox",
+		.data		= (void *)MBOX_INTR_CFG_TYPE1,
+	},
+	{
+		.compatible	= "ti,omap3-mailbox",
+		.data		= (void *)MBOX_INTR_CFG_TYPE1,
+	},
+	{
+		.compatible	= "ti,omap4-mailbox",
+		.data		= (void *)MBOX_INTR_CFG_TYPE2,
+	},
+	{
+		/* end */
+	},
+};
+MODULE_DEVICE_TABLE(of, omap_mailbox_of_match);
+
 static int omap_mbox_probe(struct platform_device *pdev)
 {
 	struct resource *mem;
 	int ret;
 	struct omap_mbox **list, *mbox, *mboxblk;
 	struct omap_mbox_pdata *pdata = pdev->dev.platform_data;
-	struct omap_mbox_dev_info *info;
+	struct omap_mbox_dev_info *info = NULL;
+	struct omap_mbox_fifo_info *finfo, *finfoblk;
 	struct omap_mbox_device *mdev;
 	struct omap_mbox_fifo *fifo;
-	u32 intr_type;
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *child;
+	const struct of_device_id *match;
+	u32 intr_type, info_count;
+	u32 num_users, num_fifos;
+	u32 tmp[3];
 	u32 l;
 	int i;
 
-	if (!pdata || !pdata->info_cnt || !pdata->info) {
+	if (!node && (!pdata || !pdata->info_cnt || !pdata->info)) {
 		pr_err("%s: platform not supported\n", __func__);
 		return -ENODEV;
 	}
 
+	if (node) {
+		match = of_match_device(omap_mailbox_of_match, &pdev->dev);
+		if (!match)
+			return -ENODEV;
+		intr_type = (u32)match->data;
+
+		if (of_property_read_u32(node, "ti,mbox-num-users",
+					 &num_users))
+			return -ENODEV;
+
+		if (of_property_read_u32(node, "ti,mbox-num-fifos",
+					 &num_fifos))
+			return -ENODEV;
+
+		info_count = of_get_available_child_count(node);
+		if (!info_count) {
+			dev_err(&pdev->dev, "no available mbox devices found\n");
+			return -ENODEV;
+		}
+	} else { /* non-DT device creation */
+		info_count = pdata->info_cnt;
+		info = pdata->info;
+		intr_type = pdata->intr_type;
+		num_users = pdata->num_users;
+		num_fifos = pdata->num_fifos;
+	}
+
+	finfoblk = devm_kzalloc(&pdev->dev, info_count * sizeof(*finfoblk),
+				GFP_KERNEL);
+	if (!finfoblk)
+		return -ENOMEM;
+
+	finfo = finfoblk;
+	child = NULL;
+	for (i = 0; i < info_count; i++, finfo++) {
+		if (node) {
+			child = of_get_next_available_child(node, child);
+			ret = of_property_read_u32_array(child, "ti,mbox-tx",
+							 tmp, ARRAY_SIZE(tmp));
+			if (ret)
+				return ret;
+			finfo->tx_id = tmp[0];
+			finfo->tx_irq = tmp[1];
+			finfo->tx_usr = tmp[2];
+
+			ret = of_property_read_u32_array(child, "ti,mbox-rx",
+							 tmp, ARRAY_SIZE(tmp));
+			if (ret)
+				return ret;
+			finfo->rx_id = tmp[0];
+			finfo->rx_irq = tmp[1];
+			finfo->rx_usr = tmp[2];
+
+			finfo->name = child->name;
+		} else {
+			finfo->tx_id = info->tx_id;
+			finfo->rx_id = info->rx_id;
+			finfo->tx_usr = info->usr_id;
+			finfo->tx_irq = info->irq_id;
+			finfo->rx_usr = info->usr_id;
+			finfo->rx_irq = info->irq_id;
+			finfo->name = info->name;
+			info++;
+		}
+		if (finfo->tx_id >= num_fifos || finfo->rx_id >= num_fifos ||
+		    finfo->tx_usr >= num_users || finfo->rx_usr >= num_users)
+			return -EINVAL;
+	}
+
 	mdev = devm_kzalloc(&pdev->dev, sizeof(*mdev), GFP_KERNEL);
 	if (!mdev)
 		return -ENOMEM;
@@ -615,41 +722,40 @@ static int omap_mbox_probe(struct platform_device *pdev)
 		return PTR_ERR(mdev->mbox_base);
 
 	/* allocate one extra for marking end of list */
-	list = devm_kzalloc(&pdev->dev, (pdata->info_cnt + 1) * sizeof(*list),
+	list = devm_kzalloc(&pdev->dev, (info_count + 1) * sizeof(*list),
 			    GFP_KERNEL);
 	if (!list)
 		return -ENOMEM;
 
-	mboxblk = devm_kzalloc(&pdev->dev, pdata->info_cnt * sizeof(*mbox),
+	mboxblk = devm_kzalloc(&pdev->dev, info_count * sizeof(*mbox),
 			       GFP_KERNEL);
 	if (!mboxblk)
 		return -ENOMEM;
 
-	info = pdata->info;
-	intr_type = pdata->intr_type;
 	mbox = mboxblk;
-	for (i = 0; i < pdata->info_cnt; i++, info++) {
+	finfo = finfoblk;
+	for (i = 0; i < info_count; i++, finfo++) {
 		fifo = &mbox->tx_fifo;
-		fifo->msg = MAILBOX_MESSAGE(info->tx_id);
-		fifo->fifo_stat = MAILBOX_FIFOSTATUS(info->tx_id);
-		fifo->intr_bit = MAILBOX_IRQ_NOTFULL(info->tx_id);
-		fifo->irqenable = MAILBOX_IRQENABLE(intr_type, info->usr_id);
-		fifo->irqstatus = MAILBOX_IRQSTATUS(intr_type, info->usr_id);
-		fifo->irqdisable = MAILBOX_IRQDISABLE(intr_type, info->usr_id);
+		fifo->msg = MAILBOX_MESSAGE(finfo->tx_id);
+		fifo->fifo_stat = MAILBOX_FIFOSTATUS(finfo->tx_id);
+		fifo->intr_bit = MAILBOX_IRQ_NOTFULL(finfo->tx_id);
+		fifo->irqenable = MAILBOX_IRQENABLE(intr_type, finfo->tx_usr);
+		fifo->irqstatus = MAILBOX_IRQSTATUS(intr_type, finfo->tx_usr);
+		fifo->irqdisable = MAILBOX_IRQDISABLE(intr_type, finfo->tx_usr);
 
 		fifo = &mbox->rx_fifo;
-		fifo->msg =  MAILBOX_MESSAGE(info->rx_id);
-		fifo->msg_stat =  MAILBOX_MSGSTATUS(info->rx_id);
-		fifo->intr_bit = MAILBOX_IRQ_NEWMSG(info->rx_id);
-		fifo->irqenable = MAILBOX_IRQENABLE(intr_type, info->usr_id);
-		fifo->irqstatus = MAILBOX_IRQSTATUS(intr_type, info->usr_id);
-		fifo->irqdisable = MAILBOX_IRQDISABLE(intr_type, info->usr_id);
+		fifo->msg = MAILBOX_MESSAGE(finfo->rx_id);
+		fifo->msg_stat =  MAILBOX_MSGSTATUS(finfo->rx_id);
+		fifo->intr_bit = MAILBOX_IRQ_NEWMSG(finfo->rx_id);
+		fifo->irqenable = MAILBOX_IRQENABLE(intr_type, finfo->rx_usr);
+		fifo->irqstatus = MAILBOX_IRQSTATUS(intr_type, finfo->rx_usr);
+		fifo->irqdisable = MAILBOX_IRQDISABLE(intr_type, finfo->rx_usr);
 
 		mbox->intr_type = intr_type;
 
 		mbox->parent = mdev;
-		mbox->name = info->name;
-		mbox->irq = platform_get_irq(pdev, info->irq_id);
+		mbox->name = finfo->name;
+		mbox->irq = platform_get_irq(pdev, finfo->tx_irq);
 		if (mbox->irq < 0)
 			return mbox->irq;
 		list[i] = mbox++;
@@ -657,8 +763,8 @@ static int omap_mbox_probe(struct platform_device *pdev)
 
 	mutex_init(&mdev->cfg_lock);
 	mdev->dev = &pdev->dev;
-	mdev->num_users = pdata->num_users;
-	mdev->num_fifos = pdata->num_fifos;
+	mdev->num_users = num_users;
+	mdev->num_fifos = num_fifos;
 	mdev->mboxes = list;
 	ret = omap_mbox_register(mdev);
 	if (ret)
@@ -684,6 +790,7 @@ static int omap_mbox_probe(struct platform_device *pdev)
 	if (ret < 0)
 		goto unregister;
 
+	devm_kfree(&pdev->dev, finfoblk);
 	return 0;
 
 unregister:
@@ -708,6 +815,7 @@ static struct platform_driver omap_mbox_driver = {
 	.driver	= {
 		.name = "omap-mailbox",
 		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(omap_mailbox_of_match),
 	},
 };
 
diff --git a/drivers/mailbox/pl320-ipc.c b/drivers/mailbox/pl320-ipc.c
index d873cbae2fbb..f3755e0aa935 100644
--- a/drivers/mailbox/pl320-ipc.c
+++ b/drivers/mailbox/pl320-ipc.c
@@ -26,7 +26,7 @@
 #include <linux/device.h>
 #include <linux/amba/bus.h>
 
-#include <linux/mailbox.h>
+#include <linux/pl320-ipc.h>
 
 #define IPCMxSOURCE(m)		((m) * 0x40)
 #define IPCMxDSET(m)		(((m) * 0x40) + 0x004)
diff --git a/drivers/md/bcache/super.c b/drivers/md/bcache/super.c
index d4713d098a39..4dd2bb7167f0 100644
--- a/drivers/md/bcache/super.c
+++ b/drivers/md/bcache/super.c
@@ -842,6 +842,7 @@ static int bcache_device_init(struct bcache_device *d, unsigned block_size,
 	q->limits.logical_block_size	= block_size;
 	q->limits.physical_block_size	= block_size;
 	set_bit(QUEUE_FLAG_NONROT,	&d->disk->queue->queue_flags);
+	clear_bit(QUEUE_FLAG_ADD_RANDOM, &d->disk->queue->queue_flags);
 	set_bit(QUEUE_FLAG_DISCARD,	&d->disk->queue->queue_flags);
 
 	blk_queue_flush(q, REQ_FLUSH|REQ_FUA);
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 67f8b31e2054..da3604e73e8a 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -879,7 +879,6 @@ void bitmap_unplug(struct bitmap *bitmap)
 {
 	unsigned long i;
 	int dirty, need_write;
-	int wait = 0;
 
 	if (!bitmap || !bitmap->storage.filemap ||
 	    test_bit(BITMAP_STALE, &bitmap->flags))
@@ -897,16 +896,13 @@ void bitmap_unplug(struct bitmap *bitmap)
 			clear_page_attr(bitmap, i, BITMAP_PAGE_PENDING);
 			write_page(bitmap, bitmap->storage.filemap[i], 0);
 		}
-		if (dirty)
-			wait = 1;
-	}
-	if (wait) { /* if any writes were performed, we need to wait on them */
-		if (bitmap->storage.file)
-			wait_event(bitmap->write_wait,
-				   atomic_read(&bitmap->pending_writes)==0);
-		else
-			md_super_wait(bitmap->mddev);
 	}
+	if (bitmap->storage.file)
+		wait_event(bitmap->write_wait,
+			   atomic_read(&bitmap->pending_writes)==0);
+	else
+		md_super_wait(bitmap->mddev);
+
 	if (test_bit(BITMAP_WRITE_ERROR, &bitmap->flags))
 		bitmap_file_kick(bitmap);
 }
diff --git a/drivers/md/dm-bufio.c b/drivers/md/dm-bufio.c
index ab472c557d18..825ca1f87639 100644
--- a/drivers/md/dm-bufio.c
+++ b/drivers/md/dm-bufio.c
@@ -465,6 +465,7 @@ static void __relink_lru(struct dm_buffer *b, int dirty)
 	c->n_buffers[dirty]++;
 	b->list_mode = dirty;
 	list_move(&b->lru_list, &c->lru[dirty]);
+	b->last_accessed = jiffies;
 }
 
 /*----------------------------------------------------------------
@@ -720,7 +721,6 @@ static void __wait_for_free_buffer(struct dm_bufio_client *c)
 
 	io_schedule();
 
-	set_task_state(current, TASK_RUNNING);
 	remove_wait_queue(&c->free_buffer_wait, &wait);
 
 	dm_bufio_lock(c);
@@ -1472,9 +1472,9 @@ static long __scan(struct dm_bufio_client *c, unsigned long nr_to_scan,
 		list_for_each_entry_safe_reverse(b, tmp, &c->lru[l], lru_list) {
 			freed += __cleanup_old_buffer(b, gfp_mask, 0);
 			if (!--nr_to_scan)
-				break;
+				return freed;
+			dm_bufio_cond_resched();
 		}
-		dm_bufio_cond_resched();
 	}
 	return freed;
 }
diff --git a/drivers/md/dm-cache-target.c b/drivers/md/dm-cache-target.c
index 1af40ee209e2..7130505c2425 100644
--- a/drivers/md/dm-cache-target.c
+++ b/drivers/md/dm-cache-target.c
@@ -895,8 +895,8 @@ static void migration_success_pre_commit(struct dm_cache_migration *mg)
 	struct cache *cache = mg->cache;
 
 	if (mg->writeback) {
-		cell_defer(cache, mg->old_ocell, false);
 		clear_dirty(cache, mg->old_oblock, mg->cblock);
+		cell_defer(cache, mg->old_ocell, false);
 		cleanup_migration(mg);
 		return;
 
@@ -951,13 +951,13 @@ static void migration_success_post_commit(struct dm_cache_migration *mg)
 		}
 
 	} else {
+		clear_dirty(cache, mg->new_oblock, mg->cblock);
 		if (mg->requeue_holder)
 			cell_defer(cache, mg->new_ocell, true);
 		else {
 			bio_endio(mg->new_ocell->holder, 0);
 			cell_defer(cache, mg->new_ocell, false);
 		}
-		clear_dirty(cache, mg->new_oblock, mg->cblock);
 		cleanup_migration(mg);
 	}
 }
diff --git a/drivers/md/dm-crypt.c b/drivers/md/dm-crypt.c
index 2785007e0e46..fc93b9330af4 100644
--- a/drivers/md/dm-crypt.c
+++ b/drivers/md/dm-crypt.c
@@ -526,29 +526,26 @@ static int crypt_iv_lmk_one(struct crypt_config *cc, u8 *iv,
 			    u8 *data)
 {
 	struct iv_lmk_private *lmk = &cc->iv_gen_private.lmk;
-	struct {
-		struct shash_desc desc;
-		char ctx[crypto_shash_descsize(lmk->hash_tfm)];
-	} sdesc;
+	SHASH_DESC_ON_STACK(desc, lmk->hash_tfm);
 	struct md5_state md5state;
 	__le32 buf[4];
 	int i, r;
 
-	sdesc.desc.tfm = lmk->hash_tfm;
-	sdesc.desc.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+	desc->tfm = lmk->hash_tfm;
+	desc->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
 
-	r = crypto_shash_init(&sdesc.desc);
+	r = crypto_shash_init(desc);
 	if (r)
 		return r;
 
 	if (lmk->seed) {
-		r = crypto_shash_update(&sdesc.desc, lmk->seed, LMK_SEED_SIZE);
+		r = crypto_shash_update(desc, lmk->seed, LMK_SEED_SIZE);
 		if (r)
 			return r;
 	}
 
 	/* Sector is always 512B, block size 16, add data of blocks 1-31 */
-	r = crypto_shash_update(&sdesc.desc, data + 16, 16 * 31);
+	r = crypto_shash_update(desc, data + 16, 16 * 31);
 	if (r)
 		return r;
 
@@ -557,12 +554,12 @@ static int crypt_iv_lmk_one(struct crypt_config *cc, u8 *iv,
 	buf[1] = cpu_to_le32((((u64)dmreq->iv_sector >> 32) & 0x00FFFFFF) | 0x80000000);
 	buf[2] = cpu_to_le32(4024);
 	buf[3] = 0;
-	r = crypto_shash_update(&sdesc.desc, (u8 *)buf, sizeof(buf));
+	r = crypto_shash_update(desc, (u8 *)buf, sizeof(buf));
 	if (r)
 		return r;
 
 	/* No MD5 padding here */
-	r = crypto_shash_export(&sdesc.desc, &md5state);
+	r = crypto_shash_export(desc, &md5state);
 	if (r)
 		return r;
 
@@ -679,10 +676,7 @@ static int crypt_iv_tcw_whitening(struct crypt_config *cc,
 	struct iv_tcw_private *tcw = &cc->iv_gen_private.tcw;
 	u64 sector = cpu_to_le64((u64)dmreq->iv_sector);
 	u8 buf[TCW_WHITENING_SIZE];
-	struct {
-		struct shash_desc desc;
-		char ctx[crypto_shash_descsize(tcw->crc32_tfm)];
-	} sdesc;
+	SHASH_DESC_ON_STACK(desc, tcw->crc32_tfm);
 	int i, r;
 
 	/* xor whitening with sector number */
@@ -691,16 +685,16 @@ static int crypt_iv_tcw_whitening(struct crypt_config *cc,
 	crypto_xor(&buf[8], (u8 *)&sector, 8);
 
 	/* calculate crc32 for every 32bit part and xor it */
-	sdesc.desc.tfm = tcw->crc32_tfm;
-	sdesc.desc.flags = CRYPTO_TFM_REQ_MAY_SLEEP;
+	desc->tfm = tcw->crc32_tfm;
+	desc->flags = CRYPTO_TFM_REQ_MAY_SLEEP;
 	for (i = 0; i < 4; i++) {
-		r = crypto_shash_init(&sdesc.desc);
+		r = crypto_shash_init(desc);
 		if (r)
 			goto out;
-		r = crypto_shash_update(&sdesc.desc, &buf[i * 4], 4);
+		r = crypto_shash_update(desc, &buf[i * 4], 4);
 		if (r)
 			goto out;
-		r = crypto_shash_final(&sdesc.desc, &buf[i * 4]);
+		r = crypto_shash_final(desc, &buf[i * 4]);
 		if (r)
 			goto out;
 	}
@@ -1688,6 +1682,7 @@ static int crypt_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 	unsigned int key_size, opt_params;
 	unsigned long long tmpll;
 	int ret;
+	size_t iv_size_padding;
 	struct dm_arg_set as;
 	const char *opt_string;
 	char dummy;
@@ -1724,20 +1719,32 @@ static int crypt_ctr(struct dm_target *ti, unsigned int argc, char **argv)
 
 	cc->dmreq_start = sizeof(struct ablkcipher_request);
 	cc->dmreq_start += crypto_ablkcipher_reqsize(any_tfm(cc));
-	cc->dmreq_start = ALIGN(cc->dmreq_start, crypto_tfm_ctx_alignment());
-	cc->dmreq_start += crypto_ablkcipher_alignmask(any_tfm(cc)) &
-			   ~(crypto_tfm_ctx_alignment() - 1);
+	cc->dmreq_start = ALIGN(cc->dmreq_start, __alignof__(struct dm_crypt_request));
+
+	if (crypto_ablkcipher_alignmask(any_tfm(cc)) < CRYPTO_MINALIGN) {
+		/* Allocate the padding exactly */
+		iv_size_padding = -(cc->dmreq_start + sizeof(struct dm_crypt_request))
+				& crypto_ablkcipher_alignmask(any_tfm(cc));
+	} else {
+		/*
+		 * If the cipher requires greater alignment than kmalloc
+		 * alignment, we don't know the exact position of the
+		 * initialization vector. We must assume worst case.
+		 */
+		iv_size_padding = crypto_ablkcipher_alignmask(any_tfm(cc));
+	}
 
 	cc->req_pool = mempool_create_kmalloc_pool(MIN_IOS, cc->dmreq_start +
-			sizeof(struct dm_crypt_request) + cc->iv_size);
+			sizeof(struct dm_crypt_request) + iv_size_padding + cc->iv_size);
 	if (!cc->req_pool) {
 		ti->error = "Cannot allocate crypt request mempool";
 		goto bad;
 	}
 
 	cc->per_bio_data_size = ti->per_bio_data_size =
-				sizeof(struct dm_crypt_io) + cc->dmreq_start +
-				sizeof(struct dm_crypt_request) + cc->iv_size;
+		ALIGN(sizeof(struct dm_crypt_io) + cc->dmreq_start +
+		      sizeof(struct dm_crypt_request) + iv_size_padding + cc->iv_size,
+		      ARCH_KMALLOC_MINALIGN);
 
 	cc->page_pool = mempool_create_page_pool(MIN_POOL_PAGES, 0);
 	if (!cc->page_pool) {
diff --git a/drivers/md/dm-ioctl.c b/drivers/md/dm-ioctl.c
index 51521429fb59..0be9381365d7 100644
--- a/drivers/md/dm-ioctl.c
+++ b/drivers/md/dm-ioctl.c
@@ -1418,7 +1418,7 @@ static void retrieve_deps(struct dm_table *table,
 	deps->count = count;
 	count = 0;
 	list_for_each_entry (dd, dm_table_get_devices(table), list)
-		deps->dev[count++] = huge_encode_dev(dd->dm_dev.bdev->bd_dev);
+		deps->dev[count++] = huge_encode_dev(dd->dm_dev->bdev->bd_dev);
 
 	param->data_size = param->data_start + needed;
 }
diff --git a/drivers/md/dm-log-userspace-transfer.c b/drivers/md/dm-log-userspace-transfer.c
index b428c0ae63d5..39ad9664d397 100644
--- a/drivers/md/dm-log-userspace-transfer.c
+++ b/drivers/md/dm-log-userspace-transfer.c
@@ -272,7 +272,7 @@ int dm_ulog_tfr_init(void)
 
 	r = cn_add_callback(&ulog_cn_id, "dmlogusr", cn_ulog_callback);
 	if (r) {
-		cn_del_callback(&ulog_cn_id);
+		kfree(prealloced_cn_msg);
 		return r;
 	}
 
diff --git a/drivers/md/dm-mpath.c b/drivers/md/dm-mpath.c
index 833d7e752f06..7b6b0f0f831a 100644
--- a/drivers/md/dm-mpath.c
+++ b/drivers/md/dm-mpath.c
@@ -317,8 +317,10 @@ static void __choose_pgpath(struct multipath *m, size_t nr_bytes)
 	struct priority_group *pg;
 	unsigned bypassed = 1;
 
-	if (!m->nr_valid_paths)
+	if (!m->nr_valid_paths) {
+		m->queue_io = 0;
 		goto failed;
+	}
 
 	/* Were we instructed to switch PG? */
 	if (m->next_pg) {
diff --git a/drivers/md/dm-raid.c b/drivers/md/dm-raid.c
index 4880b69e2e9e..4857fa4a5484 100644
--- a/drivers/md/dm-raid.c
+++ b/drivers/md/dm-raid.c
@@ -1,6 +1,6 @@
 /*
  * Copyright (C) 2010-2011 Neil Brown
- * Copyright (C) 2010-2011 Red Hat, Inc. All rights reserved.
+ * Copyright (C) 2010-2014 Red Hat, Inc. All rights reserved.
  *
  * This file is released under the GPL.
  */
@@ -18,6 +18,8 @@
 
 #define DM_MSG_PREFIX "raid"
 
+static bool devices_handle_discard_safely = false;
+
 /*
  * The following flags are used by dm-raid.c to set up the array state.
  * They must be cleared before md_run is called.
@@ -475,6 +477,8 @@ too_many:
  *                                      will form the "stripe"
  *    [[no]sync]			Force or prevent recovery of the
  *                                      entire array
+ *    [devices_handle_discard_safely]	Allow discards on RAID4/5/6; useful if RAID
+ *					member device(s) properly support TRIM/UNMAP
  *    [rebuild <idx>]			Rebuild the drive indicated by the index
  *    [daemon_sleep <ms>]		Time between bitmap daemon work to
  *                                      clear bits
@@ -1150,6 +1154,49 @@ static int analyse_superblocks(struct dm_target *ti, struct raid_set *rs)
 }
 
 /*
+ * Enable/disable discard support on RAID set depending on
+ * RAID level and discard properties of underlying RAID members.
+ */
+static void configure_discard_support(struct dm_target *ti, struct raid_set *rs)
+{
+	int i;
+	bool raid456;
+
+	/* Assume discards not supported until after checks below. */
+	ti->discards_supported = false;
+
+	/* RAID level 4,5,6 require discard_zeroes_data for data integrity! */
+	raid456 = (rs->md.level == 4 || rs->md.level == 5 || rs->md.level == 6);
+
+	for (i = 0; i < rs->md.raid_disks; i++) {
+		struct request_queue *q = bdev_get_queue(rs->dev[i].rdev.bdev);
+
+		if (!q || !blk_queue_discard(q))
+			return;
+
+		if (raid456) {
+			if (!q->limits.discard_zeroes_data)
+				return;
+			if (!devices_handle_discard_safely) {
+				DMERR("raid456 discard support disabled due to discard_zeroes_data uncertainty.");
+				DMERR("Set dm-raid.devices_handle_discard_safely=Y to override.");
+				return;
+			}
+		}
+	}
+
+	/* All RAID members properly support discards */
+	ti->discards_supported = true;
+
+	/*
+	 * RAID1 and RAID10 personalities require bio splitting,
+	 * RAID0/4/5/6 don't and process large discard bios properly.
+	 */
+	ti->split_discard_bios = !!(rs->md.level == 1 || rs->md.level == 10);
+	ti->num_discard_bios = 1;
+}
+
+/*
  * Construct a RAID4/5/6 mapping:
  * Args:
  *	<raid_type> <#raid_params> <raid_params>		\
@@ -1231,6 +1278,11 @@ static int raid_ctr(struct dm_target *ti, unsigned argc, char **argv)
 	ti->private = rs;
 	ti->num_flush_bios = 1;
 
+	/*
+	 * Disable/enable discard support on RAID set.
+	 */
+	configure_discard_support(ti, rs);
+
 	mutex_lock(&rs->md.reconfig_mutex);
 	ret = md_run(&rs->md);
 	rs->md.in_sync = 0; /* Assume already marked dirty */
@@ -1652,7 +1704,7 @@ static void raid_resume(struct dm_target *ti)
 
 static struct target_type raid_target = {
 	.name = "raid",
-	.version = {1, 5, 2},
+	.version = {1, 6, 0},
 	.module = THIS_MODULE,
 	.ctr = raid_ctr,
 	.dtr = raid_dtr,
@@ -1683,6 +1735,10 @@ static void __exit dm_raid_exit(void)
 module_init(dm_raid_init);
 module_exit(dm_raid_exit);
 
+module_param(devices_handle_discard_safely, bool, 0644);
+MODULE_PARM_DESC(devices_handle_discard_safely,
+		 "Set to Y if all devices in each array reliably return zeroes on reads from discarded regions");
+
 MODULE_DESCRIPTION(DM_NAME " raid4/5/6 target");
 MODULE_ALIAS("dm-raid1");
 MODULE_ALIAS("dm-raid10");
diff --git a/drivers/md/dm-stats.c b/drivers/md/dm-stats.c
index 28a90122a5a8..87f86c77b094 100644
--- a/drivers/md/dm-stats.c
+++ b/drivers/md/dm-stats.c
@@ -548,7 +548,7 @@ void dm_stats_account_io(struct dm_stats *stats, unsigned long bi_rw,
 		 * A race condition can at worst result in the merged flag being
 		 * misrepresented, so we don't have to disable preemption here.
 		 */
-		last = __this_cpu_ptr(stats->last);
+		last = raw_cpu_ptr(stats->last);
 		stats_aux->merged =
 			(bi_sector == (ACCESS_ONCE(last->last_sector) &&
 				       ((bi_rw & (REQ_WRITE | REQ_DISCARD)) ==
diff --git a/drivers/md/dm-table.c b/drivers/md/dm-table.c
index f9c6cb8dbcf8..b2bd1ebf4562 100644
--- a/drivers/md/dm-table.c
+++ b/drivers/md/dm-table.c
@@ -210,15 +210,16 @@ int dm_table_create(struct dm_table **result, fmode_t mode,
 	return 0;
 }
 
-static void free_devices(struct list_head *devices)
+static void free_devices(struct list_head *devices, struct mapped_device *md)
 {
 	struct list_head *tmp, *next;
 
 	list_for_each_safe(tmp, next, devices) {
 		struct dm_dev_internal *dd =
 		    list_entry(tmp, struct dm_dev_internal, list);
-		DMWARN("dm_table_destroy: dm_put_device call missing for %s",
-		       dd->dm_dev.name);
+		DMWARN("%s: dm_table_destroy: dm_put_device call missing for %s",
+		       dm_device_name(md), dd->dm_dev->name);
+		dm_put_table_device(md, dd->dm_dev);
 		kfree(dd);
 	}
 }
@@ -247,7 +248,7 @@ void dm_table_destroy(struct dm_table *t)
 	vfree(t->highs);
 
 	/* free the device list */
-	free_devices(&t->devices);
+	free_devices(&t->devices, t->md);
 
 	dm_free_md_mempools(t->mempools);
 
@@ -262,53 +263,13 @@ static struct dm_dev_internal *find_device(struct list_head *l, dev_t dev)
 	struct dm_dev_internal *dd;
 
 	list_for_each_entry (dd, l, list)
-		if (dd->dm_dev.bdev->bd_dev == dev)
+		if (dd->dm_dev->bdev->bd_dev == dev)
 			return dd;
 
 	return NULL;
 }
 
 /*
- * Open a device so we can use it as a map destination.
- */
-static int open_dev(struct dm_dev_internal *d, dev_t dev,
-		    struct mapped_device *md)
-{
-	static char *_claim_ptr = "I belong to device-mapper";
-	struct block_device *bdev;
-
-	int r;
-
-	BUG_ON(d->dm_dev.bdev);
-
-	bdev = blkdev_get_by_dev(dev, d->dm_dev.mode | FMODE_EXCL, _claim_ptr);
-	if (IS_ERR(bdev))
-		return PTR_ERR(bdev);
-
-	r = bd_link_disk_holder(bdev, dm_disk(md));
-	if (r) {
-		blkdev_put(bdev, d->dm_dev.mode | FMODE_EXCL);
-		return r;
-	}
-
-	d->dm_dev.bdev = bdev;
-	return 0;
-}
-
-/*
- * Close a device that we've been using.
- */
-static void close_dev(struct dm_dev_internal *d, struct mapped_device *md)
-{
-	if (!d->dm_dev.bdev)
-		return;
-
-	bd_unlink_disk_holder(d->dm_dev.bdev, dm_disk(md));
-	blkdev_put(d->dm_dev.bdev, d->dm_dev.mode | FMODE_EXCL);
-	d->dm_dev.bdev = NULL;
-}
-
-/*
  * If possible, this checks an area of a destination device is invalid.
  */
 static int device_area_is_invalid(struct dm_target *ti, struct dm_dev *dev,
@@ -386,19 +347,17 @@ static int upgrade_mode(struct dm_dev_internal *dd, fmode_t new_mode,
 			struct mapped_device *md)
 {
 	int r;
-	struct dm_dev_internal dd_new, dd_old;
+	struct dm_dev *old_dev, *new_dev;
 
-	dd_new = dd_old = *dd;
+	old_dev = dd->dm_dev;
 
-	dd_new.dm_dev.mode |= new_mode;
-	dd_new.dm_dev.bdev = NULL;
-
-	r = open_dev(&dd_new, dd->dm_dev.bdev->bd_dev, md);
+	r = dm_get_table_device(md, dd->dm_dev->bdev->bd_dev,
+				dd->dm_dev->mode | new_mode, &new_dev);
 	if (r)
 		return r;
 
-	dd->dm_dev.mode |= new_mode;
-	close_dev(&dd_old, md);
+	dd->dm_dev = new_dev;
+	dm_put_table_device(md, old_dev);
 
 	return 0;
 }
@@ -440,27 +399,22 @@ int dm_get_device(struct dm_target *ti, const char *path, fmode_t mode,
 		if (!dd)
 			return -ENOMEM;
 
-		dd->dm_dev.mode = mode;
-		dd->dm_dev.bdev = NULL;
-
-		if ((r = open_dev(dd, dev, t->md))) {
+		if ((r = dm_get_table_device(t->md, dev, mode, &dd->dm_dev))) {
 			kfree(dd);
 			return r;
 		}
 
-		format_dev_t(dd->dm_dev.name, dev);
-
 		atomic_set(&dd->count, 0);
 		list_add(&dd->list, &t->devices);
 
-	} else if (dd->dm_dev.mode != (mode | dd->dm_dev.mode)) {
+	} else if (dd->dm_dev->mode != (mode | dd->dm_dev->mode)) {
 		r = upgrade_mode(dd, mode, t->md);
 		if (r)
 			return r;
 	}
 	atomic_inc(&dd->count);
 
-	*result = &dd->dm_dev;
+	*result = dd->dm_dev;
 	return 0;
 }
 EXPORT_SYMBOL(dm_get_device);
@@ -505,11 +459,23 @@ static int dm_set_device_limits(struct dm_target *ti, struct dm_dev *dev,
  */
 void dm_put_device(struct dm_target *ti, struct dm_dev *d)
 {
-	struct dm_dev_internal *dd = container_of(d, struct dm_dev_internal,
-						  dm_dev);
+	int found = 0;
+	struct list_head *devices = &ti->table->devices;
+	struct dm_dev_internal *dd;
 
+	list_for_each_entry(dd, devices, list) {
+		if (dd->dm_dev == d) {
+			found = 1;
+			break;
+		}
+	}
+	if (!found) {
+		DMWARN("%s: device %s not in table devices list",
+		       dm_device_name(ti->table->md), d->name);
+		return;
+	}
 	if (atomic_dec_and_test(&dd->count)) {
-		close_dev(dd, ti->table->md);
+		dm_put_table_device(ti->table->md, d);
 		list_del(&dd->list);
 		kfree(dd);
 	}
@@ -906,7 +872,7 @@ static int dm_table_set_type(struct dm_table *t)
 	/* Non-request-stackable devices can't be used for request-based dm */
 	devices = dm_table_get_devices(t);
 	list_for_each_entry(dd, devices, list) {
-		if (!blk_queue_stackable(bdev_get_queue(dd->dm_dev.bdev))) {
+		if (!blk_queue_stackable(bdev_get_queue(dd->dm_dev->bdev))) {
 			DMWARN("table load rejected: including"
 			       " non-request-stackable devices");
 			return -EINVAL;
@@ -1043,7 +1009,7 @@ static struct gendisk * dm_table_get_integrity_disk(struct dm_table *t,
 	struct gendisk *prev_disk = NULL, *template_disk = NULL;
 
 	list_for_each_entry(dd, devices, list) {
-		template_disk = dd->dm_dev.bdev->bd_disk;
+		template_disk = dd->dm_dev->bdev->bd_disk;
 		if (!blk_get_integrity(template_disk))
 			goto no_integrity;
 		if (!match_all && !blk_integrity_is_initialized(template_disk))
@@ -1629,7 +1595,7 @@ int dm_table_any_congested(struct dm_table *t, int bdi_bits)
 	int r = 0;
 
 	list_for_each_entry(dd, devices, list) {
-		struct request_queue *q = bdev_get_queue(dd->dm_dev.bdev);
+		struct request_queue *q = bdev_get_queue(dd->dm_dev->bdev);
 		char b[BDEVNAME_SIZE];
 
 		if (likely(q))
@@ -1637,7 +1603,7 @@ int dm_table_any_congested(struct dm_table *t, int bdi_bits)
 		else
 			DMWARN_LIMIT("%s: any_congested: nonexistent device %s",
 				     dm_device_name(t->md),
-				     bdevname(dd->dm_dev.bdev, b));
+				     bdevname(dd->dm_dev->bdev, b));
 	}
 
 	list_for_each_entry(cb, &t->target_callbacks, list)
diff --git a/drivers/md/dm.c b/drivers/md/dm.c
index 32b958dbc499..58f3927fd7cc 100644
--- a/drivers/md/dm.c
+++ b/drivers/md/dm.c
@@ -142,6 +142,9 @@ struct mapped_device {
 	 */
 	struct dm_table *map;
 
+	struct list_head table_devices;
+	struct mutex table_devices_lock;
+
 	unsigned long flags;
 
 	struct request_queue *queue;
@@ -212,6 +215,12 @@ struct dm_md_mempools {
 	struct bio_set *bs;
 };
 
+struct table_device {
+	struct list_head list;
+	atomic_t count;
+	struct dm_dev dm_dev;
+};
+
 #define RESERVED_BIO_BASED_IOS		16
 #define RESERVED_REQUEST_BASED_IOS	256
 #define RESERVED_MAX_IOS		1024
@@ -670,6 +679,120 @@ static void dm_put_live_table_fast(struct mapped_device *md) __releases(RCU)
 }
 
 /*
+ * Open a table device so we can use it as a map destination.
+ */
+static int open_table_device(struct table_device *td, dev_t dev,
+			     struct mapped_device *md)
+{
+	static char *_claim_ptr = "I belong to device-mapper";
+	struct block_device *bdev;
+
+	int r;
+
+	BUG_ON(td->dm_dev.bdev);
+
+	bdev = blkdev_get_by_dev(dev, td->dm_dev.mode | FMODE_EXCL, _claim_ptr);
+	if (IS_ERR(bdev))
+		return PTR_ERR(bdev);
+
+	r = bd_link_disk_holder(bdev, dm_disk(md));
+	if (r) {
+		blkdev_put(bdev, td->dm_dev.mode | FMODE_EXCL);
+		return r;
+	}
+
+	td->dm_dev.bdev = bdev;
+	return 0;
+}
+
+/*
+ * Close a table device that we've been using.
+ */
+static void close_table_device(struct table_device *td, struct mapped_device *md)
+{
+	if (!td->dm_dev.bdev)
+		return;
+
+	bd_unlink_disk_holder(td->dm_dev.bdev, dm_disk(md));
+	blkdev_put(td->dm_dev.bdev, td->dm_dev.mode | FMODE_EXCL);
+	td->dm_dev.bdev = NULL;
+}
+
+static struct table_device *find_table_device(struct list_head *l, dev_t dev,
+					      fmode_t mode) {
+	struct table_device *td;
+
+	list_for_each_entry(td, l, list)
+		if (td->dm_dev.bdev->bd_dev == dev && td->dm_dev.mode == mode)
+			return td;
+
+	return NULL;
+}
+
+int dm_get_table_device(struct mapped_device *md, dev_t dev, fmode_t mode,
+			struct dm_dev **result) {
+	int r;
+	struct table_device *td;
+
+	mutex_lock(&md->table_devices_lock);
+	td = find_table_device(&md->table_devices, dev, mode);
+	if (!td) {
+		td = kmalloc(sizeof(*td), GFP_KERNEL);
+		if (!td) {
+			mutex_unlock(&md->table_devices_lock);
+			return -ENOMEM;
+		}
+
+		td->dm_dev.mode = mode;
+		td->dm_dev.bdev = NULL;
+
+		if ((r = open_table_device(td, dev, md))) {
+			mutex_unlock(&md->table_devices_lock);
+			kfree(td);
+			return r;
+		}
+
+		format_dev_t(td->dm_dev.name, dev);
+
+		atomic_set(&td->count, 0);
+		list_add(&td->list, &md->table_devices);
+	}
+	atomic_inc(&td->count);
+	mutex_unlock(&md->table_devices_lock);
+
+	*result = &td->dm_dev;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(dm_get_table_device);
+
+void dm_put_table_device(struct mapped_device *md, struct dm_dev *d)
+{
+	struct table_device *td = container_of(d, struct table_device, dm_dev);
+
+	mutex_lock(&md->table_devices_lock);
+	if (atomic_dec_and_test(&td->count)) {
+		close_table_device(td, md);
+		list_del(&td->list);
+		kfree(td);
+	}
+	mutex_unlock(&md->table_devices_lock);
+}
+EXPORT_SYMBOL(dm_put_table_device);
+
+static void free_table_devices(struct list_head *devices)
+{
+	struct list_head *tmp, *next;
+
+	list_for_each_safe(tmp, next, devices) {
+		struct table_device *td = list_entry(tmp, struct table_device, list);
+
+		DMWARN("dm_destroy: %s still exists with %d references",
+		       td->dm_dev.name, atomic_read(&td->count));
+		kfree(td);
+	}
+}
+
+/*
  * Get the geometry associated with a dm device
  */
 int dm_get_geometry(struct mapped_device *md, struct hd_geometry *geo)
@@ -1249,13 +1372,13 @@ static void clone_bio(struct dm_target_io *tio, struct bio *bio,
 }
 
 static struct dm_target_io *alloc_tio(struct clone_info *ci,
-				      struct dm_target *ti, int nr_iovecs,
+				      struct dm_target *ti,
 				      unsigned target_bio_nr)
 {
 	struct dm_target_io *tio;
 	struct bio *clone;
 
-	clone = bio_alloc_bioset(GFP_NOIO, nr_iovecs, ci->md->bs);
+	clone = bio_alloc_bioset(GFP_NOIO, 0, ci->md->bs);
 	tio = container_of(clone, struct dm_target_io, clone);
 
 	tio->io = ci->io;
@@ -1269,17 +1392,12 @@ static void __clone_and_map_simple_bio(struct clone_info *ci,
 				       struct dm_target *ti,
 				       unsigned target_bio_nr, unsigned *len)
 {
-	struct dm_target_io *tio = alloc_tio(ci, ti, ci->bio->bi_max_vecs, target_bio_nr);
+	struct dm_target_io *tio = alloc_tio(ci, ti, target_bio_nr);
 	struct bio *clone = &tio->clone;
 
 	tio->len_ptr = len;
 
-	/*
-	 * Discard requests require the bio's inline iovecs be initialized.
-	 * ci->bio->bi_max_vecs is BIO_INLINE_VECS anyway, for both flush
-	 * and discard, so no need for concern about wasted bvec allocations.
-	 */
-	 __bio_clone_fast(clone, ci->bio);
+	__bio_clone_fast(clone, ci->bio);
 	if (len)
 		bio_setup_sector(clone, ci->sector, *len);
 
@@ -1322,7 +1440,7 @@ static void __clone_and_map_data_bio(struct clone_info *ci, struct dm_target *ti
 		num_target_bios = ti->num_write_bios(ti, bio);
 
 	for (target_bio_nr = 0; target_bio_nr < num_target_bios; target_bio_nr++) {
-		tio = alloc_tio(ci, ti, 0, target_bio_nr);
+		tio = alloc_tio(ci, ti, target_bio_nr);
 		tio->len_ptr = len;
 		clone_bio(tio, bio, sector, *len);
 		__map_bio(tio);
@@ -1949,12 +2067,14 @@ static struct mapped_device *alloc_dev(int minor)
 	md->type = DM_TYPE_NONE;
 	mutex_init(&md->suspend_lock);
 	mutex_init(&md->type_lock);
+	mutex_init(&md->table_devices_lock);
 	spin_lock_init(&md->deferred_lock);
 	atomic_set(&md->holders, 1);
 	atomic_set(&md->open_count, 0);
 	atomic_set(&md->event_nr, 0);
 	atomic_set(&md->uevent_seq, 0);
 	INIT_LIST_HEAD(&md->uevent_list);
+	INIT_LIST_HEAD(&md->table_devices);
 	spin_lock_init(&md->uevent_lock);
 
 	md->queue = blk_alloc_queue(GFP_KERNEL);
@@ -2040,6 +2160,7 @@ static void free_dev(struct mapped_device *md)
 	blk_integrity_unregister(md->disk);
 	del_gendisk(md->disk);
 	cleanup_srcu_struct(&md->io_barrier);
+	free_table_devices(&md->table_devices);
 	free_minor(minor);
 
 	spin_lock(&_minor_lock);
@@ -2900,7 +3021,7 @@ struct dm_md_mempools *dm_alloc_md_mempools(unsigned type, unsigned integrity, u
 	if (!pools->io_pool)
 		goto out;
 
-	pools->bs = bioset_create(pool_size, front_pad);
+	pools->bs = bioset_create_nobvec(pool_size, front_pad);
 	if (!pools->bs)
 		goto out;
 
diff --git a/drivers/md/dm.h b/drivers/md/dm.h
index e81d2152fa68..988c7fb7b145 100644
--- a/drivers/md/dm.h
+++ b/drivers/md/dm.h
@@ -44,7 +44,7 @@
 struct dm_dev_internal {
 	struct list_head list;
 	atomic_t count;
-	struct dm_dev dm_dev;
+	struct dm_dev *dm_dev;
 };
 
 struct dm_table;
@@ -188,6 +188,9 @@ int dm_cancel_deferred_remove(struct mapped_device *md);
 int dm_request_based(struct mapped_device *md);
 sector_t dm_get_size(struct mapped_device *md);
 struct request_queue *dm_get_md_queue(struct mapped_device *md);
+int dm_get_table_device(struct mapped_device *md, dev_t dev, fmode_t mode,
+			struct dm_dev **result);
+void dm_put_table_device(struct mapped_device *md, struct dm_dev *d);
 struct dm_stats *dm_get_stats(struct mapped_device *md);
 
 int dm_kobject_uevent(struct mapped_device *md, enum kobject_action action,
diff --git a/drivers/md/linear.c b/drivers/md/linear.c
index 56f534b4a2d2..64713b77df1c 100644
--- a/drivers/md/linear.c
+++ b/drivers/md/linear.c
@@ -10,10 +10,10 @@
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2, or (at your option)
    any later version.
-   
+
    You should have received a copy of the GNU General Public License
    (for example /usr/src/linux/COPYING); if not, write to the Free
-   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.  
+   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
 #include <linux/blkdev.h>
@@ -25,7 +25,7 @@
 #include "linear.h"
 
 /*
- * find which device holds a particular offset 
+ * find which device holds a particular offset
  */
 static inline struct dev_info *which_dev(struct mddev *mddev, sector_t sector)
 {
@@ -355,7 +355,6 @@ static void linear_status (struct seq_file *seq, struct mddev *mddev)
 	seq_printf(seq, " %dk rounding", mddev->chunk_sectors / 2);
 }
 
-
 static struct md_personality linear_personality =
 {
 	.name		= "linear",
@@ -379,7 +378,6 @@ static void linear_exit (void)
 	unregister_md_personality (&linear_personality);
 }
 
-
 module_init(linear_init);
 module_exit(linear_exit);
 MODULE_LICENSE("GPL");
diff --git a/drivers/md/md.c b/drivers/md/md.c
index 1294238610df..4dfa15da9cb8 100644
--- a/drivers/md/md.c
+++ b/drivers/md/md.c
@@ -1,6 +1,6 @@
 /*
    md.c : Multiple Devices driver for Linux
-	  Copyright (C) 1998, 1999, 2000 Ingo Molnar
+     Copyright (C) 1998, 1999, 2000 Ingo Molnar
 
      completely rewritten, based on the MD driver code from Marc Zyngier
 
@@ -66,8 +66,6 @@ static void autostart_arrays(int part);
 static LIST_HEAD(pers_list);
 static DEFINE_SPINLOCK(pers_lock);
 
-static void md_print_devices(void);
-
 static DECLARE_WAIT_QUEUE_HEAD(resync_wait);
 static struct workqueue_struct *md_wq;
 static struct workqueue_struct *md_misc_wq;
@@ -75,8 +73,6 @@ static struct workqueue_struct *md_misc_wq;
 static int remove_and_add_spares(struct mddev *mddev,
 				 struct md_rdev *this);
 
-#define MD_BUG(x...) { printk("md: bug in file %s, line %d\n", __FILE__, __LINE__); md_print_devices(); }
-
 /*
  * Default number of read corrections we'll attempt on an rdev
  * before ejecting it from the array. We divide the read error
@@ -218,7 +214,6 @@ static void md_new_event_inintr(struct mddev *mddev)
 static LIST_HEAD(all_mddevs);
 static DEFINE_SPINLOCK(all_mddevs_lock);
 
-
 /*
  * iterates through all used mddevs in the system.
  * We take care to grab the all_mddevs_lock whenever navigating
@@ -228,7 +223,7 @@ static DEFINE_SPINLOCK(all_mddevs_lock);
  */
 #define for_each_mddev(_mddev,_tmp)					\
 									\
-	for (({ spin_lock(&all_mddevs_lock); 				\
+	for (({ spin_lock(&all_mddevs_lock);				\
 		_tmp = all_mddevs.next;					\
 		_mddev = NULL;});					\
 	     ({ if (_tmp != &all_mddevs)				\
@@ -241,7 +236,6 @@ static DEFINE_SPINLOCK(all_mddevs_lock);
 		_tmp = _tmp->next;})					\
 		)
 
-
 /* Rather than calling directly into the personality make_request function,
  * IO requests come here first so that we can check if the device is
  * being suspended pending a reconfiguration.
@@ -488,7 +482,7 @@ void mddev_init(struct mddev *mddev)
 }
 EXPORT_SYMBOL_GPL(mddev_init);
 
-static struct mddev * mddev_find(dev_t unit)
+static struct mddev *mddev_find(dev_t unit)
 {
 	struct mddev *mddev, *new = NULL;
 
@@ -530,7 +524,7 @@ static struct mddev * mddev_find(dev_t unit)
 				kfree(new);
 				return NULL;
 			}
-				
+
 			is_free = 1;
 			list_for_each_entry(mddev, &all_mddevs, all_mddevs)
 				if (mddev->unit == dev) {
@@ -562,7 +556,7 @@ static struct mddev * mddev_find(dev_t unit)
 	goto retry;
 }
 
-static inline int __must_check mddev_lock(struct mddev * mddev)
+static inline int __must_check mddev_lock(struct mddev *mddev)
 {
 	return mutex_lock_interruptible(&mddev->reconfig_mutex);
 }
@@ -570,7 +564,7 @@ static inline int __must_check mddev_lock(struct mddev * mddev)
 /* Sometimes we need to take the lock in a situation where
  * failure due to interrupts is not acceptable.
  */
-static inline void mddev_lock_nointr(struct mddev * mddev)
+static inline void mddev_lock_nointr(struct mddev *mddev)
 {
 	mutex_lock(&mddev->reconfig_mutex);
 }
@@ -580,14 +574,14 @@ static inline int mddev_is_locked(struct mddev *mddev)
 	return mutex_is_locked(&mddev->reconfig_mutex);
 }
 
-static inline int mddev_trylock(struct mddev * mddev)
+static inline int mddev_trylock(struct mddev *mddev)
 {
 	return mutex_trylock(&mddev->reconfig_mutex);
 }
 
 static struct attribute_group md_redundancy_group;
 
-static void mddev_unlock(struct mddev * mddev)
+static void mddev_unlock(struct mddev *mddev)
 {
 	if (mddev->to_remove) {
 		/* These cannot be removed under reconfig_mutex as
@@ -630,17 +624,6 @@ static void mddev_unlock(struct mddev * mddev)
 	spin_unlock(&pers_lock);
 }
 
-static struct md_rdev * find_rdev_nr(struct mddev *mddev, int nr)
-{
-	struct md_rdev *rdev;
-
-	rdev_for_each(rdev, mddev)
-		if (rdev->desc_nr == nr)
-			return rdev;
-
-	return NULL;
-}
-
 static struct md_rdev *find_rdev_nr_rcu(struct mddev *mddev, int nr)
 {
 	struct md_rdev *rdev;
@@ -693,11 +676,8 @@ static inline sector_t calc_dev_sboffset(struct md_rdev *rdev)
 	return MD_NEW_SIZE_SECTORS(num_sectors);
 }
 
-static int alloc_disk_sb(struct md_rdev * rdev)
+static int alloc_disk_sb(struct md_rdev *rdev)
 {
-	if (rdev->sb_page)
-		MD_BUG();
-
 	rdev->sb_page = alloc_page(GFP_KERNEL);
 	if (!rdev->sb_page) {
 		printk(KERN_ALERT "md: out of memory.\n");
@@ -766,14 +746,7 @@ void md_super_write(struct mddev *mddev, struct md_rdev *rdev,
 void md_super_wait(struct mddev *mddev)
 {
 	/* wait for all superblock writes that were scheduled to complete */
-	DEFINE_WAIT(wq);
-	for(;;) {
-		prepare_to_wait(&mddev->sb_wait, &wq, TASK_UNINTERRUPTIBLE);
-		if (atomic_read(&mddev->pending_writes)==0)
-			break;
-		schedule();
-	}
-	finish_wait(&mddev->sb_wait, &wq);
+	wait_event(mddev->sb_wait, atomic_read(&mddev->pending_writes)==0);
 }
 
 int sync_page_io(struct md_rdev *rdev, sector_t sector, int size,
@@ -801,17 +774,13 @@ int sync_page_io(struct md_rdev *rdev, sector_t sector, int size,
 }
 EXPORT_SYMBOL_GPL(sync_page_io);
 
-static int read_disk_sb(struct md_rdev * rdev, int size)
+static int read_disk_sb(struct md_rdev *rdev, int size)
 {
 	char b[BDEVNAME_SIZE];
-	if (!rdev->sb_page) {
-		MD_BUG();
-		return -EINVAL;
-	}
+
 	if (rdev->sb_loaded)
 		return 0;
 
-
 	if (!sync_page_io(rdev, 0, size, rdev->sb_page, READ, true))
 		goto fail;
 	rdev->sb_loaded = 1;
@@ -825,7 +794,7 @@ fail:
 
 static int uuid_equal(mdp_super_t *sb1, mdp_super_t *sb2)
 {
-	return 	sb1->set_uuid0 == sb2->set_uuid0 &&
+	return	sb1->set_uuid0 == sb2->set_uuid0 &&
 		sb1->set_uuid1 == sb2->set_uuid1 &&
 		sb1->set_uuid2 == sb2->set_uuid2 &&
 		sb1->set_uuid3 == sb2->set_uuid3;
@@ -861,14 +830,13 @@ abort:
 	return ret;
 }
 
-
 static u32 md_csum_fold(u32 csum)
 {
 	csum = (csum & 0xffff) + (csum >> 16);
 	return (csum & 0xffff) + (csum >> 16);
 }
 
-static unsigned int calc_sb_csum(mdp_super_t * sb)
+static unsigned int calc_sb_csum(mdp_super_t *sb)
 {
 	u64 newcsum = 0;
 	u32 *sb32 = (u32*)sb;
@@ -882,7 +850,6 @@ static unsigned int calc_sb_csum(mdp_super_t * sb)
 		newcsum += sb32[i];
 	csum = (newcsum & 0xffffffff) + (newcsum>>32);
 
-
 #ifdef CONFIG_ALPHA
 	/* This used to use csum_partial, which was wrong for several
 	 * reasons including that different results are returned on
@@ -899,7 +866,6 @@ static unsigned int calc_sb_csum(mdp_super_t * sb)
 	return csum;
 }
 
-
 /*
  * Handle superblock details.
  * We want to be able to handle multiple superblock formats
@@ -965,7 +931,7 @@ int md_check_no_bitmap(struct mddev *mddev)
 EXPORT_SYMBOL(md_check_no_bitmap);
 
 /*
- * load_super for 0.90.0 
+ * load_super for 0.90.0
  */
 static int super_90_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_version)
 {
@@ -1044,7 +1010,7 @@ static int super_90_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor
 		ev2 = md_event(refsb);
 		if (ev1 > ev2)
 			ret = 1;
-		else 
+		else
 			ret = 0;
 	}
 	rdev->sectors = rdev->sb_start;
@@ -1118,7 +1084,7 @@ static int super_90_validate(struct mddev *mddev, struct md_rdev *rdev)
 		if (sb->state & (1<<MD_SB_CLEAN))
 			mddev->recovery_cp = MaxSector;
 		else {
-			if (sb->events_hi == sb->cp_events_hi && 
+			if (sb->events_hi == sb->cp_events_hi &&
 				sb->events_lo == sb->cp_events_lo) {
 				mddev->recovery_cp = sb->recovery_cp;
 			} else
@@ -1146,7 +1112,7 @@ static int super_90_validate(struct mddev *mddev, struct md_rdev *rdev)
 		++ev1;
 		if (sb->disks[rdev->desc_nr].state & (
 			    (1<<MD_DISK_SYNC) | (1 << MD_DISK_ACTIVE)))
-			if (ev1 < mddev->events) 
+			if (ev1 < mddev->events)
 				return -EINVAL;
 	} else if (mddev->bitmap) {
 		/* if adding to array with a bitmap, then we can accept an
@@ -1197,7 +1163,6 @@ static void super_90_sync(struct mddev *mddev, struct md_rdev *rdev)
 	struct md_rdev *rdev2;
 	int next_spare = mddev->raid_disks;
 
-
 	/* make rdev->sb match mddev data..
 	 *
 	 * 1/ zero out disks
@@ -1366,7 +1331,7 @@ super_90_allow_new_offset(struct md_rdev *rdev, unsigned long long new_offset)
  * version 1 superblock
  */
 
-static __le32 calc_sb_1_csum(struct mdp_superblock_1 * sb)
+static __le32 calc_sb_1_csum(struct mdp_superblock_1 *sb)
 {
 	__le32 disk_csum;
 	u32 csum;
@@ -1430,7 +1395,6 @@ static int super_1_load(struct md_rdev *rdev, struct md_rdev *refdev, int minor_
 	ret = read_disk_sb(rdev, 4096);
 	if (ret) return ret;
 
-
 	sb = page_address(rdev->sb_page);
 
 	if (sb->magic != cpu_to_le32(MD_SB_MAGIC) ||
@@ -1817,7 +1781,7 @@ retry:
 
 	for (i=0; i<max_dev;i++)
 		sb->dev_roles[i] = cpu_to_le16(0xfffe);
-	
+
 	rdev_for_each(rdev2, mddev) {
 		i = rdev2->desc_nr;
 		if (test_bit(Faulty, &rdev2->flags))
@@ -2033,18 +1997,13 @@ void md_integrity_add_rdev(struct md_rdev *rdev, struct mddev *mddev)
 }
 EXPORT_SYMBOL(md_integrity_add_rdev);
 
-static int bind_rdev_to_array(struct md_rdev * rdev, struct mddev * mddev)
+static int bind_rdev_to_array(struct md_rdev *rdev, struct mddev *mddev)
 {
 	char b[BDEVNAME_SIZE];
 	struct kobject *ko;
 	char *s;
 	int err;
 
-	if (rdev->mddev) {
-		MD_BUG();
-		return -EINVAL;
-	}
-
 	/* prevent duplicates */
 	if (find_rdev(mddev, rdev->bdev->bd_dev))
 		return -EEXIST;
@@ -2067,16 +2026,21 @@ static int bind_rdev_to_array(struct md_rdev * rdev, struct mddev * mddev)
 	 * If it is -1, assign a free number, else
 	 * check number is not in use
 	 */
+	rcu_read_lock();
 	if (rdev->desc_nr < 0) {
 		int choice = 0;
-		if (mddev->pers) choice = mddev->raid_disks;
-		while (find_rdev_nr(mddev, choice))
+		if (mddev->pers)
+			choice = mddev->raid_disks;
+		while (find_rdev_nr_rcu(mddev, choice))
 			choice++;
 		rdev->desc_nr = choice;
 	} else {
-		if (find_rdev_nr(mddev, rdev->desc_nr))
+		if (find_rdev_nr_rcu(mddev, rdev->desc_nr)) {
+			rcu_read_unlock();
 			return -EBUSY;
+		}
 	}
+	rcu_read_unlock();
 	if (mddev->max_disks && rdev->desc_nr >= mddev->max_disks) {
 		printk(KERN_WARNING "md: %s: array is limited to %d devices\n",
 		       mdname(mddev), mddev->max_disks);
@@ -2118,13 +2082,10 @@ static void md_delayed_delete(struct work_struct *ws)
 	kobject_put(&rdev->kobj);
 }
 
-static void unbind_rdev_from_array(struct md_rdev * rdev)
+static void unbind_rdev_from_array(struct md_rdev *rdev)
 {
 	char b[BDEVNAME_SIZE];
-	if (!rdev->mddev) {
-		MD_BUG();
-		return;
-	}
+
 	bd_unlink_disk_holder(rdev->bdev, rdev->mddev->gendisk);
 	list_del_rcu(&rdev->same_set);
 	printk(KERN_INFO "md: unbind<%s>\n", bdevname(rdev->bdev,b));
@@ -2169,20 +2130,17 @@ static void unlock_rdev(struct md_rdev *rdev)
 {
 	struct block_device *bdev = rdev->bdev;
 	rdev->bdev = NULL;
-	if (!bdev)
-		MD_BUG();
 	blkdev_put(bdev, FMODE_READ|FMODE_WRITE|FMODE_EXCL);
 }
 
 void md_autodetect_dev(dev_t dev);
 
-static void export_rdev(struct md_rdev * rdev)
+static void export_rdev(struct md_rdev *rdev)
 {
 	char b[BDEVNAME_SIZE];
+
 	printk(KERN_INFO "md: export_rdev(%s)\n",
 		bdevname(rdev->bdev,b));
-	if (rdev->mddev)
-		MD_BUG();
 	md_rdev_clear(rdev);
 #ifndef MODULE
 	if (test_bit(AutoDetected, &rdev->flags))
@@ -2192,7 +2150,7 @@ static void export_rdev(struct md_rdev * rdev)
 	kobject_put(&rdev->kobj);
 }
 
-static void kick_rdev_from_array(struct md_rdev * rdev)
+static void kick_rdev_from_array(struct md_rdev *rdev)
 {
 	unbind_rdev_from_array(rdev);
 	export_rdev(rdev);
@@ -2200,153 +2158,18 @@ static void kick_rdev_from_array(struct md_rdev * rdev)
 
 static void export_array(struct mddev *mddev)
 {
-	struct md_rdev *rdev, *tmp;
+	struct md_rdev *rdev;
 
-	rdev_for_each_safe(rdev, tmp, mddev) {
-		if (!rdev->mddev) {
-			MD_BUG();
-			continue;
-		}
+	while (!list_empty(&mddev->disks)) {
+		rdev = list_first_entry(&mddev->disks, struct md_rdev,
+					same_set);
 		kick_rdev_from_array(rdev);
 	}
-	if (!list_empty(&mddev->disks))
-		MD_BUG();
 	mddev->raid_disks = 0;
 	mddev->major_version = 0;
 }
 
-static void print_desc(mdp_disk_t *desc)
-{
-	printk(" DISK<N:%d,(%d,%d),R:%d,S:%d>\n", desc->number,
-		desc->major,desc->minor,desc->raid_disk,desc->state);
-}
-
-static void print_sb_90(mdp_super_t *sb)
-{
-	int i;
-
-	printk(KERN_INFO 
-		"md:  SB: (V:%d.%d.%d) ID:<%08x.%08x.%08x.%08x> CT:%08x\n",
-		sb->major_version, sb->minor_version, sb->patch_version,
-		sb->set_uuid0, sb->set_uuid1, sb->set_uuid2, sb->set_uuid3,
-		sb->ctime);
-	printk(KERN_INFO "md:     L%d S%08d ND:%d RD:%d md%d LO:%d CS:%d\n",
-		sb->level, sb->size, sb->nr_disks, sb->raid_disks,
-		sb->md_minor, sb->layout, sb->chunk_size);
-	printk(KERN_INFO "md:     UT:%08x ST:%d AD:%d WD:%d"
-		" FD:%d SD:%d CSUM:%08x E:%08lx\n",
-		sb->utime, sb->state, sb->active_disks, sb->working_disks,
-		sb->failed_disks, sb->spare_disks,
-		sb->sb_csum, (unsigned long)sb->events_lo);
-
-	printk(KERN_INFO);
-	for (i = 0; i < MD_SB_DISKS; i++) {
-		mdp_disk_t *desc;
-
-		desc = sb->disks + i;
-		if (desc->number || desc->major || desc->minor ||
-		    desc->raid_disk || (desc->state && (desc->state != 4))) {
-			printk("     D %2d: ", i);
-			print_desc(desc);
-		}
-	}
-	printk(KERN_INFO "md:     THIS: ");
-	print_desc(&sb->this_disk);
-}
-
-static void print_sb_1(struct mdp_superblock_1 *sb)
-{
-	__u8 *uuid;
-
-	uuid = sb->set_uuid;
-	printk(KERN_INFO
-	       "md:  SB: (V:%u) (F:0x%08x) Array-ID:<%pU>\n"
-	       "md:    Name: \"%s\" CT:%llu\n",
-		le32_to_cpu(sb->major_version),
-		le32_to_cpu(sb->feature_map),
-		uuid,
-		sb->set_name,
-		(unsigned long long)le64_to_cpu(sb->ctime)
-		       & MD_SUPERBLOCK_1_TIME_SEC_MASK);
-
-	uuid = sb->device_uuid;
-	printk(KERN_INFO
-	       "md:       L%u SZ%llu RD:%u LO:%u CS:%u DO:%llu DS:%llu SO:%llu"
-			" RO:%llu\n"
-	       "md:     Dev:%08x UUID: %pU\n"
-	       "md:       (F:0x%08x) UT:%llu Events:%llu ResyncOffset:%llu CSUM:0x%08x\n"
-	       "md:         (MaxDev:%u) \n",
-		le32_to_cpu(sb->level),
-		(unsigned long long)le64_to_cpu(sb->size),
-		le32_to_cpu(sb->raid_disks),
-		le32_to_cpu(sb->layout),
-		le32_to_cpu(sb->chunksize),
-		(unsigned long long)le64_to_cpu(sb->data_offset),
-		(unsigned long long)le64_to_cpu(sb->data_size),
-		(unsigned long long)le64_to_cpu(sb->super_offset),
-		(unsigned long long)le64_to_cpu(sb->recovery_offset),
-		le32_to_cpu(sb->dev_number),
-		uuid,
-		sb->devflags,
-		(unsigned long long)le64_to_cpu(sb->utime) & MD_SUPERBLOCK_1_TIME_SEC_MASK,
-		(unsigned long long)le64_to_cpu(sb->events),
-		(unsigned long long)le64_to_cpu(sb->resync_offset),
-		le32_to_cpu(sb->sb_csum),
-		le32_to_cpu(sb->max_dev)
-		);
-}
-
-static void print_rdev(struct md_rdev *rdev, int major_version)
-{
-	char b[BDEVNAME_SIZE];
-	printk(KERN_INFO "md: rdev %s, Sect:%08llu F:%d S:%d DN:%u\n",
-		bdevname(rdev->bdev, b), (unsigned long long)rdev->sectors,
-	        test_bit(Faulty, &rdev->flags), test_bit(In_sync, &rdev->flags),
-	        rdev->desc_nr);
-	if (rdev->sb_loaded) {
-		printk(KERN_INFO "md: rdev superblock (MJ:%d):\n", major_version);
-		switch (major_version) {
-		case 0:
-			print_sb_90(page_address(rdev->sb_page));
-			break;
-		case 1:
-			print_sb_1(page_address(rdev->sb_page));
-			break;
-		}
-	} else
-		printk(KERN_INFO "md: no rdev superblock!\n");
-}
-
-static void md_print_devices(void)
-{
-	struct list_head *tmp;
-	struct md_rdev *rdev;
-	struct mddev *mddev;
-	char b[BDEVNAME_SIZE];
-
-	printk("\n");
-	printk("md:	**********************************\n");
-	printk("md:	* <COMPLETE RAID STATE PRINTOUT> *\n");
-	printk("md:	**********************************\n");
-	for_each_mddev(mddev, tmp) {
-
-		if (mddev->bitmap)
-			bitmap_print_sb(mddev->bitmap);
-		else
-			printk("%s: ", mdname(mddev));
-		rdev_for_each(rdev, mddev)
-			printk("<%s>", bdevname(rdev->bdev,b));
-		printk("\n");
-
-		rdev_for_each(rdev, mddev)
-			print_rdev(rdev, mddev->major_version);
-	}
-	printk("md:	**********************************\n");
-	printk("\n");
-}
-
-
-static void sync_sbs(struct mddev * mddev, int nospares)
+static void sync_sbs(struct mddev *mddev, int nospares)
 {
 	/* Update each superblock (in-memory image), but
 	 * if we are allowed to, skip spares which already
@@ -2369,7 +2192,7 @@ static void sync_sbs(struct mddev * mddev, int nospares)
 	}
 }
 
-static void md_update_sb(struct mddev * mddev, int force_change)
+static void md_update_sb(struct mddev *mddev, int force_change)
 {
 	struct md_rdev *rdev;
 	int sync_req;
@@ -2390,7 +2213,7 @@ repeat:
 		    mddev->curr_resync_completed > rdev->recovery_offset)
 				rdev->recovery_offset = mddev->curr_resync_completed;
 
-	}	
+	}
 	if (!mddev->persistent) {
 		clear_bit(MD_CHANGE_CLEAN, &mddev->flags);
 		clear_bit(MD_CHANGE_DEVS, &mddev->flags);
@@ -2453,15 +2276,12 @@ repeat:
 		mddev->can_decrease_events = nospares;
 	}
 
-	if (!mddev->events) {
-		/*
-		 * oops, this 64-bit counter should never wrap.
-		 * Either we are in around ~1 trillion A.C., assuming
-		 * 1 reboot per second, or we have a bug:
-		 */
-		MD_BUG();
-		mddev->events --;
-	}
+	/*
+	 * This 64-bit counter should never wrap.
+	 * Either we are in around ~1 trillion A.C., assuming
+	 * 1 reboot per second, or we have a bug...
+	 */
+	WARN_ON(mddev->events == 0);
 
 	rdev_for_each(rdev, mddev) {
 		if (rdev->badblocks.changed)
@@ -2668,10 +2488,12 @@ state_store(struct md_rdev *rdev, const char *buf, size_t len)
 		set_bit(In_sync, &rdev->flags);
 		err = 0;
 	} else if (cmd_match(buf, "-insync") && rdev->raid_disk >= 0) {
-		clear_bit(In_sync, &rdev->flags);
-		rdev->saved_raid_disk = rdev->raid_disk;
-		rdev->raid_disk = -1;
-		err = 0;
+		if (rdev->mddev->pers == NULL) {
+			clear_bit(In_sync, &rdev->flags);
+			rdev->saved_raid_disk = rdev->raid_disk;
+			rdev->raid_disk = -1;
+			err = 0;
+		}
 	} else if (cmd_match(buf, "write_error")) {
 		set_bit(WriteErrorSeen, &rdev->flags);
 		err = 0;
@@ -2829,7 +2651,6 @@ slot_store(struct md_rdev *rdev, const char *buf, size_t len)
 	return len;
 }
 
-
 static struct rdev_sysfs_entry rdev_slot =
 __ATTR(slot, S_IRUGO|S_IWUSR, slot_show, slot_store);
 
@@ -2980,20 +2801,20 @@ rdev_size_store(struct md_rdev *rdev, const char *buf, size_t len)
 
 	rdev->sectors = sectors;
 	if (sectors > oldsectors && my_mddev->external) {
-		/* need to check that all other rdevs with the same ->bdev
-		 * do not overlap.  We need to unlock the mddev to avoid
-		 * a deadlock.  We have already changed rdev->sectors, and if
-		 * we have to change it back, we will have the lock again.
+		/* Need to check that all other rdevs with the same
+		 * ->bdev do not overlap.  'rcu' is sufficient to walk
+		 * the rdev lists safely.
+		 * This check does not provide a hard guarantee, it
+		 * just helps avoid dangerous mistakes.
 		 */
 		struct mddev *mddev;
 		int overlap = 0;
 		struct list_head *tmp;
 
-		mddev_unlock(my_mddev);
+		rcu_read_lock();
 		for_each_mddev(mddev, tmp) {
 			struct md_rdev *rdev2;
 
-			mddev_lock_nointr(mddev);
 			rdev_for_each(rdev2, mddev)
 				if (rdev->bdev == rdev2->bdev &&
 				    rdev != rdev2 &&
@@ -3003,13 +2824,12 @@ rdev_size_store(struct md_rdev *rdev, const char *buf, size_t len)
 					overlap = 1;
 					break;
 				}
-			mddev_unlock(mddev);
 			if (overlap) {
 				mddev_put(mddev);
 				break;
 			}
 		}
-		mddev_lock_nointr(my_mddev);
+		rcu_read_unlock();
 		if (overlap) {
 			/* Someone else could have slipped in a size
 			 * change here, but doing so is just silly.
@@ -3027,7 +2847,6 @@ rdev_size_store(struct md_rdev *rdev, const char *buf, size_t len)
 static struct rdev_sysfs_entry rdev_size =
 __ATTR(size, S_IRUGO|S_IWUSR, rdev_size_show, rdev_size_store);
 
-
 static ssize_t recovery_start_show(struct md_rdev *rdev, char *page)
 {
 	unsigned long long recovery_start = rdev->recovery_offset;
@@ -3063,7 +2882,6 @@ static ssize_t recovery_start_store(struct md_rdev *rdev, const char *buf, size_
 static struct rdev_sysfs_entry rdev_recovery_start =
 __ATTR(recovery_start, S_IRUGO|S_IWUSR, recovery_start_show, recovery_start_store);
 
-
 static ssize_t
 badblocks_show(struct badblocks *bb, char *page, int unack);
 static ssize_t
@@ -3084,7 +2902,6 @@ static ssize_t bb_store(struct md_rdev *rdev, const char *page, size_t len)
 static struct rdev_sysfs_entry rdev_bad_blocks =
 __ATTR(bad_blocks, S_IRUGO|S_IWUSR, bb_show, bb_store);
 
-
 static ssize_t ubb_show(struct md_rdev *rdev, char *page)
 {
 	return badblocks_show(&rdev->badblocks, page, 1);
@@ -3241,7 +3058,7 @@ static struct md_rdev *md_import_device(dev_t newdev, int super_format, int supe
 
 	size = i_size_read(rdev->bdev->bd_inode) >> BLOCK_SIZE_BITS;
 	if (!size) {
-		printk(KERN_WARNING 
+		printk(KERN_WARNING
 			"md: %s has zero or unknown size, marking faulty!\n",
 			bdevname(rdev->bdev,b));
 		err = -EINVAL;
@@ -3260,7 +3077,7 @@ static struct md_rdev *md_import_device(dev_t newdev, int super_format, int supe
 			goto abort_free;
 		}
 		if (err < 0) {
-			printk(KERN_WARNING 
+			printk(KERN_WARNING
 				"md: could not read %s's sb, not importing!\n",
 				bdevname(rdev->bdev,b));
 			goto abort_free;
@@ -3281,8 +3098,7 @@ abort_free:
  * Check a full RAID array for plausibility
  */
 
-
-static void analyze_sbs(struct mddev * mddev)
+static void analyze_sbs(struct mddev *mddev)
 {
 	int i;
 	struct md_rdev *rdev, *freshest, *tmp;
@@ -3300,12 +3116,11 @@ static void analyze_sbs(struct mddev * mddev)
 		default:
 			printk( KERN_ERR \
 				"md: fatal superblock inconsistency in %s"
-				" -- removing from array\n", 
+				" -- removing from array\n",
 				bdevname(rdev->bdev,b));
 			kick_rdev_from_array(rdev);
 		}
 
-
 	super_types[mddev->major_version].
 		validate_super(mddev, freshest);
 
@@ -3344,7 +3159,7 @@ static void analyze_sbs(struct mddev * mddev)
 /* Read a fixed-point number.
  * Numbers in sysfs attributes should be in "standard" units where
  * possible, so time should be in seconds.
- * However we internally use a a much smaller unit such as 
+ * However we internally use a a much smaller unit such as
  * milliseconds or jiffies.
  * This function takes a decimal number with a possible fractional
  * component, and produces an integer which is the result of
@@ -3381,7 +3196,6 @@ int strict_strtoul_scaled(const char *cp, unsigned long *res, int scale)
 	return 0;
 }
 
-
 static void md_safemode_timeout(unsigned long data);
 
 static ssize_t
@@ -3524,7 +3338,7 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 	/* Looks like we have a winner */
 	mddev_suspend(mddev);
 	mddev->pers->stop(mddev);
-	
+
 	if (mddev->pers->sync_request == NULL &&
 	    pers->sync_request != NULL) {
 		/* need to add the md_redundancy_group */
@@ -3533,7 +3347,7 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 			       "md: cannot register extra attributes for %s\n",
 			       mdname(mddev));
 		mddev->sysfs_action = sysfs_get_dirent(mddev->kobj.sd, "sync_action");
-	}		
+	}
 	if (mddev->pers->sync_request != NULL &&
 	    pers->sync_request == NULL) {
 		/* need to remove the md_redundancy_group */
@@ -3611,7 +3425,6 @@ level_store(struct mddev *mddev, const char *buf, size_t len)
 static struct md_sysfs_entry md_level =
 __ATTR(level, S_IRUGO|S_IWUSR, level_show, level_store);
 
-
 static ssize_t
 layout_show(struct mddev *mddev, char *page)
 {
@@ -3654,7 +3467,6 @@ layout_store(struct mddev *mddev, const char *buf, size_t len)
 static struct md_sysfs_entry md_layout =
 __ATTR(layout, S_IRUGO|S_IWUSR, layout_show, layout_store);
 
-
 static ssize_t
 raid_disks_show(struct mddev *mddev, char *page)
 {
@@ -3859,9 +3671,9 @@ array_state_show(struct mddev *mddev, char *page)
 	return sprintf(page, "%s\n", array_states[st]);
 }
 
-static int do_md_stop(struct mddev * mddev, int ro, struct block_device *bdev);
-static int md_set_readonly(struct mddev * mddev, struct block_device *bdev);
-static int do_md_run(struct mddev * mddev);
+static int do_md_stop(struct mddev *mddev, int ro, struct block_device *bdev);
+static int md_set_readonly(struct mddev *mddev, struct block_device *bdev);
+static int do_md_run(struct mddev *mddev);
 static int restart_array(struct mddev *mddev);
 
 static ssize_t
@@ -4012,7 +3824,6 @@ new_dev_store(struct mddev *mddev, const char *buf, size_t len)
 	    minor != MINOR(dev))
 		return -EOVERFLOW;
 
-
 	if (mddev->persistent) {
 		rdev = md_import_device(dev, mddev->major_version,
 					mddev->minor_version);
@@ -4108,7 +3919,6 @@ size_store(struct mddev *mddev, const char *buf, size_t len)
 static struct md_sysfs_entry md_size =
 __ATTR(component_size, S_IRUGO|S_IWUSR, size_show, size_store);
 
-
 /* Metadata version.
  * This is one of
  *   'none' for arrays with no metadata (good luck...)
@@ -4490,7 +4300,7 @@ suspend_lo_store(struct mddev *mddev, const char *buf, size_t len)
 	unsigned long long new = simple_strtoull(buf, &e, 10);
 	unsigned long long old = mddev->suspend_lo;
 
-	if (mddev->pers == NULL || 
+	if (mddev->pers == NULL ||
 	    mddev->pers->quiesce == NULL)
 		return -EINVAL;
 	if (buf == e || (*e && *e != '\n'))
@@ -4510,7 +4320,6 @@ suspend_lo_store(struct mddev *mddev, const char *buf, size_t len)
 static struct md_sysfs_entry md_suspend_lo =
 __ATTR(suspend_lo, S_IRUGO|S_IWUSR, suspend_lo_show, suspend_lo_store);
 
-
 static ssize_t
 suspend_hi_show(struct mddev *mddev, char *page)
 {
@@ -4698,7 +4507,6 @@ static struct attribute_group md_redundancy_group = {
 	.attrs = md_redundancy_attrs,
 };
 
-
 static ssize_t
 md_attr_show(struct kobject *kobj, struct attribute *attr, char *page)
 {
@@ -5111,7 +4919,7 @@ int md_run(struct mddev *mddev)
 	} else if (mddev->ro == 2) /* auto-readonly not meaningful */
 		mddev->ro = 0;
 
- 	atomic_set(&mddev->writes_pending,0);
+	atomic_set(&mddev->writes_pending,0);
 	atomic_set(&mddev->max_corr_read_errors,
 		   MD_DEFAULT_MAX_CORRECTED_READ_ERRORS);
 	mddev->safemode = 0;
@@ -5125,9 +4933,9 @@ int md_run(struct mddev *mddev)
 		if (rdev->raid_disk >= 0)
 			if (sysfs_link_rdev(mddev, rdev))
 				/* failure here is OK */;
-	
+
 	set_bit(MD_RECOVERY_NEEDED, &mddev->recovery);
-	
+
 	if (mddev->flags & MD_UPDATE_SB_FLAGS)
 		md_update_sb(mddev, 0);
 
@@ -5307,7 +5115,7 @@ static int md_set_readonly(struct mddev *mddev, struct block_device *bdev)
 	mddev_lock_nointr(mddev);
 
 	mutex_lock(&mddev->open_mutex);
-	if (atomic_read(&mddev->openers) > !!bdev ||
+	if ((mddev->pers && atomic_read(&mddev->openers) > !!bdev) ||
 	    mddev->sync_thread ||
 	    (bdev && !test_bit(MD_STILL_CLOSED, &mddev->flags))) {
 		printk("md: %s still in use.\n",mdname(mddev));
@@ -5339,7 +5147,7 @@ out:
  *   0 - completely stop and dis-assemble array
  *   2 - stop but do not disassemble array
  */
-static int do_md_stop(struct mddev * mddev, int mode,
+static int do_md_stop(struct mddev *mddev, int mode,
 		      struct block_device *bdev)
 {
 	struct gendisk *disk = mddev->gendisk;
@@ -5362,7 +5170,7 @@ static int do_md_stop(struct mddev * mddev, int mode,
 	mddev_lock_nointr(mddev);
 
 	mutex_lock(&mddev->open_mutex);
-	if (atomic_read(&mddev->openers) > !!bdev ||
+	if ((mddev->pers && atomic_read(&mddev->openers) > !!bdev) ||
 	    mddev->sysfs_active ||
 	    mddev->sync_thread ||
 	    (bdev && !test_bit(MD_STILL_CLOSED, &mddev->flags))) {
@@ -5512,12 +5320,12 @@ static void autorun_devices(int part)
 				"md: cannot allocate memory for md drive.\n");
 			break;
 		}
-		if (mddev_lock(mddev)) 
+		if (mddev_lock(mddev))
 			printk(KERN_WARNING "md: %s locked, cannot run\n",
 			       mdname(mddev));
 		else if (mddev->raid_disks || mddev->major_version
 			 || !list_empty(&mddev->disks)) {
-			printk(KERN_WARNING 
+			printk(KERN_WARNING
 				"md: %s already running, cannot run %s\n",
 				mdname(mddev), bdevname(rdev0->bdev,b));
 			mddev_unlock(mddev);
@@ -5545,7 +5353,7 @@ static void autorun_devices(int part)
 }
 #endif /* !MODULE */
 
-static int get_version(void __user * arg)
+static int get_version(void __user *arg)
 {
 	mdu_version_t ver;
 
@@ -5559,7 +5367,7 @@ static int get_version(void __user * arg)
 	return 0;
 }
 
-static int get_array_info(struct mddev * mddev, void __user * arg)
+static int get_array_info(struct mddev *mddev, void __user *arg)
 {
 	mdu_array_info_t info;
 	int nr,working,insync,failed,spare;
@@ -5574,7 +5382,7 @@ static int get_array_info(struct mddev * mddev, void __user * arg)
 		else {
 			working++;
 			if (test_bit(In_sync, &rdev->flags))
-				insync++;	
+				insync++;
 			else
 				spare++;
 		}
@@ -5614,7 +5422,7 @@ static int get_array_info(struct mddev * mddev, void __user * arg)
 	return 0;
 }
 
-static int get_bitmap_file(struct mddev * mddev, void __user * arg)
+static int get_bitmap_file(struct mddev *mddev, void __user * arg)
 {
 	mdu_bitmap_file_t *file = NULL; /* too big for stack allocation */
 	char *ptr, *buf = NULL;
@@ -5652,7 +5460,7 @@ out:
 	return err;
 }
 
-static int get_disk_info(struct mddev * mddev, void __user * arg)
+static int get_disk_info(struct mddev *mddev, void __user * arg)
 {
 	mdu_disk_info_t info;
 	struct md_rdev *rdev;
@@ -5688,7 +5496,7 @@ static int get_disk_info(struct mddev * mddev, void __user * arg)
 	return 0;
 }
 
-static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
+static int add_new_disk(struct mddev *mddev, mdu_disk_info_t *info)
 {
 	char b[BDEVNAME_SIZE], b2[BDEVNAME_SIZE];
 	struct md_rdev *rdev;
@@ -5702,7 +5510,7 @@ static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
 		/* expecting a device which has a superblock */
 		rdev = md_import_device(dev, mddev->major_version, mddev->minor_version);
 		if (IS_ERR(rdev)) {
-			printk(KERN_WARNING 
+			printk(KERN_WARNING
 				"md: md_import_device returned %ld\n",
 				PTR_ERR(rdev));
 			return PTR_ERR(rdev);
@@ -5714,9 +5522,9 @@ static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
 			err = super_types[mddev->major_version]
 				.load_super(rdev, rdev0, mddev->minor_version);
 			if (err < 0) {
-				printk(KERN_WARNING 
+				printk(KERN_WARNING
 					"md: %s has different UUID to %s\n",
-					bdevname(rdev->bdev,b), 
+					bdevname(rdev->bdev,b),
 					bdevname(rdev0->bdev,b2));
 				export_rdev(rdev);
 				return -EINVAL;
@@ -5736,7 +5544,7 @@ static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
 	if (mddev->pers) {
 		int err;
 		if (!mddev->pers->hot_add_disk) {
-			printk(KERN_WARNING 
+			printk(KERN_WARNING
 				"%s: personality does not support diskops!\n",
 			       mdname(mddev));
 			return -EINVAL;
@@ -5747,7 +5555,7 @@ static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
 		else
 			rdev = md_import_device(dev, -1, -1);
 		if (IS_ERR(rdev)) {
-			printk(KERN_WARNING 
+			printk(KERN_WARNING
 				"md: md_import_device returned %ld\n",
 				PTR_ERR(rdev));
 			return PTR_ERR(rdev);
@@ -5821,7 +5629,7 @@ static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
 		int err;
 		rdev = md_import_device(dev, -1, 0);
 		if (IS_ERR(rdev)) {
-			printk(KERN_WARNING 
+			printk(KERN_WARNING
 				"md: error, md_import_device() returned %ld\n",
 				PTR_ERR(rdev));
 			return PTR_ERR(rdev);
@@ -5856,7 +5664,7 @@ static int add_new_disk(struct mddev * mddev, mdu_disk_info_t *info)
 	return 0;
 }
 
-static int hot_remove_disk(struct mddev * mddev, dev_t dev)
+static int hot_remove_disk(struct mddev *mddev, dev_t dev)
 {
 	char b[BDEVNAME_SIZE];
 	struct md_rdev *rdev;
@@ -5882,7 +5690,7 @@ busy:
 	return -EBUSY;
 }
 
-static int hot_add_disk(struct mddev * mddev, dev_t dev)
+static int hot_add_disk(struct mddev *mddev, dev_t dev)
 {
 	char b[BDEVNAME_SIZE];
 	int err;
@@ -5898,7 +5706,7 @@ static int hot_add_disk(struct mddev * mddev, dev_t dev)
 		return -EINVAL;
 	}
 	if (!mddev->pers->hot_add_disk) {
-		printk(KERN_WARNING 
+		printk(KERN_WARNING
 			"%s: personality does not support diskops!\n",
 			mdname(mddev));
 		return -EINVAL;
@@ -5906,7 +5714,7 @@ static int hot_add_disk(struct mddev * mddev, dev_t dev)
 
 	rdev = md_import_device(dev, -1, 0);
 	if (IS_ERR(rdev)) {
-		printk(KERN_WARNING 
+		printk(KERN_WARNING
 			"md: error, md_import_device() returned %ld\n",
 			PTR_ERR(rdev));
 		return -EINVAL;
@@ -5920,7 +5728,7 @@ static int hot_add_disk(struct mddev * mddev, dev_t dev)
 	rdev->sectors = rdev->sb_start;
 
 	if (test_bit(Faulty, &rdev->flags)) {
-		printk(KERN_WARNING 
+		printk(KERN_WARNING
 			"md: can not hot-add faulty %s disk to %s!\n",
 			bdevname(rdev->bdev,b), mdname(mddev));
 		err = -EINVAL;
@@ -5968,7 +5776,6 @@ static int set_bitmap_file(struct mddev *mddev, int fd)
 		/* we should be able to change the bitmap.. */
 	}
 
-
 	if (fd >= 0) {
 		struct inode *inode;
 		if (mddev->bitmap)
@@ -6039,7 +5846,7 @@ static int set_bitmap_file(struct mddev *mddev, int fd)
  *  The minor and patch _version numbers are also kept incase the
  *  super_block handler wishes to interpret them.
  */
-static int set_array_info(struct mddev * mddev, mdu_array_info_t *info)
+static int set_array_info(struct mddev *mddev, mdu_array_info_t *info)
 {
 
 	if (info->raid_disks == 0) {
@@ -6048,7 +5855,7 @@ static int set_array_info(struct mddev * mddev, mdu_array_info_t *info)
 		    info->major_version >= ARRAY_SIZE(super_types) ||
 		    super_types[info->major_version].name == NULL) {
 			/* maybe try to auto-load a module? */
-			printk(KERN_INFO 
+			printk(KERN_INFO
 				"md: superblock version %d not known\n",
 				info->major_version);
 			return -EINVAL;
@@ -6196,7 +6003,6 @@ static int update_raid_disks(struct mddev *mddev, int raid_disks)
 	return rv;
 }
 
-
 /*
  * update_array_info is used to change the configuration of an
  * on-line array.
@@ -6347,7 +6153,6 @@ static inline bool md_ioctl_valid(unsigned int cmd)
 	case GET_DISK_INFO:
 	case HOT_ADD_DISK:
 	case HOT_REMOVE_DISK:
-	case PRINT_RAID_DEBUG:
 	case RAID_AUTORUN:
 	case RAID_VERSION:
 	case RESTART_ARRAY_RW:
@@ -6391,18 +6196,13 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 	switch (cmd) {
 	case RAID_VERSION:
 		err = get_version(argp);
-		goto done;
-
-	case PRINT_RAID_DEBUG:
-		err = 0;
-		md_print_devices();
-		goto done;
+		goto out;
 
 #ifndef MODULE
 	case RAID_AUTORUN:
 		err = 0;
 		autostart_arrays(arg);
-		goto done;
+		goto out;
 #endif
 	default:;
 	}
@@ -6415,7 +6215,7 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 
 	if (!mddev) {
 		BUG();
-		goto abort;
+		goto out;
 	}
 
 	/* Some actions do not requires the mutex */
@@ -6425,18 +6225,18 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 			err = -ENODEV;
 		else
 			err = get_array_info(mddev, argp);
-		goto abort;
+		goto out;
 
 	case GET_DISK_INFO:
 		if (!mddev->raid_disks && !mddev->external)
 			err = -ENODEV;
 		else
 			err = get_disk_info(mddev, argp);
-		goto abort;
+		goto out;
 
 	case SET_DISK_FAULTY:
 		err = set_disk_faulty(mddev, new_decode_dev(arg));
-		goto abort;
+		goto out;
 	}
 
 	if (cmd == ADD_NEW_DISK)
@@ -6454,10 +6254,10 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 		 * and writes
 		 */
 		mutex_lock(&mddev->open_mutex);
-		if (atomic_read(&mddev->openers) > 1) {
+		if (mddev->pers && atomic_read(&mddev->openers) > 1) {
 			mutex_unlock(&mddev->open_mutex);
 			err = -EBUSY;
-			goto abort;
+			goto out;
 		}
 		set_bit(MD_STILL_CLOSED, &mddev->flags);
 		mutex_unlock(&mddev->open_mutex);
@@ -6465,10 +6265,10 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 	}
 	err = mddev_lock(mddev);
 	if (err) {
-		printk(KERN_INFO 
+		printk(KERN_INFO
 			"md: ioctl lock interrupted, reason %d, cmd %d\n",
 			err, cmd);
-		goto abort;
+		goto out;
 	}
 
 	if (cmd == SET_ARRAY_INFO) {
@@ -6477,38 +6277,38 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 			memset(&info, 0, sizeof(info));
 		else if (copy_from_user(&info, argp, sizeof(info))) {
 			err = -EFAULT;
-			goto abort_unlock;
+			goto unlock;
 		}
 		if (mddev->pers) {
 			err = update_array_info(mddev, &info);
 			if (err) {
 				printk(KERN_WARNING "md: couldn't update"
 				       " array info. %d\n", err);
-				goto abort_unlock;
+				goto unlock;
 			}
-			goto done_unlock;
+			goto unlock;
 		}
 		if (!list_empty(&mddev->disks)) {
 			printk(KERN_WARNING
 			       "md: array %s already has disks!\n",
 			       mdname(mddev));
 			err = -EBUSY;
-			goto abort_unlock;
+			goto unlock;
 		}
 		if (mddev->raid_disks) {
 			printk(KERN_WARNING
 			       "md: array %s already initialised!\n",
 			       mdname(mddev));
 			err = -EBUSY;
-			goto abort_unlock;
+			goto unlock;
 		}
 		err = set_array_info(mddev, &info);
 		if (err) {
 			printk(KERN_WARNING "md: couldn't set"
 			       " array info. %d\n", err);
-			goto abort_unlock;
+			goto unlock;
 		}
-		goto done_unlock;
+		goto unlock;
 	}
 
 	/*
@@ -6521,7 +6321,7 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 	    && cmd != RUN_ARRAY && cmd != SET_BITMAP_FILE
 	    && cmd != GET_BITMAP_FILE) {
 		err = -ENODEV;
-		goto abort_unlock;
+		goto unlock;
 	}
 
 	/*
@@ -6530,23 +6330,23 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 	switch (cmd) {
 	case GET_BITMAP_FILE:
 		err = get_bitmap_file(mddev, argp);
-		goto done_unlock;
+		goto unlock;
 
 	case RESTART_ARRAY_RW:
 		err = restart_array(mddev);
-		goto done_unlock;
+		goto unlock;
 
 	case STOP_ARRAY:
 		err = do_md_stop(mddev, 0, bdev);
-		goto done_unlock;
+		goto unlock;
 
 	case STOP_ARRAY_RO:
 		err = md_set_readonly(mddev, bdev);
-		goto done_unlock;
+		goto unlock;
 
 	case HOT_REMOVE_DISK:
 		err = hot_remove_disk(mddev, new_decode_dev(arg));
-		goto done_unlock;
+		goto unlock;
 
 	case ADD_NEW_DISK:
 		/* We can support ADD_NEW_DISK on read-only arrays
@@ -6562,14 +6362,14 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 				break;
 			else
 				err = add_new_disk(mddev, &info);
-			goto done_unlock;
+			goto unlock;
 		}
 		break;
 
 	case BLKROSET:
 		if (get_user(ro, (int __user *)(arg))) {
 			err = -EFAULT;
-			goto done_unlock;
+			goto unlock;
 		}
 		err = -EINVAL;
 
@@ -6577,11 +6377,11 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 		 * does not matter, no writes are coming
 		 */
 		if (ro)
-			goto done_unlock;
+			goto unlock;
 
 		/* are we are already prepared for writes? */
 		if (mddev->ro != 1)
-			goto done_unlock;
+			goto unlock;
 
 		/* transitioning to readauto need only happen for
 		 * arrays that call md_write_start
@@ -6593,17 +6393,14 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 				set_disk_ro(mddev->gendisk, 0);
 			}
 		}
-		goto done_unlock;
+		goto unlock;
 	}
 
 	/*
 	 * The remaining ioctls are changing the state of the
 	 * superblock, so we do not allow them on read-only arrays.
-	 * However non-MD ioctls (e.g. get-size) will still come through
-	 * here and hit the 'default' below, so only disallow
-	 * 'md' ioctls, and switch to rw mode if started auto-readonly.
 	 */
-	if (_IOC_TYPE(cmd) == MD_MAJOR && mddev->ro && mddev->pers) {
+	if (mddev->ro && mddev->pers) {
 		if (mddev->ro == 2) {
 			mddev->ro = 0;
 			sysfs_notify_dirent_safe(mddev->sysfs_state);
@@ -6621,7 +6418,7 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 			}
 		} else {
 			err = -EROFS;
-			goto abort_unlock;
+			goto unlock;
 		}
 	}
 
@@ -6633,38 +6430,32 @@ static int md_ioctl(struct block_device *bdev, fmode_t mode,
 			err = -EFAULT;
 		else
 			err = add_new_disk(mddev, &info);
-		goto done_unlock;
+		goto unlock;
 	}
 
 	case HOT_ADD_DISK:
 		err = hot_add_disk(mddev, new_decode_dev(arg));
-		goto done_unlock;
+		goto unlock;
 
 	case RUN_ARRAY:
 		err = do_md_run(mddev);
-		goto done_unlock;
+		goto unlock;
 
 	case SET_BITMAP_FILE:
 		err = set_bitmap_file(mddev, (int)arg);
-		goto done_unlock;
+		goto unlock;
 
 	default:
 		err = -EINVAL;
-		goto abort_unlock;
+		goto unlock;
 	}
 
-done_unlock:
-abort_unlock:
+unlock:
 	if (mddev->hold_active == UNTIL_IOCTL &&
 	    err != -EINVAL)
 		mddev->hold_active = 0;
 	mddev_unlock(mddev);
-
-	return err;
-done:
-	if (err)
-		MD_BUG();
-abort:
+out:
 	return err;
 }
 #ifdef CONFIG_COMPAT
@@ -6726,7 +6517,7 @@ static int md_open(struct block_device *bdev, fmode_t mode)
 
 static void md_release(struct gendisk *disk, fmode_t mode)
 {
- 	struct mddev *mddev = disk->private_data;
+	struct mddev *mddev = disk->private_data;
 
 	BUG_ON(!mddev);
 	atomic_dec(&mddev->openers);
@@ -6761,7 +6552,7 @@ static const struct block_device_operations md_fops =
 	.revalidate_disk= md_revalidate,
 };
 
-static int md_thread(void * arg)
+static int md_thread(void *arg)
 {
 	struct md_thread *thread = arg;
 
@@ -6810,6 +6601,7 @@ void md_wakeup_thread(struct md_thread *thread)
 		wake_up(&thread->wqueue);
 	}
 }
+EXPORT_SYMBOL(md_wakeup_thread);
 
 struct md_thread *md_register_thread(void (*run) (struct md_thread *),
 		struct mddev *mddev, const char *name)
@@ -6835,6 +6627,7 @@ struct md_thread *md_register_thread(void (*run) (struct md_thread *),
 	}
 	return thread;
 }
+EXPORT_SYMBOL(md_register_thread);
 
 void md_unregister_thread(struct md_thread **threadp)
 {
@@ -6852,14 +6645,10 @@ void md_unregister_thread(struct md_thread **threadp)
 	kthread_stop(thread->tsk);
 	kfree(thread);
 }
+EXPORT_SYMBOL(md_unregister_thread);
 
 void md_error(struct mddev *mddev, struct md_rdev *rdev)
 {
-	if (!mddev) {
-		MD_BUG();
-		return;
-	}
-
 	if (!rdev || test_bit(Faulty, &rdev->flags))
 		return;
 
@@ -6876,6 +6665,7 @@ void md_error(struct mddev *mddev, struct md_rdev *rdev)
 		queue_work(md_misc_wq, &mddev->event_work);
 	md_new_event_inintr(mddev);
 }
+EXPORT_SYMBOL(md_error);
 
 /* seq_file implementation /proc/mdstat */
 
@@ -6898,8 +6688,7 @@ static void status_unused(struct seq_file *seq)
 	seq_printf(seq, "\n");
 }
 
-
-static void status_resync(struct seq_file *seq, struct mddev * mddev)
+static void status_resync(struct seq_file *seq, struct mddev *mddev)
 {
 	sector_t max_sectors, resync, res;
 	unsigned long dt, db;
@@ -6919,13 +6708,7 @@ static void status_resync(struct seq_file *seq, struct mddev * mddev)
 	else
 		max_sectors = mddev->dev_sectors;
 
-	/*
-	 * Should not happen.
-	 */
-	if (!max_sectors) {
-		MD_BUG();
-		return;
-	}
+	WARN_ON(max_sectors == 0);
 	/* Pick 'scale' such that (resync>>scale)*1000 will fit
 	 * in a sector_t, and (max_sectors>>scale) will fit in a
 	 * u32, as those are the requirements for sector_div.
@@ -7021,7 +6804,7 @@ static void *md_seq_next(struct seq_file *seq, void *v, loff_t *pos)
 {
 	struct list_head *tmp;
 	struct mddev *next_mddev, *mddev = v;
-	
+
 	++*pos;
 	if (v == (void*)2)
 		return NULL;
@@ -7036,7 +6819,7 @@ static void *md_seq_next(struct seq_file *seq, void *v, loff_t *pos)
 	else {
 		next_mddev = (void*)2;
 		*pos = 0x10000;
-	}		
+	}
 	spin_unlock(&all_mddevs_lock);
 
 	if (v != (void*)1)
@@ -7132,7 +6915,7 @@ static int md_seq_show(struct seq_file *seq, void *v)
 
 		if (mddev->pers) {
 			mddev->pers->status(seq, mddev);
-	 		seq_printf(seq, "\n      ");
+			seq_printf(seq, "\n      ");
 			if (mddev->pers->sync_request) {
 				if (mddev->curr_resync > 2) {
 					status_resync(seq, mddev);
@@ -7150,7 +6933,7 @@ static int md_seq_show(struct seq_file *seq, void *v)
 		seq_printf(seq, "\n");
 	}
 	mddev_unlock(mddev);
-	
+
 	return 0;
 }
 
@@ -7204,12 +6987,14 @@ static const struct file_operations md_seq_fops = {
 
 int register_md_personality(struct md_personality *p)
 {
+	printk(KERN_INFO "md: %s personality registered for level %d\n",
+						p->name, p->level);
 	spin_lock(&pers_lock);
 	list_add_tail(&p->list, &pers_list);
-	printk(KERN_INFO "md: %s personality registered for level %d\n", p->name, p->level);
 	spin_unlock(&pers_lock);
 	return 0;
 }
+EXPORT_SYMBOL(register_md_personality);
 
 int unregister_md_personality(struct md_personality *p)
 {
@@ -7219,10 +7004,11 @@ int unregister_md_personality(struct md_personality *p)
 	spin_unlock(&pers_lock);
 	return 0;
 }
+EXPORT_SYMBOL(unregister_md_personality);
 
 static int is_mddev_idle(struct mddev *mddev, int init)
 {
-	struct md_rdev * rdev;
+	struct md_rdev *rdev;
 	int idle;
 	int curr_events;
 
@@ -7276,7 +7062,7 @@ void md_done_sync(struct mddev *mddev, int blocks, int ok)
 		// stop recovery, signal do_sync ....
 	}
 }
-
+EXPORT_SYMBOL(md_done_sync);
 
 /* md_write_start(mddev, bi)
  * If we need to update some array metadata (e.g. 'active' flag
@@ -7317,6 +7103,7 @@ void md_write_start(struct mddev *mddev, struct bio *bi)
 	wait_event(mddev->sb_wait,
 		   !test_bit(MD_CHANGE_PENDING, &mddev->flags));
 }
+EXPORT_SYMBOL(md_write_start);
 
 void md_write_end(struct mddev *mddev)
 {
@@ -7327,6 +7114,7 @@ void md_write_end(struct mddev *mddev)
 			mod_timer(&mddev->safemode_timer, jiffies + mddev->safemode_delay);
 	}
 }
+EXPORT_SYMBOL(md_write_end);
 
 /* md_allow_write(mddev)
  * Calling this ensures that the array is marked 'active' so that writes
@@ -7784,6 +7572,33 @@ no_add:
 	return spares;
 }
 
+static void md_start_sync(struct work_struct *ws)
+{
+	struct mddev *mddev = container_of(ws, struct mddev, del_work);
+
+	mddev->sync_thread = md_register_thread(md_do_sync,
+						mddev,
+						"resync");
+	if (!mddev->sync_thread) {
+		printk(KERN_ERR "%s: could not start resync"
+		       " thread...\n",
+		       mdname(mddev));
+		/* leave the spares where they are, it shouldn't hurt */
+		clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
+		clear_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
+		clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
+		clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
+		clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
+		if (test_and_clear_bit(MD_RECOVERY_RECOVER,
+				       &mddev->recovery))
+			if (mddev->sysfs_action)
+				sysfs_notify_dirent_safe(mddev->sysfs_action);
+	} else
+		md_wakeup_thread(mddev->sync_thread);
+	sysfs_notify_dirent_safe(mddev->sysfs_action);
+	md_new_event(mddev);
+}
+
 /*
  * This routine is regularly called by all per-raid-array threads to
  * deal with generic issues like resync and super-block update.
@@ -7900,7 +7715,7 @@ void md_check_recovery(struct mddev *mddev)
 
 		if (!test_and_clear_bit(MD_RECOVERY_NEEDED, &mddev->recovery) ||
 		    test_bit(MD_RECOVERY_FROZEN, &mddev->recovery))
-			goto unlock;
+			goto not_running;
 		/* no recovery is running.
 		 * remove any failed drives, then
 		 * add spares if possible.
@@ -7912,7 +7727,7 @@ void md_check_recovery(struct mddev *mddev)
 			if (mddev->pers->check_reshape == NULL ||
 			    mddev->pers->check_reshape(mddev) != 0)
 				/* Cannot proceed */
-				goto unlock;
+				goto not_running;
 			set_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
 			clear_bit(MD_RECOVERY_RECOVER, &mddev->recovery);
 		} else if ((spares = remove_and_add_spares(mddev, NULL))) {
@@ -7925,7 +7740,7 @@ void md_check_recovery(struct mddev *mddev)
 			clear_bit(MD_RECOVERY_RECOVER, &mddev->recovery);
 		} else if (!test_bit(MD_RECOVERY_SYNC, &mddev->recovery))
 			/* nothing to be done ... */
-			goto unlock;
+			goto not_running;
 
 		if (mddev->pers->sync_request) {
 			if (spares) {
@@ -7935,27 +7750,11 @@ void md_check_recovery(struct mddev *mddev)
 				 */
 				bitmap_write_all(mddev->bitmap);
 			}
-			mddev->sync_thread = md_register_thread(md_do_sync,
-								mddev,
-								"resync");
-			if (!mddev->sync_thread) {
-				printk(KERN_ERR "%s: could not start resync"
-					" thread...\n", 
-					mdname(mddev));
-				/* leave the spares where they are, it shouldn't hurt */
-				clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
-				clear_bit(MD_RECOVERY_SYNC, &mddev->recovery);
-				clear_bit(MD_RECOVERY_RESHAPE, &mddev->recovery);
-				clear_bit(MD_RECOVERY_REQUESTED, &mddev->recovery);
-				clear_bit(MD_RECOVERY_CHECK, &mddev->recovery);
-			} else
-				md_wakeup_thread(mddev->sync_thread);
-			sysfs_notify_dirent_safe(mddev->sysfs_action);
-			md_new_event(mddev);
+			INIT_WORK(&mddev->del_work, md_start_sync);
+			queue_work(md_misc_wq, &mddev->del_work);
+			goto unlock;
 		}
-	unlock:
-		wake_up(&mddev->sb_wait);
-
+	not_running:
 		if (!mddev->sync_thread) {
 			clear_bit(MD_RECOVERY_RUNNING, &mddev->recovery);
 			if (test_and_clear_bit(MD_RECOVERY_RECOVER,
@@ -7963,9 +7762,12 @@ void md_check_recovery(struct mddev *mddev)
 				if (mddev->sysfs_action)
 					sysfs_notify_dirent_safe(mddev->sysfs_action);
 		}
+	unlock:
+		wake_up(&mddev->sb_wait);
 		mddev_unlock(mddev);
 	}
 }
+EXPORT_SYMBOL(md_check_recovery);
 
 void md_reap_sync_thread(struct mddev *mddev)
 {
@@ -8008,6 +7810,7 @@ void md_reap_sync_thread(struct mddev *mddev)
 	if (mddev->event_work.func)
 		queue_work(md_misc_wq, &mddev->event_work);
 }
+EXPORT_SYMBOL(md_reap_sync_thread);
 
 void md_wait_for_blocked_rdev(struct md_rdev *rdev, struct mddev *mddev)
 {
@@ -8641,7 +8444,6 @@ void md_autodetect_dev(dev_t dev)
 	}
 }
 
-
 static void autostart_arrays(int part)
 {
 	struct md_rdev *rdev;
@@ -8665,10 +8467,9 @@ static void autostart_arrays(int part)
 		if (IS_ERR(rdev))
 			continue;
 
-		if (test_bit(Faulty, &rdev->flags)) {
-			MD_BUG();
+		if (test_bit(Faulty, &rdev->flags))
 			continue;
-		}
+
 		set_bit(AutoDetected, &rdev->flags);
 		list_add(&rdev->same_set, &pending_raid_disks);
 		i_passed++;
@@ -8736,20 +8537,8 @@ static int set_ro(const char *val, struct kernel_param *kp)
 
 module_param_call(start_ro, set_ro, get_ro, NULL, S_IRUSR|S_IWUSR);
 module_param(start_dirty_degraded, int, S_IRUGO|S_IWUSR);
-
 module_param_call(new_array, add_named_array, NULL, NULL, S_IWUSR);
 
-EXPORT_SYMBOL(register_md_personality);
-EXPORT_SYMBOL(unregister_md_personality);
-EXPORT_SYMBOL(md_error);
-EXPORT_SYMBOL(md_done_sync);
-EXPORT_SYMBOL(md_write_start);
-EXPORT_SYMBOL(md_write_end);
-EXPORT_SYMBOL(md_register_thread);
-EXPORT_SYMBOL(md_unregister_thread);
-EXPORT_SYMBOL(md_wakeup_thread);
-EXPORT_SYMBOL(md_check_recovery);
-EXPORT_SYMBOL(md_reap_sync_thread);
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("MD RAID framework");
 MODULE_ALIAS("md");
diff --git a/drivers/md/md.h b/drivers/md/md.h
index a49d991f3fe1..03cec5bdcaae 100644
--- a/drivers/md/md.h
+++ b/drivers/md/md.h
@@ -1,15 +1,15 @@
 /*
    md.h : kernel internal structure of the Linux MD driver
           Copyright (C) 1996-98 Ingo Molnar, Gadi Oxman
-	  
+
    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2, or (at your option)
    any later version.
-   
+
    You should have received a copy of the GNU General Public License
    (for example /usr/src/linux/COPYING); if not, write to the Free
-   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.  
+   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
 #ifndef _MD_MD_H
@@ -56,7 +56,7 @@ struct md_rdev {
 	__u64		sb_events;
 	sector_t	data_offset;	/* start of data in array */
 	sector_t	new_data_offset;/* only relevant while reshaping */
-	sector_t 	sb_start;	/* offset of the super block (in 512byte sectors) */
+	sector_t	sb_start;	/* offset of the super block (in 512byte sectors) */
 	int		sb_size;	/* bytes in the superblock */
 	int		preferred_minor;	/* autorun support */
 
@@ -239,7 +239,7 @@ struct mddev {
 					minor_version,
 					patch_version;
 	int				persistent;
-	int 				external;	/* metadata is
+	int				external;	/* metadata is
 							 * managed externally */
 	char				metadata_type[17]; /* externally set*/
 	int				chunk_sectors;
@@ -248,7 +248,7 @@ struct mddev {
 	char				clevel[16];
 	int				raid_disks;
 	int				max_disks;
-	sector_t			dev_sectors; 	/* used size of
+	sector_t			dev_sectors;	/* used size of
 							 * component devices */
 	sector_t			array_sectors; /* exported array size */
 	int				external_size; /* size managed
@@ -312,7 +312,7 @@ struct mddev {
 	int				parallel_resync;
 
 	int				ok_start_degraded;
-	/* recovery/resync flags 
+	/* recovery/resync flags
 	 * NEEDED:   we might need to start a resync/recover
 	 * RUNNING:  a thread is running, or about to be started
 	 * SYNC:     actually doing a resync, not a recovery
@@ -392,20 +392,20 @@ struct mddev {
 
 	unsigned int			safemode;	/* if set, update "clean" superblock
 							 * when no writes pending.
-							 */ 
+							 */
 	unsigned int			safemode_delay;
 	struct timer_list		safemode_timer;
-	atomic_t			writes_pending; 
+	atomic_t			writes_pending;
 	struct request_queue		*queue;	/* for plugging ... */
 
-	struct bitmap                   *bitmap; /* the bitmap for the device */
+	struct bitmap			*bitmap; /* the bitmap for the device */
 	struct {
 		struct file		*file; /* the bitmap file */
 		loff_t			offset; /* offset from superblock of
 						 * start of bitmap. May be
 						 * negative, but not '0'
 						 * For external metadata, offset
-						 * from start of device. 
+						 * from start of device.
 						 */
 		unsigned long		space; /* space available at this offset */
 		loff_t			default_offset; /* this is the offset to use when
@@ -421,7 +421,7 @@ struct mddev {
 		int			external;
 	} bitmap_info;
 
-	atomic_t 			max_corr_read_errors; /* max read retries */
+	atomic_t			max_corr_read_errors; /* max read retries */
 	struct list_head		all_mddevs;
 
 	struct attribute_group		*to_remove;
@@ -439,7 +439,6 @@ struct mddev {
 	void (*sync_super)(struct mddev *mddev, struct md_rdev *rdev);
 };
 
-
 static inline void rdev_dec_pending(struct md_rdev *rdev, struct mddev *mddev)
 {
 	int faulty = test_bit(Faulty, &rdev->flags);
@@ -449,7 +448,7 @@ static inline void rdev_dec_pending(struct md_rdev *rdev, struct mddev *mddev)
 
 static inline void md_sync_acct(struct block_device *bdev, unsigned long nr_sectors)
 {
-        atomic_add(nr_sectors, &bdev->bd_contains->bd_disk->sync_io);
+	atomic_add(nr_sectors, &bdev->bd_contains->bd_disk->sync_io);
 }
 
 struct md_personality
@@ -463,7 +462,7 @@ struct md_personality
 	int (*stop)(struct mddev *mddev);
 	void (*status)(struct seq_file *seq, struct mddev *mddev);
 	/* error_handler must set ->faulty and clear ->in_sync
-	 * if appropriate, and should abort recovery if needed 
+	 * if appropriate, and should abort recovery if needed
 	 */
 	void (*error_handler)(struct mddev *mddev, struct md_rdev *rdev);
 	int (*hot_add_disk) (struct mddev *mddev, struct md_rdev *rdev);
@@ -493,7 +492,6 @@ struct md_personality
 	void *(*takeover) (struct mddev *mddev);
 };
 
-
 struct md_sysfs_entry {
 	struct attribute attr;
 	ssize_t (*show)(struct mddev *, char *);
@@ -560,7 +558,7 @@ struct md_thread {
 	void			(*run) (struct md_thread *thread);
 	struct mddev		*mddev;
 	wait_queue_head_t	wqueue;
-	unsigned long           flags;
+	unsigned long		flags;
 	struct task_struct	*tsk;
 	unsigned long		timeout;
 	void			*private;
@@ -594,7 +592,7 @@ extern void md_flush_request(struct mddev *mddev, struct bio *bio);
 extern void md_super_write(struct mddev *mddev, struct md_rdev *rdev,
 			   sector_t sector, int size, struct page *page);
 extern void md_super_wait(struct mddev *mddev);
-extern int sync_page_io(struct md_rdev *rdev, sector_t sector, int size, 
+extern int sync_page_io(struct md_rdev *rdev, sector_t sector, int size,
 			struct page *page, int rw, bool metadata_op);
 extern void md_do_sync(struct md_thread *thread);
 extern void md_new_event(struct mddev *mddev);
diff --git a/drivers/md/multipath.c b/drivers/md/multipath.c
index 849ad39f547b..399272f9c042 100644
--- a/drivers/md/multipath.c
+++ b/drivers/md/multipath.c
@@ -31,13 +31,12 @@
 
 #define	NR_RESERVED_BUFS	32
 
-
 static int multipath_map (struct mpconf *conf)
 {
 	int i, disks = conf->raid_disks;
 
 	/*
-	 * Later we do read balancing on the read side 
+	 * Later we do read balancing on the read side
 	 * now we use the first available disk.
 	 */
 
@@ -68,7 +67,6 @@ static void multipath_reschedule_retry (struct multipath_bh *mp_bh)
 	md_wakeup_thread(mddev->thread);
 }
 
-
 /*
  * multipath_end_bh_io() is called when we have finished servicing a multipathed
  * operation and are ready to return a success/failure code to the buffer
@@ -98,8 +96,8 @@ static void multipath_end_request(struct bio *bio, int error)
 		 */
 		char b[BDEVNAME_SIZE];
 		md_error (mp_bh->mddev, rdev);
-		printk(KERN_ERR "multipath: %s: rescheduling sector %llu\n", 
-		       bdevname(rdev->bdev,b), 
+		printk(KERN_ERR "multipath: %s: rescheduling sector %llu\n",
+		       bdevname(rdev->bdev,b),
 		       (unsigned long long)bio->bi_iter.bi_sector);
 		multipath_reschedule_retry(mp_bh);
 	} else
@@ -145,12 +143,12 @@ static void multipath_status (struct seq_file *seq, struct mddev *mddev)
 {
 	struct mpconf *conf = mddev->private;
 	int i;
-	
+
 	seq_printf (seq, " [%d/%d] [", conf->raid_disks,
 		    conf->raid_disks - mddev->degraded);
 	for (i = 0; i < conf->raid_disks; i++)
 		seq_printf (seq, "%s",
-			       conf->multipaths[i].rdev && 
+			       conf->multipaths[i].rdev &&
 			       test_bit(In_sync, &conf->multipaths[i].rdev->flags) ? "U" : "_");
 	seq_printf (seq, "]");
 }
@@ -195,7 +193,7 @@ static void multipath_error (struct mddev *mddev, struct md_rdev *rdev)
 		 * first check if this is a queued request for a device
 		 * which has just failed.
 		 */
-		printk(KERN_ALERT 
+		printk(KERN_ALERT
 		       "multipath: only one IO path left and IO error.\n");
 		/* leave it active... it's all we have */
 		return;
@@ -242,7 +240,6 @@ static void print_multipath_conf (struct mpconf *conf)
 	}
 }
 
-
 static int multipath_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 {
 	struct mpconf *conf = mddev->private;
@@ -325,8 +322,6 @@ abort:
 	return err;
 }
 
-
-
 /*
  * This is a kernel thread which:
  *
@@ -356,7 +351,7 @@ static void multipathd(struct md_thread *thread)
 
 		bio = &mp_bh->bio;
 		bio->bi_iter.bi_sector = mp_bh->master_bio->bi_iter.bi_sector;
-		
+
 		if ((mp_bh->path = multipath_map (conf))<0) {
 			printk(KERN_ALERT "multipath: %s: unrecoverable IO read"
 				" error for block %llu\n",
@@ -414,7 +409,7 @@ static int multipath_run (struct mddev *mddev)
 	conf = kzalloc(sizeof(struct mpconf), GFP_KERNEL);
 	mddev->private = conf;
 	if (!conf) {
-		printk(KERN_ERR 
+		printk(KERN_ERR
 			"multipath: couldn't allocate memory for %s\n",
 			mdname(mddev));
 		goto out;
@@ -423,7 +418,7 @@ static int multipath_run (struct mddev *mddev)
 	conf->multipaths = kzalloc(sizeof(struct multipath_info)*mddev->raid_disks,
 				   GFP_KERNEL);
 	if (!conf->multipaths) {
-		printk(KERN_ERR 
+		printk(KERN_ERR
 			"multipath: couldn't allocate memory for %s\n",
 			mdname(mddev));
 		goto out_free_conf;
@@ -469,7 +464,7 @@ static int multipath_run (struct mddev *mddev)
 	conf->pool = mempool_create_kmalloc_pool(NR_RESERVED_BUFS,
 						 sizeof(struct multipath_bh));
 	if (conf->pool == NULL) {
-		printk(KERN_ERR 
+		printk(KERN_ERR
 			"multipath: couldn't allocate memory for %s\n",
 			mdname(mddev));
 		goto out_free_conf;
@@ -485,7 +480,7 @@ static int multipath_run (struct mddev *mddev)
 		}
 	}
 
-	printk(KERN_INFO 
+	printk(KERN_INFO
 		"multipath: array %s active with %d out of %d IO paths\n",
 		mdname(mddev), conf->raid_disks - mddev->degraded,
 	       mddev->raid_disks);
@@ -512,7 +507,6 @@ out:
 	return -EIO;
 }
 
-
 static int multipath_stop (struct mddev *mddev)
 {
 	struct mpconf *conf = mddev->private;
diff --git a/drivers/md/raid0.c b/drivers/md/raid0.c
index cf91f5910c7c..ba6b85de96d2 100644
--- a/drivers/md/raid0.c
+++ b/drivers/md/raid0.c
@@ -1,10 +1,9 @@
 /*
    raid0.c : Multiple Devices driver for Linux
-             Copyright (C) 1994-96 Marc ZYNGIER
+	     Copyright (C) 1994-96 Marc ZYNGIER
 	     <zyngier@ufr-info-p7.ibp.fr> or
 	     <maz@gloups.fdn.fr>
-             Copyright (C) 1999, 2000 Ingo Molnar, Red Hat
-
+	     Copyright (C) 1999, 2000 Ingo Molnar, Red Hat
 
    RAID-0 management functions.
 
@@ -12,10 +11,10 @@
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2, or (at your option)
    any later version.
-   
+
    You should have received a copy of the GNU General Public License
    (for example /usr/src/linux/COPYING); if not, write to the Free
-   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.  
+   Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 
 #include <linux/blkdev.h>
diff --git a/drivers/md/raid1.c b/drivers/md/raid1.c
index d7690f86fdb9..40b35be34f8d 100644
--- a/drivers/md/raid1.c
+++ b/drivers/md/raid1.c
@@ -494,7 +494,6 @@ static void raid1_end_write_request(struct bio *bio, int error)
 		bio_put(to_put);
 }
 
-
 /*
  * This routine returns the disk from which the requested read should
  * be done. There is a per-array 'next expected sequential IO' sector
@@ -540,11 +539,7 @@ static int read_balance(struct r1conf *conf, struct r1bio *r1_bio, int *max_sect
 	has_nonrot_disk = 0;
 	choose_next_idle = 0;
 
-	if (conf->mddev->recovery_cp < MaxSector &&
-	    (this_sector + sectors >= conf->next_resync))
-		choose_first = 1;
-	else
-		choose_first = 0;
+	choose_first = (conf->mddev->recovery_cp < this_sector + sectors);
 
 	for (disk = 0 ; disk < conf->raid_disks * 2 ; disk++) {
 		sector_t dist;
@@ -831,7 +826,7 @@ static void flush_pending_writes(struct r1conf *conf)
  *    there is no normal IO happeing.  It must arrange to call
  *    lower_barrier when the particular background IO completes.
  */
-static void raise_barrier(struct r1conf *conf)
+static void raise_barrier(struct r1conf *conf, sector_t sector_nr)
 {
 	spin_lock_irq(&conf->resync_lock);
 
@@ -841,6 +836,7 @@ static void raise_barrier(struct r1conf *conf)
 
 	/* block any new IO from starting */
 	conf->barrier++;
+	conf->next_resync = sector_nr;
 
 	/* For these conditions we must wait:
 	 * A: while the array is in frozen state
@@ -849,14 +845,17 @@ static void raise_barrier(struct r1conf *conf)
 	 * C: next_resync + RESYNC_SECTORS > start_next_window, meaning
 	 *    next resync will reach to the window which normal bios are
 	 *    handling.
+	 * D: while there are any active requests in the current window.
 	 */
 	wait_event_lock_irq(conf->wait_barrier,
 			    !conf->array_frozen &&
 			    conf->barrier < RESYNC_DEPTH &&
+			    conf->current_window_requests == 0 &&
 			    (conf->start_next_window >=
 			     conf->next_resync + RESYNC_SECTORS),
 			    conf->resync_lock);
 
+	conf->nr_pending++;
 	spin_unlock_irq(&conf->resync_lock);
 }
 
@@ -866,6 +865,7 @@ static void lower_barrier(struct r1conf *conf)
 	BUG_ON(conf->barrier <= 0);
 	spin_lock_irqsave(&conf->resync_lock, flags);
 	conf->barrier--;
+	conf->nr_pending--;
 	spin_unlock_irqrestore(&conf->resync_lock, flags);
 	wake_up(&conf->wait_barrier);
 }
@@ -877,12 +877,10 @@ static bool need_to_wait_for_sync(struct r1conf *conf, struct bio *bio)
 	if (conf->array_frozen || !bio)
 		wait = true;
 	else if (conf->barrier && bio_data_dir(bio) == WRITE) {
-		if (conf->next_resync < RESYNC_WINDOW_SECTORS)
-			wait = true;
-		else if ((conf->next_resync - RESYNC_WINDOW_SECTORS
-				>= bio_end_sector(bio)) ||
-			 (conf->next_resync + NEXT_NORMALIO_DISTANCE
-				<= bio->bi_iter.bi_sector))
+		if ((conf->mddev->curr_resync_completed
+		     >= bio_end_sector(bio)) ||
+		    (conf->next_resync + NEXT_NORMALIO_DISTANCE
+		     <= bio->bi_iter.bi_sector))
 			wait = false;
 		else
 			wait = true;
@@ -902,25 +900,25 @@ static sector_t wait_barrier(struct r1conf *conf, struct bio *bio)
 		 * However if there are already pending
 		 * requests (preventing the barrier from
 		 * rising completely), and the
-		 * pre-process bio queue isn't empty,
+		 * per-process bio queue isn't empty,
 		 * then don't wait, as we need to empty
-		 * that queue to get the nr_pending
-		 * count down.
+		 * that queue to allow conf->start_next_window
+		 * to increase.
 		 */
 		wait_event_lock_irq(conf->wait_barrier,
 				    !conf->array_frozen &&
 				    (!conf->barrier ||
-				    ((conf->start_next_window <
-				      conf->next_resync + RESYNC_SECTORS) &&
-				     current->bio_list &&
-				     !bio_list_empty(current->bio_list))),
+				     ((conf->start_next_window <
+				       conf->next_resync + RESYNC_SECTORS) &&
+				      current->bio_list &&
+				      !bio_list_empty(current->bio_list))),
 				    conf->resync_lock);
 		conf->nr_waiting--;
 	}
 
 	if (bio && bio_data_dir(bio) == WRITE) {
-		if (conf->next_resync + NEXT_NORMALIO_DISTANCE
-		    <= bio->bi_iter.bi_sector) {
+		if (bio->bi_iter.bi_sector >=
+		    conf->mddev->curr_resync_completed) {
 			if (conf->start_next_window == MaxSector)
 				conf->start_next_window =
 					conf->next_resync +
@@ -1002,8 +1000,7 @@ static void unfreeze_array(struct r1conf *conf)
 	spin_unlock_irq(&conf->resync_lock);
 }
 
-
-/* duplicate the data pages for behind I/O 
+/* duplicate the data pages for behind I/O
  */
 static void alloc_behind_pages(struct bio *bio, struct r1bio *r1_bio)
 {
@@ -1186,6 +1183,7 @@ read_again:
 				   atomic_read(&bitmap->behind_writes) == 0);
 		}
 		r1_bio->read_disk = rdisk;
+		r1_bio->start_next_window = 0;
 
 		read_bio = bio_clone_mddev(bio, GFP_NOIO, mddev);
 		bio_trim(read_bio, r1_bio->sector - bio->bi_iter.bi_sector,
@@ -1471,7 +1469,6 @@ static void status(struct seq_file *seq, struct mddev *mddev)
 	seq_printf(seq, "]");
 }
 
-
 static void error(struct mddev *mddev, struct md_rdev *rdev)
 {
 	char b[BDEVNAME_SIZE];
@@ -1548,8 +1545,13 @@ static void close_sync(struct r1conf *conf)
 	mempool_destroy(conf->r1buf_pool);
 	conf->r1buf_pool = NULL;
 
+	spin_lock_irq(&conf->resync_lock);
 	conf->next_resync = 0;
 	conf->start_next_window = MaxSector;
+	conf->current_window_requests +=
+		conf->next_window_requests;
+	conf->next_window_requests = 0;
+	spin_unlock_irq(&conf->resync_lock);
 }
 
 static int raid1_spare_active(struct mddev *mddev)
@@ -1560,7 +1562,7 @@ static int raid1_spare_active(struct mddev *mddev)
 	unsigned long flags;
 
 	/*
-	 * Find all failed disks within the RAID1 configuration 
+	 * Find all failed disks within the RAID1 configuration
 	 * and mark them readable.
 	 * Called under mddev lock, so rcu protection not needed.
 	 */
@@ -1601,7 +1603,6 @@ static int raid1_spare_active(struct mddev *mddev)
 	return count;
 }
 
-
 static int raid1_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 {
 	struct r1conf *conf = mddev->private;
@@ -1730,7 +1731,6 @@ abort:
 	return err;
 }
 
-
 static void end_sync_read(struct bio *bio, int error)
 {
 	struct r1bio *r1_bio = bio->bi_private;
@@ -1942,7 +1942,7 @@ static int fix_sync_read_error(struct r1bio *r1_bio)
 	return 1;
 }
 
-static int process_checks(struct r1bio *r1_bio)
+static void process_checks(struct r1bio *r1_bio)
 {
 	/* We have read all readable devices.  If we haven't
 	 * got the block, then there is no hope left.
@@ -2034,7 +2034,6 @@ static int process_checks(struct r1bio *r1_bio)
 
 		bio_copy_data(sbio, pbio);
 	}
-	return 0;
 }
 
 static void sync_request_write(struct mddev *mddev, struct r1bio *r1_bio)
@@ -2052,8 +2051,8 @@ static void sync_request_write(struct mddev *mddev, struct r1bio *r1_bio)
 			return;
 
 	if (test_bit(MD_RECOVERY_REQUESTED, &mddev->recovery))
-		if (process_checks(r1_bio) < 0)
-			return;
+		process_checks(r1_bio);
+
 	/*
 	 * schedule writes
 	 */
@@ -2150,7 +2149,7 @@ static void fix_read_error(struct r1conf *conf, int read_disk,
 			d--;
 			rdev = conf->mirrors[d].rdev;
 			if (rdev &&
-			    test_bit(In_sync, &rdev->flags))
+			    !test_bit(Faulty, &rdev->flags))
 				r1_sync_page_io(rdev, sect, s,
 						conf->tmppage, WRITE);
 		}
@@ -2162,7 +2161,7 @@ static void fix_read_error(struct r1conf *conf, int read_disk,
 			d--;
 			rdev = conf->mirrors[d].rdev;
 			if (rdev &&
-			    test_bit(In_sync, &rdev->flags)) {
+			    !test_bit(Faulty, &rdev->flags)) {
 				if (r1_sync_page_io(rdev, sect, s,
 						    conf->tmppage, READ)) {
 					atomic_add(s, &rdev->corrected_errors);
@@ -2453,7 +2452,6 @@ static void raid1d(struct md_thread *thread)
 	blk_finish_plug(&plug);
 }
 
-
 static int init_resync(struct r1conf *conf)
 {
 	int buffs;
@@ -2541,9 +2539,8 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipp
 
 	bitmap_cond_end_sync(mddev->bitmap, sector_nr);
 	r1_bio = mempool_alloc(conf->r1buf_pool, GFP_NOIO);
-	raise_barrier(conf);
 
-	conf->next_resync = sector_nr;
+	raise_barrier(conf, sector_nr);
 
 	rcu_read_lock();
 	/*
@@ -2718,7 +2715,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr, int *skipp
 						/* remove last page from this bio */
 						bio->bi_vcnt--;
 						bio->bi_iter.bi_size -= len;
-						bio->bi_flags &= ~(1<< BIO_SEG_VALID);
+						__clear_bit(BIO_SEG_VALID, &bio->bi_flags);
 					}
 					goto bio_full;
 				}
@@ -2943,9 +2940,9 @@ static int run(struct mddev *mddev)
 		printk(KERN_NOTICE "md/raid1:%s: not clean"
 		       " -- starting background reconstruction\n",
 		       mdname(mddev));
-	printk(KERN_INFO 
+	printk(KERN_INFO
 		"md/raid1:%s: active with %d out of %d mirrors\n",
-		mdname(mddev), mddev->raid_disks - mddev->degraded, 
+		mdname(mddev), mddev->raid_disks - mddev->degraded,
 		mddev->raid_disks);
 
 	/*
diff --git a/drivers/md/raid1.h b/drivers/md/raid1.h
index 9bebca7bff2f..33bda55ef9f7 100644
--- a/drivers/md/raid1.h
+++ b/drivers/md/raid1.h
@@ -90,7 +90,6 @@ struct r1conf {
 	 */
 	int			recovery_disabled;
 
-
 	/* poolinfo contains information about the content of the
 	 * mempools - it changes when the array grows or shrinks
 	 */
@@ -103,7 +102,6 @@ struct r1conf {
 	 */
 	struct page		*tmppage;
 
-
 	/* When taking over an array from a different personality, we store
 	 * the new thread here until we fully activate the array.
 	 */
diff --git a/drivers/md/raid10.c b/drivers/md/raid10.c
index b08c18871323..32e282f4c83c 100644
--- a/drivers/md/raid10.c
+++ b/drivers/md/raid10.c
@@ -366,7 +366,6 @@ static void raid10_end_read_request(struct bio *bio, int error)
 	struct md_rdev *rdev;
 	struct r10conf *conf = r10_bio->mddev->private;
 
-
 	slot = r10_bio->read_slot;
 	dev = r10_bio->devs[slot].devnum;
 	rdev = r10_bio->devs[slot].rdev;
@@ -1559,7 +1558,6 @@ static void make_request(struct mddev *mddev, struct bio *bio)
 
 	md_write_start(mddev, bio);
 
-
 	do {
 
 		/*
@@ -1782,7 +1780,6 @@ static int raid10_spare_active(struct mddev *mddev)
 	return count;
 }
 
-
 static int raid10_add_disk(struct mddev *mddev, struct md_rdev *rdev)
 {
 	struct r10conf *conf = mddev->private;
@@ -1929,7 +1926,6 @@ abort:
 	return err;
 }
 
-
 static void end_sync_read(struct bio *bio, int error)
 {
 	struct r10bio *r10_bio = bio->bi_private;
@@ -2295,7 +2291,6 @@ static void recovery_request_write(struct mddev *mddev, struct r10bio *r10_bio)
 	}
 }
 
-
 /*
  * Used by fix_read_error() to decay the per rdev read_errors.
  * We halve the read error count for every hour that has elapsed
@@ -2852,7 +2847,6 @@ static void raid10d(struct md_thread *thread)
 	blk_finish_plug(&plug);
 }
 
-
 static int init_resync(struct r10conf *conf)
 {
 	int buffs;
@@ -2953,6 +2947,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		 */
 		if (test_bit(MD_RECOVERY_RESHAPE, &mddev->recovery)) {
 			end_reshape(conf);
+			close_sync(conf);
 			return 0;
 		}
 
@@ -3081,6 +3076,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 			}
 
 			r10_bio = mempool_alloc(conf->r10buf_pool, GFP_NOIO);
+			r10_bio->state = 0;
 			raise_barrier(conf, rb2 != NULL);
 			atomic_set(&r10_bio->remaining, 0);
 
@@ -3269,6 +3265,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 		if (sync_blocks < max_sync)
 			max_sync = sync_blocks;
 		r10_bio = mempool_alloc(conf->r10buf_pool, GFP_NOIO);
+		r10_bio->state = 0;
 
 		r10_bio->mddev = mddev;
 		atomic_set(&r10_bio->remaining, 0);
@@ -3385,7 +3382,7 @@ static sector_t sync_request(struct mddev *mddev, sector_t sector_nr,
 				/* remove last page from this bio */
 				bio2->bi_vcnt--;
 				bio2->bi_iter.bi_size -= len;
-				bio2->bi_flags &= ~(1<< BIO_SEG_VALID);
+				__clear_bit(BIO_SEG_VALID, &bio2->bi_flags);
 			}
 			goto bio_full;
 		}
@@ -3773,7 +3770,6 @@ static int run(struct mddev *mddev)
 		blk_queue_merge_bvec(mddev->queue, raid10_mergeable_bvec);
 	}
 
-
 	if (md_integrity_register(mddev))
 		goto out_free_conf;
 
@@ -3831,6 +3827,8 @@ static int stop(struct mddev *mddev)
 		mempool_destroy(conf->r10bio_pool);
 	safe_put_page(conf->tmppage);
 	kfree(conf->mirrors);
+	kfree(conf->mirrors_old);
+	kfree(conf->mirrors_new);
 	kfree(conf);
 	mddev->private = NULL;
 	return 0;
@@ -4118,7 +4116,7 @@ static int raid10_start_reshape(struct mddev *mddev)
 		memcpy(conf->mirrors_new, conf->mirrors,
 		       sizeof(struct raid10_info)*conf->prev.raid_disks);
 		smp_mb();
-		kfree(conf->mirrors_old); /* FIXME and elsewhere */
+		kfree(conf->mirrors_old);
 		conf->mirrors_old = conf->mirrors;
 		conf->mirrors = conf->mirrors_new;
 		conf->mirrors_new = NULL;
@@ -4384,6 +4382,7 @@ static sector_t reshape_request(struct mddev *mddev, sector_t sector_nr,
 read_more:
 	/* Now schedule reads for blocks from sector_nr to last */
 	r10_bio = mempool_alloc(conf->r10buf_pool, GFP_NOIO);
+	r10_bio->state = 0;
 	raise_barrier(conf, sectors_done != 0);
 	atomic_set(&r10_bio->remaining, 0);
 	r10_bio->mddev = mddev;
@@ -4398,6 +4397,7 @@ read_more:
 		 * on all the target devices.
 		 */
 		// FIXME
+		mempool_free(r10_bio, conf->r10buf_pool);
 		set_bit(MD_RECOVERY_INTR, &mddev->recovery);
 		return sectors_done;
 	}
@@ -4410,8 +4410,8 @@ read_more:
 	read_bio->bi_private = r10_bio;
 	read_bio->bi_end_io = end_sync_read;
 	read_bio->bi_rw = READ;
-	read_bio->bi_flags &= ~(BIO_POOL_MASK - 1);
-	read_bio->bi_flags |= 1 << BIO_UPTODATE;
+	read_bio->bi_flags &= (~0UL << BIO_RESET_BITS);
+	__set_bit(BIO_UPTODATE, &read_bio->bi_flags);
 	read_bio->bi_vcnt = 0;
 	read_bio->bi_iter.bi_size = 0;
 	r10_bio->master_bio = read_bio;
@@ -4468,7 +4468,7 @@ read_more:
 				/* Remove last page from this bio */
 				bio2->bi_vcnt--;
 				bio2->bi_iter.bi_size -= len;
-				bio2->bi_flags &= ~(1<<BIO_SEG_VALID);
+				__clear_bit(BIO_SEG_VALID, &bio2->bi_flags);
 			}
 			goto bio_full;
 		}
@@ -4570,7 +4570,6 @@ static void end_reshape(struct r10conf *conf)
 	conf->fullsync = 0;
 }
 
-
 static int handle_reshape_read_error(struct mddev *mddev,
 				     struct r10bio *r10_bio)
 {
diff --git a/drivers/md/raid5.c b/drivers/md/raid5.c
index 6234b2e84587..9c66e5997fc8 100644
--- a/drivers/md/raid5.c
+++ b/drivers/md/raid5.c
@@ -64,6 +64,10 @@
 #define cpu_to_group(cpu) cpu_to_node(cpu)
 #define ANY_GROUP NUMA_NO_NODE
 
+static bool devices_handle_discard_safely = false;
+module_param(devices_handle_discard_safely, bool, 0644);
+MODULE_PARM_DESC(devices_handle_discard_safely,
+		 "Set to Y if all devices in each array reliably return zeroes on reads from discarded regions");
 static struct workqueue_struct *raid5_wq;
 /*
  * Stripe cache
@@ -459,7 +463,6 @@ static inline void insert_hash(struct r5conf *conf, struct stripe_head *sh)
 	hlist_add_head(&sh->hash, hp);
 }
 
-
 /* find an idle stripe, make sure it is unhashed, and return it. */
 static struct stripe_head *get_free_stripe(struct r5conf *conf, int hash)
 {
@@ -527,9 +530,7 @@ static void init_stripe(struct stripe_head *sh, sector_t sector, int previous)
 	BUG_ON(stripe_operations_active(sh));
 
 	pr_debug("init_stripe called, stripe %llu\n",
-		(unsigned long long)sh->sector);
-
-	remove_hash(sh);
+		(unsigned long long)sector);
 retry:
 	seq = read_seqcount_begin(&conf->gen_lock);
 	sh->generation = conf->generation - previous;
@@ -538,7 +539,6 @@ retry:
 	stripe_set_idx(sector, conf, previous, sh);
 	sh->state = 0;
 
-
 	for (i = sh->disks; i--; ) {
 		struct r5dev *dev = &sh->dev[i];
 
@@ -1346,7 +1346,6 @@ ops_run_compute6_2(struct stripe_head *sh, struct raid5_percpu *percpu)
 	}
 }
 
-
 static void ops_complete_prexor(void *stripe_head_ref)
 {
 	struct stripe_head *sh = stripe_head_ref;
@@ -2415,7 +2414,6 @@ static sector_t raid5_compute_sector(struct r5conf *conf, sector_t r_sector,
 	return new_sector;
 }
 
-
 static sector_t compute_blocknr(struct stripe_head *sh, int i, int previous)
 {
 	struct r5conf *conf = sh->raid_conf;
@@ -2433,7 +2431,6 @@ static sector_t compute_blocknr(struct stripe_head *sh, int i, int previous)
 	sector_t r_sector;
 	struct stripe_head sh2;
 
-
 	chunk_offset = sector_div(new_sector, sectors_per_chunk);
 	stripe = new_sector;
 
@@ -2537,7 +2534,6 @@ static sector_t compute_blocknr(struct stripe_head *sh, int i, int previous)
 	return r_sector;
 }
 
-
 static void
 schedule_reconstruction(struct stripe_head *sh, struct stripe_head_state *s,
 			 int rcw, int expand)
@@ -2922,7 +2918,7 @@ static int fetch_block(struct stripe_head *sh, struct stripe_head_state *s,
 	      (!test_bit(R5_Insync, &dev->flags) || test_bit(STRIPE_PREREAD_ACTIVE, &sh->state)) &&
 	      !test_bit(R5_OVERWRITE, &fdev[0]->flags)) ||
 	     (sh->raid_conf->level == 6 && s->failed && s->to_write &&
-	      s->to_write < sh->raid_conf->raid_disks - 2 &&
+	      s->to_write - s->non_overwrite < sh->raid_conf->raid_disks - 2 &&
 	      (!test_bit(R5_Insync, &dev->flags) || test_bit(STRIPE_PREREAD_ACTIVE, &sh->state))))) {
 		/* we would like to get this block, possibly by computing it,
 		 * otherwise read it if the backing disk is insync
@@ -3009,7 +3005,6 @@ static void handle_stripe_fill(struct stripe_head *sh,
 	set_bit(STRIPE_HANDLE, &sh->state);
 }
 
-
 /* handle_stripe_clean_event
  * any written block on an uptodate or failed drive can be returned.
  * Note that if we 'wrote' to a failed drive, it will be UPTODATE, but
@@ -3300,7 +3295,6 @@ static void handle_parity_checks5(struct r5conf *conf, struct stripe_head *sh,
 	}
 }
 
-
 static void handle_parity_checks6(struct r5conf *conf, struct stripe_head *sh,
 				  struct stripe_head_state *s,
 				  int disks)
@@ -3817,6 +3811,8 @@ static void handle_stripe(struct stripe_head *sh)
 				set_bit(R5_Wantwrite, &dev->flags);
 				if (prexor)
 					continue;
+				if (s.failed > 1)
+					continue;
 				if (!test_bit(R5_Insync, &dev->flags) ||
 				    ((i == sh->pd_idx || i == sh->qd_idx)  &&
 				     s.failed == 0))
@@ -3933,7 +3929,6 @@ static void handle_stripe(struct stripe_head *sh)
 			}
 		}
 
-
 	/* Finish reconstruct operations initiated by the expansion process */
 	if (sh->reconstruct_state == reconstruct_state_result) {
 		struct stripe_head *sh_src
@@ -4131,7 +4126,6 @@ static int raid5_mergeable_bvec(struct request_queue *q,
 		return max;
 }
 
-
 static int in_chunk_boundary(struct mddev *mddev, struct bio *bio)
 {
 	sector_t sector = bio->bi_iter.bi_sector + get_start_sect(bio->bi_bdev);
@@ -4161,7 +4155,6 @@ static void add_bio_to_retry(struct bio *bi,struct r5conf *conf)
 	md_wakeup_thread(conf->mddev->thread);
 }
 
-
 static struct bio *remove_bio_from_retry(struct r5conf *conf)
 {
 	struct bio *bi;
@@ -4185,7 +4178,6 @@ static struct bio *remove_bio_from_retry(struct r5conf *conf)
 	return bi;
 }
 
-
 /*
  *  The "raid5_align_endio" should check if the read succeeded and if it
  *  did, call bio_endio on the original bio (having bio_put the new bio
@@ -4218,7 +4210,6 @@ static void raid5_align_endio(struct bio *bi, int error)
 		return;
 	}
 
-
 	pr_debug("raid5_align_endio : io error...handing IO for a retry\n");
 
 	add_bio_to_retry(raid_bi, conf);
@@ -4243,7 +4234,6 @@ static int bio_fits_rdev(struct bio *bi)
 	return 1;
 }
 
-
 static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 {
 	struct r5conf *conf = mddev->private;
@@ -4295,7 +4285,7 @@ static int chunk_aligned_read(struct mddev *mddev, struct bio * raid_bio)
 		rcu_read_unlock();
 		raid_bio->bi_next = (void*)rdev;
 		align_bi->bi_bdev =  rdev->bdev;
-		align_bi->bi_flags &= ~(1 << BIO_SEG_VALID);
+		__clear_bit(BIO_SEG_VALID, &align_bi->bi_flags);
 
 		if (!bio_fits_rdev(align_bi) ||
 		    is_badblock(rdev, align_bi->bi_iter.bi_sector,
@@ -5440,7 +5430,6 @@ raid5_skip_copy = __ATTR(skip_copy, S_IRUGO | S_IWUSR,
 					raid5_show_skip_copy,
 					raid5_store_skip_copy);
 
-
 static ssize_t
 stripe_cache_active_show(struct mddev *mddev, char *page)
 {
@@ -5892,7 +5881,6 @@ static struct r5conf *setup_conf(struct mddev *mddev)
 		return ERR_PTR(-ENOMEM);
 }
 
-
 static int only_parity(int raid_disk, int algo, int raid_disks, int max_degraded)
 {
 	switch (algo) {
@@ -5905,7 +5893,7 @@ static int only_parity(int raid_disk, int algo, int raid_disks, int max_degraded
 			return 1;
 		break;
 	case ALGORITHM_PARITY_0_6:
-		if (raid_disk == 0 || 
+		if (raid_disk == 0 ||
 		    raid_disk == raid_disks - 1)
 			return 1;
 		break;
@@ -6159,7 +6147,6 @@ static int run(struct mddev *mddev)
 							"reshape");
 	}
 
-
 	/* Ok, everything is just fine now */
 	if (mddev->to_remove == &raid5_attrs_group)
 		mddev->to_remove = NULL;
@@ -6206,7 +6193,7 @@ static int run(struct mddev *mddev)
 		mddev->queue->limits.discard_granularity = stripe;
 		/*
 		 * unaligned part of discard request will be ignored, so can't
-		 * guarantee discard_zerors_data
+		 * guarantee discard_zeroes_data
 		 */
 		mddev->queue->limits.discard_zeroes_data = 0;
 
@@ -6231,6 +6218,18 @@ static int run(struct mddev *mddev)
 			    !bdev_get_queue(rdev->bdev)->
 						limits.discard_zeroes_data)
 				discard_supported = false;
+			/* Unfortunately, discard_zeroes_data is not currently
+			 * a guarantee - just a hint.  So we only allow DISCARD
+			 * if the sysadmin has confirmed that only safe devices
+			 * are in use by setting a module parameter.
+			 */
+			if (!devices_handle_discard_safely) {
+				if (discard_supported) {
+					pr_info("md/raid456: discard support disabled due to uncertainty.\n");
+					pr_info("Set raid456.devices_handle_discard_safely=Y to override.\n");
+				}
+				discard_supported = false;
+			}
 		}
 
 		if (discard_supported &&
@@ -6796,7 +6795,6 @@ static void raid5_quiesce(struct mddev *mddev, int state)
 	}
 }
 
-
 static void *raid45_takeover_raid0(struct mddev *mddev, int level)
 {
 	struct r0conf *raid0_conf = mddev->private;
@@ -6823,7 +6821,6 @@ static void *raid45_takeover_raid0(struct mddev *mddev, int level)
 	return setup_conf(mddev);
 }
 
-
 static void *raid5_takeover_raid1(struct mddev *mddev)
 {
 	int chunksect;
@@ -6884,7 +6881,6 @@ static void *raid5_takeover_raid6(struct mddev *mddev)
 	return setup_conf(mddev);
 }
 
-
 static int raid5_check_reshape(struct mddev *mddev)
 {
 	/* For a 2-drive array, the layout and chunk size can be changed
@@ -7033,7 +7029,6 @@ static void *raid6_takeover(struct mddev *mddev)
 	return setup_conf(mddev);
 }
 
-
 static struct md_personality raid6_personality =
 {
 	.name		= "raid6",
diff --git a/drivers/md/raid5.h b/drivers/md/raid5.h
index bc72cd4be5f8..d59f5ca743cd 100644
--- a/drivers/md/raid5.h
+++ b/drivers/md/raid5.h
@@ -155,7 +155,7 @@
  */
 
 /*
- * Operations state - intermediate states that are visible outside of 
+ * Operations state - intermediate states that are visible outside of
  *   STRIPE_ACTIVE.
  * In general _idle indicates nothing is running, _run indicates a data
  * processing operation is active, and _result means the data processing result
@@ -364,7 +364,6 @@ enum {
  * HANDLE gets cleared if stripe_handle leaves nothing locked.
  */
 
-
 struct disk_info {
 	struct md_rdev	*rdev, *replacement;
 };
@@ -528,7 +527,6 @@ struct r5conf {
 #define ALGORITHM_ROTATING_N_RESTART	9 /* DDF PRL=6 RLQ=2 */
 #define ALGORITHM_ROTATING_N_CONTINUE	10 /*DDF PRL=6 RLQ=3 */
 
-
 /* For every RAID5 algorithm we define a RAID6 algorithm
  * with exactly the same layout for data and parity, and
  * with the Q block always on the last device (N-1).
diff --git a/drivers/media/Kconfig b/drivers/media/Kconfig
index f60bad491eb6..3c89fcbc621e 100644
--- a/drivers/media/Kconfig
+++ b/drivers/media/Kconfig
@@ -182,7 +182,6 @@ config MEDIA_SUBDRV_AUTOSELECT
 	depends on HAS_IOMEM
 	select I2C
 	select I2C_MUX
-	select SPI
 	default y
 	help
 	  By default, a media driver auto-selects all possible ancillary
diff --git a/drivers/media/common/b2c2/flexcop.h b/drivers/media/common/b2c2/flexcop.h
index 897b10c85ad9..8942bdacbf61 100644
--- a/drivers/media/common/b2c2/flexcop.h
+++ b/drivers/media/common/b2c2/flexcop.h
@@ -4,7 +4,7 @@
  * see flexcop.c for copyright information
  */
 #ifndef __FLEXCOP_H__
-#define __FLEXCOP_H___
+#define __FLEXCOP_H__
 
 #define FC_LOG_PREFIX "b2c2-flexcop"
 #include "flexcop-common.h"
diff --git a/drivers/media/common/cx2341x.c b/drivers/media/common/cx2341x.c
index 103ef6bad2e2..be763150b8aa 100644
--- a/drivers/media/common/cx2341x.c
+++ b/drivers/media/common/cx2341x.c
@@ -1490,6 +1490,7 @@ static struct v4l2_ctrl *cx2341x_ctrl_new_custom(struct v4l2_ctrl_handler *hdl,
 {
 	struct v4l2_ctrl_config cfg;
 
+	memset(&cfg, 0, sizeof(cfg));
 	cx2341x_ctrl_fill(id, &cfg.name, &cfg.type, &min, &max, &step, &def, &cfg.flags);
 	cfg.ops = &cx2341x_ops;
 	cfg.id = id;
diff --git a/drivers/media/common/saa7146/saa7146_fops.c b/drivers/media/common/saa7146/saa7146_fops.c
index 6c47f3fe9b0f..b7d63933dae6 100644
--- a/drivers/media/common/saa7146/saa7146_fops.c
+++ b/drivers/media/common/saa7146/saa7146_fops.c
@@ -311,7 +311,6 @@ static int fops_mmap(struct file *file, struct vm_area_struct * vma)
 		}
 	default:
 		BUG();
-		return 0;
 	}
 
 	if (mutex_lock_interruptible(vdev->lock))
@@ -399,7 +398,6 @@ static ssize_t fops_read(struct file *file, char __user *data, size_t count, lof
 		return -EINVAL;
 	default:
 		BUG();
-		return 0;
 	}
 }
 
@@ -423,7 +421,6 @@ static ssize_t fops_write(struct file *file, const char __user *data, size_t cou
 		return -EINVAL;
 	default:
 		BUG();
-		return -EINVAL;
 	}
 }
 
diff --git a/drivers/media/common/siano/sms-cards.c b/drivers/media/common/siano/sms-cards.c
index 82769993eeb7..82c7a1289f05 100644
--- a/drivers/media/common/siano/sms-cards.c
+++ b/drivers/media/common/siano/sms-cards.c
@@ -157,6 +157,12 @@ static struct sms_board sms_boards[] = {
 		.type = SMS_DENVER_2160,
 		.default_mode = DEVICE_MODE_DAB_TDMB,
 	},
+	[SMS1XXX_BOARD_PCTV_77E] = {
+		.name	= "Hauppauge microStick 77e",
+		.type	= SMS_NOVA_B0,
+		.fw[DEVICE_MODE_DVBT_BDA] = SMS_FW_DVB_NOVA_12MHZ_B0,
+		.default_mode = DEVICE_MODE_DVBT_BDA,
+	},
 };
 
 struct sms_board *sms_get_board(unsigned id)
diff --git a/drivers/media/common/siano/sms-cards.h b/drivers/media/common/siano/sms-cards.h
index c63b544c49c5..4c4caddf9869 100644
--- a/drivers/media/common/siano/sms-cards.h
+++ b/drivers/media/common/siano/sms-cards.h
@@ -45,6 +45,7 @@
 #define SMS1XXX_BOARD_SIANO_RIO		18
 #define SMS1XXX_BOARD_SIANO_DENVER_1530	19
 #define SMS1XXX_BOARD_SIANO_DENVER_2160 20
+#define SMS1XXX_BOARD_PCTV_77E		21
 
 struct sms_board_gpio_cfg {
 	int lna_vhf_exist;
diff --git a/drivers/media/common/siano/smscoreapi.c b/drivers/media/common/siano/smscoreapi.c
index 050984c5b1e3..a3677438205e 100644
--- a/drivers/media/common/siano/smscoreapi.c
+++ b/drivers/media/common/siano/smscoreapi.c
@@ -2129,8 +2129,6 @@ int smscore_gpio_get_level(struct smscore_device_t *coredev, u8 pin_num,
 
 static int __init smscore_module_init(void)
 {
-	int rc = 0;
-
 	INIT_LIST_HEAD(&g_smscore_notifyees);
 	INIT_LIST_HEAD(&g_smscore_devices);
 	kmutex_init(&g_smscore_deviceslock);
@@ -2138,7 +2136,7 @@ static int __init smscore_module_init(void)
 	INIT_LIST_HEAD(&g_smscore_registry);
 	kmutex_init(&g_smscore_registrylock);
 
-	return rc;
+	return 0;
 }
 
 static void __exit smscore_module_exit(void)
diff --git a/drivers/media/dvb-core/dmxdev.c b/drivers/media/dvb-core/dmxdev.c
index c0363f1b6c90..abff803ad69a 100644
--- a/drivers/media/dvb-core/dmxdev.c
+++ b/drivers/media/dvb-core/dmxdev.c
@@ -1087,8 +1087,8 @@ static unsigned int dvb_demux_poll(struct file *file, poll_table *wait)
 	struct dmxdev_filter *dmxdevfilter = file->private_data;
 	unsigned int mask = 0;
 
-	if (!dmxdevfilter)
-		return -EINVAL;
+	if ((!dmxdevfilter) || dmxdevfilter->dev->exit)
+		return POLLERR;
 
 	poll_wait(file, &dmxdevfilter->buffer.queue, wait);
 
@@ -1181,6 +1181,9 @@ static unsigned int dvb_dvr_poll(struct file *file, poll_table *wait)
 
 	dprintk("function : %s\n", __func__);
 
+	if (dmxdev->exit)
+		return POLLERR;
+
 	poll_wait(file, &dmxdev->dvr_buffer.queue, wait);
 
 	if ((file->f_flags & O_ACCMODE) == O_RDONLY) {
diff --git a/drivers/media/dvb-core/dvb-usb-ids.h b/drivers/media/dvb-core/dvb-usb-ids.h
index 5135a096bfa6..e07a84e7bc56 100644
--- a/drivers/media/dvb-core/dvb-usb-ids.h
+++ b/drivers/media/dvb-core/dvb-usb-ids.h
@@ -144,6 +144,7 @@
 #define USB_PID_ITETECH_IT9135				0x9135
 #define USB_PID_ITETECH_IT9135_9005			0x9005
 #define USB_PID_ITETECH_IT9135_9006			0x9006
+#define USB_PID_ITETECH_IT9303				0x9306
 #define USB_PID_KWORLD_399U				0xe399
 #define USB_PID_KWORLD_399U_2				0xe400
 #define USB_PID_KWORLD_395U				0xe396
@@ -244,6 +245,7 @@
 #define USB_PID_TECHNOTREND_CONNECT_S2400               0x3006
 #define USB_PID_TECHNOTREND_CONNECT_S2400_8KEEPROM	0x3009
 #define USB_PID_TECHNOTREND_CONNECT_CT3650		0x300d
+#define USB_PID_TECHNOTREND_CONNECT_CT2_4650_CI		0x3012
 #define USB_PID_TECHNOTREND_TVSTICK_CT2_4400		0x3014
 #define USB_PID_TERRATEC_CINERGY_DT_XS_DIVERSITY	0x005a
 #define USB_PID_TERRATEC_CINERGY_DT_XS_DIVERSITY_2	0x0081
@@ -280,6 +282,8 @@
 #define USB_PID_PCTV_400E				0x020f
 #define USB_PID_PCTV_450E				0x0222
 #define USB_PID_PCTV_452E				0x021f
+#define USB_PID_PCTV_78E				0x025a
+#define USB_PID_PCTV_79E				0x0262
 #define USB_PID_REALTEK_RTL2831U			0x2831
 #define USB_PID_REALTEK_RTL2832U			0x2832
 #define USB_PID_TECHNOTREND_CONNECT_S2_3600		0x3007
diff --git a/drivers/media/dvb-core/dvb_frontend.c b/drivers/media/dvb-core/dvb_frontend.c
index c2a6a0a85813..b8579ee68bd6 100644
--- a/drivers/media/dvb-core/dvb_frontend.c
+++ b/drivers/media/dvb-core/dvb_frontend.c
@@ -1934,15 +1934,13 @@ static int dvb_frontend_ioctl_properties(struct file *file,
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int err = 0;
 
-	struct dtv_properties *tvps = NULL;
+	struct dtv_properties *tvps = parg;
 	struct dtv_property *tvp = NULL;
 	int i;
 
 	dev_dbg(fe->dvb->device, "%s:\n", __func__);
 
-	if(cmd == FE_SET_PROPERTY) {
-		tvps = (struct dtv_properties __user *)parg;
-
+	if (cmd == FE_SET_PROPERTY) {
 		dev_dbg(fe->dvb->device, "%s: properties.num = %d\n", __func__, tvps->num);
 		dev_dbg(fe->dvb->device, "%s: properties.props = %p\n", __func__, tvps->props);
 
@@ -1957,7 +1955,8 @@ static int dvb_frontend_ioctl_properties(struct file *file,
 			goto out;
 		}
 
-		if (copy_from_user(tvp, tvps->props, tvps->num * sizeof(struct dtv_property))) {
+		if (copy_from_user(tvp, (void __user *)tvps->props,
+				   tvps->num * sizeof(struct dtv_property))) {
 			err = -EFAULT;
 			goto out;
 		}
@@ -1972,10 +1971,7 @@ static int dvb_frontend_ioctl_properties(struct file *file,
 		if (c->state == DTV_TUNE)
 			dev_dbg(fe->dvb->device, "%s: Property cache is full, tuning\n", __func__);
 
-	} else
-	if(cmd == FE_GET_PROPERTY) {
-		tvps = (struct dtv_properties __user *)parg;
-
+	} else if (cmd == FE_GET_PROPERTY) {
 		dev_dbg(fe->dvb->device, "%s: properties.num = %d\n", __func__, tvps->num);
 		dev_dbg(fe->dvb->device, "%s: properties.props = %p\n", __func__, tvps->props);
 
@@ -1990,7 +1986,8 @@ static int dvb_frontend_ioctl_properties(struct file *file,
 			goto out;
 		}
 
-		if (copy_from_user(tvp, tvps->props, tvps->num * sizeof(struct dtv_property))) {
+		if (copy_from_user(tvp, (void __user *)tvps->props,
+				   tvps->num * sizeof(struct dtv_property))) {
 			err = -EFAULT;
 			goto out;
 		}
@@ -2012,7 +2009,8 @@ static int dvb_frontend_ioctl_properties(struct file *file,
 			(tvp + i)->result = err;
 		}
 
-		if (copy_to_user(tvps->props, tvp, tvps->num * sizeof(struct dtv_property))) {
+		if (copy_to_user((void __user *)tvps->props, tvp,
+				 tvps->num * sizeof(struct dtv_property))) {
 			err = -EFAULT;
 			goto out;
 		}
@@ -2072,6 +2070,23 @@ static int dtv_set_frontend(struct dvb_frontend *fe)
 	case SYS_DVBC_ANNEX_C:
 		rolloff = 113;
 		break;
+	case SYS_DVBS:
+	case SYS_TURBO:
+		rolloff = 135;
+		break;
+	case SYS_DVBS2:
+		switch (c->rolloff) {
+		case ROLLOFF_20:
+			rolloff = 120;
+			break;
+		case ROLLOFF_25:
+			rolloff = 125;
+			break;
+		default:
+		case ROLLOFF_35:
+			rolloff = 135;
+		}
+		break;
 	default:
 		break;
 	}
@@ -2550,7 +2565,9 @@ int dvb_frontend_suspend(struct dvb_frontend *fe)
 	dev_dbg(fe->dvb->device, "%s: adap=%d fe=%d\n", __func__, fe->dvb->num,
 			fe->id);
 
-	if (fe->ops.tuner_ops.sleep)
+	if (fe->ops.tuner_ops.suspend)
+		ret = fe->ops.tuner_ops.suspend(fe);
+	else if (fe->ops.tuner_ops.sleep)
 		ret = fe->ops.tuner_ops.sleep(fe);
 
 	if (fe->ops.sleep)
@@ -2572,7 +2589,9 @@ int dvb_frontend_resume(struct dvb_frontend *fe)
 	if (fe->ops.init)
 		ret = fe->ops.init(fe);
 
-	if (fe->ops.tuner_ops.init)
+	if (fe->ops.tuner_ops.resume)
+		ret = fe->ops.tuner_ops.resume(fe);
+	else if (fe->ops.tuner_ops.init)
 		ret = fe->ops.tuner_ops.init(fe);
 
 	fe->exit = DVB_FE_NO_EXIT;
diff --git a/drivers/media/dvb-core/dvb_frontend.h b/drivers/media/dvb-core/dvb_frontend.h
index d398de4b6ef4..816269e5f706 100644
--- a/drivers/media/dvb-core/dvb_frontend.h
+++ b/drivers/media/dvb-core/dvb_frontend.h
@@ -201,6 +201,8 @@ struct dvb_tuner_ops {
 	int (*release)(struct dvb_frontend *fe);
 	int (*init)(struct dvb_frontend *fe);
 	int (*sleep)(struct dvb_frontend *fe);
+	int (*suspend)(struct dvb_frontend *fe);
+	int (*resume)(struct dvb_frontend *fe);
 
 	/** This is for simple PLLs - set all parameters in one go. */
 	int (*set_params)(struct dvb_frontend *fe);
diff --git a/drivers/media/dvb-core/dvb_ringbuffer.c b/drivers/media/dvb-core/dvb_ringbuffer.c
index a5712cd7c65f..1100e98a7b1d 100644
--- a/drivers/media/dvb-core/dvb_ringbuffer.c
+++ b/drivers/media/dvb-core/dvb_ringbuffer.c
@@ -166,6 +166,31 @@ ssize_t dvb_ringbuffer_write(struct dvb_ringbuffer *rbuf, const u8 *buf, size_t
 	return len;
 }
 
+ssize_t dvb_ringbuffer_write_user(struct dvb_ringbuffer *rbuf,
+				  const u8 __user *buf, size_t len)
+{
+	int status;
+	size_t todo = len;
+	size_t split;
+
+	split = (rbuf->pwrite + len > rbuf->size) ? rbuf->size - rbuf->pwrite : 0;
+
+	if (split > 0) {
+		status = copy_from_user(rbuf->data+rbuf->pwrite, buf, split);
+		if (status)
+			return len - todo;
+		buf += split;
+		todo -= split;
+		rbuf->pwrite = 0;
+	}
+	status = copy_from_user(rbuf->data+rbuf->pwrite, buf, todo);
+	if (status)
+		return len - todo;
+	rbuf->pwrite = (rbuf->pwrite + todo) % rbuf->size;
+
+	return len;
+}
+
 ssize_t dvb_ringbuffer_pkt_write(struct dvb_ringbuffer *rbuf, u8* buf, size_t len)
 {
 	int status;
@@ -297,3 +322,4 @@ EXPORT_SYMBOL(dvb_ringbuffer_flush_spinlock_wakeup);
 EXPORT_SYMBOL(dvb_ringbuffer_read_user);
 EXPORT_SYMBOL(dvb_ringbuffer_read);
 EXPORT_SYMBOL(dvb_ringbuffer_write);
+EXPORT_SYMBOL(dvb_ringbuffer_write_user);
diff --git a/drivers/media/dvb-core/dvb_ringbuffer.h b/drivers/media/dvb-core/dvb_ringbuffer.h
index 41f04dae69b6..9e1e11b7c39c 100644
--- a/drivers/media/dvb-core/dvb_ringbuffer.h
+++ b/drivers/media/dvb-core/dvb_ringbuffer.h
@@ -133,6 +133,8 @@ extern void dvb_ringbuffer_read(struct dvb_ringbuffer *rbuf,
 */
 extern ssize_t dvb_ringbuffer_write(struct dvb_ringbuffer *rbuf, const u8 *buf,
 				    size_t len);
+extern ssize_t dvb_ringbuffer_write_user(struct dvb_ringbuffer *rbuf,
+				         const u8 __user *buf, size_t len);
 
 
 /**
diff --git a/drivers/media/dvb-frontends/Kconfig b/drivers/media/dvb-frontends/Kconfig
index fe0ddcca192c..5a134547e325 100644
--- a/drivers/media/dvb-frontends/Kconfig
+++ b/drivers/media/dvb-frontends/Kconfig
@@ -471,6 +471,11 @@ config DVB_SI2168
 	help
 	  Say Y when you want to support this frontend.
 
+config DVB_AS102_FE
+	tristate
+	depends on DVB_CORE
+	default DVB_AS102
+
 comment "DVB-C (cable) frontends"
 	depends on DVB_CORE
 
@@ -643,6 +648,14 @@ config DVB_MB86A20S
 	  A driver for Fujitsu mb86a20s ISDB-T/ISDB-Tsb demodulator.
 	  Say Y when you want to support this frontend.
 
+config DVB_TC90522
+	tristate "Toshiba TC90522"
+	depends on DVB_CORE && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  A Toshiba TC90522 2xISDB-T + 2xISDB-S demodulator.
+	  Say Y when you want to support this frontend.
+
 comment "Digital terrestrial only tuners/PLL"
 	depends on DVB_CORE
 
@@ -720,6 +733,13 @@ config DVB_A8293
 	depends on DVB_CORE && I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 
+config DVB_SP2
+	tristate "CIMaX SP2"
+	depends on DVB_CORE && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  CIMaX SP2/SP2HF Common Interface module.
+
 config DVB_LGS8GL5
 	tristate "Silicon Legend LGS-8GL5 demodulator (OFDM)"
 	depends on DVB_CORE && I2C
diff --git a/drivers/media/dvb-frontends/Makefile b/drivers/media/dvb-frontends/Makefile
index edf103d45920..ba59df63d050 100644
--- a/drivers/media/dvb-frontends/Makefile
+++ b/drivers/media/dvb-frontends/Makefile
@@ -107,10 +107,12 @@ obj-$(CONFIG_DVB_DRXK) += drxk.o
 obj-$(CONFIG_DVB_TDA18271C2DD) += tda18271c2dd.o
 obj-$(CONFIG_DVB_SI2165) += si2165.o
 obj-$(CONFIG_DVB_A8293) += a8293.o
+obj-$(CONFIG_DVB_SP2) += sp2.o
 obj-$(CONFIG_DVB_TDA10071) += tda10071.o
 obj-$(CONFIG_DVB_RTL2830) += rtl2830.o
 obj-$(CONFIG_DVB_RTL2832) += rtl2832.o
 obj-$(CONFIG_DVB_RTL2832_SDR) += rtl2832_sdr.o
 obj-$(CONFIG_DVB_M88RS2000) += m88rs2000.o
 obj-$(CONFIG_DVB_AF9033) += af9033.o
-
+obj-$(CONFIG_DVB_AS102_FE) += as102_fe.o
+obj-$(CONFIG_DVB_TC90522) += tc90522.o
diff --git a/drivers/media/dvb-frontends/af9013.c b/drivers/media/dvb-frontends/af9013.c
index ecf6388d2200..8001690d7576 100644
--- a/drivers/media/dvb-frontends/af9013.c
+++ b/drivers/media/dvb-frontends/af9013.c
@@ -683,7 +683,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 
 	switch (c->transmission_mode) {
 	case TRANSMISSION_MODE_AUTO:
-		auto_mode = 1;
+		auto_mode = true;
 		break;
 	case TRANSMISSION_MODE_2K:
 		break;
@@ -693,12 +693,12 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 	default:
 		dev_dbg(&state->i2c->dev, "%s: invalid transmission_mode\n",
 				__func__);
-		auto_mode = 1;
+		auto_mode = true;
 	}
 
 	switch (c->guard_interval) {
 	case GUARD_INTERVAL_AUTO:
-		auto_mode = 1;
+		auto_mode = true;
 		break;
 	case GUARD_INTERVAL_1_32:
 		break;
@@ -714,12 +714,12 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 	default:
 		dev_dbg(&state->i2c->dev, "%s: invalid guard_interval\n",
 				__func__);
-		auto_mode = 1;
+		auto_mode = true;
 	}
 
 	switch (c->hierarchy) {
 	case HIERARCHY_AUTO:
-		auto_mode = 1;
+		auto_mode = true;
 		break;
 	case HIERARCHY_NONE:
 		break;
@@ -734,12 +734,12 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		break;
 	default:
 		dev_dbg(&state->i2c->dev, "%s: invalid hierarchy\n", __func__);
-		auto_mode = 1;
+		auto_mode = true;
 	}
 
 	switch (c->modulation) {
 	case QAM_AUTO:
-		auto_mode = 1;
+		auto_mode = true;
 		break;
 	case QPSK:
 		break;
@@ -751,7 +751,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 		break;
 	default:
 		dev_dbg(&state->i2c->dev, "%s: invalid modulation\n", __func__);
-		auto_mode = 1;
+		auto_mode = true;
 	}
 
 	/* Use HP. How and which case we can switch to LP? */
@@ -759,7 +759,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 
 	switch (c->code_rate_HP) {
 	case FEC_AUTO:
-		auto_mode = 1;
+		auto_mode = true;
 		break;
 	case FEC_1_2:
 		break;
@@ -778,12 +778,12 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 	default:
 		dev_dbg(&state->i2c->dev, "%s: invalid code_rate_HP\n",
 				__func__);
-		auto_mode = 1;
+		auto_mode = true;
 	}
 
 	switch (c->code_rate_LP) {
 	case FEC_AUTO:
-		auto_mode = 1;
+		auto_mode = true;
 		break;
 	case FEC_1_2:
 		break;
@@ -804,7 +804,7 @@ static int af9013_set_frontend(struct dvb_frontend *fe)
 	default:
 		dev_dbg(&state->i2c->dev, "%s: invalid code_rate_LP\n",
 				__func__);
-		auto_mode = 1;
+		auto_mode = true;
 	}
 
 	switch (c->bandwidth_hz) {
diff --git a/drivers/media/dvb-frontends/af9033.c b/drivers/media/dvb-frontends/af9033.c
index be4bec2a9640..63a89c1c59ff 100644
--- a/drivers/media/dvb-frontends/af9033.c
+++ b/drivers/media/dvb-frontends/af9033.c
@@ -24,29 +24,35 @@
 /* Max transfer size done by I2C transfer functions */
 #define MAX_XFER_SIZE  64
 
-struct af9033_state {
-	struct i2c_adapter *i2c;
+struct af9033_dev {
+	struct i2c_client *client;
 	struct dvb_frontend fe;
 	struct af9033_config cfg;
+	bool is_af9035;
+	bool is_it9135;
 
 	u32 bandwidth_hz;
 	bool ts_mode_parallel;
 	bool ts_mode_serial;
 
-	u32 ber;
-	u32 ucb;
-	unsigned long last_stat_check;
+	fe_status_t fe_status;
+	u64 post_bit_error_prev; /* for old read_ber we return (curr - prev) */
+	u64 post_bit_error;
+	u64 post_bit_count;
+	u64 error_block_count;
+	u64 total_block_count;
+	struct delayed_work stat_work;
 };
 
 /* write multiple registers */
-static int af9033_wr_regs(struct af9033_state *state, u32 reg, const u8 *val,
+static int af9033_wr_regs(struct af9033_dev *dev, u32 reg, const u8 *val,
 		int len)
 {
 	int ret;
 	u8 buf[MAX_XFER_SIZE];
 	struct i2c_msg msg[1] = {
 		{
-			.addr = state->cfg.i2c_addr,
+			.addr = dev->client->addr,
 			.flags = 0,
 			.len = 3 + len,
 			.buf = buf,
@@ -54,9 +60,9 @@ static int af9033_wr_regs(struct af9033_state *state, u32 reg, const u8 *val,
 	};
 
 	if (3 + len > sizeof(buf)) {
-		dev_warn(&state->i2c->dev,
-			 "%s: i2c wr reg=%04x: len=%d is too big!\n",
-			 KBUILD_MODNAME, reg, len);
+		dev_warn(&dev->client->dev,
+				"i2c wr reg=%04x: len=%d is too big!\n",
+				reg, len);
 		return -EINVAL;
 	}
 
@@ -65,12 +71,12 @@ static int af9033_wr_regs(struct af9033_state *state, u32 reg, const u8 *val,
 	buf[2] = (reg >>  0) & 0xff;
 	memcpy(&buf[3], val, len);
 
-	ret = i2c_transfer(state->i2c, msg, 1);
+	ret = i2c_transfer(dev->client->adapter, msg, 1);
 	if (ret == 1) {
 		ret = 0;
 	} else {
-		dev_warn(&state->i2c->dev, "%s: i2c wr failed=%d reg=%06x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
+		dev_warn(&dev->client->dev, "i2c wr failed=%d reg=%06x len=%d\n",
+				ret, reg, len);
 		ret = -EREMOTEIO;
 	}
 
@@ -78,31 +84,31 @@ static int af9033_wr_regs(struct af9033_state *state, u32 reg, const u8 *val,
 }
 
 /* read multiple registers */
-static int af9033_rd_regs(struct af9033_state *state, u32 reg, u8 *val, int len)
+static int af9033_rd_regs(struct af9033_dev *dev, u32 reg, u8 *val, int len)
 {
 	int ret;
 	u8 buf[3] = { (reg >> 16) & 0xff, (reg >> 8) & 0xff,
 			(reg >> 0) & 0xff };
 	struct i2c_msg msg[2] = {
 		{
-			.addr = state->cfg.i2c_addr,
+			.addr = dev->client->addr,
 			.flags = 0,
 			.len = sizeof(buf),
 			.buf = buf
 		}, {
-			.addr = state->cfg.i2c_addr,
+			.addr = dev->client->addr,
 			.flags = I2C_M_RD,
 			.len = len,
 			.buf = val
 		}
 	};
 
-	ret = i2c_transfer(state->i2c, msg, 2);
+	ret = i2c_transfer(dev->client->adapter, msg, 2);
 	if (ret == 2) {
 		ret = 0;
 	} else {
-		dev_warn(&state->i2c->dev, "%s: i2c rd failed=%d reg=%06x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
+		dev_warn(&dev->client->dev, "i2c rd failed=%d reg=%06x len=%d\n",
+				ret, reg, len);
 		ret = -EREMOTEIO;
 	}
 
@@ -111,19 +117,19 @@ static int af9033_rd_regs(struct af9033_state *state, u32 reg, u8 *val, int len)
 
 
 /* write single register */
-static int af9033_wr_reg(struct af9033_state *state, u32 reg, u8 val)
+static int af9033_wr_reg(struct af9033_dev *dev, u32 reg, u8 val)
 {
-	return af9033_wr_regs(state, reg, &val, 1);
+	return af9033_wr_regs(dev, reg, &val, 1);
 }
 
 /* read single register */
-static int af9033_rd_reg(struct af9033_state *state, u32 reg, u8 *val)
+static int af9033_rd_reg(struct af9033_dev *dev, u32 reg, u8 *val)
 {
-	return af9033_rd_regs(state, reg, val, 1);
+	return af9033_rd_regs(dev, reg, val, 1);
 }
 
 /* write single register with mask */
-static int af9033_wr_reg_mask(struct af9033_state *state, u32 reg, u8 val,
+static int af9033_wr_reg_mask(struct af9033_dev *dev, u32 reg, u8 val,
 		u8 mask)
 {
 	int ret;
@@ -131,7 +137,7 @@ static int af9033_wr_reg_mask(struct af9033_state *state, u32 reg, u8 val,
 
 	/* no need for read if whole reg is written */
 	if (mask != 0xff) {
-		ret = af9033_rd_regs(state, reg, &tmp, 1);
+		ret = af9033_rd_regs(dev, reg, &tmp, 1);
 		if (ret)
 			return ret;
 
@@ -140,17 +146,17 @@ static int af9033_wr_reg_mask(struct af9033_state *state, u32 reg, u8 val,
 		val |= tmp;
 	}
 
-	return af9033_wr_regs(state, reg, &val, 1);
+	return af9033_wr_regs(dev, reg, &val, 1);
 }
 
 /* read single register with mask */
-static int af9033_rd_reg_mask(struct af9033_state *state, u32 reg, u8 *val,
+static int af9033_rd_reg_mask(struct af9033_dev *dev, u32 reg, u8 *val,
 		u8 mask)
 {
 	int ret, i;
 	u8 tmp;
 
-	ret = af9033_rd_regs(state, reg, &tmp, 1);
+	ret = af9033_rd_regs(dev, reg, &tmp, 1);
 	if (ret)
 		return ret;
 
@@ -167,18 +173,17 @@ static int af9033_rd_reg_mask(struct af9033_state *state, u32 reg, u8 *val,
 }
 
 /* write reg val table using reg addr auto increment */
-static int af9033_wr_reg_val_tab(struct af9033_state *state,
+static int af9033_wr_reg_val_tab(struct af9033_dev *dev,
 		const struct reg_val *tab, int tab_len)
 {
 #define MAX_TAB_LEN 212
 	int ret, i, j;
 	u8 buf[1 + MAX_TAB_LEN];
 
-	dev_dbg(&state->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
+	dev_dbg(&dev->client->dev, "tab_len=%d\n", tab_len);
 
 	if (tab_len > sizeof(buf)) {
-		dev_warn(&state->i2c->dev, "%s: tab len %d is too big\n",
-				KBUILD_MODNAME, tab_len);
+		dev_warn(&dev->client->dev, "tab len %d is too big\n", tab_len);
 		return -EINVAL;
 	}
 
@@ -186,7 +191,7 @@ static int af9033_wr_reg_val_tab(struct af9033_state *state,
 		buf[j] = tab[i].val;
 
 		if (i == tab_len - 1 || tab[i].reg != tab[i + 1].reg - 1) {
-			ret = af9033_wr_regs(state, tab[i].reg - j, buf, j + 1);
+			ret = af9033_wr_regs(dev, tab[i].reg - j, buf, j + 1);
 			if (ret < 0)
 				goto err;
 
@@ -199,16 +204,16 @@ static int af9033_wr_reg_val_tab(struct af9033_state *state,
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
-static u32 af9033_div(struct af9033_state *state, u32 a, u32 b, u32 x)
+static u32 af9033_div(struct af9033_dev *dev, u32 a, u32 b, u32 x)
 {
 	u32 r = 0, c = 0, i;
 
-	dev_dbg(&state->i2c->dev, "%s: a=%d b=%d x=%d\n", __func__, a, b, x);
+	dev_dbg(&dev->client->dev, "a=%d b=%d x=%d\n", a, b, x);
 
 	if (a > b) {
 		c = a / b;
@@ -225,22 +230,15 @@ static u32 af9033_div(struct af9033_state *state, u32 a, u32 b, u32 x)
 	}
 	r = (c << (u32)x) + r;
 
-	dev_dbg(&state->i2c->dev, "%s: a=%d b=%d x=%d r=%d r=%x\n",
-			__func__, a, b, x, r, r);
+	dev_dbg(&dev->client->dev, "a=%d b=%d x=%d r=%d r=%x\n", a, b, x, r, r);
 
 	return r;
 }
 
-static void af9033_release(struct dvb_frontend *fe)
-{
-	struct af9033_state *state = fe->demodulator_priv;
-
-	kfree(state);
-}
-
 static int af9033_init(struct dvb_frontend *fe)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i, len;
 	const struct reg_val *init;
 	u8 buf[4];
@@ -248,7 +246,7 @@ static int af9033_init(struct dvb_frontend *fe)
 	struct reg_val_mask tab[] = {
 		{ 0x80fb24, 0x00, 0x08 },
 		{ 0x80004c, 0x00, 0xff },
-		{ 0x00f641, state->cfg.tuner, 0xff },
+		{ 0x00f641, dev->cfg.tuner, 0xff },
 		{ 0x80f5ca, 0x01, 0x01 },
 		{ 0x80f715, 0x01, 0x01 },
 		{ 0x00f41f, 0x04, 0x04 },
@@ -267,75 +265,82 @@ static int af9033_init(struct dvb_frontend *fe)
 		{ 0x00d830, 0x01, 0xff },
 		{ 0x00d831, 0x00, 0xff },
 		{ 0x00d832, 0x00, 0xff },
-		{ 0x80f985, state->ts_mode_serial, 0x01 },
-		{ 0x80f986, state->ts_mode_parallel, 0x01 },
+		{ 0x80f985, dev->ts_mode_serial, 0x01 },
+		{ 0x80f986, dev->ts_mode_parallel, 0x01 },
 		{ 0x00d827, 0x00, 0xff },
 		{ 0x00d829, 0x00, 0xff },
-		{ 0x800045, state->cfg.adc_multiplier, 0xff },
+		{ 0x800045, dev->cfg.adc_multiplier, 0xff },
 	};
 
 	/* program clock control */
-	clock_cw = af9033_div(state, state->cfg.clock, 1000000ul, 19ul);
+	clock_cw = af9033_div(dev, dev->cfg.clock, 1000000ul, 19ul);
 	buf[0] = (clock_cw >>  0) & 0xff;
 	buf[1] = (clock_cw >>  8) & 0xff;
 	buf[2] = (clock_cw >> 16) & 0xff;
 	buf[3] = (clock_cw >> 24) & 0xff;
 
-	dev_dbg(&state->i2c->dev, "%s: clock=%d clock_cw=%08x\n",
-			__func__, state->cfg.clock, clock_cw);
+	dev_dbg(&dev->client->dev, "clock=%d clock_cw=%08x\n",
+			dev->cfg.clock, clock_cw);
 
-	ret = af9033_wr_regs(state, 0x800025, buf, 4);
+	ret = af9033_wr_regs(dev, 0x800025, buf, 4);
 	if (ret < 0)
 		goto err;
 
 	/* program ADC control */
 	for (i = 0; i < ARRAY_SIZE(clock_adc_lut); i++) {
-		if (clock_adc_lut[i].clock == state->cfg.clock)
+		if (clock_adc_lut[i].clock == dev->cfg.clock)
 			break;
 	}
 
-	adc_cw = af9033_div(state, clock_adc_lut[i].adc, 1000000ul, 19ul);
+	adc_cw = af9033_div(dev, clock_adc_lut[i].adc, 1000000ul, 19ul);
 	buf[0] = (adc_cw >>  0) & 0xff;
 	buf[1] = (adc_cw >>  8) & 0xff;
 	buf[2] = (adc_cw >> 16) & 0xff;
 
-	dev_dbg(&state->i2c->dev, "%s: adc=%d adc_cw=%06x\n",
-			__func__, clock_adc_lut[i].adc, adc_cw);
+	dev_dbg(&dev->client->dev, "adc=%d adc_cw=%06x\n",
+			clock_adc_lut[i].adc, adc_cw);
 
-	ret = af9033_wr_regs(state, 0x80f1cd, buf, 3);
+	ret = af9033_wr_regs(dev, 0x80f1cd, buf, 3);
 	if (ret < 0)
 		goto err;
 
 	/* program register table */
 	for (i = 0; i < ARRAY_SIZE(tab); i++) {
-		ret = af9033_wr_reg_mask(state, tab[i].reg, tab[i].val,
+		ret = af9033_wr_reg_mask(dev, tab[i].reg, tab[i].val,
 				tab[i].mask);
 		if (ret < 0)
 			goto err;
 	}
 
+	/* clock output */
+	if (dev->cfg.dyn0_clk) {
+		ret = af9033_wr_reg(dev, 0x80fba8, 0x00);
+		if (ret < 0)
+			goto err;
+	}
+
 	/* settings for TS interface */
-	if (state->cfg.ts_mode == AF9033_TS_MODE_USB) {
-		ret = af9033_wr_reg_mask(state, 0x80f9a5, 0x00, 0x01);
+	if (dev->cfg.ts_mode == AF9033_TS_MODE_USB) {
+		ret = af9033_wr_reg_mask(dev, 0x80f9a5, 0x00, 0x01);
 		if (ret < 0)
 			goto err;
 
-		ret = af9033_wr_reg_mask(state, 0x80f9b5, 0x01, 0x01);
+		ret = af9033_wr_reg_mask(dev, 0x80f9b5, 0x01, 0x01);
 		if (ret < 0)
 			goto err;
 	} else {
-		ret = af9033_wr_reg_mask(state, 0x80f990, 0x00, 0x01);
+		ret = af9033_wr_reg_mask(dev, 0x80f990, 0x00, 0x01);
 		if (ret < 0)
 			goto err;
 
-		ret = af9033_wr_reg_mask(state, 0x80f9b5, 0x00, 0x01);
+		ret = af9033_wr_reg_mask(dev, 0x80f9b5, 0x00, 0x01);
 		if (ret < 0)
 			goto err;
 	}
 
 	/* load OFSM settings */
-	dev_dbg(&state->i2c->dev, "%s: load ofsm settings\n", __func__);
-	switch (state->cfg.tuner) {
+	dev_dbg(&dev->client->dev, "load ofsm settings\n");
+	switch (dev->cfg.tuner) {
 	case AF9033_TUNER_IT9135_38:
 	case AF9033_TUNER_IT9135_51:
 	case AF9033_TUNER_IT9135_52:
@@ -354,14 +359,13 @@ static int af9033_init(struct dvb_frontend *fe)
 		break;
 	}
 
-	ret = af9033_wr_reg_val_tab(state, init, len);
+	ret = af9033_wr_reg_val_tab(dev, init, len);
 	if (ret < 0)
 		goto err;
 
 	/* load tuner specific settings */
-	dev_dbg(&state->i2c->dev, "%s: load tuner specific settings\n",
-			__func__);
-	switch (state->cfg.tuner) {
+	dev_dbg(&dev->client->dev, "load tuner specific settings\n");
+	switch (dev->cfg.tuner) {
 	case AF9033_TUNER_TUA9001:
 		len = ARRAY_SIZE(tuner_init_tua9001);
 		init = tuner_init_tua9001;
@@ -411,90 +415,108 @@ static int af9033_init(struct dvb_frontend *fe)
 		init = tuner_init_it9135_62;
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: unsupported tuner ID=%d\n",
-				__func__, state->cfg.tuner);
+		dev_dbg(&dev->client->dev, "unsupported tuner ID=%d\n",
+				dev->cfg.tuner);
 		ret = -ENODEV;
 		goto err;
 	}
 
-	ret = af9033_wr_reg_val_tab(state, init, len);
+	ret = af9033_wr_reg_val_tab(dev, init, len);
 	if (ret < 0)
 		goto err;
 
-	if (state->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
-		ret = af9033_wr_reg_mask(state, 0x00d91c, 0x01, 0x01);
+	if (dev->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
+		ret = af9033_wr_reg_mask(dev, 0x00d91c, 0x01, 0x01);
 		if (ret < 0)
 			goto err;
 
-		ret = af9033_wr_reg_mask(state, 0x00d917, 0x00, 0x01);
+		ret = af9033_wr_reg_mask(dev, 0x00d917, 0x00, 0x01);
 		if (ret < 0)
 			goto err;
 
-		ret = af9033_wr_reg_mask(state, 0x00d916, 0x00, 0x01);
+		ret = af9033_wr_reg_mask(dev, 0x00d916, 0x00, 0x01);
 		if (ret < 0)
 			goto err;
 	}
 
-	switch (state->cfg.tuner) {
+	switch (dev->cfg.tuner) {
 	case AF9033_TUNER_IT9135_60:
 	case AF9033_TUNER_IT9135_61:
 	case AF9033_TUNER_IT9135_62:
-		ret = af9033_wr_reg(state, 0x800000, 0x01);
+		ret = af9033_wr_reg(dev, 0x800000, 0x01);
 		if (ret < 0)
 			goto err;
 	}
 
-	state->bandwidth_hz = 0; /* force to program all parameters */
+	dev->bandwidth_hz = 0; /* force to program all parameters */
+	/* init stats here in order signal app which stats are supported */
+	c->strength.len = 1;
+	c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->cnr.len = 1;
+	c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->block_count.len = 1;
+	c->block_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->block_error.len = 1;
+	c->block_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_count.len = 1;
+	c->post_bit_count.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	c->post_bit_error.len = 1;
+	c->post_bit_error.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	/* start statistics polling */
+	schedule_delayed_work(&dev->stat_work, msecs_to_jiffies(2000));
 
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
 static int af9033_sleep(struct dvb_frontend *fe)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	int ret, i;
 	u8 tmp;
 
-	ret = af9033_wr_reg(state, 0x80004c, 1);
+	/* stop statistics polling */
+	cancel_delayed_work_sync(&dev->stat_work);
+
+	ret = af9033_wr_reg(dev, 0x80004c, 1);
 	if (ret < 0)
 		goto err;
 
-	ret = af9033_wr_reg(state, 0x800000, 0);
+	ret = af9033_wr_reg(dev, 0x800000, 0);
 	if (ret < 0)
 		goto err;
 
 	for (i = 100, tmp = 1; i && tmp; i--) {
-		ret = af9033_rd_reg(state, 0x80004c, &tmp);
+		ret = af9033_rd_reg(dev, 0x80004c, &tmp);
 		if (ret < 0)
 			goto err;
 
 		usleep_range(200, 10000);
 	}
 
-	dev_dbg(&state->i2c->dev, "%s: loop=%d\n", __func__, i);
+	dev_dbg(&dev->client->dev, "loop=%d\n", i);
 
 	if (i == 0) {
 		ret = -ETIMEDOUT;
 		goto err;
 	}
 
-	ret = af9033_wr_reg_mask(state, 0x80fb24, 0x08, 0x08);
+	ret = af9033_wr_reg_mask(dev, 0x80fb24, 0x08, 0x08);
 	if (ret < 0)
 		goto err;
 
 	/* prevent current leak (?) */
-	if (state->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
+	if (dev->cfg.ts_mode == AF9033_TS_MODE_SERIAL) {
 		/* enable parallel TS */
-		ret = af9033_wr_reg_mask(state, 0x00d917, 0x00, 0x01);
+		ret = af9033_wr_reg_mask(dev, 0x00d917, 0x00, 0x01);
 		if (ret < 0)
 			goto err;
 
-		ret = af9033_wr_reg_mask(state, 0x00d916, 0x01, 0x01);
+		ret = af9033_wr_reg_mask(dev, 0x00d916, 0x01, 0x01);
 		if (ret < 0)
 			goto err;
 	}
@@ -502,7 +524,7 @@ static int af9033_sleep(struct dvb_frontend *fe)
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -520,14 +542,14 @@ static int af9033_get_tune_settings(struct dvb_frontend *fe,
 
 static int af9033_set_frontend(struct dvb_frontend *fe)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i, spec_inv, sampling_freq;
 	u8 tmp, buf[3], bandwidth_reg_val;
 	u32 if_frequency, freq_cw, adc_freq;
 
-	dev_dbg(&state->i2c->dev, "%s: frequency=%d bandwidth_hz=%d\n",
-			__func__, c->frequency, c->bandwidth_hz);
+	dev_dbg(&dev->client->dev, "frequency=%d bandwidth_hz=%d\n",
+			c->frequency, c->bandwidth_hz);
 
 	/* check bandwidth */
 	switch (c->bandwidth_hz) {
@@ -541,8 +563,7 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 		bandwidth_reg_val = 0x02;
 		break;
 	default:
-		dev_dbg(&state->i2c->dev, "%s: invalid bandwidth_hz\n",
-				__func__);
+		dev_dbg(&dev->client->dev, "invalid bandwidth_hz\n");
 		ret = -EINVAL;
 		goto err;
 	}
@@ -552,23 +573,23 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 		fe->ops.tuner_ops.set_params(fe);
 
 	/* program CFOE coefficients */
-	if (c->bandwidth_hz != state->bandwidth_hz) {
+	if (c->bandwidth_hz != dev->bandwidth_hz) {
 		for (i = 0; i < ARRAY_SIZE(coeff_lut); i++) {
-			if (coeff_lut[i].clock == state->cfg.clock &&
+			if (coeff_lut[i].clock == dev->cfg.clock &&
 				coeff_lut[i].bandwidth_hz == c->bandwidth_hz) {
 				break;
 			}
 		}
-		ret =  af9033_wr_regs(state, 0x800001,
+		ret =  af9033_wr_regs(dev, 0x800001,
 				coeff_lut[i].val, sizeof(coeff_lut[i].val));
 	}
 
 	/* program frequency control */
-	if (c->bandwidth_hz != state->bandwidth_hz) {
-		spec_inv = state->cfg.spec_inv ? -1 : 1;
+	if (c->bandwidth_hz != dev->bandwidth_hz) {
+		spec_inv = dev->cfg.spec_inv ? -1 : 1;
 
 		for (i = 0; i < ARRAY_SIZE(clock_adc_lut); i++) {
-			if (clock_adc_lut[i].clock == state->cfg.clock)
+			if (clock_adc_lut[i].clock == dev->cfg.clock)
 				break;
 		}
 		adc_freq = clock_adc_lut[i].adc;
@@ -589,12 +610,12 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 		else
 			sampling_freq *= -1;
 
-		freq_cw = af9033_div(state, sampling_freq, adc_freq, 23ul);
+		freq_cw = af9033_div(dev, sampling_freq, adc_freq, 23ul);
 
 		if (spec_inv == -1)
 			freq_cw = 0x800000 - freq_cw;
 
-		if (state->cfg.adc_multiplier == AF9033_ADC_MULTIPLIER_2X)
+		if (dev->cfg.adc_multiplier == AF9033_ADC_MULTIPLIER_2X)
 			freq_cw /= 2;
 
 		buf[0] = (freq_cw >>  0) & 0xff;
@@ -605,26 +626,26 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 		if (if_frequency == 0)
 			buf[2] = 0;
 
-		ret = af9033_wr_regs(state, 0x800029, buf, 3);
+		ret = af9033_wr_regs(dev, 0x800029, buf, 3);
 		if (ret < 0)
 			goto err;
 
-		state->bandwidth_hz = c->bandwidth_hz;
+		dev->bandwidth_hz = c->bandwidth_hz;
 	}
 
-	ret = af9033_wr_reg_mask(state, 0x80f904, bandwidth_reg_val, 0x03);
+	ret = af9033_wr_reg_mask(dev, 0x80f904, bandwidth_reg_val, 0x03);
 	if (ret < 0)
 		goto err;
 
-	ret = af9033_wr_reg(state, 0x800040, 0x00);
+	ret = af9033_wr_reg(dev, 0x800040, 0x00);
 	if (ret < 0)
 		goto err;
 
-	ret = af9033_wr_reg(state, 0x800047, 0x00);
+	ret = af9033_wr_reg(dev, 0x800047, 0x00);
 	if (ret < 0)
 		goto err;
 
-	ret = af9033_wr_reg_mask(state, 0x80f999, 0x00, 0x01);
+	ret = af9033_wr_reg_mask(dev, 0x80f999, 0x00, 0x01);
 	if (ret < 0)
 		goto err;
 
@@ -633,33 +654,33 @@ static int af9033_set_frontend(struct dvb_frontend *fe)
 	else
 		tmp = 0x01; /* UHF */
 
-	ret = af9033_wr_reg(state, 0x80004b, tmp);
+	ret = af9033_wr_reg(dev, 0x80004b, tmp);
 	if (ret < 0)
 		goto err;
 
-	ret = af9033_wr_reg(state, 0x800000, 0x00);
+	ret = af9033_wr_reg(dev, 0x800000, 0x00);
 	if (ret < 0)
 		goto err;
 
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
 static int af9033_get_frontend(struct dvb_frontend *fe)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 	u8 buf[8];
 
-	dev_dbg(&state->i2c->dev, "%s:\n", __func__);
+	dev_dbg(&dev->client->dev, "\n");
 
 	/* read all needed registers */
-	ret = af9033_rd_regs(state, 0x80f900, buf, sizeof(buf));
+	ret = af9033_rd_regs(dev, 0x80f900, buf, sizeof(buf));
 	if (ret < 0)
 		goto err;
 
@@ -771,21 +792,21 @@ static int af9033_get_frontend(struct dvb_frontend *fe)
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
 static int af9033_read_status(struct dvb_frontend *fe, fe_status_t *status)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	int ret;
 	u8 tmp;
 
 	*status = 0;
 
 	/* radio channel status, 0=no result, 1=has signal, 2=no signal */
-	ret = af9033_rd_reg(state, 0x800047, &tmp);
+	ret = af9033_rd_reg(dev, 0x800047, &tmp);
 	if (ret < 0)
 		goto err;
 
@@ -795,7 +816,7 @@ static int af9033_read_status(struct dvb_frontend *fe, fe_status_t *status)
 
 	if (tmp != 0x02) {
 		/* TPS lock */
-		ret = af9033_rd_reg_mask(state, 0x80f5a9, &tmp, 0x01);
+		ret = af9033_rd_reg_mask(dev, 0x80f5a9, &tmp, 0x01);
 		if (ret < 0)
 			goto err;
 
@@ -804,7 +825,7 @@ static int af9033_read_status(struct dvb_frontend *fe, fe_status_t *status)
 					FE_HAS_VITERBI;
 
 		/* full lock */
-		ret = af9033_rd_reg_mask(state, 0x80f999, &tmp, 0x01);
+		ret = af9033_rd_reg_mask(dev, 0x80f999, &tmp, 0x01);
 		if (ret < 0)
 			goto err;
 
@@ -814,76 +835,38 @@ static int af9033_read_status(struct dvb_frontend *fe, fe_status_t *status)
 					FE_HAS_LOCK;
 	}
 
+	dev->fe_status = *status;
+
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
 static int af9033_read_snr(struct dvb_frontend *fe, u16 *snr)
 {
-	struct af9033_state *state = fe->demodulator_priv;
-	int ret, i, len;
-	u8 buf[3], tmp;
-	u32 snr_val;
-	const struct val_snr *uninitialized_var(snr_lut);
-
-	/* read value */
-	ret = af9033_rd_regs(state, 0x80002c, buf, 3);
-	if (ret < 0)
-		goto err;
+	struct af9033_dev *dev = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &dev->fe.dtv_property_cache;
 
-	snr_val = (buf[2] << 16) | (buf[1] << 8) | buf[0];
-
-	/* read current modulation */
-	ret = af9033_rd_reg(state, 0x80f903, &tmp);
-	if (ret < 0)
-		goto err;
-
-	switch ((tmp >> 0) & 3) {
-	case 0:
-		len = ARRAY_SIZE(qpsk_snr_lut);
-		snr_lut = qpsk_snr_lut;
-		break;
-	case 1:
-		len = ARRAY_SIZE(qam16_snr_lut);
-		snr_lut = qam16_snr_lut;
-		break;
-	case 2:
-		len = ARRAY_SIZE(qam64_snr_lut);
-		snr_lut = qam64_snr_lut;
-		break;
-	default:
-		goto err;
-	}
-
-	for (i = 0; i < len; i++) {
-		tmp = snr_lut[i].snr;
-
-		if (snr_val < snr_lut[i].val)
-			break;
-	}
-
-	*snr = tmp * 10; /* dB/10 */
+	/* use DVBv5 CNR */
+	if (c->cnr.stat[0].scale == FE_SCALE_DECIBEL)
+		*snr = div_s64(c->cnr.stat[0].svalue, 100); /* 1000x => 10x */
+	else
+		*snr = 0;
 
 	return 0;
-
-err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
-
-	return ret;
 }
 
 static int af9033_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	int ret;
 	u8 strength2;
 
 	/* read signal strength of 0-100 scale */
-	ret = af9033_rd_reg(state, 0x800048, &strength2);
+	ret = af9033_rd_reg(dev, 0x800048, &strength2);
 	if (ret < 0)
 		goto err;
 
@@ -893,244 +876,225 @@ static int af9033_read_signal_strength(struct dvb_frontend *fe, u16 *strength)
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
-
-	return ret;
-}
-
-static int af9033_update_ch_stat(struct af9033_state *state)
-{
-	int ret = 0;
-	u32 err_cnt, bit_cnt;
-	u16 abort_cnt;
-	u8 buf[7];
-
-	/* only update data every half second */
-	if (time_after(jiffies, state->last_stat_check + msecs_to_jiffies(500))) {
-		ret = af9033_rd_regs(state, 0x800032, buf, sizeof(buf));
-		if (ret < 0)
-			goto err;
-		/* in 8 byte packets? */
-		abort_cnt = (buf[1] << 8) + buf[0];
-		/* in bits */
-		err_cnt = (buf[4] << 16) + (buf[3] << 8) + buf[2];
-		/* in 8 byte packets? always(?) 0x2710 = 10000 */
-		bit_cnt = (buf[6] << 8) + buf[5];
-
-		if (bit_cnt < abort_cnt) {
-			abort_cnt = 1000;
-			state->ber = 0xffffffff;
-		} else {
-			/* 8 byte packets, that have not been rejected already */
-			bit_cnt -= (u32)abort_cnt;
-			if (bit_cnt == 0) {
-				state->ber = 0xffffffff;
-			} else {
-				err_cnt -= (u32)abort_cnt * 8 * 8;
-				bit_cnt *= 8 * 8;
-				state->ber = err_cnt * (0xffffffff / bit_cnt);
-			}
-		}
-		state->ucb += abort_cnt;
-		state->last_stat_check = jiffies;
-	}
-
-	return 0;
-err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
 static int af9033_read_ber(struct dvb_frontend *fe, u32 *ber)
 {
-	struct af9033_state *state = fe->demodulator_priv;
-	int ret;
+	struct af9033_dev *dev = fe->demodulator_priv;
 
-	ret = af9033_update_ch_stat(state);
-	if (ret < 0)
-		return ret;
-
-	*ber = state->ber;
+	*ber = (dev->post_bit_error - dev->post_bit_error_prev);
+	dev->post_bit_error_prev = dev->post_bit_error;
 
 	return 0;
 }
 
 static int af9033_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
 {
-	struct af9033_state *state = fe->demodulator_priv;
-	int ret;
-
-	ret = af9033_update_ch_stat(state);
-	if (ret < 0)
-		return ret;
-
-	*ucblocks = state->ucb;
+	struct af9033_dev *dev = fe->demodulator_priv;
 
+	*ucblocks = dev->error_block_count;
 	return 0;
 }
 
 static int af9033_i2c_gate_ctrl(struct dvb_frontend *fe, int enable)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	int ret;
 
-	dev_dbg(&state->i2c->dev, "%s: enable=%d\n", __func__, enable);
+	dev_dbg(&dev->client->dev, "enable=%d\n", enable);
 
-	ret = af9033_wr_reg_mask(state, 0x00fa04, enable, 0x01);
+	ret = af9033_wr_reg_mask(dev, 0x00fa04, enable, 0x01);
 	if (ret < 0)
 		goto err;
 
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
 static int af9033_pid_filter_ctrl(struct dvb_frontend *fe, int onoff)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	int ret;
 
-	dev_dbg(&state->i2c->dev, "%s: onoff=%d\n", __func__, onoff);
+	dev_dbg(&dev->client->dev, "onoff=%d\n", onoff);
 
-	ret = af9033_wr_reg_mask(state, 0x80f993, onoff, 0x01);
+	ret = af9033_wr_reg_mask(dev, 0x80f993, onoff, 0x01);
 	if (ret < 0)
 		goto err;
 
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
-static int af9033_pid_filter(struct dvb_frontend *fe, int index, u16 pid, int onoff)
+static int af9033_pid_filter(struct dvb_frontend *fe, int index, u16 pid,
+		int onoff)
 {
-	struct af9033_state *state = fe->demodulator_priv;
+	struct af9033_dev *dev = fe->demodulator_priv;
 	int ret;
 	u8 wbuf[2] = {(pid >> 0) & 0xff, (pid >> 8) & 0xff};
 
-	dev_dbg(&state->i2c->dev, "%s: index=%d pid=%04x onoff=%d\n",
-			__func__, index, pid, onoff);
+	dev_dbg(&dev->client->dev, "index=%d pid=%04x onoff=%d\n",
+			index, pid, onoff);
 
 	if (pid > 0x1fff)
 		return 0;
 
-	ret = af9033_wr_regs(state, 0x80f996, wbuf, 2);
+	ret = af9033_wr_regs(dev, 0x80f996, wbuf, 2);
 	if (ret < 0)
 		goto err;
 
-	ret = af9033_wr_reg(state, 0x80f994, onoff);
+	ret = af9033_wr_reg(dev, 0x80f994, onoff);
 	if (ret < 0)
 		goto err;
 
-	ret = af9033_wr_reg(state, 0x80f995, index);
+	ret = af9033_wr_reg(dev, 0x80f995, index);
 	if (ret < 0)
 		goto err;
 
 	return 0;
 
 err:
-	dev_dbg(&state->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
-static struct dvb_frontend_ops af9033_ops;
-
-struct dvb_frontend *af9033_attach(const struct af9033_config *config,
-				   struct i2c_adapter *i2c,
-				   struct af9033_ops *ops)
+static void af9033_stat_work(struct work_struct *work)
 {
-	int ret;
-	struct af9033_state *state;
-	u8 buf[8];
+	struct af9033_dev *dev = container_of(work, struct af9033_dev, stat_work.work);
+	struct dtv_frontend_properties *c = &dev->fe.dtv_property_cache;
+	int ret, tmp, i, len;
+	u8 u8tmp, buf[7];
+
+	dev_dbg(&dev->client->dev, "\n");
+
+	/* signal strength */
+	if (dev->fe_status & FE_HAS_SIGNAL) {
+		if (dev->is_af9035) {
+			ret = af9033_rd_reg(dev, 0x80004a, &u8tmp);
+			tmp = -u8tmp * 1000;
+		} else {
+			ret = af9033_rd_reg(dev, 0x8000f7, &u8tmp);
+			tmp = (u8tmp - 100) * 1000;
+		}
+		if (ret)
+			goto err;
 
-	dev_dbg(&i2c->dev, "%s:\n", __func__);
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_DECIBEL;
+		c->strength.stat[0].svalue = tmp;
+	} else {
+		c->strength.len = 1;
+		c->strength.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
-	/* allocate memory for the internal state */
-	state = kzalloc(sizeof(struct af9033_state), GFP_KERNEL);
-	if (state == NULL)
-		goto err;
+	/* CNR */
+	if (dev->fe_status & FE_HAS_VITERBI) {
+		u32 snr_val;
+		const struct val_snr *snr_lut;
 
-	/* setup the state */
-	state->i2c = i2c;
-	memcpy(&state->cfg, config, sizeof(struct af9033_config));
+		/* read value */
+		ret = af9033_rd_regs(dev, 0x80002c, buf, 3);
+		if (ret)
+			goto err;
 
-	if (state->cfg.clock != 12000000) {
-		dev_err(&state->i2c->dev, "%s: af9033: unsupported clock=%d, " \
-				"only 12000000 Hz is supported currently\n",
-				KBUILD_MODNAME, state->cfg.clock);
-		goto err;
-	}
+		snr_val = (buf[2] << 16) | (buf[1] << 8) | (buf[0] << 0);
 
-	/* firmware version */
-	ret = af9033_rd_regs(state, 0x0083e9, &buf[0], 4);
-	if (ret < 0)
-		goto err;
+		/* read current modulation */
+		ret = af9033_rd_reg(dev, 0x80f903, &u8tmp);
+		if (ret)
+			goto err;
 
-	ret = af9033_rd_regs(state, 0x804191, &buf[4], 4);
-	if (ret < 0)
-		goto err;
+		switch ((u8tmp >> 0) & 3) {
+		case 0:
+			len = ARRAY_SIZE(qpsk_snr_lut);
+			snr_lut = qpsk_snr_lut;
+			break;
+		case 1:
+			len = ARRAY_SIZE(qam16_snr_lut);
+			snr_lut = qam16_snr_lut;
+			break;
+		case 2:
+			len = ARRAY_SIZE(qam64_snr_lut);
+			snr_lut = qam64_snr_lut;
+			break;
+		default:
+			goto err_schedule_delayed_work;
+		}
 
-	dev_info(&state->i2c->dev, "%s: firmware version: LINK=%d.%d.%d.%d " \
-			"OFDM=%d.%d.%d.%d\n", KBUILD_MODNAME, buf[0], buf[1],
-			buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
+		for (i = 0; i < len; i++) {
+			tmp = snr_lut[i].snr * 1000;
+			if (snr_val < snr_lut[i].val)
+				break;
+		}
 
-	/* sleep */
-	switch (state->cfg.tuner) {
-	case AF9033_TUNER_IT9135_38:
-	case AF9033_TUNER_IT9135_51:
-	case AF9033_TUNER_IT9135_52:
-	case AF9033_TUNER_IT9135_60:
-	case AF9033_TUNER_IT9135_61:
-	case AF9033_TUNER_IT9135_62:
-		/* IT9135 did not like to sleep at that early */
-		break;
-	default:
-		ret = af9033_wr_reg(state, 0x80004c, 1);
-		if (ret < 0)
-			goto err;
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_DECIBEL;
+		c->cnr.stat[0].svalue = tmp;
+	} else {
+		c->cnr.len = 1;
+		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	}
 
-		ret = af9033_wr_reg(state, 0x800000, 0);
-		if (ret < 0)
+	/* UCB/PER/BER */
+	if (dev->fe_status & FE_HAS_LOCK) {
+		/* outer FEC, 204 byte packets */
+		u16 abort_packet_count, rsd_packet_count;
+		/* inner FEC, bits */
+		u32 rsd_bit_err_count;
+
+		/*
+		 * Packet count used for measurement is 10000
+		 * (rsd_packet_count). Maybe it should be increased?
+		 */
+
+		ret = af9033_rd_regs(dev, 0x800032, buf, 7);
+		if (ret)
 			goto err;
-	}
 
-	/* configure internal TS mode */
-	switch (state->cfg.ts_mode) {
-	case AF9033_TS_MODE_PARALLEL:
-		state->ts_mode_parallel = true;
-		break;
-	case AF9033_TS_MODE_SERIAL:
-		state->ts_mode_serial = true;
-		break;
-	case AF9033_TS_MODE_USB:
-		/* usb mode for AF9035 */
-	default:
-		break;
-	}
+		abort_packet_count = (buf[1] << 8) | (buf[0] << 0);
+		rsd_bit_err_count = (buf[4] << 16) | (buf[3] << 8) | buf[2];
+		rsd_packet_count = (buf[6] << 8) | (buf[5] << 0);
 
-	/* create dvb_frontend */
-	memcpy(&state->fe.ops, &af9033_ops, sizeof(struct dvb_frontend_ops));
-	state->fe.demodulator_priv = state;
+		dev->error_block_count += abort_packet_count;
+		dev->total_block_count += rsd_packet_count;
+		dev->post_bit_error += rsd_bit_err_count;
+		dev->post_bit_count += rsd_packet_count * 204 * 8;
 
-	if (ops) {
-		ops->pid_filter = af9033_pid_filter;
-		ops->pid_filter_ctrl = af9033_pid_filter_ctrl;
-	}
+		c->block_count.len = 1;
+		c->block_count.stat[0].scale = FE_SCALE_COUNTER;
+		c->block_count.stat[0].uvalue = dev->total_block_count;
 
-	return &state->fe;
+		c->block_error.len = 1;
+		c->block_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->block_error.stat[0].uvalue = dev->error_block_count;
 
+		c->post_bit_count.len = 1;
+		c->post_bit_count.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_count.stat[0].uvalue = dev->post_bit_count;
+
+		c->post_bit_error.len = 1;
+		c->post_bit_error.stat[0].scale = FE_SCALE_COUNTER;
+		c->post_bit_error.stat[0].uvalue = dev->post_bit_error;
+	}
+
+err_schedule_delayed_work:
+	schedule_delayed_work(&dev->stat_work, msecs_to_jiffies(2000));
+	return;
 err:
-	kfree(state);
-	return NULL;
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 }
-EXPORT_SYMBOL(af9033_attach);
 
 static struct dvb_frontend_ops af9033_ops = {
 	.delsys = { SYS_DVBT },
@@ -1157,8 +1121,6 @@ static struct dvb_frontend_ops af9033_ops = {
 			FE_CAN_MUTE_TS
 	},
 
-	.release = af9033_release,
-
 	.init = af9033_init,
 	.sleep = af9033_sleep,
 
@@ -1175,6 +1137,150 @@ static struct dvb_frontend_ops af9033_ops = {
 	.i2c_gate_ctrl = af9033_i2c_gate_ctrl,
 };
 
+static int af9033_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct af9033_config *cfg = client->dev.platform_data;
+	struct af9033_dev *dev;
+	int ret;
+	u8 buf[8];
+	u32 reg;
+
+	/* allocate memory for the internal state */
+	dev = kzalloc(sizeof(struct af9033_dev), GFP_KERNEL);
+	if (dev == NULL) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "Could not allocate memory for state\n");
+		goto err;
+	}
+
+	/* setup the state */
+	dev->client = client;
+	INIT_DELAYED_WORK(&dev->stat_work, af9033_stat_work);
+	memcpy(&dev->cfg, cfg, sizeof(struct af9033_config));
+
+	if (dev->cfg.clock != 12000000) {
+		ret = -ENODEV;
+		dev_err(&dev->client->dev,
+				"unsupported clock %d Hz, only 12000000 Hz is supported currently\n",
+				dev->cfg.clock);
+		goto err_kfree;
+	}
+
+	/* firmware version */
+	switch (dev->cfg.tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		dev->is_it9135 = true;
+		reg = 0x004bfc;
+		break;
+	default:
+		dev->is_af9035 = true;
+		reg = 0x0083e9;
+		break;
+	}
+
+	ret = af9033_rd_regs(dev, reg, &buf[0], 4);
+	if (ret < 0)
+		goto err_kfree;
+
+	ret = af9033_rd_regs(dev, 0x804191, &buf[4], 4);
+	if (ret < 0)
+		goto err_kfree;
+
+	dev_info(&dev->client->dev,
+			"firmware version: LINK %d.%d.%d.%d - OFDM %d.%d.%d.%d\n",
+			buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6],
+			buf[7]);
+
+	/* sleep */
+	switch (dev->cfg.tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		/* IT9135 did not like to sleep at that early */
+		break;
+	default:
+		ret = af9033_wr_reg(dev, 0x80004c, 1);
+		if (ret < 0)
+			goto err_kfree;
+
+		ret = af9033_wr_reg(dev, 0x800000, 0);
+		if (ret < 0)
+			goto err_kfree;
+	}
+
+	/* configure internal TS mode */
+	switch (dev->cfg.ts_mode) {
+	case AF9033_TS_MODE_PARALLEL:
+		dev->ts_mode_parallel = true;
+		break;
+	case AF9033_TS_MODE_SERIAL:
+		dev->ts_mode_serial = true;
+		break;
+	case AF9033_TS_MODE_USB:
+		/* usb mode for AF9035 */
+	default:
+		break;
+	}
+
+	/* create dvb_frontend */
+	memcpy(&dev->fe.ops, &af9033_ops, sizeof(struct dvb_frontend_ops));
+	dev->fe.demodulator_priv = dev;
+	*cfg->fe = &dev->fe;
+	if (cfg->ops) {
+		cfg->ops->pid_filter = af9033_pid_filter;
+		cfg->ops->pid_filter_ctrl = af9033_pid_filter_ctrl;
+	}
+	i2c_set_clientdata(client, dev);
+
+	dev_info(&dev->client->dev, "Afatech AF9033 successfully attached\n");
+	return 0;
+err_kfree:
+	kfree(dev);
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	return ret;
+}
+
+static int af9033_remove(struct i2c_client *client)
+{
+	struct af9033_dev *dev = i2c_get_clientdata(client);
+
+	dev_dbg(&dev->client->dev, "\n");
+
+	dev->fe.ops.release = NULL;
+	dev->fe.demodulator_priv = NULL;
+	kfree(dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id af9033_id_table[] = {
+	{"af9033", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, af9033_id_table);
+
+static struct i2c_driver af9033_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "af9033",
+	},
+	.probe		= af9033_probe,
+	.remove		= af9033_remove,
+	.id_table	= af9033_id_table,
+};
+
+module_i2c_driver(af9033_driver);
+
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
 MODULE_DESCRIPTION("Afatech AF9033 DVB-T demodulator driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb-frontends/af9033.h b/drivers/media/dvb-frontends/af9033.h
index 539f4db678b8..6ad22b69a636 100644
--- a/drivers/media/dvb-frontends/af9033.h
+++ b/drivers/media/dvb-frontends/af9033.h
@@ -24,13 +24,12 @@
 
 #include <linux/kconfig.h>
 
+/*
+ * I2C address (TODO: are these in 8-bit format?)
+ * 0x38, 0x3a, 0x3c, 0x3e
+ */
 struct af9033_config {
 	/*
-	 * I2C address
-	 */
-	u8 i2c_addr;
-
-	/*
 	 * clock Hz
 	 * 12000000, 22000000, 24000000, 34000000, 32000000, 28000000, 26000000,
 	 * 30000000, 36000000, 20480000, 16384000
@@ -75,8 +74,23 @@ struct af9033_config {
 	 * input spectrum inversion
 	 */
 	bool spec_inv;
-};
 
+	/*
+	 *
+	 */
+	bool dyn0_clk;
+
+	/*
+	 * PID filter ops
+	 */
+	struct af9033_ops *ops;
+
+	/*
+	 * frontend
+	 * returned by that driver
+	 */
+	struct dvb_frontend **fe;
+};
 
 struct af9033_ops {
 	int (*pid_filter_ctrl)(struct dvb_frontend *fe, int onoff);
@@ -84,36 +98,4 @@ struct af9033_ops {
 			  int onoff);
 };
 
-
-#if IS_ENABLED(CONFIG_DVB_AF9033)
-extern
-struct dvb_frontend *af9033_attach(const struct af9033_config *config,
-				   struct i2c_adapter *i2c,
-				   struct af9033_ops *ops);
-
-#else
-static inline
-struct dvb_frontend *af9033_attach(const struct af9033_config *config,
-				   struct i2c_adapter *i2c,
-				   struct af9033_ops *ops)
-{
-	pr_warn("%s: driver disabled by Kconfig\n", __func__);
-	return NULL;
-}
-
-static inline int af9033_pid_filter_ctrl(struct dvb_frontend *fe, int onoff)
-{
-	pr_warn("%s: driver disabled by Kconfig\n", __func__);
-	return -ENODEV;
-}
-
-static inline int af9033_pid_filter(struct dvb_frontend *fe, int index, u16 pid,
-	int onoff)
-{
-	pr_warn("%s: driver disabled by Kconfig\n", __func__);
-	return -ENODEV;
-}
-
-#endif
-
 #endif /* AF9033_H */
diff --git a/drivers/media/dvb-frontends/af9033_priv.h b/drivers/media/dvb-frontends/af9033_priv.h
index fc2ad581e302..c12c92cb5855 100644
--- a/drivers/media/dvb-frontends/af9033_priv.h
+++ b/drivers/media/dvb-frontends/af9033_priv.h
@@ -24,6 +24,7 @@
 
 #include "dvb_frontend.h"
 #include "af9033.h"
+#include <linux/math64.h>
 
 struct reg_val {
 	u32 reg;
@@ -1418,7 +1419,7 @@ static const struct reg_val tuner_init_it9135_60[] = {
 	{ 0x800068, 0x0a },
 	{ 0x80006a, 0x03 },
 	{ 0x800070, 0x0a },
-	{ 0x800071, 0x05 },
+	{ 0x800071, 0x0a },
 	{ 0x800072, 0x02 },
 	{ 0x800075, 0x8c },
 	{ 0x800076, 0x8c },
@@ -1484,7 +1485,6 @@ static const struct reg_val tuner_init_it9135_60[] = {
 	{ 0x800104, 0x02 },
 	{ 0x800105, 0xbe },
 	{ 0x800106, 0x00 },
-	{ 0x800109, 0x02 },
 	{ 0x800115, 0x0a },
 	{ 0x800116, 0x03 },
 	{ 0x80011a, 0xbe },
@@ -1510,7 +1510,6 @@ static const struct reg_val tuner_init_it9135_60[] = {
 	{ 0x80014b, 0x8c },
 	{ 0x80014d, 0xac },
 	{ 0x80014e, 0xc6 },
-	{ 0x80014f, 0x03 },
 	{ 0x800151, 0x1e },
 	{ 0x800153, 0xbc },
 	{ 0x800178, 0x09 },
@@ -1522,9 +1521,10 @@ static const struct reg_val tuner_init_it9135_60[] = {
 	{ 0x80018d, 0x5f },
 	{ 0x80018f, 0xa0 },
 	{ 0x800190, 0x5a },
-	{ 0x80ed02, 0xff },
-	{ 0x80ee42, 0xff },
-	{ 0x80ee82, 0xff },
+	{ 0x800191, 0x00 },
+	{ 0x80ed02, 0x40 },
+	{ 0x80ee42, 0x40 },
+	{ 0x80ee82, 0x40 },
 	{ 0x80f000, 0x0f },
 	{ 0x80f01f, 0x8c },
 	{ 0x80f020, 0x00 },
@@ -1699,7 +1699,6 @@ static const struct reg_val tuner_init_it9135_61[] = {
 	{ 0x800104, 0x02 },
 	{ 0x800105, 0xc8 },
 	{ 0x800106, 0x00 },
-	{ 0x800109, 0x02 },
 	{ 0x800115, 0x0a },
 	{ 0x800116, 0x03 },
 	{ 0x80011a, 0xc6 },
@@ -1725,7 +1724,6 @@ static const struct reg_val tuner_init_it9135_61[] = {
 	{ 0x80014b, 0x8c },
 	{ 0x80014d, 0xa8 },
 	{ 0x80014e, 0xc6 },
-	{ 0x80014f, 0x03 },
 	{ 0x800151, 0x28 },
 	{ 0x800153, 0xcc },
 	{ 0x800178, 0x09 },
@@ -1737,9 +1735,10 @@ static const struct reg_val tuner_init_it9135_61[] = {
 	{ 0x80018d, 0x5f },
 	{ 0x80018f, 0xfb },
 	{ 0x800190, 0x5c },
-	{ 0x80ed02, 0xff },
-	{ 0x80ee42, 0xff },
-	{ 0x80ee82, 0xff },
+	{ 0x800191, 0x00 },
+	{ 0x80ed02, 0x40 },
+	{ 0x80ee42, 0x40 },
+	{ 0x80ee82, 0x40 },
 	{ 0x80f000, 0x0f },
 	{ 0x80f01f, 0x8c },
 	{ 0x80f020, 0x00 },
diff --git a/drivers/media/dvb-frontends/as102_fe.c b/drivers/media/dvb-frontends/as102_fe.c
new file mode 100644
index 000000000000..493665899565
--- /dev/null
+++ b/drivers/media/dvb-frontends/as102_fe.c
@@ -0,0 +1,480 @@
+/*
+ * Abilis Systems Single DVB-T Receiver
+ * Copyright (C) 2008 Pierrick Hascoet <pierrick.hascoet@abilis.com>
+ * Copyright (C) 2010 Devin Heitmueller <dheitmueller@kernellabs.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <dvb_frontend.h>
+
+#include "as102_fe.h"
+
+struct as102_state {
+	struct dvb_frontend frontend;
+	struct as10x_demod_stats demod_stats;
+
+	const struct as102_fe_ops *ops;
+	void *priv;
+	uint8_t elna_cfg;
+
+	/* signal strength */
+	uint16_t signal_strength;
+	/* bit error rate */
+	uint32_t ber;
+};
+
+static uint8_t as102_fe_get_code_rate(fe_code_rate_t arg)
+{
+	uint8_t c;
+
+	switch (arg) {
+	case FEC_1_2:
+		c = CODE_RATE_1_2;
+		break;
+	case FEC_2_3:
+		c = CODE_RATE_2_3;
+		break;
+	case FEC_3_4:
+		c = CODE_RATE_3_4;
+		break;
+	case FEC_5_6:
+		c = CODE_RATE_5_6;
+		break;
+	case FEC_7_8:
+		c = CODE_RATE_7_8;
+		break;
+	default:
+		c = CODE_RATE_UNKNOWN;
+		break;
+	}
+
+	return c;
+}
+
+static int as102_fe_set_frontend(struct dvb_frontend *fe)
+{
+	struct as102_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	struct as10x_tune_args tune_args = { 0 };
+
+	/* set frequency */
+	tune_args.freq = c->frequency / 1000;
+
+	/* fix interleaving_mode */
+	tune_args.interleaving_mode = INTLV_NATIVE;
+
+	switch (c->bandwidth_hz) {
+	case 8000000:
+		tune_args.bandwidth = BW_8_MHZ;
+		break;
+	case 7000000:
+		tune_args.bandwidth = BW_7_MHZ;
+		break;
+	case 6000000:
+		tune_args.bandwidth = BW_6_MHZ;
+		break;
+	default:
+		tune_args.bandwidth = BW_8_MHZ;
+	}
+
+	switch (c->guard_interval) {
+	case GUARD_INTERVAL_1_32:
+		tune_args.guard_interval = GUARD_INT_1_32;
+		break;
+	case GUARD_INTERVAL_1_16:
+		tune_args.guard_interval = GUARD_INT_1_16;
+		break;
+	case GUARD_INTERVAL_1_8:
+		tune_args.guard_interval = GUARD_INT_1_8;
+		break;
+	case GUARD_INTERVAL_1_4:
+		tune_args.guard_interval = GUARD_INT_1_4;
+		break;
+	case GUARD_INTERVAL_AUTO:
+	default:
+		tune_args.guard_interval = GUARD_UNKNOWN;
+		break;
+	}
+
+	switch (c->modulation) {
+	case QPSK:
+		tune_args.modulation = CONST_QPSK;
+		break;
+	case QAM_16:
+		tune_args.modulation = CONST_QAM16;
+		break;
+	case QAM_64:
+		tune_args.modulation = CONST_QAM64;
+		break;
+	default:
+		tune_args.modulation = CONST_UNKNOWN;
+		break;
+	}
+
+	switch (c->transmission_mode) {
+	case TRANSMISSION_MODE_2K:
+		tune_args.transmission_mode = TRANS_MODE_2K;
+		break;
+	case TRANSMISSION_MODE_8K:
+		tune_args.transmission_mode = TRANS_MODE_8K;
+		break;
+	default:
+		tune_args.transmission_mode = TRANS_MODE_UNKNOWN;
+	}
+
+	switch (c->hierarchy) {
+	case HIERARCHY_NONE:
+		tune_args.hierarchy = HIER_NONE;
+		break;
+	case HIERARCHY_1:
+		tune_args.hierarchy = HIER_ALPHA_1;
+		break;
+	case HIERARCHY_2:
+		tune_args.hierarchy = HIER_ALPHA_2;
+		break;
+	case HIERARCHY_4:
+		tune_args.hierarchy = HIER_ALPHA_4;
+		break;
+	case HIERARCHY_AUTO:
+		tune_args.hierarchy = HIER_UNKNOWN;
+		break;
+	}
+
+	pr_debug("as102: tuner parameters: freq: %d  bw: 0x%02x  gi: 0x%02x\n",
+			c->frequency,
+			tune_args.bandwidth,
+			tune_args.guard_interval);
+
+	/*
+	 * Detect a hierarchy selection
+	 * if HP/LP are both set to FEC_NONE, HP will be selected.
+	 */
+	if ((tune_args.hierarchy != HIER_NONE) &&
+		       ((c->code_rate_LP == FEC_NONE) ||
+			(c->code_rate_HP == FEC_NONE))) {
+
+		if (c->code_rate_LP == FEC_NONE) {
+			tune_args.hier_select = HIER_HIGH_PRIORITY;
+			tune_args.code_rate =
+			   as102_fe_get_code_rate(c->code_rate_HP);
+		}
+
+		if (c->code_rate_HP == FEC_NONE) {
+			tune_args.hier_select = HIER_LOW_PRIORITY;
+			tune_args.code_rate =
+			   as102_fe_get_code_rate(c->code_rate_LP);
+		}
+
+		pr_debug("as102: \thierarchy: 0x%02x  selected: %s  code_rate_%s: 0x%02x\n",
+			tune_args.hierarchy,
+			tune_args.hier_select == HIER_HIGH_PRIORITY ?
+			"HP" : "LP",
+			tune_args.hier_select == HIER_HIGH_PRIORITY ?
+			"HP" : "LP",
+			tune_args.code_rate);
+	} else {
+		tune_args.code_rate =
+			as102_fe_get_code_rate(c->code_rate_HP);
+	}
+
+	/* Set frontend arguments */
+	return state->ops->set_tune(state->priv, &tune_args);
+}
+
+static int as102_fe_get_frontend(struct dvb_frontend *fe)
+{
+	struct as102_state *state = fe->demodulator_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret = 0;
+	struct as10x_tps tps = { 0 };
+
+	/* send abilis command: GET_TPS */
+	ret = state->ops->get_tps(state->priv, &tps);
+	if (ret < 0)
+		return ret;
+
+	/* extract constellation */
+	switch (tps.modulation) {
+	case CONST_QPSK:
+		c->modulation = QPSK;
+		break;
+	case CONST_QAM16:
+		c->modulation = QAM_16;
+		break;
+	case CONST_QAM64:
+		c->modulation = QAM_64;
+		break;
+	}
+
+	/* extract hierarchy */
+	switch (tps.hierarchy) {
+	case HIER_NONE:
+		c->hierarchy = HIERARCHY_NONE;
+		break;
+	case HIER_ALPHA_1:
+		c->hierarchy = HIERARCHY_1;
+		break;
+	case HIER_ALPHA_2:
+		c->hierarchy = HIERARCHY_2;
+		break;
+	case HIER_ALPHA_4:
+		c->hierarchy = HIERARCHY_4;
+		break;
+	}
+
+	/* extract code rate HP */
+	switch (tps.code_rate_HP) {
+	case CODE_RATE_1_2:
+		c->code_rate_HP = FEC_1_2;
+		break;
+	case CODE_RATE_2_3:
+		c->code_rate_HP = FEC_2_3;
+		break;
+	case CODE_RATE_3_4:
+		c->code_rate_HP = FEC_3_4;
+		break;
+	case CODE_RATE_5_6:
+		c->code_rate_HP = FEC_5_6;
+		break;
+	case CODE_RATE_7_8:
+		c->code_rate_HP = FEC_7_8;
+		break;
+	}
+
+	/* extract code rate LP */
+	switch (tps.code_rate_LP) {
+	case CODE_RATE_1_2:
+		c->code_rate_LP = FEC_1_2;
+		break;
+	case CODE_RATE_2_3:
+		c->code_rate_LP = FEC_2_3;
+		break;
+	case CODE_RATE_3_4:
+		c->code_rate_LP = FEC_3_4;
+		break;
+	case CODE_RATE_5_6:
+		c->code_rate_LP = FEC_5_6;
+		break;
+	case CODE_RATE_7_8:
+		c->code_rate_LP = FEC_7_8;
+		break;
+	}
+
+	/* extract guard interval */
+	switch (tps.guard_interval) {
+	case GUARD_INT_1_32:
+		c->guard_interval = GUARD_INTERVAL_1_32;
+		break;
+	case GUARD_INT_1_16:
+		c->guard_interval = GUARD_INTERVAL_1_16;
+		break;
+	case GUARD_INT_1_8:
+		c->guard_interval = GUARD_INTERVAL_1_8;
+		break;
+	case GUARD_INT_1_4:
+		c->guard_interval = GUARD_INTERVAL_1_4;
+		break;
+	}
+
+	/* extract transmission mode */
+	switch (tps.transmission_mode) {
+	case TRANS_MODE_2K:
+		c->transmission_mode = TRANSMISSION_MODE_2K;
+		break;
+	case TRANS_MODE_8K:
+		c->transmission_mode = TRANSMISSION_MODE_8K;
+		break;
+	}
+
+	return 0;
+}
+
+static int as102_fe_get_tune_settings(struct dvb_frontend *fe,
+			struct dvb_frontend_tune_settings *settings) {
+
+	settings->min_delay_ms = 1000;
+
+	return 0;
+}
+
+static int as102_fe_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	int ret = 0;
+	struct as102_state *state = fe->demodulator_priv;
+	struct as10x_tune_status tstate = { 0 };
+
+	/* send abilis command: GET_TUNE_STATUS */
+	ret = state->ops->get_status(state->priv, &tstate);
+	if (ret < 0)
+		return ret;
+
+	state->signal_strength  = tstate.signal_strength;
+	state->ber  = tstate.BER;
+
+	switch (tstate.tune_state) {
+	case TUNE_STATUS_SIGNAL_DVB_OK:
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER;
+		break;
+	case TUNE_STATUS_STREAM_DETECTED:
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER | FE_HAS_SYNC |
+			  FE_HAS_VITERBI;
+		break;
+	case TUNE_STATUS_STREAM_TUNED:
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER | FE_HAS_SYNC |
+			  FE_HAS_LOCK | FE_HAS_VITERBI;
+		break;
+	default:
+		*status = TUNE_STATUS_NOT_TUNED;
+	}
+
+	pr_debug("as102: tuner status: 0x%02x, strength %d, per: %d, ber: %d\n",
+		 tstate.tune_state, tstate.signal_strength,
+		 tstate.PER, tstate.BER);
+
+	if (!(*status & FE_HAS_LOCK)) {
+		memset(&state->demod_stats, 0, sizeof(state->demod_stats));
+		return 0;
+	}
+
+	ret = state->ops->get_stats(state->priv, &state->demod_stats);
+	if (ret < 0)
+		memset(&state->demod_stats, 0, sizeof(state->demod_stats));
+
+	return ret;
+}
+
+/*
+ * Note:
+ * - in AS102 SNR=MER
+ *   - the SNR will be returned in linear terms, i.e. not in dB
+ *   - the accuracy equals ±2dB for a SNR range from 4dB to 30dB
+ *   - the accuracy is >2dB for SNR values outside this range
+ */
+static int as102_fe_read_snr(struct dvb_frontend *fe, u16 *snr)
+{
+	struct as102_state *state = fe->demodulator_priv;
+
+	*snr = state->demod_stats.mer;
+
+	return 0;
+}
+
+static int as102_fe_read_ber(struct dvb_frontend *fe, u32 *ber)
+{
+	struct as102_state *state = fe->demodulator_priv;
+
+	*ber = state->ber;
+
+	return 0;
+}
+
+static int as102_fe_read_signal_strength(struct dvb_frontend *fe,
+					 u16 *strength)
+{
+	struct as102_state *state = fe->demodulator_priv;
+
+	*strength = (((0xffff * 400) * state->signal_strength + 41000) * 2);
+
+	return 0;
+}
+
+static int as102_fe_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
+{
+	struct as102_state *state = fe->demodulator_priv;
+
+	if (state->demod_stats.has_started)
+		*ucblocks = state->demod_stats.bad_frame_count;
+	else
+		*ucblocks = 0;
+
+	return 0;
+}
+
+static int as102_fe_ts_bus_ctrl(struct dvb_frontend *fe, int acquire)
+{
+	struct as102_state *state = fe->demodulator_priv;
+
+	return state->ops->stream_ctrl(state->priv, acquire,
+				      state->elna_cfg);
+}
+
+static void as102_fe_release(struct dvb_frontend *fe)
+{
+	struct as102_state *state = fe->demodulator_priv;
+
+	kfree(state);
+}
+
+
+static struct dvb_frontend_ops as102_fe_ops = {
+	.delsys = { SYS_DVBT },
+	.info = {
+		.name			= "Abilis AS102 DVB-T",
+		.frequency_min		= 174000000,
+		.frequency_max		= 862000000,
+		.frequency_stepsize	= 166667,
+		.caps = FE_CAN_INVERSION_AUTO
+			| FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4
+			| FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO
+			| FE_CAN_QAM_16 | FE_CAN_QAM_64 | FE_CAN_QPSK
+			| FE_CAN_QAM_AUTO
+			| FE_CAN_TRANSMISSION_MODE_AUTO
+			| FE_CAN_GUARD_INTERVAL_AUTO
+			| FE_CAN_HIERARCHY_AUTO
+			| FE_CAN_RECOVER
+			| FE_CAN_MUTE_TS
+	},
+
+	.set_frontend		= as102_fe_set_frontend,
+	.get_frontend		= as102_fe_get_frontend,
+	.get_tune_settings	= as102_fe_get_tune_settings,
+
+	.read_status		= as102_fe_read_status,
+	.read_snr		= as102_fe_read_snr,
+	.read_ber		= as102_fe_read_ber,
+	.read_signal_strength	= as102_fe_read_signal_strength,
+	.read_ucblocks		= as102_fe_read_ucblocks,
+	.ts_bus_ctrl		= as102_fe_ts_bus_ctrl,
+	.release		= as102_fe_release,
+};
+
+struct dvb_frontend *as102_attach(const char *name,
+				  const struct as102_fe_ops *ops,
+				  void *priv,
+				  uint8_t elna_cfg)
+{
+	struct as102_state *state;
+	struct dvb_frontend *fe;
+
+	state = kzalloc(sizeof(struct as102_state), GFP_KERNEL);
+	if (state == NULL) {
+		pr_err("%s: unable to allocate memory for state\n", __func__);
+		return NULL;
+	}
+	fe = &state->frontend;
+	fe->demodulator_priv = state;
+	state->ops = ops;
+	state->priv = priv;
+	state->elna_cfg = elna_cfg;
+
+	/* init frontend callback ops */
+	memcpy(&fe->ops, &as102_fe_ops, sizeof(struct dvb_frontend_ops));
+	strncpy(fe->ops.info.name, name, sizeof(fe->ops.info.name));
+
+	return fe;
+
+}
+EXPORT_SYMBOL_GPL(as102_attach);
+
+MODULE_DESCRIPTION("as102-fe");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Pierrick Hascoet <pierrick.hascoet@abilis.com>");
diff --git a/drivers/media/dvb-frontends/as102_fe.h b/drivers/media/dvb-frontends/as102_fe.h
new file mode 100644
index 000000000000..a7c91430ca3d
--- /dev/null
+++ b/drivers/media/dvb-frontends/as102_fe.h
@@ -0,0 +1,29 @@
+/*
+ * Abilis Systems Single DVB-T Receiver
+ * Copyright (C) 2014 Mauro Carvalho Chehab <m.chehab@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "as102_fe_types.h"
+
+struct as102_fe_ops {
+	int (*set_tune)(void *priv, struct as10x_tune_args *tune_args);
+	int (*get_tps)(void *priv, struct as10x_tps *tps);
+	int (*get_status)(void *priv, struct as10x_tune_status *tstate);
+	int (*get_stats)(void *priv, struct as10x_demod_stats *demod_stats);
+	int (*stream_ctrl)(void *priv, int acquire, uint32_t elna_cfg);
+};
+
+struct dvb_frontend *as102_attach(const char *name,
+				  const struct as102_fe_ops *ops,
+				  void *priv,
+				  uint8_t elna_cfg);
diff --git a/drivers/staging/media/as102/as10x_types.h b/drivers/media/dvb-frontends/as102_fe_types.h
index af26e057d9a2..80a5398b580f 100644
--- a/drivers/staging/media/as102/as10x_types.h
+++ b/drivers/media/dvb-frontends/as102_fe_types.h
@@ -11,16 +11,10 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #ifndef _AS10X_TYPES_H_
 #define _AS10X_TYPES_H_
 
-#include "as10x_handle.h"
-
 /*********************************/
 /*       MACRO DEFINITIONS       */
 /*********************************/
diff --git a/drivers/media/dvb-frontends/bcm3510.c b/drivers/media/dvb-frontends/bcm3510.c
index 39a29dd29519..638c7aa0fb7e 100644
--- a/drivers/media/dvb-frontends/bcm3510.c
+++ b/drivers/media/dvb-frontends/bcm3510.c
@@ -639,12 +639,12 @@ static int bcm3510_download_firmware(struct dvb_frontend* fe)
 		err("could not load firmware (%s): %d",BCM3510_DEFAULT_FIRMWARE,ret);
 		return ret;
 	}
-	deb_info("got firmware: %zd\n",fw->size);
+	deb_info("got firmware: %zu\n", fw->size);
 
 	b = fw->data;
 	for (i = 0; i < fw->size;) {
-		addr = le16_to_cpu( *( (u16 *)&b[i] ) );
-		len  = le16_to_cpu( *( (u16 *)&b[i+2] ) );
+		addr = le16_to_cpu(*((__le16 *)&b[i]));
+		len  = le16_to_cpu(*((__le16 *)&b[i+2]));
 		deb_info("firmware chunk, addr: 0x%04x, len: 0x%04x, total length: 0x%04zx\n",addr,len,fw->size);
 		if ((ret = bcm3510_write_ram(st,addr,&b[i+4],len)) < 0) {
 			err("firmware download failed: %d\n",ret);
diff --git a/drivers/media/dvb-frontends/cx24123.c b/drivers/media/dvb-frontends/cx24123.c
index 72fb5838cae0..7975c6608e20 100644
--- a/drivers/media/dvb-frontends/cx24123.c
+++ b/drivers/media/dvb-frontends/cx24123.c
@@ -1095,6 +1095,7 @@ struct dvb_frontend *cx24123_attach(const struct cx24123_config *config,
 		sizeof(state->tuner_i2c_adapter.name));
 	state->tuner_i2c_adapter.algo      = &cx24123_tuner_i2c_algo;
 	state->tuner_i2c_adapter.algo_data = NULL;
+	state->tuner_i2c_adapter.dev.parent = i2c->dev.parent;
 	i2c_set_adapdata(&state->tuner_i2c_adapter, state);
 	if (i2c_add_adapter(&state->tuner_i2c_adapter) < 0) {
 		err("tuner i2c bus could not be initialized\n");
diff --git a/drivers/media/dvb-frontends/cxd2820r_c.c b/drivers/media/dvb-frontends/cxd2820r_c.c
index 0f4657e01cde..149fdca3fb44 100644
--- a/drivers/media/dvb-frontends/cxd2820r_c.c
+++ b/drivers/media/dvb-frontends/cxd2820r_c.c
@@ -65,7 +65,7 @@ int cxd2820r_set_frontend_c(struct dvb_frontend *fe)
 	}
 
 	priv->delivery_system = SYS_DVBC_ANNEX_A;
-	priv->ber_running = 0; /* tune stops BER counter */
+	priv->ber_running = false; /* tune stops BER counter */
 
 	/* program IF frequency */
 	if (fe->ops.tuner_ops.get_if_frequency) {
@@ -168,7 +168,7 @@ int cxd2820r_read_ber_c(struct dvb_frontend *fe, u32 *ber)
 			start_ber = 1;
 		}
 	} else {
-		priv->ber_running = 1;
+		priv->ber_running = true;
 		start_ber = 1;
 	}
 
diff --git a/drivers/media/dvb-frontends/cxd2820r_core.c b/drivers/media/dvb-frontends/cxd2820r_core.c
index 03930d5e9fea..422e84bbb008 100644
--- a/drivers/media/dvb-frontends/cxd2820r_core.c
+++ b/drivers/media/dvb-frontends/cxd2820r_core.c
@@ -564,10 +564,10 @@ static enum dvbfe_search cxd2820r_search(struct dvb_frontend *fe)
 
 	/* check if we have a valid signal */
 	if (status & FE_HAS_LOCK) {
-		priv->last_tune_failed = 0;
+		priv->last_tune_failed = false;
 		return DVBFE_ALGO_SEARCH_SUCCESS;
 	} else {
-		priv->last_tune_failed = 1;
+		priv->last_tune_failed = true;
 		return DVBFE_ALGO_SEARCH_AGAIN;
 	}
 
@@ -584,18 +584,14 @@ static int cxd2820r_get_frontend_algo(struct dvb_frontend *fe)
 static void cxd2820r_release(struct dvb_frontend *fe)
 {
 	struct cxd2820r_priv *priv = fe->demodulator_priv;
-	int uninitialized_var(ret); /* silence compiler warning */
 
 	dev_dbg(&priv->i2c->dev, "%s\n", __func__);
 
 #ifdef CONFIG_GPIOLIB
 	/* remove GPIOs */
-	if (priv->gpio_chip.label) {
-		ret = gpiochip_remove(&priv->gpio_chip);
-		if (ret)
-			dev_err(&priv->i2c->dev, "%s: gpiochip_remove() " \
-					"failed=%d\n", KBUILD_MODNAME, ret);
-	}
+	if (priv->gpio_chip.label)
+		gpiochip_remove(&priv->gpio_chip);
+
 #endif
 	kfree(priv);
 	return;
diff --git a/drivers/media/dvb-frontends/cxd2820r_t.c b/drivers/media/dvb-frontends/cxd2820r_t.c
index 9b5a45b907bc..51401d036530 100644
--- a/drivers/media/dvb-frontends/cxd2820r_t.c
+++ b/drivers/media/dvb-frontends/cxd2820r_t.c
@@ -89,7 +89,7 @@ int cxd2820r_set_frontend_t(struct dvb_frontend *fe)
 	}
 
 	priv->delivery_system = SYS_DVBT;
-	priv->ber_running = 0; /* tune stops BER counter */
+	priv->ber_running = false; /* tune stops BER counter */
 
 	/* program IF frequency */
 	if (fe->ops.tuner_ops.get_if_frequency) {
@@ -272,7 +272,7 @@ int cxd2820r_read_ber_t(struct dvb_frontend *fe, u32 *ber)
 			start_ber = 1;
 		}
 	} else {
-		priv->ber_running = 1;
+		priv->ber_running = true;
 		start_ber = 1;
 	}
 
diff --git a/drivers/media/dvb-frontends/dib7000p.c b/drivers/media/dvb-frontends/dib7000p.c
index 661760d60232..589134e95175 100644
--- a/drivers/media/dvb-frontends/dib7000p.c
+++ b/drivers/media/dvb-frontends/dib7000p.c
@@ -2559,7 +2559,7 @@ static void dib7090_setHostBusMux(struct dib7000p_state *state, int mode)
 	dib7000p_write_word(state, 1288, reg_1288);
 }
 
-int dib7090_set_diversity_in(struct dvb_frontend *fe, int onoff)
+static int dib7090_set_diversity_in(struct dvb_frontend *fe, int onoff)
 {
 	struct dib7000p_state *state = fe->demodulator_priv;
 	u16 reg_1287;
diff --git a/drivers/media/dvb-frontends/drx39xyj/drxj.c b/drivers/media/dvb-frontends/drx39xyj/drxj.c
index 7ca7a21df183..5ec221ffdfca 100644
--- a/drivers/media/dvb-frontends/drx39xyj/drxj.c
+++ b/drivers/media/dvb-frontends/drx39xyj/drxj.c
@@ -2174,7 +2174,7 @@ int drxj_dap_atomic_read_reg32(struct i2c_device_addr *dev_addr,
 				     u32 addr,
 				     u32 *data, u32 flags)
 {
-	u8 buf[sizeof(*data)];
+	u8 buf[sizeof(*data)] = { 0 };
 	int rc = -EIO;
 	u32 word = 0;
 
@@ -4193,7 +4193,7 @@ int drxj_dap_scu_atomic_read_reg16(struct i2c_device_addr *dev_addr,
 					 u32 addr,
 					 u16 *data, u32 flags)
 {
-	u8 buf[2];
+	u8 buf[2] = { 0 };
 	int rc = -EIO;
 	u16 word = 0;
 
@@ -10667,7 +10667,7 @@ ctrl_sig_quality(struct drx_demod_instance *demod,
 	enum drx_standard standard = ext_attr->standard;
 	int rc;
 	u32 ber, cnt, err, pkt;
-	u16 mer, strength;
+	u16 mer, strength = 0;
 
 	rc = get_sig_strength(demod, &strength);
 	if (rc < 0) {
@@ -11602,7 +11602,7 @@ static u16 drx_u_code_compute_crc(u8 *block_data, u16 nr_words)
 	u32 carry = 0;
 
 	while (i < nr_words) {
-		crc_word |= (u32)be16_to_cpu(*(u32 *)(block_data));
+		crc_word |= (u32)be16_to_cpu(*(__be16 *)(block_data));
 		for (j = 0; j < 16; j++) {
 			crc_word <<= 1;
 			if (carry != 0)
@@ -11629,7 +11629,7 @@ static int drx_check_firmware(struct drx_demod_instance *demod, u8 *mc_data,
 	int i;
 	unsigned count = 2 * sizeof(u16);
 	u32 mc_dev_type, mc_version, mc_base_version;
-	u16 mc_nr_of_blks = be16_to_cpu(*(u32 *)(mc_data + sizeof(u16)));
+	u16 mc_nr_of_blks = be16_to_cpu(*(__be16 *)(mc_data + sizeof(u16)));
 
 	/*
 	 * Scan microcode blocks first for version info
@@ -11647,13 +11647,13 @@ static int drx_check_firmware(struct drx_demod_instance *demod, u8 *mc_data,
 			goto eof;
 
 		/* Process block header */
-		block_hdr.addr = be32_to_cpu(*(u32 *)(mc_data + count));
+		block_hdr.addr = be32_to_cpu(*(__be32 *)(mc_data + count));
 		count += sizeof(u32);
-		block_hdr.size = be16_to_cpu(*(u32 *)(mc_data + count));
+		block_hdr.size = be16_to_cpu(*(__be16 *)(mc_data + count));
 		count += sizeof(u16);
-		block_hdr.flags = be16_to_cpu(*(u32 *)(mc_data + count));
+		block_hdr.flags = be16_to_cpu(*(__be16 *)(mc_data + count));
 		count += sizeof(u16);
-		block_hdr.CRC = be16_to_cpu(*(u32 *)(mc_data + count));
+		block_hdr.CRC = be16_to_cpu(*(__be16 *)(mc_data + count));
 		count += sizeof(u16);
 
 		pr_debug("%u: addr %u, size %u, flags 0x%04x, CRC 0x%04x\n",
@@ -11667,7 +11667,7 @@ static int drx_check_firmware(struct drx_demod_instance *demod, u8 *mc_data,
 			if (block_hdr.addr + sizeof(u16) > size)
 				goto eof;
 
-			auxtype = be16_to_cpu(*(u32 *)(auxblk));
+			auxtype = be16_to_cpu(*(__be16 *)(auxblk));
 
 			/* Aux block. Check type */
 			if (DRX_ISMCVERTYPE(auxtype)) {
@@ -11675,11 +11675,11 @@ static int drx_check_firmware(struct drx_demod_instance *demod, u8 *mc_data,
 					goto eof;
 
 				auxblk += sizeof(u16);
-				mc_dev_type = be32_to_cpu(*(u32 *)(auxblk));
+				mc_dev_type = be32_to_cpu(*(__be32 *)(auxblk));
 				auxblk += sizeof(u32);
-				mc_version = be32_to_cpu(*(u32 *)(auxblk));
+				mc_version = be32_to_cpu(*(__be32 *)(auxblk));
 				auxblk += sizeof(u32);
-				mc_base_version = be32_to_cpu(*(u32 *)(auxblk));
+				mc_base_version = be32_to_cpu(*(__be32 *)(auxblk));
 
 				DRX_ATTR_MCRECORD(demod).aux_type = auxtype;
 				DRX_ATTR_MCRECORD(demod).mc_dev_type = mc_dev_type;
@@ -11765,9 +11765,9 @@ static int drx_ctrl_u_code(struct drx_demod_instance *demod,
 
 	mc_data = (void *)mc_data_init;
 	/* Check data */
-	mc_magic_word = be16_to_cpu(*(u32 *)(mc_data));
+	mc_magic_word = be16_to_cpu(*(__be16 *)(mc_data));
 	mc_data += sizeof(u16);
-	mc_nr_of_blks = be16_to_cpu(*(u32 *)(mc_data));
+	mc_nr_of_blks = be16_to_cpu(*(__be16 *)(mc_data));
 	mc_data += sizeof(u16);
 
 	if ((mc_magic_word != DRX_UCODE_MAGIC_WORD) || (mc_nr_of_blks == 0)) {
@@ -11791,13 +11791,13 @@ static int drx_ctrl_u_code(struct drx_demod_instance *demod,
 		u16 mc_block_nr_bytes = 0;
 
 		/* Process block header */
-		block_hdr.addr = be32_to_cpu(*(u32 *)(mc_data));
+		block_hdr.addr = be32_to_cpu(*(__be32 *)(mc_data));
 		mc_data += sizeof(u32);
-		block_hdr.size = be16_to_cpu(*(u32 *)(mc_data));
+		block_hdr.size = be16_to_cpu(*(__be16 *)(mc_data));
 		mc_data += sizeof(u16);
-		block_hdr.flags = be16_to_cpu(*(u32 *)(mc_data));
+		block_hdr.flags = be16_to_cpu(*(__be16 *)(mc_data));
 		mc_data += sizeof(u16);
-		block_hdr.CRC = be16_to_cpu(*(u32 *)(mc_data));
+		block_hdr.CRC = be16_to_cpu(*(__be16 *)(mc_data));
 		mc_data += sizeof(u16);
 
 		pr_debug("%u: addr %u, size %u, flags 0x%04x, CRC 0x%04x\n",
diff --git a/drivers/media/dvb-frontends/drxd_hard.c b/drivers/media/dvb-frontends/drxd_hard.c
index ae2276db77bc..687e893d29fe 100644
--- a/drivers/media/dvb-frontends/drxd_hard.c
+++ b/drivers/media/dvb-frontends/drxd_hard.c
@@ -2628,10 +2628,11 @@ static int DRXD_init(struct drxd_state *state, const u8 *fw, u32 fw_size)
 			break;
 
 		/* Apply I2c address patch to B1 */
-		if (!state->type_A && state->m_HiI2cPatch != NULL)
+		if (!state->type_A && state->m_HiI2cPatch != NULL) {
 			status = WriteTable(state, state->m_HiI2cPatch);
 			if (status < 0)
 				break;
+		}
 
 		if (state->type_A) {
 			/* HI firmware patch for UIO readout,
@@ -2830,14 +2831,8 @@ static int drxd_read_status(struct dvb_frontend *fe, fe_status_t * status)
 static int drxd_init(struct dvb_frontend *fe)
 {
 	struct drxd_state *state = fe->demodulator_priv;
-	int err = 0;
 
-/*	if (request_firmware(&state->fw, "drxd.fw", state->dev)<0) */
 	return DRXD_init(state, NULL, 0);
-
-	err = DRXD_init(state, state->fw->data, state->fw->size);
-	release_firmware(state->fw);
-	return err;
 }
 
 static int drxd_config_i2c(struct dvb_frontend *fe, int onoff)
diff --git a/drivers/media/dvb-frontends/drxk_hard.c b/drivers/media/dvb-frontends/drxk_hard.c
index cce94a75b2e1..672195147d01 100644
--- a/drivers/media/dvb-frontends/drxk_hard.c
+++ b/drivers/media/dvb-frontends/drxk_hard.c
@@ -1028,7 +1028,7 @@ static int hi_command(struct drxk_state *state, u16 cmd, u16 *p_result)
 		    ((state->m_hi_cfg_ctrl) &
 		     SIO_HI_RA_RAM_PAR_5_CFG_SLEEP__M) ==
 		    SIO_HI_RA_RAM_PAR_5_CFG_SLEEP_ZZZ);
-	if (powerdown_cmd == false) {
+	if (!powerdown_cmd) {
 		/* Wait until command rdy */
 		u32 retry_count = 0;
 		u16 wait_cmd;
@@ -1129,7 +1129,7 @@ static int mpegts_configure_pins(struct drxk_state *state, bool mpeg_enable)
 	if (status < 0)
 		goto error;
 
-	if (mpeg_enable == false) {
+	if (!mpeg_enable) {
 		/*  Set MPEG TS pads to inputmode */
 		status = write16(state, SIO_PDR_MSTRT_CFG__A, 0x0000);
 		if (status < 0)
@@ -1190,7 +1190,7 @@ static int mpegts_configure_pins(struct drxk_state *state, bool mpeg_enable)
 		if (status < 0)
 			goto error;
 
-		if (state->m_enable_parallel == true) {
+		if (state->m_enable_parallel) {
 			/* parallel -> enable MD1 to MD7 */
 			status = write16(state, SIO_PDR_MD1_CFG__A,
 					 sio_pdr_mdx_cfg);
@@ -1392,7 +1392,7 @@ static int dvbt_enable_ofdm_token_ring(struct drxk_state *state, bool enable)
 
 	dprintk(1, "\n");
 
-	if (enable == false) {
+	if (!enable) {
 		desired_ctrl = SIO_OFDM_SH_OFDM_RING_ENABLE_OFF;
 		desired_status = SIO_OFDM_SH_OFDM_RING_STATUS_DOWN;
 	}
@@ -2012,7 +2012,7 @@ static int mpegts_dto_setup(struct drxk_state *state,
 		goto error;
 	fec_oc_reg_mode &= (~FEC_OC_MODE_PARITY__M);
 	fec_oc_reg_ipr_mode &= (~FEC_OC_IPR_MODE_MVAL_DIS_PAR__M);
-	if (state->m_insert_rs_byte == true) {
+	if (state->m_insert_rs_byte) {
 		/* enable parity symbol forward */
 		fec_oc_reg_mode |= FEC_OC_MODE_PARITY__M;
 		/* MVAL disable during parity bytes */
@@ -2023,7 +2023,7 @@ static int mpegts_dto_setup(struct drxk_state *state,
 
 	/* Check serial or parallel output */
 	fec_oc_reg_ipr_mode &= (~(FEC_OC_IPR_MODE_SERIAL__M));
-	if (state->m_enable_parallel == false) {
+	if (!state->m_enable_parallel) {
 		/* MPEG data output is serial -> set ipr_mode[0] */
 		fec_oc_reg_ipr_mode |= FEC_OC_IPR_MODE_SERIAL__M;
 	}
@@ -2136,19 +2136,19 @@ static int mpegts_configure_polarity(struct drxk_state *state)
 
 	/* Control selective inversion of output bits */
 	fec_oc_reg_ipr_invert &= (~(invert_data_mask));
-	if (state->m_invert_data == true)
+	if (state->m_invert_data)
 		fec_oc_reg_ipr_invert |= invert_data_mask;
 	fec_oc_reg_ipr_invert &= (~(FEC_OC_IPR_INVERT_MERR__M));
-	if (state->m_invert_err == true)
+	if (state->m_invert_err)
 		fec_oc_reg_ipr_invert |= FEC_OC_IPR_INVERT_MERR__M;
 	fec_oc_reg_ipr_invert &= (~(FEC_OC_IPR_INVERT_MSTRT__M));
-	if (state->m_invert_str == true)
+	if (state->m_invert_str)
 		fec_oc_reg_ipr_invert |= FEC_OC_IPR_INVERT_MSTRT__M;
 	fec_oc_reg_ipr_invert &= (~(FEC_OC_IPR_INVERT_MVAL__M));
-	if (state->m_invert_val == true)
+	if (state->m_invert_val)
 		fec_oc_reg_ipr_invert |= FEC_OC_IPR_INVERT_MVAL__M;
 	fec_oc_reg_ipr_invert &= (~(FEC_OC_IPR_INVERT_MCLK__M));
-	if (state->m_invert_clk == true)
+	if (state->m_invert_clk)
 		fec_oc_reg_ipr_invert |= FEC_OC_IPR_INVERT_MCLK__M;
 
 	return write16(state, FEC_OC_IPR_INVERT__A, fec_oc_reg_ipr_invert);
@@ -2220,12 +2220,13 @@ static int set_agc_rf(struct drxk_state *state,
 		}
 
 		/* Set TOP, only if IF-AGC is in AUTO mode */
-		if (p_if_agc_settings->ctrl_mode == DRXK_AGC_CTRL_AUTO)
+		if (p_if_agc_settings->ctrl_mode == DRXK_AGC_CTRL_AUTO) {
 			status = write16(state,
 					 SCU_RAM_AGC_IF_IACCU_HI_TGT_MAX__A,
 					 p_agc_cfg->top);
 			if (status < 0)
 				goto error;
+		}
 
 		/* Cut-Off current */
 		status = write16(state, SCU_RAM_AGC_RF_IACCU_HI_CO__A,
@@ -3352,7 +3353,7 @@ static int dvbt_ctrl_set_inc_enable(struct drxk_state *state, bool *enabled)
 	int status;
 
 	dprintk(1, "\n");
-	if (*enabled == true)
+	if (*enabled)
 		status = write16(state, IQM_CF_BYPASSDET__A, 0);
 	else
 		status = write16(state, IQM_CF_BYPASSDET__A, 1);
@@ -3368,7 +3369,7 @@ static int dvbt_ctrl_set_fr_enable(struct drxk_state *state, bool *enabled)
 	int status;
 
 	dprintk(1, "\n");
-	if (*enabled == true) {
+	if (*enabled) {
 		/* write mask to 1 */
 		status = write16(state, OFDM_SC_RA_RAM_FR_THRES_8K__A,
 				   DEFAULT_FR_THRES_8K);
@@ -6794,11 +6795,11 @@ struct dvb_frontend *drxk_attach(const struct drxk_config *config,
 	state->enable_merr_cfg = config->enable_merr_cfg;
 
 	if (config->dynamic_clk) {
-		state->m_dvbt_static_clk = 0;
-		state->m_dvbc_static_clk = 0;
+		state->m_dvbt_static_clk = false;
+		state->m_dvbc_static_clk = false;
 	} else {
-		state->m_dvbt_static_clk = 1;
-		state->m_dvbc_static_clk = 1;
+		state->m_dvbt_static_clk = true;
+		state->m_dvbc_static_clk = true;
 	}
 
 
diff --git a/drivers/media/dvb-frontends/m88ds3103.c b/drivers/media/dvb-frontends/m88ds3103.c
index dfe0c2f7f1ef..81657e94c5a4 100644
--- a/drivers/media/dvb-frontends/m88ds3103.c
+++ b/drivers/media/dvb-frontends/m88ds3103.c
@@ -159,6 +159,7 @@ static int m88ds3103_wr_reg_val_tab(struct m88ds3103_priv *priv,
 {
 	int ret, i, j;
 	u8 buf[83];
+
 	dev_dbg(&priv->i2c->dev, "%s: tab_len=%d\n", __func__, tab_len);
 
 	if (tab_len > 83) {
@@ -247,8 +248,9 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 	u8 u8tmp, u8tmp1, u8tmp2;
 	u8 buf[2];
 	u16 u16tmp, divide_ratio;
-	u32 tuner_frequency, target_mclk, ts_clk;
+	u32 tuner_frequency, target_mclk;
 	s32 s32tmp;
+
 	dev_dbg(&priv->i2c->dev,
 			"%s: delivery_system=%d modulation=%d frequency=%d symbol_rate=%d inversion=%d pilot=%d rolloff=%d\n",
 			__func__, c->delivery_system,
@@ -316,9 +318,6 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 				target_mclk = 144000;
 			break;
 		case M88DS3103_TS_PARALLEL:
-		case M88DS3103_TS_PARALLEL_12:
-		case M88DS3103_TS_PARALLEL_16:
-		case M88DS3103_TS_PARALLEL_19_2:
 		case M88DS3103_TS_CI:
 			if (c->symbol_rate < 18000000)
 				target_mclk = 96000;
@@ -352,33 +351,17 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 	switch (priv->cfg->ts_mode) {
 	case M88DS3103_TS_SERIAL:
 		u8tmp1 = 0x00;
-		ts_clk = 0;
-		u8tmp = 0x46;
+		u8tmp = 0x06;
 		break;
 	case M88DS3103_TS_SERIAL_D7:
 		u8tmp1 = 0x20;
-		ts_clk = 0;
-		u8tmp = 0x46;
+		u8tmp = 0x06;
 		break;
 	case M88DS3103_TS_PARALLEL:
-		ts_clk = 24000;
-		u8tmp = 0x42;
-		break;
-	case M88DS3103_TS_PARALLEL_12:
-		ts_clk = 12000;
-		u8tmp = 0x42;
-		break;
-	case M88DS3103_TS_PARALLEL_16:
-		ts_clk = 16000;
-		u8tmp = 0x42;
-		break;
-	case M88DS3103_TS_PARALLEL_19_2:
-		ts_clk = 19200;
-		u8tmp = 0x42;
+		u8tmp = 0x02;
 		break;
 	case M88DS3103_TS_CI:
-		ts_clk = 6000;
-		u8tmp = 0x43;
+		u8tmp = 0x03;
 		break;
 	default:
 		dev_dbg(&priv->i2c->dev, "%s: invalid ts_mode\n", __func__);
@@ -386,6 +369,9 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 		goto err;
 	}
 
+	if (priv->cfg->ts_clk_pol)
+		u8tmp |= 0x40;
+
 	/* TS mode */
 	ret = m88ds3103_wr_reg(priv, 0xfd, u8tmp);
 	if (ret)
@@ -399,8 +385,8 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 			goto err;
 	}
 
-	if (ts_clk) {
-		divide_ratio = DIV_ROUND_UP(target_mclk, ts_clk);
+	if (priv->cfg->ts_clk) {
+		divide_ratio = DIV_ROUND_UP(target_mclk, priv->cfg->ts_clk);
 		u8tmp1 = divide_ratio / 2;
 		u8tmp2 = DIV_ROUND_UP(divide_ratio, 2);
 	} else {
@@ -411,7 +397,7 @@ static int m88ds3103_set_frontend(struct dvb_frontend *fe)
 
 	dev_dbg(&priv->i2c->dev,
 			"%s: target_mclk=%d ts_clk=%d divide_ratio=%d\n",
-			__func__, target_mclk, ts_clk, divide_ratio);
+			__func__, target_mclk, priv->cfg->ts_clk, divide_ratio);
 
 	u8tmp1--;
 	u8tmp2--;
@@ -536,6 +522,7 @@ static int m88ds3103_init(struct dvb_frontend *fe)
 	const struct firmware *fw = NULL;
 	u8 *fw_file = M88DS3103_FIRMWARE;
 	u8 u8tmp;
+
 	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 
 	/* set cold state by default */
@@ -648,6 +635,7 @@ static int m88ds3103_sleep(struct dvb_frontend *fe)
 {
 	struct m88ds3103_priv *priv = fe->demodulator_priv;
 	int ret;
+
 	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 
 	priv->delivery_system = SYS_UNDEFINED;
@@ -682,6 +670,7 @@ static int m88ds3103_get_frontend(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 	u8 buf[3];
+
 	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 
 	if (!priv->warm || !(priv->fe_status & FE_HAS_LOCK)) {
@@ -857,6 +846,7 @@ static int m88ds3103_read_snr(struct dvb_frontend *fe, u16 *snr)
 	u8 buf[3];
 	u16 noise, signal;
 	u32 noise_tot, signal_tot;
+
 	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 	/* reports SNR in resolution of 0.1 dB */
 
@@ -933,6 +923,7 @@ static int m88ds3103_read_ber(struct dvb_frontend *fe, u32 *ber)
 	int ret;
 	unsigned int utmp;
 	u8 buf[3], u8tmp;
+
 	dev_dbg(&priv->i2c->dev, "%s:\n", __func__);
 
 	switch (c->delivery_system) {
@@ -1013,6 +1004,7 @@ static int m88ds3103_set_tone(struct dvb_frontend *fe,
 	struct m88ds3103_priv *priv = fe->demodulator_priv;
 	int ret;
 	u8 u8tmp, tone, reg_a1_mask;
+
 	dev_dbg(&priv->i2c->dev, "%s: fe_sec_tone_mode=%d\n", __func__,
 			fe_sec_tone_mode);
 
@@ -1053,12 +1045,64 @@ err:
 	return ret;
 }
 
+static int m88ds3103_set_voltage(struct dvb_frontend *fe,
+	fe_sec_voltage_t fe_sec_voltage)
+{
+	struct m88ds3103_priv *priv = fe->demodulator_priv;
+	int ret;
+	u8 u8tmp;
+	bool voltage_sel, voltage_dis;
+
+	dev_dbg(&priv->i2c->dev, "%s: fe_sec_voltage=%d\n", __func__,
+			fe_sec_voltage);
+
+	if (!priv->warm) {
+		ret = -EAGAIN;
+		goto err;
+	}
+
+	switch (fe_sec_voltage) {
+	case SEC_VOLTAGE_18:
+		voltage_sel = true;
+		voltage_dis = false;
+		break;
+	case SEC_VOLTAGE_13:
+		voltage_sel = false;
+		voltage_dis = false;
+		break;
+	case SEC_VOLTAGE_OFF:
+		voltage_sel = false;
+		voltage_dis = true;
+		break;
+	default:
+		dev_dbg(&priv->i2c->dev, "%s: invalid fe_sec_voltage\n",
+				__func__);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* output pin polarity */
+	voltage_sel ^= priv->cfg->lnb_hv_pol;
+	voltage_dis ^= priv->cfg->lnb_en_pol;
+
+	u8tmp = voltage_dis << 1 | voltage_sel << 0;
+	ret = m88ds3103_wr_reg_mask(priv, 0xa2, u8tmp, 0x03);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
 static int m88ds3103_diseqc_send_master_cmd(struct dvb_frontend *fe,
 		struct dvb_diseqc_master_cmd *diseqc_cmd)
 {
 	struct m88ds3103_priv *priv = fe->demodulator_priv;
 	int ret, i;
 	u8 u8tmp;
+
 	dev_dbg(&priv->i2c->dev, "%s: msg=%*ph\n", __func__,
 			diseqc_cmd->msg_len, diseqc_cmd->msg);
 
@@ -1130,6 +1174,7 @@ static int m88ds3103_diseqc_send_burst(struct dvb_frontend *fe,
 	struct m88ds3103_priv *priv = fe->demodulator_priv;
 	int ret, i;
 	u8 u8tmp, burst;
+
 	dev_dbg(&priv->i2c->dev, "%s: fe_sec_mini_cmd=%d\n", __func__,
 			fe_sec_mini_cmd);
 
@@ -1202,6 +1247,7 @@ static int m88ds3103_get_tune_settings(struct dvb_frontend *fe,
 static void m88ds3103_release(struct dvb_frontend *fe)
 {
 	struct m88ds3103_priv *priv = fe->demodulator_priv;
+
 	i2c_del_mux_adapter(priv->i2c_adapter);
 	kfree(priv);
 }
@@ -1370,6 +1416,7 @@ static struct dvb_frontend_ops m88ds3103_ops = {
 	.diseqc_send_burst = m88ds3103_diseqc_send_burst,
 
 	.set_tone = m88ds3103_set_tone,
+	.set_voltage = m88ds3103_set_voltage,
 };
 
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
diff --git a/drivers/media/dvb-frontends/m88ds3103.h b/drivers/media/dvb-frontends/m88ds3103.h
index bbb7e3aa5675..9b3b4962da7c 100644
--- a/drivers/media/dvb-frontends/m88ds3103.h
+++ b/drivers/media/dvb-frontends/m88ds3103.h
@@ -47,14 +47,23 @@ struct m88ds3103_config {
 	 */
 #define M88DS3103_TS_SERIAL             0 /* TS output pin D0, normal */
 #define M88DS3103_TS_SERIAL_D7          1 /* TS output pin D7 */
-#define M88DS3103_TS_PARALLEL           2 /* 24 MHz, normal */
-#define M88DS3103_TS_PARALLEL_12        3 /* 12 MHz */
-#define M88DS3103_TS_PARALLEL_16        4 /* 16 MHz */
-#define M88DS3103_TS_PARALLEL_19_2      5 /* 19.2 MHz */
-#define M88DS3103_TS_CI                 6 /* 6 MHz */
+#define M88DS3103_TS_PARALLEL           2 /* TS Parallel mode */
+#define M88DS3103_TS_CI                 3 /* TS CI Mode */
 	u8 ts_mode;
 
 	/*
+	 * TS clk in KHz
+	 * Default: 0.
+	 */
+	u32 ts_clk;
+
+	/*
+	 * TS clk polarity.
+	 * Default: 0. 1-active at falling edge; 0-active at rising edge.
+	 */
+	u8 ts_clk_pol:1;
+
+	/*
 	 * spectrum inversion
 	 * Default: 0
 	 */
@@ -86,6 +95,22 @@ struct m88ds3103_config {
 	 * Default: none, must set
 	 */
 	u8 agc;
+
+	/*
+	 * LNB H/V pin polarity
+	 * Default: 0.
+	 * 1: pin high set to VOLTAGE_13, pin low to set VOLTAGE_18.
+	 * 0: pin high set to VOLTAGE_18, pin low to set VOLTAGE_13.
+	 */
+	u8 lnb_hv_pol:1;
+
+	/*
+	 * LNB enable pin polarity
+	 * Default: 0.
+	 * 1: pin high to enable, pin low to disable.
+	 * 0: pin high to disable, pin low to enable.
+	 */
+	u8 lnb_en_pol:1;
 };
 
 /*
diff --git a/drivers/media/dvb-frontends/mb86a16.c b/drivers/media/dvb-frontends/mb86a16.c
index 9ae40abfd71a..3ddea4471d2b 100644
--- a/drivers/media/dvb-frontends/mb86a16.c
+++ b/drivers/media/dvb-frontends/mb86a16.c
@@ -28,7 +28,7 @@
 #include "mb86a16.h"
 #include "mb86a16_priv.h"
 
-unsigned int verbose = 5;
+static unsigned int verbose = 5;
 module_param(verbose, int, 0644);
 
 #define ABS(x)		((x) < 0 ? (-x) : (x))
@@ -115,9 +115,11 @@ static int mb86a16_read(struct mb86a16_state *state, u8 reg, u8 *val)
 	};
 	ret = i2c_transfer(state->i2c_adap, msg, 2);
 	if (ret != 2) {
-		dprintk(verbose, MB86A16_ERROR, 1, "read error(reg=0x%02x, ret=0x%i)",
+		dprintk(verbose, MB86A16_ERROR, 1, "read error(reg=0x%02x, ret=%i)",
 			reg, ret);
 
+		if (ret < 0)
+			return ret;
 		return -EREMOTEIO;
 	}
 	*val = b1[0];
diff --git a/drivers/media/dvb-frontends/mb86a20s.c b/drivers/media/dvb-frontends/mb86a20s.c
index b931179c70a4..e6f165a5b90d 100644
--- a/drivers/media/dvb-frontends/mb86a20s.c
+++ b/drivers/media/dvb-frontends/mb86a20s.c
@@ -33,7 +33,7 @@ enum mb86a20s_bandwidth {
 	MB86A20S_3SEG = 3,
 };
 
-u8 mb86a20s_subchannel[] = {
+static u8 mb86a20s_subchannel[] = {
 	0xb0, 0xc0, 0xd0, 0xe0,
 	0xf0, 0x00, 0x10, 0x20,
 };
@@ -1228,7 +1228,7 @@ struct linear_segments {
  * All tables below return a dB/1000 measurement
  */
 
-static struct linear_segments cnr_to_db_table[] = {
+static const struct linear_segments cnr_to_db_table[] = {
 	{ 19648,     0},
 	{ 18187,  1000},
 	{ 16534,  2000},
@@ -1262,7 +1262,7 @@ static struct linear_segments cnr_to_db_table[] = {
 	{   788, 30000},
 };
 
-static struct linear_segments cnr_64qam_table[] = {
+static const struct linear_segments cnr_64qam_table[] = {
 	{ 3922688,     0},
 	{ 3920384,  1000},
 	{ 3902720,  2000},
@@ -1296,7 +1296,7 @@ static struct linear_segments cnr_64qam_table[] = {
 	{  388864, 30000},
 };
 
-static struct linear_segments cnr_16qam_table[] = {
+static const struct linear_segments cnr_16qam_table[] = {
 	{ 5314816,     0},
 	{ 5219072,  1000},
 	{ 5118720,  2000},
@@ -1330,7 +1330,7 @@ static struct linear_segments cnr_16qam_table[] = {
 	{   95744, 30000},
 };
 
-struct linear_segments cnr_qpsk_table[] = {
+static const struct linear_segments cnr_qpsk_table[] = {
 	{ 2834176,     0},
 	{ 2683648,  1000},
 	{ 2536960,  2000},
@@ -1364,7 +1364,7 @@ struct linear_segments cnr_qpsk_table[] = {
 	{   11520, 30000},
 };
 
-static u32 interpolate_value(u32 value, struct linear_segments *segments,
+static u32 interpolate_value(u32 value, const struct linear_segments *segments,
 			     unsigned len)
 {
 	u64 tmp64;
@@ -1448,7 +1448,7 @@ static int mb86a20s_get_blk_error_layer_CNR(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	u32 mer, cnr;
 	int rc, val, layer;
-	struct linear_segments *segs;
+	const struct linear_segments *segs;
 	unsigned segs_len;
 
 	dev_dbg(&state->i2c->dev, "%s called.\n", __func__);
diff --git a/drivers/media/dvb-frontends/mt312.c b/drivers/media/dvb-frontends/mt312.c
index a74ac0ddb833..2163490c1e6b 100644
--- a/drivers/media/dvb-frontends/mt312.c
+++ b/drivers/media/dvb-frontends/mt312.c
@@ -103,7 +103,7 @@ static int mt312_write(struct mt312_state *state, const enum mt312_reg_addr reg,
 
 	if (1 + count > sizeof(buf)) {
 		printk(KERN_WARNING
-		       "mt312: write: len=%zd is too big!\n", count);
+		       "mt312: write: len=%zu is too big!\n", count);
 		return -EINVAL;
 	}
 
diff --git a/drivers/media/dvb-frontends/or51211.c b/drivers/media/dvb-frontends/or51211.c
index 10cfc0579168..873ea1da844b 100644
--- a/drivers/media/dvb-frontends/or51211.c
+++ b/drivers/media/dvb-frontends/or51211.c
@@ -111,7 +111,7 @@ static int or51211_load_firmware (struct dvb_frontend* fe,
 	u8 tudata[585];
 	int i;
 
-	dprintk("Firmware is %zd bytes\n",fw->size);
+	dprintk("Firmware is %zu bytes\n", fw->size);
 
 	/* Get eprom data */
 	tudata[0] = 17;
diff --git a/drivers/media/dvb-frontends/rtl2832.c b/drivers/media/dvb-frontends/rtl2832.c
index fdbed35c87fa..eb737cf29a36 100644
--- a/drivers/media/dvb-frontends/rtl2832.c
+++ b/drivers/media/dvb-frontends/rtl2832.c
@@ -936,7 +936,7 @@ static void rtl2832_i2c_gate_work(struct work_struct *work)
 	if (ret != 1)
 		goto err;
 
-	priv->i2c_gate_state = 0;
+	priv->i2c_gate_state = false;
 
 	return;
 err:
diff --git a/drivers/media/dvb-frontends/rtl2832_sdr.c b/drivers/media/dvb-frontends/rtl2832_sdr.c
index 023e0f49c786..7bf98cf6bbe1 100644
--- a/drivers/media/dvb-frontends/rtl2832_sdr.c
+++ b/drivers/media/dvb-frontends/rtl2832_sdr.c
@@ -329,7 +329,7 @@ static int rtl2832_sdr_rd_reg_mask(struct rtl2832_sdr_state *s, u16 reg,
 static struct rtl2832_sdr_frame_buf *rtl2832_sdr_get_next_fill_buf(
 		struct rtl2832_sdr_state *s)
 {
-	unsigned long flags = 0;
+	unsigned long flags;
 	struct rtl2832_sdr_frame_buf *buf = NULL;
 
 	spin_lock_irqsave(&s->queued_bufs_lock, flags);
@@ -365,17 +365,19 @@ static unsigned int rtl2832_sdr_convert_stream(struct rtl2832_sdr_state *s,
 		dst_len = 0;
 	}
 
-	/* calculate samping rate and output it in 10 seconds intervals */
+	/* calculate sample rate and output it in 10 seconds intervals */
 	if (unlikely(time_is_before_jiffies(s->jiffies_next))) {
-#define MSECS 10000UL
+		#define MSECS 10000UL
+		unsigned int msecs = jiffies_to_msecs(jiffies -
+				s->jiffies_next + msecs_to_jiffies(MSECS));
 		unsigned int samples = s->sample - s->sample_measured;
 
 		s->jiffies_next = jiffies + msecs_to_jiffies(MSECS);
 		s->sample_measured = s->sample;
 		dev_dbg(&s->udev->dev,
-				"slen=%d samples=%u msecs=%lu sampling rate=%lu\n",
-				src_len, samples, MSECS,
-				samples * 1000UL / MSECS);
+				"slen=%u samples=%u msecs=%u sample rate=%lu\n",
+				src_len, samples, msecs,
+				samples * 1000UL / msecs);
 	}
 
 	/* total number of I+Q pairs */
@@ -394,8 +396,8 @@ static void rtl2832_sdr_urb_complete(struct urb *urb)
 	struct rtl2832_sdr_frame_buf *fbuf;
 
 	dev_dbg_ratelimited(&s->udev->dev,
-			"%s: status=%d length=%d/%d errors=%d\n",
-			__func__, urb->status, urb->actual_length,
+			"status=%d length=%d/%d errors=%d\n",
+			urb->status, urb->actual_length,
 			urb->transfer_buffer_length, urb->error_count);
 
 	switch (urb->status) {
@@ -443,7 +445,7 @@ static int rtl2832_sdr_kill_urbs(struct rtl2832_sdr_state *s)
 	int i;
 
 	for (i = s->urbs_submitted - 1; i >= 0; i--) {
-		dev_dbg(&s->udev->dev, "%s: kill urb=%d\n", __func__, i);
+		dev_dbg(&s->udev->dev, "kill urb=%d\n", i);
 		/* stop the URB */
 		usb_kill_urb(s->urb_list[i]);
 	}
@@ -457,7 +459,7 @@ static int rtl2832_sdr_submit_urbs(struct rtl2832_sdr_state *s)
 	int i, ret;
 
 	for (i = 0; i < s->urbs_initialized; i++) {
-		dev_dbg(&s->udev->dev, "%s: submit urb=%d\n", __func__, i);
+		dev_dbg(&s->udev->dev, "submit urb=%d\n", i);
 		ret = usb_submit_urb(s->urb_list[i], GFP_ATOMIC);
 		if (ret) {
 			dev_err(&s->udev->dev,
@@ -477,8 +479,7 @@ static int rtl2832_sdr_free_stream_bufs(struct rtl2832_sdr_state *s)
 	if (s->flags & USB_STATE_URB_BUF) {
 		while (s->buf_num) {
 			s->buf_num--;
-			dev_dbg(&s->udev->dev, "%s: free buf=%d\n",
-					__func__, s->buf_num);
+			dev_dbg(&s->udev->dev, "free buf=%d\n", s->buf_num);
 			usb_free_coherent(s->udev, s->buf_size,
 					  s->buf_list[s->buf_num],
 					  s->dma_addr[s->buf_num]);
@@ -494,24 +495,22 @@ static int rtl2832_sdr_alloc_stream_bufs(struct rtl2832_sdr_state *s)
 	s->buf_num = 0;
 	s->buf_size = BULK_BUFFER_SIZE;
 
-	dev_dbg(&s->udev->dev,
-			"%s: all in all I will use %u bytes for streaming\n",
-			__func__,  MAX_BULK_BUFS * BULK_BUFFER_SIZE);
+	dev_dbg(&s->udev->dev, "all in all I will use %u bytes for streaming\n",
+			MAX_BULK_BUFS * BULK_BUFFER_SIZE);
 
 	for (s->buf_num = 0; s->buf_num < MAX_BULK_BUFS; s->buf_num++) {
 		s->buf_list[s->buf_num] = usb_alloc_coherent(s->udev,
 				BULK_BUFFER_SIZE, GFP_ATOMIC,
 				&s->dma_addr[s->buf_num]);
 		if (!s->buf_list[s->buf_num]) {
-			dev_dbg(&s->udev->dev, "%s: alloc buf=%d failed\n",
-					__func__, s->buf_num);
+			dev_dbg(&s->udev->dev, "alloc buf=%d failed\n",
+					s->buf_num);
 			rtl2832_sdr_free_stream_bufs(s);
 			return -ENOMEM;
 		}
 
-		dev_dbg(&s->udev->dev, "%s: alloc buf=%d %p (dma %llu)\n",
-				__func__, s->buf_num,
-				s->buf_list[s->buf_num],
+		dev_dbg(&s->udev->dev, "alloc buf=%d %p (dma %llu)\n",
+				s->buf_num, s->buf_list[s->buf_num],
 				(long long)s->dma_addr[s->buf_num]);
 		s->flags |= USB_STATE_URB_BUF;
 	}
@@ -527,8 +526,7 @@ static int rtl2832_sdr_free_urbs(struct rtl2832_sdr_state *s)
 
 	for (i = s->urbs_initialized - 1; i >= 0; i--) {
 		if (s->urb_list[i]) {
-			dev_dbg(&s->udev->dev, "%s: free urb=%d\n",
-					__func__, i);
+			dev_dbg(&s->udev->dev, "free urb=%d\n", i);
 			/* free the URBs */
 			usb_free_urb(s->urb_list[i]);
 		}
@@ -544,10 +542,10 @@ static int rtl2832_sdr_alloc_urbs(struct rtl2832_sdr_state *s)
 
 	/* allocate the URBs */
 	for (i = 0; i < MAX_BULK_BUFS; i++) {
-		dev_dbg(&s->udev->dev, "%s: alloc urb=%d\n", __func__, i);
+		dev_dbg(&s->udev->dev, "alloc urb=%d\n", i);
 		s->urb_list[i] = usb_alloc_urb(0, GFP_ATOMIC);
 		if (!s->urb_list[i]) {
-			dev_dbg(&s->udev->dev, "%s: failed\n", __func__);
+			dev_dbg(&s->udev->dev, "failed\n");
 			for (j = 0; j < i; j++)
 				usb_free_urb(s->urb_list[j]);
 			return -ENOMEM;
@@ -570,9 +568,9 @@ static int rtl2832_sdr_alloc_urbs(struct rtl2832_sdr_state *s)
 /* Must be called with vb_queue_lock hold */
 static void rtl2832_sdr_cleanup_queued_bufs(struct rtl2832_sdr_state *s)
 {
-	unsigned long flags = 0;
+	unsigned long flags;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	spin_lock_irqsave(&s->queued_bufs_lock, flags);
 	while (!list_empty(&s->queued_bufs)) {
@@ -591,7 +589,7 @@ static void rtl2832_sdr_release_sec(struct dvb_frontend *fe)
 {
 	struct rtl2832_sdr_state *s = fe->sec_priv;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	mutex_lock(&s->vb_queue_lock);
 	mutex_lock(&s->v4l2_lock);
@@ -613,7 +611,7 @@ static int rtl2832_sdr_querycap(struct file *file, void *fh,
 {
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
 	strlcpy(cap->card, s->vdev.name, sizeof(cap->card));
@@ -631,15 +629,15 @@ static int rtl2832_sdr_queue_setup(struct vb2_queue *vq,
 {
 	struct rtl2832_sdr_state *s = vb2_get_drv_priv(vq);
 
-	dev_dbg(&s->udev->dev, "%s: *nbuffers=%d\n", __func__, *nbuffers);
+	dev_dbg(&s->udev->dev, "nbuffers=%d\n", *nbuffers);
 
 	/* Need at least 8 buffers */
 	if (vq->num_buffers + *nbuffers < 8)
 		*nbuffers = 8 - vq->num_buffers;
 	*nplanes = 1;
 	sizes[0] = PAGE_ALIGN(s->buffersize);
-	dev_dbg(&s->udev->dev, "%s: nbuffers=%d sizes[0]=%d\n",
-			__func__, *nbuffers, sizes[0]);
+	dev_dbg(&s->udev->dev, "nbuffers=%d sizes[0]=%d\n",
+			*nbuffers, sizes[0]);
 	return 0;
 }
 
@@ -659,7 +657,7 @@ static void rtl2832_sdr_buf_queue(struct vb2_buffer *vb)
 	struct rtl2832_sdr_state *s = vb2_get_drv_priv(vb->vb2_queue);
 	struct rtl2832_sdr_frame_buf *buf =
 			container_of(vb, struct rtl2832_sdr_frame_buf, vb);
-	unsigned long flags = 0;
+	unsigned long flags;
 
 	/* Check the device has not disconnected between prep and queuing */
 	if (!s->udev) {
@@ -681,7 +679,7 @@ static int rtl2832_sdr_set_adc(struct rtl2832_sdr_state *s)
 	u64 u64tmp;
 	u32 u32tmp;
 
-	dev_dbg(&s->udev->dev, "%s: f_adc=%u\n", __func__, s->f_adc);
+	dev_dbg(&s->udev->dev, "f_adc=%u\n", s->f_adc);
 
 	if (!test_bit(POWER_ON, &s->flags))
 		return 0;
@@ -715,8 +713,7 @@ static int rtl2832_sdr_set_adc(struct rtl2832_sdr_state *s)
 	u64tmp = -u64tmp;
 	u32tmp = u64tmp & 0x3fffff;
 
-	dev_dbg(&s->udev->dev, "%s: f_if=%u if_ctl=%08x\n",
-			__func__, f_if, u32tmp);
+	dev_dbg(&s->udev->dev, "f_if=%u if_ctl=%08x\n", f_if, u32tmp);
 
 	buf[0] = (u32tmp >> 16) & 0xff;
 	buf[1] = (u32tmp >>  8) & 0xff;
@@ -903,7 +900,7 @@ static void rtl2832_sdr_unset_adc(struct rtl2832_sdr_state *s)
 {
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	/* PID filter */
 	ret = rtl2832_sdr_wr_regs(s, 0x061, "\xe0", 1);
@@ -964,8 +961,8 @@ static int rtl2832_sdr_set_tuner_freq(struct rtl2832_sdr_state *s)
 	c->frequency = s->f_tuner;
 	c->delivery_system = SYS_DVBT;
 
-	dev_dbg(&s->udev->dev, "%s: frequency=%u bandwidth=%d\n",
-			__func__, c->frequency, c->bandwidth_hz);
+	dev_dbg(&s->udev->dev, "frequency=%u bandwidth=%d\n",
+			c->frequency, c->bandwidth_hz);
 
 	if (!test_bit(POWER_ON, &s->flags))
 		return 0;
@@ -980,7 +977,7 @@ static int rtl2832_sdr_set_tuner(struct rtl2832_sdr_state *s)
 {
 	struct dvb_frontend *fe = s->fe;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	if (fe->ops.tuner_ops.init)
 		fe->ops.tuner_ops.init(fe);
@@ -992,7 +989,7 @@ static void rtl2832_sdr_unset_tuner(struct rtl2832_sdr_state *s)
 {
 	struct dvb_frontend *fe = s->fe;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	if (fe->ops.tuner_ops.sleep)
 		fe->ops.tuner_ops.sleep(fe);
@@ -1005,7 +1002,7 @@ static int rtl2832_sdr_start_streaming(struct vb2_queue *vq, unsigned int count)
 	struct rtl2832_sdr_state *s = vb2_get_drv_priv(vq);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	if (!s->udev)
 		return -ENODEV;
@@ -1054,7 +1051,7 @@ static void rtl2832_sdr_stop_streaming(struct vb2_queue *vq)
 {
 	struct rtl2832_sdr_state *s = vb2_get_drv_priv(vq);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	mutex_lock(&s->v4l2_lock);
 
@@ -1088,8 +1085,7 @@ static int rtl2832_sdr_g_tuner(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s: index=%d type=%d\n",
-			__func__, v->index, v->type);
+	dev_dbg(&s->udev->dev, "index=%d type=%d\n", v->index, v->type);
 
 	if (v->index == 0) {
 		strlcpy(v->name, "ADC: Realtek RTL2832", sizeof(v->name));
@@ -1115,7 +1111,7 @@ static int rtl2832_sdr_s_tuner(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	if (v->index > 1)
 		return -EINVAL;
@@ -1127,8 +1123,8 @@ static int rtl2832_sdr_enum_freq_bands(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d index=%d\n",
-			__func__, band->tuner, band->type, band->index);
+	dev_dbg(&s->udev->dev, "tuner=%d type=%d index=%d\n",
+			band->tuner, band->type, band->index);
 
 	if (band->tuner == 0) {
 		if (band->index >= ARRAY_SIZE(bands_adc))
@@ -1153,8 +1149,8 @@ static int rtl2832_sdr_g_frequency(struct file *file, void *priv,
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 	int ret  = 0;
 
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d\n",
-			__func__, f->tuner, f->type);
+	dev_dbg(&s->udev->dev, "tuner=%d type=%d\n",
+			f->tuner, f->type);
 
 	if (f->tuner == 0) {
 		f->frequency = s->f_adc;
@@ -1175,8 +1171,8 @@ static int rtl2832_sdr_s_frequency(struct file *file, void *priv,
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 	int ret, band;
 
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d frequency=%u\n",
-			__func__, f->tuner, f->type, f->frequency);
+	dev_dbg(&s->udev->dev, "tuner=%d type=%d frequency=%u\n",
+			f->tuner, f->type, f->frequency);
 
 	/* ADC band midpoints */
 	#define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2)
@@ -1194,15 +1190,13 @@ static int rtl2832_sdr_s_frequency(struct file *file, void *priv,
 				bands_adc[band].rangelow,
 				bands_adc[band].rangehigh);
 
-		dev_dbg(&s->udev->dev, "%s: ADC frequency=%u Hz\n",
-				__func__, s->f_adc);
+		dev_dbg(&s->udev->dev, "ADC frequency=%u Hz\n", s->f_adc);
 		ret = rtl2832_sdr_set_adc(s);
 	} else if (f->tuner == 1) {
 		s->f_tuner = clamp_t(unsigned int, f->frequency,
 				bands_fm[0].rangelow,
 				bands_fm[0].rangehigh);
-		dev_dbg(&s->udev->dev, "%s: RF frequency=%u Hz\n",
-				__func__, f->frequency);
+		dev_dbg(&s->udev->dev, "RF frequency=%u Hz\n", f->frequency);
 
 		ret = rtl2832_sdr_set_tuner_freq(s);
 	} else {
@@ -1217,7 +1211,7 @@ static int rtl2832_sdr_enum_fmt_sdr_cap(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	if (f->index >= s->num_formats)
 		return -EINVAL;
@@ -1233,7 +1227,7 @@ static int rtl2832_sdr_g_fmt_sdr_cap(struct file *file, void *priv,
 {
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(&s->udev->dev, "\n");
 
 	f->fmt.sdr.pixelformat = s->pixelformat;
 	f->fmt.sdr.buffersize = s->buffersize;
@@ -1250,7 +1244,7 @@ static int rtl2832_sdr_s_fmt_sdr_cap(struct file *file, void *priv,
 	struct vb2_queue *q = &s->vb_queue;
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
+	dev_dbg(&s->udev->dev, "pixelformat fourcc %4.4s\n",
 			(char *)&f->fmt.sdr.pixelformat);
 
 	if (vb2_is_busy(q))
@@ -1280,7 +1274,7 @@ static int rtl2832_sdr_try_fmt_sdr_cap(struct file *file, void *priv,
 	struct rtl2832_sdr_state *s = video_drvdata(file);
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
+	dev_dbg(&s->udev->dev, "pixelformat fourcc %4.4s\n",
 			(char *)&f->fmt.sdr.pixelformat);
 
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
@@ -1354,8 +1348,8 @@ static int rtl2832_sdr_s_ctrl(struct v4l2_ctrl *ctrl)
 	int ret;
 
 	dev_dbg(&s->udev->dev,
-			"%s: id=%d name=%s val=%d min=%lld max=%lld step=%lld\n",
-			__func__, ctrl->id, ctrl->name, ctrl->val,
+			"id=%d name=%s val=%d min=%lld max=%lld step=%lld\n",
+			ctrl->id, ctrl->name, ctrl->val,
 			ctrl->minimum, ctrl->maximum, ctrl->step);
 
 	switch (ctrl->id) {
@@ -1432,7 +1426,7 @@ struct dvb_frontend *rtl2832_sdr_attach(struct dvb_frontend *fe,
 	s->pixelformat = formats[0].pixelformat;
 	s->buffersize = formats[0].buffersize;
 	s->num_formats = NUM_FORMATS;
-	if (rtl2832_sdr_emulated_fmt == false)
+	if (!rtl2832_sdr_emulated_fmt)
 		s->num_formats -= 1;
 
 	mutex_init(&s->v4l2_lock);
diff --git a/drivers/media/dvb-frontends/si2165.c b/drivers/media/dvb-frontends/si2165.c
index 3a2d6c5aded6..98ddb49ad52b 100644
--- a/drivers/media/dvb-frontends/si2165.c
+++ b/drivers/media/dvb-frontends/si2165.c
@@ -1,5 +1,5 @@
 /*
-    Driver for Silicon Labs SI2165 DVB-C/-T Demodulator
+    Driver for Silicon Labs Si2161 DVB-T and Si2165 DVB-C/-T Demodulator
 
     Copyright (C) 2013-2014 Matthias Schwarzott <zzam@gentoo.org>
 
@@ -44,9 +44,7 @@ struct si2165_state {
 
 	struct si2165_config config;
 
-	/* chip revision */
-	u8 revcode;
-	/* chip type */
+	u8 chip_revcode;
 	u8 chip_type;
 
 	/* calculated by xtal and div settings */
@@ -312,7 +310,7 @@ static u32 si2165_get_fe_clk(struct si2165_state *state)
 	return state->adc_clk;
 }
 
-static bool si2165_wait_init_done(struct si2165_state *state)
+static int si2165_wait_init_done(struct si2165_state *state)
 {
 	int ret = -EINVAL;
 	u8 val = 0;
@@ -407,7 +405,7 @@ static int si2165_upload_firmware(struct si2165_state *state)
 	int ret;
 
 	const struct firmware *fw = NULL;
-	u8 *fw_file = SI2165_FIRMWARE;
+	u8 *fw_file;
 	const u8 *data;
 	u32 len;
 	u32 offset;
@@ -415,10 +413,20 @@ static int si2165_upload_firmware(struct si2165_state *state)
 	u8 block_count;
 	u16 crc_expected;
 
+	switch (state->chip_revcode) {
+	case 0x03: /* revision D */
+		fw_file = SI2165_FIRMWARE_REV_D;
+		break;
+	default:
+		dev_info(&state->i2c->dev, "%s: no firmware file for revision=%d\n",
+			KBUILD_MODNAME, state->chip_revcode);
+		return 0;
+	}
+
 	/* request the firmware, this will block and timeout */
 	ret = request_firmware(&fw, fw_file, state->i2c->dev.parent);
 	if (ret) {
-		dev_warn(&state->i2c->dev, "%s: firmare file '%s' not found\n",
+		dev_warn(&state->i2c->dev, "%s: firmware file '%s' not found\n",
 				KBUILD_MODNAME, fw_file);
 		goto error;
 	}
@@ -908,7 +916,7 @@ static void si2165_release(struct dvb_frontend *fe)
 
 static struct dvb_frontend_ops si2165_ops = {
 	.info = {
-		.name = "Silicon Labs Si2165",
+		.name = "Silicon Labs ",
 		.caps =	FE_CAN_FEC_1_2 |
 			FE_CAN_FEC_2_3 |
 			FE_CAN_FEC_3_4 |
@@ -948,6 +956,8 @@ struct dvb_frontend *si2165_attach(const struct si2165_config *config,
 	int n;
 	int io_ret;
 	u8 val;
+	char rev_char;
+	const char *chip_name;
 
 	if (config == NULL || i2c == NULL)
 		goto error;
@@ -984,7 +994,7 @@ struct dvb_frontend *si2165_attach(const struct si2165_config *config,
 	if (val != state->config.chip_mode)
 		goto error;
 
-	io_ret = si2165_readreg8(state, 0x0023 , &state->revcode);
+	io_ret = si2165_readreg8(state, 0x0023, &state->chip_revcode);
 	if (io_ret < 0)
 		goto error;
 
@@ -997,22 +1007,35 @@ struct dvb_frontend *si2165_attach(const struct si2165_config *config,
 	if (io_ret < 0)
 		goto error;
 
-	dev_info(&state->i2c->dev, "%s: hardware revision 0x%02x, chip type 0x%02x\n",
-		 KBUILD_MODNAME, state->revcode, state->chip_type);
+	if (state->chip_revcode < 26)
+		rev_char = 'A' + state->chip_revcode;
+	else
+		rev_char = '?';
 
-	/* It is a guess that register 0x0118 (chip type?) can be used to
-	 * differ between si2161, si2163 and si2165
-	 * Only si2165 has been tested.
-	 */
-	if (state->revcode == 0x03 && state->chip_type == 0x07) {
+	switch (state->chip_type) {
+	case 0x06:
+		chip_name = "Si2161";
+		state->has_dvbt = true;
+		break;
+	case 0x07:
+		chip_name = "Si2165";
 		state->has_dvbt = true;
 		state->has_dvbc = true;
-	} else {
-		dev_err(&state->i2c->dev, "%s: Unsupported chip.\n",
-			KBUILD_MODNAME);
+		break;
+	default:
+		dev_err(&state->i2c->dev, "%s: Unsupported Silicon Labs chip (type %d, rev %d)\n",
+			KBUILD_MODNAME, state->chip_type, state->chip_revcode);
 		goto error;
 	}
 
+	dev_info(&state->i2c->dev,
+		"%s: Detected Silicon Labs %s-%c (type %d, rev %d)\n",
+		KBUILD_MODNAME, chip_name, rev_char, state->chip_type,
+		state->chip_revcode);
+
+	strlcat(state->frontend.ops.info.name, chip_name,
+			sizeof(state->frontend.ops.info.name));
+
 	n = 0;
 	if (state->has_dvbt) {
 		state->frontend.ops.delsys[n++] = SYS_DVBT;
@@ -1037,4 +1060,4 @@ MODULE_PARM_DESC(debug, "Turn on/off frontend debugging (default:off).");
 MODULE_DESCRIPTION("Silicon Labs Si2165 DVB-C/-T Demodulator driver");
 MODULE_AUTHOR("Matthias Schwarzott <zzam@gentoo.org>");
 MODULE_LICENSE("GPL");
-MODULE_FIRMWARE(SI2165_FIRMWARE);
+MODULE_FIRMWARE(SI2165_FIRMWARE_REV_D);
diff --git a/drivers/media/dvb-frontends/si2165_priv.h b/drivers/media/dvb-frontends/si2165_priv.h
index d4cc93fe1096..2b70cf12cd79 100644
--- a/drivers/media/dvb-frontends/si2165_priv.h
+++ b/drivers/media/dvb-frontends/si2165_priv.h
@@ -18,6 +18,6 @@
 #ifndef _DVB_SI2165_PRIV
 #define _DVB_SI2165_PRIV
 
-#define SI2165_FIRMWARE "dvb-demod-si2165.fw"
+#define SI2165_FIRMWARE_REV_D "dvb-demod-si2165.fw"
 
 #endif /* _DVB_SI2165_PRIV */
diff --git a/drivers/media/dvb-frontends/si2168.c b/drivers/media/dvb-frontends/si2168.c
index 8f81d979de30..1cd93be281ed 100644
--- a/drivers/media/dvb-frontends/si2168.c
+++ b/drivers/media/dvb-frontends/si2168.c
@@ -55,8 +55,7 @@ static int si2168_cmd_execute(struct si2168 *s, struct si2168_cmd *cmd)
 				break;
 		}
 
-		dev_dbg(&s->client->dev, "%s: cmd execution took %d ms\n",
-				__func__,
+		dev_dbg(&s->client->dev, "cmd execution took %d ms\n",
 				jiffies_to_msecs(jiffies) -
 				(jiffies_to_msecs(timeout) - TIMEOUT));
 
@@ -75,7 +74,7 @@ err_mutex_unlock:
 
 	return 0;
 err:
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -150,12 +149,12 @@ static int si2168_read_status(struct dvb_frontend *fe, fe_status_t *status)
 		c->cnr.stat[0].scale = FE_SCALE_NOT_AVAILABLE;
 	}
 
-	dev_dbg(&s->client->dev, "%s: status=%02x args=%*ph\n",
-			__func__, *status, cmd.rlen, cmd.args);
+	dev_dbg(&s->client->dev, "status=%02x args=%*ph\n",
+			*status, cmd.rlen, cmd.args);
 
 	return 0;
 err:
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -168,10 +167,10 @@ static int si2168_set_frontend(struct dvb_frontend *fe)
 	u8 bandwidth, delivery_system;
 
 	dev_dbg(&s->client->dev,
-			"%s: delivery_system=%u modulation=%u frequency=%u bandwidth_hz=%u symbol_rate=%u inversion=%u\n",
-			__func__, c->delivery_system, c->modulation,
+			"delivery_system=%u modulation=%u frequency=%u bandwidth_hz=%u symbol_rate=%u inversion=%u, stream_id=%d\n",
+			c->delivery_system, c->modulation,
 			c->frequency, c->bandwidth_hz, c->symbol_rate,
-			c->inversion);
+			c->inversion, c->stream_id);
 
 	if (!s->active) {
 		ret = -EAGAIN;
@@ -235,6 +234,18 @@ static int si2168_set_frontend(struct dvb_frontend *fe)
 	if (ret)
 		goto err;
 
+	if (c->delivery_system == SYS_DVBT2) {
+		/* select PLP */
+		cmd.args[0] = 0x52;
+		cmd.args[1] = c->stream_id & 0xff;
+		cmd.args[2] = c->stream_id == NO_STREAM_ID_FILTER ? 0 : 1;
+		cmd.wlen = 3;
+		cmd.rlen = 1;
+		ret = si2168_cmd_execute(s, &cmd);
+		if (ret)
+			goto err;
+	}
+
 	memcpy(cmd.args, "\x51\x03", 2);
 	cmd.wlen = 2;
 	cmd.rlen = 12;
@@ -297,13 +308,6 @@ static int si2168_set_frontend(struct dvb_frontend *fe)
 	if (ret)
 		goto err;
 
-	memcpy(cmd.args, "\x14\x00\x01\x10\x16\x00", 6);
-	cmd.wlen = 6;
-	cmd.rlen = 4;
-	ret = si2168_cmd_execute(s, &cmd);
-	if (ret)
-		goto err;
-
 	memcpy(cmd.args, "\x14\x00\x09\x10\xe3\x18", 6);
 	cmd.wlen = 6;
 	cmd.rlen = 4;
@@ -343,7 +347,7 @@ static int si2168_set_frontend(struct dvb_frontend *fe)
 
 	return 0;
 err:
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -357,8 +361,9 @@ static int si2168_init(struct dvb_frontend *fe)
 	struct si2168_cmd cmd;
 	unsigned int chip_id;
 
-	dev_dbg(&s->client->dev, "%s:\n", __func__);
+	dev_dbg(&s->client->dev, "\n");
 
+	/* initialize */
 	memcpy(cmd.args, "\xc0\x12\x00\x0c\x00\x0d\x16\x00\x00\x00\x00\x00\x00", 13);
 	cmd.wlen = 13;
 	cmd.rlen = 0;
@@ -366,6 +371,26 @@ static int si2168_init(struct dvb_frontend *fe)
 	if (ret)
 		goto err;
 
+	if (s->fw_loaded) {
+		/* resume */
+		memcpy(cmd.args, "\xc0\x06\x08\x0f\x00\x20\x21\x01", 8);
+		cmd.wlen = 8;
+		cmd.rlen = 1;
+		ret = si2168_cmd_execute(s, &cmd);
+		if (ret)
+			goto err;
+
+		memcpy(cmd.args, "\x85", 1);
+		cmd.wlen = 1;
+		cmd.rlen = 1;
+		ret = si2168_cmd_execute(s, &cmd);
+		if (ret)
+			goto err;
+
+		goto warm;
+	}
+
+	/* power up */
 	memcpy(cmd.args, "\xc0\x06\x01\x0f\x00\x20\x20\x01", 8);
 	cmd.wlen = 8;
 	cmd.rlen = 1;
@@ -400,16 +425,16 @@ static int si2168_init(struct dvb_frontend *fe)
 		break;
 	default:
 		dev_err(&s->client->dev,
-				"%s: unkown chip version Si21%d-%c%c%c\n",
-				KBUILD_MODNAME, cmd.args[2], cmd.args[1],
+				"unknown chip version Si21%d-%c%c%c\n",
+				cmd.args[2], cmd.args[1],
 				cmd.args[3], cmd.args[4]);
 		ret = -EINVAL;
 		goto err;
 	}
 
 	/* cold state - try to download firmware */
-	dev_info(&s->client->dev, "%s: found a '%s' in cold state\n",
-			KBUILD_MODNAME, si2168_ops.info.name);
+	dev_info(&s->client->dev, "found a '%s' in cold state\n",
+			si2168_ops.info.name);
 
 	/* request the firmware, this will block and timeout */
 	ret = request_firmware(&fw, fw_file, &s->client->dev);
@@ -422,18 +447,18 @@ static int si2168_init(struct dvb_frontend *fe)
 
 		if (ret == 0) {
 			dev_notice(&s->client->dev,
-					"%s: please install firmware file '%s'\n",
-					KBUILD_MODNAME, SI2168_B40_FIRMWARE);
+					"please install firmware file '%s'\n",
+					SI2168_B40_FIRMWARE);
 		} else {
 			dev_err(&s->client->dev,
-					"%s: firmware file '%s' not found\n",
-					KBUILD_MODNAME, fw_file);
+					"firmware file '%s' not found\n",
+					fw_file);
 			goto err;
 		}
 	}
 
-	dev_info(&s->client->dev, "%s: downloading firmware from file '%s'\n",
-			KBUILD_MODNAME, fw_file);
+	dev_info(&s->client->dev, "downloading firmware from file '%s'\n",
+			fw_file);
 
 	for (remaining = fw->size; remaining > 0; remaining -= i2c_wr_max) {
 		len = remaining;
@@ -446,8 +471,8 @@ static int si2168_init(struct dvb_frontend *fe)
 		ret = si2168_cmd_execute(s, &cmd);
 		if (ret) {
 			dev_err(&s->client->dev,
-					"%s: firmware download failed=%d\n",
-					KBUILD_MODNAME, ret);
+					"firmware download failed=%d\n",
+					ret);
 			goto err;
 		}
 	}
@@ -462,8 +487,20 @@ static int si2168_init(struct dvb_frontend *fe)
 	if (ret)
 		goto err;
 
-	dev_info(&s->client->dev, "%s: found a '%s' in warm state\n",
-			KBUILD_MODNAME, si2168_ops.info.name);
+	/* set ts mode */
+	memcpy(cmd.args, "\x14\x00\x01\x10\x10\x00", 6);
+	cmd.args[4] |= s->ts_mode;
+	cmd.wlen = 6;
+	cmd.rlen = 4;
+	ret = si2168_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	s->fw_loaded = true;
+
+warm:
+	dev_info(&s->client->dev, "found a '%s' in warm state\n",
+			si2168_ops.info.name);
 
 	s->active = true;
 
@@ -472,7 +509,7 @@ err:
 	if (fw)
 		release_firmware(fw);
 
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -482,7 +519,7 @@ static int si2168_sleep(struct dvb_frontend *fe)
 	int ret;
 	struct si2168_cmd cmd;
 
-	dev_dbg(&s->client->dev, "%s:\n", __func__);
+	dev_dbg(&s->client->dev, "\n");
 
 	s->active = false;
 
@@ -495,7 +532,7 @@ static int si2168_sleep(struct dvb_frontend *fe)
 
 	return 0;
 err:
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -528,8 +565,7 @@ static int si2168_select(struct i2c_adapter *adap, void *mux_priv, u32 chan)
 	/* open tuner I2C gate */
 	ret = __i2c_transfer(s->client->adapter, &gate_open_msg, 1);
 	if (ret != 1) {
-		dev_warn(&s->client->dev, "%s: i2c write failed=%d\n",
-				KBUILD_MODNAME, ret);
+		dev_warn(&s->client->dev, "i2c write failed=%d\n", ret);
 		if (ret >= 0)
 			ret = -EREMOTEIO;
 	} else {
@@ -553,8 +589,7 @@ static int si2168_deselect(struct i2c_adapter *adap, void *mux_priv, u32 chan)
 	/* close tuner I2C gate */
 	ret = __i2c_transfer(s->client->adapter, &gate_close_msg, 1);
 	if (ret != 1) {
-		dev_warn(&s->client->dev, "%s: i2c write failed=%d\n",
-				KBUILD_MODNAME, ret);
+		dev_warn(&s->client->dev, "i2c write failed=%d\n", ret);
 		if (ret >= 0)
 			ret = -EREMOTEIO;
 	} else {
@@ -587,7 +622,8 @@ static const struct dvb_frontend_ops si2168_ops = {
 			FE_CAN_GUARD_INTERVAL_AUTO |
 			FE_CAN_HIERARCHY_AUTO |
 			FE_CAN_MUTE_TS |
-			FE_CAN_2G_MODULATION
+			FE_CAN_2G_MODULATION |
+			FE_CAN_MULTISTREAM
 	},
 
 	.get_tune_settings = si2168_get_tune_settings,
@@ -607,12 +643,12 @@ static int si2168_probe(struct i2c_client *client,
 	struct si2168 *s;
 	int ret;
 
-	dev_dbg(&client->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	s = kzalloc(sizeof(struct si2168), GFP_KERNEL);
 	if (!s) {
 		ret = -ENOMEM;
-		dev_err(&client->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		dev_err(&client->dev, "kzalloc() failed\n");
 		goto err;
 	}
 
@@ -633,16 +669,17 @@ static int si2168_probe(struct i2c_client *client,
 
 	*config->i2c_adapter = s->adapter;
 	*config->fe = &s->fe;
+	s->ts_mode = config->ts_mode;
+	s->fw_loaded = false;
 
 	i2c_set_clientdata(client, s);
 
 	dev_info(&s->client->dev,
-			"%s: Silicon Labs Si2168 successfully attached\n",
-			KBUILD_MODNAME);
+			"Silicon Labs Si2168 successfully attached\n");
 	return 0;
 err:
 	kfree(s);
-	dev_dbg(&client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -650,7 +687,7 @@ static int si2168_remove(struct i2c_client *client)
 {
 	struct si2168 *s = i2c_get_clientdata(client);
 
-	dev_dbg(&client->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	i2c_del_mux_adapter(s->adapter);
 
diff --git a/drivers/media/dvb-frontends/si2168.h b/drivers/media/dvb-frontends/si2168.h
index 3c5b5ab01796..e086d6719451 100644
--- a/drivers/media/dvb-frontends/si2168.h
+++ b/drivers/media/dvb-frontends/si2168.h
@@ -34,6 +34,12 @@ struct si2168_config {
 	 * returned by driver
 	 */
 	struct i2c_adapter **i2c_adapter;
+
+	/* TS mode */
+	u8 ts_mode;
 };
 
+#define SI2168_TS_PARALLEL	0x06
+#define SI2168_TS_SERIAL	0x03
+
 #endif
diff --git a/drivers/media/dvb-frontends/si2168_priv.h b/drivers/media/dvb-frontends/si2168_priv.h
index ebbf502ec313..e13983ed4be1 100644
--- a/drivers/media/dvb-frontends/si2168_priv.h
+++ b/drivers/media/dvb-frontends/si2168_priv.h
@@ -36,6 +36,8 @@ struct si2168 {
 	fe_delivery_system_t delivery_system;
 	fe_status_t fe_status;
 	bool active;
+	bool fw_loaded;
+	u8 ts_mode;
 };
 
 /* firmare command struct */
diff --git a/drivers/media/dvb-frontends/si21xx.c b/drivers/media/dvb-frontends/si21xx.c
index 73b47cc6a13b..16850e2bf02f 100644
--- a/drivers/media/dvb-frontends/si21xx.c
+++ b/drivers/media/dvb-frontends/si21xx.c
@@ -236,6 +236,9 @@ static int si21_writeregs(struct si21xx_state *state, u8 reg1,
 				.len = len + 1
 	};
 
+	if (len > sizeof(buf) - 1)
+		return -EINVAL;
+
 	msg.buf[0] =  reg1;
 	memcpy(msg.buf + 1, data, len);
 
diff --git a/drivers/media/dvb-frontends/sp2.c b/drivers/media/dvb-frontends/sp2.c
new file mode 100644
index 000000000000..9b684d5c8f91
--- /dev/null
+++ b/drivers/media/dvb-frontends/sp2.c
@@ -0,0 +1,441 @@
+/*
+ * CIMaX SP2/SP2HF (Atmel T90FJR) CI driver
+ *
+ * Copyright (C) 2014 Olli Salonen <olli.salonen@iki.fi>
+ *
+ * Heavily based on CIMax2(R) SP2 driver in conjunction with NetUp Dual
+ * DVB-S2 CI card (cimax2) with following copyrights:
+ *
+ *  Copyright (C) 2009 NetUP Inc.
+ *  Copyright (C) 2009 Igor M. Liplianin <liplianin@netup.ru>
+ *  Copyright (C) 2009 Abylay Ospan <aospan@netup.ru>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ */
+
+#include "sp2_priv.h"
+
+static int sp2_read_i2c(struct sp2 *s, u8 reg, u8 *buf, int len)
+{
+	int ret;
+	struct i2c_client *client = s->client;
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_msg msg[] = {
+		{
+			.addr = client->addr,
+			.flags = 0,
+			.buf = &reg,
+			.len = 1
+		}, {
+			.addr = client->addr,
+			.flags	= I2C_M_RD,
+			.buf = buf,
+			.len = len
+		}
+	};
+
+	ret = i2c_transfer(adap, msg, 2);
+
+	if (ret != 2) {
+		dev_err(&client->dev, "i2c read error, reg = 0x%02x, status = %d\n",
+				reg, ret);
+		if (ret < 0)
+			return ret;
+		else
+			return -EIO;
+	}
+
+	dev_dbg(&s->client->dev, "addr=0x%04x, reg = 0x%02x, data = %02x\n",
+				client->addr, reg, buf[0]);
+
+	return 0;
+}
+
+static int sp2_write_i2c(struct sp2 *s, u8 reg, u8 *buf, int len)
+{
+	int ret;
+	u8 buffer[35];
+	struct i2c_client *client = s->client;
+	struct i2c_adapter *adap = client->adapter;
+	struct i2c_msg msg = {
+		.addr = client->addr,
+		.flags = 0,
+		.buf = &buffer[0],
+		.len = len + 1
+	};
+
+	if ((len + 1) > sizeof(buffer)) {
+		dev_err(&client->dev, "i2c wr reg=%02x: len=%d is too big!\n",
+				reg, len);
+		return -EINVAL;
+	}
+
+	buffer[0] = reg;
+	memcpy(&buffer[1], buf, len);
+
+	ret = i2c_transfer(adap, &msg, 1);
+
+	if (ret != 1) {
+		dev_err(&client->dev, "i2c write error, reg = 0x%02x, status = %d\n",
+				reg, ret);
+		if (ret < 0)
+			return ret;
+		else
+			return -EIO;
+	}
+
+	return 0;
+}
+
+static int sp2_ci_op_cam(struct dvb_ca_en50221 *en50221, int slot, u8 acs,
+			u8 read, int addr, u8 data)
+{
+	struct sp2 *s = en50221->data;
+	u8 store;
+	int mem, ret;
+	int (*ci_op_cam)(void*, u8, int, u8, int*) = s->ci_control;
+
+	dev_dbg(&s->client->dev, "slot=%d, acs=0x%02x, addr=0x%04x, data = 0x%02x",
+			slot, acs, addr, data);
+
+	if (slot != 0)
+		return -EINVAL;
+
+	/*
+	 * change module access type between IO space and attribute memory
+	 * when needed
+	 */
+	if (s->module_access_type != acs) {
+		ret = sp2_read_i2c(s, 0x00, &store, 1);
+
+		if (ret)
+			return ret;
+
+		store &= ~(SP2_MOD_CTL_ACS1 | SP2_MOD_CTL_ACS0);
+		store |= acs;
+
+		ret = sp2_write_i2c(s, 0x00, &store, 1);
+		if (ret)
+			return ret;
+	}
+
+	s->module_access_type = acs;
+
+	/* implementation of ci_op_cam is device specific */
+	if (ci_op_cam) {
+		ret = ci_op_cam(s->priv, read, addr, data, &mem);
+	} else {
+		dev_err(&s->client->dev, "callback not defined");
+		return -EINVAL;
+	}
+
+	if (ret)
+		return ret;
+
+	if (read) {
+		dev_dbg(&s->client->dev, "cam read, addr=0x%04x, data = 0x%04x",
+				addr, mem);
+		return mem;
+	} else {
+		return 0;
+	}
+}
+
+int sp2_ci_read_attribute_mem(struct dvb_ca_en50221 *en50221,
+				int slot, int addr)
+{
+	return sp2_ci_op_cam(en50221, slot, SP2_CI_ATTR_ACS,
+			SP2_CI_RD, addr, 0);
+}
+
+int sp2_ci_write_attribute_mem(struct dvb_ca_en50221 *en50221,
+				int slot, int addr, u8 data)
+{
+	return sp2_ci_op_cam(en50221, slot, SP2_CI_ATTR_ACS,
+			SP2_CI_WR, addr, data);
+}
+
+int sp2_ci_read_cam_control(struct dvb_ca_en50221 *en50221,
+				int slot, u8 addr)
+{
+	return sp2_ci_op_cam(en50221, slot, SP2_CI_IO_ACS,
+			SP2_CI_RD, addr, 0);
+}
+
+int sp2_ci_write_cam_control(struct dvb_ca_en50221 *en50221,
+				int slot, u8 addr, u8 data)
+{
+	return sp2_ci_op_cam(en50221, slot, SP2_CI_IO_ACS,
+			SP2_CI_WR, addr, data);
+}
+
+int sp2_ci_slot_reset(struct dvb_ca_en50221 *en50221, int slot)
+{
+	struct sp2 *s = en50221->data;
+	u8 buf;
+	int ret;
+
+	dev_dbg(&s->client->dev, "slot: %d\n", slot);
+
+	if (slot != 0)
+		return -EINVAL;
+
+	/* RST on */
+	buf = SP2_MOD_CTL_RST;
+	ret = sp2_write_i2c(s, 0x00, &buf, 1);
+
+	if (ret)
+		return ret;
+
+	usleep_range(500, 600);
+
+	/* RST off */
+	buf = 0x00;
+	ret = sp2_write_i2c(s, 0x00, &buf, 1);
+
+	if (ret)
+		return ret;
+
+	msleep(1000);
+
+	return 0;
+}
+
+int sp2_ci_slot_shutdown(struct dvb_ca_en50221 *en50221, int slot)
+{
+	struct sp2 *s = en50221->data;
+
+	dev_dbg(&s->client->dev, "slot:%d\n", slot);
+
+	/* not implemented */
+	return 0;
+}
+
+int sp2_ci_slot_ts_enable(struct dvb_ca_en50221 *en50221, int slot)
+{
+	struct sp2 *s = en50221->data;
+	u8 buf;
+
+	dev_dbg(&s->client->dev, "slot:%d\n", slot);
+
+	if (slot != 0)
+		return -EINVAL;
+
+	sp2_read_i2c(s, 0x00, &buf, 1);
+
+	/* disable bypass and enable TS */
+	buf |= (SP2_MOD_CTL_TSOEN | SP2_MOD_CTL_TSIEN);
+	return sp2_write_i2c(s, 0, &buf, 1);
+}
+
+int sp2_ci_poll_slot_status(struct dvb_ca_en50221 *en50221,
+				int slot, int open)
+{
+	struct sp2 *s = en50221->data;
+	u8 buf[2];
+	int ret;
+
+	dev_dbg(&s->client->dev, "slot:%d open:%d\n", slot, open);
+
+	/*
+	 * CAM module INSERT/REMOVE processing. Slow operation because of i2c
+	 * transfers. Throttle read to one per sec.
+	 */
+	if (time_after(jiffies, s->next_status_checked_time)) {
+		ret = sp2_read_i2c(s, 0x00, buf, 1);
+		s->next_status_checked_time = jiffies +	msecs_to_jiffies(1000);
+
+		if (ret)
+			return 0;
+
+		if (buf[0] & SP2_MOD_CTL_DET)
+			s->status = DVB_CA_EN50221_POLL_CAM_PRESENT |
+					DVB_CA_EN50221_POLL_CAM_READY;
+		else
+			s->status = 0;
+	}
+
+	return s->status;
+}
+
+int sp2_init(struct sp2 *s)
+{
+	int ret = 0;
+	u8 buf;
+	u8 cimax_init[34] = {
+		0x00, /* module A control*/
+		0x00, /* auto select mask high A */
+		0x00, /* auto select mask low A */
+		0x00, /* auto select pattern high A */
+		0x00, /* auto select pattern low A */
+		0x44, /* memory access time A, 600 ns */
+		0x00, /* invert input A */
+		0x00, /* RFU */
+		0x00, /* RFU */
+		0x00, /* module B control*/
+		0x00, /* auto select mask high B */
+		0x00, /* auto select mask low B */
+		0x00, /* auto select pattern high B */
+		0x00, /* auto select pattern low B */
+		0x44, /* memory access time B, 600 ns */
+		0x00, /* invert input B */
+		0x00, /* RFU */
+		0x00, /* RFU */
+		0x00, /* auto select mask high Ext */
+		0x00, /* auto select mask low Ext */
+		0x00, /* auto select pattern high Ext */
+		0x00, /* auto select pattern low Ext */
+		0x00, /* RFU */
+		0x02, /* destination - module A */
+		0x01, /* power control reg, VCC power on */
+		0x00, /* RFU */
+		0x00, /* int status read only */
+		0x00, /* Interrupt Mask Register */
+		0x05, /* EXTINT=active-high, INT=push-pull */
+		0x00, /* USCG1 */
+		0x04, /* ack active low */
+		0x00, /* LOCK = 0 */
+		0x22, /* unknown */
+		0x00, /* synchronization? */
+	};
+
+	dev_dbg(&s->client->dev, "\n");
+
+	s->ca.owner = THIS_MODULE;
+	s->ca.read_attribute_mem = sp2_ci_read_attribute_mem;
+	s->ca.write_attribute_mem = sp2_ci_write_attribute_mem;
+	s->ca.read_cam_control = sp2_ci_read_cam_control;
+	s->ca.write_cam_control = sp2_ci_write_cam_control;
+	s->ca.slot_reset = sp2_ci_slot_reset;
+	s->ca.slot_shutdown = sp2_ci_slot_shutdown;
+	s->ca.slot_ts_enable = sp2_ci_slot_ts_enable;
+	s->ca.poll_slot_status = sp2_ci_poll_slot_status;
+	s->ca.data = s;
+	s->module_access_type = 0;
+
+	/* initialize all regs */
+	ret = sp2_write_i2c(s, 0x00, &cimax_init[0], 34);
+	if (ret)
+		goto err;
+
+	/* lock registers */
+	buf = 1;
+	ret = sp2_write_i2c(s, 0x1f, &buf, 1);
+	if (ret)
+		goto err;
+
+	/* power on slots */
+	ret = sp2_write_i2c(s, 0x18, &buf, 1);
+	if (ret)
+		goto err;
+
+	ret = dvb_ca_en50221_init(s->dvb_adap, &s->ca, 0, 1);
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&s->client->dev, "init failed=%d\n", ret);
+	return ret;
+}
+
+int sp2_exit(struct i2c_client *client)
+{
+	struct sp2 *s;
+
+	dev_dbg(&client->dev, "\n");
+
+	if (client == NULL)
+		return 0;
+
+	s = i2c_get_clientdata(client);
+	if (s == NULL)
+		return 0;
+
+	if (s->ca.data == NULL)
+		return 0;
+
+	dvb_ca_en50221_release(&s->ca);
+
+	return 0;
+}
+
+static int sp2_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct sp2_config *cfg = client->dev.platform_data;
+	struct sp2 *s;
+	int ret;
+
+	dev_dbg(&client->dev, "\n");
+
+	s = kzalloc(sizeof(struct sp2), GFP_KERNEL);
+	if (!s) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "kzalloc() failed\n");
+		goto err;
+	}
+
+	s->client = client;
+	s->dvb_adap = cfg->dvb_adap;
+	s->priv = cfg->priv;
+	s->ci_control = cfg->ci_control;
+
+	i2c_set_clientdata(client, s);
+
+	ret = sp2_init(s);
+	if (ret)
+		goto err;
+
+	dev_info(&s->client->dev, "CIMaX SP2 successfully attached\n");
+	return 0;
+err:
+	dev_dbg(&client->dev, "init failed=%d\n", ret);
+	kfree(s);
+
+	return ret;
+}
+
+static int sp2_remove(struct i2c_client *client)
+{
+	struct si2157 *s = i2c_get_clientdata(client);
+
+	dev_dbg(&client->dev, "\n");
+
+	sp2_exit(client);
+	if (s != NULL)
+		kfree(s);
+
+	return 0;
+}
+
+static const struct i2c_device_id sp2_id[] = {
+	{"sp2", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, sp2_id);
+
+static struct i2c_driver sp2_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "sp2",
+	},
+	.probe		= sp2_probe,
+	.remove		= sp2_remove,
+	.id_table	= sp2_id,
+};
+
+module_i2c_driver(sp2_driver);
+
+MODULE_DESCRIPTION("CIMaX SP2/HF CI driver");
+MODULE_AUTHOR("Olli Salonen <olli.salonen@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb-frontends/sp2.h b/drivers/media/dvb-frontends/sp2.h
new file mode 100644
index 000000000000..6cceea022d49
--- /dev/null
+++ b/drivers/media/dvb-frontends/sp2.h
@@ -0,0 +1,53 @@
+/*
+ * CIMaX SP2/HF CI driver
+ *
+ * Copyright (C) 2014 Olli Salonen <olli.salonen@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ */
+
+#ifndef SP2_H
+#define SP2_H
+
+#include <linux/kconfig.h>
+#include "dvb_ca_en50221.h"
+
+/*
+ * I2C address
+ * 0x40 (port 0)
+ * 0x41 (port 1)
+ */
+struct sp2_config {
+	/* dvb_adapter to attach the ci to */
+	struct dvb_adapter *dvb_adap;
+
+	/* function ci_control handles the device specific ci ops */
+	void *ci_control;
+
+	/* priv is passed back to function ci_control */
+	void *priv;
+};
+
+extern int sp2_ci_read_attribute_mem(struct dvb_ca_en50221 *en50221,
+					int slot, int addr);
+extern int sp2_ci_write_attribute_mem(struct dvb_ca_en50221 *en50221,
+					int slot, int addr, u8 data);
+extern int sp2_ci_read_cam_control(struct dvb_ca_en50221 *en50221,
+					int slot, u8 addr);
+extern int sp2_ci_write_cam_control(struct dvb_ca_en50221 *en50221,
+					int slot, u8 addr, u8 data);
+extern int sp2_ci_slot_reset(struct dvb_ca_en50221 *en50221, int slot);
+extern int sp2_ci_slot_shutdown(struct dvb_ca_en50221 *en50221, int slot);
+extern int sp2_ci_slot_ts_enable(struct dvb_ca_en50221 *en50221, int slot);
+extern int sp2_ci_poll_slot_status(struct dvb_ca_en50221 *en50221,
+					int slot, int open);
+
+#endif
diff --git a/drivers/media/dvb-frontends/sp2_priv.h b/drivers/media/dvb-frontends/sp2_priv.h
new file mode 100644
index 000000000000..37fef7bcd63f
--- /dev/null
+++ b/drivers/media/dvb-frontends/sp2_priv.h
@@ -0,0 +1,50 @@
+/*
+ * CIMaX SP2/HF CI driver
+ *
+ * Copyright (C) 2014 Olli Salonen <olli.salonen@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ */
+
+#ifndef SP2_PRIV_H
+#define SP2_PRIV_H
+
+#include "sp2.h"
+#include "dvb_frontend.h"
+
+/* state struct */
+struct sp2 {
+	int status;
+	struct i2c_client *client;
+	struct dvb_adapter *dvb_adap;
+	struct dvb_ca_en50221 ca;
+	int module_access_type;
+	unsigned long next_status_checked_time;
+	void *priv;
+	void *ci_control;
+};
+
+#define SP2_CI_ATTR_ACS		0x00
+#define SP2_CI_IO_ACS		0x04
+#define SP2_CI_WR		0
+#define SP2_CI_RD		1
+
+/* Module control register (0x00 module A, 0x09 module B) bits */
+#define SP2_MOD_CTL_DET		0x01
+#define SP2_MOD_CTL_AUTO	0x02
+#define SP2_MOD_CTL_ACS0	0x04
+#define SP2_MOD_CTL_ACS1	0x08
+#define SP2_MOD_CTL_HAD		0x10
+#define SP2_MOD_CTL_TSIEN	0x20
+#define SP2_MOD_CTL_TSOEN	0x40
+#define SP2_MOD_CTL_RST		0x80
+
+#endif
diff --git a/drivers/media/dvb-frontends/sp8870.c b/drivers/media/dvb-frontends/sp8870.c
index 2aa8ef76eba2..57dc2abaa87b 100644
--- a/drivers/media/dvb-frontends/sp8870.c
+++ b/drivers/media/dvb-frontends/sp8870.c
@@ -394,8 +394,7 @@ static int sp8870_read_ber (struct dvb_frontend* fe, u32 * ber)
 	if (ret < 0)
 		return -EIO;
 
-	 tmp = ret << 6;
-
+	tmp = ret << 6;
 	if (tmp >= 0x3FFF0)
 		tmp = ~0;
 
diff --git a/drivers/media/dvb-frontends/stv0367.c b/drivers/media/dvb-frontends/stv0367.c
index 59b6e661acc0..b31ff265ff24 100644
--- a/drivers/media/dvb-frontends/stv0367.c
+++ b/drivers/media/dvb-frontends/stv0367.c
@@ -59,7 +59,6 @@ struct stv0367cab_state {
 	int locked;			/* channel found		*/
 	u32 freq_khz;			/* found frequency (in kHz)	*/
 	u32 symbol_rate;		/* found symbol rate (in Bds)	*/
-	enum stv0367cab_mod modulation;	/* modulation			*/
 	fe_spectral_inversion_t	spect_inv; /* Spectrum Inversion	*/
 };
 
@@ -554,7 +553,7 @@ static struct st_register def0367ter[STV0367TER_NBREGS] = {
 #define RF_LOOKUP_TABLE_SIZE  31
 #define RF_LOOKUP_TABLE2_SIZE 16
 /* RF Level (for RF AGC->AGC1) Lookup Table, depends on the board and tuner.*/
-s32 stv0367cab_RF_LookUp1[RF_LOOKUP_TABLE_SIZE][RF_LOOKUP_TABLE_SIZE] = {
+static const s32 stv0367cab_RF_LookUp1[RF_LOOKUP_TABLE_SIZE][RF_LOOKUP_TABLE_SIZE] = {
 	{/*AGC1*/
 		48, 50, 51, 53, 54, 56, 57, 58, 60, 61, 62, 63,
 		64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75,
@@ -566,7 +565,7 @@ s32 stv0367cab_RF_LookUp1[RF_LOOKUP_TABLE_SIZE][RF_LOOKUP_TABLE_SIZE] = {
 	}
 };
 /* RF Level (for IF AGC->AGC2) Lookup Table, depends on the board and tuner.*/
-s32 stv0367cab_RF_LookUp2[RF_LOOKUP_TABLE2_SIZE][RF_LOOKUP_TABLE2_SIZE] = {
+static const s32 stv0367cab_RF_LookUp2[RF_LOOKUP_TABLE2_SIZE][RF_LOOKUP_TABLE2_SIZE] = {
 	{/*AGC2*/
 		28, 29, 31, 32, 34, 35, 36, 37,
 		38, 39, 40, 41, 42, 43, 44, 45,
@@ -1935,8 +1934,6 @@ static int stv0367ter_get_frontend(struct dvb_frontend *fe)
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
 	struct stv0367_state *state = fe->demodulator_priv;
 	struct stv0367ter_state *ter_state = state->ter_state;
-
-	int error = 0;
 	enum stv0367_ter_mode mode;
 	int constell = 0,/* snr = 0,*/ Data = 0;
 
@@ -2020,7 +2017,7 @@ static int stv0367ter_get_frontend(struct dvb_frontend *fe)
 
 	p->guard_interval = stv0367_readbits(state, F367TER_SYR_GUARD);
 
-	return error;
+	return 0;
 }
 
 static int stv0367ter_read_snr(struct dvb_frontend *fe, u16 *snr)
@@ -2999,7 +2996,6 @@ enum stv0367_cab_signal_type stv0367cab_algo(struct stv0367_state *state,
 
 	if (QAMFEC_Lock) {
 		signalType = FE_CAB_DATAOK;
-		cab_state->modulation = p->modulation;
 		cab_state->spect_inv = stv0367_readbits(state,
 							F367CAB_QUAD_INV);
 #if 0
@@ -3165,7 +3161,7 @@ static int stv0367cab_get_frontend(struct dvb_frontend *fe)
 	case FE_CAB_MOD_QAM128:
 		p->modulation = QAM_128;
 		break;
-	case QAM_256:
+	case FE_CAB_MOD_QAM256:
 		p->modulation = QAM_256;
 		break;
 	default:
diff --git a/drivers/media/dvb-frontends/stv0900_core.c b/drivers/media/dvb-frontends/stv0900_core.c
index e5a87b57d855..2c88abfab531 100644
--- a/drivers/media/dvb-frontends/stv0900_core.c
+++ b/drivers/media/dvb-frontends/stv0900_core.c
@@ -1270,7 +1270,6 @@ enum fe_stv0900_error stv0900_st_dvbs2_single(struct stv0900_internal *intp,
 					enum fe_stv0900_demod_mode LDPC_Mode,
 					enum fe_stv0900_demod_num demod)
 {
-	enum fe_stv0900_error error = STV0900_NO_ERROR;
 	s32 reg_ind;
 
 	dprintk("%s\n", __func__);
@@ -1337,7 +1336,7 @@ enum fe_stv0900_error stv0900_st_dvbs2_single(struct stv0900_internal *intp,
 		break;
 	}
 
-	return error;
+	return STV0900_NO_ERROR;
 }
 
 static enum fe_stv0900_error stv0900_init_internal(struct dvb_frontend *fe,
@@ -1555,8 +1554,6 @@ static int stv0900_status(struct stv0900_internal *intp,
 static int stv0900_set_mis(struct stv0900_internal *intp,
 				enum fe_stv0900_demod_num demod, int mis)
 {
-	enum fe_stv0900_error error = STV0900_NO_ERROR;
-
 	dprintk("%s\n", __func__);
 
 	if (mis < 0 || mis > 255) {
@@ -1569,7 +1566,7 @@ static int stv0900_set_mis(struct stv0900_internal *intp,
 		stv0900_write_reg(intp, ISIBITENA, 0xff);
 	}
 
-	return error;
+	return STV0900_NO_ERROR;
 }
 
 
diff --git a/drivers/media/dvb-frontends/stv0900_sw.c b/drivers/media/dvb-frontends/stv0900_sw.c
index 4ce1d260b3eb..a0a7b1664c53 100644
--- a/drivers/media/dvb-frontends/stv0900_sw.c
+++ b/drivers/media/dvb-frontends/stv0900_sw.c
@@ -1733,9 +1733,10 @@ static void stv0900_set_search_standard(struct stv0900_internal *intp,
 		break;
 	case STV0900_SEARCH_DSS:
 		dprintk("Search Standard = DSS\n");
-	case STV0900_SEARCH_DVBS2:
 		break;
+	case STV0900_SEARCH_DVBS2:
 		dprintk("Search Standard = DVBS2\n");
+		break;
 	case STV0900_AUTO_SEARCH:
 	default:
 		dprintk("Search Standard = AUTO\n");
diff --git a/drivers/media/dvb-frontends/tc90522.c b/drivers/media/dvb-frontends/tc90522.c
new file mode 100644
index 000000000000..d9905fb52f84
--- /dev/null
+++ b/drivers/media/dvb-frontends/tc90522.c
@@ -0,0 +1,840 @@
+/*
+ * Toshiba TC90522 Demodulator
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * NOTICE:
+ * This driver is incomplete and lacks init/config of the chips,
+ * as the necessary info is not disclosed.
+ * It assumes that users of this driver (such as a PCI bridge of
+ * DTV receiver cards) properly init and configure the chip
+ * via I2C *before* calling this driver's init() function.
+ *
+ * Currently, PT3 driver is the only one that uses this driver,
+ * and contains init/config code in its firmware.
+ * Thus some part of the code might be dependent on PT3 specific config.
+ */
+
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include <linux/dvb/frontend.h>
+#include "dvb_math.h"
+#include "tc90522.h"
+
+#define TC90522_I2C_THRU_REG 0xfe
+
+#define TC90522_MODULE_IDX(addr) (((u8)(addr) & 0x02U) >> 1)
+
+struct tc90522_state {
+	struct tc90522_config cfg;
+	struct dvb_frontend fe;
+	struct i2c_client *i2c_client;
+	struct i2c_adapter tuner_i2c;
+
+	bool lna;
+};
+
+struct reg_val {
+	u8 reg;
+	u8 val;
+};
+
+static int
+reg_write(struct tc90522_state *state, const struct reg_val *regs, int num)
+{
+	int i, ret;
+	struct i2c_msg msg;
+
+	ret = 0;
+	msg.addr = state->i2c_client->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	for (i = 0; i < num; i++) {
+		msg.buf = (u8 *)&regs[i];
+		ret = i2c_transfer(state->i2c_client->adapter, &msg, 1);
+		if (ret == 0)
+			ret = -EIO;
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int reg_read(struct tc90522_state *state, u8 reg, u8 *val, u8 len)
+{
+	struct i2c_msg msgs[2] = {
+		{
+			.addr = state->i2c_client->addr,
+			.flags = 0,
+			.buf = &reg,
+			.len = 1,
+		},
+		{
+			.addr = state->i2c_client->addr,
+			.flags = I2C_M_RD,
+			.buf = val,
+			.len = len,
+		},
+	};
+	int ret;
+
+	ret = i2c_transfer(state->i2c_client->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret == ARRAY_SIZE(msgs))
+		ret = 0;
+	else if (ret >= 0)
+		ret = -EIO;
+	return ret;
+}
+
+static struct tc90522_state *cfg_to_state(struct tc90522_config *c)
+{
+	return container_of(c, struct tc90522_state, cfg);
+}
+
+
+static int tc90522s_set_tsid(struct dvb_frontend *fe)
+{
+	struct reg_val set_tsid[] = {
+		{ 0x8f, 00 },
+		{ 0x90, 00 }
+	};
+
+	set_tsid[0].val = (fe->dtv_property_cache.stream_id & 0xff00) >> 8;
+	set_tsid[1].val = fe->dtv_property_cache.stream_id & 0xff;
+	return reg_write(fe->demodulator_priv, set_tsid, ARRAY_SIZE(set_tsid));
+}
+
+static int tc90522t_set_layers(struct dvb_frontend *fe)
+{
+	struct reg_val rv;
+	u8 laysel;
+
+	laysel = ~fe->dtv_property_cache.isdbt_layer_enabled & 0x07;
+	laysel = (laysel & 0x01) << 2 | (laysel & 0x02) | (laysel & 0x04) >> 2;
+	rv.reg = 0x71;
+	rv.val = laysel;
+	return reg_write(fe->demodulator_priv, &rv, 1);
+}
+
+/* frontend ops */
+
+static int tc90522s_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct tc90522_state *state;
+	int ret;
+	u8 reg;
+
+	state = fe->demodulator_priv;
+	ret = reg_read(state, 0xc3, &reg, 1);
+	if (ret < 0)
+		return ret;
+
+	*status = 0;
+	if (reg & 0x80) /* input level under min ? */
+		return 0;
+	*status |= FE_HAS_SIGNAL;
+
+	if (reg & 0x60) /* carrier? */
+		return 0;
+	*status |= FE_HAS_CARRIER | FE_HAS_VITERBI | FE_HAS_SYNC;
+
+	if (reg & 0x10)
+		return 0;
+	if (reg_read(state, 0xc5, &reg, 1) < 0 || !(reg & 0x03))
+		return 0;
+	*status |= FE_HAS_LOCK;
+	return 0;
+}
+
+static int tc90522t_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct tc90522_state *state;
+	int ret;
+	u8 reg;
+
+	state = fe->demodulator_priv;
+	ret = reg_read(state, 0x96, &reg, 1);
+	if (ret < 0)
+		return ret;
+
+	*status = 0;
+	if (reg & 0xe0) {
+		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER | FE_HAS_VITERBI
+				| FE_HAS_SYNC | FE_HAS_LOCK;
+		return 0;
+	}
+
+	ret = reg_read(state, 0x80, &reg, 1);
+	if (ret < 0)
+		return ret;
+
+	if (reg & 0xf0)
+		return 0;
+	*status |= FE_HAS_SIGNAL | FE_HAS_CARRIER;
+
+	if (reg & 0x0c)
+		return 0;
+	*status |= FE_HAS_SYNC | FE_HAS_VITERBI;
+
+	if (reg & 0x02)
+		return 0;
+	*status |= FE_HAS_LOCK;
+	return 0;
+}
+
+static const fe_code_rate_t fec_conv_sat[] = {
+	FEC_NONE, /* unused */
+	FEC_1_2, /* for BPSK */
+	FEC_1_2, FEC_2_3, FEC_3_4, FEC_5_6, FEC_7_8, /* for QPSK */
+	FEC_2_3, /* for 8PSK. (trellis code) */
+};
+
+static int tc90522s_get_frontend(struct dvb_frontend *fe)
+{
+	struct tc90522_state *state;
+	struct dtv_frontend_properties *c;
+	struct dtv_fe_stats *stats;
+	int ret, i;
+	int layers;
+	u8 val[10];
+	u32 cndat;
+
+	state = fe->demodulator_priv;
+	c = &fe->dtv_property_cache;
+	c->delivery_system = SYS_ISDBS;
+
+	layers = 0;
+	ret = reg_read(state, 0xe8, val, 3);
+	if (ret == 0) {
+		int slots;
+		u8 v;
+
+		/* high/single layer */
+		v = (val[0] & 0x70) >> 4;
+		c->modulation = (v == 7) ? PSK_8 : QPSK;
+		c->fec_inner = fec_conv_sat[v];
+		c->layer[0].fec = c->fec_inner;
+		c->layer[0].modulation = c->modulation;
+		c->layer[0].segment_count = val[1] & 0x3f; /* slots */
+
+		/* low layer */
+		v = (val[0] & 0x07);
+		c->layer[1].fec = fec_conv_sat[v];
+		if (v == 0)  /* no low layer */
+			c->layer[1].segment_count = 0;
+		else
+			c->layer[1].segment_count = val[2] & 0x3f; /* slots */
+		/* actually, BPSK if v==1, but not defined in fe_modulation_t */
+		c->layer[1].modulation = QPSK;
+		layers = (v > 0) ? 2 : 1;
+
+		slots =  c->layer[0].segment_count +  c->layer[1].segment_count;
+		c->symbol_rate = 28860000 * slots / 48;
+	}
+
+	/* statistics */
+
+	stats = &c->strength;
+	stats->len = 0;
+	/* let the connected tuner set RSSI property cache */
+	if (fe->ops.tuner_ops.get_rf_strength) {
+		u16 dummy;
+
+		fe->ops.tuner_ops.get_rf_strength(fe, &dummy);
+	}
+
+	stats = &c->cnr;
+	stats->len = 1;
+	stats->stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	cndat = 0;
+	ret = reg_read(state, 0xbc, val, 2);
+	if (ret == 0)
+		cndat = val[0] << 8 | val[1];
+	if (cndat >= 3000) {
+		u32 p, p4;
+		s64 cn;
+
+		cndat -= 3000;  /* cndat: 4.12 fixed point float */
+		/*
+		 * cnr[mdB] = -1634.6 * P^5 + 14341 * P^4 - 50259 * P^3
+		 *                 + 88977 * P^2 - 89565 * P + 58857
+		 *  (P = sqrt(cndat) / 64)
+		 */
+		/* p := sqrt(cndat) << 8 = P << 14, 2.14 fixed  point float */
+		/* cn = cnr << 3 */
+		p = int_sqrt(cndat << 16);
+		p4 = cndat * cndat;
+		cn = div64_s64(-16346LL * p4 * p, 10) >> 35;
+		cn += (14341LL * p4) >> 21;
+		cn -= (50259LL * cndat * p) >> 23;
+		cn += (88977LL * cndat) >> 9;
+		cn -= (89565LL * p) >> 11;
+		cn += 58857  << 3;
+		stats->stat[0].svalue = cn >> 3;
+		stats->stat[0].scale = FE_SCALE_DECIBEL;
+	}
+
+	/* per-layer post viterbi BER (or PER? config dependent?) */
+	stats = &c->post_bit_error;
+	memset(stats, 0, sizeof(*stats));
+	stats->len = layers;
+	ret = reg_read(state, 0xeb, val, 10);
+	if (ret < 0)
+		for (i = 0; i < layers; i++)
+			stats->stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	else {
+		for (i = 0; i < layers; i++) {
+			stats->stat[i].scale = FE_SCALE_COUNTER;
+			stats->stat[i].uvalue = val[i * 5] << 16
+				| val[i * 5 + 1] << 8 | val[i * 5 + 2];
+		}
+	}
+	stats = &c->post_bit_count;
+	memset(stats, 0, sizeof(*stats));
+	stats->len = layers;
+	if (ret < 0)
+		for (i = 0; i < layers; i++)
+			stats->stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	else {
+		for (i = 0; i < layers; i++) {
+			stats->stat[i].scale = FE_SCALE_COUNTER;
+			stats->stat[i].uvalue =
+				val[i * 5 + 3] << 8 | val[i * 5 + 4];
+			stats->stat[i].uvalue *= 204 * 8;
+		}
+	}
+
+	return 0;
+}
+
+
+static const fe_transmit_mode_t tm_conv[] = {
+	TRANSMISSION_MODE_2K,
+	TRANSMISSION_MODE_4K,
+	TRANSMISSION_MODE_8K,
+	0
+};
+
+static const fe_code_rate_t fec_conv_ter[] = {
+	FEC_1_2, FEC_2_3, FEC_3_4, FEC_5_6, FEC_7_8, 0, 0, 0
+};
+
+static const fe_modulation_t mod_conv[] = {
+	DQPSK, QPSK, QAM_16, QAM_64, 0, 0, 0, 0
+};
+
+static int tc90522t_get_frontend(struct dvb_frontend *fe)
+{
+	struct tc90522_state *state;
+	struct dtv_frontend_properties *c;
+	struct dtv_fe_stats *stats;
+	int ret, i;
+	int layers;
+	u8 val[15], mode;
+	u32 cndat;
+
+	state = fe->demodulator_priv;
+	c = &fe->dtv_property_cache;
+	c->delivery_system = SYS_ISDBT;
+	c->bandwidth_hz = 6000000;
+	mode = 1;
+	ret = reg_read(state, 0xb0, val, 1);
+	if (ret == 0) {
+		mode = (val[0] & 0xc0) >> 2;
+		c->transmission_mode = tm_conv[mode];
+		c->guard_interval = (val[0] & 0x30) >> 4;
+	}
+
+	ret = reg_read(state, 0xb2, val, 6);
+	layers = 0;
+	if (ret == 0) {
+		u8 v;
+
+		c->isdbt_partial_reception = val[0] & 0x01;
+		c->isdbt_sb_mode = (val[0] & 0xc0) == 0x01;
+
+		/* layer A */
+		v = (val[2] & 0x78) >> 3;
+		if (v == 0x0f)
+			c->layer[0].segment_count = 0;
+		else {
+			layers++;
+			c->layer[0].segment_count = v;
+			c->layer[0].fec = fec_conv_ter[(val[1] & 0x1c) >> 2];
+			c->layer[0].modulation = mod_conv[(val[1] & 0xe0) >> 5];
+			v = (val[1] & 0x03) << 1 | (val[2] & 0x80) >> 7;
+			c->layer[0].interleaving = v;
+		}
+
+		/* layer B */
+		v = (val[3] & 0x03) << 1 | (val[4] & 0xc0) >> 6;
+		if (v == 0x0f)
+			c->layer[1].segment_count = 0;
+		else {
+			layers++;
+			c->layer[1].segment_count = v;
+			c->layer[1].fec = fec_conv_ter[(val[3] & 0xe0) >> 5];
+			c->layer[1].modulation = mod_conv[(val[2] & 0x07)];
+			c->layer[1].interleaving = (val[3] & 0x1c) >> 2;
+		}
+
+		/* layer C */
+		v = (val[5] & 0x1e) >> 1;
+		if (v == 0x0f)
+			c->layer[2].segment_count = 0;
+		else {
+			layers++;
+			c->layer[2].segment_count = v;
+			c->layer[2].fec = fec_conv_ter[(val[4] & 0x07)];
+			c->layer[2].modulation = mod_conv[(val[4] & 0x38) >> 3];
+			c->layer[2].interleaving = (val[5] & 0xe0) >> 5;
+		}
+	}
+
+	/* statistics */
+
+	stats = &c->strength;
+	stats->len = 0;
+	/* let the connected tuner set RSSI property cache */
+	if (fe->ops.tuner_ops.get_rf_strength) {
+		u16 dummy;
+
+		fe->ops.tuner_ops.get_rf_strength(fe, &dummy);
+	}
+
+	stats = &c->cnr;
+	stats->len = 1;
+	stats->stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	cndat = 0;
+	ret = reg_read(state, 0x8b, val, 3);
+	if (ret == 0)
+		cndat = val[0] << 16 | val[1] << 8 | val[2];
+	if (cndat != 0) {
+		u32 p, tmp;
+		s64 cn;
+
+		/*
+		 * cnr[mdB] = 0.024 P^4 - 1.6 P^3 + 39.8 P^2 + 549.1 P + 3096.5
+		 * (P = 10log10(5505024/cndat))
+		 */
+		/* cn = cnr << 3 (61.3 fixed point float */
+		/* p = 10log10(5505024/cndat) << 24  (8.24 fixed point float)*/
+		p = intlog10(5505024) - intlog10(cndat);
+		p *= 10;
+
+		cn = 24772;
+		cn += div64_s64(43827LL * p, 10) >> 24;
+		tmp = p >> 8;
+		cn += div64_s64(3184LL * tmp * tmp, 10) >> 32;
+		tmp = p >> 13;
+		cn -= div64_s64(128LL * tmp * tmp * tmp, 10) >> 33;
+		tmp = p >> 18;
+		cn += div64_s64(192LL * tmp * tmp * tmp * tmp, 1000) >> 24;
+
+		stats->stat[0].svalue = cn >> 3;
+		stats->stat[0].scale = FE_SCALE_DECIBEL;
+	}
+
+	/* per-layer post viterbi BER (or PER? config dependent?) */
+	stats = &c->post_bit_error;
+	memset(stats, 0, sizeof(*stats));
+	stats->len = layers;
+	ret = reg_read(state, 0x9d, val, 15);
+	if (ret < 0)
+		for (i = 0; i < layers; i++)
+			stats->stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	else {
+		for (i = 0; i < layers; i++) {
+			stats->stat[i].scale = FE_SCALE_COUNTER;
+			stats->stat[i].uvalue = val[i * 3] << 16
+				| val[i * 3 + 1] << 8 | val[i * 3 + 2];
+		}
+	}
+	stats = &c->post_bit_count;
+	memset(stats, 0, sizeof(*stats));
+	stats->len = layers;
+	if (ret < 0)
+		for (i = 0; i < layers; i++)
+			stats->stat[i].scale = FE_SCALE_NOT_AVAILABLE;
+	else {
+		for (i = 0; i < layers; i++) {
+			stats->stat[i].scale = FE_SCALE_COUNTER;
+			stats->stat[i].uvalue =
+				val[9 + i * 2] << 8 | val[9 + i * 2 + 1];
+			stats->stat[i].uvalue *= 204 * 8;
+		}
+	}
+
+	return 0;
+}
+
+static const struct reg_val reset_sat = { 0x03, 0x01 };
+static const struct reg_val reset_ter = { 0x01, 0x40 };
+
+static int tc90522_set_frontend(struct dvb_frontend *fe)
+{
+	struct tc90522_state *state;
+	int ret;
+
+	state = fe->demodulator_priv;
+
+	if (fe->ops.tuner_ops.set_params)
+		ret = fe->ops.tuner_ops.set_params(fe);
+	else
+		ret = -ENODEV;
+	if (ret < 0)
+		goto failed;
+
+	if (fe->ops.delsys[0] == SYS_ISDBS) {
+		ret = tc90522s_set_tsid(fe);
+		if (ret < 0)
+			goto failed;
+		ret = reg_write(state, &reset_sat, 1);
+	} else {
+		ret = tc90522t_set_layers(fe);
+		if (ret < 0)
+			goto failed;
+		ret = reg_write(state, &reset_ter, 1);
+	}
+	if (ret < 0)
+		goto failed;
+
+	return 0;
+
+failed:
+	dev_warn(&state->tuner_i2c.dev, "(%s) failed. [adap%d-fe%d]\n",
+			__func__, fe->dvb->num, fe->id);
+	return ret;
+}
+
+static int tc90522_get_tune_settings(struct dvb_frontend *fe,
+	struct dvb_frontend_tune_settings *settings)
+{
+	if (fe->ops.delsys[0] == SYS_ISDBS) {
+		settings->min_delay_ms = 250;
+		settings->step_size = 1000;
+		settings->max_drift = settings->step_size * 2;
+	} else {
+		settings->min_delay_ms = 400;
+		settings->step_size = 142857;
+		settings->max_drift = settings->step_size;
+	}
+	return 0;
+}
+
+static int tc90522_set_if_agc(struct dvb_frontend *fe, bool on)
+{
+	struct reg_val agc_sat[] = {
+		{ 0x0a, 0x00 },
+		{ 0x10, 0x30 },
+		{ 0x11, 0x00 },
+		{ 0x03, 0x01 },
+	};
+	struct reg_val agc_ter[] = {
+		{ 0x25, 0x00 },
+		{ 0x23, 0x4c },
+		{ 0x01, 0x40 },
+	};
+	struct tc90522_state *state;
+	struct reg_val *rv;
+	int num;
+
+	state = fe->demodulator_priv;
+	if (fe->ops.delsys[0] == SYS_ISDBS) {
+		agc_sat[0].val = on ? 0xff : 0x00;
+		agc_sat[1].val |= 0x80;
+		agc_sat[1].val |= on ? 0x01 : 0x00;
+		agc_sat[2].val |= on ? 0x40 : 0x00;
+		rv = agc_sat;
+		num = ARRAY_SIZE(agc_sat);
+	} else {
+		agc_ter[0].val = on ? 0x40 : 0x00;
+		agc_ter[1].val |= on ? 0x00 : 0x01;
+		rv = agc_ter;
+		num = ARRAY_SIZE(agc_ter);
+	}
+	return reg_write(state, rv, num);
+}
+
+static const struct reg_val sleep_sat = { 0x17, 0x01 };
+static const struct reg_val sleep_ter = { 0x03, 0x90 };
+
+static int tc90522_sleep(struct dvb_frontend *fe)
+{
+	struct tc90522_state *state;
+	int ret;
+
+	state = fe->demodulator_priv;
+	if (fe->ops.delsys[0] == SYS_ISDBS)
+		ret = reg_write(state, &sleep_sat, 1);
+	else {
+		ret = reg_write(state, &sleep_ter, 1);
+		if (ret == 0 && fe->ops.set_lna &&
+		    fe->dtv_property_cache.lna == LNA_AUTO) {
+			fe->dtv_property_cache.lna = 0;
+			ret = fe->ops.set_lna(fe);
+			fe->dtv_property_cache.lna = LNA_AUTO;
+		}
+	}
+	if (ret < 0)
+		dev_warn(&state->tuner_i2c.dev,
+			"(%s) failed. [adap%d-fe%d]\n",
+			__func__, fe->dvb->num, fe->id);
+	return ret;
+}
+
+static const struct reg_val wakeup_sat = { 0x17, 0x00 };
+static const struct reg_val wakeup_ter = { 0x03, 0x80 };
+
+static int tc90522_init(struct dvb_frontend *fe)
+{
+	struct tc90522_state *state;
+	int ret;
+
+	/*
+	 * Because the init sequence is not public,
+	 * the parent device/driver should have init'ed the device before.
+	 * just wake up the device here.
+	 */
+
+	state = fe->demodulator_priv;
+	if (fe->ops.delsys[0] == SYS_ISDBS)
+		ret = reg_write(state, &wakeup_sat, 1);
+	else {
+		ret = reg_write(state, &wakeup_ter, 1);
+		if (ret == 0 && fe->ops.set_lna &&
+		    fe->dtv_property_cache.lna == LNA_AUTO) {
+			fe->dtv_property_cache.lna = 1;
+			ret = fe->ops.set_lna(fe);
+			fe->dtv_property_cache.lna = LNA_AUTO;
+		}
+	}
+	if (ret < 0) {
+		dev_warn(&state->tuner_i2c.dev,
+			"(%s) failed. [adap%d-fe%d]\n",
+			__func__, fe->dvb->num, fe->id);
+		return ret;
+	}
+
+	/* prefer 'all-layers' to 'none' as a default */
+	if (fe->dtv_property_cache.isdbt_layer_enabled == 0)
+		fe->dtv_property_cache.isdbt_layer_enabled = 7;
+	return tc90522_set_if_agc(fe, true);
+}
+
+
+/*
+ * tuner I2C adapter functions
+ */
+
+static int
+tc90522_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
+{
+	struct tc90522_state *state;
+	struct i2c_msg *new_msgs;
+	int i, j;
+	int ret, rd_num;
+	u8 wbuf[256];
+	u8 *p, *bufend;
+
+	if (num <= 0)
+		return -EINVAL;
+
+	rd_num = 0;
+	for (i = 0; i < num; i++)
+		if (msgs[i].flags & I2C_M_RD)
+			rd_num++;
+	new_msgs = kmalloc(sizeof(*new_msgs) * (num + rd_num), GFP_KERNEL);
+	if (!new_msgs)
+		return -ENOMEM;
+
+	state = i2c_get_adapdata(adap);
+	p = wbuf;
+	bufend = wbuf + sizeof(wbuf);
+	for (i = 0, j = 0; i < num; i++, j++) {
+		new_msgs[j].addr = state->i2c_client->addr;
+		new_msgs[j].flags = msgs[i].flags;
+
+		if (msgs[i].flags & I2C_M_RD) {
+			new_msgs[j].flags &= ~I2C_M_RD;
+			if (p + 2 > bufend)
+				break;
+			p[0] = TC90522_I2C_THRU_REG;
+			p[1] = msgs[i].addr << 1 | 0x01;
+			new_msgs[j].buf = p;
+			new_msgs[j].len = 2;
+			p += 2;
+			j++;
+			new_msgs[j].addr = state->i2c_client->addr;
+			new_msgs[j].flags = msgs[i].flags;
+			new_msgs[j].buf = msgs[i].buf;
+			new_msgs[j].len = msgs[i].len;
+			continue;
+		}
+
+		if (p + msgs[i].len + 2 > bufend)
+			break;
+		p[0] = TC90522_I2C_THRU_REG;
+		p[1] = msgs[i].addr << 1;
+		memcpy(p + 2, msgs[i].buf, msgs[i].len);
+		new_msgs[j].buf = p;
+		new_msgs[j].len = msgs[i].len + 2;
+		p += new_msgs[j].len;
+	}
+
+	if (i < num)
+		ret = -ENOMEM;
+	else
+		ret = i2c_transfer(state->i2c_client->adapter, new_msgs, j);
+	if (ret >= 0 && ret < j)
+		ret = -EIO;
+	kfree(new_msgs);
+	return (ret == j) ? num : ret;
+}
+
+static u32 tc90522_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C;
+}
+
+static const struct i2c_algorithm tc90522_tuner_i2c_algo = {
+	.master_xfer   = &tc90522_master_xfer,
+	.functionality = &tc90522_functionality,
+};
+
+
+/*
+ * I2C driver functions
+ */
+
+static const struct dvb_frontend_ops tc90522_ops_sat = {
+	.delsys = { SYS_ISDBS },
+	.info = {
+		.name = "Toshiba TC90522 ISDB-S module",
+		.frequency_min =  950000,
+		.frequency_max = 2150000,
+		.caps = FE_CAN_INVERSION_AUTO | FE_CAN_FEC_AUTO |
+			FE_CAN_QAM_AUTO | FE_CAN_TRANSMISSION_MODE_AUTO |
+			FE_CAN_GUARD_INTERVAL_AUTO | FE_CAN_HIERARCHY_AUTO,
+	},
+
+	.init = tc90522_init,
+	.sleep = tc90522_sleep,
+	.set_frontend = tc90522_set_frontend,
+	.get_tune_settings = tc90522_get_tune_settings,
+
+	.get_frontend = tc90522s_get_frontend,
+	.read_status = tc90522s_read_status,
+};
+
+static const struct dvb_frontend_ops tc90522_ops_ter = {
+	.delsys = { SYS_ISDBT },
+	.info = {
+		.name = "Toshiba TC90522 ISDB-T module",
+		.frequency_min = 470000000,
+		.frequency_max = 770000000,
+		.frequency_stepsize = 142857,
+		.caps = FE_CAN_INVERSION_AUTO |
+			FE_CAN_FEC_1_2  | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4 |
+			FE_CAN_FEC_5_6  | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO |
+			FE_CAN_QPSK     | FE_CAN_QAM_16 | FE_CAN_QAM_64 |
+			FE_CAN_QAM_AUTO | FE_CAN_TRANSMISSION_MODE_AUTO |
+			FE_CAN_GUARD_INTERVAL_AUTO | FE_CAN_RECOVER |
+			FE_CAN_HIERARCHY_AUTO,
+	},
+
+	.init = tc90522_init,
+	.sleep = tc90522_sleep,
+	.set_frontend = tc90522_set_frontend,
+	.get_tune_settings = tc90522_get_tune_settings,
+
+	.get_frontend = tc90522t_get_frontend,
+	.read_status = tc90522t_read_status,
+};
+
+
+static int tc90522_probe(struct i2c_client *client,
+			 const struct i2c_device_id *id)
+{
+	struct tc90522_state *state;
+	struct tc90522_config *cfg;
+	const struct dvb_frontend_ops *ops;
+	struct i2c_adapter *adap;
+	int ret;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+	state->i2c_client = client;
+
+	cfg = client->dev.platform_data;
+	memcpy(&state->cfg, cfg, sizeof(state->cfg));
+	cfg->fe = state->cfg.fe = &state->fe;
+	ops =  id->driver_data == 0 ? &tc90522_ops_sat : &tc90522_ops_ter;
+	memcpy(&state->fe.ops, ops, sizeof(*ops));
+	state->fe.demodulator_priv = state;
+
+	adap = &state->tuner_i2c;
+	adap->owner = THIS_MODULE;
+	adap->algo = &tc90522_tuner_i2c_algo;
+	adap->dev.parent = &client->dev;
+	strlcpy(adap->name, "tc90522_sub", sizeof(adap->name));
+	i2c_set_adapdata(adap, state);
+	ret = i2c_add_adapter(adap);
+	if (ret < 0)
+		goto err;
+	cfg->tuner_i2c = state->cfg.tuner_i2c = adap;
+
+	i2c_set_clientdata(client, &state->cfg);
+	dev_info(&client->dev, "Toshiba TC90522 attached.\n");
+	return 0;
+
+err:
+	kfree(state);
+	return ret;
+}
+
+static int tc90522_remove(struct i2c_client *client)
+{
+	struct tc90522_state *state;
+
+	state = cfg_to_state(i2c_get_clientdata(client));
+	i2c_del_adapter(&state->tuner_i2c);
+	kfree(state);
+	return 0;
+}
+
+
+static const struct i2c_device_id tc90522_id[] = {
+	{ TC90522_I2C_DEV_SAT, 0 },
+	{ TC90522_I2C_DEV_TER, 1 },
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, tc90522_id);
+
+static struct i2c_driver tc90522_driver = {
+	.driver = {
+		.name	= "tc90522",
+	},
+	.probe		= tc90522_probe,
+	.remove		= tc90522_remove,
+	.id_table	= tc90522_id,
+};
+
+module_i2c_driver(tc90522_driver);
+
+MODULE_DESCRIPTION("Toshiba TC90522 frontend");
+MODULE_AUTHOR("Akihiro TSUKADA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/dvb-frontends/tc90522.h b/drivers/media/dvb-frontends/tc90522.h
new file mode 100644
index 000000000000..b1cbddfa6ee6
--- /dev/null
+++ b/drivers/media/dvb-frontends/tc90522.h
@@ -0,0 +1,42 @@
+/*
+ * Toshiba TC90522 Demodulator
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * The demod has 4 input (2xISDB-T and 2xISDB-S),
+ * and provides independent sub modules for each input.
+ * As the sub modules work in parallel and have the separate i2c addr's,
+ * this driver treats each sub module as one demod device.
+ */
+
+#ifndef TC90522_H
+#define TC90522_H
+
+#include <linux/i2c.h>
+#include "dvb_frontend.h"
+
+/* I2C device types */
+#define TC90522_I2C_DEV_SAT "tc90522sat"
+#define TC90522_I2C_DEV_TER "tc90522ter"
+
+struct tc90522_config {
+	/* [OUT] frontend returned by driver */
+	struct dvb_frontend *fe;
+
+	/* [OUT] tuner I2C adapter returned by driver */
+	struct i2c_adapter *tuner_i2c;
+};
+
+#endif /* TC90522_H */
diff --git a/drivers/media/dvb-frontends/tda10071.c b/drivers/media/dvb-frontends/tda10071.c
index 9619be5d4827..4a19b85995f1 100644
--- a/drivers/media/dvb-frontends/tda10071.c
+++ b/drivers/media/dvb-frontends/tda10071.c
@@ -1037,7 +1037,7 @@ static int tda10071_init(struct dvb_frontend *fe)
 			ret = -EFAULT;
 			goto error;
 		} else {
-			priv->warm = 1;
+			priv->warm = true;
 		}
 
 		cmd.args[0] = CMD_GET_FW_VERSION;
diff --git a/drivers/media/dvb-frontends/zl10039.c b/drivers/media/dvb-frontends/zl10039.c
index 91b6b2e9b792..ee09ec26c553 100644
--- a/drivers/media/dvb-frontends/zl10039.c
+++ b/drivers/media/dvb-frontends/zl10039.c
@@ -111,7 +111,7 @@ static int zl10039_write(struct zl10039_state *state,
 
 	if (1 + count > sizeof(buf)) {
 		printk(KERN_WARNING
-		       "%s: i2c wr reg=%04x: len=%zd is too big!\n",
+		       "%s: i2c wr reg=%04x: len=%zu is too big!\n",
 		       KBUILD_MODNAME, reg, count);
 		return -EINVAL;
 	}
diff --git a/drivers/media/firewire/firedtv-avc.c b/drivers/media/firewire/firedtv-avc.c
index d1a1a1324ef8..251a556112a9 100644
--- a/drivers/media/firewire/firedtv-avc.c
+++ b/drivers/media/firewire/firedtv-avc.c
@@ -1157,6 +1157,10 @@ int avc_ca_pmt(struct firedtv *fdtv, char *msg, int length)
 		if (pmt_cmd_id != 1 && pmt_cmd_id != 4)
 			dev_err(fdtv->device,
 				"invalid pmt_cmd_id %d\n", pmt_cmd_id);
+		if (program_info_length > sizeof(c->operand) - 4 - write_pos) {
+			ret = -EINVAL;
+			goto out;
+		}
 
 		memcpy(&c->operand[write_pos], &msg[read_pos],
 		       program_info_length);
@@ -1180,6 +1184,12 @@ int avc_ca_pmt(struct firedtv *fdtv, char *msg, int length)
 				dev_err(fdtv->device, "invalid pmt_cmd_id %d "
 					"at stream level\n", pmt_cmd_id);
 
+			if (es_info_length > sizeof(c->operand) - 4 -
+					     write_pos) {
+				ret = -EINVAL;
+				goto out;
+			}
+
 			memcpy(&c->operand[write_pos], &msg[read_pos],
 			       es_info_length);
 			read_pos += es_info_length;
diff --git a/drivers/media/i2c/adv7343_regs.h b/drivers/media/i2c/adv7343_regs.h
index 446606764346..2f04ce4b9118 100644
--- a/drivers/media/i2c/adv7343_regs.h
+++ b/drivers/media/i2c/adv7343_regs.h
@@ -13,7 +13,7 @@
  * GNU General Public License for more details.
  */
 
-#ifndef ADV7343_REG_H
+#ifndef ADV7343_REGS_H
 #define ADV7343_REGS_H
 
 struct adv7343_std_info {
diff --git a/drivers/media/i2c/adv7604.c b/drivers/media/i2c/adv7604.c
index d4fa213ba74a..47795ff71688 100644
--- a/drivers/media/i2c/adv7604.c
+++ b/drivers/media/i2c/adv7604.c
@@ -1593,7 +1593,7 @@ static int adv7604_query_dv_timings(struct v4l2_subdev *sd,
 			bt->height += hdmi_read16(sd, 0x0b, 0xfff);
 			bt->il_vfrontporch = hdmi_read16(sd, 0x2c, 0x1fff) / 2;
 			bt->il_vsync = hdmi_read16(sd, 0x30, 0x1fff) / 2;
-			bt->vbackporch = hdmi_read16(sd, 0x34, 0x1fff) / 2;
+			bt->il_vbackporch = hdmi_read16(sd, 0x34, 0x1fff) / 2;
 		}
 		adv7604_fill_optional_dv_timings_fields(sd, timings);
 	} else {
@@ -2325,7 +2325,7 @@ static int adv7604_log_status(struct v4l2_subdev *sd)
 	v4l2_info(sd, "HDCP keys read: %s%s\n",
 			(hdmi_read(sd, 0x04) & 0x20) ? "yes" : "no",
 			(hdmi_read(sd, 0x04) & 0x10) ? "ERROR" : "");
-	if (!is_hdmi(sd)) {
+	if (is_hdmi(sd)) {
 		bool audio_pll_locked = hdmi_read(sd, 0x04) & 0x01;
 		bool audio_sample_packet_detect = hdmi_read(sd, 0x18) & 0x01;
 		bool audio_mute = io_read(sd, 0x65) & 0x40;
diff --git a/drivers/media/i2c/adv7842.c b/drivers/media/i2c/adv7842.c
index 0d554919cdd5..48b628bc6714 100644
--- a/drivers/media/i2c/adv7842.c
+++ b/drivers/media/i2c/adv7842.c
@@ -1435,6 +1435,8 @@ static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
 
 	v4l2_dbg(1, debug, sd, "%s:\n", __func__);
 
+	memset(timings, 0, sizeof(struct v4l2_dv_timings));
+
 	/* SDP block */
 	if (state->mode == ADV7842_MODE_SDP)
 		return -ENODATA;
@@ -1483,7 +1485,7 @@ static int adv7842_query_dv_timings(struct v4l2_subdev *sd,
 					hdmi_read(sd, 0x2d)) / 2;
 			bt->il_vsync = ((hdmi_read(sd, 0x30) & 0x1f) * 256 +
 					hdmi_read(sd, 0x31)) / 2;
-			bt->vbackporch = ((hdmi_read(sd, 0x34) & 0x1f) * 256 +
+			bt->il_vbackporch = ((hdmi_read(sd, 0x34) & 0x1f) * 256 +
 					hdmi_read(sd, 0x35)) / 2;
 		}
 		adv7842_fill_optional_dv_timings_fields(sd, timings);
diff --git a/drivers/media/i2c/cx25840/cx25840-ir.c b/drivers/media/i2c/cx25840/cx25840-ir.c
index e6588ee5bdb0..4cf8f18bf097 100644
--- a/drivers/media/i2c/cx25840/cx25840-ir.c
+++ b/drivers/media/i2c/cx25840/cx25840-ir.c
@@ -224,7 +224,7 @@ static inline unsigned int lpf_count_to_us(unsigned int count)
 }
 
 /*
- * FIFO register pulse width count compuations
+ * FIFO register pulse width count computations
  */
 static u32 clock_divider_to_resolution(u16 divider)
 {
diff --git a/drivers/media/i2c/lm3560.c b/drivers/media/i2c/lm3560.c
index c23de593c17d..d9ece4b2d047 100644
--- a/drivers/media/i2c/lm3560.c
+++ b/drivers/media/i2c/lm3560.c
@@ -100,14 +100,14 @@ static int lm3560_enable_ctrl(struct lm3560_flash *flash,
 	int rval;
 
 	if (led_no == LM3560_LED0) {
-		if (on == true)
+		if (on)
 			rval = regmap_update_bits(flash->regmap,
 						  REG_ENABLE, 0x08, 0x08);
 		else
 			rval = regmap_update_bits(flash->regmap,
 						  REG_ENABLE, 0x08, 0x00);
 	} else {
-		if (on == true)
+		if (on)
 			rval = regmap_update_bits(flash->regmap,
 						  REG_ENABLE, 0x10, 0x10);
 		else
diff --git a/drivers/media/i2c/ov7670.c b/drivers/media/i2c/ov7670.c
index cdd7c1b7259b..dd3db2458a4f 100644
--- a/drivers/media/i2c/ov7670.c
+++ b/drivers/media/i2c/ov7670.c
@@ -19,6 +19,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-mediabus.h>
+#include <media/v4l2-image-sizes.h>
 #include <media/ov7670.h>
 
 MODULE_AUTHOR("Jonathan Corbet <corbet@lwn.net>");
@@ -30,19 +31,6 @@ module_param(debug, bool, 0644);
 MODULE_PARM_DESC(debug, "Debug level (0-1)");
 
 /*
- * Basic window sizes.  These probably belong somewhere more globally
- * useful.
- */
-#define VGA_WIDTH	640
-#define VGA_HEIGHT	480
-#define QVGA_WIDTH	320
-#define QVGA_HEIGHT	240
-#define CIF_WIDTH	352
-#define CIF_HEIGHT	288
-#define QCIF_WIDTH	176
-#define	QCIF_HEIGHT	144
-
-/*
  * The 7670 sits on i2c with ID 0x42
  */
 #define OV7670_I2C_ADDR 0x42
diff --git a/drivers/media/i2c/s5k5baf.c b/drivers/media/i2c/s5k5baf.c
index 564f05f2c9ef..0e461a6fd065 100644
--- a/drivers/media/i2c/s5k5baf.c
+++ b/drivers/media/i2c/s5k5baf.c
@@ -816,7 +816,7 @@ static void s5k5baf_hw_find_min_fiv(struct s5k5baf *state)
 				 "error setting frame interval: %d\n", err);
 			state->error = -EINVAL;
 		}
-	};
+	}
 	v4l2_err(&state->sd, "cannot find correct frame interval\n");
 	state->error = -ERANGE;
 }
diff --git a/drivers/media/i2c/saa6752hs.c b/drivers/media/i2c/saa6752hs.c
index 04e9e55018a5..4024ea6f1371 100644
--- a/drivers/media/i2c/saa6752hs.c
+++ b/drivers/media/i2c/saa6752hs.c
@@ -660,7 +660,7 @@ static const struct v4l2_subdev_ops saa6752hs_ops = {
 static int saa6752hs_probe(struct i2c_client *client,
 		const struct i2c_device_id *id)
 {
-	struct saa6752hs_state *h = kzalloc(sizeof(*h), GFP_KERNEL);
+	struct saa6752hs_state *h;
 	struct v4l2_subdev *sd;
 	struct v4l2_ctrl_handler *hdl;
 	u8 addr = 0x13;
@@ -668,6 +668,8 @@ static int saa6752hs_probe(struct i2c_client *client,
 
 	v4l_info(client, "chip found @ 0x%x (%s)\n",
 			client->addr << 1, client->adapter->name);
+
+	h = devm_kzalloc(&client->dev, sizeof(*h), GFP_KERNEL);
 	if (h == NULL)
 		return -ENOMEM;
 	sd = &h->sd;
@@ -752,7 +754,6 @@ static int saa6752hs_probe(struct i2c_client *client,
 		int err = hdl->error;
 
 		v4l2_ctrl_handler_free(hdl);
-		kfree(h);
 		return err;
 	}
 	v4l2_ctrl_cluster(3, &h->video_bitrate_mode);
@@ -767,7 +768,6 @@ static int saa6752hs_remove(struct i2c_client *client)
 
 	v4l2_device_unregister_subdev(sd);
 	v4l2_ctrl_handler_free(&to_state(sd)->hdl);
-	kfree(to_state(sd));
 	return 0;
 }
 
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c
index 1eaf975d3612..932ed9be9ff3 100644
--- a/drivers/media/i2c/smiapp/smiapp-core.c
+++ b/drivers/media/i2c/smiapp/smiapp-core.c
@@ -31,8 +31,9 @@
 #include <linux/device.h>
 #include <linux/gpio.h>
 #include <linux/module.h>
-#include <linux/slab.h>
 #include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/smiapp.h>
 #include <linux/v4l2-mediabus.h>
 #include <media/v4l2-device.h>
 
@@ -297,8 +298,9 @@ static int smiapp_pll_update(struct smiapp_sensor *sensor)
 	if (rval < 0)
 		return rval;
 
-	*sensor->pixel_rate_parray->p_cur.p_s64 = pll->vt_pix_clk_freq_hz;
-	*sensor->pixel_rate_csi->p_cur.p_s64 = pll->pixel_rate_csi;
+	__v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_parray,
+				 pll->vt_pix_clk_freq_hz);
+	__v4l2_ctrl_s_ctrl_int64(sensor->pixel_rate_csi, pll->pixel_rate_csi);
 
 	return 0;
 }
@@ -319,13 +321,7 @@ static void __smiapp_update_exposure_limits(struct smiapp_sensor *sensor)
 		+ sensor->vblank->val
 		- sensor->limits[SMIAPP_LIMIT_COARSE_INTEGRATION_TIME_MAX_MARGIN];
 
-	ctrl->maximum = max;
-	if (ctrl->default_value > max)
-		ctrl->default_value = max;
-	if (ctrl->val > max)
-		ctrl->val = max;
-	if (ctrl->cur.val > max)
-		ctrl->cur.val = max;
+	__v4l2_ctrl_modify_range(ctrl, ctrl->minimum, max, ctrl->step, max);
 }
 
 /*
@@ -404,6 +400,14 @@ static void smiapp_update_mbus_formats(struct smiapp_sensor *sensor)
 		pixel_order_str[pixel_order]);
 }
 
+static const char * const smiapp_test_patterns[] = {
+	"Disabled",
+	"Solid Colour",
+	"Eight Vertical Colour Bars",
+	"Colour Bars With Fade to Grey",
+	"Pseudorandom Sequence (PN9)",
+};
+
 static int smiapp_set_ctrl(struct v4l2_ctrl *ctrl)
 {
 	struct smiapp_sensor *sensor =
@@ -477,6 +481,39 @@ static int smiapp_set_ctrl(struct v4l2_ctrl *ctrl)
 
 		return smiapp_pll_update(sensor);
 
+	case V4L2_CID_TEST_PATTERN: {
+		unsigned int i;
+
+		for (i = 0; i < ARRAY_SIZE(sensor->test_data); i++)
+			v4l2_ctrl_activate(
+				sensor->test_data[i],
+				ctrl->val ==
+				V4L2_SMIAPP_TEST_PATTERN_MODE_SOLID_COLOUR);
+
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_TEST_PATTERN_MODE, ctrl->val);
+	}
+
+	case V4L2_CID_TEST_PATTERN_RED:
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_TEST_DATA_RED, ctrl->val);
+
+	case V4L2_CID_TEST_PATTERN_GREENR:
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_TEST_DATA_GREENR, ctrl->val);
+
+	case V4L2_CID_TEST_PATTERN_BLUE:
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_TEST_DATA_BLUE, ctrl->val);
+
+	case V4L2_CID_TEST_PATTERN_GREENB:
+		return smiapp_write(
+			sensor, SMIAPP_REG_U16_TEST_DATA_GREENB, ctrl->val);
+
+	case V4L2_CID_PIXEL_RATE:
+		/* For v4l2_ctrl_s_ctrl_int64() used internally. */
+		return 0;
+
 	default:
 		return -EINVAL;
 	}
@@ -489,10 +526,10 @@ static const struct v4l2_ctrl_ops smiapp_ctrl_ops = {
 static int smiapp_init_controls(struct smiapp_sensor *sensor)
 {
 	struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd);
-	unsigned int max;
+	unsigned int max, i;
 	int rval;
 
-	rval = v4l2_ctrl_handler_init(&sensor->pixel_array->ctrl_handler, 7);
+	rval = v4l2_ctrl_handler_init(&sensor->pixel_array->ctrl_handler, 12);
 	if (rval)
 		return rval;
 	sensor->pixel_array->ctrl_handler.lock = &sensor->mutex;
@@ -535,6 +572,20 @@ static int smiapp_init_controls(struct smiapp_sensor *sensor)
 		&sensor->pixel_array->ctrl_handler, &smiapp_ctrl_ops,
 		V4L2_CID_PIXEL_RATE, 1, INT_MAX, 1, 1);
 
+	v4l2_ctrl_new_std_menu_items(&sensor->pixel_array->ctrl_handler,
+				     &smiapp_ctrl_ops, V4L2_CID_TEST_PATTERN,
+				     ARRAY_SIZE(smiapp_test_patterns) - 1,
+				     0, 0, smiapp_test_patterns);
+
+	for (i = 0; i < ARRAY_SIZE(sensor->test_data); i++) {
+		int max_value = (1 << sensor->csi_format->width) - 1;
+		sensor->test_data[i] =
+			v4l2_ctrl_new_std(
+				&sensor->pixel_array->ctrl_handler,
+				&smiapp_ctrl_ops, V4L2_CID_TEST_PATTERN_RED + i,
+				0, max_value, 1, max_value);
+	}
+
 	if (sensor->pixel_array->ctrl_handler.error) {
 		dev_err(&client->dev,
 			"pixel array controls initialization failed (%d)\n",
@@ -782,36 +833,25 @@ static void smiapp_update_blanking(struct smiapp_sensor *sensor)
 {
 	struct v4l2_ctrl *vblank = sensor->vblank;
 	struct v4l2_ctrl *hblank = sensor->hblank;
+	int min, max;
 
-	vblank->minimum =
-		max_t(int,
-		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES],
-		      sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN] -
-		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height);
-	vblank->maximum =
-		sensor->limits[SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN] -
+	min = max_t(int,
+		    sensor->limits[SMIAPP_LIMIT_MIN_FRAME_BLANKING_LINES],
+		    sensor->limits[SMIAPP_LIMIT_MIN_FRAME_LENGTH_LINES_BIN] -
+		    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height);
+	max = sensor->limits[SMIAPP_LIMIT_MAX_FRAME_LENGTH_LINES_BIN] -
 		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].height;
 
-	vblank->val = clamp_t(int, vblank->val,
-			      vblank->minimum, vblank->maximum);
-	vblank->default_value = vblank->minimum;
-	vblank->val = vblank->val;
-	vblank->cur.val = vblank->val;
-
-	hblank->minimum =
-		max_t(int,
-		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN] -
-		      sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width,
-		      sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN]);
-	hblank->maximum =
-		sensor->limits[SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN] -
+	__v4l2_ctrl_modify_range(vblank, min, max, vblank->step, min);
+
+	min = max_t(int,
+		    sensor->limits[SMIAPP_LIMIT_MIN_LINE_LENGTH_PCK_BIN] -
+		    sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width,
+		    sensor->limits[SMIAPP_LIMIT_MIN_LINE_BLANKING_PCK_BIN]);
+	max = sensor->limits[SMIAPP_LIMIT_MAX_LINE_LENGTH_PCK_BIN] -
 		sensor->pixel_array->crop[SMIAPP_PA_PAD_SRC].width;
 
-	hblank->val = clamp_t(int, hblank->val,
-			      hblank->minimum, hblank->maximum);
-	hblank->default_value = hblank->minimum;
-	hblank->val = hblank->val;
-	hblank->cur.val = hblank->val;
+	__v4l2_ctrl_modify_range(hblank, min, max, hblank->step, min);
 
 	__smiapp_update_exposure_limits(sensor);
 }
@@ -1272,7 +1312,7 @@ static void smiapp_power_off(struct smiapp_sensor *sensor)
 		clk_disable_unprepare(sensor->ext_clk);
 	usleep_range(5000, 5000);
 	regulator_disable(sensor->vana);
-	sensor->streaming = 0;
+	sensor->streaming = false;
 }
 
 static int smiapp_set_power(struct v4l2_subdev *subdev, int on)
@@ -1282,19 +1322,12 @@ static int smiapp_set_power(struct v4l2_subdev *subdev, int on)
 
 	mutex_lock(&sensor->power_mutex);
 
-	/*
-	 * If the power count is modified from 0 to != 0 or from != 0
-	 * to 0, update the power state.
-	 */
-	if (!sensor->power_count == !on)
-		goto out;
-
-	if (on) {
+	if (on && !sensor->power_count) {
 		/* Power on and perform initialisation. */
 		ret = smiapp_power_on(sensor);
 		if (ret < 0)
 			goto out;
-	} else {
+	} else if (!on && sensor->power_count == 1) {
 		smiapp_power_off(sensor);
 	}
 
@@ -1469,13 +1502,13 @@ static int smiapp_set_stream(struct v4l2_subdev *subdev, int enable)
 		return 0;
 
 	if (enable) {
-		sensor->streaming = 1;
+		sensor->streaming = true;
 		rval = smiapp_start_streaming(sensor);
 		if (rval < 0)
-			sensor->streaming = 0;
+			sensor->streaming = false;
 	} else {
 		rval = smiapp_stop_streaming(sensor);
-		sensor->streaming = 0;
+		sensor->streaming = false;
 	}
 
 	return rval;
@@ -1671,17 +1704,34 @@ static int smiapp_set_format(struct v4l2_subdev *subdev,
 	if (fmt->pad == ssd->source_pad) {
 		u32 code = fmt->format.code;
 		int rval = __smiapp_get_format(subdev, fh, fmt);
+		bool range_changed = false;
+		unsigned int i;
 
 		if (!rval && subdev == &sensor->src->sd) {
 			const struct smiapp_csi_data_format *csi_format =
 				smiapp_validate_csi_data_format(sensor, code);
-			if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE)
+
+			if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
+				if (csi_format->width !=
+				    sensor->csi_format->width)
+					range_changed = true;
+
 				sensor->csi_format = csi_format;
+			}
+
 			fmt->format.code = csi_format->code;
 		}
 
 		mutex_unlock(&sensor->mutex);
-		return rval;
+		if (rval || !range_changed)
+			return rval;
+
+		for (i = 0; i < ARRAY_SIZE(sensor->test_data); i++)
+			v4l2_ctrl_modify_range(
+				sensor->test_data[i],
+				0, (1 << sensor->csi_format->width) - 1, 1, 0);
+
+		return 0;
 	}
 
 	/* Sink pad. Width and height are changeable here. */
@@ -2572,7 +2622,7 @@ static int smiapp_registered(struct v4l2_subdev *subdev)
 
 		this->sd.flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
 		this->sd.internal_ops = &smiapp_internal_ops;
-		this->sd.owner = NULL;
+		this->sd.owner = THIS_MODULE;
 		v4l2_set_subdevdata(&this->sd, client);
 
 		rval = media_entity_init(&this->sd.entity,
diff --git a/drivers/media/i2c/smiapp/smiapp.h b/drivers/media/i2c/smiapp/smiapp.h
index 7cc5aae662fd..874b49ffd88f 100644
--- a/drivers/media/i2c/smiapp/smiapp.h
+++ b/drivers/media/i2c/smiapp/smiapp.h
@@ -54,6 +54,8 @@
 	(1000 +	(SMIAPP_RESET_DELAY_CLOCKS * 1000	\
 		 + (clk) / 1000 - 1) / ((clk) / 1000))
 
+#define SMIAPP_COLOUR_COMPONENTS	4
+
 #include "smiapp-limits.h"
 
 struct smiapp_quirk;
@@ -241,6 +243,8 @@ struct smiapp_sensor {
 	/* src controls */
 	struct v4l2_ctrl *link_freq;
 	struct v4l2_ctrl *pixel_rate_csi;
+	/* test pattern colour components */
+	struct v4l2_ctrl *test_data[SMIAPP_COLOUR_COMPONENTS];
 };
 
 #define to_smiapp_subdev(_sd)				\
diff --git a/drivers/media/i2c/soc_camera/mt9t112.c b/drivers/media/i2c/soc_camera/mt9t112.c
index 46f431a13782..996d7b4007a5 100644
--- a/drivers/media/i2c/soc_camera/mt9t112.c
+++ b/drivers/media/i2c/soc_camera/mt9t112.c
@@ -29,6 +29,7 @@
 #include <media/soc_camera.h>
 #include <media/v4l2-clk.h>
 #include <media/v4l2-common.h>
+#include <media/v4l2-image-sizes.h>
 
 /* you can check PLL/clock info */
 /* #define EXT_CLOCK 24000000 */
@@ -42,9 +43,6 @@
 #define MAX_WIDTH   2048
 #define MAX_HEIGHT  1536
 
-#define VGA_WIDTH   640
-#define VGA_HEIGHT  480
-
 /*
  * macro of read/write
  */
diff --git a/drivers/media/i2c/soc_camera/ov772x.c b/drivers/media/i2c/soc_camera/ov772x.c
index 7f2b3c8926af..970a04e1e56e 100644
--- a/drivers/media/i2c/soc_camera/ov772x.c
+++ b/drivers/media/i2c/soc_camera/ov772x.c
@@ -29,6 +29,7 @@
 #include <media/v4l2-clk.h>
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-subdev.h>
+#include <media/v4l2-image-sizes.h>
 
 /*
  * register offset
@@ -360,10 +361,6 @@
 #define SCAL0_ACTRL     0x08 /* Auto scaling factor control */
 #define SCAL1_2_ACTRL   0x04 /* Auto scaling factor control */
 
-#define VGA_WIDTH		640
-#define VGA_HEIGHT		480
-#define QVGA_WIDTH		320
-#define QVGA_HEIGHT		240
 #define OV772X_MAX_WIDTH	VGA_WIDTH
 #define OV772X_MAX_HEIGHT	VGA_HEIGHT
 
diff --git a/drivers/media/i2c/soc_camera/ov9740.c b/drivers/media/i2c/soc_camera/ov9740.c
index ea76863dfdb4..ee9eb635d540 100644
--- a/drivers/media/i2c/soc_camera/ov9740.c
+++ b/drivers/media/i2c/soc_camera/ov9740.c
@@ -564,13 +564,13 @@ static int ov9740_set_res(struct i2c_client *client, u32 width, u32 height)
 	u32 y_start;
 	u32 x_end;
 	u32 y_end;
-	bool scaling = 0;
+	bool scaling = false;
 	u32 scale_input_x;
 	u32 scale_input_y;
 	int ret;
 
 	if ((width != OV9740_MAX_WIDTH) || (height != OV9740_MAX_HEIGHT))
-		scaling = 1;
+		scaling = true;
 
 	/*
 	 * Try to use as much of the sensor area as possible when supporting
diff --git a/drivers/media/i2c/tda7432.c b/drivers/media/i2c/tda7432.c
index 72af644fa051..cf93021a6500 100644
--- a/drivers/media/i2c/tda7432.c
+++ b/drivers/media/i2c/tda7432.c
@@ -293,7 +293,7 @@ static int tda7432_s_ctrl(struct v4l2_ctrl *ctrl)
 		if (t->mute->val) {
 			lf |= TDA7432_MUTE;
 			lr |= TDA7432_MUTE;
-			lf |= TDA7432_MUTE;
+			rf |= TDA7432_MUTE;
 			rr |= TDA7432_MUTE;
 		}
 		/* Mute & update balance*/
diff --git a/drivers/media/i2c/tvp7002.c b/drivers/media/i2c/tvp7002.c
index 11f2387e1dab..51bac762638b 100644
--- a/drivers/media/i2c/tvp7002.c
+++ b/drivers/media/i2c/tvp7002.c
@@ -775,25 +775,20 @@ static int tvp7002_enum_mbus_fmt(struct v4l2_subdev *sd, unsigned index,
 static int tvp7002_s_stream(struct v4l2_subdev *sd, int enable)
 {
 	struct tvp7002 *device = to_tvp7002(sd);
-	int error = 0;
+	int error;
 
 	if (device->streaming == enable)
 		return 0;
 
-	if (enable) {
-		/* Set output state on (low impedance means stream on) */
-		error = tvp7002_write(sd, TVP7002_MISC_CTL_2, 0x00);
-		device->streaming = enable;
-	} else {
-		/* Set output state off (high impedance means stream off) */
-		error = tvp7002_write(sd, TVP7002_MISC_CTL_2, 0x03);
-		if (error)
-			v4l2_dbg(1, debug, sd, "Unable to stop streaming\n");
-
-		device->streaming = enable;
+	/* low impedance: on, high impedance: off */
+	error = tvp7002_write(sd, TVP7002_MISC_CTL_2, enable ? 0x00 : 0x03);
+	if (error) {
+		v4l2_dbg(1, debug, sd, "Fail to set streaming\n");
+		return error;
 	}
 
-	return error;
+	device->streaming = enable;
+	return 0;
 }
 
 /*
diff --git a/drivers/media/i2c/vs6624.c b/drivers/media/i2c/vs6624.c
index 23f4f65fccd7..373f2df52492 100644
--- a/drivers/media/i2c/vs6624.c
+++ b/drivers/media/i2c/vs6624.c
@@ -30,22 +30,10 @@
 #include <media/v4l2-ctrls.h>
 #include <media/v4l2-device.h>
 #include <media/v4l2-mediabus.h>
+#include <media/v4l2-image-sizes.h>
 
 #include "vs6624_regs.h"
 
-#define VGA_WIDTH       640
-#define VGA_HEIGHT      480
-#define QVGA_WIDTH      320
-#define QVGA_HEIGHT     240
-#define QQVGA_WIDTH     160
-#define QQVGA_HEIGHT    120
-#define CIF_WIDTH       352
-#define CIF_HEIGHT      288
-#define QCIF_WIDTH      176
-#define QCIF_HEIGHT     144
-#define QQCIF_WIDTH     88
-#define QQCIF_HEIGHT    72
-
 #define MAX_FRAME_RATE  30
 
 struct vs6624 {
diff --git a/drivers/media/media-device.c b/drivers/media/media-device.c
index 73a432934bd8..7b39440192d6 100644
--- a/drivers/media/media-device.c
+++ b/drivers/media/media-device.c
@@ -103,10 +103,8 @@ static long media_device_enum_entities(struct media_device *mdev,
 		return -EINVAL;
 
 	u_ent.id = ent->id;
-	if (ent->name) {
-		strncpy(u_ent.name, ent->name, sizeof(u_ent.name));
-		u_ent.name[sizeof(u_ent.name) - 1] = '\0';
-	}
+	if (ent->name)
+		strlcpy(u_ent.name, ent->name, sizeof(u_ent.name));
 	u_ent.type = ent->type;
 	u_ent.revision = ent->revision;
 	u_ent.flags = ent->flags;
diff --git a/drivers/media/media-devnode.c b/drivers/media/media-devnode.c
index 7acd19c881de..ebf9626e5ae5 100644
--- a/drivers/media/media-devnode.c
+++ b/drivers/media/media-devnode.c
@@ -192,7 +192,6 @@ static int media_open(struct inode *inode, struct file *filp)
 static int media_release(struct inode *inode, struct file *filp)
 {
 	struct media_devnode *mdev = media_devnode_data(filp);
-	int ret = 0;
 
 	if (mdev->fops->release)
 		mdev->fops->release(filp);
@@ -201,7 +200,7 @@ static int media_release(struct inode *inode, struct file *filp)
 	   return value is ignored. */
 	put_device(&mdev->dev);
 	filp->private_data = NULL;
-	return ret;
+	return 0;
 }
 
 static const struct file_operations media_devnode_fops = {
diff --git a/drivers/media/parport/pms.c b/drivers/media/parport/pms.c
index 9bc105b3db1b..e6b497528cea 100644
--- a/drivers/media/parport/pms.c
+++ b/drivers/media/parport/pms.c
@@ -629,11 +629,15 @@ static int pms_capture(struct pms *dev, char __user *buf, int rgb555, int count)
 {
 	int y;
 	int dw = 2 * dev->width;
-	char tmp[dw + 32]; /* using a temp buffer is faster than direct  */
+	char *tmp; /* using a temp buffer is faster than direct  */
 	int cnt = 0;
 	int len = 0;
 	unsigned char r8 = 0x5;  /* value for reg8  */
 
+	tmp = kmalloc(dw + 32, GFP_KERNEL);
+	if (!tmp)
+		return 0;
+
 	if (rgb555)
 		r8 |= 0x20; /* else use untranslated rgb = 565 */
 	mvv_write(dev, 0x08, r8); /* capture rgb555/565, init DRAM, PC enable */
@@ -664,6 +668,7 @@ static int pms_capture(struct pms *dev, char __user *buf, int rgb555, int count)
 			len += dt;
 		}
 	}
+	kfree(tmp);
 	return len;
 }
 
diff --git a/drivers/media/pci/Kconfig b/drivers/media/pci/Kconfig
index 5c16c9c2203e..f8cec8e8cf82 100644
--- a/drivers/media/pci/Kconfig
+++ b/drivers/media/pci/Kconfig
@@ -20,6 +20,7 @@ source "drivers/media/pci/ivtv/Kconfig"
 source "drivers/media/pci/zoran/Kconfig"
 source "drivers/media/pci/saa7146/Kconfig"
 source "drivers/media/pci/solo6x10/Kconfig"
+source "drivers/media/pci/tw68/Kconfig"
 endif
 
 if MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT
@@ -41,6 +42,7 @@ source "drivers/media/pci/b2c2/Kconfig"
 source "drivers/media/pci/pluto2/Kconfig"
 source "drivers/media/pci/dm1105/Kconfig"
 source "drivers/media/pci/pt1/Kconfig"
+source "drivers/media/pci/pt3/Kconfig"
 source "drivers/media/pci/mantis/Kconfig"
 source "drivers/media/pci/ngene/Kconfig"
 source "drivers/media/pci/ddbridge/Kconfig"
diff --git a/drivers/media/pci/Makefile b/drivers/media/pci/Makefile
index e5b53fb569ef..a12926e4b51f 100644
--- a/drivers/media/pci/Makefile
+++ b/drivers/media/pci/Makefile
@@ -7,10 +7,10 @@ obj-y        +=	ttpci/		\
 		pluto2/		\
 		dm1105/		\
 		pt1/		\
+		pt3/		\
 		mantis/		\
 		ngene/		\
 		ddbridge/	\
-		b2c2/		\
 		saa7146/
 
 obj-$(CONFIG_VIDEO_IVTV) += ivtv/
@@ -22,6 +22,7 @@ obj-$(CONFIG_VIDEO_CX88) += cx88/
 obj-$(CONFIG_VIDEO_BT848) += bt8xx/
 obj-$(CONFIG_VIDEO_SAA7134) += saa7134/
 obj-$(CONFIG_VIDEO_SAA7164) += saa7164/
+obj-$(CONFIG_VIDEO_TW68) += tw68/
 obj-$(CONFIG_VIDEO_MEYE) += meye/
 obj-$(CONFIG_STA2X11_VIP) += sta2x11/
 obj-$(CONFIG_VIDEO_SOLO6X10) += solo6x10/
diff --git a/drivers/media/pci/bt8xx/bttv-driver.c b/drivers/media/pci/bt8xx/bttv-driver.c
index 970e542d3a51..4a8176c09fc9 100644
--- a/drivers/media/pci/bt8xx/bttv-driver.c
+++ b/drivers/media/pci/bt8xx/bttv-driver.c
@@ -1531,7 +1531,6 @@ bttv_switch_overlay(struct bttv *btv, struct bttv_fh *fh,
 {
 	struct bttv_buffer *old;
 	unsigned long flags;
-	int retval = 0;
 
 	dprintk("switch_overlay: enter [new=%p]\n", new);
 	if (new)
@@ -1551,7 +1550,7 @@ bttv_switch_overlay(struct bttv *btv, struct bttv_fh *fh,
 	if (NULL == new)
 		free_btres_lock(btv,fh,RESOURCE_OVERLAY);
 	dprintk("switch_overlay: done\n");
-	return retval;
+	return 0;
 }
 
 /* ----------------------------------------------------------------------- */
@@ -3856,7 +3855,7 @@ static irqreturn_t bttv_irq(int irq, void *dev_id)
 
 				btwrite(btread(BT848_INT_MASK) & (-1 ^ BT848_INT_GPINT),
 						BT848_INT_MASK);
-			};
+			}
 
 			bttv_print_irqbits(stat,astat);
 
diff --git a/drivers/media/pci/bt8xx/dst_ca.c b/drivers/media/pci/bt8xx/dst_ca.c
index 0e788fca992c..c22c4ae06844 100644
--- a/drivers/media/pci/bt8xx/dst_ca.c
+++ b/drivers/media/pci/bt8xx/dst_ca.c
@@ -674,11 +674,9 @@ static int dst_ca_release(struct inode *inode, struct file *file)
 
 static ssize_t dst_ca_read(struct file *file, char __user *buffer, size_t length, loff_t *offset)
 {
-	ssize_t bytes_read = 0;
-
 	dprintk(verbose, DST_CA_DEBUG, 1, " Device read.");
 
-	return bytes_read;
+	return 0;
 }
 
 static ssize_t dst_ca_write(struct file *file, const char __user *buffer, size_t length, loff_t *offset)
diff --git a/drivers/media/pci/cx18/cx18-alsa-pcm.c b/drivers/media/pci/cx18/cx18-alsa-pcm.c
index 180077c49123..ffb6acdc575f 100644
--- a/drivers/media/pci/cx18/cx18-alsa-pcm.c
+++ b/drivers/media/pci/cx18/cx18-alsa-pcm.c
@@ -80,7 +80,7 @@ void cx18_alsa_announce_pcm_data(struct snd_cx18_card *cxsc, u8 *pcm_data,
 	int period_elapsed = 0;
 	int length;
 
-	dprintk("cx18 alsa announce ptr=%p data=%p num_bytes=%zd\n", cxsc,
+	dprintk("cx18 alsa announce ptr=%p data=%p num_bytes=%zu\n", cxsc,
 		pcm_data, num_bytes);
 
 	substream = cxsc->capture_pcm_substream;
diff --git a/drivers/media/pci/cx18/cx18-driver.c b/drivers/media/pci/cx18/cx18-driver.c
index 716bdc57fac6..83f5074706f9 100644
--- a/drivers/media/pci/cx18/cx18-driver.c
+++ b/drivers/media/pci/cx18/cx18-driver.c
@@ -1091,6 +1091,7 @@ static int cx18_probe(struct pci_dev *pci_dev,
 		setup.addr = ADDR_UNSET;
 		setup.type = cx->options.tuner;
 		setup.mode_mask = T_ANALOG_TV;  /* matches TV tuners */
+		setup.config = NULL;
 		if (cx->options.radio > 0)
 			setup.mode_mask |= T_RADIO;
 		setup.tuner_callback = (setup.type == TUNER_XC2028) ?
diff --git a/drivers/media/pci/cx18/cx18-firmware.c b/drivers/media/pci/cx18/cx18-firmware.c
index a1c1cec05f98..c6c83445f8bf 100644
--- a/drivers/media/pci/cx18/cx18-firmware.c
+++ b/drivers/media/pci/cx18/cx18-firmware.c
@@ -130,7 +130,7 @@ static int load_cpu_fw_direct(const char *fn, u8 __iomem *mem, struct cx18 *cx)
 		}
 	}
 	if (!test_bit(CX18_F_I_LOADED_FW, &cx->i_flags))
-		CX18_INFO("loaded %s firmware (%zd bytes)\n", fn, fw->size);
+		CX18_INFO("loaded %s firmware (%zu bytes)\n", fn, fw->size);
 	size = fw->size;
 	release_firmware(fw);
 	cx18_setup_page(cx, SCB_OFFSET);
@@ -164,7 +164,7 @@ static int load_apu_fw_direct(const char *fn, u8 __iomem *dst, struct cx18 *cx,
 
 	apu_version = (vers[0] << 24) | (vers[4] << 16) | vers[32];
 	while (offset + sizeof(seghdr) < fw->size) {
-		const u32 *shptr = src + offset / 4;
+		const __le32 *shptr = (__force __le32 *)src + offset / 4;
 
 		seghdr.sync1 = le32_to_cpu(shptr[0]);
 		seghdr.sync2 = le32_to_cpu(shptr[1]);
@@ -202,7 +202,7 @@ static int load_apu_fw_direct(const char *fn, u8 __iomem *dst, struct cx18 *cx,
 		offset += seghdr.size;
 	}
 	if (!test_bit(CX18_F_I_LOADED_FW, &cx->i_flags))
-		CX18_INFO("loaded %s firmware V%08x (%zd bytes)\n",
+		CX18_INFO("loaded %s firmware V%08x (%zu bytes)\n",
 				fn, apu_version, fw->size);
 	size = fw->size;
 	release_firmware(fw);
diff --git a/drivers/media/pci/cx18/cx18-queue.c b/drivers/media/pci/cx18/cx18-queue.c
index 8884537bd62f..2a247d264b87 100644
--- a/drivers/media/pci/cx18/cx18-queue.c
+++ b/drivers/media/pci/cx18/cx18-queue.c
@@ -364,7 +364,7 @@ int cx18_stream_alloc(struct cx18_stream *s)
 					((char __iomem *)cx->scb->cpu_mdl));
 
 		CX18_ERR("Too many buffers, cannot fit in SCB area\n");
-		CX18_ERR("Max buffers = %zd\n",
+		CX18_ERR("Max buffers = %zu\n",
 			bufsz / sizeof(struct cx18_mdl_ent));
 		return -ENOMEM;
 	}
diff --git a/drivers/media/pci/cx23885/Kconfig b/drivers/media/pci/cx23885/Kconfig
index e12c006e6e2d..f613314b360b 100644
--- a/drivers/media/pci/cx23885/Kconfig
+++ b/drivers/media/pci/cx23885/Kconfig
@@ -3,12 +3,11 @@ config VIDEO_CX23885
 	depends on DVB_CORE && VIDEO_DEV && PCI && I2C && INPUT && SND
 	select SND_PCM
 	select I2C_ALGOBIT
-	select VIDEO_BTCX
 	select VIDEO_TUNER
 	select VIDEO_TVEEPROM
 	depends on RC_CORE
-	select VIDEOBUF_DVB
-	select VIDEOBUF_DMA_SG
+	select VIDEOBUF2_DVB
+	select VIDEOBUF2_DMA_SG
 	select VIDEO_CX25840
 	select VIDEO_CX2341X
 	select DVB_DIB7000P if MEDIA_SUBDRV_AUTOSELECT
@@ -32,12 +31,16 @@ config VIDEO_CX23885
 	select DVB_A8293 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_MB86A20S if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_SI2165 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_SI2168 if MEDIA_SUBDRV_AUTOSELECT
+	select DVB_M88DS3103 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_MT2063 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_MT2131 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_XC2028 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA8290 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_TDA18271 if MEDIA_SUBDRV_AUTOSELECT
 	select MEDIA_TUNER_XC5000 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_SI2157 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_M88TS2022 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_TUNER_DIB0070 if MEDIA_SUBDRV_AUTOSELECT
 	---help---
 	  This is a video4linux driver for Conexant 23885 based
diff --git a/drivers/media/pci/cx23885/Makefile b/drivers/media/pci/cx23885/Makefile
index 2a2cafb8cf5b..a2cbdcf15a8c 100644
--- a/drivers/media/pci/cx23885/Makefile
+++ b/drivers/media/pci/cx23885/Makefile
@@ -8,7 +8,6 @@ obj-$(CONFIG_VIDEO_CX23885) += cx23885.o
 obj-$(CONFIG_MEDIA_ALTERA_CI) += altera-ci.o
 
 ccflags-y += -Idrivers/media/i2c
-ccflags-y += -Idrivers/media/common
 ccflags-y += -Idrivers/media/tuners
 ccflags-y += -Idrivers/media/dvb-core
 ccflags-y += -Idrivers/media/dvb-frontends
diff --git a/drivers/media/pci/cx23885/altera-ci.c b/drivers/media/pci/cx23885/altera-ci.c
index 2926f7fadccd..2bbbf545b042 100644
--- a/drivers/media/pci/cx23885/altera-ci.c
+++ b/drivers/media/pci/cx23885/altera-ci.c
@@ -16,10 +16,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 /*
@@ -52,8 +48,8 @@
  * |  DATA7|  DATA6|  DATA5|  DATA4|  DATA3|  DATA2|  DATA1|  DATA0|
  * +-------+-------+-------+-------+-------+-------+-------+-------+
  */
-#include <media/videobuf-dma-sg.h>
-#include <media/videobuf-dvb.h>
+#include <dvb_demux.h>
+#include <dvb_frontend.h>
 #include "altera-ci.h"
 #include "dvb_ca_en50221.h"
 
diff --git a/drivers/media/pci/cx23885/altera-ci.h b/drivers/media/pci/cx23885/altera-ci.h
index 4998c96caebe..5028f0cf83f4 100644
--- a/drivers/media/pci/cx23885/altera-ci.h
+++ b/drivers/media/pci/cx23885/altera-ci.h
@@ -16,10 +16,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #ifndef __ALTERA_CI_H
 #define __ALTERA_CI_H
diff --git a/drivers/media/pci/cx23885/cimax2.c b/drivers/media/pci/cx23885/cimax2.c
index 16fa7ea4d4aa..631e4f24aea6 100644
--- a/drivers/media/pci/cx23885/cimax2.c
+++ b/drivers/media/pci/cx23885/cimax2.c
@@ -17,10 +17,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include "cx23885.h"
diff --git a/drivers/media/pci/cx23885/cimax2.h b/drivers/media/pci/cx23885/cimax2.h
index 518744a4c8a5..565e958f6f8d 100644
--- a/drivers/media/pci/cx23885/cimax2.h
+++ b/drivers/media/pci/cx23885/cimax2.h
@@ -17,10 +17,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #ifndef CIMAX2_H
diff --git a/drivers/media/pci/cx23885/cx23885-417.c b/drivers/media/pci/cx23885/cx23885-417.c
index bf89fc88692e..3948db386fb5 100644
--- a/drivers/media/pci/cx23885/cx23885-417.c
+++ b/drivers/media/pci/cx23885/cx23885-417.c
@@ -18,10 +18,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/module.h>
@@ -865,6 +861,11 @@ static int cx23885_api_cmd(struct cx23885_dev *dev,
 	return err;
 }
 
+static int cx23885_api_func(void *priv, u32 cmd, int in, int out, u32 data[CX2341X_MBOX_MAX_DATA])
+{
+	return cx23885_mbox_func(priv, cmd, in, out, data);
+}
+
 static int cx23885_find_mailbox(struct cx23885_dev *dev)
 {
 	u32 signature[4] = {
@@ -941,7 +942,7 @@ static int cx23885_load_firmware(struct cx23885_dev *dev)
 
 	if (firmware->size != CX23885_FIRM_IMAGE_SIZE) {
 		printk(KERN_ERR "ERROR: Firmware size mismatch "
-			"(have %zd, expected %d)\n",
+			"(have %zu, expected %d)\n",
 			firmware->size, CX23885_FIRM_IMAGE_SIZE);
 		release_firmware(firmware);
 		return -1;
@@ -1033,12 +1034,12 @@ static void cx23885_codec_settings(struct cx23885_dev *dev)
 	cx23885_api_cmd(dev, CX2341X_ENC_SET_FRAME_SIZE, 2, 0,
 				dev->ts1.height, dev->ts1.width);
 
-	dev->mpeg_params.width = dev->ts1.width;
-	dev->mpeg_params.height = dev->ts1.height;
-	dev->mpeg_params.is_50hz =
+	dev->cxhdl.width = dev->ts1.width;
+	dev->cxhdl.height = dev->ts1.height;
+	dev->cxhdl.is_50hz =
 		(dev->encodernorm.id & V4L2_STD_625_50) != 0;
 
-	cx2341x_update(dev, cx23885_mbox_func, NULL, &dev->mpeg_params);
+	cx2341x_handler_setup(&dev->cxhdl);
 
 	cx23885_api_cmd(dev, CX2341X_ENC_MISC, 2, 0, 3, 1);
 	cx23885_api_cmd(dev, CX2341X_ENC_MISC, 2, 0, 4, 1);
@@ -1137,85 +1138,107 @@ static int cx23885_initialize_codec(struct cx23885_dev *dev, int startencoder)
 
 /* ------------------------------------------------------------------ */
 
-static int bb_buf_setup(struct videobuf_queue *q,
-	unsigned int *count, unsigned int *size)
+static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct cx23885_fh *fh = q->priv_data;
-
-	fh->dev->ts1.ts_packet_size  = mpeglinesize;
-	fh->dev->ts1.ts_packet_count = mpeglines;
-
-	*size = fh->dev->ts1.ts_packet_size * fh->dev->ts1.ts_packet_count;
-	*count = mpegbufs;
+	struct cx23885_dev *dev = q->drv_priv;
 
+	dev->ts1.ts_packet_size  = mpeglinesize;
+	dev->ts1.ts_packet_count = mpeglines;
+	*num_planes = 1;
+	sizes[0] = mpeglinesize * mpeglines;
+	*num_buffers = mpegbufs;
 	return 0;
 }
 
-static int bb_buf_prepare(struct videobuf_queue *q,
-	struct videobuf_buffer *vb, enum v4l2_field field)
+static int buffer_prepare(struct vb2_buffer *vb)
 {
-	struct cx23885_fh *fh = q->priv_data;
-	return cx23885_buf_prepare(q, &fh->dev->ts1,
-		(struct cx23885_buffer *)vb,
-		field);
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer *buf =
+		container_of(vb, struct cx23885_buffer, vb);
+
+	return cx23885_buf_prepare(buf, &dev->ts1);
 }
 
-static void bb_buf_queue(struct videobuf_queue *q,
-	struct videobuf_buffer *vb)
+static void buffer_finish(struct vb2_buffer *vb)
 {
-	struct cx23885_fh *fh = q->priv_data;
-	cx23885_buf_queue(&fh->dev->ts1, (struct cx23885_buffer *)vb);
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer *buf = container_of(vb,
+		struct cx23885_buffer, vb);
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+
+	cx23885_free_buffer(dev, buf);
+
+	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
-static void bb_buf_release(struct videobuf_queue *q,
-	struct videobuf_buffer *vb)
+static void buffer_queue(struct vb2_buffer *vb)
 {
-	cx23885_free_buffer(q, (struct cx23885_buffer *)vb);
-}
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer   *buf = container_of(vb,
+		struct cx23885_buffer, vb);
 
-static struct videobuf_queue_ops cx23885_qops = {
-	.buf_setup    = bb_buf_setup,
-	.buf_prepare  = bb_buf_prepare,
-	.buf_queue    = bb_buf_queue,
-	.buf_release  = bb_buf_release,
-};
+	cx23885_buf_queue(&dev->ts1, buf);
+}
 
-/* ------------------------------------------------------------------ */
+static int cx23885_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct cx23885_dev *dev = q->drv_priv;
+	struct cx23885_dmaqueue *dmaq = &dev->ts1.mpegq;
+	unsigned long flags;
+	int ret;
+
+	ret = cx23885_initialize_codec(dev, 1);
+	if (ret == 0) {
+		struct cx23885_buffer *buf = list_entry(dmaq->active.next,
+			struct cx23885_buffer, queue);
+
+		cx23885_start_dma(&dev->ts1, dmaq, buf);
+		return 0;
+	}
+	spin_lock_irqsave(&dev->slock, flags);
+	while (!list_empty(&dmaq->active)) {
+		struct cx23885_buffer *buf = list_entry(dmaq->active.next,
+			struct cx23885_buffer, queue);
 
-static const u32 *ctrl_classes[] = {
-	cx2341x_mpeg_ctrls,
-	NULL
-};
+		list_del(&buf->queue);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+	}
+	spin_unlock_irqrestore(&dev->slock, flags);
+	return ret;
+}
 
-static int cx23885_queryctrl(struct cx23885_dev *dev,
-	struct v4l2_queryctrl *qctrl)
+static void cx23885_stop_streaming(struct vb2_queue *q)
 {
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (qctrl->id == 0)
-		return -EINVAL;
+	struct cx23885_dev *dev = q->drv_priv;
 
-	/* MPEG V4L2 controls */
-	if (cx2341x_ctrl_query(&dev->mpeg_params, qctrl))
-		qctrl->flags |= V4L2_CTRL_FLAG_DISABLED;
+	/* stop mpeg capture */
+	cx23885_api_cmd(dev, CX2341X_ENC_STOP_CAPTURE, 3, 0,
+			CX23885_END_NOW, CX23885_MPEG_CAPTURE,
+			CX23885_RAW_BITS_NONE);
 
-	return 0;
+	msleep(500);
+	cx23885_417_check_encoder(dev);
+	cx23885_cancel_buffers(&dev->ts1);
 }
 
-static int cx23885_querymenu(struct cx23885_dev *dev,
-	struct v4l2_querymenu *qmenu)
-{
-	struct v4l2_queryctrl qctrl;
+static struct vb2_ops cx23885_qops = {
+	.queue_setup    = queue_setup,
+	.buf_prepare  = buffer_prepare,
+	.buf_finish = buffer_finish,
+	.buf_queue    = buffer_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = cx23885_start_streaming,
+	.stop_streaming = cx23885_stop_streaming,
+};
 
-	qctrl.id = qmenu->id;
-	cx23885_queryctrl(dev, &qctrl);
-	return v4l2_ctrl_query_menu(qmenu, &qctrl,
-		cx2341x_ctrl_get_menu(&dev->mpeg_params, qmenu->id));
-}
+/* ------------------------------------------------------------------ */
 
 static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	*id = dev->tvnorm;
 	return 0;
@@ -1223,29 +1246,26 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 
 static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id id)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	unsigned int i;
+	int ret;
 
 	for (i = 0; i < ARRAY_SIZE(cx23885_tvnorms); i++)
 		if (id & cx23885_tvnorms[i].id)
 			break;
 	if (i == ARRAY_SIZE(cx23885_tvnorms))
 		return -EINVAL;
-	dev->encodernorm = cx23885_tvnorms[i];
 
-	/* Have the drier core notify the subdevices */
-	mutex_lock(&dev->lock);
-	cx23885_set_tvnorm(dev, id);
-	mutex_unlock(&dev->lock);
-
-	return 0;
+	ret = cx23885_set_tvnorm(dev, id);
+	if (!ret)
+		dev->encodernorm = cx23885_tvnorms[i];
+	return ret;
 }
 
 static int vidioc_enum_input(struct file *file, void *priv,
 	struct v4l2_input *i)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	dprintk(1, "%s()\n", __func__);
 	return cx23885_enum_input(dev, i);
 }
@@ -1263,8 +1283,7 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 static int vidioc_g_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *t)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (dev->tuner_type == TUNER_ABSENT)
 		return -EINVAL;
@@ -1281,8 +1300,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 static int vidioc_s_tuner(struct file *file, void *priv,
 				const struct v4l2_tuner *t)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (dev->tuner_type == TUNER_ABSENT)
 		return -EINVAL;
@@ -1296,8 +1314,7 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 static int vidioc_g_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (dev->tuner_type == TUNER_ABSENT)
 		return -EINVAL;
@@ -1315,27 +1332,10 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 	return cx23885_set_frequency(file, priv, f);
 }
 
-static int vidioc_g_ctrl(struct file *file, void *priv,
-	struct v4l2_control *ctl)
-{
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
-
-	return cx23885_get_control(dev, ctl);
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-	struct v4l2_control *ctl)
-{
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
-
-	return cx23885_set_control(dev, ctl);
-}
-
 static int vidioc_querycap(struct file *file, void  *priv,
 				struct v4l2_capability *cap)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	struct cx23885_tsport  *tsport = &dev->ts1;
 
 	strlcpy(cap->driver, dev->name, sizeof(cap->driver));
@@ -1368,8 +1368,7 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
@@ -1378,285 +1377,63 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	f->fmt.pix.colorspace   = 0;
 	f->fmt.pix.width        = dev->ts1.width;
 	f->fmt.pix.height       = dev->ts1.height;
-	f->fmt.pix.field        = fh->mpegq.field;
-	dprintk(1, "VIDIOC_G_FMT: w: %d, h: %d, f: %d\n",
-		dev->ts1.width, dev->ts1.height, fh->mpegq.field);
+	f->fmt.pix.field        = V4L2_FIELD_INTERLACED;
+	dprintk(1, "VIDIOC_G_FMT: w: %d, h: %d\n",
+		dev->ts1.width, dev->ts1.height);
 	return 0;
 }
 
 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
 	f->fmt.pix.sizeimage    =
 		dev->ts1.ts_packet_size * dev->ts1.ts_packet_count;
 	f->fmt.pix.colorspace   = 0;
-	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d, f: %d\n",
-		dev->ts1.width, dev->ts1.height, fh->mpegq.field);
+	f->fmt.pix.field        = V4L2_FIELD_INTERLACED;
+	dprintk(1, "VIDIOC_TRY_FMT: w: %d, h: %d\n",
+		dev->ts1.width, dev->ts1.height);
 	return 0;
 }
 
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *f)
 {
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	f->fmt.pix.pixelformat  = V4L2_PIX_FMT_MPEG;
 	f->fmt.pix.bytesperline = 0;
 	f->fmt.pix.sizeimage    =
 		dev->ts1.ts_packet_size * dev->ts1.ts_packet_count;
 	f->fmt.pix.colorspace   = 0;
+	f->fmt.pix.field        = V4L2_FIELD_INTERLACED;
 	dprintk(1, "VIDIOC_S_FMT: w: %d, h: %d, f: %d\n",
 		f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.field);
 	return 0;
 }
 
-static int vidioc_reqbufs(struct file *file, void *priv,
-				struct v4l2_requestbuffers *p)
-{
-	struct cx23885_fh  *fh  = file->private_data;
-
-	return videobuf_reqbufs(&fh->mpegq, p);
-}
-
-static int vidioc_querybuf(struct file *file, void *priv,
-				struct v4l2_buffer *p)
-{
-	struct cx23885_fh  *fh  = file->private_data;
-
-	return videobuf_querybuf(&fh->mpegq, p);
-}
-
-static int vidioc_qbuf(struct file *file, void *priv,
-				struct v4l2_buffer *p)
-{
-	struct cx23885_fh  *fh  = file->private_data;
-
-	return videobuf_qbuf(&fh->mpegq, p);
-}
-
-static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
-{
-	struct cx23885_fh  *fh  = priv;
-
-	return videobuf_dqbuf(&fh->mpegq, b, file->f_flags & O_NONBLOCK);
-}
-
-
-static int vidioc_streamon(struct file *file, void *priv,
-				enum v4l2_buf_type i)
-{
-	struct cx23885_fh  *fh  = file->private_data;
-
-	return videobuf_streamon(&fh->mpegq);
-}
-
-static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct cx23885_fh  *fh  = file->private_data;
-
-	return videobuf_streamoff(&fh->mpegq);
-}
-
-static int vidioc_g_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx23885_fh  *fh  = priv;
-	struct cx23885_dev *dev = fh->dev;
-
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-	return cx2341x_ext_ctrls(&dev->mpeg_params, 0, f, VIDIOC_G_EXT_CTRLS);
-}
-
-static int vidioc_s_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx23885_fh  *fh  = priv;
-	struct cx23885_dev *dev = fh->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-
-	p = dev->mpeg_params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_S_EXT_CTRLS);
-
-	if (err == 0) {
-		err = cx2341x_update(dev, cx23885_mbox_func,
-			&dev->mpeg_params, &p);
-		dev->mpeg_params = p;
-	}
-	return err;
-}
-
-static int vidioc_try_ext_ctrls(struct file *file, void *priv,
-				struct v4l2_ext_controls *f)
-{
-	struct cx23885_fh  *fh  = priv;
-	struct cx23885_dev *dev = fh->dev;
-	struct cx2341x_mpeg_params p;
-	int err;
-
-	if (f->ctrl_class != V4L2_CTRL_CLASS_MPEG)
-		return -EINVAL;
-
-	p = dev->mpeg_params;
-	err = cx2341x_ext_ctrls(&p, 0, f, VIDIOC_TRY_EXT_CTRLS);
-	return err;
-}
-
 static int vidioc_log_status(struct file *file, void *priv)
 {
-	struct cx23885_fh  *fh  = priv;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	char name[32 + 2];
 
 	snprintf(name, sizeof(name), "%s/2", dev->name);
-	printk(KERN_INFO
-		"%s/2: ============  START LOG STATUS  ============\n",
-	       dev->name);
 	call_all(dev, core, log_status);
-	cx2341x_log_status(&dev->mpeg_params, name);
-	printk(KERN_INFO
-		"%s/2: =============  END LOG STATUS  =============\n",
-	       dev->name);
+	v4l2_ctrl_handler_log_status(&dev->cxhdl.hdl, name);
 	return 0;
 }
 
-static int vidioc_querymenu(struct file *file, void *priv,
-				struct v4l2_querymenu *a)
-{
-	struct cx23885_fh  *fh  = priv;
-	struct cx23885_dev *dev = fh->dev;
-
-	return cx23885_querymenu(dev, a);
-}
-
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *c)
-{
-	struct cx23885_fh  *fh  = priv;
-	struct cx23885_dev *dev = fh->dev;
-
-	return cx23885_queryctrl(dev, c);
-}
-
-static int mpeg_open(struct file *file)
-{
-	struct cx23885_dev *dev = video_drvdata(file);
-	struct cx23885_fh *fh;
-
-	dprintk(2, "%s()\n", __func__);
-
-	/* allocate + initialize per filehandle data */
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (!fh)
-		return -ENOMEM;
-
-	file->private_data = fh;
-	fh->dev      = dev;
-
-	videobuf_queue_sg_init(&fh->mpegq, &cx23885_qops,
-			    &dev->pci->dev, &dev->ts1.slock,
-			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			    V4L2_FIELD_INTERLACED,
-			    sizeof(struct cx23885_buffer),
-			    fh, NULL);
-	return 0;
-}
-
-static int mpeg_release(struct file *file)
-{
-	struct cx23885_fh  *fh  = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
-
-	dprintk(2, "%s()\n", __func__);
-
-	/* FIXME: Review this crap */
-	/* Shut device down on last close */
-	if (atomic_cmpxchg(&fh->v4l_reading, 1, 0) == 1) {
-		if (atomic_dec_return(&dev->v4l_reader_count) == 0) {
-			/* stop mpeg capture */
-			cx23885_api_cmd(fh->dev, CX2341X_ENC_STOP_CAPTURE, 3, 0,
-				CX23885_END_NOW, CX23885_MPEG_CAPTURE,
-				CX23885_RAW_BITS_NONE);
-
-			msleep(500);
-			cx23885_417_check_encoder(dev);
-
-			cx23885_cancel_buffers(&fh->dev->ts1);
-		}
-	}
-
-	if (fh->mpegq.streaming)
-		videobuf_streamoff(&fh->mpegq);
-	if (fh->mpegq.reading)
-		videobuf_read_stop(&fh->mpegq);
-
-	videobuf_mmap_free(&fh->mpegq);
-	file->private_data = NULL;
-	kfree(fh);
-
-	return 0;
-}
-
-static ssize_t mpeg_read(struct file *file, char __user *data,
-	size_t count, loff_t *ppos)
-{
-	struct cx23885_fh *fh = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
-
-	dprintk(2, "%s()\n", __func__);
-
-	/* Deal w/ A/V decoder * and mpeg encoder sync issues. */
-	/* Start mpeg encoder on first read. */
-	if (atomic_cmpxchg(&fh->v4l_reading, 0, 1) == 0) {
-		if (atomic_inc_return(&dev->v4l_reader_count) == 1) {
-			if (cx23885_initialize_codec(dev, 1) < 0)
-				return -EINVAL;
-		}
-	}
-
-	return videobuf_read_stream(&fh->mpegq, data, count, ppos, 0,
-				    file->f_flags & O_NONBLOCK);
-}
-
-static unsigned int mpeg_poll(struct file *file,
-	struct poll_table_struct *wait)
-{
-	struct cx23885_fh *fh = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
-
-	dprintk(2, "%s\n", __func__);
-
-	return videobuf_poll_stream(file, &fh->mpegq, wait);
-}
-
-static int mpeg_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct cx23885_fh *fh = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
-
-	dprintk(2, "%s()\n", __func__);
-
-	return videobuf_mmap_mapper(&fh->mpegq, vma);
-}
-
 static struct v4l2_file_operations mpeg_fops = {
 	.owner	       = THIS_MODULE,
-	.open	       = mpeg_open,
-	.release       = mpeg_release,
-	.read	       = mpeg_read,
-	.poll          = mpeg_poll,
-	.mmap	       = mpeg_mmap,
-	.ioctl	       = video_ioctl2,
+	.open           = v4l2_fh_open,
+	.release        = vb2_fop_release,
+	.read           = vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap,
 };
 
 static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
@@ -1669,25 +1446,19 @@ static const struct v4l2_ioctl_ops mpeg_ioctl_ops = {
 	.vidioc_s_tuner		 = vidioc_s_tuner,
 	.vidioc_g_frequency	 = vidioc_g_frequency,
 	.vidioc_s_frequency	 = vidioc_s_frequency,
-	.vidioc_s_ctrl		 = vidioc_s_ctrl,
-	.vidioc_g_ctrl		 = vidioc_g_ctrl,
 	.vidioc_querycap	 = vidioc_querycap,
 	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_g_fmt_vid_cap	 = vidioc_g_fmt_vid_cap,
 	.vidioc_try_fmt_vid_cap	 = vidioc_try_fmt_vid_cap,
 	.vidioc_s_fmt_vid_cap	 = vidioc_s_fmt_vid_cap,
-	.vidioc_reqbufs		 = vidioc_reqbufs,
-	.vidioc_querybuf	 = vidioc_querybuf,
-	.vidioc_qbuf		 = vidioc_qbuf,
-	.vidioc_dqbuf		 = vidioc_dqbuf,
-	.vidioc_streamon	 = vidioc_streamon,
-	.vidioc_streamoff	 = vidioc_streamoff,
-	.vidioc_g_ext_ctrls	 = vidioc_g_ext_ctrls,
-	.vidioc_s_ext_ctrls	 = vidioc_s_ext_ctrls,
-	.vidioc_try_ext_ctrls	 = vidioc_try_ext_ctrls,
+	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
+	.vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf      = vb2_ioctl_querybuf,
+	.vidioc_qbuf          = vb2_ioctl_qbuf,
+	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
+	.vidioc_streamon      = vb2_ioctl_streamon,
+	.vidioc_streamoff     = vb2_ioctl_streamoff,
 	.vidioc_log_status	 = vidioc_log_status,
-	.vidioc_querymenu	 = vidioc_querymenu,
-	.vidioc_queryctrl	 = vidioc_queryctrl,
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	.vidioc_g_chip_info	 = cx23885_g_chip_info,
 	.vidioc_g_register	 = cx23885_g_register,
@@ -1711,6 +1482,7 @@ void cx23885_417_unregister(struct cx23885_dev *dev)
 			video_unregister_device(dev->v4l_device);
 		else
 			video_device_release(dev->v4l_device);
+		v4l2_ctrl_handler_free(&dev->cxhdl.hdl);
 		dev->v4l_device = NULL;
 	}
 }
@@ -1742,6 +1514,7 @@ int cx23885_417_register(struct cx23885_dev *dev)
 	/* FIXME: Port1 hardcoded here */
 	int err = -ENODEV;
 	struct cx23885_tsport *tsport = &dev->ts1;
+	struct vb2_queue *q;
 
 	dprintk(1, "%s()\n", __func__);
 
@@ -1757,14 +1530,36 @@ int cx23885_417_register(struct cx23885_dev *dev)
 		tsport->height = 576;
 
 	tsport->width = 720;
-	cx2341x_fill_defaults(&dev->mpeg_params);
-
-	dev->mpeg_params.port = CX2341X_PORT_SERIAL;
+	dev->cxhdl.port = CX2341X_PORT_SERIAL;
+	err = cx2341x_handler_init(&dev->cxhdl, 50);
+	if (err)
+		return err;
+	dev->cxhdl.priv = dev;
+	dev->cxhdl.func = cx23885_api_func;
+	cx2341x_handler_set_50hz(&dev->cxhdl, tsport->height == 576);
+	v4l2_ctrl_add_handler(&dev->ctrl_handler, &dev->cxhdl.hdl, NULL);
 
 	/* Allocate and initialize V4L video device */
 	dev->v4l_device = cx23885_video_dev_alloc(tsport,
 		dev->pci, &cx23885_mpeg_template, "mpeg");
+	q = &dev->vb2_mpegq;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+	q->gfp_flags = GFP_DMA32;
+	q->min_buffers_needed = 2;
+	q->drv_priv = dev;
+	q->buf_struct_size = sizeof(struct cx23885_buffer);
+	q->ops = &cx23885_qops;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &dev->lock;
+
+	err = vb2_queue_init(q);
+	if (err < 0)
+		return err;
 	video_set_drvdata(dev->v4l_device, dev);
+	dev->v4l_device->lock = &dev->lock;
+	dev->v4l_device->queue = q;
 	err = video_register_device(dev->v4l_device,
 		VFL_TYPE_GRABBER, -1);
 	if (err < 0) {
diff --git a/drivers/media/pci/cx23885/cx23885-alsa.c b/drivers/media/pci/cx23885/cx23885-alsa.c
index 554798dcedd0..ae7c2e89ad1c 100644
--- a/drivers/media/pci/cx23885/cx23885-alsa.c
+++ b/drivers/media/pci/cx23885/cx23885-alsa.c
@@ -15,10 +15,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/module.h>
@@ -84,6 +80,82 @@ MODULE_PARM_DESC(audio_debug, "enable debug messages [analog audio]");
 #define AUD_INT_MCHG_IRQ        (1 << 21)
 #define GP_COUNT_CONTROL_RESET	0x3
 
+static int cx23885_alsa_dma_init(struct cx23885_audio_dev *chip, int nr_pages)
+{
+	struct cx23885_audio_buffer *buf = chip->buf;
+	struct page *pg;
+	int i;
+
+	buf->vaddr = vmalloc_32(nr_pages << PAGE_SHIFT);
+	if (NULL == buf->vaddr) {
+		dprintk(1, "vmalloc_32(%d pages) failed\n", nr_pages);
+		return -ENOMEM;
+	}
+
+	dprintk(1, "vmalloc is at addr 0x%08lx, size=%d\n",
+				(unsigned long)buf->vaddr,
+				nr_pages << PAGE_SHIFT);
+
+	memset(buf->vaddr, 0, nr_pages << PAGE_SHIFT);
+	buf->nr_pages = nr_pages;
+
+	buf->sglist = vzalloc(buf->nr_pages * sizeof(*buf->sglist));
+	if (NULL == buf->sglist)
+		goto vzalloc_err;
+
+	sg_init_table(buf->sglist, buf->nr_pages);
+	for (i = 0; i < buf->nr_pages; i++) {
+		pg = vmalloc_to_page(buf->vaddr + i * PAGE_SIZE);
+		if (NULL == pg)
+			goto vmalloc_to_page_err;
+		sg_set_page(&buf->sglist[i], pg, PAGE_SIZE, 0);
+	}
+	return 0;
+
+vmalloc_to_page_err:
+	vfree(buf->sglist);
+	buf->sglist = NULL;
+vzalloc_err:
+	vfree(buf->vaddr);
+	buf->vaddr = NULL;
+	return -ENOMEM;
+}
+
+static int cx23885_alsa_dma_map(struct cx23885_audio_dev *dev)
+{
+	struct cx23885_audio_buffer *buf = dev->buf;
+
+	buf->sglen = dma_map_sg(&dev->pci->dev, buf->sglist,
+			buf->nr_pages, PCI_DMA_FROMDEVICE);
+
+	if (0 == buf->sglen) {
+		pr_warn("%s: cx23885_alsa_map_sg failed\n", __func__);
+		return -ENOMEM;
+	}
+	return 0;
+}
+
+static int cx23885_alsa_dma_unmap(struct cx23885_audio_dev *dev)
+{
+	struct cx23885_audio_buffer *buf = dev->buf;
+
+	if (!buf->sglen)
+		return 0;
+
+	dma_unmap_sg(&dev->pci->dev, buf->sglist, buf->sglen, PCI_DMA_FROMDEVICE);
+	buf->sglen = 0;
+	return 0;
+}
+
+static int cx23885_alsa_dma_free(struct cx23885_audio_buffer *buf)
+{
+	vfree(buf->sglist);
+	buf->sglist = NULL;
+	vfree(buf->vaddr);
+	buf->vaddr = NULL;
+	return 0;
+}
+
 /*
  * BOARD Specific: Sets audio DMA
  */
@@ -198,15 +270,18 @@ int cx23885_audio_irq(struct cx23885_dev *dev, u32 status, u32 mask)
 
 static int dsp_buffer_free(struct cx23885_audio_dev *chip)
 {
+	struct cx23885_riscmem *risc;
+
 	BUG_ON(!chip->dma_size);
 
 	dprintk(2, "Freeing buffer\n");
-	videobuf_dma_unmap(&chip->pci->dev, chip->dma_risc);
-	videobuf_dma_free(chip->dma_risc);
-	btcx_riscmem_free(chip->pci, &chip->buf->risc);
+	cx23885_alsa_dma_unmap(chip);
+	cx23885_alsa_dma_free(chip->buf);
+	risc = &chip->buf->risc;
+	pci_free_consistent(chip->pci, risc->size, risc->cpu, risc->dma);
 	kfree(chip->buf);
 
-	chip->dma_risc = NULL;
+	chip->buf = NULL;
 	chip->dma_size = 0;
 
 	return 0;
@@ -289,6 +364,7 @@ static int snd_cx23885_close(struct snd_pcm_substream *substream)
 	return 0;
 }
 
+
 /*
  * hw_params callback
  */
@@ -296,8 +372,6 @@ static int snd_cx23885_hw_params(struct snd_pcm_substream *substream,
 			      struct snd_pcm_hw_params *hw_params)
 {
 	struct cx23885_audio_dev *chip = snd_pcm_substream_chip(substream);
-	struct videobuf_dmabuf *dma;
-
 	struct cx23885_audio_buffer *buf;
 	int ret;
 
@@ -318,19 +392,18 @@ static int snd_cx23885_hw_params(struct snd_pcm_substream *substream,
 		return -ENOMEM;
 
 	buf->bpl = chip->period_size;
+	chip->buf = buf;
 
-	dma = &buf->dma;
-	videobuf_dma_init(dma);
-	ret = videobuf_dma_init_kernel(dma, PCI_DMA_FROMDEVICE,
+	ret = cx23885_alsa_dma_init(chip,
 			(PAGE_ALIGN(chip->dma_size) >> PAGE_SHIFT));
 	if (ret < 0)
 		goto error;
 
-	ret = videobuf_dma_map(&chip->pci->dev, dma);
+	ret = cx23885_alsa_dma_map(chip);
 	if (ret < 0)
 		goto error;
 
-	ret = cx23885_risc_databuffer(chip->pci, &buf->risc, dma->sglist,
+	ret = cx23885_risc_databuffer(chip->pci, &buf->risc, buf->sglist,
 				   chip->period_size, chip->num_periods, 1);
 	if (ret < 0)
 		goto error;
@@ -340,10 +413,7 @@ static int snd_cx23885_hw_params(struct snd_pcm_substream *substream,
 	buf->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
 	buf->risc.jmp[2] = cpu_to_le32(0); /* bits 63-32 */
 
-	chip->buf = buf;
-	chip->dma_risc = dma;
-
-	substream->runtime->dma_area = chip->dma_risc->vaddr;
+	substream->runtime->dma_area = chip->buf->vaddr;
 	substream->runtime->dma_bytes = chip->dma_size;
 	substream->runtime->dma_addr = 0;
 
@@ -351,6 +421,7 @@ static int snd_cx23885_hw_params(struct snd_pcm_substream *substream,
 
 error:
 	kfree(buf);
+	chip->buf = NULL;
 	return ret;
 }
 
diff --git a/drivers/media/pci/cx23885/cx23885-av.c b/drivers/media/pci/cx23885/cx23885-av.c
index c443b7ac5adf..877dad89107e 100644
--- a/drivers/media/pci/cx23885/cx23885-av.c
+++ b/drivers/media/pci/cx23885/cx23885-av.c
@@ -14,11 +14,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #include "cx23885.h"
diff --git a/drivers/media/pci/cx23885/cx23885-av.h b/drivers/media/pci/cx23885/cx23885-av.h
index d2915c3e53a2..97f232f8efb9 100644
--- a/drivers/media/pci/cx23885/cx23885-av.h
+++ b/drivers/media/pci/cx23885/cx23885-av.h
@@ -14,11 +14,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #ifndef _CX23885_AV_H_
diff --git a/drivers/media/pci/cx23885/cx23885-cards.c b/drivers/media/pci/cx23885/cx23885-cards.c
index c2b608007190..88c257d1161b 100644
--- a/drivers/media/pci/cx23885/cx23885-cards.c
+++ b/drivers/media/pci/cx23885/cx23885-cards.c
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/init.h>
@@ -679,6 +675,11 @@ struct cx23885_board cx23885_boards[] = {
 			.amux   = CX25840_AUDIO7,
 		} },
 	},
+	[CX23885_BOARD_DVBSKY_T9580] = {
+		.name		= "DVBSky T9580",
+		.portb		= CX23885_MPEG_DVB,
+		.portc		= CX23885_MPEG_DVB,
+	},
 };
 const unsigned int cx23885_bcount = ARRAY_SIZE(cx23885_boards);
 
@@ -934,6 +935,10 @@ struct cx23885_subid cx23885_subids[] = {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb98,
 		.card      = CX23885_BOARD_DVICO_FUSIONHDTV_DVB_T_DUAL_EXP2,
+	}, {
+		.subvendor = 0x4254,
+		.subdevice = 0x9580,
+		.card      = CX23885_BOARD_DVBSKY_T9580,
 	},
 };
 const unsigned int cx23885_idcount = ARRAY_SIZE(cx23885_subids);
@@ -1528,6 +1533,14 @@ void cx23885_gpio_setup(struct cx23885_dev *dev)
 		cx_set(GP0_IO, 0x00040004);
 		mdelay(60);
 		break;
+	case CX23885_BOARD_DVBSKY_T9580:
+		/* enable GPIO3-18 pins */
+		cx_write(MC417_CTL, 0x00000037);
+		cx23885_gpio_enable(dev, GPIO_2 | GPIO_11, 1);
+		cx23885_gpio_clear(dev, GPIO_2 | GPIO_11);
+		mdelay(100);
+		cx23885_gpio_set(dev, GPIO_2 | GPIO_11);
+		break;
 	}
 }
 
@@ -1851,6 +1864,14 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 		ts2->ts_clk_en_val = 0x1; /* Enable TS_CLK */
 		ts2->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
 		break;
+	case CX23885_BOARD_DVBSKY_T9580:
+		ts1->gen_ctrl_val  = 0x5; /* Parallel */
+		ts1->ts_clk_en_val = 0x1; /* Enable TS_CLK */
+		ts1->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
+		ts2->gen_ctrl_val  = 0x8; /* Serial bus */
+		ts2->ts_clk_en_val = 0x1; /* Enable TS_CLK */
+		ts2->src_sel_val   = CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO;
+		break;
 	case CX23885_BOARD_HAUPPAUGE_HVR1250:
 	case CX23885_BOARD_HAUPPAUGE_HVR1500:
 	case CX23885_BOARD_HAUPPAUGE_HVR1500Q:
@@ -1913,6 +1934,7 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 	case CX23885_BOARD_AVERMEDIA_HC81R:
 	case CX23885_BOARD_TBS_6980:
 	case CX23885_BOARD_TBS_6981:
+	case CX23885_BOARD_DVBSKY_T9580:
 		dev->sd_cx25840 = v4l2_i2c_new_subdev(&dev->v4l2_dev,
 				&dev->i2c_bus[2].i2c_adap,
 				"cx25840", 0x88 >> 1, NULL);
@@ -1970,5 +1992,3 @@ void cx23885_card_setup(struct cx23885_dev *dev)
 	}
 	}
 }
-
-/* ------------------------------------------------------------------ */
diff --git a/drivers/media/pci/cx23885/cx23885-core.c b/drivers/media/pci/cx23885/cx23885-core.c
index edcd79db1e4e..331eddac7222 100644
--- a/drivers/media/pci/cx23885/cx23885-core.c
+++ b/drivers/media/pci/cx23885/cx23885-core.c
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/init.h>
@@ -420,39 +416,23 @@ static int cx23885_risc_decode(u32 risc)
 	return incr[risc >> 28] ? incr[risc >> 28] : 1;
 }
 
-void cx23885_wakeup(struct cx23885_tsport *port,
+static void cx23885_wakeup(struct cx23885_tsport *port,
 			   struct cx23885_dmaqueue *q, u32 count)
 {
 	struct cx23885_dev *dev = port->dev;
 	struct cx23885_buffer *buf;
-	int bc;
-
-	for (bc = 0;; bc++) {
-		if (list_empty(&q->active))
-			break;
-		buf = list_entry(q->active.next,
-				 struct cx23885_buffer, vb.queue);
-
-		/* count comes from the hw and is is 16bit wide --
-		 * this trick handles wrap-arounds correctly for
-		 * up to 32767 buffers in flight... */
-		if ((s16) (count - buf->count) < 0)
-			break;
 
-		v4l2_get_timestamp(&buf->vb.ts);
-		dprintk(2, "[%p/%d] wakeup reg=%d buf=%d\n", buf, buf->vb.i,
-			count, buf->count);
-		buf->vb.state = VIDEOBUF_DONE;
-		list_del(&buf->vb.queue);
-		wake_up(&buf->vb.done);
-	}
 	if (list_empty(&q->active))
-		del_timer(&q->timeout);
-	else
-		mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
-	if (bc != 1)
-		printk(KERN_WARNING "%s: %d buffers handled (should be 1)\n",
-		       __func__, bc);
+		return;
+	buf = list_entry(q->active.next,
+			 struct cx23885_buffer, queue);
+
+	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+	buf->vb.v4l2_buf.sequence = q->count++;
+	dprintk(1, "[%p/%d] wakeup reg=%d buf=%d\n", buf, buf->vb.v4l2_buf.index,
+		count, q->count);
+	list_del(&buf->queue);
+	vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
 }
 
 int cx23885_sram_channel_setup(struct cx23885_dev *dev,
@@ -482,8 +462,8 @@ int cx23885_sram_channel_setup(struct cx23885_dev *dev,
 		lines = 6;
 	BUG_ON(lines < 2);
 
-	cx_write(8 + 0, RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
-	cx_write(8 + 4, 8);
+	cx_write(8 + 0, RISC_JUMP | RISC_CNT_RESET);
+	cx_write(8 + 4, 12);
 	cx_write(8 + 8, 0);
 
 	/* write CDT */
@@ -590,7 +570,7 @@ void cx23885_sram_channel_dump(struct cx23885_dev *dev,
 }
 
 static void cx23885_risc_disasm(struct cx23885_tsport *port,
-				struct btcx_riscmem *risc)
+				struct cx23885_riscmem *risc)
 {
 	struct cx23885_dev *dev = port->dev;
 	unsigned int i, j, n;
@@ -699,10 +679,6 @@ static int get_resources(struct cx23885_dev *dev)
 	return -EBUSY;
 }
 
-static void cx23885_timeout(unsigned long data);
-int cx23885_risc_stopper(struct pci_dev *pci, struct btcx_riscmem *risc,
-				u32 reg, u32 mask, u32 value);
-
 static int cx23885_init_tsport(struct cx23885_dev *dev,
 	struct cx23885_tsport *port, int portno)
 {
@@ -719,11 +695,6 @@ static int cx23885_init_tsport(struct cx23885_dev *dev,
 	port->nr = portno;
 
 	INIT_LIST_HEAD(&port->mpegq.active);
-	INIT_LIST_HEAD(&port->mpegq.queued);
-	port->mpegq.timeout.function = cx23885_timeout;
-	port->mpegq.timeout.data = (unsigned long)port;
-	init_timer(&port->mpegq.timeout);
-
 	mutex_init(&port->frontends.lock);
 	INIT_LIST_HEAD(&port->frontends.felist);
 	port->frontends.active_fe_id = 0;
@@ -776,9 +747,6 @@ static int cx23885_init_tsport(struct cx23885_dev *dev,
 		BUG();
 	}
 
-	cx23885_risc_stopper(dev->pci, &port->mpegq.stopper,
-		     port->reg_dma_ctl, port->dma_ctl_val, 0x00);
-
 	return 0;
 }
 
@@ -1089,11 +1057,18 @@ static void cx23885_dev_unregister(struct cx23885_dev *dev)
 static __le32 *cx23885_risc_field(__le32 *rp, struct scatterlist *sglist,
 			       unsigned int offset, u32 sync_line,
 			       unsigned int bpl, unsigned int padding,
-			       unsigned int lines,  unsigned int lpi)
+			       unsigned int lines,  unsigned int lpi, bool jump)
 {
 	struct scatterlist *sg;
 	unsigned int line, todo, sol;
 
+
+	if (jump) {
+		*(rp++) = cpu_to_le32(RISC_JUMP);
+		*(rp++) = cpu_to_le32(0);
+		*(rp++) = cpu_to_le32(0); /* bits 63-32 */
+	}
+
 	/* sync instruction */
 	if (sync_line != NO_SYNC_LINE)
 		*(rp++) = cpu_to_le32(RISC_RESYNC | sync_line);
@@ -1146,14 +1121,13 @@ static __le32 *cx23885_risc_field(__le32 *rp, struct scatterlist *sglist,
 	return rp;
 }
 
-int cx23885_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
+int cx23885_risc_buffer(struct pci_dev *pci, struct cx23885_riscmem *risc,
 			struct scatterlist *sglist, unsigned int top_offset,
 			unsigned int bottom_offset, unsigned int bpl,
 			unsigned int padding, unsigned int lines)
 {
 	u32 instructions, fields;
 	__le32 *rp;
-	int rc;
 
 	fields = 0;
 	if (UNSET != top_offset)
@@ -1168,19 +1142,20 @@ int cx23885_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
 	/* write and jump need and extra dword */
 	instructions  = fields * (1 + ((bpl + padding) * lines)
 		/ PAGE_SIZE + lines);
-	instructions += 2;
-	rc = btcx_riscmem_alloc(pci, risc, instructions*12);
-	if (rc < 0)
-		return rc;
+	instructions += 5;
+	risc->size = instructions * 12;
+	risc->cpu = pci_alloc_consistent(pci, risc->size, &risc->dma);
+	if (risc->cpu == NULL)
+		return -ENOMEM;
 
 	/* write risc instructions */
 	rp = risc->cpu;
 	if (UNSET != top_offset)
 		rp = cx23885_risc_field(rp, sglist, top_offset, 0,
-					bpl, padding, lines, 0);
+					bpl, padding, lines, 0, true);
 	if (UNSET != bottom_offset)
 		rp = cx23885_risc_field(rp, sglist, bottom_offset, 0x200,
-					bpl, padding, lines, 0);
+					bpl, padding, lines, 0, UNSET == top_offset);
 
 	/* save pointer to jmp instruction address */
 	risc->jmp = rp;
@@ -1189,14 +1164,13 @@ int cx23885_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
 }
 
 int cx23885_risc_databuffer(struct pci_dev *pci,
-				   struct btcx_riscmem *risc,
+				   struct cx23885_riscmem *risc,
 				   struct scatterlist *sglist,
 				   unsigned int bpl,
 				   unsigned int lines, unsigned int lpi)
 {
 	u32 instructions;
 	__le32 *rp;
-	int rc;
 
 	/* estimate risc mem: worst case is one write per page border +
 	   one write per scan line + syncs + jump (all 2 dwords).  Here
@@ -1204,16 +1178,17 @@ int cx23885_risc_databuffer(struct pci_dev *pci,
 	   than PAGE_SIZE */
 	/* Jump and write need an extra dword */
 	instructions  = 1 + (bpl * lines) / PAGE_SIZE + lines;
-	instructions += 1;
+	instructions += 4;
 
-	rc = btcx_riscmem_alloc(pci, risc, instructions*12);
-	if (rc < 0)
-		return rc;
+	risc->size = instructions * 12;
+	risc->cpu = pci_alloc_consistent(pci, risc->size, &risc->dma);
+	if (risc->cpu == NULL)
+		return -ENOMEM;
 
 	/* write risc instructions */
 	rp = risc->cpu;
 	rp = cx23885_risc_field(rp, sglist, 0, NO_SYNC_LINE,
-				bpl, 0, lines, lpi);
+				bpl, 0, lines, lpi, lpi == 0);
 
 	/* save pointer to jmp instruction address */
 	risc->jmp = rp;
@@ -1221,14 +1196,13 @@ int cx23885_risc_databuffer(struct pci_dev *pci,
 	return 0;
 }
 
-int cx23885_risc_vbibuffer(struct pci_dev *pci, struct btcx_riscmem *risc,
+int cx23885_risc_vbibuffer(struct pci_dev *pci, struct cx23885_riscmem *risc,
 			struct scatterlist *sglist, unsigned int top_offset,
 			unsigned int bottom_offset, unsigned int bpl,
 			unsigned int padding, unsigned int lines)
 {
 	u32 instructions, fields;
 	__le32 *rp;
-	int rc;
 
 	fields = 0;
 	if (UNSET != top_offset)
@@ -1243,22 +1217,23 @@ int cx23885_risc_vbibuffer(struct pci_dev *pci, struct btcx_riscmem *risc,
 	/* write and jump need and extra dword */
 	instructions  = fields * (1 + ((bpl + padding) * lines)
 		/ PAGE_SIZE + lines);
-	instructions += 2;
-	rc = btcx_riscmem_alloc(pci, risc, instructions*12);
-	if (rc < 0)
-		return rc;
+	instructions += 5;
+	risc->size = instructions * 12;
+	risc->cpu = pci_alloc_consistent(pci, risc->size, &risc->dma);
+	if (risc->cpu == NULL)
+		return -ENOMEM;
 	/* write risc instructions */
 	rp = risc->cpu;
 
 	/* Sync to line 6, so US CC line 21 will appear in line '12'
 	 * in the userland vbi payload */
 	if (UNSET != top_offset)
-		rp = cx23885_risc_field(rp, sglist, top_offset, 6,
-					bpl, padding, lines, 0);
+		rp = cx23885_risc_field(rp, sglist, top_offset, 0,
+					bpl, padding, lines, 0, true);
 
 	if (UNSET != bottom_offset)
-		rp = cx23885_risc_field(rp, sglist, bottom_offset, 0x207,
-					bpl, padding, lines, 0);
+		rp = cx23885_risc_field(rp, sglist, bottom_offset, 0x200,
+					bpl, padding, lines, 0, UNSET == top_offset);
 
 
 
@@ -1269,38 +1244,12 @@ int cx23885_risc_vbibuffer(struct pci_dev *pci, struct btcx_riscmem *risc,
 }
 
 
-int cx23885_risc_stopper(struct pci_dev *pci, struct btcx_riscmem *risc,
-				u32 reg, u32 mask, u32 value)
+void cx23885_free_buffer(struct cx23885_dev *dev, struct cx23885_buffer *buf)
 {
-	__le32 *rp;
-	int rc;
-
-	rc = btcx_riscmem_alloc(pci, risc, 4*16);
-	if (rc < 0)
-		return rc;
-
-	/* write risc instructions */
-	rp = risc->cpu;
-	*(rp++) = cpu_to_le32(RISC_WRITECR  | RISC_IRQ2);
-	*(rp++) = cpu_to_le32(reg);
-	*(rp++) = cpu_to_le32(value);
-	*(rp++) = cpu_to_le32(mask);
-	*(rp++) = cpu_to_le32(RISC_JUMP);
-	*(rp++) = cpu_to_le32(risc->dma);
-	*(rp++) = cpu_to_le32(0); /* bits 63-32 */
-	return 0;
-}
-
-void cx23885_free_buffer(struct videobuf_queue *q, struct cx23885_buffer *buf)
-{
-	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
+	struct cx23885_riscmem *risc = &buf->risc;
 
 	BUG_ON(in_interrupt());
-	videobuf_waiton(q, &buf->vb, 0, 0);
-	videobuf_dma_unmap(q->dev, dma);
-	videobuf_dma_free(dma);
-	btcx_riscmem_free(to_pci_dev(q->dev), &buf->risc);
-	buf->vb.state = VIDEOBUF_NEEDS_INIT;
+	pci_free_consistent(dev->pci, risc->size, risc->cpu, risc->dma);
 }
 
 static void cx23885_tsport_reg_dump(struct cx23885_tsport *port)
@@ -1355,7 +1304,7 @@ static void cx23885_tsport_reg_dump(struct cx23885_tsport *port)
 		port->reg_ts_int_msk, cx_read(port->reg_ts_int_msk));
 }
 
-static int cx23885_start_dma(struct cx23885_tsport *port,
+int cx23885_start_dma(struct cx23885_tsport *port,
 			     struct cx23885_dmaqueue *q,
 			     struct cx23885_buffer   *buf)
 {
@@ -1363,7 +1312,7 @@ static int cx23885_start_dma(struct cx23885_tsport *port,
 	u32 reg;
 
 	dprintk(1, "%s() w: %d, h: %d, f: %d\n", __func__,
-		buf->vb.width, buf->vb.height, buf->vb.field);
+		dev->width, dev->height, dev->field);
 
 	/* Stop the fifo and risc engine for this port */
 	cx_clear(port->reg_dma_ctl, port->dma_ctl_val);
@@ -1379,7 +1328,7 @@ static int cx23885_start_dma(struct cx23885_tsport *port,
 	}
 
 	/* write TS length to chip */
-	cx_write(port->reg_lngth, buf->vb.width);
+	cx_write(port->reg_lngth, port->ts_packet_size);
 
 	if ((!(cx23885_boards[dev->board].portb & CX23885_MPEG_DVB)) &&
 		(!(cx23885_boards[dev->board].portc & CX23885_MPEG_DVB))) {
@@ -1408,7 +1357,7 @@ static int cx23885_start_dma(struct cx23885_tsport *port,
 	/* NOTE: this is 2 (reserved) for portb, does it matter? */
 	/* reset counter to zero */
 	cx_write(port->reg_gpcnt_ctl, 3);
-	q->count = 1;
+	q->count = 0;
 
 	/* Set VIDB pins to input */
 	if (cx23885_boards[dev->board].portb == CX23885_MPEG_DVB) {
@@ -1497,134 +1446,83 @@ static int cx23885_stop_dma(struct cx23885_tsport *port)
 	return 0;
 }
 
-int cx23885_restart_queue(struct cx23885_tsport *port,
-				struct cx23885_dmaqueue *q)
-{
-	struct cx23885_dev *dev = port->dev;
-	struct cx23885_buffer *buf;
-
-	dprintk(5, "%s()\n", __func__);
-	if (list_empty(&q->active)) {
-		struct cx23885_buffer *prev;
-		prev = NULL;
-
-		dprintk(5, "%s() queue is empty\n", __func__);
-
-		for (;;) {
-			if (list_empty(&q->queued))
-				return 0;
-			buf = list_entry(q->queued.next, struct cx23885_buffer,
-					 vb.queue);
-			if (NULL == prev) {
-				list_move_tail(&buf->vb.queue, &q->active);
-				cx23885_start_dma(port, q, buf);
-				buf->vb.state = VIDEOBUF_ACTIVE;
-				buf->count    = q->count++;
-				mod_timer(&q->timeout, jiffies+BUFFER_TIMEOUT);
-				dprintk(5, "[%p/%d] restart_queue - f/active\n",
-					buf, buf->vb.i);
-
-			} else if (prev->vb.width  == buf->vb.width  &&
-				   prev->vb.height == buf->vb.height &&
-				   prev->fmt       == buf->fmt) {
-				list_move_tail(&buf->vb.queue, &q->active);
-				buf->vb.state = VIDEOBUF_ACTIVE;
-				buf->count    = q->count++;
-				prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
-				/* 64 bit bits 63-32 */
-				prev->risc.jmp[2] = cpu_to_le32(0);
-				dprintk(5, "[%p/%d] restart_queue - m/active\n",
-					buf, buf->vb.i);
-			} else {
-				return 0;
-			}
-			prev = buf;
-		}
-		return 0;
-	}
-
-	buf = list_entry(q->active.next, struct cx23885_buffer, vb.queue);
-	dprintk(2, "restart_queue [%p/%d]: restart dma\n",
-		buf, buf->vb.i);
-	cx23885_start_dma(port, q, buf);
-	list_for_each_entry(buf, &q->active, vb.queue)
-		buf->count = q->count++;
-	mod_timer(&q->timeout, jiffies + BUFFER_TIMEOUT);
-	return 0;
-}
-
 /* ------------------------------------------------------------------ */
 
-int cx23885_buf_prepare(struct videobuf_queue *q, struct cx23885_tsport *port,
-			struct cx23885_buffer *buf, enum v4l2_field field)
+int cx23885_buf_prepare(struct cx23885_buffer *buf, struct cx23885_tsport *port)
 {
 	struct cx23885_dev *dev = port->dev;
 	int size = port->ts_packet_size * port->ts_packet_count;
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(&buf->vb, 0);
 	int rc;
 
 	dprintk(1, "%s: %p\n", __func__, buf);
-	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size)
+	if (vb2_plane_size(&buf->vb, 0) < size)
 		return -EINVAL;
+	vb2_set_plane_payload(&buf->vb, 0, size);
 
-	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
-		buf->vb.width  = port->ts_packet_size;
-		buf->vb.height = port->ts_packet_count;
-		buf->vb.size   = size;
-		buf->vb.field  = field /*V4L2_FIELD_TOP*/;
-
-		rc = videobuf_iolock(q, &buf->vb, NULL);
-		if (0 != rc)
-			goto fail;
-		cx23885_risc_databuffer(dev->pci, &buf->risc,
-					videobuf_to_dma(&buf->vb)->sglist,
-					buf->vb.width, buf->vb.height, 0);
-	}
-	buf->vb.state = VIDEOBUF_PREPARED;
-	return 0;
+	rc = dma_map_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
+	if (!rc)
+		return -EIO;
 
- fail:
-	cx23885_free_buffer(q, buf);
-	return rc;
+	cx23885_risc_databuffer(dev->pci, &buf->risc,
+				sgt->sgl,
+				port->ts_packet_size, port->ts_packet_count, 0);
+	return 0;
 }
 
+/*
+ * The risc program for each buffer works as follows: it starts with a simple
+ * 'JUMP to addr + 12', which is effectively a NOP. Then the code to DMA the
+ * buffer follows and at the end we have a JUMP back to the start + 12 (skipping
+ * the initial JUMP).
+ *
+ * This is the risc program of the first buffer to be queued if the active list
+ * is empty and it just keeps DMAing this buffer without generating any
+ * interrupts.
+ *
+ * If a new buffer is added then the initial JUMP in the code for that buffer
+ * will generate an interrupt which signals that the previous buffer has been
+ * DMAed successfully and that it can be returned to userspace.
+ *
+ * It also sets the final jump of the previous buffer to the start of the new
+ * buffer, thus chaining the new buffer into the DMA chain. This is a single
+ * atomic u32 write, so there is no race condition.
+ *
+ * The end-result of all this that you only get an interrupt when a buffer
+ * is ready, so the control flow is very easy.
+ */
 void cx23885_buf_queue(struct cx23885_tsport *port, struct cx23885_buffer *buf)
 {
 	struct cx23885_buffer    *prev;
 	struct cx23885_dev *dev = port->dev;
 	struct cx23885_dmaqueue  *cx88q = &port->mpegq;
+	unsigned long flags;
 
-	/* add jump to stopper */
-	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
-	buf->risc.jmp[1] = cpu_to_le32(cx88q->stopper.dma);
+	buf->risc.cpu[1] = cpu_to_le32(buf->risc.dma + 12);
+	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_CNT_INC);
+	buf->risc.jmp[1] = cpu_to_le32(buf->risc.dma + 12);
 	buf->risc.jmp[2] = cpu_to_le32(0); /* bits 63-32 */
 
+	spin_lock_irqsave(&dev->slock, flags);
 	if (list_empty(&cx88q->active)) {
-		dprintk(1, "queue is empty - first active\n");
-		list_add_tail(&buf->vb.queue, &cx88q->active);
-		cx23885_start_dma(port, cx88q, buf);
-		buf->vb.state = VIDEOBUF_ACTIVE;
-		buf->count    = cx88q->count++;
-		mod_timer(&cx88q->timeout, jiffies + BUFFER_TIMEOUT);
+		list_add_tail(&buf->queue, &cx88q->active);
 		dprintk(1, "[%p/%d] %s - first active\n",
-			buf, buf->vb.i, __func__);
+			buf, buf->vb.v4l2_buf.index, __func__);
 	} else {
-		dprintk(1, "queue is not empty - append to active\n");
+		buf->risc.cpu[0] |= cpu_to_le32(RISC_IRQ1);
 		prev = list_entry(cx88q->active.prev, struct cx23885_buffer,
-				  vb.queue);
-		list_add_tail(&buf->vb.queue, &cx88q->active);
-		buf->vb.state = VIDEOBUF_ACTIVE;
-		buf->count    = cx88q->count++;
+				  queue);
+		list_add_tail(&buf->queue, &cx88q->active);
 		prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
-		prev->risc.jmp[2] = cpu_to_le32(0); /* 64 bit bits 63-32 */
 		dprintk(1, "[%p/%d] %s - append to active\n",
-			 buf, buf->vb.i, __func__);
+			 buf, buf->vb.v4l2_buf.index, __func__);
 	}
+	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
 /* ----------------------------------------------------------- */
 
-static void do_cancel_buffers(struct cx23885_tsport *port, char *reason,
-			      int restart)
+static void do_cancel_buffers(struct cx23885_tsport *port, char *reason)
 {
 	struct cx23885_dev *dev = port->dev;
 	struct cx23885_dmaqueue *q = &port->mpegq;
@@ -1634,16 +1532,11 @@ static void do_cancel_buffers(struct cx23885_tsport *port, char *reason,
 	spin_lock_irqsave(&port->slock, flags);
 	while (!list_empty(&q->active)) {
 		buf = list_entry(q->active.next, struct cx23885_buffer,
-				 vb.queue);
-		list_del(&buf->vb.queue);
-		buf->vb.state = VIDEOBUF_ERROR;
-		wake_up(&buf->vb.done);
+				 queue);
+		list_del(&buf->queue);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 		dprintk(1, "[%p/%d] %s - dma=0x%08lx\n",
-			buf, buf->vb.i, reason, (unsigned long)buf->risc.dma);
-	}
-	if (restart) {
-		dprintk(1, "restarting queue\n");
-		cx23885_restart_queue(port, q);
+			buf, buf->vb.v4l2_buf.index, reason, (unsigned long)buf->risc.dma);
 	}
 	spin_unlock_irqrestore(&port->slock, flags);
 }
@@ -1651,27 +1544,10 @@ static void do_cancel_buffers(struct cx23885_tsport *port, char *reason,
 void cx23885_cancel_buffers(struct cx23885_tsport *port)
 {
 	struct cx23885_dev *dev = port->dev;
-	struct cx23885_dmaqueue *q = &port->mpegq;
-
-	dprintk(1, "%s()\n", __func__);
-	del_timer_sync(&q->timeout);
-	cx23885_stop_dma(port);
-	do_cancel_buffers(port, "cancel", 0);
-}
-
-static void cx23885_timeout(unsigned long data)
-{
-	struct cx23885_tsport *port = (struct cx23885_tsport *)data;
-	struct cx23885_dev *dev = port->dev;
 
 	dprintk(1, "%s()\n", __func__);
-
-	if (debug > 5)
-		cx23885_sram_channel_dump(dev,
-			&dev->sram_channels[port->sram_chno]);
-
 	cx23885_stop_dma(port);
-	do_cancel_buffers(port, "timeout", 1);
+	do_cancel_buffers(port, "cancel");
 }
 
 int cx23885_irq_417(struct cx23885_dev *dev, u32 status)
@@ -1721,11 +1597,6 @@ int cx23885_irq_417(struct cx23885_dev *dev, u32 status)
 		spin_lock(&port->slock);
 		cx23885_wakeup(port, &port->mpegq, count);
 		spin_unlock(&port->slock);
-	} else if (status & VID_B_MSK_RISCI2) {
-		dprintk(7, "        VID_B_MSK_RISCI2\n");
-		spin_lock(&port->slock);
-		cx23885_restart_queue(port, &port->mpegq);
-		spin_unlock(&port->slock);
 	}
 	if (status) {
 		cx_write(port->reg_ts_int_stat, status);
@@ -1777,14 +1648,6 @@ static int cx23885_irq_ts(struct cx23885_tsport *port, u32 status)
 		cx23885_wakeup(port, &port->mpegq, count);
 		spin_unlock(&port->slock);
 
-	} else if (status & VID_BC_MSK_RISCI2) {
-
-		dprintk(7, " (RISCI2            0x%08x)\n", VID_BC_MSK_RISCI2);
-
-		spin_lock(&port->slock);
-		cx23885_restart_queue(port, &port->mpegq);
-		spin_unlock(&port->slock);
-
 	}
 	if (status) {
 		cx_write(port->reg_ts_int_stat, status);
@@ -2087,6 +1950,7 @@ static int cx23885_initdev(struct pci_dev *pci_dev,
 			   const struct pci_device_id *pci_id)
 {
 	struct cx23885_dev *dev;
+	struct v4l2_ctrl_handler *hdl;
 	int err;
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
@@ -2097,6 +1961,14 @@ static int cx23885_initdev(struct pci_dev *pci_dev,
 	if (err < 0)
 		goto fail_free;
 
+	hdl = &dev->ctrl_handler;
+	v4l2_ctrl_handler_init(hdl, 6);
+	if (hdl->error) {
+		err = hdl->error;
+		goto fail_ctrl;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
+
 	/* Prepare to handle notifications from subdevices */
 	cx23885_v4l2_dev_notify_init(dev);
 
@@ -2104,12 +1976,12 @@ static int cx23885_initdev(struct pci_dev *pci_dev,
 	dev->pci = pci_dev;
 	if (pci_enable_device(pci_dev)) {
 		err = -EIO;
-		goto fail_unreg;
+		goto fail_ctrl;
 	}
 
 	if (cx23885_dev_setup(dev) < 0) {
 		err = -EINVAL;
-		goto fail_unreg;
+		goto fail_ctrl;
 	}
 
 	/* print pci info */
@@ -2157,7 +2029,8 @@ static int cx23885_initdev(struct pci_dev *pci_dev,
 
 fail_irq:
 	cx23885_dev_unregister(dev);
-fail_unreg:
+fail_ctrl:
+	v4l2_ctrl_handler_free(hdl);
 	v4l2_device_unregister(&dev->v4l2_dev);
 fail_free:
 	kfree(dev);
@@ -2180,6 +2053,7 @@ static void cx23885_finidev(struct pci_dev *pci_dev)
 	free_irq(pci_dev->irq, dev);
 
 	cx23885_dev_unregister(dev);
+	v4l2_ctrl_handler_free(&dev->ctrl_handler);
 	v4l2_device_unregister(v4l2_dev);
 	kfree(dev);
 }
diff --git a/drivers/media/pci/cx23885/cx23885-dvb.c b/drivers/media/pci/cx23885/cx23885-dvb.c
index 968fecc32f9c..13734b8c7917 100644
--- a/drivers/media/pci/cx23885/cx23885-dvb.c
+++ b/drivers/media/pci/cx23885/cx23885-dvb.c
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/module.h>
@@ -73,6 +69,10 @@
 #include "a8293.h"
 #include "mb86a20s.h"
 #include "si2165.h"
+#include "si2168.h"
+#include "si2157.h"
+#include "m88ds3103.h"
+#include "m88ts2022.h"
 
 static unsigned int debug;
 
@@ -91,59 +91,95 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 /* ------------------------------------------------------------------ */
 
-static int dvb_buf_setup(struct videobuf_queue *q,
-			 unsigned int *count, unsigned int *size)
+static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct cx23885_tsport *port = q->priv_data;
+	struct cx23885_tsport *port = q->drv_priv;
 
 	port->ts_packet_size  = 188 * 4;
 	port->ts_packet_count = 32;
-
-	*size  = port->ts_packet_size * port->ts_packet_count;
-	*count = 32;
+	*num_planes = 1;
+	sizes[0] = port->ts_packet_size * port->ts_packet_count;
+	*num_buffers = 32;
 	return 0;
 }
 
-static int dvb_buf_prepare(struct videobuf_queue *q,
-			   struct videobuf_buffer *vb, enum v4l2_field field)
+
+static int buffer_prepare(struct vb2_buffer *vb)
 {
-	struct cx23885_tsport *port = q->priv_data;
-	return cx23885_buf_prepare(q, port, (struct cx23885_buffer *)vb, field);
+	struct cx23885_tsport *port = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer *buf =
+		container_of(vb, struct cx23885_buffer, vb);
+
+	return cx23885_buf_prepare(buf, port);
 }
 
-static void dvb_buf_queue(struct videobuf_queue *q, struct videobuf_buffer *vb)
+static void buffer_finish(struct vb2_buffer *vb)
 {
-	struct cx23885_tsport *port = q->priv_data;
-	cx23885_buf_queue(port, (struct cx23885_buffer *)vb);
+	struct cx23885_tsport *port = vb->vb2_queue->drv_priv;
+	struct cx23885_dev *dev = port->dev;
+	struct cx23885_buffer *buf = container_of(vb,
+		struct cx23885_buffer, vb);
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+
+	cx23885_free_buffer(dev, buf);
+
+	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
-static void dvb_buf_release(struct videobuf_queue *q,
-			    struct videobuf_buffer *vb)
+static void buffer_queue(struct vb2_buffer *vb)
 {
-	cx23885_free_buffer(q, (struct cx23885_buffer *)vb);
+	struct cx23885_tsport *port = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer   *buf = container_of(vb,
+		struct cx23885_buffer, vb);
+
+	cx23885_buf_queue(port, buf);
 }
 
 static void cx23885_dvb_gate_ctrl(struct cx23885_tsport  *port, int open)
 {
-	struct videobuf_dvb_frontends *f;
-	struct videobuf_dvb_frontend *fe;
+	struct vb2_dvb_frontends *f;
+	struct vb2_dvb_frontend *fe;
 
 	f = &port->frontends;
 
 	if (f->gate <= 1) /* undefined or fe0 */
-		fe = videobuf_dvb_get_frontend(f, 1);
+		fe = vb2_dvb_get_frontend(f, 1);
 	else
-		fe = videobuf_dvb_get_frontend(f, f->gate);
+		fe = vb2_dvb_get_frontend(f, f->gate);
 
 	if (fe && fe->dvb.frontend && fe->dvb.frontend->ops.i2c_gate_ctrl)
 		fe->dvb.frontend->ops.i2c_gate_ctrl(fe->dvb.frontend, open);
 }
 
-static struct videobuf_queue_ops dvb_qops = {
-	.buf_setup    = dvb_buf_setup,
-	.buf_prepare  = dvb_buf_prepare,
-	.buf_queue    = dvb_buf_queue,
-	.buf_release  = dvb_buf_release,
+static int cx23885_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct cx23885_tsport *port = q->drv_priv;
+	struct cx23885_dmaqueue *dmaq = &port->mpegq;
+	struct cx23885_buffer *buf = list_entry(dmaq->active.next,
+			struct cx23885_buffer, queue);
+
+	cx23885_start_dma(port, dmaq, buf);
+	return 0;
+}
+
+static void cx23885_stop_streaming(struct vb2_queue *q)
+{
+	struct cx23885_tsport *port = q->drv_priv;
+
+	cx23885_cancel_buffers(port);
+}
+
+static struct vb2_ops dvb_qops = {
+	.queue_setup    = queue_setup,
+	.buf_prepare  = buffer_prepare,
+	.buf_finish = buffer_finish,
+	.buf_queue    = buffer_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = cx23885_start_streaming,
+	.stop_streaming = cx23885_stop_streaming,
 };
 
 static struct s5h1409_config hauppauge_generic_config = {
@@ -551,6 +587,35 @@ static int p8000_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 	return 0;
 }
 
+static int dvbsky_t9580_set_voltage(struct dvb_frontend *fe,
+					fe_sec_voltage_t voltage)
+{
+	struct cx23885_tsport *port = fe->dvb->priv;
+	struct cx23885_dev *dev = port->dev;
+
+	cx23885_gpio_enable(dev, GPIO_0 | GPIO_1, 1);
+
+	switch (voltage) {
+	case SEC_VOLTAGE_13:
+		cx23885_gpio_set(dev, GPIO_1);
+		cx23885_gpio_clear(dev, GPIO_0);
+		break;
+	case SEC_VOLTAGE_18:
+		cx23885_gpio_set(dev, GPIO_1);
+		cx23885_gpio_set(dev, GPIO_0);
+		break;
+	case SEC_VOLTAGE_OFF:
+		cx23885_gpio_clear(dev, GPIO_1);
+		cx23885_gpio_clear(dev, GPIO_0);
+		break;
+	}
+
+	/* call the frontend set_voltage function */
+	port->fe_set_voltage(fe, voltage);
+
+	return 0;
+}
+
 static int cx23885_dvb_set_frontend(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
@@ -715,6 +780,19 @@ static const struct si2165_config hauppauge_hvr4400_si2165_config = {
 	.ref_freq_Hz	= 16000000,
 };
 
+static const struct m88ds3103_config dvbsky_t9580_m88ds3103_config = {
+	.i2c_addr = 0x68,
+	.clock = 27000000,
+	.i2c_wr_max = 33,
+	.clock_out = 0,
+	.ts_mode = M88DS3103_TS_PARALLEL,
+	.ts_clk = 16000,
+	.ts_clk_pol = 1,
+	.lnb_en_pol = 1,
+	.lnb_hv_pol = 0,
+	.agc = 0x99,
+};
+
 static int netup_altera_fpga_rw(void *device, int flag, int data, int read)
 {
 	struct cx23885_dev *dev = (struct cx23885_dev *)device;
@@ -863,16 +941,23 @@ static int dvb_register(struct cx23885_tsport *port)
 	struct dib7000p_ops dib7000p_ops;
 	struct cx23885_dev *dev = port->dev;
 	struct cx23885_i2c *i2c_bus = NULL, *i2c_bus2 = NULL;
-	struct videobuf_dvb_frontend *fe0, *fe1 = NULL;
+	struct vb2_dvb_frontend *fe0, *fe1 = NULL;
+	struct si2168_config si2168_config;
+	struct si2157_config si2157_config;
+	struct m88ts2022_config m88ts2022_config;
+	struct i2c_board_info info;
+	struct i2c_adapter *adapter;
+	struct i2c_client *client_demod;
+	struct i2c_client *client_tuner;
 	int mfe_shared = 0; /* bus not shared by default */
 	int ret;
 
 	/* Get the first frontend */
-	fe0 = videobuf_dvb_get_frontend(&port->frontends, 1);
+	fe0 = vb2_dvb_get_frontend(&port->frontends, 1);
 	if (!fe0)
 		return -EINVAL;
 
-	/* init struct videobuf_dvb */
+	/* init struct vb2_dvb */
 	fe0->dvb.name = dev->name;
 
 	/* multi-frontend gate control is undefined or defaults to fe0 */
@@ -1392,7 +1477,7 @@ static int dvb_register(struct cx23885_tsport *port)
 			fe0->dvb.frontend->ops.tuner_ops.init(fe0->dvb.frontend);
 		}
 		/* MFE frontend 2 */
-		fe1 = videobuf_dvb_get_frontend(&port->frontends, 2);
+		fe1 = vb2_dvb_get_frontend(&port->frontends, 2);
 		if (fe1 == NULL)
 			goto frontend_detach;
 		/* DVB-C init */
@@ -1491,7 +1576,7 @@ static int dvb_register(struct cx23885_tsport *port)
 					&hauppauge_hvr4400_si2165_config,
 					&i2c_bus->i2c_adap);
 			if (fe0->dvb.frontend != NULL) {
-				fe0->dvb.frontend->ops.i2c_gate_ctrl = 0;
+				fe0->dvb.frontend->ops.i2c_gate_ctrl = NULL;
 				if (!dvb_attach(tda18271_attach,
 						fe0->dvb.frontend,
 						0x60, &i2c_bus2->i2c_adap,
@@ -1501,6 +1586,97 @@ static int dvb_register(struct cx23885_tsport *port)
 			break;
 		}
 		break;
+	case CX23885_BOARD_DVBSKY_T9580:
+		i2c_bus = &dev->i2c_bus[0];
+		i2c_bus2 = &dev->i2c_bus[1];
+		switch (port->nr) {
+		/* port b - satellite */
+		case 1:
+			/* attach frontend */
+			fe0->dvb.frontend = dvb_attach(m88ds3103_attach,
+					&dvbsky_t9580_m88ds3103_config,
+					&i2c_bus2->i2c_adap, &adapter);
+			if (fe0->dvb.frontend == NULL)
+				break;
+
+			/* attach tuner */
+			m88ts2022_config.fe = fe0->dvb.frontend;
+			m88ts2022_config.clock = 27000000;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "m88ts2022", I2C_NAME_SIZE);
+			info.addr = 0x60;
+			info.platform_data = &m88ts2022_config;
+			request_module(info.type);
+			client_tuner = i2c_new_device(adapter, &info);
+			if (client_tuner == NULL ||
+					client_tuner->dev.driver == NULL)
+				goto frontend_detach;
+			if (!try_module_get(client_tuner->dev.driver->owner)) {
+				i2c_unregister_device(client_tuner);
+				goto frontend_detach;
+			}
+
+			/* delegate signal strength measurement to tuner */
+			fe0->dvb.frontend->ops.read_signal_strength =
+				fe0->dvb.frontend->ops.tuner_ops.get_rf_strength;
+
+			/*
+			 * for setting the voltage we need to set GPIOs on
+			 * the card.
+			 */
+			port->fe_set_voltage =
+				fe0->dvb.frontend->ops.set_voltage;
+			fe0->dvb.frontend->ops.set_voltage =
+				dvbsky_t9580_set_voltage;
+
+			port->i2c_client_tuner = client_tuner;
+
+			break;
+		/* port c - terrestrial/cable */
+		case 2:
+			/* attach frontend */
+			si2168_config.i2c_adapter = &adapter;
+			si2168_config.fe = &fe0->dvb.frontend;
+			si2168_config.ts_mode = SI2168_TS_SERIAL;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2168", I2C_NAME_SIZE);
+			info.addr = 0x64;
+			info.platform_data = &si2168_config;
+			request_module(info.type);
+			client_demod = i2c_new_device(&i2c_bus->i2c_adap, &info);
+			if (client_demod == NULL ||
+					client_demod->dev.driver == NULL)
+				goto frontend_detach;
+			if (!try_module_get(client_demod->dev.driver->owner)) {
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			port->i2c_client_demod = client_demod;
+
+			/* attach tuner */
+			si2157_config.fe = fe0->dvb.frontend;
+			memset(&info, 0, sizeof(struct i2c_board_info));
+			strlcpy(info.type, "si2157", I2C_NAME_SIZE);
+			info.addr = 0x60;
+			info.platform_data = &si2157_config;
+			request_module(info.type);
+			client_tuner = i2c_new_device(adapter, &info);
+			if (client_tuner == NULL ||
+					client_tuner->dev.driver == NULL) {
+				module_put(client_demod->dev.driver->owner);
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			if (!try_module_get(client_tuner->dev.driver->owner)) {
+				i2c_unregister_device(client_tuner);
+				module_put(client_demod->dev.driver->owner);
+				i2c_unregister_device(client_demod);
+				goto frontend_detach;
+			}
+			port->i2c_client_tuner = client_tuner;
+			break;
+		}
+		break;
 	default:
 		printk(KERN_INFO "%s: The frontend of your DVB/ATSC card "
 			" isn't supported yet\n",
@@ -1532,7 +1708,7 @@ static int dvb_register(struct cx23885_tsport *port)
 		fe0->dvb.frontend->ops.analog_ops.standby(fe0->dvb.frontend);
 
 	/* register everything */
-	ret = videobuf_dvb_register_bus(&port->frontends, THIS_MODULE, port,
+	ret = vb2_dvb_register_bus(&port->frontends, THIS_MODULE, port,
 					&dev->pci->dev, adapter_nr, mfe_shared);
 	if (ret)
 		goto frontend_detach;
@@ -1575,20 +1751,36 @@ static int dvb_register(struct cx23885_tsport *port)
 		memcpy(port->frontends.adapter.proposed_mac, eeprom + 0xa0, 6);
 		break;
 		}
+	case CX23885_BOARD_DVBSKY_T9580: {
+		u8 eeprom[256]; /* 24C02 i2c eeprom */
+
+		if (port->nr > 2)
+			break;
+
+		/* Read entire EEPROM */
+		dev->i2c_bus[0].i2c_client.addr = 0xa0 >> 1;
+		tveeprom_read(&dev->i2c_bus[0].i2c_client, eeprom,
+				sizeof(eeprom));
+		printk(KERN_INFO "DVBSky T9580 port %d MAC address: %pM\n",
+			port->nr, eeprom + 0xc0 + (port->nr-1) * 8);
+		memcpy(port->frontends.adapter.proposed_mac, eeprom + 0xc0 +
+			(port->nr-1) * 8, 6);
+		break;
+		}
 	}
 
 	return ret;
 
 frontend_detach:
 	port->gate_ctrl = NULL;
-	videobuf_dvb_dealloc_frontends(&port->frontends);
+	vb2_dvb_dealloc_frontends(&port->frontends);
 	return -EINVAL;
 }
 
 int cx23885_dvb_register(struct cx23885_tsport *port)
 {
 
-	struct videobuf_dvb_frontend *fe0;
+	struct vb2_dvb_frontend *fe0;
 	struct cx23885_dev *dev = port->dev;
 	int err, i;
 
@@ -1605,13 +1797,15 @@ int cx23885_dvb_register(struct cx23885_tsport *port)
 		port->num_frontends);
 
 	for (i = 1; i <= port->num_frontends; i++) {
-		if (videobuf_dvb_alloc_frontend(
+		struct vb2_queue *q;
+
+		if (vb2_dvb_alloc_frontend(
 			&port->frontends, i) == NULL) {
 			printk(KERN_ERR "%s() failed to alloc\n", __func__);
 			return -ENOMEM;
 		}
 
-		fe0 = videobuf_dvb_get_frontend(&port->frontends, i);
+		fe0 = vb2_dvb_get_frontend(&port->frontends, i);
 		if (!fe0)
 			err = -EINVAL;
 
@@ -1627,10 +1821,21 @@ int cx23885_dvb_register(struct cx23885_tsport *port)
 		/* dvb stuff */
 		/* We have to init the queue for each frontend on a port. */
 		printk(KERN_INFO "%s: cx23885 based dvb card\n", dev->name);
-		videobuf_queue_sg_init(&fe0->dvb.dvbq, &dvb_qops,
-			    &dev->pci->dev, &port->slock,
-			    V4L2_BUF_TYPE_VIDEO_CAPTURE, V4L2_FIELD_TOP,
-			    sizeof(struct cx23885_buffer), port, NULL);
+		q = &fe0->dvb.dvbq;
+		q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+		q->gfp_flags = GFP_DMA32;
+		q->min_buffers_needed = 2;
+		q->drv_priv = port;
+		q->buf_struct_size = sizeof(struct cx23885_buffer);
+		q->ops = &dvb_qops;
+		q->mem_ops = &vb2_dma_sg_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->lock = &dev->lock;
+
+		err = vb2_queue_init(q);
+		if (err < 0)
+			return err;
 	}
 	err = dvb_register(port);
 	if (err != 0)
@@ -1642,18 +1847,27 @@ int cx23885_dvb_register(struct cx23885_tsport *port)
 
 int cx23885_dvb_unregister(struct cx23885_tsport *port)
 {
-	struct videobuf_dvb_frontend *fe0;
-
-	/* FIXME: in an error condition where the we have
-	 * an expected number of frontends (attach problem)
-	 * then this might not clean up correctly, if 1
-	 * is invalid.
-	 * This comment only applies to future boards IF they
-	 * implement MFE support.
-	 */
-	fe0 = videobuf_dvb_get_frontend(&port->frontends, 1);
+	struct vb2_dvb_frontend *fe0;
+	struct i2c_client *client;
+
+	/* remove I2C client for tuner */
+	client = port->i2c_client_tuner;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
+	/* remove I2C client for demodulator */
+	client = port->i2c_client_demod;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
+	fe0 = vb2_dvb_get_frontend(&port->frontends, 1);
+
 	if (fe0 && fe0->dvb.frontend)
-		videobuf_dvb_unregister_bus(&port->frontends);
+		vb2_dvb_unregister_bus(&port->frontends);
 
 	switch (port->dev->board) {
 	case CX23885_BOARD_NETUP_DUAL_DVBS2_CI:
@@ -1668,4 +1882,3 @@ int cx23885_dvb_unregister(struct cx23885_tsport *port)
 
 	return 0;
 }
-
diff --git a/drivers/media/pci/cx23885/cx23885-f300.c b/drivers/media/pci/cx23885/cx23885-f300.c
index 5444cc526008..6f817d8732da 100644
--- a/drivers/media/pci/cx23885/cx23885-f300.c
+++ b/drivers/media/pci/cx23885/cx23885-f300.c
@@ -22,10 +22,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include "cx23885.h"
diff --git a/drivers/media/pci/cx23885/cx23885-i2c.c b/drivers/media/pci/cx23885/cx23885-i2c.c
index 4887314339cb..fd71306af6e2 100644
--- a/drivers/media/pci/cx23885/cx23885-i2c.c
+++ b/drivers/media/pci/cx23885/cx23885-i2c.c
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/module.h>
@@ -386,11 +382,3 @@ void cx23885_av_clk(struct cx23885_dev *dev, int enable)
 
 	i2c_xfer(&dev->i2c_bus[2].i2c_adap, &msg, 1);
 }
-
-/* ----------------------------------------------------------------------- */
-
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
diff --git a/drivers/media/pci/cx23885/cx23885-input.c b/drivers/media/pci/cx23885/cx23885-input.c
index 1940c18e186c..9d37fe661691 100644
--- a/drivers/media/pci/cx23885/cx23885-input.c
+++ b/drivers/media/pci/cx23885/cx23885-input.c
@@ -28,11 +28,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #include <linux/slab.h>
diff --git a/drivers/media/pci/cx23885/cx23885-input.h b/drivers/media/pci/cx23885/cx23885-input.h
index 87dc44e69977..6199c7e86e83 100644
--- a/drivers/media/pci/cx23885/cx23885-input.h
+++ b/drivers/media/pci/cx23885/cx23885-input.h
@@ -14,11 +14,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #ifndef _CX23885_INPUT_H_
diff --git a/drivers/media/pci/cx23885/cx23885-ioctl.c b/drivers/media/pci/cx23885/cx23885-ioctl.c
index 271d69d1ca8c..d2cdd40f79f5 100644
--- a/drivers/media/pci/cx23885/cx23885-ioctl.c
+++ b/drivers/media/pci/cx23885/cx23885-ioctl.c
@@ -15,10 +15,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include "cx23885.h"
@@ -28,7 +24,7 @@
 int cx23885_g_chip_info(struct file *file, void *fh,
 			 struct v4l2_dbg_chip_info *chip)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)fh)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (chip->match.addr > 1)
 		return -EINVAL;
@@ -64,7 +60,7 @@ static int cx23417_g_register(struct cx23885_dev *dev,
 int cx23885_g_register(struct file *file, void *fh,
 		       struct v4l2_dbg_register *reg)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)fh)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (reg->match.addr > 1)
 		return -EINVAL;
@@ -96,7 +92,7 @@ static int cx23417_s_register(struct cx23885_dev *dev,
 int cx23885_s_register(struct file *file, void *fh,
 		       const struct v4l2_dbg_register *reg)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)fh)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (reg->match.addr > 1)
 		return -EINVAL;
diff --git a/drivers/media/pci/cx23885/cx23885-ioctl.h b/drivers/media/pci/cx23885/cx23885-ioctl.h
index 92d9f0774366..cc5dbb6c1afc 100644
--- a/drivers/media/pci/cx23885/cx23885-ioctl.h
+++ b/drivers/media/pci/cx23885/cx23885-ioctl.h
@@ -15,10 +15,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #ifndef _CX23885_IOCTL_H_
diff --git a/drivers/media/pci/cx23885/cx23885-ir.c b/drivers/media/pci/cx23885/cx23885-ir.c
index bfef19359291..89dc4cc3e1ce 100644
--- a/drivers/media/pci/cx23885/cx23885-ir.c
+++ b/drivers/media/pci/cx23885/cx23885-ir.c
@@ -14,11 +14,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #include <media/v4l2-device.h>
diff --git a/drivers/media/pci/cx23885/cx23885-ir.h b/drivers/media/pci/cx23885/cx23885-ir.h
index 0c9d8bda9e28..8e93d1f10ae0 100644
--- a/drivers/media/pci/cx23885/cx23885-ir.h
+++ b/drivers/media/pci/cx23885/cx23885-ir.h
@@ -14,11 +14,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #ifndef _CX23885_IR_H_
diff --git a/drivers/media/pci/cx23885/cx23885-reg.h b/drivers/media/pci/cx23885/cx23885-reg.h
index a99936e0cbc2..2d3cbafe2402 100644
--- a/drivers/media/pci/cx23885/cx23885-reg.h
+++ b/drivers/media/pci/cx23885/cx23885-reg.h
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #ifndef _CX23885_REG_H_
diff --git a/drivers/media/pci/cx23885/cx23885-vbi.c b/drivers/media/pci/cx23885/cx23885-vbi.c
index a1154f035bc1..a7c6ef8f3ea3 100644
--- a/drivers/media/pci/cx23885/cx23885-vbi.c
+++ b/drivers/media/pci/cx23885/cx23885-vbi.c
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/kernel.h>
@@ -42,33 +38,32 @@ MODULE_PARM_DESC(vbi_debug, "enable debug messages [vbi]");
 /* ------------------------------------------------------------------ */
 
 #define VBI_LINE_LENGTH 1440
-#define NTSC_VBI_START_LINE 10        /* line 10 - 21 */
-#define NTSC_VBI_END_LINE   21
-#define NTSC_VBI_LINES      (NTSC_VBI_END_LINE - NTSC_VBI_START_LINE + 1)
+#define VBI_NTSC_LINE_COUNT 12
+#define VBI_PAL_LINE_COUNT 18
 
 
 int cx23885_vbi_fmt(struct file *file, void *priv,
 	struct v4l2_format *f)
 {
-	struct cx23885_fh *fh = priv;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
+	f->fmt.vbi.sampling_rate = 27000000;
+	f->fmt.vbi.samples_per_line = VBI_LINE_LENGTH;
+	f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
+	f->fmt.vbi.offset = 0;
+	f->fmt.vbi.flags = 0;
 	if (dev->tvnorm & V4L2_STD_525_60) {
 		/* ntsc */
-		f->fmt.vbi.samples_per_line = VBI_LINE_LENGTH;
-		f->fmt.vbi.sampling_rate = 27000000;
-		f->fmt.vbi.sample_format = V4L2_PIX_FMT_GREY;
-		f->fmt.vbi.offset = 0;
-		f->fmt.vbi.flags = 0;
-		f->fmt.vbi.start[0] = 10;
-		f->fmt.vbi.count[0] = 17;
-		f->fmt.vbi.start[1] = 263 + 10 + 1;
-		f->fmt.vbi.count[1] = 17;
+		f->fmt.vbi.start[0] = V4L2_VBI_ITU_525_F1_START + 9;
+		f->fmt.vbi.start[1] = V4L2_VBI_ITU_525_F2_START + 9;
+		f->fmt.vbi.count[0] = VBI_NTSC_LINE_COUNT;
+		f->fmt.vbi.count[1] = VBI_NTSC_LINE_COUNT;
 	} else if (dev->tvnorm & V4L2_STD_625_50) {
 		/* pal */
-		f->fmt.vbi.sampling_rate = 35468950;
-		f->fmt.vbi.start[0] = 7 - 1;
-		f->fmt.vbi.start[1] = 319 - 1;
+		f->fmt.vbi.start[0] = V4L2_VBI_ITU_625_F1_START + 5;
+		f->fmt.vbi.start[1] = V4L2_VBI_ITU_625_F2_START + 5;
+		f->fmt.vbi.count[0] = VBI_PAL_LINE_COUNT;
+		f->fmt.vbi.count[1] = VBI_PAL_LINE_COUNT;
 	}
 
 	return 0;
@@ -94,15 +89,6 @@ int cx23885_vbi_irq(struct cx23885_dev *dev, u32 status)
 		handled++;
 	}
 
-	if (status & VID_BC_MSK_VBI_RISCI2) {
-		dprintk(1, "%s() VID_BC_MSK_VBI_RISCI2\n", __func__);
-		dprintk(2, "stopper vbi\n");
-		spin_lock(&dev->slock);
-		cx23885_restart_vbi_queue(dev, &dev->vbiq);
-		spin_unlock(&dev->slock);
-		handled++;
-	}
-
 	return handled;
 }
 
@@ -114,13 +100,13 @@ static int cx23885_start_vbi_dma(struct cx23885_dev    *dev,
 
 	/* setup fifo + format */
 	cx23885_sram_channel_setup(dev, &dev->sram_channels[SRAM_CH02],
-				buf->vb.width, buf->risc.dma);
+				VBI_LINE_LENGTH, buf->risc.dma);
 
 	/* reset counter */
 	cx_write(VID_A_GPCNT_CTL, 3);
 	cx_write(VID_A_VBI_CTRL, 3);
 	cx_write(VBI_A_GPCNT_CTL, 3);
-	q->count = 1;
+	q->count = 0;
 
 	/* enable irq */
 	cx23885_irq_add_enable(dev, 0x01);
@@ -133,163 +119,153 @@ static int cx23885_start_vbi_dma(struct cx23885_dev    *dev,
 	return 0;
 }
 
+/* ------------------------------------------------------------------ */
 
-int cx23885_restart_vbi_queue(struct cx23885_dev    *dev,
-			     struct cx23885_dmaqueue *q)
+static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct cx23885_buffer *buf;
-	struct list_head *item;
-
-	if (list_empty(&q->active))
-		return 0;
-
-	buf = list_entry(q->active.next, struct cx23885_buffer, vb.queue);
-	dprintk(2, "restart_queue [%p/%d]: restart dma\n",
-		buf, buf->vb.i);
-	cx23885_start_vbi_dma(dev, q, buf);
-	list_for_each(item, &q->active) {
-		buf = list_entry(item, struct cx23885_buffer, vb.queue);
-		buf->count = q->count++;
-	}
-	mod_timer(&q->timeout, jiffies + (BUFFER_TIMEOUT / 30));
+	struct cx23885_dev *dev = q->drv_priv;
+	unsigned lines = VBI_PAL_LINE_COUNT;
+
+	if (dev->tvnorm & V4L2_STD_525_60)
+		lines = VBI_NTSC_LINE_COUNT;
+	*num_planes = 1;
+	sizes[0] = lines * VBI_LINE_LENGTH * 2;
 	return 0;
 }
 
-void cx23885_vbi_timeout(unsigned long data)
+static int buffer_prepare(struct vb2_buffer *vb)
 {
-	struct cx23885_dev *dev = (struct cx23885_dev *)data;
-	struct cx23885_dmaqueue *q = &dev->vbiq;
-	struct cx23885_buffer *buf;
-	unsigned long flags;
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer *buf = container_of(vb,
+		struct cx23885_buffer, vb);
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+	unsigned lines = VBI_PAL_LINE_COUNT;
+	int ret;
 
-	/* Stop the VBI engine */
-	cx_clear(VID_A_DMA_CTL, 0x22);
+	if (dev->tvnorm & V4L2_STD_525_60)
+		lines = VBI_NTSC_LINE_COUNT;
 
-	spin_lock_irqsave(&dev->slock, flags);
-	while (!list_empty(&q->active)) {
-		buf = list_entry(q->active.next, struct cx23885_buffer,
-			vb.queue);
-		list_del(&buf->vb.queue);
-		buf->vb.state = VIDEOBUF_ERROR;
-		wake_up(&buf->vb.done);
-		printk("%s/0: [%p/%d] timeout - dma=0x%08lx\n", dev->name,
-		       buf, buf->vb.i, (unsigned long)buf->risc.dma);
-	}
-	cx23885_restart_vbi_queue(dev, q);
-	spin_unlock_irqrestore(&dev->slock, flags);
-}
+	if (vb2_plane_size(vb, 0) < lines * VBI_LINE_LENGTH * 2)
+		return -EINVAL;
+	vb2_set_plane_payload(vb, 0, lines * VBI_LINE_LENGTH * 2);
 
-/* ------------------------------------------------------------------ */
-#define VBI_LINE_LENGTH 1440
-#define VBI_LINE_COUNT 17
+	ret = dma_map_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
+	if (!ret)
+		return -EIO;
 
-static int
-vbi_setup(struct videobuf_queue *q, unsigned int *count, unsigned int *size)
-{
-	*size = VBI_LINE_COUNT * VBI_LINE_LENGTH * 2;
-	if (0 == *count)
-		*count = vbibufs;
-	if (*count < 2)
-		*count = 2;
-	if (*count > 32)
-		*count = 32;
+	cx23885_risc_vbibuffer(dev->pci, &buf->risc,
+			 sgt->sgl,
+			 0, VBI_LINE_LENGTH * lines,
+			 VBI_LINE_LENGTH, 0,
+			 lines);
 	return 0;
 }
 
-static int
-vbi_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
-	    enum v4l2_field field)
+static void buffer_finish(struct vb2_buffer *vb)
 {
-	struct cx23885_fh *fh  = q->priv_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
 	struct cx23885_buffer *buf = container_of(vb,
 		struct cx23885_buffer, vb);
-	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
-	unsigned int size;
-	int rc;
-
-	size = VBI_LINE_COUNT * VBI_LINE_LENGTH * 2;
-	if (0 != buf->vb.baddr  &&  buf->vb.bsize < size)
-		return -EINVAL;
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 
-	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
-		buf->vb.width  = VBI_LINE_LENGTH;
-		buf->vb.height = VBI_LINE_COUNT;
-		buf->vb.size   = size;
-		buf->vb.field  = V4L2_FIELD_SEQ_TB;
-
-		rc = videobuf_iolock(q, &buf->vb, NULL);
-		if (0 != rc)
-			goto fail;
-		cx23885_risc_vbibuffer(dev->pci, &buf->risc,
-				 dma->sglist,
-				 0, buf->vb.width * buf->vb.height,
-				 buf->vb.width, 0,
-				 buf->vb.height);
-	}
-	buf->vb.state = VIDEOBUF_PREPARED;
-	return 0;
+	cx23885_free_buffer(vb->vb2_queue->drv_priv, buf);
 
- fail:
-	cx23885_free_buffer(q, buf);
-	return rc;
+	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
-static void
-vbi_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+/*
+ * The risc program for each buffer works as follows: it starts with a simple
+ * 'JUMP to addr + 12', which is effectively a NOP. Then the code to DMA the
+ * buffer follows and at the end we have a JUMP back to the start + 12 (skipping
+ * the initial JUMP).
+ *
+ * This is the risc program of the first buffer to be queued if the active list
+ * is empty and it just keeps DMAing this buffer without generating any
+ * interrupts.
+ *
+ * If a new buffer is added then the initial JUMP in the code for that buffer
+ * will generate an interrupt which signals that the previous buffer has been
+ * DMAed successfully and that it can be returned to userspace.
+ *
+ * It also sets the final jump of the previous buffer to the start of the new
+ * buffer, thus chaining the new buffer into the DMA chain. This is a single
+ * atomic u32 write, so there is no race condition.
+ *
+ * The end-result of all this that you only get an interrupt when a buffer
+ * is ready, so the control flow is very easy.
+ */
+static void buffer_queue(struct vb2_buffer *vb)
 {
-	struct cx23885_buffer   *buf =
-		container_of(vb, struct cx23885_buffer, vb);
-	struct cx23885_buffer   *prev;
-	struct cx23885_fh       *fh   = vq->priv_data;
-	struct cx23885_dev      *dev  = fh->dev;
-	struct cx23885_dmaqueue *q    = &dev->vbiq;
-
-	/* add jump to stopper */
-	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
-	buf->risc.jmp[1] = cpu_to_le32(q->stopper.dma);
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer *buf = container_of(vb, struct cx23885_buffer, vb);
+	struct cx23885_buffer *prev;
+	struct cx23885_dmaqueue *q = &dev->vbiq;
+	unsigned long flags;
+
+	buf->risc.cpu[1] = cpu_to_le32(buf->risc.dma + 12);
+	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_CNT_INC);
+	buf->risc.jmp[1] = cpu_to_le32(buf->risc.dma + 12);
 	buf->risc.jmp[2] = cpu_to_le32(0); /* bits 63-32 */
 
 	if (list_empty(&q->active)) {
-		list_add_tail(&buf->vb.queue, &q->active);
-		cx23885_start_vbi_dma(dev, q, buf);
-		buf->vb.state = VIDEOBUF_ACTIVE;
-		buf->count    = q->count++;
-		mod_timer(&q->timeout, jiffies + (BUFFER_TIMEOUT / 30));
+		spin_lock_irqsave(&dev->slock, flags);
+		list_add_tail(&buf->queue, &q->active);
+		spin_unlock_irqrestore(&dev->slock, flags);
 		dprintk(2, "[%p/%d] vbi_queue - first active\n",
-			buf, buf->vb.i);
+			buf, buf->vb.v4l2_buf.index);
 
 	} else {
+		buf->risc.cpu[0] |= cpu_to_le32(RISC_IRQ1);
 		prev = list_entry(q->active.prev, struct cx23885_buffer,
-			vb.queue);
-		list_add_tail(&buf->vb.queue, &q->active);
-		buf->vb.state = VIDEOBUF_ACTIVE;
-		buf->count    = q->count++;
+			queue);
+		spin_lock_irqsave(&dev->slock, flags);
+		list_add_tail(&buf->queue, &q->active);
+		spin_unlock_irqrestore(&dev->slock, flags);
 		prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
-		prev->risc.jmp[2] = cpu_to_le32(0); /* Bits 63-32 */
 		dprintk(2, "[%p/%d] buffer_queue - append to active\n",
-			buf, buf->vb.i);
+			buf, buf->vb.v4l2_buf.index);
 	}
 }
 
-static void vbi_release(struct videobuf_queue *q, struct videobuf_buffer *vb)
+static int cx23885_start_streaming(struct vb2_queue *q, unsigned int count)
 {
-	struct cx23885_buffer *buf =
-		container_of(vb, struct cx23885_buffer, vb);
+	struct cx23885_dev *dev = q->drv_priv;
+	struct cx23885_dmaqueue *dmaq = &dev->vbiq;
+	struct cx23885_buffer *buf = list_entry(dmaq->active.next,
+			struct cx23885_buffer, queue);
 
-	cx23885_free_buffer(q, buf);
+	cx23885_start_vbi_dma(dev, dmaq, buf);
+	return 0;
 }
 
-struct videobuf_queue_ops cx23885_vbi_qops = {
-	.buf_setup    = vbi_setup,
-	.buf_prepare  = vbi_prepare,
-	.buf_queue    = vbi_queue,
-	.buf_release  = vbi_release,
-};
+static void cx23885_stop_streaming(struct vb2_queue *q)
+{
+	struct cx23885_dev *dev = q->drv_priv;
+	struct cx23885_dmaqueue *dmaq = &dev->vbiq;
+	unsigned long flags;
 
-/* ------------------------------------------------------------------ */
-/*
- * Local variables:
- * c-basic-offset: 8
- * End:
- */
+	cx_clear(VID_A_DMA_CTL, 0x22); /* FIFO and RISC enable */
+	spin_lock_irqsave(&dev->slock, flags);
+	while (!list_empty(&dmaq->active)) {
+		struct cx23885_buffer *buf = list_entry(dmaq->active.next,
+			struct cx23885_buffer, queue);
+
+		list_del(&buf->queue);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+
+struct vb2_ops cx23885_vbi_qops = {
+	.queue_setup    = queue_setup,
+	.buf_prepare  = buffer_prepare,
+	.buf_finish = buffer_finish,
+	.buf_queue    = buffer_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = cx23885_start_streaming,
+	.stop_streaming = cx23885_stop_streaming,
+};
diff --git a/drivers/media/pci/cx23885/cx23885-video.c b/drivers/media/pci/cx23885/cx23885-video.c
index 91e4cb457296..682a4f95df6b 100644
--- a/drivers/media/pci/cx23885/cx23885-video.c
+++ b/drivers/media/pci/cx23885/cx23885-video.c
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/init.h>
@@ -35,6 +31,7 @@
 #include "cx23885-video.h"
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
+#include <media/v4l2-event.h>
 #include "cx23885-ioctl.h"
 #include "tuner-xc2028.h"
 
@@ -48,15 +45,12 @@ MODULE_LICENSE("GPL");
 
 static unsigned int video_nr[] = {[0 ... (CX23885_MAXBOARDS - 1)] = UNSET };
 static unsigned int vbi_nr[]   = {[0 ... (CX23885_MAXBOARDS - 1)] = UNSET };
-static unsigned int radio_nr[] = {[0 ... (CX23885_MAXBOARDS - 1)] = UNSET };
 
 module_param_array(video_nr, int, NULL, 0444);
 module_param_array(vbi_nr,   int, NULL, 0444);
-module_param_array(radio_nr, int, NULL, 0444);
 
 MODULE_PARM_DESC(video_nr, "video device numbers");
 MODULE_PARM_DESC(vbi_nr, "vbi device numbers");
-MODULE_PARM_DESC(radio_nr, "radio device numbers");
 
 static unsigned int video_debug;
 module_param(video_debug, int, 0644);
@@ -79,77 +73,14 @@ MODULE_PARM_DESC(vid_limit, "capture memory limit in megabytes");
 /* static data                                                         */
 
 #define FORMAT_FLAGS_PACKED       0x01
-#if 0
-static struct cx23885_fmt formats[] = {
-	{
-		.name     = "8 bpp, gray",
-		.fourcc   = V4L2_PIX_FMT_GREY,
-		.depth    = 8,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "15 bpp RGB, le",
-		.fourcc   = V4L2_PIX_FMT_RGB555,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "15 bpp RGB, be",
-		.fourcc   = V4L2_PIX_FMT_RGB555X,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "16 bpp RGB, le",
-		.fourcc   = V4L2_PIX_FMT_RGB565,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "16 bpp RGB, be",
-		.fourcc   = V4L2_PIX_FMT_RGB565X,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "24 bpp RGB, le",
-		.fourcc   = V4L2_PIX_FMT_BGR24,
-		.depth    = 24,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "32 bpp RGB, le",
-		.fourcc   = V4L2_PIX_FMT_BGR32,
-		.depth    = 32,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "32 bpp RGB, be",
-		.fourcc   = V4L2_PIX_FMT_RGB32,
-		.depth    = 32,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "4:2:2, packed, YUYV",
-		.fourcc   = V4L2_PIX_FMT_YUYV,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-		.name     = "4:2:2, packed, UYVY",
-		.fourcc   = V4L2_PIX_FMT_UYVY,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	},
-};
-#else
 static struct cx23885_fmt formats[] = {
 	{
-#if 0
-		.name     = "4:2:2, packed, UYVY",
-		.fourcc   = V4L2_PIX_FMT_UYVY,
-		.depth    = 16,
-		.flags    = FORMAT_FLAGS_PACKED,
-	}, {
-#endif
 		.name     = "4:2:2, packed, YUYV",
 		.fourcc   = V4L2_PIX_FMT_YUYV,
 		.depth    = 16,
 		.flags    = FORMAT_FLAGS_PACKED,
 	}
 };
-#endif
 
 static struct cx23885_fmt *format_by_fourcc(unsigned int fourcc)
 {
@@ -158,163 +89,27 @@ static struct cx23885_fmt *format_by_fourcc(unsigned int fourcc)
 	for (i = 0; i < ARRAY_SIZE(formats); i++)
 		if (formats[i].fourcc == fourcc)
 			return formats+i;
-
-	printk(KERN_ERR "%s(%c%c%c%c) NOT FOUND\n", __func__,
-		(fourcc & 0xff),
-		((fourcc >> 8) & 0xff),
-		((fourcc >> 16) & 0xff),
-		((fourcc >> 24) & 0xff)
-		);
 	return NULL;
 }
 
 /* ------------------------------------------------------------------- */
 
-static const struct v4l2_queryctrl no_ctl = {
-	.name  = "42",
-	.flags = V4L2_CTRL_FLAG_DISABLED,
-};
-
-static struct cx23885_ctrl cx23885_ctls[] = {
-	/* --- video --- */
-	{
-		.v = {
-			.id            = V4L2_CID_BRIGHTNESS,
-			.name          = "Brightness",
-			.minimum       = 0x00,
-			.maximum       = 0xff,
-			.step          = 1,
-			.default_value = 0x7f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 128,
-		.reg                   = LUMA_CTRL,
-		.mask                  = 0x00ff,
-		.shift                 = 0,
-	}, {
-		.v = {
-			.id            = V4L2_CID_CONTRAST,
-			.name          = "Contrast",
-			.minimum       = 0,
-			.maximum       = 0x7f,
-			.step          = 1,
-			.default_value = 0x3f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 0,
-		.reg                   = LUMA_CTRL,
-		.mask                  = 0xff00,
-		.shift                 = 8,
-	}, {
-		.v = {
-			.id            = V4L2_CID_HUE,
-			.name          = "Hue",
-			.minimum       = -127,
-			.maximum       = 128,
-			.step          = 1,
-			.default_value = 0x0,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 128,
-		.reg                   = CHROMA_CTRL,
-		.mask                  = 0xff0000,
-		.shift                 = 16,
-	}, {
-		/* strictly, this only describes only U saturation.
-		 * V saturation is handled specially through code.
-		 */
-		.v = {
-			.id            = V4L2_CID_SATURATION,
-			.name          = "Saturation",
-			.minimum       = 0,
-			.maximum       = 0x7f,
-			.step          = 1,
-			.default_value = 0x3f,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.off                   = 0,
-		.reg                   = CHROMA_CTRL,
-		.mask                  = 0x00ff,
-		.shift                 = 0,
-	}, {
-	/* --- audio --- */
-		.v = {
-			.id            = V4L2_CID_AUDIO_MUTE,
-			.name          = "Mute",
-			.minimum       = 0,
-			.maximum       = 1,
-			.default_value = 1,
-			.type          = V4L2_CTRL_TYPE_BOOLEAN,
-		},
-		.reg                   = PATH1_CTL1,
-		.mask                  = (0x1f << 24),
-		.shift                 = 24,
-	}, {
-		.v = {
-			.id            = V4L2_CID_AUDIO_VOLUME,
-			.name          = "Volume",
-			.minimum       = 0,
-			.maximum       = 65535,
-			.step          = 65535 / 100,
-			.default_value = 65535,
-			.type          = V4L2_CTRL_TYPE_INTEGER,
-		},
-		.reg                   = PATH1_VOL_CTL,
-		.mask                  = 0xff,
-		.shift                 = 0,
-	}
-};
-static const int CX23885_CTLS = ARRAY_SIZE(cx23885_ctls);
-
-/* Must be sorted from low to high control ID! */
-static const u32 cx23885_user_ctrls[] = {
-	V4L2_CID_USER_CLASS,
-	V4L2_CID_BRIGHTNESS,
-	V4L2_CID_CONTRAST,
-	V4L2_CID_SATURATION,
-	V4L2_CID_HUE,
-	V4L2_CID_AUDIO_VOLUME,
-	V4L2_CID_AUDIO_MUTE,
-	0
-};
-
-static const u32 *ctrl_classes[] = {
-	cx23885_user_ctrls,
-	NULL
-};
-
 void cx23885_video_wakeup(struct cx23885_dev *dev,
 	struct cx23885_dmaqueue *q, u32 count)
 {
 	struct cx23885_buffer *buf;
-	int bc;
-
-	for (bc = 0;; bc++) {
-		if (list_empty(&q->active))
-			break;
-		buf = list_entry(q->active.next,
-				 struct cx23885_buffer, vb.queue);
-
-		/* count comes from the hw and is is 16bit wide --
-		 * this trick handles wrap-arounds correctly for
-		 * up to 32767 buffers in flight... */
-		if ((s16) (count - buf->count) < 0)
-			break;
-
-		v4l2_get_timestamp(&buf->vb.ts);
-		dprintk(2, "[%p/%d] wakeup reg=%d buf=%d\n", buf, buf->vb.i,
-			count, buf->count);
-		buf->vb.state = VIDEOBUF_DONE;
-		list_del(&buf->vb.queue);
-		wake_up(&buf->vb.done);
-	}
+
 	if (list_empty(&q->active))
-		del_timer(&q->timeout);
-	else
-		mod_timer(&q->timeout, jiffies+BUFFER_TIMEOUT);
-	if (bc != 1)
-		printk(KERN_ERR "%s: %d buffers handled (should be 1)\n",
-			__func__, bc);
+		return;
+	buf = list_entry(q->active.next,
+			struct cx23885_buffer, queue);
+
+	buf->vb.v4l2_buf.sequence = q->count++;
+	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+	dprintk(2, "[%p/%d] wakeup reg=%d buf=%d\n", buf, buf->vb.v4l2_buf.index,
+			count, q->count);
+	list_del(&buf->queue);
+	vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
 }
 
 int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm)
@@ -324,6 +119,12 @@ int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm)
 		(unsigned int)norm,
 		v4l2_norm_to_name(norm));
 
+	if (dev->tvnorm != norm) {
+		if (vb2_is_busy(&dev->vb2_vidq) || vb2_is_busy(&dev->vb2_vbiq) ||
+		    vb2_is_busy(&dev->vb2_mpegq))
+			return -EBUSY;
+	}
+
 	dev->tvnorm = norm;
 
 	call_all(dev, video, s_std, norm);
@@ -345,79 +146,13 @@ static struct video_device *cx23885_vdev_init(struct cx23885_dev *dev,
 	*vfd = *template;
 	vfd->v4l2_dev = &dev->v4l2_dev;
 	vfd->release = video_device_release;
+	vfd->lock = &dev->lock;
 	snprintf(vfd->name, sizeof(vfd->name), "%s (%s)",
 		 cx23885_boards[dev->board].name, type);
 	video_set_drvdata(vfd, dev);
 	return vfd;
 }
 
-static int cx23885_ctrl_query(struct v4l2_queryctrl *qctrl)
-{
-	int i;
-
-	if (qctrl->id < V4L2_CID_BASE ||
-	    qctrl->id >= V4L2_CID_LASTP1)
-		return -EINVAL;
-	for (i = 0; i < CX23885_CTLS; i++)
-		if (cx23885_ctls[i].v.id == qctrl->id)
-			break;
-	if (i == CX23885_CTLS) {
-		*qctrl = no_ctl;
-		return 0;
-	}
-	*qctrl = cx23885_ctls[i].v;
-	return 0;
-}
-
-/* ------------------------------------------------------------------- */
-/* resource management                                                 */
-
-static int res_get(struct cx23885_dev *dev, struct cx23885_fh *fh,
-	unsigned int bit)
-{
-	dprintk(1, "%s()\n", __func__);
-	if (fh->resources & bit)
-		/* have it already allocated */
-		return 1;
-
-	/* is it free? */
-	mutex_lock(&dev->lock);
-	if (dev->resources & bit) {
-		/* no, someone else uses it */
-		mutex_unlock(&dev->lock);
-		return 0;
-	}
-	/* it's free, grab it */
-	fh->resources  |= bit;
-	dev->resources |= bit;
-	dprintk(1, "res: get %d\n", bit);
-	mutex_unlock(&dev->lock);
-	return 1;
-}
-
-static int res_check(struct cx23885_fh *fh, unsigned int bit)
-{
-	return fh->resources & bit;
-}
-
-static int res_locked(struct cx23885_dev *dev, unsigned int bit)
-{
-	return dev->resources & bit;
-}
-
-static void res_free(struct cx23885_dev *dev, struct cx23885_fh *fh,
-	unsigned int bits)
-{
-	BUG_ON((fh->resources & bits) != bits);
-	dprintk(1, "%s()\n", __func__);
-
-	mutex_lock(&dev->lock);
-	fh->resources  &= ~bits;
-	dev->resources &= ~bits;
-	dprintk(1, "res: put %d\n", bits);
-	mutex_unlock(&dev->lock);
-}
-
 int cx23885_flatiron_write(struct cx23885_dev *dev, u8 reg, u8 data)
 {
 	/* 8 bit registers, 8 bit values */
@@ -567,7 +302,7 @@ static int cx23885_start_video_dma(struct cx23885_dev *dev,
 
 	/* reset counter */
 	cx_write(VID_A_GPCNT_CTL, 3);
-	q->count = 1;
+	q->count = 0;
 
 	/* enable irq */
 	cx23885_irq_add_enable(dev, 0x01);
@@ -580,479 +315,206 @@ static int cx23885_start_video_dma(struct cx23885_dev *dev,
 	return 0;
 }
 
-
-static int cx23885_restart_video_queue(struct cx23885_dev *dev,
-			       struct cx23885_dmaqueue *q)
-{
-	struct cx23885_buffer *buf, *prev;
-	struct list_head *item;
-	dprintk(1, "%s()\n", __func__);
-
-	if (!list_empty(&q->active)) {
-		buf = list_entry(q->active.next, struct cx23885_buffer,
-			vb.queue);
-		dprintk(2, "restart_queue [%p/%d]: restart dma\n",
-			buf, buf->vb.i);
-		cx23885_start_video_dma(dev, q, buf);
-		list_for_each(item, &q->active) {
-			buf = list_entry(item, struct cx23885_buffer,
-				vb.queue);
-			buf->count    = q->count++;
-		}
-		mod_timer(&q->timeout, jiffies+BUFFER_TIMEOUT);
-		return 0;
-	}
-
-	prev = NULL;
-	for (;;) {
-		if (list_empty(&q->queued))
-			return 0;
-		buf = list_entry(q->queued.next, struct cx23885_buffer,
-			vb.queue);
-		if (NULL == prev) {
-			list_move_tail(&buf->vb.queue, &q->active);
-			cx23885_start_video_dma(dev, q, buf);
-			buf->vb.state = VIDEOBUF_ACTIVE;
-			buf->count    = q->count++;
-			mod_timer(&q->timeout, jiffies+BUFFER_TIMEOUT);
-			dprintk(2, "[%p/%d] restart_queue - first active\n",
-				buf, buf->vb.i);
-
-		} else if (prev->vb.width  == buf->vb.width  &&
-			   prev->vb.height == buf->vb.height &&
-			   prev->fmt       == buf->fmt) {
-			list_move_tail(&buf->vb.queue, &q->active);
-			buf->vb.state = VIDEOBUF_ACTIVE;
-			buf->count    = q->count++;
-			prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
-			prev->risc.jmp[2] = cpu_to_le32(0); /* Bits 63 - 32 */
-			dprintk(2, "[%p/%d] restart_queue - move to active\n",
-				buf, buf->vb.i);
-		} else {
-			return 0;
-		}
-		prev = buf;
-	}
-}
-
-static int buffer_setup(struct videobuf_queue *q, unsigned int *count,
-	unsigned int *size)
+static int queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
 {
-	struct cx23885_fh *fh = q->priv_data;
+	struct cx23885_dev *dev = q->drv_priv;
 
-	*size = fh->fmt->depth*fh->width*fh->height >> 3;
-	if (0 == *count)
-		*count = 32;
-	if (*size * *count > vid_limit * 1024 * 1024)
-		*count = (vid_limit * 1024 * 1024) / *size;
+	*num_planes = 1;
+	sizes[0] = (dev->fmt->depth * dev->width * dev->height) >> 3;
 	return 0;
 }
 
-static int buffer_prepare(struct videobuf_queue *q, struct videobuf_buffer *vb,
-	       enum v4l2_field field)
+static int buffer_prepare(struct vb2_buffer *vb)
 {
-	struct cx23885_fh *fh  = q->priv_data;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
 	struct cx23885_buffer *buf =
 		container_of(vb, struct cx23885_buffer, vb);
-	int rc, init_buffer = 0;
 	u32 line0_offset, line1_offset;
-	struct videobuf_dmabuf *dma = videobuf_to_dma(&buf->vb);
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
 	int field_tff;
+	int ret;
 
-	BUG_ON(NULL == fh->fmt);
-	if (fh->width  < 48 || fh->width  > norm_maxw(dev->tvnorm) ||
-	    fh->height < 32 || fh->height > norm_maxh(dev->tvnorm))
-		return -EINVAL;
-	buf->vb.size = (fh->width * fh->height * fh->fmt->depth) >> 3;
-	if (0 != buf->vb.baddr  &&  buf->vb.bsize < buf->vb.size)
+	buf->bpl = (dev->width * dev->fmt->depth) >> 3;
+
+	if (vb2_plane_size(vb, 0) < dev->height * buf->bpl)
 		return -EINVAL;
+	vb2_set_plane_payload(vb, 0, dev->height * buf->bpl);
 
-	if (buf->fmt       != fh->fmt    ||
-	    buf->vb.width  != fh->width  ||
-	    buf->vb.height != fh->height ||
-	    buf->vb.field  != field) {
-		buf->fmt       = fh->fmt;
-		buf->vb.width  = fh->width;
-		buf->vb.height = fh->height;
-		buf->vb.field  = field;
-		init_buffer = 1;
-	}
+	ret = dma_map_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
+	if (!ret)
+		return -EIO;
 
-	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
-		init_buffer = 1;
-		rc = videobuf_iolock(q, &buf->vb, NULL);
-		if (0 != rc)
-			goto fail;
-	}
+	switch (dev->field) {
+	case V4L2_FIELD_TOP:
+		cx23885_risc_buffer(dev->pci, &buf->risc,
+				sgt->sgl, 0, UNSET,
+				buf->bpl, 0, dev->height);
+		break;
+	case V4L2_FIELD_BOTTOM:
+		cx23885_risc_buffer(dev->pci, &buf->risc,
+				sgt->sgl, UNSET, 0,
+				buf->bpl, 0, dev->height);
+		break;
+	case V4L2_FIELD_INTERLACED:
+		if (dev->tvnorm & V4L2_STD_525_60)
+			/* NTSC or  */
+			field_tff = 1;
+		else
+			field_tff = 0;
+
+		if (cx23885_boards[dev->board].force_bff)
+			/* PAL / SECAM OR 888 in NTSC MODE */
+			field_tff = 0;
 
-	if (init_buffer) {
-		buf->bpl = buf->vb.width * buf->fmt->depth >> 3;
-		switch (buf->vb.field) {
-		case V4L2_FIELD_TOP:
-			cx23885_risc_buffer(dev->pci, &buf->risc,
-					 dma->sglist, 0, UNSET,
-					 buf->bpl, 0, buf->vb.height);
-			break;
-		case V4L2_FIELD_BOTTOM:
-			cx23885_risc_buffer(dev->pci, &buf->risc,
-					 dma->sglist, UNSET, 0,
-					 buf->bpl, 0, buf->vb.height);
-			break;
-		case V4L2_FIELD_INTERLACED:
-			if (dev->tvnorm & V4L2_STD_NTSC)
-				/* NTSC or  */
-				field_tff = 1;
-			else
-				field_tff = 0;
-
-			if (cx23885_boards[dev->board].force_bff)
-				/* PAL / SECAM OR 888 in NTSC MODE */
-				field_tff = 0;
-
-			if (field_tff) {
-				/* cx25840 transmits NTSC bottom field first */
-				dprintk(1, "%s() Creating TFF/NTSC risc\n",
+		if (field_tff) {
+			/* cx25840 transmits NTSC bottom field first */
+			dprintk(1, "%s() Creating TFF/NTSC risc\n",
 					__func__);
-				line0_offset = buf->bpl;
-				line1_offset = 0;
-			} else {
-				/* All other formats are top field first */
-				dprintk(1, "%s() Creating BFF/PAL/SECAM risc\n",
+			line0_offset = buf->bpl;
+			line1_offset = 0;
+		} else {
+			/* All other formats are top field first */
+			dprintk(1, "%s() Creating BFF/PAL/SECAM risc\n",
 					__func__);
-				line0_offset = 0;
-				line1_offset = buf->bpl;
-			}
-			cx23885_risc_buffer(dev->pci, &buf->risc,
-					dma->sglist, line0_offset,
-					line1_offset,
-					buf->bpl, buf->bpl,
-					buf->vb.height >> 1);
-			break;
-		case V4L2_FIELD_SEQ_TB:
-			cx23885_risc_buffer(dev->pci, &buf->risc,
-					 dma->sglist,
-					 0, buf->bpl * (buf->vb.height >> 1),
-					 buf->bpl, 0,
-					 buf->vb.height >> 1);
-			break;
-		case V4L2_FIELD_SEQ_BT:
-			cx23885_risc_buffer(dev->pci, &buf->risc,
-					 dma->sglist,
-					 buf->bpl * (buf->vb.height >> 1), 0,
-					 buf->bpl, 0,
-					 buf->vb.height >> 1);
-			break;
-		default:
-			BUG();
+			line0_offset = 0;
+			line1_offset = buf->bpl;
 		}
+		cx23885_risc_buffer(dev->pci, &buf->risc,
+				sgt->sgl, line0_offset,
+				line1_offset,
+				buf->bpl, buf->bpl,
+				dev->height >> 1);
+		break;
+	case V4L2_FIELD_SEQ_TB:
+		cx23885_risc_buffer(dev->pci, &buf->risc,
+				sgt->sgl,
+				0, buf->bpl * (dev->height >> 1),
+				buf->bpl, 0,
+				dev->height >> 1);
+		break;
+	case V4L2_FIELD_SEQ_BT:
+		cx23885_risc_buffer(dev->pci, &buf->risc,
+				sgt->sgl,
+				buf->bpl * (dev->height >> 1), 0,
+				buf->bpl, 0,
+				dev->height >> 1);
+		break;
+	default:
+		BUG();
 	}
-	dprintk(2, "[%p/%d] buffer_prep - %dx%d %dbpp \"%s\" - dma=0x%08lx\n",
-		buf, buf->vb.i,
-		fh->width, fh->height, fh->fmt->depth, fh->fmt->name,
+	dprintk(2, "[%p/%d] buffer_init - %dx%d %dbpp \"%s\" - dma=0x%08lx\n",
+		buf, buf->vb.v4l2_buf.index,
+		dev->width, dev->height, dev->fmt->depth, dev->fmt->name,
 		(unsigned long)buf->risc.dma);
-
-	buf->vb.state = VIDEOBUF_PREPARED;
 	return 0;
+}
+
+static void buffer_finish(struct vb2_buffer *vb)
+{
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
+	struct cx23885_buffer *buf = container_of(vb,
+		struct cx23885_buffer, vb);
+	struct sg_table *sgt = vb2_dma_sg_plane_desc(vb, 0);
+
+	cx23885_free_buffer(vb->vb2_queue->drv_priv, buf);
 
- fail:
-	cx23885_free_buffer(q, buf);
-	return rc;
+	dma_unmap_sg(&dev->pci->dev, sgt->sgl, sgt->nents, DMA_FROM_DEVICE);
 }
 
-static void buffer_queue(struct videobuf_queue *vq, struct videobuf_buffer *vb)
+/*
+ * The risc program for each buffer works as follows: it starts with a simple
+ * 'JUMP to addr + 12', which is effectively a NOP. Then the code to DMA the
+ * buffer follows and at the end we have a JUMP back to the start + 12 (skipping
+ * the initial JUMP).
+ *
+ * This is the risc program of the first buffer to be queued if the active list
+ * is empty and it just keeps DMAing this buffer without generating any
+ * interrupts.
+ *
+ * If a new buffer is added then the initial JUMP in the code for that buffer
+ * will generate an interrupt which signals that the previous buffer has been
+ * DMAed successfully and that it can be returned to userspace.
+ *
+ * It also sets the final jump of the previous buffer to the start of the new
+ * buffer, thus chaining the new buffer into the DMA chain. This is a single
+ * atomic u32 write, so there is no race condition.
+ *
+ * The end-result of all this that you only get an interrupt when a buffer
+ * is ready, so the control flow is very easy.
+ */
+static void buffer_queue(struct vb2_buffer *vb)
 {
+	struct cx23885_dev *dev = vb->vb2_queue->drv_priv;
 	struct cx23885_buffer   *buf = container_of(vb,
 		struct cx23885_buffer, vb);
 	struct cx23885_buffer   *prev;
-	struct cx23885_fh       *fh   = vq->priv_data;
-	struct cx23885_dev      *dev  = fh->dev;
 	struct cx23885_dmaqueue *q    = &dev->vidq;
+	unsigned long flags;
 
-	/* add jump to stopper */
-	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_IRQ1 | RISC_CNT_INC);
-	buf->risc.jmp[1] = cpu_to_le32(q->stopper.dma);
+	/* add jump to start */
+	buf->risc.cpu[1] = cpu_to_le32(buf->risc.dma + 12);
+	buf->risc.jmp[0] = cpu_to_le32(RISC_JUMP | RISC_CNT_INC);
+	buf->risc.jmp[1] = cpu_to_le32(buf->risc.dma + 12);
 	buf->risc.jmp[2] = cpu_to_le32(0); /* bits 63-32 */
 
-	if (!list_empty(&q->queued)) {
-		list_add_tail(&buf->vb.queue, &q->queued);
-		buf->vb.state = VIDEOBUF_QUEUED;
-		dprintk(2, "[%p/%d] buffer_queue - append to queued\n",
-			buf, buf->vb.i);
-
-	} else if (list_empty(&q->active)) {
-		list_add_tail(&buf->vb.queue, &q->active);
-		cx23885_start_video_dma(dev, q, buf);
-		buf->vb.state = VIDEOBUF_ACTIVE;
-		buf->count    = q->count++;
-		mod_timer(&q->timeout, jiffies+BUFFER_TIMEOUT);
+	spin_lock_irqsave(&dev->slock, flags);
+	if (list_empty(&q->active)) {
+		list_add_tail(&buf->queue, &q->active);
 		dprintk(2, "[%p/%d] buffer_queue - first active\n",
-			buf, buf->vb.i);
-
+			buf, buf->vb.v4l2_buf.index);
 	} else {
+		buf->risc.cpu[0] |= cpu_to_le32(RISC_IRQ1);
 		prev = list_entry(q->active.prev, struct cx23885_buffer,
-			vb.queue);
-		if (prev->vb.width  == buf->vb.width  &&
-		    prev->vb.height == buf->vb.height &&
-		    prev->fmt       == buf->fmt) {
-			list_add_tail(&buf->vb.queue, &q->active);
-			buf->vb.state = VIDEOBUF_ACTIVE;
-			buf->count    = q->count++;
-			prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
-			/* 64 bit bits 63-32 */
-			prev->risc.jmp[2] = cpu_to_le32(0);
-			dprintk(2, "[%p/%d] buffer_queue - append to active\n",
-				buf, buf->vb.i);
-
-		} else {
-			list_add_tail(&buf->vb.queue, &q->queued);
-			buf->vb.state = VIDEOBUF_QUEUED;
-			dprintk(2, "[%p/%d] buffer_queue - first queued\n",
-				buf, buf->vb.i);
-		}
-	}
-}
-
-static void buffer_release(struct videobuf_queue *q,
-	struct videobuf_buffer *vb)
-{
-	struct cx23885_buffer *buf = container_of(vb,
-		struct cx23885_buffer, vb);
-
-	cx23885_free_buffer(q, buf);
-}
-
-static struct videobuf_queue_ops cx23885_video_qops = {
-	.buf_setup    = buffer_setup,
-	.buf_prepare  = buffer_prepare,
-	.buf_queue    = buffer_queue,
-	.buf_release  = buffer_release,
-};
-
-static struct videobuf_queue *get_queue(struct cx23885_fh *fh)
-{
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return &fh->vidq;
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
-		return &fh->vbiq;
-	default:
-		BUG();
-		return NULL;
-	}
-}
-
-static int get_resource(struct cx23885_fh *fh)
-{
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return RESOURCE_VIDEO;
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
-		return RESOURCE_VBI;
-	default:
-		BUG();
-		return 0;
+			queue);
+		list_add_tail(&buf->queue, &q->active);
+		prev->risc.jmp[1] = cpu_to_le32(buf->risc.dma);
+		dprintk(2, "[%p/%d] buffer_queue - append to active\n",
+				buf, buf->vb.v4l2_buf.index);
 	}
+	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static int video_open(struct file *file)
+static int cx23885_start_streaming(struct vb2_queue *q, unsigned int count)
 {
-	struct video_device *vdev = video_devdata(file);
-	struct cx23885_dev *dev = video_drvdata(file);
-	struct cx23885_fh *fh;
-	enum v4l2_buf_type type = 0;
-	int radio = 0;
-
-	switch (vdev->vfl_type) {
-	case VFL_TYPE_GRABBER:
-		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-		break;
-	case VFL_TYPE_VBI:
-		type = V4L2_BUF_TYPE_VBI_CAPTURE;
-		break;
-	case VFL_TYPE_RADIO:
-		radio = 1;
-		break;
-	}
-
-	dprintk(1, "open dev=%s radio=%d type=%s\n",
-		video_device_node_name(vdev), radio, v4l2_type_names[type]);
-
-	/* allocate + initialize per filehandle data */
-	fh = kzalloc(sizeof(*fh), GFP_KERNEL);
-	if (NULL == fh)
-		return -ENOMEM;
-
-	file->private_data = fh;
-	fh->dev      = dev;
-	fh->radio    = radio;
-	fh->type     = type;
-	fh->width    = 320;
-	fh->height   = 240;
-	fh->fmt      = format_by_fourcc(V4L2_PIX_FMT_YUYV);
-
-	videobuf_queue_sg_init(&fh->vidq, &cx23885_video_qops,
-			    &dev->pci->dev, &dev->slock,
-			    V4L2_BUF_TYPE_VIDEO_CAPTURE,
-			    V4L2_FIELD_INTERLACED,
-			    sizeof(struct cx23885_buffer),
-			    fh, NULL);
-
-	videobuf_queue_sg_init(&fh->vbiq, &cx23885_vbi_qops,
-		&dev->pci->dev, &dev->slock,
-		V4L2_BUF_TYPE_VBI_CAPTURE,
-		V4L2_FIELD_SEQ_TB,
-		sizeof(struct cx23885_buffer),
-		fh, NULL);
-
-
-	dprintk(1, "post videobuf_queue_init()\n");
+	struct cx23885_dev *dev = q->drv_priv;
+	struct cx23885_dmaqueue *dmaq = &dev->vidq;
+	struct cx23885_buffer *buf = list_entry(dmaq->active.next,
+			struct cx23885_buffer, queue);
 
+	cx23885_start_video_dma(dev, dmaq, buf);
 	return 0;
 }
 
-static ssize_t video_read(struct file *file, char __user *data,
-	size_t count, loff_t *ppos)
+static void cx23885_stop_streaming(struct vb2_queue *q)
 {
-	struct cx23885_fh *fh = file->private_data;
-
-	switch (fh->type) {
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		if (res_locked(fh->dev, RESOURCE_VIDEO))
-			return -EBUSY;
-		return videobuf_read_one(&fh->vidq, data, count, ppos,
-					 file->f_flags & O_NONBLOCK);
-	case V4L2_BUF_TYPE_VBI_CAPTURE:
-		if (!res_get(fh->dev, fh, RESOURCE_VBI))
-			return -EBUSY;
-		return videobuf_read_stream(&fh->vbiq, data, count, ppos, 1,
-					    file->f_flags & O_NONBLOCK);
-	default:
-		BUG();
-		return 0;
-	}
-}
-
-static unsigned int video_poll(struct file *file,
-	struct poll_table_struct *wait)
-{
-	struct cx23885_fh *fh = file->private_data;
-	struct cx23885_buffer *buf;
-	unsigned int rc = POLLERR;
-
-	if (V4L2_BUF_TYPE_VBI_CAPTURE == fh->type) {
-		if (!res_get(fh->dev, fh, RESOURCE_VBI))
-			return POLLERR;
-		return videobuf_poll_stream(file, &fh->vbiq, wait);
-	}
-
-	mutex_lock(&fh->vidq.vb_lock);
-	if (res_check(fh, RESOURCE_VIDEO)) {
-		/* streaming capture */
-		if (list_empty(&fh->vidq.stream))
-			goto done;
-		buf = list_entry(fh->vidq.stream.next,
-			struct cx23885_buffer, vb.stream);
-	} else {
-		/* read() capture */
-		buf = (struct cx23885_buffer *)fh->vidq.read_buf;
-		if (NULL == buf)
-			goto done;
-	}
-	poll_wait(file, &buf->vb.done, wait);
-	if (buf->vb.state == VIDEOBUF_DONE ||
-	    buf->vb.state == VIDEOBUF_ERROR)
-		rc =  POLLIN|POLLRDNORM;
-	else
-		rc = 0;
-done:
-	mutex_unlock(&fh->vidq.vb_lock);
-	return rc;
-}
-
-static int video_release(struct file *file)
-{
-	struct cx23885_fh *fh = file->private_data;
-	struct cx23885_dev *dev = fh->dev;
-
-	/* turn off overlay */
-	if (res_check(fh, RESOURCE_OVERLAY)) {
-		/* FIXME */
-		res_free(dev, fh, RESOURCE_OVERLAY);
-	}
+	struct cx23885_dev *dev = q->drv_priv;
+	struct cx23885_dmaqueue *dmaq = &dev->vidq;
+	unsigned long flags;
 
-	/* stop video capture */
-	if (res_check(fh, RESOURCE_VIDEO)) {
-		videobuf_queue_cancel(&fh->vidq);
-		res_free(dev, fh, RESOURCE_VIDEO);
-	}
-	if (fh->vidq.read_buf) {
-		buffer_release(&fh->vidq, fh->vidq.read_buf);
-		kfree(fh->vidq.read_buf);
-	}
+	cx_clear(VID_A_DMA_CTL, 0x11);
+	spin_lock_irqsave(&dev->slock, flags);
+	while (!list_empty(&dmaq->active)) {
+		struct cx23885_buffer *buf = list_entry(dmaq->active.next,
+			struct cx23885_buffer, queue);
 
-	/* stop vbi capture */
-	if (res_check(fh, RESOURCE_VBI)) {
-		if (fh->vbiq.streaming)
-			videobuf_streamoff(&fh->vbiq);
-		if (fh->vbiq.reading)
-			videobuf_read_stop(&fh->vbiq);
-		res_free(dev, fh, RESOURCE_VBI);
+		list_del(&buf->queue);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 	}
-
-	videobuf_mmap_free(&fh->vidq);
-	videobuf_mmap_free(&fh->vbiq);
-
-	file->private_data = NULL;
-	kfree(fh);
-
-	/* We are not putting the tuner to sleep here on exit, because
-	 * we want to use the mpeg encoder in another session to capture
-	 * tuner video. Closing this will result in no video to the encoder.
-	 */
-
-	return 0;
-}
-
-static int video_mmap(struct file *file, struct vm_area_struct *vma)
-{
-	struct cx23885_fh *fh = file->private_data;
-
-	return videobuf_mmap_mapper(get_queue(fh), vma);
-}
-
-/* ------------------------------------------------------------------ */
-/* VIDEO CTRL IOCTLS                                                  */
-
-int cx23885_get_control(struct cx23885_dev *dev,
-	struct v4l2_control *ctl)
-{
-	dprintk(1, "%s() calling cx25840(VIDIOC_G_CTRL)\n", __func__);
-	call_all(dev, core, g_ctrl, ctl);
-	return 0;
-}
-
-int cx23885_set_control(struct cx23885_dev *dev,
-	struct v4l2_control *ctl)
-{
-	dprintk(1, "%s() calling cx25840(VIDIOC_S_CTRL)\n", __func__);
-	call_all(dev, core, s_ctrl, ctl);
-
-	return 0;
+	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
-static void init_controls(struct cx23885_dev *dev)
-{
-	struct v4l2_control ctrl;
-	int i;
-
-	for (i = 0; i < CX23885_CTLS; i++) {
-		ctrl.id = cx23885_ctls[i].v.id;
-		ctrl.value = cx23885_ctls[i].v.default_value;
-
-		cx23885_set_control(dev, &ctrl);
-	}
-}
+static struct vb2_ops cx23885_video_qops = {
+	.queue_setup    = queue_setup,
+	.buf_prepare  = buffer_prepare,
+	.buf_finish = buffer_finish,
+	.buf_queue    = buffer_queue,
+	.wait_prepare = vb2_ops_wait_prepare,
+	.wait_finish = vb2_ops_wait_finish,
+	.start_streaming = cx23885_start_streaming,
+	.stop_streaming = cx23885_stop_streaming,
+};
 
 /* ------------------------------------------------------------------ */
 /* VIDEO IOCTLS                                                       */
@@ -1060,16 +522,17 @@ static void init_controls(struct cx23885_dev *dev)
 static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 	struct v4l2_format *f)
 {
-	struct cx23885_fh *fh   = priv;
+	struct cx23885_dev *dev = video_drvdata(file);
 
-	f->fmt.pix.width        = fh->width;
-	f->fmt.pix.height       = fh->height;
-	f->fmt.pix.field        = fh->vidq.field;
-	f->fmt.pix.pixelformat  = fh->fmt->fourcc;
+	f->fmt.pix.width        = dev->width;
+	f->fmt.pix.height       = dev->height;
+	f->fmt.pix.field        = dev->field;
+	f->fmt.pix.pixelformat  = dev->fmt->fourcc;
 	f->fmt.pix.bytesperline =
-		(f->fmt.pix.width * fh->fmt->depth) >> 3;
+		(f->fmt.pix.width * dev->fmt->depth) >> 3;
 	f->fmt.pix.sizeimage =
 		f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace   = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
@@ -1077,7 +540,7 @@ static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
 static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 	struct v4l2_format *f)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	struct cx23885_fmt *fmt;
 	enum v4l2_field   field;
 	unsigned int      maxw, maxh;
@@ -1102,9 +565,12 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		maxh = maxh / 2;
 		break;
 	case V4L2_FIELD_INTERLACED:
+	case V4L2_FIELD_SEQ_TB:
+	case V4L2_FIELD_SEQ_BT:
 		break;
 	default:
-		return -EINVAL;
+		field = V4L2_FIELD_INTERLACED;
+		break;
 	}
 
 	f->fmt.pix.field = field;
@@ -1114,6 +580,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 		(f->fmt.pix.width * fmt->depth) >> 3;
 	f->fmt.pix.sizeimage =
 		f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
 
 	return 0;
 }
@@ -1121,8 +588,7 @@ static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
 static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	struct v4l2_format *f)
 {
-	struct cx23885_fh *fh = priv;
-	struct cx23885_dev *dev  = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	struct v4l2_mbus_framefmt mbus_fmt;
 	int err;
 
@@ -1131,34 +597,44 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 
 	if (0 != err)
 		return err;
-	fh->fmt        = format_by_fourcc(f->fmt.pix.pixelformat);
-	fh->width      = f->fmt.pix.width;
-	fh->height     = f->fmt.pix.height;
-	fh->vidq.field = f->fmt.pix.field;
+
+	if (vb2_is_busy(&dev->vb2_vidq) || vb2_is_busy(&dev->vb2_vbiq) ||
+	    vb2_is_busy(&dev->vb2_mpegq))
+		return -EBUSY;
+
+	dev->fmt        = format_by_fourcc(f->fmt.pix.pixelformat);
+	dev->width      = f->fmt.pix.width;
+	dev->height     = f->fmt.pix.height;
+	dev->field	= f->fmt.pix.field;
 	dprintk(2, "%s() width=%d height=%d field=%d\n", __func__,
-		fh->width, fh->height, fh->vidq.field);
+		dev->width, dev->height, dev->field);
 	v4l2_fill_mbus_format(&mbus_fmt, &f->fmt.pix, V4L2_MBUS_FMT_FIXED);
 	call_all(dev, video, s_mbus_fmt, &mbus_fmt);
 	v4l2_fill_pix_format(&f->fmt.pix, &mbus_fmt);
+	/* s_mbus_fmt overwrites f->fmt.pix.field, restore it */
+	f->fmt.pix.field = dev->field;
 	return 0;
 }
 
 static int vidioc_querycap(struct file *file, void  *priv,
 	struct v4l2_capability *cap)
 {
-	struct cx23885_dev *dev  = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
 
 	strcpy(cap->driver, "cx23885");
 	strlcpy(cap->card, cx23885_boards[dev->board].name,
 		sizeof(cap->card));
 	sprintf(cap->bus_info, "PCIe:%s", pci_name(dev->pci));
-	cap->capabilities =
-		V4L2_CAP_VIDEO_CAPTURE |
-		V4L2_CAP_READWRITE     |
-		V4L2_CAP_STREAMING     |
-		V4L2_CAP_VBI_CAPTURE;
+	cap->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_STREAMING | V4L2_CAP_AUDIO;
 	if (dev->tuner_type != TUNER_ABSENT)
-		cap->capabilities |= V4L2_CAP_TUNER;
+		cap->device_caps |= V4L2_CAP_TUNER;
+	if (vdev->vfl_type == VFL_TYPE_VBI)
+		cap->device_caps |= V4L2_CAP_VBI_CAPTURE;
+	else
+		cap->device_caps |= V4L2_CAP_VIDEO_CAPTURE;
+	cap->capabilities = cap->device_caps | V4L2_CAP_VBI_CAPTURE |
+		V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_DEVICE_CAPS;
 	return 0;
 }
 
@@ -1175,85 +651,9 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
 	return 0;
 }
 
-static int vidioc_reqbufs(struct file *file, void *priv,
-	struct v4l2_requestbuffers *p)
-{
-	struct cx23885_fh *fh = priv;
-	return videobuf_reqbufs(get_queue(fh), p);
-}
-
-static int vidioc_querybuf(struct file *file, void *priv,
-	struct v4l2_buffer *p)
-{
-	struct cx23885_fh *fh = priv;
-	return videobuf_querybuf(get_queue(fh), p);
-}
-
-static int vidioc_qbuf(struct file *file, void *priv,
-	struct v4l2_buffer *p)
-{
-	struct cx23885_fh *fh = priv;
-	return videobuf_qbuf(get_queue(fh), p);
-}
-
-static int vidioc_dqbuf(struct file *file, void *priv,
-	struct v4l2_buffer *p)
-{
-	struct cx23885_fh *fh = priv;
-	return videobuf_dqbuf(get_queue(fh), p,
-				file->f_flags & O_NONBLOCK);
-}
-
-static int vidioc_streamon(struct file *file, void *priv,
-	enum v4l2_buf_type i)
-{
-	struct cx23885_fh *fh = priv;
-	struct cx23885_dev *dev = fh->dev;
-	dprintk(1, "%s()\n", __func__);
-
-	if ((fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
-		(fh->type != V4L2_BUF_TYPE_VBI_CAPTURE))
-		return -EINVAL;
-	if (unlikely(i != fh->type))
-		return -EINVAL;
-
-	if (unlikely(!res_get(dev, fh, get_resource(fh))))
-		return -EBUSY;
-
-	/* Don't start VBI streaming unless vida streaming
-	 * has already started.
-	 */
-	if ((fh->type == V4L2_BUF_TYPE_VBI_CAPTURE) &&
-		((cx_read(VID_A_DMA_CTL) & 0x11) == 0))
-		return -EINVAL;
-
-	return videobuf_streamon(get_queue(fh));
-}
-
-static int vidioc_streamoff(struct file *file, void *priv, enum v4l2_buf_type i)
-{
-	struct cx23885_fh *fh = priv;
-	struct cx23885_dev *dev = fh->dev;
-	int err, res;
-	dprintk(1, "%s()\n", __func__);
-
-	if ((fh->type != V4L2_BUF_TYPE_VIDEO_CAPTURE) &&
-		(fh->type != V4L2_BUF_TYPE_VBI_CAPTURE))
-		return -EINVAL;
-	if (i != fh->type)
-		return -EINVAL;
-
-	res = get_resource(fh);
-	err = videobuf_streamoff(get_queue(fh));
-	if (err < 0)
-		return err;
-	res_free(dev, fh, res);
-	return 0;
-}
-
 static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	dprintk(1, "%s()\n", __func__);
 
 	*id = dev->tvnorm;
@@ -1262,14 +662,10 @@ static int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
 
 static int vidioc_s_std(struct file *file, void *priv, v4l2_std_id tvnorms)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	dprintk(1, "%s()\n", __func__);
 
-	mutex_lock(&dev->lock);
-	cx23885_set_tvnorm(dev, tvnorms);
-	mutex_unlock(&dev->lock);
-
-	return 0;
+	return cx23885_set_tvnorm(dev, tvnorms);
 }
 
 int cx23885_enum_input(struct cx23885_dev *dev, struct v4l2_input *i)
@@ -1299,16 +695,16 @@ int cx23885_enum_input(struct cx23885_dev *dev, struct v4l2_input *i)
 	i->index = n;
 	i->type  = V4L2_INPUT_TYPE_CAMERA;
 	strcpy(i->name, iname[INPUT(n)->type]);
+	i->std = CX23885_NORMS;
 	if ((CX23885_VMUX_TELEVISION == INPUT(n)->type) ||
 		(CX23885_VMUX_CABLE == INPUT(n)->type)) {
 		i->type = V4L2_INPUT_TYPE_TUNER;
-		i->std = CX23885_NORMS;
+		i->audioset = 4;
+	} else {
+		/* Two selectable audio inputs for non-tv inputs */
+		i->audioset = 3;
 	}
 
-	/* Two selectable audio inputs for non-tv inputs */
-	if (INPUT(n)->type != CX23885_VMUX_TELEVISION)
-		i->audioset = 0x3;
-
 	if (dev->input == n) {
 		/* enum'd input matches our configured input.
 		 * Ask the video decoder to process the call
@@ -1324,14 +720,14 @@ int cx23885_enum_input(struct cx23885_dev *dev, struct v4l2_input *i)
 static int vidioc_enum_input(struct file *file, void *priv,
 				struct v4l2_input *i)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	dprintk(1, "%s()\n", __func__);
 	return cx23885_enum_input(dev, i);
 }
 
 int cx23885_get_input(struct file *file, void *priv, unsigned int *i)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	*i = dev->input;
 	dprintk(1, "%s() returns %d\n", __func__, *i);
@@ -1345,7 +741,7 @@ static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
 
 int cx23885_set_input(struct file *file, void *priv, unsigned int i)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	dprintk(1, "%s(%d)\n", __func__, i);
 
@@ -1357,13 +753,11 @@ int cx23885_set_input(struct file *file, void *priv, unsigned int i)
 	if (INPUT(i)->type == 0)
 		return -EINVAL;
 
-	mutex_lock(&dev->lock);
 	cx23885_video_mux(dev, i);
 
 	/* By default establish the default audio input for the card also */
 	/* Caller is free to use VIDIOC_S_AUDIO to override afterwards */
 	cx23885_audio_mux(dev, i);
-	mutex_unlock(&dev->lock);
 	return 0;
 }
 
@@ -1374,39 +768,32 @@ static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
 
 static int vidioc_log_status(struct file *file, void *priv)
 {
-	struct cx23885_fh  *fh  = priv;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
-	printk(KERN_INFO
-		"%s/0: ============  START LOG STATUS  ============\n",
-		dev->name);
 	call_all(dev, core, log_status);
-	printk(KERN_INFO
-		"%s/0: =============  END LOG STATUS  =============\n",
-		dev->name);
 	return 0;
 }
 
 static int cx23885_query_audinput(struct file *file, void *priv,
 	struct v4l2_audio *i)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	static const char *iname[] = {
 		[0] = "Baseband L/R 1",
 		[1] = "Baseband L/R 2",
+		[2] = "TV",
 	};
 	unsigned int n;
 	dprintk(1, "%s()\n", __func__);
 
 	n = i->index;
-	if (n >= 2)
+	if (n >= 3)
 		return -EINVAL;
 
 	memset(i, 0, sizeof(*i));
 	i->index = n;
 	strcpy(i->name, iname[n]);
-	i->capability  = V4L2_AUDCAP_STEREO;
-	i->mode  = V4L2_AUDMODE_AVL;
+	i->capability = V4L2_AUDCAP_STEREO;
 	return 0;
 
 }
@@ -1420,9 +807,13 @@ static int vidioc_enum_audinput(struct file *file, void *priv,
 static int vidioc_g_audinput(struct file *file, void *priv,
 	struct v4l2_audio *i)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
-	i->index = dev->audinput;
+	if ((CX23885_VMUX_TELEVISION == INPUT(dev->input)->type) ||
+		(CX23885_VMUX_CABLE == INPUT(dev->input)->type))
+		i->index = 2;
+	else
+		i->index = dev->audinput;
 	dprintk(1, "%s(input=%d)\n", __func__, i->index);
 
 	return cx23885_query_audinput(file, priv, i);
@@ -1431,8 +822,13 @@ static int vidioc_g_audinput(struct file *file, void *priv,
 static int vidioc_s_audinput(struct file *file, void *priv,
 	const struct v4l2_audio *i)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
-	if (i->index >= 2)
+	struct cx23885_dev *dev = video_drvdata(file);
+
+	if ((CX23885_VMUX_TELEVISION == INPUT(dev->input)->type) ||
+		(CX23885_VMUX_CABLE == INPUT(dev->input)->type)) {
+		return i->index != 2 ? -EINVAL : 0;
+	}
+	if (i->index > 1)
 		return -EINVAL;
 
 	dprintk(1, "%s(%d)\n", __func__, i->index);
@@ -1445,35 +841,10 @@ static int vidioc_s_audinput(struct file *file, void *priv,
 	return 0;
 }
 
-static int vidioc_queryctrl(struct file *file, void *priv,
-				struct v4l2_queryctrl *qctrl)
-{
-	qctrl->id = v4l2_ctrl_next(ctrl_classes, qctrl->id);
-	if (unlikely(qctrl->id == 0))
-		return -EINVAL;
-	return cx23885_ctrl_query(qctrl);
-}
-
-static int vidioc_g_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctl)
-{
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
-
-	return cx23885_get_control(dev, ctl);
-}
-
-static int vidioc_s_ctrl(struct file *file, void *priv,
-				struct v4l2_control *ctl)
-{
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
-
-	return cx23885_set_control(dev, ctl);
-}
-
 static int vidioc_g_tuner(struct file *file, void *priv,
 				struct v4l2_tuner *t)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (dev->tuner_type == TUNER_ABSENT)
 		return -EINVAL;
@@ -1489,7 +860,7 @@ static int vidioc_g_tuner(struct file *file, void *priv,
 static int vidioc_s_tuner(struct file *file, void *priv,
 				const struct v4l2_tuner *t)
 {
-	struct cx23885_dev *dev = ((struct cx23885_fh *)priv)->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (dev->tuner_type == TUNER_ABSENT)
 		return -EINVAL;
@@ -1504,14 +875,12 @@ static int vidioc_s_tuner(struct file *file, void *priv,
 static int vidioc_g_frequency(struct file *file, void *priv,
 				struct v4l2_frequency *f)
 {
-	struct cx23885_fh *fh = priv;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 
 	if (dev->tuner_type == TUNER_ABSENT)
 		return -EINVAL;
 
-	/* f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV; */
-	f->type = fh->radio ? V4L2_TUNER_RADIO : V4L2_TUNER_ANALOG_TV;
+	f->type = V4L2_TUNER_ANALOG_TV;
 	f->frequency = dev->freq;
 
 	call_all(dev, tuner, g_frequency, f);
@@ -1521,20 +890,23 @@ static int vidioc_g_frequency(struct file *file, void *priv,
 
 static int cx23885_set_freq(struct cx23885_dev *dev, const struct v4l2_frequency *f)
 {
-	struct v4l2_control ctrl;
+	struct v4l2_ctrl *mute;
+	int old_mute_val = 1;
 
 	if (dev->tuner_type == TUNER_ABSENT)
 		return -EINVAL;
 	if (unlikely(f->tuner != 0))
 		return -EINVAL;
 
-	mutex_lock(&dev->lock);
 	dev->freq = f->frequency;
 
 	/* I need to mute audio here */
-	ctrl.id = V4L2_CID_AUDIO_MUTE;
-	ctrl.value = 1;
-	cx23885_set_control(dev, &ctrl);
+	mute = v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+	if (mute) {
+		old_mute_val = v4l2_ctrl_g_ctrl(mute);
+		if (!old_mute_val)
+			v4l2_ctrl_s_ctrl(mute, 1);
+	}
 
 	call_all(dev, tuner, s_frequency, f);
 
@@ -1542,10 +914,8 @@ static int cx23885_set_freq(struct cx23885_dev *dev, const struct v4l2_frequency
 	msleep(100);
 
 	/* I need to unmute audio here */
-	ctrl.value = 0;
-	cx23885_set_control(dev, &ctrl);
-
-	mutex_unlock(&dev->lock);
+	if (old_mute_val == 0)
+		v4l2_ctrl_s_ctrl(mute, old_mute_val);
 
 	return 0;
 }
@@ -1553,8 +923,9 @@ static int cx23885_set_freq(struct cx23885_dev *dev, const struct v4l2_frequency
 static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
 	const struct v4l2_frequency *f)
 {
-	struct v4l2_control ctrl;
-	struct videobuf_dvb_frontend *vfe;
+	struct v4l2_ctrl *mute;
+	int old_mute_val = 1;
+	struct vb2_dvb_frontend *vfe;
 	struct dvb_frontend *fe;
 
 	struct analog_parameters params = {
@@ -1564,21 +935,22 @@ static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
 		.frequency = f->frequency
 	};
 
-	mutex_lock(&dev->lock);
 	dev->freq = f->frequency;
 
 	/* I need to mute audio here */
-	ctrl.id = V4L2_CID_AUDIO_MUTE;
-	ctrl.value = 1;
-	cx23885_set_control(dev, &ctrl);
+	mute = v4l2_ctrl_find(&dev->ctrl_handler, V4L2_CID_AUDIO_MUTE);
+	if (mute) {
+		old_mute_val = v4l2_ctrl_g_ctrl(mute);
+		if (!old_mute_val)
+			v4l2_ctrl_s_ctrl(mute, 1);
+	}
 
 	/* If HVR1850 */
 	dprintk(1, "%s() frequency=%d tuner=%d std=0x%llx\n", __func__,
 		params.frequency, f->tuner, params.std);
 
-	vfe = videobuf_dvb_get_frontend(&dev->ts2.frontends, 1);
+	vfe = vb2_dvb_get_frontend(&dev->ts2.frontends, 1);
 	if (!vfe) {
-		mutex_unlock(&dev->lock);
 		return -EINVAL;
 	}
 
@@ -1600,10 +972,8 @@ static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
 	msleep(100);
 
 	/* I need to unmute audio here */
-	ctrl.value = 0;
-	cx23885_set_control(dev, &ctrl);
-
-	mutex_unlock(&dev->lock);
+	if (old_mute_val == 0)
+		v4l2_ctrl_s_ctrl(mute, old_mute_val);
 
 	return 0;
 }
@@ -1611,8 +981,7 @@ static int cx23885_set_freq_via_ops(struct cx23885_dev *dev,
 int cx23885_set_frequency(struct file *file, void *priv,
 	const struct v4l2_frequency *f)
 {
-	struct cx23885_fh *fh = priv;
-	struct cx23885_dev *dev = fh->dev;
+	struct cx23885_dev *dev = video_drvdata(file);
 	int ret;
 
 	switch (dev->board) {
@@ -1636,28 +1005,6 @@ static int vidioc_s_frequency(struct file *file, void *priv,
 
 /* ----------------------------------------------------------- */
 
-static void cx23885_vid_timeout(unsigned long data)
-{
-	struct cx23885_dev *dev = (struct cx23885_dev *)data;
-	struct cx23885_dmaqueue *q = &dev->vidq;
-	struct cx23885_buffer *buf;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev->slock, flags);
-	while (!list_empty(&q->active)) {
-		buf = list_entry(q->active.next,
-			struct cx23885_buffer, vb.queue);
-		list_del(&buf->vb.queue);
-		buf->vb.state = VIDEOBUF_ERROR;
-		wake_up(&buf->vb.done);
-		printk(KERN_ERR "%s: [%p/%d] timeout - dma=0x%08lx\n",
-			dev->name, buf, buf->vb.i,
-			(unsigned long)buf->risc.dma);
-	}
-	cx23885_restart_video_queue(dev, q);
-	spin_unlock_irqrestore(&dev->slock, flags);
-}
-
 int cx23885_video_irq(struct cx23885_dev *dev, u32 status)
 {
 	u32 mask, count;
@@ -1702,13 +1049,6 @@ int cx23885_video_irq(struct cx23885_dev *dev, u32 status)
 		spin_unlock(&dev->slock);
 		handled++;
 	}
-	if (status & VID_BC_MSK_RISCI2) {
-		dprintk(2, "stopper video\n");
-		spin_lock(&dev->slock);
-		cx23885_restart_video_queue(dev, &dev->vidq);
-		spin_unlock(&dev->slock);
-		handled++;
-	}
 
 	/* Allow the VBI framework to process it's payload */
 	handled += cx23885_vbi_irq(dev, status);
@@ -1721,12 +1061,12 @@ int cx23885_video_irq(struct cx23885_dev *dev, u32 status)
 
 static const struct v4l2_file_operations video_fops = {
 	.owner	       = THIS_MODULE,
-	.open	       = video_open,
-	.release       = video_release,
-	.read	       = video_read,
-	.poll          = video_poll,
-	.mmap	       = video_mmap,
-	.ioctl	       = video_ioctl2,
+	.open           = v4l2_fh_open,
+	.release        = vb2_fop_release,
+	.read           = vb2_fop_read,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap,
 };
 
 static const struct v4l2_ioctl_ops video_ioctl_ops = {
@@ -1738,21 +1078,19 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_g_fmt_vbi_cap     = cx23885_vbi_fmt,
 	.vidioc_try_fmt_vbi_cap   = cx23885_vbi_fmt,
 	.vidioc_s_fmt_vbi_cap     = cx23885_vbi_fmt,
-	.vidioc_reqbufs       = vidioc_reqbufs,
-	.vidioc_querybuf      = vidioc_querybuf,
-	.vidioc_qbuf          = vidioc_qbuf,
-	.vidioc_dqbuf         = vidioc_dqbuf,
+	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
+	.vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf      = vb2_ioctl_querybuf,
+	.vidioc_qbuf          = vb2_ioctl_qbuf,
+	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
+	.vidioc_streamon      = vb2_ioctl_streamon,
+	.vidioc_streamoff     = vb2_ioctl_streamoff,
 	.vidioc_s_std         = vidioc_s_std,
 	.vidioc_g_std         = vidioc_g_std,
 	.vidioc_enum_input    = vidioc_enum_input,
 	.vidioc_g_input       = vidioc_g_input,
 	.vidioc_s_input       = vidioc_s_input,
 	.vidioc_log_status    = vidioc_log_status,
-	.vidioc_queryctrl     = vidioc_queryctrl,
-	.vidioc_g_ctrl        = vidioc_g_ctrl,
-	.vidioc_s_ctrl        = vidioc_s_ctrl,
-	.vidioc_streamon      = vidioc_streamon,
-	.vidioc_streamoff     = vidioc_streamoff,
 	.vidioc_g_tuner       = vidioc_g_tuner,
 	.vidioc_s_tuner       = vidioc_s_tuner,
 	.vidioc_g_frequency   = vidioc_g_frequency,
@@ -1765,6 +1103,8 @@ static const struct v4l2_ioctl_ops video_ioctl_ops = {
 	.vidioc_enumaudio     = vidioc_enum_audinput,
 	.vidioc_g_audio       = vidioc_g_audinput,
 	.vidioc_s_audio       = vidioc_s_audinput,
+	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
 };
 
 static struct video_device cx23885_vbi_template;
@@ -1775,14 +1115,6 @@ static struct video_device cx23885_video_template = {
 	.tvnorms              = CX23885_NORMS,
 };
 
-static const struct v4l2_file_operations radio_fops = {
-	.owner         = THIS_MODULE,
-	.open          = video_open,
-	.release       = video_release,
-	.ioctl         = video_ioctl2,
-};
-
-
 void cx23885_video_unregister(struct cx23885_dev *dev)
 {
 	dprintk(1, "%s()\n", __func__);
@@ -1794,7 +1126,6 @@ void cx23885_video_unregister(struct cx23885_dev *dev)
 		else
 			video_device_release(dev->vbi_dev);
 		dev->vbi_dev = NULL;
-		btcx_riscmem_free(dev->pci, &dev->vbiq.stopper);
 	}
 	if (dev->video_dev) {
 		if (video_is_registered(dev->video_dev))
@@ -1802,8 +1133,6 @@ void cx23885_video_unregister(struct cx23885_dev *dev)
 		else
 			video_device_release(dev->video_dev);
 		dev->video_dev = NULL;
-
-		btcx_riscmem_free(dev->pci, &dev->vidq.stopper);
 	}
 
 	if (dev->audio_dev)
@@ -1812,6 +1141,7 @@ void cx23885_video_unregister(struct cx23885_dev *dev)
 
 int cx23885_video_register(struct cx23885_dev *dev)
 {
+	struct vb2_queue *q;
 	int err;
 
 	dprintk(1, "%s()\n", __func__);
@@ -1822,24 +1152,16 @@ int cx23885_video_register(struct cx23885_dev *dev)
 	strcpy(cx23885_vbi_template.name, "cx23885-vbi");
 
 	dev->tvnorm = V4L2_STD_NTSC_M;
+	dev->fmt = format_by_fourcc(V4L2_PIX_FMT_YUYV);
+	dev->field = V4L2_FIELD_INTERLACED;
+	dev->width = 720;
+	dev->height = norm_maxh(dev->tvnorm);
 
 	/* init video dma queues */
 	INIT_LIST_HEAD(&dev->vidq.active);
-	INIT_LIST_HEAD(&dev->vidq.queued);
-	dev->vidq.timeout.function = cx23885_vid_timeout;
-	dev->vidq.timeout.data = (unsigned long)dev;
-	init_timer(&dev->vidq.timeout);
-	cx23885_risc_stopper(dev->pci, &dev->vidq.stopper,
-		VID_A_DMA_CTL, 0x11, 0x00);
 
 	/* init vbi dma queues */
 	INIT_LIST_HEAD(&dev->vbiq.active);
-	INIT_LIST_HEAD(&dev->vbiq.queued);
-	dev->vbiq.timeout.function = cx23885_vbi_timeout;
-	dev->vbiq.timeout.data = (unsigned long)dev;
-	init_timer(&dev->vbiq.timeout);
-	cx23885_risc_stopper(dev->pci, &dev->vbiq.stopper,
-		VID_A_DMA_CTL, 0x22, 0x00);
 
 	cx23885_irq_add_enable(dev, 0x01);
 
@@ -1893,9 +1215,49 @@ int cx23885_video_register(struct cx23885_dev *dev)
 		}
 	}
 
+	/* initial device configuration */
+	mutex_lock(&dev->lock);
+	cx23885_set_tvnorm(dev, dev->tvnorm);
+	cx23885_video_mux(dev, 0);
+	cx23885_audio_mux(dev, 0);
+	mutex_unlock(&dev->lock);
+
+	q = &dev->vb2_vidq;
+	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+	q->gfp_flags = GFP_DMA32;
+	q->min_buffers_needed = 2;
+	q->drv_priv = dev;
+	q->buf_struct_size = sizeof(struct cx23885_buffer);
+	q->ops = &cx23885_video_qops;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &dev->lock;
+
+	err = vb2_queue_init(q);
+	if (err < 0)
+		goto fail_unreg;
+
+	q = &dev->vb2_vbiq;
+	q->type = V4L2_BUF_TYPE_VBI_CAPTURE;
+	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+	q->gfp_flags = GFP_DMA32;
+	q->min_buffers_needed = 2;
+	q->drv_priv = dev;
+	q->buf_struct_size = sizeof(struct cx23885_buffer);
+	q->ops = &cx23885_vbi_qops;
+	q->mem_ops = &vb2_dma_sg_memops;
+	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	q->lock = &dev->lock;
+
+	err = vb2_queue_init(q);
+	if (err < 0)
+		goto fail_unreg;
+
 	/* register Video device */
 	dev->video_dev = cx23885_vdev_init(dev, dev->pci,
 		&cx23885_video_template, "video");
+	dev->video_dev->queue = &dev->vb2_vidq;
 	err = video_register_device(dev->video_dev, VFL_TYPE_GRABBER,
 				    video_nr[dev->nr]);
 	if (err < 0) {
@@ -1909,6 +1271,7 @@ int cx23885_video_register(struct cx23885_dev *dev)
 	/* register VBI device */
 	dev->vbi_dev = cx23885_vdev_init(dev, dev->pci,
 		&cx23885_vbi_template, "vbi");
+	dev->vbi_dev->queue = &dev->vb2_vbiq;
 	err = video_register_device(dev->vbi_dev, VFL_TYPE_VBI,
 				    vbi_nr[dev->nr]);
 	if (err < 0) {
@@ -1922,18 +1285,9 @@ int cx23885_video_register(struct cx23885_dev *dev)
 	/* Register ALSA audio device */
 	dev->audio_dev = cx23885_audio_register(dev);
 
-	/* initial device configuration */
-	mutex_lock(&dev->lock);
-	cx23885_set_tvnorm(dev, dev->tvnorm);
-	init_controls(dev);
-	cx23885_video_mux(dev, 0);
-	cx23885_audio_mux(dev, 0);
-	mutex_unlock(&dev->lock);
-
 	return 0;
 
 fail_unreg:
 	cx23885_video_unregister(dev);
 	return err;
 }
-
diff --git a/drivers/media/pci/cx23885/cx23885-video.h b/drivers/media/pci/cx23885/cx23885-video.h
index c961a2b0de0f..291e8f3189f0 100644
--- a/drivers/media/pci/cx23885/cx23885-video.h
+++ b/drivers/media/pci/cx23885/cx23885-video.h
@@ -12,11 +12,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #ifndef _CX23885_VIDEO_H_
diff --git a/drivers/media/pci/cx23885/cx23885.h b/drivers/media/pci/cx23885/cx23885.h
index 0e086c03da67..6c35e6115969 100644
--- a/drivers/media/pci/cx23885/cx23885.h
+++ b/drivers/media/pci/cx23885/cx23885.h
@@ -13,10 +13,6 @@
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/pci.h>
@@ -25,19 +21,20 @@
 #include <linux/slab.h>
 
 #include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-ctrls.h>
 #include <media/tuner.h>
 #include <media/tveeprom.h>
-#include <media/videobuf-dma-sg.h>
-#include <media/videobuf-dvb.h>
+#include <media/videobuf2-dma-sg.h>
+#include <media/videobuf2-dvb.h>
 #include <media/rc-core.h>
 
-#include "btcx-risc.h"
 #include "cx23885-reg.h"
 #include "media/cx2341x.h"
 
 #include <linux/mutex.h>
 
-#define CX23885_VERSION "0.0.3"
+#define CX23885_VERSION "0.0.4"
 
 #define UNSET (-1U)
 
@@ -46,9 +43,6 @@
 /* Max number of inputs by card */
 #define MAX_CX23885_INPUT 8
 #define INPUT(nr) (&cx23885_boards[dev->board].input[nr])
-#define RESOURCE_OVERLAY       1
-#define RESOURCE_VIDEO         2
-#define RESOURCE_VBI           4
 
 #define BUFFER_TIMEOUT     (HZ)  /* 0.5 seconds */
 
@@ -98,6 +92,7 @@
 #define CX23885_BOARD_LEADTEK_WINFAST_PXPVR2200 42
 #define CX23885_BOARD_HAUPPAUGE_IMPACTVCBE     43
 #define CX23885_BOARD_DVICO_FUSIONHDTV_DVB_T_DUAL_EXP2 44
+#define CX23885_BOARD_DVBSKY_T9580             45
 
 #define GPIO_0 0x00000001
 #define GPIO_1 0x00000002
@@ -131,14 +126,6 @@ struct cx23885_fmt {
 	u32   cxformat;
 };
 
-struct cx23885_ctrl {
-	struct v4l2_queryctrl v;
-	u32                   off;
-	u32                   reg;
-	u32                   mask;
-	u32                   shift;
-};
-
 struct cx23885_tvnorm {
 	char		*name;
 	v4l2_std_id	id;
@@ -146,30 +133,6 @@ struct cx23885_tvnorm {
 	u32		cxoformat;
 };
 
-struct cx23885_fh {
-	struct cx23885_dev         *dev;
-	enum v4l2_buf_type         type;
-	int                        radio;
-	u32                        resources;
-
-	/* video overlay */
-	struct v4l2_window         win;
-	struct v4l2_clip           *clips;
-	unsigned int               nclips;
-
-	/* video capture */
-	struct cx23885_fmt         *fmt;
-	unsigned int               width, height;
-
-	/* vbi capture */
-	struct videobuf_queue      vidq;
-	struct videobuf_queue      vbiq;
-
-	/* MPEG Encoder specifics ONLY */
-	struct videobuf_queue      mpegq;
-	atomic_t                   v4l_reading;
-};
-
 enum cx23885_itype {
 	CX23885_VMUX_COMPOSITE1 = 1,
 	CX23885_VMUX_COMPOSITE2,
@@ -189,14 +152,22 @@ enum cx23885_src_sel_type {
 	CX23885_SRC_SEL_PARALLEL_MPEG_VIDEO
 };
 
+struct cx23885_riscmem {
+	unsigned int   size;
+	__le32         *cpu;
+	__le32         *jmp;
+	dma_addr_t     dma;
+};
+
 /* buffer for one video frame */
 struct cx23885_buffer {
 	/* common v4l buffer stuff -- must be first */
-	struct videobuf_buffer vb;
+	struct vb2_buffer vb;
+	struct list_head queue;
 
 	/* cx23885 specific */
 	unsigned int           bpl;
-	struct btcx_riscmem    risc;
+	struct cx23885_riscmem risc;
 	struct cx23885_fmt     *fmt;
 	u32                    count;
 };
@@ -268,9 +239,6 @@ struct cx23885_i2c {
 
 struct cx23885_dmaqueue {
 	struct list_head       active;
-	struct list_head       queued;
-	struct timer_list      timeout;
-	struct btcx_riscmem    stopper;
 	u32                    count;
 };
 
@@ -280,7 +248,7 @@ struct cx23885_tsport {
 	int                        nr;
 	int                        sram_chno;
 
-	struct videobuf_dvb_frontends frontends;
+	struct vb2_dvb_frontends   frontends;
 
 	/* dma queues */
 	struct cx23885_dmaqueue    mpegq;
@@ -326,7 +294,12 @@ struct cx23885_tsport {
 	/* Workaround for a temp dvb_frontend that the tuner can attached to */
 	struct dvb_frontend analog_fe;
 
+	struct i2c_client *i2c_client_demod;
+	struct i2c_client *i2c_client_tuner;
+
 	int (*set_frontend)(struct dvb_frontend *fe);
+	int (*fe_set_voltage)(struct dvb_frontend *fe,
+				fe_sec_voltage_t voltage);
 };
 
 struct cx23885_kernel_ir {
@@ -339,8 +312,11 @@ struct cx23885_kernel_ir {
 
 struct cx23885_audio_buffer {
 	unsigned int		bpl;
-	struct btcx_riscmem	risc;
-	struct videobuf_dmabuf	dma;
+	struct cx23885_riscmem	risc;
+	void			*vaddr;
+	struct scatterlist	*sglist;
+	int                     sglen;
+	int                     nr_pages;
 };
 
 struct cx23885_audio_dev {
@@ -358,8 +334,6 @@ struct cx23885_audio_dev {
 	unsigned int		period_size;
 	unsigned int		num_periods;
 
-	struct videobuf_dmabuf	*dma_risc;
-
 	struct cx23885_audio_buffer   *buf;
 
 	struct snd_pcm_substream *substream;
@@ -368,6 +342,7 @@ struct cx23885_audio_dev {
 struct cx23885_dev {
 	atomic_t                   refcount;
 	struct v4l2_device 	   v4l2_dev;
+	struct v4l2_ctrl_handler   ctrl_handler;
 
 	/* pci stuff */
 	struct pci_dev             *pci;
@@ -407,7 +382,6 @@ struct cx23885_dev {
 	} bridge;
 
 	/* Analog video */
-	u32                        resources;
 	unsigned int               input;
 	unsigned int               audinput; /* Selectable audio input */
 	u32                        tvaudio;
@@ -417,7 +391,6 @@ struct cx23885_dev {
 	unsigned int               tuner_bus;
 	unsigned int               radio_type;
 	unsigned char              radio_addr;
-	unsigned int               has_radio;
 	struct v4l2_subdev 	   *sd_cx25840;
 	struct work_struct	   cx25840_work;
 
@@ -435,17 +408,24 @@ struct cx23885_dev {
 	u32                        freq;
 	struct video_device        *video_dev;
 	struct video_device        *vbi_dev;
-	struct video_device        *radio_dev;
+
+	/* video capture */
+	struct cx23885_fmt         *fmt;
+	unsigned int               width, height;
+	unsigned		   field;
 
 	struct cx23885_dmaqueue    vidq;
+	struct vb2_queue           vb2_vidq;
 	struct cx23885_dmaqueue    vbiq;
+	struct vb2_queue           vb2_vbiq;
+
 	spinlock_t                 slock;
 
 	/* MPEG Encoder ONLY settings */
 	u32                        cx23417_mailbox;
-	struct cx2341x_mpeg_params mpeg_params;
+	struct cx2341x_handler     cxhdl;
 	struct video_device        *v4l_device;
-	atomic_t                   v4l_reader_count;
+	struct vb2_queue           vb2_mpegq;
 	struct cx23885_tvnorm      encodernorm;
 
 	/* Analog raw audio */
@@ -521,26 +501,21 @@ extern int cx23885_sram_channel_setup(struct cx23885_dev *dev,
 extern void cx23885_sram_channel_dump(struct cx23885_dev *dev,
 	struct sram_channel *ch);
 
-extern int cx23885_risc_stopper(struct pci_dev *pci, struct btcx_riscmem *risc,
-	u32 reg, u32 mask, u32 value);
-
-extern int cx23885_risc_buffer(struct pci_dev *pci, struct btcx_riscmem *risc,
+extern int cx23885_risc_buffer(struct pci_dev *pci, struct cx23885_riscmem *risc,
 	struct scatterlist *sglist,
 	unsigned int top_offset, unsigned int bottom_offset,
 	unsigned int bpl, unsigned int padding, unsigned int lines);
 
 extern int cx23885_risc_vbibuffer(struct pci_dev *pci,
-	struct btcx_riscmem *risc, struct scatterlist *sglist,
+	struct cx23885_riscmem *risc, struct scatterlist *sglist,
 	unsigned int top_offset, unsigned int bottom_offset,
 	unsigned int bpl, unsigned int padding, unsigned int lines);
 
+int cx23885_start_dma(struct cx23885_tsport *port,
+			     struct cx23885_dmaqueue *q,
+			     struct cx23885_buffer   *buf);
 void cx23885_cancel_buffers(struct cx23885_tsport *port);
 
-extern int cx23885_restart_queue(struct cx23885_tsport *port,
-				struct cx23885_dmaqueue *q);
-
-extern void cx23885_wakeup(struct cx23885_tsport *port,
-			   struct cx23885_dmaqueue *q, u32 count);
 
 extern void cx23885_gpio_set(struct cx23885_dev *dev, u32 mask);
 extern void cx23885_gpio_clear(struct cx23885_dev *dev, u32 mask);
@@ -574,13 +549,11 @@ extern void cx23885_card_setup_pre_i2c(struct cx23885_dev *dev);
 extern int cx23885_dvb_register(struct cx23885_tsport *port);
 extern int cx23885_dvb_unregister(struct cx23885_tsport *port);
 
-extern int cx23885_buf_prepare(struct videobuf_queue *q,
-			       struct cx23885_tsport *port,
-			       struct cx23885_buffer *buf,
-			       enum v4l2_field field);
+extern int cx23885_buf_prepare(struct cx23885_buffer *buf,
+			       struct cx23885_tsport *port);
 extern void cx23885_buf_queue(struct cx23885_tsport *port,
 			      struct cx23885_buffer *buf);
-extern void cx23885_free_buffer(struct videobuf_queue *q,
+extern void cx23885_free_buffer(struct cx23885_dev *dev,
 				struct cx23885_buffer *buf);
 
 /* ----------------------------------------------------------- */
@@ -595,8 +568,6 @@ int cx23885_enum_input(struct cx23885_dev *dev, struct v4l2_input *i);
 int cx23885_set_input(struct file *file, void *priv, unsigned int i);
 int cx23885_get_input(struct file *file, void *priv, unsigned int *i);
 int cx23885_set_frequency(struct file *file, void *priv, const struct v4l2_frequency *f);
-int cx23885_set_control(struct cx23885_dev *dev, struct v4l2_control *ctl);
-int cx23885_get_control(struct cx23885_dev *dev, struct v4l2_control *ctl);
 int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm);
 
 /* ----------------------------------------------------------- */
@@ -604,9 +575,7 @@ int cx23885_set_tvnorm(struct cx23885_dev *dev, v4l2_std_id norm);
 extern int cx23885_vbi_fmt(struct file *file, void *priv,
 	struct v4l2_format *f);
 extern void cx23885_vbi_timeout(unsigned long data);
-extern struct videobuf_queue_ops cx23885_vbi_qops;
-extern int cx23885_restart_vbi_queue(struct cx23885_dev *dev,
-	struct cx23885_dmaqueue *q);
+extern struct vb2_ops cx23885_vbi_qops;
 extern int cx23885_vbi_irq(struct cx23885_dev *dev, u32 status);
 
 /* cx23885-i2c.c                                                */
@@ -638,7 +607,7 @@ extern struct cx23885_audio_dev *cx23885_audio_register(
 extern void cx23885_audio_unregister(struct cx23885_dev *dev);
 extern int cx23885_audio_irq(struct cx23885_dev *dev, u32 status, u32 mask);
 extern int cx23885_risc_databuffer(struct pci_dev *pci,
-				   struct btcx_riscmem *risc,
+				   struct cx23885_riscmem *risc,
 				   struct scatterlist *sglist,
 				   unsigned int bpl,
 				   unsigned int lines,
@@ -649,15 +618,10 @@ extern int cx23885_risc_databuffer(struct pci_dev *pci,
 
 static inline unsigned int norm_maxw(v4l2_std_id norm)
 {
-	return (norm & (V4L2_STD_MN & ~V4L2_STD_PAL_Nc)) ? 720 : 768;
+	return (norm & V4L2_STD_525_60) ? 720 : 768;
 }
 
 static inline unsigned int norm_maxh(v4l2_std_id norm)
 {
-	return (norm & V4L2_STD_625_50) ? 576 : 480;
-}
-
-static inline unsigned int norm_swidth(v4l2_std_id norm)
-{
-	return (norm & (V4L2_STD_MN & ~V4L2_STD_PAL_Nc)) ? 754 : 922;
+	return (norm & V4L2_STD_525_60) ? 480 : 576;
 }
diff --git a/drivers/media/pci/cx23885/cx23888-ir.c b/drivers/media/pci/cx23885/cx23888-ir.c
index 2c951dec2d33..c1aa888af705 100644
--- a/drivers/media/pci/cx23885/cx23888-ir.c
+++ b/drivers/media/pci/cx23885/cx23888-ir.c
@@ -14,11 +14,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #include <linux/kfifo.h>
@@ -263,7 +258,7 @@ static inline unsigned int lpf_count_to_us(unsigned int count)
 }
 
 /*
- * FIFO register pulse width count compuations
+ * FIFO register pulse width count computations
  */
 static u32 clock_divider_to_resolution(u16 divider)
 {
diff --git a/drivers/media/pci/cx23885/cx23888-ir.h b/drivers/media/pci/cx23885/cx23888-ir.h
index d2de41caaf1d..ff74a93575d6 100644
--- a/drivers/media/pci/cx23885/cx23888-ir.h
+++ b/drivers/media/pci/cx23885/cx23888-ir.h
@@ -14,11 +14,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
- *  02110-1301, USA.
  */
 
 #ifndef _CX23888_IR_H_
diff --git a/drivers/media/pci/cx23885/netup-eeprom.c b/drivers/media/pci/cx23885/netup-eeprom.c
index 98a48f500684..b6542ee4385b 100644
--- a/drivers/media/pci/cx23885/netup-eeprom.c
+++ b/drivers/media/pci/cx23885/netup-eeprom.c
@@ -17,10 +17,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #
diff --git a/drivers/media/pci/cx23885/netup-eeprom.h b/drivers/media/pci/cx23885/netup-eeprom.h
index 13926e18feba..90cac5b655d5 100644
--- a/drivers/media/pci/cx23885/netup-eeprom.h
+++ b/drivers/media/pci/cx23885/netup-eeprom.h
@@ -16,10 +16,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #ifndef NETUP_EEPROM_H
diff --git a/drivers/media/pci/cx23885/netup-init.c b/drivers/media/pci/cx23885/netup-init.c
index 0044fef7ca24..76d9487aafc8 100644
--- a/drivers/media/pci/cx23885/netup-init.c
+++ b/drivers/media/pci/cx23885/netup-init.c
@@ -17,10 +17,6 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include "cx23885.h"
diff --git a/drivers/media/pci/cx23885/netup-init.h b/drivers/media/pci/cx23885/netup-init.h
index d26ae4b1590e..daaa212adfba 100644
--- a/drivers/media/pci/cx23885/netup-init.h
+++ b/drivers/media/pci/cx23885/netup-init.h
@@ -17,9 +17,5 @@
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 extern void netup_initialize(struct cx23885_dev *dev);
diff --git a/drivers/media/pci/cx25821/cx25821-video-upstream.c b/drivers/media/pci/cx25821/cx25821-video-upstream.c
index 1f43be0b04c8..a664997e1958 100644
--- a/drivers/media/pci/cx25821/cx25821-video-upstream.c
+++ b/drivers/media/pci/cx25821/cx25821-video-upstream.c
@@ -330,8 +330,9 @@ int cx25821_write_frame(struct cx25821_channel *chan,
 
 	if (frame_size - curpos < count)
 		count = frame_size - curpos;
-	memcpy((char *)out->_data_buf_virt_addr + frame_offset + curpos,
-			data, count);
+	if (copy_from_user((__force char *)out->_data_buf_virt_addr + frame_offset + curpos,
+				data, count))
+		return -EFAULT;
 	curpos += count;
 	if (curpos == frame_size) {
 		out->_frame_count++;
diff --git a/drivers/media/pci/cx88/cx88-cards.c b/drivers/media/pci/cx88/cx88-cards.c
index e18a7ace08b1..851754bf1291 100644
--- a/drivers/media/pci/cx88/cx88-cards.c
+++ b/drivers/media/pci/cx88/cx88-cards.c
@@ -78,19 +78,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 0,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE2,
 			.vmux   = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE3,
 			.vmux   = 2,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE4,
 			.vmux   = 3,
-		}},
+		} },
 	},
 	[CX88_BOARD_HAUPPAUGE] = {
 		.name		= "Hauppauge WinTV 34xxx models",
@@ -99,23 +99,23 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0xff00,  // internal decoder
-		},{
+		}, {
 			.type   = CX88_VMUX_DEBUG,
 			.vmux   = 0,
 			.gpio0  = 0xff01,  // mono from tuner chip
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0xff02,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0xff02,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0xff01,
@@ -127,13 +127,13 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
-		}},
+		} },
 	},
 	[CX88_BOARD_PIXELVIEW] = {
 		.name           = "PixelView",
@@ -141,17 +141,17 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0xff00,  // internal decoder
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
-		}},
+		} },
 		.radio = {
 			 .type  = CX88_RADIO,
 			 .gpio0 = 0xff10,
@@ -164,19 +164,19 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT | TDA9887_INTERCARRIER,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x03ff,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x03fe,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x03fe,
-		}},
+		} },
 	},
 	[CX88_BOARD_WINFAST2000XP_EXPERT] = {
 		.name           = "Leadtek Winfast 2000XP Expert",
@@ -185,28 +185,28 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0	= 0x00F5e700,
 			.gpio1  = 0x00003004,
 			.gpio2  = 0x00F5e700,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0	= 0x00F5c700,
 			.gpio1  = 0x00003004,
 			.gpio2  = 0x00F5c700,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0	= 0x00F5c700,
 			.gpio1  = 0x00003004,
 			.gpio2  = 0x00F5c700,
 			.gpio3  = 0x02000000,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0	= 0x00F5d700,
@@ -222,19 +222,19 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio1  = 0xe09f,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio1  = 0xe05f,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio1  = 0xe05f,
-		}},
+		} },
 		.radio = {
 			.gpio1  = 0xe0df,
 			.type   = CX88_RADIO,
@@ -249,25 +249,25 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf	= TDA9887_PRESENT | TDA9887_INTERCARRIER_NTSC,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x000040bf,
 			.gpio1  = 0x000080c0,
 			.gpio2  = 0x0000ff40,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x000040bf,
 			.gpio1  = 0x000080c0,
 			.gpio2  = 0x0000ff40,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x000040bf,
 			.gpio1  = 0x000080c0,
 			.gpio2  = 0x0000ff40,
-		}},
+		} },
 		.radio = {
 			 .type   = CX88_RADIO,
 			 .vmux   = 3,
@@ -283,14 +283,14 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x0035e700,
 			.gpio1  = 0x00003004,
 			.gpio2  = 0x0035e700,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
@@ -298,14 +298,14 @@ static const struct cx88_board cx88_boards[] = {
 			.gpio1  = 0x00003004,
 			.gpio2  = 0x0035c700,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x0035c700,
 			.gpio1  = 0x0035c700,
 			.gpio2  = 0x02000000,
 			.gpio3  = 0x02000000,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0x0035d700,
@@ -322,22 +322,22 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x0000bde2,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x0000bde6,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x0000bde6,
 			.audioroute = 1,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0x0000bd62,
@@ -351,16 +351,16 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 0,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE2,
 			.vmux   = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
-		}},
+		} },
 	},
 	[CX88_BOARD_PROLINK_PLAYTVPVR] = {
 		.name           = "Prolink PlayTV PVR",
@@ -369,19 +369,19 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf	= TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0xbff0,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0xbff3,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0xbff3,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0xbff0,
@@ -394,16 +394,16 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x0000fde6,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x0000fde6, // 0x0000fda6 L,R RCA audio in?
 			.audioroute = 1,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0x0000fde2,
@@ -417,22 +417,22 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00000fbf,
 			.gpio2  = 0x0000fc08,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x00000fbf,
 			.gpio2  = 0x0000fc68,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x00000fbf,
 			.gpio2  = 0x0000fc68,
-		}},
+		} },
 	},
 	[CX88_BOARD_KWORLD_DVB_T] = {
 		.name           = "KWorld/VStream XPert DVB-T",
@@ -440,17 +440,17 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x0700,
 			.gpio2  = 0x0101,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x0700,
 			.gpio2  = 0x0101,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_DVICO_FUSIONHDTV_DVB_T1] = {
@@ -459,15 +459,15 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x000027df,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x000027df,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_KWORLD_LTV883] = {
@@ -476,23 +476,23 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x07f8,
-		},{
+		}, {
 			.type   = CX88_VMUX_DEBUG,
 			.vmux   = 0,
 			.gpio0  = 0x07f9,  // mono from tuner chip
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x000007fa,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x000007fa,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0x000007f8,
@@ -521,23 +521,23 @@ static const struct cx88_board cx88_boards[] = {
 		    0 - normal RF
 		    1 - high RF
 		*/
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0	= 0x0f0d,
-		},{
+		}, {
 			.type   = CX88_VMUX_CABLE,
 			.vmux   = 0,
 			.gpio0	= 0x0f05,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0	= 0x0f00,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0	= 0x0f00,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_HAUPPAUGE_DVB_T1] = {
@@ -546,10 +546,10 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_CONEXANT_DVB_T1] = {
@@ -558,10 +558,10 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_PROVIDEO_PV259] = {
@@ -570,11 +570,11 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.audioroute = 1,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_BLACKBIRD,
 	},
 	[CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_PLUS] = {
@@ -583,15 +583,15 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x000027df,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x000027df,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_DNTV_LIVE_DVB_T] = {
@@ -600,17 +600,17 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input		= {{
+		.input		= { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x00000700,
 			.gpio2  = 0x00000101,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x00000700,
 			.gpio2  = 0x00000101,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_PCHDTV_HD3000] = {
@@ -632,19 +632,19 @@ static const struct cx88_board cx88_boards[] = {
 		 *
 		 * GPIO[16] = Remote control input
 		 */
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00008484,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x00008400,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x00008400,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0x00008404,
@@ -659,25 +659,25 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0xed1a,
 			.gpio2  = 0x00ff,
-		},{
+		}, {
 			.type   = CX88_VMUX_DEBUG,
 			.vmux   = 0,
 			.gpio0  = 0xff01,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0xff02,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0xed92,
 			.gpio2  = 0x00ff,
-		}},
+		} },
 		.radio = {
 			 .type   = CX88_RADIO,
 			 .gpio0  = 0xed96,
@@ -692,22 +692,22 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00009d80,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x00009d76,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x00009d76,
 			.audioroute = 1,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0x00009d00,
@@ -722,19 +722,19 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 1,
 			.gpio1  = 0x0000e03f,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 2,
 			.gpio1  = 0x0000e07f,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 3,
 			.gpio1  = 0x0000e07f,
-		}}
+		} }
 	},
 	[CX88_BOARD_PIXELVIEW_PLAYTV_ULTRA_PRO] = {
 		.name           = "PixelView PlayTV Ultra Pro (Stereo)",
@@ -745,19 +745,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_addr	= ADDR_UNSET,
 		/* Some variants use a tda9874 and so need the tvaudio module. */
 		.audio_chip     = CX88_AUDIO_TVAUDIO,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0xbf61,  /* internal decoder */
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0	= 0xbf63,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0	= 0xbf63,
-		}},
+		} },
 		.radio = {
 			 .type  = CX88_RADIO,
 			 .gpio0 = 0xbf60,
@@ -770,19 +770,19 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x97ed,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x97e9,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x97e9,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_ADSTECH_DVB_T_PCI] = {
@@ -791,32 +791,32 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x0700,
 			.gpio2  = 0x0101,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x0700,
 			.gpio2  = 0x0101,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_TERRATEC_CINERGY_1400_DVB_T1] = {
 		.name           = "TerraTec Cinergy 1400 DVB-T",
 		.tuner_type     = TUNER_ABSENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 2,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_DVICO_FUSIONHDTV_5_GOLD] = {
@@ -826,19 +826,19 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x87fd,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x87f9,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x87f9,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_AVERMEDIA_ULTRATV_MC_550] = {
@@ -848,22 +848,22 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 0,
 			.gpio0  = 0x0000cd73,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 1,
 			.gpio0  = 0x0000cd73,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 3,
 			.gpio0  = 0x0000cdb3,
 			.audioroute = 1,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.vmux   = 2,
@@ -876,21 +876,21 @@ static const struct cx88_board cx88_boards[] = {
 		 /* Alexander Wold <awold@bigfoot.com> */
 		 .name           = "Kworld V-Stream Xpert DVD",
 		 .tuner_type     = UNSET,
-		 .input          = {{
+		 .input          = { {
 			 .type   = CX88_VMUX_COMPOSITE1,
 			 .vmux   = 1,
 			 .gpio0  = 0x03000000,
 			 .gpio1  = 0x01000000,
 			 .gpio2  = 0x02000000,
 			 .gpio3  = 0x00100000,
-		 },{
+		 }, {
 			 .type   = CX88_VMUX_SVIDEO,
 			 .vmux   = 2,
 			 .gpio0  = 0x03000000,
 			 .gpio1  = 0x01000000,
 			 .gpio2  = 0x02000000,
 			 .gpio3  = 0x00100000,
-		 }},
+		 } },
 	},
 	[CX88_BOARD_ATI_HDTVWONDER] = {
 		.name           = "ATI HDTV Wonder",
@@ -898,28 +898,28 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00000ff7,
 			.gpio1  = 0x000000ff,
 			.gpio2  = 0x00000001,
 			.gpio3  = 0x00000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x00000ffe,
 			.gpio1  = 0x000000ff,
 			.gpio2  = 0x00000001,
 			.gpio3  = 0x00000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x00000ffe,
 			.gpio1  = 0x000000ff,
 			.gpio2  = 0x00000001,
 			.gpio3  = 0x00000000,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_WINFAST_DTV1000] = {
@@ -928,16 +928,16 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_AVERTV_303] = {
@@ -947,28 +947,28 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00ff,
 			.gpio1  = 0xe09f,
 			.gpio2  = 0x0010,
 			.gpio3  = 0x0000,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x00ff,
 			.gpio1  = 0xe05f,
 			.gpio2  = 0x0010,
 			.gpio3  = 0x0000,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x00ff,
 			.gpio1  = 0xe05f,
 			.gpio2  = 0x0010,
 			.gpio3  = 0x0000,
-		}},
+		} },
 	},
 	[CX88_BOARD_HAUPPAUGE_NOVASPLUS_S1] = {
 		.name		= "Hauppauge Nova-S-Plus DVB-S",
@@ -978,22 +978,22 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_addr	= ADDR_UNSET,
 		.audio_chip	= CX88_AUDIO_WM8775,
 		.i2sinputcntl   = 2,
-		.input		= {{
+		.input		= { {
 			.type	= CX88_VMUX_DVB,
 			.vmux	= 0,
 			/* 2: Line-In */
 			.audioroute = 2,
-		},{
+		}, {
 			.type	= CX88_VMUX_COMPOSITE1,
 			.vmux	= 1,
 			/* 2: Line-In */
 			.audioroute = 2,
-		},{
+		}, {
 			.type	= CX88_VMUX_SVIDEO,
 			.vmux	= 2,
 			/* 2: Line-In */
 			.audioroute = 2,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_HAUPPAUGE_NOVASE2_S1] = {
@@ -1002,10 +1002,10 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type	= UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input		= {{
+		.input		= { {
 			.type	= CX88_VMUX_DVB,
 			.vmux	= 0,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_KWORLD_DVBS_100] = {
@@ -1015,22 +1015,22 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.audio_chip = CX88_AUDIO_WM8775,
-		.input		= {{
+		.input		= { {
 			.type	= CX88_VMUX_DVB,
 			.vmux	= 0,
 			/* 2: Line-In */
 			.audioroute = 2,
-		},{
+		}, {
 			.type	= CX88_VMUX_COMPOSITE1,
 			.vmux	= 1,
 			/* 2: Line-In */
 			.audioroute = 2,
-		},{
+		}, {
 			.type	= CX88_VMUX_SVIDEO,
 			.vmux	= 2,
 			/* 2: Line-In */
 			.audioroute = 2,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_HAUPPAUGE_HVR1100] = {
@@ -1040,16 +1040,16 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input		= {{
+		.input		= { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
-		},{
+		}, {
 			.type	= CX88_VMUX_COMPOSITE1,
 			.vmux	= 1,
-		},{
+		}, {
 			.type	= CX88_VMUX_SVIDEO,
 			.vmux	= 2,
-		}},
+		} },
 		/* fixme: Add radio support */
 		.mpeg           = CX88_MPEG_DVB,
 	},
@@ -1060,13 +1060,13 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input		= {{
+		.input		= { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
-		},{
+		}, {
 			.type	= CX88_VMUX_COMPOSITE1,
 			.vmux	= 1,
-		}},
+		} },
 		/* fixme: Add radio support */
 		.mpeg           = CX88_MPEG_DVB,
 	},
@@ -1078,19 +1078,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT | TDA9887_PORT1_ACTIVE |
 				  TDA9887_PORT2_ACTIVE,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0xf80808,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0	= 0xf80808,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0	= 0xf80808,
-		}},
+		} },
 		.radio = {
 			 .type  = CX88_RADIO,
 			 .gpio0 = 0xf80808,
@@ -1106,17 +1106,17 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x0700,
 			.gpio2  = 0x0101,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x0700,
 			.gpio2  = 0x0101,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_DUAL] = {
@@ -1125,15 +1125,15 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x000067df,
-		 },{
+		 }, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x000067df,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_KWORLD_HARDWARE_MPEG_TV_XPERT] = {
@@ -1142,22 +1142,22 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x3de2,
 			.gpio2  = 0x00ff,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x3de6,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x3de6,
 			.audioroute = 1,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0  = 0x3de6,
@@ -1171,19 +1171,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x0000a75f,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x0000a75b,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x0000a75b,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_PCHDTV_HD5500] = {
@@ -1193,19 +1193,19 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x87fd,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x87f9,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x87f9,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_KWORLD_MCE200_DELUXE] = {
@@ -1217,11 +1217,11 @@ static const struct cx88_board cx88_boards[] = {
 		.tda9887_conf   = TDA9887_PRESENT,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x0000BDE6
-		}},
+		} },
 		.mpeg           = CX88_MPEG_BLACKBIRD,
 	},
 	[CX88_BOARD_PIXELVIEW_PLAYTV_P7000] = {
@@ -1233,11 +1233,11 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_addr	= ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT | TDA9887_PORT1_ACTIVE |
 				  TDA9887_PORT2_ACTIVE,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x5da6,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_BLACKBIRD,
 	},
 	[CX88_BOARD_NPGTECH_REALTV_TOP10FM] = {
@@ -1246,19 +1246,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0	= 0x0788,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0	= 0x078b,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0	= 0x078b,
-		}},
+		} },
 		.radio = {
 			 .type  = CX88_RADIO,
 			 .gpio0 = 0x074a,
@@ -1271,7 +1271,7 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00017304,
@@ -1299,7 +1299,7 @@ static const struct cx88_board cx88_boards[] = {
 			.gpio1  = 0x0000b207,
 			.gpio2  = 0x0001d701,
 			.gpio3  = 0x02000000,
-		}},
+		} },
 		.radio = {
 			 .type  = CX88_RADIO,
 			 .gpio0 = 0x00015702,
@@ -1316,35 +1316,35 @@ static const struct cx88_board cx88_boards[] = {
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00017300,
 			.gpio1  = 0x00008207,
 			.gpio2	= 0x00000000,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x00018300,
 			.gpio1  = 0x0000f207,
 			.gpio2	= 0x00017304,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x00018301,
 			.gpio1  = 0x0000f207,
 			.gpio2	= 0x00017304,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x00018301,
 			.gpio1  = 0x0000f207,
 			.gpio2	= 0x00017304,
 			.gpio3  = 0x02000000,
-		}},
+		} },
 		.radio = {
 			 .type  = CX88_RADIO,
 			 .gpio0 = 0x00015702,
@@ -1360,13 +1360,13 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type    = UNSET,
 		.tuner_addr    = ADDR_UNSET,
 		.radio_addr    = ADDR_UNSET,
-		.input  = {{
+		.input  = { {
 			.type  = CX88_VMUX_DVB,
 			.vmux  = 0,
-		},{
+		}, {
 			.type  = CX88_VMUX_COMPOSITE1,
 			.vmux  = 1,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_HAUPPAUGE_HVR3000] = {
@@ -1377,25 +1377,25 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_addr     = ADDR_UNSET,
 		.tda9887_conf   = TDA9887_PRESENT,
 		.audio_chip     = CX88_AUDIO_WM8775,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x84bf,
 			/* 1: TV Audio / FM Mono */
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x84bf,
 			/* 2: Line-In */
 			.audioroute = 2,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x84bf,
 			/* 2: Line-In */
 			.audioroute = 2,
-		}},
+		} },
 		.radio = {
 			.type   = CX88_RADIO,
 			.gpio0	= 0x84bf,
@@ -1411,19 +1411,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x0709,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x070b,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x070b,
-		}},
+		} },
 	},
 	[CX88_BOARD_TE_DTV_250_OEM_SWANN] = {
 		.name           = "Shenzhen Tungsten Ages Tech TE-DTV-250 / Swann OEM",
@@ -1431,28 +1431,28 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x003fffff,
 			.gpio1  = 0x00e00000,
 			.gpio2  = 0x003fffff,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x003fffff,
 			.gpio1  = 0x00e00000,
 			.gpio2  = 0x003fffff,
 			.gpio3  = 0x02000000,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x003fffff,
 			.gpio1  = 0x00e00000,
 			.gpio2  = 0x003fffff,
 			.gpio3  = 0x02000000,
-		}},
+		} },
 	},
 	[CX88_BOARD_HAUPPAUGE_HVR1300] = {
 		.name		= "Hauppauge WinTV-HVR1300 DVB-T/Hybrid MPEG Encoder",
@@ -1465,25 +1465,25 @@ static const struct cx88_board cx88_boards[] = {
 		/*
 		 * gpio0 as reported by Mike Crash <mike AT mikecrash.com>
 		 */
-		.input		= {{
+		.input		= { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0	= 0xef88,
 			/* 1: TV Audio / FM Mono */
 			.audioroute = 1,
-		},{
+		}, {
 			.type	= CX88_VMUX_COMPOSITE1,
 			.vmux	= 1,
 			.gpio0	= 0xef88,
 			/* 2: Line-In */
 			.audioroute = 2,
-		},{
+		}, {
 			.type	= CX88_VMUX_SVIDEO,
 			.vmux	= 2,
 			.gpio0	= 0xef88,
 			/* 2: Line-In */
 			.audioroute = 2,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB | CX88_MPEG_BLACKBIRD,
 		.radio = {
 			.type   = CX88_RADIO,
@@ -1510,19 +1510,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DEBUG,
 			.vmux   = 3,
 			.gpio0  = 0x04ff,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x07fa,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x07fa,
-		}},
+		} },
 	},
 	[CX88_BOARD_PINNACLE_PCTV_HD_800i] = {
 		.name           = "Pinnacle PCTV HD 800i",
@@ -1530,24 +1530,24 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x04fb,
 			.gpio1  = 0x10ff,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x04fb,
 			.gpio1  = 0x10ef,
 			.audioroute = 1,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x04fb,
 			.gpio1  = 0x10ef,
 			.audioroute = 1,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_DVICO_FUSIONHDTV_5_PCI_NANO] = {
@@ -1557,7 +1557,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x000027df, /* Unconfirmed */
@@ -1815,19 +1815,19 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr	= ADDR_UNSET,
 		.radio_addr	= ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x10df,
-		},{
+		}, {
 			.type   = CX88_VMUX_COMPOSITE1,
 			.vmux   = 1,
 			.gpio0  = 0x16d9,
-		},{
+		}, {
 			.type   = CX88_VMUX_SVIDEO,
 			.vmux   = 2,
 			.gpio0  = 0x16d9,
-		}},
+		} },
 		.mpeg           = CX88_MPEG_DVB,
 	},
 	[CX88_BOARD_PROLINK_PV_8000GT] = {
@@ -1967,7 +1967,7 @@ static const struct cx88_board cx88_boards[] = {
 		 * 3: Line-In Expansion
 		 * 4: FM Stereo
 		 */
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0xc4bf,
@@ -2001,7 +2001,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2013,7 +2013,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2025,7 +2025,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2037,7 +2037,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2049,7 +2049,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2061,7 +2061,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2073,7 +2073,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 			.gpio0  = 0x8080,
@@ -2086,7 +2086,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2098,7 +2098,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2110,7 +2110,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_DVB,
 			.vmux   = 0,
 		} },
@@ -2170,7 +2170,7 @@ static const struct cx88_board cx88_boards[] = {
 		 * 13: audio source (0=tuner audio,1=line in)
 		 * 14: FM (0=on,1=off ???)
 		 */
-		.input          = {{
+		.input          = { {
 			.type   = CX88_VMUX_TELEVISION,
 			.vmux   = 0,
 			.gpio0  = 0x0400,       /* pin 2 = 0 */
@@ -2211,7 +2211,7 @@ static const struct cx88_board cx88_boards[] = {
 		 * 13: audio source (0=tuner audio,1=line in)
 		 * 14: FM (0=on,1=off ???)
 		 */
-		.input		= {{
+		.input		= { {
 			.type	= CX88_VMUX_TELEVISION,
 			.vmux	= 0,
 			.gpio0	= 0x0400,	/* pin 2 = 0 */
@@ -2229,7 +2229,7 @@ static const struct cx88_board cx88_boards[] = {
 			.gpio0	= 0x0400,	/* pin 2 = 0 */
 			.gpio1	= 0x6060,	/* pin 13 = 1, pin 14 = 1 */
 			.gpio2	= 0x0000,
-		}},
+		} },
 		.radio = {
 			.type	= CX88_RADIO,
 			.gpio0	= 0x0400,	/* pin 2 = 0 */
@@ -2252,7 +2252,7 @@ static const struct cx88_board cx88_boards[] = {
 		 *  14: 0: FM radio
 		 *  16: 0: RF input is cable
 		 */
-		.input		= {{
+		.input		= { {
 			.type	= CX88_VMUX_TELEVISION,
 			.vmux	= 0,
 			.gpio0	= 0x0403,
@@ -2280,7 +2280,7 @@ static const struct cx88_board cx88_boards[] = {
 			.gpio1	= 0xF0F7,
 			.gpio2	= 0x0101,
 			.gpio3	= 0x0000,
-		}},
+		} },
 		.radio = {
 			.type	= CX88_RADIO,
 			.gpio0	= 0x0403,
@@ -2308,7 +2308,7 @@ static const struct cx88_board cx88_boards[] = {
 		.radio_type     = UNSET,
 		.tuner_addr     = ADDR_UNSET,
 		.radio_addr     = ADDR_UNSET,
-		.input          = {{
+		.input          = { {
 		       .type   = CX88_VMUX_DVB,
 		       .vmux   = 0,
 		} },
@@ -2324,19 +2324,19 @@ static const struct cx88_subid cx88_subids[] = {
 		.subvendor = 0x0070,
 		.subdevice = 0x3400,
 		.card      = CX88_BOARD_HAUPPAUGE,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x3401,
 		.card      = CX88_BOARD_HAUPPAUGE,
-	},{
+	}, {
 		.subvendor = 0x14c7,
 		.subdevice = 0x0106,
 		.card      = CX88_BOARD_GDI,
-	},{
+	}, {
 		.subvendor = 0x14c7,
 		.subdevice = 0x0107, /* with mpeg encoder */
 		.card      = CX88_BOARD_GDI,
-	},{
+	}, {
 		.subvendor = PCI_VENDOR_ID_ATI,
 		.subdevice = 0x00f8,
 		.card      = CX88_BOARD_ATI_WONDER_PRO,
@@ -2348,176 +2348,176 @@ static const struct cx88_subid cx88_subids[] = {
 		.subvendor = 0x107d,
 		.subdevice = 0x6611,
 		.card      = CX88_BOARD_WINFAST2000XP_EXPERT,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x6613,	/* NTSC */
 		.card      = CX88_BOARD_WINFAST2000XP_EXPERT,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x6620,
 		.card      = CX88_BOARD_WINFAST_DV2000,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x663b,
 		.card      = CX88_BOARD_LEADTEK_PVR2000,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x663c,
 		.card      = CX88_BOARD_LEADTEK_PVR2000,
-	},{
+	}, {
 		.subvendor = 0x1461,
 		.subdevice = 0x000b,
 		.card      = CX88_BOARD_AVERTV_STUDIO_303,
-	},{
+	}, {
 		.subvendor = 0x1462,
 		.subdevice = 0x8606,
 		.card      = CX88_BOARD_MSI_TVANYWHERE_MASTER,
-	},{
+	}, {
 		.subvendor = 0x10fc,
 		.subdevice = 0xd003,
 		.card      = CX88_BOARD_IODATA_GVVCP3PCI,
-	},{
+	}, {
 		.subvendor = 0x1043,
 		.subdevice = 0x4823,  /* with mpeg encoder */
 		.card      = CX88_BOARD_ASUS_PVR_416,
-	},{
+	}, {
 		.subvendor = 0x17de,
 		.subdevice = 0x08a6,
 		.card      = CX88_BOARD_KWORLD_DVB_T,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xd810,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_3_GOLD_Q,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xd820,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_3_GOLD_T,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb00,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_DVB_T1,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9002,
 		.card      = CX88_BOARD_HAUPPAUGE_DVB_T1,
-	},{
+	}, {
 		.subvendor = 0x14f1,
 		.subdevice = 0x0187,
 		.card      = CX88_BOARD_CONEXANT_DVB_T1,
-	},{
+	}, {
 		.subvendor = 0x1540,
 		.subdevice = 0x2580,
 		.card      = CX88_BOARD_PROVIDEO_PV259,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb10,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_PLUS,
-	},{
+	}, {
 		.subvendor = 0x1554,
 		.subdevice = 0x4811,
 		.card      = CX88_BOARD_PIXELVIEW,
-	},{
+	}, {
 		.subvendor = 0x7063,
 		.subdevice = 0x3000, /* HD-3000 card */
 		.card      = CX88_BOARD_PCHDTV_HD3000,
-	},{
+	}, {
 		.subvendor = 0x17de,
 		.subdevice = 0xa8a6,
 		.card      = CX88_BOARD_DNTV_LIVE_DVB_T,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x2801,
 		.card      = CX88_BOARD_HAUPPAUGE_ROSLYN,
-	},{
+	}, {
 		.subvendor = 0x14f1,
 		.subdevice = 0x0342,
 		.card      = CX88_BOARD_DIGITALLOGIC_MEC,
-	},{
+	}, {
 		.subvendor = 0x10fc,
 		.subdevice = 0xd035,
 		.card      = CX88_BOARD_IODATA_GVBCTV7E,
-	},{
+	}, {
 		.subvendor = 0x1421,
 		.subdevice = 0x0334,
 		.card      = CX88_BOARD_ADSTECH_DVB_T_PCI,
-	},{
+	}, {
 		.subvendor = 0x153b,
 		.subdevice = 0x1166,
 		.card      = CX88_BOARD_TERRATEC_CINERGY_1400_DVB_T1,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xd500,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_5_GOLD,
-	},{
+	}, {
 		.subvendor = 0x1461,
 		.subdevice = 0x8011,
 		.card      = CX88_BOARD_AVERMEDIA_ULTRATV_MC_550,
-	},{
+	}, {
 		.subvendor = PCI_VENDOR_ID_ATI,
 		.subdevice = 0xa101,
 		.card      = CX88_BOARD_ATI_HDTVWONDER,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x665f,
 		.card      = CX88_BOARD_WINFAST_DTV1000,
-	},{
+	}, {
 		.subvendor = 0x1461,
 		.subdevice = 0x000a,
 		.card      = CX88_BOARD_AVERTV_303,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9200,
 		.card      = CX88_BOARD_HAUPPAUGE_NOVASE2_S1,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9201,
 		.card      = CX88_BOARD_HAUPPAUGE_NOVASPLUS_S1,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9202,
 		.card      = CX88_BOARD_HAUPPAUGE_NOVASPLUS_S1,
-	},{
+	}, {
 		.subvendor = 0x17de,
 		.subdevice = 0x08b2,
 		.card      = CX88_BOARD_KWORLD_DVBS_100,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9400,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR1100,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9402,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR1100,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9800,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR1100LP,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9802,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR1100LP,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9001,
 		.card      = CX88_BOARD_HAUPPAUGE_DVB_T1,
-	},{
+	}, {
 		.subvendor = 0x1822,
 		.subdevice = 0x0025,
 		.card      = CX88_BOARD_DNTV_LIVE_DVB_T_PRO,
-	},{
+	}, {
 		.subvendor = 0x17de,
 		.subdevice = 0x08a1,
 		.card      = CX88_BOARD_KWORLD_DVB_T_CX22702,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb50,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_DUAL,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb54,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_DUAL,
 		/* Re-branded DViCO: DigitalNow DVB-T Dual */
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb11,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_PLUS,
@@ -2530,55 +2530,55 @@ static const struct cx88_subid cx88_subids[] = {
 		.subvendor = 0x17de,
 		.subdevice = 0x0840,
 		.card      = CX88_BOARD_KWORLD_HARDWARE_MPEG_TV_XPERT,
-	},{
+	}, {
 		.subvendor = 0x1421,
 		.subdevice = 0x0305,
 		.card      = CX88_BOARD_KWORLD_HARDWARE_MPEG_TV_XPERT,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb40,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_HYBRID,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdb44,
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_DVB_T_HYBRID,
-	},{
+	}, {
 		.subvendor = 0x7063,
 		.subdevice = 0x5500,
 		.card      = CX88_BOARD_PCHDTV_HD5500,
-	},{
+	}, {
 		.subvendor = 0x17de,
 		.subdevice = 0x0841,
 		.card      = CX88_BOARD_KWORLD_MCE200_DELUXE,
-	},{
+	}, {
 		.subvendor = 0x1822,
 		.subdevice = 0x0019,
 		.card      = CX88_BOARD_DNTV_LIVE_DVB_T_PRO,
-	},{
+	}, {
 		.subvendor = 0x1554,
 		.subdevice = 0x4813,
 		.card      = CX88_BOARD_PIXELVIEW_PLAYTV_P7000,
-	},{
+	}, {
 		.subvendor = 0x14f1,
 		.subdevice = 0x0842,
 		.card      = CX88_BOARD_NPGTECH_REALTV_TOP10FM,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x665e,
 		.card      = CX88_BOARD_WINFAST_DTV2000H,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x6f2b,
 		.card      = CX88_BOARD_WINFAST_DTV2000H_J,
-	},{
+	}, {
 		.subvendor = 0x18ac,
 		.subdevice = 0xd800, /* FusionHDTV 3 Gold (original revision) */
 		.card      = CX88_BOARD_DVICO_FUSIONHDTV_3_GOLD_Q,
-	},{
+	}, {
 		.subvendor = 0x14f1,
 		.subdevice = 0x0084,
 		.card      = CX88_BOARD_GENIATECH_DVBS,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x1404,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR3000,
@@ -2590,60 +2590,60 @@ static const struct cx88_subid cx88_subids[] = {
 		.subvendor = 0x18ac,
 		.subdevice = 0xdccd,
 		.card      = CX88_BOARD_SAMSUNG_SMT_7020,
-	},{
+	}, {
 		.subvendor = 0x1461,
 		.subdevice = 0xc111, /* AverMedia M150-D */
 		/* This board is known to work with the ASUS PVR416 config */
 		.card      = CX88_BOARD_ASUS_PVR_416,
-	},{
+	}, {
 		.subvendor = 0xc180,
 		.subdevice = 0xc980,
 		.card      = CX88_BOARD_TE_DTV_250_OEM_SWANN,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9600,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR1300,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9601,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR1300,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9602,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR1300,
-	},{
+	}, {
 		.subvendor = 0x107d,
 		.subdevice = 0x6632,
 		.card      = CX88_BOARD_LEADTEK_PVR2000,
-	},{
+	}, {
 		.subvendor = 0x12ab,
 		.subdevice = 0x2300, /* Club3D Zap TV2100 */
 		.card      = CX88_BOARD_KWORLD_DVB_T_CX22702,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x9000,
 		.card      = CX88_BOARD_HAUPPAUGE_DVB_T1,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x1400,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR3000,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x1401,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR3000,
-	},{
+	}, {
 		.subvendor = 0x0070,
 		.subdevice = 0x1402,
 		.card      = CX88_BOARD_HAUPPAUGE_HVR3000,
-	},{
+	}, {
 		.subvendor = 0x1421,
 		.subdevice = 0x0341, /* ADS Tech InstantTV DVB-S */
 		.card      = CX88_BOARD_KWORLD_DVBS_100,
-	},{
+	}, {
 		.subvendor = 0x1421,
 		.subdevice = 0x0390,
 		.card      = CX88_BOARD_ADSTECH_PTV_390,
-	},{
+	}, {
 		.subvendor = 0x11bd,
 		.subdevice = 0x0051,
 		.card      = CX88_BOARD_PINNACLE_PCTV_HD_800i,
diff --git a/drivers/media/pci/cx88/cx88-video.c b/drivers/media/pci/cx88/cx88-video.c
index ed8cb9037b6f..ce27e6d4f16e 100644
--- a/drivers/media/pci/cx88/cx88-video.c
+++ b/drivers/media/pci/cx88/cx88-video.c
@@ -696,7 +696,6 @@ static struct videobuf_queue *get_queue(struct file *file)
 		return &fh->vbiq;
 	default:
 		BUG();
-		return NULL;
 	}
 }
 
@@ -711,7 +710,6 @@ static int get_resource(struct file *file)
 		return RESOURCE_VBI;
 	default:
 		BUG();
-		return 0;
 	}
 }
 
@@ -812,7 +810,6 @@ video_read(struct file *file, char __user *data, size_t count, loff_t *ppos)
 					    file->f_flags & O_NONBLOCK);
 	default:
 		BUG();
-		return 0;
 	}
 }
 
diff --git a/drivers/media/pci/ddbridge/ddbridge-core.c b/drivers/media/pci/ddbridge/ddbridge-core.c
index da8f848be3b8..c82e855a0814 100644
--- a/drivers/media/pci/ddbridge/ddbridge-core.c
+++ b/drivers/media/pci/ddbridge/ddbridge-core.c
@@ -149,7 +149,7 @@ static u32 ddb_i2c_functionality(struct i2c_adapter *adap)
 	return I2C_FUNC_SMBUS_EMUL;
 }
 
-struct i2c_algorithm ddb_i2c_algo = {
+static struct i2c_algorithm ddb_i2c_algo = {
 	.master_xfer   = ddb_i2c_master_xfer,
 	.functionality = ddb_i2c_functionality,
 };
@@ -266,7 +266,7 @@ static void io_free(struct pci_dev *pdev, u8 **vbuf,
 	for (i = 0; i < num; i++) {
 		if (vbuf[i]) {
 			pci_free_consistent(pdev, size, vbuf[i], pbuf[i]);
-			vbuf[i] = 0;
+			vbuf[i] = NULL;
 		}
 	}
 }
@@ -440,7 +440,7 @@ static u32 ddb_output_free(struct ddb_output *output)
 }
 
 static ssize_t ddb_output_write(struct ddb_output *output,
-				const u8 *buf, size_t count)
+				const __user u8 *buf, size_t count)
 {
 	struct ddb *dev = output->port->dev;
 	u32 idx, off, stat = output->stat;
@@ -506,7 +506,7 @@ static u32 ddb_input_avail(struct ddb_input *input)
 	return 0;
 }
 
-static ssize_t ddb_input_read(struct ddb_input *input, u8 *buf, size_t count)
+static ssize_t ddb_input_read(struct ddb_input *input, __user u8 *buf, size_t count)
 {
 	struct ddb *dev = input->port->dev;
 	u32 left = count;
@@ -849,7 +849,7 @@ static int dvb_input_attach(struct ddb_input *input)
 		return ret;
 	input->attached = 4;
 
-	input->fe = 0;
+	input->fe = NULL;
 	switch (port->type) {
 	case DDB_TUNER_DVBS_ST:
 		if (demod_attach_stv0900(input, 0) < 0)
@@ -895,7 +895,7 @@ static int dvb_input_attach(struct ddb_input *input)
 /****************************************************************************/
 /****************************************************************************/
 
-static ssize_t ts_write(struct file *file, const char *buf,
+static ssize_t ts_write(struct file *file, const __user char *buf,
 			size_t count, loff_t *ppos)
 {
 	struct dvb_device *dvbdev = file->private_data;
@@ -920,7 +920,7 @@ static ssize_t ts_write(struct file *file, const char *buf,
 	return (left == count) ? -EAGAIN : (count - left);
 }
 
-static ssize_t ts_read(struct file *file, char *buf,
+static ssize_t ts_read(struct file *file, __user char *buf,
 		       size_t count, loff_t *ppos)
 {
 	struct dvb_device *dvbdev = file->private_data;
@@ -975,11 +975,9 @@ static const struct file_operations ci_fops = {
 	.open    = dvb_generic_open,
 	.release = dvb_generic_release,
 	.poll    = ts_poll,
-	.mmap    = 0,
 };
 
 static struct dvb_device dvbdev_ci = {
-	.priv    = 0,
 	.readers = -1,
 	.writers = -1,
 	.users   = -1,
@@ -1038,7 +1036,7 @@ static void output_tasklet(unsigned long data)
 }
 
 
-struct cxd2099_cfg cxd_cfg = {
+static struct cxd2099_cfg cxd_cfg = {
 	.bitrate =  62000,
 	.adr     =  0x40,
 	.polarity = 1,
@@ -1127,7 +1125,7 @@ static void ddb_ports_detach(struct ddb *dev)
 				ddb_output_stop(port->output);
 				dvb_ca_en50221_release(port->en);
 				kfree(port->en);
-				port->en = 0;
+				port->en = NULL;
 				dvb_unregister_adapter(&port->output->adap);
 			}
 			break;
@@ -1413,9 +1411,9 @@ static int flashio(struct ddb *dev, u8 *wbuf, u32 wlen, u8 *rbuf, u32 rlen)
 #define DDB_MAGIC 'd'
 
 struct ddb_flashio {
-	__u8 *write_buf;
+	__user __u8 *write_buf;
 	__u32 write_len;
-	__u8 *read_buf;
+	__user __u8 *read_buf;
 	__u32 read_len;
 };
 
@@ -1439,7 +1437,7 @@ static int ddb_open(struct inode *inode, struct file *file)
 static long ddb_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
 	struct ddb *dev = file->private_data;
-	void *parg = (void *)arg;
+	__user void *parg = (__user void *)arg;
 	int res;
 
 	switch (cmd) {
@@ -1558,7 +1556,7 @@ static void ddb_remove(struct pci_dev *pdev)
 	ddb_device_destroy(dev);
 
 	ddb_unmap(dev);
-	pci_set_drvdata(pdev, 0);
+	pci_set_drvdata(pdev, NULL);
 	pci_disable_device(pdev);
 }
 
@@ -1637,7 +1635,7 @@ fail1:
 fail:
 	printk(KERN_ERR "fail\n");
 	ddb_unmap(dev);
-	pci_set_drvdata(pdev, 0);
+	pci_set_drvdata(pdev, NULL);
 	pci_disable_device(pdev);
 	return -1;
 }
diff --git a/drivers/media/pci/ddbridge/ddbridge.h b/drivers/media/pci/ddbridge/ddbridge.h
index 8b1b41d2a52d..be87fbd90456 100644
--- a/drivers/media/pci/ddbridge/ddbridge.h
+++ b/drivers/media/pci/ddbridge/ddbridge.h
@@ -156,7 +156,7 @@ struct ddb_port {
 
 struct ddb {
 	struct pci_dev        *pdev;
-	unsigned char         *regs;
+	unsigned char __iomem *regs;
 	struct ddb_port        port[DDB_MAX_PORT];
 	struct ddb_i2c         i2c[DDB_MAX_I2C];
 	struct ddb_input       input[DDB_MAX_INPUT];
@@ -173,12 +173,10 @@ struct ddb {
 /****************************************************************************/
 
 #define ddbwritel(_val, _adr)        writel((_val), \
-				     (char *) (dev->regs+(_adr)))
-#define ddbreadl(_adr)               readl((char *) (dev->regs+(_adr)))
-#define ddbcpyto(_adr, _src, _count) memcpy_toio((char *)	\
-				     (dev->regs+(_adr)), (_src), (_count))
-#define ddbcpyfrom(_dst, _adr, _count) memcpy_fromio((_dst), (char *) \
-				       (dev->regs+(_adr)), (_count))
+				     dev->regs+(_adr))
+#define ddbreadl(_adr)               readl(dev->regs+(_adr))
+#define ddbcpyto(_adr, _src, _count) memcpy_toio(dev->regs+(_adr), (_src), (_count))
+#define ddbcpyfrom(_dst, _adr, _count) memcpy_fromio((_dst), dev->regs+(_adr), (_count))
 
 /****************************************************************************/
 
diff --git a/drivers/media/pci/dm1105/dm1105.c b/drivers/media/pci/dm1105/dm1105.c
index e8826c535ccd..ed11716731e9 100644
--- a/drivers/media/pci/dm1105/dm1105.c
+++ b/drivers/media/pci/dm1105/dm1105.c
@@ -614,7 +614,7 @@ static int dm1105_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t voltage)
 
 static void dm1105_set_dma_addr(struct dm1105_dev *dev)
 {
-	dm_writel(DM1105_STADR, cpu_to_le32(dev->dma_addr));
+	dm_writel(DM1105_STADR, (__force u32)cpu_to_le32(dev->dma_addr));
 }
 
 static int dm1105_dma_map(struct dm1105_dev *dev)
diff --git a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
index 7a9b98bc208b..7bf9cbca4fa6 100644
--- a/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
+++ b/drivers/media/pci/ivtv/ivtv-alsa-pcm.c
@@ -81,7 +81,7 @@ static void ivtv_alsa_announce_pcm_data(struct snd_ivtv_card *itvsc,
 	int period_elapsed = 0;
 	int length;
 
-	dprintk("ivtv alsa announce ptr=%p data=%p num_bytes=%zd\n", itvsc,
+	dprintk("ivtv alsa announce ptr=%p data=%p num_bytes=%zu\n", itvsc,
 		pcm_data, num_bytes);
 
 	substream = itvsc->capture_pcm_substream;
diff --git a/drivers/media/pci/ivtv/ivtv-firmware.c b/drivers/media/pci/ivtv/ivtv-firmware.c
index ed73edd2bcd3..4b0e758a7bce 100644
--- a/drivers/media/pci/ivtv/ivtv-firmware.c
+++ b/drivers/media/pci/ivtv/ivtv-firmware.c
@@ -65,7 +65,7 @@ retry:
 			   the wrong file was sometimes loaded. So we check filesizes to
 			   see if at least the right-sized file was loaded. If not, then we
 			   retry. */
-			IVTV_INFO("Retry: file loaded was not %s (expected size %ld, got %zd)\n", fn, size, fw->size);
+			IVTV_INFO("Retry: file loaded was not %s (expected size %ld, got %zu)\n", fn, size, fw->size);
 			release_firmware(fw);
 			retries--;
 			goto retry;
@@ -76,7 +76,7 @@ retry:
 			dst++;
 			src++;
 		}
-		IVTV_INFO("Loaded %s firmware (%zd bytes)\n", fn, fw->size);
+		IVTV_INFO("Loaded %s firmware (%zu bytes)\n", fn, fw->size);
 		release_firmware(fw);
 		return size;
 	}
diff --git a/drivers/media/pci/ivtv/ivtv-irq.c b/drivers/media/pci/ivtv/ivtv-irq.c
index 19a7c9b990a3..ab6d5d25aa6f 100644
--- a/drivers/media/pci/ivtv/ivtv-irq.c
+++ b/drivers/media/pci/ivtv/ivtv-irq.c
@@ -192,11 +192,11 @@ static int stream_enc_dma_append(struct ivtv_stream *s, u32 data[CX2341X_MBOX_MA
 		if (itv->has_cx23415 && (s->type == IVTV_ENC_STREAM_TYPE_PCM ||
 		    s->type == IVTV_DEC_STREAM_TYPE_VBI)) {
 			s->pending_backup = read_dec(offset - IVTV_DECODER_OFFSET);
-			write_dec_sync(cpu_to_le32(DMA_MAGIC_COOKIE), offset - IVTV_DECODER_OFFSET);
+			write_dec_sync(DMA_MAGIC_COOKIE, offset - IVTV_DECODER_OFFSET);
 		}
 		else {
 			s->pending_backup = read_enc(offset);
-			write_enc_sync(cpu_to_le32(DMA_MAGIC_COOKIE), offset);
+			write_enc_sync(DMA_MAGIC_COOKIE, offset);
 		}
 		s->pending_offset = offset;
 	}
@@ -275,13 +275,11 @@ static void dma_post(struct ivtv_stream *s)
 
 		if (x == 0 && ivtv_use_dma(s)) {
 			offset = s->dma_last_offset;
-			if (u32buf[offset / 4] != DMA_MAGIC_COOKIE)
+			if (le32_to_cpu(u32buf[offset / 4]) != DMA_MAGIC_COOKIE)
 			{
-				for (offset = 0; offset < 64; offset++) {
-					if (u32buf[offset] == DMA_MAGIC_COOKIE) {
+				for (offset = 0; offset < 64; offset++)
+					if (le32_to_cpu(u32buf[offset]) == DMA_MAGIC_COOKIE)
 						break;
-					}
-				}
 				offset *= 4;
 				if (offset == 256) {
 					IVTV_DEBUG_WARN("%s: Couldn't find start of buffer within the first 256 bytes\n", s->name);
diff --git a/drivers/media/pci/mantis/hopper_vp3028.c b/drivers/media/pci/mantis/hopper_vp3028.c
index 68a29f8bdf73..1032db6bb789 100644
--- a/drivers/media/pci/mantis/hopper_vp3028.c
+++ b/drivers/media/pci/mantis/hopper_vp3028.c
@@ -34,7 +34,7 @@
 #include "mantis_dvb.h"
 #include "hopper_vp3028.h"
 
-struct zl10353_config hopper_vp3028_config = {
+static struct zl10353_config hopper_vp3028_config = {
 	.demod_address	= 0x0f,
 };
 
diff --git a/drivers/media/pci/mantis/mantis_common.h b/drivers/media/pci/mantis/mantis_common.h
index f2410cf0a6bf..8ff448bb792d 100644
--- a/drivers/media/pci/mantis/mantis_common.h
+++ b/drivers/media/pci/mantis/mantis_common.h
@@ -127,7 +127,7 @@ struct mantis_pci {
 	u32			last_block;
 	u8			*buf_cpu;
 	dma_addr_t		buf_dma;
-	u32			*risc_cpu;
+	__le32			*risc_cpu;
 	dma_addr_t		risc_dma;
 
 	struct tasklet_struct	tasklet;
diff --git a/drivers/media/pci/mantis/mantis_vp1033.c b/drivers/media/pci/mantis/mantis_vp1033.c
index 115003e8d19d..12a6adb2bd7e 100644
--- a/drivers/media/pci/mantis/mantis_vp1033.c
+++ b/drivers/media/pci/mantis/mantis_vp1033.c
@@ -35,7 +35,7 @@
 #include "mantis_vp1033.h"
 #include "mantis_reg.h"
 
-u8 lgtdqcs001f_inittab[] = {
+static u8 lgtdqcs001f_inittab[] = {
 	0x01, 0x15,
 	0x02, 0x30,
 	0x03, 0x00,
@@ -150,7 +150,7 @@ static int lgtdqcs001f_set_symbol_rate(struct dvb_frontend *fe,
 	return 0;
 }
 
-struct stv0299_config lgtdqcs001f_config = {
+static struct stv0299_config lgtdqcs001f_config = {
 	.demod_address		= 0x68,
 	.inittab		= lgtdqcs001f_inittab,
 	.mclk			= 88000000UL,
diff --git a/drivers/media/pci/mantis/mantis_vp1034.c b/drivers/media/pci/mantis/mantis_vp1034.c
index 430ae84ce528..7c1bd167225c 100644
--- a/drivers/media/pci/mantis/mantis_vp1034.c
+++ b/drivers/media/pci/mantis/mantis_vp1034.c
@@ -36,7 +36,7 @@
 #include "mantis_vp1034.h"
 #include "mantis_reg.h"
 
-struct mb86a16_config vp1034_mb86a16_config = {
+static struct mb86a16_config vp1034_mb86a16_config = {
 	.demod_address	= 0x08,
 	.set_voltage	= vp1034_set_voltage,
 };
diff --git a/drivers/media/pci/mantis/mantis_vp1041.c b/drivers/media/pci/mantis/mantis_vp1041.c
index 07a20748b707..7082fcbc94a1 100644
--- a/drivers/media/pci/mantis/mantis_vp1041.c
+++ b/drivers/media/pci/mantis/mantis_vp1041.c
@@ -263,7 +263,7 @@ static const struct stb0899_s1_reg vp1041_stb0899_s1_init_3[] = {
 	{ 0xffff			, 0xff },
 };
 
-struct stb0899_config vp1041_stb0899_config = {
+static struct stb0899_config vp1041_stb0899_config = {
 	.init_dev		= vp1041_stb0899_s1_init_1,
 	.init_s2_demod		= stb0899_s2_init_2,
 	.init_s1_demod		= vp1041_stb0899_s1_init_3,
@@ -300,7 +300,7 @@ struct stb0899_config vp1041_stb0899_config = {
 	.tuner_set_rfsiggain	= NULL,
 };
 
-struct stb6100_config vp1041_stb6100_config = {
+static struct stb6100_config vp1041_stb6100_config = {
 	.tuner_address	= 0x60,
 	.refclock	= 27000000,
 };
diff --git a/drivers/media/pci/mantis/mantis_vp2033.c b/drivers/media/pci/mantis/mantis_vp2033.c
index 1ca6837fbe46..8d48b5abe04a 100644
--- a/drivers/media/pci/mantis/mantis_vp2033.c
+++ b/drivers/media/pci/mantis/mantis_vp2033.c
@@ -37,12 +37,12 @@
 #define MANTIS_MODEL_NAME	"VP-2033"
 #define MANTIS_DEV_TYPE		"DVB-C"
 
-struct tda1002x_config vp2033_tda1002x_cu1216_config = {
+static struct tda1002x_config vp2033_tda1002x_cu1216_config = {
 	.demod_address = 0x18 >> 1,
 	.invert = 1,
 };
 
-struct tda10023_config vp2033_tda10023_cu1216_config = {
+static struct tda10023_config vp2033_tda10023_cu1216_config = {
 	.demod_address = 0x18 >> 1,
 	.invert = 1,
 };
diff --git a/drivers/media/pci/mantis/mantis_vp2040.c b/drivers/media/pci/mantis/mantis_vp2040.c
index d480741afd78..8dd17d7c0881 100644
--- a/drivers/media/pci/mantis/mantis_vp2040.c
+++ b/drivers/media/pci/mantis/mantis_vp2040.c
@@ -37,12 +37,12 @@
 #define MANTIS_MODEL_NAME	"VP-2040"
 #define MANTIS_DEV_TYPE		"DVB-C"
 
-struct tda1002x_config vp2040_tda1002x_cu1216_config = {
+static struct tda1002x_config vp2040_tda1002x_cu1216_config = {
 	.demod_address	= 0x18 >> 1,
 	.invert		= 1,
 };
 
-struct tda10023_config vp2040_tda10023_cu1216_config = {
+static struct tda10023_config vp2040_tda10023_cu1216_config = {
 	.demod_address	= 0x18 >> 1,
 	.invert		= 1,
 };
diff --git a/drivers/media/pci/mantis/mantis_vp3030.c b/drivers/media/pci/mantis/mantis_vp3030.c
index c09308cd3ac6..5c1dd925bdd5 100644
--- a/drivers/media/pci/mantis/mantis_vp3030.c
+++ b/drivers/media/pci/mantis/mantis_vp3030.c
@@ -35,11 +35,11 @@
 #include "mantis_dvb.h"
 #include "mantis_vp3030.h"
 
-struct zl10353_config mantis_vp3030_config = {
+static struct zl10353_config mantis_vp3030_config = {
 	.demod_address		= 0x0f,
 };
 
-struct tda665x_config env57h12d5_config = {
+static struct tda665x_config env57h12d5_config = {
 	.name			= "ENV57H12D5 (ET-50DT)",
 	.addr			= 0x60,
 	.frequency_min		=  47000000,
diff --git a/drivers/media/pci/ngene/ngene-cards.c b/drivers/media/pci/ngene/ngene-cards.c
index 9e82d2105d53..039bed3cc919 100644
--- a/drivers/media/pci/ngene/ngene-cards.c
+++ b/drivers/media/pci/ngene/ngene-cards.c
@@ -696,7 +696,7 @@ static struct ngene_info ngene_info_m780 = {
 	.demod_attach   = { NULL, demod_attach_lg330x },
 
 	/* Ensure these are NULL else the frame will call them (as funcs) */
-	.tuner_attach   = { 0, 0, 0, 0 },
+	.tuner_attach   = { NULL, NULL, NULL, NULL },
 	.fe_config      = { NULL, &aver_m780 },
 	.avf            = { 0 },
 
diff --git a/drivers/media/pci/ngene/ngene-core.c b/drivers/media/pci/ngene/ngene-core.c
index 4930b55fd5f4..e29bc3af4baf 100644
--- a/drivers/media/pci/ngene/ngene-core.c
+++ b/drivers/media/pci/ngene/ngene-core.c
@@ -57,15 +57,13 @@ DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
 
 #define dprintk	if (debug) printk
 
-#define ngwriteb(dat, adr)         writeb((dat), (char *)(dev->iomem + (adr)))
-#define ngwritel(dat, adr)         writel((dat), (char *)(dev->iomem + (adr)))
-#define ngwriteb(dat, adr)         writeb((dat), (char *)(dev->iomem + (adr)))
+#define ngwriteb(dat, adr)         writeb((dat), dev->iomem + (adr))
+#define ngwritel(dat, adr)         writel((dat), dev->iomem + (adr))
+#define ngwriteb(dat, adr)         writeb((dat), dev->iomem + (adr))
 #define ngreadl(adr)               readl(dev->iomem + (adr))
 #define ngreadb(adr)               readb(dev->iomem + (adr))
-#define ngcpyto(adr, src, count)   memcpy_toio((char *) \
-				   (dev->iomem + (adr)), (src), (count))
-#define ngcpyfrom(dst, adr, count) memcpy_fromio((dst), (char *) \
-				   (dev->iomem + (adr)), (count))
+#define ngcpyto(adr, src, count)   memcpy_toio(dev->iomem + (adr), (src), (count))
+#define ngcpyfrom(dst, adr, count) memcpy_fromio((dst), dev->iomem + (adr), (count))
 
 /****************************************************************************/
 /* nGene interrupt handler **************************************************/
@@ -1592,7 +1590,7 @@ static void cxd_detach(struct ngene *dev)
 
 	dvb_ca_en50221_release(ci->en);
 	kfree(ci->en);
-	ci->en = 0;
+	ci->en = NULL;
 }
 
 /***********************************/
diff --git a/drivers/media/pci/ngene/ngene-dvb.c b/drivers/media/pci/ngene/ngene-dvb.c
index fcb16a615aab..59bb2858c8d0 100644
--- a/drivers/media/pci/ngene/ngene-dvb.c
+++ b/drivers/media/pci/ngene/ngene-dvb.c
@@ -47,7 +47,7 @@
 /* COMMAND API interface ****************************************************/
 /****************************************************************************/
 
-static ssize_t ts_write(struct file *file, const char *buf,
+static ssize_t ts_write(struct file *file, const char __user *buf,
 			size_t count, loff_t *ppos)
 {
 	struct dvb_device *dvbdev = file->private_data;
@@ -59,12 +59,12 @@ static ssize_t ts_write(struct file *file, const char *buf,
 				     (&dev->tsout_rbuf) >= count) < 0)
 		return 0;
 
-	dvb_ringbuffer_write(&dev->tsout_rbuf, buf, count);
+	dvb_ringbuffer_write_user(&dev->tsout_rbuf, buf, count);
 
 	return count;
 }
 
-static ssize_t ts_read(struct file *file, char *buf,
+static ssize_t ts_read(struct file *file, char __user *buf,
 		       size_t count, loff_t *ppos)
 {
 	struct dvb_device *dvbdev = file->private_data;
@@ -97,7 +97,6 @@ static const struct file_operations ci_fops = {
 };
 
 struct dvb_device ngene_dvbdev_ci = {
-	.priv    = 0,
 	.readers = -1,
 	.writers = -1,
 	.users   = -1,
diff --git a/drivers/media/pci/ngene/ngene.h b/drivers/media/pci/ngene/ngene.h
index 22c39ff6bfa0..51e2fbd18b1b 100644
--- a/drivers/media/pci/ngene/ngene.h
+++ b/drivers/media/pci/ngene/ngene.h
@@ -737,7 +737,7 @@ typedef void (tx_cb_t)(struct ngene *, u32);
 struct ngene {
 	int                   nr;
 	struct pci_dev       *pci_dev;
-	unsigned char        *iomem;
+	unsigned char __iomem *iomem;
 
 	/*struct i2c_adapter  i2c_adapter;*/
 
diff --git a/drivers/media/pci/pt3/Kconfig b/drivers/media/pci/pt3/Kconfig
new file mode 100644
index 000000000000..16c208ae0079
--- /dev/null
+++ b/drivers/media/pci/pt3/Kconfig
@@ -0,0 +1,10 @@
+config DVB_PT3
+	tristate "Earthsoft PT3 cards"
+	depends on DVB_CORE && PCI && I2C
+	select DVB_TC90522 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_QM1D1C0042 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_MXL301RF if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Support for Earthsoft PT3 PCIe cards.
+
+	  Say Y or M if you own such a device and want to use it.
diff --git a/drivers/media/pci/pt3/Makefile b/drivers/media/pci/pt3/Makefile
new file mode 100644
index 000000000000..396f146b1c18
--- /dev/null
+++ b/drivers/media/pci/pt3/Makefile
@@ -0,0 +1,8 @@
+
+earth-pt3-objs += pt3.o pt3_i2c.o pt3_dma.o
+
+obj-$(CONFIG_DVB_PT3) += earth-pt3.o
+
+ccflags-y += -Idrivers/media/dvb-core
+ccflags-y += -Idrivers/media/dvb-frontends
+ccflags-y += -Idrivers/media/tuners
diff --git a/drivers/media/pci/pt3/pt3.c b/drivers/media/pci/pt3/pt3.c
new file mode 100644
index 000000000000..1fdeac11501a
--- /dev/null
+++ b/drivers/media/pci/pt3/pt3.c
@@ -0,0 +1,876 @@
+/*
+ * Earthsoft PT3 driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/freezer.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/string.h>
+
+#include "dmxdev.h"
+#include "dvbdev.h"
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+
+#include "pt3.h"
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+static bool one_adapter;
+module_param(one_adapter, bool, 0444);
+MODULE_PARM_DESC(one_adapter, "Place FE's together under one adapter.");
+
+static int num_bufs = 4;
+module_param(num_bufs, int, 0444);
+MODULE_PARM_DESC(num_bufs, "Number of DMA buffer (188KiB) per FE.");
+
+
+static const struct i2c_algorithm pt3_i2c_algo = {
+	.master_xfer   = &pt3_i2c_master_xfer,
+	.functionality = &pt3_i2c_functionality,
+};
+
+static const struct pt3_adap_config adap_conf[PT3_NUM_FE] = {
+	{
+		.demod_info = {
+			I2C_BOARD_INFO(TC90522_I2C_DEV_SAT, 0x11),
+		},
+		.tuner_info = {
+			I2C_BOARD_INFO("qm1d1c0042", 0x63),
+		},
+		.tuner_cfg.qm1d1c0042 = {
+			.lpf = 1,
+		},
+		.init_freq = 1049480 - 300,
+	},
+	{
+		.demod_info = {
+			I2C_BOARD_INFO(TC90522_I2C_DEV_TER, 0x10),
+		},
+		.tuner_info = {
+			I2C_BOARD_INFO("mxl301rf", 0x62),
+		},
+		.init_freq = 515142857,
+	},
+	{
+		.demod_info = {
+			I2C_BOARD_INFO(TC90522_I2C_DEV_SAT, 0x13),
+		},
+		.tuner_info = {
+			I2C_BOARD_INFO("qm1d1c0042", 0x60),
+		},
+		.tuner_cfg.qm1d1c0042 = {
+			.lpf = 1,
+		},
+		.init_freq = 1049480 + 300,
+	},
+	{
+		.demod_info = {
+			I2C_BOARD_INFO(TC90522_I2C_DEV_TER, 0x12),
+		},
+		.tuner_info = {
+			I2C_BOARD_INFO("mxl301rf", 0x61),
+		},
+		.init_freq = 521142857,
+	},
+};
+
+
+struct reg_val {
+	u8 reg;
+	u8 val;
+};
+
+static int
+pt3_demod_write(struct pt3_adapter *adap, const struct reg_val *data, int num)
+{
+	struct i2c_msg msg;
+	int i, ret;
+
+	ret = 0;
+	msg.addr = adap->i2c_demod->addr;
+	msg.flags = 0;
+	msg.len = 2;
+	for (i = 0; i < num; i++) {
+		msg.buf = (u8 *)&data[i];
+		ret = i2c_transfer(adap->i2c_demod->adapter, &msg, 1);
+		if (ret == 0)
+			ret = -EREMOTE;
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static inline void pt3_lnb_ctrl(struct pt3_board *pt3, bool on)
+{
+	iowrite32((on ? 0x0f : 0x0c), pt3->regs[0] + REG_SYSTEM_W);
+}
+
+static inline struct pt3_adapter *pt3_find_adapter(struct dvb_frontend *fe)
+{
+	struct pt3_board *pt3;
+	int i;
+
+	if (one_adapter) {
+		pt3 = fe->dvb->priv;
+		for (i = 0; i < PT3_NUM_FE; i++)
+			if (pt3->adaps[i]->fe == fe)
+				return pt3->adaps[i];
+	}
+	return container_of(fe->dvb, struct pt3_adapter, dvb_adap);
+}
+
+/*
+ * all 4 tuners in PT3 are packaged in a can module (Sharp VA4M6JC2103).
+ * it seems that they share the power lines and Amp power line and
+ * adaps[3] controls those powers.
+ */
+static int
+pt3_set_tuner_power(struct pt3_board *pt3, bool tuner_on, bool amp_on)
+{
+	struct reg_val rv = { 0x1e, 0x99 };
+
+	if (tuner_on)
+		rv.val |= 0x40;
+	if (amp_on)
+		rv.val |= 0x04;
+	return pt3_demod_write(pt3->adaps[PT3_NUM_FE - 1], &rv, 1);
+}
+
+static int pt3_set_lna(struct dvb_frontend *fe)
+{
+	struct pt3_adapter *adap;
+	struct pt3_board *pt3;
+	u32 val;
+	int ret;
+
+	/* LNA is shared btw. 2 TERR-tuners */
+
+	adap = pt3_find_adapter(fe);
+	val = fe->dtv_property_cache.lna;
+	if (val == LNA_AUTO || val == adap->cur_lna)
+		return 0;
+
+	pt3 = adap->dvb_adap.priv;
+	if (mutex_lock_interruptible(&pt3->lock))
+		return -ERESTARTSYS;
+	if (val)
+		pt3->lna_on_cnt++;
+	else
+		pt3->lna_on_cnt--;
+
+	if (val && pt3->lna_on_cnt <= 1) {
+		pt3->lna_on_cnt = 1;
+		ret = pt3_set_tuner_power(pt3, true, true);
+	} else if (!val && pt3->lna_on_cnt <= 0) {
+		pt3->lna_on_cnt = 0;
+		ret = pt3_set_tuner_power(pt3, true, false);
+	} else
+		ret = 0;
+	mutex_unlock(&pt3->lock);
+	adap->cur_lna = (val != 0);
+	return ret;
+}
+
+static int pt3_set_voltage(struct dvb_frontend *fe, fe_sec_voltage_t volt)
+{
+	struct pt3_adapter *adap;
+	struct pt3_board *pt3;
+	bool on;
+
+	/* LNB power is shared btw. 2 SAT-tuners */
+
+	adap = pt3_find_adapter(fe);
+	on = (volt != SEC_VOLTAGE_OFF);
+	if (on == adap->cur_lnb)
+		return 0;
+	adap->cur_lnb = on;
+	pt3 = adap->dvb_adap.priv;
+	if (mutex_lock_interruptible(&pt3->lock))
+		return -ERESTARTSYS;
+	if (on)
+		pt3->lnb_on_cnt++;
+	else
+		pt3->lnb_on_cnt--;
+
+	if (on && pt3->lnb_on_cnt <= 1) {
+		pt3->lnb_on_cnt = 1;
+		pt3_lnb_ctrl(pt3, true);
+	} else if (!on && pt3->lnb_on_cnt <= 0) {
+		pt3->lnb_on_cnt = 0;
+		pt3_lnb_ctrl(pt3, false);
+	}
+	mutex_unlock(&pt3->lock);
+	return 0;
+}
+
+/* register values used in pt3_fe_init() */
+
+static const struct reg_val init0_sat[] = {
+	{ 0x03, 0x01 },
+	{ 0x1e, 0x10 },
+};
+static const struct reg_val init0_ter[] = {
+	{ 0x01, 0x40 },
+	{ 0x1c, 0x10 },
+};
+static const struct reg_val cfg_sat[] = {
+	{ 0x1c, 0x15 },
+	{ 0x1f, 0x04 },
+};
+static const struct reg_val cfg_ter[] = {
+	{ 0x1d, 0x01 },
+};
+
+/*
+ * pt3_fe_init: initialize demod sub modules and ISDB-T tuners all at once.
+ *
+ * As for demod IC (TC90522) and ISDB-T tuners (MxL301RF),
+ * the i2c sequences for init'ing them are not public and hidden in a ROM,
+ * and include the board specific configurations as well.
+ * They are stored in a lump and cannot be taken out / accessed separately,
+ * thus cannot be moved to the FE/tuner driver.
+ */
+static int pt3_fe_init(struct pt3_board *pt3)
+{
+	int i, ret;
+	struct dvb_frontend *fe;
+
+	pt3_i2c_reset(pt3);
+	ret = pt3_init_all_demods(pt3);
+	if (ret < 0) {
+		dev_warn(&pt3->pdev->dev, "Failed to init demod chips.");
+		return ret;
+	}
+
+	/* additional config? */
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		fe = pt3->adaps[i]->fe;
+
+		if (fe->ops.delsys[0] == SYS_ISDBS)
+			ret = pt3_demod_write(pt3->adaps[i],
+					      init0_sat, ARRAY_SIZE(init0_sat));
+		else
+			ret = pt3_demod_write(pt3->adaps[i],
+					      init0_ter, ARRAY_SIZE(init0_ter));
+		if (ret < 0) {
+			dev_warn(&pt3->pdev->dev,
+				 "demod[%d] faild in init sequence0.", i);
+			return ret;
+		}
+		ret = fe->ops.init(fe);
+		if (ret < 0)
+			return ret;
+	}
+
+	usleep_range(2000, 4000);
+	ret = pt3_set_tuner_power(pt3, true, false);
+	if (ret < 0) {
+		dev_warn(&pt3->pdev->dev, "Failed to control tuner module.");
+		return ret;
+	}
+
+	/* output pin configuration */
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		fe = pt3->adaps[i]->fe;
+		if (fe->ops.delsys[0] == SYS_ISDBS)
+			ret = pt3_demod_write(pt3->adaps[i],
+						cfg_sat, ARRAY_SIZE(cfg_sat));
+		else
+			ret = pt3_demod_write(pt3->adaps[i],
+						cfg_ter, ARRAY_SIZE(cfg_ter));
+		if (ret < 0) {
+			dev_warn(&pt3->pdev->dev,
+				 "demod[%d] faild in init sequence1.", i);
+			return ret;
+		}
+	}
+	usleep_range(4000, 6000);
+
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		fe = pt3->adaps[i]->fe;
+		if (fe->ops.delsys[0] != SYS_ISDBS)
+			continue;
+		/* init and wake-up ISDB-S tuners */
+		ret = fe->ops.tuner_ops.init(fe);
+		if (ret < 0) {
+			dev_warn(&pt3->pdev->dev,
+				 "Failed to init SAT-tuner[%d].", i);
+			return ret;
+		}
+	}
+	ret = pt3_init_all_mxl301rf(pt3);
+	if (ret < 0) {
+		dev_warn(&pt3->pdev->dev, "Failed to init TERR-tuners.");
+		return ret;
+	}
+
+	ret = pt3_set_tuner_power(pt3, true, true);
+	if (ret < 0) {
+		dev_warn(&pt3->pdev->dev, "Failed to control tuner module.");
+		return ret;
+	}
+
+	/* Wake up all tuners and make an initial tuning,
+	 * in order to avoid interference among the tuners in the module,
+	 * according to the doc from the manufacturer.
+	 */
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		fe = pt3->adaps[i]->fe;
+		ret = 0;
+		if (fe->ops.delsys[0] == SYS_ISDBT)
+			ret = fe->ops.tuner_ops.init(fe);
+		/* set only when called from pt3_probe(), not resume() */
+		if (ret == 0 && fe->dtv_property_cache.frequency == 0) {
+			fe->dtv_property_cache.frequency =
+						adap_conf[i].init_freq;
+			ret = fe->ops.tuner_ops.set_params(fe);
+		}
+		if (ret < 0) {
+			dev_warn(&pt3->pdev->dev,
+				 "Failed in initial tuning of tuner[%d].", i);
+			return ret;
+		}
+	}
+
+	/* and sleep again, waiting to be opened by users. */
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		fe = pt3->adaps[i]->fe;
+		if (fe->ops.tuner_ops.sleep)
+			ret = fe->ops.tuner_ops.sleep(fe);
+		if (ret < 0)
+			break;
+		if (fe->ops.sleep)
+			ret = fe->ops.sleep(fe);
+		if (ret < 0)
+			break;
+		if (fe->ops.delsys[0] == SYS_ISDBS)
+			fe->ops.set_voltage = &pt3_set_voltage;
+		else
+			fe->ops.set_lna = &pt3_set_lna;
+	}
+	if (i < PT3_NUM_FE) {
+		dev_warn(&pt3->pdev->dev, "FE[%d] failed to standby.", i);
+		return ret;
+	}
+	return 0;
+}
+
+
+static int pt3_attach_fe(struct pt3_board *pt3, int i)
+{
+	struct i2c_board_info info;
+	struct tc90522_config cfg;
+	struct i2c_client *cl;
+	struct dvb_adapter *dvb_adap;
+	int ret;
+
+	info = adap_conf[i].demod_info;
+	cfg = adap_conf[i].demod_cfg;
+	cfg.tuner_i2c = NULL;
+	info.platform_data = &cfg;
+
+	ret = -ENODEV;
+	request_module("tc90522");
+	cl = i2c_new_device(&pt3->i2c_adap, &info);
+	if (!cl || !cl->dev.driver)
+		return -ENODEV;
+	pt3->adaps[i]->i2c_demod = cl;
+	if (!try_module_get(cl->dev.driver->owner))
+		goto err_demod_i2c_unregister_device;
+
+	if (!strncmp(cl->name, TC90522_I2C_DEV_SAT, sizeof(cl->name))) {
+		struct qm1d1c0042_config tcfg;
+
+		tcfg = adap_conf[i].tuner_cfg.qm1d1c0042;
+		tcfg.fe = cfg.fe;
+		info = adap_conf[i].tuner_info;
+		info.platform_data = &tcfg;
+		request_module("qm1d1c0042");
+		cl = i2c_new_device(cfg.tuner_i2c, &info);
+	} else {
+		struct mxl301rf_config tcfg;
+
+		tcfg = adap_conf[i].tuner_cfg.mxl301rf;
+		tcfg.fe = cfg.fe;
+		info = adap_conf[i].tuner_info;
+		info.platform_data = &tcfg;
+		request_module("mxl301rf");
+		cl = i2c_new_device(cfg.tuner_i2c, &info);
+	}
+	if (!cl || !cl->dev.driver)
+		goto err_demod_module_put;
+	pt3->adaps[i]->i2c_tuner = cl;
+	if (!try_module_get(cl->dev.driver->owner))
+		goto err_tuner_i2c_unregister_device;
+
+	dvb_adap = &pt3->adaps[one_adapter ? 0 : i]->dvb_adap;
+	ret = dvb_register_frontend(dvb_adap, cfg.fe);
+	if (ret < 0)
+		goto err_tuner_module_put;
+	pt3->adaps[i]->fe = cfg.fe;
+	return 0;
+
+err_tuner_module_put:
+	module_put(pt3->adaps[i]->i2c_tuner->dev.driver->owner);
+err_tuner_i2c_unregister_device:
+	i2c_unregister_device(pt3->adaps[i]->i2c_tuner);
+err_demod_module_put:
+	module_put(pt3->adaps[i]->i2c_demod->dev.driver->owner);
+err_demod_i2c_unregister_device:
+	i2c_unregister_device(pt3->adaps[i]->i2c_demod);
+
+	return ret;
+}
+
+
+static int pt3_fetch_thread(void *data)
+{
+	struct pt3_adapter *adap = data;
+	ktime_t delay;
+	bool was_frozen;
+
+#define PT3_INITIAL_BUF_DROPS 4
+#define PT3_FETCH_DELAY 10
+#define PT3_FETCH_DELAY_DELTA 2
+
+	pt3_init_dmabuf(adap);
+	adap->num_discard = PT3_INITIAL_BUF_DROPS;
+
+	dev_dbg(adap->dvb_adap.device,
+		"PT3: [%s] started.\n", adap->thread->comm);
+	set_freezable();
+	while (!kthread_freezable_should_stop(&was_frozen)) {
+		if (was_frozen)
+			adap->num_discard = PT3_INITIAL_BUF_DROPS;
+
+		pt3_proc_dma(adap);
+
+		delay = ktime_set(0, PT3_FETCH_DELAY * NSEC_PER_MSEC);
+		set_current_state(TASK_UNINTERRUPTIBLE);
+		freezable_schedule_hrtimeout_range(&delay,
+					PT3_FETCH_DELAY_DELTA * NSEC_PER_MSEC,
+					HRTIMER_MODE_REL);
+	}
+	dev_dbg(adap->dvb_adap.device,
+		"PT3: [%s] exited.\n", adap->thread->comm);
+	adap->thread = NULL;
+	return 0;
+}
+
+static int pt3_start_streaming(struct pt3_adapter *adap)
+{
+	struct task_struct *thread;
+
+	/* start fetching thread */
+	thread = kthread_run(pt3_fetch_thread, adap, "pt3-ad%i-dmx%i",
+				adap->dvb_adap.num, adap->dmxdev.dvbdev->id);
+	if (IS_ERR(thread)) {
+		int ret = PTR_ERR(thread);
+
+		dev_warn(adap->dvb_adap.device,
+			"PT3 (adap:%d, dmx:%d): failed to start kthread.\n",
+			adap->dvb_adap.num, adap->dmxdev.dvbdev->id);
+		return ret;
+	}
+	adap->thread = thread;
+
+	return pt3_start_dma(adap);
+}
+
+static int pt3_stop_streaming(struct pt3_adapter *adap)
+{
+	int ret;
+
+	ret = pt3_stop_dma(adap);
+	if (ret)
+		dev_warn(adap->dvb_adap.device,
+			"PT3: failed to stop streaming of adap:%d/FE:%d\n",
+			adap->dvb_adap.num, adap->fe->id);
+
+	/* kill the fetching thread */
+	ret = kthread_stop(adap->thread);
+	return ret;
+}
+
+static int pt3_start_feed(struct dvb_demux_feed *feed)
+{
+	struct pt3_adapter *adap;
+
+	if (signal_pending(current))
+		return -EINTR;
+
+	adap = container_of(feed->demux, struct pt3_adapter, demux);
+	adap->num_feeds++;
+	if (adap->thread)
+		return 0;
+	if (adap->num_feeds != 1) {
+		dev_warn(adap->dvb_adap.device,
+			"%s: unmatched start/stop_feed in adap:%i/dmx:%i.\n",
+			__func__, adap->dvb_adap.num, adap->dmxdev.dvbdev->id);
+		adap->num_feeds = 1;
+	}
+
+	return pt3_start_streaming(adap);
+
+}
+
+static int pt3_stop_feed(struct dvb_demux_feed *feed)
+{
+	struct pt3_adapter *adap;
+
+	adap = container_of(feed->demux, struct pt3_adapter, demux);
+
+	adap->num_feeds--;
+	if (adap->num_feeds > 0 || !adap->thread)
+		return 0;
+	adap->num_feeds = 0;
+
+	return pt3_stop_streaming(adap);
+}
+
+
+static int pt3_alloc_adapter(struct pt3_board *pt3, int index)
+{
+	int ret;
+	struct pt3_adapter *adap;
+	struct dvb_adapter *da;
+
+	adap = kzalloc(sizeof(*adap), GFP_KERNEL);
+	if (!adap) {
+		dev_err(&pt3->pdev->dev, "failed to alloc mem for adapter.\n");
+		return -ENOMEM;
+	}
+	pt3->adaps[index] = adap;
+	adap->adap_idx = index;
+
+	if (index == 0 || !one_adapter) {
+		ret = dvb_register_adapter(&adap->dvb_adap, "PT3 DVB",
+				THIS_MODULE, &pt3->pdev->dev, adapter_nr);
+		if (ret < 0) {
+			dev_err(&pt3->pdev->dev,
+				"failed to register adapter dev.\n");
+			goto err_mem;
+		}
+		da = &adap->dvb_adap;
+	} else
+		da = &pt3->adaps[0]->dvb_adap;
+
+	adap->dvb_adap.priv = pt3;
+	adap->demux.dmx.capabilities = DMX_TS_FILTERING | DMX_SECTION_FILTERING;
+	adap->demux.priv = adap;
+	adap->demux.feednum = 256;
+	adap->demux.filternum = 256;
+	adap->demux.start_feed = pt3_start_feed;
+	adap->demux.stop_feed = pt3_stop_feed;
+	ret = dvb_dmx_init(&adap->demux);
+	if (ret < 0) {
+		dev_err(&pt3->pdev->dev, "failed to init dmx dev.\n");
+		goto err_adap;
+	}
+
+	adap->dmxdev.filternum = 256;
+	adap->dmxdev.demux = &adap->demux.dmx;
+	ret = dvb_dmxdev_init(&adap->dmxdev, da);
+	if (ret < 0) {
+		dev_err(&pt3->pdev->dev, "failed to init dmxdev.\n");
+		goto err_demux;
+	}
+
+	ret = pt3_alloc_dmabuf(adap);
+	if (ret) {
+		dev_err(&pt3->pdev->dev, "failed to alloc DMA buffers.\n");
+		goto err_dmabuf;
+	}
+
+	return 0;
+
+err_dmabuf:
+	pt3_free_dmabuf(adap);
+	dvb_dmxdev_release(&adap->dmxdev);
+err_demux:
+	dvb_dmx_release(&adap->demux);
+err_adap:
+	if (index == 0 || !one_adapter)
+		dvb_unregister_adapter(da);
+err_mem:
+	kfree(adap);
+	pt3->adaps[index] = NULL;
+	return ret;
+}
+
+static void pt3_cleanup_adapter(struct pt3_board *pt3, int index)
+{
+	struct pt3_adapter *adap;
+	struct dmx_demux *dmx;
+
+	adap = pt3->adaps[index];
+	if (adap == NULL)
+		return;
+
+	/* stop demux kthread */
+	if (adap->thread)
+		pt3_stop_streaming(adap);
+
+	dmx = &adap->demux.dmx;
+	dmx->close(dmx);
+	if (adap->fe) {
+		adap->fe->callback = NULL;
+		if (adap->fe->frontend_priv)
+			dvb_unregister_frontend(adap->fe);
+		if (adap->i2c_tuner) {
+			module_put(adap->i2c_tuner->dev.driver->owner);
+			i2c_unregister_device(adap->i2c_tuner);
+		}
+		if (adap->i2c_demod) {
+			module_put(adap->i2c_demod->dev.driver->owner);
+			i2c_unregister_device(adap->i2c_demod);
+		}
+	}
+	pt3_free_dmabuf(adap);
+	dvb_dmxdev_release(&adap->dmxdev);
+	dvb_dmx_release(&adap->demux);
+	if (index == 0 || !one_adapter)
+		dvb_unregister_adapter(&adap->dvb_adap);
+	kfree(adap);
+	pt3->adaps[index] = NULL;
+}
+
+#ifdef CONFIG_PM_SLEEP
+
+static int pt3_suspend(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pt3_board *pt3 = pci_get_drvdata(pdev);
+	int i;
+	struct pt3_adapter *adap;
+
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		adap = pt3->adaps[i];
+		if (adap->num_feeds > 0)
+			pt3_stop_dma(adap);
+		dvb_frontend_suspend(adap->fe);
+		pt3_free_dmabuf(adap);
+	}
+
+	pt3_lnb_ctrl(pt3, false);
+	pt3_set_tuner_power(pt3, false, false);
+	return 0;
+}
+
+static int pt3_resume(struct device *dev)
+{
+	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pt3_board *pt3 = pci_get_drvdata(pdev);
+	int i, ret;
+	struct pt3_adapter *adap;
+
+	ret = pt3_fe_init(pt3);
+	if (ret)
+		return ret;
+
+	if (pt3->lna_on_cnt > 0)
+		pt3_set_tuner_power(pt3, true, true);
+	if (pt3->lnb_on_cnt > 0)
+		pt3_lnb_ctrl(pt3, true);
+
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		adap = pt3->adaps[i];
+		dvb_frontend_resume(adap->fe);
+		ret = pt3_alloc_dmabuf(adap);
+		if (ret) {
+			dev_err(&pt3->pdev->dev, "failed to alloc DMA bufs.\n");
+			continue;
+		}
+		if (adap->num_feeds > 0)
+			pt3_start_dma(adap);
+	}
+
+	return 0;
+}
+
+#endif /* CONFIG_PM_SLEEP */
+
+
+static void pt3_remove(struct pci_dev *pdev)
+{
+	struct pt3_board *pt3;
+	int i;
+
+	pt3 = pci_get_drvdata(pdev);
+	for (i = PT3_NUM_FE - 1; i >= 0; i--)
+		pt3_cleanup_adapter(pt3, i);
+	i2c_del_adapter(&pt3->i2c_adap);
+	kfree(pt3->i2c_buf);
+	pci_iounmap(pt3->pdev, pt3->regs[0]);
+	pci_iounmap(pt3->pdev, pt3->regs[1]);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	kfree(pt3);
+}
+
+static int pt3_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	u8 rev;
+	u32 ver;
+	int i, ret;
+	struct pt3_board *pt3;
+	struct i2c_adapter *i2c;
+
+	if (pci_read_config_byte(pdev, PCI_REVISION_ID, &rev) || rev != 1)
+		return -ENODEV;
+
+	ret = pci_enable_device(pdev);
+	if (ret < 0)
+		return -ENODEV;
+	pci_set_master(pdev);
+
+	ret = pci_request_regions(pdev, DRV_NAME);
+	if (ret < 0)
+		goto err_disable_device;
+
+	ret = dma_set_mask(&pdev->dev, DMA_BIT_MASK(64));
+	if (ret == 0)
+		dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(64));
+	else {
+		ret = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
+		if (ret == 0)
+			dma_set_coherent_mask(&pdev->dev, DMA_BIT_MASK(32));
+		else {
+			dev_err(&pdev->dev, "Failed to set DMA mask.\n");
+			goto err_release_regions;
+		}
+		dev_info(&pdev->dev, "Use 32bit DMA.\n");
+	}
+
+	pt3 = kzalloc(sizeof(*pt3), GFP_KERNEL);
+	if (!pt3) {
+		dev_err(&pdev->dev, "Failed to alloc mem for this dev.\n");
+		ret = -ENOMEM;
+		goto err_release_regions;
+	}
+	pci_set_drvdata(pdev, pt3);
+	pt3->pdev = pdev;
+	mutex_init(&pt3->lock);
+	pt3->regs[0] = pci_ioremap_bar(pdev, 0);
+	pt3->regs[1] = pci_ioremap_bar(pdev, 2);
+	if (pt3->regs[0] == NULL || pt3->regs[1] == NULL) {
+		dev_err(&pdev->dev, "Failed to ioremap.\n");
+		ret = -ENOMEM;
+		goto err_kfree;
+	}
+
+	ver = ioread32(pt3->regs[0] + REG_VERSION);
+	if ((ver >> 16) != 0x0301) {
+		dev_warn(&pdev->dev, "PT%d, I/F-ver.:%d not supported",
+			ver >> 24, (ver & 0x00ff0000) >> 16);
+		ret = -ENODEV;
+		goto err_iounmap;
+	}
+
+	pt3->num_bufs = clamp_val(num_bufs, MIN_DATA_BUFS, MAX_DATA_BUFS);
+
+	pt3->i2c_buf = kmalloc(sizeof(*pt3->i2c_buf), GFP_KERNEL);
+	if (pt3->i2c_buf == NULL) {
+		dev_err(&pdev->dev, "Failed to alloc mem for i2c.\n");
+		ret = -ENOMEM;
+		goto err_iounmap;
+	}
+	i2c = &pt3->i2c_adap;
+	i2c->owner = THIS_MODULE;
+	i2c->algo = &pt3_i2c_algo;
+	i2c->algo_data = NULL;
+	i2c->dev.parent = &pdev->dev;
+	strlcpy(i2c->name, DRV_NAME, sizeof(i2c->name));
+	i2c_set_adapdata(i2c, pt3);
+	ret = i2c_add_adapter(i2c);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to add i2c adapter.\n");
+		goto err_i2cbuf;
+	}
+
+	for (i = 0; i < PT3_NUM_FE; i++) {
+		ret = pt3_alloc_adapter(pt3, i);
+		if (ret < 0)
+			break;
+
+		ret = pt3_attach_fe(pt3, i);
+		if (ret < 0)
+			break;
+	}
+	if (i < PT3_NUM_FE) {
+		dev_err(&pdev->dev, "Failed to create FE%d.\n", i);
+		goto err_cleanup_adapters;
+	}
+
+	ret = pt3_fe_init(pt3);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to init frontends.\n");
+		i = PT3_NUM_FE - 1;
+		goto err_cleanup_adapters;
+	}
+
+	dev_info(&pdev->dev,
+		"successfully init'ed PT%d (fw:0x%02x, I/F:0x%02x).\n",
+		ver >> 24, (ver >> 8) & 0xff, (ver >> 16) & 0xff);
+	return 0;
+
+err_cleanup_adapters:
+	while (i >= 0)
+		pt3_cleanup_adapter(pt3, i--);
+	i2c_del_adapter(i2c);
+err_i2cbuf:
+	kfree(pt3->i2c_buf);
+err_iounmap:
+	if (pt3->regs[0])
+		pci_iounmap(pdev, pt3->regs[0]);
+	if (pt3->regs[1])
+		pci_iounmap(pdev, pt3->regs[1]);
+err_kfree:
+	kfree(pt3);
+err_release_regions:
+	pci_release_regions(pdev);
+err_disable_device:
+	pci_disable_device(pdev);
+	return ret;
+
+}
+
+static const struct pci_device_id pt3_id_table[] = {
+	{ PCI_DEVICE_SUB(0x1172, 0x4c15, 0xee8d, 0x0368) },
+	{ },
+};
+MODULE_DEVICE_TABLE(pci, pt3_id_table);
+
+static SIMPLE_DEV_PM_OPS(pt3_pm_ops, pt3_suspend, pt3_resume);
+
+static struct pci_driver pt3_driver = {
+	.name		= DRV_NAME,
+	.probe		= pt3_probe,
+	.remove		= pt3_remove,
+	.id_table	= pt3_id_table,
+
+	.driver.pm	= &pt3_pm_ops,
+};
+
+module_pci_driver(pt3_driver);
+
+MODULE_DESCRIPTION("Earthsoft PT3 Driver");
+MODULE_AUTHOR("Akihiro TSUKADA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/pci/pt3/pt3.h b/drivers/media/pci/pt3/pt3.h
new file mode 100644
index 000000000000..1b3f2ad25db3
--- /dev/null
+++ b/drivers/media/pci/pt3/pt3.h
@@ -0,0 +1,186 @@
+/*
+ * Earthsoft PT3 driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef PT3_H
+#define PT3_H
+
+#include <linux/atomic.h>
+#include <linux/types.h>
+
+#include "dvb_demux.h"
+#include "dvb_frontend.h"
+#include "dmxdev.h"
+
+#include "tc90522.h"
+#include "mxl301rf.h"
+#include "qm1d1c0042.h"
+
+#define DRV_NAME KBUILD_MODNAME
+
+#define PT3_NUM_FE 4
+
+/*
+ * register index of the FPGA chip
+ */
+#define REG_VERSION	0x00
+#define REG_BUS		0x04
+#define REG_SYSTEM_W	0x08
+#define REG_SYSTEM_R	0x0c
+#define REG_I2C_W	0x10
+#define REG_I2C_R	0x14
+#define REG_RAM_W	0x18
+#define REG_RAM_R	0x1c
+#define REG_DMA_BASE	0x40	/* regs for FE[i] = REG_DMA_BASE + 0x18 * i */
+#define OFST_DMA_DESC_L	0x00
+#define OFST_DMA_DESC_H	0x04
+#define OFST_DMA_CTL	0x08
+#define OFST_TS_CTL	0x0c
+#define OFST_STATUS	0x10
+#define OFST_TS_ERR	0x14
+
+/*
+ * internal buffer for I2C
+ */
+#define PT3_I2C_MAX 4091
+struct pt3_i2cbuf {
+	u8  data[PT3_I2C_MAX];
+	u8  tmp;
+	u32 num_cmds;
+};
+
+/*
+ * DMA things
+ */
+#define TS_PACKET_SZ  188
+/* DMA transfers must not cross 4GiB, so use one page / transfer */
+#define DATA_XFER_SZ   4096
+#define DATA_BUF_XFERS 47
+/* (num_bufs * DATA_BUF_SZ) % TS_PACKET_SZ must be 0 */
+#define DATA_BUF_SZ    (DATA_BUF_XFERS * DATA_XFER_SZ)
+#define MAX_DATA_BUFS  16
+#define MIN_DATA_BUFS   2
+
+#define DESCS_IN_PAGE (PAGE_SIZE / sizeof(struct xfer_desc))
+#define MAX_NUM_XFERS (MAX_DATA_BUFS * DATA_BUF_XFERS)
+#define MAX_DESC_BUFS DIV_ROUND_UP(MAX_NUM_XFERS, DESCS_IN_PAGE)
+
+/* DMA transfer description.
+ * device is passed a pointer to this struct, dma-reads it,
+ * and gets the DMA buffer ring for storing TS data.
+ */
+struct xfer_desc {
+	u32 addr_l; /* bus address of target data buffer */
+	u32 addr_h;
+	u32 size;
+	u32 next_l; /* bus adddress of the next xfer_desc */
+	u32 next_h;
+};
+
+/* A DMA mapping of a page containing xfer_desc's */
+struct xfer_desc_buffer {
+	dma_addr_t b_addr;
+	struct xfer_desc *descs; /* PAGE_SIZE (xfer_desc[DESCS_IN_PAGE]) */
+};
+
+/* A DMA mapping of a data buffer */
+struct dma_data_buffer {
+	dma_addr_t b_addr;
+	u8 *data; /* size: u8[PAGE_SIZE] */
+};
+
+/*
+ * device things
+ */
+struct pt3_adap_config {
+	struct i2c_board_info demod_info;
+	struct tc90522_config demod_cfg;
+
+	struct i2c_board_info tuner_info;
+	union tuner_config {
+		struct qm1d1c0042_config qm1d1c0042;
+		struct mxl301rf_config   mxl301rf;
+	} tuner_cfg;
+	u32 init_freq;
+};
+
+struct pt3_adapter {
+	struct dvb_adapter  dvb_adap;  /* dvb_adap.priv => struct pt3_board */
+	int adap_idx;
+
+	struct dvb_demux    demux;
+	struct dmxdev       dmxdev;
+	struct dvb_frontend *fe;
+	struct i2c_client   *i2c_demod;
+	struct i2c_client   *i2c_tuner;
+
+	/* data fetch thread */
+	struct task_struct *thread;
+	int num_feeds;
+
+	bool cur_lna;
+	bool cur_lnb; /* current LNB power status (on/off) */
+
+	/* items below are for DMA */
+	struct dma_data_buffer buffer[MAX_DATA_BUFS];
+	int buf_idx;
+	int buf_ofs;
+	int num_bufs;  /* == pt3_board->num_bufs */
+	int num_discard; /* how many access units to discard initially */
+
+	struct xfer_desc_buffer desc_buf[MAX_DESC_BUFS];
+	int num_desc_bufs;  /* == num_bufs * DATA_BUF_XFERS / DESCS_IN_PAGE */
+};
+
+
+struct pt3_board {
+	struct pci_dev *pdev;
+	void __iomem *regs[2];
+	/* regs[0]: registers, regs[1]: internal memory, used for I2C */
+
+	struct mutex lock;
+
+	/* LNB power shared among sat-FEs */
+	int lnb_on_cnt; /* LNB power on count */
+
+	/* LNA shared among terr-FEs */
+	int lna_on_cnt; /* booster enabled count */
+
+	int num_bufs;  /* number of DMA buffers allocated/mapped per FE */
+
+	struct i2c_adapter i2c_adap;
+	struct pt3_i2cbuf *i2c_buf;
+
+	struct pt3_adapter *adaps[PT3_NUM_FE];
+};
+
+
+/*
+ * prototypes
+ */
+extern int  pt3_alloc_dmabuf(struct pt3_adapter *adap);
+extern void pt3_init_dmabuf(struct pt3_adapter *adap);
+extern void pt3_free_dmabuf(struct pt3_adapter *adap);
+extern int  pt3_start_dma(struct pt3_adapter *adap);
+extern int  pt3_stop_dma(struct pt3_adapter *adap);
+extern int  pt3_proc_dma(struct pt3_adapter *adap);
+
+extern int  pt3_i2c_master_xfer(struct i2c_adapter *adap,
+				struct i2c_msg *msgs, int num);
+extern u32  pt3_i2c_functionality(struct i2c_adapter *adap);
+extern void pt3_i2c_reset(struct pt3_board *pt3);
+extern int  pt3_init_all_demods(struct pt3_board *pt3);
+extern int  pt3_init_all_mxl301rf(struct pt3_board *pt3);
+#endif /* PT3_H */
diff --git a/drivers/media/pci/pt3/pt3_dma.c b/drivers/media/pci/pt3/pt3_dma.c
new file mode 100644
index 000000000000..f0ce90437fac
--- /dev/null
+++ b/drivers/media/pci/pt3/pt3_dma.c
@@ -0,0 +1,225 @@
+/*
+ * Earthsoft PT3 driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/dma-mapping.h>
+#include <linux/kernel.h>
+#include <linux/pci.h>
+
+#include "pt3.h"
+
+#define PT3_ACCESS_UNIT (TS_PACKET_SZ * 128)
+#define PT3_BUF_CANARY  (0x74)
+
+static u32 get_dma_base(int idx)
+{
+	int i;
+
+	i = (idx == 1 || idx == 2) ? 3 - idx : idx;
+	return REG_DMA_BASE + 0x18 * i;
+}
+
+int pt3_stop_dma(struct pt3_adapter *adap)
+{
+	struct pt3_board *pt3 = adap->dvb_adap.priv;
+	u32 base;
+	u32 stat;
+	int retry;
+
+	base = get_dma_base(adap->adap_idx);
+	stat = ioread32(pt3->regs[0] + base + OFST_STATUS);
+	if (!(stat & 0x01))
+		return 0;
+
+	iowrite32(0x02, pt3->regs[0] + base + OFST_DMA_CTL);
+	for (retry = 0; retry < 5; retry++) {
+		stat = ioread32(pt3->regs[0] + base + OFST_STATUS);
+		if (!(stat & 0x01))
+			return 0;
+		msleep(50);
+	}
+	return -EIO;
+}
+
+int pt3_start_dma(struct pt3_adapter *adap)
+{
+	struct pt3_board *pt3 = adap->dvb_adap.priv;
+	u32 base = get_dma_base(adap->adap_idx);
+
+	iowrite32(0x02, pt3->regs[0] + base + OFST_DMA_CTL);
+	iowrite32(lower_32_bits(adap->desc_buf[0].b_addr),
+			pt3->regs[0] + base + OFST_DMA_DESC_L);
+	iowrite32(upper_32_bits(adap->desc_buf[0].b_addr),
+			pt3->regs[0] + base + OFST_DMA_DESC_H);
+	iowrite32(0x01, pt3->regs[0] + base + OFST_DMA_CTL);
+	return 0;
+}
+
+
+static u8 *next_unit(struct pt3_adapter *adap, int *idx, int *ofs)
+{
+	*ofs += PT3_ACCESS_UNIT;
+	if (*ofs >= DATA_BUF_SZ) {
+		*ofs -= DATA_BUF_SZ;
+		(*idx)++;
+		if (*idx == adap->num_bufs)
+			*idx = 0;
+	}
+	return &adap->buffer[*idx].data[*ofs];
+}
+
+int pt3_proc_dma(struct pt3_adapter *adap)
+{
+	int idx, ofs;
+
+	idx = adap->buf_idx;
+	ofs = adap->buf_ofs;
+
+	if (adap->buffer[idx].data[ofs] == PT3_BUF_CANARY)
+		return 0;
+
+	while (*next_unit(adap, &idx, &ofs) != PT3_BUF_CANARY) {
+		u8 *p;
+
+		p = &adap->buffer[adap->buf_idx].data[adap->buf_ofs];
+		if (adap->num_discard > 0)
+			adap->num_discard--;
+		else if (adap->buf_ofs + PT3_ACCESS_UNIT > DATA_BUF_SZ) {
+			dvb_dmx_swfilter_packets(&adap->demux, p,
+				(DATA_BUF_SZ - adap->buf_ofs) / TS_PACKET_SZ);
+			dvb_dmx_swfilter_packets(&adap->demux,
+				adap->buffer[idx].data, ofs / TS_PACKET_SZ);
+		} else
+			dvb_dmx_swfilter_packets(&adap->demux, p,
+				PT3_ACCESS_UNIT / TS_PACKET_SZ);
+
+		*p = PT3_BUF_CANARY;
+		adap->buf_idx = idx;
+		adap->buf_ofs = ofs;
+	}
+	return 0;
+}
+
+void pt3_init_dmabuf(struct pt3_adapter *adap)
+{
+	int idx, ofs;
+	u8 *p;
+
+	idx = 0;
+	ofs = 0;
+	p = adap->buffer[0].data;
+	/* mark the whole buffers as "not written yet" */
+	while (idx < adap->num_bufs) {
+		p[ofs] = PT3_BUF_CANARY;
+		ofs += PT3_ACCESS_UNIT;
+		if (ofs >= DATA_BUF_SZ) {
+			ofs -= DATA_BUF_SZ;
+			idx++;
+			p = adap->buffer[idx].data;
+		}
+	}
+	adap->buf_idx = 0;
+	adap->buf_ofs = 0;
+}
+
+void pt3_free_dmabuf(struct pt3_adapter *adap)
+{
+	struct pt3_board *pt3;
+	int i;
+
+	pt3 = adap->dvb_adap.priv;
+	for (i = 0; i < adap->num_bufs; i++)
+		dma_free_coherent(&pt3->pdev->dev, DATA_BUF_SZ,
+			adap->buffer[i].data, adap->buffer[i].b_addr);
+	adap->num_bufs = 0;
+
+	for (i = 0; i < adap->num_desc_bufs; i++)
+		dma_free_coherent(&pt3->pdev->dev, PAGE_SIZE,
+			adap->desc_buf[i].descs, adap->desc_buf[i].b_addr);
+	adap->num_desc_bufs = 0;
+}
+
+
+int pt3_alloc_dmabuf(struct pt3_adapter *adap)
+{
+	struct pt3_board *pt3;
+	void *p;
+	int i, j;
+	int idx, ofs;
+	int num_desc_bufs;
+	dma_addr_t data_addr, desc_addr;
+	struct xfer_desc *d;
+
+	pt3 = adap->dvb_adap.priv;
+	adap->num_bufs = 0;
+	adap->num_desc_bufs = 0;
+	for (i = 0; i < pt3->num_bufs; i++) {
+		p = dma_alloc_coherent(&pt3->pdev->dev, DATA_BUF_SZ,
+					&adap->buffer[i].b_addr, GFP_KERNEL);
+		if (p == NULL)
+			goto failed;
+		adap->buffer[i].data = p;
+		adap->num_bufs++;
+	}
+	pt3_init_dmabuf(adap);
+
+	/* build circular-linked pointers (xfer_desc) to the data buffers*/
+	idx = 0;
+	ofs = 0;
+	num_desc_bufs =
+		DIV_ROUND_UP(adap->num_bufs * DATA_BUF_XFERS, DESCS_IN_PAGE);
+	for (i = 0; i < num_desc_bufs; i++) {
+		p = dma_alloc_coherent(&pt3->pdev->dev, PAGE_SIZE,
+					&desc_addr, GFP_KERNEL);
+		if (p == NULL)
+			goto failed;
+		adap->num_desc_bufs++;
+		adap->desc_buf[i].descs = p;
+		adap->desc_buf[i].b_addr = desc_addr;
+
+		if (i > 0) {
+			d = &adap->desc_buf[i - 1].descs[DESCS_IN_PAGE - 1];
+			d->next_l = lower_32_bits(desc_addr);
+			d->next_h = upper_32_bits(desc_addr);
+		}
+		for (j = 0; j < DESCS_IN_PAGE; j++) {
+			data_addr = adap->buffer[idx].b_addr + ofs;
+			d = &adap->desc_buf[i].descs[j];
+			d->addr_l = lower_32_bits(data_addr);
+			d->addr_h = upper_32_bits(data_addr);
+			d->size = DATA_XFER_SZ;
+
+			desc_addr += sizeof(struct xfer_desc);
+			d->next_l = lower_32_bits(desc_addr);
+			d->next_h = upper_32_bits(desc_addr);
+
+			ofs += DATA_XFER_SZ;
+			if (ofs >= DATA_BUF_SZ) {
+				ofs -= DATA_BUF_SZ;
+				idx++;
+				if (idx >= adap->num_bufs) {
+					desc_addr = adap->desc_buf[0].b_addr;
+					d->next_l = lower_32_bits(desc_addr);
+					d->next_h = upper_32_bits(desc_addr);
+					return 0;
+				}
+			}
+		}
+	}
+	return 0;
+
+failed:
+	pt3_free_dmabuf(adap);
+	return -ENOMEM;
+}
diff --git a/drivers/media/pci/pt3/pt3_i2c.c b/drivers/media/pci/pt3/pt3_i2c.c
new file mode 100644
index 000000000000..ec6a8a2e4744
--- /dev/null
+++ b/drivers/media/pci/pt3/pt3_i2c.c
@@ -0,0 +1,240 @@
+/*
+ * Earthsoft PT3 driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/i2c.h>
+#include <linux/io.h>
+#include <linux/pci.h>
+
+#include "pt3.h"
+
+#define PT3_I2C_BASE  2048
+#define PT3_CMD_ADDR_NORMAL 0
+#define PT3_CMD_ADDR_INIT_DEMOD  4096
+#define PT3_CMD_ADDR_INIT_TUNER  (4096 + 2042)
+
+/* masks for I2C status register */
+#define STAT_SEQ_RUNNING 0x1
+#define STAT_SEQ_ERROR   0x6
+#define STAT_NO_SEQ      0x8
+
+#define PT3_I2C_RUN   (1 << 16)
+#define PT3_I2C_RESET (1 << 17)
+
+enum ctl_cmd {
+	I_END,
+	I_ADDRESS,
+	I_CLOCK_L,
+	I_CLOCK_H,
+	I_DATA_L,
+	I_DATA_H,
+	I_RESET,
+	I_SLEEP,
+	I_DATA_L_NOP  = 0x08,
+	I_DATA_H_NOP  = 0x0c,
+	I_DATA_H_READ = 0x0d,
+	I_DATA_H_ACK0 = 0x0e,
+	I_DATA_H_ACK1 = 0x0f,
+};
+
+
+static void cmdbuf_add(struct pt3_i2cbuf *cbuf, enum ctl_cmd cmd)
+{
+	int buf_idx;
+
+	if ((cbuf->num_cmds % 2) == 0)
+		cbuf->tmp = cmd;
+	else {
+		cbuf->tmp |= cmd << 4;
+		buf_idx = cbuf->num_cmds / 2;
+		if (buf_idx < ARRAY_SIZE(cbuf->data))
+			cbuf->data[buf_idx] = cbuf->tmp;
+	}
+	cbuf->num_cmds++;
+}
+
+static void put_end(struct pt3_i2cbuf *cbuf)
+{
+	cmdbuf_add(cbuf, I_END);
+	if (cbuf->num_cmds % 2)
+		cmdbuf_add(cbuf, I_END);
+}
+
+static void put_start(struct pt3_i2cbuf *cbuf)
+{
+	cmdbuf_add(cbuf, I_DATA_H);
+	cmdbuf_add(cbuf, I_CLOCK_H);
+	cmdbuf_add(cbuf, I_DATA_L);
+	cmdbuf_add(cbuf, I_CLOCK_L);
+}
+
+static void put_byte_write(struct pt3_i2cbuf *cbuf, u8 val)
+{
+	u8 mask;
+
+	mask = 0x80;
+	for (mask = 0x80; mask > 0; mask >>= 1)
+		cmdbuf_add(cbuf, (val & mask) ? I_DATA_H_NOP : I_DATA_L_NOP);
+	cmdbuf_add(cbuf, I_DATA_H_ACK0);
+}
+
+static void put_byte_read(struct pt3_i2cbuf *cbuf, u32 size)
+{
+	int i, j;
+
+	for (i = 0; i < size; i++) {
+		for (j = 0; j < 8; j++)
+			cmdbuf_add(cbuf, I_DATA_H_READ);
+		cmdbuf_add(cbuf, (i == size - 1) ? I_DATA_H_NOP : I_DATA_L_NOP);
+	}
+}
+
+static void put_stop(struct pt3_i2cbuf *cbuf)
+{
+	cmdbuf_add(cbuf, I_DATA_L);
+	cmdbuf_add(cbuf, I_CLOCK_H);
+	cmdbuf_add(cbuf, I_DATA_H);
+}
+
+
+/* translates msgs to internal commands for bit-banging */
+static void translate(struct pt3_i2cbuf *cbuf, struct i2c_msg *msgs, int num)
+{
+	int i, j;
+	bool rd;
+
+	cbuf->num_cmds = 0;
+	for (i = 0; i < num; i++) {
+		rd = !!(msgs[i].flags & I2C_M_RD);
+		put_start(cbuf);
+		put_byte_write(cbuf, msgs[i].addr << 1 | rd);
+		if (rd)
+			put_byte_read(cbuf, msgs[i].len);
+		else
+			for (j = 0; j < msgs[i].len; j++)
+				put_byte_write(cbuf, msgs[i].buf[j]);
+	}
+	if (num > 0) {
+		put_stop(cbuf);
+		put_end(cbuf);
+	}
+}
+
+static int wait_i2c_result(struct pt3_board *pt3, u32 *result, int max_wait)
+{
+	int i;
+	u32 v;
+
+	for (i = 0; i < max_wait; i++) {
+		v = ioread32(pt3->regs[0] + REG_I2C_R);
+		if (!(v & STAT_SEQ_RUNNING))
+			break;
+		usleep_range(500, 750);
+	}
+	if (i >= max_wait)
+		return -EIO;
+	if (result)
+		*result = v;
+	return 0;
+}
+
+/* send [pre-]translated i2c msgs stored at addr */
+static int send_i2c_cmd(struct pt3_board *pt3, u32 addr)
+{
+	u32 ret;
+
+	/* make sure that previous transactions had finished */
+	if (wait_i2c_result(pt3, NULL, 50)) {
+		dev_warn(&pt3->pdev->dev, "(%s) prev. transaction stalled\n",
+				__func__);
+		return -EIO;
+	}
+
+	iowrite32(PT3_I2C_RUN | addr, pt3->regs[0] + REG_I2C_W);
+	usleep_range(200, 300);
+	/* wait for the current transaction to finish */
+	if (wait_i2c_result(pt3, &ret, 500) || (ret & STAT_SEQ_ERROR)) {
+		dev_warn(&pt3->pdev->dev, "(%s) failed.\n", __func__);
+		return -EIO;
+	}
+	return 0;
+}
+
+
+/* init commands for each demod are combined into one transaction
+ *  and hidden in ROM with the address PT3_CMD_ADDR_INIT_DEMOD.
+ */
+int  pt3_init_all_demods(struct pt3_board *pt3)
+{
+	ioread32(pt3->regs[0] + REG_I2C_R);
+	return send_i2c_cmd(pt3, PT3_CMD_ADDR_INIT_DEMOD);
+}
+
+/* init commands for two ISDB-T tuners are hidden in ROM. */
+int  pt3_init_all_mxl301rf(struct pt3_board *pt3)
+{
+	usleep_range(1000, 2000);
+	return send_i2c_cmd(pt3, PT3_CMD_ADDR_INIT_TUNER);
+}
+
+void pt3_i2c_reset(struct pt3_board *pt3)
+{
+	iowrite32(PT3_I2C_RESET, pt3->regs[0] + REG_I2C_W);
+}
+
+/*
+ * I2C algorithm
+ */
+int
+pt3_i2c_master_xfer(struct i2c_adapter *adap, struct i2c_msg *msgs, int num)
+{
+	struct pt3_board *pt3;
+	struct pt3_i2cbuf *cbuf;
+	int i;
+	void __iomem *p;
+
+	pt3 = i2c_get_adapdata(adap);
+	cbuf = pt3->i2c_buf;
+
+	for (i = 0; i < num; i++)
+		if (msgs[i].flags & I2C_M_RECV_LEN) {
+			dev_warn(&pt3->pdev->dev,
+				"(%s) I2C_M_RECV_LEN not supported.\n",
+				__func__);
+			return -EINVAL;
+		}
+
+	translate(cbuf, msgs, num);
+	memcpy_toio(pt3->regs[1] + PT3_I2C_BASE + PT3_CMD_ADDR_NORMAL / 2,
+			cbuf->data, cbuf->num_cmds);
+
+	if (send_i2c_cmd(pt3, PT3_CMD_ADDR_NORMAL) < 0)
+		return -EIO;
+
+	p = pt3->regs[1] + PT3_I2C_BASE;
+	for (i = 0; i < num; i++)
+		if ((msgs[i].flags & I2C_M_RD) && msgs[i].len > 0) {
+			memcpy_fromio(msgs[i].buf, p, msgs[i].len);
+			p += msgs[i].len;
+		}
+
+	return num;
+}
+
+u32 pt3_i2c_functionality(struct i2c_adapter *adap)
+{
+	return I2C_FUNC_I2C;
+}
diff --git a/drivers/media/pci/saa7134/Kconfig b/drivers/media/pci/saa7134/Kconfig
index 18ae75546302..b44e0d70907e 100644
--- a/drivers/media/pci/saa7134/Kconfig
+++ b/drivers/media/pci/saa7134/Kconfig
@@ -63,3 +63,11 @@ config VIDEO_SAA7134_DVB
 
 	  To compile this driver as a module, choose M here: the
 	  module will be called saa7134-dvb.
+
+config VIDEO_SAA7134_GO7007
+	tristate "go7007 support for saa7134 based TV cards"
+	depends on VIDEO_SAA7134
+	depends on VIDEO_GO7007
+	---help---
+	  Enables saa7134 driver support for boards with go7007
+	  MPEG encoder (WIS Voyager or compatible).
diff --git a/drivers/media/pci/saa7134/Makefile b/drivers/media/pci/saa7134/Makefile
index 58de9b085689..09c43da67588 100644
--- a/drivers/media/pci/saa7134/Makefile
+++ b/drivers/media/pci/saa7134/Makefile
@@ -5,6 +5,7 @@ saa7134-y +=	saa7134-video.o
 saa7134-$(CONFIG_VIDEO_SAA7134_RC) += saa7134-input.o
 
 obj-$(CONFIG_VIDEO_SAA7134) +=  saa7134.o saa7134-empress.o
+obj-$(CONFIG_VIDEO_SAA7134_GO7007) += saa7134-go7007.o
 
 obj-$(CONFIG_VIDEO_SAA7134_ALSA) += saa7134-alsa.o
 
@@ -14,3 +15,4 @@ ccflags-y += -I$(srctree)/drivers/media/i2c
 ccflags-y += -I$(srctree)/drivers/media/tuners
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
+ccflags-y += -I$(srctree)/drivers/media/usb/go7007
diff --git a/drivers/media/pci/saa7134/saa7134-cards.c b/drivers/media/pci/saa7134/saa7134-cards.c
index 6e4bdb90aa92..3ca078057755 100644
--- a/drivers/media/pci/saa7134/saa7134-cards.c
+++ b/drivers/media/pci/saa7134/saa7134-cards.c
@@ -5827,6 +5827,29 @@ struct saa7134_board saa7134_boards[] = {
 			.gpio = 0x0000800,
 		},
 	},
+	[SAA7134_BOARD_WIS_VOYAGER] = {
+		.name           = "WIS Voyager or compatible",
+		.audio_clock    = 0x00200000,
+		.tuner_type	= TUNER_PHILIPS_TDA8290,
+		.radio_type     = UNSET,
+		.tuner_addr     = ADDR_UNSET,
+		.radio_addr     = ADDR_UNSET,
+		.mpeg		= SAA7134_MPEG_GO7007,
+		.inputs		= { {
+			.name = name_comp1,
+			.vmux = 0,
+			.amux = LINE2,
+		}, {
+			.name = name_tv,
+			.vmux = 3,
+			.amux = TV,
+			.tv   = 1,
+		}, {
+			.name = name_svideo,
+			.vmux = 6,
+		.amux = LINE1,
+		} },
+	},
 
 };
 
@@ -7080,6 +7103,12 @@ struct pci_device_id saa7134_pci_tbl[] = {
 		.subdevice    = 0x2055, /* AverTV Satellite Hybrid+FM A706 */
 		.driver_data  = SAA7134_BOARD_AVERMEDIA_A706,
 	}, {
+		.vendor       = PCI_VENDOR_ID_PHILIPS,
+		.device       = PCI_DEVICE_ID_PHILIPS_SAA7133,
+		.subvendor    = 0x1905, /* WIS */
+		.subdevice    = 0x7007,
+		.driver_data  = SAA7134_BOARD_WIS_VOYAGER,
+	}, {
 		/* --- boards without eeprom + subsystem ID --- */
 		.vendor       = PCI_VENDOR_ID_PHILIPS,
 		.device       = PCI_DEVICE_ID_PHILIPS_SAA7134,
diff --git a/drivers/media/pci/saa7134/saa7134-core.c b/drivers/media/pci/saa7134/saa7134-core.c
index 9ff03a69ced4..236ed725f933 100644
--- a/drivers/media/pci/saa7134/saa7134-core.c
+++ b/drivers/media/pci/saa7134/saa7134-core.c
@@ -160,6 +160,8 @@ static void request_module_async(struct work_struct *work){
 		request_module("saa7134-empress");
 	if (card_is_dvb(dev))
 		request_module("saa7134-dvb");
+	if (card_is_go7007(dev))
+		request_module("saa7134-go7007");
 	if (alsa) {
 		if (dev->pci->device != PCI_DEVICE_ID_PHILIPS_SAA7130)
 			request_module("saa7134-alsa");
@@ -563,8 +565,12 @@ static irqreturn_t saa7134_irq(int irq, void *dev_id)
 			saa7134_irq_vbi_done(dev,status);
 
 		if ((report & SAA7134_IRQ_REPORT_DONE_RA2) &&
-		    card_has_mpeg(dev))
-			saa7134_irq_ts_done(dev,status);
+		    card_has_mpeg(dev)) {
+			if (dev->mops->irq_ts_done != NULL)
+				dev->mops->irq_ts_done(dev, status);
+			else
+				saa7134_irq_ts_done(dev, status);
+		}
 
 		if (report & SAA7134_IRQ_REPORT_GPIO16) {
 			switch (dev->has_remote) {
diff --git a/drivers/media/pci/saa7134/saa7134-go7007.c b/drivers/media/pci/saa7134/saa7134-go7007.c
new file mode 100644
index 000000000000..54e650b4dff1
--- /dev/null
+++ b/drivers/media/pci/saa7134/saa7134-go7007.c
@@ -0,0 +1,531 @@
+/*
+ * Copyright (C) 2005-2006 Micronas USA Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License (Version 2) as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/spinlock.h>
+#include <linux/wait.h>
+#include <linux/list.h>
+#include <linux/slab.h>
+#include <linux/time.h>
+#include <linux/mm.h>
+#include <linux/usb.h>
+#include <linux/i2c.h>
+#include <asm/byteorder.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-subdev.h>
+
+#include "saa7134.h"
+#include "saa7134-reg.h"
+#include "go7007-priv.h"
+
+/*#define GO7007_HPI_DEBUG*/
+
+enum hpi_address {
+	HPI_ADDR_VIDEO_BUFFER = 0xe4,
+	HPI_ADDR_INIT_BUFFER = 0xea,
+	HPI_ADDR_INTR_RET_VALUE = 0xee,
+	HPI_ADDR_INTR_RET_DATA = 0xec,
+	HPI_ADDR_INTR_STATUS = 0xf4,
+	HPI_ADDR_INTR_WR_PARAM = 0xf6,
+	HPI_ADDR_INTR_WR_INDEX = 0xf8,
+};
+
+enum gpio_command {
+	GPIO_COMMAND_RESET = 0x00, /* 000b */
+	GPIO_COMMAND_REQ1  = 0x04, /* 001b */
+	GPIO_COMMAND_WRITE = 0x20, /* 010b */
+	GPIO_COMMAND_REQ2  = 0x24, /* 011b */
+	GPIO_COMMAND_READ  = 0x80, /* 100b */
+	GPIO_COMMAND_VIDEO = 0x84, /* 101b */
+	GPIO_COMMAND_IDLE  = 0xA0, /* 110b */
+	GPIO_COMMAND_ADDR  = 0xA4, /* 111b */
+};
+
+struct saa7134_go7007 {
+	struct v4l2_subdev sd;
+	struct saa7134_dev *dev;
+	u8 *top;
+	u8 *bottom;
+	dma_addr_t top_dma;
+	dma_addr_t bottom_dma;
+};
+
+static inline struct saa7134_go7007 *to_state(struct v4l2_subdev *sd)
+{
+	return container_of(sd, struct saa7134_go7007, sd);
+}
+
+static const struct go7007_board_info board_voyager = {
+	.flags		 = 0,
+	.sensor_flags	 = GO7007_SENSOR_656 |
+				GO7007_SENSOR_VALID_ENABLE |
+				GO7007_SENSOR_TV |
+				GO7007_SENSOR_VBI,
+	.audio_flags	= GO7007_AUDIO_I2S_MODE_1 |
+				GO7007_AUDIO_WORD_16,
+	.audio_rate	 = 48000,
+	.audio_bclk_div	 = 8,
+	.audio_main_div	 = 2,
+	.hpi_buffer_cap  = 7,
+	.num_inputs	 = 1,
+	.inputs		 = {
+		{
+			.name		= "SAA7134",
+		},
+	},
+};
+
+/********************* Driver for GPIO HPI interface *********************/
+
+static int gpio_write(struct saa7134_dev *dev, u8 addr, u16 data)
+{
+	saa_writeb(SAA7134_GPIO_GPMODE0, 0xff);
+
+	/* Write HPI address */
+	saa_writeb(SAA7134_GPIO_GPSTATUS0, addr);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_ADDR);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+
+	/* Write low byte */
+	saa_writeb(SAA7134_GPIO_GPSTATUS0, data & 0xff);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_WRITE);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+
+	/* Write high byte */
+	saa_writeb(SAA7134_GPIO_GPSTATUS0, data >> 8);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_WRITE);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+
+	return 0;
+}
+
+static int gpio_read(struct saa7134_dev *dev, u8 addr, u16 *data)
+{
+	saa_writeb(SAA7134_GPIO_GPMODE0, 0xff);
+
+	/* Write HPI address */
+	saa_writeb(SAA7134_GPIO_GPSTATUS0, addr);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_ADDR);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+
+	saa_writeb(SAA7134_GPIO_GPMODE0, 0x00);
+
+	/* Read low byte */
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_READ);
+	saa_clearb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+	saa_setb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+	*data = saa_readb(SAA7134_GPIO_GPSTATUS0);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+
+	/* Read high byte */
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_READ);
+	saa_clearb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+	saa_setb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+	*data |= saa_readb(SAA7134_GPIO_GPSTATUS0) << 8;
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+
+	return 0;
+}
+
+static int saa7134_go7007_interface_reset(struct go7007 *go)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev = saa->dev;
+	u16 intr_val, intr_data;
+	int count = 20;
+
+	saa_clearb(SAA7134_TS_PARALLEL, 0x80); /* Disable TS interface */
+	saa_writeb(SAA7134_GPIO_GPMODE2, 0xa4);
+	saa_writeb(SAA7134_GPIO_GPMODE0, 0xff);
+
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_REQ1);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_RESET);
+	msleep(1);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_REQ1);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_REQ2);
+	msleep(10);
+
+	saa_clearb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+	saa_setb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+
+	saa_readb(SAA7134_GPIO_GPSTATUS2);
+	/*pr_debug("status is %s\n", saa_readb(SAA7134_GPIO_GPSTATUS2) & 0x40 ? "OK" : "not OK"); */
+
+	/* enter command mode...(?) */
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_REQ1);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_REQ2);
+
+	do {
+		saa_clearb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+		saa_setb(SAA7134_GPIO_GPMODE3, SAA7134_GPIO_GPRESCAN);
+		saa_readb(SAA7134_GPIO_GPSTATUS2);
+		/*pr_info("gpio is %08x\n", saa_readl(SAA7134_GPIO_GPSTATUS0 >> 2)); */
+	} while (--count > 0);
+
+	/* Wait for an interrupt to indicate successful hardware reset */
+	if (go7007_read_interrupt(go, &intr_val, &intr_data) < 0 ||
+			(intr_val & ~0x1) != 0x55aa) {
+		pr_err("saa7134-go7007: unable to reset the GO7007\n");
+		return -1;
+	}
+	return 0;
+}
+
+static int saa7134_go7007_write_interrupt(struct go7007 *go, int addr, int data)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev = saa->dev;
+	int i;
+	u16 status_reg;
+
+#ifdef GO7007_HPI_DEBUG
+	pr_debug("saa7134-go7007: WriteInterrupt: %04x %04x\n", addr, data);
+#endif
+
+	for (i = 0; i < 100; ++i) {
+		gpio_read(dev, HPI_ADDR_INTR_STATUS, &status_reg);
+		if (!(status_reg & 0x0010))
+			break;
+		msleep(10);
+	}
+	if (i == 100) {
+		pr_err("saa7134-go7007: device is hung, status reg = 0x%04x\n",
+			status_reg);
+		return -1;
+	}
+	gpio_write(dev, HPI_ADDR_INTR_WR_PARAM, data);
+	gpio_write(dev, HPI_ADDR_INTR_WR_INDEX, addr);
+
+	return 0;
+}
+
+static int saa7134_go7007_read_interrupt(struct go7007 *go)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev = saa->dev;
+
+	/* XXX we need to wait if there is no interrupt available */
+	go->interrupt_available = 1;
+	gpio_read(dev, HPI_ADDR_INTR_RET_VALUE, &go->interrupt_value);
+	gpio_read(dev, HPI_ADDR_INTR_RET_DATA, &go->interrupt_data);
+#ifdef GO7007_HPI_DEBUG
+	pr_debug("saa7134-go7007: ReadInterrupt: %04x %04x\n",
+			go->interrupt_value, go->interrupt_data);
+#endif
+	return 0;
+}
+
+static void saa7134_go7007_irq_ts_done(struct saa7134_dev *dev,
+						unsigned long status)
+{
+	struct go7007 *go = video_get_drvdata(dev->empress_dev);
+	struct saa7134_go7007 *saa = go->hpi_context;
+
+	if (!vb2_is_streaming(&go->vidq))
+		return;
+	if (0 != (status & 0x000f0000))
+		pr_debug("saa7134-go7007: irq: lost %ld\n",
+				(status >> 16) & 0x0f);
+	if (status & 0x100000) {
+		dma_sync_single_for_cpu(&dev->pci->dev,
+					saa->bottom_dma, PAGE_SIZE, DMA_FROM_DEVICE);
+		go7007_parse_video_stream(go, saa->bottom, PAGE_SIZE);
+		saa_writel(SAA7134_RS_BA2(5), saa->bottom_dma);
+	} else {
+		dma_sync_single_for_cpu(&dev->pci->dev,
+					saa->top_dma, PAGE_SIZE, DMA_FROM_DEVICE);
+		go7007_parse_video_stream(go, saa->top, PAGE_SIZE);
+		saa_writel(SAA7134_RS_BA1(5), saa->top_dma);
+	}
+}
+
+static int saa7134_go7007_stream_start(struct go7007 *go)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev = saa->dev;
+
+	saa->top_dma = dma_map_page(&dev->pci->dev, virt_to_page(saa->top),
+			0, PAGE_SIZE, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&dev->pci->dev, saa->top_dma))
+		return -ENOMEM;
+	saa->bottom_dma = dma_map_page(&dev->pci->dev,
+			virt_to_page(saa->bottom),
+			0, PAGE_SIZE, DMA_FROM_DEVICE);
+	if (dma_mapping_error(&dev->pci->dev, saa->bottom_dma)) {
+		dma_unmap_page(&dev->pci->dev, saa->top_dma, PAGE_SIZE,
+				DMA_FROM_DEVICE);
+		return -ENOMEM;
+	}
+
+	saa_writel(SAA7134_VIDEO_PORT_CTRL0 >> 2, 0xA300B000);
+	saa_writel(SAA7134_VIDEO_PORT_CTRL4 >> 2, 0x40000200);
+
+	/* Set HPI interface for video */
+	saa_writeb(SAA7134_GPIO_GPMODE0, 0xff);
+	saa_writeb(SAA7134_GPIO_GPSTATUS0, HPI_ADDR_VIDEO_BUFFER);
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_ADDR);
+	saa_writeb(SAA7134_GPIO_GPMODE0, 0x00);
+
+	/* Enable TS interface */
+	saa_writeb(SAA7134_TS_PARALLEL, 0xe6);
+
+	/* Reset TS interface */
+	saa_setb(SAA7134_TS_SERIAL1, 0x01);
+	saa_clearb(SAA7134_TS_SERIAL1, 0x01);
+
+	/* Set up transfer block size */
+	saa_writeb(SAA7134_TS_PARALLEL_SERIAL, 128 - 1);
+	saa_writeb(SAA7134_TS_DMA0, (PAGE_SIZE >> 7) - 1);
+	saa_writeb(SAA7134_TS_DMA1, 0);
+	saa_writeb(SAA7134_TS_DMA2, 0);
+
+	/* Enable video streaming mode */
+	saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_VIDEO);
+
+	saa_writel(SAA7134_RS_BA1(5), saa->top_dma);
+	saa_writel(SAA7134_RS_BA2(5), saa->bottom_dma);
+	saa_writel(SAA7134_RS_PITCH(5), 128);
+	saa_writel(SAA7134_RS_CONTROL(5), SAA7134_RS_CONTROL_BURST_MAX);
+
+	/* Enable TS FIFO */
+	saa_setl(SAA7134_MAIN_CTRL, SAA7134_MAIN_CTRL_TE5);
+
+	/* Enable DMA IRQ */
+	saa_setl(SAA7134_IRQ1,
+			SAA7134_IRQ1_INTE_RA2_1 | SAA7134_IRQ1_INTE_RA2_0);
+
+	return 0;
+}
+
+static int saa7134_go7007_stream_stop(struct go7007 *go)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev;
+
+	if (!saa)
+		return -EINVAL;
+	dev = saa->dev;
+	if (!dev)
+		return -EINVAL;
+
+	/* Shut down TS FIFO */
+	saa_clearl(SAA7134_MAIN_CTRL, SAA7134_MAIN_CTRL_TE5);
+
+	/* Disable DMA IRQ */
+	saa_clearl(SAA7134_IRQ1,
+			SAA7134_IRQ1_INTE_RA2_1 | SAA7134_IRQ1_INTE_RA2_0);
+
+	/* Disable TS interface */
+	saa_clearb(SAA7134_TS_PARALLEL, 0x80);
+
+	dma_unmap_page(&dev->pci->dev, saa->top_dma, PAGE_SIZE,
+			DMA_FROM_DEVICE);
+	dma_unmap_page(&dev->pci->dev, saa->bottom_dma, PAGE_SIZE,
+			DMA_FROM_DEVICE);
+
+	return 0;
+}
+
+static int saa7134_go7007_send_firmware(struct go7007 *go, u8 *data, int len)
+{
+	struct saa7134_go7007 *saa = go->hpi_context;
+	struct saa7134_dev *dev = saa->dev;
+	u16 status_reg;
+	int i;
+
+#ifdef GO7007_HPI_DEBUG
+	pr_debug("saa7134-go7007: DownloadBuffer sending %d bytes\n", len);
+#endif
+
+	while (len > 0) {
+		i = len > 64 ? 64 : len;
+		saa_writeb(SAA7134_GPIO_GPMODE0, 0xff);
+		saa_writeb(SAA7134_GPIO_GPSTATUS0, HPI_ADDR_INIT_BUFFER);
+		saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_ADDR);
+		saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+		while (i-- > 0) {
+			saa_writeb(SAA7134_GPIO_GPSTATUS0, *data);
+			saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_WRITE);
+			saa_writeb(SAA7134_GPIO_GPSTATUS2, GPIO_COMMAND_IDLE);
+			++data;
+			--len;
+		}
+		for (i = 0; i < 100; ++i) {
+			gpio_read(dev, HPI_ADDR_INTR_STATUS, &status_reg);
+			if (!(status_reg & 0x0002))
+				break;
+		}
+		if (i == 100) {
+			pr_err("saa7134-go7007: device is hung, status reg = 0x%04x\n",
+			       status_reg);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+static struct go7007_hpi_ops saa7134_go7007_hpi_ops = {
+	.interface_reset	= saa7134_go7007_interface_reset,
+	.write_interrupt	= saa7134_go7007_write_interrupt,
+	.read_interrupt		= saa7134_go7007_read_interrupt,
+	.stream_start		= saa7134_go7007_stream_start,
+	.stream_stop		= saa7134_go7007_stream_stop,
+	.send_firmware		= saa7134_go7007_send_firmware,
+};
+MODULE_FIRMWARE("go7007/go7007tv.bin");
+
+/* --------------------------------------------------------------------------*/
+
+static int saa7134_go7007_s_std(struct v4l2_subdev *sd, v4l2_std_id norm)
+{
+#if 0
+	struct saa7134_go7007 *saa = to_state(sd);
+	struct saa7134_dev *dev = saa->dev;
+
+	return saa7134_s_std_internal(dev, NULL, norm);
+#else
+	return 0;
+#endif
+}
+
+static const struct v4l2_subdev_video_ops saa7134_go7007_video_ops = {
+	.s_std = saa7134_go7007_s_std,
+};
+
+static const struct v4l2_subdev_ops saa7134_go7007_sd_ops = {
+	.video = &saa7134_go7007_video_ops,
+};
+
+/* --------------------------------------------------------------------------*/
+
+
+/********************* Add/remove functions *********************/
+
+static int saa7134_go7007_init(struct saa7134_dev *dev)
+{
+	struct go7007 *go;
+	struct saa7134_go7007 *saa;
+	struct v4l2_subdev *sd;
+
+	pr_debug("saa7134-go7007: probing new SAA713X board\n");
+
+	go = go7007_alloc(&board_voyager, &dev->pci->dev);
+	if (go == NULL)
+		return -ENOMEM;
+
+	saa = kzalloc(sizeof(struct saa7134_go7007), GFP_KERNEL);
+	if (saa == NULL) {
+		kfree(go);
+		return -ENOMEM;
+	}
+
+	go->board_id = GO7007_BOARDID_PCI_VOYAGER;
+	snprintf(go->bus_info, sizeof(go->bus_info), "PCI:%s", pci_name(dev->pci));
+	strlcpy(go->name, saa7134_boards[dev->board].name, sizeof(go->name));
+	go->hpi_ops = &saa7134_go7007_hpi_ops;
+	go->hpi_context = saa;
+	saa->dev = dev;
+
+	/* Init the subdevice interface */
+	sd = &saa->sd;
+	v4l2_subdev_init(sd, &saa7134_go7007_sd_ops);
+	v4l2_set_subdevdata(sd, saa);
+	strncpy(sd->name, "saa7134-go7007", sizeof(sd->name));
+
+	/* Allocate a couple pages for receiving the compressed stream */
+	saa->top = (u8 *)get_zeroed_page(GFP_KERNEL);
+	if (!saa->top)
+		goto allocfail;
+	saa->bottom = (u8 *)get_zeroed_page(GFP_KERNEL);
+	if (!saa->bottom)
+		goto allocfail;
+
+	/* Boot the GO7007 */
+	if (go7007_boot_encoder(go, go->board_info->flags &
+					GO7007_BOARD_USE_ONBOARD_I2C) < 0)
+		goto allocfail;
+
+	/* Do any final GO7007 initialization, then register the
+	 * V4L2 and ALSA interfaces */
+	if (go7007_register_encoder(go, go->board_info->num_i2c_devs) < 0)
+		goto allocfail;
+
+	/* Register the subdevice interface with the go7007 device */
+	if (v4l2_device_register_subdev(&go->v4l2_dev, sd) < 0)
+		pr_info("saa7134-go7007: register subdev failed\n");
+
+	dev->empress_dev = &go->vdev;
+
+	go->status = STATUS_ONLINE;
+	return 0;
+
+allocfail:
+	if (saa->top)
+		free_page((unsigned long)saa->top);
+	if (saa->bottom)
+		free_page((unsigned long)saa->bottom);
+	kfree(saa);
+	kfree(go);
+	return -ENOMEM;
+}
+
+static int saa7134_go7007_fini(struct saa7134_dev *dev)
+{
+	struct go7007 *go;
+	struct saa7134_go7007 *saa;
+
+	if (NULL == dev->empress_dev)
+		return 0;
+
+	go = video_get_drvdata(dev->empress_dev);
+	if (go->audio_enabled)
+		go7007_snd_remove(go);
+
+	saa = go->hpi_context;
+	go->status = STATUS_SHUTDOWN;
+	free_page((unsigned long)saa->top);
+	free_page((unsigned long)saa->bottom);
+	v4l2_device_unregister_subdev(&saa->sd);
+	kfree(saa);
+	video_unregister_device(&go->vdev);
+
+	v4l2_device_put(&go->v4l2_dev);
+	dev->empress_dev = NULL;
+
+	return 0;
+}
+
+static struct saa7134_mpeg_ops saa7134_go7007_ops = {
+	.type          = SAA7134_MPEG_GO7007,
+	.init          = saa7134_go7007_init,
+	.fini          = saa7134_go7007_fini,
+	.irq_ts_done   = saa7134_go7007_irq_ts_done,
+};
+
+static int __init saa7134_go7007_mod_init(void)
+{
+	return saa7134_ts_register(&saa7134_go7007_ops);
+}
+
+static void __exit saa7134_go7007_mod_cleanup(void)
+{
+	saa7134_ts_unregister(&saa7134_go7007_ops);
+}
+
+module_init(saa7134_go7007_mod_init);
+module_exit(saa7134_go7007_mod_cleanup);
+
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/media/pci/saa7134/saa7134-vbi.c b/drivers/media/pci/saa7134/saa7134-vbi.c
index c06dbe17a87f..4f0b1012e4f3 100644
--- a/drivers/media/pci/saa7134/saa7134-vbi.c
+++ b/drivers/media/pci/saa7134/saa7134-vbi.c
@@ -43,7 +43,7 @@ MODULE_PARM_DESC(vbibufs,"number of vbi buffers, range 2-32");
 
 /* ------------------------------------------------------------------ */
 
-#define VBI_LINE_COUNT     16
+#define VBI_LINE_COUNT     17
 #define VBI_LINE_LENGTH  2048
 #define VBI_SCALE       0x200
 
diff --git a/drivers/media/pci/saa7134/saa7134-video.c b/drivers/media/pci/saa7134/saa7134-video.c
index 0cfa2ca6a32a..fc4a427cb51f 100644
--- a/drivers/media/pci/saa7134/saa7134-video.c
+++ b/drivers/media/pci/saa7134/saa7134-video.c
@@ -201,7 +201,7 @@ static struct saa7134_format formats[] = {
 		.video_v_start = 24,	\
 		.video_v_stop  = 311,	\
 		.vbi_v_start_0 = 7,	\
-		.vbi_v_stop_0  = 22,	\
+		.vbi_v_stop_0  = 23,	\
 		.vbi_v_start_1 = 319,   \
 		.src_timing    = 4
 
diff --git a/drivers/media/pci/saa7134/saa7134.h b/drivers/media/pci/saa7134/saa7134.h
index e47edd4b57ce..1a82dd07205b 100644
--- a/drivers/media/pci/saa7134/saa7134.h
+++ b/drivers/media/pci/saa7134/saa7134.h
@@ -338,6 +338,7 @@ struct saa7134_card_ir {
 #define SAA7134_BOARD_ASUSTeK_PS3_100      190
 #define SAA7134_BOARD_HAWELL_HW_9004V1      191
 #define SAA7134_BOARD_AVERMEDIA_A706		192
+#define SAA7134_BOARD_WIS_VOYAGER           193
 
 #define SAA7134_MAXBOARDS 32
 #define SAA7134_INPUT_MAX 8
@@ -368,6 +369,7 @@ enum saa7134_mpeg_type {
 	SAA7134_MPEG_UNUSED,
 	SAA7134_MPEG_EMPRESS,
 	SAA7134_MPEG_DVB,
+	SAA7134_MPEG_GO7007,
 };
 
 enum saa7134_mpeg_ts_type {
@@ -407,6 +409,7 @@ struct saa7134_board {
 #define card_has_radio(dev)   (NULL != saa7134_boards[dev->board].radio.name)
 #define card_is_empress(dev)  (SAA7134_MPEG_EMPRESS == saa7134_boards[dev->board].mpeg)
 #define card_is_dvb(dev)      (SAA7134_MPEG_DVB     == saa7134_boards[dev->board].mpeg)
+#define card_is_go7007(dev)   (SAA7134_MPEG_GO7007  == saa7134_boards[dev->board].mpeg)
 #define card_has_mpeg(dev)    (SAA7134_MPEG_UNUSED  != saa7134_boards[dev->board].mpeg)
 #define card(dev)             (saa7134_boards[dev->board])
 #define card_in(dev,n)        (saa7134_boards[dev->board].inputs[n])
@@ -522,6 +525,8 @@ struct saa7134_mpeg_ops {
 	int                        (*init)(struct saa7134_dev *dev);
 	int                        (*fini)(struct saa7134_dev *dev);
 	void                       (*signal_change)(struct saa7134_dev *dev);
+	void                       (*irq_ts_done)(struct saa7134_dev *dev,
+						  unsigned long status);
 };
 
 /* global device status */
diff --git a/drivers/media/pci/saa7164/saa7164-api.c b/drivers/media/pci/saa7164/saa7164-api.c
index e042963d377d..4f3b1dd18ba4 100644
--- a/drivers/media/pci/saa7164/saa7164-api.c
+++ b/drivers/media/pci/saa7164/saa7164-api.c
@@ -680,7 +680,6 @@ static int saa7164_api_set_dif(struct saa7164_port *port, u8 reg, u8 val)
 int saa7164_api_configure_dif(struct saa7164_port *port, u32 std)
 {
 	struct saa7164_dev *dev = port->dev;
-	int ret = 0;
 	u8 agc_disable;
 
 	dprintk(DBGLVL_API, "%s(nr=%d, 0x%x)\n", __func__, port->nr, std);
@@ -733,7 +732,7 @@ int saa7164_api_configure_dif(struct saa7164_port *port, u32 std)
 	saa7164_api_set_dif(port, 0x04, 0x00); /* Active (again) */
 	msleep(100);
 
-	return ret;
+	return 0;
 }
 
 /* Ensure the dif is in the correct state for the operating mode
diff --git a/drivers/media/pci/saa7164/saa7164-core.c b/drivers/media/pci/saa7164/saa7164-core.c
index 1bf06970ca3e..cc1be8a7a451 100644
--- a/drivers/media/pci/saa7164/saa7164-core.c
+++ b/drivers/media/pci/saa7164/saa7164-core.c
@@ -52,7 +52,7 @@ unsigned int saa_debug;
 module_param_named(debug, saa_debug, int, 0644);
 MODULE_PARM_DESC(debug, "enable debug messages");
 
-unsigned int fw_debug;
+static unsigned int fw_debug;
 module_param(fw_debug, int, 0644);
 MODULE_PARM_DESC(fw_debug, "Firmware debug level def:2");
 
@@ -72,7 +72,7 @@ static unsigned int card[]  = {[0 ... (SAA7164_MAXBOARDS - 1)] = UNSET };
 module_param_array(card,  int, NULL, 0444);
 MODULE_PARM_DESC(card, "card type");
 
-unsigned int print_histogram = 64;
+static unsigned int print_histogram = 64;
 module_param(print_histogram, int, 0644);
 MODULE_PARM_DESC(print_histogram, "print histogram values once");
 
@@ -80,7 +80,7 @@ unsigned int crc_checking = 1;
 module_param(crc_checking, int, 0644);
 MODULE_PARM_DESC(crc_checking, "enable crc sanity checking on buffers");
 
-unsigned int guard_checking = 1;
+static unsigned int guard_checking = 1;
 module_param(guard_checking, int, 0644);
 MODULE_PARM_DESC(guard_checking,
 	"enable dma sanity checking for buffer overruns");
diff --git a/drivers/media/pci/solo6x10/Kconfig b/drivers/media/pci/solo6x10/Kconfig
index d9e06a6bf1eb..0fb91dc7ca73 100644
--- a/drivers/media/pci/solo6x10/Kconfig
+++ b/drivers/media/pci/solo6x10/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_SOLO6X10
 	tristate "Bluecherry / Softlogic 6x10 capture cards (MPEG-4/H.264)"
 	depends on PCI && VIDEO_DEV && SND && I2C
+	depends on HAS_DMA
 	select BITREVERSE
 	select FONT_SUPPORT
 	select FONT_8x16
diff --git a/drivers/media/pci/solo6x10/solo6x10-disp.c b/drivers/media/pci/solo6x10/solo6x10-disp.c
index 5ea9cac03968..11c98f0625e4 100644
--- a/drivers/media/pci/solo6x10/solo6x10-disp.c
+++ b/drivers/media/pci/solo6x10/solo6x10-disp.c
@@ -172,7 +172,7 @@ static void solo_vout_config(struct solo_dev *solo_dev)
 static int solo_dma_vin_region(struct solo_dev *solo_dev, u32 off,
 			       u16 val, int reg_size)
 {
-	u16 *buf;
+	__le16 *buf;
 	const int n = 64, size = n * sizeof(*buf);
 	int i, ret = 0;
 
@@ -211,7 +211,7 @@ int solo_set_motion_block(struct solo_dev *solo_dev, u8 ch,
 {
 	const unsigned size = sizeof(u16) * 64;
 	u32 off = SOLO_MOT_FLAG_AREA + ch * SOLO_MOT_THRESH_SIZE * 2;
-	u16 *buf;
+	__le16 *buf;
 	int x, y;
 	int ret = 0;
 
diff --git a/drivers/media/pci/solo6x10/solo6x10-eeprom.c b/drivers/media/pci/solo6x10/solo6x10-eeprom.c
index af40b3aba410..da25ce4a6952 100644
--- a/drivers/media/pci/solo6x10/solo6x10-eeprom.c
+++ b/drivers/media/pci/solo6x10/solo6x10-eeprom.c
@@ -100,7 +100,7 @@ unsigned int solo_eeprom_ewen(struct solo_dev *solo_dev, int w_en)
 	return retval;
 }
 
-unsigned short solo_eeprom_read(struct solo_dev *solo_dev, int loc)
+__be16 solo_eeprom_read(struct solo_dev *solo_dev, int loc)
 {
 	int read_cmd = loc | (EE_READ_CMD << ADDR_LEN);
 	unsigned short retval = 0;
@@ -117,11 +117,11 @@ unsigned short solo_eeprom_read(struct solo_dev *solo_dev, int loc)
 
 	solo_eeprom_reg_write(solo_dev, ~EE_CS);
 
-	return retval;
+	return (__force __be16)retval;
 }
 
 int solo_eeprom_write(struct solo_dev *solo_dev, int loc,
-		      unsigned short data)
+		      __be16 data)
 {
 	int write_cmd = loc | (EE_WRITE_CMD << ADDR_LEN);
 	unsigned int retval;
@@ -130,7 +130,7 @@ int solo_eeprom_write(struct solo_dev *solo_dev, int loc,
 	solo_eeprom_cmd(solo_dev, write_cmd);
 
 	for (i = 15; i >= 0; i--) {
-		unsigned int dataval = (data >> i) & 1;
+		unsigned int dataval = ((__force unsigned)data >> i) & 1;
 
 		solo_eeprom_reg_write(solo_dev, EE_ENB);
 		solo_eeprom_reg_write(solo_dev,
diff --git a/drivers/media/pci/solo6x10/solo6x10.h b/drivers/media/pci/solo6x10/solo6x10.h
index c6154b00fcbd..72017b7f0a75 100644
--- a/drivers/media/pci/solo6x10/solo6x10.h
+++ b/drivers/media/pci/solo6x10/solo6x10.h
@@ -394,9 +394,9 @@ int solo_osd_print(struct solo_enc_dev *solo_enc);
 
 /* EEPROM commands */
 unsigned int solo_eeprom_ewen(struct solo_dev *solo_dev, int w_en);
-unsigned short solo_eeprom_read(struct solo_dev *solo_dev, int loc);
+__be16 solo_eeprom_read(struct solo_dev *solo_dev, int loc);
 int solo_eeprom_write(struct solo_dev *solo_dev, int loc,
-		      unsigned short data);
+		      __be16 data);
 
 /* JPEG Qp functions */
 void solo_s_jpeg_qp(struct solo_dev *solo_dev, unsigned int ch,
diff --git a/drivers/media/pci/sta2x11/Kconfig b/drivers/media/pci/sta2x11/Kconfig
index 03130157db83..f6f30abc088b 100644
--- a/drivers/media/pci/sta2x11/Kconfig
+++ b/drivers/media/pci/sta2x11/Kconfig
@@ -1,6 +1,7 @@
 config STA2X11_VIP
 	tristate "STA2X11 VIP Video For Linux"
 	depends on STA2X11
+	depends on HAS_DMA
 	select VIDEO_ADV7180 if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEOBUF2_DMA_CONTIG
 	depends on PCI && VIDEO_V4L2 && VIRT_TO_BUS
diff --git a/drivers/media/pci/sta2x11/sta2x11_vip.c b/drivers/media/pci/sta2x11/sta2x11_vip.c
index 365bd21301ba..22450f583da1 100644
--- a/drivers/media/pci/sta2x11/sta2x11_vip.c
+++ b/drivers/media/pci/sta2x11/sta2x11_vip.c
@@ -152,7 +152,7 @@ struct sta2x11_vip {
 	int tcount, bcount;
 	int overflow;
 
-	void *iomem;	/* I/O Memory */
+	void __iomem *iomem;	/* I/O Memory */
 	struct vip_config *config;
 };
 
diff --git a/drivers/media/pci/ttpci/Kconfig b/drivers/media/pci/ttpci/Kconfig
index 0dcb8cd77676..7b83151ed6c4 100644
--- a/drivers/media/pci/ttpci/Kconfig
+++ b/drivers/media/pci/ttpci/Kconfig
@@ -1,8 +1,12 @@
+config DVB_AV7110_IR
+	bool
+
 config DVB_AV7110
 	tristate "AV7110 cards"
 	depends on DVB_CORE && PCI && I2C
 	select TTPCI_EEPROM
 	select VIDEO_SAA7146_VV
+	select DVB_AV7110_IR if INPUT_EVDEV=y || INPUT_EVDEV=DVB_AV7110
 	depends on VIDEO_DEV	# dependencies of VIDEO_SAA7146_VV
 	select DVB_VES1820 if MEDIA_SUBDRV_AUTOSELECT
 	select DVB_VES1X93 if MEDIA_SUBDRV_AUTOSELECT
diff --git a/drivers/media/pci/ttpci/Makefile b/drivers/media/pci/ttpci/Makefile
index 98905963ff08..49f71b1eaf14 100644
--- a/drivers/media/pci/ttpci/Makefile
+++ b/drivers/media/pci/ttpci/Makefile
@@ -5,7 +5,7 @@
 
 dvb-ttpci-objs := av7110_hw.o av7110_v4l.o av7110_av.o av7110_ca.o av7110.o av7110_ipack.o
 
-ifdef CONFIG_INPUT_EVDEV
+ifdef CONFIG_DVB_AV7110_IR
 dvb-ttpci-objs += av7110_ir.o
 endif
 
diff --git a/drivers/media/pci/ttpci/av7110.c b/drivers/media/pci/ttpci/av7110.c
index f38329d29daa..c1f0617a6973 100644
--- a/drivers/media/pci/ttpci/av7110.c
+++ b/drivers/media/pci/ttpci/av7110.c
@@ -235,7 +235,7 @@ static void recover_arm(struct av7110 *av7110)
 
 	restart_feeds(av7110);
 
-#if IS_ENABLED(CONFIG_INPUT_EVDEV)
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
 	av7110_check_ir_config(av7110, true);
 #endif
 }
@@ -268,7 +268,7 @@ static int arm_thread(void *data)
 		if (!av7110->arm_ready)
 			continue;
 
-#if IS_ENABLED(CONFIG_INPUT_EVDEV)
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
 		av7110_check_ir_config(av7110, false);
 #endif
 
@@ -2725,7 +2725,7 @@ static int av7110_attach(struct saa7146_dev* dev,
 
 	mutex_init(&av7110->ioctl_mutex);
 
-#if IS_ENABLED(CONFIG_INPUT_EVDEV)
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
 	av7110_ir_init(av7110);
 #endif
 	printk(KERN_INFO "dvb-ttpci: found av7110-%d.\n", av7110_num);
@@ -2768,7 +2768,7 @@ static int av7110_detach(struct saa7146_dev* saa)
 	struct av7110 *av7110 = saa->ext_priv;
 	dprintk(4, "%p\n", av7110);
 
-#if IS_ENABLED(CONFIG_INPUT_EVDEV)
+#if IS_ENABLED(CONFIG_DVB_AV7110_IR)
 	av7110_ir_exit(av7110);
 #endif
 	if (budgetpatch || av7110->full_ts) {
diff --git a/drivers/media/pci/tw68/Kconfig b/drivers/media/pci/tw68/Kconfig
new file mode 100644
index 000000000000..5425ba1e320d
--- /dev/null
+++ b/drivers/media/pci/tw68/Kconfig
@@ -0,0 +1,10 @@
+config VIDEO_TW68
+	tristate "Techwell tw68x Video For Linux"
+	depends on VIDEO_DEV && PCI && VIDEO_V4L2
+	select I2C_ALGOBIT
+	select VIDEOBUF2_DMA_SG
+	---help---
+	  Support for Techwell tw68xx based frame grabber boards.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called tw68.
diff --git a/drivers/media/pci/tw68/Makefile b/drivers/media/pci/tw68/Makefile
new file mode 100644
index 000000000000..3d02f28b14fb
--- /dev/null
+++ b/drivers/media/pci/tw68/Makefile
@@ -0,0 +1,3 @@
+tw68-objs := tw68-core.o tw68-video.o tw68-risc.o
+
+obj-$(CONFIG_VIDEO_TW68) += tw68.o
diff --git a/drivers/media/pci/tw68/tw68-core.c b/drivers/media/pci/tw68/tw68-core.c
new file mode 100644
index 000000000000..a6fb48cf7aae
--- /dev/null
+++ b/drivers/media/pci/tw68/tw68-core.c
@@ -0,0 +1,434 @@
+/*
+ *  tw68-core.c
+ *  Core functions for the Techwell 68xx driver
+ *
+ *  Much of this code is derived from the cx88 and sa7134 drivers, which
+ *  were in turn derived from the bt87x driver.  The original work was by
+ *  Gerd Knorr; more recently the code was enhanced by Mauro Carvalho Chehab,
+ *  Hans Verkuil, Andy Walls and many others.  Their work is gratefully
+ *  acknowledged.  Full credit goes to them - any problems within this code
+ *  are mine.
+ *
+ *  Copyright (C) 2009  William M. Brack
+ *
+ *  Refactored and updated to the latest v4l core frameworks:
+ *
+ *  Copyright (C) 2014 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/kmod.h>
+#include <linux/sound.h>
+#include <linux/interrupt.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/dma-mapping.h>
+#include <linux/pm.h>
+
+#include <media/v4l2-dev.h>
+#include "tw68.h"
+#include "tw68-reg.h"
+
+MODULE_DESCRIPTION("v4l2 driver module for tw6800 based video capture cards");
+MODULE_AUTHOR("William M. Brack");
+MODULE_AUTHOR("Hans Verkuil <hverkuil@xs4all.nl>");
+MODULE_LICENSE("GPL");
+
+static unsigned int latency = UNSET;
+module_param(latency, int, 0444);
+MODULE_PARM_DESC(latency, "pci latency timer");
+
+static unsigned int video_nr[] = {[0 ... (TW68_MAXBOARDS - 1)] = UNSET };
+module_param_array(video_nr, int, NULL, 0444);
+MODULE_PARM_DESC(video_nr, "video device number");
+
+static unsigned int card[] = {[0 ... (TW68_MAXBOARDS - 1)] = UNSET };
+module_param_array(card, int, NULL, 0444);
+MODULE_PARM_DESC(card, "card type");
+
+static atomic_t tw68_instance = ATOMIC_INIT(0);
+
+/* ------------------------------------------------------------------ */
+
+/*
+ * Please add any new PCI IDs to: http://pci-ids.ucw.cz.  This keeps
+ * the PCI ID database up to date.  Note that the entries must be
+ * added under vendor 0x1797 (Techwell Inc.) as subsystem IDs.
+ */
+static const struct pci_device_id tw68_pci_tbl[] = {
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_6800)},
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_6801)},
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_6804)},
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_6816_1)},
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_6816_2)},
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_6816_3)},
+	{PCI_DEVICE(PCI_VENDOR_ID_TECHWELL, PCI_DEVICE_ID_6816_4)},
+	{0,}
+};
+
+/* ------------------------------------------------------------------ */
+
+
+/*
+ * The device is given a "soft reset". According to the specifications,
+ * after this "all register content remain unchanged", so we also write
+ * to all specified registers manually as well (mostly to manufacturer's
+ * specified reset values)
+ */
+static int tw68_hw_init1(struct tw68_dev *dev)
+{
+	/* Assure all interrupts are disabled */
+	tw_writel(TW68_INTMASK, 0);		/* 020 */
+	/* Clear any pending interrupts */
+	tw_writel(TW68_INTSTAT, 0xffffffff);	/* 01C */
+	/* Stop risc processor, set default buffer level */
+	tw_writel(TW68_DMAC, 0x1600);
+
+	tw_writeb(TW68_ACNTL, 0x80);	/* 218	soft reset */
+	msleep(100);
+
+	tw_writeb(TW68_INFORM, 0x40);	/* 208	mux0, 27mhz xtal */
+	tw_writeb(TW68_OPFORM, 0x04);	/* 20C	analog line-lock */
+	tw_writeb(TW68_HSYNC, 0);	/* 210	color-killer high sens */
+	tw_writeb(TW68_ACNTL, 0x42);	/* 218	int vref #2, chroma adc off */
+
+	tw_writeb(TW68_CROP_HI, 0x02);	/* 21C	Hactive m.s. bits */
+	tw_writeb(TW68_VDELAY_LO, 0x12);/* 220	Mfg specified reset value */
+	tw_writeb(TW68_VACTIVE_LO, 0xf0);
+	tw_writeb(TW68_HDELAY_LO, 0x0f);
+	tw_writeb(TW68_HACTIVE_LO, 0xd0);
+
+	tw_writeb(TW68_CNTRL1, 0xcd);	/* 230	Wide Chroma BPF B/W
+					 *	Secam reduction, Adap comb for
+					 *	NTSC, Op Mode 1 */
+
+	tw_writeb(TW68_VSCALE_LO, 0);	/* 234 */
+	tw_writeb(TW68_SCALE_HI, 0x11);	/* 238 */
+	tw_writeb(TW68_HSCALE_LO, 0);	/* 23c */
+	tw_writeb(TW68_BRIGHT, 0);	/* 240 */
+	tw_writeb(TW68_CONTRAST, 0x5c);	/* 244 */
+	tw_writeb(TW68_SHARPNESS, 0x51);/* 248 */
+	tw_writeb(TW68_SAT_U, 0x80);	/* 24C */
+	tw_writeb(TW68_SAT_V, 0x80);	/* 250 */
+	tw_writeb(TW68_HUE, 0x00);	/* 254 */
+
+	/* TODO - Check that none of these are set by control defaults */
+	tw_writeb(TW68_SHARP2, 0x53);	/* 258	Mfg specified reset val */
+	tw_writeb(TW68_VSHARP, 0x80);	/* 25C	Sharpness Coring val 8 */
+	tw_writeb(TW68_CORING, 0x44);	/* 260	CTI and Vert Peak coring */
+	tw_writeb(TW68_CNTRL2, 0x00);	/* 268	No power saving enabled */
+	tw_writeb(TW68_SDT, 0x07);	/* 270	Enable shadow reg, auto-det */
+	tw_writeb(TW68_SDTR, 0x7f);	/* 274	All stds recog, don't start */
+	tw_writeb(TW68_CLMPG, 0x50);	/* 280	Clamp end at 40 sys clocks */
+	tw_writeb(TW68_IAGC, 0x22);	/* 284	Mfg specified reset val */
+	tw_writeb(TW68_AGCGAIN, 0xf0);	/* 288	AGC gain when loop disabled */
+	tw_writeb(TW68_PEAKWT, 0xd8);	/* 28C	White peak threshold */
+	tw_writeb(TW68_CLMPL, 0x3c);	/* 290	Y channel clamp level */
+/*	tw_writeb(TW68_SYNCT, 0x38);*/	/* 294	Sync amplitude */
+	tw_writeb(TW68_SYNCT, 0x30);	/* 294	Sync amplitude */
+	tw_writeb(TW68_MISSCNT, 0x44);	/* 298	Horiz sync, VCR detect sens */
+	tw_writeb(TW68_PCLAMP, 0x28);	/* 29C	Clamp pos from PLL sync */
+	/* Bit DETV of VCNTL1 helps sync multi cams/chip board */
+	tw_writeb(TW68_VCNTL1, 0x04);	/* 2A0 */
+	tw_writeb(TW68_VCNTL2, 0);	/* 2A4 */
+	tw_writeb(TW68_CKILL, 0x68);	/* 2A8	Mfg specified reset val */
+	tw_writeb(TW68_COMB, 0x44);	/* 2AC	Mfg specified reset val */
+	tw_writeb(TW68_LDLY, 0x30);	/* 2B0	Max positive luma delay */
+	tw_writeb(TW68_MISC1, 0x14);	/* 2B4	Mfg specified reset val */
+	tw_writeb(TW68_LOOP, 0xa5);	/* 2B8	Mfg specified reset val */
+	tw_writeb(TW68_MISC2, 0xe0);	/* 2BC	Enable colour killer */
+	tw_writeb(TW68_MVSN, 0);	/* 2C0 */
+	tw_writeb(TW68_CLMD, 0x05);	/* 2CC	slice level auto, clamp med. */
+	tw_writeb(TW68_IDCNTL, 0);	/* 2D0	Writing zero to this register
+					 *	selects NTSC ID detection,
+					 *	but doesn't change the
+					 *	sensitivity (which has a reset
+					 *	value of 1E).  Since we are
+					 *	not doing auto-detection, it
+					 *	has no real effect */
+	tw_writeb(TW68_CLCNTL1, 0);	/* 2D4 */
+	tw_writel(TW68_VBIC, 0x03);	/* 010 */
+	tw_writel(TW68_CAP_CTL, 0x03);	/* 040	Enable both even & odd flds */
+	tw_writel(TW68_DMAC, 0x2000);	/* patch set had 0x2080 */
+	tw_writel(TW68_TESTREG, 0);	/* 02C */
+
+	/*
+	 * Some common boards, especially inexpensive single-chip models,
+	 * use the GPIO bits 0-3 to control an on-board video-output mux.
+	 * For these boards, we need to set up the GPIO register into
+	 * "normal" mode, set bits 0-3 as output, and then set those bits
+	 * zero.
+	 *
+	 * Eventually, it would be nice if we could identify these boards
+	 * uniquely, and only do this initialisation if the board has been
+	 * identify.  For the moment, however, it shouldn't hurt anything
+	 * to do these steps.
+	 */
+	tw_writel(TW68_GPIOC, 0);	/* Set the GPIO to "normal", no ints */
+	tw_writel(TW68_GPOE, 0x0f);	/* Set bits 0-3 to "output" */
+	tw_writel(TW68_GPDATA, 0);	/* Set all bits to low state */
+
+	/* Initialize the device control structures */
+	mutex_init(&dev->lock);
+	spin_lock_init(&dev->slock);
+
+	/* Initialize any subsystems */
+	tw68_video_init1(dev);
+	return 0;
+}
+
+static irqreturn_t tw68_irq(int irq, void *dev_id)
+{
+	struct tw68_dev *dev = dev_id;
+	u32 status, orig;
+	int loop;
+
+	status = orig = tw_readl(TW68_INTSTAT) & dev->pci_irqmask;
+	/* Check if anything to do */
+	if (0 == status)
+		return IRQ_NONE;	/* Nope - return */
+	for (loop = 0; loop < 10; loop++) {
+		if (status & dev->board_virqmask)	/* video interrupt */
+			tw68_irq_video_done(dev, status);
+		status = tw_readl(TW68_INTSTAT) & dev->pci_irqmask;
+		if (0 == status)
+			return IRQ_HANDLED;
+	}
+	dev_dbg(&dev->pci->dev, "%s: **** INTERRUPT NOT HANDLED - clearing mask (orig 0x%08x, cur 0x%08x)",
+			dev->name, orig, tw_readl(TW68_INTSTAT));
+	dev_dbg(&dev->pci->dev, "%s: pci_irqmask 0x%08x; board_virqmask 0x%08x ****\n",
+			dev->name, dev->pci_irqmask, dev->board_virqmask);
+	tw_clearl(TW68_INTMASK, dev->pci_irqmask);
+	return IRQ_HANDLED;
+}
+
+static int tw68_initdev(struct pci_dev *pci_dev,
+				     const struct pci_device_id *pci_id)
+{
+	struct tw68_dev *dev;
+	int vidnr = -1;
+	int err;
+
+	dev = devm_kzalloc(&pci_dev->dev, sizeof(*dev), GFP_KERNEL);
+	if (NULL == dev)
+		return -ENOMEM;
+
+	dev->instance = v4l2_device_set_name(&dev->v4l2_dev, "tw68",
+						&tw68_instance);
+
+	err = v4l2_device_register(&pci_dev->dev, &dev->v4l2_dev);
+	if (err)
+		return err;
+
+	/* pci init */
+	dev->pci = pci_dev;
+	if (pci_enable_device(pci_dev)) {
+		err = -EIO;
+		goto fail1;
+	}
+
+	dev->name = dev->v4l2_dev.name;
+
+	if (UNSET != latency) {
+		pr_info("%s: setting pci latency timer to %d\n",
+		       dev->name, latency);
+		pci_write_config_byte(pci_dev, PCI_LATENCY_TIMER, latency);
+	}
+
+	/* print pci info */
+	pci_read_config_byte(pci_dev, PCI_CLASS_REVISION, &dev->pci_rev);
+	pci_read_config_byte(pci_dev, PCI_LATENCY_TIMER,  &dev->pci_lat);
+	pr_info("%s: found at %s, rev: %d, irq: %d, latency: %d, mmio: 0x%llx\n",
+		dev->name, pci_name(pci_dev), dev->pci_rev, pci_dev->irq,
+		dev->pci_lat, (u64)pci_resource_start(pci_dev, 0));
+	pci_set_master(pci_dev);
+	if (!pci_dma_supported(pci_dev, DMA_BIT_MASK(32))) {
+		pr_info("%s: Oops: no 32bit PCI DMA ???\n", dev->name);
+		err = -EIO;
+		goto fail1;
+	}
+
+	switch (pci_id->device) {
+	case PCI_DEVICE_ID_6800:	/* TW6800 */
+		dev->vdecoder = TW6800;
+		dev->board_virqmask = TW68_VID_INTS;
+		break;
+	case PCI_DEVICE_ID_6801:	/* Video decoder for TW6802 */
+		dev->vdecoder = TW6801;
+		dev->board_virqmask = TW68_VID_INTS | TW68_VID_INTSX;
+		break;
+	case PCI_DEVICE_ID_6804:	/* Video decoder for TW6804 */
+		dev->vdecoder = TW6804;
+		dev->board_virqmask = TW68_VID_INTS | TW68_VID_INTSX;
+		break;
+	default:
+		dev->vdecoder = TWXXXX;	/* To be announced */
+		dev->board_virqmask = TW68_VID_INTS | TW68_VID_INTSX;
+		break;
+	}
+
+	/* get mmio */
+	if (!request_mem_region(pci_resource_start(pci_dev, 0),
+				pci_resource_len(pci_dev, 0),
+				dev->name)) {
+		err = -EBUSY;
+		pr_err("%s: can't get MMIO memory @ 0x%llx\n",
+			dev->name,
+			(unsigned long long)pci_resource_start(pci_dev, 0));
+		goto fail1;
+	}
+	dev->lmmio = ioremap(pci_resource_start(pci_dev, 0),
+			     pci_resource_len(pci_dev, 0));
+	dev->bmmio = (__u8 __iomem *)dev->lmmio;
+	if (NULL == dev->lmmio) {
+		err = -EIO;
+		pr_err("%s: can't ioremap() MMIO memory\n",
+		       dev->name);
+		goto fail2;
+	}
+	/* initialize hardware #1 */
+	/* Then do any initialisation wanted before interrupts are on */
+	tw68_hw_init1(dev);
+
+	/* get irq */
+	err = devm_request_irq(&pci_dev->dev, pci_dev->irq, tw68_irq,
+			  IRQF_SHARED | IRQF_DISABLED, dev->name, dev);
+	if (err < 0) {
+		pr_err("%s: can't get IRQ %d\n",
+		       dev->name, pci_dev->irq);
+		goto fail3;
+	}
+
+	/*
+	 *  Now do remainder of initialisation, first for
+	 *  things unique for this card, then for general board
+	 */
+	if (dev->instance < TW68_MAXBOARDS)
+		vidnr = video_nr[dev->instance];
+	/* initialise video function first */
+	err = tw68_video_init2(dev, vidnr);
+	if (err < 0) {
+		pr_err("%s: can't register video device\n",
+		       dev->name);
+		goto fail4;
+	}
+	tw_setl(TW68_INTMASK, dev->pci_irqmask);
+
+	pr_info("%s: registered device %s\n",
+	       dev->name, video_device_node_name(&dev->vdev));
+
+	return 0;
+
+fail4:
+	video_unregister_device(&dev->vdev);
+fail3:
+	iounmap(dev->lmmio);
+fail2:
+	release_mem_region(pci_resource_start(pci_dev, 0),
+			   pci_resource_len(pci_dev, 0));
+fail1:
+	v4l2_device_unregister(&dev->v4l2_dev);
+	return err;
+}
+
+static void tw68_finidev(struct pci_dev *pci_dev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct tw68_dev *dev =
+		container_of(v4l2_dev, struct tw68_dev, v4l2_dev);
+
+	/* shutdown subsystems */
+	tw_clearl(TW68_DMAC, TW68_DMAP_EN | TW68_FIFO_EN);
+	tw_writel(TW68_INTMASK, 0);
+
+	/* unregister */
+	video_unregister_device(&dev->vdev);
+	v4l2_ctrl_handler_free(&dev->hdl);
+
+	/* release resources */
+	iounmap(dev->lmmio);
+	release_mem_region(pci_resource_start(pci_dev, 0),
+			   pci_resource_len(pci_dev, 0));
+
+	v4l2_device_unregister(&dev->v4l2_dev);
+}
+
+#ifdef CONFIG_PM
+
+static int tw68_suspend(struct pci_dev *pci_dev , pm_message_t state)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct tw68_dev *dev = container_of(v4l2_dev,
+				struct tw68_dev, v4l2_dev);
+
+	tw_clearl(TW68_DMAC, TW68_DMAP_EN | TW68_FIFO_EN);
+	dev->pci_irqmask &= ~TW68_VID_INTS;
+	tw_writel(TW68_INTMASK, 0);
+
+	synchronize_irq(pci_dev->irq);
+
+	pci_save_state(pci_dev);
+	pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
+	vb2_discard_done(&dev->vidq);
+
+	return 0;
+}
+
+static int tw68_resume(struct pci_dev *pci_dev)
+{
+	struct v4l2_device *v4l2_dev = pci_get_drvdata(pci_dev);
+	struct tw68_dev *dev = container_of(v4l2_dev,
+					    struct tw68_dev, v4l2_dev);
+	struct tw68_buf *buf;
+	unsigned long flags;
+
+	pci_set_power_state(pci_dev, PCI_D0);
+	pci_restore_state(pci_dev);
+
+	/* Do things that are done in tw68_initdev ,
+		except of initializing memory structures.*/
+
+	msleep(100);
+
+	tw68_set_tvnorm_hw(dev);
+
+	/*resume unfinished buffer(s)*/
+	spin_lock_irqsave(&dev->slock, flags);
+	buf = container_of(dev->active.next, struct tw68_buf, list);
+
+	tw68_video_start_dma(dev, buf);
+
+	spin_unlock_irqrestore(&dev->slock, flags);
+
+	return 0;
+}
+#endif
+
+/* ----------------------------------------------------------- */
+
+static struct pci_driver tw68_pci_driver = {
+	.name	  = "tw68",
+	.id_table = tw68_pci_tbl,
+	.probe	  = tw68_initdev,
+	.remove	  = tw68_finidev,
+#ifdef CONFIG_PM
+	.suspend  = tw68_suspend,
+	.resume   = tw68_resume
+#endif
+};
+
+module_pci_driver(tw68_pci_driver);
diff --git a/drivers/media/pci/tw68/tw68-reg.h b/drivers/media/pci/tw68/tw68-reg.h
new file mode 100644
index 000000000000..f60b3a896fa7
--- /dev/null
+++ b/drivers/media/pci/tw68/tw68-reg.h
@@ -0,0 +1,195 @@
+/*
+ *  tw68-reg.h - TW68xx register offsets
+ *
+ *  Much of this code is derived from the cx88 and sa7134 drivers, which
+ *  were in turn derived from the bt87x driver.  The original work was by
+ *  Gerd Knorr; more recently the code was enhanced by Mauro Carvalho Chehab,
+ *  Hans Verkuil, Andy Walls and many others.  Their work is gratefully
+ *  acknowledged.  Full credit goes to them - any problems within this code
+ *  are mine.
+ *
+ *  Copyright (C) William M. Brack
+ *
+ *  Refactored and updated to the latest v4l core frameworks:
+ *
+ *  Copyright (C) 2014 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+*/
+
+#ifndef _TW68_REG_H_
+#define _TW68_REG_H_
+
+/* ---------------------------------------------------------------------- */
+#define	TW68_DMAC		0x000
+#define	TW68_DMAP_SA		0x004
+#define	TW68_DMAP_EXE		0x008
+#define	TW68_DMAP_PP		0x00c
+#define	TW68_VBIC		0x010
+#define	TW68_SBUSC		0x014
+#define	TW68_SBUSSD		0x018
+#define	TW68_INTSTAT		0x01C
+#define	TW68_INTMASK		0x020
+#define	TW68_GPIOC		0x024
+#define	TW68_GPOE		0x028
+#define	TW68_TESTREG		0x02C
+#define	TW68_SBUSRD		0x030
+#define	TW68_SBUS_TRIG		0x034
+#define	TW68_CAP_CTL		0x040
+#define	TW68_SUBSYS		0x054
+#define	TW68_I2C_RST		0x064
+#define	TW68_VBIINST		0x06C
+/* define bits in FIFO and DMAP Control reg */
+#define	TW68_DMAP_EN		(1 << 0)
+#define	TW68_FIFO_EN		(1 << 1)
+/* define the Interrupt Status Register bits */
+#define	TW68_SBDONE		(1 << 0)
+#define	TW68_DMAPI		(1 << 1)
+#define	TW68_GPINT		(1 << 2)
+#define	TW68_FFOF		(1 << 3)
+#define	TW68_FDMIS		(1 << 4)
+#define	TW68_DMAPERR		(1 << 5)
+#define	TW68_PABORT		(1 << 6)
+#define	TW68_SBDONE2		(1 << 12)
+#define	TW68_SBERR2		(1 << 13)
+#define	TW68_PPERR		(1 << 14)
+#define	TW68_FFERR		(1 << 15)
+#define	TW68_DET50		(1 << 16)
+#define	TW68_FLOCK		(1 << 17)
+#define	TW68_CCVALID		(1 << 18)
+#define	TW68_VLOCK		(1 << 19)
+#define	TW68_FIELD		(1 << 20)
+#define	TW68_SLOCK		(1 << 21)
+#define	TW68_HLOCK		(1 << 22)
+#define	TW68_VDLOSS		(1 << 23)
+#define	TW68_SBERR		(1 << 24)
+/* define the i2c control register bits */
+#define	TW68_SBMODE		(0)
+#define	TW68_WREN		(1)
+#define	TW68_SSCLK		(6)
+#define	TW68_SSDAT		(7)
+#define	TW68_SBCLK		(8)
+#define	TW68_WDLEN		(16)
+#define	TW68_RDLEN		(20)
+#define	TW68_SBRW		(24)
+#define	TW68_SBDEV		(25)
+
+#define	TW68_SBMODE_B		(1 << TW68_SBMODE)
+#define	TW68_WREN_B		(1 << TW68_WREN)
+#define	TW68_SSCLK_B		(1 << TW68_SSCLK)
+#define	TW68_SSDAT_B		(1 << TW68_SSDAT)
+#define	TW68_SBRW_B		(1 << TW68_SBRW)
+
+#define	TW68_GPDATA		0x100
+#define	TW68_STATUS1		0x204
+#define	TW68_INFORM		0x208
+#define	TW68_OPFORM		0x20C
+#define	TW68_HSYNC		0x210
+#define	TW68_ACNTL		0x218
+#define	TW68_CROP_HI		0x21C
+#define	TW68_VDELAY_LO		0x220
+#define	TW68_VACTIVE_LO		0x224
+#define	TW68_HDELAY_LO		0x228
+#define	TW68_HACTIVE_LO		0x22C
+#define	TW68_CNTRL1		0x230
+#define	TW68_VSCALE_LO		0x234
+#define	TW68_SCALE_HI		0x238
+#define	TW68_HSCALE_LO		0x23C
+#define	TW68_BRIGHT		0x240
+#define	TW68_CONTRAST		0x244
+#define	TW68_SHARPNESS		0x248
+#define	TW68_SAT_U		0x24C
+#define	TW68_SAT_V		0x250
+#define	TW68_HUE		0x254
+#define	TW68_SHARP2		0x258
+#define	TW68_VSHARP		0x25C
+#define	TW68_CORING		0x260
+#define	TW68_VBICNTL		0x264
+#define	TW68_CNTRL2		0x268
+#define	TW68_CC_DATA		0x26C
+#define	TW68_SDT		0x270
+#define	TW68_SDTR		0x274
+#define	TW68_RESERV2		0x278
+#define	TW68_RESERV3		0x27C
+#define	TW68_CLMPG		0x280
+#define	TW68_IAGC		0x284
+#define	TW68_AGCGAIN		0x288
+#define	TW68_PEAKWT		0x28C
+#define	TW68_CLMPL		0x290
+#define	TW68_SYNCT		0x294
+#define	TW68_MISSCNT		0x298
+#define	TW68_PCLAMP		0x29C
+#define	TW68_VCNTL1		0x2A0
+#define	TW68_VCNTL2		0x2A4
+#define	TW68_CKILL		0x2A8
+#define	TW68_COMB		0x2AC
+#define	TW68_LDLY		0x2B0
+#define	TW68_MISC1		0x2B4
+#define	TW68_LOOP		0x2B8
+#define	TW68_MISC2		0x2BC
+#define	TW68_MVSN		0x2C0
+#define	TW68_STATUS2		0x2C4
+#define	TW68_HFREF		0x2C8
+#define	TW68_CLMD		0x2CC
+#define	TW68_IDCNTL		0x2D0
+#define	TW68_CLCNTL1		0x2D4
+
+/* Audio */
+#define	TW68_ACKI1		0x300
+#define	TW68_ACKI2		0x304
+#define	TW68_ACKI3		0x308
+#define	TW68_ACKN1		0x30C
+#define	TW68_ACKN2		0x310
+#define	TW68_ACKN3		0x314
+#define	TW68_SDIV		0x318
+#define	TW68_LRDIV		0x31C
+#define	TW68_ACCNTL		0x320
+
+#define	TW68_VSCTL		0x3B8
+#define	TW68_CHROMAGVAL		0x3BC
+
+#define	TW68_F2CROP_HI		0x3DC
+#define	TW68_F2VDELAY_LO	0x3E0
+#define	TW68_F2VACTIVE_LO	0x3E4
+#define	TW68_F2HDELAY_LO	0x3E8
+#define	TW68_F2HACTIVE_LO	0x3EC
+#define	TW68_F2CNT		0x3F0
+#define	TW68_F2VSCALE_LO	0x3F4
+#define	TW68_F2SCALE_HI		0x3F8
+#define	TW68_F2HSCALE_LO	0x3FC
+
+#define	RISC_INT_BIT		0x08000000
+#define	RISC_SYNCO		0xC0000000
+#define	RISC_SYNCE		0xD0000000
+#define	RISC_JUMP		0xB0000000
+#define	RISC_LINESTART		0x90000000
+#define	RISC_INLINE		0xA0000000
+
+#define VideoFormatNTSC		 0
+#define VideoFormatNTSCJapan	 0
+#define VideoFormatPALBDGHI	 1
+#define VideoFormatSECAM	 2
+#define VideoFormatNTSC443	 3
+#define VideoFormatPALM		 4
+#define VideoFormatPALN		 5
+#define VideoFormatPALNC	 5
+#define VideoFormatPAL60	 6
+#define VideoFormatAuto		 7
+
+#define ColorFormatRGB32	 0x00
+#define ColorFormatRGB24	 0x10
+#define ColorFormatRGB16	 0x20
+#define ColorFormatRGB15	 0x30
+#define ColorFormatYUY2		 0x40
+#define ColorFormatBSWAP         0x04
+#define ColorFormatWSWAP         0x08
+#define ColorFormatGamma         0x80
+#endif
diff --git a/drivers/media/pci/tw68/tw68-risc.c b/drivers/media/pci/tw68/tw68-risc.c
new file mode 100644
index 000000000000..7439db212a69
--- /dev/null
+++ b/drivers/media/pci/tw68/tw68-risc.c
@@ -0,0 +1,230 @@
+/*
+ *  tw68_risc.c
+ *  Part of the device driver for Techwell 68xx based cards
+ *
+ *  Much of this code is derived from the cx88 and sa7134 drivers, which
+ *  were in turn derived from the bt87x driver.  The original work was by
+ *  Gerd Knorr; more recently the code was enhanced by Mauro Carvalho Chehab,
+ *  Hans Verkuil, Andy Walls and many others.  Their work is gratefully
+ *  acknowledged.  Full credit goes to them - any problems within this code
+ *  are mine.
+ *
+ *  Copyright (C) 2009  William M. Brack
+ *
+ *  Refactored and updated to the latest v4l core frameworks:
+ *
+ *  Copyright (C) 2014 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include "tw68.h"
+
+/**
+ *  @rp		pointer to current risc program position
+ *  @sglist	pointer to "scatter-gather list" of buffer pointers
+ *  @offset	offset to target memory buffer
+ *  @sync_line	0 -> no sync, 1 -> odd sync, 2 -> even sync
+ *  @bpl	number of bytes per scan line
+ *  @padding	number of bytes of padding to add
+ *  @lines	number of lines in field
+ *  @jump	insert a jump at the start
+ */
+static __le32 *tw68_risc_field(__le32 *rp, struct scatterlist *sglist,
+			    unsigned int offset, u32 sync_line,
+			    unsigned int bpl, unsigned int padding,
+			    unsigned int lines, bool jump)
+{
+	struct scatterlist *sg;
+	unsigned int line, todo, done;
+
+	if (jump) {
+		*(rp++) = cpu_to_le32(RISC_JUMP);
+		*(rp++) = 0;
+	}
+
+	/* sync instruction */
+	if (sync_line == 1)
+		*(rp++) = cpu_to_le32(RISC_SYNCO);
+	else
+		*(rp++) = cpu_to_le32(RISC_SYNCE);
+	*(rp++) = 0;
+
+	/* scan lines */
+	sg = sglist;
+	for (line = 0; line < lines; line++) {
+		/* calculate next starting position */
+		while (offset && offset >= sg_dma_len(sg)) {
+			offset -= sg_dma_len(sg);
+			sg = sg_next(sg);
+		}
+		if (bpl <= sg_dma_len(sg) - offset) {
+			/* fits into current chunk */
+			*(rp++) = cpu_to_le32(RISC_LINESTART |
+					      /* (offset<<12) |*/  bpl);
+			*(rp++) = cpu_to_le32(sg_dma_address(sg) + offset);
+			offset += bpl;
+		} else {
+			/*
+			 * scanline needs to be split.  Put the start in
+			 * whatever memory remains using RISC_LINESTART,
+			 * then the remainder into following addresses
+			 * given by the scatter-gather list.
+			 */
+			todo = bpl;	/* one full line to be done */
+			/* first fragment */
+			done = (sg_dma_len(sg) - offset);
+			*(rp++) = cpu_to_le32(RISC_LINESTART |
+						(7 << 24) |
+						done);
+			*(rp++) = cpu_to_le32(sg_dma_address(sg) + offset);
+			todo -= done;
+			sg = sg_next(sg);
+			/* succeeding fragments have no offset */
+			while (todo > sg_dma_len(sg)) {
+				*(rp++) = cpu_to_le32(RISC_INLINE |
+						(done << 12) |
+						sg_dma_len(sg));
+				*(rp++) = cpu_to_le32(sg_dma_address(sg));
+				todo -= sg_dma_len(sg);
+				sg = sg_next(sg);
+				done += sg_dma_len(sg);
+			}
+			if (todo) {
+				/* final chunk - offset 0, count 'todo' */
+				*(rp++) = cpu_to_le32(RISC_INLINE |
+							(done << 12) |
+							todo);
+				*(rp++) = cpu_to_le32(sg_dma_address(sg));
+			}
+			offset = todo;
+		}
+		offset += padding;
+	}
+
+	return rp;
+}
+
+/**
+ * tw68_risc_buffer
+ *
+ *	This routine is called by tw68-video.  It allocates
+ *	memory for the dma controller "program" and then fills in that
+ *	memory with the appropriate "instructions".
+ *
+ *	@pci_dev	structure with info about the pci
+ *			slot which our device is in.
+ *	@risc		structure with info about the memory
+ *			used for our controller program.
+ *	@sglist		scatter-gather list entry
+ *	@top_offset	offset within the risc program area for the
+ *			first odd frame line
+ *	@bottom_offset	offset within the risc program area for the
+ *			first even frame line
+ *	@bpl		number of data bytes per scan line
+ *	@padding	number of extra bytes to add at end of line
+ *	@lines		number of scan lines
+ */
+int tw68_risc_buffer(struct pci_dev *pci,
+			struct tw68_buf *buf,
+			struct scatterlist *sglist,
+			unsigned int top_offset,
+			unsigned int bottom_offset,
+			unsigned int bpl,
+			unsigned int padding,
+			unsigned int lines)
+{
+	u32 instructions, fields;
+	__le32 *rp;
+
+	fields = 0;
+	if (UNSET != top_offset)
+		fields++;
+	if (UNSET != bottom_offset)
+		fields++;
+	/*
+	 * estimate risc mem: worst case is one write per page border +
+	 * one write per scan line + syncs + 2 jumps (all 2 dwords).
+	 * Padding can cause next bpl to start close to a page border.
+	 * First DMA region may be smaller than PAGE_SIZE
+	 */
+	instructions  = fields * (1 + (((bpl + padding) * lines) /
+			 PAGE_SIZE) + lines) + 4;
+	buf->size = instructions * 8;
+	buf->cpu = pci_alloc_consistent(pci, buf->size, &buf->dma);
+	if (buf->cpu == NULL)
+		return -ENOMEM;
+
+	/* write risc instructions */
+	rp = buf->cpu;
+	if (UNSET != top_offset)	/* generates SYNCO */
+		rp = tw68_risc_field(rp, sglist, top_offset, 1,
+				     bpl, padding, lines, true);
+	if (UNSET != bottom_offset)	/* generates SYNCE */
+		rp = tw68_risc_field(rp, sglist, bottom_offset, 2,
+				     bpl, padding, lines, top_offset == UNSET);
+
+	/* save pointer to jmp instruction address */
+	buf->jmp = rp;
+	buf->cpu[1] = cpu_to_le32(buf->dma + 8);
+	/* assure risc buffer hasn't overflowed */
+	BUG_ON((buf->jmp - buf->cpu + 2) * sizeof(buf->cpu[0]) > buf->size);
+	return 0;
+}
+
+#if 0
+/* ------------------------------------------------------------------ */
+/* debug helper code                                                  */
+
+static void tw68_risc_decode(u32 risc, u32 addr)
+{
+#define	RISC_OP(reg)	(((reg) >> 28) & 7)
+	static struct instr_details {
+		char *name;
+		u8 has_data_type;
+		u8 has_byte_info;
+		u8 has_addr;
+	} instr[8] = {
+		[RISC_OP(RISC_SYNCO)]	  = {"syncOdd", 0, 0, 0},
+		[RISC_OP(RISC_SYNCE)]	  = {"syncEven", 0, 0, 0},
+		[RISC_OP(RISC_JUMP)]	  = {"jump", 0, 0, 1},
+		[RISC_OP(RISC_LINESTART)] = {"lineStart", 1, 1, 1},
+		[RISC_OP(RISC_INLINE)]	  = {"inline", 1, 1, 1},
+	};
+	u32 p;
+
+	p = RISC_OP(risc);
+	if (!(risc & 0x80000000) || !instr[p].name) {
+		pr_debug("0x%08x [ INVALID ]\n", risc);
+		return;
+	}
+	pr_debug("0x%08x %-9s IRQ=%d",
+		risc, instr[p].name, (risc >> 27) & 1);
+	if (instr[p].has_data_type)
+		pr_debug(" Type=%d", (risc >> 24) & 7);
+	if (instr[p].has_byte_info)
+		pr_debug(" Start=0x%03x Count=%03u",
+			(risc >> 12) & 0xfff, risc & 0xfff);
+	if (instr[p].has_addr)
+		pr_debug(" StartAddr=0x%08x", addr);
+	pr_debug("\n");
+}
+
+void tw68_risc_program_dump(struct tw68_core *core, struct tw68_buf *buf)
+{
+	const __le32 *addr;
+
+	pr_debug("%s: risc_program_dump: risc=%p, buf->cpu=0x%p, buf->jmp=0x%p\n",
+		  core->name, buf, buf->cpu, buf->jmp);
+	for (addr = buf->cpu; addr <= buf->jmp; addr += 2)
+		tw68_risc_decode(*addr, *(addr+1));
+}
+#endif
diff --git a/drivers/media/pci/tw68/tw68-video.c b/drivers/media/pci/tw68/tw68-video.c
new file mode 100644
index 000000000000..5c94ac7c88d9
--- /dev/null
+++ b/drivers/media/pci/tw68/tw68-video.c
@@ -0,0 +1,1051 @@
+/*
+ *  tw68 functions to handle video data
+ *
+ *  Much of this code is derived from the cx88 and sa7134 drivers, which
+ *  were in turn derived from the bt87x driver.  The original work was by
+ *  Gerd Knorr; more recently the code was enhanced by Mauro Carvalho Chehab,
+ *  Hans Verkuil, Andy Walls and many others.  Their work is gratefully
+ *  acknowledged.  Full credit goes to them - any problems within this code
+ *  are mine.
+ *
+ *  Copyright (C) 2009  William M. Brack
+ *
+ *  Refactored and updated to the latest v4l core frameworks:
+ *
+ *  Copyright (C) 2014 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "tw68.h"
+#include "tw68-reg.h"
+
+/* ------------------------------------------------------------------ */
+/* data structs for video                                             */
+/*
+ * FIXME -
+ * Note that the saa7134 has formats, e.g. YUV420, which are classified
+ * as "planar".  These affect overlay mode, and are flagged with a field
+ * ".planar" in the format.  Do we need to implement this in this driver?
+ */
+static const struct tw68_format formats[] = {
+	{
+		.name		= "15 bpp RGB, le",
+		.fourcc		= V4L2_PIX_FMT_RGB555,
+		.depth		= 16,
+		.twformat	= ColorFormatRGB15,
+	}, {
+		.name		= "15 bpp RGB, be",
+		.fourcc		= V4L2_PIX_FMT_RGB555X,
+		.depth		= 16,
+		.twformat	= ColorFormatRGB15 | ColorFormatBSWAP,
+	}, {
+		.name		= "16 bpp RGB, le",
+		.fourcc		= V4L2_PIX_FMT_RGB565,
+		.depth		= 16,
+		.twformat	= ColorFormatRGB16,
+	}, {
+		.name		= "16 bpp RGB, be",
+		.fourcc		= V4L2_PIX_FMT_RGB565X,
+		.depth		= 16,
+		.twformat	= ColorFormatRGB16 | ColorFormatBSWAP,
+	}, {
+		.name		= "24 bpp RGB, le",
+		.fourcc		= V4L2_PIX_FMT_BGR24,
+		.depth		= 24,
+		.twformat	= ColorFormatRGB24,
+	}, {
+		.name		= "24 bpp RGB, be",
+		.fourcc		= V4L2_PIX_FMT_RGB24,
+		.depth		= 24,
+		.twformat	= ColorFormatRGB24 | ColorFormatBSWAP,
+	}, {
+		.name		= "32 bpp RGB, le",
+		.fourcc		= V4L2_PIX_FMT_BGR32,
+		.depth		= 32,
+		.twformat	= ColorFormatRGB32,
+	}, {
+		.name		= "32 bpp RGB, be",
+		.fourcc		= V4L2_PIX_FMT_RGB32,
+		.depth		= 32,
+		.twformat	= ColorFormatRGB32 | ColorFormatBSWAP |
+				  ColorFormatWSWAP,
+	}, {
+		.name		= "4:2:2 packed, YUYV",
+		.fourcc		= V4L2_PIX_FMT_YUYV,
+		.depth		= 16,
+		.twformat	= ColorFormatYUY2,
+	}, {
+		.name		= "4:2:2 packed, UYVY",
+		.fourcc		= V4L2_PIX_FMT_UYVY,
+		.depth		= 16,
+		.twformat	= ColorFormatYUY2 | ColorFormatBSWAP,
+	}
+};
+#define FORMATS ARRAY_SIZE(formats)
+
+#define NORM_625_50			\
+		.h_delay	= 3,	\
+		.h_delay0	= 133,	\
+		.h_start	= 0,	\
+		.h_stop		= 719,	\
+		.v_delay	= 24,	\
+		.vbi_v_start_0	= 7,	\
+		.vbi_v_stop_0	= 22,	\
+		.video_v_start	= 24,	\
+		.video_v_stop	= 311,	\
+		.vbi_v_start_1	= 319
+
+#define NORM_525_60			\
+		.h_delay	= 8,	\
+		.h_delay0	= 138,	\
+		.h_start	= 0,	\
+		.h_stop		= 719,	\
+		.v_delay	= 22,	\
+		.vbi_v_start_0	= 10,	\
+		.vbi_v_stop_0	= 21,	\
+		.video_v_start	= 22,	\
+		.video_v_stop	= 262,	\
+		.vbi_v_start_1	= 273
+
+/*
+ * The following table is searched by tw68_s_std, first for a specific
+ * match, then for an entry which contains the desired id.  The table
+ * entries should therefore be ordered in ascending order of specificity.
+ */
+static const struct tw68_tvnorm tvnorms[] = {
+	{
+		.name		= "PAL", /* autodetect */
+		.id		= V4L2_STD_PAL,
+		NORM_625_50,
+
+		.sync_control	= 0x18,
+		.luma_control	= 0x40,
+		.chroma_ctrl1	= 0x81,
+		.chroma_gain	= 0x2a,
+		.chroma_ctrl2	= 0x06,
+		.vgate_misc	= 0x1c,
+		.format		= VideoFormatPALBDGHI,
+	}, {
+		.name		= "NTSC",
+		.id		= V4L2_STD_NTSC,
+		NORM_525_60,
+
+		.sync_control	= 0x59,
+		.luma_control	= 0x40,
+		.chroma_ctrl1	= 0x89,
+		.chroma_gain	= 0x2a,
+		.chroma_ctrl2	= 0x0e,
+		.vgate_misc	= 0x18,
+		.format		= VideoFormatNTSC,
+	}, {
+		.name		= "SECAM",
+		.id		= V4L2_STD_SECAM,
+		NORM_625_50,
+
+		.sync_control	= 0x18,
+		.luma_control	= 0x1b,
+		.chroma_ctrl1	= 0xd1,
+		.chroma_gain	= 0x80,
+		.chroma_ctrl2	= 0x00,
+		.vgate_misc	= 0x1c,
+		.format		= VideoFormatSECAM,
+	}, {
+		.name		= "PAL-M",
+		.id		= V4L2_STD_PAL_M,
+		NORM_525_60,
+
+		.sync_control	= 0x59,
+		.luma_control	= 0x40,
+		.chroma_ctrl1	= 0xb9,
+		.chroma_gain	= 0x2a,
+		.chroma_ctrl2	= 0x0e,
+		.vgate_misc	= 0x18,
+		.format		= VideoFormatPALM,
+	}, {
+		.name		= "PAL-Nc",
+		.id		= V4L2_STD_PAL_Nc,
+		NORM_625_50,
+
+		.sync_control	= 0x18,
+		.luma_control	= 0x40,
+		.chroma_ctrl1	= 0xa1,
+		.chroma_gain	= 0x2a,
+		.chroma_ctrl2	= 0x06,
+		.vgate_misc	= 0x1c,
+		.format		= VideoFormatPALNC,
+	}, {
+		.name		= "PAL-60",
+		.id		= V4L2_STD_PAL_60,
+		.h_delay	= 186,
+		.h_start	= 0,
+		.h_stop		= 719,
+		.v_delay	= 26,
+		.video_v_start	= 23,
+		.video_v_stop	= 262,
+		.vbi_v_start_0	= 10,
+		.vbi_v_stop_0	= 21,
+		.vbi_v_start_1	= 273,
+
+		.sync_control	= 0x18,
+		.luma_control	= 0x40,
+		.chroma_ctrl1	= 0x81,
+		.chroma_gain	= 0x2a,
+		.chroma_ctrl2	= 0x06,
+		.vgate_misc	= 0x1c,
+		.format		= VideoFormatPAL60,
+	}
+};
+#define TVNORMS ARRAY_SIZE(tvnorms)
+
+static const struct tw68_format *format_by_fourcc(unsigned int fourcc)
+{
+	unsigned int i;
+
+	for (i = 0; i < FORMATS; i++)
+		if (formats[i].fourcc == fourcc)
+			return formats+i;
+	return NULL;
+}
+
+
+/* ------------------------------------------------------------------ */
+/*
+ * Note that the cropping rectangles are described in terms of a single
+ * frame, i.e. line positions are only 1/2 the interlaced equivalent
+ */
+static void set_tvnorm(struct tw68_dev *dev, const struct tw68_tvnorm *norm)
+{
+	if (norm != dev->tvnorm) {
+		dev->width = 720;
+		dev->height = (norm->id & V4L2_STD_525_60) ? 480 : 576;
+		dev->tvnorm = norm;
+		tw68_set_tvnorm_hw(dev);
+	}
+}
+
+/*
+ * tw68_set_scale
+ *
+ * Scaling and Cropping for video decoding
+ *
+ * We are working with 3 values for horizontal and vertical - scale,
+ * delay and active.
+ *
+ * HACTIVE represent the actual number of pixels in the "usable" image,
+ * before scaling.  HDELAY represents the number of pixels skipped
+ * between the start of the horizontal sync and the start of the image.
+ * HSCALE is calculated using the formula
+ *	HSCALE = (HACTIVE / (#pixels desired)) * 256
+ *
+ * The vertical registers are similar, except based upon the total number
+ * of lines in the image, and the first line of the image (i.e. ignoring
+ * vertical sync and VBI).
+ *
+ * Note that the number of bytes reaching the FIFO (and hence needing
+ * to be processed by the DMAP program) is completely dependent upon
+ * these values, especially HSCALE.
+ *
+ * Parameters:
+ *	@dev		pointer to the device structure, needed for
+ *			getting current norm (as well as debug print)
+ *	@width		actual image width (from user buffer)
+ *	@height		actual image height
+ *	@field		indicates Top, Bottom or Interlaced
+ */
+static int tw68_set_scale(struct tw68_dev *dev, unsigned int width,
+			  unsigned int height, enum v4l2_field field)
+{
+	const struct tw68_tvnorm *norm = dev->tvnorm;
+	/* set individually for debugging clarity */
+	int hactive, hdelay, hscale;
+	int vactive, vdelay, vscale;
+	int comb;
+
+	if (V4L2_FIELD_HAS_BOTH(field))	/* if field is interlaced */
+		height /= 2;		/* we must set for 1-frame */
+
+	pr_debug("%s: width=%d, height=%d, both=%d\n"
+		 "  tvnorm h_delay=%d, h_start=%d, h_stop=%d, "
+		 "v_delay=%d, v_start=%d, v_stop=%d\n" , __func__,
+		width, height, V4L2_FIELD_HAS_BOTH(field),
+		norm->h_delay, norm->h_start, norm->h_stop,
+		norm->v_delay, norm->video_v_start,
+		norm->video_v_stop);
+
+	switch (dev->vdecoder) {
+	case TW6800:
+		hdelay = norm->h_delay0;
+		break;
+	default:
+		hdelay = norm->h_delay;
+		break;
+	}
+
+	hdelay += norm->h_start;
+	hactive = norm->h_stop - norm->h_start + 1;
+
+	hscale = (hactive * 256) / (width);
+
+	vdelay = norm->v_delay;
+	vactive = ((norm->id & V4L2_STD_525_60) ? 524 : 624) / 2 - norm->video_v_start;
+	vscale = (vactive * 256) / height;
+
+	pr_debug("%s: %dx%d [%s%s,%s]\n", __func__,
+		width, height,
+		V4L2_FIELD_HAS_TOP(field)    ? "T" : "",
+		V4L2_FIELD_HAS_BOTTOM(field) ? "B" : "",
+		v4l2_norm_to_name(dev->tvnorm->id));
+	pr_debug("%s: hactive=%d, hdelay=%d, hscale=%d; "
+		"vactive=%d, vdelay=%d, vscale=%d\n", __func__,
+		hactive, hdelay, hscale, vactive, vdelay, vscale);
+
+	comb =	((vdelay & 0x300)  >> 2) |
+		((vactive & 0x300) >> 4) |
+		((hdelay & 0x300)  >> 6) |
+		((hactive & 0x300) >> 8);
+	pr_debug("%s: setting CROP_HI=%02x, VDELAY_LO=%02x, "
+		"VACTIVE_LO=%02x, HDELAY_LO=%02x, HACTIVE_LO=%02x\n",
+		__func__, comb, vdelay, vactive, hdelay, hactive);
+	tw_writeb(TW68_CROP_HI, comb);
+	tw_writeb(TW68_VDELAY_LO, vdelay & 0xff);
+	tw_writeb(TW68_VACTIVE_LO, vactive & 0xff);
+	tw_writeb(TW68_HDELAY_LO, hdelay & 0xff);
+	tw_writeb(TW68_HACTIVE_LO, hactive & 0xff);
+
+	comb = ((vscale & 0xf00) >> 4) | ((hscale & 0xf00) >> 8);
+	pr_debug("%s: setting SCALE_HI=%02x, VSCALE_LO=%02x, "
+		"HSCALE_LO=%02x\n", __func__, comb, vscale, hscale);
+	tw_writeb(TW68_SCALE_HI, comb);
+	tw_writeb(TW68_VSCALE_LO, vscale);
+	tw_writeb(TW68_HSCALE_LO, hscale);
+
+	return 0;
+}
+
+/* ------------------------------------------------------------------ */
+
+int tw68_video_start_dma(struct tw68_dev *dev, struct tw68_buf *buf)
+{
+	/* Set cropping and scaling */
+	tw68_set_scale(dev, dev->width, dev->height, dev->field);
+	/*
+	 *  Set start address for RISC program.  Note that if the DMAP
+	 *  processor is currently running, it must be stopped before
+	 *  a new address can be set.
+	 */
+	tw_clearl(TW68_DMAC, TW68_DMAP_EN);
+	tw_writel(TW68_DMAP_SA, buf->dma);
+	/* Clear any pending interrupts */
+	tw_writel(TW68_INTSTAT, dev->board_virqmask);
+	/* Enable the risc engine and the fifo */
+	tw_andorl(TW68_DMAC, 0xff, dev->fmt->twformat |
+		ColorFormatGamma | TW68_DMAP_EN | TW68_FIFO_EN);
+	dev->pci_irqmask |= dev->board_virqmask;
+	tw_setl(TW68_INTMASK, dev->pci_irqmask);
+	return 0;
+}
+
+/* ------------------------------------------------------------------ */
+
+/* calc max # of buffers from size (must not exceed the 4MB virtual
+ * address space per DMA channel) */
+static int tw68_buffer_count(unsigned int size, unsigned int count)
+{
+	unsigned int maxcount;
+
+	maxcount = (4 * 1024 * 1024) / roundup(size, PAGE_SIZE);
+	if (count > maxcount)
+		count = maxcount;
+	return count;
+}
+
+/* ------------------------------------------------------------- */
+/* vb2 queue operations                                          */
+
+static int tw68_queue_setup(struct vb2_queue *q, const struct v4l2_format *fmt,
+			   unsigned int *num_buffers, unsigned int *num_planes,
+			   unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct tw68_dev *dev = vb2_get_drv_priv(q);
+	unsigned tot_bufs = q->num_buffers + *num_buffers;
+
+	sizes[0] = (dev->fmt->depth * dev->width * dev->height) >> 3;
+	/*
+	 * We allow create_bufs, but only if the sizeimage is the same as the
+	 * current sizeimage. The tw68_buffer_count calculation becomes quite
+	 * difficult otherwise.
+	 */
+	if (fmt && fmt->fmt.pix.sizeimage < sizes[0])
+		return -EINVAL;
+	*num_planes = 1;
+	if (tot_bufs < 2)
+		tot_bufs = 2;
+	tot_bufs = tw68_buffer_count(sizes[0], tot_bufs);
+	*num_buffers = tot_bufs - q->num_buffers;
+
+	return 0;
+}
+
+/*
+ * The risc program for each buffers works as follows: it starts with a simple
+ * 'JUMP to addr + 8', which is effectively a NOP. Then the program to DMA the
+ * buffer follows and at the end we have a JUMP back to the start + 8 (skipping
+ * the initial JUMP).
+ *
+ * This is the program of the first buffer to be queued if the active list is
+ * empty and it just keeps DMAing this buffer without generating any interrupts.
+ *
+ * If a new buffer is added then the initial JUMP in the program generates an
+ * interrupt as well which signals that the previous buffer has been DMAed
+ * successfully and that it can be returned to userspace.
+ *
+ * It also sets the final jump of the previous buffer to the start of the new
+ * buffer, thus chaining the new buffer into the DMA chain. This is a single
+ * atomic u32 write, so there is no race condition.
+ *
+ * The end-result of all this that you only get an interrupt when a buffer
+ * is ready, so the control flow is very easy.
+ */
+static void tw68_buf_queue(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct tw68_dev *dev = vb2_get_drv_priv(vq);
+	struct tw68_buf *buf = container_of(vb, struct tw68_buf, vb);
+	struct tw68_buf *prev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->slock, flags);
+
+	/* append a 'JUMP to start of buffer' to the buffer risc program */
+	buf->jmp[0] = cpu_to_le32(RISC_JUMP);
+	buf->jmp[1] = cpu_to_le32(buf->dma + 8);
+
+	if (!list_empty(&dev->active)) {
+		prev = list_entry(dev->active.prev, struct tw68_buf, list);
+		buf->cpu[0] |= cpu_to_le32(RISC_INT_BIT);
+		prev->jmp[1] = cpu_to_le32(buf->dma);
+	}
+	list_add_tail(&buf->list, &dev->active);
+	spin_unlock_irqrestore(&dev->slock, flags);
+}
+
+/*
+ * buffer_prepare
+ *
+ * Set the ancilliary information into the buffer structure.  This
+ * includes generating the necessary risc program if it hasn't already
+ * been done for the current buffer format.
+ * The structure fh contains the details of the format requested by the
+ * user - type, width, height and #fields.  This is compared with the
+ * last format set for the current buffer.  If they differ, the risc
+ * code (which controls the filling of the buffer) is (re-)generated.
+ */
+static int tw68_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct tw68_dev *dev = vb2_get_drv_priv(vq);
+	struct tw68_buf *buf = container_of(vb, struct tw68_buf, vb);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(vb, 0);
+	unsigned size, bpl;
+	int rc;
+
+	size = (dev->width * dev->height * dev->fmt->depth) >> 3;
+	if (vb2_plane_size(vb, 0) < size)
+		return -EINVAL;
+	vb2_set_plane_payload(vb, 0, size);
+
+	rc = dma_map_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
+	if (!rc)
+		return -EIO;
+
+	bpl = (dev->width * dev->fmt->depth) >> 3;
+	switch (dev->field) {
+	case V4L2_FIELD_TOP:
+		tw68_risc_buffer(dev->pci, buf, dma->sgl,
+				 0, UNSET, bpl, 0, dev->height);
+		break;
+	case V4L2_FIELD_BOTTOM:
+		tw68_risc_buffer(dev->pci, buf, dma->sgl,
+				 UNSET, 0, bpl, 0, dev->height);
+		break;
+	case V4L2_FIELD_SEQ_TB:
+		tw68_risc_buffer(dev->pci, buf, dma->sgl,
+				 0, bpl * (dev->height >> 1),
+				 bpl, 0, dev->height >> 1);
+		break;
+	case V4L2_FIELD_SEQ_BT:
+		tw68_risc_buffer(dev->pci, buf, dma->sgl,
+				 bpl * (dev->height >> 1), 0,
+				 bpl, 0, dev->height >> 1);
+		break;
+	case V4L2_FIELD_INTERLACED:
+	default:
+		tw68_risc_buffer(dev->pci, buf, dma->sgl,
+				 0, bpl, bpl, bpl, dev->height >> 1);
+		break;
+	}
+	return 0;
+}
+
+static void tw68_buf_finish(struct vb2_buffer *vb)
+{
+	struct vb2_queue *vq = vb->vb2_queue;
+	struct tw68_dev *dev = vb2_get_drv_priv(vq);
+	struct sg_table *dma = vb2_dma_sg_plane_desc(vb, 0);
+	struct tw68_buf *buf = container_of(vb, struct tw68_buf, vb);
+
+	dma_unmap_sg(&dev->pci->dev, dma->sgl, dma->nents, DMA_FROM_DEVICE);
+
+	pci_free_consistent(dev->pci, buf->size, buf->cpu, buf->dma);
+}
+
+static int tw68_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct tw68_dev *dev = vb2_get_drv_priv(q);
+	struct tw68_buf *buf =
+		container_of(dev->active.next, struct tw68_buf, list);
+
+	dev->seqnr = 0;
+	tw68_video_start_dma(dev, buf);
+	return 0;
+}
+
+static void tw68_stop_streaming(struct vb2_queue *q)
+{
+	struct tw68_dev *dev = vb2_get_drv_priv(q);
+
+	/* Stop risc & fifo */
+	tw_clearl(TW68_DMAC, TW68_DMAP_EN | TW68_FIFO_EN);
+	while (!list_empty(&dev->active)) {
+		struct tw68_buf *buf =
+			container_of(dev->active.next, struct tw68_buf, list);
+
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+}
+
+static struct vb2_ops tw68_video_qops = {
+	.queue_setup	= tw68_queue_setup,
+	.buf_queue	= tw68_buf_queue,
+	.buf_prepare	= tw68_buf_prepare,
+	.buf_finish	= tw68_buf_finish,
+	.start_streaming = tw68_start_streaming,
+	.stop_streaming = tw68_stop_streaming,
+	.wait_prepare	= vb2_ops_wait_prepare,
+	.wait_finish	= vb2_ops_wait_finish,
+};
+
+/* ------------------------------------------------------------------ */
+
+static int tw68_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct tw68_dev *dev =
+		container_of(ctrl->handler, struct tw68_dev, hdl);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		tw_writeb(TW68_BRIGHT, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		tw_writeb(TW68_HUE, ctrl->val);
+		break;
+	case V4L2_CID_CONTRAST:
+		tw_writeb(TW68_CONTRAST, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		tw_writeb(TW68_SAT_U, ctrl->val);
+		tw_writeb(TW68_SAT_V, ctrl->val);
+		break;
+	case V4L2_CID_COLOR_KILLER:
+		if (ctrl->val)
+			tw_andorb(TW68_MISC2, 0xe0, 0xe0);
+		else
+			tw_andorb(TW68_MISC2, 0xe0, 0x00);
+		break;
+	case V4L2_CID_CHROMA_AGC:
+		if (ctrl->val)
+			tw_andorb(TW68_LOOP, 0x30, 0x20);
+		else
+			tw_andorb(TW68_LOOP, 0x30, 0x00);
+		break;
+	}
+	return 0;
+}
+
+/* ------------------------------------------------------------------ */
+
+/*
+ * Note that this routine returns what is stored in the fh structure, and
+ * does not interrogate any of the device registers.
+ */
+static int tw68_g_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	f->fmt.pix.width        = dev->width;
+	f->fmt.pix.height       = dev->height;
+	f->fmt.pix.field        = dev->field;
+	f->fmt.pix.pixelformat  = dev->fmt->fourcc;
+	f->fmt.pix.bytesperline =
+		(f->fmt.pix.width * (dev->fmt->depth)) >> 3;
+	f->fmt.pix.sizeimage =
+		f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace	= V4L2_COLORSPACE_SMPTE170M;
+	f->fmt.pix.priv = 0;
+	return 0;
+}
+
+static int tw68_try_fmt_vid_cap(struct file *file, void *priv,
+						struct v4l2_format *f)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+	const struct tw68_format *fmt;
+	enum v4l2_field field;
+	unsigned int maxh;
+
+	fmt = format_by_fourcc(f->fmt.pix.pixelformat);
+	if (NULL == fmt)
+		return -EINVAL;
+
+	field = f->fmt.pix.field;
+	maxh  = (dev->tvnorm->id & V4L2_STD_525_60) ? 480 : 576;
+
+	switch (field) {
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+		break;
+	case V4L2_FIELD_INTERLACED:
+	case V4L2_FIELD_SEQ_BT:
+	case V4L2_FIELD_SEQ_TB:
+		maxh = maxh * 2;
+		break;
+	default:
+		field = (f->fmt.pix.height > maxh / 2)
+			? V4L2_FIELD_INTERLACED
+			: V4L2_FIELD_BOTTOM;
+		break;
+	}
+
+	f->fmt.pix.field = field;
+	if (f->fmt.pix.width  < 48)
+		f->fmt.pix.width  = 48;
+	if (f->fmt.pix.height < 32)
+		f->fmt.pix.height = 32;
+	if (f->fmt.pix.width > 720)
+		f->fmt.pix.width = 720;
+	if (f->fmt.pix.height > maxh)
+		f->fmt.pix.height = maxh;
+	f->fmt.pix.width &= ~0x03;
+	f->fmt.pix.bytesperline =
+		(f->fmt.pix.width * (fmt->depth)) >> 3;
+	f->fmt.pix.sizeimage =
+		f->fmt.pix.height * f->fmt.pix.bytesperline;
+	f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
+	return 0;
+}
+
+/*
+ * Note that tw68_s_fmt_vid_cap sets the information into the fh structure,
+ * and it will be used for all future new buffers.  However, there could be
+ * some number of buffers on the "active" chain which will be filled before
+ * the change takes place.
+ */
+static int tw68_s_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+	int err;
+
+	err = tw68_try_fmt_vid_cap(file, priv, f);
+	if (0 != err)
+		return err;
+
+	dev->fmt = format_by_fourcc(f->fmt.pix.pixelformat);
+	dev->width = f->fmt.pix.width;
+	dev->height = f->fmt.pix.height;
+	dev->field = f->fmt.pix.field;
+	return 0;
+}
+
+static int tw68_enum_input(struct file *file, void *priv,
+					struct v4l2_input *i)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+	unsigned int n;
+
+	n = i->index;
+	if (n >= TW68_INPUT_MAX)
+		return -EINVAL;
+	i->index = n;
+	i->type = V4L2_INPUT_TYPE_CAMERA;
+	snprintf(i->name, sizeof(i->name), "Composite %d", n);
+
+	/* If the query is for the current input, get live data */
+	if (n == dev->input) {
+		int v1 = tw_readb(TW68_STATUS1);
+		int v2 = tw_readb(TW68_MVSN);
+
+		if (0 != (v1 & (1 << 7)))
+			i->status |= V4L2_IN_ST_NO_SYNC;
+		if (0 != (v1 & (1 << 6)))
+			i->status |= V4L2_IN_ST_NO_H_LOCK;
+		if (0 != (v1 & (1 << 2)))
+			i->status |= V4L2_IN_ST_NO_SIGNAL;
+		if (0 != (v1 & 1 << 1))
+			i->status |= V4L2_IN_ST_NO_COLOR;
+		if (0 != (v2 & (1 << 2)))
+			i->status |= V4L2_IN_ST_MACROVISION;
+	}
+	i->std = video_devdata(file)->tvnorms;
+	return 0;
+}
+
+static int tw68_g_input(struct file *file, void *priv, unsigned int *i)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	*i = dev->input;
+	return 0;
+}
+
+static int tw68_s_input(struct file *file, void *priv, unsigned int i)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	if (i >= TW68_INPUT_MAX)
+		return -EINVAL;
+	dev->input = i;
+	tw_andorb(TW68_INFORM, 0x03 << 2, dev->input << 2);
+	return 0;
+}
+
+static int tw68_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *cap)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	strcpy(cap->driver, "tw68");
+	strlcpy(cap->card, "Techwell Capture Card",
+		sizeof(cap->card));
+	sprintf(cap->bus_info, "PCI:%s", pci_name(dev->pci));
+	cap->device_caps =
+		V4L2_CAP_VIDEO_CAPTURE |
+		V4L2_CAP_READWRITE |
+		V4L2_CAP_STREAMING;
+
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int tw68_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+	unsigned int i;
+
+	if (vb2_is_busy(&dev->vidq))
+		return -EBUSY;
+
+	/* Look for match on complete norm id (may have mult bits) */
+	for (i = 0; i < TVNORMS; i++) {
+		if (id == tvnorms[i].id)
+			break;
+	}
+
+	/* If no exact match, look for norm which contains this one */
+	if (i == TVNORMS) {
+		for (i = 0; i < TVNORMS; i++)
+			if (id & tvnorms[i].id)
+				break;
+	}
+	/* If still not matched, give up */
+	if (i == TVNORMS)
+		return -EINVAL;
+
+	set_tvnorm(dev, &tvnorms[i]);	/* do the actual setting */
+	return 0;
+}
+
+static int tw68_g_std(struct file *file, void *priv, v4l2_std_id *id)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	*id = dev->tvnorm->id;
+	return 0;
+}
+
+static int tw68_enum_fmt_vid_cap(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	if (f->index >= FORMATS)
+		return -EINVAL;
+
+	strlcpy(f->description, formats[f->index].name,
+		sizeof(f->description));
+
+	f->pixelformat = formats[f->index].fourcc;
+
+	return 0;
+}
+
+/*
+ * Used strictly for internal development and debugging, this routine
+ * prints out the current register contents for the tw68xx device.
+ */
+static void tw68_dump_regs(struct tw68_dev *dev)
+{
+	unsigned char line[80];
+	int i, j, k;
+	unsigned char *cptr;
+
+	pr_info("Full dump of TW68 registers:\n");
+	/* First we do the PCI regs, 8 4-byte regs per line */
+	for (i = 0; i < 0x100; i += 32) {
+		cptr = line;
+		cptr += sprintf(cptr, "%03x  ", i);
+		/* j steps through the next 4 words */
+		for (j = i; j < i + 16; j += 4)
+			cptr += sprintf(cptr, "%08x ", tw_readl(j));
+		*cptr++ = ' ';
+		for (; j < i + 32; j += 4)
+			cptr += sprintf(cptr, "%08x ", tw_readl(j));
+		*cptr++ = '\n';
+		*cptr = 0;
+		pr_info("%s", line);
+	}
+	/* Next the control regs, which are single-byte, address mod 4 */
+	while (i < 0x400) {
+		cptr = line;
+		cptr += sprintf(cptr, "%03x ", i);
+		/* Print out 4 groups of 4 bytes */
+		for (j = 0; j < 4; j++) {
+			for (k = 0; k < 4; k++) {
+				cptr += sprintf(cptr, "%02x ",
+					tw_readb(i));
+				i += 4;
+			}
+			*cptr++ = ' ';
+		}
+		*cptr++ = '\n';
+		*cptr = 0;
+		pr_info("%s", line);
+	}
+}
+
+static int vidioc_log_status(struct file *file, void *priv)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	tw68_dump_regs(dev);
+	return v4l2_ctrl_log_status(file, priv);
+}
+
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+static int vidioc_g_register(struct file *file, void *priv,
+			      struct v4l2_dbg_register *reg)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	if (reg->size == 1)
+		reg->val = tw_readb(reg->reg);
+	else
+		reg->val = tw_readl(reg->reg);
+	return 0;
+}
+
+static int vidioc_s_register(struct file *file, void *priv,
+				const struct v4l2_dbg_register *reg)
+{
+	struct tw68_dev *dev = video_drvdata(file);
+
+	if (reg->size == 1)
+		tw_writeb(reg->reg, reg->val);
+	else
+		tw_writel(reg->reg & 0xffff, reg->val);
+	return 0;
+}
+#endif
+
+static const struct v4l2_ctrl_ops tw68_ctrl_ops = {
+	.s_ctrl = tw68_s_ctrl,
+};
+
+static const struct v4l2_file_operations video_fops = {
+	.owner			= THIS_MODULE,
+	.open			= v4l2_fh_open,
+	.release		= vb2_fop_release,
+	.read			= vb2_fop_read,
+	.poll			= vb2_fop_poll,
+	.mmap			= vb2_fop_mmap,
+	.unlocked_ioctl		= video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops video_ioctl_ops = {
+	.vidioc_querycap		= tw68_querycap,
+	.vidioc_enum_fmt_vid_cap	= tw68_enum_fmt_vid_cap,
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+	.vidioc_s_std			= tw68_s_std,
+	.vidioc_g_std			= tw68_g_std,
+	.vidioc_enum_input		= tw68_enum_input,
+	.vidioc_g_input			= tw68_g_input,
+	.vidioc_s_input			= tw68_s_input,
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+	.vidioc_g_fmt_vid_cap		= tw68_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= tw68_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= tw68_s_fmt_vid_cap,
+	.vidioc_log_status		= vidioc_log_status,
+	.vidioc_subscribe_event		= v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+#ifdef CONFIG_VIDEO_ADV_DEBUG
+	.vidioc_g_register              = vidioc_g_register,
+	.vidioc_s_register              = vidioc_s_register,
+#endif
+};
+
+static struct video_device tw68_video_template = {
+	.name			= "tw68_video",
+	.fops			= &video_fops,
+	.ioctl_ops		= &video_ioctl_ops,
+	.release		= video_device_release_empty,
+	.tvnorms		= TW68_NORMS,
+};
+
+/* ------------------------------------------------------------------ */
+/* exported stuff                                                     */
+void tw68_set_tvnorm_hw(struct tw68_dev *dev)
+{
+	tw_andorb(TW68_SDT, 0x07, dev->tvnorm->format);
+}
+
+int tw68_video_init1(struct tw68_dev *dev)
+{
+	struct v4l2_ctrl_handler *hdl = &dev->hdl;
+
+	v4l2_ctrl_handler_init(hdl, 6);
+	v4l2_ctrl_new_std(hdl, &tw68_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, -128, 127, 1, 20);
+	v4l2_ctrl_new_std(hdl, &tw68_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 100);
+	v4l2_ctrl_new_std(hdl, &tw68_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+	/* NTSC only */
+	v4l2_ctrl_new_std(hdl, &tw68_ctrl_ops,
+			V4L2_CID_HUE, -128, 127, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw68_ctrl_ops,
+			V4L2_CID_COLOR_KILLER, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(hdl, &tw68_ctrl_ops,
+			V4L2_CID_CHROMA_AGC, 0, 1, 1, 1);
+	if (hdl->error) {
+		v4l2_ctrl_handler_free(hdl);
+		return hdl->error;
+	}
+	dev->v4l2_dev.ctrl_handler = hdl;
+	v4l2_ctrl_handler_setup(hdl);
+	return 0;
+}
+
+int tw68_video_init2(struct tw68_dev *dev, int video_nr)
+{
+	int ret;
+
+	set_tvnorm(dev, &tvnorms[0]);
+
+	dev->fmt      = format_by_fourcc(V4L2_PIX_FMT_BGR24);
+	dev->width    = 720;
+	dev->height   = 576;
+	dev->field    = V4L2_FIELD_INTERLACED;
+
+	INIT_LIST_HEAD(&dev->active);
+	dev->vidq.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dev->vidq.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	dev->vidq.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ | VB2_DMABUF;
+	dev->vidq.ops = &tw68_video_qops;
+	dev->vidq.mem_ops = &vb2_dma_sg_memops;
+	dev->vidq.drv_priv = dev;
+	dev->vidq.gfp_flags = __GFP_DMA32;
+	dev->vidq.buf_struct_size = sizeof(struct tw68_buf);
+	dev->vidq.lock = &dev->lock;
+	dev->vidq.min_buffers_needed = 2;
+	ret = vb2_queue_init(&dev->vidq);
+	if (ret)
+		return ret;
+	dev->vdev = tw68_video_template;
+	dev->vdev.v4l2_dev = &dev->v4l2_dev;
+	dev->vdev.lock = &dev->lock;
+	dev->vdev.queue = &dev->vidq;
+	video_set_drvdata(&dev->vdev, dev);
+	return video_register_device(&dev->vdev, VFL_TYPE_GRABBER, video_nr);
+}
+
+/*
+ * tw68_irq_video_done
+ */
+void tw68_irq_video_done(struct tw68_dev *dev, unsigned long status)
+{
+	__u32 reg;
+
+	/* reset interrupts handled by this routine */
+	tw_writel(TW68_INTSTAT, status);
+	/*
+	 * Check most likely first
+	 *
+	 * DMAPI shows we have reached the end of the risc code
+	 * for the current buffer.
+	 */
+	if (status & TW68_DMAPI) {
+		struct tw68_buf *buf;
+
+		spin_lock(&dev->slock);
+		buf = list_entry(dev->active.next, struct tw68_buf, list);
+		list_del(&buf->list);
+		spin_unlock(&dev->slock);
+		v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+		buf->vb.v4l2_buf.field = dev->field;
+		buf->vb.v4l2_buf.sequence = dev->seqnr++;
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
+		status &= ~(TW68_DMAPI);
+		if (0 == status)
+			return;
+	}
+	if (status & (TW68_VLOCK | TW68_HLOCK))
+		dev_dbg(&dev->pci->dev, "Lost sync\n");
+	if (status & TW68_PABORT)
+		dev_err(&dev->pci->dev, "PABORT interrupt\n");
+	if (status & TW68_DMAPERR)
+		dev_err(&dev->pci->dev, "DMAPERR interrupt\n");
+	/*
+	 * On TW6800, FDMIS is apparently generated if video input is switched
+	 * during operation.  Therefore, it is not enabled for that chip.
+	 */
+	if (status & TW68_FDMIS)
+		dev_dbg(&dev->pci->dev, "FDMIS interrupt\n");
+	if (status & TW68_FFOF) {
+		/* probably a logic error */
+		reg = tw_readl(TW68_DMAC) & TW68_FIFO_EN;
+		tw_clearl(TW68_DMAC, TW68_FIFO_EN);
+		dev_dbg(&dev->pci->dev, "FFOF interrupt\n");
+		tw_setl(TW68_DMAC, reg);
+	}
+	if (status & TW68_FFERR)
+		dev_dbg(&dev->pci->dev, "FFERR interrupt\n");
+}
diff --git a/drivers/media/pci/tw68/tw68.h b/drivers/media/pci/tw68/tw68.h
new file mode 100644
index 000000000000..2c8abe26b13b
--- /dev/null
+++ b/drivers/media/pci/tw68/tw68.h
@@ -0,0 +1,231 @@
+/*
+ *  tw68 driver common header file
+ *
+ *  Much of this code is derived from the cx88 and sa7134 drivers, which
+ *  were in turn derived from the bt87x driver.  The original work was by
+ *  Gerd Knorr; more recently the code was enhanced by Mauro Carvalho Chehab,
+ *  Hans Verkuil, Andy Walls and many others.  Their work is gratefully
+ *  acknowledged.  Full credit goes to them - any problems within this code
+ *  are mine.
+ *
+ *  Copyright (C) 2009  William M. Brack
+ *
+ *  Refactored and updated to the latest v4l core frameworks:
+ *
+ *  Copyright (C) 2014 Hans Verkuil <hverkuil@xs4all.nl>
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#include <linux/version.h>
+#include <linux/pci.h>
+#include <linux/videodev2.h>
+#include <linux/notifier.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/io.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/videobuf2-dma-sg.h>
+
+#include "tw68-reg.h"
+
+#define	UNSET	(-1U)
+
+/* system vendor and device ID's */
+#define	PCI_VENDOR_ID_TECHWELL	0x1797
+#define	PCI_DEVICE_ID_6800	0x6800
+#define	PCI_DEVICE_ID_6801	0x6801
+#define	PCI_DEVICE_ID_AUDIO2	0x6802
+#define	PCI_DEVICE_ID_TS3	0x6803
+#define	PCI_DEVICE_ID_6804	0x6804
+#define	PCI_DEVICE_ID_AUDIO5	0x6805
+#define	PCI_DEVICE_ID_TS6	0x6806
+
+/* tw6816 based cards */
+#define	PCI_DEVICE_ID_6816_1   0x6810
+#define	PCI_DEVICE_ID_6816_2   0x6811
+#define	PCI_DEVICE_ID_6816_3   0x6812
+#define	PCI_DEVICE_ID_6816_4   0x6813
+
+#define TW68_NORMS ( \
+	V4L2_STD_NTSC    | V4L2_STD_PAL       | V4L2_STD_SECAM    | \
+	V4L2_STD_PAL_M   | V4L2_STD_PAL_Nc    | V4L2_STD_PAL_60)
+
+#define	TW68_VID_INTS	(TW68_FFERR | TW68_PABORT | TW68_DMAPERR | \
+			 TW68_FFOF   | TW68_DMAPI)
+/* TW6800 chips have trouble with these, so we don't set them for that chip */
+#define	TW68_VID_INTSX	(TW68_FDMIS | TW68_HLOCK | TW68_VLOCK)
+
+#define	TW68_I2C_INTS	(TW68_SBERR | TW68_SBDONE | TW68_SBERR2  | \
+			 TW68_SBDONE2)
+
+enum tw68_decoder_type {
+	TW6800,
+	TW6801,
+	TW6804,
+	TWXXXX,
+};
+
+/* ----------------------------------------------------------- */
+/* static data                                                 */
+
+struct tw68_tvnorm {
+	char		*name;
+	v4l2_std_id	id;
+
+	/* video decoder */
+	u32	sync_control;
+	u32	luma_control;
+	u32	chroma_ctrl1;
+	u32	chroma_gain;
+	u32	chroma_ctrl2;
+	u32	vgate_misc;
+
+	/* video scaler */
+	u32	h_delay;
+	u32	h_delay0;	/* for TW6800 */
+	u32	h_start;
+	u32	h_stop;
+	u32	v_delay;
+	u32	video_v_start;
+	u32	video_v_stop;
+	u32	vbi_v_start_0;
+	u32	vbi_v_stop_0;
+	u32	vbi_v_start_1;
+
+	/* Techwell specific */
+	u32	format;
+};
+
+struct tw68_format {
+	char	*name;
+	u32	fourcc;
+	u32	depth;
+	u32	twformat;
+};
+
+/* ----------------------------------------------------------- */
+/* card configuration					  */
+
+#define TW68_BOARD_NOAUTO		UNSET
+#define TW68_BOARD_UNKNOWN		0
+#define	TW68_BOARD_GENERIC_6802		1
+
+#define	TW68_MAXBOARDS			16
+#define	TW68_INPUT_MAX			4
+
+/* ----------------------------------------------------------- */
+/* device / file handle status                                 */
+
+#define	BUFFER_TIMEOUT	msecs_to_jiffies(500)	/* 0.5 seconds */
+
+struct tw68_dev;	/* forward delclaration */
+
+/* buffer for one video/vbi/ts frame */
+struct tw68_buf {
+	struct vb2_buffer vb;
+	struct list_head list;
+
+	unsigned int   size;
+	__le32         *cpu;
+	__le32         *jmp;
+	dma_addr_t     dma;
+};
+
+struct tw68_fmt {
+	char			*name;
+	u32			fourcc;	/* v4l2 format id */
+	int			depth;
+	int			flags;
+	u32			twformat;
+};
+
+/* global device status */
+struct tw68_dev {
+	struct mutex		lock;
+	spinlock_t		slock;
+	u16			instance;
+	struct v4l2_device	v4l2_dev;
+
+	/* various device info */
+	enum tw68_decoder_type	vdecoder;
+	struct video_device	vdev;
+	struct v4l2_ctrl_handler hdl;
+
+	/* pci i/o */
+	char			*name;
+	struct pci_dev		*pci;
+	unsigned char		pci_rev, pci_lat;
+	u32			__iomem *lmmio;
+	u8			__iomem *bmmio;
+	u32			pci_irqmask;
+	/* The irq mask to be used will depend upon the chip type */
+	u32			board_virqmask;
+
+	/* video capture */
+	const struct tw68_format *fmt;
+	unsigned		width, height;
+	unsigned		seqnr;
+	unsigned		field;
+	struct vb2_queue	vidq;
+	struct list_head	active;
+
+	/* various v4l controls */
+	const struct tw68_tvnorm *tvnorm;	/* video */
+
+	int			input;
+};
+
+/* ----------------------------------------------------------- */
+
+#define tw_readl(reg)		readl(dev->lmmio + ((reg) >> 2))
+#define	tw_readb(reg)		readb(dev->bmmio + (reg))
+#define tw_writel(reg, value)	writel((value), dev->lmmio + ((reg) >> 2))
+#define	tw_writeb(reg, value)	writeb((value), dev->bmmio + (reg))
+
+#define tw_andorl(reg, mask, value) \
+		writel((readl(dev->lmmio+((reg)>>2)) & ~(mask)) |\
+		((value) & (mask)), dev->lmmio+((reg)>>2))
+#define	tw_andorb(reg, mask, value) \
+		writeb((readb(dev->bmmio + (reg)) & ~(mask)) |\
+		((value) & (mask)), dev->bmmio+(reg))
+#define tw_setl(reg, bit)	tw_andorl((reg), (bit), (bit))
+#define	tw_setb(reg, bit)	tw_andorb((reg), (bit), (bit))
+#define	tw_clearl(reg, bit)	\
+		writel((readl(dev->lmmio + ((reg) >> 2)) & ~(bit)), \
+		dev->lmmio + ((reg) >> 2))
+#define	tw_clearb(reg, bit)	\
+		writeb((readb(dev->bmmio+(reg)) & ~(bit)), \
+		dev->bmmio + (reg))
+
+#define tw_wait(us) { udelay(us); }
+
+/* ----------------------------------------------------------- */
+/* tw68-video.c                                                */
+
+void tw68_set_tvnorm_hw(struct tw68_dev *dev);
+
+int tw68_video_init1(struct tw68_dev *dev);
+int tw68_video_init2(struct tw68_dev *dev, int video_nr);
+void tw68_irq_video_done(struct tw68_dev *dev, unsigned long status);
+int tw68_video_start_dma(struct tw68_dev *dev, struct tw68_buf *buf);
+
+/* ----------------------------------------------------------- */
+/* tw68-risc.c                                                 */
+
+int tw68_risc_buffer(struct pci_dev *pci, struct tw68_buf *buf,
+	struct scatterlist *sglist, unsigned int top_offset,
+	unsigned int bottom_offset, unsigned int bpl,
+	unsigned int padding, unsigned int lines);
diff --git a/drivers/media/pci/zoran/zoran_device.c b/drivers/media/pci/zoran/zoran_device.c
index bf34b93f23ee..b6801e035ea4 100644
--- a/drivers/media/pci/zoran/zoran_device.c
+++ b/drivers/media/pci/zoran/zoran_device.c
@@ -682,7 +682,7 @@ set_videobus_dir (struct zoran *zr,
 	switch (zr->card.type) {
 	case LML33:
 	case LML33R10:
-		if (lml33dpath == 0)
+		if (!lml33dpath)
 			GPIO(zr, 5, val);
 		else
 			GPIO(zr, 5, 1);
diff --git a/drivers/media/platform/Kconfig b/drivers/media/platform/Kconfig
index 6d86646d9743..bee9074ebc13 100644
--- a/drivers/media/platform/Kconfig
+++ b/drivers/media/platform/Kconfig
@@ -56,7 +56,8 @@ config VIDEO_VIU
 
 config VIDEO_TIMBERDALE
 	tristate "Support for timberdale Video In/LogiWIN"
-	depends on MFD_TIMBERDALE && VIDEO_V4L2 && I2C && DMADEVICES
+	depends on VIDEO_V4L2 && I2C && DMADEVICES
+	depends on MFD_TIMBERDALE || COMPILE_TEST
 	select DMA_ENGINE
 	select TIMB_DMA
 	select VIDEO_ADV7180
@@ -74,7 +75,8 @@ config VIDEO_VINO
 
 config VIDEO_M32R_AR
 	tristate "AR devices"
-	depends on M32R && VIDEO_V4L2
+	depends on VIDEO_V4L2
+	depends on M32R || COMPILE_TEST
 	---help---
 	  This is a video4linux driver for the Renesas AR (Artificial Retina)
 	  camera module.
@@ -94,6 +96,7 @@ config VIDEO_M32R_AR_M64278
 config VIDEO_OMAP3
 	tristate "OMAP 3 Camera support"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API && ARCH_OMAP3
+	depends on HAS_DMA
 	select ARM_DMA_USE_IOMMU
 	select OMAP_IOMMU
 	select VIDEOBUF2_DMA_CONTIG
@@ -109,7 +112,9 @@ config VIDEO_OMAP3_DEBUG
 config VIDEO_S3C_CAMIF
 	tristate "Samsung S3C24XX/S3C64XX SoC Camera Interface driver"
 	depends on VIDEO_V4L2 && I2C && VIDEO_V4L2_SUBDEV_API
-	depends on (ARCH_S3C64XX || PLAT_S3C24XX) && PM_RUNTIME
+	depends on PM_RUNTIME
+	depends on ARCH_S3C64XX || PLAT_S3C24XX || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  This is a v4l2 driver for s3c24xx and s3c64xx SoC series camera
@@ -140,6 +145,7 @@ if V4L_MEM2MEM_DRIVERS
 config VIDEO_CODA
 	tristate "Chips&Media Coda multi-standard codec IP"
 	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_MXC
+	depends on HAS_DMA
 	select SRAM
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
@@ -151,6 +157,7 @@ config VIDEO_CODA
 config VIDEO_MEM2MEM_DEINTERLACE
 	tristate "Deinterlace support"
 	depends on VIDEO_DEV && VIDEO_V4L2 && DMA_ENGINE
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	help
@@ -158,7 +165,9 @@ config VIDEO_MEM2MEM_DEINTERLACE
 
 config VIDEO_SAMSUNG_S5P_G2D
 	tristate "Samsung S5P and EXYNOS4 G2D 2d graphics accelerator driver"
-	depends on VIDEO_DEV && VIDEO_V4L2 && (PLAT_S5P || ARCH_EXYNOS)
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on PLAT_S5P || ARCH_EXYNOS || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	default n
@@ -168,7 +177,9 @@ config VIDEO_SAMSUNG_S5P_G2D
 
 config VIDEO_SAMSUNG_S5P_JPEG
 	tristate "Samsung S5P/Exynos3250/Exynos4 JPEG codec driver"
-	depends on VIDEO_DEV && VIDEO_V4L2 && (PLAT_S5P || ARCH_EXYNOS)
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on PLAT_S5P || ARCH_EXYNOS || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	---help---
@@ -177,7 +188,9 @@ config VIDEO_SAMSUNG_S5P_JPEG
 
 config VIDEO_SAMSUNG_S5P_MFC
 	tristate "Samsung S5P MFC Video Codec"
-	depends on VIDEO_DEV && VIDEO_V4L2 && (PLAT_S5P || ARCH_EXYNOS)
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on PLAT_S5P || ARCH_EXYNOS || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	default n
 	help
@@ -185,7 +198,9 @@ config VIDEO_SAMSUNG_S5P_MFC
 
 config VIDEO_MX2_EMMAPRP
 	tristate "MX2 eMMa-PrP support"
-	depends on VIDEO_DEV && VIDEO_V4L2 && SOC_IMX27
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on SOC_IMX27 || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	help
@@ -195,7 +210,9 @@ config VIDEO_MX2_EMMAPRP
 
 config VIDEO_SAMSUNG_EXYNOS_GSC
 	tristate "Samsung Exynos G-Scaler driver"
-	depends on VIDEO_DEV && VIDEO_V4L2 && ARCH_EXYNOS5
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on ARCH_EXYNOS5 || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	help
@@ -204,6 +221,7 @@ config VIDEO_SAMSUNG_EXYNOS_GSC
 config VIDEO_SH_VEU
 	tristate "SuperH VEU mem2mem video processing driver"
 	depends on VIDEO_DEV && VIDEO_V4L2 && HAS_DMA
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	help
@@ -213,6 +231,7 @@ config VIDEO_SH_VEU
 config VIDEO_RENESAS_VSP1
 	tristate "Renesas VSP1 Video Processing Engine"
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API && HAS_DMA
+	depends on ARCH_SHMOBILE || COMPILE_TEST
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  This is a V4L2 driver for the Renesas VSP1 video processing engine.
@@ -222,7 +241,9 @@ config VIDEO_RENESAS_VSP1
 
 config VIDEO_TI_VPE
 	tristate "TI VPE (Video Processing Engine) driver"
-	depends on VIDEO_DEV && VIDEO_V4L2 && SOC_DRA7XX
+	depends on VIDEO_DEV && VIDEO_V4L2
+	depends on SOC_DRA7XX || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	default n
@@ -243,19 +264,8 @@ menuconfig V4L_TEST_DRIVERS
 	depends on MEDIA_CAMERA_SUPPORT
 
 if V4L_TEST_DRIVERS
-config VIDEO_VIVI
-	tristate "Virtual Video Driver"
-	depends on VIDEO_DEV && VIDEO_V4L2 && !SPARC32 && !SPARC64
-	select FONT_SUPPORT
-	select FONT_8x16
-	select VIDEOBUF2_VMALLOC
-	default n
-	---help---
-	  Enables a virtual video driver. This device shows a color bar
-	  and a timestamp, as a real device would generate by using V4L2
-	  api.
-	  Say Y here if you want to test video apps or debug V4L devices.
-	  In doubt, say N.
+
+source "drivers/media/platform/vivid/Kconfig"
 
 config VIDEO_MEM2MEM_TESTDEV
 	tristate "Virtual test device for mem2mem framework"
diff --git a/drivers/media/platform/Makefile b/drivers/media/platform/Makefile
index e5269da91906..579046bc276f 100644
--- a/drivers/media/platform/Makefile
+++ b/drivers/media/platform/Makefile
@@ -15,14 +15,14 @@ obj-$(CONFIG_VIDEO_MMP_CAMERA) += marvell-ccic/
 obj-$(CONFIG_VIDEO_OMAP3)	+= omap3isp/
 
 obj-$(CONFIG_VIDEO_VIU) += fsl-viu.o
-obj-$(CONFIG_VIDEO_VIVI) += vivi.o
 
+obj-$(CONFIG_VIDEO_VIVID)		+= vivid/
 obj-$(CONFIG_VIDEO_MEM2MEM_TESTDEV) += mem2mem_testdev.o
 
 obj-$(CONFIG_VIDEO_TI_VPE)		+= ti-vpe/
 
 obj-$(CONFIG_VIDEO_MX2_EMMAPRP)		+= mx2_emmaprp.o
-obj-$(CONFIG_VIDEO_CODA) 		+= coda.o
+obj-$(CONFIG_VIDEO_CODA) 		+= coda/
 
 obj-$(CONFIG_VIDEO_SH_VEU)		+= sh_veu.o
 
@@ -47,8 +47,6 @@ obj-$(CONFIG_SOC_CAMERA)		+= soc_camera/
 
 obj-$(CONFIG_VIDEO_RENESAS_VSP1)	+= vsp1/
 
-obj-y	+= davinci/
-
-obj-$(CONFIG_ARCH_OMAP)	+= omap/
+obj-y	+= omap/
 
 ccflags-y += -I$(srctree)/drivers/media/i2c
diff --git a/drivers/media/platform/blackfin/Kconfig b/drivers/media/platform/blackfin/Kconfig
index cc239972fa2c..68fa90151b8f 100644
--- a/drivers/media/platform/blackfin/Kconfig
+++ b/drivers/media/platform/blackfin/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_BLACKFIN_CAPTURE
 	tristate "Blackfin Video Capture Driver"
 	depends on VIDEO_V4L2 && BLACKFIN && I2C
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	  V4L2 bridge driver for Blackfin video capture device.
diff --git a/drivers/media/platform/coda.c b/drivers/media/platform/coda.c
deleted file mode 100644
index 3a6d1d2b429e..000000000000
--- a/drivers/media/platform/coda.c
+++ /dev/null
@@ -1,3933 +0,0 @@
-/*
- * Coda multi-standard codec IP
- *
- * Copyright (C) 2012 Vista Silicon S.L.
- *    Javier Martin, <javier.martin@vista-silicon.com>
- *    Xavier Duret
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- */
-
-#include <linux/clk.h>
-#include <linux/debugfs.h>
-#include <linux/delay.h>
-#include <linux/firmware.h>
-#include <linux/genalloc.h>
-#include <linux/interrupt.h>
-#include <linux/io.h>
-#include <linux/irq.h>
-#include <linux/kfifo.h>
-#include <linux/module.h>
-#include <linux/of_device.h>
-#include <linux/platform_device.h>
-#include <linux/pm_runtime.h>
-#include <linux/slab.h>
-#include <linux/videodev2.h>
-#include <linux/of.h>
-#include <linux/platform_data/coda.h>
-#include <linux/reset.h>
-
-#include <media/v4l2-ctrls.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-event.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-mem2mem.h>
-#include <media/videobuf2-core.h>
-#include <media/videobuf2-dma-contig.h>
-
-#include "coda.h"
-
-#define CODA_NAME		"coda"
-
-#define CODADX6_MAX_INSTANCES	4
-
-#define CODA_PARA_BUF_SIZE	(10 * 1024)
-#define CODA_ISRAM_SIZE	(2048 * 2)
-
-#define CODA7_PS_BUF_SIZE	0x28000
-#define CODA9_PS_SAVE_SIZE	(512 * 1024)
-
-#define CODA_MAX_FRAMEBUFFERS	8
-
-#define CODA_MAX_FRAME_SIZE	0x100000
-#define FMO_SLICE_SAVE_BUF_SIZE         (32)
-#define CODA_DEFAULT_GAMMA		4096
-#define CODA9_DEFAULT_GAMMA		24576	/* 0.75 * 32768 */
-
-#define MIN_W 176
-#define MIN_H 144
-
-#define S_ALIGN		1 /* multiple of 2 */
-#define W_ALIGN		1 /* multiple of 2 */
-#define H_ALIGN		1 /* multiple of 2 */
-
-#define fh_to_ctx(__fh)	container_of(__fh, struct coda_ctx, fh)
-
-static int coda_debug;
-module_param(coda_debug, int, 0644);
-MODULE_PARM_DESC(coda_debug, "Debug level (0-1)");
-
-enum {
-	V4L2_M2M_SRC = 0,
-	V4L2_M2M_DST = 1,
-};
-
-enum coda_inst_type {
-	CODA_INST_ENCODER,
-	CODA_INST_DECODER,
-};
-
-enum coda_product {
-	CODA_DX6 = 0xf001,
-	CODA_7541 = 0xf012,
-	CODA_960 = 0xf020,
-};
-
-struct coda_fmt {
-	char *name;
-	u32 fourcc;
-};
-
-struct coda_codec {
-	u32 mode;
-	u32 src_fourcc;
-	u32 dst_fourcc;
-	u32 max_w;
-	u32 max_h;
-};
-
-struct coda_devtype {
-	char			*firmware;
-	enum coda_product	product;
-	struct coda_codec	*codecs;
-	unsigned int		num_codecs;
-	size_t			workbuf_size;
-	size_t			tempbuf_size;
-	size_t			iram_size;
-};
-
-/* Per-queue, driver-specific private data */
-struct coda_q_data {
-	unsigned int		width;
-	unsigned int		height;
-	unsigned int		bytesperline;
-	unsigned int		sizeimage;
-	unsigned int		fourcc;
-	struct v4l2_rect	rect;
-};
-
-struct coda_aux_buf {
-	void			*vaddr;
-	dma_addr_t		paddr;
-	u32			size;
-	struct debugfs_blob_wrapper blob;
-	struct dentry		*dentry;
-};
-
-struct coda_dev {
-	struct v4l2_device	v4l2_dev;
-	struct video_device	vfd;
-	struct platform_device	*plat_dev;
-	const struct coda_devtype *devtype;
-
-	void __iomem		*regs_base;
-	struct clk		*clk_per;
-	struct clk		*clk_ahb;
-	struct reset_control	*rstc;
-
-	struct coda_aux_buf	codebuf;
-	struct coda_aux_buf	tempbuf;
-	struct coda_aux_buf	workbuf;
-	struct gen_pool		*iram_pool;
-	struct coda_aux_buf	iram;
-
-	spinlock_t		irqlock;
-	struct mutex		dev_mutex;
-	struct mutex		coda_mutex;
-	struct workqueue_struct	*workqueue;
-	struct v4l2_m2m_dev	*m2m_dev;
-	struct vb2_alloc_ctx	*alloc_ctx;
-	struct list_head	instances;
-	unsigned long		instance_mask;
-	struct dentry		*debugfs_root;
-};
-
-struct coda_params {
-	u8			rot_mode;
-	u8			h264_intra_qp;
-	u8			h264_inter_qp;
-	u8			h264_min_qp;
-	u8			h264_max_qp;
-	u8			h264_deblk_enabled;
-	u8			h264_deblk_alpha;
-	u8			h264_deblk_beta;
-	u8			mpeg4_intra_qp;
-	u8			mpeg4_inter_qp;
-	u8			gop_size;
-	int			intra_refresh;
-	int			codec_mode;
-	int			codec_mode_aux;
-	enum v4l2_mpeg_video_multi_slice_mode slice_mode;
-	u32			framerate;
-	u16			bitrate;
-	u32			slice_max_bits;
-	u32			slice_max_mb;
-};
-
-struct coda_iram_info {
-	u32		axi_sram_use;
-	phys_addr_t	buf_bit_use;
-	phys_addr_t	buf_ip_ac_dc_use;
-	phys_addr_t	buf_dbk_y_use;
-	phys_addr_t	buf_dbk_c_use;
-	phys_addr_t	buf_ovl_use;
-	phys_addr_t	buf_btp_use;
-	phys_addr_t	search_ram_paddr;
-	int		search_ram_size;
-	int		remaining;
-	phys_addr_t	next_paddr;
-};
-
-struct gdi_tiled_map {
-	int xy2ca_map[16];
-	int xy2ba_map[16];
-	int xy2ra_map[16];
-	int rbc2axi_map[32];
-	int xy2rbc_config;
-	int map_type;
-#define GDI_LINEAR_FRAME_MAP 0
-};
-
-struct coda_timestamp {
-	struct list_head	list;
-	u32			sequence;
-	struct v4l2_timecode	timecode;
-	struct timeval		timestamp;
-};
-
-struct coda_ctx {
-	struct coda_dev			*dev;
-	struct mutex			buffer_mutex;
-	struct list_head		list;
-	struct work_struct		pic_run_work;
-	struct work_struct		seq_end_work;
-	struct completion		completion;
-	int				aborting;
-	int				initialized;
-	int				streamon_out;
-	int				streamon_cap;
-	u32				isequence;
-	u32				qsequence;
-	u32				osequence;
-	u32				sequence_offset;
-	struct coda_q_data		q_data[2];
-	enum coda_inst_type		inst_type;
-	struct coda_codec		*codec;
-	enum v4l2_colorspace		colorspace;
-	struct coda_params		params;
-	struct v4l2_ctrl_handler	ctrls;
-	struct v4l2_fh			fh;
-	int				gopcounter;
-	int				runcounter;
-	char				vpu_header[3][64];
-	int				vpu_header_size[3];
-	struct kfifo			bitstream_fifo;
-	struct mutex			bitstream_mutex;
-	struct coda_aux_buf		bitstream;
-	bool				hold;
-	struct coda_aux_buf		parabuf;
-	struct coda_aux_buf		psbuf;
-	struct coda_aux_buf		slicebuf;
-	struct coda_aux_buf		internal_frames[CODA_MAX_FRAMEBUFFERS];
-	u32				frame_types[CODA_MAX_FRAMEBUFFERS];
-	struct coda_timestamp		frame_timestamps[CODA_MAX_FRAMEBUFFERS];
-	u32				frame_errors[CODA_MAX_FRAMEBUFFERS];
-	struct list_head		timestamp_list;
-	struct coda_aux_buf		workbuf;
-	int				num_internal_frames;
-	int				idx;
-	int				reg_idx;
-	struct coda_iram_info		iram_info;
-	struct gdi_tiled_map		tiled_map;
-	u32				bit_stream_param;
-	u32				frm_dis_flg;
-	u32				frame_mem_ctrl;
-	int				display_idx;
-	struct dentry			*debugfs_entry;
-};
-
-static const u8 coda_filler_nal[14] = { 0x00, 0x00, 0x00, 0x01, 0x0c, 0xff,
-			0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80 };
-static const u8 coda_filler_size[8] = { 0, 7, 14, 13, 12, 11, 10, 9 };
-
-static inline void coda_write(struct coda_dev *dev, u32 data, u32 reg)
-{
-	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-		 "%s: data=0x%x, reg=0x%x\n", __func__, data, reg);
-	writel(data, dev->regs_base + reg);
-}
-
-static inline unsigned int coda_read(struct coda_dev *dev, u32 reg)
-{
-	u32 data;
-	data = readl(dev->regs_base + reg);
-	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-		 "%s: data=0x%x, reg=0x%x\n", __func__, data, reg);
-	return data;
-}
-
-static inline unsigned long coda_isbusy(struct coda_dev *dev)
-{
-	return coda_read(dev, CODA_REG_BIT_BUSY);
-}
-
-static inline int coda_is_initialized(struct coda_dev *dev)
-{
-	return (coda_read(dev, CODA_REG_BIT_CUR_PC) != 0);
-}
-
-static int coda_wait_timeout(struct coda_dev *dev)
-{
-	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
-
-	while (coda_isbusy(dev)) {
-		if (time_after(jiffies, timeout))
-			return -ETIMEDOUT;
-	}
-	return 0;
-}
-
-static void coda_command_async(struct coda_ctx *ctx, int cmd)
-{
-	struct coda_dev *dev = ctx->dev;
-
-	if (dev->devtype->product == CODA_960 ||
-	    dev->devtype->product == CODA_7541) {
-		/* Restore context related registers to CODA */
-		coda_write(dev, ctx->bit_stream_param,
-				CODA_REG_BIT_BIT_STREAM_PARAM);
-		coda_write(dev, ctx->frm_dis_flg,
-				CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
-		coda_write(dev, ctx->frame_mem_ctrl,
-				CODA_REG_BIT_FRAME_MEM_CTRL);
-		coda_write(dev, ctx->workbuf.paddr, CODA_REG_BIT_WORK_BUF_ADDR);
-	}
-
-	if (dev->devtype->product == CODA_960) {
-		coda_write(dev, 1, CODA9_GDI_WPROT_ERR_CLR);
-		coda_write(dev, 0, CODA9_GDI_WPROT_RGN_EN);
-	}
-
-	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
-
-	coda_write(dev, ctx->idx, CODA_REG_BIT_RUN_INDEX);
-	coda_write(dev, ctx->params.codec_mode, CODA_REG_BIT_RUN_COD_STD);
-	coda_write(dev, ctx->params.codec_mode_aux, CODA7_REG_BIT_RUN_AUX_STD);
-
-	coda_write(dev, cmd, CODA_REG_BIT_RUN_COMMAND);
-}
-
-static int coda_command_sync(struct coda_ctx *ctx, int cmd)
-{
-	struct coda_dev *dev = ctx->dev;
-
-	coda_command_async(ctx, cmd);
-	return coda_wait_timeout(dev);
-}
-
-static int coda_hw_reset(struct coda_ctx *ctx)
-{
-	struct coda_dev *dev = ctx->dev;
-	unsigned long timeout;
-	unsigned int idx;
-	int ret;
-
-	if (!dev->rstc)
-		return -ENOENT;
-
-	idx = coda_read(dev, CODA_REG_BIT_RUN_INDEX);
-
-	timeout = jiffies + msecs_to_jiffies(100);
-	coda_write(dev, 0x11, CODA9_GDI_BUS_CTRL);
-	while (coda_read(dev, CODA9_GDI_BUS_STATUS) != 0x77) {
-		if (time_after(jiffies, timeout))
-			return -ETIME;
-		cpu_relax();
-	}
-
-	ret = reset_control_reset(dev->rstc);
-	if (ret < 0)
-		return ret;
-
-	coda_write(dev, 0x00, CODA9_GDI_BUS_CTRL);
-	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
-	coda_write(dev, CODA_REG_RUN_ENABLE, CODA_REG_BIT_CODE_RUN);
-	ret = coda_wait_timeout(dev);
-	coda_write(dev, idx, CODA_REG_BIT_RUN_INDEX);
-
-	return ret;
-}
-
-static struct coda_q_data *get_q_data(struct coda_ctx *ctx,
-					 enum v4l2_buf_type type)
-{
-	switch (type) {
-	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
-		return &(ctx->q_data[V4L2_M2M_SRC]);
-	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
-		return &(ctx->q_data[V4L2_M2M_DST]);
-	default:
-		return NULL;
-	}
-}
-
-/*
- * Array of all formats supported by any version of Coda:
- */
-static struct coda_fmt coda_formats[] = {
-	{
-		.name = "YUV 4:2:0 Planar, YCbCr",
-		.fourcc = V4L2_PIX_FMT_YUV420,
-	},
-	{
-		.name = "YUV 4:2:0 Planar, YCrCb",
-		.fourcc = V4L2_PIX_FMT_YVU420,
-	},
-	{
-		.name = "H264 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_H264,
-	},
-	{
-		.name = "MPEG4 Encoded Stream",
-		.fourcc = V4L2_PIX_FMT_MPEG4,
-	},
-};
-
-#define CODA_CODEC(mode, src_fourcc, dst_fourcc, max_w, max_h) \
-	{ mode, src_fourcc, dst_fourcc, max_w, max_h }
-
-/*
- * Arrays of codecs supported by each given version of Coda:
- *  i.MX27 -> codadx6
- *  i.MX5x -> coda7
- *  i.MX6  -> coda960
- * Use V4L2_PIX_FMT_YUV420 as placeholder for all supported YUV 4:2:0 variants
- */
-static struct coda_codec codadx6_codecs[] = {
-	CODA_CODEC(CODADX6_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,  720, 576),
-	CODA_CODEC(CODADX6_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4, 720, 576),
-};
-
-static struct coda_codec coda7_codecs[] = {
-	CODA_CODEC(CODA7_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,   1280, 720),
-	CODA_CODEC(CODA7_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1280, 720),
-	CODA_CODEC(CODA7_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1080),
-	CODA_CODEC(CODA7_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1080),
-};
-
-static struct coda_codec coda9_codecs[] = {
-	CODA_CODEC(CODA9_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,   1920, 1080),
-	CODA_CODEC(CODA9_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1920, 1080),
-	CODA_CODEC(CODA9_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1080),
-	CODA_CODEC(CODA9_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1080),
-};
-
-static bool coda_format_is_yuv(u32 fourcc)
-{
-	switch (fourcc) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-		return true;
-	default:
-		return false;
-	}
-}
-
-/*
- * Normalize all supported YUV 4:2:0 formats to the value used in the codec
- * tables.
- */
-static u32 coda_format_normalize_yuv(u32 fourcc)
-{
-	return coda_format_is_yuv(fourcc) ? V4L2_PIX_FMT_YUV420 : fourcc;
-}
-
-static struct coda_codec *coda_find_codec(struct coda_dev *dev, int src_fourcc,
-					  int dst_fourcc)
-{
-	struct coda_codec *codecs = dev->devtype->codecs;
-	int num_codecs = dev->devtype->num_codecs;
-	int k;
-
-	src_fourcc = coda_format_normalize_yuv(src_fourcc);
-	dst_fourcc = coda_format_normalize_yuv(dst_fourcc);
-	if (src_fourcc == dst_fourcc)
-		return NULL;
-
-	for (k = 0; k < num_codecs; k++) {
-		if (codecs[k].src_fourcc == src_fourcc &&
-		    codecs[k].dst_fourcc == dst_fourcc)
-			break;
-	}
-
-	if (k == num_codecs)
-		return NULL;
-
-	return &codecs[k];
-}
-
-static void coda_get_max_dimensions(struct coda_dev *dev,
-				    struct coda_codec *codec,
-				    int *max_w, int *max_h)
-{
-	struct coda_codec *codecs = dev->devtype->codecs;
-	int num_codecs = dev->devtype->num_codecs;
-	unsigned int w, h;
-	int k;
-
-	if (codec) {
-		w = codec->max_w;
-		h = codec->max_h;
-	} else {
-		for (k = 0, w = 0, h = 0; k < num_codecs; k++) {
-			w = max(w, codecs[k].max_w);
-			h = max(h, codecs[k].max_h);
-		}
-	}
-
-	if (max_w)
-		*max_w = w;
-	if (max_h)
-		*max_h = h;
-}
-
-static char *coda_product_name(int product)
-{
-	static char buf[9];
-
-	switch (product) {
-	case CODA_DX6:
-		return "CodaDx6";
-	case CODA_7541:
-		return "CODA7541";
-	case CODA_960:
-		return "CODA960";
-	default:
-		snprintf(buf, sizeof(buf), "(0x%04x)", product);
-		return buf;
-	}
-}
-
-/*
- * V4L2 ioctl() operations.
- */
-static int coda_querycap(struct file *file, void *priv,
-			 struct v4l2_capability *cap)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-
-	strlcpy(cap->driver, CODA_NAME, sizeof(cap->driver));
-	strlcpy(cap->card, coda_product_name(ctx->dev->devtype->product),
-		sizeof(cap->card));
-	strlcpy(cap->bus_info, "platform:" CODA_NAME, sizeof(cap->bus_info));
-	/*
-	 * This is only a mem-to-mem video device. The capture and output
-	 * device capability flags are left only for backward compatibility
-	 * and are scheduled for removal.
-	 */
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OUTPUT |
-			   V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-
-	return 0;
-}
-
-static int enum_fmt(void *priv, struct v4l2_fmtdesc *f,
-			enum v4l2_buf_type type, int src_fourcc)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	struct coda_codec *codecs = ctx->dev->devtype->codecs;
-	struct coda_fmt *formats = coda_formats;
-	struct coda_fmt *fmt;
-	int num_codecs = ctx->dev->devtype->num_codecs;
-	int num_formats = ARRAY_SIZE(coda_formats);
-	int i, k, num = 0;
-
-	for (i = 0; i < num_formats; i++) {
-		/* Both uncompressed formats are always supported */
-		if (coda_format_is_yuv(formats[i].fourcc) &&
-		    !coda_format_is_yuv(src_fourcc)) {
-			if (num == f->index)
-				break;
-			++num;
-			continue;
-		}
-		/* Compressed formats may be supported, check the codec list */
-		for (k = 0; k < num_codecs; k++) {
-			/* if src_fourcc is set, only consider matching codecs */
-			if (type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-			    formats[i].fourcc == codecs[k].dst_fourcc &&
-			    (!src_fourcc || src_fourcc == codecs[k].src_fourcc))
-				break;
-			if (type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
-			    formats[i].fourcc == codecs[k].src_fourcc)
-				break;
-		}
-		if (k < num_codecs) {
-			if (num == f->index)
-				break;
-			++num;
-		}
-	}
-
-	if (i < num_formats) {
-		fmt = &formats[i];
-		strlcpy(f->description, fmt->name, sizeof(f->description));
-		f->pixelformat = fmt->fourcc;
-		if (!coda_format_is_yuv(fmt->fourcc))
-			f->flags |= V4L2_FMT_FLAG_COMPRESSED;
-		return 0;
-	}
-
-	/* Format not found */
-	return -EINVAL;
-}
-
-static int coda_enum_fmt_vid_cap(struct file *file, void *priv,
-				 struct v4l2_fmtdesc *f)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	struct vb2_queue *src_vq;
-	struct coda_q_data *q_data_src;
-
-	/* If the source format is already fixed, only list matching formats */
-	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	if (vb2_is_streaming(src_vq)) {
-		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-
-		return enum_fmt(priv, f, V4L2_BUF_TYPE_VIDEO_CAPTURE,
-				q_data_src->fourcc);
-	}
-
-	return enum_fmt(priv, f, V4L2_BUF_TYPE_VIDEO_CAPTURE, 0);
-}
-
-static int coda_enum_fmt_vid_out(struct file *file, void *priv,
-				 struct v4l2_fmtdesc *f)
-{
-	return enum_fmt(priv, f, V4L2_BUF_TYPE_VIDEO_OUTPUT, 0);
-}
-
-static int coda_g_fmt(struct file *file, void *priv,
-		      struct v4l2_format *f)
-{
-	struct coda_q_data *q_data;
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-
-	q_data = get_q_data(ctx, f->type);
-	if (!q_data)
-		return -EINVAL;
-
-	f->fmt.pix.field	= V4L2_FIELD_NONE;
-	f->fmt.pix.pixelformat	= q_data->fourcc;
-	f->fmt.pix.width	= q_data->width;
-	f->fmt.pix.height	= q_data->height;
-	f->fmt.pix.bytesperline = q_data->bytesperline;
-
-	f->fmt.pix.sizeimage	= q_data->sizeimage;
-	f->fmt.pix.colorspace	= ctx->colorspace;
-
-	return 0;
-}
-
-static int coda_try_fmt(struct coda_ctx *ctx, struct coda_codec *codec,
-			struct v4l2_format *f)
-{
-	struct coda_dev *dev = ctx->dev;
-	struct coda_q_data *q_data;
-	unsigned int max_w, max_h;
-	enum v4l2_field field;
-
-	field = f->fmt.pix.field;
-	if (field == V4L2_FIELD_ANY)
-		field = V4L2_FIELD_NONE;
-	else if (V4L2_FIELD_NONE != field)
-		return -EINVAL;
-
-	/* V4L2 specification suggests the driver corrects the format struct
-	 * if any of the dimensions is unsupported */
-	f->fmt.pix.field = field;
-
-	coda_get_max_dimensions(dev, codec, &max_w, &max_h);
-	v4l_bound_align_image(&f->fmt.pix.width, MIN_W, max_w, W_ALIGN,
-			      &f->fmt.pix.height, MIN_H, max_h, H_ALIGN,
-			      S_ALIGN);
-
-	switch (f->fmt.pix.pixelformat) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-	case V4L2_PIX_FMT_H264:
-	case V4L2_PIX_FMT_MPEG4:
-	case V4L2_PIX_FMT_JPEG:
-		break;
-	default:
-		q_data = get_q_data(ctx, f->type);
-		if (!q_data)
-			return -EINVAL;
-		f->fmt.pix.pixelformat = q_data->fourcc;
-	}
-
-	switch (f->fmt.pix.pixelformat) {
-	case V4L2_PIX_FMT_YUV420:
-	case V4L2_PIX_FMT_YVU420:
-		/* Frame stride must be multiple of 8, but 16 for h.264 */
-		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
-		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
-					f->fmt.pix.height * 3 / 2;
-		break;
-	case V4L2_PIX_FMT_H264:
-	case V4L2_PIX_FMT_MPEG4:
-	case V4L2_PIX_FMT_JPEG:
-		f->fmt.pix.bytesperline = 0;
-		f->fmt.pix.sizeimage = CODA_MAX_FRAME_SIZE;
-		break;
-	default:
-		BUG();
-	}
-
-	return 0;
-}
-
-static int coda_try_fmt_vid_cap(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	struct coda_codec *codec;
-	struct vb2_queue *src_vq;
-	int ret;
-
-	/*
-	 * If the source format is already fixed, try to find a codec that
-	 * converts to the given destination format
-	 */
-	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	if (vb2_is_streaming(src_vq)) {
-		struct coda_q_data *q_data_src;
-
-		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-		codec = coda_find_codec(ctx->dev, q_data_src->fourcc,
-					f->fmt.pix.pixelformat);
-		if (!codec)
-			return -EINVAL;
-	} else {
-		/* Otherwise determine codec by encoded format, if possible */
-		codec = coda_find_codec(ctx->dev, V4L2_PIX_FMT_YUV420,
-					f->fmt.pix.pixelformat);
-	}
-
-	f->fmt.pix.colorspace = ctx->colorspace;
-
-	ret = coda_try_fmt(ctx, codec, f);
-	if (ret < 0)
-		return ret;
-
-	/* The h.264 decoder only returns complete 16x16 macroblocks */
-	if (codec && codec->src_fourcc == V4L2_PIX_FMT_H264) {
-		f->fmt.pix.width = f->fmt.pix.width;
-		f->fmt.pix.height = round_up(f->fmt.pix.height, 16);
-		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
-		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
-				       f->fmt.pix.height * 3 / 2;
-	}
-
-	return 0;
-}
-
-static int coda_try_fmt_vid_out(struct file *file, void *priv,
-				struct v4l2_format *f)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	struct coda_codec *codec;
-
-	/* Determine codec by encoded format, returns NULL if raw or invalid */
-	codec = coda_find_codec(ctx->dev, f->fmt.pix.pixelformat,
-				V4L2_PIX_FMT_YUV420);
-
-	if (!f->fmt.pix.colorspace)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
-
-	return coda_try_fmt(ctx, codec, f);
-}
-
-static int coda_s_fmt(struct coda_ctx *ctx, struct v4l2_format *f)
-{
-	struct coda_q_data *q_data;
-	struct vb2_queue *vq;
-
-	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
-	if (!vq)
-		return -EINVAL;
-
-	q_data = get_q_data(ctx, f->type);
-	if (!q_data)
-		return -EINVAL;
-
-	if (vb2_is_busy(vq)) {
-		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
-		return -EBUSY;
-	}
-
-	q_data->fourcc = f->fmt.pix.pixelformat;
-	q_data->width = f->fmt.pix.width;
-	q_data->height = f->fmt.pix.height;
-	q_data->bytesperline = f->fmt.pix.bytesperline;
-	q_data->sizeimage = f->fmt.pix.sizeimage;
-	q_data->rect.left = 0;
-	q_data->rect.top = 0;
-	q_data->rect.width = f->fmt.pix.width;
-	q_data->rect.height = f->fmt.pix.height;
-
-	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-		"Setting format for type %d, wxh: %dx%d, fmt: %d\n",
-		f->type, q_data->width, q_data->height, q_data->fourcc);
-
-	return 0;
-}
-
-static int coda_s_fmt_vid_cap(struct file *file, void *priv,
-			      struct v4l2_format *f)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	int ret;
-
-	ret = coda_try_fmt_vid_cap(file, priv, f);
-	if (ret)
-		return ret;
-
-	return coda_s_fmt(ctx, f);
-}
-
-static int coda_s_fmt_vid_out(struct file *file, void *priv,
-			      struct v4l2_format *f)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	int ret;
-
-	ret = coda_try_fmt_vid_out(file, priv, f);
-	if (ret)
-		return ret;
-
-	ret = coda_s_fmt(ctx, f);
-	if (ret)
-		ctx->colorspace = f->fmt.pix.colorspace;
-
-	return ret;
-}
-
-static int coda_qbuf(struct file *file, void *priv,
-		     struct v4l2_buffer *buf)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-
-	return v4l2_m2m_qbuf(file, ctx->fh.m2m_ctx, buf);
-}
-
-static bool coda_buf_is_end_of_stream(struct coda_ctx *ctx,
-				      struct v4l2_buffer *buf)
-{
-	struct vb2_queue *src_vq;
-
-	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-
-	return ((ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) &&
-		(buf->sequence == (ctx->qsequence - 1)));
-}
-
-static int coda_dqbuf(struct file *file, void *priv,
-		      struct v4l2_buffer *buf)
-{
-	struct coda_ctx *ctx = fh_to_ctx(priv);
-	int ret;
-
-	ret = v4l2_m2m_dqbuf(file, ctx->fh.m2m_ctx, buf);
-
-	/* If this is the last capture buffer, emit an end-of-stream event */
-	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
-	    coda_buf_is_end_of_stream(ctx, buf)) {
-		const struct v4l2_event eos_event = {
-			.type = V4L2_EVENT_EOS
-		};
-
-		v4l2_event_queue_fh(&ctx->fh, &eos_event);
-	}
-
-	return ret;
-}
-
-static int coda_g_selection(struct file *file, void *fh,
-			    struct v4l2_selection *s)
-{
-	struct coda_ctx *ctx = fh_to_ctx(fh);
-	struct coda_q_data *q_data;
-	struct v4l2_rect r, *rsel;
-
-	q_data = get_q_data(ctx, s->type);
-	if (!q_data)
-		return -EINVAL;
-
-	r.left = 0;
-	r.top = 0;
-	r.width = q_data->width;
-	r.height = q_data->height;
-	rsel = &q_data->rect;
-
-	switch (s->target) {
-	case V4L2_SEL_TGT_CROP_DEFAULT:
-	case V4L2_SEL_TGT_CROP_BOUNDS:
-		rsel = &r;
-		/* fallthrough */
-	case V4L2_SEL_TGT_CROP:
-		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
-			return -EINVAL;
-		break;
-	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
-	case V4L2_SEL_TGT_COMPOSE_PADDED:
-		rsel = &r;
-		/* fallthrough */
-	case V4L2_SEL_TGT_COMPOSE:
-	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
-		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-			return -EINVAL;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	s->r = *rsel;
-
-	return 0;
-}
-
-static int coda_try_decoder_cmd(struct file *file, void *fh,
-				struct v4l2_decoder_cmd *dc)
-{
-	if (dc->cmd != V4L2_DEC_CMD_STOP)
-		return -EINVAL;
-
-	if (dc->flags & V4L2_DEC_CMD_STOP_TO_BLACK)
-		return -EINVAL;
-
-	if (!(dc->flags & V4L2_DEC_CMD_STOP_IMMEDIATELY) && (dc->stop.pts != 0))
-		return -EINVAL;
-
-	return 0;
-}
-
-static int coda_decoder_cmd(struct file *file, void *fh,
-			    struct v4l2_decoder_cmd *dc)
-{
-	struct coda_ctx *ctx = fh_to_ctx(fh);
-	struct coda_dev *dev = ctx->dev;
-	int ret;
-
-	ret = coda_try_decoder_cmd(file, fh, dc);
-	if (ret < 0)
-		return ret;
-
-	/* Ignore decoder stop command silently in encoder context */
-	if (ctx->inst_type != CODA_INST_DECODER)
-		return 0;
-
-	/* Set the strem-end flag on this context */
-	ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
-
-	if ((dev->devtype->product == CODA_960) &&
-	    coda_isbusy(dev) &&
-	    (ctx->idx == coda_read(dev, CODA_REG_BIT_RUN_INDEX))) {
-		/* If this context is currently running, update the hardware flag */
-		coda_write(dev, ctx->bit_stream_param, CODA_REG_BIT_BIT_STREAM_PARAM);
-	}
-	ctx->hold = false;
-	v4l2_m2m_try_schedule(ctx->fh.m2m_ctx);
-
-	return 0;
-}
-
-static int coda_subscribe_event(struct v4l2_fh *fh,
-				const struct v4l2_event_subscription *sub)
-{
-	switch (sub->type) {
-	case V4L2_EVENT_EOS:
-		return v4l2_event_subscribe(fh, sub, 0, NULL);
-	default:
-		return v4l2_ctrl_subscribe_event(fh, sub);
-	}
-}
-
-static const struct v4l2_ioctl_ops coda_ioctl_ops = {
-	.vidioc_querycap	= coda_querycap,
-
-	.vidioc_enum_fmt_vid_cap = coda_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap	= coda_g_fmt,
-	.vidioc_try_fmt_vid_cap	= coda_try_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap	= coda_s_fmt_vid_cap,
-
-	.vidioc_enum_fmt_vid_out = coda_enum_fmt_vid_out,
-	.vidioc_g_fmt_vid_out	= coda_g_fmt,
-	.vidioc_try_fmt_vid_out	= coda_try_fmt_vid_out,
-	.vidioc_s_fmt_vid_out	= coda_s_fmt_vid_out,
-
-	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
-	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
-
-	.vidioc_qbuf		= coda_qbuf,
-	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
-	.vidioc_dqbuf		= coda_dqbuf,
-	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
-
-	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
-	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
-
-	.vidioc_g_selection	= coda_g_selection,
-
-	.vidioc_try_decoder_cmd	= coda_try_decoder_cmd,
-	.vidioc_decoder_cmd	= coda_decoder_cmd,
-
-	.vidioc_subscribe_event = coda_subscribe_event,
-	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
-};
-
-static int coda_start_decoding(struct coda_ctx *ctx);
-
-static inline int coda_get_bitstream_payload(struct coda_ctx *ctx)
-{
-	return kfifo_len(&ctx->bitstream_fifo);
-}
-
-static void coda_kfifo_sync_from_device(struct coda_ctx *ctx)
-{
-	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
-	struct coda_dev *dev = ctx->dev;
-	u32 rd_ptr;
-
-	rd_ptr = coda_read(dev, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
-	kfifo->out = (kfifo->in & ~kfifo->mask) |
-		      (rd_ptr - ctx->bitstream.paddr);
-	if (kfifo->out > kfifo->in)
-		kfifo->out -= kfifo->mask + 1;
-}
-
-static void coda_kfifo_sync_to_device_full(struct coda_ctx *ctx)
-{
-	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
-	struct coda_dev *dev = ctx->dev;
-	u32 rd_ptr, wr_ptr;
-
-	rd_ptr = ctx->bitstream.paddr + (kfifo->out & kfifo->mask);
-	coda_write(dev, rd_ptr, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
-	wr_ptr = ctx->bitstream.paddr + (kfifo->in & kfifo->mask);
-	coda_write(dev, wr_ptr, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
-}
-
-static void coda_kfifo_sync_to_device_write(struct coda_ctx *ctx)
-{
-	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
-	struct coda_dev *dev = ctx->dev;
-	u32 wr_ptr;
-
-	wr_ptr = ctx->bitstream.paddr + (kfifo->in & kfifo->mask);
-	coda_write(dev, wr_ptr, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
-}
-
-static int coda_bitstream_queue(struct coda_ctx *ctx, struct vb2_buffer *src_buf)
-{
-	u32 src_size = vb2_get_plane_payload(src_buf, 0);
-	u32 n;
-
-	n = kfifo_in(&ctx->bitstream_fifo, vb2_plane_vaddr(src_buf, 0), src_size);
-	if (n < src_size)
-		return -ENOSPC;
-
-	dma_sync_single_for_device(&ctx->dev->plat_dev->dev, ctx->bitstream.paddr,
-				   ctx->bitstream.size, DMA_TO_DEVICE);
-
-	src_buf->v4l2_buf.sequence = ctx->qsequence++;
-
-	return 0;
-}
-
-static bool coda_bitstream_try_queue(struct coda_ctx *ctx,
-				     struct vb2_buffer *src_buf)
-{
-	int ret;
-
-	if (coda_get_bitstream_payload(ctx) +
-	    vb2_get_plane_payload(src_buf, 0) + 512 >= ctx->bitstream.size)
-		return false;
-
-	if (vb2_plane_vaddr(src_buf, 0) == NULL) {
-		v4l2_err(&ctx->dev->v4l2_dev, "trying to queue empty buffer\n");
-		return true;
-	}
-
-	ret = coda_bitstream_queue(ctx, src_buf);
-	if (ret < 0) {
-		v4l2_err(&ctx->dev->v4l2_dev, "bitstream buffer overflow\n");
-		return false;
-	}
-	/* Sync read pointer to device */
-	if (ctx == v4l2_m2m_get_curr_priv(ctx->dev->m2m_dev))
-		coda_kfifo_sync_to_device_write(ctx);
-
-	ctx->hold = false;
-
-	return true;
-}
-
-static void coda_fill_bitstream(struct coda_ctx *ctx)
-{
-	struct vb2_buffer *src_buf;
-	struct coda_timestamp *ts;
-
-	while (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) > 0) {
-		src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
-
-		if (coda_bitstream_try_queue(ctx, src_buf)) {
-			/*
-			 * Source buffer is queued in the bitstream ringbuffer;
-			 * queue the timestamp and mark source buffer as done
-			 */
-			src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
-
-			ts = kmalloc(sizeof(*ts), GFP_KERNEL);
-			if (ts) {
-				ts->sequence = src_buf->v4l2_buf.sequence;
-				ts->timecode = src_buf->v4l2_buf.timecode;
-				ts->timestamp = src_buf->v4l2_buf.timestamp;
-				list_add_tail(&ts->list, &ctx->timestamp_list);
-			}
-
-			v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
-		} else {
-			break;
-		}
-	}
-}
-
-static void coda_set_gdi_regs(struct coda_ctx *ctx)
-{
-	struct gdi_tiled_map *tiled_map = &ctx->tiled_map;
-	struct coda_dev *dev = ctx->dev;
-	int i;
-
-	for (i = 0; i < 16; i++)
-		coda_write(dev, tiled_map->xy2ca_map[i],
-				CODA9_GDI_XY2_CAS_0 + 4 * i);
-	for (i = 0; i < 4; i++)
-		coda_write(dev, tiled_map->xy2ba_map[i],
-				CODA9_GDI_XY2_BA_0 + 4 * i);
-	for (i = 0; i < 16; i++)
-		coda_write(dev, tiled_map->xy2ra_map[i],
-				CODA9_GDI_XY2_RAS_0 + 4 * i);
-	coda_write(dev, tiled_map->xy2rbc_config, CODA9_GDI_XY2_RBC_CONFIG);
-	for (i = 0; i < 32; i++)
-		coda_write(dev, tiled_map->rbc2axi_map[i],
-				CODA9_GDI_RBC2_AXI_0 + 4 * i);
-}
-
-/*
- * Mem-to-mem operations.
- */
-static int coda_prepare_decode(struct coda_ctx *ctx)
-{
-	struct vb2_buffer *dst_buf;
-	struct coda_dev *dev = ctx->dev;
-	struct coda_q_data *q_data_dst;
-	u32 stridey, height;
-	u32 picture_y, picture_cb, picture_cr;
-
-	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
-	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-
-	if (ctx->params.rot_mode & CODA_ROT_90) {
-		stridey = q_data_dst->height;
-		height = q_data_dst->width;
-	} else {
-		stridey = q_data_dst->width;
-		height = q_data_dst->height;
-	}
-
-	/* Try to copy source buffer contents into the bitstream ringbuffer */
-	mutex_lock(&ctx->bitstream_mutex);
-	coda_fill_bitstream(ctx);
-	mutex_unlock(&ctx->bitstream_mutex);
-
-	if (coda_get_bitstream_payload(ctx) < 512 &&
-	    (!(ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG))) {
-		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-			 "bitstream payload: %d, skipping\n",
-			 coda_get_bitstream_payload(ctx));
-		v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
-		return -EAGAIN;
-	}
-
-	/* Run coda_start_decoding (again) if not yet initialized */
-	if (!ctx->initialized) {
-		int ret = coda_start_decoding(ctx);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "failed to start decoding\n");
-			v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
-			return -EAGAIN;
-		} else {
-			ctx->initialized = 1;
-		}
-	}
-
-	if (dev->devtype->product == CODA_960)
-		coda_set_gdi_regs(ctx);
-
-	/* Set rotator output */
-	picture_y = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
-	if (q_data_dst->fourcc == V4L2_PIX_FMT_YVU420) {
-		/* Switch Cr and Cb for YVU420 format */
-		picture_cr = picture_y + stridey * height;
-		picture_cb = picture_cr + stridey / 2 * height / 2;
-	} else {
-		picture_cb = picture_y + stridey * height;
-		picture_cr = picture_cb + stridey / 2 * height / 2;
-	}
-
-	if (dev->devtype->product == CODA_960) {
-		/*
-		 * The CODA960 seems to have an internal list of buffers with
-		 * 64 entries that includes the registered frame buffers as
-		 * well as the rotator buffer output.
-		 * ROT_INDEX needs to be < 0x40, but > ctx->num_internal_frames.
-		 */
-		coda_write(dev, CODA_MAX_FRAMEBUFFERS + dst_buf->v4l2_buf.index,
-				CODA9_CMD_DEC_PIC_ROT_INDEX);
-		coda_write(dev, picture_y, CODA9_CMD_DEC_PIC_ROT_ADDR_Y);
-		coda_write(dev, picture_cb, CODA9_CMD_DEC_PIC_ROT_ADDR_CB);
-		coda_write(dev, picture_cr, CODA9_CMD_DEC_PIC_ROT_ADDR_CR);
-		coda_write(dev, stridey, CODA9_CMD_DEC_PIC_ROT_STRIDE);
-	} else {
-		coda_write(dev, picture_y, CODA_CMD_DEC_PIC_ROT_ADDR_Y);
-		coda_write(dev, picture_cb, CODA_CMD_DEC_PIC_ROT_ADDR_CB);
-		coda_write(dev, picture_cr, CODA_CMD_DEC_PIC_ROT_ADDR_CR);
-		coda_write(dev, stridey, CODA_CMD_DEC_PIC_ROT_STRIDE);
-	}
-	coda_write(dev, CODA_ROT_MIR_ENABLE | ctx->params.rot_mode,
-			CODA_CMD_DEC_PIC_ROT_MODE);
-
-	switch (dev->devtype->product) {
-	case CODA_DX6:
-		/* TBD */
-	case CODA_7541:
-		coda_write(dev, CODA_PRE_SCAN_EN, CODA_CMD_DEC_PIC_OPTION);
-		break;
-	case CODA_960:
-		coda_write(dev, (1 << 10), CODA_CMD_DEC_PIC_OPTION); /* 'hardcode to use interrupt disable mode'? */
-		break;
-	}
-
-	coda_write(dev, 0, CODA_CMD_DEC_PIC_SKIP_NUM);
-
-	coda_write(dev, 0, CODA_CMD_DEC_PIC_BB_START);
-	coda_write(dev, 0, CODA_CMD_DEC_PIC_START_BYTE);
-
-	return 0;
-}
-
-static void coda_prepare_encode(struct coda_ctx *ctx)
-{
-	struct coda_q_data *q_data_src, *q_data_dst;
-	struct vb2_buffer *src_buf, *dst_buf;
-	struct coda_dev *dev = ctx->dev;
-	int force_ipicture;
-	int quant_param = 0;
-	u32 picture_y, picture_cb, picture_cr;
-	u32 pic_stream_buffer_addr, pic_stream_buffer_size;
-	u32 dst_fourcc;
-
-	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
-	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
-	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-	dst_fourcc = q_data_dst->fourcc;
-
-	src_buf->v4l2_buf.sequence = ctx->osequence;
-	dst_buf->v4l2_buf.sequence = ctx->osequence;
-	ctx->osequence++;
-
-	/*
-	 * Workaround coda firmware BUG that only marks the first
-	 * frame as IDR. This is a problem for some decoders that can't
-	 * recover when a frame is lost.
-	 */
-	if (src_buf->v4l2_buf.sequence % ctx->params.gop_size) {
-		src_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_PFRAME;
-		src_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_KEYFRAME;
-	} else {
-		src_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
-		src_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_PFRAME;
-	}
-
-	if (dev->devtype->product == CODA_960)
-		coda_set_gdi_regs(ctx);
-
-	/*
-	 * Copy headers at the beginning of the first frame for H.264 only.
-	 * In MPEG4 they are already copied by the coda.
-	 */
-	if (src_buf->v4l2_buf.sequence == 0) {
-		pic_stream_buffer_addr =
-			vb2_dma_contig_plane_dma_addr(dst_buf, 0) +
-			ctx->vpu_header_size[0] +
-			ctx->vpu_header_size[1] +
-			ctx->vpu_header_size[2];
-		pic_stream_buffer_size = CODA_MAX_FRAME_SIZE -
-			ctx->vpu_header_size[0] -
-			ctx->vpu_header_size[1] -
-			ctx->vpu_header_size[2];
-		memcpy(vb2_plane_vaddr(dst_buf, 0),
-		       &ctx->vpu_header[0][0], ctx->vpu_header_size[0]);
-		memcpy(vb2_plane_vaddr(dst_buf, 0) + ctx->vpu_header_size[0],
-		       &ctx->vpu_header[1][0], ctx->vpu_header_size[1]);
-		memcpy(vb2_plane_vaddr(dst_buf, 0) + ctx->vpu_header_size[0] +
-			ctx->vpu_header_size[1], &ctx->vpu_header[2][0],
-			ctx->vpu_header_size[2]);
-	} else {
-		pic_stream_buffer_addr =
-			vb2_dma_contig_plane_dma_addr(dst_buf, 0);
-		pic_stream_buffer_size = CODA_MAX_FRAME_SIZE;
-	}
-
-	if (src_buf->v4l2_buf.flags & V4L2_BUF_FLAG_KEYFRAME) {
-		force_ipicture = 1;
-		switch (dst_fourcc) {
-		case V4L2_PIX_FMT_H264:
-			quant_param = ctx->params.h264_intra_qp;
-			break;
-		case V4L2_PIX_FMT_MPEG4:
-			quant_param = ctx->params.mpeg4_intra_qp;
-			break;
-		default:
-			v4l2_warn(&ctx->dev->v4l2_dev,
-				"cannot set intra qp, fmt not supported\n");
-			break;
-		}
-	} else {
-		force_ipicture = 0;
-		switch (dst_fourcc) {
-		case V4L2_PIX_FMT_H264:
-			quant_param = ctx->params.h264_inter_qp;
-			break;
-		case V4L2_PIX_FMT_MPEG4:
-			quant_param = ctx->params.mpeg4_inter_qp;
-			break;
-		default:
-			v4l2_warn(&ctx->dev->v4l2_dev,
-				"cannot set inter qp, fmt not supported\n");
-			break;
-		}
-	}
-
-	/* submit */
-	coda_write(dev, CODA_ROT_MIR_ENABLE | ctx->params.rot_mode, CODA_CMD_ENC_PIC_ROT_MODE);
-	coda_write(dev, quant_param, CODA_CMD_ENC_PIC_QS);
-
-
-	picture_y = vb2_dma_contig_plane_dma_addr(src_buf, 0);
-	switch (q_data_src->fourcc) {
-	case V4L2_PIX_FMT_YVU420:
-		/* Switch Cb and Cr for YVU420 format */
-		picture_cr = picture_y + q_data_src->bytesperline *
-				q_data_src->height;
-		picture_cb = picture_cr + q_data_src->bytesperline / 2 *
-				q_data_src->height / 2;
-		break;
-	case V4L2_PIX_FMT_YUV420:
-	default:
-		picture_cb = picture_y + q_data_src->bytesperline *
-				q_data_src->height;
-		picture_cr = picture_cb + q_data_src->bytesperline / 2 *
-				q_data_src->height / 2;
-		break;
-	}
-
-	if (dev->devtype->product == CODA_960) {
-		coda_write(dev, 4/*FIXME: 0*/, CODA9_CMD_ENC_PIC_SRC_INDEX);
-		coda_write(dev, q_data_src->width, CODA9_CMD_ENC_PIC_SRC_STRIDE);
-		coda_write(dev, 0, CODA9_CMD_ENC_PIC_SUB_FRAME_SYNC);
-
-		coda_write(dev, picture_y, CODA9_CMD_ENC_PIC_SRC_ADDR_Y);
-		coda_write(dev, picture_cb, CODA9_CMD_ENC_PIC_SRC_ADDR_CB);
-		coda_write(dev, picture_cr, CODA9_CMD_ENC_PIC_SRC_ADDR_CR);
-	} else {
-		coda_write(dev, picture_y, CODA_CMD_ENC_PIC_SRC_ADDR_Y);
-		coda_write(dev, picture_cb, CODA_CMD_ENC_PIC_SRC_ADDR_CB);
-		coda_write(dev, picture_cr, CODA_CMD_ENC_PIC_SRC_ADDR_CR);
-	}
-	coda_write(dev, force_ipicture << 1 & 0x2,
-		   CODA_CMD_ENC_PIC_OPTION);
-
-	coda_write(dev, pic_stream_buffer_addr, CODA_CMD_ENC_PIC_BB_START);
-	coda_write(dev, pic_stream_buffer_size / 1024,
-		   CODA_CMD_ENC_PIC_BB_SIZE);
-
-	if (!ctx->streamon_out) {
-		/* After streamoff on the output side, set the stream end flag */
-		ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
-		coda_write(dev, ctx->bit_stream_param, CODA_REG_BIT_BIT_STREAM_PARAM);
-	}
-}
-
-static void coda_device_run(void *m2m_priv)
-{
-	struct coda_ctx *ctx = m2m_priv;
-	struct coda_dev *dev = ctx->dev;
-
-	queue_work(dev->workqueue, &ctx->pic_run_work);
-}
-
-static void coda_free_framebuffers(struct coda_ctx *ctx);
-static void coda_free_context_buffers(struct coda_ctx *ctx);
-
-static void coda_seq_end_work(struct work_struct *work)
-{
-	struct coda_ctx *ctx = container_of(work, struct coda_ctx, seq_end_work);
-	struct coda_dev *dev = ctx->dev;
-
-	mutex_lock(&ctx->buffer_mutex);
-	mutex_lock(&dev->coda_mutex);
-
-	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-		 "%d: %s: sent command 'SEQ_END' to coda\n", ctx->idx, __func__);
-	if (coda_command_sync(ctx, CODA_COMMAND_SEQ_END)) {
-		v4l2_err(&dev->v4l2_dev,
-			 "CODA_COMMAND_SEQ_END failed\n");
-	}
-
-	kfifo_init(&ctx->bitstream_fifo,
-		ctx->bitstream.vaddr, ctx->bitstream.size);
-
-	coda_free_framebuffers(ctx);
-	coda_free_context_buffers(ctx);
-
-	mutex_unlock(&dev->coda_mutex);
-	mutex_unlock(&ctx->buffer_mutex);
-}
-
-static void coda_finish_decode(struct coda_ctx *ctx);
-static void coda_finish_encode(struct coda_ctx *ctx);
-
-static void coda_pic_run_work(struct work_struct *work)
-{
-	struct coda_ctx *ctx = container_of(work, struct coda_ctx, pic_run_work);
-	struct coda_dev *dev = ctx->dev;
-	int ret;
-
-	mutex_lock(&ctx->buffer_mutex);
-	mutex_lock(&dev->coda_mutex);
-
-	if (ctx->inst_type == CODA_INST_DECODER) {
-		ret = coda_prepare_decode(ctx);
-		if (ret < 0) {
-			mutex_unlock(&dev->coda_mutex);
-			mutex_unlock(&ctx->buffer_mutex);
-			/* job_finish scheduled by prepare_decode */
-			return;
-		}
-	} else {
-		coda_prepare_encode(ctx);
-	}
-
-	if (dev->devtype->product != CODA_DX6)
-		coda_write(dev, ctx->iram_info.axi_sram_use,
-				CODA7_REG_BIT_AXI_SRAM_USE);
-
-	if (ctx->inst_type == CODA_INST_DECODER)
-		coda_kfifo_sync_to_device_full(ctx);
-	coda_command_async(ctx, CODA_COMMAND_PIC_RUN);
-
-	if (!wait_for_completion_timeout(&ctx->completion, msecs_to_jiffies(1000))) {
-		dev_err(&dev->plat_dev->dev, "CODA PIC_RUN timeout\n");
-
-		ctx->hold = true;
-
-		coda_hw_reset(ctx);
-	} else if (!ctx->aborting) {
-		if (ctx->inst_type == CODA_INST_DECODER)
-			coda_finish_decode(ctx);
-		else
-			coda_finish_encode(ctx);
-	}
-
-	if (ctx->aborting || (!ctx->streamon_cap && !ctx->streamon_out))
-		queue_work(dev->workqueue, &ctx->seq_end_work);
-
-	mutex_unlock(&dev->coda_mutex);
-	mutex_unlock(&ctx->buffer_mutex);
-
-	v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
-}
-
-static int coda_job_ready(void *m2m_priv)
-{
-	struct coda_ctx *ctx = m2m_priv;
-
-	/*
-	 * For both 'P' and 'key' frame cases 1 picture
-	 * and 1 frame are needed. In the decoder case,
-	 * the compressed frame can be in the bitstream.
-	 */
-	if (!v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) &&
-	    ctx->inst_type != CODA_INST_DECODER) {
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			 "not ready: not enough video buffers.\n");
-		return 0;
-	}
-
-	if (!v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx)) {
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			 "not ready: not enough video capture buffers.\n");
-		return 0;
-	}
-
-	if (ctx->hold ||
-	    ((ctx->inst_type == CODA_INST_DECODER) &&
-	     (coda_get_bitstream_payload(ctx) < 512) &&
-	     !(ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG))) {
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			 "%d: not ready: not enough bitstream data.\n",
-			 ctx->idx);
-		return 0;
-	}
-
-	if (ctx->aborting) {
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			 "not ready: aborting\n");
-		return 0;
-	}
-
-	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			"job ready\n");
-	return 1;
-}
-
-static void coda_job_abort(void *priv)
-{
-	struct coda_ctx *ctx = priv;
-
-	ctx->aborting = 1;
-
-	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-		 "Aborting task\n");
-}
-
-static void coda_lock(void *m2m_priv)
-{
-	struct coda_ctx *ctx = m2m_priv;
-	struct coda_dev *pcdev = ctx->dev;
-	mutex_lock(&pcdev->dev_mutex);
-}
-
-static void coda_unlock(void *m2m_priv)
-{
-	struct coda_ctx *ctx = m2m_priv;
-	struct coda_dev *pcdev = ctx->dev;
-	mutex_unlock(&pcdev->dev_mutex);
-}
-
-static struct v4l2_m2m_ops coda_m2m_ops = {
-	.device_run	= coda_device_run,
-	.job_ready	= coda_job_ready,
-	.job_abort	= coda_job_abort,
-	.lock		= coda_lock,
-	.unlock		= coda_unlock,
-};
-
-static void coda_set_tiled_map_type(struct coda_ctx *ctx, int tiled_map_type)
-{
-	struct gdi_tiled_map *tiled_map = &ctx->tiled_map;
-	int luma_map, chro_map, i;
-
-	memset(tiled_map, 0, sizeof(*tiled_map));
-
-	luma_map = 64;
-	chro_map = 64;
-	tiled_map->map_type = tiled_map_type;
-	for (i = 0; i < 16; i++)
-		tiled_map->xy2ca_map[i] = luma_map << 8 | chro_map;
-	for (i = 0; i < 4; i++)
-		tiled_map->xy2ba_map[i] = luma_map << 8 | chro_map;
-	for (i = 0; i < 16; i++)
-		tiled_map->xy2ra_map[i] = luma_map << 8 | chro_map;
-
-	if (tiled_map_type == GDI_LINEAR_FRAME_MAP) {
-		tiled_map->xy2rbc_config = 0;
-	} else {
-		dev_err(&ctx->dev->plat_dev->dev, "invalid map type: %d\n",
-			tiled_map_type);
-		return;
-	}
-}
-
-static void set_default_params(struct coda_ctx *ctx)
-{
-	int max_w;
-	int max_h;
-
-	ctx->codec = &ctx->dev->devtype->codecs[0];
-	max_w = ctx->codec->max_w;
-	max_h = ctx->codec->max_h;
-
-	ctx->params.codec_mode = CODA_MODE_INVALID;
-	ctx->colorspace = V4L2_COLORSPACE_REC709;
-	ctx->params.framerate = 30;
-	ctx->aborting = 0;
-
-	/* Default formats for output and input queues */
-	ctx->q_data[V4L2_M2M_SRC].fourcc = ctx->codec->src_fourcc;
-	ctx->q_data[V4L2_M2M_DST].fourcc = ctx->codec->dst_fourcc;
-	ctx->q_data[V4L2_M2M_SRC].width = max_w;
-	ctx->q_data[V4L2_M2M_SRC].height = max_h;
-	ctx->q_data[V4L2_M2M_SRC].bytesperline = max_w;
-	ctx->q_data[V4L2_M2M_SRC].sizeimage = (max_w * max_h * 3) / 2;
-	ctx->q_data[V4L2_M2M_DST].width = max_w;
-	ctx->q_data[V4L2_M2M_DST].height = max_h;
-	ctx->q_data[V4L2_M2M_DST].bytesperline = 0;
-	ctx->q_data[V4L2_M2M_DST].sizeimage = CODA_MAX_FRAME_SIZE;
-	ctx->q_data[V4L2_M2M_SRC].rect.width = max_w;
-	ctx->q_data[V4L2_M2M_SRC].rect.height = max_h;
-	ctx->q_data[V4L2_M2M_DST].rect.width = max_w;
-	ctx->q_data[V4L2_M2M_DST].rect.height = max_h;
-
-	if (ctx->dev->devtype->product == CODA_960)
-		coda_set_tiled_map_type(ctx, GDI_LINEAR_FRAME_MAP);
-}
-
-/*
- * Queue operations
- */
-static int coda_queue_setup(struct vb2_queue *vq,
-				const struct v4l2_format *fmt,
-				unsigned int *nbuffers, unsigned int *nplanes,
-				unsigned int sizes[], void *alloc_ctxs[])
-{
-	struct coda_ctx *ctx = vb2_get_drv_priv(vq);
-	struct coda_q_data *q_data;
-	unsigned int size;
-
-	q_data = get_q_data(ctx, vq->type);
-	size = q_data->sizeimage;
-
-	*nplanes = 1;
-	sizes[0] = size;
-
-	alloc_ctxs[0] = ctx->dev->alloc_ctx;
-
-	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-		 "get %d buffer(s) of size %d each.\n", *nbuffers, size);
-
-	return 0;
-}
-
-static int coda_buf_prepare(struct vb2_buffer *vb)
-{
-	struct coda_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-	struct coda_q_data *q_data;
-
-	q_data = get_q_data(ctx, vb->vb2_queue->type);
-
-	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
-		v4l2_warn(&ctx->dev->v4l2_dev,
-			  "%s data will not fit into plane (%lu < %lu)\n",
-			  __func__, vb2_plane_size(vb, 0),
-			  (long)q_data->sizeimage);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static void coda_buf_queue(struct vb2_buffer *vb)
-{
-	struct coda_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
-	struct coda_dev *dev = ctx->dev;
-	struct coda_q_data *q_data;
-
-	q_data = get_q_data(ctx, vb->vb2_queue->type);
-
-	/*
-	 * In the decoder case, immediately try to copy the buffer into the
-	 * bitstream ringbuffer and mark it as ready to be dequeued.
-	 */
-	if (q_data->fourcc == V4L2_PIX_FMT_H264 &&
-	    vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		/*
-		 * For backwards compatibility, queuing an empty buffer marks
-		 * the stream end
-		 */
-		if (vb2_get_plane_payload(vb, 0) == 0) {
-			ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
-			if ((dev->devtype->product == CODA_960) &&
-			    coda_isbusy(dev) &&
-			    (ctx->idx == coda_read(dev, CODA_REG_BIT_RUN_INDEX))) {
-				/* if this decoder instance is running, set the stream end flag */
-				coda_write(dev, ctx->bit_stream_param, CODA_REG_BIT_BIT_STREAM_PARAM);
-			}
-		}
-		mutex_lock(&ctx->bitstream_mutex);
-		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
-		coda_fill_bitstream(ctx);
-		mutex_unlock(&ctx->bitstream_mutex);
-	} else {
-		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
-	}
-}
-
-static void coda_parabuf_write(struct coda_ctx *ctx, int index, u32 value)
-{
-	struct coda_dev *dev = ctx->dev;
-	u32 *p = ctx->parabuf.vaddr;
-
-	if (dev->devtype->product == CODA_DX6)
-		p[index] = value;
-	else
-		p[index ^ 1] = value;
-}
-
-static int coda_alloc_aux_buf(struct coda_dev *dev,
-			      struct coda_aux_buf *buf, size_t size,
-			      const char *name, struct dentry *parent)
-{
-	buf->vaddr = dma_alloc_coherent(&dev->plat_dev->dev, size, &buf->paddr,
-					GFP_KERNEL);
-	if (!buf->vaddr)
-		return -ENOMEM;
-
-	buf->size = size;
-
-	if (name && parent) {
-		buf->blob.data = buf->vaddr;
-		buf->blob.size = size;
-		buf->dentry = debugfs_create_blob(name, 0644, parent, &buf->blob);
-		if (!buf->dentry)
-			dev_warn(&dev->plat_dev->dev,
-				 "failed to create debugfs entry %s\n", name);
-	}
-
-	return 0;
-}
-
-static inline int coda_alloc_context_buf(struct coda_ctx *ctx,
-					 struct coda_aux_buf *buf, size_t size,
-					 const char *name)
-{
-	return coda_alloc_aux_buf(ctx->dev, buf, size, name, ctx->debugfs_entry);
-}
-
-static void coda_free_aux_buf(struct coda_dev *dev,
-			      struct coda_aux_buf *buf)
-{
-	if (buf->vaddr) {
-		dma_free_coherent(&dev->plat_dev->dev, buf->size,
-				  buf->vaddr, buf->paddr);
-		buf->vaddr = NULL;
-		buf->size = 0;
-	}
-	debugfs_remove(buf->dentry);
-}
-
-static void coda_free_framebuffers(struct coda_ctx *ctx)
-{
-	int i;
-
-	for (i = 0; i < CODA_MAX_FRAMEBUFFERS; i++)
-		coda_free_aux_buf(ctx->dev, &ctx->internal_frames[i]);
-}
-
-static int coda_alloc_framebuffers(struct coda_ctx *ctx, struct coda_q_data *q_data, u32 fourcc)
-{
-	struct coda_dev *dev = ctx->dev;
-	int width, height;
-	dma_addr_t paddr;
-	int ysize;
-	int ret;
-	int i;
-
-	if (ctx->codec && (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 ||
-	     ctx->codec->dst_fourcc == V4L2_PIX_FMT_H264)) {
-		width = round_up(q_data->width, 16);
-		height = round_up(q_data->height, 16);
-	} else {
-		width = round_up(q_data->width, 8);
-		height = q_data->height;
-	}
-	ysize = width * height;
-
-	/* Allocate frame buffers */
-	for (i = 0; i < ctx->num_internal_frames; i++) {
-		size_t size;
-		char *name;
-
-		size = ysize + ysize / 2;
-		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 &&
-		    dev->devtype->product != CODA_DX6)
-			size += ysize / 4;
-		name = kasprintf(GFP_KERNEL, "fb%d", i);
-		ret = coda_alloc_context_buf(ctx, &ctx->internal_frames[i],
-					     size, name);
-		kfree(name);
-		if (ret < 0) {
-			coda_free_framebuffers(ctx);
-			return ret;
-		}
-	}
-
-	/* Register frame buffers in the parameter buffer */
-	for (i = 0; i < ctx->num_internal_frames; i++) {
-		paddr = ctx->internal_frames[i].paddr;
-		coda_parabuf_write(ctx, i * 3 + 0, paddr); /* Y */
-		coda_parabuf_write(ctx, i * 3 + 1, paddr + ysize); /* Cb */
-		coda_parabuf_write(ctx, i * 3 + 2, paddr + ysize + ysize/4); /* Cr */
-
-		/* mvcol buffer for h.264 */
-		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 &&
-		    dev->devtype->product != CODA_DX6)
-			coda_parabuf_write(ctx, 96 + i,
-					   ctx->internal_frames[i].paddr +
-					   ysize + ysize/4 + ysize/4);
-	}
-
-	/* mvcol buffer for mpeg4 */
-	if ((dev->devtype->product != CODA_DX6) &&
-	    (ctx->codec->src_fourcc == V4L2_PIX_FMT_MPEG4))
-		coda_parabuf_write(ctx, 97, ctx->internal_frames[i].paddr +
-					    ysize + ysize/4 + ysize/4);
-
-	return 0;
-}
-
-static int coda_h264_padding(int size, char *p)
-{
-	int nal_size;
-	int diff;
-
-	diff = size - (size & ~0x7);
-	if (diff == 0)
-		return 0;
-
-	nal_size = coda_filler_size[diff];
-	memcpy(p, coda_filler_nal, nal_size);
-
-	/* Add rbsp stop bit and trailing at the end */
-	*(p + nal_size - 1) = 0x80;
-
-	return nal_size;
-}
-
-static phys_addr_t coda_iram_alloc(struct coda_iram_info *iram, size_t size)
-{
-	phys_addr_t ret;
-
-	size = round_up(size, 1024);
-	if (size > iram->remaining)
-		return 0;
-	iram->remaining -= size;
-
-	ret = iram->next_paddr;
-	iram->next_paddr += size;
-
-	return ret;
-}
-
-static void coda_setup_iram(struct coda_ctx *ctx)
-{
-	struct coda_iram_info *iram_info = &ctx->iram_info;
-	struct coda_dev *dev = ctx->dev;
-	int mb_width;
-	int dbk_bits;
-	int bit_bits;
-	int ip_bits;
-
-	memset(iram_info, 0, sizeof(*iram_info));
-	iram_info->next_paddr = dev->iram.paddr;
-	iram_info->remaining = dev->iram.size;
-
-	switch (dev->devtype->product) {
-	case CODA_7541:
-		dbk_bits = CODA7_USE_HOST_DBK_ENABLE | CODA7_USE_DBK_ENABLE;
-		bit_bits = CODA7_USE_HOST_BIT_ENABLE | CODA7_USE_BIT_ENABLE;
-		ip_bits = CODA7_USE_HOST_IP_ENABLE | CODA7_USE_IP_ENABLE;
-		break;
-	case CODA_960:
-		dbk_bits = CODA9_USE_HOST_DBK_ENABLE | CODA9_USE_DBK_ENABLE;
-		bit_bits = CODA9_USE_HOST_BIT_ENABLE | CODA7_USE_BIT_ENABLE;
-		ip_bits = CODA9_USE_HOST_IP_ENABLE | CODA7_USE_IP_ENABLE;
-		break;
-	default: /* CODA_DX6 */
-		return;
-	}
-
-	if (ctx->inst_type == CODA_INST_ENCODER) {
-		struct coda_q_data *q_data_src;
-
-		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-		mb_width = DIV_ROUND_UP(q_data_src->width, 16);
-
-		/* Prioritize in case IRAM is too small for everything */
-		if (dev->devtype->product == CODA_7541) {
-			iram_info->search_ram_size = round_up(mb_width * 16 *
-							      36 + 2048, 1024);
-			iram_info->search_ram_paddr = coda_iram_alloc(iram_info,
-							iram_info->search_ram_size);
-			if (!iram_info->search_ram_paddr) {
-				pr_err("IRAM is smaller than the search ram size\n");
-				goto out;
-			}
-			iram_info->axi_sram_use |= CODA7_USE_HOST_ME_ENABLE |
-						   CODA7_USE_ME_ENABLE;
-		}
-
-		/* Only H.264BP and H.263P3 are considered */
-		iram_info->buf_dbk_y_use = coda_iram_alloc(iram_info, 64 * mb_width);
-		iram_info->buf_dbk_c_use = coda_iram_alloc(iram_info, 64 * mb_width);
-		if (!iram_info->buf_dbk_c_use)
-			goto out;
-		iram_info->axi_sram_use |= dbk_bits;
-
-		iram_info->buf_bit_use = coda_iram_alloc(iram_info, 128 * mb_width);
-		if (!iram_info->buf_bit_use)
-			goto out;
-		iram_info->axi_sram_use |= bit_bits;
-
-		iram_info->buf_ip_ac_dc_use = coda_iram_alloc(iram_info, 128 * mb_width);
-		if (!iram_info->buf_ip_ac_dc_use)
-			goto out;
-		iram_info->axi_sram_use |= ip_bits;
-
-		/* OVL and BTP disabled for encoder */
-	} else if (ctx->inst_type == CODA_INST_DECODER) {
-		struct coda_q_data *q_data_dst;
-
-		q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-		mb_width = DIV_ROUND_UP(q_data_dst->width, 16);
-
-		iram_info->buf_dbk_y_use = coda_iram_alloc(iram_info, 128 * mb_width);
-		iram_info->buf_dbk_c_use = coda_iram_alloc(iram_info, 128 * mb_width);
-		if (!iram_info->buf_dbk_c_use)
-			goto out;
-		iram_info->axi_sram_use |= dbk_bits;
-
-		iram_info->buf_bit_use = coda_iram_alloc(iram_info, 128 * mb_width);
-		if (!iram_info->buf_bit_use)
-			goto out;
-		iram_info->axi_sram_use |= bit_bits;
-
-		iram_info->buf_ip_ac_dc_use = coda_iram_alloc(iram_info, 128 * mb_width);
-		if (!iram_info->buf_ip_ac_dc_use)
-			goto out;
-		iram_info->axi_sram_use |= ip_bits;
-
-		/* OVL and BTP unused as there is no VC1 support yet */
-	}
-
-out:
-	if (!(iram_info->axi_sram_use & CODA7_USE_HOST_IP_ENABLE))
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			 "IRAM smaller than needed\n");
-
-	if (dev->devtype->product == CODA_7541) {
-		/* TODO - Enabling these causes picture errors on CODA7541 */
-		if (ctx->inst_type == CODA_INST_DECODER) {
-			/* fw 1.4.50 */
-			iram_info->axi_sram_use &= ~(CODA7_USE_HOST_IP_ENABLE |
-						     CODA7_USE_IP_ENABLE);
-		} else {
-			/* fw 13.4.29 */
-			iram_info->axi_sram_use &= ~(CODA7_USE_HOST_IP_ENABLE |
-						     CODA7_USE_HOST_DBK_ENABLE |
-						     CODA7_USE_IP_ENABLE |
-						     CODA7_USE_DBK_ENABLE);
-		}
-	}
-}
-
-static void coda_free_context_buffers(struct coda_ctx *ctx)
-{
-	struct coda_dev *dev = ctx->dev;
-
-	coda_free_aux_buf(dev, &ctx->slicebuf);
-	coda_free_aux_buf(dev, &ctx->psbuf);
-	if (dev->devtype->product != CODA_DX6)
-		coda_free_aux_buf(dev, &ctx->workbuf);
-}
-
-static int coda_alloc_context_buffers(struct coda_ctx *ctx,
-				      struct coda_q_data *q_data)
-{
-	struct coda_dev *dev = ctx->dev;
-	size_t size;
-	int ret;
-
-	if (dev->devtype->product == CODA_DX6)
-		return 0;
-
-	if (ctx->psbuf.vaddr) {
-		v4l2_err(&dev->v4l2_dev, "psmembuf still allocated\n");
-		return -EBUSY;
-	}
-	if (ctx->slicebuf.vaddr) {
-		v4l2_err(&dev->v4l2_dev, "slicebuf still allocated\n");
-		return -EBUSY;
-	}
-	if (ctx->workbuf.vaddr) {
-		v4l2_err(&dev->v4l2_dev, "context buffer still allocated\n");
-		ret = -EBUSY;
-		return -ENOMEM;
-	}
-
-	if (q_data->fourcc == V4L2_PIX_FMT_H264) {
-		/* worst case slice size */
-		size = (DIV_ROUND_UP(q_data->width, 16) *
-			DIV_ROUND_UP(q_data->height, 16)) * 3200 / 8 + 512;
-		ret = coda_alloc_context_buf(ctx, &ctx->slicebuf, size, "slicebuf");
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "failed to allocate %d byte slice buffer",
-				 ctx->slicebuf.size);
-			return ret;
-		}
-	}
-
-	if (dev->devtype->product == CODA_7541) {
-		ret = coda_alloc_context_buf(ctx, &ctx->psbuf, CODA7_PS_BUF_SIZE, "psbuf");
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "failed to allocate psmem buffer");
-			goto err;
-		}
-	}
-
-	size = dev->devtype->workbuf_size;
-	if (dev->devtype->product == CODA_960 &&
-	    q_data->fourcc == V4L2_PIX_FMT_H264)
-		size += CODA9_PS_SAVE_SIZE;
-	ret = coda_alloc_context_buf(ctx, &ctx->workbuf, size, "workbuf");
-	if (ret < 0) {
-		v4l2_err(&dev->v4l2_dev, "failed to allocate %d byte context buffer",
-			 ctx->workbuf.size);
-		goto err;
-	}
-
-	return 0;
-
-err:
-	coda_free_context_buffers(ctx);
-	return ret;
-}
-
-static int coda_start_decoding(struct coda_ctx *ctx)
-{
-	struct coda_q_data *q_data_src, *q_data_dst;
-	u32 bitstream_buf, bitstream_size;
-	struct coda_dev *dev = ctx->dev;
-	int width, height;
-	u32 src_fourcc;
-	u32 val;
-	int ret;
-
-	/* Start decoding */
-	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-	bitstream_buf = ctx->bitstream.paddr;
-	bitstream_size = ctx->bitstream.size;
-	src_fourcc = q_data_src->fourcc;
-
-	coda_write(dev, ctx->parabuf.paddr, CODA_REG_BIT_PARA_BUF_ADDR);
-
-	/* Update coda bitstream read and write pointers from kfifo */
-	coda_kfifo_sync_to_device_full(ctx);
-
-	ctx->display_idx = -1;
-	ctx->frm_dis_flg = 0;
-	coda_write(dev, 0, CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
-
-	coda_write(dev, CODA_BIT_DEC_SEQ_INIT_ESCAPE,
-			CODA_REG_BIT_BIT_STREAM_PARAM);
-
-	coda_write(dev, bitstream_buf, CODA_CMD_DEC_SEQ_BB_START);
-	coda_write(dev, bitstream_size / 1024, CODA_CMD_DEC_SEQ_BB_SIZE);
-	val = 0;
-	if ((dev->devtype->product == CODA_7541) ||
-	    (dev->devtype->product == CODA_960))
-		val |= CODA_REORDER_ENABLE;
-	coda_write(dev, val, CODA_CMD_DEC_SEQ_OPTION);
-
-	ctx->params.codec_mode = ctx->codec->mode;
-	if (dev->devtype->product == CODA_960 &&
-	    src_fourcc == V4L2_PIX_FMT_MPEG4)
-		ctx->params.codec_mode_aux = CODA_MP4_AUX_MPEG4;
-	else
-		ctx->params.codec_mode_aux = 0;
-	if (src_fourcc == V4L2_PIX_FMT_H264) {
-		if (dev->devtype->product == CODA_7541) {
-			coda_write(dev, ctx->psbuf.paddr,
-					CODA_CMD_DEC_SEQ_PS_BB_START);
-			coda_write(dev, (CODA7_PS_BUF_SIZE / 1024),
-					CODA_CMD_DEC_SEQ_PS_BB_SIZE);
-		}
-		if (dev->devtype->product == CODA_960) {
-			coda_write(dev, 0, CODA_CMD_DEC_SEQ_X264_MV_EN);
-			coda_write(dev, 512, CODA_CMD_DEC_SEQ_SPP_CHUNK_SIZE);
-		}
-	}
-	if (dev->devtype->product != CODA_960) {
-		coda_write(dev, 0, CODA_CMD_DEC_SEQ_SRC_SIZE);
-	}
-
-	if (coda_command_sync(ctx, CODA_COMMAND_SEQ_INIT)) {
-		v4l2_err(&dev->v4l2_dev, "CODA_COMMAND_SEQ_INIT timeout\n");
-		coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
-		return -ETIMEDOUT;
-	}
-
-	/* Update kfifo out pointer from coda bitstream read pointer */
-	coda_kfifo_sync_from_device(ctx);
-
-	coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
-
-	if (coda_read(dev, CODA_RET_DEC_SEQ_SUCCESS) == 0) {
-		v4l2_err(&dev->v4l2_dev,
-			"CODA_COMMAND_SEQ_INIT failed, error code = %d\n",
-			coda_read(dev, CODA_RET_DEC_SEQ_ERR_REASON));
-		return -EAGAIN;
-	}
-
-	val = coda_read(dev, CODA_RET_DEC_SEQ_SRC_SIZE);
-	if (dev->devtype->product == CODA_DX6) {
-		width = (val >> CODADX6_PICWIDTH_OFFSET) & CODADX6_PICWIDTH_MASK;
-		height = val & CODADX6_PICHEIGHT_MASK;
-	} else {
-		width = (val >> CODA7_PICWIDTH_OFFSET) & CODA7_PICWIDTH_MASK;
-		height = val & CODA7_PICHEIGHT_MASK;
-	}
-
-	if (width > q_data_dst->width || height > q_data_dst->height) {
-		v4l2_err(&dev->v4l2_dev, "stream is %dx%d, not %dx%d\n",
-			 width, height, q_data_dst->width, q_data_dst->height);
-		return -EINVAL;
-	}
-
-	width = round_up(width, 16);
-	height = round_up(height, 16);
-
-	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "%s instance %d now: %dx%d\n",
-		 __func__, ctx->idx, width, height);
-
-	ctx->num_internal_frames = coda_read(dev, CODA_RET_DEC_SEQ_FRAME_NEED);
-	if (ctx->num_internal_frames > CODA_MAX_FRAMEBUFFERS) {
-		v4l2_err(&dev->v4l2_dev,
-			 "not enough framebuffers to decode (%d < %d)\n",
-			 CODA_MAX_FRAMEBUFFERS, ctx->num_internal_frames);
-		return -EINVAL;
-	}
-
-	if (src_fourcc == V4L2_PIX_FMT_H264) {
-		u32 left_right;
-		u32 top_bottom;
-
-		left_right = coda_read(dev, CODA_RET_DEC_SEQ_CROP_LEFT_RIGHT);
-		top_bottom = coda_read(dev, CODA_RET_DEC_SEQ_CROP_TOP_BOTTOM);
-
-		q_data_dst->rect.left = (left_right >> 10) & 0x3ff;
-		q_data_dst->rect.top = (top_bottom >> 10) & 0x3ff;
-		q_data_dst->rect.width = width - q_data_dst->rect.left -
-					 (left_right & 0x3ff);
-		q_data_dst->rect.height = height - q_data_dst->rect.top -
-					  (top_bottom & 0x3ff);
-	}
-
-	ret = coda_alloc_framebuffers(ctx, q_data_dst, src_fourcc);
-	if (ret < 0)
-		return ret;
-
-	/* Tell the decoder how many frame buffers we allocated. */
-	coda_write(dev, ctx->num_internal_frames, CODA_CMD_SET_FRAME_BUF_NUM);
-	coda_write(dev, width, CODA_CMD_SET_FRAME_BUF_STRIDE);
-
-	if (dev->devtype->product != CODA_DX6) {
-		/* Set secondary AXI IRAM */
-		coda_setup_iram(ctx);
-
-		coda_write(dev, ctx->iram_info.buf_bit_use,
-				CODA7_CMD_SET_FRAME_AXI_BIT_ADDR);
-		coda_write(dev, ctx->iram_info.buf_ip_ac_dc_use,
-				CODA7_CMD_SET_FRAME_AXI_IPACDC_ADDR);
-		coda_write(dev, ctx->iram_info.buf_dbk_y_use,
-				CODA7_CMD_SET_FRAME_AXI_DBKY_ADDR);
-		coda_write(dev, ctx->iram_info.buf_dbk_c_use,
-				CODA7_CMD_SET_FRAME_AXI_DBKC_ADDR);
-		coda_write(dev, ctx->iram_info.buf_ovl_use,
-				CODA7_CMD_SET_FRAME_AXI_OVL_ADDR);
-		if (dev->devtype->product == CODA_960)
-			coda_write(dev, ctx->iram_info.buf_btp_use,
-					CODA9_CMD_SET_FRAME_AXI_BTP_ADDR);
-	}
-
-	if (dev->devtype->product == CODA_960) {
-		coda_write(dev, -1, CODA9_CMD_SET_FRAME_DELAY);
-
-		coda_write(dev, 0x20262024, CODA9_CMD_SET_FRAME_CACHE_SIZE);
-		coda_write(dev, 2 << CODA9_CACHE_PAGEMERGE_OFFSET |
-				32 << CODA9_CACHE_LUMA_BUFFER_SIZE_OFFSET |
-				8 << CODA9_CACHE_CB_BUFFER_SIZE_OFFSET |
-				8 << CODA9_CACHE_CR_BUFFER_SIZE_OFFSET,
-				CODA9_CMD_SET_FRAME_CACHE_CONFIG);
-	}
-
-	if (src_fourcc == V4L2_PIX_FMT_H264) {
-		coda_write(dev, ctx->slicebuf.paddr,
-				CODA_CMD_SET_FRAME_SLICE_BB_START);
-		coda_write(dev, ctx->slicebuf.size / 1024,
-				CODA_CMD_SET_FRAME_SLICE_BB_SIZE);
-	}
-
-	if (dev->devtype->product == CODA_7541) {
-		int max_mb_x = 1920 / 16;
-		int max_mb_y = 1088 / 16;
-		int max_mb_num = max_mb_x * max_mb_y;
-
-		coda_write(dev, max_mb_num << 16 | max_mb_x << 8 | max_mb_y,
-				CODA7_CMD_SET_FRAME_MAX_DEC_SIZE);
-	} else if (dev->devtype->product == CODA_960) {
-		int max_mb_x = 1920 / 16;
-		int max_mb_y = 1088 / 16;
-		int max_mb_num = max_mb_x * max_mb_y;
-
-		coda_write(dev, max_mb_num << 16 | max_mb_x << 8 | max_mb_y,
-				CODA9_CMD_SET_FRAME_MAX_DEC_SIZE);
-	}
-
-	if (coda_command_sync(ctx, CODA_COMMAND_SET_FRAME_BUF)) {
-		v4l2_err(&ctx->dev->v4l2_dev,
-			 "CODA_COMMAND_SET_FRAME_BUF timeout\n");
-		return -ETIMEDOUT;
-	}
-
-	return 0;
-}
-
-static int coda_encode_header(struct coda_ctx *ctx, struct vb2_buffer *buf,
-			      int header_code, u8 *header, int *size)
-{
-	struct coda_dev *dev = ctx->dev;
-	size_t bufsize;
-	int ret;
-	int i;
-
-	if (dev->devtype->product == CODA_960)
-		memset(vb2_plane_vaddr(buf, 0), 0, 64);
-
-	coda_write(dev, vb2_dma_contig_plane_dma_addr(buf, 0),
-		   CODA_CMD_ENC_HEADER_BB_START);
-	bufsize = vb2_plane_size(buf, 0);
-	if (dev->devtype->product == CODA_960)
-		bufsize /= 1024;
-	coda_write(dev, bufsize, CODA_CMD_ENC_HEADER_BB_SIZE);
-	coda_write(dev, header_code, CODA_CMD_ENC_HEADER_CODE);
-	ret = coda_command_sync(ctx, CODA_COMMAND_ENCODE_HEADER);
-	if (ret < 0) {
-		v4l2_err(&dev->v4l2_dev, "CODA_COMMAND_ENCODE_HEADER timeout\n");
-		return ret;
-	}
-
-	if (dev->devtype->product == CODA_960) {
-		for (i = 63; i > 0; i--)
-			if (((char *)vb2_plane_vaddr(buf, 0))[i] != 0)
-				break;
-		*size = i + 1;
-	} else {
-		*size = coda_read(dev, CODA_REG_BIT_WR_PTR(ctx->reg_idx)) -
-			coda_read(dev, CODA_CMD_ENC_HEADER_BB_START);
-	}
-	memcpy(header, vb2_plane_vaddr(buf, 0), *size);
-
-	return 0;
-}
-
-static int coda_start_encoding(struct coda_ctx *ctx);
-
-static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
-{
-	struct coda_ctx *ctx = vb2_get_drv_priv(q);
-	struct v4l2_device *v4l2_dev = &ctx->dev->v4l2_dev;
-	struct coda_dev *dev = ctx->dev;
-	struct coda_q_data *q_data_src, *q_data_dst;
-	u32 dst_fourcc;
-	int ret = 0;
-
-	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		if (q_data_src->fourcc == V4L2_PIX_FMT_H264) {
-			if (coda_get_bitstream_payload(ctx) < 512)
-				return -EINVAL;
-		} else {
-			if (count < 1)
-				return -EINVAL;
-		}
-
-		ctx->streamon_out = 1;
-
-		if (coda_format_is_yuv(q_data_src->fourcc))
-			ctx->inst_type = CODA_INST_ENCODER;
-		else
-			ctx->inst_type = CODA_INST_DECODER;
-	} else {
-		if (count < 1)
-			return -EINVAL;
-
-		ctx->streamon_cap = 1;
-	}
-
-	/* Don't start the coda unless both queues are on */
-	if (!(ctx->streamon_out & ctx->streamon_cap))
-		return 0;
-
-	/* Allow decoder device_run with no new buffers queued */
-	if (ctx->inst_type == CODA_INST_DECODER)
-		v4l2_m2m_set_src_buffered(ctx->fh.m2m_ctx, true);
-
-	ctx->gopcounter = ctx->params.gop_size - 1;
-	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-	dst_fourcc = q_data_dst->fourcc;
-
-	ctx->codec = coda_find_codec(ctx->dev, q_data_src->fourcc,
-				     q_data_dst->fourcc);
-	if (!ctx->codec) {
-		v4l2_err(v4l2_dev, "couldn't tell instance type.\n");
-		return -EINVAL;
-	}
-
-	/* Allocate per-instance buffers */
-	ret = coda_alloc_context_buffers(ctx, q_data_src);
-	if (ret < 0)
-		return ret;
-
-	if (ctx->inst_type == CODA_INST_DECODER) {
-		mutex_lock(&dev->coda_mutex);
-		ret = coda_start_decoding(ctx);
-		mutex_unlock(&dev->coda_mutex);
-		if (ret == -EAGAIN)
-			return 0;
-		else if (ret < 0)
-			return ret;
-	} else {
-		ret = coda_start_encoding(ctx);
-	}
-
-	ctx->initialized = 1;
-	return ret;
-}
-
-static int coda_start_encoding(struct coda_ctx *ctx)
-{
-	struct coda_dev *dev = ctx->dev;
-	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
-	struct coda_q_data *q_data_src, *q_data_dst;
-	u32 bitstream_buf, bitstream_size;
-	struct vb2_buffer *buf;
-	int gamma, ret, value;
-	u32 dst_fourcc;
-
-	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-	dst_fourcc = q_data_dst->fourcc;
-
-	buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
-	bitstream_buf = vb2_dma_contig_plane_dma_addr(buf, 0);
-	bitstream_size = q_data_dst->sizeimage;
-
-	if (!coda_is_initialized(dev)) {
-		v4l2_err(v4l2_dev, "coda is not initialized.\n");
-		return -EFAULT;
-	}
-
-	mutex_lock(&dev->coda_mutex);
-
-	coda_write(dev, ctx->parabuf.paddr, CODA_REG_BIT_PARA_BUF_ADDR);
-	coda_write(dev, bitstream_buf, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
-	coda_write(dev, bitstream_buf, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
-	switch (dev->devtype->product) {
-	case CODA_DX6:
-		coda_write(dev, CODADX6_STREAM_BUF_DYNALLOC_EN |
-			CODADX6_STREAM_BUF_PIC_RESET, CODA_REG_BIT_STREAM_CTRL);
-		break;
-	case CODA_960:
-		coda_write(dev, 0, CODA9_GDI_WPROT_RGN_EN);
-		/* fallthrough */
-	case CODA_7541:
-		coda_write(dev, CODA7_STREAM_BUF_DYNALLOC_EN |
-			CODA7_STREAM_BUF_PIC_RESET, CODA_REG_BIT_STREAM_CTRL);
-		break;
-	}
-
-	value = coda_read(dev, CODA_REG_BIT_FRAME_MEM_CTRL);
-	value &= ~(1 << 2 | 0x7 << 9);
-	ctx->frame_mem_ctrl = value;
-	coda_write(dev, value, CODA_REG_BIT_FRAME_MEM_CTRL);
-
-	if (dev->devtype->product == CODA_DX6) {
-		/* Configure the coda */
-		coda_write(dev, dev->iram.paddr, CODADX6_REG_BIT_SEARCH_RAM_BASE_ADDR);
-	}
-
-	/* Could set rotation here if needed */
-	switch (dev->devtype->product) {
-	case CODA_DX6:
-		value = (q_data_src->width & CODADX6_PICWIDTH_MASK) << CODADX6_PICWIDTH_OFFSET;
-		value |= (q_data_src->height & CODADX6_PICHEIGHT_MASK) << CODA_PICHEIGHT_OFFSET;
-		break;
-	case CODA_7541:
-		if (dst_fourcc == V4L2_PIX_FMT_H264) {
-			value = (round_up(q_data_src->width, 16) &
-				 CODA7_PICWIDTH_MASK) << CODA7_PICWIDTH_OFFSET;
-			value |= (round_up(q_data_src->height, 16) &
-				  CODA7_PICHEIGHT_MASK) << CODA_PICHEIGHT_OFFSET;
-			break;
-		}
-		/* fallthrough */
-	case CODA_960:
-		value = (q_data_src->width & CODA7_PICWIDTH_MASK) << CODA7_PICWIDTH_OFFSET;
-		value |= (q_data_src->height & CODA7_PICHEIGHT_MASK) << CODA_PICHEIGHT_OFFSET;
-	}
-	coda_write(dev, value, CODA_CMD_ENC_SEQ_SRC_SIZE);
-	coda_write(dev, ctx->params.framerate,
-		   CODA_CMD_ENC_SEQ_SRC_F_RATE);
-
-	ctx->params.codec_mode = ctx->codec->mode;
-	switch (dst_fourcc) {
-	case V4L2_PIX_FMT_MPEG4:
-		if (dev->devtype->product == CODA_960)
-			coda_write(dev, CODA9_STD_MPEG4, CODA_CMD_ENC_SEQ_COD_STD);
-		else
-			coda_write(dev, CODA_STD_MPEG4, CODA_CMD_ENC_SEQ_COD_STD);
-		coda_write(dev, 0, CODA_CMD_ENC_SEQ_MP4_PARA);
-		break;
-	case V4L2_PIX_FMT_H264:
-		if (dev->devtype->product == CODA_960)
-			coda_write(dev, CODA9_STD_H264, CODA_CMD_ENC_SEQ_COD_STD);
-		else
-			coda_write(dev, CODA_STD_H264, CODA_CMD_ENC_SEQ_COD_STD);
-		if (ctx->params.h264_deblk_enabled) {
-			value = ((ctx->params.h264_deblk_alpha &
-				  CODA_264PARAM_DEBLKFILTEROFFSETALPHA_MASK) <<
-				 CODA_264PARAM_DEBLKFILTEROFFSETALPHA_OFFSET) |
-				((ctx->params.h264_deblk_beta &
-				  CODA_264PARAM_DEBLKFILTEROFFSETBETA_MASK) <<
-				 CODA_264PARAM_DEBLKFILTEROFFSETBETA_OFFSET);
-		} else {
-			value = 1 << CODA_264PARAM_DISABLEDEBLK_OFFSET;
-		}
-		coda_write(dev, value, CODA_CMD_ENC_SEQ_264_PARA);
-		break;
-	default:
-		v4l2_err(v4l2_dev,
-			 "dst format (0x%08x) invalid.\n", dst_fourcc);
-		ret = -EINVAL;
-		goto out;
-	}
-
-	switch (ctx->params.slice_mode) {
-	case V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE:
-		value = 0;
-		break;
-	case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB:
-		value  = (ctx->params.slice_max_mb & CODA_SLICING_SIZE_MASK) << CODA_SLICING_SIZE_OFFSET;
-		value |= (1 & CODA_SLICING_UNIT_MASK) << CODA_SLICING_UNIT_OFFSET;
-		value |=  1 & CODA_SLICING_MODE_MASK;
-		break;
-	case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES:
-		value  = (ctx->params.slice_max_bits & CODA_SLICING_SIZE_MASK) << CODA_SLICING_SIZE_OFFSET;
-		value |= (0 & CODA_SLICING_UNIT_MASK) << CODA_SLICING_UNIT_OFFSET;
-		value |=  1 & CODA_SLICING_MODE_MASK;
-		break;
-	}
-	coda_write(dev, value, CODA_CMD_ENC_SEQ_SLICE_MODE);
-	value = ctx->params.gop_size & CODA_GOP_SIZE_MASK;
-	coda_write(dev, value, CODA_CMD_ENC_SEQ_GOP_SIZE);
-
-	if (ctx->params.bitrate) {
-		/* Rate control enabled */
-		value = (ctx->params.bitrate & CODA_RATECONTROL_BITRATE_MASK) << CODA_RATECONTROL_BITRATE_OFFSET;
-		value |=  1 & CODA_RATECONTROL_ENABLE_MASK;
-		if (dev->devtype->product == CODA_960)
-			value |= BIT(31); /* disable autoskip */
-	} else {
-		value = 0;
-	}
-	coda_write(dev, value, CODA_CMD_ENC_SEQ_RC_PARA);
-
-	coda_write(dev, 0, CODA_CMD_ENC_SEQ_RC_BUF_SIZE);
-	coda_write(dev, ctx->params.intra_refresh,
-		   CODA_CMD_ENC_SEQ_INTRA_REFRESH);
-
-	coda_write(dev, bitstream_buf, CODA_CMD_ENC_SEQ_BB_START);
-	coda_write(dev, bitstream_size / 1024, CODA_CMD_ENC_SEQ_BB_SIZE);
-
-
-	value = 0;
-	if (dev->devtype->product == CODA_960)
-		gamma = CODA9_DEFAULT_GAMMA;
-	else
-		gamma = CODA_DEFAULT_GAMMA;
-	if (gamma > 0) {
-		coda_write(dev, (gamma & CODA_GAMMA_MASK) << CODA_GAMMA_OFFSET,
-			   CODA_CMD_ENC_SEQ_RC_GAMMA);
-	}
-
-	if (ctx->params.h264_min_qp || ctx->params.h264_max_qp) {
-		coda_write(dev,
-			   ctx->params.h264_min_qp << CODA_QPMIN_OFFSET |
-			   ctx->params.h264_max_qp << CODA_QPMAX_OFFSET,
-			   CODA_CMD_ENC_SEQ_RC_QP_MIN_MAX);
-	}
-	if (dev->devtype->product == CODA_960) {
-		if (ctx->params.h264_max_qp)
-			value |= 1 << CODA9_OPTION_RCQPMAX_OFFSET;
-		if (CODA_DEFAULT_GAMMA > 0)
-			value |= 1 << CODA9_OPTION_GAMMA_OFFSET;
-	} else {
-		if (CODA_DEFAULT_GAMMA > 0) {
-			if (dev->devtype->product == CODA_DX6)
-				value |= 1 << CODADX6_OPTION_GAMMA_OFFSET;
-			else
-				value |= 1 << CODA7_OPTION_GAMMA_OFFSET;
-		}
-		if (ctx->params.h264_min_qp)
-			value |= 1 << CODA7_OPTION_RCQPMIN_OFFSET;
-		if (ctx->params.h264_max_qp)
-			value |= 1 << CODA7_OPTION_RCQPMAX_OFFSET;
-	}
-	coda_write(dev, value, CODA_CMD_ENC_SEQ_OPTION);
-
-	coda_write(dev, 0, CODA_CMD_ENC_SEQ_RC_INTERVAL_MODE);
-
-	coda_setup_iram(ctx);
-
-	if (dst_fourcc == V4L2_PIX_FMT_H264) {
-		switch (dev->devtype->product) {
-		case CODA_DX6:
-			value = FMO_SLICE_SAVE_BUF_SIZE << 7;
-			coda_write(dev, value, CODADX6_CMD_ENC_SEQ_FMO);
-			break;
-		case CODA_7541:
-			coda_write(dev, ctx->iram_info.search_ram_paddr,
-					CODA7_CMD_ENC_SEQ_SEARCH_BASE);
-			coda_write(dev, ctx->iram_info.search_ram_size,
-					CODA7_CMD_ENC_SEQ_SEARCH_SIZE);
-			break;
-		case CODA_960:
-			coda_write(dev, 0, CODA9_CMD_ENC_SEQ_ME_OPTION);
-			coda_write(dev, 0, CODA9_CMD_ENC_SEQ_INTRA_WEIGHT);
-		}
-	}
-
-	ret = coda_command_sync(ctx, CODA_COMMAND_SEQ_INIT);
-	if (ret < 0) {
-		v4l2_err(v4l2_dev, "CODA_COMMAND_SEQ_INIT timeout\n");
-		goto out;
-	}
-
-	if (coda_read(dev, CODA_RET_ENC_SEQ_SUCCESS) == 0) {
-		v4l2_err(v4l2_dev, "CODA_COMMAND_SEQ_INIT failed\n");
-		ret = -EFAULT;
-		goto out;
-	}
-
-	if (dev->devtype->product == CODA_960)
-		ctx->num_internal_frames = 4;
-	else
-		ctx->num_internal_frames = 2;
-	ret = coda_alloc_framebuffers(ctx, q_data_src, dst_fourcc);
-	if (ret < 0) {
-		v4l2_err(v4l2_dev, "failed to allocate framebuffers\n");
-		goto out;
-	}
-
-	coda_write(dev, ctx->num_internal_frames, CODA_CMD_SET_FRAME_BUF_NUM);
-	coda_write(dev, q_data_src->bytesperline,
-			CODA_CMD_SET_FRAME_BUF_STRIDE);
-	if (dev->devtype->product == CODA_7541) {
-		coda_write(dev, q_data_src->bytesperline,
-				CODA7_CMD_SET_FRAME_SOURCE_BUF_STRIDE);
-	}
-	if (dev->devtype->product != CODA_DX6) {
-		coda_write(dev, ctx->iram_info.buf_bit_use,
-				CODA7_CMD_SET_FRAME_AXI_BIT_ADDR);
-		coda_write(dev, ctx->iram_info.buf_ip_ac_dc_use,
-				CODA7_CMD_SET_FRAME_AXI_IPACDC_ADDR);
-		coda_write(dev, ctx->iram_info.buf_dbk_y_use,
-				CODA7_CMD_SET_FRAME_AXI_DBKY_ADDR);
-		coda_write(dev, ctx->iram_info.buf_dbk_c_use,
-				CODA7_CMD_SET_FRAME_AXI_DBKC_ADDR);
-		coda_write(dev, ctx->iram_info.buf_ovl_use,
-				CODA7_CMD_SET_FRAME_AXI_OVL_ADDR);
-		if (dev->devtype->product == CODA_960) {
-			coda_write(dev, ctx->iram_info.buf_btp_use,
-					CODA9_CMD_SET_FRAME_AXI_BTP_ADDR);
-
-			/* FIXME */
-			coda_write(dev, ctx->internal_frames[2].paddr, CODA9_CMD_SET_FRAME_SUBSAMP_A);
-			coda_write(dev, ctx->internal_frames[3].paddr, CODA9_CMD_SET_FRAME_SUBSAMP_B);
-		}
-	}
-
-	ret = coda_command_sync(ctx, CODA_COMMAND_SET_FRAME_BUF);
-	if (ret < 0) {
-		v4l2_err(v4l2_dev, "CODA_COMMAND_SET_FRAME_BUF timeout\n");
-		goto out;
-	}
-
-	/* Save stream headers */
-	buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
-	switch (dst_fourcc) {
-	case V4L2_PIX_FMT_H264:
-		/*
-		 * Get SPS in the first frame and copy it to an
-		 * intermediate buffer.
-		 */
-		ret = coda_encode_header(ctx, buf, CODA_HEADER_H264_SPS,
-					 &ctx->vpu_header[0][0],
-					 &ctx->vpu_header_size[0]);
-		if (ret < 0)
-			goto out;
-
-		/*
-		 * Get PPS in the first frame and copy it to an
-		 * intermediate buffer.
-		 */
-		ret = coda_encode_header(ctx, buf, CODA_HEADER_H264_PPS,
-					 &ctx->vpu_header[1][0],
-					 &ctx->vpu_header_size[1]);
-		if (ret < 0)
-			goto out;
-
-		/*
-		 * Length of H.264 headers is variable and thus it might not be
-		 * aligned for the coda to append the encoded frame. In that is
-		 * the case a filler NAL must be added to header 2.
-		 */
-		ctx->vpu_header_size[2] = coda_h264_padding(
-					(ctx->vpu_header_size[0] +
-					 ctx->vpu_header_size[1]),
-					 ctx->vpu_header[2]);
-		break;
-	case V4L2_PIX_FMT_MPEG4:
-		/*
-		 * Get VOS in the first frame and copy it to an
-		 * intermediate buffer
-		 */
-		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VOS,
-					 &ctx->vpu_header[0][0],
-					 &ctx->vpu_header_size[0]);
-		if (ret < 0)
-			goto out;
-
-		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VIS,
-					 &ctx->vpu_header[1][0],
-					 &ctx->vpu_header_size[1]);
-		if (ret < 0)
-			goto out;
-
-		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VOL,
-					 &ctx->vpu_header[2][0],
-					 &ctx->vpu_header_size[2]);
-		if (ret < 0)
-			goto out;
-		break;
-	default:
-		/* No more formats need to save headers at the moment */
-		break;
-	}
-
-out:
-	mutex_unlock(&dev->coda_mutex);
-	return ret;
-}
-
-static void coda_stop_streaming(struct vb2_queue *q)
-{
-	struct coda_ctx *ctx = vb2_get_drv_priv(q);
-	struct coda_dev *dev = ctx->dev;
-
-	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
-		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-			 "%s: output\n", __func__);
-		ctx->streamon_out = 0;
-
-		if (ctx->inst_type == CODA_INST_DECODER &&
-		    coda_isbusy(dev) && ctx->idx == coda_read(dev, CODA_REG_BIT_RUN_INDEX)) {
-			/* if this decoder instance is running, set the stream end flag */
-			if (dev->devtype->product == CODA_960) {
-				u32 val = coda_read(dev, CODA_REG_BIT_BIT_STREAM_PARAM);
-
-				val |= CODA_BIT_STREAM_END_FLAG;
-				coda_write(dev, val, CODA_REG_BIT_BIT_STREAM_PARAM);
-				ctx->bit_stream_param = val;
-			}
-		}
-		ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
-
-		ctx->isequence = 0;
-	} else {
-		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-			 "%s: capture\n", __func__);
-		ctx->streamon_cap = 0;
-
-		ctx->osequence = 0;
-		ctx->sequence_offset = 0;
-	}
-
-	if (!ctx->streamon_out && !ctx->streamon_cap) {
-		struct coda_timestamp *ts;
-
-		while (!list_empty(&ctx->timestamp_list)) {
-			ts = list_first_entry(&ctx->timestamp_list,
-					      struct coda_timestamp, list);
-			list_del(&ts->list);
-			kfree(ts);
-		}
-		kfifo_init(&ctx->bitstream_fifo,
-			ctx->bitstream.vaddr, ctx->bitstream.size);
-		ctx->runcounter = 0;
-	}
-}
-
-static struct vb2_ops coda_qops = {
-	.queue_setup		= coda_queue_setup,
-	.buf_prepare		= coda_buf_prepare,
-	.buf_queue		= coda_buf_queue,
-	.start_streaming	= coda_start_streaming,
-	.stop_streaming		= coda_stop_streaming,
-	.wait_prepare		= vb2_ops_wait_prepare,
-	.wait_finish		= vb2_ops_wait_finish,
-};
-
-static int coda_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct coda_ctx *ctx =
-			container_of(ctrl->handler, struct coda_ctx, ctrls);
-
-	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-		 "s_ctrl: id = %d, val = %d\n", ctrl->id, ctrl->val);
-
-	switch (ctrl->id) {
-	case V4L2_CID_HFLIP:
-		if (ctrl->val)
-			ctx->params.rot_mode |= CODA_MIR_HOR;
-		else
-			ctx->params.rot_mode &= ~CODA_MIR_HOR;
-		break;
-	case V4L2_CID_VFLIP:
-		if (ctrl->val)
-			ctx->params.rot_mode |= CODA_MIR_VER;
-		else
-			ctx->params.rot_mode &= ~CODA_MIR_VER;
-		break;
-	case V4L2_CID_MPEG_VIDEO_BITRATE:
-		ctx->params.bitrate = ctrl->val / 1000;
-		break;
-	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
-		ctx->params.gop_size = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
-		ctx->params.h264_intra_qp = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:
-		ctx->params.h264_inter_qp = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
-		ctx->params.h264_min_qp = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
-		ctx->params.h264_max_qp = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:
-		ctx->params.h264_deblk_alpha = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:
-		ctx->params.h264_deblk_beta = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
-		ctx->params.h264_deblk_enabled = (ctrl->val ==
-				V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED);
-		break;
-	case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:
-		ctx->params.mpeg4_intra_qp = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:
-		ctx->params.mpeg4_inter_qp = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
-		ctx->params.slice_mode = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:
-		ctx->params.slice_max_mb = ctrl->val;
-		break;
-	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:
-		ctx->params.slice_max_bits = ctrl->val * 8;
-		break;
-	case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
-		break;
-	case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:
-		ctx->params.intra_refresh = ctrl->val;
-		break;
-	default:
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			"Invalid control, id=%d, val=%d\n",
-			ctrl->id, ctrl->val);
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static struct v4l2_ctrl_ops coda_ctrl_ops = {
-	.s_ctrl = coda_s_ctrl,
-};
-
-static int coda_ctrls_setup(struct coda_ctx *ctx)
-{
-	v4l2_ctrl_handler_init(&ctx->ctrls, 9);
-
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_HFLIP, 0, 1, 1, 0);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_VFLIP, 0, 1, 1, 0);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_BITRATE, 0, 32767000, 1, 0);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_GOP_SIZE, 1, 60, 1, 16);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP, 0, 51, 1, 25);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP, 0, 51, 1, 25);
-	if (ctx->dev->devtype->product != CODA_960) {
-		v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-			V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 0, 51, 1, 12);
-	}
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_H264_MAX_QP, 0, 51, 1, 51);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, 0, 15, 1, 0);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, 0, 15, 1, 0);
-	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE,
-		V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED, 0x0,
-		V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP, 1, 31, 1, 2);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP, 1, 31, 1, 2);
-	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE,
-		V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES, 0x0,
-		V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB, 1, 0x3fffffff, 1, 1);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES, 1, 0x3fffffff, 1, 500);
-	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_HEADER_MODE,
-		V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
-		(1 << V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE),
-		V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME);
-	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
-		V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB, 0, 1920 * 1088 / 256, 1, 0);
-
-	if (ctx->ctrls.error) {
-		v4l2_err(&ctx->dev->v4l2_dev, "control initialization error (%d)",
-			ctx->ctrls.error);
-		return -EINVAL;
-	}
-
-	return v4l2_ctrl_handler_setup(&ctx->ctrls);
-}
-
-static int coda_queue_init(void *priv, struct vb2_queue *src_vq,
-		      struct vb2_queue *dst_vq)
-{
-	struct coda_ctx *ctx = priv;
-	int ret;
-
-	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
-	src_vq->io_modes = VB2_DMABUF | VB2_MMAP | VB2_USERPTR;
-	src_vq->drv_priv = ctx;
-	src_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
-	src_vq->ops = &coda_qops;
-	src_vq->mem_ops = &vb2_dma_contig_memops;
-	src_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
-	src_vq->lock = &ctx->dev->dev_mutex;
-
-	ret = vb2_queue_init(src_vq);
-	if (ret)
-		return ret;
-
-	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	dst_vq->io_modes = VB2_DMABUF | VB2_MMAP | VB2_USERPTR;
-	dst_vq->drv_priv = ctx;
-	dst_vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
-	dst_vq->ops = &coda_qops;
-	dst_vq->mem_ops = &vb2_dma_contig_memops;
-	dst_vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
-	dst_vq->lock = &ctx->dev->dev_mutex;
-
-	return vb2_queue_init(dst_vq);
-}
-
-static int coda_next_free_instance(struct coda_dev *dev)
-{
-	int idx = ffz(dev->instance_mask);
-
-	if ((idx < 0) ||
-	    (dev->devtype->product == CODA_DX6 && idx > CODADX6_MAX_INSTANCES))
-		return -EBUSY;
-
-	return idx;
-}
-
-static int coda_open(struct file *file)
-{
-	struct coda_dev *dev = video_drvdata(file);
-	struct coda_ctx *ctx = NULL;
-	char *name;
-	int ret;
-	int idx;
-
-	ctx = kzalloc(sizeof *ctx, GFP_KERNEL);
-	if (!ctx)
-		return -ENOMEM;
-
-	idx = coda_next_free_instance(dev);
-	if (idx < 0) {
-		ret = idx;
-		goto err_coda_max;
-	}
-	set_bit(idx, &dev->instance_mask);
-
-	name = kasprintf(GFP_KERNEL, "context%d", idx);
-	ctx->debugfs_entry = debugfs_create_dir(name, dev->debugfs_root);
-	kfree(name);
-
-	init_completion(&ctx->completion);
-	INIT_WORK(&ctx->pic_run_work, coda_pic_run_work);
-	INIT_WORK(&ctx->seq_end_work, coda_seq_end_work);
-	v4l2_fh_init(&ctx->fh, video_devdata(file));
-	file->private_data = &ctx->fh;
-	v4l2_fh_add(&ctx->fh);
-	ctx->dev = dev;
-	ctx->idx = idx;
-	switch (dev->devtype->product) {
-	case CODA_7541:
-	case CODA_960:
-		ctx->reg_idx = 0;
-		break;
-	default:
-		ctx->reg_idx = idx;
-	}
-
-	/* Power up and upload firmware if necessary */
-	ret = pm_runtime_get_sync(&dev->plat_dev->dev);
-	if (ret < 0) {
-		v4l2_err(&dev->v4l2_dev, "failed to power up: %d\n", ret);
-		goto err_pm_get;
-	}
-
-	ret = clk_prepare_enable(dev->clk_per);
-	if (ret)
-		goto err_clk_per;
-
-	ret = clk_prepare_enable(dev->clk_ahb);
-	if (ret)
-		goto err_clk_ahb;
-
-	set_default_params(ctx);
-	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx,
-					 &coda_queue_init);
-	if (IS_ERR(ctx->fh.m2m_ctx)) {
-		ret = PTR_ERR(ctx->fh.m2m_ctx);
-
-		v4l2_err(&dev->v4l2_dev, "%s return error (%d)\n",
-			 __func__, ret);
-		goto err_ctx_init;
-	}
-
-	ret = coda_ctrls_setup(ctx);
-	if (ret) {
-		v4l2_err(&dev->v4l2_dev, "failed to setup coda controls\n");
-		goto err_ctrls_setup;
-	}
-
-	ctx->fh.ctrl_handler = &ctx->ctrls;
-
-	ret = coda_alloc_context_buf(ctx, &ctx->parabuf, CODA_PARA_BUF_SIZE,
-				     "parabuf");
-	if (ret < 0) {
-		v4l2_err(&dev->v4l2_dev, "failed to allocate parabuf");
-		goto err_dma_alloc;
-	}
-
-	ctx->bitstream.size = CODA_MAX_FRAME_SIZE;
-	ctx->bitstream.vaddr = dma_alloc_writecombine(&dev->plat_dev->dev,
-			ctx->bitstream.size, &ctx->bitstream.paddr, GFP_KERNEL);
-	if (!ctx->bitstream.vaddr) {
-		v4l2_err(&dev->v4l2_dev, "failed to allocate bitstream ringbuffer");
-		ret = -ENOMEM;
-		goto err_dma_writecombine;
-	}
-	kfifo_init(&ctx->bitstream_fifo,
-		ctx->bitstream.vaddr, ctx->bitstream.size);
-	mutex_init(&ctx->bitstream_mutex);
-	mutex_init(&ctx->buffer_mutex);
-	INIT_LIST_HEAD(&ctx->timestamp_list);
-
-	coda_lock(ctx);
-	list_add(&ctx->list, &dev->instances);
-	coda_unlock(ctx);
-
-	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "Created instance %d (%p)\n",
-		 ctx->idx, ctx);
-
-	return 0;
-
-err_dma_writecombine:
-	coda_free_context_buffers(ctx);
-	if (ctx->dev->devtype->product == CODA_DX6)
-		coda_free_aux_buf(dev, &ctx->workbuf);
-	coda_free_aux_buf(dev, &ctx->parabuf);
-err_dma_alloc:
-	v4l2_ctrl_handler_free(&ctx->ctrls);
-err_ctrls_setup:
-	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
-err_ctx_init:
-	clk_disable_unprepare(dev->clk_ahb);
-err_clk_ahb:
-	clk_disable_unprepare(dev->clk_per);
-err_clk_per:
-	pm_runtime_put_sync(&dev->plat_dev->dev);
-err_pm_get:
-	v4l2_fh_del(&ctx->fh);
-	v4l2_fh_exit(&ctx->fh);
-	clear_bit(ctx->idx, &dev->instance_mask);
-err_coda_max:
-	kfree(ctx);
-	return ret;
-}
-
-static int coda_release(struct file *file)
-{
-	struct coda_dev *dev = video_drvdata(file);
-	struct coda_ctx *ctx = fh_to_ctx(file->private_data);
-
-	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "Releasing instance %p\n",
-		 ctx);
-
-	debugfs_remove_recursive(ctx->debugfs_entry);
-
-	/* If this instance is running, call .job_abort and wait for it to end */
-	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
-
-	/* In case the instance was not running, we still need to call SEQ_END */
-	if (ctx->initialized) {
-		queue_work(dev->workqueue, &ctx->seq_end_work);
-		flush_work(&ctx->seq_end_work);
-	}
-
-	coda_free_framebuffers(ctx);
-
-	coda_lock(ctx);
-	list_del(&ctx->list);
-	coda_unlock(ctx);
-
-	dma_free_writecombine(&dev->plat_dev->dev, ctx->bitstream.size,
-		ctx->bitstream.vaddr, ctx->bitstream.paddr);
-	coda_free_context_buffers(ctx);
-	if (ctx->dev->devtype->product == CODA_DX6)
-		coda_free_aux_buf(dev, &ctx->workbuf);
-
-	coda_free_aux_buf(dev, &ctx->parabuf);
-	v4l2_ctrl_handler_free(&ctx->ctrls);
-	clk_disable_unprepare(dev->clk_ahb);
-	clk_disable_unprepare(dev->clk_per);
-	pm_runtime_put_sync(&dev->plat_dev->dev);
-	v4l2_fh_del(&ctx->fh);
-	v4l2_fh_exit(&ctx->fh);
-	clear_bit(ctx->idx, &dev->instance_mask);
-	kfree(ctx);
-
-	return 0;
-}
-
-static const struct v4l2_file_operations coda_fops = {
-	.owner		= THIS_MODULE,
-	.open		= coda_open,
-	.release	= coda_release,
-	.poll		= v4l2_m2m_fop_poll,
-	.unlocked_ioctl	= video_ioctl2,
-	.mmap		= v4l2_m2m_fop_mmap,
-};
-
-static void coda_finish_decode(struct coda_ctx *ctx)
-{
-	struct coda_dev *dev = ctx->dev;
-	struct coda_q_data *q_data_src;
-	struct coda_q_data *q_data_dst;
-	struct vb2_buffer *dst_buf;
-	struct coda_timestamp *ts;
-	int width, height;
-	int decoded_idx;
-	int display_idx;
-	u32 src_fourcc;
-	int success;
-	u32 err_mb;
-	u32 val;
-
-	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
-
-	/* Update kfifo out pointer from coda bitstream read pointer */
-	coda_kfifo_sync_from_device(ctx);
-
-	/*
-	 * in stream-end mode, the read pointer can overshoot the write pointer
-	 * by up to 512 bytes
-	 */
-	if (ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) {
-		if (coda_get_bitstream_payload(ctx) >= 0x100000 - 512)
-			kfifo_init(&ctx->bitstream_fifo,
-				ctx->bitstream.vaddr, ctx->bitstream.size);
-	}
-
-	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
-	src_fourcc = q_data_src->fourcc;
-
-	val = coda_read(dev, CODA_RET_DEC_PIC_SUCCESS);
-	if (val != 1)
-		pr_err("DEC_PIC_SUCCESS = %d\n", val);
-
-	success = val & 0x1;
-	if (!success)
-		v4l2_err(&dev->v4l2_dev, "decode failed\n");
-
-	if (src_fourcc == V4L2_PIX_FMT_H264) {
-		if (val & (1 << 3))
-			v4l2_err(&dev->v4l2_dev,
-				 "insufficient PS buffer space (%d bytes)\n",
-				 ctx->psbuf.size);
-		if (val & (1 << 2))
-			v4l2_err(&dev->v4l2_dev,
-				 "insufficient slice buffer space (%d bytes)\n",
-				 ctx->slicebuf.size);
-	}
-
-	val = coda_read(dev, CODA_RET_DEC_PIC_SIZE);
-	width = (val >> 16) & 0xffff;
-	height = val & 0xffff;
-
-	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
-
-	/* frame crop information */
-	if (src_fourcc == V4L2_PIX_FMT_H264) {
-		u32 left_right;
-		u32 top_bottom;
-
-		left_right = coda_read(dev, CODA_RET_DEC_PIC_CROP_LEFT_RIGHT);
-		top_bottom = coda_read(dev, CODA_RET_DEC_PIC_CROP_TOP_BOTTOM);
-
-		if (left_right == 0xffffffff && top_bottom == 0xffffffff) {
-			/* Keep current crop information */
-		} else {
-			struct v4l2_rect *rect = &q_data_dst->rect;
-
-			rect->left = left_right >> 16 & 0xffff;
-			rect->top = top_bottom >> 16 & 0xffff;
-			rect->width = width - rect->left -
-				      (left_right & 0xffff);
-			rect->height = height - rect->top -
-				       (top_bottom & 0xffff);
-		}
-	} else {
-		/* no cropping */
-	}
-
-	err_mb = coda_read(dev, CODA_RET_DEC_PIC_ERR_MB);
-	if (err_mb > 0)
-		v4l2_err(&dev->v4l2_dev,
-			 "errors in %d macroblocks\n", err_mb);
-
-	if (dev->devtype->product == CODA_7541) {
-		val = coda_read(dev, CODA_RET_DEC_PIC_OPTION);
-		if (val == 0) {
-			/* not enough bitstream data */
-			v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-				 "prescan failed: %d\n", val);
-			ctx->hold = true;
-			return;
-		}
-	}
-
-	ctx->frm_dis_flg = coda_read(dev, CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
-
-	/*
-	 * The previous display frame was copied out by the rotator,
-	 * now it can be overwritten again
-	 */
-	if (ctx->display_idx >= 0 &&
-	    ctx->display_idx < ctx->num_internal_frames) {
-		ctx->frm_dis_flg &= ~(1 << ctx->display_idx);
-		coda_write(dev, ctx->frm_dis_flg,
-				CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
-	}
-
-	/*
-	 * The index of the last decoded frame, not necessarily in
-	 * display order, and the index of the next display frame.
-	 * The latter could have been decoded in a previous run.
-	 */
-	decoded_idx = coda_read(dev, CODA_RET_DEC_PIC_CUR_IDX);
-	display_idx = coda_read(dev, CODA_RET_DEC_PIC_FRAME_IDX);
-
-	if (decoded_idx == -1) {
-		/* no frame was decoded, but we might have a display frame */
-		if (display_idx >= 0 && display_idx < ctx->num_internal_frames)
-			ctx->sequence_offset++;
-		else if (ctx->display_idx < 0)
-			ctx->hold = true;
-	} else if (decoded_idx == -2) {
-		/* no frame was decoded, we still return the remaining buffers */
-	} else if (decoded_idx < 0 || decoded_idx >= ctx->num_internal_frames) {
-		v4l2_err(&dev->v4l2_dev,
-			 "decoded frame index out of range: %d\n", decoded_idx);
-	} else {
-		ts = list_first_entry(&ctx->timestamp_list,
-				      struct coda_timestamp, list);
-		list_del(&ts->list);
-		val = coda_read(dev, CODA_RET_DEC_PIC_FRAME_NUM) - 1;
-		val -= ctx->sequence_offset;
-		if (val != (ts->sequence & 0xffff)) {
-			v4l2_err(&dev->v4l2_dev,
-				 "sequence number mismatch (%d(%d) != %d)\n",
-				 val, ctx->sequence_offset, ts->sequence);
-		}
-		ctx->frame_timestamps[decoded_idx] = *ts;
-		kfree(ts);
-
-		val = coda_read(dev, CODA_RET_DEC_PIC_TYPE) & 0x7;
-		if (val == 0)
-			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_KEYFRAME;
-		else if (val == 1)
-			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_PFRAME;
-		else
-			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_BFRAME;
-
-		ctx->frame_errors[decoded_idx] = err_mb;
-	}
-
-	if (display_idx == -1) {
-		/*
-		 * no more frames to be decoded, but there could still
-		 * be rotator output to dequeue
-		 */
-		ctx->hold = true;
-	} else if (display_idx == -3) {
-		/* possibly prescan failure */
-	} else if (display_idx < 0 || display_idx >= ctx->num_internal_frames) {
-		v4l2_err(&dev->v4l2_dev,
-			 "presentation frame index out of range: %d\n",
-			 display_idx);
-	}
-
-	/* If a frame was copied out, return it */
-	if (ctx->display_idx >= 0 &&
-	    ctx->display_idx < ctx->num_internal_frames) {
-		dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
-		dst_buf->v4l2_buf.sequence = ctx->osequence++;
-
-		dst_buf->v4l2_buf.flags &= ~(V4L2_BUF_FLAG_KEYFRAME |
-					     V4L2_BUF_FLAG_PFRAME |
-					     V4L2_BUF_FLAG_BFRAME);
-		dst_buf->v4l2_buf.flags |= ctx->frame_types[ctx->display_idx];
-		ts = &ctx->frame_timestamps[ctx->display_idx];
-		dst_buf->v4l2_buf.timecode = ts->timecode;
-		dst_buf->v4l2_buf.timestamp = ts->timestamp;
-
-		vb2_set_plane_payload(dst_buf, 0, width * height * 3 / 2);
-
-		v4l2_m2m_buf_done(dst_buf, ctx->frame_errors[display_idx] ?
-				  VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
-
-		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-			"job finished: decoding frame (%d) (%s)\n",
-			dst_buf->v4l2_buf.sequence,
-			(dst_buf->v4l2_buf.flags & V4L2_BUF_FLAG_KEYFRAME) ?
-			"KEYFRAME" : "PFRAME");
-	} else {
-		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-			"job finished: no frame decoded\n");
-	}
-
-	/* The rotator will copy the current display frame next time */
-	ctx->display_idx = display_idx;
-}
-
-static void coda_finish_encode(struct coda_ctx *ctx)
-{
-	struct vb2_buffer *src_buf, *dst_buf;
-	struct coda_dev *dev = ctx->dev;
-	u32 wr_ptr, start_ptr;
-
-	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
-	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
-
-	/* Get results from the coda */
-	start_ptr = coda_read(dev, CODA_CMD_ENC_PIC_BB_START);
-	wr_ptr = coda_read(dev, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
-
-	/* Calculate bytesused field */
-	if (dst_buf->v4l2_buf.sequence == 0) {
-		vb2_set_plane_payload(dst_buf, 0, wr_ptr - start_ptr +
-					ctx->vpu_header_size[0] +
-					ctx->vpu_header_size[1] +
-					ctx->vpu_header_size[2]);
-	} else {
-		vb2_set_plane_payload(dst_buf, 0, wr_ptr - start_ptr);
-	}
-
-	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev, "frame size = %u\n",
-		 wr_ptr - start_ptr);
-
-	coda_read(dev, CODA_RET_ENC_PIC_SLICE_NUM);
-	coda_read(dev, CODA_RET_ENC_PIC_FLAG);
-
-	if (coda_read(dev, CODA_RET_ENC_PIC_TYPE) == 0) {
-		dst_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
-		dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_PFRAME;
-	} else {
-		dst_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_PFRAME;
-		dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_KEYFRAME;
-	}
-
-	dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
-	dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
-	dst_buf->v4l2_buf.flags |=
-		src_buf->v4l2_buf.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
-	dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
-
-	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
-
-	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
-	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
-
-	ctx->gopcounter--;
-	if (ctx->gopcounter < 0)
-		ctx->gopcounter = ctx->params.gop_size - 1;
-
-	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
-		"job finished: encoding frame (%d) (%s)\n",
-		dst_buf->v4l2_buf.sequence,
-		(dst_buf->v4l2_buf.flags & V4L2_BUF_FLAG_KEYFRAME) ?
-		"KEYFRAME" : "PFRAME");
-}
-
-static irqreturn_t coda_irq_handler(int irq, void *data)
-{
-	struct coda_dev *dev = data;
-	struct coda_ctx *ctx;
-
-	/* read status register to attend the IRQ */
-	coda_read(dev, CODA_REG_BIT_INT_STATUS);
-	coda_write(dev, CODA_REG_BIT_INT_CLEAR_SET,
-		      CODA_REG_BIT_INT_CLEAR);
-
-	ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
-	if (ctx == NULL) {
-		v4l2_err(&dev->v4l2_dev, "Instance released before the end of transaction\n");
-		mutex_unlock(&dev->coda_mutex);
-		return IRQ_HANDLED;
-	}
-
-	if (ctx->aborting) {
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			 "task has been aborted\n");
-	}
-
-	if (coda_isbusy(ctx->dev)) {
-		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
-			 "coda is still busy!!!!\n");
-		return IRQ_NONE;
-	}
-
-	complete(&ctx->completion);
-
-	return IRQ_HANDLED;
-}
-
-static u32 coda_supported_firmwares[] = {
-	CODA_FIRMWARE_VERNUM(CODA_DX6, 2, 2, 5),
-	CODA_FIRMWARE_VERNUM(CODA_7541, 1, 4, 50),
-	CODA_FIRMWARE_VERNUM(CODA_960, 2, 1, 5),
-};
-
-static bool coda_firmware_supported(u32 vernum)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(coda_supported_firmwares); i++)
-		if (vernum == coda_supported_firmwares[i])
-			return true;
-	return false;
-}
-
-static int coda_hw_init(struct coda_dev *dev)
-{
-	u32 data;
-	u16 *p;
-	int i, ret;
-
-	ret = clk_prepare_enable(dev->clk_per);
-	if (ret)
-		goto err_clk_per;
-
-	ret = clk_prepare_enable(dev->clk_ahb);
-	if (ret)
-		goto err_clk_ahb;
-
-	if (dev->rstc)
-		reset_control_reset(dev->rstc);
-
-	/*
-	 * Copy the first CODA_ISRAM_SIZE in the internal SRAM.
-	 * The 16-bit chars in the code buffer are in memory access
-	 * order, re-sort them to CODA order for register download.
-	 * Data in this SRAM survives a reboot.
-	 */
-	p = (u16 *)dev->codebuf.vaddr;
-	if (dev->devtype->product == CODA_DX6) {
-		for (i = 0; i < (CODA_ISRAM_SIZE / 2); i++)  {
-			data = CODA_DOWN_ADDRESS_SET(i) |
-				CODA_DOWN_DATA_SET(p[i ^ 1]);
-			coda_write(dev, data, CODA_REG_BIT_CODE_DOWN);
-		}
-	} else {
-		for (i = 0; i < (CODA_ISRAM_SIZE / 2); i++) {
-			data = CODA_DOWN_ADDRESS_SET(i) |
-				CODA_DOWN_DATA_SET(p[round_down(i, 4) +
-							3 - (i % 4)]);
-			coda_write(dev, data, CODA_REG_BIT_CODE_DOWN);
-		}
-	}
-
-	/* Clear registers */
-	for (i = 0; i < 64; i++)
-		coda_write(dev, 0, CODA_REG_BIT_CODE_BUF_ADDR + i * 4);
-
-	/* Tell the BIT where to find everything it needs */
-	if (dev->devtype->product == CODA_960 ||
-	    dev->devtype->product == CODA_7541) {
-		coda_write(dev, dev->tempbuf.paddr,
-				CODA_REG_BIT_TEMP_BUF_ADDR);
-		coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
-	} else {
-		coda_write(dev, dev->workbuf.paddr,
-			      CODA_REG_BIT_WORK_BUF_ADDR);
-	}
-	coda_write(dev, dev->codebuf.paddr,
-		      CODA_REG_BIT_CODE_BUF_ADDR);
-	coda_write(dev, 0, CODA_REG_BIT_CODE_RUN);
-
-	/* Set default values */
-	switch (dev->devtype->product) {
-	case CODA_DX6:
-		coda_write(dev, CODADX6_STREAM_BUF_PIC_FLUSH, CODA_REG_BIT_STREAM_CTRL);
-		break;
-	default:
-		coda_write(dev, CODA7_STREAM_BUF_PIC_FLUSH, CODA_REG_BIT_STREAM_CTRL);
-	}
-	if (dev->devtype->product == CODA_960)
-		coda_write(dev, 1 << 12, CODA_REG_BIT_FRAME_MEM_CTRL);
-	else
-		coda_write(dev, 0, CODA_REG_BIT_FRAME_MEM_CTRL);
-
-	if (dev->devtype->product != CODA_DX6)
-		coda_write(dev, 0, CODA7_REG_BIT_AXI_SRAM_USE);
-
-	coda_write(dev, CODA_INT_INTERRUPT_ENABLE,
-		      CODA_REG_BIT_INT_ENABLE);
-
-	/* Reset VPU and start processor */
-	data = coda_read(dev, CODA_REG_BIT_CODE_RESET);
-	data |= CODA_REG_RESET_ENABLE;
-	coda_write(dev, data, CODA_REG_BIT_CODE_RESET);
-	udelay(10);
-	data &= ~CODA_REG_RESET_ENABLE;
-	coda_write(dev, data, CODA_REG_BIT_CODE_RESET);
-	coda_write(dev, CODA_REG_RUN_ENABLE, CODA_REG_BIT_CODE_RUN);
-
-	clk_disable_unprepare(dev->clk_ahb);
-	clk_disable_unprepare(dev->clk_per);
-
-	return 0;
-
-err_clk_ahb:
-	clk_disable_unprepare(dev->clk_per);
-err_clk_per:
-	return ret;
-}
-
-static int coda_check_firmware(struct coda_dev *dev)
-{
-	u16 product, major, minor, release;
-	u32 data;
-	int ret;
-
-	ret = clk_prepare_enable(dev->clk_per);
-	if (ret)
-		goto err_clk_per;
-
-	ret = clk_prepare_enable(dev->clk_ahb);
-	if (ret)
-		goto err_clk_ahb;
-
-	coda_write(dev, 0, CODA_CMD_FIRMWARE_VERNUM);
-	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
-	coda_write(dev, 0, CODA_REG_BIT_RUN_INDEX);
-	coda_write(dev, 0, CODA_REG_BIT_RUN_COD_STD);
-	coda_write(dev, CODA_COMMAND_FIRMWARE_GET, CODA_REG_BIT_RUN_COMMAND);
-	if (coda_wait_timeout(dev)) {
-		v4l2_err(&dev->v4l2_dev, "firmware get command error\n");
-		ret = -EIO;
-		goto err_run_cmd;
-	}
-
-	if (dev->devtype->product == CODA_960) {
-		data = coda_read(dev, CODA9_CMD_FIRMWARE_CODE_REV);
-		v4l2_info(&dev->v4l2_dev, "Firmware code revision: %d\n",
-			  data);
-	}
-
-	/* Check we are compatible with the loaded firmware */
-	data = coda_read(dev, CODA_CMD_FIRMWARE_VERNUM);
-	product = CODA_FIRMWARE_PRODUCT(data);
-	major = CODA_FIRMWARE_MAJOR(data);
-	minor = CODA_FIRMWARE_MINOR(data);
-	release = CODA_FIRMWARE_RELEASE(data);
-
-	clk_disable_unprepare(dev->clk_per);
-	clk_disable_unprepare(dev->clk_ahb);
-
-	if (product != dev->devtype->product) {
-		v4l2_err(&dev->v4l2_dev, "Wrong firmware. Hw: %s, Fw: %s,"
-			 " Version: %u.%u.%u\n",
-			 coda_product_name(dev->devtype->product),
-			 coda_product_name(product), major, minor, release);
-		return -EINVAL;
-	}
-
-	v4l2_info(&dev->v4l2_dev, "Initialized %s.\n",
-		  coda_product_name(product));
-
-	if (coda_firmware_supported(data)) {
-		v4l2_info(&dev->v4l2_dev, "Firmware version: %u.%u.%u\n",
-			  major, minor, release);
-	} else {
-		v4l2_warn(&dev->v4l2_dev, "Unsupported firmware version: "
-			  "%u.%u.%u\n", major, minor, release);
-	}
-
-	return 0;
-
-err_run_cmd:
-	clk_disable_unprepare(dev->clk_ahb);
-err_clk_ahb:
-	clk_disable_unprepare(dev->clk_per);
-err_clk_per:
-	return ret;
-}
-
-static void coda_fw_callback(const struct firmware *fw, void *context)
-{
-	struct coda_dev *dev = context;
-	struct platform_device *pdev = dev->plat_dev;
-	int ret;
-
-	if (!fw) {
-		v4l2_err(&dev->v4l2_dev, "firmware request failed\n");
-		return;
-	}
-
-	/* allocate auxiliary per-device code buffer for the BIT processor */
-	ret = coda_alloc_aux_buf(dev, &dev->codebuf, fw->size, "codebuf",
-				 dev->debugfs_root);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to allocate code buffer\n");
-		return;
-	}
-
-	/* Copy the whole firmware image to the code buffer */
-	memcpy(dev->codebuf.vaddr, fw->data, fw->size);
-	release_firmware(fw);
-
-	if (pm_runtime_enabled(&pdev->dev) && pdev->dev.pm_domain) {
-		/*
-		 * Enabling power temporarily will cause coda_hw_init to be
-		 * called via coda_runtime_resume by the pm domain.
-		 */
-		ret = pm_runtime_get_sync(&dev->plat_dev->dev);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "failed to power on: %d\n",
-				 ret);
-			return;
-		}
-
-		ret = coda_check_firmware(dev);
-		if (ret < 0)
-			return;
-
-		pm_runtime_put_sync(&dev->plat_dev->dev);
-	} else {
-		/*
-		 * If runtime pm is disabled or pm_domain is not set,
-		 * initialize once manually.
-		 */
-		ret = coda_hw_init(dev);
-		if (ret < 0) {
-			v4l2_err(&dev->v4l2_dev, "HW initialization failed\n");
-			return;
-		}
-
-		ret = coda_check_firmware(dev);
-		if (ret < 0)
-			return;
-	}
-
-	dev->vfd.fops	= &coda_fops,
-	dev->vfd.ioctl_ops	= &coda_ioctl_ops;
-	dev->vfd.release	= video_device_release_empty,
-	dev->vfd.lock	= &dev->dev_mutex;
-	dev->vfd.v4l2_dev	= &dev->v4l2_dev;
-	dev->vfd.vfl_dir	= VFL_DIR_M2M;
-	snprintf(dev->vfd.name, sizeof(dev->vfd.name), "%s", CODA_NAME);
-	video_set_drvdata(&dev->vfd, dev);
-
-	dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
-	if (IS_ERR(dev->alloc_ctx)) {
-		v4l2_err(&dev->v4l2_dev, "Failed to alloc vb2 context\n");
-		return;
-	}
-
-	dev->m2m_dev = v4l2_m2m_init(&coda_m2m_ops);
-	if (IS_ERR(dev->m2m_dev)) {
-		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
-		goto rel_ctx;
-	}
-
-	ret = video_register_device(&dev->vfd, VFL_TYPE_GRABBER, 0);
-	if (ret) {
-		v4l2_err(&dev->v4l2_dev, "Failed to register video device\n");
-		goto rel_m2m;
-	}
-	v4l2_info(&dev->v4l2_dev, "codec registered as /dev/video%d\n",
-		  dev->vfd.num);
-
-	return;
-
-rel_m2m:
-	v4l2_m2m_release(dev->m2m_dev);
-rel_ctx:
-	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
-}
-
-static int coda_firmware_request(struct coda_dev *dev)
-{
-	char *fw = dev->devtype->firmware;
-
-	dev_dbg(&dev->plat_dev->dev, "requesting firmware '%s' for %s\n", fw,
-		coda_product_name(dev->devtype->product));
-
-	return request_firmware_nowait(THIS_MODULE, true,
-		fw, &dev->plat_dev->dev, GFP_KERNEL, dev, coda_fw_callback);
-}
-
-enum coda_platform {
-	CODA_IMX27,
-	CODA_IMX53,
-	CODA_IMX6Q,
-	CODA_IMX6DL,
-};
-
-static const struct coda_devtype coda_devdata[] = {
-	[CODA_IMX27] = {
-		.firmware     = "v4l-codadx6-imx27.bin",
-		.product      = CODA_DX6,
-		.codecs       = codadx6_codecs,
-		.num_codecs   = ARRAY_SIZE(codadx6_codecs),
-		.workbuf_size = 288 * 1024 + FMO_SLICE_SAVE_BUF_SIZE * 8 * 1024,
-		.iram_size    = 0xb000,
-	},
-	[CODA_IMX53] = {
-		.firmware     = "v4l-coda7541-imx53.bin",
-		.product      = CODA_7541,
-		.codecs       = coda7_codecs,
-		.num_codecs   = ARRAY_SIZE(coda7_codecs),
-		.workbuf_size = 128 * 1024,
-		.tempbuf_size = 304 * 1024,
-		.iram_size    = 0x14000,
-	},
-	[CODA_IMX6Q] = {
-		.firmware     = "v4l-coda960-imx6q.bin",
-		.product      = CODA_960,
-		.codecs       = coda9_codecs,
-		.num_codecs   = ARRAY_SIZE(coda9_codecs),
-		.workbuf_size = 80 * 1024,
-		.tempbuf_size = 204 * 1024,
-		.iram_size    = 0x21000,
-	},
-	[CODA_IMX6DL] = {
-		.firmware     = "v4l-coda960-imx6dl.bin",
-		.product      = CODA_960,
-		.codecs       = coda9_codecs,
-		.num_codecs   = ARRAY_SIZE(coda9_codecs),
-		.workbuf_size = 80 * 1024,
-		.tempbuf_size = 204 * 1024,
-		.iram_size    = 0x20000,
-	},
-};
-
-static struct platform_device_id coda_platform_ids[] = {
-	{ .name = "coda-imx27", .driver_data = CODA_IMX27 },
-	{ .name = "coda-imx53", .driver_data = CODA_IMX53 },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(platform, coda_platform_ids);
-
-#ifdef CONFIG_OF
-static const struct of_device_id coda_dt_ids[] = {
-	{ .compatible = "fsl,imx27-vpu", .data = &coda_devdata[CODA_IMX27] },
-	{ .compatible = "fsl,imx53-vpu", .data = &coda_devdata[CODA_IMX53] },
-	{ .compatible = "fsl,imx6q-vpu", .data = &coda_devdata[CODA_IMX6Q] },
-	{ .compatible = "fsl,imx6dl-vpu", .data = &coda_devdata[CODA_IMX6DL] },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, coda_dt_ids);
-#endif
-
-static int coda_probe(struct platform_device *pdev)
-{
-	const struct of_device_id *of_id =
-			of_match_device(of_match_ptr(coda_dt_ids), &pdev->dev);
-	const struct platform_device_id *pdev_id;
-	struct coda_platform_data *pdata = pdev->dev.platform_data;
-	struct device_node *np = pdev->dev.of_node;
-	struct gen_pool *pool;
-	struct coda_dev *dev;
-	struct resource *res;
-	int ret, irq;
-
-	dev = devm_kzalloc(&pdev->dev, sizeof *dev, GFP_KERNEL);
-	if (!dev) {
-		dev_err(&pdev->dev, "Not enough memory for %s\n",
-			CODA_NAME);
-		return -ENOMEM;
-	}
-
-	spin_lock_init(&dev->irqlock);
-	INIT_LIST_HEAD(&dev->instances);
-
-	dev->plat_dev = pdev;
-	dev->clk_per = devm_clk_get(&pdev->dev, "per");
-	if (IS_ERR(dev->clk_per)) {
-		dev_err(&pdev->dev, "Could not get per clock\n");
-		return PTR_ERR(dev->clk_per);
-	}
-
-	dev->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
-	if (IS_ERR(dev->clk_ahb)) {
-		dev_err(&pdev->dev, "Could not get ahb clock\n");
-		return PTR_ERR(dev->clk_ahb);
-	}
-
-	/* Get  memory for physical registers */
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	dev->regs_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(dev->regs_base))
-		return PTR_ERR(dev->regs_base);
-
-	/* IRQ */
-	irq = platform_get_irq(pdev, 0);
-	if (irq < 0) {
-		dev_err(&pdev->dev, "failed to get irq resource\n");
-		return irq;
-	}
-
-	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL, coda_irq_handler,
-			IRQF_ONESHOT, dev_name(&pdev->dev), dev);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "failed to request irq: %d\n", ret);
-		return ret;
-	}
-
-	dev->rstc = devm_reset_control_get_optional(&pdev->dev, NULL);
-	if (IS_ERR(dev->rstc)) {
-		ret = PTR_ERR(dev->rstc);
-		if (ret == -ENOENT || ret == -ENOSYS) {
-			dev->rstc = NULL;
-		} else {
-			dev_err(&pdev->dev, "failed get reset control: %d\n", ret);
-			return ret;
-		}
-	}
-
-	/* Get IRAM pool from device tree or platform data */
-	pool = of_get_named_gen_pool(np, "iram", 0);
-	if (!pool && pdata)
-		pool = dev_get_gen_pool(pdata->iram_dev);
-	if (!pool) {
-		dev_err(&pdev->dev, "iram pool not available\n");
-		return -ENOMEM;
-	}
-	dev->iram_pool = pool;
-
-	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
-	if (ret)
-		return ret;
-
-	mutex_init(&dev->dev_mutex);
-	mutex_init(&dev->coda_mutex);
-
-	pdev_id = of_id ? of_id->data : platform_get_device_id(pdev);
-
-	if (of_id) {
-		dev->devtype = of_id->data;
-	} else if (pdev_id) {
-		dev->devtype = &coda_devdata[pdev_id->driver_data];
-	} else {
-		v4l2_device_unregister(&dev->v4l2_dev);
-		return -EINVAL;
-	}
-
-	dev->debugfs_root = debugfs_create_dir("coda", NULL);
-	if (!dev->debugfs_root)
-		dev_warn(&pdev->dev, "failed to create debugfs root\n");
-
-	/* allocate auxiliary per-device buffers for the BIT processor */
-	if (dev->devtype->product == CODA_DX6) {
-		ret = coda_alloc_aux_buf(dev, &dev->workbuf,
-					 dev->devtype->workbuf_size, "workbuf",
-					 dev->debugfs_root);
-		if (ret < 0) {
-			dev_err(&pdev->dev, "failed to allocate work buffer\n");
-			v4l2_device_unregister(&dev->v4l2_dev);
-			return ret;
-		}
-	}
-
-	if (dev->devtype->tempbuf_size) {
-		ret = coda_alloc_aux_buf(dev, &dev->tempbuf,
-					 dev->devtype->tempbuf_size, "tempbuf",
-					 dev->debugfs_root);
-		if (ret < 0) {
-			dev_err(&pdev->dev, "failed to allocate temp buffer\n");
-			v4l2_device_unregister(&dev->v4l2_dev);
-			return ret;
-		}
-	}
-
-	dev->iram.size = dev->devtype->iram_size;
-	dev->iram.vaddr = gen_pool_dma_alloc(dev->iram_pool, dev->iram.size,
-					     &dev->iram.paddr);
-	if (!dev->iram.vaddr) {
-		dev_err(&pdev->dev, "unable to alloc iram\n");
-		return -ENOMEM;
-	}
-
-	dev->iram.blob.data = dev->iram.vaddr;
-	dev->iram.blob.size = dev->iram.size;
-	dev->iram.dentry = debugfs_create_blob("iram", 0644, dev->debugfs_root,
-					       &dev->iram.blob);
-
-	dev->workqueue = alloc_workqueue("coda", WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
-	if (!dev->workqueue) {
-		dev_err(&pdev->dev, "unable to alloc workqueue\n");
-		return -ENOMEM;
-	}
-
-	platform_set_drvdata(pdev, dev);
-
-	pm_runtime_enable(&pdev->dev);
-
-	return coda_firmware_request(dev);
-}
-
-static int coda_remove(struct platform_device *pdev)
-{
-	struct coda_dev *dev = platform_get_drvdata(pdev);
-
-	video_unregister_device(&dev->vfd);
-	if (dev->m2m_dev)
-		v4l2_m2m_release(dev->m2m_dev);
-	pm_runtime_disable(&pdev->dev);
-	if (dev->alloc_ctx)
-		vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
-	v4l2_device_unregister(&dev->v4l2_dev);
-	destroy_workqueue(dev->workqueue);
-	if (dev->iram.vaddr)
-		gen_pool_free(dev->iram_pool, (unsigned long)dev->iram.vaddr,
-			      dev->iram.size);
-	coda_free_aux_buf(dev, &dev->codebuf);
-	coda_free_aux_buf(dev, &dev->tempbuf);
-	coda_free_aux_buf(dev, &dev->workbuf);
-	debugfs_remove_recursive(dev->debugfs_root);
-	return 0;
-}
-
-#ifdef CONFIG_PM_RUNTIME
-static int coda_runtime_resume(struct device *dev)
-{
-	struct coda_dev *cdev = dev_get_drvdata(dev);
-	int ret = 0;
-
-	if (dev->pm_domain) {
-		ret = coda_hw_init(cdev);
-		if (ret)
-			v4l2_err(&cdev->v4l2_dev, "HW initialization failed\n");
-	}
-
-	return ret;
-}
-#endif
-
-static const struct dev_pm_ops coda_pm_ops = {
-	SET_RUNTIME_PM_OPS(NULL, coda_runtime_resume, NULL)
-};
-
-static struct platform_driver coda_driver = {
-	.probe	= coda_probe,
-	.remove	= coda_remove,
-	.driver	= {
-		.name	= CODA_NAME,
-		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(coda_dt_ids),
-		.pm	= &coda_pm_ops,
-	},
-	.id_table = coda_platform_ids,
-};
-
-module_platform_driver(coda_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com>");
-MODULE_DESCRIPTION("Coda multi-standard codec V4L2 driver");
diff --git a/drivers/media/platform/coda/Makefile b/drivers/media/platform/coda/Makefile
new file mode 100644
index 000000000000..3543291e6273
--- /dev/null
+++ b/drivers/media/platform/coda/Makefile
@@ -0,0 +1,3 @@
+coda-objs := coda-common.o coda-bit.o coda-h264.o
+
+obj-$(CONFIG_VIDEO_CODA) += coda.o
diff --git a/drivers/media/platform/coda/coda-bit.c b/drivers/media/platform/coda/coda-bit.c
new file mode 100644
index 000000000000..9b8ea8bbeb4e
--- /dev/null
+++ b/drivers/media/platform/coda/coda-bit.c
@@ -0,0 +1,1861 @@
+/*
+ * Coda multi-standard codec IP - BIT processor functions
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ * Copyright (C) 2012-2014 Philipp Zabel, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/irqreturn.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-common.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+#include <media/videobuf2-vmalloc.h>
+
+#include "coda.h"
+
+#define CODA7_PS_BUF_SIZE	0x28000
+#define CODA9_PS_SAVE_SIZE	(512 * 1024)
+
+#define CODA_DEFAULT_GAMMA	4096
+#define CODA9_DEFAULT_GAMMA	24576	/* 0.75 * 32768 */
+
+static inline int coda_is_initialized(struct coda_dev *dev)
+{
+	return coda_read(dev, CODA_REG_BIT_CUR_PC) != 0;
+}
+
+static inline unsigned long coda_isbusy(struct coda_dev *dev)
+{
+	return coda_read(dev, CODA_REG_BIT_BUSY);
+}
+
+static int coda_wait_timeout(struct coda_dev *dev)
+{
+	unsigned long timeout = jiffies + msecs_to_jiffies(1000);
+
+	while (coda_isbusy(dev)) {
+		if (time_after(jiffies, timeout))
+			return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+static void coda_command_async(struct coda_ctx *ctx, int cmd)
+{
+	struct coda_dev *dev = ctx->dev;
+
+	if (dev->devtype->product == CODA_960 ||
+	    dev->devtype->product == CODA_7541) {
+		/* Restore context related registers to CODA */
+		coda_write(dev, ctx->bit_stream_param,
+				CODA_REG_BIT_BIT_STREAM_PARAM);
+		coda_write(dev, ctx->frm_dis_flg,
+				CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+		coda_write(dev, ctx->frame_mem_ctrl,
+				CODA_REG_BIT_FRAME_MEM_CTRL);
+		coda_write(dev, ctx->workbuf.paddr, CODA_REG_BIT_WORK_BUF_ADDR);
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		coda_write(dev, 1, CODA9_GDI_WPROT_ERR_CLR);
+		coda_write(dev, 0, CODA9_GDI_WPROT_RGN_EN);
+	}
+
+	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
+
+	coda_write(dev, ctx->idx, CODA_REG_BIT_RUN_INDEX);
+	coda_write(dev, ctx->params.codec_mode, CODA_REG_BIT_RUN_COD_STD);
+	coda_write(dev, ctx->params.codec_mode_aux, CODA7_REG_BIT_RUN_AUX_STD);
+
+	coda_write(dev, cmd, CODA_REG_BIT_RUN_COMMAND);
+}
+
+static int coda_command_sync(struct coda_ctx *ctx, int cmd)
+{
+	struct coda_dev *dev = ctx->dev;
+
+	coda_command_async(ctx, cmd);
+	return coda_wait_timeout(dev);
+}
+
+int coda_hw_reset(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	unsigned long timeout;
+	unsigned int idx;
+	int ret;
+
+	if (!dev->rstc)
+		return -ENOENT;
+
+	idx = coda_read(dev, CODA_REG_BIT_RUN_INDEX);
+
+	if (dev->devtype->product == CODA_960) {
+		timeout = jiffies + msecs_to_jiffies(100);
+		coda_write(dev, 0x11, CODA9_GDI_BUS_CTRL);
+		while (coda_read(dev, CODA9_GDI_BUS_STATUS) != 0x77) {
+			if (time_after(jiffies, timeout))
+				return -ETIME;
+			cpu_relax();
+		}
+	}
+
+	ret = reset_control_reset(dev->rstc);
+	if (ret < 0)
+		return ret;
+
+	if (dev->devtype->product == CODA_960)
+		coda_write(dev, 0x00, CODA9_GDI_BUS_CTRL);
+	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
+	coda_write(dev, CODA_REG_RUN_ENABLE, CODA_REG_BIT_CODE_RUN);
+	ret = coda_wait_timeout(dev);
+	coda_write(dev, idx, CODA_REG_BIT_RUN_INDEX);
+
+	return ret;
+}
+
+static void coda_kfifo_sync_from_device(struct coda_ctx *ctx)
+{
+	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
+	struct coda_dev *dev = ctx->dev;
+	u32 rd_ptr;
+
+	rd_ptr = coda_read(dev, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
+	kfifo->out = (kfifo->in & ~kfifo->mask) |
+		      (rd_ptr - ctx->bitstream.paddr);
+	if (kfifo->out > kfifo->in)
+		kfifo->out -= kfifo->mask + 1;
+}
+
+static void coda_kfifo_sync_to_device_full(struct coda_ctx *ctx)
+{
+	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
+	struct coda_dev *dev = ctx->dev;
+	u32 rd_ptr, wr_ptr;
+
+	rd_ptr = ctx->bitstream.paddr + (kfifo->out & kfifo->mask);
+	coda_write(dev, rd_ptr, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
+	wr_ptr = ctx->bitstream.paddr + (kfifo->in & kfifo->mask);
+	coda_write(dev, wr_ptr, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+}
+
+static void coda_kfifo_sync_to_device_write(struct coda_ctx *ctx)
+{
+	struct __kfifo *kfifo = &ctx->bitstream_fifo.kfifo;
+	struct coda_dev *dev = ctx->dev;
+	u32 wr_ptr;
+
+	wr_ptr = ctx->bitstream.paddr + (kfifo->in & kfifo->mask);
+	coda_write(dev, wr_ptr, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+}
+
+static int coda_bitstream_queue(struct coda_ctx *ctx,
+				struct vb2_buffer *src_buf)
+{
+	u32 src_size = vb2_get_plane_payload(src_buf, 0);
+	u32 n;
+
+	n = kfifo_in(&ctx->bitstream_fifo, vb2_plane_vaddr(src_buf, 0),
+		     src_size);
+	if (n < src_size)
+		return -ENOSPC;
+
+	dma_sync_single_for_device(&ctx->dev->plat_dev->dev,
+				   ctx->bitstream.paddr, ctx->bitstream.size,
+				   DMA_TO_DEVICE);
+
+	src_buf->v4l2_buf.sequence = ctx->qsequence++;
+
+	return 0;
+}
+
+static bool coda_bitstream_try_queue(struct coda_ctx *ctx,
+				     struct vb2_buffer *src_buf)
+{
+	int ret;
+
+	if (coda_get_bitstream_payload(ctx) +
+	    vb2_get_plane_payload(src_buf, 0) + 512 >= ctx->bitstream.size)
+		return false;
+
+	if (vb2_plane_vaddr(src_buf, 0) == NULL) {
+		v4l2_err(&ctx->dev->v4l2_dev, "trying to queue empty buffer\n");
+		return true;
+	}
+
+	ret = coda_bitstream_queue(ctx, src_buf);
+	if (ret < 0) {
+		v4l2_err(&ctx->dev->v4l2_dev, "bitstream buffer overflow\n");
+		return false;
+	}
+	/* Sync read pointer to device */
+	if (ctx == v4l2_m2m_get_curr_priv(ctx->dev->m2m_dev))
+		coda_kfifo_sync_to_device_write(ctx);
+
+	ctx->hold = false;
+
+	return true;
+}
+
+void coda_fill_bitstream(struct coda_ctx *ctx)
+{
+	struct vb2_buffer *src_buf;
+	struct coda_timestamp *ts;
+
+	while (v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) > 0) {
+		src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+
+		if (coda_bitstream_try_queue(ctx, src_buf)) {
+			/*
+			 * Source buffer is queued in the bitstream ringbuffer;
+			 * queue the timestamp and mark source buffer as done
+			 */
+			src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+
+			ts = kmalloc(sizeof(*ts), GFP_KERNEL);
+			if (ts) {
+				ts->sequence = src_buf->v4l2_buf.sequence;
+				ts->timecode = src_buf->v4l2_buf.timecode;
+				ts->timestamp = src_buf->v4l2_buf.timestamp;
+				list_add_tail(&ts->list, &ctx->timestamp_list);
+			}
+
+			v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+		} else {
+			break;
+		}
+	}
+}
+
+void coda_bit_stream_end_flag(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+
+	ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
+
+	/* If this context is currently running, update the hardware flag */
+	if ((dev->devtype->product == CODA_960) &&
+	    coda_isbusy(dev) &&
+	    (ctx->idx == coda_read(dev, CODA_REG_BIT_RUN_INDEX))) {
+		coda_write(dev, ctx->bit_stream_param,
+			   CODA_REG_BIT_BIT_STREAM_PARAM);
+	}
+}
+
+static void coda_parabuf_write(struct coda_ctx *ctx, int index, u32 value)
+{
+	struct coda_dev *dev = ctx->dev;
+	u32 *p = ctx->parabuf.vaddr;
+
+	if (dev->devtype->product == CODA_DX6)
+		p[index] = value;
+	else
+		p[index ^ 1] = value;
+}
+
+static void coda_free_framebuffers(struct coda_ctx *ctx)
+{
+	int i;
+
+	for (i = 0; i < CODA_MAX_FRAMEBUFFERS; i++)
+		coda_free_aux_buf(ctx->dev, &ctx->internal_frames[i]);
+}
+
+static int coda_alloc_framebuffers(struct coda_ctx *ctx,
+				   struct coda_q_data *q_data, u32 fourcc)
+{
+	struct coda_dev *dev = ctx->dev;
+	int width, height;
+	dma_addr_t paddr;
+	int ysize;
+	int ret;
+	int i;
+
+	if (ctx->codec && (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 ||
+	     ctx->codec->dst_fourcc == V4L2_PIX_FMT_H264)) {
+		width = round_up(q_data->width, 16);
+		height = round_up(q_data->height, 16);
+	} else {
+		width = round_up(q_data->width, 8);
+		height = q_data->height;
+	}
+	ysize = width * height;
+
+	/* Allocate frame buffers */
+	for (i = 0; i < ctx->num_internal_frames; i++) {
+		size_t size;
+		char *name;
+
+		size = ysize + ysize / 2;
+		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 &&
+		    dev->devtype->product != CODA_DX6)
+			size += ysize / 4;
+		name = kasprintf(GFP_KERNEL, "fb%d", i);
+		ret = coda_alloc_context_buf(ctx, &ctx->internal_frames[i],
+					     size, name);
+		kfree(name);
+		if (ret < 0) {
+			coda_free_framebuffers(ctx);
+			return ret;
+		}
+	}
+
+	/* Register frame buffers in the parameter buffer */
+	for (i = 0; i < ctx->num_internal_frames; i++) {
+		paddr = ctx->internal_frames[i].paddr;
+		/* Start addresses of Y, Cb, Cr planes */
+		coda_parabuf_write(ctx, i * 3 + 0, paddr);
+		coda_parabuf_write(ctx, i * 3 + 1, paddr + ysize);
+		coda_parabuf_write(ctx, i * 3 + 2, paddr + ysize + ysize / 4);
+
+		/* mvcol buffer for h.264 */
+		if (ctx->codec->src_fourcc == V4L2_PIX_FMT_H264 &&
+		    dev->devtype->product != CODA_DX6)
+			coda_parabuf_write(ctx, 96 + i,
+					   ctx->internal_frames[i].paddr +
+					   ysize + ysize/4 + ysize/4);
+	}
+
+	/* mvcol buffer for mpeg4 */
+	if ((dev->devtype->product != CODA_DX6) &&
+	    (ctx->codec->src_fourcc == V4L2_PIX_FMT_MPEG4))
+		coda_parabuf_write(ctx, 97, ctx->internal_frames[i].paddr +
+					    ysize + ysize/4 + ysize/4);
+
+	return 0;
+}
+
+static void coda_free_context_buffers(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+
+	coda_free_aux_buf(dev, &ctx->slicebuf);
+	coda_free_aux_buf(dev, &ctx->psbuf);
+	if (dev->devtype->product != CODA_DX6)
+		coda_free_aux_buf(dev, &ctx->workbuf);
+}
+
+static int coda_alloc_context_buffers(struct coda_ctx *ctx,
+				      struct coda_q_data *q_data)
+{
+	struct coda_dev *dev = ctx->dev;
+	size_t size;
+	int ret;
+
+	if (dev->devtype->product == CODA_DX6)
+		return 0;
+
+	if (ctx->psbuf.vaddr) {
+		v4l2_err(&dev->v4l2_dev, "psmembuf still allocated\n");
+		return -EBUSY;
+	}
+	if (ctx->slicebuf.vaddr) {
+		v4l2_err(&dev->v4l2_dev, "slicebuf still allocated\n");
+		return -EBUSY;
+	}
+	if (ctx->workbuf.vaddr) {
+		v4l2_err(&dev->v4l2_dev, "context buffer still allocated\n");
+		ret = -EBUSY;
+		return -ENOMEM;
+	}
+
+	if (q_data->fourcc == V4L2_PIX_FMT_H264) {
+		/* worst case slice size */
+		size = (DIV_ROUND_UP(q_data->width, 16) *
+			DIV_ROUND_UP(q_data->height, 16)) * 3200 / 8 + 512;
+		ret = coda_alloc_context_buf(ctx, &ctx->slicebuf, size,
+					     "slicebuf");
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev,
+				 "failed to allocate %d byte slice buffer",
+				 ctx->slicebuf.size);
+			return ret;
+		}
+	}
+
+	if (dev->devtype->product == CODA_7541) {
+		ret = coda_alloc_context_buf(ctx, &ctx->psbuf,
+					     CODA7_PS_BUF_SIZE, "psbuf");
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev,
+				 "failed to allocate psmem buffer");
+			goto err;
+		}
+	}
+
+	size = dev->devtype->workbuf_size;
+	if (dev->devtype->product == CODA_960 &&
+	    q_data->fourcc == V4L2_PIX_FMT_H264)
+		size += CODA9_PS_SAVE_SIZE;
+	ret = coda_alloc_context_buf(ctx, &ctx->workbuf, size, "workbuf");
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev,
+			 "failed to allocate %d byte context buffer",
+			 ctx->workbuf.size);
+		goto err;
+	}
+
+	return 0;
+
+err:
+	coda_free_context_buffers(ctx);
+	return ret;
+}
+
+static int coda_encode_header(struct coda_ctx *ctx, struct vb2_buffer *buf,
+			      int header_code, u8 *header, int *size)
+{
+	struct coda_dev *dev = ctx->dev;
+	size_t bufsize;
+	int ret;
+	int i;
+
+	if (dev->devtype->product == CODA_960)
+		memset(vb2_plane_vaddr(buf, 0), 0, 64);
+
+	coda_write(dev, vb2_dma_contig_plane_dma_addr(buf, 0),
+		   CODA_CMD_ENC_HEADER_BB_START);
+	bufsize = vb2_plane_size(buf, 0);
+	if (dev->devtype->product == CODA_960)
+		bufsize /= 1024;
+	coda_write(dev, bufsize, CODA_CMD_ENC_HEADER_BB_SIZE);
+	coda_write(dev, header_code, CODA_CMD_ENC_HEADER_CODE);
+	ret = coda_command_sync(ctx, CODA_COMMAND_ENCODE_HEADER);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "CODA_COMMAND_ENCODE_HEADER timeout\n");
+		return ret;
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		for (i = 63; i > 0; i--)
+			if (((char *)vb2_plane_vaddr(buf, 0))[i] != 0)
+				break;
+		*size = i + 1;
+	} else {
+		*size = coda_read(dev, CODA_REG_BIT_WR_PTR(ctx->reg_idx)) -
+			coda_read(dev, CODA_CMD_ENC_HEADER_BB_START);
+	}
+	memcpy(header, vb2_plane_vaddr(buf, 0), *size);
+
+	return 0;
+}
+
+static phys_addr_t coda_iram_alloc(struct coda_iram_info *iram, size_t size)
+{
+	phys_addr_t ret;
+
+	size = round_up(size, 1024);
+	if (size > iram->remaining)
+		return 0;
+	iram->remaining -= size;
+
+	ret = iram->next_paddr;
+	iram->next_paddr += size;
+
+	return ret;
+}
+
+static void coda_setup_iram(struct coda_ctx *ctx)
+{
+	struct coda_iram_info *iram_info = &ctx->iram_info;
+	struct coda_dev *dev = ctx->dev;
+	int w64, w128;
+	int mb_width;
+	int dbk_bits;
+	int bit_bits;
+	int ip_bits;
+
+	memset(iram_info, 0, sizeof(*iram_info));
+	iram_info->next_paddr = dev->iram.paddr;
+	iram_info->remaining = dev->iram.size;
+
+	if (!dev->iram.vaddr)
+		return;
+
+	switch (dev->devtype->product) {
+	case CODA_7541:
+		dbk_bits = CODA7_USE_HOST_DBK_ENABLE | CODA7_USE_DBK_ENABLE;
+		bit_bits = CODA7_USE_HOST_BIT_ENABLE | CODA7_USE_BIT_ENABLE;
+		ip_bits = CODA7_USE_HOST_IP_ENABLE | CODA7_USE_IP_ENABLE;
+		break;
+	case CODA_960:
+		dbk_bits = CODA9_USE_HOST_DBK_ENABLE | CODA9_USE_DBK_ENABLE;
+		bit_bits = CODA9_USE_HOST_BIT_ENABLE | CODA7_USE_BIT_ENABLE;
+		ip_bits = CODA9_USE_HOST_IP_ENABLE | CODA7_USE_IP_ENABLE;
+		break;
+	default: /* CODA_DX6 */
+		return;
+	}
+
+	if (ctx->inst_type == CODA_INST_ENCODER) {
+		struct coda_q_data *q_data_src;
+
+		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+		mb_width = DIV_ROUND_UP(q_data_src->width, 16);
+		w128 = mb_width * 128;
+		w64 = mb_width * 64;
+
+		/* Prioritize in case IRAM is too small for everything */
+		if (dev->devtype->product == CODA_7541) {
+			iram_info->search_ram_size = round_up(mb_width * 16 *
+							      36 + 2048, 1024);
+			iram_info->search_ram_paddr = coda_iram_alloc(iram_info,
+						iram_info->search_ram_size);
+			if (!iram_info->search_ram_paddr) {
+				pr_err("IRAM is smaller than the search ram size\n");
+				goto out;
+			}
+			iram_info->axi_sram_use |= CODA7_USE_HOST_ME_ENABLE |
+						   CODA7_USE_ME_ENABLE;
+		}
+
+		/* Only H.264BP and H.263P3 are considered */
+		iram_info->buf_dbk_y_use = coda_iram_alloc(iram_info, w64);
+		iram_info->buf_dbk_c_use = coda_iram_alloc(iram_info, w64);
+		if (!iram_info->buf_dbk_c_use)
+			goto out;
+		iram_info->axi_sram_use |= dbk_bits;
+
+		iram_info->buf_bit_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_bit_use)
+			goto out;
+		iram_info->axi_sram_use |= bit_bits;
+
+		iram_info->buf_ip_ac_dc_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_ip_ac_dc_use)
+			goto out;
+		iram_info->axi_sram_use |= ip_bits;
+
+		/* OVL and BTP disabled for encoder */
+	} else if (ctx->inst_type == CODA_INST_DECODER) {
+		struct coda_q_data *q_data_dst;
+
+		q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+		mb_width = DIV_ROUND_UP(q_data_dst->width, 16);
+		w128 = mb_width * 128;
+
+		iram_info->buf_dbk_y_use = coda_iram_alloc(iram_info, w128);
+		iram_info->buf_dbk_c_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_dbk_c_use)
+			goto out;
+		iram_info->axi_sram_use |= dbk_bits;
+
+		iram_info->buf_bit_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_bit_use)
+			goto out;
+		iram_info->axi_sram_use |= bit_bits;
+
+		iram_info->buf_ip_ac_dc_use = coda_iram_alloc(iram_info, w128);
+		if (!iram_info->buf_ip_ac_dc_use)
+			goto out;
+		iram_info->axi_sram_use |= ip_bits;
+
+		/* OVL and BTP unused as there is no VC1 support yet */
+	}
+
+out:
+	if (!(iram_info->axi_sram_use & CODA7_USE_HOST_IP_ENABLE))
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "IRAM smaller than needed\n");
+
+	if (dev->devtype->product == CODA_7541) {
+		/* TODO - Enabling these causes picture errors on CODA7541 */
+		if (ctx->inst_type == CODA_INST_DECODER) {
+			/* fw 1.4.50 */
+			iram_info->axi_sram_use &= ~(CODA7_USE_HOST_IP_ENABLE |
+						     CODA7_USE_IP_ENABLE);
+		} else {
+			/* fw 13.4.29 */
+			iram_info->axi_sram_use &= ~(CODA7_USE_HOST_IP_ENABLE |
+						     CODA7_USE_HOST_DBK_ENABLE |
+						     CODA7_USE_IP_ENABLE |
+						     CODA7_USE_DBK_ENABLE);
+		}
+	}
+}
+
+static u32 coda_supported_firmwares[] = {
+	CODA_FIRMWARE_VERNUM(CODA_DX6, 2, 2, 5),
+	CODA_FIRMWARE_VERNUM(CODA_7541, 1, 4, 50),
+	CODA_FIRMWARE_VERNUM(CODA_960, 2, 1, 5),
+};
+
+static bool coda_firmware_supported(u32 vernum)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(coda_supported_firmwares); i++)
+		if (vernum == coda_supported_firmwares[i])
+			return true;
+	return false;
+}
+
+int coda_check_firmware(struct coda_dev *dev)
+{
+	u16 product, major, minor, release;
+	u32 data;
+	int ret;
+
+	ret = clk_prepare_enable(dev->clk_per);
+	if (ret)
+		goto err_clk_per;
+
+	ret = clk_prepare_enable(dev->clk_ahb);
+	if (ret)
+		goto err_clk_ahb;
+
+	coda_write(dev, 0, CODA_CMD_FIRMWARE_VERNUM);
+	coda_write(dev, CODA_REG_BIT_BUSY_FLAG, CODA_REG_BIT_BUSY);
+	coda_write(dev, 0, CODA_REG_BIT_RUN_INDEX);
+	coda_write(dev, 0, CODA_REG_BIT_RUN_COD_STD);
+	coda_write(dev, CODA_COMMAND_FIRMWARE_GET, CODA_REG_BIT_RUN_COMMAND);
+	if (coda_wait_timeout(dev)) {
+		v4l2_err(&dev->v4l2_dev, "firmware get command error\n");
+		ret = -EIO;
+		goto err_run_cmd;
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		data = coda_read(dev, CODA9_CMD_FIRMWARE_CODE_REV);
+		v4l2_info(&dev->v4l2_dev, "Firmware code revision: %d\n",
+			  data);
+	}
+
+	/* Check we are compatible with the loaded firmware */
+	data = coda_read(dev, CODA_CMD_FIRMWARE_VERNUM);
+	product = CODA_FIRMWARE_PRODUCT(data);
+	major = CODA_FIRMWARE_MAJOR(data);
+	minor = CODA_FIRMWARE_MINOR(data);
+	release = CODA_FIRMWARE_RELEASE(data);
+
+	clk_disable_unprepare(dev->clk_per);
+	clk_disable_unprepare(dev->clk_ahb);
+
+	if (product != dev->devtype->product) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Wrong firmware. Hw: %s, Fw: %s, Version: %u.%u.%u\n",
+			 coda_product_name(dev->devtype->product),
+			 coda_product_name(product), major, minor, release);
+		return -EINVAL;
+	}
+
+	v4l2_info(&dev->v4l2_dev, "Initialized %s.\n",
+		  coda_product_name(product));
+
+	if (coda_firmware_supported(data)) {
+		v4l2_info(&dev->v4l2_dev, "Firmware version: %u.%u.%u\n",
+			  major, minor, release);
+	} else {
+		v4l2_warn(&dev->v4l2_dev,
+			  "Unsupported firmware version: %u.%u.%u\n",
+			  major, minor, release);
+	}
+
+	return 0;
+
+err_run_cmd:
+	clk_disable_unprepare(dev->clk_ahb);
+err_clk_ahb:
+	clk_disable_unprepare(dev->clk_per);
+err_clk_per:
+	return ret;
+}
+
+/*
+ * Encoder context operations
+ */
+
+static int coda_start_encoding(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	struct v4l2_device *v4l2_dev = &dev->v4l2_dev;
+	struct coda_q_data *q_data_src, *q_data_dst;
+	u32 bitstream_buf, bitstream_size;
+	struct vb2_buffer *buf;
+	int gamma, ret, value;
+	u32 dst_fourcc;
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	dst_fourcc = q_data_dst->fourcc;
+
+	/* Allocate per-instance buffers */
+	ret = coda_alloc_context_buffers(ctx, q_data_src);
+	if (ret < 0)
+		return ret;
+
+	buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	bitstream_buf = vb2_dma_contig_plane_dma_addr(buf, 0);
+	bitstream_size = q_data_dst->sizeimage;
+
+	if (!coda_is_initialized(dev)) {
+		v4l2_err(v4l2_dev, "coda is not initialized.\n");
+		return -EFAULT;
+	}
+
+	mutex_lock(&dev->coda_mutex);
+
+	coda_write(dev, ctx->parabuf.paddr, CODA_REG_BIT_PARA_BUF_ADDR);
+	coda_write(dev, bitstream_buf, CODA_REG_BIT_RD_PTR(ctx->reg_idx));
+	coda_write(dev, bitstream_buf, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		coda_write(dev, CODADX6_STREAM_BUF_DYNALLOC_EN |
+			CODADX6_STREAM_BUF_PIC_RESET, CODA_REG_BIT_STREAM_CTRL);
+		break;
+	case CODA_960:
+		coda_write(dev, 0, CODA9_GDI_WPROT_RGN_EN);
+		/* fallthrough */
+	case CODA_7541:
+		coda_write(dev, CODA7_STREAM_BUF_DYNALLOC_EN |
+			CODA7_STREAM_BUF_PIC_RESET, CODA_REG_BIT_STREAM_CTRL);
+		break;
+	}
+
+	value = coda_read(dev, CODA_REG_BIT_FRAME_MEM_CTRL);
+	value &= ~(1 << 2 | 0x7 << 9);
+	ctx->frame_mem_ctrl = value;
+	coda_write(dev, value, CODA_REG_BIT_FRAME_MEM_CTRL);
+
+	if (dev->devtype->product == CODA_DX6) {
+		/* Configure the coda */
+		coda_write(dev, dev->iram.paddr,
+			   CODADX6_REG_BIT_SEARCH_RAM_BASE_ADDR);
+	}
+
+	/* Could set rotation here if needed */
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		value = (q_data_src->width & CODADX6_PICWIDTH_MASK)
+			<< CODADX6_PICWIDTH_OFFSET;
+		value |= (q_data_src->height & CODADX6_PICHEIGHT_MASK)
+			 << CODA_PICHEIGHT_OFFSET;
+		break;
+	case CODA_7541:
+		if (dst_fourcc == V4L2_PIX_FMT_H264) {
+			value = (round_up(q_data_src->width, 16) &
+				 CODA7_PICWIDTH_MASK) << CODA7_PICWIDTH_OFFSET;
+			value |= (round_up(q_data_src->height, 16) &
+				 CODA7_PICHEIGHT_MASK) << CODA_PICHEIGHT_OFFSET;
+			break;
+		}
+		/* fallthrough */
+	case CODA_960:
+		value = (q_data_src->width & CODA7_PICWIDTH_MASK)
+			<< CODA7_PICWIDTH_OFFSET;
+		value |= (q_data_src->height & CODA7_PICHEIGHT_MASK)
+			 << CODA_PICHEIGHT_OFFSET;
+	}
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_SRC_SIZE);
+	coda_write(dev, ctx->params.framerate,
+		   CODA_CMD_ENC_SEQ_SRC_F_RATE);
+
+	ctx->params.codec_mode = ctx->codec->mode;
+	switch (dst_fourcc) {
+	case V4L2_PIX_FMT_MPEG4:
+		if (dev->devtype->product == CODA_960)
+			coda_write(dev, CODA9_STD_MPEG4,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		else
+			coda_write(dev, CODA_STD_MPEG4,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		coda_write(dev, 0, CODA_CMD_ENC_SEQ_MP4_PARA);
+		break;
+	case V4L2_PIX_FMT_H264:
+		if (dev->devtype->product == CODA_960)
+			coda_write(dev, CODA9_STD_H264,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		else
+			coda_write(dev, CODA_STD_H264,
+				   CODA_CMD_ENC_SEQ_COD_STD);
+		if (ctx->params.h264_deblk_enabled) {
+			value = ((ctx->params.h264_deblk_alpha &
+				  CODA_264PARAM_DEBLKFILTEROFFSETALPHA_MASK) <<
+				 CODA_264PARAM_DEBLKFILTEROFFSETALPHA_OFFSET) |
+				((ctx->params.h264_deblk_beta &
+				  CODA_264PARAM_DEBLKFILTEROFFSETBETA_MASK) <<
+				 CODA_264PARAM_DEBLKFILTEROFFSETBETA_OFFSET);
+		} else {
+			value = 1 << CODA_264PARAM_DISABLEDEBLK_OFFSET;
+		}
+		coda_write(dev, value, CODA_CMD_ENC_SEQ_264_PARA);
+		break;
+	default:
+		v4l2_err(v4l2_dev,
+			 "dst format (0x%08x) invalid.\n", dst_fourcc);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	switch (ctx->params.slice_mode) {
+	case V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE:
+		value = 0;
+		break;
+	case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB:
+		value  = (ctx->params.slice_max_mb & CODA_SLICING_SIZE_MASK)
+			 << CODA_SLICING_SIZE_OFFSET;
+		value |= (1 & CODA_SLICING_UNIT_MASK)
+			 << CODA_SLICING_UNIT_OFFSET;
+		value |=  1 & CODA_SLICING_MODE_MASK;
+		break;
+	case V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES:
+		value  = (ctx->params.slice_max_bits & CODA_SLICING_SIZE_MASK)
+			 << CODA_SLICING_SIZE_OFFSET;
+		value |= (0 & CODA_SLICING_UNIT_MASK)
+			 << CODA_SLICING_UNIT_OFFSET;
+		value |=  1 & CODA_SLICING_MODE_MASK;
+		break;
+	}
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_SLICE_MODE);
+	value = ctx->params.gop_size & CODA_GOP_SIZE_MASK;
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_GOP_SIZE);
+
+	if (ctx->params.bitrate) {
+		/* Rate control enabled */
+		value = (ctx->params.bitrate & CODA_RATECONTROL_BITRATE_MASK)
+			<< CODA_RATECONTROL_BITRATE_OFFSET;
+		value |=  1 & CODA_RATECONTROL_ENABLE_MASK;
+		if (dev->devtype->product == CODA_960)
+			value |= BIT(31); /* disable autoskip */
+	} else {
+		value = 0;
+	}
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_RC_PARA);
+
+	coda_write(dev, 0, CODA_CMD_ENC_SEQ_RC_BUF_SIZE);
+	coda_write(dev, ctx->params.intra_refresh,
+		   CODA_CMD_ENC_SEQ_INTRA_REFRESH);
+
+	coda_write(dev, bitstream_buf, CODA_CMD_ENC_SEQ_BB_START);
+	coda_write(dev, bitstream_size / 1024, CODA_CMD_ENC_SEQ_BB_SIZE);
+
+
+	value = 0;
+	if (dev->devtype->product == CODA_960)
+		gamma = CODA9_DEFAULT_GAMMA;
+	else
+		gamma = CODA_DEFAULT_GAMMA;
+	if (gamma > 0) {
+		coda_write(dev, (gamma & CODA_GAMMA_MASK) << CODA_GAMMA_OFFSET,
+			   CODA_CMD_ENC_SEQ_RC_GAMMA);
+	}
+
+	if (ctx->params.h264_min_qp || ctx->params.h264_max_qp) {
+		coda_write(dev,
+			   ctx->params.h264_min_qp << CODA_QPMIN_OFFSET |
+			   ctx->params.h264_max_qp << CODA_QPMAX_OFFSET,
+			   CODA_CMD_ENC_SEQ_RC_QP_MIN_MAX);
+	}
+	if (dev->devtype->product == CODA_960) {
+		if (ctx->params.h264_max_qp)
+			value |= 1 << CODA9_OPTION_RCQPMAX_OFFSET;
+		if (CODA_DEFAULT_GAMMA > 0)
+			value |= 1 << CODA9_OPTION_GAMMA_OFFSET;
+	} else {
+		if (CODA_DEFAULT_GAMMA > 0) {
+			if (dev->devtype->product == CODA_DX6)
+				value |= 1 << CODADX6_OPTION_GAMMA_OFFSET;
+			else
+				value |= 1 << CODA7_OPTION_GAMMA_OFFSET;
+		}
+		if (ctx->params.h264_min_qp)
+			value |= 1 << CODA7_OPTION_RCQPMIN_OFFSET;
+		if (ctx->params.h264_max_qp)
+			value |= 1 << CODA7_OPTION_RCQPMAX_OFFSET;
+	}
+	coda_write(dev, value, CODA_CMD_ENC_SEQ_OPTION);
+
+	coda_write(dev, 0, CODA_CMD_ENC_SEQ_RC_INTERVAL_MODE);
+
+	coda_setup_iram(ctx);
+
+	if (dst_fourcc == V4L2_PIX_FMT_H264) {
+		switch (dev->devtype->product) {
+		case CODA_DX6:
+			value = FMO_SLICE_SAVE_BUF_SIZE << 7;
+			coda_write(dev, value, CODADX6_CMD_ENC_SEQ_FMO);
+			break;
+		case CODA_7541:
+			coda_write(dev, ctx->iram_info.search_ram_paddr,
+					CODA7_CMD_ENC_SEQ_SEARCH_BASE);
+			coda_write(dev, ctx->iram_info.search_ram_size,
+					CODA7_CMD_ENC_SEQ_SEARCH_SIZE);
+			break;
+		case CODA_960:
+			coda_write(dev, 0, CODA9_CMD_ENC_SEQ_ME_OPTION);
+			coda_write(dev, 0, CODA9_CMD_ENC_SEQ_INTRA_WEIGHT);
+		}
+	}
+
+	ret = coda_command_sync(ctx, CODA_COMMAND_SEQ_INIT);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "CODA_COMMAND_SEQ_INIT timeout\n");
+		goto out;
+	}
+
+	if (coda_read(dev, CODA_RET_ENC_SEQ_SUCCESS) == 0) {
+		v4l2_err(v4l2_dev, "CODA_COMMAND_SEQ_INIT failed\n");
+		ret = -EFAULT;
+		goto out;
+	}
+
+	if (dev->devtype->product == CODA_960)
+		ctx->num_internal_frames = 4;
+	else
+		ctx->num_internal_frames = 2;
+	ret = coda_alloc_framebuffers(ctx, q_data_src, dst_fourcc);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "failed to allocate framebuffers\n");
+		goto out;
+	}
+
+	coda_write(dev, ctx->num_internal_frames, CODA_CMD_SET_FRAME_BUF_NUM);
+	coda_write(dev, q_data_src->bytesperline,
+			CODA_CMD_SET_FRAME_BUF_STRIDE);
+	if (dev->devtype->product == CODA_7541) {
+		coda_write(dev, q_data_src->bytesperline,
+				CODA7_CMD_SET_FRAME_SOURCE_BUF_STRIDE);
+	}
+	if (dev->devtype->product != CODA_DX6) {
+		coda_write(dev, ctx->iram_info.buf_bit_use,
+				CODA7_CMD_SET_FRAME_AXI_BIT_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ip_ac_dc_use,
+				CODA7_CMD_SET_FRAME_AXI_IPACDC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_y_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKY_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_c_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ovl_use,
+				CODA7_CMD_SET_FRAME_AXI_OVL_ADDR);
+		if (dev->devtype->product == CODA_960) {
+			coda_write(dev, ctx->iram_info.buf_btp_use,
+					CODA9_CMD_SET_FRAME_AXI_BTP_ADDR);
+
+			/* FIXME */
+			coda_write(dev, ctx->internal_frames[2].paddr,
+				   CODA9_CMD_SET_FRAME_SUBSAMP_A);
+			coda_write(dev, ctx->internal_frames[3].paddr,
+				   CODA9_CMD_SET_FRAME_SUBSAMP_B);
+		}
+	}
+
+	ret = coda_command_sync(ctx, CODA_COMMAND_SET_FRAME_BUF);
+	if (ret < 0) {
+		v4l2_err(v4l2_dev, "CODA_COMMAND_SET_FRAME_BUF timeout\n");
+		goto out;
+	}
+
+	/* Save stream headers */
+	buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	switch (dst_fourcc) {
+	case V4L2_PIX_FMT_H264:
+		/*
+		 * Get SPS in the first frame and copy it to an
+		 * intermediate buffer.
+		 */
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_H264_SPS,
+					 &ctx->vpu_header[0][0],
+					 &ctx->vpu_header_size[0]);
+		if (ret < 0)
+			goto out;
+
+		/*
+		 * Get PPS in the first frame and copy it to an
+		 * intermediate buffer.
+		 */
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_H264_PPS,
+					 &ctx->vpu_header[1][0],
+					 &ctx->vpu_header_size[1]);
+		if (ret < 0)
+			goto out;
+
+		/*
+		 * Length of H.264 headers is variable and thus it might not be
+		 * aligned for the coda to append the encoded frame. In that is
+		 * the case a filler NAL must be added to header 2.
+		 */
+		ctx->vpu_header_size[2] = coda_h264_padding(
+					(ctx->vpu_header_size[0] +
+					 ctx->vpu_header_size[1]),
+					 ctx->vpu_header[2]);
+		break;
+	case V4L2_PIX_FMT_MPEG4:
+		/*
+		 * Get VOS in the first frame and copy it to an
+		 * intermediate buffer
+		 */
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VOS,
+					 &ctx->vpu_header[0][0],
+					 &ctx->vpu_header_size[0]);
+		if (ret < 0)
+			goto out;
+
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VIS,
+					 &ctx->vpu_header[1][0],
+					 &ctx->vpu_header_size[1]);
+		if (ret < 0)
+			goto out;
+
+		ret = coda_encode_header(ctx, buf, CODA_HEADER_MP4V_VOL,
+					 &ctx->vpu_header[2][0],
+					 &ctx->vpu_header_size[2]);
+		if (ret < 0)
+			goto out;
+		break;
+	default:
+		/* No more formats need to save headers at the moment */
+		break;
+	}
+
+out:
+	mutex_unlock(&dev->coda_mutex);
+	return ret;
+}
+
+static int coda_prepare_encode(struct coda_ctx *ctx)
+{
+	struct coda_q_data *q_data_src, *q_data_dst;
+	struct vb2_buffer *src_buf, *dst_buf;
+	struct coda_dev *dev = ctx->dev;
+	int force_ipicture;
+	int quant_param = 0;
+	u32 picture_y, picture_cb, picture_cr;
+	u32 pic_stream_buffer_addr, pic_stream_buffer_size;
+	u32 dst_fourcc;
+
+	src_buf = v4l2_m2m_next_src_buf(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	dst_fourcc = q_data_dst->fourcc;
+
+	src_buf->v4l2_buf.sequence = ctx->osequence;
+	dst_buf->v4l2_buf.sequence = ctx->osequence;
+	ctx->osequence++;
+
+	/*
+	 * Workaround coda firmware BUG that only marks the first
+	 * frame as IDR. This is a problem for some decoders that can't
+	 * recover when a frame is lost.
+	 */
+	if (src_buf->v4l2_buf.sequence % ctx->params.gop_size) {
+		src_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_PFRAME;
+		src_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_KEYFRAME;
+	} else {
+		src_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+		src_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_PFRAME;
+	}
+
+	if (dev->devtype->product == CODA_960)
+		coda_set_gdi_regs(ctx);
+
+	/*
+	 * Copy headers at the beginning of the first frame for H.264 only.
+	 * In MPEG4 they are already copied by the coda.
+	 */
+	if (src_buf->v4l2_buf.sequence == 0) {
+		pic_stream_buffer_addr =
+			vb2_dma_contig_plane_dma_addr(dst_buf, 0) +
+			ctx->vpu_header_size[0] +
+			ctx->vpu_header_size[1] +
+			ctx->vpu_header_size[2];
+		pic_stream_buffer_size = CODA_MAX_FRAME_SIZE -
+			ctx->vpu_header_size[0] -
+			ctx->vpu_header_size[1] -
+			ctx->vpu_header_size[2];
+		memcpy(vb2_plane_vaddr(dst_buf, 0),
+		       &ctx->vpu_header[0][0], ctx->vpu_header_size[0]);
+		memcpy(vb2_plane_vaddr(dst_buf, 0) + ctx->vpu_header_size[0],
+		       &ctx->vpu_header[1][0], ctx->vpu_header_size[1]);
+		memcpy(vb2_plane_vaddr(dst_buf, 0) + ctx->vpu_header_size[0] +
+			ctx->vpu_header_size[1], &ctx->vpu_header[2][0],
+			ctx->vpu_header_size[2]);
+	} else {
+		pic_stream_buffer_addr =
+			vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+		pic_stream_buffer_size = CODA_MAX_FRAME_SIZE;
+	}
+
+	if (src_buf->v4l2_buf.flags & V4L2_BUF_FLAG_KEYFRAME) {
+		force_ipicture = 1;
+		switch (dst_fourcc) {
+		case V4L2_PIX_FMT_H264:
+			quant_param = ctx->params.h264_intra_qp;
+			break;
+		case V4L2_PIX_FMT_MPEG4:
+			quant_param = ctx->params.mpeg4_intra_qp;
+			break;
+		default:
+			v4l2_warn(&ctx->dev->v4l2_dev,
+				"cannot set intra qp, fmt not supported\n");
+			break;
+		}
+	} else {
+		force_ipicture = 0;
+		switch (dst_fourcc) {
+		case V4L2_PIX_FMT_H264:
+			quant_param = ctx->params.h264_inter_qp;
+			break;
+		case V4L2_PIX_FMT_MPEG4:
+			quant_param = ctx->params.mpeg4_inter_qp;
+			break;
+		default:
+			v4l2_warn(&ctx->dev->v4l2_dev,
+				"cannot set inter qp, fmt not supported\n");
+			break;
+		}
+	}
+
+	/* submit */
+	coda_write(dev, CODA_ROT_MIR_ENABLE | ctx->params.rot_mode,
+		   CODA_CMD_ENC_PIC_ROT_MODE);
+	coda_write(dev, quant_param, CODA_CMD_ENC_PIC_QS);
+
+
+	picture_y = vb2_dma_contig_plane_dma_addr(src_buf, 0);
+	switch (q_data_src->fourcc) {
+	case V4L2_PIX_FMT_YVU420:
+		/* Switch Cb and Cr for YVU420 format */
+		picture_cr = picture_y + q_data_src->bytesperline *
+				q_data_src->height;
+		picture_cb = picture_cr + q_data_src->bytesperline / 2 *
+				q_data_src->height / 2;
+		break;
+	case V4L2_PIX_FMT_YUV420:
+	default:
+		picture_cb = picture_y + q_data_src->bytesperline *
+				q_data_src->height;
+		picture_cr = picture_cb + q_data_src->bytesperline / 2 *
+				q_data_src->height / 2;
+		break;
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		coda_write(dev, 4/*FIXME: 0*/, CODA9_CMD_ENC_PIC_SRC_INDEX);
+		coda_write(dev, q_data_src->width, CODA9_CMD_ENC_PIC_SRC_STRIDE);
+		coda_write(dev, 0, CODA9_CMD_ENC_PIC_SUB_FRAME_SYNC);
+
+		coda_write(dev, picture_y, CODA9_CMD_ENC_PIC_SRC_ADDR_Y);
+		coda_write(dev, picture_cb, CODA9_CMD_ENC_PIC_SRC_ADDR_CB);
+		coda_write(dev, picture_cr, CODA9_CMD_ENC_PIC_SRC_ADDR_CR);
+	} else {
+		coda_write(dev, picture_y, CODA_CMD_ENC_PIC_SRC_ADDR_Y);
+		coda_write(dev, picture_cb, CODA_CMD_ENC_PIC_SRC_ADDR_CB);
+		coda_write(dev, picture_cr, CODA_CMD_ENC_PIC_SRC_ADDR_CR);
+	}
+	coda_write(dev, force_ipicture << 1 & 0x2,
+		   CODA_CMD_ENC_PIC_OPTION);
+
+	coda_write(dev, pic_stream_buffer_addr, CODA_CMD_ENC_PIC_BB_START);
+	coda_write(dev, pic_stream_buffer_size / 1024,
+		   CODA_CMD_ENC_PIC_BB_SIZE);
+
+	if (!ctx->streamon_out) {
+		/* After streamoff on the output side, set stream end flag */
+		ctx->bit_stream_param |= CODA_BIT_STREAM_END_FLAG;
+		coda_write(dev, ctx->bit_stream_param,
+			   CODA_REG_BIT_BIT_STREAM_PARAM);
+	}
+
+	if (dev->devtype->product != CODA_DX6)
+		coda_write(dev, ctx->iram_info.axi_sram_use,
+				CODA7_REG_BIT_AXI_SRAM_USE);
+
+	coda_command_async(ctx, CODA_COMMAND_PIC_RUN);
+
+	return 0;
+}
+
+static void coda_finish_encode(struct coda_ctx *ctx)
+{
+	struct vb2_buffer *src_buf, *dst_buf;
+	struct coda_dev *dev = ctx->dev;
+	u32 wr_ptr, start_ptr;
+
+	src_buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx);
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+
+	/* Get results from the coda */
+	start_ptr = coda_read(dev, CODA_CMD_ENC_PIC_BB_START);
+	wr_ptr = coda_read(dev, CODA_REG_BIT_WR_PTR(ctx->reg_idx));
+
+	/* Calculate bytesused field */
+	if (dst_buf->v4l2_buf.sequence == 0) {
+		vb2_set_plane_payload(dst_buf, 0, wr_ptr - start_ptr +
+					ctx->vpu_header_size[0] +
+					ctx->vpu_header_size[1] +
+					ctx->vpu_header_size[2]);
+	} else {
+		vb2_set_plane_payload(dst_buf, 0, wr_ptr - start_ptr);
+	}
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev, "frame size = %u\n",
+		 wr_ptr - start_ptr);
+
+	coda_read(dev, CODA_RET_ENC_PIC_SLICE_NUM);
+	coda_read(dev, CODA_RET_ENC_PIC_FLAG);
+
+	if (coda_read(dev, CODA_RET_ENC_PIC_TYPE) == 0) {
+		dst_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_KEYFRAME;
+		dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_PFRAME;
+	} else {
+		dst_buf->v4l2_buf.flags |= V4L2_BUF_FLAG_PFRAME;
+		dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_KEYFRAME;
+	}
+
+	dst_buf->v4l2_buf.timestamp = src_buf->v4l2_buf.timestamp;
+	dst_buf->v4l2_buf.flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_buf->v4l2_buf.flags |=
+		src_buf->v4l2_buf.flags & V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+	dst_buf->v4l2_buf.timecode = src_buf->v4l2_buf.timecode;
+
+	v4l2_m2m_buf_done(src_buf, VB2_BUF_STATE_DONE);
+
+	dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+	v4l2_m2m_buf_done(dst_buf, VB2_BUF_STATE_DONE);
+
+	ctx->gopcounter--;
+	if (ctx->gopcounter < 0)
+		ctx->gopcounter = ctx->params.gop_size - 1;
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+		"job finished: encoding frame (%d) (%s)\n",
+		dst_buf->v4l2_buf.sequence,
+		(dst_buf->v4l2_buf.flags & V4L2_BUF_FLAG_KEYFRAME) ?
+		"KEYFRAME" : "PFRAME");
+}
+
+static void coda_seq_end_work(struct work_struct *work)
+{
+	struct coda_ctx *ctx = container_of(work, struct coda_ctx, seq_end_work);
+	struct coda_dev *dev = ctx->dev;
+
+	mutex_lock(&ctx->buffer_mutex);
+	mutex_lock(&dev->coda_mutex);
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+		 "%d: %s: sent command 'SEQ_END' to coda\n", ctx->idx,
+		 __func__);
+	if (coda_command_sync(ctx, CODA_COMMAND_SEQ_END)) {
+		v4l2_err(&dev->v4l2_dev,
+			 "CODA_COMMAND_SEQ_END failed\n");
+	}
+
+	kfifo_init(&ctx->bitstream_fifo,
+		ctx->bitstream.vaddr, ctx->bitstream.size);
+
+	coda_free_framebuffers(ctx);
+	coda_free_context_buffers(ctx);
+
+	mutex_unlock(&dev->coda_mutex);
+	mutex_unlock(&ctx->buffer_mutex);
+}
+
+static void coda_bit_release(struct coda_ctx *ctx)
+{
+	coda_free_framebuffers(ctx);
+	coda_free_context_buffers(ctx);
+}
+
+const struct coda_context_ops coda_bit_encode_ops = {
+	.queue_init = coda_encoder_queue_init,
+	.start_streaming = coda_start_encoding,
+	.prepare_run = coda_prepare_encode,
+	.finish_run = coda_finish_encode,
+	.seq_end_work = coda_seq_end_work,
+	.release = coda_bit_release,
+};
+
+/*
+ * Decoder context operations
+ */
+
+static int __coda_start_decoding(struct coda_ctx *ctx)
+{
+	struct coda_q_data *q_data_src, *q_data_dst;
+	u32 bitstream_buf, bitstream_size;
+	struct coda_dev *dev = ctx->dev;
+	int width, height;
+	u32 src_fourcc;
+	u32 val;
+	int ret;
+
+	/* Start decoding */
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	bitstream_buf = ctx->bitstream.paddr;
+	bitstream_size = ctx->bitstream.size;
+	src_fourcc = q_data_src->fourcc;
+
+	/* Allocate per-instance buffers */
+	ret = coda_alloc_context_buffers(ctx, q_data_src);
+	if (ret < 0)
+		return ret;
+
+	coda_write(dev, ctx->parabuf.paddr, CODA_REG_BIT_PARA_BUF_ADDR);
+
+	/* Update coda bitstream read and write pointers from kfifo */
+	coda_kfifo_sync_to_device_full(ctx);
+
+	ctx->display_idx = -1;
+	ctx->frm_dis_flg = 0;
+	coda_write(dev, 0, CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+
+	coda_write(dev, CODA_BIT_DEC_SEQ_INIT_ESCAPE,
+			CODA_REG_BIT_BIT_STREAM_PARAM);
+
+	coda_write(dev, bitstream_buf, CODA_CMD_DEC_SEQ_BB_START);
+	coda_write(dev, bitstream_size / 1024, CODA_CMD_DEC_SEQ_BB_SIZE);
+	val = 0;
+	if ((dev->devtype->product == CODA_7541) ||
+	    (dev->devtype->product == CODA_960))
+		val |= CODA_REORDER_ENABLE;
+	coda_write(dev, val, CODA_CMD_DEC_SEQ_OPTION);
+
+	ctx->params.codec_mode = ctx->codec->mode;
+	if (dev->devtype->product == CODA_960 &&
+	    src_fourcc == V4L2_PIX_FMT_MPEG4)
+		ctx->params.codec_mode_aux = CODA_MP4_AUX_MPEG4;
+	else
+		ctx->params.codec_mode_aux = 0;
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		if (dev->devtype->product == CODA_7541) {
+			coda_write(dev, ctx->psbuf.paddr,
+					CODA_CMD_DEC_SEQ_PS_BB_START);
+			coda_write(dev, (CODA7_PS_BUF_SIZE / 1024),
+					CODA_CMD_DEC_SEQ_PS_BB_SIZE);
+		}
+		if (dev->devtype->product == CODA_960) {
+			coda_write(dev, 0, CODA_CMD_DEC_SEQ_X264_MV_EN);
+			coda_write(dev, 512, CODA_CMD_DEC_SEQ_SPP_CHUNK_SIZE);
+		}
+	}
+	if (dev->devtype->product != CODA_960)
+		coda_write(dev, 0, CODA_CMD_DEC_SEQ_SRC_SIZE);
+
+	if (coda_command_sync(ctx, CODA_COMMAND_SEQ_INIT)) {
+		v4l2_err(&dev->v4l2_dev, "CODA_COMMAND_SEQ_INIT timeout\n");
+		coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
+		return -ETIMEDOUT;
+	}
+
+	/* Update kfifo out pointer from coda bitstream read pointer */
+	coda_kfifo_sync_from_device(ctx);
+
+	coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
+
+	if (coda_read(dev, CODA_RET_DEC_SEQ_SUCCESS) == 0) {
+		v4l2_err(&dev->v4l2_dev,
+			"CODA_COMMAND_SEQ_INIT failed, error code = %d\n",
+			coda_read(dev, CODA_RET_DEC_SEQ_ERR_REASON));
+		return -EAGAIN;
+	}
+
+	val = coda_read(dev, CODA_RET_DEC_SEQ_SRC_SIZE);
+	if (dev->devtype->product == CODA_DX6) {
+		width = (val >> CODADX6_PICWIDTH_OFFSET) & CODADX6_PICWIDTH_MASK;
+		height = val & CODADX6_PICHEIGHT_MASK;
+	} else {
+		width = (val >> CODA7_PICWIDTH_OFFSET) & CODA7_PICWIDTH_MASK;
+		height = val & CODA7_PICHEIGHT_MASK;
+	}
+
+	if (width > q_data_dst->width || height > q_data_dst->height) {
+		v4l2_err(&dev->v4l2_dev, "stream is %dx%d, not %dx%d\n",
+			 width, height, q_data_dst->width, q_data_dst->height);
+		return -EINVAL;
+	}
+
+	width = round_up(width, 16);
+	height = round_up(height, 16);
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "%s instance %d now: %dx%d\n",
+		 __func__, ctx->idx, width, height);
+
+	ctx->num_internal_frames = coda_read(dev, CODA_RET_DEC_SEQ_FRAME_NEED);
+	if (ctx->num_internal_frames > CODA_MAX_FRAMEBUFFERS) {
+		v4l2_err(&dev->v4l2_dev,
+			 "not enough framebuffers to decode (%d < %d)\n",
+			 CODA_MAX_FRAMEBUFFERS, ctx->num_internal_frames);
+		return -EINVAL;
+	}
+
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		u32 left_right;
+		u32 top_bottom;
+
+		left_right = coda_read(dev, CODA_RET_DEC_SEQ_CROP_LEFT_RIGHT);
+		top_bottom = coda_read(dev, CODA_RET_DEC_SEQ_CROP_TOP_BOTTOM);
+
+		q_data_dst->rect.left = (left_right >> 10) & 0x3ff;
+		q_data_dst->rect.top = (top_bottom >> 10) & 0x3ff;
+		q_data_dst->rect.width = width - q_data_dst->rect.left -
+					 (left_right & 0x3ff);
+		q_data_dst->rect.height = height - q_data_dst->rect.top -
+					  (top_bottom & 0x3ff);
+	}
+
+	ret = coda_alloc_framebuffers(ctx, q_data_dst, src_fourcc);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "failed to allocate framebuffers\n");
+		return ret;
+	}
+
+	/* Tell the decoder how many frame buffers we allocated. */
+	coda_write(dev, ctx->num_internal_frames, CODA_CMD_SET_FRAME_BUF_NUM);
+	coda_write(dev, width, CODA_CMD_SET_FRAME_BUF_STRIDE);
+
+	if (dev->devtype->product != CODA_DX6) {
+		/* Set secondary AXI IRAM */
+		coda_setup_iram(ctx);
+
+		coda_write(dev, ctx->iram_info.buf_bit_use,
+				CODA7_CMD_SET_FRAME_AXI_BIT_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ip_ac_dc_use,
+				CODA7_CMD_SET_FRAME_AXI_IPACDC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_y_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKY_ADDR);
+		coda_write(dev, ctx->iram_info.buf_dbk_c_use,
+				CODA7_CMD_SET_FRAME_AXI_DBKC_ADDR);
+		coda_write(dev, ctx->iram_info.buf_ovl_use,
+				CODA7_CMD_SET_FRAME_AXI_OVL_ADDR);
+		if (dev->devtype->product == CODA_960)
+			coda_write(dev, ctx->iram_info.buf_btp_use,
+					CODA9_CMD_SET_FRAME_AXI_BTP_ADDR);
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		coda_write(dev, -1, CODA9_CMD_SET_FRAME_DELAY);
+
+		coda_write(dev, 0x20262024, CODA9_CMD_SET_FRAME_CACHE_SIZE);
+		coda_write(dev, 2 << CODA9_CACHE_PAGEMERGE_OFFSET |
+				32 << CODA9_CACHE_LUMA_BUFFER_SIZE_OFFSET |
+				8 << CODA9_CACHE_CB_BUFFER_SIZE_OFFSET |
+				8 << CODA9_CACHE_CR_BUFFER_SIZE_OFFSET,
+				CODA9_CMD_SET_FRAME_CACHE_CONFIG);
+	}
+
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		coda_write(dev, ctx->slicebuf.paddr,
+				CODA_CMD_SET_FRAME_SLICE_BB_START);
+		coda_write(dev, ctx->slicebuf.size / 1024,
+				CODA_CMD_SET_FRAME_SLICE_BB_SIZE);
+	}
+
+	if (dev->devtype->product == CODA_7541) {
+		int max_mb_x = 1920 / 16;
+		int max_mb_y = 1088 / 16;
+		int max_mb_num = max_mb_x * max_mb_y;
+
+		coda_write(dev, max_mb_num << 16 | max_mb_x << 8 | max_mb_y,
+				CODA7_CMD_SET_FRAME_MAX_DEC_SIZE);
+	} else if (dev->devtype->product == CODA_960) {
+		int max_mb_x = 1920 / 16;
+		int max_mb_y = 1088 / 16;
+		int max_mb_num = max_mb_x * max_mb_y;
+
+		coda_write(dev, max_mb_num << 16 | max_mb_x << 8 | max_mb_y,
+				CODA9_CMD_SET_FRAME_MAX_DEC_SIZE);
+	}
+
+	if (coda_command_sync(ctx, CODA_COMMAND_SET_FRAME_BUF)) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			 "CODA_COMMAND_SET_FRAME_BUF timeout\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int coda_start_decoding(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	int ret;
+
+	mutex_lock(&dev->coda_mutex);
+	ret = __coda_start_decoding(ctx);
+	mutex_unlock(&dev->coda_mutex);
+
+	return ret;
+}
+
+static int coda_prepare_decode(struct coda_ctx *ctx)
+{
+	struct vb2_buffer *dst_buf;
+	struct coda_dev *dev = ctx->dev;
+	struct coda_q_data *q_data_dst;
+	u32 stridey, height;
+	u32 picture_y, picture_cb, picture_cr;
+
+	dst_buf = v4l2_m2m_next_dst_buf(ctx->fh.m2m_ctx);
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	if (ctx->params.rot_mode & CODA_ROT_90) {
+		stridey = q_data_dst->height;
+		height = q_data_dst->width;
+	} else {
+		stridey = q_data_dst->width;
+		height = q_data_dst->height;
+	}
+
+	/* Try to copy source buffer contents into the bitstream ringbuffer */
+	mutex_lock(&ctx->bitstream_mutex);
+	coda_fill_bitstream(ctx);
+	mutex_unlock(&ctx->bitstream_mutex);
+
+	if (coda_get_bitstream_payload(ctx) < 512 &&
+	    (!(ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG))) {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			 "bitstream payload: %d, skipping\n",
+			 coda_get_bitstream_payload(ctx));
+		v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
+		return -EAGAIN;
+	}
+
+	/* Run coda_start_decoding (again) if not yet initialized */
+	if (!ctx->initialized) {
+		int ret = __coda_start_decoding(ctx);
+
+		if (ret < 0) {
+			v4l2_err(&dev->v4l2_dev, "failed to start decoding\n");
+			v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
+			return -EAGAIN;
+		} else {
+			ctx->initialized = 1;
+		}
+	}
+
+	if (dev->devtype->product == CODA_960)
+		coda_set_gdi_regs(ctx);
+
+	/* Set rotator output */
+	picture_y = vb2_dma_contig_plane_dma_addr(dst_buf, 0);
+	if (q_data_dst->fourcc == V4L2_PIX_FMT_YVU420) {
+		/* Switch Cr and Cb for YVU420 format */
+		picture_cr = picture_y + stridey * height;
+		picture_cb = picture_cr + stridey / 2 * height / 2;
+	} else {
+		picture_cb = picture_y + stridey * height;
+		picture_cr = picture_cb + stridey / 2 * height / 2;
+	}
+
+	if (dev->devtype->product == CODA_960) {
+		/*
+		 * The CODA960 seems to have an internal list of buffers with
+		 * 64 entries that includes the registered frame buffers as
+		 * well as the rotator buffer output.
+		 * ROT_INDEX needs to be < 0x40, but > ctx->num_internal_frames.
+		 */
+		coda_write(dev, CODA_MAX_FRAMEBUFFERS + dst_buf->v4l2_buf.index,
+				CODA9_CMD_DEC_PIC_ROT_INDEX);
+		coda_write(dev, picture_y, CODA9_CMD_DEC_PIC_ROT_ADDR_Y);
+		coda_write(dev, picture_cb, CODA9_CMD_DEC_PIC_ROT_ADDR_CB);
+		coda_write(dev, picture_cr, CODA9_CMD_DEC_PIC_ROT_ADDR_CR);
+		coda_write(dev, stridey, CODA9_CMD_DEC_PIC_ROT_STRIDE);
+	} else {
+		coda_write(dev, picture_y, CODA_CMD_DEC_PIC_ROT_ADDR_Y);
+		coda_write(dev, picture_cb, CODA_CMD_DEC_PIC_ROT_ADDR_CB);
+		coda_write(dev, picture_cr, CODA_CMD_DEC_PIC_ROT_ADDR_CR);
+		coda_write(dev, stridey, CODA_CMD_DEC_PIC_ROT_STRIDE);
+	}
+	coda_write(dev, CODA_ROT_MIR_ENABLE | ctx->params.rot_mode,
+			CODA_CMD_DEC_PIC_ROT_MODE);
+
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		/* TBD */
+	case CODA_7541:
+		coda_write(dev, CODA_PRE_SCAN_EN, CODA_CMD_DEC_PIC_OPTION);
+		break;
+	case CODA_960:
+		/* 'hardcode to use interrupt disable mode'? */
+		coda_write(dev, (1 << 10), CODA_CMD_DEC_PIC_OPTION);
+		break;
+	}
+
+	coda_write(dev, 0, CODA_CMD_DEC_PIC_SKIP_NUM);
+
+	coda_write(dev, 0, CODA_CMD_DEC_PIC_BB_START);
+	coda_write(dev, 0, CODA_CMD_DEC_PIC_START_BYTE);
+
+	if (dev->devtype->product != CODA_DX6)
+		coda_write(dev, ctx->iram_info.axi_sram_use,
+				CODA7_REG_BIT_AXI_SRAM_USE);
+
+	coda_kfifo_sync_to_device_full(ctx);
+
+	coda_command_async(ctx, CODA_COMMAND_PIC_RUN);
+
+	return 0;
+}
+
+static void coda_finish_decode(struct coda_ctx *ctx)
+{
+	struct coda_dev *dev = ctx->dev;
+	struct coda_q_data *q_data_src;
+	struct coda_q_data *q_data_dst;
+	struct vb2_buffer *dst_buf;
+	struct coda_timestamp *ts;
+	int width, height;
+	int decoded_idx;
+	int display_idx;
+	u32 src_fourcc;
+	int success;
+	u32 err_mb;
+	u32 val;
+
+	/* Update kfifo out pointer from coda bitstream read pointer */
+	coda_kfifo_sync_from_device(ctx);
+
+	/*
+	 * in stream-end mode, the read pointer can overshoot the write pointer
+	 * by up to 512 bytes
+	 */
+	if (ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) {
+		if (coda_get_bitstream_payload(ctx) >= CODA_MAX_FRAME_SIZE - 512)
+			kfifo_init(&ctx->bitstream_fifo,
+				ctx->bitstream.vaddr, ctx->bitstream.size);
+	}
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	src_fourcc = q_data_src->fourcc;
+
+	val = coda_read(dev, CODA_RET_DEC_PIC_SUCCESS);
+	if (val != 1)
+		pr_err("DEC_PIC_SUCCESS = %d\n", val);
+
+	success = val & 0x1;
+	if (!success)
+		v4l2_err(&dev->v4l2_dev, "decode failed\n");
+
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		if (val & (1 << 3))
+			v4l2_err(&dev->v4l2_dev,
+				 "insufficient PS buffer space (%d bytes)\n",
+				 ctx->psbuf.size);
+		if (val & (1 << 2))
+			v4l2_err(&dev->v4l2_dev,
+				 "insufficient slice buffer space (%d bytes)\n",
+				 ctx->slicebuf.size);
+	}
+
+	val = coda_read(dev, CODA_RET_DEC_PIC_SIZE);
+	width = (val >> 16) & 0xffff;
+	height = val & 0xffff;
+
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	/* frame crop information */
+	if (src_fourcc == V4L2_PIX_FMT_H264) {
+		u32 left_right;
+		u32 top_bottom;
+
+		left_right = coda_read(dev, CODA_RET_DEC_PIC_CROP_LEFT_RIGHT);
+		top_bottom = coda_read(dev, CODA_RET_DEC_PIC_CROP_TOP_BOTTOM);
+
+		if (left_right == 0xffffffff && top_bottom == 0xffffffff) {
+			/* Keep current crop information */
+		} else {
+			struct v4l2_rect *rect = &q_data_dst->rect;
+
+			rect->left = left_right >> 16 & 0xffff;
+			rect->top = top_bottom >> 16 & 0xffff;
+			rect->width = width - rect->left -
+				      (left_right & 0xffff);
+			rect->height = height - rect->top -
+				       (top_bottom & 0xffff);
+		}
+	} else {
+		/* no cropping */
+	}
+
+	err_mb = coda_read(dev, CODA_RET_DEC_PIC_ERR_MB);
+	if (err_mb > 0)
+		v4l2_err(&dev->v4l2_dev,
+			 "errors in %d macroblocks\n", err_mb);
+
+	if (dev->devtype->product == CODA_7541) {
+		val = coda_read(dev, CODA_RET_DEC_PIC_OPTION);
+		if (val == 0) {
+			/* not enough bitstream data */
+			v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+				 "prescan failed: %d\n", val);
+			ctx->hold = true;
+			return;
+		}
+	}
+
+	ctx->frm_dis_flg = coda_read(dev,
+				     CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+
+	/*
+	 * The previous display frame was copied out by the rotator,
+	 * now it can be overwritten again
+	 */
+	if (ctx->display_idx >= 0 &&
+	    ctx->display_idx < ctx->num_internal_frames) {
+		ctx->frm_dis_flg &= ~(1 << ctx->display_idx);
+		coda_write(dev, ctx->frm_dis_flg,
+				CODA_REG_BIT_FRM_DIS_FLG(ctx->reg_idx));
+	}
+
+	/*
+	 * The index of the last decoded frame, not necessarily in
+	 * display order, and the index of the next display frame.
+	 * The latter could have been decoded in a previous run.
+	 */
+	decoded_idx = coda_read(dev, CODA_RET_DEC_PIC_CUR_IDX);
+	display_idx = coda_read(dev, CODA_RET_DEC_PIC_FRAME_IDX);
+
+	if (decoded_idx == -1) {
+		/* no frame was decoded, but we might have a display frame */
+		if (display_idx >= 0 && display_idx < ctx->num_internal_frames)
+			ctx->sequence_offset++;
+		else if (ctx->display_idx < 0)
+			ctx->hold = true;
+	} else if (decoded_idx == -2) {
+		/* no frame was decoded, we still return remaining buffers */
+	} else if (decoded_idx < 0 || decoded_idx >= ctx->num_internal_frames) {
+		v4l2_err(&dev->v4l2_dev,
+			 "decoded frame index out of range: %d\n", decoded_idx);
+	} else {
+		val = coda_read(dev, CODA_RET_DEC_PIC_FRAME_NUM) - 1;
+		val -= ctx->sequence_offset;
+		mutex_lock(&ctx->bitstream_mutex);
+		if (!list_empty(&ctx->timestamp_list)) {
+			ts = list_first_entry(&ctx->timestamp_list,
+					      struct coda_timestamp, list);
+			list_del(&ts->list);
+			if (val != (ts->sequence & 0xffff)) {
+				v4l2_err(&dev->v4l2_dev,
+					 "sequence number mismatch (%d(%d) != %d)\n",
+					 val, ctx->sequence_offset,
+					 ts->sequence);
+			}
+			ctx->frame_timestamps[decoded_idx] = *ts;
+			kfree(ts);
+		} else {
+			v4l2_err(&dev->v4l2_dev, "empty timestamp list!\n");
+			memset(&ctx->frame_timestamps[decoded_idx], 0,
+			       sizeof(struct coda_timestamp));
+			ctx->frame_timestamps[decoded_idx].sequence = val;
+		}
+		mutex_unlock(&ctx->bitstream_mutex);
+
+		val = coda_read(dev, CODA_RET_DEC_PIC_TYPE) & 0x7;
+		if (val == 0)
+			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_KEYFRAME;
+		else if (val == 1)
+			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_PFRAME;
+		else
+			ctx->frame_types[decoded_idx] = V4L2_BUF_FLAG_BFRAME;
+
+		ctx->frame_errors[decoded_idx] = err_mb;
+	}
+
+	if (display_idx == -1) {
+		/*
+		 * no more frames to be decoded, but there could still
+		 * be rotator output to dequeue
+		 */
+		ctx->hold = true;
+	} else if (display_idx == -3) {
+		/* possibly prescan failure */
+	} else if (display_idx < 0 || display_idx >= ctx->num_internal_frames) {
+		v4l2_err(&dev->v4l2_dev,
+			 "presentation frame index out of range: %d\n",
+			 display_idx);
+	}
+
+	/* If a frame was copied out, return it */
+	if (ctx->display_idx >= 0 &&
+	    ctx->display_idx < ctx->num_internal_frames) {
+		dst_buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx);
+		dst_buf->v4l2_buf.sequence = ctx->osequence++;
+
+		dst_buf->v4l2_buf.flags &= ~(V4L2_BUF_FLAG_KEYFRAME |
+					     V4L2_BUF_FLAG_PFRAME |
+					     V4L2_BUF_FLAG_BFRAME);
+		dst_buf->v4l2_buf.flags |= ctx->frame_types[ctx->display_idx];
+		ts = &ctx->frame_timestamps[ctx->display_idx];
+		dst_buf->v4l2_buf.timecode = ts->timecode;
+		dst_buf->v4l2_buf.timestamp = ts->timestamp;
+
+		vb2_set_plane_payload(dst_buf, 0, width * height * 3 / 2);
+
+		v4l2_m2m_buf_done(dst_buf, ctx->frame_errors[display_idx] ?
+				  VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			"job finished: decoding frame (%d) (%s)\n",
+			dst_buf->v4l2_buf.sequence,
+			(dst_buf->v4l2_buf.flags & V4L2_BUF_FLAG_KEYFRAME) ?
+			"KEYFRAME" : "PFRAME");
+	} else {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			"job finished: no frame decoded\n");
+	}
+
+	/* The rotator will copy the current display frame next time */
+	ctx->display_idx = display_idx;
+}
+
+const struct coda_context_ops coda_bit_decode_ops = {
+	.queue_init = coda_decoder_queue_init,
+	.start_streaming = coda_start_decoding,
+	.prepare_run = coda_prepare_decode,
+	.finish_run = coda_finish_decode,
+	.seq_end_work = coda_seq_end_work,
+	.release = coda_bit_release,
+};
+
+irqreturn_t coda_irq_handler(int irq, void *data)
+{
+	struct coda_dev *dev = data;
+	struct coda_ctx *ctx;
+
+	/* read status register to attend the IRQ */
+	coda_read(dev, CODA_REG_BIT_INT_STATUS);
+	coda_write(dev, CODA_REG_BIT_INT_CLEAR_SET,
+		      CODA_REG_BIT_INT_CLEAR);
+
+	ctx = v4l2_m2m_get_curr_priv(dev->m2m_dev);
+	if (ctx == NULL) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Instance released before the end of transaction\n");
+		mutex_unlock(&dev->coda_mutex);
+		return IRQ_HANDLED;
+	}
+
+	if (ctx->aborting) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "task has been aborted\n");
+	}
+
+	if (coda_isbusy(ctx->dev)) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "coda is still busy!!!!\n");
+		return IRQ_NONE;
+	}
+
+	complete(&ctx->completion);
+
+	return IRQ_HANDLED;
+}
diff --git a/drivers/media/platform/coda/coda-common.c b/drivers/media/platform/coda/coda-common.c
new file mode 100644
index 000000000000..ced47609f5ef
--- /dev/null
+++ b/drivers/media/platform/coda/coda-common.c
@@ -0,0 +1,2052 @@
+/*
+ * Coda multi-standard codec IP
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/firmware.h>
+#include <linux/genalloc.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kfifo.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/slab.h>
+#include <linux/videodev2.h>
+#include <linux/of.h>
+#include <linux/platform_data/coda.h>
+#include <linux/reset.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-mem2mem.h>
+#include <media/videobuf2-core.h>
+#include <media/videobuf2-dma-contig.h>
+
+#include "coda.h"
+
+#define CODA_NAME		"coda"
+
+#define CODADX6_MAX_INSTANCES	4
+
+#define CODA_PARA_BUF_SIZE	(10 * 1024)
+#define CODA_ISRAM_SIZE	(2048 * 2)
+
+#define MIN_W 176
+#define MIN_H 144
+
+#define S_ALIGN		1 /* multiple of 2 */
+#define W_ALIGN		1 /* multiple of 2 */
+#define H_ALIGN		1 /* multiple of 2 */
+
+#define fh_to_ctx(__fh)	container_of(__fh, struct coda_ctx, fh)
+
+int coda_debug;
+module_param(coda_debug, int, 0644);
+MODULE_PARM_DESC(coda_debug, "Debug level (0-2)");
+
+struct coda_fmt {
+	char *name;
+	u32 fourcc;
+};
+
+void coda_write(struct coda_dev *dev, u32 data, u32 reg)
+{
+	v4l2_dbg(2, coda_debug, &dev->v4l2_dev,
+		 "%s: data=0x%x, reg=0x%x\n", __func__, data, reg);
+	writel(data, dev->regs_base + reg);
+}
+
+unsigned int coda_read(struct coda_dev *dev, u32 reg)
+{
+	u32 data;
+
+	data = readl(dev->regs_base + reg);
+	v4l2_dbg(2, coda_debug, &dev->v4l2_dev,
+		 "%s: data=0x%x, reg=0x%x\n", __func__, data, reg);
+	return data;
+}
+
+/*
+ * Array of all formats supported by any version of Coda:
+ */
+static const struct coda_fmt coda_formats[] = {
+	{
+		.name = "YUV 4:2:0 Planar, YCbCr",
+		.fourcc = V4L2_PIX_FMT_YUV420,
+	},
+	{
+		.name = "YUV 4:2:0 Planar, YCrCb",
+		.fourcc = V4L2_PIX_FMT_YVU420,
+	},
+	{
+		.name = "H264 Encoded Stream",
+		.fourcc = V4L2_PIX_FMT_H264,
+	},
+	{
+		.name = "MPEG4 Encoded Stream",
+		.fourcc = V4L2_PIX_FMT_MPEG4,
+	},
+};
+
+#define CODA_CODEC(mode, src_fourcc, dst_fourcc, max_w, max_h) \
+	{ mode, src_fourcc, dst_fourcc, max_w, max_h }
+
+/*
+ * Arrays of codecs supported by each given version of Coda:
+ *  i.MX27 -> codadx6
+ *  i.MX5x -> coda7
+ *  i.MX6  -> coda960
+ * Use V4L2_PIX_FMT_YUV420 as placeholder for all supported YUV 4:2:0 variants
+ */
+static const struct coda_codec codadx6_codecs[] = {
+	CODA_CODEC(CODADX6_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,  720, 576),
+	CODA_CODEC(CODADX6_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4, 720, 576),
+};
+
+static const struct coda_codec coda7_codecs[] = {
+	CODA_CODEC(CODA7_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,   1280, 720),
+	CODA_CODEC(CODA7_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1280, 720),
+	CODA_CODEC(CODA7_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1088),
+	CODA_CODEC(CODA7_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1088),
+};
+
+static const struct coda_codec coda9_codecs[] = {
+	CODA_CODEC(CODA9_MODE_ENCODE_H264, V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_H264,   1920, 1088),
+	CODA_CODEC(CODA9_MODE_ENCODE_MP4,  V4L2_PIX_FMT_YUV420, V4L2_PIX_FMT_MPEG4,  1920, 1088),
+	CODA_CODEC(CODA9_MODE_DECODE_H264, V4L2_PIX_FMT_H264,   V4L2_PIX_FMT_YUV420, 1920, 1088),
+	CODA_CODEC(CODA9_MODE_DECODE_MP4,  V4L2_PIX_FMT_MPEG4,  V4L2_PIX_FMT_YUV420, 1920, 1088),
+};
+
+static bool coda_format_is_yuv(u32 fourcc)
+{
+	switch (fourcc) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		return true;
+	default:
+		return false;
+	}
+}
+
+/*
+ * Normalize all supported YUV 4:2:0 formats to the value used in the codec
+ * tables.
+ */
+static u32 coda_format_normalize_yuv(u32 fourcc)
+{
+	return coda_format_is_yuv(fourcc) ? V4L2_PIX_FMT_YUV420 : fourcc;
+}
+
+static const struct coda_codec *coda_find_codec(struct coda_dev *dev,
+						int src_fourcc, int dst_fourcc)
+{
+	const struct coda_codec *codecs = dev->devtype->codecs;
+	int num_codecs = dev->devtype->num_codecs;
+	int k;
+
+	src_fourcc = coda_format_normalize_yuv(src_fourcc);
+	dst_fourcc = coda_format_normalize_yuv(dst_fourcc);
+	if (src_fourcc == dst_fourcc)
+		return NULL;
+
+	for (k = 0; k < num_codecs; k++) {
+		if (codecs[k].src_fourcc == src_fourcc &&
+		    codecs[k].dst_fourcc == dst_fourcc)
+			break;
+	}
+
+	if (k == num_codecs)
+		return NULL;
+
+	return &codecs[k];
+}
+
+static void coda_get_max_dimensions(struct coda_dev *dev,
+				    const struct coda_codec *codec,
+				    int *max_w, int *max_h)
+{
+	const struct coda_codec *codecs = dev->devtype->codecs;
+	int num_codecs = dev->devtype->num_codecs;
+	unsigned int w, h;
+	int k;
+
+	if (codec) {
+		w = codec->max_w;
+		h = codec->max_h;
+	} else {
+		for (k = 0, w = 0, h = 0; k < num_codecs; k++) {
+			w = max(w, codecs[k].max_w);
+			h = max(h, codecs[k].max_h);
+		}
+	}
+
+	if (max_w)
+		*max_w = w;
+	if (max_h)
+		*max_h = h;
+}
+
+const char *coda_product_name(int product)
+{
+	static char buf[9];
+
+	switch (product) {
+	case CODA_DX6:
+		return "CodaDx6";
+	case CODA_7541:
+		return "CODA7541";
+	case CODA_960:
+		return "CODA960";
+	default:
+		snprintf(buf, sizeof(buf), "(0x%04x)", product);
+		return buf;
+	}
+}
+
+/*
+ * V4L2 ioctl() operations.
+ */
+static int coda_querycap(struct file *file, void *priv,
+			 struct v4l2_capability *cap)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+
+	strlcpy(cap->driver, CODA_NAME, sizeof(cap->driver));
+	strlcpy(cap->card, coda_product_name(ctx->dev->devtype->product),
+		sizeof(cap->card));
+	strlcpy(cap->bus_info, "platform:" CODA_NAME, sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_VIDEO_M2M | V4L2_CAP_STREAMING;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int coda_enum_fmt(struct file *file, void *priv,
+			 struct v4l2_fmtdesc *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	const struct coda_codec *codecs = ctx->dev->devtype->codecs;
+	const struct coda_fmt *formats = coda_formats;
+	const struct coda_fmt *fmt;
+	int num_codecs = ctx->dev->devtype->num_codecs;
+	int num_formats = ARRAY_SIZE(coda_formats);
+	int i, k, num = 0;
+	bool yuv;
+
+	if (ctx->inst_type == CODA_INST_ENCODER)
+		yuv = (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	else
+		yuv = (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE);
+
+	for (i = 0; i < num_formats; i++) {
+		/* Skip either raw or compressed formats */
+		if (yuv != coda_format_is_yuv(formats[i].fourcc))
+			continue;
+		/* All uncompressed formats are always supported */
+		if (yuv) {
+			if (num == f->index)
+				break;
+			++num;
+			continue;
+		}
+		/* Compressed formats may be supported, check the codec list */
+		for (k = 0; k < num_codecs; k++) {
+			if (f->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+			    formats[i].fourcc == codecs[k].dst_fourcc)
+				break;
+			if (f->type == V4L2_BUF_TYPE_VIDEO_OUTPUT &&
+			    formats[i].fourcc == codecs[k].src_fourcc)
+				break;
+		}
+		if (k < num_codecs) {
+			if (num == f->index)
+				break;
+			++num;
+		}
+	}
+
+	if (i < num_formats) {
+		fmt = &formats[i];
+		strlcpy(f->description, fmt->name, sizeof(f->description));
+		f->pixelformat = fmt->fourcc;
+		if (!yuv)
+			f->flags |= V4L2_FMT_FLAG_COMPRESSED;
+		return 0;
+	}
+
+	/* Format not found */
+	return -EINVAL;
+}
+
+static int coda_g_fmt(struct file *file, void *priv,
+		      struct v4l2_format *f)
+{
+	struct coda_q_data *q_data;
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	f->fmt.pix.field	= V4L2_FIELD_NONE;
+	f->fmt.pix.pixelformat	= q_data->fourcc;
+	f->fmt.pix.width	= q_data->width;
+	f->fmt.pix.height	= q_data->height;
+	f->fmt.pix.bytesperline = q_data->bytesperline;
+
+	f->fmt.pix.sizeimage	= q_data->sizeimage;
+	f->fmt.pix.colorspace	= ctx->colorspace;
+
+	return 0;
+}
+
+static int coda_try_fmt(struct coda_ctx *ctx, const struct coda_codec *codec,
+			struct v4l2_format *f)
+{
+	struct coda_dev *dev = ctx->dev;
+	struct coda_q_data *q_data;
+	unsigned int max_w, max_h;
+	enum v4l2_field field;
+
+	field = f->fmt.pix.field;
+	if (field == V4L2_FIELD_ANY)
+		field = V4L2_FIELD_NONE;
+	else if (V4L2_FIELD_NONE != field)
+		return -EINVAL;
+
+	/* V4L2 specification suggests the driver corrects the format struct
+	 * if any of the dimensions is unsupported */
+	f->fmt.pix.field = field;
+
+	coda_get_max_dimensions(dev, codec, &max_w, &max_h);
+	v4l_bound_align_image(&f->fmt.pix.width, MIN_W, max_w, W_ALIGN,
+			      &f->fmt.pix.height, MIN_H, max_h, H_ALIGN,
+			      S_ALIGN);
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+	case V4L2_PIX_FMT_H264:
+	case V4L2_PIX_FMT_MPEG4:
+	case V4L2_PIX_FMT_JPEG:
+		break;
+	default:
+		q_data = get_q_data(ctx, f->type);
+		if (!q_data)
+			return -EINVAL;
+		f->fmt.pix.pixelformat = q_data->fourcc;
+	}
+
+	switch (f->fmt.pix.pixelformat) {
+	case V4L2_PIX_FMT_YUV420:
+	case V4L2_PIX_FMT_YVU420:
+		/* Frame stride must be multiple of 8, but 16 for h.264 */
+		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
+		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
+					f->fmt.pix.height * 3 / 2;
+		break;
+	case V4L2_PIX_FMT_H264:
+	case V4L2_PIX_FMT_MPEG4:
+	case V4L2_PIX_FMT_JPEG:
+		f->fmt.pix.bytesperline = 0;
+		f->fmt.pix.sizeimage = CODA_MAX_FRAME_SIZE;
+		break;
+	default:
+		BUG();
+	}
+
+	return 0;
+}
+
+static int coda_try_fmt_vid_cap(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	const struct coda_codec *codec = NULL;
+	struct vb2_queue *src_vq;
+	int ret;
+
+	/*
+	 * If the source format is already fixed, try to find a codec that
+	 * converts to the given destination format
+	 */
+	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	if (vb2_is_streaming(src_vq)) {
+		struct coda_q_data *q_data_src;
+
+		q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+		codec = coda_find_codec(ctx->dev, q_data_src->fourcc,
+					f->fmt.pix.pixelformat);
+		if (!codec)
+			return -EINVAL;
+
+		f->fmt.pix.width = q_data_src->width;
+		f->fmt.pix.height = q_data_src->height;
+	} else {
+		/* Otherwise determine codec by encoded format, if possible */
+		codec = coda_find_codec(ctx->dev, V4L2_PIX_FMT_YUV420,
+					f->fmt.pix.pixelformat);
+	}
+
+	f->fmt.pix.colorspace = ctx->colorspace;
+
+	ret = coda_try_fmt(ctx, codec, f);
+	if (ret < 0)
+		return ret;
+
+	/* The h.264 decoder only returns complete 16x16 macroblocks */
+	if (codec && codec->src_fourcc == V4L2_PIX_FMT_H264) {
+		f->fmt.pix.width = f->fmt.pix.width;
+		f->fmt.pix.height = round_up(f->fmt.pix.height, 16);
+		f->fmt.pix.bytesperline = round_up(f->fmt.pix.width, 16);
+		f->fmt.pix.sizeimage = f->fmt.pix.bytesperline *
+				       f->fmt.pix.height * 3 / 2;
+	}
+
+	return 0;
+}
+
+static int coda_try_fmt_vid_out(struct file *file, void *priv,
+				struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	const struct coda_codec *codec = NULL;
+
+	/* Determine codec by encoded format, returns NULL if raw or invalid */
+	if (ctx->inst_type == CODA_INST_DECODER) {
+		codec = coda_find_codec(ctx->dev, f->fmt.pix.pixelformat,
+					V4L2_PIX_FMT_YUV420);
+		if (!codec)
+			codec = coda_find_codec(ctx->dev, V4L2_PIX_FMT_H264,
+						V4L2_PIX_FMT_YUV420);
+		if (!codec)
+			return -EINVAL;
+	}
+
+	if (!f->fmt.pix.colorspace)
+		f->fmt.pix.colorspace = V4L2_COLORSPACE_REC709;
+
+	return coda_try_fmt(ctx, codec, f);
+}
+
+static int coda_s_fmt(struct coda_ctx *ctx, struct v4l2_format *f)
+{
+	struct coda_q_data *q_data;
+	struct vb2_queue *vq;
+
+	vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, f->type);
+	if (!vq)
+		return -EINVAL;
+
+	q_data = get_q_data(ctx, f->type);
+	if (!q_data)
+		return -EINVAL;
+
+	if (vb2_is_busy(vq)) {
+		v4l2_err(&ctx->dev->v4l2_dev, "%s queue busy\n", __func__);
+		return -EBUSY;
+	}
+
+	q_data->fourcc = f->fmt.pix.pixelformat;
+	q_data->width = f->fmt.pix.width;
+	q_data->height = f->fmt.pix.height;
+	q_data->bytesperline = f->fmt.pix.bytesperline;
+	q_data->sizeimage = f->fmt.pix.sizeimage;
+	q_data->rect.left = 0;
+	q_data->rect.top = 0;
+	q_data->rect.width = f->fmt.pix.width;
+	q_data->rect.height = f->fmt.pix.height;
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		"Setting format for type %d, wxh: %dx%d, fmt: %d\n",
+		f->type, q_data->width, q_data->height, q_data->fourcc);
+
+	return 0;
+}
+
+static int coda_s_fmt_vid_cap(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+
+	ret = coda_try_fmt_vid_cap(file, priv, f);
+	if (ret)
+		return ret;
+
+	return coda_s_fmt(ctx, f);
+}
+
+static int coda_s_fmt_vid_out(struct file *file, void *priv,
+			      struct v4l2_format *f)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	struct v4l2_format f_cap;
+	int ret;
+
+	ret = coda_try_fmt_vid_out(file, priv, f);
+	if (ret)
+		return ret;
+
+	ret = coda_s_fmt(ctx, f);
+	if (ret)
+		return ret;
+
+	ctx->colorspace = f->fmt.pix.colorspace;
+
+	f_cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	coda_g_fmt(file, priv, &f_cap);
+	f_cap.fmt.pix.width = f->fmt.pix.width;
+	f_cap.fmt.pix.height = f->fmt.pix.height;
+
+	ret = coda_try_fmt_vid_cap(file, priv, &f_cap);
+	if (ret)
+		return ret;
+
+	return coda_s_fmt(ctx, &f_cap);
+}
+
+static int coda_qbuf(struct file *file, void *priv,
+		     struct v4l2_buffer *buf)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+
+	return v4l2_m2m_qbuf(file, ctx->fh.m2m_ctx, buf);
+}
+
+static bool coda_buf_is_end_of_stream(struct coda_ctx *ctx,
+				      struct v4l2_buffer *buf)
+{
+	struct vb2_queue *src_vq;
+
+	src_vq = v4l2_m2m_get_vq(ctx->fh.m2m_ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+
+	return ((ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG) &&
+		(buf->sequence == (ctx->qsequence - 1)));
+}
+
+static int coda_dqbuf(struct file *file, void *priv,
+		      struct v4l2_buffer *buf)
+{
+	struct coda_ctx *ctx = fh_to_ctx(priv);
+	int ret;
+
+	ret = v4l2_m2m_dqbuf(file, ctx->fh.m2m_ctx, buf);
+
+	/* If this is the last capture buffer, emit an end-of-stream event */
+	if (buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+	    coda_buf_is_end_of_stream(ctx, buf)) {
+		const struct v4l2_event eos_event = {
+			.type = V4L2_EVENT_EOS
+		};
+
+		v4l2_event_queue_fh(&ctx->fh, &eos_event);
+	}
+
+	return ret;
+}
+
+static int coda_g_selection(struct file *file, void *fh,
+			    struct v4l2_selection *s)
+{
+	struct coda_ctx *ctx = fh_to_ctx(fh);
+	struct coda_q_data *q_data;
+	struct v4l2_rect r, *rsel;
+
+	q_data = get_q_data(ctx, s->type);
+	if (!q_data)
+		return -EINVAL;
+
+	r.left = 0;
+	r.top = 0;
+	r.width = q_data->width;
+	r.height = q_data->height;
+	rsel = &q_data->rect;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		rsel = &r;
+		/* fallthrough */
+	case V4L2_SEL_TGT_CROP:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			return -EINVAL;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+	case V4L2_SEL_TGT_COMPOSE_PADDED:
+		rsel = &r;
+		/* fallthrough */
+	case V4L2_SEL_TGT_COMPOSE:
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	s->r = *rsel;
+
+	return 0;
+}
+
+static int coda_try_decoder_cmd(struct file *file, void *fh,
+				struct v4l2_decoder_cmd *dc)
+{
+	if (dc->cmd != V4L2_DEC_CMD_STOP)
+		return -EINVAL;
+
+	if (dc->flags & V4L2_DEC_CMD_STOP_TO_BLACK)
+		return -EINVAL;
+
+	if (!(dc->flags & V4L2_DEC_CMD_STOP_IMMEDIATELY) && (dc->stop.pts != 0))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int coda_decoder_cmd(struct file *file, void *fh,
+			    struct v4l2_decoder_cmd *dc)
+{
+	struct coda_ctx *ctx = fh_to_ctx(fh);
+	int ret;
+
+	ret = coda_try_decoder_cmd(file, fh, dc);
+	if (ret < 0)
+		return ret;
+
+	/* Ignore decoder stop command silently in encoder context */
+	if (ctx->inst_type != CODA_INST_DECODER)
+		return 0;
+
+	/* Set the stream-end flag on this context */
+	coda_bit_stream_end_flag(ctx);
+	ctx->hold = false;
+	v4l2_m2m_try_schedule(ctx->fh.m2m_ctx);
+
+	return 0;
+}
+
+static int coda_subscribe_event(struct v4l2_fh *fh,
+				const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_EOS:
+		return v4l2_event_subscribe(fh, sub, 0, NULL);
+	default:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	}
+}
+
+static const struct v4l2_ioctl_ops coda_ioctl_ops = {
+	.vidioc_querycap	= coda_querycap,
+
+	.vidioc_enum_fmt_vid_cap = coda_enum_fmt,
+	.vidioc_g_fmt_vid_cap	= coda_g_fmt,
+	.vidioc_try_fmt_vid_cap	= coda_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap	= coda_s_fmt_vid_cap,
+
+	.vidioc_enum_fmt_vid_out = coda_enum_fmt,
+	.vidioc_g_fmt_vid_out	= coda_g_fmt,
+	.vidioc_try_fmt_vid_out	= coda_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out	= coda_s_fmt_vid_out,
+
+	.vidioc_reqbufs		= v4l2_m2m_ioctl_reqbufs,
+	.vidioc_querybuf	= v4l2_m2m_ioctl_querybuf,
+
+	.vidioc_qbuf		= coda_qbuf,
+	.vidioc_expbuf		= v4l2_m2m_ioctl_expbuf,
+	.vidioc_dqbuf		= coda_dqbuf,
+	.vidioc_create_bufs	= v4l2_m2m_ioctl_create_bufs,
+
+	.vidioc_streamon	= v4l2_m2m_ioctl_streamon,
+	.vidioc_streamoff	= v4l2_m2m_ioctl_streamoff,
+
+	.vidioc_g_selection	= coda_g_selection,
+
+	.vidioc_try_decoder_cmd	= coda_try_decoder_cmd,
+	.vidioc_decoder_cmd	= coda_decoder_cmd,
+
+	.vidioc_subscribe_event = coda_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+};
+
+void coda_set_gdi_regs(struct coda_ctx *ctx)
+{
+	struct gdi_tiled_map *tiled_map = &ctx->tiled_map;
+	struct coda_dev *dev = ctx->dev;
+	int i;
+
+	for (i = 0; i < 16; i++)
+		coda_write(dev, tiled_map->xy2ca_map[i],
+				CODA9_GDI_XY2_CAS_0 + 4 * i);
+	for (i = 0; i < 4; i++)
+		coda_write(dev, tiled_map->xy2ba_map[i],
+				CODA9_GDI_XY2_BA_0 + 4 * i);
+	for (i = 0; i < 16; i++)
+		coda_write(dev, tiled_map->xy2ra_map[i],
+				CODA9_GDI_XY2_RAS_0 + 4 * i);
+	coda_write(dev, tiled_map->xy2rbc_config, CODA9_GDI_XY2_RBC_CONFIG);
+	for (i = 0; i < 32; i++)
+		coda_write(dev, tiled_map->rbc2axi_map[i],
+				CODA9_GDI_RBC2_AXI_0 + 4 * i);
+}
+
+/*
+ * Mem-to-mem operations.
+ */
+
+static void coda_device_run(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+	struct coda_dev *dev = ctx->dev;
+
+	queue_work(dev->workqueue, &ctx->pic_run_work);
+}
+
+static void coda_pic_run_work(struct work_struct *work)
+{
+	struct coda_ctx *ctx = container_of(work, struct coda_ctx, pic_run_work);
+	struct coda_dev *dev = ctx->dev;
+	int ret;
+
+	mutex_lock(&ctx->buffer_mutex);
+	mutex_lock(&dev->coda_mutex);
+
+	ret = ctx->ops->prepare_run(ctx);
+	if (ret < 0 && ctx->inst_type == CODA_INST_DECODER) {
+		mutex_unlock(&dev->coda_mutex);
+		mutex_unlock(&ctx->buffer_mutex);
+		/* job_finish scheduled by prepare_decode */
+		return;
+	}
+
+	if (!wait_for_completion_timeout(&ctx->completion,
+					 msecs_to_jiffies(1000))) {
+		dev_err(&dev->plat_dev->dev, "CODA PIC_RUN timeout\n");
+
+		ctx->hold = true;
+
+		coda_hw_reset(ctx);
+	} else if (!ctx->aborting) {
+		ctx->ops->finish_run(ctx);
+	}
+
+	if (ctx->aborting || (!ctx->streamon_cap && !ctx->streamon_out))
+		queue_work(dev->workqueue, &ctx->seq_end_work);
+
+	mutex_unlock(&dev->coda_mutex);
+	mutex_unlock(&ctx->buffer_mutex);
+
+	v4l2_m2m_job_finish(ctx->dev->m2m_dev, ctx->fh.m2m_ctx);
+}
+
+static int coda_job_ready(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+
+	/*
+	 * For both 'P' and 'key' frame cases 1 picture
+	 * and 1 frame are needed. In the decoder case,
+	 * the compressed frame can be in the bitstream.
+	 */
+	if (!v4l2_m2m_num_src_bufs_ready(ctx->fh.m2m_ctx) &&
+	    ctx->inst_type != CODA_INST_DECODER) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "not ready: not enough video buffers.\n");
+		return 0;
+	}
+
+	if (!v4l2_m2m_num_dst_bufs_ready(ctx->fh.m2m_ctx)) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "not ready: not enough video capture buffers.\n");
+		return 0;
+	}
+
+	if (ctx->hold ||
+	    ((ctx->inst_type == CODA_INST_DECODER) &&
+	     (coda_get_bitstream_payload(ctx) < 512) &&
+	     !(ctx->bit_stream_param & CODA_BIT_STREAM_END_FLAG))) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "%d: not ready: not enough bitstream data.\n",
+			 ctx->idx);
+		return 0;
+	}
+
+	if (ctx->aborting) {
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			 "not ready: aborting\n");
+		return 0;
+	}
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			"job ready\n");
+	return 1;
+}
+
+static void coda_job_abort(void *priv)
+{
+	struct coda_ctx *ctx = priv;
+
+	ctx->aborting = 1;
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		 "Aborting task\n");
+}
+
+static void coda_lock(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+	struct coda_dev *pcdev = ctx->dev;
+
+	mutex_lock(&pcdev->dev_mutex);
+}
+
+static void coda_unlock(void *m2m_priv)
+{
+	struct coda_ctx *ctx = m2m_priv;
+	struct coda_dev *pcdev = ctx->dev;
+
+	mutex_unlock(&pcdev->dev_mutex);
+}
+
+static const struct v4l2_m2m_ops coda_m2m_ops = {
+	.device_run	= coda_device_run,
+	.job_ready	= coda_job_ready,
+	.job_abort	= coda_job_abort,
+	.lock		= coda_lock,
+	.unlock		= coda_unlock,
+};
+
+static void coda_set_tiled_map_type(struct coda_ctx *ctx, int tiled_map_type)
+{
+	struct gdi_tiled_map *tiled_map = &ctx->tiled_map;
+	int luma_map, chro_map, i;
+
+	memset(tiled_map, 0, sizeof(*tiled_map));
+
+	luma_map = 64;
+	chro_map = 64;
+	tiled_map->map_type = tiled_map_type;
+	for (i = 0; i < 16; i++)
+		tiled_map->xy2ca_map[i] = luma_map << 8 | chro_map;
+	for (i = 0; i < 4; i++)
+		tiled_map->xy2ba_map[i] = luma_map << 8 | chro_map;
+	for (i = 0; i < 16; i++)
+		tiled_map->xy2ra_map[i] = luma_map << 8 | chro_map;
+
+	if (tiled_map_type == GDI_LINEAR_FRAME_MAP) {
+		tiled_map->xy2rbc_config = 0;
+	} else {
+		dev_err(&ctx->dev->plat_dev->dev, "invalid map type: %d\n",
+			tiled_map_type);
+		return;
+	}
+}
+
+static void set_default_params(struct coda_ctx *ctx)
+{
+	u32 src_fourcc, dst_fourcc;
+	int max_w;
+	int max_h;
+
+	if (ctx->inst_type == CODA_INST_ENCODER) {
+		src_fourcc = V4L2_PIX_FMT_YUV420;
+		dst_fourcc = V4L2_PIX_FMT_H264;
+	} else {
+		src_fourcc = V4L2_PIX_FMT_H264;
+		dst_fourcc = V4L2_PIX_FMT_YUV420;
+	}
+	ctx->codec = coda_find_codec(ctx->dev, src_fourcc, dst_fourcc);
+	max_w = ctx->codec->max_w;
+	max_h = ctx->codec->max_h;
+
+	ctx->params.codec_mode = ctx->codec->mode;
+	ctx->colorspace = V4L2_COLORSPACE_REC709;
+	ctx->params.framerate = 30;
+	ctx->aborting = 0;
+
+	/* Default formats for output and input queues */
+	ctx->q_data[V4L2_M2M_SRC].fourcc = ctx->codec->src_fourcc;
+	ctx->q_data[V4L2_M2M_DST].fourcc = ctx->codec->dst_fourcc;
+	ctx->q_data[V4L2_M2M_SRC].width = max_w;
+	ctx->q_data[V4L2_M2M_SRC].height = max_h;
+	ctx->q_data[V4L2_M2M_DST].width = max_w;
+	ctx->q_data[V4L2_M2M_DST].height = max_h;
+	if (ctx->codec->src_fourcc == V4L2_PIX_FMT_YUV420) {
+		ctx->q_data[V4L2_M2M_SRC].bytesperline = max_w;
+		ctx->q_data[V4L2_M2M_SRC].sizeimage = (max_w * max_h * 3) / 2;
+		ctx->q_data[V4L2_M2M_DST].bytesperline = 0;
+		ctx->q_data[V4L2_M2M_DST].sizeimage = CODA_MAX_FRAME_SIZE;
+	} else {
+		ctx->q_data[V4L2_M2M_SRC].bytesperline = 0;
+		ctx->q_data[V4L2_M2M_SRC].sizeimage = CODA_MAX_FRAME_SIZE;
+		ctx->q_data[V4L2_M2M_DST].bytesperline = max_w;
+		ctx->q_data[V4L2_M2M_DST].sizeimage = (max_w * max_h * 3) / 2;
+	}
+	ctx->q_data[V4L2_M2M_SRC].rect.width = max_w;
+	ctx->q_data[V4L2_M2M_SRC].rect.height = max_h;
+	ctx->q_data[V4L2_M2M_DST].rect.width = max_w;
+	ctx->q_data[V4L2_M2M_DST].rect.height = max_h;
+
+	if (ctx->dev->devtype->product == CODA_960)
+		coda_set_tiled_map_type(ctx, GDI_LINEAR_FRAME_MAP);
+}
+
+/*
+ * Queue operations
+ */
+static int coda_queue_setup(struct vb2_queue *vq,
+				const struct v4l2_format *fmt,
+				unsigned int *nbuffers, unsigned int *nplanes,
+				unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(vq);
+	struct coda_q_data *q_data;
+	unsigned int size;
+
+	q_data = get_q_data(ctx, vq->type);
+	size = q_data->sizeimage;
+
+	*nplanes = 1;
+	sizes[0] = size;
+
+	alloc_ctxs[0] = ctx->dev->alloc_ctx;
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		 "get %d buffer(s) of size %d each.\n", *nbuffers, size);
+
+	return 0;
+}
+
+static int coda_buf_prepare(struct vb2_buffer *vb)
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct coda_q_data *q_data;
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+
+	if (vb2_plane_size(vb, 0) < q_data->sizeimage) {
+		v4l2_warn(&ctx->dev->v4l2_dev,
+			  "%s data will not fit into plane (%lu < %lu)\n",
+			  __func__, vb2_plane_size(vb, 0),
+			  (long)q_data->sizeimage);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void coda_buf_queue(struct vb2_buffer *vb)
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(vb->vb2_queue);
+	struct coda_q_data *q_data;
+
+	q_data = get_q_data(ctx, vb->vb2_queue->type);
+
+	/*
+	 * In the decoder case, immediately try to copy the buffer into the
+	 * bitstream ringbuffer and mark it as ready to be dequeued.
+	 */
+	if (q_data->fourcc == V4L2_PIX_FMT_H264 &&
+	    vb->vb2_queue->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		/*
+		 * For backwards compatibility, queuing an empty buffer marks
+		 * the stream end
+		 */
+		if (vb2_get_plane_payload(vb, 0) == 0)
+			coda_bit_stream_end_flag(ctx);
+		mutex_lock(&ctx->bitstream_mutex);
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
+		if (vb2_is_streaming(vb->vb2_queue))
+			coda_fill_bitstream(ctx);
+		mutex_unlock(&ctx->bitstream_mutex);
+	} else {
+		v4l2_m2m_buf_queue(ctx->fh.m2m_ctx, vb);
+	}
+}
+
+int coda_alloc_aux_buf(struct coda_dev *dev, struct coda_aux_buf *buf,
+		       size_t size, const char *name, struct dentry *parent)
+{
+	buf->vaddr = dma_alloc_coherent(&dev->plat_dev->dev, size, &buf->paddr,
+					GFP_KERNEL);
+	if (!buf->vaddr) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to allocate %s buffer of size %u\n",
+			 name, size);
+		return -ENOMEM;
+	}
+
+	buf->size = size;
+
+	if (name && parent) {
+		buf->blob.data = buf->vaddr;
+		buf->blob.size = size;
+		buf->dentry = debugfs_create_blob(name, 0644, parent,
+						  &buf->blob);
+		if (!buf->dentry)
+			dev_warn(&dev->plat_dev->dev,
+				 "failed to create debugfs entry %s\n", name);
+	}
+
+	return 0;
+}
+
+void coda_free_aux_buf(struct coda_dev *dev,
+		       struct coda_aux_buf *buf)
+{
+	if (buf->vaddr) {
+		dma_free_coherent(&dev->plat_dev->dev, buf->size,
+				  buf->vaddr, buf->paddr);
+		buf->vaddr = NULL;
+		buf->size = 0;
+	}
+	debugfs_remove(buf->dentry);
+}
+
+static int coda_start_streaming(struct vb2_queue *q, unsigned int count)
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(q);
+	struct v4l2_device *v4l2_dev = &ctx->dev->v4l2_dev;
+	struct coda_q_data *q_data_src, *q_data_dst;
+	struct vb2_buffer *buf;
+	u32 dst_fourcc;
+	int ret = 0;
+
+	q_data_src = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		if (q_data_src->fourcc == V4L2_PIX_FMT_H264) {
+			/* copy the buffers that where queued before streamon */
+			mutex_lock(&ctx->bitstream_mutex);
+			coda_fill_bitstream(ctx);
+			mutex_unlock(&ctx->bitstream_mutex);
+
+			if (coda_get_bitstream_payload(ctx) < 512) {
+				ret = -EINVAL;
+				goto err;
+			}
+		} else {
+			if (count < 1) {
+				ret = -EINVAL;
+				goto err;
+			}
+		}
+
+		ctx->streamon_out = 1;
+	} else {
+		if (count < 1) {
+			ret = -EINVAL;
+			goto err;
+		}
+
+		ctx->streamon_cap = 1;
+	}
+
+	/* Don't start the coda unless both queues are on */
+	if (!(ctx->streamon_out & ctx->streamon_cap))
+		return 0;
+
+	/* Allow decoder device_run with no new buffers queued */
+	if (ctx->inst_type == CODA_INST_DECODER)
+		v4l2_m2m_set_src_buffered(ctx->fh.m2m_ctx, true);
+
+	ctx->gopcounter = ctx->params.gop_size - 1;
+	q_data_dst = get_q_data(ctx, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+	dst_fourcc = q_data_dst->fourcc;
+
+	ctx->codec = coda_find_codec(ctx->dev, q_data_src->fourcc,
+				     q_data_dst->fourcc);
+	if (!ctx->codec) {
+		v4l2_err(v4l2_dev, "couldn't tell instance type.\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	ret = ctx->ops->start_streaming(ctx);
+	if (ctx->inst_type == CODA_INST_DECODER) {
+		if (ret == -EAGAIN)
+			return 0;
+		else if (ret < 0)
+			goto err;
+	}
+
+	ctx->initialized = 1;
+	return ret;
+
+err:
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_DEQUEUED);
+	} else {
+		while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_DEQUEUED);
+	}
+	return ret;
+}
+
+static void coda_stop_streaming(struct vb2_queue *q)
+{
+	struct coda_ctx *ctx = vb2_get_drv_priv(q);
+	struct coda_dev *dev = ctx->dev;
+	struct vb2_buffer *buf;
+
+	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT) {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			 "%s: output\n", __func__);
+		ctx->streamon_out = 0;
+
+		coda_bit_stream_end_flag(ctx);
+
+		ctx->isequence = 0;
+
+		while ((buf = v4l2_m2m_src_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+	} else {
+		v4l2_dbg(1, coda_debug, &dev->v4l2_dev,
+			 "%s: capture\n", __func__);
+		ctx->streamon_cap = 0;
+
+		ctx->osequence = 0;
+		ctx->sequence_offset = 0;
+
+		while ((buf = v4l2_m2m_dst_buf_remove(ctx->fh.m2m_ctx)))
+			v4l2_m2m_buf_done(buf, VB2_BUF_STATE_ERROR);
+	}
+
+	if (!ctx->streamon_out && !ctx->streamon_cap) {
+		struct coda_timestamp *ts;
+
+		mutex_lock(&ctx->bitstream_mutex);
+		while (!list_empty(&ctx->timestamp_list)) {
+			ts = list_first_entry(&ctx->timestamp_list,
+					      struct coda_timestamp, list);
+			list_del(&ts->list);
+			kfree(ts);
+		}
+		mutex_unlock(&ctx->bitstream_mutex);
+		kfifo_init(&ctx->bitstream_fifo,
+			ctx->bitstream.vaddr, ctx->bitstream.size);
+		ctx->runcounter = 0;
+	}
+}
+
+static const struct vb2_ops coda_qops = {
+	.queue_setup		= coda_queue_setup,
+	.buf_prepare		= coda_buf_prepare,
+	.buf_queue		= coda_buf_queue,
+	.start_streaming	= coda_start_streaming,
+	.stop_streaming		= coda_stop_streaming,
+	.wait_prepare		= vb2_ops_wait_prepare,
+	.wait_finish		= vb2_ops_wait_finish,
+};
+
+static int coda_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct coda_ctx *ctx =
+			container_of(ctrl->handler, struct coda_ctx, ctrls);
+
+	v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+		 "s_ctrl: id = %d, val = %d\n", ctrl->id, ctrl->val);
+
+	switch (ctrl->id) {
+	case V4L2_CID_HFLIP:
+		if (ctrl->val)
+			ctx->params.rot_mode |= CODA_MIR_HOR;
+		else
+			ctx->params.rot_mode &= ~CODA_MIR_HOR;
+		break;
+	case V4L2_CID_VFLIP:
+		if (ctrl->val)
+			ctx->params.rot_mode |= CODA_MIR_VER;
+		else
+			ctx->params.rot_mode &= ~CODA_MIR_VER;
+		break;
+	case V4L2_CID_MPEG_VIDEO_BITRATE:
+		ctx->params.bitrate = ctrl->val / 1000;
+		break;
+	case V4L2_CID_MPEG_VIDEO_GOP_SIZE:
+		ctx->params.gop_size = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP:
+		ctx->params.h264_intra_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP:
+		ctx->params.h264_inter_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MIN_QP:
+		ctx->params.h264_min_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_MAX_QP:
+		ctx->params.h264_max_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA:
+		ctx->params.h264_deblk_alpha = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA:
+		ctx->params.h264_deblk_beta = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE:
+		ctx->params.h264_deblk_enabled = (ctrl->val ==
+				V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED);
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP:
+		ctx->params.mpeg4_intra_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP:
+		ctx->params.mpeg4_inter_qp = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE:
+		ctx->params.slice_mode = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB:
+		ctx->params.slice_max_mb = ctrl->val;
+		break;
+	case V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES:
+		ctx->params.slice_max_bits = ctrl->val * 8;
+		break;
+	case V4L2_CID_MPEG_VIDEO_HEADER_MODE:
+		break;
+	case V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB:
+		ctx->params.intra_refresh = ctrl->val;
+		break;
+	default:
+		v4l2_dbg(1, coda_debug, &ctx->dev->v4l2_dev,
+			"Invalid control, id=%d, val=%d\n",
+			ctrl->id, ctrl->val);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops coda_ctrl_ops = {
+	.s_ctrl = coda_s_ctrl,
+};
+
+static int coda_ctrls_setup(struct coda_ctx *ctx)
+{
+	v4l2_ctrl_handler_init(&ctx->ctrls, 9);
+
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_HFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_VFLIP, 0, 1, 1, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_BITRATE, 0, 32767000, 1, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_GOP_SIZE, 1, 60, 1, 16);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_I_FRAME_QP, 0, 51, 1, 25);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_P_FRAME_QP, 0, 51, 1, 25);
+	if (ctx->dev->devtype->product != CODA_960) {
+		v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+			V4L2_CID_MPEG_VIDEO_H264_MIN_QP, 0, 51, 1, 12);
+	}
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_MAX_QP, 0, 51, 1, 51);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, 0, 15, 1, 0);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, 0, 15, 1, 0);
+	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE,
+		V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_DISABLED, 0x0,
+		V4L2_MPEG_VIDEO_H264_LOOP_FILTER_MODE_ENABLED);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MPEG4_I_FRAME_QP, 1, 31, 1, 2);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MPEG4_P_FRAME_QP, 1, 31, 1, 2);
+	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE,
+		V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES, 0x0,
+		V4L2_MPEG_VIDEO_MULTI_SLICE_MODE_SINGLE);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_MB, 1, 0x3fffffff, 1, 1);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MAX_BYTES, 1, 0x3fffffff, 1,
+		500);
+	v4l2_ctrl_new_std_menu(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_HEADER_MODE,
+		V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME,
+		(1 << V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE),
+		V4L2_MPEG_VIDEO_HEADER_MODE_JOINED_WITH_1ST_FRAME);
+	v4l2_ctrl_new_std(&ctx->ctrls, &coda_ctrl_ops,
+		V4L2_CID_MPEG_VIDEO_CYCLIC_INTRA_REFRESH_MB, 0,
+		1920 * 1088 / 256, 1, 0);
+
+	if (ctx->ctrls.error) {
+		v4l2_err(&ctx->dev->v4l2_dev,
+			"control initialization error (%d)",
+			ctx->ctrls.error);
+		return -EINVAL;
+	}
+
+	return v4l2_ctrl_handler_setup(&ctx->ctrls);
+}
+
+static int coda_queue_init(struct coda_ctx *ctx, struct vb2_queue *vq)
+{
+	vq->drv_priv = ctx;
+	vq->ops = &coda_qops;
+	vq->buf_struct_size = sizeof(struct v4l2_m2m_buffer);
+	vq->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_COPY;
+	vq->lock = &ctx->dev->dev_mutex;
+
+	return vb2_queue_init(vq);
+}
+
+int coda_encoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq)
+{
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+
+	ret = coda_queue_init(priv, src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+
+	return coda_queue_init(priv, dst_vq);
+}
+
+int coda_decoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq)
+{
+	int ret;
+
+	src_vq->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	src_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	src_vq->mem_ops = &vb2_dma_contig_memops;
+
+	ret = coda_queue_init(priv, src_vq);
+	if (ret)
+		return ret;
+
+	dst_vq->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	dst_vq->io_modes = VB2_DMABUF | VB2_MMAP;
+	dst_vq->mem_ops = &vb2_dma_contig_memops;
+
+	return coda_queue_init(priv, dst_vq);
+}
+
+static int coda_next_free_instance(struct coda_dev *dev)
+{
+	int idx = ffz(dev->instance_mask);
+
+	if ((idx < 0) ||
+	    (dev->devtype->product == CODA_DX6 && idx > CODADX6_MAX_INSTANCES))
+		return -EBUSY;
+
+	return idx;
+}
+
+static int coda_open(struct file *file, enum coda_inst_type inst_type,
+		     const struct coda_context_ops *ctx_ops)
+{
+	struct coda_dev *dev = video_drvdata(file);
+	struct coda_ctx *ctx = NULL;
+	char *name;
+	int ret;
+	int idx;
+
+	ctx = kzalloc(sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	idx = coda_next_free_instance(dev);
+	if (idx < 0) {
+		ret = idx;
+		goto err_coda_max;
+	}
+	set_bit(idx, &dev->instance_mask);
+
+	name = kasprintf(GFP_KERNEL, "context%d", idx);
+	ctx->debugfs_entry = debugfs_create_dir(name, dev->debugfs_root);
+	kfree(name);
+
+	ctx->inst_type = inst_type;
+	ctx->ops = ctx_ops;
+	init_completion(&ctx->completion);
+	INIT_WORK(&ctx->pic_run_work, coda_pic_run_work);
+	INIT_WORK(&ctx->seq_end_work, ctx->ops->seq_end_work);
+	v4l2_fh_init(&ctx->fh, video_devdata(file));
+	file->private_data = &ctx->fh;
+	v4l2_fh_add(&ctx->fh);
+	ctx->dev = dev;
+	ctx->idx = idx;
+	switch (dev->devtype->product) {
+	case CODA_7541:
+	case CODA_960:
+		ctx->reg_idx = 0;
+		break;
+	default:
+		ctx->reg_idx = idx;
+	}
+
+	/* Power up and upload firmware if necessary */
+	ret = pm_runtime_get_sync(&dev->plat_dev->dev);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "failed to power up: %d\n", ret);
+		goto err_pm_get;
+	}
+
+	ret = clk_prepare_enable(dev->clk_per);
+	if (ret)
+		goto err_clk_per;
+
+	ret = clk_prepare_enable(dev->clk_ahb);
+	if (ret)
+		goto err_clk_ahb;
+
+	set_default_params(ctx);
+	ctx->fh.m2m_ctx = v4l2_m2m_ctx_init(dev->m2m_dev, ctx,
+					    ctx->ops->queue_init);
+	if (IS_ERR(ctx->fh.m2m_ctx)) {
+		ret = PTR_ERR(ctx->fh.m2m_ctx);
+
+		v4l2_err(&dev->v4l2_dev, "%s return error (%d)\n",
+			 __func__, ret);
+		goto err_ctx_init;
+	}
+
+	ret = coda_ctrls_setup(ctx);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev, "failed to setup coda controls\n");
+		goto err_ctrls_setup;
+	}
+
+	ctx->fh.ctrl_handler = &ctx->ctrls;
+
+	ret = coda_alloc_context_buf(ctx, &ctx->parabuf, CODA_PARA_BUF_SIZE,
+				     "parabuf");
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "failed to allocate parabuf");
+		goto err_dma_alloc;
+	}
+
+	ctx->bitstream.size = CODA_MAX_FRAME_SIZE;
+	ctx->bitstream.vaddr = dma_alloc_writecombine(&dev->plat_dev->dev,
+			ctx->bitstream.size, &ctx->bitstream.paddr, GFP_KERNEL);
+	if (!ctx->bitstream.vaddr) {
+		v4l2_err(&dev->v4l2_dev,
+			 "failed to allocate bitstream ringbuffer");
+		ret = -ENOMEM;
+		goto err_dma_writecombine;
+	}
+	kfifo_init(&ctx->bitstream_fifo,
+		ctx->bitstream.vaddr, ctx->bitstream.size);
+	mutex_init(&ctx->bitstream_mutex);
+	mutex_init(&ctx->buffer_mutex);
+	INIT_LIST_HEAD(&ctx->timestamp_list);
+
+	coda_lock(ctx);
+	list_add(&ctx->list, &dev->instances);
+	coda_unlock(ctx);
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "Created instance %d (%p)\n",
+		 ctx->idx, ctx);
+
+	return 0;
+
+err_dma_writecombine:
+	if (ctx->dev->devtype->product == CODA_DX6)
+		coda_free_aux_buf(dev, &ctx->workbuf);
+	coda_free_aux_buf(dev, &ctx->parabuf);
+err_dma_alloc:
+	v4l2_ctrl_handler_free(&ctx->ctrls);
+err_ctrls_setup:
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+err_ctx_init:
+	clk_disable_unprepare(dev->clk_ahb);
+err_clk_ahb:
+	clk_disable_unprepare(dev->clk_per);
+err_clk_per:
+	pm_runtime_put_sync(&dev->plat_dev->dev);
+err_pm_get:
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	clear_bit(ctx->idx, &dev->instance_mask);
+err_coda_max:
+	kfree(ctx);
+	return ret;
+}
+
+static int coda_encoder_open(struct file *file)
+{
+	return coda_open(file, CODA_INST_ENCODER, &coda_bit_encode_ops);
+}
+
+static int coda_decoder_open(struct file *file)
+{
+	return coda_open(file, CODA_INST_DECODER, &coda_bit_decode_ops);
+}
+
+static int coda_release(struct file *file)
+{
+	struct coda_dev *dev = video_drvdata(file);
+	struct coda_ctx *ctx = fh_to_ctx(file->private_data);
+
+	v4l2_dbg(1, coda_debug, &dev->v4l2_dev, "Releasing instance %p\n",
+		 ctx);
+
+	debugfs_remove_recursive(ctx->debugfs_entry);
+
+	/* If this instance is running, call .job_abort and wait for it to end */
+	v4l2_m2m_ctx_release(ctx->fh.m2m_ctx);
+
+	/* In case the instance was not running, we still need to call SEQ_END */
+	if (ctx->initialized) {
+		queue_work(dev->workqueue, &ctx->seq_end_work);
+		flush_work(&ctx->seq_end_work);
+	}
+
+	coda_lock(ctx);
+	list_del(&ctx->list);
+	coda_unlock(ctx);
+
+	dma_free_writecombine(&dev->plat_dev->dev, ctx->bitstream.size,
+		ctx->bitstream.vaddr, ctx->bitstream.paddr);
+	if (ctx->dev->devtype->product == CODA_DX6)
+		coda_free_aux_buf(dev, &ctx->workbuf);
+
+	coda_free_aux_buf(dev, &ctx->parabuf);
+	v4l2_ctrl_handler_free(&ctx->ctrls);
+	clk_disable_unprepare(dev->clk_ahb);
+	clk_disable_unprepare(dev->clk_per);
+	pm_runtime_put_sync(&dev->plat_dev->dev);
+	v4l2_fh_del(&ctx->fh);
+	v4l2_fh_exit(&ctx->fh);
+	clear_bit(ctx->idx, &dev->instance_mask);
+	if (ctx->ops->release)
+		ctx->ops->release(ctx);
+	kfree(ctx);
+
+	return 0;
+}
+
+static const struct v4l2_file_operations coda_encoder_fops = {
+	.owner		= THIS_MODULE,
+	.open		= coda_encoder_open,
+	.release	= coda_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static const struct v4l2_file_operations coda_decoder_fops = {
+	.owner		= THIS_MODULE,
+	.open		= coda_decoder_open,
+	.release	= coda_release,
+	.poll		= v4l2_m2m_fop_poll,
+	.unlocked_ioctl	= video_ioctl2,
+	.mmap		= v4l2_m2m_fop_mmap,
+};
+
+static int coda_hw_init(struct coda_dev *dev)
+{
+	u32 data;
+	u16 *p;
+	int i, ret;
+
+	ret = clk_prepare_enable(dev->clk_per);
+	if (ret)
+		goto err_clk_per;
+
+	ret = clk_prepare_enable(dev->clk_ahb);
+	if (ret)
+		goto err_clk_ahb;
+
+	if (dev->rstc)
+		reset_control_reset(dev->rstc);
+
+	/*
+	 * Copy the first CODA_ISRAM_SIZE in the internal SRAM.
+	 * The 16-bit chars in the code buffer are in memory access
+	 * order, re-sort them to CODA order for register download.
+	 * Data in this SRAM survives a reboot.
+	 */
+	p = (u16 *)dev->codebuf.vaddr;
+	if (dev->devtype->product == CODA_DX6) {
+		for (i = 0; i < (CODA_ISRAM_SIZE / 2); i++)  {
+			data = CODA_DOWN_ADDRESS_SET(i) |
+				CODA_DOWN_DATA_SET(p[i ^ 1]);
+			coda_write(dev, data, CODA_REG_BIT_CODE_DOWN);
+		}
+	} else {
+		for (i = 0; i < (CODA_ISRAM_SIZE / 2); i++) {
+			data = CODA_DOWN_ADDRESS_SET(i) |
+				CODA_DOWN_DATA_SET(p[round_down(i, 4) +
+							3 - (i % 4)]);
+			coda_write(dev, data, CODA_REG_BIT_CODE_DOWN);
+		}
+	}
+
+	/* Clear registers */
+	for (i = 0; i < 64; i++)
+		coda_write(dev, 0, CODA_REG_BIT_CODE_BUF_ADDR + i * 4);
+
+	/* Tell the BIT where to find everything it needs */
+	if (dev->devtype->product == CODA_960 ||
+	    dev->devtype->product == CODA_7541) {
+		coda_write(dev, dev->tempbuf.paddr,
+				CODA_REG_BIT_TEMP_BUF_ADDR);
+		coda_write(dev, 0, CODA_REG_BIT_BIT_STREAM_PARAM);
+	} else {
+		coda_write(dev, dev->workbuf.paddr,
+			      CODA_REG_BIT_WORK_BUF_ADDR);
+	}
+	coda_write(dev, dev->codebuf.paddr,
+		      CODA_REG_BIT_CODE_BUF_ADDR);
+	coda_write(dev, 0, CODA_REG_BIT_CODE_RUN);
+
+	/* Set default values */
+	switch (dev->devtype->product) {
+	case CODA_DX6:
+		coda_write(dev, CODADX6_STREAM_BUF_PIC_FLUSH,
+			   CODA_REG_BIT_STREAM_CTRL);
+		break;
+	default:
+		coda_write(dev, CODA7_STREAM_BUF_PIC_FLUSH,
+			   CODA_REG_BIT_STREAM_CTRL);
+	}
+	if (dev->devtype->product == CODA_960)
+		coda_write(dev, 1 << 12, CODA_REG_BIT_FRAME_MEM_CTRL);
+	else
+		coda_write(dev, 0, CODA_REG_BIT_FRAME_MEM_CTRL);
+
+	if (dev->devtype->product != CODA_DX6)
+		coda_write(dev, 0, CODA7_REG_BIT_AXI_SRAM_USE);
+
+	coda_write(dev, CODA_INT_INTERRUPT_ENABLE,
+		      CODA_REG_BIT_INT_ENABLE);
+
+	/* Reset VPU and start processor */
+	data = coda_read(dev, CODA_REG_BIT_CODE_RESET);
+	data |= CODA_REG_RESET_ENABLE;
+	coda_write(dev, data, CODA_REG_BIT_CODE_RESET);
+	udelay(10);
+	data &= ~CODA_REG_RESET_ENABLE;
+	coda_write(dev, data, CODA_REG_BIT_CODE_RESET);
+	coda_write(dev, CODA_REG_RUN_ENABLE, CODA_REG_BIT_CODE_RUN);
+
+	clk_disable_unprepare(dev->clk_ahb);
+	clk_disable_unprepare(dev->clk_per);
+
+	return 0;
+
+err_clk_ahb:
+	clk_disable_unprepare(dev->clk_per);
+err_clk_per:
+	return ret;
+}
+
+static int coda_register_device(struct coda_dev *dev, struct video_device *vfd)
+{
+	vfd->release	= video_device_release_empty,
+	vfd->lock	= &dev->dev_mutex;
+	vfd->v4l2_dev	= &dev->v4l2_dev;
+	vfd->vfl_dir	= VFL_DIR_M2M;
+	video_set_drvdata(vfd, dev);
+
+	/* Not applicable, use the selection API instead */
+	v4l2_disable_ioctl(vfd, VIDIOC_CROPCAP);
+	v4l2_disable_ioctl(vfd, VIDIOC_G_CROP);
+	v4l2_disable_ioctl(vfd, VIDIOC_S_CROP);
+
+	return video_register_device(vfd, VFL_TYPE_GRABBER, 0);
+}
+
+static void coda_fw_callback(const struct firmware *fw, void *context)
+{
+	struct coda_dev *dev = context;
+	struct platform_device *pdev = dev->plat_dev;
+	int ret;
+
+	if (!fw) {
+		v4l2_err(&dev->v4l2_dev, "firmware request failed\n");
+		goto put_pm;
+	}
+
+	/* allocate auxiliary per-device code buffer for the BIT processor */
+	ret = coda_alloc_aux_buf(dev, &dev->codebuf, fw->size, "codebuf",
+				 dev->debugfs_root);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to allocate code buffer\n");
+		goto put_pm;
+	}
+
+	/* Copy the whole firmware image to the code buffer */
+	memcpy(dev->codebuf.vaddr, fw->data, fw->size);
+	release_firmware(fw);
+
+	ret = coda_hw_init(dev);
+	if (ret < 0) {
+		v4l2_err(&dev->v4l2_dev, "HW initialization failed\n");
+		goto put_pm;
+	}
+
+	ret = coda_check_firmware(dev);
+	if (ret < 0)
+		goto put_pm;
+
+	dev->alloc_ctx = vb2_dma_contig_init_ctx(&pdev->dev);
+	if (IS_ERR(dev->alloc_ctx)) {
+		v4l2_err(&dev->v4l2_dev, "Failed to alloc vb2 context\n");
+		goto put_pm;
+	}
+
+	dev->m2m_dev = v4l2_m2m_init(&coda_m2m_ops);
+	if (IS_ERR(dev->m2m_dev)) {
+		v4l2_err(&dev->v4l2_dev, "Failed to init mem2mem device\n");
+		goto rel_ctx;
+	}
+
+	dev->vfd[0].fops      = &coda_encoder_fops,
+	dev->vfd[0].ioctl_ops = &coda_ioctl_ops;
+	snprintf(dev->vfd[0].name, sizeof(dev->vfd[0].name), "coda-encoder");
+	ret = coda_register_device(dev, &dev->vfd[0]);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to register encoder video device\n");
+		goto rel_m2m;
+	}
+
+	dev->vfd[1].fops      = &coda_decoder_fops,
+	dev->vfd[1].ioctl_ops = &coda_ioctl_ops;
+	snprintf(dev->vfd[1].name, sizeof(dev->vfd[1].name), "coda-decoder");
+	ret = coda_register_device(dev, &dev->vfd[1]);
+	if (ret) {
+		v4l2_err(&dev->v4l2_dev,
+			 "Failed to register decoder video device\n");
+		goto rel_m2m;
+	}
+
+	v4l2_info(&dev->v4l2_dev, "codec registered as /dev/video[%d-%d]\n",
+		  dev->vfd[0].num, dev->vfd[1].num);
+
+	pm_runtime_put_sync(&pdev->dev);
+	return;
+
+rel_m2m:
+	v4l2_m2m_release(dev->m2m_dev);
+rel_ctx:
+	vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+put_pm:
+	pm_runtime_put_sync(&pdev->dev);
+}
+
+static int coda_firmware_request(struct coda_dev *dev)
+{
+	char *fw = dev->devtype->firmware;
+
+	dev_dbg(&dev->plat_dev->dev, "requesting firmware '%s' for %s\n", fw,
+		coda_product_name(dev->devtype->product));
+
+	return request_firmware_nowait(THIS_MODULE, true,
+		fw, &dev->plat_dev->dev, GFP_KERNEL, dev, coda_fw_callback);
+}
+
+enum coda_platform {
+	CODA_IMX27,
+	CODA_IMX53,
+	CODA_IMX6Q,
+	CODA_IMX6DL,
+};
+
+static const struct coda_devtype coda_devdata[] = {
+	[CODA_IMX27] = {
+		.firmware     = "v4l-codadx6-imx27.bin",
+		.product      = CODA_DX6,
+		.codecs       = codadx6_codecs,
+		.num_codecs   = ARRAY_SIZE(codadx6_codecs),
+		.workbuf_size = 288 * 1024 + FMO_SLICE_SAVE_BUF_SIZE * 8 * 1024,
+		.iram_size    = 0xb000,
+	},
+	[CODA_IMX53] = {
+		.firmware     = "v4l-coda7541-imx53.bin",
+		.product      = CODA_7541,
+		.codecs       = coda7_codecs,
+		.num_codecs   = ARRAY_SIZE(coda7_codecs),
+		.workbuf_size = 128 * 1024,
+		.tempbuf_size = 304 * 1024,
+		.iram_size    = 0x14000,
+	},
+	[CODA_IMX6Q] = {
+		.firmware     = "v4l-coda960-imx6q.bin",
+		.product      = CODA_960,
+		.codecs       = coda9_codecs,
+		.num_codecs   = ARRAY_SIZE(coda9_codecs),
+		.workbuf_size = 80 * 1024,
+		.tempbuf_size = 204 * 1024,
+		.iram_size    = 0x21000,
+	},
+	[CODA_IMX6DL] = {
+		.firmware     = "v4l-coda960-imx6dl.bin",
+		.product      = CODA_960,
+		.codecs       = coda9_codecs,
+		.num_codecs   = ARRAY_SIZE(coda9_codecs),
+		.workbuf_size = 80 * 1024,
+		.tempbuf_size = 204 * 1024,
+		.iram_size    = 0x20000,
+	},
+};
+
+static struct platform_device_id coda_platform_ids[] = {
+	{ .name = "coda-imx27", .driver_data = CODA_IMX27 },
+	{ .name = "coda-imx53", .driver_data = CODA_IMX53 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(platform, coda_platform_ids);
+
+#ifdef CONFIG_OF
+static const struct of_device_id coda_dt_ids[] = {
+	{ .compatible = "fsl,imx27-vpu", .data = &coda_devdata[CODA_IMX27] },
+	{ .compatible = "fsl,imx53-vpu", .data = &coda_devdata[CODA_IMX53] },
+	{ .compatible = "fsl,imx6q-vpu", .data = &coda_devdata[CODA_IMX6Q] },
+	{ .compatible = "fsl,imx6dl-vpu", .data = &coda_devdata[CODA_IMX6DL] },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, coda_dt_ids);
+#endif
+
+static int coda_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *of_id =
+			of_match_device(of_match_ptr(coda_dt_ids), &pdev->dev);
+	const struct platform_device_id *pdev_id;
+	struct coda_platform_data *pdata = pdev->dev.platform_data;
+	struct device_node *np = pdev->dev.of_node;
+	struct gen_pool *pool;
+	struct coda_dev *dev;
+	struct resource *res;
+	int ret, irq;
+
+	dev = devm_kzalloc(&pdev->dev, sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
+		dev_err(&pdev->dev, "Not enough memory for %s\n",
+			CODA_NAME);
+		return -ENOMEM;
+	}
+
+	spin_lock_init(&dev->irqlock);
+	INIT_LIST_HEAD(&dev->instances);
+
+	dev->plat_dev = pdev;
+	dev->clk_per = devm_clk_get(&pdev->dev, "per");
+	if (IS_ERR(dev->clk_per)) {
+		dev_err(&pdev->dev, "Could not get per clock\n");
+		return PTR_ERR(dev->clk_per);
+	}
+
+	dev->clk_ahb = devm_clk_get(&pdev->dev, "ahb");
+	if (IS_ERR(dev->clk_ahb)) {
+		dev_err(&pdev->dev, "Could not get ahb clock\n");
+		return PTR_ERR(dev->clk_ahb);
+	}
+
+	/* Get  memory for physical registers */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dev->regs_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dev->regs_base))
+		return PTR_ERR(dev->regs_base);
+
+	/* IRQ */
+	irq = platform_get_irq_byname(pdev, "bit");
+	if (irq < 0)
+		irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "failed to get irq resource\n");
+		return irq;
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, irq, NULL, coda_irq_handler,
+			IRQF_ONESHOT, dev_name(&pdev->dev), dev);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to request irq: %d\n", ret);
+		return ret;
+	}
+
+	dev->rstc = devm_reset_control_get_optional(&pdev->dev, NULL);
+	if (IS_ERR(dev->rstc)) {
+		ret = PTR_ERR(dev->rstc);
+		if (ret == -ENOENT || ret == -ENOSYS) {
+			dev->rstc = NULL;
+		} else {
+			dev_err(&pdev->dev, "failed get reset control: %d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	/* Get IRAM pool from device tree or platform data */
+	pool = of_get_named_gen_pool(np, "iram", 0);
+	if (!pool && pdata)
+		pool = dev_get_gen_pool(pdata->iram_dev);
+	if (!pool) {
+		dev_err(&pdev->dev, "iram pool not available\n");
+		return -ENOMEM;
+	}
+	dev->iram_pool = pool;
+
+	ret = v4l2_device_register(&pdev->dev, &dev->v4l2_dev);
+	if (ret)
+		return ret;
+
+	mutex_init(&dev->dev_mutex);
+	mutex_init(&dev->coda_mutex);
+
+	pdev_id = of_id ? of_id->data : platform_get_device_id(pdev);
+
+	if (of_id) {
+		dev->devtype = of_id->data;
+	} else if (pdev_id) {
+		dev->devtype = &coda_devdata[pdev_id->driver_data];
+	} else {
+		v4l2_device_unregister(&dev->v4l2_dev);
+		return -EINVAL;
+	}
+
+	dev->debugfs_root = debugfs_create_dir("coda", NULL);
+	if (!dev->debugfs_root)
+		dev_warn(&pdev->dev, "failed to create debugfs root\n");
+
+	/* allocate auxiliary per-device buffers for the BIT processor */
+	if (dev->devtype->product == CODA_DX6) {
+		ret = coda_alloc_aux_buf(dev, &dev->workbuf,
+					 dev->devtype->workbuf_size, "workbuf",
+					 dev->debugfs_root);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to allocate work buffer\n");
+			v4l2_device_unregister(&dev->v4l2_dev);
+			return ret;
+		}
+	}
+
+	if (dev->devtype->tempbuf_size) {
+		ret = coda_alloc_aux_buf(dev, &dev->tempbuf,
+					 dev->devtype->tempbuf_size, "tempbuf",
+					 dev->debugfs_root);
+		if (ret < 0) {
+			dev_err(&pdev->dev, "failed to allocate temp buffer\n");
+			v4l2_device_unregister(&dev->v4l2_dev);
+			return ret;
+		}
+	}
+
+	dev->iram.size = dev->devtype->iram_size;
+	dev->iram.vaddr = gen_pool_dma_alloc(dev->iram_pool, dev->iram.size,
+					     &dev->iram.paddr);
+	if (!dev->iram.vaddr) {
+		dev_warn(&pdev->dev, "unable to alloc iram\n");
+	} else {
+		dev->iram.blob.data = dev->iram.vaddr;
+		dev->iram.blob.size = dev->iram.size;
+		dev->iram.dentry = debugfs_create_blob("iram", 0644,
+						       dev->debugfs_root,
+						       &dev->iram.blob);
+	}
+
+	dev->workqueue = alloc_workqueue("coda", WQ_UNBOUND | WQ_MEM_RECLAIM, 1);
+	if (!dev->workqueue) {
+		dev_err(&pdev->dev, "unable to alloc workqueue\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, dev);
+
+	/*
+	 * Start activated so we can directly call coda_hw_init in
+	 * coda_fw_callback regardless of whether CONFIG_PM_RUNTIME is
+	 * enabled or whether the device is associated with a PM domain.
+	 */
+	pm_runtime_get_noresume(&pdev->dev);
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	return coda_firmware_request(dev);
+}
+
+static int coda_remove(struct platform_device *pdev)
+{
+	struct coda_dev *dev = platform_get_drvdata(pdev);
+
+	video_unregister_device(&dev->vfd[0]);
+	video_unregister_device(&dev->vfd[1]);
+	if (dev->m2m_dev)
+		v4l2_m2m_release(dev->m2m_dev);
+	pm_runtime_disable(&pdev->dev);
+	if (dev->alloc_ctx)
+		vb2_dma_contig_cleanup_ctx(dev->alloc_ctx);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	destroy_workqueue(dev->workqueue);
+	if (dev->iram.vaddr)
+		gen_pool_free(dev->iram_pool, (unsigned long)dev->iram.vaddr,
+			      dev->iram.size);
+	coda_free_aux_buf(dev, &dev->codebuf);
+	coda_free_aux_buf(dev, &dev->tempbuf);
+	coda_free_aux_buf(dev, &dev->workbuf);
+	debugfs_remove_recursive(dev->debugfs_root);
+	return 0;
+}
+
+#ifdef CONFIG_PM_RUNTIME
+static int coda_runtime_resume(struct device *dev)
+{
+	struct coda_dev *cdev = dev_get_drvdata(dev);
+	int ret = 0;
+
+	if (dev->pm_domain && cdev->codebuf.vaddr) {
+		ret = coda_hw_init(cdev);
+		if (ret)
+			v4l2_err(&cdev->v4l2_dev, "HW initialization failed\n");
+	}
+
+	return ret;
+}
+#endif
+
+static const struct dev_pm_ops coda_pm_ops = {
+	SET_RUNTIME_PM_OPS(NULL, coda_runtime_resume, NULL)
+};
+
+static struct platform_driver coda_driver = {
+	.probe	= coda_probe,
+	.remove	= coda_remove,
+	.driver	= {
+		.name	= CODA_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(coda_dt_ids),
+		.pm	= &coda_pm_ops,
+	},
+	.id_table = coda_platform_ids,
+};
+
+module_platform_driver(coda_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Javier Martin <javier.martin@vista-silicon.com>");
+MODULE_DESCRIPTION("Coda multi-standard codec V4L2 driver");
diff --git a/drivers/media/platform/coda/coda-h264.c b/drivers/media/platform/coda/coda-h264.c
new file mode 100644
index 000000000000..456773af1f1d
--- /dev/null
+++ b/drivers/media/platform/coda/coda-h264.c
@@ -0,0 +1,37 @@
+/*
+ * Coda multi-standard codec IP - H.264 helper functions
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+
+static const u8 coda_filler_nal[14] = { 0x00, 0x00, 0x00, 0x01, 0x0c, 0xff,
+			0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x80 };
+static const u8 coda_filler_size[8] = { 0, 7, 14, 13, 12, 11, 10, 9 };
+
+int coda_h264_padding(int size, char *p)
+{
+	int nal_size;
+	int diff;
+
+	diff = size - (size & ~0x7);
+	if (diff == 0)
+		return 0;
+
+	nal_size = coda_filler_size[diff];
+	memcpy(p, coda_filler_nal, nal_size);
+
+	/* Add rbsp stop bit and trailing at the end */
+	*(p + nal_size - 1) = 0x80;
+
+	return nal_size;
+}
diff --git a/drivers/media/platform/coda/coda.h b/drivers/media/platform/coda/coda.h
new file mode 100644
index 000000000000..bbc18c0dacd9
--- /dev/null
+++ b/drivers/media/platform/coda/coda.h
@@ -0,0 +1,287 @@
+/*
+ * Coda multi-standard codec IP
+ *
+ * Copyright (C) 2012 Vista Silicon S.L.
+ *    Javier Martin, <javier.martin@vista-silicon.com>
+ *    Xavier Duret
+ * Copyright (C) 2012-2014 Philipp Zabel, Pengutronix
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/irqreturn.h>
+#include <linux/mutex.h>
+#include <linux/kfifo.h>
+#include <linux/videodev2.h>
+
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-fh.h>
+#include <media/videobuf2-core.h>
+
+#include "coda_regs.h"
+
+#define CODA_MAX_FRAMEBUFFERS	8
+#define CODA_MAX_FRAME_SIZE	0x100000
+#define FMO_SLICE_SAVE_BUF_SIZE	(32)
+
+enum {
+	V4L2_M2M_SRC = 0,
+	V4L2_M2M_DST = 1,
+};
+
+enum coda_inst_type {
+	CODA_INST_ENCODER,
+	CODA_INST_DECODER,
+};
+
+enum coda_product {
+	CODA_DX6 = 0xf001,
+	CODA_7541 = 0xf012,
+	CODA_960 = 0xf020,
+};
+
+struct coda_devtype {
+	char			*firmware;
+	enum coda_product	product;
+	const struct coda_codec	*codecs;
+	unsigned int		num_codecs;
+	size_t			workbuf_size;
+	size_t			tempbuf_size;
+	size_t			iram_size;
+};
+
+struct coda_aux_buf {
+	void			*vaddr;
+	dma_addr_t		paddr;
+	u32			size;
+	struct debugfs_blob_wrapper blob;
+	struct dentry		*dentry;
+};
+
+struct coda_dev {
+	struct v4l2_device	v4l2_dev;
+	struct video_device	vfd[2];
+	struct platform_device	*plat_dev;
+	const struct coda_devtype *devtype;
+
+	void __iomem		*regs_base;
+	struct clk		*clk_per;
+	struct clk		*clk_ahb;
+	struct reset_control	*rstc;
+
+	struct coda_aux_buf	codebuf;
+	struct coda_aux_buf	tempbuf;
+	struct coda_aux_buf	workbuf;
+	struct gen_pool		*iram_pool;
+	struct coda_aux_buf	iram;
+
+	spinlock_t		irqlock;
+	struct mutex		dev_mutex;
+	struct mutex		coda_mutex;
+	struct workqueue_struct	*workqueue;
+	struct v4l2_m2m_dev	*m2m_dev;
+	struct vb2_alloc_ctx	*alloc_ctx;
+	struct list_head	instances;
+	unsigned long		instance_mask;
+	struct dentry		*debugfs_root;
+};
+
+struct coda_codec {
+	u32 mode;
+	u32 src_fourcc;
+	u32 dst_fourcc;
+	u32 max_w;
+	u32 max_h;
+};
+
+struct coda_huff_tab;
+
+struct coda_params {
+	u8			rot_mode;
+	u8			h264_intra_qp;
+	u8			h264_inter_qp;
+	u8			h264_min_qp;
+	u8			h264_max_qp;
+	u8			h264_deblk_enabled;
+	u8			h264_deblk_alpha;
+	u8			h264_deblk_beta;
+	u8			mpeg4_intra_qp;
+	u8			mpeg4_inter_qp;
+	u8			gop_size;
+	int			intra_refresh;
+	int			codec_mode;
+	int			codec_mode_aux;
+	enum v4l2_mpeg_video_multi_slice_mode slice_mode;
+	u32			framerate;
+	u16			bitrate;
+	u32			slice_max_bits;
+	u32			slice_max_mb;
+};
+
+struct coda_timestamp {
+	struct list_head	list;
+	u32			sequence;
+	struct v4l2_timecode	timecode;
+	struct timeval		timestamp;
+};
+
+/* Per-queue, driver-specific private data */
+struct coda_q_data {
+	unsigned int		width;
+	unsigned int		height;
+	unsigned int		bytesperline;
+	unsigned int		sizeimage;
+	unsigned int		fourcc;
+	struct v4l2_rect	rect;
+};
+
+struct coda_iram_info {
+	u32		axi_sram_use;
+	phys_addr_t	buf_bit_use;
+	phys_addr_t	buf_ip_ac_dc_use;
+	phys_addr_t	buf_dbk_y_use;
+	phys_addr_t	buf_dbk_c_use;
+	phys_addr_t	buf_ovl_use;
+	phys_addr_t	buf_btp_use;
+	phys_addr_t	search_ram_paddr;
+	int		search_ram_size;
+	int		remaining;
+	phys_addr_t	next_paddr;
+};
+
+struct gdi_tiled_map {
+	int xy2ca_map[16];
+	int xy2ba_map[16];
+	int xy2ra_map[16];
+	int rbc2axi_map[32];
+	int xy2rbc_config;
+	int map_type;
+#define GDI_LINEAR_FRAME_MAP 0
+};
+
+struct coda_ctx;
+
+struct coda_context_ops {
+	int (*queue_init)(void *priv, struct vb2_queue *src_vq,
+			  struct vb2_queue *dst_vq);
+	int (*start_streaming)(struct coda_ctx *ctx);
+	int (*prepare_run)(struct coda_ctx *ctx);
+	void (*finish_run)(struct coda_ctx *ctx);
+	void (*seq_end_work)(struct work_struct *work);
+	void (*release)(struct coda_ctx *ctx);
+};
+
+struct coda_ctx {
+	struct coda_dev			*dev;
+	struct mutex			buffer_mutex;
+	struct list_head		list;
+	struct work_struct		pic_run_work;
+	struct work_struct		seq_end_work;
+	struct completion		completion;
+	const struct coda_context_ops	*ops;
+	int				aborting;
+	int				initialized;
+	int				streamon_out;
+	int				streamon_cap;
+	u32				isequence;
+	u32				qsequence;
+	u32				osequence;
+	u32				sequence_offset;
+	struct coda_q_data		q_data[2];
+	enum coda_inst_type		inst_type;
+	const struct coda_codec		*codec;
+	enum v4l2_colorspace		colorspace;
+	struct coda_params		params;
+	struct v4l2_ctrl_handler	ctrls;
+	struct v4l2_fh			fh;
+	int				gopcounter;
+	int				runcounter;
+	char				vpu_header[3][64];
+	int				vpu_header_size[3];
+	struct kfifo			bitstream_fifo;
+	struct mutex			bitstream_mutex;
+	struct coda_aux_buf		bitstream;
+	bool				hold;
+	struct coda_aux_buf		parabuf;
+	struct coda_aux_buf		psbuf;
+	struct coda_aux_buf		slicebuf;
+	struct coda_aux_buf		internal_frames[CODA_MAX_FRAMEBUFFERS];
+	u32				frame_types[CODA_MAX_FRAMEBUFFERS];
+	struct coda_timestamp		frame_timestamps[CODA_MAX_FRAMEBUFFERS];
+	u32				frame_errors[CODA_MAX_FRAMEBUFFERS];
+	struct list_head		timestamp_list;
+	struct coda_aux_buf		workbuf;
+	int				num_internal_frames;
+	int				idx;
+	int				reg_idx;
+	struct coda_iram_info		iram_info;
+	struct gdi_tiled_map		tiled_map;
+	u32				bit_stream_param;
+	u32				frm_dis_flg;
+	u32				frame_mem_ctrl;
+	int				display_idx;
+	struct dentry			*debugfs_entry;
+};
+
+extern int coda_debug;
+
+void coda_write(struct coda_dev *dev, u32 data, u32 reg);
+unsigned int coda_read(struct coda_dev *dev, u32 reg);
+
+int coda_alloc_aux_buf(struct coda_dev *dev, struct coda_aux_buf *buf,
+		       size_t size, const char *name, struct dentry *parent);
+void coda_free_aux_buf(struct coda_dev *dev, struct coda_aux_buf *buf);
+
+static inline int coda_alloc_context_buf(struct coda_ctx *ctx,
+					 struct coda_aux_buf *buf, size_t size,
+					 const char *name)
+{
+	return coda_alloc_aux_buf(ctx->dev, buf, size, name, ctx->debugfs_entry);
+}
+
+int coda_encoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq);
+int coda_decoder_queue_init(void *priv, struct vb2_queue *src_vq,
+			    struct vb2_queue *dst_vq);
+
+int coda_hw_reset(struct coda_ctx *ctx);
+
+void coda_fill_bitstream(struct coda_ctx *ctx);
+
+void coda_set_gdi_regs(struct coda_ctx *ctx);
+
+static inline struct coda_q_data *get_q_data(struct coda_ctx *ctx,
+					     enum v4l2_buf_type type)
+{
+	switch (type) {
+	case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+		return &(ctx->q_data[V4L2_M2M_SRC]);
+	case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+		return &(ctx->q_data[V4L2_M2M_DST]);
+	default:
+		return NULL;
+	}
+}
+
+const char *coda_product_name(int product);
+
+int coda_check_firmware(struct coda_dev *dev);
+
+static inline int coda_get_bitstream_payload(struct coda_ctx *ctx)
+{
+	return kfifo_len(&ctx->bitstream_fifo);
+}
+
+void coda_bit_stream_end_flag(struct coda_ctx *ctx);
+
+int coda_h264_padding(int size, char *p);
+
+extern const struct coda_context_ops coda_bit_encode_ops;
+extern const struct coda_context_ops coda_bit_decode_ops;
+
+irqreturn_t coda_irq_handler(int irq, void *data);
diff --git a/drivers/media/platform/coda.h b/drivers/media/platform/coda/coda_regs.h
index c791275e307b..c791275e307b 100644
--- a/drivers/media/platform/coda.h
+++ b/drivers/media/platform/coda/coda_regs.h
diff --git a/drivers/media/platform/davinci/Kconfig b/drivers/media/platform/davinci/Kconfig
index afb3aec1320e..d9e1ddb586b1 100644
--- a/drivers/media/platform/davinci/Kconfig
+++ b/drivers/media/platform/davinci/Kconfig
@@ -1,6 +1,8 @@
 config VIDEO_DAVINCI_VPIF_DISPLAY
 	tristate "TI DaVinci VPIF V4L2-Display driver"
-	depends on VIDEO_DEV && ARCH_DAVINCI
+	depends on VIDEO_DEV
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select VIDEO_ADV7343 if MEDIA_SUBDRV_AUTOSELECT
 	select VIDEO_THS7303 if MEDIA_SUBDRV_AUTOSELECT
@@ -14,7 +16,9 @@ config VIDEO_DAVINCI_VPIF_DISPLAY
 
 config VIDEO_DAVINCI_VPIF_CAPTURE
 	tristate "TI DaVinci VPIF video capture driver"
-	depends on VIDEO_DEV && ARCH_DAVINCI
+	depends on VIDEO_DEV
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	  Enables Davinci VPIF module used for capture devices.
@@ -26,7 +30,9 @@ config VIDEO_DAVINCI_VPIF_CAPTURE
 
 config VIDEO_DM6446_CCDC
 	tristate "TI DM6446 CCDC video capture driver"
-	depends on VIDEO_V4L2 && (ARCH_DAVINCI || ARCH_OMAP3)
+	depends on VIDEO_V4L2
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables DaVinci CCD hw module. DaVinci CCDC hw interfaces
@@ -40,7 +46,9 @@ config VIDEO_DM6446_CCDC
 
 config VIDEO_DM355_CCDC
 	tristate "TI DM355 CCDC video capture driver"
-	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	depends on VIDEO_V4L2
+	depends on ARCH_DAVINCI || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables DM355 CCD hw module. DM355 CCDC hw interfaces
@@ -55,6 +63,7 @@ config VIDEO_DM355_CCDC
 config VIDEO_DM365_ISIF
 	tristate "TI DM365 ISIF video capture driver"
 	depends on VIDEO_V4L2 && ARCH_DAVINCI
+	depends on HAS_DMA
 	select VIDEOBUF_DMA_CONTIG
 	help
 	   Enables ISIF hw module. This is the hardware module for
@@ -67,6 +76,7 @@ config VIDEO_DM365_ISIF
 config VIDEO_DAVINCI_VPBE_DISPLAY
 	tristate "TI DaVinci VPBE V4L2-Display driver"
 	depends on ARCH_DAVINCI
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	    Enables Davinci VPBE module used for display devices.
diff --git a/drivers/media/platform/davinci/dm355_ccdc.c b/drivers/media/platform/davinci/dm355_ccdc.c
index 05f8fb7f7b70..3f44deb5b7a7 100644
--- a/drivers/media/platform/davinci/dm355_ccdc.c
+++ b/drivers/media/platform/davinci/dm355_ccdc.c
@@ -460,7 +460,7 @@ static void ccdc_config_black_compense(struct ccdc_black_compensation *bcomp)
  * ccdc_write_dfc_entry()
  * write an entry in the dfc table.
  */
-int ccdc_write_dfc_entry(int index, struct ccdc_vertical_dft *dfc)
+static int ccdc_write_dfc_entry(int index, struct ccdc_vertical_dft *dfc)
 {
 /* TODO This is to be re-visited and adjusted */
 #define DFC_WRITE_WAIT_COUNT	1000
diff --git a/drivers/media/platform/davinci/dm644x_ccdc.c b/drivers/media/platform/davinci/dm644x_ccdc.c
index 07e98df3d867..62a0ebb01056 100644
--- a/drivers/media/platform/davinci/dm644x_ccdc.c
+++ b/drivers/media/platform/davinci/dm644x_ccdc.c
@@ -130,9 +130,9 @@ static void ccdc_enable_vport(int flag)
  * This function will configure the window size
  * to be capture in CCDC reg
  */
-void ccdc_setwin(struct v4l2_rect *image_win,
-		enum ccdc_frmfmt frm_fmt,
-		int ppc)
+static void ccdc_setwin(struct v4l2_rect *image_win,
+			enum ccdc_frmfmt frm_fmt,
+			int ppc)
 {
 	int horz_start, horz_nr_pixels;
 	int vert_start, vert_nr_lines;
@@ -291,7 +291,7 @@ static int ccdc_update_raw_params(struct ccdc_config_params_raw *raw_params)
 		dev_dbg(ccdc_cfg.dev, "\n copy_from_user failed");
 		return -EFAULT;
 	}
-	config_params->fault_pxl.fpc_table_addr = (unsigned int)fpc_physaddr;
+	config_params->fault_pxl.fpc_table_addr = (unsigned long)fpc_physaddr;
 	return 0;
 }
 
@@ -370,7 +370,7 @@ static int ccdc_set_params(void __user *params)
  * ccdc_config_ycbcr()
  * This function will configure CCDC for YCbCr video capture
  */
-void ccdc_config_ycbcr(void)
+static void ccdc_config_ycbcr(void)
 {
 	struct ccdc_params_ycbcr *params = &ccdc_cfg.ycbcr;
 	u32 syn_mode;
@@ -506,7 +506,7 @@ static void ccdc_config_fpc(struct ccdc_fault_pixel *fpc)
 
 	/* Configure Fault pixel if needed */
 	regw(fpc->fpc_table_addr, CCDC_FPC_ADDR);
-	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%x to FPC_ADDR...\n",
+	dev_dbg(ccdc_cfg.dev, "\nWriting 0x%lx to FPC_ADDR...\n",
 		       (fpc->fpc_table_addr));
 	/* Write the FPC params with FPC disable */
 	val = fpc->fp_num & CCDC_FPC_FPC_NUM_MASK;
@@ -523,7 +523,7 @@ static void ccdc_config_fpc(struct ccdc_fault_pixel *fpc)
  * ccdc_config_raw()
  * This function will configure CCDC for Raw capture mode
  */
-void ccdc_config_raw(void)
+static void ccdc_config_raw(void)
 {
 	struct ccdc_params_raw *params = &ccdc_cfg.bayer;
 	struct ccdc_config_params_raw *config_params =
diff --git a/drivers/media/platform/davinci/vpfe_capture.c b/drivers/media/platform/davinci/vpfe_capture.c
index ea7661a27479..de55f47a77db 100644
--- a/drivers/media/platform/davinci/vpfe_capture.c
+++ b/drivers/media/platform/davinci/vpfe_capture.c
@@ -125,7 +125,7 @@ static DEFINE_MUTEX(ccdc_lock);
 /* ccdc configuration */
 static struct ccdc_config *ccdc_cfg;
 
-const struct vpfe_standard vpfe_standards[] = {
+static const struct vpfe_standard vpfe_standards[] = {
 	{V4L2_STD_525_60, 720, 480, {11, 10}, 1},
 	{V4L2_STD_625_50, 720, 576, {54, 59}, 1},
 };
@@ -442,11 +442,10 @@ static int vpfe_config_image_format(struct vpfe_device *vpfe_dev,
 		return ret;
 
 	/* Update the values of sizeimage and bytesperline */
-	if (!ret) {
-		pix->bytesperline = ccdc_dev->hw_ops.get_line_length();
-		pix->sizeimage = pix->bytesperline * pix->height;
-	}
-	return ret;
+	pix->bytesperline = ccdc_dev->hw_ops.get_line_length();
+	pix->sizeimage = pix->bytesperline * pix->height;
+
+	return 0;
 }
 
 static int vpfe_initialize_device(struct vpfe_device *vpfe_dev)
@@ -943,12 +942,11 @@ static int vpfe_g_fmt_vid_cap(struct file *file, void *priv,
 				struct v4l2_format *fmt)
 {
 	struct vpfe_device *vpfe_dev = video_drvdata(file);
-	int ret = 0;
 
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev, "vpfe_g_fmt_vid_cap\n");
 	/* Fill in the information about format */
 	*fmt = vpfe_dev->fmt;
-	return ret;
+	return 0;
 }
 
 static int vpfe_enum_fmt_vid_cap(struct file *file, void  *priv,
@@ -1914,7 +1912,7 @@ static int vpfe_probe(struct platform_device *pdev)
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
 		"trying to register vpfe device.\n");
 	v4l2_dbg(1, debug, &vpfe_dev->v4l2_dev,
-		"video_dev=%x\n", (int)&vpfe_dev->video_dev);
+		"video_dev=%p\n", &vpfe_dev->video_dev);
 	vpfe_dev->fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 	ret = video_register_device(vpfe_dev->video_dev,
 				    VFL_TYPE_GRABBER, -1);
diff --git a/drivers/media/platform/davinci/vpif.c b/drivers/media/platform/davinci/vpif.c
index cd08e5248387..3dad5bd7fe0a 100644
--- a/drivers/media/platform/davinci/vpif.c
+++ b/drivers/media/platform/davinci/vpif.c
@@ -38,6 +38,7 @@ MODULE_LICENSE("GPL");
 #define VPIF_CH3_MAX_MODES	2
 
 spinlock_t vpif_lock;
+EXPORT_SYMBOL_GPL(vpif_lock);
 
 void __iomem *vpif_base;
 EXPORT_SYMBOL_GPL(vpif_base);
diff --git a/drivers/media/platform/davinci/vpif_capture.c b/drivers/media/platform/davinci/vpif_capture.c
index b054b7eec53d..3ccb26ff43c8 100644
--- a/drivers/media/platform/davinci/vpif_capture.c
+++ b/drivers/media/platform/davinci/vpif_capture.c
@@ -213,8 +213,6 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 	/* Remove buffer from the buffer queue */
 	list_del(&common->cur_frm->list);
 	spin_unlock_irqrestore(&common->irqlock, flags);
-	/* Mark state of the current frame to active */
-	common->cur_frm->vb.state = VB2_BUF_STATE_ACTIVE;
 
 	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb, 0);
 
@@ -350,7 +348,6 @@ static void vpif_schedule_next_buffer(struct common_obj *common)
 	/* Remove that buffer from the buffer queue */
 	list_del(&common->next_frm->list);
 	spin_unlock(&common->irqlock);
-	common->next_frm->vb.state = VB2_BUF_STATE_ACTIVE;
 	addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb, 0);
 
 	/* Set top and bottom field addresses in VPIF registers */
@@ -373,7 +370,6 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 	struct vpif_device *dev = &vpif_obj;
 	struct common_obj *common;
 	struct channel_obj *ch;
-	enum v4l2_field field;
 	int channel_id = 0;
 	int fid = -1, i;
 
@@ -383,8 +379,6 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 
 	ch = dev->dev[channel_id];
 
-	field = ch->common[VPIF_VIDEO_INDEX].fmt.fmt.pix.field;
-
 	for (i = 0; i < VPIF_NUMBER_OF_OBJECTS; i++) {
 		common = &ch->common[i];
 		/* skip If streaming is not started in this channel */
@@ -533,7 +527,7 @@ static int vpif_update_std_info(struct channel_obj *ch)
  */
 static void vpif_calculate_offsets(struct channel_obj *ch)
 {
-	unsigned int hpitch, vpitch, sizeimage;
+	unsigned int hpitch, sizeimage;
 	struct video_obj *vid_ch = &(ch->video);
 	struct vpif_params *vpifparams = &ch->vpifparams;
 	struct common_obj *common = &ch->common[VPIF_VIDEO_INDEX];
@@ -552,7 +546,6 @@ static void vpif_calculate_offsets(struct channel_obj *ch)
 	sizeimage = common->fmt.fmt.pix.sizeimage;
 
 	hpitch = common->fmt.fmt.pix.bytesperline;
-	vpitch = sizeimage / (hpitch * 2);
 
 	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
 	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) {
@@ -1603,7 +1596,7 @@ static int vpif_suspend(struct device *dev)
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
 
-		if (!vb2_is_streaming(&common->buffer_queue))
+		if (!vb2_start_streaming_called(&common->buffer_queue))
 			continue;
 
 		mutex_lock(&common->lock);
@@ -1637,7 +1630,7 @@ static int vpif_resume(struct device *dev)
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
 
-		if (!vb2_is_streaming(&common->buffer_queue))
+		if (!vb2_start_streaming_called(&common->buffer_queue))
 			continue;
 
 		mutex_lock(&common->lock);
diff --git a/drivers/media/platform/davinci/vpif_display.c b/drivers/media/platform/davinci/vpif_display.c
index a03ec7381cfe..8d6ced56253c 100644
--- a/drivers/media/platform/davinci/vpif_display.c
+++ b/drivers/media/platform/davinci/vpif_display.c
@@ -196,8 +196,6 @@ static int vpif_start_streaming(struct vb2_queue *vq, unsigned int count)
 
 	list_del(&common->cur_frm->list);
 	spin_unlock_irqrestore(&common->irqlock, flags);
-	/* Mark state of the current frame to active */
-	common->cur_frm->vb.state = VB2_BUF_STATE_ACTIVE;
 
 	addr = vb2_dma_contig_plane_dma_addr(&common->cur_frm->vb, 0);
 	common->set_addr((addr + common->ytop_off),
@@ -306,8 +304,6 @@ static void process_progressive_mode(struct common_obj *common)
 	/* Remove that buffer from the buffer queue */
 	list_del(&common->next_frm->list);
 	spin_unlock(&common->irqlock);
-	/* Mark status of the buffer as active */
-	common->next_frm->vb.state = VB2_BUF_STATE_ACTIVE;
 
 	/* Set top and bottom field addrs in VPIF registers */
 	addr = vb2_dma_contig_plane_dma_addr(&common->next_frm->vb, 0);
@@ -360,7 +356,6 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 	struct vpif_device *dev = &vpif_obj;
 	struct channel_obj *ch;
 	struct common_obj *common;
-	enum v4l2_field field;
 	int fid = -1, i;
 	int channel_id = 0;
 
@@ -369,7 +364,6 @@ static irqreturn_t vpif_channel_isr(int irq, void *dev_id)
 		return IRQ_NONE;
 
 	ch = dev->dev[channel_id];
-	field = ch->common[VPIF_VIDEO_INDEX].fmt.fmt.pix.field;
 	for (i = 0; i < VPIF_NUMOBJECTS; i++) {
 		common = &ch->common[i];
 		/* If streaming is started in this channel */
@@ -502,7 +496,7 @@ static void vpif_calculate_offsets(struct channel_obj *ch)
 	struct vpif_params *vpifparams = &ch->vpifparams;
 	enum v4l2_field field = common->fmt.fmt.pix.field;
 	struct video_obj *vid_ch = &ch->video;
-	unsigned int hpitch, vpitch, sizeimage;
+	unsigned int hpitch, sizeimage;
 
 	if (V4L2_FIELD_ANY == common->fmt.fmt.pix.field) {
 		if (ch->vpifparams.std_info.frm_fmt)
@@ -516,7 +510,6 @@ static void vpif_calculate_offsets(struct channel_obj *ch)
 	sizeimage = common->fmt.fmt.pix.sizeimage;
 
 	hpitch = common->fmt.fmt.pix.bytesperline;
-	vpitch = sizeimage / (hpitch * 2);
 	if ((V4L2_FIELD_NONE == vid_ch->buf_field) ||
 	    (V4L2_FIELD_INTERLACED == vid_ch->buf_field)) {
 		common->ytop_off = 0;
@@ -813,17 +806,14 @@ static int vpif_set_output(struct vpif_display_config *vpif_cfg,
 {
 	struct vpif_display_chan_config *chan_cfg =
 		&vpif_cfg->chan_config[ch->channel_id];
-	struct vpif_subdev_info *subdev_info = NULL;
 	struct v4l2_subdev *sd = NULL;
 	u32 input = 0, output = 0;
 	int sd_index;
 	int ret;
 
 	sd_index = vpif_output_to_subdev(vpif_cfg, chan_cfg, index);
-	if (sd_index >= 0) {
+	if (sd_index >= 0)
 		sd = vpif_obj.sd[sd_index];
-		subdev_info = &vpif_cfg->subdevinfo[sd_index];
-	}
 
 	if (sd) {
 		input = chan_cfg->outputs[index].input_route;
@@ -1210,8 +1200,8 @@ static int vpif_probe_complete(void)
 		INIT_LIST_HEAD(&common->dma_queue);
 
 		/* register video device */
-		vpif_dbg(1, debug, "channel=%x,channel->video_dev=%x\n",
-			 (int)ch, (int)&ch->video_dev);
+		vpif_dbg(1, debug, "channel=%p,channel->video_dev=%p\n",
+			 ch, &ch->video_dev);
 
 		/* Initialize the video_device structure */
 		vdev = ch->video_dev;
@@ -1410,7 +1400,7 @@ static int vpif_suspend(struct device *dev)
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
 
-		if (!vb2_is_streaming(&common->buffer_queue))
+		if (!vb2_start_streaming_called(&common->buffer_queue))
 			continue;
 
 		mutex_lock(&common->lock);
@@ -1442,7 +1432,7 @@ static int vpif_resume(struct device *dev)
 		ch = vpif_obj.dev[i];
 		common = &ch->common[VPIF_VIDEO_INDEX];
 
-		if (!vb2_is_streaming(&common->buffer_queue))
+		if (!vb2_start_streaming_called(&common->buffer_queue))
 			continue;
 
 		mutex_lock(&common->lock);
diff --git a/drivers/media/platform/exynos-gsc/gsc-core.c b/drivers/media/platform/exynos-gsc/gsc-core.c
index 9d0cc04d7ab7..b4c9f1d08968 100644
--- a/drivers/media/platform/exynos-gsc/gsc-core.c
+++ b/drivers/media/platform/exynos-gsc/gsc-core.c
@@ -852,8 +852,8 @@ int gsc_prepare_addr(struct gsc_ctx *ctx, struct vb2_buffer *vb,
 		(frame->fmt->pixelformat == V4L2_PIX_FMT_YVU420M))
 		swap(addr->cb, addr->cr);
 
-	pr_debug("ADDR: y= 0x%X  cb= 0x%X cr= 0x%X ret= %d",
-		addr->y, addr->cb, addr->cr, ret);
+	pr_debug("ADDR: y= %pad  cb= %pad cr= %pad ret= %d",
+		&addr->y, &addr->cb, &addr->cr, ret);
 
 	return ret;
 }
@@ -1086,7 +1086,7 @@ static int gsc_probe(struct platform_device *pdev)
 	else
 		gsc->id = pdev->id;
 
-	if (gsc->id < 0 || gsc->id >= drv_data->num_entities) {
+	if (gsc->id >= drv_data->num_entities) {
 		dev_err(dev, "Invalid platform device id: %d\n", gsc->id);
 		return -EINVAL;
 	}
diff --git a/drivers/media/platform/exynos-gsc/gsc-m2m.c b/drivers/media/platform/exynos-gsc/gsc-m2m.c
index e434f1f03d7b..74e1de637e8f 100644
--- a/drivers/media/platform/exynos-gsc/gsc-m2m.c
+++ b/drivers/media/platform/exynos-gsc/gsc-m2m.c
@@ -362,7 +362,6 @@ static int gsc_m2m_reqbufs(struct file *file, void *fh,
 {
 	struct gsc_ctx *ctx = fh_to_ctx(fh);
 	struct gsc_dev *gsc = ctx->gsc_dev;
-	struct gsc_frame *frame;
 	u32 max_cnt;
 
 	max_cnt = (reqbufs->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) ?
@@ -376,8 +375,6 @@ static int gsc_m2m_reqbufs(struct file *file, void *fh,
 			gsc_ctx_state_lock_clear(GSC_DST_FMT, ctx);
 	}
 
-	frame = ctx_get_frame(ctx, reqbufs->type);
-
 	return v4l2_m2m_reqbufs(file, ctx->m2m_ctx, reqbufs);
 }
 
diff --git a/drivers/media/platform/exynos-gsc/gsc-regs.c b/drivers/media/platform/exynos-gsc/gsc-regs.c
index e22d147a6940..ce12a1100511 100644
--- a/drivers/media/platform/exynos-gsc/gsc-regs.c
+++ b/drivers/media/platform/exynos-gsc/gsc-regs.c
@@ -90,8 +90,8 @@ void gsc_hw_set_output_buf_masking(struct gsc_dev *dev, u32 shift,
 void gsc_hw_set_input_addr(struct gsc_dev *dev, struct gsc_addr *addr,
 				int index)
 {
-	pr_debug("src_buf[%d]: 0x%X, cb: 0x%X, cr: 0x%X", index,
-			addr->y, addr->cb, addr->cr);
+	pr_debug("src_buf[%d]: %pad, cb: %pad, cr: %pad", index,
+			&addr->y, &addr->cb, &addr->cr);
 	writel(addr->y, dev->regs + GSC_IN_BASE_ADDR_Y(index));
 	writel(addr->cb, dev->regs + GSC_IN_BASE_ADDR_CB(index));
 	writel(addr->cr, dev->regs + GSC_IN_BASE_ADDR_CR(index));
@@ -101,8 +101,8 @@ void gsc_hw_set_input_addr(struct gsc_dev *dev, struct gsc_addr *addr,
 void gsc_hw_set_output_addr(struct gsc_dev *dev,
 			     struct gsc_addr *addr, int index)
 {
-	pr_debug("dst_buf[%d]: 0x%X, cb: 0x%X, cr: 0x%X",
-			index, addr->y, addr->cb, addr->cr);
+	pr_debug("dst_buf[%d]: %pad, cb: %pad, cr: %pad",
+			index, &addr->y, &addr->cb, &addr->cr);
 	writel(addr->y, dev->regs + GSC_OUT_BASE_ADDR_Y(index));
 	writel(addr->cb, dev->regs + GSC_OUT_BASE_ADDR_CB(index));
 	writel(addr->cr, dev->regs + GSC_OUT_BASE_ADDR_CR(index));
diff --git a/drivers/media/platform/exynos4-is/Kconfig b/drivers/media/platform/exynos4-is/Kconfig
index 5dcaa0a80540..77c951237744 100644
--- a/drivers/media/platform/exynos4-is/Kconfig
+++ b/drivers/media/platform/exynos4-is/Kconfig
@@ -2,7 +2,7 @@
 config VIDEO_SAMSUNG_EXYNOS4_IS
 	bool "Samsung S5P/EXYNOS4 SoC series Camera Subsystem driver"
 	depends on VIDEO_V4L2 && VIDEO_V4L2_SUBDEV_API
-	depends on (PLAT_S5P || ARCH_EXYNOS)
+	depends on (PLAT_S5P || ARCH_EXYNOS || COMPILE_TEST)
 	depends on OF && COMMON_CLK
 	help
 	  Say Y here to enable camera host interface devices for
@@ -16,6 +16,7 @@ config VIDEO_EXYNOS4_IS_COMMON
 config VIDEO_S5P_FIMC
 	tristate "S5P/EXYNOS4 FIMC/CAMIF camera interface driver"
 	depends on I2C
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select V4L2_MEM2MEM_DEV
 	select MFD_SYSCON
@@ -43,6 +44,7 @@ if SOC_EXYNOS4212 || SOC_EXYNOS4412 || SOC_EXYNOS5250
 config VIDEO_EXYNOS_FIMC_LITE
 	tristate "EXYNOS FIMC-LITE camera interface driver"
 	depends on I2C
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select VIDEO_EXYNOS4_IS_COMMON
 	help
@@ -55,6 +57,7 @@ endif
 
 config VIDEO_EXYNOS4_FIMC_IS
 	tristate "EXYNOS4x12 FIMC-IS (Imaging Subsystem) driver"
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	depends on OF
 	select FW_LOADER
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.c b/drivers/media/platform/exynos4-is/fimc-is-errno.c
index e8519e151c1a..e050e63fe358 100644
--- a/drivers/media/platform/exynos4-is/fimc-is-errno.c
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.c
@@ -15,7 +15,7 @@
 
 #include "fimc-is-errno.h"
 
-const char * const fimc_is_param_strerr(unsigned int error)
+const char *fimc_is_param_strerr(unsigned int error)
 {
 	switch (error) {
 	case ERROR_COMMON_CMD:
@@ -146,7 +146,7 @@ const char * const fimc_is_param_strerr(unsigned int error)
 	}
 }
 
-const char * const fimc_is_strerr(unsigned int error)
+const char *fimc_is_strerr(unsigned int error)
 {
 	error &= ~IS_ERROR_TIME_OUT_FLAG;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-is-errno.h b/drivers/media/platform/exynos4-is/fimc-is-errno.h
index 3de6f6da6f87..ef981e74513a 100644
--- a/drivers/media/platform/exynos4-is/fimc-is-errno.h
+++ b/drivers/media/platform/exynos4-is/fimc-is-errno.h
@@ -242,7 +242,7 @@ enum fimc_is_error {
 	ERROR_SCALER_FLIP				= 521,
 };
 
-const char * const fimc_is_strerr(unsigned int error);
-const char * const fimc_is_param_strerr(unsigned int error);
+const char *fimc_is_strerr(unsigned int error);
+const char *fimc_is_param_strerr(unsigned int error);
 
 #endif /* FIMC_IS_ERR_H_ */
diff --git a/drivers/media/platform/exynos4-is/fimc-is-param.c b/drivers/media/platform/exynos4-is/fimc-is-param.c
index bf1465d1bf6d..72b9b436c5c0 100644
--- a/drivers/media/platform/exynos4-is/fimc-is-param.c
+++ b/drivers/media/platform/exynos4-is/fimc-is-param.c
@@ -667,7 +667,6 @@ void __is_set_fd_config_orientation_val(struct fimc_is *is, u32 val)
 void fimc_is_set_initial_params(struct fimc_is *is)
 {
 	struct global_param *global;
-	struct sensor_param *sensor;
 	struct isp_param *isp;
 	struct drc_param *drc;
 	struct fd_param *fd;
@@ -676,7 +675,6 @@ void fimc_is_set_initial_params(struct fimc_is *is)
 
 	index = is->config_index;
 	global = &is->config[index].global;
-	sensor = &is->config[index].sensor;
 	isp = &is->config[index].isp;
 	drc = &is->config[index].drc;
 	fd = &is->config[index].fd;
diff --git a/drivers/media/platform/exynos4-is/fimc-is.c b/drivers/media/platform/exynos4-is/fimc-is.c
index 5476dce3ad29..22162b2567da 100644
--- a/drivers/media/platform/exynos4-is/fimc-is.c
+++ b/drivers/media/platform/exynos4-is/fimc-is.c
@@ -388,7 +388,7 @@ static void fimc_is_load_firmware(const struct firmware *fw, void *context)
 	mutex_lock(&is->lock);
 
 	if (fw->size < FIMC_IS_FW_SIZE_MIN || fw->size > FIMC_IS_FW_SIZE_MAX) {
-		dev_err(dev, "wrong firmware size: %d\n", fw->size);
+		dev_err(dev, "wrong firmware size: %zu\n", fw->size);
 		goto done;
 	}
 
@@ -416,7 +416,7 @@ static void fimc_is_load_firmware(const struct firmware *fw, void *context)
 
 	dev_info(dev, "loaded firmware: %s, rev. %s\n",
 		 is->fw.info, is->fw.version);
-	dev_dbg(dev, "FW size: %d, paddr: %#x\n", fw->size, is->memory.paddr);
+	dev_dbg(dev, "FW size: %zu, paddr: %pad\n", fw->size, &is->memory.paddr);
 
 	is->is_shared_region->chip_id = 0xe4412;
 	is->is_shared_region->chip_rev_no = 1;
@@ -693,9 +693,9 @@ int fimc_is_hw_initialize(struct fimc_is *is)
 		return -EIO;
 	}
 
-	pr_debug("shared region: %#x, parameter region: %#x\n",
-		 is->memory.paddr + FIMC_IS_SHARED_REGION_OFFSET,
-		 is->is_dma_p_region);
+	pr_debug("shared region: %pad, parameter region: %pad\n",
+		 &is->memory.paddr + FIMC_IS_SHARED_REGION_OFFSET,
+		 &is->is_dma_p_region);
 
 	is->setfile.sub_index = 0;
 
diff --git a/drivers/media/platform/exynos4-is/fimc-isp-video.c b/drivers/media/platform/exynos4-is/fimc-isp-video.c
index 93f9cf2ebcd6..76b6b4d14616 100644
--- a/drivers/media/platform/exynos4-is/fimc-isp-video.c
+++ b/drivers/media/platform/exynos4-is/fimc-isp-video.c
@@ -219,9 +219,9 @@ static void isp_video_capture_buffer_queue(struct vb2_buffer *vb)
 							ivb->dma_addr[i];
 
 			isp_dbg(2, &video->ve.vdev,
-				"dma_buf %d (%d/%d/%d) addr: %#x\n",
-				buf_index, ivb->index, i, vb->v4l2_buf.index,
-				ivb->dma_addr[i]);
+				"dma_buf %pad (%d/%d/%d) addr: %pad\n",
+				&buf_index, ivb->index, i, vb->v4l2_buf.index,
+				&ivb->dma_addr[i]);
 		}
 
 		if (++video->buf_count < video->reqbufs_count)
@@ -313,7 +313,6 @@ static int isp_video_release(struct file *file)
 	struct fimc_is_video *ivc = &isp->video_capture;
 	struct media_entity *entity = &ivc->ve.vdev.entity;
 	struct media_device *mdev = entity->parent;
-	int ret = 0;
 
 	mutex_lock(&isp->video_lock);
 
@@ -335,7 +334,7 @@ static int isp_video_release(struct file *file)
 	pm_runtime_put(&isp->pdev->dev);
 	mutex_unlock(&isp->video_lock);
 
-	return ret;
+	return 0;
 }
 
 static const struct v4l2_file_operations isp_video_fops = {
diff --git a/drivers/media/platform/exynos4-is/media-dev.c b/drivers/media/platform/exynos4-is/media-dev.c
index 344718df5c62..54c49d5e7690 100644
--- a/drivers/media/platform/exynos4-is/media-dev.c
+++ b/drivers/media/platform/exynos4-is/media-dev.c
@@ -1098,8 +1098,10 @@ static int fimc_md_link_notify(struct media_link *link, unsigned int flags,
 	if (notification == MEDIA_DEV_NOTIFY_PRE_LINK_CH) {
 		if (!(flags & MEDIA_LNK_FL_ENABLED))
 			ret = __fimc_md_modify_pipelines(sink, false);
+#if 0
 		else
-			; /* TODO: Link state change validation */
+			/* TODO: Link state change validation */
+#endif
 	/* After link activation */
 	} else if (notification == MEDIA_DEV_NOTIFY_POST_LINK_CH &&
 		   (link->flags & MEDIA_LNK_FL_ENABLED)) {
diff --git a/drivers/media/platform/exynos4-is/mipi-csis.c b/drivers/media/platform/exynos4-is/mipi-csis.c
index ae54ef5f535d..db6fd14d1936 100644
--- a/drivers/media/platform/exynos4-is/mipi-csis.c
+++ b/drivers/media/platform/exynos4-is/mipi-csis.c
@@ -25,6 +25,7 @@
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
+#include <linux/sizes.h>
 #include <linux/slab.h>
 #include <linux/spinlock.h>
 #include <linux/videodev2.h>
@@ -752,7 +753,7 @@ static int s5pcsis_parse_dt(struct platform_device *pdev,
 	v4l2_of_parse_endpoint(node, &endpoint);
 
 	state->index = endpoint.base.port - FIMC_INPUT_MIPI_CSI2_0;
-	if (state->index < 0 || state->index >= CSIS_MAX_ENTITIES)
+	if (state->index >= CSIS_MAX_ENTITIES)
 		return -ENXIO;
 
 	/* Get MIPI CSI-2 bus configration from the endpoint node. */
diff --git a/drivers/media/platform/marvell-ccic/Kconfig b/drivers/media/platform/marvell-ccic/Kconfig
index bf739e3b3398..6265d36adceb 100644
--- a/drivers/media/platform/marvell-ccic/Kconfig
+++ b/drivers/media/platform/marvell-ccic/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_CAFE_CCIC
 	tristate "Marvell 88ALP01 (Cafe) CMOS Camera Controller support"
 	depends on PCI && I2C && VIDEO_V4L2
+	depends on HAS_DMA
 	select VIDEO_OV7670
 	select VIDEOBUF2_VMALLOC
 	select VIDEOBUF2_DMA_CONTIG
@@ -12,6 +13,7 @@ config VIDEO_CAFE_CCIC
 config VIDEO_MMP_CAMERA
 	tristate "Marvell Armada 610 integrated camera controller support"
 	depends on ARCH_MMP && I2C && VIDEO_V4L2
+	depends on HAS_DMA
 	select VIDEO_OV7670
 	select I2C_GPIO
 	select VIDEOBUF2_DMA_SG
diff --git a/drivers/media/platform/marvell-ccic/mcam-core.c b/drivers/media/platform/marvell-ccic/mcam-core.c
index be4b51212106..7a86c77bffa0 100644
--- a/drivers/media/platform/marvell-ccic/mcam-core.c
+++ b/drivers/media/platform/marvell-ccic/mcam-core.c
@@ -67,7 +67,7 @@ MODULE_PARM_DESC(dma_buf_size,
 		"parameters require larger buffers, an attempt to reallocate "
 		"will be made.");
 #else /* MCAM_MODE_VMALLOC */
-static const bool alloc_bufs_at_read = 0;
+static const bool alloc_bufs_at_read;
 static const int n_dma_bufs = 3;  /* Used by S/G_PARM */
 #endif /* MCAM_MODE_VMALLOC */
 
diff --git a/drivers/media/platform/mx2_emmaprp.c b/drivers/media/platform/mx2_emmaprp.c
index fa8f7cabe364..4971ff21f82b 100644
--- a/drivers/media/platform/mx2_emmaprp.c
+++ b/drivers/media/platform/mx2_emmaprp.c
@@ -27,7 +27,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/videobuf2-dma-contig.h>
-#include <asm/sizes.h>
+#include <linux/sizes.h>
 
 #define EMMAPRP_MODULE_NAME "mem2mem-emmaprp"
 
diff --git a/drivers/media/platform/omap/Kconfig b/drivers/media/platform/omap/Kconfig
index 37ad446b35b3..05de442d24e4 100644
--- a/drivers/media/platform/omap/Kconfig
+++ b/drivers/media/platform/omap/Kconfig
@@ -3,7 +3,7 @@ config VIDEO_OMAP2_VOUT_VRFB
 
 config VIDEO_OMAP2_VOUT
 	tristate "OMAP2/OMAP3 V4L2-Display driver"
-	depends on ARCH_OMAP2 || ARCH_OMAP3
+	depends on ARCH_OMAP2 || ARCH_OMAP3 || (COMPILE_TEST && HAS_MMU)
 	select VIDEOBUF_GEN
 	select VIDEOBUF_DMA_CONTIG
 	select OMAP2_DSS if HAS_IOMEM && ARCH_OMAP2PLUS
diff --git a/drivers/media/platform/omap/omap_vout.c b/drivers/media/platform/omap/omap_vout.c
index 2d177fa58471..64ab6fb06b9c 100644
--- a/drivers/media/platform/omap/omap_vout.c
+++ b/drivers/media/platform/omap/omap_vout.c
@@ -369,7 +369,7 @@ static int omapvid_setup_overlay(struct omap_vout_device *vout,
 {
 	int ret = 0;
 	struct omap_overlay_info info;
-	int cropheight, cropwidth, pixheight, pixwidth;
+	int cropheight, cropwidth, pixwidth;
 
 	if ((ovl->caps & OMAP_DSS_OVL_CAP_SCALE) == 0 &&
 			(outw != vout->pix.width || outh != vout->pix.height)) {
@@ -389,12 +389,10 @@ static int omapvid_setup_overlay(struct omap_vout_device *vout,
 	if (is_rotation_90_or_270(vout)) {
 		cropheight = vout->crop.width;
 		cropwidth = vout->crop.height;
-		pixheight = vout->pix.width;
 		pixwidth = vout->pix.height;
 	} else {
 		cropheight = vout->crop.height;
 		cropwidth = vout->crop.width;
-		pixheight = vout->pix.height;
 		pixwidth = vout->pix.width;
 	}
 
@@ -991,7 +989,7 @@ static int omap_vout_release(struct file *file)
 		mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN |
 			DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_VSYNC2;
 		omap_dispc_unregister_isr(omap_vout_isr, vout, mask);
-		vout->streaming = 0;
+		vout->streaming = false;
 
 		videobuf_streamoff(q);
 		videobuf_queue_cancel(q);
@@ -1451,12 +1449,10 @@ static int vidioc_s_ctrl(struct file *file, void *fh, struct v4l2_control *a)
 	}
 	case V4L2_CID_VFLIP:
 	{
-		struct omap_overlay *ovl;
 		struct omapvideo_info *ovid;
 		unsigned int  mirror = a->value;
 
 		ovid = &vout->vid_info;
-		ovl = ovid->overlays[0];
 
 		mutex_lock(&vout->lock);
 		if (mirror && ovid->rotation_type == VOUT_ROT_NONE) {
@@ -1489,7 +1485,7 @@ static int vidioc_reqbufs(struct file *file, void *fh,
 	struct omap_vout_device *vout = fh;
 	struct videobuf_queue *q = &vout->vbq;
 
-	if ((req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT) || (req->count < 0))
+	if (req->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
 		return -EINVAL;
 	/* if memory is not mmp or userptr
 	   return error */
@@ -1648,7 +1644,7 @@ static int vidioc_streamon(struct file *file, void *fh, enum v4l2_buf_type i)
 	vout->field_id = 0;
 
 	/* set flag here. Next QBUF will start DMA */
-	vout->streaming = 1;
+	vout->streaming = true;
 
 	vout->first_int = 1;
 
@@ -1708,7 +1704,7 @@ static int vidioc_streamoff(struct file *file, void *fh, enum v4l2_buf_type i)
 	if (!vout->streaming)
 		return -EINVAL;
 
-	vout->streaming = 0;
+	vout->streaming = false;
 	mask = DISPC_IRQ_VSYNC | DISPC_IRQ_EVSYNC_EVEN | DISPC_IRQ_EVSYNC_ODD
 		| DISPC_IRQ_VSYNC2;
 
@@ -1916,7 +1912,7 @@ static int __init omap_vout_setup_video_data(struct omap_vout_device *vout)
 	control[0].id = V4L2_CID_ROTATE;
 	control[0].value = 0;
 	vout->rotation = 0;
-	vout->mirror = 0;
+	vout->mirror = false;
 	vout->control[2].id = V4L2_CID_HFLIP;
 	vout->control[2].value = 0;
 	if (vout->vid_info.rotation_type == VOUT_ROT_VRFB)
diff --git a/drivers/media/platform/omap/omap_vout_vrfb.c b/drivers/media/platform/omap/omap_vout_vrfb.c
index 62e7e5783ce8..aa39306afc73 100644
--- a/drivers/media/platform/omap/omap_vout_vrfb.c
+++ b/drivers/media/platform/omap/omap_vout_vrfb.c
@@ -148,7 +148,7 @@ int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
 			ret =  -ENOMEM;
 			goto release_vrfb_ctx;
 		}
-		vout->vrfb_static_allocation = 1;
+		vout->vrfb_static_allocation = true;
 	}
 	return 0;
 
@@ -336,7 +336,7 @@ void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout)
 		offset = vout->vrfb_context[0].yoffset *
 			vout->vrfb_context[0].bytespp;
 		temp_ps = ps / vr_ps;
-		if (mirroring == 0) {
+		if (!mirroring) {
 			*cropped_offset = offset + line_length *
 				temp_ps * cleft + crop->top * temp_ps;
 		} else {
@@ -350,7 +350,7 @@ void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout)
 			vout->vrfb_context[0].bytespp) +
 			(vout->vrfb_context[0].xoffset *
 			vout->vrfb_context[0].bytespp));
-		if (mirroring == 0) {
+		if (!mirroring) {
 			*cropped_offset = offset + (line_length * ps * ctop) +
 				(cleft / vr_ps) * ps;
 
@@ -364,7 +364,7 @@ void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout)
 		offset = MAX_PIXELS_PER_LINE * vout->vrfb_context[0].xoffset *
 			vout->vrfb_context[0].bytespp;
 		temp_ps = ps / vr_ps;
-		if (mirroring == 0) {
+		if (!mirroring) {
 			*cropped_offset = offset + line_length *
 			    temp_ps * crop->left + ctop * ps;
 		} else {
@@ -375,7 +375,7 @@ void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout)
 		}
 		break;
 	case dss_rotation_0_degree:
-		if (mirroring == 0) {
+		if (!mirroring) {
 			*cropped_offset = (line_length * ps) *
 				crop->top + (crop->left / vr_ps) * ps;
 		} else {
diff --git a/drivers/media/platform/omap/omap_vout_vrfb.h b/drivers/media/platform/omap/omap_vout_vrfb.h
index ffde741e0590..4c2314839b48 100644
--- a/drivers/media/platform/omap/omap_vout_vrfb.h
+++ b/drivers/media/platform/omap/omap_vout_vrfb.h
@@ -23,18 +23,18 @@ int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
 			struct videobuf_buffer *vb);
 void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout);
 #else
-void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout) { }
-int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
+static inline void omap_vout_free_vrfb_buffers(struct omap_vout_device *vout) { };
+static inline int omap_vout_setup_vrfb_bufs(struct platform_device *pdev, int vid_num,
 			u32 static_vrfb_allocation)
-		{ return 0; }
-void omap_vout_release_vrfb(struct omap_vout_device *vout) { }
-int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
+		{ return 0; };
+static inline void omap_vout_release_vrfb(struct omap_vout_device *vout) { };
+static inline int omap_vout_vrfb_buffer_setup(struct omap_vout_device *vout,
 			unsigned int *count, unsigned int startindex)
-		{ return 0; }
-int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
+		{ return 0; };
+static inline int omap_vout_prepare_vrfb(struct omap_vout_device *vout,
 			struct videobuf_buffer *vb)
-		{ return 0; }
-void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout) { }
+		{ return 0; };
+static inline void omap_vout_calculate_vrfb_offset(struct omap_vout_device *vout) { };
 #endif
 
 #endif
diff --git a/drivers/media/platform/omap3isp/cfa_coef_table.h b/drivers/media/platform/omap3isp/cfa_coef_table.h
index c84df0706f3e..e75b0eb2519b 100644
--- a/drivers/media/platform/omap3isp/cfa_coef_table.h
+++ b/drivers/media/platform/omap3isp/cfa_coef_table.h
@@ -11,16 +11,6 @@
  * This program is free software; you can redistribute it and/or
  * modify it under the terms of the GNU General Public License
  * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 { 244, 0, 247,   0,  12,  27,  36, 247, 250,   0,  27,   0,   4, 250,  12, 244,
diff --git a/drivers/media/platform/omap3isp/gamma_table.h b/drivers/media/platform/omap3isp/gamma_table.h
index 78deebf7d965..3b507078016d 100644
--- a/drivers/media/platform/omap3isp/gamma_table.h
+++ b/drivers/media/platform/omap3isp/gamma_table.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
   0,   0,   1,   2,   3,   3,   4,   5,   6,   8,  10,  12,  14,  16,  18,  20,
diff --git a/drivers/media/platform/omap3isp/isp.c b/drivers/media/platform/omap3isp/isp.c
index 2c7aa6720569..72265e58ca60 100644
--- a/drivers/media/platform/omap3isp/isp.c
+++ b/drivers/media/platform/omap3isp/isp.c
@@ -40,16 +40,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <asm/cacheflush.h>
@@ -999,16 +989,14 @@ static int isp_pipeline_disable(struct isp_pipeline *pipe)
 					 video, s_stream, 0);
 		}
 
-		v4l2_subdev_call(subdev, video, s_stream, 0);
+		ret = v4l2_subdev_call(subdev, video, s_stream, 0);
 
 		if (subdev == &isp->isp_res.subdev)
-			ret = isp_pipeline_wait(isp, isp_pipeline_wait_resizer);
+			ret |= isp_pipeline_wait(isp, isp_pipeline_wait_resizer);
 		else if (subdev == &isp->isp_prev.subdev)
-			ret = isp_pipeline_wait(isp, isp_pipeline_wait_preview);
+			ret |= isp_pipeline_wait(isp, isp_pipeline_wait_preview);
 		else if (subdev == &isp->isp_ccdc.subdev)
-			ret = isp_pipeline_wait(isp, isp_pipeline_wait_ccdc);
-		else
-			ret = 0;
+			ret |= isp_pipeline_wait(isp, isp_pipeline_wait_ccdc);
 
 		/* Handle stop failures. An entity that fails to stop can
 		 * usually just be restarted. Flag the stop failure nonetheless
diff --git a/drivers/media/platform/omap3isp/isp.h b/drivers/media/platform/omap3isp/isp.h
index 2c314eea1252..cfdfc8714b6b 100644
--- a/drivers/media/platform/omap3isp/isp.h
+++ b/drivers/media/platform/omap3isp/isp.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_CORE_H
diff --git a/drivers/media/platform/omap3isp/ispccdc.c b/drivers/media/platform/omap3isp/ispccdc.c
index 9f727d20f06d..81a9dc053d58 100644
--- a/drivers/media/platform/omap3isp/ispccdc.c
+++ b/drivers/media/platform/omap3isp/ispccdc.c
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/module.h>
@@ -491,14 +481,13 @@ done:
 static inline int ccdc_lsc_is_configured(struct isp_ccdc_device *ccdc)
 {
 	unsigned long flags;
+	int ret;
 
 	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
-	if (ccdc->lsc.active) {
-		spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
-		return 1;
-	}
+	ret = ccdc->lsc.active != NULL;
 	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
-	return 0;
+
+	return ret;
 }
 
 static int ccdc_lsc_enable(struct isp_ccdc_device *ccdc)
@@ -818,29 +807,48 @@ static void ccdc_config_vp(struct isp_ccdc_device *ccdc)
 	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
 	struct isp_device *isp = to_isp_device(ccdc);
 	const struct isp_format_info *info;
+	struct v4l2_mbus_framefmt *format;
 	unsigned long l3_ick = pipe->l3_ick;
 	unsigned int max_div = isp->revision == ISP_REVISION_15_0 ? 64 : 8;
 	unsigned int div = 0;
-	u32 fmtcfg_vp;
+	u32 fmtcfg = ISPCCDC_FMTCFG_VPEN;
+
+	format = &ccdc->formats[CCDC_PAD_SOURCE_VP];
+
+	if (!format->code) {
+		/* Disable the video port when the input format isn't supported.
+		 * This is indicated by a pixel code set to 0.
+		 */
+		isp_reg_writel(isp, 0, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMTCFG);
+		return;
+	}
+
+	isp_reg_writel(isp, (0 << ISPCCDC_FMT_HORZ_FMTSPH_SHIFT) |
+		       (format->width << ISPCCDC_FMT_HORZ_FMTLNH_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMT_HORZ);
+	isp_reg_writel(isp, (0 << ISPCCDC_FMT_VERT_FMTSLV_SHIFT) |
+		       ((format->height + 1) << ISPCCDC_FMT_VERT_FMTLNV_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMT_VERT);
 
-	fmtcfg_vp = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMTCFG)
-		  & ~(ISPCCDC_FMTCFG_VPIN_MASK | ISPCCDC_FMTCFG_VPIF_FRQ_MASK);
+	isp_reg_writel(isp, (format->width << ISPCCDC_VP_OUT_HORZ_NUM_SHIFT) |
+		       (format->height << ISPCCDC_VP_OUT_VERT_NUM_SHIFT),
+		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VP_OUT);
 
 	info = omap3isp_video_format_info(ccdc->formats[CCDC_PAD_SINK].code);
 
 	switch (info->width) {
 	case 8:
 	case 10:
-		fmtcfg_vp |= ISPCCDC_FMTCFG_VPIN_9_0;
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_9_0;
 		break;
 	case 11:
-		fmtcfg_vp |= ISPCCDC_FMTCFG_VPIN_10_1;
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_10_1;
 		break;
 	case 12:
-		fmtcfg_vp |= ISPCCDC_FMTCFG_VPIN_11_2;
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_11_2;
 		break;
 	case 13:
-		fmtcfg_vp |= ISPCCDC_FMTCFG_VPIN_12_3;
+		fmtcfg |= ISPCCDC_FMTCFG_VPIN_12_3;
 		break;
 	}
 
@@ -850,75 +858,59 @@ static void ccdc_config_vp(struct isp_ccdc_device *ccdc)
 		div = l3_ick / pipe->external_rate;
 
 	div = clamp(div, 2U, max_div);
-	fmtcfg_vp |= (div - 2) << ISPCCDC_FMTCFG_VPIF_FRQ_SHIFT;
+	fmtcfg |= (div - 2) << ISPCCDC_FMTCFG_VPIF_FRQ_SHIFT;
 
-	isp_reg_writel(isp, fmtcfg_vp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMTCFG);
-}
-
-/*
- * ccdc_enable_vp - Enable Video Port.
- * @ccdc: Pointer to ISP CCDC device.
- * @enable: 0 Disables VP, 1 Enables VP
- *
- * This is needed for outputting image to Preview, H3A and HIST ISP submodules.
- */
-static void ccdc_enable_vp(struct isp_ccdc_device *ccdc, u8 enable)
-{
-	struct isp_device *isp = to_isp_device(ccdc);
-
-	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMTCFG,
-			ISPCCDC_FMTCFG_VPEN, enable ? ISPCCDC_FMTCFG_VPEN : 0);
+	isp_reg_writel(isp, fmtcfg, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMTCFG);
 }
 
 /*
  * ccdc_config_outlineoffset - Configure memory saving output line offset
  * @ccdc: Pointer to ISP CCDC device.
- * @offset: Address offset to start a new line. Must be twice the
- *          Output width and aligned on 32 byte boundary
- * @oddeven: Specifies the odd/even line pattern to be chosen to store the
- *           output.
- * @numlines: Set the value 0-3 for +1-4lines, 4-7 for -1-4lines.
+ * @bpl: Number of bytes per line when stored in memory.
+ * @field: Field order when storing interlaced formats in memory.
+ *
+ * Configure the offsets for the line output control:
+ *
+ * - The horizontal line offset is defined as the number of bytes between the
+ *   start of two consecutive lines in memory. Set it to the given bytes per
+ *   line value.
+ *
+ * - The field offset value is defined as the number of lines to offset the
+ *   start of the field identified by FID = 1. Set it to one.
  *
- * - Configures the output line offset when stored in memory
- * - Sets the odd/even line pattern to store the output
- *    (EVENEVEN (1), ODDEVEN (2), EVENODD (3), ODDODD (4))
- * - Configures the number of even and odd line fields in case of rearranging
- * the lines.
+ * - The line offset values are defined as the number of lines (as defined by
+ *   the horizontal line offset) between the start of two consecutive lines for
+ *   all combinations of odd/even lines in odd/even fields. When interleaving
+ *   fields set them all to two lines, and to one line otherwise.
  */
 static void ccdc_config_outlineoffset(struct isp_ccdc_device *ccdc,
-					u32 offset, u8 oddeven, u8 numlines)
+				      unsigned int bpl,
+				      enum v4l2_field field)
 {
 	struct isp_device *isp = to_isp_device(ccdc);
+	u32 sdofst = 0;
 
-	isp_reg_writel(isp, offset & 0xffff,
-		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_HSIZE_OFF);
+	isp_reg_writel(isp, bpl & 0xffff, OMAP3_ISP_IOMEM_CCDC,
+		       ISPCCDC_HSIZE_OFF);
 
-	isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST,
-		    ISPCCDC_SDOFST_FINV);
-
-	isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST,
-		    ISPCCDC_SDOFST_FOFST_4L);
-
-	switch (oddeven) {
-	case EVENEVEN:
-		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST,
-			    (numlines & 0x7) << ISPCCDC_SDOFST_LOFST0_SHIFT);
-		break;
-	case ODDEVEN:
-		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST,
-			    (numlines & 0x7) << ISPCCDC_SDOFST_LOFST1_SHIFT);
-		break;
-	case EVENODD:
-		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST,
-			    (numlines & 0x7) << ISPCCDC_SDOFST_LOFST2_SHIFT);
-		break;
-	case ODDODD:
-		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST,
-			    (numlines & 0x7) << ISPCCDC_SDOFST_LOFST3_SHIFT);
+	switch (field) {
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+		/* When interleaving fields in memory offset field one by one
+		 * line and set the line offset to two lines.
+		 */
+		sdofst |= (1 << ISPCCDC_SDOFST_LOFST0_SHIFT)
+		       |  (1 << ISPCCDC_SDOFST_LOFST1_SHIFT)
+		       |  (1 << ISPCCDC_SDOFST_LOFST2_SHIFT)
+		       |  (1 << ISPCCDC_SDOFST_LOFST3_SHIFT);
 		break;
+
 	default:
+		/* In all other cases set the line offsets to one line. */
 		break;
 	}
+
+	isp_reg_writel(isp, sdofst, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SDOFST);
 }
 
 /*
@@ -981,10 +973,16 @@ static void ccdc_config_sync_if(struct isp_ccdc_device *ccdc,
 
 	if (format->code == V4L2_MBUS_FMT_YUYV8_2X8 ||
 	    format->code == V4L2_MBUS_FMT_UYVY8_2X8) {
-		/* The bridge is enabled for YUV8 formats. Configure the input
-		 * mode accordingly.
+		/* According to the OMAP3 TRM the input mode only affects SYNC
+		 * mode, enabling BT.656 mode should take precedence. However,
+		 * in practice setting the input mode to YCbCr data on 8 bits
+		 * seems to be required in BT.656 mode. In SYNC mode set it to
+		 * YCbCr on 16 bits as the bridge is enabled in that case.
 		 */
-		syn_mode |= ISPCCDC_SYN_MODE_INPMOD_YCBCR16;
+		if (ccdc->bt656)
+			syn_mode |= ISPCCDC_SYN_MODE_INPMOD_YCBCR8;
+		else
+			syn_mode |= ISPCCDC_SYN_MODE_INPMOD_YCBCR16;
 	}
 
 	switch (data_size) {
@@ -1008,9 +1006,15 @@ static void ccdc_config_sync_if(struct isp_ccdc_device *ccdc,
 	if (pdata && pdata->hs_pol)
 		syn_mode |= ISPCCDC_SYN_MODE_HDPOL;
 
-	if (pdata && pdata->vs_pol)
+	/* The polarity of the vertical sync signal output by the BT.656
+	 * decoder is not documented and seems to be active low.
+	 */
+	if ((pdata && pdata->vs_pol) || ccdc->bt656)
 		syn_mode |= ISPCCDC_SYN_MODE_VDPOL;
 
+	if (pdata && pdata->fld_pol)
+		syn_mode |= ISPCCDC_SYN_MODE_FLDPOL;
+
 	isp_reg_writel(isp, syn_mode, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE);
 
 	/* The CCDC_CFG.Y8POS bit is used in YCbCr8 input mode only. The
@@ -1023,8 +1027,16 @@ static void ccdc_config_sync_if(struct isp_ccdc_device *ccdc,
 		isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG,
 			    ISPCCDC_CFG_Y8POS);
 
-	isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_REC656IF,
-		    ISPCCDC_REC656IF_R656ON);
+	/* Enable or disable BT.656 mode, including error correction for the
+	 * synchronization codes.
+	 */
+	if (ccdc->bt656)
+		isp_reg_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_REC656IF,
+			    ISPCCDC_REC656IF_R656ON | ISPCCDC_REC656IF_ECCFVH);
+	else
+		isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_REC656IF,
+			    ISPCCDC_REC656IF_R656ON | ISPCCDC_REC656IF_ECCFVH);
+
 }
 
 /* CCDC formats descriptions */
@@ -1115,17 +1127,33 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 	unsigned long flags;
 	unsigned int bridge;
 	unsigned int shift;
+	unsigned int nph;
+	unsigned int sph;
 	u32 syn_mode;
 	u32 ccdc_pattern;
 
+	ccdc->bt656 = false;
+	ccdc->fields = 0;
+
 	pad = media_entity_remote_pad(&ccdc->pads[CCDC_PAD_SINK]);
 	sensor = media_entity_to_v4l2_subdev(pad->entity);
-	if (ccdc->input == CCDC_INPUT_PARALLEL)
+	if (ccdc->input == CCDC_INPUT_PARALLEL) {
+		struct v4l2_mbus_config cfg;
+		int ret;
+
+		ret = v4l2_subdev_call(sensor, video, g_mbus_config, &cfg);
+		if (!ret)
+			ccdc->bt656 = cfg.type == V4L2_MBUS_BT656;
+
 		pdata = &((struct isp_v4l2_subdevs_group *)sensor->host_priv)
 			->bus.parallel;
+	}
+
+	/* CCDC_PAD_SINK */
+	format = &ccdc->formats[CCDC_PAD_SINK];
 
 	/* Compute the lane shifter shift value and enable the bridge when the
-	 * input format is YUV.
+	 * input format is a non-BT.656 YUV variant.
 	 */
 	fmt_src.pad = pad->index;
 	fmt_src.which = V4L2_SUBDEV_FORMAT_ACTIVE;
@@ -1134,12 +1162,13 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 		depth_in = fmt_info->width;
 	}
 
-	fmt_info = omap3isp_video_format_info
-		(isp->isp_ccdc.formats[CCDC_PAD_SINK].code);
+	fmt_info = omap3isp_video_format_info(format->code);
 	depth_out = fmt_info->width;
 	shift = depth_in - depth_out;
 
-	if (fmt_info->code == V4L2_MBUS_FMT_YUYV8_2X8)
+	if (ccdc->bt656)
+		bridge = ISPCTRL_PAR_BRIDGE_DISABLE;
+	else if (fmt_info->code == V4L2_MBUS_FMT_YUYV8_2X8)
 		bridge = ISPCTRL_PAR_BRIDGE_LENDIAN;
 	else if (fmt_info->code == V4L2_MBUS_FMT_UYVY8_2X8)
 		bridge = ISPCTRL_PAR_BRIDGE_BENDIAN;
@@ -1148,6 +1177,7 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 
 	omap3isp_configure_bridge(isp, ccdc->input, pdata, shift, bridge);
 
+	/* Configure the sync interface. */
 	ccdc_config_sync_if(ccdc, pdata, depth_out);
 
 	syn_mode = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE);
@@ -1167,9 +1197,6 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 	else
 		syn_mode &= ~ISPCCDC_SYN_MODE_SDR2RSZ;
 
-	/* CCDC_PAD_SINK */
-	format = &ccdc->formats[CCDC_PAD_SINK];
-
 	/* Mosaic filter */
 	switch (format->code) {
 	case V4L2_MBUS_FMT_SRGGB10_1X10:
@@ -1202,16 +1229,40 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 	format = &ccdc->formats[CCDC_PAD_SOURCE_OF];
 	crop = &ccdc->crop;
 
-	isp_reg_writel(isp, (crop->left << ISPCCDC_HORZ_INFO_SPH_SHIFT) |
-		       ((crop->width - 1) << ISPCCDC_HORZ_INFO_NPH_SHIFT),
+	/* The horizontal coordinates are expressed in pixel clock cycles. We
+	 * need two cycles per pixel in BT.656 mode, and one cycle per pixel in
+	 * SYNC mode regardless of the format as the bridge is enabled for YUV
+	 * formats in that case.
+	 */
+	if (ccdc->bt656) {
+		sph = crop->left * 2;
+		nph = crop->width * 2 - 1;
+	} else {
+		sph = crop->left;
+		nph = crop->width - 1;
+	}
+
+	isp_reg_writel(isp, (sph << ISPCCDC_HORZ_INFO_SPH_SHIFT) |
+		       (nph << ISPCCDC_HORZ_INFO_NPH_SHIFT),
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_HORZ_INFO);
-	isp_reg_writel(isp, crop->top << ISPCCDC_VERT_START_SLV0_SHIFT,
+	isp_reg_writel(isp, (crop->top << ISPCCDC_VERT_START_SLV0_SHIFT) |
+		       (crop->top << ISPCCDC_VERT_START_SLV1_SHIFT),
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_START);
 	isp_reg_writel(isp, (crop->height - 1)
 			<< ISPCCDC_VERT_LINES_NLV_SHIFT,
 		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VERT_LINES);
 
-	ccdc_config_outlineoffset(ccdc, ccdc->video_out.bpl_value, 0, 0);
+	ccdc_config_outlineoffset(ccdc, ccdc->video_out.bpl_value,
+				  format->field);
+
+	/* When interleaving fields enable processing of the field input signal.
+	 * This will cause the line output control module to apply the field
+	 * offset to field 1.
+	 */
+	if (ccdc->formats[CCDC_PAD_SINK].field == V4L2_FIELD_ALTERNATE &&
+	    (format->field == V4L2_FIELD_INTERLACED_TB ||
+	     format->field == V4L2_FIELD_INTERLACED_BT))
+		syn_mode |= ISPCCDC_SYN_MODE_FLDMODE;
 
 	/* The CCDC outputs data in UYVY order by default. Swap bytes to get
 	 * YUYV.
@@ -1223,8 +1274,11 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 		isp_reg_clr(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_CFG,
 			    ISPCCDC_CFG_BSWD);
 
-	/* Use PACK8 mode for 1byte per pixel formats. */
-	if (omap3isp_video_format_info(format->code)->width <= 8)
+	/* Use PACK8 mode for 1byte per pixel formats. Check for BT.656 mode
+	 * explicitly as the driver reports 1X16 instead of 2X8 at the OF pad
+	 * for simplicity.
+	 */
+	if (omap3isp_video_format_info(format->code)->width <= 8 || ccdc->bt656)
 		syn_mode |= ISPCCDC_SYN_MODE_PACK8;
 	else
 		syn_mode &= ~ISPCCDC_SYN_MODE_PACK8;
@@ -1232,18 +1286,7 @@ static void ccdc_configure(struct isp_ccdc_device *ccdc)
 	isp_reg_writel(isp, syn_mode, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE);
 
 	/* CCDC_PAD_SOURCE_VP */
-	format = &ccdc->formats[CCDC_PAD_SOURCE_VP];
-
-	isp_reg_writel(isp, (0 << ISPCCDC_FMT_HORZ_FMTSPH_SHIFT) |
-		       (format->width << ISPCCDC_FMT_HORZ_FMTLNH_SHIFT),
-		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMT_HORZ);
-	isp_reg_writel(isp, (0 << ISPCCDC_FMT_VERT_FMTSLV_SHIFT) |
-		       ((format->height + 1) << ISPCCDC_FMT_VERT_FMTLNV_SHIFT),
-		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_FMT_VERT);
-
-	isp_reg_writel(isp, (format->width << ISPCCDC_VP_OUT_HORZ_NUM_SHIFT) |
-		       (format->height << ISPCCDC_VP_OUT_VERT_NUM_SHIFT),
-		       OMAP3_ISP_IOMEM_CCDC, ISPCCDC_VP_OUT);
+	ccdc_config_vp(ccdc);
 
 	/* Lens shading correction. */
 	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
@@ -1277,6 +1320,8 @@ static void __ccdc_enable(struct isp_ccdc_device *ccdc, int enable)
 
 	isp_reg_clr_set(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_PCR,
 			ISPCCDC_PCR_EN, enable ? ISPCCDC_PCR_EN : 0);
+
+	ccdc->running = enable;
 }
 
 static int ccdc_disable(struct isp_ccdc_device *ccdc)
@@ -1287,6 +1332,8 @@ static int ccdc_disable(struct isp_ccdc_device *ccdc)
 	spin_lock_irqsave(&ccdc->lock, flags);
 	if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS)
 		ccdc->stopping = CCDC_STOP_REQUEST;
+	if (!ccdc->running)
+		ccdc->stopping = CCDC_STOP_FINISHED;
 	spin_unlock_irqrestore(&ccdc->lock, flags);
 
 	ret = wait_event_timeout(ccdc->wait,
@@ -1369,14 +1416,14 @@ static int ccdc_sbl_wait_idle(struct isp_ccdc_device *ccdc,
 	return -EBUSY;
 }
 
-/* __ccdc_handle_stopping - Handle CCDC and/or LSC stopping sequence
+/* ccdc_handle_stopping - Handle CCDC and/or LSC stopping sequence
  * @ccdc: Pointer to ISP CCDC device.
  * @event: Pointing which event trigger handler
  *
  * Return 1 when the event and stopping request combination is satisfied,
  * zero otherwise.
  */
-static int __ccdc_handle_stopping(struct isp_ccdc_device *ccdc, u32 event)
+static int ccdc_handle_stopping(struct isp_ccdc_device *ccdc, u32 event)
 {
 	int rval = 0;
 
@@ -1458,7 +1505,7 @@ static void ccdc_lsc_isr(struct isp_ccdc_device *ccdc, u32 events)
 	if (ccdc->lsc.state == LSC_STATE_STOPPING)
 		ccdc->lsc.state = LSC_STATE_STOPPED;
 
-	if (__ccdc_handle_stopping(ccdc, CCDC_EVENT_LSC_DONE))
+	if (ccdc_handle_stopping(ccdc, CCDC_EVENT_LSC_DONE))
 		goto done;
 
 	if (ccdc->lsc.state != LSC_STATE_RECONFIG)
@@ -1486,12 +1533,59 @@ done:
 	spin_unlock_irqrestore(&ccdc->lsc.req_lock, flags);
 }
 
+/*
+ * Check whether the CCDC has captured all fields necessary to complete the
+ * buffer.
+ */
+static bool ccdc_has_all_fields(struct isp_ccdc_device *ccdc)
+{
+	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
+	struct isp_device *isp = to_isp_device(ccdc);
+	enum v4l2_field of_field = ccdc->formats[CCDC_PAD_SOURCE_OF].field;
+	enum v4l2_field field;
+
+	/* When the input is progressive fields don't matter. */
+	if (of_field == V4L2_FIELD_NONE)
+		return true;
+
+	/* Read the current field identifier. */
+	field = isp_reg_readl(isp, OMAP3_ISP_IOMEM_CCDC, ISPCCDC_SYN_MODE)
+	      & ISPCCDC_SYN_MODE_FLDSTAT
+	      ? V4L2_FIELD_BOTTOM : V4L2_FIELD_TOP;
+
+	/* When capturing fields in alternate order just store the current field
+	 * identifier in the pipeline.
+	 */
+	if (of_field == V4L2_FIELD_ALTERNATE) {
+		pipe->field = field;
+		return true;
+	}
+
+	/* The format is interlaced. Make sure we've captured both fields. */
+	ccdc->fields |= field == V4L2_FIELD_BOTTOM
+		      ? CCDC_FIELD_BOTTOM : CCDC_FIELD_TOP;
+
+	if (ccdc->fields != CCDC_FIELD_BOTH)
+		return false;
+
+	/* Verify that the field just captured corresponds to the last field
+	 * needed based on the desired field order.
+	 */
+	if ((of_field == V4L2_FIELD_INTERLACED_TB && field == V4L2_FIELD_TOP) ||
+	    (of_field == V4L2_FIELD_INTERLACED_BT && field == V4L2_FIELD_BOTTOM))
+		return false;
+
+	/* The buffer can be completed, reset the fields for the next buffer. */
+	ccdc->fields = 0;
+
+	return true;
+}
+
 static int ccdc_isr_buffer(struct isp_ccdc_device *ccdc)
 {
 	struct isp_pipeline *pipe = to_isp_pipeline(&ccdc->subdev.entity);
 	struct isp_device *isp = to_isp_device(ccdc);
 	struct isp_buffer *buffer;
-	int restart = 0;
 
 	/* The CCDC generates VD0 interrupts even when disabled (the datasheet
 	 * doesn't explicitly state if that's supposed to happen or not, so it
@@ -1500,30 +1594,31 @@ static int ccdc_isr_buffer(struct isp_ccdc_device *ccdc)
 	 * would thus not be enough, we need to handle the situation explicitly.
 	 */
 	if (list_empty(&ccdc->video_out.dmaqueue))
-		goto done;
+		return 0;
 
 	/* We're in continuous mode, and memory writes were disabled due to a
 	 * buffer underrun. Reenable them now that we have a buffer. The buffer
 	 * address has been set in ccdc_video_queue.
 	 */
 	if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS && ccdc->underrun) {
-		restart = 1;
 		ccdc->underrun = 0;
-		goto done;
+		return 1;
 	}
 
+	/* Wait for the CCDC to become idle. */
 	if (ccdc_sbl_wait_idle(ccdc, 1000)) {
 		dev_info(isp->dev, "CCDC won't become idle!\n");
 		isp->crashed |= 1U << ccdc->subdev.entity.id;
 		omap3isp_pipeline_cancel_stream(pipe);
-		goto done;
+		return 0;
 	}
 
+	if (!ccdc_has_all_fields(ccdc))
+		return 1;
+
 	buffer = omap3isp_video_buffer_next(&ccdc->video_out);
-	if (buffer != NULL) {
+	if (buffer != NULL)
 		ccdc_set_outaddr(ccdc, buffer->dma);
-		restart = 1;
-	}
 
 	pipe->state |= ISP_PIPELINE_IDLE_OUTPUT;
 
@@ -1532,8 +1627,7 @@ static int ccdc_isr_buffer(struct isp_ccdc_device *ccdc)
 		omap3isp_pipeline_set_stream(pipe,
 					ISP_PIPELINE_STREAM_SINGLESHOT);
 
-done:
-	return restart;
+	return buffer != NULL;
 }
 
 /*
@@ -1547,11 +1641,38 @@ static void ccdc_vd0_isr(struct isp_ccdc_device *ccdc)
 	unsigned long flags;
 	int restart = 0;
 
+	/* In BT.656 mode the CCDC doesn't generate an HS/VS interrupt. We thus
+	 * need to increment the frame counter here.
+	 */
+	if (ccdc->bt656) {
+		struct isp_pipeline *pipe =
+			to_isp_pipeline(&ccdc->subdev.entity);
+
+		atomic_inc(&pipe->frame_number);
+	}
+
+	/* Emulate a VD1 interrupt for BT.656 mode, as we can't stop the CCDC in
+	 * the VD1 interrupt handler in that mode without risking a CCDC stall
+	 * if a short frame is received.
+	 */
+	if (ccdc->bt656) {
+		spin_lock_irqsave(&ccdc->lock, flags);
+		if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS &&
+		    ccdc->output & CCDC_OUTPUT_MEMORY) {
+			if (ccdc->lsc.state != LSC_STATE_STOPPED)
+				__ccdc_lsc_enable(ccdc, 0);
+			__ccdc_enable(ccdc, 0);
+		}
+		ccdc_handle_stopping(ccdc, CCDC_EVENT_VD1);
+		spin_unlock_irqrestore(&ccdc->lock, flags);
+	}
+
 	if (ccdc->output & CCDC_OUTPUT_MEMORY)
 		restart = ccdc_isr_buffer(ccdc);
 
 	spin_lock_irqsave(&ccdc->lock, flags);
-	if (__ccdc_handle_stopping(ccdc, CCDC_EVENT_VD0)) {
+
+	if (ccdc_handle_stopping(ccdc, CCDC_EVENT_VD0)) {
 		spin_unlock_irqrestore(&ccdc->lock, flags);
 		return;
 	}
@@ -1572,6 +1693,18 @@ static void ccdc_vd1_isr(struct isp_ccdc_device *ccdc)
 {
 	unsigned long flags;
 
+	/* In BT.656 mode the synchronization signals are generated by the CCDC
+	 * from the embedded sync codes. The VD0 and VD1 interrupts are thus
+	 * only triggered when the CCDC is enabled, unlike external sync mode
+	 * where the line counter runs even when the CCDC is stopped. We can't
+	 * disable the CCDC at VD1 time, as no VD0 interrupt would be generated
+	 * for a short frame, which would result in the CCDC being stopped and
+	 * no VD interrupt generated anymore. The CCDC is stopped from the VD0
+	 * interrupt handler instead for BT.656.
+	 */
+	if (ccdc->bt656)
+		return;
+
 	spin_lock_irqsave(&ccdc->lsc.req_lock, flags);
 
 	/*
@@ -1601,7 +1734,7 @@ static void ccdc_vd1_isr(struct isp_ccdc_device *ccdc)
 		break;
 	}
 
-	if (__ccdc_handle_stopping(ccdc, CCDC_EVENT_VD1))
+	if (ccdc_handle_stopping(ccdc, CCDC_EVENT_VD1))
 		goto done;
 
 	if (ccdc->lsc.request == NULL)
@@ -1656,6 +1789,8 @@ int omap3isp_ccdc_isr(struct isp_ccdc_device *ccdc, u32 events)
 static int ccdc_video_queue(struct isp_video *video, struct isp_buffer *buffer)
 {
 	struct isp_ccdc_device *ccdc = &video->isp->isp_ccdc;
+	unsigned long flags;
+	bool restart = false;
 
 	if (!(ccdc->output & CCDC_OUTPUT_MEMORY))
 		return -ENODEV;
@@ -1664,9 +1799,20 @@ static int ccdc_video_queue(struct isp_video *video, struct isp_buffer *buffer)
 
 	/* We now have a buffer queued on the output, restart the pipeline
 	 * on the next CCDC interrupt if running in continuous mode (or when
-	 * starting the stream).
+	 * starting the stream) in external sync mode, or immediately in BT.656
+	 * sync mode as no CCDC interrupt is generated when the CCDC is stopped
+	 * in that case.
 	 */
-	ccdc->underrun = 1;
+	spin_lock_irqsave(&ccdc->lock, flags);
+	if (ccdc->state == ISP_PIPELINE_STREAM_CONTINUOUS && !ccdc->running &&
+	    ccdc->bt656)
+		restart = true;
+	else
+		ccdc->underrun = 1;
+	spin_unlock_irqrestore(&ccdc->lock, flags);
+
+	if (restart)
+		ccdc_enable(ccdc);
 
 	return 0;
 }
@@ -1753,11 +1899,6 @@ static int ccdc_set_stream(struct v4l2_subdev *sd, int enable)
 
 		ccdc_configure(ccdc);
 
-		/* TODO: Don't configure the video port if all of its output
-		 * links are inactive.
-		 */
-		ccdc_config_vp(ccdc);
-		ccdc_enable_vp(ccdc, 1);
 		ccdc_print_status(ccdc);
 	}
 
@@ -1830,6 +1971,7 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 	unsigned int width = fmt->width;
 	unsigned int height = fmt->height;
 	struct v4l2_rect *crop;
+	enum v4l2_field field;
 	unsigned int i;
 
 	switch (pad) {
@@ -1846,14 +1988,24 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		/* Clamp the input size. */
 		fmt->width = clamp_t(u32, width, 32, 4096);
 		fmt->height = clamp_t(u32, height, 32, 4096);
+
+		/* Default to progressive field order. */
+		if (fmt->field == V4L2_FIELD_ANY)
+			fmt->field = V4L2_FIELD_NONE;
+
 		break;
 
 	case CCDC_PAD_SOURCE_OF:
 		pixelcode = fmt->code;
+		field = fmt->field;
 		*fmt = *__ccdc_get_format(ccdc, fh, CCDC_PAD_SINK, which);
 
-		/* YUV formats are converted from 2X8 to 1X16 by the bridge and
-		 * can be byte-swapped.
+		/* In SYNC mode the bridge converts YUV formats from 2X8 to
+		 * 1X16. In BT.656 no such conversion occurs. As we don't know
+		 * at this point whether the source will use SYNC or BT.656 mode
+		 * let's pretend the conversion always occurs. The CCDC will be
+		 * configured to pack bytes in BT.656, hiding the inaccuracy.
+		 * In all cases bytes can be swapped.
 		 */
 		if (fmt->code == V4L2_MBUS_FMT_YUYV8_2X8 ||
 		    fmt->code == V4L2_MBUS_FMT_UYVY8_2X8) {
@@ -1874,6 +2026,17 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 		crop = __ccdc_get_crop(ccdc, fh, which);
 		fmt->width = crop->width;
 		fmt->height = crop->height;
+
+		/* When input format is interlaced with alternating fields the
+		 * CCDC can interleave the fields.
+		 */
+		if (fmt->field == V4L2_FIELD_ALTERNATE &&
+		    (field == V4L2_FIELD_INTERLACED_TB ||
+		     field == V4L2_FIELD_INTERLACED_BT)) {
+			fmt->field = field;
+			fmt->height *= 2;
+		}
+
 		break;
 
 	case CCDC_PAD_SOURCE_VP:
@@ -1901,7 +2064,6 @@ ccdc_try_format(struct isp_ccdc_device *ccdc, struct v4l2_subdev_fh *fh,
 	 * stored on 2 bytes.
 	 */
 	fmt->colorspace = V4L2_COLORSPACE_SRGB;
-	fmt->field = V4L2_FIELD_NONE;
 }
 
 /*
diff --git a/drivers/media/platform/omap3isp/ispccdc.h b/drivers/media/platform/omap3isp/ispccdc.h
index f65061602c71..3440a7097940 100644
--- a/drivers/media/platform/omap3isp/ispccdc.h
+++ b/drivers/media/platform/omap3isp/ispccdc.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_CCDC_H
@@ -103,6 +93,10 @@ struct ispccdc_lsc {
 #define CCDC_PAD_SOURCE_VP		2
 #define CCDC_PADS_NUM			3
 
+#define CCDC_FIELD_TOP			1
+#define CCDC_FIELD_BOTTOM		2
+#define CCDC_FIELD_BOTH			3
+
 /*
  * struct isp_ccdc_device - Structure for the CCDC module to store its own
  *			    information
@@ -123,11 +117,14 @@ struct ispccdc_lsc {
  * @lsc: Lens shading compensation configuration
  * @update: Bitmask of controls to update during the next interrupt
  * @shadow_update: Controls update in progress by userspace
+ * @bt656: Whether the input interface uses BT.656 synchronization
+ * @fields: The fields (CCDC_FIELD_*) stored in the current buffer
  * @underrun: A buffer underrun occurred and a new buffer has been queued
  * @state: Streaming state
  * @lock: Serializes shadow_update with interrupt handler
  * @wait: Wait queue used to stop the module
  * @stopping: Stopping state
+ * @running: Is the CCDC hardware running
  * @ioctl_lock: Serializes ioctl calls and LSC requests freeing
  */
 struct isp_ccdc_device {
@@ -151,11 +148,15 @@ struct isp_ccdc_device {
 	unsigned int update;
 	unsigned int shadow_update;
 
+	bool bt656;
+	unsigned int fields;
+
 	unsigned int underrun:1;
 	enum isp_pipeline_stream_state state;
 	spinlock_t lock;
 	wait_queue_head_t wait;
 	unsigned int stopping;
+	bool running;
 	struct mutex ioctl_lock;
 };
 
diff --git a/drivers/media/platform/omap3isp/ispccp2.c b/drivers/media/platform/omap3isp/ispccp2.c
index f3801db9095c..9cb49b3c04bd 100644
--- a/drivers/media/platform/omap3isp/ispccp2.c
+++ b/drivers/media/platform/omap3isp/ispccp2.c
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/delay.h>
diff --git a/drivers/media/platform/omap3isp/ispccp2.h b/drivers/media/platform/omap3isp/ispccp2.h
index 76d65f4576ef..4662bffa79e3 100644
--- a/drivers/media/platform/omap3isp/ispccp2.h
+++ b/drivers/media/platform/omap3isp/ispccp2.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_CCP2_H
diff --git a/drivers/media/platform/omap3isp/ispcsi2.c b/drivers/media/platform/omap3isp/ispcsi2.c
index 5a2e47e58b84..6530b255f103 100644
--- a/drivers/media/platform/omap3isp/ispcsi2.c
+++ b/drivers/media/platform/omap3isp/ispcsi2.c
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 #include <linux/delay.h>
 #include <media/v4l2-common.h>
diff --git a/drivers/media/platform/omap3isp/ispcsi2.h b/drivers/media/platform/omap3isp/ispcsi2.h
index c57729b7e86e..453ed62fe394 100644
--- a/drivers/media/platform/omap3isp/ispcsi2.h
+++ b/drivers/media/platform/omap3isp/ispcsi2.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_CSI2_H
diff --git a/drivers/media/platform/omap3isp/ispcsiphy.c b/drivers/media/platform/omap3isp/ispcsiphy.c
index c09de32f986a..e033f2237a72 100644
--- a/drivers/media/platform/omap3isp/ispcsiphy.c
+++ b/drivers/media/platform/omap3isp/ispcsiphy.c
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/delay.h>
diff --git a/drivers/media/platform/omap3isp/ispcsiphy.h b/drivers/media/platform/omap3isp/ispcsiphy.h
index 14551fd77697..e17c88beab92 100644
--- a/drivers/media/platform/omap3isp/ispcsiphy.h
+++ b/drivers/media/platform/omap3isp/ispcsiphy.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_CSI_PHY_H
diff --git a/drivers/media/platform/omap3isp/isph3a.h b/drivers/media/platform/omap3isp/isph3a.h
index fb09fd4ca755..e5b28d0f3b0f 100644
--- a/drivers/media/platform/omap3isp/isph3a.h
+++ b/drivers/media/platform/omap3isp/isph3a.h
@@ -13,16 +13,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_H3A_H
diff --git a/drivers/media/platform/omap3isp/isph3a_aewb.c b/drivers/media/platform/omap3isp/isph3a_aewb.c
index d6811ce263eb..b208c5417146 100644
--- a/drivers/media/platform/omap3isp/isph3a_aewb.c
+++ b/drivers/media/platform/omap3isp/isph3a_aewb.c
@@ -13,16 +13,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/slab.h>
diff --git a/drivers/media/platform/omap3isp/isph3a_af.c b/drivers/media/platform/omap3isp/isph3a_af.c
index 6fc960cd30f5..8a83e195f3e3 100644
--- a/drivers/media/platform/omap3isp/isph3a_af.c
+++ b/drivers/media/platform/omap3isp/isph3a_af.c
@@ -13,16 +13,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 /* Linux specific include files */
diff --git a/drivers/media/platform/omap3isp/isphist.c b/drivers/media/platform/omap3isp/isphist.c
index 06a5f8164eaa..ce822c34c843 100644
--- a/drivers/media/platform/omap3isp/isphist.c
+++ b/drivers/media/platform/omap3isp/isphist.c
@@ -13,16 +13,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/delay.h>
diff --git a/drivers/media/platform/omap3isp/isphist.h b/drivers/media/platform/omap3isp/isphist.h
index 0b2a38ec94c4..3b5415517dcd 100644
--- a/drivers/media/platform/omap3isp/isphist.h
+++ b/drivers/media/platform/omap3isp/isphist.h
@@ -13,16 +13,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_HIST_H
diff --git a/drivers/media/platform/omap3isp/isppreview.c b/drivers/media/platform/omap3isp/isppreview.c
index 720809b07e75..605f57ef0a49 100644
--- a/drivers/media/platform/omap3isp/isppreview.c
+++ b/drivers/media/platform/omap3isp/isppreview.c
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/device.h>
diff --git a/drivers/media/platform/omap3isp/isppreview.h b/drivers/media/platform/omap3isp/isppreview.h
index f66923407f8c..16fdc03a3d43 100644
--- a/drivers/media/platform/omap3isp/isppreview.h
+++ b/drivers/media/platform/omap3isp/isppreview.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_PREVIEW_H
diff --git a/drivers/media/platform/omap3isp/ispreg.h b/drivers/media/platform/omap3isp/ispreg.h
index b7d90e6fb01d..b5ea8da0b904 100644
--- a/drivers/media/platform/omap3isp/ispreg.h
+++ b/drivers/media/platform/omap3isp/ispreg.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_REG_H
@@ -740,17 +730,13 @@
 
 #define ISPCCDC_HSIZE_OFF_SHIFT			0
 
-#define ISPCCDC_SDOFST_FINV			(1 << 14)
-#define ISPCCDC_SDOFST_FOFST_1L			0
-#define ISPCCDC_SDOFST_FOFST_4L			(3 << 12)
+#define ISPCCDC_SDOFST_FIINV			(1 << 14)
+#define ISPCCDC_SDOFST_FOFST_SHIFT		12
+#define ISPCCDC_SDOFST_FOFST_MASK		(3 << 12)
 #define ISPCCDC_SDOFST_LOFST3_SHIFT		0
 #define ISPCCDC_SDOFST_LOFST2_SHIFT		3
 #define ISPCCDC_SDOFST_LOFST1_SHIFT		6
 #define ISPCCDC_SDOFST_LOFST0_SHIFT		9
-#define EVENEVEN				1
-#define ODDEVEN					2
-#define EVENODD					3
-#define ODDODD					4
 
 #define ISPCCDC_CLAMP_OBGAIN_SHIFT		0
 #define ISPCCDC_CLAMP_OBST_SHIFT		10
diff --git a/drivers/media/platform/omap3isp/ispresizer.c b/drivers/media/platform/omap3isp/ispresizer.c
index 6f077c2377db..05d1ace57451 100644
--- a/drivers/media/platform/omap3isp/ispresizer.c
+++ b/drivers/media/platform/omap3isp/ispresizer.c
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/device.h>
@@ -239,7 +229,7 @@ static void resizer_set_phase(struct isp_res_device *res, u32 h_phase,
 			      u32 v_phase)
 {
 	struct isp_device *isp = to_isp_device(res);
-	u32 rgval = 0;
+	u32 rgval;
 
 	rgval = isp_reg_readl(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT) &
 	      ~(ISPRSZ_CNT_HSTPH_MASK | ISPRSZ_CNT_VSTPH_MASK);
@@ -275,7 +265,7 @@ static void resizer_set_luma(struct isp_res_device *res,
 			     struct resizer_luma_yenh *luma)
 {
 	struct isp_device *isp = to_isp_device(res);
-	u32 rgval = 0;
+	u32 rgval;
 
 	rgval  = (luma->algo << ISPRSZ_YENH_ALGO_SHIFT)
 		  & ISPRSZ_YENH_ALGO_MASK;
@@ -322,7 +312,7 @@ static void resizer_set_ratio(struct isp_res_device *res,
 {
 	struct isp_device *isp = to_isp_device(res);
 	const u16 *h_filter, *v_filter;
-	u32 rgval = 0;
+	u32 rgval;
 
 	rgval = isp_reg_readl(isp, OMAP3_ISP_IOMEM_RESZ, ISPRSZ_CNT) &
 			      ~(ISPRSZ_CNT_HRSZ_MASK | ISPRSZ_CNT_VRSZ_MASK);
@@ -365,9 +355,8 @@ static void resizer_set_output_size(struct isp_res_device *res,
 				    u32 width, u32 height)
 {
 	struct isp_device *isp = to_isp_device(res);
-	u32 rgval = 0;
+	u32 rgval;
 
-	dev_dbg(isp->dev, "Output size[w/h]: %dx%d\n", width, height);
 	rgval  = (width << ISPRSZ_OUT_SIZE_HORZ_SHIFT)
 		 & ISPRSZ_OUT_SIZE_HORZ_MASK;
 	rgval |= (height << ISPRSZ_OUT_SIZE_VERT_SHIFT)
@@ -409,7 +398,7 @@ static void resizer_set_output_offset(struct isp_res_device *res, u32 offset)
 static void resizer_set_start(struct isp_res_device *res, u32 left, u32 top)
 {
 	struct isp_device *isp = to_isp_device(res);
-	u32 rgval = 0;
+	u32 rgval;
 
 	rgval = (left << ISPRSZ_IN_START_HORZ_ST_SHIFT)
 		& ISPRSZ_IN_START_HORZ_ST_MASK;
@@ -429,9 +418,7 @@ static void resizer_set_input_size(struct isp_res_device *res,
 				   u32 width, u32 height)
 {
 	struct isp_device *isp = to_isp_device(res);
-	u32 rgval = 0;
-
-	dev_dbg(isp->dev, "Input size[w/h]: %dx%d\n", width, height);
+	u32 rgval;
 
 	rgval = (width << ISPRSZ_IN_SIZE_HORZ_SHIFT)
 		& ISPRSZ_IN_SIZE_HORZ_MASK;
@@ -1075,10 +1062,13 @@ static void resizer_isr_buffer(struct isp_res_device *res)
 void omap3isp_resizer_isr(struct isp_res_device *res)
 {
 	struct v4l2_mbus_framefmt *informat, *outformat;
+	unsigned long flags;
 
 	if (omap3isp_module_sync_is_stopping(&res->wait, &res->stopping))
 		return;
 
+	spin_lock_irqsave(&res->lock, flags);
+
 	if (res->applycrop) {
 		outformat = __resizer_get_format(res, NULL, RESZ_PAD_SOURCE,
 					      V4L2_SUBDEV_FORMAT_ACTIVE);
@@ -1088,6 +1078,8 @@ void omap3isp_resizer_isr(struct isp_res_device *res)
 		res->applycrop = 0;
 	}
 
+	spin_unlock_irqrestore(&res->lock, flags);
+
 	resizer_isr_buffer(res);
 }
 
@@ -1290,8 +1282,10 @@ static int resizer_set_selection(struct v4l2_subdev *sd,
 {
 	struct isp_res_device *res = v4l2_get_subdevdata(sd);
 	struct isp_device *isp = to_isp_device(res);
-	struct v4l2_mbus_framefmt *format_sink, *format_source;
+	const struct v4l2_mbus_framefmt *format_sink;
+	struct v4l2_mbus_framefmt format_source;
 	struct resizer_ratio ratio;
+	unsigned long flags;
 
 	if (sel->target != V4L2_SEL_TGT_CROP ||
 	    sel->pad != RESZ_PAD_SINK)
@@ -1299,16 +1293,14 @@ static int resizer_set_selection(struct v4l2_subdev *sd,
 
 	format_sink = __resizer_get_format(res, fh, RESZ_PAD_SINK,
 					   sel->which);
-	format_source = __resizer_get_format(res, fh, RESZ_PAD_SOURCE,
-					     sel->which);
-
-	dev_dbg(isp->dev, "%s: L=%d,T=%d,W=%d,H=%d,which=%d\n", __func__,
-		sel->r.left, sel->r.top, sel->r.width, sel->r.height,
-		sel->which);
+	format_source = *__resizer_get_format(res, fh, RESZ_PAD_SOURCE,
+					      sel->which);
 
-	dev_dbg(isp->dev, "%s: input=%dx%d, output=%dx%d\n", __func__,
+	dev_dbg(isp->dev, "%s(%s): req %ux%u -> (%d,%d)/%ux%u -> %ux%u\n",
+		__func__, sel->which == V4L2_SUBDEV_FORMAT_TRY ? "try" : "act",
 		format_sink->width, format_sink->height,
-		format_source->width, format_source->height);
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height,
+		format_source.width, format_source.height);
 
 	/* Clamp the crop rectangle to the bounds, and then mangle it further to
 	 * fulfill the TRM equations. Store the clamped but otherwise unmangled
@@ -1318,23 +1310,39 @@ static int resizer_set_selection(struct v4l2_subdev *sd,
 	 * smaller input crop rectangle every time the output size is set if we
 	 * stored the mangled rectangle.
 	 */
-	resizer_try_crop(format_sink, format_source, &sel->r);
+	resizer_try_crop(format_sink, &format_source, &sel->r);
 	*__resizer_get_crop(res, fh, sel->which) = sel->r;
-	resizer_calc_ratios(res, &sel->r, format_source, &ratio);
+	resizer_calc_ratios(res, &sel->r, &format_source, &ratio);
 
-	if (sel->which == V4L2_SUBDEV_FORMAT_TRY)
+	dev_dbg(isp->dev, "%s(%s): got %ux%u -> (%d,%d)/%ux%u -> %ux%u\n",
+		__func__, sel->which == V4L2_SUBDEV_FORMAT_TRY ? "try" : "act",
+		format_sink->width, format_sink->height,
+		sel->r.left, sel->r.top, sel->r.width, sel->r.height,
+		format_source.width, format_source.height);
+
+	if (sel->which == V4L2_SUBDEV_FORMAT_TRY) {
+		*__resizer_get_format(res, fh, RESZ_PAD_SOURCE, sel->which) =
+			format_source;
 		return 0;
+	}
+
+	/* Update the source format, resizing ratios and crop rectangle. If
+	 * streaming is on the IRQ handler will reprogram the resizer after the
+	 * current frame. We thus we need to protect against race conditions.
+	 */
+	spin_lock_irqsave(&res->lock, flags);
+
+	*__resizer_get_format(res, fh, RESZ_PAD_SOURCE, sel->which) =
+		format_source;
 
 	res->ratio = ratio;
 	res->crop.active = sel->r;
 
-	/*
-	 * set_selection can be called while streaming is on. In this case the
-	 * crop values will be set in the next IRQ.
-	 */
 	if (res->state != ISP_PIPELINE_STREAM_STOPPED)
 		res->applycrop = 1;
 
+	spin_unlock_irqrestore(&res->lock, flags);
+
 	return 0;
 }
 
@@ -1781,6 +1789,8 @@ int omap3isp_resizer_init(struct isp_device *isp)
 
 	init_waitqueue_head(&res->wait);
 	atomic_set(&res->stopping, 0);
+	spin_lock_init(&res->lock);
+
 	return resizer_init_entities(res);
 }
 
diff --git a/drivers/media/platform/omap3isp/ispresizer.h b/drivers/media/platform/omap3isp/ispresizer.h
index 9b01e9047c15..5414542912e2 100644
--- a/drivers/media/platform/omap3isp/ispresizer.h
+++ b/drivers/media/platform/omap3isp/ispresizer.h
@@ -12,21 +12,12 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_RESIZER_H
 #define OMAP3_ISP_RESIZER_H
 
+#include <linux/spinlock.h>
 #include <linux/types.h>
 
 /*
@@ -96,6 +87,7 @@ enum resizer_input_entity {
 
 /*
  * struct isp_res_device - OMAP3 ISP resizer module
+ * @lock: Protects formats and crop rectangles between set_selection and IRQ
  * @crop.request: Crop rectangle requested by the user
  * @crop.active: Active crop rectangle (based on hardware requirements)
  */
@@ -116,6 +108,7 @@ struct isp_res_device {
 	enum isp_pipeline_stream_state state;
 	wait_queue_head_t wait;
 	atomic_t stopping;
+	spinlock_t lock;
 
 	struct {
 		struct v4l2_rect request;
diff --git a/drivers/media/platform/omap3isp/ispstat.c b/drivers/media/platform/omap3isp/ispstat.c
index e6cbc1eaf4ca..a94e8340508f 100644
--- a/drivers/media/platform/omap3isp/ispstat.c
+++ b/drivers/media/platform/omap3isp/ispstat.c
@@ -13,16 +13,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <linux/dma-mapping.h>
diff --git a/drivers/media/platform/omap3isp/ispstat.h b/drivers/media/platform/omap3isp/ispstat.h
index 58d6ac7cb664..b32b29677e2c 100644
--- a/drivers/media/platform/omap3isp/ispstat.h
+++ b/drivers/media/platform/omap3isp/ispstat.h
@@ -13,16 +13,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_STAT_H
diff --git a/drivers/media/platform/omap3isp/ispvideo.c b/drivers/media/platform/omap3isp/ispvideo.c
index e36bac26476c..bc38c88c7bd9 100644
--- a/drivers/media/platform/omap3isp/ispvideo.c
+++ b/drivers/media/platform/omap3isp/ispvideo.c
@@ -11,16 +11,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #include <asm/cacheflush.h>
@@ -319,10 +309,11 @@ isp_video_check_format(struct isp_video *video, struct isp_video_fh *vfh)
 	    vfh->format.fmt.pix.height != format.fmt.pix.height ||
 	    vfh->format.fmt.pix.width != format.fmt.pix.width ||
 	    vfh->format.fmt.pix.bytesperline != format.fmt.pix.bytesperline ||
-	    vfh->format.fmt.pix.sizeimage != format.fmt.pix.sizeimage)
+	    vfh->format.fmt.pix.sizeimage != format.fmt.pix.sizeimage ||
+	    vfh->format.fmt.pix.field != format.fmt.pix.field)
 		return -EINVAL;
 
-	return ret;
+	return 0;
 }
 
 /* -----------------------------------------------------------------------------
@@ -491,6 +482,11 @@ struct isp_buffer *omap3isp_video_buffer_next(struct isp_video *video)
 	else
 		buf->vb.v4l2_buf.sequence = atomic_read(&pipe->frame_number);
 
+	if (pipe->field != V4L2_FIELD_NONE)
+		buf->vb.v4l2_buf.sequence /= 2;
+
+	buf->vb.v4l2_buf.field = pipe->field;
+
 	/* Report pipeline errors to userspace on the capture device side. */
 	if (video->type == V4L2_BUF_TYPE_VIDEO_CAPTURE && pipe->error) {
 		state = VB2_BUF_STATE_ERROR;
@@ -641,7 +637,40 @@ isp_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
 	if (format->type != video->type)
 		return -EINVAL;
 
-	mutex_lock(&video->mutex);
+	/* Replace unsupported field orders with sane defaults. */
+	switch (format->fmt.pix.field) {
+	case V4L2_FIELD_NONE:
+		/* Progressive is supported everywhere. */
+		break;
+	case V4L2_FIELD_ALTERNATE:
+		/* ALTERNATE is not supported on output nodes. */
+		if (video->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+			format->fmt.pix.field = V4L2_FIELD_NONE;
+		break;
+	case V4L2_FIELD_INTERLACED:
+		/* The ISP has no concept of video standard, select the
+		 * top-bottom order when the unqualified interlaced order is
+		 * requested.
+		 */
+		format->fmt.pix.field = V4L2_FIELD_INTERLACED_TB;
+		/* Fall-through */
+	case V4L2_FIELD_INTERLACED_TB:
+	case V4L2_FIELD_INTERLACED_BT:
+		/* Interlaced orders are only supported at the CCDC output. */
+		if (video != &video->isp->isp_ccdc.video_out)
+			format->fmt.pix.field = V4L2_FIELD_NONE;
+		break;
+	case V4L2_FIELD_TOP:
+	case V4L2_FIELD_BOTTOM:
+	case V4L2_FIELD_SEQ_TB:
+	case V4L2_FIELD_SEQ_BT:
+	default:
+		/* All other field orders are currently unsupported, default to
+		 * progressive.
+		 */
+		format->fmt.pix.field = V4L2_FIELD_NONE;
+		break;
+	}
 
 	/* Fill the bytesperline and sizeimage fields by converting to media bus
 	 * format and back to pixel format.
@@ -649,9 +678,10 @@ isp_video_set_format(struct file *file, void *fh, struct v4l2_format *format)
 	isp_video_pix_to_mbus(&format->fmt.pix, &fmt);
 	isp_video_mbus_to_pix(video, &fmt, &format->fmt.pix);
 
+	mutex_lock(&video->mutex);
 	vfh->format = *format;
-
 	mutex_unlock(&video->mutex);
+
 	return 0;
 }
 
@@ -1039,6 +1069,7 @@ isp_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	video->queue = &vfh->queue;
 	INIT_LIST_HEAD(&video->dmaqueue);
 	atomic_set(&pipe->frame_number, -1);
+	pipe->field = vfh->format.fmt.pix.field;
 
 	mutex_lock(&video->queue_lock);
 	ret = vb2_streamon(&vfh->queue, type);
diff --git a/drivers/media/platform/omap3isp/ispvideo.h b/drivers/media/platform/omap3isp/ispvideo.h
index 7d2e82122ecd..0b7efedc3da9 100644
--- a/drivers/media/platform/omap3isp/ispvideo.h
+++ b/drivers/media/platform/omap3isp/ispvideo.h
@@ -11,16 +11,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 #ifndef OMAP3_ISP_VIDEO_H
@@ -88,6 +78,7 @@ enum isp_pipeline_state {
 
 /*
  * struct isp_pipeline - An ISP hardware pipeline
+ * @field: The field being processed by the pipeline
  * @error: A hardware error occurred during capture
  * @entities: Bitmask of entities in the pipeline (indexed by entity ID)
  */
@@ -101,6 +92,7 @@ struct isp_pipeline {
 	u32 entities;
 	unsigned long l3_ick;
 	unsigned int max_rate;
+	enum v4l2_field field;
 	atomic_t frame_number;
 	bool do_propagation; /* of frame number */
 	bool error;
diff --git a/drivers/media/platform/omap3isp/luma_enhance_table.h b/drivers/media/platform/omap3isp/luma_enhance_table.h
index 098b45e2280f..81c5b1566469 100644
--- a/drivers/media/platform/omap3isp/luma_enhance_table.h
+++ b/drivers/media/platform/omap3isp/luma_enhance_table.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552, 1047552,
diff --git a/drivers/media/platform/omap3isp/noise_filter_table.h b/drivers/media/platform/omap3isp/noise_filter_table.h
index d50451a4a242..5073f9847937 100644
--- a/drivers/media/platform/omap3isp/noise_filter_table.h
+++ b/drivers/media/platform/omap3isp/noise_filter_table.h
@@ -12,16 +12,6 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
  */
 
 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16, 16,
diff --git a/drivers/media/platform/s3c-camif/camif-capture.c b/drivers/media/platform/s3c-camif/camif-capture.c
index f33641384e15..4f81b4c9d113 100644
--- a/drivers/media/platform/s3c-camif/camif-capture.c
+++ b/drivers/media/platform/s3c-camif/camif-capture.c
@@ -280,8 +280,8 @@ static int camif_prepare_addr(struct camif_vp *vp, struct vb2_buffer *vb,
 		return -EINVAL;
 	}
 
-	pr_debug("DMA address: y: %#x  cb: %#x cr: %#x\n",
-		 paddr->y, paddr->cb, paddr->cr);
+	pr_debug("DMA address: y: %pad  cb: %pad cr: %pad\n",
+		 &paddr->y, &paddr->cb, &paddr->cr);
 
 	return 0;
 }
diff --git a/drivers/media/platform/s3c-camif/camif-regs.c b/drivers/media/platform/s3c-camif/camif-regs.c
index ebf5b184cce4..6e0c9988a191 100644
--- a/drivers/media/platform/s3c-camif/camif-regs.c
+++ b/drivers/media/platform/s3c-camif/camif-regs.c
@@ -214,8 +214,8 @@ void camif_hw_set_output_addr(struct camif_vp *vp,
 								paddr->cr);
 	}
 
-	pr_debug("dst_buf[%d]: %#X, cb: %#X, cr: %#X\n",
-		 i, paddr->y, paddr->cb, paddr->cr);
+	pr_debug("dst_buf[%d]: %pad, cb: %pad, cr: %pad\n",
+		 i, &paddr->y, &paddr->cb, &paddr->cr);
 }
 
 static void camif_hw_set_out_dma_size(struct camif_vp *vp)
diff --git a/drivers/media/platform/s5p-g2d/g2d.c b/drivers/media/platform/s5p-g2d/g2d.c
index 357af1ebaeda..d79e214ce8ce 100644
--- a/drivers/media/platform/s5p-g2d/g2d.c
+++ b/drivers/media/platform/s5p-g2d/g2d.c
@@ -490,14 +490,13 @@ static void job_abort(void *prv)
 {
 	struct g2d_ctx *ctx = prv;
 	struct g2d_dev *dev = ctx->dev;
-	int ret;
 
 	if (dev->curr == NULL) /* No job currently running */
 		return;
 
-	ret = wait_event_timeout(dev->irq_queue,
-		dev->curr == NULL,
-		msecs_to_jiffies(G2D_TIMEOUT));
+	wait_event_timeout(dev->irq_queue,
+			   dev->curr == NULL,
+			   msecs_to_jiffies(G2D_TIMEOUT));
 }
 
 static void device_run(void *prv)
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-core.c b/drivers/media/platform/s5p-jpeg/jpeg-core.c
index e66acbc2a82d..e525a7c8d885 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-core.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-core.c
@@ -729,7 +729,7 @@ static inline void exynos4_jpeg_set_qtbl_chr(void __iomem *regs, int quality)
 			     ARRAY_SIZE(qtbl_chrominance[quality]));
 }
 
-void exynos4_jpeg_set_huff_tbl(void __iomem *base)
+static void exynos4_jpeg_set_huff_tbl(void __iomem *base)
 {
 	exynos4_jpeg_set_tbl(base, hdctbl0, EXYNOS4_HUFF_TBL_HDCLL,
 							ARRAY_SIZE(hdctbl0));
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
index d26e1f846553..e8c2cad93962 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos3250.c
@@ -233,6 +233,7 @@ void exynos3250_jpeg_set_x(void __iomem *regs, unsigned int x)
 	writel(reg, regs + EXYNOS3250_JPGX);
 }
 
+#if 0	/* Currently unused */
 unsigned int exynos3250_jpeg_get_y(void __iomem *regs)
 {
 	return readl(regs + EXYNOS3250_JPGY);
@@ -242,6 +243,7 @@ unsigned int exynos3250_jpeg_get_x(void __iomem *regs)
 {
 	return readl(regs + EXYNOS3250_JPGX);
 }
+#endif
 
 void exynos3250_jpeg_interrupts_enable(void __iomem *regs)
 {
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c
index da8d6a1a984f..ab6d6f43c96f 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-exynos4.c
@@ -23,7 +23,7 @@ void exynos4_jpeg_sw_reset(void __iomem *base)
 	reg = readl(base + EXYNOS4_JPEG_CNTL_REG);
 	writel(reg & ~EXYNOS4_SOFT_RESET_HI, base + EXYNOS4_JPEG_CNTL_REG);
 
-	ndelay(100000);
+	udelay(100);
 
 	writel(reg | EXYNOS4_SOFT_RESET_HI, base + EXYNOS4_JPEG_CNTL_REG);
 }
@@ -151,9 +151,6 @@ void exynos4_jpeg_set_enc_out_fmt(void __iomem *base, unsigned int out_fmt)
 
 void exynos4_jpeg_set_interrupt(void __iomem *base)
 {
-	unsigned int reg;
-
-	reg = readl(base + EXYNOS4_INT_EN_REG) & ~EXYNOS4_INT_EN_MASK;
 	writel(EXYNOS4_INT_EN_ALL, base + EXYNOS4_INT_EN_REG);
 }
 
@@ -185,7 +182,7 @@ void exynos4_jpeg_set_huf_table_enable(void __iomem *base, int value)
 		writel(reg | EXYNOS4_HUF_TBL_EN,
 					base + EXYNOS4_JPEG_CNTL_REG);
 	else
-		writel(reg | ~EXYNOS4_HUF_TBL_EN,
+		writel(reg & ~EXYNOS4_HUF_TBL_EN,
 					base + EXYNOS4_JPEG_CNTL_REG);
 }
 
@@ -196,9 +193,9 @@ void exynos4_jpeg_set_sys_int_enable(void __iomem *base, int value)
 	reg = readl(base + EXYNOS4_JPEG_CNTL_REG) & ~(EXYNOS4_SYS_INT_EN);
 
 	if (value == 1)
-		writel(EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
+		writel(reg | EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
 	else
-		writel(~EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
+		writel(reg & ~EXYNOS4_SYS_INT_EN, base + EXYNOS4_JPEG_CNTL_REG);
 }
 
 void exynos4_jpeg_set_stream_buf_address(void __iomem *base,
diff --git a/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c
index 52407d790726..e3b8e67e005f 100644
--- a/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c
+++ b/drivers/media/platform/s5p-jpeg/jpeg-hw-s5p.c
@@ -324,11 +324,9 @@ int s5p_jpeg_stream_stat_ok(void __iomem *regs)
 
 void s5p_jpeg_clear_int(void __iomem *regs)
 {
-	unsigned long reg;
-
-	reg = readl(regs + S5P_JPGINTST);
+	readl(regs + S5P_JPGINTST);
 	writel(S5P_INT_RELEASE, regs + S5P_JPGCOM);
-	reg = readl(regs + S5P_JPGOPR);
+	readl(regs + S5P_JPGOPR);
 }
 
 unsigned int s5p_jpeg_compressed_size(void __iomem *regs)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc.c b/drivers/media/platform/s5p-mfc/s5p_mfc.c
index d35b0418ab37..165bc86c5962 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc.c
@@ -37,8 +37,8 @@
 #define S5P_MFC_DEC_NAME	"s5p-mfc-dec"
 #define S5P_MFC_ENC_NAME	"s5p-mfc-enc"
 
-int debug;
-module_param(debug, int, S_IRUGO | S_IWUSR);
+int mfc_debug_level;
+module_param_named(debug, mfc_debug_level, int, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Debug level - higher value produces more verbose messages");
 
 /* Helper functions for interrupt processing */
@@ -150,10 +150,10 @@ static void s5p_mfc_watchdog_worker(struct work_struct *work)
 		if (!ctx)
 			continue;
 		ctx->state = MFCINST_ERROR;
-		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
-				&ctx->vq_dst);
-		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
-				&ctx->vq_src);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->dst_queue, &ctx->vq_dst);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->src_queue, &ctx->vq_src);
 		clear_work_bit(ctx);
 		wake_up_ctx(ctx, S5P_MFC_R2H_CMD_ERR_RET, 0);
 	}
@@ -264,7 +264,12 @@ static void s5p_mfc_handle_frame_new(struct s5p_mfc_ctx *ctx, unsigned int err)
 	unsigned int frame_type;
 
 	dspl_y_addr = s5p_mfc_hw_call(dev->mfc_ops, get_dspl_y_adr, dev);
-	frame_type = s5p_mfc_hw_call(dev->mfc_ops, get_disp_frame_type, ctx);
+	if (IS_MFCV6_PLUS(dev))
+		frame_type = s5p_mfc_hw_call(dev->mfc_ops,
+			get_disp_frame_type, ctx);
+	else
+		frame_type = s5p_mfc_hw_call(dev->mfc_ops,
+			get_dec_frame_type, dev);
 
 	/* If frame is same as previous then skip and do not dequeue */
 	if (frame_type == S5P_FIMV_DECODE_FRAME_SKIPPED) {
@@ -327,12 +332,12 @@ static void s5p_mfc_handle_frame(struct s5p_mfc_ctx *ctx,
 	if (res_change == S5P_FIMV_RES_INCREASE ||
 		res_change == S5P_FIMV_RES_DECREASE) {
 		ctx->state = MFCINST_RES_CHANGE_INIT;
-		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 		wake_up_ctx(ctx, reason, err);
 		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 			BUG();
 		s5p_mfc_clock_off();
-		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 		return;
 	}
 	if (ctx->dpb_flush_flag)
@@ -400,7 +405,7 @@ leave_handle_frame:
 	if ((ctx->src_queue_cnt == 0 && ctx->state != MFCINST_FINISHING)
 				    || ctx->dst_queue_cnt < ctx->pb_count)
 		clear_work_bit(ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 	wake_up_ctx(ctx, reason, err);
 	if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 		BUG();
@@ -409,7 +414,7 @@ leave_handle_frame:
 	if (test_bit(0, &dev->enter_suspend))
 		wake_up_dev(dev, reason, err);
 	else
-		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 }
 
 /* Error handling for interrupt */
@@ -435,10 +440,10 @@ static void s5p_mfc_handle_error(struct s5p_mfc_dev *dev,
 			ctx->state = MFCINST_ERROR;
 			/* Mark all dst buffers as having an error */
 			spin_lock_irqsave(&dev->irqlock, flags);
-			s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue,
+			s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
 						&ctx->dst_queue, &ctx->vq_dst);
 			/* Mark all src buffers as having an error */
-			s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue,
+			s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
 						&ctx->src_queue, &ctx->vq_src);
 			spin_unlock_irqrestore(&dev->irqlock, flags);
 			wake_up_ctx(ctx, reason, err);
@@ -452,7 +457,7 @@ static void s5p_mfc_handle_error(struct s5p_mfc_dev *dev,
 	}
 	if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 		BUG();
-	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 	s5p_mfc_clock_off();
 	wake_up_dev(dev, reason, err);
 	return;
@@ -476,7 +481,7 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 		ctx->img_height = s5p_mfc_hw_call(dev->mfc_ops, get_img_height,
 				dev);
 
-		s5p_mfc_hw_call(dev->mfc_ops, dec_calc_dpb_size, ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, dec_calc_dpb_size, ctx);
 
 		ctx->pb_count = s5p_mfc_hw_call(dev->mfc_ops, get_dpb_count,
 				dev);
@@ -503,12 +508,12 @@ static void s5p_mfc_handle_seq_done(struct s5p_mfc_ctx *ctx,
 			ctx->head_processed = 1;
 		}
 	}
-	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 	clear_work_bit(ctx);
 	if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 		BUG();
 	s5p_mfc_clock_off();
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 	wake_up_ctx(ctx, reason, err);
 }
 
@@ -523,7 +528,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 	if (ctx == NULL)
 		return;
 	dev = ctx->dev;
-	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 	ctx->int_type = reason;
 	ctx->int_err = err;
 	ctx->int_cond = 1;
@@ -550,7 +555,7 @@ static void s5p_mfc_handle_init_buffers(struct s5p_mfc_ctx *ctx,
 		s5p_mfc_clock_off();
 
 		wake_up(&ctx->queue);
-		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 	} else {
 		if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 			BUG();
@@ -591,7 +596,7 @@ static void s5p_mfc_handle_stream_complete(struct s5p_mfc_ctx *ctx,
 
 	s5p_mfc_clock_off();
 	wake_up(&ctx->queue);
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 }
 
 /* Interrupt processing */
@@ -628,12 +633,12 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 		if (ctx->c_ops->post_frame_start) {
 			if (ctx->c_ops->post_frame_start(ctx))
 				mfc_err("post_frame_start() failed\n");
-			s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+			s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 			wake_up_ctx(ctx, reason, err);
 			if (test_and_clear_bit(0, &dev->hw_lock) == 0)
 				BUG();
 			s5p_mfc_clock_off();
-			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 		} else {
 			s5p_mfc_handle_frame(ctx, reason, err);
 		}
@@ -663,7 +668,7 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 	case S5P_MFC_R2H_CMD_WAKEUP_RET:
 		if (ctx)
 			clear_work_bit(ctx);
-		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 		wake_up_dev(dev, reason, err);
 		clear_bit(0, &dev->hw_lock);
 		clear_bit(0, &dev->enter_suspend);
@@ -685,12 +690,12 @@ static irqreturn_t s5p_mfc_irq(int irq, void *priv)
 
 	default:
 		mfc_debug(2, "Unknown int reason\n");
-		s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 	}
 	mfc_debug_leave();
 	return IRQ_HANDLED;
 irq_cleanup_hw:
-	s5p_mfc_hw_call(dev->mfc_ops, clear_int_flags, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, clear_int_flags, dev);
 	ctx->int_type = reason;
 	ctx->int_err = err;
 	ctx->int_cond = 1;
@@ -699,7 +704,7 @@ irq_cleanup_hw:
 
 	s5p_mfc_clock_off();
 
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 	mfc_debug(2, "Exit via irq_cleanup_hw\n");
 	return IRQ_HANDLED;
 }
@@ -1311,11 +1316,9 @@ static int s5p_mfc_runtime_resume(struct device *dev)
 {
 	struct platform_device *pdev = to_platform_device(dev);
 	struct s5p_mfc_dev *m_dev = platform_get_drvdata(pdev);
-	int pre_power;
 
 	if (!m_dev->alloc_ctx)
 		return 0;
-	pre_power = atomic_read(&m_dev->pm.power);
 	atomic_set(&m_dev->pm.power, 1);
 	return 0;
 }
@@ -1328,20 +1331,20 @@ static const struct dev_pm_ops s5p_mfc_pm_ops = {
 			   NULL)
 };
 
-struct s5p_mfc_buf_size_v5 mfc_buf_size_v5 = {
+static struct s5p_mfc_buf_size_v5 mfc_buf_size_v5 = {
 	.h264_ctx	= MFC_H264_CTX_BUF_SIZE,
 	.non_h264_ctx	= MFC_CTX_BUF_SIZE,
 	.dsc		= DESC_BUF_SIZE,
 	.shm		= SHARED_BUF_SIZE,
 };
 
-struct s5p_mfc_buf_size buf_size_v5 = {
+static struct s5p_mfc_buf_size buf_size_v5 = {
 	.fw	= MAX_FW_SIZE,
 	.cpb	= MAX_CPB_SIZE,
 	.priv	= &mfc_buf_size_v5,
 };
 
-struct s5p_mfc_buf_align mfc_buf_align_v5 = {
+static struct s5p_mfc_buf_align mfc_buf_align_v5 = {
 	.base = MFC_BASE_ALIGN_ORDER,
 };
 
@@ -1354,7 +1357,7 @@ static struct s5p_mfc_variant mfc_drvdata_v5 = {
 	.fw_name[0]	= "s5p-mfc.fw",
 };
 
-struct s5p_mfc_buf_size_v6 mfc_buf_size_v6 = {
+static struct s5p_mfc_buf_size_v6 mfc_buf_size_v6 = {
 	.dev_ctx	= MFC_CTX_BUF_SIZE_V6,
 	.h264_dec_ctx	= MFC_H264_DEC_CTX_BUF_SIZE_V6,
 	.other_dec_ctx	= MFC_OTHER_DEC_CTX_BUF_SIZE_V6,
@@ -1362,13 +1365,13 @@ struct s5p_mfc_buf_size_v6 mfc_buf_size_v6 = {
 	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V6,
 };
 
-struct s5p_mfc_buf_size buf_size_v6 = {
+static struct s5p_mfc_buf_size buf_size_v6 = {
 	.fw	= MAX_FW_SIZE_V6,
 	.cpb	= MAX_CPB_SIZE_V6,
 	.priv	= &mfc_buf_size_v6,
 };
 
-struct s5p_mfc_buf_align mfc_buf_align_v6 = {
+static struct s5p_mfc_buf_align mfc_buf_align_v6 = {
 	.base = 0,
 };
 
@@ -1386,7 +1389,7 @@ static struct s5p_mfc_variant mfc_drvdata_v6 = {
 	.fw_name[1]     = "s5p-mfc-v6-v2.fw",
 };
 
-struct s5p_mfc_buf_size_v6 mfc_buf_size_v7 = {
+static struct s5p_mfc_buf_size_v6 mfc_buf_size_v7 = {
 	.dev_ctx	= MFC_CTX_BUF_SIZE_V7,
 	.h264_dec_ctx	= MFC_H264_DEC_CTX_BUF_SIZE_V7,
 	.other_dec_ctx	= MFC_OTHER_DEC_CTX_BUF_SIZE_V7,
@@ -1394,13 +1397,13 @@ struct s5p_mfc_buf_size_v6 mfc_buf_size_v7 = {
 	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V7,
 };
 
-struct s5p_mfc_buf_size buf_size_v7 = {
+static struct s5p_mfc_buf_size buf_size_v7 = {
 	.fw	= MAX_FW_SIZE_V7,
 	.cpb	= MAX_CPB_SIZE_V7,
 	.priv	= &mfc_buf_size_v7,
 };
 
-struct s5p_mfc_buf_align mfc_buf_align_v7 = {
+static struct s5p_mfc_buf_align mfc_buf_align_v7 = {
 	.base = 0,
 };
 
@@ -1413,7 +1416,7 @@ static struct s5p_mfc_variant mfc_drvdata_v7 = {
 	.fw_name[0]     = "s5p-mfc-v7.fw",
 };
 
-struct s5p_mfc_buf_size_v6 mfc_buf_size_v8 = {
+static struct s5p_mfc_buf_size_v6 mfc_buf_size_v8 = {
 	.dev_ctx	= MFC_CTX_BUF_SIZE_V8,
 	.h264_dec_ctx	= MFC_H264_DEC_CTX_BUF_SIZE_V8,
 	.other_dec_ctx	= MFC_OTHER_DEC_CTX_BUF_SIZE_V8,
@@ -1421,13 +1424,13 @@ struct s5p_mfc_buf_size_v6 mfc_buf_size_v8 = {
 	.other_enc_ctx	= MFC_OTHER_ENC_CTX_BUF_SIZE_V8,
 };
 
-struct s5p_mfc_buf_size buf_size_v8 = {
+static struct s5p_mfc_buf_size buf_size_v8 = {
 	.fw	= MAX_FW_SIZE_V8,
 	.cpb	= MAX_CPB_SIZE_V8,
 	.priv	= &mfc_buf_size_v8,
 };
 
-struct s5p_mfc_buf_align mfc_buf_align_v8 = {
+static struct s5p_mfc_buf_align mfc_buf_align_v8 = {
 	.base = 0,
 };
 
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
index 9a6efd6c1329..8c4739ca16d6 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v5.c
@@ -14,6 +14,7 @@
 #include "s5p_mfc_cmd.h"
 #include "s5p_mfc_common.h"
 #include "s5p_mfc_debug.h"
+#include "s5p_mfc_cmd_v5.h"
 
 /* This function is used to send a command to the MFC */
 static int s5p_mfc_cmd_host2risc_v5(struct s5p_mfc_dev *dev, int cmd,
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
index ec1a5947ed7d..f17609669b96 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_cmd_v6.c
@@ -16,6 +16,7 @@
 #include "s5p_mfc_debug.h"
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
+#include "s5p_mfc_cmd_v6.h"
 
 static int s5p_mfc_cmd_host2risc_v6(struct s5p_mfc_dev *dev, int cmd,
 				struct s5p_mfc_cmd_args *args)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
index 01816ffb384b..3e41ca1293ed 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_common.h
@@ -698,6 +698,12 @@ struct mfc_control {
 #define s5p_mfc_hw_call(f, op, args...) \
 	((f && f->op) ? f->op(args) : -ENODEV)
 
+#define s5p_mfc_hw_call_void(f, op, args...) \
+do { \
+	if (f && f->op) \
+		f->op(args); \
+} while (0)
+
 #define fh_to_ctx(__fh) container_of(__fh, struct s5p_mfc_ctx, fh)
 #define ctrl_to_ctx(__ctrl) \
 	container_of((__ctrl)->handler, struct s5p_mfc_ctx, ctrl_handler)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
index ca9f78922832..0c885a8a0e9f 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_ctrl.c
@@ -21,6 +21,7 @@
 #include "s5p_mfc_intr.h"
 #include "s5p_mfc_opr.h"
 #include "s5p_mfc_pm.h"
+#include "s5p_mfc_ctrl.h"
 
 /* Allocate memory for firmware */
 int s5p_mfc_alloc_firmware(struct s5p_mfc_dev *dev)
@@ -188,12 +189,12 @@ static inline void s5p_mfc_init_memctrl(struct s5p_mfc_dev *dev)
 {
 	if (IS_MFCV6_PLUS(dev)) {
 		mfc_write(dev, dev->bank1, S5P_FIMV_RISC_BASE_ADDRESS_V6);
-		mfc_debug(2, "Base Address : %08x\n", dev->bank1);
+		mfc_debug(2, "Base Address : %pad\n", &dev->bank1);
 	} else {
 		mfc_write(dev, dev->bank1, S5P_FIMV_MC_DRAMBASE_ADR_A);
 		mfc_write(dev, dev->bank2, S5P_FIMV_MC_DRAMBASE_ADR_B);
-		mfc_debug(2, "Bank1: %08x, Bank2: %08x\n",
-				dev->bank1, dev->bank2);
+		mfc_debug(2, "Bank1: %pad, Bank2: %pad\n",
+				&dev->bank1, &dev->bank2);
 	}
 }
 
@@ -257,9 +258,9 @@ int s5p_mfc_init_hw(struct s5p_mfc_dev *dev)
 		s5p_mfc_clock_off();
 		return ret;
 	}
-	mfc_debug(2, "Ok, now will write a command to init the system\n");
+	mfc_debug(2, "Ok, now will wait for completion of hardware init\n");
 	if (s5p_mfc_wait_for_done_dev(dev, S5P_MFC_R2H_CMD_SYS_INIT_RET)) {
-		mfc_err("Failed to load firmware\n");
+		mfc_err("Failed to init hardware\n");
 		s5p_mfc_reset(dev);
 		s5p_mfc_clock_off();
 		return -EIO;
@@ -293,7 +294,7 @@ void s5p_mfc_deinit_hw(struct s5p_mfc_dev *dev)
 	s5p_mfc_clock_on();
 
 	s5p_mfc_reset(dev);
-	s5p_mfc_hw_call(dev->mfc_ops, release_dev_context_buffer, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_dev_context_buffer, dev);
 
 	s5p_mfc_clock_off();
 }
@@ -396,7 +397,7 @@ int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
 
 	set_work_bit_irqsave(ctx);
 	s5p_mfc_clean_ctx_int_flags(ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 	if (s5p_mfc_wait_for_done_ctx(ctx,
 		S5P_MFC_R2H_CMD_OPEN_INSTANCE_RET, 0)) {
 		/* Error or timeout */
@@ -410,9 +411,9 @@ int s5p_mfc_open_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
 
 err_free_desc_buf:
 	if (ctx->type == MFCINST_DECODER)
-		s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer, ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, release_dec_desc_buffer, ctx);
 err_free_inst_buf:
-	s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_instance_buffer, ctx);
 err:
 	return ret;
 }
@@ -422,17 +423,17 @@ void s5p_mfc_close_mfc_inst(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx)
 	ctx->state = MFCINST_RETURN_INST;
 	set_work_bit_irqsave(ctx);
 	s5p_mfc_clean_ctx_int_flags(ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 	/* Wait until instance is returned or timeout occurred */
 	if (s5p_mfc_wait_for_done_ctx(ctx,
 				S5P_MFC_R2H_CMD_CLOSE_INSTANCE_RET, 0))
 		mfc_err("Err returning instance\n");
 
 	/* Free resources */
-	s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers, ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, release_instance_buffer, ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers, ctx);
+	s5p_mfc_hw_call_void(dev->mfc_ops, release_instance_buffer, ctx);
 	if (ctx->type == MFCINST_DECODER)
-		s5p_mfc_hw_call(dev->mfc_ops, release_dec_desc_buffer, ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, release_dec_desc_buffer, ctx);
 
 	ctx->inst_no = MFC_NO_INSTANCE_SET;
 	ctx->state = MFCINST_FREE;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_debug.h b/drivers/media/platform/s5p-mfc/s5p_mfc_debug.h
index 8e608f5aa0d7..5936923c631c 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_debug.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_debug.h
@@ -1,5 +1,5 @@
 /*
- * drivers/media/platform/samsung/mfc5/s5p_mfc_debug.h
+ * drivers/media/platform/s5p-mfc/s5p_mfc_debug.h
  *
  * Header file for Samsung MFC (Multi Function Codec - FIMV) driver
  * This file contains debug macros
@@ -18,11 +18,11 @@
 #define DEBUG
 
 #ifdef DEBUG
-extern int debug;
+extern int mfc_debug_level;
 
 #define mfc_debug(level, fmt, args...)				\
 	do {							\
-		if (debug >= level)				\
+		if (mfc_debug_level >= level)			\
 			printk(KERN_DEBUG "%s:%d: " fmt,	\
 				__func__, __LINE__, ##args);	\
 	} while (0)
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
index 9103258b7df3..a98fe023deaf 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_dec.c
@@ -283,17 +283,13 @@ static int vidioc_querycap(struct file *file, void *priv,
 
 /* Enumerate format */
 static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
-							bool mplane, bool out)
+							bool out)
 {
 	struct s5p_mfc_dev *dev = video_drvdata(file);
 	struct s5p_mfc_fmt *fmt;
 	int i, j = 0;
 
 	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
-		if (mplane && formats[i].num_planes == 1)
-			continue;
-		else if (!mplane && formats[i].num_planes > 1)
-			continue;
 		if (out && formats[i].type != MFC_FMT_DEC)
 			continue;
 		else if (!out && formats[i].type != MFC_FMT_RAW)
@@ -313,28 +309,16 @@ static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
 	return 0;
 }
 
-static int vidioc_enum_fmt_vid_cap(struct file *file, void *pirv,
-							struct v4l2_fmtdesc *f)
-{
-	return vidioc_enum_fmt(file, f, false, false);
-}
-
 static int vidioc_enum_fmt_vid_cap_mplane(struct file *file, void *pirv,
 							struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(file, f, true, false);
-}
-
-static int vidioc_enum_fmt_vid_out(struct file *file, void *priv,
-							struct v4l2_fmtdesc *f)
-{
-	return vidioc_enum_fmt(file, f, false, true);
+	return vidioc_enum_fmt(file, f, false);
 }
 
 static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *priv,
 							struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(file, f, true, true);
+	return vidioc_enum_fmt(file, f, true);
 }
 
 /* Get format */
@@ -543,7 +527,7 @@ static int reqbufs_capture(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
 		ret = vb2_reqbufs(&ctx->vq_dst, reqbufs);
 		if (ret)
 			goto out;
-		s5p_mfc_hw_call(dev->mfc_ops, release_codec_buffers, ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, release_codec_buffers, ctx);
 		ctx->dst_bufs_cnt = 0;
 	} else if (ctx->capture_state == QUEUE_FREE) {
 		WARN_ON(ctx->dst_bufs_cnt != 0);
@@ -571,7 +555,7 @@ static int reqbufs_capture(struct s5p_mfc_dev *dev, struct s5p_mfc_ctx *ctx,
 
 		if (s5p_mfc_ctx_ready(ctx))
 			set_work_bit_irqsave(ctx);
-		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 		s5p_mfc_wait_for_done_ctx(ctx, S5P_MFC_R2H_CMD_INIT_BUFFERS_RET,
 					  0);
 	} else {
@@ -823,8 +807,8 @@ static int vidioc_g_crop(struct file *file, void *priv,
 	return 0;
 }
 
-int vidioc_decoder_cmd(struct file *file, void *priv,
-						struct v4l2_decoder_cmd *cmd)
+static int vidioc_decoder_cmd(struct file *file, void *priv,
+			      struct v4l2_decoder_cmd *cmd)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -846,7 +830,7 @@ int vidioc_decoder_cmd(struct file *file, void *priv,
 			if (s5p_mfc_ctx_ready(ctx))
 				set_work_bit_irqsave(ctx);
 			spin_unlock_irqrestore(&dev->irqlock, flags);
-			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 		} else {
 			mfc_err("EOS: marking last buffer of stream");
 			buf = list_entry(ctx->src_queue.prev,
@@ -881,9 +865,7 @@ static int vidioc_subscribe_event(struct v4l2_fh *fh,
 /* v4l2_ioctl_ops */
 static const struct v4l2_ioctl_ops s5p_mfc_dec_ioctl_ops = {
 	.vidioc_querycap = vidioc_querycap,
-	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap_mplane,
-	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 	.vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out_mplane,
 	.vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt,
 	.vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt,
@@ -990,7 +972,7 @@ static int s5p_mfc_buf_init(struct vb2_buffer *vb)
 	if (vq->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		if (ctx->capture_state == QUEUE_BUFS_MMAPED)
 			return 0;
-		for (i = 0; i <= ctx->src_fmt->num_planes ; i++) {
+		for (i = 0; i < ctx->dst_fmt->num_planes; i++) {
 			if (IS_ERR_OR_NULL(ERR_PTR(
 					vb2_dma_contig_plane_dma_addr(vb, i)))) {
 				mfc_err("Plane mem not allocated\n");
@@ -1044,7 +1026,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	/* If context is ready then dev = work->data;schedule it to run */
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 	return 0;
 }
 
@@ -1065,8 +1047,8 @@ static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 	}
 	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
 		spin_lock_irqsave(&dev->irqlock, flags);
-		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
-				&ctx->vq_dst);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->dst_queue, &ctx->vq_dst);
 		INIT_LIST_HEAD(&ctx->dst_queue);
 		ctx->dst_queue_cnt = 0;
 		ctx->dpb_flush_flag = 1;
@@ -1076,7 +1058,7 @@ static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 			ctx->state = MFCINST_FLUSH;
 			set_work_bit_irqsave(ctx);
 			s5p_mfc_clean_ctx_int_flags(ctx);
-			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 			if (s5p_mfc_wait_for_done_ctx(ctx,
 				S5P_MFC_R2H_CMD_DPB_FLUSH_RET, 0))
 				mfc_err("Err flushing buffers\n");
@@ -1084,8 +1066,8 @@ static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 	}
 	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		spin_lock_irqsave(&dev->irqlock, flags);
-		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
-				&ctx->vq_src);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->src_queue, &ctx->vq_src);
 		INIT_LIST_HEAD(&ctx->src_queue);
 		ctx->src_queue_cnt = 0;
 		spin_unlock_irqrestore(&dev->irqlock, flags);
@@ -1124,7 +1106,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 	}
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_dec_qops = {
@@ -1220,7 +1202,7 @@ void s5p_mfc_dec_init(struct s5p_mfc_ctx *ctx)
 	else
 		f.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12MT;
 	ctx->dst_fmt = find_format(&f, MFC_FMT_RAW);
-	mfc_debug(2, "Default src_fmt is %x, dest_fmt is %x\n",
-			(unsigned int)ctx->src_fmt, (unsigned int)ctx->dst_fmt);
+	mfc_debug(2, "Default src_fmt is %p, dest_fmt is %p\n",
+			ctx->src_fmt, ctx->dst_fmt);
 }
 
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
index d26b2484ca10..a904a1c7bb21 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_enc.c
@@ -739,14 +739,11 @@ static int s5p_mfc_ctx_ready(struct s5p_mfc_ctx *ctx)
 static void cleanup_ref_queue(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_buf *mb_entry;
-	unsigned long mb_y_addr, mb_c_addr;
 
 	/* move buffers in ref queue to src queue */
 	while (!list_empty(&ctx->ref_queue)) {
 		mb_entry = list_entry((&ctx->ref_queue)->next,
 						struct s5p_mfc_buf, list);
-		mb_y_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 0);
-		mb_c_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 1);
 		list_del(&mb_entry->list);
 		ctx->ref_queue_cnt--;
 		list_add_tail(&mb_entry->list, &ctx->src_queue);
@@ -770,7 +767,7 @@ static int enc_pre_seq_start(struct s5p_mfc_ctx *ctx)
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
 	dst_size = vb2_plane_size(dst_mb->b, 0);
-	s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
 			dst_size);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
 	return 0;
@@ -803,7 +800,7 @@ static int enc_post_seq_start(struct s5p_mfc_ctx *ctx)
 		ctx->state = MFCINST_RUNNING;
 		if (s5p_mfc_ctx_ready(ctx))
 			set_work_bit_irqsave(ctx);
-		s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+		s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 	} else {
 		enc_pb_count = s5p_mfc_hw_call(dev->mfc_ops,
 				get_enc_dpb_count, dev);
@@ -828,15 +825,15 @@ static int enc_pre_frame_start(struct s5p_mfc_ctx *ctx)
 	src_mb = list_entry(ctx->src_queue.next, struct s5p_mfc_buf, list);
 	src_y_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 0);
 	src_c_addr = vb2_dma_contig_plane_dma_addr(src_mb->b, 1);
-	s5p_mfc_hw_call(dev->mfc_ops, set_enc_frame_buffer, ctx, src_y_addr,
-			src_c_addr);
+	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_frame_buffer, ctx,
+							src_y_addr, src_c_addr);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
 
 	spin_lock_irqsave(&dev->irqlock, flags);
 	dst_mb = list_entry(ctx->dst_queue.next, struct s5p_mfc_buf, list);
 	dst_addr = vb2_dma_contig_plane_dma_addr(dst_mb->b, 0);
 	dst_size = vb2_plane_size(dst_mb->b, 0);
-	s5p_mfc_hw_call(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
+	s5p_mfc_hw_call_void(dev->mfc_ops, set_enc_stream_buffer, ctx, dst_addr,
 			dst_size);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
 
@@ -861,7 +858,7 @@ static int enc_post_frame_start(struct s5p_mfc_ctx *ctx)
 		  mfc_read(dev, S5P_FIMV_ENC_SI_PIC_CNT));
 	spin_lock_irqsave(&dev->irqlock, flags);
 	if (slice_type >= 0) {
-		s5p_mfc_hw_call(dev->mfc_ops, get_enc_frame_buffer, ctx,
+		s5p_mfc_hw_call_void(dev->mfc_ops, get_enc_frame_buffer, ctx,
 				&enc_y_addr, &enc_c_addr);
 		list_for_each_entry(mb_entry, &ctx->src_queue, list) {
 			mb_y_addr = vb2_dma_contig_plane_dma_addr(mb_entry->b, 0);
@@ -954,17 +951,13 @@ static int vidioc_querycap(struct file *file, void *priv,
 }
 
 static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
-							bool mplane, bool out)
+							bool out)
 {
 	struct s5p_mfc_dev *dev = video_drvdata(file);
 	struct s5p_mfc_fmt *fmt;
 	int i, j = 0;
 
 	for (i = 0; i < ARRAY_SIZE(formats); ++i) {
-		if (mplane && formats[i].num_planes == 1)
-			continue;
-		else if (!mplane && formats[i].num_planes > 1)
-			continue;
 		if (out && formats[i].type != MFC_FMT_RAW)
 			continue;
 		else if (!out && formats[i].type != MFC_FMT_ENC)
@@ -984,28 +977,16 @@ static int vidioc_enum_fmt(struct file *file, struct v4l2_fmtdesc *f,
 	return -EINVAL;
 }
 
-static int vidioc_enum_fmt_vid_cap(struct file *file, void *pirv,
-				   struct v4l2_fmtdesc *f)
-{
-	return vidioc_enum_fmt(file, f, false, false);
-}
-
 static int vidioc_enum_fmt_vid_cap_mplane(struct file *file, void *pirv,
 					  struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(file, f, true, false);
-}
-
-static int vidioc_enum_fmt_vid_out(struct file *file, void *prov,
-				   struct v4l2_fmtdesc *f)
-{
-	return vidioc_enum_fmt(file, f, false, true);
+	return vidioc_enum_fmt(file, f, false);
 }
 
 static int vidioc_enum_fmt_vid_out_mplane(struct file *file, void *prov,
 					  struct v4l2_fmtdesc *f)
 {
-	return vidioc_enum_fmt(file, f, true, true);
+	return vidioc_enum_fmt(file, f, true);
 }
 
 static int vidioc_g_fmt(struct file *file, void *priv, struct v4l2_format *f)
@@ -1127,7 +1108,7 @@ static int vidioc_s_fmt(struct file *file, void *priv, struct v4l2_format *f)
 			pix_fmt_mp->width, pix_fmt_mp->height,
 			ctx->img_width, ctx->img_height);
 
-		s5p_mfc_hw_call(dev->mfc_ops, enc_calc_src_size, ctx);
+		s5p_mfc_hw_call_void(dev->mfc_ops, enc_calc_src_size, ctx);
 		pix_fmt_mp->plane_fmt[0].sizeimage = ctx->luma_size;
 		pix_fmt_mp->plane_fmt[0].bytesperline = ctx->buf_width;
 		pix_fmt_mp->plane_fmt[1].sizeimage = ctx->chroma_size;
@@ -1681,8 +1662,8 @@ static int vidioc_g_parm(struct file *file, void *priv,
 	return 0;
 }
 
-int vidioc_encoder_cmd(struct file *file, void *priv,
-						struct v4l2_encoder_cmd *cmd)
+static int vidioc_encoder_cmd(struct file *file, void *priv,
+			      struct v4l2_encoder_cmd *cmd)
 {
 	struct s5p_mfc_ctx *ctx = fh_to_ctx(priv);
 	struct s5p_mfc_dev *dev = ctx->dev;
@@ -1704,7 +1685,7 @@ int vidioc_encoder_cmd(struct file *file, void *priv,
 			if (s5p_mfc_ctx_ready(ctx))
 				set_work_bit_irqsave(ctx);
 			spin_unlock_irqrestore(&dev->irqlock, flags);
-			s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+			s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 		} else {
 			mfc_debug(2, "EOS: marking last buffer of stream\n");
 			buf = list_entry(ctx->src_queue.prev,
@@ -1736,9 +1717,7 @@ static int vidioc_subscribe_event(struct v4l2_fh *fh,
 
 static const struct v4l2_ioctl_ops s5p_mfc_enc_ioctl_ops = {
 	.vidioc_querycap = vidioc_querycap,
-	.vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
 	.vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_cap_mplane,
-	.vidioc_enum_fmt_vid_out = vidioc_enum_fmt_vid_out,
 	.vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_out_mplane,
 	.vidioc_g_fmt_vid_cap_mplane = vidioc_g_fmt,
 	.vidioc_g_fmt_vid_out_mplane = vidioc_g_fmt,
@@ -1771,13 +1750,13 @@ static int check_vb_with_fmt(struct s5p_mfc_fmt *fmt, struct vb2_buffer *vb)
 		return -EINVAL;
 	}
 	for (i = 0; i < fmt->num_planes; i++) {
-		if (!vb2_dma_contig_plane_dma_addr(vb, i)) {
+		dma_addr_t dma = vb2_dma_contig_plane_dma_addr(vb, i);
+		if (!dma) {
 			mfc_err("failed to get plane cookie\n");
 			return -EINVAL;
 		}
-		mfc_debug(2, "index: %d, plane[%d] cookie: 0x%08zx\n",
-			  vb->v4l2_buf.index, i,
-			  vb2_dma_contig_plane_dma_addr(vb, i));
+		mfc_debug(2, "index: %d, plane[%d] cookie: %pad\n",
+			  vb->v4l2_buf.index, i, &dma);
 	}
 	return 0;
 }
@@ -1897,7 +1876,7 @@ static int s5p_mfc_buf_prepare(struct vb2_buffer *vb)
 		ret = check_vb_with_fmt(ctx->dst_fmt, vb);
 		if (ret < 0)
 			return ret;
-		mfc_debug(2, "plane size: %ld, dst size: %d\n",
+		mfc_debug(2, "plane size: %ld, dst size: %zu\n",
 			vb2_plane_size(vb, 0), ctx->enc_dst_buf_size);
 		if (vb2_plane_size(vb, 0) < ctx->enc_dst_buf_size) {
 			mfc_err("plane size is too small for capture\n");
@@ -1948,7 +1927,7 @@ static int s5p_mfc_start_streaming(struct vb2_queue *q, unsigned int count)
 	/* If context is ready then dev = work->data;schedule it to run */
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 
 	return 0;
 }
@@ -1969,14 +1948,14 @@ static void s5p_mfc_stop_streaming(struct vb2_queue *q)
 	ctx->state = MFCINST_FINISHED;
 	spin_lock_irqsave(&dev->irqlock, flags);
 	if (q->type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
-		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->dst_queue,
-				&ctx->vq_dst);
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue,
+						&ctx->dst_queue, &ctx->vq_dst);
 		INIT_LIST_HEAD(&ctx->dst_queue);
 		ctx->dst_queue_cnt = 0;
 	}
 	if (q->type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
 		cleanup_ref_queue(ctx);
-		s5p_mfc_hw_call(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
+		s5p_mfc_hw_call_void(dev->mfc_ops, cleanup_queue, &ctx->src_queue,
 				&ctx->vq_src);
 		INIT_LIST_HEAD(&ctx->src_queue);
 		ctx->src_queue_cnt = 0;
@@ -2017,7 +1996,7 @@ static void s5p_mfc_buf_queue(struct vb2_buffer *vb)
 	}
 	if (s5p_mfc_ctx_ready(ctx))
 		set_work_bit_irqsave(ctx);
-	s5p_mfc_hw_call(dev->mfc_ops, try_run, dev);
+	s5p_mfc_hw_call_void(dev->mfc_ops, try_run, dev);
 }
 
 static struct vb2_ops s5p_mfc_enc_qops = {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
index c9a227428e6a..00a1d8b2a8c2 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.c
@@ -41,7 +41,7 @@ int s5p_mfc_alloc_priv_buf(struct device *dev,
 					struct s5p_mfc_priv_buf *b)
 {
 
-	mfc_debug(3, "Allocating priv: %d\n", b->size);
+	mfc_debug(3, "Allocating priv: %zu\n", b->size);
 
 	b->virt = dma_alloc_coherent(dev, b->size, &b->dma, GFP_KERNEL);
 
@@ -50,7 +50,7 @@ int s5p_mfc_alloc_priv_buf(struct device *dev,
 		return -ENOMEM;
 	}
 
-	mfc_debug(3, "Allocated addr %p %08x\n", b->virt, b->dma);
+	mfc_debug(3, "Allocated addr %p %pad\n", b->virt, &b->dma);
 	return 0;
 }
 
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
index 7a7ad32ee608..de2b8c69daa5 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr.h
@@ -20,254 +20,254 @@
 struct s5p_mfc_regs {
 
 	/* codec common registers */
-	void *risc_on;
-	void *risc2host_int;
-	void *host2risc_int;
-	void *risc_base_address;
-	void *mfc_reset;
-	void *host2risc_command;
-	void *risc2host_command;
-	void *mfc_bus_reset_ctrl;
-	void *firmware_version;
-	void *instance_id;
-	void *codec_type;
-	void *context_mem_addr;
-	void *context_mem_size;
-	void *pixel_format;
-	void *metadata_enable;
-	void *mfc_version;
-	void *dbg_info_enable;
-	void *dbg_buffer_addr;
-	void *dbg_buffer_size;
-	void *hed_control;
-	void *mfc_timeout_value;
-	void *hed_shared_mem_addr;
-	void *dis_shared_mem_addr;/* only v7 */
-	void *ret_instance_id;
-	void *error_code;
-	void *dbg_buffer_output_size;
-	void *metadata_status;
-	void *metadata_addr_mb_info;
-	void *metadata_size_mb_info;
-	void *dbg_info_stage_counter;
+	volatile void __iomem *risc_on;
+	volatile void __iomem *risc2host_int;
+	volatile void __iomem *host2risc_int;
+	volatile void __iomem *risc_base_address;
+	volatile void __iomem *mfc_reset;
+	volatile void __iomem *host2risc_command;
+	volatile void __iomem *risc2host_command;
+	volatile void __iomem *mfc_bus_reset_ctrl;
+	volatile void __iomem *firmware_version;
+	volatile void __iomem *instance_id;
+	volatile void __iomem *codec_type;
+	volatile void __iomem *context_mem_addr;
+	volatile void __iomem *context_mem_size;
+	volatile void __iomem *pixel_format;
+	volatile void __iomem *metadata_enable;
+	volatile void __iomem *mfc_version;
+	volatile void __iomem *dbg_info_enable;
+	volatile void __iomem *dbg_buffer_addr;
+	volatile void __iomem *dbg_buffer_size;
+	volatile void __iomem *hed_control;
+	volatile void __iomem *mfc_timeout_value;
+	volatile void __iomem *hed_shared_mem_addr;
+	volatile void __iomem *dis_shared_mem_addr;/* only v7 */
+	volatile void __iomem *ret_instance_id;
+	volatile void __iomem *error_code;
+	volatile void __iomem *dbg_buffer_output_size;
+	volatile void __iomem *metadata_status;
+	volatile void __iomem *metadata_addr_mb_info;
+	volatile void __iomem *metadata_size_mb_info;
+	volatile void __iomem *dbg_info_stage_counter;
 
 	/* decoder registers */
-	void *d_crc_ctrl;
-	void *d_dec_options;
-	void *d_display_delay;
-	void *d_set_frame_width;
-	void *d_set_frame_height;
-	void *d_sei_enable;
-	void *d_min_num_dpb;
-	void *d_min_first_plane_dpb_size;
-	void *d_min_second_plane_dpb_size;
-	void *d_min_third_plane_dpb_size;/* only v8 */
-	void *d_min_num_mv;
-	void *d_mvc_num_views;
-	void *d_min_num_dis;/* only v7 */
-	void *d_min_first_dis_size;/* only v7 */
-	void *d_min_second_dis_size;/* only v7 */
-	void *d_min_third_dis_size;/* only v7 */
-	void *d_post_filter_luma_dpb0;/*  v7 and v8 */
-	void *d_post_filter_luma_dpb1;/* v7 and v8 */
-	void *d_post_filter_luma_dpb2;/* only v7 */
-	void *d_post_filter_chroma_dpb0;/* v7 and v8 */
-	void *d_post_filter_chroma_dpb1;/* v7 and v8 */
-	void *d_post_filter_chroma_dpb2;/* only v7 */
-	void *d_num_dpb;
-	void *d_num_mv;
-	void *d_init_buffer_options;
-	void *d_first_plane_dpb_stride_size;/* only v8 */
-	void *d_second_plane_dpb_stride_size;/* only v8 */
-	void *d_third_plane_dpb_stride_size;/* only v8 */
-	void *d_first_plane_dpb_size;
-	void *d_second_plane_dpb_size;
-	void *d_third_plane_dpb_size;/* only v8 */
-	void *d_mv_buffer_size;
-	void *d_first_plane_dpb;
-	void *d_second_plane_dpb;
-	void *d_third_plane_dpb;
-	void *d_mv_buffer;
-	void *d_scratch_buffer_addr;
-	void *d_scratch_buffer_size;
-	void *d_metadata_buffer_addr;
-	void *d_metadata_buffer_size;
-	void *d_nal_start_options;/* v7 and v8 */
-	void *d_cpb_buffer_addr;
-	void *d_cpb_buffer_size;
-	void *d_available_dpb_flag_upper;
-	void *d_available_dpb_flag_lower;
-	void *d_cpb_buffer_offset;
-	void *d_slice_if_enable;
-	void *d_picture_tag;
-	void *d_stream_data_size;
-	void *d_dynamic_dpb_flag_upper;/* v7 and v8 */
-	void *d_dynamic_dpb_flag_lower;/* v7 and v8 */
-	void *d_display_frame_width;
-	void *d_display_frame_height;
-	void *d_display_status;
-	void *d_display_first_plane_addr;
-	void *d_display_second_plane_addr;
-	void *d_display_third_plane_addr;/* only v8 */
-	void *d_display_frame_type;
-	void *d_display_crop_info1;
-	void *d_display_crop_info2;
-	void *d_display_picture_profile;
-	void *d_display_luma_crc;/* v7 and v8 */
-	void *d_display_chroma0_crc;/* v7 and v8 */
-	void *d_display_chroma1_crc;/* only v8 */
-	void *d_display_luma_crc_top;/* only v6 */
-	void *d_display_chroma_crc_top;/* only v6 */
-	void *d_display_luma_crc_bot;/* only v6 */
-	void *d_display_chroma_crc_bot;/* only v6 */
-	void *d_display_aspect_ratio;
-	void *d_display_extended_ar;
-	void *d_decoded_frame_width;
-	void *d_decoded_frame_height;
-	void *d_decoded_status;
-	void *d_decoded_first_plane_addr;
-	void *d_decoded_second_plane_addr;
-	void *d_decoded_third_plane_addr;/* only v8 */
-	void *d_decoded_frame_type;
-	void *d_decoded_crop_info1;
-	void *d_decoded_crop_info2;
-	void *d_decoded_picture_profile;
-	void *d_decoded_nal_size;
-	void *d_decoded_luma_crc;
-	void *d_decoded_chroma0_crc;
-	void *d_decoded_chroma1_crc;/* only v8 */
-	void *d_ret_picture_tag_top;
-	void *d_ret_picture_tag_bot;
-	void *d_ret_picture_time_top;
-	void *d_ret_picture_time_bot;
-	void *d_chroma_format;
-	void *d_vc1_info;/* v7 and v8 */
-	void *d_mpeg4_info;
-	void *d_h264_info;
-	void *d_metadata_addr_concealed_mb;
-	void *d_metadata_size_concealed_mb;
-	void *d_metadata_addr_vc1_param;
-	void *d_metadata_size_vc1_param;
-	void *d_metadata_addr_sei_nal;
-	void *d_metadata_size_sei_nal;
-	void *d_metadata_addr_vui;
-	void *d_metadata_size_vui;
-	void *d_metadata_addr_mvcvui;/* v7 and v8 */
-	void *d_metadata_size_mvcvui;/* v7 and v8 */
-	void *d_mvc_view_id;
-	void *d_frame_pack_sei_avail;
-	void *d_frame_pack_arrgment_id;
-	void *d_frame_pack_sei_info;
-	void *d_frame_pack_grid_pos;
-	void *d_display_recovery_sei_info;/* v7 and v8 */
-	void *d_decoded_recovery_sei_info;/* v7 and v8 */
-	void *d_display_first_addr;/* only v7 */
-	void *d_display_second_addr;/* only v7 */
-	void *d_display_third_addr;/* only v7 */
-	void *d_decoded_first_addr;/* only v7 */
-	void *d_decoded_second_addr;/* only v7 */
-	void *d_decoded_third_addr;/* only v7 */
-	void *d_used_dpb_flag_upper;/* v7 and v8 */
-	void *d_used_dpb_flag_lower;/* v7 and v8 */
+	volatile void __iomem *d_crc_ctrl;
+	volatile void __iomem *d_dec_options;
+	volatile void __iomem *d_display_delay;
+	volatile void __iomem *d_set_frame_width;
+	volatile void __iomem *d_set_frame_height;
+	volatile void __iomem *d_sei_enable;
+	volatile void __iomem *d_min_num_dpb;
+	volatile void __iomem *d_min_first_plane_dpb_size;
+	volatile void __iomem *d_min_second_plane_dpb_size;
+	volatile void __iomem *d_min_third_plane_dpb_size;/* only v8 */
+	volatile void __iomem *d_min_num_mv;
+	volatile void __iomem *d_mvc_num_views;
+	volatile void __iomem *d_min_num_dis;/* only v7 */
+	volatile void __iomem *d_min_first_dis_size;/* only v7 */
+	volatile void __iomem *d_min_second_dis_size;/* only v7 */
+	volatile void __iomem *d_min_third_dis_size;/* only v7 */
+	volatile void __iomem *d_post_filter_luma_dpb0;/*  v7 and v8 */
+	volatile void __iomem *d_post_filter_luma_dpb1;/* v7 and v8 */
+	volatile void __iomem *d_post_filter_luma_dpb2;/* only v7 */
+	volatile void __iomem *d_post_filter_chroma_dpb0;/* v7 and v8 */
+	volatile void __iomem *d_post_filter_chroma_dpb1;/* v7 and v8 */
+	volatile void __iomem *d_post_filter_chroma_dpb2;/* only v7 */
+	volatile void __iomem *d_num_dpb;
+	volatile void __iomem *d_num_mv;
+	volatile void __iomem *d_init_buffer_options;
+	volatile void __iomem *d_first_plane_dpb_stride_size;/* only v8 */
+	volatile void __iomem *d_second_plane_dpb_stride_size;/* only v8 */
+	volatile void __iomem *d_third_plane_dpb_stride_size;/* only v8 */
+	volatile void __iomem *d_first_plane_dpb_size;
+	volatile void __iomem *d_second_plane_dpb_size;
+	volatile void __iomem *d_third_plane_dpb_size;/* only v8 */
+	volatile void __iomem *d_mv_buffer_size;
+	volatile void __iomem *d_first_plane_dpb;
+	volatile void __iomem *d_second_plane_dpb;
+	volatile void __iomem *d_third_plane_dpb;
+	volatile void __iomem *d_mv_buffer;
+	volatile void __iomem *d_scratch_buffer_addr;
+	volatile void __iomem *d_scratch_buffer_size;
+	volatile void __iomem *d_metadata_buffer_addr;
+	volatile void __iomem *d_metadata_buffer_size;
+	volatile void __iomem *d_nal_start_options;/* v7 and v8 */
+	volatile void __iomem *d_cpb_buffer_addr;
+	volatile void __iomem *d_cpb_buffer_size;
+	volatile void __iomem *d_available_dpb_flag_upper;
+	volatile void __iomem *d_available_dpb_flag_lower;
+	volatile void __iomem *d_cpb_buffer_offset;
+	volatile void __iomem *d_slice_if_enable;
+	volatile void __iomem *d_picture_tag;
+	volatile void __iomem *d_stream_data_size;
+	volatile void __iomem *d_dynamic_dpb_flag_upper;/* v7 and v8 */
+	volatile void __iomem *d_dynamic_dpb_flag_lower;/* v7 and v8 */
+	volatile void __iomem *d_display_frame_width;
+	volatile void __iomem *d_display_frame_height;
+	volatile void __iomem *d_display_status;
+	volatile void __iomem *d_display_first_plane_addr;
+	volatile void __iomem *d_display_second_plane_addr;
+	volatile void __iomem *d_display_third_plane_addr;/* only v8 */
+	volatile void __iomem *d_display_frame_type;
+	volatile void __iomem *d_display_crop_info1;
+	volatile void __iomem *d_display_crop_info2;
+	volatile void __iomem *d_display_picture_profile;
+	volatile void __iomem *d_display_luma_crc;/* v7 and v8 */
+	volatile void __iomem *d_display_chroma0_crc;/* v7 and v8 */
+	volatile void __iomem *d_display_chroma1_crc;/* only v8 */
+	volatile void __iomem *d_display_luma_crc_top;/* only v6 */
+	volatile void __iomem *d_display_chroma_crc_top;/* only v6 */
+	volatile void __iomem *d_display_luma_crc_bot;/* only v6 */
+	volatile void __iomem *d_display_chroma_crc_bot;/* only v6 */
+	volatile void __iomem *d_display_aspect_ratio;
+	volatile void __iomem *d_display_extended_ar;
+	volatile void __iomem *d_decoded_frame_width;
+	volatile void __iomem *d_decoded_frame_height;
+	volatile void __iomem *d_decoded_status;
+	volatile void __iomem *d_decoded_first_plane_addr;
+	volatile void __iomem *d_decoded_second_plane_addr;
+	volatile void __iomem *d_decoded_third_plane_addr;/* only v8 */
+	volatile void __iomem *d_decoded_frame_type;
+	volatile void __iomem *d_decoded_crop_info1;
+	volatile void __iomem *d_decoded_crop_info2;
+	volatile void __iomem *d_decoded_picture_profile;
+	volatile void __iomem *d_decoded_nal_size;
+	volatile void __iomem *d_decoded_luma_crc;
+	volatile void __iomem *d_decoded_chroma0_crc;
+	volatile void __iomem *d_decoded_chroma1_crc;/* only v8 */
+	volatile void __iomem *d_ret_picture_tag_top;
+	volatile void __iomem *d_ret_picture_tag_bot;
+	volatile void __iomem *d_ret_picture_time_top;
+	volatile void __iomem *d_ret_picture_time_bot;
+	volatile void __iomem *d_chroma_format;
+	volatile void __iomem *d_vc1_info;/* v7 and v8 */
+	volatile void __iomem *d_mpeg4_info;
+	volatile void __iomem *d_h264_info;
+	volatile void __iomem *d_metadata_addr_concealed_mb;
+	volatile void __iomem *d_metadata_size_concealed_mb;
+	volatile void __iomem *d_metadata_addr_vc1_param;
+	volatile void __iomem *d_metadata_size_vc1_param;
+	volatile void __iomem *d_metadata_addr_sei_nal;
+	volatile void __iomem *d_metadata_size_sei_nal;
+	volatile void __iomem *d_metadata_addr_vui;
+	volatile void __iomem *d_metadata_size_vui;
+	volatile void __iomem *d_metadata_addr_mvcvui;/* v7 and v8 */
+	volatile void __iomem *d_metadata_size_mvcvui;/* v7 and v8 */
+	volatile void __iomem *d_mvc_view_id;
+	volatile void __iomem *d_frame_pack_sei_avail;
+	volatile void __iomem *d_frame_pack_arrgment_id;
+	volatile void __iomem *d_frame_pack_sei_info;
+	volatile void __iomem *d_frame_pack_grid_pos;
+	volatile void __iomem *d_display_recovery_sei_info;/* v7 and v8 */
+	volatile void __iomem *d_decoded_recovery_sei_info;/* v7 and v8 */
+	volatile void __iomem *d_display_first_addr;/* only v7 */
+	volatile void __iomem *d_display_second_addr;/* only v7 */
+	volatile void __iomem *d_display_third_addr;/* only v7 */
+	volatile void __iomem *d_decoded_first_addr;/* only v7 */
+	volatile void __iomem *d_decoded_second_addr;/* only v7 */
+	volatile void __iomem *d_decoded_third_addr;/* only v7 */
+	volatile void __iomem *d_used_dpb_flag_upper;/* v7 and v8 */
+	volatile void __iomem *d_used_dpb_flag_lower;/* v7 and v8 */
 
 	/* encoder registers */
-	void *e_frame_width;
-	void *e_frame_height;
-	void *e_cropped_frame_width;
-	void *e_cropped_frame_height;
-	void *e_frame_crop_offset;
-	void *e_enc_options;
-	void *e_picture_profile;
-	void *e_vbv_buffer_size;
-	void *e_vbv_init_delay;
-	void *e_fixed_picture_qp;
-	void *e_rc_config;
-	void *e_rc_qp_bound;
-	void *e_rc_qp_bound_pb;/* v7 and v8 */
-	void *e_rc_mode;
-	void *e_mb_rc_config;
-	void *e_padding_ctrl;
-	void *e_air_threshold;
-	void *e_mv_hor_range;
-	void *e_mv_ver_range;
-	void *e_num_dpb;
-	void *e_luma_dpb;
-	void *e_chroma_dpb;
-	void *e_me_buffer;
-	void *e_scratch_buffer_addr;
-	void *e_scratch_buffer_size;
-	void *e_tmv_buffer0;
-	void *e_tmv_buffer1;
-	void *e_ir_buffer_addr;/* v7 and v8 */
-	void *e_source_first_plane_addr;
-	void *e_source_second_plane_addr;
-	void *e_source_third_plane_addr;/* v7 and v8 */
-	void *e_source_first_plane_stride;/* v7 and v8 */
-	void *e_source_second_plane_stride;/* v7 and v8 */
-	void *e_source_third_plane_stride;/* v7 and v8 */
-	void *e_stream_buffer_addr;
-	void *e_stream_buffer_size;
-	void *e_roi_buffer_addr;
-	void *e_param_change;
-	void *e_ir_size;
-	void *e_gop_config;
-	void *e_mslice_mode;
-	void *e_mslice_size_mb;
-	void *e_mslice_size_bits;
-	void *e_frame_insertion;
-	void *e_rc_frame_rate;
-	void *e_rc_bit_rate;
-	void *e_rc_roi_ctrl;
-	void *e_picture_tag;
-	void *e_bit_count_enable;
-	void *e_max_bit_count;
-	void *e_min_bit_count;
-	void *e_metadata_buffer_addr;
-	void *e_metadata_buffer_size;
-	void *e_encoded_source_first_plane_addr;
-	void *e_encoded_source_second_plane_addr;
-	void *e_encoded_source_third_plane_addr;/* v7 and v8 */
-	void *e_stream_size;
-	void *e_slice_type;
-	void *e_picture_count;
-	void *e_ret_picture_tag;
-	void *e_stream_buffer_write_pointer; /*  only v6 */
-	void *e_recon_luma_dpb_addr;
-	void *e_recon_chroma_dpb_addr;
-	void *e_metadata_addr_enc_slice;
-	void *e_metadata_size_enc_slice;
-	void *e_mpeg4_options;
-	void *e_mpeg4_hec_period;
-	void *e_aspect_ratio;
-	void *e_extended_sar;
-	void *e_h264_options;
-	void *e_h264_options_2;/* v7 and v8 */
-	void *e_h264_lf_alpha_offset;
-	void *e_h264_lf_beta_offset;
-	void *e_h264_i_period;
-	void *e_h264_fmo_slice_grp_map_type;
-	void *e_h264_fmo_num_slice_grp_minus1;
-	void *e_h264_fmo_slice_grp_change_dir;
-	void *e_h264_fmo_slice_grp_change_rate_minus1;
-	void *e_h264_fmo_run_length_minus1_0;
-	void *e_h264_aso_slice_order_0;
-	void *e_h264_chroma_qp_offset;
-	void *e_h264_num_t_layer;
-	void *e_h264_hierarchical_qp_layer0;
-	void *e_h264_frame_packing_sei_info;
-	void *e_h264_nal_control;/* v7 and v8 */
-	void *e_mvc_frame_qp_view1;
-	void *e_mvc_rc_bit_rate_view1;
-	void *e_mvc_rc_qbound_view1;
-	void *e_mvc_rc_mode_view1;
-	void *e_mvc_inter_view_prediction_on;
-	void *e_vp8_options;/* v7 and v8 */
-	void *e_vp8_filter_options;/* v7 and v8 */
-	void *e_vp8_golden_frame_option;/* v7 and v8 */
-	void *e_vp8_num_t_layer;/* v7 and v8 */
-	void *e_vp8_hierarchical_qp_layer0;/* v7 and v8 */
-	void *e_vp8_hierarchical_qp_layer1;/* v7 and v8 */
-	void *e_vp8_hierarchical_qp_layer2;/* v7 and v8 */
+	volatile void __iomem *e_frame_width;
+	volatile void __iomem *e_frame_height;
+	volatile void __iomem *e_cropped_frame_width;
+	volatile void __iomem *e_cropped_frame_height;
+	volatile void __iomem *e_frame_crop_offset;
+	volatile void __iomem *e_enc_options;
+	volatile void __iomem *e_picture_profile;
+	volatile void __iomem *e_vbv_buffer_size;
+	volatile void __iomem *e_vbv_init_delay;
+	volatile void __iomem *e_fixed_picture_qp;
+	volatile void __iomem *e_rc_config;
+	volatile void __iomem *e_rc_qp_bound;
+	volatile void __iomem *e_rc_qp_bound_pb;/* v7 and v8 */
+	volatile void __iomem *e_rc_mode;
+	volatile void __iomem *e_mb_rc_config;
+	volatile void __iomem *e_padding_ctrl;
+	volatile void __iomem *e_air_threshold;
+	volatile void __iomem *e_mv_hor_range;
+	volatile void __iomem *e_mv_ver_range;
+	volatile void __iomem *e_num_dpb;
+	volatile void __iomem *e_luma_dpb;
+	volatile void __iomem *e_chroma_dpb;
+	volatile void __iomem *e_me_buffer;
+	volatile void __iomem *e_scratch_buffer_addr;
+	volatile void __iomem *e_scratch_buffer_size;
+	volatile void __iomem *e_tmv_buffer0;
+	volatile void __iomem *e_tmv_buffer1;
+	volatile void __iomem *e_ir_buffer_addr;/* v7 and v8 */
+	volatile void __iomem *e_source_first_plane_addr;
+	volatile void __iomem *e_source_second_plane_addr;
+	volatile void __iomem *e_source_third_plane_addr;/* v7 and v8 */
+	volatile void __iomem *e_source_first_plane_stride;/* v7 and v8 */
+	volatile void __iomem *e_source_second_plane_stride;/* v7 and v8 */
+	volatile void __iomem *e_source_third_plane_stride;/* v7 and v8 */
+	volatile void __iomem *e_stream_buffer_addr;
+	volatile void __iomem *e_stream_buffer_size;
+	volatile void __iomem *e_roi_buffer_addr;
+	volatile void __iomem *e_param_change;
+	volatile void __iomem *e_ir_size;
+	volatile void __iomem *e_gop_config;
+	volatile void __iomem *e_mslice_mode;
+	volatile void __iomem *e_mslice_size_mb;
+	volatile void __iomem *e_mslice_size_bits;
+	volatile void __iomem *e_frame_insertion;
+	volatile void __iomem *e_rc_frame_rate;
+	volatile void __iomem *e_rc_bit_rate;
+	volatile void __iomem *e_rc_roi_ctrl;
+	volatile void __iomem *e_picture_tag;
+	volatile void __iomem *e_bit_count_enable;
+	volatile void __iomem *e_max_bit_count;
+	volatile void __iomem *e_min_bit_count;
+	volatile void __iomem *e_metadata_buffer_addr;
+	volatile void __iomem *e_metadata_buffer_size;
+	volatile void __iomem *e_encoded_source_first_plane_addr;
+	volatile void __iomem *e_encoded_source_second_plane_addr;
+	volatile void __iomem *e_encoded_source_third_plane_addr;/* v7 and v8 */
+	volatile void __iomem *e_stream_size;
+	volatile void __iomem *e_slice_type;
+	volatile void __iomem *e_picture_count;
+	volatile void __iomem *e_ret_picture_tag;
+	volatile void __iomem *e_stream_buffer_write_pointer; /*  only v6 */
+	volatile void __iomem *e_recon_luma_dpb_addr;
+	volatile void __iomem *e_recon_chroma_dpb_addr;
+	volatile void __iomem *e_metadata_addr_enc_slice;
+	volatile void __iomem *e_metadata_size_enc_slice;
+	volatile void __iomem *e_mpeg4_options;
+	volatile void __iomem *e_mpeg4_hec_period;
+	volatile void __iomem *e_aspect_ratio;
+	volatile void __iomem *e_extended_sar;
+	volatile void __iomem *e_h264_options;
+	volatile void __iomem *e_h264_options_2;/* v7 and v8 */
+	volatile void __iomem *e_h264_lf_alpha_offset;
+	volatile void __iomem *e_h264_lf_beta_offset;
+	volatile void __iomem *e_h264_i_period;
+	volatile void __iomem *e_h264_fmo_slice_grp_map_type;
+	volatile void __iomem *e_h264_fmo_num_slice_grp_minus1;
+	volatile void __iomem *e_h264_fmo_slice_grp_change_dir;
+	volatile void __iomem *e_h264_fmo_slice_grp_change_rate_minus1;
+	volatile void __iomem *e_h264_fmo_run_length_minus1_0;
+	volatile void __iomem *e_h264_aso_slice_order_0;
+	volatile void __iomem *e_h264_chroma_qp_offset;
+	volatile void __iomem *e_h264_num_t_layer;
+	volatile void __iomem *e_h264_hierarchical_qp_layer0;
+	volatile void __iomem *e_h264_frame_packing_sei_info;
+	volatile void __iomem *e_h264_nal_control;/* v7 and v8 */
+	volatile void __iomem *e_mvc_frame_qp_view1;
+	volatile void __iomem *e_mvc_rc_bit_rate_view1;
+	volatile void __iomem *e_mvc_rc_qbound_view1;
+	volatile void __iomem *e_mvc_rc_mode_view1;
+	volatile void __iomem *e_mvc_inter_view_prediction_on;
+	volatile void __iomem *e_vp8_options;/* v7 and v8 */
+	volatile void __iomem *e_vp8_filter_options;/* v7 and v8 */
+	volatile void __iomem *e_vp8_golden_frame_option;/* v7 and v8 */
+	volatile void __iomem *e_vp8_num_t_layer;/* v7 and v8 */
+	volatile void __iomem *e_vp8_hierarchical_qp_layer0;/* v7 and v8 */
+	volatile void __iomem *e_vp8_hierarchical_qp_layer1;/* v7 and v8 */
+	volatile void __iomem *e_vp8_hierarchical_qp_layer2;/* v7 and v8 */
 };
 
 struct s5p_mfc_hw_ops {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
index 58ec7bb26ebc..7cf07963187d 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v5.c
@@ -228,6 +228,7 @@ static int s5p_mfc_alloc_instance_buffer_v5(struct s5p_mfc_ctx *ctx)
 	ret = s5p_mfc_alloc_priv_buf(dev->mem_dev_l, &ctx->shm);
 	if (ret) {
 		mfc_err("Failed to allocate shared memory buffer\n");
+		s5p_mfc_release_priv_buf(dev->mem_dev_l, &ctx->ctx);
 		return ret;
 	}
 
@@ -262,7 +263,7 @@ static void s5p_mfc_release_dev_context_buffer_v5(struct s5p_mfc_dev *dev)
 static void s5p_mfc_write_info_v5(struct s5p_mfc_ctx *ctx, unsigned int data,
 			unsigned int ofs)
 {
-	writel(data, (ctx->shm.virt + ofs));
+	writel(data, (volatile void __iomem *)(ctx->shm.virt + ofs));
 	wmb();
 }
 
@@ -270,7 +271,7 @@ static unsigned int s5p_mfc_read_info_v5(struct s5p_mfc_ctx *ctx,
 				unsigned int ofs)
 {
 	rmb();
-	return readl(ctx->shm.virt + ofs);
+	return readl((volatile void __iomem *)(ctx->shm.virt + ofs));
 }
 
 static void s5p_mfc_dec_calc_dpb_size_v5(struct s5p_mfc_ctx *ctx)
@@ -377,7 +378,7 @@ static int s5p_mfc_set_dec_stream_buffer_v5(struct s5p_mfc_ctx *ctx,
 /* Set decoding frame buffer */
 static int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
 {
-	unsigned int frame_size, i;
+	unsigned int frame_size_lu, i;
 	unsigned int frame_size_ch, frame_size_mv;
 	struct s5p_mfc_dev *dev = ctx->dev;
 	unsigned int dpb;
@@ -465,23 +466,23 @@ static int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
 			ctx->codec_mode);
 		return -EINVAL;
 	}
-	frame_size = ctx->luma_size;
+	frame_size_lu = ctx->luma_size;
 	frame_size_ch = ctx->chroma_size;
 	frame_size_mv = ctx->mv_size;
-	mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size, frame_size_ch,
+	mfc_debug(2, "Frm size: %d ch: %d mv: %d\n", frame_size_lu, frame_size_ch,
 								frame_size_mv);
 	for (i = 0; i < ctx->total_dpb_count; i++) {
 		/* Bank2 */
-		mfc_debug(2, "Luma %d: %x\n", i,
+		mfc_debug(2, "Luma %d: %zx\n", i,
 					ctx->dst_bufs[i].cookie.raw.luma);
 		mfc_write(dev, OFFSETB(ctx->dst_bufs[i].cookie.raw.luma),
 						S5P_FIMV_DEC_LUMA_ADR + i * 4);
-		mfc_debug(2, "\tChroma %d: %x\n", i,
+		mfc_debug(2, "\tChroma %d: %zx\n", i,
 					ctx->dst_bufs[i].cookie.raw.chroma);
 		mfc_write(dev, OFFSETA(ctx->dst_bufs[i].cookie.raw.chroma),
 					       S5P_FIMV_DEC_CHROMA_ADR + i * 4);
 		if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC) {
-			mfc_debug(2, "\tBuf2: %x, size: %d\n",
+			mfc_debug(2, "\tBuf2: %zx, size: %d\n",
 							buf_addr2, buf_size2);
 			mfc_write(dev, OFFSETB(buf_addr2),
 						S5P_FIMV_H264_MV_ADR + i * 4);
@@ -489,14 +490,14 @@ static int s5p_mfc_set_dec_frame_buffer_v5(struct s5p_mfc_ctx *ctx)
 			buf_size2 -= frame_size_mv;
 		}
 	}
-	mfc_debug(2, "Buf1: %u, buf_size1: %d\n", buf_addr1, buf_size1);
+	mfc_debug(2, "Buf1: %zu, buf_size1: %d\n", buf_addr1, buf_size1);
 	mfc_debug(2, "Buf 1/2 size after: %d/%d (frames %d)\n",
 			buf_size1,  buf_size2, ctx->total_dpb_count);
 	if (buf_size1 < 0 || buf_size2 < 0) {
 		mfc_debug(2, "Not enough memory has been allocated\n");
 		return -ENOMEM;
 	}
-	s5p_mfc_write_info_v5(ctx, frame_size, ALLOC_LUMA_DPB_SIZE);
+	s5p_mfc_write_info_v5(ctx, frame_size_lu, ALLOC_LUMA_DPB_SIZE);
 	s5p_mfc_write_info_v5(ctx, frame_size_ch, ALLOC_CHROMA_DPB_SIZE);
 	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC)
 		s5p_mfc_write_info_v5(ctx, frame_size_mv, ALLOC_MV_SIZE);
@@ -566,7 +567,7 @@ static int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
 		enc_ref_c_size = ALIGN(guard_width * guard_height,
 				       S5P_FIMV_NV12MT_SALIGN);
 	}
-	mfc_debug(2, "buf_size1: %d, buf_size2: %d\n", buf_size1, buf_size2);
+	mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n", buf_size1, buf_size2);
 	switch (ctx->codec_mode) {
 	case S5P_MFC_CODEC_H264_ENC:
 		for (i = 0; i < 2; i++) {
@@ -605,7 +606,7 @@ static int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
 					S5P_FIMV_H264_NBOR_INFO_ADR);
 		buf_addr1 += S5P_FIMV_ENC_NBORINFO_SIZE;
 		buf_size1 -= S5P_FIMV_ENC_NBORINFO_SIZE;
-		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+		mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n",
 			buf_size1, buf_size2);
 		break;
 	case S5P_MFC_CODEC_MPEG4_ENC:
@@ -636,7 +637,7 @@ static int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
 						S5P_FIMV_MPEG4_ACDC_COEF_ADR);
 		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
 		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+		mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n",
 			buf_size1, buf_size2);
 		break;
 	case S5P_MFC_CODEC_H263_ENC:
@@ -662,7 +663,7 @@ static int s5p_mfc_set_enc_ref_buffer_v5(struct s5p_mfc_ctx *ctx)
 		mfc_write(dev, OFFSETA(buf_addr1), S5P_FIMV_H263_ACDC_COEF_ADR);
 		buf_addr1 += S5P_FIMV_ENC_ACDCCOEF_SIZE;
 		buf_size1 -= S5P_FIMV_ENC_ACDCCOEF_SIZE;
-		mfc_debug(2, "buf_size1: %d, buf_size2: %d\n",
+		mfc_debug(2, "buf_size1: %zu, buf_size2: %zu\n",
 			buf_size1, buf_size2);
 		break;
 	default:
@@ -1186,7 +1187,6 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 	struct s5p_mfc_dev *dev = ctx->dev;
 	struct s5p_mfc_buf *temp_vb;
 	unsigned long flags;
-	unsigned int index;
 
 	if (ctx->state == MFCINST_FINISHING) {
 		last_frame = MFC_DEC_LAST_FRAME;
@@ -1211,7 +1211,6 @@ static int s5p_mfc_run_dec_frame(struct s5p_mfc_ctx *ctx, int last_frame)
 		vb2_dma_contig_plane_dma_addr(temp_vb->b, 0),
 		ctx->consumed_stream, temp_vb->b->v4l2_planes[0].bytesused);
 	spin_unlock_irqrestore(&dev->irqlock, flags);
-	index = temp_vb->b->v4l2_buf.index;
 	dev->curr_ctx = ctx->num;
 	s5p_mfc_clean_ctx_int_flags(ctx);
 	if (temp_vb->b->v4l2_planes[0].bytesused == 0) {
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
index c1c12f8d8f68..8798b14bacce 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_opr_v6.c
@@ -43,11 +43,6 @@
 	} while (0)
 #endif /* S5P_MFC_DEBUG_REGWRITE */
 
-#define READL(reg) \
-	(WARN_ON_ONCE(!(reg)) ? 0 : readl(reg))
-#define WRITEL(data, reg) \
-	(WARN_ON_ONCE(!(reg)) ? 0 : writel((data), (reg)))
-
 #define IS_MFCV6_V2(dev) (!IS_MFCV7_PLUS(dev) && dev->fw_ver == MFC_FW_V2)
 
 /* Allocate temporary buffers for decoding */
@@ -105,7 +100,7 @@ static int s5p_mfc_alloc_codec_buffers_v6(struct s5p_mfc_ctx *ctx)
 						mb_width, mb_height),
 						S5P_FIMV_ME_BUFFER_ALIGN_V6);
 
-		mfc_debug(2, "recon luma size: %d chroma size: %d\n",
+		mfc_debug(2, "recon luma size: %zu chroma size: %zu\n",
 			  ctx->luma_dpb_size, ctx->chroma_dpb_size);
 	} else {
 		return -EINVAL;
@@ -416,10 +411,10 @@ static int s5p_mfc_set_dec_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
 	mfc_debug(2, "inst_no: %d, buf_addr: 0x%08x,\n"
 		"buf_size: 0x%08x (%d)\n",
 		ctx->inst_no, buf_addr, strm_size, strm_size);
-	WRITEL(strm_size, mfc_regs->d_stream_data_size);
-	WRITEL(buf_addr, mfc_regs->d_cpb_buffer_addr);
-	WRITEL(buf_size->cpb, mfc_regs->d_cpb_buffer_size);
-	WRITEL(start_num_byte, mfc_regs->d_cpb_buffer_offset);
+	writel(strm_size, mfc_regs->d_stream_data_size);
+	writel(buf_addr, mfc_regs->d_cpb_buffer_addr);
+	writel(buf_size->cpb, mfc_regs->d_cpb_buffer_size);
+	writel(start_num_byte, mfc_regs->d_cpb_buffer_offset);
 
 	mfc_debug_leave();
 	return 0;
@@ -443,17 +438,17 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 	mfc_debug(2, "Total DPB COUNT: %d\n", ctx->total_dpb_count);
 	mfc_debug(2, "Setting display delay to %d\n", ctx->display_delay);
 
-	WRITEL(ctx->total_dpb_count, mfc_regs->d_num_dpb);
-	WRITEL(ctx->luma_size, mfc_regs->d_first_plane_dpb_size);
-	WRITEL(ctx->chroma_size, mfc_regs->d_second_plane_dpb_size);
+	writel(ctx->total_dpb_count, mfc_regs->d_num_dpb);
+	writel(ctx->luma_size, mfc_regs->d_first_plane_dpb_size);
+	writel(ctx->chroma_size, mfc_regs->d_second_plane_dpb_size);
 
-	WRITEL(buf_addr1, mfc_regs->d_scratch_buffer_addr);
-	WRITEL(ctx->scratch_buf_size, mfc_regs->d_scratch_buffer_size);
+	writel(buf_addr1, mfc_regs->d_scratch_buffer_addr);
+	writel(ctx->scratch_buf_size, mfc_regs->d_scratch_buffer_size);
 
 	if (IS_MFCV8(dev)) {
-		WRITEL(ctx->img_width,
+		writel(ctx->img_width,
 			mfc_regs->d_first_plane_dpb_stride_size);
-		WRITEL(ctx->img_width,
+		writel(ctx->img_width,
 			mfc_regs->d_second_plane_dpb_stride_size);
 	}
 
@@ -462,8 +457,8 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 
 	if (ctx->codec_mode == S5P_FIMV_CODEC_H264_DEC ||
 			ctx->codec_mode == S5P_FIMV_CODEC_H264_MVC_DEC){
-		WRITEL(ctx->mv_size, mfc_regs->d_mv_buffer_size);
-		WRITEL(ctx->mv_count, mfc_regs->d_num_mv);
+		writel(ctx->mv_size, mfc_regs->d_mv_buffer_size);
+		writel(ctx->mv_count, mfc_regs->d_num_mv);
 	}
 
 	frame_size = ctx->luma_size;
@@ -474,13 +469,13 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 
 	for (i = 0; i < ctx->total_dpb_count; i++) {
 		/* Bank2 */
-		mfc_debug(2, "Luma %d: %x\n", i,
+		mfc_debug(2, "Luma %d: %zx\n", i,
 					ctx->dst_bufs[i].cookie.raw.luma);
-		WRITEL(ctx->dst_bufs[i].cookie.raw.luma,
+		writel(ctx->dst_bufs[i].cookie.raw.luma,
 				mfc_regs->d_first_plane_dpb + i * 4);
-		mfc_debug(2, "\tChroma %d: %x\n", i,
+		mfc_debug(2, "\tChroma %d: %zx\n", i,
 					ctx->dst_bufs[i].cookie.raw.chroma);
-		WRITEL(ctx->dst_bufs[i].cookie.raw.chroma,
+		writel(ctx->dst_bufs[i].cookie.raw.chroma,
 				mfc_regs->d_second_plane_dpb + i * 4);
 	}
 	if (ctx->codec_mode == S5P_MFC_CODEC_H264_DEC ||
@@ -492,23 +487,23 @@ static int s5p_mfc_set_dec_frame_buffer_v6(struct s5p_mfc_ctx *ctx)
 			align_gap = buf_addr1 - align_gap;
 			buf_size1 -= align_gap;
 
-			mfc_debug(2, "\tBuf1: %x, size: %d\n",
+			mfc_debug(2, "\tBuf1: %zx, size: %d\n",
 					buf_addr1, buf_size1);
-			WRITEL(buf_addr1, mfc_regs->d_mv_buffer + i * 4);
+			writel(buf_addr1, mfc_regs->d_mv_buffer + i * 4);
 			buf_addr1 += frame_size_mv;
 			buf_size1 -= frame_size_mv;
 		}
 	}
 
-	mfc_debug(2, "Buf1: %u, buf_size1: %d (frames %d)\n",
+	mfc_debug(2, "Buf1: %zu, buf_size1: %d (frames %d)\n",
 			buf_addr1, buf_size1, ctx->total_dpb_count);
 	if (buf_size1 < 0) {
 		mfc_debug(2, "Not enough memory has been allocated.\n");
 		return -ENOMEM;
 	}
 
-	WRITEL(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
 
 	mfc_debug(2, "After setting buffers.\n");
@@ -522,8 +517,8 @@ static int s5p_mfc_set_enc_stream_buffer_v6(struct s5p_mfc_ctx *ctx,
 	struct s5p_mfc_dev *dev = ctx->dev;
 	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
-	WRITEL(addr, mfc_regs->e_stream_buffer_addr); /* 16B align */
-	WRITEL(size, mfc_regs->e_stream_buffer_size);
+	writel(addr, mfc_regs->e_stream_buffer_addr); /* 16B align */
+	writel(size, mfc_regs->e_stream_buffer_size);
 
 	mfc_debug(2, "stream buf addr: 0x%08lx, size: 0x%d\n",
 		  addr, size);
@@ -537,8 +532,8 @@ static void s5p_mfc_set_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 	struct s5p_mfc_dev *dev = ctx->dev;
 	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
-	WRITEL(y_addr, mfc_regs->e_source_first_plane_addr);
-	WRITEL(c_addr, mfc_regs->e_source_second_plane_addr);
+	writel(y_addr, mfc_regs->e_source_first_plane_addr);
+	writel(c_addr, mfc_regs->e_source_second_plane_addr);
 
 	mfc_debug(2, "enc src y buf addr: 0x%08lx\n", y_addr);
 	mfc_debug(2, "enc src c buf addr: 0x%08lx\n", c_addr);
@@ -551,11 +546,11 @@ static void s5p_mfc_get_enc_frame_buffer_v6(struct s5p_mfc_ctx *ctx,
 	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 	unsigned long enc_recon_y_addr, enc_recon_c_addr;
 
-	*y_addr = READL(mfc_regs->e_encoded_source_first_plane_addr);
-	*c_addr = READL(mfc_regs->e_encoded_source_second_plane_addr);
+	*y_addr = readl(mfc_regs->e_encoded_source_first_plane_addr);
+	*c_addr = readl(mfc_regs->e_encoded_source_second_plane_addr);
 
-	enc_recon_y_addr = READL(mfc_regs->e_recon_luma_dpb_addr);
-	enc_recon_c_addr = READL(mfc_regs->e_recon_chroma_dpb_addr);
+	enc_recon_y_addr = readl(mfc_regs->e_recon_luma_dpb_addr);
+	enc_recon_c_addr = readl(mfc_regs->e_recon_chroma_dpb_addr);
 
 	mfc_debug(2, "recon y addr: 0x%08lx\n", enc_recon_y_addr);
 	mfc_debug(2, "recon c addr: 0x%08lx\n", enc_recon_c_addr);
@@ -577,36 +572,36 @@ static int s5p_mfc_set_enc_ref_buffer_v6(struct s5p_mfc_ctx *ctx)
 	mfc_debug(2, "Buf1: %p (%d)\n", (void *)buf_addr1, buf_size1);
 
 	for (i = 0; i < ctx->pb_count; i++) {
-		WRITEL(buf_addr1, mfc_regs->e_luma_dpb + (4 * i));
+		writel(buf_addr1, mfc_regs->e_luma_dpb + (4 * i));
 		buf_addr1 += ctx->luma_dpb_size;
-		WRITEL(buf_addr1, mfc_regs->e_chroma_dpb + (4 * i));
+		writel(buf_addr1, mfc_regs->e_chroma_dpb + (4 * i));
 		buf_addr1 += ctx->chroma_dpb_size;
-		WRITEL(buf_addr1, mfc_regs->e_me_buffer + (4 * i));
+		writel(buf_addr1, mfc_regs->e_me_buffer + (4 * i));
 		buf_addr1 += ctx->me_buffer_size;
 		buf_size1 -= (ctx->luma_dpb_size + ctx->chroma_dpb_size +
 			ctx->me_buffer_size);
 	}
 
-	WRITEL(buf_addr1, mfc_regs->e_scratch_buffer_addr);
-	WRITEL(ctx->scratch_buf_size, mfc_regs->e_scratch_buffer_size);
+	writel(buf_addr1, mfc_regs->e_scratch_buffer_addr);
+	writel(ctx->scratch_buf_size, mfc_regs->e_scratch_buffer_size);
 	buf_addr1 += ctx->scratch_buf_size;
 	buf_size1 -= ctx->scratch_buf_size;
 
-	WRITEL(buf_addr1, mfc_regs->e_tmv_buffer0);
+	writel(buf_addr1, mfc_regs->e_tmv_buffer0);
 	buf_addr1 += ctx->tmv_buffer_size >> 1;
-	WRITEL(buf_addr1, mfc_regs->e_tmv_buffer1);
+	writel(buf_addr1, mfc_regs->e_tmv_buffer1);
 	buf_addr1 += ctx->tmv_buffer_size >> 1;
 	buf_size1 -= ctx->tmv_buffer_size;
 
-	mfc_debug(2, "Buf1: %u, buf_size1: %d (ref frames %d)\n",
+	mfc_debug(2, "Buf1: %zu, buf_size1: %d (ref frames %d)\n",
 			buf_addr1, buf_size1, ctx->pb_count);
 	if (buf_size1 < 0) {
 		mfc_debug(2, "Not enough memory has been allocated.\n");
 		return -ENOMEM;
 	}
 
-	WRITEL(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_INIT_BUFS_V6, NULL);
 
 	mfc_debug_leave();
@@ -621,15 +616,15 @@ static int s5p_mfc_set_slice_mode(struct s5p_mfc_ctx *ctx)
 
 	/* multi-slice control */
 	/* multi-slice MB number or bit size */
-	WRITEL(ctx->slice_mode, mfc_regs->e_mslice_mode);
+	writel(ctx->slice_mode, mfc_regs->e_mslice_mode);
 	if (ctx->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
-		WRITEL(ctx->slice_size.mb, mfc_regs->e_mslice_size_mb);
+		writel(ctx->slice_size.mb, mfc_regs->e_mslice_size_mb);
 	} else if (ctx->slice_mode ==
 			V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
-		WRITEL(ctx->slice_size.bits, mfc_regs->e_mslice_size_bits);
+		writel(ctx->slice_size.bits, mfc_regs->e_mslice_size_bits);
 	} else {
-		WRITEL(0x0, mfc_regs->e_mslice_size_mb);
-		WRITEL(0x0, mfc_regs->e_mslice_size_bits);
+		writel(0x0, mfc_regs->e_mslice_size_mb);
+		writel(0x0, mfc_regs->e_mslice_size_bits);
 	}
 
 	return 0;
@@ -645,21 +640,21 @@ static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
 	mfc_debug_enter();
 
 	/* width */
-	WRITEL(ctx->img_width, mfc_regs->e_frame_width); /* 16 align */
+	writel(ctx->img_width, mfc_regs->e_frame_width); /* 16 align */
 	/* height */
-	WRITEL(ctx->img_height, mfc_regs->e_frame_height); /* 16 align */
+	writel(ctx->img_height, mfc_regs->e_frame_height); /* 16 align */
 
 	/* cropped width */
-	WRITEL(ctx->img_width, mfc_regs->e_cropped_frame_width);
+	writel(ctx->img_width, mfc_regs->e_cropped_frame_width);
 	/* cropped height */
-	WRITEL(ctx->img_height, mfc_regs->e_cropped_frame_height);
+	writel(ctx->img_height, mfc_regs->e_cropped_frame_height);
 	/* cropped offset */
-	WRITEL(0x0, mfc_regs->e_frame_crop_offset);
+	writel(0x0, mfc_regs->e_frame_crop_offset);
 
 	/* pictype : IDR period */
 	reg = 0;
 	reg |= p->gop_size & 0xFFFF;
-	WRITEL(reg, mfc_regs->e_gop_config);
+	writel(reg, mfc_regs->e_gop_config);
 
 	/* multi-slice control */
 	/* multi-slice MB number or bit size */
@@ -667,65 +662,65 @@ static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
 	reg = 0;
 	if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_MB) {
 		reg |= (0x1 << 3);
-		WRITEL(reg, mfc_regs->e_enc_options);
+		writel(reg, mfc_regs->e_enc_options);
 		ctx->slice_size.mb = p->slice_mb;
 	} else if (p->slice_mode == V4L2_MPEG_VIDEO_MULTI_SICE_MODE_MAX_BYTES) {
 		reg |= (0x1 << 3);
-		WRITEL(reg, mfc_regs->e_enc_options);
+		writel(reg, mfc_regs->e_enc_options);
 		ctx->slice_size.bits = p->slice_bit;
 	} else {
 		reg &= ~(0x1 << 3);
-		WRITEL(reg, mfc_regs->e_enc_options);
+		writel(reg, mfc_regs->e_enc_options);
 	}
 
 	s5p_mfc_set_slice_mode(ctx);
 
 	/* cyclic intra refresh */
-	WRITEL(p->intra_refresh_mb, mfc_regs->e_ir_size);
-	reg = READL(mfc_regs->e_enc_options);
+	writel(p->intra_refresh_mb, mfc_regs->e_ir_size);
+	reg = readl(mfc_regs->e_enc_options);
 	if (p->intra_refresh_mb == 0)
 		reg &= ~(0x1 << 4);
 	else
 		reg |= (0x1 << 4);
-	WRITEL(reg, mfc_regs->e_enc_options);
+	writel(reg, mfc_regs->e_enc_options);
 
 	/* 'NON_REFERENCE_STORE_ENABLE' for debugging */
-	reg = READL(mfc_regs->e_enc_options);
+	reg = readl(mfc_regs->e_enc_options);
 	reg &= ~(0x1 << 9);
-	WRITEL(reg, mfc_regs->e_enc_options);
+	writel(reg, mfc_regs->e_enc_options);
 
 	/* memory structure cur. frame */
 	if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12M) {
 		/* 0: Linear, 1: 2D tiled*/
-		reg = READL(mfc_regs->e_enc_options);
+		reg = readl(mfc_regs->e_enc_options);
 		reg &= ~(0x1 << 7);
-		WRITEL(reg, mfc_regs->e_enc_options);
+		writel(reg, mfc_regs->e_enc_options);
 		/* 0: NV12(CbCr), 1: NV21(CrCb) */
-		WRITEL(0x0, mfc_regs->pixel_format);
+		writel(0x0, mfc_regs->pixel_format);
 	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV21M) {
 		/* 0: Linear, 1: 2D tiled*/
-		reg = READL(mfc_regs->e_enc_options);
+		reg = readl(mfc_regs->e_enc_options);
 		reg &= ~(0x1 << 7);
-		WRITEL(reg, mfc_regs->e_enc_options);
+		writel(reg, mfc_regs->e_enc_options);
 		/* 0: NV12(CbCr), 1: NV21(CrCb) */
-		WRITEL(0x1, mfc_regs->pixel_format);
+		writel(0x1, mfc_regs->pixel_format);
 	} else if (ctx->src_fmt->fourcc == V4L2_PIX_FMT_NV12MT_16X16) {
 		/* 0: Linear, 1: 2D tiled*/
-		reg = READL(mfc_regs->e_enc_options);
+		reg = readl(mfc_regs->e_enc_options);
 		reg |= (0x1 << 7);
-		WRITEL(reg, mfc_regs->e_enc_options);
+		writel(reg, mfc_regs->e_enc_options);
 		/* 0: NV12(CbCr), 1: NV21(CrCb) */
-		WRITEL(0x0, mfc_regs->pixel_format);
+		writel(0x0, mfc_regs->pixel_format);
 	}
 
 	/* memory structure recon. frame */
 	/* 0: Linear, 1: 2D tiled */
-	reg = READL(mfc_regs->e_enc_options);
+	reg = readl(mfc_regs->e_enc_options);
 	reg |= (0x1 << 8);
-	WRITEL(reg, mfc_regs->e_enc_options);
+	writel(reg, mfc_regs->e_enc_options);
 
 	/* padding control & value */
-	WRITEL(0x0, mfc_regs->e_padding_ctrl);
+	writel(0x0, mfc_regs->e_padding_ctrl);
 	if (p->pad) {
 		reg = 0;
 		/** enable */
@@ -736,64 +731,64 @@ static int s5p_mfc_set_enc_params(struct s5p_mfc_ctx *ctx)
 		reg |= ((p->pad_cb & 0xFF) << 8);
 		/** y value */
 		reg |= p->pad_luma & 0xFF;
-		WRITEL(reg, mfc_regs->e_padding_ctrl);
+		writel(reg, mfc_regs->e_padding_ctrl);
 	}
 
 	/* rate control config. */
 	reg = 0;
 	/* frame-level rate control */
 	reg |= ((p->rc_frame & 0x1) << 9);
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/* bit rate */
 	if (p->rc_frame)
-		WRITEL(p->rc_bitrate,
+		writel(p->rc_bitrate,
 			mfc_regs->e_rc_bit_rate);
 	else
-		WRITEL(1, mfc_regs->e_rc_bit_rate);
+		writel(1, mfc_regs->e_rc_bit_rate);
 
 	/* reaction coefficient */
 	if (p->rc_frame) {
 		if (p->rc_reaction_coeff < TIGHT_CBR_MAX) /* tight CBR */
-			WRITEL(1, mfc_regs->e_rc_mode);
+			writel(1, mfc_regs->e_rc_mode);
 		else					  /* loose CBR */
-			WRITEL(2, mfc_regs->e_rc_mode);
+			writel(2, mfc_regs->e_rc_mode);
 	}
 
 	/* seq header ctrl */
-	reg = READL(mfc_regs->e_enc_options);
+	reg = readl(mfc_regs->e_enc_options);
 	reg &= ~(0x1 << 2);
 	reg |= ((p->seq_hdr_mode & 0x1) << 2);
 
 	/* frame skip mode */
 	reg &= ~(0x3);
 	reg |= (p->frame_skip_mode & 0x3);
-	WRITEL(reg, mfc_regs->e_enc_options);
+	writel(reg, mfc_regs->e_enc_options);
 
 	/* 'DROP_CONTROL_ENABLE', disable */
-	reg = READL(mfc_regs->e_rc_config);
+	reg = readl(mfc_regs->e_rc_config);
 	reg &= ~(0x1 << 10);
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/* setting for MV range [16, 256] */
 	reg = (p->mv_h_range & S5P_FIMV_E_MV_RANGE_V6_MASK);
-	WRITEL(reg, mfc_regs->e_mv_hor_range);
+	writel(reg, mfc_regs->e_mv_hor_range);
 
 	reg = (p->mv_v_range & S5P_FIMV_E_MV_RANGE_V6_MASK);
-	WRITEL(reg, mfc_regs->e_mv_ver_range);
+	writel(reg, mfc_regs->e_mv_ver_range);
 
-	WRITEL(0x0, mfc_regs->e_frame_insertion);
-	WRITEL(0x0, mfc_regs->e_roi_buffer_addr);
-	WRITEL(0x0, mfc_regs->e_param_change);
-	WRITEL(0x0, mfc_regs->e_rc_roi_ctrl);
-	WRITEL(0x0, mfc_regs->e_picture_tag);
+	writel(0x0, mfc_regs->e_frame_insertion);
+	writel(0x0, mfc_regs->e_roi_buffer_addr);
+	writel(0x0, mfc_regs->e_param_change);
+	writel(0x0, mfc_regs->e_rc_roi_ctrl);
+	writel(0x0, mfc_regs->e_picture_tag);
 
-	WRITEL(0x0, mfc_regs->e_bit_count_enable);
-	WRITEL(0x0, mfc_regs->e_max_bit_count);
-	WRITEL(0x0, mfc_regs->e_min_bit_count);
+	writel(0x0, mfc_regs->e_bit_count_enable);
+	writel(0x0, mfc_regs->e_max_bit_count);
+	writel(0x0, mfc_regs->e_min_bit_count);
 
-	WRITEL(0x0, mfc_regs->e_metadata_buffer_addr);
-	WRITEL(0x0, mfc_regs->e_metadata_buffer_size);
+	writel(0x0, mfc_regs->e_metadata_buffer_addr);
+	writel(0x0, mfc_regs->e_metadata_buffer_size);
 
 	mfc_debug_leave();
 
@@ -814,10 +809,10 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 	s5p_mfc_set_enc_params(ctx);
 
 	/* pictype : number of B */
-	reg = READL(mfc_regs->e_gop_config);
+	reg = readl(mfc_regs->e_gop_config);
 	reg &= ~(0x3 << 16);
 	reg |= ((p->num_b_frame & 0x3) << 16);
-	WRITEL(reg, mfc_regs->e_gop_config);
+	writel(reg, mfc_regs->e_gop_config);
 
 	/* profile & level */
 	reg = 0;
@@ -825,19 +820,19 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 	reg |= ((p_h264->level & 0xFF) << 8);
 	/** profile - 0 ~ 3 */
 	reg |= p_h264->profile & 0x3F;
-	WRITEL(reg, mfc_regs->e_picture_profile);
+	writel(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(mfc_regs->e_rc_config);
+	reg = readl(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/** frame QP */
 	reg &= ~(0x3F);
 	reg |= p_h264->rc_frame_qp & 0x3F;
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/* max & min value of QP */
 	reg = 0;
@@ -845,16 +840,16 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 	reg |= ((p_h264->rc_max_qp & 0x3F) << 8);
 	/** min QP */
 	reg |= p_h264->rc_min_qp & 0x3F;
-	WRITEL(reg, mfc_regs->e_rc_qp_bound);
+	writel(reg, mfc_regs->e_rc_qp_bound);
 
 	/* other QPs */
-	WRITEL(0x0, mfc_regs->e_fixed_picture_qp);
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
 	if (!p->rc_frame && !p->rc_mb) {
 		reg = 0;
 		reg |= ((p_h264->rc_b_frame_qp & 0x3F) << 16);
 		reg |= ((p_h264->rc_p_frame_qp & 0x3F) << 8);
 		reg |= p_h264->rc_frame_qp & 0x3F;
-		WRITEL(reg, mfc_regs->e_fixed_picture_qp);
+		writel(reg, mfc_regs->e_fixed_picture_qp);
 	}
 
 	/* frame rate */
@@ -862,38 +857,38 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 		reg = 0;
 		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
 		reg |= p->rc_framerate_denom & 0xFFFF;
-		WRITEL(reg, mfc_regs->e_rc_frame_rate);
+		writel(reg, mfc_regs->e_rc_frame_rate);
 	}
 
 	/* vbv buffer size */
 	if (p->frame_skip_mode ==
 			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-		WRITEL(p_h264->cpb_size & 0xFFFF,
+		writel(p_h264->cpb_size & 0xFFFF,
 				mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	/* interlace */
 	reg = 0;
 	reg |= ((p_h264->interlace & 0x1) << 3);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* height */
 	if (p_h264->interlace) {
-		WRITEL(ctx->img_height >> 1,
+		writel(ctx->img_height >> 1,
 				mfc_regs->e_frame_height); /* 32 align */
 		/* cropped height */
-		WRITEL(ctx->img_height >> 1,
+		writel(ctx->img_height >> 1,
 				mfc_regs->e_cropped_frame_height);
 	}
 
 	/* loop filter ctrl */
-	reg = READL(mfc_regs->e_h264_options);
+	reg = readl(mfc_regs->e_h264_options);
 	reg &= ~(0x3 << 1);
 	reg |= ((p_h264->loop_filter_mode & 0x3) << 1);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* loopfilter alpha offset */
 	if (p_h264->loop_filter_alpha < 0) {
@@ -903,7 +898,7 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 		reg = 0x00;
 		reg |= (p_h264->loop_filter_alpha & 0xF);
 	}
-	WRITEL(reg, mfc_regs->e_h264_lf_alpha_offset);
+	writel(reg, mfc_regs->e_h264_lf_alpha_offset);
 
 	/* loopfilter beta offset */
 	if (p_h264->loop_filter_beta < 0) {
@@ -913,28 +908,28 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 		reg = 0x00;
 		reg |= (p_h264->loop_filter_beta & 0xF);
 	}
-	WRITEL(reg, mfc_regs->e_h264_lf_beta_offset);
+	writel(reg, mfc_regs->e_h264_lf_beta_offset);
 
 	/* entropy coding mode */
-	reg = READL(mfc_regs->e_h264_options);
+	reg = readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1);
 	reg |= p_h264->entropy_mode & 0x1;
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* number of ref. picture */
-	reg = READL(mfc_regs->e_h264_options);
+	reg = readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 7);
 	reg |= (((p_h264->num_ref_pic_4p - 1) & 0x1) << 7);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* 8x8 transform enable */
-	reg = READL(mfc_regs->e_h264_options);
+	reg = readl(mfc_regs->e_h264_options);
 	reg &= ~(0x3 << 12);
 	reg |= ((p_h264->_8x8_transform & 0x3) << 12);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* macroblock adaptive scaling features */
-	WRITEL(0x0, mfc_regs->e_mb_rc_config);
+	writel(0x0, mfc_regs->e_mb_rc_config);
 	if (p->rc_mb) {
 		reg = 0;
 		/** dark region */
@@ -945,95 +940,95 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 		reg |= ((p_h264->rc_mb_static & 0x1) << 1);
 		/** high activity region */
 		reg |= p_h264->rc_mb_activity & 0x1;
-		WRITEL(reg, mfc_regs->e_mb_rc_config);
+		writel(reg, mfc_regs->e_mb_rc_config);
 	}
 
 	/* aspect ratio VUI */
-	READL(mfc_regs->e_h264_options);
+	readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 5);
 	reg |= ((p_h264->vui_sar & 0x1) << 5);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
-	WRITEL(0x0, mfc_regs->e_aspect_ratio);
-	WRITEL(0x0, mfc_regs->e_extended_sar);
+	writel(0x0, mfc_regs->e_aspect_ratio);
+	writel(0x0, mfc_regs->e_extended_sar);
 	if (p_h264->vui_sar) {
 		/* aspect ration IDC */
 		reg = 0;
 		reg |= p_h264->vui_sar_idc & 0xFF;
-		WRITEL(reg, mfc_regs->e_aspect_ratio);
+		writel(reg, mfc_regs->e_aspect_ratio);
 		if (p_h264->vui_sar_idc == 0xFF) {
 			/* extended SAR */
 			reg = 0;
 			reg |= (p_h264->vui_ext_sar_width & 0xFFFF) << 16;
 			reg |= p_h264->vui_ext_sar_height & 0xFFFF;
-			WRITEL(reg, mfc_regs->e_extended_sar);
+			writel(reg, mfc_regs->e_extended_sar);
 		}
 	}
 
 	/* intra picture period for H.264 open GOP */
 	/* control */
-	READL(mfc_regs->e_h264_options);
+	readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 4);
 	reg |= ((p_h264->open_gop & 0x1) << 4);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* value */
-	WRITEL(0x0, mfc_regs->e_h264_i_period);
+	writel(0x0, mfc_regs->e_h264_i_period);
 	if (p_h264->open_gop) {
 		reg = 0;
 		reg |= p_h264->open_gop_size & 0xFFFF;
-		WRITEL(reg, mfc_regs->e_h264_i_period);
+		writel(reg, mfc_regs->e_h264_i_period);
 	}
 
 	/* 'WEIGHTED_BI_PREDICTION' for B is disable */
-	READL(mfc_regs->e_h264_options);
+	readl(mfc_regs->e_h264_options);
 	reg &= ~(0x3 << 9);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* 'CONSTRAINED_INTRA_PRED_ENABLE' is disable */
-	READL(mfc_regs->e_h264_options);
+	readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 14);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* ASO */
-	READL(mfc_regs->e_h264_options);
+	readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 6);
 	reg |= ((p_h264->aso & 0x1) << 6);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 
 	/* hier qp enable */
-	READL(mfc_regs->e_h264_options);
+	readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 8);
 	reg |= ((p_h264->open_gop & 0x1) << 8);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 	reg = 0;
 	if (p_h264->hier_qp && p_h264->hier_qp_layer) {
 		reg |= (p_h264->hier_qp_type & 0x1) << 0x3;
 		reg |= p_h264->hier_qp_layer & 0x7;
-		WRITEL(reg, mfc_regs->e_h264_num_t_layer);
+		writel(reg, mfc_regs->e_h264_num_t_layer);
 		/* QP value for each layer */
 		for (i = 0; i < p_h264->hier_qp_layer &&
 				i < ARRAY_SIZE(p_h264->hier_qp_layer_qp); i++) {
-			WRITEL(p_h264->hier_qp_layer_qp[i],
+			writel(p_h264->hier_qp_layer_qp[i],
 				mfc_regs->e_h264_hierarchical_qp_layer0
 				+ i * 4);
 		}
 	}
 	/* number of coding layer should be zero when hierarchical is disable */
-	WRITEL(reg, mfc_regs->e_h264_num_t_layer);
+	writel(reg, mfc_regs->e_h264_num_t_layer);
 
 	/* frame packing SEI generation */
-	READL(mfc_regs->e_h264_options);
+	readl(mfc_regs->e_h264_options);
 	reg &= ~(0x1 << 25);
 	reg |= ((p_h264->sei_frame_packing & 0x1) << 25);
-	WRITEL(reg, mfc_regs->e_h264_options);
+	writel(reg, mfc_regs->e_h264_options);
 	if (p_h264->sei_frame_packing) {
 		reg = 0;
 		/** current frame0 flag */
 		reg |= ((p_h264->sei_fp_curr_frame_0 & 0x1) << 2);
 		/** arrangement type */
 		reg |= p_h264->sei_fp_arrangement_type & 0x3;
-		WRITEL(reg, mfc_regs->e_h264_frame_packing_sei_info);
+		writel(reg, mfc_regs->e_h264_frame_packing_sei_info);
 	}
 
 	if (p_h264->fmo) {
@@ -1042,7 +1037,7 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 			if (p_h264->fmo_slice_grp > 4)
 				p_h264->fmo_slice_grp = 4;
 			for (i = 0; i < (p_h264->fmo_slice_grp & 0xF); i++)
-				WRITEL(p_h264->fmo_run_len[i] - 1,
+				writel(p_h264->fmo_run_len[i] - 1,
 					mfc_regs->e_h264_fmo_run_length_minus1_0
 					+ i * 4);
 			break;
@@ -1054,10 +1049,10 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 		case V4L2_MPEG_VIDEO_H264_FMO_MAP_TYPE_WIPE_SCAN:
 			if (p_h264->fmo_slice_grp > 2)
 				p_h264->fmo_slice_grp = 2;
-			WRITEL(p_h264->fmo_chg_dir & 0x1,
+			writel(p_h264->fmo_chg_dir & 0x1,
 				mfc_regs->e_h264_fmo_slice_grp_change_dir);
 			/* the valid range is 0 ~ number of macroblocks -1 */
-			WRITEL(p_h264->fmo_chg_rate,
+			writel(p_h264->fmo_chg_rate,
 			mfc_regs->e_h264_fmo_slice_grp_change_rate_minus1);
 			break;
 		default:
@@ -1068,12 +1063,12 @@ static int s5p_mfc_set_enc_params_h264(struct s5p_mfc_ctx *ctx)
 			break;
 		}
 
-		WRITEL(p_h264->fmo_map_type,
+		writel(p_h264->fmo_map_type,
 				mfc_regs->e_h264_fmo_slice_grp_map_type);
-		WRITEL(p_h264->fmo_slice_grp - 1,
+		writel(p_h264->fmo_slice_grp - 1,
 				mfc_regs->e_h264_fmo_num_slice_grp_minus1);
 	} else {
-		WRITEL(0, mfc_regs->e_h264_fmo_num_slice_grp_minus1);
+		writel(0, mfc_regs->e_h264_fmo_num_slice_grp_minus1);
 	}
 
 	mfc_debug_leave();
@@ -1094,10 +1089,10 @@ static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
 	s5p_mfc_set_enc_params(ctx);
 
 	/* pictype : number of B */
-	reg = READL(mfc_regs->e_gop_config);
+	reg = readl(mfc_regs->e_gop_config);
 	reg &= ~(0x3 << 16);
 	reg |= ((p->num_b_frame & 0x3) << 16);
-	WRITEL(reg, mfc_regs->e_gop_config);
+	writel(reg, mfc_regs->e_gop_config);
 
 	/* profile & level */
 	reg = 0;
@@ -1105,19 +1100,19 @@ static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
 	reg |= ((p_mpeg4->level & 0xFF) << 8);
 	/** profile - 0 ~ 1 */
 	reg |= p_mpeg4->profile & 0x3F;
-	WRITEL(reg, mfc_regs->e_picture_profile);
+	writel(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(mfc_regs->e_rc_config);
+	reg = readl(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/** frame QP */
 	reg &= ~(0x3F);
 	reg |= p_mpeg4->rc_frame_qp & 0x3F;
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/* max & min value of QP */
 	reg = 0;
@@ -1125,16 +1120,16 @@ static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
 	reg |= ((p_mpeg4->rc_max_qp & 0x3F) << 8);
 	/** min QP */
 	reg |= p_mpeg4->rc_min_qp & 0x3F;
-	WRITEL(reg, mfc_regs->e_rc_qp_bound);
+	writel(reg, mfc_regs->e_rc_qp_bound);
 
 	/* other QPs */
-	WRITEL(0x0, mfc_regs->e_fixed_picture_qp);
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
 	if (!p->rc_frame && !p->rc_mb) {
 		reg = 0;
 		reg |= ((p_mpeg4->rc_b_frame_qp & 0x3F) << 16);
 		reg |= ((p_mpeg4->rc_p_frame_qp & 0x3F) << 8);
 		reg |= p_mpeg4->rc_frame_qp & 0x3F;
-		WRITEL(reg, mfc_regs->e_fixed_picture_qp);
+		writel(reg, mfc_regs->e_fixed_picture_qp);
 	}
 
 	/* frame rate */
@@ -1142,21 +1137,21 @@ static int s5p_mfc_set_enc_params_mpeg4(struct s5p_mfc_ctx *ctx)
 		reg = 0;
 		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
 		reg |= p->rc_framerate_denom & 0xFFFF;
-		WRITEL(reg, mfc_regs->e_rc_frame_rate);
+		writel(reg, mfc_regs->e_rc_frame_rate);
 	}
 
 	/* vbv buffer size */
 	if (p->frame_skip_mode ==
 			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-		WRITEL(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
+		writel(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	/* Disable HEC */
-	WRITEL(0x0, mfc_regs->e_mpeg4_options);
-	WRITEL(0x0, mfc_regs->e_mpeg4_hec_period);
+	writel(0x0, mfc_regs->e_mpeg4_options);
+	writel(0x0, mfc_regs->e_mpeg4_hec_period);
 
 	mfc_debug_leave();
 
@@ -1179,19 +1174,19 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 	reg = 0;
 	/** profile */
 	reg |= (0x1 << 4);
-	WRITEL(reg, mfc_regs->e_picture_profile);
+	writel(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(mfc_regs->e_rc_config);
+	reg = readl(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/** frame QP */
 	reg &= ~(0x3F);
 	reg |= p_h263->rc_frame_qp & 0x3F;
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/* max & min value of QP */
 	reg = 0;
@@ -1199,16 +1194,16 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 	reg |= ((p_h263->rc_max_qp & 0x3F) << 8);
 	/** min QP */
 	reg |= p_h263->rc_min_qp & 0x3F;
-	WRITEL(reg, mfc_regs->e_rc_qp_bound);
+	writel(reg, mfc_regs->e_rc_qp_bound);
 
 	/* other QPs */
-	WRITEL(0x0, mfc_regs->e_fixed_picture_qp);
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
 	if (!p->rc_frame && !p->rc_mb) {
 		reg = 0;
 		reg |= ((p_h263->rc_b_frame_qp & 0x3F) << 16);
 		reg |= ((p_h263->rc_p_frame_qp & 0x3F) << 8);
 		reg |= p_h263->rc_frame_qp & 0x3F;
-		WRITEL(reg, mfc_regs->e_fixed_picture_qp);
+		writel(reg, mfc_regs->e_fixed_picture_qp);
 	}
 
 	/* frame rate */
@@ -1216,16 +1211,16 @@ static int s5p_mfc_set_enc_params_h263(struct s5p_mfc_ctx *ctx)
 		reg = 0;
 		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
 		reg |= p->rc_framerate_denom & 0xFFFF;
-		WRITEL(reg, mfc_regs->e_rc_frame_rate);
+		writel(reg, mfc_regs->e_rc_frame_rate);
 	}
 
 	/* vbv buffer size */
 	if (p->frame_skip_mode ==
 			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-		WRITEL(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
+		writel(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	mfc_debug_leave();
@@ -1247,57 +1242,57 @@ static int s5p_mfc_set_enc_params_vp8(struct s5p_mfc_ctx *ctx)
 	s5p_mfc_set_enc_params(ctx);
 
 	/* pictype : number of B */
-	reg = READL(mfc_regs->e_gop_config);
+	reg = readl(mfc_regs->e_gop_config);
 	reg &= ~(0x3 << 16);
 	reg |= ((p->num_b_frame & 0x3) << 16);
-	WRITEL(reg, mfc_regs->e_gop_config);
+	writel(reg, mfc_regs->e_gop_config);
 
 	/* profile - 0 ~ 3 */
 	reg = p_vp8->profile & 0x3;
-	WRITEL(reg, mfc_regs->e_picture_profile);
+	writel(reg, mfc_regs->e_picture_profile);
 
 	/* rate control config. */
-	reg = READL(mfc_regs->e_rc_config);
+	reg = readl(mfc_regs->e_rc_config);
 	/** macroblock level rate control */
 	reg &= ~(0x1 << 8);
 	reg |= ((p->rc_mb & 0x1) << 8);
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/* frame rate */
 	if (p->rc_frame && p->rc_framerate_num && p->rc_framerate_denom) {
 		reg = 0;
 		reg |= ((p->rc_framerate_num & 0xFFFF) << 16);
 		reg |= p->rc_framerate_denom & 0xFFFF;
-		WRITEL(reg, mfc_regs->e_rc_frame_rate);
+		writel(reg, mfc_regs->e_rc_frame_rate);
 	}
 
 	/* frame QP */
 	reg &= ~(0x7F);
 	reg |= p_vp8->rc_frame_qp & 0x7F;
-	WRITEL(reg, mfc_regs->e_rc_config);
+	writel(reg, mfc_regs->e_rc_config);
 
 	/* other QPs */
-	WRITEL(0x0, mfc_regs->e_fixed_picture_qp);
+	writel(0x0, mfc_regs->e_fixed_picture_qp);
 	if (!p->rc_frame && !p->rc_mb) {
 		reg = 0;
 		reg |= ((p_vp8->rc_p_frame_qp & 0x7F) << 8);
 		reg |= p_vp8->rc_frame_qp & 0x7F;
-		WRITEL(reg, mfc_regs->e_fixed_picture_qp);
+		writel(reg, mfc_regs->e_fixed_picture_qp);
 	}
 
 	/* max QP */
 	reg = ((p_vp8->rc_max_qp & 0x7F) << 8);
 	/* min QP */
 	reg |= p_vp8->rc_min_qp & 0x7F;
-	WRITEL(reg, mfc_regs->e_rc_qp_bound);
+	writel(reg, mfc_regs->e_rc_qp_bound);
 
 	/* vbv buffer size */
 	if (p->frame_skip_mode ==
 			V4L2_MPEG_MFC51_VIDEO_FRAME_SKIP_MODE_BUF_LIMIT) {
-		WRITEL(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
+		writel(p->vbv_size & 0xFFFF, mfc_regs->e_vbv_buffer_size);
 
 		if (p->rc_frame)
-			WRITEL(p->vbv_delay, mfc_regs->e_vbv_init_delay);
+			writel(p->vbv_delay, mfc_regs->e_vbv_init_delay);
 	}
 
 	/* VP8 specific params */
@@ -1319,7 +1314,7 @@ static int s5p_mfc_set_enc_params_vp8(struct s5p_mfc_ctx *ctx)
 	}
 	reg |= (val & 0xF) << 3;
 	reg |= (p_vp8->num_ref & 0x2);
-	WRITEL(reg, mfc_regs->e_vp8_options);
+	writel(reg, mfc_regs->e_vp8_options);
 
 	mfc_debug_leave();
 
@@ -1338,9 +1333,9 @@ static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 	mfc_debug(2, "InstNo: %d/%d\n", ctx->inst_no,
 			S5P_FIMV_CH_SEQ_HEADER_V6);
 	mfc_debug(2, "BUFs: %08x %08x %08x\n",
-		  READL(mfc_regs->d_cpb_buffer_addr),
-		  READL(mfc_regs->d_cpb_buffer_addr),
-		  READL(mfc_regs->d_cpb_buffer_addr));
+		  readl(mfc_regs->d_cpb_buffer_addr),
+		  readl(mfc_regs->d_cpb_buffer_addr),
+		  readl(mfc_regs->d_cpb_buffer_addr));
 
 	/* FMO_ASO_CTRL - 0: Enable, 1: Disable */
 	reg |= (fmo_aso_ctrl << S5P_FIMV_D_OPT_FMO_ASO_CTRL_MASK_V6);
@@ -1351,11 +1346,11 @@ static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 	 * set to negative value. */
 	if (ctx->display_delay >= 0) {
 		reg |= (0x1 << S5P_FIMV_D_OPT_DDELAY_EN_SHIFT_V6);
-		WRITEL(ctx->display_delay, mfc_regs->d_display_delay);
+		writel(ctx->display_delay, mfc_regs->d_display_delay);
 	}
 
 	if (IS_MFCV7_PLUS(dev) || IS_MFCV6_V2(dev)) {
-		WRITEL(reg, mfc_regs->d_dec_options);
+		writel(reg, mfc_regs->d_dec_options);
 		reg = 0;
 	}
 
@@ -1370,22 +1365,22 @@ static int s5p_mfc_init_decode_v6(struct s5p_mfc_ctx *ctx)
 		reg |= (0x1 << S5P_FIMV_D_OPT_TILE_MODE_SHIFT_V6);
 
 	if (IS_MFCV7_PLUS(dev) || IS_MFCV6_V2(dev))
-		WRITEL(reg, mfc_regs->d_init_buffer_options);
+		writel(reg, mfc_regs->d_init_buffer_options);
 	else
-		WRITEL(reg, mfc_regs->d_dec_options);
+		writel(reg, mfc_regs->d_dec_options);
 
 	/* 0: NV12(CbCr), 1: NV21(CrCb) */
 	if (ctx->dst_fmt->fourcc == V4L2_PIX_FMT_NV21M)
-		WRITEL(0x1, mfc_regs->pixel_format);
+		writel(0x1, mfc_regs->pixel_format);
 	else
-		WRITEL(0x0, mfc_regs->pixel_format);
+		writel(0x0, mfc_regs->pixel_format);
 
 
 	/* sei parse */
-	WRITEL(ctx->sei_fp_parse & 0x1, mfc_regs->d_sei_enable);
+	writel(ctx->sei_fp_parse & 0x1, mfc_regs->d_sei_enable);
 
-	WRITEL(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
 
 	mfc_debug_leave();
@@ -1400,8 +1395,8 @@ static inline void s5p_mfc_set_flush(struct s5p_mfc_ctx *ctx, int flush)
 	if (flush) {
 		dev->curr_ctx = ctx->num;
 		s5p_mfc_clean_ctx_int_flags(ctx);
-		WRITEL(ctx->inst_no, mfc_regs->instance_id);
-		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+		writel(ctx->inst_no, mfc_regs->instance_id);
+		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 				S5P_FIMV_H2R_CMD_FLUSH_V6, NULL);
 	}
 }
@@ -1413,19 +1408,19 @@ static int s5p_mfc_decode_one_frame_v6(struct s5p_mfc_ctx *ctx,
 	struct s5p_mfc_dev *dev = ctx->dev;
 	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
 
-	WRITEL(ctx->dec_dst_flag, mfc_regs->d_available_dpb_flag_lower);
-	WRITEL(ctx->slice_interface & 0x1, mfc_regs->d_slice_if_enable);
+	writel(ctx->dec_dst_flag, mfc_regs->d_available_dpb_flag_lower);
+	writel(ctx->slice_interface & 0x1, mfc_regs->d_slice_if_enable);
 
-	WRITEL(ctx->inst_no, mfc_regs->instance_id);
+	writel(ctx->inst_no, mfc_regs->instance_id);
 	/* Issue different commands to instance basing on whether it
 	 * is the last frame or not. */
 	switch (last_frame) {
 	case 0:
-		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 				S5P_FIMV_CH_FRAME_START_V6, NULL);
 		break;
 	case 1:
-		s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+		s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 				S5P_FIMV_CH_LAST_FRAME_V6, NULL);
 		break;
 	default:
@@ -1458,12 +1453,12 @@ static int s5p_mfc_init_encode_v6(struct s5p_mfc_ctx *ctx)
 
 	/* Set stride lengths for v7 & above */
 	if (IS_MFCV7_PLUS(dev)) {
-		WRITEL(ctx->img_width, mfc_regs->e_source_first_plane_stride);
-		WRITEL(ctx->img_width, mfc_regs->e_source_second_plane_stride);
+		writel(ctx->img_width, mfc_regs->e_source_first_plane_stride);
+		writel(ctx->img_width, mfc_regs->e_source_second_plane_stride);
 	}
 
-	WRITEL(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_SEQ_HEADER_V6, NULL);
 
 	return 0;
@@ -1479,7 +1474,7 @@ static int s5p_mfc_h264_set_aso_slice_order_v6(struct s5p_mfc_ctx *ctx)
 
 	if (p_h264->aso) {
 		for (i = 0; i < ARRAY_SIZE(p_h264->aso_slice_order); i++) {
-			WRITEL(p_h264->aso_slice_order[i],
+			writel(p_h264->aso_slice_order[i],
 				mfc_regs->e_h264_aso_slice_order_0 + i * 4);
 		}
 	}
@@ -1501,8 +1496,8 @@ static int s5p_mfc_encode_one_frame_v6(struct s5p_mfc_ctx *ctx)
 
 	s5p_mfc_set_slice_mode(ctx);
 
-	WRITEL(ctx->inst_no, mfc_regs->instance_id);
-	s5p_mfc_hw_call(dev->mfc_cmds, cmd_host2risc, dev,
+	writel(ctx->inst_no, mfc_regs->instance_id);
+	s5p_mfc_hw_call_void(dev->mfc_cmds, cmd_host2risc, dev,
 			S5P_FIMV_CH_FRAME_START_V6, NULL);
 
 	mfc_debug(2, "--\n");
@@ -1877,15 +1872,15 @@ static void s5p_mfc_cleanup_queue_v6(struct list_head *lh, struct vb2_queue *vq)
 static void s5p_mfc_clear_int_flags_v6(struct s5p_mfc_dev *dev)
 {
 	const struct s5p_mfc_regs *mfc_regs = dev->mfc_regs;
-	WRITEL(0, mfc_regs->risc2host_command);
-	WRITEL(0, mfc_regs->risc2host_int);
+	writel(0, mfc_regs->risc2host_command);
+	writel(0, mfc_regs->risc2host_int);
 }
 
 static void s5p_mfc_write_info_v6(struct s5p_mfc_ctx *ctx, unsigned int data,
 		unsigned int ofs)
 {
 	s5p_mfc_clock_on();
-	WRITEL(data, (void *)ofs);
+	writel(data, (volatile void __iomem *)((unsigned long)ofs));
 	s5p_mfc_clock_off();
 }
 
@@ -1895,7 +1890,7 @@ s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
 	int ret;
 
 	s5p_mfc_clock_on();
-	ret = READL((void *)ofs);
+	ret = readl((volatile void __iomem *)((unsigned long)ofs));
 	s5p_mfc_clock_off();
 
 	return ret;
@@ -1903,51 +1898,51 @@ s5p_mfc_read_info_v6(struct s5p_mfc_ctx *ctx, unsigned int ofs)
 
 static int s5p_mfc_get_dspl_y_adr_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_display_first_plane_addr);
+	return readl(dev->mfc_regs->d_display_first_plane_addr);
 }
 
 static int s5p_mfc_get_dec_y_adr_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_decoded_first_plane_addr);
+	return readl(dev->mfc_regs->d_decoded_first_plane_addr);
 }
 
 static int s5p_mfc_get_dspl_status_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_display_status);
+	return readl(dev->mfc_regs->d_display_status);
 }
 
 static int s5p_mfc_get_dec_status_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_decoded_status);
+	return readl(dev->mfc_regs->d_decoded_status);
 }
 
 static int s5p_mfc_get_dec_frame_type_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_decoded_frame_type) &
+	return readl(dev->mfc_regs->d_decoded_frame_type) &
 		S5P_FIMV_DECODE_FRAME_MASK_V6;
 }
 
 static int s5p_mfc_get_disp_frame_type_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	return READL(dev->mfc_regs->d_display_frame_type) &
+	return readl(dev->mfc_regs->d_display_frame_type) &
 		S5P_FIMV_DECODE_FRAME_MASK_V6;
 }
 
 static int s5p_mfc_get_consumed_stream_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_decoded_nal_size);
+	return readl(dev->mfc_regs->d_decoded_nal_size);
 }
 
 static int s5p_mfc_get_int_reason_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->risc2host_command) &
+	return readl(dev->mfc_regs->risc2host_command) &
 		S5P_FIMV_RISC2HOST_CMD_MASK;
 }
 
 static int s5p_mfc_get_int_err_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->error_code);
+	return readl(dev->mfc_regs->error_code);
 }
 
 static int s5p_mfc_err_dec_v6(unsigned int err)
@@ -1962,87 +1957,87 @@ static int s5p_mfc_err_dspl_v6(unsigned int err)
 
 static int s5p_mfc_get_img_width_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_display_frame_width);
+	return readl(dev->mfc_regs->d_display_frame_width);
 }
 
 static int s5p_mfc_get_img_height_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_display_frame_height);
+	return readl(dev->mfc_regs->d_display_frame_height);
 }
 
 static int s5p_mfc_get_dpb_count_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_min_num_dpb);
+	return readl(dev->mfc_regs->d_min_num_dpb);
 }
 
 static int s5p_mfc_get_mv_count_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_min_num_mv);
+	return readl(dev->mfc_regs->d_min_num_mv);
 }
 
 static int s5p_mfc_get_inst_no_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->ret_instance_id);
+	return readl(dev->mfc_regs->ret_instance_id);
 }
 
 static int s5p_mfc_get_enc_dpb_count_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->e_num_dpb);
+	return readl(dev->mfc_regs->e_num_dpb);
 }
 
 static int s5p_mfc_get_enc_strm_size_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->e_stream_size);
+	return readl(dev->mfc_regs->e_stream_size);
 }
 
 static int s5p_mfc_get_enc_slice_type_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->e_slice_type);
+	return readl(dev->mfc_regs->e_slice_type);
 }
 
 static int s5p_mfc_get_enc_pic_count_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->e_picture_count);
+	return readl(dev->mfc_regs->e_picture_count);
 }
 
 static int s5p_mfc_get_sei_avail_status_v6(struct s5p_mfc_ctx *ctx)
 {
 	struct s5p_mfc_dev *dev = ctx->dev;
-	return READL(dev->mfc_regs->d_frame_pack_sei_avail);
+	return readl(dev->mfc_regs->d_frame_pack_sei_avail);
 }
 
 static int s5p_mfc_get_mvc_num_views_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_mvc_num_views);
+	return readl(dev->mfc_regs->d_mvc_num_views);
 }
 
 static int s5p_mfc_get_mvc_view_id_v6(struct s5p_mfc_dev *dev)
 {
-	return READL(dev->mfc_regs->d_mvc_view_id);
+	return readl(dev->mfc_regs->d_mvc_view_id);
 }
 
 static unsigned int s5p_mfc_get_pic_type_top_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx,
-		(unsigned int) ctx->dev->mfc_regs->d_ret_picture_tag_top);
+		(__force unsigned long) ctx->dev->mfc_regs->d_ret_picture_tag_top);
 }
 
 static unsigned int s5p_mfc_get_pic_type_bot_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx,
-		(unsigned int) ctx->dev->mfc_regs->d_ret_picture_tag_bot);
+		(__force unsigned long) ctx->dev->mfc_regs->d_ret_picture_tag_bot);
 }
 
 static unsigned int s5p_mfc_get_crop_info_h_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx,
-		(unsigned int) ctx->dev->mfc_regs->d_display_crop_info1);
+		(__force unsigned long) ctx->dev->mfc_regs->d_display_crop_info1);
 }
 
 static unsigned int s5p_mfc_get_crop_info_v_v6(struct s5p_mfc_ctx *ctx)
 {
 	return s5p_mfc_read_info_v6(ctx,
-		(unsigned int) ctx->dev->mfc_regs->d_display_crop_info2);
+		(__force unsigned long) ctx->dev->mfc_regs->d_display_crop_info2);
 }
 
 static struct s5p_mfc_regs mfc_regs;
diff --git a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
index b6a8be97a96c..826c48945bf5 100644
--- a/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
+++ b/drivers/media/platform/s5p-mfc/s5p_mfc_pm.c
@@ -21,7 +21,7 @@
 #include "s5p_mfc_pm.h"
 
 #define MFC_GATE_CLK_NAME	"mfc"
-#define MFC_SCLK_NAME		"sclk-mfc"
+#define MFC_SCLK_NAME		"sclk_mfc"
 #define MFC_SCLK_RATE		(200 * 1000000)
 
 #define CLK_DEBUG
diff --git a/drivers/media/platform/s5p-tv/Kconfig b/drivers/media/platform/s5p-tv/Kconfig
index 369a4c191e18..a9d56f8936b4 100644
--- a/drivers/media/platform/s5p-tv/Kconfig
+++ b/drivers/media/platform/s5p-tv/Kconfig
@@ -8,7 +8,8 @@
 
 config VIDEO_SAMSUNG_S5P_TV
 	bool "Samsung TV driver for S5P platform"
-	depends on (PLAT_S5P || ARCH_EXYNOS) && PM_RUNTIME
+	depends on PM_RUNTIME
+	depends on PLAT_S5P || ARCH_EXYNOS || COMPILE_TEST
 	default n
 	---help---
 	  Say Y here to enable selecting the TV output devices for
@@ -70,6 +71,7 @@ config VIDEO_SAMSUNG_S5P_MIXER
 	tristate "Samsung Mixer and Video Processor Driver"
 	depends on VIDEO_DEV && VIDEO_V4L2
 	depends on VIDEO_SAMSUNG_S5P_TV
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	  Say Y here if you want support for the Mixer in Samsung S5P SoCs.
diff --git a/drivers/media/platform/s5p-tv/hdmi_drv.c b/drivers/media/platform/s5p-tv/hdmi_drv.c
index 754740f4b671..37c8bd694c5f 100644
--- a/drivers/media/platform/s5p-tv/hdmi_drv.c
+++ b/drivers/media/platform/s5p-tv/hdmi_drv.c
@@ -615,7 +615,7 @@ static int hdmi_s_power(struct v4l2_subdev *sd, int on)
 	else
 		ret = pm_runtime_put_sync(hdev->dev);
 	/* only values < 0 indicate errors */
-	return IS_ERR_VALUE(ret) ? ret : 0;
+	return ret < 0 ? ret : 0;
 }
 
 static int hdmi_s_dv_timings(struct v4l2_subdev *sd,
diff --git a/drivers/media/platform/s5p-tv/sdo_drv.c b/drivers/media/platform/s5p-tv/sdo_drv.c
index 5a7c3796f22e..72cf892dd008 100644
--- a/drivers/media/platform/s5p-tv/sdo_drv.c
+++ b/drivers/media/platform/s5p-tv/sdo_drv.c
@@ -190,7 +190,7 @@ static int sdo_s_power(struct v4l2_subdev *sd, int on)
 		ret = pm_runtime_put_sync(dev);
 
 	/* only values < 0 indicate errors */
-	return IS_ERR_VALUE(ret) ? ret : 0;
+	return ret < 0 ? ret : 0;
 }
 
 static int sdo_streamon(struct sdo_device *sdev)
diff --git a/drivers/media/platform/s5p-tv/sii9234_drv.c b/drivers/media/platform/s5p-tv/sii9234_drv.c
index 3dd762e5b67e..db8c17bb4aaa 100644
--- a/drivers/media/platform/s5p-tv/sii9234_drv.c
+++ b/drivers/media/platform/s5p-tv/sii9234_drv.c
@@ -289,7 +289,7 @@ static int sii9234_s_power(struct v4l2_subdev *sd, int on)
 	else
 		ret = pm_runtime_put(&ctx->client->dev);
 	/* only values < 0 indicate errors */
-	return IS_ERR_VALUE(ret) ? ret : 0;
+	return ret < 0 ? ret : 0;
 }
 
 static int sii9234_s_stream(struct v4l2_subdev *sd, int enable)
diff --git a/drivers/media/platform/sh_veu.c b/drivers/media/platform/sh_veu.c
index 8dc279d4d561..be3b3bc71a0f 100644
--- a/drivers/media/platform/sh_veu.c
+++ b/drivers/media/platform/sh_veu.c
@@ -26,6 +26,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-mem2mem.h>
+#include <media/v4l2-image-sizes.h>
 #include <media/videobuf2-dma-contig.h>
 
 #define VEU_STR 0x00 /* start register */
@@ -135,9 +136,6 @@ enum sh_veu_fmt_idx {
 	SH_VEU_FMT_RGB24,
 };
 
-#define VGA_WIDTH	640
-#define VGA_HEIGHT	480
-
 #define DEFAULT_IN_WIDTH	VGA_WIDTH
 #define DEFAULT_IN_HEIGHT	VGA_HEIGHT
 #define DEFAULT_IN_FMTIDX	SH_VEU_FMT_NV12
diff --git a/drivers/media/platform/soc_camera/Kconfig b/drivers/media/platform/soc_camera/Kconfig
index 6540847f4e1d..f2776cd415ca 100644
--- a/drivers/media/platform/soc_camera/Kconfig
+++ b/drivers/media/platform/soc_camera/Kconfig
@@ -20,6 +20,8 @@ config SOC_CAMERA_PLATFORM
 config VIDEO_MX3
 	tristate "i.MX3x Camera Sensor Interface driver"
 	depends on VIDEO_DEV && MX3_IPU && SOC_CAMERA
+	depends on MX3_IPU || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  This is a v4l2 driver for the i.MX3x Camera Sensor Interface
@@ -35,6 +37,7 @@ config VIDEO_RCAR_VIN
 	tristate "R-Car Video Input (VIN) support"
 	depends on VIDEO_DEV && SOC_CAMERA
 	depends on ARCH_SHMOBILE || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select SOC_CAMERA_SCALE_CROP
 	---help---
@@ -51,6 +54,7 @@ config VIDEO_SH_MOBILE_CEU
 	tristate "SuperH Mobile CEU Interface driver"
 	depends on VIDEO_DEV && SOC_CAMERA && HAS_DMA && HAVE_CLK
 	depends on ARCH_SHMOBILE || SUPERH || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	select SOC_CAMERA_SCALE_CROP
 	---help---
@@ -58,7 +62,9 @@ config VIDEO_SH_MOBILE_CEU
 
 config VIDEO_OMAP1
 	tristate "OMAP1 Camera Interface driver"
-	depends on VIDEO_DEV && ARCH_OMAP1 && SOC_CAMERA
+	depends on VIDEO_DEV && SOC_CAMERA
+	depends on ARCH_OMAP1
+	depends on HAS_DMA
 	select VIDEOBUF_DMA_CONTIG
 	select VIDEOBUF_DMA_SG
 	---help---
@@ -66,14 +72,18 @@ config VIDEO_OMAP1
 
 config VIDEO_MX2
 	tristate "i.MX27 Camera Sensor Interface driver"
-	depends on VIDEO_DEV && SOC_CAMERA && SOC_IMX27
+	depends on VIDEO_DEV && SOC_CAMERA
+	depends on SOC_IMX27 || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  This is a v4l2 driver for the i.MX27 Camera Sensor Interface
 
 config VIDEO_ATMEL_ISI
 	tristate "ATMEL Image Sensor Interface (ISI) support"
-	depends on VIDEO_DEV && SOC_CAMERA && ARCH_AT91
+	depends on VIDEO_DEV && SOC_CAMERA
+	depends on ARCH_AT91 || COMPILE_TEST
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  This module makes the ATMEL Image Sensor Interface available
diff --git a/drivers/media/platform/soc_camera/atmel-isi.c b/drivers/media/platform/soc_camera/atmel-isi.c
index 3408b045b3f1..c5291b001057 100644
--- a/drivers/media/platform/soc_camera/atmel-isi.c
+++ b/drivers/media/platform/soc_camera/atmel-isi.c
@@ -54,7 +54,7 @@ static void set_dma_ctrl(struct fbd *fb_desc, u32 ctrl)
 struct isi_dma_desc {
 	struct list_head list;
 	struct fbd *p_fbd;
-	u32 fbd_phys;
+	dma_addr_t fbd_phys;
 };
 
 /* Frame buffer data */
@@ -75,7 +75,7 @@ struct atmel_isi {
 
 	/* Allocate descriptors for dma buffer use */
 	struct fbd			*p_fb_descriptors;
-	u32				fb_descriptors_phys;
+	dma_addr_t			fb_descriptors_phys;
 	struct				list_head dma_desc_head;
 	struct isi_dma_desc		dma_desc[MAX_BUFFER_NUM];
 
@@ -169,7 +169,7 @@ static irqreturn_t atmel_isi_handle_streaming(struct atmel_isi *isi)
 		isi->active = list_entry(isi->video_buffer_list.next,
 					struct frame_buffer, list);
 		isi_writel(isi, ISI_DMA_C_DSCR,
-			isi->active->p_dma_desc->fbd_phys);
+			(u32)isi->active->p_dma_desc->fbd_phys);
 		isi_writel(isi, ISI_DMA_C_CTRL,
 			ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
 		isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
@@ -346,7 +346,7 @@ static void start_dma(struct atmel_isi *isi, struct frame_buffer *buffer)
 		return;
 	}
 
-	isi_writel(isi, ISI_DMA_C_DSCR, buffer->p_dma_desc->fbd_phys);
+	isi_writel(isi, ISI_DMA_C_DSCR, (u32)buffer->p_dma_desc->fbd_phys);
 	isi_writel(isi, ISI_DMA_C_CTRL, ISI_DMA_CTRL_FETCH | ISI_DMA_CTRL_DONE);
 	isi_writel(isi, ISI_DMA_CHER, ISI_DMA_CHSR_C_CH);
 
@@ -384,7 +384,6 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 	struct soc_camera_device *icd = soc_camera_from_vb2q(vq);
 	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
 	struct atmel_isi *isi = ici->priv;
-	u32 sr = 0;
 	int ret;
 
 	/* Reset ISI */
@@ -394,11 +393,11 @@ static int start_streaming(struct vb2_queue *vq, unsigned int count)
 		return ret;
 	}
 	/* Disable all interrupts */
-	isi_writel(isi, ISI_INTDIS, ~0UL);
+	isi_writel(isi, ISI_INTDIS, (u32)~0UL);
 
 	spin_lock_irq(&isi->lock);
 	/* Clear any pending interrupt */
-	sr = isi_readl(isi, ISI_STATUS);
+	isi_readl(isi, ISI_STATUS);
 
 	if (count)
 		start_dma(isi, isi->active);
diff --git a/drivers/media/platform/soc_camera/mx2_camera.c b/drivers/media/platform/soc_camera/mx2_camera.c
index b40bc2e5ba47..2347612a4cc1 100644
--- a/drivers/media/platform/soc_camera/mx2_camera.c
+++ b/drivers/media/platform/soc_camera/mx2_camera.c
@@ -809,10 +809,9 @@ static int mx2_camera_init_videobuf(struct vb2_queue *q,
 
 static int mx27_camera_emma_prp_reset(struct mx2_camera_dev *pcdev)
 {
-	u32 cntl;
 	int count = 0;
 
-	cntl = readl(pcdev->base_emma + PRP_CNTL);
+	readl(pcdev->base_emma + PRP_CNTL);
 	writel(PRP_CNTL_SWRST, pcdev->base_emma + PRP_CNTL);
 	while (count++ < 100) {
 		if (!(readl(pcdev->base_emma + PRP_CNTL) & PRP_CNTL_SWRST))
@@ -1003,7 +1002,7 @@ static int mx2_emmaprp_resize(struct mx2_camera_dev *pcdev,
 			      struct v4l2_mbus_framefmt *mf_in,
 			      struct v4l2_pix_format *pix_out, bool apply)
 {
-	int num, den;
+	unsigned int num, den;
 	unsigned long m;
 	int i, dir;
 
diff --git a/drivers/media/platform/soc_camera/mx3_camera.c b/drivers/media/platform/soc_camera/mx3_camera.c
index 83315dfeef62..7696a873510d 100644
--- a/drivers/media/platform/soc_camera/mx3_camera.c
+++ b/drivers/media/platform/soc_camera/mx3_camera.c
@@ -415,10 +415,8 @@ static void mx3_stop_streaming(struct vb2_queue *q)
 	struct mx3_camera_buffer *buf, *tmp;
 	unsigned long flags;
 
-	if (ichan) {
-		struct dma_chan *chan = &ichan->dma_chan;
-		chan->device->device_control(chan, DMA_PAUSE, 0);
-	}
+	if (ichan)
+		dmaengine_pause(&ichan->dma_chan);
 
 	spin_lock_irqsave(&mx3_cam->lock, flags);
 
diff --git a/drivers/media/platform/soc_camera/pxa_camera.c b/drivers/media/platform/soc_camera/pxa_camera.c
index 64dc80ccd6f9..66178fc9f9eb 100644
--- a/drivers/media/platform/soc_camera/pxa_camera.c
+++ b/drivers/media/platform/soc_camera/pxa_camera.c
@@ -1694,7 +1694,7 @@ static int pxa_camera_pdata_from_dt(struct device *dev,
 		break;
 	default:
 		break;
-	};
+	}
 
 	if (ep.bus.parallel.flags & V4L2_MBUS_MASTER)
 		pcdev->platform_flags |= PXA_CAMERA_MASTER;
diff --git a/drivers/media/platform/soc_camera/rcar_vin.c b/drivers/media/platform/soc_camera/rcar_vin.c
index 85d579f65f52..20defcb8b31b 100644
--- a/drivers/media/platform/soc_camera/rcar_vin.c
+++ b/drivers/media/platform/soc_camera/rcar_vin.c
@@ -981,7 +981,7 @@ static int rcar_vin_get_formats(struct soc_camera_device *icd, unsigned int idx,
 
 		if (shift == 3) {
 			dev_err(dev,
-				"Failed to configure the client below %ux%x\n",
+				"Failed to configure the client below %ux%u\n",
 				mf.width, mf.height);
 			return -EIO;
 		}
@@ -1502,7 +1502,7 @@ static int rcar_vin_probe(struct platform_device *pdev)
 	} else {
 		priv->ici.nr = of_alias_get_id(pdev->dev.of_node, "vin");
 		priv->chip = (enum chip_id)match->data;
-	};
+	}
 
 	spin_lock_init(&priv->lock);
 	INIT_LIST_HEAD(&priv->capture);
diff --git a/drivers/media/platform/soc_camera/soc_camera.c b/drivers/media/platform/soc_camera/soc_camera.c
index f4308fed5431..8e61b976da19 100644
--- a/drivers/media/platform/soc_camera/soc_camera.c
+++ b/drivers/media/platform/soc_camera/soc_camera.c
@@ -437,6 +437,22 @@ static int soc_camera_prepare_buf(struct file *file, void *priv,
 		return vb2_prepare_buf(&icd->vb2_vidq, b);
 }
 
+static int soc_camera_expbuf(struct file *file, void *priv,
+			     struct v4l2_exportbuffer *p)
+{
+	struct soc_camera_device *icd = file->private_data;
+	struct soc_camera_host *ici = to_soc_camera_host(icd->parent);
+
+	if (icd->streamer != file)
+		return -EBUSY;
+
+	/* videobuf2 only */
+	if (ici->ops->init_videobuf)
+		return -EINVAL;
+	else
+		return vb2_expbuf(&icd->vb2_vidq, p);
+}
+
 /* Always entered with .host_lock held */
 static int soc_camera_init_user_formats(struct soc_camera_device *icd)
 {
@@ -1347,13 +1363,11 @@ static int soc_camera_i2c_init(struct soc_camera_device *icd,
 		return -ENODEV;
 	}
 
-	ssdd = kzalloc(sizeof(*ssdd), GFP_KERNEL);
+	ssdd = kmemdup(&sdesc->subdev_desc, sizeof(*ssdd), GFP_KERNEL);
 	if (!ssdd) {
 		ret = -ENOMEM;
 		goto ealloc;
 	}
-
-	memcpy(ssdd, &sdesc->subdev_desc, sizeof(*ssdd));
 	/*
 	 * In synchronous case we request regulators ourselves in
 	 * soc_camera_pdrv_probe(), make sure the subdevice driver doesn't try
@@ -2085,6 +2099,7 @@ static const struct v4l2_ioctl_ops soc_camera_ioctl_ops = {
 	.vidioc_dqbuf		 = soc_camera_dqbuf,
 	.vidioc_create_bufs	 = soc_camera_create_bufs,
 	.vidioc_prepare_buf	 = soc_camera_prepare_buf,
+	.vidioc_expbuf		 = soc_camera_expbuf,
 	.vidioc_streamon	 = soc_camera_streamon,
 	.vidioc_streamoff	 = soc_camera_streamoff,
 	.vidioc_cropcap		 = soc_camera_cropcap,
diff --git a/drivers/media/platform/ti-vpe/vpdma.c b/drivers/media/platform/ti-vpe/vpdma.c
index a51a01359805..3e2e3a33e6ed 100644
--- a/drivers/media/platform/ti-vpe/vpdma.c
+++ b/drivers/media/platform/ti-vpe/vpdma.c
@@ -329,7 +329,7 @@ int vpdma_alloc_desc_buf(struct vpdma_buf *buf, size_t size)
 	if (!buf->addr)
 		return -ENOMEM;
 
-	WARN_ON((u32) buf->addr & VPDMA_DESC_ALIGN);
+	WARN_ON(((unsigned long)buf->addr & VPDMA_DESC_ALIGN) != 0);
 
 	return 0;
 }
@@ -584,7 +584,7 @@ static void dump_dtd(struct vpdma_dtd *dtd)
 		pr_debug("word1: line_length = %d, xfer_height = %d\n",
 			dtd_get_line_length(dtd), dtd_get_xfer_height(dtd));
 
-	pr_debug("word2: start_addr = 0x%08x\n", dtd->start_addr);
+	pr_debug("word2: start_addr = %pad\n", &dtd->start_addr);
 
 	pr_debug("word3: pkt_type = %d, mode = %d, dir = %d, chan = %d, "
 		"pri = %d, next_chan = %d\n", dtd_get_pkt_type(dtd),
diff --git a/drivers/media/platform/ti-vpe/vpe.c b/drivers/media/platform/ti-vpe/vpe.c
index 972f43f69206..9a081c291159 100644
--- a/drivers/media/platform/ti-vpe/vpe.c
+++ b/drivers/media/platform/ti-vpe/vpe.c
@@ -31,6 +31,7 @@
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <linux/log2.h>
+#include <linux/sizes.h>
 
 #include <media/v4l2-common.h>
 #include <media/v4l2-ctrls.h>
@@ -138,12 +139,12 @@ struct vpe_dei_regs {
  * default expert DEI register values, unlikely to be modified.
  */
 static const struct vpe_dei_regs dei_regs = {
-	0x020C0804u,
-	0x0118100Fu,
-	0x08040200u,
-	0x1010100Cu,
-	0x10101010u,
-	0x10101010u,
+	.mdt_spacial_freq_thr_reg = 0x020C0804u,
+	.edi_config_reg = 0x0118100Fu,
+	.edi_lut_reg0 = 0x08040200u,
+	.edi_lut_reg1 = 0x1010100Cu,
+	.edi_lut_reg2 = 0x10101010u,
+	.edi_lut_reg3 = 0x10101010u,
 };
 
 /*
@@ -834,10 +835,10 @@ static int set_srcdst_params(struct vpe_ctx *ctx)
 					VPDMA_STRIDE_ALIGN);
 		mv_buf_size = bytes_per_line * s_q_data->height;
 
-		ctx->deinterlacing = 1;
+		ctx->deinterlacing = true;
 		src_h <<= 1;
 	} else {
-		ctx->deinterlacing = 0;
+		ctx->deinterlacing = false;
 		mv_buf_size = 0;
 	}
 
@@ -2343,8 +2344,7 @@ v4l2_dev_unreg:
 
 static int vpe_remove(struct platform_device *pdev)
 {
-	struct vpe_dev *dev =
-		(struct vpe_dev *) platform_get_drvdata(pdev);
+	struct vpe_dev *dev = platform_get_drvdata(pdev);
 
 	v4l2_info(&dev->v4l2_dev, "Removing " VPE_MODULE_NAME);
 
diff --git a/drivers/media/platform/via-camera.c b/drivers/media/platform/via-camera.c
index b4f9d03636e3..ae6870cb8339 100644
--- a/drivers/media/platform/via-camera.c
+++ b/drivers/media/platform/via-camera.c
@@ -18,6 +18,7 @@
 #include <media/v4l2-device.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-ctrls.h>
+#include <media/v4l2-image-sizes.h>
 #include <media/ov7670.h>
 #include <media/videobuf-dma-sg.h>
 #include <linux/delay.h>
@@ -49,14 +50,6 @@ MODULE_PARM_DESC(override_serial,
 		"to force-enable the camera.");
 
 /*
- * Basic window sizes.
- */
-#define VGA_WIDTH	640
-#define VGA_HEIGHT	480
-#define QCIF_WIDTH	176
-#define	QCIF_HEIGHT	144
-
-/*
  * The structure describing our camera.
  */
 enum viacam_opstate { S_IDLE = 0, S_RUNNING = 1 };
@@ -89,7 +82,7 @@ struct via_camera {
 	 * live in frame buffer memory, so we don't call them "DMA".
 	 */
 	unsigned int cb_offsets[3];	/* offsets into fb mem */
-	u8 *cb_addrs[3];		/* Kernel-space addresses */
+	u8 __iomem *cb_addrs[3];		/* Kernel-space addresses */
 	int n_cap_bufs;			/* How many are we using? */
 	int next_buf;
 	struct videobuf_queue vb_queue;
@@ -1283,7 +1276,7 @@ static bool viacam_serial_is_enabled(void)
 			VIACAM_SERIAL_CREG, &cbyte);
 	if ((cbyte & VIACAM_SERIAL_BIT) == 0)
 		return false; /* Not enabled */
-	if (override_serial == 0) {
+	if (!override_serial) {
 		printk(KERN_NOTICE "Via camera: serial port is enabled, " \
 				"refusing to load.\n");
 		printk(KERN_NOTICE "Specify override_serial=1 to force " \
diff --git a/drivers/media/platform/vivi.c b/drivers/media/platform/vivi.c
deleted file mode 100644
index 80333714ffa7..000000000000
--- a/drivers/media/platform/vivi.c
+++ /dev/null
@@ -1,1542 +0,0 @@
-/*
- * Virtual Video driver - This code emulates a real video device with v4l2 api
- *
- * Copyright (c) 2006 by:
- *      Mauro Carvalho Chehab <mchehab--a.t--infradead.org>
- *      Ted Walther <ted--a.t--enumera.com>
- *      John Sokol <sokol--a.t--videotechnology.com>
- *      http://v4l.videotechnology.com/
- *
- *      Conversion to videobuf2 by Pawel Osciak & Marek Szyprowski
- *      Copyright (c) 2010 Samsung Electronics
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the BSD Licence, GNU General Public License
- * as published by the Free Software Foundation; either version 2 of the
- * License, or (at your option) any later version
- */
-#include <linux/module.h>
-#include <linux/errno.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/sched.h>
-#include <linux/slab.h>
-#include <linux/font.h>
-#include <linux/mutex.h>
-#include <linux/videodev2.h>
-#include <linux/kthread.h>
-#include <linux/freezer.h>
-#include <media/videobuf2-vmalloc.h>
-#include <media/v4l2-device.h>
-#include <media/v4l2-ioctl.h>
-#include <media/v4l2-ctrls.h>
-#include <media/v4l2-fh.h>
-#include <media/v4l2-event.h>
-#include <media/v4l2-common.h>
-
-#define VIVI_MODULE_NAME "vivi"
-
-/* Maximum allowed frame rate
- *
- * Vivi will allow setting timeperframe in [1/FPS_MAX - FPS_MAX/1] range.
- *
- * Ideally FPS_MAX should be infinity, i.e. practically UINT_MAX, but that
- * might hit application errors when they manipulate these values.
- *
- * Besides, for tpf < 1ms image-generation logic should be changed, to avoid
- * producing frames with equal content.
- */
-#define FPS_MAX 1000
-
-#define MAX_WIDTH 1920
-#define MAX_HEIGHT 1200
-
-#define VIVI_VERSION "0.8.1"
-
-MODULE_DESCRIPTION("Video Technology Magazine Virtual Video Capture Board");
-MODULE_AUTHOR("Mauro Carvalho Chehab, Ted Walther and John Sokol");
-MODULE_LICENSE("Dual BSD/GPL");
-MODULE_VERSION(VIVI_VERSION);
-
-static unsigned video_nr = -1;
-module_param(video_nr, uint, 0644);
-MODULE_PARM_DESC(video_nr, "videoX start number, -1 is autodetect");
-
-static unsigned n_devs = 1;
-module_param(n_devs, uint, 0644);
-MODULE_PARM_DESC(n_devs, "number of video devices to create");
-
-static unsigned debug;
-module_param(debug, uint, 0644);
-MODULE_PARM_DESC(debug, "activates debug info");
-
-/* Global font descriptor */
-static const u8 *font8x16;
-
-/* timeperframe: min/max and default */
-static const struct v4l2_fract
-	tpf_min     = {.numerator = 1,		.denominator = FPS_MAX},
-	tpf_max     = {.numerator = FPS_MAX,	.denominator = 1},
-	tpf_default = {.numerator = 1001,	.denominator = 30000};	/* NTSC */
-
-#define dprintk(dev, level, fmt, arg...) \
-	v4l2_dbg(level, debug, &dev->v4l2_dev, fmt, ## arg)
-
-/* ------------------------------------------------------------------
-	Basic structures
-   ------------------------------------------------------------------*/
-
-struct vivi_fmt {
-	const char *name;
-	u32   fourcc;          /* v4l2 format id */
-	u8    depth;
-	bool  is_yuv;
-};
-
-static const struct vivi_fmt formats[] = {
-	{
-		.name     = "4:2:2, packed, YUYV",
-		.fourcc   = V4L2_PIX_FMT_YUYV,
-		.depth    = 16,
-		.is_yuv   = true,
-	},
-	{
-		.name     = "4:2:2, packed, UYVY",
-		.fourcc   = V4L2_PIX_FMT_UYVY,
-		.depth    = 16,
-		.is_yuv   = true,
-	},
-	{
-		.name     = "4:2:2, packed, YVYU",
-		.fourcc   = V4L2_PIX_FMT_YVYU,
-		.depth    = 16,
-		.is_yuv   = true,
-	},
-	{
-		.name     = "4:2:2, packed, VYUY",
-		.fourcc   = V4L2_PIX_FMT_VYUY,
-		.depth    = 16,
-		.is_yuv   = true,
-	},
-	{
-		.name     = "RGB565 (LE)",
-		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
-		.depth    = 16,
-	},
-	{
-		.name     = "RGB565 (BE)",
-		.fourcc   = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
-		.depth    = 16,
-	},
-	{
-		.name     = "RGB555 (LE)",
-		.fourcc   = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */
-		.depth    = 16,
-	},
-	{
-		.name     = "RGB555 (BE)",
-		.fourcc   = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
-		.depth    = 16,
-	},
-	{
-		.name     = "RGB24 (LE)",
-		.fourcc   = V4L2_PIX_FMT_RGB24, /* rgb */
-		.depth    = 24,
-	},
-	{
-		.name     = "RGB24 (BE)",
-		.fourcc   = V4L2_PIX_FMT_BGR24, /* bgr */
-		.depth    = 24,
-	},
-	{
-		.name     = "RGB32 (LE)",
-		.fourcc   = V4L2_PIX_FMT_RGB32, /* argb */
-		.depth    = 32,
-	},
-	{
-		.name     = "RGB32 (BE)",
-		.fourcc   = V4L2_PIX_FMT_BGR32, /* bgra */
-		.depth    = 32,
-	},
-};
-
-static const struct vivi_fmt *__get_format(u32 pixelformat)
-{
-	const struct vivi_fmt *fmt;
-	unsigned int k;
-
-	for (k = 0; k < ARRAY_SIZE(formats); k++) {
-		fmt = &formats[k];
-		if (fmt->fourcc == pixelformat)
-			break;
-	}
-
-	if (k == ARRAY_SIZE(formats))
-		return NULL;
-
-	return &formats[k];
-}
-
-static const struct vivi_fmt *get_format(struct v4l2_format *f)
-{
-	return __get_format(f->fmt.pix.pixelformat);
-}
-
-/* buffer for one video frame */
-struct vivi_buffer {
-	/* common v4l buffer stuff -- must be first */
-	struct vb2_buffer	vb;
-	struct list_head	list;
-};
-
-struct vivi_dmaqueue {
-	struct list_head       active;
-
-	/* thread for generating video stream*/
-	struct task_struct         *kthread;
-	wait_queue_head_t          wq;
-	/* Counters to control fps rate */
-	int                        frame;
-	int                        ini_jiffies;
-};
-
-static LIST_HEAD(vivi_devlist);
-
-struct vivi_dev {
-	struct list_head           vivi_devlist;
-	struct v4l2_device 	   v4l2_dev;
-	struct v4l2_ctrl_handler   ctrl_handler;
-	struct video_device	   vdev;
-
-	/* controls */
-	struct v4l2_ctrl	   *brightness;
-	struct v4l2_ctrl	   *contrast;
-	struct v4l2_ctrl	   *saturation;
-	struct v4l2_ctrl	   *hue;
-	struct {
-		/* autogain/gain cluster */
-		struct v4l2_ctrl	   *autogain;
-		struct v4l2_ctrl	   *gain;
-	};
-	struct v4l2_ctrl	   *volume;
-	struct v4l2_ctrl	   *alpha;
-	struct v4l2_ctrl	   *button;
-	struct v4l2_ctrl	   *boolean;
-	struct v4l2_ctrl	   *int32;
-	struct v4l2_ctrl	   *int64;
-	struct v4l2_ctrl	   *menu;
-	struct v4l2_ctrl	   *string;
-	struct v4l2_ctrl	   *bitmask;
-	struct v4l2_ctrl	   *int_menu;
-
-	spinlock_t                 slock;
-	struct mutex		   mutex;
-
-	struct vivi_dmaqueue       vidq;
-
-	/* Several counters */
-	unsigned 		   ms;
-	unsigned long              jiffies;
-	unsigned		   button_pressed;
-
-	int			   mv_count;	/* Controls bars movement */
-
-	/* Input Number */
-	int			   input;
-
-	/* video capture */
-	const struct vivi_fmt      *fmt;
-	struct v4l2_fract          timeperframe;
-	unsigned int               width, height;
-	struct vb2_queue	   vb_vidq;
-	unsigned int		   seq_count;
-
-	u8			   bars[9][3];
-	u8			   line[MAX_WIDTH * 8] __attribute__((__aligned__(4)));
-	unsigned int		   pixelsize;
-	u8			   alpha_component;
-	u32			   textfg, textbg;
-};
-
-/* ------------------------------------------------------------------
-	DMA and thread functions
-   ------------------------------------------------------------------*/
-
-/* Bars and Colors should match positions */
-
-enum colors {
-	WHITE,
-	AMBER,
-	CYAN,
-	GREEN,
-	MAGENTA,
-	RED,
-	BLUE,
-	BLACK,
-	TEXT_BLACK,
-};
-
-/* R   G   B */
-#define COLOR_WHITE	{204, 204, 204}
-#define COLOR_AMBER	{208, 208,   0}
-#define COLOR_CYAN	{  0, 206, 206}
-#define	COLOR_GREEN	{  0, 239,   0}
-#define COLOR_MAGENTA	{239,   0, 239}
-#define COLOR_RED	{205,   0,   0}
-#define COLOR_BLUE	{  0,   0, 255}
-#define COLOR_BLACK	{  0,   0,   0}
-
-struct bar_std {
-	u8 bar[9][3];
-};
-
-/* Maximum number of bars are 10 - otherwise, the input print code
-   should be modified */
-static const struct bar_std bars[] = {
-	{	/* Standard ITU-R color bar sequence */
-		{ COLOR_WHITE, COLOR_AMBER, COLOR_CYAN, COLOR_GREEN,
-		  COLOR_MAGENTA, COLOR_RED, COLOR_BLUE, COLOR_BLACK, COLOR_BLACK }
-	}, {
-		{ COLOR_WHITE, COLOR_AMBER, COLOR_BLACK, COLOR_WHITE,
-		  COLOR_AMBER, COLOR_BLACK, COLOR_WHITE, COLOR_AMBER, COLOR_BLACK }
-	}, {
-		{ COLOR_WHITE, COLOR_CYAN, COLOR_BLACK, COLOR_WHITE,
-		  COLOR_CYAN, COLOR_BLACK, COLOR_WHITE, COLOR_CYAN, COLOR_BLACK }
-	}, {
-		{ COLOR_WHITE, COLOR_GREEN, COLOR_BLACK, COLOR_WHITE,
-		  COLOR_GREEN, COLOR_BLACK, COLOR_WHITE, COLOR_GREEN, COLOR_BLACK }
-	},
-};
-
-#define NUM_INPUTS ARRAY_SIZE(bars)
-
-#define TO_Y(r, g, b) \
-	(((16829 * r + 33039 * g + 6416 * b  + 32768) >> 16) + 16)
-/* RGB to  V(Cr) Color transform */
-#define TO_V(r, g, b) \
-	(((28784 * r - 24103 * g - 4681 * b  + 32768) >> 16) + 128)
-/* RGB to  U(Cb) Color transform */
-#define TO_U(r, g, b) \
-	(((-9714 * r - 19070 * g + 28784 * b + 32768) >> 16) + 128)
-
-/* precalculate color bar values to speed up rendering */
-static void precalculate_bars(struct vivi_dev *dev)
-{
-	u8 r, g, b;
-	int k, is_yuv;
-
-	for (k = 0; k < 9; k++) {
-		r = bars[dev->input].bar[k][0];
-		g = bars[dev->input].bar[k][1];
-		b = bars[dev->input].bar[k][2];
-		is_yuv = dev->fmt->is_yuv;
-
-		switch (dev->fmt->fourcc) {
-		case V4L2_PIX_FMT_RGB565:
-		case V4L2_PIX_FMT_RGB565X:
-			r >>= 3;
-			g >>= 2;
-			b >>= 3;
-			break;
-		case V4L2_PIX_FMT_RGB555:
-		case V4L2_PIX_FMT_RGB555X:
-			r >>= 3;
-			g >>= 3;
-			b >>= 3;
-			break;
-		case V4L2_PIX_FMT_YUYV:
-		case V4L2_PIX_FMT_UYVY:
-		case V4L2_PIX_FMT_YVYU:
-		case V4L2_PIX_FMT_VYUY:
-		case V4L2_PIX_FMT_RGB24:
-		case V4L2_PIX_FMT_BGR24:
-		case V4L2_PIX_FMT_RGB32:
-		case V4L2_PIX_FMT_BGR32:
-			break;
-		}
-
-		if (is_yuv) {
-			dev->bars[k][0] = TO_Y(r, g, b);	/* Luma */
-			dev->bars[k][1] = TO_U(r, g, b);	/* Cb */
-			dev->bars[k][2] = TO_V(r, g, b);	/* Cr */
-		} else {
-			dev->bars[k][0] = r;
-			dev->bars[k][1] = g;
-			dev->bars[k][2] = b;
-		}
-	}
-}
-
-/* 'odd' is true for pixels 1, 3, 5, etc. and false for pixels 0, 2, 4, etc. */
-static void gen_twopix(struct vivi_dev *dev, u8 *buf, int colorpos, bool odd)
-{
-	u8 r_y, g_u, b_v;
-	u8 alpha = dev->alpha_component;
-	int color;
-	u8 *p;
-
-	r_y = dev->bars[colorpos][0]; /* R or precalculated Y */
-	g_u = dev->bars[colorpos][1]; /* G or precalculated U */
-	b_v = dev->bars[colorpos][2]; /* B or precalculated V */
-
-	for (color = 0; color < dev->pixelsize; color++) {
-		p = buf + color;
-
-		switch (dev->fmt->fourcc) {
-		case V4L2_PIX_FMT_YUYV:
-			switch (color) {
-			case 0:
-				*p = r_y;
-				break;
-			case 1:
-				*p = odd ? b_v : g_u;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_UYVY:
-			switch (color) {
-			case 0:
-				*p = odd ? b_v : g_u;
-				break;
-			case 1:
-				*p = r_y;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_YVYU:
-			switch (color) {
-			case 0:
-				*p = r_y;
-				break;
-			case 1:
-				*p = odd ? g_u : b_v;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_VYUY:
-			switch (color) {
-			case 0:
-				*p = odd ? g_u : b_v;
-				break;
-			case 1:
-				*p = r_y;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_RGB565:
-			switch (color) {
-			case 0:
-				*p = (g_u << 5) | b_v;
-				break;
-			case 1:
-				*p = (r_y << 3) | (g_u >> 3);
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_RGB565X:
-			switch (color) {
-			case 0:
-				*p = (r_y << 3) | (g_u >> 3);
-				break;
-			case 1:
-				*p = (g_u << 5) | b_v;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_RGB555:
-			switch (color) {
-			case 0:
-				*p = (g_u << 5) | b_v;
-				break;
-			case 1:
-				*p = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_RGB555X:
-			switch (color) {
-			case 0:
-				*p = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
-				break;
-			case 1:
-				*p = (g_u << 5) | b_v;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_RGB24:
-			switch (color) {
-			case 0:
-				*p = r_y;
-				break;
-			case 1:
-				*p = g_u;
-				break;
-			case 2:
-				*p = b_v;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_BGR24:
-			switch (color) {
-			case 0:
-				*p = b_v;
-				break;
-			case 1:
-				*p = g_u;
-				break;
-			case 2:
-				*p = r_y;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_RGB32:
-			switch (color) {
-			case 0:
-				*p = alpha;
-				break;
-			case 1:
-				*p = r_y;
-				break;
-			case 2:
-				*p = g_u;
-				break;
-			case 3:
-				*p = b_v;
-				break;
-			}
-			break;
-		case V4L2_PIX_FMT_BGR32:
-			switch (color) {
-			case 0:
-				*p = b_v;
-				break;
-			case 1:
-				*p = g_u;
-				break;
-			case 2:
-				*p = r_y;
-				break;
-			case 3:
-				*p = alpha;
-				break;
-			}
-			break;
-		}
-	}
-}
-
-static void precalculate_line(struct vivi_dev *dev)
-{
-	unsigned pixsize  = dev->pixelsize;
-	unsigned pixsize2 = 2*pixsize;
-	int colorpos;
-	u8 *pos;
-
-	for (colorpos = 0; colorpos < 16; ++colorpos) {
-		u8 pix[8];
-		int wstart =  colorpos    * dev->width / 8;
-		int wend   = (colorpos+1) * dev->width / 8;
-		int w;
-
-		gen_twopix(dev, &pix[0],        colorpos % 8, 0);
-		gen_twopix(dev, &pix[pixsize],  colorpos % 8, 1);
-
-		for (w = wstart/2*2, pos = dev->line + w*pixsize; w < wend; w += 2, pos += pixsize2)
-			memcpy(pos, pix, pixsize2);
-	}
-}
-
-/* need this to do rgb24 rendering */
-typedef struct { u16 __; u8 _; } __attribute__((packed)) x24;
-
-static void gen_text(struct vivi_dev *dev, char *basep,
-					int y, int x, char *text)
-{
-	int line;
-	unsigned int width = dev->width;
-
-	/* Checks if it is possible to show string */
-	if (y + 16 >= dev->height || x + strlen(text) * 8 >= width)
-		return;
-
-	/* Print stream time */
-#define PRINTSTR(PIXTYPE) do {	\
-	PIXTYPE fg;	\
-	PIXTYPE bg;	\
-	memcpy(&fg, &dev->textfg, sizeof(PIXTYPE));	\
-	memcpy(&bg, &dev->textbg, sizeof(PIXTYPE));	\
-	\
-	for (line = 0; line < 16; line++) {	\
-		PIXTYPE *pos = (PIXTYPE *)( basep + ((y + line) * width + x) * sizeof(PIXTYPE) );	\
-		u8 *s;	\
-	\
-		for (s = text; *s; s++) {	\
-			u8 chr = font8x16[*s * 16 + line];	\
-	\
-			pos[0] = (chr & (0x01 << 7) ? fg : bg);	\
-			pos[1] = (chr & (0x01 << 6) ? fg : bg);	\
-			pos[2] = (chr & (0x01 << 5) ? fg : bg);	\
-			pos[3] = (chr & (0x01 << 4) ? fg : bg);	\
-			pos[4] = (chr & (0x01 << 3) ? fg : bg);	\
-			pos[5] = (chr & (0x01 << 2) ? fg : bg);	\
-			pos[6] = (chr & (0x01 << 1) ? fg : bg);	\
-			pos[7] = (chr & (0x01 << 0) ? fg : bg);	\
-	\
-			pos += 8;	\
-		}	\
-	}	\
-} while (0)
-
-	switch (dev->pixelsize) {
-	case 2:
-		PRINTSTR(u16); break;
-	case 4:
-		PRINTSTR(u32); break;
-	case 3:
-		PRINTSTR(x24); break;
-	}
-}
-
-static void vivi_fillbuff(struct vivi_dev *dev, struct vivi_buffer *buf)
-{
-	int stride = dev->width * dev->pixelsize;
-	int hmax = dev->height;
-	void *vbuf = vb2_plane_vaddr(&buf->vb, 0);
-	unsigned ms;
-	char str[100];
-	int h, line = 1;
-	u8 *linestart;
-	s32 gain;
-
-	if (!vbuf)
-		return;
-
-	linestart = dev->line + (dev->mv_count % dev->width) * dev->pixelsize;
-
-	for (h = 0; h < hmax; h++)
-		memcpy(vbuf + h * stride, linestart, stride);
-
-	/* Updates stream time */
-
-	gen_twopix(dev, (u8 *)&dev->textbg, TEXT_BLACK, /*odd=*/ 0);
-	gen_twopix(dev, (u8 *)&dev->textfg, WHITE, /*odd=*/ 0);
-
-	dev->ms += jiffies_to_msecs(jiffies - dev->jiffies);
-	dev->jiffies = jiffies;
-	ms = dev->ms;
-	snprintf(str, sizeof(str), " %02d:%02d:%02d:%03d ",
-			(ms / (60 * 60 * 1000)) % 24,
-			(ms / (60 * 1000)) % 60,
-			(ms / 1000) % 60,
-			ms % 1000);
-	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " %dx%d, input %d ",
-			dev->width, dev->height, dev->input);
-	gen_text(dev, vbuf, line++ * 16, 16, str);
-
-	gain = v4l2_ctrl_g_ctrl(dev->gain);
-	mutex_lock(dev->ctrl_handler.lock);
-	snprintf(str, sizeof(str), " brightness %3d, contrast %3d, saturation %3d, hue %d ",
-			dev->brightness->cur.val,
-			dev->contrast->cur.val,
-			dev->saturation->cur.val,
-			dev->hue->cur.val);
-	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " autogain %d, gain %3d, volume %3d, alpha 0x%02x ",
-			dev->autogain->cur.val, gain, dev->volume->cur.val,
-			dev->alpha->cur.val);
-	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " int32 %d, int64 %lld, bitmask %08x ",
-			dev->int32->cur.val,
-			*dev->int64->p_cur.p_s64,
-			dev->bitmask->cur.val);
-	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " boolean %d, menu %s, string \"%s\" ",
-			dev->boolean->cur.val,
-			dev->menu->qmenu[dev->menu->cur.val],
-			dev->string->p_cur.p_char);
-	gen_text(dev, vbuf, line++ * 16, 16, str);
-	snprintf(str, sizeof(str), " integer_menu %lld, value %d ",
-			dev->int_menu->qmenu_int[dev->int_menu->cur.val],
-			dev->int_menu->cur.val);
-	gen_text(dev, vbuf, line++ * 16, 16, str);
-	mutex_unlock(dev->ctrl_handler.lock);
-	if (dev->button_pressed) {
-		dev->button_pressed--;
-		snprintf(str, sizeof(str), " button pressed!");
-		gen_text(dev, vbuf, line++ * 16, 16, str);
-	}
-
-	dev->mv_count += 2;
-
-	buf->vb.v4l2_buf.field = V4L2_FIELD_INTERLACED;
-	buf->vb.v4l2_buf.sequence = dev->seq_count++;
-	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
-}
-
-static void vivi_thread_tick(struct vivi_dev *dev)
-{
-	struct vivi_dmaqueue *dma_q = &dev->vidq;
-	struct vivi_buffer *buf;
-	unsigned long flags = 0;
-
-	dprintk(dev, 1, "Thread tick\n");
-
-	spin_lock_irqsave(&dev->slock, flags);
-	if (list_empty(&dma_q->active)) {
-		dprintk(dev, 1, "No active queue to serve\n");
-		spin_unlock_irqrestore(&dev->slock, flags);
-		return;
-	}
-
-	buf = list_entry(dma_q->active.next, struct vivi_buffer, list);
-	list_del(&buf->list);
-	spin_unlock_irqrestore(&dev->slock, flags);
-
-	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
-
-	/* Fill buffer */
-	vivi_fillbuff(dev, buf);
-	dprintk(dev, 1, "filled buffer %p\n", buf);
-
-	vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
-	dprintk(dev, 2, "[%p/%d] done\n", buf, buf->vb.v4l2_buf.index);
-}
-
-#define frames_to_ms(dev, frames)				\
-	((frames * dev->timeperframe.numerator * 1000) / dev->timeperframe.denominator)
-
-static void vivi_sleep(struct vivi_dev *dev)
-{
-	struct vivi_dmaqueue *dma_q = &dev->vidq;
-	int timeout;
-	DECLARE_WAITQUEUE(wait, current);
-
-	dprintk(dev, 1, "%s dma_q=0x%08lx\n", __func__,
-		(unsigned long)dma_q);
-
-	add_wait_queue(&dma_q->wq, &wait);
-	if (kthread_should_stop())
-		goto stop_task;
-
-	/* Calculate time to wake up */
-	timeout = msecs_to_jiffies(frames_to_ms(dev, 1));
-
-	vivi_thread_tick(dev);
-
-	schedule_timeout_interruptible(timeout);
-
-stop_task:
-	remove_wait_queue(&dma_q->wq, &wait);
-	try_to_freeze();
-}
-
-static int vivi_thread(void *data)
-{
-	struct vivi_dev *dev = data;
-
-	dprintk(dev, 1, "thread started\n");
-
-	set_freezable();
-
-	for (;;) {
-		vivi_sleep(dev);
-
-		if (kthread_should_stop())
-			break;
-	}
-	dprintk(dev, 1, "thread: exit\n");
-	return 0;
-}
-
-static int vivi_start_generating(struct vivi_dev *dev)
-{
-	struct vivi_dmaqueue *dma_q = &dev->vidq;
-
-	dprintk(dev, 1, "%s\n", __func__);
-
-	/* Resets frame counters */
-	dev->ms = 0;
-	dev->mv_count = 0;
-	dev->jiffies = jiffies;
-
-	dma_q->frame = 0;
-	dma_q->ini_jiffies = jiffies;
-	dma_q->kthread = kthread_run(vivi_thread, dev, "%s",
-				     dev->v4l2_dev.name);
-
-	if (IS_ERR(dma_q->kthread)) {
-		v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
-		return PTR_ERR(dma_q->kthread);
-	}
-	/* Wakes thread */
-	wake_up_interruptible(&dma_q->wq);
-
-	dprintk(dev, 1, "returning from %s\n", __func__);
-	return 0;
-}
-
-static void vivi_stop_generating(struct vivi_dev *dev)
-{
-	struct vivi_dmaqueue *dma_q = &dev->vidq;
-
-	dprintk(dev, 1, "%s\n", __func__);
-
-	/* shutdown control thread */
-	if (dma_q->kthread) {
-		kthread_stop(dma_q->kthread);
-		dma_q->kthread = NULL;
-	}
-
-	/*
-	 * Typical driver might need to wait here until dma engine stops.
-	 * In this case we can abort imiedetly, so it's just a noop.
-	 */
-
-	/* Release all active buffers */
-	while (!list_empty(&dma_q->active)) {
-		struct vivi_buffer *buf;
-		buf = list_entry(dma_q->active.next, struct vivi_buffer, list);
-		list_del(&buf->list);
-		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
-		dprintk(dev, 2, "[%p/%d] done\n", buf, buf->vb.v4l2_buf.index);
-	}
-}
-/* ------------------------------------------------------------------
-	Videobuf operations
-   ------------------------------------------------------------------*/
-static int queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
-				unsigned int *nbuffers, unsigned int *nplanes,
-				unsigned int sizes[], void *alloc_ctxs[])
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vq);
-	unsigned long size;
-
-	size = dev->width * dev->height * dev->pixelsize;
-	if (fmt) {
-		if (fmt->fmt.pix.sizeimage < size)
-			return -EINVAL;
-		size = fmt->fmt.pix.sizeimage;
-		/* check against insane over 8K resolution buffers */
-		if (size > 7680 * 4320 * dev->pixelsize)
-			return -EINVAL;
-	}
-
-	*nplanes = 1;
-
-	sizes[0] = size;
-
-	/*
-	 * videobuf2-vmalloc allocator is context-less so no need to set
-	 * alloc_ctxs array.
-	 */
-
-	dprintk(dev, 1, "%s, count=%d, size=%ld\n", __func__,
-		*nbuffers, size);
-
-	return 0;
-}
-
-static int buffer_prepare(struct vb2_buffer *vb)
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
-	struct vivi_buffer *buf = container_of(vb, struct vivi_buffer, vb);
-	unsigned long size;
-
-	dprintk(dev, 1, "%s, field=%d\n", __func__, vb->v4l2_buf.field);
-
-	BUG_ON(NULL == dev->fmt);
-
-	/*
-	 * Theses properties only change when queue is idle, see s_fmt.
-	 * The below checks should not be performed here, on each
-	 * buffer_prepare (i.e. on each qbuf). Most of the code in this function
-	 * should thus be moved to buffer_init and s_fmt.
-	 */
-	if (dev->width  < 48 || dev->width  > MAX_WIDTH ||
-	    dev->height < 32 || dev->height > MAX_HEIGHT)
-		return -EINVAL;
-
-	size = dev->width * dev->height * dev->pixelsize;
-	if (vb2_plane_size(vb, 0) < size) {
-		dprintk(dev, 1, "%s data will not fit into plane (%lu < %lu)\n",
-				__func__, vb2_plane_size(vb, 0), size);
-		return -EINVAL;
-	}
-
-	vb2_set_plane_payload(&buf->vb, 0, size);
-
-	precalculate_bars(dev);
-	precalculate_line(dev);
-
-	return 0;
-}
-
-static void buffer_queue(struct vb2_buffer *vb)
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
-	struct vivi_buffer *buf = container_of(vb, struct vivi_buffer, vb);
-	struct vivi_dmaqueue *vidq = &dev->vidq;
-	unsigned long flags = 0;
-
-	dprintk(dev, 1, "%s\n", __func__);
-
-	spin_lock_irqsave(&dev->slock, flags);
-	list_add_tail(&buf->list, &vidq->active);
-	spin_unlock_irqrestore(&dev->slock, flags);
-}
-
-static int start_streaming(struct vb2_queue *vq, unsigned int count)
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vq);
-	int err;
-
-	dprintk(dev, 1, "%s\n", __func__);
-	dev->seq_count = 0;
-	err = vivi_start_generating(dev);
-	if (err) {
-		struct vivi_buffer *buf, *tmp;
-
-		list_for_each_entry_safe(buf, tmp, &dev->vidq.active, list) {
-			list_del(&buf->list);
-			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
-		}
-	}
-	return err;
-}
-
-/* abort streaming and wait for last buffer */
-static void stop_streaming(struct vb2_queue *vq)
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vq);
-	dprintk(dev, 1, "%s\n", __func__);
-	vivi_stop_generating(dev);
-}
-
-static void vivi_lock(struct vb2_queue *vq)
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vq);
-	mutex_lock(&dev->mutex);
-}
-
-static void vivi_unlock(struct vb2_queue *vq)
-{
-	struct vivi_dev *dev = vb2_get_drv_priv(vq);
-	mutex_unlock(&dev->mutex);
-}
-
-
-static const struct vb2_ops vivi_video_qops = {
-	.queue_setup		= queue_setup,
-	.buf_prepare		= buffer_prepare,
-	.buf_queue		= buffer_queue,
-	.start_streaming	= start_streaming,
-	.stop_streaming		= stop_streaming,
-	.wait_prepare		= vivi_unlock,
-	.wait_finish		= vivi_lock,
-};
-
-/* ------------------------------------------------------------------
-	IOCTL vidioc handling
-   ------------------------------------------------------------------*/
-static int vidioc_querycap(struct file *file, void  *priv,
-					struct v4l2_capability *cap)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-
-	strcpy(cap->driver, "vivi");
-	strcpy(cap->card, "vivi");
-	snprintf(cap->bus_info, sizeof(cap->bus_info),
-			"platform:%s", dev->v4l2_dev.name);
-	cap->device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_STREAMING |
-			    V4L2_CAP_READWRITE;
-	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
-	return 0;
-}
-
-static int vidioc_enum_fmt_vid_cap(struct file *file, void  *priv,
-					struct v4l2_fmtdesc *f)
-{
-	const struct vivi_fmt *fmt;
-
-	if (f->index >= ARRAY_SIZE(formats))
-		return -EINVAL;
-
-	fmt = &formats[f->index];
-
-	strlcpy(f->description, fmt->name, sizeof(f->description));
-	f->pixelformat = fmt->fourcc;
-	return 0;
-}
-
-static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
-					struct v4l2_format *f)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-
-	f->fmt.pix.width        = dev->width;
-	f->fmt.pix.height       = dev->height;
-	f->fmt.pix.field        = V4L2_FIELD_INTERLACED;
-	f->fmt.pix.pixelformat  = dev->fmt->fourcc;
-	f->fmt.pix.bytesperline =
-		(f->fmt.pix.width * dev->fmt->depth) >> 3;
-	f->fmt.pix.sizeimage =
-		f->fmt.pix.height * f->fmt.pix.bytesperline;
-	if (dev->fmt->is_yuv)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
-	else
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
-	return 0;
-}
-
-static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
-			struct v4l2_format *f)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-	const struct vivi_fmt *fmt;
-
-	fmt = get_format(f);
-	if (!fmt) {
-		dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
-			f->fmt.pix.pixelformat);
-		f->fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
-		fmt = get_format(f);
-	}
-
-	f->fmt.pix.field = V4L2_FIELD_INTERLACED;
-	v4l_bound_align_image(&f->fmt.pix.width, 48, MAX_WIDTH, 2,
-			      &f->fmt.pix.height, 32, MAX_HEIGHT, 0, 0);
-	f->fmt.pix.bytesperline =
-		(f->fmt.pix.width * fmt->depth) >> 3;
-	f->fmt.pix.sizeimage =
-		f->fmt.pix.height * f->fmt.pix.bytesperline;
-	if (fmt->is_yuv)
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
-	else
-		f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
-	return 0;
-}
-
-static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
-					struct v4l2_format *f)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-	struct vb2_queue *q = &dev->vb_vidq;
-
-	int ret = vidioc_try_fmt_vid_cap(file, priv, f);
-	if (ret < 0)
-		return ret;
-
-	if (vb2_is_busy(q)) {
-		dprintk(dev, 1, "%s device busy\n", __func__);
-		return -EBUSY;
-	}
-
-	dev->fmt = get_format(f);
-	dev->pixelsize = dev->fmt->depth / 8;
-	dev->width = f->fmt.pix.width;
-	dev->height = f->fmt.pix.height;
-
-	return 0;
-}
-
-static int vidioc_enum_framesizes(struct file *file, void *fh,
-					 struct v4l2_frmsizeenum *fsize)
-{
-	static const struct v4l2_frmsize_stepwise sizes = {
-		48, MAX_WIDTH, 4,
-		32, MAX_HEIGHT, 1
-	};
-	int i;
-
-	if (fsize->index)
-		return -EINVAL;
-	for (i = 0; i < ARRAY_SIZE(formats); i++)
-		if (formats[i].fourcc == fsize->pixel_format)
-			break;
-	if (i == ARRAY_SIZE(formats))
-		return -EINVAL;
-	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
-	fsize->stepwise = sizes;
-	return 0;
-}
-
-/* only one input in this sample driver */
-static int vidioc_enum_input(struct file *file, void *priv,
-				struct v4l2_input *inp)
-{
-	if (inp->index >= NUM_INPUTS)
-		return -EINVAL;
-
-	inp->type = V4L2_INPUT_TYPE_CAMERA;
-	sprintf(inp->name, "Camera %u", inp->index);
-	return 0;
-}
-
-static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-
-	*i = dev->input;
-	return 0;
-}
-
-static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-
-	if (i >= NUM_INPUTS)
-		return -EINVAL;
-
-	if (i == dev->input)
-		return 0;
-
-	dev->input = i;
-	/*
-	 * Modify the brightness range depending on the input.
-	 * This makes it easy to use vivi to test if applications can
-	 * handle control range modifications and is also how this is
-	 * typically used in practice as different inputs may be hooked
-	 * up to different receivers with different control ranges.
-	 */
-	v4l2_ctrl_modify_range(dev->brightness,
-			128 * i, 255 + 128 * i, 1, 127 + 128 * i);
-	precalculate_bars(dev);
-	precalculate_line(dev);
-	return 0;
-}
-
-/* timeperframe is arbitrary and continuous */
-static int vidioc_enum_frameintervals(struct file *file, void *priv,
-					     struct v4l2_frmivalenum *fival)
-{
-	const struct vivi_fmt *fmt;
-
-	if (fival->index)
-		return -EINVAL;
-
-	fmt = __get_format(fival->pixel_format);
-	if (!fmt)
-		return -EINVAL;
-
-	/* check for valid width/height */
-	if (fival->width < 48 || fival->width > MAX_WIDTH || (fival->width & 3))
-		return -EINVAL;
-	if (fival->height < 32 || fival->height > MAX_HEIGHT)
-		return -EINVAL;
-
-	fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
-
-	/* fill in stepwise (step=1.0 is required by V4L2 spec) */
-	fival->stepwise.min  = tpf_min;
-	fival->stepwise.max  = tpf_max;
-	fival->stepwise.step = (struct v4l2_fract) {1, 1};
-
-	return 0;
-}
-
-static int vidioc_g_parm(struct file *file, void *priv,
-			  struct v4l2_streamparm *parm)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
-	parm->parm.capture.timeperframe = dev->timeperframe;
-	parm->parm.capture.readbuffers  = 1;
-	return 0;
-}
-
-#define FRACT_CMP(a, OP, b)	\
-	((u64)(a).numerator * (b).denominator  OP  (u64)(b).numerator * (a).denominator)
-
-static int vidioc_s_parm(struct file *file, void *priv,
-			  struct v4l2_streamparm *parm)
-{
-	struct vivi_dev *dev = video_drvdata(file);
-	struct v4l2_fract tpf;
-
-	if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
-		return -EINVAL;
-
-	tpf = parm->parm.capture.timeperframe;
-
-	/* tpf: {*, 0} resets timing; clip to [min, max]*/
-	tpf = tpf.denominator ? tpf : tpf_default;
-	tpf = FRACT_CMP(tpf, <, tpf_min) ? tpf_min : tpf;
-	tpf = FRACT_CMP(tpf, >, tpf_max) ? tpf_max : tpf;
-
-	dev->timeperframe = tpf;
-	parm->parm.capture.timeperframe = tpf;
-	parm->parm.capture.readbuffers  = 1;
-	return 0;
-}
-
-/* --- controls ---------------------------------------------- */
-
-static int vivi_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct vivi_dev *dev = container_of(ctrl->handler, struct vivi_dev, ctrl_handler);
-
-	if (ctrl == dev->autogain)
-		dev->gain->val = jiffies & 0xff;
-	return 0;
-}
-
-static int vivi_s_ctrl(struct v4l2_ctrl *ctrl)
-{
-	struct vivi_dev *dev = container_of(ctrl->handler, struct vivi_dev, ctrl_handler);
-
-	switch (ctrl->id) {
-	case V4L2_CID_ALPHA_COMPONENT:
-		dev->alpha_component = ctrl->val;
-		break;
-	default:
-		if (ctrl == dev->button)
-			dev->button_pressed = 30;
-		break;
-	}
-	return 0;
-}
-
-/* ------------------------------------------------------------------
-	File operations for the device
-   ------------------------------------------------------------------*/
-
-static const struct v4l2_ctrl_ops vivi_ctrl_ops = {
-	.g_volatile_ctrl = vivi_g_volatile_ctrl,
-	.s_ctrl = vivi_s_ctrl,
-};
-
-#define VIVI_CID_CUSTOM_BASE	(V4L2_CID_USER_BASE | 0xf000)
-
-static const struct v4l2_ctrl_config vivi_ctrl_button = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 0,
-	.name = "Button",
-	.type = V4L2_CTRL_TYPE_BUTTON,
-};
-
-static const struct v4l2_ctrl_config vivi_ctrl_boolean = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 1,
-	.name = "Boolean",
-	.type = V4L2_CTRL_TYPE_BOOLEAN,
-	.min = 0,
-	.max = 1,
-	.step = 1,
-	.def = 1,
-};
-
-static const struct v4l2_ctrl_config vivi_ctrl_int32 = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 2,
-	.name = "Integer 32 Bits",
-	.type = V4L2_CTRL_TYPE_INTEGER,
-	.min = -0x80000000LL,
-	.max = 0x7fffffff,
-	.step = 1,
-};
-
-static const struct v4l2_ctrl_config vivi_ctrl_int64 = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 3,
-	.name = "Integer 64 Bits",
-	.type = V4L2_CTRL_TYPE_INTEGER64,
-	.min = LLONG_MIN,
-	.max = LLONG_MAX,
-	.step = 1,
-};
-
-static const char * const vivi_ctrl_menu_strings[] = {
-	"Menu Item 0 (Skipped)",
-	"Menu Item 1",
-	"Menu Item 2 (Skipped)",
-	"Menu Item 3",
-	"Menu Item 4",
-	"Menu Item 5 (Skipped)",
-	NULL,
-};
-
-static const struct v4l2_ctrl_config vivi_ctrl_menu = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 4,
-	.name = "Menu",
-	.type = V4L2_CTRL_TYPE_MENU,
-	.min = 1,
-	.max = 4,
-	.def = 3,
-	.menu_skip_mask = 0x04,
-	.qmenu = vivi_ctrl_menu_strings,
-};
-
-static const struct v4l2_ctrl_config vivi_ctrl_string = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 5,
-	.name = "String",
-	.type = V4L2_CTRL_TYPE_STRING,
-	.min = 2,
-	.max = 4,
-	.step = 1,
-};
-
-static const struct v4l2_ctrl_config vivi_ctrl_bitmask = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 6,
-	.name = "Bitmask",
-	.type = V4L2_CTRL_TYPE_BITMASK,
-	.def = 0x80002000,
-	.min = 0,
-	.max = 0x80402010,
-	.step = 0,
-};
-
-static const s64 vivi_ctrl_int_menu_values[] = {
-	1, 1, 2, 3, 5, 8, 13, 21, 42,
-};
-
-static const struct v4l2_ctrl_config vivi_ctrl_int_menu = {
-	.ops = &vivi_ctrl_ops,
-	.id = VIVI_CID_CUSTOM_BASE + 7,
-	.name = "Integer menu",
-	.type = V4L2_CTRL_TYPE_INTEGER_MENU,
-	.min = 1,
-	.max = 8,
-	.def = 4,
-	.menu_skip_mask = 0x02,
-	.qmenu_int = vivi_ctrl_int_menu_values,
-};
-
-static const struct v4l2_file_operations vivi_fops = {
-	.owner		= THIS_MODULE,
-	.open           = v4l2_fh_open,
-	.release        = vb2_fop_release,
-	.read           = vb2_fop_read,
-	.poll		= vb2_fop_poll,
-	.unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
-	.mmap           = vb2_fop_mmap,
-};
-
-static const struct v4l2_ioctl_ops vivi_ioctl_ops = {
-	.vidioc_querycap      = vidioc_querycap,
-	.vidioc_enum_fmt_vid_cap  = vidioc_enum_fmt_vid_cap,
-	.vidioc_g_fmt_vid_cap     = vidioc_g_fmt_vid_cap,
-	.vidioc_try_fmt_vid_cap   = vidioc_try_fmt_vid_cap,
-	.vidioc_s_fmt_vid_cap     = vidioc_s_fmt_vid_cap,
-	.vidioc_enum_framesizes   = vidioc_enum_framesizes,
-	.vidioc_reqbufs       = vb2_ioctl_reqbufs,
-	.vidioc_create_bufs   = vb2_ioctl_create_bufs,
-	.vidioc_prepare_buf   = vb2_ioctl_prepare_buf,
-	.vidioc_querybuf      = vb2_ioctl_querybuf,
-	.vidioc_qbuf          = vb2_ioctl_qbuf,
-	.vidioc_dqbuf         = vb2_ioctl_dqbuf,
-	.vidioc_enum_input    = vidioc_enum_input,
-	.vidioc_g_input       = vidioc_g_input,
-	.vidioc_s_input       = vidioc_s_input,
-	.vidioc_enum_frameintervals = vidioc_enum_frameintervals,
-	.vidioc_g_parm        = vidioc_g_parm,
-	.vidioc_s_parm        = vidioc_s_parm,
-	.vidioc_streamon      = vb2_ioctl_streamon,
-	.vidioc_streamoff     = vb2_ioctl_streamoff,
-	.vidioc_log_status    = v4l2_ctrl_log_status,
-	.vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
-	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
-};
-
-static const struct video_device vivi_template = {
-	.name		= "vivi",
-	.fops           = &vivi_fops,
-	.ioctl_ops 	= &vivi_ioctl_ops,
-	.release	= video_device_release_empty,
-};
-
-/* -----------------------------------------------------------------
-	Initialization and module stuff
-   ------------------------------------------------------------------*/
-
-static int vivi_release(void)
-{
-	struct vivi_dev *dev;
-	struct list_head *list;
-
-	while (!list_empty(&vivi_devlist)) {
-		list = vivi_devlist.next;
-		list_del(list);
-		dev = list_entry(list, struct vivi_dev, vivi_devlist);
-
-		v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
-			video_device_node_name(&dev->vdev));
-		video_unregister_device(&dev->vdev);
-		v4l2_device_unregister(&dev->v4l2_dev);
-		v4l2_ctrl_handler_free(&dev->ctrl_handler);
-		kfree(dev);
-	}
-
-	return 0;
-}
-
-static int __init vivi_create_instance(int inst)
-{
-	struct vivi_dev *dev;
-	struct video_device *vfd;
-	struct v4l2_ctrl_handler *hdl;
-	struct vb2_queue *q;
-	int ret;
-
-	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-	if (!dev)
-		return -ENOMEM;
-
-	snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
-			"%s-%03d", VIVI_MODULE_NAME, inst);
-	ret = v4l2_device_register(NULL, &dev->v4l2_dev);
-	if (ret)
-		goto free_dev;
-
-	dev->fmt = &formats[0];
-	dev->timeperframe = tpf_default;
-	dev->width = 640;
-	dev->height = 480;
-	dev->pixelsize = dev->fmt->depth / 8;
-	hdl = &dev->ctrl_handler;
-	v4l2_ctrl_handler_init(hdl, 11);
-	dev->volume = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_AUDIO_VOLUME, 0, 255, 1, 200);
-	dev->brightness = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_BRIGHTNESS, 0, 255, 1, 127);
-	dev->contrast = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_CONTRAST, 0, 255, 1, 16);
-	dev->saturation = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_SATURATION, 0, 255, 1, 127);
-	dev->hue = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_HUE, -128, 127, 1, 0);
-	dev->autogain = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
-	dev->gain = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_GAIN, 0, 255, 1, 100);
-	dev->alpha = v4l2_ctrl_new_std(hdl, &vivi_ctrl_ops,
-			V4L2_CID_ALPHA_COMPONENT, 0, 255, 1, 0);
-	dev->button = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_button, NULL);
-	dev->int32 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int32, NULL);
-	dev->int64 = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int64, NULL);
-	dev->boolean = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_boolean, NULL);
-	dev->menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_menu, NULL);
-	dev->string = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_string, NULL);
-	dev->bitmask = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_bitmask, NULL);
-	dev->int_menu = v4l2_ctrl_new_custom(hdl, &vivi_ctrl_int_menu, NULL);
-	if (hdl->error) {
-		ret = hdl->error;
-		goto unreg_dev;
-	}
-	v4l2_ctrl_auto_cluster(2, &dev->autogain, 0, true);
-	dev->v4l2_dev.ctrl_handler = hdl;
-
-	/* initialize locks */
-	spin_lock_init(&dev->slock);
-
-	/* initialize queue */
-	q = &dev->vb_vidq;
-	q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-	q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
-	q->drv_priv = dev;
-	q->buf_struct_size = sizeof(struct vivi_buffer);
-	q->ops = &vivi_video_qops;
-	q->mem_ops = &vb2_vmalloc_memops;
-	q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
-
-	ret = vb2_queue_init(q);
-	if (ret)
-		goto unreg_dev;
-
-	mutex_init(&dev->mutex);
-
-	/* init video dma queues */
-	INIT_LIST_HEAD(&dev->vidq.active);
-	init_waitqueue_head(&dev->vidq.wq);
-
-	vfd = &dev->vdev;
-	*vfd = vivi_template;
-	vfd->debug = debug;
-	vfd->v4l2_dev = &dev->v4l2_dev;
-	vfd->queue = q;
-
-	/*
-	 * Provide a mutex to v4l2 core. It will be used to protect
-	 * all fops and v4l2 ioctls.
-	 */
-	vfd->lock = &dev->mutex;
-	video_set_drvdata(vfd, dev);
-
-	ret = video_register_device(vfd, VFL_TYPE_GRABBER, video_nr);
-	if (ret < 0)
-		goto unreg_dev;
-
-	/* Now that everything is fine, let's add it to device list */
-	list_add_tail(&dev->vivi_devlist, &vivi_devlist);
-
-	v4l2_info(&dev->v4l2_dev, "V4L2 device registered as %s\n",
-		  video_device_node_name(vfd));
-	return 0;
-
-unreg_dev:
-	v4l2_ctrl_handler_free(hdl);
-	v4l2_device_unregister(&dev->v4l2_dev);
-free_dev:
-	kfree(dev);
-	return ret;
-}
-
-/* This routine allocates from 1 to n_devs virtual drivers.
-
-   The real maximum number of virtual drivers will depend on how many drivers
-   will succeed. This is limited to the maximum number of devices that
-   videodev supports, which is equal to VIDEO_NUM_DEVICES.
- */
-static int __init vivi_init(void)
-{
-	const struct font_desc *font = find_font("VGA8x16");
-	int ret = 0, i;
-
-	if (font == NULL) {
-		printk(KERN_ERR "vivi: could not find font\n");
-		return -ENODEV;
-	}
-	font8x16 = font->data;
-
-	if (n_devs <= 0)
-		n_devs = 1;
-
-	for (i = 0; i < n_devs; i++) {
-		ret = vivi_create_instance(i);
-		if (ret) {
-			/* If some instantiations succeeded, keep driver */
-			if (i)
-				ret = 0;
-			break;
-		}
-	}
-
-	if (ret < 0) {
-		printk(KERN_ERR "vivi: error %d while loading driver\n", ret);
-		return ret;
-	}
-
-	printk(KERN_INFO "Video Technology Magazine Virtual Video "
-			"Capture Board ver %s successfully loaded.\n",
-			VIVI_VERSION);
-
-	/* n_devs will reflect the actual number of allocated devices */
-	n_devs = i;
-
-	return ret;
-}
-
-static void __exit vivi_exit(void)
-{
-	vivi_release();
-}
-
-module_init(vivi_init);
-module_exit(vivi_exit);
diff --git a/drivers/media/platform/vivid/Kconfig b/drivers/media/platform/vivid/Kconfig
new file mode 100644
index 000000000000..d71139a2ae00
--- /dev/null
+++ b/drivers/media/platform/vivid/Kconfig
@@ -0,0 +1,19 @@
+config VIDEO_VIVID
+	tristate "Virtual Video Test Driver"
+	depends on VIDEO_DEV && VIDEO_V4L2 && !SPARC32 && !SPARC64
+	select FONT_SUPPORT
+	select FONT_8x16
+	select VIDEOBUF2_VMALLOC
+	default n
+	---help---
+	  Enables a virtual video driver. This driver emulates a webcam,
+	  TV, S-Video and HDMI capture hardware, including VBI support for
+	  the SDTV inputs. Also video output, VBI output, radio receivers,
+	  transmitters and software defined radio capture is emulated.
+
+	  It is highly configurable and is ideal for testing applications.
+	  Error injection is supported to test rare errors that are hard
+	  to reproduce in real hardware.
+
+	  Say Y here if you want to test video apps or debug V4L devices.
+	  When in doubt, say N.
diff --git a/drivers/media/platform/vivid/Makefile b/drivers/media/platform/vivid/Makefile
new file mode 100644
index 000000000000..756fc12851df
--- /dev/null
+++ b/drivers/media/platform/vivid/Makefile
@@ -0,0 +1,6 @@
+vivid-objs := vivid-core.o vivid-ctrls.o vivid-vid-common.o vivid-vbi-gen.o \
+		vivid-vid-cap.o vivid-vid-out.o vivid-kthread-cap.o vivid-kthread-out.o \
+		vivid-radio-rx.o vivid-radio-tx.o vivid-radio-common.o \
+		vivid-rds-gen.o vivid-sdr-cap.o vivid-vbi-cap.o vivid-vbi-out.o \
+		vivid-osd.o vivid-tpg.o vivid-tpg-colors.o
+obj-$(CONFIG_VIDEO_VIVID) += vivid.o
diff --git a/drivers/media/platform/vivid/vivid-core.c b/drivers/media/platform/vivid/vivid-core.c
new file mode 100644
index 000000000000..2c61a62ab48b
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-core.c
@@ -0,0 +1,1390 @@
+/*
+ * vivid-core.c - A Virtual Video Test Driver, core initialization
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+
+#define VIVID_MODULE_NAME "vivid"
+
+/* The maximum number of vivid devices */
+#define VIVID_MAX_DEVS 64
+
+MODULE_DESCRIPTION("Virtual Video Test Driver");
+MODULE_AUTHOR("Hans Verkuil");
+MODULE_LICENSE("GPL");
+
+static unsigned n_devs = 1;
+module_param(n_devs, uint, 0444);
+MODULE_PARM_DESC(n_devs, " number of driver instances to create");
+
+static int vid_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vid_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vid_cap_nr, " videoX start number, -1 is autodetect");
+
+static int vid_out_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vid_out_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vid_out_nr, " videoX start number, -1 is autodetect");
+
+static int vbi_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vbi_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vbi_cap_nr, " vbiX start number, -1 is autodetect");
+
+static int vbi_out_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(vbi_out_nr, int, NULL, 0444);
+MODULE_PARM_DESC(vbi_out_nr, " vbiX start number, -1 is autodetect");
+
+static int sdr_cap_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(sdr_cap_nr, int, NULL, 0444);
+MODULE_PARM_DESC(sdr_cap_nr, " swradioX start number, -1 is autodetect");
+
+static int radio_rx_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(radio_rx_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_rx_nr, " radioX start number, -1 is autodetect");
+
+static int radio_tx_nr[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(radio_tx_nr, int, NULL, 0444);
+MODULE_PARM_DESC(radio_tx_nr, " radioX start number, -1 is autodetect");
+
+static int ccs_cap_mode[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(ccs_cap_mode, int, NULL, 0444);
+MODULE_PARM_DESC(ccs_cap_mode, " capture crop/compose/scale mode:\n"
+			   "\t\t    bit 0=crop, 1=compose, 2=scale,\n"
+			   "\t\t    -1=user-controlled (default)");
+
+static int ccs_out_mode[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = -1 };
+module_param_array(ccs_out_mode, int, NULL, 0444);
+MODULE_PARM_DESC(ccs_out_mode, " output crop/compose/scale mode:\n"
+			   "\t\t    bit 0=crop, 1=compose, 2=scale,\n"
+			   "\t\t    -1=user-controlled (default)");
+
+static unsigned multiplanar[VIVID_MAX_DEVS];
+module_param_array(multiplanar, uint, NULL, 0444);
+MODULE_PARM_DESC(multiplanar, " 0 (default) is alternating single and multiplanar devices,\n"
+			      "\t\t    1 is single planar devices,\n"
+			      "\t\t    2 is multiplanar devices");
+
+/* Default: video + vbi-cap (raw and sliced) + radio rx + radio tx + sdr + vbi-out + vid-out */
+static unsigned node_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 0x1d3d };
+module_param_array(node_types, uint, NULL, 0444);
+MODULE_PARM_DESC(node_types, " node types, default is 0x1d3d. Bitmask with the following meaning:\n"
+			     "\t\t    bit 0: Video Capture node\n"
+			     "\t\t    bit 2-3: VBI Capture node: 0 = none, 1 = raw vbi, 2 = sliced vbi, 3 = both\n"
+			     "\t\t    bit 4: Radio Receiver node\n"
+			     "\t\t    bit 5: Software Defined Radio Receiver node\n"
+			     "\t\t    bit 8: Video Output node\n"
+			     "\t\t    bit 10-11: VBI Output node: 0 = none, 1 = raw vbi, 2 = sliced vbi, 3 = both\n"
+			     "\t\t    bit 12: Radio Transmitter node\n"
+			     "\t\t    bit 16: Framebuffer for testing overlays");
+
+/* Default: 4 inputs */
+static unsigned num_inputs[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 4 };
+module_param_array(num_inputs, uint, NULL, 0444);
+MODULE_PARM_DESC(num_inputs, " number of inputs, default is 4");
+
+/* Default: input 0 = WEBCAM, 1 = TV, 2 = SVID, 3 = HDMI */
+static unsigned input_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 0xe4 };
+module_param_array(input_types, uint, NULL, 0444);
+MODULE_PARM_DESC(input_types, " input types, default is 0xe4. Two bits per input,\n"
+			      "\t\t    bits 0-1 == input 0, bits 31-30 == input 15.\n"
+			      "\t\t    Type 0 == webcam, 1 == TV, 2 == S-Video, 3 == HDMI");
+
+/* Default: 2 outputs */
+static unsigned num_outputs[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 2 };
+module_param_array(num_outputs, uint, NULL, 0444);
+MODULE_PARM_DESC(num_outputs, " number of outputs, default is 2");
+
+/* Default: output 0 = SVID, 1 = HDMI */
+static unsigned output_types[VIVID_MAX_DEVS] = { [0 ... (VIVID_MAX_DEVS - 1)] = 2 };
+module_param_array(output_types, uint, NULL, 0444);
+MODULE_PARM_DESC(output_types, " output types, default is 0x02. One bit per output,\n"
+			      "\t\t    bit 0 == output 0, bit 15 == output 15.\n"
+			      "\t\t    Type 0 == S-Video, 1 == HDMI");
+
+unsigned vivid_debug;
+module_param(vivid_debug, uint, 0644);
+MODULE_PARM_DESC(vivid_debug, " activates debug info");
+
+static bool no_error_inj;
+module_param(no_error_inj, bool, 0444);
+MODULE_PARM_DESC(no_error_inj, " if set disable the error injecting controls");
+
+static struct vivid_dev *vivid_devs[VIVID_MAX_DEVS];
+
+const struct v4l2_rect vivid_min_rect = {
+	0, 0, MIN_WIDTH, MIN_HEIGHT
+};
+
+const struct v4l2_rect vivid_max_rect = {
+	0, 0, MAX_WIDTH * MAX_ZOOM, MAX_HEIGHT * MAX_ZOOM
+};
+
+static const u8 vivid_hdmi_edid[256] = {
+	0x00, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x00,
+	0x63, 0x3a, 0xaa, 0x55, 0x00, 0x00, 0x00, 0x00,
+	0x0a, 0x18, 0x01, 0x03, 0x80, 0x10, 0x09, 0x78,
+	0x0e, 0x00, 0xb2, 0xa0, 0x57, 0x49, 0x9b, 0x26,
+	0x10, 0x48, 0x4f, 0x2f, 0xcf, 0x00, 0x31, 0x59,
+	0x45, 0x59, 0x81, 0x80, 0x81, 0x40, 0x90, 0x40,
+	0x95, 0x00, 0xa9, 0x40, 0xb3, 0x00, 0x02, 0x3a,
+	0x80, 0x18, 0x71, 0x38, 0x2d, 0x40, 0x58, 0x2c,
+	0x46, 0x00, 0x10, 0x09, 0x00, 0x00, 0x00, 0x1e,
+	0x00, 0x00, 0x00, 0xfd, 0x00, 0x18, 0x55, 0x18,
+	0x5e, 0x11, 0x00, 0x0a, 0x20, 0x20, 0x20, 0x20,
+	0x20, 0x20, 0x00, 0x00, 0x00, 0xfc, 0x00,  'v',
+	'4',   'l',  '2',  '-',  'h',  'd',  'm',  'i',
+	0x0a, 0x0a, 0x0a, 0x0a, 0x00, 0x00, 0x00, 0x10,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xf0,
+
+	0x02, 0x03, 0x1a, 0xc0, 0x48, 0xa2, 0x10, 0x04,
+	0x02, 0x01, 0x21, 0x14, 0x13, 0x23, 0x09, 0x07,
+	0x07, 0x65, 0x03, 0x0c, 0x00, 0x10, 0x00, 0xe2,
+	0x00, 0x2a, 0x01, 0x1d, 0x00, 0x80, 0x51, 0xd0,
+	0x1c, 0x20, 0x40, 0x80, 0x35, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x1e, 0x8c, 0x0a, 0xd0, 0x8a,
+	0x20, 0xe0, 0x2d, 0x10, 0x10, 0x3e, 0x96, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd7
+};
+
+void vivid_lock(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	mutex_lock(&dev->mutex);
+}
+
+void vivid_unlock(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	mutex_unlock(&dev->mutex);
+}
+
+static int vidioc_querycap(struct file *file, void  *priv,
+					struct v4l2_capability *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	strcpy(cap->driver, "vivid");
+	strcpy(cap->card, "vivid");
+	snprintf(cap->bus_info, sizeof(cap->bus_info),
+			"platform:%s", dev->v4l2_dev.name);
+
+	if (vdev->vfl_type == VFL_TYPE_GRABBER && vdev->vfl_dir == VFL_DIR_RX)
+		cap->device_caps = dev->vid_cap_caps;
+	if (vdev->vfl_type == VFL_TYPE_GRABBER && vdev->vfl_dir == VFL_DIR_TX)
+		cap->device_caps = dev->vid_out_caps;
+	else if (vdev->vfl_type == VFL_TYPE_VBI && vdev->vfl_dir == VFL_DIR_RX)
+		cap->device_caps = dev->vbi_cap_caps;
+	else if (vdev->vfl_type == VFL_TYPE_VBI && vdev->vfl_dir == VFL_DIR_TX)
+		cap->device_caps = dev->vbi_out_caps;
+	else if (vdev->vfl_type == VFL_TYPE_SDR)
+		cap->device_caps = dev->sdr_cap_caps;
+	else if (vdev->vfl_type == VFL_TYPE_RADIO && vdev->vfl_dir == VFL_DIR_RX)
+		cap->device_caps = dev->radio_rx_caps;
+	else if (vdev->vfl_type == VFL_TYPE_RADIO && vdev->vfl_dir == VFL_DIR_TX)
+		cap->device_caps = dev->radio_tx_caps;
+	cap->capabilities = dev->vid_cap_caps | dev->vid_out_caps |
+		dev->vbi_cap_caps | dev->vbi_out_caps |
+		dev->radio_rx_caps | dev->radio_tx_caps |
+		dev->sdr_cap_caps | V4L2_CAP_DEVICE_CAPS;
+	return 0;
+}
+
+static int vidioc_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_s_hw_freq_seek(file, fh, a);
+	return -ENOTTY;
+}
+
+static int vidioc_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_enum_freq_bands(file, fh, band);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_enum_freq_bands(file, fh, band);
+	return -ENOTTY;
+}
+
+static int vidioc_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_g_tuner(file, fh, vt);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_g_tuner(file, fh, vt);
+	return vivid_video_g_tuner(file, fh, vt);
+}
+
+static int vidioc_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_rx_s_tuner(file, fh, vt);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_s_tuner(file, fh, vt);
+	return vivid_video_s_tuner(file, fh, vt);
+}
+
+static int vidioc_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_g_frequency(file,
+			vdev->vfl_dir == VFL_DIR_RX ?
+			&dev->radio_rx_freq : &dev->radio_tx_freq, vf);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_g_frequency(file, fh, vf);
+	return vivid_video_g_frequency(file, fh, vf);
+}
+
+static int vidioc_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_type == VFL_TYPE_RADIO)
+		return vivid_radio_s_frequency(file,
+			vdev->vfl_dir == VFL_DIR_RX ?
+			&dev->radio_rx_freq : &dev->radio_tx_freq, vf);
+	if (vdev->vfl_type == VFL_TYPE_SDR)
+		return vivid_sdr_s_frequency(file, fh, vf);
+	return vivid_video_s_frequency(file, fh, vf);
+}
+
+static int vidioc_overlay(struct file *file, void *fh, unsigned i)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_overlay(file, fh, i);
+	return vivid_vid_out_overlay(file, fh, i);
+}
+
+static int vidioc_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_g_fbuf(file, fh, a);
+	return vivid_vid_out_g_fbuf(file, fh, a);
+}
+
+static int vidioc_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_fbuf(file, fh, a);
+	return vivid_vid_out_s_fbuf(file, fh, a);
+}
+
+static int vidioc_s_std(struct file *file, void *fh, v4l2_std_id id)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_std(file, fh, id);
+	return vivid_vid_out_s_std(file, fh, id);
+}
+
+static int vidioc_s_dv_timings(struct file *file, void *fh, struct v4l2_dv_timings *timings)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_dv_timings(file, fh, timings);
+	return vivid_vid_out_s_dv_timings(file, fh, timings);
+}
+
+static int vidioc_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cc)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_cropcap(file, fh, cc);
+	return vivid_vid_out_cropcap(file, fh, cc);
+}
+
+static int vidioc_g_selection(struct file *file, void *fh,
+			      struct v4l2_selection *sel)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_g_selection(file, fh, sel);
+	return vivid_vid_out_g_selection(file, fh, sel);
+}
+
+static int vidioc_s_selection(struct file *file, void *fh,
+			      struct v4l2_selection *sel)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_selection(file, fh, sel);
+	return vivid_vid_out_s_selection(file, fh, sel);
+}
+
+static int vidioc_g_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_g_parm(file, fh, parm);
+	return vivid_vid_out_g_parm(file, fh, parm);
+}
+
+static int vidioc_s_parm(struct file *file, void *fh,
+			  struct v4l2_streamparm *parm)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_vid_cap_s_parm(file, fh, parm);
+	return vivid_vid_out_g_parm(file, fh, parm);
+}
+
+static ssize_t vivid_radio_read(struct file *file, char __user *buf,
+			 size_t size, loff_t *offset)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_TX)
+		return -EINVAL;
+	return vivid_radio_rx_read(file, buf, size, offset);
+}
+
+static ssize_t vivid_radio_write(struct file *file, const char __user *buf,
+			  size_t size, loff_t *offset)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return -EINVAL;
+	return vivid_radio_tx_write(file, buf, size, offset);
+}
+
+static unsigned int vivid_radio_poll(struct file *file, struct poll_table_struct *wait)
+{
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX)
+		return vivid_radio_rx_poll(file, wait);
+	return vivid_radio_tx_poll(file, wait);
+}
+
+static bool vivid_is_in_use(struct video_device *vdev)
+{
+	unsigned long flags;
+	bool res;
+
+	spin_lock_irqsave(&vdev->fh_lock, flags);
+	res = !list_empty(&vdev->fh_list);
+	spin_unlock_irqrestore(&vdev->fh_lock, flags);
+	return res;
+}
+
+static bool vivid_is_last_user(struct vivid_dev *dev)
+{
+	unsigned uses = vivid_is_in_use(&dev->vid_cap_dev) +
+			vivid_is_in_use(&dev->vid_out_dev) +
+			vivid_is_in_use(&dev->vbi_cap_dev) +
+			vivid_is_in_use(&dev->vbi_out_dev) +
+			vivid_is_in_use(&dev->sdr_cap_dev) +
+			vivid_is_in_use(&dev->radio_rx_dev) +
+			vivid_is_in_use(&dev->radio_tx_dev);
+
+	return uses == 1;
+}
+
+static int vivid_fop_release(struct file *file)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	mutex_lock(&dev->mutex);
+	if (!no_error_inj && v4l2_fh_is_singular_file(file) &&
+	    !video_is_registered(vdev) && vivid_is_last_user(dev)) {
+		/*
+		 * I am the last user of this driver, and a disconnect
+		 * was forced (since this video_device is unregistered),
+		 * so re-register all video_device's again.
+		 */
+		v4l2_info(&dev->v4l2_dev, "reconnect\n");
+		set_bit(V4L2_FL_REGISTERED, &dev->vid_cap_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->vid_out_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->vbi_cap_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->vbi_out_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->sdr_cap_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->radio_rx_dev.flags);
+		set_bit(V4L2_FL_REGISTERED, &dev->radio_tx_dev.flags);
+	}
+	mutex_unlock(&dev->mutex);
+	if (file->private_data == dev->overlay_cap_owner)
+		dev->overlay_cap_owner = NULL;
+	if (file->private_data == dev->radio_rx_rds_owner) {
+		dev->radio_rx_rds_last_block = 0;
+		dev->radio_rx_rds_owner = NULL;
+	}
+	if (file->private_data == dev->radio_tx_rds_owner) {
+		dev->radio_tx_rds_last_block = 0;
+		dev->radio_tx_rds_owner = NULL;
+	}
+	if (vdev->queue)
+		return vb2_fop_release(file);
+	return v4l2_fh_release(file);
+}
+
+static const struct v4l2_file_operations vivid_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = vivid_fop_release,
+	.read           = vb2_fop_read,
+	.write          = vb2_fop_write,
+	.poll		= vb2_fop_poll,
+	.unlocked_ioctl = video_ioctl2,
+	.mmap           = vb2_fop_mmap,
+};
+
+static const struct v4l2_file_operations vivid_radio_fops = {
+	.owner		= THIS_MODULE,
+	.open           = v4l2_fh_open,
+	.release        = vivid_fop_release,
+	.read           = vivid_radio_read,
+	.write          = vivid_radio_write,
+	.poll		= vivid_radio_poll,
+	.unlocked_ioctl = video_ioctl2,
+};
+
+static const struct v4l2_ioctl_ops vivid_ioctl_ops = {
+	.vidioc_querycap		= vidioc_querycap,
+
+	.vidioc_enum_fmt_vid_cap	= vidioc_enum_fmt_vid,
+	.vidioc_g_fmt_vid_cap		= vidioc_g_fmt_vid_cap,
+	.vidioc_try_fmt_vid_cap		= vidioc_try_fmt_vid_cap,
+	.vidioc_s_fmt_vid_cap		= vidioc_s_fmt_vid_cap,
+	.vidioc_enum_fmt_vid_cap_mplane = vidioc_enum_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_cap_mplane	= vidioc_g_fmt_vid_cap_mplane,
+	.vidioc_try_fmt_vid_cap_mplane	= vidioc_try_fmt_vid_cap_mplane,
+	.vidioc_s_fmt_vid_cap_mplane	= vidioc_s_fmt_vid_cap_mplane,
+
+	.vidioc_enum_fmt_vid_out	= vidioc_enum_fmt_vid,
+	.vidioc_g_fmt_vid_out		= vidioc_g_fmt_vid_out,
+	.vidioc_try_fmt_vid_out		= vidioc_try_fmt_vid_out,
+	.vidioc_s_fmt_vid_out		= vidioc_s_fmt_vid_out,
+	.vidioc_enum_fmt_vid_out_mplane = vidioc_enum_fmt_vid_mplane,
+	.vidioc_g_fmt_vid_out_mplane	= vidioc_g_fmt_vid_out_mplane,
+	.vidioc_try_fmt_vid_out_mplane	= vidioc_try_fmt_vid_out_mplane,
+	.vidioc_s_fmt_vid_out_mplane	= vidioc_s_fmt_vid_out_mplane,
+
+	.vidioc_g_selection		= vidioc_g_selection,
+	.vidioc_s_selection		= vidioc_s_selection,
+	.vidioc_cropcap			= vidioc_cropcap,
+
+	.vidioc_g_fmt_vbi_cap		= vidioc_g_fmt_vbi_cap,
+	.vidioc_try_fmt_vbi_cap		= vidioc_g_fmt_vbi_cap,
+	.vidioc_s_fmt_vbi_cap		= vidioc_s_fmt_vbi_cap,
+
+	.vidioc_g_fmt_sliced_vbi_cap    = vidioc_g_fmt_sliced_vbi_cap,
+	.vidioc_try_fmt_sliced_vbi_cap  = vidioc_try_fmt_sliced_vbi_cap,
+	.vidioc_s_fmt_sliced_vbi_cap    = vidioc_s_fmt_sliced_vbi_cap,
+	.vidioc_g_sliced_vbi_cap	= vidioc_g_sliced_vbi_cap,
+
+	.vidioc_g_fmt_vbi_out		= vidioc_g_fmt_vbi_out,
+	.vidioc_try_fmt_vbi_out		= vidioc_g_fmt_vbi_out,
+	.vidioc_s_fmt_vbi_out		= vidioc_s_fmt_vbi_out,
+
+	.vidioc_g_fmt_sliced_vbi_out    = vidioc_g_fmt_sliced_vbi_out,
+	.vidioc_try_fmt_sliced_vbi_out  = vidioc_try_fmt_sliced_vbi_out,
+	.vidioc_s_fmt_sliced_vbi_out    = vidioc_s_fmt_sliced_vbi_out,
+
+	.vidioc_enum_fmt_sdr_cap	= vidioc_enum_fmt_sdr_cap,
+	.vidioc_g_fmt_sdr_cap		= vidioc_g_fmt_sdr_cap,
+	.vidioc_try_fmt_sdr_cap		= vidioc_g_fmt_sdr_cap,
+	.vidioc_s_fmt_sdr_cap		= vidioc_g_fmt_sdr_cap,
+
+	.vidioc_overlay			= vidioc_overlay,
+	.vidioc_enum_framesizes		= vidioc_enum_framesizes,
+	.vidioc_enum_frameintervals	= vidioc_enum_frameintervals,
+	.vidioc_g_parm			= vidioc_g_parm,
+	.vidioc_s_parm			= vidioc_s_parm,
+
+	.vidioc_enum_fmt_vid_overlay	= vidioc_enum_fmt_vid_overlay,
+	.vidioc_g_fmt_vid_overlay	= vidioc_g_fmt_vid_overlay,
+	.vidioc_try_fmt_vid_overlay	= vidioc_try_fmt_vid_overlay,
+	.vidioc_s_fmt_vid_overlay	= vidioc_s_fmt_vid_overlay,
+	.vidioc_g_fmt_vid_out_overlay	= vidioc_g_fmt_vid_out_overlay,
+	.vidioc_try_fmt_vid_out_overlay	= vidioc_try_fmt_vid_out_overlay,
+	.vidioc_s_fmt_vid_out_overlay	= vidioc_s_fmt_vid_out_overlay,
+	.vidioc_g_fbuf			= vidioc_g_fbuf,
+	.vidioc_s_fbuf			= vidioc_s_fbuf,
+
+	.vidioc_reqbufs			= vb2_ioctl_reqbufs,
+	.vidioc_create_bufs		= vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf		= vb2_ioctl_prepare_buf,
+	.vidioc_querybuf		= vb2_ioctl_querybuf,
+	.vidioc_qbuf			= vb2_ioctl_qbuf,
+	.vidioc_dqbuf			= vb2_ioctl_dqbuf,
+/* Not yet	.vidioc_expbuf		= vb2_ioctl_expbuf,*/
+	.vidioc_streamon		= vb2_ioctl_streamon,
+	.vidioc_streamoff		= vb2_ioctl_streamoff,
+
+	.vidioc_enum_input		= vidioc_enum_input,
+	.vidioc_g_input			= vidioc_g_input,
+	.vidioc_s_input			= vidioc_s_input,
+	.vidioc_s_audio			= vidioc_s_audio,
+	.vidioc_g_audio			= vidioc_g_audio,
+	.vidioc_enumaudio		= vidioc_enumaudio,
+	.vidioc_s_frequency		= vidioc_s_frequency,
+	.vidioc_g_frequency		= vidioc_g_frequency,
+	.vidioc_s_tuner			= vidioc_s_tuner,
+	.vidioc_g_tuner			= vidioc_g_tuner,
+	.vidioc_s_modulator		= vidioc_s_modulator,
+	.vidioc_g_modulator		= vidioc_g_modulator,
+	.vidioc_s_hw_freq_seek		= vidioc_s_hw_freq_seek,
+	.vidioc_enum_freq_bands		= vidioc_enum_freq_bands,
+
+	.vidioc_enum_output		= vidioc_enum_output,
+	.vidioc_g_output		= vidioc_g_output,
+	.vidioc_s_output		= vidioc_s_output,
+	.vidioc_s_audout		= vidioc_s_audout,
+	.vidioc_g_audout		= vidioc_g_audout,
+	.vidioc_enumaudout		= vidioc_enumaudout,
+
+	.vidioc_querystd		= vidioc_querystd,
+	.vidioc_g_std			= vidioc_g_std,
+	.vidioc_s_std			= vidioc_s_std,
+	.vidioc_s_dv_timings		= vidioc_s_dv_timings,
+	.vidioc_g_dv_timings		= vidioc_g_dv_timings,
+	.vidioc_query_dv_timings	= vidioc_query_dv_timings,
+	.vidioc_enum_dv_timings		= vidioc_enum_dv_timings,
+	.vidioc_dv_timings_cap		= vidioc_dv_timings_cap,
+	.vidioc_g_edid			= vidioc_g_edid,
+	.vidioc_s_edid			= vidioc_s_edid,
+
+	.vidioc_log_status		= v4l2_ctrl_log_status,
+	.vidioc_subscribe_event		= vidioc_subscribe_event,
+	.vidioc_unsubscribe_event	= v4l2_event_unsubscribe,
+};
+
+/* -----------------------------------------------------------------
+	Initialization and module stuff
+   ------------------------------------------------------------------*/
+
+static int __init vivid_create_instance(int inst)
+{
+	static const struct v4l2_dv_timings def_dv_timings =
+					V4L2_DV_BT_CEA_1280X720P60;
+	unsigned in_type_counter[4] = { 0, 0, 0, 0 };
+	unsigned out_type_counter[4] = { 0, 0, 0, 0 };
+	int ccs_cap = ccs_cap_mode[inst];
+	int ccs_out = ccs_out_mode[inst];
+	bool has_tuner;
+	bool has_modulator;
+	struct vivid_dev *dev;
+	struct video_device *vfd;
+	struct vb2_queue *q;
+	unsigned node_type = node_types[inst];
+	v4l2_std_id tvnorms_cap = 0, tvnorms_out = 0;
+	int ret;
+	int i;
+
+	/* allocate main vivid state structure */
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->inst = inst;
+
+	/* register v4l2_device */
+	snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
+			"%s-%03d", VIVID_MODULE_NAME, inst);
+	ret = v4l2_device_register(NULL, &dev->v4l2_dev);
+	if (ret)
+		goto free_dev;
+
+	/* start detecting feature set */
+
+	/* do we use single- or multi-planar? */
+	if (multiplanar[inst] == 0)
+		dev->multiplanar = inst & 1;
+	else
+		dev->multiplanar = multiplanar[inst] > 1;
+	v4l2_info(&dev->v4l2_dev, "using %splanar format API\n",
+			dev->multiplanar ? "multi" : "single ");
+
+	/* how many inputs do we have and of what type? */
+	dev->num_inputs = num_inputs[inst];
+	if (dev->num_inputs < 1)
+		dev->num_inputs = 1;
+	if (dev->num_inputs >= MAX_INPUTS)
+		dev->num_inputs = MAX_INPUTS;
+	for (i = 0; i < dev->num_inputs; i++) {
+		dev->input_type[i] = (input_types[inst] >> (i * 2)) & 0x3;
+		dev->input_name_counter[i] = in_type_counter[dev->input_type[i]]++;
+	}
+	dev->has_audio_inputs = in_type_counter[TV] && in_type_counter[SVID];
+
+	/* how many outputs do we have and of what type? */
+	dev->num_outputs = num_outputs[inst];
+	if (dev->num_outputs < 1)
+		dev->num_outputs = 1;
+	if (dev->num_outputs >= MAX_OUTPUTS)
+		dev->num_outputs = MAX_OUTPUTS;
+	for (i = 0; i < dev->num_outputs; i++) {
+		dev->output_type[i] = ((output_types[inst] >> i) & 1) ? HDMI : SVID;
+		dev->output_name_counter[i] = out_type_counter[dev->output_type[i]]++;
+	}
+	dev->has_audio_outputs = out_type_counter[SVID];
+
+	/* do we create a video capture device? */
+	dev->has_vid_cap = node_type & 0x0001;
+
+	/* do we create a vbi capture device? */
+	if (in_type_counter[TV] || in_type_counter[SVID]) {
+		dev->has_raw_vbi_cap = node_type & 0x0004;
+		dev->has_sliced_vbi_cap = node_type & 0x0008;
+		dev->has_vbi_cap = dev->has_raw_vbi_cap | dev->has_sliced_vbi_cap;
+	}
+
+	/* do we create a video output device? */
+	dev->has_vid_out = node_type & 0x0100;
+
+	/* do we create a vbi output device? */
+	if (out_type_counter[SVID]) {
+		dev->has_raw_vbi_out = node_type & 0x0400;
+		dev->has_sliced_vbi_out = node_type & 0x0800;
+		dev->has_vbi_out = dev->has_raw_vbi_out | dev->has_sliced_vbi_out;
+	}
+
+	/* do we create a radio receiver device? */
+	dev->has_radio_rx = node_type & 0x0010;
+
+	/* do we create a radio transmitter device? */
+	dev->has_radio_tx = node_type & 0x1000;
+
+	/* do we create a software defined radio capture device? */
+	dev->has_sdr_cap = node_type & 0x0020;
+
+	/* do we have a tuner? */
+	has_tuner = ((dev->has_vid_cap || dev->has_vbi_cap) && in_type_counter[TV]) ||
+		    dev->has_radio_rx || dev->has_sdr_cap;
+
+	/* do we have a modulator? */
+	has_modulator = dev->has_radio_tx;
+
+	if (dev->has_vid_cap)
+		/* do we have a framebuffer for overlay testing? */
+		dev->has_fb = node_type & 0x10000;
+
+	/* can we do crop/compose/scaling while capturing? */
+	if (no_error_inj && ccs_cap == -1)
+		ccs_cap = 7;
+
+	/* if ccs_cap == -1, then the use can select it using controls */
+	if (ccs_cap != -1) {
+		dev->has_crop_cap = ccs_cap & 1;
+		dev->has_compose_cap = ccs_cap & 2;
+		dev->has_scaler_cap = ccs_cap & 4;
+		v4l2_info(&dev->v4l2_dev, "Capture Crop: %c Compose: %c Scaler: %c\n",
+			dev->has_crop_cap ? 'Y' : 'N',
+			dev->has_compose_cap ? 'Y' : 'N',
+			dev->has_scaler_cap ? 'Y' : 'N');
+	}
+
+	/* can we do crop/compose/scaling with video output? */
+	if (no_error_inj && ccs_out == -1)
+		ccs_out = 7;
+
+	/* if ccs_out == -1, then the use can select it using controls */
+	if (ccs_out != -1) {
+		dev->has_crop_out = ccs_out & 1;
+		dev->has_compose_out = ccs_out & 2;
+		dev->has_scaler_out = ccs_out & 4;
+		v4l2_info(&dev->v4l2_dev, "Output Crop: %c Compose: %c Scaler: %c\n",
+			dev->has_crop_out ? 'Y' : 'N',
+			dev->has_compose_out ? 'Y' : 'N',
+			dev->has_scaler_out ? 'Y' : 'N');
+	}
+
+	/* end detecting feature set */
+
+	if (dev->has_vid_cap) {
+		/* set up the capabilities of the video capture device */
+		dev->vid_cap_caps = dev->multiplanar ?
+			V4L2_CAP_VIDEO_CAPTURE_MPLANE :
+			V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY;
+		dev->vid_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_inputs)
+			dev->vid_cap_caps |= V4L2_CAP_AUDIO;
+		if (in_type_counter[TV])
+			dev->vid_cap_caps |= V4L2_CAP_TUNER;
+	}
+	if (dev->has_vid_out) {
+		/* set up the capabilities of the video output device */
+		dev->vid_out_caps = dev->multiplanar ?
+			V4L2_CAP_VIDEO_OUTPUT_MPLANE :
+			V4L2_CAP_VIDEO_OUTPUT;
+		if (dev->has_fb)
+			dev->vid_out_caps |= V4L2_CAP_VIDEO_OUTPUT_OVERLAY;
+		dev->vid_out_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_outputs)
+			dev->vid_out_caps |= V4L2_CAP_AUDIO;
+	}
+	if (dev->has_vbi_cap) {
+		/* set up the capabilities of the vbi capture device */
+		dev->vbi_cap_caps = (dev->has_raw_vbi_cap ? V4L2_CAP_VBI_CAPTURE : 0) |
+				    (dev->has_sliced_vbi_cap ? V4L2_CAP_SLICED_VBI_CAPTURE : 0);
+		dev->vbi_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_inputs)
+			dev->vbi_cap_caps |= V4L2_CAP_AUDIO;
+		if (in_type_counter[TV])
+			dev->vbi_cap_caps |= V4L2_CAP_TUNER;
+	}
+	if (dev->has_vbi_out) {
+		/* set up the capabilities of the vbi output device */
+		dev->vbi_out_caps = (dev->has_raw_vbi_out ? V4L2_CAP_VBI_OUTPUT : 0) |
+				    (dev->has_sliced_vbi_out ? V4L2_CAP_SLICED_VBI_OUTPUT : 0);
+		dev->vbi_out_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+		if (dev->has_audio_outputs)
+			dev->vbi_out_caps |= V4L2_CAP_AUDIO;
+	}
+	if (dev->has_sdr_cap) {
+		/* set up the capabilities of the sdr capture device */
+		dev->sdr_cap_caps = V4L2_CAP_SDR_CAPTURE | V4L2_CAP_TUNER;
+		dev->sdr_cap_caps |= V4L2_CAP_STREAMING | V4L2_CAP_READWRITE;
+	}
+	/* set up the capabilities of the radio receiver device */
+	if (dev->has_radio_rx)
+		dev->radio_rx_caps = V4L2_CAP_RADIO | V4L2_CAP_RDS_CAPTURE |
+				     V4L2_CAP_HW_FREQ_SEEK | V4L2_CAP_TUNER |
+				     V4L2_CAP_READWRITE;
+	/* set up the capabilities of the radio transmitter device */
+	if (dev->has_radio_tx)
+		dev->radio_tx_caps = V4L2_CAP_RDS_OUTPUT | V4L2_CAP_MODULATOR |
+				     V4L2_CAP_READWRITE;
+
+	/* initialize the test pattern generator */
+	tpg_init(&dev->tpg, 640, 360);
+	if (tpg_alloc(&dev->tpg, MAX_ZOOM * MAX_WIDTH))
+		goto free_dev;
+	dev->scaled_line = vzalloc(MAX_ZOOM * MAX_WIDTH);
+	if (!dev->scaled_line)
+		goto free_dev;
+	dev->blended_line = vzalloc(MAX_ZOOM * MAX_WIDTH);
+	if (!dev->blended_line)
+		goto free_dev;
+
+	/* load the edid */
+	dev->edid = vmalloc(256 * 128);
+	if (!dev->edid)
+		goto free_dev;
+
+	/* create a string array containing the names of all the preset timings */
+	while (v4l2_dv_timings_presets[dev->query_dv_timings_size].bt.width)
+		dev->query_dv_timings_size++;
+	dev->query_dv_timings_qmenu = kmalloc(dev->query_dv_timings_size *
+					   (sizeof(void *) + 32), GFP_KERNEL);
+	if (dev->query_dv_timings_qmenu == NULL)
+		goto free_dev;
+	for (i = 0; i < dev->query_dv_timings_size; i++) {
+		const struct v4l2_bt_timings *bt = &v4l2_dv_timings_presets[i].bt;
+		char *p = (char *)&dev->query_dv_timings_qmenu[dev->query_dv_timings_size];
+		u32 htot, vtot;
+
+		p += i * 32;
+		dev->query_dv_timings_qmenu[i] = p;
+
+		htot = V4L2_DV_BT_FRAME_WIDTH(bt);
+		vtot = V4L2_DV_BT_FRAME_HEIGHT(bt);
+		snprintf(p, 32, "%ux%u%s%u",
+			bt->width, bt->height, bt->interlaced ? "i" : "p",
+			(u32)bt->pixelclock / (htot * vtot));
+	}
+
+	/* disable invalid ioctls based on the feature set */
+	if (!dev->has_audio_inputs) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_AUDIO);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUMAUDIO);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_AUDIO);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_AUDIO);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_ENUMAUDIO);
+	}
+	if (!dev->has_audio_outputs) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_AUDOUT);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_AUDOUT);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUMAUDOUT);
+		v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_S_AUDOUT);
+		v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_G_AUDOUT);
+		v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_ENUMAUDOUT);
+	}
+	if (!in_type_counter[TV] && !in_type_counter[SVID]) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_STD);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUMSTD);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_QUERYSTD);
+	}
+	if (!out_type_counter[SVID]) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_STD);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_STD);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUMSTD);
+	}
+	if (!has_tuner && !has_modulator) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_FREQUENCY);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_FREQUENCY);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_FREQUENCY);
+	}
+	if (!has_tuner) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_TUNER);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_TUNER);
+		v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_G_TUNER);
+	}
+	if (in_type_counter[HDMI] == 0) {
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_EDID);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_EDID);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_DV_TIMINGS_CAP);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_G_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_ENUM_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_QUERY_DV_TIMINGS);
+	}
+	if (out_type_counter[HDMI] == 0) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_EDID);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_DV_TIMINGS_CAP);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_DV_TIMINGS);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_DV_TIMINGS);
+	}
+	if (!dev->has_fb) {
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_FBUF);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_FBUF);
+		v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_OVERLAY);
+	}
+	v4l2_disable_ioctl(&dev->vid_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+	v4l2_disable_ioctl(&dev->vbi_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+	v4l2_disable_ioctl(&dev->sdr_cap_dev, VIDIOC_S_HW_FREQ_SEEK);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_S_FREQUENCY);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_G_FREQUENCY);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_FRAMESIZES);
+	v4l2_disable_ioctl(&dev->vid_out_dev, VIDIOC_ENUM_FRAMEINTERVALS);
+	v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_S_FREQUENCY);
+	v4l2_disable_ioctl(&dev->vbi_out_dev, VIDIOC_G_FREQUENCY);
+
+	/* configure internal data */
+	dev->fmt_cap = &vivid_formats[0];
+	dev->fmt_out = &vivid_formats[0];
+	if (!dev->multiplanar)
+		vivid_formats[0].data_offset[0] = 0;
+	dev->webcam_size_idx = 1;
+	dev->webcam_ival_idx = 3;
+	tpg_s_fourcc(&dev->tpg, dev->fmt_cap->fourcc);
+	dev->std_cap = V4L2_STD_PAL;
+	dev->std_out = V4L2_STD_PAL;
+	if (dev->input_type[0] == TV || dev->input_type[0] == SVID)
+		tvnorms_cap = V4L2_STD_ALL;
+	if (dev->output_type[0] == SVID)
+		tvnorms_out = V4L2_STD_ALL;
+	dev->dv_timings_cap = def_dv_timings;
+	dev->dv_timings_out = def_dv_timings;
+	dev->tv_freq = 2804 /* 175.25 * 16 */;
+	dev->tv_audmode = V4L2_TUNER_MODE_STEREO;
+	dev->tv_field_cap = V4L2_FIELD_INTERLACED;
+	dev->tv_field_out = V4L2_FIELD_INTERLACED;
+	dev->radio_rx_freq = 95000 * 16;
+	dev->radio_rx_audmode = V4L2_TUNER_MODE_STEREO;
+	if (dev->has_radio_tx) {
+		dev->radio_tx_freq = 95500 * 16;
+		dev->radio_rds_loop = false;
+	}
+	dev->radio_tx_subchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_RDS;
+	dev->sdr_adc_freq = 300000;
+	dev->sdr_fm_freq = 50000000;
+	dev->edid_max_blocks = dev->edid_blocks = 2;
+	memcpy(dev->edid, vivid_hdmi_edid, sizeof(vivid_hdmi_edid));
+	ktime_get_ts(&dev->radio_rds_init_ts);
+
+	/* create all controls */
+	ret = vivid_create_controls(dev, ccs_cap == -1, ccs_out == -1, no_error_inj,
+			in_type_counter[TV] || in_type_counter[SVID] ||
+			out_type_counter[SVID],
+			in_type_counter[HDMI] || out_type_counter[HDMI]);
+	if (ret)
+		goto unreg_dev;
+
+	/*
+	 * update the capture and output formats to do a proper initial
+	 * configuration.
+	 */
+	vivid_update_format_cap(dev, false);
+	vivid_update_format_out(dev);
+
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vid_cap);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vid_out);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vbi_cap);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_vbi_out);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_radio_rx);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_radio_tx);
+	v4l2_ctrl_handler_setup(&dev->ctrl_hdl_sdr_cap);
+
+	/* initialize overlay */
+	dev->fb_cap.fmt.width = dev->src_rect.width;
+	dev->fb_cap.fmt.height = dev->src_rect.height;
+	dev->fb_cap.fmt.pixelformat = dev->fmt_cap->fourcc;
+	dev->fb_cap.fmt.bytesperline = dev->src_rect.width * tpg_g_twopixelsize(&dev->tpg, 0) / 2;
+	dev->fb_cap.fmt.sizeimage = dev->src_rect.height * dev->fb_cap.fmt.bytesperline;
+
+	/* initialize locks */
+	spin_lock_init(&dev->slock);
+	mutex_init(&dev->mutex);
+
+	/* init dma queues */
+	INIT_LIST_HEAD(&dev->vid_cap_active);
+	INIT_LIST_HEAD(&dev->vid_out_active);
+	INIT_LIST_HEAD(&dev->vbi_cap_active);
+	INIT_LIST_HEAD(&dev->vbi_out_active);
+	INIT_LIST_HEAD(&dev->sdr_cap_active);
+
+	/* start creating the vb2 queues */
+	if (dev->has_vid_cap) {
+		/* initialize vid_cap queue */
+		q = &dev->vb_vid_cap_q;
+		q->type = dev->multiplanar ? V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+			V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vid_cap_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_vid_out) {
+		/* initialize vid_out queue */
+		q = &dev->vb_vid_out_q;
+		q->type = dev->multiplanar ? V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+			V4L2_BUF_TYPE_VIDEO_OUTPUT;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_WRITE;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vid_out_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_vbi_cap) {
+		/* initialize vbi_cap queue */
+		q = &dev->vb_vbi_cap_q;
+		q->type = dev->has_raw_vbi_cap ? V4L2_BUF_TYPE_VBI_CAPTURE :
+					      V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vbi_cap_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_vbi_out) {
+		/* initialize vbi_out queue */
+		q = &dev->vb_vbi_out_q;
+		q->type = dev->has_raw_vbi_out ? V4L2_BUF_TYPE_VBI_OUTPUT :
+					      V4L2_BUF_TYPE_SLICED_VBI_OUTPUT;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_WRITE;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_vbi_out_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 2;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_sdr_cap) {
+		/* initialize sdr_cap queue */
+		q = &dev->vb_sdr_cap_q;
+		q->type = V4L2_BUF_TYPE_SDR_CAPTURE;
+		q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF | VB2_READ;
+		q->drv_priv = dev;
+		q->buf_struct_size = sizeof(struct vivid_buffer);
+		q->ops = &vivid_sdr_cap_qops;
+		q->mem_ops = &vb2_vmalloc_memops;
+		q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+		q->min_buffers_needed = 8;
+
+		ret = vb2_queue_init(q);
+		if (ret)
+			goto unreg_dev;
+	}
+
+	if (dev->has_fb) {
+		/* Create framebuffer for testing capture/output overlay */
+		ret = vivid_fb_init(dev);
+		if (ret)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "Framebuffer device registered as fb%d\n",
+				dev->fb_info.node);
+	}
+
+	/* finally start creating the device nodes */
+	if (dev->has_vid_cap) {
+		vfd = &dev->vid_cap_dev;
+		strlcpy(vfd->name, "vivid-vid-cap", sizeof(vfd->name));
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vid_cap_q;
+		vfd->tvnorms = tvnorms_cap;
+
+		/*
+		 * Provide a mutex to v4l2 core. It will be used to protect
+		 * all fops and v4l2 ioctls.
+		 */
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_GRABBER, vid_cap_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_vid_out) {
+		vfd = &dev->vid_out_dev;
+		strlcpy(vfd->name, "vivid-vid-out", sizeof(vfd->name));
+		vfd->vfl_dir = VFL_DIR_TX;
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vid_out_q;
+		vfd->tvnorms = tvnorms_out;
+
+		/*
+		 * Provide a mutex to v4l2 core. It will be used to protect
+		 * all fops and v4l2 ioctls.
+		 */
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_GRABBER, vid_out_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 output device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_vbi_cap) {
+		vfd = &dev->vbi_cap_dev;
+		strlcpy(vfd->name, "vivid-vbi-cap", sizeof(vfd->name));
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vbi_cap_q;
+		vfd->lock = &dev->mutex;
+		vfd->tvnorms = tvnorms_cap;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_VBI, vbi_cap_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s, supports %s VBI\n",
+					  video_device_node_name(vfd),
+					  (dev->has_raw_vbi_cap && dev->has_sliced_vbi_cap) ?
+					  "raw and sliced" :
+					  (dev->has_raw_vbi_cap ? "raw" : "sliced"));
+	}
+
+	if (dev->has_vbi_out) {
+		vfd = &dev->vbi_out_dev;
+		strlcpy(vfd->name, "vivid-vbi-out", sizeof(vfd->name));
+		vfd->vfl_dir = VFL_DIR_TX;
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_vbi_out_q;
+		vfd->lock = &dev->mutex;
+		vfd->tvnorms = tvnorms_out;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_VBI, vbi_out_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 output device registered as %s, supports %s VBI\n",
+					  video_device_node_name(vfd),
+					  (dev->has_raw_vbi_out && dev->has_sliced_vbi_out) ?
+					  "raw and sliced" :
+					  (dev->has_raw_vbi_out ? "raw" : "sliced"));
+	}
+
+	if (dev->has_sdr_cap) {
+		vfd = &dev->sdr_cap_dev;
+		strlcpy(vfd->name, "vivid-sdr-cap", sizeof(vfd->name));
+		vfd->fops = &vivid_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->queue = &dev->vb_sdr_cap_q;
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_SDR, sdr_cap_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 capture device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_radio_rx) {
+		vfd = &dev->radio_rx_dev;
+		strlcpy(vfd->name, "vivid-rad-rx", sizeof(vfd->name));
+		vfd->fops = &vivid_radio_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_RADIO, radio_rx_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 receiver device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	if (dev->has_radio_tx) {
+		vfd = &dev->radio_tx_dev;
+		strlcpy(vfd->name, "vivid-rad-tx", sizeof(vfd->name));
+		vfd->vfl_dir = VFL_DIR_TX;
+		vfd->fops = &vivid_radio_fops;
+		vfd->ioctl_ops = &vivid_ioctl_ops;
+		vfd->release = video_device_release_empty;
+		vfd->v4l2_dev = &dev->v4l2_dev;
+		vfd->lock = &dev->mutex;
+		video_set_drvdata(vfd, dev);
+
+		ret = video_register_device(vfd, VFL_TYPE_RADIO, radio_tx_nr[inst]);
+		if (ret < 0)
+			goto unreg_dev;
+		v4l2_info(&dev->v4l2_dev, "V4L2 transmitter device registered as %s\n",
+					  video_device_node_name(vfd));
+	}
+
+	/* Now that everything is fine, let's add it to device list */
+	vivid_devs[inst] = dev;
+
+	return 0;
+
+unreg_dev:
+	video_unregister_device(&dev->radio_tx_dev);
+	video_unregister_device(&dev->radio_rx_dev);
+	video_unregister_device(&dev->sdr_cap_dev);
+	video_unregister_device(&dev->vbi_out_dev);
+	video_unregister_device(&dev->vbi_cap_dev);
+	video_unregister_device(&dev->vid_out_dev);
+	video_unregister_device(&dev->vid_cap_dev);
+	vivid_free_controls(dev);
+	v4l2_device_unregister(&dev->v4l2_dev);
+free_dev:
+	vfree(dev->scaled_line);
+	vfree(dev->blended_line);
+	vfree(dev->edid);
+	tpg_free(&dev->tpg);
+	kfree(dev->query_dv_timings_qmenu);
+	kfree(dev);
+	return ret;
+}
+
+/* This routine allocates from 1 to n_devs virtual drivers.
+
+   The real maximum number of virtual drivers will depend on how many drivers
+   will succeed. This is limited to the maximum number of devices that
+   videodev supports, which is equal to VIDEO_NUM_DEVICES.
+ */
+static int __init vivid_init(void)
+{
+	const struct font_desc *font = find_font("VGA8x16");
+	int ret = 0, i;
+
+	if (font == NULL) {
+		pr_err("vivid: could not find font\n");
+		return -ENODEV;
+	}
+
+	tpg_set_font(font->data);
+
+	n_devs = clamp_t(unsigned, n_devs, 1, VIVID_MAX_DEVS);
+
+	for (i = 0; i < n_devs; i++) {
+		ret = vivid_create_instance(i);
+		if (ret) {
+			/* If some instantiations succeeded, keep driver */
+			if (i)
+				ret = 0;
+			break;
+		}
+	}
+
+	if (ret < 0) {
+		pr_err("vivid: error %d while loading driver\n", ret);
+		return ret;
+	}
+
+	/* n_devs will reflect the actual number of allocated devices */
+	n_devs = i;
+
+	return ret;
+}
+
+static void __exit vivid_exit(void)
+{
+	struct vivid_dev *dev;
+	unsigned i;
+
+	for (i = 0; vivid_devs[i]; i++) {
+		dev = vivid_devs[i];
+
+		if (dev->has_vid_cap) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vid_cap_dev));
+			video_unregister_device(&dev->vid_cap_dev);
+		}
+		if (dev->has_vid_out) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vid_out_dev));
+			video_unregister_device(&dev->vid_out_dev);
+		}
+		if (dev->has_vbi_cap) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vbi_cap_dev));
+			video_unregister_device(&dev->vbi_cap_dev);
+		}
+		if (dev->has_vbi_out) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->vbi_out_dev));
+			video_unregister_device(&dev->vbi_out_dev);
+		}
+		if (dev->has_sdr_cap) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->sdr_cap_dev));
+			video_unregister_device(&dev->sdr_cap_dev);
+		}
+		if (dev->has_radio_rx) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->radio_rx_dev));
+			video_unregister_device(&dev->radio_rx_dev);
+		}
+		if (dev->has_radio_tx) {
+			v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
+				video_device_node_name(&dev->radio_tx_dev));
+			video_unregister_device(&dev->radio_tx_dev);
+		}
+		if (dev->has_fb) {
+			v4l2_info(&dev->v4l2_dev, "unregistering fb%d\n",
+				dev->fb_info.node);
+			unregister_framebuffer(&dev->fb_info);
+			vivid_fb_release_buffers(dev);
+		}
+		v4l2_device_unregister(&dev->v4l2_dev);
+		vivid_free_controls(dev);
+		vfree(dev->scaled_line);
+		vfree(dev->blended_line);
+		vfree(dev->edid);
+		vfree(dev->bitmap_cap);
+		vfree(dev->bitmap_out);
+		tpg_free(&dev->tpg);
+		kfree(dev->query_dv_timings_qmenu);
+		kfree(dev);
+		vivid_devs[i] = NULL;
+	}
+}
+
+module_init(vivid_init);
+module_exit(vivid_exit);
diff --git a/drivers/media/platform/vivid/vivid-core.h b/drivers/media/platform/vivid/vivid-core.h
new file mode 100644
index 000000000000..811c286491a5
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-core.h
@@ -0,0 +1,520 @@
+/*
+ * vivid-core.h - core datastructures
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_CORE_H_
+#define _VIVID_CORE_H_
+
+#include <linux/fb.h>
+#include <media/videobuf2-core.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-dev.h>
+#include <media/v4l2-ctrls.h>
+#include "vivid-tpg.h"
+#include "vivid-rds-gen.h"
+#include "vivid-vbi-gen.h"
+
+#define dprintk(dev, level, fmt, arg...) \
+	v4l2_dbg(level, vivid_debug, &dev->v4l2_dev, fmt, ## arg)
+
+/* Maximum allowed frame rate
+ *
+ * vivid will allow setting timeperframe in [1/FPS_MAX - FPS_MAX/1] range.
+ *
+ * Ideally FPS_MAX should be infinity, i.e. practically UINT_MAX, but that
+ * might hit application errors when they manipulate these values.
+ *
+ * Besides, for tpf < 10ms image-generation logic should be changed, to avoid
+ * producing frames with equal content.
+ */
+#define FPS_MAX 100
+
+/* The maximum number of clip rectangles */
+#define MAX_CLIPS  16
+/* The maximum number of inputs */
+#define MAX_INPUTS 16
+/* The maximum number of outputs */
+#define MAX_OUTPUTS 16
+/* The maximum up or down scaling factor is 4 */
+#define MAX_ZOOM  4
+/* The maximum image width/height are set to 4K DMT */
+#define MAX_WIDTH  4096
+#define MAX_HEIGHT 2160
+/* The minimum image width/height */
+#define MIN_WIDTH  16
+#define MIN_HEIGHT 16
+/* The data_offset of plane 0 for the multiplanar formats */
+#define PLANE0_DATA_OFFSET 128
+
+/* The supported TV frequency range in MHz */
+#define MIN_TV_FREQ (44U * 16U)
+#define MAX_TV_FREQ (958U * 16U)
+
+/* The number of samples returned in every SDR buffer */
+#define SDR_CAP_SAMPLES_PER_BUF 0x4000
+
+/* used by the threads to know when to resync internal counters */
+#define JIFFIES_PER_DAY (3600U * 24U * HZ)
+#define JIFFIES_RESYNC (JIFFIES_PER_DAY * (0xf0000000U / JIFFIES_PER_DAY))
+
+extern const struct v4l2_rect vivid_min_rect;
+extern const struct v4l2_rect vivid_max_rect;
+extern unsigned vivid_debug;
+
+struct vivid_fmt {
+	const char *name;
+	u32	fourcc;          /* v4l2 format id */
+	u8	depth;
+	bool	is_yuv;
+	bool	can_do_overlay;
+	u32	alpha_mask;
+	u8	planes;
+	u32	data_offset[2];
+};
+
+extern struct vivid_fmt vivid_formats[];
+
+/* buffer for one video frame */
+struct vivid_buffer {
+	/* common v4l buffer stuff -- must be first */
+	struct vb2_buffer	vb;
+	struct list_head	list;
+};
+
+enum vivid_input {
+	WEBCAM,
+	TV,
+	SVID,
+	HDMI,
+};
+
+enum vivid_signal_mode {
+	CURRENT_DV_TIMINGS,
+	CURRENT_STD = CURRENT_DV_TIMINGS,
+	NO_SIGNAL,
+	NO_LOCK,
+	OUT_OF_RANGE,
+	SELECTED_DV_TIMINGS,
+	SELECTED_STD = SELECTED_DV_TIMINGS,
+	CYCLE_DV_TIMINGS,
+	CYCLE_STD = CYCLE_DV_TIMINGS,
+	CUSTOM_DV_TIMINGS,
+};
+
+#define VIVID_INVALID_SIGNAL(mode) \
+	((mode) == NO_SIGNAL || (mode) == NO_LOCK || (mode) == OUT_OF_RANGE)
+
+struct vivid_dev {
+	unsigned			inst;
+	struct v4l2_device		v4l2_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_user_gen;
+	struct v4l2_ctrl_handler	ctrl_hdl_user_vid;
+	struct v4l2_ctrl_handler	ctrl_hdl_user_aud;
+	struct v4l2_ctrl_handler	ctrl_hdl_streaming;
+	struct v4l2_ctrl_handler	ctrl_hdl_sdtv_cap;
+	struct v4l2_ctrl_handler	ctrl_hdl_loop_out;
+	struct video_device		vid_cap_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vid_cap;
+	struct video_device		vid_out_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vid_out;
+	struct video_device		vbi_cap_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vbi_cap;
+	struct video_device		vbi_out_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_vbi_out;
+	struct video_device		radio_rx_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_radio_rx;
+	struct video_device		radio_tx_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_radio_tx;
+	struct video_device		sdr_cap_dev;
+	struct v4l2_ctrl_handler	ctrl_hdl_sdr_cap;
+	spinlock_t			slock;
+	struct mutex			mutex;
+
+	/* capabilities */
+	u32				vid_cap_caps;
+	u32				vid_out_caps;
+	u32				vbi_cap_caps;
+	u32				vbi_out_caps;
+	u32				sdr_cap_caps;
+	u32				radio_rx_caps;
+	u32				radio_tx_caps;
+
+	/* supported features */
+	bool				multiplanar;
+	unsigned			num_inputs;
+	u8				input_type[MAX_INPUTS];
+	u8				input_name_counter[MAX_INPUTS];
+	unsigned			num_outputs;
+	u8				output_type[MAX_OUTPUTS];
+	u8				output_name_counter[MAX_OUTPUTS];
+	bool				has_audio_inputs;
+	bool				has_audio_outputs;
+	bool				has_vid_cap;
+	bool				has_vid_out;
+	bool				has_vbi_cap;
+	bool				has_raw_vbi_cap;
+	bool				has_sliced_vbi_cap;
+	bool				has_vbi_out;
+	bool				has_raw_vbi_out;
+	bool				has_sliced_vbi_out;
+	bool				has_radio_rx;
+	bool				has_radio_tx;
+	bool				has_sdr_cap;
+	bool				has_fb;
+
+	bool				can_loop_video;
+
+	/* controls */
+	struct v4l2_ctrl		*brightness;
+	struct v4l2_ctrl		*contrast;
+	struct v4l2_ctrl		*saturation;
+	struct v4l2_ctrl		*hue;
+	struct {
+		/* autogain/gain cluster */
+		struct v4l2_ctrl	*autogain;
+		struct v4l2_ctrl	*gain;
+	};
+	struct v4l2_ctrl		*volume;
+	struct v4l2_ctrl		*mute;
+	struct v4l2_ctrl		*alpha;
+	struct v4l2_ctrl		*button;
+	struct v4l2_ctrl		*boolean;
+	struct v4l2_ctrl		*int32;
+	struct v4l2_ctrl		*int64;
+	struct v4l2_ctrl		*menu;
+	struct v4l2_ctrl		*string;
+	struct v4l2_ctrl		*bitmask;
+	struct v4l2_ctrl		*int_menu;
+	struct v4l2_ctrl		*test_pattern;
+	struct v4l2_ctrl		*colorspace;
+	struct v4l2_ctrl		*rgb_range_cap;
+	struct v4l2_ctrl		*real_rgb_range_cap;
+	struct {
+		/* std_signal_mode/standard cluster */
+		struct v4l2_ctrl	*ctrl_std_signal_mode;
+		struct v4l2_ctrl	*ctrl_standard;
+	};
+	struct {
+		/* dv_timings_signal_mode/timings cluster */
+		struct v4l2_ctrl	*ctrl_dv_timings_signal_mode;
+		struct v4l2_ctrl	*ctrl_dv_timings;
+	};
+	struct v4l2_ctrl		*ctrl_has_crop_cap;
+	struct v4l2_ctrl		*ctrl_has_compose_cap;
+	struct v4l2_ctrl		*ctrl_has_scaler_cap;
+	struct v4l2_ctrl		*ctrl_has_crop_out;
+	struct v4l2_ctrl		*ctrl_has_compose_out;
+	struct v4l2_ctrl		*ctrl_has_scaler_out;
+	struct v4l2_ctrl		*ctrl_tx_mode;
+	struct v4l2_ctrl		*ctrl_tx_rgb_range;
+
+	struct v4l2_ctrl		*radio_tx_rds_pi;
+	struct v4l2_ctrl		*radio_tx_rds_pty;
+	struct v4l2_ctrl		*radio_tx_rds_mono_stereo;
+	struct v4l2_ctrl		*radio_tx_rds_art_head;
+	struct v4l2_ctrl		*radio_tx_rds_compressed;
+	struct v4l2_ctrl		*radio_tx_rds_dyn_pty;
+	struct v4l2_ctrl		*radio_tx_rds_ta;
+	struct v4l2_ctrl		*radio_tx_rds_tp;
+	struct v4l2_ctrl		*radio_tx_rds_ms;
+	struct v4l2_ctrl		*radio_tx_rds_psname;
+	struct v4l2_ctrl		*radio_tx_rds_radiotext;
+
+	struct v4l2_ctrl		*radio_rx_rds_pty;
+	struct v4l2_ctrl		*radio_rx_rds_ta;
+	struct v4l2_ctrl		*radio_rx_rds_tp;
+	struct v4l2_ctrl		*radio_rx_rds_ms;
+	struct v4l2_ctrl		*radio_rx_rds_psname;
+	struct v4l2_ctrl		*radio_rx_rds_radiotext;
+
+	unsigned			input_brightness[MAX_INPUTS];
+	unsigned			osd_mode;
+	unsigned			button_pressed;
+	bool				sensor_hflip;
+	bool				sensor_vflip;
+	bool				hflip;
+	bool				vflip;
+	bool				vbi_cap_interlaced;
+	bool				loop_video;
+
+	/* Framebuffer */
+	unsigned long			video_pbase;
+	void				*video_vbase;
+	u32				video_buffer_size;
+	int				display_width;
+	int				display_height;
+	int				display_byte_stride;
+	int				bits_per_pixel;
+	int				bytes_per_pixel;
+	struct fb_info			fb_info;
+	struct fb_var_screeninfo	fb_defined;
+	struct fb_fix_screeninfo	fb_fix;
+
+	/* Error injection */
+	bool				queue_setup_error;
+	bool				buf_prepare_error;
+	bool				start_streaming_error;
+	bool				dqbuf_error;
+	bool				seq_wrap;
+	bool				time_wrap;
+	__kernel_time_t			time_wrap_offset;
+	unsigned			perc_dropped_buffers;
+	enum vivid_signal_mode		std_signal_mode;
+	unsigned			query_std_last;
+	v4l2_std_id			query_std;
+	enum tpg_video_aspect		std_aspect_ratio;
+
+	enum vivid_signal_mode		dv_timings_signal_mode;
+	char				**query_dv_timings_qmenu;
+	unsigned			query_dv_timings_size;
+	unsigned			query_dv_timings_last;
+	unsigned			query_dv_timings;
+	enum tpg_video_aspect		dv_timings_aspect_ratio;
+
+	/* Input */
+	unsigned			input;
+	v4l2_std_id			std_cap;
+	struct v4l2_dv_timings		dv_timings_cap;
+	u32				service_set_cap;
+	struct vivid_vbi_gen_data	vbi_gen;
+	u8				*edid;
+	unsigned			edid_blocks;
+	unsigned			edid_max_blocks;
+	unsigned			webcam_size_idx;
+	unsigned			webcam_ival_idx;
+	unsigned			tv_freq;
+	unsigned			tv_audmode;
+	unsigned			tv_field_cap;
+	unsigned			tv_audio_input;
+
+	/* Capture Overlay */
+	struct v4l2_framebuffer		fb_cap;
+	struct v4l2_fh			*overlay_cap_owner;
+	void				*fb_vbase_cap;
+	int				overlay_cap_top, overlay_cap_left;
+	enum v4l2_field			overlay_cap_field;
+	void				*bitmap_cap;
+	struct v4l2_clip		clips_cap[MAX_CLIPS];
+	struct v4l2_clip		try_clips_cap[MAX_CLIPS];
+	unsigned			clipcount_cap;
+
+	/* Output */
+	unsigned			output;
+	v4l2_std_id			std_out;
+	struct v4l2_dv_timings		dv_timings_out;
+	u32				colorspace_out;
+	u32				service_set_out;
+	u32				bytesperline_out[2];
+	unsigned			tv_field_out;
+	unsigned			tv_audio_output;
+	bool				vbi_out_have_wss;
+	u8				vbi_out_wss[2];
+	bool				vbi_out_have_cc[2];
+	u8				vbi_out_cc[2][2];
+	bool				dvi_d_out;
+	u8				*scaled_line;
+	u8				*blended_line;
+	unsigned			cur_scaled_line;
+
+	/* Output Overlay */
+	void				*fb_vbase_out;
+	bool				overlay_out_enabled;
+	int				overlay_out_top, overlay_out_left;
+	void				*bitmap_out;
+	struct v4l2_clip		clips_out[MAX_CLIPS];
+	struct v4l2_clip		try_clips_out[MAX_CLIPS];
+	unsigned			clipcount_out;
+	unsigned			fbuf_out_flags;
+	u32				chromakey_out;
+	u8				global_alpha_out;
+
+	/* video capture */
+	struct tpg_data			tpg;
+	unsigned			ms_vid_cap;
+	bool				must_blank[VIDEO_MAX_FRAME];
+
+	const struct vivid_fmt		*fmt_cap;
+	struct v4l2_fract		timeperframe_vid_cap;
+	enum v4l2_field			field_cap;
+	struct v4l2_rect		src_rect;
+	struct v4l2_rect		fmt_cap_rect;
+	struct v4l2_rect		crop_cap;
+	struct v4l2_rect		compose_cap;
+	struct v4l2_rect		crop_bounds_cap;
+	struct vb2_queue		vb_vid_cap_q;
+	struct list_head		vid_cap_active;
+	struct vb2_queue		vb_vbi_cap_q;
+	struct list_head		vbi_cap_active;
+
+	/* thread for generating video capture stream */
+	struct task_struct		*kthread_vid_cap;
+	unsigned long			jiffies_vid_cap;
+	u32				cap_seq_offset;
+	u32				cap_seq_count;
+	bool				cap_seq_resync;
+	u32				vid_cap_seq_start;
+	u32				vid_cap_seq_count;
+	bool				vid_cap_streaming;
+	u32				vbi_cap_seq_start;
+	u32				vbi_cap_seq_count;
+	bool				vbi_cap_streaming;
+	bool				stream_sliced_vbi_cap;
+
+	/* video output */
+	const struct vivid_fmt		*fmt_out;
+	struct v4l2_fract		timeperframe_vid_out;
+	enum v4l2_field			field_out;
+	struct v4l2_rect		sink_rect;
+	struct v4l2_rect		fmt_out_rect;
+	struct v4l2_rect		crop_out;
+	struct v4l2_rect		compose_out;
+	struct v4l2_rect		compose_bounds_out;
+	struct vb2_queue		vb_vid_out_q;
+	struct list_head		vid_out_active;
+	struct vb2_queue		vb_vbi_out_q;
+	struct list_head		vbi_out_active;
+
+	/* video loop precalculated rectangles */
+
+	/*
+	 * Intersection between what the output side composes and the capture side
+	 * crops. I.e., what actually needs to be copied from the output buffer to
+	 * the capture buffer.
+	 */
+	struct v4l2_rect		loop_vid_copy;
+	/* The part of the output buffer that (after scaling) corresponds to loop_vid_copy. */
+	struct v4l2_rect		loop_vid_out;
+	/* The part of the capture buffer that (after scaling) corresponds to loop_vid_copy. */
+	struct v4l2_rect		loop_vid_cap;
+	/*
+	 * The intersection of the framebuffer, the overlay output window and
+	 * loop_vid_copy. I.e., the part of the framebuffer that actually should be
+	 * blended with the compose_out rectangle. This uses the framebuffer origin.
+	 */
+	struct v4l2_rect		loop_fb_copy;
+	/* The same as loop_fb_copy but with compose_out origin. */
+	struct v4l2_rect		loop_vid_overlay;
+	/*
+	 * The part of the capture buffer that (after scaling) corresponds
+	 * to loop_vid_overlay.
+	 */
+	struct v4l2_rect		loop_vid_overlay_cap;
+
+	/* thread for generating video output stream */
+	struct task_struct		*kthread_vid_out;
+	unsigned long			jiffies_vid_out;
+	u32				out_seq_offset;
+	u32				out_seq_count;
+	bool				out_seq_resync;
+	u32				vid_out_seq_start;
+	u32				vid_out_seq_count;
+	bool				vid_out_streaming;
+	u32				vbi_out_seq_start;
+	u32				vbi_out_seq_count;
+	bool				vbi_out_streaming;
+	bool				stream_sliced_vbi_out;
+
+	/* SDR capture */
+	struct vb2_queue		vb_sdr_cap_q;
+	struct list_head		sdr_cap_active;
+	unsigned			sdr_adc_freq;
+	unsigned			sdr_fm_freq;
+	int				sdr_fixp_src_phase;
+	int				sdr_fixp_mod_phase;
+
+	bool				tstamp_src_is_soe;
+	bool				has_crop_cap;
+	bool				has_compose_cap;
+	bool				has_scaler_cap;
+	bool				has_crop_out;
+	bool				has_compose_out;
+	bool				has_scaler_out;
+
+	/* thread for generating SDR stream */
+	struct task_struct		*kthread_sdr_cap;
+	unsigned long			jiffies_sdr_cap;
+	u32				sdr_cap_seq_offset;
+	u32				sdr_cap_seq_count;
+	bool				sdr_cap_seq_resync;
+
+	/* RDS generator */
+	struct vivid_rds_gen		rds_gen;
+
+	/* Radio receiver */
+	unsigned			radio_rx_freq;
+	unsigned			radio_rx_audmode;
+	int				radio_rx_sig_qual;
+	unsigned			radio_rx_hw_seek_mode;
+	bool				radio_rx_hw_seek_prog_lim;
+	bool				radio_rx_rds_controls;
+	bool				radio_rx_rds_enabled;
+	unsigned			radio_rx_rds_use_alternates;
+	unsigned			radio_rx_rds_last_block;
+	struct v4l2_fh			*radio_rx_rds_owner;
+
+	/* Radio transmitter */
+	unsigned			radio_tx_freq;
+	unsigned			radio_tx_subchans;
+	bool				radio_tx_rds_controls;
+	unsigned			radio_tx_rds_last_block;
+	struct v4l2_fh			*radio_tx_rds_owner;
+
+	/* Shared between radio receiver and transmitter */
+	bool				radio_rds_loop;
+	struct timespec			radio_rds_init_ts;
+};
+
+static inline bool vivid_is_webcam(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == WEBCAM;
+}
+
+static inline bool vivid_is_tv_cap(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == TV;
+}
+
+static inline bool vivid_is_svid_cap(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == SVID;
+}
+
+static inline bool vivid_is_hdmi_cap(const struct vivid_dev *dev)
+{
+	return dev->input_type[dev->input] == HDMI;
+}
+
+static inline bool vivid_is_sdtv_cap(const struct vivid_dev *dev)
+{
+	return vivid_is_tv_cap(dev) || vivid_is_svid_cap(dev);
+}
+
+static inline bool vivid_is_svid_out(const struct vivid_dev *dev)
+{
+	return dev->output_type[dev->output] == SVID;
+}
+
+static inline bool vivid_is_hdmi_out(const struct vivid_dev *dev)
+{
+	return dev->output_type[dev->output] == HDMI;
+}
+
+void vivid_lock(struct vb2_queue *vq);
+void vivid_unlock(struct vb2_queue *vq);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-ctrls.c b/drivers/media/platform/vivid/vivid-ctrls.c
new file mode 100644
index 000000000000..d5cbf0038f24
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-ctrls.c
@@ -0,0 +1,1502 @@
+/*
+ * vivid-ctrls.c - control support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-vid-common.h"
+#include "vivid-radio-common.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+
+#define VIVID_CID_CUSTOM_BASE		(V4L2_CID_USER_BASE | 0xf000)
+#define VIVID_CID_BUTTON		(VIVID_CID_CUSTOM_BASE + 0)
+#define VIVID_CID_BOOLEAN		(VIVID_CID_CUSTOM_BASE + 1)
+#define VIVID_CID_INTEGER		(VIVID_CID_CUSTOM_BASE + 2)
+#define VIVID_CID_INTEGER64		(VIVID_CID_CUSTOM_BASE + 3)
+#define VIVID_CID_MENU			(VIVID_CID_CUSTOM_BASE + 4)
+#define VIVID_CID_STRING		(VIVID_CID_CUSTOM_BASE + 5)
+#define VIVID_CID_BITMASK		(VIVID_CID_CUSTOM_BASE + 6)
+#define VIVID_CID_INTMENU		(VIVID_CID_CUSTOM_BASE + 7)
+
+#define VIVID_CID_VIVID_BASE		(0x00f00000 | 0xf000)
+#define VIVID_CID_VIVID_CLASS		(0x00f00000 | 1)
+#define VIVID_CID_TEST_PATTERN		(VIVID_CID_VIVID_BASE + 0)
+#define VIVID_CID_OSD_TEXT_MODE		(VIVID_CID_VIVID_BASE + 1)
+#define VIVID_CID_HOR_MOVEMENT		(VIVID_CID_VIVID_BASE + 2)
+#define VIVID_CID_VERT_MOVEMENT		(VIVID_CID_VIVID_BASE + 3)
+#define VIVID_CID_SHOW_BORDER		(VIVID_CID_VIVID_BASE + 4)
+#define VIVID_CID_SHOW_SQUARE		(VIVID_CID_VIVID_BASE + 5)
+#define VIVID_CID_INSERT_SAV		(VIVID_CID_VIVID_BASE + 6)
+#define VIVID_CID_INSERT_EAV		(VIVID_CID_VIVID_BASE + 7)
+#define VIVID_CID_VBI_CAP_INTERLACED	(VIVID_CID_VIVID_BASE + 8)
+
+#define VIVID_CID_HFLIP			(VIVID_CID_VIVID_BASE + 20)
+#define VIVID_CID_VFLIP			(VIVID_CID_VIVID_BASE + 21)
+#define VIVID_CID_STD_ASPECT_RATIO	(VIVID_CID_VIVID_BASE + 22)
+#define VIVID_CID_DV_TIMINGS_ASPECT_RATIO	(VIVID_CID_VIVID_BASE + 23)
+#define VIVID_CID_TSTAMP_SRC		(VIVID_CID_VIVID_BASE + 24)
+#define VIVID_CID_COLORSPACE		(VIVID_CID_VIVID_BASE + 25)
+#define VIVID_CID_LIMITED_RGB_RANGE	(VIVID_CID_VIVID_BASE + 26)
+#define VIVID_CID_ALPHA_MODE		(VIVID_CID_VIVID_BASE + 27)
+#define VIVID_CID_HAS_CROP_CAP		(VIVID_CID_VIVID_BASE + 28)
+#define VIVID_CID_HAS_COMPOSE_CAP	(VIVID_CID_VIVID_BASE + 29)
+#define VIVID_CID_HAS_SCALER_CAP	(VIVID_CID_VIVID_BASE + 30)
+#define VIVID_CID_HAS_CROP_OUT		(VIVID_CID_VIVID_BASE + 31)
+#define VIVID_CID_HAS_COMPOSE_OUT	(VIVID_CID_VIVID_BASE + 32)
+#define VIVID_CID_HAS_SCALER_OUT	(VIVID_CID_VIVID_BASE + 33)
+#define VIVID_CID_LOOP_VIDEO		(VIVID_CID_VIVID_BASE + 34)
+#define VIVID_CID_SEQ_WRAP		(VIVID_CID_VIVID_BASE + 35)
+#define VIVID_CID_TIME_WRAP		(VIVID_CID_VIVID_BASE + 36)
+#define VIVID_CID_MAX_EDID_BLOCKS	(VIVID_CID_VIVID_BASE + 37)
+#define VIVID_CID_PERCENTAGE_FILL	(VIVID_CID_VIVID_BASE + 38)
+
+#define VIVID_CID_STD_SIGNAL_MODE	(VIVID_CID_VIVID_BASE + 60)
+#define VIVID_CID_STANDARD		(VIVID_CID_VIVID_BASE + 61)
+#define VIVID_CID_DV_TIMINGS_SIGNAL_MODE	(VIVID_CID_VIVID_BASE + 62)
+#define VIVID_CID_DV_TIMINGS		(VIVID_CID_VIVID_BASE + 63)
+#define VIVID_CID_PERC_DROPPED		(VIVID_CID_VIVID_BASE + 64)
+#define VIVID_CID_DISCONNECT		(VIVID_CID_VIVID_BASE + 65)
+#define VIVID_CID_DQBUF_ERROR		(VIVID_CID_VIVID_BASE + 66)
+#define VIVID_CID_QUEUE_SETUP_ERROR	(VIVID_CID_VIVID_BASE + 67)
+#define VIVID_CID_BUF_PREPARE_ERROR	(VIVID_CID_VIVID_BASE + 68)
+#define VIVID_CID_START_STR_ERROR	(VIVID_CID_VIVID_BASE + 69)
+#define VIVID_CID_QUEUE_ERROR		(VIVID_CID_VIVID_BASE + 70)
+#define VIVID_CID_CLEAR_FB		(VIVID_CID_VIVID_BASE + 71)
+
+#define VIVID_CID_RADIO_SEEK_MODE	(VIVID_CID_VIVID_BASE + 90)
+#define VIVID_CID_RADIO_SEEK_PROG_LIM	(VIVID_CID_VIVID_BASE + 91)
+#define VIVID_CID_RADIO_RX_RDS_RBDS	(VIVID_CID_VIVID_BASE + 92)
+#define VIVID_CID_RADIO_RX_RDS_BLOCKIO	(VIVID_CID_VIVID_BASE + 93)
+
+#define VIVID_CID_RADIO_TX_RDS_BLOCKIO	(VIVID_CID_VIVID_BASE + 94)
+
+
+/* General User Controls */
+
+static int vivid_user_gen_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_gen);
+
+	switch (ctrl->id) {
+	case VIVID_CID_DISCONNECT:
+		v4l2_info(&dev->v4l2_dev, "disconnect\n");
+		clear_bit(V4L2_FL_REGISTERED, &dev->vid_cap_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->vid_out_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->vbi_cap_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->vbi_out_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->sdr_cap_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->radio_rx_dev.flags);
+		clear_bit(V4L2_FL_REGISTERED, &dev->radio_tx_dev.flags);
+		break;
+	case VIVID_CID_CLEAR_FB:
+		vivid_clear_fb(dev);
+		break;
+	case VIVID_CID_BUTTON:
+		dev->button_pressed = 30;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_user_gen_ctrl_ops = {
+	.s_ctrl = vivid_user_gen_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_button = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_BUTTON,
+	.name = "Button",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_boolean = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_BOOLEAN,
+	.name = "Boolean",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.min = 0,
+	.max = 1,
+	.step = 1,
+	.def = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int32 = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_INTEGER,
+	.name = "Integer 32 Bits",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0xffffffff80000000ULL,
+	.max = 0x7fffffff,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int64 = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_INTEGER64,
+	.name = "Integer 64 Bits",
+	.type = V4L2_CTRL_TYPE_INTEGER64,
+	.min = 0x8000000000000000ULL,
+	.max = 0x7fffffffffffffffLL,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_menu_strings[] = {
+	"Menu Item 0 (Skipped)",
+	"Menu Item 1",
+	"Menu Item 2 (Skipped)",
+	"Menu Item 3",
+	"Menu Item 4",
+	"Menu Item 5 (Skipped)",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_menu = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_MENU,
+	.name = "Menu",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.max = 4,
+	.def = 3,
+	.menu_skip_mask = 0x04,
+	.qmenu = vivid_ctrl_menu_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_string = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_STRING,
+	.name = "String",
+	.type = V4L2_CTRL_TYPE_STRING,
+	.min = 2,
+	.max = 4,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_bitmask = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_BITMASK,
+	.name = "Bitmask",
+	.type = V4L2_CTRL_TYPE_BITMASK,
+	.def = 0x80002000,
+	.min = 0,
+	.max = 0x80402010,
+	.step = 0,
+};
+
+static const s64 vivid_ctrl_int_menu_values[] = {
+	1, 1, 2, 3, 5, 8, 13, 21, 42,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_int_menu = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_INTMENU,
+	.name = "Integer Menu",
+	.type = V4L2_CTRL_TYPE_INTEGER_MENU,
+	.min = 1,
+	.max = 8,
+	.def = 4,
+	.menu_skip_mask = 0x02,
+	.qmenu_int = vivid_ctrl_int_menu_values,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_disconnect = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_DISCONNECT,
+	.name = "Disconnect",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_clear_fb = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.id = VIVID_CID_CLEAR_FB,
+	.name = "Clear Framebuffer",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+
+/* Video User Controls */
+
+static int vivid_user_vid_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_vid);
+
+	switch (ctrl->id) {
+	case V4L2_CID_AUTOGAIN:
+		dev->gain->val = dev->jiffies_vid_cap & 0xff;
+		break;
+	}
+	return 0;
+}
+
+static int vivid_user_vid_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_user_vid);
+
+	switch (ctrl->id) {
+	case V4L2_CID_BRIGHTNESS:
+		dev->input_brightness[dev->input] = ctrl->val - dev->input * 128;
+		tpg_s_brightness(&dev->tpg, dev->input_brightness[dev->input]);
+		break;
+	case V4L2_CID_CONTRAST:
+		tpg_s_contrast(&dev->tpg, ctrl->val);
+		break;
+	case V4L2_CID_SATURATION:
+		tpg_s_saturation(&dev->tpg, ctrl->val);
+		break;
+	case V4L2_CID_HUE:
+		tpg_s_hue(&dev->tpg, ctrl->val);
+		break;
+	case V4L2_CID_HFLIP:
+		dev->hflip = ctrl->val;
+		tpg_s_hflip(&dev->tpg, dev->sensor_hflip ^ dev->hflip);
+		break;
+	case V4L2_CID_VFLIP:
+		dev->vflip = ctrl->val;
+		tpg_s_vflip(&dev->tpg, dev->sensor_vflip ^ dev->vflip);
+		break;
+	case V4L2_CID_ALPHA_COMPONENT:
+		tpg_s_alpha_component(&dev->tpg, ctrl->val);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_user_vid_ctrl_ops = {
+	.g_volatile_ctrl = vivid_user_vid_g_volatile_ctrl,
+	.s_ctrl = vivid_user_vid_s_ctrl,
+};
+
+
+/* Video Capture Controls */
+
+static int vivid_vid_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_cap);
+	unsigned i;
+
+	switch (ctrl->id) {
+	case VIVID_CID_TEST_PATTERN:
+		vivid_update_quality(dev);
+		tpg_s_pattern(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_COLORSPACE:
+		tpg_s_colorspace(&dev->tpg, ctrl->val);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		vivid_send_source_change(dev, WEBCAM);
+		break;
+	case V4L2_CID_DV_RX_RGB_RANGE:
+		if (!vivid_is_hdmi_cap(dev))
+			break;
+		tpg_s_rgb_range(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_LIMITED_RGB_RANGE:
+		tpg_s_real_rgb_range(&dev->tpg, ctrl->val ?
+				V4L2_DV_RGB_RANGE_LIMITED : V4L2_DV_RGB_RANGE_FULL);
+		break;
+	case VIVID_CID_ALPHA_MODE:
+		tpg_s_alpha_mode(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_HOR_MOVEMENT:
+		tpg_s_mv_hor_mode(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_VERT_MOVEMENT:
+		tpg_s_mv_vert_mode(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_OSD_TEXT_MODE:
+		dev->osd_mode = ctrl->val;
+		break;
+	case VIVID_CID_PERCENTAGE_FILL:
+		tpg_s_perc_fill(&dev->tpg, ctrl->val);
+		for (i = 0; i < VIDEO_MAX_FRAME; i++)
+			dev->must_blank[i] = ctrl->val < 100;
+		break;
+	case VIVID_CID_INSERT_SAV:
+		tpg_s_insert_sav(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_INSERT_EAV:
+		tpg_s_insert_eav(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_HFLIP:
+		dev->sensor_hflip = ctrl->val;
+		tpg_s_hflip(&dev->tpg, dev->sensor_hflip ^ dev->hflip);
+		break;
+	case VIVID_CID_VFLIP:
+		dev->sensor_vflip = ctrl->val;
+		tpg_s_vflip(&dev->tpg, dev->sensor_vflip ^ dev->vflip);
+		break;
+	case VIVID_CID_HAS_CROP_CAP:
+		dev->has_crop_cap = ctrl->val;
+		vivid_update_format_cap(dev, true);
+		break;
+	case VIVID_CID_HAS_COMPOSE_CAP:
+		dev->has_compose_cap = ctrl->val;
+		vivid_update_format_cap(dev, true);
+		break;
+	case VIVID_CID_HAS_SCALER_CAP:
+		dev->has_scaler_cap = ctrl->val;
+		vivid_update_format_cap(dev, true);
+		break;
+	case VIVID_CID_SHOW_BORDER:
+		tpg_s_show_border(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_SHOW_SQUARE:
+		tpg_s_show_square(&dev->tpg, ctrl->val);
+		break;
+	case VIVID_CID_STD_ASPECT_RATIO:
+		dev->std_aspect_ratio = ctrl->val;
+		tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+		break;
+	case VIVID_CID_DV_TIMINGS_SIGNAL_MODE:
+		dev->dv_timings_signal_mode = dev->ctrl_dv_timings_signal_mode->val;
+		if (dev->dv_timings_signal_mode == SELECTED_DV_TIMINGS)
+			dev->query_dv_timings = dev->ctrl_dv_timings->val;
+		v4l2_ctrl_activate(dev->ctrl_dv_timings,
+				dev->dv_timings_signal_mode == SELECTED_DV_TIMINGS);
+		vivid_update_quality(dev);
+		vivid_send_source_change(dev, HDMI);
+		break;
+	case VIVID_CID_DV_TIMINGS_ASPECT_RATIO:
+		dev->dv_timings_aspect_ratio = ctrl->val;
+		tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+		break;
+	case VIVID_CID_TSTAMP_SRC:
+		dev->tstamp_src_is_soe = ctrl->val;
+		dev->vb_vid_cap_q.timestamp_flags &= ~V4L2_BUF_FLAG_TSTAMP_SRC_MASK;
+		if (dev->tstamp_src_is_soe)
+			dev->vb_vid_cap_q.timestamp_flags |= V4L2_BUF_FLAG_TSTAMP_SRC_SOE;
+		break;
+	case VIVID_CID_MAX_EDID_BLOCKS:
+		dev->edid_max_blocks = ctrl->val;
+		if (dev->edid_blocks > dev->edid_max_blocks)
+			dev->edid_blocks = dev->edid_max_blocks;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vid_cap_ctrl_ops = {
+	.s_ctrl = vivid_vid_cap_s_ctrl,
+};
+
+static const char * const vivid_ctrl_hor_movement_strings[] = {
+	"Move Left Fast",
+	"Move Left",
+	"Move Left Slow",
+	"No Movement",
+	"Move Right Slow",
+	"Move Right",
+	"Move Right Fast",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_hor_movement = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HOR_MOVEMENT,
+	.name = "Horizontal Movement",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = TPG_MOVE_POS_FAST,
+	.def = TPG_MOVE_NONE,
+	.qmenu = vivid_ctrl_hor_movement_strings,
+};
+
+static const char * const vivid_ctrl_vert_movement_strings[] = {
+	"Move Up Fast",
+	"Move Up",
+	"Move Up Slow",
+	"No Movement",
+	"Move Down Slow",
+	"Move Down",
+	"Move Down Fast",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vert_movement = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_VERT_MOVEMENT,
+	.name = "Vertical Movement",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = TPG_MOVE_POS_FAST,
+	.def = TPG_MOVE_NONE,
+	.qmenu = vivid_ctrl_vert_movement_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_show_border = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_SHOW_BORDER,
+	.name = "Show Border",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_show_square = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_SHOW_SQUARE,
+	.name = "Show Square",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_osd_mode_strings[] = {
+	"All",
+	"Counters Only",
+	"None",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_osd_mode = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_OSD_TEXT_MODE,
+	.name = "OSD Text Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 2,
+	.qmenu = vivid_ctrl_osd_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_perc_fill = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_PERCENTAGE_FILL,
+	.name = "Fill Percentage of Frame",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 100,
+	.def = 100,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_insert_sav = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_INSERT_SAV,
+	.name = "Insert SAV Code in Image",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_insert_eav = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_INSERT_EAV,
+	.name = "Insert EAV Code in Image",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_hflip = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HFLIP,
+	.name = "Sensor Flipped Horizontally",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vflip = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_VFLIP,
+	.name = "Sensor Flipped Vertically",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_crop_cap = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HAS_CROP_CAP,
+	.name = "Enable Capture Cropping",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_compose_cap = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HAS_COMPOSE_CAP,
+	.name = "Enable Capture Composing",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_cap = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_HAS_SCALER_CAP,
+	.name = "Enable Capture Scaler",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_tstamp_src_strings[] = {
+	"End of Frame",
+	"Start of Exposure",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_tstamp_src = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_TSTAMP_SRC,
+	.name = "Timestamp Source",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 1,
+	.qmenu = vivid_ctrl_tstamp_src_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_std_aspect_ratio = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_STD_ASPECT_RATIO,
+	.name = "Standard Aspect Ratio",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.max = 4,
+	.def = 1,
+	.qmenu = tpg_aspect_strings,
+};
+
+static const char * const vivid_ctrl_dv_timings_signal_mode_strings[] = {
+	"Current DV Timings",
+	"No Signal",
+	"No Lock",
+	"Out of Range",
+	"Selected DV Timings",
+	"Cycle Through All DV Timings",
+	"Custom DV Timings",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dv_timings_signal_mode = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_DV_TIMINGS_SIGNAL_MODE,
+	.name = "DV Timings Signal Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 5,
+	.qmenu = vivid_ctrl_dv_timings_signal_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dv_timings_aspect_ratio = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_DV_TIMINGS_ASPECT_RATIO,
+	.name = "DV Timings Aspect Ratio",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 3,
+	.qmenu = tpg_aspect_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_max_edid_blocks = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_MAX_EDID_BLOCKS,
+	.name = "Maximum EDID Blocks",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 1,
+	.max = 256,
+	.def = 2,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_colorspace_strings[] = {
+	"",
+	"SMPTE 170M",
+	"SMPTE 240M",
+	"REC 709",
+	"", /* Skip Bt878 entry */
+	"470 System M",
+	"470 System BG",
+	"", /* Skip JPEG entry */
+	"sRGB",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_colorspace = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_COLORSPACE,
+	.name = "Colorspace",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.min = 1,
+	.max = 8,
+	.menu_skip_mask = (1 << 4) | (1 << 7),
+	.def = 8,
+	.qmenu = vivid_ctrl_colorspace_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_alpha_mode = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_ALPHA_MODE,
+	.name = "Apply Alpha To Red Only",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_limited_rgb_range = {
+	.ops = &vivid_vid_cap_ctrl_ops,
+	.id = VIVID_CID_LIMITED_RGB_RANGE,
+	.name = "Limited RGB Range (16-235)",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* VBI Capture Control */
+
+static int vivid_vbi_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vbi_cap);
+
+	switch (ctrl->id) {
+	case VIVID_CID_VBI_CAP_INTERLACED:
+		dev->vbi_cap_interlaced = ctrl->val;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vbi_cap_ctrl_ops = {
+	.s_ctrl = vivid_vbi_cap_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_vbi_cap_interlaced = {
+	.ops = &vivid_vbi_cap_ctrl_ops,
+	.id = VIVID_CID_VBI_CAP_INTERLACED,
+	.name = "Interlaced VBI Format",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* Video Output Controls */
+
+static int vivid_vid_out_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_vid_out);
+	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+
+	switch (ctrl->id) {
+	case VIVID_CID_HAS_CROP_OUT:
+		dev->has_crop_out = ctrl->val;
+		vivid_update_format_out(dev);
+		break;
+	case VIVID_CID_HAS_COMPOSE_OUT:
+		dev->has_compose_out = ctrl->val;
+		vivid_update_format_out(dev);
+		break;
+	case VIVID_CID_HAS_SCALER_OUT:
+		dev->has_scaler_out = ctrl->val;
+		vivid_update_format_out(dev);
+		break;
+	case V4L2_CID_DV_TX_MODE:
+		dev->dvi_d_out = ctrl->val == V4L2_DV_TX_MODE_DVI_D;
+		if (!vivid_is_hdmi_out(dev))
+			break;
+		if (!dev->dvi_d_out && (bt->standards & V4L2_DV_BT_STD_CEA861)) {
+			if (bt->width == 720 && bt->height <= 576)
+				dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+			else
+				dev->colorspace_out = V4L2_COLORSPACE_REC709;
+		} else {
+			dev->colorspace_out = V4L2_COLORSPACE_SRGB;
+		}
+		if (dev->loop_video)
+			vivid_send_source_change(dev, HDMI);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_vid_out_ctrl_ops = {
+	.s_ctrl = vivid_vid_out_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_crop_out = {
+	.ops = &vivid_vid_out_ctrl_ops,
+	.id = VIVID_CID_HAS_CROP_OUT,
+	.name = "Enable Output Cropping",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_compose_out = {
+	.ops = &vivid_vid_out_ctrl_ops,
+	.id = VIVID_CID_HAS_COMPOSE_OUT,
+	.name = "Enable Output Composing",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_has_scaler_out = {
+	.ops = &vivid_vid_out_ctrl_ops,
+	.id = VIVID_CID_HAS_SCALER_OUT,
+	.name = "Enable Output Scaler",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.def = 1,
+	.step = 1,
+};
+
+
+/* Streaming Controls */
+
+static int vivid_streaming_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_streaming);
+	struct timeval tv;
+
+	switch (ctrl->id) {
+	case VIVID_CID_DQBUF_ERROR:
+		dev->dqbuf_error = true;
+		break;
+	case VIVID_CID_PERC_DROPPED:
+		dev->perc_dropped_buffers = ctrl->val;
+		break;
+	case VIVID_CID_QUEUE_SETUP_ERROR:
+		dev->queue_setup_error = true;
+		break;
+	case VIVID_CID_BUF_PREPARE_ERROR:
+		dev->buf_prepare_error = true;
+		break;
+	case VIVID_CID_START_STR_ERROR:
+		dev->start_streaming_error = true;
+		break;
+	case VIVID_CID_QUEUE_ERROR:
+		if (dev->vb_vid_cap_q.start_streaming_called)
+			vb2_queue_error(&dev->vb_vid_cap_q);
+		if (dev->vb_vbi_cap_q.start_streaming_called)
+			vb2_queue_error(&dev->vb_vbi_cap_q);
+		if (dev->vb_vid_out_q.start_streaming_called)
+			vb2_queue_error(&dev->vb_vid_out_q);
+		if (dev->vb_vbi_out_q.start_streaming_called)
+			vb2_queue_error(&dev->vb_vbi_out_q);
+		if (dev->vb_sdr_cap_q.start_streaming_called)
+			vb2_queue_error(&dev->vb_sdr_cap_q);
+		break;
+	case VIVID_CID_SEQ_WRAP:
+		dev->seq_wrap = ctrl->val;
+		break;
+	case VIVID_CID_TIME_WRAP:
+		dev->time_wrap = ctrl->val;
+		if (ctrl->val == 0) {
+			dev->time_wrap_offset = 0;
+			break;
+		}
+		v4l2_get_timestamp(&tv);
+		dev->time_wrap_offset = -tv.tv_sec - 16;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_streaming_ctrl_ops = {
+	.s_ctrl = vivid_streaming_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_dqbuf_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_DQBUF_ERROR,
+	.name = "Inject V4L2_BUF_FLAG_ERROR",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_perc_dropped = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_PERC_DROPPED,
+	.name = "Percentage of Dropped Buffers",
+	.type = V4L2_CTRL_TYPE_INTEGER,
+	.min = 0,
+	.max = 100,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_queue_setup_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_QUEUE_SETUP_ERROR,
+	.name = "Inject VIDIOC_REQBUFS Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_buf_prepare_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_BUF_PREPARE_ERROR,
+	.name = "Inject VIDIOC_QBUF Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_start_streaming_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_START_STR_ERROR,
+	.name = "Inject VIDIOC_STREAMON Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_queue_error = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_QUEUE_ERROR,
+	.name = "Inject Fatal Streaming Error",
+	.type = V4L2_CTRL_TYPE_BUTTON,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_seq_wrap = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_SEQ_WRAP,
+	.name = "Wrap Sequence Number",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_time_wrap = {
+	.ops = &vivid_streaming_ctrl_ops,
+	.id = VIVID_CID_TIME_WRAP,
+	.name = "Wrap Timestamp",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* SDTV Capture Controls */
+
+static int vivid_sdtv_cap_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_sdtv_cap);
+
+	switch (ctrl->id) {
+	case VIVID_CID_STD_SIGNAL_MODE:
+		dev->std_signal_mode = dev->ctrl_std_signal_mode->val;
+		if (dev->std_signal_mode == SELECTED_STD)
+			dev->query_std = vivid_standard[dev->ctrl_standard->val];
+		v4l2_ctrl_activate(dev->ctrl_standard, dev->std_signal_mode == SELECTED_STD);
+		vivid_update_quality(dev);
+		vivid_send_source_change(dev, TV);
+		vivid_send_source_change(dev, SVID);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_sdtv_cap_ctrl_ops = {
+	.s_ctrl = vivid_sdtv_cap_s_ctrl,
+};
+
+static const char * const vivid_ctrl_std_signal_mode_strings[] = {
+	"Current Standard",
+	"No Signal",
+	"No Lock",
+	"",
+	"Selected Standard",
+	"Cycle Through All Standards",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_std_signal_mode = {
+	.ops = &vivid_sdtv_cap_ctrl_ops,
+	.id = VIVID_CID_STD_SIGNAL_MODE,
+	.name = "Standard Signal Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 5,
+	.menu_skip_mask = 1 << 3,
+	.qmenu = vivid_ctrl_std_signal_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_standard = {
+	.ops = &vivid_sdtv_cap_ctrl_ops,
+	.id = VIVID_CID_STANDARD,
+	.name = "Standard",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 14,
+	.qmenu = vivid_ctrl_standard_strings,
+};
+
+
+
+/* Radio Receiver Controls */
+
+static int vivid_radio_rx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_radio_rx);
+
+	switch (ctrl->id) {
+	case VIVID_CID_RADIO_SEEK_MODE:
+		dev->radio_rx_hw_seek_mode = ctrl->val;
+		break;
+	case VIVID_CID_RADIO_SEEK_PROG_LIM:
+		dev->radio_rx_hw_seek_prog_lim = ctrl->val;
+		break;
+	case VIVID_CID_RADIO_RX_RDS_RBDS:
+		dev->rds_gen.use_rbds = ctrl->val;
+		break;
+	case VIVID_CID_RADIO_RX_RDS_BLOCKIO:
+		dev->radio_rx_rds_controls = ctrl->val;
+		dev->radio_rx_caps &= ~V4L2_CAP_READWRITE;
+		dev->radio_rx_rds_use_alternates = false;
+		if (!dev->radio_rx_rds_controls) {
+			dev->radio_rx_caps |= V4L2_CAP_READWRITE;
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, 0);
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, 0);
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, 0);
+			__v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, 0);
+			__v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, "");
+			__v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, "");
+		}
+		v4l2_ctrl_activate(dev->radio_rx_rds_pty, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_psname, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_radiotext, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_ta, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_tp, dev->radio_rx_rds_controls);
+		v4l2_ctrl_activate(dev->radio_rx_rds_ms, dev->radio_rx_rds_controls);
+		break;
+	case V4L2_CID_RDS_RECEPTION:
+		dev->radio_rx_rds_enabled = ctrl->val;
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_radio_rx_ctrl_ops = {
+	.s_ctrl = vivid_radio_rx_s_ctrl,
+};
+
+static const char * const vivid_ctrl_radio_rds_mode_strings[] = {
+	"Block I/O",
+	"Controls",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_rx_rds_blockio = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_RX_RDS_BLOCKIO,
+	.name = "RDS Rx I/O Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.qmenu = vivid_ctrl_radio_rds_mode_strings,
+	.max = 1,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_rx_rds_rbds = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_RX_RDS_RBDS,
+	.name = "Generate RBDS Instead of RDS",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+static const char * const vivid_ctrl_radio_hw_seek_mode_strings[] = {
+	"Bounded",
+	"Wrap Around",
+	"Both",
+	NULL,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_hw_seek_mode = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_SEEK_MODE,
+	.name = "Radio HW Seek Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.max = 2,
+	.qmenu = vivid_ctrl_radio_hw_seek_mode_strings,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_hw_seek_prog_lim = {
+	.ops = &vivid_radio_rx_ctrl_ops,
+	.id = VIVID_CID_RADIO_SEEK_PROG_LIM,
+	.name = "Radio Programmable HW Seek",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+/* Radio Transmitter Controls */
+
+static int vivid_radio_tx_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_radio_tx);
+
+	switch (ctrl->id) {
+	case VIVID_CID_RADIO_TX_RDS_BLOCKIO:
+		dev->radio_tx_rds_controls = ctrl->val;
+		dev->radio_tx_caps &= ~V4L2_CAP_READWRITE;
+		if (!dev->radio_tx_rds_controls)
+			dev->radio_tx_caps |= V4L2_CAP_READWRITE;
+		break;
+	case V4L2_CID_RDS_TX_PTY:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, ctrl->val);
+		break;
+	case V4L2_CID_RDS_TX_PS_NAME:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, ctrl->p_new.p_char);
+		break;
+	case V4L2_CID_RDS_TX_RADIO_TEXT:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, ctrl->p_new.p_char);
+		break;
+	case V4L2_CID_RDS_TX_TRAFFIC_ANNOUNCEMENT:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, ctrl->val);
+		break;
+	case V4L2_CID_RDS_TX_TRAFFIC_PROGRAM:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, ctrl->val);
+		break;
+	case V4L2_CID_RDS_TX_MUSIC_SPEECH:
+		if (dev->radio_rx_rds_controls)
+			v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, ctrl->val);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_radio_tx_ctrl_ops = {
+	.s_ctrl = vivid_radio_tx_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_radio_tx_rds_blockio = {
+	.ops = &vivid_radio_tx_ctrl_ops,
+	.id = VIVID_CID_RADIO_TX_RDS_BLOCKIO,
+	.name = "RDS Tx I/O Mode",
+	.type = V4L2_CTRL_TYPE_MENU,
+	.qmenu = vivid_ctrl_radio_rds_mode_strings,
+	.max = 1,
+	.def = 1,
+};
+
+
+
+/* Video Loop Control */
+
+static int vivid_loop_out_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct vivid_dev *dev = container_of(ctrl->handler, struct vivid_dev, ctrl_hdl_loop_out);
+
+	switch (ctrl->id) {
+	case VIVID_CID_LOOP_VIDEO:
+		dev->loop_video = ctrl->val;
+		vivid_update_quality(dev);
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+		break;
+	}
+	return 0;
+}
+
+static const struct v4l2_ctrl_ops vivid_loop_out_ctrl_ops = {
+	.s_ctrl = vivid_loop_out_s_ctrl,
+};
+
+static const struct v4l2_ctrl_config vivid_ctrl_loop_video = {
+	.ops = &vivid_loop_out_ctrl_ops,
+	.id = VIVID_CID_LOOP_VIDEO,
+	.name = "Loop Video",
+	.type = V4L2_CTRL_TYPE_BOOLEAN,
+	.max = 1,
+	.step = 1,
+};
+
+
+static const struct v4l2_ctrl_config vivid_ctrl_class = {
+	.ops = &vivid_user_gen_ctrl_ops,
+	.flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
+	.id = VIVID_CID_VIVID_CLASS,
+	.name = "Vivid Controls",
+	.type = V4L2_CTRL_TYPE_CTRL_CLASS,
+};
+
+int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
+		bool show_ccs_out, bool no_error_inj,
+		bool has_sdtv, bool has_hdmi)
+{
+	struct v4l2_ctrl_handler *hdl_user_gen = &dev->ctrl_hdl_user_gen;
+	struct v4l2_ctrl_handler *hdl_user_vid = &dev->ctrl_hdl_user_vid;
+	struct v4l2_ctrl_handler *hdl_user_aud = &dev->ctrl_hdl_user_aud;
+	struct v4l2_ctrl_handler *hdl_streaming = &dev->ctrl_hdl_streaming;
+	struct v4l2_ctrl_handler *hdl_sdtv_cap = &dev->ctrl_hdl_sdtv_cap;
+	struct v4l2_ctrl_handler *hdl_loop_out = &dev->ctrl_hdl_loop_out;
+	struct v4l2_ctrl_handler *hdl_vid_cap = &dev->ctrl_hdl_vid_cap;
+	struct v4l2_ctrl_handler *hdl_vid_out = &dev->ctrl_hdl_vid_out;
+	struct v4l2_ctrl_handler *hdl_vbi_cap = &dev->ctrl_hdl_vbi_cap;
+	struct v4l2_ctrl_handler *hdl_vbi_out = &dev->ctrl_hdl_vbi_out;
+	struct v4l2_ctrl_handler *hdl_radio_rx = &dev->ctrl_hdl_radio_rx;
+	struct v4l2_ctrl_handler *hdl_radio_tx = &dev->ctrl_hdl_radio_tx;
+	struct v4l2_ctrl_handler *hdl_sdr_cap = &dev->ctrl_hdl_sdr_cap;
+	struct v4l2_ctrl_config vivid_ctrl_dv_timings = {
+		.ops = &vivid_vid_cap_ctrl_ops,
+		.id = VIVID_CID_DV_TIMINGS,
+		.name = "DV Timings",
+		.type = V4L2_CTRL_TYPE_MENU,
+	};
+	int i;
+
+	v4l2_ctrl_handler_init(hdl_user_gen, 10);
+	v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_user_vid, 9);
+	v4l2_ctrl_new_custom(hdl_user_vid, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_user_aud, 2);
+	v4l2_ctrl_new_custom(hdl_user_aud, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_streaming, 8);
+	v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_sdtv_cap, 2);
+	v4l2_ctrl_new_custom(hdl_sdtv_cap, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_loop_out, 1);
+	v4l2_ctrl_new_custom(hdl_loop_out, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vid_cap, 55);
+	v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vid_out, 26);
+	v4l2_ctrl_new_custom(hdl_vid_out, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vbi_cap, 21);
+	v4l2_ctrl_new_custom(hdl_vbi_cap, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_vbi_out, 19);
+	v4l2_ctrl_new_custom(hdl_vbi_out, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_radio_rx, 17);
+	v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_radio_tx, 17);
+	v4l2_ctrl_new_custom(hdl_radio_tx, &vivid_ctrl_class, NULL);
+	v4l2_ctrl_handler_init(hdl_sdr_cap, 18);
+	v4l2_ctrl_new_custom(hdl_sdr_cap, &vivid_ctrl_class, NULL);
+
+	/* User Controls */
+	dev->volume = v4l2_ctrl_new_std(hdl_user_aud, NULL,
+		V4L2_CID_AUDIO_VOLUME, 0, 255, 1, 200);
+	dev->mute = v4l2_ctrl_new_std(hdl_user_aud, NULL,
+		V4L2_CID_AUDIO_MUTE, 0, 1, 1, 0);
+	if (dev->has_vid_cap) {
+		dev->brightness = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
+		for (i = 0; i < MAX_INPUTS; i++)
+			dev->input_brightness[i] = 128;
+		dev->contrast = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_CONTRAST, 0, 255, 1, 128);
+		dev->saturation = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_SATURATION, 0, 255, 1, 128);
+		dev->hue = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_HUE, -128, 128, 1, 0);
+		v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_HFLIP, 0, 1, 1, 0);
+		v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_VFLIP, 0, 1, 1, 0);
+		dev->autogain = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_AUTOGAIN, 0, 1, 1, 1);
+		dev->gain = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_GAIN, 0, 255, 1, 100);
+		dev->alpha = v4l2_ctrl_new_std(hdl_user_vid, &vivid_user_vid_ctrl_ops,
+			V4L2_CID_ALPHA_COMPONENT, 0, 255, 1, 0);
+	}
+	dev->button = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_button, NULL);
+	dev->int32 = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int32, NULL);
+	dev->int64 = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int64, NULL);
+	dev->boolean = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_boolean, NULL);
+	dev->menu = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_menu, NULL);
+	dev->string = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_string, NULL);
+	dev->bitmask = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_bitmask, NULL);
+	dev->int_menu = v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_int_menu, NULL);
+
+	if (dev->has_vid_cap) {
+		/* Image Processing Controls */
+		struct v4l2_ctrl_config vivid_ctrl_test_pattern = {
+			.ops = &vivid_vid_cap_ctrl_ops,
+			.id = VIVID_CID_TEST_PATTERN,
+			.name = "Test Pattern",
+			.type = V4L2_CTRL_TYPE_MENU,
+			.max = TPG_PAT_NOISE,
+			.qmenu = tpg_pattern_strings,
+		};
+
+		dev->test_pattern = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_test_pattern, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_perc_fill, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_hor_movement, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_vert_movement, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_osd_mode, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_show_border, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_show_square, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_hflip, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_vflip, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_sav, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_insert_eav, NULL);
+		if (show_ccs_cap) {
+			dev->ctrl_has_crop_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_has_crop_cap, NULL);
+			dev->ctrl_has_compose_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_has_compose_cap, NULL);
+			dev->ctrl_has_scaler_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+				&vivid_ctrl_has_scaler_cap, NULL);
+		}
+
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_tstamp_src, NULL);
+		dev->colorspace = v4l2_ctrl_new_custom(hdl_vid_cap,
+			&vivid_ctrl_colorspace, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_alpha_mode, NULL);
+	}
+
+	if (dev->has_vid_out && show_ccs_out) {
+		dev->ctrl_has_crop_out = v4l2_ctrl_new_custom(hdl_vid_out,
+			&vivid_ctrl_has_crop_out, NULL);
+		dev->ctrl_has_compose_out = v4l2_ctrl_new_custom(hdl_vid_out,
+			&vivid_ctrl_has_compose_out, NULL);
+		dev->ctrl_has_scaler_out = v4l2_ctrl_new_custom(hdl_vid_out,
+			&vivid_ctrl_has_scaler_out, NULL);
+	}
+
+	/*
+	 * Testing this driver with v4l2-compliance will trigger the error
+	 * injection controls, and after that nothing will work as expected.
+	 * So we have a module option to drop these error injecting controls
+	 * allowing us to run v4l2_compliance again.
+	 */
+	if (!no_error_inj) {
+		v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_disconnect, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_dqbuf_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_perc_dropped, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_queue_setup_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_buf_prepare_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_start_streaming_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_queue_error, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_seq_wrap, NULL);
+		v4l2_ctrl_new_custom(hdl_streaming, &vivid_ctrl_time_wrap, NULL);
+	}
+
+	if (has_sdtv && (dev->has_vid_cap || dev->has_vbi_cap)) {
+		if (dev->has_vid_cap)
+			v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_std_aspect_ratio, NULL);
+		dev->ctrl_std_signal_mode = v4l2_ctrl_new_custom(hdl_sdtv_cap,
+			&vivid_ctrl_std_signal_mode, NULL);
+		dev->ctrl_standard = v4l2_ctrl_new_custom(hdl_sdtv_cap,
+			&vivid_ctrl_standard, NULL);
+		if (dev->ctrl_std_signal_mode)
+			v4l2_ctrl_cluster(2, &dev->ctrl_std_signal_mode);
+		if (dev->has_raw_vbi_cap)
+			v4l2_ctrl_new_custom(hdl_vbi_cap, &vivid_ctrl_vbi_cap_interlaced, NULL);
+	}
+
+	if (has_hdmi && dev->has_vid_cap) {
+		dev->ctrl_dv_timings_signal_mode = v4l2_ctrl_new_custom(hdl_vid_cap,
+					&vivid_ctrl_dv_timings_signal_mode, NULL);
+
+		vivid_ctrl_dv_timings.max = dev->query_dv_timings_size - 1;
+		vivid_ctrl_dv_timings.qmenu =
+			(const char * const *)dev->query_dv_timings_qmenu;
+		dev->ctrl_dv_timings = v4l2_ctrl_new_custom(hdl_vid_cap,
+			&vivid_ctrl_dv_timings, NULL);
+		if (dev->ctrl_dv_timings_signal_mode)
+			v4l2_ctrl_cluster(2, &dev->ctrl_dv_timings_signal_mode);
+
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_dv_timings_aspect_ratio, NULL);
+		v4l2_ctrl_new_custom(hdl_vid_cap, &vivid_ctrl_max_edid_blocks, NULL);
+		dev->real_rgb_range_cap = v4l2_ctrl_new_custom(hdl_vid_cap,
+			&vivid_ctrl_limited_rgb_range, NULL);
+		dev->rgb_range_cap = v4l2_ctrl_new_std_menu(hdl_vid_cap,
+			&vivid_vid_cap_ctrl_ops,
+			V4L2_CID_DV_RX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+	}
+	if (has_hdmi && dev->has_vid_out) {
+		/*
+		 * We aren't doing anything with this at the moment, but
+		 * HDMI outputs typically have this controls.
+		 */
+		dev->ctrl_tx_rgb_range = v4l2_ctrl_new_std_menu(hdl_vid_out, NULL,
+			V4L2_CID_DV_TX_RGB_RANGE, V4L2_DV_RGB_RANGE_FULL,
+			0, V4L2_DV_RGB_RANGE_AUTO);
+		dev->ctrl_tx_mode = v4l2_ctrl_new_std_menu(hdl_vid_out, NULL,
+			V4L2_CID_DV_TX_MODE, V4L2_DV_TX_MODE_HDMI,
+			0, V4L2_DV_TX_MODE_HDMI);
+	}
+	if ((dev->has_vid_cap && dev->has_vid_out) ||
+	    (dev->has_vbi_cap && dev->has_vbi_out))
+		v4l2_ctrl_new_custom(hdl_loop_out, &vivid_ctrl_loop_video, NULL);
+
+	if (dev->has_fb)
+		v4l2_ctrl_new_custom(hdl_user_gen, &vivid_ctrl_clear_fb, NULL);
+
+	if (dev->has_radio_rx) {
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_hw_seek_mode, NULL);
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_hw_seek_prog_lim, NULL);
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_rx_rds_blockio, NULL);
+		v4l2_ctrl_new_custom(hdl_radio_rx, &vivid_ctrl_radio_rx_rds_rbds, NULL);
+		v4l2_ctrl_new_std(hdl_radio_rx, &vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RECEPTION, 0, 1, 1, 1);
+		dev->radio_rx_rds_pty = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_PTY, 0, 31, 1, 0);
+		dev->radio_rx_rds_psname = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_PS_NAME, 0, 8, 8, 0);
+		dev->radio_rx_rds_radiotext = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_RADIO_TEXT, 0, 64, 64, 0);
+		dev->radio_rx_rds_ta = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_TRAFFIC_ANNOUNCEMENT, 0, 1, 1, 0);
+		dev->radio_rx_rds_tp = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_TRAFFIC_PROGRAM, 0, 1, 1, 0);
+		dev->radio_rx_rds_ms = v4l2_ctrl_new_std(hdl_radio_rx,
+			&vivid_radio_rx_ctrl_ops,
+			V4L2_CID_RDS_RX_MUSIC_SPEECH, 0, 1, 1, 1);
+	}
+	if (dev->has_radio_tx) {
+		v4l2_ctrl_new_custom(hdl_radio_tx,
+			&vivid_ctrl_radio_tx_rds_blockio, NULL);
+		dev->radio_tx_rds_pi = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_PI, 0, 0xffff, 1, 0x8088);
+		dev->radio_tx_rds_pty = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_PTY, 0, 31, 1, 3);
+		dev->radio_tx_rds_psname = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_PS_NAME, 0, 8, 8, 0);
+		if (dev->radio_tx_rds_psname)
+			v4l2_ctrl_s_ctrl_string(dev->radio_tx_rds_psname, "VIVID-TX");
+		dev->radio_tx_rds_radiotext = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_RADIO_TEXT, 0, 64 * 2, 64, 0);
+		if (dev->radio_tx_rds_radiotext)
+			v4l2_ctrl_s_ctrl_string(dev->radio_tx_rds_radiotext,
+			       "This is a VIVID default Radio Text template text, change at will");
+		dev->radio_tx_rds_mono_stereo = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_MONO_STEREO, 0, 1, 1, 1);
+		dev->radio_tx_rds_art_head = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_ARTIFICIAL_HEAD, 0, 1, 1, 0);
+		dev->radio_tx_rds_compressed = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_COMPRESSED, 0, 1, 1, 0);
+		dev->radio_tx_rds_dyn_pty = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_DYNAMIC_PTY, 0, 1, 1, 0);
+		dev->radio_tx_rds_ta = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_TRAFFIC_ANNOUNCEMENT, 0, 1, 1, 0);
+		dev->radio_tx_rds_tp = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_TRAFFIC_PROGRAM, 0, 1, 1, 1);
+		dev->radio_tx_rds_ms = v4l2_ctrl_new_std(hdl_radio_tx,
+			&vivid_radio_tx_ctrl_ops,
+			V4L2_CID_RDS_TX_MUSIC_SPEECH, 0, 1, 1, 1);
+	}
+	if (hdl_user_gen->error)
+		return hdl_user_gen->error;
+	if (hdl_user_vid->error)
+		return hdl_user_vid->error;
+	if (hdl_user_aud->error)
+		return hdl_user_aud->error;
+	if (hdl_streaming->error)
+		return hdl_streaming->error;
+	if (hdl_sdr_cap->error)
+		return hdl_sdr_cap->error;
+	if (hdl_loop_out->error)
+		return hdl_loop_out->error;
+
+	if (dev->autogain)
+		v4l2_ctrl_auto_cluster(2, &dev->autogain, 0, true);
+
+	if (dev->has_vid_cap) {
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_vid, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_user_aud, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_streaming, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_cap, hdl_sdtv_cap, NULL);
+		if (hdl_vid_cap->error)
+			return hdl_vid_cap->error;
+		dev->vid_cap_dev.ctrl_handler = hdl_vid_cap;
+	}
+	if (dev->has_vid_out) {
+		v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_out, hdl_user_aud, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_out, hdl_streaming, NULL);
+		v4l2_ctrl_add_handler(hdl_vid_out, hdl_loop_out, NULL);
+		if (hdl_vid_out->error)
+			return hdl_vid_out->error;
+		dev->vid_out_dev.ctrl_handler = hdl_vid_out;
+	}
+	if (dev->has_vbi_cap) {
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_streaming, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_cap, hdl_sdtv_cap, NULL);
+		if (hdl_vbi_cap->error)
+			return hdl_vbi_cap->error;
+		dev->vbi_cap_dev.ctrl_handler = hdl_vbi_cap;
+	}
+	if (dev->has_vbi_out) {
+		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_streaming, NULL);
+		v4l2_ctrl_add_handler(hdl_vbi_out, hdl_loop_out, NULL);
+		if (hdl_vbi_out->error)
+			return hdl_vbi_out->error;
+		dev->vbi_out_dev.ctrl_handler = hdl_vbi_out;
+	}
+	if (dev->has_radio_rx) {
+		v4l2_ctrl_add_handler(hdl_radio_rx, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_radio_rx, hdl_user_aud, NULL);
+		if (hdl_radio_rx->error)
+			return hdl_radio_rx->error;
+		dev->radio_rx_dev.ctrl_handler = hdl_radio_rx;
+	}
+	if (dev->has_radio_tx) {
+		v4l2_ctrl_add_handler(hdl_radio_tx, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_radio_tx, hdl_user_aud, NULL);
+		if (hdl_radio_tx->error)
+			return hdl_radio_tx->error;
+		dev->radio_tx_dev.ctrl_handler = hdl_radio_tx;
+	}
+	if (dev->has_sdr_cap) {
+		v4l2_ctrl_add_handler(hdl_sdr_cap, hdl_user_gen, NULL);
+		v4l2_ctrl_add_handler(hdl_sdr_cap, hdl_streaming, NULL);
+		if (hdl_sdr_cap->error)
+			return hdl_sdr_cap->error;
+		dev->sdr_cap_dev.ctrl_handler = hdl_sdr_cap;
+	}
+	return 0;
+}
+
+void vivid_free_controls(struct vivid_dev *dev)
+{
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vid_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vid_out);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vbi_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_vbi_out);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_radio_rx);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_radio_tx);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_sdr_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_gen);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_vid);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_user_aud);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_streaming);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_sdtv_cap);
+	v4l2_ctrl_handler_free(&dev->ctrl_hdl_loop_out);
+}
diff --git a/drivers/media/platform/vivid/vivid-ctrls.h b/drivers/media/platform/vivid/vivid-ctrls.h
new file mode 100644
index 000000000000..9bcca9d56359
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-ctrls.h
@@ -0,0 +1,34 @@
+/*
+ * vivid-ctrls.h - control support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_CTRLS_H_
+#define _VIVID_CTRLS_H_
+
+enum vivid_hw_seek_modes {
+	VIVID_HW_SEEK_BOUNDED,
+	VIVID_HW_SEEK_WRAP,
+	VIVID_HW_SEEK_BOTH,
+};
+
+int vivid_create_controls(struct vivid_dev *dev, bool show_ccs_cap,
+		bool show_ccs_out, bool no_error_inj,
+		bool has_sdtv, bool has_hdmi);
+void vivid_free_controls(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-kthread-cap.c b/drivers/media/platform/vivid/vivid-kthread-cap.c
new file mode 100644
index 000000000000..39a67cfae120
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-cap.c
@@ -0,0 +1,886 @@
+/*
+ * vivid-kthread-cap.h - video/vbi capture thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/random.h>
+#include <linux/v4l2-dv-timings.h>
+#include <asm/div64.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+#include "vivid-kthread-cap.h"
+
+static inline v4l2_std_id vivid_get_std_cap(const struct vivid_dev *dev)
+{
+	if (vivid_is_sdtv_cap(dev))
+		return dev->std_cap;
+	return 0;
+}
+
+static void copy_pix(struct vivid_dev *dev, int win_y, int win_x,
+			u16 *cap, const u16 *osd)
+{
+	u16 out;
+	int left = dev->overlay_out_left;
+	int top = dev->overlay_out_top;
+	int fb_x = win_x + left;
+	int fb_y = win_y + top;
+	int i;
+
+	out = *cap;
+	*cap = *osd;
+	if (dev->bitmap_out) {
+		const u8 *p = dev->bitmap_out;
+		unsigned stride = (dev->compose_out.width + 7) / 8;
+
+		win_x -= dev->compose_out.left;
+		win_y -= dev->compose_out.top;
+		if (!(p[stride * win_y + win_x / 8] & (1 << (win_x & 7))))
+			return;
+	}
+
+	for (i = 0; i < dev->clipcount_out; i++) {
+		struct v4l2_rect *r = &dev->clips_out[i].c;
+
+		if (fb_y >= r->top && fb_y < r->top + r->height &&
+		    fb_x >= r->left && fb_x < r->left + r->width)
+			return;
+	}
+	if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_CHROMAKEY) &&
+	    *osd != dev->chromakey_out)
+		return;
+	if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_SRC_CHROMAKEY) &&
+	    out == dev->chromakey_out)
+		return;
+	if (dev->fmt_cap->alpha_mask) {
+		if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_GLOBAL_ALPHA) &&
+		    dev->global_alpha_out)
+			return;
+		if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_LOCAL_ALPHA) &&
+		    *cap & dev->fmt_cap->alpha_mask)
+			return;
+		if ((dev->fbuf_out_flags & V4L2_FBUF_FLAG_LOCAL_INV_ALPHA) &&
+		    !(*cap & dev->fmt_cap->alpha_mask))
+			return;
+	}
+	*cap = out;
+}
+
+static void blend_line(struct vivid_dev *dev, unsigned y_offset, unsigned x_offset,
+		u8 *vcapbuf, const u8 *vosdbuf,
+		unsigned width, unsigned pixsize)
+{
+	unsigned x;
+
+	for (x = 0; x < width; x++, vcapbuf += pixsize, vosdbuf += pixsize) {
+		copy_pix(dev, y_offset, x_offset + x,
+			 (u16 *)vcapbuf, (const u16 *)vosdbuf);
+	}
+}
+
+static void scale_line(const u8 *src, u8 *dst, unsigned srcw, unsigned dstw, unsigned twopixsize)
+{
+	/* Coarse scaling with Bresenham */
+	unsigned int_part;
+	unsigned fract_part;
+	unsigned src_x = 0;
+	unsigned error = 0;
+	unsigned x;
+
+	/*
+	 * We always combine two pixels to prevent color bleed in the packed
+	 * yuv case.
+	 */
+	srcw /= 2;
+	dstw /= 2;
+	int_part = srcw / dstw;
+	fract_part = srcw % dstw;
+	for (x = 0; x < dstw; x++, dst += twopixsize) {
+		memcpy(dst, src + src_x * twopixsize, twopixsize);
+		src_x += int_part;
+		error += fract_part;
+		if (error >= dstw) {
+			error -= dstw;
+			src_x++;
+		}
+	}
+}
+
+/*
+ * Precalculate the rectangles needed to perform video looping:
+ *
+ * The nominal pipeline is that the video output buffer is cropped by
+ * crop_out, scaled to compose_out, overlaid with the output overlay,
+ * cropped on the capture side by crop_cap and scaled again to the video
+ * capture buffer using compose_cap.
+ *
+ * To keep things efficient we calculate the intersection of compose_out
+ * and crop_cap (since that's the only part of the video that will
+ * actually end up in the capture buffer), determine which part of the
+ * video output buffer that is and which part of the video capture buffer
+ * so we can scale the video straight from the output buffer to the capture
+ * buffer without any intermediate steps.
+ *
+ * If we need to deal with an output overlay, then there is no choice and
+ * that intermediate step still has to be taken. For the output overlay
+ * support we calculate the intersection of the framebuffer and the overlay
+ * window (which may be partially or wholly outside of the framebuffer
+ * itself) and the intersection of that with loop_vid_copy (i.e. the part of
+ * the actual looped video that will be overlaid). The result is calculated
+ * both in framebuffer coordinates (loop_fb_copy) and compose_out coordinates
+ * (loop_vid_overlay). Finally calculate the part of the capture buffer that
+ * will receive that overlaid video.
+ */
+static void vivid_precalc_copy_rects(struct vivid_dev *dev)
+{
+	/* Framebuffer rectangle */
+	struct v4l2_rect r_fb = {
+		0, 0, dev->display_width, dev->display_height
+	};
+	/* Overlay window rectangle in framebuffer coordinates */
+	struct v4l2_rect r_overlay = {
+		dev->overlay_out_left, dev->overlay_out_top,
+		dev->compose_out.width, dev->compose_out.height
+	};
+
+	dev->loop_vid_copy = rect_intersect(&dev->crop_cap, &dev->compose_out);
+
+	dev->loop_vid_out = dev->loop_vid_copy;
+	rect_scale(&dev->loop_vid_out, &dev->compose_out, &dev->crop_out);
+	dev->loop_vid_out.left += dev->crop_out.left;
+	dev->loop_vid_out.top += dev->crop_out.top;
+
+	dev->loop_vid_cap = dev->loop_vid_copy;
+	rect_scale(&dev->loop_vid_cap, &dev->crop_cap, &dev->compose_cap);
+
+	dprintk(dev, 1,
+		"loop_vid_copy: %dx%d@%dx%d loop_vid_out: %dx%d@%dx%d loop_vid_cap: %dx%d@%dx%d\n",
+		dev->loop_vid_copy.width, dev->loop_vid_copy.height,
+		dev->loop_vid_copy.left, dev->loop_vid_copy.top,
+		dev->loop_vid_out.width, dev->loop_vid_out.height,
+		dev->loop_vid_out.left, dev->loop_vid_out.top,
+		dev->loop_vid_cap.width, dev->loop_vid_cap.height,
+		dev->loop_vid_cap.left, dev->loop_vid_cap.top);
+
+	r_overlay = rect_intersect(&r_fb, &r_overlay);
+
+	/* shift r_overlay to the same origin as compose_out */
+	r_overlay.left += dev->compose_out.left - dev->overlay_out_left;
+	r_overlay.top += dev->compose_out.top - dev->overlay_out_top;
+
+	dev->loop_vid_overlay = rect_intersect(&r_overlay, &dev->loop_vid_copy);
+	dev->loop_fb_copy = dev->loop_vid_overlay;
+
+	/* shift dev->loop_fb_copy back again to the fb origin */
+	dev->loop_fb_copy.left -= dev->compose_out.left - dev->overlay_out_left;
+	dev->loop_fb_copy.top -= dev->compose_out.top - dev->overlay_out_top;
+
+	dev->loop_vid_overlay_cap = dev->loop_vid_overlay;
+	rect_scale(&dev->loop_vid_overlay_cap, &dev->crop_cap, &dev->compose_cap);
+
+	dprintk(dev, 1,
+		"loop_fb_copy: %dx%d@%dx%d loop_vid_overlay: %dx%d@%dx%d loop_vid_overlay_cap: %dx%d@%dx%d\n",
+		dev->loop_fb_copy.width, dev->loop_fb_copy.height,
+		dev->loop_fb_copy.left, dev->loop_fb_copy.top,
+		dev->loop_vid_overlay.width, dev->loop_vid_overlay.height,
+		dev->loop_vid_overlay.left, dev->loop_vid_overlay.top,
+		dev->loop_vid_overlay_cap.width, dev->loop_vid_overlay_cap.height,
+		dev->loop_vid_overlay_cap.left, dev->loop_vid_overlay_cap.top);
+}
+
+static int vivid_copy_buffer(struct vivid_dev *dev, unsigned p, u8 *vcapbuf,
+		struct vivid_buffer *vid_cap_buf)
+{
+	bool blank = dev->must_blank[vid_cap_buf->vb.v4l2_buf.index];
+	struct tpg_data *tpg = &dev->tpg;
+	struct vivid_buffer *vid_out_buf = NULL;
+	unsigned pixsize = tpg_g_twopixelsize(tpg, p) / 2;
+	unsigned img_width = dev->compose_cap.width;
+	unsigned img_height = dev->compose_cap.height;
+	unsigned stride_cap = tpg->bytesperline[p];
+	unsigned stride_out = dev->bytesperline_out[p];
+	unsigned stride_osd = dev->display_byte_stride;
+	unsigned hmax = (img_height * tpg->perc_fill) / 100;
+	u8 *voutbuf;
+	u8 *vosdbuf = NULL;
+	unsigned y;
+	bool blend = dev->bitmap_out || dev->clipcount_out || dev->fbuf_out_flags;
+	/* Coarse scaling with Bresenham */
+	unsigned vid_out_int_part;
+	unsigned vid_out_fract_part;
+	unsigned vid_out_y = 0;
+	unsigned vid_out_error = 0;
+	unsigned vid_overlay_int_part = 0;
+	unsigned vid_overlay_fract_part = 0;
+	unsigned vid_overlay_y = 0;
+	unsigned vid_overlay_error = 0;
+	unsigned vid_cap_right;
+	bool quick;
+
+	vid_out_int_part = dev->loop_vid_out.height / dev->loop_vid_cap.height;
+	vid_out_fract_part = dev->loop_vid_out.height % dev->loop_vid_cap.height;
+
+	if (!list_empty(&dev->vid_out_active))
+		vid_out_buf = list_entry(dev->vid_out_active.next,
+					 struct vivid_buffer, list);
+	if (vid_out_buf == NULL)
+		return -ENODATA;
+
+	vid_cap_buf->vb.v4l2_buf.field = vid_out_buf->vb.v4l2_buf.field;
+
+	voutbuf = vb2_plane_vaddr(&vid_out_buf->vb, p) +
+				  vid_out_buf->vb.v4l2_planes[p].data_offset;
+	voutbuf += dev->loop_vid_out.left * pixsize + dev->loop_vid_out.top * stride_out;
+	vcapbuf += dev->compose_cap.left * pixsize + dev->compose_cap.top * stride_cap;
+
+	if (dev->loop_vid_copy.width == 0 || dev->loop_vid_copy.height == 0) {
+		/*
+		 * If there is nothing to copy, then just fill the capture window
+		 * with black.
+		 */
+		for (y = 0; y < hmax; y++, vcapbuf += stride_cap)
+			memcpy(vcapbuf, tpg->black_line[p], img_width * pixsize);
+		return 0;
+	}
+
+	if (dev->overlay_out_enabled &&
+	    dev->loop_vid_overlay.width && dev->loop_vid_overlay.height) {
+		vosdbuf = dev->video_vbase;
+		vosdbuf += dev->loop_fb_copy.left * pixsize +
+			   dev->loop_fb_copy.top * stride_osd;
+		vid_overlay_int_part = dev->loop_vid_overlay.height /
+				       dev->loop_vid_overlay_cap.height;
+		vid_overlay_fract_part = dev->loop_vid_overlay.height %
+					 dev->loop_vid_overlay_cap.height;
+	}
+
+	vid_cap_right = dev->loop_vid_cap.left + dev->loop_vid_cap.width;
+	/* quick is true if no video scaling is needed */
+	quick = dev->loop_vid_out.width == dev->loop_vid_cap.width;
+
+	dev->cur_scaled_line = dev->loop_vid_out.height;
+	for (y = 0; y < hmax; y++, vcapbuf += stride_cap) {
+		/* osdline is true if this line requires overlay blending */
+		bool osdline = vosdbuf && y >= dev->loop_vid_overlay_cap.top &&
+			  y < dev->loop_vid_overlay_cap.top + dev->loop_vid_overlay_cap.height;
+
+		/*
+		 * If this line of the capture buffer doesn't get any video, then
+		 * just fill with black.
+		 */
+		if (y < dev->loop_vid_cap.top ||
+		    y >= dev->loop_vid_cap.top + dev->loop_vid_cap.height) {
+			memcpy(vcapbuf, tpg->black_line[p], img_width * pixsize);
+			continue;
+		}
+
+		/* fill the left border with black */
+		if (dev->loop_vid_cap.left)
+			memcpy(vcapbuf, tpg->black_line[p], dev->loop_vid_cap.left * pixsize);
+
+		/* fill the right border with black */
+		if (vid_cap_right < img_width)
+			memcpy(vcapbuf + vid_cap_right * pixsize,
+				tpg->black_line[p], (img_width - vid_cap_right) * pixsize);
+
+		if (quick && !osdline) {
+			memcpy(vcapbuf + dev->loop_vid_cap.left * pixsize,
+			       voutbuf + vid_out_y * stride_out,
+			       dev->loop_vid_cap.width * pixsize);
+			goto update_vid_out_y;
+		}
+		if (dev->cur_scaled_line == vid_out_y) {
+			memcpy(vcapbuf + dev->loop_vid_cap.left * pixsize,
+			       dev->scaled_line,
+			       dev->loop_vid_cap.width * pixsize);
+			goto update_vid_out_y;
+		}
+		if (!osdline) {
+			scale_line(voutbuf + vid_out_y * stride_out, dev->scaled_line,
+				dev->loop_vid_out.width, dev->loop_vid_cap.width,
+				tpg_g_twopixelsize(tpg, p));
+		} else {
+			/*
+			 * Offset in bytes within loop_vid_copy to the start of the
+			 * loop_vid_overlay rectangle.
+			 */
+			unsigned offset =
+				(dev->loop_vid_overlay.left - dev->loop_vid_copy.left) * pixsize;
+			u8 *osd = vosdbuf + vid_overlay_y * stride_osd;
+
+			scale_line(voutbuf + vid_out_y * stride_out, dev->blended_line,
+				dev->loop_vid_out.width, dev->loop_vid_copy.width,
+				tpg_g_twopixelsize(tpg, p));
+			if (blend)
+				blend_line(dev, vid_overlay_y + dev->loop_vid_overlay.top,
+					   dev->loop_vid_overlay.left,
+					   dev->blended_line + offset, osd,
+					   dev->loop_vid_overlay.width, pixsize);
+			else
+				memcpy(dev->blended_line + offset,
+				       osd, dev->loop_vid_overlay.width * pixsize);
+			scale_line(dev->blended_line, dev->scaled_line,
+					dev->loop_vid_copy.width, dev->loop_vid_cap.width,
+					tpg_g_twopixelsize(tpg, p));
+		}
+		dev->cur_scaled_line = vid_out_y;
+		memcpy(vcapbuf + dev->loop_vid_cap.left * pixsize,
+		       dev->scaled_line,
+		       dev->loop_vid_cap.width * pixsize);
+
+update_vid_out_y:
+		if (osdline) {
+			vid_overlay_y += vid_overlay_int_part;
+			vid_overlay_error += vid_overlay_fract_part;
+			if (vid_overlay_error >= dev->loop_vid_overlay_cap.height) {
+				vid_overlay_error -= dev->loop_vid_overlay_cap.height;
+				vid_overlay_y++;
+			}
+		}
+		vid_out_y += vid_out_int_part;
+		vid_out_error += vid_out_fract_part;
+		if (vid_out_error >= dev->loop_vid_cap.height) {
+			vid_out_error -= dev->loop_vid_cap.height;
+			vid_out_y++;
+		}
+	}
+
+	if (!blank)
+		return 0;
+	for (; y < img_height; y++, vcapbuf += stride_cap)
+		memcpy(vcapbuf, tpg->contrast_line[p], img_width * pixsize);
+	return 0;
+}
+
+static void vivid_fillbuff(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_cap) ? 2 : 1;
+	unsigned line_height = 16 / factor;
+	bool is_tv = vivid_is_sdtv_cap(dev);
+	bool is_60hz = is_tv && (dev->std_cap & V4L2_STD_525_60);
+	unsigned p;
+	int line = 1;
+	u8 *basep[TPG_MAX_PLANES][2];
+	unsigned ms;
+	char str[100];
+	s32 gain;
+	bool is_loop = false;
+
+	if (dev->loop_video && dev->can_loop_video &&
+	    ((vivid_is_svid_cap(dev) && !VIVID_INVALID_SIGNAL(dev->std_signal_mode)) ||
+	     (vivid_is_hdmi_cap(dev) && !VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode))))
+		is_loop = true;
+
+	buf->vb.v4l2_buf.sequence = dev->vid_cap_seq_count;
+	/*
+	 * Take the timestamp now if the timestamp source is set to
+	 * "Start of Exposure".
+	 */
+	if (dev->tstamp_src_is_soe)
+		v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE) {
+		/*
+		 * 60 Hz standards start with the bottom field, 50 Hz standards
+		 * with the top field. So if the 0-based seq_count is even,
+		 * then the field is TOP for 50 Hz and BOTTOM for 60 Hz
+		 * standards.
+		 */
+		buf->vb.v4l2_buf.field = ((dev->vid_cap_seq_count & 1) ^ is_60hz) ?
+			V4L2_FIELD_TOP : V4L2_FIELD_BOTTOM;
+		/*
+		 * The sequence counter counts frames, not fields. So divide
+		 * by two.
+		 */
+		buf->vb.v4l2_buf.sequence /= 2;
+	} else {
+		buf->vb.v4l2_buf.field = dev->field_cap;
+	}
+	tpg_s_field(&dev->tpg, buf->vb.v4l2_buf.field);
+	tpg_s_perc_fill_blank(&dev->tpg, dev->must_blank[buf->vb.v4l2_buf.index]);
+
+	vivid_precalc_copy_rects(dev);
+
+	for (p = 0; p < tpg_g_planes(&dev->tpg); p++) {
+		void *vbuf = vb2_plane_vaddr(&buf->vb, p);
+
+		/*
+		 * The first plane of a multiplanar format has a non-zero
+		 * data_offset. This helps testing whether the application
+		 * correctly supports non-zero data offsets.
+		 */
+		if (dev->fmt_cap->data_offset[p]) {
+			memset(vbuf, dev->fmt_cap->data_offset[p] & 0xff,
+			       dev->fmt_cap->data_offset[p]);
+			vbuf += dev->fmt_cap->data_offset[p];
+		}
+		tpg_calc_text_basep(&dev->tpg, basep, p, vbuf);
+		if (!is_loop || vivid_copy_buffer(dev, p, vbuf, buf))
+			tpg_fillbuffer(&dev->tpg, vivid_get_std_cap(dev), p, vbuf);
+	}
+	dev->must_blank[buf->vb.v4l2_buf.index] = false;
+
+	/* Updates stream time, only update at the start of a new frame. */
+	if (dev->field_cap != V4L2_FIELD_ALTERNATE || (buf->vb.v4l2_buf.sequence & 1) == 0)
+		dev->ms_vid_cap = jiffies_to_msecs(jiffies - dev->jiffies_vid_cap);
+
+	ms = dev->ms_vid_cap;
+	if (dev->osd_mode <= 1) {
+		snprintf(str, sizeof(str), " %02d:%02d:%02d:%03d %u%s",
+				(ms / (60 * 60 * 1000)) % 24,
+				(ms / (60 * 1000)) % 60,
+				(ms / 1000) % 60,
+				ms % 1000,
+				buf->vb.v4l2_buf.sequence,
+				(dev->field_cap == V4L2_FIELD_ALTERNATE) ?
+					(buf->vb.v4l2_buf.field == V4L2_FIELD_TOP ?
+					 " top" : " bottom") : "");
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+	}
+	if (dev->osd_mode == 0) {
+		snprintf(str, sizeof(str), " %dx%d, input %d ",
+				dev->src_rect.width, dev->src_rect.height, dev->input);
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+
+		gain = v4l2_ctrl_g_ctrl(dev->gain);
+		mutex_lock(dev->ctrl_hdl_user_vid.lock);
+		snprintf(str, sizeof(str),
+			" brightness %3d, contrast %3d, saturation %3d, hue %d ",
+			dev->brightness->cur.val,
+			dev->contrast->cur.val,
+			dev->saturation->cur.val,
+			dev->hue->cur.val);
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+		snprintf(str, sizeof(str),
+			" autogain %d, gain %3d, alpha 0x%02x ",
+			dev->autogain->cur.val, gain, dev->alpha->cur.val);
+		mutex_unlock(dev->ctrl_hdl_user_vid.lock);
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+		mutex_lock(dev->ctrl_hdl_user_aud.lock);
+		snprintf(str, sizeof(str),
+			" volume %3d, mute %d ",
+			dev->volume->cur.val, dev->mute->cur.val);
+		mutex_unlock(dev->ctrl_hdl_user_aud.lock);
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+		mutex_lock(dev->ctrl_hdl_user_gen.lock);
+		snprintf(str, sizeof(str), " int32 %d, int64 %lld, bitmask %08x ",
+			dev->int32->cur.val,
+			*dev->int64->p_cur.p_s64,
+			dev->bitmask->cur.val);
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+		snprintf(str, sizeof(str), " boolean %d, menu %s, string \"%s\" ",
+			dev->boolean->cur.val,
+			dev->menu->qmenu[dev->menu->cur.val],
+			dev->string->p_cur.p_char);
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+		snprintf(str, sizeof(str), " integer_menu %lld, value %d ",
+			dev->int_menu->qmenu_int[dev->int_menu->cur.val],
+			dev->int_menu->cur.val);
+		mutex_unlock(dev->ctrl_hdl_user_gen.lock);
+		tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+		if (dev->button_pressed) {
+			dev->button_pressed--;
+			snprintf(str, sizeof(str), " button pressed!");
+			tpg_gen_text(&dev->tpg, basep, line++ * line_height, 16, str);
+		}
+	}
+
+	/*
+	 * If "End of Frame" is specified at the timestamp source, then take
+	 * the timestamp now.
+	 */
+	if (!dev->tstamp_src_is_soe)
+		v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+	buf->vb.v4l2_buf.timestamp.tv_sec += dev->time_wrap_offset;
+}
+
+/*
+ * Return true if this pixel coordinate is a valid video pixel.
+ */
+static bool valid_pix(struct vivid_dev *dev, int win_y, int win_x, int fb_y, int fb_x)
+{
+	int i;
+
+	if (dev->bitmap_cap) {
+		/*
+		 * Only if the corresponding bit in the bitmap is set can
+		 * the video pixel be shown. Coordinates are relative to
+		 * the overlay window set by VIDIOC_S_FMT.
+		 */
+		const u8 *p = dev->bitmap_cap;
+		unsigned stride = (dev->compose_cap.width + 7) / 8;
+
+		if (!(p[stride * win_y + win_x / 8] & (1 << (win_x & 7))))
+			return false;
+	}
+
+	for (i = 0; i < dev->clipcount_cap; i++) {
+		/*
+		 * Only if the framebuffer coordinate is not in any of the
+		 * clip rectangles will be video pixel be shown.
+		 */
+		struct v4l2_rect *r = &dev->clips_cap[i].c;
+
+		if (fb_y >= r->top && fb_y < r->top + r->height &&
+		    fb_x >= r->left && fb_x < r->left + r->width)
+			return false;
+	}
+	return true;
+}
+
+/*
+ * Draw the image into the overlay buffer.
+ * Note that the combination of overlay and multiplanar is not supported.
+ */
+static void vivid_overlay(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	struct tpg_data *tpg = &dev->tpg;
+	unsigned pixsize = tpg_g_twopixelsize(tpg, 0) / 2;
+	void *vbase = dev->fb_vbase_cap;
+	void *vbuf = vb2_plane_vaddr(&buf->vb, 0);
+	unsigned img_width = dev->compose_cap.width;
+	unsigned img_height = dev->compose_cap.height;
+	unsigned stride = tpg->bytesperline[0];
+	/* if quick is true, then valid_pix() doesn't have to be called */
+	bool quick = dev->bitmap_cap == NULL && dev->clipcount_cap == 0;
+	int x, y, w, out_x = 0;
+
+	if ((dev->overlay_cap_field == V4L2_FIELD_TOP ||
+	     dev->overlay_cap_field == V4L2_FIELD_BOTTOM) &&
+	    dev->overlay_cap_field != buf->vb.v4l2_buf.field)
+		return;
+
+	vbuf += dev->compose_cap.left * pixsize + dev->compose_cap.top * stride;
+	x = dev->overlay_cap_left;
+	w = img_width;
+	if (x < 0) {
+		out_x = -x;
+		w = w - out_x;
+		x = 0;
+	} else {
+		w = dev->fb_cap.fmt.width - x;
+		if (w > img_width)
+			w = img_width;
+	}
+	if (w <= 0)
+		return;
+	if (dev->overlay_cap_top >= 0)
+		vbase += dev->overlay_cap_top * dev->fb_cap.fmt.bytesperline;
+	for (y = dev->overlay_cap_top;
+	     y < dev->overlay_cap_top + (int)img_height;
+	     y++, vbuf += stride) {
+		int px;
+
+		if (y < 0 || y > dev->fb_cap.fmt.height)
+			continue;
+		if (quick) {
+			memcpy(vbase + x * pixsize,
+			       vbuf + out_x * pixsize, w * pixsize);
+			vbase += dev->fb_cap.fmt.bytesperline;
+			continue;
+		}
+		for (px = 0; px < w; px++) {
+			if (!valid_pix(dev, y - dev->overlay_cap_top,
+				       px + out_x, y, px + x))
+				continue;
+			memcpy(vbase + (px + x) * pixsize,
+			       vbuf + (px + out_x) * pixsize,
+			       pixsize);
+		}
+		vbase += dev->fb_cap.fmt.bytesperline;
+	}
+}
+
+static void vivid_thread_vid_cap_tick(struct vivid_dev *dev, int dropped_bufs)
+{
+	struct vivid_buffer *vid_cap_buf = NULL;
+	struct vivid_buffer *vbi_cap_buf = NULL;
+
+	dprintk(dev, 1, "Video Capture Thread Tick\n");
+
+	while (dropped_bufs-- > 1)
+		tpg_update_mv_count(&dev->tpg,
+				dev->field_cap == V4L2_FIELD_NONE ||
+				dev->field_cap == V4L2_FIELD_ALTERNATE);
+
+	/* Drop a certain percentage of buffers. */
+	if (dev->perc_dropped_buffers &&
+	    prandom_u32_max(100) < dev->perc_dropped_buffers)
+		goto update_mv;
+
+	spin_lock(&dev->slock);
+	if (!list_empty(&dev->vid_cap_active)) {
+		vid_cap_buf = list_entry(dev->vid_cap_active.next, struct vivid_buffer, list);
+		list_del(&vid_cap_buf->list);
+	}
+	if (!list_empty(&dev->vbi_cap_active)) {
+		if (dev->field_cap != V4L2_FIELD_ALTERNATE ||
+		    (dev->vbi_cap_seq_count & 1)) {
+			vbi_cap_buf = list_entry(dev->vbi_cap_active.next,
+						 struct vivid_buffer, list);
+			list_del(&vbi_cap_buf->list);
+		}
+	}
+	spin_unlock(&dev->slock);
+
+	if (!vid_cap_buf && !vbi_cap_buf)
+		goto update_mv;
+
+	if (vid_cap_buf) {
+		/* Fill buffer */
+		vivid_fillbuff(dev, vid_cap_buf);
+		dprintk(dev, 1, "filled buffer %d\n",
+			vid_cap_buf->vb.v4l2_buf.index);
+
+		/* Handle overlay */
+		if (dev->overlay_cap_owner && dev->fb_cap.base &&
+				dev->fb_cap.fmt.pixelformat == dev->fmt_cap->fourcc)
+			vivid_overlay(dev, vid_cap_buf);
+
+		vb2_buffer_done(&vid_cap_buf->vb, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vid_cap buffer %d done\n",
+				vid_cap_buf->vb.v4l2_buf.index);
+	}
+
+	if (vbi_cap_buf) {
+		if (dev->stream_sliced_vbi_cap)
+			vivid_sliced_vbi_cap_process(dev, vbi_cap_buf);
+		else
+			vivid_raw_vbi_cap_process(dev, vbi_cap_buf);
+		vb2_buffer_done(&vbi_cap_buf->vb, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vbi_cap %d done\n",
+				vbi_cap_buf->vb.v4l2_buf.index);
+	}
+	dev->dqbuf_error = false;
+
+update_mv:
+	/* Update the test pattern movement counters */
+	tpg_update_mv_count(&dev->tpg, dev->field_cap == V4L2_FIELD_NONE ||
+				       dev->field_cap == V4L2_FIELD_ALTERNATE);
+}
+
+static int vivid_thread_vid_cap(void *data)
+{
+	struct vivid_dev *dev = data;
+	u64 numerators_since_start;
+	u64 buffers_since_start;
+	u64 next_jiffies_since_start;
+	unsigned long jiffies_since_start;
+	unsigned long cur_jiffies;
+	unsigned wait_jiffies;
+	unsigned numerator;
+	unsigned denominator;
+	int dropped_bufs;
+
+	dprintk(dev, 1, "Video Capture Thread Start\n");
+
+	set_freezable();
+
+	/* Resets frame counters */
+	dev->cap_seq_offset = 0;
+	dev->cap_seq_count = 0;
+	dev->cap_seq_resync = false;
+	dev->jiffies_vid_cap = jiffies;
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&dev->mutex);
+		cur_jiffies = jiffies;
+		if (dev->cap_seq_resync) {
+			dev->jiffies_vid_cap = cur_jiffies;
+			dev->cap_seq_offset = dev->cap_seq_count + 1;
+			dev->cap_seq_count = 0;
+			dev->cap_seq_resync = false;
+		}
+		numerator = dev->timeperframe_vid_cap.numerator;
+		denominator = dev->timeperframe_vid_cap.denominator;
+
+		if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+			denominator *= 2;
+
+		/* Calculate the number of jiffies since we started streaming */
+		jiffies_since_start = cur_jiffies - dev->jiffies_vid_cap;
+		/* Get the number of buffers streamed since the start */
+		buffers_since_start = (u64)jiffies_since_start * denominator +
+				      (HZ * numerator) / 2;
+		do_div(buffers_since_start, HZ * numerator);
+
+		/*
+		 * After more than 0xf0000000 (rounded down to a multiple of
+		 * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+		 * jiffies have passed since we started streaming reset the
+		 * counters and keep track of the sequence offset.
+		 */
+		if (jiffies_since_start > JIFFIES_RESYNC) {
+			dev->jiffies_vid_cap = cur_jiffies;
+			dev->cap_seq_offset = buffers_since_start;
+			buffers_since_start = 0;
+		}
+		dropped_bufs = buffers_since_start + dev->cap_seq_offset - dev->cap_seq_count;
+		dev->cap_seq_count = buffers_since_start + dev->cap_seq_offset;
+		dev->vid_cap_seq_count = dev->cap_seq_count - dev->vid_cap_seq_start;
+		dev->vbi_cap_seq_count = dev->cap_seq_count - dev->vbi_cap_seq_start;
+
+		vivid_thread_vid_cap_tick(dev, dropped_bufs);
+
+		/*
+		 * Calculate the number of 'numerators' streamed since we started,
+		 * including the current buffer.
+		 */
+		numerators_since_start = ++buffers_since_start * numerator;
+
+		/* And the number of jiffies since we started */
+		jiffies_since_start = jiffies - dev->jiffies_vid_cap;
+
+		mutex_unlock(&dev->mutex);
+
+		/*
+		 * Calculate when that next buffer is supposed to start
+		 * in jiffies since we started streaming.
+		 */
+		next_jiffies_since_start = numerators_since_start * HZ +
+					   denominator / 2;
+		do_div(next_jiffies_since_start, denominator);
+		/* If it is in the past, then just schedule asap */
+		if (next_jiffies_since_start < jiffies_since_start)
+			next_jiffies_since_start = jiffies_since_start;
+
+		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+		schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+	}
+	dprintk(dev, 1, "Video Capture Thread End\n");
+	return 0;
+}
+
+static void vivid_grab_controls(struct vivid_dev *dev, bool grab)
+{
+	v4l2_ctrl_grab(dev->ctrl_has_crop_cap, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_compose_cap, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_scaler_cap, grab);
+}
+
+int vivid_start_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_cap) {
+		u32 seq_count = dev->cap_seq_count + dev->seq_wrap * 128;
+
+		if (pstreaming == &dev->vid_cap_streaming)
+			dev->vid_cap_seq_start = seq_count;
+		else
+			dev->vbi_cap_seq_start = seq_count;
+		*pstreaming = true;
+		return 0;
+	}
+
+	/* Resets frame counters */
+	tpg_init_mv_count(&dev->tpg);
+
+	dev->vid_cap_seq_start = dev->seq_wrap * 128;
+	dev->vbi_cap_seq_start = dev->seq_wrap * 128;
+
+	dev->kthread_vid_cap = kthread_run(vivid_thread_vid_cap, dev,
+			"%s-vid-cap", dev->v4l2_dev.name);
+
+	if (IS_ERR(dev->kthread_vid_cap)) {
+		v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+		return PTR_ERR(dev->kthread_vid_cap);
+	}
+	*pstreaming = true;
+	vivid_grab_controls(dev, true);
+
+	dprintk(dev, 1, "returning from %s\n", __func__);
+	return 0;
+}
+
+void vivid_stop_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_cap == NULL)
+		return;
+
+	*pstreaming = false;
+	if (pstreaming == &dev->vid_cap_streaming) {
+		/* Release all active buffers */
+		while (!list_empty(&dev->vid_cap_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vid_cap_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vid_cap buffer %d done\n",
+				buf->vb.v4l2_buf.index);
+		}
+	}
+
+	if (pstreaming == &dev->vbi_cap_streaming) {
+		while (!list_empty(&dev->vbi_cap_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vbi_cap_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vbi_cap buffer %d done\n",
+				buf->vb.v4l2_buf.index);
+		}
+	}
+
+	if (dev->vid_cap_streaming || dev->vbi_cap_streaming)
+		return;
+
+	/* shutdown control thread */
+	vivid_grab_controls(dev, false);
+	mutex_unlock(&dev->mutex);
+	kthread_stop(dev->kthread_vid_cap);
+	dev->kthread_vid_cap = NULL;
+	mutex_lock(&dev->mutex);
+}
diff --git a/drivers/media/platform/vivid/vivid-kthread-cap.h b/drivers/media/platform/vivid/vivid-kthread-cap.h
new file mode 100644
index 000000000000..5b92fc9a0d04
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-cap.h
@@ -0,0 +1,26 @@
+/*
+ * vivid-kthread-cap.h - video/vbi capture thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_KTHREAD_CAP_H_
+#define _VIVID_KTHREAD_CAP_H_
+
+int vivid_start_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming);
+void vivid_stop_generating_vid_cap(struct vivid_dev *dev, bool *pstreaming);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-kthread-out.c b/drivers/media/platform/vivid/vivid-kthread-out.c
new file mode 100644
index 000000000000..d9f36ccd7efb
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-out.c
@@ -0,0 +1,305 @@
+/*
+ * vivid-kthread-out.h - video/vbi output thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/random.h>
+#include <linux/v4l2-dv-timings.h>
+#include <asm/div64.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-dv-timings.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-vid-cap.h"
+#include "vivid-vid-out.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-rx.h"
+#include "vivid-radio-tx.h"
+#include "vivid-sdr-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-out.h"
+#include "vivid-osd.h"
+#include "vivid-ctrls.h"
+#include "vivid-kthread-out.h"
+
+static void vivid_thread_vid_out_tick(struct vivid_dev *dev)
+{
+	struct vivid_buffer *vid_out_buf = NULL;
+	struct vivid_buffer *vbi_out_buf = NULL;
+
+	dprintk(dev, 1, "Video Output Thread Tick\n");
+
+	/* Drop a certain percentage of buffers. */
+	if (dev->perc_dropped_buffers &&
+	    prandom_u32_max(100) < dev->perc_dropped_buffers)
+		return;
+
+	spin_lock(&dev->slock);
+	/*
+	 * Only dequeue buffer if there is at least one more pending.
+	 * This makes video loopback possible.
+	 */
+	if (!list_empty(&dev->vid_out_active) &&
+	    !list_is_singular(&dev->vid_out_active)) {
+		vid_out_buf = list_entry(dev->vid_out_active.next,
+					 struct vivid_buffer, list);
+		list_del(&vid_out_buf->list);
+	}
+	if (!list_empty(&dev->vbi_out_active) &&
+	    (dev->field_out != V4L2_FIELD_ALTERNATE ||
+	     (dev->vbi_out_seq_count & 1))) {
+		vbi_out_buf = list_entry(dev->vbi_out_active.next,
+					 struct vivid_buffer, list);
+		list_del(&vbi_out_buf->list);
+	}
+	spin_unlock(&dev->slock);
+
+	if (!vid_out_buf && !vbi_out_buf)
+		return;
+
+	if (vid_out_buf) {
+		vid_out_buf->vb.v4l2_buf.sequence = dev->vid_out_seq_count;
+		if (dev->field_out == V4L2_FIELD_ALTERNATE) {
+			/*
+			 * The sequence counter counts frames, not fields. So divide
+			 * by two.
+			 */
+			vid_out_buf->vb.v4l2_buf.sequence /= 2;
+		}
+		v4l2_get_timestamp(&vid_out_buf->vb.v4l2_buf.timestamp);
+		vid_out_buf->vb.v4l2_buf.timestamp.tv_sec += dev->time_wrap_offset;
+		vb2_buffer_done(&vid_out_buf->vb, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vid_out buffer %d done\n",
+			vid_out_buf->vb.v4l2_buf.index);
+	}
+
+	if (vbi_out_buf) {
+		if (dev->stream_sliced_vbi_out)
+			vivid_sliced_vbi_out_process(dev, vbi_out_buf);
+
+		vbi_out_buf->vb.v4l2_buf.sequence = dev->vbi_out_seq_count;
+		v4l2_get_timestamp(&vbi_out_buf->vb.v4l2_buf.timestamp);
+		vbi_out_buf->vb.v4l2_buf.timestamp.tv_sec += dev->time_wrap_offset;
+		vb2_buffer_done(&vbi_out_buf->vb, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dprintk(dev, 2, "vbi_out buffer %d done\n",
+			vbi_out_buf->vb.v4l2_buf.index);
+	}
+	dev->dqbuf_error = false;
+}
+
+static int vivid_thread_vid_out(void *data)
+{
+	struct vivid_dev *dev = data;
+	u64 numerators_since_start;
+	u64 buffers_since_start;
+	u64 next_jiffies_since_start;
+	unsigned long jiffies_since_start;
+	unsigned long cur_jiffies;
+	unsigned wait_jiffies;
+	unsigned numerator;
+	unsigned denominator;
+
+	dprintk(dev, 1, "Video Output Thread Start\n");
+
+	set_freezable();
+
+	/* Resets frame counters */
+	dev->out_seq_offset = 0;
+	if (dev->seq_wrap)
+		dev->out_seq_count = 0xffffff80U;
+	dev->jiffies_vid_out = jiffies;
+	dev->vid_out_seq_start = dev->vbi_out_seq_start = 0;
+	dev->out_seq_resync = false;
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&dev->mutex);
+		cur_jiffies = jiffies;
+		if (dev->out_seq_resync) {
+			dev->jiffies_vid_out = cur_jiffies;
+			dev->out_seq_offset = dev->out_seq_count + 1;
+			dev->out_seq_count = 0;
+			dev->out_seq_resync = false;
+		}
+		numerator = dev->timeperframe_vid_out.numerator;
+		denominator = dev->timeperframe_vid_out.denominator;
+
+		if (dev->field_out == V4L2_FIELD_ALTERNATE)
+			denominator *= 2;
+
+		/* Calculate the number of jiffies since we started streaming */
+		jiffies_since_start = cur_jiffies - dev->jiffies_vid_out;
+		/* Get the number of buffers streamed since the start */
+		buffers_since_start = (u64)jiffies_since_start * denominator +
+				      (HZ * numerator) / 2;
+		do_div(buffers_since_start, HZ * numerator);
+
+		/*
+		 * After more than 0xf0000000 (rounded down to a multiple of
+		 * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+		 * jiffies have passed since we started streaming reset the
+		 * counters and keep track of the sequence offset.
+		 */
+		if (jiffies_since_start > JIFFIES_RESYNC) {
+			dev->jiffies_vid_out = cur_jiffies;
+			dev->out_seq_offset = buffers_since_start;
+			buffers_since_start = 0;
+		}
+		dev->out_seq_count = buffers_since_start + dev->out_seq_offset;
+		dev->vid_out_seq_count = dev->out_seq_count - dev->vid_out_seq_start;
+		dev->vbi_out_seq_count = dev->out_seq_count - dev->vbi_out_seq_start;
+
+		vivid_thread_vid_out_tick(dev);
+		mutex_unlock(&dev->mutex);
+
+		/*
+		 * Calculate the number of 'numerators' streamed since we started,
+		 * not including the current buffer.
+		 */
+		numerators_since_start = buffers_since_start * numerator;
+
+		/* And the number of jiffies since we started */
+		jiffies_since_start = jiffies - dev->jiffies_vid_out;
+
+		/* Increase by the 'numerator' of one buffer */
+		numerators_since_start += numerator;
+		/*
+		 * Calculate when that next buffer is supposed to start
+		 * in jiffies since we started streaming.
+		 */
+		next_jiffies_since_start = numerators_since_start * HZ +
+					   denominator / 2;
+		do_div(next_jiffies_since_start, denominator);
+		/* If it is in the past, then just schedule asap */
+		if (next_jiffies_since_start < jiffies_since_start)
+			next_jiffies_since_start = jiffies_since_start;
+
+		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+		schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+	}
+	dprintk(dev, 1, "Video Output Thread End\n");
+	return 0;
+}
+
+static void vivid_grab_controls(struct vivid_dev *dev, bool grab)
+{
+	v4l2_ctrl_grab(dev->ctrl_has_crop_out, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_compose_out, grab);
+	v4l2_ctrl_grab(dev->ctrl_has_scaler_out, grab);
+	v4l2_ctrl_grab(dev->ctrl_tx_mode, grab);
+	v4l2_ctrl_grab(dev->ctrl_tx_rgb_range, grab);
+}
+
+int vivid_start_generating_vid_out(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_out) {
+		u32 seq_count = dev->out_seq_count + dev->seq_wrap * 128;
+
+		if (pstreaming == &dev->vid_out_streaming)
+			dev->vid_out_seq_start = seq_count;
+		else
+			dev->vbi_out_seq_start = seq_count;
+		*pstreaming = true;
+		return 0;
+	}
+
+	/* Resets frame counters */
+	dev->jiffies_vid_out = jiffies;
+	dev->vid_out_seq_start = dev->seq_wrap * 128;
+	dev->vbi_out_seq_start = dev->seq_wrap * 128;
+
+	dev->kthread_vid_out = kthread_run(vivid_thread_vid_out, dev,
+			"%s-vid-out", dev->v4l2_dev.name);
+
+	if (IS_ERR(dev->kthread_vid_out)) {
+		v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+		return PTR_ERR(dev->kthread_vid_out);
+	}
+	*pstreaming = true;
+	vivid_grab_controls(dev, true);
+
+	dprintk(dev, 1, "returning from %s\n", __func__);
+	return 0;
+}
+
+void vivid_stop_generating_vid_out(struct vivid_dev *dev, bool *pstreaming)
+{
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->kthread_vid_out == NULL)
+		return;
+
+	*pstreaming = false;
+	if (pstreaming == &dev->vid_out_streaming) {
+		/* Release all active buffers */
+		while (!list_empty(&dev->vid_out_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vid_out_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vid_out buffer %d done\n",
+				buf->vb.v4l2_buf.index);
+		}
+	}
+
+	if (pstreaming == &dev->vbi_out_streaming) {
+		while (!list_empty(&dev->vbi_out_active)) {
+			struct vivid_buffer *buf;
+
+			buf = list_entry(dev->vbi_out_active.next,
+					 struct vivid_buffer, list);
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+			dprintk(dev, 2, "vbi_out buffer %d done\n",
+				buf->vb.v4l2_buf.index);
+		}
+	}
+
+	if (dev->vid_out_streaming || dev->vbi_out_streaming)
+		return;
+
+	/* shutdown control thread */
+	vivid_grab_controls(dev, false);
+	mutex_unlock(&dev->mutex);
+	kthread_stop(dev->kthread_vid_out);
+	dev->kthread_vid_out = NULL;
+	mutex_lock(&dev->mutex);
+}
diff --git a/drivers/media/platform/vivid/vivid-kthread-out.h b/drivers/media/platform/vivid/vivid-kthread-out.h
new file mode 100644
index 000000000000..2bf04a17b05d
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-kthread-out.h
@@ -0,0 +1,26 @@
+/*
+ * vivid-kthread-out.h - video/vbi output thread support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_KTHREAD_OUT_H_
+#define _VIVID_KTHREAD_OUT_H_
+
+int vivid_start_generating_vid_out(struct vivid_dev *dev, bool *pstreaming);
+void vivid_stop_generating_vid_out(struct vivid_dev *dev, bool *pstreaming);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-osd.c b/drivers/media/platform/vivid/vivid-osd.c
new file mode 100644
index 000000000000..084d346fb4c4
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-osd.c
@@ -0,0 +1,400 @@
+/*
+ * vivid-osd.c - osd support for testing overlays.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/font.h>
+#include <linux/mutex.h>
+#include <linux/videodev2.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/fb.h>
+#include <media/videobuf2-vmalloc.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-fh.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-osd.h"
+
+#define MAX_OSD_WIDTH  720
+#define MAX_OSD_HEIGHT 576
+
+/*
+ * Order: white, yellow, cyan, green, magenta, red, blue, black,
+ * and same again with the alpha bit set (if any)
+ */
+static const u16 rgb555[16] = {
+	0x7fff, 0x7fe0, 0x03ff, 0x03e0, 0x7c1f, 0x7c00, 0x001f, 0x0000,
+	0xffff, 0xffe0, 0x83ff, 0x83e0, 0xfc1f, 0xfc00, 0x801f, 0x8000
+};
+
+static const u16 rgb565[16] = {
+	0xffff, 0xffe0, 0x07ff, 0x07e0, 0xf81f, 0xf800, 0x001f, 0x0000,
+	0xffff, 0xffe0, 0x07ff, 0x07e0, 0xf81f, 0xf800, 0x001f, 0x0000
+};
+
+void vivid_clear_fb(struct vivid_dev *dev)
+{
+	void *p = dev->video_vbase;
+	const u16 *rgb = rgb555;
+	unsigned x, y;
+
+	if (dev->fb_defined.green.length == 6)
+		rgb = rgb565;
+
+	for (y = 0; y < dev->display_height; y++) {
+		u16 *d = p;
+
+		for (x = 0; x < dev->display_width; x++)
+			d[x] = rgb[(y / 16 + x / 16) % 16];
+		p += dev->display_byte_stride;
+	}
+}
+
+/* --------------------------------------------------------------------- */
+
+static int vivid_fb_ioctl(struct fb_info *info, unsigned cmd, unsigned long arg)
+{
+	struct vivid_dev *dev = (struct vivid_dev *)info->par;
+
+	switch (cmd) {
+	case FBIOGET_VBLANK: {
+		struct fb_vblank vblank;
+
+		vblank.flags = FB_VBLANK_HAVE_COUNT | FB_VBLANK_HAVE_VCOUNT |
+			FB_VBLANK_HAVE_VSYNC;
+		vblank.count = 0;
+		vblank.vcount = 0;
+		vblank.hcount = 0;
+		if (copy_to_user((void __user *)arg, &vblank, sizeof(vblank)))
+			return -EFAULT;
+		return 0;
+	}
+
+	default:
+		dprintk(dev, 1, "Unknown ioctl %08x\n", cmd);
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/* Framebuffer device handling */
+
+static int vivid_fb_set_var(struct vivid_dev *dev, struct fb_var_screeninfo *var)
+{
+	dprintk(dev, 1, "vivid_fb_set_var\n");
+
+	if (var->bits_per_pixel != 16) {
+		dprintk(dev, 1, "vivid_fb_set_var - Invalid bpp\n");
+		return -EINVAL;
+	}
+	dev->display_byte_stride = var->xres * dev->bytes_per_pixel;
+
+	return 0;
+}
+
+static int vivid_fb_get_fix(struct vivid_dev *dev, struct fb_fix_screeninfo *fix)
+{
+	dprintk(dev, 1, "vivid_fb_get_fix\n");
+	memset(fix, 0, sizeof(struct fb_fix_screeninfo));
+	strlcpy(fix->id, "vioverlay fb", sizeof(fix->id));
+	fix->smem_start = dev->video_pbase;
+	fix->smem_len = dev->video_buffer_size;
+	fix->type = FB_TYPE_PACKED_PIXELS;
+	fix->visual = FB_VISUAL_TRUECOLOR;
+	fix->xpanstep = 1;
+	fix->ypanstep = 1;
+	fix->ywrapstep = 0;
+	fix->line_length = dev->display_byte_stride;
+	fix->accel = FB_ACCEL_NONE;
+	return 0;
+}
+
+/* Check the requested display mode, returning -EINVAL if we can't
+   handle it. */
+
+static int _vivid_fb_check_var(struct fb_var_screeninfo *var, struct vivid_dev *dev)
+{
+	dprintk(dev, 1, "vivid_fb_check_var\n");
+
+	var->bits_per_pixel = 16;
+	if (var->green.length == 5) {
+		var->red.offset = 10;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 5;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		var->transp.offset = 15;
+		var->transp.length = 1;
+	} else {
+		var->red.offset = 11;
+		var->red.length = 5;
+		var->green.offset = 5;
+		var->green.length = 6;
+		var->blue.offset = 0;
+		var->blue.length = 5;
+		var->transp.offset = 0;
+		var->transp.length = 0;
+	}
+	var->xoffset = var->yoffset = 0;
+	var->left_margin = var->upper_margin = 0;
+	var->nonstd = 0;
+
+	var->vmode &= ~FB_VMODE_MASK;
+	var->vmode = FB_VMODE_NONINTERLACED;
+
+	/* Dummy values */
+	var->hsync_len = 24;
+	var->vsync_len = 2;
+	var->pixclock = 84316;
+	var->right_margin = 776;
+	var->lower_margin = 591;
+	return 0;
+}
+
+static int vivid_fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	struct vivid_dev *dev = (struct vivid_dev *) info->par;
+
+	dprintk(dev, 1, "vivid_fb_check_var\n");
+	return _vivid_fb_check_var(var, dev);
+}
+
+static int vivid_fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	return 0;
+}
+
+static int vivid_fb_set_par(struct fb_info *info)
+{
+	int rc = 0;
+	struct vivid_dev *dev = (struct vivid_dev *) info->par;
+
+	dprintk(dev, 1, "vivid_fb_set_par\n");
+
+	rc = vivid_fb_set_var(dev, &info->var);
+	vivid_fb_get_fix(dev, &info->fix);
+	return rc;
+}
+
+static int vivid_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
+				unsigned blue, unsigned transp,
+				struct fb_info *info)
+{
+	u32 color, *palette;
+
+	if (regno >= info->cmap.len)
+		return -EINVAL;
+
+	color = ((transp & 0xFF00) << 16) | ((red & 0xFF00) << 8) |
+		 (green & 0xFF00) | ((blue & 0xFF00) >> 8);
+	if (regno >= 16)
+		return -EINVAL;
+
+	palette = info->pseudo_palette;
+	if (info->var.bits_per_pixel == 16) {
+		switch (info->var.green.length) {
+		case 6:
+			color = (red & 0xf800) |
+				((green & 0xfc00) >> 5) |
+				((blue & 0xf800) >> 11);
+			break;
+		case 5:
+			color = ((red & 0xf800) >> 1) |
+				((green & 0xf800) >> 6) |
+				((blue & 0xf800) >> 11) |
+				(transp ? 0x8000 : 0);
+			break;
+		}
+	}
+	palette[regno] = color;
+	return 0;
+}
+
+/* We don't really support blanking. All this does is enable or
+   disable the OSD. */
+static int vivid_fb_blank(int blank_mode, struct fb_info *info)
+{
+	struct vivid_dev *dev = (struct vivid_dev *)info->par;
+
+	dprintk(dev, 1, "Set blanking mode : %d\n", blank_mode);
+	switch (blank_mode) {
+	case FB_BLANK_UNBLANK:
+		break;
+	case FB_BLANK_NORMAL:
+	case FB_BLANK_HSYNC_SUSPEND:
+	case FB_BLANK_VSYNC_SUSPEND:
+	case FB_BLANK_POWERDOWN:
+		break;
+	}
+	return 0;
+}
+
+static struct fb_ops vivid_fb_ops = {
+	.owner = THIS_MODULE,
+	.fb_check_var   = vivid_fb_check_var,
+	.fb_set_par     = vivid_fb_set_par,
+	.fb_setcolreg   = vivid_fb_setcolreg,
+	.fb_fillrect    = cfb_fillrect,
+	.fb_copyarea    = cfb_copyarea,
+	.fb_imageblit   = cfb_imageblit,
+	.fb_cursor      = NULL,
+	.fb_ioctl       = vivid_fb_ioctl,
+	.fb_pan_display = vivid_fb_pan_display,
+	.fb_blank       = vivid_fb_blank,
+};
+
+/* Initialization */
+
+
+/* Setup our initial video mode */
+static int vivid_fb_init_vidmode(struct vivid_dev *dev)
+{
+	struct v4l2_rect start_window;
+
+	/* Color mode */
+
+	dev->bits_per_pixel = 16;
+	dev->bytes_per_pixel = dev->bits_per_pixel / 8;
+
+	start_window.width = MAX_OSD_WIDTH;
+	start_window.left = 0;
+
+	dev->display_byte_stride = start_window.width * dev->bytes_per_pixel;
+
+	/* Vertical size & position */
+
+	start_window.height = MAX_OSD_HEIGHT;
+	start_window.top = 0;
+
+	dev->display_width = start_window.width;
+	dev->display_height = start_window.height;
+
+	/* Generate a valid fb_var_screeninfo */
+
+	dev->fb_defined.xres = dev->display_width;
+	dev->fb_defined.yres = dev->display_height;
+	dev->fb_defined.xres_virtual = dev->display_width;
+	dev->fb_defined.yres_virtual = dev->display_height;
+	dev->fb_defined.bits_per_pixel = dev->bits_per_pixel;
+	dev->fb_defined.vmode = FB_VMODE_NONINTERLACED;
+	dev->fb_defined.left_margin = start_window.left + 1;
+	dev->fb_defined.upper_margin = start_window.top + 1;
+	dev->fb_defined.accel_flags = FB_ACCEL_NONE;
+	dev->fb_defined.nonstd = 0;
+	/* set default to 1:5:5:5 */
+	dev->fb_defined.green.length = 5;
+
+	/* We've filled in the most data, let the usual mode check
+	   routine fill in the rest. */
+	_vivid_fb_check_var(&dev->fb_defined, dev);
+
+	/* Generate valid fb_fix_screeninfo */
+
+	vivid_fb_get_fix(dev, &dev->fb_fix);
+
+	/* Generate valid fb_info */
+
+	dev->fb_info.node = -1;
+	dev->fb_info.flags = FBINFO_FLAG_DEFAULT;
+	dev->fb_info.fbops = &vivid_fb_ops;
+	dev->fb_info.par = dev;
+	dev->fb_info.var = dev->fb_defined;
+	dev->fb_info.fix = dev->fb_fix;
+	dev->fb_info.screen_base = (u8 __iomem *)dev->video_vbase;
+	dev->fb_info.fbops = &vivid_fb_ops;
+
+	/* Supply some monitor specs. Bogus values will do for now */
+	dev->fb_info.monspecs.hfmin = 8000;
+	dev->fb_info.monspecs.hfmax = 70000;
+	dev->fb_info.monspecs.vfmin = 10;
+	dev->fb_info.monspecs.vfmax = 100;
+
+	/* Allocate color map */
+	if (fb_alloc_cmap(&dev->fb_info.cmap, 256, 1)) {
+		pr_err("abort, unable to alloc cmap\n");
+		return -ENOMEM;
+	}
+
+	/* Allocate the pseudo palette */
+	dev->fb_info.pseudo_palette = kmalloc_array(16, sizeof(u32), GFP_KERNEL);
+
+	return dev->fb_info.pseudo_palette ? 0 : -ENOMEM;
+}
+
+/* Release any memory we've grabbed */
+void vivid_fb_release_buffers(struct vivid_dev *dev)
+{
+	if (dev->video_vbase == NULL)
+		return;
+
+	/* Release cmap */
+	if (dev->fb_info.cmap.len)
+		fb_dealloc_cmap(&dev->fb_info.cmap);
+
+	/* Release pseudo palette */
+	kfree(dev->fb_info.pseudo_palette);
+	kfree((void *)dev->video_vbase);
+}
+
+/* Initialize the specified card */
+
+int vivid_fb_init(struct vivid_dev *dev)
+{
+	int ret;
+
+	dev->video_buffer_size = MAX_OSD_HEIGHT * MAX_OSD_WIDTH * 2;
+	dev->video_vbase = kzalloc(dev->video_buffer_size, GFP_KERNEL | GFP_DMA32);
+	if (dev->video_vbase == NULL)
+		return -ENOMEM;
+	dev->video_pbase = virt_to_phys(dev->video_vbase);
+
+	pr_info("Framebuffer at 0x%lx, mapped to 0x%p, size %dk\n",
+			dev->video_pbase, dev->video_vbase,
+			dev->video_buffer_size / 1024);
+
+	/* Set the startup video mode information */
+	ret = vivid_fb_init_vidmode(dev);
+	if (ret) {
+		vivid_fb_release_buffers(dev);
+		return ret;
+	}
+
+	vivid_clear_fb(dev);
+
+	/* Register the framebuffer */
+	if (register_framebuffer(&dev->fb_info) < 0) {
+		vivid_fb_release_buffers(dev);
+		return -EINVAL;
+	}
+
+	/* Set the card to the requested mode */
+	vivid_fb_set_par(&dev->fb_info);
+	return 0;
+
+}
diff --git a/drivers/media/platform/vivid/vivid-osd.h b/drivers/media/platform/vivid/vivid-osd.h
new file mode 100644
index 000000000000..57c9daa5940a
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-osd.h
@@ -0,0 +1,27 @@
+/*
+ * vivid-osd.h - output overlay support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_OSD_H_
+#define _VIVID_OSD_H_
+
+int vivid_fb_init(struct vivid_dev *dev);
+void vivid_fb_release_buffers(struct vivid_dev *dev);
+void vivid_clear_fb(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-radio-common.c b/drivers/media/platform/vivid/vivid-radio-common.c
new file mode 100644
index 000000000000..78c1e920670a
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-common.c
@@ -0,0 +1,189 @@
+/*
+ * vivid-radio-common.c - common radio rx/tx support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-rds-gen.h"
+
+/*
+ * These functions are shared between the vivid receiver and transmitter
+ * since both use the same frequency bands.
+ */
+
+const struct v4l2_frequency_band vivid_radio_bands[TOT_BANDS] = {
+	/* Band FM */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			      V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = FM_FREQ_RANGE_LOW,
+		.rangehigh  = FM_FREQ_RANGE_HIGH,
+		.modulation = V4L2_BAND_MODULATION_FM,
+	},
+	/* Band AM */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 1,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = AM_FREQ_RANGE_LOW,
+		.rangehigh  = AM_FREQ_RANGE_HIGH,
+		.modulation = V4L2_BAND_MODULATION_AM,
+	},
+	/* Band SW */
+	{
+		.type = V4L2_TUNER_RADIO,
+		.index = 2,
+		.capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = SW_FREQ_RANGE_LOW,
+		.rangehigh  = SW_FREQ_RANGE_HIGH,
+		.modulation = V4L2_BAND_MODULATION_AM,
+	},
+};
+
+/*
+ * Initialize the RDS generator. If we can loop, then the RDS generator
+ * is set up with the values from the RDS TX controls, otherwise it
+ * will fill in standard values using one of two alternates.
+ */
+void vivid_radio_rds_init(struct vivid_dev *dev)
+{
+	struct vivid_rds_gen *rds = &dev->rds_gen;
+	bool alt = dev->radio_rx_rds_use_alternates;
+
+	/* Do nothing, blocks will be filled by the transmitter */
+	if (dev->radio_rds_loop && !dev->radio_tx_rds_controls)
+		return;
+
+	if (dev->radio_rds_loop) {
+		v4l2_ctrl_lock(dev->radio_tx_rds_pi);
+		rds->picode = dev->radio_tx_rds_pi->cur.val;
+		rds->pty = dev->radio_tx_rds_pty->cur.val;
+		rds->mono_stereo = dev->radio_tx_rds_mono_stereo->cur.val;
+		rds->art_head = dev->radio_tx_rds_art_head->cur.val;
+		rds->compressed = dev->radio_tx_rds_compressed->cur.val;
+		rds->dyn_pty = dev->radio_tx_rds_dyn_pty->cur.val;
+		rds->ta = dev->radio_tx_rds_ta->cur.val;
+		rds->tp = dev->radio_tx_rds_tp->cur.val;
+		rds->ms = dev->radio_tx_rds_ms->cur.val;
+		strlcpy(rds->psname,
+			dev->radio_tx_rds_psname->p_cur.p_char,
+			sizeof(rds->psname));
+		strlcpy(rds->radiotext,
+			dev->radio_tx_rds_radiotext->p_cur.p_char + alt * 64,
+			sizeof(rds->radiotext));
+		v4l2_ctrl_unlock(dev->radio_tx_rds_pi);
+	} else {
+		vivid_rds_gen_fill(rds, dev->radio_rx_freq, alt);
+	}
+	if (dev->radio_rx_rds_controls) {
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_pty, rds->pty);
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ta, rds->ta);
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_tp, rds->tp);
+		v4l2_ctrl_s_ctrl(dev->radio_rx_rds_ms, rds->ms);
+		v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_psname, rds->psname);
+		v4l2_ctrl_s_ctrl_string(dev->radio_rx_rds_radiotext, rds->radiotext);
+		if (!dev->radio_rds_loop)
+			dev->radio_rx_rds_use_alternates = !dev->radio_rx_rds_use_alternates;
+	}
+	vivid_rds_generate(rds);
+}
+
+/*
+ * Calculate the emulated signal quality taking into account the frequency
+ * the transmitter is using.
+ */
+static void vivid_radio_calc_sig_qual(struct vivid_dev *dev)
+{
+	int mod = 16000;
+	int delta = 800;
+	int sig_qual, sig_qual_tx = mod;
+
+	/*
+	 * For SW and FM there is a channel every 1000 kHz, for AM there is one
+	 * every 100 kHz.
+	 */
+	if (dev->radio_rx_freq <= AM_FREQ_RANGE_HIGH) {
+		mod /= 10;
+		delta /= 10;
+	}
+	sig_qual = (dev->radio_rx_freq + delta) % mod - delta;
+	if (dev->has_radio_tx)
+		sig_qual_tx = dev->radio_rx_freq - dev->radio_tx_freq;
+	if (abs(sig_qual_tx) <= abs(sig_qual)) {
+		sig_qual = sig_qual_tx;
+		/*
+		 * Zero the internal rds buffer if we are going to loop
+		 * rds blocks.
+		 */
+		if (!dev->radio_rds_loop && !dev->radio_tx_rds_controls)
+			memset(dev->rds_gen.data, 0,
+			       sizeof(dev->rds_gen.data));
+		dev->radio_rds_loop = dev->radio_rx_freq >= FM_FREQ_RANGE_LOW;
+	} else {
+		dev->radio_rds_loop = false;
+	}
+	if (dev->radio_rx_freq <= AM_FREQ_RANGE_HIGH)
+		sig_qual *= 10;
+	dev->radio_rx_sig_qual = sig_qual;
+}
+
+int vivid_radio_g_frequency(struct file *file, const unsigned *pfreq, struct v4l2_frequency *vf)
+{
+	if (vf->tuner != 0)
+		return -EINVAL;
+	vf->frequency = *pfreq;
+	return 0;
+}
+
+int vivid_radio_s_frequency(struct file *file, unsigned *pfreq, const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned freq;
+	unsigned band;
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+
+	if (vf->frequency >= (FM_FREQ_RANGE_LOW + SW_FREQ_RANGE_HIGH) / 2)
+		band = BAND_FM;
+	else if (vf->frequency <= (AM_FREQ_RANGE_HIGH + SW_FREQ_RANGE_LOW) / 2)
+		band = BAND_AM;
+	else
+		band = BAND_SW;
+
+	freq = clamp_t(u32, vf->frequency, vivid_radio_bands[band].rangelow,
+					   vivid_radio_bands[band].rangehigh);
+	*pfreq = freq;
+
+	/*
+	 * For both receiver and transmitter recalculate the signal quality
+	 * (since that depends on both frequencies) and re-init the rds
+	 * generator.
+	 */
+	vivid_radio_calc_sig_qual(dev);
+	vivid_radio_rds_init(dev);
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-radio-common.h b/drivers/media/platform/vivid/vivid-radio-common.h
new file mode 100644
index 000000000000..92fe589141b7
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-common.h
@@ -0,0 +1,40 @@
+/*
+ * vivid-radio-common.h - common radio rx/tx support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RADIO_COMMON_H_
+#define _VIVID_RADIO_COMMON_H_
+
+/* The supported radio frequency ranges in kHz */
+#define FM_FREQ_RANGE_LOW       (64000U * 16U)
+#define FM_FREQ_RANGE_HIGH      (108000U * 16U)
+#define AM_FREQ_RANGE_LOW       (520U * 16U)
+#define AM_FREQ_RANGE_HIGH      (1710U * 16U)
+#define SW_FREQ_RANGE_LOW       (2300U * 16U)
+#define SW_FREQ_RANGE_HIGH      (26100U * 16U)
+
+enum { BAND_FM, BAND_AM, BAND_SW, TOT_BANDS };
+
+extern const struct v4l2_frequency_band vivid_radio_bands[TOT_BANDS];
+
+int vivid_radio_g_frequency(struct file *file, const unsigned *freq, struct v4l2_frequency *vf);
+int vivid_radio_s_frequency(struct file *file, unsigned *freq, const struct v4l2_frequency *vf);
+
+void vivid_radio_rds_init(struct vivid_dev *dev);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-radio-rx.c b/drivers/media/platform/vivid/vivid-radio-rx.c
new file mode 100644
index 000000000000..c7651a506668
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-rx.c
@@ -0,0 +1,287 @@
+/*
+ * vivid-radio-rx.c - radio receiver support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-rds-gen.h"
+#include "vivid-radio-rx.h"
+
+ssize_t vivid_radio_rx_read(struct file *file, char __user *buf,
+			 size_t size, loff_t *offset)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct timespec ts;
+	struct v4l2_rds_data *data = dev->rds_gen.data;
+	bool use_alternates;
+	unsigned blk;
+	int perc;
+	int i;
+
+	if (dev->radio_rx_rds_controls)
+		return -EINVAL;
+	if (size < sizeof(*data))
+		return 0;
+	size = sizeof(*data) * (size / sizeof(*data));
+
+	if (mutex_lock_interruptible(&dev->mutex))
+		return -ERESTARTSYS;
+	if (dev->radio_rx_rds_owner &&
+	    file->private_data != dev->radio_rx_rds_owner) {
+		mutex_unlock(&dev->mutex);
+		return -EBUSY;
+	}
+	if (dev->radio_rx_rds_owner == NULL) {
+		vivid_radio_rds_init(dev);
+		dev->radio_rx_rds_owner = file->private_data;
+	}
+
+retry:
+	ktime_get_ts(&ts);
+	use_alternates = ts.tv_sec % 10 >= 5;
+	if (dev->radio_rx_rds_last_block == 0 ||
+	    dev->radio_rx_rds_use_alternates != use_alternates) {
+		dev->radio_rx_rds_use_alternates = use_alternates;
+		/* Re-init the RDS generator */
+		vivid_radio_rds_init(dev);
+	}
+	ts = timespec_sub(ts, dev->radio_rds_init_ts);
+	blk = ts.tv_sec * 100 + ts.tv_nsec / 10000000;
+	blk = (blk * VIVID_RDS_GEN_BLOCKS) / 500;
+	if (blk >= dev->radio_rx_rds_last_block + VIVID_RDS_GEN_BLOCKS)
+		dev->radio_rx_rds_last_block = blk - VIVID_RDS_GEN_BLOCKS + 1;
+
+	/*
+	 * No data is available if there hasn't been time to get new data,
+	 * or if the RDS receiver has been disabled, or if we use the data
+	 * from the RDS transmitter and that RDS transmitter has been disabled,
+	 * or if the signal quality is too weak.
+	 */
+	if (blk == dev->radio_rx_rds_last_block || !dev->radio_rx_rds_enabled ||
+	    (dev->radio_rds_loop && !(dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) ||
+	    abs(dev->radio_rx_sig_qual) > 200) {
+		mutex_unlock(&dev->mutex);
+		if (file->f_flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+		if (msleep_interruptible(20) && signal_pending(current))
+			return -EINTR;
+		if (mutex_lock_interruptible(&dev->mutex))
+			return -ERESTARTSYS;
+		goto retry;
+	}
+
+	/* abs(dev->radio_rx_sig_qual) <= 200, map that to a 0-50% range */
+	perc = abs(dev->radio_rx_sig_qual) / 4;
+
+	for (i = 0; i < size && blk > dev->radio_rx_rds_last_block;
+			dev->radio_rx_rds_last_block++) {
+		unsigned data_blk = dev->radio_rx_rds_last_block % VIVID_RDS_GEN_BLOCKS;
+		struct v4l2_rds_data rds = data[data_blk];
+
+		if (data_blk == 0 && dev->radio_rds_loop)
+			vivid_radio_rds_init(dev);
+		if (perc && prandom_u32_max(100) < perc) {
+			switch (prandom_u32_max(4)) {
+			case 0:
+				rds.block |= V4L2_RDS_BLOCK_CORRECTED;
+				break;
+			case 1:
+				rds.block |= V4L2_RDS_BLOCK_INVALID;
+				break;
+			case 2:
+				rds.block |= V4L2_RDS_BLOCK_ERROR;
+				rds.lsb = prandom_u32_max(256);
+				rds.msb = prandom_u32_max(256);
+				break;
+			case 3: /* Skip block altogether */
+				if (i)
+					continue;
+				/*
+				 * Must make sure at least one block is
+				 * returned, otherwise the application
+				 * might think that end-of-file occurred.
+				 */
+				break;
+			}
+		}
+		if (copy_to_user(buf + i, &rds, sizeof(rds))) {
+			i = -EFAULT;
+			break;
+		}
+		i += sizeof(rds);
+	}
+	mutex_unlock(&dev->mutex);
+	return i;
+}
+
+unsigned int vivid_radio_rx_poll(struct file *file, struct poll_table_struct *wait)
+{
+	return POLLIN | POLLRDNORM | v4l2_ctrl_poll(file, wait);
+}
+
+int vivid_radio_rx_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
+{
+	if (band->tuner != 0)
+		return -EINVAL;
+
+	if (band->index >= TOT_BANDS)
+		return -EINVAL;
+
+	*band = vivid_radio_bands[band->index];
+	return 0;
+}
+
+int vivid_radio_rx_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned low, high;
+	unsigned freq;
+	unsigned spacing;
+	unsigned band;
+
+	if (a->tuner)
+		return -EINVAL;
+	if (a->wrap_around && dev->radio_rx_hw_seek_mode == VIVID_HW_SEEK_BOUNDED)
+		return -EINVAL;
+
+	if (!a->wrap_around && dev->radio_rx_hw_seek_mode == VIVID_HW_SEEK_WRAP)
+		return -EINVAL;
+	if (!a->rangelow ^ !a->rangehigh)
+		return -EINVAL;
+
+	if (file->f_flags & O_NONBLOCK)
+		return -EWOULDBLOCK;
+
+	if (a->rangelow) {
+		for (band = 0; band < TOT_BANDS; band++)
+			if (a->rangelow >= vivid_radio_bands[band].rangelow &&
+			    a->rangehigh <= vivid_radio_bands[band].rangehigh)
+				break;
+		if (band == TOT_BANDS)
+			return -EINVAL;
+		if (!dev->radio_rx_hw_seek_prog_lim &&
+		    (a->rangelow != vivid_radio_bands[band].rangelow ||
+		     a->rangehigh != vivid_radio_bands[band].rangehigh))
+			return -EINVAL;
+		low = a->rangelow;
+		high = a->rangehigh;
+	} else {
+		for (band = 0; band < TOT_BANDS; band++)
+			if (dev->radio_rx_freq >= vivid_radio_bands[band].rangelow &&
+			    dev->radio_rx_freq <= vivid_radio_bands[band].rangehigh)
+				break;
+		low = vivid_radio_bands[band].rangelow;
+		high = vivid_radio_bands[band].rangehigh;
+	}
+	spacing = band == BAND_AM ? 1600 : 16000;
+	freq = clamp(dev->radio_rx_freq, low, high);
+
+	if (a->seek_upward) {
+		freq = spacing * (freq / spacing) + spacing;
+		if (freq > high) {
+			if (!a->wrap_around)
+				return -ENODATA;
+			freq = spacing * (low / spacing) + spacing;
+			if (freq >= dev->radio_rx_freq)
+				return -ENODATA;
+		}
+	} else {
+		freq = spacing * ((freq + spacing - 1) / spacing) - spacing;
+		if (freq < low) {
+			if (!a->wrap_around)
+				return -ENODATA;
+			freq = spacing * ((high + spacing - 1) / spacing) - spacing;
+			if (freq <= dev->radio_rx_freq)
+				return -ENODATA;
+		}
+	}
+	return 0;
+}
+
+int vivid_radio_rx_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	int delta = 800;
+	int sig_qual;
+
+	if (vt->index > 0)
+		return -EINVAL;
+
+	strlcpy(vt->name, "AM/FM/SW Receiver", sizeof(vt->name));
+	vt->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			 V4L2_TUNER_CAP_FREQ_BANDS | V4L2_TUNER_CAP_RDS |
+			 (dev->radio_rx_rds_controls ?
+				V4L2_TUNER_CAP_RDS_CONTROLS :
+				V4L2_TUNER_CAP_RDS_BLOCK_IO) |
+			 (dev->radio_rx_hw_seek_prog_lim ?
+				V4L2_TUNER_CAP_HWSEEK_PROG_LIM : 0);
+	switch (dev->radio_rx_hw_seek_mode) {
+	case VIVID_HW_SEEK_BOUNDED:
+		vt->capability |= V4L2_TUNER_CAP_HWSEEK_BOUNDED;
+		break;
+	case VIVID_HW_SEEK_WRAP:
+		vt->capability |= V4L2_TUNER_CAP_HWSEEK_WRAP;
+		break;
+	case VIVID_HW_SEEK_BOTH:
+		vt->capability |= V4L2_TUNER_CAP_HWSEEK_WRAP |
+				  V4L2_TUNER_CAP_HWSEEK_BOUNDED;
+		break;
+	}
+	vt->rangelow = AM_FREQ_RANGE_LOW;
+	vt->rangehigh = FM_FREQ_RANGE_HIGH;
+	sig_qual = dev->radio_rx_sig_qual;
+	vt->signal = abs(sig_qual) > delta ? 0 :
+		     0xffff - (abs(sig_qual) * 0xffff) / delta;
+	vt->afc = sig_qual > delta ? 0 : sig_qual;
+	if (abs(sig_qual) > delta)
+		vt->rxsubchans = 0;
+	else if (dev->radio_rx_freq < FM_FREQ_RANGE_LOW || vt->signal < 0x8000)
+		vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+	else if (dev->radio_rds_loop && !(dev->radio_tx_subchans & V4L2_TUNER_SUB_STEREO))
+		vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+	else
+		vt->rxsubchans = V4L2_TUNER_SUB_STEREO;
+	if (dev->radio_rx_rds_enabled &&
+	    (!dev->radio_rds_loop || (dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) &&
+	    dev->radio_rx_freq >= FM_FREQ_RANGE_LOW && vt->signal >= 0xc000)
+		vt->rxsubchans |= V4L2_TUNER_SUB_RDS;
+	if (dev->radio_rx_rds_controls)
+		vivid_radio_rds_init(dev);
+	vt->audmode = dev->radio_rx_audmode;
+	return 0;
+}
+
+int vivid_radio_rx_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vt->index)
+		return -EINVAL;
+	dev->radio_rx_audmode = vt->audmode >= V4L2_TUNER_MODE_STEREO;
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-radio-rx.h b/drivers/media/platform/vivid/vivid-radio-rx.h
new file mode 100644
index 000000000000..1077d8f061eb
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-rx.h
@@ -0,0 +1,31 @@
+/*
+ * vivid-radio-rx.h - radio receiver support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RADIO_RX_H_
+#define _VIVID_RADIO_RX_H_
+
+ssize_t vivid_radio_rx_read(struct file *, char __user *, size_t, loff_t *);
+unsigned int vivid_radio_rx_poll(struct file *file, struct poll_table_struct *wait);
+
+int vivid_radio_rx_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band);
+int vivid_radio_rx_s_hw_freq_seek(struct file *file, void *fh, const struct v4l2_hw_freq_seek *a);
+int vivid_radio_rx_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vivid_radio_rx_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-radio-tx.c b/drivers/media/platform/vivid/vivid-radio-tx.c
new file mode 100644
index 000000000000..8c59d4f53200
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-tx.c
@@ -0,0 +1,141 @@
+/*
+ * vivid-radio-tx.c - radio transmitter support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-radio-common.h"
+#include "vivid-radio-tx.h"
+
+ssize_t vivid_radio_tx_write(struct file *file, const char __user *buf,
+			  size_t size, loff_t *offset)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rds_data *data = dev->rds_gen.data;
+	struct timespec ts;
+	unsigned blk;
+	int i;
+
+	if (dev->radio_tx_rds_controls)
+		return -EINVAL;
+
+	if (size < sizeof(*data))
+		return -EINVAL;
+	size = sizeof(*data) * (size / sizeof(*data));
+
+	if (mutex_lock_interruptible(&dev->mutex))
+		return -ERESTARTSYS;
+	if (dev->radio_tx_rds_owner &&
+	    file->private_data != dev->radio_tx_rds_owner) {
+		mutex_unlock(&dev->mutex);
+		return -EBUSY;
+	}
+	dev->radio_tx_rds_owner = file->private_data;
+
+retry:
+	ktime_get_ts(&ts);
+	ts = timespec_sub(ts, dev->radio_rds_init_ts);
+	blk = ts.tv_sec * 100 + ts.tv_nsec / 10000000;
+	blk = (blk * VIVID_RDS_GEN_BLOCKS) / 500;
+	if (blk - VIVID_RDS_GEN_BLOCKS >= dev->radio_tx_rds_last_block)
+		dev->radio_tx_rds_last_block = blk - VIVID_RDS_GEN_BLOCKS + 1;
+
+	/*
+	 * No data is available if there hasn't been time to get new data,
+	 * or if the RDS receiver has been disabled, or if we use the data
+	 * from the RDS transmitter and that RDS transmitter has been disabled,
+	 * or if the signal quality is too weak.
+	 */
+	if (blk == dev->radio_tx_rds_last_block ||
+	    !(dev->radio_tx_subchans & V4L2_TUNER_SUB_RDS)) {
+		mutex_unlock(&dev->mutex);
+		if (file->f_flags & O_NONBLOCK)
+			return -EWOULDBLOCK;
+		if (msleep_interruptible(20) && signal_pending(current))
+			return -EINTR;
+		if (mutex_lock_interruptible(&dev->mutex))
+			return -ERESTARTSYS;
+		goto retry;
+	}
+
+	for (i = 0; i < size && blk > dev->radio_tx_rds_last_block;
+			dev->radio_tx_rds_last_block++) {
+		unsigned data_blk = dev->radio_tx_rds_last_block % VIVID_RDS_GEN_BLOCKS;
+		struct v4l2_rds_data rds;
+
+		if (copy_from_user(&rds, buf + i, sizeof(rds))) {
+			i = -EFAULT;
+			break;
+		}
+		i += sizeof(rds);
+		if (!dev->radio_rds_loop)
+			continue;
+		if ((rds.block & V4L2_RDS_BLOCK_MSK) == V4L2_RDS_BLOCK_INVALID ||
+		    (rds.block & V4L2_RDS_BLOCK_ERROR))
+			continue;
+		rds.block &= V4L2_RDS_BLOCK_MSK;
+		data[data_blk] = rds;
+	}
+	mutex_unlock(&dev->mutex);
+	return i;
+}
+
+unsigned int vivid_radio_tx_poll(struct file *file, struct poll_table_struct *wait)
+{
+	return POLLOUT | POLLWRNORM | v4l2_ctrl_poll(file, wait);
+}
+
+int vidioc_g_modulator(struct file *file, void *fh, struct v4l2_modulator *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (a->index > 0)
+		return -EINVAL;
+
+	strlcpy(a->name, "AM/FM/SW Transmitter", sizeof(a->name));
+	a->capability = V4L2_TUNER_CAP_LOW | V4L2_TUNER_CAP_STEREO |
+			V4L2_TUNER_CAP_FREQ_BANDS | V4L2_TUNER_CAP_RDS |
+			(dev->radio_tx_rds_controls ?
+				V4L2_TUNER_CAP_RDS_CONTROLS :
+				V4L2_TUNER_CAP_RDS_BLOCK_IO);
+	a->rangelow = AM_FREQ_RANGE_LOW;
+	a->rangehigh = FM_FREQ_RANGE_HIGH;
+	a->txsubchans = dev->radio_tx_subchans;
+	return 0;
+}
+
+int vidioc_s_modulator(struct file *file, void *fh, const struct v4l2_modulator *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (a->index)
+		return -EINVAL;
+	if (a->txsubchans & ~0x13)
+		return -EINVAL;
+	dev->radio_tx_subchans = a->txsubchans;
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-radio-tx.h b/drivers/media/platform/vivid/vivid-radio-tx.h
new file mode 100644
index 000000000000..7f8ff7547119
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-radio-tx.h
@@ -0,0 +1,29 @@
+/*
+ * vivid-radio-tx.h - radio transmitter support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RADIO_TX_H_
+#define _VIVID_RADIO_TX_H_
+
+ssize_t vivid_radio_tx_write(struct file *, const char __user *, size_t, loff_t *);
+unsigned int vivid_radio_tx_poll(struct file *file, struct poll_table_struct *wait);
+
+int vidioc_g_modulator(struct file *file, void *fh, struct v4l2_modulator *a);
+int vidioc_s_modulator(struct file *file, void *fh, const struct v4l2_modulator *a);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-rds-gen.c b/drivers/media/platform/vivid/vivid-rds-gen.c
new file mode 100644
index 000000000000..c382343fdb66
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-rds-gen.c
@@ -0,0 +1,166 @@
+/*
+ * vivid-rds-gen.c - rds (radio data system) generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+
+#include "vivid-rds-gen.h"
+
+static u8 vivid_get_di(const struct vivid_rds_gen *rds, unsigned grp)
+{
+	switch (grp) {
+	case 0:
+		return (rds->dyn_pty << 2) | (grp & 3);
+	case 1:
+		return (rds->compressed << 2) | (grp & 3);
+	case 2:
+		return (rds->art_head << 2) | (grp & 3);
+	case 3:
+		return (rds->mono_stereo << 2) | (grp & 3);
+	}
+	return 0;
+}
+
+/*
+ * This RDS generator creates 57 RDS groups (one group == four RDS blocks).
+ * Groups 0-3, 22-25 and 44-47 (spaced 22 groups apart) are filled with a
+ * standard 0B group containing the PI code and PS name.
+ *
+ * Groups 4-19 and 26-41 use group 2A for the radio text.
+ *
+ * Group 56 contains the time (group 4A).
+ *
+ * All remaining groups use a filler group 15B block that just repeats
+ * the PI and PTY codes.
+ */
+void vivid_rds_generate(struct vivid_rds_gen *rds)
+{
+	struct v4l2_rds_data *data = rds->data;
+	unsigned grp;
+	struct tm tm;
+	unsigned date;
+	unsigned time;
+	int l;
+
+	for (grp = 0; grp < VIVID_RDS_GEN_GROUPS; grp++, data += VIVID_RDS_GEN_BLKS_PER_GRP) {
+		data[0].lsb = rds->picode & 0xff;
+		data[0].msb = rds->picode >> 8;
+		data[0].block = V4L2_RDS_BLOCK_A | (V4L2_RDS_BLOCK_A << 3);
+		data[1].lsb = rds->pty << 5;
+		data[1].msb = (rds->pty >> 3) | (rds->tp << 2);
+		data[1].block = V4L2_RDS_BLOCK_B | (V4L2_RDS_BLOCK_B << 3);
+		data[3].block = V4L2_RDS_BLOCK_D | (V4L2_RDS_BLOCK_D << 3);
+
+		switch (grp) {
+		case 0 ... 3:
+		case 22 ... 25:
+		case 44 ... 47: /* Group 0B */
+			data[1].lsb |= (rds->ta << 4) | (rds->ms << 3);
+			data[1].lsb |= vivid_get_di(rds, grp % 22);
+			data[1].msb |= 1 << 3;
+			data[2].lsb = rds->picode & 0xff;
+			data[2].msb = rds->picode >> 8;
+			data[2].block = V4L2_RDS_BLOCK_C_ALT | (V4L2_RDS_BLOCK_C_ALT << 3);
+			data[3].lsb = rds->psname[2 * (grp % 22) + 1];
+			data[3].msb = rds->psname[2 * (grp % 22)];
+			break;
+		case 4 ... 19:
+		case 26 ... 41: /* Group 2A */
+			data[1].lsb |= (grp - 4) % 22;
+			data[1].msb |= 4 << 3;
+			data[2].msb = rds->radiotext[4 * ((grp - 4) % 22)];
+			data[2].lsb = rds->radiotext[4 * ((grp - 4) % 22) + 1];
+			data[2].block = V4L2_RDS_BLOCK_C | (V4L2_RDS_BLOCK_C << 3);
+			data[3].msb = rds->radiotext[4 * ((grp - 4) % 22) + 2];
+			data[3].lsb = rds->radiotext[4 * ((grp - 4) % 22) + 3];
+			break;
+		case 56:
+			/*
+			 * Group 4A
+			 *
+			 * Uses the algorithm from Annex G of the RDS standard
+			 * EN 50067:1998 to convert a UTC date to an RDS Modified
+			 * Julian Day.
+			 */
+			time_to_tm(get_seconds(), 0, &tm);
+			l = tm.tm_mon <= 1;
+			date = 14956 + tm.tm_mday + ((tm.tm_year - l) * 1461) / 4 +
+				((tm.tm_mon + 2 + l * 12) * 306001) / 10000;
+			time = (tm.tm_hour << 12) |
+			       (tm.tm_min << 6) |
+			       (sys_tz.tz_minuteswest >= 0 ? 0x20 : 0) |
+			       (abs(sys_tz.tz_minuteswest) / 30);
+			data[1].lsb &= ~3;
+			data[1].lsb |= date >> 15;
+			data[1].msb |= 8 << 3;
+			data[2].lsb = (date << 1) & 0xfe;
+			data[2].lsb |= (time >> 16) & 1;
+			data[2].msb = (date >> 7) & 0xff;
+			data[2].block = V4L2_RDS_BLOCK_C | (V4L2_RDS_BLOCK_C << 3);
+			data[3].lsb = time & 0xff;
+			data[3].msb = (time >> 8) & 0xff;
+			break;
+		default: /* Group 15B */
+			data[1].lsb |= (rds->ta << 4) | (rds->ms << 3);
+			data[1].lsb |= vivid_get_di(rds, grp % 22);
+			data[1].msb |= 0x1f << 3;
+			data[2].lsb = rds->picode & 0xff;
+			data[2].msb = rds->picode >> 8;
+			data[2].block = V4L2_RDS_BLOCK_C_ALT | (V4L2_RDS_BLOCK_C_ALT << 3);
+			data[3].lsb = rds->pty << 5;
+			data[3].lsb |= (rds->ta << 4) | (rds->ms << 3);
+			data[3].lsb |= vivid_get_di(rds, grp % 22);
+			data[3].msb |= rds->pty >> 3;
+			data[3].msb |= 0x1f << 3;
+			break;
+		}
+	}
+}
+
+void vivid_rds_gen_fill(struct vivid_rds_gen *rds, unsigned freq,
+			  bool alt)
+{
+	/* Alternate PTY between Info and Weather */
+	if (rds->use_rbds) {
+		rds->picode = 0x2e75; /* 'KLNX' call sign */
+		rds->pty = alt ? 29 : 2;
+	} else {
+		rds->picode = 0x8088;
+		rds->pty = alt ? 16 : 3;
+	}
+	rds->mono_stereo = true;
+	rds->art_head = false;
+	rds->compressed = false;
+	rds->dyn_pty = false;
+	rds->tp = true;
+	rds->ta = alt;
+	rds->ms = true;
+	snprintf(rds->psname, sizeof(rds->psname), "%6d.%1d",
+		 freq / 16, ((freq & 0xf) * 10) / 16);
+	if (alt)
+		strlcpy(rds->radiotext,
+			" The Radio Data System can switch between different Radio Texts ",
+			sizeof(rds->radiotext));
+	else
+		strlcpy(rds->radiotext,
+			"An example of Radio Text as transmitted by the Radio Data System",
+			sizeof(rds->radiotext));
+}
diff --git a/drivers/media/platform/vivid/vivid-rds-gen.h b/drivers/media/platform/vivid/vivid-rds-gen.h
new file mode 100644
index 000000000000..eff4bf552ed3
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-rds-gen.h
@@ -0,0 +1,53 @@
+/*
+ * vivid-rds-gen.h - rds (radio data system) generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_RDS_GEN_H_
+#define _VIVID_RDS_GEN_H_
+
+/*
+ * It takes almost exactly 5 seconds to transmit 57 RDS groups.
+ * Each group has 4 blocks and each block has a payload of 16 bits + a
+ * block identification. The driver will generate the contents of these
+ * 57 groups only when necessary and it will just be played continuously.
+ */
+#define VIVID_RDS_GEN_GROUPS 57
+#define VIVID_RDS_GEN_BLKS_PER_GRP 4
+#define VIVID_RDS_GEN_BLOCKS (VIVID_RDS_GEN_BLKS_PER_GRP * VIVID_RDS_GEN_GROUPS)
+
+struct vivid_rds_gen {
+	struct v4l2_rds_data	data[VIVID_RDS_GEN_BLOCKS];
+	bool			use_rbds;
+	u16			picode;
+	u8			pty;
+	bool			mono_stereo;
+	bool			art_head;
+	bool			compressed;
+	bool			dyn_pty;
+	bool			ta;
+	bool			tp;
+	bool			ms;
+	char			psname[8 + 1];
+	char			radiotext[64 + 1];
+};
+
+void vivid_rds_gen_fill(struct vivid_rds_gen *rds, unsigned freq,
+		    bool use_alternate);
+void vivid_rds_generate(struct vivid_rds_gen *rds);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-sdr-cap.c b/drivers/media/platform/vivid/vivid-sdr-cap.c
new file mode 100644
index 000000000000..8c5d661cfc49
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-sdr-cap.c
@@ -0,0 +1,499 @@
+/*
+ * vivid-sdr-cap.c - software defined radio support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/kthread.h>
+#include <linux/freezer.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-ctrls.h"
+#include "vivid-sdr-cap.h"
+
+static const struct v4l2_frequency_band bands_adc[] = {
+	{
+		.tuner = 0,
+		.type = V4L2_TUNER_ADC,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =  300000,
+		.rangehigh  =  300000,
+	},
+	{
+		.tuner = 0,
+		.type = V4L2_TUNER_ADC,
+		.index = 1,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =  900001,
+		.rangehigh  = 2800000,
+	},
+	{
+		.tuner = 0,
+		.type = V4L2_TUNER_ADC,
+		.index = 2,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   = 3200000,
+		.rangehigh  = 3200000,
+	},
+};
+
+/* ADC band midpoints */
+#define BAND_ADC_0 ((bands_adc[0].rangehigh + bands_adc[1].rangelow) / 2)
+#define BAND_ADC_1 ((bands_adc[1].rangehigh + bands_adc[2].rangelow) / 2)
+
+static const struct v4l2_frequency_band bands_fm[] = {
+	{
+		.tuner = 1,
+		.type = V4L2_TUNER_RF,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =    50000000,
+		.rangehigh  =  2000000000,
+	},
+};
+
+static void vivid_thread_sdr_cap_tick(struct vivid_dev *dev)
+{
+	struct vivid_buffer *sdr_cap_buf = NULL;
+
+	dprintk(dev, 1, "SDR Capture Thread Tick\n");
+
+	/* Drop a certain percentage of buffers. */
+	if (dev->perc_dropped_buffers &&
+	    prandom_u32_max(100) < dev->perc_dropped_buffers)
+		return;
+
+	spin_lock(&dev->slock);
+	if (!list_empty(&dev->sdr_cap_active)) {
+		sdr_cap_buf = list_entry(dev->sdr_cap_active.next,
+					 struct vivid_buffer, list);
+		list_del(&sdr_cap_buf->list);
+	}
+	spin_unlock(&dev->slock);
+
+	if (sdr_cap_buf) {
+		sdr_cap_buf->vb.v4l2_buf.sequence = dev->sdr_cap_seq_count;
+		vivid_sdr_cap_process(dev, sdr_cap_buf);
+		v4l2_get_timestamp(&sdr_cap_buf->vb.v4l2_buf.timestamp);
+		sdr_cap_buf->vb.v4l2_buf.timestamp.tv_sec += dev->time_wrap_offset;
+		vb2_buffer_done(&sdr_cap_buf->vb, dev->dqbuf_error ?
+				VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
+		dev->dqbuf_error = false;
+	}
+}
+
+static int vivid_thread_sdr_cap(void *data)
+{
+	struct vivid_dev *dev = data;
+	u64 samples_since_start;
+	u64 buffers_since_start;
+	u64 next_jiffies_since_start;
+	unsigned long jiffies_since_start;
+	unsigned long cur_jiffies;
+	unsigned wait_jiffies;
+
+	dprintk(dev, 1, "SDR Capture Thread Start\n");
+
+	set_freezable();
+
+	/* Resets frame counters */
+	dev->sdr_cap_seq_offset = 0;
+	if (dev->seq_wrap)
+		dev->sdr_cap_seq_offset = 0xffffff80U;
+	dev->jiffies_sdr_cap = jiffies;
+	dev->sdr_cap_seq_resync = false;
+
+	for (;;) {
+		try_to_freeze();
+		if (kthread_should_stop())
+			break;
+
+		mutex_lock(&dev->mutex);
+		cur_jiffies = jiffies;
+		if (dev->sdr_cap_seq_resync) {
+			dev->jiffies_sdr_cap = cur_jiffies;
+			dev->sdr_cap_seq_offset = dev->sdr_cap_seq_count + 1;
+			dev->sdr_cap_seq_count = 0;
+			dev->sdr_cap_seq_resync = false;
+		}
+		/* Calculate the number of jiffies since we started streaming */
+		jiffies_since_start = cur_jiffies - dev->jiffies_sdr_cap;
+		/* Get the number of buffers streamed since the start */
+		buffers_since_start = (u64)jiffies_since_start * dev->sdr_adc_freq +
+				      (HZ * SDR_CAP_SAMPLES_PER_BUF) / 2;
+		do_div(buffers_since_start, HZ * SDR_CAP_SAMPLES_PER_BUF);
+
+		/*
+		 * After more than 0xf0000000 (rounded down to a multiple of
+		 * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
+		 * jiffies have passed since we started streaming reset the
+		 * counters and keep track of the sequence offset.
+		 */
+		if (jiffies_since_start > JIFFIES_RESYNC) {
+			dev->jiffies_sdr_cap = cur_jiffies;
+			dev->sdr_cap_seq_offset = buffers_since_start;
+			buffers_since_start = 0;
+		}
+		dev->sdr_cap_seq_count = buffers_since_start + dev->sdr_cap_seq_offset;
+
+		vivid_thread_sdr_cap_tick(dev);
+		mutex_unlock(&dev->mutex);
+
+		/*
+		 * Calculate the number of samples streamed since we started,
+		 * not including the current buffer.
+		 */
+		samples_since_start = buffers_since_start * SDR_CAP_SAMPLES_PER_BUF;
+
+		/* And the number of jiffies since we started */
+		jiffies_since_start = jiffies - dev->jiffies_sdr_cap;
+
+		/* Increase by the number of samples in one buffer */
+		samples_since_start += SDR_CAP_SAMPLES_PER_BUF;
+		/*
+		 * Calculate when that next buffer is supposed to start
+		 * in jiffies since we started streaming.
+		 */
+		next_jiffies_since_start = samples_since_start * HZ +
+					   dev->sdr_adc_freq / 2;
+		do_div(next_jiffies_since_start, dev->sdr_adc_freq);
+		/* If it is in the past, then just schedule asap */
+		if (next_jiffies_since_start < jiffies_since_start)
+			next_jiffies_since_start = jiffies_since_start;
+
+		wait_jiffies = next_jiffies_since_start - jiffies_since_start;
+		schedule_timeout_interruptible(wait_jiffies ? wait_jiffies : 1);
+	}
+	dprintk(dev, 1, "SDR Capture Thread End\n");
+	return 0;
+}
+
+static int sdr_cap_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	/* 2 = max 16-bit sample returned */
+	sizes[0] = SDR_CAP_SAMPLES_PER_BUF * 2;
+	*nplanes = 1;
+	return 0;
+}
+
+static int sdr_cap_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned size = SDR_CAP_SAMPLES_PER_BUF * 2;
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+				__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void sdr_cap_buf_queue(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vb, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->sdr_cap_active);
+	spin_unlock(&dev->slock);
+}
+
+static int sdr_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err = 0;
+
+	dprintk(dev, 1, "%s\n", __func__);
+	dev->sdr_cap_seq_count = 0;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else if (dev->kthread_sdr_cap == NULL) {
+		dev->kthread_sdr_cap = kthread_run(vivid_thread_sdr_cap, dev,
+				"%s-sdr-cap", dev->v4l2_dev.name);
+
+		if (IS_ERR(dev->kthread_sdr_cap)) {
+			v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
+			err = PTR_ERR(dev->kthread_sdr_cap);
+			dev->kthread_sdr_cap = NULL;
+		}
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->sdr_cap_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void sdr_cap_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	if (dev->kthread_sdr_cap == NULL)
+		return;
+
+	while (!list_empty(&dev->sdr_cap_active)) {
+		struct vivid_buffer *buf;
+
+		buf = list_entry(dev->sdr_cap_active.next, struct vivid_buffer, list);
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+
+	/* shutdown control thread */
+	mutex_unlock(&dev->mutex);
+	kthread_stop(dev->kthread_sdr_cap);
+	dev->kthread_sdr_cap = NULL;
+	mutex_lock(&dev->mutex);
+}
+
+const struct vb2_ops vivid_sdr_cap_qops = {
+	.queue_setup		= sdr_cap_queue_setup,
+	.buf_prepare		= sdr_cap_buf_prepare,
+	.buf_queue		= sdr_cap_buf_queue,
+	.start_streaming	= sdr_cap_start_streaming,
+	.stop_streaming		= sdr_cap_stop_streaming,
+	.wait_prepare		= vivid_unlock,
+	.wait_finish		= vivid_lock,
+};
+
+int vivid_sdr_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band)
+{
+	switch (band->tuner) {
+	case 0:
+		if (band->index >= ARRAY_SIZE(bands_adc))
+			return -EINVAL;
+		*band = bands_adc[band->index];
+		return 0;
+	case 1:
+		if (band->index >= ARRAY_SIZE(bands_fm))
+			return -EINVAL;
+		*band = bands_fm[band->index];
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	switch (vf->tuner) {
+	case 0:
+		vf->frequency = dev->sdr_adc_freq;
+		vf->type = V4L2_TUNER_ADC;
+		return 0;
+	case 1:
+		vf->frequency = dev->sdr_fm_freq;
+		vf->type = V4L2_TUNER_RF;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned freq = vf->frequency;
+	unsigned band;
+
+	switch (vf->tuner) {
+	case 0:
+		if (vf->type != V4L2_TUNER_ADC)
+			return -EINVAL;
+		if (freq < BAND_ADC_0)
+			band = 0;
+		else if (freq < BAND_ADC_1)
+			band = 1;
+		else
+			band = 2;
+
+		freq = clamp_t(unsigned, freq,
+				bands_adc[band].rangelow,
+				bands_adc[band].rangehigh);
+
+		if (vb2_is_streaming(&dev->vb_sdr_cap_q) &&
+		    freq != dev->sdr_adc_freq) {
+			/* resync the thread's timings */
+			dev->sdr_cap_seq_resync = true;
+		}
+		dev->sdr_adc_freq = freq;
+		return 0;
+	case 1:
+		if (vf->type != V4L2_TUNER_RF)
+			return -EINVAL;
+		dev->sdr_fm_freq = clamp_t(unsigned, freq,
+				bands_fm[0].rangelow,
+				bands_fm[0].rangehigh);
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	switch (vt->index) {
+	case 0:
+		strlcpy(vt->name, "ADC", sizeof(vt->name));
+		vt->type = V4L2_TUNER_ADC;
+		vt->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+		vt->rangelow = bands_adc[0].rangelow;
+		vt->rangehigh = bands_adc[2].rangehigh;
+		return 0;
+	case 1:
+		strlcpy(vt->name, "RF", sizeof(vt->name));
+		vt->type = V4L2_TUNER_RF;
+		vt->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+		vt->rangelow = bands_fm[0].rangelow;
+		vt->rangehigh = bands_fm[0].rangehigh;
+		return 0;
+	default:
+		return -EINVAL;
+	}
+}
+
+int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	if (vt->index > 1)
+		return -EINVAL;
+	return 0;
+}
+
+int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f)
+{
+	if (f->index)
+		return -EINVAL;
+	f->pixelformat = V4L2_SDR_FMT_CU8;
+	strlcpy(f->description, "IQ U8", sizeof(f->description));
+	return 0;
+}
+
+int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f)
+{
+	f->fmt.sdr.pixelformat = V4L2_SDR_FMT_CU8;
+	f->fmt.sdr.buffersize = SDR_CAP_SAMPLES_PER_BUF * 2;
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	return 0;
+}
+
+#define FIXP_FRAC    (1 << 15)
+#define FIXP_PI      ((int)(FIXP_FRAC * 3.141592653589))
+
+/* cos() from cx88 driver: cx88-dsp.c */
+static s32 fixp_cos(unsigned int x)
+{
+	u32 t2, t4, t6, t8;
+	u16 period = x / FIXP_PI;
+
+	if (period % 2)
+		return -fixp_cos(x - FIXP_PI);
+	x = x % FIXP_PI;
+	if (x > FIXP_PI/2)
+		return -fixp_cos(FIXP_PI/2 - (x % (FIXP_PI/2)));
+	/* Now x is between 0 and FIXP_PI/2.
+	 * To calculate cos(x) we use it's Taylor polinom. */
+	t2 = x*x/FIXP_FRAC/2;
+	t4 = t2*x/FIXP_FRAC*x/FIXP_FRAC/3/4;
+	t6 = t4*x/FIXP_FRAC*x/FIXP_FRAC/5/6;
+	t8 = t6*x/FIXP_FRAC*x/FIXP_FRAC/7/8;
+	return FIXP_FRAC-t2+t4-t6+t8;
+}
+
+static inline s32 fixp_sin(unsigned int x)
+{
+	return -fixp_cos(x + (FIXP_PI / 2));
+}
+
+void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	u8 *vbuf = vb2_plane_vaddr(&buf->vb, 0);
+	unsigned long i;
+	unsigned long plane_size = vb2_plane_size(&buf->vb, 0);
+	int fixp_src_phase_step, fixp_i, fixp_q;
+
+	/*
+	 * TODO: Generated beep tone goes very crackly when sample rate is
+	 * increased to ~1Msps or more. That is because of huge rounding error
+	 * of phase angle caused by used cosine implementation.
+	 */
+
+	/* calculate phase step */
+	#define BEEP_FREQ 1000 /* 1kHz beep */
+	fixp_src_phase_step = DIV_ROUND_CLOSEST(2 * FIXP_PI * BEEP_FREQ,
+			dev->sdr_adc_freq);
+
+	for (i = 0; i < plane_size; i += 2) {
+		dev->sdr_fixp_mod_phase += fixp_cos(dev->sdr_fixp_src_phase);
+		dev->sdr_fixp_src_phase += fixp_src_phase_step;
+
+		/*
+		 * Transfer phases to [0 / 2xPI] in order to avoid variable
+		 * overflow and make it suitable for cosine implementation
+		 * used, which does not support negative angles.
+		 */
+		while (dev->sdr_fixp_mod_phase < (0 * FIXP_PI))
+			dev->sdr_fixp_mod_phase += (2 * FIXP_PI);
+		while (dev->sdr_fixp_mod_phase > (2 * FIXP_PI))
+			dev->sdr_fixp_mod_phase -= (2 * FIXP_PI);
+
+		while (dev->sdr_fixp_src_phase > (2 * FIXP_PI))
+			dev->sdr_fixp_src_phase -= (2 * FIXP_PI);
+
+		fixp_i = fixp_cos(dev->sdr_fixp_mod_phase);
+		fixp_q = fixp_sin(dev->sdr_fixp_mod_phase);
+
+		/* convert 'fixp float' to u8 */
+		/* u8 = X * 127.5f + 127.5f; where X is float [-1.0 / +1.0] */
+		fixp_i = fixp_i * 1275 + FIXP_FRAC * 1275;
+		fixp_q = fixp_q * 1275 + FIXP_FRAC * 1275;
+		*vbuf++ = DIV_ROUND_CLOSEST(fixp_i, FIXP_FRAC * 10);
+		*vbuf++ = DIV_ROUND_CLOSEST(fixp_q, FIXP_FRAC * 10);
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-sdr-cap.h b/drivers/media/platform/vivid/vivid-sdr-cap.h
new file mode 100644
index 000000000000..79c1890de972
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-sdr-cap.h
@@ -0,0 +1,34 @@
+/*
+ * vivid-sdr-cap.h - software defined radio support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_SDR_CAP_H_
+#define _VIVID_SDR_CAP_H_
+
+int vivid_sdr_enum_freq_bands(struct file *file, void *fh, struct v4l2_frequency_band *band);
+int vivid_sdr_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int vivid_sdr_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
+int vivid_sdr_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vivid_sdr_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+int vidioc_enum_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_sdr_cap(struct file *file, void *fh, struct v4l2_format *f);
+void vivid_sdr_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+
+extern const struct vb2_ops vivid_sdr_cap_qops;
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-tpg-colors.c b/drivers/media/platform/vivid/vivid-tpg-colors.c
new file mode 100644
index 000000000000..2adddc0ca662
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg-colors.c
@@ -0,0 +1,310 @@
+/*
+ * vivid-color.c - A table that converts colors to various colorspaces
+ *
+ * The test pattern generator uses the tpg_colors for its test patterns.
+ * For testing colorspaces the first 8 colors of that table need to be
+ * converted to their equivalent in the target colorspace.
+ *
+ * The tpg_csc_colors[] table is the result of that conversion and since
+ * it is precalculated the colorspace conversion is just a simple table
+ * lookup.
+ *
+ * This source also contains the code used to generate the tpg_csc_colors
+ * table. Run the following command to compile it:
+ *
+ *	gcc vivid-colors.c -DCOMPILE_APP -o gen-colors -lm
+ *
+ * and run the utility.
+ *
+ * Note that the converted colors are in the range 0x000-0xff0 (so times 16)
+ * in order to preserve precision.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/videodev2.h>
+
+#include "vivid-tpg-colors.h"
+
+/* sRGB colors with range [0-255] */
+const struct color tpg_colors[TPG_COLOR_MAX] = {
+	/*
+	 * Colors to test colorspace conversion: converting these colors
+	 * to other colorspaces will never lead to out-of-gamut colors.
+	 */
+	{ 191, 191, 191 }, /* TPG_COLOR_CSC_WHITE */
+	{ 191, 191,  50 }, /* TPG_COLOR_CSC_YELLOW */
+	{  50, 191, 191 }, /* TPG_COLOR_CSC_CYAN */
+	{  50, 191,  50 }, /* TPG_COLOR_CSC_GREEN */
+	{ 191,  50, 191 }, /* TPG_COLOR_CSC_MAGENTA */
+	{ 191,  50,  50 }, /* TPG_COLOR_CSC_RED */
+	{  50,  50, 191 }, /* TPG_COLOR_CSC_BLUE */
+	{  50,  50,  50 }, /* TPG_COLOR_CSC_BLACK */
+
+	/* 75% colors */
+	{ 191, 191,   0 }, /* TPG_COLOR_75_YELLOW */
+	{   0, 191, 191 }, /* TPG_COLOR_75_CYAN */
+	{   0, 191,   0 }, /* TPG_COLOR_75_GREEN */
+	{ 191,   0, 191 }, /* TPG_COLOR_75_MAGENTA */
+	{ 191,   0,   0 }, /* TPG_COLOR_75_RED */
+	{   0,   0, 191 }, /* TPG_COLOR_75_BLUE */
+
+	/* 100% colors */
+	{ 255, 255, 255 }, /* TPG_COLOR_100_WHITE */
+	{ 255, 255,   0 }, /* TPG_COLOR_100_YELLOW */
+	{   0, 255, 255 }, /* TPG_COLOR_100_CYAN */
+	{   0, 255,   0 }, /* TPG_COLOR_100_GREEN */
+	{ 255,   0, 255 }, /* TPG_COLOR_100_MAGENTA */
+	{ 255,   0,   0 }, /* TPG_COLOR_100_RED */
+	{   0,   0, 255 }, /* TPG_COLOR_100_BLUE */
+	{   0,   0,   0 }, /* TPG_COLOR_100_BLACK */
+
+	{   0,   0,   0 }, /* TPG_COLOR_RANDOM placeholder */
+};
+
+#ifndef COMPILE_APP
+
+/* Generated table */
+const struct color16 tpg_csc_colors[V4L2_COLORSPACE_SRGB + 1][TPG_COLOR_CSC_BLACK + 1] = {
+	[V4L2_COLORSPACE_SMPTE170M][0] = { 2953, 2939, 2939 },
+	[V4L2_COLORSPACE_SMPTE170M][1] = { 2954, 2963, 585 },
+	[V4L2_COLORSPACE_SMPTE170M][2] = { 84, 2967, 2937 },
+	[V4L2_COLORSPACE_SMPTE170M][3] = { 93, 2990, 575 },
+	[V4L2_COLORSPACE_SMPTE170M][4] = { 3030, 259, 2933 },
+	[V4L2_COLORSPACE_SMPTE170M][5] = { 3031, 406, 557 },
+	[V4L2_COLORSPACE_SMPTE170M][6] = { 544, 428, 2931 },
+	[V4L2_COLORSPACE_SMPTE170M][7] = { 551, 547, 547 },
+	[V4L2_COLORSPACE_SMPTE240M][0] = { 2926, 2926, 2926 },
+	[V4L2_COLORSPACE_SMPTE240M][1] = { 2926, 2926, 857 },
+	[V4L2_COLORSPACE_SMPTE240M][2] = { 1594, 2901, 2901 },
+	[V4L2_COLORSPACE_SMPTE240M][3] = { 1594, 2901, 774 },
+	[V4L2_COLORSPACE_SMPTE240M][4] = { 2484, 618, 2858 },
+	[V4L2_COLORSPACE_SMPTE240M][5] = { 2484, 618, 617 },
+	[V4L2_COLORSPACE_SMPTE240M][6] = { 507, 507, 2832 },
+	[V4L2_COLORSPACE_SMPTE240M][7] = { 507, 507, 507 },
+	[V4L2_COLORSPACE_REC709][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_REC709][1] = { 2939, 2939, 547 },
+	[V4L2_COLORSPACE_REC709][2] = { 547, 2939, 2939 },
+	[V4L2_COLORSPACE_REC709][3] = { 547, 2939, 547 },
+	[V4L2_COLORSPACE_REC709][4] = { 2939, 547, 2939 },
+	[V4L2_COLORSPACE_REC709][5] = { 2939, 547, 547 },
+	[V4L2_COLORSPACE_REC709][6] = { 547, 547, 2939 },
+	[V4L2_COLORSPACE_REC709][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][0] = { 2894, 2988, 2808 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][1] = { 2847, 3070, 843 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][2] = { 1656, 2962, 2783 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][3] = { 1572, 3045, 763 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][4] = { 2477, 229, 2743 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][5] = { 2422, 672, 614 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][6] = { 725, 63, 2718 },
+	[V4L2_COLORSPACE_470_SYSTEM_M][7] = { 534, 561, 509 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][0] = { 2939, 2939, 2939 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][1] = { 2939, 2939, 621 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][2] = { 786, 2939, 2939 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][3] = { 786, 2939, 621 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][4] = { 2879, 547, 2923 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][5] = { 2879, 547, 547 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][6] = { 547, 547, 2923 },
+	[V4L2_COLORSPACE_470_SYSTEM_BG][7] = { 547, 547, 547 },
+	[V4L2_COLORSPACE_SRGB][0] = { 3056, 3056, 3056 },
+	[V4L2_COLORSPACE_SRGB][1] = { 3056, 3056, 800 },
+	[V4L2_COLORSPACE_SRGB][2] = { 800, 3056, 3056 },
+	[V4L2_COLORSPACE_SRGB][3] = { 800, 3056, 800 },
+	[V4L2_COLORSPACE_SRGB][4] = { 3056, 800, 3056 },
+	[V4L2_COLORSPACE_SRGB][5] = { 3056, 800, 800 },
+	[V4L2_COLORSPACE_SRGB][6] = { 800, 800, 3056 },
+	[V4L2_COLORSPACE_SRGB][7] = { 800, 800, 800 },
+};
+
+#else
+
+/* This code generates the table above */
+
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+static const double rec709_to_ntsc1953[3][3] = {
+	{ 0.6698, 0.2678,  0.0323 },
+	{ 0.0185, 1.0742, -0.0603 },
+	{ 0.0162, 0.0432,  0.8551 }
+};
+
+static const double rec709_to_ebu[3][3] = {
+	{ 0.9578, 0.0422, 0      },
+	{ 0     , 1     , 0      },
+	{ 0     , 0.0118, 0.9882 }
+};
+
+static const double rec709_to_170m[3][3] = {
+	{  1.0654, -0.0554, -0.0010 },
+	{ -0.0196,  1.0364, -0.0167 },
+	{  0.0016,  0.0044,  0.9940 }
+};
+
+static const double rec709_to_240m[3][3] = {
+	{ 0.7151, 0.2849, 0      },
+	{ 0.0179, 0.9821, 0      },
+	{ 0.0177, 0.0472, 0.9350 }
+};
+
+
+static void mult_matrix(double *r, double *g, double *b, const double m[3][3])
+{
+	double ir, ig, ib;
+
+	ir = m[0][0] * (*r) + m[0][1] * (*g) + m[0][2] * (*b);
+	ig = m[1][0] * (*r) + m[1][1] * (*g) + m[1][2] * (*b);
+	ib = m[2][0] * (*r) + m[2][1] * (*g) + m[2][2] * (*b);
+	*r = ir;
+	*g = ig;
+	*b = ib;
+}
+
+static double transfer_srgb_to_rgb(double v)
+{
+	return (v <= 0.03928) ? v / 12.92 : pow((v + 0.055) / 1.055, 2.4);
+}
+
+static double transfer_rgb_to_smpte240m(double v)
+{
+	return (v <= 0.0228) ? v * 4.0 : 1.1115 * pow(v, 0.45) - 0.1115;
+}
+
+static double transfer_rgb_to_rec709(double v)
+{
+	return (v < 0.018) ? v * 4.5 : 1.099 * pow(v, 0.45) - 0.099;
+}
+
+static double transfer_srgb_to_rec709(double v)
+{
+	return transfer_rgb_to_rec709(transfer_srgb_to_rgb(v));
+}
+
+static void csc(enum v4l2_colorspace colorspace, double *r, double *g, double *b)
+{
+	/* Convert the primaries of Rec. 709 Linear RGB */
+	switch (colorspace) {
+	case V4L2_COLORSPACE_SMPTE240M:
+		*r = transfer_srgb_to_rgb(*r);
+		*g = transfer_srgb_to_rgb(*g);
+		*b = transfer_srgb_to_rgb(*b);
+		mult_matrix(r, g, b, rec709_to_240m);
+		break;
+	case V4L2_COLORSPACE_SMPTE170M:
+		*r = transfer_srgb_to_rgb(*r);
+		*g = transfer_srgb_to_rgb(*g);
+		*b = transfer_srgb_to_rgb(*b);
+		mult_matrix(r, g, b, rec709_to_170m);
+		break;
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		*r = transfer_srgb_to_rgb(*r);
+		*g = transfer_srgb_to_rgb(*g);
+		*b = transfer_srgb_to_rgb(*b);
+		mult_matrix(r, g, b, rec709_to_ebu);
+		break;
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+		*r = transfer_srgb_to_rgb(*r);
+		*g = transfer_srgb_to_rgb(*g);
+		*b = transfer_srgb_to_rgb(*b);
+		mult_matrix(r, g, b, rec709_to_ntsc1953);
+		break;
+	case V4L2_COLORSPACE_SRGB:
+	case V4L2_COLORSPACE_REC709:
+	default:
+		break;
+	}
+
+	*r = ((*r) < 0) ? 0 : (((*r) > 1) ? 1 : (*r));
+	*g = ((*g) < 0) ? 0 : (((*g) > 1) ? 1 : (*g));
+	*b = ((*b) < 0) ? 0 : (((*b) > 1) ? 1 : (*b));
+
+	/* Encode to gamma corrected colorspace */
+	switch (colorspace) {
+	case V4L2_COLORSPACE_SMPTE240M:
+		*r = transfer_rgb_to_smpte240m(*r);
+		*g = transfer_rgb_to_smpte240m(*g);
+		*b = transfer_rgb_to_smpte240m(*b);
+		break;
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		*r = transfer_rgb_to_rec709(*r);
+		*g = transfer_rgb_to_rec709(*g);
+		*b = transfer_rgb_to_rec709(*b);
+		break;
+	case V4L2_COLORSPACE_SRGB:
+		break;
+	case V4L2_COLORSPACE_REC709:
+	default:
+		*r = transfer_srgb_to_rec709(*r);
+		*g = transfer_srgb_to_rec709(*g);
+		*b = transfer_srgb_to_rec709(*b);
+		break;
+	}
+}
+
+int main(int argc, char **argv)
+{
+	static const unsigned colorspaces[] = {
+		0,
+		V4L2_COLORSPACE_SMPTE170M,
+		V4L2_COLORSPACE_SMPTE240M,
+		V4L2_COLORSPACE_REC709,
+		0,
+		V4L2_COLORSPACE_470_SYSTEM_M,
+		V4L2_COLORSPACE_470_SYSTEM_BG,
+		0,
+		V4L2_COLORSPACE_SRGB,
+	};
+	static const char * const colorspace_names[] = {
+		"",
+		"V4L2_COLORSPACE_SMPTE170M",
+		"V4L2_COLORSPACE_SMPTE240M",
+		"V4L2_COLORSPACE_REC709",
+		"",
+		"V4L2_COLORSPACE_470_SYSTEM_M",
+		"V4L2_COLORSPACE_470_SYSTEM_BG",
+		"",
+		"V4L2_COLORSPACE_SRGB",
+	};
+	int i;
+	int c;
+
+	printf("/* Generated table */\n");
+	printf("const struct color16 tpg_csc_colors[V4L2_COLORSPACE_SRGB + 1][TPG_COLOR_CSC_BLACK + 1] = {\n");
+	for (c = 0; c <= V4L2_COLORSPACE_SRGB; c++) {
+		for (i = 0; i <= TPG_COLOR_CSC_BLACK; i++) {
+			double r, g, b;
+
+			if (colorspaces[c] == 0)
+				continue;
+
+			r = tpg_colors[i].r / 255.0;
+			g = tpg_colors[i].g / 255.0;
+			b = tpg_colors[i].b / 255.0;
+
+			csc(c, &r, &g, &b);
+
+			printf("\t[%s][%d] = { %d, %d, %d },\n", colorspace_names[c], i,
+				(int)(r * 4080), (int)(g * 4080), (int)(b * 4080));
+		}
+	}
+	printf("};\n\n");
+	return 0;
+}
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-tpg-colors.h b/drivers/media/platform/vivid/vivid-tpg-colors.h
new file mode 100644
index 000000000000..a2678fbec256
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg-colors.h
@@ -0,0 +1,64 @@
+/*
+ * vivid-color.h - Color definitions for the test pattern generator
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_COLORS_H_
+#define _VIVID_COLORS_H_
+
+struct color {
+	unsigned char r, g, b;
+};
+
+struct color16 {
+	int r, g, b;
+};
+
+enum tpg_color {
+	TPG_COLOR_CSC_WHITE,
+	TPG_COLOR_CSC_YELLOW,
+	TPG_COLOR_CSC_CYAN,
+	TPG_COLOR_CSC_GREEN,
+	TPG_COLOR_CSC_MAGENTA,
+	TPG_COLOR_CSC_RED,
+	TPG_COLOR_CSC_BLUE,
+	TPG_COLOR_CSC_BLACK,
+	TPG_COLOR_75_YELLOW,
+	TPG_COLOR_75_CYAN,
+	TPG_COLOR_75_GREEN,
+	TPG_COLOR_75_MAGENTA,
+	TPG_COLOR_75_RED,
+	TPG_COLOR_75_BLUE,
+	TPG_COLOR_100_WHITE,
+	TPG_COLOR_100_YELLOW,
+	TPG_COLOR_100_CYAN,
+	TPG_COLOR_100_GREEN,
+	TPG_COLOR_100_MAGENTA,
+	TPG_COLOR_100_RED,
+	TPG_COLOR_100_BLUE,
+	TPG_COLOR_100_BLACK,
+	TPG_COLOR_TEXTFG,
+	TPG_COLOR_TEXTBG,
+	TPG_COLOR_RANDOM,
+	TPG_COLOR_RAMP,
+	TPG_COLOR_MAX = TPG_COLOR_RAMP + 256
+};
+
+extern const struct color tpg_colors[TPG_COLOR_MAX];
+extern const struct color16 tpg_csc_colors[V4L2_COLORSPACE_SRGB + 1][TPG_COLOR_CSC_BLACK + 1];
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-tpg.c b/drivers/media/platform/vivid/vivid-tpg.c
new file mode 100644
index 000000000000..0c6fa53fa646
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg.c
@@ -0,0 +1,1439 @@
+/*
+ * vivid-tpg.c - Test Pattern Generator
+ *
+ * Note: gen_twopix and tpg_gen_text are based on code from vivi.c. See the
+ * vivi.c source for the copyright information of those functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "vivid-tpg.h"
+
+/* Must remain in sync with enum tpg_pattern */
+const char * const tpg_pattern_strings[] = {
+	"75% Colorbar",
+	"100% Colorbar",
+	"CSC Colorbar",
+	"Horizontal 100% Colorbar",
+	"100% Color Squares",
+	"100% Black",
+	"100% White",
+	"100% Red",
+	"100% Green",
+	"100% Blue",
+	"16x16 Checkers",
+	"1x1 Checkers",
+	"Alternating Hor Lines",
+	"Alternating Vert Lines",
+	"One Pixel Wide Cross",
+	"Two Pixels Wide Cross",
+	"Ten Pixels Wide Cross",
+	"Gray Ramp",
+	"Noise",
+	NULL
+};
+
+/* Must remain in sync with enum tpg_aspect */
+const char * const tpg_aspect_strings[] = {
+	"Source Width x Height",
+	"4x3",
+	"14x9",
+	"16x9",
+	"16x9 Anamorphic",
+	NULL
+};
+
+/*
+ * Sine table: sin[0] = 127 * sin(-180 degrees)
+ *             sin[128] = 127 * sin(0 degrees)
+ *             sin[256] = 127 * sin(180 degrees)
+ */
+static const s8 sin[257] = {
+	   0,   -4,   -7,  -11,  -13,  -18,  -20,  -22,  -26,  -29,  -33,  -35,  -37,  -41,  -43,  -48,
+	 -50,  -52,  -56,  -58,  -62,  -63,  -65,  -69,  -71,  -75,  -76,  -78,  -82,  -83,  -87,  -88,
+	 -90,  -93,  -94,  -97,  -99, -101, -103, -104, -107, -108, -110, -111, -112, -114, -115, -117,
+	-118, -119, -120, -121, -122, -123, -123, -124, -125, -125, -126, -126, -127, -127, -127, -127,
+	-127, -127, -127, -127, -126, -126, -125, -125, -124, -124, -123, -122, -121, -120, -119, -118,
+	-117, -116, -114, -113, -111, -110, -109, -107, -105, -103, -101, -100,  -97,  -96,  -93,  -91,
+	 -90,  -87,  -85,  -82,  -80,  -76,  -75,  -73,  -69,  -67,  -63,  -62,  -60,  -56,  -54,  -50,
+	 -48,  -46,  -41,  -39,  -35,  -33,  -31,  -26,  -24,  -20,  -18,  -15,  -11,   -9,   -4,   -2,
+	   0,    2,    4,    9,   11,   15,   18,   20,   24,   26,   31,   33,   35,   39,   41,   46,
+	  48,   50,   54,   56,   60,   62,   64,   67,   69,   73,   75,   76,   80,   82,   85,   87,
+	  90,   91,   93,   96,   97,  100,  101,  103,  105,  107,  109,  110,  111,  113,  114,  116,
+	 117,  118,  119,  120,  121,  122,  123,  124,  124,  125,  125,  126,  126,  127,  127,  127,
+	 127,  127,  127,  127,  127,  126,  126,  125,  125,  124,  123,  123,  122,  121,  120,  119,
+	 118,  117,  115,  114,  112,  111,  110,  108,  107,  104,  103,  101,   99,   97,   94,   93,
+	  90,   88,   87,   83,   82,   78,   76,   75,   71,   69,   65,   64,   62,   58,   56,   52,
+	  50,   48,   43,   41,   37,   35,   33,   29,   26,   22,   20,   18,   13,   11,    7,    4,
+	   0,
+};
+
+#define cos(idx) sin[((idx) + 64) % sizeof(sin)]
+
+/* Global font descriptor */
+static const u8 *font8x16;
+
+void tpg_set_font(const u8 *f)
+{
+	font8x16 = f;
+}
+
+void tpg_init(struct tpg_data *tpg, unsigned w, unsigned h)
+{
+	memset(tpg, 0, sizeof(*tpg));
+	tpg->scaled_width = tpg->src_width = w;
+	tpg->src_height = tpg->buf_height = h;
+	tpg->crop.width = tpg->compose.width = w;
+	tpg->crop.height = tpg->compose.height = h;
+	tpg->recalc_colors = true;
+	tpg->recalc_square_border = true;
+	tpg->brightness = 128;
+	tpg->contrast = 128;
+	tpg->saturation = 128;
+	tpg->hue = 0;
+	tpg->mv_hor_mode = TPG_MOVE_NONE;
+	tpg->mv_vert_mode = TPG_MOVE_NONE;
+	tpg->field = V4L2_FIELD_NONE;
+	tpg_s_fourcc(tpg, V4L2_PIX_FMT_RGB24);
+	tpg->colorspace = V4L2_COLORSPACE_SRGB;
+	tpg->perc_fill = 100;
+}
+
+int tpg_alloc(struct tpg_data *tpg, unsigned max_w)
+{
+	unsigned pat;
+	unsigned plane;
+
+	tpg->max_line_width = max_w;
+	for (pat = 0; pat < TPG_MAX_PAT_LINES; pat++) {
+		for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+			unsigned pixelsz = plane ? 1 : 4;
+
+			tpg->lines[pat][plane] = vzalloc(max_w * 2 * pixelsz);
+			if (!tpg->lines[pat][plane])
+				return -ENOMEM;
+		}
+	}
+	for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+		unsigned pixelsz = plane ? 1 : 4;
+
+		tpg->contrast_line[plane] = vzalloc(max_w * pixelsz);
+		if (!tpg->contrast_line[plane])
+			return -ENOMEM;
+		tpg->black_line[plane] = vzalloc(max_w * pixelsz);
+		if (!tpg->black_line[plane])
+			return -ENOMEM;
+		tpg->random_line[plane] = vzalloc(max_w * pixelsz);
+		if (!tpg->random_line[plane])
+			return -ENOMEM;
+	}
+	return 0;
+}
+
+void tpg_free(struct tpg_data *tpg)
+{
+	unsigned pat;
+	unsigned plane;
+
+	for (pat = 0; pat < TPG_MAX_PAT_LINES; pat++)
+		for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+			vfree(tpg->lines[pat][plane]);
+			tpg->lines[pat][plane] = NULL;
+		}
+	for (plane = 0; plane < TPG_MAX_PLANES; plane++) {
+		vfree(tpg->contrast_line[plane]);
+		vfree(tpg->black_line[plane]);
+		vfree(tpg->random_line[plane]);
+		tpg->contrast_line[plane] = NULL;
+		tpg->black_line[plane] = NULL;
+		tpg->random_line[plane] = NULL;
+	}
+}
+
+bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc)
+{
+	tpg->fourcc = fourcc;
+	tpg->planes = 1;
+	tpg->recalc_colors = true;
+	switch (fourcc) {
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_RGB555:
+	case V4L2_PIX_FMT_XRGB555:
+	case V4L2_PIX_FMT_ARGB555:
+	case V4L2_PIX_FMT_RGB555X:
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_XRGB32:
+	case V4L2_PIX_FMT_XBGR32:
+	case V4L2_PIX_FMT_ARGB32:
+	case V4L2_PIX_FMT_ABGR32:
+		tpg->is_yuv = false;
+		break;
+	case V4L2_PIX_FMT_NV16M:
+	case V4L2_PIX_FMT_NV61M:
+		tpg->planes = 2;
+		/* fall-through */
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_VYUY:
+		tpg->is_yuv = true;
+		break;
+	default:
+		return false;
+	}
+
+	switch (fourcc) {
+	case V4L2_PIX_FMT_RGB565:
+	case V4L2_PIX_FMT_RGB565X:
+	case V4L2_PIX_FMT_RGB555:
+	case V4L2_PIX_FMT_XRGB555:
+	case V4L2_PIX_FMT_ARGB555:
+	case V4L2_PIX_FMT_RGB555X:
+	case V4L2_PIX_FMT_YUYV:
+	case V4L2_PIX_FMT_UYVY:
+	case V4L2_PIX_FMT_YVYU:
+	case V4L2_PIX_FMT_VYUY:
+		tpg->twopixelsize[0] = 2 * 2;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+	case V4L2_PIX_FMT_BGR24:
+		tpg->twopixelsize[0] = 2 * 3;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_XRGB32:
+	case V4L2_PIX_FMT_XBGR32:
+	case V4L2_PIX_FMT_ARGB32:
+	case V4L2_PIX_FMT_ABGR32:
+		tpg->twopixelsize[0] = 2 * 4;
+		break;
+	case V4L2_PIX_FMT_NV16M:
+	case V4L2_PIX_FMT_NV61M:
+		tpg->twopixelsize[0] = 2;
+		tpg->twopixelsize[1] = 2;
+		break;
+	}
+	return true;
+}
+
+void tpg_s_crop_compose(struct tpg_data *tpg, const struct v4l2_rect *crop,
+		const struct v4l2_rect *compose)
+{
+	tpg->crop = *crop;
+	tpg->compose = *compose;
+	tpg->scaled_width = (tpg->src_width * tpg->compose.width +
+				 tpg->crop.width - 1) / tpg->crop.width;
+	tpg->scaled_width &= ~1;
+	if (tpg->scaled_width > tpg->max_line_width)
+		tpg->scaled_width = tpg->max_line_width;
+	if (tpg->scaled_width < 2)
+		tpg->scaled_width = 2;
+	tpg->recalc_lines = true;
+}
+
+void tpg_reset_source(struct tpg_data *tpg, unsigned width, unsigned height,
+		       u32 field)
+{
+	unsigned p;
+
+	tpg->src_width = width;
+	tpg->src_height = height;
+	tpg->field = field;
+	tpg->buf_height = height;
+	if (V4L2_FIELD_HAS_T_OR_B(field))
+		tpg->buf_height /= 2;
+	tpg->scaled_width = width;
+	tpg->crop.top = tpg->crop.left = 0;
+	tpg->crop.width = width;
+	tpg->crop.height = height;
+	tpg->compose.top = tpg->compose.left = 0;
+	tpg->compose.width = width;
+	tpg->compose.height = tpg->buf_height;
+	for (p = 0; p < tpg->planes; p++)
+		tpg->bytesperline[p] = width * tpg->twopixelsize[p] / 2;
+	tpg->recalc_square_border = true;
+}
+
+static enum tpg_color tpg_get_textbg_color(struct tpg_data *tpg)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_BLACK:
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_CSC_COLORBAR:
+		return TPG_COLOR_CSC_BLACK;
+	default:
+		return TPG_COLOR_100_BLACK;
+	}
+}
+
+static enum tpg_color tpg_get_textfg_color(struct tpg_data *tpg)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_75_COLORBAR:
+	case TPG_PAT_CSC_COLORBAR:
+		return TPG_COLOR_CSC_WHITE;
+	case TPG_PAT_BLACK:
+		return TPG_COLOR_100_BLACK;
+	default:
+		return TPG_COLOR_100_WHITE;
+	}
+}
+
+static u16 color_to_y(struct tpg_data *tpg, int r, int g, int b)
+{
+	switch (tpg->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		return ((16829 * r + 33039 * g + 6416 * b + 16 * 32768) >> 16) + (16 << 4);
+	case V4L2_COLORSPACE_SMPTE240M:
+		return ((11932 * r + 39455 * g + 4897 * b + 16 * 32768) >> 16) + (16 << 4);
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_SRGB:
+	default:
+		return ((11966 * r + 40254 * g + 4064 * b + 16 * 32768) >> 16) + (16 << 4);
+	}
+}
+
+static u16 color_to_cb(struct tpg_data *tpg, int r, int g, int b)
+{
+	switch (tpg->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		return ((-9714 * r - 19070 * g + 28784 * b + 16 * 32768) >> 16) + (128 << 4);
+	case V4L2_COLORSPACE_SMPTE240M:
+		return ((-6684 * r - 22100 * g + 28784 * b + 16 * 32768) >> 16) + (128 << 4);
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_SRGB:
+	default:
+		return ((-6596 * r - 22189 * g + 28784 * b + 16 * 32768) >> 16) + (128 << 4);
+	}
+}
+
+static u16 color_to_cr(struct tpg_data *tpg, int r, int g, int b)
+{
+	switch (tpg->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		return ((28784 * r - 24103 * g - 4681 * b + 16 * 32768) >> 16) + (128 << 4);
+	case V4L2_COLORSPACE_SMPTE240M:
+		return ((28784 * r - 25606 * g - 3178 * b + 16 * 32768) >> 16) + (128 << 4);
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_SRGB:
+	default:
+		return ((28784 * r - 26145 * g - 2639 * b + 16 * 32768) >> 16) + (128 << 4);
+	}
+}
+
+static u16 ycbcr_to_r(struct tpg_data *tpg, int y, int cb, int cr)
+{
+	int r;
+
+	y -= 16 << 4;
+	cb -= 128 << 4;
+	cr -= 128 << 4;
+	switch (tpg->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		r = 4769 * y + 6537 * cr;
+		break;
+	case V4L2_COLORSPACE_SMPTE240M:
+		r = 4769 * y + 7376 * cr;
+		break;
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_SRGB:
+	default:
+		r = 4769 * y + 7343 * cr;
+		break;
+	}
+	return clamp(r >> 12, 0, 0xff0);
+}
+
+static u16 ycbcr_to_g(struct tpg_data *tpg, int y, int cb, int cr)
+{
+	int g;
+
+	y -= 16 << 4;
+	cb -= 128 << 4;
+	cr -= 128 << 4;
+	switch (tpg->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		g = 4769 * y - 1605 * cb - 3330 * cr;
+		break;
+	case V4L2_COLORSPACE_SMPTE240M:
+		g = 4769 * y - 1055 * cb - 2341 * cr;
+		break;
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_SRGB:
+	default:
+		g = 4769 * y - 873 * cb - 2183 * cr;
+		break;
+	}
+	return clamp(g >> 12, 0, 0xff0);
+}
+
+static u16 ycbcr_to_b(struct tpg_data *tpg, int y, int cb, int cr)
+{
+	int b;
+
+	y -= 16 << 4;
+	cb -= 128 << 4;
+	cr -= 128 << 4;
+	switch (tpg->colorspace) {
+	case V4L2_COLORSPACE_SMPTE170M:
+	case V4L2_COLORSPACE_470_SYSTEM_M:
+	case V4L2_COLORSPACE_470_SYSTEM_BG:
+		b = 4769 * y + 7343 * cb;
+		break;
+	case V4L2_COLORSPACE_SMPTE240M:
+		b = 4769 * y + 8552 * cb;
+		break;
+	case V4L2_COLORSPACE_REC709:
+	case V4L2_COLORSPACE_SRGB:
+	default:
+		b = 4769 * y + 8652 * cb;
+		break;
+	}
+	return clamp(b >> 12, 0, 0xff0);
+}
+
+/* precalculate color bar values to speed up rendering */
+static void precalculate_color(struct tpg_data *tpg, int k)
+{
+	int col = k;
+	int r = tpg_colors[col].r;
+	int g = tpg_colors[col].g;
+	int b = tpg_colors[col].b;
+
+	if (k == TPG_COLOR_TEXTBG) {
+		col = tpg_get_textbg_color(tpg);
+
+		r = tpg_colors[col].r;
+		g = tpg_colors[col].g;
+		b = tpg_colors[col].b;
+	} else if (k == TPG_COLOR_TEXTFG) {
+		col = tpg_get_textfg_color(tpg);
+
+		r = tpg_colors[col].r;
+		g = tpg_colors[col].g;
+		b = tpg_colors[col].b;
+	} else if (tpg->pattern == TPG_PAT_NOISE) {
+		r = g = b = prandom_u32_max(256);
+	} else if (k == TPG_COLOR_RANDOM) {
+		r = g = b = tpg->qual_offset + prandom_u32_max(196);
+	} else if (k >= TPG_COLOR_RAMP) {
+		r = g = b = k - TPG_COLOR_RAMP;
+	}
+
+	if (tpg->pattern == TPG_PAT_CSC_COLORBAR && col <= TPG_COLOR_CSC_BLACK) {
+		r = tpg_csc_colors[tpg->colorspace][col].r;
+		g = tpg_csc_colors[tpg->colorspace][col].g;
+		b = tpg_csc_colors[tpg->colorspace][col].b;
+	} else {
+		r <<= 4;
+		g <<= 4;
+		b <<= 4;
+	}
+	if (tpg->qual == TPG_QUAL_GRAY)
+		r = g = b = color_to_y(tpg, r, g, b);
+
+	/*
+	 * The assumption is that the RGB output is always full range,
+	 * so only if the rgb_range overrides the 'real' rgb range do
+	 * we need to convert the RGB values.
+	 *
+	 * Currently there is no way of signalling to userspace if you
+	 * are actually giving it limited range RGB (or full range
+	 * YUV for that matter).
+	 *
+	 * Remember that r, g and b are still in the 0 - 0xff0 range.
+	 */
+	if (tpg->real_rgb_range == V4L2_DV_RGB_RANGE_LIMITED &&
+	    tpg->rgb_range == V4L2_DV_RGB_RANGE_FULL) {
+		/*
+		 * Convert from full range (which is what r, g and b are)
+		 * to limited range (which is the 'real' RGB range), which
+		 * is then interpreted as full range.
+		 */
+		r = (r * 219) / 255 + (16 << 4);
+		g = (g * 219) / 255 + (16 << 4);
+		b = (b * 219) / 255 + (16 << 4);
+	} else if (tpg->real_rgb_range != V4L2_DV_RGB_RANGE_LIMITED &&
+		   tpg->rgb_range == V4L2_DV_RGB_RANGE_LIMITED) {
+		/*
+		 * Clamp r, g and b to the limited range and convert to full
+		 * range since that's what we deliver.
+		 */
+		r = clamp(r, 16 << 4, 235 << 4);
+		g = clamp(g, 16 << 4, 235 << 4);
+		b = clamp(b, 16 << 4, 235 << 4);
+		r = (r - (16 << 4)) * 255 / 219;
+		g = (g - (16 << 4)) * 255 / 219;
+		b = (b - (16 << 4)) * 255 / 219;
+	}
+
+	if (tpg->brightness != 128 || tpg->contrast != 128 ||
+	    tpg->saturation != 128 || tpg->hue) {
+		/* Implement these operations */
+
+		/* First convert to YCbCr */
+		int y = color_to_y(tpg, r, g, b);	/* Luma */
+		int cb = color_to_cb(tpg, r, g, b);	/* Cb */
+		int cr = color_to_cr(tpg, r, g, b);	/* Cr */
+		int tmp_cb, tmp_cr;
+
+		y = (16 << 4) + ((y - (16 << 4)) * tpg->contrast) / 128;
+		y += (tpg->brightness << 4) - (128 << 4);
+
+		cb -= 128 << 4;
+		cr -= 128 << 4;
+		tmp_cb = (cb * cos(128 + tpg->hue)) / 127 + (cr * sin[128 + tpg->hue]) / 127;
+		tmp_cr = (cr * cos(128 + tpg->hue)) / 127 - (cb * sin[128 + tpg->hue]) / 127;
+
+		cb = (128 << 4) + (tmp_cb * tpg->contrast * tpg->saturation) / (128 * 128);
+		cr = (128 << 4) + (tmp_cr * tpg->contrast * tpg->saturation) / (128 * 128);
+		if (tpg->is_yuv) {
+			tpg->colors[k][0] = clamp(y >> 4, 1, 254);
+			tpg->colors[k][1] = clamp(cb >> 4, 1, 254);
+			tpg->colors[k][2] = clamp(cr >> 4, 1, 254);
+			return;
+		}
+		r = ycbcr_to_r(tpg, y, cb, cr);
+		g = ycbcr_to_g(tpg, y, cb, cr);
+		b = ycbcr_to_b(tpg, y, cb, cr);
+	}
+
+	if (tpg->is_yuv) {
+		/* Convert to YCbCr */
+		u16 y = color_to_y(tpg, r, g, b);	/* Luma */
+		u16 cb = color_to_cb(tpg, r, g, b);	/* Cb */
+		u16 cr = color_to_cr(tpg, r, g, b);	/* Cr */
+
+		tpg->colors[k][0] = clamp(y >> 4, 1, 254);
+		tpg->colors[k][1] = clamp(cb >> 4, 1, 254);
+		tpg->colors[k][2] = clamp(cr >> 4, 1, 254);
+	} else {
+		switch (tpg->fourcc) {
+		case V4L2_PIX_FMT_RGB565:
+		case V4L2_PIX_FMT_RGB565X:
+			r >>= 7;
+			g >>= 6;
+			b >>= 7;
+			break;
+		case V4L2_PIX_FMT_RGB555:
+		case V4L2_PIX_FMT_XRGB555:
+		case V4L2_PIX_FMT_ARGB555:
+		case V4L2_PIX_FMT_RGB555X:
+			r >>= 7;
+			g >>= 7;
+			b >>= 7;
+			break;
+		default:
+			r >>= 4;
+			g >>= 4;
+			b >>= 4;
+			break;
+		}
+
+		tpg->colors[k][0] = r;
+		tpg->colors[k][1] = g;
+		tpg->colors[k][2] = b;
+	}
+}
+
+static void tpg_precalculate_colors(struct tpg_data *tpg)
+{
+	int k;
+
+	for (k = 0; k < TPG_COLOR_MAX; k++)
+		precalculate_color(tpg, k);
+}
+
+/* 'odd' is true for pixels 1, 3, 5, etc. and false for pixels 0, 2, 4, etc. */
+static void gen_twopix(struct tpg_data *tpg,
+		u8 buf[TPG_MAX_PLANES][8], int color, bool odd)
+{
+	unsigned offset = odd * tpg->twopixelsize[0] / 2;
+	u8 alpha = tpg->alpha_component;
+	u8 r_y, g_u, b_v;
+
+	if (tpg->alpha_red_only && color != TPG_COLOR_CSC_RED &&
+				   color != TPG_COLOR_100_RED &&
+				   color != TPG_COLOR_75_RED)
+		alpha = 0;
+	if (color == TPG_COLOR_RANDOM)
+		precalculate_color(tpg, color);
+	r_y = tpg->colors[color][0]; /* R or precalculated Y */
+	g_u = tpg->colors[color][1]; /* G or precalculated U */
+	b_v = tpg->colors[color][2]; /* B or precalculated V */
+
+	switch (tpg->fourcc) {
+	case V4L2_PIX_FMT_NV16M:
+		buf[0][offset] = r_y;
+		buf[1][offset] = odd ? b_v : g_u;
+		break;
+	case V4L2_PIX_FMT_NV61M:
+		buf[0][offset] = r_y;
+		buf[1][offset] = odd ? g_u : b_v;
+		break;
+
+	case V4L2_PIX_FMT_YUYV:
+		buf[0][offset] = r_y;
+		buf[0][offset + 1] = odd ? b_v : g_u;
+		break;
+	case V4L2_PIX_FMT_UYVY:
+		buf[0][offset] = odd ? b_v : g_u;
+		buf[0][offset + 1] = r_y;
+		break;
+	case V4L2_PIX_FMT_YVYU:
+		buf[0][offset] = r_y;
+		buf[0][offset + 1] = odd ? g_u : b_v;
+		break;
+	case V4L2_PIX_FMT_VYUY:
+		buf[0][offset] = odd ? g_u : b_v;
+		buf[0][offset + 1] = r_y;
+		break;
+	case V4L2_PIX_FMT_RGB565:
+		buf[0][offset] = (g_u << 5) | b_v;
+		buf[0][offset + 1] = (r_y << 3) | (g_u >> 3);
+		break;
+	case V4L2_PIX_FMT_RGB565X:
+		buf[0][offset] = (r_y << 3) | (g_u >> 3);
+		buf[0][offset + 1] = (g_u << 5) | b_v;
+		break;
+	case V4L2_PIX_FMT_RGB555:
+	case V4L2_PIX_FMT_XRGB555:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_ARGB555:
+		buf[0][offset] = (g_u << 5) | b_v;
+		buf[0][offset + 1] = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
+		break;
+	case V4L2_PIX_FMT_RGB555X:
+		buf[0][offset] = (alpha & 0x80) | (r_y << 2) | (g_u >> 3);
+		buf[0][offset + 1] = (g_u << 5) | b_v;
+		break;
+	case V4L2_PIX_FMT_RGB24:
+		buf[0][offset] = r_y;
+		buf[0][offset + 1] = g_u;
+		buf[0][offset + 2] = b_v;
+		break;
+	case V4L2_PIX_FMT_BGR24:
+		buf[0][offset] = b_v;
+		buf[0][offset + 1] = g_u;
+		buf[0][offset + 2] = r_y;
+		break;
+	case V4L2_PIX_FMT_RGB32:
+	case V4L2_PIX_FMT_XRGB32:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_ARGB32:
+		buf[0][offset] = alpha;
+		buf[0][offset + 1] = r_y;
+		buf[0][offset + 2] = g_u;
+		buf[0][offset + 3] = b_v;
+		break;
+	case V4L2_PIX_FMT_BGR32:
+	case V4L2_PIX_FMT_XBGR32:
+		alpha = 0;
+		/* fall through */
+	case V4L2_PIX_FMT_ABGR32:
+		buf[0][offset] = b_v;
+		buf[0][offset + 1] = g_u;
+		buf[0][offset + 2] = r_y;
+		buf[0][offset + 3] = alpha;
+		break;
+	}
+}
+
+/* Return how many pattern lines are used by the current pattern. */
+static unsigned tpg_get_pat_lines(struct tpg_data *tpg)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_CHECKERS_16X16:
+	case TPG_PAT_CHECKERS_1X1:
+	case TPG_PAT_ALTERNATING_HLINES:
+	case TPG_PAT_CROSS_1_PIXEL:
+	case TPG_PAT_CROSS_2_PIXELS:
+	case TPG_PAT_CROSS_10_PIXELS:
+		return 2;
+	case TPG_PAT_100_COLORSQUARES:
+	case TPG_PAT_100_HCOLORBAR:
+		return 8;
+	default:
+		return 1;
+	}
+}
+
+/* Which pattern line should be used for the given frame line. */
+static unsigned tpg_get_pat_line(struct tpg_data *tpg, unsigned line)
+{
+	switch (tpg->pattern) {
+	case TPG_PAT_CHECKERS_16X16:
+		return (line >> 4) & 1;
+	case TPG_PAT_CHECKERS_1X1:
+	case TPG_PAT_ALTERNATING_HLINES:
+		return line & 1;
+	case TPG_PAT_100_COLORSQUARES:
+	case TPG_PAT_100_HCOLORBAR:
+		return (line * 8) / tpg->src_height;
+	case TPG_PAT_CROSS_1_PIXEL:
+		return line == tpg->src_height / 2;
+	case TPG_PAT_CROSS_2_PIXELS:
+		return (line + 1) / 2 == tpg->src_height / 4;
+	case TPG_PAT_CROSS_10_PIXELS:
+		return (line + 10) / 20 == tpg->src_height / 40;
+	default:
+		return 0;
+	}
+}
+
+/*
+ * Which color should be used for the given pattern line and X coordinate.
+ * Note: x is in the range 0 to 2 * tpg->src_width.
+ */
+static enum tpg_color tpg_get_color(struct tpg_data *tpg, unsigned pat_line, unsigned x)
+{
+	/* Maximum number of bars are TPG_COLOR_MAX - otherwise, the input print code
+	   should be modified */
+	static const enum tpg_color bars[3][8] = {
+		/* Standard ITU-R 75% color bar sequence */
+		{ TPG_COLOR_CSC_WHITE,   TPG_COLOR_75_YELLOW,
+		  TPG_COLOR_75_CYAN,     TPG_COLOR_75_GREEN,
+		  TPG_COLOR_75_MAGENTA,  TPG_COLOR_75_RED,
+		  TPG_COLOR_75_BLUE,     TPG_COLOR_100_BLACK, },
+		/* Standard ITU-R 100% color bar sequence */
+		{ TPG_COLOR_100_WHITE,   TPG_COLOR_100_YELLOW,
+		  TPG_COLOR_100_CYAN,    TPG_COLOR_100_GREEN,
+		  TPG_COLOR_100_MAGENTA, TPG_COLOR_100_RED,
+		  TPG_COLOR_100_BLUE,    TPG_COLOR_100_BLACK, },
+		/* Color bar sequence suitable to test CSC */
+		{ TPG_COLOR_CSC_WHITE,   TPG_COLOR_CSC_YELLOW,
+		  TPG_COLOR_CSC_CYAN,    TPG_COLOR_CSC_GREEN,
+		  TPG_COLOR_CSC_MAGENTA, TPG_COLOR_CSC_RED,
+		  TPG_COLOR_CSC_BLUE,    TPG_COLOR_CSC_BLACK, },
+	};
+
+	switch (tpg->pattern) {
+	case TPG_PAT_75_COLORBAR:
+	case TPG_PAT_100_COLORBAR:
+	case TPG_PAT_CSC_COLORBAR:
+		return bars[tpg->pattern][((x * 8) / tpg->src_width) % 8];
+	case TPG_PAT_100_COLORSQUARES:
+		return bars[1][(pat_line + (x * 8) / tpg->src_width) % 8];
+	case TPG_PAT_100_HCOLORBAR:
+		return bars[1][pat_line];
+	case TPG_PAT_BLACK:
+		return TPG_COLOR_100_BLACK;
+	case TPG_PAT_WHITE:
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_RED:
+		return TPG_COLOR_100_RED;
+	case TPG_PAT_GREEN:
+		return TPG_COLOR_100_GREEN;
+	case TPG_PAT_BLUE:
+		return TPG_COLOR_100_BLUE;
+	case TPG_PAT_CHECKERS_16X16:
+		return (((x >> 4) & 1) ^ (pat_line & 1)) ?
+			TPG_COLOR_100_BLACK : TPG_COLOR_100_WHITE;
+	case TPG_PAT_CHECKERS_1X1:
+		return ((x & 1) ^ (pat_line & 1)) ?
+			TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
+	case TPG_PAT_ALTERNATING_HLINES:
+		return pat_line ? TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
+	case TPG_PAT_ALTERNATING_VLINES:
+		return (x & 1) ? TPG_COLOR_100_WHITE : TPG_COLOR_100_BLACK;
+	case TPG_PAT_CROSS_1_PIXEL:
+		if (pat_line || (x % tpg->src_width) == tpg->src_width / 2)
+			return TPG_COLOR_100_BLACK;
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_CROSS_2_PIXELS:
+		if (pat_line || ((x % tpg->src_width) + 1) / 2 == tpg->src_width / 4)
+			return TPG_COLOR_100_BLACK;
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_CROSS_10_PIXELS:
+		if (pat_line || ((x % tpg->src_width) + 10) / 20 == tpg->src_width / 40)
+			return TPG_COLOR_100_BLACK;
+		return TPG_COLOR_100_WHITE;
+	case TPG_PAT_GRAY_RAMP:
+		return TPG_COLOR_RAMP + ((x % tpg->src_width) * 256) / tpg->src_width;
+	default:
+		return TPG_COLOR_100_RED;
+	}
+}
+
+/*
+ * Given the pixel aspect ratio and video aspect ratio calculate the
+ * coordinates of a centered square and the coordinates of the border of
+ * the active video area. The coordinates are relative to the source
+ * frame rectangle.
+ */
+static void tpg_calculate_square_border(struct tpg_data *tpg)
+{
+	unsigned w = tpg->src_width;
+	unsigned h = tpg->src_height;
+	unsigned sq_w, sq_h;
+
+	sq_w = (w * 2 / 5) & ~1;
+	if (((w - sq_w) / 2) & 1)
+		sq_w += 2;
+	sq_h = sq_w;
+	tpg->square.width = sq_w;
+	if (tpg->vid_aspect == TPG_VIDEO_ASPECT_16X9_ANAMORPHIC) {
+		unsigned ana_sq_w = (sq_w / 4) * 3;
+
+		if (((w - ana_sq_w) / 2) & 1)
+			ana_sq_w += 2;
+		tpg->square.width = ana_sq_w;
+	}
+	tpg->square.left = (w - tpg->square.width) / 2;
+	if (tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC)
+		sq_h = sq_w * 10 / 11;
+	else if (tpg->pix_aspect == TPG_PIXEL_ASPECT_PAL)
+		sq_h = sq_w * 59 / 54;
+	tpg->square.height = sq_h;
+	tpg->square.top = (h - sq_h) / 2;
+	tpg->border.left = 0;
+	tpg->border.width = w;
+	tpg->border.top = 0;
+	tpg->border.height = h;
+	switch (tpg->vid_aspect) {
+	case TPG_VIDEO_ASPECT_4X3:
+		if (tpg->pix_aspect)
+			return;
+		if (3 * w >= 4 * h) {
+			tpg->border.width = ((4 * h) / 3) & ~1;
+			if (((w - tpg->border.width) / 2) & ~1)
+				tpg->border.width -= 2;
+			tpg->border.left = (w - tpg->border.width) / 2;
+			break;
+		}
+		tpg->border.height = ((3 * w) / 4) & ~1;
+		tpg->border.top = (h - tpg->border.height) / 2;
+		break;
+	case TPG_VIDEO_ASPECT_14X9_CENTRE:
+		if (tpg->pix_aspect) {
+			tpg->border.height = tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC ? 420 : 506;
+			tpg->border.top = (h - tpg->border.height) / 2;
+			break;
+		}
+		if (9 * w >= 14 * h) {
+			tpg->border.width = ((14 * h) / 9) & ~1;
+			if (((w - tpg->border.width) / 2) & ~1)
+				tpg->border.width -= 2;
+			tpg->border.left = (w - tpg->border.width) / 2;
+			break;
+		}
+		tpg->border.height = ((9 * w) / 14) & ~1;
+		tpg->border.top = (h - tpg->border.height) / 2;
+		break;
+	case TPG_VIDEO_ASPECT_16X9_CENTRE:
+		if (tpg->pix_aspect) {
+			tpg->border.height = tpg->pix_aspect == TPG_PIXEL_ASPECT_NTSC ? 368 : 442;
+			tpg->border.top = (h - tpg->border.height) / 2;
+			break;
+		}
+		if (9 * w >= 16 * h) {
+			tpg->border.width = ((16 * h) / 9) & ~1;
+			if (((w - tpg->border.width) / 2) & ~1)
+				tpg->border.width -= 2;
+			tpg->border.left = (w - tpg->border.width) / 2;
+			break;
+		}
+		tpg->border.height = ((9 * w) / 16) & ~1;
+		tpg->border.top = (h - tpg->border.height) / 2;
+		break;
+	default:
+		break;
+	}
+}
+
+static void tpg_precalculate_line(struct tpg_data *tpg)
+{
+	enum tpg_color contrast;
+	unsigned pat;
+	unsigned p;
+	unsigned x;
+
+	switch (tpg->pattern) {
+	case TPG_PAT_GREEN:
+		contrast = TPG_COLOR_100_RED;
+		break;
+	case TPG_PAT_CSC_COLORBAR:
+		contrast = TPG_COLOR_CSC_GREEN;
+		break;
+	default:
+		contrast = TPG_COLOR_100_GREEN;
+		break;
+	}
+
+	for (pat = 0; pat < tpg_get_pat_lines(tpg); pat++) {
+		/* Coarse scaling with Bresenham */
+		unsigned int_part = tpg->src_width / tpg->scaled_width;
+		unsigned fract_part = tpg->src_width % tpg->scaled_width;
+		unsigned src_x = 0;
+		unsigned error = 0;
+
+		for (x = 0; x < tpg->scaled_width * 2; x += 2) {
+			unsigned real_x = src_x;
+			enum tpg_color color1, color2;
+			u8 pix[TPG_MAX_PLANES][8];
+
+			real_x = tpg->hflip ? tpg->src_width * 2 - real_x - 2 : real_x;
+			color1 = tpg_get_color(tpg, pat, real_x);
+
+			src_x += int_part;
+			error += fract_part;
+			if (error >= tpg->scaled_width) {
+				error -= tpg->scaled_width;
+				src_x++;
+			}
+
+			real_x = src_x;
+			real_x = tpg->hflip ? tpg->src_width * 2 - real_x - 2 : real_x;
+			color2 = tpg_get_color(tpg, pat, real_x);
+
+			src_x += int_part;
+			error += fract_part;
+			if (error >= tpg->scaled_width) {
+				error -= tpg->scaled_width;
+				src_x++;
+			}
+
+			gen_twopix(tpg, pix, tpg->hflip ? color2 : color1, 0);
+			gen_twopix(tpg, pix, tpg->hflip ? color1 : color2, 1);
+			for (p = 0; p < tpg->planes; p++) {
+				unsigned twopixsize = tpg->twopixelsize[p];
+				u8 *pos = tpg->lines[pat][p] + x * twopixsize / 2;
+
+				memcpy(pos, pix[p], twopixsize);
+			}
+		}
+	}
+	for (x = 0; x < tpg->scaled_width; x += 2) {
+		u8 pix[TPG_MAX_PLANES][8];
+
+		gen_twopix(tpg, pix, contrast, 0);
+		gen_twopix(tpg, pix, contrast, 1);
+		for (p = 0; p < tpg->planes; p++) {
+			unsigned twopixsize = tpg->twopixelsize[p];
+			u8 *pos = tpg->contrast_line[p] + x * twopixsize / 2;
+
+			memcpy(pos, pix[p], twopixsize);
+		}
+	}
+	for (x = 0; x < tpg->scaled_width; x += 2) {
+		u8 pix[TPG_MAX_PLANES][8];
+
+		gen_twopix(tpg, pix, TPG_COLOR_100_BLACK, 0);
+		gen_twopix(tpg, pix, TPG_COLOR_100_BLACK, 1);
+		for (p = 0; p < tpg->planes; p++) {
+			unsigned twopixsize = tpg->twopixelsize[p];
+			u8 *pos = tpg->black_line[p] + x * twopixsize / 2;
+
+			memcpy(pos, pix[p], twopixsize);
+		}
+	}
+	for (x = 0; x < tpg->scaled_width * 2; x += 2) {
+		u8 pix[TPG_MAX_PLANES][8];
+
+		gen_twopix(tpg, pix, TPG_COLOR_RANDOM, 0);
+		gen_twopix(tpg, pix, TPG_COLOR_RANDOM, 1);
+		for (p = 0; p < tpg->planes; p++) {
+			unsigned twopixsize = tpg->twopixelsize[p];
+			u8 *pos = tpg->random_line[p] + x * twopixsize / 2;
+
+			memcpy(pos, pix[p], twopixsize);
+		}
+	}
+	gen_twopix(tpg, tpg->textbg, TPG_COLOR_TEXTBG, 0);
+	gen_twopix(tpg, tpg->textbg, TPG_COLOR_TEXTBG, 1);
+	gen_twopix(tpg, tpg->textfg, TPG_COLOR_TEXTFG, 0);
+	gen_twopix(tpg, tpg->textfg, TPG_COLOR_TEXTFG, 1);
+}
+
+/* need this to do rgb24 rendering */
+typedef struct { u16 __; u8 _; } __packed x24;
+
+void tpg_gen_text(struct tpg_data *tpg, u8 *basep[TPG_MAX_PLANES][2],
+		int y, int x, char *text)
+{
+	int line;
+	unsigned step = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
+	unsigned div = step;
+	unsigned first = 0;
+	unsigned len = strlen(text);
+	unsigned p;
+
+	if (font8x16 == NULL || basep == NULL)
+		return;
+
+	/* Checks if it is possible to show string */
+	if (y + 16 >= tpg->compose.height || x + 8 >= tpg->compose.width)
+		return;
+
+	if (len > (tpg->compose.width - x) / 8)
+		len = (tpg->compose.width - x) / 8;
+	if (tpg->vflip)
+		y = tpg->compose.height - y - 16;
+	if (tpg->hflip)
+		x = tpg->compose.width - x - 8;
+	y += tpg->compose.top;
+	x += tpg->compose.left;
+	if (tpg->field == V4L2_FIELD_BOTTOM)
+		first = 1;
+	else if (tpg->field == V4L2_FIELD_SEQ_TB || tpg->field == V4L2_FIELD_SEQ_BT)
+		div = 2;
+
+	for (p = 0; p < tpg->planes; p++) {
+		/* Print stream time */
+#define PRINTSTR(PIXTYPE) do {	\
+	PIXTYPE fg;	\
+	PIXTYPE bg;	\
+	memcpy(&fg, tpg->textfg[p], sizeof(PIXTYPE));	\
+	memcpy(&bg, tpg->textbg[p], sizeof(PIXTYPE));	\
+	\
+	for (line = first; line < 16; line += step) {	\
+		int l = tpg->vflip ? 15 - line : line; \
+		PIXTYPE *pos = (PIXTYPE *)(basep[p][line & 1] + \
+			       ((y * step + l) / div) * tpg->bytesperline[p] + \
+			       x * sizeof(PIXTYPE));	\
+		unsigned s;	\
+	\
+		for (s = 0; s < len; s++) {	\
+			u8 chr = font8x16[text[s] * 16 + line];	\
+	\
+			if (tpg->hflip) { \
+				pos[7] = (chr & (0x01 << 7) ? fg : bg);	\
+				pos[6] = (chr & (0x01 << 6) ? fg : bg);	\
+				pos[5] = (chr & (0x01 << 5) ? fg : bg);	\
+				pos[4] = (chr & (0x01 << 4) ? fg : bg);	\
+				pos[3] = (chr & (0x01 << 3) ? fg : bg);	\
+				pos[2] = (chr & (0x01 << 2) ? fg : bg);	\
+				pos[1] = (chr & (0x01 << 1) ? fg : bg);	\
+				pos[0] = (chr & (0x01 << 0) ? fg : bg);	\
+			} else { \
+				pos[0] = (chr & (0x01 << 7) ? fg : bg);	\
+				pos[1] = (chr & (0x01 << 6) ? fg : bg);	\
+				pos[2] = (chr & (0x01 << 5) ? fg : bg);	\
+				pos[3] = (chr & (0x01 << 4) ? fg : bg);	\
+				pos[4] = (chr & (0x01 << 3) ? fg : bg);	\
+				pos[5] = (chr & (0x01 << 2) ? fg : bg);	\
+				pos[6] = (chr & (0x01 << 1) ? fg : bg);	\
+				pos[7] = (chr & (0x01 << 0) ? fg : bg);	\
+			} \
+	\
+			pos += tpg->hflip ? -8 : 8;	\
+		}	\
+	}	\
+} while (0)
+
+		switch (tpg->twopixelsize[p]) {
+		case 2:
+			PRINTSTR(u8); break;
+		case 4:
+			PRINTSTR(u16); break;
+		case 6:
+			PRINTSTR(x24); break;
+		case 8:
+			PRINTSTR(u32); break;
+		}
+	}
+}
+
+void tpg_update_mv_step(struct tpg_data *tpg)
+{
+	int factor = tpg->mv_hor_mode > TPG_MOVE_NONE ? -1 : 1;
+
+	if (tpg->hflip)
+		factor = -factor;
+	switch (tpg->mv_hor_mode) {
+	case TPG_MOVE_NEG_FAST:
+	case TPG_MOVE_POS_FAST:
+		tpg->mv_hor_step = ((tpg->src_width + 319) / 320) * 4;
+		break;
+	case TPG_MOVE_NEG:
+	case TPG_MOVE_POS:
+		tpg->mv_hor_step = ((tpg->src_width + 639) / 640) * 4;
+		break;
+	case TPG_MOVE_NEG_SLOW:
+	case TPG_MOVE_POS_SLOW:
+		tpg->mv_hor_step = 2;
+		break;
+	case TPG_MOVE_NONE:
+		tpg->mv_hor_step = 0;
+		break;
+	}
+	if (factor < 0)
+		tpg->mv_hor_step = tpg->src_width - tpg->mv_hor_step;
+
+	factor = tpg->mv_vert_mode > TPG_MOVE_NONE ? -1 : 1;
+	switch (tpg->mv_vert_mode) {
+	case TPG_MOVE_NEG_FAST:
+	case TPG_MOVE_POS_FAST:
+		tpg->mv_vert_step = ((tpg->src_width + 319) / 320) * 4;
+		break;
+	case TPG_MOVE_NEG:
+	case TPG_MOVE_POS:
+		tpg->mv_vert_step = ((tpg->src_width + 639) / 640) * 4;
+		break;
+	case TPG_MOVE_NEG_SLOW:
+	case TPG_MOVE_POS_SLOW:
+		tpg->mv_vert_step = 1;
+		break;
+	case TPG_MOVE_NONE:
+		tpg->mv_vert_step = 0;
+		break;
+	}
+	if (factor < 0)
+		tpg->mv_vert_step = tpg->src_height - tpg->mv_vert_step;
+}
+
+/* Map the line number relative to the crop rectangle to a frame line number */
+static unsigned tpg_calc_frameline(struct tpg_data *tpg, unsigned src_y,
+				    unsigned field)
+{
+	switch (field) {
+	case V4L2_FIELD_TOP:
+		return tpg->crop.top + src_y * 2;
+	case V4L2_FIELD_BOTTOM:
+		return tpg->crop.top + src_y * 2 + 1;
+	default:
+		return src_y + tpg->crop.top;
+	}
+}
+
+/*
+ * Map the line number relative to the compose rectangle to a destination
+ * buffer line number.
+ */
+static unsigned tpg_calc_buffer_line(struct tpg_data *tpg, unsigned y,
+				    unsigned field)
+{
+	y += tpg->compose.top;
+	switch (field) {
+	case V4L2_FIELD_SEQ_TB:
+		if (y & 1)
+			return tpg->buf_height / 2 + y / 2;
+		return y / 2;
+	case V4L2_FIELD_SEQ_BT:
+		if (y & 1)
+			return y / 2;
+		return tpg->buf_height / 2 + y / 2;
+	default:
+		return y;
+	}
+}
+
+static void tpg_recalc(struct tpg_data *tpg)
+{
+	if (tpg->recalc_colors) {
+		tpg->recalc_colors = false;
+		tpg->recalc_lines = true;
+		tpg_precalculate_colors(tpg);
+	}
+	if (tpg->recalc_square_border) {
+		tpg->recalc_square_border = false;
+		tpg_calculate_square_border(tpg);
+	}
+	if (tpg->recalc_lines) {
+		tpg->recalc_lines = false;
+		tpg_precalculate_line(tpg);
+	}
+}
+
+void tpg_calc_text_basep(struct tpg_data *tpg,
+		u8 *basep[TPG_MAX_PLANES][2], unsigned p, u8 *vbuf)
+{
+	unsigned stride = tpg->bytesperline[p];
+
+	tpg_recalc(tpg);
+
+	basep[p][0] = vbuf;
+	basep[p][1] = vbuf;
+	if (tpg->field == V4L2_FIELD_SEQ_TB)
+		basep[p][1] += tpg->buf_height * stride / 2;
+	else if (tpg->field == V4L2_FIELD_SEQ_BT)
+		basep[p][0] += tpg->buf_height * stride / 2;
+}
+
+void tpg_fillbuffer(struct tpg_data *tpg, v4l2_std_id std, unsigned p, u8 *vbuf)
+{
+	bool is_tv = std;
+	bool is_60hz = is_tv && (std & V4L2_STD_525_60);
+	unsigned mv_hor_old = tpg->mv_hor_count % tpg->src_width;
+	unsigned mv_hor_new = (tpg->mv_hor_count + tpg->mv_hor_step) % tpg->src_width;
+	unsigned mv_vert_old = tpg->mv_vert_count % tpg->src_height;
+	unsigned mv_vert_new = (tpg->mv_vert_count + tpg->mv_vert_step) % tpg->src_height;
+	unsigned wss_width;
+	unsigned f;
+	int hmax = (tpg->compose.height * tpg->perc_fill) / 100;
+	int h;
+	unsigned twopixsize = tpg->twopixelsize[p];
+	unsigned img_width = tpg->compose.width * twopixsize / 2;
+	unsigned line_offset;
+	unsigned left_pillar_width = 0;
+	unsigned right_pillar_start = img_width;
+	unsigned stride = tpg->bytesperline[p];
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(tpg->field) ? 2 : 1;
+	u8 *orig_vbuf = vbuf;
+
+	/* Coarse scaling with Bresenham */
+	unsigned int_part = (tpg->crop.height / factor) / tpg->compose.height;
+	unsigned fract_part = (tpg->crop.height / factor) % tpg->compose.height;
+	unsigned src_y = 0;
+	unsigned error = 0;
+
+	tpg_recalc(tpg);
+
+	mv_hor_old = (mv_hor_old * tpg->scaled_width / tpg->src_width) & ~1;
+	mv_hor_new = (mv_hor_new * tpg->scaled_width / tpg->src_width) & ~1;
+	wss_width = tpg->crop.left < tpg->src_width / 2 ?
+			tpg->src_width / 2 - tpg->crop.left : 0;
+	if (wss_width > tpg->crop.width)
+		wss_width = tpg->crop.width;
+	wss_width = wss_width * tpg->scaled_width / tpg->src_width;
+
+	vbuf += tpg->compose.left * twopixsize / 2;
+	line_offset = tpg->crop.left * tpg->scaled_width / tpg->src_width;
+	line_offset = (line_offset & ~1) * twopixsize / 2;
+	if (tpg->crop.left < tpg->border.left) {
+		left_pillar_width = tpg->border.left - tpg->crop.left;
+		if (left_pillar_width > tpg->crop.width)
+			left_pillar_width = tpg->crop.width;
+		left_pillar_width = (left_pillar_width * tpg->scaled_width) / tpg->src_width;
+		left_pillar_width = (left_pillar_width & ~1) * twopixsize / 2;
+	}
+	if (tpg->crop.left + tpg->crop.width > tpg->border.left + tpg->border.width) {
+		right_pillar_start = tpg->border.left + tpg->border.width - tpg->crop.left;
+		right_pillar_start = (right_pillar_start * tpg->scaled_width) / tpg->src_width;
+		right_pillar_start = (right_pillar_start & ~1) * twopixsize / 2;
+		if (right_pillar_start > img_width)
+			right_pillar_start = img_width;
+	}
+
+	f = tpg->field == (is_60hz ? V4L2_FIELD_TOP : V4L2_FIELD_BOTTOM);
+
+	for (h = 0; h < tpg->compose.height; h++) {
+		bool even;
+		bool fill_blank = false;
+		unsigned frame_line;
+		unsigned buf_line;
+		unsigned pat_line_old;
+		unsigned pat_line_new;
+		u8 *linestart_older;
+		u8 *linestart_newer;
+		u8 *linestart_top;
+		u8 *linestart_bottom;
+
+		frame_line = tpg_calc_frameline(tpg, src_y, tpg->field);
+		even = !(frame_line & 1);
+		buf_line = tpg_calc_buffer_line(tpg, h, tpg->field);
+		src_y += int_part;
+		error += fract_part;
+		if (error >= tpg->compose.height) {
+			error -= tpg->compose.height;
+			src_y++;
+		}
+
+		if (h >= hmax) {
+			if (hmax == tpg->compose.height)
+				continue;
+			if (!tpg->perc_fill_blank)
+				continue;
+			fill_blank = true;
+		}
+
+		if (tpg->vflip)
+			frame_line = tpg->src_height - frame_line - 1;
+
+		if (fill_blank) {
+			linestart_older = tpg->contrast_line[p];
+			linestart_newer = tpg->contrast_line[p];
+		} else if (tpg->qual != TPG_QUAL_NOISE &&
+			   (frame_line < tpg->border.top ||
+			    frame_line >= tpg->border.top + tpg->border.height)) {
+			linestart_older = tpg->black_line[p];
+			linestart_newer = tpg->black_line[p];
+		} else if (tpg->pattern == TPG_PAT_NOISE || tpg->qual == TPG_QUAL_NOISE) {
+			linestart_older = tpg->random_line[p] +
+					  twopixsize * prandom_u32_max(tpg->src_width / 2);
+			linestart_newer = tpg->random_line[p] +
+					  twopixsize * prandom_u32_max(tpg->src_width / 2);
+		} else {
+			pat_line_old = tpg_get_pat_line(tpg,
+						(frame_line + mv_vert_old) % tpg->src_height);
+			pat_line_new = tpg_get_pat_line(tpg,
+						(frame_line + mv_vert_new) % tpg->src_height);
+			linestart_older = tpg->lines[pat_line_old][p] +
+					  mv_hor_old * twopixsize / 2;
+			linestart_newer = tpg->lines[pat_line_new][p] +
+					  mv_hor_new * twopixsize / 2;
+			linestart_older += line_offset;
+			linestart_newer += line_offset;
+		}
+		if (is_60hz) {
+			linestart_top = linestart_newer;
+			linestart_bottom = linestart_older;
+		} else {
+			linestart_top = linestart_older;
+			linestart_bottom = linestart_newer;
+		}
+
+		switch (tpg->field) {
+		case V4L2_FIELD_INTERLACED:
+		case V4L2_FIELD_INTERLACED_TB:
+		case V4L2_FIELD_SEQ_TB:
+		case V4L2_FIELD_SEQ_BT:
+			if (even)
+				memcpy(vbuf + buf_line * stride, linestart_top, img_width);
+			else
+				memcpy(vbuf + buf_line * stride, linestart_bottom, img_width);
+			break;
+		case V4L2_FIELD_INTERLACED_BT:
+			if (even)
+				memcpy(vbuf + buf_line * stride, linestart_bottom, img_width);
+			else
+				memcpy(vbuf + buf_line * stride, linestart_top, img_width);
+			break;
+		case V4L2_FIELD_TOP:
+			memcpy(vbuf + buf_line * stride, linestart_top, img_width);
+			break;
+		case V4L2_FIELD_BOTTOM:
+			memcpy(vbuf + buf_line * stride, linestart_bottom, img_width);
+			break;
+		case V4L2_FIELD_NONE:
+		default:
+			memcpy(vbuf + buf_line * stride, linestart_older, img_width);
+			break;
+		}
+
+		if (is_tv && !is_60hz && frame_line == 0 && wss_width) {
+			/*
+			 * Replace the first half of the top line of a 50 Hz frame
+			 * with random data to simulate a WSS signal.
+			 */
+			u8 *wss = tpg->random_line[p] +
+				  twopixsize * prandom_u32_max(tpg->src_width / 2);
+
+			memcpy(vbuf + buf_line * stride, wss, wss_width * twopixsize / 2);
+		}
+	}
+
+	vbuf = orig_vbuf;
+	vbuf += tpg->compose.left * twopixsize / 2;
+	src_y = 0;
+	error = 0;
+	for (h = 0; h < tpg->compose.height; h++) {
+		unsigned frame_line = tpg_calc_frameline(tpg, src_y, tpg->field);
+		unsigned buf_line = tpg_calc_buffer_line(tpg, h, tpg->field);
+		const struct v4l2_rect *sq = &tpg->square;
+		const struct v4l2_rect *b = &tpg->border;
+		const struct v4l2_rect *c = &tpg->crop;
+
+		src_y += int_part;
+		error += fract_part;
+		if (error >= tpg->compose.height) {
+			error -= tpg->compose.height;
+			src_y++;
+		}
+
+		if (tpg->show_border && frame_line >= b->top &&
+		    frame_line < b->top + b->height) {
+			unsigned bottom = b->top + b->height - 1;
+			unsigned left = left_pillar_width;
+			unsigned right = right_pillar_start;
+
+			if (frame_line == b->top || frame_line == b->top + 1 ||
+			    frame_line == bottom || frame_line == bottom - 1) {
+				memcpy(vbuf + buf_line * stride + left, tpg->contrast_line[p],
+						right - left);
+			} else {
+				if (b->left >= c->left &&
+				    b->left < c->left + c->width)
+					memcpy(vbuf + buf_line * stride + left,
+						tpg->contrast_line[p], twopixsize);
+				if (b->left + b->width > c->left &&
+				    b->left + b->width <= c->left + c->width)
+					memcpy(vbuf + buf_line * stride + right - twopixsize,
+						tpg->contrast_line[p], twopixsize);
+			}
+		}
+		if (tpg->qual != TPG_QUAL_NOISE && frame_line >= b->top &&
+		    frame_line < b->top + b->height) {
+			memcpy(vbuf + buf_line * stride, tpg->black_line[p], left_pillar_width);
+			memcpy(vbuf + buf_line * stride + right_pillar_start, tpg->black_line[p],
+			       img_width - right_pillar_start);
+		}
+		if (tpg->show_square && frame_line >= sq->top &&
+		    frame_line < sq->top + sq->height &&
+		    sq->left < c->left + c->width &&
+		    sq->left + sq->width >= c->left) {
+			unsigned left = sq->left;
+			unsigned width = sq->width;
+
+			if (c->left > left) {
+				width -= c->left - left;
+				left = c->left;
+			}
+			if (c->left + c->width < left + width)
+				width -= left + width - c->left - c->width;
+			left -= c->left;
+			left = (left * tpg->scaled_width) / tpg->src_width;
+			left = (left & ~1) * twopixsize / 2;
+			width = (width * tpg->scaled_width) / tpg->src_width;
+			width = (width & ~1) * twopixsize / 2;
+			memcpy(vbuf + buf_line * stride + left, tpg->contrast_line[p], width);
+		}
+		if (tpg->insert_sav) {
+			unsigned offset = (tpg->compose.width / 6) * twopixsize;
+			u8 *p = vbuf + buf_line * stride + offset;
+			unsigned vact = 0, hact = 0;
+
+			p[0] = 0xff;
+			p[1] = 0;
+			p[2] = 0;
+			p[3] = 0x80 | (f << 6) | (vact << 5) | (hact << 4) |
+				((hact ^ vact) << 3) |
+				((hact ^ f) << 2) |
+				((f ^ vact) << 1) |
+				(hact ^ vact ^ f);
+		}
+		if (tpg->insert_eav) {
+			unsigned offset = (tpg->compose.width / 6) * 2 * twopixsize;
+			u8 *p = vbuf + buf_line * stride + offset;
+			unsigned vact = 0, hact = 1;
+
+			p[0] = 0xff;
+			p[1] = 0;
+			p[2] = 0;
+			p[3] = 0x80 | (f << 6) | (vact << 5) | (hact << 4) |
+				((hact ^ vact) << 3) |
+				((hact ^ f) << 2) |
+				((f ^ vact) << 1) |
+				(hact ^ vact ^ f);
+		}
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-tpg.h b/drivers/media/platform/vivid/vivid-tpg.h
new file mode 100644
index 000000000000..8ef3e52ba3be
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-tpg.h
@@ -0,0 +1,439 @@
+/*
+ * vivid-tpg.h - Test Pattern Generator
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_TPG_H_
+#define _VIVID_TPG_H_
+
+#include <linux/version.h>
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <linux/random.h>
+#include <linux/slab.h>
+#include <linux/vmalloc.h>
+#include <linux/videodev2.h>
+
+#include "vivid-tpg-colors.h"
+
+enum tpg_pattern {
+	TPG_PAT_75_COLORBAR,
+	TPG_PAT_100_COLORBAR,
+	TPG_PAT_CSC_COLORBAR,
+	TPG_PAT_100_HCOLORBAR,
+	TPG_PAT_100_COLORSQUARES,
+	TPG_PAT_BLACK,
+	TPG_PAT_WHITE,
+	TPG_PAT_RED,
+	TPG_PAT_GREEN,
+	TPG_PAT_BLUE,
+	TPG_PAT_CHECKERS_16X16,
+	TPG_PAT_CHECKERS_1X1,
+	TPG_PAT_ALTERNATING_HLINES,
+	TPG_PAT_ALTERNATING_VLINES,
+	TPG_PAT_CROSS_1_PIXEL,
+	TPG_PAT_CROSS_2_PIXELS,
+	TPG_PAT_CROSS_10_PIXELS,
+	TPG_PAT_GRAY_RAMP,
+
+	/* Must be the last pattern */
+	TPG_PAT_NOISE,
+};
+
+extern const char * const tpg_pattern_strings[];
+
+enum tpg_quality {
+	TPG_QUAL_COLOR,
+	TPG_QUAL_GRAY,
+	TPG_QUAL_NOISE
+};
+
+enum tpg_video_aspect {
+	TPG_VIDEO_ASPECT_IMAGE,
+	TPG_VIDEO_ASPECT_4X3,
+	TPG_VIDEO_ASPECT_14X9_CENTRE,
+	TPG_VIDEO_ASPECT_16X9_CENTRE,
+	TPG_VIDEO_ASPECT_16X9_ANAMORPHIC,
+};
+
+enum tpg_pixel_aspect {
+	TPG_PIXEL_ASPECT_SQUARE,
+	TPG_PIXEL_ASPECT_NTSC,
+	TPG_PIXEL_ASPECT_PAL,
+};
+
+enum tpg_move_mode {
+	TPG_MOVE_NEG_FAST,
+	TPG_MOVE_NEG,
+	TPG_MOVE_NEG_SLOW,
+	TPG_MOVE_NONE,
+	TPG_MOVE_POS_SLOW,
+	TPG_MOVE_POS,
+	TPG_MOVE_POS_FAST,
+};
+
+extern const char * const tpg_aspect_strings[];
+
+#define TPG_MAX_PLANES 2
+#define TPG_MAX_PAT_LINES 8
+
+struct tpg_data {
+	/* Source frame size */
+	unsigned			src_width, src_height;
+	/* Buffer height */
+	unsigned			buf_height;
+	/* Scaled output frame size */
+	unsigned			scaled_width;
+	u32				field;
+	/* crop coordinates are frame-based */
+	struct v4l2_rect		crop;
+	/* compose coordinates are format-based */
+	struct v4l2_rect		compose;
+	/* border and square coordinates are frame-based */
+	struct v4l2_rect		border;
+	struct v4l2_rect		square;
+
+	/* Color-related fields */
+	enum tpg_quality		qual;
+	unsigned			qual_offset;
+	u8				alpha_component;
+	bool				alpha_red_only;
+	u8				brightness;
+	u8				contrast;
+	u8				saturation;
+	s16				hue;
+	u32				fourcc;
+	bool				is_yuv;
+	u32				colorspace;
+	enum tpg_video_aspect		vid_aspect;
+	enum tpg_pixel_aspect		pix_aspect;
+	unsigned			rgb_range;
+	unsigned			real_rgb_range;
+	unsigned			planes;
+	/* Used to store the colors in native format, either RGB or YUV */
+	u8				colors[TPG_COLOR_MAX][3];
+	u8				textfg[TPG_MAX_PLANES][8], textbg[TPG_MAX_PLANES][8];
+	/* size in bytes for two pixels in each plane */
+	unsigned			twopixelsize[TPG_MAX_PLANES];
+	unsigned			bytesperline[TPG_MAX_PLANES];
+
+	/* Configuration */
+	enum tpg_pattern		pattern;
+	bool				hflip;
+	bool				vflip;
+	unsigned			perc_fill;
+	bool				perc_fill_blank;
+	bool				show_border;
+	bool				show_square;
+	bool				insert_sav;
+	bool				insert_eav;
+
+	/* Test pattern movement */
+	enum tpg_move_mode		mv_hor_mode;
+	int				mv_hor_count;
+	int				mv_hor_step;
+	enum tpg_move_mode		mv_vert_mode;
+	int				mv_vert_count;
+	int				mv_vert_step;
+
+	bool				recalc_colors;
+	bool				recalc_lines;
+	bool				recalc_square_border;
+
+	/* Used to store TPG_MAX_PAT_LINES lines, each with up to two planes */
+	unsigned			max_line_width;
+	u8				*lines[TPG_MAX_PAT_LINES][TPG_MAX_PLANES];
+	u8				*random_line[TPG_MAX_PLANES];
+	u8				*contrast_line[TPG_MAX_PLANES];
+	u8				*black_line[TPG_MAX_PLANES];
+};
+
+void tpg_init(struct tpg_data *tpg, unsigned w, unsigned h);
+int tpg_alloc(struct tpg_data *tpg, unsigned max_w);
+void tpg_free(struct tpg_data *tpg);
+void tpg_reset_source(struct tpg_data *tpg, unsigned width, unsigned height,
+		       u32 field);
+
+void tpg_set_font(const u8 *f);
+void tpg_gen_text(struct tpg_data *tpg,
+		u8 *basep[TPG_MAX_PLANES][2], int y, int x, char *text);
+void tpg_calc_text_basep(struct tpg_data *tpg,
+		u8 *basep[TPG_MAX_PLANES][2], unsigned p, u8 *vbuf);
+void tpg_fillbuffer(struct tpg_data *tpg, v4l2_std_id std, unsigned p, u8 *vbuf);
+bool tpg_s_fourcc(struct tpg_data *tpg, u32 fourcc);
+void tpg_s_crop_compose(struct tpg_data *tpg, const struct v4l2_rect *crop,
+		const struct v4l2_rect *compose);
+
+static inline void tpg_s_pattern(struct tpg_data *tpg, enum tpg_pattern pattern)
+{
+	if (tpg->pattern == pattern)
+		return;
+	tpg->pattern = pattern;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_quality(struct tpg_data *tpg,
+				    enum tpg_quality qual, unsigned qual_offset)
+{
+	if (tpg->qual == qual && tpg->qual_offset == qual_offset)
+		return;
+	tpg->qual = qual;
+	tpg->qual_offset = qual_offset;
+	tpg->recalc_colors = true;
+}
+
+static inline enum tpg_quality tpg_g_quality(const struct tpg_data *tpg)
+{
+	return tpg->qual;
+}
+
+static inline void tpg_s_alpha_component(struct tpg_data *tpg,
+					    u8 alpha_component)
+{
+	if (tpg->alpha_component == alpha_component)
+		return;
+	tpg->alpha_component = alpha_component;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_alpha_mode(struct tpg_data *tpg,
+					    bool red_only)
+{
+	if (tpg->alpha_red_only == red_only)
+		return;
+	tpg->alpha_red_only = red_only;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_brightness(struct tpg_data *tpg,
+					u8 brightness)
+{
+	if (tpg->brightness == brightness)
+		return;
+	tpg->brightness = brightness;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_contrast(struct tpg_data *tpg,
+					u8 contrast)
+{
+	if (tpg->contrast == contrast)
+		return;
+	tpg->contrast = contrast;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_saturation(struct tpg_data *tpg,
+					u8 saturation)
+{
+	if (tpg->saturation == saturation)
+		return;
+	tpg->saturation = saturation;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_hue(struct tpg_data *tpg,
+					s16 hue)
+{
+	if (tpg->hue == hue)
+		return;
+	tpg->hue = hue;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_rgb_range(struct tpg_data *tpg,
+					unsigned rgb_range)
+{
+	if (tpg->rgb_range == rgb_range)
+		return;
+	tpg->rgb_range = rgb_range;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_real_rgb_range(struct tpg_data *tpg,
+					unsigned rgb_range)
+{
+	if (tpg->real_rgb_range == rgb_range)
+		return;
+	tpg->real_rgb_range = rgb_range;
+	tpg->recalc_colors = true;
+}
+
+static inline void tpg_s_colorspace(struct tpg_data *tpg, u32 colorspace)
+{
+	if (tpg->colorspace == colorspace)
+		return;
+	tpg->colorspace = colorspace;
+	tpg->recalc_colors = true;
+}
+
+static inline u32 tpg_g_colorspace(const struct tpg_data *tpg)
+{
+	return tpg->colorspace;
+}
+
+static inline unsigned tpg_g_planes(const struct tpg_data *tpg)
+{
+	return tpg->planes;
+}
+
+static inline unsigned tpg_g_twopixelsize(const struct tpg_data *tpg, unsigned plane)
+{
+	return tpg->twopixelsize[plane];
+}
+
+static inline unsigned tpg_g_bytesperline(const struct tpg_data *tpg, unsigned plane)
+{
+	return tpg->bytesperline[plane];
+}
+
+static inline void tpg_s_bytesperline(struct tpg_data *tpg, unsigned plane, unsigned bpl)
+{
+	tpg->bytesperline[plane] = bpl;
+}
+
+static inline void tpg_s_buf_height(struct tpg_data *tpg, unsigned h)
+{
+	tpg->buf_height = h;
+}
+
+static inline void tpg_s_field(struct tpg_data *tpg, unsigned field)
+{
+	tpg->field = field;
+}
+
+static inline void tpg_s_perc_fill(struct tpg_data *tpg,
+				      unsigned perc_fill)
+{
+	tpg->perc_fill = perc_fill;
+}
+
+static inline unsigned tpg_g_perc_fill(const struct tpg_data *tpg)
+{
+	return tpg->perc_fill;
+}
+
+static inline void tpg_s_perc_fill_blank(struct tpg_data *tpg,
+					 bool perc_fill_blank)
+{
+	tpg->perc_fill_blank = perc_fill_blank;
+}
+
+static inline void tpg_s_video_aspect(struct tpg_data *tpg,
+					enum tpg_video_aspect vid_aspect)
+{
+	if (tpg->vid_aspect == vid_aspect)
+		return;
+	tpg->vid_aspect = vid_aspect;
+	tpg->recalc_square_border = true;
+}
+
+static inline enum tpg_video_aspect tpg_g_video_aspect(const struct tpg_data *tpg)
+{
+	return tpg->vid_aspect;
+}
+
+static inline void tpg_s_pixel_aspect(struct tpg_data *tpg,
+					enum tpg_pixel_aspect pix_aspect)
+{
+	if (tpg->pix_aspect == pix_aspect)
+		return;
+	tpg->pix_aspect = pix_aspect;
+	tpg->recalc_square_border = true;
+}
+
+static inline void tpg_s_show_border(struct tpg_data *tpg,
+					bool show_border)
+{
+	tpg->show_border = show_border;
+}
+
+static inline void tpg_s_show_square(struct tpg_data *tpg,
+					bool show_square)
+{
+	tpg->show_square = show_square;
+}
+
+static inline void tpg_s_insert_sav(struct tpg_data *tpg, bool insert_sav)
+{
+	tpg->insert_sav = insert_sav;
+}
+
+static inline void tpg_s_insert_eav(struct tpg_data *tpg, bool insert_eav)
+{
+	tpg->insert_eav = insert_eav;
+}
+
+void tpg_update_mv_step(struct tpg_data *tpg);
+
+static inline void tpg_s_mv_hor_mode(struct tpg_data *tpg,
+				enum tpg_move_mode mv_hor_mode)
+{
+	tpg->mv_hor_mode = mv_hor_mode;
+	tpg_update_mv_step(tpg);
+}
+
+static inline void tpg_s_mv_vert_mode(struct tpg_data *tpg,
+				enum tpg_move_mode mv_vert_mode)
+{
+	tpg->mv_vert_mode = mv_vert_mode;
+	tpg_update_mv_step(tpg);
+}
+
+static inline void tpg_init_mv_count(struct tpg_data *tpg)
+{
+	tpg->mv_hor_count = tpg->mv_vert_count = 0;
+}
+
+static inline void tpg_update_mv_count(struct tpg_data *tpg, bool frame_is_field)
+{
+	tpg->mv_hor_count += tpg->mv_hor_step * (frame_is_field ? 1 : 2);
+	tpg->mv_vert_count += tpg->mv_vert_step * (frame_is_field ? 1 : 2);
+}
+
+static inline void tpg_s_hflip(struct tpg_data *tpg, bool hflip)
+{
+	if (tpg->hflip == hflip)
+		return;
+	tpg->hflip = hflip;
+	tpg_update_mv_step(tpg);
+	tpg->recalc_lines = true;
+}
+
+static inline bool tpg_g_hflip(const struct tpg_data *tpg)
+{
+	return tpg->hflip;
+}
+
+static inline void tpg_s_vflip(struct tpg_data *tpg, bool vflip)
+{
+	tpg->vflip = vflip;
+}
+
+static inline bool tpg_g_vflip(const struct tpg_data *tpg)
+{
+	return tpg->vflip;
+}
+
+static inline bool tpg_pattern_is_static(const struct tpg_data *tpg)
+{
+	return tpg->pattern != TPG_PAT_NOISE &&
+	       tpg->mv_hor_mode == TPG_MOVE_NONE &&
+	       tpg->mv_vert_mode == TPG_MOVE_NONE;
+}
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vbi-cap.c b/drivers/media/platform/vivid/vivid-vbi-cap.c
new file mode 100644
index 000000000000..2166d0bf6fe2
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-cap.c
@@ -0,0 +1,371 @@
+/*
+ * vivid-vbi-cap.c - vbi capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-vbi-cap.h"
+#include "vivid-vbi-gen.h"
+
+static void vivid_sliced_vbi_cap_fill(struct vivid_dev *dev, unsigned seqnr)
+{
+	struct vivid_vbi_gen_data *vbi_gen = &dev->vbi_gen;
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+
+	vivid_vbi_gen_sliced(vbi_gen, is_60hz, seqnr);
+
+	if (!is_60hz) {
+		if (dev->loop_video) {
+			if (dev->vbi_out_have_wss) {
+				vbi_gen->data[12].data[0] = dev->vbi_out_wss[0];
+				vbi_gen->data[12].data[1] = dev->vbi_out_wss[1];
+			} else {
+				vbi_gen->data[12].id = 0;
+			}
+		} else {
+			switch (tpg_g_video_aspect(&dev->tpg)) {
+			case TPG_VIDEO_ASPECT_14X9_CENTRE:
+				vbi_gen->data[12].data[0] = 0x01;
+				break;
+			case TPG_VIDEO_ASPECT_16X9_CENTRE:
+				vbi_gen->data[12].data[0] = 0x0b;
+				break;
+			case TPG_VIDEO_ASPECT_16X9_ANAMORPHIC:
+				vbi_gen->data[12].data[0] = 0x07;
+				break;
+			case TPG_VIDEO_ASPECT_4X3:
+			default:
+				vbi_gen->data[12].data[0] = 0x08;
+				break;
+			}
+		}
+	} else if (dev->loop_video && is_60hz) {
+		if (dev->vbi_out_have_cc[0]) {
+			vbi_gen->data[0].data[0] = dev->vbi_out_cc[0][0];
+			vbi_gen->data[0].data[1] = dev->vbi_out_cc[0][1];
+		} else {
+			vbi_gen->data[0].id = 0;
+		}
+		if (dev->vbi_out_have_cc[1]) {
+			vbi_gen->data[1].data[0] = dev->vbi_out_cc[1][0];
+			vbi_gen->data[1].data[1] = dev->vbi_out_cc[1][1];
+		} else {
+			vbi_gen->data[1].id = 0;
+		}
+	}
+}
+
+static void vivid_g_fmt_vbi_cap(struct vivid_dev *dev, struct v4l2_vbi_format *vbi)
+{
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+
+	vbi->sampling_rate = 27000000;
+	vbi->offset = 24;
+	vbi->samples_per_line = 1440;
+	vbi->sample_format = V4L2_PIX_FMT_GREY;
+	vbi->start[0] = is_60hz ? V4L2_VBI_ITU_525_F1_START + 9 : V4L2_VBI_ITU_625_F1_START + 5;
+	vbi->start[1] = is_60hz ? V4L2_VBI_ITU_525_F2_START + 9 : V4L2_VBI_ITU_625_F2_START + 5;
+	vbi->count[0] = vbi->count[1] = is_60hz ? 12 : 18;
+	vbi->flags = dev->vbi_cap_interlaced ? V4L2_VBI_INTERLACED : 0;
+	vbi->reserved[0] = 0;
+	vbi->reserved[1] = 0;
+}
+
+void vivid_raw_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	struct v4l2_vbi_format vbi;
+	u8 *vbuf = vb2_plane_vaddr(&buf->vb, 0);
+
+	vivid_g_fmt_vbi_cap(dev, &vbi);
+	buf->vb.v4l2_buf.sequence = dev->vbi_cap_seq_count;
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+		buf->vb.v4l2_buf.sequence /= 2;
+
+	vivid_sliced_vbi_cap_fill(dev, buf->vb.v4l2_buf.sequence);
+
+	memset(vbuf, 0x10, vb2_plane_size(&buf->vb, 0));
+
+	if (!VIVID_INVALID_SIGNAL(dev->std_signal_mode))
+		vivid_vbi_gen_raw(&dev->vbi_gen, &vbi, vbuf);
+
+	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+	buf->vb.v4l2_buf.timestamp.tv_sec += dev->time_wrap_offset;
+}
+
+
+void vivid_sliced_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	struct v4l2_sliced_vbi_data *vbuf = vb2_plane_vaddr(&buf->vb, 0);
+
+	buf->vb.v4l2_buf.sequence = dev->vbi_cap_seq_count;
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE)
+		buf->vb.v4l2_buf.sequence /= 2;
+
+	vivid_sliced_vbi_cap_fill(dev, buf->vb.v4l2_buf.sequence);
+
+	memset(vbuf, 0, vb2_plane_size(&buf->vb, 0));
+	if (!VIVID_INVALID_SIGNAL(dev->std_signal_mode)) {
+		unsigned i;
+
+		for (i = 0; i < 25; i++)
+			vbuf[i] = dev->vbi_gen.data[i];
+	}
+
+	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
+	buf->vb.v4l2_buf.timestamp.tv_sec += dev->time_wrap_offset;
+}
+
+static int vbi_cap_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+	unsigned size = vq->type == V4L2_BUF_TYPE_SLICED_VBI_CAPTURE ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -EINVAL;
+
+	sizes[0] = size;
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = 1;
+	return 0;
+}
+
+static int vbi_cap_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+	unsigned size = vb->vb2_queue->type == V4L2_BUF_TYPE_SLICED_VBI_CAPTURE ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+				__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void vbi_cap_buf_queue(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vb, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vbi_cap_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vbi_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err;
+
+	dprintk(dev, 1, "%s\n", __func__);
+	dev->vbi_cap_seq_count = 0;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_cap(dev, &dev->vbi_cap_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vbi_cap_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vbi_cap_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_cap(dev, &dev->vbi_cap_streaming);
+}
+
+const struct vb2_ops vivid_vbi_cap_qops = {
+	.queue_setup		= vbi_cap_queue_setup,
+	.buf_prepare		= vbi_cap_buf_prepare,
+	.buf_queue		= vbi_cap_buf_queue,
+	.start_streaming	= vbi_cap_start_streaming,
+	.stop_streaming		= vbi_cap_stop_streaming,
+	.wait_prepare		= vivid_unlock,
+	.wait_finish		= vivid_lock,
+};
+
+int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_vbi_format *vbi = &f->fmt.vbi;
+
+	if (!vivid_is_sdtv_cap(dev) || !dev->has_raw_vbi_cap)
+		return -EINVAL;
+
+	vivid_g_fmt_vbi_cap(dev, vbi);
+	return 0;
+}
+
+int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	int ret = vidioc_g_fmt_vbi_cap(file, priv, f);
+
+	if (ret)
+		return ret;
+	if (dev->stream_sliced_vbi_cap && vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+	dev->stream_sliced_vbi_cap = false;
+	dev->vbi_cap_dev.queue->type = V4L2_BUF_TYPE_VBI_CAPTURE;
+	return 0;
+}
+
+void vivid_fill_service_lines(struct v4l2_sliced_vbi_format *vbi, u32 service_set)
+{
+	vbi->io_size = sizeof(struct v4l2_sliced_vbi_data) * 36;
+	vbi->service_set = service_set;
+	memset(vbi->service_lines, 0, sizeof(vbi->service_lines));
+	memset(vbi->reserved, 0, sizeof(vbi->reserved));
+
+	if (vbi->service_set == 0)
+		return;
+
+	if (vbi->service_set & V4L2_SLICED_CAPTION_525) {
+		vbi->service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+		vbi->service_lines[1][21] = V4L2_SLICED_CAPTION_525;
+	}
+	if (vbi->service_set & V4L2_SLICED_WSS_625) {
+		unsigned i;
+
+		for (i = 7; i <= 18; i++)
+			vbi->service_lines[0][i] =
+			vbi->service_lines[1][i] = V4L2_SLICED_TELETEXT_B;
+		vbi->service_lines[0][23] = V4L2_SLICED_WSS_625;
+	}
+}
+
+int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+
+	if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap)
+		return -EINVAL;
+
+	vivid_fill_service_lines(vbi, dev->service_set_cap);
+	return 0;
+}
+
+int vidioc_try_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	bool is_60hz = dev->std_cap & V4L2_STD_525_60;
+	u32 service_set = vbi->service_set;
+
+	if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap)
+		return -EINVAL;
+
+	service_set &= is_60hz ? V4L2_SLICED_CAPTION_525 :
+				 V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+	vivid_fill_service_lines(vbi, service_set);
+	return 0;
+}
+
+int vidioc_s_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	int ret = vidioc_try_fmt_sliced_vbi_cap(file, fh, fmt);
+
+	if (ret)
+		return ret;
+	if (!dev->stream_sliced_vbi_cap && vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+	dev->service_set_cap = vbi->service_set;
+	dev->stream_sliced_vbi_cap = true;
+	dev->vbi_cap_dev.queue->type = V4L2_BUF_TYPE_SLICED_VBI_CAPTURE;
+	return 0;
+}
+
+int vidioc_g_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_sliced_vbi_cap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+	bool is_60hz;
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		is_60hz = dev->std_cap & V4L2_STD_525_60;
+		if (!vivid_is_sdtv_cap(dev) || !dev->has_sliced_vbi_cap ||
+		    cap->type != V4L2_BUF_TYPE_SLICED_VBI_CAPTURE)
+			return -EINVAL;
+	} else {
+		is_60hz = dev->std_out & V4L2_STD_525_60;
+		if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out ||
+		    cap->type != V4L2_BUF_TYPE_SLICED_VBI_OUTPUT)
+			return -EINVAL;
+	}
+
+	cap->service_set = is_60hz ? V4L2_SLICED_CAPTION_525 :
+				     V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+	if (is_60hz) {
+		cap->service_lines[0][21] = V4L2_SLICED_CAPTION_525;
+		cap->service_lines[1][21] = V4L2_SLICED_CAPTION_525;
+	} else {
+		unsigned i;
+
+		for (i = 7; i <= 18; i++)
+			cap->service_lines[0][i] =
+			cap->service_lines[1][i] = V4L2_SLICED_TELETEXT_B;
+		cap->service_lines[0][23] = V4L2_SLICED_WSS_625;
+	}
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-vbi-cap.h b/drivers/media/platform/vivid/vivid-vbi-cap.h
new file mode 100644
index 000000000000..2d8ea0bac743
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-cap.h
@@ -0,0 +1,40 @@
+/*
+ * vivid-vbi-cap.h - vbi capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VBI_CAP_H_
+#define _VIVID_VBI_CAP_H_
+
+void vivid_fill_time_of_day_packet(u8 *packet);
+void vivid_raw_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+void vivid_sliced_vbi_cap_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vidioc_g_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_s_fmt_vbi_cap(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_g_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_try_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_s_fmt_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_g_sliced_vbi_cap(struct file *file, void *fh, struct v4l2_sliced_vbi_cap *cap);
+
+void vivid_fill_service_lines(struct v4l2_sliced_vbi_format *vbi, u32 service_set);
+
+extern const struct vb2_ops vivid_vbi_cap_qops;
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vbi-gen.c b/drivers/media/platform/vivid/vivid-vbi-gen.c
new file mode 100644
index 000000000000..a2159de83d0b
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-gen.c
@@ -0,0 +1,323 @@
+/*
+ * vivid-vbi-gen.c - vbi generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/ktime.h>
+#include <linux/string.h>
+#include <linux/videodev2.h>
+
+#include "vivid-vbi-gen.h"
+
+static void wss_insert(u8 *wss, u32 val, unsigned size)
+{
+	while (size--)
+		*wss++ = (val & (1 << size)) ? 0xc0 : 0x10;
+}
+
+static void vivid_vbi_gen_wss_raw(const struct v4l2_sliced_vbi_data *data,
+		u8 *buf, unsigned sampling_rate)
+{
+	const unsigned rate = 5000000;	/* WSS has a 5 MHz transmission rate */
+	u8 wss[29 + 24 + 24 + 24 + 18 + 18] = { 0 };
+	const unsigned zero = 0x07;
+	const unsigned one = 0x38;
+	unsigned bit = 0;
+	u16 wss_data;
+	int i;
+
+	wss_insert(wss + bit, 0x1f1c71c7, 29); bit += 29;
+	wss_insert(wss + bit, 0x1e3c1f, 24); bit += 24;
+
+	wss_data = (data->data[1] << 8) | data->data[0];
+	for (i = 0; i <= 13; i++, bit += 6)
+		wss_insert(wss + bit, (wss_data & (1 << i)) ? one : zero, 6);
+
+	for (i = 0, bit = 0; bit < sizeof(wss); bit++) {
+		unsigned n = ((bit + 1) * sampling_rate) / rate;
+
+		while (i < n)
+			buf[i++] = wss[bit];
+	}
+}
+
+static void vivid_vbi_gen_teletext_raw(const struct v4l2_sliced_vbi_data *data,
+		u8 *buf, unsigned sampling_rate)
+{
+	const unsigned rate = 6937500 / 10;	/* Teletext has a 6.9375 MHz transmission rate */
+	u8 teletext[45] = { 0x55, 0x55, 0x27 };
+	unsigned bit = 0;
+	int i;
+
+	memcpy(teletext + 3, data->data, sizeof(teletext) - 3);
+	/* prevents 32 bit overflow */
+	sampling_rate /= 10;
+
+	for (i = 0, bit = 0; bit < sizeof(teletext) * 8; bit++) {
+		unsigned n = ((bit + 1) * sampling_rate) / rate;
+		u8 val = (teletext[bit / 8] & (1 << (bit & 7))) ? 0xc0 : 0x10;
+
+		while (i < n)
+			buf[i++] = val;
+	}
+}
+
+static void cc_insert(u8 *cc, u8 ch)
+{
+	unsigned tot = 0;
+	unsigned i;
+
+	for (i = 0; i < 7; i++) {
+		cc[2 * i] = cc[2 * i + 1] = (ch & (1 << i)) ? 1 : 0;
+		tot += cc[2 * i];
+	}
+	cc[14] = cc[15] = !(tot & 1);
+}
+
+#define CC_PREAMBLE_BITS (14 + 4 + 2)
+
+static void vivid_vbi_gen_cc_raw(const struct v4l2_sliced_vbi_data *data,
+		u8 *buf, unsigned sampling_rate)
+{
+	const unsigned rate = 1000000;	/* CC has a 1 MHz transmission rate */
+
+	u8 cc[CC_PREAMBLE_BITS + 2 * 16] = {
+		/* Clock run-in: 7 cycles */
+		0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1, 0, 1,
+		/* 2 cycles of 0 */
+		0, 0, 0, 0,
+		/* Start bit of 1 (each bit is two cycles) */
+		1, 1
+	};
+	unsigned bit, i;
+
+	cc_insert(cc + CC_PREAMBLE_BITS, data->data[0]);
+	cc_insert(cc + CC_PREAMBLE_BITS + 16, data->data[1]);
+
+	for (i = 0, bit = 0; bit < sizeof(cc); bit++) {
+		unsigned n = ((bit + 1) * sampling_rate) / rate;
+
+		while (i < n)
+			buf[i++] = cc[bit] ? 0xc0 : 0x10;
+	}
+}
+
+void vivid_vbi_gen_raw(const struct vivid_vbi_gen_data *vbi,
+		const struct v4l2_vbi_format *vbi_fmt, u8 *buf)
+{
+	unsigned idx;
+
+	for (idx = 0; idx < 25; idx++) {
+		const struct v4l2_sliced_vbi_data *data = vbi->data + idx;
+		unsigned start_2nd_field;
+		unsigned line = data->line;
+		u8 *linebuf = buf;
+
+		start_2nd_field = (data->id & V4L2_SLICED_VBI_525) ? 263 : 313;
+		if (data->field)
+			line += start_2nd_field;
+		line -= vbi_fmt->start[data->field];
+
+		if (vbi_fmt->flags & V4L2_VBI_INTERLACED)
+			linebuf += (line * 2 + data->field) *
+				vbi_fmt->samples_per_line;
+		else
+			linebuf += (line + data->field * vbi_fmt->count[0]) *
+				vbi_fmt->samples_per_line;
+		if (data->id == V4L2_SLICED_CAPTION_525)
+			vivid_vbi_gen_cc_raw(data, linebuf, vbi_fmt->sampling_rate);
+		else if (data->id == V4L2_SLICED_WSS_625)
+			vivid_vbi_gen_wss_raw(data, linebuf, vbi_fmt->sampling_rate);
+		else if (data->id == V4L2_SLICED_TELETEXT_B)
+			vivid_vbi_gen_teletext_raw(data, linebuf, vbi_fmt->sampling_rate);
+	}
+}
+
+static const u8 vivid_cc_sequence1[30] = {
+	0x14, 0x20,	/* Resume Caption Loading */
+	'H',  'e',
+	'l',  'l',
+	'o',  ' ',
+	'w',  'o',
+	'r',  'l',
+	'd',  '!',
+	0x14, 0x2f,	/* End of Caption */
+};
+
+static const u8 vivid_cc_sequence2[30] = {
+	0x14, 0x20,	/* Resume Caption Loading */
+	'C',  'l',
+	'o',  's',
+	'e',  'd',
+	' ',  'c',
+	'a',  'p',
+	't',  'i',
+	'o',  'n',
+	's',  ' ',
+	't',  'e',
+	's',  't',
+	0x14, 0x2f,	/* End of Caption */
+};
+
+static u8 calc_parity(u8 val)
+{
+	unsigned i;
+	unsigned tot = 0;
+
+	for (i = 0; i < 7; i++)
+		tot += (val & (1 << i)) ? 1 : 0;
+	return val | ((tot & 1) ? 0 : 0x80);
+}
+
+static void vivid_vbi_gen_set_time_of_day(u8 *packet)
+{
+	struct tm tm;
+	u8 checksum, i;
+
+	time_to_tm(get_seconds(), 0, &tm);
+	packet[0] = calc_parity(0x07);
+	packet[1] = calc_parity(0x01);
+	packet[2] = calc_parity(0x40 | tm.tm_min);
+	packet[3] = calc_parity(0x40 | tm.tm_hour);
+	packet[4] = calc_parity(0x40 | tm.tm_mday);
+	if (tm.tm_mday == 1 && tm.tm_mon == 2 &&
+	    sys_tz.tz_minuteswest > tm.tm_min + tm.tm_hour * 60)
+		packet[4] = calc_parity(0x60 | tm.tm_mday);
+	packet[5] = calc_parity(0x40 | (1 + tm.tm_mon));
+	packet[6] = calc_parity(0x40 | (1 + tm.tm_wday));
+	packet[7] = calc_parity(0x40 | ((tm.tm_year - 90) & 0x3f));
+	packet[8] = calc_parity(0x0f);
+	for (checksum = i = 0; i <= 8; i++)
+		checksum += packet[i] & 0x7f;
+	packet[9] = calc_parity(0x100 - checksum);
+	checksum = 0;
+	packet[10] = calc_parity(0x07);
+	packet[11] = calc_parity(0x04);
+	if (sys_tz.tz_minuteswest >= 0)
+		packet[12] = calc_parity(0x40 | ((sys_tz.tz_minuteswest / 60) & 0x1f));
+	else
+		packet[12] = calc_parity(0x40 | ((24 + sys_tz.tz_minuteswest / 60) & 0x1f));
+	packet[13] = calc_parity(0);
+	packet[14] = calc_parity(0x0f);
+	for (checksum = 0, i = 10; i <= 14; i++)
+		checksum += packet[i] & 0x7f;
+	packet[15] = calc_parity(0x100 - checksum);
+}
+
+static const u8 hamming[16] = {
+	0x15, 0x02, 0x49, 0x5e, 0x64, 0x73, 0x38, 0x2f,
+	0xd0, 0xc7, 0x8c, 0x9b, 0xa1, 0xb6, 0xfd, 0xea
+};
+
+static void vivid_vbi_gen_teletext(u8 *packet, unsigned line, unsigned frame)
+{
+	unsigned offset = 2;
+	unsigned i;
+
+	packet[0] = hamming[1 + ((line & 1) << 3)];
+	packet[1] = hamming[line >> 1];
+	memset(packet + 2, 0x20, 40);
+	if (line == 0) {
+		/* subcode */
+		packet[2] = hamming[frame % 10];
+		packet[3] = hamming[frame / 10];
+		packet[4] = hamming[0];
+		packet[5] = hamming[0];
+		packet[6] = hamming[0];
+		packet[7] = hamming[0];
+		packet[8] = hamming[0];
+		packet[9] = hamming[1];
+		offset = 10;
+	}
+	packet += offset;
+	memcpy(packet, "Page: 100 Row: 10", 17);
+	packet[7] = '0' + frame / 10;
+	packet[8] = '0' + frame % 10;
+	packet[15] = '0' + line / 10;
+	packet[16] = '0' + line % 10;
+	for (i = 0; i < 42 - offset; i++)
+		packet[i] = calc_parity(packet[i]);
+}
+
+void vivid_vbi_gen_sliced(struct vivid_vbi_gen_data *vbi,
+		bool is_60hz, unsigned seqnr)
+{
+	struct v4l2_sliced_vbi_data *data0 = vbi->data;
+	struct v4l2_sliced_vbi_data *data1 = vbi->data + 1;
+	unsigned frame = seqnr % 60;
+
+	memset(vbi->data, 0, sizeof(vbi->data));
+
+	if (!is_60hz) {
+		unsigned i;
+
+		for (i = 0; i <= 11; i++) {
+			data0->id = V4L2_SLICED_TELETEXT_B;
+			data0->line = 7 + i;
+			vivid_vbi_gen_teletext(data0->data, i, frame);
+			data0++;
+		}
+		data0->id = V4L2_SLICED_WSS_625;
+		data0->line = 23;
+		/* 4x3 video aspect ratio */
+		data0->data[0] = 0x08;
+		data0++;
+		for (i = 0; i <= 11; i++) {
+			data0->id = V4L2_SLICED_TELETEXT_B;
+			data0->field = 1;
+			data0->line = 7 + i;
+			vivid_vbi_gen_teletext(data0->data, 12 + i, frame);
+			data0++;
+		}
+		return;
+	}
+
+	data0->id = V4L2_SLICED_CAPTION_525;
+	data0->line = 21;
+	data1->id = V4L2_SLICED_CAPTION_525;
+	data1->field = 1;
+	data1->line = 21;
+
+	if (frame < 15) {
+		data0->data[0] = calc_parity(vivid_cc_sequence1[2 * frame]);
+		data0->data[1] = calc_parity(vivid_cc_sequence1[2 * frame + 1]);
+	} else if (frame >= 30 && frame < 45) {
+		frame -= 30;
+		data0->data[0] = calc_parity(vivid_cc_sequence2[2 * frame]);
+		data0->data[1] = calc_parity(vivid_cc_sequence2[2 * frame + 1]);
+	} else {
+		data0->data[0] = calc_parity(0);
+		data0->data[1] = calc_parity(0);
+	}
+
+	frame = seqnr % (30 * 60);
+	switch (frame) {
+	case 0:
+		vivid_vbi_gen_set_time_of_day(vbi->time_of_day_packet);
+		/* fall through */
+	case 1 ... 7:
+		data1->data[0] = vbi->time_of_day_packet[frame * 2];
+		data1->data[1] = vbi->time_of_day_packet[frame * 2 + 1];
+		break;
+	default:
+		data1->data[0] = calc_parity(0);
+		data1->data[1] = calc_parity(0);
+		break;
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-vbi-gen.h b/drivers/media/platform/vivid/vivid-vbi-gen.h
new file mode 100644
index 000000000000..8444abe905ea
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-gen.h
@@ -0,0 +1,33 @@
+/*
+ * vivid-vbi-gen.h - vbi generator support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VBI_GEN_H_
+#define _VIVID_VBI_GEN_H_
+
+struct vivid_vbi_gen_data {
+	struct v4l2_sliced_vbi_data data[25];
+	u8 time_of_day_packet[16];
+};
+
+void vivid_vbi_gen_sliced(struct vivid_vbi_gen_data *vbi,
+		bool is_60hz, unsigned seqnr);
+void vivid_vbi_gen_raw(const struct vivid_vbi_gen_data *vbi,
+		const struct v4l2_vbi_format *vbi_fmt, u8 *buf);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vbi-out.c b/drivers/media/platform/vivid/vivid-vbi-out.c
new file mode 100644
index 000000000000..9d00a07ecdcd
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-out.c
@@ -0,0 +1,248 @@
+/*
+ * vivid-vbi-out.c - vbi output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/videodev2.h>
+#include <media/v4l2-common.h>
+
+#include "vivid-core.h"
+#include "vivid-kthread-out.h"
+#include "vivid-vbi-out.h"
+#include "vivid-vbi-cap.h"
+
+static int vbi_out_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+	unsigned size = vq->type == V4L2_BUF_TYPE_SLICED_VBI_OUTPUT ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	if (!vivid_is_svid_out(dev))
+		return -EINVAL;
+
+	sizes[0] = size;
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = 1;
+	return 0;
+}
+
+static int vbi_out_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+	unsigned size = vb->vb2_queue->type == V4L2_BUF_TYPE_SLICED_VBI_OUTPUT ?
+		36 * sizeof(struct v4l2_sliced_vbi_data) :
+		1440 * 2 * (is_60hz ? 12 : 18);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	if (vb2_plane_size(vb, 0) < size) {
+		dprintk(dev, 1, "%s data will not fit into plane (%lu < %u)\n",
+				__func__, vb2_plane_size(vb, 0), size);
+		return -EINVAL;
+	}
+	vb2_set_plane_payload(vb, 0, size);
+
+	return 0;
+}
+
+static void vbi_out_buf_queue(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vb, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vbi_out_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vbi_out_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err;
+
+	dprintk(dev, 1, "%s\n", __func__);
+	dev->vbi_out_seq_count = 0;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_out(dev, &dev->vbi_out_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vbi_out_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vbi_out_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_out(dev, &dev->vbi_out_streaming);
+	dev->vbi_out_have_wss = false;
+	dev->vbi_out_have_cc[0] = false;
+	dev->vbi_out_have_cc[1] = false;
+}
+
+const struct vb2_ops vivid_vbi_out_qops = {
+	.queue_setup		= vbi_out_queue_setup,
+	.buf_prepare		= vbi_out_buf_prepare,
+	.buf_queue		= vbi_out_buf_queue,
+	.start_streaming	= vbi_out_start_streaming,
+	.stop_streaming		= vbi_out_stop_streaming,
+	.wait_prepare		= vivid_unlock,
+	.wait_finish		= vivid_lock,
+};
+
+int vidioc_g_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_vbi_format *vbi = &f->fmt.vbi;
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+
+	if (!vivid_is_svid_out(dev) || !dev->has_raw_vbi_out)
+		return -EINVAL;
+
+	vbi->sampling_rate = 25000000;
+	vbi->offset = 24;
+	vbi->samples_per_line = 1440;
+	vbi->sample_format = V4L2_PIX_FMT_GREY;
+	vbi->start[0] = is_60hz ? V4L2_VBI_ITU_525_F1_START + 9 : V4L2_VBI_ITU_625_F1_START + 5;
+	vbi->start[1] = is_60hz ? V4L2_VBI_ITU_525_F2_START + 9 : V4L2_VBI_ITU_625_F2_START + 5;
+	vbi->count[0] = vbi->count[1] = is_60hz ? 12 : 18;
+	vbi->flags = dev->vbi_cap_interlaced ? V4L2_VBI_INTERLACED : 0;
+	vbi->reserved[0] = 0;
+	vbi->reserved[1] = 0;
+	return 0;
+}
+
+int vidioc_s_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	int ret = vidioc_g_fmt_vbi_out(file, priv, f);
+
+	if (ret)
+		return ret;
+	if (vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+	dev->stream_sliced_vbi_out = false;
+	dev->vbi_out_dev.queue->type = V4L2_BUF_TYPE_VBI_OUTPUT;
+	return 0;
+}
+
+int vidioc_g_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+
+	if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out)
+		return -EINVAL;
+
+	vivid_fill_service_lines(vbi, dev->service_set_out);
+	return 0;
+}
+
+int vidioc_try_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	bool is_60hz = dev->std_out & V4L2_STD_525_60;
+	u32 service_set = vbi->service_set;
+
+	if (!vivid_is_svid_out(dev) || !dev->has_sliced_vbi_out)
+		return -EINVAL;
+
+	service_set &= is_60hz ? V4L2_SLICED_CAPTION_525 :
+				 V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+	vivid_fill_service_lines(vbi, service_set);
+	return 0;
+}
+
+int vidioc_s_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_sliced_vbi_format *vbi = &fmt->fmt.sliced;
+	int ret = vidioc_try_fmt_sliced_vbi_out(file, fh, fmt);
+
+	if (ret)
+		return ret;
+	if (vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+	dev->service_set_out = vbi->service_set;
+	dev->stream_sliced_vbi_out = true;
+	dev->vbi_out_dev.queue->type = V4L2_BUF_TYPE_SLICED_VBI_OUTPUT;
+	return 0;
+}
+
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev, struct vivid_buffer *buf)
+{
+	struct v4l2_sliced_vbi_data *vbi = vb2_plane_vaddr(&buf->vb, 0);
+	unsigned elems = vb2_get_plane_payload(&buf->vb, 0) / sizeof(*vbi);
+
+	dev->vbi_out_have_cc[0] = false;
+	dev->vbi_out_have_cc[1] = false;
+	dev->vbi_out_have_wss = false;
+	while (elems--) {
+		switch (vbi->id) {
+		case V4L2_SLICED_CAPTION_525:
+			if ((dev->std_out & V4L2_STD_525_60) && vbi->line == 21) {
+				dev->vbi_out_have_cc[!!vbi->field] = true;
+				dev->vbi_out_cc[!!vbi->field][0] = vbi->data[0];
+				dev->vbi_out_cc[!!vbi->field][1] = vbi->data[1];
+			}
+			break;
+		case V4L2_SLICED_WSS_625:
+			if ((dev->std_out & V4L2_STD_625_50) &&
+			    vbi->field == 0 && vbi->line == 23) {
+				dev->vbi_out_have_wss = true;
+				dev->vbi_out_wss[0] = vbi->data[0];
+				dev->vbi_out_wss[1] = vbi->data[1];
+			}
+			break;
+		}
+		vbi++;
+	}
+}
diff --git a/drivers/media/platform/vivid/vivid-vbi-out.h b/drivers/media/platform/vivid/vivid-vbi-out.h
new file mode 100644
index 000000000000..6555ba9d2860
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vbi-out.h
@@ -0,0 +1,34 @@
+/*
+ * vivid-vbi-out.h - vbi output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VBI_OUT_H_
+#define _VIVID_VBI_OUT_H_
+
+void vivid_sliced_vbi_out_process(struct vivid_dev *dev, struct vivid_buffer *buf);
+int vidioc_g_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_s_fmt_vbi_out(struct file *file, void *priv,
+					struct v4l2_format *f);
+int vidioc_g_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_try_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+int vidioc_s_fmt_sliced_vbi_out(struct file *file, void *fh, struct v4l2_format *fmt);
+
+extern const struct vb2_ops vivid_vbi_out_qops;
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.c b/drivers/media/platform/vivid/vivid-vid-cap.c
new file mode 100644
index 000000000000..331c54429b40
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-cap.c
@@ -0,0 +1,1730 @@
+/*
+ * vivid-vid-cap.c - video capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/vmalloc.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-kthread-cap.h"
+#include "vivid-vid-cap.h"
+
+/* timeperframe: min/max and default */
+static const struct v4l2_fract
+	tpf_min     = {.numerator = 1,		.denominator = FPS_MAX},
+	tpf_max     = {.numerator = FPS_MAX,	.denominator = 1},
+	tpf_default = {.numerator = 1,		.denominator = 30};
+
+static const struct vivid_fmt formats_ovl[] = {
+	{
+		.name     = "RGB565 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+		.depth    = 16,
+		.planes   = 1,
+	},
+	{
+		.name     = "XRGB555 (LE)",
+		.fourcc   = V4L2_PIX_FMT_XRGB555, /* gggbbbbb arrrrrgg */
+		.depth    = 16,
+		.planes   = 1,
+	},
+	{
+		.name     = "ARGB555 (LE)",
+		.fourcc   = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
+		.depth    = 16,
+		.planes   = 1,
+	},
+};
+
+/* The number of discrete webcam framesizes */
+#define VIVID_WEBCAM_SIZES 3
+/* The number of discrete webcam frameintervals */
+#define VIVID_WEBCAM_IVALS (VIVID_WEBCAM_SIZES * 2)
+
+/* Sizes must be in increasing order */
+static const struct v4l2_frmsize_discrete webcam_sizes[VIVID_WEBCAM_SIZES] = {
+	{  320, 180 },
+	{  640, 360 },
+	{ 1280, 720 },
+};
+
+/*
+ * Intervals must be in increasing order and there must be twice as many
+ * elements in this array as there are in webcam_sizes.
+ */
+static const struct v4l2_fract webcam_intervals[VIVID_WEBCAM_IVALS] = {
+	{  1, 10 },
+	{  1, 15 },
+	{  1, 25 },
+	{  1, 30 },
+	{  1, 50 },
+	{  1, 60 },
+};
+
+static const struct v4l2_discrete_probe webcam_probe = {
+	webcam_sizes,
+	VIVID_WEBCAM_SIZES
+};
+
+static int vid_cap_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	unsigned planes = tpg_g_planes(&dev->tpg);
+	unsigned h = dev->fmt_cap_rect.height;
+	unsigned p;
+
+	if (dev->field_cap == V4L2_FIELD_ALTERNATE) {
+		/*
+		 * You cannot use read() with FIELD_ALTERNATE since the field
+		 * information (TOP/BOTTOM) cannot be passed back to the user.
+		 */
+		if (vb2_fileio_is_active(vq))
+			return -EINVAL;
+	}
+
+	if (dev->queue_setup_error) {
+		/*
+		 * Error injection: test what happens if queue_setup() returns
+		 * an error.
+		 */
+		dev->queue_setup_error = false;
+		return -EINVAL;
+	}
+	if (fmt) {
+		const struct v4l2_pix_format_mplane *mp;
+		struct v4l2_format mp_fmt;
+		const struct vivid_fmt *vfmt;
+
+		if (!V4L2_TYPE_IS_MULTIPLANAR(fmt->type)) {
+			fmt_sp2mp(fmt, &mp_fmt);
+			fmt = &mp_fmt;
+		}
+		mp = &fmt->fmt.pix_mp;
+		/*
+		 * Check if the number of planes in the specified format match
+		 * the number of planes in the current format. You can't mix that.
+		 */
+		if (mp->num_planes != planes)
+			return -EINVAL;
+		vfmt = vivid_get_format(dev, mp->pixelformat);
+		for (p = 0; p < planes; p++) {
+			sizes[p] = mp->plane_fmt[p].sizeimage;
+			if (sizes[0] < tpg_g_bytesperline(&dev->tpg, 0) * h +
+							vfmt->data_offset[p])
+				return -EINVAL;
+		}
+	} else {
+		for (p = 0; p < planes; p++)
+			sizes[p] = tpg_g_bytesperline(&dev->tpg, p) * h +
+					dev->fmt_cap->data_offset[p];
+	}
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = planes;
+
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	if (planes == 2)
+		dprintk(dev, 1, "%s, count=%d, sizes=%u, %u\n", __func__,
+			*nbuffers, sizes[0], sizes[1]);
+	else
+		dprintk(dev, 1, "%s, count=%d, size=%u\n", __func__,
+			*nbuffers, sizes[0]);
+
+	return 0;
+}
+
+static int vid_cap_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size;
+	unsigned planes = tpg_g_planes(&dev->tpg);
+	unsigned p;
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (WARN_ON(NULL == dev->fmt_cap))
+		return -EINVAL;
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+	for (p = 0; p < planes; p++) {
+		size = tpg_g_bytesperline(&dev->tpg, p) * dev->fmt_cap_rect.height +
+			dev->fmt_cap->data_offset[p];
+
+		if (vb2_plane_size(vb, 0) < size) {
+			dprintk(dev, 1, "%s data will not fit into plane %u (%lu < %lu)\n",
+					__func__, p, vb2_plane_size(vb, 0), size);
+			return -EINVAL;
+		}
+
+		vb2_set_plane_payload(vb, p, size);
+		vb->v4l2_planes[p].data_offset = dev->fmt_cap->data_offset[p];
+	}
+
+	return 0;
+}
+
+static void vid_cap_buf_finish(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct v4l2_timecode *tc = &vb->v4l2_buf.timecode;
+	unsigned fps = 25;
+	unsigned seq = vb->v4l2_buf.sequence;
+
+	if (!vivid_is_sdtv_cap(dev))
+		return;
+
+	/*
+	 * Set the timecode. Rarely used, so it is interesting to
+	 * test this.
+	 */
+	vb->v4l2_buf.flags |= V4L2_BUF_FLAG_TIMECODE;
+	if (dev->std_cap & V4L2_STD_525_60)
+		fps = 30;
+	tc->type = (fps == 30) ? V4L2_TC_TYPE_30FPS : V4L2_TC_TYPE_25FPS;
+	tc->flags = 0;
+	tc->frames = seq % fps;
+	tc->seconds = (seq / fps) % 60;
+	tc->minutes = (seq / (60 * fps)) % 60;
+	tc->hours = (seq / (60 * 60 * fps)) % 24;
+}
+
+static void vid_cap_buf_queue(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vb, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vid_cap_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vid_cap_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	unsigned i;
+	int err;
+
+	if (vb2_is_streaming(&dev->vb_vid_out_q))
+		dev->can_loop_video = vivid_vid_can_loop(dev);
+
+	if (dev->kthread_vid_cap)
+		return 0;
+
+	dev->vid_cap_seq_count = 0;
+	dprintk(dev, 1, "%s\n", __func__);
+	for (i = 0; i < VIDEO_MAX_FRAME; i++)
+		dev->must_blank[i] = tpg_g_perc_fill(&dev->tpg) < 100;
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_cap(dev, &dev->vid_cap_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vid_cap_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vid_cap_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_cap(dev, &dev->vid_cap_streaming);
+	dev->can_loop_video = false;
+}
+
+const struct vb2_ops vivid_vid_cap_qops = {
+	.queue_setup		= vid_cap_queue_setup,
+	.buf_prepare		= vid_cap_buf_prepare,
+	.buf_finish		= vid_cap_buf_finish,
+	.buf_queue		= vid_cap_buf_queue,
+	.start_streaming	= vid_cap_start_streaming,
+	.stop_streaming		= vid_cap_stop_streaming,
+	.wait_prepare		= vivid_unlock,
+	.wait_finish		= vivid_lock,
+};
+
+/*
+ * Determine the 'picture' quality based on the current TV frequency: either
+ * COLOR for a good 'signal', GRAY (grayscale picture) for a slightly off
+ * signal or NOISE for no signal.
+ */
+void vivid_update_quality(struct vivid_dev *dev)
+{
+	unsigned freq_modulus;
+
+	if (dev->loop_video && (vivid_is_svid_cap(dev) || vivid_is_hdmi_cap(dev))) {
+		/*
+		 * The 'noise' will only be replaced by the actual video
+		 * if the output video matches the input video settings.
+		 */
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+		return;
+	}
+	if (vivid_is_hdmi_cap(dev) && VIVID_INVALID_SIGNAL(dev->dv_timings_signal_mode)) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+		return;
+	}
+	if (vivid_is_sdtv_cap(dev) && VIVID_INVALID_SIGNAL(dev->std_signal_mode)) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE, 0);
+		return;
+	}
+	if (!vivid_is_tv_cap(dev)) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_COLOR, 0);
+		return;
+	}
+
+	/*
+	 * There is a fake channel every 6 MHz at 49.25, 55.25, etc.
+	 * From +/- 0.25 MHz around the channel there is color, and from
+	 * +/- 1 MHz there is grayscale (chroma is lost).
+	 * Everywhere else it is just noise.
+	 */
+	freq_modulus = (dev->tv_freq - 676 /* (43.25-1) * 16 */) % (6 * 16);
+	if (freq_modulus > 2 * 16) {
+		tpg_s_quality(&dev->tpg, TPG_QUAL_NOISE,
+			next_pseudo_random32(dev->tv_freq ^ 0x55) & 0x3f);
+		return;
+	}
+	if (freq_modulus < 12 /*0.75 * 16*/ || freq_modulus > 20 /*1.25 * 16*/)
+		tpg_s_quality(&dev->tpg, TPG_QUAL_GRAY, 0);
+	else
+		tpg_s_quality(&dev->tpg, TPG_QUAL_COLOR, 0);
+}
+
+/*
+ * Get the current picture quality and the associated afc value.
+ */
+static enum tpg_quality vivid_get_quality(struct vivid_dev *dev, s32 *afc)
+{
+	unsigned freq_modulus;
+
+	if (afc)
+		*afc = 0;
+	if (tpg_g_quality(&dev->tpg) == TPG_QUAL_COLOR ||
+	    tpg_g_quality(&dev->tpg) == TPG_QUAL_NOISE)
+		return tpg_g_quality(&dev->tpg);
+
+	/*
+	 * There is a fake channel every 6 MHz at 49.25, 55.25, etc.
+	 * From +/- 0.25 MHz around the channel there is color, and from
+	 * +/- 1 MHz there is grayscale (chroma is lost).
+	 * Everywhere else it is just gray.
+	 */
+	freq_modulus = (dev->tv_freq - 676 /* (43.25-1) * 16 */) % (6 * 16);
+	if (afc)
+		*afc = freq_modulus - 1 * 16;
+	return TPG_QUAL_GRAY;
+}
+
+enum tpg_video_aspect vivid_get_video_aspect(const struct vivid_dev *dev)
+{
+	if (vivid_is_sdtv_cap(dev))
+		return dev->std_aspect_ratio;
+
+	if (vivid_is_hdmi_cap(dev))
+		return dev->dv_timings_aspect_ratio;
+
+	return TPG_VIDEO_ASPECT_IMAGE;
+}
+
+static enum tpg_pixel_aspect vivid_get_pixel_aspect(const struct vivid_dev *dev)
+{
+	if (vivid_is_sdtv_cap(dev))
+		return (dev->std_cap & V4L2_STD_525_60) ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	if (vivid_is_hdmi_cap(dev) &&
+	    dev->src_rect.width == 720 && dev->src_rect.height <= 576)
+		return dev->src_rect.height == 480 ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	return TPG_PIXEL_ASPECT_SQUARE;
+}
+
+/*
+ * Called whenever the format has to be reset which can occur when
+ * changing inputs, standard, timings, etc.
+ */
+void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls)
+{
+	struct v4l2_bt_timings *bt = &dev->dv_timings_cap.bt;
+	unsigned size;
+
+	switch (dev->input_type[dev->input]) {
+	case WEBCAM:
+	default:
+		dev->src_rect.width = webcam_sizes[dev->webcam_size_idx].width;
+		dev->src_rect.height = webcam_sizes[dev->webcam_size_idx].height;
+		dev->timeperframe_vid_cap = webcam_intervals[dev->webcam_ival_idx];
+		dev->field_cap = V4L2_FIELD_NONE;
+		tpg_s_rgb_range(&dev->tpg, V4L2_DV_RGB_RANGE_AUTO);
+		break;
+	case TV:
+	case SVID:
+		dev->field_cap = dev->tv_field_cap;
+		dev->src_rect.width = 720;
+		if (dev->std_cap & V4L2_STD_525_60) {
+			dev->src_rect.height = 480;
+			dev->timeperframe_vid_cap = (struct v4l2_fract) { 1001, 30000 };
+			dev->service_set_cap = V4L2_SLICED_CAPTION_525;
+		} else {
+			dev->src_rect.height = 576;
+			dev->timeperframe_vid_cap = (struct v4l2_fract) { 1000, 25000 };
+			dev->service_set_cap = V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+		}
+		tpg_s_rgb_range(&dev->tpg, V4L2_DV_RGB_RANGE_AUTO);
+		break;
+	case HDMI:
+		dev->src_rect.width = bt->width;
+		dev->src_rect.height = bt->height;
+		size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+		dev->timeperframe_vid_cap = (struct v4l2_fract) {
+			size / 100, (u32)bt->pixelclock / 100
+		};
+		if (bt->interlaced)
+			dev->field_cap = V4L2_FIELD_ALTERNATE;
+		else
+			dev->field_cap = V4L2_FIELD_NONE;
+
+		/*
+		 * We can be called from within s_ctrl, in that case we can't
+		 * set/get controls. Luckily we don't need to in that case.
+		 */
+		if (keep_controls || !dev->colorspace)
+			break;
+		if (bt->standards & V4L2_DV_BT_STD_CEA861) {
+			if (bt->width == 720 && bt->height <= 576)
+				v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_SMPTE170M);
+			else
+				v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_REC709);
+			v4l2_ctrl_s_ctrl(dev->real_rgb_range_cap, 1);
+		} else {
+			v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_SRGB);
+			v4l2_ctrl_s_ctrl(dev->real_rgb_range_cap, 0);
+		}
+		tpg_s_rgb_range(&dev->tpg, v4l2_ctrl_g_ctrl(dev->rgb_range_cap));
+		break;
+	}
+	vivid_update_quality(dev);
+	tpg_reset_source(&dev->tpg, dev->src_rect.width, dev->src_rect.height, dev->field_cap);
+	dev->crop_cap = dev->src_rect;
+	dev->crop_bounds_cap = dev->src_rect;
+	dev->compose_cap = dev->crop_cap;
+	if (V4L2_FIELD_HAS_T_OR_B(dev->field_cap))
+		dev->compose_cap.height /= 2;
+	dev->fmt_cap_rect = dev->compose_cap;
+	tpg_s_video_aspect(&dev->tpg, vivid_get_video_aspect(dev));
+	tpg_s_pixel_aspect(&dev->tpg, vivid_get_pixel_aspect(dev));
+	tpg_update_mv_step(&dev->tpg);
+}
+
+/* Map the field to something that is valid for the current input */
+static enum v4l2_field vivid_field_cap(struct vivid_dev *dev, enum v4l2_field field)
+{
+	if (vivid_is_sdtv_cap(dev)) {
+		switch (field) {
+		case V4L2_FIELD_INTERLACED_TB:
+		case V4L2_FIELD_INTERLACED_BT:
+		case V4L2_FIELD_SEQ_TB:
+		case V4L2_FIELD_SEQ_BT:
+		case V4L2_FIELD_TOP:
+		case V4L2_FIELD_BOTTOM:
+		case V4L2_FIELD_ALTERNATE:
+			return field;
+		case V4L2_FIELD_INTERLACED:
+		default:
+			return V4L2_FIELD_INTERLACED;
+		}
+	}
+	if (vivid_is_hdmi_cap(dev))
+		return dev->dv_timings_cap.bt.interlaced ? V4L2_FIELD_ALTERNATE :
+						       V4L2_FIELD_NONE;
+	return V4L2_FIELD_NONE;
+}
+
+static unsigned vivid_colorspace_cap(struct vivid_dev *dev)
+{
+	if (!dev->loop_video || vivid_is_webcam(dev) || vivid_is_tv_cap(dev))
+		return tpg_g_colorspace(&dev->tpg);
+	return dev->colorspace_out;
+}
+
+int vivid_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	unsigned p;
+
+	mp->width        = dev->fmt_cap_rect.width;
+	mp->height       = dev->fmt_cap_rect.height;
+	mp->field        = dev->field_cap;
+	mp->pixelformat  = dev->fmt_cap->fourcc;
+	mp->colorspace   = vivid_colorspace_cap(dev);
+	mp->num_planes = dev->fmt_cap->planes;
+	for (p = 0; p < mp->num_planes; p++) {
+		mp->plane_fmt[p].bytesperline = tpg_g_bytesperline(&dev->tpg, p);
+		mp->plane_fmt[p].sizeimage =
+			mp->plane_fmt[p].bytesperline * mp->height +
+			dev->fmt_cap->data_offset[p];
+	}
+	return 0;
+}
+
+int vivid_try_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt = mp->plane_fmt;
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+	unsigned bytesperline, max_bpl;
+	unsigned factor = 1;
+	unsigned w, h;
+	unsigned p;
+
+	fmt = vivid_get_format(dev, mp->pixelformat);
+	if (!fmt) {
+		dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
+			mp->pixelformat);
+		mp->pixelformat = V4L2_PIX_FMT_YUYV;
+		fmt = vivid_get_format(dev, mp->pixelformat);
+	}
+
+	mp->field = vivid_field_cap(dev, mp->field);
+	if (vivid_is_webcam(dev)) {
+		const struct v4l2_frmsize_discrete *sz =
+			v4l2_find_nearest_format(&webcam_probe, mp->width, mp->height);
+
+		w = sz->width;
+		h = sz->height;
+	} else if (vivid_is_sdtv_cap(dev)) {
+		w = 720;
+		h = (dev->std_cap & V4L2_STD_525_60) ? 480 : 576;
+	} else {
+		w = dev->src_rect.width;
+		h = dev->src_rect.height;
+	}
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+	if (vivid_is_webcam(dev) ||
+	    (!dev->has_scaler_cap && !dev->has_crop_cap && !dev->has_compose_cap)) {
+		mp->width = w;
+		mp->height = h / factor;
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height * factor };
+
+		rect_set_min_size(&r, &vivid_min_rect);
+		rect_set_max_size(&r, &vivid_max_rect);
+		if (dev->has_scaler_cap && !dev->has_compose_cap) {
+			struct v4l2_rect max_r = { 0, 0, MAX_ZOOM * w, MAX_ZOOM * h };
+
+			rect_set_max_size(&r, &max_r);
+		} else if (!dev->has_scaler_cap && dev->has_crop_cap && !dev->has_compose_cap) {
+			rect_set_max_size(&r, &dev->src_rect);
+		} else if (!dev->has_scaler_cap && !dev->has_crop_cap) {
+			rect_set_min_size(&r, &dev->src_rect);
+		}
+		mp->width = r.width;
+		mp->height = r.height / factor;
+	}
+
+	/* This driver supports custom bytesperline values */
+
+	/* Calculate the minimum supported bytesperline value */
+	bytesperline = (mp->width * fmt->depth) >> 3;
+	/* Calculate the maximum supported bytesperline value */
+	max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->depth) >> 3;
+	mp->num_planes = fmt->planes;
+	for (p = 0; p < mp->num_planes; p++) {
+		if (pfmt[p].bytesperline > max_bpl)
+			pfmt[p].bytesperline = max_bpl;
+		if (pfmt[p].bytesperline < bytesperline)
+			pfmt[p].bytesperline = bytesperline;
+		pfmt[p].sizeimage = pfmt[p].bytesperline * mp->height +
+			fmt->data_offset[p];
+		memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
+	}
+	mp->colorspace = vivid_colorspace_cap(dev);
+	memset(mp->reserved, 0, sizeof(mp->reserved));
+	return 0;
+}
+
+int vivid_s_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_cap;
+	struct v4l2_rect *compose = &dev->compose_cap;
+	struct vb2_queue *q = &dev->vb_vid_cap_q;
+	int ret = vivid_try_fmt_vid_cap(file, priv, f);
+	unsigned factor = 1;
+	unsigned i;
+
+	if (ret < 0)
+		return ret;
+
+	if (vb2_is_busy(q)) {
+		dprintk(dev, 1, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	if (dev->overlay_cap_owner && dev->fb_cap.fmt.pixelformat != mp->pixelformat) {
+		dprintk(dev, 1, "overlay is active, can't change pixelformat\n");
+		return -EBUSY;
+	}
+
+	dev->fmt_cap = vivid_get_format(dev, mp->pixelformat);
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+
+	/* Note: the webcam input doesn't support scaling, cropping or composing */
+
+	if (!vivid_is_webcam(dev) &&
+	    (dev->has_scaler_cap || dev->has_crop_cap || dev->has_compose_cap)) {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		if (dev->has_scaler_cap) {
+			if (dev->has_compose_cap)
+				rect_map_inside(compose, &r);
+			else
+				*compose = r;
+			if (dev->has_crop_cap && !dev->has_compose_cap) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					r.width / MAX_ZOOM,
+					factor * r.height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					r.width * MAX_ZOOM,
+					factor * r.height * MAX_ZOOM
+				};
+
+				rect_set_min_size(crop, &min_r);
+				rect_set_max_size(crop, &max_r);
+				rect_map_inside(crop, &dev->crop_bounds_cap);
+			} else if (dev->has_crop_cap) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					compose->width / MAX_ZOOM,
+					factor * compose->height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					compose->width * MAX_ZOOM,
+					factor * compose->height * MAX_ZOOM
+				};
+
+				rect_set_min_size(crop, &min_r);
+				rect_set_max_size(crop, &max_r);
+				rect_map_inside(crop, &dev->crop_bounds_cap);
+			}
+		} else if (dev->has_crop_cap && !dev->has_compose_cap) {
+			r.height *= factor;
+			rect_set_size_to(crop, &r);
+			rect_map_inside(crop, &dev->crop_bounds_cap);
+			r = *crop;
+			r.height /= factor;
+			rect_set_size_to(compose, &r);
+		} else if (!dev->has_crop_cap) {
+			rect_map_inside(compose, &r);
+		} else {
+			r.height *= factor;
+			rect_set_max_size(crop, &r);
+			rect_map_inside(crop, &dev->crop_bounds_cap);
+			compose->top *= factor;
+			compose->height *= factor;
+			rect_set_size_to(compose, crop);
+			rect_map_inside(compose, &r);
+			compose->top /= factor;
+			compose->height /= factor;
+		}
+	} else if (vivid_is_webcam(dev)) {
+		/* Guaranteed to be a match */
+		for (i = 0; i < ARRAY_SIZE(webcam_sizes); i++)
+			if (webcam_sizes[i].width == mp->width &&
+					webcam_sizes[i].height == mp->height)
+				break;
+		dev->webcam_size_idx = i;
+		if (dev->webcam_ival_idx >= 2 * (3 - i))
+			dev->webcam_ival_idx = 2 * (3 - i) - 1;
+		vivid_update_format_cap(dev, false);
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		rect_set_size_to(compose, &r);
+		r.height *= factor;
+		rect_set_size_to(crop, &r);
+	}
+
+	dev->fmt_cap_rect.width = mp->width;
+	dev->fmt_cap_rect.height = mp->height;
+	tpg_s_buf_height(&dev->tpg, mp->height);
+	tpg_s_bytesperline(&dev->tpg, 0, mp->plane_fmt[0].bytesperline);
+	if (tpg_g_planes(&dev->tpg) > 1)
+		tpg_s_bytesperline(&dev->tpg, 1, mp->plane_fmt[1].bytesperline);
+	dev->field_cap = mp->field;
+	tpg_s_field(&dev->tpg, dev->field_cap);
+	tpg_s_crop_compose(&dev->tpg, &dev->crop_cap, &dev->compose_cap);
+	tpg_s_fourcc(&dev->tpg, dev->fmt_cap->fourcc);
+	if (vivid_is_sdtv_cap(dev))
+		dev->tv_field_cap = mp->field;
+	tpg_update_mv_step(&dev->tpg);
+	return 0;
+}
+
+int vidioc_g_fmt_vid_cap_mplane(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_g_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_try_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_s_fmt_vid_cap_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_s_fmt_vid_cap(file, priv, f);
+}
+
+int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_g_fmt_vid_cap);
+}
+
+int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_try_fmt_vid_cap);
+}
+
+int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_s_fmt_vid_cap);
+}
+
+int vivid_vid_cap_g_selection(struct file *file, void *priv,
+			      struct v4l2_selection *sel)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->has_crop_cap && !dev->has_compose_cap)
+		return -ENOTTY;
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (vivid_is_webcam(dev))
+		return -EINVAL;
+
+	sel->r.left = sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_cap)
+			return -EINVAL;
+		sel->r = dev->crop_cap;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		if (!dev->has_crop_cap)
+			return -EINVAL;
+		sel->r = dev->src_rect;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		sel->r = vivid_max_rect;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		sel->r = dev->compose_cap;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		sel->r = dev->fmt_cap_rect;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_cap;
+	struct v4l2_rect *compose = &dev->compose_cap;
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_cap) ? 2 : 1;
+	int ret;
+
+	if (!dev->has_crop_cap && !dev->has_compose_cap)
+		return -ENOTTY;
+	if (s->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+	if (vivid_is_webcam(dev))
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_cap)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->src_rect);
+		rect_map_inside(&s->r, &dev->crop_bounds_cap);
+		s->r.top /= factor;
+		s->r.height /= factor;
+		if (dev->has_scaler_cap) {
+			struct v4l2_rect fmt = dev->fmt_cap_rect;
+			struct v4l2_rect max_rect = {
+				0, 0,
+				s->r.width * MAX_ZOOM,
+				s->r.height * MAX_ZOOM
+			};
+			struct v4l2_rect min_rect = {
+				0, 0,
+				s->r.width / MAX_ZOOM,
+				s->r.height / MAX_ZOOM
+			};
+
+			rect_set_min_size(&fmt, &min_rect);
+			if (!dev->has_compose_cap)
+				rect_set_max_size(&fmt, &max_rect);
+			if (!rect_same_size(&dev->fmt_cap_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_cap_q))
+				return -EBUSY;
+			if (dev->has_compose_cap) {
+				rect_set_min_size(compose, &min_rect);
+				rect_set_max_size(compose, &max_rect);
+			}
+			dev->fmt_cap_rect = fmt;
+			tpg_s_buf_height(&dev->tpg, fmt.height);
+		} else if (dev->has_compose_cap) {
+			struct v4l2_rect fmt = dev->fmt_cap_rect;
+
+			rect_set_min_size(&fmt, &s->r);
+			if (!rect_same_size(&dev->fmt_cap_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_cap_q))
+				return -EBUSY;
+			dev->fmt_cap_rect = fmt;
+			tpg_s_buf_height(&dev->tpg, fmt.height);
+			rect_set_size_to(compose, &s->r);
+			rect_map_inside(compose, &dev->fmt_cap_rect);
+		} else {
+			if (!rect_same_size(&s->r, &dev->fmt_cap_rect) &&
+			    vb2_is_busy(&dev->vb_vid_cap_q))
+				return -EBUSY;
+			rect_set_size_to(&dev->fmt_cap_rect, &s->r);
+			rect_set_size_to(compose, &s->r);
+			rect_map_inside(compose, &dev->fmt_cap_rect);
+			tpg_s_buf_height(&dev->tpg, dev->fmt_cap_rect.height);
+		}
+		s->r.top *= factor;
+		s->r.height *= factor;
+		*crop = s->r;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_cap)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->fmt_cap_rect);
+		if (dev->has_scaler_cap) {
+			struct v4l2_rect max_rect = {
+				0, 0,
+				dev->src_rect.width * MAX_ZOOM,
+				(dev->src_rect.height / factor) * MAX_ZOOM
+			};
+
+			rect_set_max_size(&s->r, &max_rect);
+			if (dev->has_crop_cap) {
+				struct v4l2_rect min_rect = {
+					0, 0,
+					s->r.width / MAX_ZOOM,
+					(s->r.height * factor) / MAX_ZOOM
+				};
+				struct v4l2_rect max_rect = {
+					0, 0,
+					s->r.width * MAX_ZOOM,
+					(s->r.height * factor) * MAX_ZOOM
+				};
+
+				rect_set_min_size(crop, &min_rect);
+				rect_set_max_size(crop, &max_rect);
+				rect_map_inside(crop, &dev->crop_bounds_cap);
+			}
+		} else if (dev->has_crop_cap) {
+			s->r.top *= factor;
+			s->r.height *= factor;
+			rect_set_max_size(&s->r, &dev->src_rect);
+			rect_set_size_to(crop, &s->r);
+			rect_map_inside(crop, &dev->crop_bounds_cap);
+			s->r.top /= factor;
+			s->r.height /= factor;
+		} else {
+			rect_set_size_to(&s->r, &dev->src_rect);
+			s->r.height /= factor;
+		}
+		rect_map_inside(&s->r, &dev->fmt_cap_rect);
+		if (dev->bitmap_cap && (compose->width != s->r.width ||
+					compose->height != s->r.height)) {
+			kfree(dev->bitmap_cap);
+			dev->bitmap_cap = NULL;
+		}
+		*compose = s->r;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	tpg_s_crop_compose(&dev->tpg, crop, compose);
+	return 0;
+}
+
+int vivid_vid_cap_cropcap(struct file *file, void *priv,
+			      struct v4l2_cropcap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (cap->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
+		return -EINVAL;
+
+	switch (vivid_get_pixel_aspect(dev)) {
+	case TPG_PIXEL_ASPECT_NTSC:
+		cap->pixelaspect.numerator = 11;
+		cap->pixelaspect.denominator = 10;
+		break;
+	case TPG_PIXEL_ASPECT_PAL:
+		cap->pixelaspect.numerator = 54;
+		cap->pixelaspect.denominator = 59;
+		break;
+	case TPG_PIXEL_ASPECT_SQUARE:
+		cap->pixelaspect.numerator = 1;
+		cap->pixelaspect.denominator = 1;
+		break;
+	}
+	return 0;
+}
+
+int vidioc_enum_fmt_vid_overlay(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	const struct vivid_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(formats_ovl))
+		return -EINVAL;
+
+	fmt = &formats_ovl[f->index];
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_cap;
+	struct v4l2_window *win = &f->fmt.win;
+	unsigned clipcount = win->clipcount;
+
+	win->w.top = dev->overlay_cap_top;
+	win->w.left = dev->overlay_cap_left;
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	win->field = dev->overlay_cap_field;
+	win->clipcount = dev->clipcount_cap;
+	if (clipcount > dev->clipcount_cap)
+		clipcount = dev->clipcount_cap;
+	if (dev->bitmap_cap == NULL)
+		win->bitmap = NULL;
+	else if (win->bitmap) {
+		if (copy_to_user(win->bitmap, dev->bitmap_cap,
+		    ((compose->width + 7) / 8) * compose->height))
+			return -EFAULT;
+	}
+	if (clipcount && win->clips) {
+		if (copy_to_user(win->clips, dev->clips_cap,
+				 clipcount * sizeof(dev->clips_cap[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_cap;
+	struct v4l2_window *win = &f->fmt.win;
+	int i, j;
+
+	win->w.left = clamp_t(int, win->w.left,
+			      -dev->fb_cap.fmt.width, dev->fb_cap.fmt.width);
+	win->w.top = clamp_t(int, win->w.top,
+			     -dev->fb_cap.fmt.height, dev->fb_cap.fmt.height);
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	if (win->field != V4L2_FIELD_BOTTOM && win->field != V4L2_FIELD_TOP)
+		win->field = V4L2_FIELD_ANY;
+	win->chromakey = 0;
+	win->global_alpha = 0;
+	if (win->clipcount && !win->clips)
+		win->clipcount = 0;
+	if (win->clipcount > MAX_CLIPS)
+		win->clipcount = MAX_CLIPS;
+	if (win->clipcount) {
+		if (copy_from_user(dev->try_clips_cap, win->clips,
+				   win->clipcount * sizeof(dev->clips_cap[0])))
+			return -EFAULT;
+		for (i = 0; i < win->clipcount; i++) {
+			struct v4l2_rect *r = &dev->try_clips_cap[i].c;
+
+			r->top = clamp_t(s32, r->top, 0, dev->fb_cap.fmt.height - 1);
+			r->height = clamp_t(s32, r->height, 1, dev->fb_cap.fmt.height - r->top);
+			r->left = clamp_t(u32, r->left, 0, dev->fb_cap.fmt.width - 1);
+			r->width = clamp_t(u32, r->width, 1, dev->fb_cap.fmt.width - r->left);
+		}
+		/*
+		 * Yeah, so sue me, it's an O(n^2) algorithm. But n is a small
+		 * number and it's typically a one-time deal.
+		 */
+		for (i = 0; i < win->clipcount - 1; i++) {
+			struct v4l2_rect *r1 = &dev->try_clips_cap[i].c;
+
+			for (j = i + 1; j < win->clipcount; j++) {
+				struct v4l2_rect *r2 = &dev->try_clips_cap[j].c;
+
+				if (rect_overlap(r1, r2))
+					return -EINVAL;
+			}
+		}
+		if (copy_to_user(win->clips, dev->try_clips_cap,
+				 win->clipcount * sizeof(dev->clips_cap[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_cap;
+	struct v4l2_window *win = &f->fmt.win;
+	int ret = vidioc_try_fmt_vid_overlay(file, priv, f);
+	unsigned bitmap_size = ((compose->width + 7) / 8) * compose->height;
+	unsigned clips_size = win->clipcount * sizeof(dev->clips_cap[0]);
+	void *new_bitmap = NULL;
+
+	if (ret)
+		return ret;
+
+	if (win->bitmap) {
+		new_bitmap = vzalloc(bitmap_size);
+
+		if (new_bitmap == NULL)
+			return -ENOMEM;
+		if (copy_from_user(new_bitmap, win->bitmap, bitmap_size)) {
+			vfree(new_bitmap);
+			return -EFAULT;
+		}
+	}
+
+	dev->overlay_cap_top = win->w.top;
+	dev->overlay_cap_left = win->w.left;
+	dev->overlay_cap_field = win->field;
+	vfree(dev->bitmap_cap);
+	dev->bitmap_cap = new_bitmap;
+	dev->clipcount_cap = win->clipcount;
+	if (dev->clipcount_cap)
+		memcpy(dev->clips_cap, dev->try_clips_cap, clips_size);
+	return 0;
+}
+
+int vivid_vid_cap_overlay(struct file *file, void *fh, unsigned i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (i && dev->fb_vbase_cap == NULL)
+		return -EINVAL;
+
+	if (i && dev->fb_cap.fmt.pixelformat != dev->fmt_cap->fourcc) {
+		dprintk(dev, 1, "mismatch between overlay and video capture pixelformats\n");
+		return -EINVAL;
+	}
+
+	if (dev->overlay_cap_owner && dev->overlay_cap_owner != fh)
+		return -EBUSY;
+	dev->overlay_cap_owner = i ? fh : NULL;
+	return 0;
+}
+
+int vivid_vid_cap_g_fbuf(struct file *file, void *fh,
+				struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	*a = dev->fb_cap;
+	a->capability = V4L2_FBUF_CAP_BITMAP_CLIPPING |
+			V4L2_FBUF_CAP_LIST_CLIPPING;
+	a->flags = V4L2_FBUF_FLAG_PRIMARY;
+	a->fmt.field = V4L2_FIELD_NONE;
+	a->fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	a->fmt.priv = 0;
+	return 0;
+}
+
+int vivid_vid_cap_s_fbuf(struct file *file, void *fh,
+				const struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+
+	if (!capable(CAP_SYS_ADMIN) && !capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	if (dev->overlay_cap_owner)
+		return -EBUSY;
+
+	if (a->base == NULL) {
+		dev->fb_cap.base = NULL;
+		dev->fb_vbase_cap = NULL;
+		return 0;
+	}
+
+	if (a->fmt.width < 48 || a->fmt.height < 32)
+		return -EINVAL;
+	fmt = vivid_get_format(dev, a->fmt.pixelformat);
+	if (!fmt || !fmt->can_do_overlay)
+		return -EINVAL;
+	if (a->fmt.bytesperline < (a->fmt.width * fmt->depth) / 8)
+		return -EINVAL;
+	if (a->fmt.height * a->fmt.bytesperline < a->fmt.sizeimage)
+		return -EINVAL;
+
+	dev->fb_vbase_cap = phys_to_virt((unsigned long)a->base);
+	dev->fb_cap = *a;
+	dev->overlay_cap_left = clamp_t(int, dev->overlay_cap_left,
+				    -dev->fb_cap.fmt.width, dev->fb_cap.fmt.width);
+	dev->overlay_cap_top = clamp_t(int, dev->overlay_cap_top,
+				   -dev->fb_cap.fmt.height, dev->fb_cap.fmt.height);
+	return 0;
+}
+
+static const struct v4l2_audio vivid_audio_inputs[] = {
+	{ 0, "TV", V4L2_AUDCAP_STEREO },
+	{ 1, "Line-In", V4L2_AUDCAP_STEREO },
+};
+
+int vidioc_enum_input(struct file *file, void *priv,
+				struct v4l2_input *inp)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (inp->index >= dev->num_inputs)
+		return -EINVAL;
+
+	inp->type = V4L2_INPUT_TYPE_CAMERA;
+	switch (dev->input_type[inp->index]) {
+	case WEBCAM:
+		snprintf(inp->name, sizeof(inp->name), "Webcam %u",
+				dev->input_name_counter[inp->index]);
+		inp->capabilities = 0;
+		break;
+	case TV:
+		snprintf(inp->name, sizeof(inp->name), "TV %u",
+				dev->input_name_counter[inp->index]);
+		inp->type = V4L2_INPUT_TYPE_TUNER;
+		inp->std = V4L2_STD_ALL;
+		if (dev->has_audio_inputs)
+			inp->audioset = (1 << ARRAY_SIZE(vivid_audio_inputs)) - 1;
+		inp->capabilities = V4L2_IN_CAP_STD;
+		break;
+	case SVID:
+		snprintf(inp->name, sizeof(inp->name), "S-Video %u",
+				dev->input_name_counter[inp->index]);
+		inp->std = V4L2_STD_ALL;
+		if (dev->has_audio_inputs)
+			inp->audioset = (1 << ARRAY_SIZE(vivid_audio_inputs)) - 1;
+		inp->capabilities = V4L2_IN_CAP_STD;
+		break;
+	case HDMI:
+		snprintf(inp->name, sizeof(inp->name), "HDMI %u",
+				dev->input_name_counter[inp->index]);
+		inp->capabilities = V4L2_IN_CAP_DV_TIMINGS;
+		if (dev->edid_blocks == 0 ||
+		    dev->dv_timings_signal_mode == NO_SIGNAL)
+			inp->status |= V4L2_IN_ST_NO_SIGNAL;
+		else if (dev->dv_timings_signal_mode == NO_LOCK ||
+			 dev->dv_timings_signal_mode == OUT_OF_RANGE)
+			inp->status |= V4L2_IN_ST_NO_H_LOCK;
+		break;
+	}
+	if (dev->sensor_hflip)
+		inp->status |= V4L2_IN_ST_HFLIP;
+	if (dev->sensor_vflip)
+		inp->status |= V4L2_IN_ST_VFLIP;
+	if (dev->input == inp->index && vivid_is_sdtv_cap(dev)) {
+		if (dev->std_signal_mode == NO_SIGNAL) {
+			inp->status |= V4L2_IN_ST_NO_SIGNAL;
+		} else if (dev->std_signal_mode == NO_LOCK) {
+			inp->status |= V4L2_IN_ST_NO_H_LOCK;
+		} else if (vivid_is_tv_cap(dev)) {
+			switch (tpg_g_quality(&dev->tpg)) {
+			case TPG_QUAL_GRAY:
+				inp->status |= V4L2_IN_ST_COLOR_KILL;
+				break;
+			case TPG_QUAL_NOISE:
+				inp->status |= V4L2_IN_ST_NO_H_LOCK;
+				break;
+			default:
+				break;
+			}
+		}
+	}
+	return 0;
+}
+
+int vidioc_g_input(struct file *file, void *priv, unsigned *i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	*i = dev->input;
+	return 0;
+}
+
+int vidioc_s_input(struct file *file, void *priv, unsigned i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_bt_timings *bt = &dev->dv_timings_cap.bt;
+	unsigned brightness;
+
+	if (i >= dev->num_inputs)
+		return -EINVAL;
+
+	if (i == dev->input)
+		return 0;
+
+	if (vb2_is_busy(&dev->vb_vid_cap_q) || vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+
+	dev->input = i;
+	dev->vid_cap_dev.tvnorms = 0;
+	if (dev->input_type[i] == TV || dev->input_type[i] == SVID) {
+		dev->tv_audio_input = (dev->input_type[i] == TV) ? 0 : 1;
+		dev->vid_cap_dev.tvnorms = V4L2_STD_ALL;
+	}
+	dev->vbi_cap_dev.tvnorms = dev->vid_cap_dev.tvnorms;
+	vivid_update_format_cap(dev, false);
+
+	if (dev->colorspace) {
+		switch (dev->input_type[i]) {
+		case WEBCAM:
+			v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_SRGB);
+			break;
+		case TV:
+		case SVID:
+			v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_SMPTE170M);
+			break;
+		case HDMI:
+			if (bt->standards & V4L2_DV_BT_STD_CEA861) {
+				if (dev->src_rect.width == 720 && dev->src_rect.height <= 576)
+					v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_SMPTE170M);
+				else
+					v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_REC709);
+			} else {
+				v4l2_ctrl_s_ctrl(dev->colorspace, V4L2_COLORSPACE_SRGB);
+			}
+			break;
+		}
+	}
+
+	/*
+	 * Modify the brightness range depending on the input.
+	 * This makes it easy to use vivid to test if applications can
+	 * handle control range modifications and is also how this is
+	 * typically used in practice as different inputs may be hooked
+	 * up to different receivers with different control ranges.
+	 */
+	brightness = 128 * i + dev->input_brightness[i];
+	v4l2_ctrl_modify_range(dev->brightness,
+			128 * i, 255 + 128 * i, 1, 128 + 128 * i);
+	v4l2_ctrl_s_ctrl(dev->brightness, brightness);
+	return 0;
+}
+
+int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *vin)
+{
+	if (vin->index >= ARRAY_SIZE(vivid_audio_inputs))
+		return -EINVAL;
+	*vin = vivid_audio_inputs[vin->index];
+	return 0;
+}
+
+int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *vin)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -EINVAL;
+	*vin = vivid_audio_inputs[dev->tv_audio_input];
+	return 0;
+}
+
+int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *vin)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -EINVAL;
+	if (vin->index >= ARRAY_SIZE(vivid_audio_inputs))
+		return -EINVAL;
+	dev->tv_audio_input = vin->index;
+	return 0;
+}
+
+int vivid_video_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+	vf->frequency = dev->tv_freq;
+	return 0;
+}
+
+int vivid_video_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vf->tuner != 0)
+		return -EINVAL;
+	dev->tv_freq = clamp_t(unsigned, vf->frequency, MIN_TV_FREQ, MAX_TV_FREQ);
+	if (vivid_is_tv_cap(dev))
+		vivid_update_quality(dev);
+	return 0;
+}
+
+int vivid_video_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (vt->index != 0)
+		return -EINVAL;
+	if (vt->audmode > V4L2_TUNER_MODE_LANG1_LANG2)
+		return -EINVAL;
+	dev->tv_audmode = vt->audmode;
+	return 0;
+}
+
+int vivid_video_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	enum tpg_quality qual;
+
+	if (vt->index != 0)
+		return -EINVAL;
+
+	vt->capability = V4L2_TUNER_CAP_NORM | V4L2_TUNER_CAP_STEREO |
+			 V4L2_TUNER_CAP_LANG1 | V4L2_TUNER_CAP_LANG2;
+	vt->audmode = dev->tv_audmode;
+	vt->rangelow = MIN_TV_FREQ;
+	vt->rangehigh = MAX_TV_FREQ;
+	qual = vivid_get_quality(dev, &vt->afc);
+	if (qual == TPG_QUAL_COLOR)
+		vt->signal = 0xffff;
+	else if (qual == TPG_QUAL_GRAY)
+		vt->signal = 0x8000;
+	else
+		vt->signal = 0;
+	if (qual == TPG_QUAL_NOISE) {
+		vt->rxsubchans = 0;
+	} else if (qual == TPG_QUAL_GRAY) {
+		vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+	} else {
+		unsigned channel_nr = dev->tv_freq / (6 * 16);
+		unsigned options = (dev->std_cap & V4L2_STD_NTSC_M) ? 4 : 3;
+
+		switch (channel_nr % options) {
+		case 0:
+			vt->rxsubchans = V4L2_TUNER_SUB_MONO;
+			break;
+		case 1:
+			vt->rxsubchans = V4L2_TUNER_SUB_STEREO;
+			break;
+		case 2:
+			if (dev->std_cap & V4L2_STD_NTSC_M)
+				vt->rxsubchans = V4L2_TUNER_SUB_MONO | V4L2_TUNER_SUB_SAP;
+			else
+				vt->rxsubchans = V4L2_TUNER_SUB_LANG1 | V4L2_TUNER_SUB_LANG2;
+			break;
+		case 3:
+			vt->rxsubchans = V4L2_TUNER_SUB_STEREO | V4L2_TUNER_SUB_SAP;
+			break;
+		}
+	}
+	strlcpy(vt->name, "TV Tuner", sizeof(vt->name));
+	return 0;
+}
+
+/* Must remain in sync with the vivid_ctrl_standard_strings array */
+const v4l2_std_id vivid_standard[] = {
+	V4L2_STD_NTSC_M,
+	V4L2_STD_NTSC_M_JP,
+	V4L2_STD_NTSC_M_KR,
+	V4L2_STD_NTSC_443,
+	V4L2_STD_PAL_BG | V4L2_STD_PAL_H,
+	V4L2_STD_PAL_I,
+	V4L2_STD_PAL_DK,
+	V4L2_STD_PAL_M,
+	V4L2_STD_PAL_N,
+	V4L2_STD_PAL_Nc,
+	V4L2_STD_PAL_60,
+	V4L2_STD_SECAM_B | V4L2_STD_SECAM_G | V4L2_STD_SECAM_H,
+	V4L2_STD_SECAM_DK,
+	V4L2_STD_SECAM_L,
+	V4L2_STD_SECAM_LC,
+	V4L2_STD_UNKNOWN
+};
+
+/* Must remain in sync with the vivid_standard array */
+const char * const vivid_ctrl_standard_strings[] = {
+	"NTSC-M",
+	"NTSC-M-JP",
+	"NTSC-M-KR",
+	"NTSC-443",
+	"PAL-BGH",
+	"PAL-I",
+	"PAL-DK",
+	"PAL-M",
+	"PAL-N",
+	"PAL-Nc",
+	"PAL-60",
+	"SECAM-BGH",
+	"SECAM-DK",
+	"SECAM-L",
+	"SECAM-Lc",
+	NULL,
+};
+
+int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -ENODATA;
+	if (dev->std_signal_mode == NO_SIGNAL ||
+	    dev->std_signal_mode == NO_LOCK) {
+		*id = V4L2_STD_UNKNOWN;
+		return 0;
+	}
+	if (vivid_is_tv_cap(dev) && tpg_g_quality(&dev->tpg) == TPG_QUAL_NOISE) {
+		*id = V4L2_STD_UNKNOWN;
+	} else if (dev->std_signal_mode == CURRENT_STD) {
+		*id = dev->std_cap;
+	} else if (dev->std_signal_mode == SELECTED_STD) {
+		*id = dev->query_std;
+	} else {
+		*id = vivid_standard[dev->query_std_last];
+		dev->query_std_last = (dev->query_std_last + 1) % ARRAY_SIZE(vivid_standard);
+	}
+
+	return 0;
+}
+
+int vivid_vid_cap_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_sdtv_cap(dev))
+		return -ENODATA;
+	if (dev->std_cap == id)
+		return 0;
+	if (vb2_is_busy(&dev->vb_vid_cap_q) || vb2_is_busy(&dev->vb_vbi_cap_q))
+		return -EBUSY;
+	dev->std_cap = id;
+	vivid_update_format_cap(dev, false);
+	return 0;
+}
+
+int vivid_vid_cap_s_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_hdmi_cap(dev))
+		return -ENODATA;
+	if (vb2_is_busy(&dev->vb_vid_cap_q))
+		return -EBUSY;
+	if (!v4l2_find_dv_timings_cap(timings, &vivid_dv_timings_cap,
+				0, NULL, NULL))
+		return -EINVAL;
+	if (v4l2_match_dv_timings(timings, &dev->dv_timings_cap, 0))
+		return 0;
+	dev->dv_timings_cap = *timings;
+	vivid_update_format_cap(dev, false);
+	return 0;
+}
+
+int vidioc_query_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_hdmi_cap(dev))
+		return -ENODATA;
+	if (dev->dv_timings_signal_mode == NO_SIGNAL ||
+	    dev->edid_blocks == 0)
+		return -ENOLINK;
+	if (dev->dv_timings_signal_mode == NO_LOCK)
+		return -ENOLCK;
+	if (dev->dv_timings_signal_mode == OUT_OF_RANGE) {
+		timings->bt.pixelclock = vivid_dv_timings_cap.bt.max_pixelclock * 2;
+		return -ERANGE;
+	}
+	if (dev->dv_timings_signal_mode == CURRENT_DV_TIMINGS) {
+		*timings = dev->dv_timings_cap;
+	} else if (dev->dv_timings_signal_mode == SELECTED_DV_TIMINGS) {
+		*timings = v4l2_dv_timings_presets[dev->query_dv_timings];
+	} else {
+		*timings = v4l2_dv_timings_presets[dev->query_dv_timings_last];
+		dev->query_dv_timings_last = (dev->query_dv_timings_last + 1) %
+						dev->query_dv_timings_size;
+	}
+	return 0;
+}
+
+int vidioc_s_edid(struct file *file, void *_fh,
+			 struct v4l2_edid *edid)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+	if (edid->pad >= dev->num_inputs)
+		return -EINVAL;
+	if (dev->input_type[edid->pad] != HDMI || edid->start_block)
+		return -EINVAL;
+	if (edid->blocks == 0) {
+		dev->edid_blocks = 0;
+		return 0;
+	}
+	if (edid->blocks > dev->edid_max_blocks) {
+		edid->blocks = dev->edid_max_blocks;
+		return -E2BIG;
+	}
+	dev->edid_blocks = edid->blocks;
+	memcpy(dev->edid, edid->edid, edid->blocks * 128);
+	return 0;
+}
+
+int vidioc_enum_framesizes(struct file *file, void *fh,
+					 struct v4l2_frmsizeenum *fsize)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_webcam(dev) && !dev->has_scaler_cap)
+		return -EINVAL;
+	if (vivid_get_format(dev, fsize->pixel_format) == NULL)
+		return -EINVAL;
+	if (vivid_is_webcam(dev)) {
+		if (fsize->index >= ARRAY_SIZE(webcam_sizes))
+			return -EINVAL;
+		fsize->type = V4L2_FRMSIZE_TYPE_DISCRETE;
+		fsize->discrete = webcam_sizes[fsize->index];
+		return 0;
+	}
+	if (fsize->index)
+		return -EINVAL;
+	fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
+	fsize->stepwise.min_width = MIN_WIDTH;
+	fsize->stepwise.max_width = MAX_WIDTH * MAX_ZOOM;
+	fsize->stepwise.step_width = 2;
+	fsize->stepwise.min_height = MIN_HEIGHT;
+	fsize->stepwise.max_height = MAX_HEIGHT * MAX_ZOOM;
+	fsize->stepwise.step_height = 2;
+	return 0;
+}
+
+/* timeperframe is arbitrary and continuous */
+int vidioc_enum_frameintervals(struct file *file, void *priv,
+					     struct v4l2_frmivalenum *fival)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+	int i;
+
+	fmt = vivid_get_format(dev, fival->pixel_format);
+	if (!fmt)
+		return -EINVAL;
+
+	if (!vivid_is_webcam(dev)) {
+		static const struct v4l2_fract step = { 1, 1 };
+
+		if (fival->index)
+			return -EINVAL;
+		if (fival->width < MIN_WIDTH || fival->width > MAX_WIDTH * MAX_ZOOM)
+			return -EINVAL;
+		if (fival->height < MIN_HEIGHT || fival->height > MAX_HEIGHT * MAX_ZOOM)
+			return -EINVAL;
+		fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
+		fival->stepwise.min = tpf_min;
+		fival->stepwise.max = tpf_max;
+		fival->stepwise.step = step;
+		return 0;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(webcam_sizes); i++)
+		if (fival->width == webcam_sizes[i].width &&
+		    fival->height == webcam_sizes[i].height)
+			break;
+	if (i == ARRAY_SIZE(webcam_sizes))
+		return -EINVAL;
+	if (fival->index >= 2 * (3 - i))
+		return -EINVAL;
+	fival->type = V4L2_FRMIVAL_TYPE_DISCRETE;
+	fival->discrete = webcam_intervals[fival->index];
+	return 0;
+}
+
+int vivid_vid_cap_g_parm(struct file *file, void *priv,
+			  struct v4l2_streamparm *parm)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (parm->type != (dev->multiplanar ?
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE))
+		return -EINVAL;
+
+	parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
+	parm->parm.capture.timeperframe = dev->timeperframe_vid_cap;
+	parm->parm.capture.readbuffers  = 1;
+	return 0;
+}
+
+#define FRACT_CMP(a, OP, b)	\
+	((u64)(a).numerator * (b).denominator  OP  (u64)(b).numerator * (a).denominator)
+
+int vivid_vid_cap_s_parm(struct file *file, void *priv,
+			  struct v4l2_streamparm *parm)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	unsigned ival_sz = 2 * (3 - dev->webcam_size_idx);
+	struct v4l2_fract tpf;
+	unsigned i;
+
+	if (parm->type != (dev->multiplanar ?
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE :
+			   V4L2_BUF_TYPE_VIDEO_CAPTURE))
+		return -EINVAL;
+	if (!vivid_is_webcam(dev))
+		return vivid_vid_cap_g_parm(file, priv, parm);
+
+	tpf = parm->parm.capture.timeperframe;
+
+	if (tpf.denominator == 0)
+		tpf = webcam_intervals[ival_sz - 1];
+	for (i = 0; i < ival_sz; i++)
+		if (FRACT_CMP(tpf, >=, webcam_intervals[i]))
+			break;
+	if (i == ival_sz)
+		i = ival_sz - 1;
+	dev->webcam_ival_idx = i;
+	tpf = webcam_intervals[dev->webcam_ival_idx];
+	tpf = FRACT_CMP(tpf, <, tpf_min) ? tpf_min : tpf;
+	tpf = FRACT_CMP(tpf, >, tpf_max) ? tpf_max : tpf;
+
+	/* resync the thread's timings */
+	dev->cap_seq_resync = true;
+	dev->timeperframe_vid_cap = tpf;
+	parm->parm.capture.timeperframe = tpf;
+	parm->parm.capture.readbuffers  = 1;
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-vid-cap.h b/drivers/media/platform/vivid/vivid-vid-cap.h
new file mode 100644
index 000000000000..94079815dbc2
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-cap.h
@@ -0,0 +1,71 @@
+/*
+ * vivid-vid-cap.h - video capture support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VID_CAP_H_
+#define _VIVID_VID_CAP_H_
+
+void vivid_update_quality(struct vivid_dev *dev);
+void vivid_update_format_cap(struct vivid_dev *dev, bool keep_controls);
+enum tpg_video_aspect vivid_get_video_aspect(const struct vivid_dev *dev);
+
+extern const v4l2_std_id vivid_standard[];
+extern const char * const vivid_ctrl_standard_strings[];
+
+extern const struct vb2_ops vivid_vid_cap_qops;
+
+int vivid_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_cap_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_cap(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_cap_g_selection(struct file *file, void *priv, struct v4l2_selection *sel);
+int vivid_vid_cap_s_selection(struct file *file, void *fh, struct v4l2_selection *s);
+int vivid_vid_cap_cropcap(struct file *file, void *priv, struct v4l2_cropcap *cap);
+int vidioc_enum_fmt_vid_overlay(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_cap_overlay(struct file *file, void *fh, unsigned i);
+int vivid_vid_cap_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a);
+int vivid_vid_cap_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a);
+int vidioc_enum_input(struct file *file, void *priv, struct v4l2_input *inp);
+int vidioc_g_input(struct file *file, void *priv, unsigned *i);
+int vidioc_s_input(struct file *file, void *priv, unsigned i);
+int vidioc_enumaudio(struct file *file, void *fh, struct v4l2_audio *vin);
+int vidioc_g_audio(struct file *file, void *fh, struct v4l2_audio *vin);
+int vidioc_s_audio(struct file *file, void *fh, const struct v4l2_audio *vin);
+int vivid_video_g_frequency(struct file *file, void *fh, struct v4l2_frequency *vf);
+int vivid_video_s_frequency(struct file *file, void *fh, const struct v4l2_frequency *vf);
+int vivid_video_s_tuner(struct file *file, void *fh, const struct v4l2_tuner *vt);
+int vivid_video_g_tuner(struct file *file, void *fh, struct v4l2_tuner *vt);
+int vidioc_querystd(struct file *file, void *priv, v4l2_std_id *id);
+int vivid_vid_cap_s_std(struct file *file, void *priv, v4l2_std_id id);
+int vivid_vid_cap_s_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_query_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_s_edid(struct file *file, void *_fh, struct v4l2_edid *edid);
+int vidioc_enum_framesizes(struct file *file, void *fh, struct v4l2_frmsizeenum *fsize);
+int vidioc_enum_frameintervals(struct file *file, void *priv, struct v4l2_frmivalenum *fival);
+int vivid_vid_cap_g_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+int vivid_vid_cap_s_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vid-common.c b/drivers/media/platform/vivid/vivid-vid-common.c
new file mode 100644
index 000000000000..16cd6d2d2ed6
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-common.c
@@ -0,0 +1,571 @@
+/*
+ * vivid-vid-common.c - common video support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+
+const struct v4l2_dv_timings_cap vivid_dv_timings_cap = {
+	.type = V4L2_DV_BT_656_1120,
+	/* keep this initialization for compatibility with GCC < 4.4.6 */
+	.reserved = { 0 },
+	V4L2_INIT_BT_TIMINGS(0, MAX_WIDTH, 0, MAX_HEIGHT, 25000000, 600000000,
+		V4L2_DV_BT_STD_CEA861 | V4L2_DV_BT_STD_DMT,
+		V4L2_DV_BT_CAP_PROGRESSIVE | V4L2_DV_BT_CAP_INTERLACED)
+};
+
+/* ------------------------------------------------------------------
+	Basic structures
+   ------------------------------------------------------------------*/
+
+struct vivid_fmt vivid_formats[] = {
+	{
+		.name     = "4:2:2, packed, YUYV",
+		.fourcc   = V4L2_PIX_FMT_YUYV,
+		.depth    = 16,
+		.is_yuv   = true,
+		.planes   = 1,
+		.data_offset = { PLANE0_DATA_OFFSET, 0 },
+	},
+	{
+		.name     = "4:2:2, packed, UYVY",
+		.fourcc   = V4L2_PIX_FMT_UYVY,
+		.depth    = 16,
+		.is_yuv   = true,
+		.planes   = 1,
+	},
+	{
+		.name     = "4:2:2, packed, YVYU",
+		.fourcc   = V4L2_PIX_FMT_YVYU,
+		.depth    = 16,
+		.is_yuv   = true,
+		.planes   = 1,
+	},
+	{
+		.name     = "4:2:2, packed, VYUY",
+		.fourcc   = V4L2_PIX_FMT_VYUY,
+		.depth    = 16,
+		.is_yuv   = true,
+		.planes   = 1,
+	},
+	{
+		.name     = "RGB565 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB565, /* gggbbbbb rrrrrggg */
+		.depth    = 16,
+		.planes   = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.name     = "RGB565 (BE)",
+		.fourcc   = V4L2_PIX_FMT_RGB565X, /* rrrrrggg gggbbbbb */
+		.depth    = 16,
+		.planes   = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.name     = "RGB555 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB555, /* gggbbbbb arrrrrgg */
+		.depth    = 16,
+		.planes   = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.name     = "XRGB555 (LE)",
+		.fourcc   = V4L2_PIX_FMT_XRGB555, /* gggbbbbb arrrrrgg */
+		.depth    = 16,
+		.planes   = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.name     = "ARGB555 (LE)",
+		.fourcc   = V4L2_PIX_FMT_ARGB555, /* gggbbbbb arrrrrgg */
+		.depth    = 16,
+		.planes   = 1,
+		.can_do_overlay = true,
+		.alpha_mask = 0x8000,
+	},
+	{
+		.name     = "RGB555 (BE)",
+		.fourcc   = V4L2_PIX_FMT_RGB555X, /* arrrrrgg gggbbbbb */
+		.depth    = 16,
+		.planes   = 1,
+		.can_do_overlay = true,
+	},
+	{
+		.name     = "RGB24 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB24, /* rgb */
+		.depth    = 24,
+		.planes   = 1,
+	},
+	{
+		.name     = "RGB24 (BE)",
+		.fourcc   = V4L2_PIX_FMT_BGR24, /* bgr */
+		.depth    = 24,
+		.planes   = 1,
+	},
+	{
+		.name     = "RGB32 (LE)",
+		.fourcc   = V4L2_PIX_FMT_RGB32, /* argb */
+		.depth    = 32,
+		.planes   = 1,
+	},
+	{
+		.name     = "RGB32 (BE)",
+		.fourcc   = V4L2_PIX_FMT_BGR32, /* bgra */
+		.depth    = 32,
+		.planes   = 1,
+	},
+	{
+		.name     = "XRGB32 (LE)",
+		.fourcc   = V4L2_PIX_FMT_XRGB32, /* argb */
+		.depth    = 32,
+		.planes   = 1,
+	},
+	{
+		.name     = "XRGB32 (BE)",
+		.fourcc   = V4L2_PIX_FMT_XBGR32, /* bgra */
+		.depth    = 32,
+		.planes   = 1,
+	},
+	{
+		.name     = "ARGB32 (LE)",
+		.fourcc   = V4L2_PIX_FMT_ARGB32, /* argb */
+		.depth    = 32,
+		.planes   = 1,
+		.alpha_mask = 0x000000ff,
+	},
+	{
+		.name     = "ARGB32 (BE)",
+		.fourcc   = V4L2_PIX_FMT_ABGR32, /* bgra */
+		.depth    = 32,
+		.planes   = 1,
+		.alpha_mask = 0xff000000,
+	},
+	{
+		.name     = "4:2:2, planar, YUV",
+		.fourcc   = V4L2_PIX_FMT_NV16M,
+		.depth    = 8,
+		.is_yuv   = true,
+		.planes   = 2,
+		.data_offset = { PLANE0_DATA_OFFSET, 0 },
+	},
+	{
+		.name     = "4:2:2, planar, YVU",
+		.fourcc   = V4L2_PIX_FMT_NV61M,
+		.depth    = 8,
+		.is_yuv   = true,
+		.planes   = 2,
+		.data_offset = { 0, PLANE0_DATA_OFFSET },
+	},
+};
+
+/* There are 2 multiplanar formats in the list */
+#define VIVID_MPLANAR_FORMATS 2
+
+const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat)
+{
+	const struct vivid_fmt *fmt;
+	unsigned k;
+
+	for (k = 0; k < ARRAY_SIZE(vivid_formats); k++) {
+		fmt = &vivid_formats[k];
+		if (fmt->fourcc == pixelformat)
+			if (fmt->planes == 1 || dev->multiplanar)
+				return fmt;
+	}
+
+	return NULL;
+}
+
+bool vivid_vid_can_loop(struct vivid_dev *dev)
+{
+	if (dev->src_rect.width != dev->sink_rect.width ||
+	    dev->src_rect.height != dev->sink_rect.height)
+		return false;
+	if (dev->fmt_cap->fourcc != dev->fmt_out->fourcc)
+		return false;
+	if (dev->field_cap != dev->field_out)
+		return false;
+	if (vivid_is_svid_cap(dev) && vivid_is_svid_out(dev)) {
+		if (!(dev->std_cap & V4L2_STD_525_60) !=
+		    !(dev->std_out & V4L2_STD_525_60))
+			return false;
+		return true;
+	}
+	if (vivid_is_hdmi_cap(dev) && vivid_is_hdmi_out(dev))
+		return true;
+	return false;
+}
+
+void vivid_send_source_change(struct vivid_dev *dev, unsigned type)
+{
+	struct v4l2_event ev = {
+		.type = V4L2_EVENT_SOURCE_CHANGE,
+		.u.src_change.changes = V4L2_EVENT_SRC_CH_RESOLUTION,
+	};
+	unsigned i;
+
+	for (i = 0; i < dev->num_inputs; i++) {
+		ev.id = i;
+		if (dev->input_type[i] == type) {
+			if (video_is_registered(&dev->vid_cap_dev) && dev->has_vid_cap)
+				v4l2_event_queue(&dev->vid_cap_dev, &ev);
+			if (video_is_registered(&dev->vbi_cap_dev) && dev->has_vbi_cap)
+				v4l2_event_queue(&dev->vbi_cap_dev, &ev);
+		}
+	}
+}
+
+/*
+ * Conversion function that converts a single-planar format to a
+ * single-plane multiplanar format.
+ */
+void fmt_sp2mp(const struct v4l2_format *sp_fmt, struct v4l2_format *mp_fmt)
+{
+	struct v4l2_pix_format_mplane *mp = &mp_fmt->fmt.pix_mp;
+	struct v4l2_plane_pix_format *ppix = &mp->plane_fmt[0];
+	const struct v4l2_pix_format *pix = &sp_fmt->fmt.pix;
+	bool is_out = sp_fmt->type == V4L2_BUF_TYPE_VIDEO_OUTPUT;
+
+	memset(mp->reserved, 0, sizeof(mp->reserved));
+	mp_fmt->type = is_out ? V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+			   V4L2_CAP_VIDEO_CAPTURE_MPLANE;
+	mp->width = pix->width;
+	mp->height = pix->height;
+	mp->pixelformat = pix->pixelformat;
+	mp->field = pix->field;
+	mp->colorspace = pix->colorspace;
+	mp->num_planes = 1;
+	mp->flags = pix->flags;
+	ppix->sizeimage = pix->sizeimage;
+	ppix->bytesperline = pix->bytesperline;
+	memset(ppix->reserved, 0, sizeof(ppix->reserved));
+}
+
+int fmt_sp2mp_func(struct file *file, void *priv,
+		struct v4l2_format *f, fmtfunc func)
+{
+	struct v4l2_format fmt;
+	struct v4l2_pix_format_mplane *mp = &fmt.fmt.pix_mp;
+	struct v4l2_plane_pix_format *ppix = &mp->plane_fmt[0];
+	struct v4l2_pix_format *pix = &f->fmt.pix;
+	int ret;
+
+	/* Converts to a mplane format */
+	fmt_sp2mp(f, &fmt);
+	/* Passes it to the generic mplane format function */
+	ret = func(file, priv, &fmt);
+	/* Copies back the mplane data to the single plane format */
+	pix->width = mp->width;
+	pix->height = mp->height;
+	pix->pixelformat = mp->pixelformat;
+	pix->field = mp->field;
+	pix->colorspace = mp->colorspace;
+	pix->sizeimage = ppix->sizeimage;
+	pix->bytesperline = ppix->bytesperline;
+	pix->flags = mp->flags;
+	return ret;
+}
+
+/* v4l2_rect helper function: copy the width/height values */
+void rect_set_size_to(struct v4l2_rect *r, const struct v4l2_rect *size)
+{
+	r->width = size->width;
+	r->height = size->height;
+}
+
+/* v4l2_rect helper function: width and height of r should be >= min_size */
+void rect_set_min_size(struct v4l2_rect *r, const struct v4l2_rect *min_size)
+{
+	if (r->width < min_size->width)
+		r->width = min_size->width;
+	if (r->height < min_size->height)
+		r->height = min_size->height;
+}
+
+/* v4l2_rect helper function: width and height of r should be <= max_size */
+void rect_set_max_size(struct v4l2_rect *r, const struct v4l2_rect *max_size)
+{
+	if (r->width > max_size->width)
+		r->width = max_size->width;
+	if (r->height > max_size->height)
+		r->height = max_size->height;
+}
+
+/* v4l2_rect helper function: r should be inside boundary */
+void rect_map_inside(struct v4l2_rect *r, const struct v4l2_rect *boundary)
+{
+	rect_set_max_size(r, boundary);
+	if (r->left < boundary->left)
+		r->left = boundary->left;
+	if (r->top < boundary->top)
+		r->top = boundary->top;
+	if (r->left + r->width > boundary->width)
+		r->left = boundary->width - r->width;
+	if (r->top + r->height > boundary->height)
+		r->top = boundary->height - r->height;
+}
+
+/* v4l2_rect helper function: return true if r1 has the same size as r2 */
+bool rect_same_size(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
+{
+	return r1->width == r2->width && r1->height == r2->height;
+}
+
+/* v4l2_rect helper function: calculate the intersection of two rects */
+struct v4l2_rect rect_intersect(const struct v4l2_rect *a, const struct v4l2_rect *b)
+{
+	struct v4l2_rect r;
+	int right, bottom;
+
+	r.top = max(a->top, b->top);
+	r.left = max(a->left, b->left);
+	bottom = min(a->top + a->height, b->top + b->height);
+	right = min(a->left + a->width, b->left + b->width);
+	r.height = max(0, bottom - r.top);
+	r.width = max(0, right - r.left);
+	return r;
+}
+
+/*
+ * v4l2_rect helper function: scale rect r by to->width / from->width and
+ * to->height / from->height.
+ */
+void rect_scale(struct v4l2_rect *r, const struct v4l2_rect *from,
+				     const struct v4l2_rect *to)
+{
+	if (from->width == 0 || from->height == 0) {
+		r->left = r->top = r->width = r->height = 0;
+		return;
+	}
+	r->left = (((r->left - from->left) * to->width) / from->width) & ~1;
+	r->width = ((r->width * to->width) / from->width) & ~1;
+	r->top = ((r->top - from->top) * to->height) / from->height;
+	r->height = (r->height * to->height) / from->height;
+}
+
+bool rect_overlap(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
+{
+	/*
+	 * IF the left side of r1 is to the right of the right side of r2 OR
+	 *    the left side of r2 is to the right of the right side of r1 THEN
+	 * they do not overlap.
+	 */
+	if (r1->left >= r2->left + r2->width ||
+	    r2->left >= r1->left + r1->width)
+		return false;
+	/*
+	 * IF the top side of r1 is below the bottom of r2 OR
+	 *    the top side of r2 is below the bottom of r1 THEN
+	 * they do not overlap.
+	 */
+	if (r1->top >= r2->top + r2->height ||
+	    r2->top >= r1->top + r1->height)
+		return false;
+	return true;
+}
+int vivid_vid_adjust_sel(unsigned flags, struct v4l2_rect *r)
+{
+	unsigned w = r->width;
+	unsigned h = r->height;
+
+	if (!(flags & V4L2_SEL_FLAG_LE)) {
+		w++;
+		h++;
+		if (w < 2)
+			w = 2;
+		if (h < 2)
+			h = 2;
+	}
+	if (!(flags & V4L2_SEL_FLAG_GE)) {
+		if (w > MAX_WIDTH)
+			w = MAX_WIDTH;
+		if (h > MAX_HEIGHT)
+			h = MAX_HEIGHT;
+	}
+	w = w & ~1;
+	h = h & ~1;
+	if (w < 2 || h < 2)
+		return -ERANGE;
+	if (w > MAX_WIDTH || h > MAX_HEIGHT)
+		return -ERANGE;
+	if (r->top < 0)
+		r->top = 0;
+	if (r->left < 0)
+		r->left = 0;
+	r->left &= ~1;
+	r->top &= ~1;
+	if (r->left + w > MAX_WIDTH)
+		r->left = MAX_WIDTH - w;
+	if (r->top + h > MAX_HEIGHT)
+		r->top = MAX_HEIGHT - h;
+	if ((flags & (V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE)) ==
+			(V4L2_SEL_FLAG_GE | V4L2_SEL_FLAG_LE) &&
+	    (r->width != w || r->height != h))
+		return -ERANGE;
+	r->width = w;
+	r->height = h;
+	return 0;
+}
+
+int vivid_enum_fmt_vid(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct vivid_fmt *fmt;
+
+	if (f->index >= ARRAY_SIZE(vivid_formats) -
+	    (dev->multiplanar ? 0 : VIVID_MPLANAR_FORMATS))
+		return -EINVAL;
+
+	fmt = &vivid_formats[f->index];
+
+	strlcpy(f->description, fmt->name, sizeof(f->description));
+	f->pixelformat = fmt->fourcc;
+	return 0;
+}
+
+int vidioc_enum_fmt_vid_mplane(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_enum_fmt_vid(file, priv, f);
+}
+
+int vidioc_enum_fmt_vid(struct file *file, void  *priv,
+					struct v4l2_fmtdesc *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return vivid_enum_fmt_vid(file, priv, f);
+}
+
+int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_sdtv_cap(dev))
+			return -ENODATA;
+		*id = dev->std_cap;
+	} else {
+		if (!vivid_is_svid_out(dev))
+			return -ENODATA;
+		*id = dev->std_out;
+	}
+	return 0;
+}
+
+int vidioc_g_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_hdmi_cap(dev))
+			return -ENODATA;
+		*timings = dev->dv_timings_cap;
+	} else {
+		if (!vivid_is_hdmi_out(dev))
+			return -ENODATA;
+		*timings = dev->dv_timings_out;
+	}
+	return 0;
+}
+
+int vidioc_enum_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_enum_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_hdmi_cap(dev))
+			return -ENODATA;
+	} else {
+		if (!vivid_is_hdmi_out(dev))
+			return -ENODATA;
+	}
+	return v4l2_enum_dv_timings_cap(timings, &vivid_dv_timings_cap,
+			NULL, NULL);
+}
+
+int vidioc_dv_timings_cap(struct file *file, void *_fh,
+				    struct v4l2_dv_timings_cap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (!vivid_is_hdmi_cap(dev))
+			return -ENODATA;
+	} else {
+		if (!vivid_is_hdmi_out(dev))
+			return -ENODATA;
+	}
+	*cap = vivid_dv_timings_cap;
+	return 0;
+}
+
+int vidioc_g_edid(struct file *file, void *_fh,
+			 struct v4l2_edid *edid)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct video_device *vdev = video_devdata(file);
+
+	memset(edid->reserved, 0, sizeof(edid->reserved));
+	if (vdev->vfl_dir == VFL_DIR_RX) {
+		if (edid->pad >= dev->num_inputs)
+			return -EINVAL;
+		if (dev->input_type[edid->pad] != HDMI)
+			return -EINVAL;
+	} else {
+		if (edid->pad >= dev->num_outputs)
+			return -EINVAL;
+		if (dev->output_type[edid->pad] != HDMI)
+			return -EINVAL;
+	}
+	if (edid->start_block == 0 && edid->blocks == 0) {
+		edid->blocks = dev->edid_blocks;
+		return 0;
+	}
+	if (dev->edid_blocks == 0)
+		return -ENODATA;
+	if (edid->start_block >= dev->edid_blocks)
+		return -EINVAL;
+	if (edid->start_block + edid->blocks > dev->edid_blocks)
+		edid->blocks = dev->edid_blocks - edid->start_block;
+	memcpy(edid->edid, dev->edid, edid->blocks * 128);
+	return 0;
+}
diff --git a/drivers/media/platform/vivid/vivid-vid-common.h b/drivers/media/platform/vivid/vivid-vid-common.h
new file mode 100644
index 000000000000..3ec4fa85c9b9
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-common.h
@@ -0,0 +1,61 @@
+/*
+ * vivid-vid-common.h - common video support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VID_COMMON_H_
+#define _VIVID_VID_COMMON_H_
+
+typedef int (*fmtfunc)(struct file *file, void *priv, struct v4l2_format *f);
+
+/*
+ * Conversion function that converts a single-planar format to a
+ * single-plane multiplanar format.
+ */
+void fmt_sp2mp(const struct v4l2_format *sp_fmt, struct v4l2_format *mp_fmt);
+int fmt_sp2mp_func(struct file *file, void *priv,
+		struct v4l2_format *f, fmtfunc func);
+
+extern const struct v4l2_dv_timings_cap vivid_dv_timings_cap;
+
+const struct vivid_fmt *vivid_get_format(struct vivid_dev *dev, u32 pixelformat);
+
+bool vivid_vid_can_loop(struct vivid_dev *dev);
+void vivid_send_source_change(struct vivid_dev *dev, unsigned type);
+
+bool rect_overlap(const struct v4l2_rect *r1, const struct v4l2_rect *r2);
+void rect_set_size_to(struct v4l2_rect *r, const struct v4l2_rect *size);
+void rect_set_min_size(struct v4l2_rect *r, const struct v4l2_rect *min_size);
+void rect_set_max_size(struct v4l2_rect *r, const struct v4l2_rect *max_size);
+void rect_map_inside(struct v4l2_rect *r, const struct v4l2_rect *boundary);
+bool rect_same_size(const struct v4l2_rect *r1, const struct v4l2_rect *r2);
+struct v4l2_rect rect_intersect(const struct v4l2_rect *a, const struct v4l2_rect *b);
+void rect_scale(struct v4l2_rect *r, const struct v4l2_rect *from,
+				     const struct v4l2_rect *to);
+int vivid_vid_adjust_sel(unsigned flags, struct v4l2_rect *r);
+
+int vivid_enum_fmt_vid(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_enum_fmt_vid_mplane(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_enum_fmt_vid(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_std(struct file *file, void *priv, v4l2_std_id *id);
+int vidioc_g_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vidioc_enum_dv_timings(struct file *file, void *_fh, struct v4l2_enum_dv_timings *timings);
+int vidioc_dv_timings_cap(struct file *file, void *_fh, struct v4l2_dv_timings_cap *cap);
+int vidioc_g_edid(struct file *file, void *_fh, struct v4l2_edid *edid);
+int vidioc_subscribe_event(struct v4l2_fh *fh, const struct v4l2_event_subscription *sub);
+
+#endif
diff --git a/drivers/media/platform/vivid/vivid-vid-out.c b/drivers/media/platform/vivid/vivid-vid-out.c
new file mode 100644
index 000000000000..69c2dbd2d165
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-out.c
@@ -0,0 +1,1146 @@
+/*
+ * vivid-vid-out.c - video output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <linux/errno.h>
+#include <linux/kernel.h>
+#include <linux/sched.h>
+#include <linux/videodev2.h>
+#include <linux/v4l2-dv-timings.h>
+#include <media/v4l2-common.h>
+#include <media/v4l2-event.h>
+#include <media/v4l2-dv-timings.h>
+
+#include "vivid-core.h"
+#include "vivid-vid-common.h"
+#include "vivid-kthread-out.h"
+#include "vivid-vid-out.h"
+
+static int vid_out_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
+		       unsigned *nbuffers, unsigned *nplanes,
+		       unsigned sizes[], void *alloc_ctxs[])
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	unsigned planes = dev->fmt_out->planes;
+	unsigned h = dev->fmt_out_rect.height;
+	unsigned size = dev->bytesperline_out[0] * h;
+
+	if (dev->field_out == V4L2_FIELD_ALTERNATE) {
+		/*
+		 * You cannot use write() with FIELD_ALTERNATE since the field
+		 * information (TOP/BOTTOM) cannot be passed to the kernel.
+		 */
+		if (vb2_fileio_is_active(vq))
+			return -EINVAL;
+	}
+
+	if (dev->queue_setup_error) {
+		/*
+		 * Error injection: test what happens if queue_setup() returns
+		 * an error.
+		 */
+		dev->queue_setup_error = false;
+		return -EINVAL;
+	}
+
+	if (fmt) {
+		const struct v4l2_pix_format_mplane *mp;
+		struct v4l2_format mp_fmt;
+
+		if (!V4L2_TYPE_IS_MULTIPLANAR(fmt->type)) {
+			fmt_sp2mp(fmt, &mp_fmt);
+			fmt = &mp_fmt;
+		}
+		mp = &fmt->fmt.pix_mp;
+		/*
+		 * Check if the number of planes in the specified format match
+		 * the number of planes in the current format. You can't mix that.
+		 */
+		if (mp->num_planes != planes)
+			return -EINVAL;
+		sizes[0] = mp->plane_fmt[0].sizeimage;
+		if (planes == 2) {
+			sizes[1] = mp->plane_fmt[1].sizeimage;
+			if (sizes[0] < dev->bytesperline_out[0] * h ||
+			    sizes[1] < dev->bytesperline_out[1] * h)
+				return -EINVAL;
+		} else if (sizes[0] < size) {
+			return -EINVAL;
+		}
+	} else {
+		if (planes == 2) {
+			sizes[0] = dev->bytesperline_out[0] * h;
+			sizes[1] = dev->bytesperline_out[1] * h;
+		} else {
+			sizes[0] = size;
+		}
+	}
+
+	if (vq->num_buffers + *nbuffers < 2)
+		*nbuffers = 2 - vq->num_buffers;
+
+	*nplanes = planes;
+
+	/*
+	 * videobuf2-vmalloc allocator is context-less so no need to set
+	 * alloc_ctxs array.
+	 */
+
+	if (planes == 2)
+		dprintk(dev, 1, "%s, count=%d, sizes=%u, %u\n", __func__,
+			*nbuffers, sizes[0], sizes[1]);
+	else
+		dprintk(dev, 1, "%s, count=%d, size=%u\n", __func__,
+			*nbuffers, sizes[0]);
+	return 0;
+}
+
+static int vid_out_buf_prepare(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	unsigned long size;
+	unsigned planes = dev->fmt_out->planes;
+	unsigned p;
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	if (WARN_ON(NULL == dev->fmt_out))
+		return -EINVAL;
+
+	if (dev->buf_prepare_error) {
+		/*
+		 * Error injection: test what happens if buf_prepare() returns
+		 * an error.
+		 */
+		dev->buf_prepare_error = false;
+		return -EINVAL;
+	}
+
+	if (dev->field_out != V4L2_FIELD_ALTERNATE)
+		vb->v4l2_buf.field = dev->field_out;
+	else if (vb->v4l2_buf.field != V4L2_FIELD_TOP &&
+		 vb->v4l2_buf.field != V4L2_FIELD_BOTTOM)
+		return -EINVAL;
+
+	for (p = 0; p < planes; p++) {
+		size = dev->bytesperline_out[p] * dev->fmt_out_rect.height +
+			vb->v4l2_planes[p].data_offset;
+
+		if (vb2_get_plane_payload(vb, p) < size) {
+			dprintk(dev, 1, "%s the payload is too small for plane %u (%lu < %lu)\n",
+					__func__, p, vb2_get_plane_payload(vb, p), size);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static void vid_out_buf_queue(struct vb2_buffer *vb)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct vivid_buffer *buf = container_of(vb, struct vivid_buffer, vb);
+
+	dprintk(dev, 1, "%s\n", __func__);
+
+	spin_lock(&dev->slock);
+	list_add_tail(&buf->list, &dev->vid_out_active);
+	spin_unlock(&dev->slock);
+}
+
+static int vid_out_start_streaming(struct vb2_queue *vq, unsigned count)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+	int err;
+
+	if (vb2_is_streaming(&dev->vb_vid_cap_q))
+		dev->can_loop_video = vivid_vid_can_loop(dev);
+
+	if (dev->kthread_vid_out)
+		return 0;
+
+	dev->vid_out_seq_count = 0;
+	dprintk(dev, 1, "%s\n", __func__);
+	if (dev->start_streaming_error) {
+		dev->start_streaming_error = false;
+		err = -EINVAL;
+	} else {
+		err = vivid_start_generating_vid_out(dev, &dev->vid_out_streaming);
+	}
+	if (err) {
+		struct vivid_buffer *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->vid_out_active, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+	}
+	return err;
+}
+
+/* abort streaming and wait for last buffer */
+static void vid_out_stop_streaming(struct vb2_queue *vq)
+{
+	struct vivid_dev *dev = vb2_get_drv_priv(vq);
+
+	dprintk(dev, 1, "%s\n", __func__);
+	vivid_stop_generating_vid_out(dev, &dev->vid_out_streaming);
+	dev->can_loop_video = false;
+}
+
+const struct vb2_ops vivid_vid_out_qops = {
+	.queue_setup		= vid_out_queue_setup,
+	.buf_prepare		= vid_out_buf_prepare,
+	.buf_queue		= vid_out_buf_queue,
+	.start_streaming	= vid_out_start_streaming,
+	.stop_streaming		= vid_out_stop_streaming,
+	.wait_prepare		= vivid_unlock,
+	.wait_finish		= vivid_lock,
+};
+
+/*
+ * Called whenever the format has to be reset which can occur when
+ * changing outputs, standard, timings, etc.
+ */
+void vivid_update_format_out(struct vivid_dev *dev)
+{
+	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+	unsigned size;
+
+	switch (dev->output_type[dev->output]) {
+	case SVID:
+	default:
+		dev->field_out = dev->tv_field_out;
+		dev->sink_rect.width = 720;
+		if (dev->std_out & V4L2_STD_525_60) {
+			dev->sink_rect.height = 480;
+			dev->timeperframe_vid_out = (struct v4l2_fract) { 1001, 30000 };
+			dev->service_set_out = V4L2_SLICED_CAPTION_525;
+		} else {
+			dev->sink_rect.height = 576;
+			dev->timeperframe_vid_out = (struct v4l2_fract) { 1000, 25000 };
+			dev->service_set_out = V4L2_SLICED_WSS_625 | V4L2_SLICED_TELETEXT_B;
+		}
+		dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+		break;
+	case HDMI:
+		dev->sink_rect.width = bt->width;
+		dev->sink_rect.height = bt->height;
+		size = V4L2_DV_BT_FRAME_WIDTH(bt) * V4L2_DV_BT_FRAME_HEIGHT(bt);
+		dev->timeperframe_vid_out = (struct v4l2_fract) {
+			size / 100, (u32)bt->pixelclock / 100
+		};
+		if (bt->interlaced)
+			dev->field_out = V4L2_FIELD_ALTERNATE;
+		else
+			dev->field_out = V4L2_FIELD_NONE;
+		if (!dev->dvi_d_out && (bt->standards & V4L2_DV_BT_STD_CEA861)) {
+			if (bt->width == 720 && bt->height <= 576)
+				dev->colorspace_out = V4L2_COLORSPACE_SMPTE170M;
+			else
+				dev->colorspace_out = V4L2_COLORSPACE_REC709;
+		} else {
+			dev->colorspace_out = V4L2_COLORSPACE_SRGB;
+		}
+		break;
+	}
+	dev->compose_out = dev->sink_rect;
+	dev->compose_bounds_out = dev->sink_rect;
+	dev->crop_out = dev->compose_out;
+	if (V4L2_FIELD_HAS_T_OR_B(dev->field_out))
+		dev->crop_out.height /= 2;
+	dev->fmt_out_rect = dev->crop_out;
+	dev->bytesperline_out[0] = (dev->sink_rect.width * dev->fmt_out->depth) / 8;
+	if (dev->fmt_out->planes == 2)
+		dev->bytesperline_out[1] = (dev->sink_rect.width * dev->fmt_out->depth) / 8;
+}
+
+/* Map the field to something that is valid for the current output */
+static enum v4l2_field vivid_field_out(struct vivid_dev *dev, enum v4l2_field field)
+{
+	if (vivid_is_svid_out(dev)) {
+		switch (field) {
+		case V4L2_FIELD_INTERLACED_TB:
+		case V4L2_FIELD_INTERLACED_BT:
+		case V4L2_FIELD_SEQ_TB:
+		case V4L2_FIELD_SEQ_BT:
+		case V4L2_FIELD_ALTERNATE:
+			return field;
+		case V4L2_FIELD_INTERLACED:
+		default:
+			return V4L2_FIELD_INTERLACED;
+		}
+	}
+	if (vivid_is_hdmi_out(dev))
+		return dev->dv_timings_out.bt.interlaced ? V4L2_FIELD_ALTERNATE :
+						       V4L2_FIELD_NONE;
+	return V4L2_FIELD_NONE;
+}
+
+static enum tpg_pixel_aspect vivid_get_pixel_aspect(const struct vivid_dev *dev)
+{
+	if (vivid_is_svid_out(dev))
+		return (dev->std_out & V4L2_STD_525_60) ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	if (vivid_is_hdmi_out(dev) &&
+	    dev->sink_rect.width == 720 && dev->sink_rect.height <= 576)
+		return dev->sink_rect.height == 480 ?
+			TPG_PIXEL_ASPECT_NTSC : TPG_PIXEL_ASPECT_PAL;
+
+	return TPG_PIXEL_ASPECT_SQUARE;
+}
+
+int vivid_g_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	unsigned p;
+
+	mp->width        = dev->fmt_out_rect.width;
+	mp->height       = dev->fmt_out_rect.height;
+	mp->field        = dev->field_out;
+	mp->pixelformat  = dev->fmt_out->fourcc;
+	mp->colorspace   = dev->colorspace_out;
+	mp->num_planes = dev->fmt_out->planes;
+	for (p = 0; p < mp->num_planes; p++) {
+		mp->plane_fmt[p].bytesperline = dev->bytesperline_out[p];
+		mp->plane_fmt[p].sizeimage =
+			mp->plane_fmt[p].bytesperline * mp->height;
+	}
+	return 0;
+}
+
+int vivid_try_fmt_vid_out(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_bt_timings *bt = &dev->dv_timings_out.bt;
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct v4l2_plane_pix_format *pfmt = mp->plane_fmt;
+	const struct vivid_fmt *fmt;
+	unsigned bytesperline, max_bpl;
+	unsigned factor = 1;
+	unsigned w, h;
+	unsigned p;
+
+	fmt = vivid_get_format(dev, mp->pixelformat);
+	if (!fmt) {
+		dprintk(dev, 1, "Fourcc format (0x%08x) unknown.\n",
+			mp->pixelformat);
+		mp->pixelformat = V4L2_PIX_FMT_YUYV;
+		fmt = vivid_get_format(dev, mp->pixelformat);
+	}
+
+	mp->field = vivid_field_out(dev, mp->field);
+	if (vivid_is_svid_out(dev)) {
+		w = 720;
+		h = (dev->std_out & V4L2_STD_525_60) ? 480 : 576;
+	} else {
+		w = dev->sink_rect.width;
+		h = dev->sink_rect.height;
+	}
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+	if (!dev->has_scaler_out && !dev->has_crop_out && !dev->has_compose_out) {
+		mp->width = w;
+		mp->height = h / factor;
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height * factor };
+
+		rect_set_min_size(&r, &vivid_min_rect);
+		rect_set_max_size(&r, &vivid_max_rect);
+		if (dev->has_scaler_out && !dev->has_crop_out) {
+			struct v4l2_rect max_r = { 0, 0, MAX_ZOOM * w, MAX_ZOOM * h };
+
+			rect_set_max_size(&r, &max_r);
+		} else if (!dev->has_scaler_out && dev->has_compose_out && !dev->has_crop_out) {
+			rect_set_max_size(&r, &dev->sink_rect);
+		} else if (!dev->has_scaler_out && !dev->has_compose_out) {
+			rect_set_min_size(&r, &dev->sink_rect);
+		}
+		mp->width = r.width;
+		mp->height = r.height / factor;
+	}
+
+	/* This driver supports custom bytesperline values */
+
+	/* Calculate the minimum supported bytesperline value */
+	bytesperline = (mp->width * fmt->depth) >> 3;
+	/* Calculate the maximum supported bytesperline value */
+	max_bpl = (MAX_ZOOM * MAX_WIDTH * fmt->depth) >> 3;
+	mp->num_planes = fmt->planes;
+	for (p = 0; p < mp->num_planes; p++) {
+		if (pfmt[p].bytesperline > max_bpl)
+			pfmt[p].bytesperline = max_bpl;
+		if (pfmt[p].bytesperline < bytesperline)
+			pfmt[p].bytesperline = bytesperline;
+		pfmt[p].sizeimage = pfmt[p].bytesperline * mp->height;
+		memset(pfmt[p].reserved, 0, sizeof(pfmt[p].reserved));
+	}
+	if (vivid_is_svid_out(dev))
+		mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	else if (dev->dvi_d_out || !(bt->standards & V4L2_DV_BT_STD_CEA861))
+		mp->colorspace = V4L2_COLORSPACE_SRGB;
+	else if (bt->width == 720 && bt->height <= 576)
+		mp->colorspace = V4L2_COLORSPACE_SMPTE170M;
+	else if (mp->colorspace != V4L2_COLORSPACE_SMPTE170M &&
+		 mp->colorspace != V4L2_COLORSPACE_REC709 &&
+		 mp->colorspace != V4L2_COLORSPACE_SRGB)
+		mp->colorspace = V4L2_COLORSPACE_REC709;
+	memset(mp->reserved, 0, sizeof(mp->reserved));
+	return 0;
+}
+
+int vivid_s_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct v4l2_pix_format_mplane *mp = &f->fmt.pix_mp;
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_out;
+	struct v4l2_rect *compose = &dev->compose_out;
+	struct vb2_queue *q = &dev->vb_vid_out_q;
+	int ret = vivid_try_fmt_vid_out(file, priv, f);
+	unsigned factor = 1;
+
+	if (ret < 0)
+		return ret;
+
+	if (vb2_is_busy(q) &&
+	    (vivid_is_svid_out(dev) ||
+	     mp->width != dev->fmt_out_rect.width ||
+	     mp->height != dev->fmt_out_rect.height ||
+	     mp->pixelformat != dev->fmt_out->fourcc ||
+	     mp->field != dev->field_out)) {
+		dprintk(dev, 1, "%s device busy\n", __func__);
+		return -EBUSY;
+	}
+
+	/*
+	 * Allow for changing the colorspace on the fly. Useful for testing
+	 * purposes, and it is something that HDMI transmitters are able
+	 * to do.
+	 */
+	if (vb2_is_busy(q))
+		goto set_colorspace;
+
+	dev->fmt_out = vivid_get_format(dev, mp->pixelformat);
+	if (V4L2_FIELD_HAS_T_OR_B(mp->field))
+		factor = 2;
+
+	if (dev->has_scaler_out || dev->has_crop_out || dev->has_compose_out) {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		if (dev->has_scaler_out) {
+			if (dev->has_crop_out)
+				rect_map_inside(crop, &r);
+			else
+				*crop = r;
+			if (dev->has_compose_out && !dev->has_crop_out) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					r.width / MAX_ZOOM,
+					factor * r.height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					r.width * MAX_ZOOM,
+					factor * r.height * MAX_ZOOM
+				};
+
+				rect_set_min_size(compose, &min_r);
+				rect_set_max_size(compose, &max_r);
+				rect_map_inside(compose, &dev->compose_bounds_out);
+			} else if (dev->has_compose_out) {
+				struct v4l2_rect min_r = {
+					0, 0,
+					crop->width / MAX_ZOOM,
+					factor * crop->height / MAX_ZOOM
+				};
+				struct v4l2_rect max_r = {
+					0, 0,
+					crop->width * MAX_ZOOM,
+					factor * crop->height * MAX_ZOOM
+				};
+
+				rect_set_min_size(compose, &min_r);
+				rect_set_max_size(compose, &max_r);
+				rect_map_inside(compose, &dev->compose_bounds_out);
+			}
+		} else if (dev->has_compose_out && !dev->has_crop_out) {
+			rect_set_size_to(crop, &r);
+			r.height *= factor;
+			rect_set_size_to(compose, &r);
+			rect_map_inside(compose, &dev->compose_bounds_out);
+		} else if (!dev->has_compose_out) {
+			rect_map_inside(crop, &r);
+			r.height /= factor;
+			rect_set_size_to(compose, &r);
+		} else {
+			r.height *= factor;
+			rect_set_max_size(compose, &r);
+			rect_map_inside(compose, &dev->compose_bounds_out);
+			crop->top *= factor;
+			crop->height *= factor;
+			rect_set_size_to(crop, compose);
+			rect_map_inside(crop, &r);
+			crop->top /= factor;
+			crop->height /= factor;
+		}
+	} else {
+		struct v4l2_rect r = { 0, 0, mp->width, mp->height };
+
+		rect_set_size_to(crop, &r);
+		r.height /= factor;
+		rect_set_size_to(compose, &r);
+	}
+
+	dev->fmt_out_rect.width = mp->width;
+	dev->fmt_out_rect.height = mp->height;
+	dev->bytesperline_out[0] = mp->plane_fmt[0].bytesperline;
+	if (mp->num_planes > 1)
+		dev->bytesperline_out[1] = mp->plane_fmt[1].bytesperline;
+	dev->field_out = mp->field;
+	if (vivid_is_svid_out(dev))
+		dev->tv_field_out = mp->field;
+
+set_colorspace:
+	dev->colorspace_out = mp->colorspace;
+	if (dev->loop_video) {
+		vivid_send_source_change(dev, SVID);
+		vivid_send_source_change(dev, HDMI);
+	}
+	return 0;
+}
+
+int vidioc_g_fmt_vid_out_mplane(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_g_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_try_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_s_fmt_vid_out_mplane(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->multiplanar)
+		return -ENOTTY;
+	return vivid_s_fmt_vid_out(file, priv, f);
+}
+
+int vidioc_g_fmt_vid_out(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_g_fmt_vid_out);
+}
+
+int vidioc_try_fmt_vid_out(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_try_fmt_vid_out);
+}
+
+int vidioc_s_fmt_vid_out(struct file *file, void *priv,
+			struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (dev->multiplanar)
+		return -ENOTTY;
+	return fmt_sp2mp_func(file, priv, f, vivid_s_fmt_vid_out);
+}
+
+int vivid_vid_out_g_selection(struct file *file, void *priv,
+			      struct v4l2_selection *sel)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!dev->has_crop_out && !dev->has_compose_out)
+		return -ENOTTY;
+	if (sel->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	sel->r.left = sel->r.top = 0;
+	switch (sel->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_out)
+			return -EINVAL;
+		sel->r = dev->crop_out;
+		break;
+	case V4L2_SEL_TGT_CROP_DEFAULT:
+		if (!dev->has_crop_out)
+			return -EINVAL;
+		sel->r = dev->fmt_out_rect;
+		break;
+	case V4L2_SEL_TGT_CROP_BOUNDS:
+		if (!dev->has_compose_out)
+			return -EINVAL;
+		sel->r = vivid_max_rect;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_out)
+			return -EINVAL;
+		sel->r = dev->compose_out;
+		break;
+	case V4L2_SEL_TGT_COMPOSE_DEFAULT:
+	case V4L2_SEL_TGT_COMPOSE_BOUNDS:
+		if (!dev->has_compose_out)
+			return -EINVAL;
+		sel->r = dev->sink_rect;
+		break;
+	default:
+		return -EINVAL;
+	}
+	return 0;
+}
+
+int vivid_vid_out_s_selection(struct file *file, void *fh, struct v4l2_selection *s)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	struct v4l2_rect *crop = &dev->crop_out;
+	struct v4l2_rect *compose = &dev->compose_out;
+	unsigned factor = V4L2_FIELD_HAS_T_OR_B(dev->field_out) ? 2 : 1;
+	int ret;
+
+	if (!dev->has_crop_out && !dev->has_compose_out)
+		return -ENOTTY;
+	if (s->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (s->target) {
+	case V4L2_SEL_TGT_CROP:
+		if (!dev->has_crop_out)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->fmt_out_rect);
+		if (dev->has_scaler_out) {
+			struct v4l2_rect max_rect = {
+				0, 0,
+				dev->sink_rect.width * MAX_ZOOM,
+				(dev->sink_rect.height / factor) * MAX_ZOOM
+			};
+
+			rect_set_max_size(&s->r, &max_rect);
+			if (dev->has_compose_out) {
+				struct v4l2_rect min_rect = {
+					0, 0,
+					s->r.width / MAX_ZOOM,
+					(s->r.height * factor) / MAX_ZOOM
+				};
+				struct v4l2_rect max_rect = {
+					0, 0,
+					s->r.width * MAX_ZOOM,
+					(s->r.height * factor) * MAX_ZOOM
+				};
+
+				rect_set_min_size(compose, &min_rect);
+				rect_set_max_size(compose, &max_rect);
+				rect_map_inside(compose, &dev->compose_bounds_out);
+			}
+		} else if (dev->has_compose_out) {
+			s->r.top *= factor;
+			s->r.height *= factor;
+			rect_set_max_size(&s->r, &dev->sink_rect);
+			rect_set_size_to(compose, &s->r);
+			rect_map_inside(compose, &dev->compose_bounds_out);
+			s->r.top /= factor;
+			s->r.height /= factor;
+		} else {
+			rect_set_size_to(&s->r, &dev->sink_rect);
+			s->r.height /= factor;
+		}
+		rect_map_inside(&s->r, &dev->fmt_out_rect);
+		*crop = s->r;
+		break;
+	case V4L2_SEL_TGT_COMPOSE:
+		if (!dev->has_compose_out)
+			return -EINVAL;
+		ret = vivid_vid_adjust_sel(s->flags, &s->r);
+		if (ret)
+			return ret;
+		rect_set_min_size(&s->r, &vivid_min_rect);
+		rect_set_max_size(&s->r, &dev->sink_rect);
+		rect_map_inside(&s->r, &dev->compose_bounds_out);
+		s->r.top /= factor;
+		s->r.height /= factor;
+		if (dev->has_scaler_out) {
+			struct v4l2_rect fmt = dev->fmt_out_rect;
+			struct v4l2_rect max_rect = {
+				0, 0,
+				s->r.width * MAX_ZOOM,
+				s->r.height * MAX_ZOOM
+			};
+			struct v4l2_rect min_rect = {
+				0, 0,
+				s->r.width / MAX_ZOOM,
+				s->r.height / MAX_ZOOM
+			};
+
+			rect_set_min_size(&fmt, &min_rect);
+			if (!dev->has_crop_out)
+				rect_set_max_size(&fmt, &max_rect);
+			if (!rect_same_size(&dev->fmt_out_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_out_q))
+				return -EBUSY;
+			if (dev->has_crop_out) {
+				rect_set_min_size(crop, &min_rect);
+				rect_set_max_size(crop, &max_rect);
+			}
+			dev->fmt_out_rect = fmt;
+		} else if (dev->has_crop_out) {
+			struct v4l2_rect fmt = dev->fmt_out_rect;
+
+			rect_set_min_size(&fmt, &s->r);
+			if (!rect_same_size(&dev->fmt_out_rect, &fmt) &&
+			    vb2_is_busy(&dev->vb_vid_out_q))
+				return -EBUSY;
+			dev->fmt_out_rect = fmt;
+			rect_set_size_to(crop, &s->r);
+			rect_map_inside(crop, &dev->fmt_out_rect);
+		} else {
+			if (!rect_same_size(&s->r, &dev->fmt_out_rect) &&
+			    vb2_is_busy(&dev->vb_vid_out_q))
+				return -EBUSY;
+			rect_set_size_to(&dev->fmt_out_rect, &s->r);
+			rect_set_size_to(crop, &s->r);
+			crop->height /= factor;
+			rect_map_inside(crop, &dev->fmt_out_rect);
+		}
+		s->r.top *= factor;
+		s->r.height *= factor;
+		if (dev->bitmap_out && (compose->width != s->r.width ||
+					compose->height != s->r.height)) {
+			kfree(dev->bitmap_out);
+			dev->bitmap_out = NULL;
+		}
+		*compose = s->r;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+int vivid_vid_out_cropcap(struct file *file, void *priv,
+			      struct v4l2_cropcap *cap)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (cap->type != V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		return -EINVAL;
+
+	switch (vivid_get_pixel_aspect(dev)) {
+	case TPG_PIXEL_ASPECT_NTSC:
+		cap->pixelaspect.numerator = 11;
+		cap->pixelaspect.denominator = 10;
+		break;
+	case TPG_PIXEL_ASPECT_PAL:
+		cap->pixelaspect.numerator = 54;
+		cap->pixelaspect.denominator = 59;
+		break;
+	case TPG_PIXEL_ASPECT_SQUARE:
+		cap->pixelaspect.numerator = 1;
+		cap->pixelaspect.denominator = 1;
+		break;
+	}
+	return 0;
+}
+
+int vidioc_g_fmt_vid_out_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_out;
+	struct v4l2_window *win = &f->fmt.win;
+	unsigned clipcount = win->clipcount;
+
+	if (!dev->has_fb)
+		return -EINVAL;
+	win->w.top = dev->overlay_out_top;
+	win->w.left = dev->overlay_out_left;
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	win->clipcount = dev->clipcount_out;
+	win->field = V4L2_FIELD_ANY;
+	win->chromakey = dev->chromakey_out;
+	win->global_alpha = dev->global_alpha_out;
+	if (clipcount > dev->clipcount_out)
+		clipcount = dev->clipcount_out;
+	if (dev->bitmap_out == NULL)
+		win->bitmap = NULL;
+	else if (win->bitmap) {
+		if (copy_to_user(win->bitmap, dev->bitmap_out,
+		    ((dev->compose_out.width + 7) / 8) * dev->compose_out.height))
+			return -EFAULT;
+	}
+	if (clipcount && win->clips) {
+		if (copy_to_user(win->clips, dev->clips_out,
+				 clipcount * sizeof(dev->clips_out[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_try_fmt_vid_out_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_out;
+	struct v4l2_window *win = &f->fmt.win;
+	int i, j;
+
+	if (!dev->has_fb)
+		return -EINVAL;
+	win->w.left = clamp_t(int, win->w.left,
+			      -dev->display_width, dev->display_width);
+	win->w.top = clamp_t(int, win->w.top,
+			     -dev->display_height, dev->display_height);
+	win->w.width = compose->width;
+	win->w.height = compose->height;
+	/*
+	 * It makes no sense for an OSD to overlay only top or bottom fields,
+	 * so always set this to ANY.
+	 */
+	win->field = V4L2_FIELD_ANY;
+	if (win->clipcount && !win->clips)
+		win->clipcount = 0;
+	if (win->clipcount > MAX_CLIPS)
+		win->clipcount = MAX_CLIPS;
+	if (win->clipcount) {
+		if (copy_from_user(dev->try_clips_out, win->clips,
+				   win->clipcount * sizeof(dev->clips_out[0])))
+			return -EFAULT;
+		for (i = 0; i < win->clipcount; i++) {
+			struct v4l2_rect *r = &dev->try_clips_out[i].c;
+
+			r->top = clamp_t(s32, r->top, 0, dev->display_height - 1);
+			r->height = clamp_t(s32, r->height, 1, dev->display_height - r->top);
+			r->left = clamp_t(u32, r->left, 0, dev->display_width - 1);
+			r->width = clamp_t(u32, r->width, 1, dev->display_width - r->left);
+		}
+		/*
+		 * Yeah, so sue me, it's an O(n^2) algorithm. But n is a small
+		 * number and it's typically a one-time deal.
+		 */
+		for (i = 0; i < win->clipcount - 1; i++) {
+			struct v4l2_rect *r1 = &dev->try_clips_out[i].c;
+
+			for (j = i + 1; j < win->clipcount; j++) {
+				struct v4l2_rect *r2 = &dev->try_clips_out[j].c;
+
+				if (rect_overlap(r1, r2))
+					return -EINVAL;
+			}
+		}
+		if (copy_to_user(win->clips, dev->try_clips_out,
+				 win->clipcount * sizeof(dev->clips_out[0])))
+			return -EFAULT;
+	}
+	return 0;
+}
+
+int vidioc_s_fmt_vid_out_overlay(struct file *file, void *priv,
+					struct v4l2_format *f)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const struct v4l2_rect *compose = &dev->compose_out;
+	struct v4l2_window *win = &f->fmt.win;
+	int ret = vidioc_try_fmt_vid_out_overlay(file, priv, f);
+	unsigned bitmap_size = ((compose->width + 7) / 8) * compose->height;
+	unsigned clips_size = win->clipcount * sizeof(dev->clips_out[0]);
+	void *new_bitmap = NULL;
+
+	if (ret)
+		return ret;
+
+	if (win->bitmap) {
+		new_bitmap = memdup_user(win->bitmap, bitmap_size);
+
+		if (IS_ERR(new_bitmap))
+			return PTR_ERR(new_bitmap);
+	}
+
+	dev->overlay_out_top = win->w.top;
+	dev->overlay_out_left = win->w.left;
+	kfree(dev->bitmap_out);
+	dev->bitmap_out = new_bitmap;
+	dev->clipcount_out = win->clipcount;
+	if (dev->clipcount_out)
+		memcpy(dev->clips_out, dev->try_clips_out, clips_size);
+	dev->chromakey_out = win->chromakey;
+	dev->global_alpha_out = win->global_alpha;
+	return ret;
+}
+
+int vivid_vid_out_overlay(struct file *file, void *fh, unsigned i)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (i && !dev->fmt_out->can_do_overlay) {
+		dprintk(dev, 1, "unsupported output format for output overlay\n");
+		return -EINVAL;
+	}
+
+	dev->overlay_out_enabled = i;
+	return 0;
+}
+
+int vivid_vid_out_g_fbuf(struct file *file, void *fh,
+				struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
+			V4L2_FBUF_CAP_BITMAP_CLIPPING |
+			V4L2_FBUF_CAP_LIST_CLIPPING |
+			V4L2_FBUF_CAP_CHROMAKEY |
+			V4L2_FBUF_CAP_SRC_CHROMAKEY |
+			V4L2_FBUF_CAP_GLOBAL_ALPHA |
+			V4L2_FBUF_CAP_LOCAL_ALPHA |
+			V4L2_FBUF_CAP_LOCAL_INV_ALPHA;
+	a->flags = V4L2_FBUF_FLAG_OVERLAY | dev->fbuf_out_flags;
+	a->base = (void *)dev->video_pbase;
+	a->fmt.width = dev->display_width;
+	a->fmt.height = dev->display_height;
+	if (dev->fb_defined.green.length == 5)
+		a->fmt.pixelformat = V4L2_PIX_FMT_ARGB555;
+	else
+		a->fmt.pixelformat = V4L2_PIX_FMT_RGB565;
+	a->fmt.bytesperline = dev->display_byte_stride;
+	a->fmt.sizeimage = a->fmt.height * a->fmt.bytesperline;
+	a->fmt.field = V4L2_FIELD_NONE;
+	a->fmt.colorspace = V4L2_COLORSPACE_SRGB;
+	a->fmt.priv = 0;
+	return 0;
+}
+
+int vivid_vid_out_s_fbuf(struct file *file, void *fh,
+				const struct v4l2_framebuffer *a)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+	const unsigned chroma_flags = V4L2_FBUF_FLAG_CHROMAKEY |
+				      V4L2_FBUF_FLAG_SRC_CHROMAKEY;
+	const unsigned alpha_flags = V4L2_FBUF_FLAG_GLOBAL_ALPHA |
+				     V4L2_FBUF_FLAG_LOCAL_ALPHA |
+				     V4L2_FBUF_FLAG_LOCAL_INV_ALPHA;
+
+
+	if ((a->flags & chroma_flags) == chroma_flags)
+		return -EINVAL;
+	switch (a->flags & alpha_flags) {
+	case 0:
+	case V4L2_FBUF_FLAG_GLOBAL_ALPHA:
+	case V4L2_FBUF_FLAG_LOCAL_ALPHA:
+	case V4L2_FBUF_FLAG_LOCAL_INV_ALPHA:
+		break;
+	default:
+		return -EINVAL;
+	}
+	dev->fbuf_out_flags &= ~(chroma_flags | alpha_flags);
+	dev->fbuf_out_flags = a->flags & (chroma_flags | alpha_flags);
+	return 0;
+}
+
+static const struct v4l2_audioout vivid_audio_outputs[] = {
+	{ 0, "Line-Out 1" },
+	{ 1, "Line-Out 2" },
+};
+
+int vidioc_enum_output(struct file *file, void *priv,
+				struct v4l2_output *out)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (out->index >= dev->num_outputs)
+		return -EINVAL;
+
+	out->type = V4L2_OUTPUT_TYPE_ANALOG;
+	switch (dev->output_type[out->index]) {
+	case SVID:
+		snprintf(out->name, sizeof(out->name), "S-Video %u",
+				dev->output_name_counter[out->index]);
+		out->std = V4L2_STD_ALL;
+		if (dev->has_audio_outputs)
+			out->audioset = (1 << ARRAY_SIZE(vivid_audio_outputs)) - 1;
+		out->capabilities = V4L2_OUT_CAP_STD;
+		break;
+	case HDMI:
+		snprintf(out->name, sizeof(out->name), "HDMI %u",
+				dev->output_name_counter[out->index]);
+		out->capabilities = V4L2_OUT_CAP_DV_TIMINGS;
+		break;
+	}
+	return 0;
+}
+
+int vidioc_g_output(struct file *file, void *priv, unsigned *o)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	*o = dev->output;
+	return 0;
+}
+
+int vidioc_s_output(struct file *file, void *priv, unsigned o)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (o >= dev->num_outputs)
+		return -EINVAL;
+
+	if (o == dev->output)
+		return 0;
+
+	if (vb2_is_busy(&dev->vb_vid_out_q) || vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+
+	dev->output = o;
+	dev->tv_audio_output = 0;
+	if (dev->output_type[o] == SVID)
+		dev->vid_out_dev.tvnorms = V4L2_STD_ALL;
+	else
+		dev->vid_out_dev.tvnorms = 0;
+
+	dev->vbi_out_dev.tvnorms = dev->vid_out_dev.tvnorms;
+	vivid_update_format_out(dev);
+	return 0;
+}
+
+int vidioc_enumaudout(struct file *file, void *fh, struct v4l2_audioout *vout)
+{
+	if (vout->index >= ARRAY_SIZE(vivid_audio_outputs))
+		return -EINVAL;
+	*vout = vivid_audio_outputs[vout->index];
+	return 0;
+}
+
+int vidioc_g_audout(struct file *file, void *fh, struct v4l2_audioout *vout)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_svid_out(dev))
+		return -EINVAL;
+	*vout = vivid_audio_outputs[dev->tv_audio_output];
+	return 0;
+}
+
+int vidioc_s_audout(struct file *file, void *fh, const struct v4l2_audioout *vout)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_svid_out(dev))
+		return -EINVAL;
+	if (vout->index >= ARRAY_SIZE(vivid_audio_outputs))
+		return -EINVAL;
+	dev->tv_audio_output = vout->index;
+	return 0;
+}
+
+int vivid_vid_out_s_std(struct file *file, void *priv, v4l2_std_id id)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_svid_out(dev))
+		return -ENODATA;
+	if (dev->std_out == id)
+		return 0;
+	if (vb2_is_busy(&dev->vb_vid_out_q) || vb2_is_busy(&dev->vb_vbi_out_q))
+		return -EBUSY;
+	dev->std_out = id;
+	vivid_update_format_out(dev);
+	return 0;
+}
+
+int vivid_vid_out_s_dv_timings(struct file *file, void *_fh,
+				    struct v4l2_dv_timings *timings)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (!vivid_is_hdmi_out(dev))
+		return -ENODATA;
+	if (vb2_is_busy(&dev->vb_vid_out_q))
+		return -EBUSY;
+	if (!v4l2_find_dv_timings_cap(timings, &vivid_dv_timings_cap,
+				0, NULL, NULL))
+		return -EINVAL;
+	if (v4l2_match_dv_timings(timings, &dev->dv_timings_out, 0))
+		return 0;
+	dev->dv_timings_out = *timings;
+	vivid_update_format_out(dev);
+	return 0;
+}
+
+int vivid_vid_out_g_parm(struct file *file, void *priv,
+			  struct v4l2_streamparm *parm)
+{
+	struct vivid_dev *dev = video_drvdata(file);
+
+	if (parm->type != (dev->multiplanar ?
+			   V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE :
+			   V4L2_BUF_TYPE_VIDEO_OUTPUT))
+		return -EINVAL;
+
+	parm->parm.output.capability   = V4L2_CAP_TIMEPERFRAME;
+	parm->parm.output.timeperframe = dev->timeperframe_vid_out;
+	parm->parm.output.writebuffers  = 1;
+return 0;
+}
+
+int vidioc_subscribe_event(struct v4l2_fh *fh,
+			const struct v4l2_event_subscription *sub)
+{
+	switch (sub->type) {
+	case V4L2_EVENT_CTRL:
+		return v4l2_ctrl_subscribe_event(fh, sub);
+	case V4L2_EVENT_SOURCE_CHANGE:
+		if (fh->vdev->vfl_dir == VFL_DIR_RX)
+			return v4l2_src_change_event_subscribe(fh, sub);
+		break;
+	default:
+		break;
+	}
+	return -EINVAL;
+}
diff --git a/drivers/media/platform/vivid/vivid-vid-out.h b/drivers/media/platform/vivid/vivid-vid-out.h
new file mode 100644
index 000000000000..dfa84db184ed
--- /dev/null
+++ b/drivers/media/platform/vivid/vivid-vid-out.h
@@ -0,0 +1,56 @@
+/*
+ * vivid-vid-out.h - video output support functions.
+ *
+ * Copyright 2014 Cisco Systems, Inc. and/or its affiliates. All rights reserved.
+ *
+ * This program is free software; you may redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+ * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
+ * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
+ * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#ifndef _VIVID_VID_OUT_H_
+#define _VIVID_VID_OUT_H_
+
+extern const struct vb2_ops vivid_vid_out_qops;
+
+void vivid_update_format_out(struct vivid_dev *dev);
+
+int vivid_g_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_try_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_s_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out_mplane(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_g_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_out_g_selection(struct file *file, void *priv, struct v4l2_selection *sel);
+int vivid_vid_out_s_selection(struct file *file, void *fh, struct v4l2_selection *s);
+int vivid_vid_out_cropcap(struct file *file, void *fh, struct v4l2_cropcap *cap);
+int vidioc_enum_fmt_vid_out_overlay(struct file *file, void  *priv, struct v4l2_fmtdesc *f);
+int vidioc_g_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_try_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vidioc_s_fmt_vid_out_overlay(struct file *file, void *priv, struct v4l2_format *f);
+int vivid_vid_out_overlay(struct file *file, void *fh, unsigned i);
+int vivid_vid_out_g_fbuf(struct file *file, void *fh, struct v4l2_framebuffer *a);
+int vivid_vid_out_s_fbuf(struct file *file, void *fh, const struct v4l2_framebuffer *a);
+int vidioc_enum_output(struct file *file, void *priv, struct v4l2_output *out);
+int vidioc_g_output(struct file *file, void *priv, unsigned *i);
+int vidioc_s_output(struct file *file, void *priv, unsigned i);
+int vidioc_enumaudout(struct file *file, void *fh, struct v4l2_audioout *vout);
+int vidioc_g_audout(struct file *file, void *fh, struct v4l2_audioout *vout);
+int vidioc_s_audout(struct file *file, void *fh, const struct v4l2_audioout *vout);
+int vivid_vid_out_s_std(struct file *file, void *priv, v4l2_std_id id);
+int vivid_vid_out_s_dv_timings(struct file *file, void *_fh, struct v4l2_dv_timings *timings);
+int vivid_vid_out_g_parm(struct file *file, void *priv, struct v4l2_streamparm *parm);
+
+#endif
diff --git a/drivers/media/radio/radio-gemtek.c b/drivers/media/radio/radio-gemtek.c
index 235c0e349820..cff1eb144a5c 100644
--- a/drivers/media/radio/radio-gemtek.c
+++ b/drivers/media/radio/radio-gemtek.c
@@ -332,7 +332,7 @@ static int __init gemtek_init(void)
 
 static void __exit gemtek_exit(void)
 {
-	hardmute = 1;	/* Turn off PLL */
+	hardmute = true;	/* Turn off PLL */
 #ifdef CONFIG_PNP
 	pnp_unregister_driver(&gemtek_driver.pnp_driver);
 #endif
diff --git a/drivers/media/radio/radio-miropcm20.c b/drivers/media/radio/radio-miropcm20.c
index 998919e97dfe..7b35e633118d 100644
--- a/drivers/media/radio/radio-miropcm20.c
+++ b/drivers/media/radio/radio-miropcm20.c
@@ -27,6 +27,7 @@
 
 #include <linux/module.h>
 #include <linux/init.h>
+#include <linux/io.h>
 #include <linux/delay.h>
 #include <linux/videodev2.h>
 #include <linux/kthread.h>
diff --git a/drivers/media/radio/radio-sf16fmi.c b/drivers/media/radio/radio-sf16fmi.c
index d7ce8fe6b5ae..28a89466cddc 100644
--- a/drivers/media/radio/radio-sf16fmi.c
+++ b/drivers/media/radio/radio-sf16fmi.c
@@ -56,7 +56,7 @@ struct fmi
 
 static struct fmi fmi_card;
 static struct pnp_dev *dev;
-bool pnp_attached;
+static bool pnp_attached;
 
 #define RSF16_MINFREQ (87U * 16000)
 #define RSF16_MAXFREQ (108U * 16000)
@@ -285,7 +285,7 @@ static int __init fmi_init(void)
 				io = isapnp_fmi_probe();
 				if (io < 0)
 					continue;
-				pnp_attached = 1;
+				pnp_attached = true;
 			}
 			if (!request_region(io, 2, "radio-sf16fmi")) {
 				if (pnp_attached)
@@ -349,7 +349,7 @@ static int __init fmi_init(void)
 	mutex_init(&fmi->lock);
 
 	/* mute card and set default frequency */
-	fmi->mute = 1;
+	fmi->mute = true;
 	fmi->curfreq = RSF16_MINFREQ;
 	fmi_set_freq(fmi);
 
diff --git a/drivers/media/radio/radio-sf16fmr2.c b/drivers/media/radio/radio-sf16fmr2.c
index 93d864eb8306..b8d61cbc18cb 100644
--- a/drivers/media/radio/radio-sf16fmr2.c
+++ b/drivers/media/radio/radio-sf16fmr2.c
@@ -305,7 +305,7 @@ static void fmr2_pnp_remove(struct pnp_dev *pdev)
 	pnp_set_drvdata(pdev, NULL);
 }
 
-struct isa_driver fmr2_isa_driver = {
+static struct isa_driver fmr2_isa_driver = {
 	.match		= fmr2_isa_match,
 	.remove		= fmr2_isa_remove,
 	.driver		= {
@@ -313,7 +313,7 @@ struct isa_driver fmr2_isa_driver = {
 	},
 };
 
-struct pnp_driver fmr2_pnp_driver = {
+static struct pnp_driver fmr2_pnp_driver = {
 	.name		= "radio-sf16fmr2",
 	.id_table	= fmr2_pnp_ids,
 	.probe		= fmr2_pnp_probe,
diff --git a/drivers/media/radio/radio-tea5764.c b/drivers/media/radio/radio-tea5764.c
index 925049654c5b..cc3990111411 100644
--- a/drivers/media/radio/radio-tea5764.c
+++ b/drivers/media/radio/radio-tea5764.c
@@ -124,11 +124,11 @@ struct tea5764_regs {
 
 struct tea5764_write_regs {
 	u8 intreg;				/* INTMSK */
-	u16 frqset;				/* FRQSETMSB & FRQSETLSB */
-	u16 tnctrl;				/* TNCTRL1 & TNCTRL2 */
-	u16 testreg;				/* TESTBITS & TESTMODE */
-	u16 rdsctrl;				/* RDSCTRL1 & RDSCTRL2 */
-	u16 rdsbbl;				/* PAUSEDET & RDSBBL */
+	__be16 frqset;				/* FRQSETMSB & FRQSETLSB */
+	__be16 tnctrl;				/* TNCTRL1 & TNCTRL2 */
+	__be16 testreg;				/* TESTBITS & TESTMODE */
+	__be16 rdsctrl;				/* RDSCTRL1 & RDSCTRL2 */
+	__be16 rdsbbl;				/* PAUSEDET & RDSBBL */
 } __attribute__ ((packed));
 
 #ifdef CONFIG_RADIO_TEA5764_XTAL
@@ -165,7 +165,7 @@ static int tea5764_i2c_read(struct tea5764_device *radio)
 	if (i2c_transfer(radio->i2c_client->adapter, msgs, 1) != 1)
 		return -EIO;
 	for (i = 0; i < sizeof(struct tea5764_regs) / sizeof(u16); i++)
-		p[i] = __be16_to_cpu(p[i]);
+		p[i] = __be16_to_cpu((__force __be16)p[i]);
 
 	return 0;
 }
diff --git a/drivers/media/radio/si470x/radio-si470x-common.c b/drivers/media/radio/si470x/radio-si470x-common.c
index 0e750aef656a..909c3f92d839 100644
--- a/drivers/media/radio/si470x/radio-si470x-common.c
+++ b/drivers/media/radio/si470x/radio-si470x-common.c
@@ -208,7 +208,7 @@ static int si470x_set_band(struct si470x_device *radio, int band)
 static int si470x_set_chan(struct si470x_device *radio, unsigned short chan)
 {
 	int retval;
-	bool timed_out = 0;
+	bool timed_out = false;
 
 	/* start tuning */
 	radio->registers[CHANNEL] &= ~CHANNEL_CHAN;
@@ -300,7 +300,7 @@ static int si470x_set_seek(struct si470x_device *radio,
 {
 	int band, retval;
 	unsigned int freq;
-	bool timed_out = 0;
+	bool timed_out = false;
 
 	/* set band */
 	if (seek->rangelow || seek->rangehigh) {
diff --git a/drivers/media/radio/si470x/radio-si470x-usb.c b/drivers/media/radio/si470x/radio-si470x-usb.c
index 494fac061306..57f0bc3b60e7 100644
--- a/drivers/media/radio/si470x/radio-si470x-usb.c
+++ b/drivers/media/radio/si470x/radio-si470x-usb.c
@@ -607,9 +607,7 @@ static int si470x_usb_driver_probe(struct usb_interface *intf,
 	/* Set up interrupt endpoint information. */
 	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
 		endpoint = &iface_desc->endpoint[i].desc;
-		if (((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK) ==
-		 USB_DIR_IN) && ((endpoint->bmAttributes &
-		 USB_ENDPOINT_XFERTYPE_MASK) == USB_ENDPOINT_XFER_INT))
+		if (usb_endpoint_is_int_in(endpoint))
 			radio->int_in_endpoint = endpoint;
 	}
 	if (!radio->int_in_endpoint) {
diff --git a/drivers/media/radio/wl128x/fmdrv_common.c b/drivers/media/radio/wl128x/fmdrv_common.c
index 4b2e9e8298e1..6f28f6e02ea5 100644
--- a/drivers/media/radio/wl128x/fmdrv_common.c
+++ b/drivers/media/radio/wl128x/fmdrv_common.c
@@ -440,7 +440,7 @@ static int fm_send_cmd(struct fmdev *fmdev, u8 fm_op, u16 type,	void *payload,
 		 * command with u16 payload - convert to be16
 		 */
 		if (payload != NULL)
-			*(u16 *)payload = cpu_to_be16(*(u16 *)payload);
+			*(__be16 *)payload = cpu_to_be16(*(u16 *)payload);
 
 	} else if (payload != NULL) {
 		fm_cb(skb)->fm_op = *((u8 *)payload + 2);
@@ -595,7 +595,7 @@ static void fm_irq_handle_flag_getcmd_resp(struct fmdev *fmdev)
 	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
 	memcpy(&fmdev->irq_info.flag, skb->data, fm_evt_hdr->dlen);
 
-	fmdev->irq_info.flag = be16_to_cpu(fmdev->irq_info.flag);
+	fmdev->irq_info.flag = be16_to_cpu((__force __be16)fmdev->irq_info.flag);
 	fmdbg("irq: flag register(0x%x)\n", fmdev->irq_info.flag);
 
 	/* Continue next function in interrupt handler table */
@@ -764,7 +764,7 @@ static void fm_irq_handle_rdsdata_getcmd_resp(struct fmdev *fmdev)
 			 * Extract PI code and store in local cache.
 			 * We need this during AF switch processing.
 			 */
-			cur_picode = be16_to_cpu(rds_fmt.data.groupgeneral.pidata);
+			cur_picode = be16_to_cpu((__force __be16)rds_fmt.data.groupgeneral.pidata);
 			if (fmdev->rx.stat_info.picode != cur_picode)
 				fmdev->rx.stat_info.picode = cur_picode;
 
@@ -989,7 +989,7 @@ static void fm_irq_afjump_rd_freq_resp(struct fmdev *fmdev)
 	/* Skip header info and copy only response data */
 	skb_pull(skb, sizeof(struct fm_event_msg_hdr));
 	memcpy(&read_freq, skb->data, sizeof(read_freq));
-	read_freq = be16_to_cpu(read_freq);
+	read_freq = be16_to_cpu((__force __be16)read_freq);
 	curr_freq = fmdev->rx.region.bot_freq + ((u32)read_freq * FM_FREQ_MUL);
 
 	jumped_freq = fmdev->rx.stat_info.af_cache[fmdev->rx.afjump_idx];
@@ -1317,7 +1317,8 @@ static int load_default_rx_configuration(struct fmdev *fmdev)
 /* Does FM power on sequence */
 static int fm_power_up(struct fmdev *fmdev, u8 mode)
 {
-	u16 payload, asic_id, asic_ver;
+	u16 payload;
+	__be16 asic_id, asic_ver;
 	int resp_len, ret;
 	u8 fw_name[50];
 
diff --git a/drivers/media/radio/wl128x/fmdrv_rx.c b/drivers/media/radio/wl128x/fmdrv_rx.c
index ebf09a3927de..09632cb26cb6 100644
--- a/drivers/media/radio/wl128x/fmdrv_rx.c
+++ b/drivers/media/radio/wl128x/fmdrv_rx.c
@@ -116,7 +116,7 @@ int fm_rx_set_freq(struct fmdev *fmdev, u32 freq)
 	if (ret < 0)
 		goto exit;
 
-	curr_frq = be16_to_cpu(curr_frq);
+	curr_frq = be16_to_cpu((__force __be16)curr_frq);
 	curr_frq_in_khz = (fmdev->rx.region.bot_freq + ((u32)curr_frq * FM_FREQ_MUL));
 
 	if (curr_frq_in_khz != freq) {
@@ -189,7 +189,7 @@ int fm_rx_seek(struct fmdev *fmdev, u32 seek_upward,
 	if (ret < 0)
 		return ret;
 
-	curr_frq = be16_to_cpu(curr_frq);
+	curr_frq = be16_to_cpu((__force __be16)curr_frq);
 	last_frq = (fmdev->rx.region.top_freq - fmdev->rx.region.bot_freq) / FM_FREQ_MUL;
 
 	/* Check the offset in order to be aligned to the channel spacing*/
@@ -285,7 +285,7 @@ again:
 		if (ret < 0)
 			return ret;
 
-		curr_frq = be16_to_cpu(curr_frq);
+		curr_frq = be16_to_cpu((__force __be16)curr_frq);
 		fmdev->rx.freq = (fmdev->rx.region.bot_freq +
 				((u32)curr_frq * FM_FREQ_MUL));
 
@@ -517,7 +517,7 @@ int fm_rx_set_rfdepend_softmute(struct fmdev *fmdev, u8 rfdepend_mute)
 /* Returns the signal strength level of current channel */
 int fm_rx_get_rssi_level(struct fmdev *fmdev, u16 *rssilvl)
 {
-	u16 curr_rssi_lel;
+	__be16 curr_rssi_lel;
 	u32 resp_len;
 	int ret;
 
@@ -608,7 +608,7 @@ int fm_rx_set_stereo_mono(struct fmdev *fmdev, u16 mode)
 /* Gets current RX stereo/mono mode */
 int fm_rx_get_stereo_mono(struct fmdev *fmdev, u16 *mode)
 {
-	u16 curr_mode;
+	__be16 curr_mode;
 	u32 resp_len;
 	int ret;
 
diff --git a/drivers/media/radio/wl128x/fmdrv_tx.c b/drivers/media/radio/wl128x/fmdrv_tx.c
index 6ea33e09d63b..839970b0f313 100644
--- a/drivers/media/radio/wl128x/fmdrv_tx.c
+++ b/drivers/media/radio/wl128x/fmdrv_tx.c
@@ -374,7 +374,7 @@ int fm_tx_get_tune_cap_val(struct fmdev *fmdev)
 	if (ret < 0)
 		return ret;
 
-	curr_val = be16_to_cpu(curr_val);
+	curr_val = be16_to_cpu((__force __be16)curr_val);
 
 	return curr_val;
 }
diff --git a/drivers/media/rc/Kconfig b/drivers/media/rc/Kconfig
index 5e626af8e313..8ce08107a69d 100644
--- a/drivers/media/rc/Kconfig
+++ b/drivers/media/rc/Kconfig
@@ -164,6 +164,16 @@ config IR_ENE
 	   To compile this driver as a module, choose M here: the
 	   module will be called ene_ir.
 
+config IR_HIX5HD2
+	tristate "Hisilicon hix5hd2 IR remote control"
+	depends on RC_CORE
+	help
+	 Say Y here if you want to use hisilicon hix5hd2 remote control.
+	 To compile this driver as a module, choose M here: the module will be
+	 called ir-hix5hd2.
+
+	 If you're not sure, select N here
+
 config IR_IMON
 	tristate "SoundGraph iMON Receiver and Display"
 	depends on USB_ARCH_HAS_HCD
@@ -333,7 +343,8 @@ config IR_GPIO_CIR
 
 config RC_ST
 	tristate "ST remote control receiver"
-	depends on ARCH_STI && RC_CORE
+	depends on RC_CORE
+	depends on ARCH_STI || COMPILE_TEST
 	help
 	 Say Y here if you want support for ST remote control driver
 	 which allows both IR and UHF RX.
@@ -344,7 +355,7 @@ config RC_ST
 config IR_SUNXI
     tristate "SUNXI IR remote control"
     depends on RC_CORE
-    depends on ARCH_SUNXI
+    depends on ARCH_SUNXI || COMPILE_TEST
     ---help---
       Say Y if you want to use sunXi internal IR Controller
 
diff --git a/drivers/media/rc/Makefile b/drivers/media/rc/Makefile
index 9f9843a1af5f..0989f940e9cf 100644
--- a/drivers/media/rc/Makefile
+++ b/drivers/media/rc/Makefile
@@ -17,6 +17,7 @@ obj-$(CONFIG_IR_XMP_DECODER) += ir-xmp-decoder.o
 
 # stand-alone IR receivers/transmitters
 obj-$(CONFIG_RC_ATI_REMOTE) += ati_remote.o
+obj-$(CONFIG_IR_HIX5HD2) += ir-hix5hd2.o
 obj-$(CONFIG_IR_IMON) += imon.o
 obj-$(CONFIG_IR_ITE_CIR) += ite-cir.o
 obj-$(CONFIG_IR_MCEUSB) += mceusb.o
diff --git a/drivers/media/rc/ene_ir.c b/drivers/media/rc/ene_ir.c
index d16d9b496b92..e80f2c6c5f1a 100644
--- a/drivers/media/rc/ene_ir.c
+++ b/drivers/media/rc/ene_ir.c
@@ -979,7 +979,7 @@ static int ene_transmit(struct rc_dev *rdev, unsigned *buf, unsigned n)
 	dev->tx_reg = 0;
 	dev->tx_done = 0;
 	dev->tx_sample = 0;
-	dev->tx_sample_pulse = 0;
+	dev->tx_sample_pulse = false;
 
 	dbg("TX: %d samples", dev->tx_len);
 
diff --git a/drivers/media/rc/fintek-cir.c b/drivers/media/rc/fintek-cir.c
index f0a1f7d31ee6..b5167573240e 100644
--- a/drivers/media/rc/fintek-cir.c
+++ b/drivers/media/rc/fintek-cir.c
@@ -148,7 +148,6 @@ static int fintek_hw_detect(struct fintek_dev *fintek)
 	u8 vendor_major, vendor_minor;
 	u8 portsel, ir_class;
 	u16 vendor, chip;
-	int ret = 0;
 
 	fintek_config_mode_enable(fintek);
 
@@ -208,7 +207,7 @@ static int fintek_hw_detect(struct fintek_dev *fintek)
 
 	spin_unlock_irqrestore(&fintek->fintek_lock, flags);
 
-	return ret;
+	return 0;
 }
 
 static void fintek_cir_ldev_init(struct fintek_dev *fintek)
@@ -644,7 +643,6 @@ static int fintek_suspend(struct pnp_dev *pdev, pm_message_t state)
 
 static int fintek_resume(struct pnp_dev *pdev)
 {
-	int ret = 0;
 	struct fintek_dev *fintek = pnp_get_drvdata(pdev);
 
 	fit_dbg("%s called", __func__);
@@ -661,7 +659,7 @@ static int fintek_resume(struct pnp_dev *pdev)
 
 	fintek_cir_regs_init(fintek);
 
-	return ret;
+	return 0;
 }
 
 static void fintek_shutdown(struct pnp_dev *pdev)
diff --git a/drivers/media/rc/img-ir/img-ir-hw.c b/drivers/media/rc/img-ir/img-ir-hw.c
index bfb282a714e8..ec49f94425fc 100644
--- a/drivers/media/rc/img-ir/img-ir-hw.c
+++ b/drivers/media/rc/img-ir/img-ir-hw.c
@@ -25,12 +25,6 @@
 /* Decoders lock (only modified to preprocess them) */
 static DEFINE_SPINLOCK(img_ir_decoders_lock);
 
-extern struct img_ir_decoder img_ir_nec;
-extern struct img_ir_decoder img_ir_jvc;
-extern struct img_ir_decoder img_ir_sony;
-extern struct img_ir_decoder img_ir_sharp;
-extern struct img_ir_decoder img_ir_sanyo;
-
 static bool img_ir_decoders_preprocessed;
 static struct img_ir_decoder *img_ir_decoders[] = {
 #ifdef CONFIG_IR_IMG_NEC
diff --git a/drivers/media/rc/img-ir/img-ir-hw.h b/drivers/media/rc/img-ir/img-ir-hw.h
index 3e40ce87b898..8fcc16c32c5b 100644
--- a/drivers/media/rc/img-ir/img-ir-hw.h
+++ b/drivers/media/rc/img-ir/img-ir-hw.h
@@ -168,6 +168,12 @@ struct img_ir_decoder {
 		      struct img_ir_filter *out, u64 protocols);
 };
 
+extern struct img_ir_decoder img_ir_nec;
+extern struct img_ir_decoder img_ir_jvc;
+extern struct img_ir_decoder img_ir_sony;
+extern struct img_ir_decoder img_ir_sharp;
+extern struct img_ir_decoder img_ir_sanyo;
+
 /**
  * struct img_ir_reg_timings - Reg values for decoder timings at clock rate.
  * @ctrl:	Processed control register value.
diff --git a/drivers/media/rc/imon.c b/drivers/media/rc/imon.c
index 7115e68ba697..b8837dd39bb2 100644
--- a/drivers/media/rc/imon.c
+++ b/drivers/media/rc/imon.c
@@ -87,6 +87,18 @@ static ssize_t lcd_write(struct file *file, const char __user *buf,
 
 /*** G L O B A L S ***/
 
+struct imon_panel_key_table {
+	u64 hw_code;
+	u32 keycode;
+};
+
+struct imon_usb_dev_descr {
+	__u16 flags;
+#define IMON_NO_FLAGS 0
+#define IMON_NEED_20MS_PKT_DELAY 1
+	struct imon_panel_key_table key_table[];
+};
+
 struct imon_context {
 	struct device *dev;
 	/* Newer devices have two interfaces */
@@ -150,6 +162,8 @@ struct imon_context {
 	struct timer_list ttimer;	/* touch screen timer */
 	int touch_x;			/* x coordinate on touchscreen */
 	int touch_y;			/* y coordinate on touchscreen */
+	struct imon_usb_dev_descr *dev_descr; /* device description with key
+						 table for front panels */
 };
 
 #define TOUCH_TIMEOUT	(HZ/30)
@@ -186,8 +200,132 @@ enum {
 	IMON_KEY_PANEL	= 2,
 };
 
-enum {
-	IMON_NEED_20MS_PKT_DELAY = 1
+static struct usb_class_driver imon_vfd_class = {
+	.name		= DEVICE_NAME,
+	.fops		= &vfd_fops,
+	.minor_base	= DISPLAY_MINOR_BASE,
+};
+
+static struct usb_class_driver imon_lcd_class = {
+	.name		= DEVICE_NAME,
+	.fops		= &lcd_fops,
+	.minor_base	= DISPLAY_MINOR_BASE,
+};
+
+/* imon receiver front panel/knob key table */
+static const struct imon_usb_dev_descr imon_default_table = {
+	.flags = IMON_NO_FLAGS,
+	.key_table = {
+		{ 0x000000000f00ffeell, KEY_MEDIA }, /* Go */
+		{ 0x000000001200ffeell, KEY_UP },
+		{ 0x000000001300ffeell, KEY_DOWN },
+		{ 0x000000001400ffeell, KEY_LEFT },
+		{ 0x000000001500ffeell, KEY_RIGHT },
+		{ 0x000000001600ffeell, KEY_ENTER },
+		{ 0x000000001700ffeell, KEY_ESC },
+		{ 0x000000001f00ffeell, KEY_AUDIO },
+		{ 0x000000002000ffeell, KEY_VIDEO },
+		{ 0x000000002100ffeell, KEY_CAMERA },
+		{ 0x000000002700ffeell, KEY_DVD },
+		{ 0x000000002300ffeell, KEY_TV },
+		{ 0x000000002b00ffeell, KEY_EXIT },
+		{ 0x000000002c00ffeell, KEY_SELECT },
+		{ 0x000000002d00ffeell, KEY_MENU },
+		{ 0x000000000500ffeell, KEY_PREVIOUS },
+		{ 0x000000000700ffeell, KEY_REWIND },
+		{ 0x000000000400ffeell, KEY_STOP },
+		{ 0x000000003c00ffeell, KEY_PLAYPAUSE },
+		{ 0x000000000800ffeell, KEY_FASTFORWARD },
+		{ 0x000000000600ffeell, KEY_NEXT },
+		{ 0x000000010000ffeell, KEY_RIGHT },
+		{ 0x000001000000ffeell, KEY_LEFT },
+		{ 0x000000003d00ffeell, KEY_SELECT },
+		{ 0x000100000000ffeell, KEY_VOLUMEUP },
+		{ 0x010000000000ffeell, KEY_VOLUMEDOWN },
+		{ 0x000000000100ffeell, KEY_MUTE },
+		/* 0xffdc iMON MCE VFD */
+		{ 0x00010000ffffffeell, KEY_VOLUMEUP },
+		{ 0x01000000ffffffeell, KEY_VOLUMEDOWN },
+		{ 0x00000001ffffffeell, KEY_MUTE },
+		{ 0x0000000fffffffeell, KEY_MEDIA },
+		{ 0x00000012ffffffeell, KEY_UP },
+		{ 0x00000013ffffffeell, KEY_DOWN },
+		{ 0x00000014ffffffeell, KEY_LEFT },
+		{ 0x00000015ffffffeell, KEY_RIGHT },
+		{ 0x00000016ffffffeell, KEY_ENTER },
+		{ 0x00000017ffffffeell, KEY_ESC },
+		/* iMON Knob values */
+		{ 0x000100ffffffffeell, KEY_VOLUMEUP },
+		{ 0x010000ffffffffeell, KEY_VOLUMEDOWN },
+		{ 0x000008ffffffffeell, KEY_MUTE },
+		{ 0, KEY_RESERVED },
+	}
+};
+
+static const struct imon_usb_dev_descr imon_OEM_VFD = {
+	.flags = IMON_NEED_20MS_PKT_DELAY,
+	.key_table = {
+		{ 0x000000000f00ffeell, KEY_MEDIA }, /* Go */
+		{ 0x000000001200ffeell, KEY_UP },
+		{ 0x000000001300ffeell, KEY_DOWN },
+		{ 0x000000001400ffeell, KEY_LEFT },
+		{ 0x000000001500ffeell, KEY_RIGHT },
+		{ 0x000000001600ffeell, KEY_ENTER },
+		{ 0x000000001700ffeell, KEY_ESC },
+		{ 0x000000001f00ffeell, KEY_AUDIO },
+		{ 0x000000002b00ffeell, KEY_EXIT },
+		{ 0x000000002c00ffeell, KEY_SELECT },
+		{ 0x000000002d00ffeell, KEY_MENU },
+		{ 0x000000000500ffeell, KEY_PREVIOUS },
+		{ 0x000000000700ffeell, KEY_REWIND },
+		{ 0x000000000400ffeell, KEY_STOP },
+		{ 0x000000003c00ffeell, KEY_PLAYPAUSE },
+		{ 0x000000000800ffeell, KEY_FASTFORWARD },
+		{ 0x000000000600ffeell, KEY_NEXT },
+		{ 0x000000010000ffeell, KEY_RIGHT },
+		{ 0x000001000000ffeell, KEY_LEFT },
+		{ 0x000000003d00ffeell, KEY_SELECT },
+		{ 0x000100000000ffeell, KEY_VOLUMEUP },
+		{ 0x010000000000ffeell, KEY_VOLUMEDOWN },
+		{ 0x000000000100ffeell, KEY_MUTE },
+		/* 0xffdc iMON MCE VFD */
+		{ 0x00010000ffffffeell, KEY_VOLUMEUP },
+		{ 0x01000000ffffffeell, KEY_VOLUMEDOWN },
+		{ 0x00000001ffffffeell, KEY_MUTE },
+		{ 0x0000000fffffffeell, KEY_MEDIA },
+		{ 0x00000012ffffffeell, KEY_UP },
+		{ 0x00000013ffffffeell, KEY_DOWN },
+		{ 0x00000014ffffffeell, KEY_LEFT },
+		{ 0x00000015ffffffeell, KEY_RIGHT },
+		{ 0x00000016ffffffeell, KEY_ENTER },
+		{ 0x00000017ffffffeell, KEY_ESC },
+		/* iMON Knob values */
+		{ 0x000100ffffffffeell, KEY_VOLUMEUP },
+		{ 0x010000ffffffffeell, KEY_VOLUMEDOWN },
+		{ 0x000008ffffffffeell, KEY_MUTE },
+		{ 0, KEY_RESERVED },
+	}
+};
+
+/* imon receiver front panel/knob key table for DH102*/
+static const struct imon_usb_dev_descr imon_DH102 = {
+	.flags = IMON_NO_FLAGS,
+	.key_table = {
+		{ 0x000100000000ffeell, KEY_VOLUMEUP },
+		{ 0x010000000000ffeell, KEY_VOLUMEDOWN },
+		{ 0x000000010000ffeell, KEY_MUTE },
+		{ 0x0000000f0000ffeell, KEY_MEDIA },
+		{ 0x000000120000ffeell, KEY_UP },
+		{ 0x000000130000ffeell, KEY_DOWN },
+		{ 0x000000140000ffeell, KEY_LEFT },
+		{ 0x000000150000ffeell, KEY_RIGHT },
+		{ 0x000000160000ffeell, KEY_ENTER },
+		{ 0x000000170000ffeell, KEY_ESC },
+		{ 0x0000002b0000ffeell, KEY_EXIT },
+		{ 0x0000002c0000ffeell, KEY_SELECT },
+		{ 0x0000002d0000ffeell, KEY_MENU },
+		{ 0, KEY_RESERVED }
+	}
 };
 
 /*
@@ -208,7 +346,8 @@ static struct usb_device_id imon_usb_id_table[] = {
 	 * SoundGraph iMON PAD (IR & LCD)
 	 * SoundGraph iMON Knob (IR only)
 	 */
-	{ USB_DEVICE(0x15c2, 0xffdc) },
+	{ USB_DEVICE(0x15c2, 0xffdc),
+	  .driver_info = (unsigned long)&imon_default_table },
 
 	/*
 	 * Newer devices, all driven by the latest iMON Windows driver, full
@@ -216,43 +355,62 @@ static struct usb_device_id imon_usb_id_table[] = {
 	 * Need user input to fill in details on unknown devices.
 	 */
 	/* SoundGraph iMON OEM Touch LCD (IR & 7" VGA LCD) */
-	{ USB_DEVICE(0x15c2, 0x0034) },
+	{ USB_DEVICE(0x15c2, 0x0034),
+	  .driver_info = (unsigned long)&imon_DH102 },
 	/* SoundGraph iMON OEM Touch LCD (IR & 4.3" VGA LCD) */
-	{ USB_DEVICE(0x15c2, 0x0035) },
+	{ USB_DEVICE(0x15c2, 0x0035),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* SoundGraph iMON OEM VFD (IR & VFD) */
-	{ USB_DEVICE(0x15c2, 0x0036), .driver_info = IMON_NEED_20MS_PKT_DELAY },
+	{ USB_DEVICE(0x15c2, 0x0036),
+	  .driver_info = (unsigned long)&imon_OEM_VFD },
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x0037) },
+	{ USB_DEVICE(0x15c2, 0x0037),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* SoundGraph iMON OEM LCD (IR & LCD) */
-	{ USB_DEVICE(0x15c2, 0x0038) },
+	{ USB_DEVICE(0x15c2, 0x0038),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* SoundGraph iMON UltraBay (IR & LCD) */
-	{ USB_DEVICE(0x15c2, 0x0039) },
+	{ USB_DEVICE(0x15c2, 0x0039),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x003a) },
+	{ USB_DEVICE(0x15c2, 0x003a),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x003b) },
+	{ USB_DEVICE(0x15c2, 0x003b),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* SoundGraph iMON OEM Inside (IR only) */
-	{ USB_DEVICE(0x15c2, 0x003c) },
+	{ USB_DEVICE(0x15c2, 0x003c),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x003d) },
+	{ USB_DEVICE(0x15c2, 0x003d),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x003e) },
+	{ USB_DEVICE(0x15c2, 0x003e),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x003f) },
+	{ USB_DEVICE(0x15c2, 0x003f),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x0040) },
+	{ USB_DEVICE(0x15c2, 0x0040),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* SoundGraph iMON MINI (IR only) */
-	{ USB_DEVICE(0x15c2, 0x0041) },
+	{ USB_DEVICE(0x15c2, 0x0041),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* Antec Veris Multimedia Station EZ External (IR only) */
-	{ USB_DEVICE(0x15c2, 0x0042) },
+	{ USB_DEVICE(0x15c2, 0x0042),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* Antec Veris Multimedia Station Basic Internal (IR only) */
-	{ USB_DEVICE(0x15c2, 0x0043) },
+	{ USB_DEVICE(0x15c2, 0x0043),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* Antec Veris Multimedia Station Elite (IR & VFD) */
-	{ USB_DEVICE(0x15c2, 0x0044) },
+	{ USB_DEVICE(0x15c2, 0x0044),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* Antec Veris Multimedia Station Premiere (IR & LCD) */
-	{ USB_DEVICE(0x15c2, 0x0045) },
+	{ USB_DEVICE(0x15c2, 0x0045),
+	  .driver_info = (unsigned long)&imon_default_table},
 	/* device specifics unknown */
-	{ USB_DEVICE(0x15c2, 0x0046) },
+	{ USB_DEVICE(0x15c2, 0x0046),
+	  .driver_info = (unsigned long)&imon_default_table},
 	{}
 };
 
@@ -266,67 +424,6 @@ static struct usb_driver imon_driver = {
 	.id_table	= imon_usb_id_table,
 };
 
-static struct usb_class_driver imon_vfd_class = {
-	.name		= DEVICE_NAME,
-	.fops		= &vfd_fops,
-	.minor_base	= DISPLAY_MINOR_BASE,
-};
-
-static struct usb_class_driver imon_lcd_class = {
-	.name		= DEVICE_NAME,
-	.fops		= &lcd_fops,
-	.minor_base	= DISPLAY_MINOR_BASE,
-};
-
-/* imon receiver front panel/knob key table */
-static const struct {
-	u64 hw_code;
-	u32 keycode;
-} imon_panel_key_table[] = {
-	{ 0x000000000f00ffeell, KEY_MEDIA }, /* Go */
-	{ 0x000000001200ffeell, KEY_UP },
-	{ 0x000000001300ffeell, KEY_DOWN },
-	{ 0x000000001400ffeell, KEY_LEFT },
-	{ 0x000000001500ffeell, KEY_RIGHT },
-	{ 0x000000001600ffeell, KEY_ENTER },
-	{ 0x000000001700ffeell, KEY_ESC },
-	{ 0x000000001f00ffeell, KEY_AUDIO },
-	{ 0x000000002000ffeell, KEY_VIDEO },
-	{ 0x000000002100ffeell, KEY_CAMERA },
-	{ 0x000000002700ffeell, KEY_DVD },
-	{ 0x000000002300ffeell, KEY_TV },
-	{ 0x000000002b00ffeell, KEY_EXIT },
-	{ 0x000000002c00ffeell, KEY_SELECT },
-	{ 0x000000002d00ffeell, KEY_MENU },
-	{ 0x000000000500ffeell, KEY_PREVIOUS },
-	{ 0x000000000700ffeell, KEY_REWIND },
-	{ 0x000000000400ffeell, KEY_STOP },
-	{ 0x000000003c00ffeell, KEY_PLAYPAUSE },
-	{ 0x000000000800ffeell, KEY_FASTFORWARD },
-	{ 0x000000000600ffeell, KEY_NEXT },
-	{ 0x000000010000ffeell, KEY_RIGHT },
-	{ 0x000001000000ffeell, KEY_LEFT },
-	{ 0x000000003d00ffeell, KEY_SELECT },
-	{ 0x000100000000ffeell, KEY_VOLUMEUP },
-	{ 0x010000000000ffeell, KEY_VOLUMEDOWN },
-	{ 0x000000000100ffeell, KEY_MUTE },
-	/* 0xffdc iMON MCE VFD */
-	{ 0x00010000ffffffeell, KEY_VOLUMEUP },
-	{ 0x01000000ffffffeell, KEY_VOLUMEDOWN },
-	{ 0x00000001ffffffeell, KEY_MUTE },
-	{ 0x0000000fffffffeell, KEY_MEDIA },
-	{ 0x00000012ffffffeell, KEY_UP },
-	{ 0x00000013ffffffeell, KEY_DOWN },
-	{ 0x00000014ffffffeell, KEY_LEFT },
-	{ 0x00000015ffffffeell, KEY_RIGHT },
-	{ 0x00000016ffffffeell, KEY_ENTER },
-	{ 0x00000017ffffffeell, KEY_ESC },
-	/* iMON Knob values */
-	{ 0x000100ffffffffeell, KEY_VOLUMEUP },
-	{ 0x010000ffffffffeell, KEY_VOLUMEDOWN },
-	{ 0x000008ffffffffeell, KEY_MUTE },
-};
-
 /* to prevent races between open() and disconnect(), probing, etc */
 static DEFINE_MUTEX(driver_lock);
 
@@ -1210,18 +1307,19 @@ static u32 imon_mce_key_lookup(struct imon_context *ictx, u32 scancode)
 	return keycode;
 }
 
-static u32 imon_panel_key_lookup(u64 code)
+static u32 imon_panel_key_lookup(struct imon_context *ictx, u64 code)
 {
 	int i;
 	u32 keycode = KEY_RESERVED;
+	struct imon_panel_key_table *key_table = ictx->dev_descr->key_table;
 
-	for (i = 0; i < ARRAY_SIZE(imon_panel_key_table); i++) {
-		if (imon_panel_key_table[i].hw_code == (code | 0xffee)) {
-			keycode = imon_panel_key_table[i].keycode;
+	for (i = 0; key_table[i].hw_code != 0; i++) {
+		if (key_table[i].hw_code == (code | 0xffee)) {
+			keycode = key_table[i].keycode;
 			break;
 		}
 	}
-
+	ictx->release_code = false;
 	return keycode;
 }
 
@@ -1340,7 +1438,7 @@ static void imon_pad_to_keys(struct imon_context *ictx, unsigned char *buf)
 				}
 				buf[2] = dir & 0xFF;
 				buf[3] = (dir >> 8) & 0xFF;
-				scancode = be32_to_cpu(*((u32 *)buf));
+				scancode = be32_to_cpu(*((__be32 *)buf));
 			}
 		} else {
 			/*
@@ -1404,7 +1502,7 @@ static void imon_pad_to_keys(struct imon_context *ictx, unsigned char *buf)
 			}
 			buf[2] = dir & 0xFF;
 			buf[3] = (dir >> 8) & 0xFF;
-			scancode = be32_to_cpu(*((u32 *)buf));
+			scancode = be32_to_cpu(*((__be32 *)buf));
 		} else {
 			/*
 			 * Hack alert: instead of using keycodes, we have
@@ -1509,11 +1607,12 @@ static void imon_incoming_packet(struct imon_context *ictx,
 
 	/* Figure out what key was pressed */
 	if (len == 8 && buf[7] == 0xee) {
-		scancode = be64_to_cpu(*((u64 *)buf));
+		scancode = be64_to_cpu(*((__be64 *)buf));
 		ktype = IMON_KEY_PANEL;
-		kc = imon_panel_key_lookup(scancode);
+		kc = imon_panel_key_lookup(ictx, scancode);
+		ictx->release_code = false;
 	} else {
-		scancode = be32_to_cpu(*((u32 *)buf));
+		scancode = be32_to_cpu(*((__be32 *)buf));
 		if (ictx->rc_type == RC_BIT_RC6_MCE) {
 			ktype = IMON_KEY_IMON;
 			if (buf[0] == 0x80)
@@ -1908,6 +2007,7 @@ out:
 
 static struct input_dev *imon_init_idev(struct imon_context *ictx)
 {
+	struct imon_panel_key_table *key_table = ictx->dev_descr->key_table;
 	struct input_dev *idev;
 	int ret, i;
 
@@ -1933,8 +2033,8 @@ static struct input_dev *imon_init_idev(struct imon_context *ictx)
 		BIT_MASK(REL_WHEEL);
 
 	/* panel and/or knob code support */
-	for (i = 0; i < ARRAY_SIZE(imon_panel_key_table); i++) {
-		u32 kc = imon_panel_key_table[i].keycode;
+	for (i = 0; key_table[i].hw_code != 0; i++) {
+		u32 kc = key_table[i].keycode;
 		__set_bit(kc, idev->keybit);
 	}
 
@@ -2023,7 +2123,7 @@ static bool imon_find_endpoints(struct imon_context *ictx,
 	for (i = 0; i < num_endpts && !(ir_ep_found && display_ep_found); ++i) {
 		ep = &iface_desc->endpoint[i].desc;
 		ep_dir = ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK;
-		ep_type = ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
+		ep_type = usb_endpoint_type(ep);
 
 		if (!ir_ep_found && ep_dir == USB_DIR_IN &&
 		    ep_type == USB_ENDPOINT_XFER_INT) {
@@ -2135,9 +2235,11 @@ static struct imon_context *imon_init_intf0(struct usb_interface *intf,
 	ictx->vendor  = le16_to_cpu(ictx->usbdev_intf0->descriptor.idVendor);
 	ictx->product = le16_to_cpu(ictx->usbdev_intf0->descriptor.idProduct);
 
+	/* save drive info for later accessing the panel/knob key table */
+	ictx->dev_descr = (struct imon_usb_dev_descr *)id->driver_info;
 	/* default send_packet delay is 5ms but some devices need more */
-	ictx->send_packet_delay = id->driver_info & IMON_NEED_20MS_PKT_DELAY ?
-				  20 : 5;
+	ictx->send_packet_delay = ictx->dev_descr->flags &
+				  IMON_NEED_20MS_PKT_DELAY ? 20 : 5;
 
 	ret = -ENODEV;
 	iface_desc = intf->cur_altsetting;
@@ -2181,6 +2283,7 @@ idev_setup_failed:
 	usb_kill_urb(ictx->rx_urb_intf0);
 urb_submit_failed:
 find_endpoint_failed:
+	usb_put_dev(ictx->usbdev_intf0);
 	mutex_unlock(&ictx->lock);
 	usb_free_urb(tx_urb);
 tx_urb_alloc_failed:
@@ -2253,6 +2356,7 @@ urb_submit_failed:
 		input_unregister_device(ictx->touch);
 touch_setup_failed:
 find_endpoint_failed:
+	usb_put_dev(ictx->usbdev_intf1);
 	mutex_unlock(&ictx->lock);
 	usb_free_urb(rx_urb);
 rx_urb_alloc_failed:
@@ -2366,11 +2470,13 @@ static int imon_probe(struct usb_interface *interface,
 		 usbdev->bus->busnum, usbdev->devnum);
 
 	mutex_unlock(&driver_lock);
+	usb_put_dev(usbdev);
 
 	return 0;
 
 fail:
 	mutex_unlock(&driver_lock);
+	usb_put_dev(usbdev);
 	dev_err(dev, "unable to register, err %d\n", ret);
 
 	return ret;
@@ -2410,6 +2516,7 @@ static void imon_disconnect(struct usb_interface *interface)
 	if (ifnum == 0) {
 		ictx->dev_present_intf0 = false;
 		usb_kill_urb(ictx->rx_urb_intf0);
+		usb_put_dev(ictx->usbdev_intf0);
 		input_unregister_device(ictx->idev);
 		rc_unregister_device(ictx->rdev);
 		if (ictx->display_supported) {
@@ -2421,6 +2528,7 @@ static void imon_disconnect(struct usb_interface *interface)
 	} else {
 		ictx->dev_present_intf1 = false;
 		usb_kill_urb(ictx->rx_urb_intf1);
+		usb_put_dev(ictx->usbdev_intf1);
 		if (ictx->display_type == IMON_DISPLAY_TYPE_VGA) {
 			input_unregister_device(ictx->touch);
 			del_timer_sync(&ictx->ttimer);
diff --git a/drivers/media/rc/ir-hix5hd2.c b/drivers/media/rc/ir-hix5hd2.c
new file mode 100644
index 000000000000..08bbd4f508cd
--- /dev/null
+++ b/drivers/media/rc/ir-hix5hd2.c
@@ -0,0 +1,351 @@
+/*
+ * Copyright (c) 2014 Linaro Ltd.
+ * Copyright (c) 2014 Hisilicon Limited.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/regmap.h>
+#include <media/rc-core.h>
+
+/* Allow the driver to compile on all architectures */
+#ifndef writel_relaxed
+# define writel_relaxed writel
+#endif
+#ifndef readl_relaxed
+# define readl_relaxed readl
+#endif
+
+#define IR_ENABLE		0x00
+#define IR_CONFIG		0x04
+#define CNT_LEADS		0x08
+#define CNT_LEADE		0x0c
+#define CNT_SLEADE		0x10
+#define CNT0_B			0x14
+#define CNT1_B			0x18
+#define IR_BUSY			0x1c
+#define IR_DATAH		0x20
+#define IR_DATAL		0x24
+#define IR_INTM			0x28
+#define IR_INTS			0x2c
+#define IR_INTC			0x30
+#define IR_START		0x34
+
+/* interrupt mask */
+#define INTMS_SYMBRCV		(BIT(24) | BIT(8))
+#define INTMS_TIMEOUT		(BIT(25) | BIT(9))
+#define INTMS_OVERFLOW		(BIT(26) | BIT(10))
+#define INT_CLR_OVERFLOW	BIT(18)
+#define INT_CLR_TIMEOUT		BIT(17)
+#define INT_CLR_RCV		BIT(16)
+#define INT_CLR_RCVTIMEOUT	(BIT(16) | BIT(17))
+
+#define IR_CLK			0x48
+#define IR_CLK_ENABLE		BIT(4)
+#define IR_CLK_RESET		BIT(5)
+
+#define IR_CFG_WIDTH_MASK	0xffff
+#define IR_CFG_WIDTH_SHIFT	16
+#define IR_CFG_FORMAT_MASK	0x3
+#define IR_CFG_FORMAT_SHIFT	14
+#define IR_CFG_INT_LEVEL_MASK	0x3f
+#define IR_CFG_INT_LEVEL_SHIFT	8
+/* only support raw mode */
+#define IR_CFG_MODE_RAW		BIT(7)
+#define IR_CFG_FREQ_MASK	0x7f
+#define IR_CFG_FREQ_SHIFT	0
+#define IR_CFG_INT_THRESHOLD	1
+/* symbol start from low to high, symbol stream end at high*/
+#define IR_CFG_SYMBOL_FMT	0
+#define IR_CFG_SYMBOL_MAXWIDTH	0x3e80
+
+#define IR_HIX5HD2_NAME		"hix5hd2-ir"
+
+struct hix5hd2_ir_priv {
+	int			irq;
+	void volatile __iomem	*base;
+	struct device		*dev;
+	struct rc_dev		*rdev;
+	struct regmap		*regmap;
+	struct clk		*clock;
+	unsigned long		rate;
+};
+
+static void hix5hd2_ir_enable(struct hix5hd2_ir_priv *dev, bool on)
+{
+	u32 val;
+
+	regmap_read(dev->regmap, IR_CLK, &val);
+	if (on) {
+		val &= ~IR_CLK_RESET;
+		val |= IR_CLK_ENABLE;
+	} else {
+		val &= ~IR_CLK_ENABLE;
+		val |= IR_CLK_RESET;
+	}
+	regmap_write(dev->regmap, IR_CLK, val);
+}
+
+static int hix5hd2_ir_config(struct hix5hd2_ir_priv *priv)
+{
+	int timeout = 10000;
+	u32 val, rate;
+
+	writel_relaxed(0x01, priv->base + IR_ENABLE);
+	while (readl_relaxed(priv->base + IR_BUSY)) {
+		if (timeout--) {
+			udelay(1);
+		} else {
+			dev_err(priv->dev, "IR_BUSY timeout\n");
+			return -ETIMEDOUT;
+		}
+	}
+
+	/* Now only support raw mode, with symbol start from low to high */
+	rate = DIV_ROUND_CLOSEST(priv->rate, 1000000);
+	val = IR_CFG_SYMBOL_MAXWIDTH & IR_CFG_WIDTH_MASK << IR_CFG_WIDTH_SHIFT;
+	val |= IR_CFG_SYMBOL_FMT & IR_CFG_FORMAT_MASK << IR_CFG_FORMAT_SHIFT;
+	val |= (IR_CFG_INT_THRESHOLD - 1) & IR_CFG_INT_LEVEL_MASK
+	       << IR_CFG_INT_LEVEL_SHIFT;
+	val |= IR_CFG_MODE_RAW;
+	val |= (rate - 1) & IR_CFG_FREQ_MASK << IR_CFG_FREQ_SHIFT;
+	writel_relaxed(val, priv->base + IR_CONFIG);
+
+	writel_relaxed(0x00, priv->base + IR_INTM);
+	/* write arbitrary value to start  */
+	writel_relaxed(0x01, priv->base + IR_START);
+	return 0;
+}
+
+static int hix5hd2_ir_open(struct rc_dev *rdev)
+{
+	struct hix5hd2_ir_priv *priv = rdev->priv;
+
+	hix5hd2_ir_enable(priv, true);
+	return hix5hd2_ir_config(priv);
+}
+
+static void hix5hd2_ir_close(struct rc_dev *rdev)
+{
+	struct hix5hd2_ir_priv *priv = rdev->priv;
+
+	hix5hd2_ir_enable(priv, false);
+}
+
+static irqreturn_t hix5hd2_ir_rx_interrupt(int irq, void *data)
+{
+	u32 symb_num, symb_val, symb_time;
+	u32 data_l, data_h;
+	u32 irq_sr, i;
+	struct hix5hd2_ir_priv *priv = data;
+
+	irq_sr = readl_relaxed(priv->base + IR_INTS);
+	if (irq_sr & INTMS_OVERFLOW) {
+		/*
+		 * we must read IR_DATAL first, then we can clean up
+		 * IR_INTS availably since logic would not clear
+		 * fifo when overflow, drv do the job
+		 */
+		ir_raw_event_reset(priv->rdev);
+		symb_num = readl_relaxed(priv->base + IR_DATAH);
+		for (i = 0; i < symb_num; i++)
+			readl_relaxed(priv->base + IR_DATAL);
+
+		writel_relaxed(INT_CLR_OVERFLOW, priv->base + IR_INTC);
+		dev_info(priv->dev, "overflow, level=%d\n",
+			 IR_CFG_INT_THRESHOLD);
+	}
+
+	if ((irq_sr & INTMS_SYMBRCV) || (irq_sr & INTMS_TIMEOUT)) {
+		DEFINE_IR_RAW_EVENT(ev);
+
+		symb_num = readl_relaxed(priv->base + IR_DATAH);
+		for (i = 0; i < symb_num; i++) {
+			symb_val = readl_relaxed(priv->base + IR_DATAL);
+			data_l = ((symb_val & 0xffff) * 10);
+			data_h =  ((symb_val >> 16) & 0xffff) * 10;
+			symb_time = (data_l + data_h) / 10;
+
+			ev.duration = US_TO_NS(data_l);
+			ev.pulse = true;
+			ir_raw_event_store(priv->rdev, &ev);
+
+			if (symb_time < IR_CFG_SYMBOL_MAXWIDTH) {
+				ev.duration = US_TO_NS(data_h);
+				ev.pulse = false;
+				ir_raw_event_store(priv->rdev, &ev);
+			} else {
+				ir_raw_event_set_idle(priv->rdev, true);
+			}
+		}
+
+		if (irq_sr & INTMS_SYMBRCV)
+			writel_relaxed(INT_CLR_RCV, priv->base + IR_INTC);
+		if (irq_sr & INTMS_TIMEOUT)
+			writel_relaxed(INT_CLR_TIMEOUT, priv->base + IR_INTC);
+	}
+
+	/* Empty software fifo */
+	ir_raw_event_handle(priv->rdev);
+	return IRQ_HANDLED;
+}
+
+static int hix5hd2_ir_probe(struct platform_device *pdev)
+{
+	struct rc_dev *rdev;
+	struct device *dev = &pdev->dev;
+	struct resource *res;
+	struct hix5hd2_ir_priv *priv;
+	struct device_node *node = pdev->dev.of_node;
+	const char *map_name;
+	int ret;
+
+	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->regmap = syscon_regmap_lookup_by_phandle(node,
+						       "hisilicon,power-syscon");
+	if (IS_ERR(priv->regmap)) {
+		dev_err(dev, "no power-reg\n");
+		return -EINVAL;
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	priv->base = devm_ioremap_resource(dev, res);
+	if (IS_ERR((__force void *)priv->base))
+		return PTR_ERR((__force void *)priv->base);
+
+	priv->irq = platform_get_irq(pdev, 0);
+	if (priv->irq < 0) {
+		dev_err(dev, "irq can not get\n");
+		return priv->irq;
+	}
+
+	rdev = rc_allocate_device();
+	if (!rdev)
+		return -ENOMEM;
+
+	priv->clock = devm_clk_get(dev, NULL);
+	if (IS_ERR(priv->clock)) {
+		dev_err(dev, "clock not found\n");
+		ret = PTR_ERR(priv->clock);
+		goto err;
+	}
+	clk_prepare_enable(priv->clock);
+	priv->rate = clk_get_rate(priv->clock);
+
+	rdev->driver_type = RC_DRIVER_IR_RAW;
+	rdev->allowed_protocols = RC_BIT_ALL;
+	rdev->priv = priv;
+	rdev->open = hix5hd2_ir_open;
+	rdev->close = hix5hd2_ir_close;
+	rdev->driver_name = IR_HIX5HD2_NAME;
+	map_name = of_get_property(node, "linux,rc-map-name", NULL);
+	rdev->map_name = map_name ?: RC_MAP_EMPTY;
+	rdev->input_name = IR_HIX5HD2_NAME;
+	rdev->input_phys = IR_HIX5HD2_NAME "/input0";
+	rdev->input_id.bustype = BUS_HOST;
+	rdev->input_id.vendor = 0x0001;
+	rdev->input_id.product = 0x0001;
+	rdev->input_id.version = 0x0100;
+	rdev->rx_resolution = US_TO_NS(10);
+	rdev->timeout = US_TO_NS(IR_CFG_SYMBOL_MAXWIDTH * 10);
+
+	ret = rc_register_device(rdev);
+	if (ret < 0)
+		goto clkerr;
+
+	if (devm_request_irq(dev, priv->irq, hix5hd2_ir_rx_interrupt,
+			     IRQF_NO_SUSPEND, pdev->name, priv) < 0) {
+		dev_err(dev, "IRQ %d register failed\n", priv->irq);
+		ret = -EINVAL;
+		goto regerr;
+	}
+
+	priv->rdev = rdev;
+	priv->dev = dev;
+	platform_set_drvdata(pdev, priv);
+
+	return ret;
+
+regerr:
+	rc_unregister_device(rdev);
+	rdev = NULL;
+clkerr:
+	clk_disable_unprepare(priv->clock);
+err:
+	rc_free_device(rdev);
+	dev_err(dev, "Unable to register device (%d)\n", ret);
+	return ret;
+}
+
+static int hix5hd2_ir_remove(struct platform_device *pdev)
+{
+	struct hix5hd2_ir_priv *priv = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(priv->clock);
+	rc_unregister_device(priv->rdev);
+	return 0;
+}
+
+#ifdef CONFIG_PM
+static int hix5hd2_ir_suspend(struct device *dev)
+{
+	struct hix5hd2_ir_priv *priv = dev_get_drvdata(dev);
+
+	clk_disable_unprepare(priv->clock);
+	hix5hd2_ir_enable(priv, false);
+
+	return 0;
+}
+
+static int hix5hd2_ir_resume(struct device *dev)
+{
+	struct hix5hd2_ir_priv *priv = dev_get_drvdata(dev);
+
+	hix5hd2_ir_enable(priv, true);
+	clk_prepare_enable(priv->clock);
+
+	writel_relaxed(0x01, priv->base + IR_ENABLE);
+	writel_relaxed(0x00, priv->base + IR_INTM);
+	writel_relaxed(0xff, priv->base + IR_INTC);
+	writel_relaxed(0x01, priv->base + IR_START);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(hix5hd2_ir_pm_ops, hix5hd2_ir_suspend,
+			 hix5hd2_ir_resume);
+
+static struct of_device_id hix5hd2_ir_table[] = {
+	{ .compatible = "hisilicon,hix5hd2-ir", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, hix5hd2_ir_table);
+
+static struct platform_driver hix5hd2_ir_driver = {
+	.driver = {
+		.name = IR_HIX5HD2_NAME,
+		.of_match_table = hix5hd2_ir_table,
+		.pm     = &hix5hd2_ir_pm_ops,
+	},
+	.probe = hix5hd2_ir_probe,
+	.remove = hix5hd2_ir_remove,
+};
+
+module_platform_driver(hix5hd2_ir_driver);
+
+MODULE_DESCRIPTION("IR controller driver for hix5hd2 platforms");
+MODULE_AUTHOR("Guoxiong Yan <yanguoxiong@huawei.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:hix5hd2-ir");
diff --git a/drivers/media/rc/ite-cir.c b/drivers/media/rc/ite-cir.c
index 447fe35862dc..56abf9120cc2 100644
--- a/drivers/media/rc/ite-cir.c
+++ b/drivers/media/rc/ite-cir.c
@@ -1666,7 +1666,6 @@ static int ite_suspend(struct pnp_dev *pdev, pm_message_t state)
 
 static int ite_resume(struct pnp_dev *pdev)
 {
-	int ret = 0;
 	struct ite_dev *dev = pnp_get_drvdata(pdev);
 	unsigned long flags;
 
@@ -1681,7 +1680,7 @@ static int ite_resume(struct pnp_dev *pdev)
 
 	spin_unlock_irqrestore(&dev->lock, flags);
 
-	return ret;
+	return 0;
 }
 
 static void ite_shutdown(struct pnp_dev *pdev)
diff --git a/drivers/media/rc/keymaps/Kconfig b/drivers/media/rc/keymaps/Kconfig
index 8e615fd55852..767423bbbdd0 100644
--- a/drivers/media/rc/keymaps/Kconfig
+++ b/drivers/media/rc/keymaps/Kconfig
@@ -12,4 +12,4 @@ config RC_MAP
 	   The ir-keytable program, available at v4l-utils package
 	   provide the tool and the same RC maps for load from
 	   userspace. Its available at
-			http://git.linuxtv.org/v4l-utils
+		http://git.linuxtv.org/cgit.cgi/v4l-utils.git/
diff --git a/drivers/media/rc/keymaps/Makefile b/drivers/media/rc/keymaps/Makefile
index 0b8c54919010..abf60794223d 100644
--- a/drivers/media/rc/keymaps/Makefile
+++ b/drivers/media/rc/keymaps/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_RC_MAP) += rc-adstech-dvb-t-pci.o \
 			rc-dm1105-nec.o \
 			rc-dntv-live-dvb-t.o \
 			rc-dntv-live-dvbt-pro.o \
+			rc-dvbsky.o \
 			rc-em-terratec.o \
 			rc-encore-enltv2.o \
 			rc-encore-enltv.o \
diff --git a/drivers/media/rc/keymaps/rc-dvbsky.c b/drivers/media/rc/keymaps/rc-dvbsky.c
new file mode 100644
index 000000000000..c5115a1165d1
--- /dev/null
+++ b/drivers/media/rc/keymaps/rc-dvbsky.c
@@ -0,0 +1,78 @@
+/* rc-dvbsky.c - Keytable for DVBSky Remote Controllers
+ *
+ * keymap imported from ir-keymaps.c
+ *
+ *
+ * Copyright (c) 2010-2012 by Nibble Max <nibble.max@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <media/rc-map.h>
+#include <linux/module.h>
+/*
+ * This table contains the complete RC5 code, instead of just the data part
+ */
+
+static struct rc_map_table rc5_dvbsky[] = {
+	{ 0x0000, KEY_0 },
+	{ 0x0001, KEY_1 },
+	{ 0x0002, KEY_2 },
+	{ 0x0003, KEY_3 },
+	{ 0x0004, KEY_4 },
+	{ 0x0005, KEY_5 },
+	{ 0x0006, KEY_6 },
+	{ 0x0007, KEY_7 },
+	{ 0x0008, KEY_8 },
+	{ 0x0009, KEY_9 },
+	{ 0x000a, KEY_MUTE },
+	{ 0x000d, KEY_OK },
+	{ 0x000b, KEY_STOP },
+	{ 0x000c, KEY_EXIT },
+	{ 0x000e, KEY_CAMERA }, /*Snap shot*/
+	{ 0x000f, KEY_SUBTITLE }, /*PIP*/
+	{ 0x0010, KEY_VOLUMEUP },
+	{ 0x0011, KEY_VOLUMEDOWN },
+	{ 0x0012, KEY_FAVORITES },
+	{ 0x0013, KEY_LIST }, /*Info*/
+	{ 0x0016, KEY_PAUSE },
+	{ 0x0017, KEY_PLAY },
+	{ 0x001f, KEY_RECORD },
+	{ 0x0020, KEY_CHANNELDOWN },
+	{ 0x0021, KEY_CHANNELUP },
+	{ 0x0025, KEY_POWER2 },
+	{ 0x0026, KEY_REWIND },
+	{ 0x0027, KEY_FASTFORWARD },
+	{ 0x0029, KEY_LAST },
+	{ 0x002b, KEY_MENU },
+	{ 0x002c, KEY_EPG },
+	{ 0x002d, KEY_ZOOM },
+};
+
+static struct rc_map_list rc5_dvbsky_map = {
+	.map = {
+		.scan    = rc5_dvbsky,
+		.size    = ARRAY_SIZE(rc5_dvbsky),
+		.rc_type = RC_TYPE_RC5,
+		.name    = RC_MAP_DVBSKY,
+	}
+};
+
+static int __init init_rc_map_rc5_dvbsky(void)
+{
+	return rc_map_register(&rc5_dvbsky_map);
+}
+
+static void __exit exit_rc_map_rc5_dvbsky(void)
+{
+	rc_map_unregister(&rc5_dvbsky_map);
+}
+
+module_init(init_rc_map_rc5_dvbsky)
+module_exit(exit_rc_map_rc5_dvbsky)
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Nibble Max <nibble.max@gmail.com>");
diff --git a/drivers/media/rc/lirc_dev.c b/drivers/media/rc/lirc_dev.c
index dc5cbffcd5a2..249d2fbc8f37 100644
--- a/drivers/media/rc/lirc_dev.c
+++ b/drivers/media/rc/lirc_dev.c
@@ -595,7 +595,7 @@ long lirc_dev_fop_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 	switch (cmd) {
 	case LIRC_GET_FEATURES:
-		result = put_user(ir->d.features, (__u32 *)arg);
+		result = put_user(ir->d.features, (__u32 __user *)arg);
 		break;
 	case LIRC_GET_REC_MODE:
 		if (!(ir->d.features & LIRC_CAN_REC_MASK)) {
@@ -605,7 +605,7 @@ long lirc_dev_fop_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 		result = put_user(LIRC_REC2MODE
 				  (ir->d.features & LIRC_CAN_REC_MASK),
-				  (__u32 *)arg);
+				  (__u32 __user *)arg);
 		break;
 	case LIRC_SET_REC_MODE:
 		if (!(ir->d.features & LIRC_CAN_REC_MASK)) {
@@ -613,7 +613,7 @@ long lirc_dev_fop_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			break;
 		}
 
-		result = get_user(mode, (__u32 *)arg);
+		result = get_user(mode, (__u32 __user *)arg);
 		if (!result && !(LIRC_MODE2REC(mode) & ir->d.features))
 			result = -EINVAL;
 		/*
@@ -622,7 +622,7 @@ long lirc_dev_fop_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		 */
 		break;
 	case LIRC_GET_LENGTH:
-		result = put_user(ir->d.code_length, (__u32 *)arg);
+		result = put_user(ir->d.code_length, (__u32 __user *)arg);
 		break;
 	case LIRC_GET_MIN_TIMEOUT:
 		if (!(ir->d.features & LIRC_CAN_SET_REC_TIMEOUT) ||
@@ -631,7 +631,7 @@ long lirc_dev_fop_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			break;
 		}
 
-		result = put_user(ir->d.min_timeout, (__u32 *)arg);
+		result = put_user(ir->d.min_timeout, (__u32 __user *)arg);
 		break;
 	case LIRC_GET_MAX_TIMEOUT:
 		if (!(ir->d.features & LIRC_CAN_SET_REC_TIMEOUT) ||
@@ -640,7 +640,7 @@ long lirc_dev_fop_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			break;
 		}
 
-		result = put_user(ir->d.max_timeout, (__u32 *)arg);
+		result = put_user(ir->d.max_timeout, (__u32 __user *)arg);
 		break;
 	default:
 		result = -EINVAL;
@@ -736,7 +736,7 @@ ssize_t lirc_dev_fop_read(struct file *file,
 			}
 		} else {
 			lirc_buffer_read(ir->buf, buf);
-			ret = copy_to_user((void *)buffer+written, buf,
+			ret = copy_to_user((void __user *)buffer+written, buf,
 					   ir->buf->chunk_size);
 			if (!ret)
 				written += ir->buf->chunk_size;
diff --git a/drivers/media/rc/mceusb.c b/drivers/media/rc/mceusb.c
index 45b0894288e5..2cdb740cde48 100644
--- a/drivers/media/rc/mceusb.c
+++ b/drivers/media/rc/mceusb.c
@@ -397,6 +397,10 @@ static struct usb_device_id mceusb_dev_table[] = {
 	  .driver_info = HAUPPAUGE_CX_HYBRID_TV },
 	{ USB_DEVICE(VENDOR_HAUPPAUGE, 0xb131),
 	  .driver_info = HAUPPAUGE_CX_HYBRID_TV },
+	{ USB_DEVICE(VENDOR_HAUPPAUGE, 0xb138),
+	  .driver_info = HAUPPAUGE_CX_HYBRID_TV },
+	{ USB_DEVICE(VENDOR_HAUPPAUGE, 0xb139),
+	  .driver_info = HAUPPAUGE_CX_HYBRID_TV },
 	{ USB_DEVICE(VENDOR_PCTV, 0x0259),
 	  .driver_info = HAUPPAUGE_CX_HYBRID_TV },
 	{ USB_DEVICE(VENDOR_PCTV, 0x025e),
@@ -1198,10 +1202,9 @@ static void mceusb_flash_led(struct mceusb_dev *ir)
 	mce_async_out(ir, FLASH_LED, sizeof(FLASH_LED));
 }
 
-static struct rc_dev *mceusb_init_rc_dev(struct mceusb_dev *ir,
-					 struct usb_interface *intf)
+static struct rc_dev *mceusb_init_rc_dev(struct mceusb_dev *ir)
 {
-	struct usb_device *udev = usb_get_dev(interface_to_usbdev(intf));
+	struct usb_device *udev = ir->usbdev;
 	struct device *dev = ir->dev;
 	struct rc_dev *rc;
 	int ret;
@@ -1341,7 +1344,7 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 	if (!ir->urb_in)
 		goto urb_in_alloc_fail;
 
-	ir->usbdev = dev;
+	ir->usbdev = usb_get_dev(dev);
 	ir->dev = &intf->dev;
 	ir->len_in = maxp;
 	ir->flags.microsoft_gen1 = is_microsoft_gen1;
@@ -1362,7 +1365,7 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 		snprintf(name + strlen(name), sizeof(name) - strlen(name),
 			 " %s", buf);
 
-	ir->rc = mceusb_init_rc_dev(ir, intf);
+	ir->rc = mceusb_init_rc_dev(ir);
 	if (!ir->rc)
 		goto rc_dev_fail;
 
@@ -1408,6 +1411,7 @@ static int mceusb_dev_probe(struct usb_interface *intf,
 
 	/* Error-handling path */
 rc_dev_fail:
+	usb_put_dev(ir->usbdev);
 	usb_free_urb(ir->urb_in);
 urb_in_alloc_fail:
 	usb_free_coherent(dev, maxp, ir->buf_in, ir->dma_in);
@@ -1435,6 +1439,7 @@ static void mceusb_dev_disconnect(struct usb_interface *intf)
 	usb_kill_urb(ir->urb_in);
 	usb_free_urb(ir->urb_in);
 	usb_free_coherent(dev, ir->len_in, ir->buf_in, ir->dma_in);
+	usb_put_dev(dev);
 
 	kfree(ir);
 }
diff --git a/drivers/media/rc/nuvoton-cir.c b/drivers/media/rc/nuvoton-cir.c
index 7f4fd859bba5..9c2c8635ff33 100644
--- a/drivers/media/rc/nuvoton-cir.c
+++ b/drivers/media/rc/nuvoton-cir.c
@@ -229,7 +229,6 @@ static int nvt_hw_detect(struct nvt_dev *nvt)
 {
 	unsigned long flags;
 	u8 chip_major, chip_minor;
-	int ret = 0;
 	char chip_id[12];
 	bool chip_unknown = false;
 
@@ -285,7 +284,7 @@ static int nvt_hw_detect(struct nvt_dev *nvt)
 	nvt->chip_minor = chip_minor;
 	spin_unlock_irqrestore(&nvt->nvt_lock, flags);
 
-	return ret;
+	return 0;
 }
 
 static void nvt_cir_ldev_init(struct nvt_dev *nvt)
@@ -1177,7 +1176,6 @@ static int nvt_suspend(struct pnp_dev *pdev, pm_message_t state)
 
 static int nvt_resume(struct pnp_dev *pdev)
 {
-	int ret = 0;
 	struct nvt_dev *nvt = pnp_get_drvdata(pdev);
 
 	nvt_dbg("%s called", __func__);
@@ -1195,7 +1193,7 @@ static int nvt_resume(struct pnp_dev *pdev)
 	nvt_cir_regs_init(nvt);
 	nvt_cir_wake_regs_init(nvt);
 
-	return ret;
+	return 0;
 }
 
 static void nvt_shutdown(struct pnp_dev *pdev)
diff --git a/drivers/media/rc/st_rc.c b/drivers/media/rc/st_rc.c
index 5c151351afa4..0e758ae2e529 100644
--- a/drivers/media/rc/st_rc.c
+++ b/drivers/media/rc/st_rc.c
@@ -22,8 +22,8 @@ struct st_rc_device {
 	int				irq;
 	int				irq_wake;
 	struct clk			*sys_clock;
-	void				*base;	/* Register base address */
-	void				*rx_base;/* RX Register base address */
+	volatile void __iomem		*base;	/* Register base address */
+	volatile void __iomem		*rx_base;/* RX Register base address */
 	struct rc_dev			*rdev;
 	bool				overclocking;
 	int				sample_mult;
@@ -267,8 +267,8 @@ static int st_rc_probe(struct platform_device *pdev)
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 
 	rc_dev->base = devm_ioremap_resource(dev, res);
-	if (IS_ERR(rc_dev->base)) {
-		ret = PTR_ERR(rc_dev->base);
+	if (IS_ERR((__force void *)rc_dev->base)) {
+		ret = PTR_ERR((__force void *)rc_dev->base);
 		goto err;
 	}
 
@@ -278,7 +278,7 @@ static int st_rc_probe(struct platform_device *pdev)
 		rc_dev->rx_base = rc_dev->base;
 
 
-	rc_dev->rstc = reset_control_get(dev, NULL);
+	rc_dev->rstc = reset_control_get_optional(dev, NULL);
 	if (IS_ERR(rc_dev->rstc))
 		rc_dev->rstc = NULL;
 
@@ -376,9 +376,10 @@ static int st_rc_resume(struct device *dev)
 	return 0;
 }
 
-static SIMPLE_DEV_PM_OPS(st_rc_pm_ops, st_rc_suspend, st_rc_resume);
 #endif
 
+static SIMPLE_DEV_PM_OPS(st_rc_pm_ops, st_rc_suspend, st_rc_resume);
+
 #ifdef CONFIG_OF
 static struct of_device_id st_rc_match[] = {
 	{ .compatible = "st,comms-irb", },
@@ -391,11 +392,8 @@ MODULE_DEVICE_TABLE(of, st_rc_match);
 static struct platform_driver st_rc_driver = {
 	.driver = {
 		.name = IR_ST_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(st_rc_match),
-#ifdef CONFIG_PM
 		.pm     = &st_rc_pm_ops,
-#endif
 	},
 	.probe = st_rc_probe,
 	.remove = st_rc_remove,
diff --git a/drivers/media/rc/streamzap.c b/drivers/media/rc/streamzap.c
index 80c4feeb01ea..bf4a44272f0e 100644
--- a/drivers/media/rc/streamzap.c
+++ b/drivers/media/rc/streamzap.c
@@ -362,16 +362,14 @@ static int streamzap_probe(struct usb_interface *intf,
 	}
 
 	sz->endpoint = &(iface_host->endpoint[0].desc);
-	if ((sz->endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK)
-	    != USB_DIR_IN) {
+	if (!usb_endpoint_dir_in(sz->endpoint)) {
 		dev_err(&intf->dev, "%s: endpoint doesn't match input device "
 			"02%02x\n", __func__, sz->endpoint->bEndpointAddress);
 		retval = -ENODEV;
 		goto free_sz;
 	}
 
-	if ((sz->endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-	    != USB_ENDPOINT_XFER_INT) {
+	if (!usb_endpoint_xfer_int(sz->endpoint)) {
 		dev_err(&intf->dev, "%s: endpoint attributes don't match xfer "
 			"02%02x\n", __func__, sz->endpoint->bmAttributes);
 		retval = -ENODEV;
diff --git a/drivers/media/tuners/Kconfig b/drivers/media/tuners/Kconfig
index d79fd1ce5a18..f039dc2a21cf 100644
--- a/drivers/media/tuners/Kconfig
+++ b/drivers/media/tuners/Kconfig
@@ -204,6 +204,7 @@ config MEDIA_TUNER_FC0013
 config MEDIA_TUNER_TDA18212
 	tristate "NXP TDA18212 silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  NXP TDA18212 silicon tuner driver.
@@ -226,6 +227,7 @@ config MEDIA_TUNER_FC2580
 config MEDIA_TUNER_M88TS2022
 	tristate "Montage M88TS2022 silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Montage M88TS2022 silicon tuner driver.
@@ -247,6 +249,7 @@ config MEDIA_TUNER_SI2157
 config MEDIA_TUNER_IT913X
 	tristate "ITE Tech IT913x silicon tuner"
 	depends on MEDIA_SUPPORT && I2C
+	select REGMAP_I2C
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  ITE Tech IT913x silicon tuner driver.
@@ -257,4 +260,18 @@ config MEDIA_TUNER_R820T
 	default m if !MEDIA_SUBDRV_AUTOSELECT
 	help
 	  Rafael Micro R820T silicon tuner driver.
+
+config MEDIA_TUNER_MXL301RF
+	tristate "MaxLinear MxL301RF tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  MaxLinear MxL301RF OFDM tuner driver.
+
+config MEDIA_TUNER_QM1D1C0042
+	tristate "Sharp QM1D1C0042 tuner"
+	depends on MEDIA_SUPPORT && I2C
+	default m if !MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Sharp QM1D1C0042 trellis coded 8PSK tuner driver.
 endmenu
diff --git a/drivers/media/tuners/Makefile b/drivers/media/tuners/Makefile
index 5591699755ba..49fcf8033848 100644
--- a/drivers/media/tuners/Makefile
+++ b/drivers/media/tuners/Makefile
@@ -37,8 +37,10 @@ obj-$(CONFIG_MEDIA_TUNER_M88TS2022) += m88ts2022.o
 obj-$(CONFIG_MEDIA_TUNER_FC0011) += fc0011.o
 obj-$(CONFIG_MEDIA_TUNER_FC0012) += fc0012.o
 obj-$(CONFIG_MEDIA_TUNER_FC0013) += fc0013.o
-obj-$(CONFIG_MEDIA_TUNER_IT913X) += tuner_it913x.o
+obj-$(CONFIG_MEDIA_TUNER_IT913X) += it913x.o
 obj-$(CONFIG_MEDIA_TUNER_R820T) += r820t.o
+obj-$(CONFIG_MEDIA_TUNER_MXL301RF) += mxl301rf.o
+obj-$(CONFIG_MEDIA_TUNER_QM1D1C0042) += qm1d1c0042.o
 
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
diff --git a/drivers/media/tuners/e4000.c b/drivers/media/tuners/e4000.c
index 90d93348f20c..510239f80c0d 100644
--- a/drivers/media/tuners/e4000.c
+++ b/drivers/media/tuners/e4000.c
@@ -26,7 +26,7 @@ static int e4000_init(struct dvb_frontend *fe)
 	struct e4000 *s = fe->tuner_priv;
 	int ret;
 
-	dev_dbg(&s->client->dev, "%s:\n", __func__);
+	dev_dbg(&s->client->dev, "\n");
 
 	/* dummy I2C to ensure I2C wakes up */
 	ret = regmap_write(s->regmap, 0x02, 0x40);
@@ -87,7 +87,7 @@ static int e4000_init(struct dvb_frontend *fe)
 	s->active = true;
 err:
 	if (ret)
-		dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -97,7 +97,7 @@ static int e4000_sleep(struct dvb_frontend *fe)
 	struct e4000 *s = fe->tuner_priv;
 	int ret;
 
-	dev_dbg(&s->client->dev, "%s:\n", __func__);
+	dev_dbg(&s->client->dev, "\n");
 
 	s->active = false;
 
@@ -106,7 +106,7 @@ static int e4000_sleep(struct dvb_frontend *fe)
 		goto err;
 err:
 	if (ret)
-		dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -121,9 +121,8 @@ static int e4000_set_params(struct dvb_frontend *fe)
 	u8 buf[5], i_data[4], q_data[4];
 
 	dev_dbg(&s->client->dev,
-			"%s: delivery_system=%d frequency=%u bandwidth_hz=%u\n",
-			__func__, c->delivery_system, c->frequency,
-			c->bandwidth_hz);
+			"delivery_system=%d frequency=%u bandwidth_hz=%u\n",
+			c->delivery_system, c->frequency, c->bandwidth_hz);
 
 	/* gain control manual */
 	ret = regmap_write(s->regmap, 0x1a, 0x00);
@@ -150,9 +149,8 @@ static int e4000_set_params(struct dvb_frontend *fe)
 	buf[3] = 0x00;
 	buf[4] = e4000_pll_lut[i].div;
 
-	dev_dbg(&s->client->dev,
-			"%s: f_vco=%llu pll div=%d sigma_delta=%04x\n",
-			__func__, f_vco, buf[0], sigma_delta);
+	dev_dbg(&s->client->dev, "f_vco=%llu pll div=%d sigma_delta=%04x\n",
+			f_vco, buf[0], sigma_delta);
 
 	ret = regmap_bulk_write(s->regmap, 0x09, buf, 5);
 	if (ret)
@@ -253,7 +251,7 @@ static int e4000_set_params(struct dvb_frontend *fe)
 		goto err;
 err:
 	if (ret)
-		dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -262,7 +260,7 @@ static int e4000_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
 	struct e4000 *s = fe->tuner_priv;
 
-	dev_dbg(&s->client->dev, "%s:\n", __func__);
+	dev_dbg(&s->client->dev, "\n");
 
 	*frequency = 0; /* Zero-IF */
 
@@ -276,10 +274,9 @@ static int e4000_set_lna_gain(struct dvb_frontend *fe)
 	int ret;
 	u8 u8tmp;
 
-	dev_dbg(&s->client->dev, "%s: lna auto=%d->%d val=%d->%d\n",
-			__func__, s->lna_gain_auto->cur.val,
-			s->lna_gain_auto->val, s->lna_gain->cur.val,
-			s->lna_gain->val);
+	dev_dbg(&s->client->dev, "lna auto=%d->%d val=%d->%d\n",
+			s->lna_gain_auto->cur.val, s->lna_gain_auto->val,
+			s->lna_gain->cur.val, s->lna_gain->val);
 
 	if (s->lna_gain_auto->val && s->if_gain_auto->cur.val)
 		u8tmp = 0x17;
@@ -301,7 +298,7 @@ static int e4000_set_lna_gain(struct dvb_frontend *fe)
 	}
 err:
 	if (ret)
-		dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -312,10 +309,9 @@ static int e4000_set_mixer_gain(struct dvb_frontend *fe)
 	int ret;
 	u8 u8tmp;
 
-	dev_dbg(&s->client->dev, "%s: mixer auto=%d->%d val=%d->%d\n",
-			__func__, s->mixer_gain_auto->cur.val,
-			s->mixer_gain_auto->val, s->mixer_gain->cur.val,
-			s->mixer_gain->val);
+	dev_dbg(&s->client->dev, "mixer auto=%d->%d val=%d->%d\n",
+			s->mixer_gain_auto->cur.val, s->mixer_gain_auto->val,
+			s->mixer_gain->cur.val, s->mixer_gain->val);
 
 	if (s->mixer_gain_auto->val)
 		u8tmp = 0x15;
@@ -333,7 +329,7 @@ static int e4000_set_mixer_gain(struct dvb_frontend *fe)
 	}
 err:
 	if (ret)
-		dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -345,10 +341,9 @@ static int e4000_set_if_gain(struct dvb_frontend *fe)
 	u8 buf[2];
 	u8 u8tmp;
 
-	dev_dbg(&s->client->dev, "%s: if auto=%d->%d val=%d->%d\n",
-			__func__, s->if_gain_auto->cur.val,
-			s->if_gain_auto->val, s->if_gain->cur.val,
-			s->if_gain->val);
+	dev_dbg(&s->client->dev, "if auto=%d->%d val=%d->%d\n",
+			s->if_gain_auto->cur.val, s->if_gain_auto->val,
+			s->if_gain->cur.val, s->if_gain->val);
 
 	if (s->if_gain_auto->val && s->lna_gain_auto->cur.val)
 		u8tmp = 0x17;
@@ -372,7 +367,7 @@ static int e4000_set_if_gain(struct dvb_frontend *fe)
 	}
 err:
 	if (ret)
-		dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -390,7 +385,7 @@ static int e4000_pll_lock(struct dvb_frontend *fe)
 	s->pll_lock->val = (utmp & 0x01);
 err:
 	if (ret)
-		dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&s->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -400,7 +395,7 @@ static int e4000_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 	struct e4000 *s = container_of(ctrl->handler, struct e4000, hdl);
 	int ret;
 
-	if (s->active == false)
+	if (!s->active)
 		return 0;
 
 	switch (ctrl->id) {
@@ -408,8 +403,8 @@ static int e4000_g_volatile_ctrl(struct v4l2_ctrl *ctrl)
 		ret = e4000_pll_lock(s->fe);
 		break;
 	default:
-		dev_dbg(&s->client->dev, "%s: unknown ctrl: id=%d name=%s\n",
-				__func__, ctrl->id, ctrl->name);
+		dev_dbg(&s->client->dev, "unknown ctrl: id=%d name=%s\n",
+				ctrl->id, ctrl->name);
 		ret = -EINVAL;
 	}
 
@@ -423,7 +418,7 @@ static int e4000_s_ctrl(struct v4l2_ctrl *ctrl)
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
 
-	if (s->active == false)
+	if (!s->active)
 		return 0;
 
 	switch (ctrl->id) {
@@ -445,8 +440,8 @@ static int e4000_s_ctrl(struct v4l2_ctrl *ctrl)
 		ret = e4000_set_if_gain(s->fe);
 		break;
 	default:
-		dev_dbg(&s->client->dev, "%s: unknown ctrl: id=%d name=%s\n",
-				__func__, ctrl->id, ctrl->name);
+		dev_dbg(&s->client->dev, "unknown ctrl: id=%d name=%s\n",
+				ctrl->id, ctrl->name);
 		ret = -EINVAL;
 	}
 
@@ -494,7 +489,7 @@ static int e4000_probe(struct i2c_client *client,
 	s = kzalloc(sizeof(struct e4000), GFP_KERNEL);
 	if (!s) {
 		ret = -ENOMEM;
-		dev_err(&client->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		dev_err(&client->dev, "kzalloc() failed\n");
 		goto err;
 	}
 
@@ -512,7 +507,7 @@ static int e4000_probe(struct i2c_client *client,
 	if (ret)
 		goto err;
 
-	dev_dbg(&s->client->dev, "%s: chip id=%02x\n", __func__, utmp);
+	dev_dbg(&s->client->dev, "chip id=%02x\n", utmp);
 
 	if (utmp != 0x40) {
 		ret = -ENODEV;
@@ -559,9 +554,7 @@ static int e4000_probe(struct i2c_client *client,
 	s->sd.ctrl_handler = &s->hdl;
 #endif
 
-	dev_info(&s->client->dev,
-			"%s: Elonics E4000 successfully identified\n",
-			KBUILD_MODNAME);
+	dev_info(&s->client->dev, "Elonics E4000 successfully identified\n");
 
 	fe->tuner_priv = s;
 	memcpy(&fe->ops.tuner_ops, &e4000_tuner_ops,
@@ -573,7 +566,7 @@ static int e4000_probe(struct i2c_client *client,
 	return 0;
 err:
 	if (ret) {
-		dev_dbg(&client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&client->dev, "failed=%d\n", ret);
 		kfree(s);
 	}
 
@@ -586,7 +579,7 @@ static int e4000_remove(struct i2c_client *client)
 	struct e4000 *s = container_of(sd, struct e4000, sd);
 	struct dvb_frontend *fe = s->fe;
 
-	dev_dbg(&client->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 #if IS_ENABLED(CONFIG_VIDEO_V4L2)
 	v4l2_ctrl_handler_free(&s->hdl);
diff --git a/drivers/media/tuners/it913x.c b/drivers/media/tuners/it913x.c
new file mode 100644
index 000000000000..a076c87eda7a
--- /dev/null
+++ b/drivers/media/tuners/it913x.c
@@ -0,0 +1,478 @@
+/*
+ * ITE IT913X silicon tuner driver
+ *
+ *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
+ *  IT9137 Copyright (C) ITE Tech Inc.
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; if not, write to the Free Software
+ *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.=
+ */
+
+#include "it913x.h"
+#include <linux/regmap.h>
+
+struct it913x_dev {
+	struct i2c_client *client;
+	struct regmap *regmap;
+	struct dvb_frontend *fe;
+	u8 chip_ver:2;
+	u8 role:2;
+	u16 xtal;
+	u8 fdiv;
+	u8 clk_mode;
+	u32 fn_min;
+	bool active;
+};
+
+static int it913x_init(struct dvb_frontend *fe)
+{
+	struct it913x_dev *dev = fe->tuner_priv;
+	int ret;
+	unsigned int utmp;
+	u8 iqik_m_cal, nv_val, buf[2];
+	static const u8 nv[] = {48, 32, 24, 16, 12, 8, 6, 4, 2};
+	unsigned long timeout;
+
+	dev_dbg(&dev->client->dev, "role %u\n", dev->role);
+
+	ret = regmap_write(dev->regmap, 0x80ec4c, 0x68);
+	if (ret)
+		goto err;
+
+	usleep_range(10000, 100000);
+
+	ret = regmap_read(dev->regmap, 0x80ec86, &utmp);
+	if (ret)
+		goto err;
+
+	switch (utmp) {
+	case 0:
+		/* 12.000 MHz */
+		dev->clk_mode = utmp;
+		dev->xtal = 2000;
+		dev->fdiv = 3;
+		iqik_m_cal = 16;
+		break;
+	case 1:
+		/* 20.480 MHz */
+		dev->clk_mode = utmp;
+		dev->xtal = 640;
+		dev->fdiv = 1;
+		iqik_m_cal = 6;
+		break;
+	default:
+		dev_err(&dev->client->dev, "unknown clock identifier %d\n", utmp);
+		goto err;
+	}
+
+	ret = regmap_read(dev->regmap, 0x80ed03,  &utmp);
+	if (ret)
+		goto err;
+
+	else if (utmp < ARRAY_SIZE(nv))
+		nv_val = nv[utmp];
+	else
+		nv_val = 2;
+
+	#define TIMEOUT 50
+	timeout = jiffies + msecs_to_jiffies(TIMEOUT);
+	while (!time_after(jiffies, timeout)) {
+		ret = regmap_bulk_read(dev->regmap, 0x80ed23, buf, 2);
+		if (ret)
+			goto err;
+
+		utmp = (buf[1] << 8) | (buf[0] << 0);
+		if (utmp)
+			break;
+	}
+
+	dev_dbg(&dev->client->dev, "r_fbc_m_bdry took %u ms, val %u\n",
+			jiffies_to_msecs(jiffies) -
+			(jiffies_to_msecs(timeout) - TIMEOUT), utmp);
+
+	dev->fn_min = dev->xtal * utmp;
+	dev->fn_min /= (dev->fdiv * nv_val);
+	dev->fn_min *= 1000;
+	dev_dbg(&dev->client->dev, "fn_min %u\n", dev->fn_min);
+
+	/*
+	 * Chip version BX never sets that flag so we just wait 50ms in that
+	 * case. It is possible poll BX similarly than AX and then timeout in
+	 * order to get 50ms delay, but that causes about 120 extra I2C
+	 * messages. As for now, we just wait and reduce IO.
+	 */
+	if (dev->chip_ver == 1) {
+		#define TIMEOUT 50
+		timeout = jiffies + msecs_to_jiffies(TIMEOUT);
+		while (!time_after(jiffies, timeout)) {
+			ret = regmap_read(dev->regmap, 0x80ec82, &utmp);
+			if (ret)
+				goto err;
+
+			if (utmp)
+				break;
+		}
+
+		dev_dbg(&dev->client->dev, "p_tsm_init_mode took %u ms, val %u\n",
+				jiffies_to_msecs(jiffies) -
+				(jiffies_to_msecs(timeout) - TIMEOUT), utmp);
+	} else {
+		msleep(50);
+	}
+
+	ret = regmap_write(dev->regmap, 0x80ed81, iqik_m_cal);
+	if (ret)
+		goto err;
+
+	ret = regmap_write(dev->regmap, 0x80ec57, 0x00);
+	if (ret)
+		goto err;
+
+	ret = regmap_write(dev->regmap, 0x80ec58, 0x00);
+	if (ret)
+		goto err;
+
+	ret = regmap_write(dev->regmap, 0x80ec40, 0x01);
+	if (ret)
+		goto err;
+
+	dev->active = true;
+
+	return 0;
+err:
+	dev_dbg(&dev->client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int it913x_sleep(struct dvb_frontend *fe)
+{
+	struct it913x_dev *dev = fe->tuner_priv;
+	int ret, len;
+
+	dev_dbg(&dev->client->dev, "role %u\n", dev->role);
+
+	dev->active = false;
+
+	ret  = regmap_bulk_write(dev->regmap, 0x80ec40, "\x00", 1);
+	if (ret)
+		goto err;
+
+	/*
+	 * Writing '0x00' to master tuner register '0x80ec08' causes slave tuner
+	 * communication lost. Due to that, we cannot put master full sleep.
+	 */
+	if (dev->role == IT913X_ROLE_DUAL_MASTER)
+		len = 4;
+	else
+		len = 15;
+
+	dev_dbg(&dev->client->dev, "role %u, len %d\n", dev->role, len);
+
+	ret = regmap_bulk_write(dev->regmap, 0x80ec02,
+			"\x3f\x1f\x3f\x3e\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00",
+			len);
+	if (ret)
+		goto err;
+
+	ret = regmap_bulk_write(dev->regmap, 0x80ec12, "\x00\x00\x00\x00", 4);
+	if (ret)
+		goto err;
+
+	ret = regmap_bulk_write(dev->regmap, 0x80ec17,
+			"\x00\x00\x00\x00\x00\x00\x00\x00\x00", 9);
+	if (ret)
+		goto err;
+
+	ret = regmap_bulk_write(dev->regmap, 0x80ec22,
+			"\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00", 10);
+	if (ret)
+		goto err;
+
+	ret = regmap_bulk_write(dev->regmap, 0x80ec20, "\x00", 1);
+	if (ret)
+		goto err;
+
+	ret = regmap_bulk_write(dev->regmap, 0x80ec3f, "\x01", 1);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&dev->client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int it913x_set_params(struct dvb_frontend *fe)
+{
+	struct it913x_dev *dev = fe->tuner_priv;
+	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
+	int ret;
+	unsigned int utmp;
+	u32 pre_lo_freq, t_cal_freq;
+	u16 iqik_m_cal, n_div;
+	u8 u8tmp, n, l_band, lna_band;
+
+	dev_dbg(&dev->client->dev, "role=%u, frequency %u, bandwidth_hz %u\n",
+			dev->role, c->frequency, c->bandwidth_hz);
+
+	if (!dev->active) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (c->frequency <=         74000000) {
+		n_div = 48;
+		n = 0;
+	} else if (c->frequency <= 111000000) {
+		n_div = 32;
+		n = 1;
+	} else if (c->frequency <= 148000000) {
+		n_div = 24;
+		n = 2;
+	} else if (c->frequency <= 222000000) {
+		n_div = 16;
+		n = 3;
+	} else if (c->frequency <= 296000000) {
+		n_div = 12;
+		n = 4;
+	} else if (c->frequency <= 445000000) {
+		n_div = 8;
+		n = 5;
+	} else if (c->frequency <= dev->fn_min) {
+		n_div = 6;
+		n = 6;
+	} else if (c->frequency <= 950000000) {
+		n_div = 4;
+		n = 7;
+	} else {
+		n_div = 2;
+		n = 0;
+	}
+
+	ret = regmap_read(dev->regmap, 0x80ed81, &utmp);
+	if (ret)
+		goto err;
+
+	iqik_m_cal = utmp * n_div;
+
+	if (utmp < 0x20) {
+		if (dev->clk_mode == 0)
+			iqik_m_cal = (iqik_m_cal * 9) >> 5;
+		else
+			iqik_m_cal >>= 1;
+	} else {
+		iqik_m_cal = 0x40 - iqik_m_cal;
+		if (dev->clk_mode == 0)
+			iqik_m_cal = ~((iqik_m_cal * 9) >> 5);
+		else
+			iqik_m_cal = ~(iqik_m_cal >> 1);
+	}
+
+	t_cal_freq = (c->frequency / 1000) * n_div * dev->fdiv;
+	pre_lo_freq = t_cal_freq / dev->xtal;
+	utmp = pre_lo_freq * dev->xtal;
+
+	if ((t_cal_freq - utmp) >= (dev->xtal >> 1))
+		pre_lo_freq++;
+
+	pre_lo_freq += (u32) n << 13;
+	/* Frequency OMEGA_IQIK_M_CAL_MID*/
+	t_cal_freq = pre_lo_freq + (u32)iqik_m_cal;
+	dev_dbg(&dev->client->dev, "t_cal_freq %u, pre_lo_freq %u\n",
+			t_cal_freq, pre_lo_freq);
+
+	if (c->frequency <=         440000000) {
+		l_band = 0;
+		lna_band = 0;
+	} else if (c->frequency <=  484000000) {
+		l_band = 1;
+		lna_band = 1;
+	} else if (c->frequency <=  533000000) {
+		l_band = 1;
+		lna_band = 2;
+	} else if (c->frequency <=  587000000) {
+		l_band = 1;
+		lna_band = 3;
+	} else if (c->frequency <=  645000000) {
+		l_band = 1;
+		lna_band = 4;
+	} else if (c->frequency <=  710000000) {
+		l_band = 1;
+		lna_band = 5;
+	} else if (c->frequency <=  782000000) {
+		l_band = 1;
+		lna_band = 6;
+	} else if (c->frequency <=  860000000) {
+		l_band = 1;
+		lna_band = 7;
+	} else if (c->frequency <= 1492000000) {
+		l_band = 1;
+		lna_band = 0;
+	} else if (c->frequency <= 1685000000) {
+		l_band = 1;
+		lna_band = 1;
+	} else {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* XXX: latest windows driver does not set that at all */
+	ret = regmap_write(dev->regmap, 0x80ee06, lna_band);
+	if (ret)
+		goto err;
+
+	if (c->bandwidth_hz <=      5000000)
+		u8tmp = 0;
+	else if (c->bandwidth_hz <= 6000000)
+		u8tmp = 2;
+	else if (c->bandwidth_hz <= 7000000)
+		u8tmp = 4;
+	else
+		u8tmp = 6;       /* 8000000 */
+
+	ret = regmap_write(dev->regmap, 0x80ec56, u8tmp);
+	if (ret)
+		goto err;
+
+	/* XXX: latest windows driver sets different value (a8 != 68) */
+	ret = regmap_write(dev->regmap, 0x80ec4c, 0xa0 | (l_band << 3));
+	if (ret)
+		goto err;
+
+	ret = regmap_write(dev->regmap, 0x80ec4d, (t_cal_freq >> 0) & 0xff);
+	if (ret)
+		goto err;
+
+	ret = regmap_write(dev->regmap, 0x80ec4e, (t_cal_freq >> 8) & 0xff);
+	if (ret)
+		goto err;
+
+	ret = regmap_write(dev->regmap, 0x80011e, (pre_lo_freq >> 0) & 0xff);
+	if (ret)
+		goto err;
+
+	ret = regmap_write(dev->regmap, 0x80011f, (pre_lo_freq >> 8) & 0xff);
+	if (ret)
+		goto err;
+
+	return 0;
+err:
+	dev_dbg(&dev->client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static const struct dvb_tuner_ops it913x_tuner_ops = {
+	.info = {
+		.name           = "ITE IT913X",
+		.frequency_min  = 174000000,
+		.frequency_max  = 862000000,
+	},
+
+	.init = it913x_init,
+	.sleep = it913x_sleep,
+	.set_params = it913x_set_params,
+};
+
+static int it913x_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
+{
+	struct it913x_config *cfg = client->dev.platform_data;
+	struct dvb_frontend *fe = cfg->fe;
+	struct it913x_dev *dev;
+	int ret;
+	char *chip_ver_str;
+	static const struct regmap_config regmap_config = {
+		.reg_bits = 24,
+		.val_bits = 8,
+	};
+
+	dev = kzalloc(sizeof(struct it913x_dev), GFP_KERNEL);
+	if (dev == NULL) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "kzalloc() failed\n");
+		goto err;
+	}
+
+	dev->client = client;
+	dev->fe = cfg->fe;
+	dev->chip_ver = cfg->chip_ver;
+	dev->role = cfg->role;
+	dev->regmap = regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err_kfree;
+	}
+
+	fe->tuner_priv = dev;
+	memcpy(&fe->ops.tuner_ops, &it913x_tuner_ops,
+			sizeof(struct dvb_tuner_ops));
+	i2c_set_clientdata(client, dev);
+
+	if (dev->chip_ver == 1)
+		chip_ver_str = "AX";
+	else if (dev->chip_ver == 2)
+		chip_ver_str = "BX";
+	else
+		chip_ver_str = "??";
+
+	dev_info(&dev->client->dev, "ITE IT913X %s successfully attached\n",
+			chip_ver_str);
+	dev_dbg(&dev->client->dev, "chip_ver %u, role %u\n",
+			dev->chip_ver, dev->role);
+	return 0;
+
+err_kfree:
+	kfree(dev);
+err:
+	dev_dbg(&client->dev, "failed %d\n", ret);
+	return ret;
+}
+
+static int it913x_remove(struct i2c_client *client)
+{
+	struct it913x_dev *dev = i2c_get_clientdata(client);
+	struct dvb_frontend *fe = dev->fe;
+
+	dev_dbg(&client->dev, "\n");
+
+	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
+	fe->tuner_priv = NULL;
+	regmap_exit(dev->regmap);
+	kfree(dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id it913x_id_table[] = {
+	{"it913x", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, it913x_id_table);
+
+static struct i2c_driver it913x_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "it913x",
+	},
+	.probe		= it913x_probe,
+	.remove		= it913x_remove,
+	.id_table	= it913x_id_table,
+};
+
+module_i2c_driver(it913x_driver);
+
+MODULE_DESCRIPTION("ITE IT913X silicon tuner driver");
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/tuner_it913x.h b/drivers/media/tuners/it913x.h
index 12dd36bd9e79..33de53d4a566 100644
--- a/drivers/media/tuners/tuner_it913x.h
+++ b/drivers/media/tuners/it913x.h
@@ -25,21 +25,30 @@
 
 #include "dvb_frontend.h"
 
-#if defined(CONFIG_MEDIA_TUNER_IT913X) || \
-	(defined(CONFIG_MEDIA_TUNER_IT913X_MODULE) && defined(MODULE))
-extern struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
-	struct i2c_adapter *i2c_adap,
-	u8 i2c_addr,
-	u8 config);
-#else
-static inline struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
-	struct i2c_adapter *i2c_adap,
-	u8 i2c_addr,
-	u8 config)
-{
-	pr_warn("%s: driver disabled by Kconfig\n", __func__);
-	return NULL;
-}
-#endif
+/*
+ * I2C address
+ * 0x38, 0x3a, 0x3c, 0x3e
+ */
+struct it913x_config {
+	/*
+	 * pointer to DVB frontend
+	 */
+	struct dvb_frontend *fe;
+
+	/*
+	 * chip version
+	 * 1 = IT9135 AX
+	 * 2 = IT9135 BX
+	 */
+	unsigned int chip_ver:2;
+
+	/*
+	 * tuner role
+	 */
+#define IT913X_ROLE_SINGLE         0
+#define IT913X_ROLE_DUAL_MASTER    1
+#define IT913X_ROLE_DUAL_SLAVE     2
+	unsigned int role:2;
+};
 
 #endif
diff --git a/drivers/media/tuners/m88ts2022.c b/drivers/media/tuners/m88ts2022.c
index 40c42dec721b..caa542346891 100644
--- a/drivers/media/tuners/m88ts2022.c
+++ b/drivers/media/tuners/m88ts2022.c
@@ -18,120 +18,11 @@
 
 #include "m88ts2022_priv.h"
 
-/* write multiple registers */
-static int m88ts2022_wr_regs(struct m88ts2022_priv *priv,
-		u8 reg, const u8 *val, int len)
+static int m88ts2022_cmd(struct m88ts2022_dev *dev, int op, int sleep, u8 reg,
+		u8 mask, u8 val, u8 *reg_val)
 {
-#define MAX_WR_LEN 3
-#define MAX_WR_XFER_LEN (MAX_WR_LEN + 1)
-	int ret;
-	u8 buf[MAX_WR_XFER_LEN];
-	struct i2c_msg msg[1] = {
-		{
-			.addr = priv->client->addr,
-			.flags = 0,
-			.len = 1 + len,
-			.buf = buf,
-		}
-	};
-
-	if (WARN_ON(len > MAX_WR_LEN))
-		return -EINVAL;
-
-	buf[0] = reg;
-	memcpy(&buf[1], val, len);
-
-	ret = i2c_transfer(priv->client->adapter, msg, 1);
-	if (ret == 1) {
-		ret = 0;
-	} else {
-		dev_warn(&priv->client->dev,
-				"%s: i2c wr failed=%d reg=%02x len=%d\n",
-				KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-
-	return ret;
-}
-
-/* read multiple registers */
-static int m88ts2022_rd_regs(struct m88ts2022_priv *priv, u8 reg,
-		u8 *val, int len)
-{
-#define MAX_RD_LEN 1
-#define MAX_RD_XFER_LEN (MAX_RD_LEN)
-	int ret;
-	u8 buf[MAX_RD_XFER_LEN];
-	struct i2c_msg msg[2] = {
-		{
-			.addr = priv->client->addr,
-			.flags = 0,
-			.len = 1,
-			.buf = &reg,
-		}, {
-			.addr = priv->client->addr,
-			.flags = I2C_M_RD,
-			.len = len,
-			.buf = buf,
-		}
-	};
-
-	if (WARN_ON(len > MAX_RD_LEN))
-		return -EINVAL;
-
-	ret = i2c_transfer(priv->client->adapter, msg, 2);
-	if (ret == 2) {
-		memcpy(val, buf, len);
-		ret = 0;
-	} else {
-		dev_warn(&priv->client->dev,
-				"%s: i2c rd failed=%d reg=%02x len=%d\n",
-				KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-
-	return ret;
-}
-
-/* write single register */
-static int m88ts2022_wr_reg(struct m88ts2022_priv *priv, u8 reg, u8 val)
-{
-	return m88ts2022_wr_regs(priv, reg, &val, 1);
-}
-
-/* read single register */
-static int m88ts2022_rd_reg(struct m88ts2022_priv *priv, u8 reg, u8 *val)
-{
-	return m88ts2022_rd_regs(priv, reg, val, 1);
-}
-
-/* write single register with mask */
-static int m88ts2022_wr_reg_mask(struct m88ts2022_priv *priv,
-		u8 reg, u8 val, u8 mask)
-{
-	int ret;
-	u8 u8tmp;
-
-	/* no need for read if whole reg is written */
-	if (mask != 0xff) {
-		ret = m88ts2022_rd_regs(priv, reg, &u8tmp, 1);
-		if (ret)
-			return ret;
-
-		val &= mask;
-		u8tmp &= ~mask;
-		val |= u8tmp;
-	}
-
-	return m88ts2022_wr_regs(priv, reg, &val, 1);
-}
-
-static int m88ts2022_cmd(struct dvb_frontend *fe,
-		int op, int sleep, u8 reg, u8 mask, u8 val, u8 *reg_val)
-{
-	struct m88ts2022_priv *priv = fe->tuner_priv;
 	int ret, i;
-	u8 u8tmp;
+	unsigned int utmp;
 	struct m88ts2022_reg_val reg_vals[] = {
 		{0x51, 0x1f - op},
 		{0x51, 0x1f},
@@ -140,12 +31,12 @@ static int m88ts2022_cmd(struct dvb_frontend *fe,
 	};
 
 	for (i = 0; i < 2; i++) {
-		dev_dbg(&priv->client->dev,
-				"%s: i=%d op=%02x reg=%02x mask=%02x val=%02x\n",
-				__func__, i, op, reg, mask, val);
+		dev_dbg(&dev->client->dev,
+				"i=%d op=%02x reg=%02x mask=%02x val=%02x\n",
+				i, op, reg, mask, val);
 
 		for (i = 0; i < ARRAY_SIZE(reg_vals); i++) {
-			ret = m88ts2022_wr_reg(priv, reg_vals[i].reg,
+			ret = regmap_write(dev->regmap, reg_vals[i].reg,
 					reg_vals[i].val);
 			if (ret)
 				goto err;
@@ -153,37 +44,38 @@ static int m88ts2022_cmd(struct dvb_frontend *fe,
 
 		usleep_range(sleep * 1000, sleep * 10000);
 
-		ret = m88ts2022_rd_reg(priv, reg, &u8tmp);
+		ret = regmap_read(dev->regmap, reg, &utmp);
 		if (ret)
 			goto err;
 
-		if ((u8tmp & mask) != val)
+		if ((utmp & mask) != val)
 			break;
 	}
 
 	if (reg_val)
-		*reg_val = u8tmp;
+		*reg_val = utmp;
 err:
 	return ret;
 }
 
 static int m88ts2022_set_params(struct dvb_frontend *fe)
 {
-	struct m88ts2022_priv *priv = fe->tuner_priv;
+	struct m88ts2022_dev *dev = fe->tuner_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret;
-	unsigned int frequency_khz, frequency_offset_khz, f_3db_hz;
+	unsigned int utmp, frequency_khz, frequency_offset_khz, f_3db_hz;
 	unsigned int f_ref_khz, f_vco_khz, div_ref, div_out, pll_n, gdiv28;
 	u8 buf[3], u8tmp, cap_code, lpf_gm, lpf_mxdiv, div_max, div_min;
 	u16 u16tmp;
-	dev_dbg(&priv->client->dev,
-			"%s: frequency=%d symbol_rate=%d rolloff=%d\n",
-			__func__, c->frequency, c->symbol_rate, c->rolloff);
+
+	dev_dbg(&dev->client->dev,
+			"frequency=%d symbol_rate=%d rolloff=%d\n",
+			c->frequency, c->symbol_rate, c->rolloff);
 	/*
 	 * Integer-N PLL synthesizer
 	 * kHz is used for all calculations to keep calculations within 32-bit
 	 */
-	f_ref_khz = DIV_ROUND_CLOSEST(priv->cfg.clock, 1000);
+	f_ref_khz = DIV_ROUND_CLOSEST(dev->cfg.clock, 1000);
 	div_ref = DIV_ROUND_CLOSEST(f_ref_khz, 2000);
 
 	if (c->symbol_rate < 5000000)
@@ -203,14 +95,14 @@ static int m88ts2022_set_params(struct dvb_frontend *fe)
 
 	buf[0] = u8tmp;
 	buf[1] = 0x40;
-	ret = m88ts2022_wr_regs(priv, 0x10, buf, 2);
+	ret = regmap_bulk_write(dev->regmap, 0x10, buf, 2);
 	if (ret)
 		goto err;
 
 	f_vco_khz = frequency_khz * div_out;
 	pll_n = f_vco_khz * div_ref / f_ref_khz;
 	pll_n += pll_n % 2;
-	priv->frequency_khz = pll_n * f_ref_khz / div_ref / div_out;
+	dev->frequency_khz = pll_n * f_ref_khz / div_ref / div_out;
 
 	if (pll_n < 4095)
 		u16tmp = pll_n - 1024;
@@ -222,88 +114,87 @@ static int m88ts2022_set_params(struct dvb_frontend *fe)
 	buf[0] = (u16tmp >> 8) & 0x3f;
 	buf[1] = (u16tmp >> 0) & 0xff;
 	buf[2] = div_ref - 8;
-	ret = m88ts2022_wr_regs(priv, 0x01, buf, 3);
+	ret = regmap_bulk_write(dev->regmap, 0x01, buf, 3);
 	if (ret)
 		goto err;
 
-	dev_dbg(&priv->client->dev,
-			"%s: frequency=%u offset=%d f_vco_khz=%u pll_n=%u div_ref=%u div_out=%u\n",
-			__func__, priv->frequency_khz,
-			priv->frequency_khz - c->frequency, f_vco_khz, pll_n,
-			div_ref, div_out);
+	dev_dbg(&dev->client->dev,
+			"frequency=%u offset=%d f_vco_khz=%u pll_n=%u div_ref=%u div_out=%u\n",
+			dev->frequency_khz, dev->frequency_khz - c->frequency,
+			f_vco_khz, pll_n, div_ref, div_out);
 
-	ret = m88ts2022_cmd(fe, 0x10, 5, 0x15, 0x40, 0x00, NULL);
+	ret = m88ts2022_cmd(dev, 0x10, 5, 0x15, 0x40, 0x00, NULL);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_rd_reg(priv, 0x14, &u8tmp);
+	ret = regmap_read(dev->regmap, 0x14, &utmp);
 	if (ret)
 		goto err;
 
-	u8tmp &= 0x7f;
-	if (u8tmp < 64) {
-		ret = m88ts2022_wr_reg_mask(priv, 0x10, 0x80, 0x80);
+	utmp &= 0x7f;
+	if (utmp < 64) {
+		ret = regmap_update_bits(dev->regmap, 0x10, 0x80, 0x80);
 		if (ret)
 			goto err;
 
-		ret = m88ts2022_wr_reg(priv, 0x11, 0x6f);
+		ret = regmap_write(dev->regmap, 0x11, 0x6f);
 		if (ret)
 			goto err;
 
-		ret = m88ts2022_cmd(fe, 0x10, 5, 0x15, 0x40, 0x00, NULL);
+		ret = m88ts2022_cmd(dev, 0x10, 5, 0x15, 0x40, 0x00, NULL);
 		if (ret)
 			goto err;
 	}
 
-	ret = m88ts2022_rd_reg(priv, 0x14, &u8tmp);
+	ret = regmap_read(dev->regmap, 0x14, &utmp);
 	if (ret)
 		goto err;
 
-	u8tmp &= 0x1f;
-	if (u8tmp > 19) {
-		ret = m88ts2022_wr_reg_mask(priv, 0x10, 0x00, 0x02);
+	utmp &= 0x1f;
+	if (utmp > 19) {
+		ret = regmap_update_bits(dev->regmap, 0x10, 0x02, 0x00);
 		if (ret)
 			goto err;
 	}
 
-	ret = m88ts2022_cmd(fe, 0x08, 5, 0x3c, 0xff, 0x00, NULL);
+	ret = m88ts2022_cmd(dev, 0x08, 5, 0x3c, 0xff, 0x00, NULL);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x25, 0x00);
+	ret = regmap_write(dev->regmap, 0x25, 0x00);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x27, 0x70);
+	ret = regmap_write(dev->regmap, 0x27, 0x70);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x41, 0x09);
+	ret = regmap_write(dev->regmap, 0x41, 0x09);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x08, 0x0b);
+	ret = regmap_write(dev->regmap, 0x08, 0x0b);
 	if (ret)
 		goto err;
 
 	/* filters */
 	gdiv28 = DIV_ROUND_CLOSEST(f_ref_khz * 1694U, 1000000U);
 
-	ret = m88ts2022_wr_reg(priv, 0x04, gdiv28);
+	ret = regmap_write(dev->regmap, 0x04, gdiv28);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	ret = m88ts2022_cmd(dev, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
 	if (ret)
 		goto err;
 
 	cap_code = u8tmp & 0x3f;
 
-	ret = m88ts2022_wr_reg(priv, 0x41, 0x0d);
+	ret = regmap_write(dev->regmap, 0x41, 0x0d);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	ret = m88ts2022_cmd(dev, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
 	if (ret)
 		goto err;
 
@@ -314,7 +205,7 @@ static int m88ts2022_set_params(struct dvb_frontend *fe)
 	div_min = gdiv28 * 78 / 100;
 	div_max = clamp_val(div_max, 0U, 63U);
 
-	f_3db_hz = c->symbol_rate * 135UL / 200UL;
+	f_3db_hz = mult_frac(c->symbol_rate, 135, 200);
 	f_3db_hz +=  2000000U + (frequency_offset_khz * 1000U);
 	f_3db_hz = clamp(f_3db_hz, 7000000U, 40000000U);
 
@@ -327,25 +218,25 @@ static int m88ts2022_set_params(struct dvb_frontend *fe)
 		lpf_mxdiv = DIV_ROUND_CLOSEST(++lpf_gm * LPF_COEFF * f_ref_khz, f_3db_hz);
 	lpf_mxdiv = clamp_val(lpf_mxdiv, 0U, div_max);
 
-	ret = m88ts2022_wr_reg(priv, 0x04, lpf_mxdiv);
+	ret = regmap_write(dev->regmap, 0x04, lpf_mxdiv);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x06, lpf_gm);
+	ret = regmap_write(dev->regmap, 0x06, lpf_gm);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	ret = m88ts2022_cmd(dev, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
 	if (ret)
 		goto err;
 
 	cap_code = u8tmp & 0x3f;
 
-	ret = m88ts2022_wr_reg(priv, 0x41, 0x09);
+	ret = regmap_write(dev->regmap, 0x41, 0x09);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_cmd(fe, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
+	ret = m88ts2022_cmd(dev, 0x04, 2, 0x26, 0xff, 0x00, &u8tmp);
 	if (ret)
 		goto err;
 
@@ -353,31 +244,31 @@ static int m88ts2022_set_params(struct dvb_frontend *fe)
 	cap_code = (cap_code + u8tmp) / 2;
 
 	u8tmp = cap_code | 0x80;
-	ret = m88ts2022_wr_reg(priv, 0x25, u8tmp);
+	ret = regmap_write(dev->regmap, 0x25, u8tmp);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x27, 0x30);
+	ret = regmap_write(dev->regmap, 0x27, 0x30);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x08, 0x09);
+	ret = regmap_write(dev->regmap, 0x08, 0x09);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_cmd(fe, 0x01, 20, 0x21, 0xff, 0x00, NULL);
+	ret = m88ts2022_cmd(dev, 0x01, 20, 0x21, 0xff, 0x00, NULL);
 	if (ret)
 		goto err;
 err:
 	if (ret)
-		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 
 	return ret;
 }
 
 static int m88ts2022_init(struct dvb_frontend *fe)
 {
-	struct m88ts2022_priv *priv = fe->tuner_priv;
+	struct m88ts2022_dev *dev = fe->tuner_priv;
 	int ret, i;
 	u8 u8tmp;
 	static const struct m88ts2022_reg_val reg_vals[] = {
@@ -393,23 +284,24 @@ static int m88ts2022_init(struct dvb_frontend *fe)
 		{0x24, 0x02},
 		{0x12, 0xa0},
 	};
-	dev_dbg(&priv->client->dev, "%s:\n", __func__);
 
-	ret = m88ts2022_wr_reg(priv, 0x00, 0x01);
+	dev_dbg(&dev->client->dev, "\n");
+
+	ret = regmap_write(dev->regmap, 0x00, 0x01);
 	if (ret)
 		goto err;
 
-	ret = m88ts2022_wr_reg(priv, 0x00, 0x03);
+	ret = regmap_write(dev->regmap, 0x00, 0x03);
 	if (ret)
 		goto err;
 
-	switch (priv->cfg.clock_out) {
+	switch (dev->cfg.clock_out) {
 	case M88TS2022_CLOCK_OUT_DISABLED:
 		u8tmp = 0x60;
 		break;
 	case M88TS2022_CLOCK_OUT_ENABLED:
 		u8tmp = 0x70;
-		ret = m88ts2022_wr_reg(priv, 0x05, priv->cfg.clock_out_div);
+		ret = regmap_write(dev->regmap, 0x05, dev->cfg.clock_out_div);
 		if (ret)
 			goto err;
 		break;
@@ -420,58 +312,61 @@ static int m88ts2022_init(struct dvb_frontend *fe)
 		goto err;
 	}
 
-	ret = m88ts2022_wr_reg(priv, 0x42, u8tmp);
+	ret = regmap_write(dev->regmap, 0x42, u8tmp);
 	if (ret)
 		goto err;
 
-	if (priv->cfg.loop_through)
+	if (dev->cfg.loop_through)
 		u8tmp = 0xec;
 	else
 		u8tmp = 0x6c;
 
-	ret = m88ts2022_wr_reg(priv, 0x62, u8tmp);
+	ret = regmap_write(dev->regmap, 0x62, u8tmp);
 	if (ret)
 		goto err;
 
 	for (i = 0; i < ARRAY_SIZE(reg_vals); i++) {
-		ret = m88ts2022_wr_reg(priv, reg_vals[i].reg, reg_vals[i].val);
+		ret = regmap_write(dev->regmap, reg_vals[i].reg, reg_vals[i].val);
 		if (ret)
 			goto err;
 	}
 err:
 	if (ret)
-		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ts2022_sleep(struct dvb_frontend *fe)
 {
-	struct m88ts2022_priv *priv = fe->tuner_priv;
+	struct m88ts2022_dev *dev = fe->tuner_priv;
 	int ret;
-	dev_dbg(&priv->client->dev, "%s:\n", __func__);
 
-	ret = m88ts2022_wr_reg(priv, 0x00, 0x00);
+	dev_dbg(&dev->client->dev, "\n");
+
+	ret = regmap_write(dev->regmap, 0x00, 0x00);
 	if (ret)
 		goto err;
 err:
 	if (ret)
-		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
 static int m88ts2022_get_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
-	struct m88ts2022_priv *priv = fe->tuner_priv;
-	dev_dbg(&priv->client->dev, "%s:\n", __func__);
+	struct m88ts2022_dev *dev = fe->tuner_priv;
 
-	*frequency = priv->frequency_khz;
+	dev_dbg(&dev->client->dev, "\n");
+
+	*frequency = dev->frequency_khz;
 	return 0;
 }
 
 static int m88ts2022_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
-	struct m88ts2022_priv *priv = fe->tuner_priv;
-	dev_dbg(&priv->client->dev, "%s:\n", __func__);
+	struct m88ts2022_dev *dev = fe->tuner_priv;
+
+	dev_dbg(&dev->client->dev, "\n");
 
 	*frequency = 0; /* Zero-IF */
 	return 0;
@@ -479,31 +374,30 @@ static int m88ts2022_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
 
 static int m88ts2022_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
 {
-	struct m88ts2022_priv *priv = fe->tuner_priv;
+	struct m88ts2022_dev *dev = fe->tuner_priv;
 	int ret;
-	u8 u8tmp;
 	u16 gain, u16tmp;
-	unsigned int gain1, gain2, gain3;
+	unsigned int utmp, gain1, gain2, gain3;
 
-	ret = m88ts2022_rd_reg(priv, 0x3d, &u8tmp);
+	ret = regmap_read(dev->regmap, 0x3d, &utmp);
 	if (ret)
 		goto err;
 
-	gain1 = (u8tmp >> 0) & 0x1f;
+	gain1 = (utmp >> 0) & 0x1f;
 	gain1 = clamp(gain1, 0U, 15U);
 
-	ret = m88ts2022_rd_reg(priv, 0x21, &u8tmp);
+	ret = regmap_read(dev->regmap, 0x21, &utmp);
 	if (ret)
 		goto err;
 
-	gain2 = (u8tmp >> 0) & 0x1f;
+	gain2 = (utmp >> 0) & 0x1f;
 	gain2 = clamp(gain2, 2U, 16U);
 
-	ret = m88ts2022_rd_reg(priv, 0x66, &u8tmp);
+	ret = regmap_read(dev->regmap, 0x66, &utmp);
 	if (ret)
 		goto err;
 
-	gain3 = (u8tmp >> 3) & 0x07;
+	gain3 = (utmp >> 3) & 0x07;
 	gain3 = clamp(gain3, 0U, 6U);
 
 	gain = gain1 * 265 + gain2 * 338 + gain3 * 285;
@@ -515,7 +409,7 @@ static int m88ts2022_get_rf_strength(struct dvb_frontend *fe, u16 *strength)
 	*strength = (u16tmp - 59000) * 0xffff / (61500 - 59000);
 err:
 	if (ret)
-		dev_dbg(&priv->client->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -540,46 +434,56 @@ static int m88ts2022_probe(struct i2c_client *client,
 {
 	struct m88ts2022_config *cfg = client->dev.platform_data;
 	struct dvb_frontend *fe = cfg->fe;
-	struct m88ts2022_priv *priv;
+	struct m88ts2022_dev *dev;
 	int ret;
-	u8 chip_id, u8tmp;
+	u8 u8tmp;
+	unsigned int utmp;
+	static const struct regmap_config regmap_config = {
+		.reg_bits = 8,
+		.val_bits = 8,
+	};
 
-	priv = kzalloc(sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (!dev) {
 		ret = -ENOMEM;
-		dev_err(&client->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		dev_err(&client->dev, "kzalloc() failed\n");
 		goto err;
 	}
 
-	memcpy(&priv->cfg, cfg, sizeof(struct m88ts2022_config));
-	priv->client = client;
+	memcpy(&dev->cfg, cfg, sizeof(struct m88ts2022_config));
+	dev->client = client;
+	dev->regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err;
+	}
 
 	/* check if the tuner is there */
-	ret = m88ts2022_rd_reg(priv, 0x00, &u8tmp);
+	ret = regmap_read(dev->regmap, 0x00, &utmp);
 	if (ret)
 		goto err;
 
-	if ((u8tmp & 0x03) == 0x00) {
-		ret = m88ts2022_wr_reg(priv, 0x00, 0x01);
-		if (ret < 0)
+	if ((utmp & 0x03) == 0x00) {
+		ret = regmap_write(dev->regmap, 0x00, 0x01);
+		if (ret)
 			goto err;
 
 		usleep_range(2000, 50000);
 	}
 
-	ret = m88ts2022_wr_reg(priv, 0x00, 0x03);
+	ret = regmap_write(dev->regmap, 0x00, 0x03);
 	if (ret)
 		goto err;
 
 	usleep_range(2000, 50000);
 
-	ret = m88ts2022_rd_reg(priv, 0x00, &chip_id);
+	ret = regmap_read(dev->regmap, 0x00, &utmp);
 	if (ret)
 		goto err;
 
-	dev_dbg(&priv->client->dev, "%s: chip_id=%02x\n", __func__, chip_id);
+	dev_dbg(&dev->client->dev, "chip_id=%02x\n", utmp);
 
-	switch (chip_id) {
+	switch (utmp) {
 	case 0xc3:
 	case 0x83:
 		break;
@@ -587,13 +491,13 @@ static int m88ts2022_probe(struct i2c_client *client,
 		goto err;
 	}
 
-	switch (priv->cfg.clock_out) {
+	switch (dev->cfg.clock_out) {
 	case M88TS2022_CLOCK_OUT_DISABLED:
 		u8tmp = 0x60;
 		break;
 	case M88TS2022_CLOCK_OUT_ENABLED:
 		u8tmp = 0x70;
-		ret = m88ts2022_wr_reg(priv, 0x05, priv->cfg.clock_out_div);
+		ret = regmap_write(dev->regmap, 0x05, dev->cfg.clock_out_div);
 		if (ret)
 			goto err;
 		break;
@@ -604,49 +508,48 @@ static int m88ts2022_probe(struct i2c_client *client,
 		goto err;
 	}
 
-	ret = m88ts2022_wr_reg(priv, 0x42, u8tmp);
+	ret = regmap_write(dev->regmap, 0x42, u8tmp);
 	if (ret)
 		goto err;
 
-	if (priv->cfg.loop_through)
+	if (dev->cfg.loop_through)
 		u8tmp = 0xec;
 	else
 		u8tmp = 0x6c;
 
-	ret = m88ts2022_wr_reg(priv, 0x62, u8tmp);
+	ret = regmap_write(dev->regmap, 0x62, u8tmp);
 	if (ret)
 		goto err;
 
 	/* sleep */
-	ret = m88ts2022_wr_reg(priv, 0x00, 0x00);
+	ret = regmap_write(dev->regmap, 0x00, 0x00);
 	if (ret)
 		goto err;
 
-	dev_info(&priv->client->dev,
-			"%s: Montage M88TS2022 successfully identified\n",
-			KBUILD_MODNAME);
+	dev_info(&dev->client->dev, "Montage M88TS2022 successfully identified\n");
 
-	fe->tuner_priv = priv;
+	fe->tuner_priv = dev;
 	memcpy(&fe->ops.tuner_ops, &m88ts2022_tuner_ops,
 			sizeof(struct dvb_tuner_ops));
 
-	i2c_set_clientdata(client, priv);
+	i2c_set_clientdata(client, dev);
 	return 0;
 err:
-	dev_dbg(&client->dev, "%s: failed=%d\n", __func__, ret);
-	kfree(priv);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	kfree(dev);
 	return ret;
 }
 
 static int m88ts2022_remove(struct i2c_client *client)
 {
-	struct m88ts2022_priv *priv = i2c_get_clientdata(client);
-	struct dvb_frontend *fe = priv->cfg.fe;
-	dev_dbg(&client->dev, "%s:\n", __func__);
+	struct m88ts2022_dev *dev = i2c_get_clientdata(client);
+	struct dvb_frontend *fe = dev->cfg.fe;
+
+	dev_dbg(&client->dev, "\n");
 
 	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
 	fe->tuner_priv = NULL;
-	kfree(priv);
+	kfree(dev);
 
 	return 0;
 }
diff --git a/drivers/media/tuners/m88ts2022_priv.h b/drivers/media/tuners/m88ts2022_priv.h
index 0363dd866a2d..feeb5ad6beef 100644
--- a/drivers/media/tuners/m88ts2022_priv.h
+++ b/drivers/media/tuners/m88ts2022_priv.h
@@ -18,11 +18,12 @@
 #define M88TS2022_PRIV_H
 
 #include "m88ts2022.h"
+#include <linux/regmap.h>
 
-struct m88ts2022_priv {
+struct m88ts2022_dev {
 	struct m88ts2022_config cfg;
 	struct i2c_client *client;
-	struct dvb_frontend *fe;
+	struct regmap *regmap;
 	u32 frequency_khz;
 };
 
diff --git a/drivers/media/tuners/msi001.c b/drivers/media/tuners/msi001.c
index ee99e372c943..26019e731993 100644
--- a/drivers/media/tuners/msi001.c
+++ b/drivers/media/tuners/msi001.c
@@ -67,7 +67,8 @@ static int msi001_set_gain(struct msi001 *s, int lna_gain, int mixer_gain,
 {
 	int ret;
 	u32 reg;
-	dev_dbg(&s->spi->dev, "%s: lna=%d mixer=%d if=%d\n", __func__,
+
+	dev_dbg(&s->spi->dev, "lna=%d mixer=%d if=%d\n",
 			lna_gain, mixer_gain, if_gain);
 
 	reg = 1 << 0;
@@ -83,7 +84,7 @@ static int msi001_set_gain(struct msi001 *s, int lna_gain, int mixer_gain,
 
 	return 0;
 err:
-	dev_dbg(&s->spi->dev, "%s: failed %d\n", __func__, ret);
+	dev_dbg(&s->spi->dev, "failed %d\n", ret);
 	return ret;
 };
 
@@ -94,6 +95,7 @@ static int msi001_set_tuner(struct msi001 *s)
 	u32 reg;
 	u64 f_vco, tmp64;
 	u8 mode, filter_mode, lo_div;
+
 	static const struct {
 		u32 rf;
 		u8 mode;
@@ -145,9 +147,7 @@ static int msi001_set_tuner(struct msi001 *s)
 	#define R_REF 4
 	#define F_OUT_STEP 1
 
-	dev_dbg(&s->spi->dev,
-			"%s: f_rf=%d f_if=%d\n",
-			__func__, f_rf, f_if);
+	dev_dbg(&s->spi->dev, "f_rf=%d f_if=%d\n", f_rf, f_if);
 
 	for (i = 0; i < ARRAY_SIZE(band_lut); i++) {
 		if (f_rf <= band_lut[i].rf) {
@@ -198,8 +198,7 @@ static int msi001_set_tuner(struct msi001 *s)
 
 	s->bandwidth->val = bandwidth_lut[i].freq;
 
-	dev_dbg(&s->spi->dev, "%s: bandwidth selected=%d\n",
-			__func__, bandwidth_lut[i].freq);
+	dev_dbg(&s->spi->dev, "bandwidth selected=%d\n", bandwidth_lut[i].freq);
 
 	f_vco = (u64) (f_rf + f_if + f_if1) * lo_div;
 	tmp64 = f_vco;
@@ -225,9 +224,8 @@ static int msi001_set_tuner(struct msi001 *s)
 	tmp += 1ul * F_REF * R_REF * frac / thresh;
 	tmp /= lo_div;
 
-	dev_dbg(&s->spi->dev,
-			"%s: rf=%u:%u n=%d thresh=%d frac=%d\n",
-				__func__, f_rf, tmp, n, thresh, frac);
+	dev_dbg(&s->spi->dev, "rf=%u:%u n=%d thresh=%d frac=%d\n",
+				f_rf, tmp, n, thresh, frac);
 
 	ret = msi001_wreg(s, 0x00000e);
 	if (ret)
@@ -276,7 +274,7 @@ static int msi001_set_tuner(struct msi001 *s)
 
 	return 0;
 err:
-	dev_dbg(&s->spi->dev, "%s: failed %d\n", __func__, ret);
+	dev_dbg(&s->spi->dev, "failed %d\n", ret);
 	return ret;
 };
 
@@ -284,7 +282,8 @@ static int msi001_s_power(struct v4l2_subdev *sd, int on)
 {
 	struct msi001 *s = sd_to_msi001(sd);
 	int ret;
-	dev_dbg(&s->spi->dev, "%s: on=%d\n", __func__, on);
+
+	dev_dbg(&s->spi->dev, "on=%d\n", on);
 
 	if (on)
 		ret = 0;
@@ -301,7 +300,8 @@ static const struct v4l2_subdev_core_ops msi001_core_ops = {
 static int msi001_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
 {
 	struct msi001 *s = sd_to_msi001(sd);
-	dev_dbg(&s->spi->dev, "%s: index=%d\n", __func__, v->index);
+
+	dev_dbg(&s->spi->dev, "index=%d\n", v->index);
 
 	strlcpy(v->name, "Mirics MSi001", sizeof(v->name));
 	v->type = V4L2_TUNER_RF;
@@ -315,14 +315,16 @@ static int msi001_g_tuner(struct v4l2_subdev *sd, struct v4l2_tuner *v)
 static int msi001_s_tuner(struct v4l2_subdev *sd, const struct v4l2_tuner *v)
 {
 	struct msi001 *s = sd_to_msi001(sd);
-	dev_dbg(&s->spi->dev, "%s: index=%d\n", __func__, v->index);
+
+	dev_dbg(&s->spi->dev, "index=%d\n", v->index);
 	return 0;
 }
 
 static int msi001_g_frequency(struct v4l2_subdev *sd, struct v4l2_frequency *f)
 {
 	struct msi001 *s = sd_to_msi001(sd);
-	dev_dbg(&s->spi->dev, "%s: tuner=%d\n", __func__, f->tuner);
+
+	dev_dbg(&s->spi->dev, "tuner=%d\n", f->tuner);
 	f->frequency = s->f_tuner;
 	return 0;
 }
@@ -332,8 +334,9 @@ static int msi001_s_frequency(struct v4l2_subdev *sd,
 {
 	struct msi001 *s = sd_to_msi001(sd);
 	unsigned int band;
-	dev_dbg(&s->spi->dev, "%s: tuner=%d type=%d frequency=%u\n",
-			__func__, f->tuner, f->type, f->frequency);
+
+	dev_dbg(&s->spi->dev, "tuner=%d type=%d frequency=%u\n",
+			f->tuner, f->type, f->frequency);
 
 	if (f->frequency < ((bands[0].rangehigh + bands[1].rangelow) / 2))
 		band = 0;
@@ -349,8 +352,9 @@ static int msi001_enum_freq_bands(struct v4l2_subdev *sd,
 		struct v4l2_frequency_band *band)
 {
 	struct msi001 *s = sd_to_msi001(sd);
-	dev_dbg(&s->spi->dev, "%s: tuner=%d type=%d index=%d\n",
-			__func__, band->tuner, band->type, band->index);
+
+	dev_dbg(&s->spi->dev, "tuner=%d type=%d index=%d\n",
+			band->tuner, band->type, band->index);
 
 	if (band->index >= ARRAY_SIZE(bands))
 		return -EINVAL;
@@ -380,9 +384,10 @@ static int msi001_s_ctrl(struct v4l2_ctrl *ctrl)
 	struct msi001 *s = container_of(ctrl->handler, struct msi001, hdl);
 
 	int ret;
+
 	dev_dbg(&s->spi->dev,
-			"%s: id=%d name=%s val=%d min=%lld max=%lld step=%lld\n",
-			__func__, ctrl->id, ctrl->name, ctrl->val,
+			"id=%d name=%s val=%d min=%lld max=%lld step=%lld\n",
+			ctrl->id, ctrl->name, ctrl->val,
 			ctrl->minimum, ctrl->maximum, ctrl->step);
 
 	switch (ctrl->id) {
@@ -403,8 +408,7 @@ static int msi001_s_ctrl(struct v4l2_ctrl *ctrl)
 				s->mixer_gain->cur.val, s->if_gain->val);
 		break;
 	default:
-		dev_dbg(&s->spi->dev, "%s: unkown control %d\n",
-				__func__, ctrl->id);
+		dev_dbg(&s->spi->dev, "unkown control %d\n", ctrl->id);
 		ret = -EINVAL;
 	}
 
@@ -419,7 +423,8 @@ static int msi001_probe(struct spi_device *spi)
 {
 	struct msi001 *s;
 	int ret;
-	dev_dbg(&spi->dev, "%s:\n", __func__);
+
+	dev_dbg(&spi->dev, "\n");
 
 	s = kzalloc(sizeof(struct msi001), GFP_KERNEL);
 	if (s == NULL) {
@@ -466,7 +471,8 @@ static int msi001_remove(struct spi_device *spi)
 {
 	struct v4l2_subdev *sd = spi_get_drvdata(spi);
 	struct msi001 *s = sd_to_msi001(sd);
-	dev_dbg(&spi->dev, "%s:\n", __func__);
+
+	dev_dbg(&spi->dev, "\n");
 
 	/*
 	 * Registered by v4l2_spi_new_subdev() from master driver, but we must
diff --git a/drivers/media/tuners/mt2060.c b/drivers/media/tuners/mt2060.c
index 13381de58a84..b87b2549d58d 100644
--- a/drivers/media/tuners/mt2060.c
+++ b/drivers/media/tuners/mt2060.c
@@ -157,7 +157,6 @@ static int mt2060_set_params(struct dvb_frontend *fe)
 {
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	struct mt2060_priv *priv;
-	int ret=0;
 	int i=0;
 	u32 freq;
 	u8  lnaband;
@@ -240,7 +239,7 @@ static int mt2060_set_params(struct dvb_frontend *fe)
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 0); /* close i2c_gate */
 
-	return ret;
+	return 0;
 }
 
 static void mt2060_calibrate(struct mt2060_priv *priv)
diff --git a/drivers/media/tuners/mt2063.c b/drivers/media/tuners/mt2063.c
index f640dcf4a81d..9e9c5eb4cb66 100644
--- a/drivers/media/tuners/mt2063.c
+++ b/drivers/media/tuners/mt2063.c
@@ -1216,7 +1216,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 	if (status >= 0) {
 		val =
 		    (state->
-		     reg[MT2063_REG_PD1_TGT] & (u8) ~0x40) | (RFAGCEN[Mode]
+		     reg[MT2063_REG_PD1_TGT] & ~0x40) | (RFAGCEN[Mode]
 								   ? 0x40 :
 								   0x00);
 		if (state->reg[MT2063_REG_PD1_TGT] != val)
@@ -1225,7 +1225,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* LNARin */
 	if (status >= 0) {
-		u8 val = (state->reg[MT2063_REG_CTRL_2C] & (u8) ~0x03) |
+		u8 val = (state->reg[MT2063_REG_CTRL_2C] & ~0x03) |
 			 (LNARIN[Mode] & 0x03);
 		if (state->reg[MT2063_REG_CTRL_2C] != val)
 			status |= mt2063_setreg(state, MT2063_REG_CTRL_2C, val);
@@ -1235,19 +1235,19 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 	if (status >= 0) {
 		val =
 		    (state->
-		     reg[MT2063_REG_FIFF_CTRL2] & (u8) ~0xF0) |
+		     reg[MT2063_REG_FIFF_CTRL2] & ~0xF0) |
 		    (FIFFQEN[Mode] << 7) | (FIFFQ[Mode] << 4);
 		if (state->reg[MT2063_REG_FIFF_CTRL2] != val) {
 			status |=
 			    mt2063_setreg(state, MT2063_REG_FIFF_CTRL2, val);
 			/* trigger FIFF calibration, needed after changing FIFFQ */
 			val =
-			    (state->reg[MT2063_REG_FIFF_CTRL] | (u8) 0x01);
+			    (state->reg[MT2063_REG_FIFF_CTRL] | 0x01);
 			status |=
 			    mt2063_setreg(state, MT2063_REG_FIFF_CTRL, val);
 			val =
 			    (state->
-			     reg[MT2063_REG_FIFF_CTRL] & (u8) ~0x01);
+			     reg[MT2063_REG_FIFF_CTRL] & ~0x01);
 			status |=
 			    mt2063_setreg(state, MT2063_REG_FIFF_CTRL, val);
 		}
@@ -1259,7 +1259,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* acLNAmax */
 	if (status >= 0) {
-		u8 val = (state->reg[MT2063_REG_LNA_OV] & (u8) ~0x1F) |
+		u8 val = (state->reg[MT2063_REG_LNA_OV] & ~0x1F) |
 			 (ACLNAMAX[Mode] & 0x1F);
 		if (state->reg[MT2063_REG_LNA_OV] != val)
 			status |= mt2063_setreg(state, MT2063_REG_LNA_OV, val);
@@ -1267,7 +1267,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* LNATGT */
 	if (status >= 0) {
-		u8 val = (state->reg[MT2063_REG_LNA_TGT] & (u8) ~0x3F) |
+		u8 val = (state->reg[MT2063_REG_LNA_TGT] & ~0x3F) |
 			 (LNATGT[Mode] & 0x3F);
 		if (state->reg[MT2063_REG_LNA_TGT] != val)
 			status |= mt2063_setreg(state, MT2063_REG_LNA_TGT, val);
@@ -1275,7 +1275,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* ACRF */
 	if (status >= 0) {
-		u8 val = (state->reg[MT2063_REG_RF_OV] & (u8) ~0x1F) |
+		u8 val = (state->reg[MT2063_REG_RF_OV] & ~0x1F) |
 			 (ACRFMAX[Mode] & 0x1F);
 		if (state->reg[MT2063_REG_RF_OV] != val)
 			status |= mt2063_setreg(state, MT2063_REG_RF_OV, val);
@@ -1283,7 +1283,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* PD1TGT */
 	if (status >= 0) {
-		u8 val = (state->reg[MT2063_REG_PD1_TGT] & (u8) ~0x3F) |
+		u8 val = (state->reg[MT2063_REG_PD1_TGT] & ~0x3F) |
 			 (PD1TGT[Mode] & 0x3F);
 		if (state->reg[MT2063_REG_PD1_TGT] != val)
 			status |= mt2063_setreg(state, MT2063_REG_PD1_TGT, val);
@@ -1294,7 +1294,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 		u8 val = ACFIFMAX[Mode];
 		if (state->reg[MT2063_REG_PART_REV] != MT2063_B3 && val > 5)
 			val = 5;
-		val = (state->reg[MT2063_REG_FIF_OV] & (u8) ~0x1F) |
+		val = (state->reg[MT2063_REG_FIF_OV] & ~0x1F) |
 		      (val & 0x1F);
 		if (state->reg[MT2063_REG_FIF_OV] != val)
 			status |= mt2063_setreg(state, MT2063_REG_FIF_OV, val);
@@ -1302,7 +1302,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* PD2TGT */
 	if (status >= 0) {
-		u8 val = (state->reg[MT2063_REG_PD2_TGT] & (u8) ~0x3F) |
+		u8 val = (state->reg[MT2063_REG_PD2_TGT] & ~0x3F) |
 		    (PD2TGT[Mode] & 0x3F);
 		if (state->reg[MT2063_REG_PD2_TGT] != val)
 			status |= mt2063_setreg(state, MT2063_REG_PD2_TGT, val);
@@ -1310,7 +1310,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* Ignore ATN Overload */
 	if (status >= 0) {
-		val = (state->reg[MT2063_REG_LNA_TGT] & (u8) ~0x80) |
+		val = (state->reg[MT2063_REG_LNA_TGT] & ~0x80) |
 		      (RFOVDIS[Mode] ? 0x80 : 0x00);
 		if (state->reg[MT2063_REG_LNA_TGT] != val)
 			status |= mt2063_setreg(state, MT2063_REG_LNA_TGT, val);
@@ -1318,7 +1318,7 @@ static u32 MT2063_SetReceiverMode(struct mt2063_state *state,
 
 	/* Ignore FIF Overload */
 	if (status >= 0) {
-		val = (state->reg[MT2063_REG_PD1_TGT] & (u8) ~0x80) |
+		val = (state->reg[MT2063_REG_PD1_TGT] & ~0x80) |
 		      (FIFOVDIS[Mode] ? 0x80 : 0x00);
 		if (state->reg[MT2063_REG_PD1_TGT] != val)
 			status |= mt2063_setreg(state, MT2063_REG_PD1_TGT, val);
diff --git a/drivers/media/tuners/mxl301rf.c b/drivers/media/tuners/mxl301rf.c
new file mode 100644
index 000000000000..1575a5db776a
--- /dev/null
+++ b/drivers/media/tuners/mxl301rf.c
@@ -0,0 +1,349 @@
+/*
+ * MaxLinear MxL301RF OFDM tuner driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * NOTICE:
+ * This driver is incomplete and lacks init/config of the chips,
+ * as the necessary info is not disclosed.
+ * Other features like get_if_frequency() are missing as well.
+ * It assumes that users of this driver (such as a PCI bridge of
+ * DTV receiver cards) properly init and configure the chip
+ * via I2C *before* calling this driver's init() function.
+ *
+ * Currently, PT3 driver is the only one that uses this driver,
+ * and contains init/config code in its firmware.
+ * Thus some part of the code might be dependent on PT3 specific config.
+ */
+
+#include <linux/kernel.h>
+#include "mxl301rf.h"
+
+struct mxl301rf_state {
+	struct mxl301rf_config cfg;
+	struct i2c_client *i2c;
+};
+
+static struct mxl301rf_state *cfg_to_state(struct mxl301rf_config *c)
+{
+	return container_of(c, struct mxl301rf_state, cfg);
+}
+
+static int raw_write(struct mxl301rf_state *state, const u8 *buf, int len)
+{
+	int ret;
+
+	ret = i2c_master_send(state->i2c, buf, len);
+	if (ret >= 0 && ret < len)
+		ret = -EIO;
+	return (ret == len) ? 0 : ret;
+}
+
+static int reg_write(struct mxl301rf_state *state, u8 reg, u8 val)
+{
+	u8 buf[2] = { reg, val };
+
+	return raw_write(state, buf, 2);
+}
+
+static int reg_read(struct mxl301rf_state *state, u8 reg, u8 *val)
+{
+	u8 wbuf[2] = { 0xfb, reg };
+	int ret;
+
+	ret = raw_write(state, wbuf, sizeof(wbuf));
+	if (ret == 0)
+		ret = i2c_master_recv(state->i2c, val, 1);
+	if (ret >= 0 && ret < 1)
+		ret = -EIO;
+	return (ret == 1) ? 0 : ret;
+}
+
+/* tuner_ops */
+
+/* get RSSI and update propery cache, set to *out in % */
+static int mxl301rf_get_rf_strength(struct dvb_frontend *fe, u16 *out)
+{
+	struct mxl301rf_state *state;
+	int ret;
+	u8  rf_in1, rf_in2, rf_off1, rf_off2;
+	u16 rf_in, rf_off;
+	s64 level;
+	struct dtv_fe_stats *rssi;
+
+	rssi = &fe->dtv_property_cache.strength;
+	rssi->len = 1;
+	rssi->stat[0].scale = FE_SCALE_NOT_AVAILABLE;
+	*out = 0;
+
+	state = fe->tuner_priv;
+	ret = reg_write(state, 0x14, 0x01);
+	if (ret < 0)
+		return ret;
+	usleep_range(1000, 2000);
+
+	ret = reg_read(state, 0x18, &rf_in1);
+	if (ret == 0)
+		ret = reg_read(state, 0x19, &rf_in2);
+	if (ret == 0)
+		ret = reg_read(state, 0xd6, &rf_off1);
+	if (ret == 0)
+		ret = reg_read(state, 0xd7, &rf_off2);
+	if (ret != 0)
+		return ret;
+
+	rf_in = (rf_in2 & 0x07) << 8 | rf_in1;
+	rf_off = (rf_off2 & 0x0f) << 5 | (rf_off1 >> 3);
+	level = rf_in - rf_off - (113 << 3); /* x8 dBm */
+	level = level * 1000 / 8;
+	rssi->stat[0].svalue = level;
+	rssi->stat[0].scale = FE_SCALE_DECIBEL;
+	/* *out = (level - min) * 100 / (max - min) */
+	*out = (rf_in - rf_off + (1 << 9) - 1) * 100 / ((5 << 9) - 2);
+	return 0;
+}
+
+/* spur shift parameters */
+struct shf {
+	u32	freq;		/* Channel center frequency */
+	u32	ofst_th;	/* Offset frequency threshold */
+	u8	shf_val;	/* Spur shift value */
+	u8	shf_dir;	/* Spur shift direction */
+};
+
+static const struct shf shf_tab[] = {
+	{  64500, 500, 0x92, 0x07 },
+	{ 191500, 300, 0xe2, 0x07 },
+	{ 205500, 500, 0x2c, 0x04 },
+	{ 212500, 500, 0x1e, 0x04 },
+	{ 226500, 500, 0xd4, 0x07 },
+	{  99143, 500, 0x9c, 0x07 },
+	{ 173143, 500, 0xd4, 0x07 },
+	{ 191143, 300, 0xd4, 0x07 },
+	{ 207143, 500, 0xce, 0x07 },
+	{ 225143, 500, 0xce, 0x07 },
+	{ 243143, 500, 0xd4, 0x07 },
+	{ 261143, 500, 0xd4, 0x07 },
+	{ 291143, 500, 0xd4, 0x07 },
+	{ 339143, 500, 0x2c, 0x04 },
+	{ 117143, 500, 0x7a, 0x07 },
+	{ 135143, 300, 0x7a, 0x07 },
+	{ 153143, 500, 0x01, 0x07 }
+};
+
+struct reg_val {
+	u8 reg;
+	u8 val;
+} __attribute__ ((__packed__));
+
+static const struct reg_val set_idac[] = {
+	{ 0x0d, 0x00 },
+	{ 0x0c, 0x67 },
+	{ 0x6f, 0x89 },
+	{ 0x70, 0x0c },
+	{ 0x6f, 0x8a },
+	{ 0x70, 0x0e },
+	{ 0x6f, 0x8b },
+	{ 0x70, 0x1c },
+};
+
+static int mxl301rf_set_params(struct dvb_frontend *fe)
+{
+	struct reg_val tune0[] = {
+		{ 0x13, 0x00 },		/* abort tuning */
+		{ 0x3b, 0xc0 },
+		{ 0x3b, 0x80 },
+		{ 0x10, 0x95 },		/* BW */
+		{ 0x1a, 0x05 },
+		{ 0x61, 0x00 },		/* spur shift value (placeholder) */
+		{ 0x62, 0xa0 }		/* spur shift direction (placeholder) */
+	};
+
+	struct reg_val tune1[] = {
+		{ 0x11, 0x40 },		/* RF frequency L (placeholder) */
+		{ 0x12, 0x0e },		/* RF frequency H (placeholder) */
+		{ 0x13, 0x01 }		/* start tune */
+	};
+
+	struct mxl301rf_state *state;
+	u32 freq;
+	u16 f;
+	u32 tmp, div;
+	int i, ret;
+
+	state = fe->tuner_priv;
+	freq = fe->dtv_property_cache.frequency;
+
+	/* spur shift function (for analog) */
+	for (i = 0; i < ARRAY_SIZE(shf_tab); i++) {
+		if (freq >= (shf_tab[i].freq - shf_tab[i].ofst_th) * 1000 &&
+		    freq <= (shf_tab[i].freq + shf_tab[i].ofst_th) * 1000) {
+			tune0[5].val = shf_tab[i].shf_val;
+			tune0[6].val = 0xa0 | shf_tab[i].shf_dir;
+			break;
+		}
+	}
+	ret = raw_write(state, (u8 *) tune0, sizeof(tune0));
+	if (ret < 0)
+		goto failed;
+	usleep_range(3000, 4000);
+
+	/* convert freq to 10.6 fixed point float [MHz] */
+	f = freq / 1000000;
+	tmp = freq % 1000000;
+	div = 1000000;
+	for (i = 0; i < 6; i++) {
+		f <<= 1;
+		div >>= 1;
+		if (tmp > div) {
+			tmp -= div;
+			f |= 1;
+		}
+	}
+	if (tmp > 7812)
+		f++;
+	tune1[0].val = f & 0xff;
+	tune1[1].val = f >> 8;
+	ret = raw_write(state, (u8 *) tune1, sizeof(tune1));
+	if (ret < 0)
+		goto failed;
+	msleep(31);
+
+	ret = reg_write(state, 0x1a, 0x0d);
+	if (ret < 0)
+		goto failed;
+	ret = raw_write(state, (u8 *) set_idac, sizeof(set_idac));
+	if (ret < 0)
+		goto failed;
+	return 0;
+
+failed:
+	dev_warn(&state->i2c->dev, "(%s) failed. [adap%d-fe%d]\n",
+		__func__, fe->dvb->num, fe->id);
+	return ret;
+}
+
+static const struct reg_val standby_data[] = {
+	{ 0x01, 0x00 },
+	{ 0x13, 0x00 }
+};
+
+static int mxl301rf_sleep(struct dvb_frontend *fe)
+{
+	struct mxl301rf_state *state;
+	int ret;
+
+	state = fe->tuner_priv;
+	ret = raw_write(state, (u8 *)standby_data, sizeof(standby_data));
+	if (ret < 0)
+		dev_warn(&state->i2c->dev, "(%s) failed. [adap%d-fe%d]\n",
+			__func__, fe->dvb->num, fe->id);
+	return ret;
+}
+
+
+/* init sequence is not public.
+ * the parent must have init'ed the device.
+ * just wake up here.
+ */
+static int mxl301rf_init(struct dvb_frontend *fe)
+{
+	struct mxl301rf_state *state;
+	int ret;
+
+	state = fe->tuner_priv;
+
+	ret = reg_write(state, 0x01, 0x01);
+	if (ret < 0) {
+		dev_warn(&state->i2c->dev, "(%s) failed. [adap%d-fe%d]\n",
+			 __func__, fe->dvb->num, fe->id);
+		return ret;
+	}
+	return 0;
+}
+
+/* I2C driver functions */
+
+static const struct dvb_tuner_ops mxl301rf_ops = {
+	.info = {
+		.name = "MaxLinear MxL301RF",
+
+		.frequency_min =  93000000,
+		.frequency_max = 803142857,
+	},
+
+	.init = mxl301rf_init,
+	.sleep = mxl301rf_sleep,
+
+	.set_params = mxl301rf_set_params,
+	.get_rf_strength = mxl301rf_get_rf_strength,
+};
+
+
+static int mxl301rf_probe(struct i2c_client *client,
+			  const struct i2c_device_id *id)
+{
+	struct mxl301rf_state *state;
+	struct mxl301rf_config *cfg;
+	struct dvb_frontend *fe;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+
+	state->i2c = client;
+	cfg = client->dev.platform_data;
+
+	memcpy(&state->cfg, cfg, sizeof(state->cfg));
+	fe = cfg->fe;
+	fe->tuner_priv = state;
+	memcpy(&fe->ops.tuner_ops, &mxl301rf_ops, sizeof(mxl301rf_ops));
+
+	i2c_set_clientdata(client, &state->cfg);
+	dev_info(&client->dev, "MaxLinear MxL301RF attached.\n");
+	return 0;
+}
+
+static int mxl301rf_remove(struct i2c_client *client)
+{
+	struct mxl301rf_state *state;
+
+	state = cfg_to_state(i2c_get_clientdata(client));
+	state->cfg.fe->tuner_priv = NULL;
+	kfree(state);
+	return 0;
+}
+
+
+static const struct i2c_device_id mxl301rf_id[] = {
+	{"mxl301rf", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, mxl301rf_id);
+
+static struct i2c_driver mxl301rf_driver = {
+	.driver = {
+		.name	= "mxl301rf",
+	},
+	.probe		= mxl301rf_probe,
+	.remove		= mxl301rf_remove,
+	.id_table	= mxl301rf_id,
+};
+
+module_i2c_driver(mxl301rf_driver);
+
+MODULE_DESCRIPTION("MaxLinear MXL301RF tuner");
+MODULE_AUTHOR("Akihiro TSUKADA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/mxl301rf.h b/drivers/media/tuners/mxl301rf.h
new file mode 100644
index 000000000000..19e68405f00d
--- /dev/null
+++ b/drivers/media/tuners/mxl301rf.h
@@ -0,0 +1,26 @@
+/*
+ * MaxLinear MxL301RF OFDM tuner driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef MXL301RF_H
+#define MXL301RF_H
+
+#include "dvb_frontend.h"
+
+struct mxl301rf_config {
+	struct dvb_frontend *fe;
+};
+
+#endif /* MXL301RF_H */
diff --git a/drivers/media/tuners/mxl5005s.c b/drivers/media/tuners/mxl5005s.c
index b473b76cb278..92a3be4fde87 100644
--- a/drivers/media/tuners/mxl5005s.c
+++ b/drivers/media/tuners/mxl5005s.c
@@ -1692,7 +1692,6 @@ static u16 MXL5005_TunerConfig(struct dvb_frontend *fe,
 	)
 {
 	struct mxl5005s_state *state = fe->tuner_priv;
-	u16 status = 0;
 
 	state->Mode = Mode;
 	state->IF_Mode = IF_mode;
@@ -1715,7 +1714,7 @@ static u16 MXL5005_TunerConfig(struct dvb_frontend *fe,
 	/* Synthesizer LO frequency calculation */
 	MXL_SynthIFLO_Calc(fe);
 
-	return status;
+	return 0;
 }
 
 static void MXL_SynthIFLO_Calc(struct dvb_frontend *fe)
diff --git a/drivers/media/tuners/qm1d1c0042.c b/drivers/media/tuners/qm1d1c0042.c
new file mode 100644
index 000000000000..18bc745ed108
--- /dev/null
+++ b/drivers/media/tuners/qm1d1c0042.c
@@ -0,0 +1,448 @@
+/*
+ * Sharp QM1D1C0042 8PSK tuner driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * NOTICE:
+ * As the disclosed information on the chip is very limited,
+ * this driver lacks some features, including chip config like IF freq.
+ * It assumes that users of this driver (such as a PCI bridge of
+ * DTV receiver cards) know the relevant info and
+ * configure the chip via I2C if necessary.
+ *
+ * Currently, PT3 driver is the only one that uses this driver,
+ * and contains init/config code in its firmware.
+ * Thus some part of the code might be dependent on PT3 specific config.
+ */
+
+#include <linux/kernel.h>
+#include <linux/math64.h>
+#include "qm1d1c0042.h"
+
+#define QM1D1C0042_NUM_REGS 0x20
+
+static const u8 reg_initval[QM1D1C0042_NUM_REGS] = {
+	0x48, 0x1c, 0xa0, 0x10, 0xbc, 0xc5, 0x20, 0x33,
+	0x06, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00,
+	0x00, 0xff, 0xf3, 0x00, 0x2a, 0x64, 0xa6, 0x86,
+	0x8c, 0xcf, 0xb8, 0xf1, 0xa8, 0xf2, 0x89, 0x00
+};
+
+static const struct qm1d1c0042_config default_cfg = {
+	.xtal_freq = 16000,
+	.lpf = 1,
+	.fast_srch = 0,
+	.lpf_wait = 20,
+	.fast_srch_wait = 4,
+	.normal_srch_wait = 15,
+};
+
+struct qm1d1c0042_state {
+	struct qm1d1c0042_config cfg;
+	struct i2c_client *i2c;
+	u8 regs[QM1D1C0042_NUM_REGS];
+};
+
+static struct qm1d1c0042_state *cfg_to_state(struct qm1d1c0042_config *c)
+{
+	return container_of(c, struct qm1d1c0042_state, cfg);
+}
+
+static int reg_write(struct qm1d1c0042_state *state, u8 reg, u8 val)
+{
+	u8 wbuf[2] = { reg, val };
+	int ret;
+
+	ret = i2c_master_send(state->i2c, wbuf, sizeof(wbuf));
+	if (ret >= 0 && ret < sizeof(wbuf))
+		ret = -EIO;
+	return (ret == sizeof(wbuf)) ? 0 : ret;
+}
+
+static int reg_read(struct qm1d1c0042_state *state, u8 reg, u8 *val)
+{
+	struct i2c_msg msgs[2] = {
+		{
+			.addr = state->i2c->addr,
+			.flags = 0,
+			.buf = &reg,
+			.len = 1,
+		},
+		{
+			.addr = state->i2c->addr,
+			.flags = I2C_M_RD,
+			.buf = val,
+			.len = 1,
+		},
+	};
+	int ret;
+
+	ret = i2c_transfer(state->i2c->adapter, msgs, ARRAY_SIZE(msgs));
+	if (ret >= 0 && ret < ARRAY_SIZE(msgs))
+		ret = -EIO;
+	return (ret == ARRAY_SIZE(msgs)) ? 0 : ret;
+}
+
+
+static int qm1d1c0042_set_srch_mode(struct qm1d1c0042_state *state, bool fast)
+{
+	if (fast)
+		state->regs[0x03] |= 0x01; /* set fast search mode */
+	else
+		state->regs[0x03] &= ~0x01 & 0xff;
+
+	return reg_write(state, 0x03, state->regs[0x03]);
+}
+
+static int qm1d1c0042_wakeup(struct qm1d1c0042_state *state)
+{
+	int ret;
+
+	state->regs[0x01] |= 1 << 3;             /* BB_Reg_enable */
+	state->regs[0x01] &= (~(1 << 0)) & 0xff; /* NORMAL (wake-up) */
+	state->regs[0x05] &= (~(1 << 3)) & 0xff; /* pfd_rst NORMAL */
+	ret = reg_write(state, 0x01, state->regs[0x01]);
+	if (ret == 0)
+		ret = reg_write(state, 0x05, state->regs[0x05]);
+
+	if (ret < 0)
+		dev_warn(&state->i2c->dev, "(%s) failed. [adap%d-fe%d]\n",
+			__func__, state->cfg.fe->dvb->num, state->cfg.fe->id);
+	return ret;
+}
+
+/* tuner_ops */
+
+static int qm1d1c0042_set_config(struct dvb_frontend *fe, void *priv_cfg)
+{
+	struct qm1d1c0042_state *state;
+	struct qm1d1c0042_config *cfg;
+
+	state = fe->tuner_priv;
+	cfg = priv_cfg;
+
+	if (cfg->fe)
+		state->cfg.fe = cfg->fe;
+
+	if (cfg->xtal_freq != QM1D1C0042_CFG_XTAL_DFLT)
+		dev_warn(&state->i2c->dev,
+			"(%s) changing xtal_freq not supported. ", __func__);
+	state->cfg.xtal_freq = default_cfg.xtal_freq;
+
+	state->cfg.lpf = cfg->lpf;
+	state->cfg.fast_srch = cfg->fast_srch;
+
+	if (cfg->lpf_wait != QM1D1C0042_CFG_WAIT_DFLT)
+		state->cfg.lpf_wait = cfg->lpf_wait;
+	else
+		state->cfg.lpf_wait = default_cfg.lpf_wait;
+
+	if (cfg->fast_srch_wait != QM1D1C0042_CFG_WAIT_DFLT)
+		state->cfg.fast_srch_wait = cfg->fast_srch_wait;
+	else
+		state->cfg.fast_srch_wait = default_cfg.fast_srch_wait;
+
+	if (cfg->normal_srch_wait != QM1D1C0042_CFG_WAIT_DFLT)
+		state->cfg.normal_srch_wait = cfg->normal_srch_wait;
+	else
+		state->cfg.normal_srch_wait = default_cfg.normal_srch_wait;
+	return 0;
+}
+
+/* divisor, vco_band parameters */
+/*  {maxfreq,  param1(band?), param2(div?) */
+static const u32 conv_table[9][3] = {
+	{ 2151000, 1, 7 },
+	{ 1950000, 1, 6 },
+	{ 1800000, 1, 5 },
+	{ 1600000, 1, 4 },
+	{ 1450000, 1, 3 },
+	{ 1250000, 1, 2 },
+	{ 1200000, 0, 7 },
+	{  975000, 0, 6 },
+	{  950000, 0, 0 }
+};
+
+static int qm1d1c0042_set_params(struct dvb_frontend *fe)
+{
+	struct qm1d1c0042_state *state;
+	u32 freq;
+	int i, ret;
+	u8 val, mask;
+	u32 a, sd;
+	s32 b;
+
+	state = fe->tuner_priv;
+	freq = fe->dtv_property_cache.frequency;
+
+	state->regs[0x08] &= 0xf0;
+	state->regs[0x08] |= 0x09;
+
+	state->regs[0x13] &= 0x9f;
+	state->regs[0x13] |= 0x20;
+
+	/* div2/vco_band */
+	val = state->regs[0x02] & 0x0f;
+	for (i = 0; i < 8; i++)
+		if (freq < conv_table[i][0] && freq >= conv_table[i + 1][0]) {
+			val |= conv_table[i][1] << 7;
+			val |= conv_table[i][2] << 4;
+			break;
+		}
+	ret = reg_write(state, 0x02, val);
+	if (ret < 0)
+		return ret;
+
+	a = (freq + state->cfg.xtal_freq / 2) / state->cfg.xtal_freq;
+
+	state->regs[0x06] &= 0x40;
+	state->regs[0x06] |= (a - 12) / 4;
+	ret = reg_write(state, 0x06, state->regs[0x06]);
+	if (ret < 0)
+		return ret;
+
+	state->regs[0x07] &= 0xf0;
+	state->regs[0x07] |= (a - 4 * ((a - 12) / 4 + 1) - 5) & 0x0f;
+	ret = reg_write(state, 0x07, state->regs[0x07]);
+	if (ret < 0)
+		return ret;
+
+	/* LPF */
+	val = state->regs[0x08];
+	if (state->cfg.lpf) {
+		/* LPF_CLK, LPF_FC */
+		val &= 0xf0;
+		val |= 0x02;
+	}
+	ret = reg_write(state, 0x08, val);
+	if (ret < 0)
+		return ret;
+
+	/*
+	 * b = (freq / state->cfg.xtal_freq - a) << 20;
+	 * sd = b          (b >= 0)
+	 *      1<<22 + b  (b < 0)
+	 */
+	b = (s32)div64_s64(((s64) freq) << 20, state->cfg.xtal_freq)
+			   - (((s64) a) << 20);
+
+	if (b >= 0)
+		sd = b;
+	else
+		sd = (1 << 22) + b;
+
+	state->regs[0x09] &= 0xc0;
+	state->regs[0x09] |= (sd >> 16) & 0x3f;
+	state->regs[0x0a] = (sd >> 8) & 0xff;
+	state->regs[0x0b] = sd & 0xff;
+	ret = reg_write(state, 0x09, state->regs[0x09]);
+	if (ret == 0)
+		ret = reg_write(state, 0x0a, state->regs[0x0a]);
+	if (ret == 0)
+		ret = reg_write(state, 0x0b, state->regs[0x0b]);
+	if (ret != 0)
+		return ret;
+
+	if (!state->cfg.lpf) {
+		/* CSEL_Offset */
+		ret = reg_write(state, 0x13, state->regs[0x13]);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* VCO_TM, LPF_TM */
+	mask = state->cfg.lpf ? 0x3f : 0x7f;
+	val = state->regs[0x0c] & mask;
+	ret = reg_write(state, 0x0c, val);
+	if (ret < 0)
+		return ret;
+	usleep_range(2000, 3000);
+	val = state->regs[0x0c] | ~mask;
+	ret = reg_write(state, 0x0c, val);
+	if (ret < 0)
+		return ret;
+
+	if (state->cfg.lpf)
+		msleep(state->cfg.lpf_wait);
+	else if (state->regs[0x03] & 0x01)
+		msleep(state->cfg.fast_srch_wait);
+	else
+		msleep(state->cfg.normal_srch_wait);
+
+	if (state->cfg.lpf) {
+		/* LPF_FC */
+		ret = reg_write(state, 0x08, 0x09);
+		if (ret < 0)
+			return ret;
+
+		/* CSEL_Offset */
+		ret = reg_write(state, 0x13, state->regs[0x13]);
+		if (ret < 0)
+			return ret;
+	}
+	return 0;
+}
+
+static int qm1d1c0042_sleep(struct dvb_frontend *fe)
+{
+	struct qm1d1c0042_state *state;
+	int ret;
+
+	state = fe->tuner_priv;
+	state->regs[0x01] &= (~(1 << 3)) & 0xff; /* BB_Reg_disable */
+	state->regs[0x01] |= 1 << 0;             /* STDBY */
+	state->regs[0x05] |= 1 << 3;             /* pfd_rst STANDBY */
+	ret = reg_write(state, 0x05, state->regs[0x05]);
+	if (ret == 0)
+		ret = reg_write(state, 0x01, state->regs[0x01]);
+	if (ret < 0)
+		dev_warn(&state->i2c->dev, "(%s) failed. [adap%d-fe%d]\n",
+			__func__, fe->dvb->num, fe->id);
+	return ret;
+}
+
+static int qm1d1c0042_init(struct dvb_frontend *fe)
+{
+	struct qm1d1c0042_state *state;
+	u8 val;
+	int i, ret;
+
+	state = fe->tuner_priv;
+	memcpy(state->regs, reg_initval, sizeof(reg_initval));
+
+	reg_write(state, 0x01, 0x0c);
+	reg_write(state, 0x01, 0x0c);
+
+	ret = reg_write(state, 0x01, 0x0c); /* soft reset on */
+	if (ret < 0)
+		goto failed;
+	usleep_range(2000, 3000);
+
+	val = state->regs[0x01] | 0x10;
+	ret = reg_write(state, 0x01, val); /* soft reset off */
+	if (ret < 0)
+		goto failed;
+
+	/* check ID */
+	ret = reg_read(state, 0x00, &val);
+	if (ret < 0 || val != 0x48)
+		goto failed;
+	usleep_range(2000, 3000);
+
+	state->regs[0x0c] |= 0x40;
+	ret = reg_write(state, 0x0c, state->regs[0x0c]);
+	if (ret < 0)
+		goto failed;
+	msleep(state->cfg.lpf_wait);
+
+	/* set all writable registers */
+	for (i = 1; i <= 0x0c ; i++) {
+		ret = reg_write(state, i, state->regs[i]);
+		if (ret < 0)
+			goto failed;
+	}
+	for (i = 0x11; i < QM1D1C0042_NUM_REGS; i++) {
+		ret = reg_write(state, i, state->regs[i]);
+		if (ret < 0)
+			goto failed;
+	}
+
+	ret = qm1d1c0042_wakeup(state);
+	if (ret < 0)
+		goto failed;
+
+	ret = qm1d1c0042_set_srch_mode(state, state->cfg.fast_srch);
+	if (ret < 0)
+		goto failed;
+
+	return ret;
+
+failed:
+	dev_warn(&state->i2c->dev, "(%s) failed. [adap%d-fe%d]\n",
+		__func__, fe->dvb->num, fe->id);
+	return ret;
+}
+
+/* I2C driver functions */
+
+static const struct dvb_tuner_ops qm1d1c0042_ops = {
+	.info = {
+		.name = "Sharp QM1D1C0042",
+
+		.frequency_min =  950000,
+		.frequency_max = 2150000,
+	},
+
+	.init = qm1d1c0042_init,
+	.sleep = qm1d1c0042_sleep,
+	.set_config = qm1d1c0042_set_config,
+	.set_params = qm1d1c0042_set_params,
+};
+
+
+static int qm1d1c0042_probe(struct i2c_client *client,
+			    const struct i2c_device_id *id)
+{
+	struct qm1d1c0042_state *state;
+	struct qm1d1c0042_config *cfg;
+	struct dvb_frontend *fe;
+
+	state = kzalloc(sizeof(*state), GFP_KERNEL);
+	if (!state)
+		return -ENOMEM;
+	state->i2c = client;
+
+	cfg = client->dev.platform_data;
+	fe = cfg->fe;
+	fe->tuner_priv = state;
+	qm1d1c0042_set_config(fe, cfg);
+	memcpy(&fe->ops.tuner_ops, &qm1d1c0042_ops, sizeof(qm1d1c0042_ops));
+
+	i2c_set_clientdata(client, &state->cfg);
+	dev_info(&client->dev, "Sharp QM1D1C0042 attached.\n");
+	return 0;
+}
+
+static int qm1d1c0042_remove(struct i2c_client *client)
+{
+	struct qm1d1c0042_state *state;
+
+	state = cfg_to_state(i2c_get_clientdata(client));
+	state->cfg.fe->tuner_priv = NULL;
+	kfree(state);
+	return 0;
+}
+
+
+static const struct i2c_device_id qm1d1c0042_id[] = {
+	{"qm1d1c0042", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, qm1d1c0042_id);
+
+static struct i2c_driver qm1d1c0042_driver = {
+	.driver = {
+		.name	= "qm1d1c0042",
+	},
+	.probe		= qm1d1c0042_probe,
+	.remove		= qm1d1c0042_remove,
+	.id_table	= qm1d1c0042_id,
+};
+
+module_i2c_driver(qm1d1c0042_driver);
+
+MODULE_DESCRIPTION("Sharp QM1D1C0042 tuner");
+MODULE_AUTHOR("Akihiro TSUKADA");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/qm1d1c0042.h b/drivers/media/tuners/qm1d1c0042.h
new file mode 100644
index 000000000000..4f5c18816c44
--- /dev/null
+++ b/drivers/media/tuners/qm1d1c0042.h
@@ -0,0 +1,37 @@
+/*
+ * Sharp QM1D1C0042 8PSK tuner driver
+ *
+ * Copyright (C) 2014 Akihiro Tsukada <tskd08@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef QM1D1C0042_H
+#define QM1D1C0042_H
+
+#include "dvb_frontend.h"
+
+
+struct qm1d1c0042_config {
+	struct dvb_frontend *fe;
+
+	u32  xtal_freq;    /* [kHz] */ /* currently ignored */
+	bool lpf;          /* enable LPF */
+	bool fast_srch;    /* enable fast search mode, no LPF */
+	u32  lpf_wait;         /* wait in tuning with LPF enabled. [ms] */
+	u32  fast_srch_wait;   /* with fast-search mode, no LPF. [ms] */
+	u32  normal_srch_wait; /* with no LPF/fast-search mode. [ms] */
+};
+/* special values indicating to use the default in qm1d1c0042_config */
+#define QM1D1C0042_CFG_XTAL_DFLT 0
+#define QM1D1C0042_CFG_WAIT_DFLT 0
+
+#endif /* QM1D1C0042_H */
diff --git a/drivers/media/tuners/si2157.c b/drivers/media/tuners/si2157.c
index 6c53edb73a63..cf97142e01e6 100644
--- a/drivers/media/tuners/si2157.c
+++ b/drivers/media/tuners/si2157.c
@@ -1,5 +1,5 @@
 /*
- * Silicon Labs Si2157/2158 silicon tuner driver
+ * Silicon Labs Si2147/2157/2158 silicon tuner driver
  *
  * Copyright (C) 2014 Antti Palosaari <crope@iki.fi>
  *
@@ -55,8 +55,7 @@ static int si2157_cmd_execute(struct si2157 *s, struct si2157_cmd *cmd)
 				break;
 		}
 
-		dev_dbg(&s->client->dev, "%s: cmd execution took %d ms\n",
-				__func__,
+		dev_dbg(&s->client->dev, "cmd execution took %d ms\n",
 				jiffies_to_msecs(jiffies) -
 				(jiffies_to_msecs(timeout) - TIMEOUT));
 
@@ -75,7 +74,7 @@ err_mutex_unlock:
 
 	return 0;
 err:
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -88,9 +87,12 @@ static int si2157_init(struct dvb_frontend *fe)
 	u8 *fw_file;
 	unsigned int chip_id;
 
-	dev_dbg(&s->client->dev, "%s:\n", __func__);
+	dev_dbg(&s->client->dev, "\n");
 
-	/* configure? */
+	if (s->fw_loaded)
+		goto warm;
+
+	/* power up */
 	memcpy(cmd.args, "\xc0\x00\x0c\x00\x00\x01\x01\x01\x01\x01\x01\x02\x00\x00\x01", 15);
 	cmd.wlen = 15;
 	cmd.rlen = 1;
@@ -111,45 +113,47 @@ static int si2157_init(struct dvb_frontend *fe)
 
 	#define SI2158_A20 ('A' << 24 | 58 << 16 | '2' << 8 | '0' << 0)
 	#define SI2157_A30 ('A' << 24 | 57 << 16 | '3' << 8 | '0' << 0)
+	#define SI2147_A30 ('A' << 24 | 47 << 16 | '3' << 8 | '0' << 0)
 
 	switch (chip_id) {
 	case SI2158_A20:
 		fw_file = SI2158_A20_FIRMWARE;
 		break;
 	case SI2157_A30:
+	case SI2147_A30:
 		goto skip_fw_download;
 		break;
 	default:
 		dev_err(&s->client->dev,
-				"%s: unkown chip version Si21%d-%c%c%c\n",
-				KBUILD_MODNAME, cmd.args[2], cmd.args[1],
+				"unknown chip version Si21%d-%c%c%c\n",
+				cmd.args[2], cmd.args[1],
 				cmd.args[3], cmd.args[4]);
 		ret = -EINVAL;
 		goto err;
 	}
 
 	/* cold state - try to download firmware */
-	dev_info(&s->client->dev, "%s: found a '%s' in cold state\n",
-			KBUILD_MODNAME, si2157_ops.info.name);
+	dev_info(&s->client->dev, "found a '%s' in cold state\n",
+			si2157_ops.info.name);
 
 	/* request the firmware, this will block and timeout */
 	ret = request_firmware(&fw, fw_file, &s->client->dev);
 	if (ret) {
-		dev_err(&s->client->dev, "%s: firmware file '%s' not found\n",
-				KBUILD_MODNAME, fw_file);
+		dev_err(&s->client->dev, "firmware file '%s' not found\n",
+				fw_file);
 		goto err;
 	}
 
 	/* firmware should be n chunks of 17 bytes */
 	if (fw->size % 17 != 0) {
-		dev_err(&s->client->dev, "%s: firmware file '%s' is invalid\n",
-				KBUILD_MODNAME, fw_file);
+		dev_err(&s->client->dev, "firmware file '%s' is invalid\n",
+				fw_file);
 		ret = -EINVAL;
 		goto err;
 	}
 
-	dev_info(&s->client->dev, "%s: downloading firmware from file '%s'\n",
-			KBUILD_MODNAME, fw_file);
+	dev_info(&s->client->dev, "downloading firmware from file '%s'\n",
+			fw_file);
 
 	for (remaining = fw->size; remaining > 0; remaining -= 17) {
 		len = fw->data[fw->size - remaining];
@@ -159,8 +163,8 @@ static int si2157_init(struct dvb_frontend *fe)
 		ret = si2157_cmd_execute(s, &cmd);
 		if (ret) {
 			dev_err(&s->client->dev,
-					"%s: firmware download failed=%d\n",
-					KBUILD_MODNAME, ret);
+					"firmware download failed=%d\n",
+					ret);
 			goto err;
 		}
 	}
@@ -177,14 +181,17 @@ skip_fw_download:
 	if (ret)
 		goto err;
 
-	s->active = true;
+	s->fw_loaded = true;
 
+warm:
+	s->active = true;
 	return 0;
+
 err:
 	if (fw)
 		release_firmware(fw);
 
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -194,20 +201,21 @@ static int si2157_sleep(struct dvb_frontend *fe)
 	int ret;
 	struct si2157_cmd cmd;
 
-	dev_dbg(&s->client->dev, "%s:\n", __func__);
+	dev_dbg(&s->client->dev, "\n");
 
 	s->active = false;
 
-	memcpy(cmd.args, "\x13", 1);
-	cmd.wlen = 1;
-	cmd.rlen = 0;
+	/* standby */
+	memcpy(cmd.args, "\x16\x00", 2);
+	cmd.wlen = 2;
+	cmd.rlen = 1;
 	ret = si2157_cmd_execute(s, &cmd);
 	if (ret)
 		goto err;
 
 	return 0;
 err:
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -220,8 +228,8 @@ static int si2157_set_params(struct dvb_frontend *fe)
 	u8 bandwidth, delivery_system;
 
 	dev_dbg(&s->client->dev,
-			"%s: delivery_system=%d frequency=%u bandwidth_hz=%u\n",
-			__func__, c->delivery_system, c->frequency,
+			"delivery_system=%d frequency=%u bandwidth_hz=%u\n",
+			c->delivery_system, c->frequency,
 			c->bandwidth_hz);
 
 	if (!s->active) {
@@ -239,6 +247,9 @@ static int si2157_set_params(struct dvb_frontend *fe)
 		bandwidth = 0x0f;
 
 	switch (c->delivery_system) {
+	case SYS_ATSC:
+			delivery_system = 0x00;
+			break;
 	case SYS_DVBT:
 	case SYS_DVBT2: /* it seems DVB-T and DVB-T2 both are 0x20 here */
 			delivery_system = 0x20;
@@ -256,7 +267,14 @@ static int si2157_set_params(struct dvb_frontend *fe)
 	if (s->inversion)
 		cmd.args[5] = 0x01;
 	cmd.wlen = 6;
-	cmd.rlen = 1;
+	cmd.rlen = 4;
+	ret = si2157_cmd_execute(s, &cmd);
+	if (ret)
+		goto err;
+
+	memcpy(cmd.args, "\x14\x00\x02\x07\x01\x00", 6);
+	cmd.wlen = 6;
+	cmd.rlen = 4;
 	ret = si2157_cmd_execute(s, &cmd);
 	if (ret)
 		goto err;
@@ -275,7 +293,7 @@ static int si2157_set_params(struct dvb_frontend *fe)
 
 	return 0;
 err:
-	dev_dbg(&s->client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&s->client->dev, "failed=%d\n", ret);
 	return ret;
 }
 
@@ -310,13 +328,14 @@ static int si2157_probe(struct i2c_client *client,
 	s = kzalloc(sizeof(struct si2157), GFP_KERNEL);
 	if (!s) {
 		ret = -ENOMEM;
-		dev_err(&client->dev, "%s: kzalloc() failed\n", KBUILD_MODNAME);
+		dev_err(&client->dev, "kzalloc() failed\n");
 		goto err;
 	}
 
 	s->client = client;
 	s->fe = cfg->fe;
 	s->inversion = cfg->inversion;
+	s->fw_loaded = false;
 	mutex_init(&s->i2c_mutex);
 
 	/* check if the tuner is there */
@@ -333,11 +352,10 @@ static int si2157_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, s);
 
 	dev_info(&s->client->dev,
-			"%s: Silicon Labs Si2157/Si2158 successfully attached\n",
-			KBUILD_MODNAME);
+			"Silicon Labs Si2157/Si2158 successfully attached\n");
 	return 0;
 err:
-	dev_dbg(&client->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&client->dev, "failed=%d\n", ret);
 	kfree(s);
 
 	return ret;
@@ -348,7 +366,7 @@ static int si2157_remove(struct i2c_client *client)
 	struct si2157 *s = i2c_get_clientdata(client);
 	struct dvb_frontend *fe = s->fe;
 
-	dev_dbg(&client->dev, "%s:\n", __func__);
+	dev_dbg(&client->dev, "\n");
 
 	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
 	fe->tuner_priv = NULL;
diff --git a/drivers/media/tuners/si2157.h b/drivers/media/tuners/si2157.h
index 6da4d5d1c817..d3b19cadb4a1 100644
--- a/drivers/media/tuners/si2157.h
+++ b/drivers/media/tuners/si2157.h
@@ -1,5 +1,5 @@
 /*
- * Silicon Labs Si2157/2158 silicon tuner driver
+ * Silicon Labs Si2147/2157/2158 silicon tuner driver
  *
  * Copyright (C) 2014 Antti Palosaari <crope@iki.fi>
  *
diff --git a/drivers/media/tuners/si2157_priv.h b/drivers/media/tuners/si2157_priv.h
index 3ddab5e6b500..e71ffafed951 100644
--- a/drivers/media/tuners/si2157_priv.h
+++ b/drivers/media/tuners/si2157_priv.h
@@ -1,5 +1,5 @@
 /*
- * Silicon Labs Si2157/2158 silicon tuner driver
+ * Silicon Labs Si2147/2157/2158 silicon tuner driver
  *
  * Copyright (C) 2014 Antti Palosaari <crope@iki.fi>
  *
@@ -26,6 +26,7 @@ struct si2157 {
 	struct i2c_client *client;
 	struct dvb_frontend *fe;
 	bool active;
+	bool fw_loaded;
 	bool inversion;
 };
 
diff --git a/drivers/media/tuners/tda18212.c b/drivers/media/tuners/tda18212.c
index 05a4ac9edb6b..d93e0667b46b 100644
--- a/drivers/media/tuners/tda18212.c
+++ b/drivers/media/tuners/tda18212.c
@@ -19,125 +19,19 @@
  */
 
 #include "tda18212.h"
+#include <linux/regmap.h>
 
-/* Max transfer size done by I2C transfer functions */
-#define MAX_XFER_SIZE  64
-
-struct tda18212_priv {
-	struct tda18212_config *cfg;
-	struct i2c_adapter *i2c;
+struct tda18212_dev {
+	struct tda18212_config cfg;
+	struct i2c_client *client;
+	struct regmap *regmap;
 
 	u32 if_frequency;
 };
 
-/* write multiple registers */
-static int tda18212_wr_regs(struct tda18212_priv *priv, u8 reg, u8 *val,
-	int len)
-{
-	int ret;
-	u8 buf[MAX_XFER_SIZE];
-	struct i2c_msg msg[1] = {
-		{
-			.addr = priv->cfg->i2c_address,
-			.flags = 0,
-			.len = 1 + len,
-			.buf = buf,
-		}
-	};
-
-	if (1 + len > sizeof(buf)) {
-		dev_warn(&priv->i2c->dev,
-			 "%s: i2c wr reg=%04x: len=%d is too big!\n",
-			 KBUILD_MODNAME, reg, len);
-		return -EINVAL;
-	}
-
-	buf[0] = reg;
-	memcpy(&buf[1], val, len);
-
-	ret = i2c_transfer(priv->i2c, msg, 1);
-	if (ret == 1) {
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev, "%s: i2c wr failed=%d reg=%02x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-	return ret;
-}
-
-/* read multiple registers */
-static int tda18212_rd_regs(struct tda18212_priv *priv, u8 reg, u8 *val,
-	int len)
-{
-	int ret;
-	u8 buf[MAX_XFER_SIZE];
-	struct i2c_msg msg[2] = {
-		{
-			.addr = priv->cfg->i2c_address,
-			.flags = 0,
-			.len = 1,
-			.buf = &reg,
-		}, {
-			.addr = priv->cfg->i2c_address,
-			.flags = I2C_M_RD,
-			.len = len,
-			.buf = buf,
-		}
-	};
-
-	if (len > sizeof(buf)) {
-		dev_warn(&priv->i2c->dev,
-			 "%s: i2c rd reg=%04x: len=%d is too big!\n",
-			 KBUILD_MODNAME, reg, len);
-		return -EINVAL;
-	}
-
-	ret = i2c_transfer(priv->i2c, msg, 2);
-	if (ret == 2) {
-		memcpy(val, buf, len);
-		ret = 0;
-	} else {
-		dev_warn(&priv->i2c->dev, "%s: i2c rd failed=%d reg=%02x " \
-				"len=%d\n", KBUILD_MODNAME, ret, reg, len);
-		ret = -EREMOTEIO;
-	}
-
-	return ret;
-}
-
-/* write single register */
-static int tda18212_wr_reg(struct tda18212_priv *priv, u8 reg, u8 val)
-{
-	return tda18212_wr_regs(priv, reg, &val, 1);
-}
-
-/* read single register */
-static int tda18212_rd_reg(struct tda18212_priv *priv, u8 reg, u8 *val)
-{
-	return tda18212_rd_regs(priv, reg, val, 1);
-}
-
-#if 0 /* keep, useful when developing driver */
-static void tda18212_dump_regs(struct tda18212_priv *priv)
-{
-	int i;
-	u8 buf[256];
-
-	#define TDA18212_RD_LEN 32
-	for (i = 0; i < sizeof(buf); i += TDA18212_RD_LEN)
-		tda18212_rd_regs(priv, i, &buf[i], TDA18212_RD_LEN);
-
-	print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET, 32, 1, buf,
-		sizeof(buf), true);
-
-	return;
-}
-#endif
-
 static int tda18212_set_params(struct dvb_frontend *fe)
 {
-	struct tda18212_priv *priv = fe->tuner_priv;
+	struct tda18212_dev *dev = fe->tuner_priv;
 	struct dtv_frontend_properties *c = &fe->dtv_property_cache;
 	int ret, i;
 	u32 if_khz;
@@ -166,9 +60,9 @@ static int tda18212_set_params(struct dvb_frontend *fe)
 		[ATSC_QAM] = { 0x7d, 0x20, 0x63 },
 	};
 
-	dev_dbg(&priv->i2c->dev,
-			"%s: delivery_system=%d frequency=%d bandwidth_hz=%d\n",
-			__func__, c->delivery_system, c->frequency,
+	dev_dbg(&dev->client->dev,
+			"delivery_system=%d frequency=%d bandwidth_hz=%d\n",
+			c->delivery_system, c->frequency,
 			c->bandwidth_hz);
 
 	if (fe->ops.i2c_gate_ctrl)
@@ -176,25 +70,25 @@ static int tda18212_set_params(struct dvb_frontend *fe)
 
 	switch (c->delivery_system) {
 	case SYS_ATSC:
-		if_khz = priv->cfg->if_atsc_vsb;
+		if_khz = dev->cfg.if_atsc_vsb;
 		i = ATSC_VSB;
 		break;
 	case SYS_DVBC_ANNEX_B:
-		if_khz = priv->cfg->if_atsc_qam;
+		if_khz = dev->cfg.if_atsc_qam;
 		i = ATSC_QAM;
 		break;
 	case SYS_DVBT:
 		switch (c->bandwidth_hz) {
 		case 6000000:
-			if_khz = priv->cfg->if_dvbt_6;
+			if_khz = dev->cfg.if_dvbt_6;
 			i = DVBT_6;
 			break;
 		case 7000000:
-			if_khz = priv->cfg->if_dvbt_7;
+			if_khz = dev->cfg.if_dvbt_7;
 			i = DVBT_7;
 			break;
 		case 8000000:
-			if_khz = priv->cfg->if_dvbt_8;
+			if_khz = dev->cfg.if_dvbt_8;
 			i = DVBT_8;
 			break;
 		default:
@@ -205,15 +99,15 @@ static int tda18212_set_params(struct dvb_frontend *fe)
 	case SYS_DVBT2:
 		switch (c->bandwidth_hz) {
 		case 6000000:
-			if_khz = priv->cfg->if_dvbt2_6;
+			if_khz = dev->cfg.if_dvbt2_6;
 			i = DVBT2_6;
 			break;
 		case 7000000:
-			if_khz = priv->cfg->if_dvbt2_7;
+			if_khz = dev->cfg.if_dvbt2_7;
 			i = DVBT2_7;
 			break;
 		case 8000000:
-			if_khz = priv->cfg->if_dvbt2_8;
+			if_khz = dev->cfg.if_dvbt2_8;
 			i = DVBT2_8;
 			break;
 		default:
@@ -223,7 +117,7 @@ static int tda18212_set_params(struct dvb_frontend *fe)
 		break;
 	case SYS_DVBC_ANNEX_A:
 	case SYS_DVBC_ANNEX_C:
-		if_khz = priv->cfg->if_dvbc;
+		if_khz = dev->cfg.if_dvbc;
 		i = DVBC_8;
 		break;
 	default:
@@ -231,15 +125,15 @@ static int tda18212_set_params(struct dvb_frontend *fe)
 		goto error;
 	}
 
-	ret = tda18212_wr_reg(priv, 0x23, bw_params[i][2]);
+	ret = regmap_write(dev->regmap, 0x23, bw_params[i][2]);
 	if (ret)
 		goto error;
 
-	ret = tda18212_wr_reg(priv, 0x06, 0x00);
+	ret = regmap_write(dev->regmap, 0x06, 0x00);
 	if (ret)
 		goto error;
 
-	ret = tda18212_wr_reg(priv, 0x0f, bw_params[i][0]);
+	ret = regmap_write(dev->regmap, 0x0f, bw_params[i][0]);
 	if (ret)
 		goto error;
 
@@ -252,12 +146,12 @@ static int tda18212_set_params(struct dvb_frontend *fe)
 	buf[6] = ((c->frequency / 1000) >>  0) & 0xff;
 	buf[7] = 0xc1;
 	buf[8] = 0x01;
-	ret = tda18212_wr_regs(priv, 0x12, buf, sizeof(buf));
+	ret = regmap_bulk_write(dev->regmap, 0x12, buf, sizeof(buf));
 	if (ret)
 		goto error;
 
 	/* actual IF rounded as it is on register */
-	priv->if_frequency = buf[3] * 50 * 1000;
+	dev->if_frequency = buf[3] * 50 * 1000;
 
 exit:
 	if (fe->ops.i2c_gate_ctrl)
@@ -266,26 +160,19 @@ exit:
 	return ret;
 
 error:
-	dev_dbg(&priv->i2c->dev, "%s: failed=%d\n", __func__, ret);
+	dev_dbg(&dev->client->dev, "failed=%d\n", ret);
 	goto exit;
 }
 
 static int tda18212_get_if_frequency(struct dvb_frontend *fe, u32 *frequency)
 {
-	struct tda18212_priv *priv = fe->tuner_priv;
+	struct tda18212_dev *dev = fe->tuner_priv;
 
-	*frequency = priv->if_frequency;
+	*frequency = dev->if_frequency;
 
 	return 0;
 }
 
-static int tda18212_release(struct dvb_frontend *fe)
-{
-	kfree(fe->tuner_priv);
-	fe->tuner_priv = NULL;
-	return 0;
-}
-
 static const struct dvb_tuner_ops tda18212_tuner_ops = {
 	.info = {
 		.name           = "NXP TDA18212",
@@ -295,53 +182,110 @@ static const struct dvb_tuner_ops tda18212_tuner_ops = {
 		.frequency_step =      1000,
 	},
 
-	.release       = tda18212_release,
-
 	.set_params    = tda18212_set_params,
 	.get_if_frequency = tda18212_get_if_frequency,
 };
 
-struct dvb_frontend *tda18212_attach(struct dvb_frontend *fe,
-	struct i2c_adapter *i2c, struct tda18212_config *cfg)
+static int tda18212_probe(struct i2c_client *client,
+		const struct i2c_device_id *id)
 {
-	struct tda18212_priv *priv = NULL;
+	struct tda18212_config *cfg = client->dev.platform_data;
+	struct dvb_frontend *fe = cfg->fe;
+	struct tda18212_dev *dev;
 	int ret;
-	u8 val;
+	unsigned int chip_id;
+	char *version;
+	static const struct regmap_config regmap_config = {
+		.reg_bits = 8,
+		.val_bits = 8,
+	};
 
-	priv = kzalloc(sizeof(struct tda18212_priv), GFP_KERNEL);
-	if (priv == NULL)
-		return NULL;
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL) {
+		ret = -ENOMEM;
+		dev_err(&client->dev, "kzalloc() failed\n");
+		goto err;
+	}
 
-	priv->cfg = cfg;
-	priv->i2c = i2c;
-	fe->tuner_priv = priv;
+	memcpy(&dev->cfg, cfg, sizeof(struct tda18212_config));
+	dev->client = client;
+	dev->regmap = devm_regmap_init_i2c(client, &regmap_config);
+	if (IS_ERR(dev->regmap)) {
+		ret = PTR_ERR(dev->regmap);
+		goto err;
+	}
 
+	/* check if the tuner is there */
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 1); /* open I2C-gate */
 
-	/* check if the tuner is there */
-	ret = tda18212_rd_reg(priv, 0x00, &val);
+	ret = regmap_read(dev->regmap, 0x00, &chip_id);
+	dev_dbg(&dev->client->dev, "chip_id=%02x\n", chip_id);
 
 	if (fe->ops.i2c_gate_ctrl)
 		fe->ops.i2c_gate_ctrl(fe, 0); /* close I2C-gate */
 
-	if (!ret)
-		dev_dbg(&priv->i2c->dev, "%s: chip id=%02x\n", __func__, val);
-	if (ret || val != 0xc7) {
-		kfree(priv);
-		return NULL;
+	if (ret)
+		goto err;
+
+	switch (chip_id) {
+	case 0xc7:
+		version = "M"; /* master */
+		break;
+	case 0x47:
+		version = "S"; /* slave */
+		break;
+	default:
+		ret = -ENODEV;
+		goto err;
 	}
 
-	dev_info(&priv->i2c->dev,
-			"%s: NXP TDA18212HN successfully identified\n",
-			KBUILD_MODNAME);
+	dev_info(&dev->client->dev,
+			"NXP TDA18212HN/%s successfully identified\n", version);
 
+	fe->tuner_priv = dev;
 	memcpy(&fe->ops.tuner_ops, &tda18212_tuner_ops,
-		sizeof(struct dvb_tuner_ops));
+			sizeof(struct dvb_tuner_ops));
+	i2c_set_clientdata(client, dev);
 
-	return fe;
+	return 0;
+err:
+	dev_dbg(&client->dev, "failed=%d\n", ret);
+	kfree(dev);
+	return ret;
 }
-EXPORT_SYMBOL(tda18212_attach);
+
+static int tda18212_remove(struct i2c_client *client)
+{
+	struct tda18212_dev *dev = i2c_get_clientdata(client);
+	struct dvb_frontend *fe = dev->cfg.fe;
+
+	dev_dbg(&client->dev, "\n");
+
+	memset(&fe->ops.tuner_ops, 0, sizeof(struct dvb_tuner_ops));
+	fe->tuner_priv = NULL;
+	kfree(dev);
+
+	return 0;
+}
+
+static const struct i2c_device_id tda18212_id[] = {
+	{"tda18212", 0},
+	{}
+};
+MODULE_DEVICE_TABLE(i2c, tda18212_id);
+
+static struct i2c_driver tda18212_driver = {
+	.driver = {
+		.owner	= THIS_MODULE,
+		.name	= "tda18212",
+	},
+	.probe		= tda18212_probe,
+	.remove		= tda18212_remove,
+	.id_table	= tda18212_id,
+};
+
+module_i2c_driver(tda18212_driver);
 
 MODULE_DESCRIPTION("NXP TDA18212HN silicon tuner driver");
 MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
diff --git a/drivers/media/tuners/tda18212.h b/drivers/media/tuners/tda18212.h
index c36b49e4b274..e58c9096d79c 100644
--- a/drivers/media/tuners/tda18212.h
+++ b/drivers/media/tuners/tda18212.h
@@ -25,8 +25,6 @@
 #include "dvb_frontend.h"
 
 struct tda18212_config {
-	u8 i2c_address;
-
 	u16 if_dvbt_6;
 	u16 if_dvbt_7;
 	u16 if_dvbt_8;
@@ -37,18 +35,11 @@ struct tda18212_config {
 	u16 if_dvbc;
 	u16 if_atsc_vsb;
 	u16 if_atsc_qam;
-};
 
-#if IS_ENABLED(CONFIG_MEDIA_TUNER_TDA18212)
-extern struct dvb_frontend *tda18212_attach(struct dvb_frontend *fe,
-	struct i2c_adapter *i2c, struct tda18212_config *cfg);
-#else
-static inline struct dvb_frontend *tda18212_attach(struct dvb_frontend *fe,
-	struct i2c_adapter *i2c, struct tda18212_config *cfg)
-{
-	printk(KERN_WARNING "%s: driver disabled by Kconfig\n", __func__);
-	return NULL;
-}
-#endif
+	/*
+	 * pointer to DVB frontend
+	 */
+	struct dvb_frontend *fe;
+};
 
 #endif
diff --git a/drivers/media/tuners/tda18271-common.c b/drivers/media/tuners/tda18271-common.c
index 18c77afe2e4f..86e5e3110118 100644
--- a/drivers/media/tuners/tda18271-common.c
+++ b/drivers/media/tuners/tda18271-common.c
@@ -714,12 +714,11 @@ fail:
 	return ret;
 }
 
-int _tda_printk(struct tda18271_priv *state, const char *level,
-		const char *func, const char *fmt, ...)
+void _tda_printk(struct tda18271_priv *state, const char *level,
+		 const char *func, const char *fmt, ...)
 {
 	struct va_format vaf;
 	va_list args;
-	int rtn;
 
 	va_start(args, fmt);
 
@@ -727,15 +726,13 @@ int _tda_printk(struct tda18271_priv *state, const char *level,
 	vaf.va = &args;
 
 	if (state)
-		rtn = printk("%s%s: [%d-%04x|%c] %pV",
-			     level, func, i2c_adapter_id(state->i2c_props.adap),
-			     state->i2c_props.addr,
-			     (state->role == TDA18271_MASTER) ? 'M' : 'S',
-			     &vaf);
+		printk("%s%s: [%d-%04x|%c] %pV",
+		       level, func, i2c_adapter_id(state->i2c_props.adap),
+		       state->i2c_props.addr,
+		       (state->role == TDA18271_MASTER) ? 'M' : 'S',
+		       &vaf);
 	else
-		rtn = printk("%s%s: %pV", level, func, &vaf);
+		printk("%s%s: %pV", level, func, &vaf);
 
 	va_end(args);
-
-	return rtn;
 }
diff --git a/drivers/media/tuners/tda18271-priv.h b/drivers/media/tuners/tda18271-priv.h
index 454c152ccaa0..b36a7b754772 100644
--- a/drivers/media/tuners/tda18271-priv.h
+++ b/drivers/media/tuners/tda18271-priv.h
@@ -139,8 +139,8 @@ extern int tda18271_debug;
 #define DBG_CAL  16
 
 __attribute__((format(printf, 4, 5)))
-int _tda_printk(struct tda18271_priv *state, const char *level,
-		const char *func, const char *fmt, ...);
+void _tda_printk(struct tda18271_priv *state, const char *level,
+		 const char *func, const char *fmt, ...);
 
 #define tda_printk(st, lvl, fmt, arg...)			\
 	_tda_printk(st, lvl, __func__, fmt, ##arg)
diff --git a/drivers/media/tuners/tuner-xc2028.c b/drivers/media/tuners/tuner-xc2028.c
index 565eeebb3aeb..d12f5e4ad8bf 100644
--- a/drivers/media/tuners/tuner-xc2028.c
+++ b/drivers/media/tuners/tuner-xc2028.c
@@ -178,67 +178,67 @@ static int xc2028_get_reg(struct xc2028_data *priv, u16 reg, u16 *val)
 #define dump_firm_type(t) 	dump_firm_type_and_int_freq(t, 0)
 static void dump_firm_type_and_int_freq(unsigned int type, u16 int_freq)
 {
-	 if (type & BASE)
+	if (type & BASE)
 		printk("BASE ");
-	 if (type & INIT1)
+	if (type & INIT1)
 		printk("INIT1 ");
-	 if (type & F8MHZ)
+	if (type & F8MHZ)
 		printk("F8MHZ ");
-	 if (type & MTS)
+	if (type & MTS)
 		printk("MTS ");
-	 if (type & D2620)
+	if (type & D2620)
 		printk("D2620 ");
-	 if (type & D2633)
+	if (type & D2633)
 		printk("D2633 ");
-	 if (type & DTV6)
+	if (type & DTV6)
 		printk("DTV6 ");
-	 if (type & QAM)
+	if (type & QAM)
 		printk("QAM ");
-	 if (type & DTV7)
+	if (type & DTV7)
 		printk("DTV7 ");
-	 if (type & DTV78)
+	if (type & DTV78)
 		printk("DTV78 ");
-	 if (type & DTV8)
+	if (type & DTV8)
 		printk("DTV8 ");
-	 if (type & FM)
+	if (type & FM)
 		printk("FM ");
-	 if (type & INPUT1)
+	if (type & INPUT1)
 		printk("INPUT1 ");
-	 if (type & LCD)
+	if (type & LCD)
 		printk("LCD ");
-	 if (type & NOGD)
+	if (type & NOGD)
 		printk("NOGD ");
-	 if (type & MONO)
+	if (type & MONO)
 		printk("MONO ");
-	 if (type & ATSC)
+	if (type & ATSC)
 		printk("ATSC ");
-	 if (type & IF)
+	if (type & IF)
 		printk("IF ");
-	 if (type & LG60)
+	if (type & LG60)
 		printk("LG60 ");
-	 if (type & ATI638)
+	if (type & ATI638)
 		printk("ATI638 ");
-	 if (type & OREN538)
+	if (type & OREN538)
 		printk("OREN538 ");
-	 if (type & OREN36)
+	if (type & OREN36)
 		printk("OREN36 ");
-	 if (type & TOYOTA388)
+	if (type & TOYOTA388)
 		printk("TOYOTA388 ");
-	 if (type & TOYOTA794)
+	if (type & TOYOTA794)
 		printk("TOYOTA794 ");
-	 if (type & DIBCOM52)
+	if (type & DIBCOM52)
 		printk("DIBCOM52 ");
-	 if (type & ZARLINK456)
+	if (type & ZARLINK456)
 		printk("ZARLINK456 ");
-	 if (type & CHINA)
+	if (type & CHINA)
 		printk("CHINA ");
-	 if (type & F6MHZ)
+	if (type & F6MHZ)
 		printk("F6MHZ ");
-	 if (type & INPUT2)
+	if (type & INPUT2)
 		printk("INPUT2 ");
-	 if (type & SCODE)
+	if (type & SCODE)
 		printk("SCODE ");
-	 if (type & HAS_IF)
+	if (type & HAS_IF)
 		printk("HAS_IF_%d ", int_freq);
 }
 
diff --git a/drivers/media/tuners/tuner_it913x.c b/drivers/media/tuners/tuner_it913x.c
deleted file mode 100644
index 6f30d7e535b8..000000000000
--- a/drivers/media/tuners/tuner_it913x.c
+++ /dev/null
@@ -1,447 +0,0 @@
-/*
- * ITE Tech IT9137 silicon tuner driver
- *
- *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
- *  IT9137 Copyright (C) ITE Tech Inc.
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.=
- */
-
-#include "tuner_it913x_priv.h"
-
-struct it913x_state {
-	struct i2c_adapter *i2c_adap;
-	u8 i2c_addr;
-	u8 chip_ver;
-	u8 tuner_type;
-	u8 firmware_ver;
-	u16 tun_xtal;
-	u8 tun_fdiv;
-	u8 tun_clk_mode;
-	u32 tun_fn_min;
-};
-
-/* read multiple registers */
-static int it913x_rd_regs(struct it913x_state *state,
-		u32 reg, u8 *data, u8 count)
-{
-	int ret;
-	u8 b[3];
-	struct i2c_msg msg[2] = {
-		{ .addr = state->i2c_addr, .flags = 0,
-			.buf = b, .len = sizeof(b) },
-		{ .addr = state->i2c_addr, .flags = I2C_M_RD,
-			.buf = data, .len = count }
-	};
-	b[0] = (u8)(reg >> 16) & 0xff;
-	b[1] = (u8)(reg >> 8) & 0xff;
-	b[2] = (u8) reg & 0xff;
-	b[0] |= 0x80; /* All reads from demodulator */
-
-	ret = i2c_transfer(state->i2c_adap, msg, 2);
-
-	return ret;
-}
-
-/* read single register */
-static int it913x_rd_reg(struct it913x_state *state, u32 reg)
-{
-	int ret;
-	u8 b[1];
-	ret = it913x_rd_regs(state, reg, &b[0], sizeof(b));
-	return (ret < 0) ? -ENODEV : b[0];
-}
-
-/* write multiple registers */
-static int it913x_wr_regs(struct it913x_state *state,
-		u8 pro, u32 reg, u8 buf[], u8 count)
-{
-	u8 b[256];
-	struct i2c_msg msg[1] = {
-		{ .addr = state->i2c_addr, .flags = 0,
-		  .buf = b, .len = 3 + count }
-	};
-	int ret;
-	b[0] = (u8)(reg >> 16) & 0xff;
-	b[1] = (u8)(reg >> 8) & 0xff;
-	b[2] = (u8) reg & 0xff;
-	memcpy(&b[3], buf, count);
-
-	if (pro == PRO_DMOD)
-		b[0] |= 0x80;
-
-	ret = i2c_transfer(state->i2c_adap, msg, 1);
-
-	if (ret < 0)
-		return -EIO;
-
-	return 0;
-}
-
-/* write single register */
-static int it913x_wr_reg(struct it913x_state *state,
-		u8 pro, u32 reg, u32 data)
-{
-	int ret;
-	u8 b[4];
-	u8 s;
-
-	b[0] = data >> 24;
-	b[1] = (data >> 16) & 0xff;
-	b[2] = (data >> 8) & 0xff;
-	b[3] = data & 0xff;
-	/* expand write as needed */
-	if (data < 0x100)
-		s = 3;
-	else if (data < 0x1000)
-		s = 2;
-	else if (data < 0x100000)
-		s = 1;
-	else
-		s = 0;
-
-	ret = it913x_wr_regs(state, pro, reg, &b[s], sizeof(b) - s);
-
-	return ret;
-}
-
-static int it913x_script_loader(struct it913x_state *state,
-		struct it913xset *loadscript)
-{
-	int ret, i;
-	if (loadscript == NULL)
-		return -EINVAL;
-
-	for (i = 0; i < 1000; ++i) {
-		if (loadscript[i].pro == 0xff)
-			break;
-		ret = it913x_wr_regs(state, loadscript[i].pro,
-			loadscript[i].address,
-			loadscript[i].reg, loadscript[i].count);
-		if (ret < 0)
-			return -ENODEV;
-	}
-	return 0;
-}
-
-static int it913x_init(struct dvb_frontend *fe)
-{
-	struct it913x_state *state = fe->tuner_priv;
-	int ret, i, reg;
-	u8 val, nv_val;
-	u8 nv[] = {48, 32, 24, 16, 12, 8, 6, 4, 2};
-	u8 b[2];
-
-	reg = it913x_rd_reg(state, 0xec86);
-	switch (reg) {
-	case 0:
-		state->tun_clk_mode = reg;
-		state->tun_xtal = 2000;
-		state->tun_fdiv = 3;
-		val = 16;
-		break;
-	case -ENODEV:
-		return -ENODEV;
-	case 1:
-	default:
-		state->tun_clk_mode = reg;
-		state->tun_xtal = 640;
-		state->tun_fdiv = 1;
-		val = 6;
-		break;
-	}
-
-	reg = it913x_rd_reg(state, 0xed03);
-
-	if (reg < 0)
-		return -ENODEV;
-	else if (reg < ARRAY_SIZE(nv))
-		nv_val = nv[reg];
-	else
-		nv_val = 2;
-
-	for (i = 0; i < 50; i++) {
-		ret = it913x_rd_regs(state, 0xed23, &b[0], sizeof(b));
-		reg = (b[1] << 8) + b[0];
-		if (reg > 0)
-			break;
-		if (ret < 0)
-			return -ENODEV;
-		udelay(2000);
-	}
-	state->tun_fn_min = state->tun_xtal * reg;
-	state->tun_fn_min /= (state->tun_fdiv * nv_val);
-	dev_dbg(&state->i2c_adap->dev, "%s: Tuner fn_min %d\n", __func__,
-			state->tun_fn_min);
-
-	if (state->chip_ver > 1)
-		msleep(50);
-	else {
-		for (i = 0; i < 50; i++) {
-			reg = it913x_rd_reg(state, 0xec82);
-			if (reg > 0)
-				break;
-			if (reg < 0)
-				return -ENODEV;
-			udelay(2000);
-		}
-	}
-
-	/* Power Up Tuner - common all versions */
-	ret = it913x_wr_reg(state, PRO_DMOD, 0xec40, 0x1);
-	ret |= it913x_wr_reg(state, PRO_DMOD, 0xfba8, 0x0);
-	ret |= it913x_wr_reg(state, PRO_DMOD, 0xec57, 0x0);
-	ret |= it913x_wr_reg(state, PRO_DMOD, 0xec58, 0x0);
-
-	return it913x_wr_reg(state, PRO_DMOD, 0xed81, val);
-}
-
-static int it9137_set_params(struct dvb_frontend *fe)
-{
-	struct it913x_state *state = fe->tuner_priv;
-	struct it913xset *set_tuner = set_it9137_template;
-	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
-	u32 bandwidth = p->bandwidth_hz;
-	u32 frequency_m = p->frequency;
-	int ret, reg;
-	u32 frequency = frequency_m / 1000;
-	u32 freq, temp_f, tmp;
-	u16 iqik_m_cal;
-	u16 n_div;
-	u8 n;
-	u8 l_band;
-	u8 lna_band;
-	u8 bw;
-
-	if (state->firmware_ver == 1)
-		set_tuner = set_it9135_template;
-	else
-		set_tuner = set_it9137_template;
-
-	dev_dbg(&state->i2c_adap->dev, "%s: Tuner Frequency %d Bandwidth %d\n",
-			__func__, frequency, bandwidth);
-
-	if (frequency >= 51000 && frequency <= 440000) {
-		l_band = 0;
-		lna_band = 0;
-	} else if (frequency > 440000 && frequency <= 484000) {
-		l_band = 1;
-		lna_band = 1;
-	} else if (frequency > 484000 && frequency <= 533000) {
-		l_band = 1;
-		lna_band = 2;
-	} else if (frequency > 533000 && frequency <= 587000) {
-		l_band = 1;
-		lna_band = 3;
-	} else if (frequency > 587000 && frequency <= 645000) {
-		l_band = 1;
-		lna_band = 4;
-	} else if (frequency > 645000 && frequency <= 710000) {
-		l_band = 1;
-		lna_band = 5;
-	} else if (frequency > 710000 && frequency <= 782000) {
-		l_band = 1;
-		lna_band = 6;
-	} else if (frequency > 782000 && frequency <= 860000) {
-		l_band = 1;
-		lna_band = 7;
-	} else if (frequency > 1450000 && frequency <= 1492000) {
-		l_band = 1;
-		lna_band = 0;
-	} else if (frequency > 1660000 && frequency <= 1685000) {
-		l_band = 1;
-		lna_band = 1;
-	} else
-		return -EINVAL;
-	set_tuner[0].reg[0] = lna_band;
-
-	switch (bandwidth) {
-	case 5000000:
-		bw = 0;
-		break;
-	case 6000000:
-		bw = 2;
-		break;
-	case 7000000:
-		bw = 4;
-		break;
-	default:
-	case 8000000:
-		bw = 6;
-		break;
-	}
-
-	set_tuner[1].reg[0] = bw;
-	set_tuner[2].reg[0] = 0xa0 | (l_band << 3);
-
-	if (frequency > 53000 && frequency <= 74000) {
-		n_div = 48;
-		n = 0;
-	} else if (frequency > 74000 && frequency <= 111000) {
-		n_div = 32;
-		n = 1;
-	} else if (frequency > 111000 && frequency <= 148000) {
-		n_div = 24;
-		n = 2;
-	} else if (frequency > 148000 && frequency <= 222000) {
-		n_div = 16;
-		n = 3;
-	} else if (frequency > 222000 && frequency <= 296000) {
-		n_div = 12;
-		n = 4;
-	} else if (frequency > 296000 && frequency <= 445000) {
-		n_div = 8;
-		n = 5;
-	} else if (frequency > 445000 && frequency <= state->tun_fn_min) {
-		n_div = 6;
-		n = 6;
-	} else if (frequency > state->tun_fn_min && frequency <= 950000) {
-		n_div = 4;
-		n = 7;
-	} else if (frequency > 1450000 && frequency <= 1680000) {
-		n_div = 2;
-		n = 0;
-	} else
-		return -EINVAL;
-
-	reg = it913x_rd_reg(state, 0xed81);
-	iqik_m_cal = (u16)reg * n_div;
-
-	if (reg < 0x20) {
-		if (state->tun_clk_mode == 0)
-			iqik_m_cal = (iqik_m_cal * 9) >> 5;
-		else
-			iqik_m_cal >>= 1;
-	} else {
-		iqik_m_cal = 0x40 - iqik_m_cal;
-		if (state->tun_clk_mode == 0)
-			iqik_m_cal = ~((iqik_m_cal * 9) >> 5);
-		else
-			iqik_m_cal = ~(iqik_m_cal >> 1);
-	}
-
-	temp_f = frequency * (u32)n_div * (u32)state->tun_fdiv;
-	freq = temp_f / state->tun_xtal;
-	tmp = freq * state->tun_xtal;
-
-	if ((temp_f - tmp) >= (state->tun_xtal >> 1))
-		freq++;
-
-	freq += (u32) n << 13;
-	/* Frequency OMEGA_IQIK_M_CAL_MID*/
-	temp_f = freq + (u32)iqik_m_cal;
-
-	set_tuner[3].reg[0] =  temp_f & 0xff;
-	set_tuner[4].reg[0] =  (temp_f >> 8) & 0xff;
-
-	dev_dbg(&state->i2c_adap->dev, "%s: High Frequency = %04x\n",
-			__func__, temp_f);
-
-	/* Lower frequency */
-	set_tuner[5].reg[0] =  freq & 0xff;
-	set_tuner[6].reg[0] =  (freq >> 8) & 0xff;
-
-	dev_dbg(&state->i2c_adap->dev, "%s: low Frequency = %04x\n",
-			__func__, freq);
-
-	ret = it913x_script_loader(state, set_tuner);
-
-	return (ret < 0) ? -ENODEV : 0;
-}
-
-/* Power sequence */
-/* Power Up	Tuner on -> Frontend suspend off -> Tuner clk on */
-/* Power Down	Frontend suspend on -> Tuner clk off -> Tuner off */
-
-static int it913x_sleep(struct dvb_frontend *fe)
-{
-	struct it913x_state *state = fe->tuner_priv;
-	return it913x_script_loader(state, it9137_tuner_off);
-}
-
-static int it913x_release(struct dvb_frontend *fe)
-{
-	kfree(fe->tuner_priv);
-	return 0;
-}
-
-static const struct dvb_tuner_ops it913x_tuner_ops = {
-	.info = {
-		.name           = "ITE Tech IT913X",
-		.frequency_min  = 174000000,
-		.frequency_max  = 862000000,
-	},
-
-	.release = it913x_release,
-
-	.init = it913x_init,
-	.sleep = it913x_sleep,
-	.set_params = it9137_set_params,
-};
-
-struct dvb_frontend *it913x_attach(struct dvb_frontend *fe,
-		struct i2c_adapter *i2c_adap, u8 i2c_addr, u8 config)
-{
-	struct it913x_state *state = NULL;
-
-	/* allocate memory for the internal state */
-	state = kzalloc(sizeof(struct it913x_state), GFP_KERNEL);
-	if (state == NULL)
-		return NULL;
-
-	state->i2c_adap = i2c_adap;
-	state->i2c_addr = i2c_addr;
-
-	switch (config) {
-	case AF9033_TUNER_IT9135_38:
-	case AF9033_TUNER_IT9135_51:
-	case AF9033_TUNER_IT9135_52:
-		state->chip_ver = 0x01;
-		break;
-	case AF9033_TUNER_IT9135_60:
-	case AF9033_TUNER_IT9135_61:
-	case AF9033_TUNER_IT9135_62:
-		state->chip_ver = 0x02;
-		break;
-	default:
-		dev_dbg(&i2c_adap->dev,
-				"%s: invalid config=%02x\n", __func__, config);
-		goto error;
-	}
-
-	state->tuner_type = config;
-	state->firmware_ver = 1;
-
-	fe->tuner_priv = state;
-	memcpy(&fe->ops.tuner_ops, &it913x_tuner_ops,
-			sizeof(struct dvb_tuner_ops));
-
-	dev_info(&i2c_adap->dev,
-			"%s: ITE Tech IT913X successfully attached\n",
-			KBUILD_MODNAME);
-	dev_dbg(&i2c_adap->dev, "%s: config=%02x chip_ver=%02x\n",
-			__func__, config, state->chip_ver);
-
-	return fe;
-error:
-	kfree(state);
-	return NULL;
-}
-EXPORT_SYMBOL(it913x_attach);
-
-MODULE_DESCRIPTION("ITE Tech IT913X silicon tuner driver");
-MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
-MODULE_LICENSE("GPL");
diff --git a/drivers/media/tuners/tuner_it913x_priv.h b/drivers/media/tuners/tuner_it913x_priv.h
deleted file mode 100644
index ce652108aa5d..000000000000
--- a/drivers/media/tuners/tuner_it913x_priv.h
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * ITE Tech IT9137 silicon tuner driver
- *
- *  Copyright (C) 2011 Malcolm Priestley (tvboxspy@gmail.com)
- *  IT9137 Copyright (C) ITE Tech Inc.
- *
- *  This program is free software; you can redistribute it and/or modify
- *  it under the terms of the GNU General Public License as published by
- *  the Free Software Foundation; either version 2 of the License, or
- *  (at your option) any later version.
- *
- *  This program is distributed in the hope that it will be useful,
- *  but WITHOUT ANY WARRANTY; without even the implied warranty of
- *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *
- *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; if not, write to the Free Software
- *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.=
- */
-
-#ifndef IT913X_PRIV_H
-#define IT913X_PRIV_H
-
-#include "tuner_it913x.h"
-#include "af9033.h"
-
-#define PRO_LINK		0x0
-#define PRO_DMOD		0x1
-#define TRIGGER_OFSM		0x0000
-
-struct it913xset {	u32 pro;
-			u32 address;
-			u8 reg[15];
-			u8 count;
-};
-
-/* Tuner setting scripts (still keeping it9137) */
-static struct it913xset it9137_tuner_off[] = {
-	{PRO_DMOD, 0xfba8, {0x01}, 0x01}, /* Tuner Clock Off  */
-	{PRO_DMOD, 0xec40, {0x00}, 0x01}, /* Power Down Tuner */
-	{PRO_DMOD, 0xec02, {0x3f, 0x1f, 0x3f, 0x3f}, 0x04},
-	{PRO_DMOD, 0xec06, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-				0x00, 0x00, 0x00, 0x00}, 0x0c},
-	{PRO_DMOD, 0xec12, {0x00, 0x00, 0x00, 0x00}, 0x04},
-	{PRO_DMOD, 0xec17, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-				0x00}, 0x09},
-	{PRO_DMOD, 0xec22, {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
-				0x00, 0x00}, 0x0a},
-	{PRO_DMOD, 0xec20, {0x00}, 0x01},
-	{PRO_DMOD, 0xec3f, {0x01}, 0x01},
-	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
-};
-
-static struct it913xset set_it9135_template[] = {
-	{PRO_DMOD, 0xee06, {0x00}, 0x01},
-	{PRO_DMOD, 0xec56, {0x00}, 0x01},
-	{PRO_DMOD, 0xec4c, {0x00}, 0x01},
-	{PRO_DMOD, 0xec4d, {0x00}, 0x01},
-	{PRO_DMOD, 0xec4e, {0x00}, 0x01},
-	{PRO_DMOD, 0x011e, {0x00}, 0x01}, /* Older Devices */
-	{PRO_DMOD, 0x011f, {0x00}, 0x01},
-	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
-};
-
-static struct it913xset set_it9137_template[] = {
-	{PRO_DMOD, 0xee06, {0x00}, 0x01},
-	{PRO_DMOD, 0xec56, {0x00}, 0x01},
-	{PRO_DMOD, 0xec4c, {0x00}, 0x01},
-	{PRO_DMOD, 0xec4d, {0x00}, 0x01},
-	{PRO_DMOD, 0xec4e, {0x00}, 0x01},
-	{PRO_DMOD, 0xec4f, {0x00}, 0x01},
-	{PRO_DMOD, 0xec50, {0x00}, 0x01},
-	{0xff, 0x0000, {0x00}, 0x00}, /* Terminating Entry */
-};
-
-#endif
diff --git a/drivers/media/tuners/xc4000.c b/drivers/media/tuners/xc4000.c
index f9ab79e3432d..219ebafae70f 100644
--- a/drivers/media/tuners/xc4000.c
+++ b/drivers/media/tuners/xc4000.c
@@ -569,67 +569,67 @@ static int xc4000_readreg(struct xc4000_priv *priv, u16 reg, u16 *val)
 #define dump_firm_type(t)	dump_firm_type_and_int_freq(t, 0)
 static void dump_firm_type_and_int_freq(unsigned int type, u16 int_freq)
 {
-	 if (type & BASE)
+	if (type & BASE)
 		printk(KERN_CONT "BASE ");
-	 if (type & INIT1)
+	if (type & INIT1)
 		printk(KERN_CONT "INIT1 ");
-	 if (type & F8MHZ)
+	if (type & F8MHZ)
 		printk(KERN_CONT "F8MHZ ");
-	 if (type & MTS)
+	if (type & MTS)
 		printk(KERN_CONT "MTS ");
-	 if (type & D2620)
+	if (type & D2620)
 		printk(KERN_CONT "D2620 ");
-	 if (type & D2633)
+	if (type & D2633)
 		printk(KERN_CONT "D2633 ");
-	 if (type & DTV6)
+	if (type & DTV6)
 		printk(KERN_CONT "DTV6 ");
-	 if (type & QAM)
+	if (type & QAM)
 		printk(KERN_CONT "QAM ");
-	 if (type & DTV7)
+	if (type & DTV7)
 		printk(KERN_CONT "DTV7 ");
-	 if (type & DTV78)
+	if (type & DTV78)
 		printk(KERN_CONT "DTV78 ");
-	 if (type & DTV8)
+	if (type & DTV8)
 		printk(KERN_CONT "DTV8 ");
-	 if (type & FM)
+	if (type & FM)
 		printk(KERN_CONT "FM ");
-	 if (type & INPUT1)
+	if (type & INPUT1)
 		printk(KERN_CONT "INPUT1 ");
-	 if (type & LCD)
+	if (type & LCD)
 		printk(KERN_CONT "LCD ");
-	 if (type & NOGD)
+	if (type & NOGD)
 		printk(KERN_CONT "NOGD ");
-	 if (type & MONO)
+	if (type & MONO)
 		printk(KERN_CONT "MONO ");
-	 if (type & ATSC)
+	if (type & ATSC)
 		printk(KERN_CONT "ATSC ");
-	 if (type & IF)
+	if (type & IF)
 		printk(KERN_CONT "IF ");
-	 if (type & LG60)
+	if (type & LG60)
 		printk(KERN_CONT "LG60 ");
-	 if (type & ATI638)
+	if (type & ATI638)
 		printk(KERN_CONT "ATI638 ");
-	 if (type & OREN538)
+	if (type & OREN538)
 		printk(KERN_CONT "OREN538 ");
-	 if (type & OREN36)
+	if (type & OREN36)
 		printk(KERN_CONT "OREN36 ");
-	 if (type & TOYOTA388)
+	if (type & TOYOTA388)
 		printk(KERN_CONT "TOYOTA388 ");
-	 if (type & TOYOTA794)
+	if (type & TOYOTA794)
 		printk(KERN_CONT "TOYOTA794 ");
-	 if (type & DIBCOM52)
+	if (type & DIBCOM52)
 		printk(KERN_CONT "DIBCOM52 ");
-	 if (type & ZARLINK456)
+	if (type & ZARLINK456)
 		printk(KERN_CONT "ZARLINK456 ");
-	 if (type & CHINA)
+	if (type & CHINA)
 		printk(KERN_CONT "CHINA ");
-	 if (type & F6MHZ)
+	if (type & F6MHZ)
 		printk(KERN_CONT "F6MHZ ");
-	 if (type & INPUT2)
+	if (type & INPUT2)
 		printk(KERN_CONT "INPUT2 ");
-	 if (type & SCODE)
+	if (type & SCODE)
 		printk(KERN_CONT "SCODE ");
-	 if (type & HAS_IF)
+	if (type & HAS_IF)
 		printk(KERN_CONT "HAS_IF_%d ", int_freq);
 }
 
diff --git a/drivers/media/tuners/xc5000.c b/drivers/media/tuners/xc5000.c
index e135760f7d48..e44c8aba6074 100644
--- a/drivers/media/tuners/xc5000.c
+++ b/drivers/media/tuners/xc5000.c
@@ -59,6 +59,7 @@ struct xc5000_priv {
 	u32 freq_hz, freq_offset;
 	u32 bandwidth;
 	u8  video_standard;
+	unsigned int mode;
 	u8  rf_mode;
 	u8  radio_input;
 
@@ -69,6 +70,8 @@ struct xc5000_priv {
 
 	struct dvb_frontend *fe;
 	struct delayed_work timer_sleep;
+
+	const struct firmware   *firmware;
 };
 
 /* Misc Defines */
@@ -712,9 +715,50 @@ static void xc_debug_dump(struct xc5000_priv *priv)
 	}
 }
 
-static int xc5000_set_params(struct dvb_frontend *fe)
+static int xc5000_tune_digital(struct dvb_frontend *fe)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+	int ret;
+	u32 bw = fe->dtv_property_cache.bandwidth_hz;
+
+	ret = xc_set_signal_source(priv, priv->rf_mode);
+	if (ret != 0) {
+		printk(KERN_ERR
+			"xc5000: xc_set_signal_source(%d) failed\n",
+			priv->rf_mode);
+		return -EREMOTEIO;
+	}
+
+	ret = xc_set_tv_standard(priv,
+		xc5000_standard[priv->video_standard].video_mode,
+		xc5000_standard[priv->video_standard].audio_mode, 0);
+	if (ret != 0) {
+		printk(KERN_ERR "xc5000: xc_set_tv_standard failed\n");
+		return -EREMOTEIO;
+	}
+
+	ret = xc_set_IF_frequency(priv, priv->if_khz);
+	if (ret != 0) {
+		printk(KERN_ERR "xc5000: xc_Set_IF_frequency(%d) failed\n",
+		       priv->if_khz);
+		return -EIO;
+	}
+
+	xc_write_reg(priv, XREG_OUTPUT_AMP, 0x8a);
+
+	xc_tune_channel(priv, priv->freq_hz, XC_TUNE_DIGITAL);
+
+	if (debug)
+		xc_debug_dump(priv);
+
+	priv->bandwidth = bw;
+
+	return 0;
+}
+
+static int xc5000_set_digital_params(struct dvb_frontend *fe)
 {
-	int ret, b;
+	int b;
 	struct xc5000_priv *priv = fe->tuner_priv;
 	u32 bw = fe->dtv_property_cache.bandwidth_hz;
 	u32 freq = fe->dtv_property_cache.frequency;
@@ -794,43 +838,12 @@ static int xc5000_set_params(struct dvb_frontend *fe)
 	}
 
 	priv->freq_hz = freq - priv->freq_offset;
+	priv->mode = V4L2_TUNER_DIGITAL_TV;
 
 	dprintk(1, "%s() frequency=%d (compensated to %d)\n",
 		__func__, freq, priv->freq_hz);
 
-	ret = xc_set_signal_source(priv, priv->rf_mode);
-	if (ret != 0) {
-		printk(KERN_ERR
-			"xc5000: xc_set_signal_source(%d) failed\n",
-			priv->rf_mode);
-		return -EREMOTEIO;
-	}
-
-	ret = xc_set_tv_standard(priv,
-		xc5000_standard[priv->video_standard].video_mode,
-		xc5000_standard[priv->video_standard].audio_mode, 0);
-	if (ret != 0) {
-		printk(KERN_ERR "xc5000: xc_set_tv_standard failed\n");
-		return -EREMOTEIO;
-	}
-
-	ret = xc_set_IF_frequency(priv, priv->if_khz);
-	if (ret != 0) {
-		printk(KERN_ERR "xc5000: xc_Set_IF_frequency(%d) failed\n",
-		       priv->if_khz);
-		return -EIO;
-	}
-
-	xc_write_reg(priv, XREG_OUTPUT_AMP, 0x8a);
-
-	xc_tune_channel(priv, priv->freq_hz, XC_TUNE_DIGITAL);
-
-	if (debug)
-		xc_debug_dump(priv);
-
-	priv->bandwidth = bw;
-
-	return 0;
+	return xc5000_tune_digital(fe);
 }
 
 static int xc5000_is_firmware_loaded(struct dvb_frontend *fe)
@@ -852,12 +865,10 @@ static int xc5000_is_firmware_loaded(struct dvb_frontend *fe)
 	return ret;
 }
 
-static int xc5000_set_tv_freq(struct dvb_frontend *fe,
-	struct analog_parameters *params)
+static void xc5000_config_tv(struct dvb_frontend *fe,
+			     struct analog_parameters *params)
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
-	u16 pll_lock_status;
-	int ret;
 
 	dprintk(1, "%s() frequency=%d (in units of 62.5khz)\n",
 		__func__, params->frequency);
@@ -876,42 +887,49 @@ static int xc5000_set_tv_freq(struct dvb_frontend *fe,
 	if (params->std & V4L2_STD_MN) {
 		/* default to BTSC audio standard */
 		priv->video_standard = MN_NTSC_PAL_BTSC;
-		goto tune_channel;
+		return;
 	}
 
 	if (params->std & V4L2_STD_PAL_BG) {
 		/* default to NICAM audio standard */
 		priv->video_standard = BG_PAL_NICAM;
-		goto tune_channel;
+		return;
 	}
 
 	if (params->std & V4L2_STD_PAL_I) {
 		/* default to NICAM audio standard */
 		priv->video_standard = I_PAL_NICAM;
-		goto tune_channel;
+		return;
 	}
 
 	if (params->std & V4L2_STD_PAL_DK) {
 		/* default to NICAM audio standard */
 		priv->video_standard = DK_PAL_NICAM;
-		goto tune_channel;
+		return;
 	}
 
 	if (params->std & V4L2_STD_SECAM_DK) {
 		/* default to A2 DK1 audio standard */
 		priv->video_standard = DK_SECAM_A2DK1;
-		goto tune_channel;
+		return;
 	}
 
 	if (params->std & V4L2_STD_SECAM_L) {
 		priv->video_standard = L_SECAM_NICAM;
-		goto tune_channel;
+		return;
 	}
 
 	if (params->std & V4L2_STD_SECAM_LC) {
 		priv->video_standard = LC_SECAM_NICAM;
-		goto tune_channel;
+		return;
 	}
+}
+
+static int xc5000_set_tv_freq(struct dvb_frontend *fe)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+	u16 pll_lock_status;
+	int ret;
 
 tune_channel:
 	ret = xc_set_signal_source(priv, priv->rf_mode);
@@ -955,12 +973,11 @@ tune_channel:
 	return 0;
 }
 
-static int xc5000_set_radio_freq(struct dvb_frontend *fe,
-	struct analog_parameters *params)
+static int xc5000_config_radio(struct dvb_frontend *fe,
+			       struct analog_parameters *params)
+
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
-	int ret = -EINVAL;
-	u8 radio_input;
 
 	dprintk(1, "%s() frequency=%d (in units of khz)\n",
 		__func__, params->frequency);
@@ -970,6 +987,18 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 		return -EINVAL;
 	}
 
+	priv->freq_hz = params->frequency * 125 / 2;
+	priv->rf_mode = XC_RF_MODE_AIR;
+
+	return 0;
+}
+
+static int xc5000_set_radio_freq(struct dvb_frontend *fe)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+	int ret;
+	u8 radio_input;
+
 	if (priv->radio_input == XC5000_RADIO_FM1)
 		radio_input = FM_RADIO_INPUT1;
 	else if  (priv->radio_input == XC5000_RADIO_FM2)
@@ -982,10 +1011,6 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 		return -EINVAL;
 	}
 
-	priv->freq_hz = params->frequency * 125 / 2;
-
-	priv->rf_mode = XC_RF_MODE_AIR;
-
 	ret = xc_set_tv_standard(priv, xc5000_standard[radio_input].video_mode,
 			       xc5000_standard[radio_input].audio_mode, radio_input);
 
@@ -1013,34 +1038,53 @@ static int xc5000_set_radio_freq(struct dvb_frontend *fe,
 	return 0;
 }
 
-static int xc5000_set_analog_params(struct dvb_frontend *fe,
-			     struct analog_parameters *params)
+static int xc5000_set_params(struct dvb_frontend *fe)
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
-	int ret = -EINVAL;
-
-	if (priv->i2c_props.adap == NULL)
-		return -EINVAL;
 
 	if (xc_load_fw_and_init_tuner(fe, 0) != 0) {
 		dprintk(1, "Unable to load firmware and init tuner\n");
 		return -EINVAL;
 	}
 
+	switch (priv->mode) {
+	case V4L2_TUNER_RADIO:
+		return xc5000_set_radio_freq(fe);
+	case V4L2_TUNER_ANALOG_TV:
+		return xc5000_set_tv_freq(fe);
+	case V4L2_TUNER_DIGITAL_TV:
+		return xc5000_tune_digital(fe);
+	}
+
+	return 0;
+}
+
+static int xc5000_set_analog_params(struct dvb_frontend *fe,
+			     struct analog_parameters *params)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+	int ret;
+
+	if (priv->i2c_props.adap == NULL)
+		return -EINVAL;
+
 	switch (params->mode) {
 	case V4L2_TUNER_RADIO:
-		ret = xc5000_set_radio_freq(fe, params);
+		ret = xc5000_config_radio(fe, params);
+		if (ret)
+			return ret;
 		break;
 	case V4L2_TUNER_ANALOG_TV:
-	case V4L2_TUNER_DIGITAL_TV:
-		ret = xc5000_set_tv_freq(fe, params);
+		xc5000_config_tv(fe, params);
+		break;
+	default:
 		break;
 	}
+	priv->mode = params->mode;
 
-	return ret;
+	return xc5000_set_params(fe);
 }
 
-
 static int xc5000_get_frequency(struct dvb_frontend *fe, u32 *freq)
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
@@ -1094,20 +1138,23 @@ static int xc_load_fw_and_init_tuner(struct dvb_frontend *fe, int force)
 	if (!force && xc5000_is_firmware_loaded(fe) == 0)
 		return 0;
 
-	ret = request_firmware(&fw, desired_fw->name,
-			       priv->i2c_props.adap->dev.parent);
-	if (ret) {
-		printk(KERN_ERR "xc5000: Upload failed. (file not found?)\n");
-		return ret;
-	}
-
-	dprintk(1, "firmware read %Zu bytes.\n", fw->size);
+	if (!priv->firmware) {
+		ret = request_firmware(&fw, desired_fw->name,
+					priv->i2c_props.adap->dev.parent);
+		if (ret) {
+			pr_err("xc5000: Upload failed. rc %d\n", ret);
+			return ret;
+		}
+		dprintk(1, "firmware read %Zu bytes.\n", fw->size);
 
-	if (fw->size != desired_fw->size) {
-		printk(KERN_ERR "xc5000: Firmware file with incorrect size\n");
-		ret = -EINVAL;
-		goto err;
-	}
+		if (fw->size != desired_fw->size) {
+			pr_err("xc5000: Firmware file with incorrect size\n");
+			release_firmware(fw);
+			return -EINVAL;
+		}
+		priv->firmware = fw;
+	} else
+		fw = priv->firmware;
 
 	/* Try up to 5 times to load firmware */
 	for (i = 0; i < 5; i++) {
@@ -1190,7 +1237,6 @@ err:
 	else
 		printk(KERN_CONT " - too many retries. Giving up\n");
 
-	release_firmware(fw);
 	return ret;
 }
 
@@ -1229,6 +1275,38 @@ static int xc5000_sleep(struct dvb_frontend *fe)
 	return 0;
 }
 
+static int xc5000_suspend(struct dvb_frontend *fe)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+	int ret;
+
+	dprintk(1, "%s()\n", __func__);
+
+	cancel_delayed_work(&priv->timer_sleep);
+
+	ret = xc5000_tuner_reset(fe);
+	if (ret != 0)
+		printk(KERN_ERR
+			"xc5000: %s() unable to shutdown tuner\n",
+			__func__);
+
+	return 0;
+}
+
+static int xc5000_resume(struct dvb_frontend *fe)
+{
+	struct xc5000_priv *priv = fe->tuner_priv;
+
+	dprintk(1, "%s()\n", __func__);
+
+	/* suspended before firmware is loaded.
+	   Avoid firmware load in resume path. */
+	if (!priv->firmware)
+		return 0;
+
+	return xc5000_set_params(fe);
+}
+
 static int xc5000_init(struct dvb_frontend *fe)
 {
 	struct xc5000_priv *priv = fe->tuner_priv;
@@ -1256,6 +1334,8 @@ static int xc5000_release(struct dvb_frontend *fe)
 	if (priv) {
 		cancel_delayed_work(&priv->timer_sleep);
 		hybrid_tuner_release_state(priv);
+		if (priv->firmware)
+			release_firmware(priv->firmware);
 	}
 
 	mutex_unlock(&xc5000_list_mutex);
@@ -1293,9 +1373,11 @@ static const struct dvb_tuner_ops xc5000_tuner_ops = {
 	.release	   = xc5000_release,
 	.init		   = xc5000_init,
 	.sleep		   = xc5000_sleep,
+	.suspend	   = xc5000_suspend,
+	.resume		   = xc5000_resume,
 
 	.set_config	   = xc5000_set_config,
-	.set_params	   = xc5000_set_params,
+	.set_params	   = xc5000_set_digital_params,
 	.set_analog_params = xc5000_set_analog_params,
 	.get_frequency	   = xc5000_get_frequency,
 	.get_if_frequency  = xc5000_get_if_frequency,
diff --git a/drivers/media/usb/Kconfig b/drivers/media/usb/Kconfig
index 94d51e092db3..056181f2f569 100644
--- a/drivers/media/usb/Kconfig
+++ b/drivers/media/usb/Kconfig
@@ -46,6 +46,7 @@ source "drivers/media/usb/ttusb-budget/Kconfig"
 source "drivers/media/usb/ttusb-dec/Kconfig"
 source "drivers/media/usb/siano/Kconfig"
 source "drivers/media/usb/b2c2/Kconfig"
+source "drivers/media/usb/as102/Kconfig"
 endif
 
 if (MEDIA_CAMERA_SUPPORT || MEDIA_ANALOG_TV_SUPPORT || MEDIA_DIGITAL_TV_SUPPORT)
@@ -55,8 +56,9 @@ endif
 
 if MEDIA_SDR_SUPPORT
 	comment "Software defined radio USB devices"
-source "drivers/media/usb/msi2500/Kconfig"
 source "drivers/media/usb/airspy/Kconfig"
+source "drivers/media/usb/hackrf/Kconfig"
+source "drivers/media/usb/msi2500/Kconfig"
 endif
 
 endif #MEDIA_USB_SUPPORT
diff --git a/drivers/media/usb/Makefile b/drivers/media/usb/Makefile
index f438efffefc5..6f2eb7c8416c 100644
--- a/drivers/media/usb/Makefile
+++ b/drivers/media/usb/Makefile
@@ -9,8 +9,9 @@ obj-y += zr364xx/ stkwebcam/ s2255/
 obj-$(CONFIG_USB_VIDEO_CLASS)	+= uvc/
 obj-$(CONFIG_USB_GSPCA)         += gspca/
 obj-$(CONFIG_USB_PWC)           += pwc/
-obj-$(CONFIG_USB_MSI2500)       += msi2500/
 obj-$(CONFIG_USB_AIRSPY)        += airspy/
+obj-$(CONFIG_USB_HACKRF)        += hackrf/
+obj-$(CONFIG_USB_MSI2500)       += msi2500/
 obj-$(CONFIG_VIDEO_CPIA2) += cpia2/
 obj-$(CONFIG_VIDEO_AU0828) += au0828/
 obj-$(CONFIG_VIDEO_HDPVR)	+= hdpvr/
@@ -23,3 +24,4 @@ obj-$(CONFIG_VIDEO_TM6000) += tm6000/
 obj-$(CONFIG_VIDEO_EM28XX) += em28xx/
 obj-$(CONFIG_VIDEO_USBTV) += usbtv/
 obj-$(CONFIG_VIDEO_GO7007) += go7007/
+obj-$(CONFIG_DVB_AS102) += as102/
diff --git a/drivers/media/usb/airspy/airspy.c b/drivers/media/usb/airspy/airspy.c
index cb0e515d80ae..4069234abed5 100644
--- a/drivers/media/usb/airspy/airspy.c
+++ b/drivers/media/usb/airspy/airspy.c
@@ -107,6 +107,7 @@ struct airspy {
 #define USB_STATE_URB_BUF  (1 << 3)
 	unsigned long flags;
 
+	struct device *dev;
 	struct usb_device *udev;
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
@@ -154,16 +155,15 @@ struct airspy {
 	unsigned int sample_measured;
 };
 
-#define airspy_dbg_usb_control_msg(_udev, _r, _t, _v, _i, _b, _l) { \
+#define airspy_dbg_usb_control_msg(_dev, _r, _t, _v, _i, _b, _l) { \
 	char *_direction; \
 	if (_t & USB_DIR_IN) \
 		_direction = "<<<"; \
 	else \
 		_direction = ">>>"; \
-	dev_dbg(&_udev->dev, "%s: %02x %02x %02x %02x %02x %02x %02x %02x " \
-			"%s %*ph\n",  __func__, _t, _r, _v & 0xff, _v >> 8, \
-			_i & 0xff, _i >> 8, _l & 0xff, _l >> 8, _direction, \
-			_l, _b); \
+	dev_dbg(_dev, "%02x %02x %02x %02x %02x %02x %02x %02x %s %*ph\n", \
+			_t, _r, _v & 0xff, _v >> 8, _i & 0xff, _i >> 8, \
+			_l & 0xff, _l >> 8, _direction, _l, _b); \
 }
 
 /* execute firmware command */
@@ -192,7 +192,7 @@ static int airspy_ctrl_msg(struct airspy *s, u8 request, u16 value, u16 index,
 		requesttype = (USB_TYPE_VENDOR | USB_DIR_IN);
 		break;
 	default:
-		dev_err(&s->udev->dev, "Unknown command %02x\n", request);
+		dev_err(s->dev, "Unknown command %02x\n", request);
 		ret = -EINVAL;
 		goto err;
 	}
@@ -203,11 +203,10 @@ static int airspy_ctrl_msg(struct airspy *s, u8 request, u16 value, u16 index,
 
 	ret = usb_control_msg(s->udev, pipe, request, requesttype, value,
 			index, s->buf, size, 1000);
-	airspy_dbg_usb_control_msg(s->udev, request, requesttype, value,
+	airspy_dbg_usb_control_msg(s->dev, request, requesttype, value,
 			index, s->buf, size);
 	if (ret < 0) {
-		dev_err(&s->udev->dev,
-				"usb_control_msg() failed %d request %02x\n",
+		dev_err(s->dev, "usb_control_msg() failed %d request %02x\n",
 				ret, request);
 		goto err;
 	}
@@ -224,7 +223,7 @@ err:
 /* Private functions */
 static struct airspy_frame_buf *airspy_get_next_fill_buf(struct airspy *s)
 {
-	unsigned long flags = 0;
+	unsigned long flags;
 	struct airspy_frame_buf *buf = NULL;
 
 	spin_lock_irqsave(&s->queued_bufs_lock, flags);
@@ -251,16 +250,18 @@ static unsigned int airspy_convert_stream(struct airspy *s,
 		dst_len = 0;
 	}
 
-	/* calculate samping rate and output it in 10 seconds intervals */
+	/* calculate sample rate and output it in 10 seconds intervals */
 	if (unlikely(time_is_before_jiffies(s->jiffies_next))) {
 		#define MSECS 10000UL
+		unsigned int msecs = jiffies_to_msecs(jiffies -
+				s->jiffies_next + msecs_to_jiffies(MSECS));
 		unsigned int samples = s->sample - s->sample_measured;
+
 		s->jiffies_next = jiffies + msecs_to_jiffies(MSECS);
 		s->sample_measured = s->sample;
-		dev_dbg(&s->udev->dev,
-				"slen=%d samples=%u msecs=%lu sample rate=%lu\n",
-				src_len, samples, MSECS,
-				samples * 1000UL / MSECS);
+		dev_dbg(s->dev, "slen=%u samples=%u msecs=%u sample rate=%lu\n",
+				src_len, samples, msecs,
+				samples * 1000UL / msecs);
 	}
 
 	/* total number of samples */
@@ -278,9 +279,8 @@ static void airspy_urb_complete(struct urb *urb)
 	struct airspy *s = urb->context;
 	struct airspy_frame_buf *fbuf;
 
-	dev_dbg_ratelimited(&s->udev->dev,
-			"%s: status=%d length=%d/%d errors=%d\n",
-			__func__, urb->status, urb->actual_length,
+	dev_dbg_ratelimited(s->dev, "status=%d length=%d/%d errors=%d\n",
+			urb->status, urb->actual_length,
 			urb->transfer_buffer_length, urb->error_count);
 
 	switch (urb->status) {
@@ -292,8 +292,7 @@ static void airspy_urb_complete(struct urb *urb)
 	case -ESHUTDOWN:
 		return;
 	default:            /* error */
-		dev_err_ratelimited(&s->udev->dev, "URB failed %d\n",
-				urb->status);
+		dev_err_ratelimited(s->dev, "URB failed %d\n", urb->status);
 		break;
 	}
 
@@ -304,7 +303,7 @@ static void airspy_urb_complete(struct urb *urb)
 		fbuf = airspy_get_next_fill_buf(s);
 		if (unlikely(fbuf == NULL)) {
 			s->vb_full++;
-			dev_notice_ratelimited(&s->udev->dev,
+			dev_notice_ratelimited(s->dev,
 					"videobuf is full, %d packets dropped\n",
 					s->vb_full);
 			goto skip;
@@ -328,7 +327,7 @@ static int airspy_kill_urbs(struct airspy *s)
 	int i;
 
 	for (i = s->urbs_submitted - 1; i >= 0; i--) {
-		dev_dbg(&s->udev->dev, "%s: kill urb=%d\n", __func__, i);
+		dev_dbg(s->dev, "kill urb=%d\n", i);
 		/* stop the URB */
 		usb_kill_urb(s->urb_list[i]);
 	}
@@ -342,11 +341,10 @@ static int airspy_submit_urbs(struct airspy *s)
 	int i, ret;
 
 	for (i = 0; i < s->urbs_initialized; i++) {
-		dev_dbg(&s->udev->dev, "%s: submit urb=%d\n", __func__, i);
+		dev_dbg(s->dev, "submit urb=%d\n", i);
 		ret = usb_submit_urb(s->urb_list[i], GFP_ATOMIC);
 		if (ret) {
-			dev_err(&s->udev->dev,
-					"Could not submit URB no. %d - get them all back\n",
+			dev_err(s->dev, "Could not submit URB no. %d - get them all back\n",
 					i);
 			airspy_kill_urbs(s);
 			return ret;
@@ -362,8 +360,7 @@ static int airspy_free_stream_bufs(struct airspy *s)
 	if (s->flags & USB_STATE_URB_BUF) {
 		while (s->buf_num) {
 			s->buf_num--;
-			dev_dbg(&s->udev->dev, "%s: free buf=%d\n",
-					__func__, s->buf_num);
+			dev_dbg(s->dev, "free buf=%d\n", s->buf_num);
 			usb_free_coherent(s->udev, s->buf_size,
 					  s->buf_list[s->buf_num],
 					  s->dma_addr[s->buf_num]);
@@ -379,23 +376,20 @@ static int airspy_alloc_stream_bufs(struct airspy *s)
 	s->buf_num = 0;
 	s->buf_size = BULK_BUFFER_SIZE;
 
-	dev_dbg(&s->udev->dev,
-			"%s: all in all I will use %u bytes for streaming\n",
-			__func__,  MAX_BULK_BUFS * BULK_BUFFER_SIZE);
+	dev_dbg(s->dev, "all in all I will use %u bytes for streaming\n",
+			MAX_BULK_BUFS * BULK_BUFFER_SIZE);
 
 	for (s->buf_num = 0; s->buf_num < MAX_BULK_BUFS; s->buf_num++) {
 		s->buf_list[s->buf_num] = usb_alloc_coherent(s->udev,
 				BULK_BUFFER_SIZE, GFP_ATOMIC,
 				&s->dma_addr[s->buf_num]);
 		if (!s->buf_list[s->buf_num]) {
-			dev_dbg(&s->udev->dev, "%s: alloc buf=%d failed\n",
-					__func__, s->buf_num);
+			dev_dbg(s->dev, "alloc buf=%d failed\n", s->buf_num);
 			airspy_free_stream_bufs(s);
 			return -ENOMEM;
 		}
 
-		dev_dbg(&s->udev->dev, "%s: alloc buf=%d %p (dma %llu)\n",
-				__func__, s->buf_num,
+		dev_dbg(s->dev, "alloc buf=%d %p (dma %llu)\n", s->buf_num,
 				s->buf_list[s->buf_num],
 				(long long)s->dma_addr[s->buf_num]);
 		s->flags |= USB_STATE_URB_BUF;
@@ -412,8 +406,7 @@ static int airspy_free_urbs(struct airspy *s)
 
 	for (i = s->urbs_initialized - 1; i >= 0; i--) {
 		if (s->urb_list[i]) {
-			dev_dbg(&s->udev->dev, "%s: free urb=%d\n",
-					__func__, i);
+			dev_dbg(s->dev, "free urb=%d\n", i);
 			/* free the URBs */
 			usb_free_urb(s->urb_list[i]);
 		}
@@ -429,10 +422,10 @@ static int airspy_alloc_urbs(struct airspy *s)
 
 	/* allocate the URBs */
 	for (i = 0; i < MAX_BULK_BUFS; i++) {
-		dev_dbg(&s->udev->dev, "%s: alloc urb=%d\n", __func__, i);
+		dev_dbg(s->dev, "alloc urb=%d\n", i);
 		s->urb_list[i] = usb_alloc_urb(0, GFP_ATOMIC);
 		if (!s->urb_list[i]) {
-			dev_dbg(&s->udev->dev, "%s: failed\n", __func__);
+			dev_dbg(s->dev, "failed\n");
 			for (j = 0; j < i; j++)
 				usb_free_urb(s->urb_list[j]);
 			return -ENOMEM;
@@ -455,13 +448,14 @@ static int airspy_alloc_urbs(struct airspy *s)
 /* Must be called with vb_queue_lock hold */
 static void airspy_cleanup_queued_bufs(struct airspy *s)
 {
-	unsigned long flags = 0;
+	unsigned long flags;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	spin_lock_irqsave(&s->queued_bufs_lock, flags);
 	while (!list_empty(&s->queued_bufs)) {
 		struct airspy_frame_buf *buf;
+
 		buf = list_entry(s->queued_bufs.next,
 				struct airspy_frame_buf, list);
 		list_del(&buf->list);
@@ -476,7 +470,7 @@ static void airspy_disconnect(struct usb_interface *intf)
 	struct v4l2_device *v = usb_get_intfdata(intf);
 	struct airspy *s = container_of(v, struct airspy, v4l2_dev);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	mutex_lock(&s->vb_queue_lock);
 	mutex_lock(&s->v4l2_lock);
@@ -497,7 +491,7 @@ static int airspy_queue_setup(struct vb2_queue *vq,
 {
 	struct airspy *s = vb2_get_drv_priv(vq);
 
-	dev_dbg(&s->udev->dev, "%s: *nbuffers=%d\n", __func__, *nbuffers);
+	dev_dbg(s->dev, "nbuffers=%d\n", *nbuffers);
 
 	/* Need at least 8 buffers */
 	if (vq->num_buffers + *nbuffers < 8)
@@ -505,8 +499,7 @@ static int airspy_queue_setup(struct vb2_queue *vq,
 	*nplanes = 1;
 	sizes[0] = PAGE_ALIGN(s->buffersize);
 
-	dev_dbg(&s->udev->dev, "%s: nbuffers=%d sizes[0]=%d\n",
-			__func__, *nbuffers, sizes[0]);
+	dev_dbg(s->dev, "nbuffers=%d sizes[0]=%d\n", *nbuffers, sizes[0]);
 	return 0;
 }
 
@@ -515,7 +508,7 @@ static void airspy_buf_queue(struct vb2_buffer *vb)
 	struct airspy *s = vb2_get_drv_priv(vb->vb2_queue);
 	struct airspy_frame_buf *buf =
 			container_of(vb, struct airspy_frame_buf, vb);
-	unsigned long flags = 0;
+	unsigned long flags;
 
 	/* Check the device has not disconnected between prep and queuing */
 	if (unlikely(!s->udev)) {
@@ -533,34 +526,56 @@ static int airspy_start_streaming(struct vb2_queue *vq, unsigned int count)
 	struct airspy *s = vb2_get_drv_priv(vq);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	if (!s->udev)
 		return -ENODEV;
 
 	mutex_lock(&s->v4l2_lock);
 
-	set_bit(POWER_ON, &s->flags);
-
 	s->sequence = 0;
 
+	set_bit(POWER_ON, &s->flags);
+
 	ret = airspy_alloc_stream_bufs(s);
 	if (ret)
-		goto err;
+		goto err_clear_bit;
 
 	ret = airspy_alloc_urbs(s);
 	if (ret)
-		goto err;
+		goto err_free_stream_bufs;
 
 	ret = airspy_submit_urbs(s);
 	if (ret)
-		goto err;
+		goto err_free_urbs;
 
 	/* start hardware streaming */
 	ret = airspy_ctrl_msg(s, CMD_RECEIVER_MODE, 1, 0, NULL, 0);
 	if (ret)
-		goto err;
-err:
+		goto err_kill_urbs;
+
+	goto exit_mutex_unlock;
+
+err_kill_urbs:
+	airspy_kill_urbs(s);
+err_free_urbs:
+	airspy_free_urbs(s);
+err_free_stream_bufs:
+	airspy_free_stream_bufs(s);
+err_clear_bit:
+	clear_bit(POWER_ON, &s->flags);
+
+	/* return all queued buffers to vb2 */
+	{
+		struct airspy_frame_buf *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &s->queued_bufs, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+	}
+
+exit_mutex_unlock:
 	mutex_unlock(&s->v4l2_lock);
 
 	return ret;
@@ -570,7 +585,7 @@ static void airspy_stop_streaming(struct vb2_queue *vq)
 {
 	struct airspy *s = vb2_get_drv_priv(vq);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	mutex_lock(&s->v4l2_lock);
 
@@ -602,8 +617,6 @@ static int airspy_querycap(struct file *file, void *fh,
 {
 	struct airspy *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
-
 	strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
 	strlcpy(cap->card, s->vdev.name, sizeof(cap->card));
 	usb_make_path(s->udev, cap->bus_info, sizeof(cap->bus_info));
@@ -617,10 +630,6 @@ static int airspy_querycap(struct file *file, void *fh,
 static int airspy_enum_fmt_sdr_cap(struct file *file, void *priv,
 		struct v4l2_fmtdesc *f)
 {
-	struct airspy *s = video_drvdata(file);
-
-	dev_dbg(&s->udev->dev, "%s: index=%d\n", __func__, f->index);
-
 	if (f->index >= NUM_FORMATS)
 		return -EINVAL;
 
@@ -635,9 +644,6 @@ static int airspy_g_fmt_sdr_cap(struct file *file, void *priv,
 {
 	struct airspy *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
-			(char *)&s->pixelformat);
-
 	f->fmt.sdr.pixelformat = s->pixelformat;
 	f->fmt.sdr.buffersize = s->buffersize;
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
@@ -652,9 +658,6 @@ static int airspy_s_fmt_sdr_cap(struct file *file, void *priv,
 	struct vb2_queue *q = &s->vb_queue;
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
-			(char *)&f->fmt.sdr.pixelformat);
-
 	if (vb2_is_busy(q))
 		return -EBUSY;
 
@@ -679,12 +682,8 @@ static int airspy_s_fmt_sdr_cap(struct file *file, void *priv,
 static int airspy_try_fmt_sdr_cap(struct file *file, void *priv,
 		struct v4l2_format *f)
 {
-	struct airspy *s = video_drvdata(file);
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
-			(char *)&f->fmt.sdr.pixelformat);
-
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
 	for (i = 0; i < NUM_FORMATS; i++) {
 		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
@@ -702,11 +701,8 @@ static int airspy_try_fmt_sdr_cap(struct file *file, void *priv,
 static int airspy_s_tuner(struct file *file, void *priv,
 		const struct v4l2_tuner *v)
 {
-	struct airspy *s = video_drvdata(file);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s: index=%d\n", __func__, v->index);
-
 	if (v->index == 0)
 		ret = 0;
 	else if (v->index == 1)
@@ -719,11 +715,8 @@ static int airspy_s_tuner(struct file *file, void *priv,
 
 static int airspy_g_tuner(struct file *file, void *priv, struct v4l2_tuner *v)
 {
-	struct airspy *s = video_drvdata(file);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s: index=%d\n", __func__, v->index);
-
 	if (v->index == 0) {
 		strlcpy(v->name, "AirSpy ADC", sizeof(v->name));
 		v->type = V4L2_TUNER_ADC;
@@ -749,17 +742,18 @@ static int airspy_g_frequency(struct file *file, void *priv,
 		struct v4l2_frequency *f)
 {
 	struct airspy *s = video_drvdata(file);
-	int ret  = 0;
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d\n",
-			__func__, f->tuner, f->type);
+	int ret;
 
 	if (f->tuner == 0) {
 		f->type = V4L2_TUNER_ADC;
 		f->frequency = s->f_adc;
+		dev_dbg(s->dev, "ADC frequency=%u Hz\n", s->f_adc);
 		ret = 0;
 	} else if (f->tuner == 1) {
 		f->type = V4L2_TUNER_RF;
 		f->frequency = s->f_rf;
+		dev_dbg(s->dev, "RF frequency=%u Hz\n", s->f_rf);
+		ret = 0;
 	} else {
 		ret = -EINVAL;
 	}
@@ -774,22 +768,17 @@ static int airspy_s_frequency(struct file *file, void *priv,
 	int ret;
 	u8 buf[4];
 
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d frequency=%u\n",
-			__func__, f->tuner, f->type, f->frequency);
-
 	if (f->tuner == 0) {
 		s->f_adc = clamp_t(unsigned int, f->frequency,
 				bands[0].rangelow,
 				bands[0].rangehigh);
-		dev_dbg(&s->udev->dev, "%s: ADC frequency=%u Hz\n",
-				__func__, s->f_adc);
+		dev_dbg(s->dev, "ADC frequency=%u Hz\n", s->f_adc);
 		ret = 0;
 	} else if (f->tuner == 1) {
 		s->f_rf = clamp_t(unsigned int, f->frequency,
 				bands_rf[0].rangelow,
 				bands_rf[0].rangehigh);
-		dev_dbg(&s->udev->dev, "%s: RF frequency=%u Hz\n",
-				__func__, s->f_rf);
+		dev_dbg(s->dev, "RF frequency=%u Hz\n", s->f_rf);
 		buf[0] = (s->f_rf >>  0) & 0xff;
 		buf[1] = (s->f_rf >>  8) & 0xff;
 		buf[2] = (s->f_rf >> 16) & 0xff;
@@ -805,10 +794,7 @@ static int airspy_s_frequency(struct file *file, void *priv,
 static int airspy_enum_freq_bands(struct file *file, void *priv,
 		struct v4l2_frequency_band *band)
 {
-	struct airspy *s = video_drvdata(file);
 	int ret;
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d index=%d\n",
-			__func__, band->tuner, band->type, band->index);
 
 	if (band->tuner == 0) {
 		if (band->index >= ARRAY_SIZE(bands)) {
@@ -892,10 +878,9 @@ static int airspy_set_lna_gain(struct airspy *s)
 	int ret;
 	u8 u8tmp;
 
-	dev_dbg(&s->udev->dev, "%s: lna auto=%d->%d val=%d->%d\n",
-			__func__, s->lna_gain_auto->cur.val,
-			s->lna_gain_auto->val, s->lna_gain->cur.val,
-			s->lna_gain->val);
+	dev_dbg(s->dev, "lna auto=%d->%d val=%d->%d\n",
+			s->lna_gain_auto->cur.val, s->lna_gain_auto->val,
+			s->lna_gain->cur.val, s->lna_gain->val);
 
 	ret = airspy_ctrl_msg(s, CMD_SET_LNA_AGC, 0, s->lna_gain_auto->val,
 			&u8tmp, 1);
@@ -910,7 +895,7 @@ static int airspy_set_lna_gain(struct airspy *s)
 	}
 err:
 	if (ret)
-		dev_dbg(&s->udev->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(s->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -920,10 +905,9 @@ static int airspy_set_mixer_gain(struct airspy *s)
 	int ret;
 	u8 u8tmp;
 
-	dev_dbg(&s->udev->dev, "%s: mixer auto=%d->%d val=%d->%d\n",
-			__func__, s->mixer_gain_auto->cur.val,
-			s->mixer_gain_auto->val, s->mixer_gain->cur.val,
-			s->mixer_gain->val);
+	dev_dbg(s->dev, "mixer auto=%d->%d val=%d->%d\n",
+			s->mixer_gain_auto->cur.val, s->mixer_gain_auto->val,
+			s->mixer_gain->cur.val, s->mixer_gain->val);
 
 	ret = airspy_ctrl_msg(s, CMD_SET_MIXER_AGC, 0, s->mixer_gain_auto->val,
 			&u8tmp, 1);
@@ -938,7 +922,7 @@ static int airspy_set_mixer_gain(struct airspy *s)
 	}
 err:
 	if (ret)
-		dev_dbg(&s->udev->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(s->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -948,8 +932,7 @@ static int airspy_set_if_gain(struct airspy *s)
 	int ret;
 	u8 u8tmp;
 
-	dev_dbg(&s->udev->dev, "%s: val=%d->%d\n",
-			__func__, s->if_gain->cur.val, s->if_gain->val);
+	dev_dbg(s->dev, "val=%d->%d\n", s->if_gain->cur.val, s->if_gain->val);
 
 	ret = airspy_ctrl_msg(s, CMD_SET_VGA_GAIN, 0, s->if_gain->val,
 			&u8tmp, 1);
@@ -957,7 +940,7 @@ static int airspy_set_if_gain(struct airspy *s)
 		goto err;
 err:
 	if (ret)
-		dev_dbg(&s->udev->dev, "%s: failed=%d\n", __func__, ret);
+		dev_dbg(s->dev, "failed=%d\n", ret);
 
 	return ret;
 }
@@ -980,8 +963,8 @@ static int airspy_s_ctrl(struct v4l2_ctrl *ctrl)
 		ret = airspy_set_if_gain(s);
 		break;
 	default:
-		dev_dbg(&s->udev->dev, "%s: unknown ctrl: id=%d name=%s\n",
-				__func__, ctrl->id, ctrl->name);
+		dev_dbg(s->dev, "unknown ctrl: id=%d name=%s\n",
+				ctrl->id, ctrl->name);
 		ret = -EINVAL;
 	}
 
@@ -995,15 +978,13 @@ static const struct v4l2_ctrl_ops airspy_ctrl_ops = {
 static int airspy_probe(struct usb_interface *intf,
 		const struct usb_device_id *id)
 {
-	struct usb_device *udev = interface_to_usbdev(intf);
-	struct airspy *s = NULL;
+	struct airspy *s;
 	int ret;
 	u8 u8tmp, buf[BUF_SIZE];
 
 	s = kzalloc(sizeof(struct airspy), GFP_KERNEL);
 	if (s == NULL) {
-		dev_err(&udev->dev,
-				"Could not allocate memory for airspy state\n");
+		dev_err(&intf->dev, "Could not allocate memory for state\n");
 		return -ENOMEM;
 	}
 
@@ -1011,7 +992,8 @@ static int airspy_probe(struct usb_interface *intf,
 	mutex_init(&s->vb_queue_lock);
 	spin_lock_init(&s->queued_bufs_lock);
 	INIT_LIST_HEAD(&s->queued_bufs);
-	s->udev = udev;
+	s->dev = &intf->dev;
+	s->udev = interface_to_usbdev(intf);
 	s->f_adc = bands[0].rangelow;
 	s->f_rf = bands_rf[0].rangelow;
 	s->pixelformat = formats[0].pixelformat;
@@ -1023,14 +1005,14 @@ static int airspy_probe(struct usb_interface *intf,
 		ret = airspy_ctrl_msg(s, CMD_VERSION_STRING_READ, 0, 0,
 				buf, BUF_SIZE);
 	if (ret) {
-		dev_err(&s->udev->dev, "Could not detect board\n");
+		dev_err(s->dev, "Could not detect board\n");
 		goto err_free_mem;
 	}
 
 	buf[BUF_SIZE - 1] = '\0';
 
-	dev_info(&s->udev->dev, "Board ID: %02x\n", u8tmp);
-	dev_info(&s->udev->dev, "Firmware version: %s\n", buf);
+	dev_info(s->dev, "Board ID: %02x\n", u8tmp);
+	dev_info(s->dev, "Firmware version: %s\n", buf);
 
 	/* Init videobuf2 queue structure */
 	s->vb_queue.type = V4L2_BUF_TYPE_SDR_CAPTURE;
@@ -1042,7 +1024,7 @@ static int airspy_probe(struct usb_interface *intf,
 	s->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	ret = vb2_queue_init(&s->vb_queue);
 	if (ret) {
-		dev_err(&s->udev->dev, "Could not initialize vb2 queue\n");
+		dev_err(s->dev, "Could not initialize vb2 queue\n");
 		goto err_free_mem;
 	}
 
@@ -1056,8 +1038,7 @@ static int airspy_probe(struct usb_interface *intf,
 	s->v4l2_dev.release = airspy_video_release;
 	ret = v4l2_device_register(&intf->dev, &s->v4l2_dev);
 	if (ret) {
-		dev_err(&s->udev->dev,
-				"Failed to register v4l2-device (%d)\n", ret);
+		dev_err(s->dev, "Failed to register v4l2-device (%d)\n", ret);
 		goto err_free_mem;
 	}
 
@@ -1077,7 +1058,7 @@ static int airspy_probe(struct usb_interface *intf,
 			V4L2_CID_RF_TUNER_IF_GAIN, 0, 15, 1, 0);
 	if (s->hdl.error) {
 		ret = s->hdl.error;
-		dev_err(&s->udev->dev, "Could not initialize controls\n");
+		dev_err(s->dev, "Could not initialize controls\n");
 		goto err_free_controls;
 	}
 
@@ -1089,16 +1070,13 @@ static int airspy_probe(struct usb_interface *intf,
 
 	ret = video_register_device(&s->vdev, VFL_TYPE_SDR, -1);
 	if (ret) {
-		dev_err(&s->udev->dev,
-				"Failed to register as video device (%d)\n",
+		dev_err(s->dev, "Failed to register as video device (%d)\n",
 				ret);
 		goto err_unregister_v4l2_dev;
 	}
-	dev_info(&s->udev->dev, "Registered as %s\n",
+	dev_info(s->dev, "Registered as %s\n",
 			video_device_node_name(&s->vdev));
-	dev_notice(&s->udev->dev,
-			"%s: SDR API is still slightly experimental and functionality changes may follow\n",
-			KBUILD_MODNAME);
+	dev_notice(s->dev, "SDR API is still slightly experimental and functionality changes may follow\n");
 	return 0;
 
 err_free_controls:
diff --git a/drivers/staging/media/as102/Kconfig b/drivers/media/usb/as102/Kconfig
index 28aba00dc629..28aba00dc629 100644
--- a/drivers/staging/media/as102/Kconfig
+++ b/drivers/media/usb/as102/Kconfig
diff --git a/drivers/staging/media/as102/Makefile b/drivers/media/usb/as102/Makefile
index 8916d8a909bc..22f43eee4a3b 100644
--- a/drivers/staging/media/as102/Makefile
+++ b/drivers/media/usb/as102/Makefile
@@ -1,6 +1,7 @@
 dvb-as102-objs := as102_drv.o as102_fw.o as10x_cmd.o as10x_cmd_stream.o \
-		as102_fe.o as102_usb_drv.o as10x_cmd_cfg.o
+		  as102_usb_drv.o as10x_cmd_cfg.o
 
 obj-$(CONFIG_DVB_AS102) += dvb-as102.o
 
 ccflags-y += -Idrivers/media/dvb-core
+ccflags-y += -Idrivers/media/dvb-frontends
diff --git a/drivers/staging/media/as102/as102_drv.c b/drivers/media/usb/as102/as102_drv.c
index 09d64cd67502..8be1474b2c36 100644
--- a/drivers/staging/media/as102/as102_drv.c
+++ b/drivers/media/usb/as102/as102_drv.c
@@ -12,10 +12,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #include <linux/kernel.h>
 #include <linux/errno.h>
@@ -28,13 +24,11 @@
 
 /* header file for usb device driver*/
 #include "as102_drv.h"
+#include "as10x_cmd.h"
+#include "as102_fe.h"
 #include "as102_fw.h"
 #include "dvbdev.h"
 
-int as102_debug;
-module_param_named(debug, as102_debug, int, 0644);
-MODULE_PARM_DESC(debug, "Turn on/off debugging (default: off)");
-
 int dual_tuner;
 module_param_named(dual_tuner, dual_tuner, int, 0644);
 MODULE_PARM_DESC(dual_tuner, "Activate Dual-Tuner config (default: off)");
@@ -74,7 +68,8 @@ static void as102_stop_stream(struct as102_dev_t *dev)
 			return;
 
 		if (as10x_cmd_stop_streaming(bus_adap) < 0)
-			dprintk(debug, "as10x_cmd_stop_streaming failed\n");
+			dev_dbg(&dev->bus_adap.usb_dev->dev,
+				"as10x_cmd_stop_streaming failed\n");
 
 		mutex_unlock(&dev->bus_adap.lock);
 	}
@@ -112,14 +107,16 @@ static int as10x_pid_filter(struct as102_dev_t *dev,
 	int ret = -EFAULT;
 
 	if (mutex_lock_interruptible(&dev->bus_adap.lock)) {
-		dprintk(debug, "mutex_lock_interruptible(lock) failed !\n");
+		dev_dbg(&dev->bus_adap.usb_dev->dev,
+			"amutex_lock_interruptible(lock) failed !\n");
 		return -EBUSY;
 	}
 
 	switch (onoff) {
 	case 0:
 		ret = as10x_cmd_del_PID_filter(bus_adap, (uint16_t) pid);
-		dprintk(debug, "DEL_PID_FILTER([%02d] 0x%04x) ret = %d\n",
+		dev_dbg(&dev->bus_adap.usb_dev->dev,
+			"DEL_PID_FILTER([%02d] 0x%04x) ret = %d\n",
 			index, pid, ret);
 		break;
 	case 1:
@@ -131,7 +128,7 @@ static int as10x_pid_filter(struct as102_dev_t *dev,
 		filter.pid = pid;
 
 		ret = as10x_cmd_add_PID_filter(bus_adap, &filter);
-		dprintk(debug,
+		dev_dbg(&dev->bus_adap.usb_dev->dev,
 			"ADD_PID_FILTER([%02d -> %02d], 0x%04x) ret = %d\n",
 			index, filter.idx, filter.pid, ret);
 		break;
@@ -181,6 +178,119 @@ static int as102_dvb_dmx_stop_feed(struct dvb_demux_feed *dvbdmxfeed)
 	return 0;
 }
 
+static int as102_set_tune(void *priv, struct as10x_tune_args *tune_args)
+{
+	struct as10x_bus_adapter_t *bus_adap = priv;
+	int ret;
+
+	/* Set frontend arguments */
+	if (mutex_lock_interruptible(&bus_adap->lock))
+		return -EBUSY;
+
+	ret =  as10x_cmd_set_tune(bus_adap, tune_args);
+	if (ret != 0)
+		dev_dbg(&bus_adap->usb_dev->dev,
+			"as10x_cmd_set_tune failed. (err = %d)\n", ret);
+
+	mutex_unlock(&bus_adap->lock);
+
+	return ret;
+}
+
+static int as102_get_tps(void *priv, struct as10x_tps *tps)
+{
+	struct as10x_bus_adapter_t *bus_adap = priv;
+	int ret;
+
+	if (mutex_lock_interruptible(&bus_adap->lock))
+		return -EBUSY;
+
+	/* send abilis command: GET_TPS */
+	ret = as10x_cmd_get_tps(bus_adap, tps);
+
+	mutex_unlock(&bus_adap->lock);
+
+	return ret;
+}
+
+static int as102_get_status(void *priv, struct as10x_tune_status *tstate)
+{
+	struct as10x_bus_adapter_t *bus_adap = priv;
+	int ret;
+
+	if (mutex_lock_interruptible(&bus_adap->lock))
+		return -EBUSY;
+
+	/* send abilis command: GET_TUNE_STATUS */
+	ret = as10x_cmd_get_tune_status(bus_adap, tstate);
+	if (ret < 0) {
+		dev_dbg(&bus_adap->usb_dev->dev,
+			"as10x_cmd_get_tune_status failed (err = %d)\n",
+			ret);
+	}
+
+	mutex_unlock(&bus_adap->lock);
+
+	return ret;
+}
+
+static int as102_get_stats(void *priv, struct as10x_demod_stats *demod_stats)
+{
+	struct as10x_bus_adapter_t *bus_adap = priv;
+	int ret;
+
+	if (mutex_lock_interruptible(&bus_adap->lock))
+		return -EBUSY;
+
+	/* send abilis command: GET_TUNE_STATUS */
+	ret = as10x_cmd_get_demod_stats(bus_adap, demod_stats);
+	if (ret < 0) {
+		dev_dbg(&bus_adap->usb_dev->dev,
+			"as10x_cmd_get_demod_stats failed (probably not tuned)\n");
+	} else {
+		dev_dbg(&bus_adap->usb_dev->dev,
+			"demod status: fc: 0x%08x, bad fc: 0x%08x, bytes corrected: 0x%08x , MER: 0x%04x\n",
+			demod_stats->frame_count,
+			demod_stats->bad_frame_count,
+			demod_stats->bytes_fixed_by_rs,
+			demod_stats->mer);
+	}
+	mutex_unlock(&bus_adap->lock);
+
+	return ret;
+}
+
+static int as102_stream_ctrl(void *priv, int acquire, uint32_t elna_cfg)
+{
+	struct as10x_bus_adapter_t *bus_adap = priv;
+	int ret;
+
+	if (mutex_lock_interruptible(&bus_adap->lock))
+		return -EBUSY;
+
+	if (acquire) {
+		if (elna_enable)
+			as10x_cmd_set_context(bus_adap,
+					      CONTEXT_LNA, elna_cfg);
+
+		ret = as10x_cmd_turn_on(bus_adap);
+	} else {
+		ret = as10x_cmd_turn_off(bus_adap);
+	}
+
+	mutex_unlock(&bus_adap->lock);
+
+	return ret;
+}
+
+static const struct as102_fe_ops as102_fe_ops = {
+	.set_tune = as102_set_tune,
+	.get_tps  = as102_get_tps,
+	.get_status = as102_get_status,
+	.get_stats = as102_get_stats,
+	.stream_ctrl = as102_stream_ctrl,
+};
+
 int as102_dvb_register(struct as102_dev_t *as102_dev)
 {
 	struct device *dev = &as102_dev->bus_adap.usb_dev->dev;
@@ -221,7 +331,18 @@ int as102_dvb_register(struct as102_dev_t *as102_dev)
 		goto edmxdinit;
 	}
 
-	ret = as102_dvb_register_fe(as102_dev, &as102_dev->dvb_fe);
+	/* Attach the frontend */
+	as102_dev->dvb_fe = dvb_attach(as102_attach, as102_dev->name,
+				       &as102_fe_ops,
+				       &as102_dev->bus_adap,
+				       as102_dev->elna_cfg);
+	if (!as102_dev->dvb_fe) {
+		dev_err(dev, "%s: as102_attach() failed: %d",
+		    __func__, ret);
+		goto efereg;
+	}
+
+	ret =  dvb_register_frontend(&as102_dev->dvb_adap, as102_dev->dvb_fe);
 	if (ret < 0) {
 		dev_err(dev, "%s: as102_dvb_register_frontend() failed: %d",
 		    __func__, ret);
@@ -257,7 +378,10 @@ edmxinit:
 void as102_dvb_unregister(struct as102_dev_t *as102_dev)
 {
 	/* unregister as102 frontend */
-	as102_dvb_unregister_fe(&as102_dev->dvb_fe);
+	dvb_unregister_frontend(as102_dev->dvb_fe);
+
+	/* detach frontend */
+	dvb_frontend_detach(as102_dev->dvb_fe);
 
 	/* unregister demux device */
 	dvb_dmxdev_release(&as102_dev->dvb_dmxdev);
diff --git a/drivers/staging/media/as102/as102_drv.h b/drivers/media/usb/as102/as102_drv.h
index a06837dcc05d..aee2d76e8dfc 100644
--- a/drivers/staging/media/as102/as102_drv.h
+++ b/drivers/media/usb/as102/as102_drv.h
@@ -11,33 +11,25 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#ifndef _AS102_DRV_H
+#define _AS102_DRV_H
 #include <linux/usb.h>
 #include <dvb_demux.h>
 #include <dvb_frontend.h>
 #include <dmxdev.h>
+#include "as10x_handle.h"
 #include "as10x_cmd.h"
 #include "as102_usb_drv.h"
 
 #define DRIVER_FULL_NAME "Abilis Systems as10x usb driver"
 #define DRIVER_NAME "as10x_usb"
 
-extern int as102_debug;
 #define debug	as102_debug
 extern struct usb_driver as102_usb_driver;
 extern int elna_enable;
 
-#define dprintk(debug, args...) \
-	do { if (debug) {	\
-		pr_debug("%s: ", __func__);	\
-		printk(args);	\
-	} } while (0)
-
 #define AS102_DEVICE_MAJOR	192
 
 #define AS102_USB_BUF_SIZE	512
@@ -71,17 +63,10 @@ struct as102_dev_t {
 	uint8_t elna_cfg;
 
 	struct dvb_adapter dvb_adap;
-	struct dvb_frontend dvb_fe;
+	struct dvb_frontend *dvb_fe;
 	struct dvb_demux dvb_dmx;
 	struct dmxdev dvb_dmxdev;
 
-	/* demodulator stats */
-	struct as10x_demod_stats demod_stats;
-	/* signal strength */
-	uint16_t signal_strength;
-	/* bit error rate */
-	uint32_t ber;
-
 	/* timer handle to trig ts stream download */
 	struct timer_list timer_handle;
 
@@ -95,5 +80,4 @@ struct as102_dev_t {
 int as102_dvb_register(struct as102_dev_t *dev);
 void as102_dvb_unregister(struct as102_dev_t *dev);
 
-int as102_dvb_register_fe(struct as102_dev_t *dev, struct dvb_frontend *fe);
-int as102_dvb_unregister_fe(struct dvb_frontend *dev);
+#endif
diff --git a/drivers/staging/media/as102/as102_fw.c b/drivers/media/usb/as102/as102_fw.c
index f33f752c0aad..07d08c49f4d4 100644
--- a/drivers/staging/media/as102/as102_fw.c
+++ b/drivers/media/usb/as102/as102_fw.c
@@ -12,10 +12,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #include <linux/kernel.h>
 #include <linux/errno.h>
diff --git a/drivers/staging/media/as102/as102_fw.h b/drivers/media/usb/as102/as102_fw.h
index 4bfc6849d95a..2732b784216d 100644
--- a/drivers/staging/media/as102/as102_fw.h
+++ b/drivers/media/usb/as102/as102_fw.h
@@ -11,10 +11,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #define MAX_FW_PKT_SIZE	64
 
diff --git a/drivers/staging/media/as102/as102_usb_drv.c b/drivers/media/usb/as102/as102_usb_drv.c
index e6f6278e97d6..3f669066ccf6 100644
--- a/drivers/staging/media/as102/as102_usb_drv.c
+++ b/drivers/media/usb/as102/as102_usb_drv.c
@@ -12,10 +12,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #include <linux/kernel.h>
 #include <linux/errno.h>
@@ -104,21 +100,22 @@ static int as102_usb_xfer_cmd(struct as10x_bus_adapter_t *bus_adap,
 				      send_buf, send_buf_len,
 				      USB_CTRL_SET_TIMEOUT /* 200 */);
 		if (ret < 0) {
-			dprintk(debug, "usb_control_msg(send) failed, err %i\n",
-					ret);
+			dev_dbg(&bus_adap->usb_dev->dev,
+				"usb_control_msg(send) failed, err %i\n", ret);
 			return ret;
 		}
 
 		if (ret != send_buf_len) {
-			dprintk(debug, "only wrote %d of %d bytes\n",
-					ret, send_buf_len);
+			dev_dbg(&bus_adap->usb_dev->dev,
+			"only wrote %d of %d bytes\n", ret, send_buf_len);
 			return -1;
 		}
 	}
 
 	if (recv_buf != NULL) {
 #ifdef TRACE
-		dprintk(debug, "want to read: %d bytes\n", recv_buf_len);
+		dev_dbg(bus_adap->usb_dev->dev,
+			"want to read: %d bytes\n", recv_buf_len);
 #endif
 		ret = usb_control_msg(bus_adap->usb_dev,
 				      usb_rcvctrlpipe(bus_adap->usb_dev, 0),
@@ -130,12 +127,13 @@ static int as102_usb_xfer_cmd(struct as10x_bus_adapter_t *bus_adap,
 				      recv_buf, recv_buf_len,
 				      USB_CTRL_GET_TIMEOUT /* 200 */);
 		if (ret < 0) {
-			dprintk(debug, "usb_control_msg(recv) failed, err %i\n",
-					ret);
+			dev_dbg(&bus_adap->usb_dev->dev,
+				"usb_control_msg(recv) failed, err %i\n", ret);
 			return ret;
 		}
 #ifdef TRACE
-		dprintk(debug, "read %d bytes\n", recv_buf_len);
+		dev_dbg(bus_adap->usb_dev->dev,
+			"read %d bytes\n", recv_buf_len);
 #endif
 	}
 
@@ -147,28 +145,29 @@ static int as102_send_ep1(struct as10x_bus_adapter_t *bus_adap,
 			  int send_buf_len,
 			  int swap32)
 {
-	int ret = 0, actual_len;
+	int ret, actual_len;
 
 	ret = usb_bulk_msg(bus_adap->usb_dev,
 			   usb_sndbulkpipe(bus_adap->usb_dev, 1),
 			   send_buf, send_buf_len, &actual_len, 200);
 	if (ret) {
-		dprintk(debug, "usb_bulk_msg(send) failed, err %i\n", ret);
+		dev_dbg(&bus_adap->usb_dev->dev,
+			"usb_bulk_msg(send) failed, err %i\n", ret);
 		return ret;
 	}
 
 	if (actual_len != send_buf_len) {
-		dprintk(debug, "only wrote %d of %d bytes\n",
-				actual_len, send_buf_len);
+		dev_dbg(&bus_adap->usb_dev->dev, "only wrote %d of %d bytes\n",
+			actual_len, send_buf_len);
 		return -1;
 	}
-	return ret ? ret : actual_len;
+	return actual_len;
 }
 
 static int as102_read_ep2(struct as10x_bus_adapter_t *bus_adap,
 		   unsigned char *recv_buf, int recv_buf_len)
 {
-	int ret = 0, actual_len;
+	int ret, actual_len;
 
 	if (recv_buf == NULL)
 		return -EINVAL;
@@ -177,16 +176,17 @@ static int as102_read_ep2(struct as10x_bus_adapter_t *bus_adap,
 			   usb_rcvbulkpipe(bus_adap->usb_dev, 2),
 			   recv_buf, recv_buf_len, &actual_len, 200);
 	if (ret) {
-		dprintk(debug, "usb_bulk_msg(recv) failed, err %i\n", ret);
+		dev_dbg(&bus_adap->usb_dev->dev,
+			"usb_bulk_msg(recv) failed, err %i\n", ret);
 		return ret;
 	}
 
 	if (actual_len != recv_buf_len) {
-		dprintk(debug, "only read %d of %d bytes\n",
-				actual_len, recv_buf_len);
+		dev_dbg(&bus_adap->usb_dev->dev, "only read %d of %d bytes\n",
+			actual_len, recv_buf_len);
 		return -1;
 	}
-	return ret ? ret : actual_len;
+	return actual_len;
 }
 
 static struct as102_priv_ops_t as102_priv_ops = {
@@ -211,7 +211,8 @@ static int as102_submit_urb_stream(struct as102_dev_t *dev, struct urb *urb)
 
 	err = usb_submit_urb(urb, GFP_ATOMIC);
 	if (err)
-		dprintk(debug, "%s: usb_submit_urb failed\n", __func__);
+		dev_dbg(&urb->dev->dev,
+			"%s: usb_submit_urb failed\n", __func__);
 
 	return err;
 }
@@ -256,7 +257,8 @@ static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 				       GFP_KERNEL,
 				       &dev->dma_addr);
 	if (!dev->stream) {
-		dprintk(debug, "%s: usb_buffer_alloc failed\n", __func__);
+		dev_dbg(&dev->bus_adap.usb_dev->dev,
+			"%s: usb_buffer_alloc failed\n", __func__);
 		return -ENOMEM;
 	}
 
@@ -268,7 +270,8 @@ static int as102_alloc_usb_stream_buffer(struct as102_dev_t *dev)
 
 		urb = usb_alloc_urb(0, GFP_ATOMIC);
 		if (urb == NULL) {
-			dprintk(debug, "%s: usb_alloc_urb failed\n", __func__);
+			dev_dbg(&dev->bus_adap.usb_dev->dev,
+				"%s: usb_alloc_urb failed\n", __func__);
 			as102_free_usb_stream_buffer(dev);
 			return -ENOMEM;
 		}
diff --git a/drivers/staging/media/as102/as102_usb_drv.h b/drivers/media/usb/as102/as102_usb_drv.h
index 1ad1ec52b11e..4fb1baa8cac0 100644
--- a/drivers/staging/media/as102/as102_usb_drv.h
+++ b/drivers/media/usb/as102/as102_usb_drv.h
@@ -12,10 +12,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #ifndef _AS102_USB_DRV_H_
 #define _AS102_USB_DRV_H_
diff --git a/drivers/staging/media/as102/as10x_cmd.c b/drivers/media/usb/as102/as10x_cmd.c
index 9e49f15a7c9f..870617994410 100644
--- a/drivers/staging/media/as102/as10x_cmd.c
+++ b/drivers/media/usb/as102/as10x_cmd.c
@@ -12,15 +12,10 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/kernel.h>
 #include "as102_drv.h"
-#include "as10x_types.h"
 #include "as10x_cmd.h"
 
 /**
@@ -126,7 +121,7 @@ int as10x_cmd_set_tune(struct as10x_bus_adapter_t *adap,
 
 	/* fill command */
 	preq->body.set_tune.req.proc_id = cpu_to_le16(CONTROL_PROC_SETTUNE);
-	preq->body.set_tune.req.args.freq = cpu_to_le32(ptune->freq);
+	preq->body.set_tune.req.args.freq = (__force __u32)cpu_to_le32(ptune->freq);
 	preq->body.set_tune.req.args.bandwidth = ptune->bandwidth;
 	preq->body.set_tune.req.args.hier_select = ptune->hier_select;
 	preq->body.set_tune.req.args.modulation = ptune->modulation;
@@ -204,9 +199,9 @@ int as10x_cmd_get_tune_status(struct as10x_bus_adapter_t *adap,
 	/* Response OK -> get response data */
 	pstatus->tune_state = prsp->body.get_tune_status.rsp.sts.tune_state;
 	pstatus->signal_strength  =
-		le16_to_cpu(prsp->body.get_tune_status.rsp.sts.signal_strength);
-	pstatus->PER = le16_to_cpu(prsp->body.get_tune_status.rsp.sts.PER);
-	pstatus->BER = le16_to_cpu(prsp->body.get_tune_status.rsp.sts.BER);
+		le16_to_cpu((__force __le16)prsp->body.get_tune_status.rsp.sts.signal_strength);
+	pstatus->PER = le16_to_cpu((__force __le16)prsp->body.get_tune_status.rsp.sts.PER);
+	pstatus->BER = le16_to_cpu((__force __le16)prsp->body.get_tune_status.rsp.sts.BER);
 
 out:
 	return error;
@@ -264,7 +259,7 @@ int as10x_cmd_get_tps(struct as10x_bus_adapter_t *adap, struct as10x_tps *ptps)
 	ptps->transmission_mode  = prsp->body.get_tps.rsp.tps.transmission_mode;
 	ptps->DVBH_mask_HP = prsp->body.get_tps.rsp.tps.DVBH_mask_HP;
 	ptps->DVBH_mask_LP = prsp->body.get_tps.rsp.tps.DVBH_mask_LP;
-	ptps->cell_ID = le16_to_cpu(prsp->body.get_tps.rsp.tps.cell_ID);
+	ptps->cell_ID = le16_to_cpu((__force __le16)prsp->body.get_tps.rsp.tps.cell_ID);
 
 out:
 	return error;
@@ -315,13 +310,13 @@ int as10x_cmd_get_demod_stats(struct as10x_bus_adapter_t *adap,
 
 	/* Response OK -> get response data */
 	pdemod_stats->frame_count =
-		le32_to_cpu(prsp->body.get_demod_stats.rsp.stats.frame_count);
+		le32_to_cpu((__force __le32)prsp->body.get_demod_stats.rsp.stats.frame_count);
 	pdemod_stats->bad_frame_count =
-		le32_to_cpu(prsp->body.get_demod_stats.rsp.stats.bad_frame_count);
+		le32_to_cpu((__force __le32)prsp->body.get_demod_stats.rsp.stats.bad_frame_count);
 	pdemod_stats->bytes_fixed_by_rs =
-		le32_to_cpu(prsp->body.get_demod_stats.rsp.stats.bytes_fixed_by_rs);
+		le32_to_cpu((__force __le32)prsp->body.get_demod_stats.rsp.stats.bytes_fixed_by_rs);
 	pdemod_stats->mer =
-		le16_to_cpu(prsp->body.get_demod_stats.rsp.stats.mer);
+		le16_to_cpu((__force __le16)prsp->body.get_demod_stats.rsp.stats.mer);
 	pdemod_stats->has_started =
 		prsp->body.get_demod_stats.rsp.stats.has_started;
 
diff --git a/drivers/staging/media/as102/as10x_cmd.h b/drivers/media/usb/as102/as10x_cmd.h
index e21ec6c702a9..e06b84e2ff79 100644
--- a/drivers/staging/media/as102/as10x_cmd.h
+++ b/drivers/media/usb/as102/as10x_cmd.h
@@ -11,19 +11,13 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 #ifndef _AS10X_CMD_H_
 #define _AS10X_CMD_H_
 
-#ifdef __KERNEL__
 #include <linux/kernel.h>
-#endif
 
-#include "as10x_types.h"
+#include "as102_fe_types.h"
 
 /*********************************/
 /*       MACRO DEFINITIONS       */
@@ -98,12 +92,12 @@ union as10x_turn_on {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 	} __packed rsp;
@@ -113,12 +107,12 @@ union as10x_turn_off {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t err;
 	} __packed rsp;
@@ -128,14 +122,14 @@ union as10x_set_tune {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* tune params */
 		struct as10x_tune_args args;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* response error */
 		uint8_t error;
 	} __packed rsp;
@@ -145,12 +139,12 @@ union as10x_get_tune_status {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* response error */
 		uint8_t error;
 		/* tune status */
@@ -162,12 +156,12 @@ union as10x_get_tps {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* response error */
 		uint8_t error;
 		/* tps details */
@@ -179,12 +173,12 @@ union as10x_common {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t  proc_id;
+		__le16  proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* response error */
 		uint8_t error;
 	} __packed rsp;
@@ -194,9 +188,9 @@ union as10x_add_pid_filter {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t  proc_id;
+		__le16  proc_id;
 		/* PID to filter */
-		uint16_t  pid;
+		__le16  pid;
 		/* stream type (MPE, PSI/SI or PES )*/
 		uint8_t stream_type;
 		/* PID index in filter table */
@@ -205,7 +199,7 @@ union as10x_add_pid_filter {
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* response error */
 		uint8_t error;
 		/* Filter id */
@@ -217,14 +211,14 @@ union as10x_del_pid_filter {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t  proc_id;
+		__le16  proc_id;
 		/* PID to remove */
-		uint16_t  pid;
+		__le16  pid;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* response error */
 		uint8_t error;
 	} __packed rsp;
@@ -234,12 +228,12 @@ union as10x_start_streaming {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 	} __packed rsp;
@@ -249,12 +243,12 @@ union as10x_stop_streaming {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 	} __packed rsp;
@@ -264,12 +258,12 @@ union as10x_get_demod_stats {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 		/* demod stats */
@@ -281,12 +275,12 @@ union as10x_get_impulse_resp {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 		/* impulse response ready */
@@ -298,22 +292,22 @@ union as10x_fw_context {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* value to write (for set context)*/
 		struct as10x_register_value reg_val;
 		/* context tag */
-		uint16_t tag;
+		__le16 tag;
 		/* context request type */
-		uint16_t type;
+		__le16 type;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* value read (for get context) */
 		struct as10x_register_value reg_val;
 		/* context request type */
-		uint16_t type;
+		__le16 type;
 		/* error */
 		uint8_t error;
 	} __packed rsp;
@@ -323,7 +317,7 @@ union as10x_set_register {
 	/* request */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* register description */
 		struct as10x_register_addr reg_addr;
 		/* register content */
@@ -332,7 +326,7 @@ union as10x_set_register {
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 	} __packed rsp;
@@ -342,14 +336,14 @@ union as10x_get_register {
 	/* request */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* register description */
 		struct as10x_register_addr reg_addr;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 		/* register content */
@@ -361,24 +355,24 @@ union as10x_cfg_change_mode {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* mode */
 		uint8_t mode;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 	} __packed rsp;
 } __packed;
 
 struct as10x_cmd_header_t {
-	uint16_t req_id;
-	uint16_t prog;
-	uint16_t version;
-	uint16_t data_len;
+	__le16 req_id;
+	__le16 prog;
+	__le16 version;
+	__le16 data_len;
 } __packed;
 
 #define DUMP_BLOCK_SIZE 16
@@ -387,18 +381,18 @@ union as10x_dump_memory {
 	/* request */
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* dump memory type request */
 		uint8_t dump_req;
 		/* register description */
 		struct as10x_register_addr reg_addr;
 		/* nb blocks to read */
-		uint16_t num_blocks;
+		__le16 num_blocks;
 	} __packed req;
 	/* response */
 	struct {
 		/* response identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 		/* dump response */
@@ -406,8 +400,8 @@ union as10x_dump_memory {
 		/* data */
 		union {
 			uint8_t  data8[DUMP_BLOCK_SIZE];
-			uint16_t data16[DUMP_BLOCK_SIZE / sizeof(uint16_t)];
-			uint32_t data32[DUMP_BLOCK_SIZE / sizeof(uint32_t)];
+			__le16 data16[DUMP_BLOCK_SIZE / sizeof(__le16)];
+			__le32 data32[DUMP_BLOCK_SIZE / sizeof(__le32)];
 		} __packed u;
 	} __packed rsp;
 } __packed;
@@ -415,13 +409,13 @@ union as10x_dump_memory {
 union as10x_dumplog_memory {
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* dump memory type request */
 		uint8_t dump_req;
 	} __packed req;
 	struct {
 		/* request identifier */
-		uint16_t proc_id;
+		__le16 proc_id;
 		/* error */
 		uint8_t error;
 		/* dump response */
@@ -434,13 +428,13 @@ union as10x_dumplog_memory {
 union as10x_raw_data {
 	/* request */
 	struct {
-		uint16_t proc_id;
+		__le16 proc_id;
 		uint8_t data[64 - sizeof(struct as10x_cmd_header_t)
 			     - 2 /* proc_id */];
 	} __packed req;
 	/* response */
 	struct {
-		uint16_t proc_id;
+		__le16 proc_id;
 		uint8_t error;
 		uint8_t data[64 - sizeof(struct as10x_cmd_header_t)
 			     - 2 /* proc_id */ - 1 /* rc */];
diff --git a/drivers/staging/media/as102/as10x_cmd_cfg.c b/drivers/media/usb/as102/as10x_cmd_cfg.c
index b1e300d88753..c87f2ca223a2 100644
--- a/drivers/staging/media/as102/as10x_cmd_cfg.c
+++ b/drivers/media/usb/as102/as10x_cmd_cfg.c
@@ -11,15 +11,10 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/kernel.h>
 #include "as102_drv.h"
-#include "as10x_types.h"
 #include "as10x_cmd.h"
 
 /***************************/
@@ -74,7 +69,7 @@ int as10x_cmd_get_context(struct as10x_bus_adapter_t *adap, uint16_t tag,
 
 	if (error == 0) {
 		/* Response OK -> get response data */
-		*pvalue = le32_to_cpu(prsp->body.context.rsp.reg_val.u.value32);
+		*pvalue = le32_to_cpu((__force __le32)prsp->body.context.rsp.reg_val.u.value32);
 		/* value returned is always a 32-bit value */
 	}
 
@@ -106,7 +101,7 @@ int as10x_cmd_set_context(struct as10x_bus_adapter_t *adap, uint16_t tag,
 	/* fill command */
 	pcmd->body.context.req.proc_id = cpu_to_le16(CONTROL_PROC_CONTEXT);
 	/* pcmd->body.context.req.reg_val.mode initialization is not required */
-	pcmd->body.context.req.reg_val.u.value32 = cpu_to_le32(value);
+	pcmd->body.context.req.reg_val.u.value32 = (__force u32)cpu_to_le32(value);
 	pcmd->body.context.req.tag = cpu_to_le16(tag);
 	pcmd->body.context.req.type = cpu_to_le16(SET_CONTEXT_DATA);
 
diff --git a/drivers/staging/media/as102/as10x_cmd_stream.c b/drivers/media/usb/as102/as10x_cmd_stream.c
index 1088ca1fe92f..126aea976639 100644
--- a/drivers/staging/media/as102/as10x_cmd_stream.c
+++ b/drivers/media/usb/as102/as10x_cmd_stream.c
@@ -11,10 +11,6 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/kernel.h>
diff --git a/drivers/staging/media/as102/as10x_handle.h b/drivers/media/usb/as102/as10x_handle.h
index 5638b191b780..d6b58c770500 100644
--- a/drivers/staging/media/as102/as10x_handle.h
+++ b/drivers/media/usb/as102/as10x_handle.h
@@ -11,12 +11,9 @@
  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
-#ifdef __KERNEL__
+#ifndef _AS10X_HANDLE_H
+#define _AS10X_HANDLE_H
 struct as10x_bus_adapter_t;
 struct as102_dev_t;
 
diff --git a/drivers/media/usb/au0828/au0828-cards.c b/drivers/media/usb/au0828/au0828-cards.c
index 2c6b7da137ed..9eb77ac2153b 100644
--- a/drivers/media/usb/au0828/au0828-cards.c
+++ b/drivers/media/usb/au0828/au0828-cards.c
@@ -46,6 +46,8 @@ struct au0828_board au0828_boards[] = {
 		.name	= "Hauppauge HVR850",
 		.tuner_type = TUNER_XC5000,
 		.tuner_addr = 0x61,
+		.has_ir_i2c = 1,
+		.has_analog = 1,
 		.i2c_clk_divider = AU0828_I2C_CLK_250KHZ,
 		.input = {
 			{
@@ -72,12 +74,7 @@ struct au0828_board au0828_boards[] = {
 		.tuner_type = TUNER_XC5000,
 		.tuner_addr = 0x61,
 		.has_ir_i2c = 1,
-		/* The au0828 hardware i2c implementation does not properly
-		   support the xc5000's i2c clock stretching.  So we need to
-		   lower the clock frequency enough where the 15us clock
-		   stretch fits inside of a normal clock cycle, or else the
-		   au0828 fails to set the STOP bit.  A 30 KHz clock puts the
-		   clock pulse width at 18us */
+		.has_analog = 1,
 		.i2c_clk_divider = AU0828_I2C_CLK_250KHZ,
 		.input = {
 			{
@@ -101,20 +98,20 @@ struct au0828_board au0828_boards[] = {
 	},
 	[AU0828_BOARD_HAUPPAUGE_HVR950Q_MXL] = {
 		.name	= "Hauppauge HVR950Q rev xxF8",
-		.tuner_type = UNSET,
-		.tuner_addr = ADDR_UNSET,
+		.tuner_type = TUNER_XC5000,
+		.tuner_addr = 0x61,
 		.i2c_clk_divider = AU0828_I2C_CLK_250KHZ,
 	},
 	[AU0828_BOARD_DVICO_FUSIONHDTV7] = {
 		.name	= "DViCO FusionHDTV USB",
-		.tuner_type = UNSET,
-		.tuner_addr = ADDR_UNSET,
+		.tuner_type = TUNER_XC5000,
+		.tuner_addr = 0x61,
 		.i2c_clk_divider = AU0828_I2C_CLK_250KHZ,
 	},
 	[AU0828_BOARD_HAUPPAUGE_WOODBURY] = {
 		.name = "Hauppauge Woodbury",
-		.tuner_type = UNSET,
-		.tuner_addr = ADDR_UNSET,
+		.tuner_type = TUNER_NXP_TDA18271,
+		.tuner_addr = 0x60,
 		.i2c_clk_divider = AU0828_I2C_CLK_250KHZ,
 	},
 };
@@ -142,8 +139,7 @@ int au0828_tuner_callback(void *priv, int component, int command, int arg)
 			mdelay(10);
 			return 0;
 		} else {
-			printk(KERN_ERR
-				"%s(): Unknown command.\n", __func__);
+			pr_err("%s(): Unknown command.\n", __func__);
 			return -EINVAL;
 		}
 		break;
@@ -177,12 +173,12 @@ static void hauppauge_eeprom(struct au0828_dev *dev, u8 *eeprom_data)
 	case 72500: /* WinTV-HVR950q (OEM, No IR, ATSC/QAM */
 		break;
 	default:
-		printk(KERN_WARNING "%s: warning: "
-		       "unknown hauppauge model #%d\n", __func__, tv.model);
+		pr_warn("%s: warning: unknown hauppauge model #%d\n",
+			__func__, tv.model);
 		break;
 	}
 
-	printk(KERN_INFO "%s: hauppauge eeprom: model=%d\n",
+	pr_info("%s: hauppauge eeprom: model=%d\n",
 	       __func__, tv.model);
 }
 
@@ -228,16 +224,16 @@ void au0828_card_analog_fe_setup(struct au0828_dev *dev)
 		sd = v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
 				"au8522", 0x8e >> 1, NULL);
 		if (sd == NULL)
-			printk(KERN_ERR "analog subdev registration failed\n");
+			pr_err("analog subdev registration failed\n");
 	}
 
 	/* Setup tuners */
-	if (dev->board.tuner_type != TUNER_ABSENT) {
+	if (dev->board.tuner_type != TUNER_ABSENT && dev->board.has_analog) {
 		/* Load the tuner module, which does the attach */
 		sd = v4l2_i2c_new_subdev(&dev->v4l2_dev, &dev->i2c_adap,
 				"tuner", dev->board.tuner_addr, NULL);
 		if (sd == NULL)
-			printk(KERN_ERR "tuner subdev registration fail\n");
+			pr_err("tuner subdev registration fail\n");
 
 		tun_setup.mode_mask      = mode_mask;
 		tun_setup.type           = dev->board.tuner_type;
diff --git a/drivers/media/usb/au0828/au0828-core.c b/drivers/media/usb/au0828/au0828-core.c
index 56025e689442..bc064803b6c7 100644
--- a/drivers/media/usb/au0828/au0828-core.c
+++ b/drivers/media/usb/au0828/au0828-core.c
@@ -19,14 +19,14 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "au0828.h"
+
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/videodev2.h>
 #include <media/v4l2-common.h>
 #include <linux/mutex.h>
 
-#include "au0828.h"
-
 /*
  * 1 = General debug messages
  * 2 = USB handling
@@ -90,7 +90,7 @@ static int send_control_msg(struct au0828_dev *dev, u16 request, u32 value,
 		status = min(status, 0);
 
 		if (status < 0) {
-			printk(KERN_ERR "%s() Failed sending control message, error %d.\n",
+			pr_err("%s() Failed sending control message, error %d.\n",
 				__func__, status);
 		}
 
@@ -115,7 +115,7 @@ static int recv_control_msg(struct au0828_dev *dev, u16 request, u32 value,
 		status = min(status, 0);
 
 		if (status < 0) {
-			printk(KERN_ERR "%s() Failed receiving control message, error %d.\n",
+			pr_err("%s() Failed receiving control message, error %d.\n",
 				__func__, status);
 		}
 
@@ -153,9 +153,7 @@ static void au0828_usb_disconnect(struct usb_interface *interface)
 
 	dprintk(1, "%s()\n", __func__);
 
-#ifdef CONFIG_VIDEO_AU0828_RC
 	au0828_rc_unregister(dev);
-#endif
 	/* Digital TV */
 	au0828_dvb_unregister(dev);
 
@@ -199,15 +197,14 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	 * not enough even for most Digital TV streams.
 	 */
 	if (usbdev->speed != USB_SPEED_HIGH && disable_usb_speed_check == 0) {
-		printk(KERN_ERR "au0828: Device initialization failed.\n");
-		printk(KERN_ERR "au0828: Device must be connected to a "
-		       "high-speed USB 2.0 port.\n");
+		pr_err("au0828: Device initialization failed.\n");
+		pr_err("au0828: Device must be connected to a high-speed USB 2.0 port.\n");
 		return -ENODEV;
 	}
 
 	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
 	if (dev == NULL) {
-		printk(KERN_ERR "%s() Unable to allocate memory\n", __func__);
+		pr_err("%s() Unable to allocate memory\n", __func__);
 		return -ENOMEM;
 	}
 
@@ -266,10 +263,8 @@ static int au0828_usb_probe(struct usb_interface *interface,
 		pr_err("%s() au0282_dev_register failed\n",
 		       __func__);
 
-#ifdef CONFIG_VIDEO_AU0828_RC
 	/* Remote controller */
 	au0828_rc_register(dev);
-#endif
 
 	/*
 	 * Store the pointer to the au0828_dev so it can be accessed in
@@ -277,7 +272,7 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	 */
 	usb_set_intfdata(interface, dev);
 
-	printk(KERN_INFO "Registered device AU0828 [%s]\n",
+	pr_info("Registered device AU0828 [%s]\n",
 		dev->board.name == NULL ? "Unset" : dev->board.name);
 
 	mutex_unlock(&dev->lock);
@@ -285,13 +280,56 @@ static int au0828_usb_probe(struct usb_interface *interface,
 	return retval;
 }
 
+static int au0828_suspend(struct usb_interface *interface,
+				pm_message_t message)
+{
+	struct au0828_dev *dev = usb_get_intfdata(interface);
+
+	if (!dev)
+		return 0;
+
+	pr_info("Suspend\n");
+
+	au0828_rc_suspend(dev);
+	au0828_v4l2_suspend(dev);
+	au0828_dvb_suspend(dev);
+
+	/* FIXME: should suspend also ATV/DTV */
+
+	return 0;
+}
+
+static int au0828_resume(struct usb_interface *interface)
+{
+	struct au0828_dev *dev = usb_get_intfdata(interface);
+	if (!dev)
+		return 0;
+
+	pr_info("Resume\n");
+
+	/* Power Up the bridge */
+	au0828_write(dev, REG_600, 1 << 4);
+
+	/* Bring up the GPIO's and supporting devices */
+	au0828_gpio_setup(dev);
+
+	au0828_rc_resume(dev);
+	au0828_v4l2_resume(dev);
+	au0828_dvb_resume(dev);
+
+	/* FIXME: should resume also ATV/DTV */
+
+	return 0;
+}
+
 static struct usb_driver au0828_usb_driver = {
-	.name		= DRIVER_NAME,
+	.name		= KBUILD_MODNAME,
 	.probe		= au0828_usb_probe,
 	.disconnect	= au0828_usb_disconnect,
 	.id_table	= au0828_usb_id_table,
-
-	/* FIXME: Add suspend and resume functions */
+	.suspend	= au0828_suspend,
+	.resume		= au0828_resume,
+	.reset_resume	= au0828_resume,
 };
 
 static int __init au0828_init(void)
@@ -299,27 +337,27 @@ static int __init au0828_init(void)
 	int ret;
 
 	if (au0828_debug & 1)
-		printk(KERN_INFO "%s() Debugging is enabled\n", __func__);
+		pr_info("%s() Debugging is enabled\n", __func__);
 
 	if (au0828_debug & 2)
-		printk(KERN_INFO "%s() USB Debugging is enabled\n", __func__);
+		pr_info("%s() USB Debugging is enabled\n", __func__);
 
 	if (au0828_debug & 4)
-		printk(KERN_INFO "%s() I2C Debugging is enabled\n", __func__);
+		pr_info("%s() I2C Debugging is enabled\n", __func__);
 
 	if (au0828_debug & 8)
-		printk(KERN_INFO "%s() Bridge Debugging is enabled\n",
+		pr_info("%s() Bridge Debugging is enabled\n",
 		       __func__);
 
 	if (au0828_debug & 16)
-		printk(KERN_INFO "%s() IR Debugging is enabled\n",
+		pr_info("%s() IR Debugging is enabled\n",
 		       __func__);
 
-	printk(KERN_INFO "au0828 driver loaded\n");
+	pr_info("au0828 driver loaded\n");
 
 	ret = usb_register(&au0828_usb_driver);
 	if (ret)
-		printk(KERN_ERR "usb_register failed, error = %d\n", ret);
+		pr_err("usb_register failed, error = %d\n", ret);
 
 	return ret;
 }
diff --git a/drivers/media/usb/au0828/au0828-dvb.c b/drivers/media/usb/au0828/au0828-dvb.c
index d8b5d9480279..00ab1563d142 100644
--- a/drivers/media/usb/au0828/au0828-dvb.c
+++ b/drivers/media/usb/au0828/au0828-dvb.c
@@ -19,15 +19,15 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "au0828.h"
+
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/device.h>
-#include <linux/suspend.h>
 #include <media/v4l2-common.h>
 #include <media/tuner.h>
 
-#include "au0828.h"
 #include "au8522.h"
 #include "xc5000.h"
 #include "mxl5007t.h"
@@ -121,13 +121,13 @@ static void urb_completion(struct urb *purb)
 		return;
 	}
 
-	if (dev->urb_streaming == 0) {
+	if (!dev->urb_streaming) {
 		dprintk(2, "%s: not streaming!\n", __func__);
 		return;
 	}
 
 	if (ptype != PIPE_BULK) {
-		printk(KERN_ERR "%s: Unsupported URB type %d\n",
+		pr_err("%s: Unsupported URB type %d\n",
 		       __func__, ptype);
 		return;
 	}
@@ -159,7 +159,10 @@ static int stop_urb_transfer(struct au0828_dev *dev)
 
 	dprintk(2, "%s()\n", __func__);
 
-	dev->urb_streaming = 0;
+	if (!dev->urb_streaming)
+		return 0;
+
+	dev->urb_streaming = false;
 	for (i = 0; i < URB_COUNT; i++) {
 		if (dev->urbs[i]) {
 			usb_kill_urb(dev->urbs[i]);
@@ -202,8 +205,7 @@ static int start_urb_transfer(struct au0828_dev *dev)
 		if (!purb->transfer_buffer) {
 			usb_free_urb(purb);
 			dev->urbs[i] = NULL;
-			printk(KERN_ERR
-			       "%s: failed big buffer allocation, err = %d\n",
+			pr_err("%s: failed big buffer allocation, err = %d\n",
 			       __func__, ret);
 			goto err;
 		}
@@ -224,13 +226,13 @@ static int start_urb_transfer(struct au0828_dev *dev)
 		ret = usb_submit_urb(dev->urbs[i], GFP_ATOMIC);
 		if (ret != 0) {
 			stop_urb_transfer(dev);
-			printk(KERN_ERR "%s: failed urb submission, "
-			       "err = %d\n", __func__, ret);
+			pr_err("%s: failed urb submission, err = %d\n",
+			       __func__, ret);
 			return ret;
 		}
 	}
 
-	dev->urb_streaming = 1;
+	dev->urb_streaming = true;
 	ret = 0;
 
 err:
@@ -268,7 +270,7 @@ static int au0828_dvb_start_feed(struct dvb_demux_feed *feed)
 	if (!demux->dmx.frontend)
 		return -EINVAL;
 
-	if (dvb) {
+	if (dvb->frontend) {
 		mutex_lock(&dvb->lock);
 		dvb->start_count++;
 		dprintk(1, "%s(), start_count: %d, stop_count: %d\n", __func__,
@@ -297,7 +299,7 @@ static int au0828_dvb_stop_feed(struct dvb_demux_feed *feed)
 
 	dprintk(1, "%s()\n", __func__);
 
-	if (dvb) {
+	if (dvb->frontend) {
 		cancel_work_sync(&dev->restart_streaming);
 
 		mutex_lock(&dvb->lock);
@@ -324,7 +326,7 @@ static void au0828_restart_dvb_streaming(struct work_struct *work)
 					      restart_streaming);
 	struct au0828_dvb *dvb = &dev->dvb;
 
-	if (dev->urb_streaming == 0)
+	if (!dev->urb_streaming)
 		return;
 
 	dprintk(1, "Restarting streaming...!\n");
@@ -393,9 +395,8 @@ static int dvb_register(struct au0828_dev *dev)
 			if (!dev->dig_transfer_buffer[i]) {
 				result = -ENOMEM;
 
-				printk(KERN_ERR
-				       "%s: failed buffer allocation (errno = %d)\n",
-				       DRIVER_NAME, result);
+				pr_err("failed buffer allocation (errno = %d)\n",
+				       result);
 				goto fail_adapter;
 			}
 		}
@@ -404,11 +405,12 @@ static int dvb_register(struct au0828_dev *dev)
 	INIT_WORK(&dev->restart_streaming, au0828_restart_dvb_streaming);
 
 	/* register adapter */
-	result = dvb_register_adapter(&dvb->adapter, DRIVER_NAME, THIS_MODULE,
+	result = dvb_register_adapter(&dvb->adapter,
+				      KBUILD_MODNAME, THIS_MODULE,
 				      &dev->usbdev->dev, adapter_nr);
 	if (result < 0) {
-		printk(KERN_ERR "%s: dvb_register_adapter failed "
-		       "(errno = %d)\n", DRIVER_NAME, result);
+		pr_err("dvb_register_adapter failed (errno = %d)\n",
+		       result);
 		goto fail_adapter;
 	}
 	dvb->adapter.priv = dev;
@@ -416,8 +418,8 @@ static int dvb_register(struct au0828_dev *dev)
 	/* register frontend */
 	result = dvb_register_frontend(&dvb->adapter, dvb->frontend);
 	if (result < 0) {
-		printk(KERN_ERR "%s: dvb_register_frontend failed "
-		       "(errno = %d)\n", DRIVER_NAME, result);
+		pr_err("dvb_register_frontend failed (errno = %d)\n",
+		       result);
 		goto fail_frontend;
 	}
 
@@ -436,8 +438,7 @@ static int dvb_register(struct au0828_dev *dev)
 	dvb->demux.stop_feed  = au0828_dvb_stop_feed;
 	result = dvb_dmx_init(&dvb->demux);
 	if (result < 0) {
-		printk(KERN_ERR "%s: dvb_dmx_init failed (errno = %d)\n",
-		       DRIVER_NAME, result);
+		pr_err("dvb_dmx_init failed (errno = %d)\n", result);
 		goto fail_dmx;
 	}
 
@@ -446,31 +447,29 @@ static int dvb_register(struct au0828_dev *dev)
 	dvb->dmxdev.capabilities = 0;
 	result = dvb_dmxdev_init(&dvb->dmxdev, &dvb->adapter);
 	if (result < 0) {
-		printk(KERN_ERR "%s: dvb_dmxdev_init failed (errno = %d)\n",
-		       DRIVER_NAME, result);
+		pr_err("dvb_dmxdev_init failed (errno = %d)\n", result);
 		goto fail_dmxdev;
 	}
 
 	dvb->fe_hw.source = DMX_FRONTEND_0;
 	result = dvb->demux.dmx.add_frontend(&dvb->demux.dmx, &dvb->fe_hw);
 	if (result < 0) {
-		printk(KERN_ERR "%s: add_frontend failed "
-		       "(DMX_FRONTEND_0, errno = %d)\n", DRIVER_NAME, result);
+		pr_err("add_frontend failed (DMX_FRONTEND_0, errno = %d)\n",
+		       result);
 		goto fail_fe_hw;
 	}
 
 	dvb->fe_mem.source = DMX_MEMORY_FE;
 	result = dvb->demux.dmx.add_frontend(&dvb->demux.dmx, &dvb->fe_mem);
 	if (result < 0) {
-		printk(KERN_ERR "%s: add_frontend failed "
-		       "(DMX_MEMORY_FE, errno = %d)\n", DRIVER_NAME, result);
+		pr_err("add_frontend failed (DMX_MEMORY_FE, errno = %d)\n",
+		       result);
 		goto fail_fe_mem;
 	}
 
 	result = dvb->demux.dmx.connect_frontend(&dvb->demux.dmx, &dvb->fe_hw);
 	if (result < 0) {
-		printk(KERN_ERR "%s: connect_frontend failed (errno = %d)\n",
-		       DRIVER_NAME, result);
+		pr_err("connect_frontend failed (errno = %d)\n", result);
 		goto fail_fe_conn;
 	}
 
@@ -530,8 +529,7 @@ void au0828_dvb_unregister(struct au0828_dev *dev)
 		for (i = 0; i < URB_COUNT; i++)
 			kfree(dev->dig_transfer_buffer[i]);
 	}
-
-
+	dvb->frontend = NULL;
 }
 
 /* All the DVB attach calls go here, this function get's modified
@@ -596,12 +594,11 @@ int au0828_dvb_register(struct au0828_dev *dev)
 		}
 		break;
 	default:
-		printk(KERN_WARNING "The frontend of your DVB/ATSC card "
-		       "isn't supported yet\n");
+		pr_warn("The frontend of your DVB/ATSC card isn't supported yet\n");
 		break;
 	}
 	if (NULL == dvb->frontend) {
-		printk(KERN_ERR "%s() Frontend initialization failed\n",
+		pr_err("%s() Frontend initialization failed\n",
 		       __func__);
 		return -1;
 	}
@@ -613,8 +610,49 @@ int au0828_dvb_register(struct au0828_dev *dev)
 	if (ret < 0) {
 		if (dvb->frontend->ops.release)
 			dvb->frontend->ops.release(dvb->frontend);
+		dvb->frontend = NULL;
 		return ret;
 	}
 
 	return 0;
 }
+
+void au0828_dvb_suspend(struct au0828_dev *dev)
+{
+	struct au0828_dvb *dvb = &dev->dvb;
+	int rc;
+
+	if (dvb->frontend) {
+		if (dev->urb_streaming) {
+			cancel_work_sync(&dev->restart_streaming);
+			/* Stop transport */
+			mutex_lock(&dvb->lock);
+			stop_urb_transfer(dev);
+			au0828_stop_transport(dev, 1);
+			mutex_unlock(&dvb->lock);
+			dev->need_urb_start = true;
+		}
+		/* suspend frontend - does tuner and fe to sleep */
+		rc = dvb_frontend_suspend(dvb->frontend);
+		pr_info("au0828_dvb_suspend(): Suspending DVB fe %d\n", rc);
+	}
+}
+
+void au0828_dvb_resume(struct au0828_dev *dev)
+{
+	struct au0828_dvb *dvb = &dev->dvb;
+	int rc;
+
+	if (dvb->frontend) {
+		/* resume frontend - does fe and tuner init */
+		rc = dvb_frontend_resume(dvb->frontend);
+		pr_info("au0828_dvb_resume(): Resuming DVB fe %d\n", rc);
+		if (dev->need_urb_start) {
+			/* Start transport */
+			mutex_lock(&dvb->lock);
+			au0828_start_transport(dev);
+			start_urb_transfer(dev);
+			mutex_unlock(&dvb->lock);
+		}
+	}
+}
diff --git a/drivers/media/usb/au0828/au0828-i2c.c b/drivers/media/usb/au0828/au0828-i2c.c
index daaeaf1b089c..ae7ac6669769 100644
--- a/drivers/media/usb/au0828/au0828-i2c.c
+++ b/drivers/media/usb/au0828/au0828-i2c.c
@@ -19,13 +19,14 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#include "au0828.h"
+
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/io.h>
 
-#include "au0828.h"
 #include "media/tuner.h"
 #include <media/v4l2-common.h>
 
@@ -340,7 +341,7 @@ static struct i2c_algorithm au0828_i2c_algo_template = {
 /* ----------------------------------------------------------------------- */
 
 static struct i2c_adapter au0828_i2c_adap_template = {
-	.name              = DRIVER_NAME,
+	.name              = KBUILD_MODNAME,
 	.owner             = THIS_MODULE,
 	.algo              = &au0828_i2c_algo_template,
 };
@@ -365,7 +366,7 @@ static void do_i2c_scan(char *name, struct i2c_client *c)
 		rc = i2c_master_recv(c, &buf, 0);
 		if (rc < 0)
 			continue;
-		printk(KERN_INFO "%s: i2c scan: found device @ 0x%x  [%s]\n",
+		pr_info("%s: i2c scan: found device @ 0x%x  [%s]\n",
 		       name, i << 1, i2c_devs[i] ? i2c_devs[i] : "???");
 	}
 }
@@ -381,7 +382,7 @@ int au0828_i2c_register(struct au0828_dev *dev)
 
 	dev->i2c_adap.dev.parent = &dev->usbdev->dev;
 
-	strlcpy(dev->i2c_adap.name, DRIVER_NAME,
+	strlcpy(dev->i2c_adap.name, KBUILD_MODNAME,
 		sizeof(dev->i2c_adap.name));
 
 	dev->i2c_adap.algo = &dev->i2c_algo;
@@ -396,11 +397,11 @@ int au0828_i2c_register(struct au0828_dev *dev)
 	dev->i2c_client.adapter = &dev->i2c_adap;
 
 	if (0 == dev->i2c_rc) {
-		printk(KERN_INFO "%s: i2c bus registered\n", DRIVER_NAME);
+		pr_info("i2c bus registered\n");
 		if (i2c_scan)
-			do_i2c_scan(DRIVER_NAME, &dev->i2c_client);
+			do_i2c_scan(KBUILD_MODNAME, &dev->i2c_client);
 	} else
-		printk(KERN_INFO "%s: i2c bus register FAILED\n", DRIVER_NAME);
+		pr_info("i2c bus register FAILED\n");
 
 	return dev->i2c_rc;
 }
diff --git a/drivers/media/usb/au0828/au0828-input.c b/drivers/media/usb/au0828/au0828-input.c
index fd0d3a90ce7d..63995f97dc65 100644
--- a/drivers/media/usb/au0828/au0828-input.c
+++ b/drivers/media/usb/au0828/au0828-input.c
@@ -17,6 +17,8 @@
   GNU General Public License for more details.
  */
 
+#include "au0828.h"
+
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/delay.h>
@@ -25,7 +27,9 @@
 #include <linux/slab.h>
 #include <media/rc-core.h>
 
-#include "au0828.h"
+static int disable_ir;
+module_param(disable_ir,        int, 0444);
+MODULE_PARM_DESC(disable_ir, "disable infrared remote support");
 
 struct au0828_rc {
 	struct au0828_dev *dev;
@@ -90,14 +94,19 @@ static int au8522_rc_read(struct au0828_rc *ir, u16 reg, int val,
 static int au8522_rc_andor(struct au0828_rc *ir, u16 reg, u8 mask, u8 value)
 {
 	int rc;
-	char buf;
+	char buf, oldbuf;
 
 	rc = au8522_rc_read(ir, reg, -1, &buf, 1);
 	if (rc < 0)
 		return rc;
 
+	oldbuf = buf;
 	buf = (buf & ~mask) | (value & mask);
 
+	/* Nothing to do, just return */
+	if (buf == oldbuf)
+		return 0;
+
 	return au8522_rc_write(ir, reg, buf);
 }
 
@@ -122,8 +131,11 @@ static int au0828_get_key_au8522(struct au0828_rc *ir)
 
 	/* Check IR int */
 	rc = au8522_rc_read(ir, 0xe1, -1, buf, 1);
-	if (rc < 0 || !(buf[0] & (1 << 4)))
+	if (rc < 0 || !(buf[0] & (1 << 4))) {
+		/* Be sure that IR is enabled */
+		au8522_rc_set(ir, 0xe0, 1 << 4);
 		return 0;
+	}
 
 	/* Something arrived. Get the data */
 	rc = au8522_rc_read(ir, 0xe3, 0x11, buf, sizeof(buf));
@@ -135,8 +147,6 @@ static int au0828_get_key_au8522(struct au0828_rc *ir)
 	/* Disable IR */
 	au8522_rc_clear(ir, 0xe0, 1 << 4);
 
-	usleep_range(45000, 46000);
-
 	/* Enable IR */
 	au8522_rc_set(ir, 0xe0, 1 << 4);
 
@@ -243,10 +253,10 @@ static void au0828_rc_stop(struct rc_dev *rc)
 {
 	struct au0828_rc *ir = rc->priv;
 
+	cancel_delayed_work_sync(&ir->work);
+
 	/* Disable IR */
 	au8522_rc_clear(ir, 0xe0, 1 << 4);
-
-	cancel_delayed_work_sync(&ir->work);
 }
 
 static int au0828_probe_i2c_ir(struct au0828_dev *dev)
@@ -273,7 +283,7 @@ int au0828_rc_register(struct au0828_dev *dev)
 	int err = -ENOMEM;
 	u16 i2c_rc_dev_addr = 0;
 
-	if (!dev->board.has_ir_i2c)
+	if (!dev->board.has_ir_i2c || disable_ir)
 		return 0;
 
 	i2c_rc_dev_addr = au0828_probe_i2c_ir(dev);
@@ -368,8 +378,13 @@ int au0828_rc_suspend(struct au0828_dev *dev)
 	if (!ir)
 		return 0;
 
+	pr_info("Stopping RC\n");
+
 	cancel_delayed_work_sync(&ir->work);
 
+	/* Disable IR */
+	au8522_rc_clear(ir, 0xe0, 1 << 4);
+
 	return 0;
 }
 
@@ -380,6 +395,11 @@ int au0828_rc_resume(struct au0828_dev *dev)
 	if (!ir)
 		return 0;
 
+	pr_info("Restarting RC\n");
+
+	/* Enable IR */
+	au8522_rc_set(ir, 0xe0, 1 << 4);
+
 	schedule_delayed_work(&ir->work, msecs_to_jiffies(ir->polling));
 
 	return 0;
diff --git a/drivers/media/usb/au0828/au0828-vbi.c b/drivers/media/usb/au0828/au0828-vbi.c
index 63f593070ee8..932d24f42b24 100644
--- a/drivers/media/usb/au0828/au0828-vbi.c
+++ b/drivers/media/usb/au0828/au0828-vbi.c
@@ -21,13 +21,13 @@
    02110-1301, USA.
  */
 
+#include "au0828.h"
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/slab.h>
 
-#include "au0828.h"
-
 static unsigned int vbibufs = 5;
 module_param(vbibufs, int, 0644);
 MODULE_PARM_DESC(vbibufs, "number of vbi buffers, range 2-32");
diff --git a/drivers/media/usb/au0828/au0828-video.c b/drivers/media/usb/au0828/au0828-video.c
index 98f7ea1d6d63..5f337b118bff 100644
--- a/drivers/media/usb/au0828/au0828-video.c
+++ b/drivers/media/usb/au0828/au0828-video.c
@@ -28,16 +28,16 @@
  *
  */
 
+#include "au0828.h"
+
 #include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/device.h>
-#include <linux/suspend.h>
 #include <media/v4l2-common.h>
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-event.h>
 #include <media/tuner.h>
-#include "au0828.h"
 #include "au0828-reg.h"
 
 static DEFINE_MUTEX(au0828_sysfs_lock);
@@ -53,7 +53,7 @@ MODULE_PARM_DESC(isoc_debug, "enable debug messages [isoc transfers]");
 #define au0828_isocdbg(fmt, arg...) \
 do {\
 	if (isoc_debug) { \
-		printk(KERN_INFO "au0828 %s :"fmt, \
+		pr_info("au0828 %s :"fmt, \
 		       __func__ , ##arg);	   \
 	} \
   } while (0)
@@ -106,12 +106,12 @@ static inline void print_err_status(struct au0828_dev *dev,
 static int check_dev(struct au0828_dev *dev)
 {
 	if (dev->dev_state & DEV_DISCONNECTED) {
-		printk(KERN_INFO "v4l2 ioctl: device not present\n");
+		pr_info("v4l2 ioctl: device not present\n");
 		return -ENODEV;
 	}
 
 	if (dev->dev_state & DEV_MISCONFIGURED) {
-		printk(KERN_INFO "v4l2 ioctl: device is misconfigured; "
+		pr_info("v4l2 ioctl: device is misconfigured; "
 		       "close and open it again\n");
 		return -EIO;
 	}
@@ -159,6 +159,7 @@ static void au0828_irq_callback(struct urb *urb)
 		au0828_isocdbg("urb resubmit failed (error=%i)\n",
 			       urb->status);
 	}
+	dev->stream_state = STREAM_ON;
 }
 
 /*
@@ -198,6 +199,8 @@ static void au0828_uninit_isoc(struct au0828_dev *dev)
 	dev->isoc_ctl.urb = NULL;
 	dev->isoc_ctl.transfer_buffer = NULL;
 	dev->isoc_ctl.num_bufs = 0;
+
+	dev->stream_state = STREAM_OFF;
 }
 
 /*
@@ -717,7 +720,7 @@ buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
 	if (VIDEOBUF_NEEDS_INIT == buf->vb.state) {
 		rc = videobuf_iolock(vq, &buf->vb, NULL);
 		if (rc < 0) {
-			printk(KERN_INFO "videobuf_iolock failed\n");
+			pr_info("videobuf_iolock failed\n");
 			goto fail;
 		}
 	}
@@ -730,7 +733,7 @@ buffer_prepare(struct videobuf_queue *vq, struct videobuf_buffer *vb,
 				      AU0828_MAX_ISO_BUFS, dev->max_pkt_size,
 				      au0828_isoc_copy);
 		if (rc < 0) {
-			printk(KERN_INFO "au0828_init_isoc failed\n");
+			pr_info("au0828_init_isoc failed\n");
 			goto fail;
 		}
 	}
@@ -801,7 +804,7 @@ static int au0828_analog_stream_enable(struct au0828_dev *d)
 		/* set au0828 interface0 to AS5 here again */
 		ret = usb_set_interface(d->usbdev, 0, 5);
 		if (ret < 0) {
-			printk(KERN_INFO "Au0828 can't set alt setting to 5!\n");
+			pr_info("Au0828 can't set alt setting to 5!\n");
 			return -EBUSY;
 		}
 	}
@@ -1090,7 +1093,7 @@ static int au0828_v4l2_close(struct file *filp)
 		   USB bandwidth */
 		ret = usb_set_interface(dev->usbdev, 0, 0);
 		if (ret < 0)
-			printk(KERN_INFO "Au0828 can't set alternate to 0!\n");
+			pr_info("Au0828 can't set alternate to 0!\n");
 	}
 	mutex_unlock(&dev->lock);
 
@@ -1344,7 +1347,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 		return rc;
 
 	if (videobuf_queue_is_busy(&fh->vb_vidq)) {
-		printk(KERN_INFO "%s queue busy\n", __func__);
+		pr_info("%s queue busy\n", __func__);
 		rc = -EBUSY;
 		goto out;
 	}
@@ -1868,6 +1871,69 @@ static int vidioc_dqbuf(struct file *file, void *priv, struct v4l2_buffer *b)
 	return rc;
 }
 
+void au0828_v4l2_suspend(struct au0828_dev *dev)
+{
+	struct urb *urb;
+	int i;
+
+	pr_info("stopping V4L2\n");
+
+	if (dev->stream_state == STREAM_ON) {
+		pr_info("stopping V4L2 active URBs\n");
+		au0828_analog_stream_disable(dev);
+		/* stop urbs */
+		for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
+			urb = dev->isoc_ctl.urb[i];
+			if (urb) {
+				if (!irqs_disabled())
+					usb_kill_urb(urb);
+				else
+					usb_unlink_urb(urb);
+			}
+		}
+	}
+
+	if (dev->vid_timeout_running)
+		del_timer_sync(&dev->vid_timeout);
+	if (dev->vbi_timeout_running)
+		del_timer_sync(&dev->vbi_timeout);
+}
+
+void au0828_v4l2_resume(struct au0828_dev *dev)
+{
+	int i, rc;
+
+	pr_info("restarting V4L2\n");
+
+	if (dev->stream_state == STREAM_ON) {
+		au0828_stream_interrupt(dev);
+		au0828_init_tuner(dev);
+	}
+
+	if (dev->vid_timeout_running)
+		mod_timer(&dev->vid_timeout, jiffies + (HZ / 10));
+	if (dev->vbi_timeout_running)
+		mod_timer(&dev->vbi_timeout, jiffies + (HZ / 10));
+
+	/* If we were doing ac97 instead of i2s, it would go here...*/
+	au0828_i2s_init(dev);
+
+	au0828_analog_stream_enable(dev);
+
+	if (!(dev->stream_state == STREAM_ON)) {
+		au0828_analog_stream_reset(dev);
+		/* submit urbs */
+		for (i = 0; i < dev->isoc_ctl.num_bufs; i++) {
+			rc = usb_submit_urb(dev->isoc_ctl.urb[i], GFP_ATOMIC);
+			if (rc) {
+				au0828_isocdbg("submit of urb %i failed (error=%i)\n",
+					       i, rc);
+				au0828_uninit_isoc(dev);
+			}
+		}
+	}
+}
+
 static struct v4l2_file_operations au0828_v4l_fops = {
 	.owner      = THIS_MODULE,
 	.open       = au0828_v4l2_open,
@@ -1939,7 +2005,7 @@ int au0828_analog_register(struct au0828_dev *dev,
 	retval = usb_set_interface(dev->usbdev,
 			interface->cur_altsetting->desc.bInterfaceNumber, 5);
 	if (retval != 0) {
-		printk(KERN_INFO "Failure setting usb interface0 to as5\n");
+		pr_info("Failure setting usb interface0 to as5\n");
 		return retval;
 	}
 
@@ -1963,7 +2029,7 @@ int au0828_analog_register(struct au0828_dev *dev,
 		}
 	}
 	if (!(dev->isoc_in_endpointaddr)) {
-		printk(KERN_INFO "Could not locate isoc endpoint\n");
+		pr_info("Could not locate isoc endpoint\n");
 		kfree(dev);
 		return -ENODEV;
 	}
diff --git a/drivers/media/usb/au0828/au0828.h b/drivers/media/usb/au0828/au0828.h
index 96bec05d7dac..36815a369c68 100644
--- a/drivers/media/usb/au0828/au0828.h
+++ b/drivers/media/usb/au0828/au0828.h
@@ -19,6 +19,8 @@
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
+#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
+
 #include <linux/usb.h>
 #include <linux/i2c.h>
 #include <linux/i2c-algo-bit.h>
@@ -42,7 +44,6 @@
 #include "au0828-reg.h"
 #include "au0828-cards.h"
 
-#define DRIVER_NAME "au0828"
 #define URB_COUNT   16
 #define URB_BUFSIZE (0xe522)
 
@@ -89,6 +90,7 @@ struct au0828_board {
 	unsigned char tuner_addr;
 	unsigned char i2c_clk_divider;
 	unsigned char has_ir_i2c:1;
+	unsigned char has_analog:1;
 	struct au0828_input input[AU0828_MAX_INPUT];
 
 };
@@ -266,8 +268,8 @@ struct au0828_dev {
 	char *transfer_buffer[AU0828_MAX_ISO_BUFS];/* transfer buffers for isoc
 						   transfer */
 
-	/* USB / URB Related */
-	int		urb_streaming;
+	/* DVB USB / URB Related */
+	bool		urb_streaming, need_urb_start;
 	struct urb	*urbs[URB_COUNT];
 
 	/* Preallocated transfer digital transfer buffers */
@@ -311,22 +313,38 @@ int au0828_analog_register(struct au0828_dev *dev,
 			   struct usb_interface *interface);
 int au0828_analog_stream_disable(struct au0828_dev *d);
 void au0828_analog_unregister(struct au0828_dev *dev);
+#ifdef CONFIG_VIDEO_AU0828_V4L2
+void au0828_v4l2_suspend(struct au0828_dev *dev);
+void au0828_v4l2_resume(struct au0828_dev *dev);
+#else
+static inline void au0828_v4l2_suspend(struct au0828_dev *dev) { };
+static inline void au0828_v4l2_resume(struct au0828_dev *dev) { };
+#endif
 
 /* ----------------------------------------------------------- */
 /* au0828-dvb.c */
 extern int au0828_dvb_register(struct au0828_dev *dev);
 extern void au0828_dvb_unregister(struct au0828_dev *dev);
+void au0828_dvb_suspend(struct au0828_dev *dev);
+void au0828_dvb_resume(struct au0828_dev *dev);
 
 /* au0828-vbi.c */
 extern struct videobuf_queue_ops au0828_vbi_qops;
 
 #define dprintk(level, fmt, arg...)\
 	do { if (au0828_debug & level)\
-		printk(KERN_DEBUG DRIVER_NAME "/0: " fmt, ## arg);\
+		printk(KERN_DEBUG pr_fmt(fmt), ## arg);\
 	} while (0)
 
 /* au0828-input.c */
-int au0828_rc_register(struct au0828_dev *dev);
-void au0828_rc_unregister(struct au0828_dev *dev);
-int au0828_rc_suspend(struct au0828_dev *dev);
-int au0828_rc_resume(struct au0828_dev *dev);
+#ifdef CONFIG_VIDEO_AU0828_RC
+extern int au0828_rc_register(struct au0828_dev *dev);
+extern void au0828_rc_unregister(struct au0828_dev *dev);
+extern int au0828_rc_suspend(struct au0828_dev *dev);
+extern int au0828_rc_resume(struct au0828_dev *dev);
+#else
+static inline int au0828_rc_register(struct au0828_dev *dev) { return 0; }
+static inline void au0828_rc_unregister(struct au0828_dev *dev) { }
+static inline int au0828_rc_suspend(struct au0828_dev *dev) { return 0; }
+static inline int au0828_rc_resume(struct au0828_dev *dev) { return 0; }
+#endif
diff --git a/drivers/media/usb/cx231xx/cx231xx-avcore.c b/drivers/media/usb/cx231xx/cx231xx-avcore.c
index a428c10e1a16..40a69879fc0a 100644
--- a/drivers/media/usb/cx231xx/cx231xx-avcore.c
+++ b/drivers/media/usb/cx231xx/cx231xx-avcore.c
@@ -1595,7 +1595,7 @@ void cx231xx_set_DIF_bandpass(struct cx231xx *dev, u32 if_freq,
 		if_freq = 16000000;
 	}
 
-	cx231xx_info("Enter IF=%zd\n",
+	cx231xx_info("Enter IF=%zu\n",
 			ARRAY_SIZE(Dif_set_array));
 	for (i = 0; i < ARRAY_SIZE(Dif_set_array); i++) {
 		if (Dif_set_array[i].if_freq == if_freq) {
@@ -2223,7 +2223,7 @@ int cx231xx_set_power_mode(struct cx231xx *dev, enum AV_MODE mode)
 	if (status < 0)
 		return status;
 
-	tmp = le32_to_cpu(*((u32 *) value));
+	tmp = le32_to_cpu(*((__le32 *) value));
 
 	switch (mode) {
 	case POLARIS_AVMODE_ENXTERNAL_AV:
@@ -2444,7 +2444,7 @@ int cx231xx_power_suspend(struct cx231xx *dev)
 	if (status > 0)
 		return status;
 
-	tmp = le32_to_cpu(*((u32 *) value));
+	tmp = le32_to_cpu(*((__le32 *) value));
 	tmp &= (~PWR_MODE_MASK);
 
 	value[0] = (u8) tmp;
@@ -2472,7 +2472,7 @@ int cx231xx_start_stream(struct cx231xx *dev, u32 ep_mask)
 	if (status < 0)
 		return status;
 
-	tmp = le32_to_cpu(*((u32 *) value));
+	tmp = le32_to_cpu(*((__le32 *) value));
 	tmp |= ep_mask;
 	value[0] = (u8) tmp;
 	value[1] = (u8) (tmp >> 8);
@@ -2497,7 +2497,7 @@ int cx231xx_stop_stream(struct cx231xx *dev, u32 ep_mask)
 	if (status < 0)
 		return status;
 
-	tmp = le32_to_cpu(*((u32 *) value));
+	tmp = le32_to_cpu(*((__le32 *) value));
 	tmp &= (~ep_mask);
 	value[0] = (u8) tmp;
 	value[1] = (u8) (tmp >> 8);
@@ -2644,7 +2644,7 @@ static int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u32 gpio_val)
 {
 	int status = 0;
 
-	gpio_val = cpu_to_le32(gpio_val);
+	gpio_val = (__force u32)cpu_to_le32(gpio_val);
 	status = cx231xx_send_gpio_cmd(dev, gpio_bit, (u8 *)&gpio_val, 4, 0, 0);
 
 	return status;
@@ -2652,7 +2652,7 @@ static int cx231xx_set_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u32 gpio_val)
 
 static int cx231xx_get_gpio_bit(struct cx231xx *dev, u32 gpio_bit, u32 *gpio_val)
 {
-	u32 tmp;
+	__le32 tmp;
 	int status = 0;
 
 	status = cx231xx_send_gpio_cmd(dev, gpio_bit, (u8 *)&tmp, 4, 0, 1);
diff --git a/drivers/media/usb/cx231xx/cx231xx-cards.c b/drivers/media/usb/cx231xx/cx231xx-cards.c
index 8039b769f258..791f00c6276b 100644
--- a/drivers/media/usb/cx231xx/cx231xx-cards.c
+++ b/drivers/media/usb/cx231xx/cx231xx-cards.c
@@ -705,7 +705,7 @@ struct cx231xx_board cx231xx_boards[] = {
 		},
 	},
 	[CX231XX_BOARD_HAUPPAUGE_930C_HD_1113xx] = {
-		.name = "Hauppauge WinTV 930C-HD (1113xx) / PCTV QuatroStick 521e",
+		.name = "Hauppauge WinTV 930C-HD (1113xx) / HVR-900H (111xxx) / PCTV QuatroStick 521e",
 		.tuner_type = TUNER_NXP_TDA18271,
 		.tuner_addr = 0x60,
 		.tuner_gpio = RDE250_XCV_TUNER,
@@ -744,7 +744,7 @@ struct cx231xx_board cx231xx_boards[] = {
 		} },
 	},
 	[CX231XX_BOARD_HAUPPAUGE_930C_HD_1114xx] = {
-		.name = "Hauppauge WinTV 930C-HD (1114xx) / PCTV QuatroStick 522e",
+		.name = "Hauppauge WinTV 930C-HD (1114xx) / HVR-901H (1114xx) / PCTV QuatroStick 522e",
 		.tuner_type = TUNER_ABSENT,
 		.tuner_addr = 0x60,
 		.tuner_gpio = RDE250_XCV_TUNER,
@@ -815,6 +815,12 @@ struct usb_device_id cx231xx_id_table[] = {
 	 .driver_info = CX231XX_BOARD_HAUPPAUGE_930C_HD_1113xx},
 	{USB_DEVICE(0x2040, 0xb131),
 	 .driver_info = CX231XX_BOARD_HAUPPAUGE_930C_HD_1114xx},
+	/* Hauppauge WinTV-HVR-900-H */
+	{USB_DEVICE(0x2040, 0xb138),
+	 .driver_info = CX231XX_BOARD_HAUPPAUGE_930C_HD_1113xx},
+	/* Hauppauge WinTV-HVR-901-H */
+	{USB_DEVICE(0x2040, 0xb139),
+	 .driver_info = CX231XX_BOARD_HAUPPAUGE_930C_HD_1114xx},
 	{USB_DEVICE(0x2040, 0xb140),
 	 .driver_info = CX231XX_BOARD_HAUPPAUGE_EXETER},
 	{USB_DEVICE(0x2040, 0xc200),
diff --git a/drivers/media/usb/cx231xx/cx231xx-core.c b/drivers/media/usb/cx231xx/cx231xx-core.c
index 513194aa6561..180103e48036 100644
--- a/drivers/media/usb/cx231xx/cx231xx-core.c
+++ b/drivers/media/usb/cx231xx/cx231xx-core.c
@@ -1491,7 +1491,7 @@ int cx231xx_mode_register(struct cx231xx *dev, u16 address, u32 mode)
 	if (status < 0)
 		return status;
 
-	tmp = le32_to_cpu(*((u32 *) value));
+	tmp = le32_to_cpu(*((__le32 *) value));
 	tmp |= mode;
 
 	value[0] = (u8) tmp;
diff --git a/drivers/media/usb/cx231xx/cx231xx-dvb.c b/drivers/media/usb/cx231xx/cx231xx-dvb.c
index 1fa79741d199..6c7b5e250eed 100644
--- a/drivers/media/usb/cx231xx/cx231xx-dvb.c
+++ b/drivers/media/usb/cx231xx/cx231xx-dvb.c
@@ -403,8 +403,6 @@ static int attach_xc5000(u8 addr, struct cx231xx *dev)
 
 int cx231xx_set_analog_freq(struct cx231xx *dev, u32 freq)
 {
-	int status = 0;
-
 	if ((dev->dvb != NULL) && (dev->dvb->frontend != NULL)) {
 
 		struct dvb_tuner_ops *dops = &dev->dvb->frontend->ops.tuner_ops;
@@ -423,7 +421,7 @@ int cx231xx_set_analog_freq(struct cx231xx *dev, u32 freq)
 
 	}
 
-	return status;
+	return 0;
 }
 
 int cx231xx_reset_analog_tuner(struct cx231xx *dev)
@@ -740,7 +738,7 @@ static int dvb_init(struct cx231xx *dev)
 			goto out_free;
 		}
 
-		dev->dvb->frontend->ops.i2c_gate_ctrl = 0;
+		dev->dvb->frontend->ops.i2c_gate_ctrl = NULL;
 
 		/* define general-purpose callback pointer */
 		dvb->frontend->callback = cx231xx_tuner_callback;
@@ -773,7 +771,7 @@ static int dvb_init(struct cx231xx *dev)
 			goto out_free;
 		}
 
-		dev->dvb->frontend->ops.i2c_gate_ctrl = 0;
+		dev->dvb->frontend->ops.i2c_gate_ctrl = NULL;
 
 		/* define general-purpose callback pointer */
 		dvb->frontend->callback = cx231xx_tuner_callback;
diff --git a/drivers/media/usb/dvb-usb-v2/Kconfig b/drivers/media/usb/dvb-usb-v2/Kconfig
index 66645b02c854..5b34323ad207 100644
--- a/drivers/media/usb/dvb-usb-v2/Kconfig
+++ b/drivers/media/usb/dvb-usb-v2/Kconfig
@@ -141,3 +141,10 @@ config DVB_USB_RTL28XXU
 	help
 	  Say Y here to support the Realtek RTL28xxU DVB USB receiver.
 
+config DVB_USB_DVBSKY
+	tristate "DVBSky USB support"
+	depends on DVB_USB_V2
+	select DVB_M88DS3103 if MEDIA_SUBDRV_AUTOSELECT
+	select MEDIA_TUNER_M88TS2022 if MEDIA_SUBDRV_AUTOSELECT
+	help
+	  Say Y here to support the USB receivers from DVBSky.
diff --git a/drivers/media/usb/dvb-usb-v2/Makefile b/drivers/media/usb/dvb-usb-v2/Makefile
index bc38f03394cd..f10d4df0eae5 100644
--- a/drivers/media/usb/dvb-usb-v2/Makefile
+++ b/drivers/media/usb/dvb-usb-v2/Makefile
@@ -37,6 +37,9 @@ obj-$(CONFIG_DVB_USB_MXL111SF) += mxl111sf-tuner.o
 dvb-usb-rtl28xxu-objs := rtl28xxu.o
 obj-$(CONFIG_DVB_USB_RTL28XXU) += dvb-usb-rtl28xxu.o
 
+dvb-usb-dvbsky-objs := dvbsky.o
+obj-$(CONFIG_DVB_USB_DVBSKY) += dvb-usb-dvbsky.o
+
 ccflags-y += -I$(srctree)/drivers/media/dvb-core
 ccflags-y += -I$(srctree)/drivers/media/dvb-frontends
 ccflags-y += -I$(srctree)/drivers/media/tuners
diff --git a/drivers/media/usb/dvb-usb-v2/af9015.c b/drivers/media/usb/dvb-usb-v2/af9015.c
index 5ca738ab44e0..16c0b7d4f8e7 100644
--- a/drivers/media/usb/dvb-usb-v2/af9015.c
+++ b/drivers/media/usb/dvb-usb-v2/af9015.c
@@ -419,7 +419,7 @@ static int af9015_eeprom_hash(struct dvb_usb_device *d)
 	/* calculate checksum */
 	for (i = 0; i < AF9015_EEPROM_SIZE / sizeof(u32); i++) {
 		state->eeprom_sum *= GOLDEN_RATIO_PRIME_32;
-		state->eeprom_sum += le32_to_cpu(((u32 *)buf)[i]);
+		state->eeprom_sum += le32_to_cpu(((__le32 *)buf)[i]);
 	}
 
 	for (i = 0; i < AF9015_EEPROM_SIZE; i += 16)
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.c b/drivers/media/usb/dvb-usb-v2/af9035.c
index 75ec1c659fdd..00758c83eec7 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.c
+++ b/drivers/media/usb/dvb-usb-v2/af9035.c
@@ -193,6 +193,92 @@ static int af9035_wr_reg_mask(struct dvb_usb_device *d, u32 reg, u8 val,
 	return af9035_wr_regs(d, reg, &val, 1);
 }
 
+static int af9035_add_i2c_dev(struct dvb_usb_device *d, char *type, u8 addr,
+		void *platform_data, struct i2c_adapter *adapter)
+{
+	int ret, num;
+	struct state *state = d_to_priv(d);
+	struct i2c_client *client;
+	struct i2c_board_info board_info = {
+		.addr = addr,
+		.platform_data = platform_data,
+	};
+
+	strlcpy(board_info.type, type, I2C_NAME_SIZE);
+
+	/* find first free client */
+	for (num = 0; num < AF9035_I2C_CLIENT_MAX; num++) {
+		if (state->i2c_client[num] == NULL)
+			break;
+	}
+
+	dev_dbg(&d->udev->dev, "%s: num=%d\n", __func__, num);
+
+	if (num == AF9035_I2C_CLIENT_MAX) {
+		dev_err(&d->udev->dev, "%s: I2C client out of index\n",
+				KBUILD_MODNAME);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	request_module(board_info.type);
+
+	/* register I2C device */
+	client = i2c_new_device(adapter, &board_info);
+	if (client == NULL || client->dev.driver == NULL) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	/* increase I2C driver usage count */
+	if (!try_module_get(client->dev.driver->owner)) {
+		i2c_unregister_device(client);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	state->i2c_client[num] = client;
+	return 0;
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static void af9035_del_i2c_dev(struct dvb_usb_device *d)
+{
+	int num;
+	struct state *state = d_to_priv(d);
+	struct i2c_client *client;
+
+	/* find last used client */
+	num = AF9035_I2C_CLIENT_MAX;
+	while (num--) {
+		if (state->i2c_client[num] != NULL)
+			break;
+	}
+
+	dev_dbg(&d->udev->dev, "%s: num=%d\n", __func__, num);
+
+	if (num == -1) {
+		dev_err(&d->udev->dev, "%s: I2C client out of index\n",
+				KBUILD_MODNAME);
+		goto err;
+	}
+
+	client = state->i2c_client[num];
+
+	/* decrease I2C driver usage count */
+	module_put(client->dev.driver->owner);
+
+	/* unregister I2C device */
+	i2c_unregister_device(client);
+
+	state->i2c_client[num] = NULL;
+	return;
+err:
+	dev_dbg(&d->udev->dev, "%s: failed\n", __func__);
+}
+
 static int af9035_i2c_master_xfer(struct i2c_adapter *adap,
 		struct i2c_msg msg[], int num)
 {
@@ -204,7 +290,7 @@ static int af9035_i2c_master_xfer(struct i2c_adapter *adap,
 		return -EAGAIN;
 
 	/*
-	 * I2C sub header is 5 bytes long. Meaning of those bytes are:
+	 * AF9035 I2C sub header is 5 bytes long. Meaning of those bytes are:
 	 * 0: data len
 	 * 1: I2C addr << 1
 	 * 2: reg addr len
@@ -218,110 +304,156 @@ static int af9035_i2c_master_xfer(struct i2c_adapter *adap,
 	 * NOTE: As a firmware knows tuner type there is very small possibility
 	 * there could be some tuner I2C hacks done by firmware and this may
 	 * lead problems if firmware expects those bytes are used.
+	 *
+	 * TODO: Here is few hacks. AF9035 chip integrates AF9033 demodulator.
+	 * IT9135 chip integrates AF9033 demodulator and RF tuner. For dual
+	 * tuner devices, there is also external AF9033 demodulator connected
+	 * via external I2C bus. All AF9033 demod I2C traffic, both single and
+	 * dual tuner configuration, is covered by firmware - actual USB IO
+	 * looks just like a memory access.
+	 * In case of IT913x chip, there is own tuner driver. It is implemented
+	 * currently as a I2C driver, even tuner IP block is likely build
+	 * directly into the demodulator memory space and there is no own I2C
+	 * bus. I2C subsystem does not allow register multiple devices to same
+	 * bus, having same slave address. Due to that we reuse demod address,
+	 * shifted by one bit, on that case.
+	 *
+	 * For IT930x we use a different command and the sub header is
+	 * different as well:
+	 * 0: data len
+	 * 1: I2C bus (0x03 seems to be only value used)
+	 * 2: I2C addr << 1
 	 */
-	if (num == 2 && !(msg[0].flags & I2C_M_RD) &&
-			(msg[1].flags & I2C_M_RD)) {
+#define AF9035_IS_I2C_XFER_WRITE_READ(_msg, _num) \
+	(_num == 2 && !(_msg[0].flags & I2C_M_RD) && (_msg[1].flags & I2C_M_RD))
+#define AF9035_IS_I2C_XFER_WRITE(_msg, _num) \
+	(_num == 1 && !(_msg[0].flags & I2C_M_RD))
+#define AF9035_IS_I2C_XFER_READ(_msg, _num) \
+	(_num == 1 && (_msg[0].flags & I2C_M_RD))
+
+	if (AF9035_IS_I2C_XFER_WRITE_READ(msg, num)) {
 		if (msg[0].len > 40 || msg[1].len > 40) {
 			/* TODO: correct limits > 40 */
 			ret = -EOPNOTSUPP;
-		} else if ((msg[0].addr == state->af9033_config[0].i2c_addr) ||
-			   (msg[0].addr == state->af9033_config[1].i2c_addr)) {
+		} else if ((msg[0].addr == state->af9033_i2c_addr[0]) ||
+			   (msg[0].addr == state->af9033_i2c_addr[1]) ||
+			   (state->chip_type == 0x9135)) {
 			/* demod access via firmware interface */
 			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
 					msg[0].buf[2];
 
-			if (msg[0].addr == state->af9033_config[1].i2c_addr)
+			if (msg[0].addr == state->af9033_i2c_addr[1] ||
+			    msg[0].addr == (state->af9033_i2c_addr[1] >> 1))
 				reg |= 0x100000;
 
 			ret = af9035_rd_regs(d, reg, &msg[1].buf[0],
 					msg[1].len);
 		} else {
-			/* I2C */
+			/* I2C write + read */
 			u8 buf[MAX_XFER_SIZE];
 			struct usb_req req = { CMD_I2C_RD, 0, 5 + msg[0].len,
 					buf, msg[1].len, msg[1].buf };
 
-			if (5 + msg[0].len > sizeof(buf)) {
-				dev_warn(&d->udev->dev,
-					 "%s: i2c xfer: len=%d is too big!\n",
-					 KBUILD_MODNAME, msg[0].len);
-				ret = -EOPNOTSUPP;
-				goto unlock;
+			if (state->chip_type == 0x9306) {
+				req.cmd = CMD_GENERIC_I2C_RD;
+				req.wlen = 3 + msg[0].len;
 			}
 			req.mbox |= ((msg[0].addr & 0x80)  >>  3);
+
 			buf[0] = msg[1].len;
-			buf[1] = msg[0].addr << 1;
-			buf[2] = 0x00; /* reg addr len */
-			buf[3] = 0x00; /* reg addr MSB */
-			buf[4] = 0x00; /* reg addr LSB */
-			memcpy(&buf[5], msg[0].buf, msg[0].len);
+			if (state->chip_type == 0x9306) {
+				buf[1] = 0x03; /* I2C bus */
+				buf[2] = msg[0].addr << 1;
+				memcpy(&buf[3], msg[0].buf, msg[0].len);
+			} else {
+				buf[1] = msg[0].addr << 1;
+				buf[2] = 0x00; /* reg addr len */
+				buf[3] = 0x00; /* reg addr MSB */
+				buf[4] = 0x00; /* reg addr LSB */
+				memcpy(&buf[5], msg[0].buf, msg[0].len);
+			}
 			ret = af9035_ctrl_msg(d, &req);
 		}
-	} else if (num == 1 && !(msg[0].flags & I2C_M_RD)) {
+	} else if (AF9035_IS_I2C_XFER_WRITE(msg, num)) {
 		if (msg[0].len > 40) {
 			/* TODO: correct limits > 40 */
 			ret = -EOPNOTSUPP;
-		} else if ((msg[0].addr == state->af9033_config[0].i2c_addr) ||
-			   (msg[0].addr == state->af9033_config[1].i2c_addr)) {
+		} else if ((msg[0].addr == state->af9033_i2c_addr[0]) ||
+			   (msg[0].addr == state->af9033_i2c_addr[1]) ||
+			   (state->chip_type == 0x9135)) {
 			/* demod access via firmware interface */
 			u32 reg = msg[0].buf[0] << 16 | msg[0].buf[1] << 8 |
 					msg[0].buf[2];
 
-			if (msg[0].addr == state->af9033_config[1].i2c_addr)
+			if (msg[0].addr == state->af9033_i2c_addr[1] ||
+			    msg[0].addr == (state->af9033_i2c_addr[1] >> 1))
 				reg |= 0x100000;
 
 			ret = af9035_wr_regs(d, reg, &msg[0].buf[3],
 					msg[0].len - 3);
 		} else {
-			/* I2C */
+			/* I2C write */
 			u8 buf[MAX_XFER_SIZE];
 			struct usb_req req = { CMD_I2C_WR, 0, 5 + msg[0].len,
 					buf, 0, NULL };
 
-			if (5 + msg[0].len > sizeof(buf)) {
-				dev_warn(&d->udev->dev,
-					 "%s: i2c xfer: len=%d is too big!\n",
-					 KBUILD_MODNAME, msg[0].len);
-				ret = -EOPNOTSUPP;
-				goto unlock;
+			if (state->chip_type == 0x9306) {
+				req.cmd = CMD_GENERIC_I2C_WR;
+				req.wlen = 3 + msg[0].len;
 			}
+
 			req.mbox |= ((msg[0].addr & 0x80)  >>  3);
 			buf[0] = msg[0].len;
-			buf[1] = msg[0].addr << 1;
-			buf[2] = 0x00; /* reg addr len */
-			buf[3] = 0x00; /* reg addr MSB */
-			buf[4] = 0x00; /* reg addr LSB */
-			memcpy(&buf[5], msg[0].buf, msg[0].len);
+			if (state->chip_type == 0x9306) {
+				buf[1] = 0x03; /* I2C bus */
+				buf[2] = msg[0].addr << 1;
+				memcpy(&buf[3], msg[0].buf, msg[0].len);
+			} else {
+				buf[1] = msg[0].addr << 1;
+				buf[2] = 0x00; /* reg addr len */
+				buf[3] = 0x00; /* reg addr MSB */
+				buf[4] = 0x00; /* reg addr LSB */
+				memcpy(&buf[5], msg[0].buf, msg[0].len);
+			}
 			ret = af9035_ctrl_msg(d, &req);
 		}
-	} else if (num == 1 && (msg[0].flags & I2C_M_RD)) {
+	} else if (AF9035_IS_I2C_XFER_READ(msg, num)) {
 		if (msg[0].len > 40) {
 			/* TODO: correct limits > 40 */
 			ret = -EOPNOTSUPP;
 		} else {
-			/* I2C */
+			/* I2C read */
 			u8 buf[5];
 			struct usb_req req = { CMD_I2C_RD, 0, sizeof(buf),
-					buf, msg[0].len, msg[0].buf };
+						buf, msg[0].len, msg[0].buf };
+
+			if (state->chip_type == 0x9306) {
+				req.cmd = CMD_GENERIC_I2C_RD;
+				req.wlen = 3;
+			}
 			req.mbox |= ((msg[0].addr & 0x80)  >>  3);
 			buf[0] = msg[0].len;
-			buf[1] = msg[0].addr << 1;
-			buf[2] = 0x00; /* reg addr len */
-			buf[3] = 0x00; /* reg addr MSB */
-			buf[4] = 0x00; /* reg addr LSB */
+			if (state->chip_type == 0x9306) {
+				buf[1] = 0x03; /* I2C bus */
+				buf[2] = msg[0].addr << 1;
+			} else {
+				buf[1] = msg[0].addr << 1;
+				buf[2] = 0x00; /* reg addr len */
+				buf[3] = 0x00; /* reg addr MSB */
+				buf[4] = 0x00; /* reg addr LSB */
+			}
 			ret = af9035_ctrl_msg(d, &req);
 		}
 	} else {
 		/*
 		 * We support only three kind of I2C transactions:
-		 * 1) 1 x read + 1 x write (repeated start)
+		 * 1) 1 x write + 1 x read (repeated start)
 		 * 2) 1 x write
 		 * 3) 1 x read
 		 */
 		ret = -EOPNOTSUPP;
 	}
 
-unlock:
 	mutex_unlock(&d->i2c_mutex);
 
 	if (ret < 0)
@@ -371,6 +503,9 @@ static int af9035_identify_state(struct dvb_usb_device *d, const char **name)
 		else
 			*name = AF9035_FIRMWARE_IT9135_V1;
 		state->eeprom_addr = EEPROM_BASE_IT9135;
+	} else if (state->chip_type == 0x9306) {
+		*name = AF9035_FIRMWARE_IT9303;
+		state->eeprom_addr = EEPROM_BASE_IT9135;
 	} else {
 		*name = AF9035_FIRMWARE_AF9035;
 		state->eeprom_addr = EEPROM_BASE_AF9035;
@@ -536,6 +671,7 @@ static int af9035_download_firmware(struct dvb_usb_device *d,
 	u8 tmp;
 	struct usb_req req = { 0, 0, 0, NULL, 0, NULL };
 	struct usb_req req_fw_ver = { CMD_FW_QUERYINFO, 0, 1, wbuf, 4, rbuf };
+
 	dev_dbg(&d->udev->dev, "%s:\n", __func__);
 
 	/*
@@ -579,7 +715,8 @@ static int af9035_download_firmware(struct dvb_usb_device *d,
 		if (!tmp)
 			tmp = 0x3a;
 
-		if (state->chip_type == 0x9135) {
+		if ((state->chip_type == 0x9135) ||
+				(state->chip_type == 0x9306)) {
 			ret = af9035_wr_reg(d, 0x004bfb, tmp);
 			if (ret < 0)
 				goto err;
@@ -640,23 +777,26 @@ static int af9035_read_config(struct dvb_usb_device *d)
 	u16 tmp16, addr;
 
 	/* demod I2C "address" */
-	state->af9033_config[0].i2c_addr = 0x38;
-	state->af9033_config[1].i2c_addr = 0x3a;
+	state->af9033_i2c_addr[0] = 0x38;
+	state->af9033_i2c_addr[1] = 0x3a;
 	state->af9033_config[0].adc_multiplier = AF9033_ADC_MULTIPLIER_2X;
 	state->af9033_config[1].adc_multiplier = AF9033_ADC_MULTIPLIER_2X;
 	state->af9033_config[0].ts_mode = AF9033_TS_MODE_USB;
 	state->af9033_config[1].ts_mode = AF9033_TS_MODE_SERIAL;
 
-	/* eeprom memory mapped location */
 	if (state->chip_type == 0x9135) {
+		/* feed clock for integrated RF tuner */
+		state->af9033_config[0].dyn0_clk = true;
+		state->af9033_config[1].dyn0_clk = true;
+
 		if (state->chip_version == 0x02) {
 			state->af9033_config[0].tuner = AF9033_TUNER_IT9135_60;
 			state->af9033_config[1].tuner = AF9033_TUNER_IT9135_60;
-			tmp16 = 0x00461d;
+			tmp16 = 0x00461d; /* eeprom memory mapped location */
 		} else {
 			state->af9033_config[0].tuner = AF9033_TUNER_IT9135_38;
 			state->af9033_config[1].tuner = AF9033_TUNER_IT9135_38;
-			tmp16 = 0x00461b;
+			tmp16 = 0x00461b; /* eeprom memory mapped location */
 		}
 
 		/* check if eeprom exists */
@@ -668,8 +808,16 @@ static int af9035_read_config(struct dvb_usb_device *d)
 			dev_dbg(&d->udev->dev, "%s: no eeprom\n", __func__);
 			goto skip_eeprom;
 		}
+	} else if (state->chip_type == 0x9306) {
+		/*
+		 * IT930x is an USB bridge, only single demod-single tuner
+		 * configurations seen so far.
+		 */
+		return 0;
 	}
 
+
+
 	/* check if there is dual tuners */
 	ret = af9035_rd_reg(d, state->eeprom_addr + EEPROM_TS_MODE, &tmp);
 	if (ret < 0)
@@ -690,7 +838,7 @@ static int af9035_read_config(struct dvb_usb_device *d)
 			goto err;
 
 		if (tmp)
-			state->af9033_config[1].i2c_addr = tmp;
+			state->af9033_i2c_addr[1] = tmp;
 
 		dev_dbg(&d->udev->dev, "%s: 2nd demod I2C addr=%02x\n",
 				__func__, tmp);
@@ -799,25 +947,6 @@ static int af9035_read_config(struct dvb_usb_device *d)
 		addr += 0x10; /* shift for the 2nd tuner params */
 	}
 
-	/*
-	 * These AVerMedia devices has a bad EEPROM content :-(
-	 * Override some wrong values here.
-	 */
-	if (le16_to_cpu(d->udev->descriptor.idVendor) == USB_VID_AVERMEDIA) {
-		switch (le16_to_cpu(d->udev->descriptor.idProduct)) {
-		case USB_PID_AVERMEDIA_A835B_1835:
-		case USB_PID_AVERMEDIA_A835B_2835:
-		case USB_PID_AVERMEDIA_A835B_3835:
-			dev_info(&d->udev->dev,
-				 "%s: overriding tuner from %02x to %02x\n",
-				 KBUILD_MODNAME, state->af9033_config[0].tuner,
-				 AF9033_TUNER_IT9135_60);
-
-			state->af9033_config[0].tuner = AF9033_TUNER_IT9135_60;
-			break;
-		}
-	}
-
 skip_eeprom:
 	/* get demod clock */
 	ret = af9035_rd_reg(d, 0x00d800, &tmp);
@@ -990,6 +1119,7 @@ static int af9035_frontend_callback(void *adapter_priv, int component,
 static int af9035_get_adapter_count(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
+
 	return state->dual_mode + 1;
 }
 
@@ -998,7 +1128,8 @@ static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
 	struct state *state = adap_to_priv(adap);
 	struct dvb_usb_device *d = adap_to_d(adap);
 	int ret;
-	dev_dbg(&d->udev->dev, "%s:\n", __func__);
+
+	dev_dbg(&d->udev->dev, "%s: adap->id=%d\n", __func__, adap->id);
 
 	if (!state->af9033_config[adap->id].tuner) {
 		/* unsupported tuner */
@@ -1006,9 +1137,13 @@ static int af9035_frontend_attach(struct dvb_usb_adapter *adap)
 		goto err;
 	}
 
-	/* attach demodulator */
-	adap->fe[0] = dvb_attach(af9033_attach, &state->af9033_config[adap->id],
-			&d->i2c_adap, &state->ops);
+	state->af9033_config[adap->id].fe = &adap->fe[0];
+	state->af9033_config[adap->id].ops = &state->ops;
+	ret = af9035_add_i2c_dev(d, "af9033", state->af9033_i2c_addr[adap->id],
+			&state->af9033_config[adap->id], &d->i2c_adap);
+	if (ret)
+		goto err;
+
 	if (adap->fe[0] == NULL) {
 		ret = -ENODEV;
 		goto err;
@@ -1026,6 +1161,78 @@ err:
 	return ret;
 }
 
+static int it930x_frontend_attach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap_to_priv(adap);
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret;
+	struct si2168_config si2168_config;
+	struct i2c_adapter *adapter;
+
+	dev_dbg(&d->udev->dev, "adap->id=%d\n", adap->id);
+
+	si2168_config.i2c_adapter = &adapter;
+	si2168_config.fe = &adap->fe[0];
+	si2168_config.ts_mode = SI2168_TS_SERIAL;
+
+	state->af9033_config[adap->id].fe = &adap->fe[0];
+	state->af9033_config[adap->id].ops = &state->ops;
+	ret = af9035_add_i2c_dev(d, "si2168", 0x67, &si2168_config,
+				&d->i2c_adap);
+	if (ret)
+		goto err;
+
+	if (adap->fe[0] == NULL) {
+		ret = -ENODEV;
+		goto err;
+	}
+	state->i2c_adapter_demod = adapter;
+
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+static int af9035_frontend_detach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap_to_priv(adap);
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int demod2;
+
+	dev_dbg(&d->udev->dev, "%s: adap->id=%d\n", __func__, adap->id);
+
+	/*
+	 * For dual tuner devices we have to resolve 2nd demod client, as there
+	 * is two different kind of tuner drivers; one is using I2C binding
+	 * and the other is using DVB attach/detach binding.
+	 */
+	switch (state->af9033_config[adap->id].tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		demod2 = 2;
+		break;
+	default:
+		demod2 = 1;
+	}
+
+	if (adap->id == 1) {
+		if (state->i2c_client[demod2])
+			af9035_del_i2c_dev(d);
+	} else if (adap->id == 0) {
+		if (state->i2c_client[0])
+			af9035_del_i2c_dev(d);
+	}
+
+	return 0;
+}
+
 static struct tua9001_config af9035_tua9001_config = {
 	.i2c_addr = 0x60,
 };
@@ -1084,7 +1291,8 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 	struct dvb_frontend *fe;
 	struct i2c_msg msg[1];
 	u8 tuner_addr;
-	dev_dbg(&d->udev->dev, "%s:\n", __func__);
+
+	dev_dbg(&d->udev->dev, "%s: adap->id=%d\n", __func__, adap->id);
 
 	/*
 	 * XXX: Hack used in that function: we abuse unused I2C address bit [7]
@@ -1243,14 +1451,53 @@ static int af9035_tuner_attach(struct dvb_usb_adapter *adap)
 	case AF9033_TUNER_IT9135_38:
 	case AF9033_TUNER_IT9135_51:
 	case AF9033_TUNER_IT9135_52:
+	{
+		struct it913x_config it913x_config = {
+			.fe = adap->fe[0],
+			.chip_ver = 1,
+		};
+
+		if (state->dual_mode) {
+			if (adap->id == 0)
+				it913x_config.role = IT913X_ROLE_DUAL_MASTER;
+			else
+				it913x_config.role = IT913X_ROLE_DUAL_SLAVE;
+		}
+
+		ret = af9035_add_i2c_dev(d, "it913x",
+				state->af9033_i2c_addr[adap->id] >> 1,
+				&it913x_config, &d->i2c_adap);
+		if (ret)
+			goto err;
+
+		fe = adap->fe[0];
+		break;
+	}
 	case AF9033_TUNER_IT9135_60:
 	case AF9033_TUNER_IT9135_61:
 	case AF9033_TUNER_IT9135_62:
-		/* attach tuner */
-		fe = dvb_attach(it913x_attach, adap->fe[0], &d->i2c_adap,
-				state->af9033_config[adap->id].i2c_addr,
-				state->af9033_config[0].tuner);
+	{
+		struct it913x_config it913x_config = {
+			.fe = adap->fe[0],
+			.chip_ver = 2,
+		};
+
+		if (state->dual_mode) {
+			if (adap->id == 0)
+				it913x_config.role = IT913X_ROLE_DUAL_MASTER;
+			else
+				it913x_config.role = IT913X_ROLE_DUAL_SLAVE;
+		}
+
+		ret = af9035_add_i2c_dev(d, "it913x",
+				state->af9033_i2c_addr[adap->id] >> 1,
+				&it913x_config, &d->i2c_adap);
+		if (ret)
+			goto err;
+
+		fe = adap->fe[0];
 		break;
+	}
 	default:
 		fe = NULL;
 	}
@@ -1268,6 +1515,119 @@ err:
 	return ret;
 }
 
+static int it930x_tuner_attach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap_to_priv(adap);
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret;
+	struct si2157_config si2157_config;
+
+	dev_dbg(&d->udev->dev, "%s: adap->id=%d\n", __func__, adap->id);
+
+	/* I2C master bus 2 clock speed 300k */
+	ret = af9035_wr_reg(d, 0x00f6a7, 0x07);
+	if (ret < 0)
+		goto err;
+
+	/* I2C master bus 1,3 clock speed 300k */
+	ret = af9035_wr_reg(d, 0x00f103, 0x07);
+	if (ret < 0)
+		goto err;
+
+	/* set gpio11 low */
+	ret = af9035_wr_reg_mask(d, 0xd8d4, 0x01, 0x01);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg_mask(d, 0xd8d5, 0x01, 0x01);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg_mask(d, 0xd8d3, 0x01, 0x01);
+	if (ret < 0)
+		goto err;
+
+	/* Tuner enable using gpiot2_en, gpiot2_on and gpiot2_o (reset) */
+	ret = af9035_wr_reg_mask(d, 0xd8b8, 0x01, 0x01);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg_mask(d, 0xd8b9, 0x01, 0x01);
+	if (ret < 0)
+		goto err;
+
+	ret = af9035_wr_reg_mask(d, 0xd8b7, 0x00, 0x01);
+	if (ret < 0)
+		goto err;
+
+	msleep(200);
+
+	ret = af9035_wr_reg_mask(d, 0xd8b7, 0x01, 0x01);
+	if (ret < 0)
+		goto err;
+
+	memset(&si2157_config, 0, sizeof(si2157_config));
+	si2157_config.fe = adap->fe[0];
+	ret = af9035_add_i2c_dev(d, "si2157", 0x63,
+			&si2157_config, state->i2c_adapter_demod);
+
+	if (ret)
+		goto err;
+
+	return 0;
+
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+
+static int it930x_tuner_detach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap_to_priv(adap);
+	struct dvb_usb_device *d = adap_to_d(adap);
+
+	dev_dbg(&d->udev->dev, "adap->id=%d\n", adap->id);
+
+	if (adap->id == 1) {
+		if (state->i2c_client[3])
+			af9035_del_i2c_dev(d);
+	} else if (adap->id == 0) {
+		if (state->i2c_client[1])
+			af9035_del_i2c_dev(d);
+	}
+
+	return 0;
+}
+
+
+static int af9035_tuner_detach(struct dvb_usb_adapter *adap)
+{
+	struct state *state = adap_to_priv(adap);
+	struct dvb_usb_device *d = adap_to_d(adap);
+
+	dev_dbg(&d->udev->dev, "%s: adap->id=%d\n", __func__, adap->id);
+
+	switch (state->af9033_config[adap->id].tuner) {
+	case AF9033_TUNER_IT9135_38:
+	case AF9033_TUNER_IT9135_51:
+	case AF9033_TUNER_IT9135_52:
+	case AF9033_TUNER_IT9135_60:
+	case AF9033_TUNER_IT9135_61:
+	case AF9033_TUNER_IT9135_62:
+		if (adap->id == 1) {
+			if (state->i2c_client[3])
+				af9035_del_i2c_dev(d);
+		} else if (adap->id == 0) {
+			if (state->i2c_client[1])
+				af9035_del_i2c_dev(d);
+		}
+	}
+
+	return 0;
+}
+
 static int af9035_init(struct dvb_usb_device *d)
 {
 	struct state *state = d_to_priv(d);
@@ -1315,6 +1675,89 @@ err:
 	return ret;
 }
 
+static int it930x_init(struct dvb_usb_device *d)
+{
+	struct state *state = d_to_priv(d);
+	int ret, i;
+	u16 frame_size = (d->udev->speed == USB_SPEED_FULL ? 5 : 816) * 188 / 4;
+	u8 packet_size = (d->udev->speed == USB_SPEED_FULL ? 64 : 512) / 4;
+	struct reg_val_mask tab[] = {
+		{ 0x00da1a, 0x00, 0x01 }, /* ignore_sync_byte */
+		{ 0x00f41f, 0x04, 0x04 }, /* dvbt_inten */
+		{ 0x00da10, 0x00, 0x01 }, /* mpeg_full_speed */
+		{ 0x00f41a, 0x01, 0x01 }, /* dvbt_en */
+		{ 0x00da1d, 0x01, 0x01 }, /* mp2_sw_rst, reset EP4 */
+		{ 0x00dd11, 0x00, 0x20 }, /* ep4_tx_en, disable EP4 */
+		{ 0x00dd13, 0x00, 0x20 }, /* ep4_tx_nak, disable EP4 NAK */
+		{ 0x00dd11, 0x20, 0x20 }, /* ep4_tx_en, enable EP4 */
+		{ 0x00dd11, 0x00, 0x40 }, /* ep5_tx_en, disable EP5 */
+		{ 0x00dd13, 0x00, 0x40 }, /* ep5_tx_nak, disable EP5 NAK */
+		{ 0x00dd11, state->dual_mode << 6, 0x40 }, /* enable EP5 */
+		{ 0x00dd88, (frame_size >> 0) & 0xff, 0xff},
+		{ 0x00dd89, (frame_size >> 8) & 0xff, 0xff},
+		{ 0x00dd0c, packet_size, 0xff},
+		{ 0x00dd8a, (frame_size >> 0) & 0xff, 0xff},
+		{ 0x00dd8b, (frame_size >> 8) & 0xff, 0xff},
+		{ 0x00dd0d, packet_size, 0xff },
+		{ 0x00da1d, 0x00, 0x01 }, /* mp2_sw_rst, disable */
+		{ 0x00d833, 0x01, 0xff }, /* slew rate ctrl: slew rate boosts */
+		{ 0x00d830, 0x00, 0xff }, /* Bit 0 of output driving control */
+		{ 0x00d831, 0x01, 0xff }, /* Bit 1 of output driving control */
+		{ 0x00d832, 0x00, 0xff }, /* Bit 2 of output driving control */
+
+		/* suspend gpio1 for TS-C */
+		{ 0x00d8b0, 0x01, 0xff }, /* gpio1 */
+		{ 0x00d8b1, 0x01, 0xff }, /* gpio1 */
+		{ 0x00d8af, 0x00, 0xff }, /* gpio1 */
+
+		/* suspend gpio7 for TS-D */
+		{ 0x00d8c4, 0x01, 0xff }, /* gpio7 */
+		{ 0x00d8c5, 0x01, 0xff }, /* gpio7 */
+		{ 0x00d8c3, 0x00, 0xff }, /* gpio7 */
+
+		/* suspend gpio13 for TS-B */
+		{ 0x00d8dc, 0x01, 0xff }, /* gpio13 */
+		{ 0x00d8dd, 0x01, 0xff }, /* gpio13 */
+		{ 0x00d8db, 0x00, 0xff }, /* gpio13 */
+
+		/* suspend gpio14 for TS-E */
+		{ 0x00d8e4, 0x01, 0xff }, /* gpio14 */
+		{ 0x00d8e5, 0x01, 0xff }, /* gpio14 */
+		{ 0x00d8e3, 0x00, 0xff }, /* gpio14 */
+
+		/* suspend gpio15 for TS-A */
+		{ 0x00d8e8, 0x01, 0xff }, /* gpio15 */
+		{ 0x00d8e9, 0x01, 0xff }, /* gpio15 */
+		{ 0x00d8e7, 0x00, 0xff }, /* gpio15 */
+
+		{ 0x00da58, 0x00, 0x01 }, /* ts_in_src, serial */
+		{ 0x00da73, 0x01, 0xff }, /* ts0_aggre_mode */
+		{ 0x00da78, 0x47, 0xff }, /* ts0_sync_byte */
+		{ 0x00da4c, 0x01, 0xff }, /* ts0_en */
+		{ 0x00da5a, 0x1f, 0xff }, /* ts_fail_ignore */
+	};
+
+	dev_dbg(&d->udev->dev,
+			"%s: USB speed=%d frame_size=%04x packet_size=%02x\n",
+			__func__, d->udev->speed, frame_size, packet_size);
+
+	/* init endpoints */
+	for (i = 0; i < ARRAY_SIZE(tab); i++) {
+		ret = af9035_wr_reg_mask(d, tab[i].reg,
+				tab[i].val, tab[i].mask);
+
+		if (ret < 0)
+			goto err;
+	}
+
+	return 0;
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+
+	return ret;
+}
+
+
 #if IS_ENABLED(CONFIG_RC_CORE)
 static int af9035_rc_query(struct dvb_usb_device *d)
 {
@@ -1409,6 +1852,7 @@ static int af9035_get_stream_config(struct dvb_frontend *fe, u8 *ts_type,
 		struct usb_data_stream_properties *stream)
 {
 	struct dvb_usb_device *d = fe_to_d(fe);
+
 	dev_dbg(&d->udev->dev, "%s: adap=%d\n", __func__, fe_to_adap(fe)->id);
 
 	if (d->udev->speed == USB_SPEED_FULL)
@@ -1486,7 +1930,9 @@ static const struct dvb_usb_device_properties af9035_props = {
 	.i2c_algo = &af9035_i2c_algo,
 	.read_config = af9035_read_config,
 	.frontend_attach = af9035_frontend_attach,
+	.frontend_detach = af9035_frontend_detach,
 	.tuner_attach = af9035_tuner_attach,
+	.tuner_detach = af9035_tuner_detach,
 	.init = af9035_init,
 	.get_rc_config = af9035_get_rc_config,
 	.get_stream_config = af9035_get_stream_config,
@@ -1515,6 +1961,37 @@ static const struct dvb_usb_device_properties af9035_props = {
 	},
 };
 
+static const struct dvb_usb_device_properties it930x_props = {
+	.driver_name = KBUILD_MODNAME,
+	.owner = THIS_MODULE,
+	.adapter_nr = adapter_nr,
+	.size_of_priv = sizeof(struct state),
+
+	.generic_bulk_ctrl_endpoint = 0x02,
+	.generic_bulk_ctrl_endpoint_response = 0x81,
+
+	.identify_state = af9035_identify_state,
+	.download_firmware = af9035_download_firmware,
+
+	.i2c_algo = &af9035_i2c_algo,
+	.read_config = af9035_read_config,
+	.frontend_attach = it930x_frontend_attach,
+	.frontend_detach = af9035_frontend_detach,
+	.tuner_attach = it930x_tuner_attach,
+	.tuner_detach = it930x_tuner_detach,
+	.init = it930x_init,
+	.get_stream_config = af9035_get_stream_config,
+
+	.get_adapter_count = af9035_get_adapter_count,
+	.adapter = {
+		{
+			.stream = DVB_USB_STREAM_BULK(0x84, 4, 816 * 188),
+		}, {
+			.stream = DVB_USB_STREAM_BULK(0x85, 4, 816 * 188),
+		},
+	},
+};
+
 static const struct usb_device_id af9035_id_table[] = {
 	/* AF9035 devices */
 	{ DVB_USB_DEVICE(USB_VID_AFATECH, USB_PID_AFATECH_AF9035_9035,
@@ -1568,13 +2045,21 @@ static const struct usb_device_id af9035_id_table[] = {
 	{ DVB_USB_DEVICE(USB_VID_KWORLD_2, USB_PID_CTVDIGDUAL_V2,
 		&af9035_props, "Digital Dual TV Receiver CTVDIGDUAL_V2",
 							RC_MAP_IT913X_V1) },
+	/* IT930x devices */
+	{ DVB_USB_DEVICE(USB_VID_ITETECH, USB_PID_ITETECH_IT9303,
+		&it930x_props, "ITE 9303 Generic", NULL) },
 	/* XXX: that same ID [0ccd:0099] is used by af9015 driver too */
 	{ DVB_USB_DEVICE(USB_VID_TERRATEC, 0x0099,
-		&af9035_props, "TerraTec Cinergy T Stick Dual RC (rev. 2)", NULL) },
+		&af9035_props, "TerraTec Cinergy T Stick Dual RC (rev. 2)",
+		NULL) },
 	{ DVB_USB_DEVICE(USB_VID_LEADTEK, 0x6a05,
 		&af9035_props, "Leadtek WinFast DTV Dongle Dual", NULL) },
 	{ DVB_USB_DEVICE(USB_VID_HAUPPAUGE, 0xf900,
 		&af9035_props, "Hauppauge WinTV-MiniStick 2", NULL) },
+	{ DVB_USB_DEVICE(USB_VID_PCTV, USB_PID_PCTV_78E,
+		&af9035_props, "PCTV AndroiDTV (78e)", RC_MAP_IT913X_V1) },
+	{ DVB_USB_DEVICE(USB_VID_PCTV, USB_PID_PCTV_79E,
+		&af9035_props, "PCTV microStick (79e)", RC_MAP_IT913X_V2) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, af9035_id_table);
@@ -1599,3 +2084,4 @@ MODULE_LICENSE("GPL");
 MODULE_FIRMWARE(AF9035_FIRMWARE_AF9035);
 MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135_V1);
 MODULE_FIRMWARE(AF9035_FIRMWARE_IT9135_V2);
+MODULE_FIRMWARE(AF9035_FIRMWARE_IT9303);
diff --git a/drivers/media/usb/dvb-usb-v2/af9035.h b/drivers/media/usb/dvb-usb-v2/af9035.h
index c21902fdd4c4..416a97f05ec8 100644
--- a/drivers/media/usb/dvb-usb-v2/af9035.h
+++ b/drivers/media/usb/dvb-usb-v2/af9035.h
@@ -30,7 +30,9 @@
 #include "mxl5007t.h"
 #include "tda18218.h"
 #include "fc2580.h"
-#include "tuner_it913x.h"
+#include "it913x.h"
+#include "si2168.h"
+#include "si2157.h"
 
 struct reg_val {
 	u32 reg;
@@ -61,9 +63,12 @@ struct state {
 	u16 chip_type;
 	u8 dual_mode:1;
 	u16 eeprom_addr;
+	u8 af9033_i2c_addr[2];
 	struct af9033_config af9033_config[2];
-
 	struct af9033_ops ops;
+	#define AF9035_I2C_CLIENT_MAX 4
+	struct i2c_client *i2c_client[AF9035_I2C_CLIENT_MAX];
+	struct i2c_adapter *i2c_adapter_demod;
 };
 
 static const u32 clock_lut_af9035[] = {
@@ -97,6 +102,7 @@ static const u32 clock_lut_it9135[] = {
 #define AF9035_FIRMWARE_AF9035 "dvb-usb-af9035-02.fw"
 #define AF9035_FIRMWARE_IT9135_V1 "dvb-usb-it9135-01.fw"
 #define AF9035_FIRMWARE_IT9135_V2 "dvb-usb-it9135-02.fw"
+#define AF9035_FIRMWARE_IT9303 "dvb-usb-it9303-01.fw"
 
 /*
  * eeprom is memory mapped as read only. Writing that memory mapped address
@@ -138,5 +144,7 @@ static const u32 clock_lut_it9135[] = {
 #define CMD_FW_DL_BEGIN             0x24
 #define CMD_FW_DL_END               0x25
 #define CMD_FW_SCATTER_WR           0x29
+#define CMD_GENERIC_I2C_RD          0x2a
+#define CMD_GENERIC_I2C_WR          0x2b
 
 #endif
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.c b/drivers/media/usb/dvb-usb-v2/anysee.c
index e4a2382196f0..d3c5f230e97a 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.c
+++ b/drivers/media/usb/dvb-usb-v2/anysee.c
@@ -332,7 +332,6 @@ static struct tda10023_config anysee_tda10023_tda18212_config = {
 };
 
 static struct tda18212_config anysee_tda18212_config = {
-	.i2c_address = (0xc0 >> 1),
 	.if_dvbt_6 = 4150,
 	.if_dvbt_7 = 4150,
 	.if_dvbt_8 = 4150,
@@ -340,7 +339,6 @@ static struct tda18212_config anysee_tda18212_config = {
 };
 
 static struct tda18212_config anysee_tda18212_config2 = {
-	.i2c_address = 0x60 /* (0xc0 >> 1) */,
 	.if_dvbt_6 = 3550,
 	.if_dvbt_7 = 3700,
 	.if_dvbt_8 = 4150,
@@ -632,6 +630,92 @@ error:
 	return ret;
 }
 
+static int anysee_add_i2c_dev(struct dvb_usb_device *d, char *type, u8 addr,
+		void *platform_data)
+{
+	int ret, num;
+	struct anysee_state *state = d_to_priv(d);
+	struct i2c_client *client;
+	struct i2c_adapter *adapter = &d->i2c_adap;
+	struct i2c_board_info board_info = {
+		.addr = addr,
+		.platform_data = platform_data,
+	};
+
+	strlcpy(board_info.type, type, I2C_NAME_SIZE);
+
+	/* find first free client */
+	for (num = 0; num < ANYSEE_I2C_CLIENT_MAX; num++) {
+		if (state->i2c_client[num] == NULL)
+			break;
+	}
+
+	dev_dbg(&d->udev->dev, "%s: num=%d\n", __func__, num);
+
+	if (num == ANYSEE_I2C_CLIENT_MAX) {
+		dev_err(&d->udev->dev, "%s: I2C client out of index\n",
+				KBUILD_MODNAME);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	request_module(board_info.type);
+
+	/* register I2C device */
+	client = i2c_new_device(adapter, &board_info);
+	if (client == NULL || client->dev.driver == NULL) {
+		ret = -ENODEV;
+		goto err;
+	}
+
+	/* increase I2C driver usage count */
+	if (!try_module_get(client->dev.driver->owner)) {
+		i2c_unregister_device(client);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	state->i2c_client[num] = client;
+	return 0;
+err:
+	dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, ret);
+	return ret;
+}
+
+static void anysee_del_i2c_dev(struct dvb_usb_device *d)
+{
+	int num;
+	struct anysee_state *state = d_to_priv(d);
+	struct i2c_client *client;
+
+	/* find last used client */
+	num = ANYSEE_I2C_CLIENT_MAX;
+	while (num--) {
+		if (state->i2c_client[num] != NULL)
+			break;
+	}
+
+	dev_dbg(&d->udev->dev, "%s: num=%d\n", __func__, num);
+
+	if (num == -1) {
+		dev_err(&d->udev->dev, "%s: I2C client out of index\n",
+				KBUILD_MODNAME);
+		goto err;
+	}
+
+	client = state->i2c_client[num];
+
+	/* decrease I2C driver usage count */
+	module_put(client->dev.driver->owner);
+
+	/* unregister I2C device */
+	i2c_unregister_device(client);
+
+	state->i2c_client[num] = NULL;
+err:
+	dev_dbg(&d->udev->dev, "%s: failed\n", __func__);
+}
+
 static int anysee_frontend_attach(struct dvb_usb_adapter *adap)
 {
 	struct anysee_state *state = adap_to_priv(adap);
@@ -640,12 +724,12 @@ static int anysee_frontend_attach(struct dvb_usb_adapter *adap)
 	u8 tmp;
 	struct i2c_msg msg[2] = {
 		{
-			.addr = anysee_tda18212_config.i2c_address,
+			.addr = 0x60,
 			.flags = 0,
 			.len = 1,
 			.buf = "\x00",
 		}, {
-			.addr = anysee_tda18212_config.i2c_address,
+			.addr = 0x60,
 			.flags = I2C_M_RD,
 			.len = 1,
 			.buf = &tmp,
@@ -723,9 +807,11 @@ static int anysee_frontend_attach(struct dvb_usb_adapter *adap)
 		/* probe TDA18212 */
 		tmp = 0;
 		ret = i2c_transfer(&d->i2c_adap, msg, 2);
-		if (ret == 2 && tmp == 0xc7)
+		if (ret == 2 && tmp == 0xc7) {
 			dev_dbg(&d->udev->dev, "%s: TDA18212 found\n",
 					__func__);
+			state->has_tda18212 = true;
+		}
 		else
 			tmp = 0;
 
@@ -939,46 +1025,63 @@ static int anysee_tuner_attach(struct dvb_usb_adapter *adap)
 		 * fails attach old simple PLL. */
 
 		/* attach tuner */
-		fe = dvb_attach(tda18212_attach, adap->fe[0], &d->i2c_adap,
-				&anysee_tda18212_config);
+		if (state->has_tda18212) {
+			struct tda18212_config tda18212_config =
+					anysee_tda18212_config;
 
-		if (fe && adap->fe[1]) {
-			/* attach tuner for 2nd FE */
-			fe = dvb_attach(tda18212_attach, adap->fe[1],
-					&d->i2c_adap, &anysee_tda18212_config);
-			break;
-		} else if (fe) {
-			break;
-		}
-
-		/* attach tuner */
-		fe = dvb_attach(dvb_pll_attach, adap->fe[0], (0xc0 >> 1),
-				&d->i2c_adap, DVB_PLL_SAMSUNG_DTOS403IH102A);
+			tda18212_config.fe = adap->fe[0];
+			ret = anysee_add_i2c_dev(d, "tda18212", 0x60,
+					&tda18212_config);
+			if (ret)
+				goto err;
+
+			/* copy tuner ops for 2nd FE as tuner is shared */
+			if (adap->fe[1]) {
+				adap->fe[1]->tuner_priv =
+						adap->fe[0]->tuner_priv;
+				memcpy(&adap->fe[1]->ops.tuner_ops,
+						&adap->fe[0]->ops.tuner_ops,
+						sizeof(struct dvb_tuner_ops));
+			}
 
-		if (fe && adap->fe[1]) {
-			/* attach tuner for 2nd FE */
-			fe = dvb_attach(dvb_pll_attach, adap->fe[1],
+			return 0;
+		} else {
+			/* attach tuner */
+			fe = dvb_attach(dvb_pll_attach, adap->fe[0],
 					(0xc0 >> 1), &d->i2c_adap,
 					DVB_PLL_SAMSUNG_DTOS403IH102A);
+
+			if (fe && adap->fe[1]) {
+				/* attach tuner for 2nd FE */
+				fe = dvb_attach(dvb_pll_attach, adap->fe[1],
+						(0xc0 >> 1), &d->i2c_adap,
+						DVB_PLL_SAMSUNG_DTOS403IH102A);
+			}
 		}
 
 		break;
 	case ANYSEE_HW_508TC: /* 18 */
 	case ANYSEE_HW_508PTC: /* 21 */
+	{
 		/* E7 TC */
 		/* E7 PTC */
+		struct tda18212_config tda18212_config = anysee_tda18212_config;
 
-		/* attach tuner */
-		fe = dvb_attach(tda18212_attach, adap->fe[0], &d->i2c_adap,
-				&anysee_tda18212_config);
-
-		if (fe) {
-			/* attach tuner for 2nd FE */
-			fe = dvb_attach(tda18212_attach, adap->fe[1],
-					&d->i2c_adap, &anysee_tda18212_config);
+		tda18212_config.fe = adap->fe[0];
+		ret = anysee_add_i2c_dev(d, "tda18212", 0x60, &tda18212_config);
+		if (ret)
+			goto err;
+
+		/* copy tuner ops for 2nd FE as tuner is shared */
+		if (adap->fe[1]) {
+			adap->fe[1]->tuner_priv = adap->fe[0]->tuner_priv;
+			memcpy(&adap->fe[1]->ops.tuner_ops,
+					&adap->fe[0]->ops.tuner_ops,
+					sizeof(struct dvb_tuner_ops));
 		}
 
-		break;
+		return 0;
+	}
 	case ANYSEE_HW_508S2: /* 19 */
 	case ANYSEE_HW_508PS2: /* 22 */
 		/* E7 S2 */
@@ -997,13 +1100,18 @@ static int anysee_tuner_attach(struct dvb_usb_adapter *adap)
 		break;
 
 	case ANYSEE_HW_508T2C: /* 20 */
+	{
 		/* E7 T2C */
+		struct tda18212_config tda18212_config =
+				anysee_tda18212_config2;
 
-		/* attach tuner */
-		fe = dvb_attach(tda18212_attach, adap->fe[0], &d->i2c_adap,
-				&anysee_tda18212_config2);
+		tda18212_config.fe = adap->fe[0];
+		ret = anysee_add_i2c_dev(d, "tda18212", 0x60, &tda18212_config);
+		if (ret)
+			goto err;
 
-		break;
+		return 0;
+	}
 	default:
 		fe = NULL;
 	}
@@ -1012,7 +1120,7 @@ static int anysee_tuner_attach(struct dvb_usb_adapter *adap)
 		ret = 0;
 	else
 		ret = -ENODEV;
-
+err:
 	return ret;
 }
 
@@ -1270,6 +1378,11 @@ static int anysee_init(struct dvb_usb_device *d)
 
 static void anysee_exit(struct dvb_usb_device *d)
 {
+	struct anysee_state *state = d_to_priv(d);
+
+	if (state->i2c_client[0])
+		anysee_del_i2c_dev(d);
+
 	return anysee_ci_release(d);
 }
 
diff --git a/drivers/media/usb/dvb-usb-v2/anysee.h b/drivers/media/usb/dvb-usb-v2/anysee.h
index 8f426d9fc6e1..3ca2bca4ebaf 100644
--- a/drivers/media/usb/dvb-usb-v2/anysee.h
+++ b/drivers/media/usb/dvb-usb-v2/anysee.h
@@ -55,8 +55,11 @@ struct anysee_state {
 	u8 buf[64];
 	u8 seq;
 	u8 hw; /* PCB ID */
+	#define ANYSEE_I2C_CLIENT_MAX 1
+	struct i2c_client *i2c_client[ANYSEE_I2C_CLIENT_MAX];
 	u8 fe_id:1; /* frondend ID */
 	u8 has_ci:1;
+	u8 has_tda18212:1;
 	u8 ci_attached:1;
 	struct dvb_ca_en50221 ci;
 	unsigned long ci_cam_ready; /* jiffies */
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb.h b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
index 124b4baa7e97..14e111e13e54 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb.h
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb.h
@@ -214,6 +214,7 @@ struct dvb_usb_adapter_properties {
  * @read_config: called to resolve device configuration
  * @read_mac_address: called to resolve adapter mac-address
  * @frontend_attach: called to attach the possible frontends
+ * @frontend_detach: called to detach the possible frontends
  * @tuner_attach: called to attach the possible tuners
  * @frontend_ctrl: called to power on/off active frontend
  * @streaming_ctrl: called to start/stop the usb streaming of adapter
@@ -254,7 +255,9 @@ struct dvb_usb_device_properties {
 	int (*read_config) (struct dvb_usb_device *d);
 	int (*read_mac_address) (struct dvb_usb_adapter *, u8 []);
 	int (*frontend_attach) (struct dvb_usb_adapter *);
+	int (*frontend_detach)(struct dvb_usb_adapter *);
 	int (*tuner_attach) (struct dvb_usb_adapter *);
+	int (*tuner_detach)(struct dvb_usb_adapter *);
 	int (*frontend_ctrl) (struct dvb_frontend *, int);
 	int (*streaming_ctrl) (struct dvb_frontend *, int);
 	int (*init) (struct dvb_usb_device *);
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
index 2e90310be2af..1950f37df835 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_core.c
@@ -21,7 +21,7 @@
 
 #include "dvb_usb_common.h"
 
-int dvb_usbv2_disable_rc_polling;
+static int dvb_usbv2_disable_rc_polling;
 module_param_named(disable_rc_polling, dvb_usbv2_disable_rc_polling, int, 0644);
 MODULE_PARM_DESC(disable_rc_polling,
 		"disable remote control polling (default: 0)");
@@ -664,9 +664,10 @@ err:
 
 static int dvb_usbv2_adapter_frontend_exit(struct dvb_usb_adapter *adap)
 {
-	int i;
-	dev_dbg(&adap_to_d(adap)->udev->dev, "%s: adap=%d\n", __func__,
-			adap->id);
+	int ret, i;
+	struct dvb_usb_device *d = adap_to_d(adap);
+
+	dev_dbg(&d->udev->dev, "%s: adap=%d\n", __func__, adap->id);
 
 	for (i = MAX_NO_OF_FE_PER_ADAP - 1; i >= 0; i--) {
 		if (adap->fe[i]) {
@@ -675,6 +676,23 @@ static int dvb_usbv2_adapter_frontend_exit(struct dvb_usb_adapter *adap)
 		}
 	}
 
+	if (d->props->tuner_detach) {
+		ret = d->props->tuner_detach(adap);
+		if (ret < 0) {
+			dev_dbg(&d->udev->dev, "%s: tuner_detach() failed=%d\n",
+					__func__, ret);
+		}
+	}
+
+	if (d->props->frontend_detach) {
+		ret = d->props->frontend_detach(adap);
+		if (ret < 0) {
+			dev_dbg(&d->udev->dev,
+					"%s: frontend_detach() failed=%d\n",
+					__func__, ret);
+		}
+	}
+
 	return 0;
 }
 
@@ -762,9 +780,9 @@ static int dvb_usbv2_adapter_exit(struct dvb_usb_device *d)
 
 	for (i = MAX_NO_OF_ADAPTER_PER_DEVICE - 1; i >= 0; i--) {
 		if (d->adapter[i].props) {
-			dvb_usbv2_adapter_frontend_exit(&d->adapter[i]);
 			dvb_usbv2_adapter_dvb_exit(&d->adapter[i]);
 			dvb_usbv2_adapter_stream_exit(&d->adapter[i]);
+			dvb_usbv2_adapter_frontend_exit(&d->adapter[i]);
 		}
 	}
 
diff --git a/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c b/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
index 33ff97e708e3..22bdce15ecf3 100644
--- a/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
+++ b/drivers/media/usb/dvb-usb-v2/dvb_usb_urb.c
@@ -26,7 +26,7 @@ static int dvb_usb_v2_generic_io(struct dvb_usb_device *d,
 {
 	int ret, actual_length;
 
-	if (!d || !wbuf || !wlen || !d->props->generic_bulk_ctrl_endpoint ||
+	if (!wbuf || !wlen || !d->props->generic_bulk_ctrl_endpoint ||
 			!d->props->generic_bulk_ctrl_endpoint_response) {
 		dev_dbg(&d->udev->dev, "%s: failed=%d\n", __func__, -EINVAL);
 		return -EINVAL;
diff --git a/drivers/media/usb/dvb-usb-v2/dvbsky.c b/drivers/media/usb/dvb-usb-v2/dvbsky.c
new file mode 100644
index 000000000000..34688c89df11
--- /dev/null
+++ b/drivers/media/usb/dvb-usb-v2/dvbsky.c
@@ -0,0 +1,460 @@
+/*
+ * Driver for DVBSky USB2.0 receiver
+ *
+ * Copyright (C) 2013 Max nibble <nibble.max@gmail.com>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ *
+ *    You should have received a copy of the GNU General Public License
+ *    along with this program; if not, write to the Free Software
+ *    Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ */
+
+#include "dvb_usb.h"
+#include "m88ds3103.h"
+#include "m88ts2022.h"
+
+#define DVBSKY_MSG_DELAY	0/*2000*/
+#define DVBSKY_BUF_LEN	64
+
+DVB_DEFINE_MOD_OPT_ADAPTER_NR(adapter_nr);
+
+struct dvbsky_state {
+	struct mutex stream_mutex;
+	u8 ibuf[DVBSKY_BUF_LEN];
+	u8 obuf[DVBSKY_BUF_LEN];
+	u8 last_lock;
+	struct i2c_client *i2c_client_tuner;
+
+	/* fe hook functions*/
+	int (*fe_set_voltage)(struct dvb_frontend *fe,
+		fe_sec_voltage_t voltage);
+	int (*fe_read_status)(struct dvb_frontend *fe,
+		fe_status_t *status);
+};
+
+static int dvbsky_usb_generic_rw(struct dvb_usb_device *d,
+		u8 *wbuf, u16 wlen, u8 *rbuf, u16 rlen)
+{
+	int ret;
+	struct dvbsky_state *state = d_to_priv(d);
+
+	mutex_lock(&d->usb_mutex);
+	if (wlen != 0)
+		memcpy(state->obuf, wbuf, wlen);
+
+	ret = dvb_usbv2_generic_rw_locked(d, state->obuf, wlen,
+			state->ibuf, rlen);
+
+	if (!ret && (rlen != 0))
+		memcpy(rbuf, state->ibuf, rlen);
+
+	mutex_unlock(&d->usb_mutex);
+	return ret;
+}
+
+static int dvbsky_stream_ctrl(struct dvb_usb_device *d, u8 onoff)
+{
+	struct dvbsky_state *state = d_to_priv(d);
+	int ret;
+	u8 obuf_pre[3] = { 0x37, 0, 0 };
+	u8 obuf_post[3] = { 0x36, 3, 0 };
+
+	mutex_lock(&state->stream_mutex);
+	ret = dvbsky_usb_generic_rw(d, obuf_pre, 3, NULL, 0);
+	if (!ret && onoff) {
+		msleep(20);
+		ret = dvbsky_usb_generic_rw(d, obuf_post, 3, NULL, 0);
+	}
+	mutex_unlock(&state->stream_mutex);
+	return ret;
+}
+
+static int dvbsky_streaming_ctrl(struct dvb_frontend *fe, int onoff)
+{
+	struct dvb_usb_device *d = fe_to_d(fe);
+
+	return dvbsky_stream_ctrl(d, (onoff == 0) ? 0 : 1);
+}
+
+/* GPIO */
+static int dvbsky_gpio_ctrl(struct dvb_usb_device *d, u8 gport, u8 value)
+{
+	int ret;
+	u8 obuf[3], ibuf[2];
+
+	obuf[0] = 0x0e;
+	obuf[1] = gport;
+	obuf[2] = value;
+	ret = dvbsky_usb_generic_rw(d, obuf, 3, ibuf, 1);
+	if (ret)
+		dev_err(&d->udev->dev, "%s: %s() failed=%d\n",
+			KBUILD_MODNAME, __func__, ret);
+	return ret;
+}
+
+/* I2C */
+static int dvbsky_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
+	int num)
+{
+	struct dvb_usb_device *d = i2c_get_adapdata(adap);
+	int ret = 0;
+	u8 ibuf[64], obuf[64];
+
+	if (mutex_lock_interruptible(&d->i2c_mutex) < 0)
+		return -EAGAIN;
+
+	if (num > 2) {
+		dev_err(&d->udev->dev,
+		"dvbsky_usb: too many i2c messages[%d] than 2.", num);
+		ret = -EOPNOTSUPP;
+		goto i2c_error;
+	}
+
+	if (num == 1) {
+		if (msg[0].len > 60) {
+			dev_err(&d->udev->dev,
+			"dvbsky_usb: too many i2c bytes[%d] than 60.",
+			msg[0].len);
+			ret = -EOPNOTSUPP;
+			goto i2c_error;
+		}
+		if (msg[0].flags & I2C_M_RD) {
+			/* single read */
+			obuf[0] = 0x09;
+			obuf[1] = 0;
+			obuf[2] = msg[0].len;
+			obuf[3] = msg[0].addr;
+			ret = dvbsky_usb_generic_rw(d, obuf, 4,
+					ibuf, msg[0].len + 1);
+			if (ret)
+				dev_err(&d->udev->dev, "%s: %s() failed=%d\n",
+					KBUILD_MODNAME, __func__, ret);
+			if (!ret)
+				memcpy(msg[0].buf, &ibuf[1], msg[0].len);
+		} else {
+			/* write */
+			obuf[0] = 0x08;
+			obuf[1] = msg[0].addr;
+			obuf[2] = msg[0].len;
+			memcpy(&obuf[3], msg[0].buf, msg[0].len);
+			ret = dvbsky_usb_generic_rw(d, obuf,
+					msg[0].len + 3, ibuf, 1);
+			if (ret)
+				dev_err(&d->udev->dev, "%s: %s() failed=%d\n",
+					KBUILD_MODNAME, __func__, ret);
+		}
+	} else {
+		if ((msg[0].len > 60) || (msg[1].len > 60)) {
+			dev_err(&d->udev->dev,
+			"dvbsky_usb: too many i2c bytes[w-%d][r-%d] than 60.",
+			msg[0].len, msg[1].len);
+			ret = -EOPNOTSUPP;
+			goto i2c_error;
+		}
+		/* write then read */
+		obuf[0] = 0x09;
+		obuf[1] = msg[0].len;
+		obuf[2] = msg[1].len;
+		obuf[3] = msg[0].addr;
+		memcpy(&obuf[4], msg[0].buf, msg[0].len);
+		ret = dvbsky_usb_generic_rw(d, obuf,
+			msg[0].len + 4, ibuf, msg[1].len + 1);
+		if (ret)
+			dev_err(&d->udev->dev, "%s: %s() failed=%d\n",
+				KBUILD_MODNAME, __func__, ret);
+
+		if (!ret)
+			memcpy(msg[1].buf, &ibuf[1], msg[1].len);
+	}
+i2c_error:
+	mutex_unlock(&d->i2c_mutex);
+	return (ret) ? ret : num;
+}
+
+static u32 dvbsky_i2c_func(struct i2c_adapter *adapter)
+{
+	return I2C_FUNC_I2C;
+}
+
+static struct i2c_algorithm dvbsky_i2c_algo = {
+	.master_xfer   = dvbsky_i2c_xfer,
+	.functionality = dvbsky_i2c_func,
+};
+
+#if IS_ENABLED(CONFIG_RC_CORE)
+static int dvbsky_rc_query(struct dvb_usb_device *d)
+{
+	u32 code = 0xffff, scancode;
+	u8 rc5_command, rc5_system;
+	u8 obuf[2], ibuf[2], toggle;
+	int ret;
+
+	obuf[0] = 0x10;
+	ret = dvbsky_usb_generic_rw(d, obuf, 1, ibuf, 2);
+	if (ret)
+		dev_err(&d->udev->dev, "%s: %s() failed=%d\n",
+			KBUILD_MODNAME, __func__, ret);
+	if (ret == 0)
+		code = (ibuf[0] << 8) | ibuf[1];
+	if (code != 0xffff) {
+		dev_dbg(&d->udev->dev, "rc code: %x\n", code);
+		rc5_command = code & 0x3F;
+		rc5_system = (code & 0x7C0) >> 6;
+		toggle = (code & 0x800) ? 1 : 0;
+		scancode = rc5_system << 8 | rc5_command;
+		rc_keydown(d->rc_dev, RC_TYPE_RC5, scancode, toggle);
+	}
+	return 0;
+}
+
+static int dvbsky_get_rc_config(struct dvb_usb_device *d, struct dvb_usb_rc *rc)
+{
+	rc->allowed_protos = RC_BIT_RC5;
+	rc->query          = dvbsky_rc_query;
+	rc->interval       = 300;
+	return 0;
+}
+#else
+	#define dvbsky_get_rc_config NULL
+#endif
+
+static int dvbsky_usb_set_voltage(struct dvb_frontend *fe,
+	fe_sec_voltage_t voltage)
+{
+	struct dvb_usb_device *d = fe_to_d(fe);
+	struct dvbsky_state *state = d_to_priv(d);
+	u8 value;
+
+	if (voltage == SEC_VOLTAGE_OFF)
+		value = 0;
+	else
+		value = 1;
+	dvbsky_gpio_ctrl(d, 0x80, value);
+
+	return state->fe_set_voltage(fe, voltage);
+}
+
+static int dvbsky_read_mac_addr(struct dvb_usb_adapter *adap, u8 mac[6])
+{
+	struct dvb_usb_device *d = adap_to_d(adap);
+	u8 obuf[] = { 0x1e, 0x00 };
+	u8 ibuf[6] = { 0 };
+	struct i2c_msg msg[] = {
+		{
+			.addr = 0x51,
+			.flags = 0,
+			.buf = obuf,
+			.len = 2,
+		}, {
+			.addr = 0x51,
+			.flags = I2C_M_RD,
+			.buf = ibuf,
+			.len = 6,
+		}
+	};
+
+	if (i2c_transfer(&d->i2c_adap, msg, 2) == 2)
+		memcpy(mac, ibuf, 6);
+
+	dev_info(&d->udev->dev, "dvbsky_usb MAC address=%pM\n", mac);
+
+	return 0;
+}
+
+static int dvbsky_usb_read_status(struct dvb_frontend *fe, fe_status_t *status)
+{
+	struct dvb_usb_device *d = fe_to_d(fe);
+	struct dvbsky_state *state = d_to_priv(d);
+	int ret;
+
+	ret = state->fe_read_status(fe, status);
+
+	/* it need resync slave fifo when signal change from unlock to lock.*/
+	if ((*status & FE_HAS_LOCK) && (!state->last_lock))
+		dvbsky_stream_ctrl(d, 1);
+
+	state->last_lock = (*status & FE_HAS_LOCK) ? 1 : 0;
+	return ret;
+}
+
+static const struct m88ds3103_config dvbsky_s960_m88ds3103_config = {
+	.i2c_addr = 0x68,
+	.clock = 27000000,
+	.i2c_wr_max = 33,
+	.clock_out = 0,
+	.ts_mode = M88DS3103_TS_CI,
+	.ts_clk = 16000,
+	.ts_clk_pol = 0,
+	.agc = 0x99,
+	.lnb_hv_pol = 1,
+	.lnb_en_pol = 1,
+};
+
+static int dvbsky_s960_attach(struct dvb_usb_adapter *adap)
+{
+	struct dvbsky_state *state = adap_to_priv(adap);
+	struct dvb_usb_device *d = adap_to_d(adap);
+	int ret = 0;
+	/* demod I2C adapter */
+	struct i2c_adapter *i2c_adapter;
+	struct i2c_client *client;
+	struct i2c_board_info info;
+	struct m88ts2022_config m88ts2022_config = {
+			.clock = 27000000,
+		};
+	memset(&info, 0, sizeof(struct i2c_board_info));
+
+	/* attach demod */
+	adap->fe[0] = dvb_attach(m88ds3103_attach,
+			&dvbsky_s960_m88ds3103_config,
+			&d->i2c_adap,
+			&i2c_adapter);
+	if (!adap->fe[0]) {
+		dev_err(&d->udev->dev, "dvbsky_s960_attach fail.\n");
+		ret = -ENODEV;
+		goto fail_attach;
+	}
+
+	/* attach tuner */
+	m88ts2022_config.fe = adap->fe[0];
+	strlcpy(info.type, "m88ts2022", I2C_NAME_SIZE);
+	info.addr = 0x60;
+	info.platform_data = &m88ts2022_config;
+	request_module("m88ts2022");
+	client = i2c_new_device(i2c_adapter, &info);
+	if (client == NULL || client->dev.driver == NULL) {
+		dvb_frontend_detach(adap->fe[0]);
+		ret = -ENODEV;
+		goto fail_attach;
+	}
+
+	if (!try_module_get(client->dev.driver->owner)) {
+		i2c_unregister_device(client);
+		dvb_frontend_detach(adap->fe[0]);
+		ret = -ENODEV;
+		goto fail_attach;
+	}
+
+	/* delegate signal strength measurement to tuner */
+	adap->fe[0]->ops.read_signal_strength =
+			adap->fe[0]->ops.tuner_ops.get_rf_strength;
+
+	/* hook fe: need to resync the slave fifo when signal locks. */
+	state->fe_read_status = adap->fe[0]->ops.read_status;
+	adap->fe[0]->ops.read_status = dvbsky_usb_read_status;
+
+	/* hook fe: LNB off/on is control by Cypress usb chip. */
+	state->fe_set_voltage = adap->fe[0]->ops.set_voltage;
+	adap->fe[0]->ops.set_voltage = dvbsky_usb_set_voltage;
+
+	state->i2c_client_tuner = client;
+
+fail_attach:
+	return ret;
+}
+
+static int dvbsky_identify_state(struct dvb_usb_device *d, const char **name)
+{
+	dvbsky_gpio_ctrl(d, 0x04, 1);
+	msleep(20);
+	dvbsky_gpio_ctrl(d, 0x83, 0);
+	dvbsky_gpio_ctrl(d, 0xc0, 1);
+	msleep(100);
+	dvbsky_gpio_ctrl(d, 0x83, 1);
+	dvbsky_gpio_ctrl(d, 0xc0, 0);
+	msleep(50);
+
+	return WARM;
+}
+
+static int dvbsky_init(struct dvb_usb_device *d)
+{
+	struct dvbsky_state *state = d_to_priv(d);
+
+	/* use default interface */
+	/*
+	ret = usb_set_interface(d->udev, 0, 0);
+	if (ret)
+		return ret;
+	*/
+	mutex_init(&state->stream_mutex);
+
+	state->last_lock = 0;
+
+	return 0;
+}
+
+static void dvbsky_exit(struct dvb_usb_device *d)
+{
+	struct dvbsky_state *state = d_to_priv(d);
+	struct i2c_client *client;
+
+	client = state->i2c_client_tuner;
+	/* remove I2C tuner */
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+}
+
+/* DVB USB Driver stuff */
+static struct dvb_usb_device_properties dvbsky_s960_props = {
+	.driver_name = KBUILD_MODNAME,
+	.owner = THIS_MODULE,
+	.adapter_nr = adapter_nr,
+	.size_of_priv = sizeof(struct dvbsky_state),
+
+	.generic_bulk_ctrl_endpoint = 0x01,
+	.generic_bulk_ctrl_endpoint_response = 0x81,
+	.generic_bulk_ctrl_delay = DVBSKY_MSG_DELAY,
+
+	.i2c_algo         = &dvbsky_i2c_algo,
+	.frontend_attach  = dvbsky_s960_attach,
+	.init             = dvbsky_init,
+	.get_rc_config    = dvbsky_get_rc_config,
+	.streaming_ctrl   = dvbsky_streaming_ctrl,
+	.identify_state	  = dvbsky_identify_state,
+	.exit             = dvbsky_exit,
+	.read_mac_address = dvbsky_read_mac_addr,
+
+	.num_adapters = 1,
+	.adapter = {
+		{
+			.stream = DVB_USB_STREAM_BULK(0x82, 8, 4096),
+		}
+	}
+};
+
+static const struct usb_device_id dvbsky_id_table[] = {
+	{ DVB_USB_DEVICE(0x0572, 0x6831,
+		&dvbsky_s960_props, "DVBSky S960/S860", RC_MAP_DVBSKY) },
+	{ }
+};
+MODULE_DEVICE_TABLE(usb, dvbsky_id_table);
+
+static struct usb_driver dvbsky_usb_driver = {
+	.name = KBUILD_MODNAME,
+	.id_table = dvbsky_id_table,
+	.probe = dvb_usbv2_probe,
+	.disconnect = dvb_usbv2_disconnect,
+	.suspend = dvb_usbv2_suspend,
+	.resume = dvb_usbv2_resume,
+	.reset_resume = dvb_usbv2_reset_resume,
+	.no_dynamic_id = 1,
+	.soft_unbind = 1,
+};
+
+module_usb_driver(dvbsky_usb_driver);
+
+MODULE_AUTHOR("Max nibble <nibble.max@gmail.com>");
+MODULE_DESCRIPTION("Driver for DVBSky USB");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/usb/dvb-usb-v2/lmedm04.c b/drivers/media/usb/dvb-usb-v2/lmedm04.c
index e332af731187..9f2c5459b73a 100644
--- a/drivers/media/usb/dvb-usb-v2/lmedm04.c
+++ b/drivers/media/usb/dvb-usb-v2/lmedm04.c
@@ -1252,7 +1252,7 @@ static int lme2510_get_stream_config(struct dvb_frontend *fe, u8 *ts_type,
 
 	/* Turn PID filter on the fly by module option */
 	if (pid_filter == 2) {
-		adap->pid_filtering  = 1;
+		adap->pid_filtering  = true;
 		adap->max_feed_count = 15;
 	}
 
diff --git a/drivers/media/usb/dvb-usb-v2/mxl111sf.c b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
index b8a707e57b99..c3447eaf1104 100644
--- a/drivers/media/usb/dvb-usb-v2/mxl111sf.c
+++ b/drivers/media/usb/dvb-usb-v2/mxl111sf.c
@@ -31,11 +31,11 @@ module_param_named(debug, dvb_usb_mxl111sf_debug, int, 0644);
 MODULE_PARM_DESC(debug, "set debugging level "
 		 "(1=info, 2=xfer, 4=i2c, 8=reg, 16=adv (or-able)).");
 
-int dvb_usb_mxl111sf_isoc;
+static int dvb_usb_mxl111sf_isoc;
 module_param_named(isoc, dvb_usb_mxl111sf_isoc, int, 0644);
 MODULE_PARM_DESC(isoc, "enable usb isoc xfer (0=bulk, 1=isoc).");
 
-int dvb_usb_mxl111sf_spi;
+static int dvb_usb_mxl111sf_spi;
 module_param_named(spi, dvb_usb_mxl111sf_spi, int, 0644);
 MODULE_PARM_DESC(spi, "use spi rather than tp for data xfer (0=tp, 1=spi).");
 
@@ -43,7 +43,7 @@ MODULE_PARM_DESC(spi, "use spi rather than tp for data xfer (0=tp, 1=spi).");
 #define ANT_PATH_EXTERNAL 1
 #define ANT_PATH_INTERNAL 2
 
-int dvb_usb_mxl111sf_rfswitch =
+static int dvb_usb_mxl111sf_rfswitch =
 #if 0
 		ANT_PATH_AUTO;
 #else
@@ -887,7 +887,7 @@ static u32 mxl111sf_i2c_func(struct i2c_adapter *adapter)
 	return I2C_FUNC_I2C;
 }
 
-struct i2c_algorithm mxl111sf_i2c_algo = {
+static struct i2c_algorithm mxl111sf_i2c_algo = {
 	.master_xfer   = mxl111sf_i2c_xfer,
 	.functionality = mxl111sf_i2c_func,
 #ifdef NEED_ALGO_CONTROL
diff --git a/drivers/media/usb/dvb-usb/Kconfig b/drivers/media/usb/dvb-usb/Kconfig
index 10aef2188fbe..41d3eb922a00 100644
--- a/drivers/media/usb/dvb-usb/Kconfig
+++ b/drivers/media/usb/dvb-usb/Kconfig
@@ -130,7 +130,7 @@ config DVB_USB_CXUSB
 
 	  Medion MD95700 hybrid USB2.0 device.
 	  DViCO FusionHDTV (Bluebird) USB2.0 devices
-	  TechnoTrend TVStick CT2-4400
+	  TechnoTrend TVStick CT2-4400 and CT2-4650 CI devices
 
 config DVB_USB_M920X
 	tristate "Uli m920x DVB-T USB2.0 support"
diff --git a/drivers/media/usb/dvb-usb/af9005.c b/drivers/media/usb/dvb-usb/af9005.c
index af176b6ce738..3f4361e48a32 100644
--- a/drivers/media/usb/dvb-usb/af9005.c
+++ b/drivers/media/usb/dvb-usb/af9005.c
@@ -30,7 +30,7 @@ MODULE_PARM_DESC(debug,
 		 "set debugging level (1=info,xfer=2,rc=4,reg=8,i2c=16,fw=32 (or-able))."
 		 DVB_USB_DEBUG_STATUS);
 /* enable obnoxious led */
-bool dvb_usb_af9005_led = 1;
+bool dvb_usb_af9005_led = true;
 module_param_named(led, dvb_usb_af9005_led, bool, 0644);
 MODULE_PARM_DESC(led, "enable led (default: 1).");
 
diff --git a/drivers/media/usb/dvb-usb/cxusb.c b/drivers/media/usb/dvb-usb/cxusb.c
index 16bc579d1404..356abb369c20 100644
--- a/drivers/media/usb/dvb-usb/cxusb.c
+++ b/drivers/media/usb/dvb-usb/cxusb.c
@@ -44,6 +44,7 @@
 #include "atbm8830.h"
 #include "si2168.h"
 #include "si2157.h"
+#include "sp2.h"
 
 /* Max transfer size done by I2C transfer functions */
 #define MAX_XFER_SIZE  80
@@ -175,7 +176,7 @@ static int cxusb_i2c_xfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 
 	for (i = 0; i < num; i++) {
 
-		if (d->udev->descriptor.idVendor == USB_VID_MEDION)
+		if (le16_to_cpu(d->udev->descriptor.idVendor) == USB_VID_MEDION)
 			switch (msg[i].addr) {
 			case 0x63:
 				cxusb_gpio_tuner(d, 0);
@@ -672,6 +673,70 @@ static struct rc_map_table rc_map_d680_dmb_table[] = {
 	{ 0x0025, KEY_POWER },
 };
 
+static int cxusb_tt_ct2_4400_read_mac_address(struct dvb_usb_device *d, u8 mac[6])
+{
+	u8 wbuf[2];
+	u8 rbuf[6];
+	int ret;
+	struct i2c_msg msg[] = {
+		{
+			.addr = 0x51,
+			.flags = 0,
+			.buf = wbuf,
+			.len = 2,
+		}, {
+			.addr = 0x51,
+			.flags = I2C_M_RD,
+			.buf = rbuf,
+			.len = 6,
+		}
+	};
+
+	wbuf[0] = 0x1e;
+	wbuf[1] = 0x00;
+	ret = cxusb_i2c_xfer(&d->i2c_adap, msg, 2);
+
+	if (ret == 2) {
+		memcpy(mac, rbuf, 6);
+		return 0;
+	} else {
+		if (ret < 0)
+			return ret;
+		return -EIO;
+	}
+}
+
+static int cxusb_tt_ct2_4650_ci_ctrl(void *priv, u8 read, int addr,
+					u8 data, int *mem)
+{
+	struct dvb_usb_device *d = priv;
+	u8 wbuf[3];
+	u8 rbuf[2];
+	int ret;
+
+	wbuf[0] = (addr >> 8) & 0xff;
+	wbuf[1] = addr & 0xff;
+
+	if (read) {
+		ret = cxusb_ctrl_msg(d, CMD_SP2_CI_READ, wbuf, 2, rbuf, 2);
+	} else {
+		wbuf[2] = data;
+		ret = cxusb_ctrl_msg(d, CMD_SP2_CI_WRITE, wbuf, 3, rbuf, 1);
+	}
+
+	if (ret)
+		goto err;
+
+	if (read)
+		*mem = rbuf[1];
+
+	return 0;
+err:
+	deb_info("%s: ci usb write returned %d\n", __func__, ret);
+	return ret;
+
+}
+
 static int cxusb_dee1601_demod_init(struct dvb_frontend* fe)
 {
 	static u8 clock_config []  = { CLOCK_CTL,  0x38, 0x28 };
@@ -1350,9 +1415,12 @@ static int cxusb_tt_ct2_4400_attach(struct dvb_usb_adapter *adap)
 	struct i2c_adapter *adapter;
 	struct i2c_client *client_demod;
 	struct i2c_client *client_tuner;
+	struct i2c_client *client_ci;
 	struct i2c_board_info info;
 	struct si2168_config si2168_config;
 	struct si2157_config si2157_config;
+	struct sp2_config sp2_config;
+	u8 o[2], i;
 
 	/* reset the tuner */
 	if (cxusb_tt_ct2_4400_gpio_tuner(d, 0) < 0) {
@@ -1369,6 +1437,7 @@ static int cxusb_tt_ct2_4400_attach(struct dvb_usb_adapter *adap)
 	/* attach frontend */
 	si2168_config.i2c_adapter = &adapter;
 	si2168_config.fe = &adap->fe_adap[0].fe;
+	si2168_config.ts_mode = SI2168_TS_PARALLEL;
 	memset(&info, 0, sizeof(struct i2c_board_info));
 	strlcpy(info.type, "si2168", I2C_NAME_SIZE);
 	info.addr = 0x64;
@@ -1408,6 +1477,48 @@ static int cxusb_tt_ct2_4400_attach(struct dvb_usb_adapter *adap)
 
 	st->i2c_client_tuner = client_tuner;
 
+	/* initialize CI */
+	if (d->udev->descriptor.idProduct ==
+		USB_PID_TECHNOTREND_CONNECT_CT2_4650_CI) {
+
+		memcpy(o, "\xc0\x01", 2);
+		cxusb_ctrl_msg(d, CMD_GPIO_WRITE, o, 2, &i, 1);
+		msleep(100);
+
+		memcpy(o, "\xc0\x00", 2);
+		cxusb_ctrl_msg(d, CMD_GPIO_WRITE, o, 2, &i, 1);
+		msleep(100);
+
+		memset(&sp2_config, 0, sizeof(sp2_config));
+		sp2_config.dvb_adap = &adap->dvb_adap;
+		sp2_config.priv = d;
+		sp2_config.ci_control = cxusb_tt_ct2_4650_ci_ctrl;
+		memset(&info, 0, sizeof(struct i2c_board_info));
+		strlcpy(info.type, "sp2", I2C_NAME_SIZE);
+		info.addr = 0x40;
+		info.platform_data = &sp2_config;
+		request_module(info.type);
+		client_ci = i2c_new_device(&d->i2c_adap, &info);
+		if (client_ci == NULL || client_ci->dev.driver == NULL) {
+			module_put(client_tuner->dev.driver->owner);
+			i2c_unregister_device(client_tuner);
+			module_put(client_demod->dev.driver->owner);
+			i2c_unregister_device(client_demod);
+			return -ENODEV;
+		}
+		if (!try_module_get(client_ci->dev.driver->owner)) {
+			i2c_unregister_device(client_ci);
+			module_put(client_tuner->dev.driver->owner);
+			i2c_unregister_device(client_tuner);
+			module_put(client_demod->dev.driver->owner);
+			i2c_unregister_device(client_demod);
+			return -ENODEV;
+		}
+
+		st->i2c_client_ci = client_ci;
+
+	}
+
 	return 0;
 }
 
@@ -1537,6 +1648,13 @@ static void cxusb_disconnect(struct usb_interface *intf)
 	struct cxusb_state *st = d->priv;
 	struct i2c_client *client;
 
+	/* remove I2C client for CI */
+	client = st->i2c_client_ci;
+	if (client) {
+		module_put(client->dev.driver->owner);
+		i2c_unregister_device(client);
+	}
+
 	/* remove I2C client for tuner */
 	client = st->i2c_client_tuner;
 	if (client) {
@@ -1576,6 +1694,7 @@ static struct usb_device_id cxusb_table [] = {
 	{ USB_DEVICE(USB_VID_CONEXANT, USB_PID_CONEXANT_D680_DMB) },
 	{ USB_DEVICE(USB_VID_CONEXANT, USB_PID_MYGICA_D689) },
 	{ USB_DEVICE(USB_VID_TECHNOTREND, USB_PID_TECHNOTREND_TVSTICK_CT2_4400) },
+	{ USB_DEVICE(USB_VID_TECHNOTREND, USB_PID_TECHNOTREND_CONNECT_CT2_4650_CI) },
 	{}		/* Terminating entry */
 };
 MODULE_DEVICE_TABLE (usb, cxusb_table);
@@ -2230,6 +2349,8 @@ static struct dvb_usb_device_properties cxusb_tt_ct2_4400_properties = {
 	.size_of_priv     = sizeof(struct cxusb_state),
 
 	.num_adapters = 1,
+	.read_mac_address = cxusb_tt_ct2_4400_read_mac_address,
+
 	.adapter = {
 		{
 		.num_frontends = 1,
@@ -2265,13 +2386,18 @@ static struct dvb_usb_device_properties cxusb_tt_ct2_4400_properties = {
 		.rc_interval    = 150,
 	},
 
-	.num_device_descs = 1,
+	.num_device_descs = 2,
 	.devices = {
 		{
 			"TechnoTrend TVStick CT2-4400",
 			{ NULL },
 			{ &cxusb_table[20], NULL },
 		},
+		{
+			"TechnoTrend TT-connect CT2-4650 CI",
+			{ NULL },
+			{ &cxusb_table[21], NULL },
+		},
 	}
 };
 
diff --git a/drivers/media/usb/dvb-usb/cxusb.h b/drivers/media/usb/dvb-usb/cxusb.h
index 527ff7905e15..29f3e2ea2476 100644
--- a/drivers/media/usb/dvb-usb/cxusb.h
+++ b/drivers/media/usb/dvb-usb/cxusb.h
@@ -28,10 +28,14 @@
 #define CMD_ANALOG        0x50
 #define CMD_DIGITAL       0x51
 
+#define CMD_SP2_CI_WRITE  0x70
+#define CMD_SP2_CI_READ   0x71
+
 struct cxusb_state {
 	u8 gpio_write_state[3];
 	struct i2c_client *i2c_client_demod;
 	struct i2c_client *i2c_client_tuner;
+	struct i2c_client *i2c_client_ci;
 };
 
 #endif
diff --git a/drivers/media/usb/dvb-usb/dib0700_devices.c b/drivers/media/usb/dvb-usb/dib0700_devices.c
index ce47d3f1c850..e1757b8f5f5d 100644
--- a/drivers/media/usb/dvb-usb/dib0700_devices.c
+++ b/drivers/media/usb/dvb-usb/dib0700_devices.c
@@ -220,12 +220,21 @@ static struct dibx000_agc_config stk7700d_7000p_mt2266_agc_config[2] = {
 };
 
 static struct dibx000_bandwidth_config stk7700d_mt2266_pll_config = {
-	60000, 30000,
-	1, 8, 3, 1, 0,
-	0, 0, 1, 1, 2,
-	(3 << 14) | (1 << 12) | (524 << 0),
-	0,
-	20452225,
+	.internal = 60000,
+	.sampling = 30000,
+	.pll_prediv = 1,
+	.pll_ratio = 8,
+	.pll_range = 3,
+	.pll_reset = 1,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 1,
+	.ADClkSrc = 1,
+	.modulo = 2,
+	.sad_cfg = (3 << 14) | (1 << 12) | (524 << 0),
+	.ifreq = 0,
+	.timf = 20452225,
 };
 
 static struct dib7000p_config stk7700d_dib7000p_mt2266_config[] = {
@@ -342,57 +351,57 @@ static int stk7700d_tuner_attach(struct dvb_usb_adapter *adap)
 
 /* STK7700-PH: Digital/Analog Hybrid Tuner, e.h. Cinergy HT USB HE */
 static struct dibx000_agc_config xc3028_agc_config = {
-	BAND_VHF | BAND_UHF,       /* band_caps */
-
+	.band_caps = BAND_VHF | BAND_UHF,
 	/* P_agc_use_sd_mod1=0, P_agc_use_sd_mod2=0, P_agc_freq_pwm_div=0,
 	 * P_agc_inv_pwm1=0, P_agc_inv_pwm2=0, P_agc_inh_dc_rv_est=0,
 	 * P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=2, P_agc_write=0 */
-	(0 << 15) | (0 << 14) | (0 << 11) | (0 << 10) | (0 << 9) | (0 << 8) |
-	(3 << 5) | (0 << 4) | (2 << 1) | (0 << 0), /* setup */
-
-	712,	/* inv_gain */
-	21,	/* time_stabiliz */
-
-	0,	/* alpha_level */
-	118,	/* thlock */
-
-	0,	/* wbd_inv */
-	2867,	/* wbd_ref */
-	0,	/* wbd_sel */
-	2,	/* wbd_alpha */
-
-	0,	/* agc1_max */
-	0,	/* agc1_min */
-	39718,	/* agc2_max */
-	9930,	/* agc2_min */
-	0,	/* agc1_pt1 */
-	0,	/* agc1_pt2 */
-	0,	/* agc1_pt3 */
-	0,	/* agc1_slope1 */
-	0,	/* agc1_slope2 */
-	0,	/* agc2_pt1 */
-	128,	/* agc2_pt2 */
-	29,	/* agc2_slope1 */
-	29,	/* agc2_slope2 */
-
-	17,	/* alpha_mant */
-	27,	/* alpha_exp */
-	23,	/* beta_mant */
-	51,	/* beta_exp */
-
-	1,	/* perform_agc_softsplit */
+	.setup = (0 << 15) | (0 << 14) | (0 << 11) | (0 << 10) | (0 << 9) | (0 << 8) | (3 << 5) | (0 << 4) | (2 << 1) | (0 << 0),
+	.inv_gain = 712,
+	.time_stabiliz = 21,
+	.alpha_level = 0,
+	.thlock = 118,
+	.wbd_inv = 0,
+	.wbd_ref = 2867,
+	.wbd_sel = 0,
+	.wbd_alpha = 2,
+	.agc1_max = 0,
+	.agc1_min = 0,
+	.agc2_max = 39718,
+	.agc2_min = 9930,
+	.agc1_pt1 = 0,
+	.agc1_pt2 = 0,
+	.agc1_pt3 = 0,
+	.agc1_slope1 = 0,
+	.agc1_slope2 = 0,
+	.agc2_pt1 = 0,
+	.agc2_pt2 = 128,
+	.agc2_slope1 = 29,
+	.agc2_slope2 = 29,
+	.alpha_mant = 17,
+	.alpha_exp = 27,
+	.beta_mant = 23,
+	.beta_exp = 51,
+	.perform_agc_softsplit = 1,
 };
 
 /* PLL Configuration for COFDM BW_MHz = 8.00 with external clock = 30.00 */
 static struct dibx000_bandwidth_config xc3028_bw_config = {
-	60000, 30000, /* internal, sampling */
-	1, 8, 3, 1, 0, /* pll_cfg: prediv, ratio, range, reset, bypass */
-	0, 0, 1, 1, 0, /* misc: refdiv, bypclk_div, IO_CLK_en_core, ADClkSrc,
-			  modulo */
-	(3 << 14) | (1 << 12) | (524 << 0), /* sad_cfg: refsel, sel, freq_15k */
-	(1 << 25) | 5816102, /* ifreq = 5.200000 MHz */
-	20452225, /* timf */
-	30000000, /* xtal_hz */
+	.internal = 60000,
+	.sampling = 30000,
+	.pll_prediv = 1,
+	.pll_ratio = 8,
+	.pll_range = 3,
+	.pll_reset = 1,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 1,
+	.ADClkSrc = 1,
+	.modulo = 0,
+	.sad_cfg = (3 << 14) | (1 << 12) | (524 << 0), /* sad_cfg: refsel, sel, freq_15k */
+	.ifreq = (1 << 25) | 5816102,  /* ifreq = 5.200000 MHz */
+	.timf = 20452225,
+	.xtal_hz = 30000000,
 };
 
 static struct dib7000p_config stk7700ph_dib7700_xc3028_config = {
@@ -614,59 +623,55 @@ static struct dibx000_agc_config stk7700p_7000m_mt2060_agc_config = {
 };
 
 static struct dibx000_agc_config stk7700p_7000p_mt2060_agc_config = {
-	BAND_UHF | BAND_VHF,
-
+	.band_caps = BAND_UHF | BAND_VHF,
 	/* P_agc_use_sd_mod1=0, P_agc_use_sd_mod2=0, P_agc_freq_pwm_div=5, P_agc_inv_pwm1=0, P_agc_inv_pwm2=0,
 	 * P_agc_inh_dc_rv_est=0, P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=2, P_agc_write=0 */
-	(0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8)
-	| (3 << 5) | (0 << 4) | (2 << 1) | (0 << 0),
-
-	712,
-	41,
-
-	0,
-	118,
-
-	0,
-	4095,
-	0,
-	0,
-
-	42598,
-	16384,
-	42598,
-	    0,
-
-	  0,
-	137,
-	255,
-
-	  0,
-	255,
-
-	0,
-	0,
-
-	 0,
-	41,
-
-	15,
-	25,
-
-	28,
-	48,
-
-	0,
+	.setup = (0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8) | (3 << 5) | (0 << 4) | (2 << 1) | (0 << 0),
+	.inv_gain = 712,
+	.time_stabiliz = 41,
+	.alpha_level = 0,
+	.thlock = 118,
+	.wbd_inv = 0,
+	.wbd_ref = 4095,
+	.wbd_sel = 0,
+	.wbd_alpha = 0,
+	.agc1_max = 42598,
+	.agc1_min = 16384,
+	.agc2_max = 42598,
+	.agc2_min = 0,
+	.agc1_pt1 = 0,
+	.agc1_pt2 = 137,
+	.agc1_pt3 = 255,
+	.agc1_slope1 = 0,
+	.agc1_slope2 = 255,
+	.agc2_pt1 = 0,
+	.agc2_pt2 = 0,
+	.agc2_slope1 = 0,
+	.agc2_slope2 = 41,
+	.alpha_mant = 15,
+	.alpha_exp = 25,
+	.beta_mant = 28,
+	.beta_exp = 48,
+	.perform_agc_softsplit = 0,
 };
 
 static struct dibx000_bandwidth_config stk7700p_pll_config = {
-	60000, 30000,
-	1, 8, 3, 1, 0,
-	0, 0, 1, 1, 0,
-	(3 << 14) | (1 << 12) | (524 << 0),
-	60258167,
-	20452225,
-	30000000,
+	.internal = 60000,
+	.sampling = 30000,
+	.pll_prediv = 1,
+	.pll_ratio = 8,
+	.pll_range = 3,
+	.pll_reset = 1,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 1,
+	.ADClkSrc = 1,
+	.modulo = 0,
+	.sad_cfg = (3 << 14) | (1 << 12) | (524 << 0),
+	.ifreq = 60258167,
+	.timf = 20452225,
+	.xtal_hz = 30000000,
 };
 
 static struct dib7000m_config stk7700p_dib7000m_config = {
@@ -758,45 +763,36 @@ static int stk7700p_tuner_attach(struct dvb_usb_adapter *adap)
 
 /* DIB7070 generic */
 static struct dibx000_agc_config dib7070_agc_config = {
-	BAND_UHF | BAND_VHF | BAND_LBAND | BAND_SBAND,
+	.band_caps = BAND_UHF | BAND_VHF | BAND_LBAND | BAND_SBAND,
 	/* P_agc_use_sd_mod1=0, P_agc_use_sd_mod2=0, P_agc_freq_pwm_div=5, P_agc_inv_pwm1=0, P_agc_inv_pwm2=0,
 	 * P_agc_inh_dc_rv_est=0, P_agc_time_est=3, P_agc_freeze=0, P_agc_nb_est=5, P_agc_write=0 */
-	(0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8)
-	| (3 << 5) | (0 << 4) | (5 << 1) | (0 << 0),
-
-	600,
-	10,
-
-	0,
-	118,
-
-	0,
-	3530,
-	1,
-	5,
-
-	65535,
-		0,
-
-	65535,
-	0,
-
-	0,
-	40,
-	183,
-	206,
-	255,
-	72,
-	152,
-	88,
-	90,
-
-	17,
-	27,
-	23,
-	51,
-
-	0,
+	.setup = (0 << 15) | (0 << 14) | (5 << 11) | (0 << 10) | (0 << 9) | (0 << 8) | (3 << 5) | (0 << 4) | (5 << 1) | (0 << 0),
+	.inv_gain = 600,
+	.time_stabiliz = 10,
+	.alpha_level = 0,
+	.thlock = 118,
+	.wbd_inv = 0,
+	.wbd_ref = 3530,
+	.wbd_sel = 1,
+	.wbd_alpha = 5,
+	.agc1_max = 65535,
+	.agc1_min = 0,
+	.agc2_max = 65535,
+	.agc2_min = 0,
+	.agc1_pt1 = 0,
+	.agc1_pt2 = 40,
+	.agc1_pt3 = 183,
+	.agc1_slope1 = 206,
+	.agc1_slope2 = 255,
+	.agc2_pt1 = 72,
+	.agc2_pt2 = 152,
+	.agc2_slope1 = 88,
+	.agc2_slope2 = 90,
+	.alpha_mant = 17,
+	.alpha_exp = 27,
+	.beta_mant = 23,
+	.beta_exp = 51,
+	.perform_agc_softsplit = 0,
 };
 
 static int dib7070_tuner_reset(struct dvb_frontend *fe, int onoff)
@@ -952,13 +948,22 @@ static int stk70x0p_pid_filter_ctrl(struct dvb_usb_adapter *adapter, int onoff)
 }
 
 static struct dibx000_bandwidth_config dib7070_bw_config_12_mhz = {
-	60000, 15000,
-	1, 20, 3, 1, 0,
-	0, 0, 1, 1, 2,
-	(3 << 14) | (1 << 12) | (524 << 0),
-	(0 << 25) | 0,
-	20452225,
-	12000000,
+	.internal = 60000,
+	.sampling = 15000,
+	.pll_prediv = 1,
+	.pll_ratio = 20,
+	.pll_range = 3,
+	.pll_reset = 1,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 1,
+	.ADClkSrc = 1,
+	.modulo = 2,
+	.sad_cfg = (3 << 14) | (1 << 12) | (524 << 0),
+	.ifreq = (0 << 25) | 0,
+	.timf = 20452225,
+	.xtal_hz = 12000000,
 };
 
 static struct dib7000p_config dib7070p_dib7000p_config = {
@@ -1169,14 +1174,22 @@ static struct dibx000_agc_config dib807x_agc_config[2] = {
 };
 
 static struct dibx000_bandwidth_config dib807x_bw_config_12_mhz = {
-	60000, 15000, /* internal, sampling*/
-	1, 20, 3, 1, 0, /* pll_cfg: prediv, ratio, range, reset, bypass*/
-	0, 0, 1, 1, 2, /* misc: refdiv, bypclk_div, IO_CLK_en_core,
-			  ADClkSrc, modulo */
-	(3 << 14) | (1 << 12) | (599 << 0), /* sad_cfg: refsel, sel, freq_15k*/
-	(0 << 25) | 0, /* ifreq = 0.000000 MHz*/
-	18179755, /* timf*/
-	12000000, /* xtal_hz*/
+	.internal = 60000,
+	.sampling = 15000,
+	.pll_prediv = 1,
+	.pll_ratio = 20,
+	.pll_range = 3,
+	.pll_reset = 1,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 1,
+	.ADClkSrc = 1,
+	.modulo = 2,
+	.sad_cfg = (3 << 14) | (1 << 12) | (599 << 0),	/* sad_cfg: refsel, sel, freq_15k*/
+	.ifreq = (0 << 25) | 0,				/* ifreq = 0.000000 MHz*/
+	.timf = 18179755,
+	.xtal_hz = 12000000,
 };
 
 static struct dib8000_config dib807x_dib8000_config[2] = {
@@ -1921,13 +1934,22 @@ static struct dibx000_agc_config dib8096p_agc_config[2] = {
 };
 
 static struct dibx000_bandwidth_config dib8096p_clock_config_12_mhz = {
-	108000, 13500,
-	1, 9, 1, 0, 0,
-	0, 0, 0, 0, 2,
-	(3 << 14) | (1 << 12) | (524 << 0),
-	(0 << 25) | 0,
-	20199729,
-	12000000,
+	.internal = 108000,
+	.sampling = 13500,
+	.pll_prediv = 1,
+	.pll_ratio = 9,
+	.pll_range = 1,
+	.pll_reset = 0,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 0,
+	.ADClkSrc = 0,
+	.modulo = 2,
+	.sad_cfg = (3 << 14) | (1 << 12) | (524 << 0),
+	.ifreq = (0 << 25) | 0,
+	.timf = 20199729,
+	.xtal_hz = 12000000,
 };
 
 static struct dib8000_config tfe8096p_dib8000_config = {
@@ -2724,13 +2746,22 @@ static struct dibx000_agc_config dib7090_agc_config[2] = {
 };
 
 static struct dibx000_bandwidth_config dib7090_clock_config_12_mhz = {
-	60000, 15000,
-	1, 5, 0, 0, 0,
-	0, 0, 1, 1, 2,
-	(3 << 14) | (1 << 12) | (524 << 0),
-	(0 << 25) | 0,
-	20452225,
-	15000000,
+	.internal = 60000,
+	.sampling = 15000,
+	.pll_prediv = 1,
+	.pll_ratio = 5,
+	.pll_range = 0,
+	.pll_reset = 0,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 1,
+	.ADClkSrc = 1,
+	.modulo = 2,
+	.sad_cfg = (3 << 14) | (1 << 12) | (524 << 0),
+	.ifreq = (0 << 25) | 0,
+	.timf = 20452225,
+	.xtal_hz = 15000000,
 };
 
 static struct dib7000p_config nim7090_dib7000p_config = {
@@ -3498,14 +3529,22 @@ static struct dibx000_agc_config stk7700p_7000p_xc4000_agc_config = {
 };
 
 static struct dibx000_bandwidth_config stk7700p_xc4000_pll_config = {
-	60000, 30000,	/* internal, sampling */
-	1, 8, 3, 1, 0,	/* pll_cfg: prediv, ratio, range, reset, bypass */
-	0, 0, 1, 1, 0,	/* misc: refdiv, bypclk_div, IO_CLK_en_core, */
-			/* ADClkSrc, modulo */
-	(3 << 14) | (1 << 12) | 524,	/* sad_cfg: refsel, sel, freq_15k */
-	39370534,	/* ifreq */
-	20452225,	/* timf */
-	30000000	/* xtal */
+	.internal = 60000,
+	.sampling = 30000,
+	.pll_prediv = 1,
+	.pll_ratio = 8,
+	.pll_range = 3,
+	.pll_reset = 1,
+	.pll_bypass = 0,
+	.enable_refdiv = 0,
+	.bypclk_div = 0,
+	.IO_CLK_en_core = 1,
+	.ADClkSrc = 1,
+	.modulo = 0,
+	.sad_cfg = (3 << 14) | (1 << 12) | 524, /* sad_cfg: refsel, sel, freq_15k */
+	.ifreq = 39370534,
+	.timf = 20452225,
+	.xtal_hz = 30000000
 };
 
 /* FIXME: none of these inputs are validated yet */
diff --git a/drivers/media/usb/dvb-usb/dibusb-common.c b/drivers/media/usb/dvb-usb/dibusb-common.c
index 6d68af0c49c8..ef3a8f75f82e 100644
--- a/drivers/media/usb/dvb-usb/dibusb-common.c
+++ b/drivers/media/usb/dvb-usb/dibusb-common.c
@@ -258,8 +258,8 @@ static struct dib3000mc_config mod3000p_dib3000p_config = {
 
 int dibusb_dib3000mc_frontend_attach(struct dvb_usb_adapter *adap)
 {
-	if (adap->dev->udev->descriptor.idVendor  == USB_VID_LITEON &&
-			adap->dev->udev->descriptor.idProduct ==
+	if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_LITEON &&
+	    le16_to_cpu(adap->dev->udev->descriptor.idProduct) ==
 			USB_PID_LITEON_DVB_T_WARM) {
 		msleep(1000);
 	}
@@ -297,8 +297,8 @@ int dibusb_dib3000mc_tuner_attach(struct dvb_usb_adapter *adap)
 	struct i2c_adapter *tun_i2c;
 
 	// First IF calibration for Liteon Sticks
-	if (adap->dev->udev->descriptor.idVendor  == USB_VID_LITEON &&
-		adap->dev->udev->descriptor.idProduct == USB_PID_LITEON_DVB_T_WARM) {
+	if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_LITEON &&
+	    le16_to_cpu(adap->dev->udev->descriptor.idProduct) == USB_PID_LITEON_DVB_T_WARM) {
 
 		dibusb_read_eeprom_byte(adap->dev,0x7E,&a);
 		dibusb_read_eeprom_byte(adap->dev,0x7F,&b);
@@ -310,8 +310,8 @@ int dibusb_dib3000mc_tuner_attach(struct dvb_usb_adapter *adap)
 		else
 			warn("LITE-ON DVB-T: Strange IF1 calibration :%2X %2X\n", a, b);
 
-	} else if (adap->dev->udev->descriptor.idVendor  == USB_VID_DIBCOM &&
-		   adap->dev->udev->descriptor.idProduct == USB_PID_DIBCOM_MOD3001_WARM) {
+	} else if (le16_to_cpu(adap->dev->udev->descriptor.idVendor) == USB_VID_DIBCOM &&
+		   le16_to_cpu(adap->dev->udev->descriptor.idProduct) == USB_PID_DIBCOM_MOD3001_WARM) {
 		u8 desc;
 		dibusb_read_eeprom_byte(adap->dev, 7, &desc);
 		if (desc == 2) {
diff --git a/drivers/media/usb/dvb-usb/dw2102.c b/drivers/media/usb/dvb-usb/dw2102.c
index 2add8c507ec9..1a3df10d6bad 100644
--- a/drivers/media/usb/dvb-usb/dw2102.c
+++ b/drivers/media/usb/dvb-usb/dw2102.c
@@ -667,7 +667,7 @@ static int s6x0_i2c_transfer(struct i2c_adapter *adap, struct i2c_msg msg[],
 				obuf[1] = (msg[j].addr << 1);
 				memcpy(obuf + 2, msg[j].buf, msg[j].len);
 				dw210x_op_rw(d->udev,
-						udev->descriptor.idProduct ==
+						le16_to_cpu(udev->descriptor.idProduct) ==
 						0x7500 ? 0x92 : 0x90, 0, 0,
 						obuf, msg[j].len + 2,
 						DW210X_WRITE_MSG);
@@ -1598,7 +1598,7 @@ static int dw2102_load_firmware(struct usb_device *dev,
 	u8 reset16[] = {0, 0, 0, 0, 0, 0, 0};
 	const struct firmware *fw;
 
-	switch (dev->descriptor.idProduct) {
+	switch (le16_to_cpu(dev->descriptor.idProduct)) {
 	case 0x2101:
 		ret = request_firmware(&fw, DW2101_FIRMWARE, &dev->dev);
 		if (ret != 0) {
@@ -1641,7 +1641,7 @@ static int dw2102_load_firmware(struct usb_device *dev,
 			ret = -EINVAL;
 		}
 		/* init registers */
-		switch (dev->descriptor.idProduct) {
+		switch (le16_to_cpu(dev->descriptor.idProduct)) {
 		case USB_PID_TEVII_S650:
 			dw2104_properties.rc.core.rc_codes = RC_MAP_TEVII_NEC;
 		case USB_PID_DW2104:
@@ -1901,14 +1901,14 @@ static struct dvb_usb_device_properties s6x0_properties = {
 	}
 };
 
-struct dvb_usb_device_properties *p1100;
+static struct dvb_usb_device_properties *p1100;
 static struct dvb_usb_device_description d1100 = {
 	"Prof 1100 USB ",
 	{&dw2102_table[PROF_1100], NULL},
 	{NULL},
 };
 
-struct dvb_usb_device_properties *s660;
+static struct dvb_usb_device_properties *s660;
 static struct dvb_usb_device_description d660 = {
 	"TeVii S660 USB",
 	{&dw2102_table[TEVII_S660], NULL},
@@ -1927,14 +1927,14 @@ static struct dvb_usb_device_description d480_2 = {
 	{NULL},
 };
 
-struct dvb_usb_device_properties *p7500;
+static struct dvb_usb_device_properties *p7500;
 static struct dvb_usb_device_description d7500 = {
 	"Prof 7500 USB DVB-S2",
 	{&dw2102_table[PROF_7500], NULL},
 	{NULL},
 };
 
-struct dvb_usb_device_properties *s421;
+static struct dvb_usb_device_properties *s421;
 static struct dvb_usb_device_description d421 = {
 	"TeVii S421 PCI",
 	{&dw2102_table[TEVII_S421], NULL},
diff --git a/drivers/media/usb/dvb-usb/opera1.c b/drivers/media/usb/dvb-usb/opera1.c
index 16ba90acf539..14a2119912ba 100644
--- a/drivers/media/usb/dvb-usb/opera1.c
+++ b/drivers/media/usb/dvb-usb/opera1.c
@@ -554,8 +554,8 @@ static int opera1_probe(struct usb_interface *intf,
 {
 	struct usb_device *udev = interface_to_usbdev(intf);
 
-	if (udev->descriptor.idProduct == USB_PID_OPERA1_WARM &&
-		udev->descriptor.idVendor == USB_VID_OPERA1 &&
+	if (le16_to_cpu(udev->descriptor.idProduct) == USB_PID_OPERA1_WARM &&
+	    le16_to_cpu(udev->descriptor.idVendor) == USB_VID_OPERA1 &&
 		opera1_xilinx_load_firmware(udev, "dvb-usb-opera1-fpga-01.fw") != 0
 	    ) {
 		return -EINVAL;
diff --git a/drivers/media/usb/dvb-usb/pctv452e.c b/drivers/media/usb/dvb-usb/pctv452e.c
index bdfe8963591c..d17618fe8f5c 100644
--- a/drivers/media/usb/dvb-usb/pctv452e.c
+++ b/drivers/media/usb/dvb-usb/pctv452e.c
@@ -883,7 +883,7 @@ static int pctv452e_frontend_attach(struct dvb_usb_adapter *a)
 	if (!a->fe_adap[0].fe)
 		return -ENODEV;
 	if ((dvb_attach(lnbp22_attach, a->fe_adap[0].fe,
-					&a->dev->i2c_adap)) == 0)
+					&a->dev->i2c_adap)) == NULL)
 		err("Cannot attach lnbp22\n");
 
 	id = a->dev->desc->warm_ids[0];
@@ -900,7 +900,7 @@ static int pctv452e_tuner_attach(struct dvb_usb_adapter *a)
 	if (!a->fe_adap[0].fe)
 		return -ENODEV;
 	if (dvb_attach(stb6100_attach, a->fe_adap[0].fe, &stb6100_config,
-					&a->dev->i2c_adap) == 0) {
+					&a->dev->i2c_adap) == NULL) {
 		err("%s failed\n", __func__);
 		return -ENODEV;
 	}
@@ -965,7 +965,7 @@ static struct dvb_usb_device_properties pctv452e_properties = {
 		  .cold_ids = { NULL, NULL }, /* this is a warm only device */
 		  .warm_ids = { &pctv452e_usb_table[0], NULL }
 		},
-		{ 0 },
+		{ NULL },
 	}
 };
 
@@ -1023,7 +1023,7 @@ static struct dvb_usb_device_properties tt_connect_s2_3600_properties = {
 		  .cold_ids = { NULL, NULL },
 		  .warm_ids = { &pctv452e_usb_table[2], NULL }
 		},
-		{ 0 },
+		{ NULL },
 	}
 };
 
diff --git a/drivers/media/usb/em28xx/em28xx-audio.c b/drivers/media/usb/em28xx/em28xx-audio.c
index e881ef7b6445..957c7ae30efe 100644
--- a/drivers/media/usb/em28xx/em28xx-audio.c
+++ b/drivers/media/usb/em28xx/em28xx-audio.c
@@ -268,7 +268,7 @@ static int snd_em28xx_capture_open(struct snd_pcm_substream *substream)
 	nonblock = !!(substream->f_flags & O_NONBLOCK);
 	if (nonblock) {
 		if (!mutex_trylock(&dev->lock))
-		return -EAGAIN;
+			return -EAGAIN;
 	} else
 		mutex_lock(&dev->lock);
 
@@ -893,7 +893,7 @@ static int em28xx_audio_init(struct em28xx *dev)
 	static int          devnr;
 	int		    err;
 
-	if (!dev->has_alsa_audio) {
+	if (dev->usb_audio_type != EM28XX_USB_AUDIO_VENDOR) {
 		/* This device does not support the extension (in this case
 		   the device is expecting the snd-usb-audio module or
 		   doesn't have analog audio support at all) */
@@ -975,7 +975,7 @@ static int em28xx_audio_fini(struct em28xx *dev)
 	if (dev == NULL)
 		return 0;
 
-	if (!dev->has_alsa_audio) {
+	if (dev->usb_audio_type != EM28XX_USB_AUDIO_VENDOR) {
 		/* This device does not support the extension (in this case
 		   the device is expecting the snd-usb-audio module or
 		   doesn't have analog audio support at all) */
@@ -1003,7 +1003,7 @@ static int em28xx_audio_suspend(struct em28xx *dev)
 	if (dev == NULL)
 		return 0;
 
-	if (!dev->has_alsa_audio)
+	if (dev->usb_audio_type != EM28XX_USB_AUDIO_VENDOR)
 		return 0;
 
 	em28xx_info("Suspending audio extension");
@@ -1017,7 +1017,7 @@ static int em28xx_audio_resume(struct em28xx *dev)
 	if (dev == NULL)
 		return 0;
 
-	if (!dev->has_alsa_audio)
+	if (dev->usb_audio_type != EM28XX_USB_AUDIO_VENDOR)
 		return 0;
 
 	em28xx_info("Resuming audio extension");
diff --git a/drivers/media/usb/em28xx/em28xx-cards.c b/drivers/media/usb/em28xx/em28xx-cards.c
index a7e24848f6c8..71fa51e7984e 100644
--- a/drivers/media/usb/em28xx/em28xx-cards.c
+++ b/drivers/media/usb/em28xx/em28xx-cards.c
@@ -2246,7 +2246,7 @@ struct em28xx_board em28xx_boards[] = {
 };
 EXPORT_SYMBOL_GPL(em28xx_boards);
 
-const unsigned int em28xx_bcount = ARRAY_SIZE(em28xx_boards);
+static const unsigned int em28xx_bcount = ARRAY_SIZE(em28xx_boards);
 
 /* table of devices that work with this driver */
 struct usb_device_id em28xx_id_table[] = {
@@ -2931,9 +2931,9 @@ static void request_module_async(struct work_struct *work)
 #if defined(CONFIG_MODULES) && defined(MODULE)
 	if (dev->has_video)
 		request_module("em28xx-v4l");
-	if (dev->has_audio_class)
+	if (dev->usb_audio_type == EM28XX_USB_AUDIO_CLASS)
 		request_module("snd-usb-audio");
-	else if (dev->has_alsa_audio)
+	else if (dev->usb_audio_type == EM28XX_USB_AUDIO_VENDOR)
 		request_module("em28xx-alsa");
 	if (dev->board.has_dvb)
 		request_module("em28xx-dvb");
@@ -3098,16 +3098,6 @@ static int em28xx_init_dev(struct em28xx *dev, struct usb_device *udev,
 		}
 	}
 
-	if (dev->chip_id == CHIP_ID_EM2870 ||
-	    dev->chip_id == CHIP_ID_EM2874 ||
-	    dev->chip_id == CHIP_ID_EM28174 ||
-	    dev->chip_id == CHIP_ID_EM28178) {
-		/* Digital only device - don't load any alsa module */
-		dev->audio_mode.has_audio = false;
-		dev->has_audio_class = false;
-		dev->has_alsa_audio = false;
-	}
-
 	if (chip_name != default_chip_name)
 		printk(KERN_INFO DRIVER_NAME
 		       ": chip ID is %s\n", chip_name);
@@ -3190,7 +3180,7 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 	struct usb_device *udev;
 	struct em28xx *dev = NULL;
 	int retval;
-	bool has_audio = false, has_video = false, has_dvb = false;
+	bool has_vendor_audio = false, has_video = false, has_dvb = false;
 	int i, nr, try_bulk;
 	const int ifnum = interface->altsetting[0].desc.bInterfaceNumber;
 	char *speed;
@@ -3272,7 +3262,7 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 					break;
 				case 0x83:
 					if (usb_endpoint_xfer_isoc(e)) {
-						has_audio = true;
+						has_vendor_audio = true;
 					} else {
 						printk(KERN_INFO DRIVER_NAME
 						": error: skipping audio endpoint 0x83, because it uses bulk transfers !\n");
@@ -3328,7 +3318,7 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 		}
 	}
 
-	if (!(has_audio || has_video || has_dvb)) {
+	if (!(has_vendor_audio || has_video || has_dvb)) {
 		retval = -ENODEV;
 		goto err_free;
 	}
@@ -3375,26 +3365,27 @@ static int em28xx_usb_probe(struct usb_interface *interface,
 	dev->devno = nr;
 	dev->model = id->driver_info;
 	dev->alt   = -1;
-	dev->is_audio_only = has_audio && !(has_video || has_dvb);
-	dev->has_alsa_audio = has_audio;
-	dev->audio_mode.has_audio = has_audio;
+	dev->is_audio_only = has_vendor_audio && !(has_video || has_dvb);
 	dev->has_video = has_video;
 	dev->ifnum = ifnum;
 
-	/* Checks if audio is provided by some interface */
+	if (has_vendor_audio) {
+		printk(KERN_INFO DRIVER_NAME ": Audio interface %i found %s\n",
+		       ifnum, "(Vendor Class)");
+		dev->usb_audio_type = EM28XX_USB_AUDIO_VENDOR;
+	}
+	/* Checks if audio is provided by a USB Audio Class interface */
 	for (i = 0; i < udev->config->desc.bNumInterfaces; i++) {
 		struct usb_interface *uif = udev->config->interface[i];
 		if (uif->altsetting[0].desc.bInterfaceClass == USB_CLASS_AUDIO) {
-			dev->has_audio_class = 1;
+			if (has_vendor_audio)
+				em28xx_err("em28xx: device seems to have vendor AND usb audio class interfaces !\n"
+					   "\t\tThe vendor interface will be ignored. Please contact the developers <linux-media@vger.kernel.org>\n");
+			dev->usb_audio_type = EM28XX_USB_AUDIO_CLASS;
 			break;
 		}
 	}
 
-	if (has_audio)
-		printk(KERN_INFO DRIVER_NAME
-		       ": Audio interface %i found %s\n",
-		       ifnum,
-		       dev->has_audio_class ? "(USB Audio Class)" : "(Vendor Class)");
 	if (has_video)
 		printk(KERN_INFO DRIVER_NAME
 		       ": Video interface %i found:%s%s\n",
@@ -3524,6 +3515,7 @@ static struct usb_driver em28xx_usb_driver = {
 	.disconnect = em28xx_usb_disconnect,
 	.suspend = em28xx_usb_suspend,
 	.resume = em28xx_usb_resume,
+	.reset_resume = em28xx_usb_resume,
 	.id_table = em28xx_id_table,
 };
 
diff --git a/drivers/media/usb/em28xx/em28xx-core.c b/drivers/media/usb/em28xx/em28xx-core.c
index 523d7e92bf47..b5e52fe7957a 100644
--- a/drivers/media/usb/em28xx/em28xx-core.c
+++ b/drivers/media/usb/em28xx/em28xx-core.c
@@ -279,7 +279,7 @@ int em28xx_read_ac97(struct em28xx *dev, u8 reg)
 {
 	int ret;
 	u8 addr = (reg & 0x7f) | 0x80;
-	u16 val;
+	__le16 val;
 
 	ret = em28xx_is_ac97_ready(dev);
 	if (ret < 0)
@@ -433,7 +433,7 @@ int em28xx_audio_analog_set(struct em28xx *dev)
 	int ret, i;
 	u8 xclk;
 
-	if (!dev->audio_mode.has_audio)
+	if (dev->int_audio_type == EM28XX_INT_AUDIO_NONE)
 		return 0;
 
 	/* It is assumed that all devices use master volume for output.
@@ -505,37 +505,48 @@ int em28xx_audio_setup(struct em28xx *dev)
 {
 	int vid1, vid2, feat, cfg;
 	u32 vid;
+	u8 i2s_samplerates;
 
-	if (!dev->audio_mode.has_audio)
+	if (dev->chip_id == CHIP_ID_EM2870 ||
+	    dev->chip_id == CHIP_ID_EM2874 ||
+	    dev->chip_id == CHIP_ID_EM28174 ||
+	    dev->chip_id == CHIP_ID_EM28178) {
+		/* Digital only device - don't load any alsa module */
+		dev->int_audio_type = EM28XX_INT_AUDIO_NONE;
+		dev->usb_audio_type = EM28XX_USB_AUDIO_NONE;
 		return 0;
+	}
 
 	/* See how this device is configured */
 	cfg = em28xx_read_reg(dev, EM28XX_R00_CHIPCFG);
 	em28xx_info("Config register raw data: 0x%02x\n", cfg);
-	if (cfg < 0) {
-		/* Register read error?  */
-		cfg = EM28XX_CHIPCFG_AC97; /* Be conservative */
+	if (cfg < 0) { /* Register read error */
+		/* Be conservative */
+		dev->int_audio_type = EM28XX_INT_AUDIO_AC97;
 	} else if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) == 0x00) {
 		/* The device doesn't have vendor audio at all */
-		dev->has_alsa_audio = false;
-		dev->audio_mode.has_audio = false;
+		dev->int_audio_type = EM28XX_INT_AUDIO_NONE;
+		dev->usb_audio_type = EM28XX_USB_AUDIO_NONE;
 		return 0;
 	} else if ((cfg & EM28XX_CHIPCFG_AUDIOMASK) != EM28XX_CHIPCFG_AC97) {
+		dev->int_audio_type = EM28XX_INT_AUDIO_I2S;
 		if (dev->chip_id < CHIP_ID_EM2860 &&
 	            (cfg & EM28XX_CHIPCFG_AUDIOMASK) ==
 		    EM2820_CHIPCFG_I2S_1_SAMPRATE)
-			dev->audio_mode.i2s_samplerates = 1;
+			i2s_samplerates = 1;
 		else if (dev->chip_id >= CHIP_ID_EM2860 &&
 			 (cfg & EM28XX_CHIPCFG_AUDIOMASK) ==
 			 EM2860_CHIPCFG_I2S_5_SAMPRATES)
-			dev->audio_mode.i2s_samplerates = 5;
+			i2s_samplerates = 5;
 		else
-			dev->audio_mode.i2s_samplerates = 3;
+			i2s_samplerates = 3;
 		em28xx_info("I2S Audio (%d sample rate(s))\n",
-					       dev->audio_mode.i2s_samplerates);
+					       i2s_samplerates);
 		/* Skip the code that does AC97 vendor detection */
 		dev->audio_mode.ac97 = EM28XX_NO_AC97;
 		goto init_audio;
+	} else {
+		dev->int_audio_type = EM28XX_INT_AUDIO_AC97;
 	}
 
 	dev->audio_mode.ac97 = EM28XX_AC97_OTHER;
@@ -549,8 +560,9 @@ int em28xx_audio_setup(struct em28xx *dev)
 		 */
 		em28xx_warn("AC97 chip type couldn't be determined\n");
 		dev->audio_mode.ac97 = EM28XX_NO_AC97;
-		dev->has_alsa_audio = false;
-		dev->audio_mode.has_audio = false;
+		if (dev->usb_audio_type == EM28XX_USB_AUDIO_VENDOR)
+			dev->usb_audio_type = EM28XX_USB_AUDIO_NONE;
+		dev->int_audio_type = EM28XX_INT_AUDIO_NONE;
 		goto init_audio;
 	}
 
@@ -559,15 +571,12 @@ int em28xx_audio_setup(struct em28xx *dev)
 		goto init_audio;
 
 	vid = vid1 << 16 | vid2;
-
-	dev->audio_mode.ac97_vendor_id = vid;
 	em28xx_warn("AC97 vendor ID = 0x%08x\n", vid);
 
 	feat = em28xx_read_ac97(dev, AC97_RESET);
 	if (feat < 0)
 		goto init_audio;
 
-	dev->audio_mode.ac97_feat = feat;
 	em28xx_warn("AC97 features = 0x%04x\n", feat);
 
 	/* Try to identify what audio processor we have */
@@ -586,8 +595,8 @@ init_audio:
 		em28xx_info("Empia 202 AC97 audio processor detected\n");
 		break;
 	case EM28XX_AC97_SIGMATEL:
-		em28xx_info("Sigmatel audio processor detected(stac 97%02x)\n",
-			    dev->audio_mode.ac97_vendor_id & 0xff);
+		em28xx_info("Sigmatel audio processor detected (stac 97%02x)\n",
+			    vid & 0xff);
 		break;
 	case EM28XX_AC97_OTHER:
 		em28xx_warn("Unknown AC97 audio processor detected!\n");
diff --git a/drivers/media/usb/em28xx/em28xx-dvb.c b/drivers/media/usb/em28xx/em28xx-dvb.c
index 3a3e243edf89..9682c52d67d1 100644
--- a/drivers/media/usb/em28xx/em28xx-dvb.c
+++ b/drivers/media/usb/em28xx/em28xx-dvb.c
@@ -373,7 +373,6 @@ static struct tda18271_config kworld_ub435q_v2_config = {
 };
 
 static struct tda18212_config kworld_ub435q_v3_config = {
-	.i2c_address	= 0x60,
 	.if_atsc_vsb	= 3600,
 	.if_atsc_qam	= 3600,
 };
@@ -856,7 +855,9 @@ static const struct m88ds3103_config pctv_461e_m88ds3103_config = {
 	.clock = 27000000,
 	.i2c_wr_max = 33,
 	.clock_out = 0,
-	.ts_mode = M88DS3103_TS_PARALLEL_16,
+	.ts_mode = M88DS3103_TS_PARALLEL,
+	.ts_clk = 16000,
+	.ts_clk_pol = 1,
 	.agc = 0x99,
 };
 
@@ -1435,6 +1436,15 @@ static int em28xx_dvb_init(struct em28xx *dev)
 		}
 		break;
 	case EM2874_BOARD_KWORLD_UB435Q_V3:
+	{
+		struct i2c_client *client;
+		struct i2c_adapter *adapter = &dev->i2c_adap[dev->def_i2c_bus];
+		struct i2c_board_info board_info = {
+			.type = "tda18212",
+			.addr = 0x60,
+			.platform_data = &kworld_ub435q_v3_config,
+		};
+
 		dvb->fe[0] = dvb_attach(lgdt3305_attach,
 					&em2874_lgdt3305_nogate_dev,
 					&dev->i2c_adap[dev->def_i2c_bus]);
@@ -1443,14 +1453,26 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			goto out_free;
 		}
 
-		/* Attach the demodulator. */
-		if (!dvb_attach(tda18212_attach, dvb->fe[0],
-				&dev->i2c_adap[dev->def_i2c_bus],
-				&kworld_ub435q_v3_config)) {
-			result = -EINVAL;
+		/* attach tuner */
+		kworld_ub435q_v3_config.fe = dvb->fe[0];
+		request_module("tda18212");
+		client = i2c_new_device(adapter, &board_info);
+		if (client == NULL || client->dev.driver == NULL) {
+			dvb_frontend_detach(dvb->fe[0]);
+			result = -ENODEV;
 			goto out_free;
 		}
+
+		if (!try_module_get(client->dev.driver->owner)) {
+			i2c_unregister_device(client);
+			dvb_frontend_detach(dvb->fe[0]);
+			result = -ENODEV;
+			goto out_free;
+		}
+
+		dvb->i2c_client_tuner = client;
 		break;
+	}
 	case EM2874_BOARD_PCTV_HD_MINI_80E:
 		dvb->fe[0] = dvb_attach(drx39xxj_attach, &dev->i2c_adap[dev->def_i2c_bus]);
 		if (dvb->fe[0] != NULL) {
@@ -1533,6 +1555,7 @@ static int em28xx_dvb_init(struct em28xx *dev)
 			/* attach demod */
 			si2168_config.i2c_adapter = &adapter;
 			si2168_config.fe = &dvb->fe[0];
+			si2168_config.ts_mode = SI2168_TS_PARALLEL;
 			memset(&info, 0, sizeof(struct i2c_board_info));
 			strlcpy(info.type, "si2168", I2C_NAME_SIZE);
 			info.addr = 0x64;
diff --git a/drivers/media/usb/em28xx/em28xx-input.c b/drivers/media/usb/em28xx/em28xx-input.c
index ed843bd221ea..581f6dad4ca9 100644
--- a/drivers/media/usb/em28xx/em28xx-input.c
+++ b/drivers/media/usb/em28xx/em28xx-input.c
@@ -71,8 +71,7 @@ struct em28xx_IR {
 	unsigned int last_readcount;
 	u64 rc_type;
 
-	/* i2c slave address of external device (if used) */
-	u16 i2c_dev_addr;
+	struct i2c_client *i2c_client;
 
 	int  (*get_key_i2c)(struct i2c_client *ir, enum rc_type *protocol, u32 *scancode);
 	int  (*get_key)(struct em28xx_IR *, struct em28xx_ir_poll_result *);
@@ -294,16 +293,11 @@ static int em2874_polling_getkey(struct em28xx_IR *ir,
 
 static int em28xx_i2c_ir_handle_key(struct em28xx_IR *ir)
 {
-	struct em28xx *dev = ir->dev;
 	static u32 scancode;
 	enum rc_type protocol;
 	int rc;
-	struct i2c_client client;
-
-	client.adapter = &ir->dev->i2c_adap[dev->def_i2c_bus];
-	client.addr = ir->i2c_dev_addr;
 
-	rc = ir->get_key_i2c(&client, &protocol, &scancode);
+	rc = ir->get_key_i2c(ir->i2c_client, &protocol, &scancode);
 	if (rc < 0) {
 		dprintk("ir->get_key_i2c() failed: %d\n", rc);
 		return rc;
@@ -361,7 +355,7 @@ static void em28xx_ir_work(struct work_struct *work)
 {
 	struct em28xx_IR *ir = container_of(work, struct em28xx_IR, work.work);
 
-	if (ir->i2c_dev_addr) /* external i2c device */
+	if (ir->i2c_client) /* external i2c device */
 		em28xx_i2c_ir_handle_key(ir);
 	else /* internal device */
 		em28xx_ir_handle_key(ir);
@@ -609,17 +603,17 @@ static int em28xx_register_snapshot_button(struct em28xx *dev)
 static void em28xx_init_buttons(struct em28xx *dev)
 {
 	u8  i = 0, j = 0;
-	bool addr_new = 0;
+	bool addr_new = false;
 
 	dev->button_polling_interval = EM28XX_BUTTONS_DEBOUNCED_QUERY_INTERVAL;
 	while (dev->board.buttons[i].role >= 0 &&
 			 dev->board.buttons[i].role < EM28XX_NUM_BUTTON_ROLES) {
 		struct em28xx_button *button = &dev->board.buttons[i];
 		/* Check if polling address is already on the list */
-		addr_new = 1;
+		addr_new = true;
 		for (j = 0; j < dev->num_button_polling_addresses; j++) {
 			if (button->reg_r == dev->button_polling_addresses[j]) {
-				addr_new = 0;
+				addr_new = false;
 				break;
 			}
 		}
@@ -756,7 +750,13 @@ static int em28xx_ir_init(struct em28xx *dev)
 			goto error;
 		}
 
-		ir->i2c_dev_addr = i2c_rc_dev_addr;
+		ir->i2c_client = kzalloc(sizeof(struct i2c_client), GFP_KERNEL);
+		if (!ir->i2c_client)
+			goto error;
+		ir->i2c_client->adapter = &ir->dev->i2c_adap[dev->def_i2c_bus];
+		ir->i2c_client->addr = i2c_rc_dev_addr;
+		ir->i2c_client->flags = 0;
+		/* NOTE: all other fields of i2c_client are unused */
 	} else {	/* internal device */
 		switch (dev->chip_id) {
 		case CHIP_ID_EM2860:
@@ -815,6 +815,7 @@ static int em28xx_ir_init(struct em28xx *dev)
 	return 0;
 
 error:
+	kfree(ir->i2c_client);
 	dev->ir = NULL;
 	rc_free_device(rc);
 	kfree(ir);
@@ -841,6 +842,8 @@ static int em28xx_ir_fini(struct em28xx *dev)
 	if (ir->rc)
 		rc_unregister_device(ir->rc);
 
+	kfree(ir->i2c_client);
+
 	/* done */
 	kfree(ir);
 	dev->ir = NULL;
diff --git a/drivers/media/usb/em28xx/em28xx-vbi.c b/drivers/media/usb/em28xx/em28xx-vbi.c
index 6d7f657f6f55..34ee1e03a732 100644
--- a/drivers/media/usb/em28xx/em28xx-vbi.c
+++ b/drivers/media/usb/em28xx/em28xx-vbi.c
@@ -29,17 +29,6 @@
 #include "em28xx.h"
 #include "em28xx-v4l.h"
 
-static unsigned int vbibufs = 5;
-module_param(vbibufs, int, 0644);
-MODULE_PARM_DESC(vbibufs, "number of vbi buffers, range 2-32");
-
-static unsigned int vbi_debug;
-module_param(vbi_debug, int, 0644);
-MODULE_PARM_DESC(vbi_debug, "enable debug messages [vbi]");
-
-#define dprintk(level, fmt, arg...)	if (vbi_debug >= level) \
-	printk(KERN_DEBUG "%s: " fmt, dev->core->name , ## arg)
-
 /* ------------------------------------------------------------------ */
 
 static int vbi_queue_setup(struct vb2_queue *vq, const struct v4l2_format *fmt,
diff --git a/drivers/media/usb/em28xx/em28xx-video.c b/drivers/media/usb/em28xx/em28xx-video.c
index 90dec2955f1c..03d5ece0319c 100644
--- a/drivers/media/usb/em28xx/em28xx-video.c
+++ b/drivers/media/usb/em28xx/em28xx-video.c
@@ -435,7 +435,10 @@ static inline void finish_buffer(struct em28xx *dev,
 	em28xx_isocdbg("[%p/%d] wakeup\n", buf, buf->top_field);
 
 	buf->vb.v4l2_buf.sequence = dev->v4l2->field_count++;
-	buf->vb.v4l2_buf.field = V4L2_FIELD_INTERLACED;
+	if (dev->v4l2->progressive)
+		buf->vb.v4l2_buf.field = V4L2_FIELD_NONE;
+	else
+		buf->vb.v4l2_buf.field = V4L2_FIELD_INTERLACED;
 	v4l2_get_timestamp(&buf->vb.v4l2_buf.timestamp);
 
 	vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
@@ -478,7 +481,7 @@ static void em28xx_copy_video(struct em28xx *dev,
 	lencopy = lencopy > remain ? remain : lencopy;
 
 	if ((char *)startwrite + lencopy > (char *)buf->vb_buf + buf->length) {
-		em28xx_isocdbg("Overflow of %zi bytes past buffer end (1)\n",
+		em28xx_isocdbg("Overflow of %zu bytes past buffer end (1)\n",
 			      ((char *)startwrite + lencopy) -
 			      ((char *)buf->vb_buf + buf->length));
 		remain = (char *)buf->vb_buf + buf->length -
@@ -504,7 +507,7 @@ static void em28xx_copy_video(struct em28xx *dev,
 
 		if ((char *)startwrite + lencopy > (char *)buf->vb_buf +
 		    buf->length) {
-			em28xx_isocdbg("Overflow of %zi bytes past buffer end"
+			em28xx_isocdbg("Overflow of %zu bytes past buffer end"
 				       "(2)\n",
 				       ((char *)startwrite + lencopy) -
 				       ((char *)buf->vb_buf + buf->length));
@@ -718,7 +721,7 @@ static inline void process_frame_data_em25xx(struct em28xx *dev,
 	struct em28xx_buffer    *buf = dev->usb_ctl.vid_buf;
 	struct em28xx_dmaqueue  *dmaq = &dev->vidq;
 	struct em28xx_v4l2      *v4l2 = dev->v4l2;
-	bool frame_end = 0;
+	bool frame_end = false;
 
 	/* Check for header */
 	/* NOTE: at least with bulk transfers, only the first packet
@@ -994,13 +997,16 @@ static void em28xx_stop_streaming(struct vb2_queue *vq)
 	}
 
 	spin_lock_irqsave(&dev->slock, flags);
+	if (dev->usb_ctl.vid_buf != NULL) {
+		vb2_buffer_done(&dev->usb_ctl.vid_buf->vb, VB2_BUF_STATE_ERROR);
+		dev->usb_ctl.vid_buf = NULL;
+	}
 	while (!list_empty(&vidq->active)) {
 		struct em28xx_buffer *buf;
 		buf = list_entry(vidq->active.next, struct em28xx_buffer, list);
 		list_del(&buf->list);
 		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 	}
-	dev->usb_ctl.vid_buf = NULL;
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
@@ -1021,13 +1027,16 @@ void em28xx_stop_vbi_streaming(struct vb2_queue *vq)
 	}
 
 	spin_lock_irqsave(&dev->slock, flags);
+	if (dev->usb_ctl.vbi_buf != NULL) {
+		vb2_buffer_done(&dev->usb_ctl.vbi_buf->vb, VB2_BUF_STATE_ERROR);
+		dev->usb_ctl.vbi_buf = NULL;
+	}
 	while (!list_empty(&vbiq->active)) {
 		struct em28xx_buffer *buf;
 		buf = list_entry(vbiq->active.next, struct em28xx_buffer, list);
 		list_del(&buf->list);
 		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
 	}
-	dev->usb_ctl.vbi_buf = NULL;
 	spin_unlock_irqrestore(&dev->slock, flags);
 }
 
@@ -1342,7 +1351,7 @@ static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
 	struct em28xx *dev = video_drvdata(file);
 	struct em28xx_v4l2 *v4l2 = dev->v4l2;
 
-	if (v4l2->streaming_users > 0)
+	if (vb2_is_busy(&v4l2->vb_vidq))
 		return -EBUSY;
 
 	vidioc_try_fmt_vid_cap(file, priv, f);
@@ -1711,7 +1720,7 @@ static int vidioc_querycap(struct file *file, void  *priv,
 	else
 		cap->device_caps = V4L2_CAP_READWRITE | V4L2_CAP_VBI_CAPTURE;
 
-	if (dev->audio_mode.has_audio)
+	if (dev->int_audio_type != EM28XX_INT_AUDIO_NONE)
 		cap->device_caps |= V4L2_CAP_AUDIO;
 
 	if (dev->tuner_type != TUNER_ABSENT)
@@ -1883,8 +1892,9 @@ static int em28xx_v4l2_open(struct file *filp)
 		return -EINVAL;
 	}
 
-	em28xx_videodbg("open dev=%s type=%s\n",
-			video_device_node_name(vdev), v4l2_type_names[fh_type]);
+	em28xx_videodbg("open dev=%s type=%s users=%d\n",
+			video_device_node_name(vdev), v4l2_type_names[fh_type],
+			v4l2->users);
 
 	if (mutex_lock_interruptible(&dev->lock))
 		return -ERESTARTSYS;
@@ -1897,9 +1907,7 @@ static int em28xx_v4l2_open(struct file *filp)
 		return ret;
 	}
 
-	if (v4l2_fh_is_singular_file(filp)) {
-		em28xx_videodbg("first opened filehandle, initializing device\n");
-
+	if (v4l2->users == 0) {
 		em28xx_set_mode(dev, EM28XX_ANALOG_MODE);
 
 		if (vdev->vfl_type != VFL_TYPE_RADIO)
@@ -1910,8 +1918,6 @@ static int em28xx_v4l2_open(struct file *filp)
 		 * of some i2c devices
 		 */
 		em28xx_wake_i2c(dev);
-	} else {
-		em28xx_videodbg("further filehandles are already opened\n");
 	}
 
 	if (vdev->vfl_type == VFL_TYPE_RADIO) {
@@ -1921,6 +1927,7 @@ static int em28xx_v4l2_open(struct file *filp)
 
 	kref_get(&dev->ref);
 	kref_get(&v4l2->ref);
+	v4l2->users++;
 
 	mutex_unlock(&dev->lock);
 
@@ -2027,11 +2034,12 @@ static int em28xx_v4l2_close(struct file *filp)
 	struct em28xx_v4l2    *v4l2 = dev->v4l2;
 	int              errCode;
 
-	mutex_lock(&dev->lock);
+	em28xx_videodbg("users=%d\n", v4l2->users);
 
-	if (v4l2_fh_is_singular_file(filp)) {
-		em28xx_videodbg("last opened filehandle, shutting down device\n");
+	vb2_fop_release(filp);
+	mutex_lock(&dev->lock);
 
+	if (v4l2->users == 1) {
 		/* No sense to try to write to the device */
 		if (dev->disconnected)
 			goto exit;
@@ -2050,12 +2058,10 @@ static int em28xx_v4l2_close(struct file *filp)
 			em28xx_errdev("cannot change alternate number to "
 					"0 (error=%i)\n", errCode);
 		}
-	} else {
-		em28xx_videodbg("further opened filehandles left\n");
 	}
 
 exit:
-	vb2_fop_release(filp);
+	v4l2->users--;
 	kref_put(&v4l2->ref, em28xx_free_v4l2);
 	mutex_unlock(&dev->lock);
 	kref_put(&dev->ref, em28xx_free_device);
@@ -2299,7 +2305,7 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 	v4l2->v4l2_dev.ctrl_handler = hdl;
 
 	if (dev->board.is_webcam)
-		v4l2->progressive = 1;
+		v4l2->progressive = true;
 
 	/*
 	 * Default format, used for tvp5150 or saa711x output formats
@@ -2505,7 +2511,7 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_G_FREQUENCY);
 		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_FREQUENCY);
 	}
-	if (!dev->audio_mode.has_audio) {
+	if (dev->int_audio_type == EM28XX_INT_AUDIO_NONE) {
 		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_G_AUDIO);
 		v4l2_disable_ioctl(v4l2->vdev, VIDIOC_S_AUDIO);
 	}
@@ -2535,7 +2541,7 @@ static int em28xx_v4l2_init(struct em28xx *dev)
 			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_G_FREQUENCY);
 			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_S_FREQUENCY);
 		}
-		if (!dev->audio_mode.has_audio) {
+		if (dev->int_audio_type == EM28XX_INT_AUDIO_NONE) {
 			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_G_AUDIO);
 			v4l2_disable_ioctl(v4l2->vbi_dev, VIDIOC_S_AUDIO);
 		}
diff --git a/drivers/media/usb/em28xx/em28xx.h b/drivers/media/usb/em28xx/em28xx.h
index 84ef8efdb148..a21a7463b557 100644
--- a/drivers/media/usb/em28xx/em28xx.h
+++ b/drivers/media/usb/em28xx/em28xx.h
@@ -309,13 +309,18 @@ enum em28xx_ac97_mode {
 
 struct em28xx_audio_mode {
 	enum em28xx_ac97_mode ac97;
+};
 
-	u16 ac97_feat;
-	u32 ac97_vendor_id;
-
-	unsigned int has_audio:1;
+enum em28xx_int_audio_type {
+	EM28XX_INT_AUDIO_NONE = 0,
+	EM28XX_INT_AUDIO_AC97,
+	EM28XX_INT_AUDIO_I2S,
+};
 
-	u8 i2s_samplerates;
+enum em28xx_usb_audio_type {
+	EM28XX_USB_AUDIO_NONE = 0,
+	EM28XX_USB_AUDIO_CLASS,
+	EM28XX_USB_AUDIO_VENDOR,
 };
 
 /* em28xx has two audio inputs: tuner and line in.
@@ -524,6 +529,7 @@ struct em28xx_v4l2 {
 	int sensor_yres;
 	int sensor_xtal;
 
+	int users;		/* user count for exclusive use */
 	int streaming_users;    /* number of actively streaming users */
 
 	u32 frequency;		/* selected tuner frequency */
@@ -607,9 +613,9 @@ struct em28xx {
 	unsigned int is_em25xx:1;	/* em25xx/em276x/7x/8x family bridge */
 	unsigned char disconnected:1;	/* device has been diconnected */
 	unsigned int has_video:1;
-	unsigned int has_audio_class:1;
-	unsigned int has_alsa_audio:1;
 	unsigned int is_audio_only:1;
+	enum em28xx_int_audio_type int_audio_type;
+	enum em28xx_usb_audio_type usb_audio_type;
 
 	struct em28xx_board board;
 
diff --git a/drivers/media/usb/go7007/go7007-usb.c b/drivers/media/usb/go7007/go7007-usb.c
index ece27ece8115..3f986e1178ce 100644
--- a/drivers/media/usb/go7007/go7007-usb.c
+++ b/drivers/media/usb/go7007/go7007-usb.c
@@ -696,7 +696,7 @@ static int go7007_usb_ezusb_write_interrupt(struct go7007 *go,
 				sizeof(status_reg), timeout);
 		if (r < 0)
 			break;
-		status_reg = le16_to_cpu(*((u16 *)go->usb_buf));
+		status_reg = le16_to_cpu(*((__le16 *)go->usb_buf));
 		if (!(status_reg & 0x0010))
 			break;
 		msleep(10);
@@ -751,7 +751,7 @@ static int go7007_usb_onboard_write_interrupt(struct go7007 *go,
 static void go7007_usb_readinterrupt_complete(struct urb *urb)
 {
 	struct go7007 *go = (struct go7007 *)urb->context;
-	u16 *regs = (u16 *)urb->transfer_buffer;
+	__le16 *regs = (__le16 *)urb->transfer_buffer;
 	int status = urb->status;
 
 	if (status) {
diff --git a/drivers/media/usb/gspca/gspca.c b/drivers/media/usb/gspca/gspca.c
index e8cf23c91cef..43d65057a5fe 100644
--- a/drivers/media/usb/gspca/gspca.c
+++ b/drivers/media/usb/gspca/gspca.c
@@ -876,9 +876,8 @@ static int gspca_init_transfer(struct gspca_dev *gspca_dev)
 		ep_tb[0].alt = gspca_dev->alt;
 		alt_idx = 1;
 	} else {
-
-	/* else, compute the minimum bandwidth
-	 * and build the endpoint table */
+		/* else, compute the minimum bandwidth
+		 * and build the endpoint table */
 		alt_idx = build_isoc_ep_tb(gspca_dev, intf, ep_tb);
 		if (alt_idx <= 0) {
 			pr_err("no transfer endpoint found\n");
diff --git a/drivers/media/usb/gspca/gspca.h b/drivers/media/usb/gspca/gspca.h
index f06253cd7469..d39adf90303b 100644
--- a/drivers/media/usb/gspca/gspca.h
+++ b/drivers/media/usb/gspca/gspca.h
@@ -235,6 +235,6 @@ int gspca_resume(struct usb_interface *intf);
 int gspca_expo_autogain(struct gspca_dev *gspca_dev, int avg_lum,
 	int desired_avg_lum, int deadzone, int gain_knee, int exposure_knee);
 int gspca_coarse_grained_expo_autogain(struct gspca_dev *gspca_dev,
-        int avg_lum, int desired_avg_lum, int deadzone);
+	int avg_lum, int desired_avg_lum, int deadzone);
 
 #endif /* GSPCAV2_H */
diff --git a/drivers/media/usb/gspca/kinect.c b/drivers/media/usb/gspca/kinect.c
index 45bc1f51c5d8..3cb30a37d6ac 100644
--- a/drivers/media/usb/gspca/kinect.c
+++ b/drivers/media/usb/gspca/kinect.c
@@ -51,9 +51,9 @@ struct pkt_hdr {
 
 struct cam_hdr {
 	uint8_t magic[2];
-	uint16_t len;
-	uint16_t cmd;
-	uint16_t tag;
+	__le16 len;
+	__le16 cmd;
+	__le16 tag;
 };
 
 /* specific webcam descriptor */
@@ -188,9 +188,9 @@ static int send_cmd(struct gspca_dev *gspca_dev, uint16_t cmd, void *cmdbuf,
 		       rhdr->tag, chdr->tag);
 		return -1;
 	}
-	if (cpu_to_le16(rhdr->len) != (actual_len/2)) {
+	if (le16_to_cpu(rhdr->len) != (actual_len/2)) {
 		pr_err("send_cmd: Bad len %04x != %04x\n",
-		       cpu_to_le16(rhdr->len), (int)(actual_len/2));
+		       le16_to_cpu(rhdr->len), (int)(actual_len/2));
 		return -1;
 	}
 
@@ -211,7 +211,7 @@ static int write_register(struct gspca_dev *gspca_dev, uint16_t reg,
 			uint16_t data)
 {
 	uint16_t reply[2];
-	uint16_t cmd[2];
+	__le16 cmd[2];
 	int res;
 
 	cmd[0] = cpu_to_le16(reg);
diff --git a/drivers/media/usb/gspca/sn9c20x.c b/drivers/media/usb/gspca/sn9c20x.c
index 41a9a892f79c..d0ee899584a9 100644
--- a/drivers/media/usb/gspca/sn9c20x.c
+++ b/drivers/media/usb/gspca/sn9c20x.c
@@ -1297,7 +1297,7 @@ static void set_cmatrix(struct gspca_dev *gspca_dev,
 	s32 hue_coord, hue_index = 180 + hue;
 	u8 cmatrix[21];
 
-	memset(cmatrix, 0, sizeof cmatrix);
+	memset(cmatrix, 0, sizeof(cmatrix));
 	cmatrix[2] = (contrast * 0x25 / 0x100) + 0x26;
 	cmatrix[0] = 0x13 + (cmatrix[2] - 0x26) * 0x13 / 0x25;
 	cmatrix[4] = 0x07 + (cmatrix[2] - 0x26) * 0x07 / 0x25;
@@ -1787,8 +1787,9 @@ static int sd_init(struct gspca_dev *gspca_dev)
 	struct sd *sd = (struct sd *) gspca_dev;
 	int i;
 	u8 value;
-	u8 i2c_init[9] =
-		{0x80, sd->i2c_addr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03};
+	u8 i2c_init[9] = {
+		0x80, sd->i2c_addr, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03
+	};
 
 	for (i = 0; i < ARRAY_SIZE(bridge_init); i++) {
 		value = bridge_init[i][1];
@@ -2242,8 +2243,9 @@ static void sd_pkt_scan(struct gspca_dev *gspca_dev,
 {
 	struct sd *sd = (struct sd *) gspca_dev;
 	int avg_lum, is_jpeg;
-	static const u8 frame_header[] =
-		{0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96};
+	static const u8 frame_header[] = {
+		0xff, 0xff, 0x00, 0xc4, 0xc4, 0x96
+	};
 
 	is_jpeg = (sd->fmt & 0x03) == 0;
 	if (len >= 64 && memcmp(data, frame_header, 6) == 0) {
diff --git a/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c b/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
index e60cbb3aa609..f86cec091bf4 100644
--- a/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
+++ b/drivers/media/usb/gspca/stv06xx/stv06xx_vv6410.c
@@ -259,7 +259,7 @@ static int vv6410_set_exposure(struct gspca_dev *gspca_dev, __s32 val)
 	fine = val % VV6410_CIF_LINELENGTH;
 	coarse = min(512, val / VV6410_CIF_LINELENGTH);
 
-	PDEBUG(D_CONF, "Set coarse exposure to %d, fine expsure to %d",
+	PDEBUG(D_CONF, "Set coarse exposure to %d, fine exposure to %d",
 	       coarse, fine);
 
 	err = stv06xx_write_sensor(sd, VV6410_FINEH, fine >> 8);
diff --git a/drivers/media/usb/hackrf/Kconfig b/drivers/media/usb/hackrf/Kconfig
new file mode 100644
index 000000000000..937e6f5c1e8e
--- /dev/null
+++ b/drivers/media/usb/hackrf/Kconfig
@@ -0,0 +1,10 @@
+config USB_HACKRF
+	tristate "HackRF"
+	depends on VIDEO_V4L2
+	select VIDEOBUF2_VMALLOC
+	---help---
+	  This is a video4linux2 driver for HackRF SDR device.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called hackrf
+
diff --git a/drivers/media/usb/hackrf/Makefile b/drivers/media/usb/hackrf/Makefile
new file mode 100644
index 000000000000..73064a24cd4e
--- /dev/null
+++ b/drivers/media/usb/hackrf/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_USB_HACKRF)              += hackrf.o
diff --git a/drivers/media/usb/hackrf/hackrf.c b/drivers/media/usb/hackrf/hackrf.c
new file mode 100644
index 000000000000..328b5ba47a0a
--- /dev/null
+++ b/drivers/media/usb/hackrf/hackrf.c
@@ -0,0 +1,1142 @@
+/*
+ * HackRF driver
+ *
+ * Copyright (C) 2014 Antti Palosaari <crope@iki.fi>
+ *
+ *    This program is free software; you can redistribute it and/or modify
+ *    it under the terms of the GNU General Public License as published by
+ *    the Free Software Foundation; either version 2 of the License, or
+ *    (at your option) any later version.
+ *
+ *    This program is distributed in the hope that it will be useful,
+ *    but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *    GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+#include <media/v4l2-device.h>
+#include <media/v4l2-ioctl.h>
+#include <media/v4l2-ctrls.h>
+#include <media/v4l2-event.h>
+#include <media/videobuf2-vmalloc.h>
+
+/* HackRF USB API commands (from HackRF Library) */
+enum {
+	CMD_SET_TRANSCEIVER_MODE           = 0x01,
+	CMD_SAMPLE_RATE_SET                = 0x06,
+	CMD_BASEBAND_FILTER_BANDWIDTH_SET  = 0x07,
+	CMD_BOARD_ID_READ                  = 0x0e,
+	CMD_VERSION_STRING_READ            = 0x0f,
+	CMD_SET_FREQ                       = 0x10,
+	CMD_SET_LNA_GAIN                   = 0x13,
+	CMD_SET_VGA_GAIN                   = 0x14,
+};
+
+/*
+ *       bEndpointAddress     0x81  EP 1 IN
+ *         Transfer Type            Bulk
+ *       wMaxPacketSize     0x0200  1x 512 bytes
+ */
+#define MAX_BULK_BUFS            (6)
+#define BULK_BUFFER_SIZE         (128 * 512)
+
+static const struct v4l2_frequency_band bands_adc[] = {
+	{
+		.tuner = 0,
+		.type = V4L2_TUNER_ADC,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =   200000,
+		.rangehigh  = 24000000,
+	},
+};
+
+static const struct v4l2_frequency_band bands_rf[] = {
+	{
+		.tuner = 1,
+		.type = V4L2_TUNER_RF,
+		.index = 0,
+		.capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS,
+		.rangelow   =          1,
+		.rangehigh  = 4294967294LL, /* max u32, hw goes over 7GHz */
+	},
+};
+
+/* stream formats */
+struct hackrf_format {
+	char	*name;
+	u32	pixelformat;
+	u32	buffersize;
+};
+
+/* format descriptions for capture and preview */
+static struct hackrf_format formats[] = {
+	{
+		.name		= "Complex S8",
+		.pixelformat	= V4L2_SDR_FMT_CS8,
+		.buffersize	= BULK_BUFFER_SIZE,
+	},
+};
+
+static const unsigned int NUM_FORMATS = ARRAY_SIZE(formats);
+
+/* intermediate buffers with raw data from the USB device */
+struct hackrf_frame_buf {
+	struct vb2_buffer vb;   /* common v4l buffer stuff -- must be first */
+	struct list_head list;
+};
+
+struct hackrf_dev {
+#define POWER_ON           (1 << 1)
+#define URB_BUF            (1 << 2)
+#define USB_STATE_URB_BUF  (1 << 3)
+	unsigned long flags;
+
+	struct device *dev;
+	struct usb_device *udev;
+	struct video_device vdev;
+	struct v4l2_device v4l2_dev;
+
+	/* videobuf2 queue and queued buffers list */
+	struct vb2_queue vb_queue;
+	struct list_head queued_bufs;
+	spinlock_t queued_bufs_lock; /* Protects queued_bufs */
+	unsigned sequence;	     /* Buffer sequence counter */
+	unsigned int vb_full;        /* vb is full and packets dropped */
+
+	/* Note if taking both locks v4l2_lock must always be locked first! */
+	struct mutex v4l2_lock;      /* Protects everything else */
+	struct mutex vb_queue_lock;  /* Protects vb_queue */
+
+	struct urb     *urb_list[MAX_BULK_BUFS];
+	int            buf_num;
+	unsigned long  buf_size;
+	u8             *buf_list[MAX_BULK_BUFS];
+	dma_addr_t     dma_addr[MAX_BULK_BUFS];
+	int            urbs_initialized;
+	int            urbs_submitted;
+
+	/* USB control message buffer */
+	#define BUF_SIZE 24
+	u8 buf[BUF_SIZE];
+
+	/* Current configuration */
+	unsigned int f_adc;
+	unsigned int f_rf;
+	u32 pixelformat;
+	u32 buffersize;
+
+	/* Controls */
+	struct v4l2_ctrl_handler hdl;
+	struct v4l2_ctrl *bandwidth_auto;
+	struct v4l2_ctrl *bandwidth;
+	struct v4l2_ctrl *lna_gain;
+	struct v4l2_ctrl *if_gain;
+
+	/* Sample rate calc */
+	unsigned long jiffies_next;
+	unsigned int sample;
+	unsigned int sample_measured;
+};
+
+#define hackrf_dbg_usb_control_msg(_dev, _r, _t, _v, _i, _b, _l) { \
+	char *_direction; \
+	if (_t & USB_DIR_IN) \
+		_direction = "<<<"; \
+	else \
+		_direction = ">>>"; \
+	dev_dbg(_dev, "%02x %02x %02x %02x %02x %02x %02x %02x %s %*ph\n", \
+			_t, _r, _v & 0xff, _v >> 8, _i & 0xff, \
+			_i >> 8, _l & 0xff, _l >> 8, _direction, _l, _b); \
+}
+
+/* execute firmware command */
+static int hackrf_ctrl_msg(struct hackrf_dev *dev, u8 request, u16 value,
+		u16 index, u8 *data, u16 size)
+{
+	int ret;
+	unsigned int pipe;
+	u8 requesttype;
+
+	switch (request) {
+	case CMD_SET_TRANSCEIVER_MODE:
+	case CMD_SET_FREQ:
+	case CMD_SAMPLE_RATE_SET:
+	case CMD_BASEBAND_FILTER_BANDWIDTH_SET:
+		pipe = usb_sndctrlpipe(dev->udev, 0);
+		requesttype = (USB_TYPE_VENDOR | USB_DIR_OUT);
+		break;
+	case CMD_BOARD_ID_READ:
+	case CMD_VERSION_STRING_READ:
+	case CMD_SET_LNA_GAIN:
+	case CMD_SET_VGA_GAIN:
+		pipe = usb_rcvctrlpipe(dev->udev, 0);
+		requesttype = (USB_TYPE_VENDOR | USB_DIR_IN);
+		break;
+	default:
+		dev_err(dev->dev, "Unknown command %02x\n", request);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* write request */
+	if (!(requesttype & USB_DIR_IN))
+		memcpy(dev->buf, data, size);
+
+	ret = usb_control_msg(dev->udev, pipe, request, requesttype, value,
+			index, dev->buf, size, 1000);
+	hackrf_dbg_usb_control_msg(dev->dev, request, requesttype, value,
+			index, dev->buf, size);
+	if (ret < 0) {
+		dev_err(dev->dev, "usb_control_msg() failed %d request %02x\n",
+				ret, request);
+		goto err;
+	}
+
+	/* read request */
+	if (requesttype & USB_DIR_IN)
+		memcpy(data, dev->buf, size);
+
+	return 0;
+err:
+	return ret;
+}
+
+/* Private functions */
+static struct hackrf_frame_buf *hackrf_get_next_fill_buf(struct hackrf_dev *dev)
+{
+	unsigned long flags;
+	struct hackrf_frame_buf *buf = NULL;
+
+	spin_lock_irqsave(&dev->queued_bufs_lock, flags);
+	if (list_empty(&dev->queued_bufs))
+		goto leave;
+
+	buf = list_entry(dev->queued_bufs.next, struct hackrf_frame_buf, list);
+	list_del(&buf->list);
+leave:
+	spin_unlock_irqrestore(&dev->queued_bufs_lock, flags);
+	return buf;
+}
+
+static unsigned int hackrf_convert_stream(struct hackrf_dev *dev,
+		void *dst, void *src, unsigned int src_len)
+{
+	memcpy(dst, src, src_len);
+
+	/* calculate sample rate and output it in 10 seconds intervals */
+	if (unlikely(time_is_before_jiffies(dev->jiffies_next))) {
+		#define MSECS 10000UL
+		unsigned int msecs = jiffies_to_msecs(jiffies -
+				dev->jiffies_next + msecs_to_jiffies(MSECS));
+		unsigned int samples = dev->sample - dev->sample_measured;
+
+		dev->jiffies_next = jiffies + msecs_to_jiffies(MSECS);
+		dev->sample_measured = dev->sample;
+		dev_dbg(dev->dev, "slen=%u samples=%u msecs=%u sample rate=%lu\n",
+				src_len, samples, msecs,
+				samples * 1000UL / msecs);
+	}
+
+	/* total number of samples */
+	dev->sample += src_len / 2;
+
+	return src_len;
+}
+
+/*
+ * This gets called for the bulk stream pipe. This is done in interrupt
+ * time, so it has to be fast, not crash, and not stall. Neat.
+ */
+static void hackrf_urb_complete(struct urb *urb)
+{
+	struct hackrf_dev *dev = urb->context;
+	struct hackrf_frame_buf *fbuf;
+
+	dev_dbg_ratelimited(dev->dev, "status=%d length=%d/%d errors=%d\n",
+			urb->status, urb->actual_length,
+			urb->transfer_buffer_length, urb->error_count);
+
+	switch (urb->status) {
+	case 0:             /* success */
+	case -ETIMEDOUT:    /* NAK */
+		break;
+	case -ECONNRESET:   /* kill */
+	case -ENOENT:
+	case -ESHUTDOWN:
+		return;
+	default:            /* error */
+		dev_err_ratelimited(dev->dev, "URB failed %d\n", urb->status);
+		break;
+	}
+
+	if (likely(urb->actual_length > 0)) {
+		void *ptr;
+		unsigned int len;
+		/* get free framebuffer */
+		fbuf = hackrf_get_next_fill_buf(dev);
+		if (unlikely(fbuf == NULL)) {
+			dev->vb_full++;
+			dev_notice_ratelimited(dev->dev,
+					"videobuf is full, %d packets dropped\n",
+					dev->vb_full);
+			goto skip;
+		}
+
+		/* fill framebuffer */
+		ptr = vb2_plane_vaddr(&fbuf->vb, 0);
+		len = hackrf_convert_stream(dev, ptr, urb->transfer_buffer,
+				urb->actual_length);
+		vb2_set_plane_payload(&fbuf->vb, 0, len);
+		v4l2_get_timestamp(&fbuf->vb.v4l2_buf.timestamp);
+		fbuf->vb.v4l2_buf.sequence = dev->sequence++;
+		vb2_buffer_done(&fbuf->vb, VB2_BUF_STATE_DONE);
+	}
+skip:
+	usb_submit_urb(urb, GFP_ATOMIC);
+}
+
+static int hackrf_kill_urbs(struct hackrf_dev *dev)
+{
+	int i;
+
+	for (i = dev->urbs_submitted - 1; i >= 0; i--) {
+		dev_dbg(dev->dev, "kill urb=%d\n", i);
+		/* stop the URB */
+		usb_kill_urb(dev->urb_list[i]);
+	}
+	dev->urbs_submitted = 0;
+
+	return 0;
+}
+
+static int hackrf_submit_urbs(struct hackrf_dev *dev)
+{
+	int i, ret;
+
+	for (i = 0; i < dev->urbs_initialized; i++) {
+		dev_dbg(dev->dev, "submit urb=%d\n", i);
+		ret = usb_submit_urb(dev->urb_list[i], GFP_ATOMIC);
+		if (ret) {
+			dev_err(dev->dev, "Could not submit URB no. %d - get them all back\n",
+					i);
+			hackrf_kill_urbs(dev);
+			return ret;
+		}
+		dev->urbs_submitted++;
+	}
+
+	return 0;
+}
+
+static int hackrf_free_stream_bufs(struct hackrf_dev *dev)
+{
+	if (dev->flags & USB_STATE_URB_BUF) {
+		while (dev->buf_num) {
+			dev->buf_num--;
+			dev_dbg(dev->dev, "free buf=%d\n", dev->buf_num);
+			usb_free_coherent(dev->udev, dev->buf_size,
+					  dev->buf_list[dev->buf_num],
+					  dev->dma_addr[dev->buf_num]);
+		}
+	}
+	dev->flags &= ~USB_STATE_URB_BUF;
+
+	return 0;
+}
+
+static int hackrf_alloc_stream_bufs(struct hackrf_dev *dev)
+{
+	dev->buf_num = 0;
+	dev->buf_size = BULK_BUFFER_SIZE;
+
+	dev_dbg(dev->dev, "all in all I will use %u bytes for streaming\n",
+			MAX_BULK_BUFS * BULK_BUFFER_SIZE);
+
+	for (dev->buf_num = 0; dev->buf_num < MAX_BULK_BUFS; dev->buf_num++) {
+		dev->buf_list[dev->buf_num] = usb_alloc_coherent(dev->udev,
+				BULK_BUFFER_SIZE, GFP_ATOMIC,
+				&dev->dma_addr[dev->buf_num]);
+		if (!dev->buf_list[dev->buf_num]) {
+			dev_dbg(dev->dev, "alloc buf=%d failed\n",
+					dev->buf_num);
+			hackrf_free_stream_bufs(dev);
+			return -ENOMEM;
+		}
+
+		dev_dbg(dev->dev, "alloc buf=%d %p (dma %llu)\n", dev->buf_num,
+				dev->buf_list[dev->buf_num],
+				(long long)dev->dma_addr[dev->buf_num]);
+		dev->flags |= USB_STATE_URB_BUF;
+	}
+
+	return 0;
+}
+
+static int hackrf_free_urbs(struct hackrf_dev *dev)
+{
+	int i;
+
+	hackrf_kill_urbs(dev);
+
+	for (i = dev->urbs_initialized - 1; i >= 0; i--) {
+		if (dev->urb_list[i]) {
+			dev_dbg(dev->dev, "free urb=%d\n", i);
+			/* free the URBs */
+			usb_free_urb(dev->urb_list[i]);
+		}
+	}
+	dev->urbs_initialized = 0;
+
+	return 0;
+}
+
+static int hackrf_alloc_urbs(struct hackrf_dev *dev)
+{
+	int i, j;
+
+	/* allocate the URBs */
+	for (i = 0; i < MAX_BULK_BUFS; i++) {
+		dev_dbg(dev->dev, "alloc urb=%d\n", i);
+		dev->urb_list[i] = usb_alloc_urb(0, GFP_ATOMIC);
+		if (!dev->urb_list[i]) {
+			dev_dbg(dev->dev, "failed\n");
+			for (j = 0; j < i; j++)
+				usb_free_urb(dev->urb_list[j]);
+			return -ENOMEM;
+		}
+		usb_fill_bulk_urb(dev->urb_list[i],
+				dev->udev,
+				usb_rcvbulkpipe(dev->udev, 0x81),
+				dev->buf_list[i],
+				BULK_BUFFER_SIZE,
+				hackrf_urb_complete, dev);
+
+		dev->urb_list[i]->transfer_flags = URB_NO_TRANSFER_DMA_MAP;
+		dev->urb_list[i]->transfer_dma = dev->dma_addr[i];
+		dev->urbs_initialized++;
+	}
+
+	return 0;
+}
+
+/* Must be called with vb_queue_lock hold */
+static void hackrf_cleanup_queued_bufs(struct hackrf_dev *dev)
+{
+	unsigned long flags;
+
+	dev_dbg(dev->dev, "\n");
+
+	spin_lock_irqsave(&dev->queued_bufs_lock, flags);
+	while (!list_empty(&dev->queued_bufs)) {
+		struct hackrf_frame_buf *buf;
+
+		buf = list_entry(dev->queued_bufs.next,
+				struct hackrf_frame_buf, list);
+		list_del(&buf->list);
+		vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+	}
+	spin_unlock_irqrestore(&dev->queued_bufs_lock, flags);
+}
+
+/* The user yanked out the cable... */
+static void hackrf_disconnect(struct usb_interface *intf)
+{
+	struct v4l2_device *v = usb_get_intfdata(intf);
+	struct hackrf_dev *dev = container_of(v, struct hackrf_dev, v4l2_dev);
+
+	dev_dbg(dev->dev, "\n");
+
+	mutex_lock(&dev->vb_queue_lock);
+	mutex_lock(&dev->v4l2_lock);
+	/* No need to keep the urbs around after disconnection */
+	dev->udev = NULL;
+	v4l2_device_disconnect(&dev->v4l2_dev);
+	video_unregister_device(&dev->vdev);
+	mutex_unlock(&dev->v4l2_lock);
+	mutex_unlock(&dev->vb_queue_lock);
+
+	v4l2_device_put(&dev->v4l2_dev);
+}
+
+/* Videobuf2 operations */
+static int hackrf_queue_setup(struct vb2_queue *vq,
+		const struct v4l2_format *fmt, unsigned int *nbuffers,
+		unsigned int *nplanes, unsigned int sizes[], void *alloc_ctxs[])
+{
+	struct hackrf_dev *dev = vb2_get_drv_priv(vq);
+
+	dev_dbg(dev->dev, "nbuffers=%d\n", *nbuffers);
+
+	/* Need at least 8 buffers */
+	if (vq->num_buffers + *nbuffers < 8)
+		*nbuffers = 8 - vq->num_buffers;
+	*nplanes = 1;
+	sizes[0] = PAGE_ALIGN(dev->buffersize);
+
+	dev_dbg(dev->dev, "nbuffers=%d sizes[0]=%d\n", *nbuffers, sizes[0]);
+	return 0;
+}
+
+static void hackrf_buf_queue(struct vb2_buffer *vb)
+{
+	struct hackrf_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
+	struct hackrf_frame_buf *buf =
+			container_of(vb, struct hackrf_frame_buf, vb);
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->queued_bufs_lock, flags);
+	list_add_tail(&buf->list, &dev->queued_bufs);
+	spin_unlock_irqrestore(&dev->queued_bufs_lock, flags);
+}
+
+static int hackrf_start_streaming(struct vb2_queue *vq, unsigned int count)
+{
+	struct hackrf_dev *dev = vb2_get_drv_priv(vq);
+	int ret;
+
+	dev_dbg(dev->dev, "\n");
+
+	if (!dev->udev)
+		return -ENODEV;
+
+	mutex_lock(&dev->v4l2_lock);
+
+	dev->sequence = 0;
+
+	set_bit(POWER_ON, &dev->flags);
+
+	ret = hackrf_alloc_stream_bufs(dev);
+	if (ret)
+		goto err;
+
+	ret = hackrf_alloc_urbs(dev);
+	if (ret)
+		goto err;
+
+	ret = hackrf_submit_urbs(dev);
+	if (ret)
+		goto err;
+
+	/* start hardware streaming */
+	ret = hackrf_ctrl_msg(dev, CMD_SET_TRANSCEIVER_MODE, 1, 0, NULL, 0);
+	if (ret)
+		goto err;
+
+	goto exit_mutex_unlock;
+err:
+	hackrf_kill_urbs(dev);
+	hackrf_free_urbs(dev);
+	hackrf_free_stream_bufs(dev);
+	clear_bit(POWER_ON, &dev->flags);
+
+	/* return all queued buffers to vb2 */
+	{
+		struct hackrf_frame_buf *buf, *tmp;
+
+		list_for_each_entry_safe(buf, tmp, &dev->queued_bufs, list) {
+			list_del(&buf->list);
+			vb2_buffer_done(&buf->vb, VB2_BUF_STATE_QUEUED);
+		}
+	}
+
+exit_mutex_unlock:
+	mutex_unlock(&dev->v4l2_lock);
+
+	return ret;
+}
+
+static void hackrf_stop_streaming(struct vb2_queue *vq)
+{
+	struct hackrf_dev *dev = vb2_get_drv_priv(vq);
+
+	dev_dbg(dev->dev, "\n");
+
+	mutex_lock(&dev->v4l2_lock);
+
+	/* stop hardware streaming */
+	hackrf_ctrl_msg(dev, CMD_SET_TRANSCEIVER_MODE, 0, 0, NULL, 0);
+
+	hackrf_kill_urbs(dev);
+	hackrf_free_urbs(dev);
+	hackrf_free_stream_bufs(dev);
+
+	hackrf_cleanup_queued_bufs(dev);
+
+	clear_bit(POWER_ON, &dev->flags);
+
+	mutex_unlock(&dev->v4l2_lock);
+}
+
+static struct vb2_ops hackrf_vb2_ops = {
+	.queue_setup            = hackrf_queue_setup,
+	.buf_queue              = hackrf_buf_queue,
+	.start_streaming        = hackrf_start_streaming,
+	.stop_streaming         = hackrf_stop_streaming,
+	.wait_prepare           = vb2_ops_wait_prepare,
+	.wait_finish            = vb2_ops_wait_finish,
+};
+
+static int hackrf_querycap(struct file *file, void *fh,
+		struct v4l2_capability *cap)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+
+	dev_dbg(dev->dev, "\n");
+
+	strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
+	strlcpy(cap->card, dev->vdev.name, sizeof(cap->card));
+	usb_make_path(dev->udev, cap->bus_info, sizeof(cap->bus_info));
+	cap->device_caps = V4L2_CAP_SDR_CAPTURE | V4L2_CAP_STREAMING |
+			V4L2_CAP_READWRITE | V4L2_CAP_TUNER;
+	cap->capabilities = cap->device_caps | V4L2_CAP_DEVICE_CAPS;
+
+	return 0;
+}
+
+static int hackrf_s_fmt_sdr_cap(struct file *file, void *priv,
+		struct v4l2_format *f)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+	struct vb2_queue *q = &dev->vb_queue;
+	int i;
+
+	dev_dbg(dev->dev, "pixelformat fourcc %4.4s\n",
+			(char *)&f->fmt.sdr.pixelformat);
+
+	if (vb2_is_busy(q))
+		return -EBUSY;
+
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (f->fmt.sdr.pixelformat == formats[i].pixelformat) {
+			dev->pixelformat = formats[i].pixelformat;
+			dev->buffersize = formats[i].buffersize;
+			f->fmt.sdr.buffersize = formats[i].buffersize;
+			return 0;
+		}
+	}
+
+	dev->pixelformat = formats[0].pixelformat;
+	dev->buffersize = formats[0].buffersize;
+	f->fmt.sdr.pixelformat = formats[0].pixelformat;
+	f->fmt.sdr.buffersize = formats[0].buffersize;
+
+	return 0;
+}
+
+static int hackrf_g_fmt_sdr_cap(struct file *file, void *priv,
+		struct v4l2_format *f)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+
+	dev_dbg(dev->dev, "pixelformat fourcc %4.4s\n",
+			(char *)&dev->pixelformat);
+
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	f->fmt.sdr.pixelformat = dev->pixelformat;
+	f->fmt.sdr.buffersize = dev->buffersize;
+
+	return 0;
+}
+
+static int hackrf_try_fmt_sdr_cap(struct file *file, void *priv,
+		struct v4l2_format *f)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+	int i;
+
+	dev_dbg(dev->dev, "pixelformat fourcc %4.4s\n",
+			(char *)&f->fmt.sdr.pixelformat);
+
+	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
+	for (i = 0; i < NUM_FORMATS; i++) {
+		if (formats[i].pixelformat == f->fmt.sdr.pixelformat) {
+			f->fmt.sdr.buffersize = formats[i].buffersize;
+			return 0;
+		}
+	}
+
+	f->fmt.sdr.pixelformat = formats[0].pixelformat;
+	f->fmt.sdr.buffersize = formats[0].buffersize;
+
+	return 0;
+}
+
+static int hackrf_enum_fmt_sdr_cap(struct file *file, void *priv,
+		struct v4l2_fmtdesc *f)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+
+	dev_dbg(dev->dev, "index=%d\n", f->index);
+
+	if (f->index >= NUM_FORMATS)
+		return -EINVAL;
+
+	strlcpy(f->description, formats[f->index].name, sizeof(f->description));
+	f->pixelformat = formats[f->index].pixelformat;
+
+	return 0;
+}
+
+static int hackrf_s_tuner(struct file *file, void *priv,
+		const struct v4l2_tuner *v)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+	int ret;
+
+	dev_dbg(dev->dev, "index=%d\n", v->index);
+
+	if (v->index == 0)
+		ret = 0;
+	else if (v->index == 1)
+		ret = 0;
+	else
+		ret = -EINVAL;
+
+	return ret;
+}
+
+static int hackrf_g_tuner(struct file *file, void *priv, struct v4l2_tuner *v)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+	int ret;
+
+	dev_dbg(dev->dev, "index=%d\n", v->index);
+
+	if (v->index == 0) {
+		strlcpy(v->name, "HackRF ADC", sizeof(v->name));
+		v->type = V4L2_TUNER_ADC;
+		v->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+		v->rangelow  = bands_adc[0].rangelow;
+		v->rangehigh = bands_adc[0].rangehigh;
+		ret = 0;
+	} else if (v->index == 1) {
+		strlcpy(v->name, "HackRF RF", sizeof(v->name));
+		v->type = V4L2_TUNER_RF;
+		v->capability = V4L2_TUNER_CAP_1HZ | V4L2_TUNER_CAP_FREQ_BANDS;
+		v->rangelow  = bands_rf[0].rangelow;
+		v->rangehigh = bands_rf[0].rangehigh;
+		ret = 0;
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int hackrf_s_frequency(struct file *file, void *priv,
+		const struct v4l2_frequency *f)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+	int ret;
+	unsigned int upper, lower;
+	u8 buf[8];
+
+	dev_dbg(dev->dev, "tuner=%d type=%d frequency=%u\n",
+			f->tuner, f->type, f->frequency);
+
+	if (f->tuner == 0) {
+		dev->f_adc = clamp_t(unsigned int, f->frequency,
+				bands_adc[0].rangelow, bands_adc[0].rangehigh);
+		dev_dbg(dev->dev, "ADC frequency=%u Hz\n", dev->f_adc);
+		upper = dev->f_adc;
+		lower = 1;
+		buf[0] = (upper >>  0) & 0xff;
+		buf[1] = (upper >>  8) & 0xff;
+		buf[2] = (upper >> 16) & 0xff;
+		buf[3] = (upper >> 24) & 0xff;
+		buf[4] = (lower >>  0) & 0xff;
+		buf[5] = (lower >>  8) & 0xff;
+		buf[6] = (lower >> 16) & 0xff;
+		buf[7] = (lower >> 24) & 0xff;
+		ret = hackrf_ctrl_msg(dev, CMD_SAMPLE_RATE_SET, 0, 0, buf, 8);
+	} else if (f->tuner == 1) {
+		dev->f_rf = clamp_t(unsigned int, f->frequency,
+				bands_rf[0].rangelow, bands_rf[0].rangehigh);
+		dev_dbg(dev->dev, "RF frequency=%u Hz\n", dev->f_rf);
+		upper = dev->f_rf / 1000000;
+		lower = dev->f_rf % 1000000;
+		buf[0] = (upper >>  0) & 0xff;
+		buf[1] = (upper >>  8) & 0xff;
+		buf[2] = (upper >> 16) & 0xff;
+		buf[3] = (upper >> 24) & 0xff;
+		buf[4] = (lower >>  0) & 0xff;
+		buf[5] = (lower >>  8) & 0xff;
+		buf[6] = (lower >> 16) & 0xff;
+		buf[7] = (lower >> 24) & 0xff;
+		ret = hackrf_ctrl_msg(dev, CMD_SET_FREQ, 0, 0, buf, 8);
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int hackrf_g_frequency(struct file *file, void *priv,
+		struct v4l2_frequency *f)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+	int ret;
+
+	dev_dbg(dev->dev, "tuner=%d type=%d\n", f->tuner, f->type);
+
+	if (f->tuner == 0) {
+		f->type = V4L2_TUNER_ADC;
+		f->frequency = dev->f_adc;
+		ret = 0;
+	} else if (f->tuner == 1) {
+		f->type = V4L2_TUNER_RF;
+		f->frequency = dev->f_rf;
+		ret = 0;
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static int hackrf_enum_freq_bands(struct file *file, void *priv,
+		struct v4l2_frequency_band *band)
+{
+	struct hackrf_dev *dev = video_drvdata(file);
+	int ret;
+
+	dev_dbg(dev->dev, "tuner=%d type=%d index=%d\n",
+			band->tuner, band->type, band->index);
+
+	if (band->tuner == 0) {
+		if (band->index >= ARRAY_SIZE(bands_adc)) {
+			ret = -EINVAL;
+		} else {
+			*band = bands_adc[band->index];
+			ret = 0;
+		}
+	} else if (band->tuner == 1) {
+		if (band->index >= ARRAY_SIZE(bands_rf)) {
+			ret = -EINVAL;
+		} else {
+			*band = bands_rf[band->index];
+			ret = 0;
+		}
+	} else {
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ioctl_ops hackrf_ioctl_ops = {
+	.vidioc_querycap          = hackrf_querycap,
+
+	.vidioc_s_fmt_sdr_cap     = hackrf_s_fmt_sdr_cap,
+	.vidioc_g_fmt_sdr_cap     = hackrf_g_fmt_sdr_cap,
+	.vidioc_enum_fmt_sdr_cap  = hackrf_enum_fmt_sdr_cap,
+	.vidioc_try_fmt_sdr_cap   = hackrf_try_fmt_sdr_cap,
+
+	.vidioc_reqbufs           = vb2_ioctl_reqbufs,
+	.vidioc_create_bufs       = vb2_ioctl_create_bufs,
+	.vidioc_prepare_buf       = vb2_ioctl_prepare_buf,
+	.vidioc_querybuf          = vb2_ioctl_querybuf,
+	.vidioc_qbuf              = vb2_ioctl_qbuf,
+	.vidioc_dqbuf             = vb2_ioctl_dqbuf,
+
+	.vidioc_streamon          = vb2_ioctl_streamon,
+	.vidioc_streamoff         = vb2_ioctl_streamoff,
+
+	.vidioc_s_tuner           = hackrf_s_tuner,
+	.vidioc_g_tuner           = hackrf_g_tuner,
+
+	.vidioc_s_frequency       = hackrf_s_frequency,
+	.vidioc_g_frequency       = hackrf_g_frequency,
+	.vidioc_enum_freq_bands   = hackrf_enum_freq_bands,
+
+	.vidioc_subscribe_event   = v4l2_ctrl_subscribe_event,
+	.vidioc_unsubscribe_event = v4l2_event_unsubscribe,
+	.vidioc_log_status        = v4l2_ctrl_log_status,
+};
+
+static const struct v4l2_file_operations hackrf_fops = {
+	.owner                    = THIS_MODULE,
+	.open                     = v4l2_fh_open,
+	.release                  = vb2_fop_release,
+	.read                     = vb2_fop_read,
+	.poll                     = vb2_fop_poll,
+	.mmap                     = vb2_fop_mmap,
+	.unlocked_ioctl           = video_ioctl2,
+};
+
+static struct video_device hackrf_template = {
+	.name                     = "HackRF One",
+	.release                  = video_device_release_empty,
+	.fops                     = &hackrf_fops,
+	.ioctl_ops                = &hackrf_ioctl_ops,
+};
+
+static void hackrf_video_release(struct v4l2_device *v)
+{
+	struct hackrf_dev *dev = container_of(v, struct hackrf_dev, v4l2_dev);
+
+	v4l2_ctrl_handler_free(&dev->hdl);
+	v4l2_device_unregister(&dev->v4l2_dev);
+	kfree(dev);
+}
+
+static int hackrf_set_bandwidth(struct hackrf_dev *dev)
+{
+	int ret, i;
+	u16 u16tmp, u16tmp2;
+	unsigned int bandwidth;
+
+	static const struct {
+		u32 freq;
+	} bandwidth_lut[] = {
+		{ 1750000}, /*  1.75 MHz */
+		{ 2500000}, /*  2.5  MHz */
+		{ 3500000}, /*  3.5  MHz */
+		{ 5000000}, /*  5    MHz */
+		{ 5500000}, /*  5.5  MHz */
+		{ 6000000}, /*  6    MHz */
+		{ 7000000}, /*  7    MHz */
+		{ 8000000}, /*  8    MHz */
+		{ 9000000}, /*  9    MHz */
+		{10000000}, /* 10    MHz */
+		{12000000}, /* 12    MHz */
+		{14000000}, /* 14    MHz */
+		{15000000}, /* 15    MHz */
+		{20000000}, /* 20    MHz */
+		{24000000}, /* 24    MHz */
+		{28000000}, /* 28    MHz */
+	};
+
+	dev_dbg(dev->dev, "bandwidth auto=%d->%d val=%d->%d f_adc=%u\n",
+			dev->bandwidth_auto->cur.val,
+			dev->bandwidth_auto->val, dev->bandwidth->cur.val,
+			dev->bandwidth->val, dev->f_adc);
+
+	if (dev->bandwidth_auto->val == true)
+		bandwidth = dev->f_adc;
+	else
+		bandwidth = dev->bandwidth->val;
+
+	for (i = 0; i < ARRAY_SIZE(bandwidth_lut); i++) {
+		if (bandwidth <= bandwidth_lut[i].freq) {
+			bandwidth = bandwidth_lut[i].freq;
+			break;
+		}
+	}
+
+	dev->bandwidth->val = bandwidth;
+	dev->bandwidth->cur.val = bandwidth;
+
+	dev_dbg(dev->dev, "bandwidth selected=%d\n", bandwidth_lut[i].freq);
+
+	u16tmp = 0;
+	u16tmp |= ((bandwidth >> 0) & 0xff) << 0;
+	u16tmp |= ((bandwidth >> 8) & 0xff) << 8;
+	u16tmp2 = 0;
+	u16tmp2 |= ((bandwidth >> 16) & 0xff) << 0;
+	u16tmp2 |= ((bandwidth >> 24) & 0xff) << 8;
+
+	ret = hackrf_ctrl_msg(dev, CMD_BASEBAND_FILTER_BANDWIDTH_SET,
+				u16tmp, u16tmp2, NULL, 0);
+	if (ret)
+		dev_dbg(dev->dev, "failed=%d\n", ret);
+
+	return ret;
+}
+
+static int hackrf_set_lna_gain(struct hackrf_dev *dev)
+{
+	int ret;
+	u8 u8tmp;
+
+	dev_dbg(dev->dev, "lna val=%d->%d\n",
+			dev->lna_gain->cur.val, dev->lna_gain->val);
+
+	ret = hackrf_ctrl_msg(dev, CMD_SET_LNA_GAIN, 0, dev->lna_gain->val,
+			&u8tmp, 1);
+	if (ret)
+		dev_dbg(dev->dev, "failed=%d\n", ret);
+
+	return ret;
+}
+
+static int hackrf_set_if_gain(struct hackrf_dev *dev)
+{
+	int ret;
+	u8 u8tmp;
+
+	dev_dbg(dev->dev, "val=%d->%d\n",
+			dev->if_gain->cur.val, dev->if_gain->val);
+
+	ret = hackrf_ctrl_msg(dev, CMD_SET_VGA_GAIN, 0, dev->if_gain->val,
+			&u8tmp, 1);
+	if (ret)
+		dev_dbg(dev->dev, "failed=%d\n", ret);
+
+	return ret;
+}
+
+static int hackrf_s_ctrl(struct v4l2_ctrl *ctrl)
+{
+	struct hackrf_dev *dev = container_of(ctrl->handler,
+			struct hackrf_dev, hdl);
+	int ret;
+
+	switch (ctrl->id) {
+	case V4L2_CID_RF_TUNER_BANDWIDTH_AUTO:
+	case V4L2_CID_RF_TUNER_BANDWIDTH:
+		ret = hackrf_set_bandwidth(dev);
+		break;
+	case  V4L2_CID_RF_TUNER_LNA_GAIN:
+		ret = hackrf_set_lna_gain(dev);
+		break;
+	case  V4L2_CID_RF_TUNER_IF_GAIN:
+		ret = hackrf_set_if_gain(dev);
+		break;
+	default:
+		dev_dbg(dev->dev, "unknown ctrl: id=%d name=%s\n",
+				ctrl->id, ctrl->name);
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+
+static const struct v4l2_ctrl_ops hackrf_ctrl_ops = {
+	.s_ctrl = hackrf_s_ctrl,
+};
+
+static int hackrf_probe(struct usb_interface *intf,
+		const struct usb_device_id *id)
+{
+	struct hackrf_dev *dev;
+	int ret;
+	u8 u8tmp, buf[BUF_SIZE];
+
+	dev = kzalloc(sizeof(*dev), GFP_KERNEL);
+	if (dev == NULL)
+		return -ENOMEM;
+
+	mutex_init(&dev->v4l2_lock);
+	mutex_init(&dev->vb_queue_lock);
+	spin_lock_init(&dev->queued_bufs_lock);
+	INIT_LIST_HEAD(&dev->queued_bufs);
+	dev->dev = &intf->dev;
+	dev->udev = interface_to_usbdev(intf);
+	dev->f_adc = bands_adc[0].rangelow;
+	dev->f_rf = bands_rf[0].rangelow;
+	dev->pixelformat = formats[0].pixelformat;
+	dev->buffersize = formats[0].buffersize;
+
+	/* Detect device */
+	ret = hackrf_ctrl_msg(dev, CMD_BOARD_ID_READ, 0, 0, &u8tmp, 1);
+	if (ret == 0)
+		ret = hackrf_ctrl_msg(dev, CMD_VERSION_STRING_READ, 0, 0,
+				buf, BUF_SIZE);
+	if (ret) {
+		dev_err(dev->dev, "Could not detect board\n");
+		goto err_free_mem;
+	}
+
+	buf[BUF_SIZE - 1] = '\0';
+
+	dev_info(dev->dev, "Board ID: %02x\n", u8tmp);
+	dev_info(dev->dev, "Firmware version: %s\n", buf);
+
+	/* Init videobuf2 queue structure */
+	dev->vb_queue.type = V4L2_BUF_TYPE_SDR_CAPTURE;
+	dev->vb_queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
+	dev->vb_queue.drv_priv = dev;
+	dev->vb_queue.buf_struct_size = sizeof(struct hackrf_frame_buf);
+	dev->vb_queue.ops = &hackrf_vb2_ops;
+	dev->vb_queue.mem_ops = &vb2_vmalloc_memops;
+	dev->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
+	ret = vb2_queue_init(&dev->vb_queue);
+	if (ret) {
+		dev_err(dev->dev, "Could not initialize vb2 queue\n");
+		goto err_free_mem;
+	}
+
+	/* Init video_device structure */
+	dev->vdev = hackrf_template;
+	dev->vdev.queue = &dev->vb_queue;
+	dev->vdev.queue->lock = &dev->vb_queue_lock;
+	video_set_drvdata(&dev->vdev, dev);
+
+	/* Register the v4l2_device structure */
+	dev->v4l2_dev.release = hackrf_video_release;
+	ret = v4l2_device_register(&intf->dev, &dev->v4l2_dev);
+	if (ret) {
+		dev_err(dev->dev, "Failed to register v4l2-device (%d)\n", ret);
+		goto err_free_mem;
+	}
+
+	/* Register controls */
+	v4l2_ctrl_handler_init(&dev->hdl, 4);
+	dev->bandwidth_auto = v4l2_ctrl_new_std(&dev->hdl, &hackrf_ctrl_ops,
+			V4L2_CID_RF_TUNER_BANDWIDTH_AUTO, 0, 1, 1, 1);
+	dev->bandwidth = v4l2_ctrl_new_std(&dev->hdl, &hackrf_ctrl_ops,
+			V4L2_CID_RF_TUNER_BANDWIDTH,
+			1750000, 28000000, 50000, 1750000);
+	v4l2_ctrl_auto_cluster(2, &dev->bandwidth_auto, 0, false);
+	dev->lna_gain = v4l2_ctrl_new_std(&dev->hdl, &hackrf_ctrl_ops,
+			V4L2_CID_RF_TUNER_LNA_GAIN, 0, 40, 8, 0);
+	dev->if_gain = v4l2_ctrl_new_std(&dev->hdl, &hackrf_ctrl_ops,
+			V4L2_CID_RF_TUNER_IF_GAIN, 0, 62, 2, 0);
+	if (dev->hdl.error) {
+		ret = dev->hdl.error;
+		dev_err(dev->dev, "Could not initialize controls\n");
+		goto err_free_controls;
+	}
+
+	v4l2_ctrl_handler_setup(&dev->hdl);
+
+	dev->v4l2_dev.ctrl_handler = &dev->hdl;
+	dev->vdev.v4l2_dev = &dev->v4l2_dev;
+	dev->vdev.lock = &dev->v4l2_lock;
+
+	ret = video_register_device(&dev->vdev, VFL_TYPE_SDR, -1);
+	if (ret) {
+		dev_err(dev->dev, "Failed to register as video device (%d)\n",
+				ret);
+		goto err_unregister_v4l2_dev;
+	}
+	dev_info(dev->dev, "Registered as %s\n",
+			video_device_node_name(&dev->vdev));
+	dev_notice(dev->dev, "SDR API is still slightly experimental and functionality changes may follow\n");
+	return 0;
+
+err_free_controls:
+	v4l2_ctrl_handler_free(&dev->hdl);
+err_unregister_v4l2_dev:
+	v4l2_device_unregister(&dev->v4l2_dev);
+err_free_mem:
+	kfree(dev);
+	return ret;
+}
+
+/* USB device ID list */
+static struct usb_device_id hackrf_id_table[] = {
+	{ USB_DEVICE(0x1d50, 0x6089) }, /* HackRF One */
+	{ }
+};
+MODULE_DEVICE_TABLE(usb, hackrf_id_table);
+
+/* USB subsystem interface */
+static struct usb_driver hackrf_driver = {
+	.name                     = KBUILD_MODNAME,
+	.probe                    = hackrf_probe,
+	.disconnect               = hackrf_disconnect,
+	.id_table                 = hackrf_id_table,
+};
+
+module_usb_driver(hackrf_driver);
+
+MODULE_AUTHOR("Antti Palosaari <crope@iki.fi>");
+MODULE_DESCRIPTION("HackRF");
+MODULE_LICENSE("GPL");
diff --git a/drivers/media/usb/hdpvr/hdpvr-control.c b/drivers/media/usb/hdpvr/hdpvr-control.c
index 6053661dc04b..6e86032ea5db 100644
--- a/drivers/media/usb/hdpvr/hdpvr-control.c
+++ b/drivers/media/usb/hdpvr/hdpvr-control.c
@@ -59,13 +59,10 @@ int get_video_info(struct hdpvr_device *dev, struct hdpvr_video_info *vidinf)
 			      1000);
 
 #ifdef HDPVR_DEBUG
-	if (hdpvr_debug & MSG_INFO) {
-		char print_buf[15];
-		hex_dump_to_buffer(dev->usbc_buf, 5, 16, 1, print_buf,
-				   sizeof(print_buf), 0);
+	if (hdpvr_debug & MSG_INFO)
 		v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev,
-			 "get video info returned: %d, %s\n", ret, print_buf);
-	}
+			 "get video info returned: %d, %5ph\n", ret,
+			 dev->usbc_buf);
 #endif
 	mutex_unlock(&dev->usbc_mutex);
 
@@ -82,9 +79,6 @@ int get_video_info(struct hdpvr_device *dev, struct hdpvr_video_info *vidinf)
 
 int get_input_lines_info(struct hdpvr_device *dev)
 {
-#ifdef HDPVR_DEBUG
-	char print_buf[9];
-#endif
 	int ret, lines;
 
 	mutex_lock(&dev->usbc_mutex);
@@ -96,13 +90,10 @@ int get_input_lines_info(struct hdpvr_device *dev)
 			      1000);
 
 #ifdef HDPVR_DEBUG
-	if (hdpvr_debug & MSG_INFO) {
-		hex_dump_to_buffer(dev->usbc_buf, 3, 16, 1, print_buf,
-				   sizeof(print_buf), 0);
+	if (hdpvr_debug & MSG_INFO)
 		v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev,
-			 "get input lines info returned: %d, %s\n", ret,
-			 print_buf);
-	}
+			 "get input lines info returned: %d, %3ph\n", ret,
+			 dev->usbc_buf);
 #else
 	(void)ret;	/* suppress compiler warning */
 #endif
diff --git a/drivers/media/usb/hdpvr/hdpvr-core.c b/drivers/media/usb/hdpvr/hdpvr-core.c
index c5638964c3f2..42b4cdf28cfd 100644
--- a/drivers/media/usb/hdpvr/hdpvr-core.c
+++ b/drivers/media/usb/hdpvr/hdpvr-core.c
@@ -124,14 +124,6 @@ static int device_authorization(struct hdpvr_device *dev)
 	int ret, retval = -ENOMEM;
 	char request_type = 0x38, rcv_request = 0x81;
 	char *response;
-#ifdef HDPVR_DEBUG
-	size_t buf_size = 46;
-	char *print_buf = kzalloc(5*buf_size+1, GFP_KERNEL);
-	if (!print_buf) {
-		v4l2_err(&dev->v4l2_dev, "Out of memory\n");
-		return retval;
-	}
-#endif
 
 	mutex_lock(&dev->usbc_mutex);
 	ret = usb_control_msg(dev->udev,
@@ -147,11 +139,9 @@ static int device_authorization(struct hdpvr_device *dev)
 	}
 #ifdef HDPVR_DEBUG
 	else {
-		hex_dump_to_buffer(dev->usbc_buf, 46, 16, 1, print_buf,
-				   5*buf_size+1, 0);
 		v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev,
-			 "Status request returned, len %d: %s\n",
-			 ret, print_buf);
+			 "Status request returned, len %d: %46ph\n",
+			 ret, dev->usbc_buf);
 	}
 #endif
 
@@ -189,15 +179,13 @@ static int device_authorization(struct hdpvr_device *dev)
 
 	response = dev->usbc_buf+38;
 #ifdef HDPVR_DEBUG
-	hex_dump_to_buffer(response, 8, 16, 1, print_buf, 5*buf_size+1, 0);
-	v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev, "challenge: %s\n",
-		 print_buf);
+	v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev, "challenge: %8ph\n",
+		 response);
 #endif
 	challenge(response);
 #ifdef HDPVR_DEBUG
-	hex_dump_to_buffer(response, 8, 16, 1, print_buf, 5*buf_size+1, 0);
-	v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev, " response: %s\n",
-		 print_buf);
+	v4l2_dbg(MSG_INFO, hdpvr_debug, &dev->v4l2_dev, " response: %8ph\n",
+		 response);
 #endif
 
 	msleep(100);
@@ -213,9 +201,6 @@ static int device_authorization(struct hdpvr_device *dev)
 	retval = ret != 8;
 unlock:
 	mutex_unlock(&dev->usbc_mutex);
-#ifdef HDPVR_DEBUG
-	kfree(print_buf);
-#endif
 	return retval;
 }
 
diff --git a/drivers/media/usb/msi2500/msi2500.c b/drivers/media/usb/msi2500/msi2500.c
index 26b133414032..efc761c78f72 100644
--- a/drivers/media/usb/msi2500/msi2500.c
+++ b/drivers/media/usb/msi2500/msi2500.c
@@ -120,6 +120,7 @@ struct msi2500_frame_buf {
 };
 
 struct msi2500_state {
+	struct device *dev;
 	struct video_device vdev;
 	struct v4l2_device v4l2_dev;
 	struct v4l2_subdev *v4l2_subdev;
@@ -153,14 +154,13 @@ struct msi2500_state {
 	u32 next_sample; /* for track lost packets */
 	u32 sample; /* for sample rate calc */
 	unsigned long jiffies_next;
-	unsigned int sample_ctrl_bit[4];
 };
 
 /* Private functions */
 static struct msi2500_frame_buf *msi2500_get_next_fill_buf(
 		struct msi2500_state *s)
 {
-	unsigned long flags = 0;
+	unsigned long flags;
 	struct msi2500_frame_buf *buf = NULL;
 
 	spin_lock_irqsave(&s->queued_bufs_lock, flags);
@@ -269,7 +269,7 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 		sample[i] = src[3] << 24 | src[2] << 16 | src[1] << 8 |
 				src[0] << 0;
 		if (i == 0 && s->next_sample != sample[0]) {
-			dev_dbg_ratelimited(&s->udev->dev,
+			dev_dbg_ratelimited(s->dev,
 					"%d samples lost, %d %08x:%08x\n",
 					sample[0] - s->next_sample,
 					src_len, s->next_sample, sample[0]);
@@ -279,7 +279,7 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 		 * Dump all unknown 'garbage' data - maybe we will discover
 		 * someday if there is something rational...
 		 */
-		dev_dbg_ratelimited(&s->udev->dev, "%*ph\n", 12, &src[4]);
+		dev_dbg_ratelimited(s->dev, "%*ph\n", 12, &src[4]);
 
 		src += 16; /* skip header */
 
@@ -322,8 +322,7 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 		}
 		case MSI2500_PIX_FMT_SDR_MSI2500_384: /* 384 x IQ samples */
 			/* Dump unknown 'garbage' data */
-			dev_dbg_ratelimited(&s->udev->dev,
-					"%*ph\n", 24, &src[1000]);
+			dev_dbg_ratelimited(s->dev, "%*ph\n", 24, &src[1000]);
 			memcpy(dst, src, 984);
 			src += 984 + 24;
 			dst += 984;
@@ -365,8 +364,7 @@ static int msi2500_convert_stream(struct msi2500_state *s, u8 *dst, u8 *src,
 
 		s->jiffies_next = jiffies + msecs_to_jiffies(MSECS);
 		s->sample = s->next_sample;
-		dev_dbg(&s->udev->dev,
-				"size=%u samples=%u msecs=%u sample rate=%lu\n",
+		dev_dbg(s->dev, "size=%u samples=%u msecs=%u sample rate=%lu\n",
 				src_len, samples, msecs,
 				samples * 1000UL / msecs);
 	}
@@ -387,19 +385,16 @@ static void msi2500_isoc_handler(struct urb *urb)
 
 	if (unlikely(urb->status == -ENOENT || urb->status == -ECONNRESET ||
 			urb->status == -ESHUTDOWN)) {
-		dev_dbg(&s->udev->dev, "URB (%p) unlinked %ssynchronuously\n",
+		dev_dbg(s->dev, "URB (%p) unlinked %ssynchronuously\n",
 				urb, urb->status == -ENOENT ? "" : "a");
 		return;
 	}
 
 	if (unlikely(urb->status != 0)) {
-		dev_dbg(&s->udev->dev,
-				"msi2500_isoc_handler() called with status %d\n",
-				urb->status);
+		dev_dbg(s->dev, "called with status %d\n", urb->status);
 		/* Give up after a number of contiguous errors */
 		if (++s->isoc_errors > MAX_ISOC_ERRORS)
-			dev_dbg(&s->udev->dev,
-					"Too many ISOC errors, bailing out\n");
+			dev_dbg(s->dev, "Too many ISOC errors, bailing out\n");
 		goto handler_end;
 	} else {
 		/* Reset ISOC error counter. We did get here, after all. */
@@ -413,7 +408,7 @@ static void msi2500_isoc_handler(struct urb *urb)
 		/* Check frame error */
 		fstatus = urb->iso_frame_desc[i].status;
 		if (unlikely(fstatus)) {
-			dev_dbg_ratelimited(&s->udev->dev,
+			dev_dbg_ratelimited(s->dev,
 					"frame=%d/%d has error %d skipping\n",
 					i, urb->number_of_packets, fstatus);
 			continue;
@@ -430,7 +425,7 @@ static void msi2500_isoc_handler(struct urb *urb)
 		fbuf = msi2500_get_next_fill_buf(s);
 		if (unlikely(fbuf == NULL)) {
 			s->vb_full++;
-			dev_dbg_ratelimited(&s->udev->dev,
+			dev_dbg_ratelimited(s->dev,
 					"videobuf is full, %d packets dropped\n",
 					s->vb_full);
 			continue;
@@ -446,22 +441,19 @@ static void msi2500_isoc_handler(struct urb *urb)
 handler_end:
 	i = usb_submit_urb(urb, GFP_ATOMIC);
 	if (unlikely(i != 0))
-		dev_dbg(&s->udev->dev,
-				"Error (%d) re-submitting urb in msi2500_isoc_handler\n",
-				i);
+		dev_dbg(s->dev, "Error (%d) re-submitting urb\n", i);
 }
 
 static void msi2500_iso_stop(struct msi2500_state *s)
 {
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	/* Unlinking ISOC buffers one by one */
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
 		if (s->urbs[i]) {
-			dev_dbg(&s->udev->dev, "Unlinking URB %p\n",
-					s->urbs[i]);
+			dev_dbg(s->dev, "Unlinking URB %p\n", s->urbs[i]);
 			usb_kill_urb(s->urbs[i]);
 		}
 	}
@@ -471,12 +463,12 @@ static void msi2500_iso_free(struct msi2500_state *s)
 {
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	/* Freeing ISOC buffers one by one */
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
 		if (s->urbs[i]) {
-			dev_dbg(&s->udev->dev, "Freeing URB\n");
+			dev_dbg(s->dev, "Freeing URB\n");
 			if (s->urbs[i]->transfer_buffer) {
 				usb_free_coherent(s->udev,
 					s->urbs[i]->transfer_buffer_length,
@@ -492,7 +484,7 @@ static void msi2500_iso_free(struct msi2500_state *s)
 /* Both v4l2_lock and vb_queue_lock should be locked when calling this */
 static void msi2500_isoc_cleanup(struct msi2500_state *s)
 {
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	msi2500_iso_stop(s);
 	msi2500_iso_free(s);
@@ -501,14 +493,12 @@ static void msi2500_isoc_cleanup(struct msi2500_state *s)
 /* Both v4l2_lock and vb_queue_lock should be locked when calling this */
 static int msi2500_isoc_init(struct msi2500_state *s)
 {
-	struct usb_device *udev;
 	struct urb *urb;
 	int i, j, ret;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	s->isoc_errors = 0;
-	udev = s->udev;
 
 	ret = usb_set_interface(s->udev, 0, 1);
 	if (ret)
@@ -518,23 +508,22 @@ static int msi2500_isoc_init(struct msi2500_state *s)
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
 		urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);
 		if (urb == NULL) {
-			dev_err(&s->udev->dev,
-					"Failed to allocate urb %d\n", i);
+			dev_err(s->dev, "Failed to allocate urb %d\n", i);
 			msi2500_isoc_cleanup(s);
 			return -ENOMEM;
 		}
 		s->urbs[i] = urb;
-		dev_dbg(&s->udev->dev, "Allocated URB at 0x%p\n", urb);
+		dev_dbg(s->dev, "Allocated URB at 0x%p\n", urb);
 
 		urb->interval = 1;
-		urb->dev = udev;
-		urb->pipe = usb_rcvisocpipe(udev, 0x81);
+		urb->dev = s->udev;
+		urb->pipe = usb_rcvisocpipe(s->udev, 0x81);
 		urb->transfer_flags = URB_ISO_ASAP | URB_NO_TRANSFER_DMA_MAP;
-		urb->transfer_buffer = usb_alloc_coherent(udev, ISO_BUFFER_SIZE,
+		urb->transfer_buffer = usb_alloc_coherent(s->udev,
+				ISO_BUFFER_SIZE,
 				GFP_KERNEL, &urb->transfer_dma);
 		if (urb->transfer_buffer == NULL) {
-			dev_err(&s->udev->dev,
-					"Failed to allocate urb buffer %d\n",
+			dev_err(s->dev, "Failed to allocate urb buffer %d\n",
 					i);
 			msi2500_isoc_cleanup(s);
 			return -ENOMEM;
@@ -554,13 +543,12 @@ static int msi2500_isoc_init(struct msi2500_state *s)
 	for (i = 0; i < MAX_ISO_BUFS; i++) {
 		ret = usb_submit_urb(s->urbs[i], GFP_KERNEL);
 		if (ret) {
-			dev_err(&s->udev->dev,
-					"isoc_init() submit_urb %d failed with error %d\n",
+			dev_err(s->dev, "usb_submit_urb %d failed with error %d\n",
 					i, ret);
 			msi2500_isoc_cleanup(s);
 			return ret;
 		}
-		dev_dbg(&s->udev->dev, "URB 0x%p submitted.\n", s->urbs[i]);
+		dev_dbg(s->dev, "URB 0x%p submitted.\n", s->urbs[i]);
 	}
 
 	/* All is done... */
@@ -570,9 +558,9 @@ static int msi2500_isoc_init(struct msi2500_state *s)
 /* Must be called with vb_queue_lock hold */
 static void msi2500_cleanup_queued_bufs(struct msi2500_state *s)
 {
-	unsigned long flags = 0;
+	unsigned long flags;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	spin_lock_irqsave(&s->queued_bufs_lock, flags);
 	while (!list_empty(&s->queued_bufs)) {
@@ -593,7 +581,7 @@ static void msi2500_disconnect(struct usb_interface *intf)
 	struct msi2500_state *s =
 			container_of(v, struct msi2500_state, v4l2_dev);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	mutex_lock(&s->vb_queue_lock);
 	mutex_lock(&s->v4l2_lock);
@@ -613,7 +601,7 @@ static int msi2500_querycap(struct file *file, void *fh,
 {
 	struct msi2500_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	strlcpy(cap->driver, KBUILD_MODNAME, sizeof(cap->driver));
 	strlcpy(cap->card, s->vdev.name, sizeof(cap->card));
@@ -631,14 +619,13 @@ static int msi2500_queue_setup(struct vb2_queue *vq,
 {
 	struct msi2500_state *s = vb2_get_drv_priv(vq);
 
-	dev_dbg(&s->udev->dev, "%s: *nbuffers=%d\n", __func__, *nbuffers);
+	dev_dbg(s->dev, "nbuffers=%d\n", *nbuffers);
 
 	/* Absolute min and max number of buffers available for mmap() */
 	*nbuffers = clamp_t(unsigned int, *nbuffers, 8, 32);
 	*nplanes = 1;
 	sizes[0] = PAGE_ALIGN(s->buffersize);
-	dev_dbg(&s->udev->dev, "%s: nbuffers=%d sizes[0]=%d\n",
-			__func__, *nbuffers, sizes[0]);
+	dev_dbg(s->dev, "nbuffers=%d sizes[0]=%d\n", *nbuffers, sizes[0]);
 	return 0;
 }
 
@@ -647,7 +634,7 @@ static void msi2500_buf_queue(struct vb2_buffer *vb)
 	struct msi2500_state *s = vb2_get_drv_priv(vb->vb2_queue);
 	struct msi2500_frame_buf *buf =
 			container_of(vb, struct msi2500_frame_buf, vb);
-	unsigned long flags = 0;
+	unsigned long flags;
 
 	/* Check the device has not disconnected between prep and queuing */
 	if (unlikely(!s->udev)) {
@@ -665,16 +652,15 @@ static void msi2500_buf_queue(struct vb2_buffer *vb)
 #define CMD_STOP_STREAMING     0x45
 #define CMD_READ_UNKNOW        0x48
 
-#define msi2500_dbg_usb_control_msg(_udev, _r, _t, _v, _i, _b, _l) { \
+#define msi2500_dbg_usb_control_msg(_dev, _r, _t, _v, _i, _b, _l) { \
 	char *_direction; \
 	if (_t & USB_DIR_IN) \
 		_direction = "<<<"; \
 	else \
 		_direction = ">>>"; \
-	dev_dbg(&_udev->dev, "%s: %02x %02x %02x %02x %02x %02x %02x %02x " \
-			"%s %*ph\n",  __func__, _t, _r, _v & 0xff, _v >> 8, \
-			_i & 0xff, _i >> 8, _l & 0xff, _l >> 8, _direction, \
-			_l, _b); \
+	dev_dbg(_dev, "%02x %02x %02x %02x %02x %02x %02x %02x %s %*ph\n", \
+			_t, _r, _v & 0xff, _v >> 8, _i & 0xff, _i >> 8, \
+			_l & 0xff, _l >> 8, _direction, _l, _b); \
 }
 
 static int msi2500_ctrl_msg(struct msi2500_state *s, u8 cmd, u32 data)
@@ -685,18 +671,16 @@ static int msi2500_ctrl_msg(struct msi2500_state *s, u8 cmd, u32 data)
 	u16 value = (data >> 0) & 0xffff;
 	u16 index = (data >> 16) & 0xffff;
 
-	msi2500_dbg_usb_control_msg(s->udev,
+	msi2500_dbg_usb_control_msg(s->dev,
 			request, requesttype, value, index, NULL, 0);
-
 	ret = usb_control_msg(s->udev, usb_sndctrlpipe(s->udev, 0),
 			request, requesttype, value, index, NULL, 0, 2000);
-
 	if (ret)
-		dev_err(&s->udev->dev, "%s: failed %d, cmd %02x, data %04x\n",
-				__func__, ret, cmd, data);
+		dev_err(s->dev, "failed %d, cmd %02x, data %04x\n",
+				ret, cmd, data);
 
 	return ret;
-};
+}
 
 #define F_REF 24000000
 #define DIV_R_IN 2
@@ -785,8 +769,7 @@ static int msi2500_set_usb_adc(struct msi2500_state *s)
 
 	for (div_r_out = 4; div_r_out < 16; div_r_out += 2) {
 		f_vco = f_sr * div_r_out * 12;
-		dev_dbg(&s->udev->dev, "%s: div_r_out=%d f_vco=%d\n",
-				__func__, div_r_out, f_vco);
+		dev_dbg(s->dev, "div_r_out=%d f_vco=%d\n", div_r_out, f_vco);
 		if (f_vco >= 202000000)
 			break;
 	}
@@ -800,10 +783,8 @@ static int msi2500_set_usb_adc(struct msi2500_state *s)
 	reg3 |= ((fract >> 20) & 0x000001) << 15; /* [20] */
 	reg4 |= ((fract >>  0) & 0x0fffff) <<  8; /* [19:0] */
 
-	dev_dbg(&s->udev->dev,
-			"%s: f_sr=%d f_vco=%d div_n=%d div_m=%d div_r_out=%d reg3=%08x reg4=%08x\n",
-			__func__, f_sr, f_vco, div_n, div_m, div_r_out, reg3,
-			reg4);
+	dev_dbg(s->dev, "f_sr=%d f_vco=%d div_n=%d div_m=%d div_r_out=%d reg3=%08x reg4=%08x\n",
+			f_sr, f_vco, div_n, div_m, div_r_out, reg3, reg4);
 
 	ret = msi2500_ctrl_msg(s, CMD_WREG, 0x00608008);
 	if (ret)
@@ -838,14 +819,14 @@ static int msi2500_set_usb_adc(struct msi2500_state *s)
 		goto err;
 err:
 	return ret;
-};
+}
 
 static int msi2500_start_streaming(struct vb2_queue *vq, unsigned int count)
 {
 	struct msi2500_state *s = vb2_get_drv_priv(vq);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	if (!s->udev)
 		return -ENODEV;
@@ -873,7 +854,7 @@ static void msi2500_stop_streaming(struct vb2_queue *vq)
 {
 	struct msi2500_state *s = vb2_get_drv_priv(vq);
 
-	dev_dbg(&s->udev->dev, "%s:\n", __func__);
+	dev_dbg(s->dev, "\n");
 
 	mutex_lock(&s->v4l2_lock);
 
@@ -909,7 +890,7 @@ static int msi2500_enum_fmt_sdr_cap(struct file *file, void *priv,
 {
 	struct msi2500_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s: index=%d\n", __func__, f->index);
+	dev_dbg(s->dev, "index=%d\n", f->index);
 
 	if (f->index >= s->num_formats)
 		return -EINVAL;
@@ -925,7 +906,7 @@ static int msi2500_g_fmt_sdr_cap(struct file *file, void *priv,
 {
 	struct msi2500_state *s = video_drvdata(file);
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
+	dev_dbg(s->dev, "pixelformat fourcc %4.4s\n",
 			(char *)&s->pixelformat);
 
 	f->fmt.sdr.pixelformat = s->pixelformat;
@@ -942,7 +923,7 @@ static int msi2500_s_fmt_sdr_cap(struct file *file, void *priv,
 	struct vb2_queue *q = &s->vb_queue;
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
+	dev_dbg(s->dev, "pixelformat fourcc %4.4s\n",
 			(char *)&f->fmt.sdr.pixelformat);
 
 	if (vb2_is_busy(q))
@@ -972,7 +953,7 @@ static int msi2500_try_fmt_sdr_cap(struct file *file, void *priv,
 	struct msi2500_state *s = video_drvdata(file);
 	int i;
 
-	dev_dbg(&s->udev->dev, "%s: pixelformat fourcc %4.4s\n", __func__,
+	dev_dbg(s->dev, "pixelformat fourcc %4.4s\n",
 			(char *)&f->fmt.sdr.pixelformat);
 
 	memset(f->fmt.sdr.reserved, 0, sizeof(f->fmt.sdr.reserved));
@@ -995,7 +976,7 @@ static int msi2500_s_tuner(struct file *file, void *priv,
 	struct msi2500_state *s = video_drvdata(file);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s: index=%d\n", __func__, v->index);
+	dev_dbg(s->dev, "index=%d\n", v->index);
 
 	if (v->index == 0)
 		ret = 0;
@@ -1012,7 +993,7 @@ static int msi2500_g_tuner(struct file *file, void *priv, struct v4l2_tuner *v)
 	struct msi2500_state *s = video_drvdata(file);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s: index=%d\n", __func__, v->index);
+	dev_dbg(s->dev, "index=%d\n", v->index);
 
 	if (v->index == 0) {
 		strlcpy(v->name, "Mirics MSi2500", sizeof(v->name));
@@ -1036,8 +1017,7 @@ static int msi2500_g_frequency(struct file *file, void *priv,
 	struct msi2500_state *s = video_drvdata(file);
 	int ret  = 0;
 
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d\n",
-			__func__, f->tuner, f->type);
+	dev_dbg(s->dev, "tuner=%d type=%d\n", f->tuner, f->type);
 
 	if (f->tuner == 0) {
 		f->frequency = s->f_adc;
@@ -1058,15 +1038,14 @@ static int msi2500_s_frequency(struct file *file, void *priv,
 	struct msi2500_state *s = video_drvdata(file);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d frequency=%u\n",
-			__func__, f->tuner, f->type, f->frequency);
+	dev_dbg(s->dev, "tuner=%d type=%d frequency=%u\n",
+			f->tuner, f->type, f->frequency);
 
 	if (f->tuner == 0) {
 		s->f_adc = clamp_t(unsigned int, f->frequency,
 				bands[0].rangelow,
 				bands[0].rangehigh);
-		dev_dbg(&s->udev->dev, "%s: ADC frequency=%u Hz\n",
-				__func__, s->f_adc);
+		dev_dbg(s->dev, "ADC frequency=%u Hz\n", s->f_adc);
 		ret = msi2500_set_usb_adc(s);
 	} else if (f->tuner == 1) {
 		ret = v4l2_subdev_call(s->v4l2_subdev, tuner, s_frequency, f);
@@ -1083,8 +1062,8 @@ static int msi2500_enum_freq_bands(struct file *file, void *priv,
 	struct msi2500_state *s = video_drvdata(file);
 	int ret;
 
-	dev_dbg(&s->udev->dev, "%s: tuner=%d type=%d index=%d\n",
-			__func__, band->tuner, band->type, band->index);
+	dev_dbg(s->dev, "tuner=%d type=%d index=%d\n",
+			band->tuner, band->type, band->index);
 
 	if (band->tuner == 0) {
 		if (band->index >= ARRAY_SIZE(bands)) {
@@ -1169,8 +1148,7 @@ static int msi2500_transfer_one_message(struct spi_master *master,
 	u32 data;
 
 	list_for_each_entry(t, &m->transfers, transfer_list) {
-		dev_dbg(&s->udev->dev, "%s: msg=%*ph\n",
-				__func__, t->len, t->tx_buf);
+		dev_dbg(s->dev, "msg=%*ph\n", t->len, t->tx_buf);
 		data = 0x09; /* reg 9 is SPI adapter */
 		data |= ((u8 *)t->tx_buf)[0] << 8;
 		data |= ((u8 *)t->tx_buf)[1] << 16;
@@ -1186,8 +1164,7 @@ static int msi2500_transfer_one_message(struct spi_master *master,
 static int msi2500_probe(struct usb_interface *intf,
 		const struct usb_device_id *id)
 {
-	struct usb_device *udev = interface_to_usbdev(intf);
-	struct msi2500_state *s = NULL;
+	struct msi2500_state *s;
 	struct v4l2_subdev *sd;
 	struct spi_master *master;
 	int ret;
@@ -1200,7 +1177,7 @@ static int msi2500_probe(struct usb_interface *intf,
 
 	s = kzalloc(sizeof(struct msi2500_state), GFP_KERNEL);
 	if (s == NULL) {
-		pr_err("Could not allocate memory for msi2500_state\n");
+		dev_err(&intf->dev, "Could not allocate memory for state\n");
 		return -ENOMEM;
 	}
 
@@ -1208,12 +1185,13 @@ static int msi2500_probe(struct usb_interface *intf,
 	mutex_init(&s->vb_queue_lock);
 	spin_lock_init(&s->queued_bufs_lock);
 	INIT_LIST_HEAD(&s->queued_bufs);
-	s->udev = udev;
+	s->dev = &intf->dev;
+	s->udev = interface_to_usbdev(intf);
 	s->f_adc = bands[0].rangelow;
 	s->pixelformat = formats[0].pixelformat;
 	s->buffersize = formats[0].buffersize;
 	s->num_formats = NUM_FORMATS;
-	if (msi2500_emulated_fmt == false)
+	if (!msi2500_emulated_fmt)
 		s->num_formats -= 2;
 
 	/* Init videobuf2 queue structure */
@@ -1226,7 +1204,7 @@ static int msi2500_probe(struct usb_interface *intf,
 	s->vb_queue.timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
 	ret = vb2_queue_init(&s->vb_queue);
 	if (ret) {
-		dev_err(&s->udev->dev, "Could not initialize vb2 queue\n");
+		dev_err(s->dev, "Could not initialize vb2 queue\n");
 		goto err_free_mem;
 	}
 
@@ -1240,13 +1218,12 @@ static int msi2500_probe(struct usb_interface *intf,
 	s->v4l2_dev.release = msi2500_video_release;
 	ret = v4l2_device_register(&intf->dev, &s->v4l2_dev);
 	if (ret) {
-		dev_err(&s->udev->dev,
-				"Failed to register v4l2-device (%d)\n", ret);
+		dev_err(s->dev, "Failed to register v4l2-device (%d)\n", ret);
 		goto err_free_mem;
 	}
 
 	/* SPI master adapter */
-	master = spi_alloc_master(&s->udev->dev, 0);
+	master = spi_alloc_master(s->dev, 0);
 	if (master == NULL) {
 		ret = -ENOMEM;
 		goto err_unregister_v4l2_dev;
@@ -1267,7 +1244,7 @@ static int msi2500_probe(struct usb_interface *intf,
 	sd = v4l2_spi_new_subdev(&s->v4l2_dev, master, &board_info);
 	s->v4l2_subdev = sd;
 	if (sd == NULL) {
-		dev_err(&s->udev->dev, "cannot get v4l2 subdevice\n");
+		dev_err(s->dev, "cannot get v4l2 subdevice\n");
 		ret = -ENODEV;
 		goto err_unregister_master;
 	}
@@ -1276,7 +1253,7 @@ static int msi2500_probe(struct usb_interface *intf,
 	v4l2_ctrl_handler_init(&s->hdl, 0);
 	if (s->hdl.error) {
 		ret = s->hdl.error;
-		dev_err(&s->udev->dev, "Could not initialize controls\n");
+		dev_err(s->dev, "Could not initialize controls\n");
 		goto err_free_controls;
 	}
 
@@ -1289,16 +1266,13 @@ static int msi2500_probe(struct usb_interface *intf,
 
 	ret = video_register_device(&s->vdev, VFL_TYPE_SDR, -1);
 	if (ret) {
-		dev_err(&s->udev->dev,
-				"Failed to register as video device (%d)\n",
+		dev_err(s->dev, "Failed to register as video device (%d)\n",
 				ret);
 		goto err_unregister_v4l2_dev;
 	}
-	dev_info(&s->udev->dev, "Registered as %s\n",
+	dev_info(s->dev, "Registered as %s\n",
 			video_device_node_name(&s->vdev));
-	dev_notice(&s->udev->dev,
-			"%s: SDR API is still slightly experimental and functionality changes may follow\n",
-			KBUILD_MODNAME);
+	dev_notice(s->dev, "SDR API is still slightly experimental and functionality changes may follow\n");
 
 	return 0;
 
diff --git a/drivers/media/usb/pwc/pwc-v4l.c b/drivers/media/usb/pwc/pwc-v4l.c
index aa7449eaca08..3d987984602f 100644
--- a/drivers/media/usb/pwc/pwc-v4l.c
+++ b/drivers/media/usb/pwc/pwc-v4l.c
@@ -52,7 +52,7 @@ enum { custom_autocontour, custom_contour, custom_noise_reduction,
 	custom_awb_speed, custom_awb_delay,
 	custom_save_user, custom_restore_user, custom_restore_factory };
 
-const char * const pwc_auto_whitebal_qmenu[] = {
+static const char * const pwc_auto_whitebal_qmenu[] = {
 	"Indoor (Incandescant Lighting) Mode",
 	"Outdoor (Sunlight) Mode",
 	"Indoor (Fluorescent Lighting) Mode",
diff --git a/drivers/media/usb/s2255/s2255drv.c b/drivers/media/usb/s2255/s2255drv.c
index 2c901861034a..ccc00099b261 100644
--- a/drivers/media/usb/s2255/s2255drv.c
+++ b/drivers/media/usb/s2255/s2255drv.c
@@ -2245,7 +2245,7 @@ static int s2255_probe(struct usb_interface *interface,
 	}
 
 	atomic_set(&dev->num_channels, 0);
-	dev->pid = le16_to_cpu(id->idProduct);
+	dev->pid = id->idProduct;
 	dev->fw_data = kzalloc(sizeof(struct s2255_fw), GFP_KERNEL);
 	if (!dev->fw_data)
 		goto errorFWDATA1;
diff --git a/drivers/media/usb/siano/smsusb.c b/drivers/media/usb/siano/smsusb.c
index 1836a416d806..94e10b10b66e 100644
--- a/drivers/media/usb/siano/smsusb.c
+++ b/drivers/media/usb/siano/smsusb.c
@@ -277,14 +277,14 @@ static int smsusb1_load_firmware(struct usb_device *udev, int id, int board_id)
 		rc = usb_bulk_msg(udev, usb_sndbulkpipe(udev, 2),
 				  fw_buffer, fw->size, &dummy, 1000);
 
-		sms_info("sent %zd(%d) bytes, rc %d", fw->size, dummy, rc);
+		sms_info("sent %zu(%d) bytes, rc %d", fw->size, dummy, rc);
 
 		kfree(fw_buffer);
 	} else {
 		sms_err("failed to allocate firmware buffer");
 		rc = -ENOMEM;
 	}
-	sms_info("read FW %s, size=%zd", fw_filename, fw->size);
+	sms_info("read FW %s, size=%zu", fw_filename, fw->size);
 
 	release_firmware(fw);
 
@@ -655,6 +655,8 @@ static const struct usb_device_id smsusb_id_table[] = {
 		.driver_info = SMS1XXX_BOARD_ONDA_MDTV_DATA_CARD },
 	{ USB_DEVICE(0x3275, 0x0080),
 		.driver_info = SMS1XXX_BOARD_SIANO_RIO },
+	{ USB_DEVICE(0x2013, 0x0257),
+		.driver_info = SMS1XXX_BOARD_PCTV_77E },
 	{ } /* Terminating entry */
 	};
 
diff --git a/drivers/media/usb/ttusb-dec/ttusbdecfe.c b/drivers/media/usb/ttusb-dec/ttusbdecfe.c
index 5c45c9d0712d..9c29552aedec 100644
--- a/drivers/media/usb/ttusb-dec/ttusbdecfe.c
+++ b/drivers/media/usb/ttusb-dec/ttusbdecfe.c
@@ -156,6 +156,9 @@ static int ttusbdecfe_dvbs_diseqc_send_master_cmd(struct dvb_frontend* fe, struc
 		   0x00, 0x00, 0x00, 0x00,
 		   0x00, 0x00 };
 
+	if (cmd->msg_len > sizeof(b) - 4)
+		return -EINVAL;
+
 	memcpy(&b[4], cmd->msg, cmd->msg_len);
 
 	state->config->send_command(fe, 0x72,
diff --git a/drivers/media/usb/usbtv/Kconfig b/drivers/media/usb/usbtv/Kconfig
index 7c5b86006ee6..b833c5b9094e 100644
--- a/drivers/media/usb/usbtv/Kconfig
+++ b/drivers/media/usb/usbtv/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_USBTV
         tristate "USBTV007 video capture support"
-        depends on VIDEO_V4L2
+        depends on VIDEO_V4L2 && SND
+        select SND_PCM
         select VIDEOBUF2_VMALLOC
 
         ---help---
diff --git a/drivers/media/usb/usbtv/Makefile b/drivers/media/usb/usbtv/Makefile
index 775316a88ea6..f555cf8a3dd2 100644
--- a/drivers/media/usb/usbtv/Makefile
+++ b/drivers/media/usb/usbtv/Makefile
@@ -1,4 +1,5 @@
 usbtv-y := usbtv-core.o \
-	usbtv-video.o
+	usbtv-video.o \
+	usbtv-audio.o
 
 obj-$(CONFIG_VIDEO_USBTV) += usbtv.o
diff --git a/drivers/media/usb/usbtv/usbtv-audio.c b/drivers/media/usb/usbtv/usbtv-audio.c
new file mode 100644
index 000000000000..78c12d22dfbb
--- /dev/null
+++ b/drivers/media/usb/usbtv/usbtv-audio.c
@@ -0,0 +1,385 @@
+/*
+ * Fushicai USBTV007 Audio-Video Grabber Driver
+ *
+ * Product web site:
+ * http://www.fushicai.com/products_detail/&productId=d05449ee-b690-42f9-a661-aa7353894bed.html
+ *
+ * Copyright (c) 2013 Federico Simoncelli
+ * All rights reserved.
+ * No physical hardware was harmed running Windows during the
+ * reverse-engineering activity
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions, and the following disclaimer,
+ *    without modification.
+ * 2. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * Alternatively, this software may be distributed under the terms of the
+ * GNU General Public License ("GPL").
+ */
+
+#include <sound/core.h>
+#include <sound/initval.h>
+#include <sound/ac97_codec.h>
+#include <sound/pcm_params.h>
+
+#include "usbtv.h"
+
+static struct snd_pcm_hardware snd_usbtv_digital_hw = {
+	.info = SNDRV_PCM_INFO_BATCH |
+		SNDRV_PCM_INFO_MMAP |
+		SNDRV_PCM_INFO_INTERLEAVED |
+		SNDRV_PCM_INFO_BLOCK_TRANSFER |
+		SNDRV_PCM_INFO_MMAP_VALID,
+	.formats = SNDRV_PCM_FMTBIT_S16_LE,
+	.rates = SNDRV_PCM_RATE_48000,
+	.rate_min = 48000,
+	.rate_max = 48000,
+	.channels_min = 2,
+	.channels_max = 2,
+	.period_bytes_min = 11059,
+	.period_bytes_max = 13516,
+	.periods_min = 2,
+	.periods_max = 98,
+	.buffer_bytes_max = 62720 * 8, /* value in usbaudio.c */
+};
+
+static int snd_usbtv_pcm_open(struct snd_pcm_substream *substream)
+{
+	struct usbtv *chip = snd_pcm_substream_chip(substream);
+	struct snd_pcm_runtime *runtime = substream->runtime;
+
+	chip->snd_substream = substream;
+	runtime->hw = snd_usbtv_digital_hw;
+
+	return 0;
+}
+
+static int snd_usbtv_pcm_close(struct snd_pcm_substream *substream)
+{
+	struct usbtv *chip = snd_pcm_substream_chip(substream);
+
+	if (atomic_read(&chip->snd_stream)) {
+		atomic_set(&chip->snd_stream, 0);
+		schedule_work(&chip->snd_trigger);
+	}
+
+	return 0;
+}
+
+static int snd_usbtv_hw_params(struct snd_pcm_substream *substream,
+		struct snd_pcm_hw_params *hw_params)
+{
+	int rv;
+	struct usbtv *chip = snd_pcm_substream_chip(substream);
+
+	rv = snd_pcm_lib_malloc_pages(substream,
+		params_buffer_bytes(hw_params));
+
+	if (rv < 0) {
+		dev_warn(chip->dev, "pcm audio buffer allocation failure %i\n",
+			rv);
+		return rv;
+	}
+
+	return 0;
+}
+
+static int snd_usbtv_hw_free(struct snd_pcm_substream *substream)
+{
+	snd_pcm_lib_free_pages(substream);
+	return 0;
+}
+
+static int snd_usbtv_prepare(struct snd_pcm_substream *substream)
+{
+	struct usbtv *chip = snd_pcm_substream_chip(substream);
+
+	chip->snd_buffer_pos = 0;
+	chip->snd_period_pos = 0;
+
+	return 0;
+}
+
+static void usbtv_audio_urb_received(struct urb *urb)
+{
+	struct usbtv *chip = urb->context;
+	struct snd_pcm_substream *substream = chip->snd_substream;
+	struct snd_pcm_runtime *runtime = substream->runtime;
+	size_t i, frame_bytes, chunk_length, buffer_pos, period_pos;
+	int period_elapsed;
+	void *urb_current;
+
+	switch (urb->status) {
+	case 0:
+	case -ETIMEDOUT:
+		break;
+	case -ENOENT:
+	case -EPROTO:
+	case -ECONNRESET:
+	case -ESHUTDOWN:
+		return;
+	default:
+		dev_warn(chip->dev, "unknown audio urb status %i\n",
+			urb->status);
+	}
+
+	if (!atomic_read(&chip->snd_stream))
+		return;
+
+	frame_bytes = runtime->frame_bits >> 3;
+	chunk_length = USBTV_CHUNK / frame_bytes;
+
+	buffer_pos = chip->snd_buffer_pos;
+	period_pos = chip->snd_period_pos;
+	period_elapsed = 0;
+
+	for (i = 0; i < urb->actual_length; i += USBTV_CHUNK_SIZE) {
+		urb_current = urb->transfer_buffer + i + USBTV_AUDIO_HDRSIZE;
+
+		if (buffer_pos + chunk_length >= runtime->buffer_size) {
+			size_t cnt = (runtime->buffer_size - buffer_pos) *
+				frame_bytes;
+			memcpy(runtime->dma_area + buffer_pos * frame_bytes,
+				urb_current, cnt);
+			memcpy(runtime->dma_area, urb_current + cnt,
+				chunk_length * frame_bytes - cnt);
+		} else {
+			memcpy(runtime->dma_area + buffer_pos * frame_bytes,
+				urb_current, chunk_length * frame_bytes);
+		}
+
+		buffer_pos += chunk_length;
+		period_pos += chunk_length;
+
+		if (buffer_pos >= runtime->buffer_size)
+			buffer_pos -= runtime->buffer_size;
+
+		if (period_pos >= runtime->period_size) {
+			period_pos -= runtime->period_size;
+			period_elapsed = 1;
+		}
+	}
+
+	snd_pcm_stream_lock(substream);
+
+	chip->snd_buffer_pos = buffer_pos;
+	chip->snd_period_pos = period_pos;
+
+	snd_pcm_stream_unlock(substream);
+
+	if (period_elapsed)
+		snd_pcm_period_elapsed(substream);
+
+	usb_submit_urb(urb, GFP_ATOMIC);
+}
+
+static int usbtv_audio_start(struct usbtv *chip)
+{
+	unsigned int pipe;
+	static const u16 setup[][2] = {
+		/* These seem to enable the device. */
+		{ USBTV_BASE + 0x0008, 0x0001 },
+		{ USBTV_BASE + 0x01d0, 0x00ff },
+		{ USBTV_BASE + 0x01d9, 0x0002 },
+
+		{ USBTV_BASE + 0x01da, 0x0013 },
+		{ USBTV_BASE + 0x01db, 0x0012 },
+		{ USBTV_BASE + 0x01e9, 0x0002 },
+		{ USBTV_BASE + 0x01ec, 0x006c },
+		{ USBTV_BASE + 0x0294, 0x0020 },
+		{ USBTV_BASE + 0x0255, 0x00cf },
+		{ USBTV_BASE + 0x0256, 0x0020 },
+		{ USBTV_BASE + 0x01eb, 0x0030 },
+		{ USBTV_BASE + 0x027d, 0x00a6 },
+		{ USBTV_BASE + 0x0280, 0x0011 },
+		{ USBTV_BASE + 0x0281, 0x0040 },
+		{ USBTV_BASE + 0x0282, 0x0011 },
+		{ USBTV_BASE + 0x0283, 0x0040 },
+		{ 0xf891, 0x0010 },
+
+		/* this sets the input from composite */
+		{ USBTV_BASE + 0x0284, 0x00aa },
+	};
+
+	chip->snd_bulk_urb = usb_alloc_urb(0, GFP_KERNEL);
+	if (chip->snd_bulk_urb == NULL)
+		goto err_alloc_urb;
+
+	pipe = usb_rcvbulkpipe(chip->udev, USBTV_AUDIO_ENDP);
+
+	chip->snd_bulk_urb->transfer_buffer = kzalloc(
+		USBTV_AUDIO_URBSIZE, GFP_KERNEL);
+	if (chip->snd_bulk_urb->transfer_buffer == NULL)
+		goto err_transfer_buffer;
+
+	usb_fill_bulk_urb(chip->snd_bulk_urb, chip->udev, pipe,
+		chip->snd_bulk_urb->transfer_buffer, USBTV_AUDIO_URBSIZE,
+		usbtv_audio_urb_received, chip);
+
+	/* starting the stream */
+	usbtv_set_regs(chip, setup, ARRAY_SIZE(setup));
+
+	usb_clear_halt(chip->udev, pipe);
+	usb_submit_urb(chip->snd_bulk_urb, GFP_ATOMIC);
+
+	return 0;
+
+err_transfer_buffer:
+	usb_free_urb(chip->snd_bulk_urb);
+	chip->snd_bulk_urb = NULL;
+
+err_alloc_urb:
+	return -ENOMEM;
+}
+
+static int usbtv_audio_stop(struct usbtv *chip)
+{
+	static const u16 setup[][2] = {
+	/* The original windows driver sometimes sends also:
+	 *   { USBTV_BASE + 0x00a2, 0x0013 }
+	 * but it seems useless and its real effects are untested at
+	 * the moment.
+	 */
+		{ USBTV_BASE + 0x027d, 0x0000 },
+		{ USBTV_BASE + 0x0280, 0x0010 },
+		{ USBTV_BASE + 0x0282, 0x0010 },
+	};
+
+	if (chip->snd_bulk_urb) {
+		usb_kill_urb(chip->snd_bulk_urb);
+		kfree(chip->snd_bulk_urb->transfer_buffer);
+		usb_free_urb(chip->snd_bulk_urb);
+		chip->snd_bulk_urb = NULL;
+	}
+
+	usbtv_set_regs(chip, setup, ARRAY_SIZE(setup));
+
+	return 0;
+}
+
+void usbtv_audio_suspend(struct usbtv *usbtv)
+{
+	if (atomic_read(&usbtv->snd_stream) && usbtv->snd_bulk_urb)
+		usb_kill_urb(usbtv->snd_bulk_urb);
+}
+
+void usbtv_audio_resume(struct usbtv *usbtv)
+{
+	if (atomic_read(&usbtv->snd_stream) && usbtv->snd_bulk_urb)
+		usb_submit_urb(usbtv->snd_bulk_urb, GFP_ATOMIC);
+}
+
+static void snd_usbtv_trigger(struct work_struct *work)
+{
+	struct usbtv *chip = container_of(work, struct usbtv, snd_trigger);
+
+	if (atomic_read(&chip->snd_stream))
+		usbtv_audio_start(chip);
+	else
+		usbtv_audio_stop(chip);
+}
+
+static int snd_usbtv_card_trigger(struct snd_pcm_substream *substream, int cmd)
+{
+	struct usbtv *chip = snd_pcm_substream_chip(substream);
+
+	switch (cmd) {
+	case SNDRV_PCM_TRIGGER_START:
+	case SNDRV_PCM_TRIGGER_RESUME:
+	case SNDRV_PCM_TRIGGER_PAUSE_RELEASE:
+		atomic_set(&chip->snd_stream, 1);
+		break;
+	case SNDRV_PCM_TRIGGER_STOP:
+	case SNDRV_PCM_TRIGGER_SUSPEND:
+	case SNDRV_PCM_TRIGGER_PAUSE_PUSH:
+		atomic_set(&chip->snd_stream, 0);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	schedule_work(&chip->snd_trigger);
+
+	return 0;
+}
+
+static snd_pcm_uframes_t snd_usbtv_pointer(struct snd_pcm_substream *substream)
+{
+	struct usbtv *chip = snd_pcm_substream_chip(substream);
+
+	return chip->snd_buffer_pos;
+}
+
+static struct snd_pcm_ops snd_usbtv_pcm_ops = {
+	.open = snd_usbtv_pcm_open,
+	.close = snd_usbtv_pcm_close,
+	.ioctl = snd_pcm_lib_ioctl,
+	.hw_params = snd_usbtv_hw_params,
+	.hw_free = snd_usbtv_hw_free,
+	.prepare = snd_usbtv_prepare,
+	.trigger = snd_usbtv_card_trigger,
+	.pointer = snd_usbtv_pointer,
+};
+
+int usbtv_audio_init(struct usbtv *usbtv)
+{
+	int rv;
+	struct snd_card *card;
+	struct snd_pcm *pcm;
+
+	INIT_WORK(&usbtv->snd_trigger, snd_usbtv_trigger);
+	atomic_set(&usbtv->snd_stream, 0);
+
+	rv = snd_card_new(&usbtv->udev->dev, SNDRV_DEFAULT_IDX1, "usbtv",
+		THIS_MODULE, 0, &card);
+	if (rv < 0)
+		return rv;
+
+	strlcpy(card->driver, usbtv->dev->driver->name, sizeof(card->driver));
+	strlcpy(card->shortname, "usbtv", sizeof(card->shortname));
+	snprintf(card->longname, sizeof(card->longname),
+		"USBTV Audio at bus %d device %d", usbtv->udev->bus->busnum,
+		usbtv->udev->devnum);
+
+	snd_card_set_dev(card, usbtv->dev);
+
+	usbtv->snd = card;
+
+	rv = snd_pcm_new(card, "USBTV Audio", 0, 0, 1, &pcm);
+	if (rv < 0)
+		goto err;
+
+	strlcpy(pcm->name, "USBTV Audio Input", sizeof(pcm->name));
+	pcm->info_flags = 0;
+	pcm->private_data = usbtv;
+
+	snd_pcm_set_ops(pcm, SNDRV_PCM_STREAM_CAPTURE, &snd_usbtv_pcm_ops);
+	snd_pcm_lib_preallocate_pages_for_all(pcm, SNDRV_DMA_TYPE_CONTINUOUS,
+		snd_dma_continuous_data(GFP_KERNEL), USBTV_AUDIO_BUFFER,
+		USBTV_AUDIO_BUFFER);
+
+	rv = snd_card_register(card);
+	if (rv)
+		goto err;
+
+	return 0;
+
+err:
+	usbtv->snd = NULL;
+	snd_card_free(card);
+
+	return rv;
+}
+
+void usbtv_audio_free(struct usbtv *usbtv)
+{
+	if (usbtv->snd && usbtv->udev) {
+		snd_card_free(usbtv->snd);
+		usbtv->snd = NULL;
+	}
+}
diff --git a/drivers/media/usb/usbtv/usbtv-core.c b/drivers/media/usb/usbtv/usbtv-core.c
index 473fab81b602..29428bef272c 100644
--- a/drivers/media/usb/usbtv/usbtv-core.c
+++ b/drivers/media/usb/usbtv/usbtv-core.c
@@ -1,5 +1,5 @@
 /*
- * Fushicai USBTV007 Video Grabber Driver
+ * Fushicai USBTV007 Audio-Video Grabber Driver
  *
  * Product web site:
  * http://www.fushicai.com/products_detail/&productId=d05449ee-b690-42f9-a661-aa7353894bed.html
@@ -84,12 +84,19 @@ static int usbtv_probe(struct usb_interface *intf,
 	if (ret < 0)
 		goto usbtv_video_fail;
 
+	ret = usbtv_audio_init(usbtv);
+	if (ret < 0)
+		goto usbtv_audio_fail;
+
 	/* for simplicity we exploit the v4l2_device reference counting */
 	v4l2_device_get(&usbtv->v4l2_dev);
 
-	dev_info(dev, "Fushicai USBTV007 Video Grabber\n");
+	dev_info(dev, "Fushicai USBTV007 Audio-Video Grabber\n");
 	return 0;
 
+usbtv_audio_fail:
+	usbtv_video_free(usbtv);
+
 usbtv_video_fail:
 	usb_set_intfdata(intf, NULL);
 	usb_put_dev(usbtv->udev);
@@ -101,11 +108,13 @@ usbtv_video_fail:
 static void usbtv_disconnect(struct usb_interface *intf)
 {
 	struct usbtv *usbtv = usb_get_intfdata(intf);
+
 	usb_set_intfdata(intf, NULL);
 
 	if (!usbtv)
 		return;
 
+	usbtv_audio_free(usbtv);
 	usbtv_video_free(usbtv);
 
 	usb_put_dev(usbtv->udev);
@@ -122,8 +131,8 @@ static struct usb_device_id usbtv_id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, usbtv_id_table);
 
-MODULE_AUTHOR("Lubomir Rintel");
-MODULE_DESCRIPTION("Fushicai USBTV007 Video Grabber Driver");
+MODULE_AUTHOR("Lubomir Rintel, Federico Simoncelli");
+MODULE_DESCRIPTION("Fushicai USBTV007 Audio-Video Grabber Driver");
 MODULE_LICENSE("Dual BSD/GPL");
 
 static struct usb_driver usbtv_usb_driver = {
diff --git a/drivers/media/usb/usbtv/usbtv-video.c b/drivers/media/usb/usbtv/usbtv-video.c
index 030c5854b4b3..9d3525f659f0 100644
--- a/drivers/media/usb/usbtv/usbtv-video.c
+++ b/drivers/media/usb/usbtv/usbtv-video.c
@@ -1,5 +1,5 @@
 /*
- * Fushicai USBTV007 Video Grabber Driver
+ * Fushicai USBTV007 Audio-Video Grabber Driver
  *
  * Product web site:
  * http://www.fushicai.com/products_detail/&productId=d05449ee-b690-42f9-a661-aa7353894bed.html
@@ -79,7 +79,6 @@ static int usbtv_select_input(struct usbtv *usbtv, int input)
 		{ USBTV_BASE + 0x011f, 0x00f2 },
 		{ USBTV_BASE + 0x0127, 0x0060 },
 		{ USBTV_BASE + 0x00ae, 0x0010 },
-		{ USBTV_BASE + 0x0284, 0x00aa },
 		{ USBTV_BASE + 0x0239, 0x0060 },
 	};
 
@@ -88,7 +87,6 @@ static int usbtv_select_input(struct usbtv *usbtv, int input)
 		{ USBTV_BASE + 0x011f, 0x00ff },
 		{ USBTV_BASE + 0x0127, 0x0060 },
 		{ USBTV_BASE + 0x00ae, 0x0030 },
-		{ USBTV_BASE + 0x0284, 0x0088 },
 		{ USBTV_BASE + 0x0239, 0x0060 },
 	};
 
@@ -225,7 +223,6 @@ static int usbtv_setup_capture(struct usbtv *usbtv)
 		{ USBTV_BASE + 0x0159, 0x0006 },
 		{ USBTV_BASE + 0x015d, 0x0000 },
 
-		{ USBTV_BASE + 0x0284, 0x0088 },
 		{ USBTV_BASE + 0x0003, 0x0004 },
 		{ USBTV_BASE + 0x0100, 0x00d3 },
 		{ USBTV_BASE + 0x0115, 0x0015 },
@@ -256,7 +253,7 @@ static int usbtv_setup_capture(struct usbtv *usbtv)
  * 720 pixel lines, as the chunk is 240 words long, which is 480 pixels.
  * Therefore, we break down the chunk into two halves before copyting,
  * so that we can interleave a line if needed. */
-static void usbtv_chunk_to_vbuf(u32 *frame, u32 *src, int chunk_no, int odd)
+static void usbtv_chunk_to_vbuf(u32 *frame, __be32 *src, int chunk_no, int odd)
 {
 	int half;
 
@@ -266,6 +263,7 @@ static void usbtv_chunk_to_vbuf(u32 *frame, u32 *src, int chunk_no, int odd)
 		int part_index = (line * 2 + !odd) * 3 + (part_no % 3);
 
 		u32 *dst = &frame[part_index * USBTV_CHUNK/2];
+
 		memcpy(dst, src, USBTV_CHUNK/2 * sizeof(*src));
 		src += USBTV_CHUNK/2;
 	}
@@ -274,7 +272,7 @@ static void usbtv_chunk_to_vbuf(u32 *frame, u32 *src, int chunk_no, int odd)
 /* Called for each 256-byte image chunk.
  * First word identifies the chunk, followed by 240 words of image
  * data and padding. */
-static void usbtv_image_chunk(struct usbtv *usbtv, u32 *chunk)
+static void usbtv_image_chunk(struct usbtv *usbtv, __be32 *chunk)
 {
 	int frame_id, odd, chunk_no;
 	u32 *frame;
@@ -365,7 +363,7 @@ static void usbtv_iso_cb(struct urb *ip)
 
 		for (offset = 0; USBTV_CHUNK_SIZE * offset < size; offset++)
 			usbtv_image_chunk(usbtv,
-				(u32 *)&data[USBTV_CHUNK_SIZE * offset]);
+				(__be32 *)&data[USBTV_CHUNK_SIZE * offset]);
 	}
 
 resubmit:
@@ -410,6 +408,7 @@ static void usbtv_stop(struct usbtv *usbtv)
 	/* Cancel running transfers. */
 	for (i = 0; i < USBTV_ISOC_TRANSFERS; i++) {
 		struct urb *ip = usbtv->isoc_urbs[i];
+
 		if (ip == NULL)
 			continue;
 		usb_kill_urb(ip);
@@ -434,6 +433,8 @@ static int usbtv_start(struct usbtv *usbtv)
 	int i;
 	int ret;
 
+	usbtv_audio_suspend(usbtv);
+
 	ret = usb_set_interface(usbtv->udev, 0, 0);
 	if (ret < 0)
 		return ret;
@@ -446,6 +447,8 @@ static int usbtv_start(struct usbtv *usbtv)
 	if (ret < 0)
 		return ret;
 
+	usbtv_audio_resume(usbtv);
+
 	for (i = 0; i < USBTV_ISOC_TRANSFERS; i++) {
 		struct urb *ip;
 
@@ -559,6 +562,7 @@ static int usbtv_g_input(struct file *file, void *priv, unsigned int *i)
 static int usbtv_s_input(struct file *file, void *priv, unsigned int i)
 {
 	struct usbtv *usbtv = video_drvdata(file);
+
 	return usbtv_select_input(usbtv, i);
 }
 
diff --git a/drivers/media/usb/usbtv/usbtv.h b/drivers/media/usb/usbtv/usbtv.h
index cb1d388cc647..968119581fab 100644
--- a/drivers/media/usb/usbtv/usbtv.h
+++ b/drivers/media/usb/usbtv/usbtv.h
@@ -1,5 +1,5 @@
 /*
- * Fushicai USBTV007 Video Grabber Driver
+ * Fushicai USBTV007 Audio-Video Grabber Driver
  *
  * Copyright (c) 2013 Lubomir Rintel
  * All rights reserved.
@@ -28,6 +28,7 @@
 
 /* Hardware. */
 #define USBTV_VIDEO_ENDP	0x81
+#define USBTV_AUDIO_ENDP	0x83
 #define USBTV_BASE		0xc000
 #define USBTV_REQUEST_REG	12
 
@@ -39,6 +40,10 @@
 #define USBTV_CHUNK_SIZE	256
 #define USBTV_CHUNK		240
 
+#define USBTV_AUDIO_URBSIZE	20480
+#define USBTV_AUDIO_HDRSIZE	4
+#define USBTV_AUDIO_BUFFER	65536
+
 /* Chunk header. */
 #define USBTV_MAGIC_OK(chunk)	((be32_to_cpu(chunk[0]) & 0xff000000) \
 							== 0x88000000)
@@ -91,9 +96,23 @@ struct usbtv {
 	int iso_size;
 	unsigned int sequence;
 	struct urb *isoc_urbs[USBTV_ISOC_TRANSFERS];
+
+	/* audio */
+	struct snd_card *snd;
+	struct snd_pcm_substream *snd_substream;
+	atomic_t snd_stream;
+	struct work_struct snd_trigger;
+	struct urb *snd_bulk_urb;
+	size_t snd_buffer_pos;
+	size_t snd_period_pos;
 };
 
 int usbtv_set_regs(struct usbtv *usbtv, const u16 regs[][2], int size);
 
 int usbtv_video_init(struct usbtv *usbtv);
 void usbtv_video_free(struct usbtv *usbtv);
+
+int usbtv_audio_init(struct usbtv *usbtv);
+void usbtv_audio_free(struct usbtv *usbtv);
+void usbtv_audio_suspend(struct usbtv *usbtv);
+void usbtv_audio_resume(struct usbtv *usbtv);
diff --git a/drivers/media/usb/uvc/uvc_ctrl.c b/drivers/media/usb/uvc/uvc_ctrl.c
index 0eb82106d2ff..3e59b288b8a8 100644
--- a/drivers/media/usb/uvc/uvc_ctrl.c
+++ b/drivers/media/usb/uvc/uvc_ctrl.c
@@ -309,9 +309,8 @@ static struct uvc_control_info uvc_ctrls[] = {
 		.selector	= UVC_CT_PANTILT_RELATIVE_CONTROL,
 		.index		= 12,
 		.size		= 4,
-		.flags		= UVC_CTRL_FLAG_SET_CUR | UVC_CTRL_FLAG_GET_MIN
-				| UVC_CTRL_FLAG_GET_MAX | UVC_CTRL_FLAG_GET_RES
-				| UVC_CTRL_FLAG_GET_DEF
+		.flags		= UVC_CTRL_FLAG_SET_CUR
+				| UVC_CTRL_FLAG_GET_RANGE
 				| UVC_CTRL_FLAG_AUTO_UPDATE,
 	},
 	{
@@ -391,6 +390,35 @@ static void uvc_ctrl_set_zoom(struct uvc_control_mapping *mapping,
 	data[2] = min((int)abs(value), 0xff);
 }
 
+static __s32 uvc_ctrl_get_rel_speed(struct uvc_control_mapping *mapping,
+	__u8 query, const __u8 *data)
+{
+	unsigned int first = mapping->offset / 8;
+	__s8 rel = (__s8)data[first];
+
+	switch (query) {
+	case UVC_GET_CUR:
+		return (rel == 0) ? 0 : (rel > 0 ? data[first+1]
+						 : -data[first+1]);
+	case UVC_GET_MIN:
+		return -data[first+1];
+	case UVC_GET_MAX:
+	case UVC_GET_RES:
+	case UVC_GET_DEF:
+	default:
+		return data[first+1];
+	}
+}
+
+static void uvc_ctrl_set_rel_speed(struct uvc_control_mapping *mapping,
+	__s32 value, __u8 *data)
+{
+	unsigned int first = mapping->offset / 8;
+
+	data[first] = value == 0 ? 0 : (value > 0) ? 1 : 0xff;
+	data[first+1] = min_t(int, abs(value), 0xff);
+}
+
 static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 	{
 		.id		= V4L2_CID_BRIGHTNESS,
@@ -677,6 +705,30 @@ static struct uvc_control_mapping uvc_ctrl_mappings[] = {
 		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
 	},
 	{
+		.id		= V4L2_CID_PAN_SPEED,
+		.name		= "Pan (Speed)",
+		.entity		= UVC_GUID_UVC_CAMERA,
+		.selector	= UVC_CT_PANTILT_RELATIVE_CONTROL,
+		.size		= 16,
+		.offset		= 0,
+		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
+		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.get		= uvc_ctrl_get_rel_speed,
+		.set		= uvc_ctrl_set_rel_speed,
+	},
+	{
+		.id		= V4L2_CID_TILT_SPEED,
+		.name		= "Tilt (Speed)",
+		.entity		= UVC_GUID_UVC_CAMERA,
+		.selector	= UVC_CT_PANTILT_RELATIVE_CONTROL,
+		.size		= 16,
+		.offset		= 16,
+		.v4l2_type	= V4L2_CTRL_TYPE_INTEGER,
+		.data_type	= UVC_CTRL_DATA_TYPE_SIGNED,
+		.get		= uvc_ctrl_get_rel_speed,
+		.set		= uvc_ctrl_set_rel_speed,
+	},
+	{
 		.id		= V4L2_CID_PRIVACY,
 		.name		= "Privacy",
 		.entity		= UVC_GUID_UVC_CAMERA,
@@ -1795,7 +1847,7 @@ done:
  * - Handle restore order (Auto-Exposure Mode should be restored before
  *   Exposure Time).
  */
-int uvc_ctrl_resume_device(struct uvc_device *dev)
+int uvc_ctrl_restore_values(struct uvc_device *dev)
 {
 	struct uvc_control *ctrl;
 	struct uvc_entity *entity;
diff --git a/drivers/media/usb/uvc/uvc_driver.c b/drivers/media/usb/uvc/uvc_driver.c
index f8135f4e3b52..7c8322d4fc63 100644
--- a/drivers/media/usb/uvc/uvc_driver.c
+++ b/drivers/media/usb/uvc/uvc_driver.c
@@ -2000,7 +2000,7 @@ static int __uvc_resume(struct usb_interface *intf, int reset)
 		int ret = 0;
 
 		if (reset) {
-			ret = uvc_ctrl_resume_device(dev);
+			ret = uvc_ctrl_restore_values(dev);
 			if (ret < 0)
 				return ret;
 		}
@@ -2175,6 +2175,15 @@ static struct usb_device_id uvc_ids[] = {
 	  .bInterfaceClass	= USB_CLASS_VENDOR_SPEC,
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0 },
+	/* Logitech HD Pro Webcam C920 */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x046d,
+	  .idProduct		= 0x082d,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_QUIRK_RESTORE_CTRLS_ON_INIT },
 	/* Chicony CNF7129 (Asus EEE 100HE) */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
@@ -2229,6 +2238,15 @@ static struct usb_device_id uvc_ids[] = {
 	  .bInterfaceSubClass	= 1,
 	  .bInterfaceProtocol	= 0,
 	  .driver_info		= UVC_QUIRK_PROBE_DEF },
+	/* Dell XPS M1330 (OmniVision OV7670 webcam) */
+	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
+				| USB_DEVICE_ID_MATCH_INT_INFO,
+	  .idVendor		= 0x05a9,
+	  .idProduct		= 0x7670,
+	  .bInterfaceClass	= USB_CLASS_VIDEO,
+	  .bInterfaceSubClass	= 1,
+	  .bInterfaceProtocol	= 0,
+	  .driver_info		= UVC_QUIRK_PROBE_DEF },
 	/* Apple Built-In iSight */
 	{ .match_flags		= USB_DEVICE_ID_MATCH_DEVICE
 				| USB_DEVICE_ID_MATCH_INT_INFO,
diff --git a/drivers/media/usb/uvc/uvc_v4l2.c b/drivers/media/usb/uvc/uvc_v4l2.c
index 378ae02e593b..60a8e2c3631e 100644
--- a/drivers/media/usb/uvc/uvc_v4l2.c
+++ b/drivers/media/usb/uvc/uvc_v4l2.c
@@ -318,6 +318,7 @@ static int uvc_v4l2_set_format(struct uvc_streaming *stream,
 	stream->ctrl = probe;
 	stream->cur_format = format;
 	stream->cur_frame = frame;
+	stream->frame_size = fmt->fmt.pix.sizeimage;
 
 done:
 	mutex_unlock(&stream->mutex);
diff --git a/drivers/media/usb/uvc/uvc_video.c b/drivers/media/usb/uvc/uvc_video.c
index 9144a2f3ed82..9ace520bb079 100644
--- a/drivers/media/usb/uvc/uvc_video.c
+++ b/drivers/media/usb/uvc/uvc_video.c
@@ -1143,7 +1143,7 @@ static int uvc_video_encode_data(struct uvc_streaming *stream,
 static void uvc_video_validate_buffer(const struct uvc_streaming *stream,
 				      struct uvc_buffer *buf)
 {
-	if (buf->length != buf->bytesused &&
+	if (stream->frame_size != buf->bytesused &&
 	    !(stream->cur_format->flags & UVC_FMT_FLAG_COMPRESSED))
 		buf->error = 1;
 }
@@ -1463,7 +1463,7 @@ static unsigned int uvc_endpoint_max_bpi(struct usb_device *dev,
 
 	switch (dev->speed) {
 	case USB_SPEED_SUPER:
-		return ep->ss_ep_comp.wBytesPerInterval;
+		return le16_to_cpu(ep->ss_ep_comp.wBytesPerInterval);
 	case USB_SPEED_HIGH:
 		psize = usb_endpoint_maxp(&ep->desc);
 		return (psize & 0x07ff) * (1 + ((psize >> 11) & 3));
@@ -1678,6 +1678,12 @@ static int uvc_init_video(struct uvc_streaming *stream, gfp_t gfp_flags)
 		}
 	}
 
+	/* The Logitech C920 temporarily forgets that it should not be adjusting
+	 * Exposure Absolute during init so restore controls to stored values.
+	 */
+	if (stream->dev->quirks & UVC_QUIRK_RESTORE_CTRLS_ON_INIT)
+		uvc_ctrl_restore_values(stream->dev);
+
 	return 0;
 }
 
diff --git a/drivers/media/usb/uvc/uvcvideo.h b/drivers/media/usb/uvc/uvcvideo.h
index b1f69a6d4068..6f676c29ec09 100644
--- a/drivers/media/usb/uvc/uvcvideo.h
+++ b/drivers/media/usb/uvc/uvcvideo.h
@@ -147,6 +147,7 @@
 #define UVC_QUIRK_FIX_BANDWIDTH		0x00000080
 #define UVC_QUIRK_PROBE_DEF		0x00000100
 #define UVC_QUIRK_RESTRICT_FRAME_RATE	0x00000200
+#define UVC_QUIRK_RESTORE_CTRLS_ON_INIT	0x00000400
 
 /* Format flags */
 #define UVC_FMT_FLAG_COMPRESSED		0x00000001
@@ -456,6 +457,8 @@ struct uvc_streaming {
 	struct uvc_format *def_format;
 	struct uvc_format *cur_format;
 	struct uvc_frame *cur_frame;
+	size_t frame_size;
+
 	/* Protect access to ctrl, cur_format, cur_frame and hardware video
 	 * probe control.
 	 */
@@ -688,7 +691,7 @@ extern int uvc_ctrl_add_mapping(struct uvc_video_chain *chain,
 		const struct uvc_control_mapping *mapping);
 extern int uvc_ctrl_init_device(struct uvc_device *dev);
 extern void uvc_ctrl_cleanup_device(struct uvc_device *dev);
-extern int uvc_ctrl_resume_device(struct uvc_device *dev);
+extern int uvc_ctrl_restore_values(struct uvc_device *dev);
 
 extern int uvc_ctrl_begin(struct uvc_video_chain *chain);
 extern int __uvc_ctrl_commit(struct uvc_fh *handle, int rollback,
diff --git a/drivers/media/v4l2-core/Kconfig b/drivers/media/v4l2-core/Kconfig
index 9ca0f8d59a14..ba7e21a73023 100644
--- a/drivers/media/v4l2-core/Kconfig
+++ b/drivers/media/v4l2-core/Kconfig
@@ -25,6 +25,15 @@ config VIDEO_FIXED_MINOR_RANGES
 
 	  When in doubt, say N.
 
+config VIDEO_PCI_SKELETON
+	tristate "Skeleton PCI V4L2 driver"
+	depends on PCI && BUILD_DOCSRC
+	depends on VIDEO_V4L2 && VIDEOBUF2_CORE
+	depends on VIDEOBUF2_MEMOPS && VIDEOBUF2_DMA_CONTIG
+	---help---
+	  Enable build of the skeleton PCI driver, used as a reference
+	  when developing new drivers.
+
 # Used by drivers that need tuner.ko
 config VIDEO_TUNER
 	tristate
diff --git a/drivers/media/v4l2-core/tuner-core.c b/drivers/media/v4l2-core/tuner-core.c
index 06c18ba16fa0..559f8372e2eb 100644
--- a/drivers/media/v4l2-core/tuner-core.c
+++ b/drivers/media/v4l2-core/tuner-core.c
@@ -601,7 +601,7 @@ static int tuner_probe(struct i2c_client *client,
 	t->name = "(tuner unset)";
 	t->type = UNSET;
 	t->audmode = V4L2_TUNER_MODE_STEREO;
-	t->standby = 1;
+	t->standby = true;
 	t->radio_freq = 87.5 * 16000;	/* Initial freq range */
 	t->tv_freq = 400 * 16; /* Sets freq to VHF High - needed for some PLL's to properly start */
 
@@ -1260,7 +1260,9 @@ static int tuner_suspend(struct device *dev)
 
 	tuner_dbg("suspend\n");
 
-	if (!t->standby && analog_ops->standby)
+	if (t->fe.ops.tuner_ops.suspend)
+		t->fe.ops.tuner_ops.suspend(&t->fe);
+	else if (!t->standby && analog_ops->standby)
 		analog_ops->standby(&t->fe);
 
 	return 0;
@@ -1273,7 +1275,9 @@ static int tuner_resume(struct device *dev)
 
 	tuner_dbg("resume\n");
 
-	if (!t->standby)
+	if (t->fe.ops.tuner_ops.resume)
+		t->fe.ops.tuner_ops.resume(&t->fe);
+	else if (!t->standby)
 		if (set_mode(t, t->mode) == 0)
 			set_freq(t, 0);
 
diff --git a/drivers/media/v4l2-core/v4l2-common.c b/drivers/media/v4l2-core/v4l2-common.c
index ccaa38f65cf1..2e9d81f4c1a5 100644
--- a/drivers/media/v4l2-core/v4l2-common.c
+++ b/drivers/media/v4l2-core/v4l2-common.c
@@ -435,16 +435,13 @@ static unsigned int clamp_align(unsigned int x, unsigned int min,
 	/* Bits that must be zero to be aligned */
 	unsigned int mask = ~((1 << align) - 1);
 
+	/* Clamp to aligned min and max */
+	x = clamp(x, (min + ~mask) & mask, max & mask);
+
 	/* Round to nearest aligned value */
 	if (align)
 		x = (x + (1 << (align - 1))) & mask;
 
-	/* Clamp to aligned value of min and max */
-	if (x < min)
-		x = (min + ~mask) & mask;
-	else if (x > max)
-		x = max & mask;
-
 	return x;
 }
 
diff --git a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
index cca6c2f76b3a..e502a5fb2994 100644
--- a/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
+++ b/drivers/media/v4l2-core/v4l2-compat-ioctl32.c
@@ -328,7 +328,7 @@ struct v4l2_buffer32 {
 	__u32			reserved;
 };
 
-static int get_v4l2_plane32(struct v4l2_plane *up, struct v4l2_plane32 *up32,
+static int get_v4l2_plane32(struct v4l2_plane __user *up, struct v4l2_plane32 __user *up32,
 				enum v4l2_memory memory)
 {
 	void __user *up_pln;
@@ -357,7 +357,7 @@ static int get_v4l2_plane32(struct v4l2_plane *up, struct v4l2_plane32 *up32,
 	return 0;
 }
 
-static int put_v4l2_plane32(struct v4l2_plane *up, struct v4l2_plane32 *up32,
+static int put_v4l2_plane32(struct v4l2_plane __user *up, struct v4l2_plane32 __user *up32,
 				enum v4l2_memory memory)
 {
 	if (copy_in_user(up32, up, 2 * sizeof(__u32)) ||
@@ -427,7 +427,7 @@ static int get_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		 * by passing a very big num_planes value */
 		uplane = compat_alloc_user_space(num_planes *
 						sizeof(struct v4l2_plane));
-		kp->m.planes = uplane;
+		kp->m.planes = (__force struct v4l2_plane *)uplane;
 
 		while (--num_planes >= 0) {
 			ret = get_v4l2_plane32(uplane, uplane32, kp->memory);
@@ -498,7 +498,7 @@ static int put_v4l2_buffer32(struct v4l2_buffer *kp, struct v4l2_buffer32 __user
 		if (num_planes == 0)
 			return 0;
 
-		uplane = kp->m.planes;
+		uplane = (__force struct v4l2_plane __user *)kp->m.planes;
 		if (get_user(p, &up->m.planes))
 			return -EFAULT;
 		uplane32 = compat_ptr(p);
@@ -562,7 +562,7 @@ static int get_v4l2_framebuffer32(struct v4l2_framebuffer *kp, struct v4l2_frame
 		get_user(kp->flags, &up->flags) ||
 		copy_from_user(&kp->fmt, &up->fmt, sizeof(up->fmt)))
 			return -EFAULT;
-	kp->base = compat_ptr(tmp);
+	kp->base = (__force void *)compat_ptr(tmp);
 	return 0;
 }
 
@@ -667,11 +667,15 @@ static int get_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 			n * sizeof(struct v4l2_ext_control32)))
 		return -EFAULT;
 	kcontrols = compat_alloc_user_space(n * sizeof(struct v4l2_ext_control));
-	kp->controls = kcontrols;
+	kp->controls = (__force struct v4l2_ext_control *)kcontrols;
 	while (--n >= 0) {
+		u32 id;
+
 		if (copy_in_user(kcontrols, ucontrols, sizeof(*ucontrols)))
 			return -EFAULT;
-		if (ctrl_is_pointer(kcontrols->id)) {
+		if (get_user(id, &kcontrols->id))
+			return -EFAULT;
+		if (ctrl_is_pointer(id)) {
 			void __user *s;
 
 			if (get_user(p, &ucontrols->string))
@@ -689,7 +693,8 @@ static int get_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 static int put_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext_controls32 __user *up)
 {
 	struct v4l2_ext_control32 __user *ucontrols;
-	struct v4l2_ext_control __user *kcontrols = kp->controls;
+	struct v4l2_ext_control __user *kcontrols =
+		(__force struct v4l2_ext_control __user *)kp->controls;
 	int n = kp->count;
 	compat_caddr_t p;
 
@@ -711,11 +716,14 @@ static int put_v4l2_ext_controls32(struct v4l2_ext_controls *kp, struct v4l2_ext
 
 	while (--n >= 0) {
 		unsigned size = sizeof(*ucontrols);
+		u32 id;
 
+		if (get_user(id, &kcontrols->id))
+			return -EFAULT;
 		/* Do not modify the pointer when copying a pointer control.
 		   The contents of the pointer was changed, not the pointer
 		   itself. */
-		if (ctrl_is_pointer(kcontrols->id))
+		if (ctrl_is_pointer(id))
 			size -= sizeof(ucontrols->value64);
 		if (copy_in_user(ucontrols, kcontrols, size))
 			return -EFAULT;
@@ -770,7 +778,7 @@ static int get_v4l2_edid32(struct v4l2_edid *kp, struct v4l2_edid32 __user *up)
 		get_user(tmp, &up->edid) ||
 		copy_from_user(kp->reserved, up->reserved, sizeof(kp->reserved)))
 			return -EFAULT;
-	kp->edid = compat_ptr(tmp);
+	kp->edid = (__force u8 *)compat_ptr(tmp);
 	return 0;
 }
 
@@ -783,7 +791,7 @@ static int put_v4l2_edid32(struct v4l2_edid *kp, struct v4l2_edid32 __user *up)
 		put_user(kp->start_block, &up->start_block) ||
 		put_user(kp->blocks, &up->blocks) ||
 		put_user(tmp, &up->edid) ||
-		copy_to_user(kp->reserved, up->reserved, sizeof(kp->reserved)))
+		copy_to_user(up->reserved, kp->reserved, sizeof(up->reserved)))
 			return -EFAULT;
 	return 0;
 }
diff --git a/drivers/media/v4l2-core/v4l2-ctrls.c b/drivers/media/v4l2-core/v4l2-ctrls.c
index f030d6a9e044..86012140923f 100644
--- a/drivers/media/v4l2-core/v4l2-ctrls.c
+++ b/drivers/media/v4l2-core/v4l2-ctrls.c
@@ -796,6 +796,8 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_AUTO_FOCUS_STOP:		return "Auto Focus, Stop";
 	case V4L2_CID_AUTO_FOCUS_STATUS:	return "Auto Focus, Status";
 	case V4L2_CID_AUTO_FOCUS_RANGE:		return "Auto Focus, Range";
+	case V4L2_CID_PAN_SPEED:		return "Pan, Speed";
+	case V4L2_CID_TILT_SPEED:		return "Tilt, Speed";
 
 	/* FM Radio Modulator controls */
 	/* Keep the order of the 'case's the same as in v4l2-controls.h! */
@@ -859,6 +861,10 @@ const char *v4l2_ctrl_get_name(u32 id)
 	case V4L2_CID_VBLANK:			return "Vertical Blanking";
 	case V4L2_CID_HBLANK:			return "Horizontal Blanking";
 	case V4L2_CID_ANALOGUE_GAIN:		return "Analogue Gain";
+	case V4L2_CID_TEST_PATTERN_RED:		return "Red Pixel Value";
+	case V4L2_CID_TEST_PATTERN_GREENR:	return "Green (Red) Pixel Value";
+	case V4L2_CID_TEST_PATTERN_BLUE:	return "Blue Pixel Value";
+	case V4L2_CID_TEST_PATTERN_GREENB:	return "Green (Blue) Pixel Value";
 
 	/* Image processing controls */
 	/* Keep the order of the 'case's the same as in v4l2-controls.h! */
diff --git a/drivers/media/v4l2-core/v4l2-dv-timings.c b/drivers/media/v4l2-core/v4l2-dv-timings.c
index ce1c9f5d9dee..b1d8dbb39665 100644
--- a/drivers/media/v4l2-core/v4l2-dv-timings.c
+++ b/drivers/media/v4l2-core/v4l2-dv-timings.c
@@ -164,7 +164,8 @@ bool v4l2_valid_dv_timings(const struct v4l2_dv_timings *t,
 	    bt->width > cap->max_width ||
 	    bt->pixelclock < cap->min_pixelclock ||
 	    bt->pixelclock > cap->max_pixelclock ||
-	    (cap->standards && !(bt->standards & cap->standards)) ||
+	    (cap->standards && bt->standards &&
+	     !(bt->standards & cap->standards)) ||
 	    (bt->interlaced && !(caps & V4L2_DV_BT_CAP_INTERLACED)) ||
 	    (!bt->interlaced && !(caps & V4L2_DV_BT_CAP_PROGRESSIVE)))
 		return false;
diff --git a/drivers/media/v4l2-core/v4l2-ioctl.c b/drivers/media/v4l2-core/v4l2-ioctl.c
index d15e16737eef..9ccb19a435ef 100644
--- a/drivers/media/v4l2-core/v4l2-ioctl.c
+++ b/drivers/media/v4l2-core/v4l2-ioctl.c
@@ -562,7 +562,7 @@ static void v4l_print_ext_controls(const void *arg, bool write_only)
 	pr_cont("class=0x%x, count=%d, error_idx=%d",
 			p->ctrl_class, p->count, p->error_idx);
 	for (i = 0; i < p->count; i++) {
-		if (p->controls[i].size)
+		if (!p->controls[i].size)
 			pr_cont(", id/val=0x%x/0x%x",
 				p->controls[i].id, p->controls[i].value);
 		else
@@ -1153,9 +1153,9 @@ static int v4l_g_fmt(const struct v4l2_ioctl_ops *ops,
 	switch (p->type) {
 	case V4L2_BUF_TYPE_VIDEO_OVERLAY:
 	case V4L2_BUF_TYPE_VIDEO_OUTPUT_OVERLAY: {
-		struct v4l2_clip *clips = p->fmt.win.clips;
+		struct v4l2_clip __user *clips = p->fmt.win.clips;
 		u32 clipcount = p->fmt.win.clipcount;
-		void *bitmap = p->fmt.win.bitmap;
+		void __user *bitmap = p->fmt.win.bitmap;
 
 		memset(&p->fmt, 0, sizeof(p->fmt));
 		p->fmt.win.clips = clips;
diff --git a/drivers/media/v4l2-core/v4l2-subdev.c b/drivers/media/v4l2-core/v4l2-subdev.c
index b4d235c13fbf..543631c3557a 100644
--- a/drivers/media/v4l2-core/v4l2-subdev.c
+++ b/drivers/media/v4l2-core/v4l2-subdev.c
@@ -501,11 +501,20 @@ int v4l2_subdev_link_validate_default(struct v4l2_subdev *sd,
 				      struct v4l2_subdev_format *source_fmt,
 				      struct v4l2_subdev_format *sink_fmt)
 {
+	/* The width, height and code must match. */
 	if (source_fmt->format.width != sink_fmt->format.width
 	    || source_fmt->format.height != sink_fmt->format.height
 	    || source_fmt->format.code != sink_fmt->format.code)
 		return -EINVAL;
 
+	/* The field order must match, or the sink field order must be NONE
+	 * to support interlaced hardware connected to bridges that support
+	 * progressive formats only.
+	 */
+	if (source_fmt->format.field != sink_fmt->format.field &&
+	    sink_fmt->format.field != V4L2_FIELD_NONE)
+		return -EINVAL;
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(v4l2_subdev_link_validate_default);
diff --git a/drivers/media/v4l2-core/videobuf-core.c b/drivers/media/v4l2-core/videobuf-core.c
index fb5ee5dd8fe9..b91a266d0b7e 100644
--- a/drivers/media/v4l2-core/videobuf-core.c
+++ b/drivers/media/v4l2-core/videobuf-core.c
@@ -441,11 +441,6 @@ int videobuf_reqbufs(struct videobuf_queue *q,
 	unsigned int size, count;
 	int retval;
 
-	if (req->count < 1) {
-		dprintk(1, "reqbufs: count invalid (%d)\n", req->count);
-		return -EINVAL;
-	}
-
 	if (req->memory != V4L2_MEMORY_MMAP     &&
 	    req->memory != V4L2_MEMORY_USERPTR  &&
 	    req->memory != V4L2_MEMORY_OVERLAY) {
@@ -471,6 +466,12 @@ int videobuf_reqbufs(struct videobuf_queue *q,
 		goto done;
 	}
 
+	if (req->count == 0) {
+		dprintk(1, "reqbufs: count invalid (%d)\n", req->count);
+		retval = __videobuf_free(q);
+		goto done;
+	}
+
 	count = req->count;
 	if (count > VIDEO_MAX_FRAME)
 		count = VIDEO_MAX_FRAME;
diff --git a/drivers/media/v4l2-core/videobuf-dma-sg.c b/drivers/media/v4l2-core/videobuf-dma-sg.c
index 3c8cc023a5a5..3ff15f1c9d70 100644
--- a/drivers/media/v4l2-core/videobuf-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf-dma-sg.c
@@ -253,9 +253,11 @@ int videobuf_dma_init_kernel(struct videobuf_dmabuf *dma, int direction,
 	return 0;
 out_free_pages:
 	while (i > 0) {
-		void *addr = page_address(dma->vaddr_pages[i]);
-		dma_free_coherent(dma->dev, PAGE_SIZE, addr, dma->dma_addr[i]);
+		void *addr;
+
 		i--;
+		addr = page_address(dma->vaddr_pages[i]);
+		dma_free_coherent(dma->dev, PAGE_SIZE, addr, dma->dma_addr[i]);
 	}
 	kfree(dma->dma_addr);
 	dma->dma_addr = NULL;
diff --git a/drivers/media/v4l2-core/videobuf2-core.c b/drivers/media/v4l2-core/videobuf2-core.c
index c359006074a8..f2e43de3dd87 100644
--- a/drivers/media/v4l2-core/videobuf2-core.c
+++ b/drivers/media/v4l2-core/videobuf2-core.c
@@ -36,7 +36,7 @@ module_param(debug, int, 0644);
 #define dprintk(level, fmt, arg...)					      \
 	do {								      \
 		if (debug >= level)					      \
-			pr_debug("vb2: %s: " fmt, __func__, ## arg); \
+			pr_info("vb2: %s: " fmt, __func__, ## arg); \
 	} while (0)
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
@@ -882,7 +882,9 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 		 * We already have buffers allocated, so first check if they
 		 * are not in use and can be freed.
 		 */
+		mutex_lock(&q->mmap_lock);
 		if (q->memory == V4L2_MEMORY_MMAP && __buffers_in_use(q)) {
+			mutex_unlock(&q->mmap_lock);
 			dprintk(1, "memory in use, cannot free\n");
 			return -EBUSY;
 		}
@@ -894,6 +896,7 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 		 */
 		__vb2_queue_cancel(q);
 		ret = __vb2_queue_free(q, q->num_buffers);
+		mutex_unlock(&q->mmap_lock);
 		if (ret)
 			return ret;
 
@@ -955,6 +958,7 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 		 */
 	}
 
+	mutex_lock(&q->mmap_lock);
 	q->num_buffers = allocated_buffers;
 
 	if (ret < 0) {
@@ -963,14 +967,17 @@ static int __reqbufs(struct vb2_queue *q, struct v4l2_requestbuffers *req)
 		 * from q->num_buffers.
 		 */
 		__vb2_queue_free(q, allocated_buffers);
+		mutex_unlock(&q->mmap_lock);
 		return ret;
 	}
+	mutex_unlock(&q->mmap_lock);
 
 	/*
 	 * Return the number of successfully allocated buffers
 	 * to the userspace.
 	 */
 	req->count = allocated_buffers;
+	q->waiting_for_buffers = !V4L2_TYPE_IS_OUTPUT(q->type);
 
 	return 0;
 }
@@ -1018,6 +1025,7 @@ static int __create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create
 		memset(q->plane_sizes, 0, sizeof(q->plane_sizes));
 		memset(q->alloc_ctx, 0, sizeof(q->alloc_ctx));
 		q->memory = create->memory;
+		q->waiting_for_buffers = !V4L2_TYPE_IS_OUTPUT(q->type);
 	}
 
 	num_buffers = min(create->count, VIDEO_MAX_FRAME - q->num_buffers);
@@ -1061,6 +1069,7 @@ static int __create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create
 		 */
 	}
 
+	mutex_lock(&q->mmap_lock);
 	q->num_buffers += allocated_buffers;
 
 	if (ret < 0) {
@@ -1069,8 +1078,10 @@ static int __create_bufs(struct vb2_queue *q, struct v4l2_create_buffers *create
 		 * from q->num_buffers.
 		 */
 		__vb2_queue_free(q, allocated_buffers);
+		mutex_unlock(&q->mmap_lock);
 		return -ENOMEM;
 	}
+	mutex_unlock(&q->mmap_lock);
 
 	/*
 	 * Return the number of successfully allocated buffers
@@ -1130,7 +1141,7 @@ EXPORT_SYMBOL_GPL(vb2_plane_vaddr);
  */
 void *vb2_plane_cookie(struct vb2_buffer *vb, unsigned int plane_no)
 {
-	if (plane_no > vb->num_planes || !vb->planes[plane_no].mem_priv)
+	if (plane_no >= vb->num_planes || !vb->planes[plane_no].mem_priv)
 		return NULL;
 
 	return call_ptr_memop(vb, cookie, vb->planes[plane_no].mem_priv);
@@ -1165,13 +1176,10 @@ void vb2_buffer_done(struct vb2_buffer *vb, enum vb2_buffer_state state)
 	if (WARN_ON(vb->state != VB2_BUF_STATE_ACTIVE))
 		return;
 
-	if (!q->start_streaming_called) {
-		if (WARN_ON(state != VB2_BUF_STATE_QUEUED))
-			state = VB2_BUF_STATE_QUEUED;
-	} else if (WARN_ON(state != VB2_BUF_STATE_DONE &&
-			   state != VB2_BUF_STATE_ERROR)) {
-			state = VB2_BUF_STATE_ERROR;
-	}
+	if (WARN_ON(state != VB2_BUF_STATE_DONE &&
+		    state != VB2_BUF_STATE_ERROR &&
+		    state != VB2_BUF_STATE_QUEUED))
+		state = VB2_BUF_STATE_ERROR;
 
 #ifdef CONFIG_VIDEO_ADV_DEBUG
 	/*
@@ -1582,7 +1590,6 @@ static void __enqueue_in_driver(struct vb2_buffer *vb)
 static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 {
 	struct vb2_queue *q = vb->vb2_queue;
-	struct rw_semaphore *mmap_sem;
 	int ret;
 
 	ret = __verify_length(vb, b);
@@ -1619,26 +1626,9 @@ static int __buf_prepare(struct vb2_buffer *vb, const struct v4l2_buffer *b)
 		ret = __qbuf_mmap(vb, b);
 		break;
 	case V4L2_MEMORY_USERPTR:
-		/*
-		 * In case of user pointer buffers vb2 allocators need to get
-		 * direct access to userspace pages. This requires getting
-		 * the mmap semaphore for read access in the current process
-		 * structure. The same semaphore is taken before calling mmap
-		 * operation, while both qbuf/prepare_buf and mmap are called
-		 * by the driver or v4l2 core with the driver's lock held.
-		 * To avoid an AB-BA deadlock (mmap_sem then driver's lock in
-		 * mmap and driver's lock then mmap_sem in qbuf/prepare_buf),
-		 * the videobuf2 core releases the driver's lock, takes
-		 * mmap_sem and then takes the driver's lock again.
-		 */
-		mmap_sem = &current->mm->mmap_sem;
-		call_void_qop(q, wait_prepare, q);
-		down_read(mmap_sem);
-		call_void_qop(q, wait_finish, q);
-
+		down_read(&current->mm->mmap_sem);
 		ret = __qbuf_userptr(vb, b);
-
-		up_read(mmap_sem);
+		up_read(&current->mm->mmap_sem);
 		break;
 	case V4L2_MEMORY_DMABUF:
 		ret = __qbuf_dmabuf(vb, b);
@@ -1762,6 +1752,12 @@ static int vb2_start_streaming(struct vb2_queue *q)
 	q->start_streaming_called = 0;
 
 	dprintk(1, "driver refused to start streaming\n");
+	/*
+	 * If you see this warning, then the driver isn't cleaning up properly
+	 * after a failed start_streaming(). See the start_streaming()
+	 * documentation in videobuf2-core.h for more information how buffers
+	 * should be returned to vb2 in start_streaming().
+	 */
 	if (WARN_ON(atomic_read(&q->owned_by_drv_count))) {
 		unsigned i;
 
@@ -1777,6 +1773,12 @@ static int vb2_start_streaming(struct vb2_queue *q)
 		/* Must be zero now */
 		WARN_ON(atomic_read(&q->owned_by_drv_count));
 	}
+	/*
+	 * If done_list is not empty, then start_streaming() didn't call
+	 * vb2_buffer_done(vb, VB2_BUF_STATE_QUEUED) but STATE_ERROR or
+	 * STATE_DONE.
+	 */
+	WARN_ON(!list_empty(&q->done_list));
 	return ret;
 }
 
@@ -1812,6 +1814,7 @@ static int vb2_internal_qbuf(struct vb2_queue *q, struct v4l2_buffer *b)
 	 */
 	list_add_tail(&vb->queued_entry, &q->queued_list);
 	q->queued_count++;
+	q->waiting_for_buffers = false;
 	vb->state = VB2_BUF_STATE_QUEUED;
 	if (V4L2_TYPE_IS_OUTPUT(q->type)) {
 		/*
@@ -2123,6 +2126,12 @@ static void __vb2_queue_cancel(struct vb2_queue *q)
 	if (q->start_streaming_called)
 		call_void_qop(q, stop_streaming, q);
 
+	/*
+	 * If you see this warning, then the driver isn't cleaning up properly
+	 * in stop_streaming(). See the stop_streaming() documentation in
+	 * videobuf2-core.h for more information how buffers should be returned
+	 * to vb2 in stop_streaming().
+	 */
 	if (WARN_ON(atomic_read(&q->owned_by_drv_count))) {
 		for (i = 0; i < q->num_buffers; ++i)
 			if (q->bufs[i]->state == VB2_BUF_STATE_ACTIVE)
@@ -2272,6 +2281,7 @@ static int vb2_internal_streamoff(struct vb2_queue *q, enum v4l2_buf_type type)
 	 * their normal dequeued state.
 	 */
 	__vb2_queue_cancel(q);
+	q->waiting_for_buffers = !V4L2_TYPE_IS_OUTPUT(q->type);
 
 	dprintk(3, "successful\n");
 	return 0;
@@ -2485,7 +2495,9 @@ int vb2_mmap(struct vb2_queue *q, struct vm_area_struct *vma)
 		return -EINVAL;
 	}
 
+	mutex_lock(&q->mmap_lock);
 	ret = call_memop(vb, mmap, vb->planes[plane].mem_priv, vma);
+	mutex_unlock(&q->mmap_lock);
 	if (ret)
 		return ret;
 
@@ -2504,6 +2516,7 @@ unsigned long vb2_get_unmapped_area(struct vb2_queue *q,
 	unsigned long off = pgoff << PAGE_SHIFT;
 	struct vb2_buffer *vb;
 	unsigned int buffer, plane;
+	void *vaddr;
 	int ret;
 
 	if (q->memory != V4L2_MEMORY_MMAP) {
@@ -2520,7 +2533,8 @@ unsigned long vb2_get_unmapped_area(struct vb2_queue *q,
 
 	vb = q->bufs[buffer];
 
-	return (unsigned long)vb2_plane_vaddr(vb, plane);
+	vaddr = vb2_plane_vaddr(vb, plane);
+	return vaddr ? (unsigned long)vaddr : -EINVAL;
 }
 EXPORT_SYMBOL_GPL(vb2_get_unmapped_area);
 #endif
@@ -2590,10 +2604,17 @@ unsigned int vb2_poll(struct vb2_queue *q, struct file *file, poll_table *wait)
 	}
 
 	/*
-	 * There is nothing to wait for if no buffer has been queued and the
-	 * queue isn't streaming, or if the error flag is set.
+	 * There is nothing to wait for if the queue isn't streaming, or if the
+	 * error flag is set.
 	 */
-	if ((list_empty(&q->queued_list) && !vb2_is_streaming(q)) || q->error)
+	if (!vb2_is_streaming(q) || q->error)
+		return res | POLLERR;
+	/*
+	 * For compatibility with vb1: if QBUF hasn't been called yet, then
+	 * return POLLERR as well. This only affects capture queues, output
+	 * queues will always initialize waiting_for_buffers to false.
+	 */
+	if (q->waiting_for_buffers)
 		return res | POLLERR;
 
 	/*
@@ -2660,6 +2681,7 @@ int vb2_queue_init(struct vb2_queue *q)
 	INIT_LIST_HEAD(&q->queued_list);
 	INIT_LIST_HEAD(&q->done_list);
 	spin_lock_init(&q->done_lock);
+	mutex_init(&q->mmap_lock);
 	init_waitqueue_head(&q->done_wq);
 
 	if (q->buf_struct_size == 0)
@@ -2681,7 +2703,9 @@ void vb2_queue_release(struct vb2_queue *q)
 {
 	__vb2_cleanup_fileio(q);
 	__vb2_queue_cancel(q);
+	mutex_lock(&q->mmap_lock);
 	__vb2_queue_free(q, q->num_buffers);
+	mutex_unlock(&q->mmap_lock);
 }
 EXPORT_SYMBOL_GPL(vb2_queue_release);
 
@@ -2959,6 +2983,12 @@ static size_t __vb2_perform_fileio(struct vb2_queue *q, char __user *data, size_
 		buf->queued = 0;
 		buf->size = read ? vb2_get_plane_payload(q->bufs[index], 0)
 				 : vb2_plane_size(q->bufs[index], 0);
+		/* Compensate for data_offset on read in the multiplanar case. */
+		if (is_multiplanar && read &&
+		    fileio->b.m.planes[0].data_offset < buf->size) {
+			buf->pos = fileio->b.m.planes[0].data_offset;
+			buf->size -= buf->pos;
+		}
 	} else {
 		buf = &fileio->bufs[index];
 	}
@@ -3346,15 +3376,8 @@ EXPORT_SYMBOL_GPL(vb2_ioctl_expbuf);
 int vb2_fop_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct video_device *vdev = video_devdata(file);
-	struct mutex *lock = vdev->queue->lock ? vdev->queue->lock : vdev->lock;
-	int err;
 
-	if (lock && mutex_lock_interruptible(lock))
-		return -ERESTARTSYS;
-	err = vb2_mmap(vdev->queue, vma);
-	if (lock)
-		mutex_unlock(lock);
-	return err;
+	return vb2_mmap(vdev->queue, vma);
 }
 EXPORT_SYMBOL_GPL(vb2_fop_mmap);
 
@@ -3473,15 +3496,8 @@ unsigned long vb2_fop_get_unmapped_area(struct file *file, unsigned long addr,
 		unsigned long len, unsigned long pgoff, unsigned long flags)
 {
 	struct video_device *vdev = video_devdata(file);
-	struct mutex *lock = vdev->queue->lock ? vdev->queue->lock : vdev->lock;
-	int ret;
 
-	if (lock && mutex_lock_interruptible(lock))
-		return -ERESTARTSYS;
-	ret = vb2_get_unmapped_area(vdev->queue, addr, len, pgoff, flags);
-	if (lock)
-		mutex_unlock(lock);
-	return ret;
+	return vb2_get_unmapped_area(vdev->queue, addr, len, pgoff, flags);
 }
 EXPORT_SYMBOL_GPL(vb2_fop_get_unmapped_area);
 #endif
diff --git a/drivers/media/v4l2-core/videobuf2-dma-sg.c b/drivers/media/v4l2-core/videobuf2-dma-sg.c
index adefc31bb853..9b163a440f89 100644
--- a/drivers/media/v4l2-core/videobuf2-dma-sg.c
+++ b/drivers/media/v4l2-core/videobuf2-dma-sg.c
@@ -113,7 +113,7 @@ static void *vb2_dma_sg_alloc(void *alloc_ctx, unsigned long size, gfp_t gfp_fla
 		goto fail_pages_alloc;
 
 	ret = sg_alloc_table_from_pages(&buf->sg_table, buf->pages,
-			buf->num_pages, 0, size, gfp_flags);
+			buf->num_pages, 0, size, GFP_KERNEL);
 	if (ret)
 		goto fail_table_alloc;
 
diff --git a/drivers/memory/Kconfig b/drivers/memory/Kconfig
index fab81a143bd7..6d91c27fd4c8 100644
--- a/drivers/memory/Kconfig
+++ b/drivers/memory/Kconfig
@@ -7,6 +7,16 @@ menuconfig MEMORY
 
 if MEMORY
 
+config ATMEL_SDRAMC
+	bool "Atmel (Multi-port DDR-)SDRAM Controller"
+	default y
+	depends on ARCH_AT91 && OF
+	help
+	  This driver is for Atmel SDRAM Controller or Atmel Multi-port
+	  DDR-SDRAM Controller available on Atmel AT91SAM9 and SAMA5 SoCs.
+	  Starting with the at91sam9g45, this controller supports SDR, DDR and
+	  LP-DDR memories.
+
 config TI_AEMIF
 	tristate "Texas Instruments AEMIF driver"
 	depends on (ARCH_DAVINCI || ARCH_KEYSTONE) && OF
diff --git a/drivers/memory/Makefile b/drivers/memory/Makefile
index 4055c47f45ab..c32d31981be3 100644
--- a/drivers/memory/Makefile
+++ b/drivers/memory/Makefile
@@ -5,6 +5,7 @@
 ifeq ($(CONFIG_DDR),y)
 obj-$(CONFIG_OF)		+= of_memory.o
 endif
+obj-$(CONFIG_ATMEL_SDRAMC)	+= atmel-sdramc.o
 obj-$(CONFIG_TI_AEMIF)		+= ti-aemif.o
 obj-$(CONFIG_TI_EMIF)		+= emif.o
 obj-$(CONFIG_FSL_CORENET_CF)	+= fsl-corenet-cf.o
diff --git a/drivers/memory/atmel-sdramc.c b/drivers/memory/atmel-sdramc.c
new file mode 100644
index 000000000000..fed04e8efe75
--- /dev/null
+++ b/drivers/memory/atmel-sdramc.c
@@ -0,0 +1,98 @@
+/*
+ * Atmel (Multi-port DDR-)SDRAM Controller driver
+ *
+ * Copyright (C) 2014 Atmel
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+struct at91_ramc_caps {
+	bool has_ddrck;
+	bool has_mpddr_clk;
+};
+
+static const struct at91_ramc_caps at91rm9200_caps = { };
+
+static const struct at91_ramc_caps at91sam9g45_caps = {
+	.has_ddrck = 1,
+	.has_mpddr_clk = 0,
+};
+
+static const struct at91_ramc_caps sama5d3_caps = {
+	.has_ddrck = 1,
+	.has_mpddr_clk = 1,
+};
+
+static const struct of_device_id atmel_ramc_of_match[] = {
+	{ .compatible = "atmel,at91rm9200-sdramc", .data = &at91rm9200_caps, },
+	{ .compatible = "atmel,at91sam9260-sdramc", .data = &at91rm9200_caps, },
+	{ .compatible = "atmel,at91sam9g45-ddramc", .data = &at91sam9g45_caps, },
+	{ .compatible = "atmel,sama5d3-ddramc", .data = &sama5d3_caps, },
+	{},
+};
+MODULE_DEVICE_TABLE(of, atmel_ramc_of_match);
+
+static int atmel_ramc_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	const struct at91_ramc_caps *caps;
+	struct clk *clk;
+
+	match = of_match_device(atmel_ramc_of_match, &pdev->dev);
+	caps = match->data;
+
+	if (caps->has_ddrck) {
+		clk = devm_clk_get(&pdev->dev, "ddrck");
+		if (IS_ERR(clk))
+			return PTR_ERR(clk);
+		clk_prepare_enable(clk);
+	}
+
+	if (caps->has_mpddr_clk) {
+		clk = devm_clk_get(&pdev->dev, "mpddr");
+		if (IS_ERR(clk)) {
+			pr_err("AT91 RAMC: couldn't get mpddr clock\n");
+			return PTR_ERR(clk);
+		}
+		clk_prepare_enable(clk);
+	}
+
+	return 0;
+}
+
+static struct platform_driver atmel_ramc_driver = {
+	.probe		= atmel_ramc_probe,
+	.driver		= {
+		.name	= "atmel-ramc",
+		.owner	= THIS_MODULE,
+		.of_match_table = atmel_ramc_of_match,
+	},
+};
+
+static int __init atmel_ramc_init(void)
+{
+	return platform_driver_register(&atmel_ramc_driver);
+}
+module_init(atmel_ramc_init);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Alexandre Belloni <alexandre.belloni@free-electrons.com>");
+MODULE_DESCRIPTION("Atmel (Multi-port DDR-)SDRAM Controller");
diff --git a/drivers/memstick/host/r592.c b/drivers/memstick/host/r592.c
index 31727bf285d0..e2a4f5f415b2 100644
--- a/drivers/memstick/host/r592.c
+++ b/drivers/memstick/host/r592.c
@@ -188,6 +188,7 @@ static void r592_host_reset(struct r592_device *dev)
 	r592_set_mode(dev, dev->parallel_mode);
 }
 
+#ifdef CONFIG_PM_SLEEP
 /* Disable all hardware interrupts */
 static void r592_clear_interrupts(struct r592_device *dev)
 {
@@ -195,6 +196,7 @@ static void r592_clear_interrupts(struct r592_device *dev)
 	r592_clear_reg_mask(dev, R592_REG_MSC, IRQ_ALL_ACK_MASK);
 	r592_clear_reg_mask(dev, R592_REG_MSC, IRQ_ALL_EN_MASK);
 }
+#endif
 
 /* Tests if there is an CRC error */
 static int r592_test_io_error(struct r592_device *dev)
diff --git a/drivers/message/fusion/Kconfig b/drivers/message/fusion/Kconfig
index a34a11d2fef2..63ca9841db10 100644
--- a/drivers/message/fusion/Kconfig
+++ b/drivers/message/fusion/Kconfig
@@ -29,7 +29,7 @@ config FUSION_SPI
 config FUSION_FC
 	tristate "Fusion MPT ScsiHost drivers for FC"
 	depends on PCI && SCSI
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	---help---
 	  SCSI HOST support for a Fiber Channel host adapters.
 
diff --git a/drivers/message/fusion/mptbase.c b/drivers/message/fusion/mptbase.c
index a896d948b79e..187f83629f7e 100644
--- a/drivers/message/fusion/mptbase.c
+++ b/drivers/message/fusion/mptbase.c
@@ -1400,7 +1400,6 @@ mpt_verify_adapter(int iocid, MPT_ADAPTER **iocpp)
  *	@vendor: pci vendor id
  *	@device: pci device id
  *	@revision: pci revision id
- *	@prod_name: string returned
  *
  *	Returns product string displayed when driver loads,
  *	in /proc/mpt/summary and /sysfs/class/scsi_host/host<X>/version_product
@@ -3172,12 +3171,7 @@ GetIocFacts(MPT_ADAPTER *ioc, int sleepFlag, int reason)
 			facts->FWImageSize = le32_to_cpu(facts->FWImageSize);
 		}
 
-		sz = facts->FWImageSize;
-		if ( sz & 0x01 )
-			sz += 1;
-		if ( sz & 0x02 )
-			sz += 2;
-		facts->FWImageSize = sz;
+		facts->FWImageSize = ALIGN(facts->FWImageSize, 4);
 
 		if (!facts->RequestFrameSize) {
 			/*  Something is wrong!  */
diff --git a/drivers/message/fusion/mptctl.c b/drivers/message/fusion/mptctl.c
index b0a892a2bf1b..70bb7530b22c 100644
--- a/drivers/message/fusion/mptctl.c
+++ b/drivers/message/fusion/mptctl.c
@@ -1741,12 +1741,7 @@ mptctl_replace_fw (unsigned long arg)
 
 	/* Allocate memory for the new FW image
 	 */
-	newFwSize = karg.newImageSize;
-
-	if (newFwSize & 0x01)
-		newFwSize += 1;
-	if (newFwSize & 0x02)
-		newFwSize += 2;
+	newFwSize = ALIGN(karg.newImageSize, 4);
 
 	mpt_alloc_fw_memory(ioc, newFwSize);
 	if (ioc->cached_fw == NULL)
diff --git a/drivers/message/fusion/mptspi.c b/drivers/message/fusion/mptspi.c
index 787933d43d32..613231c16194 100644
--- a/drivers/message/fusion/mptspi.c
+++ b/drivers/message/fusion/mptspi.c
@@ -1419,6 +1419,11 @@ mptspi_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		goto out_mptspi_probe;
         }
 
+	/* VMWare emulation doesn't properly implement WRITE_SAME
+	 */
+	if (pdev->subsystem_vendor == 0x15AD)
+		sh->no_write_same = 1;
+
 	spin_lock_irqsave(&ioc->FreeQlock, flags);
 
 	/* Attach the SCSI Host to the IOC structure
diff --git a/drivers/mfd/88pm860x-i2c.c b/drivers/mfd/88pm860x-i2c.c
index a93b4d0134a2..84e313107233 100644
--- a/drivers/mfd/88pm860x-i2c.c
+++ b/drivers/mfd/88pm860x-i2c.c
@@ -122,7 +122,7 @@ static int read_device(struct i2c_client *i2c, int reg,
 static int write_device(struct i2c_client *i2c, int reg,
 			int bytes, void *src)
 {
-	unsigned char buf[bytes + 1];
+	unsigned char buf[2];
 	struct i2c_adapter *adap = i2c->adapter;
 	struct i2c_msg msg;
 	int ret;
@@ -140,26 +140,6 @@ static int write_device(struct i2c_client *i2c, int reg,
 	return 0;
 }
 
-int pm860x_page_reg_read(struct i2c_client *i2c, int reg)
-{
-	unsigned char zero = 0;
-	unsigned char data;
-	int ret;
-
-	i2c_lock_adapter(i2c->adapter);
-	read_device(i2c, 0xFA, 0, &zero);
-	read_device(i2c, 0xFB, 0, &zero);
-	read_device(i2c, 0xFF, 0, &zero);
-	ret = read_device(i2c, reg, 1, &data);
-	if (ret >= 0)
-		ret = (int)data;
-	read_device(i2c, 0xFE, 0, &zero);
-	read_device(i2c, 0xFC, 0, &zero);
-	i2c_unlock_adapter(i2c->adapter);
-	return ret;
-}
-EXPORT_SYMBOL(pm860x_page_reg_read);
-
 int pm860x_page_reg_write(struct i2c_client *i2c, int reg,
 			  unsigned char data)
 {
@@ -195,47 +175,3 @@ int pm860x_page_bulk_read(struct i2c_client *i2c, int reg,
 	return ret;
 }
 EXPORT_SYMBOL(pm860x_page_bulk_read);
-
-int pm860x_page_bulk_write(struct i2c_client *i2c, int reg,
-			   int count, unsigned char *buf)
-{
-	unsigned char zero = 0;
-	int ret;
-
-	i2c_lock_adapter(i2c->adapter);
-	read_device(i2c, 0xFA, 0, &zero);
-	read_device(i2c, 0xFB, 0, &zero);
-	read_device(i2c, 0xFF, 0, &zero);
-	ret = write_device(i2c, reg, count, buf);
-	read_device(i2c, 0xFE, 0, &zero);
-	read_device(i2c, 0xFC, 0, &zero);
-	i2c_unlock_adapter(i2c->adapter);
-	i2c_unlock_adapter(i2c->adapter);
-	return ret;
-}
-EXPORT_SYMBOL(pm860x_page_bulk_write);
-
-int pm860x_page_set_bits(struct i2c_client *i2c, int reg,
-			 unsigned char mask, unsigned char data)
-{
-	unsigned char zero;
-	unsigned char value;
-	int ret;
-
-	i2c_lock_adapter(i2c->adapter);
-	read_device(i2c, 0xFA, 0, &zero);
-	read_device(i2c, 0xFB, 0, &zero);
-	read_device(i2c, 0xFF, 0, &zero);
-	ret = read_device(i2c, reg, 1, &value);
-	if (ret < 0)
-		goto out;
-	value &= ~mask;
-	value |= data;
-	ret = write_device(i2c, reg, 1, &value);
-out:
-	read_device(i2c, 0xFE, 0, &zero);
-	read_device(i2c, 0xFC, 0, &zero);
-	i2c_unlock_adapter(i2c->adapter);
-	return ret;
-}
-EXPORT_SYMBOL(pm860x_page_set_bits);
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index de5abf244746..1456ea70bbc7 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -210,6 +210,19 @@ config MFD_MC13XXX_I2C
 	help
 	  Select this if your MC13xxx is connected via an I2C bus.
 
+config MFD_HI6421_PMIC
+	tristate "HiSilicon Hi6421 PMU/Codec IC"
+	depends on OF
+	select MFD_CORE
+	select REGMAP_MMIO
+	help
+	  Add support for HiSilicon Hi6421 PMIC. Hi6421 includes multi-
+	  functions, such as regulators, RTC, codec, Coulomb counter, etc.
+	  This driver includes core APIs _only_. You have to select
+	  individul components like voltage regulators under corresponding
+	  menus in order to enable them.
+	  We communicate with the Hi6421 via memory-mapped I/O.
+
 config HTC_EGPIO
 	bool "HTC EGPIO support"
 	depends on GPIOLIB && ARM
@@ -454,6 +467,21 @@ config MFD_MAX8998
 	  additional drivers must be enabled in order to use the functionality
 	  of the device.
 
+config MFD_MENF21BMC
+	tristate "MEN 14F021P00 Board Management Controller Support"
+	depends on I2C
+	select MFD_CORE
+	help
+	  Say yes here to add support for the MEN 14F021P00 BMC
+	  which is a Board Management Controller connected to the I2C bus.
+	  The device supports multiple sub-devices like LED, HWMON and WDT.
+	  This driver provides common support for accessing the devices;
+	  additional drivers must be enabled in order to use the
+	  functionality of the BMC device.
+
+	  This driver can also be built as a module. If so the module
+	  will be called menf21bmc.
+
 config EZX_PCAP
 	bool "Motorola EZXPCAP Support"
 	depends on SPI_MASTER
@@ -539,6 +567,21 @@ config MFD_PM8921_CORE
 	  Say M here if you want to include support for PM8921 chip as a module.
 	  This will build a module called "pm8921-core".
 
+config MFD_SPMI_PMIC
+	tristate "Qualcomm SPMI PMICs"
+	depends on ARCH_QCOM || COMPILE_TEST
+	depends on OF
+	depends on SPMI
+	select REGMAP_SPMI
+	help
+	  This enables support for the Qualcomm SPMI PMICs.
+	  These PMICs are currently used with the Snapdragon 800 series of
+	  SoCs.  Note, that this will only be useful paired with descriptions
+	  of the independent functions as children nodes in the device tree.
+
+	  Say M here if you want to include support for the SPMI PMIC
+	  series as a module.  The module will be called "qcom-spmi-pmic".
+
 config MFD_RDC321X
 	tristate "RDC R-321x southbridge"
 	select MFD_CORE
@@ -582,6 +625,30 @@ config MFD_RC5T583
 	  Additional drivers must be enabled in order to use the
 	  different functionality of the device.
 
+config MFD_RK808
+	tristate "Rockchip RK808 Power Management chip"
+	depends on I2C && OF
+	select MFD_CORE
+	select REGMAP_I2C
+	select REGMAP_IRQ
+	help
+	  If you say yes here you get support for the RK808
+	  Power Management chips.
+	  This driver provides common support for accessing the device
+	  through I2C interface. The device supports multiple sub-devices
+	  including interrupts, RTC, LDO & DCDC regulators, and onkey.
+
+config MFD_RN5T618
+	tristate "Ricoh RN5T5618 PMIC"
+	depends on I2C
+	select MFD_CORE
+	select REGMAP_I2C
+	help
+	  Say yes here to add support for the Ricoh RN5T618 PMIC. This
+	  driver provides common support for accessing the device,
+	  additional drivers must be enabled in order to use the
+	  functionality of the device.
+
 config MFD_SEC_CORE
 	bool "SAMSUNG Electronics PMIC Series Support"
 	depends on I2C=y
@@ -1228,11 +1295,11 @@ config MFD_WM8350_I2C
 	  selected to enable support for the functionality of the chip.
 
 config MFD_WM8994
-	bool "Wolfson Microelectronics WM8994"
+	tristate "Wolfson Microelectronics WM8994"
 	select MFD_CORE
 	select REGMAP_I2C
 	select REGMAP_IRQ
-	depends on I2C=y
+	depends on I2C
 	help
 	  The WM8994 is a highly integrated hi-fi CODEC designed for
 	  smartphone applicatiosn.  As well as audio functionality it
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index f00148782d9b..8bd54b1253af 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -153,6 +153,7 @@ obj-$(CONFIG_MFD_SI476X_CORE)	+= si476x-core.o
 obj-$(CONFIG_MFD_CS5535)	+= cs5535-mfd.o
 obj-$(CONFIG_MFD_OMAP_USB_HOST)	+= omap-usb-host.o omap-usb-tll.o
 obj-$(CONFIG_MFD_PM8921_CORE) 	+= pm8921-core.o ssbi.o
+obj-$(CONFIG_MFD_SPMI_PMIC)	+= qcom-spmi-pmic.o
 obj-$(CONFIG_TPS65911_COMPARATOR)	+= tps65911-comparator.o
 obj-$(CONFIG_MFD_TPS65090)	+= tps65090.o
 obj-$(CONFIG_MFD_AAT2870_CORE)	+= aat2870-core.o
@@ -160,6 +161,8 @@ obj-$(CONFIG_MFD_INTEL_MSIC)	+= intel_msic.o
 obj-$(CONFIG_MFD_PALMAS)	+= palmas.o
 obj-$(CONFIG_MFD_VIPERBOARD)    += viperboard.o
 obj-$(CONFIG_MFD_RC5T583)	+= rc5t583.o rc5t583-irq.o
+obj-$(CONFIG_MFD_RK808)		+= rk808.o
+obj-$(CONFIG_MFD_RN5T618)	+= rn5t618.o
 obj-$(CONFIG_MFD_SEC_CORE)	+= sec-core.o sec-irq.o
 obj-$(CONFIG_MFD_SYSCON)	+= syscon.o
 obj-$(CONFIG_MFD_LM3533)	+= lm3533-core.o lm3533-ctrlbank.o
@@ -169,6 +172,8 @@ obj-$(CONFIG_MFD_AS3711)	+= as3711.o
 obj-$(CONFIG_MFD_AS3722)	+= as3722.o
 obj-$(CONFIG_MFD_STW481X)	+= stw481x.o
 obj-$(CONFIG_MFD_IPAQ_MICRO)	+= ipaq-micro.o
+obj-$(CONFIG_MFD_MENF21BMC)	+= menf21bmc.o
+obj-$(CONFIG_MFD_HI6421_PMIC)	+= hi6421-pmic-core.o
 
 intel-soc-pmic-objs		:= intel_soc_pmic_core.o intel_soc_pmic_crc.o
 obj-$(CONFIG_INTEL_SOC_PMIC)	+= intel-soc-pmic.o
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index ce48aa72bb42..bde2fc072410 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -1754,7 +1754,7 @@ static int ab8500_probe(struct platform_device *pdev)
 	if (ret)
 		return ret;
 
-#if CONFIG_DEBUG_FS
+#ifdef CONFIG_DEBUG_FS
 	/* Pass to debugfs */
 	ab8500_debug_resources[0].start = ab8500->irq;
 	ab8500_debug_resources[0].end = ab8500->irq;
diff --git a/drivers/mfd/arizona-core.c b/drivers/mfd/arizona-core.c
index 10a0cb90619a..bce7c0784b6b 100644
--- a/drivers/mfd/arizona-core.c
+++ b/drivers/mfd/arizona-core.c
@@ -393,18 +393,6 @@ static int arizona_runtime_resume(struct device *dev)
 		break;
 	}
 
-	switch (arizona->type) {
-	case WM5102:
-		ret = wm5102_patch(arizona);
-		if (ret != 0) {
-			dev_err(arizona->dev, "Failed to apply patch: %d\n",
-				ret);
-			goto err;
-		}
-	default:
-		break;
-	}
-
 	ret = regcache_sync(arizona->regmap);
 	if (ret != 0) {
 		dev_err(arizona->dev, "Failed to restore register cache\n");
@@ -534,7 +522,11 @@ EXPORT_SYMBOL_GPL(arizona_of_get_named_gpio);
 static int arizona_of_get_core_pdata(struct arizona *arizona)
 {
 	struct arizona_pdata *pdata = &arizona->pdata;
+	struct property *prop;
+	const __be32 *cur;
+	u32 val;
 	int ret, i;
+	int count = 0;
 
 	pdata->reset = arizona_of_get_named_gpio(arizona, "wlf,reset", true);
 
@@ -560,6 +552,15 @@ static int arizona_of_get_core_pdata(struct arizona *arizona)
 			ret);
 	}
 
+	of_property_for_each_u32(arizona->dev->of_node, "wlf,inmode", prop,
+				 cur, val) {
+		if (count == ARRAY_SIZE(arizona->pdata.inmode))
+			break;
+
+		arizona->pdata.inmode[count] = val;
+		count++;
+	}
+
 	return 0;
 }
 
@@ -784,7 +785,8 @@ int arizona_dev_init(struct arizona *arizona)
 	/* Ensure device startup is complete */
 	switch (arizona->type) {
 	case WM5102:
-		ret = regmap_read(arizona->regmap, 0x19, &val);
+		ret = regmap_read(arizona->regmap,
+				  ARIZONA_WRITE_SEQUENCER_CTRL_3, &val);
 		if (ret != 0)
 			dev_err(dev,
 				"Failed to check write sequencer state: %d\n",
@@ -945,6 +947,7 @@ int arizona_dev_init(struct arizona *arizona)
 		regmap_update_bits(arizona->regmap,
 				   ARIZONA_MIC_BIAS_CTRL_1 + i,
 				   ARIZONA_MICB1_LVL_MASK |
+				   ARIZONA_MICB1_EXT_CAP |
 				   ARIZONA_MICB1_DISCH |
 				   ARIZONA_MICB1_BYPASS |
 				   ARIZONA_MICB1_RATE, val);
diff --git a/drivers/mfd/arizona-irq.c b/drivers/mfd/arizona-irq.c
index d420dbc0e2b0..3a3fe7cc6d61 100644
--- a/drivers/mfd/arizona-irq.c
+++ b/drivers/mfd/arizona-irq.c
@@ -152,10 +152,18 @@ static void arizona_irq_disable(struct irq_data *data)
 {
 }
 
+static int arizona_irq_set_wake(struct irq_data *data, unsigned int on)
+{
+	struct arizona *arizona = irq_data_get_irq_chip_data(data);
+
+	return irq_set_irq_wake(arizona->irq, on);
+}
+
 static struct irq_chip arizona_irq_chip = {
 	.name			= "arizona",
 	.irq_disable		= arizona_irq_disable,
 	.irq_enable		= arizona_irq_enable,
+	.irq_set_wake		= arizona_irq_set_wake,
 };
 
 static int arizona_irq_map(struct irq_domain *h, unsigned int virq,
@@ -164,7 +172,7 @@ static int arizona_irq_map(struct irq_domain *h, unsigned int virq,
 	struct regmap_irq_chip_data *data = h->host_data;
 
 	irq_set_chip_data(virq, data);
-	irq_set_chip_and_handler(virq, &arizona_irq_chip, handle_edge_irq);
+	irq_set_chip_and_handler(virq, &arizona_irq_chip, handle_simple_irq);
 	irq_set_nested_thread(virq, 1);
 
 	/* ARM needs us to explicitly flag the IRQ as valid
@@ -282,7 +290,7 @@ int arizona_irq_init(struct arizona *arizona)
 
 	ret = regmap_add_irq_chip(arizona->regmap,
 				  irq_create_mapping(arizona->virq, 0),
-				  IRQF_ONESHOT, -1, aod,
+				  IRQF_ONESHOT, 0, aod,
 				  &arizona->aod_irq_chip);
 	if (ret != 0) {
 		dev_err(arizona->dev, "Failed to add AOD IRQs: %d\n", ret);
@@ -291,7 +299,7 @@ int arizona_irq_init(struct arizona *arizona)
 
 	ret = regmap_add_irq_chip(arizona->regmap,
 				  irq_create_mapping(arizona->virq, 1),
-				  IRQF_ONESHOT, -1, irq,
+				  IRQF_ONESHOT, 0, irq,
 				  &arizona->irq_chip);
 	if (ret != 0) {
 		dev_err(arizona->dev, "Failed to add main IRQs: %d\n", ret);
diff --git a/drivers/mfd/asic3.c b/drivers/mfd/asic3.c
index 9fc4186d4132..977bd3a3eed0 100644
--- a/drivers/mfd/asic3.c
+++ b/drivers/mfd/asic3.c
@@ -605,7 +605,8 @@ static int asic3_gpio_remove(struct platform_device *pdev)
 {
 	struct asic3 *asic = platform_get_drvdata(pdev);
 
-	return gpiochip_remove(&asic->gpio);
+	gpiochip_remove(&asic->gpio);
+	return 0;
 }
 
 static void asic3_clk_enable(struct asic3 *asic, struct asic3_clk *clk)
diff --git a/drivers/mfd/axp20x.c b/drivers/mfd/axp20x.c
index dee653989e3a..6231adbb295d 100644
--- a/drivers/mfd/axp20x.c
+++ b/drivers/mfd/axp20x.c
@@ -140,15 +140,6 @@ static const struct regmap_irq_chip axp20x_regmap_irq_chip = {
 	.init_ack_masked	= true,
 };
 
-static const char * const axp20x_supplies[] = {
-	"acin",
-	"vin2",
-	"vin3",
-	"ldo24in",
-	"ldo3in",
-	"ldo5in",
-};
-
 static struct mfd_cell axp20x_cells[] = {
 	{
 		.name			= "axp20x-pek",
@@ -156,8 +147,6 @@ static struct mfd_cell axp20x_cells[] = {
 		.resources		= axp20x_pek_resources,
 	}, {
 		.name			= "axp20x-regulator",
-		.parent_supplies	= axp20x_supplies,
-		.num_parent_supplies	= ARRAY_SIZE(axp20x_supplies),
 	},
 };
 
diff --git a/drivers/mfd/cros_ec.c b/drivers/mfd/cros_ec.c
index 4873f9c50452..fc0c81ef04ff 100644
--- a/drivers/mfd/cros_ec.c
+++ b/drivers/mfd/cros_ec.c
@@ -23,6 +23,9 @@
 #include <linux/mfd/core.h>
 #include <linux/mfd/cros_ec.h>
 #include <linux/mfd/cros_ec_commands.h>
+#include <linux/delay.h>
+
+#define EC_COMMAND_RETRIES	50
 
 int cros_ec_prepare_tx(struct cros_ec_device *ec_dev,
 		       struct cros_ec_command *msg)
@@ -62,6 +65,49 @@ int cros_ec_check_result(struct cros_ec_device *ec_dev,
 }
 EXPORT_SYMBOL(cros_ec_check_result);
 
+int cros_ec_cmd_xfer(struct cros_ec_device *ec_dev,
+		     struct cros_ec_command *msg)
+{
+	int ret;
+
+	mutex_lock(&ec_dev->lock);
+	ret = ec_dev->cmd_xfer(ec_dev, msg);
+	if (msg->result == EC_RES_IN_PROGRESS) {
+		int i;
+		struct cros_ec_command status_msg;
+		struct ec_response_get_comms_status status;
+
+		status_msg.version = 0;
+		status_msg.command = EC_CMD_GET_COMMS_STATUS;
+		status_msg.outdata = NULL;
+		status_msg.outsize = 0;
+		status_msg.indata = (uint8_t *)&status;
+		status_msg.insize = sizeof(status);
+
+		/*
+		 * Query the EC's status until it's no longer busy or
+		 * we encounter an error.
+		 */
+		for (i = 0; i < EC_COMMAND_RETRIES; i++) {
+			usleep_range(10000, 11000);
+
+			ret = ec_dev->cmd_xfer(ec_dev, &status_msg);
+			if (ret < 0)
+				break;
+
+			msg->result = status_msg.result;
+			if (status_msg.result != EC_RES_SUCCESS)
+				break;
+			if (!(status.flags & EC_COMMS_STATUS_PROCESSING))
+				break;
+		}
+	}
+	mutex_unlock(&ec_dev->lock);
+
+	return ret;
+}
+EXPORT_SYMBOL(cros_ec_cmd_xfer);
+
 static const struct mfd_cell cros_devs[] = {
 	{
 		.name = "cros-ec-keyb",
@@ -91,6 +137,8 @@ int cros_ec_register(struct cros_ec_device *ec_dev)
 			return -ENOMEM;
 	}
 
+	mutex_init(&ec_dev->lock);
+
 	err = mfd_add_devices(dev, 0, cros_devs,
 			      ARRAY_SIZE(cros_devs),
 			      NULL, ec_dev->irq, NULL);
diff --git a/drivers/mfd/cros_ec_spi.c b/drivers/mfd/cros_ec_spi.c
index 588c700af39c..bf6e08e8013e 100644
--- a/drivers/mfd/cros_ec_spi.c
+++ b/drivers/mfd/cros_ec_spi.c
@@ -65,6 +65,12 @@
   */
 #define EC_SPI_RECOVERY_TIME_NS	(200 * 1000)
 
+/*
+ * The EC is unresponsive for a time after a reboot command.  Add a
+ * simple delay to make sure that the bus stays locked.
+ */
+#define EC_REBOOT_DELAY_MS	50
+
 /**
  * struct cros_ec_spi - information about a SPI-connected EC
  *
@@ -73,13 +79,11 @@
  *	if no record
  * @end_of_msg_delay: used to set the delay_usecs on the spi_transfer that
  *      is sent when we want to turn off CS at the end of a transaction.
- * @lock: mutex to ensure only one user of cros_ec_cmd_xfer_spi at a time
  */
 struct cros_ec_spi {
 	struct spi_device *spi;
 	s64 last_transfer_ns;
 	unsigned int end_of_msg_delay;
-	struct mutex lock;
 };
 
 static void debug_packet(struct device *dev, const char *name, u8 *ptr,
@@ -226,13 +230,6 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 	int sum;
 	int ret = 0, final_ret;
 
-	/*
-	 * We have the shared ec_dev buffer plus we do lots of separate spi_sync
-	 * calls, so we need to make sure only one person is using this at a
-	 * time.
-	 */
-	mutex_lock(&ec_spi->lock);
-
 	len = cros_ec_prepare_tx(ec_dev, ec_msg);
 	dev_dbg(ec_dev->dev, "prepared, len=%d\n", len);
 
@@ -318,7 +315,9 @@ static int cros_ec_cmd_xfer_spi(struct cros_ec_device *ec_dev,
 
 	ret = len;
 exit:
-	mutex_unlock(&ec_spi->lock);
+	if (ec_msg->command == EC_CMD_REBOOT_EC)
+		msleep(EC_REBOOT_DELAY_MS);
+
 	return ret;
 }
 
@@ -350,7 +349,6 @@ static int cros_ec_spi_probe(struct spi_device *spi)
 	if (ec_spi == NULL)
 		return -ENOMEM;
 	ec_spi->spi = spi;
-	mutex_init(&ec_spi->lock);
 	ec_dev = devm_kzalloc(dev, sizeof(*ec_dev), GFP_KERNEL);
 	if (!ec_dev)
 		return -ENOMEM;
diff --git a/drivers/mfd/da9052-core.c b/drivers/mfd/da9052-core.c
index e8af816d73a9..52a0c2f6264f 100644
--- a/drivers/mfd/da9052-core.c
+++ b/drivers/mfd/da9052-core.c
@@ -522,7 +522,7 @@ static const struct mfd_cell da9052_subdev_info[] = {
 	},
 };
 
-struct regmap_config da9052_regmap_config = {
+const struct regmap_config da9052_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
 
diff --git a/drivers/mfd/da9052-i2c.c b/drivers/mfd/da9052-i2c.c
index 6da8ec8ff800..ec39287a245b 100644
--- a/drivers/mfd/da9052-i2c.c
+++ b/drivers/mfd/da9052-i2c.c
@@ -140,13 +140,6 @@ static int da9052_i2c_probe(struct i2c_client *client,
 	if (!da9052)
 		return -ENOMEM;
 
-	if (!i2c_check_functionality(client->adapter,
-				     I2C_FUNC_SMBUS_BYTE_DATA)) {
-		dev_info(&client->dev, "Error in %s:i2c_check_functionality\n",
-			 __func__);
-		return  -ENODEV;
-	}
-
 	da9052->dev = &client->dev;
 	da9052->chip_irq = client->irq;
 	da9052->fix_io = da9052_i2c_fix;
diff --git a/drivers/mfd/da9052-spi.c b/drivers/mfd/da9052-spi.c
index 17666b40b70c..45ae0b7d13ef 100644
--- a/drivers/mfd/da9052-spi.c
+++ b/drivers/mfd/da9052-spi.c
@@ -23,6 +23,7 @@
 
 static int da9052_spi_probe(struct spi_device *spi)
 {
+	struct regmap_config config;
 	int ret;
 	const struct spi_device_id *id = spi_get_device_id(spi);
 	struct da9052 *da9052;
@@ -40,10 +41,10 @@ static int da9052_spi_probe(struct spi_device *spi)
 
 	spi_set_drvdata(spi, da9052);
 
-	da9052_regmap_config.read_flag_mask = 1;
-	da9052_regmap_config.write_flag_mask = 0;
+	config = da9052_regmap_config;
+	config.read_flag_mask = 1;
 
-	da9052->regmap = devm_regmap_init_spi(spi, &da9052_regmap_config);
+	da9052->regmap = devm_regmap_init_spi(spi, &config);
 	if (IS_ERR(da9052->regmap)) {
 		ret = PTR_ERR(da9052->regmap);
 		dev_err(&spi->dev, "Failed to allocate register map: %d\n",
diff --git a/drivers/mfd/da9055-core.c b/drivers/mfd/da9055-core.c
index caf8dcffd0ad..b4d920c1ead1 100644
--- a/drivers/mfd/da9055-core.c
+++ b/drivers/mfd/da9055-core.c
@@ -296,73 +296,73 @@ static struct resource da9055_ld05_6_resource = {
 
 static const struct mfd_cell da9055_devs[] = {
 	{
-		.of_compatible = "dialog,da9055-gpio",
+		.of_compatible = "dlg,da9055-gpio",
 		.name = "da9055-gpio",
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.id = 1,
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.id = 2,
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.id = 3,
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.id = 4,
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.id = 5,
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.id = 6,
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.id = 7,
 		.resources = &da9055_ld05_6_resource,
 		.num_resources = 1,
 	},
 	{
-		.of_compatible = "dialog,da9055-regulator",
+		.of_compatible = "dlg,da9055-regulator",
 		.name = "da9055-regulator",
 		.resources = &da9055_ld05_6_resource,
 		.num_resources = 1,
 		.id = 8,
 	},
 	{
-		.of_compatible = "dialog,da9055-onkey",
+		.of_compatible = "dlg,da9055-onkey",
 		.name = "da9055-onkey",
 		.resources = &da9055_onkey_resource,
 		.num_resources = 1,
 	},
 	{
-		.of_compatible = "dialog,da9055-rtc",
+		.of_compatible = "dlg,da9055-rtc",
 		.name = "da9055-rtc",
 		.resources = da9055_rtc_resource,
 		.num_resources = ARRAY_SIZE(da9055_rtc_resource),
 	},
 	{
-		.of_compatible = "dialog,da9055-hwmon",
+		.of_compatible = "dlg,da9055-hwmon",
 		.name = "da9055-hwmon",
 		.resources = &da9055_hwmon_resource,
 		.num_resources = 1,
 	},
 	{
-		.of_compatible = "dialog,da9055-watchdog",
+		.of_compatible = "dlg,da9055-watchdog",
 		.name = "da9055-watchdog",
 	},
 };
diff --git a/drivers/mfd/hi6421-pmic-core.c b/drivers/mfd/hi6421-pmic-core.c
new file mode 100644
index 000000000000..321a2656fd00
--- /dev/null
+++ b/drivers/mfd/hi6421-pmic-core.c
@@ -0,0 +1,113 @@
+/*
+ * Device driver for Hi6421 IC
+ *
+ * Copyright (c) <2011-2014> HiSilicon Technologies Co., Ltd.
+ *              http://www.hisilicon.com
+ * Copyright (c) <2013-2014> Linaro Ltd.
+ *              http://www.linaro.org
+ *
+ * Author: Guodong Xu <guodong.xu@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/mfd/core.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/mfd/hi6421-pmic.h>
+
+static const struct mfd_cell hi6421_devs[] = {
+	{ .name = "hi6421-regulator", },
+};
+
+static struct regmap_config hi6421_regmap_config = {
+	.reg_bits = 32,
+	.reg_stride = 4,
+	.val_bits = 8,
+	.max_register = HI6421_REG_TO_BUS_ADDR(HI6421_REG_MAX),
+};
+
+static int hi6421_pmic_probe(struct platform_device *pdev)
+{
+	struct hi6421_pmic *pmic;
+	struct resource *res;
+	void __iomem *base;
+	int ret;
+
+	pmic = devm_kzalloc(&pdev->dev, sizeof(*pmic), GFP_KERNEL);
+	if (!pmic)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	pmic->regmap = devm_regmap_init_mmio_clk(&pdev->dev, NULL, base,
+						 &hi6421_regmap_config);
+	if (IS_ERR(pmic->regmap)) {
+		dev_err(&pdev->dev,
+			"regmap init failed: %ld\n", PTR_ERR(pmic->regmap));
+		return PTR_ERR(pmic->regmap);
+	}
+
+	/* set over-current protection debounce 8ms */
+	regmap_update_bits(pmic->regmap, HI6421_OCP_DEB_CTRL_REG,
+				(HI6421_OCP_DEB_SEL_MASK
+				 | HI6421_OCP_EN_DEBOUNCE_MASK
+				 | HI6421_OCP_AUTO_STOP_MASK),
+				(HI6421_OCP_DEB_SEL_8MS
+				 | HI6421_OCP_EN_DEBOUNCE_ENABLE));
+
+	platform_set_drvdata(pdev, pmic);
+
+	ret = mfd_add_devices(&pdev->dev, 0, hi6421_devs,
+			ARRAY_SIZE(hi6421_devs), NULL, 0, NULL);
+	if (ret) {
+		dev_err(&pdev->dev, "add mfd devices failed: %d\n", ret);
+		return ret;
+	}
+
+	return 0;
+}
+
+static int hi6421_pmic_remove(struct platform_device *pdev)
+{
+	mfd_remove_devices(&pdev->dev);
+
+	return 0;
+}
+
+static struct of_device_id of_hi6421_pmic_match_tbl[] = {
+	{ .compatible = "hisilicon,hi6421-pmic", },
+	{ },
+};
+
+static struct platform_driver hi6421_pmic_driver = {
+	.driver = {
+		.name	= "hi6421_pmic",
+		.of_match_table = of_hi6421_pmic_match_tbl,
+	},
+	.probe	= hi6421_pmic_probe,
+	.remove	= hi6421_pmic_remove,
+};
+module_platform_driver(hi6421_pmic_driver);
+
+MODULE_AUTHOR("Guodong Xu <guodong.xu@linaro.org>");
+MODULE_DESCRIPTION("Hi6421 PMIC driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/htc-i2cpld.c b/drivers/mfd/htc-i2cpld.c
index b44f0203983b..ebb9cf19e347 100644
--- a/drivers/mfd/htc-i2cpld.c
+++ b/drivers/mfd/htc-i2cpld.c
@@ -227,15 +227,12 @@ static irqreturn_t htcpld_handler(int irq, void *dev)
 static void htcpld_chip_set(struct gpio_chip *chip, unsigned offset, int val)
 {
 	struct i2c_client *client;
-	struct htcpld_chip *chip_data;
+	struct htcpld_chip *chip_data =
+		container_of(chip, struct htcpld_chip, chip_out);
 	unsigned long flags;
 
-	chip_data = container_of(chip, struct htcpld_chip, chip_out);
-	if (!chip_data)
-		return;
-
 	client = chip_data->client;
-	if (client == NULL)
+	if (!client)
 		return;
 
 	spin_lock_irqsave(&chip_data->lock, flags);
@@ -261,31 +258,18 @@ static void htcpld_chip_set_ni(struct work_struct *work)
 static int htcpld_chip_get(struct gpio_chip *chip, unsigned offset)
 {
 	struct htcpld_chip *chip_data;
-	int val = 0;
-	int is_input = 0;
-
-	/* Try out first */
-	chip_data = container_of(chip, struct htcpld_chip, chip_out);
-	if (!chip_data) {
-		/* Try in */
-		is_input = 1;
-		chip_data = container_of(chip, struct htcpld_chip, chip_in);
-		if (!chip_data)
-			return -EINVAL;
-	}
+	u8 cache;
 
-	/* Determine if this is an input or output GPIO */
-	if (!is_input)
-		/* Use the output cache */
-		val = (chip_data->cache_out >> offset) & 1;
-	else
-		/* Use the input cache */
-		val = (chip_data->cache_in >> offset) & 1;
+	if (!strncmp(chip->label, "htcpld-out", 10)) {
+		chip_data = container_of(chip, struct htcpld_chip, chip_out);
+		cache = chip_data->cache_out;
+	} else if (!strncmp(chip->label, "htcpld-in", 9)) {
+		chip_data = container_of(chip, struct htcpld_chip, chip_in);
+		cache = chip_data->cache_in;
+	} else
+		return -EINVAL;
 
-	if (val)
-		return 1;
-	else
-		return 0;
+	return (cache >> offset) & 1;
 }
 
 static int htcpld_direction_output(struct gpio_chip *chip,
@@ -376,7 +360,7 @@ static int htcpld_register_chip_i2c(
 	plat_chip_data = &pdata->chip[chip_index];
 
 	adapter = i2c_get_adapter(pdata->i2c_adapter_id);
-	if (adapter == NULL) {
+	if (!adapter) {
 		/* Eek, no such I2C adapter!  Bail out. */
 		dev_warn(dev, "Chip at i2c address 0x%x: Invalid i2c adapter %d\n",
 			 plat_chip_data->addr, pdata->i2c_adapter_id);
@@ -404,7 +388,7 @@ static int htcpld_register_chip_i2c(
 	}
 
 	i2c_set_clientdata(client, chip);
-	snprintf(client->name, I2C_NAME_SIZE, "Chip_0x%d", client->addr);
+	snprintf(client->name, I2C_NAME_SIZE, "Chip_0x%x", client->addr);
 	chip->client = client;
 
 	/* Reset the chip */
@@ -481,15 +465,9 @@ static int htcpld_register_chip_gpio(
 
 	ret = gpiochip_add(&(chip->chip_in));
 	if (ret) {
-		int error;
-
 		dev_warn(dev, "Unable to register input GPIOs for 0x%x: %d\n",
 			 plat_chip_data->addr, ret);
-
-		error = gpiochip_remove(&(chip->chip_out));
-		if (error)
-			dev_warn(dev, "Error while trying to unregister gpio chip: %d\n", error);
-
+		gpiochip_remove(&(chip->chip_out));
 		return ret;
 	}
 
diff --git a/drivers/mfd/intel_soc_pmic_core.c b/drivers/mfd/intel_soc_pmic_core.c
index 2720922f90b4..df7b0642a5b4 100644
--- a/drivers/mfd/intel_soc_pmic_core.c
+++ b/drivers/mfd/intel_soc_pmic_core.c
@@ -115,6 +115,7 @@ static void intel_soc_pmic_shutdown(struct i2c_client *i2c)
 	return;
 }
 
+#if defined(CONFIG_PM_SLEEP)
 static int intel_soc_pmic_suspend(struct device *dev)
 {
 	struct intel_soc_pmic *pmic = dev_get_drvdata(dev);
@@ -132,6 +133,7 @@ static int intel_soc_pmic_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(intel_soc_pmic_pm_ops, intel_soc_pmic_suspend,
 			 intel_soc_pmic_resume);
diff --git a/drivers/mfd/lpc_ich.c b/drivers/mfd/lpc_ich.c
index 7d8482ff5868..f35d4280b2f7 100644
--- a/drivers/mfd/lpc_ich.c
+++ b/drivers/mfd/lpc_ich.c
@@ -54,6 +54,7 @@
  *	document number TBD : Avoton SoC
  *	document number TBD : Coleto Creek
  *	document number TBD : Wildcat Point-LP
+ *	document number TBD : 9 Series
  */
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
@@ -216,6 +217,8 @@ enum lpc_chipsets {
 	LPC_BAYTRAIL,   /* Bay Trail SoC */
 	LPC_COLETO,	/* Coleto Creek */
 	LPC_WPT_LP,	/* Wildcat Point-LP */
+	LPC_BRASWELL,	/* Braswell SoC */
+	LPC_9S,		/* 9 Series */
 };
 
 static struct lpc_ich_info lpc_chipset_info[] = {
@@ -519,6 +522,14 @@ static struct lpc_ich_info lpc_chipset_info[] = {
 		.name = "Wildcat Point_LP",
 		.iTCO_version = 2,
 	},
+	[LPC_BRASWELL] = {
+		.name = "Braswell SoC",
+		.iTCO_version = 3,
+	},
+	[LPC_9S] = {
+		.name = "9 Series",
+		.iTCO_version = 2,
+	},
 };
 
 /*
@@ -745,6 +756,12 @@ static const struct pci_device_id lpc_ich_ids[] = {
 	{ PCI_VDEVICE(INTEL, 0x9cc6), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc7), LPC_WPT_LP},
 	{ PCI_VDEVICE(INTEL, 0x9cc9), LPC_WPT_LP},
+	{ PCI_VDEVICE(INTEL, 0x229c), LPC_BRASWELL},
+	{ PCI_VDEVICE(INTEL, 0x8cc1), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc2), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc3), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc4), LPC_9S},
+	{ PCI_VDEVICE(INTEL, 0x8cc6), LPC_9S},
 	{ 0, },			/* End of list */
 };
 MODULE_DEVICE_TABLE(pci, lpc_ich_ids);
diff --git a/drivers/mfd/lpc_sch.c b/drivers/mfd/lpc_sch.c
index 4ee755034f3b..c980da479a35 100644
--- a/drivers/mfd/lpc_sch.c
+++ b/drivers/mfd/lpc_sch.c
@@ -7,6 +7,7 @@
  *  Configuration Registers.
  *
  *  Copyright (c) 2010 CompuLab Ltd
+ *  Copyright (c) 2014 Intel Corp.
  *  Author: Denis Turischev <denis@compulab.co.il>
  *
  *  This program is free software; you can redistribute it and/or modify
@@ -17,10 +18,6 @@
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
- *
- *  You should have received a copy of the GNU General Public License
- *  along with this program; see the file COPYING.  If not, write to
- *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
  */
 
 #include <linux/kernel.h>
@@ -37,123 +34,165 @@
 #define GPIO_IO_SIZE	64
 #define GPIO_IO_SIZE_CENTERTON	128
 
+/* Intel Quark X1000 GPIO IRQ Number */
+#define GPIO_IRQ_QUARK_X1000	9
+
 #define WDTBASE		0x84
 #define WDT_IO_SIZE	64
 
-static struct resource smbus_sch_resource = {
-		.flags = IORESOURCE_IO,
-};
-
-static struct resource gpio_sch_resource = {
-		.flags = IORESOURCE_IO,
-};
-
-static struct resource wdt_sch_resource = {
-		.flags = IORESOURCE_IO,
+enum sch_chipsets {
+	LPC_SCH = 0,		/* Intel Poulsbo SCH */
+	LPC_ITC,		/* Intel Tunnel Creek */
+	LPC_CENTERTON,		/* Intel Centerton */
+	LPC_QUARK_X1000,	/* Intel Quark X1000 */
 };
 
-static struct mfd_cell lpc_sch_cells[3];
-
-static struct mfd_cell isch_smbus_cell = {
-	.name = "isch_smbus",
-	.num_resources = 1,
-	.resources = &smbus_sch_resource,
-	.ignore_resource_conflicts = true,
+struct lpc_sch_info {
+	unsigned int io_size_smbus;
+	unsigned int io_size_gpio;
+	unsigned int io_size_wdt;
+	int irq_gpio;
 };
 
-static struct mfd_cell sch_gpio_cell = {
-	.name = "sch_gpio",
-	.num_resources = 1,
-	.resources = &gpio_sch_resource,
-	.ignore_resource_conflicts = true,
-};
-
-static struct mfd_cell wdt_sch_cell = {
-	.name = "ie6xx_wdt",
-	.num_resources = 1,
-	.resources = &wdt_sch_resource,
-	.ignore_resource_conflicts = true,
+static struct lpc_sch_info sch_chipset_info[] = {
+	[LPC_SCH] = {
+		.io_size_smbus = SMBUS_IO_SIZE,
+		.io_size_gpio = GPIO_IO_SIZE,
+		.irq_gpio = -1,
+	},
+	[LPC_ITC] = {
+		.io_size_smbus = SMBUS_IO_SIZE,
+		.io_size_gpio = GPIO_IO_SIZE,
+		.io_size_wdt = WDT_IO_SIZE,
+		.irq_gpio = -1,
+	},
+	[LPC_CENTERTON] = {
+		.io_size_smbus = SMBUS_IO_SIZE,
+		.io_size_gpio = GPIO_IO_SIZE_CENTERTON,
+		.io_size_wdt = WDT_IO_SIZE,
+		.irq_gpio = -1,
+	},
+	[LPC_QUARK_X1000] = {
+		.io_size_gpio = GPIO_IO_SIZE,
+		.irq_gpio = GPIO_IRQ_QUARK_X1000,
+	},
 };
 
 static const struct pci_device_id lpc_sch_ids[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SCH_LPC) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_ITC_LPC) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_CENTERTON_ILB) },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_SCH_LPC), LPC_SCH },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_ITC_LPC), LPC_ITC },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_CENTERTON_ILB), LPC_CENTERTON },
+	{ PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_QUARK_X1000_ILB), LPC_QUARK_X1000 },
 	{ 0, }
 };
 MODULE_DEVICE_TABLE(pci, lpc_sch_ids);
 
-static int lpc_sch_probe(struct pci_dev *dev,
-				const struct pci_device_id *id)
+#define LPC_NO_RESOURCE		1
+#define LPC_SKIP_RESOURCE	2
+
+static int lpc_sch_get_io(struct pci_dev *pdev, int where, const char *name,
+			  struct resource *res, int size)
 {
 	unsigned int base_addr_cfg;
 	unsigned short base_addr;
-	int i, cells = 0;
-	int ret;
 
-	pci_read_config_dword(dev, SMBASE, &base_addr_cfg);
+	if (size == 0)
+		return LPC_NO_RESOURCE;
+
+	pci_read_config_dword(pdev, where, &base_addr_cfg);
 	base_addr = 0;
 	if (!(base_addr_cfg & (1 << 31)))
-		dev_warn(&dev->dev, "Decode of the SMBus I/O range disabled\n");
+		dev_warn(&pdev->dev, "Decode of the %s I/O range disabled\n",
+			 name);
 	else
 		base_addr = (unsigned short)base_addr_cfg;
 
 	if (base_addr == 0) {
-		dev_warn(&dev->dev, "I/O space for SMBus uninitialized\n");
-	} else {
-		lpc_sch_cells[cells++] = isch_smbus_cell;
-		smbus_sch_resource.start = base_addr;
-		smbus_sch_resource.end = base_addr + SMBUS_IO_SIZE - 1;
+		dev_warn(&pdev->dev, "I/O space for %s uninitialized\n", name);
+		return LPC_SKIP_RESOURCE;
 	}
 
-	pci_read_config_dword(dev, GPIOBASE, &base_addr_cfg);
-	base_addr = 0;
-	if (!(base_addr_cfg & (1 << 31)))
-		dev_warn(&dev->dev, "Decode of the GPIO I/O range disabled\n");
-	else
-		base_addr = (unsigned short)base_addr_cfg;
+	res->start = base_addr;
+	res->end = base_addr + size - 1;
+	res->flags = IORESOURCE_IO;
 
-	if (base_addr == 0) {
-		dev_warn(&dev->dev, "I/O space for GPIO uninitialized\n");
-	} else {
-		lpc_sch_cells[cells++] = sch_gpio_cell;
-		gpio_sch_resource.start = base_addr;
-		if (id->device == PCI_DEVICE_ID_INTEL_CENTERTON_ILB)
-			gpio_sch_resource.end = base_addr + GPIO_IO_SIZE_CENTERTON - 1;
-		else
-			gpio_sch_resource.end = base_addr + GPIO_IO_SIZE - 1;
-	}
+	return 0;
+}
 
-	if (id->device == PCI_DEVICE_ID_INTEL_ITC_LPC
-	    || id->device == PCI_DEVICE_ID_INTEL_CENTERTON_ILB) {
-		pci_read_config_dword(dev, WDTBASE, &base_addr_cfg);
-		base_addr = 0;
-		if (!(base_addr_cfg & (1 << 31)))
-			dev_warn(&dev->dev, "Decode of the WDT I/O range disabled\n");
-		else
-			base_addr = (unsigned short)base_addr_cfg;
-		if (base_addr == 0)
-			dev_warn(&dev->dev, "I/O space for WDT uninitialized\n");
-		else {
-			lpc_sch_cells[cells++] = wdt_sch_cell;
-			wdt_sch_resource.start = base_addr;
-			wdt_sch_resource.end = base_addr + WDT_IO_SIZE - 1;
-		}
-	}
+static int lpc_sch_populate_cell(struct pci_dev *pdev, int where,
+				 const char *name, int size, int irq,
+				 int id, struct mfd_cell *cell)
+{
+	struct resource *res;
+	int ret;
 
-	if (WARN_ON(cells > ARRAY_SIZE(lpc_sch_cells))) {
-		dev_err(&dev->dev, "Cell count exceeds array size");
-		return -ENODEV;
-	}
+	res = devm_kcalloc(&pdev->dev, 2, sizeof(*res), GFP_KERNEL);
+	if (!res)
+		return -ENOMEM;
+
+	ret = lpc_sch_get_io(pdev, where, name, res, size);
+	if (ret)
+		return ret;
+
+	memset(cell, 0, sizeof(*cell));
+
+	cell->name = name;
+	cell->resources = res;
+	cell->num_resources = 1;
+	cell->ignore_resource_conflicts = true;
+	cell->id = id;
+
+	/* Check if we need to add an IRQ resource */
+	if (irq < 0)
+		return 0;
+
+	res++;
+
+	res->start = irq;
+	res->end = irq;
+	res->flags = IORESOURCE_IRQ;
+
+	cell->num_resources++;
+
+	return 0;
+}
+
+static int lpc_sch_probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	struct mfd_cell lpc_sch_cells[3];
+	struct lpc_sch_info *info = &sch_chipset_info[id->driver_data];
+	unsigned int cells = 0;
+	int ret;
+
+	ret = lpc_sch_populate_cell(dev, SMBASE, "isch_smbus",
+				    info->io_size_smbus, -1,
+				    id->device, &lpc_sch_cells[cells]);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		cells++;
+
+	ret = lpc_sch_populate_cell(dev, GPIOBASE, "sch_gpio",
+				    info->io_size_gpio, info->irq_gpio,
+				    id->device, &lpc_sch_cells[cells]);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		cells++;
+
+	ret = lpc_sch_populate_cell(dev, WDTBASE, "ie6xx_wdt",
+				    info->io_size_wdt, -1,
+				    id->device, &lpc_sch_cells[cells]);
+	if (ret < 0)
+		return ret;
+	if (ret == 0)
+		cells++;
 
 	if (cells == 0) {
 		dev_err(&dev->dev, "All decode registers disabled.\n");
 		return -ENODEV;
 	}
 
-	for (i = 0; i < cells; i++)
-		lpc_sch_cells[i].id = id->device;
-
 	ret = mfd_add_devices(&dev->dev, 0, lpc_sch_cells, cells, NULL, 0, NULL);
 	if (ret)
 		mfd_remove_devices(&dev->dev);
diff --git a/drivers/mfd/max14577.c b/drivers/mfd/max14577.c
index 4a5e885383f8..de96b7fb1f6d 100644
--- a/drivers/mfd/max14577.c
+++ b/drivers/mfd/max14577.c
@@ -26,6 +26,87 @@
 #include <linux/mfd/max14577.h>
 #include <linux/mfd/max14577-private.h>
 
+/*
+ * Table of valid charger currents for different Maxim chipsets.
+ * It is placed here because it is used by both charger and regulator driver.
+ */
+const struct maxim_charger_current maxim_charger_currents[] = {
+	[MAXIM_DEVICE_TYPE_UNKNOWN] = { 0, 0, 0, 0 },
+	[MAXIM_DEVICE_TYPE_MAX14577] = {
+		.min		= MAX14577_CHARGER_CURRENT_LIMIT_MIN,
+		.high_start	= MAX14577_CHARGER_CURRENT_LIMIT_HIGH_START,
+		.high_step	= MAX14577_CHARGER_CURRENT_LIMIT_HIGH_STEP,
+		.max		= MAX14577_CHARGER_CURRENT_LIMIT_MAX,
+	},
+	[MAXIM_DEVICE_TYPE_MAX77836] = {
+		.min		= MAX77836_CHARGER_CURRENT_LIMIT_MIN,
+		.high_start	= MAX77836_CHARGER_CURRENT_LIMIT_HIGH_START,
+		.high_step	= MAX77836_CHARGER_CURRENT_LIMIT_HIGH_STEP,
+		.max		= MAX77836_CHARGER_CURRENT_LIMIT_MAX,
+	},
+};
+EXPORT_SYMBOL_GPL(maxim_charger_currents);
+
+/*
+ * maxim_charger_calc_reg_current - Calculate register value for current
+ * @limits:	constraints for charger, matching the MBCICHWRC register
+ * @min_ua:	minimal requested current, micro Amps
+ * @max_ua:	maximum requested current, micro Amps
+ * @dst:	destination to store calculated register value
+ *
+ * Calculates the value of MBCICHWRC (Fast Battery Charge Current) register
+ * for given current and stores it under pointed 'dst'. The stored value
+ * combines low bit (MBCICHWRCL) and high bits (MBCICHWRCH). It is also
+ * properly shifted.
+ *
+ * The calculated register value matches the current which:
+ *  - is always between <limits.min, limits.max>;
+ *  - is always less or equal to max_ua;
+ *  - is the highest possible value;
+ *  - may be lower than min_ua.
+ *
+ * On success returns 0. On error returns -EINVAL (requested min/max current
+ * is outside of given charger limits) and 'dst' is not set.
+ */
+int maxim_charger_calc_reg_current(const struct maxim_charger_current *limits,
+		unsigned int min_ua, unsigned int max_ua, u8 *dst)
+{
+	unsigned int current_bits = 0xf;
+
+	if (min_ua > max_ua)
+		return -EINVAL;
+
+	if (min_ua > limits->max || max_ua < limits->min)
+		return -EINVAL;
+
+	if (max_ua < limits->high_start) {
+		/*
+		 * Less than high_start, so set the minimal current
+		 * (turn Low Bit off, 0 as high bits).
+		 */
+		*dst = 0x0;
+		return 0;
+	}
+
+	/* max_ua is in range: <high_start, infinite>, cut it to limits.max */
+	max_ua = min(limits->max, max_ua);
+	max_ua -= limits->high_start;
+	/*
+	 * There is no risk of overflow 'max_ua' here because:
+	 *  - max_ua >= limits.high_start
+	 *  - BUILD_BUG checks that 'limits' are: max >= high_start + high_step
+	 */
+	current_bits = max_ua / limits->high_step;
+
+	/* Turn Low Bit on (use range <limits.high_start, limits.max>) ... */
+	*dst = 0x1 << CHGCTRL4_MBCICHWRCL_SHIFT;
+	/* and set proper High Bits */
+	*dst |= current_bits << CHGCTRL4_MBCICHWRCH_SHIFT;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(maxim_charger_calc_reg_current);
+
 static const struct mfd_cell max14577_devs[] = {
 	{
 		.name = "max14577-muic",
@@ -35,7 +116,10 @@ static const struct mfd_cell max14577_devs[] = {
 		.name = "max14577-regulator",
 		.of_compatible = "maxim,max14577-regulator",
 	},
-	{ .name = "max14577-charger", },
+	{
+		.name = "max14577-charger",
+		.of_compatible = "maxim,max14577-charger",
+	},
 };
 
 static const struct mfd_cell max77836_devs[] = {
@@ -372,8 +456,7 @@ static int max14577_i2c_probe(struct i2c_client *i2c,
 	}
 
 	ret = mfd_add_devices(max14577->dev, -1, mfd_devs,
-			mfd_devs_size, NULL, 0,
-			regmap_irq_get_domain(max14577->irq_data));
+			mfd_devs_size, NULL, 0, NULL);
 	if (ret < 0)
 		goto err_mfd;
 
@@ -463,6 +546,20 @@ static int __init max14577_i2c_init(void)
 	BUILD_BUG_ON(ARRAY_SIZE(max14577_i2c_id) != MAXIM_DEVICE_TYPE_NUM);
 	BUILD_BUG_ON(ARRAY_SIZE(max14577_dt_match) != MAXIM_DEVICE_TYPE_NUM);
 
+	/* Valid charger current values must be provided for each chipset */
+	BUILD_BUG_ON(ARRAY_SIZE(maxim_charger_currents) != MAXIM_DEVICE_TYPE_NUM);
+
+	/* Check for valid values for charger */
+	BUILD_BUG_ON(MAX14577_CHARGER_CURRENT_LIMIT_HIGH_START +
+			MAX14577_CHARGER_CURRENT_LIMIT_HIGH_STEP * 0xf !=
+			MAX14577_CHARGER_CURRENT_LIMIT_MAX);
+	BUILD_BUG_ON(MAX14577_CHARGER_CURRENT_LIMIT_HIGH_STEP == 0);
+
+	BUILD_BUG_ON(MAX77836_CHARGER_CURRENT_LIMIT_HIGH_START +
+			MAX77836_CHARGER_CURRENT_LIMIT_HIGH_STEP * 0xf !=
+			MAX77836_CHARGER_CURRENT_LIMIT_MAX);
+	BUILD_BUG_ON(MAX77836_CHARGER_CURRENT_LIMIT_HIGH_STEP == 0);
+
 	return i2c_add_driver(&max14577_i2c_driver);
 }
 subsys_initcall(max14577_i2c_init);
diff --git a/drivers/mfd/max77686.c b/drivers/mfd/max77686.c
index 86e552348db4..929795eae9fc 100644
--- a/drivers/mfd/max77686.c
+++ b/drivers/mfd/max77686.c
@@ -52,7 +52,7 @@ static const struct mfd_cell max77802_devs[] = {
 static bool max77802_pmic_is_accessible_reg(struct device *dev,
 					    unsigned int reg)
 {
-	return (reg >= MAX77802_REG_DEVICE_ID && reg < MAX77802_REG_PMIC_END);
+	return reg < MAX77802_REG_PMIC_END;
 }
 
 static bool max77802_rtc_is_accessible_reg(struct device *dev,
diff --git a/drivers/mfd/max77693.c b/drivers/mfd/max77693.c
index 249c139ef04a..cf008f45968c 100644
--- a/drivers/mfd/max77693.c
+++ b/drivers/mfd/max77693.c
@@ -44,9 +44,12 @@
 static const struct mfd_cell max77693_devs[] = {
 	{ .name = "max77693-pmic", },
 	{ .name = "max77693-charger", },
-	{ .name = "max77693-flash", },
 	{ .name = "max77693-muic", },
 	{ .name = "max77693-haptic", },
+	{
+		.name = "max77693-flash",
+		.of_compatible = "maxim,max77693-flash",
+	},
 };
 
 static const struct regmap_config max77693_regmap_config = {
diff --git a/drivers/mfd/max8925-i2c.c b/drivers/mfd/max8925-i2c.c
index ecbe78ead3b6..c880c895c5a6 100644
--- a/drivers/mfd/max8925-i2c.c
+++ b/drivers/mfd/max8925-i2c.c
@@ -37,7 +37,7 @@ static inline int max8925_read_device(struct i2c_client *i2c,
 static inline int max8925_write_device(struct i2c_client *i2c,
 				       int reg, int bytes, void *src)
 {
-	unsigned char buf[bytes + 1];
+	unsigned char buf[9];
 	int ret;
 
 	buf[0] = (unsigned char)reg;
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index 2b6bc868cd3d..64dde5d24b32 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -36,6 +36,9 @@
 #define MC34708_REVISION_FIN		(0x07 <<  6)
 #define MC34708_REVISION_FAB		(0x07 <<  9)
 
+#define MC13XXX_PWRCTRL		15
+#define MC13XXX_PWRCTRL_WDIRESET	(1 << 12)
+
 #define MC13XXX_ADC1		44
 #define MC13XXX_ADC1_ADEN		(1 << 0)
 #define MC13XXX_ADC1_RAND		(1 << 1)
@@ -416,6 +419,11 @@ int mc13xxx_common_init(struct device *dev)
 
 	mc13xxx->variant->print_revision(mc13xxx, revision);
 
+	ret = mc13xxx_reg_rmw(mc13xxx, MC13XXX_PWRCTRL,
+			MC13XXX_PWRCTRL_WDIRESET, MC13XXX_PWRCTRL_WDIRESET);
+	if (ret)
+		return ret;
+
 	for (i = 0; i < ARRAY_SIZE(mc13xxx->irqs); i++) {
 		mc13xxx->irqs[i].reg_offset = i / MC13XXX_IRQ_PER_REG;
 		mc13xxx->irqs[i].mask = BIT(i % MC13XXX_IRQ_PER_REG);
diff --git a/drivers/mfd/menelaus.c b/drivers/mfd/menelaus.c
index 5e2667afe2bc..9f01aef539dd 100644
--- a/drivers/mfd/menelaus.c
+++ b/drivers/mfd/menelaus.c
@@ -466,8 +466,6 @@ static int menelaus_set_voltage(const struct menelaus_vtg *vtg, int mV,
 	struct i2c_client *c = the_menelaus->client;
 
 	mutex_lock(&the_menelaus->lock);
-	if (!vtg)
-		goto set_voltage;
 
 	ret = menelaus_read_reg(vtg->vtg_reg);
 	if (ret < 0)
@@ -482,7 +480,6 @@ static int menelaus_set_voltage(const struct menelaus_vtg *vtg, int mV,
 	ret = menelaus_write_reg(vtg->vtg_reg, val);
 	if (ret < 0)
 		goto out;
-set_voltage:
 	ret = menelaus_write_reg(vtg->mode_reg, mode);
 out:
 	mutex_unlock(&the_menelaus->lock);
@@ -1186,7 +1183,7 @@ static int menelaus_probe(struct i2c_client *client,
 			  const struct i2c_device_id *id)
 {
 	struct menelaus_chip	*menelaus;
-	int			rev = 0, val;
+	int			rev = 0;
 	int			err = 0;
 	struct menelaus_platform_data *menelaus_pdata =
 					dev_get_platdata(&client->dev);
@@ -1239,10 +1236,10 @@ static int menelaus_probe(struct i2c_client *client,
 
 	pr_info("Menelaus rev %d.%d\n", rev >> 4, rev & 0x0f);
 
-	val = menelaus_read_reg(MENELAUS_VCORE_CTRL1);
-	if (val < 0)
+	err = menelaus_read_reg(MENELAUS_VCORE_CTRL1);
+	if (err < 0)
 		goto fail;
-	if (val & (1 << 7))
+	if (err & BIT(7))
 		menelaus->vcore_hw_mode = 1;
 	else
 		menelaus->vcore_hw_mode = 0;
diff --git a/drivers/mfd/menf21bmc.c b/drivers/mfd/menf21bmc.c
new file mode 100644
index 000000000000..1c274345820c
--- /dev/null
+++ b/drivers/mfd/menf21bmc.c
@@ -0,0 +1,132 @@
+/*
+ *  MEN 14F021P00 Board Management Controller (BMC) MFD Core Driver.
+ *
+ *  Copyright (C) 2014 MEN Mikro Elektronik Nuernberg GmbH
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+
+#define BMC_CMD_WDT_EXIT_PROD	0x18
+#define BMC_CMD_WDT_PROD_STAT	0x19
+#define BMC_CMD_REV_MAJOR	0x80
+#define BMC_CMD_REV_MINOR	0x81
+#define BMC_CMD_REV_MAIN	0x82
+
+static struct mfd_cell menf21bmc_cell[] = {
+	{ .name = "menf21bmc_wdt", },
+	{ .name = "menf21bmc_led", },
+	{ .name = "menf21bmc_hwmon", }
+};
+
+static int menf21bmc_wdt_exit_prod_mode(struct i2c_client *client)
+{
+	int val, ret;
+
+	val = i2c_smbus_read_byte_data(client, BMC_CMD_WDT_PROD_STAT);
+	if (val < 0)
+		return val;
+
+	/*
+	 * Production mode should be not active after delivery of the Board.
+	 * To be sure we check it, inform the user and exit the mode
+	 * if active.
+	 */
+	if (val == 0x00) {
+		dev_info(&client->dev,
+			"BMC in production mode. Exit production mode\n");
+
+		ret = i2c_smbus_write_byte(client, BMC_CMD_WDT_EXIT_PROD);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int
+menf21bmc_probe(struct i2c_client *client, const struct i2c_device_id *ids)
+{
+	int rev_major, rev_minor, rev_main;
+	int ret;
+
+	ret = i2c_check_functionality(client->adapter,
+				      I2C_FUNC_SMBUS_BYTE_DATA |
+				      I2C_FUNC_SMBUS_WORD_DATA |
+				      I2C_FUNC_SMBUS_BYTE);
+	if (!ret)
+		return -ENODEV;
+
+	rev_major = i2c_smbus_read_word_data(client, BMC_CMD_REV_MAJOR);
+	if (rev_major < 0) {
+		dev_err(&client->dev, "failed to get BMC major revision\n");
+		return rev_major;
+	}
+
+	rev_minor = i2c_smbus_read_word_data(client, BMC_CMD_REV_MINOR);
+	if (rev_minor < 0) {
+		dev_err(&client->dev, "failed to get BMC minor revision\n");
+		return rev_minor;
+	}
+
+	rev_main = i2c_smbus_read_word_data(client, BMC_CMD_REV_MAIN);
+	if (rev_main < 0) {
+		dev_err(&client->dev, "failed to get BMC main revision\n");
+		return rev_main;
+	}
+
+	dev_info(&client->dev, "FW Revision: %02d.%02d.%02d\n",
+		 rev_major, rev_minor, rev_main);
+
+	/*
+	 * We have to exit the Production Mode of the BMC to activate the
+	 * Watchdog functionality and the BIOS life sign monitoring.
+	 */
+	ret = menf21bmc_wdt_exit_prod_mode(client);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to leave production mode\n");
+		return ret;
+	}
+
+	ret = mfd_add_devices(&client->dev, 0, menf21bmc_cell,
+			      ARRAY_SIZE(menf21bmc_cell), NULL, 0, NULL);
+	if (ret < 0) {
+		dev_err(&client->dev, "failed to add BMC sub-devices\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int menf21bmc_remove(struct i2c_client *client)
+{
+	mfd_remove_devices(&client->dev);
+	return 0;
+}
+
+static const struct i2c_device_id menf21bmc_id_table[] = {
+	{ "menf21bmc" },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, menf21bmc_id_table);
+
+static struct i2c_driver menf21bmc_driver = {
+	.driver.name	= "menf21bmc",
+	.id_table	= menf21bmc_id_table,
+	.probe		= menf21bmc_probe,
+	.remove		= menf21bmc_remove,
+};
+
+module_i2c_driver(menf21bmc_driver);
+
+MODULE_DESCRIPTION("MEN 14F021P00 BMC mfd core driver");
+MODULE_AUTHOR("Andreas Werner <andreas.werner@men.de>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index 892d343193ad..f3338fe9d069 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -78,6 +78,44 @@ static int mfd_platform_add_cell(struct platform_device *pdev,
 	return 0;
 }
 
+#if IS_ENABLED(CONFIG_ACPI)
+static void mfd_acpi_add_device(const struct mfd_cell *cell,
+				struct platform_device *pdev)
+{
+	struct acpi_device *parent_adev;
+	struct acpi_device *adev;
+
+	parent_adev = ACPI_COMPANION(pdev->dev.parent);
+	if (!parent_adev)
+		return;
+
+	/*
+	 * MFD child device gets its ACPI handle either from the ACPI
+	 * device directly under the parent that matches the acpi_pnpid or
+	 * it will use the parent handle if is no acpi_pnpid is given.
+	 */
+	adev = parent_adev;
+	if (cell->acpi_pnpid) {
+		struct acpi_device_id ids[2] = {};
+		struct acpi_device *child_adev;
+
+		strlcpy(ids[0].id, cell->acpi_pnpid, sizeof(ids[0].id));
+		list_for_each_entry(child_adev, &parent_adev->children, node)
+			if (acpi_match_device_ids(child_adev, ids)) {
+				adev = child_adev;
+				break;
+			}
+	}
+
+	ACPI_COMPANION_SET(&pdev->dev, adev);
+}
+#else
+static inline void mfd_acpi_add_device(const struct mfd_cell *cell,
+				       struct platform_device *pdev)
+{
+}
+#endif
+
 static int mfd_add_device(struct device *parent, int id,
 			  const struct mfd_cell *cell, atomic_t *usage_count,
 			  struct resource *mem_base,
@@ -101,6 +139,7 @@ static int mfd_add_device(struct device *parent, int id,
 	pdev->dev.type = &mfd_dev_type;
 	pdev->dev.dma_mask = parent->dma_mask;
 	pdev->dev.dma_parms = parent->dma_parms;
+	pdev->dev.coherent_dma_mask = parent->coherent_dma_mask;
 
 	ret = regulator_bulk_register_supply_alias(
 			&pdev->dev, cell->parent_supplies,
@@ -118,6 +157,8 @@ static int mfd_add_device(struct device *parent, int id,
 		}
 	}
 
+	mfd_acpi_add_device(cell, pdev);
+
 	if (cell->pdata_size) {
 		ret = platform_device_add_data(pdev,
 					cell->platform_data, cell->pdata_size);
diff --git a/drivers/mfd/omap-usb-host.c b/drivers/mfd/omap-usb-host.c
index 33a9234b701c..83dab2f0a50e 100644
--- a/drivers/mfd/omap-usb-host.c
+++ b/drivers/mfd/omap-usb-host.c
@@ -647,7 +647,7 @@ static int usbhs_omap_probe(struct platform_device *pdev)
 		default:
 			omap->nports = OMAP3_HS_USB_PORTS;
 			dev_dbg(dev,
-			 "USB HOST Rev:0x%d not recognized, assuming %d ports\n",
+			 "USB HOST Rev:0x%x not recognized, assuming %d ports\n",
 			 omap->usbhs_rev, omap->nports);
 			break;
 		}
diff --git a/drivers/mfd/pcf50633-core.c b/drivers/mfd/pcf50633-core.c
index c87f7a0a53f8..43664eb69c93 100644
--- a/drivers/mfd/pcf50633-core.c
+++ b/drivers/mfd/pcf50633-core.c
@@ -106,10 +106,7 @@ static ssize_t show_dump_regs(struct device *dev, struct device_attribute *attr,
 			} else
 				dump[n1] = pcf50633_reg_read(pcf, n + n1);
 
-		hex_dump_to_buffer(dump, sizeof(dump), 16, 1, buf1, 128, 0);
-		buf1 += strlen(buf1);
-		*buf1++ = '\n';
-		*buf1 = '\0';
+		buf1 += sprintf(buf1, "%*ph\n", (int)sizeof(dump), dump);
 	}
 
 	return buf1 - buf;
@@ -195,8 +192,9 @@ static int pcf50633_probe(struct i2c_client *client,
 				const struct i2c_device_id *ids)
 {
 	struct pcf50633 *pcf;
+	struct platform_device *pdev;
 	struct pcf50633_platform_data *pdata = dev_get_platdata(&client->dev);
-	int i, ret;
+	int i, j, ret;
 	int version, variant;
 
 	if (!client->irq) {
@@ -243,9 +241,6 @@ static int pcf50633_probe(struct i2c_client *client,
 
 
 	for (i = 0; i < PCF50633_NUM_REGULATORS; i++) {
-		struct platform_device *pdev;
-		int j;
-
 		pdev = platform_device_alloc("pcf50633-regulator", i);
 		if (!pdev)
 			return -ENOMEM;
@@ -253,25 +248,31 @@ static int pcf50633_probe(struct i2c_client *client,
 		pdev->dev.parent = pcf->dev;
 		ret = platform_device_add_data(pdev, &pdata->reg_init_data[i],
 					       sizeof(pdata->reg_init_data[i]));
-		if (ret) {
-			platform_device_put(pdev);
-			for (j = 0; j < i; j++)
-				platform_device_put(pcf->regulator_pdev[j]);
-			return ret;
-		}
-		pcf->regulator_pdev[i] = pdev;
+		if (ret)
+			goto err;
+
+		ret = platform_device_add(pdev);
+		if (ret)
+			goto err;
 
-		platform_device_add(pdev);
+		pcf->regulator_pdev[i] = pdev;
 	}
 
 	ret = sysfs_create_group(&client->dev.kobj, &pcf_attr_group);
 	if (ret)
-		dev_err(pcf->dev, "error creating sysfs entries\n");
+		dev_warn(pcf->dev, "error creating sysfs entries\n");
 
 	if (pdata->probe_done)
 		pdata->probe_done(pcf);
 
 	return 0;
+
+err:
+	platform_device_put(pdev);
+	for (j = 0; j < i; j++)
+		platform_device_put(pcf->regulator_pdev[j]);
+
+	return ret;
 }
 
 static int pcf50633_remove(struct i2c_client *client)
diff --git a/drivers/mfd/qcom-spmi-pmic.c b/drivers/mfd/qcom-spmi-pmic.c
new file mode 100644
index 000000000000..4b8beb2a1579
--- /dev/null
+++ b/drivers/mfd/qcom-spmi-pmic.c
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/spmi.h>
+#include <linux/regmap.h>
+#include <linux/of_platform.h>
+
+static const struct regmap_config spmi_regmap_config = {
+	.reg_bits	= 16,
+	.val_bits	= 8,
+	.max_register	= 0xffff,
+	.fast_io	= true,
+};
+
+static int pmic_spmi_probe(struct spmi_device *sdev)
+{
+	struct device_node *root = sdev->dev.of_node;
+	struct regmap *regmap;
+
+	regmap = devm_regmap_init_spmi_ext(sdev, &spmi_regmap_config);
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	return of_platform_populate(root, NULL, NULL, &sdev->dev);
+}
+
+static void pmic_spmi_remove(struct spmi_device *sdev)
+{
+	of_platform_depopulate(&sdev->dev);
+}
+
+static const struct of_device_id pmic_spmi_id_table[] = {
+	{ .compatible = "qcom,spmi-pmic" },
+	{ .compatible = "qcom,pm8941" },
+	{ .compatible = "qcom,pm8841" },
+	{ .compatible = "qcom,pma8084" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, pmic_spmi_id_table);
+
+static struct spmi_driver pmic_spmi_driver = {
+	.probe = pmic_spmi_probe,
+	.remove = pmic_spmi_remove,
+	.driver = {
+		.name = "pmic-spmi",
+		.of_match_table = pmic_spmi_id_table,
+	},
+};
+module_spmi_driver(pmic_spmi_driver);
+
+MODULE_DESCRIPTION("Qualcomm SPMI PMIC driver");
+MODULE_ALIAS("spmi:spmi-pmic");
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Josh Cartwright <joshc@codeaurora.org>");
+MODULE_AUTHOR("Stanimir Varbanov <svarbanov@mm-sol.com>");
diff --git a/drivers/mfd/rk808.c b/drivers/mfd/rk808.c
new file mode 100644
index 000000000000..bd0215069875
--- /dev/null
+++ b/drivers/mfd/rk808.c
@@ -0,0 +1,275 @@
+/*
+ * MFD core driver for Rockchip RK808
+ *
+ * Copyright (c) 2014, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * Author: Chris Zhong <zyw@rock-chips.com>
+ * Author: Zhang Qing <zhangqing@rock-chips.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/i2c.h>
+#include <linux/interrupt.h>
+#include <linux/mfd/rk808.h>
+#include <linux/mfd/core.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+
+struct rk808_reg_data {
+	int addr;
+	int mask;
+	int value;
+};
+
+static bool rk808_is_volatile_reg(struct device *dev, unsigned int reg)
+{
+	/*
+	 * Notes:
+	 * - Technically the ROUND_30s bit makes RTC_CTRL_REG volatile, but
+	 *   we don't use that feature.  It's better to cache.
+	 * - It's unlikely we care that RK808_DEVCTRL_REG is volatile since
+	 *   bits are cleared in case when we shutoff anyway, but better safe.
+	 */
+
+	switch (reg) {
+	case RK808_SECONDS_REG ... RK808_WEEKS_REG:
+	case RK808_RTC_STATUS_REG:
+	case RK808_VB_MON_REG:
+	case RK808_THERMAL_REG:
+	case RK808_DCDC_UV_STS_REG:
+	case RK808_LDO_UV_STS_REG:
+	case RK808_DCDC_PG_REG:
+	case RK808_LDO_PG_REG:
+	case RK808_DEVCTRL_REG:
+	case RK808_INT_STS_REG1:
+	case RK808_INT_STS_REG2:
+		return true;
+	}
+
+	return false;
+}
+
+static const struct regmap_config rk808_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.max_register = RK808_IO_POL_REG,
+	.cache_type = REGCACHE_RBTREE,
+	.volatile_reg = rk808_is_volatile_reg,
+};
+
+static struct resource rtc_resources[] = {
+	{
+		.start  = RK808_IRQ_RTC_ALARM,
+		.end    = RK808_IRQ_RTC_ALARM,
+		.flags  = IORESOURCE_IRQ,
+	}
+};
+
+static const struct mfd_cell rk808s[] = {
+	{ .name = "rk808-clkout", },
+	{ .name = "rk808-regulator", },
+	{
+		.name = "rk808-rtc",
+		.num_resources = ARRAY_SIZE(rtc_resources),
+		.resources = &rtc_resources[0],
+	},
+};
+
+static const struct rk808_reg_data pre_init_reg[] = {
+	{ RK808_BUCK3_CONFIG_REG, BUCK_ILMIN_MASK,  BUCK_ILMIN_150MA },
+	{ RK808_BUCK4_CONFIG_REG, BUCK_ILMIN_MASK,  BUCK_ILMIN_200MA },
+	{ RK808_BOOST_CONFIG_REG, BOOST_ILMIN_MASK, BOOST_ILMIN_100MA },
+	{ RK808_BUCK1_CONFIG_REG, BUCK1_RATE_MASK,  BUCK_ILMIN_200MA },
+	{ RK808_BUCK2_CONFIG_REG, BUCK2_RATE_MASK,  BUCK_ILMIN_200MA },
+	{ RK808_VB_MON_REG,       MASK_ALL,         VB_LO_ACT |
+						    VB_LO_SEL_3500MV },
+};
+
+static const struct regmap_irq rk808_irqs[] = {
+	/* INT_STS */
+	[RK808_IRQ_VOUT_LO] = {
+		.mask = RK808_IRQ_VOUT_LO_MSK,
+		.reg_offset = 0,
+	},
+	[RK808_IRQ_VB_LO] = {
+		.mask = RK808_IRQ_VB_LO_MSK,
+		.reg_offset = 0,
+	},
+	[RK808_IRQ_PWRON] = {
+		.mask = RK808_IRQ_PWRON_MSK,
+		.reg_offset = 0,
+	},
+	[RK808_IRQ_PWRON_LP] = {
+		.mask = RK808_IRQ_PWRON_LP_MSK,
+		.reg_offset = 0,
+	},
+	[RK808_IRQ_HOTDIE] = {
+		.mask = RK808_IRQ_HOTDIE_MSK,
+		.reg_offset = 0,
+	},
+	[RK808_IRQ_RTC_ALARM] = {
+		.mask = RK808_IRQ_RTC_ALARM_MSK,
+		.reg_offset = 0,
+	},
+	[RK808_IRQ_RTC_PERIOD] = {
+		.mask = RK808_IRQ_RTC_PERIOD_MSK,
+		.reg_offset = 0,
+	},
+
+	/* INT_STS2 */
+	[RK808_IRQ_PLUG_IN_INT] = {
+		.mask = RK808_IRQ_PLUG_IN_INT_MSK,
+		.reg_offset = 1,
+	},
+	[RK808_IRQ_PLUG_OUT_INT] = {
+		.mask = RK808_IRQ_PLUG_OUT_INT_MSK,
+		.reg_offset = 1,
+	},
+};
+
+static struct regmap_irq_chip rk808_irq_chip = {
+	.name = "rk808",
+	.irqs = rk808_irqs,
+	.num_irqs = ARRAY_SIZE(rk808_irqs),
+	.num_regs = 2,
+	.irq_reg_stride = 2,
+	.status_base = RK808_INT_STS_REG1,
+	.mask_base = RK808_INT_STS_MSK_REG1,
+	.ack_base = RK808_INT_STS_REG1,
+	.init_ack_masked = true,
+};
+
+static struct i2c_client *rk808_i2c_client;
+static void rk808_device_shutdown(void)
+{
+	int ret;
+	struct rk808 *rk808 = i2c_get_clientdata(rk808_i2c_client);
+
+	if (!rk808) {
+		dev_warn(&rk808_i2c_client->dev,
+			 "have no rk808, so do nothing here\n");
+		return;
+	}
+
+	ret = regmap_update_bits(rk808->regmap,
+				 RK808_DEVCTRL_REG,
+				 DEV_OFF_RST, DEV_OFF_RST);
+	if (ret)
+		dev_err(&rk808_i2c_client->dev, "power off error!\n");
+}
+
+static int rk808_probe(struct i2c_client *client,
+		       const struct i2c_device_id *id)
+{
+	struct device_node *np = client->dev.of_node;
+	struct rk808 *rk808;
+	int pm_off = 0;
+	int ret;
+	int i;
+
+	if (!client->irq) {
+		dev_err(&client->dev, "No interrupt support, no core IRQ\n");
+		return -EINVAL;
+	}
+
+	rk808 = devm_kzalloc(&client->dev, sizeof(*rk808), GFP_KERNEL);
+	if (!rk808)
+		return -ENOMEM;
+
+	rk808->regmap = devm_regmap_init_i2c(client, &rk808_regmap_config);
+	if (IS_ERR(rk808->regmap)) {
+		dev_err(&client->dev, "regmap initialization failed\n");
+		return PTR_ERR(rk808->regmap);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(pre_init_reg); i++) {
+		ret = regmap_update_bits(rk808->regmap, pre_init_reg[i].addr,
+					 pre_init_reg[i].mask,
+					 pre_init_reg[i].value);
+		if (ret) {
+			dev_err(&client->dev,
+				"0x%x write err\n", pre_init_reg[i].addr);
+			return ret;
+		}
+	}
+
+	ret = regmap_add_irq_chip(rk808->regmap, client->irq,
+				  IRQF_ONESHOT, -1,
+				  &rk808_irq_chip, &rk808->irq_data);
+	if (ret) {
+		dev_err(&client->dev, "Failed to add irq_chip %d\n", ret);
+		return ret;
+	}
+
+	rk808->i2c = client;
+	i2c_set_clientdata(client, rk808);
+
+	ret = mfd_add_devices(&client->dev, -1,
+			      rk808s, ARRAY_SIZE(rk808s),
+			      NULL, 0, regmap_irq_get_domain(rk808->irq_data));
+	if (ret) {
+		dev_err(&client->dev, "failed to add MFD devices %d\n", ret);
+		goto err_irq;
+	}
+
+	pm_off = of_property_read_bool(np,
+				"rockchip,system-power-controller");
+	if (pm_off && !pm_power_off) {
+		rk808_i2c_client = client;
+		pm_power_off = rk808_device_shutdown;
+	}
+
+	return 0;
+
+err_irq:
+	regmap_del_irq_chip(client->irq, rk808->irq_data);
+	return ret;
+}
+
+static int rk808_remove(struct i2c_client *client)
+{
+	struct rk808 *rk808 = i2c_get_clientdata(client);
+
+	regmap_del_irq_chip(client->irq, rk808->irq_data);
+	mfd_remove_devices(&client->dev);
+	pm_power_off = NULL;
+
+	return 0;
+}
+
+static struct of_device_id rk808_of_match[] = {
+	{ .compatible = "rockchip,rk808" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, rk808_of_match);
+
+static const struct i2c_device_id rk808_ids[] = {
+	{ "rk808" },
+	{ },
+};
+MODULE_DEVICE_TABLE(i2c, rk808_ids);
+
+static struct i2c_driver rk808_i2c_driver = {
+	.driver = {
+		.name = "rk808",
+		.of_match_table = rk808_of_match,
+	},
+	.probe    = rk808_probe,
+	.remove   = rk808_remove,
+	.id_table = rk808_ids,
+};
+
+module_i2c_driver(rk808_i2c_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Chris Zhong <zyw@rock-chips.com>");
+MODULE_AUTHOR("Zhang Qing <zhangqing@rock-chips.com>");
+MODULE_DESCRIPTION("RK808 PMIC driver");
diff --git a/drivers/mfd/rn5t618.c b/drivers/mfd/rn5t618.c
new file mode 100644
index 000000000000..666857192dbe
--- /dev/null
+++ b/drivers/mfd/rn5t618.c
@@ -0,0 +1,134 @@
+/*
+ * MFD core driver for Ricoh RN5T618 PMIC
+ *
+ * Copyright (C) 2014 Beniamino Galvani <b.galvani@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/i2c.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/rn5t618.h>
+#include <linux/module.h>
+#include <linux/regmap.h>
+
+static const struct mfd_cell rn5t618_cells[] = {
+	{ .name = "rn5t618-regulator" },
+	{ .name = "rn5t618-wdt" },
+};
+
+static bool rn5t618_volatile_reg(struct device *dev, unsigned int reg)
+{
+	switch (reg) {
+	case RN5T618_WATCHDOGCNT:
+	case RN5T618_DCIRQ:
+	case RN5T618_ILIMDATAH ... RN5T618_AIN0DATAL:
+	case RN5T618_IR_ADC1 ... RN5T618_IR_ADC3:
+	case RN5T618_IR_GPR:
+	case RN5T618_IR_GPF:
+	case RN5T618_MON_IOIN:
+	case RN5T618_INTMON:
+		return true;
+	default:
+		return false;
+	}
+}
+
+static const struct regmap_config rn5t618_regmap_config = {
+	.reg_bits	= 8,
+	.val_bits	= 8,
+	.volatile_reg	= rn5t618_volatile_reg,
+	.max_register	= RN5T618_MAX_REG,
+	.cache_type	= REGCACHE_RBTREE,
+};
+
+static struct rn5t618 *rn5t618_pm_power_off;
+
+static void rn5t618_power_off(void)
+{
+	/* disable automatic repower-on */
+	regmap_update_bits(rn5t618_pm_power_off->regmap, RN5T618_REPCNT,
+			   RN5T618_REPCNT_REPWRON, 0);
+	/* start power-off sequence */
+	regmap_update_bits(rn5t618_pm_power_off->regmap, RN5T618_SLPCNT,
+			   RN5T618_SLPCNT_SWPWROFF, RN5T618_SLPCNT_SWPWROFF);
+}
+
+static int rn5t618_i2c_probe(struct i2c_client *i2c,
+			     const struct i2c_device_id *id)
+{
+	struct rn5t618 *priv;
+	int ret;
+
+	priv = devm_kzalloc(&i2c->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	i2c_set_clientdata(i2c, priv);
+
+	priv->regmap = devm_regmap_init_i2c(i2c, &rn5t618_regmap_config);
+	if (IS_ERR(priv->regmap)) {
+		ret = PTR_ERR(priv->regmap);
+		dev_err(&i2c->dev, "regmap init failed: %d\n", ret);
+		return ret;
+	}
+
+	ret = mfd_add_devices(&i2c->dev, -1, rn5t618_cells,
+			      ARRAY_SIZE(rn5t618_cells), NULL, 0, NULL);
+	if (ret) {
+		dev_err(&i2c->dev, "failed to add sub-devices: %d\n", ret);
+		return ret;
+	}
+
+	if (!pm_power_off) {
+		rn5t618_pm_power_off = priv;
+		pm_power_off = rn5t618_power_off;
+	}
+
+	return 0;
+}
+
+static int rn5t618_i2c_remove(struct i2c_client *i2c)
+{
+	struct rn5t618 *priv = i2c_get_clientdata(i2c);
+
+	if (priv == rn5t618_pm_power_off) {
+		rn5t618_pm_power_off = NULL;
+		pm_power_off = NULL;
+	}
+
+	mfd_remove_devices(&i2c->dev);
+	return 0;
+}
+
+static const struct of_device_id rn5t618_of_match[] = {
+	{ .compatible = "ricoh,rn5t618" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rn5t618_of_match);
+
+static const struct i2c_device_id rn5t618_i2c_id[] = {
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, rn5t618_i2c_id);
+
+static struct i2c_driver rn5t618_i2c_driver = {
+	.driver = {
+		.name = "rn5t618",
+		.of_match_table = of_match_ptr(rn5t618_of_match),
+	},
+	.probe = rn5t618_i2c_probe,
+	.remove = rn5t618_i2c_remove,
+	.id_table = rn5t618_i2c_id,
+};
+
+module_i2c_driver(rn5t618_i2c_driver);
+
+MODULE_AUTHOR("Beniamino Galvani <b.galvani@gmail.com>");
+MODULE_DESCRIPTION("Ricoh RN5T618 MFD driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/rtsx_pcr.c b/drivers/mfd/rtsx_pcr.c
index d01b8c249231..f2643c221d34 100644
--- a/drivers/mfd/rtsx_pcr.c
+++ b/drivers/mfd/rtsx_pcr.c
@@ -1197,7 +1197,7 @@ static int rtsx_pci_probe(struct pci_dev *pcidev,
 	pcr->msi_en = msi_en;
 	if (pcr->msi_en) {
 		ret = pci_enable_msi(pcidev);
-		if (ret < 0)
+		if (ret)
 			pcr->msi_en = false;
 	}
 
diff --git a/drivers/mfd/rtsx_usb.c b/drivers/mfd/rtsx_usb.c
index 71f387ce8cbd..9cf98d142d9a 100644
--- a/drivers/mfd/rtsx_usb.c
+++ b/drivers/mfd/rtsx_usb.c
@@ -684,7 +684,7 @@ static int rtsx_usb_suspend(struct usb_interface *intf, pm_message_t message)
 	struct rtsx_ucr *ucr =
 		(struct rtsx_ucr *)usb_get_intfdata(intf);
 
-	dev_dbg(&intf->dev, "%s called with pm message 0x%04u\n",
+	dev_dbg(&intf->dev, "%s called with pm message 0x%04x\n",
 			__func__, message.event);
 
 	/*
diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index 81e6d0932bf0..6ce6e6200359 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -514,9 +514,9 @@ unsigned long sm501_set_clock(struct device *dev,
 	unsigned long mode = smc501_readl(sm->regs + SM501_POWER_MODE_CONTROL);
 	unsigned long gate = smc501_readl(sm->regs + SM501_CURRENT_GATE);
 	unsigned long clock = smc501_readl(sm->regs + SM501_CURRENT_CLOCK);
-	unsigned char reg;
 	unsigned int pll_reg = 0;
 	unsigned long sm501_freq; /* the actual frequency achieved */
+	u64 reg;
 
 	struct sm501_clock to;
 
@@ -1047,7 +1047,6 @@ static int sm501_register_gpio(struct sm501_devdata *sm)
 	struct sm501_gpio *gpio = &sm->gpio;
 	resource_size_t iobase = sm->io_res->start + SM501_GPIO;
 	int ret;
-	int tmp;
 
 	dev_dbg(sm->dev, "registering gpio block %08llx\n",
 		(unsigned long long)iobase);
@@ -1086,11 +1085,7 @@ static int sm501_register_gpio(struct sm501_devdata *sm)
 	return 0;
 
  err_low_chip:
-	tmp = gpiochip_remove(&gpio->low.gpio);
-	if (tmp) {
-		dev_err(sm->dev, "cannot remove low chip, cannot tidy up\n");
-		return ret;
-	}
+	gpiochip_remove(&gpio->low.gpio);
 
  err_mapped:
 	iounmap(gpio->regs);
@@ -1105,18 +1100,12 @@ static int sm501_register_gpio(struct sm501_devdata *sm)
 static void sm501_gpio_remove(struct sm501_devdata *sm)
 {
 	struct sm501_gpio *gpio = &sm->gpio;
-	int ret;
 
 	if (!sm->gpio.registered)
 		return;
 
-	ret = gpiochip_remove(&gpio->low.gpio);
-	if (ret)
-		dev_err(sm->dev, "cannot remove low chip, cannot tidy up\n");
-
-	ret = gpiochip_remove(&gpio->high.gpio);
-	if (ret)
-		dev_err(sm->dev, "cannot remove high chip, cannot tidy up\n");
+	gpiochip_remove(&gpio->low.gpio);
+	gpiochip_remove(&gpio->high.gpio);
 
 	iounmap(gpio->regs);
 	release_resource(gpio->regs_res);
diff --git a/drivers/mfd/stmpe.c b/drivers/mfd/stmpe.c
index 02a17c388e87..e2f9df1c0c36 100644
--- a/drivers/mfd/stmpe.c
+++ b/drivers/mfd/stmpe.c
@@ -249,7 +249,7 @@ int stmpe_set_altfunc(struct stmpe *stmpe, u32 pins, enum stmpe_block block)
 	int af_bits = variant->af_bits;
 	int numregs = DIV_ROUND_UP(stmpe->num_gpios * af_bits, 8);
 	int mask = (1 << af_bits) - 1;
-	u8 regs[numregs];
+	u8 regs[8];
 	int af, afperreg, ret;
 
 	if (!variant->get_altfunc)
@@ -854,7 +854,7 @@ static irqreturn_t stmpe_irq(int irq, void *data)
 	struct stmpe_variant_info *variant = stmpe->variant;
 	int num = DIV_ROUND_UP(variant->num_irqs, 8);
 	u8 israddr;
-	u8 isr[num];
+	u8 isr[3];
 	int ret;
 	int i;
 
@@ -1122,7 +1122,12 @@ static void stmpe_of_probe(struct stmpe_platform_data *pdata,
 	if (pdata->id < 0)
 		pdata->id = -1;
 
-	pdata->irq_trigger = IRQF_TRIGGER_NONE;
+	pdata->irq_gpio = of_get_named_gpio_flags(np, "irq-gpio", 0,
+				&pdata->irq_trigger);
+	if (gpio_is_valid(pdata->irq_gpio))
+		pdata->irq_over_gpio = 1;
+	else
+		pdata->irq_trigger = IRQF_TRIGGER_NONE;
 
 	of_property_read_u32(np, "st,autosleep-timeout",
 			&pdata->autosleep_timeout);
diff --git a/drivers/mfd/tc6393xb.c b/drivers/mfd/tc6393xb.c
index 11c19e538551..4fac16bcd732 100644
--- a/drivers/mfd/tc6393xb.c
+++ b/drivers/mfd/tc6393xb.c
@@ -607,7 +607,7 @@ static int tc6393xb_probe(struct platform_device *dev)
 	struct tc6393xb_platform_data *tcpd = dev_get_platdata(&dev->dev);
 	struct tc6393xb *tc6393xb;
 	struct resource *iomem, *rscr;
-	int ret, temp;
+	int ret;
 
 	iomem = platform_get_resource(dev, IORESOURCE_MEM, 0);
 	if (!iomem)
@@ -714,7 +714,7 @@ err_setup:
 
 err_gpio_add:
 	if (tc6393xb->gpio.base != -1)
-		temp = gpiochip_remove(&tc6393xb->gpio);
+		gpiochip_remove(&tc6393xb->gpio);
 	tcpd->disable(dev);
 err_enable:
 	clk_disable(tc6393xb->clk);
@@ -744,13 +744,8 @@ static int tc6393xb_remove(struct platform_device *dev)
 
 	tc6393xb_detach_irq(dev);
 
-	if (tc6393xb->gpio.base != -1) {
-		ret = gpiochip_remove(&tc6393xb->gpio);
-		if (ret) {
-			dev_err(&dev->dev, "Can't remove gpio chip: %d\n", ret);
-			return ret;
-		}
-	}
+	if (tc6393xb->gpio.base != -1)
+		gpiochip_remove(&tc6393xb->gpio);
 
 	ret = tcpd->disable(dev);
 	clk_disable(tc6393xb->clk);
diff --git a/drivers/mfd/ti_am335x_tscadc.c b/drivers/mfd/ti_am335x_tscadc.c
index dd4bf5816221..d877e777cce6 100644
--- a/drivers/mfd/ti_am335x_tscadc.c
+++ b/drivers/mfd/ti_am335x_tscadc.c
@@ -53,11 +53,11 @@ void am335x_tsc_se_set_cache(struct ti_tscadc_dev *tsadc, u32 val)
 	unsigned long flags;
 
 	spin_lock_irqsave(&tsadc->reg_lock, flags);
-	tsadc->reg_se_cache = val;
+	tsadc->reg_se_cache |= val;
 	if (tsadc->adc_waiting)
 		wake_up(&tsadc->reg_se_wait);
 	else if (!tsadc->adc_in_use)
-		tscadc_writel(tsadc, REG_SE, val);
+		tscadc_writel(tsadc, REG_SE, tsadc->reg_se_cache);
 
 	spin_unlock_irqrestore(&tsadc->reg_lock, flags);
 }
@@ -96,6 +96,7 @@ static void am335x_tscadc_need_adc(struct ti_tscadc_dev *tsadc)
 void am335x_tsc_se_set_once(struct ti_tscadc_dev *tsadc, u32 val)
 {
 	spin_lock_irq(&tsadc->reg_lock);
+	tsadc->reg_se_cache |= val;
 	am335x_tscadc_need_adc(tsadc);
 
 	tscadc_writel(tsadc, REG_SE, val);
@@ -241,18 +242,20 @@ static	int ti_tscadc_probe(struct platform_device *pdev)
 	tscadc_writel(tscadc, REG_CLKDIV, tscadc->clk_div);
 
 	/* Set the control register bits */
-	ctrl = CNTRLREG_STEPCONFIGWRT |
-			CNTRLREG_STEPID;
-	if (tsc_wires > 0)
-		ctrl |= CNTRLREG_4WIRE | CNTRLREG_TSCENB;
+	ctrl = CNTRLREG_STEPCONFIGWRT |	CNTRLREG_STEPID;
 	tscadc_writel(tscadc, REG_CTRL, ctrl);
 
 	/* Set register bits for Idle Config Mode */
-	if (tsc_wires > 0)
+	if (tsc_wires > 0) {
+		tscadc->tsc_wires = tsc_wires;
+		if (tsc_wires == 5)
+			ctrl |= CNTRLREG_5WIRE | CNTRLREG_TSCENB;
+		else
+			ctrl |= CNTRLREG_4WIRE | CNTRLREG_TSCENB;
 		tscadc_idle_config(tscadc);
+	}
 
 	/* Enable the TSC module enable bit */
-	ctrl = tscadc_readl(tscadc, REG_CTRL);
 	ctrl |= CNTRLREG_TSCSSENB;
 	tscadc_writel(tscadc, REG_CTRL, ctrl);
 
@@ -324,21 +327,23 @@ static int tscadc_suspend(struct device *dev)
 static int tscadc_resume(struct device *dev)
 {
 	struct ti_tscadc_dev	*tscadc_dev = dev_get_drvdata(dev);
-	unsigned int restore, ctrl;
+	u32 ctrl;
 
 	pm_runtime_get_sync(dev);
 
 	/* context restore */
 	ctrl = CNTRLREG_STEPCONFIGWRT |	CNTRLREG_STEPID;
-	if (tscadc_dev->tsc_cell != -1)
-		ctrl |= CNTRLREG_TSCENB | CNTRLREG_4WIRE;
 	tscadc_writel(tscadc_dev, REG_CTRL, ctrl);
 
-	if (tscadc_dev->tsc_cell != -1)
+	if (tscadc_dev->tsc_cell != -1) {
+		if (tscadc_dev->tsc_wires == 5)
+			ctrl |= CNTRLREG_5WIRE | CNTRLREG_TSCENB;
+		else
+			ctrl |= CNTRLREG_4WIRE | CNTRLREG_TSCENB;
 		tscadc_idle_config(tscadc_dev);
-	restore = tscadc_readl(tscadc_dev, REG_CTRL);
-	tscadc_writel(tscadc_dev, REG_CTRL,
-			(restore | CNTRLREG_TSCSSENB));
+	}
+	ctrl |= CNTRLREG_TSCSSENB;
+	tscadc_writel(tscadc_dev, REG_CTRL, ctrl);
 
 	tscadc_writel(tscadc_dev, REG_CLKDIV, tscadc_dev->clk_div);
 
diff --git a/drivers/mfd/tps65217.c b/drivers/mfd/tps65217.c
index 3cc4c7084b92..a8ee52c95f2f 100644
--- a/drivers/mfd/tps65217.c
+++ b/drivers/mfd/tps65217.c
@@ -146,6 +146,8 @@ EXPORT_SYMBOL_GPL(tps65217_clear_bits);
 static struct regmap_config tps65217_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
+
+	.max_register = TPS65217_REG_MAX,
 };
 
 static const struct of_device_id tps65217_of_match[] = {
diff --git a/drivers/mfd/tps65910.c b/drivers/mfd/tps65910.c
index f243e75d28f3..7612d89850dd 100644
--- a/drivers/mfd/tps65910.c
+++ b/drivers/mfd/tps65910.c
@@ -486,6 +486,11 @@ static int tps65910_i2c_probe(struct i2c_client *i2c,
 	tps65910->i2c_client = i2c;
 	tps65910->id = chip_id;
 
+	/* Work around silicon erratum SWCZ010: the tps65910 may miss the
+	 * first I2C transfer. So issue a dummy transfer before the first
+	 * real transfer.
+	 */
+	i2c_master_send(i2c, "", 1);
 	tps65910->regmap = devm_regmap_init_i2c(i2c, &tps65910_regmap_config);
 	if (IS_ERR(tps65910->regmap)) {
 		ret = PTR_ERR(tps65910->regmap);
diff --git a/drivers/mfd/twl4030-irq.c b/drivers/mfd/twl4030-irq.c
index b1dabba763cf..1b772ef761cb 100644
--- a/drivers/mfd/twl4030-irq.c
+++ b/drivers/mfd/twl4030-irq.c
@@ -396,13 +396,17 @@ static int twl4030_init_sih_modules(unsigned line)
 			status = twl_i2c_read(sih->module, rxbuf,
 				sih->mask[line].isr_offset, sih->bytes_ixr);
 			if (status < 0)
-				pr_err("twl4030: err %d initializing %s %s\n",
+				pr_warn("twl4030: err %d initializing %s %s\n",
 					status, sih->name, "ISR");
 
-			if (!sih->set_cor)
+			if (!sih->set_cor) {
 				status = twl_i2c_write(sih->module, buf,
 					sih->mask[line].isr_offset,
 					sih->bytes_ixr);
+				if (status < 0)
+					pr_warn("twl4030: write failed: %d\n",
+						status);
+			}
 			/*
 			 * else COR=1 means read sufficed.
 			 * (for most SIH modules...)
diff --git a/drivers/mfd/twl4030-power.c b/drivers/mfd/twl4030-power.c
index 3bc969a5916b..cf92a6d1c532 100644
--- a/drivers/mfd/twl4030-power.c
+++ b/drivers/mfd/twl4030-power.c
@@ -627,6 +627,9 @@ static bool twl4030_power_use_poweroff(const struct twl4030_power_data *pdata,
 	if (pdata && pdata->use_poweroff)
 		return true;
 
+	if (of_property_read_bool(node, "ti,system-power-controller"))
+		return true;
+
 	if (of_property_read_bool(node, "ti,use_poweroff"))
 		return true;
 
@@ -724,24 +727,24 @@ static struct twl4030_script *omap3_idle_scripts[] = {
  * above.
  */
 static struct twl4030_resconfig omap3_idle_rconfig[] = {
-	TWL_REMAP_SLEEP(RES_VAUX1, DEV_GRP_NULL, 0, 0),
-	TWL_REMAP_SLEEP(RES_VAUX2, DEV_GRP_NULL, 0, 0),
-	TWL_REMAP_SLEEP(RES_VAUX3, DEV_GRP_NULL, 0, 0),
-	TWL_REMAP_SLEEP(RES_VAUX4, DEV_GRP_NULL, 0, 0),
-	TWL_REMAP_SLEEP(RES_VMMC1, DEV_GRP_NULL, 0, 0),
-	TWL_REMAP_SLEEP(RES_VMMC2, DEV_GRP_NULL, 0, 0),
+	TWL_REMAP_SLEEP(RES_VAUX1, TWL4030_RESCONFIG_UNDEF, 0, 0),
+	TWL_REMAP_SLEEP(RES_VAUX2, TWL4030_RESCONFIG_UNDEF, 0, 0),
+	TWL_REMAP_SLEEP(RES_VAUX3, TWL4030_RESCONFIG_UNDEF, 0, 0),
+	TWL_REMAP_SLEEP(RES_VAUX4, TWL4030_RESCONFIG_UNDEF, 0, 0),
+	TWL_REMAP_SLEEP(RES_VMMC1, TWL4030_RESCONFIG_UNDEF, 0, 0),
+	TWL_REMAP_SLEEP(RES_VMMC2, TWL4030_RESCONFIG_UNDEF, 0, 0),
 	TWL_REMAP_OFF(RES_VPLL1, DEV_GRP_P1, 3, 1),
 	TWL_REMAP_SLEEP(RES_VPLL2, DEV_GRP_P1, 0, 0),
-	TWL_REMAP_SLEEP(RES_VSIM, DEV_GRP_NULL, 0, 0),
-	TWL_REMAP_SLEEP(RES_VDAC, DEV_GRP_NULL, 0, 0),
+	TWL_REMAP_SLEEP(RES_VSIM, TWL4030_RESCONFIG_UNDEF, 0, 0),
+	TWL_REMAP_SLEEP(RES_VDAC, TWL4030_RESCONFIG_UNDEF, 0, 0),
 	TWL_REMAP_SLEEP(RES_VINTANA1, TWL_DEV_GRP_P123, 1, 2),
 	TWL_REMAP_SLEEP(RES_VINTANA2, TWL_DEV_GRP_P123, 0, 2),
 	TWL_REMAP_SLEEP(RES_VINTDIG, TWL_DEV_GRP_P123, 1, 2),
 	TWL_REMAP_SLEEP(RES_VIO, TWL_DEV_GRP_P123, 2, 2),
 	TWL_REMAP_OFF(RES_VDD1, DEV_GRP_P1, 4, 1),
 	TWL_REMAP_OFF(RES_VDD2, DEV_GRP_P1, 3, 1),
-	TWL_REMAP_SLEEP(RES_VUSB_1V5, DEV_GRP_NULL, 0, 0),
-	TWL_REMAP_SLEEP(RES_VUSB_1V8, DEV_GRP_NULL, 0, 0),
+	TWL_REMAP_SLEEP(RES_VUSB_1V5, TWL4030_RESCONFIG_UNDEF, 0, 0),
+	TWL_REMAP_SLEEP(RES_VUSB_1V8, TWL4030_RESCONFIG_UNDEF, 0, 0),
 	TWL_REMAP_SLEEP(RES_VUSB_3V1, TWL_DEV_GRP_P123, 0, 0),
 	/* Resource #20 USB charge pump skipped */
 	TWL_REMAP_SLEEP(RES_REGEN, TWL_DEV_GRP_P123, 2, 1),
diff --git a/drivers/mfd/twl6040.c b/drivers/mfd/twl6040.c
index f9c06c542a41..9687645162ae 100644
--- a/drivers/mfd/twl6040.c
+++ b/drivers/mfd/twl6040.c
@@ -679,6 +679,7 @@ static int twl6040_probe(struct i2c_client *client,
 	if (twl6040->rev < 0) {
 		dev_err(&client->dev, "Failed to read revision register: %d\n",
 			twl6040->rev);
+		ret = twl6040->rev;
 		goto gpio_err;
 	}
 
diff --git a/drivers/mfd/ucb1x00-core.c b/drivers/mfd/ucb1x00-core.c
index 153d595afaac..58ea9fdd3a15 100644
--- a/drivers/mfd/ucb1x00-core.c
+++ b/drivers/mfd/ucb1x00-core.c
@@ -621,7 +621,6 @@ static void ucb1x00_remove(struct mcp *mcp)
 	struct ucb1x00_plat_data *pdata = mcp->attached_device.platform_data;
 	struct ucb1x00 *ucb = mcp_get_drvdata(mcp);
 	struct list_head *l, *n;
-	int ret;
 
 	mutex_lock(&ucb1x00_mutex);
 	list_del(&ucb->node);
@@ -631,11 +630,8 @@ static void ucb1x00_remove(struct mcp *mcp)
 	}
 	mutex_unlock(&ucb1x00_mutex);
 
-	if (ucb->gpio.base != -1) {
-		ret = gpiochip_remove(&ucb->gpio);
-		if (ret)
-			dev_err(&ucb->dev, "Can't remove gpio chip: %d\n", ret);
-	}
+	if (ucb->gpio.base != -1)
+		gpiochip_remove(&ucb->gpio);
 
 	irq_set_chained_handler(ucb->irq, NULL);
 	irq_free_descs(ucb->irq_base, 16);
diff --git a/drivers/mfd/wm5102-tables.c b/drivers/mfd/wm5102-tables.c
index fb4d4bb0f47d..d6f35bbf795b 100644
--- a/drivers/mfd/wm5102-tables.c
+++ b/drivers/mfd/wm5102-tables.c
@@ -87,6 +87,7 @@ int wm5102_patch(struct arizona *arizona)
 	case 0:
 		wm5102_patch = wm5102_reva_patch;
 		patch_size = ARRAY_SIZE(wm5102_reva_patch);
+		break;
 	default:
 		wm5102_patch = wm5102_revb_patch;
 		patch_size = ARRAY_SIZE(wm5102_revb_patch);
@@ -245,9 +246,6 @@ const struct regmap_irq_chip wm5102_irq = {
 static const struct reg_default wm5102_reg_default[] = {
 	{ 0x00000008, 0x0019 },   /* R8     - Ctrl IF SPI CFG 1 */ 
 	{ 0x00000009, 0x0001 },   /* R9     - Ctrl IF I2C1 CFG 1 */ 
-	{ 0x00000016, 0x0000 },   /* R22    - Write Sequencer Ctrl 0 */ 
-	{ 0x00000017, 0x0000 },   /* R23    - Write Sequencer Ctrl 1 */ 
-	{ 0x00000018, 0x0000 },   /* R24    - Write Sequencer Ctrl 2 */ 
 	{ 0x00000020, 0x0000 },   /* R32    - Tone Generator 1 */ 
 	{ 0x00000021, 0x1000 },   /* R33    - Tone Generator 2 */ 
 	{ 0x00000022, 0x0000 },   /* R34    - Tone Generator 3 */ 
@@ -1016,6 +1014,7 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_WRITE_SEQUENCER_CTRL_0:
 	case ARIZONA_WRITE_SEQUENCER_CTRL_1:
 	case ARIZONA_WRITE_SEQUENCER_CTRL_2:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_3:
 	case ARIZONA_WRITE_SEQUENCER_PROM:
 	case ARIZONA_TONE_GENERATOR_1:
 	case ARIZONA_TONE_GENERATOR_2:
@@ -1060,6 +1059,8 @@ static bool wm5102_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_ASYNC_CLOCK_1:
 	case ARIZONA_ASYNC_SAMPLE_RATE_1:
 	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2_STATUS:
 	case ARIZONA_OUTPUT_SYSTEM_CLOCK:
 	case ARIZONA_OUTPUT_ASYNC_CLOCK:
 	case ARIZONA_RATE_ESTIMATOR_1:
@@ -1880,6 +1881,10 @@ static bool wm5102_volatile_register(struct device *dev, unsigned int reg)
 	switch (reg) {
 	case ARIZONA_SOFTWARE_RESET:
 	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_0:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_2:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_3:
 	case ARIZONA_OUTPUT_STATUS_1:
 	case ARIZONA_RAW_OUTPUT_STATUS_1:
 	case ARIZONA_SLIMBUS_RX_PORT_STATUS:
@@ -1889,8 +1894,13 @@ static bool wm5102_volatile_register(struct device *dev, unsigned int reg)
 	case ARIZONA_SAMPLE_RATE_3_STATUS:
 	case ARIZONA_HAPTICS_STATUS:
 	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2_STATUS:
 	case ARIZONA_FLL1_NCO_TEST_0:
 	case ARIZONA_FLL2_NCO_TEST_0:
+	case ARIZONA_DAC_COMP_1:
+	case ARIZONA_DAC_COMP_2:
+	case ARIZONA_DAC_COMP_3:
+	case ARIZONA_DAC_COMP_4:
 	case ARIZONA_FX_CTRL2:
 	case ARIZONA_INTERRUPT_STATUS_1:
 	case ARIZONA_INTERRUPT_STATUS_2:
diff --git a/drivers/mfd/wm5110-tables.c b/drivers/mfd/wm5110-tables.c
index 9b98ee559188..4642b5b816a0 100644
--- a/drivers/mfd/wm5110-tables.c
+++ b/drivers/mfd/wm5110-tables.c
@@ -666,9 +666,6 @@ static const struct reg_default wm5110_reg_default[] = {
 	{ 0x0000000A, 0x0001 },    /* R10    - Ctrl IF I2C2 CFG 1 */
 	{ 0x0000000B, 0x0036 },    /* R11    - Ctrl IF I2C1 CFG 2 */
 	{ 0x0000000C, 0x0036 },    /* R12    - Ctrl IF I2C2 CFG 2 */
-	{ 0x00000016, 0x0000 },    /* R22    - Write Sequencer Ctrl 0 */
-	{ 0x00000017, 0x0000 },    /* R23    - Write Sequencer Ctrl 1 */
-	{ 0x00000018, 0x0000 },    /* R24    - Write Sequencer Ctrl 2 */
 	{ 0x00000020, 0x0000 },    /* R32    - Tone Generator 1 */
 	{ 0x00000021, 0x1000 },    /* R33    - Tone Generator 2 */
 	{ 0x00000022, 0x0000 },    /* R34    - Tone Generator 3 */
@@ -705,6 +702,7 @@ static const struct reg_default wm5110_reg_default[] = {
 	{ 0x00000104, 0x0011 },    /* R260   - Sample rate 3 */
 	{ 0x00000112, 0x0305 },    /* R274   - Async clock 1 */
 	{ 0x00000113, 0x0011 },    /* R275   - Async sample rate 1 */
+	{ 0x00000114, 0x0011 },    /* R276   - Async sample rate 2 */
 	{ 0x00000149, 0x0000 },    /* R329   - Output system clock */
 	{ 0x0000014A, 0x0000 },    /* R330   - Output async clock */
 	{ 0x00000152, 0x0000 },    /* R338   - Rate Estimator 1 */
@@ -1741,6 +1739,8 @@ static bool wm5110_readable_register(struct device *dev, unsigned int reg)
 	case ARIZONA_ASYNC_CLOCK_1:
 	case ARIZONA_ASYNC_SAMPLE_RATE_1:
 	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2_STATUS:
 	case ARIZONA_OUTPUT_SYSTEM_CLOCK:
 	case ARIZONA_OUTPUT_ASYNC_CLOCK:
 	case ARIZONA_RATE_ESTIMATOR_1:
@@ -2815,11 +2815,15 @@ static bool wm5110_volatile_register(struct device *dev, unsigned int reg)
 	switch (reg) {
 	case ARIZONA_SOFTWARE_RESET:
 	case ARIZONA_DEVICE_REVISION:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_0:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_1:
+	case ARIZONA_WRITE_SEQUENCER_CTRL_2:
 	case ARIZONA_HAPTICS_STATUS:
 	case ARIZONA_SAMPLE_RATE_1_STATUS:
 	case ARIZONA_SAMPLE_RATE_2_STATUS:
 	case ARIZONA_SAMPLE_RATE_3_STATUS:
 	case ARIZONA_ASYNC_SAMPLE_RATE_1_STATUS:
+	case ARIZONA_ASYNC_SAMPLE_RATE_2_STATUS:
 	case ARIZONA_MIC_DETECT_3:
 	case ARIZONA_HEADPHONE_DETECT_2:
 	case ARIZONA_INPUT_ENABLES_STATUS:
diff --git a/drivers/mfd/wm8994-irq.c b/drivers/mfd/wm8994-irq.c
index e74dedda5b55..a14407edbd89 100644
--- a/drivers/mfd/wm8994-irq.c
+++ b/drivers/mfd/wm8994-irq.c
@@ -262,8 +262,10 @@ int wm8994_irq_init(struct wm8994 *wm8994)
 
 	return 0;
 }
+EXPORT_SYMBOL(wm8994_irq_init);
 
 void wm8994_irq_exit(struct wm8994 *wm8994)
 {
 	regmap_del_irq_chip(wm8994->irq, wm8994->irq_data);
 }
+EXPORT_SYMBOL(wm8994_irq_exit);
diff --git a/drivers/mfd/wm8994-regmap.c b/drivers/mfd/wm8994-regmap.c
index 770a25696468..300e9b6a2e96 100644
--- a/drivers/mfd/wm8994-regmap.c
+++ b/drivers/mfd/wm8994-regmap.c
@@ -1252,6 +1252,7 @@ struct regmap_config wm1811_regmap_config = {
 	.volatile_reg = wm1811_volatile_register,
 	.readable_reg = wm1811_readable_register,
 };
+EXPORT_SYMBOL(wm1811_regmap_config);
 
 struct regmap_config wm8994_regmap_config = {
 	.reg_bits = 16,
@@ -1266,6 +1267,7 @@ struct regmap_config wm8994_regmap_config = {
 	.volatile_reg = wm8994_volatile_register,
 	.readable_reg = wm8994_readable_register,
 };
+EXPORT_SYMBOL(wm8994_regmap_config);
 
 struct regmap_config wm8958_regmap_config = {
 	.reg_bits = 16,
@@ -1280,8 +1282,10 @@ struct regmap_config wm8958_regmap_config = {
 	.volatile_reg = wm8958_volatile_register,
 	.readable_reg = wm8958_readable_register,
 };
+EXPORT_SYMBOL(wm8958_regmap_config);
 
 struct regmap_config wm8994_base_regmap_config = {
 	.reg_bits = 16,
 	.val_bits = 16,
 };
+EXPORT_SYMBOL(wm8994_base_regmap_config);
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index b841180c7c74..bbeb4516facf 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -527,4 +527,5 @@ source "drivers/misc/vmw_vmci/Kconfig"
 source "drivers/misc/mic/Kconfig"
 source "drivers/misc/genwqe/Kconfig"
 source "drivers/misc/echo/Kconfig"
+source "drivers/misc/cxl/Kconfig"
 endmenu
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 5497d026e651..7d5c4cd118c4 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -55,3 +55,4 @@ obj-y				+= mic/
 obj-$(CONFIG_GENWQE)		+= genwqe/
 obj-$(CONFIG_ECHO)		+= echo/
 obj-$(CONFIG_VEXPRESS_SYSCFG)	+= vexpress-syscfg.o
+obj-$(CONFIG_CXL_BASE)		+= cxl/
diff --git a/drivers/misc/altera-stapl/altera.c b/drivers/misc/altera-stapl/altera.c
index 24272e022bec..bca2630d006f 100644
--- a/drivers/misc/altera-stapl/altera.c
+++ b/drivers/misc/altera-stapl/altera.c
@@ -454,7 +454,7 @@ exit_done:
 
 				name = &p[str_table + name_id];
 
-				if (strnicmp(aconf->action, name, strlen(name)) == 0) {
+				if (strncasecmp(aconf->action, name, strlen(name)) == 0) {
 					action_found = 1;
 					current_proc =
 						get_unaligned_be32(&p[action_table +
@@ -2176,7 +2176,7 @@ static int altera_get_note(u8 *p, s32 program_size,
 			key_ptr = &p[note_strings +
 					get_unaligned_be32(
 					&p[note_table + (8 * i)])];
-			if ((strnicmp(key, key_ptr, strlen(key_ptr)) == 0) &&
+			if ((strncasecmp(key, key_ptr, strlen(key_ptr)) == 0) &&
 						(key != NULL)) {
 				status = 0;
 
diff --git a/drivers/misc/atmel_tclib.c b/drivers/misc/atmel_tclib.c
index c8d8e38d0d8a..0ca05c3ec8d6 100644
--- a/drivers/misc/atmel_tclib.c
+++ b/drivers/misc/atmel_tclib.c
@@ -35,60 +35,31 @@ static LIST_HEAD(tc_list);
 /**
  * atmel_tc_alloc - allocate a specified TC block
  * @block: which block to allocate
- * @name: name to be associated with the iomem resource
  *
  * Caller allocates a block.  If it is available, a pointer to a
  * pre-initialized struct atmel_tc is returned. The caller can access
  * the registers directly through the "regs" field.
  */
-struct atmel_tc *atmel_tc_alloc(unsigned block, const char *name)
+struct atmel_tc *atmel_tc_alloc(unsigned block)
 {
 	struct atmel_tc		*tc;
 	struct platform_device	*pdev = NULL;
-	struct resource		*r;
-	size_t			size;
 
 	spin_lock(&tc_list_lock);
 	list_for_each_entry(tc, &tc_list, node) {
-		if (tc->pdev->dev.of_node) {
-			if (of_alias_get_id(tc->pdev->dev.of_node, "tcb")
-					== block) {
-				pdev = tc->pdev;
-				break;
-			}
-		} else if (tc->pdev->id == block) {
+		if (tc->allocated)
+			continue;
+
+		if ((tc->pdev->dev.of_node && tc->id == block) ||
+		    (tc->pdev->id == block)) {
 			pdev = tc->pdev;
+			tc->allocated = true;
 			break;
 		}
 	}
-
-	if (!pdev || tc->iomem)
-		goto fail;
-
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!r)
-		goto fail;
-
-	size = resource_size(r);
-	r = request_mem_region(r->start, size, name);
-	if (!r)
-		goto fail;
-
-	tc->regs = ioremap(r->start, size);
-	if (!tc->regs)
-		goto fail_ioremap;
-
-	tc->iomem = r;
-
-out:
 	spin_unlock(&tc_list_lock);
-	return tc;
 
-fail_ioremap:
-	release_mem_region(r->start, size);
-fail:
-	tc = NULL;
-	goto out;
+	return pdev ? tc : NULL;
 }
 EXPORT_SYMBOL_GPL(atmel_tc_alloc);
 
@@ -96,19 +67,14 @@ EXPORT_SYMBOL_GPL(atmel_tc_alloc);
  * atmel_tc_free - release a specified TC block
  * @tc: Timer/counter block that was returned by atmel_tc_alloc()
  *
- * This reverses the effect of atmel_tc_alloc(), unmapping the I/O
- * registers, invalidating the resource returned by that routine and
- * making the TC available to other drivers.
+ * This reverses the effect of atmel_tc_alloc(), invalidating the resource
+ * returned by that routine and making the TC available to other drivers.
  */
 void atmel_tc_free(struct atmel_tc *tc)
 {
 	spin_lock(&tc_list_lock);
-	if (tc->regs) {
-		iounmap(tc->regs);
-		release_mem_region(tc->iomem->start, resource_size(tc->iomem));
-		tc->regs = NULL;
-		tc->iomem = NULL;
-	}
+	if (tc->allocated)
+		tc->allocated = false;
 	spin_unlock(&tc_list_lock);
 }
 EXPORT_SYMBOL_GPL(atmel_tc_free);
@@ -142,25 +108,27 @@ static int __init tc_probe(struct platform_device *pdev)
 	struct atmel_tc *tc;
 	struct clk	*clk;
 	int		irq;
-
-	if (!platform_get_resource(pdev, IORESOURCE_MEM, 0))
-		return -EINVAL;
+	struct resource	*r;
+	unsigned int	i;
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
 		return -EINVAL;
 
-	tc = kzalloc(sizeof(struct atmel_tc), GFP_KERNEL);
+	tc = devm_kzalloc(&pdev->dev, sizeof(struct atmel_tc), GFP_KERNEL);
 	if (!tc)
 		return -ENOMEM;
 
 	tc->pdev = pdev;
 
-	clk = clk_get(&pdev->dev, "t0_clk");
-	if (IS_ERR(clk)) {
-		kfree(tc);
-		return -EINVAL;
-	}
+	clk = devm_clk_get(&pdev->dev, "t0_clk");
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	tc->regs = devm_ioremap_resource(&pdev->dev, r);
+	if (IS_ERR(tc->regs))
+		return PTR_ERR(tc->regs);
 
 	/* Now take SoC information if available */
 	if (pdev->dev.of_node) {
@@ -168,13 +136,17 @@ static int __init tc_probe(struct platform_device *pdev)
 		match = of_match_node(atmel_tcb_dt_ids, pdev->dev.of_node);
 		if (match)
 			tc->tcb_config = match->data;
+
+		tc->id = of_alias_get_id(tc->pdev->dev.of_node, "tcb");
+	} else {
+		tc->id = pdev->id;
 	}
 
 	tc->clk[0] = clk;
-	tc->clk[1] = clk_get(&pdev->dev, "t1_clk");
+	tc->clk[1] = devm_clk_get(&pdev->dev, "t1_clk");
 	if (IS_ERR(tc->clk[1]))
 		tc->clk[1] = clk;
-	tc->clk[2] = clk_get(&pdev->dev, "t2_clk");
+	tc->clk[2] = devm_clk_get(&pdev->dev, "t2_clk");
 	if (IS_ERR(tc->clk[2]))
 		tc->clk[2] = clk;
 
@@ -186,18 +158,33 @@ static int __init tc_probe(struct platform_device *pdev)
 	if (tc->irq[2] < 0)
 		tc->irq[2] = irq;
 
+	for (i = 0; i < 3; i++)
+		writel(ATMEL_TC_ALL_IRQ, tc->regs + ATMEL_TC_REG(i, IDR));
+
 	spin_lock(&tc_list_lock);
 	list_add_tail(&tc->node, &tc_list);
 	spin_unlock(&tc_list_lock);
 
+	platform_set_drvdata(pdev, tc);
+
 	return 0;
 }
 
+static void tc_shutdown(struct platform_device *pdev)
+{
+	int i;
+	struct atmel_tc *tc = platform_get_drvdata(pdev);
+
+	for (i = 0; i < 3; i++)
+		writel(ATMEL_TC_ALL_IRQ, tc->regs + ATMEL_TC_REG(i, IDR));
+}
+
 static struct platform_driver tc_driver = {
 	.driver = {
 		.name	= "atmel_tcb",
 		.of_match_table	= of_match_ptr(atmel_tcb_dt_ids),
 	},
+	.shutdown = tc_shutdown,
 };
 
 static int __init tc_init(void)
diff --git a/drivers/misc/carma/carma-fpga-program.c b/drivers/misc/carma/carma-fpga-program.c
index 7be89832db19..339b252fcedd 100644
--- a/drivers/misc/carma/carma-fpga-program.c
+++ b/drivers/misc/carma/carma-fpga-program.c
@@ -16,6 +16,7 @@
 #include <linux/completion.h>
 #include <linux/miscdevice.h>
 #include <linux/dmaengine.h>
+#include <linux/fsldma.h>
 #include <linux/interrupt.h>
 #include <linux/highmem.h>
 #include <linux/kernel.h>
@@ -518,23 +519,22 @@ static noinline int fpga_program_dma(struct fpga_dev *priv)
 	config.direction = DMA_MEM_TO_DEV;
 	config.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
 	config.dst_maxburst = fpga_fifo_size(priv->regs) / 2 / 4;
-	ret = chan->device->device_control(chan, DMA_SLAVE_CONFIG,
-					   (unsigned long)&config);
+	ret = dmaengine_slave_config(chan, &config);
 	if (ret) {
 		dev_err(priv->dev, "DMA slave configuration failed\n");
 		goto out_dma_unmap;
 	}
 
-	ret = chan->device->device_control(chan, FSLDMA_EXTERNAL_START, 1);
+	ret = fsl_dma_external_start(chan, 1)
 	if (ret) {
 		dev_err(priv->dev, "DMA external control setup failed\n");
 		goto out_dma_unmap;
 	}
 
 	/* setup and submit the DMA transaction */
-	tx = chan->device->device_prep_dma_sg(chan,
-					      table.sgl, num_pages,
-					      vb->sglist, vb->sglen, 0);
+
+	tx = dmaengine_prep_dma_sg(chan, table.sgl, num_pages,
+			vb->sglist, vb->sglen, 0);
 	if (!tx) {
 		dev_err(priv->dev, "Unable to prep DMA transaction\n");
 		ret = -ENOMEM;
@@ -749,13 +749,8 @@ static ssize_t fpga_read(struct file *filp, char __user *buf, size_t count,
 			 loff_t *f_pos)
 {
 	struct fpga_dev *priv = filp->private_data;
-
-	count = min_t(size_t, priv->bytes - *f_pos, count);
-	if (copy_to_user(buf, priv->vb.vaddr + *f_pos, count))
-		return -EFAULT;
-
-	*f_pos += count;
-	return count;
+	return simple_read_from_buffer(buf, count, ppos,
+				       priv->vb.vaddr, priv->bytes);
 }
 
 static loff_t fpga_llseek(struct file *filp, loff_t offset, int origin)
@@ -767,26 +762,7 @@ static loff_t fpga_llseek(struct file *filp, loff_t offset, int origin)
 	if ((filp->f_flags & O_ACCMODE) != O_RDONLY)
 		return -EINVAL;
 
-	switch (origin) {
-	case SEEK_SET: /* seek relative to the beginning of the file */
-		newpos = offset;
-		break;
-	case SEEK_CUR: /* seek relative to current position in the file */
-		newpos = filp->f_pos + offset;
-		break;
-	case SEEK_END: /* seek relative to the end of the file */
-		newpos = priv->fw_size - offset;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	/* check for sanity */
-	if (newpos > priv->fw_size)
-		return -EINVAL;
-
-	filp->f_pos = newpos;
-	return newpos;
+	return fixed_size_llseek(file, offset, origin, priv->fw_size);
 }
 
 static const struct file_operations fpga_fops = {
diff --git a/drivers/misc/cxl/Kconfig b/drivers/misc/cxl/Kconfig
new file mode 100644
index 000000000000..a990b39b4dfb
--- /dev/null
+++ b/drivers/misc/cxl/Kconfig
@@ -0,0 +1,25 @@
+#
+# IBM Coherent Accelerator (CXL) compatible devices
+#
+
+config CXL_BASE
+	bool
+	default n
+	select PPC_COPRO_BASE
+
+config CXL
+	tristate "Support for IBM Coherent Accelerators (CXL)"
+	depends on PPC_POWERNV && PCI_MSI
+	select CXL_BASE
+	default m
+	help
+	  Select this option to enable driver support for IBM Coherent
+	  Accelerators (CXL).  CXL is otherwise known as Coherent Accelerator
+	  Processor Interface (CAPI).  CAPI allows accelerators in FPGAs to be
+	  coherently attached to a CPU via an MMU.  This driver enables
+	  userspace programs to access these accelerators via /dev/cxl/afuM.N
+	  devices.
+
+	  CAPI adapters are found in POWER8 based systems.
+
+	  If unsure, say N.
diff --git a/drivers/misc/cxl/Makefile b/drivers/misc/cxl/Makefile
new file mode 100644
index 000000000000..165e98fef2c2
--- /dev/null
+++ b/drivers/misc/cxl/Makefile
@@ -0,0 +1,3 @@
+cxl-y				+= main.o file.o irq.o fault.o native.o context.o sysfs.o debugfs.o pci.o
+obj-$(CONFIG_CXL)		+= cxl.o
+obj-$(CONFIG_CXL_BASE)		+= base.o
diff --git a/drivers/misc/cxl/base.c b/drivers/misc/cxl/base.c
new file mode 100644
index 000000000000..0654ad83675e
--- /dev/null
+++ b/drivers/misc/cxl/base.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/rcupdate.h>
+#include <asm/errno.h>
+#include <misc/cxl.h>
+#include "cxl.h"
+
+/* protected by rcu */
+static struct cxl_calls *cxl_calls;
+
+atomic_t cxl_use_count = ATOMIC_INIT(0);
+EXPORT_SYMBOL(cxl_use_count);
+
+#ifdef CONFIG_CXL_MODULE
+
+static inline struct cxl_calls *cxl_calls_get(void)
+{
+	struct cxl_calls *calls = NULL;
+
+	rcu_read_lock();
+	calls = rcu_dereference(cxl_calls);
+	if (calls && !try_module_get(calls->owner))
+		calls = NULL;
+	rcu_read_unlock();
+
+	return calls;
+}
+
+static inline void cxl_calls_put(struct cxl_calls *calls)
+{
+	BUG_ON(calls != cxl_calls);
+
+	/* we don't need to rcu this, as we hold a reference to the module */
+	module_put(cxl_calls->owner);
+}
+
+#else /* !defined CONFIG_CXL_MODULE */
+
+static inline struct cxl_calls *cxl_calls_get(void)
+{
+	return cxl_calls;
+}
+
+static inline void cxl_calls_put(struct cxl_calls *calls) { }
+
+#endif /* CONFIG_CXL_MODULE */
+
+void cxl_slbia(struct mm_struct *mm)
+{
+	struct cxl_calls *calls;
+
+	calls = cxl_calls_get();
+	if (!calls)
+		return;
+
+	if (cxl_ctx_in_use())
+	    calls->cxl_slbia(mm);
+
+	cxl_calls_put(calls);
+}
+
+int register_cxl_calls(struct cxl_calls *calls)
+{
+	if (cxl_calls)
+		return -EBUSY;
+
+	rcu_assign_pointer(cxl_calls, calls);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(register_cxl_calls);
+
+void unregister_cxl_calls(struct cxl_calls *calls)
+{
+	BUG_ON(cxl_calls->owner != calls->owner);
+	RCU_INIT_POINTER(cxl_calls, NULL);
+	synchronize_rcu();
+}
+EXPORT_SYMBOL_GPL(unregister_cxl_calls);
diff --git a/drivers/misc/cxl/context.c b/drivers/misc/cxl/context.c
new file mode 100644
index 000000000000..cca472109135
--- /dev/null
+++ b/drivers/misc/cxl/context.c
@@ -0,0 +1,193 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/bitmap.h>
+#include <linux/sched.h>
+#include <linux/pid.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/debugfs.h>
+#include <linux/slab.h>
+#include <linux/idr.h>
+#include <asm/cputable.h>
+#include <asm/current.h>
+#include <asm/copro.h>
+
+#include "cxl.h"
+
+/*
+ * Allocates space for a CXL context.
+ */
+struct cxl_context *cxl_context_alloc(void)
+{
+	return kzalloc(sizeof(struct cxl_context), GFP_KERNEL);
+}
+
+/*
+ * Initialises a CXL context.
+ */
+int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master)
+{
+	int i;
+
+	spin_lock_init(&ctx->sste_lock);
+	ctx->afu = afu;
+	ctx->master = master;
+	ctx->pid = NULL; /* Set in start work ioctl */
+
+	/*
+	 * Allocate the segment table before we put it in the IDR so that we
+	 * can always access it when dereferenced from IDR. For the same
+	 * reason, the segment table is only destroyed after the context is
+	 * removed from the IDR.  Access to this in the IOCTL is protected by
+	 * Linux filesytem symantics (can't IOCTL until open is complete).
+	 */
+	i = cxl_alloc_sst(ctx);
+	if (i)
+		return i;
+
+	INIT_WORK(&ctx->fault_work, cxl_handle_fault);
+
+	init_waitqueue_head(&ctx->wq);
+	spin_lock_init(&ctx->lock);
+
+	ctx->irq_bitmap = NULL;
+	ctx->pending_irq = false;
+	ctx->pending_fault = false;
+	ctx->pending_afu_err = false;
+
+	/*
+	 * When we have to destroy all contexts in cxl_context_detach_all() we
+	 * end up with afu_release_irqs() called from inside a
+	 * idr_for_each_entry(). Hence we need to make sure that anything
+	 * dereferenced from this IDR is ok before we allocate the IDR here.
+	 * This clears out the IRQ ranges to ensure this.
+	 */
+	for (i = 0; i < CXL_IRQ_RANGES; i++)
+		ctx->irqs.range[i] = 0;
+
+	mutex_init(&ctx->status_mutex);
+
+	ctx->status = OPENED;
+
+	/*
+	 * Allocating IDR! We better make sure everything's setup that
+	 * dereferences from it.
+	 */
+	idr_preload(GFP_KERNEL);
+	spin_lock(&afu->contexts_lock);
+	i = idr_alloc(&ctx->afu->contexts_idr, ctx, 0,
+		      ctx->afu->num_procs, GFP_NOWAIT);
+	spin_unlock(&afu->contexts_lock);
+	idr_preload_end();
+	if (i < 0)
+		return i;
+
+	ctx->pe = i;
+	ctx->elem = &ctx->afu->spa[i];
+	ctx->pe_inserted = false;
+	return 0;
+}
+
+/*
+ * Map a per-context mmio space into the given vma.
+ */
+int cxl_context_iomap(struct cxl_context *ctx, struct vm_area_struct *vma)
+{
+	u64 len = vma->vm_end - vma->vm_start;
+	len = min(len, ctx->psn_size);
+
+	if (ctx->afu->current_mode == CXL_MODE_DEDICATED) {
+		vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+		return vm_iomap_memory(vma, ctx->afu->psn_phys, ctx->afu->adapter->ps_size);
+	}
+
+	/* make sure there is a valid per process space for this AFU */
+	if ((ctx->master && !ctx->afu->psa) || (!ctx->afu->pp_psa)) {
+		pr_devel("AFU doesn't support mmio space\n");
+		return -EINVAL;
+	}
+
+	/* Can't mmap until the AFU is enabled */
+	if (!ctx->afu->enabled)
+		return -EBUSY;
+
+	pr_devel("%s: mmio physical: %llx pe: %i master:%i\n", __func__,
+		 ctx->psn_phys, ctx->pe , ctx->master);
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	return vm_iomap_memory(vma, ctx->psn_phys, len);
+}
+
+/*
+ * Detach a context from the hardware. This disables interrupts and doesn't
+ * return until all outstanding interrupts for this context have completed. The
+ * hardware should no longer access *ctx after this has returned.
+ */
+static void __detach_context(struct cxl_context *ctx)
+{
+	enum cxl_context_status status;
+
+	mutex_lock(&ctx->status_mutex);
+	status = ctx->status;
+	ctx->status = CLOSED;
+	mutex_unlock(&ctx->status_mutex);
+	if (status != STARTED)
+		return;
+
+	WARN_ON(cxl_detach_process(ctx));
+	afu_release_irqs(ctx);
+	flush_work(&ctx->fault_work); /* Only needed for dedicated process */
+	wake_up_all(&ctx->wq);
+}
+
+/*
+ * Detach the given context from the AFU. This doesn't actually
+ * free the context but it should stop the context running in hardware
+ * (ie. prevent this context from generating any further interrupts
+ * so that it can be freed).
+ */
+void cxl_context_detach(struct cxl_context *ctx)
+{
+	__detach_context(ctx);
+}
+
+/*
+ * Detach all contexts on the given AFU.
+ */
+void cxl_context_detach_all(struct cxl_afu *afu)
+{
+	struct cxl_context *ctx;
+	int tmp;
+
+	rcu_read_lock();
+	idr_for_each_entry(&afu->contexts_idr, ctx, tmp)
+		/*
+		 * Anything done in here needs to be setup before the IDR is
+		 * created and torn down after the IDR removed
+		 */
+		__detach_context(ctx);
+	rcu_read_unlock();
+}
+
+void cxl_context_free(struct cxl_context *ctx)
+{
+	spin_lock(&ctx->afu->contexts_lock);
+	idr_remove(&ctx->afu->contexts_idr, ctx->pe);
+	spin_unlock(&ctx->afu->contexts_lock);
+	synchronize_rcu();
+
+	free_page((u64)ctx->sstp);
+	ctx->sstp = NULL;
+
+	put_pid(ctx->pid);
+	kfree(ctx);
+}
diff --git a/drivers/misc/cxl/cxl.h b/drivers/misc/cxl/cxl.h
new file mode 100644
index 000000000000..3d2b8677ec8a
--- /dev/null
+++ b/drivers/misc/cxl/cxl.h
@@ -0,0 +1,629 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#ifndef _CXL_H_
+#define _CXL_H_
+
+#include <linux/interrupt.h>
+#include <linux/semaphore.h>
+#include <linux/device.h>
+#include <linux/types.h>
+#include <linux/cdev.h>
+#include <linux/pid.h>
+#include <linux/io.h>
+#include <linux/pci.h>
+#include <asm/cputable.h>
+#include <asm/mmu.h>
+#include <asm/reg.h>
+#include <misc/cxl.h>
+
+#include <uapi/misc/cxl.h>
+
+extern uint cxl_verbose;
+
+#define CXL_TIMEOUT 5
+
+/*
+ * Bump version each time a user API change is made, whether it is
+ * backwards compatible ot not.
+ */
+#define CXL_API_VERSION 1
+#define CXL_API_VERSION_COMPATIBLE 1
+
+/*
+ * Opaque types to avoid accidentally passing registers for the wrong MMIO
+ *
+ * At the end of the day, I'm not married to using typedef here, but it might
+ * (and has!) help avoid bugs like mixing up CXL_PSL_CtxTime and
+ * CXL_PSL_CtxTime_An, or calling cxl_p1n_write instead of cxl_p1_write.
+ *
+ * I'm quite happy if these are changed back to #defines before upstreaming, it
+ * should be little more than a regexp search+replace operation in this file.
+ */
+typedef struct {
+	const int x;
+} cxl_p1_reg_t;
+typedef struct {
+	const int x;
+} cxl_p1n_reg_t;
+typedef struct {
+	const int x;
+} cxl_p2n_reg_t;
+#define cxl_reg_off(reg) \
+	(reg.x)
+
+/* Memory maps. Ref CXL Appendix A */
+
+/* PSL Privilege 1 Memory Map */
+/* Configuration and Control area */
+static const cxl_p1_reg_t CXL_PSL_CtxTime = {0x0000};
+static const cxl_p1_reg_t CXL_PSL_ErrIVTE = {0x0008};
+static const cxl_p1_reg_t CXL_PSL_KEY1    = {0x0010};
+static const cxl_p1_reg_t CXL_PSL_KEY2    = {0x0018};
+static const cxl_p1_reg_t CXL_PSL_Control = {0x0020};
+/* Downloading */
+static const cxl_p1_reg_t CXL_PSL_DLCNTL  = {0x0060};
+static const cxl_p1_reg_t CXL_PSL_DLADDR  = {0x0068};
+
+/* PSL Lookaside Buffer Management Area */
+static const cxl_p1_reg_t CXL_PSL_LBISEL  = {0x0080};
+static const cxl_p1_reg_t CXL_PSL_SLBIE   = {0x0088};
+static const cxl_p1_reg_t CXL_PSL_SLBIA   = {0x0090};
+static const cxl_p1_reg_t CXL_PSL_TLBIE   = {0x00A0};
+static const cxl_p1_reg_t CXL_PSL_TLBIA   = {0x00A8};
+static const cxl_p1_reg_t CXL_PSL_AFUSEL  = {0x00B0};
+
+/* 0x00C0:7EFF Implementation dependent area */
+static const cxl_p1_reg_t CXL_PSL_FIR1      = {0x0100};
+static const cxl_p1_reg_t CXL_PSL_FIR2      = {0x0108};
+static const cxl_p1_reg_t CXL_PSL_VERSION   = {0x0118};
+static const cxl_p1_reg_t CXL_PSL_RESLCKTO  = {0x0128};
+static const cxl_p1_reg_t CXL_PSL_FIR_CNTL  = {0x0148};
+static const cxl_p1_reg_t CXL_PSL_DSNDCTL   = {0x0150};
+static const cxl_p1_reg_t CXL_PSL_SNWRALLOC = {0x0158};
+static const cxl_p1_reg_t CXL_PSL_TRACE     = {0x0170};
+/* 0x7F00:7FFF Reserved PCIe MSI-X Pending Bit Array area */
+/* 0x8000:FFFF Reserved PCIe MSI-X Table Area */
+
+/* PSL Slice Privilege 1 Memory Map */
+/* Configuration Area */
+static const cxl_p1n_reg_t CXL_PSL_SR_An          = {0x00};
+static const cxl_p1n_reg_t CXL_PSL_LPID_An        = {0x08};
+static const cxl_p1n_reg_t CXL_PSL_AMBAR_An       = {0x10};
+static const cxl_p1n_reg_t CXL_PSL_SPOffset_An    = {0x18};
+static const cxl_p1n_reg_t CXL_PSL_ID_An          = {0x20};
+static const cxl_p1n_reg_t CXL_PSL_SERR_An        = {0x28};
+/* Memory Management and Lookaside Buffer Management */
+static const cxl_p1n_reg_t CXL_PSL_SDR_An         = {0x30};
+static const cxl_p1n_reg_t CXL_PSL_AMOR_An        = {0x38};
+/* Pointer Area */
+static const cxl_p1n_reg_t CXL_HAURP_An           = {0x80};
+static const cxl_p1n_reg_t CXL_PSL_SPAP_An        = {0x88};
+static const cxl_p1n_reg_t CXL_PSL_LLCMD_An       = {0x90};
+/* Control Area */
+static const cxl_p1n_reg_t CXL_PSL_SCNTL_An       = {0xA0};
+static const cxl_p1n_reg_t CXL_PSL_CtxTime_An     = {0xA8};
+static const cxl_p1n_reg_t CXL_PSL_IVTE_Offset_An = {0xB0};
+static const cxl_p1n_reg_t CXL_PSL_IVTE_Limit_An  = {0xB8};
+/* 0xC0:FF Implementation Dependent Area */
+static const cxl_p1n_reg_t CXL_PSL_FIR_SLICE_An   = {0xC0};
+static const cxl_p1n_reg_t CXL_AFU_DEBUG_An       = {0xC8};
+static const cxl_p1n_reg_t CXL_PSL_APCALLOC_A     = {0xD0};
+static const cxl_p1n_reg_t CXL_PSL_COALLOC_A      = {0xD8};
+static const cxl_p1n_reg_t CXL_PSL_RXCTL_A        = {0xE0};
+static const cxl_p1n_reg_t CXL_PSL_SLICE_TRACE    = {0xE8};
+
+/* PSL Slice Privilege 2 Memory Map */
+/* Configuration and Control Area */
+static const cxl_p2n_reg_t CXL_PSL_PID_TID_An = {0x000};
+static const cxl_p2n_reg_t CXL_CSRP_An        = {0x008};
+static const cxl_p2n_reg_t CXL_AURP0_An       = {0x010};
+static const cxl_p2n_reg_t CXL_AURP1_An       = {0x018};
+static const cxl_p2n_reg_t CXL_SSTP0_An       = {0x020};
+static const cxl_p2n_reg_t CXL_SSTP1_An       = {0x028};
+static const cxl_p2n_reg_t CXL_PSL_AMR_An     = {0x030};
+/* Segment Lookaside Buffer Management */
+static const cxl_p2n_reg_t CXL_SLBIE_An       = {0x040};
+static const cxl_p2n_reg_t CXL_SLBIA_An       = {0x048};
+static const cxl_p2n_reg_t CXL_SLBI_Select_An = {0x050};
+/* Interrupt Registers */
+static const cxl_p2n_reg_t CXL_PSL_DSISR_An   = {0x060};
+static const cxl_p2n_reg_t CXL_PSL_DAR_An     = {0x068};
+static const cxl_p2n_reg_t CXL_PSL_DSR_An     = {0x070};
+static const cxl_p2n_reg_t CXL_PSL_TFC_An     = {0x078};
+static const cxl_p2n_reg_t CXL_PSL_PEHandle_An = {0x080};
+static const cxl_p2n_reg_t CXL_PSL_ErrStat_An = {0x088};
+/* AFU Registers */
+static const cxl_p2n_reg_t CXL_AFU_Cntl_An    = {0x090};
+static const cxl_p2n_reg_t CXL_AFU_ERR_An     = {0x098};
+/* Work Element Descriptor */
+static const cxl_p2n_reg_t CXL_PSL_WED_An     = {0x0A0};
+/* 0x0C0:FFF Implementation Dependent Area */
+
+#define CXL_PSL_SPAP_Addr 0x0ffffffffffff000ULL
+#define CXL_PSL_SPAP_Size 0x0000000000000ff0ULL
+#define CXL_PSL_SPAP_Size_Shift 4
+#define CXL_PSL_SPAP_V    0x0000000000000001ULL
+
+/****** CXL_PSL_DLCNTL *****************************************************/
+#define CXL_PSL_DLCNTL_D (0x1ull << (63-28))
+#define CXL_PSL_DLCNTL_C (0x1ull << (63-29))
+#define CXL_PSL_DLCNTL_E (0x1ull << (63-30))
+#define CXL_PSL_DLCNTL_S (0x1ull << (63-31))
+#define CXL_PSL_DLCNTL_CE (CXL_PSL_DLCNTL_C | CXL_PSL_DLCNTL_E)
+#define CXL_PSL_DLCNTL_DCES (CXL_PSL_DLCNTL_D | CXL_PSL_DLCNTL_CE | CXL_PSL_DLCNTL_S)
+
+/****** CXL_PSL_SR_An ******************************************************/
+#define CXL_PSL_SR_An_SF  MSR_SF            /* 64bit */
+#define CXL_PSL_SR_An_TA  (1ull << (63-1))  /* Tags active,   GA1: 0 */
+#define CXL_PSL_SR_An_HV  MSR_HV            /* Hypervisor,    GA1: 0 */
+#define CXL_PSL_SR_An_PR  MSR_PR            /* Problem state, GA1: 1 */
+#define CXL_PSL_SR_An_ISL (1ull << (63-53)) /* Ignore Segment Large Page */
+#define CXL_PSL_SR_An_TC  (1ull << (63-54)) /* Page Table secondary hash */
+#define CXL_PSL_SR_An_US  (1ull << (63-56)) /* User state,    GA1: X */
+#define CXL_PSL_SR_An_SC  (1ull << (63-58)) /* Segment Table secondary hash */
+#define CXL_PSL_SR_An_R   MSR_DR            /* Relocate,      GA1: 1 */
+#define CXL_PSL_SR_An_MP  (1ull << (63-62)) /* Master Process */
+#define CXL_PSL_SR_An_LE  (1ull << (63-63)) /* Little Endian */
+
+/****** CXL_PSL_LLCMD_An ****************************************************/
+#define CXL_LLCMD_TERMINATE   0x0001000000000000ULL
+#define CXL_LLCMD_REMOVE      0x0002000000000000ULL
+#define CXL_LLCMD_SUSPEND     0x0003000000000000ULL
+#define CXL_LLCMD_RESUME      0x0004000000000000ULL
+#define CXL_LLCMD_ADD         0x0005000000000000ULL
+#define CXL_LLCMD_UPDATE      0x0006000000000000ULL
+#define CXL_LLCMD_HANDLE_MASK 0x000000000000ffffULL
+
+/****** CXL_PSL_ID_An ****************************************************/
+#define CXL_PSL_ID_An_F	(1ull << (63-31))
+#define CXL_PSL_ID_An_L	(1ull << (63-30))
+
+/****** CXL_PSL_SCNTL_An ****************************************************/
+#define CXL_PSL_SCNTL_An_CR          (0x1ull << (63-15))
+/* Programming Modes: */
+#define CXL_PSL_SCNTL_An_PM_MASK     (0xffffull << (63-31))
+#define CXL_PSL_SCNTL_An_PM_Shared   (0x0000ull << (63-31))
+#define CXL_PSL_SCNTL_An_PM_OS       (0x0001ull << (63-31))
+#define CXL_PSL_SCNTL_An_PM_Process  (0x0002ull << (63-31))
+#define CXL_PSL_SCNTL_An_PM_AFU      (0x0004ull << (63-31))
+#define CXL_PSL_SCNTL_An_PM_AFU_PBT  (0x0104ull << (63-31))
+/* Purge Status (ro) */
+#define CXL_PSL_SCNTL_An_Ps_MASK     (0x3ull << (63-39))
+#define CXL_PSL_SCNTL_An_Ps_Pending  (0x1ull << (63-39))
+#define CXL_PSL_SCNTL_An_Ps_Complete (0x3ull << (63-39))
+/* Purge */
+#define CXL_PSL_SCNTL_An_Pc          (0x1ull << (63-48))
+/* Suspend Status (ro) */
+#define CXL_PSL_SCNTL_An_Ss_MASK     (0x3ull << (63-55))
+#define CXL_PSL_SCNTL_An_Ss_Pending  (0x1ull << (63-55))
+#define CXL_PSL_SCNTL_An_Ss_Complete (0x3ull << (63-55))
+/* Suspend Control */
+#define CXL_PSL_SCNTL_An_Sc          (0x1ull << (63-63))
+
+/* AFU Slice Enable Status (ro) */
+#define CXL_AFU_Cntl_An_ES_MASK     (0x7ull << (63-2))
+#define CXL_AFU_Cntl_An_ES_Disabled (0x0ull << (63-2))
+#define CXL_AFU_Cntl_An_ES_Enabled  (0x4ull << (63-2))
+/* AFU Slice Enable */
+#define CXL_AFU_Cntl_An_E           (0x1ull << (63-3))
+/* AFU Slice Reset status (ro) */
+#define CXL_AFU_Cntl_An_RS_MASK     (0x3ull << (63-5))
+#define CXL_AFU_Cntl_An_RS_Pending  (0x1ull << (63-5))
+#define CXL_AFU_Cntl_An_RS_Complete (0x2ull << (63-5))
+/* AFU Slice Reset */
+#define CXL_AFU_Cntl_An_RA          (0x1ull << (63-7))
+
+/****** CXL_SSTP0/1_An ******************************************************/
+/* These top bits are for the segment that CONTAINS the segment table */
+#define CXL_SSTP0_An_B_SHIFT    SLB_VSID_SSIZE_SHIFT
+#define CXL_SSTP0_An_KS             (1ull << (63-2))
+#define CXL_SSTP0_An_KP             (1ull << (63-3))
+#define CXL_SSTP0_An_N              (1ull << (63-4))
+#define CXL_SSTP0_An_L              (1ull << (63-5))
+#define CXL_SSTP0_An_C              (1ull << (63-6))
+#define CXL_SSTP0_An_TA             (1ull << (63-7))
+#define CXL_SSTP0_An_LP_SHIFT                (63-9)  /* 2 Bits */
+/* And finally, the virtual address & size of the segment table: */
+#define CXL_SSTP0_An_SegTableSize_SHIFT      (63-31) /* 12 Bits */
+#define CXL_SSTP0_An_SegTableSize_MASK \
+	(((1ull << 12) - 1) << CXL_SSTP0_An_SegTableSize_SHIFT)
+#define CXL_SSTP0_An_STVA_U_MASK   ((1ull << (63-49))-1)
+#define CXL_SSTP1_An_STVA_L_MASK (~((1ull << (63-55))-1))
+#define CXL_SSTP1_An_V              (1ull << (63-63))
+
+/****** CXL_PSL_SLBIE_[An] **************************************************/
+/* write: */
+#define CXL_SLBIE_C        PPC_BIT(36)         /* Class */
+#define CXL_SLBIE_SS       PPC_BITMASK(37, 38) /* Segment Size */
+#define CXL_SLBIE_SS_SHIFT PPC_BITLSHIFT(38)
+#define CXL_SLBIE_TA       PPC_BIT(38)         /* Tags Active */
+/* read: */
+#define CXL_SLBIE_MAX      PPC_BITMASK(24, 31)
+#define CXL_SLBIE_PENDING  PPC_BITMASK(56, 63)
+
+/****** Common to all CXL_TLBIA/SLBIA_[An] **********************************/
+#define CXL_TLB_SLB_P          (1ull) /* Pending (read) */
+
+/****** Common to all CXL_TLB/SLB_IA/IE_[An] registers **********************/
+#define CXL_TLB_SLB_IQ_ALL     (0ull) /* Inv qualifier */
+#define CXL_TLB_SLB_IQ_LPID    (1ull) /* Inv qualifier */
+#define CXL_TLB_SLB_IQ_LPIDPID (3ull) /* Inv qualifier */
+
+/****** CXL_PSL_AFUSEL ******************************************************/
+#define CXL_PSL_AFUSEL_A (1ull << (63-55)) /* Adapter wide invalidates affect all AFUs */
+
+/****** CXL_PSL_DSISR_An ****************************************************/
+#define CXL_PSL_DSISR_An_DS (1ull << (63-0))  /* Segment not found */
+#define CXL_PSL_DSISR_An_DM (1ull << (63-1))  /* PTE not found (See also: M) or protection fault */
+#define CXL_PSL_DSISR_An_ST (1ull << (63-2))  /* Segment Table PTE not found */
+#define CXL_PSL_DSISR_An_UR (1ull << (63-3))  /* AURP PTE not found */
+#define CXL_PSL_DSISR_TRANS (CXL_PSL_DSISR_An_DS | CXL_PSL_DSISR_An_DM | CXL_PSL_DSISR_An_ST | CXL_PSL_DSISR_An_UR)
+#define CXL_PSL_DSISR_An_PE (1ull << (63-4))  /* PSL Error (implementation specific) */
+#define CXL_PSL_DSISR_An_AE (1ull << (63-5))  /* AFU Error */
+#define CXL_PSL_DSISR_An_OC (1ull << (63-6))  /* OS Context Warning */
+/* NOTE: Bits 32:63 are undefined if DSISR[DS] = 1 */
+#define CXL_PSL_DSISR_An_M  DSISR_NOHPTE      /* PTE not found */
+#define CXL_PSL_DSISR_An_P  DSISR_PROTFAULT   /* Storage protection violation */
+#define CXL_PSL_DSISR_An_A  (1ull << (63-37)) /* AFU lock access to write through or cache inhibited storage */
+#define CXL_PSL_DSISR_An_S  DSISR_ISSTORE     /* Access was afu_wr or afu_zero */
+#define CXL_PSL_DSISR_An_K  DSISR_KEYFAULT    /* Access not permitted by virtual page class key protection */
+
+/****** CXL_PSL_TFC_An ******************************************************/
+#define CXL_PSL_TFC_An_A  (1ull << (63-28)) /* Acknowledge non-translation fault */
+#define CXL_PSL_TFC_An_C  (1ull << (63-29)) /* Continue (abort transaction) */
+#define CXL_PSL_TFC_An_AE (1ull << (63-30)) /* Restart PSL with address error */
+#define CXL_PSL_TFC_An_R  (1ull << (63-31)) /* Restart PSL transaction */
+
+/* cxl_process_element->software_status */
+#define CXL_PE_SOFTWARE_STATE_V (1ul << (31 -  0)) /* Valid */
+#define CXL_PE_SOFTWARE_STATE_C (1ul << (31 - 29)) /* Complete */
+#define CXL_PE_SOFTWARE_STATE_S (1ul << (31 - 30)) /* Suspend */
+#define CXL_PE_SOFTWARE_STATE_T (1ul << (31 - 31)) /* Terminate */
+
+/* SPA->sw_command_status */
+#define CXL_SPA_SW_CMD_MASK         0xffff000000000000ULL
+#define CXL_SPA_SW_CMD_TERMINATE    0x0001000000000000ULL
+#define CXL_SPA_SW_CMD_REMOVE       0x0002000000000000ULL
+#define CXL_SPA_SW_CMD_SUSPEND      0x0003000000000000ULL
+#define CXL_SPA_SW_CMD_RESUME       0x0004000000000000ULL
+#define CXL_SPA_SW_CMD_ADD          0x0005000000000000ULL
+#define CXL_SPA_SW_CMD_UPDATE       0x0006000000000000ULL
+#define CXL_SPA_SW_STATE_MASK       0x0000ffff00000000ULL
+#define CXL_SPA_SW_STATE_TERMINATED 0x0000000100000000ULL
+#define CXL_SPA_SW_STATE_REMOVED    0x0000000200000000ULL
+#define CXL_SPA_SW_STATE_SUSPENDED  0x0000000300000000ULL
+#define CXL_SPA_SW_STATE_RESUMED    0x0000000400000000ULL
+#define CXL_SPA_SW_STATE_ADDED      0x0000000500000000ULL
+#define CXL_SPA_SW_STATE_UPDATED    0x0000000600000000ULL
+#define CXL_SPA_SW_PSL_ID_MASK      0x00000000ffff0000ULL
+#define CXL_SPA_SW_LINK_MASK        0x000000000000ffffULL
+
+#define CXL_MAX_SLICES 4
+#define MAX_AFU_MMIO_REGS 3
+
+#define CXL_MODE_DEDICATED   0x1
+#define CXL_MODE_DIRECTED    0x2
+#define CXL_MODE_TIME_SLICED 0x4
+#define CXL_SUPPORTED_MODES (CXL_MODE_DEDICATED | CXL_MODE_DIRECTED)
+
+enum cxl_context_status {
+	CLOSED,
+	OPENED,
+	STARTED
+};
+
+enum prefault_modes {
+	CXL_PREFAULT_NONE,
+	CXL_PREFAULT_WED,
+	CXL_PREFAULT_ALL,
+};
+
+struct cxl_sste {
+	__be64 esid_data;
+	__be64 vsid_data;
+};
+
+#define to_cxl_adapter(d) container_of(d, struct cxl, dev)
+#define to_cxl_afu(d) container_of(d, struct cxl_afu, dev)
+
+struct cxl_afu {
+	irq_hw_number_t psl_hwirq;
+	irq_hw_number_t serr_hwirq;
+	unsigned int serr_virq;
+	void __iomem *p1n_mmio;
+	void __iomem *p2n_mmio;
+	phys_addr_t psn_phys;
+	u64 pp_offset;
+	u64 pp_size;
+	void __iomem *afu_desc_mmio;
+	struct cxl *adapter;
+	struct device dev;
+	struct cdev afu_cdev_s, afu_cdev_m, afu_cdev_d;
+	struct device *chardev_s, *chardev_m, *chardev_d;
+	struct idr contexts_idr;
+	struct dentry *debugfs;
+	spinlock_t contexts_lock;
+	struct mutex spa_mutex;
+	spinlock_t afu_cntl_lock;
+
+	/*
+	 * Only the first part of the SPA is used for the process element
+	 * linked list. The only other part that software needs to worry about
+	 * is sw_command_status, which we store a separate pointer to.
+	 * Everything else in the SPA is only used by hardware
+	 */
+	struct cxl_process_element *spa;
+	__be64 *sw_command_status;
+	unsigned int spa_size;
+	int spa_order;
+	int spa_max_procs;
+	unsigned int psl_virq;
+
+	int pp_irqs;
+	int irqs_max;
+	int num_procs;
+	int max_procs_virtualised;
+	int slice;
+	int modes_supported;
+	int current_mode;
+	enum prefault_modes prefault_mode;
+	bool psa;
+	bool pp_psa;
+	bool enabled;
+};
+
+/*
+ * This is a cxl context.  If the PSL is in dedicated mode, there will be one
+ * of these per AFU.  If in AFU directed there can be lots of these.
+ */
+struct cxl_context {
+	struct cxl_afu *afu;
+
+	/* Problem state MMIO */
+	phys_addr_t psn_phys;
+	u64 psn_size;
+
+	spinlock_t sste_lock; /* Protects segment table entries */
+	struct cxl_sste *sstp;
+	u64 sstp0, sstp1;
+	unsigned int sst_size, sst_lru;
+
+	wait_queue_head_t wq;
+	struct pid *pid;
+	spinlock_t lock; /* Protects pending_irq_mask, pending_fault and fault_addr */
+	/* Only used in PR mode */
+	u64 process_token;
+
+	unsigned long *irq_bitmap; /* Accessed from IRQ context */
+	struct cxl_irq_ranges irqs;
+	u64 fault_addr;
+	u64 fault_dsisr;
+	u64 afu_err;
+
+	/*
+	 * This status and it's lock pretects start and detach context
+	 * from racing.  It also prevents detach from racing with
+	 * itself
+	 */
+	enum cxl_context_status status;
+	struct mutex status_mutex;
+
+
+	/* XXX: Is it possible to need multiple work items at once? */
+	struct work_struct fault_work;
+	u64 dsisr;
+	u64 dar;
+
+	struct cxl_process_element *elem;
+
+	int pe; /* process element handle */
+	u32 irq_count;
+	bool pe_inserted;
+	bool master;
+	bool kernel;
+	bool pending_irq;
+	bool pending_fault;
+	bool pending_afu_err;
+};
+
+struct cxl {
+	void __iomem *p1_mmio;
+	void __iomem *p2_mmio;
+	irq_hw_number_t err_hwirq;
+	unsigned int err_virq;
+	spinlock_t afu_list_lock;
+	struct cxl_afu *afu[CXL_MAX_SLICES];
+	struct device dev;
+	struct dentry *trace;
+	struct dentry *psl_err_chk;
+	struct dentry *debugfs;
+	struct bin_attribute cxl_attr;
+	int adapter_num;
+	int user_irqs;
+	u64 afu_desc_off;
+	u64 afu_desc_size;
+	u64 ps_off;
+	u64 ps_size;
+	u16 psl_rev;
+	u16 base_image;
+	u8 vsec_status;
+	u8 caia_major;
+	u8 caia_minor;
+	u8 slices;
+	bool user_image_loaded;
+	bool perst_loads_image;
+	bool perst_select_user;
+};
+
+int cxl_alloc_one_irq(struct cxl *adapter);
+void cxl_release_one_irq(struct cxl *adapter, int hwirq);
+int cxl_alloc_irq_ranges(struct cxl_irq_ranges *irqs, struct cxl *adapter, unsigned int num);
+void cxl_release_irq_ranges(struct cxl_irq_ranges *irqs, struct cxl *adapter);
+int cxl_setup_irq(struct cxl *adapter, unsigned int hwirq, unsigned int virq);
+
+/* common == phyp + powernv */
+struct cxl_process_element_common {
+	__be32 tid;
+	__be32 pid;
+	__be64 csrp;
+	__be64 aurp0;
+	__be64 aurp1;
+	__be64 sstp0;
+	__be64 sstp1;
+	__be64 amr;
+	u8     reserved3[4];
+	__be64 wed;
+} __packed;
+
+/* just powernv */
+struct cxl_process_element {
+	__be64 sr;
+	__be64 SPOffset;
+	__be64 sdr;
+	__be64 haurp;
+	__be32 ctxtime;
+	__be16 ivte_offsets[4];
+	__be16 ivte_ranges[4];
+	__be32 lpid;
+	struct cxl_process_element_common common;
+	__be32 software_state;
+} __packed;
+
+static inline void __iomem *_cxl_p1_addr(struct cxl *cxl, cxl_p1_reg_t reg)
+{
+	WARN_ON(!cpu_has_feature(CPU_FTR_HVMODE));
+	return cxl->p1_mmio + cxl_reg_off(reg);
+}
+
+#define cxl_p1_write(cxl, reg, val) \
+	out_be64(_cxl_p1_addr(cxl, reg), val)
+#define cxl_p1_read(cxl, reg) \
+	in_be64(_cxl_p1_addr(cxl, reg))
+
+static inline void __iomem *_cxl_p1n_addr(struct cxl_afu *afu, cxl_p1n_reg_t reg)
+{
+	WARN_ON(!cpu_has_feature(CPU_FTR_HVMODE));
+	return afu->p1n_mmio + cxl_reg_off(reg);
+}
+
+#define cxl_p1n_write(afu, reg, val) \
+	out_be64(_cxl_p1n_addr(afu, reg), val)
+#define cxl_p1n_read(afu, reg) \
+	in_be64(_cxl_p1n_addr(afu, reg))
+
+static inline void __iomem *_cxl_p2n_addr(struct cxl_afu *afu, cxl_p2n_reg_t reg)
+{
+	return afu->p2n_mmio + cxl_reg_off(reg);
+}
+
+#define cxl_p2n_write(afu, reg, val) \
+	out_be64(_cxl_p2n_addr(afu, reg), val)
+#define cxl_p2n_read(afu, reg) \
+	in_be64(_cxl_p2n_addr(afu, reg))
+
+struct cxl_calls {
+	void (*cxl_slbia)(struct mm_struct *mm);
+	struct module *owner;
+};
+int register_cxl_calls(struct cxl_calls *calls);
+void unregister_cxl_calls(struct cxl_calls *calls);
+
+int cxl_alloc_adapter_nr(struct cxl *adapter);
+void cxl_remove_adapter_nr(struct cxl *adapter);
+
+int cxl_file_init(void);
+void cxl_file_exit(void);
+int cxl_register_adapter(struct cxl *adapter);
+int cxl_register_afu(struct cxl_afu *afu);
+int cxl_chardev_d_afu_add(struct cxl_afu *afu);
+int cxl_chardev_m_afu_add(struct cxl_afu *afu);
+int cxl_chardev_s_afu_add(struct cxl_afu *afu);
+void cxl_chardev_afu_remove(struct cxl_afu *afu);
+
+void cxl_context_detach_all(struct cxl_afu *afu);
+void cxl_context_free(struct cxl_context *ctx);
+void cxl_context_detach(struct cxl_context *ctx);
+
+int cxl_sysfs_adapter_add(struct cxl *adapter);
+void cxl_sysfs_adapter_remove(struct cxl *adapter);
+int cxl_sysfs_afu_add(struct cxl_afu *afu);
+void cxl_sysfs_afu_remove(struct cxl_afu *afu);
+int cxl_sysfs_afu_m_add(struct cxl_afu *afu);
+void cxl_sysfs_afu_m_remove(struct cxl_afu *afu);
+
+int cxl_afu_activate_mode(struct cxl_afu *afu, int mode);
+int _cxl_afu_deactivate_mode(struct cxl_afu *afu, int mode);
+int cxl_afu_deactivate_mode(struct cxl_afu *afu);
+int cxl_afu_select_best_mode(struct cxl_afu *afu);
+
+unsigned int cxl_map_irq(struct cxl *adapter, irq_hw_number_t hwirq,
+		         irq_handler_t handler, void *cookie);
+void cxl_unmap_irq(unsigned int virq, void *cookie);
+int cxl_register_psl_irq(struct cxl_afu *afu);
+void cxl_release_psl_irq(struct cxl_afu *afu);
+int cxl_register_psl_err_irq(struct cxl *adapter);
+void cxl_release_psl_err_irq(struct cxl *adapter);
+int cxl_register_serr_irq(struct cxl_afu *afu);
+void cxl_release_serr_irq(struct cxl_afu *afu);
+int afu_register_irqs(struct cxl_context *ctx, u32 count);
+void afu_release_irqs(struct cxl_context *ctx);
+irqreturn_t cxl_slice_irq_err(int irq, void *data);
+
+int cxl_debugfs_init(void);
+void cxl_debugfs_exit(void);
+int cxl_debugfs_adapter_add(struct cxl *adapter);
+void cxl_debugfs_adapter_remove(struct cxl *adapter);
+int cxl_debugfs_afu_add(struct cxl_afu *afu);
+void cxl_debugfs_afu_remove(struct cxl_afu *afu);
+
+void cxl_handle_fault(struct work_struct *work);
+void cxl_prefault(struct cxl_context *ctx, u64 wed);
+
+struct cxl *get_cxl_adapter(int num);
+int cxl_alloc_sst(struct cxl_context *ctx);
+
+void init_cxl_native(void);
+
+struct cxl_context *cxl_context_alloc(void);
+int cxl_context_init(struct cxl_context *ctx, struct cxl_afu *afu, bool master);
+void cxl_context_free(struct cxl_context *ctx);
+int cxl_context_iomap(struct cxl_context *ctx, struct vm_area_struct *vma);
+
+/* This matches the layout of the H_COLLECT_CA_INT_INFO retbuf */
+struct cxl_irq_info {
+	u64 dsisr;
+	u64 dar;
+	u64 dsr;
+	u32 pid;
+	u32 tid;
+	u64 afu_err;
+	u64 errstat;
+	u64 padding[3]; /* to match the expected retbuf size for plpar_hcall9 */
+};
+
+int cxl_attach_process(struct cxl_context *ctx, bool kernel, u64 wed,
+			    u64 amr);
+int cxl_detach_process(struct cxl_context *ctx);
+
+int cxl_get_irq(struct cxl_context *ctx, struct cxl_irq_info *info);
+int cxl_ack_irq(struct cxl_context *ctx, u64 tfc, u64 psl_reset_mask);
+
+int cxl_check_error(struct cxl_afu *afu);
+int cxl_afu_slbia(struct cxl_afu *afu);
+int cxl_tlb_slb_invalidate(struct cxl *adapter);
+int cxl_afu_disable(struct cxl_afu *afu);
+int cxl_afu_reset(struct cxl_afu *afu);
+int cxl_psl_purge(struct cxl_afu *afu);
+
+void cxl_stop_trace(struct cxl *cxl);
+
+extern struct pci_driver cxl_pci_driver;
+
+#endif
diff --git a/drivers/misc/cxl/debugfs.c b/drivers/misc/cxl/debugfs.c
new file mode 100644
index 000000000000..825c412580bc
--- /dev/null
+++ b/drivers/misc/cxl/debugfs.c
@@ -0,0 +1,132 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+
+#include "cxl.h"
+
+static struct dentry *cxl_debugfs;
+
+void cxl_stop_trace(struct cxl *adapter)
+{
+	int slice;
+
+	/* Stop the trace */
+	cxl_p1_write(adapter, CXL_PSL_TRACE, 0x8000000000000017LL);
+
+	/* Stop the slice traces */
+	spin_lock(&adapter->afu_list_lock);
+	for (slice = 0; slice < adapter->slices; slice++) {
+		if (adapter->afu[slice])
+			cxl_p1n_write(adapter->afu[slice], CXL_PSL_SLICE_TRACE, 0x8000000000000000LL);
+	}
+	spin_unlock(&adapter->afu_list_lock);
+}
+
+/* Helpers to export CXL mmaped IO registers via debugfs */
+static int debugfs_io_u64_get(void *data, u64 *val)
+{
+	*val = in_be64((u64 __iomem *)data);
+	return 0;
+}
+
+static int debugfs_io_u64_set(void *data, u64 val)
+{
+	out_be64((u64 __iomem *)data, val);
+	return 0;
+}
+DEFINE_SIMPLE_ATTRIBUTE(fops_io_x64, debugfs_io_u64_get, debugfs_io_u64_set, "0x%016llx\n");
+
+static struct dentry *debugfs_create_io_x64(const char *name, umode_t mode,
+					    struct dentry *parent, u64 __iomem *value)
+{
+	return debugfs_create_file(name, mode, parent, (void *)value, &fops_io_x64);
+}
+
+int cxl_debugfs_adapter_add(struct cxl *adapter)
+{
+	struct dentry *dir;
+	char buf[32];
+
+	if (!cxl_debugfs)
+		return -ENODEV;
+
+	snprintf(buf, 32, "card%i", adapter->adapter_num);
+	dir = debugfs_create_dir(buf, cxl_debugfs);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	adapter->debugfs = dir;
+
+	debugfs_create_io_x64("fir1",     S_IRUSR, dir, _cxl_p1_addr(adapter, CXL_PSL_FIR1));
+	debugfs_create_io_x64("fir2",     S_IRUSR, dir, _cxl_p1_addr(adapter, CXL_PSL_FIR2));
+	debugfs_create_io_x64("fir_cntl", S_IRUSR, dir, _cxl_p1_addr(adapter, CXL_PSL_FIR_CNTL));
+	debugfs_create_io_x64("err_ivte", S_IRUSR, dir, _cxl_p1_addr(adapter, CXL_PSL_ErrIVTE));
+
+	debugfs_create_io_x64("trace", S_IRUSR | S_IWUSR, dir, _cxl_p1_addr(adapter, CXL_PSL_TRACE));
+
+	return 0;
+}
+
+void cxl_debugfs_adapter_remove(struct cxl *adapter)
+{
+	debugfs_remove_recursive(adapter->debugfs);
+}
+
+int cxl_debugfs_afu_add(struct cxl_afu *afu)
+{
+	struct dentry *dir;
+	char buf[32];
+
+	if (!afu->adapter->debugfs)
+		return -ENODEV;
+
+	snprintf(buf, 32, "psl%i.%i", afu->adapter->adapter_num, afu->slice);
+	dir = debugfs_create_dir(buf, afu->adapter->debugfs);
+	if (IS_ERR(dir))
+		return PTR_ERR(dir);
+	afu->debugfs = dir;
+
+	debugfs_create_io_x64("fir",        S_IRUSR, dir, _cxl_p1n_addr(afu, CXL_PSL_FIR_SLICE_An));
+	debugfs_create_io_x64("serr",       S_IRUSR, dir, _cxl_p1n_addr(afu, CXL_PSL_SERR_An));
+	debugfs_create_io_x64("afu_debug",  S_IRUSR, dir, _cxl_p1n_addr(afu, CXL_AFU_DEBUG_An));
+	debugfs_create_io_x64("sr",         S_IRUSR, dir, _cxl_p1n_addr(afu, CXL_PSL_SR_An));
+
+	debugfs_create_io_x64("dsisr",      S_IRUSR, dir, _cxl_p2n_addr(afu, CXL_PSL_DSISR_An));
+	debugfs_create_io_x64("dar",        S_IRUSR, dir, _cxl_p2n_addr(afu, CXL_PSL_DAR_An));
+	debugfs_create_io_x64("sstp0",      S_IRUSR, dir, _cxl_p2n_addr(afu, CXL_SSTP0_An));
+	debugfs_create_io_x64("sstp1",      S_IRUSR, dir, _cxl_p2n_addr(afu, CXL_SSTP1_An));
+	debugfs_create_io_x64("err_status", S_IRUSR, dir, _cxl_p2n_addr(afu, CXL_PSL_ErrStat_An));
+
+	debugfs_create_io_x64("trace", S_IRUSR | S_IWUSR, dir, _cxl_p1n_addr(afu, CXL_PSL_SLICE_TRACE));
+
+	return 0;
+}
+
+void cxl_debugfs_afu_remove(struct cxl_afu *afu)
+{
+	debugfs_remove_recursive(afu->debugfs);
+}
+
+int __init cxl_debugfs_init(void)
+{
+	struct dentry *ent;
+	ent = debugfs_create_dir("cxl", NULL);
+	if (IS_ERR(ent))
+		return PTR_ERR(ent);
+	cxl_debugfs = ent;
+
+	return 0;
+}
+
+void cxl_debugfs_exit(void)
+{
+	debugfs_remove_recursive(cxl_debugfs);
+}
diff --git a/drivers/misc/cxl/fault.c b/drivers/misc/cxl/fault.c
new file mode 100644
index 000000000000..69506ebd4d07
--- /dev/null
+++ b/drivers/misc/cxl/fault.c
@@ -0,0 +1,291 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/workqueue.h>
+#include <linux/sched.h>
+#include <linux/pid.h>
+#include <linux/mm.h>
+#include <linux/moduleparam.h>
+
+#undef MODULE_PARAM_PREFIX
+#define MODULE_PARAM_PREFIX "cxl" "."
+#include <asm/current.h>
+#include <asm/copro.h>
+#include <asm/mmu.h>
+
+#include "cxl.h"
+
+static struct cxl_sste* find_free_sste(struct cxl_sste *primary_group,
+				       bool sec_hash,
+				       struct cxl_sste *secondary_group,
+				       unsigned int *lru)
+{
+	unsigned int i, entry;
+	struct cxl_sste *sste, *group = primary_group;
+
+	for (i = 0; i < 2; i++) {
+		for (entry = 0; entry < 8; entry++) {
+			sste = group + entry;
+			if (!(be64_to_cpu(sste->esid_data) & SLB_ESID_V))
+				return sste;
+		}
+		if (!sec_hash)
+			break;
+		group = secondary_group;
+	}
+	/* Nothing free, select an entry to cast out */
+	if (sec_hash && (*lru & 0x8))
+		sste = secondary_group + (*lru & 0x7);
+	else
+		sste = primary_group + (*lru & 0x7);
+	*lru = (*lru + 1) & 0xf;
+
+	return sste;
+}
+
+static void cxl_load_segment(struct cxl_context *ctx, struct copro_slb *slb)
+{
+	/* mask is the group index, we search primary and secondary here. */
+	unsigned int mask = (ctx->sst_size >> 7)-1; /* SSTP0[SegTableSize] */
+	bool sec_hash = 1;
+	struct cxl_sste *sste;
+	unsigned int hash;
+	unsigned long flags;
+
+
+	sec_hash = !!(cxl_p1n_read(ctx->afu, CXL_PSL_SR_An) & CXL_PSL_SR_An_SC);
+
+	if (slb->vsid & SLB_VSID_B_1T)
+		hash = (slb->esid >> SID_SHIFT_1T) & mask;
+	else /* 256M */
+		hash = (slb->esid >> SID_SHIFT) & mask;
+
+	spin_lock_irqsave(&ctx->sste_lock, flags);
+	sste = find_free_sste(ctx->sstp + (hash << 3), sec_hash,
+			      ctx->sstp + ((~hash & mask) << 3), &ctx->sst_lru);
+
+	pr_devel("CXL Populating SST[%li]: %#llx %#llx\n",
+			sste - ctx->sstp, slb->vsid, slb->esid);
+
+	sste->vsid_data = cpu_to_be64(slb->vsid);
+	sste->esid_data = cpu_to_be64(slb->esid);
+	spin_unlock_irqrestore(&ctx->sste_lock, flags);
+}
+
+static int cxl_fault_segment(struct cxl_context *ctx, struct mm_struct *mm,
+			     u64 ea)
+{
+	struct copro_slb slb = {0,0};
+	int rc;
+
+	if (!(rc = copro_calculate_slb(mm, ea, &slb))) {
+		cxl_load_segment(ctx, &slb);
+	}
+
+	return rc;
+}
+
+static void cxl_ack_ae(struct cxl_context *ctx)
+{
+	unsigned long flags;
+
+	cxl_ack_irq(ctx, CXL_PSL_TFC_An_AE, 0);
+
+	spin_lock_irqsave(&ctx->lock, flags);
+	ctx->pending_fault = true;
+	ctx->fault_addr = ctx->dar;
+	ctx->fault_dsisr = ctx->dsisr;
+	spin_unlock_irqrestore(&ctx->lock, flags);
+
+	wake_up_all(&ctx->wq);
+}
+
+static int cxl_handle_segment_miss(struct cxl_context *ctx,
+				   struct mm_struct *mm, u64 ea)
+{
+	int rc;
+
+	pr_devel("CXL interrupt: Segment fault pe: %i ea: %#llx\n", ctx->pe, ea);
+
+	if ((rc = cxl_fault_segment(ctx, mm, ea)))
+		cxl_ack_ae(ctx);
+	else {
+
+		mb(); /* Order seg table write to TFC MMIO write */
+		cxl_ack_irq(ctx, CXL_PSL_TFC_An_R, 0);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static void cxl_handle_page_fault(struct cxl_context *ctx,
+				  struct mm_struct *mm, u64 dsisr, u64 dar)
+{
+	unsigned flt = 0;
+	int result;
+	unsigned long access, flags;
+
+	if ((result = copro_handle_mm_fault(mm, dar, dsisr, &flt))) {
+		pr_devel("copro_handle_mm_fault failed: %#x\n", result);
+		return cxl_ack_ae(ctx);
+	}
+
+	/*
+	 * update_mmu_cache() will not have loaded the hash since current->trap
+	 * is not a 0x400 or 0x300, so just call hash_page_mm() here.
+	 */
+	access = _PAGE_PRESENT;
+	if (dsisr & CXL_PSL_DSISR_An_S)
+		access |= _PAGE_RW;
+	if ((!ctx->kernel) || ~(dar & (1ULL << 63)))
+		access |= _PAGE_USER;
+	local_irq_save(flags);
+	hash_page_mm(mm, dar, access, 0x300);
+	local_irq_restore(flags);
+
+	pr_devel("Page fault successfully handled for pe: %i!\n", ctx->pe);
+	cxl_ack_irq(ctx, CXL_PSL_TFC_An_R, 0);
+}
+
+void cxl_handle_fault(struct work_struct *fault_work)
+{
+	struct cxl_context *ctx =
+		container_of(fault_work, struct cxl_context, fault_work);
+	u64 dsisr = ctx->dsisr;
+	u64 dar = ctx->dar;
+	struct task_struct *task;
+	struct mm_struct *mm;
+
+	if (cxl_p2n_read(ctx->afu, CXL_PSL_DSISR_An) != dsisr ||
+	    cxl_p2n_read(ctx->afu, CXL_PSL_DAR_An) != dar ||
+	    cxl_p2n_read(ctx->afu, CXL_PSL_PEHandle_An) != ctx->pe) {
+		/* Most likely explanation is harmless - a dedicated process
+		 * has detached and these were cleared by the PSL purge, but
+		 * warn about it just in case */
+		dev_notice(&ctx->afu->dev, "cxl_handle_fault: Translation fault regs changed\n");
+		return;
+	}
+
+	pr_devel("CXL BOTTOM HALF handling fault for afu pe: %i. "
+		"DSISR: %#llx DAR: %#llx\n", ctx->pe, dsisr, dar);
+
+	if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
+		pr_devel("cxl_handle_fault unable to get task %i\n",
+			 pid_nr(ctx->pid));
+		cxl_ack_ae(ctx);
+		return;
+	}
+	if (!(mm = get_task_mm(task))) {
+		pr_devel("cxl_handle_fault unable to get mm %i\n",
+			 pid_nr(ctx->pid));
+		cxl_ack_ae(ctx);
+		goto out;
+	}
+
+	if (dsisr & CXL_PSL_DSISR_An_DS)
+		cxl_handle_segment_miss(ctx, mm, dar);
+	else if (dsisr & CXL_PSL_DSISR_An_DM)
+		cxl_handle_page_fault(ctx, mm, dsisr, dar);
+	else
+		WARN(1, "cxl_handle_fault has nothing to handle\n");
+
+	mmput(mm);
+out:
+	put_task_struct(task);
+}
+
+static void cxl_prefault_one(struct cxl_context *ctx, u64 ea)
+{
+	int rc;
+	struct task_struct *task;
+	struct mm_struct *mm;
+
+	if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
+		pr_devel("cxl_prefault_one unable to get task %i\n",
+			 pid_nr(ctx->pid));
+		return;
+	}
+	if (!(mm = get_task_mm(task))) {
+		pr_devel("cxl_prefault_one unable to get mm %i\n",
+			 pid_nr(ctx->pid));
+		put_task_struct(task);
+		return;
+	}
+
+	rc = cxl_fault_segment(ctx, mm, ea);
+
+	mmput(mm);
+	put_task_struct(task);
+}
+
+static u64 next_segment(u64 ea, u64 vsid)
+{
+	if (vsid & SLB_VSID_B_1T)
+		ea |= (1ULL << 40) - 1;
+	else
+		ea |= (1ULL << 28) - 1;
+
+	return ea + 1;
+}
+
+static void cxl_prefault_vma(struct cxl_context *ctx)
+{
+	u64 ea, last_esid = 0;
+	struct copro_slb slb;
+	struct vm_area_struct *vma;
+	int rc;
+	struct task_struct *task;
+	struct mm_struct *mm;
+
+	if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
+		pr_devel("cxl_prefault_vma unable to get task %i\n",
+			 pid_nr(ctx->pid));
+		return;
+	}
+	if (!(mm = get_task_mm(task))) {
+		pr_devel("cxl_prefault_vm unable to get mm %i\n",
+			 pid_nr(ctx->pid));
+		goto out1;
+	}
+
+	down_read(&mm->mmap_sem);
+	for (vma = mm->mmap; vma; vma = vma->vm_next) {
+		for (ea = vma->vm_start; ea < vma->vm_end;
+				ea = next_segment(ea, slb.vsid)) {
+			rc = copro_calculate_slb(mm, ea, &slb);
+			if (rc)
+				continue;
+
+			if (last_esid == slb.esid)
+				continue;
+
+			cxl_load_segment(ctx, &slb);
+			last_esid = slb.esid;
+		}
+	}
+	up_read(&mm->mmap_sem);
+
+	mmput(mm);
+out1:
+	put_task_struct(task);
+}
+
+void cxl_prefault(struct cxl_context *ctx, u64 wed)
+{
+	switch (ctx->afu->prefault_mode) {
+	case CXL_PREFAULT_WED:
+		cxl_prefault_one(ctx, wed);
+		break;
+	case CXL_PREFAULT_ALL:
+		cxl_prefault_vma(ctx);
+		break;
+	default:
+		break;
+	}
+}
diff --git a/drivers/misc/cxl/file.c b/drivers/misc/cxl/file.c
new file mode 100644
index 000000000000..378b099e7c0b
--- /dev/null
+++ b/drivers/misc/cxl/file.c
@@ -0,0 +1,518 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/spinlock.h>
+#include <linux/module.h>
+#include <linux/export.h>
+#include <linux/kernel.h>
+#include <linux/bitmap.h>
+#include <linux/sched.h>
+#include <linux/poll.h>
+#include <linux/pid.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <asm/cputable.h>
+#include <asm/current.h>
+#include <asm/copro.h>
+
+#include "cxl.h"
+
+#define CXL_NUM_MINORS 256 /* Total to reserve */
+#define CXL_DEV_MINORS 13   /* 1 control + 4 AFUs * 3 (dedicated/master/shared) */
+
+#define CXL_CARD_MINOR(adapter) (adapter->adapter_num * CXL_DEV_MINORS)
+#define CXL_AFU_MINOR_D(afu) (CXL_CARD_MINOR(afu->adapter) + 1 + (3 * afu->slice))
+#define CXL_AFU_MINOR_M(afu) (CXL_AFU_MINOR_D(afu) + 1)
+#define CXL_AFU_MINOR_S(afu) (CXL_AFU_MINOR_D(afu) + 2)
+#define CXL_AFU_MKDEV_D(afu) MKDEV(MAJOR(cxl_dev), CXL_AFU_MINOR_D(afu))
+#define CXL_AFU_MKDEV_M(afu) MKDEV(MAJOR(cxl_dev), CXL_AFU_MINOR_M(afu))
+#define CXL_AFU_MKDEV_S(afu) MKDEV(MAJOR(cxl_dev), CXL_AFU_MINOR_S(afu))
+
+#define CXL_DEVT_ADAPTER(dev) (MINOR(dev) / CXL_DEV_MINORS)
+#define CXL_DEVT_AFU(dev) ((MINOR(dev) % CXL_DEV_MINORS - 1) / 3)
+
+#define CXL_DEVT_IS_CARD(dev) (MINOR(dev) % CXL_DEV_MINORS == 0)
+
+static dev_t cxl_dev;
+
+static struct class *cxl_class;
+
+static int __afu_open(struct inode *inode, struct file *file, bool master)
+{
+	struct cxl *adapter;
+	struct cxl_afu *afu;
+	struct cxl_context *ctx;
+	int adapter_num = CXL_DEVT_ADAPTER(inode->i_rdev);
+	int slice = CXL_DEVT_AFU(inode->i_rdev);
+	int rc = -ENODEV;
+
+	pr_devel("afu_open afu%i.%i\n", slice, adapter_num);
+
+	if (!(adapter = get_cxl_adapter(adapter_num)))
+		return -ENODEV;
+
+	if (slice > adapter->slices)
+		goto err_put_adapter;
+
+	spin_lock(&adapter->afu_list_lock);
+	if (!(afu = adapter->afu[slice])) {
+		spin_unlock(&adapter->afu_list_lock);
+		goto err_put_adapter;
+	}
+	get_device(&afu->dev);
+	spin_unlock(&adapter->afu_list_lock);
+
+	if (!afu->current_mode)
+		goto err_put_afu;
+
+	if (!(ctx = cxl_context_alloc())) {
+		rc = -ENOMEM;
+		goto err_put_afu;
+	}
+
+	if ((rc = cxl_context_init(ctx, afu, master)))
+		goto err_put_afu;
+
+	pr_devel("afu_open pe: %i\n", ctx->pe);
+	file->private_data = ctx;
+	cxl_ctx_get();
+
+	/* Our ref on the AFU will now hold the adapter */
+	put_device(&adapter->dev);
+
+	return 0;
+
+err_put_afu:
+	put_device(&afu->dev);
+err_put_adapter:
+	put_device(&adapter->dev);
+	return rc;
+}
+static int afu_open(struct inode *inode, struct file *file)
+{
+	return __afu_open(inode, file, false);
+}
+
+static int afu_master_open(struct inode *inode, struct file *file)
+{
+	return __afu_open(inode, file, true);
+}
+
+static int afu_release(struct inode *inode, struct file *file)
+{
+	struct cxl_context *ctx = file->private_data;
+
+	pr_devel("%s: closing cxl file descriptor. pe: %i\n",
+		 __func__, ctx->pe);
+	cxl_context_detach(ctx);
+
+	put_device(&ctx->afu->dev);
+
+	/*
+	 * At this this point all bottom halfs have finished and we should be
+	 * getting no more IRQs from the hardware for this context.  Once it's
+	 * removed from the IDR (and RCU synchronised) it's safe to free the
+	 * sstp and context.
+	 */
+	cxl_context_free(ctx);
+
+	cxl_ctx_put();
+	return 0;
+}
+
+static long afu_ioctl_start_work(struct cxl_context *ctx,
+				 struct cxl_ioctl_start_work __user *uwork)
+{
+	struct cxl_ioctl_start_work work;
+	u64 amr = 0;
+	int rc;
+
+	pr_devel("%s: pe: %i\n", __func__, ctx->pe);
+
+	mutex_lock(&ctx->status_mutex);
+	if (ctx->status != OPENED) {
+		rc = -EIO;
+		goto out;
+	}
+
+	if (copy_from_user(&work, uwork,
+			   sizeof(struct cxl_ioctl_start_work))) {
+		rc = -EFAULT;
+		goto out;
+	}
+
+	/*
+	 * if any of the reserved fields are set or any of the unused
+	 * flags are set it's invalid
+	 */
+	if (work.reserved1 || work.reserved2 || work.reserved3 ||
+	    work.reserved4 || work.reserved5 || work.reserved6 ||
+	    (work.flags & ~CXL_START_WORK_ALL)) {
+		rc = -EINVAL;
+		goto out;
+	}
+
+	if (!(work.flags & CXL_START_WORK_NUM_IRQS))
+		work.num_interrupts = ctx->afu->pp_irqs;
+	else if ((work.num_interrupts < ctx->afu->pp_irqs) ||
+		 (work.num_interrupts > ctx->afu->irqs_max)) {
+		rc =  -EINVAL;
+		goto out;
+	}
+	if ((rc = afu_register_irqs(ctx, work.num_interrupts)))
+		goto out;
+
+	if (work.flags & CXL_START_WORK_AMR)
+		amr = work.amr & mfspr(SPRN_UAMOR);
+
+	/*
+	 * We grab the PID here and not in the file open to allow for the case
+	 * where a process (master, some daemon, etc) has opened the chardev on
+	 * behalf of another process, so the AFU's mm gets bound to the process
+	 * that performs this ioctl and not the process that opened the file.
+	 */
+	ctx->pid = get_pid(get_task_pid(current, PIDTYPE_PID));
+
+	if ((rc = cxl_attach_process(ctx, false, work.work_element_descriptor,
+				     amr)))
+		goto out;
+
+	ctx->status = STARTED;
+	rc = 0;
+out:
+	mutex_unlock(&ctx->status_mutex);
+	return rc;
+}
+static long afu_ioctl_process_element(struct cxl_context *ctx,
+				      int __user *upe)
+{
+	pr_devel("%s: pe: %i\n", __func__, ctx->pe);
+
+	if (copy_to_user(upe, &ctx->pe, sizeof(__u32)))
+		return -EFAULT;
+
+	return 0;
+}
+
+static long afu_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	struct cxl_context *ctx = file->private_data;
+
+	if (ctx->status == CLOSED)
+		return -EIO;
+
+	pr_devel("afu_ioctl\n");
+	switch (cmd) {
+	case CXL_IOCTL_START_WORK:
+		return afu_ioctl_start_work(ctx, (struct cxl_ioctl_start_work __user *)arg);
+	case CXL_IOCTL_GET_PROCESS_ELEMENT:
+		return afu_ioctl_process_element(ctx, (__u32 __user *)arg);
+	}
+	return -EINVAL;
+}
+
+static long afu_compat_ioctl(struct file *file, unsigned int cmd,
+			     unsigned long arg)
+{
+	return afu_ioctl(file, cmd, arg);
+}
+
+static int afu_mmap(struct file *file, struct vm_area_struct *vm)
+{
+	struct cxl_context *ctx = file->private_data;
+
+	/* AFU must be started before we can MMIO */
+	if (ctx->status != STARTED)
+		return -EIO;
+
+	return cxl_context_iomap(ctx, vm);
+}
+
+static unsigned int afu_poll(struct file *file, struct poll_table_struct *poll)
+{
+	struct cxl_context *ctx = file->private_data;
+	int mask = 0;
+	unsigned long flags;
+
+
+	poll_wait(file, &ctx->wq, poll);
+
+	pr_devel("afu_poll wait done pe: %i\n", ctx->pe);
+
+	spin_lock_irqsave(&ctx->lock, flags);
+	if (ctx->pending_irq || ctx->pending_fault ||
+	    ctx->pending_afu_err)
+		mask |= POLLIN | POLLRDNORM;
+	else if (ctx->status == CLOSED)
+		/* Only error on closed when there are no futher events pending
+		 */
+		mask |= POLLERR;
+	spin_unlock_irqrestore(&ctx->lock, flags);
+
+	pr_devel("afu_poll pe: %i returning %#x\n", ctx->pe, mask);
+
+	return mask;
+}
+
+static inline int ctx_event_pending(struct cxl_context *ctx)
+{
+	return (ctx->pending_irq || ctx->pending_fault ||
+	    ctx->pending_afu_err || (ctx->status == CLOSED));
+}
+
+static ssize_t afu_read(struct file *file, char __user *buf, size_t count,
+			loff_t *off)
+{
+	struct cxl_context *ctx = file->private_data;
+	struct cxl_event event;
+	unsigned long flags;
+	int rc;
+	DEFINE_WAIT(wait);
+
+	if (count < CXL_READ_MIN_SIZE)
+		return -EINVAL;
+
+	spin_lock_irqsave(&ctx->lock, flags);
+
+	for (;;) {
+		prepare_to_wait(&ctx->wq, &wait, TASK_INTERRUPTIBLE);
+		if (ctx_event_pending(ctx))
+			break;
+
+		if (file->f_flags & O_NONBLOCK) {
+			rc = -EAGAIN;
+			goto out;
+		}
+
+		if (signal_pending(current)) {
+			rc = -ERESTARTSYS;
+			goto out;
+		}
+
+		spin_unlock_irqrestore(&ctx->lock, flags);
+		pr_devel("afu_read going to sleep...\n");
+		schedule();
+		pr_devel("afu_read woken up\n");
+		spin_lock_irqsave(&ctx->lock, flags);
+	}
+
+	finish_wait(&ctx->wq, &wait);
+
+	memset(&event, 0, sizeof(event));
+	event.header.process_element = ctx->pe;
+	event.header.size = sizeof(struct cxl_event_header);
+	if (ctx->pending_irq) {
+		pr_devel("afu_read delivering AFU interrupt\n");
+		event.header.size += sizeof(struct cxl_event_afu_interrupt);
+		event.header.type = CXL_EVENT_AFU_INTERRUPT;
+		event.irq.irq = find_first_bit(ctx->irq_bitmap, ctx->irq_count) + 1;
+		clear_bit(event.irq.irq - 1, ctx->irq_bitmap);
+		if (bitmap_empty(ctx->irq_bitmap, ctx->irq_count))
+			ctx->pending_irq = false;
+	} else if (ctx->pending_fault) {
+		pr_devel("afu_read delivering data storage fault\n");
+		event.header.size += sizeof(struct cxl_event_data_storage);
+		event.header.type = CXL_EVENT_DATA_STORAGE;
+		event.fault.addr = ctx->fault_addr;
+		event.fault.dsisr = ctx->fault_dsisr;
+		ctx->pending_fault = false;
+	} else if (ctx->pending_afu_err) {
+		pr_devel("afu_read delivering afu error\n");
+		event.header.size += sizeof(struct cxl_event_afu_error);
+		event.header.type = CXL_EVENT_AFU_ERROR;
+		event.afu_error.error = ctx->afu_err;
+		ctx->pending_afu_err = false;
+	} else if (ctx->status == CLOSED) {
+		pr_devel("afu_read fatal error\n");
+		spin_unlock_irqrestore(&ctx->lock, flags);
+		return -EIO;
+	} else
+		WARN(1, "afu_read must be buggy\n");
+
+	spin_unlock_irqrestore(&ctx->lock, flags);
+
+	if (copy_to_user(buf, &event, event.header.size))
+		return -EFAULT;
+	return event.header.size;
+
+out:
+	finish_wait(&ctx->wq, &wait);
+	spin_unlock_irqrestore(&ctx->lock, flags);
+	return rc;
+}
+
+static const struct file_operations afu_fops = {
+	.owner		= THIS_MODULE,
+	.open           = afu_open,
+	.poll		= afu_poll,
+	.read		= afu_read,
+	.release        = afu_release,
+	.unlocked_ioctl = afu_ioctl,
+	.compat_ioctl   = afu_compat_ioctl,
+	.mmap           = afu_mmap,
+};
+
+static const struct file_operations afu_master_fops = {
+	.owner		= THIS_MODULE,
+	.open           = afu_master_open,
+	.poll		= afu_poll,
+	.read		= afu_read,
+	.release        = afu_release,
+	.unlocked_ioctl = afu_ioctl,
+	.compat_ioctl   = afu_compat_ioctl,
+	.mmap           = afu_mmap,
+};
+
+
+static char *cxl_devnode(struct device *dev, umode_t *mode)
+{
+	if (CXL_DEVT_IS_CARD(dev->devt)) {
+		/*
+		 * These minor numbers will eventually be used to program the
+		 * PSL and AFUs once we have dynamic reprogramming support
+		 */
+		return NULL;
+	}
+	return kasprintf(GFP_KERNEL, "cxl/%s", dev_name(dev));
+}
+
+extern struct class *cxl_class;
+
+static int cxl_add_chardev(struct cxl_afu *afu, dev_t devt, struct cdev *cdev,
+			   struct device **chardev, char *postfix, char *desc,
+			   const struct file_operations *fops)
+{
+	struct device *dev;
+	int rc;
+
+	cdev_init(cdev, fops);
+	if ((rc = cdev_add(cdev, devt, 1))) {
+		dev_err(&afu->dev, "Unable to add %s chardev: %i\n", desc, rc);
+		return rc;
+	}
+
+	dev = device_create(cxl_class, &afu->dev, devt, afu,
+			"afu%i.%i%s", afu->adapter->adapter_num, afu->slice, postfix);
+	if (IS_ERR(dev)) {
+		dev_err(&afu->dev, "Unable to create %s chardev in sysfs: %i\n", desc, rc);
+		rc = PTR_ERR(dev);
+		goto err;
+	}
+
+	*chardev = dev;
+
+	return 0;
+err:
+	cdev_del(cdev);
+	return rc;
+}
+
+int cxl_chardev_d_afu_add(struct cxl_afu *afu)
+{
+	return cxl_add_chardev(afu, CXL_AFU_MKDEV_D(afu), &afu->afu_cdev_d,
+			       &afu->chardev_d, "d", "dedicated",
+			       &afu_master_fops); /* Uses master fops */
+}
+
+int cxl_chardev_m_afu_add(struct cxl_afu *afu)
+{
+	return cxl_add_chardev(afu, CXL_AFU_MKDEV_M(afu), &afu->afu_cdev_m,
+			       &afu->chardev_m, "m", "master",
+			       &afu_master_fops);
+}
+
+int cxl_chardev_s_afu_add(struct cxl_afu *afu)
+{
+	return cxl_add_chardev(afu, CXL_AFU_MKDEV_S(afu), &afu->afu_cdev_s,
+			       &afu->chardev_s, "s", "shared",
+			       &afu_fops);
+}
+
+void cxl_chardev_afu_remove(struct cxl_afu *afu)
+{
+	if (afu->chardev_d) {
+		cdev_del(&afu->afu_cdev_d);
+		device_unregister(afu->chardev_d);
+		afu->chardev_d = NULL;
+	}
+	if (afu->chardev_m) {
+		cdev_del(&afu->afu_cdev_m);
+		device_unregister(afu->chardev_m);
+		afu->chardev_m = NULL;
+	}
+	if (afu->chardev_s) {
+		cdev_del(&afu->afu_cdev_s);
+		device_unregister(afu->chardev_s);
+		afu->chardev_s = NULL;
+	}
+}
+
+int cxl_register_afu(struct cxl_afu *afu)
+{
+	afu->dev.class = cxl_class;
+
+	return device_register(&afu->dev);
+}
+
+int cxl_register_adapter(struct cxl *adapter)
+{
+	adapter->dev.class = cxl_class;
+
+	/*
+	 * Future: When we support dynamically reprogramming the PSL & AFU we
+	 * will expose the interface to do that via a chardev:
+	 * adapter->dev.devt = CXL_CARD_MKDEV(adapter);
+	 */
+
+	return device_register(&adapter->dev);
+}
+
+int __init cxl_file_init(void)
+{
+	int rc;
+
+	/*
+	 * If these change we really need to update API.  Either change some
+	 * flags or update API version number CXL_API_VERSION.
+	 */
+	BUILD_BUG_ON(CXL_API_VERSION != 1);
+	BUILD_BUG_ON(sizeof(struct cxl_ioctl_start_work) != 64);
+	BUILD_BUG_ON(sizeof(struct cxl_event_header) != 8);
+	BUILD_BUG_ON(sizeof(struct cxl_event_afu_interrupt) != 8);
+	BUILD_BUG_ON(sizeof(struct cxl_event_data_storage) != 32);
+	BUILD_BUG_ON(sizeof(struct cxl_event_afu_error) != 16);
+
+	if ((rc = alloc_chrdev_region(&cxl_dev, 0, CXL_NUM_MINORS, "cxl"))) {
+		pr_err("Unable to allocate CXL major number: %i\n", rc);
+		return rc;
+	}
+
+	pr_devel("CXL device allocated, MAJOR %i\n", MAJOR(cxl_dev));
+
+	cxl_class = class_create(THIS_MODULE, "cxl");
+	if (IS_ERR(cxl_class)) {
+		pr_err("Unable to create CXL class\n");
+		rc = PTR_ERR(cxl_class);
+		goto err;
+	}
+	cxl_class->devnode = cxl_devnode;
+
+	return 0;
+
+err:
+	unregister_chrdev_region(cxl_dev, CXL_NUM_MINORS);
+	return rc;
+}
+
+void cxl_file_exit(void)
+{
+	unregister_chrdev_region(cxl_dev, CXL_NUM_MINORS);
+	class_destroy(cxl_class);
+}
diff --git a/drivers/misc/cxl/irq.c b/drivers/misc/cxl/irq.c
new file mode 100644
index 000000000000..336020c8e1af
--- /dev/null
+++ b/drivers/misc/cxl/irq.c
@@ -0,0 +1,402 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/workqueue.h>
+#include <linux/sched.h>
+#include <linux/wait.h>
+#include <linux/slab.h>
+#include <linux/pid.h>
+#include <asm/cputable.h>
+#include <misc/cxl.h>
+
+#include "cxl.h"
+
+/* XXX: This is implementation specific */
+static irqreturn_t handle_psl_slice_error(struct cxl_context *ctx, u64 dsisr, u64 errstat)
+{
+	u64 fir1, fir2, fir_slice, serr, afu_debug;
+
+	fir1 = cxl_p1_read(ctx->afu->adapter, CXL_PSL_FIR1);
+	fir2 = cxl_p1_read(ctx->afu->adapter, CXL_PSL_FIR2);
+	fir_slice = cxl_p1n_read(ctx->afu, CXL_PSL_FIR_SLICE_An);
+	serr = cxl_p1n_read(ctx->afu, CXL_PSL_SERR_An);
+	afu_debug = cxl_p1n_read(ctx->afu, CXL_AFU_DEBUG_An);
+
+	dev_crit(&ctx->afu->dev, "PSL ERROR STATUS: 0x%.16llx\n", errstat);
+	dev_crit(&ctx->afu->dev, "PSL_FIR1: 0x%.16llx\n", fir1);
+	dev_crit(&ctx->afu->dev, "PSL_FIR2: 0x%.16llx\n", fir2);
+	dev_crit(&ctx->afu->dev, "PSL_SERR_An: 0x%.16llx\n", serr);
+	dev_crit(&ctx->afu->dev, "PSL_FIR_SLICE_An: 0x%.16llx\n", fir_slice);
+	dev_crit(&ctx->afu->dev, "CXL_PSL_AFU_DEBUG_An: 0x%.16llx\n", afu_debug);
+
+	dev_crit(&ctx->afu->dev, "STOPPING CXL TRACE\n");
+	cxl_stop_trace(ctx->afu->adapter);
+
+	return cxl_ack_irq(ctx, 0, errstat);
+}
+
+irqreturn_t cxl_slice_irq_err(int irq, void *data)
+{
+	struct cxl_afu *afu = data;
+	u64 fir_slice, errstat, serr, afu_debug;
+
+	WARN(irq, "CXL SLICE ERROR interrupt %i\n", irq);
+
+	serr = cxl_p1n_read(afu, CXL_PSL_SERR_An);
+	fir_slice = cxl_p1n_read(afu, CXL_PSL_FIR_SLICE_An);
+	errstat = cxl_p2n_read(afu, CXL_PSL_ErrStat_An);
+	afu_debug = cxl_p1n_read(afu, CXL_AFU_DEBUG_An);
+	dev_crit(&afu->dev, "PSL_SERR_An: 0x%.16llx\n", serr);
+	dev_crit(&afu->dev, "PSL_FIR_SLICE_An: 0x%.16llx\n", fir_slice);
+	dev_crit(&afu->dev, "CXL_PSL_ErrStat_An: 0x%.16llx\n", errstat);
+	dev_crit(&afu->dev, "CXL_PSL_AFU_DEBUG_An: 0x%.16llx\n", afu_debug);
+
+	cxl_p1n_write(afu, CXL_PSL_SERR_An, serr);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t cxl_irq_err(int irq, void *data)
+{
+	struct cxl *adapter = data;
+	u64 fir1, fir2, err_ivte;
+
+	WARN(1, "CXL ERROR interrupt %i\n", irq);
+
+	err_ivte = cxl_p1_read(adapter, CXL_PSL_ErrIVTE);
+	dev_crit(&adapter->dev, "PSL_ErrIVTE: 0x%.16llx\n", err_ivte);
+
+	dev_crit(&adapter->dev, "STOPPING CXL TRACE\n");
+	cxl_stop_trace(adapter);
+
+	fir1 = cxl_p1_read(adapter, CXL_PSL_FIR1);
+	fir2 = cxl_p1_read(adapter, CXL_PSL_FIR2);
+
+	dev_crit(&adapter->dev, "PSL_FIR1: 0x%.16llx\nPSL_FIR2: 0x%.16llx\n", fir1, fir2);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t schedule_cxl_fault(struct cxl_context *ctx, u64 dsisr, u64 dar)
+{
+	ctx->dsisr = dsisr;
+	ctx->dar = dar;
+	schedule_work(&ctx->fault_work);
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t cxl_irq(int irq, void *data)
+{
+	struct cxl_context *ctx = data;
+	struct cxl_irq_info irq_info;
+	u64 dsisr, dar;
+	int result;
+
+	if ((result = cxl_get_irq(ctx, &irq_info))) {
+		WARN(1, "Unable to get CXL IRQ Info: %i\n", result);
+		return IRQ_HANDLED;
+	}
+
+	dsisr = irq_info.dsisr;
+	dar = irq_info.dar;
+
+	pr_devel("CXL interrupt %i for afu pe: %i DSISR: %#llx DAR: %#llx\n", irq, ctx->pe, dsisr, dar);
+
+	if (dsisr & CXL_PSL_DSISR_An_DS) {
+		/*
+		 * We don't inherently need to sleep to handle this, but we do
+		 * need to get a ref to the task's mm, which we can't do from
+		 * irq context without the potential for a deadlock since it
+		 * takes the task_lock. An alternate option would be to keep a
+		 * reference to the task's mm the entire time it has cxl open,
+		 * but to do that we need to solve the issue where we hold a
+		 * ref to the mm, but the mm can hold a ref to the fd after an
+		 * mmap preventing anything from being cleaned up.
+		 */
+		pr_devel("Scheduling segment miss handling for later pe: %i\n", ctx->pe);
+		return schedule_cxl_fault(ctx, dsisr, dar);
+	}
+
+	if (dsisr & CXL_PSL_DSISR_An_M)
+		pr_devel("CXL interrupt: PTE not found\n");
+	if (dsisr & CXL_PSL_DSISR_An_P)
+		pr_devel("CXL interrupt: Storage protection violation\n");
+	if (dsisr & CXL_PSL_DSISR_An_A)
+		pr_devel("CXL interrupt: AFU lock access to write through or cache inhibited storage\n");
+	if (dsisr & CXL_PSL_DSISR_An_S)
+		pr_devel("CXL interrupt: Access was afu_wr or afu_zero\n");
+	if (dsisr & CXL_PSL_DSISR_An_K)
+		pr_devel("CXL interrupt: Access not permitted by virtual page class key protection\n");
+
+	if (dsisr & CXL_PSL_DSISR_An_DM) {
+		/*
+		 * In some cases we might be able to handle the fault
+		 * immediately if hash_page would succeed, but we still need
+		 * the task's mm, which as above we can't get without a lock
+		 */
+		pr_devel("Scheduling page fault handling for later pe: %i\n", ctx->pe);
+		return schedule_cxl_fault(ctx, dsisr, dar);
+	}
+	if (dsisr & CXL_PSL_DSISR_An_ST)
+		WARN(1, "CXL interrupt: Segment Table PTE not found\n");
+	if (dsisr & CXL_PSL_DSISR_An_UR)
+		pr_devel("CXL interrupt: AURP PTE not found\n");
+	if (dsisr & CXL_PSL_DSISR_An_PE)
+		return handle_psl_slice_error(ctx, dsisr, irq_info.errstat);
+	if (dsisr & CXL_PSL_DSISR_An_AE) {
+		pr_devel("CXL interrupt: AFU Error %.llx\n", irq_info.afu_err);
+
+		if (ctx->pending_afu_err) {
+			/*
+			 * This shouldn't happen - the PSL treats these errors
+			 * as fatal and will have reset the AFU, so there's not
+			 * much point buffering multiple AFU errors.
+			 * OTOH if we DO ever see a storm of these come in it's
+			 * probably best that we log them somewhere:
+			 */
+			dev_err_ratelimited(&ctx->afu->dev, "CXL AFU Error "
+					    "undelivered to pe %i: %.llx\n",
+					    ctx->pe, irq_info.afu_err);
+		} else {
+			spin_lock(&ctx->lock);
+			ctx->afu_err = irq_info.afu_err;
+			ctx->pending_afu_err = 1;
+			spin_unlock(&ctx->lock);
+
+			wake_up_all(&ctx->wq);
+		}
+
+		cxl_ack_irq(ctx, CXL_PSL_TFC_An_A, 0);
+	}
+	if (dsisr & CXL_PSL_DSISR_An_OC)
+		pr_devel("CXL interrupt: OS Context Warning\n");
+
+	WARN(1, "Unhandled CXL PSL IRQ\n");
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t cxl_irq_multiplexed(int irq, void *data)
+{
+	struct cxl_afu *afu = data;
+	struct cxl_context *ctx;
+	int ph = cxl_p2n_read(afu, CXL_PSL_PEHandle_An) & 0xffff;
+	int ret;
+
+	rcu_read_lock();
+	ctx = idr_find(&afu->contexts_idr, ph);
+	if (ctx) {
+		ret = cxl_irq(irq, ctx);
+		rcu_read_unlock();
+		return ret;
+	}
+	rcu_read_unlock();
+
+	WARN(1, "Unable to demultiplex CXL PSL IRQ\n");
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t cxl_irq_afu(int irq, void *data)
+{
+	struct cxl_context *ctx = data;
+	irq_hw_number_t hwirq = irqd_to_hwirq(irq_get_irq_data(irq));
+	int irq_off, afu_irq = 1;
+	__u16 range;
+	int r;
+
+	for (r = 1; r < CXL_IRQ_RANGES; r++) {
+		irq_off = hwirq - ctx->irqs.offset[r];
+		range = ctx->irqs.range[r];
+		if (irq_off >= 0 && irq_off < range) {
+			afu_irq += irq_off;
+			break;
+		}
+		afu_irq += range;
+	}
+	if (unlikely(r >= CXL_IRQ_RANGES)) {
+		WARN(1, "Recieved AFU IRQ out of range for pe %i (virq %i hwirq %lx)\n",
+		     ctx->pe, irq, hwirq);
+		return IRQ_HANDLED;
+	}
+
+	pr_devel("Received AFU interrupt %i for pe: %i (virq %i hwirq %lx)\n",
+	       afu_irq, ctx->pe, irq, hwirq);
+
+	if (unlikely(!ctx->irq_bitmap)) {
+		WARN(1, "Recieved AFU IRQ for context with no IRQ bitmap\n");
+		return IRQ_HANDLED;
+	}
+	spin_lock(&ctx->lock);
+	set_bit(afu_irq - 1, ctx->irq_bitmap);
+	ctx->pending_irq = true;
+	spin_unlock(&ctx->lock);
+
+	wake_up_all(&ctx->wq);
+
+	return IRQ_HANDLED;
+}
+
+unsigned int cxl_map_irq(struct cxl *adapter, irq_hw_number_t hwirq,
+			 irq_handler_t handler, void *cookie)
+{
+	unsigned int virq;
+	int result;
+
+	/* IRQ Domain? */
+	virq = irq_create_mapping(NULL, hwirq);
+	if (!virq) {
+		dev_warn(&adapter->dev, "cxl_map_irq: irq_create_mapping failed\n");
+		return 0;
+	}
+
+	cxl_setup_irq(adapter, hwirq, virq);
+
+	pr_devel("hwirq %#lx mapped to virq %u\n", hwirq, virq);
+
+	result = request_irq(virq, handler, 0, "cxl", cookie);
+	if (result) {
+		dev_warn(&adapter->dev, "cxl_map_irq: request_irq failed: %i\n", result);
+		return 0;
+	}
+
+	return virq;
+}
+
+void cxl_unmap_irq(unsigned int virq, void *cookie)
+{
+	free_irq(virq, cookie);
+	irq_dispose_mapping(virq);
+}
+
+static int cxl_register_one_irq(struct cxl *adapter,
+				irq_handler_t handler,
+				void *cookie,
+				irq_hw_number_t *dest_hwirq,
+				unsigned int *dest_virq)
+{
+	int hwirq, virq;
+
+	if ((hwirq = cxl_alloc_one_irq(adapter)) < 0)
+		return hwirq;
+
+	if (!(virq = cxl_map_irq(adapter, hwirq, handler, cookie)))
+		goto err;
+
+	*dest_hwirq = hwirq;
+	*dest_virq = virq;
+
+	return 0;
+
+err:
+	cxl_release_one_irq(adapter, hwirq);
+	return -ENOMEM;
+}
+
+int cxl_register_psl_err_irq(struct cxl *adapter)
+{
+	int rc;
+
+	if ((rc = cxl_register_one_irq(adapter, cxl_irq_err, adapter,
+				       &adapter->err_hwirq,
+				       &adapter->err_virq)))
+		return rc;
+
+	cxl_p1_write(adapter, CXL_PSL_ErrIVTE, adapter->err_hwirq & 0xffff);
+
+	return 0;
+}
+
+void cxl_release_psl_err_irq(struct cxl *adapter)
+{
+	cxl_p1_write(adapter, CXL_PSL_ErrIVTE, 0x0000000000000000);
+	cxl_unmap_irq(adapter->err_virq, adapter);
+	cxl_release_one_irq(adapter, adapter->err_hwirq);
+}
+
+int cxl_register_serr_irq(struct cxl_afu *afu)
+{
+	u64 serr;
+	int rc;
+
+	if ((rc = cxl_register_one_irq(afu->adapter, cxl_slice_irq_err, afu,
+				       &afu->serr_hwirq,
+				       &afu->serr_virq)))
+		return rc;
+
+	serr = cxl_p1n_read(afu, CXL_PSL_SERR_An);
+	serr = (serr & 0x00ffffffffff0000ULL) | (afu->serr_hwirq & 0xffff);
+	cxl_p1n_write(afu, CXL_PSL_SERR_An, serr);
+
+	return 0;
+}
+
+void cxl_release_serr_irq(struct cxl_afu *afu)
+{
+	cxl_p1n_write(afu, CXL_PSL_SERR_An, 0x0000000000000000);
+	cxl_unmap_irq(afu->serr_virq, afu);
+	cxl_release_one_irq(afu->adapter, afu->serr_hwirq);
+}
+
+int cxl_register_psl_irq(struct cxl_afu *afu)
+{
+	return cxl_register_one_irq(afu->adapter, cxl_irq_multiplexed, afu,
+			&afu->psl_hwirq, &afu->psl_virq);
+}
+
+void cxl_release_psl_irq(struct cxl_afu *afu)
+{
+	cxl_unmap_irq(afu->psl_virq, afu);
+	cxl_release_one_irq(afu->adapter, afu->psl_hwirq);
+}
+
+int afu_register_irqs(struct cxl_context *ctx, u32 count)
+{
+	irq_hw_number_t hwirq;
+	int rc, r, i;
+
+	if ((rc = cxl_alloc_irq_ranges(&ctx->irqs, ctx->afu->adapter, count)))
+		return rc;
+
+	/* Multiplexed PSL Interrupt */
+	ctx->irqs.offset[0] = ctx->afu->psl_hwirq;
+	ctx->irqs.range[0] = 1;
+
+	ctx->irq_count = count;
+	ctx->irq_bitmap = kcalloc(BITS_TO_LONGS(count),
+				  sizeof(*ctx->irq_bitmap), GFP_KERNEL);
+	if (!ctx->irq_bitmap)
+		return -ENOMEM;
+	for (r = 1; r < CXL_IRQ_RANGES; r++) {
+		hwirq = ctx->irqs.offset[r];
+		for (i = 0; i < ctx->irqs.range[r]; hwirq++, i++) {
+			cxl_map_irq(ctx->afu->adapter, hwirq,
+				     cxl_irq_afu, ctx);
+		}
+	}
+
+	return 0;
+}
+
+void afu_release_irqs(struct cxl_context *ctx)
+{
+	irq_hw_number_t hwirq;
+	unsigned int virq;
+	int r, i;
+
+	for (r = 1; r < CXL_IRQ_RANGES; r++) {
+		hwirq = ctx->irqs.offset[r];
+		for (i = 0; i < ctx->irqs.range[r]; hwirq++, i++) {
+			virq = irq_find_mapping(NULL, hwirq);
+			if (virq)
+				cxl_unmap_irq(virq, ctx);
+		}
+	}
+
+	cxl_release_irq_ranges(&ctx->irqs, ctx->afu->adapter);
+}
diff --git a/drivers/misc/cxl/main.c b/drivers/misc/cxl/main.c
new file mode 100644
index 000000000000..4cde9b661642
--- /dev/null
+++ b/drivers/misc/cxl/main.c
@@ -0,0 +1,230 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/spinlock.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/mutex.h>
+#include <linux/init.h>
+#include <linux/list.h>
+#include <linux/mm.h>
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/idr.h>
+#include <linux/pci.h>
+#include <asm/cputable.h>
+#include <misc/cxl.h>
+
+#include "cxl.h"
+
+static DEFINE_SPINLOCK(adapter_idr_lock);
+static DEFINE_IDR(cxl_adapter_idr);
+
+uint cxl_verbose;
+module_param_named(verbose, cxl_verbose, uint, 0600);
+MODULE_PARM_DESC(verbose, "Enable verbose dmesg output");
+
+static inline void _cxl_slbia(struct cxl_context *ctx, struct mm_struct *mm)
+{
+	struct task_struct *task;
+	unsigned long flags;
+	if (!(task = get_pid_task(ctx->pid, PIDTYPE_PID))) {
+		pr_devel("%s unable to get task %i\n",
+			 __func__, pid_nr(ctx->pid));
+		return;
+	}
+
+	if (task->mm != mm)
+		goto out_put;
+
+	pr_devel("%s matched mm - card: %i afu: %i pe: %i\n", __func__,
+		 ctx->afu->adapter->adapter_num, ctx->afu->slice, ctx->pe);
+
+	spin_lock_irqsave(&ctx->sste_lock, flags);
+	memset(ctx->sstp, 0, ctx->sst_size);
+	spin_unlock_irqrestore(&ctx->sste_lock, flags);
+	mb();
+	cxl_afu_slbia(ctx->afu);
+out_put:
+	put_task_struct(task);
+}
+
+static inline void cxl_slbia_core(struct mm_struct *mm)
+{
+	struct cxl *adapter;
+	struct cxl_afu *afu;
+	struct cxl_context *ctx;
+	int card, slice, id;
+
+	pr_devel("%s called\n", __func__);
+
+	spin_lock(&adapter_idr_lock);
+	idr_for_each_entry(&cxl_adapter_idr, adapter, card) {
+		/* XXX: Make this lookup faster with link from mm to ctx */
+		spin_lock(&adapter->afu_list_lock);
+		for (slice = 0; slice < adapter->slices; slice++) {
+			afu = adapter->afu[slice];
+			if (!afu->enabled)
+				continue;
+			rcu_read_lock();
+			idr_for_each_entry(&afu->contexts_idr, ctx, id)
+				_cxl_slbia(ctx, mm);
+			rcu_read_unlock();
+		}
+		spin_unlock(&adapter->afu_list_lock);
+	}
+	spin_unlock(&adapter_idr_lock);
+}
+
+static struct cxl_calls cxl_calls = {
+	.cxl_slbia = cxl_slbia_core,
+	.owner = THIS_MODULE,
+};
+
+int cxl_alloc_sst(struct cxl_context *ctx)
+{
+	unsigned long vsid;
+	u64 ea_mask, size, sstp0, sstp1;
+
+	sstp0 = 0;
+	sstp1 = 0;
+
+	ctx->sst_size = PAGE_SIZE;
+	ctx->sst_lru = 0;
+	ctx->sstp = (struct cxl_sste *)get_zeroed_page(GFP_KERNEL);
+	if (!ctx->sstp) {
+		pr_err("cxl_alloc_sst: Unable to allocate segment table\n");
+		return -ENOMEM;
+	}
+	pr_devel("SSTP allocated at 0x%p\n", ctx->sstp);
+
+	vsid  = get_kernel_vsid((u64)ctx->sstp, mmu_kernel_ssize) << 12;
+
+	sstp0 |= (u64)mmu_kernel_ssize << CXL_SSTP0_An_B_SHIFT;
+	sstp0 |= (SLB_VSID_KERNEL | mmu_psize_defs[mmu_linear_psize].sllp) << 50;
+
+	size = (((u64)ctx->sst_size >> 8) - 1) << CXL_SSTP0_An_SegTableSize_SHIFT;
+	if (unlikely(size & ~CXL_SSTP0_An_SegTableSize_MASK)) {
+		WARN(1, "Impossible segment table size\n");
+		return -EINVAL;
+	}
+	sstp0 |= size;
+
+	if (mmu_kernel_ssize == MMU_SEGSIZE_256M)
+		ea_mask = 0xfffff00ULL;
+	else
+		ea_mask = 0xffffffff00ULL;
+
+	sstp0 |=  vsid >>     (50-14);  /*   Top 14 bits of VSID */
+	sstp1 |= (vsid << (64-(50-14))) & ~ea_mask;
+	sstp1 |= (u64)ctx->sstp & ea_mask;
+	sstp1 |= CXL_SSTP1_An_V;
+
+	pr_devel("Looked up %#llx: slbfee. %#llx (ssize: %x, vsid: %#lx), copied to SSTP0: %#llx, SSTP1: %#llx\n",
+			(u64)ctx->sstp, (u64)ctx->sstp & ESID_MASK, mmu_kernel_ssize, vsid, sstp0, sstp1);
+
+	/* Store calculated sstp hardware points for use later */
+	ctx->sstp0 = sstp0;
+	ctx->sstp1 = sstp1;
+
+	return 0;
+}
+
+/* Find a CXL adapter by it's number and increase it's refcount */
+struct cxl *get_cxl_adapter(int num)
+{
+	struct cxl *adapter;
+
+	spin_lock(&adapter_idr_lock);
+	if ((adapter = idr_find(&cxl_adapter_idr, num)))
+		get_device(&adapter->dev);
+	spin_unlock(&adapter_idr_lock);
+
+	return adapter;
+}
+
+int cxl_alloc_adapter_nr(struct cxl *adapter)
+{
+	int i;
+
+	idr_preload(GFP_KERNEL);
+	spin_lock(&adapter_idr_lock);
+	i = idr_alloc(&cxl_adapter_idr, adapter, 0, 0, GFP_NOWAIT);
+	spin_unlock(&adapter_idr_lock);
+	idr_preload_end();
+	if (i < 0)
+		return i;
+
+	adapter->adapter_num = i;
+
+	return 0;
+}
+
+void cxl_remove_adapter_nr(struct cxl *adapter)
+{
+	idr_remove(&cxl_adapter_idr, adapter->adapter_num);
+}
+
+int cxl_afu_select_best_mode(struct cxl_afu *afu)
+{
+	if (afu->modes_supported & CXL_MODE_DIRECTED)
+		return cxl_afu_activate_mode(afu, CXL_MODE_DIRECTED);
+
+	if (afu->modes_supported & CXL_MODE_DEDICATED)
+		return cxl_afu_activate_mode(afu, CXL_MODE_DEDICATED);
+
+	dev_warn(&afu->dev, "No supported programming modes available\n");
+	/* We don't fail this so the user can inspect sysfs */
+	return 0;
+}
+
+static int __init init_cxl(void)
+{
+	int rc = 0;
+
+	if (!cpu_has_feature(CPU_FTR_HVMODE))
+		return -EPERM;
+
+	if ((rc = cxl_file_init()))
+		return rc;
+
+	cxl_debugfs_init();
+
+	if ((rc = register_cxl_calls(&cxl_calls)))
+		goto err;
+
+	if ((rc = pci_register_driver(&cxl_pci_driver)))
+		goto err1;
+
+	return 0;
+err1:
+	unregister_cxl_calls(&cxl_calls);
+err:
+	cxl_debugfs_exit();
+	cxl_file_exit();
+
+	return rc;
+}
+
+static void exit_cxl(void)
+{
+	pci_unregister_driver(&cxl_pci_driver);
+
+	cxl_debugfs_exit();
+	cxl_file_exit();
+	unregister_cxl_calls(&cxl_calls);
+}
+
+module_init(init_cxl);
+module_exit(exit_cxl);
+
+MODULE_DESCRIPTION("IBM Coherent Accelerator");
+MODULE_AUTHOR("Ian Munsie <imunsie@au1.ibm.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/misc/cxl/native.c b/drivers/misc/cxl/native.c
new file mode 100644
index 000000000000..623286a77114
--- /dev/null
+++ b/drivers/misc/cxl/native.c
@@ -0,0 +1,683 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/slab.h>
+#include <linux/sched.h>
+#include <linux/mutex.h>
+#include <linux/mm.h>
+#include <linux/uaccess.h>
+#include <asm/synch.h>
+#include <misc/cxl.h>
+
+#include "cxl.h"
+
+static int afu_control(struct cxl_afu *afu, u64 command,
+		       u64 result, u64 mask, bool enabled)
+{
+	u64 AFU_Cntl = cxl_p2n_read(afu, CXL_AFU_Cntl_An);
+	unsigned long timeout = jiffies + (HZ * CXL_TIMEOUT);
+
+	spin_lock(&afu->afu_cntl_lock);
+	pr_devel("AFU command starting: %llx\n", command);
+
+	cxl_p2n_write(afu, CXL_AFU_Cntl_An, AFU_Cntl | command);
+
+	AFU_Cntl = cxl_p2n_read(afu, CXL_AFU_Cntl_An);
+	while ((AFU_Cntl & mask) != result) {
+		if (time_after_eq(jiffies, timeout)) {
+			dev_warn(&afu->dev, "WARNING: AFU control timed out!\n");
+			spin_unlock(&afu->afu_cntl_lock);
+			return -EBUSY;
+		}
+		pr_devel_ratelimited("AFU control... (0x%.16llx)\n",
+				     AFU_Cntl | command);
+		cpu_relax();
+		AFU_Cntl = cxl_p2n_read(afu, CXL_AFU_Cntl_An);
+	};
+	pr_devel("AFU command complete: %llx\n", command);
+	afu->enabled = enabled;
+	spin_unlock(&afu->afu_cntl_lock);
+
+	return 0;
+}
+
+static int afu_enable(struct cxl_afu *afu)
+{
+	pr_devel("AFU enable request\n");
+
+	return afu_control(afu, CXL_AFU_Cntl_An_E,
+			   CXL_AFU_Cntl_An_ES_Enabled,
+			   CXL_AFU_Cntl_An_ES_MASK, true);
+}
+
+int cxl_afu_disable(struct cxl_afu *afu)
+{
+	pr_devel("AFU disable request\n");
+
+	return afu_control(afu, 0, CXL_AFU_Cntl_An_ES_Disabled,
+			   CXL_AFU_Cntl_An_ES_MASK, false);
+}
+
+/* This will disable as well as reset */
+int cxl_afu_reset(struct cxl_afu *afu)
+{
+	pr_devel("AFU reset request\n");
+
+	return afu_control(afu, CXL_AFU_Cntl_An_RA,
+			   CXL_AFU_Cntl_An_RS_Complete | CXL_AFU_Cntl_An_ES_Disabled,
+			   CXL_AFU_Cntl_An_RS_MASK | CXL_AFU_Cntl_An_ES_MASK,
+			   false);
+}
+
+static int afu_check_and_enable(struct cxl_afu *afu)
+{
+	if (afu->enabled)
+		return 0;
+	return afu_enable(afu);
+}
+
+int cxl_psl_purge(struct cxl_afu *afu)
+{
+	u64 PSL_CNTL = cxl_p1n_read(afu, CXL_PSL_SCNTL_An);
+	u64 AFU_Cntl = cxl_p2n_read(afu, CXL_AFU_Cntl_An);
+	u64 dsisr, dar;
+	u64 start, end;
+	unsigned long timeout = jiffies + (HZ * CXL_TIMEOUT);
+
+	pr_devel("PSL purge request\n");
+
+	if ((AFU_Cntl & CXL_AFU_Cntl_An_ES_MASK) != CXL_AFU_Cntl_An_ES_Disabled) {
+		WARN(1, "psl_purge request while AFU not disabled!\n");
+		cxl_afu_disable(afu);
+	}
+
+	cxl_p1n_write(afu, CXL_PSL_SCNTL_An,
+		       PSL_CNTL | CXL_PSL_SCNTL_An_Pc);
+	start = local_clock();
+	PSL_CNTL = cxl_p1n_read(afu, CXL_PSL_SCNTL_An);
+	while ((PSL_CNTL &  CXL_PSL_SCNTL_An_Ps_MASK)
+			== CXL_PSL_SCNTL_An_Ps_Pending) {
+		if (time_after_eq(jiffies, timeout)) {
+			dev_warn(&afu->dev, "WARNING: PSL Purge timed out!\n");
+			return -EBUSY;
+		}
+		dsisr = cxl_p2n_read(afu, CXL_PSL_DSISR_An);
+		pr_devel_ratelimited("PSL purging... PSL_CNTL: 0x%.16llx  PSL_DSISR: 0x%.16llx\n", PSL_CNTL, dsisr);
+		if (dsisr & CXL_PSL_DSISR_TRANS) {
+			dar = cxl_p2n_read(afu, CXL_PSL_DAR_An);
+			dev_notice(&afu->dev, "PSL purge terminating pending translation, DSISR: 0x%.16llx, DAR: 0x%.16llx\n", dsisr, dar);
+			cxl_p2n_write(afu, CXL_PSL_TFC_An, CXL_PSL_TFC_An_AE);
+		} else if (dsisr) {
+			dev_notice(&afu->dev, "PSL purge acknowledging pending non-translation fault, DSISR: 0x%.16llx\n", dsisr);
+			cxl_p2n_write(afu, CXL_PSL_TFC_An, CXL_PSL_TFC_An_A);
+		} else {
+			cpu_relax();
+		}
+		PSL_CNTL = cxl_p1n_read(afu, CXL_PSL_SCNTL_An);
+	};
+	end = local_clock();
+	pr_devel("PSL purged in %lld ns\n", end - start);
+
+	cxl_p1n_write(afu, CXL_PSL_SCNTL_An,
+		       PSL_CNTL & ~CXL_PSL_SCNTL_An_Pc);
+	return 0;
+}
+
+static int spa_max_procs(int spa_size)
+{
+	/*
+	 * From the CAIA:
+	 *    end_of_SPA_area = SPA_Base + ((n+4) * 128) + (( ((n*8) + 127) >> 7) * 128) + 255
+	 * Most of that junk is really just an overly-complicated way of saying
+	 * the last 256 bytes are __aligned(128), so it's really:
+	 *    end_of_SPA_area = end_of_PSL_queue_area + __aligned(128) 255
+	 * and
+	 *    end_of_PSL_queue_area = SPA_Base + ((n+4) * 128) + (n*8) - 1
+	 * so
+	 *    sizeof(SPA) = ((n+4) * 128) + (n*8) + __aligned(128) 256
+	 * Ignore the alignment (which is safe in this case as long as we are
+	 * careful with our rounding) and solve for n:
+	 */
+	return ((spa_size / 8) - 96) / 17;
+}
+
+static int alloc_spa(struct cxl_afu *afu)
+{
+	u64 spap;
+
+	/* Work out how many pages to allocate */
+	afu->spa_order = 0;
+	do {
+		afu->spa_order++;
+		afu->spa_size = (1 << afu->spa_order) * PAGE_SIZE;
+		afu->spa_max_procs = spa_max_procs(afu->spa_size);
+	} while (afu->spa_max_procs < afu->num_procs);
+
+	WARN_ON(afu->spa_size > 0x100000); /* Max size supported by the hardware */
+
+	if (!(afu->spa = (struct cxl_process_element *)
+	      __get_free_pages(GFP_KERNEL | __GFP_ZERO, afu->spa_order))) {
+		pr_err("cxl_alloc_spa: Unable to allocate scheduled process area\n");
+		return -ENOMEM;
+	}
+	pr_devel("spa pages: %i afu->spa_max_procs: %i   afu->num_procs: %i\n",
+		 1<<afu->spa_order, afu->spa_max_procs, afu->num_procs);
+
+	afu->sw_command_status = (__be64 *)((char *)afu->spa +
+					    ((afu->spa_max_procs + 3) * 128));
+
+	spap = virt_to_phys(afu->spa) & CXL_PSL_SPAP_Addr;
+	spap |= ((afu->spa_size >> (12 - CXL_PSL_SPAP_Size_Shift)) - 1) & CXL_PSL_SPAP_Size;
+	spap |= CXL_PSL_SPAP_V;
+	pr_devel("cxl: SPA allocated at 0x%p. Max processes: %i, sw_command_status: 0x%p CXL_PSL_SPAP_An=0x%016llx\n", afu->spa, afu->spa_max_procs, afu->sw_command_status, spap);
+	cxl_p1n_write(afu, CXL_PSL_SPAP_An, spap);
+
+	return 0;
+}
+
+static void release_spa(struct cxl_afu *afu)
+{
+	free_pages((unsigned long) afu->spa, afu->spa_order);
+}
+
+int cxl_tlb_slb_invalidate(struct cxl *adapter)
+{
+	unsigned long timeout = jiffies + (HZ * CXL_TIMEOUT);
+
+	pr_devel("CXL adapter wide TLBIA & SLBIA\n");
+
+	cxl_p1_write(adapter, CXL_PSL_AFUSEL, CXL_PSL_AFUSEL_A);
+
+	cxl_p1_write(adapter, CXL_PSL_TLBIA, CXL_TLB_SLB_IQ_ALL);
+	while (cxl_p1_read(adapter, CXL_PSL_TLBIA) & CXL_TLB_SLB_P) {
+		if (time_after_eq(jiffies, timeout)) {
+			dev_warn(&adapter->dev, "WARNING: CXL adapter wide TLBIA timed out!\n");
+			return -EBUSY;
+		}
+		cpu_relax();
+	}
+
+	cxl_p1_write(adapter, CXL_PSL_SLBIA, CXL_TLB_SLB_IQ_ALL);
+	while (cxl_p1_read(adapter, CXL_PSL_SLBIA) & CXL_TLB_SLB_P) {
+		if (time_after_eq(jiffies, timeout)) {
+			dev_warn(&adapter->dev, "WARNING: CXL adapter wide SLBIA timed out!\n");
+			return -EBUSY;
+		}
+		cpu_relax();
+	}
+	return 0;
+}
+
+int cxl_afu_slbia(struct cxl_afu *afu)
+{
+	unsigned long timeout = jiffies + (HZ * CXL_TIMEOUT);
+
+	pr_devel("cxl_afu_slbia issuing SLBIA command\n");
+	cxl_p2n_write(afu, CXL_SLBIA_An, CXL_TLB_SLB_IQ_ALL);
+	while (cxl_p2n_read(afu, CXL_SLBIA_An) & CXL_TLB_SLB_P) {
+		if (time_after_eq(jiffies, timeout)) {
+			dev_warn(&afu->dev, "WARNING: CXL AFU SLBIA timed out!\n");
+			return -EBUSY;
+		}
+		cpu_relax();
+	}
+	return 0;
+}
+
+static int cxl_write_sstp(struct cxl_afu *afu, u64 sstp0, u64 sstp1)
+{
+	int rc;
+
+	/* 1. Disable SSTP by writing 0 to SSTP1[V] */
+	cxl_p2n_write(afu, CXL_SSTP1_An, 0);
+
+	/* 2. Invalidate all SLB entries */
+	if ((rc = cxl_afu_slbia(afu)))
+		return rc;
+
+	/* 3. Set SSTP0_An */
+	cxl_p2n_write(afu, CXL_SSTP0_An, sstp0);
+
+	/* 4. Set SSTP1_An */
+	cxl_p2n_write(afu, CXL_SSTP1_An, sstp1);
+
+	return 0;
+}
+
+/* Using per slice version may improve performance here. (ie. SLBIA_An) */
+static void slb_invalid(struct cxl_context *ctx)
+{
+	struct cxl *adapter = ctx->afu->adapter;
+	u64 slbia;
+
+	WARN_ON(!mutex_is_locked(&ctx->afu->spa_mutex));
+
+	cxl_p1_write(adapter, CXL_PSL_LBISEL,
+			((u64)be32_to_cpu(ctx->elem->common.pid) << 32) |
+			be32_to_cpu(ctx->elem->lpid));
+	cxl_p1_write(adapter, CXL_PSL_SLBIA, CXL_TLB_SLB_IQ_LPIDPID);
+
+	while (1) {
+		slbia = cxl_p1_read(adapter, CXL_PSL_SLBIA);
+		if (!(slbia & CXL_TLB_SLB_P))
+			break;
+		cpu_relax();
+	}
+}
+
+static int do_process_element_cmd(struct cxl_context *ctx,
+				  u64 cmd, u64 pe_state)
+{
+	u64 state;
+
+	WARN_ON(!ctx->afu->enabled);
+
+	ctx->elem->software_state = cpu_to_be32(pe_state);
+	smp_wmb();
+	*(ctx->afu->sw_command_status) = cpu_to_be64(cmd | 0 | ctx->pe);
+	smp_mb();
+	cxl_p1n_write(ctx->afu, CXL_PSL_LLCMD_An, cmd | ctx->pe);
+	while (1) {
+		state = be64_to_cpup(ctx->afu->sw_command_status);
+		if (state == ~0ULL) {
+			pr_err("cxl: Error adding process element to AFU\n");
+			return -1;
+		}
+		if ((state & (CXL_SPA_SW_CMD_MASK | CXL_SPA_SW_STATE_MASK  | CXL_SPA_SW_LINK_MASK)) ==
+		    (cmd | (cmd >> 16) | ctx->pe))
+			break;
+		/*
+		 * The command won't finish in the PSL if there are
+		 * outstanding DSIs.  Hence we need to yield here in
+		 * case there are outstanding DSIs that we need to
+		 * service.  Tuning possiblity: we could wait for a
+		 * while before sched
+		 */
+		schedule();
+
+	}
+	return 0;
+}
+
+static int add_process_element(struct cxl_context *ctx)
+{
+	int rc = 0;
+
+	mutex_lock(&ctx->afu->spa_mutex);
+	pr_devel("%s Adding pe: %i started\n", __func__, ctx->pe);
+	if (!(rc = do_process_element_cmd(ctx, CXL_SPA_SW_CMD_ADD, CXL_PE_SOFTWARE_STATE_V)))
+		ctx->pe_inserted = true;
+	pr_devel("%s Adding pe: %i finished\n", __func__, ctx->pe);
+	mutex_unlock(&ctx->afu->spa_mutex);
+	return rc;
+}
+
+static int terminate_process_element(struct cxl_context *ctx)
+{
+	int rc = 0;
+
+	/* fast path terminate if it's already invalid */
+	if (!(ctx->elem->software_state & cpu_to_be32(CXL_PE_SOFTWARE_STATE_V)))
+		return rc;
+
+	mutex_lock(&ctx->afu->spa_mutex);
+	pr_devel("%s Terminate pe: %i started\n", __func__, ctx->pe);
+	rc = do_process_element_cmd(ctx, CXL_SPA_SW_CMD_TERMINATE,
+				    CXL_PE_SOFTWARE_STATE_V | CXL_PE_SOFTWARE_STATE_T);
+	ctx->elem->software_state = 0;	/* Remove Valid bit */
+	pr_devel("%s Terminate pe: %i finished\n", __func__, ctx->pe);
+	mutex_unlock(&ctx->afu->spa_mutex);
+	return rc;
+}
+
+static int remove_process_element(struct cxl_context *ctx)
+{
+	int rc = 0;
+
+	mutex_lock(&ctx->afu->spa_mutex);
+	pr_devel("%s Remove pe: %i started\n", __func__, ctx->pe);
+	if (!(rc = do_process_element_cmd(ctx, CXL_SPA_SW_CMD_REMOVE, 0)))
+		ctx->pe_inserted = false;
+	slb_invalid(ctx);
+	pr_devel("%s Remove pe: %i finished\n", __func__, ctx->pe);
+	mutex_unlock(&ctx->afu->spa_mutex);
+
+	return rc;
+}
+
+
+static void assign_psn_space(struct cxl_context *ctx)
+{
+	if (!ctx->afu->pp_size || ctx->master) {
+		ctx->psn_phys = ctx->afu->psn_phys;
+		ctx->psn_size = ctx->afu->adapter->ps_size;
+	} else {
+		ctx->psn_phys = ctx->afu->psn_phys +
+			(ctx->afu->pp_offset + ctx->afu->pp_size * ctx->pe);
+		ctx->psn_size = ctx->afu->pp_size;
+	}
+}
+
+static int activate_afu_directed(struct cxl_afu *afu)
+{
+	int rc;
+
+	dev_info(&afu->dev, "Activating AFU directed mode\n");
+
+	if (alloc_spa(afu))
+		return -ENOMEM;
+
+	cxl_p1n_write(afu, CXL_PSL_SCNTL_An, CXL_PSL_SCNTL_An_PM_AFU);
+	cxl_p1n_write(afu, CXL_PSL_AMOR_An, 0xFFFFFFFFFFFFFFFFULL);
+	cxl_p1n_write(afu, CXL_PSL_ID_An, CXL_PSL_ID_An_F | CXL_PSL_ID_An_L);
+
+	afu->current_mode = CXL_MODE_DIRECTED;
+	afu->num_procs = afu->max_procs_virtualised;
+
+	if ((rc = cxl_chardev_m_afu_add(afu)))
+		return rc;
+
+	if ((rc = cxl_sysfs_afu_m_add(afu)))
+		goto err;
+
+	if ((rc = cxl_chardev_s_afu_add(afu)))
+		goto err1;
+
+	return 0;
+err1:
+	cxl_sysfs_afu_m_remove(afu);
+err:
+	cxl_chardev_afu_remove(afu);
+	return rc;
+}
+
+#ifdef CONFIG_CPU_LITTLE_ENDIAN
+#define set_endian(sr) ((sr) |= CXL_PSL_SR_An_LE)
+#else
+#define set_endian(sr) ((sr) &= ~(CXL_PSL_SR_An_LE))
+#endif
+
+static int attach_afu_directed(struct cxl_context *ctx, u64 wed, u64 amr)
+{
+	u64 sr;
+	int r, result;
+
+	assign_psn_space(ctx);
+
+	ctx->elem->ctxtime = 0; /* disable */
+	ctx->elem->lpid = cpu_to_be32(mfspr(SPRN_LPID));
+	ctx->elem->haurp = 0; /* disable */
+	ctx->elem->sdr = cpu_to_be64(mfspr(SPRN_SDR1));
+
+	sr = CXL_PSL_SR_An_SC;
+	if (ctx->master)
+		sr |= CXL_PSL_SR_An_MP;
+	if (mfspr(SPRN_LPCR) & LPCR_TC)
+		sr |= CXL_PSL_SR_An_TC;
+	/* HV=0, PR=1, R=1 for userspace
+	 * For kernel contexts: this would need to change
+	 */
+	sr |= CXL_PSL_SR_An_PR | CXL_PSL_SR_An_R;
+	set_endian(sr);
+	sr &= ~(CXL_PSL_SR_An_HV);
+	if (!test_tsk_thread_flag(current, TIF_32BIT))
+		sr |= CXL_PSL_SR_An_SF;
+	ctx->elem->common.pid = cpu_to_be32(current->pid);
+	ctx->elem->common.tid = 0;
+	ctx->elem->sr = cpu_to_be64(sr);
+
+	ctx->elem->common.csrp = 0; /* disable */
+	ctx->elem->common.aurp0 = 0; /* disable */
+	ctx->elem->common.aurp1 = 0; /* disable */
+
+	cxl_prefault(ctx, wed);
+
+	ctx->elem->common.sstp0 = cpu_to_be64(ctx->sstp0);
+	ctx->elem->common.sstp1 = cpu_to_be64(ctx->sstp1);
+
+	for (r = 0; r < CXL_IRQ_RANGES; r++) {
+		ctx->elem->ivte_offsets[r] = cpu_to_be16(ctx->irqs.offset[r]);
+		ctx->elem->ivte_ranges[r] = cpu_to_be16(ctx->irqs.range[r]);
+	}
+
+	ctx->elem->common.amr = cpu_to_be64(amr);
+	ctx->elem->common.wed = cpu_to_be64(wed);
+
+	/* first guy needs to enable */
+	if ((result = afu_check_and_enable(ctx->afu)))
+		return result;
+
+	add_process_element(ctx);
+
+	return 0;
+}
+
+static int deactivate_afu_directed(struct cxl_afu *afu)
+{
+	dev_info(&afu->dev, "Deactivating AFU directed mode\n");
+
+	afu->current_mode = 0;
+	afu->num_procs = 0;
+
+	cxl_sysfs_afu_m_remove(afu);
+	cxl_chardev_afu_remove(afu);
+
+	cxl_afu_reset(afu);
+	cxl_afu_disable(afu);
+	cxl_psl_purge(afu);
+
+	release_spa(afu);
+
+	return 0;
+}
+
+static int activate_dedicated_process(struct cxl_afu *afu)
+{
+	dev_info(&afu->dev, "Activating dedicated process mode\n");
+
+	cxl_p1n_write(afu, CXL_PSL_SCNTL_An, CXL_PSL_SCNTL_An_PM_Process);
+
+	cxl_p1n_write(afu, CXL_PSL_CtxTime_An, 0); /* disable */
+	cxl_p1n_write(afu, CXL_PSL_SPAP_An, 0);    /* disable */
+	cxl_p1n_write(afu, CXL_PSL_AMOR_An, 0xFFFFFFFFFFFFFFFFULL);
+	cxl_p1n_write(afu, CXL_PSL_LPID_An, mfspr(SPRN_LPID));
+	cxl_p1n_write(afu, CXL_HAURP_An, 0);       /* disable */
+	cxl_p1n_write(afu, CXL_PSL_SDR_An, mfspr(SPRN_SDR1));
+
+	cxl_p2n_write(afu, CXL_CSRP_An, 0);        /* disable */
+	cxl_p2n_write(afu, CXL_AURP0_An, 0);       /* disable */
+	cxl_p2n_write(afu, CXL_AURP1_An, 0);       /* disable */
+
+	afu->current_mode = CXL_MODE_DEDICATED;
+	afu->num_procs = 1;
+
+	return cxl_chardev_d_afu_add(afu);
+}
+
+static int attach_dedicated(struct cxl_context *ctx, u64 wed, u64 amr)
+{
+	struct cxl_afu *afu = ctx->afu;
+	u64 sr;
+	int rc;
+
+	sr = CXL_PSL_SR_An_SC;
+	set_endian(sr);
+	if (ctx->master)
+		sr |= CXL_PSL_SR_An_MP;
+	if (mfspr(SPRN_LPCR) & LPCR_TC)
+		sr |= CXL_PSL_SR_An_TC;
+	sr |= CXL_PSL_SR_An_PR | CXL_PSL_SR_An_R;
+	if (!test_tsk_thread_flag(current, TIF_32BIT))
+		sr |= CXL_PSL_SR_An_SF;
+	cxl_p2n_write(afu, CXL_PSL_PID_TID_An, (u64)current->pid << 32);
+	cxl_p1n_write(afu, CXL_PSL_SR_An, sr);
+
+	if ((rc = cxl_write_sstp(afu, ctx->sstp0, ctx->sstp1)))
+		return rc;
+
+	cxl_prefault(ctx, wed);
+
+	cxl_p1n_write(afu, CXL_PSL_IVTE_Offset_An,
+		       (((u64)ctx->irqs.offset[0] & 0xffff) << 48) |
+		       (((u64)ctx->irqs.offset[1] & 0xffff) << 32) |
+		       (((u64)ctx->irqs.offset[2] & 0xffff) << 16) |
+			((u64)ctx->irqs.offset[3] & 0xffff));
+	cxl_p1n_write(afu, CXL_PSL_IVTE_Limit_An, (u64)
+		       (((u64)ctx->irqs.range[0] & 0xffff) << 48) |
+		       (((u64)ctx->irqs.range[1] & 0xffff) << 32) |
+		       (((u64)ctx->irqs.range[2] & 0xffff) << 16) |
+			((u64)ctx->irqs.range[3] & 0xffff));
+
+	cxl_p2n_write(afu, CXL_PSL_AMR_An, amr);
+
+	/* master only context for dedicated */
+	assign_psn_space(ctx);
+
+	if ((rc = cxl_afu_reset(afu)))
+		return rc;
+
+	cxl_p2n_write(afu, CXL_PSL_WED_An, wed);
+
+	return afu_enable(afu);
+}
+
+static int deactivate_dedicated_process(struct cxl_afu *afu)
+{
+	dev_info(&afu->dev, "Deactivating dedicated process mode\n");
+
+	afu->current_mode = 0;
+	afu->num_procs = 0;
+
+	cxl_chardev_afu_remove(afu);
+
+	return 0;
+}
+
+int _cxl_afu_deactivate_mode(struct cxl_afu *afu, int mode)
+{
+	if (mode == CXL_MODE_DIRECTED)
+		return deactivate_afu_directed(afu);
+	if (mode == CXL_MODE_DEDICATED)
+		return deactivate_dedicated_process(afu);
+	return 0;
+}
+
+int cxl_afu_deactivate_mode(struct cxl_afu *afu)
+{
+	return _cxl_afu_deactivate_mode(afu, afu->current_mode);
+}
+
+int cxl_afu_activate_mode(struct cxl_afu *afu, int mode)
+{
+	if (!mode)
+		return 0;
+	if (!(mode & afu->modes_supported))
+		return -EINVAL;
+
+	if (mode == CXL_MODE_DIRECTED)
+		return activate_afu_directed(afu);
+	if (mode == CXL_MODE_DEDICATED)
+		return activate_dedicated_process(afu);
+
+	return -EINVAL;
+}
+
+int cxl_attach_process(struct cxl_context *ctx, bool kernel, u64 wed, u64 amr)
+{
+	ctx->kernel = kernel;
+	if (ctx->afu->current_mode == CXL_MODE_DIRECTED)
+		return attach_afu_directed(ctx, wed, amr);
+
+	if (ctx->afu->current_mode == CXL_MODE_DEDICATED)
+		return attach_dedicated(ctx, wed, amr);
+
+	return -EINVAL;
+}
+
+static inline int detach_process_native_dedicated(struct cxl_context *ctx)
+{
+	cxl_afu_reset(ctx->afu);
+	cxl_afu_disable(ctx->afu);
+	cxl_psl_purge(ctx->afu);
+	return 0;
+}
+
+/*
+ * TODO: handle case when this is called inside a rcu_read_lock() which may
+ * happen when we unbind the driver (ie. cxl_context_detach_all()) .  Terminate
+ * & remove use a mutex lock and schedule which will not good with lock held.
+ * May need to write do_process_element_cmd() that handles outstanding page
+ * faults synchronously.
+ */
+static inline int detach_process_native_afu_directed(struct cxl_context *ctx)
+{
+	if (!ctx->pe_inserted)
+		return 0;
+	if (terminate_process_element(ctx))
+		return -1;
+	if (remove_process_element(ctx))
+		return -1;
+
+	return 0;
+}
+
+int cxl_detach_process(struct cxl_context *ctx)
+{
+	if (ctx->afu->current_mode == CXL_MODE_DEDICATED)
+		return detach_process_native_dedicated(ctx);
+
+	return detach_process_native_afu_directed(ctx);
+}
+
+int cxl_get_irq(struct cxl_context *ctx, struct cxl_irq_info *info)
+{
+	u64 pidtid;
+
+	info->dsisr = cxl_p2n_read(ctx->afu, CXL_PSL_DSISR_An);
+	info->dar = cxl_p2n_read(ctx->afu, CXL_PSL_DAR_An);
+	info->dsr = cxl_p2n_read(ctx->afu, CXL_PSL_DSR_An);
+	pidtid = cxl_p2n_read(ctx->afu, CXL_PSL_PID_TID_An);
+	info->pid = pidtid >> 32;
+	info->tid = pidtid & 0xffffffff;
+	info->afu_err = cxl_p2n_read(ctx->afu, CXL_AFU_ERR_An);
+	info->errstat = cxl_p2n_read(ctx->afu, CXL_PSL_ErrStat_An);
+
+	return 0;
+}
+
+static void recover_psl_err(struct cxl_afu *afu, u64 errstat)
+{
+	u64 dsisr;
+
+	pr_devel("RECOVERING FROM PSL ERROR... (0x%.16llx)\n", errstat);
+
+	/* Clear PSL_DSISR[PE] */
+	dsisr = cxl_p2n_read(afu, CXL_PSL_DSISR_An);
+	cxl_p2n_write(afu, CXL_PSL_DSISR_An, dsisr & ~CXL_PSL_DSISR_An_PE);
+
+	/* Write 1s to clear error status bits */
+	cxl_p2n_write(afu, CXL_PSL_ErrStat_An, errstat);
+}
+
+int cxl_ack_irq(struct cxl_context *ctx, u64 tfc, u64 psl_reset_mask)
+{
+	if (tfc)
+		cxl_p2n_write(ctx->afu, CXL_PSL_TFC_An, tfc);
+	if (psl_reset_mask)
+		recover_psl_err(ctx->afu, psl_reset_mask);
+
+	return 0;
+}
+
+int cxl_check_error(struct cxl_afu *afu)
+{
+	return (cxl_p1n_read(afu, CXL_PSL_SCNTL_An) == ~0ULL);
+}
diff --git a/drivers/misc/cxl/pci.c b/drivers/misc/cxl/pci.c
new file mode 100644
index 000000000000..10c98ab7f46e
--- /dev/null
+++ b/drivers/misc/cxl/pci.c
@@ -0,0 +1,1000 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/pci_regs.h>
+#include <linux/pci_ids.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/sort.h>
+#include <linux/pci.h>
+#include <linux/of.h>
+#include <linux/delay.h>
+#include <asm/opal.h>
+#include <asm/msi_bitmap.h>
+#include <asm/pci-bridge.h> /* for struct pci_controller */
+#include <asm/pnv-pci.h>
+
+#include "cxl.h"
+
+
+#define CXL_PCI_VSEC_ID	0x1280
+#define CXL_VSEC_MIN_SIZE 0x80
+
+#define CXL_READ_VSEC_LENGTH(dev, vsec, dest)			\
+	{							\
+		pci_read_config_word(dev, vsec + 0x6, dest);	\
+		*dest >>= 4;					\
+	}
+#define CXL_READ_VSEC_NAFUS(dev, vsec, dest) \
+	pci_read_config_byte(dev, vsec + 0x8, dest)
+
+#define CXL_READ_VSEC_STATUS(dev, vsec, dest) \
+	pci_read_config_byte(dev, vsec + 0x9, dest)
+#define CXL_STATUS_SECOND_PORT  0x80
+#define CXL_STATUS_MSI_X_FULL   0x40
+#define CXL_STATUS_MSI_X_SINGLE 0x20
+#define CXL_STATUS_FLASH_RW     0x08
+#define CXL_STATUS_FLASH_RO     0x04
+#define CXL_STATUS_LOADABLE_AFU 0x02
+#define CXL_STATUS_LOADABLE_PSL 0x01
+/* If we see these features we won't try to use the card */
+#define CXL_UNSUPPORTED_FEATURES \
+	(CXL_STATUS_MSI_X_FULL | CXL_STATUS_MSI_X_SINGLE)
+
+#define CXL_READ_VSEC_MODE_CONTROL(dev, vsec, dest) \
+	pci_read_config_byte(dev, vsec + 0xa, dest)
+#define CXL_WRITE_VSEC_MODE_CONTROL(dev, vsec, val) \
+	pci_write_config_byte(dev, vsec + 0xa, val)
+#define CXL_VSEC_PROTOCOL_MASK   0xe0
+#define CXL_VSEC_PROTOCOL_1024TB 0x80
+#define CXL_VSEC_PROTOCOL_512TB  0x40
+#define CXL_VSEC_PROTOCOL_256TB  0x20 /* Power 8 uses this */
+#define CXL_VSEC_PROTOCOL_ENABLE 0x01
+
+#define CXL_READ_VSEC_PSL_REVISION(dev, vsec, dest) \
+	pci_read_config_word(dev, vsec + 0xc, dest)
+#define CXL_READ_VSEC_CAIA_MINOR(dev, vsec, dest) \
+	pci_read_config_byte(dev, vsec + 0xe, dest)
+#define CXL_READ_VSEC_CAIA_MAJOR(dev, vsec, dest) \
+	pci_read_config_byte(dev, vsec + 0xf, dest)
+#define CXL_READ_VSEC_BASE_IMAGE(dev, vsec, dest) \
+	pci_read_config_word(dev, vsec + 0x10, dest)
+
+#define CXL_READ_VSEC_IMAGE_STATE(dev, vsec, dest) \
+	pci_read_config_byte(dev, vsec + 0x13, dest)
+#define CXL_WRITE_VSEC_IMAGE_STATE(dev, vsec, val) \
+	pci_write_config_byte(dev, vsec + 0x13, val)
+#define CXL_VSEC_USER_IMAGE_LOADED 0x80 /* RO */
+#define CXL_VSEC_PERST_LOADS_IMAGE 0x20 /* RW */
+#define CXL_VSEC_PERST_SELECT_USER 0x10 /* RW */
+
+#define CXL_READ_VSEC_AFU_DESC_OFF(dev, vsec, dest) \
+	pci_read_config_dword(dev, vsec + 0x20, dest)
+#define CXL_READ_VSEC_AFU_DESC_SIZE(dev, vsec, dest) \
+	pci_read_config_dword(dev, vsec + 0x24, dest)
+#define CXL_READ_VSEC_PS_OFF(dev, vsec, dest) \
+	pci_read_config_dword(dev, vsec + 0x28, dest)
+#define CXL_READ_VSEC_PS_SIZE(dev, vsec, dest) \
+	pci_read_config_dword(dev, vsec + 0x2c, dest)
+
+
+/* This works a little different than the p1/p2 register accesses to make it
+ * easier to pull out individual fields */
+#define AFUD_READ(afu, off)		in_be64(afu->afu_desc_mmio + off)
+#define EXTRACT_PPC_BIT(val, bit)	(!!(val & PPC_BIT(bit)))
+#define EXTRACT_PPC_BITS(val, bs, be)	((val & PPC_BITMASK(bs, be)) >> PPC_BITLSHIFT(be))
+
+#define AFUD_READ_INFO(afu)		AFUD_READ(afu, 0x0)
+#define   AFUD_NUM_INTS_PER_PROC(val)	EXTRACT_PPC_BITS(val,  0, 15)
+#define   AFUD_NUM_PROCS(val)		EXTRACT_PPC_BITS(val, 16, 31)
+#define   AFUD_NUM_CRS(val)		EXTRACT_PPC_BITS(val, 32, 47)
+#define   AFUD_MULTIMODE(val)		EXTRACT_PPC_BIT(val, 48)
+#define   AFUD_PUSH_BLOCK_TRANSFER(val)	EXTRACT_PPC_BIT(val, 55)
+#define   AFUD_DEDICATED_PROCESS(val)	EXTRACT_PPC_BIT(val, 59)
+#define   AFUD_AFU_DIRECTED(val)	EXTRACT_PPC_BIT(val, 61)
+#define   AFUD_TIME_SLICED(val)		EXTRACT_PPC_BIT(val, 63)
+#define AFUD_READ_CR(afu)		AFUD_READ(afu, 0x20)
+#define   AFUD_CR_LEN(val)		EXTRACT_PPC_BITS(val, 8, 63)
+#define AFUD_READ_CR_OFF(afu)		AFUD_READ(afu, 0x28)
+#define AFUD_READ_PPPSA(afu)		AFUD_READ(afu, 0x30)
+#define   AFUD_PPPSA_PP(val)		EXTRACT_PPC_BIT(val, 6)
+#define   AFUD_PPPSA_PSA(val)		EXTRACT_PPC_BIT(val, 7)
+#define   AFUD_PPPSA_LEN(val)		EXTRACT_PPC_BITS(val, 8, 63)
+#define AFUD_READ_PPPSA_OFF(afu)	AFUD_READ(afu, 0x38)
+#define AFUD_READ_EB(afu)		AFUD_READ(afu, 0x40)
+#define   AFUD_EB_LEN(val)		EXTRACT_PPC_BITS(val, 8, 63)
+#define AFUD_READ_EB_OFF(afu)		AFUD_READ(afu, 0x48)
+
+static DEFINE_PCI_DEVICE_TABLE(cxl_pci_tbl) = {
+	{ PCI_DEVICE(PCI_VENDOR_ID_IBM, 0x0477), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_IBM, 0x044b), },
+	{ PCI_DEVICE(PCI_VENDOR_ID_IBM, 0x04cf), },
+	{ PCI_DEVICE_CLASS(0x120000, ~0), },
+
+	{ }
+};
+MODULE_DEVICE_TABLE(pci, cxl_pci_tbl);
+
+
+/*
+ * Mostly using these wrappers to avoid confusion:
+ * priv 1 is BAR2, while priv 2 is BAR0
+ */
+static inline resource_size_t p1_base(struct pci_dev *dev)
+{
+	return pci_resource_start(dev, 2);
+}
+
+static inline resource_size_t p1_size(struct pci_dev *dev)
+{
+	return pci_resource_len(dev, 2);
+}
+
+static inline resource_size_t p2_base(struct pci_dev *dev)
+{
+	return pci_resource_start(dev, 0);
+}
+
+static inline resource_size_t p2_size(struct pci_dev *dev)
+{
+	return pci_resource_len(dev, 0);
+}
+
+static int find_cxl_vsec(struct pci_dev *dev)
+{
+	int vsec = 0;
+	u16 val;
+
+	while ((vsec = pci_find_next_ext_capability(dev, vsec, PCI_EXT_CAP_ID_VNDR))) {
+		pci_read_config_word(dev, vsec + 0x4, &val);
+		if (val == CXL_PCI_VSEC_ID)
+			return vsec;
+	}
+	return 0;
+
+}
+
+static void dump_cxl_config_space(struct pci_dev *dev)
+{
+	int vsec;
+	u32 val;
+
+	dev_info(&dev->dev, "dump_cxl_config_space\n");
+
+	pci_read_config_dword(dev, PCI_BASE_ADDRESS_0, &val);
+	dev_info(&dev->dev, "BAR0: %#.8x\n", val);
+	pci_read_config_dword(dev, PCI_BASE_ADDRESS_1, &val);
+	dev_info(&dev->dev, "BAR1: %#.8x\n", val);
+	pci_read_config_dword(dev, PCI_BASE_ADDRESS_2, &val);
+	dev_info(&dev->dev, "BAR2: %#.8x\n", val);
+	pci_read_config_dword(dev, PCI_BASE_ADDRESS_3, &val);
+	dev_info(&dev->dev, "BAR3: %#.8x\n", val);
+	pci_read_config_dword(dev, PCI_BASE_ADDRESS_4, &val);
+	dev_info(&dev->dev, "BAR4: %#.8x\n", val);
+	pci_read_config_dword(dev, PCI_BASE_ADDRESS_5, &val);
+	dev_info(&dev->dev, "BAR5: %#.8x\n", val);
+
+	dev_info(&dev->dev, "p1 regs: %#llx, len: %#llx\n",
+		p1_base(dev), p1_size(dev));
+	dev_info(&dev->dev, "p2 regs: %#llx, len: %#llx\n",
+		p1_base(dev), p2_size(dev));
+	dev_info(&dev->dev, "BAR 4/5: %#llx, len: %#llx\n",
+		pci_resource_start(dev, 4), pci_resource_len(dev, 4));
+
+	if (!(vsec = find_cxl_vsec(dev)))
+		return;
+
+#define show_reg(name, what) \
+	dev_info(&dev->dev, "cxl vsec: %30s: %#x\n", name, what)
+
+	pci_read_config_dword(dev, vsec + 0x0, &val);
+	show_reg("Cap ID", (val >> 0) & 0xffff);
+	show_reg("Cap Ver", (val >> 16) & 0xf);
+	show_reg("Next Cap Ptr", (val >> 20) & 0xfff);
+	pci_read_config_dword(dev, vsec + 0x4, &val);
+	show_reg("VSEC ID", (val >> 0) & 0xffff);
+	show_reg("VSEC Rev", (val >> 16) & 0xf);
+	show_reg("VSEC Length",	(val >> 20) & 0xfff);
+	pci_read_config_dword(dev, vsec + 0x8, &val);
+	show_reg("Num AFUs", (val >> 0) & 0xff);
+	show_reg("Status", (val >> 8) & 0xff);
+	show_reg("Mode Control", (val >> 16) & 0xff);
+	show_reg("Reserved", (val >> 24) & 0xff);
+	pci_read_config_dword(dev, vsec + 0xc, &val);
+	show_reg("PSL Rev", (val >> 0) & 0xffff);
+	show_reg("CAIA Ver", (val >> 16) & 0xffff);
+	pci_read_config_dword(dev, vsec + 0x10, &val);
+	show_reg("Base Image Rev", (val >> 0) & 0xffff);
+	show_reg("Reserved", (val >> 16) & 0x0fff);
+	show_reg("Image Control", (val >> 28) & 0x3);
+	show_reg("Reserved", (val >> 30) & 0x1);
+	show_reg("Image Loaded", (val >> 31) & 0x1);
+
+	pci_read_config_dword(dev, vsec + 0x14, &val);
+	show_reg("Reserved", val);
+	pci_read_config_dword(dev, vsec + 0x18, &val);
+	show_reg("Reserved", val);
+	pci_read_config_dword(dev, vsec + 0x1c, &val);
+	show_reg("Reserved", val);
+
+	pci_read_config_dword(dev, vsec + 0x20, &val);
+	show_reg("AFU Descriptor Offset", val);
+	pci_read_config_dword(dev, vsec + 0x24, &val);
+	show_reg("AFU Descriptor Size", val);
+	pci_read_config_dword(dev, vsec + 0x28, &val);
+	show_reg("Problem State Offset", val);
+	pci_read_config_dword(dev, vsec + 0x2c, &val);
+	show_reg("Problem State Size", val);
+
+	pci_read_config_dword(dev, vsec + 0x30, &val);
+	show_reg("Reserved", val);
+	pci_read_config_dword(dev, vsec + 0x34, &val);
+	show_reg("Reserved", val);
+	pci_read_config_dword(dev, vsec + 0x38, &val);
+	show_reg("Reserved", val);
+	pci_read_config_dword(dev, vsec + 0x3c, &val);
+	show_reg("Reserved", val);
+
+	pci_read_config_dword(dev, vsec + 0x40, &val);
+	show_reg("PSL Programming Port", val);
+	pci_read_config_dword(dev, vsec + 0x44, &val);
+	show_reg("PSL Programming Control", val);
+
+	pci_read_config_dword(dev, vsec + 0x48, &val);
+	show_reg("Reserved", val);
+	pci_read_config_dword(dev, vsec + 0x4c, &val);
+	show_reg("Reserved", val);
+
+	pci_read_config_dword(dev, vsec + 0x50, &val);
+	show_reg("Flash Address Register", val);
+	pci_read_config_dword(dev, vsec + 0x54, &val);
+	show_reg("Flash Size Register", val);
+	pci_read_config_dword(dev, vsec + 0x58, &val);
+	show_reg("Flash Status/Control Register", val);
+	pci_read_config_dword(dev, vsec + 0x58, &val);
+	show_reg("Flash Data Port", val);
+
+#undef show_reg
+}
+
+static void dump_afu_descriptor(struct cxl_afu *afu)
+{
+	u64 val;
+
+#define show_reg(name, what) \
+	dev_info(&afu->dev, "afu desc: %30s: %#llx\n", name, what)
+
+	val = AFUD_READ_INFO(afu);
+	show_reg("num_ints_per_process", AFUD_NUM_INTS_PER_PROC(val));
+	show_reg("num_of_processes", AFUD_NUM_PROCS(val));
+	show_reg("num_of_afu_CRs", AFUD_NUM_CRS(val));
+	show_reg("req_prog_mode", val & 0xffffULL);
+
+	val = AFUD_READ(afu, 0x8);
+	show_reg("Reserved", val);
+	val = AFUD_READ(afu, 0x10);
+	show_reg("Reserved", val);
+	val = AFUD_READ(afu, 0x18);
+	show_reg("Reserved", val);
+
+	val = AFUD_READ_CR(afu);
+	show_reg("Reserved", (val >> (63-7)) & 0xff);
+	show_reg("AFU_CR_len", AFUD_CR_LEN(val));
+
+	val = AFUD_READ_CR_OFF(afu);
+	show_reg("AFU_CR_offset", val);
+
+	val = AFUD_READ_PPPSA(afu);
+	show_reg("PerProcessPSA_control", (val >> (63-7)) & 0xff);
+	show_reg("PerProcessPSA Length", AFUD_PPPSA_LEN(val));
+
+	val = AFUD_READ_PPPSA_OFF(afu);
+	show_reg("PerProcessPSA_offset", val);
+
+	val = AFUD_READ_EB(afu);
+	show_reg("Reserved", (val >> (63-7)) & 0xff);
+	show_reg("AFU_EB_len", AFUD_EB_LEN(val));
+
+	val = AFUD_READ_EB_OFF(afu);
+	show_reg("AFU_EB_offset", val);
+
+#undef show_reg
+}
+
+static int init_implementation_adapter_regs(struct cxl *adapter, struct pci_dev *dev)
+{
+	struct device_node *np;
+	const __be32 *prop;
+	u64 psl_dsnctl;
+	u64 chipid;
+
+	if (!(np = pnv_pci_to_phb_node(dev)))
+		return -ENODEV;
+
+	while (np && !(prop = of_get_property(np, "ibm,chip-id", NULL)))
+		np = of_get_next_parent(np);
+	if (!np)
+		return -ENODEV;
+	chipid = be32_to_cpup(prop);
+	of_node_put(np);
+
+	/* Tell PSL where to route data to */
+	psl_dsnctl = 0x02E8900002000000ULL | (chipid << (63-5));
+	cxl_p1_write(adapter, CXL_PSL_DSNDCTL, psl_dsnctl);
+	cxl_p1_write(adapter, CXL_PSL_RESLCKTO, 0x20000000200ULL);
+	/* snoop write mask */
+	cxl_p1_write(adapter, CXL_PSL_SNWRALLOC, 0x00000000FFFFFFFFULL);
+	/* set fir_accum */
+	cxl_p1_write(adapter, CXL_PSL_FIR_CNTL, 0x0800000000000000ULL);
+	/* for debugging with trace arrays */
+	cxl_p1_write(adapter, CXL_PSL_TRACE, 0x0000FF7C00000000ULL);
+
+	return 0;
+}
+
+static int init_implementation_afu_regs(struct cxl_afu *afu)
+{
+	/* read/write masks for this slice */
+	cxl_p1n_write(afu, CXL_PSL_APCALLOC_A, 0xFFFFFFFEFEFEFEFEULL);
+	/* APC read/write masks for this slice */
+	cxl_p1n_write(afu, CXL_PSL_COALLOC_A, 0xFF000000FEFEFEFEULL);
+	/* for debugging with trace arrays */
+	cxl_p1n_write(afu, CXL_PSL_SLICE_TRACE, 0x0000FFFF00000000ULL);
+	cxl_p1n_write(afu, CXL_PSL_RXCTL_A, 0xF000000000000000ULL);
+
+	return 0;
+}
+
+int cxl_setup_irq(struct cxl *adapter, unsigned int hwirq,
+			 unsigned int virq)
+{
+	struct pci_dev *dev = to_pci_dev(adapter->dev.parent);
+
+	return pnv_cxl_ioda_msi_setup(dev, hwirq, virq);
+}
+
+int cxl_alloc_one_irq(struct cxl *adapter)
+{
+	struct pci_dev *dev = to_pci_dev(adapter->dev.parent);
+
+	return pnv_cxl_alloc_hwirqs(dev, 1);
+}
+
+void cxl_release_one_irq(struct cxl *adapter, int hwirq)
+{
+	struct pci_dev *dev = to_pci_dev(adapter->dev.parent);
+
+	return pnv_cxl_release_hwirqs(dev, hwirq, 1);
+}
+
+int cxl_alloc_irq_ranges(struct cxl_irq_ranges *irqs, struct cxl *adapter, unsigned int num)
+{
+	struct pci_dev *dev = to_pci_dev(adapter->dev.parent);
+
+	return pnv_cxl_alloc_hwirq_ranges(irqs, dev, num);
+}
+
+void cxl_release_irq_ranges(struct cxl_irq_ranges *irqs, struct cxl *adapter)
+{
+	struct pci_dev *dev = to_pci_dev(adapter->dev.parent);
+
+	pnv_cxl_release_hwirq_ranges(irqs, dev);
+}
+
+static int setup_cxl_bars(struct pci_dev *dev)
+{
+	/* Safety check in case we get backported to < 3.17 without M64 */
+	if ((p1_base(dev) < 0x100000000ULL) ||
+	    (p2_base(dev) < 0x100000000ULL)) {
+		dev_err(&dev->dev, "ABORTING: M32 BAR assignment incompatible with CXL\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * BAR 4/5 has a special meaning for CXL and must be programmed with a
+	 * special value corresponding to the CXL protocol address range.
+	 * For POWER 8 that means bits 48:49 must be set to 10
+	 */
+	pci_write_config_dword(dev, PCI_BASE_ADDRESS_4, 0x00000000);
+	pci_write_config_dword(dev, PCI_BASE_ADDRESS_5, 0x00020000);
+
+	return 0;
+}
+
+/* pciex node: ibm,opal-m64-window = <0x3d058 0x0 0x3d058 0x0 0x8 0x0>; */
+static int switch_card_to_cxl(struct pci_dev *dev)
+{
+	int vsec;
+	u8 val;
+	int rc;
+
+	dev_info(&dev->dev, "switch card to CXL\n");
+
+	if (!(vsec = find_cxl_vsec(dev))) {
+		dev_err(&dev->dev, "ABORTING: CXL VSEC not found!\n");
+		return -ENODEV;
+	}
+
+	if ((rc = CXL_READ_VSEC_MODE_CONTROL(dev, vsec, &val))) {
+		dev_err(&dev->dev, "failed to read current mode control: %i", rc);
+		return rc;
+	}
+	val &= ~CXL_VSEC_PROTOCOL_MASK;
+	val |= CXL_VSEC_PROTOCOL_256TB | CXL_VSEC_PROTOCOL_ENABLE;
+	if ((rc = CXL_WRITE_VSEC_MODE_CONTROL(dev, vsec, val))) {
+		dev_err(&dev->dev, "failed to enable CXL protocol: %i", rc);
+		return rc;
+	}
+	/*
+	 * The CAIA spec (v0.12 11.6 Bi-modal Device Support) states
+	 * we must wait 100ms after this mode switch before touching
+	 * PCIe config space.
+	 */
+	msleep(100);
+
+	return 0;
+}
+
+static int cxl_map_slice_regs(struct cxl_afu *afu, struct cxl *adapter, struct pci_dev *dev)
+{
+	u64 p1n_base, p2n_base, afu_desc;
+	const u64 p1n_size = 0x100;
+	const u64 p2n_size = 0x1000;
+
+	p1n_base = p1_base(dev) + 0x10000 + (afu->slice * p1n_size);
+	p2n_base = p2_base(dev) + (afu->slice * p2n_size);
+	afu->psn_phys = p2_base(dev) + (adapter->ps_off + (afu->slice * adapter->ps_size));
+	afu_desc = p2_base(dev) + adapter->afu_desc_off + (afu->slice * adapter->afu_desc_size);
+
+	if (!(afu->p1n_mmio = ioremap(p1n_base, p1n_size)))
+		goto err;
+	if (!(afu->p2n_mmio = ioremap(p2n_base, p2n_size)))
+		goto err1;
+	if (afu_desc) {
+		if (!(afu->afu_desc_mmio = ioremap(afu_desc, adapter->afu_desc_size)))
+			goto err2;
+	}
+
+	return 0;
+err2:
+	iounmap(afu->p2n_mmio);
+err1:
+	iounmap(afu->p1n_mmio);
+err:
+	dev_err(&afu->dev, "Error mapping AFU MMIO regions\n");
+	return -ENOMEM;
+}
+
+static void cxl_unmap_slice_regs(struct cxl_afu *afu)
+{
+	if (afu->p1n_mmio)
+		iounmap(afu->p2n_mmio);
+	if (afu->p1n_mmio)
+		iounmap(afu->p1n_mmio);
+}
+
+static void cxl_release_afu(struct device *dev)
+{
+	struct cxl_afu *afu = to_cxl_afu(dev);
+
+	pr_devel("cxl_release_afu\n");
+
+	kfree(afu);
+}
+
+static struct cxl_afu *cxl_alloc_afu(struct cxl *adapter, int slice)
+{
+	struct cxl_afu *afu;
+
+	if (!(afu = kzalloc(sizeof(struct cxl_afu), GFP_KERNEL)))
+		return NULL;
+
+	afu->adapter = adapter;
+	afu->dev.parent = &adapter->dev;
+	afu->dev.release = cxl_release_afu;
+	afu->slice = slice;
+	idr_init(&afu->contexts_idr);
+	spin_lock_init(&afu->contexts_lock);
+	spin_lock_init(&afu->afu_cntl_lock);
+	mutex_init(&afu->spa_mutex);
+
+	afu->prefault_mode = CXL_PREFAULT_NONE;
+	afu->irqs_max = afu->adapter->user_irqs;
+
+	return afu;
+}
+
+/* Expects AFU struct to have recently been zeroed out */
+static int cxl_read_afu_descriptor(struct cxl_afu *afu)
+{
+	u64 val;
+
+	val = AFUD_READ_INFO(afu);
+	afu->pp_irqs = AFUD_NUM_INTS_PER_PROC(val);
+	afu->max_procs_virtualised = AFUD_NUM_PROCS(val);
+
+	if (AFUD_AFU_DIRECTED(val))
+		afu->modes_supported |= CXL_MODE_DIRECTED;
+	if (AFUD_DEDICATED_PROCESS(val))
+		afu->modes_supported |= CXL_MODE_DEDICATED;
+	if (AFUD_TIME_SLICED(val))
+		afu->modes_supported |= CXL_MODE_TIME_SLICED;
+
+	val = AFUD_READ_PPPSA(afu);
+	afu->pp_size = AFUD_PPPSA_LEN(val) * 4096;
+	afu->psa = AFUD_PPPSA_PSA(val);
+	if ((afu->pp_psa = AFUD_PPPSA_PP(val)))
+		afu->pp_offset = AFUD_READ_PPPSA_OFF(afu);
+
+	return 0;
+}
+
+static int cxl_afu_descriptor_looks_ok(struct cxl_afu *afu)
+{
+	if (afu->psa && afu->adapter->ps_size <
+			(afu->pp_offset + afu->pp_size*afu->max_procs_virtualised)) {
+		dev_err(&afu->dev, "per-process PSA can't fit inside the PSA!\n");
+		return -ENODEV;
+	}
+
+	if (afu->pp_psa && (afu->pp_size < PAGE_SIZE))
+		dev_warn(&afu->dev, "AFU uses < PAGE_SIZE per-process PSA!");
+
+	return 0;
+}
+
+static int sanitise_afu_regs(struct cxl_afu *afu)
+{
+	u64 reg;
+
+	/*
+	 * Clear out any regs that contain either an IVTE or address or may be
+	 * waiting on an acknowledgement to try to be a bit safer as we bring
+	 * it online
+	 */
+	reg = cxl_p2n_read(afu, CXL_AFU_Cntl_An);
+	if ((reg & CXL_AFU_Cntl_An_ES_MASK) != CXL_AFU_Cntl_An_ES_Disabled) {
+		dev_warn(&afu->dev, "WARNING: AFU was not disabled: %#.16llx\n", reg);
+		if (cxl_afu_reset(afu))
+			return -EIO;
+		if (cxl_afu_disable(afu))
+			return -EIO;
+		if (cxl_psl_purge(afu))
+			return -EIO;
+	}
+	cxl_p1n_write(afu, CXL_PSL_SPAP_An, 0x0000000000000000);
+	cxl_p1n_write(afu, CXL_PSL_IVTE_Limit_An, 0x0000000000000000);
+	cxl_p1n_write(afu, CXL_PSL_IVTE_Offset_An, 0x0000000000000000);
+	cxl_p1n_write(afu, CXL_PSL_AMBAR_An, 0x0000000000000000);
+	cxl_p1n_write(afu, CXL_PSL_SPOffset_An, 0x0000000000000000);
+	cxl_p1n_write(afu, CXL_HAURP_An, 0x0000000000000000);
+	cxl_p2n_write(afu, CXL_CSRP_An, 0x0000000000000000);
+	cxl_p2n_write(afu, CXL_AURP1_An, 0x0000000000000000);
+	cxl_p2n_write(afu, CXL_AURP0_An, 0x0000000000000000);
+	cxl_p2n_write(afu, CXL_SSTP1_An, 0x0000000000000000);
+	cxl_p2n_write(afu, CXL_SSTP0_An, 0x0000000000000000);
+	reg = cxl_p2n_read(afu, CXL_PSL_DSISR_An);
+	if (reg) {
+		dev_warn(&afu->dev, "AFU had pending DSISR: %#.16llx\n", reg);
+		if (reg & CXL_PSL_DSISR_TRANS)
+			cxl_p2n_write(afu, CXL_PSL_TFC_An, CXL_PSL_TFC_An_AE);
+		else
+			cxl_p2n_write(afu, CXL_PSL_TFC_An, CXL_PSL_TFC_An_A);
+	}
+	reg = cxl_p1n_read(afu, CXL_PSL_SERR_An);
+	if (reg) {
+		if (reg & ~0xffff)
+			dev_warn(&afu->dev, "AFU had pending SERR: %#.16llx\n", reg);
+		cxl_p1n_write(afu, CXL_PSL_SERR_An, reg & ~0xffff);
+	}
+	reg = cxl_p2n_read(afu, CXL_PSL_ErrStat_An);
+	if (reg) {
+		dev_warn(&afu->dev, "AFU had pending error status: %#.16llx\n", reg);
+		cxl_p2n_write(afu, CXL_PSL_ErrStat_An, reg);
+	}
+
+	return 0;
+}
+
+static int cxl_init_afu(struct cxl *adapter, int slice, struct pci_dev *dev)
+{
+	struct cxl_afu *afu;
+	bool free = true;
+	int rc;
+
+	if (!(afu = cxl_alloc_afu(adapter, slice)))
+		return -ENOMEM;
+
+	if ((rc = dev_set_name(&afu->dev, "afu%i.%i", adapter->adapter_num, slice)))
+		goto err1;
+
+	if ((rc = cxl_map_slice_regs(afu, adapter, dev)))
+		goto err1;
+
+	if ((rc = sanitise_afu_regs(afu)))
+		goto err2;
+
+	/* We need to reset the AFU before we can read the AFU descriptor */
+	if ((rc = cxl_afu_reset(afu)))
+		goto err2;
+
+	if (cxl_verbose)
+		dump_afu_descriptor(afu);
+
+	if ((rc = cxl_read_afu_descriptor(afu)))
+		goto err2;
+
+	if ((rc = cxl_afu_descriptor_looks_ok(afu)))
+		goto err2;
+
+	if ((rc = init_implementation_afu_regs(afu)))
+		goto err2;
+
+	if ((rc = cxl_register_serr_irq(afu)))
+		goto err2;
+
+	if ((rc = cxl_register_psl_irq(afu)))
+		goto err3;
+
+	/* Don't care if this fails */
+	cxl_debugfs_afu_add(afu);
+
+	/*
+	 * After we call this function we must not free the afu directly, even
+	 * if it returns an error!
+	 */
+	if ((rc = cxl_register_afu(afu)))
+		goto err_put1;
+
+	if ((rc = cxl_sysfs_afu_add(afu)))
+		goto err_put1;
+
+
+	if ((rc = cxl_afu_select_best_mode(afu)))
+		goto err_put2;
+
+	adapter->afu[afu->slice] = afu;
+
+	return 0;
+
+err_put2:
+	cxl_sysfs_afu_remove(afu);
+err_put1:
+	device_unregister(&afu->dev);
+	free = false;
+	cxl_debugfs_afu_remove(afu);
+	cxl_release_psl_irq(afu);
+err3:
+	cxl_release_serr_irq(afu);
+err2:
+	cxl_unmap_slice_regs(afu);
+err1:
+	if (free)
+		kfree(afu);
+	return rc;
+}
+
+static void cxl_remove_afu(struct cxl_afu *afu)
+{
+	pr_devel("cxl_remove_afu\n");
+
+	if (!afu)
+		return;
+
+	cxl_sysfs_afu_remove(afu);
+	cxl_debugfs_afu_remove(afu);
+
+	spin_lock(&afu->adapter->afu_list_lock);
+	afu->adapter->afu[afu->slice] = NULL;
+	spin_unlock(&afu->adapter->afu_list_lock);
+
+	cxl_context_detach_all(afu);
+	cxl_afu_deactivate_mode(afu);
+
+	cxl_release_psl_irq(afu);
+	cxl_release_serr_irq(afu);
+	cxl_unmap_slice_regs(afu);
+
+	device_unregister(&afu->dev);
+}
+
+
+static int cxl_map_adapter_regs(struct cxl *adapter, struct pci_dev *dev)
+{
+	if (pci_request_region(dev, 2, "priv 2 regs"))
+		goto err1;
+	if (pci_request_region(dev, 0, "priv 1 regs"))
+		goto err2;
+
+	pr_devel("cxl_map_adapter_regs: p1: %#.16llx %#llx, p2: %#.16llx %#llx",
+			p1_base(dev), p1_size(dev), p2_base(dev), p2_size(dev));
+
+	if (!(adapter->p1_mmio = ioremap(p1_base(dev), p1_size(dev))))
+		goto err3;
+
+	if (!(adapter->p2_mmio = ioremap(p2_base(dev), p2_size(dev))))
+		goto err4;
+
+	return 0;
+
+err4:
+	iounmap(adapter->p1_mmio);
+	adapter->p1_mmio = NULL;
+err3:
+	pci_release_region(dev, 0);
+err2:
+	pci_release_region(dev, 2);
+err1:
+	return -ENOMEM;
+}
+
+static void cxl_unmap_adapter_regs(struct cxl *adapter)
+{
+	if (adapter->p1_mmio)
+		iounmap(adapter->p1_mmio);
+	if (adapter->p2_mmio)
+		iounmap(adapter->p2_mmio);
+}
+
+static int cxl_read_vsec(struct cxl *adapter, struct pci_dev *dev)
+{
+	int vsec;
+	u32 afu_desc_off, afu_desc_size;
+	u32 ps_off, ps_size;
+	u16 vseclen;
+	u8 image_state;
+
+	if (!(vsec = find_cxl_vsec(dev))) {
+		dev_err(&adapter->dev, "ABORTING: CXL VSEC not found!\n");
+		return -ENODEV;
+	}
+
+	CXL_READ_VSEC_LENGTH(dev, vsec, &vseclen);
+	if (vseclen < CXL_VSEC_MIN_SIZE) {
+		pr_err("ABORTING: CXL VSEC too short\n");
+		return -EINVAL;
+	}
+
+	CXL_READ_VSEC_STATUS(dev, vsec, &adapter->vsec_status);
+	CXL_READ_VSEC_PSL_REVISION(dev, vsec, &adapter->psl_rev);
+	CXL_READ_VSEC_CAIA_MAJOR(dev, vsec, &adapter->caia_major);
+	CXL_READ_VSEC_CAIA_MINOR(dev, vsec, &adapter->caia_minor);
+	CXL_READ_VSEC_BASE_IMAGE(dev, vsec, &adapter->base_image);
+	CXL_READ_VSEC_IMAGE_STATE(dev, vsec, &image_state);
+	adapter->user_image_loaded = !!(image_state & CXL_VSEC_USER_IMAGE_LOADED);
+	adapter->perst_loads_image = !!(image_state & CXL_VSEC_PERST_LOADS_IMAGE);
+	adapter->perst_select_user = !!(image_state & CXL_VSEC_PERST_SELECT_USER);
+
+	CXL_READ_VSEC_NAFUS(dev, vsec, &adapter->slices);
+	CXL_READ_VSEC_AFU_DESC_OFF(dev, vsec, &afu_desc_off);
+	CXL_READ_VSEC_AFU_DESC_SIZE(dev, vsec, &afu_desc_size);
+	CXL_READ_VSEC_PS_OFF(dev, vsec, &ps_off);
+	CXL_READ_VSEC_PS_SIZE(dev, vsec, &ps_size);
+
+	/* Convert everything to bytes, because there is NO WAY I'd look at the
+	 * code a month later and forget what units these are in ;-) */
+	adapter->ps_off = ps_off * 64 * 1024;
+	adapter->ps_size = ps_size * 64 * 1024;
+	adapter->afu_desc_off = afu_desc_off * 64 * 1024;
+	adapter->afu_desc_size = afu_desc_size *64 * 1024;
+
+	/* Total IRQs - 1 PSL ERROR - #AFU*(1 slice error + 1 DSI) */
+	adapter->user_irqs = pnv_cxl_get_irq_count(dev) - 1 - 2*adapter->slices;
+
+	return 0;
+}
+
+static int cxl_vsec_looks_ok(struct cxl *adapter, struct pci_dev *dev)
+{
+	if (adapter->vsec_status & CXL_STATUS_SECOND_PORT)
+		return -EBUSY;
+
+	if (adapter->vsec_status & CXL_UNSUPPORTED_FEATURES) {
+		dev_err(&adapter->dev, "ABORTING: CXL requires unsupported features\n");
+		return -EINVAL;
+	}
+
+	if (!adapter->slices) {
+		/* Once we support dynamic reprogramming we can use the card if
+		 * it supports loadable AFUs */
+		dev_err(&adapter->dev, "ABORTING: Device has no AFUs\n");
+		return -EINVAL;
+	}
+
+	if (!adapter->afu_desc_off || !adapter->afu_desc_size) {
+		dev_err(&adapter->dev, "ABORTING: VSEC shows no AFU descriptors\n");
+		return -EINVAL;
+	}
+
+	if (adapter->ps_size > p2_size(dev) - adapter->ps_off) {
+		dev_err(&adapter->dev, "ABORTING: Problem state size larger than "
+				   "available in BAR2: 0x%llx > 0x%llx\n",
+			 adapter->ps_size, p2_size(dev) - adapter->ps_off);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void cxl_release_adapter(struct device *dev)
+{
+	struct cxl *adapter = to_cxl_adapter(dev);
+
+	pr_devel("cxl_release_adapter\n");
+
+	kfree(adapter);
+}
+
+static struct cxl *cxl_alloc_adapter(struct pci_dev *dev)
+{
+	struct cxl *adapter;
+
+	if (!(adapter = kzalloc(sizeof(struct cxl), GFP_KERNEL)))
+		return NULL;
+
+	adapter->dev.parent = &dev->dev;
+	adapter->dev.release = cxl_release_adapter;
+	pci_set_drvdata(dev, adapter);
+	spin_lock_init(&adapter->afu_list_lock);
+
+	return adapter;
+}
+
+static int sanitise_adapter_regs(struct cxl *adapter)
+{
+	cxl_p1_write(adapter, CXL_PSL_ErrIVTE, 0x0000000000000000);
+	return cxl_tlb_slb_invalidate(adapter);
+}
+
+static struct cxl *cxl_init_adapter(struct pci_dev *dev)
+{
+	struct cxl *adapter;
+	bool free = true;
+	int rc;
+
+
+	if (!(adapter = cxl_alloc_adapter(dev)))
+		return ERR_PTR(-ENOMEM);
+
+	if ((rc = switch_card_to_cxl(dev)))
+		goto err1;
+
+	if ((rc = cxl_alloc_adapter_nr(adapter)))
+		goto err1;
+
+	if ((rc = dev_set_name(&adapter->dev, "card%i", adapter->adapter_num)))
+		goto err2;
+
+	if ((rc = cxl_read_vsec(adapter, dev)))
+		goto err2;
+
+	if ((rc = cxl_vsec_looks_ok(adapter, dev)))
+		goto err2;
+
+	if ((rc = cxl_map_adapter_regs(adapter, dev)))
+		goto err2;
+
+	if ((rc = sanitise_adapter_regs(adapter)))
+		goto err2;
+
+	if ((rc = init_implementation_adapter_regs(adapter, dev)))
+		goto err3;
+
+	if ((rc = pnv_phb_to_cxl(dev)))
+		goto err3;
+
+	if ((rc = cxl_register_psl_err_irq(adapter)))
+		goto err3;
+
+	/* Don't care if this one fails: */
+	cxl_debugfs_adapter_add(adapter);
+
+	/*
+	 * After we call this function we must not free the adapter directly,
+	 * even if it returns an error!
+	 */
+	if ((rc = cxl_register_adapter(adapter)))
+		goto err_put1;
+
+	if ((rc = cxl_sysfs_adapter_add(adapter)))
+		goto err_put1;
+
+	return adapter;
+
+err_put1:
+	device_unregister(&adapter->dev);
+	free = false;
+	cxl_debugfs_adapter_remove(adapter);
+	cxl_release_psl_err_irq(adapter);
+err3:
+	cxl_unmap_adapter_regs(adapter);
+err2:
+	cxl_remove_adapter_nr(adapter);
+err1:
+	if (free)
+		kfree(adapter);
+	return ERR_PTR(rc);
+}
+
+static void cxl_remove_adapter(struct cxl *adapter)
+{
+	struct pci_dev *pdev = to_pci_dev(adapter->dev.parent);
+
+	pr_devel("cxl_release_adapter\n");
+
+	cxl_sysfs_adapter_remove(adapter);
+	cxl_debugfs_adapter_remove(adapter);
+	cxl_release_psl_err_irq(adapter);
+	cxl_unmap_adapter_regs(adapter);
+	cxl_remove_adapter_nr(adapter);
+
+	device_unregister(&adapter->dev);
+
+	pci_release_region(pdev, 0);
+	pci_release_region(pdev, 2);
+	pci_disable_device(pdev);
+}
+
+static int cxl_probe(struct pci_dev *dev, const struct pci_device_id *id)
+{
+	struct cxl *adapter;
+	int slice;
+	int rc;
+
+	pci_dev_get(dev);
+
+	if (cxl_verbose)
+		dump_cxl_config_space(dev);
+
+	if ((rc = setup_cxl_bars(dev)))
+		return rc;
+
+	if ((rc = pci_enable_device(dev))) {
+		dev_err(&dev->dev, "pci_enable_device failed: %i\n", rc);
+		return rc;
+	}
+
+	adapter = cxl_init_adapter(dev);
+	if (IS_ERR(adapter)) {
+		dev_err(&dev->dev, "cxl_init_adapter failed: %li\n", PTR_ERR(adapter));
+		return PTR_ERR(adapter);
+	}
+
+	for (slice = 0; slice < adapter->slices; slice++) {
+		if ((rc = cxl_init_afu(adapter, slice, dev)))
+			dev_err(&dev->dev, "AFU %i failed to initialise: %i\n", slice, rc);
+	}
+
+	return 0;
+}
+
+static void cxl_remove(struct pci_dev *dev)
+{
+	struct cxl *adapter = pci_get_drvdata(dev);
+	int afu;
+
+	dev_warn(&dev->dev, "pci remove\n");
+
+	/*
+	 * Lock to prevent someone grabbing a ref through the adapter list as
+	 * we are removing it
+	 */
+	for (afu = 0; afu < adapter->slices; afu++)
+		cxl_remove_afu(adapter->afu[afu]);
+	cxl_remove_adapter(adapter);
+}
+
+struct pci_driver cxl_pci_driver = {
+	.name = "cxl-pci",
+	.id_table = cxl_pci_tbl,
+	.probe = cxl_probe,
+	.remove = cxl_remove,
+};
diff --git a/drivers/misc/cxl/sysfs.c b/drivers/misc/cxl/sysfs.c
new file mode 100644
index 000000000000..ce7ec06d87d1
--- /dev/null
+++ b/drivers/misc/cxl/sysfs.c
@@ -0,0 +1,385 @@
+/*
+ * Copyright 2014 IBM Corp.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/sysfs.h>
+
+#include "cxl.h"
+
+#define to_afu_chardev_m(d) dev_get_drvdata(d)
+
+/*********  Adapter attributes  **********************************************/
+
+static ssize_t caia_version_show(struct device *device,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct cxl *adapter = to_cxl_adapter(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%i.%i\n", adapter->caia_major,
+			 adapter->caia_minor);
+}
+
+static ssize_t psl_revision_show(struct device *device,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct cxl *adapter = to_cxl_adapter(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%i\n", adapter->psl_rev);
+}
+
+static ssize_t base_image_show(struct device *device,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	struct cxl *adapter = to_cxl_adapter(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%i\n", adapter->base_image);
+}
+
+static ssize_t image_loaded_show(struct device *device,
+				 struct device_attribute *attr,
+				 char *buf)
+{
+	struct cxl *adapter = to_cxl_adapter(device);
+
+	if (adapter->user_image_loaded)
+		return scnprintf(buf, PAGE_SIZE, "user\n");
+	return scnprintf(buf, PAGE_SIZE, "factory\n");
+}
+
+static struct device_attribute adapter_attrs[] = {
+	__ATTR_RO(caia_version),
+	__ATTR_RO(psl_revision),
+	__ATTR_RO(base_image),
+	__ATTR_RO(image_loaded),
+};
+
+
+/*********  AFU master specific attributes  **********************************/
+
+static ssize_t mmio_size_show_master(struct device *device,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct cxl_afu *afu = to_afu_chardev_m(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", afu->adapter->ps_size);
+}
+
+static ssize_t pp_mmio_off_show(struct device *device,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct cxl_afu *afu = to_afu_chardev_m(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", afu->pp_offset);
+}
+
+static ssize_t pp_mmio_len_show(struct device *device,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct cxl_afu *afu = to_afu_chardev_m(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", afu->pp_size);
+}
+
+static struct device_attribute afu_master_attrs[] = {
+	__ATTR(mmio_size, S_IRUGO, mmio_size_show_master, NULL),
+	__ATTR_RO(pp_mmio_off),
+	__ATTR_RO(pp_mmio_len),
+};
+
+
+/*********  AFU attributes  **************************************************/
+
+static ssize_t mmio_size_show(struct device *device,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+
+	if (afu->pp_size)
+		return scnprintf(buf, PAGE_SIZE, "%llu\n", afu->pp_size);
+	return scnprintf(buf, PAGE_SIZE, "%llu\n", afu->adapter->ps_size);
+}
+
+static ssize_t reset_store_afu(struct device *device,
+			       struct device_attribute *attr,
+			       const char *buf, size_t count)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+	int rc;
+
+	/* Not safe to reset if it is currently in use */
+	spin_lock(&afu->contexts_lock);
+	if (!idr_is_empty(&afu->contexts_idr)) {
+		rc = -EBUSY;
+		goto err;
+	}
+
+	if ((rc = cxl_afu_reset(afu)))
+		goto err;
+
+	rc = count;
+err:
+	spin_unlock(&afu->contexts_lock);
+	return rc;
+}
+
+static ssize_t irqs_min_show(struct device *device,
+			     struct device_attribute *attr,
+			     char *buf)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%i\n", afu->pp_irqs);
+}
+
+static ssize_t irqs_max_show(struct device *device,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+
+	return scnprintf(buf, PAGE_SIZE, "%i\n", afu->irqs_max);
+}
+
+static ssize_t irqs_max_store(struct device *device,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+	ssize_t ret;
+	int irqs_max;
+
+	ret = sscanf(buf, "%i", &irqs_max);
+	if (ret != 1)
+		return -EINVAL;
+
+	if (irqs_max < afu->pp_irqs)
+		return -EINVAL;
+
+	if (irqs_max > afu->adapter->user_irqs)
+		return -EINVAL;
+
+	afu->irqs_max = irqs_max;
+	return count;
+}
+
+static ssize_t modes_supported_show(struct device *device,
+				    struct device_attribute *attr, char *buf)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+	char *p = buf, *end = buf + PAGE_SIZE;
+
+	if (afu->modes_supported & CXL_MODE_DEDICATED)
+		p += scnprintf(p, end - p, "dedicated_process\n");
+	if (afu->modes_supported & CXL_MODE_DIRECTED)
+		p += scnprintf(p, end - p, "afu_directed\n");
+	return (p - buf);
+}
+
+static ssize_t prefault_mode_show(struct device *device,
+				  struct device_attribute *attr,
+				  char *buf)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+
+	switch (afu->prefault_mode) {
+	case CXL_PREFAULT_WED:
+		return scnprintf(buf, PAGE_SIZE, "work_element_descriptor\n");
+	case CXL_PREFAULT_ALL:
+		return scnprintf(buf, PAGE_SIZE, "all\n");
+	default:
+		return scnprintf(buf, PAGE_SIZE, "none\n");
+	}
+}
+
+static ssize_t prefault_mode_store(struct device *device,
+			  struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+	enum prefault_modes mode = -1;
+
+	if (!strncmp(buf, "work_element_descriptor", 23))
+		mode = CXL_PREFAULT_WED;
+	if (!strncmp(buf, "all", 3))
+		mode = CXL_PREFAULT_ALL;
+	if (!strncmp(buf, "none", 4))
+		mode = CXL_PREFAULT_NONE;
+
+	if (mode == -1)
+		return -EINVAL;
+
+	afu->prefault_mode = mode;
+	return count;
+}
+
+static ssize_t mode_show(struct device *device,
+			 struct device_attribute *attr,
+			 char *buf)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+
+	if (afu->current_mode == CXL_MODE_DEDICATED)
+		return scnprintf(buf, PAGE_SIZE, "dedicated_process\n");
+	if (afu->current_mode == CXL_MODE_DIRECTED)
+		return scnprintf(buf, PAGE_SIZE, "afu_directed\n");
+	return scnprintf(buf, PAGE_SIZE, "none\n");
+}
+
+static ssize_t mode_store(struct device *device, struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct cxl_afu *afu = to_cxl_afu(device);
+	int old_mode, mode = -1;
+	int rc = -EBUSY;
+
+	/* can't change this if we have a user */
+	spin_lock(&afu->contexts_lock);
+	if (!idr_is_empty(&afu->contexts_idr))
+		goto err;
+
+	if (!strncmp(buf, "dedicated_process", 17))
+		mode = CXL_MODE_DEDICATED;
+	if (!strncmp(buf, "afu_directed", 12))
+		mode = CXL_MODE_DIRECTED;
+	if (!strncmp(buf, "none", 4))
+		mode = 0;
+
+	if (mode == -1) {
+		rc = -EINVAL;
+		goto err;
+	}
+
+	/*
+	 * cxl_afu_deactivate_mode needs to be done outside the lock, prevent
+	 * other contexts coming in before we are ready:
+	 */
+	old_mode = afu->current_mode;
+	afu->current_mode = 0;
+	afu->num_procs = 0;
+
+	spin_unlock(&afu->contexts_lock);
+
+	if ((rc = _cxl_afu_deactivate_mode(afu, old_mode)))
+		return rc;
+	if ((rc = cxl_afu_activate_mode(afu, mode)))
+		return rc;
+
+	return count;
+err:
+	spin_unlock(&afu->contexts_lock);
+	return rc;
+}
+
+static ssize_t api_version_show(struct device *device,
+				struct device_attribute *attr,
+				char *buf)
+{
+	return scnprintf(buf, PAGE_SIZE, "%i\n", CXL_API_VERSION);
+}
+
+static ssize_t api_version_compatible_show(struct device *device,
+					   struct device_attribute *attr,
+					   char *buf)
+{
+	return scnprintf(buf, PAGE_SIZE, "%i\n", CXL_API_VERSION_COMPATIBLE);
+}
+
+static struct device_attribute afu_attrs[] = {
+	__ATTR_RO(mmio_size),
+	__ATTR_RO(irqs_min),
+	__ATTR_RW(irqs_max),
+	__ATTR_RO(modes_supported),
+	__ATTR_RW(mode),
+	__ATTR_RW(prefault_mode),
+	__ATTR_RO(api_version),
+	__ATTR_RO(api_version_compatible),
+	__ATTR(reset, S_IWUSR, NULL, reset_store_afu),
+};
+
+
+
+int cxl_sysfs_adapter_add(struct cxl *adapter)
+{
+	int i, rc;
+
+	for (i = 0; i < ARRAY_SIZE(adapter_attrs); i++) {
+		if ((rc = device_create_file(&adapter->dev, &adapter_attrs[i])))
+			goto err;
+	}
+	return 0;
+err:
+	for (i--; i >= 0; i--)
+		device_remove_file(&adapter->dev, &adapter_attrs[i]);
+	return rc;
+}
+void cxl_sysfs_adapter_remove(struct cxl *adapter)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(adapter_attrs); i++)
+		device_remove_file(&adapter->dev, &adapter_attrs[i]);
+}
+
+int cxl_sysfs_afu_add(struct cxl_afu *afu)
+{
+	int i, rc;
+
+	for (i = 0; i < ARRAY_SIZE(afu_attrs); i++) {
+		if ((rc = device_create_file(&afu->dev, &afu_attrs[i])))
+			goto err;
+	}
+
+	return 0;
+
+err:
+	for (i--; i >= 0; i--)
+		device_remove_file(&afu->dev, &afu_attrs[i]);
+	return rc;
+}
+
+void cxl_sysfs_afu_remove(struct cxl_afu *afu)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(afu_attrs); i++)
+		device_remove_file(&afu->dev, &afu_attrs[i]);
+}
+
+int cxl_sysfs_afu_m_add(struct cxl_afu *afu)
+{
+	int i, rc;
+
+	for (i = 0; i < ARRAY_SIZE(afu_master_attrs); i++) {
+		if ((rc = device_create_file(afu->chardev_m, &afu_master_attrs[i])))
+			goto err;
+	}
+
+	return 0;
+
+err:
+	for (i--; i >= 0; i--)
+		device_remove_file(afu->chardev_m, &afu_master_attrs[i]);
+	return rc;
+}
+
+void cxl_sysfs_afu_m_remove(struct cxl_afu *afu)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(afu_master_attrs); i++)
+		device_remove_file(afu->chardev_m, &afu_master_attrs[i]);
+}
diff --git a/drivers/misc/eeprom/eeprom.c b/drivers/misc/eeprom/eeprom.c
index 33f8673d23a6..b432873def96 100644
--- a/drivers/misc/eeprom/eeprom.c
+++ b/drivers/misc/eeprom/eeprom.c
@@ -18,7 +18,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/slab.h>
+#include <linux/device.h>
 #include <linux/jiffies.h>
 #include <linux/i2c.h>
 #include <linux/mutex.h>
@@ -159,12 +159,11 @@ static int eeprom_probe(struct i2c_client *client,
 {
 	struct i2c_adapter *adapter = client->adapter;
 	struct eeprom_data *data;
-	int err;
 
-	if (!(data = kzalloc(sizeof(struct eeprom_data), GFP_KERNEL))) {
-		err = -ENOMEM;
-		goto exit;
-	}
+	data = devm_kzalloc(&client->dev, sizeof(struct eeprom_data),
+			    GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
 
 	memset(data->data, 0xff, EEPROM_SIZE);
 	i2c_set_clientdata(client, data);
@@ -190,22 +189,12 @@ static int eeprom_probe(struct i2c_client *client,
 	}
 
 	/* create the sysfs eeprom file */
-	err = sysfs_create_bin_file(&client->dev.kobj, &eeprom_attr);
-	if (err)
-		goto exit_kfree;
-
-	return 0;
-
-exit_kfree:
-	kfree(data);
-exit:
-	return err;
+	return sysfs_create_bin_file(&client->dev.kobj, &eeprom_attr);
 }
 
 static int eeprom_remove(struct i2c_client *client)
 {
 	sysfs_remove_bin_file(&client->dev.kobj, &eeprom_attr);
-	kfree(i2c_get_clientdata(client));
 
 	return 0;
 }
diff --git a/drivers/misc/genwqe/card_base.c b/drivers/misc/genwqe/card_base.c
index 43bbabc96b6c..4cf8f82cfca2 100644
--- a/drivers/misc/genwqe/card_base.c
+++ b/drivers/misc/genwqe/card_base.c
@@ -5,7 +5,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -45,10 +45,10 @@
 MODULE_AUTHOR("Frank Haverkamp <haver@linux.vnet.ibm.com>");
 MODULE_AUTHOR("Michael Ruettger <michael@ibmra.de>");
 MODULE_AUTHOR("Joerg-Stephan Vogt <jsvogt@de.ibm.com>");
-MODULE_AUTHOR("Michal Jung <mijung@de.ibm.com>");
+MODULE_AUTHOR("Michael Jung <mijung@gmx.net>");
 
 MODULE_DESCRIPTION("GenWQE Card");
-MODULE_VERSION(DRV_VERS_STRING);
+MODULE_VERSION(DRV_VERSION);
 MODULE_LICENSE("GPL");
 
 static char genwqe_driver_name[] = GENWQE_DEVNAME;
@@ -346,8 +346,13 @@ static bool genwqe_setup_vf_jtimer(struct genwqe_dev *cd)
 	unsigned int vf;
 	u32 T = genwqe_T_psec(cd);
 	u64 x;
+	int totalvfs;
 
-	for (vf = 0; vf < pci_sriov_get_totalvfs(pci_dev); vf++) {
+	totalvfs = pci_sriov_get_totalvfs(pci_dev);
+	if (totalvfs <= 0)
+		return false;
+
+	for (vf = 0; vf < totalvfs; vf++) {
 
 		if (cd->vf_jobtimeout_msec[vf] == 0)
 			continue;
@@ -383,8 +388,9 @@ static int genwqe_ffdc_buffs_alloc(struct genwqe_dev *cd)
 
 		/* currently support only the debug units mentioned here */
 		cd->ffdc[type].entries = e;
-		cd->ffdc[type].regs = kmalloc(e * sizeof(struct genwqe_reg),
-					      GFP_KERNEL);
+		cd->ffdc[type].regs =
+			kmalloc_array(e, sizeof(struct genwqe_reg),
+				      GFP_KERNEL);
 		/*
 		 * regs == NULL is ok, the using code treats this as no regs,
 		 * Printing warning is ok in this case.
@@ -723,8 +729,8 @@ static u64 genwqe_fir_checking(struct genwqe_dev *cd)
 				__genwqe_writeq(cd, sfir_addr, sfir);
 
 				dev_dbg(&pci_dev->dev,
-					"[HM] Clearing  2ndary FIR 0x%08x "
-					"with 0x%016llx\n", sfir_addr, sfir);
+					"[HM] Clearing  2ndary FIR 0x%08x with 0x%016llx\n",
+					sfir_addr, sfir);
 
 				/*
 				 * note, these cannot be error-Firs
@@ -740,9 +746,8 @@ static u64 genwqe_fir_checking(struct genwqe_dev *cd)
 				__genwqe_writeq(cd, fir_clr_addr, mask);
 
 				dev_dbg(&pci_dev->dev,
-					"[HM] Clearing primary FIR 0x%08x "
-					"with 0x%016llx\n", fir_clr_addr,
-					mask);
+					"[HM] Clearing primary FIR 0x%08x with 0x%016llx\n",
+					fir_clr_addr, mask);
 			}
 		}
 	}
@@ -1125,6 +1130,8 @@ static int genwqe_pci_setup(struct genwqe_dev *cd)
 	}
 
 	cd->num_vfs = pci_sriov_get_totalvfs(pci_dev);
+	if (cd->num_vfs < 0)
+		cd->num_vfs = 0;
 
 	err = genwqe_read_ids(cd);
 	if (err)
@@ -1202,8 +1209,8 @@ static int genwqe_probe(struct pci_dev *pci_dev,
 		err = genwqe_health_check_start(cd);
 		if (err < 0) {
 			dev_err(&pci_dev->dev,
-				"err: cannot start health checking! "
-				"(err=%d)\n", err);
+				"err: cannot start health checking! (err=%d)\n",
+				err);
 			goto out_stop_services;
 		}
 	}
@@ -1313,11 +1320,14 @@ static void genwqe_err_resume(struct pci_dev *pci_dev)
 
 static int genwqe_sriov_configure(struct pci_dev *dev, int numvfs)
 {
+	int rc;
 	struct genwqe_dev *cd = dev_get_drvdata(&dev->dev);
 
 	if (numvfs > 0) {
 		genwqe_setup_vf_jtimer(cd);
-		pci_enable_sriov(dev, numvfs);
+		rc = pci_enable_sriov(dev, numvfs);
+		if (rc < 0)
+			return rc;
 		return numvfs;
 	}
 	if (numvfs == 0) {
diff --git a/drivers/misc/genwqe/card_base.h b/drivers/misc/genwqe/card_base.h
index 67abd8cb2247..c64d7cad1085 100644
--- a/drivers/misc/genwqe/card_base.h
+++ b/drivers/misc/genwqe/card_base.h
@@ -8,7 +8,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -201,7 +201,8 @@ static inline void genwqe_mapping_init(struct dma_mapping *m,
  * @ddcb_seq:          Sequence number of last DDCB
  * @ddcbs_in_flight:   Currently enqueued DDCBs
  * @ddcbs_completed:   Number of already completed DDCBs
- * @busy:              Number of -EBUSY returns
+ * @return_on_busy:    Number of -EBUSY returns on full queue
+ * @wait_on_busy:      Number of waits on full queue
  * @ddcb_daddr:        DMA address of first DDCB in the queue
  * @ddcb_vaddr:        Kernel virtual address of first DDCB in the queue
  * @ddcb_req:          Associated requests (one per DDCB)
@@ -218,7 +219,8 @@ struct ddcb_queue {
 	unsigned int ddcbs_in_flight;	/* number of ddcbs in processing */
 	unsigned int ddcbs_completed;
 	unsigned int ddcbs_max_in_flight;
-	unsigned int busy;		/* how many times -EBUSY? */
+	unsigned int return_on_busy;    /* how many times -EBUSY? */
+	unsigned int wait_on_busy;
 
 	dma_addr_t ddcb_daddr;		/* DMA address */
 	struct ddcb *ddcb_vaddr;	/* kernel virtual addr for DDCBs */
@@ -226,7 +228,7 @@ struct ddcb_queue {
 	wait_queue_head_t *ddcb_waitqs; /* waitqueue per ddcb */
 
 	spinlock_t ddcb_lock;		/* exclusive access to queue */
-	wait_queue_head_t ddcb_waitq;	/* wait for ddcb processing */
+	wait_queue_head_t busy_waitq;   /* wait for ddcb processing */
 
 	/* registers or the respective queue to be used */
 	u32 IO_QUEUE_CONFIG;
@@ -306,7 +308,7 @@ struct genwqe_dev {
 	struct pci_dev *pci_dev;	/* PCI device */
 	void __iomem *mmio;		/* BAR-0 MMIO start */
 	unsigned long mmio_len;
-	u16 num_vfs;
+	int num_vfs;
 	u32 vf_jobtimeout_msec[GENWQE_MAX_VFS];
 	int is_privileged;		/* access to all regs possible */
 
@@ -508,7 +510,7 @@ static inline bool dma_mapping_used(struct dma_mapping *m)
  * buildup and teardown.
  */
 int  __genwqe_execute_ddcb(struct genwqe_dev *cd,
-			   struct genwqe_ddcb_cmd *cmd);
+			   struct genwqe_ddcb_cmd *cmd, unsigned int f_flags);
 
 /**
  * __genwqe_execute_raw_ddcb() - Execute DDCB request without addr translation
@@ -520,9 +522,12 @@ int  __genwqe_execute_ddcb(struct genwqe_dev *cd,
  * modification.
  */
 int  __genwqe_execute_raw_ddcb(struct genwqe_dev *cd,
-			       struct genwqe_ddcb_cmd *cmd);
+			       struct genwqe_ddcb_cmd *cmd,
+			       unsigned int f_flags);
+int  __genwqe_enqueue_ddcb(struct genwqe_dev *cd,
+			   struct ddcb_requ *req,
+			   unsigned int f_flags);
 
-int  __genwqe_enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req);
 int  __genwqe_wait_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req);
 int  __genwqe_purge_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req);
 
diff --git a/drivers/misc/genwqe/card_ddcb.c b/drivers/misc/genwqe/card_ddcb.c
index dc9851a5540e..6d51e5f08664 100644
--- a/drivers/misc/genwqe/card_ddcb.c
+++ b/drivers/misc/genwqe/card_ddcb.c
@@ -5,7 +5,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -185,8 +185,7 @@ static void print_ddcb_info(struct genwqe_dev *cd, struct ddcb_queue *queue)
 	pddcb = queue->ddcb_vaddr;
 	for (i = 0; i < queue->ddcb_max; i++) {
 		dev_err(&pci_dev->dev,
-			"  %c %-3d: RETC=%03x SEQ=%04x "
-			"HSI=%02X SHI=%02x PRIV=%06llx CMD=%03x\n",
+			"  %c %-3d: RETC=%03x SEQ=%04x HSI=%02X SHI=%02x PRIV=%06llx CMD=%03x\n",
 			i == queue->ddcb_act ? '>' : ' ',
 			i,
 			be16_to_cpu(pddcb->retc_16),
@@ -214,6 +213,7 @@ struct genwqe_ddcb_cmd *ddcb_requ_alloc(void)
 void ddcb_requ_free(struct genwqe_ddcb_cmd *cmd)
 {
 	struct ddcb_requ *req = container_of(cmd, struct ddcb_requ, cmd);
+
 	kfree(req);
 }
 
@@ -306,7 +306,7 @@ static int enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_queue *queue,
 
 		new = (old | DDCB_NEXT_BE32);
 
-		wmb();
+		wmb();		/* need to ensure write ordering */
 		icrc_hsi_shi = cmpxchg(&prev_ddcb->icrc_hsi_shi_32, old, new);
 
 		if (icrc_hsi_shi == old)
@@ -317,7 +317,7 @@ static int enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_queue *queue,
 	ddcb_mark_tapped(pddcb);
 	num = (u64)ddcb_no << 8;
 
-	wmb();
+	wmb();			/* need to ensure write ordering */
 	__genwqe_writeq(cd, queue->IO_QUEUE_OFFSET, num); /* start queue */
 
 	return RET_DDCB_TAPPED;
@@ -390,8 +390,9 @@ static int genwqe_check_ddcb_queue(struct genwqe_dev *cd,
 		    0x00000000)
 			goto go_home; /* not completed, continue waiting */
 
-		/* Note: DDCB could be purged */
+		wmb();  /*  Add sync to decouple prev. read operations */
 
+		/* Note: DDCB could be purged */
 		req = queue->ddcb_req[queue->ddcb_act];
 		if (req == NULL) {
 			/* this occurs if DDCB is purged, not an error */
@@ -416,9 +417,7 @@ static int genwqe_check_ddcb_queue(struct genwqe_dev *cd,
 			status  = __genwqe_readq(cd, queue->IO_QUEUE_STATUS);
 
 			dev_err(&pci_dev->dev,
-				"[%s] SEQN=%04x HSI=%02x RETC=%03x "
-				" Q_ERRCNTS=%016llx Q_STATUS=%016llx\n"
-				" DDCB_DMA_ADDR=%016llx\n",
+				"[%s] SEQN=%04x HSI=%02x RETC=%03x Q_ERRCNTS=%016llx Q_STATUS=%016llx DDCB_DMA_ADDR=%016llx\n",
 				__func__, be16_to_cpu(pddcb->seqnum_16),
 				pddcb->hsi, retc_16, errcnts, status,
 				queue->ddcb_daddr + ddcb_offs);
@@ -439,8 +438,7 @@ static int genwqe_check_ddcb_queue(struct genwqe_dev *cd,
 		vcrc_16 = be16_to_cpu(pddcb->vcrc_16);
 		if (vcrc != vcrc_16) {
 			printk_ratelimited(KERN_ERR
-				"%s %s: err: wrong VCRC pre=%02x vcrc_len=%d "
-				"bytes vcrc_data=%04x is not vcrc_card=%04x\n",
+				"%s %s: err: wrong VCRC pre=%02x vcrc_len=%d bytes vcrc_data=%04x is not vcrc_card=%04x\n",
 				GENWQE_DEVNAME, dev_name(&pci_dev->dev),
 				pddcb->pre, VCRC_LENGTH(req->cmd.asv_length),
 				vcrc, vcrc_16);
@@ -450,8 +448,10 @@ static int genwqe_check_ddcb_queue(struct genwqe_dev *cd,
 		queue->ddcbs_completed++;
 		queue->ddcbs_in_flight--;
 
-		/* wake up process waiting for this DDCB */
+		/* wake up process waiting for this DDCB, and
+                   processes on the busy queue */
 		wake_up_interruptible(&queue->ddcb_waitqs[queue->ddcb_act]);
+		wake_up_interruptible(&queue->busy_waitq);
 
 pick_next_one:
 		queue->ddcb_act = (queue->ddcb_act + 1) % queue->ddcb_max;
@@ -717,8 +717,7 @@ go_home:
 	genwqe_hexdump(pci_dev, pddcb, sizeof(*pddcb));
 
 	dev_err(&pci_dev->dev,
-		"[%s] err: DDCB#%d not purged and not completed "
-		"after %d seconds QSTAT=%016llx!!\n",
+		"[%s] err: DDCB#%d not purged and not completed after %d seconds QSTAT=%016llx!!\n",
 		__func__, req->num, genwqe_ddcb_software_timeout,
 		queue_status);
 
@@ -740,7 +739,7 @@ int genwqe_init_debug_data(struct genwqe_dev *cd, struct genwqe_debug_data *d)
 	}
 
 	len  = sizeof(d->driver_version);
-	snprintf(d->driver_version, len, "%s", DRV_VERS_STRING);
+	snprintf(d->driver_version, len, "%s", DRV_VERSION);
 	d->slu_unitcfg = cd->slu_unitcfg;
 	d->app_unitcfg = cd->app_unitcfg;
 	return 0;
@@ -748,14 +747,16 @@ int genwqe_init_debug_data(struct genwqe_dev *cd, struct genwqe_debug_data *d)
 
 /**
  * __genwqe_enqueue_ddcb() - Enqueue a DDCB
- * @cd:          pointer to genwqe device descriptor
- * @req:         pointer to DDCB execution request
+ * @cd:         pointer to genwqe device descriptor
+ * @req:        pointer to DDCB execution request
+ * @f_flags:    file mode: blocking, non-blocking
  *
  * Return: 0 if enqueuing succeeded
  *         -EIO if card is unusable/PCIe problems
  *         -EBUSY if enqueuing failed
  */
-int __genwqe_enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req)
+int __genwqe_enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req,
+			  unsigned int f_flags)
 {
 	struct ddcb *pddcb;
 	unsigned long flags;
@@ -763,6 +764,7 @@ int __genwqe_enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req)
 	struct pci_dev *pci_dev = cd->pci_dev;
 	u16 icrc;
 
+ retry:
 	if (cd->card_state != GENWQE_CARD_USED) {
 		printk_ratelimited(KERN_ERR
 			"%s %s: [%s] Card is unusable/PCIe problem Req#%d\n",
@@ -788,9 +790,24 @@ int __genwqe_enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req)
 
 	pddcb = get_next_ddcb(cd, queue, &req->num);	/* get ptr and num */
 	if (pddcb == NULL) {
+		int rc;
+
 		spin_unlock_irqrestore(&queue->ddcb_lock, flags);
-		queue->busy++;
-		return -EBUSY;
+
+		if (f_flags & O_NONBLOCK) {
+			queue->return_on_busy++;
+			return -EBUSY;
+		}
+
+		queue->wait_on_busy++;
+		rc = wait_event_interruptible(queue->busy_waitq,
+					      queue_free_ddcbs(queue) != 0);
+		dev_dbg(&pci_dev->dev, "[%s] waiting for free DDCB: rc=%d\n",
+			__func__, rc);
+		if (rc == -ERESTARTSYS)
+			return rc;  /* interrupted by a signal */
+
+		goto retry;
 	}
 
 	if (queue->ddcb_req[req->num] != NULL) {
@@ -893,9 +910,11 @@ int __genwqe_enqueue_ddcb(struct genwqe_dev *cd, struct ddcb_requ *req)
  * __genwqe_execute_raw_ddcb() - Setup and execute DDCB
  * @cd:         pointer to genwqe device descriptor
  * @req:        user provided DDCB request
+ * @f_flags:    file mode: blocking, non-blocking
  */
 int __genwqe_execute_raw_ddcb(struct genwqe_dev *cd,
-			     struct genwqe_ddcb_cmd *cmd)
+			      struct genwqe_ddcb_cmd *cmd,
+			      unsigned int f_flags)
 {
 	int rc = 0;
 	struct pci_dev *pci_dev = cd->pci_dev;
@@ -911,7 +930,7 @@ int __genwqe_execute_raw_ddcb(struct genwqe_dev *cd,
 			__func__, cmd->asiv_length);
 		return -EINVAL;
 	}
-	rc = __genwqe_enqueue_ddcb(cd, req);
+	rc = __genwqe_enqueue_ddcb(cd, req, f_flags);
 	if (rc != 0)
 		return rc;
 
@@ -1017,7 +1036,8 @@ static int setup_ddcb_queue(struct genwqe_dev *cd, struct ddcb_queue *queue)
 	queue->ddcbs_in_flight = 0;  /* statistics */
 	queue->ddcbs_max_in_flight = 0;
 	queue->ddcbs_completed = 0;
-	queue->busy = 0;
+	queue->return_on_busy = 0;
+	queue->wait_on_busy = 0;
 
 	queue->ddcb_seq	  = 0x100; /* start sequence number */
 	queue->ddcb_max	  = genwqe_ddcb_max; /* module parameter */
@@ -1057,7 +1077,7 @@ static int setup_ddcb_queue(struct genwqe_dev *cd, struct ddcb_queue *queue)
 	queue->ddcb_next = 0;	/* queue is empty */
 
 	spin_lock_init(&queue->ddcb_lock);
-	init_waitqueue_head(&queue->ddcb_waitq);
+	init_waitqueue_head(&queue->busy_waitq);
 
 	val64 = ((u64)(queue->ddcb_max - 1) <<  8); /* lastptr */
 	__genwqe_writeq(cd, queue->IO_QUEUE_CONFIG,  0x07);  /* iCRC/vCRC */
@@ -1251,10 +1271,8 @@ int genwqe_setup_service_layer(struct genwqe_dev *cd)
 	}
 
 	rc = genwqe_set_interrupt_capability(cd, GENWQE_MSI_IRQS);
-	if (rc) {
-		rc = -ENODEV;
+	if (rc)
 		goto stop_kthread;
-	}
 
 	/*
 	 * We must have all wait-queues initialized when we enable the
@@ -1307,6 +1325,7 @@ static int queue_wake_up_all(struct genwqe_dev *cd)
 	for (i = 0; i < queue->ddcb_max; i++)
 		wake_up_interruptible(&queue->ddcb_waitqs[queue->ddcb_act]);
 
+	wake_up_interruptible(&queue->busy_waitq);
 	spin_unlock_irqrestore(&queue->ddcb_lock, flags);
 
 	return 0;
@@ -1346,8 +1365,8 @@ int genwqe_finish_queue(struct genwqe_dev *cd)
 			break;
 
 		dev_dbg(&pci_dev->dev,
-			"  DEBUG [%d/%d] waiting for queue to get empty: "
-			"%d requests!\n", i, waitmax, in_flight);
+			"  DEBUG [%d/%d] waiting for queue to get empty: %d requests!\n",
+			i, waitmax, in_flight);
 
 		/*
 		 * Severe severe error situation: The card itself has
diff --git a/drivers/misc/genwqe/card_ddcb.h b/drivers/misc/genwqe/card_ddcb.h
index c4f26720753e..0361a68d79a6 100644
--- a/drivers/misc/genwqe/card_ddcb.h
+++ b/drivers/misc/genwqe/card_ddcb.h
@@ -8,7 +8,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/misc/genwqe/card_debugfs.c b/drivers/misc/genwqe/card_debugfs.c
index c9b4d6d0eb99..c715534e7fe7 100644
--- a/drivers/misc/genwqe/card_debugfs.c
+++ b/drivers/misc/genwqe/card_debugfs.c
@@ -5,7 +5,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -244,14 +244,16 @@ static int genwqe_ddcb_info_show(struct seq_file *s, void *unused)
 		   "  ddcbs_in_flight:     %u\n"
 		   "  ddcbs_max_in_flight: %u\n"
 		   "  ddcbs_completed:     %u\n"
-		   "  busy:                %u\n"
+		   "  return_on_busy:      %u\n"
+		   "  wait_on_busy:        %u\n"
 		   "  irqs_processed:      %u\n",
 		   queue->ddcb_max, (long long)queue->ddcb_daddr,
 		   (long long)queue->ddcb_daddr +
 		   (queue->ddcb_max * DDCB_LENGTH),
 		   (long long)queue->ddcb_vaddr, queue->ddcbs_in_flight,
 		   queue->ddcbs_max_in_flight, queue->ddcbs_completed,
-		   queue->busy, cd->irqs_processed);
+		   queue->return_on_busy, queue->wait_on_busy,
+		   cd->irqs_processed);
 
 	/* Hardware State */
 	seq_printf(s, "  0x%08x 0x%016llx IO_QUEUE_CONFIG\n"
@@ -323,7 +325,7 @@ static int genwqe_info_show(struct seq_file *s, void *unused)
 		   "    Base Clock      : %u MHz\n"
 		   "    Arch/SVN Release: %u/%llx\n"
 		   "    Bitstream       : %llx\n",
-		   GENWQE_DEVNAME, DRV_VERS_STRING, dev_name(&pci_dev->dev),
+		   GENWQE_DEVNAME, DRV_VERSION, dev_name(&pci_dev->dev),
 		   genwqe_is_privileged(cd) ?
 		   "Physical" : "Virtual or no SR-IOV",
 		   cd->card_idx, slu_id, app_id,
diff --git a/drivers/misc/genwqe/card_dev.c b/drivers/misc/genwqe/card_dev.c
index aae42555e2ca..5918586f2f76 100644
--- a/drivers/misc/genwqe/card_dev.c
+++ b/drivers/misc/genwqe/card_dev.c
@@ -5,7 +5,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -213,9 +213,9 @@ static void genwqe_remove_mappings(struct genwqe_file *cfile)
 		 * GENWQE_MAPPING_SGL_TEMP should be removed by tidy up code.
 		 */
 		dev_err(&pci_dev->dev,
-			"[%s] %d. cleanup mapping: u_vaddr=%p "
-			"u_kaddr=%016lx dma_addr=%lx\n", __func__, i++,
-			dma_map->u_vaddr, (unsigned long)dma_map->k_vaddr,
+			"[%s] %d. cleanup mapping: u_vaddr=%p u_kaddr=%016lx dma_addr=%lx\n",
+			__func__, i++, dma_map->u_vaddr,
+			(unsigned long)dma_map->k_vaddr,
 			(unsigned long)dma_map->dma_addr);
 
 		if (dma_map->type == GENWQE_MAPPING_RAW) {
@@ -346,6 +346,7 @@ static int genwqe_open(struct inode *inode, struct file *filp)
 static int genwqe_fasync(int fd, struct file *filp, int mode)
 {
 	struct genwqe_file *cdev = (struct genwqe_file *)filp->private_data;
+
 	return fasync_helper(fd, filp, mode, &cdev->async_queue);
 }
 
@@ -515,6 +516,7 @@ static int do_flash_update(struct genwqe_file *cfile,
 	u32 crc;
 	u8 cmdopts;
 	struct genwqe_dev *cd = cfile->cd;
+	struct file *filp = cfile->filp;
 	struct pci_dev *pci_dev = cd->pci_dev;
 
 	if ((load->size & 0x3) != 0)
@@ -609,7 +611,7 @@ static int do_flash_update(struct genwqe_file *cfile,
 		/* For Genwqe5 we get back the calculated CRC */
 		*(u64 *)&req->asv[0] = 0ULL;			/* 0x80 */
 
-		rc = __genwqe_execute_raw_ddcb(cd, req);
+		rc = __genwqe_execute_raw_ddcb(cd, req, filp->f_flags);
 
 		load->retc = req->retc;
 		load->attn = req->attn;
@@ -649,6 +651,7 @@ static int do_flash_read(struct genwqe_file *cfile,
 	u8 *xbuf;
 	u8 cmdopts;
 	struct genwqe_dev *cd = cfile->cd;
+	struct file *filp = cfile->filp;
 	struct pci_dev *pci_dev = cd->pci_dev;
 	struct genwqe_ddcb_cmd *cmd;
 
@@ -726,7 +729,7 @@ static int do_flash_read(struct genwqe_file *cfile,
 		/* we only get back the calculated CRC */
 		*(u64 *)&cmd->asv[0] = 0ULL;	/* 0x80 */
 
-		rc = __genwqe_execute_raw_ddcb(cd, cmd);
+		rc = __genwqe_execute_raw_ddcb(cd, cmd, filp->f_flags);
 
 		load->retc = cmd->retc;
 		load->attn = cmd->attn;
@@ -987,13 +990,14 @@ static int genwqe_execute_ddcb(struct genwqe_file *cfile,
 {
 	int rc;
 	struct genwqe_dev *cd = cfile->cd;
+	struct file *filp = cfile->filp;
 	struct ddcb_requ *req = container_of(cmd, struct ddcb_requ, cmd);
 
 	rc = ddcb_cmd_fixups(cfile, req);
 	if (rc != 0)
 		return rc;
 
-	rc = __genwqe_execute_raw_ddcb(cd, cmd);
+	rc = __genwqe_execute_raw_ddcb(cd, cmd, filp->f_flags);
 	ddcb_cmd_cleanup(cfile, req);
 	return rc;
 }
@@ -1005,6 +1009,7 @@ static int do_execute_ddcb(struct genwqe_file *cfile,
 	struct genwqe_ddcb_cmd *cmd;
 	struct ddcb_requ *req;
 	struct genwqe_dev *cd = cfile->cd;
+	struct file *filp = cfile->filp;
 
 	cmd = ddcb_requ_alloc();
 	if (cmd == NULL)
@@ -1020,7 +1025,7 @@ static int do_execute_ddcb(struct genwqe_file *cfile,
 	if (!raw)
 		rc = genwqe_execute_ddcb(cfile, cmd);
 	else
-		rc = __genwqe_execute_raw_ddcb(cd, cmd);
+		rc = __genwqe_execute_raw_ddcb(cd, cmd, filp->f_flags);
 
 	/* Copy back only the modifed fields. Do not copy ASIV
 	   back since the copy got modified by the driver. */
diff --git a/drivers/misc/genwqe/card_sysfs.c b/drivers/misc/genwqe/card_sysfs.c
index 7232e40a3ad9..2c33fbca9225 100644
--- a/drivers/misc/genwqe/card_sysfs.c
+++ b/drivers/misc/genwqe/card_sysfs.c
@@ -5,7 +5,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -91,13 +91,6 @@ static ssize_t type_show(struct device *dev, struct device_attribute *attr,
 }
 static DEVICE_ATTR_RO(type);
 
-static ssize_t driver_show(struct device *dev, struct device_attribute *attr,
-			   char *buf)
-{
-	return sprintf(buf, "%s\n", DRV_VERS_STRING);
-}
-static DEVICE_ATTR_RO(driver);
-
 static ssize_t tempsens_show(struct device *dev, struct device_attribute *attr,
 			     char *buf)
 {
@@ -256,7 +249,6 @@ static struct attribute *genwqe_attributes[] = {
 	&dev_attr_next_bitstream.attr,
 	&dev_attr_curr_bitstream.attr,
 	&dev_attr_base_clock.attr,
-	&dev_attr_driver.attr,
 	&dev_attr_type.attr,
 	&dev_attr_version.attr,
 	&dev_attr_appid.attr,
@@ -268,7 +260,6 @@ static struct attribute *genwqe_attributes[] = {
 };
 
 static struct attribute *genwqe_normal_attributes[] = {
-	&dev_attr_driver.attr,
 	&dev_attr_type.attr,
 	&dev_attr_version.attr,
 	&dev_attr_appid.attr,
diff --git a/drivers/misc/genwqe/card_utils.c b/drivers/misc/genwqe/card_utils.c
index a6400f09229c..7cb3b7e41739 100644
--- a/drivers/misc/genwqe/card_utils.c
+++ b/drivers/misc/genwqe/card_utils.c
@@ -5,7 +5,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -150,6 +150,7 @@ int genwqe_read_app_id(struct genwqe_dev *cd, char *app_name, int len)
 	memset(app_name, 0, len);
 	for (i = 0, j = 0; j < min(len, 4); j++) {
 		char ch = (char)((app_id >> (24 - j*8)) & 0xff);
+
 		if (ch == ' ')
 			continue;
 		app_name[i++] = isprint(ch) ? ch : 'X';
@@ -304,8 +305,7 @@ int genwqe_alloc_sync_sgl(struct genwqe_dev *cd, struct genwqe_sgl *sgl,
 	sgl->nr_pages = DIV_ROUND_UP(sgl->fpage_offs + user_size, PAGE_SIZE);
 	sgl->lpage_size = (user_size - sgl->fpage_size) % PAGE_SIZE;
 
-	dev_dbg(&pci_dev->dev, "[%s] uaddr=%p usize=%8ld nr_pages=%ld "
-		"fpage_offs=%lx fpage_size=%ld lpage_size=%ld\n",
+	dev_dbg(&pci_dev->dev, "[%s] uaddr=%p usize=%8ld nr_pages=%ld fpage_offs=%lx fpage_size=%ld lpage_size=%ld\n",
 		__func__, user_addr, user_size, sgl->nr_pages,
 		sgl->fpage_offs, sgl->fpage_size, sgl->lpage_size);
 
@@ -662,6 +662,7 @@ int genwqe_user_vunmap(struct genwqe_dev *cd, struct dma_mapping *m,
 u8 genwqe_card_type(struct genwqe_dev *cd)
 {
 	u64 card_type = cd->slu_unitcfg;
+
 	return (u8)((card_type & IO_SLU_UNITCFG_TYPE_MASK) >> 20);
 }
 
diff --git a/drivers/misc/genwqe/genwqe_driver.h b/drivers/misc/genwqe/genwqe_driver.h
index a506e9aa2d57..15355350e076 100644
--- a/drivers/misc/genwqe/genwqe_driver.h
+++ b/drivers/misc/genwqe/genwqe_driver.h
@@ -8,7 +8,7 @@
  *
  * Author: Frank Haverkamp <haver@linux.vnet.ibm.com>
  * Author: Joerg-Stephan Vogt <jsvogt@de.ibm.com>
- * Author: Michael Jung <mijung@de.ibm.com>
+ * Author: Michael Jung <mijung@gmx.net>
  * Author: Michael Ruettger <michael@ibmra.de>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -36,7 +36,7 @@
 #include <asm/byteorder.h>
 #include <linux/genwqe/genwqe_card.h>
 
-#define DRV_VERS_STRING		"2.0.21"
+#define DRV_VERSION		"2.0.25"
 
 /*
  * Static minor number assignement, until we decide/implement
diff --git a/drivers/misc/lattice-ecp3-config.c b/drivers/misc/lattice-ecp3-config.c
index 7ffdb589841e..c544f1f50f52 100644
--- a/drivers/misc/lattice-ecp3-config.c
+++ b/drivers/misc/lattice-ecp3-config.c
@@ -79,6 +79,11 @@ static void firmware_load(const struct firmware *fw, void *context)
 	u32 jedec_id;
 	u32 status;
 
+	if (fw == NULL) {
+		dev_err(&spi->dev, "Cannot load firmware, aborting\n");
+		return;
+	}
+
 	if (fw->size == 0) {
 		dev_err(&spi->dev, "Error: Firmware size is 0!\n");
 		return;
@@ -242,3 +247,4 @@ module_spi_driver(lattice_ecp3_driver);
 MODULE_AUTHOR("Stefan Roese <sr@denx.de>");
 MODULE_DESCRIPTION("Lattice ECP3 FPGA configuration via SPI");
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE(FIRMWARE_NAME);
diff --git a/drivers/misc/mei/amthif.c b/drivers/misc/mei/amthif.c
index 0d6234db00fa..6cdce8477f57 100644
--- a/drivers/misc/mei/amthif.c
+++ b/drivers/misc/mei/amthif.c
@@ -20,7 +20,6 @@
 #include <linux/types.h>
 #include <linux/fcntl.h>
 #include <linux/aio.h>
-#include <linux/pci.h>
 #include <linux/ioctl.h>
 #include <linux/cdev.h>
 #include <linux/list.h>
@@ -29,6 +28,7 @@
 #include <linux/uuid.h>
 #include <linux/jiffies.h>
 #include <linux/uaccess.h>
+#include <linux/slab.h>
 
 #include <linux/mei.h>
 
@@ -64,31 +64,32 @@ void mei_amthif_reset_params(struct mei_device *dev)
  *
  * @dev: the device structure
  *
+ * Return: 0 on success, <0 on failure.
  */
 int mei_amthif_host_init(struct mei_device *dev)
 {
 	struct mei_cl *cl = &dev->iamthif_cl;
+	struct mei_me_client *me_cl;
 	unsigned char *msg_buf;
-	int ret, i;
+	int ret;
 
 	dev->iamthif_state = MEI_IAMTHIF_IDLE;
 
 	mei_cl_init(cl, dev);
 
-	i = mei_me_cl_by_uuid(dev, &mei_amthif_guid);
-	if (i < 0) {
-		dev_info(&dev->pdev->dev,
-			"amthif: failed to find the client %d\n", i);
+	me_cl = mei_me_cl_by_uuid(dev, &mei_amthif_guid);
+	if (!me_cl) {
+		dev_info(dev->dev, "amthif: failed to find the client");
 		return -ENOTTY;
 	}
 
-	cl->me_client_id = dev->me_clients[i].client_id;
+	cl->me_client_id = me_cl->client_id;
+	cl->cl_uuid = me_cl->props.protocol_name;
 
 	/* Assign iamthif_mtu to the value received from ME  */
 
-	dev->iamthif_mtu = dev->me_clients[i].props.max_msg_length;
-	dev_dbg(&dev->pdev->dev, "IAMTHIF_MTU = %d\n",
-			dev->me_clients[i].props.max_msg_length);
+	dev->iamthif_mtu = me_cl->props.max_msg_length;
+	dev_dbg(dev->dev, "IAMTHIF_MTU = %d\n", dev->iamthif_mtu);
 
 	kfree(dev->iamthif_msg_buf);
 	dev->iamthif_msg_buf = NULL;
@@ -96,17 +97,15 @@ int mei_amthif_host_init(struct mei_device *dev)
 	/* allocate storage for ME message buffer */
 	msg_buf = kcalloc(dev->iamthif_mtu,
 			sizeof(unsigned char), GFP_KERNEL);
-	if (!msg_buf) {
-		dev_err(&dev->pdev->dev, "amthif: memory allocation for ME message buffer failed.\n");
+	if (!msg_buf)
 		return -ENOMEM;
-	}
 
 	dev->iamthif_msg_buf = msg_buf;
 
 	ret = mei_cl_link(cl, MEI_IAMTHIF_HOST_CLIENT_ID);
 
 	if (ret < 0) {
-		dev_err(&dev->pdev->dev,
+		dev_err(dev->dev,
 			"amthif: failed link client %d\n", ret);
 		return ret;
 	}
@@ -124,18 +123,16 @@ int mei_amthif_host_init(struct mei_device *dev)
  * @dev: the device structure
  * @file: pointer to file object
  *
- * returns   returned a list entry on success, NULL on failure.
+ * Return:   returned a list entry on success, NULL on failure.
  */
 struct mei_cl_cb *mei_amthif_find_read_list_entry(struct mei_device *dev,
 						struct file *file)
 {
 	struct mei_cl_cb *cb;
 
-	list_for_each_entry(cb, &dev->amthif_rd_complete_list.list, list) {
-		if (cb->cl && cb->cl == &dev->iamthif_cl &&
-			cb->file_object == file)
+	list_for_each_entry(cb, &dev->amthif_rd_complete_list.list, list)
+		if (cb->file_object == file)
 			return cb;
-	}
 	return NULL;
 }
 
@@ -144,15 +141,14 @@ struct mei_cl_cb *mei_amthif_find_read_list_entry(struct mei_device *dev,
  * mei_amthif_read - read data from AMTHIF client
  *
  * @dev: the device structure
- * @if_num:  minor number
  * @file: pointer to file object
- * @*ubuf: pointer to user data in user space
+ * @ubuf: pointer to user data in user space
  * @length: data length to read
  * @offset: data read offset
  *
  * Locking: called under "dev->device_lock" lock
  *
- * returns
+ * Return:
  *  returned data length on success,
  *  zero if no data to read,
  *  negative on failure.
@@ -160,25 +156,19 @@ struct mei_cl_cb *mei_amthif_find_read_list_entry(struct mei_device *dev,
 int mei_amthif_read(struct mei_device *dev, struct file *file,
 	       char __user *ubuf, size_t length, loff_t *offset)
 {
-	int rets;
-	int wait_ret;
-	struct mei_cl_cb *cb = NULL;
 	struct mei_cl *cl = file->private_data;
+	struct mei_cl_cb *cb;
 	unsigned long timeout;
-	int i;
+	int rets;
+	int wait_ret;
 
 	/* Only possible if we are in timeout */
-	if (!cl || cl != &dev->iamthif_cl) {
-		dev_dbg(&dev->pdev->dev, "bad file ext.\n");
+	if (!cl) {
+		dev_err(dev->dev, "bad file ext.\n");
 		return -ETIME;
 	}
 
-	i = mei_me_cl_by_id(dev, dev->iamthif_cl.me_client_id);
-	if (i < 0) {
-		dev_dbg(&dev->pdev->dev, "amthif client not found.\n");
-		return -ENOTTY;
-	}
-	dev_dbg(&dev->pdev->dev, "checking amthif data\n");
+	dev_dbg(dev->dev, "checking amthif data\n");
 	cb = mei_amthif_find_read_list_entry(dev, file);
 
 	/* Check for if we can block or not*/
@@ -186,7 +176,7 @@ int mei_amthif_read(struct mei_device *dev, struct file *file,
 		return -EAGAIN;
 
 
-	dev_dbg(&dev->pdev->dev, "waiting for amthif data\n");
+	dev_dbg(dev->dev, "waiting for amthif data\n");
 	while (cb == NULL) {
 		/* unlock the Mutex */
 		mutex_unlock(&dev->device_lock);
@@ -200,21 +190,21 @@ int mei_amthif_read(struct mei_device *dev, struct file *file,
 		if (wait_ret)
 			return -ERESTARTSYS;
 
-		dev_dbg(&dev->pdev->dev, "woke up from sleep\n");
+		dev_dbg(dev->dev, "woke up from sleep\n");
 	}
 
 
-	dev_dbg(&dev->pdev->dev, "Got amthif data\n");
+	dev_dbg(dev->dev, "Got amthif data\n");
 	dev->iamthif_timer = 0;
 
 	if (cb) {
 		timeout = cb->read_time +
 			mei_secs_to_jiffies(MEI_IAMTHIF_READ_TIMER);
-		dev_dbg(&dev->pdev->dev, "amthif timeout = %lud\n",
+		dev_dbg(dev->dev, "amthif timeout = %lud\n",
 				timeout);
 
 		if  (time_after(jiffies, timeout)) {
-			dev_dbg(&dev->pdev->dev, "amthif Time out\n");
+			dev_dbg(dev->dev, "amthif Time out\n");
 			/* 15 sec for the message has expired */
 			list_del(&cb->list);
 			rets = -ETIME;
@@ -234,16 +224,16 @@ int mei_amthif_read(struct mei_device *dev, struct file *file,
 		 * remove message from deletion list
 		 */
 
-	dev_dbg(&dev->pdev->dev, "amthif cb->response_buffer size - %d\n",
+	dev_dbg(dev->dev, "amthif cb->response_buffer size - %d\n",
 	    cb->response_buffer.size);
-	dev_dbg(&dev->pdev->dev, "amthif cb->buf_idx - %lu\n", cb->buf_idx);
+	dev_dbg(dev->dev, "amthif cb->buf_idx - %lu\n", cb->buf_idx);
 
 	/* length is being truncated to PAGE_SIZE, however,
 	 * the buf_idx may point beyond */
 	length = min_t(size_t, length, (cb->buf_idx - *offset));
 
 	if (copy_to_user(ubuf, cb->response_buffer.data + *offset, length)) {
-		dev_dbg(&dev->pdev->dev, "failed to copy data to userland\n");
+		dev_dbg(dev->dev, "failed to copy data to userland\n");
 		rets = -EFAULT;
 	} else {
 		rets = length;
@@ -253,7 +243,7 @@ int mei_amthif_read(struct mei_device *dev, struct file *file,
 		}
 	}
 free:
-	dev_dbg(&dev->pdev->dev, "free amthif cb memory.\n");
+	dev_dbg(dev->dev, "free amthif cb memory.\n");
 	*offset = 0;
 	mei_io_cb_free(cb);
 out:
@@ -266,7 +256,7 @@ out:
  * @dev: the device structure
  * @cb: mei call back struct
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  *
  */
 static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
@@ -277,7 +267,7 @@ static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
 	if (!dev || !cb)
 		return -ENODEV;
 
-	dev_dbg(&dev->pdev->dev, "write data to amthif client.\n");
+	dev_dbg(dev->dev, "write data to amthif client.\n");
 
 	dev->iamthif_state = MEI_IAMTHIF_WRITING;
 	dev->iamthif_current_cb = cb;
@@ -316,12 +306,12 @@ static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
 				return -EIO;
 			dev->iamthif_flow_control_pending = true;
 			dev->iamthif_state = MEI_IAMTHIF_FLOW_CONTROL;
-			dev_dbg(&dev->pdev->dev, "add amthif cb to write waiting list\n");
+			dev_dbg(dev->dev, "add amthif cb to write waiting list\n");
 			dev->iamthif_current_cb = cb;
 			dev->iamthif_file_object = cb->file_object;
 			list_add_tail(&cb->list, &dev->write_waiting_list.list);
 		} else {
-			dev_dbg(&dev->pdev->dev, "message does not complete, so add amthif cb to write list.\n");
+			dev_dbg(dev->dev, "message does not complete, so add amthif cb to write list.\n");
 			list_add_tail(&cb->list, &dev->write_list.list);
 		}
 	} else {
@@ -336,7 +326,7 @@ static int mei_amthif_send_cmd(struct mei_device *dev, struct mei_cl_cb *cb)
  * @dev: the device structure
  * @cb: mei call back struct
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  *
  */
 int mei_amthif_write(struct mei_device *dev, struct mei_cl_cb *cb)
@@ -354,25 +344,23 @@ int mei_amthif_write(struct mei_device *dev, struct mei_cl_cb *cb)
 
 	if (!list_empty(&dev->amthif_cmd_list.list) ||
 	    dev->iamthif_state != MEI_IAMTHIF_IDLE) {
-		dev_dbg(&dev->pdev->dev,
+		dev_dbg(dev->dev,
 			"amthif state = %d\n", dev->iamthif_state);
-		dev_dbg(&dev->pdev->dev, "AMTHIF: add cb to the wait list\n");
+		dev_dbg(dev->dev, "AMTHIF: add cb to the wait list\n");
 		list_add_tail(&cb->list, &dev->amthif_cmd_list.list);
 		return 0;
 	}
 	return mei_amthif_send_cmd(dev, cb);
 }
 /**
- * mei_amthif_run_next_cmd
+ * mei_amthif_run_next_cmd - send next amt command from queue
  *
  * @dev: the device structure
- *
- * returns 0 on success, <0 on failure.
  */
 void mei_amthif_run_next_cmd(struct mei_device *dev)
 {
-	struct mei_cl_cb *pos = NULL;
-	struct mei_cl_cb *next = NULL;
+	struct mei_cl_cb *cb;
+	struct mei_cl_cb *next;
 	int status;
 
 	if (!dev)
@@ -386,21 +374,17 @@ void mei_amthif_run_next_cmd(struct mei_device *dev)
 	dev->iamthif_timer = 0;
 	dev->iamthif_file_object = NULL;
 
-	dev_dbg(&dev->pdev->dev, "complete amthif cmd_list cb.\n");
-
-	list_for_each_entry_safe(pos, next, &dev->amthif_cmd_list.list, list) {
-		list_del(&pos->list);
+	dev_dbg(dev->dev, "complete amthif cmd_list cb.\n");
 
-		if (pos->cl && pos->cl == &dev->iamthif_cl) {
-			status = mei_amthif_send_cmd(dev, pos);
-			if (status) {
-				dev_dbg(&dev->pdev->dev,
-					"amthif write failed status = %d\n",
+	list_for_each_entry_safe(cb, next, &dev->amthif_cmd_list.list, list) {
+		list_del(&cb->list);
+		if (!cb->cl)
+			continue;
+		status = mei_amthif_send_cmd(dev, cb);
+		if (status)
+			dev_warn(dev->dev, "amthif write failed status = %d\n",
 						status);
-				return;
-			}
-			break;
-		}
+		break;
 	}
 }
 
@@ -421,7 +405,7 @@ unsigned int mei_amthif_poll(struct mei_device *dev,
 		   dev->iamthif_file_object == file) {
 
 		mask |= (POLLIN | POLLRDNORM);
-		dev_dbg(&dev->pdev->dev, "run next amthif cb\n");
+		dev_dbg(dev->dev, "run next amthif cb\n");
 		mei_amthif_run_next_cmd(dev);
 	}
 	mutex_unlock(&dev->device_lock);
@@ -434,12 +418,11 @@ unsigned int mei_amthif_poll(struct mei_device *dev,
 /**
  * mei_amthif_irq_write - write iamthif command in irq thread context.
  *
- * @dev: the device structure.
- * @cb_pos: callback block.
  * @cl: private data of the file object.
+ * @cb: callback block.
  * @cmpl_list: complete list.
  *
- * returns 0, OK; otherwise, error.
+ * Return: 0, OK; otherwise, error.
  */
 int mei_amthif_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 			 struct mei_cl_cb *cmpl_list)
@@ -481,7 +464,7 @@ int mei_amthif_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 		return 0;
 	}
 
-	dev_dbg(&dev->pdev->dev, MEI_HDR_FMT,  MEI_HDR_PRM(&mei_hdr));
+	dev_dbg(dev->dev, MEI_HDR_FMT,  MEI_HDR_PRM(&mei_hdr));
 
 	rets = mei_write_message(dev, &mei_hdr,
 			dev->iamthif_msg_buf + dev->iamthif_msg_buf_index);
@@ -514,14 +497,14 @@ int mei_amthif_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 }
 
 /**
- * mei_amthif_irq_read_message - read routine after ISR to
+ * mei_amthif_irq_read_msg - read routine after ISR to
  *			handle the read amthif message
  *
  * @dev: the device structure
  * @mei_hdr: header of amthif message
  * @complete_list: An instance of our list structure
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 int mei_amthif_irq_read_msg(struct mei_device *dev,
 			    struct mei_msg_hdr *mei_hdr,
@@ -543,10 +526,10 @@ int mei_amthif_irq_read_msg(struct mei_device *dev,
 	if (!mei_hdr->msg_complete)
 		return 0;
 
-	dev_dbg(&dev->pdev->dev, "amthif_message_buffer_index =%d\n",
+	dev_dbg(dev->dev, "amthif_message_buffer_index =%d\n",
 			mei_hdr->length);
 
-	dev_dbg(&dev->pdev->dev, "completed amthif read.\n ");
+	dev_dbg(dev->dev, "completed amthif read.\n ");
 	if (!dev->iamthif_current_cb)
 		return -ENODEV;
 
@@ -559,10 +542,10 @@ int mei_amthif_irq_read_msg(struct mei_device *dev,
 	dev->iamthif_stall_timer = 0;
 	cb->buf_idx = dev->iamthif_msg_buf_index;
 	cb->read_time = jiffies;
-	if (dev->iamthif_ioctl && cb->cl == &dev->iamthif_cl) {
+	if (dev->iamthif_ioctl) {
 		/* found the iamthif cb */
-		dev_dbg(&dev->pdev->dev, "complete the amthif read cb.\n ");
-		dev_dbg(&dev->pdev->dev, "add the amthif read cb to complete.\n ");
+		dev_dbg(dev->dev, "complete the amthif read cb.\n ");
+		dev_dbg(dev->dev, "add the amthif read cb to complete.\n ");
 		list_add_tail(&cb->list, &complete_list->list);
 	}
 	return 0;
@@ -574,7 +557,7 @@ int mei_amthif_irq_read_msg(struct mei_device *dev,
  * @dev: the device structure.
  * @slots: free slots.
  *
- * returns 0, OK; otherwise, error.
+ * Return: 0, OK; otherwise, error.
  */
 int mei_amthif_irq_read(struct mei_device *dev, s32 *slots)
 {
@@ -586,11 +569,11 @@ int mei_amthif_irq_read(struct mei_device *dev, s32 *slots)
 	*slots -= msg_slots;
 
 	if (mei_hbm_cl_flow_control_req(dev, &dev->iamthif_cl)) {
-		dev_dbg(&dev->pdev->dev, "iamthif flow control failed\n");
+		dev_dbg(dev->dev, "iamthif flow control failed\n");
 		return -EIO;
 	}
 
-	dev_dbg(&dev->pdev->dev, "iamthif flow control success\n");
+	dev_dbg(dev->dev, "iamthif flow control success\n");
 	dev->iamthif_state = MEI_IAMTHIF_READING;
 	dev->iamthif_flow_control_pending = false;
 	dev->iamthif_msg_buf_index = 0;
@@ -604,7 +587,7 @@ int mei_amthif_irq_read(struct mei_device *dev, s32 *slots)
  * mei_amthif_complete - complete amthif callback.
  *
  * @dev: the device structure.
- * @cb_pos: callback block.
+ * @cb: callback block.
  */
 void mei_amthif_complete(struct mei_device *dev, struct mei_cl_cb *cb)
 {
@@ -615,15 +598,15 @@ void mei_amthif_complete(struct mei_device *dev, struct mei_cl_cb *cb)
 				dev->iamthif_msg_buf,
 				dev->iamthif_msg_buf_index);
 		list_add_tail(&cb->list, &dev->amthif_rd_complete_list.list);
-		dev_dbg(&dev->pdev->dev, "amthif read completed\n");
+		dev_dbg(dev->dev, "amthif read completed\n");
 		dev->iamthif_timer = jiffies;
-		dev_dbg(&dev->pdev->dev, "dev->iamthif_timer = %ld\n",
+		dev_dbg(dev->dev, "dev->iamthif_timer = %ld\n",
 				dev->iamthif_timer);
 	} else {
 		mei_amthif_run_next_cmd(dev);
 	}
 
-	dev_dbg(&dev->pdev->dev, "completing amthif call back.\n");
+	dev_dbg(dev->dev, "completing amthif call back.\n");
 	wake_up_interruptible(&dev->iamthif_cl.wait);
 }
 
@@ -638,7 +621,7 @@ void mei_amthif_complete(struct mei_device *dev, struct mei_cl_cb *cb)
  * mei_clear_list is called to clear resources associated with file
  * when application calls close function or Ctrl-C was pressed
  *
- * returns true if callback removed from the list, false otherwise
+ * Return: true if callback removed from the list, false otherwise
  */
 static bool mei_clear_list(struct mei_device *dev,
 		const struct file *file, struct list_head *mei_cb_list)
@@ -678,7 +661,7 @@ static bool mei_clear_list(struct mei_device *dev,
  * mei_clear_lists is called to clear resources associated with file
  * when application calls close function or Ctrl-C was pressed
  *
- * returns true if callback removed from the list, false otherwise
+ * Return: true if callback removed from the list, false otherwise
  */
 static bool mei_clear_lists(struct mei_device *dev, struct file *file)
 {
@@ -719,7 +702,7 @@ static bool mei_clear_lists(struct mei_device *dev, struct file *file)
 *  @dev: device structure
 *  @file: pointer to file structure
 *
-*  returns 0 on success, <0 on error
+*  Return: 0 on success, <0 on error
 */
 int mei_amthif_release(struct mei_device *dev, struct file *file)
 {
@@ -729,11 +712,11 @@ int mei_amthif_release(struct mei_device *dev, struct file *file)
 	if (dev->iamthif_file_object == file &&
 	    dev->iamthif_state != MEI_IAMTHIF_IDLE) {
 
-		dev_dbg(&dev->pdev->dev, "amthif canceled iamthif state %d\n",
+		dev_dbg(dev->dev, "amthif canceled iamthif state %d\n",
 		    dev->iamthif_state);
 		dev->iamthif_canceled = true;
 		if (dev->iamthif_state == MEI_IAMTHIF_READ_COMPLETE) {
-			dev_dbg(&dev->pdev->dev, "run next amthif iamthif cb\n");
+			dev_dbg(dev->dev, "run next amthif iamthif cb\n");
 			mei_amthif_run_next_cmd(dev);
 		}
 	}
diff --git a/drivers/misc/mei/bus.c b/drivers/misc/mei/bus.c
index 0e993ef28b94..4d20d60ca38d 100644
--- a/drivers/misc/mei/bus.c
+++ b/drivers/misc/mei/bus.c
@@ -22,7 +22,6 @@
 #include <linux/slab.h>
 #include <linux/mutex.h>
 #include <linux/interrupt.h>
-#include <linux/pci.h>
 #include <linux/mei_cl_bus.h>
 
 #include "mei_dev.h"
@@ -70,7 +69,7 @@ static int mei_cl_device_probe(struct device *dev)
 
 	dev_dbg(dev, "Device probe\n");
 
-	strncpy(id.name, dev_name(dev), sizeof(id.name));
+	strlcpy(id.name, dev_name(dev), sizeof(id.name));
 
 	return driver->probe(device, &id);
 }
@@ -147,7 +146,7 @@ static struct mei_cl *mei_bus_find_mei_cl_by_uuid(struct mei_device *dev,
 	struct mei_cl *cl;
 
 	list_for_each_entry(cl, &dev->device_list, device_link) {
-		if (!uuid_le_cmp(uuid, cl->device_uuid))
+		if (!uuid_le_cmp(uuid, cl->cl_uuid))
 			return cl;
 	}
 
@@ -172,7 +171,7 @@ struct mei_cl_device *mei_cl_add_device(struct mei_device *dev,
 	device->cl = cl;
 	device->ops = ops;
 
-	device->dev.parent = &dev->pdev->dev;
+	device->dev.parent = dev->dev;
 	device->dev.bus = &mei_cl_bus_type;
 	device->dev.type = &mei_cl_device_type;
 
@@ -180,7 +179,7 @@ struct mei_cl_device *mei_cl_add_device(struct mei_device *dev,
 
 	status = device_register(&device->dev);
 	if (status) {
-		dev_err(&dev->pdev->dev, "Failed to register MEI device\n");
+		dev_err(dev->dev, "Failed to register MEI device\n");
 		kfree(device);
 		return NULL;
 	}
@@ -229,8 +228,8 @@ static int ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
 			bool blocking)
 {
 	struct mei_device *dev;
+	struct mei_me_client *me_cl;
 	struct mei_cl_cb *cb;
-	int id;
 	int rets;
 
 	if (WARN_ON(!cl || !cl->dev))
@@ -242,11 +241,11 @@ static int ___mei_cl_send(struct mei_cl *cl, u8 *buf, size_t length,
 		return -ENODEV;
 
 	/* Check if we have an ME client device */
-	id = mei_me_cl_by_id(dev, cl->me_client_id);
-	if (id < 0)
-		return id;
+	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
+	if (!me_cl)
+		return -ENOTTY;
 
-	if (length > dev->me_clients[id].props.max_msg_length)
+	if (length > me_cl->props.max_msg_length)
 		return -EFBIG;
 
 	cb = mei_io_cb_init(cl, NULL);
@@ -430,7 +429,7 @@ int mei_cl_enable_device(struct mei_cl_device *device)
 	err = mei_cl_connect(cl, NULL);
 	if (err < 0) {
 		mutex_unlock(&dev->device_lock);
-		dev_err(&dev->pdev->dev, "Could not connect to the ME client");
+		dev_err(dev->dev, "Could not connect to the ME client");
 
 		return err;
 	}
@@ -462,7 +461,7 @@ int mei_cl_disable_device(struct mei_cl_device *device)
 
 	if (cl->state != MEI_FILE_CONNECTED) {
 		mutex_unlock(&dev->device_lock);
-		dev_err(&dev->pdev->dev, "Already disconnected");
+		dev_err(dev->dev, "Already disconnected");
 
 		return 0;
 	}
@@ -472,7 +471,7 @@ int mei_cl_disable_device(struct mei_cl_device *device)
 	err = mei_cl_disconnect(cl);
 	if (err < 0) {
 		mutex_unlock(&dev->device_lock);
-		dev_err(&dev->pdev->dev,
+		dev_err(dev->dev,
 			"Could not disconnect from the ME client");
 
 		return err;
diff --git a/drivers/misc/mei/client.c b/drivers/misc/mei/client.c
index 324e1de93687..bc9ba5359bc6 100644
--- a/drivers/misc/mei/client.c
+++ b/drivers/misc/mei/client.c
@@ -14,10 +14,10 @@
  *
  */
 
-#include <linux/pci.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/delay.h>
+#include <linux/slab.h>
 #include <linux/pm_runtime.h>
 
 #include <linux/mei.h>
@@ -27,47 +27,90 @@
 #include "client.h"
 
 /**
- * mei_me_cl_by_uuid - locate index of me client
+ * mei_me_cl_by_uuid - locate me client by uuid
  *
  * @dev: mei device
+ * @uuid: me client uuid
  *
  * Locking: called under "dev->device_lock" lock
  *
- * returns me client index or -ENOENT if not found
+ * Return: me client or NULL if not found
  */
-int mei_me_cl_by_uuid(const struct mei_device *dev, const uuid_le *uuid)
+struct mei_me_client *mei_me_cl_by_uuid(const struct mei_device *dev,
+					const uuid_le *uuid)
 {
-	int i;
+	struct mei_me_client *me_cl;
 
-	for (i = 0; i < dev->me_clients_num; ++i)
-		if (uuid_le_cmp(*uuid,
-				dev->me_clients[i].props.protocol_name) == 0)
-			return i;
+	list_for_each_entry(me_cl, &dev->me_clients, list)
+		if (uuid_le_cmp(*uuid, me_cl->props.protocol_name) == 0)
+			return me_cl;
 
-	return -ENOENT;
+	return NULL;
 }
 
-
 /**
- * mei_me_cl_by_id return index to me_clients for client_id
+ * mei_me_cl_by_id - locate me client by client id
  *
  * @dev: the device structure
  * @client_id: me client id
  *
  * Locking: called under "dev->device_lock" lock
  *
- * returns index on success, -ENOENT on failure.
+ * Return: me client or NULL if not found
  */
+struct mei_me_client *mei_me_cl_by_id(struct mei_device *dev, u8 client_id)
+{
+
+	struct mei_me_client *me_cl;
 
-int mei_me_cl_by_id(struct mei_device *dev, u8 client_id)
+	list_for_each_entry(me_cl, &dev->me_clients, list)
+		if (me_cl->client_id == client_id)
+			return me_cl;
+	return NULL;
+}
+
+/**
+ * mei_me_cl_by_uuid_id - locate me client by client id and uuid
+ *
+ * @dev: the device structure
+ * @uuid: me client uuid
+ * @client_id: me client id
+ *
+ * Locking: called under "dev->device_lock" lock
+ *
+ * Return: me client or NULL if not found
+ */
+struct mei_me_client *mei_me_cl_by_uuid_id(struct mei_device *dev,
+					   const uuid_le *uuid, u8 client_id)
 {
-	int i;
+	struct mei_me_client *me_cl;
 
-	for (i = 0; i < dev->me_clients_num; i++)
-		if (dev->me_clients[i].client_id == client_id)
-			return i;
+	list_for_each_entry(me_cl, &dev->me_clients, list)
+		if (uuid_le_cmp(*uuid, me_cl->props.protocol_name) == 0 &&
+		    me_cl->client_id == client_id)
+			return me_cl;
+	return NULL;
+}
 
-	return -ENOENT;
+/**
+ * mei_me_cl_remove - remove me client matching uuid and client_id
+ *
+ * @dev: the device structure
+ * @uuid: me client uuid
+ * @client_id: me client address
+ */
+void mei_me_cl_remove(struct mei_device *dev, const uuid_le *uuid, u8 client_id)
+{
+	struct mei_me_client *me_cl, *next;
+
+	list_for_each_entry_safe(me_cl, next, &dev->me_clients, list) {
+		if (uuid_le_cmp(*uuid, me_cl->props.protocol_name) == 0 &&
+		    me_cl->client_id == client_id) {
+			list_del(&me_cl->list);
+			kfree(me_cl);
+			break;
+		}
+	}
 }
 
 
@@ -77,7 +120,7 @@ int mei_me_cl_by_id(struct mei_device *dev, u8 client_id)
  * @cl1: host client 1
  * @cl2: host client 2
  *
- * returns true  - if the clients has same host and me ids
+ * Return: true  - if the clients has same host and me ids
  *         false - otherwise
  */
 static inline bool mei_cl_cmp_id(const struct mei_cl *cl1,
@@ -117,7 +160,7 @@ static void __mei_io_list_flush(struct mei_cl_cb *list,
  * @list:  An instance of our list structure
  * @cl: host client
  */
-static inline void mei_io_list_flush(struct mei_cl_cb *list, struct mei_cl *cl)
+void mei_io_list_flush(struct mei_cl_cb *list, struct mei_cl *cl)
 {
 	__mei_io_list_flush(list, cl, false);
 }
@@ -152,10 +195,10 @@ void mei_io_cb_free(struct mei_cl_cb *cb)
 /**
  * mei_io_cb_init - allocate and initialize io callback
  *
- * @cl - mei client
+ * @cl: mei client
  * @fp: pointer to file structure
  *
- * returns mei_cl_cb pointer or NULL;
+ * Return: mei_cl_cb pointer or NULL;
  */
 struct mei_cl_cb *mei_io_cb_init(struct mei_cl *cl, struct file *fp)
 {
@@ -179,7 +222,7 @@ struct mei_cl_cb *mei_io_cb_init(struct mei_cl *cl, struct file *fp)
  * @cb: io callback structure
  * @length: size of the buffer
  *
- * returns 0 on success
+ * Return: 0 on success
  *         -EINVAL if cb is NULL
  *         -ENOMEM if allocation failed
  */
@@ -203,7 +246,7 @@ int mei_io_cb_alloc_req_buf(struct mei_cl_cb *cb, size_t length)
  * @cb: io callback structure
  * @length: size of the buffer
  *
- * returns 0 on success
+ * Return: 0 on success
  *         -EINVAL if cb is NULL
  *         -ENOMEM if allocation failed
  */
@@ -228,6 +271,8 @@ int mei_io_cb_alloc_resp_buf(struct mei_cl_cb *cb, size_t length)
  * mei_cl_flush_queues - flushes queue lists belonging to cl.
  *
  * @cl: host client
+ *
+ * Return: 0 on success, -EINVAL if cl or cl->dev is NULL.
  */
 int mei_cl_flush_queues(struct mei_cl *cl)
 {
@@ -273,7 +318,7 @@ void mei_cl_init(struct mei_cl *cl, struct mei_device *dev)
  * mei_cl_allocate - allocates cl  structure and sets it up.
  *
  * @dev: mei device
- * returns  The allocated file or NULL on failure
+ * Return:  The allocated file or NULL on failure
  */
 struct mei_cl *mei_cl_allocate(struct mei_device *dev)
 {
@@ -293,7 +338,7 @@ struct mei_cl *mei_cl_allocate(struct mei_device *dev)
  *
  * @cl: host client
  *
- * returns cb on success, NULL on error
+ * Return: cb on success, NULL on error
  */
 struct mei_cl_cb *mei_cl_find_read_cb(struct mei_cl *cl)
 {
@@ -311,7 +356,7 @@ struct mei_cl_cb *mei_cl_find_read_cb(struct mei_cl *cl)
  * @cl - host client
  * @id - fixed host id or -1 for generic one
  *
- * returns 0 on success
+ * Return: 0 on success
  *	-EINVAL on incorrect values
  *	-ENONET if client not found
  */
@@ -331,13 +376,13 @@ int mei_cl_link(struct mei_cl *cl, int id)
 					MEI_CLIENTS_MAX);
 
 	if (id >= MEI_CLIENTS_MAX) {
-		dev_err(&dev->pdev->dev, "id exceeded %d", MEI_CLIENTS_MAX);
+		dev_err(dev->dev, "id exceeded %d", MEI_CLIENTS_MAX);
 		return -EMFILE;
 	}
 
 	open_handle_count = dev->open_handle_count + dev->iamthif_open_count;
 	if (open_handle_count >= MEI_MAX_OPEN_HANDLE_COUNT) {
-		dev_err(&dev->pdev->dev, "open_handle_count exceeded %d",
+		dev_err(dev->dev, "open_handle_count exceeded %d",
 			MEI_MAX_OPEN_HANDLE_COUNT);
 		return -EMFILE;
 	}
@@ -359,6 +404,8 @@ int mei_cl_link(struct mei_cl *cl, int id)
  * mei_cl_unlink - remove me_cl from the list
  *
  * @cl: host client
+ *
+ * Return: always 0
  */
 int mei_cl_unlink(struct mei_cl *cl)
 {
@@ -395,19 +442,19 @@ void mei_host_client_init(struct work_struct *work)
 {
 	struct mei_device *dev = container_of(work,
 					      struct mei_device, init_work);
-	struct mei_client_properties *client_props;
-	int i;
+	struct mei_me_client *me_cl;
+	struct mei_client_properties *props;
 
 	mutex_lock(&dev->device_lock);
 
-	for (i = 0; i < dev->me_clients_num; i++) {
-		client_props = &dev->me_clients[i].props;
+	list_for_each_entry(me_cl, &dev->me_clients, list) {
+		props = &me_cl->props;
 
-		if (!uuid_le_cmp(client_props->protocol_name, mei_amthif_guid))
+		if (!uuid_le_cmp(props->protocol_name, mei_amthif_guid))
 			mei_amthif_host_init(dev);
-		else if (!uuid_le_cmp(client_props->protocol_name, mei_wd_guid))
+		else if (!uuid_le_cmp(props->protocol_name, mei_wd_guid))
 			mei_wd_host_init(dev);
-		else if (!uuid_le_cmp(client_props->protocol_name, mei_nfc_guid))
+		else if (!uuid_le_cmp(props->protocol_name, mei_nfc_guid))
 			mei_nfc_host_init(dev);
 
 	}
@@ -417,27 +464,27 @@ void mei_host_client_init(struct work_struct *work)
 
 	mutex_unlock(&dev->device_lock);
 
-	pm_runtime_mark_last_busy(&dev->pdev->dev);
-	dev_dbg(&dev->pdev->dev, "rpm: autosuspend\n");
-	pm_runtime_autosuspend(&dev->pdev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	dev_dbg(dev->dev, "rpm: autosuspend\n");
+	pm_runtime_autosuspend(dev->dev);
 }
 
 /**
- * mei_hbuf_acquire: try to acquire host buffer
+ * mei_hbuf_acquire - try to acquire host buffer
  *
  * @dev: the device structure
- * returns true if host buffer was acquired
+ * Return: true if host buffer was acquired
  */
 bool mei_hbuf_acquire(struct mei_device *dev)
 {
 	if (mei_pg_state(dev) == MEI_PG_ON ||
 	    dev->pg_event == MEI_PG_EVENT_WAIT) {
-		dev_dbg(&dev->pdev->dev, "device is in pg\n");
+		dev_dbg(dev->dev, "device is in pg\n");
 		return false;
 	}
 
 	if (!dev->hbuf_is_ready) {
-		dev_dbg(&dev->pdev->dev, "hbuf is not ready\n");
+		dev_dbg(dev->dev, "hbuf is not ready\n");
 		return false;
 	}
 
@@ -453,7 +500,7 @@ bool mei_hbuf_acquire(struct mei_device *dev)
  *
  * Locking: called under "dev->device_lock" lock
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 int mei_cl_disconnect(struct mei_cl *cl)
 {
@@ -471,9 +518,9 @@ int mei_cl_disconnect(struct mei_cl *cl)
 	if (cl->state != MEI_FILE_DISCONNECTING)
 		return 0;
 
-	rets = pm_runtime_get(&dev->pdev->dev);
+	rets = pm_runtime_get(dev->dev);
 	if (rets < 0 && rets != -EINPROGRESS) {
-		pm_runtime_put_noidle(&dev->pdev->dev);
+		pm_runtime_put_noidle(dev->dev);
 		cl_err(dev, cl, "rpm: get failed %d\n", rets);
 		return rets;
 	}
@@ -484,7 +531,8 @@ int mei_cl_disconnect(struct mei_cl *cl)
 		goto free;
 	}
 
-	cb->fop_type = MEI_FOP_CLOSE;
+	cb->fop_type = MEI_FOP_DISCONNECT;
+
 	if (mei_hbuf_acquire(dev)) {
 		if (mei_hbm_cl_disconnect_req(dev, cl)) {
 			rets = -ENODEV;
@@ -501,7 +549,7 @@ int mei_cl_disconnect(struct mei_cl *cl)
 	}
 	mutex_unlock(&dev->device_lock);
 
-	wait_event_timeout(dev->wait_recvd_msg,
+	wait_event_timeout(cl->wait,
 			MEI_FILE_DISCONNECTED == cl->state,
 			mei_secs_to_jiffies(MEI_CL_CONNECT_TIMEOUT));
 
@@ -519,8 +567,8 @@ int mei_cl_disconnect(struct mei_cl *cl)
 	mei_io_list_flush(&dev->ctrl_wr_list, cl);
 free:
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
-	pm_runtime_mark_last_busy(&dev->pdev->dev);
-	pm_runtime_put_autosuspend(&dev->pdev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 
 	mei_io_cb_free(cb);
 	return rets;
@@ -533,7 +581,7 @@ free:
  *
  * @cl: private data of the file object
  *
- * returns true if other client is connected, false - otherwise.
+ * Return: true if other client is connected, false - otherwise.
  */
 bool mei_cl_is_other_connecting(struct mei_cl *cl)
 {
@@ -560,10 +608,11 @@ bool mei_cl_is_other_connecting(struct mei_cl *cl)
  * mei_cl_connect - connect host client to the me one
  *
  * @cl: host client
+ * @file: pointer to file structure
  *
  * Locking: called under "dev->device_lock" lock
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 int mei_cl_connect(struct mei_cl *cl, struct file *file)
 {
@@ -576,9 +625,9 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 
 	dev = cl->dev;
 
-	rets = pm_runtime_get(&dev->pdev->dev);
+	rets = pm_runtime_get(dev->dev);
 	if (rets < 0 && rets != -EINPROGRESS) {
-		pm_runtime_put_noidle(&dev->pdev->dev);
+		pm_runtime_put_noidle(dev->dev);
 		cl_err(dev, cl, "rpm: get failed %d\n", rets);
 		return rets;
 	}
@@ -601,11 +650,12 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 		cl->timer_count = MEI_CONNECT_TIMEOUT;
 		list_add_tail(&cb->list, &dev->ctrl_rd_list.list);
 	} else {
+		cl->state = MEI_FILE_INITIALIZING;
 		list_add_tail(&cb->list, &dev->ctrl_wr_list.list);
 	}
 
 	mutex_unlock(&dev->device_lock);
-	wait_event_timeout(dev->wait_recvd_msg,
+	wait_event_timeout(cl->wait,
 			(cl->state == MEI_FILE_CONNECTED ||
 			 cl->state == MEI_FILE_DISCONNECTED),
 			mei_secs_to_jiffies(MEI_CL_CONNECT_TIMEOUT));
@@ -625,8 +675,8 @@ int mei_cl_connect(struct mei_cl *cl, struct file *file)
 
 out:
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
-	pm_runtime_mark_last_busy(&dev->pdev->dev);
-	pm_runtime_put_autosuspend(&dev->pdev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 
 	mei_io_cb_free(cb);
 	return rets;
@@ -637,7 +687,7 @@ out:
  *
  * @cl: private data of the file object
  *
- * returns 1 if mei_flow_ctrl_creds >0, 0 - otherwise.
+ * Return: 1 if mei_flow_ctrl_creds >0, 0 - otherwise.
  *	-ENOENT if mei_cl is not present
  *	-EINVAL if single_recv_buf == 0
  */
@@ -645,26 +695,21 @@ int mei_cl_flow_ctrl_creds(struct mei_cl *cl)
 {
 	struct mei_device *dev;
 	struct mei_me_client *me_cl;
-	int id;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -EINVAL;
 
 	dev = cl->dev;
 
-	if (!dev->me_clients_num)
-		return 0;
-
 	if (cl->mei_flow_ctrl_creds > 0)
 		return 1;
 
-	id = mei_me_cl_by_id(dev, cl->me_client_id);
-	if (id < 0) {
+	me_cl = mei_me_cl_by_id(dev, cl->me_client_id);
+	if (!me_cl) {
 		cl_err(dev, cl, "no such me client %d\n", cl->me_client_id);
-		return id;
+		return -ENOENT;
 	}
 
-	me_cl = &dev->me_clients[id];
 	if (me_cl->mei_flow_ctrl_creds) {
 		if (WARN_ON(me_cl->props.single_recv_buf == 0))
 			return -EINVAL;
@@ -678,7 +723,7 @@ int mei_cl_flow_ctrl_creds(struct mei_cl *cl)
  *
  * @cl: private data of the file object
  *
- * @returns
+ * Return:
  *	0 on success
  *	-ENOENT when me client is not found
  *	-EINVAL when ctrl credits are <= 0
@@ -687,21 +732,19 @@ int mei_cl_flow_ctrl_reduce(struct mei_cl *cl)
 {
 	struct mei_device *dev;
 	struct mei_me_client *me_cl;
-	int id;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -EINVAL;
 
 	dev = cl->dev;
 
-	id = mei_me_cl_by_id(dev, cl->me_client_id);
-	if (id < 0) {
+	me_cl = mei_me_cl_by_id(dev, cl->me_client_id);
+	if (!me_cl) {
 		cl_err(dev, cl, "no such me client %d\n", cl->me_client_id);
-		return id;
+		return -ENOENT;
 	}
 
-	me_cl = &dev->me_clients[id];
-	if (me_cl->props.single_recv_buf != 0) {
+	if (me_cl->props.single_recv_buf) {
 		if (WARN_ON(me_cl->mei_flow_ctrl_creds <= 0))
 			return -EINVAL;
 		me_cl->mei_flow_ctrl_creds--;
@@ -717,15 +760,16 @@ int mei_cl_flow_ctrl_reduce(struct mei_cl *cl)
  * mei_cl_read_start - the start read client message function.
  *
  * @cl: host client
+ * @length: number of bytes to read
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 int mei_cl_read_start(struct mei_cl *cl, size_t length)
 {
 	struct mei_device *dev;
 	struct mei_cl_cb *cb;
+	struct mei_me_client *me_cl;
 	int rets;
-	int i;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -ENODEV;
@@ -739,15 +783,15 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length)
 		cl_dbg(dev, cl, "read is pending.\n");
 		return -EBUSY;
 	}
-	i = mei_me_cl_by_id(dev, cl->me_client_id);
-	if (i < 0) {
+	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
+	if (!me_cl) {
 		cl_err(dev, cl, "no such me client %d\n", cl->me_client_id);
 		return  -ENOTTY;
 	}
 
-	rets = pm_runtime_get(&dev->pdev->dev);
+	rets = pm_runtime_get(dev->dev);
 	if (rets < 0 && rets != -EINPROGRESS) {
-		pm_runtime_put_noidle(&dev->pdev->dev);
+		pm_runtime_put_noidle(dev->dev);
 		cl_err(dev, cl, "rpm: get failed %d\n", rets);
 		return rets;
 	}
@@ -759,7 +803,7 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length)
 	}
 
 	/* always allocate at least client max message */
-	length = max_t(size_t, length, dev->me_clients[i].props.max_msg_length);
+	length = max_t(size_t, length, me_cl->props.max_msg_length);
 	rets = mei_io_cb_alloc_resp_buf(cb, length);
 	if (rets)
 		goto out;
@@ -779,8 +823,8 @@ int mei_cl_read_start(struct mei_cl *cl, size_t length)
 
 out:
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
-	pm_runtime_mark_last_busy(&dev->pdev->dev);
-	pm_runtime_put_autosuspend(&dev->pdev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 
 	if (rets)
 		mei_io_cb_free(cb);
@@ -796,7 +840,7 @@ out:
  * @cb: callback block.
  * @cmpl_list: complete list.
  *
- * returns 0, OK; otherwise error.
+ * Return: 0, OK; otherwise error.
  */
 int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 		     struct mei_cl_cb *cmpl_list)
@@ -873,12 +917,13 @@ int mei_cl_irq_write(struct mei_cl *cl, struct mei_cl_cb *cb,
 
 /**
  * mei_cl_write - submit a write cb to mei device
-	assumes device_lock is locked
+ *	assumes device_lock is locked
  *
  * @cl: host client
- * @cl: write callback with filled data
+ * @cb: write callback with filled data
+ * @blocking: block until completed
  *
- * returns number of bytes sent on success, <0 on failure.
+ * Return: number of bytes sent on success, <0 on failure.
  */
 int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
 {
@@ -899,11 +944,11 @@ int mei_cl_write(struct mei_cl *cl, struct mei_cl_cb *cb, bool blocking)
 
 	buf = &cb->request_buffer;
 
-	cl_dbg(dev, cl, "mei_cl_write %d\n", buf->size);
+	cl_dbg(dev, cl, "size=%d\n", buf->size);
 
-	rets = pm_runtime_get(&dev->pdev->dev);
+	rets = pm_runtime_get(dev->dev);
 	if (rets < 0 && rets != -EINPROGRESS) {
-		pm_runtime_put_noidle(&dev->pdev->dev);
+		pm_runtime_put_noidle(dev->dev);
 		cl_err(dev, cl, "rpm: get failed %d\n", rets);
 		return rets;
 	}
@@ -978,8 +1023,8 @@ out:
 	rets = buf->size;
 err:
 	cl_dbg(dev, cl, "rpm: autosuspend\n");
-	pm_runtime_mark_last_busy(&dev->pdev->dev);
-	pm_runtime_put_autosuspend(&dev->pdev->dev);
+	pm_runtime_mark_last_busy(dev->dev);
+	pm_runtime_put_autosuspend(dev->dev);
 
 	return rets;
 }
@@ -1015,7 +1060,7 @@ void mei_cl_complete(struct mei_cl *cl, struct mei_cl_cb *cb)
 /**
  * mei_cl_all_disconnect - disconnect forcefully all connected clients
  *
- * @dev - mei device
+ * @dev: mei device
  */
 
 void mei_cl_all_disconnect(struct mei_device *dev)
@@ -1033,11 +1078,12 @@ void mei_cl_all_disconnect(struct mei_device *dev)
 /**
  * mei_cl_all_wakeup  - wake up all readers and writers they can be interrupted
  *
- * @dev  - mei device
+ * @dev: mei device
  */
 void mei_cl_all_wakeup(struct mei_device *dev)
 {
 	struct mei_cl *cl;
+
 	list_for_each_entry(cl, &dev->file_list, link) {
 		if (waitqueue_active(&cl->rx_wait)) {
 			cl_dbg(dev, cl, "Waking up reading client!\n");
@@ -1052,8 +1098,8 @@ void mei_cl_all_wakeup(struct mei_device *dev)
 
 /**
  * mei_cl_all_write_clear - clear all pending writes
-
- * @dev - mei device
+ *
+ * @dev: mei device
  */
 void mei_cl_all_write_clear(struct mei_device *dev)
 {
diff --git a/drivers/misc/mei/client.h b/drivers/misc/mei/client.h
index 96d5de0389f9..d9d0c1525259 100644
--- a/drivers/misc/mei/client.h
+++ b/drivers/misc/mei/client.h
@@ -24,8 +24,15 @@
 
 #include "mei_dev.h"
 
-int mei_me_cl_by_uuid(const struct mei_device *dev, const uuid_le *cuuid);
-int mei_me_cl_by_id(struct mei_device *dev, u8 client_id);
+struct mei_me_client *mei_me_cl_by_uuid(const struct mei_device *dev,
+					const uuid_le *cuuid);
+struct mei_me_client *mei_me_cl_by_id(struct mei_device *dev, u8 client_id);
+
+struct mei_me_client *mei_me_cl_by_uuid_id(struct mei_device *dev,
+					   const uuid_le *uuid, u8 client_id);
+
+void mei_me_cl_remove(struct mei_device *dev,
+		      const uuid_le *uuid, u8 client_id);
 
 /*
  * MEI IO Functions
@@ -45,6 +52,8 @@ static inline void mei_io_list_init(struct mei_cl_cb *list)
 {
 	INIT_LIST_HEAD(&list->list);
 }
+void mei_io_list_flush(struct mei_cl_cb *list, struct mei_cl *cl);
+
 /*
  * MEI Host Client Functions
  */
@@ -101,9 +110,9 @@ void mei_cl_all_write_clear(struct mei_device *dev);
 #define MEI_CL_PRM(cl) (cl)->host_client_id, (cl)->me_client_id
 
 #define cl_dbg(dev, cl, format, arg...) \
-	dev_dbg(&(dev)->pdev->dev, MEI_CL_FMT format, MEI_CL_PRM(cl), ##arg)
+	dev_dbg((dev)->dev, MEI_CL_FMT format, MEI_CL_PRM(cl), ##arg)
 
 #define cl_err(dev, cl, format, arg...) \
-	dev_err(&(dev)->pdev->dev, MEI_CL_FMT format, MEI_CL_PRM(cl), ##arg)
+	dev_err((dev)->dev, MEI_CL_FMT format, MEI_CL_PRM(cl), ##arg)
 
 #endif /* _MEI_CLIENT_H_ */
diff --git a/drivers/misc/mei/debugfs.c b/drivers/misc/mei/debugfs.c
index ced5b777c70f..ce1566715f80 100644
--- a/drivers/misc/mei/debugfs.c
+++ b/drivers/misc/mei/debugfs.c
@@ -17,7 +17,6 @@
 #include <linux/kernel.h>
 #include <linux/device.h>
 #include <linux/debugfs.h>
-#include <linux/pci.h>
 
 #include <linux/mei.h>
 
@@ -28,39 +27,47 @@ static ssize_t mei_dbgfs_read_meclients(struct file *fp, char __user *ubuf,
 					size_t cnt, loff_t *ppos)
 {
 	struct mei_device *dev = fp->private_data;
-	struct mei_me_client *cl;
-	const size_t bufsz = 1024;
-	char *buf = kzalloc(bufsz, GFP_KERNEL);
-	int i;
+	struct mei_me_client *me_cl;
+	size_t bufsz = 1;
+	char *buf;
+	int i = 0;
 	int pos = 0;
 	int ret;
 
-	if  (!buf)
-		return -ENOMEM;
-
-	pos += scnprintf(buf + pos, bufsz - pos,
-			"  |id|addr|         UUID                       |con|msg len|\n");
+#define HDR "  |id|addr|         UUID                       |con|msg len|sb|\n"
 
 	mutex_lock(&dev->device_lock);
 
+	list_for_each_entry(me_cl, &dev->me_clients, list)
+		bufsz++;
+
+	bufsz *= sizeof(HDR) + 1;
+	buf = kzalloc(bufsz, GFP_KERNEL);
+	if (!buf) {
+		mutex_unlock(&dev->device_lock);
+		return -ENOMEM;
+	}
+
+	pos += scnprintf(buf + pos, bufsz - pos, HDR);
+
 	/*  if the driver is not enabled the list won't be consistent */
 	if (dev->dev_state != MEI_DEV_ENABLED)
 		goto out;
 
-	for (i = 0; i < dev->me_clients_num; i++) {
-		cl = &dev->me_clients[i];
+	list_for_each_entry(me_cl, &dev->me_clients, list) {
 
 		/* skip me clients that cannot be connected */
-		if (cl->props.max_number_of_connections == 0)
+		if (me_cl->props.max_number_of_connections == 0)
 			continue;
 
 		pos += scnprintf(buf + pos, bufsz - pos,
-			"%2d|%2d|%4d|%pUl|%3d|%7d|\n",
-			i, cl->client_id,
-			cl->props.fixed_address,
-			&cl->props.protocol_name,
-			cl->props.max_number_of_connections,
-			cl->props.max_msg_length);
+			"%2d|%2d|%4d|%pUl|%3d|%7d|%2d|\n",
+			i++, me_cl->client_id,
+			me_cl->props.fixed_address,
+			&me_cl->props.protocol_name,
+			me_cl->props.max_number_of_connections,
+			me_cl->props.max_msg_length,
+			me_cl->props.single_recv_buf);
 	}
 out:
 	mutex_unlock(&dev->device_lock);
@@ -98,7 +105,7 @@ static ssize_t mei_dbgfs_read_active(struct file *fp, char __user *ubuf,
 
 	mutex_lock(&dev->device_lock);
 
-	/*  if the driver is not enabled the list won't b consitent */
+	/*  if the driver is not enabled the list won't be consistent */
 	if (dev->dev_state != MEI_DEV_ENABLED)
 		goto out;
 
@@ -135,8 +142,13 @@ static ssize_t mei_dbgfs_read_devstate(struct file *fp, char __user *ubuf,
 	if  (!buf)
 		return -ENOMEM;
 
-	pos += scnprintf(buf + pos, bufsz - pos, "%s\n",
+	pos += scnprintf(buf + pos, bufsz - pos, "dev: %s\n",
 			mei_dev_state_str(dev->dev_state));
+	pos += scnprintf(buf + pos, bufsz - pos, "hbm: %s\n",
+			mei_hbm_state_str(dev->hbm_state));
+	pos += scnprintf(buf + pos, bufsz - pos, "pg:  %s, %s\n",
+			mei_pg_is_enabled(dev) ? "ENABLED" : "DISABLED",
+			mei_pg_state_str(mei_pg_state(dev)));
 	ret = simple_read_from_buffer(ubuf, cnt, ppos, buf, pos);
 	kfree(buf);
 	return ret;
@@ -149,7 +161,8 @@ static const struct file_operations mei_dbgfs_fops_devstate = {
 
 /**
  * mei_dbgfs_deregister - Remove the debugfs files and directories
- * @mei - pointer to mei device private data
+ *
+ * @dev: the mei device structure
  */
 void mei_dbgfs_deregister(struct mei_device *dev)
 {
@@ -160,12 +173,17 @@ void mei_dbgfs_deregister(struct mei_device *dev)
 }
 
 /**
- * Add the debugfs files
+ * mei_dbgfs_register - Add the debugfs files
  *
+ * @dev: the mei device structure
+ * @name: the mei device name
+ *
+ * Return: 0 on success, <0 on failure.
  */
 int mei_dbgfs_register(struct mei_device *dev, const char *name)
 {
 	struct dentry *dir, *f;
+
 	dir = debugfs_create_dir(name, NULL);
 	if (!dir)
 		return -ENOMEM;
@@ -173,19 +191,19 @@ int mei_dbgfs_register(struct mei_device *dev, const char *name)
 	f = debugfs_create_file("meclients", S_IRUSR, dir,
 				dev, &mei_dbgfs_fops_meclients);
 	if (!f) {
-		dev_err(&dev->pdev->dev, "meclients: registration failed\n");
+		dev_err(dev->dev, "meclients: registration failed\n");
 		goto err;
 	}
 	f = debugfs_create_file("active", S_IRUSR, dir,
 				dev, &mei_dbgfs_fops_active);
 	if (!f) {
-		dev_err(&dev->pdev->dev, "meclients: registration failed\n");
+		dev_err(dev->dev, "meclients: registration failed\n");
 		goto err;
 	}
 	f = debugfs_create_file("devstate", S_IRUSR, dir,
 				dev, &mei_dbgfs_fops_devstate);
 	if (!f) {
-		dev_err(&dev->pdev->dev, "devstate: registration failed\n");
+		dev_err(dev->dev, "devstate: registration failed\n");
 		goto err;
 	}
 	dev->dbgfs_dir = dir;
diff --git a/drivers/misc/mei/hbm.c b/drivers/misc/mei/hbm.c
index 804106209d76..49a2653d91a5 100644
--- a/drivers/misc/mei/hbm.c
+++ b/drivers/misc/mei/hbm.c
@@ -15,16 +15,34 @@
  */
 
 #include <linux/export.h>
-#include <linux/pci.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
-#include <linux/mei.h>
 #include <linux/pm_runtime.h>
+#include <linux/slab.h>
+
+#include <linux/mei.h>
 
 #include "mei_dev.h"
 #include "hbm.h"
 #include "client.h"
 
+static const char *mei_hbm_status_str(enum mei_hbm_status status)
+{
+#define MEI_HBM_STATUS(status) case MEI_HBMS_##status: return #status
+	switch (status) {
+	MEI_HBM_STATUS(SUCCESS);
+	MEI_HBM_STATUS(CLIENT_NOT_FOUND);
+	MEI_HBM_STATUS(ALREADY_EXISTS);
+	MEI_HBM_STATUS(REJECTED);
+	MEI_HBM_STATUS(INVALID_PARAMETER);
+	MEI_HBM_STATUS(NOT_ALLOWED);
+	MEI_HBM_STATUS(ALREADY_STARTED);
+	MEI_HBM_STATUS(NOT_STARTED);
+	default: return "unknown";
+	}
+#undef MEI_HBM_STATUS
+};
+
 static const char *mei_cl_conn_status_str(enum mei_cl_connect_status status)
 {
 #define MEI_CL_CS(status) case MEI_CL_CONN_##status: return #status
@@ -39,13 +57,29 @@ static const char *mei_cl_conn_status_str(enum mei_cl_connect_status status)
 #undef MEI_CL_CCS
 }
 
+const char *mei_hbm_state_str(enum mei_hbm_state state)
+{
+#define MEI_HBM_STATE(state) case MEI_HBM_##state: return #state
+	switch (state) {
+	MEI_HBM_STATE(IDLE);
+	MEI_HBM_STATE(STARTING);
+	MEI_HBM_STATE(STARTED);
+	MEI_HBM_STATE(ENUM_CLIENTS);
+	MEI_HBM_STATE(CLIENT_PROPERTIES);
+	MEI_HBM_STATE(STOPPED);
+	default:
+		return "unknown";
+	}
+#undef MEI_HBM_STATE
+}
+
 /**
  * mei_cl_conn_status_to_errno - convert client connect response
  * status to error code
  *
  * @status: client connect response status
  *
- * returns corresponding error code
+ * Return: corresponding error code
  */
 static int mei_cl_conn_status_to_errno(enum mei_cl_connect_status status)
 {
@@ -71,60 +105,54 @@ void mei_hbm_idle(struct mei_device *dev)
 }
 
 /**
- * mei_hbm_reset - reset hbm counters and book keeping data structurs
+ * mei_me_cl_remove_all - remove all me clients
  *
  * @dev: the device structure
  */
-void mei_hbm_reset(struct mei_device *dev)
+static void mei_me_cl_remove_all(struct mei_device *dev)
 {
-	dev->me_clients_num = 0;
-	dev->me_client_presentation_num = 0;
-	dev->me_client_index = 0;
-
-	kfree(dev->me_clients);
-	dev->me_clients = NULL;
+	struct mei_me_client *me_cl, *next;
 
-	mei_hbm_idle(dev);
+	list_for_each_entry_safe(me_cl, next, &dev->me_clients, list) {
+			list_del(&me_cl->list);
+			kfree(me_cl);
+	}
 }
 
 /**
- * mei_hbm_me_cl_allocate - allocates storage for me clients
+ * mei_hbm_reset - reset hbm counters and book keeping data structurs
  *
  * @dev: the device structure
- *
- * returns 0 on success -ENOMEM on allocation failure
  */
-static int mei_hbm_me_cl_allocate(struct mei_device *dev)
+void mei_hbm_reset(struct mei_device *dev)
 {
-	struct mei_me_client *clients;
-	int b;
+	dev->me_client_index = 0;
 
-	mei_hbm_reset(dev);
+	mei_me_cl_remove_all(dev);
 
-	/* count how many ME clients we have */
-	for_each_set_bit(b, dev->me_clients_map, MEI_CLIENTS_MAX)
-		dev->me_clients_num++;
+	mei_hbm_idle(dev);
+}
 
-	if (dev->me_clients_num == 0)
-		return 0;
+/**
+ * mei_hbm_hdr - construct hbm header
+ *
+ * @hdr: hbm header
+ * @length: payload length
+ */
 
-	dev_dbg(&dev->pdev->dev, "memory allocation for ME clients size=%ld.\n",
-		dev->me_clients_num * sizeof(struct mei_me_client));
-	/* allocate storage for ME clients representation */
-	clients = kcalloc(dev->me_clients_num,
-			sizeof(struct mei_me_client), GFP_KERNEL);
-	if (!clients) {
-		dev_err(&dev->pdev->dev, "memory allocation for ME clients failed.\n");
-		return -ENOMEM;
-	}
-	dev->me_clients = clients;
-	return 0;
+static inline void mei_hbm_hdr(struct mei_msg_hdr *hdr, size_t length)
+{
+	hdr->host_addr = 0;
+	hdr->me_addr = 0;
+	hdr->length = length;
+	hdr->msg_complete = 1;
+	hdr->reserved = 0;
 }
 
 /**
  * mei_hbm_cl_hdr - construct client hbm header
  *
- * @cl: - client
+ * @cl: client
  * @hbm_cmd: host bus message command
  * @buf: buffer for cl header
  * @len: buffer length
@@ -142,38 +170,87 @@ void mei_hbm_cl_hdr(struct mei_cl *cl, u8 hbm_cmd, void *buf, size_t len)
 }
 
 /**
- * mei_hbm_cl_addr_equal - tells if they have the same address
+ * mei_hbm_cl_write - write simple hbm client message
  *
- * @cl: - client
- * @buf: buffer with cl header
+ * @dev: the device structure
+ * @cl: client
+ * @hbm_cmd: host bus message command
+ * @len: buffer length
  *
- * returns true if addresses are the same
+ * Return: 0 on success, <0 on failure.
  */
 static inline
-bool mei_hbm_cl_addr_equal(struct mei_cl *cl, void *buf)
+int mei_hbm_cl_write(struct mei_device *dev,
+		     struct mei_cl *cl, u8 hbm_cmd, size_t len)
+{
+	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
+
+	mei_hbm_hdr(mei_hdr, len);
+	mei_hbm_cl_hdr(cl, hbm_cmd, dev->wr_msg.data, len);
+
+	return mei_write_message(dev, mei_hdr, dev->wr_msg.data);
+}
+
+/**
+ * mei_hbm_cl_addr_equal - check if the client's and
+ *	the message address match
+ *
+ * @cl: client
+ * @cmd: hbm client message
+ *
+ * Return: true if addresses are the same
+ */
+static inline
+bool mei_hbm_cl_addr_equal(struct mei_cl *cl, struct mei_hbm_cl_cmd *cmd)
 {
-	struct mei_hbm_cl_cmd *cmd = buf;
 	return cl->host_client_id == cmd->host_addr &&
 		cl->me_client_id == cmd->me_addr;
 }
 
+/**
+ * mei_hbm_cl_find_by_cmd - find recipient client
+ *
+ * @dev: the device structure
+ * @buf: a buffer with hbm cl command
+ *
+ * Return: the recipient client or NULL if not found
+ */
+static inline
+struct mei_cl *mei_hbm_cl_find_by_cmd(struct mei_device *dev, void *buf)
+{
+	struct mei_hbm_cl_cmd *cmd = (struct mei_hbm_cl_cmd *)buf;
+	struct mei_cl *cl;
+
+	list_for_each_entry(cl, &dev->file_list, link)
+		if (mei_hbm_cl_addr_equal(cl, cmd))
+			return cl;
+	return NULL;
+}
+
 
+/**
+ * mei_hbm_start_wait - wait for start response message.
+ *
+ * @dev: the device structure
+ *
+ * Return: 0 on success and < 0 on failure
+ */
 int mei_hbm_start_wait(struct mei_device *dev)
 {
 	int ret;
-	if (dev->hbm_state > MEI_HBM_START)
+
+	if (dev->hbm_state > MEI_HBM_STARTING)
 		return 0;
 
 	mutex_unlock(&dev->device_lock);
-	ret = wait_event_interruptible_timeout(dev->wait_recvd_msg,
-			dev->hbm_state == MEI_HBM_IDLE ||
-			dev->hbm_state >= MEI_HBM_STARTED,
+	ret = wait_event_timeout(dev->wait_hbm_start,
+			dev->hbm_state != MEI_HBM_STARTING,
 			mei_secs_to_jiffies(MEI_HBM_TIMEOUT));
 	mutex_lock(&dev->device_lock);
 
-	if (ret <= 0 && (dev->hbm_state <= MEI_HBM_START)) {
+	if (ret == 0 && (dev->hbm_state <= MEI_HBM_STARTING)) {
 		dev->hbm_state = MEI_HBM_IDLE;
-		dev_err(&dev->pdev->dev, "waiting for mei start failed\n");
+		dev_err(dev->dev, "waiting for mei start failed\n");
 		return -ETIME;
 	}
 	return 0;
@@ -184,7 +261,7 @@ int mei_hbm_start_wait(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns 0 on success and < 0 on failure
+ * Return: 0 on success and < 0 on failure
  */
 int mei_hbm_start_req(struct mei_device *dev)
 {
@@ -193,6 +270,8 @@ int mei_hbm_start_req(struct mei_device *dev)
 	const size_t len = sizeof(struct hbm_host_version_request);
 	int ret;
 
+	mei_hbm_reset(dev);
+
 	mei_hbm_hdr(mei_hdr, len);
 
 	/* host start message */
@@ -205,12 +284,12 @@ int mei_hbm_start_req(struct mei_device *dev)
 	dev->hbm_state = MEI_HBM_IDLE;
 	ret = mei_write_message(dev, mei_hdr, dev->wr_msg.data);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "version message write failed: ret = %d\n",
+		dev_err(dev->dev, "version message write failed: ret = %d\n",
 			ret);
 		return ret;
 	}
 
-	dev->hbm_state = MEI_HBM_START;
+	dev->hbm_state = MEI_HBM_STARTING;
 	dev->init_clients_timer = MEI_CLIENTS_INIT_TIMEOUT;
 	return 0;
 }
@@ -220,7 +299,7 @@ int mei_hbm_start_req(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns 0 on success and < 0 on failure
+ * Return: 0 on success and < 0 on failure
  */
 static int mei_hbm_enum_clients_req(struct mei_device *dev)
 {
@@ -238,7 +317,7 @@ static int mei_hbm_enum_clients_req(struct mei_device *dev)
 
 	ret = mei_write_message(dev, mei_hdr, dev->wr_msg.data);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "enumeration request write failed: ret = %d.\n",
+		dev_err(dev->dev, "enumeration request write failed: ret = %d.\n",
 			ret);
 		return ret;
 	}
@@ -247,12 +326,38 @@ static int mei_hbm_enum_clients_req(struct mei_device *dev)
 	return 0;
 }
 
+/*
+ * mei_hbm_me_cl_add - add new me client to the list
+ *
+ * @dev: the device structure
+ * @res: hbm property response
+ *
+ * Return: 0 on success and -ENOMEM on allocation failure
+ */
+
+static int mei_hbm_me_cl_add(struct mei_device *dev,
+			     struct hbm_props_response *res)
+{
+	struct mei_me_client *me_cl;
+
+	me_cl = kzalloc(sizeof(struct mei_me_client), GFP_KERNEL);
+	if (!me_cl)
+		return -ENOMEM;
+
+	me_cl->props = res->client_properties;
+	me_cl->client_id = res->me_addr;
+	me_cl->mei_flow_ctrl_creds = 0;
+
+	list_add(&me_cl->list, &dev->me_clients);
+	return 0;
+}
+
 /**
  * mei_hbm_prop_req - request property for a single client
  *
  * @dev: the device structure
  *
- * returns 0 on success and < 0 on failure
+ * Return: 0 on success and < 0 on failure
  */
 
 static int mei_hbm_prop_req(struct mei_device *dev)
@@ -262,11 +367,8 @@ static int mei_hbm_prop_req(struct mei_device *dev)
 	struct hbm_props_request *prop_req;
 	const size_t len = sizeof(struct hbm_props_request);
 	unsigned long next_client_index;
-	unsigned long client_num;
 	int ret;
 
-	client_num = dev->me_client_presentation_num;
-
 	next_client_index = find_next_bit(dev->me_clients_map, MEI_CLIENTS_MAX,
 					  dev->me_client_index);
 
@@ -278,21 +380,17 @@ static int mei_hbm_prop_req(struct mei_device *dev)
 		return 0;
 	}
 
-	dev->me_clients[client_num].client_id = next_client_index;
-	dev->me_clients[client_num].mei_flow_ctrl_creds = 0;
-
 	mei_hbm_hdr(mei_hdr, len);
 	prop_req = (struct hbm_props_request *)dev->wr_msg.data;
 
 	memset(prop_req, 0, sizeof(struct hbm_props_request));
 
-
 	prop_req->hbm_cmd = HOST_CLIENT_PROPERTIES_REQ_CMD;
-	prop_req->address = next_client_index;
+	prop_req->me_addr = next_client_index;
 
 	ret = mei_write_message(dev, mei_hdr, dev->wr_msg.data);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "properties request write failed: ret = %d\n",
+		dev_err(dev->dev, "properties request write failed: ret = %d\n",
 			ret);
 		return ret;
 	}
@@ -309,7 +407,8 @@ static int mei_hbm_prop_req(struct mei_device *dev)
  * @dev: the device structure
  * @pg_cmd: the pg command code
  *
- * This function returns -EIO on write failure
+ * Return: -EIO on write failure
+ *         -EOPNOTSUPP if the operation is not supported by the protocol
  */
 int mei_hbm_pg(struct mei_device *dev, u8 pg_cmd)
 {
@@ -318,6 +417,9 @@ int mei_hbm_pg(struct mei_device *dev, u8 pg_cmd)
 	const size_t len = sizeof(struct hbm_power_gate);
 	int ret;
 
+	if (!dev->hbm_f_pg_supported)
+		return -EOPNOTSUPP;
+
 	mei_hbm_hdr(mei_hdr, len);
 
 	req = (struct hbm_power_gate *)dev->wr_msg.data;
@@ -326,7 +428,7 @@ int mei_hbm_pg(struct mei_device *dev, u8 pg_cmd)
 
 	ret = mei_write_message(dev, mei_hdr, dev->wr_msg.data);
 	if (ret)
-		dev_err(&dev->pdev->dev, "power gate command write failed.\n");
+		dev_err(dev->dev, "power gate command write failed.\n");
 	return ret;
 }
 EXPORT_SYMBOL_GPL(mei_hbm_pg);
@@ -334,10 +436,9 @@ EXPORT_SYMBOL_GPL(mei_hbm_pg);
 /**
  * mei_hbm_stop_req - send stop request message
  *
- * @dev - mei device
- * @cl: client info
+ * @dev: mei device
  *
- * This function returns -EIO on write failure
+ * Return: -EIO on write failure
  */
 static int mei_hbm_stop_req(struct mei_device *dev)
 {
@@ -361,19 +462,14 @@ static int mei_hbm_stop_req(struct mei_device *dev)
  * @dev: the device structure
  * @cl: client info
  *
- * This function returns -EIO on write failure
+ * Return: -EIO on write failure
  */
 int mei_hbm_cl_flow_control_req(struct mei_device *dev, struct mei_cl *cl)
 {
-	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
 	const size_t len = sizeof(struct hbm_flow_control);
 
-	mei_hbm_hdr(mei_hdr, len);
-	mei_hbm_cl_hdr(cl, MEI_FLOW_CONTROL_CMD, dev->wr_msg.data, len);
-
 	cl_dbg(dev, cl, "sending flow control\n");
-
-	return mei_write_message(dev, mei_hdr, dev->wr_msg.data);
+	return mei_hbm_cl_write(dev, cl, MEI_FLOW_CONTROL_CMD, len);
 }
 
 /**
@@ -382,31 +478,26 @@ int mei_hbm_cl_flow_control_req(struct mei_device *dev, struct mei_cl *cl)
  * @dev: the device structure
  * @flow: flow control.
  *
- * return 0 on success, < 0 otherwise
+ * Return: 0 on success, < 0 otherwise
  */
 static int mei_hbm_add_single_flow_creds(struct mei_device *dev,
 				  struct hbm_flow_control *flow)
 {
 	struct mei_me_client *me_cl;
-	int id;
 
-	id = mei_me_cl_by_id(dev, flow->me_addr);
-	if (id < 0) {
-		dev_err(&dev->pdev->dev, "no such me client %d\n",
+	me_cl = mei_me_cl_by_id(dev, flow->me_addr);
+	if (!me_cl) {
+		dev_err(dev->dev, "no such me client %d\n",
 			flow->me_addr);
-		return id;
+		return -ENOENT;
 	}
 
-	me_cl = &dev->me_clients[id];
-	if (me_cl->props.single_recv_buf) {
-		me_cl->mei_flow_ctrl_creds++;
-		dev_dbg(&dev->pdev->dev, "recv flow ctrl msg ME %d (single).\n",
-		    flow->me_addr);
-		dev_dbg(&dev->pdev->dev, "flow control credentials =%d.\n",
-		    me_cl->mei_flow_ctrl_creds);
-	} else {
-		BUG();	/* error in flow control */
-	}
+	if (WARN_ON(me_cl->props.single_recv_buf == 0))
+		return -EINVAL;
+
+	me_cl->mei_flow_ctrl_creds++;
+	dev_dbg(dev->dev, "recv flow ctrl msg ME %d (single) creds = %d.\n",
+	    flow->me_addr, me_cl->mei_flow_ctrl_creds);
 
 	return 0;
 }
@@ -418,7 +509,7 @@ static int mei_hbm_add_single_flow_creds(struct mei_device *dev,
  * @flow_control: flow control response bus message
  */
 static void mei_hbm_cl_flow_control_res(struct mei_device *dev,
-		struct hbm_flow_control *flow_control)
+					struct hbm_flow_control *flow_control)
 {
 	struct mei_cl *cl;
 
@@ -428,16 +519,11 @@ static void mei_hbm_cl_flow_control_res(struct mei_device *dev,
 		return;
 	}
 
-	/* normal connection */
-	list_for_each_entry(cl, &dev->file_list, link) {
-		if (mei_hbm_cl_addr_equal(cl, flow_control)) {
-			cl->mei_flow_ctrl_creds++;
-			dev_dbg(&dev->pdev->dev, "flow ctrl msg for host %d ME %d.\n",
-				flow_control->host_addr, flow_control->me_addr);
-			dev_dbg(&dev->pdev->dev, "flow control credentials = %d.\n",
-				    cl->mei_flow_ctrl_creds);
-				break;
-		}
+	cl = mei_hbm_cl_find_by_cmd(dev, flow_control);
+	if (cl) {
+		cl->mei_flow_ctrl_creds++;
+		cl_dbg(dev, cl, "flow control creds = %d.\n",
+				cl->mei_flow_ctrl_creds);
 	}
 }
 
@@ -448,17 +534,13 @@ static void mei_hbm_cl_flow_control_res(struct mei_device *dev,
  * @dev: the device structure
  * @cl: a client to disconnect from
  *
- * This function returns -EIO on write failure
+ * Return: -EIO on write failure
  */
 int mei_hbm_cl_disconnect_req(struct mei_device *dev, struct mei_cl *cl)
 {
-	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
 	const size_t len = sizeof(struct hbm_client_connect_request);
 
-	mei_hbm_hdr(mei_hdr, len);
-	mei_hbm_cl_hdr(cl, CLIENT_DISCONNECT_REQ_CMD, dev->wr_msg.data, len);
-
-	return mei_write_message(dev, mei_hdr, dev->wr_msg.data);
+	return mei_hbm_cl_write(dev, cl, CLIENT_DISCONNECT_REQ_CMD, len);
 }
 
 /**
@@ -467,53 +549,34 @@ int mei_hbm_cl_disconnect_req(struct mei_device *dev, struct mei_cl *cl)
  * @dev: the device structure
  * @cl: a client to disconnect from
  *
- * This function returns -EIO on write failure
+ * Return: -EIO on write failure
  */
 int mei_hbm_cl_disconnect_rsp(struct mei_device *dev, struct mei_cl *cl)
 {
-	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
 	const size_t len = sizeof(struct hbm_client_connect_response);
 
-	mei_hbm_hdr(mei_hdr, len);
-	mei_hbm_cl_hdr(cl, CLIENT_DISCONNECT_RES_CMD, dev->wr_msg.data, len);
-
-	return mei_write_message(dev, mei_hdr, dev->wr_msg.data);
+	return mei_hbm_cl_write(dev, cl, CLIENT_DISCONNECT_RES_CMD, len);
 }
 
 /**
- * mei_hbm_cl_disconnect_res - disconnect response from ME
+ * mei_hbm_cl_disconnect_res - update the client state according
+ *       disconnect response
  *
- * @dev: the device structure
- * @rs: disconnect response bus message
+ * @cl: mei host client
+ * @cmd: disconnect client response host bus message
  */
-static void mei_hbm_cl_disconnect_res(struct mei_device *dev,
-		struct hbm_client_connect_response *rs)
+static void mei_hbm_cl_disconnect_res(struct mei_cl *cl,
+				      struct mei_hbm_cl_cmd *cmd)
 {
-	struct mei_cl *cl;
-	struct mei_cl_cb *cb, *next;
+	struct hbm_client_connect_response *rs =
+		(struct hbm_client_connect_response *)cmd;
 
-	dev_dbg(&dev->pdev->dev, "hbm: disconnect response cl:host=%02d me=%02d status=%d\n",
+	dev_dbg(cl->dev->dev, "hbm: disconnect response cl:host=%02d me=%02d status=%d\n",
 			rs->me_addr, rs->host_addr, rs->status);
 
-	list_for_each_entry_safe(cb, next, &dev->ctrl_rd_list.list, list) {
-		cl = cb->cl;
-
-		/* this should not happen */
-		if (WARN_ON(!cl)) {
-			list_del(&cb->list);
-			return;
-		}
-
-		if (mei_hbm_cl_addr_equal(cl, rs)) {
-			list_del(&cb->list);
-			if (rs->status == MEI_CL_DISCONN_SUCCESS)
-				cl->state = MEI_FILE_DISCONNECTED;
-
-			cl->status = 0;
-			cl->timer_count = 0;
-			break;
-		}
-	}
+	if (rs->status == MEI_CL_DISCONN_SUCCESS)
+		cl->state = MEI_FILE_DISCONNECTED;
+	cl->status = 0;
 }
 
 /**
@@ -522,38 +585,55 @@ static void mei_hbm_cl_disconnect_res(struct mei_device *dev,
  * @dev: the device structure
  * @cl: a client to connect to
  *
- * returns -EIO on write failure
+ * Return: -EIO on write failure
  */
 int mei_hbm_cl_connect_req(struct mei_device *dev, struct mei_cl *cl)
 {
-	struct mei_msg_hdr *mei_hdr = &dev->wr_msg.hdr;
 	const size_t len = sizeof(struct hbm_client_connect_request);
 
-	mei_hbm_hdr(mei_hdr, len);
-	mei_hbm_cl_hdr(cl, CLIENT_CONNECT_REQ_CMD, dev->wr_msg.data, len);
+	return mei_hbm_cl_write(dev, cl, CLIENT_CONNECT_REQ_CMD, len);
+}
 
-	return mei_write_message(dev, mei_hdr,  dev->wr_msg.data);
+/**
+ * mei_hbm_cl_connect_res - update the client state according
+ *        connection response
+ *
+ * @cl: mei host client
+ * @cmd: connect client response host bus message
+ */
+static void mei_hbm_cl_connect_res(struct mei_cl *cl,
+				   struct mei_hbm_cl_cmd *cmd)
+{
+	struct hbm_client_connect_response *rs =
+		(struct hbm_client_connect_response *)cmd;
+
+	dev_dbg(cl->dev->dev, "hbm: connect response cl:host=%02d me=%02d status=%s\n",
+			rs->me_addr, rs->host_addr,
+			mei_cl_conn_status_str(rs->status));
+
+	if (rs->status == MEI_CL_CONN_SUCCESS)
+		cl->state = MEI_FILE_CONNECTED;
+	else
+		cl->state = MEI_FILE_DISCONNECTED;
+	cl->status = mei_cl_conn_status_to_errno(rs->status);
 }
 
 /**
- * mei_hbm_cl_connect_res - connect response from the ME
+ * mei_hbm_cl_res - process hbm response received on behalf
+ *         an client
  *
  * @dev: the device structure
- * @rs: connect response bus message
+ * @rs:  hbm client message
+ * @fop_type: file operation type
  */
-static void mei_hbm_cl_connect_res(struct mei_device *dev,
-		struct hbm_client_connect_response *rs)
+static void mei_hbm_cl_res(struct mei_device *dev,
+			   struct mei_hbm_cl_cmd *rs,
+			   enum mei_cb_file_ops fop_type)
 {
-
 	struct mei_cl *cl;
 	struct mei_cl_cb *cb, *next;
 
-	dev_dbg(&dev->pdev->dev, "hbm: connect response cl:host=%02d me=%02d status=%s\n",
-			rs->me_addr, rs->host_addr,
-			mei_cl_conn_status_str(rs->status));
-
 	cl = NULL;
-
 	list_for_each_entry_safe(cb, next, &dev->ctrl_rd_list.list, list) {
 
 		cl = cb->cl;
@@ -563,7 +643,7 @@ static void mei_hbm_cl_connect_res(struct mei_device *dev,
 			continue;
 		}
 
-		if (cb->fop_type !=  MEI_FOP_CONNECT)
+		if (cb->fop_type != fop_type)
 			continue;
 
 		if (mei_hbm_cl_addr_equal(cl, rs)) {
@@ -575,12 +655,19 @@ static void mei_hbm_cl_connect_res(struct mei_device *dev,
 	if (!cl)
 		return;
 
+	switch (fop_type) {
+	case MEI_FOP_CONNECT:
+		mei_hbm_cl_connect_res(cl, rs);
+		break;
+	case MEI_FOP_DISCONNECT:
+		mei_hbm_cl_disconnect_res(cl, rs);
+		break;
+	default:
+		return;
+	}
+
 	cl->timer_count = 0;
-	if (rs->status == MEI_CL_CONN_SUCCESS)
-		cl->state = MEI_FILE_CONNECTED;
-	else
-		cl->state = MEI_FILE_DISCONNECTED;
-	cl->status = mei_cl_conn_status_to_errno(rs->status);
+	wake_up(&cl->wait);
 }
 
 
@@ -591,7 +678,7 @@ static void mei_hbm_cl_connect_res(struct mei_device *dev,
  * @dev: the device structure.
  * @disconnect_req: disconnect request bus message from the me
  *
- * returns -ENOMEM on allocation failure
+ * Return: -ENOMEM on allocation failure
  */
 static int mei_hbm_fw_disconnect_req(struct mei_device *dev,
 		struct hbm_client_connect_request *disconnect_req)
@@ -599,34 +686,46 @@ static int mei_hbm_fw_disconnect_req(struct mei_device *dev,
 	struct mei_cl *cl;
 	struct mei_cl_cb *cb;
 
-	list_for_each_entry(cl, &dev->file_list, link) {
-		if (mei_hbm_cl_addr_equal(cl, disconnect_req)) {
-			dev_dbg(&dev->pdev->dev, "disconnect request host client %d ME client %d.\n",
-					disconnect_req->host_addr,
-					disconnect_req->me_addr);
-			cl->state = MEI_FILE_DISCONNECTED;
-			cl->timer_count = 0;
-
-			cb = mei_io_cb_init(cl, NULL);
-			if (!cb)
-				return -ENOMEM;
-			cb->fop_type = MEI_FOP_DISCONNECT_RSP;
-			cl_dbg(dev, cl, "add disconnect response as first\n");
-			list_add(&cb->list, &dev->ctrl_wr_list.list);
+	cl = mei_hbm_cl_find_by_cmd(dev, disconnect_req);
+	if (cl) {
+		cl_dbg(dev, cl, "disconnect request received\n");
+		cl->state = MEI_FILE_DISCONNECTED;
+		cl->timer_count = 0;
 
-			break;
-		}
+		cb = mei_io_cb_init(cl, NULL);
+		if (!cb)
+			return -ENOMEM;
+		cb->fop_type = MEI_FOP_DISCONNECT_RSP;
+		cl_dbg(dev, cl, "add disconnect response as first\n");
+		list_add(&cb->list, &dev->ctrl_wr_list.list);
 	}
 	return 0;
 }
 
+/**
+ * mei_hbm_config_features - check what hbm features and commands
+ *        are supported by the fw
+ *
+ * @dev: the device structure
+ */
+static void mei_hbm_config_features(struct mei_device *dev)
+{
+	/* Power Gating Isolation Support */
+	dev->hbm_f_pg_supported = 0;
+	if (dev->version.major_version > HBM_MAJOR_VERSION_PGI)
+		dev->hbm_f_pg_supported = 1;
+
+	if (dev->version.major_version == HBM_MAJOR_VERSION_PGI &&
+	    dev->version.minor_version >= HBM_MINOR_VERSION_PGI)
+		dev->hbm_f_pg_supported = 1;
+}
 
 /**
  * mei_hbm_version_is_supported - checks whether the driver can
  *     support the hbm version of the device
  *
  * @dev: the device structure
- * returns true if driver can support hbm version of the device
+ * Return: true if driver can support hbm version of the device
  */
 bool mei_hbm_version_is_supported(struct mei_device *dev)
 {
@@ -640,44 +739,44 @@ bool mei_hbm_version_is_supported(struct mei_device *dev)
  * handle the read bus message cmd processing.
  *
  * @dev: the device structure
- * @mei_hdr: header of bus message
+ * @hdr: header of bus message
  *
- * returns 0 on success and < 0 on failure
+ * Return: 0 on success and < 0 on failure
  */
 int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 {
 	struct mei_bus_message *mei_msg;
-	struct mei_me_client *me_client;
 	struct hbm_host_version_response *version_res;
-	struct hbm_client_connect_response *connect_res;
-	struct hbm_client_connect_response *disconnect_res;
-	struct hbm_client_connect_request *disconnect_req;
-	struct hbm_flow_control *flow_control;
 	struct hbm_props_response *props_res;
 	struct hbm_host_enum_response *enum_res;
 
+	struct mei_hbm_cl_cmd *cl_cmd;
+	struct hbm_client_connect_request *disconnect_req;
+	struct hbm_flow_control *flow_control;
+
 	/* read the message to our buffer */
 	BUG_ON(hdr->length >= sizeof(dev->rd_msg_buf));
 	mei_read_slots(dev, dev->rd_msg_buf, hdr->length);
 	mei_msg = (struct mei_bus_message *)dev->rd_msg_buf;
+	cl_cmd  = (struct mei_hbm_cl_cmd *)mei_msg;
 
 	/* ignore spurious message and prevent reset nesting
 	 * hbm is put to idle during system reset
 	 */
 	if (dev->hbm_state == MEI_HBM_IDLE) {
-		dev_dbg(&dev->pdev->dev, "hbm: state is idle ignore spurious messages\n");
+		dev_dbg(dev->dev, "hbm: state is idle ignore spurious messages\n");
 		return 0;
 	}
 
 	switch (mei_msg->hbm_cmd) {
 	case HOST_START_RES_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: start: response message received.\n");
+		dev_dbg(dev->dev, "hbm: start: response message received.\n");
 
 		dev->init_clients_timer = 0;
 
 		version_res = (struct hbm_host_version_response *)mei_msg;
 
-		dev_dbg(&dev->pdev->dev, "HBM VERSION: DRIVER=%02d:%02d DEVICE=%02d:%02d\n",
+		dev_dbg(dev->dev, "HBM VERSION: DRIVER=%02d:%02d DEVICE=%02d:%02d\n",
 				HBM_MAJOR_VERSION, HBM_MINOR_VERSION,
 				version_res->me_max_version.major_version,
 				version_res->me_max_version.minor_version);
@@ -693,19 +792,21 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		}
 
 		if (!mei_hbm_version_is_supported(dev)) {
-			dev_warn(&dev->pdev->dev, "hbm: start: version mismatch - stopping the driver.\n");
+			dev_warn(dev->dev, "hbm: start: version mismatch - stopping the driver.\n");
 
 			dev->hbm_state = MEI_HBM_STOPPED;
 			if (mei_hbm_stop_req(dev)) {
-				dev_err(&dev->pdev->dev, "hbm: start: failed to send stop request\n");
+				dev_err(dev->dev, "hbm: start: failed to send stop request\n");
 				return -EIO;
 			}
 			break;
 		}
 
+		mei_hbm_config_features(dev);
+
 		if (dev->dev_state != MEI_DEV_INIT_CLIENTS ||
-		    dev->hbm_state != MEI_HBM_START) {
-			dev_err(&dev->pdev->dev, "hbm: start: state mismatch, [%d, %d]\n",
+		    dev->hbm_state != MEI_HBM_STARTING) {
+			dev_err(dev->dev, "hbm: start: state mismatch, [%d, %d]\n",
 				dev->dev_state, dev->hbm_state);
 			return -EPROTO;
 		}
@@ -713,45 +814,39 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		dev->hbm_state = MEI_HBM_STARTED;
 
 		if (mei_hbm_enum_clients_req(dev)) {
-			dev_err(&dev->pdev->dev, "hbm: start: failed to send enumeration request\n");
+			dev_err(dev->dev, "hbm: start: failed to send enumeration request\n");
 			return -EIO;
 		}
 
-		wake_up_interruptible(&dev->wait_recvd_msg);
+		wake_up(&dev->wait_hbm_start);
 		break;
 
 	case CLIENT_CONNECT_RES_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: client connect response: message received.\n");
-
-		connect_res = (struct hbm_client_connect_response *) mei_msg;
-		mei_hbm_cl_connect_res(dev, connect_res);
-		wake_up(&dev->wait_recvd_msg);
+		dev_dbg(dev->dev, "hbm: client connect response: message received.\n");
+		mei_hbm_cl_res(dev, cl_cmd, MEI_FOP_CONNECT);
 		break;
 
 	case CLIENT_DISCONNECT_RES_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: client disconnect response: message received.\n");
-
-		disconnect_res = (struct hbm_client_connect_response *) mei_msg;
-		mei_hbm_cl_disconnect_res(dev, disconnect_res);
-		wake_up(&dev->wait_recvd_msg);
+		dev_dbg(dev->dev, "hbm: client disconnect response: message received.\n");
+		mei_hbm_cl_res(dev, cl_cmd, MEI_FOP_DISCONNECT);
 		break;
 
 	case MEI_FLOW_CONTROL_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: client flow control response: message received.\n");
+		dev_dbg(dev->dev, "hbm: client flow control response: message received.\n");
 
 		flow_control = (struct hbm_flow_control *) mei_msg;
 		mei_hbm_cl_flow_control_res(dev, flow_control);
 		break;
 
 	case MEI_PG_ISOLATION_ENTRY_RES_CMD:
-		dev_dbg(&dev->pdev->dev, "power gate isolation entry response received\n");
+		dev_dbg(dev->dev, "power gate isolation entry response received\n");
 		dev->pg_event = MEI_PG_EVENT_RECEIVED;
 		if (waitqueue_active(&dev->wait_pg))
 			wake_up(&dev->wait_pg);
 		break;
 
 	case MEI_PG_ISOLATION_EXIT_REQ_CMD:
-		dev_dbg(&dev->pdev->dev, "power gate isolation exit request received\n");
+		dev_dbg(dev->dev, "power gate isolation exit request received\n");
 		dev->pg_event = MEI_PG_EVENT_RECEIVED;
 		if (waitqueue_active(&dev->wait_pg))
 			wake_up(&dev->wait_pg);
@@ -761,44 +856,33 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 			* this is HW initiated exit from PG.
 			* Start runtime pm resume sequence to exit from PG.
 			*/
-			pm_request_resume(&dev->pdev->dev);
+			pm_request_resume(dev->dev);
 		break;
 
 	case HOST_CLIENT_PROPERTIES_RES_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: properties response: message received.\n");
+		dev_dbg(dev->dev, "hbm: properties response: message received.\n");
 
 		dev->init_clients_timer = 0;
 
-		if (dev->me_clients == NULL) {
-			dev_err(&dev->pdev->dev, "hbm: properties response: mei_clients not allocated\n");
+		if (dev->dev_state != MEI_DEV_INIT_CLIENTS ||
+		    dev->hbm_state != MEI_HBM_CLIENT_PROPERTIES) {
+			dev_err(dev->dev, "hbm: properties response: state mismatch, [%d, %d]\n",
+				dev->dev_state, dev->hbm_state);
 			return -EPROTO;
 		}
 
 		props_res = (struct hbm_props_response *)mei_msg;
-		me_client = &dev->me_clients[dev->me_client_presentation_num];
 
 		if (props_res->status) {
-			dev_err(&dev->pdev->dev, "hbm: properties response: wrong status = %d\n",
-				props_res->status);
+			dev_err(dev->dev, "hbm: properties response: wrong status = %d %s\n",
+				props_res->status,
+				mei_hbm_status_str(props_res->status));
 			return -EPROTO;
 		}
 
-		if (me_client->client_id != props_res->address) {
-			dev_err(&dev->pdev->dev, "hbm: properties response: address mismatch %d ?= %d\n",
-				me_client->client_id, props_res->address);
-			return -EPROTO;
-		}
+		mei_hbm_me_cl_add(dev, props_res);
 
-		if (dev->dev_state != MEI_DEV_INIT_CLIENTS ||
-		    dev->hbm_state != MEI_HBM_CLIENT_PROPERTIES) {
-			dev_err(&dev->pdev->dev, "hbm: properties response: state mismatch, [%d, %d]\n",
-				dev->dev_state, dev->hbm_state);
-			return -EPROTO;
-		}
-
-		me_client->props = props_res->client_properties;
 		dev->me_client_index++;
-		dev->me_client_presentation_num++;
 
 		/* request property for the next client */
 		if (mei_hbm_prop_req(dev))
@@ -807,7 +891,7 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		break;
 
 	case HOST_ENUM_RES_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: enumeration response: message received\n");
+		dev_dbg(dev->dev, "hbm: enumeration response: message received\n");
 
 		dev->init_clients_timer = 0;
 
@@ -815,20 +899,15 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		BUILD_BUG_ON(sizeof(dev->me_clients_map)
 				< sizeof(enum_res->valid_addresses));
 		memcpy(dev->me_clients_map, enum_res->valid_addresses,
-			sizeof(enum_res->valid_addresses));
+				sizeof(enum_res->valid_addresses));
 
 		if (dev->dev_state != MEI_DEV_INIT_CLIENTS ||
 		    dev->hbm_state != MEI_HBM_ENUM_CLIENTS) {
-			dev_err(&dev->pdev->dev, "hbm: enumeration response: state mismatch, [%d, %d]\n",
+			dev_err(dev->dev, "hbm: enumeration response: state mismatch, [%d, %d]\n",
 				dev->dev_state, dev->hbm_state);
 			return -EPROTO;
 		}
 
-		if (mei_hbm_me_cl_allocate(dev)) {
-			dev_err(&dev->pdev->dev, "hbm: enumeration response: cannot allocate clients array\n");
-			return -ENOMEM;
-		}
-
 		dev->hbm_state = MEI_HBM_CLIENT_PROPERTIES;
 
 		/* first property request */
@@ -838,34 +917,34 @@ int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr)
 		break;
 
 	case HOST_STOP_RES_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: stop response: message received\n");
+		dev_dbg(dev->dev, "hbm: stop response: message received\n");
 
 		dev->init_clients_timer = 0;
 
 		if (dev->hbm_state != MEI_HBM_STOPPED) {
-			dev_err(&dev->pdev->dev, "hbm: stop response: state mismatch, [%d, %d]\n",
+			dev_err(dev->dev, "hbm: stop response: state mismatch, [%d, %d]\n",
 				dev->dev_state, dev->hbm_state);
 			return -EPROTO;
 		}
 
 		dev->dev_state = MEI_DEV_POWER_DOWN;
-		dev_info(&dev->pdev->dev, "hbm: stop response: resetting.\n");
+		dev_info(dev->dev, "hbm: stop response: resetting.\n");
 		/* force the reset */
 		return -EPROTO;
 		break;
 
 	case CLIENT_DISCONNECT_REQ_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: disconnect request: message received\n");
+		dev_dbg(dev->dev, "hbm: disconnect request: message received\n");
 
 		disconnect_req = (struct hbm_client_connect_request *)mei_msg;
 		mei_hbm_fw_disconnect_req(dev, disconnect_req);
 		break;
 
 	case ME_STOP_REQ_CMD:
-		dev_dbg(&dev->pdev->dev, "hbm: stop request: message received\n");
+		dev_dbg(dev->dev, "hbm: stop request: message received\n");
 		dev->hbm_state = MEI_HBM_STOPPED;
 		if (mei_hbm_stop_req(dev)) {
-			dev_err(&dev->pdev->dev, "hbm: start: failed to send stop request\n");
+			dev_err(dev->dev, "hbm: stop request: failed to send stop request\n");
 			return -EIO;
 		}
 		break;
diff --git a/drivers/misc/mei/hbm.h b/drivers/misc/mei/hbm.h
index 683eb2835cec..b7cd3d857fd5 100644
--- a/drivers/misc/mei/hbm.h
+++ b/drivers/misc/mei/hbm.h
@@ -25,29 +25,24 @@ struct mei_cl;
  * enum mei_hbm_state - host bus message protocol state
  *
  * @MEI_HBM_IDLE : protocol not started
- * @MEI_HBM_START : start request message was sent
+ * @MEI_HBM_STARTING : start request message was sent
+ * @MEI_HBM_STARTED : start reply message was received
  * @MEI_HBM_ENUM_CLIENTS : enumeration request was sent
  * @MEI_HBM_CLIENT_PROPERTIES : acquiring clients properties
+ * @MEI_HBM_STOPPED : stopping exchange
  */
 enum mei_hbm_state {
 	MEI_HBM_IDLE = 0,
-	MEI_HBM_START,
+	MEI_HBM_STARTING,
 	MEI_HBM_STARTED,
 	MEI_HBM_ENUM_CLIENTS,
 	MEI_HBM_CLIENT_PROPERTIES,
 	MEI_HBM_STOPPED,
 };
 
-int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr);
+const char *mei_hbm_state_str(enum mei_hbm_state state);
 
-static inline void mei_hbm_hdr(struct mei_msg_hdr *hdr, size_t length)
-{
-	hdr->host_addr = 0;
-	hdr->me_addr = 0;
-	hdr->length = length;
-	hdr->msg_complete = 1;
-	hdr->reserved = 0;
-}
+int mei_hbm_dispatch(struct mei_device *dev, struct mei_msg_hdr *hdr);
 
 void mei_hbm_idle(struct mei_device *dev);
 void mei_hbm_reset(struct mei_device *dev);
diff --git a/drivers/misc/mei/hw-me.c b/drivers/misc/mei/hw-me.c
index a9a0d08f758e..4f2fd6fc1e23 100644
--- a/drivers/misc/mei/hw-me.c
+++ b/drivers/misc/mei/hw-me.c
@@ -28,10 +28,10 @@
 /**
  * mei_me_reg_read - Reads 32bit data from the mei device
  *
- * @dev: the device structure
+ * @hw: the me hardware structure
  * @offset: offset from which to read the data
  *
- * returns register value (u32)
+ * Return: register value (u32)
  */
 static inline u32 mei_me_reg_read(const struct mei_me_hw *hw,
 			       unsigned long offset)
@@ -43,7 +43,7 @@ static inline u32 mei_me_reg_read(const struct mei_me_hw *hw,
 /**
  * mei_me_reg_write - Writes 32bit data to the mei device
  *
- * @dev: the device structure
+ * @hw: the me hardware structure
  * @offset: offset from which to write the data
  * @value: register value to write (u32)
  */
@@ -59,7 +59,7 @@ static inline void mei_me_reg_write(const struct mei_me_hw *hw,
  *
  * @dev: the device structure
  *
- * returns ME_CB_RW register value (u32)
+ * Return: ME_CB_RW register value (u32)
  */
 static u32 mei_me_mecbrw_read(const struct mei_device *dev)
 {
@@ -68,9 +68,9 @@ static u32 mei_me_mecbrw_read(const struct mei_device *dev)
 /**
  * mei_me_mecsr_read - Reads 32bit data from the ME CSR
  *
- * @dev: the device structure
+ * @hw: the me hardware structure
  *
- * returns ME_CSR_HA register value (u32)
+ * Return: ME_CSR_HA register value (u32)
  */
 static inline u32 mei_me_mecsr_read(const struct mei_me_hw *hw)
 {
@@ -80,9 +80,9 @@ static inline u32 mei_me_mecsr_read(const struct mei_me_hw *hw)
 /**
  * mei_hcsr_read - Reads 32bit data from the host CSR
  *
- * @dev: the device structure
+ * @hw: the me hardware structure
  *
- * returns H_CSR register value (u32)
+ * Return: H_CSR register value (u32)
  */
 static inline u32 mei_hcsr_read(const struct mei_me_hw *hw)
 {
@@ -93,7 +93,8 @@ static inline u32 mei_hcsr_read(const struct mei_me_hw *hw)
  * mei_hcsr_set - writes H_CSR register to the mei device,
  * and ignores the H_IS bit for it is write-one-to-zero.
  *
- * @dev: the device structure
+ * @hw: the me hardware structure
+ * @hcsr: new register value
  */
 static inline void mei_hcsr_set(struct mei_me_hw *hw, u32 hcsr)
 {
@@ -101,6 +102,36 @@ static inline void mei_hcsr_set(struct mei_me_hw *hw, u32 hcsr)
 	mei_me_reg_write(hw, H_CSR, hcsr);
 }
 
+/**
+ * mei_me_fw_status - read fw status register from pci config space
+ *
+ * @dev: mei device
+ * @fw_status: fw status register values
+ *
+ * Return: 0 on success, error otherwise
+ */
+static int mei_me_fw_status(struct mei_device *dev,
+			    struct mei_fw_status *fw_status)
+{
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
+	struct mei_me_hw *hw = to_me_hw(dev);
+	const struct mei_fw_status *fw_src = &hw->cfg->fw_status;
+	int ret;
+	int i;
+
+	if (!fw_status)
+		return -EINVAL;
+
+	fw_status->count = fw_src->count;
+	for (i = 0; i < fw_src->count && i < MEI_FW_STATUS_MAX; i++) {
+		ret = pci_read_config_dword(pdev,
+			fw_src->status[i], &fw_status->status[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
 
 /**
  * mei_me_hw_config - configure hw dependent settings
@@ -121,17 +152,19 @@ static void mei_me_hw_config(struct mei_device *dev)
  * mei_me_pg_state  - translate internal pg state
  *   to the mei power gating state
  *
- * @hw -  me hardware
- * returns: MEI_PG_OFF if aliveness is on and MEI_PG_ON otherwise
+ * @dev:  mei device
+ *
+ * Return: MEI_PG_OFF if aliveness is on and MEI_PG_ON otherwise
  */
 static inline enum mei_pg_state mei_me_pg_state(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+
 	return hw->pg_state;
 }
 
 /**
- * mei_clear_interrupts - clear and stop interrupts
+ * mei_me_intr_clear - clear and stop interrupts
  *
  * @dev: the device structure
  */
@@ -139,6 +172,7 @@ static void mei_me_intr_clear(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
 	u32 hcsr = mei_hcsr_read(hw);
+
 	if ((hcsr & H_IS) == H_IS)
 		mei_me_reg_write(hw, H_CSR, hcsr);
 }
@@ -151,12 +185,13 @@ static void mei_me_intr_enable(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
 	u32 hcsr = mei_hcsr_read(hw);
+
 	hcsr |= H_IE;
 	mei_hcsr_set(hw, hcsr);
 }
 
 /**
- * mei_disable_interrupts - disables mei device interrupts
+ * mei_me_intr_disable - disables mei device interrupts
  *
  * @dev: the device structure
  */
@@ -164,6 +199,7 @@ static void mei_me_intr_disable(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
 	u32 hcsr = mei_hcsr_read(hw);
+
 	hcsr  &= ~H_IE;
 	mei_hcsr_set(hw, hcsr);
 }
@@ -190,6 +226,8 @@ static void mei_me_hw_reset_release(struct mei_device *dev)
  *
  * @dev: the device structure
  * @intr_enable: if interrupt should be enabled after reset.
+ *
+ * Return: always 0
  */
 static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 {
@@ -213,10 +251,10 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 	hcsr = mei_hcsr_read(hw);
 
 	if ((hcsr & H_RST) == 0)
-		dev_warn(&dev->pdev->dev, "H_RST is not set = 0x%08X", hcsr);
+		dev_warn(dev->dev, "H_RST is not set = 0x%08X", hcsr);
 
 	if ((hcsr & H_RDY) == H_RDY)
-		dev_warn(&dev->pdev->dev, "H_RDY is not cleared 0x%08X", hcsr);
+		dev_warn(dev->dev, "H_RDY is not cleared 0x%08X", hcsr);
 
 	if (intr_enable == false)
 		mei_me_hw_reset_release(dev);
@@ -227,26 +265,27 @@ static int mei_me_hw_reset(struct mei_device *dev, bool intr_enable)
 /**
  * mei_me_host_set_ready - enable device
  *
- * @dev - mei device
- * returns bool
+ * @dev: mei device
  */
-
 static void mei_me_host_set_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+
 	hw->host_hw_state = mei_hcsr_read(hw);
 	hw->host_hw_state |= H_IE | H_IG | H_RDY;
 	mei_hcsr_set(hw, hw->host_hw_state);
 }
+
 /**
  * mei_me_host_is_ready - check whether the host has turned ready
  *
- * @dev - mei device
- * returns bool
+ * @dev: mei device
+ * Return: bool
  */
 static bool mei_me_host_is_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+
 	hw->host_hw_state = mei_hcsr_read(hw);
 	return (hw->host_hw_state & H_RDY) == H_RDY;
 }
@@ -254,43 +293,53 @@ static bool mei_me_host_is_ready(struct mei_device *dev)
 /**
  * mei_me_hw_is_ready - check whether the me(hw) has turned ready
  *
- * @dev - mei device
- * returns bool
+ * @dev: mei device
+ * Return: bool
  */
 static bool mei_me_hw_is_ready(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
+
 	hw->me_hw_state = mei_me_mecsr_read(hw);
 	return (hw->me_hw_state & ME_RDY_HRA) == ME_RDY_HRA;
 }
 
+/**
+ * mei_me_hw_ready_wait - wait until the me(hw) has turned ready
+ *  or timeout is reached
+ *
+ * @dev: mei device
+ * Return: 0 on success, error otherwise
+ */
 static int mei_me_hw_ready_wait(struct mei_device *dev)
 {
-	int err;
-
 	mutex_unlock(&dev->device_lock);
-	err = wait_event_interruptible_timeout(dev->wait_hw_ready,
+	wait_event_timeout(dev->wait_hw_ready,
 			dev->recvd_hw_ready,
 			mei_secs_to_jiffies(MEI_HW_READY_TIMEOUT));
 	mutex_lock(&dev->device_lock);
-	if (!err && !dev->recvd_hw_ready) {
-		if (!err)
-			err = -ETIME;
-		dev_err(&dev->pdev->dev,
-			"wait hw ready failed. status = %d\n", err);
-		return err;
+	if (!dev->recvd_hw_ready) {
+		dev_err(dev->dev, "wait hw ready failed\n");
+		return -ETIME;
 	}
 
 	dev->recvd_hw_ready = false;
 	return 0;
 }
 
+/**
+ * mei_me_hw_start - hw start routine
+ *
+ * @dev: mei device
+ * Return: 0 on success, error otherwise
+ */
 static int mei_me_hw_start(struct mei_device *dev)
 {
 	int ret = mei_me_hw_ready_wait(dev);
+
 	if (ret)
 		return ret;
-	dev_dbg(&dev->pdev->dev, "hw is ready\n");
+	dev_dbg(dev->dev, "hw is ready\n");
 
 	mei_me_host_set_ready(dev);
 	return ret;
@@ -302,7 +351,7 @@ static int mei_me_hw_start(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns number of filled slots
+ * Return: number of filled slots
  */
 static unsigned char mei_hbuf_filled_slots(struct mei_device *dev)
 {
@@ -322,7 +371,7 @@ static unsigned char mei_hbuf_filled_slots(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns true if empty, false - otherwise.
+ * Return: true if empty, false - otherwise.
  */
 static bool mei_me_hbuf_is_empty(struct mei_device *dev)
 {
@@ -334,7 +383,7 @@ static bool mei_me_hbuf_is_empty(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns -EOVERFLOW if overflow, otherwise empty slots count
+ * Return: -EOVERFLOW if overflow, otherwise empty slots count
  */
 static int mei_me_hbuf_empty_slots(struct mei_device *dev)
 {
@@ -350,6 +399,13 @@ static int mei_me_hbuf_empty_slots(struct mei_device *dev)
 	return empty_slots;
 }
 
+/**
+ * mei_me_hbuf_max_len - returns size of hw buffer.
+ *
+ * @dev: the device structure
+ *
+ * Return: size of hw buffer in bytes
+ */
 static size_t mei_me_hbuf_max_len(const struct mei_device *dev)
 {
 	return dev->hbuf_depth * sizeof(u32) - sizeof(struct mei_msg_hdr);
@@ -363,7 +419,7 @@ static size_t mei_me_hbuf_max_len(const struct mei_device *dev)
  * @header: mei HECI header of message
  * @buf: message payload will be written
  *
- * This function returns -EIO if write has failed
+ * Return: -EIO if write has failed
  */
 static int mei_me_write_message(struct mei_device *dev,
 			struct mei_msg_hdr *header,
@@ -378,10 +434,10 @@ static int mei_me_write_message(struct mei_device *dev,
 	int i;
 	int empty_slots;
 
-	dev_dbg(&dev->pdev->dev, MEI_HDR_FMT, MEI_HDR_PRM(header));
+	dev_dbg(dev->dev, MEI_HDR_FMT, MEI_HDR_PRM(header));
 
 	empty_slots = mei_hbuf_empty_slots(dev);
-	dev_dbg(&dev->pdev->dev, "empty slots = %hu.\n", empty_slots);
+	dev_dbg(dev->dev, "empty slots = %hu.\n", empty_slots);
 
 	dw_cnt = mei_data2slots(length);
 	if (empty_slots < 0 || dw_cnt > empty_slots)
@@ -395,6 +451,7 @@ static int mei_me_write_message(struct mei_device *dev,
 	rem = length & 0x3;
 	if (rem > 0) {
 		u32 reg = 0;
+
 		memcpy(&reg, &buf[length - rem], rem);
 		mei_me_reg_write(hw, H_CB_WW, reg);
 	}
@@ -412,7 +469,7 @@ static int mei_me_write_message(struct mei_device *dev,
  *
  * @dev: the device structure
  *
- * returns -EOVERFLOW if overflow, otherwise filled slots count
+ * Return: -EOVERFLOW if overflow, otherwise filled slots count
  */
 static int mei_me_count_full_read_slots(struct mei_device *dev)
 {
@@ -430,7 +487,7 @@ static int mei_me_count_full_read_slots(struct mei_device *dev)
 	if (filled_slots > buffer_depth)
 		return -EOVERFLOW;
 
-	dev_dbg(&dev->pdev->dev, "filled_slots =%08x\n", filled_slots);
+	dev_dbg(dev->dev, "filled_slots =%08x\n", filled_slots);
 	return (int)filled_slots;
 }
 
@@ -440,6 +497,8 @@ static int mei_me_count_full_read_slots(struct mei_device *dev)
  * @dev: the device structure
  * @buffer: message buffer will be written
  * @buffer_length: message size will be read
+ *
+ * Return: always 0
  */
 static int mei_me_read_slots(struct mei_device *dev, unsigned char *buffer,
 		    unsigned long buffer_length)
@@ -453,6 +512,7 @@ static int mei_me_read_slots(struct mei_device *dev, unsigned char *buffer,
 
 	if (buffer_length > 0) {
 		u32 reg = mei_me_mecbrw_read(dev);
+
 		memcpy(reg_buf, &reg, buffer_length);
 	}
 
@@ -462,7 +522,7 @@ static int mei_me_read_slots(struct mei_device *dev, unsigned char *buffer,
 }
 
 /**
- * mei_me_pg_enter - write pg enter register to mei device.
+ * mei_me_pg_enter - write pg enter register
  *
  * @dev: the device structure
  */
@@ -470,12 +530,13 @@ static void mei_me_pg_enter(struct mei_device *dev)
 {
 	struct mei_me_hw *hw = to_me_hw(dev);
 	u32 reg = mei_me_reg_read(hw, H_HPG_CSR);
+
 	reg |= H_HPG_CSR_PGI;
 	mei_me_reg_write(hw, H_HPG_CSR, reg);
 }
 
 /**
- * mei_me_pg_enter - write pg enter register to mei device.
+ * mei_me_pg_exit - write pg exit register
  *
  * @dev: the device structure
  */
@@ -495,7 +556,7 @@ static void mei_me_pg_exit(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns 0 on success an error code otherwise
+ * Return: 0 on success an error code otherwise
  */
 int mei_me_pg_set_sync(struct mei_device *dev)
 {
@@ -532,7 +593,7 @@ int mei_me_pg_set_sync(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns 0 on success an error code otherwise
+ * Return: 0 on success an error code otherwise
  */
 int mei_me_pg_unset_sync(struct mei_device *dev)
 {
@@ -569,7 +630,7 @@ reply:
  *
  * @dev: the device structure
  *
- * returns: true is pg supported, false otherwise
+ * Return: true is pg supported, false otherwise
  */
 static bool mei_me_pg_is_enabled(struct mei_device *dev)
 {
@@ -579,17 +640,13 @@ static bool mei_me_pg_is_enabled(struct mei_device *dev)
 	if ((reg & ME_PGIC_HRA) == 0)
 		goto notsupported;
 
-	if (dev->version.major_version < HBM_MAJOR_VERSION_PGI)
-		goto notsupported;
-
-	if (dev->version.major_version == HBM_MAJOR_VERSION_PGI &&
-	    dev->version.minor_version < HBM_MINOR_VERSION_PGI)
+	if (!dev->hbm_f_pg_supported)
 		goto notsupported;
 
 	return true;
 
 notsupported:
-	dev_dbg(&dev->pdev->dev, "pg: not supported: HGP = %d hbm version %d.%d ?= %d.%d\n",
+	dev_dbg(dev->dev, "pg: not supported: HGP = %d hbm version %d.%d ?= %d.%d\n",
 		!!(reg & ME_PGIC_HRA),
 		dev->version.major_version,
 		dev->version.minor_version,
@@ -605,7 +662,7 @@ notsupported:
  * @irq: The irq number
  * @dev_id: pointer to the device structure
  *
- * returns irqreturn_t
+ * Return: irqreturn_t
  */
 
 irqreturn_t mei_me_irq_quick_handler(int irq, void *dev_id)
@@ -630,7 +687,7 @@ irqreturn_t mei_me_irq_quick_handler(int irq, void *dev_id)
  * @irq: The irq number
  * @dev_id: pointer to the device structure
  *
- * returns irqreturn_t
+ * Return: irqreturn_t
  *
  */
 irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
@@ -640,19 +697,19 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 	s32 slots;
 	int rets = 0;
 
-	dev_dbg(&dev->pdev->dev, "function called after ISR to handle the interrupt processing.\n");
+	dev_dbg(dev->dev, "function called after ISR to handle the interrupt processing.\n");
 	/* initialize our complete list */
 	mutex_lock(&dev->device_lock);
 	mei_io_list_init(&complete_list);
 
 	/* Ack the interrupt here
 	 * In case of MSI we don't go through the quick handler */
-	if (pci_dev_msi_enabled(dev->pdev))
+	if (pci_dev_msi_enabled(to_pci_dev(dev->dev)))
 		mei_clear_interrupts(dev);
 
 	/* check if ME wants a reset */
 	if (!mei_hw_is_ready(dev) && dev->dev_state != MEI_DEV_RESETTING) {
-		dev_warn(&dev->pdev->dev, "FW not ready: resetting.\n");
+		dev_warn(dev->dev, "FW not ready: resetting.\n");
 		schedule_work(&dev->reset_work);
 		goto end;
 	}
@@ -661,19 +718,19 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 	if (!mei_host_is_ready(dev)) {
 		if (mei_hw_is_ready(dev)) {
 			mei_me_hw_reset_release(dev);
-			dev_dbg(&dev->pdev->dev, "we need to start the dev.\n");
+			dev_dbg(dev->dev, "we need to start the dev.\n");
 
 			dev->recvd_hw_ready = true;
-			wake_up_interruptible(&dev->wait_hw_ready);
+			wake_up(&dev->wait_hw_ready);
 		} else {
-			dev_dbg(&dev->pdev->dev, "Spurious Interrupt\n");
+			dev_dbg(dev->dev, "Spurious Interrupt\n");
 		}
 		goto end;
 	}
 	/* check slots available for reading */
 	slots = mei_count_full_read_slots(dev);
 	while (slots > 0) {
-		dev_dbg(&dev->pdev->dev, "slots to read = %08x\n", slots);
+		dev_dbg(dev->dev, "slots to read = %08x\n", slots);
 		rets = mei_irq_read_handler(dev, &complete_list, &slots);
 		/* There is a race between ME write and interrupt delivery:
 		 * Not all data is always available immediately after the
@@ -683,7 +740,7 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 			break;
 
 		if (rets && dev->dev_state != MEI_DEV_RESETTING) {
-			dev_err(&dev->pdev->dev, "mei_irq_read_handler ret = %d.\n",
+			dev_err(dev->dev, "mei_irq_read_handler ret = %d.\n",
 						rets);
 			schedule_work(&dev->reset_work);
 			goto end;
@@ -705,13 +762,14 @@ irqreturn_t mei_me_irq_thread_handler(int irq, void *dev_id)
 	mei_irq_compl_handler(dev, &complete_list);
 
 end:
-	dev_dbg(&dev->pdev->dev, "interrupt thread end ret = %d\n", rets);
+	dev_dbg(dev->dev, "interrupt thread end ret = %d\n", rets);
 	mutex_unlock(&dev->device_lock);
 	return IRQ_HANDLED;
 }
 
 static const struct mei_hw_ops mei_me_hw_ops = {
 
+	.fw_status = mei_me_fw_status,
 	.pg_state  = mei_me_pg_state,
 
 	.host_is_ready = mei_me_host_is_ready,
@@ -741,6 +799,7 @@ static const struct mei_hw_ops mei_me_hw_ops = {
 static bool mei_me_fw_type_nm(struct pci_dev *pdev)
 {
 	u32 reg;
+
 	pci_read_config_dword(pdev, PCI_CFG_HFS_2, &reg);
 	/* make sure that bit 9 (NM) is up and bit 10 (DM) is down */
 	return (reg & 0x600) == 0x200;
@@ -809,23 +868,22 @@ const struct mei_cfg mei_me_lpt_cfg = {
  * @pdev: The pci device structure
  * @cfg: per device generation config
  *
- * returns The mei_device_device pointer on success, NULL on failure.
+ * Return: The mei_device_device pointer on success, NULL on failure.
  */
 struct mei_device *mei_me_dev_init(struct pci_dev *pdev,
 				   const struct mei_cfg *cfg)
 {
 	struct mei_device *dev;
+	struct mei_me_hw *hw;
 
 	dev = kzalloc(sizeof(struct mei_device) +
 			 sizeof(struct mei_me_hw), GFP_KERNEL);
 	if (!dev)
 		return NULL;
+	hw = to_me_hw(dev);
 
-	mei_device_init(dev, cfg);
-
-	dev->ops = &mei_me_hw_ops;
-
-	dev->pdev = pdev;
+	mei_device_init(dev, &pdev->dev, &mei_me_hw_ops);
+	hw->cfg = cfg;
 	return dev;
 }
 
diff --git a/drivers/misc/mei/hw-me.h b/drivers/misc/mei/hw-me.h
index 12b0f4bbe1f1..e6a59a62573a 100644
--- a/drivers/misc/mei/hw-me.h
+++ b/drivers/misc/mei/hw-me.h
@@ -19,14 +19,44 @@
 #ifndef _MEI_INTERFACE_H_
 #define _MEI_INTERFACE_H_
 
-#include <linux/mei.h>
 #include <linux/irqreturn.h>
+#include <linux/pci.h>
+#include <linux/mei.h>
+
 #include "mei_dev.h"
 #include "client.h"
 
+/*
+ * mei_cfg - mei device configuration
+ *
+ * @fw_status: FW status
+ * @quirk_probe: device exclusion quirk
+ */
+struct mei_cfg {
+	const struct mei_fw_status fw_status;
+	bool (*quirk_probe)(struct pci_dev *pdev);
+};
+
+
+#define MEI_PCI_DEVICE(dev, cfg) \
+	.vendor = PCI_VENDOR_ID_INTEL, .device = (dev), \
+	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, \
+	.driver_data = (kernel_ulong_t)&(cfg)
+
+
 #define MEI_ME_RPM_TIMEOUT    500 /* ms */
 
+/**
+ * struct mei_me_hw - me hw specific data
+ *
+ * @cfg: per device generation config and ops
+ * @mem_addr:  io memory address
+ * @host_hw_state: cached host state
+ * @me_hw_state:   cached me (fw) state
+ * @pg_state:      power gating state
+ */
 struct mei_me_hw {
+	const struct mei_cfg *cfg;
 	void __iomem *mem_addr;
 	/*
 	 * hw states of host and fw(ME)
diff --git a/drivers/misc/mei/hw-txe.c b/drivers/misc/mei/hw-txe.c
index f1cd166094f2..c5e1902e493f 100644
--- a/drivers/misc/mei/hw-txe.c
+++ b/drivers/misc/mei/hw-txe.c
@@ -28,11 +28,12 @@
 #include "hbm.h"
 
 /**
- * mei_txe_reg_read - Reads 32bit data from the device
+ * mei_txe_reg_read - Reads 32bit data from the txe device
  *
  * @base_addr: registers base address
  * @offset: register offset
  *
+ * Return: register value
  */
 static inline u32 mei_txe_reg_read(void __iomem *base_addr,
 					unsigned long offset)
@@ -41,7 +42,7 @@ static inline u32 mei_txe_reg_read(void __iomem *base_addr,
 }
 
 /**
- * mei_txe_reg_write - Writes 32bit data to the device
+ * mei_txe_reg_write - Writes 32bit data to the txe device
  *
  * @base_addr: registers base address
  * @offset: register offset
@@ -56,10 +57,12 @@ static inline void mei_txe_reg_write(void __iomem *base_addr,
 /**
  * mei_txe_sec_reg_read_silent - Reads 32bit data from the SeC BAR
  *
- * @dev: the device structure
+ * @hw: the txe hardware structure
  * @offset: register offset
  *
  * Doesn't check for aliveness while Reads 32bit data from the SeC BAR
+ *
+ * Return: register value
  */
 static inline u32 mei_txe_sec_reg_read_silent(struct mei_txe_hw *hw,
 				unsigned long offset)
@@ -70,10 +73,12 @@ static inline u32 mei_txe_sec_reg_read_silent(struct mei_txe_hw *hw,
 /**
  * mei_txe_sec_reg_read - Reads 32bit data from the SeC BAR
  *
- * @dev: the device structure
+ * @hw: the txe hardware structure
  * @offset: register offset
  *
  * Reads 32bit data from the SeC BAR and shout loud if aliveness is not set
+ *
+ * Return: register value
  */
 static inline u32 mei_txe_sec_reg_read(struct mei_txe_hw *hw,
 				unsigned long offset)
@@ -85,7 +90,7 @@ static inline u32 mei_txe_sec_reg_read(struct mei_txe_hw *hw,
  * mei_txe_sec_reg_write_silent - Writes 32bit data to the SeC BAR
  *   doesn't check for aliveness
  *
- * @dev: the device structure
+ * @hw: the txe hardware structure
  * @offset: register offset
  * @value: value to write
  *
@@ -100,7 +105,7 @@ static inline void mei_txe_sec_reg_write_silent(struct mei_txe_hw *hw,
 /**
  * mei_txe_sec_reg_write - Writes 32bit data to the SeC BAR
  *
- * @dev: the device structure
+ * @hw: the txe hardware structure
  * @offset: register offset
  * @value: value to write
  *
@@ -115,9 +120,10 @@ static inline void mei_txe_sec_reg_write(struct mei_txe_hw *hw,
 /**
  * mei_txe_br_reg_read - Reads 32bit data from the Bridge BAR
  *
- * @hw: the device structure
+ * @hw: the txe hardware structure
  * @offset: offset from which to read the data
  *
+ * Return: the byte read.
  */
 static inline u32 mei_txe_br_reg_read(struct mei_txe_hw *hw,
 				unsigned long offset)
@@ -128,7 +134,7 @@ static inline u32 mei_txe_br_reg_read(struct mei_txe_hw *hw,
 /**
  * mei_txe_br_reg_write - Writes 32bit data to the Bridge BAR
  *
- * @hw: the device structure
+ * @hw: the txe hardware structure
  * @offset: offset from which to write the data
  * @value: the byte to write
  */
@@ -147,7 +153,10 @@ static inline void mei_txe_br_reg_write(struct mei_txe_hw *hw,
  * Request for aliveness change and returns true if the change is
  *   really needed and false if aliveness is already
  *   in the requested state
- * Requires device lock to be held
+ *
+ * Locking: called under "dev->device_lock" lock
+ *
+ * Return: true if request was send
  */
 static bool mei_txe_aliveness_set(struct mei_device *dev, u32 req)
 {
@@ -155,7 +164,7 @@ static bool mei_txe_aliveness_set(struct mei_device *dev, u32 req)
 	struct mei_txe_hw *hw = to_txe_hw(dev);
 	bool do_req = hw->aliveness != req;
 
-	dev_dbg(&dev->pdev->dev, "Aliveness current=%d request=%d\n",
+	dev_dbg(dev->dev, "Aliveness current=%d request=%d\n",
 				hw->aliveness, req);
 	if (do_req) {
 		dev->pg_event = MEI_PG_EVENT_WAIT;
@@ -172,26 +181,31 @@ static bool mei_txe_aliveness_set(struct mei_device *dev, u32 req)
  *
  * Extract HICR_HOST_ALIVENESS_RESP_ACK bit from
  * from HICR_HOST_ALIVENESS_REQ register value
+ *
+ * Return: SICR_HOST_ALIVENESS_REQ_REQUESTED bit value
  */
 static u32 mei_txe_aliveness_req_get(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
 	u32 reg;
+
 	reg = mei_txe_br_reg_read(hw, SICR_HOST_ALIVENESS_REQ_REG);
 	return reg & SICR_HOST_ALIVENESS_REQ_REQUESTED;
 }
 
 /**
  * mei_txe_aliveness_get - get aliveness response register value
+ *
  * @dev: the device structure
  *
- * Extract HICR_HOST_ALIVENESS_RESP_ACK bit
- * from HICR_HOST_ALIVENESS_RESP register value
+ * Return: HICR_HOST_ALIVENESS_RESP_ACK bit from HICR_HOST_ALIVENESS_RESP
+ *         register
  */
 static u32 mei_txe_aliveness_get(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
 	u32 reg;
+
 	reg = mei_txe_br_reg_read(hw, HICR_HOST_ALIVENESS_RESP_REG);
 	return reg & HICR_HOST_ALIVENESS_RESP_ACK;
 }
@@ -203,7 +217,8 @@ static u32 mei_txe_aliveness_get(struct mei_device *dev)
  * @expected: expected aliveness value
  *
  * Polls for HICR_HOST_ALIVENESS_RESP.ALIVENESS_RESP to be set
- * returns > 0 if the expected value was received, -ETIME otherwise
+ *
+ * Return: > 0 if the expected value was received, -ETIME otherwise
  */
 static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 {
@@ -214,7 +229,7 @@ static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 		hw->aliveness = mei_txe_aliveness_get(dev);
 		if (hw->aliveness == expected) {
 			dev->pg_event = MEI_PG_EVENT_IDLE;
-			dev_dbg(&dev->pdev->dev,
+			dev_dbg(dev->dev,
 				"aliveness settled after %d msecs\n", t);
 			return t;
 		}
@@ -225,7 +240,7 @@ static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
 	} while (t < SEC_ALIVENESS_WAIT_TIMEOUT);
 
 	dev->pg_event = MEI_PG_EVENT_IDLE;
-	dev_err(&dev->pdev->dev, "aliveness timed out\n");
+	dev_err(dev->dev, "aliveness timed out\n");
 	return -ETIME;
 }
 
@@ -236,7 +251,8 @@ static int mei_txe_aliveness_poll(struct mei_device *dev, u32 expected)
  * @expected: expected aliveness value
  *
  * Waits for HICR_HOST_ALIVENESS_RESP.ALIVENESS_RESP to be set
- * returns returns 0 on success and < 0 otherwise
+ *
+ * Return: 0 on success and < 0 otherwise
  */
 static int mei_txe_aliveness_wait(struct mei_device *dev, u32 expected)
 {
@@ -259,10 +275,10 @@ static int mei_txe_aliveness_wait(struct mei_device *dev, u32 expected)
 	ret = hw->aliveness == expected ? 0 : -ETIME;
 
 	if (ret)
-		dev_warn(&dev->pdev->dev, "aliveness timed out = %ld aliveness = %d event = %d\n",
+		dev_warn(dev->dev, "aliveness timed out = %ld aliveness = %d event = %d\n",
 			err, hw->aliveness, dev->pg_event);
 	else
-		dev_dbg(&dev->pdev->dev, "aliveness settled after = %d msec aliveness = %d event = %d\n",
+		dev_dbg(dev->dev, "aliveness settled after = %d msec aliveness = %d event = %d\n",
 			jiffies_to_msecs(timeout - err),
 			hw->aliveness, dev->pg_event);
 
@@ -274,8 +290,9 @@ static int mei_txe_aliveness_wait(struct mei_device *dev, u32 expected)
  * mei_txe_aliveness_set_sync - sets an wait for aliveness to complete
  *
  * @dev: the device structure
+ * @req: requested aliveness value
  *
- * returns returns 0 on success and < 0 otherwise
+ * Return: 0 on success and < 0 otherwise
  */
 int mei_txe_aliveness_set_sync(struct mei_device *dev, u32 req)
 {
@@ -289,7 +306,7 @@ int mei_txe_aliveness_set_sync(struct mei_device *dev, u32 req)
  *
  * @dev: the device structure
  *
- * returns: true is pg supported, false otherwise
+ * Return: true is pg supported, false otherwise
  */
 static bool mei_txe_pg_is_enabled(struct mei_device *dev)
 {
@@ -302,11 +319,12 @@ static bool mei_txe_pg_is_enabled(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns: MEI_PG_OFF if aliveness is on and MEI_PG_ON otherwise
+ * Return: MEI_PG_OFF if aliveness is on and MEI_PG_ON otherwise
  */
 static inline enum mei_pg_state mei_txe_pg_state(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	return hw->aliveness ? MEI_PG_OFF : MEI_PG_ON;
 }
 
@@ -326,9 +344,10 @@ static void mei_txe_input_ready_interrupt_enable(struct mei_device *dev)
 }
 
 /**
- * mei_txe_input_doorbell_set
- *   - Sets bit 0 in SEC_IPC_INPUT_DOORBELL.IPC_INPUT_DOORBELL.
- * @dev: the device structure
+ * mei_txe_input_doorbell_set - sets bit 0 in
+ *    SEC_IPC_INPUT_DOORBELL.IPC_INPUT_DOORBELL.
+ *
+ * @hw: the txe hardware structure
  */
 static void mei_txe_input_doorbell_set(struct mei_txe_hw *hw)
 {
@@ -340,7 +359,7 @@ static void mei_txe_input_doorbell_set(struct mei_txe_hw *hw)
 /**
  * mei_txe_output_ready_set - Sets the SICR_SEC_IPC_OUTPUT_STATUS bit to 1
  *
- * @dev: the device structure
+ * @hw: the txe hardware structure
  */
 static void mei_txe_output_ready_set(struct mei_txe_hw *hw)
 {
@@ -353,11 +372,14 @@ static void mei_txe_output_ready_set(struct mei_txe_hw *hw)
  * mei_txe_is_input_ready - check if TXE is ready for receiving data
  *
  * @dev: the device structure
+ *
+ * Return: true if INPUT STATUS READY bit is set
  */
 static bool mei_txe_is_input_ready(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
 	u32 status;
+
 	status = mei_txe_sec_reg_read(hw, SEC_IPC_INPUT_STATUS_REG);
 	return !!(SEC_IPC_INPUT_STATUS_RDY & status);
 }
@@ -370,6 +392,7 @@ static bool mei_txe_is_input_ready(struct mei_device *dev)
 static inline void mei_txe_intr_clear(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	mei_txe_sec_reg_write_silent(hw, SEC_IPC_HOST_INT_STATUS_REG,
 		SEC_IPC_HOST_INT_STATUS_PENDING);
 	mei_txe_br_reg_write(hw, HISR_REG, HISR_INT_STS_MSK);
@@ -384,6 +407,7 @@ static inline void mei_txe_intr_clear(struct mei_device *dev)
 static void mei_txe_intr_disable(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	mei_txe_br_reg_write(hw, HHIER_REG, 0);
 	mei_txe_br_reg_write(hw, HIER_REG, 0);
 }
@@ -395,6 +419,7 @@ static void mei_txe_intr_disable(struct mei_device *dev)
 static void mei_txe_intr_enable(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	mei_txe_br_reg_write(hw, HHIER_REG, IPC_HHIER_MSK);
 	mei_txe_br_reg_write(hw, HIER_REG, HIER_INT_EN_MSK);
 }
@@ -407,6 +432,8 @@ static void mei_txe_intr_enable(struct mei_device *dev)
  *
  * Checks if there are pending interrupts
  * only Aliveness, Readiness, Input ready, and Output doorbell are relevant
+ *
+ * Return: true if there are pending interrupts
  */
 static bool mei_txe_pending_interrupts(struct mei_device *dev)
 {
@@ -418,7 +445,7 @@ static bool mei_txe_pending_interrupts(struct mei_device *dev)
 				      TXE_INTR_OUT_DB));
 
 	if (ret) {
-		dev_dbg(&dev->pdev->dev,
+		dev_dbg(dev->dev,
 			"Pending Interrupts InReady=%01d Readiness=%01d, Aliveness=%01d, OutDoor=%01d\n",
 			!!(hw->intr_cause & TXE_INTR_IN_READY),
 			!!(hw->intr_cause & TXE_INTR_READINESS),
@@ -440,6 +467,7 @@ static void mei_txe_input_payload_write(struct mei_device *dev,
 			unsigned long idx, u32 value)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	mei_txe_sec_reg_write(hw, SEC_IPC_INPUT_PAYLOAD_REG +
 			(idx * sizeof(u32)), value);
 }
@@ -451,12 +479,13 @@ static void mei_txe_input_payload_write(struct mei_device *dev,
  * @dev: the device structure
  * @idx: index in the device buffer
  *
- * returns register value at index
+ * Return: register value at index
  */
 static u32 mei_txe_out_data_read(const struct mei_device *dev,
 					unsigned long idx)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	return mei_txe_br_reg_read(hw,
 		BRIDGE_IPC_OUTPUT_PAYLOAD_REG + (idx * sizeof(u32)));
 }
@@ -464,26 +493,28 @@ static u32 mei_txe_out_data_read(const struct mei_device *dev,
 /* Readiness */
 
 /**
- * mei_txe_readiness_set_host_rdy
+ * mei_txe_readiness_set_host_rdy - set host readiness bit
  *
  * @dev: the device structure
  */
 static void mei_txe_readiness_set_host_rdy(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	mei_txe_br_reg_write(hw,
 		SICR_HOST_IPC_READINESS_REQ_REG,
 		SICR_HOST_IPC_READINESS_HOST_RDY);
 }
 
 /**
- * mei_txe_readiness_clear
+ * mei_txe_readiness_clear - clear host readiness bit
  *
  * @dev: the device structure
  */
 static void mei_txe_readiness_clear(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	mei_txe_br_reg_write(hw, SICR_HOST_IPC_READINESS_REQ_REG,
 				SICR_HOST_IPC_READINESS_RDY_CLR);
 }
@@ -492,10 +523,13 @@ static void mei_txe_readiness_clear(struct mei_device *dev)
  *	the HICR_SEC_IPC_READINESS register value
  *
  * @dev: the device structure
+ *
+ * Return: the HICR_SEC_IPC_READINESS register value
  */
 static u32 mei_txe_readiness_get(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	return mei_txe_br_reg_read(hw, HICR_SEC_IPC_READINESS_REG);
 }
 
@@ -504,7 +538,9 @@ static u32 mei_txe_readiness_get(struct mei_device *dev)
  * mei_txe_readiness_is_sec_rdy - check readiness
  *  for HICR_SEC_IPC_READINESS_SEC_RDY
  *
- * @readiness - cached readiness state
+ * @readiness: cached readiness state
+ *
+ * Return: true if readiness bit is set
  */
 static inline bool mei_txe_readiness_is_sec_rdy(u32 readiness)
 {
@@ -515,10 +551,13 @@ static inline bool mei_txe_readiness_is_sec_rdy(u32 readiness)
  * mei_txe_hw_is_ready - check if the hw is ready
  *
  * @dev: the device structure
+ *
+ * Return: true if sec is ready
  */
 static bool mei_txe_hw_is_ready(struct mei_device *dev)
 {
 	u32 readiness =  mei_txe_readiness_get(dev);
+
 	return mei_txe_readiness_is_sec_rdy(readiness);
 }
 
@@ -526,11 +565,14 @@ static bool mei_txe_hw_is_ready(struct mei_device *dev)
  * mei_txe_host_is_ready - check if the host is ready
  *
  * @dev: the device structure
+ *
+ * Return: true if host is ready
  */
 static inline bool mei_txe_host_is_ready(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
 	u32 reg = mei_txe_br_reg_read(hw, HICR_SEC_IPC_READINESS_REG);
+
 	return !!(reg & HICR_SEC_IPC_READINESS_HOST_RDY);
 }
 
@@ -539,7 +581,7 @@ static inline bool mei_txe_host_is_ready(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns 0 on success and -ETIME on timeout
+ * Return: 0 on success and -ETIME on timeout
  */
 static int mei_txe_readiness_wait(struct mei_device *dev)
 {
@@ -551,7 +593,7 @@ static int mei_txe_readiness_wait(struct mei_device *dev)
 			msecs_to_jiffies(SEC_RESET_WAIT_TIMEOUT));
 	mutex_lock(&dev->device_lock);
 	if (!dev->recvd_hw_ready) {
-		dev_err(&dev->pdev->dev, "wait for readiness failed\n");
+		dev_err(dev->dev, "wait for readiness failed\n");
 		return -ETIME;
 	}
 
@@ -559,6 +601,42 @@ static int mei_txe_readiness_wait(struct mei_device *dev)
 	return 0;
 }
 
+static const struct mei_fw_status mei_txe_fw_sts = {
+	.count = 2,
+	.status[0] = PCI_CFG_TXE_FW_STS0,
+	.status[1] = PCI_CFG_TXE_FW_STS1
+};
+
+/**
+ * mei_txe_fw_status - read fw status register from pci config space
+ *
+ * @dev: mei device
+ * @fw_status: fw status register values
+ *
+ * Return: 0 on success, error otherwise
+ */
+static int mei_txe_fw_status(struct mei_device *dev,
+			     struct mei_fw_status *fw_status)
+{
+	const struct mei_fw_status *fw_src = &mei_txe_fw_sts;
+	struct pci_dev *pdev = to_pci_dev(dev->dev);
+	int ret;
+	int i;
+
+	if (!fw_status)
+		return -EINVAL;
+
+	fw_status->count = fw_src->count;
+	for (i = 0; i < fw_src->count && i < MEI_FW_STATUS_MAX; i++) {
+		ret = pci_read_config_dword(pdev,
+			fw_src->status[i], &fw_status->status[i]);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
 /**
  *  mei_txe_hw_config - configure hardware at the start of the devices
  *
@@ -571,13 +649,14 @@ static void mei_txe_hw_config(struct mei_device *dev)
 {
 
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	/* Doesn't change in runtime */
 	dev->hbuf_depth = PAYLOAD_SIZE / 4;
 
 	hw->aliveness = mei_txe_aliveness_get(dev);
 	hw->readiness = mei_txe_readiness_get(dev);
 
-	dev_dbg(&dev->pdev->dev, "aliveness_resp = 0x%08x, readiness = 0x%08x.\n",
+	dev_dbg(dev->dev, "aliveness_resp = 0x%08x, readiness = 0x%08x.\n",
 		hw->aliveness, hw->readiness);
 }
 
@@ -588,7 +667,8 @@ static void mei_txe_hw_config(struct mei_device *dev)
  * @dev: the device structure
  * @header: header of message
  * @buf: message buffer will be written
- * returns 1 if success, 0 - otherwise.
+ *
+ * Return: 0 if success, <0 - otherwise.
  */
 
 static int mei_txe_write(struct mei_device *dev,
@@ -607,7 +687,7 @@ static int mei_txe_write(struct mei_device *dev,
 
 	length = header->length;
 
-	dev_dbg(&dev->pdev->dev, MEI_HDR_FMT, MEI_HDR_PRM(header));
+	dev_dbg(dev->dev, MEI_HDR_FMT, MEI_HDR_PRM(header));
 
 	dw_cnt = mei_data2slots(length);
 	if (dw_cnt > slots)
@@ -621,8 +701,9 @@ static int mei_txe_write(struct mei_device *dev,
 
 	if (!mei_txe_is_input_ready(dev)) {
 		struct mei_fw_status fw_status;
+
 		mei_fw_status(dev, &fw_status);
-		dev_err(&dev->pdev->dev, "Input is not ready " FW_STS_FMT "\n",
+		dev_err(dev->dev, "Input is not ready " FW_STS_FMT "\n",
 			FW_STS_PRM(fw_status));
 		return -EAGAIN;
 	}
@@ -635,6 +716,7 @@ static int mei_txe_write(struct mei_device *dev,
 	rem = length & 0x3;
 	if (rem > 0) {
 		u32 reg = 0;
+
 		memcpy(&reg, &buf[length - rem], rem);
 		mei_txe_input_payload_write(dev, i + 1, reg);
 	}
@@ -653,7 +735,7 @@ static int mei_txe_write(struct mei_device *dev,
  *
  * @dev: the device structure
  *
- * returns the PAYLOAD_SIZE - 4
+ * Return: the PAYLOAD_SIZE - 4
  */
 static size_t mei_txe_hbuf_max_len(const struct mei_device *dev)
 {
@@ -665,11 +747,12 @@ static size_t mei_txe_hbuf_max_len(const struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns always hbuf_depth
+ * Return: always hbuf_depth
  */
 static int mei_txe_hbuf_empty_slots(struct mei_device *dev)
 {
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+
 	return hw->slots;
 }
 
@@ -678,7 +761,7 @@ static int mei_txe_hbuf_empty_slots(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns always buffer size in dwords count
+ * Return: always buffer size in dwords count
  */
 static int mei_txe_count_full_read_slots(struct mei_device *dev)
 {
@@ -691,7 +774,7 @@ static int mei_txe_count_full_read_slots(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns mei message header
+ * Return: mei message header
  */
 
 static u32 mei_txe_read_hdr(const struct mei_device *dev)
@@ -705,33 +788,35 @@ static u32 mei_txe_read_hdr(const struct mei_device *dev)
  * @buf: message buffer will be written
  * @len: message size will be read
  *
- * returns -EINVAL on error wrong argument and 0 on success
+ * Return: -EINVAL on error wrong argument and 0 on success
  */
 static int mei_txe_read(struct mei_device *dev,
 		unsigned char *buf, unsigned long len)
 {
 
 	struct mei_txe_hw *hw = to_txe_hw(dev);
+	u32 *reg_buf, reg;
+	u32 rem;
 	u32 i;
-	u32 *reg_buf = (u32 *)buf;
-	u32 rem = len & 0x3;
 
 	if (WARN_ON(!buf || !len))
 		return -EINVAL;
 
-	dev_dbg(&dev->pdev->dev,
-		"buffer-length = %lu buf[0]0x%08X\n",
+	reg_buf = (u32 *)buf;
+	rem = len & 0x3;
+
+	dev_dbg(dev->dev, "buffer-length = %lu buf[0]0x%08X\n",
 		len, mei_txe_out_data_read(dev, 0));
 
 	for (i = 0; i < len / 4; i++) {
 		/* skip header: index starts from 1 */
-		u32 reg = mei_txe_out_data_read(dev, i + 1);
-		dev_dbg(&dev->pdev->dev, "buf[%d] = 0x%08X\n", i, reg);
+		reg = mei_txe_out_data_read(dev, i + 1);
+		dev_dbg(dev->dev, "buf[%d] = 0x%08X\n", i, reg);
 		*reg_buf++ = reg;
 	}
 
 	if (rem) {
-		u32 reg = mei_txe_out_data_read(dev, i + 1);
+		reg = mei_txe_out_data_read(dev, i + 1);
 		memcpy(reg_buf, &reg, rem);
 	}
 
@@ -745,7 +830,7 @@ static int mei_txe_read(struct mei_device *dev,
  * @dev: the device structure
  * @intr_enable: if interrupt should be enabled after reset.
  *
- * returns 0 on success and < 0 in case of error
+ * Return: 0 on success and < 0 in case of error
  */
 static int mei_txe_hw_reset(struct mei_device *dev, bool intr_enable)
 {
@@ -771,8 +856,7 @@ static int mei_txe_hw_reset(struct mei_device *dev, bool intr_enable)
 	 */
 	if (aliveness_req != hw->aliveness)
 		if (mei_txe_aliveness_poll(dev, aliveness_req) < 0) {
-			dev_err(&dev->pdev->dev,
-				"wait for aliveness settle failed ... bailing out\n");
+			dev_err(dev->dev, "wait for aliveness settle failed ... bailing out\n");
 			return -EIO;
 		}
 
@@ -782,14 +866,13 @@ static int mei_txe_hw_reset(struct mei_device *dev, bool intr_enable)
 	if (aliveness_req) {
 		mei_txe_aliveness_set(dev, 0);
 		if (mei_txe_aliveness_poll(dev, 0) < 0) {
-			dev_err(&dev->pdev->dev,
-				"wait for aliveness failed ... bailing out\n");
+			dev_err(dev->dev, "wait for aliveness failed ... bailing out\n");
 			return -EIO;
 		}
 	}
 
 	/*
-	 * Set rediness RDY_CLR bit
+	 * Set readiness RDY_CLR bit
 	 */
 	mei_txe_readiness_clear(dev);
 
@@ -801,7 +884,7 @@ static int mei_txe_hw_reset(struct mei_device *dev, bool intr_enable)
  *
  * @dev: the device structure
  *
- * returns 0 on success and < 0 in case of error
+ * Return: 0 on success an error code otherwise
  */
 static int mei_txe_hw_start(struct mei_device *dev)
 {
@@ -815,7 +898,7 @@ static int mei_txe_hw_start(struct mei_device *dev)
 
 	ret = mei_txe_readiness_wait(dev);
 	if (ret < 0) {
-		dev_err(&dev->pdev->dev, "wating for readiness failed\n");
+		dev_err(dev->dev, "waiting for readiness failed\n");
 		return ret;
 	}
 
@@ -831,7 +914,7 @@ static int mei_txe_hw_start(struct mei_device *dev)
 
 	ret = mei_txe_aliveness_set_sync(dev, 1);
 	if (ret < 0) {
-		dev_err(&dev->pdev->dev, "wait for aliveness failed ... bailing out\n");
+		dev_err(dev->dev, "wait for aliveness failed ... bailing out\n");
 		return ret;
 	}
 
@@ -857,6 +940,8 @@ static int mei_txe_hw_start(struct mei_device *dev)
  *
  * @dev: the device structure
  * @do_ack: acknowledge interrupts
+ *
+ * Return: true if found interrupts to process.
  */
 static bool mei_txe_check_and_ack_intrs(struct mei_device *dev, bool do_ack)
 {
@@ -912,7 +997,8 @@ out:
  * @irq: The irq number
  * @dev_id: pointer to the device structure
  *
- * returns irqreturn_t
+ * Return: IRQ_WAKE_THREAD if interrupt is designed for the device
+ *         IRQ_NONE otherwise
  */
 irqreturn_t mei_txe_irq_quick_handler(int irq, void *dev_id)
 {
@@ -930,8 +1016,7 @@ irqreturn_t mei_txe_irq_quick_handler(int irq, void *dev_id)
  * @irq: The irq number
  * @dev_id: pointer to the device structure
  *
- * returns irqreturn_t
- *
+ * Return: IRQ_HANDLED
  */
 irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 {
@@ -941,7 +1026,7 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 	s32 slots;
 	int rets = 0;
 
-	dev_dbg(&dev->pdev->dev, "irq thread: Interrupt Registers HHISR|HISR|SEC=%02X|%04X|%02X\n",
+	dev_dbg(dev->dev, "irq thread: Interrupt Registers HHISR|HISR|SEC=%02X|%04X|%02X\n",
 		mei_txe_br_reg_read(hw, HHISR_REG),
 		mei_txe_br_reg_read(hw, HISR_REG),
 		mei_txe_sec_reg_read_silent(hw, SEC_IPC_HOST_INT_STATUS_REG));
@@ -951,7 +1036,7 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 	mutex_lock(&dev->device_lock);
 	mei_io_list_init(&complete_list);
 
-	if (pci_dev_msi_enabled(dev->pdev))
+	if (pci_dev_msi_enabled(to_pci_dev(dev->dev)))
 		mei_txe_check_and_ack_intrs(dev, true);
 
 	/* show irq events */
@@ -965,17 +1050,17 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 	 * or TXE driver resetting the HECI interface.
 	 */
 	if (test_and_clear_bit(TXE_INTR_READINESS_BIT, &hw->intr_cause)) {
-		dev_dbg(&dev->pdev->dev, "Readiness Interrupt was received...\n");
+		dev_dbg(dev->dev, "Readiness Interrupt was received...\n");
 
 		/* Check if SeC is going through reset */
 		if (mei_txe_readiness_is_sec_rdy(hw->readiness)) {
-			dev_dbg(&dev->pdev->dev, "we need to start the dev.\n");
+			dev_dbg(dev->dev, "we need to start the dev.\n");
 			dev->recvd_hw_ready = true;
 		} else {
 			dev->recvd_hw_ready = false;
 			if (dev->dev_state != MEI_DEV_RESETTING) {
 
-				dev_warn(&dev->pdev->dev, "FW not ready: resetting.\n");
+				dev_warn(dev->dev, "FW not ready: resetting.\n");
 				schedule_work(&dev->reset_work);
 				goto end;
 
@@ -992,7 +1077,7 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 
 	if (test_and_clear_bit(TXE_INTR_ALIVENESS_BIT, &hw->intr_cause)) {
 		/* Clear the interrupt cause */
-		dev_dbg(&dev->pdev->dev,
+		dev_dbg(dev->dev,
 			"Aliveness Interrupt: Status: %d\n", hw->aliveness);
 		dev->pg_event = MEI_PG_EVENT_RECEIVED;
 		if (waitqueue_active(&hw->wait_aliveness_resp))
@@ -1008,7 +1093,7 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 		/* Read from TXE */
 		rets = mei_irq_read_handler(dev, &complete_list, &slots);
 		if (rets && dev->dev_state != MEI_DEV_RESETTING) {
-			dev_err(&dev->pdev->dev,
+			dev_err(dev->dev,
 				"mei_irq_read_handler ret = %d.\n", rets);
 
 			schedule_work(&dev->reset_work);
@@ -1026,7 +1111,7 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 		dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
 		rets = mei_irq_write_handler(dev, &complete_list);
 		if (rets && rets != -EMSGSIZE)
-			dev_err(&dev->pdev->dev, "mei_irq_write_handler ret = %d.\n",
+			dev_err(dev->dev, "mei_irq_write_handler ret = %d.\n",
 				rets);
 		dev->hbuf_is_ready = mei_hbuf_is_ready(dev);
 	}
@@ -1034,7 +1119,7 @@ irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id)
 	mei_irq_compl_handler(dev, &complete_list);
 
 end:
-	dev_dbg(&dev->pdev->dev, "interrupt thread end ret = %d\n", rets);
+	dev_dbg(dev->dev, "interrupt thread end ret = %d\n", rets);
 
 	mutex_unlock(&dev->device_lock);
 
@@ -1046,6 +1131,7 @@ static const struct mei_hw_ops mei_txe_hw_ops = {
 
 	.host_is_ready = mei_txe_host_is_ready,
 
+	.fw_status = mei_txe_fw_status,
 	.pg_state = mei_txe_pg_state,
 
 	.hw_is_ready = mei_txe_hw_is_ready,
@@ -1072,27 +1158,14 @@ static const struct mei_hw_ops mei_txe_hw_ops = {
 
 };
 
-#define MEI_CFG_TXE_FW_STS                            \
-	.fw_status.count = 2,                         \
-	.fw_status.status[0] = PCI_CFG_TXE_FW_STS0,   \
-	.fw_status.status[1] = PCI_CFG_TXE_FW_STS1
-
-const struct mei_cfg mei_txe_cfg = {
-	MEI_CFG_TXE_FW_STS,
-};
-
-
 /**
  * mei_txe_dev_init - allocates and initializes txe hardware specific structure
  *
- * @pdev - pci device
- * @cfg - per device generation config
- *
- * returns struct mei_device * on success or NULL;
+ * @pdev: pci device
  *
+ * Return: struct mei_device * on success or NULL
  */
-struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
-				    const struct mei_cfg *cfg)
+struct mei_device *mei_txe_dev_init(struct pci_dev *pdev)
 {
 	struct mei_device *dev;
 	struct mei_txe_hw *hw;
@@ -1102,15 +1175,12 @@ struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
 	if (!dev)
 		return NULL;
 
-	mei_device_init(dev, cfg);
+	mei_device_init(dev, &pdev->dev, &mei_txe_hw_ops);
 
 	hw = to_txe_hw(dev);
 
 	init_waitqueue_head(&hw->wait_aliveness_resp);
 
-	dev->ops = &mei_txe_hw_ops;
-
-	dev->pdev = pdev;
 	return dev;
 }
 
@@ -1120,6 +1190,8 @@ struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
  * @dev:   the device structure
  * @addr:  physical address start of the range
  * @range: physical range size
+ *
+ * Return: 0 on success an error code otherwise
  */
 int mei_txe_setup_satt2(struct mei_device *dev, phys_addr_t addr, u32 range)
 {
@@ -1151,7 +1223,7 @@ int mei_txe_setup_satt2(struct mei_device *dev, phys_addr_t addr, u32 range)
 	mei_txe_br_reg_write(hw, SATT2_SAP_SIZE_REG, range);
 	mei_txe_br_reg_write(hw, SATT2_BRG_BA_LSB_REG, lo32);
 	mei_txe_br_reg_write(hw, SATT2_CTRL_REG, ctrl);
-	dev_dbg(&dev->pdev->dev, "SATT2: SAP_SIZE_OFFSET=0x%08X, BRG_BA_LSB_OFFSET=0x%08X, CTRL_OFFSET=0x%08X\n",
+	dev_dbg(dev->dev, "SATT2: SAP_SIZE_OFFSET=0x%08X, BRG_BA_LSB_OFFSET=0x%08X, CTRL_OFFSET=0x%08X\n",
 		range, lo32, ctrl);
 
 	return 0;
diff --git a/drivers/misc/mei/hw-txe.h b/drivers/misc/mei/hw-txe.h
index e244af79167f..ce3ed0b88b0c 100644
--- a/drivers/misc/mei/hw-txe.h
+++ b/drivers/misc/mei/hw-txe.h
@@ -40,6 +40,7 @@
  * @mem_addr:            SeC and BRIDGE bars
  * @aliveness:           aliveness (power gating) state of the hardware
  * @readiness:           readiness state of the hardware
+ * @slots:               number of empty slots
  * @wait_aliveness_resp: aliveness wait queue
  * @intr_cause:          translated interrupt cause
  */
@@ -61,10 +62,7 @@ static inline struct mei_device *hw_txe_to_mei(struct mei_txe_hw *hw)
 	return container_of((void *)hw, struct mei_device, hw);
 }
 
-extern const struct mei_cfg mei_txe_cfg;
-
-struct mei_device *mei_txe_dev_init(struct pci_dev *pdev,
-	const struct mei_cfg *cfg);
+struct mei_device *mei_txe_dev_init(struct pci_dev *pdev);
 
 irqreturn_t mei_txe_irq_quick_handler(int irq, void *dev_id);
 irqreturn_t mei_txe_irq_thread_handler(int irq, void *dev_id);
diff --git a/drivers/misc/mei/hw.h b/drivers/misc/mei/hw.h
index dd448e58cc87..16fef6dc4dd7 100644
--- a/drivers/misc/mei/hw.h
+++ b/drivers/misc/mei/hw.h
@@ -97,23 +97,52 @@ enum mei_stop_reason_types {
 	SYSTEM_S5_ENTRY = 0x08
 };
 
+
+/**
+ * enum mei_hbm_status  - mei host bus messages return values
+ *
+ * @MEI_HBMS_SUCCESS           : status success
+ * @MEI_HBMS_CLIENT_NOT_FOUND  : client not found
+ * @MEI_HBMS_ALREADY_EXISTS    : connection already established
+ * @MEI_HBMS_REJECTED          : connection is rejected
+ * @MEI_HBMS_INVALID_PARAMETER : invalid parameter
+ * @MEI_HBMS_NOT_ALLOWED       : operation not allowed
+ * @MEI_HBMS_ALREADY_STARTED   : system is already started
+ * @MEI_HBMS_NOT_STARTED       : system not started
+ *
+ * @MEI_HBMS_MAX               : sentinel
+ */
+enum mei_hbm_status {
+	MEI_HBMS_SUCCESS           = 0,
+	MEI_HBMS_CLIENT_NOT_FOUND  = 1,
+	MEI_HBMS_ALREADY_EXISTS    = 2,
+	MEI_HBMS_REJECTED          = 3,
+	MEI_HBMS_INVALID_PARAMETER = 4,
+	MEI_HBMS_NOT_ALLOWED       = 5,
+	MEI_HBMS_ALREADY_STARTED   = 6,
+	MEI_HBMS_NOT_STARTED       = 7,
+
+	MEI_HBMS_MAX
+};
+
+
 /*
  * Client Connect Status
  * used by hbm_client_connect_response.status
  */
 enum mei_cl_connect_status {
-	MEI_CL_CONN_SUCCESS          = 0x00,
-	MEI_CL_CONN_NOT_FOUND        = 0x01,
-	MEI_CL_CONN_ALREADY_STARTED  = 0x02,
-	MEI_CL_CONN_OUT_OF_RESOURCES = 0x03,
-	MEI_CL_CONN_MESSAGE_SMALL    = 0x04
+	MEI_CL_CONN_SUCCESS          = MEI_HBMS_SUCCESS,
+	MEI_CL_CONN_NOT_FOUND        = MEI_HBMS_CLIENT_NOT_FOUND,
+	MEI_CL_CONN_ALREADY_STARTED  = MEI_HBMS_ALREADY_EXISTS,
+	MEI_CL_CONN_OUT_OF_RESOURCES = MEI_HBMS_REJECTED,
+	MEI_CL_CONN_MESSAGE_SMALL    = MEI_HBMS_INVALID_PARAMETER,
 };
 
 /*
  * Client Disconnect Status
  */
 enum  mei_cl_disconnect_status {
-	MEI_CL_DISCONN_SUCCESS = 0x00
+	MEI_CL_DISCONN_SUCCESS = MEI_HBMS_SUCCESS
 };
 
 /*
@@ -138,10 +167,10 @@ struct mei_bus_message {
  * struct hbm_cl_cmd - client specific host bus command
  *	CONNECT, DISCONNECT, and FlOW CONTROL
  *
- * @hbm_cmd - bus message command header
- * @me_addr - address of the client in ME
- * @host_addr - address of the client in the driver
- * @data
+ * @hbm_cmd: bus message command header
+ * @me_addr: address of the client in ME
+ * @host_addr: address of the client in the driver
+ * @data: generic data
  */
 struct mei_hbm_cl_cmd {
 	u8 hbm_cmd;
@@ -206,14 +235,13 @@ struct mei_client_properties {
 
 struct hbm_props_request {
 	u8 hbm_cmd;
-	u8 address;
+	u8 me_addr;
 	u8 reserved[2];
 } __packed;
 
-
 struct hbm_props_response {
 	u8 hbm_cmd;
-	u8 address;
+	u8 me_addr;
 	u8 status;
 	u8 reserved[1];
 	struct mei_client_properties client_properties;
@@ -222,8 +250,8 @@ struct hbm_props_response {
 /**
  * struct hbm_power_gate - power gate request/response
  *
- * @hbm_cmd - bus message command header
- * @reserved[3]
+ * @hbm_cmd: bus message command header
+ * @reserved: reserved
  */
 struct hbm_power_gate {
 	u8 hbm_cmd;
@@ -233,10 +261,10 @@ struct hbm_power_gate {
 /**
  * struct hbm_client_connect_request - connect/disconnect request
  *
- * @hbm_cmd - bus message command header
- * @me_addr - address of the client in ME
- * @host_addr - address of the client in the driver
- * @reserved
+ * @hbm_cmd: bus message command header
+ * @me_addr: address of the client in ME
+ * @host_addr: address of the client in the driver
+ * @reserved: reserved
  */
 struct hbm_client_connect_request {
 	u8 hbm_cmd;
@@ -248,10 +276,10 @@ struct hbm_client_connect_request {
 /**
  * struct hbm_client_connect_response - connect/disconnect response
  *
- * @hbm_cmd - bus message command header
- * @me_addr - address of the client in ME
- * @host_addr - address of the client in the driver
- * @status - status of the request
+ * @hbm_cmd: bus message command header
+ * @me_addr: address of the client in ME
+ * @host_addr: address of the client in the driver
+ * @status: status of the request
  */
 struct hbm_client_connect_response {
 	u8 hbm_cmd;
diff --git a/drivers/misc/mei/init.c b/drivers/misc/mei/init.c
index 006929222481..7901d076c127 100644
--- a/drivers/misc/mei/init.c
+++ b/drivers/misc/mei/init.c
@@ -15,7 +15,6 @@
  */
 
 #include <linux/export.h>
-#include <linux/pci.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/delay.h>
@@ -43,13 +42,23 @@ const char *mei_dev_state_str(int state)
 #undef MEI_DEV_STATE
 }
 
+const char *mei_pg_state_str(enum mei_pg_state state)
+{
+#define MEI_PG_STATE(state) case MEI_PG_##state: return #state
+	switch (state) {
+	MEI_PG_STATE(OFF);
+	MEI_PG_STATE(ON);
+	default:
+		return "unknown";
+	}
+#undef MEI_PG_STATE
+}
+
 
 /**
- * mei_cancel_work. Cancel mei background jobs
+ * mei_cancel_work - Cancel mei background jobs
  *
  * @dev: the device structure
- *
- * returns 0 on success or < 0 if the reset hasn't succeeded
  */
 void mei_cancel_work(struct mei_device *dev)
 {
@@ -64,6 +73,8 @@ EXPORT_SYMBOL_GPL(mei_cancel_work);
  * mei_reset - resets host and fw.
  *
  * @dev: the device structure
+ *
+ * Return: 0 on success or < 0 if the reset hasn't succeeded
  */
 int mei_reset(struct mei_device *dev)
 {
@@ -76,8 +87,9 @@ int mei_reset(struct mei_device *dev)
 	    state != MEI_DEV_POWER_DOWN &&
 	    state != MEI_DEV_POWER_UP) {
 		struct mei_fw_status fw_status;
+
 		mei_fw_status(dev, &fw_status);
-		dev_warn(&dev->pdev->dev,
+		dev_warn(dev->dev,
 			"unexpected reset: dev_state = %s " FW_STS_FMT "\n",
 			mei_dev_state_str(state), FW_STS_PRM(fw_status));
 	}
@@ -95,7 +107,7 @@ int mei_reset(struct mei_device *dev)
 
 	dev->reset_count++;
 	if (dev->reset_count > MEI_MAX_CONSEC_RESET) {
-		dev_err(&dev->pdev->dev, "reset: reached maximal consecutive resets: disabling the device\n");
+		dev_err(dev->dev, "reset: reached maximal consecutive resets: disabling the device\n");
 		dev->dev_state = MEI_DEV_DISABLED;
 		return -ENODEV;
 	}
@@ -116,7 +128,7 @@ int mei_reset(struct mei_device *dev)
 		mei_cl_all_wakeup(dev);
 
 		/* remove entry if already in list */
-		dev_dbg(&dev->pdev->dev, "remove iamthif and wd from the file list.\n");
+		dev_dbg(dev->dev, "remove iamthif and wd from the file list.\n");
 		mei_cl_unlink(&dev->wd_cl);
 		mei_cl_unlink(&dev->iamthif_cl);
 		mei_amthif_reset_params(dev);
@@ -128,28 +140,28 @@ int mei_reset(struct mei_device *dev)
 	dev->wd_pending = false;
 
 	if (ret) {
-		dev_err(&dev->pdev->dev, "hw_reset failed ret = %d\n", ret);
+		dev_err(dev->dev, "hw_reset failed ret = %d\n", ret);
 		return ret;
 	}
 
 	if (state == MEI_DEV_POWER_DOWN) {
-		dev_dbg(&dev->pdev->dev, "powering down: end of reset\n");
+		dev_dbg(dev->dev, "powering down: end of reset\n");
 		dev->dev_state = MEI_DEV_DISABLED;
 		return 0;
 	}
 
 	ret = mei_hw_start(dev);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "hw_start failed ret = %d\n", ret);
+		dev_err(dev->dev, "hw_start failed ret = %d\n", ret);
 		return ret;
 	}
 
-	dev_dbg(&dev->pdev->dev, "link is established start sending messages.\n");
+	dev_dbg(dev->dev, "link is established start sending messages.\n");
 
 	dev->dev_state = MEI_DEV_INIT_CLIENTS;
 	ret = mei_hbm_start_req(dev);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "hbm_start failed ret = %d\n", ret);
+		dev_err(dev->dev, "hbm_start failed ret = %d\n", ret);
 		dev->dev_state = MEI_DEV_RESETTING;
 		return ret;
 	}
@@ -163,11 +175,12 @@ EXPORT_SYMBOL_GPL(mei_reset);
  *
  * @dev: the device structure
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 int mei_start(struct mei_device *dev)
 {
 	int ret;
+
 	mutex_lock(&dev->device_lock);
 
 	/* acknowledge interrupt and stop interrupts */
@@ -175,7 +188,7 @@ int mei_start(struct mei_device *dev)
 
 	mei_hw_config(dev);
 
-	dev_dbg(&dev->pdev->dev, "reset in start the mei device.\n");
+	dev_dbg(dev->dev, "reset in start the mei device.\n");
 
 	dev->reset_count = 0;
 	do {
@@ -183,43 +196,43 @@ int mei_start(struct mei_device *dev)
 		ret = mei_reset(dev);
 
 		if (ret == -ENODEV || dev->dev_state == MEI_DEV_DISABLED) {
-			dev_err(&dev->pdev->dev, "reset failed ret = %d", ret);
+			dev_err(dev->dev, "reset failed ret = %d", ret);
 			goto err;
 		}
 	} while (ret);
 
 	/* we cannot start the device w/o hbm start message completed */
 	if (dev->dev_state == MEI_DEV_DISABLED) {
-		dev_err(&dev->pdev->dev, "reset failed");
+		dev_err(dev->dev, "reset failed");
 		goto err;
 	}
 
 	if (mei_hbm_start_wait(dev)) {
-		dev_err(&dev->pdev->dev, "HBM haven't started");
+		dev_err(dev->dev, "HBM haven't started");
 		goto err;
 	}
 
 	if (!mei_host_is_ready(dev)) {
-		dev_err(&dev->pdev->dev, "host is not ready.\n");
+		dev_err(dev->dev, "host is not ready.\n");
 		goto err;
 	}
 
 	if (!mei_hw_is_ready(dev)) {
-		dev_err(&dev->pdev->dev, "ME is not ready.\n");
+		dev_err(dev->dev, "ME is not ready.\n");
 		goto err;
 	}
 
 	if (!mei_hbm_version_is_supported(dev)) {
-		dev_dbg(&dev->pdev->dev, "MEI start failed.\n");
+		dev_dbg(dev->dev, "MEI start failed.\n");
 		goto err;
 	}
 
-	dev_dbg(&dev->pdev->dev, "link layer has been established.\n");
+	dev_dbg(dev->dev, "link layer has been established.\n");
 
 	mutex_unlock(&dev->device_lock);
 	return 0;
 err:
-	dev_err(&dev->pdev->dev, "link layer initialization failed.\n");
+	dev_err(dev->dev, "link layer initialization failed.\n");
 	dev->dev_state = MEI_DEV_DISABLED;
 	mutex_unlock(&dev->device_lock);
 	return -ENODEV;
@@ -231,7 +244,7 @@ EXPORT_SYMBOL_GPL(mei_start);
  *
  * @dev: the device structure
  *
- * returns 0 on success or -ENODEV if the restart hasn't succeeded
+ * Return: 0 on success or -ENODEV if the restart hasn't succeeded
  */
 int mei_restart(struct mei_device *dev)
 {
@@ -249,7 +262,7 @@ int mei_restart(struct mei_device *dev)
 	mutex_unlock(&dev->device_lock);
 
 	if (err == -ENODEV || dev->dev_state == MEI_DEV_DISABLED) {
-		dev_err(&dev->pdev->dev, "device disabled = %d\n", err);
+		dev_err(dev->dev, "device disabled = %d\n", err);
 		return -ENODEV;
 	}
 
@@ -275,7 +288,7 @@ static void mei_reset_work(struct work_struct *work)
 	mutex_unlock(&dev->device_lock);
 
 	if (dev->dev_state == MEI_DEV_DISABLED) {
-		dev_err(&dev->pdev->dev, "device disabled = %d\n", ret);
+		dev_err(dev->dev, "device disabled = %d\n", ret);
 		return;
 	}
 
@@ -286,7 +299,7 @@ static void mei_reset_work(struct work_struct *work)
 
 void mei_stop(struct mei_device *dev)
 {
-	dev_dbg(&dev->pdev->dev, "stopping the device.\n");
+	dev_dbg(dev->dev, "stopping the device.\n");
 
 	mei_cancel_work(dev);
 
@@ -312,7 +325,7 @@ EXPORT_SYMBOL_GPL(mei_stop);
  *
  * @dev: the device structure
  *
- * returns true of there is no pending write
+ * Return: true of there is no pending write
  */
 bool mei_write_is_idle(struct mei_device *dev)
 {
@@ -320,7 +333,7 @@ bool mei_write_is_idle(struct mei_device *dev)
 		list_empty(&dev->ctrl_wr_list.list) &&
 		list_empty(&dev->write_list.list));
 
-	dev_dbg(&dev->pdev->dev, "write pg: is idle[%d] state=%s ctrl=%d write=%d\n",
+	dev_dbg(dev->dev, "write pg: is idle[%d] state=%s ctrl=%d write=%d\n",
 		idle,
 		mei_dev_state_str(dev->dev_state),
 		list_empty(&dev->ctrl_wr_list.list),
@@ -330,36 +343,25 @@ bool mei_write_is_idle(struct mei_device *dev)
 }
 EXPORT_SYMBOL_GPL(mei_write_is_idle);
 
-int mei_fw_status(struct mei_device *dev, struct mei_fw_status *fw_status)
-{
-	int i;
-	const struct mei_fw_status *fw_src = &dev->cfg->fw_status;
-
-	if (!fw_status)
-		return -EINVAL;
-
-	fw_status->count = fw_src->count;
-	for (i = 0; i < fw_src->count && i < MEI_FW_STATUS_MAX; i++) {
-		int ret;
-		ret = pci_read_config_dword(dev->pdev,
-			fw_src->status[i], &fw_status->status[i]);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(mei_fw_status);
-
-void mei_device_init(struct mei_device *dev, const struct mei_cfg *cfg)
+/**
+ * mei_device_init  -- initialize mei_device structure
+ *
+ * @dev: the mei device
+ * @device: the device structure
+ * @hw_ops: hw operations
+ */
+void mei_device_init(struct mei_device *dev,
+		     struct device *device,
+		     const struct mei_hw_ops *hw_ops)
 {
 	/* setup our list array */
 	INIT_LIST_HEAD(&dev->file_list);
 	INIT_LIST_HEAD(&dev->device_list);
+	INIT_LIST_HEAD(&dev->me_clients);
 	mutex_init(&dev->device_lock);
 	init_waitqueue_head(&dev->wait_hw_ready);
 	init_waitqueue_head(&dev->wait_pg);
-	init_waitqueue_head(&dev->wait_recvd_msg);
+	init_waitqueue_head(&dev->wait_hbm_start);
 	init_waitqueue_head(&dev->wait_stop_wd);
 	dev->dev_state = MEI_DEV_INITIALIZING;
 	dev->reset_count = 0;
@@ -389,7 +391,8 @@ void mei_device_init(struct mei_device *dev, const struct mei_cfg *cfg)
 	bitmap_set(dev->host_clients_map, 0, 1);
 
 	dev->pg_event = MEI_PG_EVENT_IDLE;
-	dev->cfg      = cfg;
+	dev->ops      = hw_ops;
+	dev->dev      = device;
 }
 EXPORT_SYMBOL_GPL(mei_device_init);
 
diff --git a/drivers/misc/mei/interrupt.c b/drivers/misc/mei/interrupt.c
index 4e3cba6da3f5..20c6c511f438 100644
--- a/drivers/misc/mei/interrupt.c
+++ b/drivers/misc/mei/interrupt.c
@@ -16,11 +16,11 @@
 
 
 #include <linux/export.h>
-#include <linux/pci.h>
 #include <linux/kthread.h>
 #include <linux/interrupt.h>
 #include <linux/fs.h>
 #include <linux/jiffies.h>
+#include <linux/slab.h>
 
 #include <linux/mei.h>
 
@@ -33,8 +33,8 @@
  * mei_irq_compl_handler - dispatch complete handlers
  *	for the completed callbacks
  *
- * @dev - mei device
- * @compl_list - list of completed cbs
+ * @dev: mei device
+ * @compl_list: list of completed cbs
  */
 void mei_irq_compl_handler(struct mei_device *dev, struct mei_cl_cb *compl_list)
 {
@@ -47,7 +47,7 @@ void mei_irq_compl_handler(struct mei_device *dev, struct mei_cl_cb *compl_list)
 		if (!cl)
 			continue;
 
-		dev_dbg(&dev->pdev->dev, "completing call back.\n");
+		dev_dbg(dev->dev, "completing call back.\n");
 		if (cl == &dev->iamthif_cl)
 			mei_amthif_complete(dev, cb);
 		else
@@ -62,7 +62,7 @@ EXPORT_SYMBOL_GPL(mei_irq_compl_handler);
  * @cl: host client
  * @mei_hdr: header of mei client message
  *
- * returns true if matches, false otherwise
+ * Return: true if matches, false otherwise
  */
 static inline int mei_cl_hbm_equal(struct mei_cl *cl,
 			struct mei_msg_hdr *mei_hdr)
@@ -72,12 +72,12 @@ static inline int mei_cl_hbm_equal(struct mei_cl *cl,
 }
 /**
  * mei_cl_is_reading - checks if the client
-		is the one to read this message
+ *		is the one to read this message
  *
  * @cl: mei client
  * @mei_hdr: header of mei message
  *
- * returns true on match and false otherwise
+ * Return: true on match and false otherwise
  */
 static bool mei_cl_is_reading(struct mei_cl *cl, struct mei_msg_hdr *mei_hdr)
 {
@@ -87,13 +87,13 @@ static bool mei_cl_is_reading(struct mei_cl *cl, struct mei_msg_hdr *mei_hdr)
 }
 
 /**
- * mei_irq_read_client_message - process client message
+ * mei_cl_irq_read_msg - process client message
  *
  * @dev: the device structure
  * @mei_hdr: header of mei client message
  * @complete_list: An instance of our list structure
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 static int mei_cl_irq_read_msg(struct mei_device *dev,
 			       struct mei_msg_hdr *mei_hdr,
@@ -126,7 +126,6 @@ static int mei_cl_irq_read_msg(struct mei_device *dev,
 					  GFP_KERNEL);
 
 			if (!buffer) {
-				cl_err(dev, cl, "allocation failed.\n");
 				list_del(&cb->list);
 				return -ENOMEM;
 			}
@@ -149,10 +148,10 @@ static int mei_cl_irq_read_msg(struct mei_device *dev,
 		break;
 	}
 
-	dev_dbg(&dev->pdev->dev, "message read\n");
+	dev_dbg(dev->dev, "message read\n");
 	if (!buffer) {
 		mei_read_slots(dev, dev->rd_msg_buf, mei_hdr->length);
-		dev_dbg(&dev->pdev->dev, "discarding message " MEI_HDR_FMT "\n",
+		dev_dbg(dev->dev, "discarding message " MEI_HDR_FMT "\n",
 				MEI_HDR_PRM(mei_hdr));
 	}
 
@@ -166,7 +165,7 @@ static int mei_cl_irq_read_msg(struct mei_device *dev,
  * @cb: callback block.
  * @cmpl_list: complete list.
  *
- * returns 0, OK; otherwise, error.
+ * Return: 0, OK; otherwise, error.
  */
 static int mei_cl_irq_disconnect_rsp(struct mei_cl *cl, struct mei_cl_cb *cb,
 				     struct mei_cl_cb *cmpl_list)
@@ -195,16 +194,16 @@ static int mei_cl_irq_disconnect_rsp(struct mei_cl *cl, struct mei_cl_cb *cb,
 
 
 /**
- * mei_cl_irq_close - processes close related operation from
+ * mei_cl_irq_disconnect - processes close related operation from
  *	interrupt thread context - send disconnect request
  *
  * @cl: client
  * @cb: callback block.
  * @cmpl_list: complete list.
  *
- * returns 0, OK; otherwise, error.
+ * Return: 0, OK; otherwise, error.
  */
-static int mei_cl_irq_close(struct mei_cl *cl, struct mei_cl_cb *cb,
+static int mei_cl_irq_disconnect(struct mei_cl *cl, struct mei_cl_cb *cb,
 			    struct mei_cl_cb *cmpl_list)
 {
 	struct mei_device *dev = cl->dev;
@@ -235,14 +234,14 @@ static int mei_cl_irq_close(struct mei_cl *cl, struct mei_cl_cb *cb,
 
 
 /**
- * mei_cl_irq_close - processes client read related operation from the
+ * mei_cl_irq_read - processes client read related operation from the
  *	interrupt thread context - request for flow control credits
  *
  * @cl: client
  * @cb: callback block.
  * @cmpl_list: complete list.
  *
- * returns 0, OK; otherwise, error.
+ * Return: 0, OK; otherwise, error.
  */
 static int mei_cl_irq_read(struct mei_cl *cl, struct mei_cl_cb *cb,
 			   struct mei_cl_cb *cmpl_list)
@@ -279,7 +278,7 @@ static int mei_cl_irq_read(struct mei_cl *cl, struct mei_cl_cb *cb,
  * @cb: callback block.
  * @cmpl_list: complete list.
  *
- * returns 0, OK; otherwise, error.
+ * Return: 0, OK; otherwise, error.
  */
 static int mei_cl_irq_connect(struct mei_cl *cl, struct mei_cl_cb *cb,
 			      struct mei_cl_cb *cmpl_list)
@@ -322,7 +321,7 @@ static int mei_cl_irq_connect(struct mei_cl *cl, struct mei_cl_cb *cb,
  * @cmpl_list: An instance of our list structure
  * @slots: slots to read.
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 int mei_irq_read_handler(struct mei_device *dev,
 		struct mei_cl_cb *cmpl_list, s32 *slots)
@@ -334,20 +333,20 @@ int mei_irq_read_handler(struct mei_device *dev,
 	if (!dev->rd_msg_hdr) {
 		dev->rd_msg_hdr = mei_read_hdr(dev);
 		(*slots)--;
-		dev_dbg(&dev->pdev->dev, "slots =%08x.\n", *slots);
+		dev_dbg(dev->dev, "slots =%08x.\n", *slots);
 	}
 	mei_hdr = (struct mei_msg_hdr *) &dev->rd_msg_hdr;
-	dev_dbg(&dev->pdev->dev, MEI_HDR_FMT, MEI_HDR_PRM(mei_hdr));
+	dev_dbg(dev->dev, MEI_HDR_FMT, MEI_HDR_PRM(mei_hdr));
 
 	if (mei_hdr->reserved || !dev->rd_msg_hdr) {
-		dev_err(&dev->pdev->dev, "corrupted message header 0x%08X\n",
+		dev_err(dev->dev, "corrupted message header 0x%08X\n",
 				dev->rd_msg_hdr);
 		ret = -EBADMSG;
 		goto end;
 	}
 
 	if (mei_slots2data(*slots) < mei_hdr->length) {
-		dev_err(&dev->pdev->dev, "less data available than length=%08x.\n",
+		dev_err(dev->dev, "less data available than length=%08x.\n",
 				*slots);
 		/* we can't read the message */
 		ret = -ENODATA;
@@ -358,7 +357,7 @@ int mei_irq_read_handler(struct mei_device *dev,
 	if (mei_hdr->host_addr == 0 && mei_hdr->me_addr == 0) {
 		ret = mei_hbm_dispatch(dev, mei_hdr);
 		if (ret) {
-			dev_dbg(&dev->pdev->dev, "mei_hbm_dispatch failed ret = %d\n",
+			dev_dbg(dev->dev, "mei_hbm_dispatch failed ret = %d\n",
 					ret);
 			goto end;
 		}
@@ -375,7 +374,7 @@ int mei_irq_read_handler(struct mei_device *dev,
 
 	/* if no recipient cl was found we assume corrupted header */
 	if (&cl->link == &dev->file_list) {
-		dev_err(&dev->pdev->dev, "no destination client found 0x%08X\n",
+		dev_err(dev->dev, "no destination client found 0x%08X\n",
 				dev->rd_msg_hdr);
 		ret = -EBADMSG;
 		goto end;
@@ -387,14 +386,14 @@ int mei_irq_read_handler(struct mei_device *dev,
 
 		ret = mei_amthif_irq_read_msg(dev, mei_hdr, cmpl_list);
 		if (ret) {
-			dev_err(&dev->pdev->dev, "mei_amthif_irq_read_msg failed = %d\n",
+			dev_err(dev->dev, "mei_amthif_irq_read_msg failed = %d\n",
 					ret);
 			goto end;
 		}
 	} else {
 		ret = mei_cl_irq_read_msg(dev, mei_hdr, cmpl_list);
 		if (ret) {
-			dev_err(&dev->pdev->dev, "mei_cl_irq_read_msg failed = %d\n",
+			dev_err(dev->dev, "mei_cl_irq_read_msg failed = %d\n",
 					ret);
 			goto end;
 		}
@@ -407,7 +406,7 @@ reset_slots:
 
 	if (*slots == -EOVERFLOW) {
 		/* overflow - reset */
-		dev_err(&dev->pdev->dev, "resetting due to slots overflow.\n");
+		dev_err(dev->dev, "resetting due to slots overflow.\n");
 		/* set the event since message has been read */
 		ret = -ERANGE;
 		goto end;
@@ -425,7 +424,7 @@ EXPORT_SYMBOL_GPL(mei_irq_read_handler);
  * @dev: the device structure
  * @cmpl_list: An instance of our list structure
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 {
@@ -445,7 +444,7 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 		return -EMSGSIZE;
 
 	/* complete all waiting for write CB */
-	dev_dbg(&dev->pdev->dev, "complete all waiting for write cb.\n");
+	dev_dbg(dev->dev, "complete all waiting for write cb.\n");
 
 	list = &dev->write_waiting_list;
 	list_for_each_entry_safe(cb, next, &list->list, list) {
@@ -487,7 +486,7 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 	}
 
 	/* complete control write list CB */
-	dev_dbg(&dev->pdev->dev, "complete control write list cb.\n");
+	dev_dbg(dev->dev, "complete control write list cb.\n");
 	list_for_each_entry_safe(cb, next, &dev->ctrl_wr_list.list, list) {
 		cl = cb->cl;
 		if (!cl) {
@@ -495,9 +494,9 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 			return -ENODEV;
 		}
 		switch (cb->fop_type) {
-		case MEI_FOP_CLOSE:
+		case MEI_FOP_DISCONNECT:
 			/* send disconnect message */
-			ret = mei_cl_irq_close(cl, cb, cmpl_list);
+			ret = mei_cl_irq_disconnect(cl, cb, cmpl_list);
 			if (ret)
 				return ret;
 
@@ -528,7 +527,7 @@ int mei_irq_write_handler(struct mei_device *dev, struct mei_cl_cb *cmpl_list)
 
 	}
 	/* complete  write list CB */
-	dev_dbg(&dev->pdev->dev, "complete write list cb.\n");
+	dev_dbg(dev->dev, "complete write list cb.\n");
 	list_for_each_entry_safe(cb, next, &dev->write_list.list, list) {
 		cl = cb->cl;
 		if (cl == NULL)
@@ -556,8 +555,6 @@ void mei_timer(struct work_struct *work)
 {
 	unsigned long timeout;
 	struct mei_cl *cl;
-	struct mei_cl_cb  *cb_pos = NULL;
-	struct mei_cl_cb  *cb_next = NULL;
 
 	struct mei_device *dev = container_of(work,
 					struct mei_device, timer_work.work);
@@ -571,7 +568,7 @@ void mei_timer(struct work_struct *work)
 
 		if (dev->init_clients_timer) {
 			if (--dev->init_clients_timer == 0) {
-				dev_err(&dev->pdev->dev, "timer: init clients timeout hbm_state = %d.\n",
+				dev_err(dev->dev, "timer: init clients timeout hbm_state = %d.\n",
 					dev->hbm_state);
 				mei_reset(dev);
 				goto out;
@@ -586,7 +583,7 @@ void mei_timer(struct work_struct *work)
 	list_for_each_entry(cl, &dev->file_list, link) {
 		if (cl->timer_count) {
 			if (--cl->timer_count == 0) {
-				dev_err(&dev->pdev->dev, "timer: connect/disconnect timeout.\n");
+				dev_err(dev->dev, "timer: connect/disconnect timeout.\n");
 				mei_reset(dev);
 				goto out;
 			}
@@ -598,7 +595,7 @@ void mei_timer(struct work_struct *work)
 
 	if (dev->iamthif_stall_timer) {
 		if (--dev->iamthif_stall_timer == 0) {
-			dev_err(&dev->pdev->dev, "timer: amthif  hanged.\n");
+			dev_err(dev->dev, "timer: amthif  hanged.\n");
 			mei_reset(dev);
 			dev->iamthif_msg_buf_size = 0;
 			dev->iamthif_msg_buf_index = 0;
@@ -620,27 +617,20 @@ void mei_timer(struct work_struct *work)
 		timeout = dev->iamthif_timer +
 			mei_secs_to_jiffies(MEI_IAMTHIF_READ_TIMER);
 
-		dev_dbg(&dev->pdev->dev, "dev->iamthif_timer = %ld\n",
+		dev_dbg(dev->dev, "dev->iamthif_timer = %ld\n",
 				dev->iamthif_timer);
-		dev_dbg(&dev->pdev->dev, "timeout = %ld\n", timeout);
-		dev_dbg(&dev->pdev->dev, "jiffies = %ld\n", jiffies);
+		dev_dbg(dev->dev, "timeout = %ld\n", timeout);
+		dev_dbg(dev->dev, "jiffies = %ld\n", jiffies);
 		if (time_after(jiffies, timeout)) {
 			/*
 			 * User didn't read the AMTHI data on time (15sec)
 			 * freeing AMTHI for other requests
 			 */
 
-			dev_dbg(&dev->pdev->dev, "freeing AMTHI for other requests\n");
+			dev_dbg(dev->dev, "freeing AMTHI for other requests\n");
 
-			list_for_each_entry_safe(cb_pos, cb_next,
-				&dev->amthif_rd_complete_list.list, list) {
-
-				cl = cb_pos->file_object->private_data;
-
-				/* Finding the AMTHI entry. */
-				if (cl == &dev->iamthif_cl)
-					list_del(&cb_pos->list);
-			}
+			mei_io_list_flush(&dev->amthif_rd_complete_list,
+				&dev->iamthif_cl);
 			mei_io_cb_free(dev->iamthif_current_cb);
 			dev->iamthif_current_cb = NULL;
 
diff --git a/drivers/misc/mei/main.c b/drivers/misc/mei/main.c
index 401a3d526cd0..beedc91f03a6 100644
--- a/drivers/misc/mei/main.c
+++ b/drivers/misc/mei/main.c
@@ -17,12 +17,12 @@
 #include <linux/moduleparam.h>
 #include <linux/kernel.h>
 #include <linux/device.h>
+#include <linux/slab.h>
 #include <linux/fs.h>
 #include <linux/errno.h>
 #include <linux/types.h>
 #include <linux/fcntl.h>
 #include <linux/aio.h>
-#include <linux/pci.h>
 #include <linux/poll.h>
 #include <linux/init.h>
 #include <linux/ioctl.h>
@@ -44,7 +44,7 @@
  * @inode: pointer to inode structure
  * @file: pointer to file structure
  *
- * returns 0 on success, <0 on error
+ * Return: 0 on success, <0 on error
  */
 static int mei_open(struct inode *inode, struct file *file)
 {
@@ -63,7 +63,7 @@ static int mei_open(struct inode *inode, struct file *file)
 
 	err = -ENODEV;
 	if (dev->dev_state != MEI_DEV_ENABLED) {
-		dev_dbg(&dev->pdev->dev, "dev_state != MEI_ENABLED  dev_state = %s\n",
+		dev_dbg(dev->dev, "dev_state != MEI_ENABLED  dev_state = %s\n",
 		    mei_dev_state_str(dev->dev_state));
 		goto err_unlock;
 	}
@@ -96,7 +96,7 @@ err_unlock:
  * @inode: pointer to inode structure
  * @file: pointer to file structure
  *
- * returns 0 on success, <0 on error
+ * Return: 0 on success, <0 on error
  */
 static int mei_release(struct inode *inode, struct file *file)
 {
@@ -157,7 +157,7 @@ out:
  * @length: buffer length
  * @offset: data offset in buffer
  *
- * returns >=0 data length on success , <0 on error
+ * Return: >=0 data length on success , <0 on error
  */
 static ssize_t mei_read(struct file *file, char __user *ubuf,
 			size_t length, loff_t *offset)
@@ -211,7 +211,7 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 
 	err = mei_cl_read_start(cl, length);
 	if (err && err != -EBUSY) {
-		dev_dbg(&dev->pdev->dev,
+		dev_dbg(dev->dev,
 			"mei start read failure with status = %d\n", err);
 		rets = err;
 		goto out;
@@ -254,7 +254,7 @@ static ssize_t mei_read(struct file *file, char __user *ubuf,
 	}
 	/* now copy the data to user space */
 copy_buffer:
-	dev_dbg(&dev->pdev->dev, "buf.size = %d buf.idx= %ld\n",
+	dev_dbg(dev->dev, "buf.size = %d buf.idx= %ld\n",
 	    cb->response_buffer.size, cb->buf_idx);
 	if (length == 0 || ubuf == NULL || *offset > cb->buf_idx) {
 		rets = -EMSGSIZE;
@@ -266,7 +266,7 @@ copy_buffer:
 	length = min_t(size_t, length, cb->buf_idx - *offset);
 
 	if (copy_to_user(ubuf, cb->response_buffer.data + *offset, length)) {
-		dev_dbg(&dev->pdev->dev, "failed to copy data to userland\n");
+		dev_dbg(dev->dev, "failed to copy data to userland\n");
 		rets = -EFAULT;
 		goto free;
 	}
@@ -285,7 +285,7 @@ free:
 	cl->reading_state = MEI_IDLE;
 	cl->read_cb = NULL;
 out:
-	dev_dbg(&dev->pdev->dev, "end mei read rets= %d\n", rets);
+	dev_dbg(dev->dev, "end mei read rets= %d\n", rets);
 	mutex_unlock(&dev->device_lock);
 	return rets;
 }
@@ -297,17 +297,17 @@ out:
  * @length: buffer length
  * @offset: data offset in buffer
  *
- * returns >=0 data length on success , <0 on error
+ * Return: >=0 data length on success , <0 on error
  */
 static ssize_t mei_write(struct file *file, const char __user *ubuf,
 			 size_t length, loff_t *offset)
 {
 	struct mei_cl *cl = file->private_data;
+	struct mei_me_client *me_cl;
 	struct mei_cl_cb *write_cb = NULL;
 	struct mei_device *dev;
 	unsigned long timeout = 0;
 	int rets;
-	int id;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -ENODEV;
@@ -321,8 +321,8 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 		goto out;
 	}
 
-	id = mei_me_cl_by_id(dev, cl->me_client_id);
-	if (id < 0) {
+	me_cl = mei_me_cl_by_uuid_id(dev, &cl->cl_uuid, cl->me_client_id);
+	if (!me_cl) {
 		rets = -ENOTTY;
 		goto out;
 	}
@@ -332,13 +332,13 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 		goto out;
 	}
 
-	if (length > dev->me_clients[id].props.max_msg_length) {
+	if (length > me_cl->props.max_msg_length) {
 		rets = -EFBIG;
 		goto out;
 	}
 
 	if (cl->state != MEI_FILE_CONNECTED) {
-		dev_err(&dev->pdev->dev, "host client = %d,  is not connected to ME client = %d",
+		dev_err(dev->dev, "host client = %d,  is not connected to ME client = %d",
 			cl->host_client_id, cl->me_client_id);
 		rets = -ENODEV;
 		goto out;
@@ -377,7 +377,6 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 
 	write_cb = mei_io_cb_init(cl, file);
 	if (!write_cb) {
-		dev_err(&dev->pdev->dev, "write cb allocation failed\n");
 		rets = -ENOMEM;
 		goto out;
 	}
@@ -387,7 +386,7 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 
 	rets = copy_from_user(write_cb->request_buffer.data, ubuf, length);
 	if (rets) {
-		dev_dbg(&dev->pdev->dev, "failed to copy data from userland\n");
+		dev_dbg(dev->dev, "failed to copy data from userland\n");
 		rets = -EFAULT;
 		goto out;
 	}
@@ -396,7 +395,7 @@ static ssize_t mei_write(struct file *file, const char __user *ubuf,
 		rets = mei_amthif_write(dev, write_cb);
 
 		if (rets) {
-			dev_err(&dev->pdev->dev,
+			dev_err(dev->dev,
 				"amthif write failed with status = %d\n", rets);
 			goto out;
 		}
@@ -415,27 +414,23 @@ out:
 /**
  * mei_ioctl_connect_client - the connect to fw client IOCTL function
  *
- * @dev: the device structure
- * @data: IOCTL connect data, input and output parameters
  * @file: private data of the file object
+ * @data: IOCTL connect data, input and output parameters
  *
  * Locking: called under "dev->device_lock" lock
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 static int mei_ioctl_connect_client(struct file *file,
 			struct mei_connect_client_data *data)
 {
 	struct mei_device *dev;
 	struct mei_client *client;
+	struct mei_me_client *me_cl;
 	struct mei_cl *cl;
-	int i;
 	int rets;
 
 	cl = file->private_data;
-	if (WARN_ON(!cl || !cl->dev))
-		return -ENODEV;
-
 	dev = cl->dev;
 
 	if (dev->dev_state != MEI_DEV_ENABLED) {
@@ -450,28 +445,29 @@ static int mei_ioctl_connect_client(struct file *file,
 	}
 
 	/* find ME client we're trying to connect to */
-	i = mei_me_cl_by_uuid(dev, &data->in_client_uuid);
-	if (i < 0 || dev->me_clients[i].props.fixed_address) {
-		dev_dbg(&dev->pdev->dev, "Cannot connect to FW Client UUID = %pUl\n",
+	me_cl = mei_me_cl_by_uuid(dev, &data->in_client_uuid);
+	if (!me_cl || me_cl->props.fixed_address) {
+		dev_dbg(dev->dev, "Cannot connect to FW Client UUID = %pUl\n",
 				&data->in_client_uuid);
 		rets = -ENOTTY;
 		goto end;
 	}
 
-	cl->me_client_id = dev->me_clients[i].client_id;
+	cl->me_client_id = me_cl->client_id;
+	cl->cl_uuid = me_cl->props.protocol_name;
 
-	dev_dbg(&dev->pdev->dev, "Connect to FW Client ID = %d\n",
+	dev_dbg(dev->dev, "Connect to FW Client ID = %d\n",
 			cl->me_client_id);
-	dev_dbg(&dev->pdev->dev, "FW Client - Protocol Version = %d\n",
-			dev->me_clients[i].props.protocol_version);
-	dev_dbg(&dev->pdev->dev, "FW Client - Max Msg Len = %d\n",
-			dev->me_clients[i].props.max_msg_length);
+	dev_dbg(dev->dev, "FW Client - Protocol Version = %d\n",
+			me_cl->props.protocol_version);
+	dev_dbg(dev->dev, "FW Client - Max Msg Len = %d\n",
+			me_cl->props.max_msg_length);
 
 	/* if we're connecting to amthif client then we will use the
 	 * existing connection
 	 */
 	if (uuid_le_cmp(data->in_client_uuid, mei_amthif_guid) == 0) {
-		dev_dbg(&dev->pdev->dev, "FW Client is amthi\n");
+		dev_dbg(dev->dev, "FW Client is amthi\n");
 		if (dev->iamthif_cl.state != MEI_FILE_CONNECTED) {
 			rets = -ENODEV;
 			goto end;
@@ -484,10 +480,8 @@ static int mei_ioctl_connect_client(struct file *file,
 		file->private_data = &dev->iamthif_cl;
 
 		client = &data->out_client_properties;
-		client->max_msg_length =
-			dev->me_clients[i].props.max_msg_length;
-		client->protocol_version =
-			dev->me_clients[i].props.protocol_version;
+		client->max_msg_length = me_cl->props.max_msg_length;
+		client->protocol_version = me_cl->props.protocol_version;
 		rets = dev->iamthif_cl.status;
 
 		goto end;
@@ -496,9 +490,9 @@ static int mei_ioctl_connect_client(struct file *file,
 
 	/* prepare the output buffer */
 	client = &data->out_client_properties;
-	client->max_msg_length = dev->me_clients[i].props.max_msg_length;
-	client->protocol_version = dev->me_clients[i].props.protocol_version;
-	dev_dbg(&dev->pdev->dev, "Can connect?\n");
+	client->max_msg_length = me_cl->props.max_msg_length;
+	client->protocol_version = me_cl->props.protocol_version;
+	dev_dbg(dev->dev, "Can connect?\n");
 
 
 	rets = mei_cl_connect(cl, file);
@@ -507,7 +501,6 @@ end:
 	return rets;
 }
 
-
 /**
  * mei_ioctl - the IOCTL function
  *
@@ -515,24 +508,22 @@ end:
  * @cmd: ioctl command
  * @data: pointer to mei message structure
  *
- * returns 0 on success , <0 on error
+ * Return: 0 on success , <0 on error
  */
 static long mei_ioctl(struct file *file, unsigned int cmd, unsigned long data)
 {
 	struct mei_device *dev;
 	struct mei_cl *cl = file->private_data;
-	struct mei_connect_client_data *connect_data = NULL;
+	struct mei_connect_client_data connect_data;
 	int rets;
 
-	if (cmd != IOCTL_MEI_CONNECT_CLIENT)
-		return -EINVAL;
 
 	if (WARN_ON(!cl || !cl->dev))
 		return -ENODEV;
 
 	dev = cl->dev;
 
-	dev_dbg(&dev->pdev->dev, "IOCTL cmd = 0x%x", cmd);
+	dev_dbg(dev->dev, "IOCTL cmd = 0x%x", cmd);
 
 	mutex_lock(&dev->device_lock);
 	if (dev->dev_state != MEI_DEV_ENABLED) {
@@ -540,38 +531,36 @@ static long mei_ioctl(struct file *file, unsigned int cmd, unsigned long data)
 		goto out;
 	}
 
-	dev_dbg(&dev->pdev->dev, ": IOCTL_MEI_CONNECT_CLIENT.\n");
-
-	connect_data = kzalloc(sizeof(struct mei_connect_client_data),
-							GFP_KERNEL);
-	if (!connect_data) {
-		rets = -ENOMEM;
-		goto out;
-	}
-	dev_dbg(&dev->pdev->dev, "copy connect data from user\n");
-	if (copy_from_user(connect_data, (char __user *)data,
+	switch (cmd) {
+	case IOCTL_MEI_CONNECT_CLIENT:
+		dev_dbg(dev->dev, ": IOCTL_MEI_CONNECT_CLIENT.\n");
+		if (copy_from_user(&connect_data, (char __user *)data,
 				sizeof(struct mei_connect_client_data))) {
-		dev_dbg(&dev->pdev->dev, "failed to copy data from userland\n");
-		rets = -EFAULT;
-		goto out;
-	}
-
-	rets = mei_ioctl_connect_client(file, connect_data);
+			dev_dbg(dev->dev, "failed to copy data from userland\n");
+			rets = -EFAULT;
+			goto out;
+		}
 
-	/* if all is ok, copying the data back to user. */
-	if (rets)
-		goto out;
+		rets = mei_ioctl_connect_client(file, &connect_data);
+		if (rets)
+			goto out;
 
-	dev_dbg(&dev->pdev->dev, "copy connect data to user\n");
-	if (copy_to_user((char __user *)data, connect_data,
+		/* if all is ok, copying the data back to user. */
+		if (copy_to_user((char __user *)data, &connect_data,
 				sizeof(struct mei_connect_client_data))) {
-		dev_dbg(&dev->pdev->dev, "failed to copy data to userland\n");
-		rets = -EFAULT;
-		goto out;
+			dev_dbg(dev->dev, "failed to copy data to userland\n");
+			rets = -EFAULT;
+			goto out;
+		}
+
+		break;
+
+	default:
+		dev_err(dev->dev, ": unsupported ioctl %d.\n", cmd);
+		rets = -ENOIOCTLCMD;
 	}
 
 out:
-	kfree(connect_data);
 	mutex_unlock(&dev->device_lock);
 	return rets;
 }
@@ -583,7 +572,7 @@ out:
  * @cmd: ioctl command
  * @data: pointer to mei message structure
  *
- * returns 0 on success , <0 on error
+ * Return: 0 on success , <0 on error
  */
 #ifdef CONFIG_COMPAT
 static long mei_compat_ioctl(struct file *file,
@@ -600,7 +589,7 @@ static long mei_compat_ioctl(struct file *file,
  * @file: pointer to file structure
  * @wait: pointer to poll_table structure
  *
- * returns poll mask
+ * Return: poll mask
  */
 static unsigned int mei_poll(struct file *file, poll_table *wait)
 {
@@ -670,7 +659,7 @@ static DEFINE_IDR(mei_idr);
  *
  * @dev:  device pointer
  *
- * returns allocated minor, or -ENOSPC if no free minor left
+ * Return: allocated minor, or -ENOSPC if no free minor left
  */
 static int mei_minor_get(struct mei_device *dev)
 {
@@ -681,7 +670,7 @@ static int mei_minor_get(struct mei_device *dev)
 	if (ret >= 0)
 		dev->minor = ret;
 	else if (ret == -ENOSPC)
-		dev_err(&dev->pdev->dev, "too many mei devices\n");
+		dev_err(dev->dev, "too many mei devices\n");
 
 	mutex_unlock(&mei_minor_lock);
 	return ret;
diff --git a/drivers/misc/mei/mei_dev.h b/drivers/misc/mei/mei_dev.h
index 0b0d6135543b..71744b16cc8c 100644
--- a/drivers/misc/mei/mei_dev.h
+++ b/drivers/misc/mei/mei_dev.h
@@ -129,20 +129,18 @@ enum mei_wd_states {
 
 /**
  * enum mei_cb_file_ops  - file operation associated with the callback
- * @MEI_FOP_READ      - read
- * @MEI_FOP_WRITE     - write
- * @MEI_FOP_CONNECT   - connect
- * @MEI_FOP_DISCONNECT_RSP - disconnect response
- * @MEI_FOP_OPEN      - open
- * @MEI_FOP_CLOSE     - close
+ * @MEI_FOP_READ:       read
+ * @MEI_FOP_WRITE:      write
+ * @MEI_FOP_CONNECT:    connect
+ * @MEI_FOP_DISCONNECT: disconnect
+ * @MEI_FOP_DISCONNECT_RSP: disconnect response
  */
 enum mei_cb_file_ops {
 	MEI_FOP_READ = 0,
 	MEI_FOP_WRITE,
 	MEI_FOP_CONNECT,
+	MEI_FOP_DISCONNECT,
 	MEI_FOP_DISCONNECT_RSP,
-	MEI_FOP_OPEN,
-	MEI_FOP_CLOSE
 };
 
 /*
@@ -159,8 +157,8 @@ struct mei_msg_data {
 /*
  * struct mei_fw_status - storage of FW status data
  *
- * @count - number of actually available elements in array
- * @status - FW status registers
+ * @count: number of actually available elements in array
+ * @status: FW status registers
  */
 struct mei_fw_status {
 	int count;
@@ -170,11 +168,13 @@ struct mei_fw_status {
 /**
  * struct mei_me_client - representation of me (fw) client
  *
- * @props  - client properties
- * @client_id - me client id
- * @mei_flow_ctrl_creds - flow control credits
+ * @list: link in me client list
+ * @props: client properties
+ * @client_id: me client id
+ * @mei_flow_ctrl_creds: flow control credits
  */
 struct mei_me_client {
+	struct list_head list;
 	struct mei_client_properties props;
 	u8 client_id;
 	u8 mei_flow_ctrl_creds;
@@ -186,8 +186,15 @@ struct mei_cl;
 /**
  * struct mei_cl_cb - file operation callback structure
  *
- * @cl - file client who is running this operation
- * @fop_type - file operation type
+ * @list: link in callback queue
+ * @cl: file client who is running this operation
+ * @fop_type: file operation type
+ * @request_buffer: buffer to store request data
+ * @response_buffer: buffer to store response data
+ * @buf_idx: last read index
+ * @read_time: last read operation time stamp (iamthif)
+ * @file_object: pointer to file structure
+ * @internal: communication between driver and FW flag
  */
 struct mei_cl_cb {
 	struct list_head list;
@@ -201,7 +208,29 @@ struct mei_cl_cb {
 	u32 internal:1;
 };
 
-/* MEI client instance carried as file->private_data*/
+/**
+ * struct mei_cl - me client host representation
+ *    carried in file->private_data
+ *
+ * @link: link in the clients list
+ * @dev: mei parent device
+ * @state: file operation state
+ * @tx_wait: wait queue for tx completion
+ * @rx_wait: wait queue for rx completion
+ * @wait:  wait queue for management operation
+ * @status: connection status
+ * @cl_uuid: client uuid name
+ * @host_client_id: host id
+ * @me_client_id: me/fw id
+ * @mei_flow_ctrl_creds: transmit flow credentials
+ * @timer_count:  watchdog timer for operation completion
+ * @reading_state: state of the rx
+ * @writing_state: state of the tx
+ * @read_cb: current pending reading callback
+ *
+ * @device: device on the mei client bus
+ * @device_link:  link to bus clients
+ */
 struct mei_cl {
 	struct list_head link;
 	struct mei_device *dev;
@@ -210,7 +239,7 @@ struct mei_cl {
 	wait_queue_head_t rx_wait;
 	wait_queue_head_t wait;
 	int status;
-	/* ID of client connected */
+	uuid_le cl_uuid;
 	u8 host_client_id;
 	u8 me_client_id;
 	u8 mei_flow_ctrl_creds;
@@ -222,35 +251,35 @@ struct mei_cl {
 	/* MEI CL bus data */
 	struct mei_cl_device *device;
 	struct list_head device_link;
-	uuid_le device_uuid;
 };
 
 /** struct mei_hw_ops
  *
- * @host_is_ready    - query for host readiness
+ * @host_is_ready    : query for host readiness
 
- * @hw_is_ready      - query if hw is ready
- * @hw_reset         - reset hw
- * @hw_start         - start hw after reset
- * @hw_config        - configure hw
+ * @hw_is_ready      : query if hw is ready
+ * @hw_reset         : reset hw
+ * @hw_start         : start hw after reset
+ * @hw_config        : configure hw
 
- * @pg_state         - power gating state of the device
- * @pg_is_enabled    - is power gating enabled
+ * @fw_status        : get fw status registers
+ * @pg_state         : power gating state of the device
+ * @pg_is_enabled    : is power gating enabled
 
- * @intr_clear       - clear pending interrupts
- * @intr_enable      - enable interrupts
- * @intr_disable     - disable interrupts
+ * @intr_clear       : clear pending interrupts
+ * @intr_enable      : enable interrupts
+ * @intr_disable     : disable interrupts
 
- * @hbuf_free_slots  - query for write buffer empty slots
- * @hbuf_is_ready    - query if write buffer is empty
- * @hbuf_max_len     - query for write buffer max len
+ * @hbuf_free_slots  : query for write buffer empty slots
+ * @hbuf_is_ready    : query if write buffer is empty
+ * @hbuf_max_len     : query for write buffer max len
 
- * @write            - write a message to FW
+ * @write            : write a message to FW
 
- * @rdbuf_full_slots - query how many slots are filled
+ * @rdbuf_full_slots : query how many slots are filled
 
- * @read_hdr         - get first 4 bytes (header)
- * @read             - read a buffer from the FW
+ * @read_hdr         : get first 4 bytes (header)
+ * @read             : read a buffer from the FW
  */
 struct mei_hw_ops {
 
@@ -261,6 +290,8 @@ struct mei_hw_ops {
 	int (*hw_start)(struct mei_device *dev);
 	void (*hw_config)(struct mei_device *dev);
 
+
+	int (*fw_status)(struct mei_device *dev, struct mei_fw_status *fw_sts);
 	enum mei_pg_state (*pg_state)(struct mei_device *dev);
 	bool (*pg_is_enabled)(struct mei_device *dev);
 
@@ -328,11 +359,12 @@ void mei_cl_bus_exit(void);
  * when being probed and shall use it for doing ME bus I/O.
  *
  * @dev: linux driver model device pointer
- * @uuid: me client uuid
  * @cl: mei client
  * @ops: ME transport ops
+ * @event_work: async work to execute event callback
  * @event_cb: Drivers register this callback to get asynchronous ME
  *	events (e.g. Rx buffer pending) notifications.
+ * @event_context: event callback run context
  * @events: Events bitmask sent to the driver.
  * @priv_data: client private data
  */
@@ -352,7 +384,7 @@ struct mei_cl_device {
 };
 
 
- /**
+/**
  * enum mei_pg_event - power gating transition events
  *
  * @MEI_PG_EVENT_IDLE: the driver is not in power gating transition
@@ -376,67 +408,106 @@ enum mei_pg_state {
 	MEI_PG_ON =  1,
 };
 
-/*
- * mei_cfg
- *
- * @fw_status - FW status
- * @quirk_probe - device exclusion quirk
- */
-struct mei_cfg {
-	const struct mei_fw_status fw_status;
-	bool (*quirk_probe)(struct pci_dev *pdev);
-};
-
-
-#define MEI_PCI_DEVICE(dev, cfg) \
-	.vendor = PCI_VENDOR_ID_INTEL, .device = (dev), \
-	.subvendor = PCI_ANY_ID, .subdevice = PCI_ANY_ID, \
-	.driver_data = (kernel_ulong_t)&(cfg)
-
+const char *mei_pg_state_str(enum mei_pg_state state);
 
 /**
  * struct mei_device -  MEI private device struct
-
- * @pdev - pointer to pci device struct
- * @cdev - character device
- * @minor - minor number allocated for device
  *
- * @reset_count - limits the number of consecutive resets
- * @hbm_state - state of host bus message protocol
- * @pg_event - power gating event
- * @mem_addr - mem mapped base register address
-
- * @hbuf_depth - depth of hardware host/write buffer is slots
- * @hbuf_is_ready - query if the host host/write buffer is ready
- * @wr_msg - the buffer for hbm control messages
- * @cfg - per device generation config and ops
+ * @dev         : device on a bus
+ * @cdev        : character device
+ * @minor       : minor number allocated for device
+ *
+ * @read_list   : read completion list
+ * @write_list  : write pending list
+ * @write_waiting_list : write completion list
+ * @ctrl_wr_list : pending control write list
+ * @ctrl_rd_list : pending control read list
+ *
+ * @file_list   : list of opened handles
+ * @open_handle_count: number of opened handles
+ *
+ * @device_lock : big device lock
+ * @timer_work  : MEI timer delayed work (timeouts)
+ *
+ * @recvd_hw_ready : hw ready message received flag
+ *
+ * @wait_hw_ready : wait queue for receive HW ready message form FW
+ * @wait_pg     : wait queue for receive PG message from FW
+ * @wait_hbm_start : wait queue for receive HBM start message from FW
+ * @wait_stop_wd : wait queue for receive WD stop message from FW
+ *
+ * @reset_count : number of consecutive resets
+ * @dev_state   : device state
+ * @hbm_state   : state of host bus message protocol
+ * @init_clients_timer : HBM init handshake timeout
+ *
+ * @pg_event    : power gating event
+ * @pg_domain   : runtime PM domain
+ *
+ * @rd_msg_buf  : control messages buffer
+ * @rd_msg_hdr  : read message header storage
+ *
+ * @hbuf_depth  : depth of hardware host/write buffer is slots
+ * @hbuf_is_ready : query if the host host/write buffer is ready
+ * @wr_msg      : the buffer for hbm control messages
+ *
+ * @version     : HBM protocol version in use
+ * @hbm_f_pg_supported : hbm feature pgi protocol
+ *
+ * @me_clients  : list of FW clients
+ * @me_clients_map : FW clients bit map
+ * @host_clients_map : host clients id pool
+ * @me_client_index : last FW client index in enumeration
+ *
+ * @wd_cl       : watchdog client
+ * @wd_state    : watchdog client state
+ * @wd_pending  : watchdog command is pending
+ * @wd_timeout  : watchdog expiration timeout
+ * @wd_data     : watchdog message buffer
+ *
+ * @amthif_cmd_list : amthif list for cmd waiting
+ * @amthif_rd_complete_list : amthif list for reading completed cmd data
+ * @iamthif_file_object : file for current amthif operation
+ * @iamthif_cl  : amthif host client
+ * @iamthif_current_cb : amthif current operation callback
+ * @iamthif_open_count : number of opened amthif connections
+ * @iamthif_mtu : amthif client max message length
+ * @iamthif_timer : time stamp of current amthif command completion
+ * @iamthif_stall_timer : timer to detect amthif hang
+ * @iamthif_msg_buf : amthif current message buffer
+ * @iamthif_msg_buf_size : size of current amthif message request buffer
+ * @iamthif_msg_buf_index : current index in amthif message request buffer
+ * @iamthif_state : amthif processor state
+ * @iamthif_flow_control_pending: amthif waits for flow control
+ * @iamthif_ioctl : wait for completion if amthif control message
+ * @iamthif_canceled : current amthif command is canceled
+ *
+ * @init_work   : work item for the device init
+ * @reset_work  : work item for the device reset
+ *
+ * @device_list : mei client bus list
+ *
+ * @dbgfs_dir   : debugfs mei root directory
+ *
+ * @ops:        : hw specific operations
+ * @hw          : hw specific data
  */
 struct mei_device {
-	struct pci_dev *pdev;	/* pointer to pci device struct */
+	struct device *dev;
 	struct cdev cdev;
 	int minor;
 
-	/*
-	 * lists of queues
-	 */
-	/* array of pointers to aio lists */
-	struct mei_cl_cb read_list;		/* driver read queue */
-	struct mei_cl_cb write_list;		/* driver write queue */
-	struct mei_cl_cb write_waiting_list;	/* write waiting queue */
-	struct mei_cl_cb ctrl_wr_list;		/* managed write IOCTL list */
-	struct mei_cl_cb ctrl_rd_list;		/* managed read IOCTL list */
+	struct mei_cl_cb read_list;
+	struct mei_cl_cb write_list;
+	struct mei_cl_cb write_waiting_list;
+	struct mei_cl_cb ctrl_wr_list;
+	struct mei_cl_cb ctrl_rd_list;
 
-	/*
-	 * list of files
-	 */
 	struct list_head file_list;
 	long open_handle_count;
 
-	/*
-	 * lock for the device
-	 */
-	struct mutex device_lock; /* device lock */
-	struct delayed_work timer_work;	/* MEI timer delayed work (timeouts) */
+	struct mutex device_lock;
+	struct delayed_work timer_work;
 
 	bool recvd_hw_ready;
 	/*
@@ -444,7 +515,7 @@ struct mei_device {
 	 */
 	wait_queue_head_t wait_hw_ready;
 	wait_queue_head_t wait_pg;
-	wait_queue_head_t wait_recvd_msg;
+	wait_queue_head_t wait_hbm_start;
 	wait_queue_head_t wait_stop_wd;
 
 	/*
@@ -463,7 +534,7 @@ struct mei_device {
 	struct dev_pm_domain pg_domain;
 #endif /* CONFIG_PM_RUNTIME */
 
-	unsigned char rd_msg_buf[MEI_RD_MSG_BUF_SIZE];	/* control messages */
+	unsigned char rd_msg_buf[MEI_RD_MSG_BUF_SIZE];
 	u32 rd_msg_hdr;
 
 	/* write buffer */
@@ -477,12 +548,11 @@ struct mei_device {
 	} wr_msg;
 
 	struct hbm_version version;
+	unsigned int hbm_f_pg_supported:1;
 
-	struct mei_me_client *me_clients; /* Note: memory has to be allocated */
+	struct list_head me_clients;
 	DECLARE_BITMAP(me_clients_map, MEI_CLIENTS_MAX);
 	DECLARE_BITMAP(host_clients_map, MEI_CLIENTS_MAX);
-	unsigned long me_clients_num;
-	unsigned long me_client_presentation_num;
 	unsigned long me_client_index;
 
 	struct mei_cl wd_cl;
@@ -523,7 +593,6 @@ struct mei_device {
 
 
 	const struct mei_hw_ops *ops;
-	const struct mei_cfg *cfg;
 	char hw[0] __aligned(sizeof(void *));
 };
 
@@ -535,8 +604,10 @@ static inline unsigned long mei_secs_to_jiffies(unsigned long sec)
 /**
  * mei_data2slots - get slots - number of (dwords) from a message length
  *	+ size of the mei header
- * @length - size of the messages in bytes
- * returns  - number of slots
+ *
+ * @length: size of the messages in bytes
+ *
+ * Return: number of slots
  */
 static inline u32 mei_data2slots(size_t length)
 {
@@ -544,9 +615,11 @@ static inline u32 mei_data2slots(size_t length)
 }
 
 /**
- * mei_slots2data- get data in slots - bytes from slots
- * @slots -  number of available slots
- * returns  - number of bytes in slots
+ * mei_slots2data - get data in slots - bytes from slots
+ *
+ * @slots: number of available slots
+ *
+ * Return: number of bytes in slots
  */
 static inline u32 mei_slots2data(int slots)
 {
@@ -556,7 +629,9 @@ static inline u32 mei_slots2data(int slots)
 /*
  * mei init function prototypes
  */
-void mei_device_init(struct mei_device *dev, const struct mei_cfg *cfg);
+void mei_device_init(struct mei_device *dev,
+		     struct device *device,
+		     const struct mei_hw_ops *hw_ops);
 int mei_reset(struct mei_device *dev);
 int mei_start(struct mei_device *dev);
 int mei_restart(struct mei_device *dev);
@@ -622,12 +697,12 @@ int mei_wd_host_init(struct mei_device *dev);
 /*
  * mei_watchdog_register  - Registering watchdog interface
  *   once we got connection to the WD Client
- * @dev - mei device
+ * @dev: mei device
  */
 int mei_watchdog_register(struct mei_device *dev);
 /*
  * mei_watchdog_unregister  - Unregistering watchdog interface
- * @dev - mei device
+ * @dev: mei device
  */
 void mei_watchdog_unregister(struct mei_device *dev);
 
@@ -723,7 +798,11 @@ static inline int mei_count_full_read_slots(struct mei_device *dev)
 	return dev->ops->rdbuf_full_slots(dev);
 }
 
-int mei_fw_status(struct mei_device *dev, struct mei_fw_status *fw_status);
+static inline int mei_fw_status(struct mei_device *dev,
+				struct mei_fw_status *fw_status)
+{
+	return dev->ops->fw_status(dev, fw_status);
+}
 
 #define FW_STS_FMT "%08X %08X"
 #define FW_STS_PRM(fw_status) \
diff --git a/drivers/misc/mei/nfc.c b/drivers/misc/mei/nfc.c
index 3095fc514a65..622654323177 100644
--- a/drivers/misc/mei/nfc.c
+++ b/drivers/misc/mei/nfc.c
@@ -19,7 +19,8 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/device.h>
-#include <linux/pci.h>
+#include <linux/slab.h>
+
 #include <linux/mei_cl_bus.h>
 
 #include "mei_dev.h"
@@ -87,14 +88,20 @@ struct mei_nfc_hci_hdr {
 
 #define MEI_NFC_HEADER_SIZE 10
 
-/** mei_nfc_dev - NFC mei device
+/**
+ * struct mei_nfc_dev - NFC mei device
  *
  * @cl: NFC host client
  * @cl_info: NFC info host client
  * @init_work: perform connection to the info client
+ * @send_wq: send completion wait queue
  * @fw_ivn: NFC Interface Version Number
  * @vendor_id: NFC manufacturer ID
  * @radio_type: NFC radio type
+ * @bus_name: bus name
+ *
+ * @req_id:  message counter
+ * @recv_req_id: reception message counter
  */
 struct mei_nfc_dev {
 	struct mei_cl *cl;
@@ -163,7 +170,7 @@ static int mei_nfc_build_bus_name(struct mei_nfc_dev *ndev)
 			return 0;
 
 		default:
-			dev_err(&dev->pdev->dev, "Unknown radio type 0x%x\n",
+			dev_err(dev->dev, "Unknown radio type 0x%x\n",
 				ndev->radio_type);
 
 			return -EINVAL;
@@ -175,14 +182,14 @@ static int mei_nfc_build_bus_name(struct mei_nfc_dev *ndev)
 			ndev->bus_name = "pn544";
 			return 0;
 		default:
-			dev_err(&dev->pdev->dev, "Unknown radio type 0x%x\n",
+			dev_err(dev->dev, "Unknown radio type 0x%x\n",
 				ndev->radio_type);
 
 			return -EINVAL;
 		}
 
 	default:
-		dev_err(&dev->pdev->dev, "Unknown vendor ID 0x%x\n",
+		dev_err(dev->dev, "Unknown vendor ID 0x%x\n",
 			ndev->vendor_id);
 
 		return -EINVAL;
@@ -231,21 +238,21 @@ static int mei_nfc_connect(struct mei_nfc_dev *ndev)
 
 	ret = __mei_cl_send(cl, (u8 *)cmd, connect_length);
 	if (ret < 0) {
-		dev_err(&dev->pdev->dev, "Could not send connect cmd\n");
+		dev_err(dev->dev, "Could not send connect cmd\n");
 		goto err;
 	}
 
 	bytes_recv = __mei_cl_recv(cl, (u8 *)reply, connect_resp_length);
 	if (bytes_recv < 0) {
-		dev_err(&dev->pdev->dev, "Could not read connect response\n");
+		dev_err(dev->dev, "Could not read connect response\n");
 		ret = bytes_recv;
 		goto err;
 	}
 
-	dev_info(&dev->pdev->dev, "IVN 0x%x Vendor ID 0x%x\n",
+	dev_info(dev->dev, "IVN 0x%x Vendor ID 0x%x\n",
 		 connect_resp->fw_ivn, connect_resp->vendor_id);
 
-	dev_info(&dev->pdev->dev, "ME FW %d.%d.%d.%d\n",
+	dev_info(dev->dev, "ME FW %d.%d.%d.%d\n",
 		connect_resp->me_major, connect_resp->me_minor,
 		connect_resp->me_hotfix, connect_resp->me_build);
 
@@ -279,7 +286,7 @@ static int mei_nfc_if_version(struct mei_nfc_dev *ndev)
 
 	ret = __mei_cl_send(cl, (u8 *)&cmd, sizeof(struct mei_nfc_cmd));
 	if (ret < 0) {
-		dev_err(&dev->pdev->dev, "Could not send IF version cmd\n");
+		dev_err(dev->dev, "Could not send IF version cmd\n");
 		return ret;
 	}
 
@@ -293,7 +300,7 @@ static int mei_nfc_if_version(struct mei_nfc_dev *ndev)
 
 	bytes_recv = __mei_cl_recv(cl, (u8 *)reply, if_version_length);
 	if (bytes_recv < 0 || bytes_recv < sizeof(struct mei_nfc_reply)) {
-		dev_err(&dev->pdev->dev, "Could not read IF version\n");
+		dev_err(dev->dev, "Could not read IF version\n");
 		ret = -EIO;
 		goto err;
 	}
@@ -319,7 +326,7 @@ static int mei_nfc_enable(struct mei_cl_device *cldev)
 
 	ret = mei_nfc_connect(ndev);
 	if (ret < 0) {
-		dev_err(&dev->pdev->dev, "Could not connect to NFC");
+		dev_err(dev->dev, "Could not connect to NFC");
 		return ret;
 	}
 
@@ -342,9 +349,10 @@ static int mei_nfc_send(struct mei_cl_device *cldev, u8 *buf, size_t length)
 	ndev = (struct mei_nfc_dev *) cldev->priv_data;
 	dev = ndev->cl->dev;
 
+	err = -ENOMEM;
 	mei_buf = kzalloc(length + MEI_NFC_HEADER_SIZE, GFP_KERNEL);
 	if (!mei_buf)
-		return -ENOMEM;
+		goto out;
 
 	hdr = (struct mei_nfc_hci_hdr *) mei_buf;
 	hdr->cmd = MEI_NFC_CMD_HCI_SEND;
@@ -354,21 +362,19 @@ static int mei_nfc_send(struct mei_cl_device *cldev, u8 *buf, size_t length)
 	hdr->data_size = length;
 
 	memcpy(mei_buf + MEI_NFC_HEADER_SIZE, buf, length);
-
 	err = __mei_cl_send(ndev->cl, mei_buf, length + MEI_NFC_HEADER_SIZE);
 	if (err < 0)
-		return err;
-
-	kfree(mei_buf);
+		goto out;
 
 	if (!wait_event_interruptible_timeout(ndev->send_wq,
 				ndev->recv_req_id == ndev->req_id, HZ)) {
-		dev_err(&dev->pdev->dev, "NFC MEI command timeout\n");
+		dev_err(dev->dev, "NFC MEI command timeout\n");
 		err = -ETIME;
 	} else {
 		ndev->req_id++;
 	}
-
+out:
+	kfree(mei_buf);
 	return err;
 }
 
@@ -419,8 +425,7 @@ static void mei_nfc_init(struct work_struct *work)
 
 	if (mei_cl_connect(cl_info, NULL) < 0) {
 		mutex_unlock(&dev->device_lock);
-		dev_err(&dev->pdev->dev,
-			"Could not connect to the NFC INFO ME client");
+		dev_err(dev->dev, "Could not connect to the NFC INFO ME client");
 
 		goto err;
 	}
@@ -428,21 +433,19 @@ static void mei_nfc_init(struct work_struct *work)
 	mutex_unlock(&dev->device_lock);
 
 	if (mei_nfc_if_version(ndev) < 0) {
-		dev_err(&dev->pdev->dev, "Could not get the NFC interface version");
+		dev_err(dev->dev, "Could not get the NFC interface version");
 
 		goto err;
 	}
 
-	dev_info(&dev->pdev->dev,
-		"NFC MEI VERSION: IVN 0x%x Vendor ID 0x%x Type 0x%x\n",
+	dev_info(dev->dev, "NFC MEI VERSION: IVN 0x%x Vendor ID 0x%x Type 0x%x\n",
 		ndev->fw_ivn, ndev->vendor_id, ndev->radio_type);
 
 	mutex_lock(&dev->device_lock);
 
 	if (mei_cl_disconnect(cl_info) < 0) {
 		mutex_unlock(&dev->device_lock);
-		dev_err(&dev->pdev->dev,
-			"Could not disconnect the NFC INFO ME client");
+		dev_err(dev->dev, "Could not disconnect the NFC INFO ME client");
 
 		goto err;
 	}
@@ -450,15 +453,13 @@ static void mei_nfc_init(struct work_struct *work)
 	mutex_unlock(&dev->device_lock);
 
 	if (mei_nfc_build_bus_name(ndev) < 0) {
-		dev_err(&dev->pdev->dev,
-			"Could not build the bus ID name\n");
+		dev_err(dev->dev, "Could not build the bus ID name\n");
 		return;
 	}
 
 	cldev = mei_cl_add_device(dev, mei_nfc_guid, ndev->bus_name, &nfc_ops);
 	if (!cldev) {
-		dev_err(&dev->pdev->dev,
-			"Could not add the NFC device to the MEI bus\n");
+		dev_err(dev->dev, "Could not add the NFC device to the MEI bus\n");
 
 		goto err;
 	}
@@ -473,7 +474,6 @@ err:
 	mei_nfc_free(ndev);
 	mutex_unlock(&dev->device_lock);
 
-	return;
 }
 
 
@@ -481,7 +481,8 @@ int mei_nfc_host_init(struct mei_device *dev)
 {
 	struct mei_nfc_dev *ndev = &nfc_dev;
 	struct mei_cl *cl_info, *cl = NULL;
-	int i, ret;
+	struct mei_me_client *me_cl;
+	int ret;
 
 	/* already initialized */
 	if (ndev->cl_info)
@@ -499,40 +500,38 @@ int mei_nfc_host_init(struct mei_device *dev)
 	}
 
 	/* check for valid client id */
-	i = mei_me_cl_by_uuid(dev, &mei_nfc_info_guid);
-	if (i < 0) {
-		dev_info(&dev->pdev->dev, "nfc: failed to find the client\n");
+	me_cl = mei_me_cl_by_uuid(dev, &mei_nfc_info_guid);
+	if (!me_cl) {
+		dev_info(dev->dev, "nfc: failed to find the client\n");
 		ret = -ENOTTY;
 		goto err;
 	}
 
-	cl_info->me_client_id = dev->me_clients[i].client_id;
+	cl_info->me_client_id = me_cl->client_id;
+	cl_info->cl_uuid = me_cl->props.protocol_name;
 
 	ret = mei_cl_link(cl_info, MEI_HOST_CLIENT_ID_ANY);
 	if (ret)
 		goto err;
 
-	cl_info->device_uuid = mei_nfc_info_guid;
 
 	list_add_tail(&cl_info->device_link, &dev->device_list);
 
 	/* check for valid client id */
-	i = mei_me_cl_by_uuid(dev, &mei_nfc_guid);
-	if (i < 0) {
-		dev_info(&dev->pdev->dev, "nfc: failed to find the client\n");
+	me_cl = mei_me_cl_by_uuid(dev, &mei_nfc_guid);
+	if (!me_cl) {
+		dev_info(dev->dev, "nfc: failed to find the client\n");
 		ret = -ENOTTY;
 		goto err;
 	}
 
-	cl->me_client_id = dev->me_clients[i].client_id;
+	cl->me_client_id = me_cl->client_id;
+	cl->cl_uuid = me_cl->props.protocol_name;
 
 	ret = mei_cl_link(cl, MEI_HOST_CLIENT_ID_ANY);
 	if (ret)
 		goto err;
 
-	cl->device_uuid = mei_nfc_guid;
-
-
 	list_add_tail(&cl->device_link, &dev->device_list);
 
 	ndev->req_id = 1;
@@ -552,6 +551,7 @@ err:
 void mei_nfc_host_exit(struct mei_device *dev)
 {
 	struct mei_nfc_dev *ndev = &nfc_dev;
+
 	cancel_work_sync(&ndev->init_work);
 }
 
diff --git a/drivers/misc/mei/pci-me.c b/drivers/misc/mei/pci-me.c
index a0e9422b55a2..f3225b1643ab 100644
--- a/drivers/misc/mei/pci-me.c
+++ b/drivers/misc/mei/pci-me.c
@@ -98,12 +98,12 @@ static inline void mei_me_unset_pm_domain(struct mei_device *dev) {}
 #endif /* CONFIG_PM_RUNTIME */
 
 /**
- * mei_quirk_probe - probe for devices that doesn't valid ME interface
+ * mei_me_quirk_probe - probe for devices that doesn't valid ME interface
  *
  * @pdev: PCI device structure
  * @cfg: per generation config
  *
- * returns true if ME Interface is valid, false otherwise
+ * Return: true if ME Interface is valid, false otherwise
  */
 static bool mei_me_quirk_probe(struct pci_dev *pdev,
 				const struct mei_cfg *cfg)
@@ -117,12 +117,12 @@ static bool mei_me_quirk_probe(struct pci_dev *pdev,
 }
 
 /**
- * mei_probe - Device Initialization Routine
+ * mei_me_probe - Device Initialization Routine
  *
  * @pdev: PCI device structure
  * @ent: entry in kcs_pci_tbl
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 static int mei_me_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
@@ -249,7 +249,7 @@ end:
 }
 
 /**
- * mei_remove - Device Removal Routine
+ * mei_me_remove - Device Removal Routine
  *
  * @pdev: PCI device structure
  *
@@ -424,13 +424,13 @@ static int mei_me_pm_runtime_resume(struct device *device)
 }
 
 /**
- * mei_me_set_pm_domain - fill and set pm domian stucture for device
+ * mei_me_set_pm_domain - fill and set pm domain structure for device
  *
  * @dev: mei_device
  */
 static inline void mei_me_set_pm_domain(struct mei_device *dev)
 {
-	struct pci_dev *pdev  = dev->pdev;
+	struct pci_dev *pdev  = to_pci_dev(dev->dev);
 
 	if (pdev->dev.bus && pdev->dev.bus->pm) {
 		dev->pg_domain.ops = *pdev->dev.bus->pm;
@@ -444,14 +444,14 @@ static inline void mei_me_set_pm_domain(struct mei_device *dev)
 }
 
 /**
- * mei_me_unset_pm_domain - clean pm domian stucture for device
+ * mei_me_unset_pm_domain - clean pm domain structure for device
  *
  * @dev: mei_device
  */
 static inline void mei_me_unset_pm_domain(struct mei_device *dev)
 {
 	/* stop using pm callbacks if any */
-	dev->pdev->dev.pm_domain = NULL;
+	dev->dev->pm_domain = NULL;
 }
 #endif /* CONFIG_PM_RUNTIME */
 
diff --git a/drivers/misc/mei/pci-txe.c b/drivers/misc/mei/pci-txe.c
index 19de57368b7a..bee1c6fb7e75 100644
--- a/drivers/misc/mei/pci-txe.c
+++ b/drivers/misc/mei/pci-txe.c
@@ -36,7 +36,8 @@
 #include "hw-txe.h"
 
 static const struct pci_device_id mei_txe_pci_tbl[] = {
-	{MEI_PCI_DEVICE(0x0F18, mei_txe_cfg)}, /* Baytrail */
+	{PCI_VDEVICE(INTEL, 0x0F18)}, /* Baytrail */
+
 	{0, }
 };
 MODULE_DEVICE_TABLE(pci, mei_txe_pci_tbl);
@@ -52,6 +53,7 @@ static inline void mei_txe_unset_pm_domain(struct mei_device *dev) {}
 static void mei_txe_pci_iounmap(struct pci_dev *pdev, struct mei_txe_hw *hw)
 {
 	int i;
+
 	for (i = SEC_BAR; i < NUM_OF_MEM_BARS; i++) {
 		if (hw->mem_addr[i]) {
 			pci_iounmap(pdev, hw->mem_addr[i]);
@@ -65,11 +67,10 @@ static void mei_txe_pci_iounmap(struct pci_dev *pdev, struct mei_txe_hw *hw)
  * @pdev: PCI device structure
  * @ent: entry in mei_txe_pci_tbl
  *
- * returns 0 on success, <0 on failure.
+ * Return: 0 on success, <0 on failure.
  */
 static int mei_txe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
-	const struct mei_cfg *cfg = (struct mei_cfg *)(ent->driver_data);
 	struct mei_device *dev;
 	struct mei_txe_hw *hw;
 	int err;
@@ -100,7 +101,7 @@ static int mei_txe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	}
 
 	/* allocates and initializes the mei dev structure */
-	dev = mei_txe_dev_init(pdev, cfg);
+	dev = mei_txe_dev_init(pdev);
 	if (!dev) {
 		err = -ENOMEM;
 		goto release_regions;
@@ -371,13 +372,13 @@ static int mei_txe_pm_runtime_resume(struct device *device)
 }
 
 /**
- * mei_txe_set_pm_domain - fill and set pm domian stucture for device
+ * mei_txe_set_pm_domain - fill and set pm domain structure for device
  *
  * @dev: mei_device
  */
 static inline void mei_txe_set_pm_domain(struct mei_device *dev)
 {
-	struct pci_dev *pdev  = dev->pdev;
+	struct pci_dev *pdev  = to_pci_dev(dev->dev);
 
 	if (pdev->dev.bus && pdev->dev.bus->pm) {
 		dev->pg_domain.ops = *pdev->dev.bus->pm;
@@ -391,14 +392,14 @@ static inline void mei_txe_set_pm_domain(struct mei_device *dev)
 }
 
 /**
- * mei_txe_unset_pm_domain - clean pm domian stucture for device
+ * mei_txe_unset_pm_domain - clean pm domain structure for device
  *
  * @dev: mei_device
  */
 static inline void mei_txe_unset_pm_domain(struct mei_device *dev)
 {
 	/* stop using pm callbacks if any */
-	dev->pdev->dev.pm_domain = NULL;
+	dev->dev->pm_domain = NULL;
 }
 #endif /* CONFIG_PM_RUNTIME */
 
diff --git a/drivers/misc/mei/wd.c b/drivers/misc/mei/wd.c
index a84a664dfccb..b836dfffceb5 100644
--- a/drivers/misc/mei/wd.c
+++ b/drivers/misc/mei/wd.c
@@ -17,7 +17,6 @@
 #include <linux/module.h>
 #include <linux/moduleparam.h>
 #include <linux/device.h>
-#include <linux/pci.h>
 #include <linux/sched.h>
 #include <linux/watchdog.h>
 
@@ -42,7 +41,7 @@ const uuid_le mei_wd_guid = UUID_LE(0x05B79A6F, 0x4628, 0x4D7F, 0x89,
 
 static void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
 {
-	dev_dbg(&dev->pdev->dev, "wd: set timeout=%d.\n", timeout);
+	dev_dbg(dev->dev, "wd: set timeout=%d.\n", timeout);
 	memcpy(dev->wd_data, mei_start_wd_params, MEI_WD_HDR_SIZE);
 	memcpy(dev->wd_data + MEI_WD_HDR_SIZE, &timeout, sizeof(u16));
 }
@@ -52,14 +51,14 @@ static void mei_wd_set_start_timeout(struct mei_device *dev, u16 timeout)
  *
  * @dev: the device structure
  *
- * returns -ENOTTY if wd client cannot be found
+ * Return: -ENOTTY if wd client cannot be found
  *         -EIO if write has failed
  *         0 on success
  */
 int mei_wd_host_init(struct mei_device *dev)
 {
 	struct mei_cl *cl = &dev->wd_cl;
-	int id;
+	struct mei_me_client *me_cl;
 	int ret;
 
 	mei_cl_init(cl, dev);
@@ -69,25 +68,26 @@ int mei_wd_host_init(struct mei_device *dev)
 
 
 	/* check for valid client id */
-	id = mei_me_cl_by_uuid(dev, &mei_wd_guid);
-	if (id < 0) {
-		dev_info(&dev->pdev->dev, "wd: failed to find the client\n");
+	me_cl = mei_me_cl_by_uuid(dev, &mei_wd_guid);
+	if (!me_cl) {
+		dev_info(dev->dev, "wd: failed to find the client\n");
 		return -ENOTTY;
 	}
 
-	cl->me_client_id = dev->me_clients[id].client_id;
+	cl->me_client_id = me_cl->client_id;
+	cl->cl_uuid = me_cl->props.protocol_name;
 
 	ret = mei_cl_link(cl, MEI_WD_HOST_CLIENT_ID);
 
 	if (ret < 0) {
-		dev_info(&dev->pdev->dev, "wd: failed link client\n");
+		dev_info(dev->dev, "wd: failed link client\n");
 		return ret;
 	}
 
 	ret = mei_cl_connect(cl, NULL);
 
 	if (ret) {
-		dev_err(&dev->pdev->dev, "wd: failed to connect = %d\n", ret);
+		dev_err(dev->dev, "wd: failed to connect = %d\n", ret);
 		mei_cl_unlink(cl);
 		return ret;
 	}
@@ -105,7 +105,7 @@ int mei_wd_host_init(struct mei_device *dev)
  *
  * @dev: the device structure
  *
- * returns 0 if success,
+ * Return: 0 if success,
  *	-EIO when message send fails
  *	-EINVAL when invalid message is to be sent
  *	-ENODEV on flow control failure
@@ -127,19 +127,19 @@ int mei_wd_send(struct mei_device *dev)
 	else if (!memcmp(dev->wd_data, mei_stop_wd_params, MEI_WD_HDR_SIZE))
 		hdr.length = MEI_WD_STOP_MSG_SIZE;
 	else {
-		dev_err(&dev->pdev->dev, "wd: invalid message is to be sent, aborting\n");
+		dev_err(dev->dev, "wd: invalid message is to be sent, aborting\n");
 		return -EINVAL;
 	}
 
 	ret = mei_write_message(dev, &hdr, dev->wd_data);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "wd: write message failed\n");
+		dev_err(dev->dev, "wd: write message failed\n");
 		return ret;
 	}
 
 	ret = mei_cl_flow_ctrl_reduce(cl);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "wd: flow_ctrl_reduce failed.\n");
+		dev_err(dev->dev, "wd: flow_ctrl_reduce failed.\n");
 		return ret;
 	}
 
@@ -150,9 +150,8 @@ int mei_wd_send(struct mei_device *dev)
  * mei_wd_stop - sends watchdog stop message to fw.
  *
  * @dev: the device structure
- * @preserve: indicate if to keep the timeout value
  *
- * returns 0 if success
+ * Return: 0 if success
  * on error:
  *	-EIO    when message send fails
  *	-EINVAL when invalid message is to be sent
@@ -192,11 +191,10 @@ int mei_wd_stop(struct mei_device *dev)
 	if (dev->wd_state != MEI_WD_IDLE) {
 		/* timeout */
 		ret = -ETIME;
-		dev_warn(&dev->pdev->dev,
-			"wd: stop failed to complete ret=%d.\n", ret);
+		dev_warn(dev->dev, "wd: stop failed to complete ret=%d\n", ret);
 		goto err;
 	}
-	dev_dbg(&dev->pdev->dev, "wd: stop completed after %u msec\n",
+	dev_dbg(dev->dev, "wd: stop completed after %u msec\n",
 			MEI_WD_STOP_TIMEOUT - jiffies_to_msecs(ret));
 	return 0;
 err:
@@ -208,7 +206,7 @@ err:
  *
  * @wd_dev - watchdog device struct
  *
- * returns 0 if success, negative errno code for failure
+ * Return: 0 if success, negative errno code for failure
  */
 static int mei_wd_ops_start(struct watchdog_device *wd_dev)
 {
@@ -222,15 +220,13 @@ static int mei_wd_ops_start(struct watchdog_device *wd_dev)
 	mutex_lock(&dev->device_lock);
 
 	if (dev->dev_state != MEI_DEV_ENABLED) {
-		dev_dbg(&dev->pdev->dev,
-			"wd: dev_state != MEI_DEV_ENABLED  dev_state = %s\n",
+		dev_dbg(dev->dev, "wd: dev_state != MEI_DEV_ENABLED  dev_state = %s\n",
 			mei_dev_state_str(dev->dev_state));
 		goto end_unlock;
 	}
 
 	if (dev->wd_cl.state != MEI_FILE_CONNECTED)	{
-		dev_dbg(&dev->pdev->dev,
-			"MEI Driver is not connected to Watchdog Client\n");
+		dev_dbg(dev->dev, "MEI Driver is not connected to Watchdog Client\n");
 		goto end_unlock;
 	}
 
@@ -247,7 +243,7 @@ end_unlock:
  *
  * @wd_dev - watchdog device struct
  *
- * returns 0 if success, negative errno code for failure
+ * Return: 0 if success, negative errno code for failure
  */
 static int mei_wd_ops_stop(struct watchdog_device *wd_dev)
 {
@@ -269,7 +265,7 @@ static int mei_wd_ops_stop(struct watchdog_device *wd_dev)
  *
  * @wd_dev - watchdog device struct
  *
- * returns 0 if success, negative errno code for failure
+ * Return: 0 if success, negative errno code for failure
  */
 static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 {
@@ -283,7 +279,7 @@ static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 	mutex_lock(&dev->device_lock);
 
 	if (dev->wd_cl.state != MEI_FILE_CONNECTED) {
-		dev_err(&dev->pdev->dev, "wd: not connected.\n");
+		dev_err(dev->dev, "wd: not connected.\n");
 		ret = -ENODEV;
 		goto end;
 	}
@@ -296,7 +292,7 @@ static int mei_wd_ops_ping(struct watchdog_device *wd_dev)
 	/* Check if we can send the ping to HW*/
 	if (ret && mei_hbuf_acquire(dev)) {
 
-		dev_dbg(&dev->pdev->dev, "wd: sending ping\n");
+		dev_dbg(dev->dev, "wd: sending ping\n");
 
 		ret = mei_wd_send(dev);
 		if (ret)
@@ -317,7 +313,7 @@ end:
  * @wd_dev - watchdog device struct
  * @timeout - timeout value to set
  *
- * returns 0 if success, negative errno code for failure
+ * Return: 0 if success, negative errno code for failure
  */
 static int mei_wd_ops_set_timeout(struct watchdog_device *wd_dev,
 		unsigned int timeout)
@@ -379,13 +375,12 @@ int mei_watchdog_register(struct mei_device *dev)
 	ret = watchdog_register_device(&amt_wd_dev);
 	mutex_lock(&dev->device_lock);
 	if (ret) {
-		dev_err(&dev->pdev->dev, "wd: unable to register watchdog device = %d.\n",
+		dev_err(dev->dev, "wd: unable to register watchdog device = %d.\n",
 			ret);
 		return ret;
 	}
 
-	dev_dbg(&dev->pdev->dev,
-		"wd: successfully register watchdog interface.\n");
+	dev_dbg(dev->dev, "wd: successfully register watchdog interface.\n");
 	watchdog_set_drvdata(&amt_wd_dev, dev);
 	return 0;
 }
diff --git a/drivers/misc/mic/card/mic_virtio.c b/drivers/misc/mic/card/mic_virtio.c
index f14b60080c21..e64794730e21 100644
--- a/drivers/misc/mic/card/mic_virtio.c
+++ b/drivers/misc/mic/card/mic_virtio.c
@@ -462,16 +462,12 @@ static void mic_handle_config_change(struct mic_device_desc __iomem *d,
 	struct mic_device_ctrl __iomem *dc
 		= (void __iomem *)d + mic_aligned_desc_size(d);
 	struct mic_vdev *mvdev = (struct mic_vdev *)ioread64(&dc->vdev);
-	struct virtio_driver *drv;
 
 	if (ioread8(&dc->config_change) != MIC_VIRTIO_PARAM_CONFIG_CHANGED)
 		return;
 
 	dev_dbg(mdrv->dev, "%s %d\n", __func__, __LINE__);
-	drv = container_of(mvdev->vdev.dev.driver,
-				struct virtio_driver, driver);
-	if (drv->config_changed)
-		drv->config_changed(&mvdev->vdev);
+	virtio_config_changed(&mvdev->vdev);
 	iowrite8(1, &dc->guest_ack);
 }
 
diff --git a/drivers/misc/spear13xx_pcie_gadget.c b/drivers/misc/spear13xx_pcie_gadget.c
index 2e13614d41e8..fe3ad0ca9a3e 100644
--- a/drivers/misc/spear13xx_pcie_gadget.c
+++ b/drivers/misc/spear13xx_pcie_gadget.c
@@ -9,6 +9,7 @@
  * warranty of any kind, whether express or implied.
  */
 
+#include <linux/device.h>
 #include <linux/clk.h>
 #include <linux/slab.h>
 #include <linux/delay.h>
@@ -743,58 +744,33 @@ static int spear_pcie_gadget_probe(struct platform_device *pdev)
 	struct config_item		*cg_item;
 	struct configfs_subsystem *subsys;
 
-	/* get resource for application registers*/
-
-	res0 = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (!res0) {
-		dev_err(&pdev->dev, "no resource defined\n");
-		return -EBUSY;
-	}
-	if (!request_mem_region(res0->start, resource_size(res0),
-				pdev->name)) {
-		dev_err(&pdev->dev, "pcie gadget region already	claimed\n");
-		return -EBUSY;
-	}
-	/* get resource for dbi registers*/
-
-	res1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	if (!res1) {
-		dev_err(&pdev->dev, "no resource defined\n");
-		goto err_rel_res0;
-	}
-	if (!request_mem_region(res1->start, resource_size(res1),
-				pdev->name)) {
-		dev_err(&pdev->dev, "pcie gadget region already	claimed\n");
-		goto err_rel_res0;
-	}
-
-	target = kzalloc(sizeof(*target), GFP_KERNEL);
+	target = devm_kzalloc(&pdev->dev, sizeof(*target), GFP_KERNEL);
 	if (!target) {
 		dev_err(&pdev->dev, "out of memory\n");
-		status = -ENOMEM;
-		goto err_rel_res;
+		return -ENOMEM;
 	}
 
 	cg_item = &target->subsys.su_group.cg_item;
 	sprintf(cg_item->ci_namebuf, "pcie_gadget.%d", pdev->id);
 	cg_item->ci_type	= &pcie_gadget_target_type;
 	config = &target->config;
-	config->va_app_base = (void __iomem *)ioremap(res0->start,
-			resource_size(res0));
-	if (!config->va_app_base) {
+
+	/* get resource for application registers*/
+	res0 = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	config->va_app_base = devm_ioremap_resource(&pdev->dev, res0);
+	if (IS_ERR(config->va_app_base)) {
 		dev_err(&pdev->dev, "ioremap fail\n");
-		status = -ENOMEM;
-		goto err_kzalloc;
+		return PTR_ERR(config->va_app_base);
 	}
 
+	/* get resource for dbi registers*/
+	res1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
 	config->base = (void __iomem *)res1->start;
 
-	config->va_dbi_base = (void __iomem *)ioremap(res1->start,
-			resource_size(res1));
-	if (!config->va_dbi_base) {
+	config->va_dbi_base = devm_ioremap_resource(&pdev->dev, res1);
+	if (IS_ERR(config->va_dbi_base)) {
 		dev_err(&pdev->dev, "ioremap fail\n");
-		status = -ENOMEM;
-		goto err_iounmap_app;
+		return PTR_ERR(config->va_dbi_base);
 	}
 
 	platform_set_drvdata(pdev, target);
@@ -802,15 +778,15 @@ static int spear_pcie_gadget_probe(struct platform_device *pdev)
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0) {
 		dev_err(&pdev->dev, "no update irq?\n");
-		status = irq;
-		goto err_iounmap;
+		return irq;
 	}
 
-	status = request_irq(irq, spear_pcie_gadget_irq, 0, pdev->name, NULL);
+	status = devm_request_irq(&pdev->dev, irq, spear_pcie_gadget_irq,
+				  0, pdev->name, NULL);
 	if (status) {
 		dev_err(&pdev->dev,
 			"pcie gadget interrupt IRQ%d already claimed\n", irq);
-		goto err_iounmap;
+		return status;
 	}
 
 	/* Register configfs hooks */
@@ -819,7 +795,7 @@ static int spear_pcie_gadget_probe(struct platform_device *pdev)
 	mutex_init(&subsys->su_mutex);
 	status = configfs_register_subsystem(subsys);
 	if (status)
-		goto err_irq;
+		return status;
 
 	/*
 	 * init basic pcie application registers
@@ -835,13 +811,12 @@ static int spear_pcie_gadget_probe(struct platform_device *pdev)
 		clk = clk_get_sys("pcie1", NULL);
 		if (IS_ERR(clk)) {
 			pr_err("%s:couldn't get clk for pcie1\n", __func__);
-			status = PTR_ERR(clk);
-			goto err_irq;
+			return PTR_ERR(clk);
 		}
 		status = clk_enable(clk);
 		if (status) {
 			pr_err("%s:couldn't enable clk for pcie1\n", __func__);
-			goto err_irq;
+			return status;
 		}
 	} else if (pdev->id == 2) {
 		/*
@@ -851,53 +826,26 @@ static int spear_pcie_gadget_probe(struct platform_device *pdev)
 		clk = clk_get_sys("pcie2", NULL);
 		if (IS_ERR(clk)) {
 			pr_err("%s:couldn't get clk for pcie2\n", __func__);
-			status = PTR_ERR(clk);
-			goto err_irq;
+			return PTR_ERR(clk);
 		}
 		status = clk_enable(clk);
 		if (status) {
 			pr_err("%s:couldn't enable clk for pcie2\n", __func__);
-			goto err_irq;
+			return status;
 		}
 	}
 	spear13xx_pcie_device_init(config);
 
 	return 0;
-err_irq:
-	free_irq(irq, NULL);
-err_iounmap:
-	iounmap(config->va_dbi_base);
-err_iounmap_app:
-	iounmap(config->va_app_base);
-err_kzalloc:
-	kfree(target);
-err_rel_res:
-	release_mem_region(res1->start, resource_size(res1));
-err_rel_res0:
-	release_mem_region(res0->start, resource_size(res0));
-	return status;
 }
 
 static int spear_pcie_gadget_remove(struct platform_device *pdev)
 {
-	struct resource *res0, *res1;
 	static struct pcie_gadget_target *target;
-	struct spear_pcie_gadget_config *config;
-	int irq;
 
-	res0 = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	res1 = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	irq = platform_get_irq(pdev, 0);
 	target = platform_get_drvdata(pdev);
-	config = &target->config;
 
-	free_irq(irq, NULL);
-	iounmap(config->va_dbi_base);
-	iounmap(config->va_app_base);
-	release_mem_region(res1->start, resource_size(res1));
-	release_mem_region(res0->start, resource_size(res0));
 	configfs_unregister_subsystem(&target->subsys);
-	kfree(target);
 
 	return 0;
 }
diff --git a/drivers/misc/ti-st/st_core.c b/drivers/misc/ti-st/st_core.c
index 1972d57aadb3..54be83d3efdd 100644
--- a/drivers/misc/ti-st/st_core.c
+++ b/drivers/misc/ti-st/st_core.c
@@ -153,8 +153,9 @@ static void st_reg_complete(struct st_data_s *st_gdata, char err)
 				(st_gdata->list[i]->priv_data, err);
 			pr_info("protocol %d's cb sent %d\n", i, err);
 			if (err) { /* cleanup registered protocol */
-				st_gdata->protos_registered--;
 				st_gdata->is_registered[i] = false;
+				if (st_gdata->protos_registered)
+					st_gdata->protos_registered--;
 			}
 		}
 	}
@@ -639,14 +640,12 @@ long st_unregister(struct st_proto_s *proto)
 		return -EPROTONOSUPPORT;
 	}
 
-	st_gdata->protos_registered--;
+	if (st_gdata->protos_registered)
+		st_gdata->protos_registered--;
+
 	remove_channel_from_table(st_gdata, proto);
 	spin_unlock_irqrestore(&st_gdata->lock, flags);
 
-	/* paranoid check */
-	if (st_gdata->protos_registered < ST_EMPTY)
-		st_gdata->protos_registered = ST_EMPTY;
-
 	if ((st_gdata->protos_registered == ST_EMPTY) &&
 	    (!test_bit(ST_REG_PENDING, &st_gdata->st_state))) {
 		pr_info(" all chnl_ids unregistered ");
diff --git a/drivers/misc/vmw_vmci/vmci_datagram.c b/drivers/misc/vmw_vmci/vmci_datagram.c
index f3cdd904fe4d..822665245588 100644
--- a/drivers/misc/vmw_vmci/vmci_datagram.c
+++ b/drivers/misc/vmw_vmci/vmci_datagram.c
@@ -328,7 +328,8 @@ int vmci_datagram_dispatch(u32 context_id,
 
 	BUILD_BUG_ON(sizeof(struct vmci_datagram) != 24);
 
-	if (VMCI_DG_SIZE(dg) > VMCI_MAX_DG_SIZE) {
+	if (dg->payload_size > VMCI_MAX_DG_SIZE ||
+	    VMCI_DG_SIZE(dg) > VMCI_MAX_DG_SIZE) {
 		pr_devel("Payload (size=%llu bytes) too big to send\n",
 			 (unsigned long long)dg->payload_size);
 		return VMCI_ERROR_INVALID_ARGS;
diff --git a/drivers/misc/vmw_vmci/vmci_guest.c b/drivers/misc/vmw_vmci/vmci_guest.c
index 248399a881af..189b32519748 100644
--- a/drivers/misc/vmw_vmci/vmci_guest.c
+++ b/drivers/misc/vmw_vmci/vmci_guest.c
@@ -35,7 +35,6 @@
 #include "vmci_driver.h"
 #include "vmci_event.h"
 
-#define PCI_VENDOR_ID_VMWARE		0x15AD
 #define PCI_DEVICE_ID_VMWARE_VMCI	0x0740
 
 #define VMCI_UTIL_NUM_RESOURCES 1
diff --git a/drivers/mmc/card/block.c b/drivers/mmc/card/block.c
index ede41f05c392..1fa4c80ff886 100644
--- a/drivers/mmc/card/block.c
+++ b/drivers/mmc/card/block.c
@@ -977,7 +977,7 @@ static int mmc_blk_cmd_recovery(struct mmc_card *card, struct request *req,
 		return ERR_CONTINUE;
 
 	/* Now for stop errors.  These aren't fatal to the transfer. */
-	pr_err("%s: error %d sending stop command, original cmd response %#x, card status %#x\n",
+	pr_info("%s: error %d sending stop command, original cmd response %#x, card status %#x\n",
 	       req->rq_disk->disk_name, brq->stop.error,
 	       brq->cmd.resp[0], status);
 
@@ -1398,10 +1398,15 @@ static void mmc_blk_rw_rq_prep(struct mmc_queue_req *mqrq,
 		if (disable_multi)
 			brq->data.blocks = 1;
 
-		/* Some controllers can't do multiblock reads due to hw bugs */
-		if (card->host->caps2 & MMC_CAP2_NO_MULTI_READ &&
-		    rq_data_dir(req) == READ)
-			brq->data.blocks = 1;
+		/*
+		 * Some controllers have HW issues while operating
+		 * in multiple I/O mode
+		 */
+		if (card->host->ops->multi_io_quirk)
+			brq->data.blocks = card->host->ops->multi_io_quirk(card,
+						(rq_data_dir(req) == READ) ?
+						MMC_DATA_READ : MMC_DATA_WRITE,
+						brq->data.blocks);
 	}
 
 	if (brq->data.blocks > 1 || do_rel_wr) {
@@ -1923,8 +1928,8 @@ static int mmc_blk_issue_rw_rq(struct mmc_queue *mq, struct request *rqc)
 		case MMC_BLK_ECC_ERR:
 			if (brq->data.blocks > 1) {
 				/* Redo read one sector at a time */
-				pr_warning("%s: retrying using single block read\n",
-					   req->rq_disk->disk_name);
+				pr_warn("%s: retrying using single block read\n",
+					req->rq_disk->disk_name);
 				disable_multi = 1;
 				break;
 			}
@@ -2131,7 +2136,7 @@ static struct mmc_blk_data *mmc_blk_alloc_req(struct mmc_card *card,
 	md->disk->queue = md->queue.queue;
 	md->disk->driverfs_dev = parent;
 	set_disk_ro(md->disk, md->read_only || default_ro);
-	if (area_type & MMC_BLK_DATA_AREA_RPMB)
+	if (area_type & (MMC_BLK_DATA_AREA_RPMB | MMC_BLK_DATA_AREA_BOOT))
 		md->disk->flags |= GENHD_FL_NO_PART_SCAN;
 
 	/*
diff --git a/drivers/mmc/card/queue.c b/drivers/mmc/card/queue.c
index 3e049c13429c..cfa6110632c3 100644
--- a/drivers/mmc/card/queue.c
+++ b/drivers/mmc/card/queue.c
@@ -210,6 +210,7 @@ int mmc_init_queue(struct mmc_queue *mq, struct mmc_card *card,
 
 	blk_queue_prep_rq(mq->queue, mmc_prep_request);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, mq->queue);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, mq->queue);
 	if (mmc_can_erase(card))
 		mmc_queue_setup_discard(mq->queue, card);
 
@@ -229,14 +230,12 @@ int mmc_init_queue(struct mmc_queue *mq, struct mmc_card *card,
 		if (bouncesz > 512) {
 			mqrq_cur->bounce_buf = kmalloc(bouncesz, GFP_KERNEL);
 			if (!mqrq_cur->bounce_buf) {
-				pr_warning("%s: unable to "
-					"allocate bounce cur buffer\n",
+				pr_warn("%s: unable to allocate bounce cur buffer\n",
 					mmc_card_name(card));
 			}
 			mqrq_prev->bounce_buf = kmalloc(bouncesz, GFP_KERNEL);
 			if (!mqrq_prev->bounce_buf) {
-				pr_warning("%s: unable to "
-					"allocate bounce prev buffer\n",
+				pr_warn("%s: unable to allocate bounce prev buffer\n",
 					mmc_card_name(card));
 				kfree(mqrq_cur->bounce_buf);
 				mqrq_cur->bounce_buf = NULL;
diff --git a/drivers/mmc/card/sdio_uart.c b/drivers/mmc/card/sdio_uart.c
index f093cea0d060..d2de5925b73e 100644
--- a/drivers/mmc/card/sdio_uart.c
+++ b/drivers/mmc/card/sdio_uart.c
@@ -1063,8 +1063,8 @@ static int sdio_uart_probe(struct sdio_func *func,
 		return -ENOMEM;
 
 	if (func->class == SDIO_CLASS_UART) {
-		pr_warning("%s: need info on UART class basic setup\n",
-		       sdio_func_id(func));
+		pr_warn("%s: need info on UART class basic setup\n",
+			sdio_func_id(func));
 		kfree(port);
 		return -ENOSYS;
 	} else if (func->class == SDIO_CLASS_GPS) {
@@ -1082,9 +1082,8 @@ static int sdio_uart_probe(struct sdio_func *func,
 				break;
 		}
 		if (!tpl) {
-			pr_warning(
-       "%s: can't find tuple 0x91 subtuple 0 (SUBTPL_SIOREG) for GPS class\n",
-			       sdio_func_id(func));
+			pr_warn("%s: can't find tuple 0x91 subtuple 0 (SUBTPL_SIOREG) for GPS class\n",
+				sdio_func_id(func));
 			kfree(port);
 			return -EINVAL;
 		}
diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index d03a080fb9cd..f26a5f1d926d 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -433,8 +433,8 @@ static void mmc_wait_for_req_done(struct mmc_host *host,
 		 */
 		if (cmd->sanitize_busy && cmd->error == -ETIMEDOUT) {
 			if (!mmc_interrupt_hpi(host->card)) {
-				pr_warning("%s: %s: Interrupted sanitize\n",
-					   mmc_hostname(host), __func__);
+				pr_warn("%s: %s: Interrupted sanitize\n",
+					mmc_hostname(host), __func__);
 				cmd->error = 0;
 				break;
 			} else {
@@ -995,7 +995,7 @@ void mmc_set_chip_select(struct mmc_host *host, int mode)
  */
 static void __mmc_set_clock(struct mmc_host *host, unsigned int hz)
 {
-	WARN_ON(hz < host->f_min);
+	WARN_ON(hz && hz < host->f_min);
 
 	if (hz > host->f_max)
 		hz = host->f_max;
@@ -1221,15 +1221,14 @@ int mmc_regulator_get_ocrmask(struct regulator *supply)
 	int			result = 0;
 	int			count;
 	int			i;
+	int			vdd_uV;
+	int			vdd_mV;
 
 	count = regulator_count_voltages(supply);
 	if (count < 0)
 		return count;
 
 	for (i = 0; i < count; i++) {
-		int		vdd_uV;
-		int		vdd_mV;
-
 		vdd_uV = regulator_list_voltage(supply, i);
 		if (vdd_uV <= 0)
 			continue;
@@ -1238,6 +1237,15 @@ int mmc_regulator_get_ocrmask(struct regulator *supply)
 		result |= mmc_vddrange_to_ocrmask(vdd_mV, vdd_mV);
 	}
 
+	if (!result) {
+		vdd_uV = regulator_get_voltage(supply);
+		if (vdd_uV <= 0)
+			return vdd_uV;
+
+		vdd_mV = vdd_uV / 1000;
+		result = mmc_vddrange_to_ocrmask(vdd_mV, vdd_mV);
+	}
+
 	return result;
 }
 EXPORT_SYMBOL_GPL(mmc_regulator_get_ocrmask);
@@ -1263,7 +1271,6 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
 
 	if (vdd_bit) {
 		int		tmp;
-		int		voltage;
 
 		/*
 		 * REVISIT mmc_vddrange_to_ocrmask() may have set some
@@ -1280,22 +1287,7 @@ int mmc_regulator_set_ocr(struct mmc_host *mmc,
 			max_uV = min_uV + 100 * 1000;
 		}
 
-		/*
-		 * If we're using a fixed/static regulator, don't call
-		 * regulator_set_voltage; it would fail.
-		 */
-		voltage = regulator_get_voltage(supply);
-
-		if (!regulator_can_change_voltage(supply))
-			min_uV = max_uV = voltage;
-
-		if (voltage < 0)
-			result = voltage;
-		else if (voltage < min_uV || voltage > max_uV)
-			result = regulator_set_voltage(supply, min_uV, max_uV);
-		else
-			result = 0;
-
+		result = regulator_set_voltage(supply, min_uV, max_uV);
 		if (result == 0 && !mmc->regulator_enabled) {
 			result = regulator_enable(supply);
 			if (!result)
@@ -1425,8 +1417,8 @@ int mmc_set_signal_voltage(struct mmc_host *host, int signal_voltage, u32 ocr)
 	if (!host->ops->start_signal_voltage_switch)
 		return -EPERM;
 	if (!host->ops->card_busy)
-		pr_warning("%s: cannot verify signal voltage switch\n",
-				mmc_hostname(host));
+		pr_warn("%s: cannot verify signal voltage switch\n",
+			mmc_hostname(host));
 
 	cmd.opcode = SD_SWITCH_VOLTAGE;
 	cmd.arg = 0;
@@ -1761,7 +1753,7 @@ void mmc_init_erase(struct mmc_card *card)
 		card->erase_shift = ffs(card->ssr.au) - 1;
 	} else if (card->ext_csd.hc_erase_size) {
 		card->pref_erase = card->ext_csd.hc_erase_size;
-	} else {
+	} else if (card->erase_size) {
 		sz = (card->csd.capacity << (card->csd.read_blkbits - 9)) >> 11;
 		if (sz < 128)
 			card->pref_erase = 512 * 1024 / 512;
@@ -1778,7 +1770,8 @@ void mmc_init_erase(struct mmc_card *card)
 			if (sz)
 				card->pref_erase += card->erase_size - sz;
 		}
-	}
+	} else
+		card->pref_erase = 0;
 }
 
 static unsigned int mmc_mmc_erase_timeout(struct mmc_card *card,
@@ -2489,6 +2482,7 @@ void mmc_start_host(struct mmc_host *host)
 {
 	host->f_init = max(freqs[0], host->f_min);
 	host->rescan_disable = 0;
+	host->ios.power_mode = MMC_POWER_UNDEFINED;
 	if (host->caps2 & MMC_CAP2_NO_PRESCAN_POWERUP)
 		mmc_power_off(host);
 	else
diff --git a/drivers/mmc/core/host.c b/drivers/mmc/core/host.c
index 95cceae96944..03c53b72a2d6 100644
--- a/drivers/mmc/core/host.c
+++ b/drivers/mmc/core/host.c
@@ -310,9 +310,8 @@ int mmc_of_parse(struct mmc_host *host)
 {
 	struct device_node *np;
 	u32 bus_width;
-	bool explicit_inv_wp, gpio_inv_wp = false;
-	enum of_gpio_flags flags;
-	int len, ret, gpio;
+	int len, ret;
+	bool cap_invert, gpio_invert;
 
 	if (!host->parent || !host->parent->of_node)
 		return 0;
@@ -360,59 +359,62 @@ int mmc_of_parse(struct mmc_host *host)
 	if (of_find_property(np, "non-removable", &len)) {
 		host->caps |= MMC_CAP_NONREMOVABLE;
 	} else {
-		bool explicit_inv_cd, gpio_inv_cd = false;
-
-		explicit_inv_cd = of_property_read_bool(np, "cd-inverted");
+		if (of_property_read_bool(np, "cd-inverted"))
+			cap_invert = true;
+		else
+			cap_invert = false;
 
 		if (of_find_property(np, "broken-cd", &len))
 			host->caps |= MMC_CAP_NEEDS_POLL;
 
-		gpio = of_get_named_gpio_flags(np, "cd-gpios", 0, &flags);
-		if (gpio == -EPROBE_DEFER)
-			return gpio;
-		if (gpio_is_valid(gpio)) {
-			if (!(flags & OF_GPIO_ACTIVE_LOW))
-				gpio_inv_cd = true;
-
-			ret = mmc_gpio_request_cd(host, gpio, 0);
-			if (ret < 0) {
-				dev_err(host->parent,
-					"Failed to request CD GPIO #%d: %d!\n",
-					gpio, ret);
+		ret = mmc_gpiod_request_cd(host, "cd", 0, true,
+					   0, &gpio_invert);
+		if (ret) {
+			if (ret == -EPROBE_DEFER)
 				return ret;
-			} else {
-				dev_info(host->parent, "Got CD GPIO #%d.\n",
-					 gpio);
+			if (ret != -ENOENT) {
+				dev_err(host->parent,
+					"Failed to request CD GPIO: %d\n",
+					ret);
 			}
-		}
-
-		if (explicit_inv_cd ^ gpio_inv_cd)
+		} else
+			dev_info(host->parent, "Got CD GPIO\n");
+
+		/*
+		 * There are two ways to flag that the CD line is inverted:
+		 * through the cd-inverted flag and by the GPIO line itself
+		 * being inverted from the GPIO subsystem. This is a leftover
+		 * from the times when the GPIO subsystem did not make it
+		 * possible to flag a line as inverted.
+		 *
+		 * If the capability on the host AND the GPIO line are
+		 * both inverted, the end result is that the CD line is
+		 * not inverted.
+		 */
+		if (cap_invert ^ gpio_invert)
 			host->caps2 |= MMC_CAP2_CD_ACTIVE_HIGH;
 	}
 
 	/* Parse Write Protection */
-	explicit_inv_wp = of_property_read_bool(np, "wp-inverted");
-
-	gpio = of_get_named_gpio_flags(np, "wp-gpios", 0, &flags);
-	if (gpio == -EPROBE_DEFER) {
-		ret = -EPROBE_DEFER;
-		goto out;
-	}
-	if (gpio_is_valid(gpio)) {
-		if (!(flags & OF_GPIO_ACTIVE_LOW))
-			gpio_inv_wp = true;
+	if (of_property_read_bool(np, "wp-inverted"))
+		cap_invert = true;
+	else
+		cap_invert = false;
 
-		ret = mmc_gpio_request_ro(host, gpio);
-		if (ret < 0) {
-			dev_err(host->parent,
-				"Failed to request WP GPIO: %d!\n", ret);
+	ret = mmc_gpiod_request_ro(host, "wp", 0, false, 0, &gpio_invert);
+	if (ret) {
+		if (ret == -EPROBE_DEFER)
 			goto out;
-		} else {
-				dev_info(host->parent, "Got WP GPIO #%d.\n",
-					 gpio);
+		if (ret != -ENOENT) {
+			dev_err(host->parent,
+				"Failed to request WP GPIO: %d\n",
+				ret);
 		}
-	}
-	if (explicit_inv_wp ^ gpio_inv_wp)
+	} else
+		dev_info(host->parent, "Got WP GPIO\n");
+
+	/* See the comment on CD inversion above */
+	if (cap_invert ^ gpio_invert)
 		host->caps2 |= MMC_CAP2_RO_ACTIVE_HIGH;
 
 	if (of_find_property(np, "cap-sd-highspeed", &len))
@@ -452,6 +454,14 @@ int mmc_of_parse(struct mmc_host *host)
 	if (of_find_property(np, "mmc-hs400-1_2v", &len))
 		host->caps2 |= MMC_CAP2_HS400_1_2V | MMC_CAP2_HS200_1_2V_SDR;
 
+	host->dsr_req = !of_property_read_u32(np, "dsr", &host->dsr);
+	if (host->dsr_req && (host->dsr & ~0xffff)) {
+		dev_err(host->parent,
+			"device tree specified broken value for DSR: 0x%x, ignoring\n",
+			host->dsr);
+		host->dsr_req = 0;
+	}
+
 	return 0;
 
 out:
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index 1eda8dd8c867..a301a78a2bd1 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -162,6 +162,7 @@ static int mmc_decode_csd(struct mmc_card *card)
 	csd->read_partial = UNSTUFF_BITS(resp, 79, 1);
 	csd->write_misalign = UNSTUFF_BITS(resp, 78, 1);
 	csd->read_misalign = UNSTUFF_BITS(resp, 77, 1);
+	csd->dsr_imp = UNSTUFF_BITS(resp, 76, 1);
 	csd->r2w_factor = UNSTUFF_BITS(resp, 26, 3);
 	csd->write_blkbits = UNSTUFF_BITS(resp, 22, 4);
 	csd->write_partial = UNSTUFF_BITS(resp, 21, 1);
@@ -225,9 +226,7 @@ static int mmc_get_ext_csd(struct mmc_card *card, u8 **new_ext_csd)
 				"Card will be ignored.\n",
 				mmc_hostname(card->host));
 		} else {
-			pr_warning("%s: unable to read "
-				"EXT_CSD, performance might "
-				"suffer.\n",
+			pr_warn("%s: unable to read EXT_CSD, performance might suffer\n",
 				mmc_hostname(card->host));
 			err = 0;
 		}
@@ -298,6 +297,97 @@ static void mmc_select_card_type(struct mmc_card *card)
 	card->mmc_avail_type = avail_type;
 }
 
+static void mmc_manage_enhanced_area(struct mmc_card *card, u8 *ext_csd)
+{
+	u8 hc_erase_grp_sz, hc_wp_grp_sz;
+
+	/*
+	 * Disable these attributes by default
+	 */
+	card->ext_csd.enhanced_area_offset = -EINVAL;
+	card->ext_csd.enhanced_area_size = -EINVAL;
+
+	/*
+	 * Enhanced area feature support -- check whether the eMMC
+	 * card has the Enhanced area enabled.  If so, export enhanced
+	 * area offset and size to user by adding sysfs interface.
+	 */
+	if ((ext_csd[EXT_CSD_PARTITION_SUPPORT] & 0x2) &&
+	    (ext_csd[EXT_CSD_PARTITION_ATTRIBUTE] & 0x1)) {
+		if (card->ext_csd.partition_setting_completed) {
+			hc_erase_grp_sz =
+				ext_csd[EXT_CSD_HC_ERASE_GRP_SIZE];
+			hc_wp_grp_sz =
+				ext_csd[EXT_CSD_HC_WP_GRP_SIZE];
+
+			/*
+			 * calculate the enhanced data area offset, in bytes
+			 */
+			card->ext_csd.enhanced_area_offset =
+				(ext_csd[139] << 24) + (ext_csd[138] << 16) +
+				(ext_csd[137] << 8) + ext_csd[136];
+			if (mmc_card_blockaddr(card))
+				card->ext_csd.enhanced_area_offset <<= 9;
+			/*
+			 * calculate the enhanced data area size, in kilobytes
+			 */
+			card->ext_csd.enhanced_area_size =
+				(ext_csd[142] << 16) + (ext_csd[141] << 8) +
+				ext_csd[140];
+			card->ext_csd.enhanced_area_size *=
+				(size_t)(hc_erase_grp_sz * hc_wp_grp_sz);
+			card->ext_csd.enhanced_area_size <<= 9;
+		} else {
+			pr_warn("%s: defines enhanced area without partition setting complete\n",
+				mmc_hostname(card->host));
+		}
+	}
+}
+
+static void mmc_manage_gp_partitions(struct mmc_card *card, u8 *ext_csd)
+{
+	int idx;
+	u8 hc_erase_grp_sz, hc_wp_grp_sz;
+	unsigned int part_size;
+
+	/*
+	 * General purpose partition feature support --
+	 * If ext_csd has the size of general purpose partitions,
+	 * set size, part_cfg, partition name in mmc_part.
+	 */
+	if (ext_csd[EXT_CSD_PARTITION_SUPPORT] &
+	    EXT_CSD_PART_SUPPORT_PART_EN) {
+		hc_erase_grp_sz =
+			ext_csd[EXT_CSD_HC_ERASE_GRP_SIZE];
+		hc_wp_grp_sz =
+			ext_csd[EXT_CSD_HC_WP_GRP_SIZE];
+
+		for (idx = 0; idx < MMC_NUM_GP_PARTITION; idx++) {
+			if (!ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3] &&
+			    !ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 1] &&
+			    !ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 2])
+				continue;
+			if (card->ext_csd.partition_setting_completed == 0) {
+				pr_warn("%s: has partition size defined without partition complete\n",
+					mmc_hostname(card->host));
+				break;
+			}
+			part_size =
+				(ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 2]
+				<< 16) +
+				(ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 1]
+				<< 8) +
+				ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3];
+			part_size *= (size_t)(hc_erase_grp_sz *
+				hc_wp_grp_sz);
+			mmc_part_add(card, part_size << 19,
+				EXT_CSD_PART_CONFIG_ACC_GP0 + idx,
+				"gp%d", idx, false,
+				MMC_BLK_DATA_AREA_GP);
+		}
+	}
+}
+
 /*
  * Decode extended CSD.
  */
@@ -305,7 +395,6 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 {
 	int err = 0, idx;
 	unsigned int part_size;
-	u8 hc_erase_grp_sz = 0, hc_wp_grp_sz = 0;
 
 	BUG_ON(!card);
 
@@ -402,80 +491,16 @@ static int mmc_read_ext_csd(struct mmc_card *card, u8 *ext_csd)
 		ext_csd[EXT_CSD_TRIM_MULT];
 	card->ext_csd.raw_partition_support = ext_csd[EXT_CSD_PARTITION_SUPPORT];
 	if (card->ext_csd.rev >= 4) {
-		/*
-		 * Enhanced area feature support -- check whether the eMMC
-		 * card has the Enhanced area enabled.  If so, export enhanced
-		 * area offset and size to user by adding sysfs interface.
-		 */
-		if ((ext_csd[EXT_CSD_PARTITION_SUPPORT] & 0x2) &&
-		    (ext_csd[EXT_CSD_PARTITION_ATTRIBUTE] & 0x1)) {
-			hc_erase_grp_sz =
-				ext_csd[EXT_CSD_HC_ERASE_GRP_SIZE];
-			hc_wp_grp_sz =
-				ext_csd[EXT_CSD_HC_WP_GRP_SIZE];
+		if (ext_csd[EXT_CSD_PARTITION_SETTING_COMPLETED] &
+		    EXT_CSD_PART_SETTING_COMPLETED)
+			card->ext_csd.partition_setting_completed = 1;
+		else
+			card->ext_csd.partition_setting_completed = 0;
 
-			card->ext_csd.enhanced_area_en = 1;
-			/*
-			 * calculate the enhanced data area offset, in bytes
-			 */
-			card->ext_csd.enhanced_area_offset =
-				(ext_csd[139] << 24) + (ext_csd[138] << 16) +
-				(ext_csd[137] << 8) + ext_csd[136];
-			if (mmc_card_blockaddr(card))
-				card->ext_csd.enhanced_area_offset <<= 9;
-			/*
-			 * calculate the enhanced data area size, in kilobytes
-			 */
-			card->ext_csd.enhanced_area_size =
-				(ext_csd[142] << 16) + (ext_csd[141] << 8) +
-				ext_csd[140];
-			card->ext_csd.enhanced_area_size *=
-				(size_t)(hc_erase_grp_sz * hc_wp_grp_sz);
-			card->ext_csd.enhanced_area_size <<= 9;
-		} else {
-			/*
-			 * If the enhanced area is not enabled, disable these
-			 * device attributes.
-			 */
-			card->ext_csd.enhanced_area_offset = -EINVAL;
-			card->ext_csd.enhanced_area_size = -EINVAL;
-		}
+		mmc_manage_enhanced_area(card, ext_csd);
 
-		/*
-		 * General purpose partition feature support --
-		 * If ext_csd has the size of general purpose partitions,
-		 * set size, part_cfg, partition name in mmc_part.
-		 */
-		if (ext_csd[EXT_CSD_PARTITION_SUPPORT] &
-			EXT_CSD_PART_SUPPORT_PART_EN) {
-			if (card->ext_csd.enhanced_area_en != 1) {
-				hc_erase_grp_sz =
-					ext_csd[EXT_CSD_HC_ERASE_GRP_SIZE];
-				hc_wp_grp_sz =
-					ext_csd[EXT_CSD_HC_WP_GRP_SIZE];
-
-				card->ext_csd.enhanced_area_en = 1;
-			}
+		mmc_manage_gp_partitions(card, ext_csd);
 
-			for (idx = 0; idx < MMC_NUM_GP_PARTITION; idx++) {
-				if (!ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3] &&
-				!ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 1] &&
-				!ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 2])
-					continue;
-				part_size =
-				(ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 2]
-					<< 16) +
-				(ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3 + 1]
-					<< 8) +
-				ext_csd[EXT_CSD_GP_SIZE_MULT + idx * 3];
-				part_size *= (size_t)(hc_erase_grp_sz *
-					hc_wp_grp_sz);
-				mmc_part_add(card, part_size << 19,
-					EXT_CSD_PART_CONFIG_ACC_GP0 + idx,
-					"gp%d", idx, false,
-					MMC_BLK_DATA_AREA_GP);
-			}
-		}
 		card->ext_csd.sec_trim_mult =
 			ext_csd[EXT_CSD_SEC_TRIM_MULT];
 		card->ext_csd.sec_erase_mult =
@@ -789,8 +814,8 @@ static int __mmc_select_powerclass(struct mmc_card *card,
 				ext_csd->raw_pwr_cl_200_360;
 		break;
 	default:
-		pr_warning("%s: Voltage range not supported "
-			   "for power class.\n", mmc_hostname(host));
+		pr_warn("%s: Voltage range not supported for power class\n",
+			mmc_hostname(host));
 		return -EINVAL;
 	}
 
@@ -987,19 +1012,35 @@ static int mmc_select_hs_ddr(struct mmc_card *card)
 	 * 1.8V vccq at 3.3V core voltage (vcc) is not required
 	 * in the JEDEC spec for DDR.
 	 *
-	 * Do not force change in vccq since we are obviously
-	 * working and no change to vccq is needed.
+	 * Even (e)MMC card can support 3.3v to 1.2v vccq, but not all
+	 * host controller can support this, like some of the SDHCI
+	 * controller which connect to an eMMC device. Some of these
+	 * host controller still needs to use 1.8v vccq for supporting
+	 * DDR mode.
+	 *
+	 * So the sequence will be:
+	 * if (host and device can both support 1.2v IO)
+	 *	use 1.2v IO;
+	 * else if (host and device can both support 1.8v IO)
+	 *	use 1.8v IO;
+	 * so if host and device can only support 3.3v IO, this is the
+	 * last choice.
 	 *
 	 * WARNING: eMMC rules are NOT the same as SD DDR
 	 */
-	if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_DDR_1_2V) {
-		err = __mmc_set_signal_voltage(host,
-				MMC_SIGNAL_VOLTAGE_120);
-		if (err)
-			return err;
-	}
+	err = -EINVAL;
+	if (card->mmc_avail_type & EXT_CSD_CARD_TYPE_DDR_1_2V)
+		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_120);
+
+	if (err && (card->mmc_avail_type & EXT_CSD_CARD_TYPE_DDR_1_8V))
+		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_180);
 
-	mmc_set_timing(host, MMC_TIMING_MMC_DDR52);
+	/* make sure vccq is 3.3v after switching disaster */
+	if (err)
+		err = __mmc_set_signal_voltage(host, MMC_SIGNAL_VOLTAGE_330);
+
+	if (!err)
+		mmc_set_timing(host, MMC_TIMING_MMC_DDR52);
 
 	return err;
 }
@@ -1134,6 +1175,38 @@ bus_speed:
 	return err;
 }
 
+const u8 tuning_blk_pattern_4bit[MMC_TUNING_BLK_PATTERN_4BIT_SIZE] = {
+	0xff, 0x0f, 0xff, 0x00, 0xff, 0xcc, 0xc3, 0xcc,
+	0xc3, 0x3c, 0xcc, 0xff, 0xfe, 0xff, 0xfe, 0xef,
+	0xff, 0xdf, 0xff, 0xdd, 0xff, 0xfb, 0xff, 0xfb,
+	0xbf, 0xff, 0x7f, 0xff, 0x77, 0xf7, 0xbd, 0xef,
+	0xff, 0xf0, 0xff, 0xf0, 0x0f, 0xfc, 0xcc, 0x3c,
+	0xcc, 0x33, 0xcc, 0xcf, 0xff, 0xef, 0xff, 0xee,
+	0xff, 0xfd, 0xff, 0xfd, 0xdf, 0xff, 0xbf, 0xff,
+	0xbb, 0xff, 0xf7, 0xff, 0xf7, 0x7f, 0x7b, 0xde,
+};
+EXPORT_SYMBOL(tuning_blk_pattern_4bit);
+
+const u8 tuning_blk_pattern_8bit[MMC_TUNING_BLK_PATTERN_8BIT_SIZE] = {
+	0xff, 0xff, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00,
+	0xff, 0xff, 0xcc, 0xcc, 0xcc, 0x33, 0xcc, 0xcc,
+	0xcc, 0x33, 0x33, 0xcc, 0xcc, 0xcc, 0xff, 0xff,
+	0xff, 0xee, 0xff, 0xff, 0xff, 0xee, 0xee, 0xff,
+	0xff, 0xff, 0xdd, 0xff, 0xff, 0xff, 0xdd, 0xdd,
+	0xff, 0xff, 0xff, 0xbb, 0xff, 0xff, 0xff, 0xbb,
+	0xbb, 0xff, 0xff, 0xff, 0x77, 0xff, 0xff, 0xff,
+	0x77, 0x77, 0xff, 0x77, 0xbb, 0xdd, 0xee, 0xff,
+	0xff, 0xff, 0xff, 0x00, 0xff, 0xff, 0xff, 0x00,
+	0x00, 0xff, 0xff, 0xcc, 0xcc, 0xcc, 0x33, 0xcc,
+	0xcc, 0xcc, 0x33, 0x33, 0xcc, 0xcc, 0xcc, 0xff,
+	0xff, 0xff, 0xee, 0xff, 0xff, 0xff, 0xee, 0xee,
+	0xff, 0xff, 0xff, 0xdd, 0xff, 0xff, 0xff, 0xdd,
+	0xdd, 0xff, 0xff, 0xff, 0xbb, 0xff, 0xff, 0xff,
+	0xbb, 0xbb, 0xff, 0xff, 0xff, 0x77, 0xff, 0xff,
+	0xff, 0x77, 0x77, 0xff, 0x77, 0xbb, 0xdd, 0xee,
+};
+EXPORT_SYMBOL(tuning_blk_pattern_8bit);
+
 /*
  * Execute tuning sequence to seek the proper bus operating
  * conditions for HS200 and HS400, which sends CMD21 to the device.
@@ -1272,6 +1345,13 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	}
 
 	/*
+	 * handling only for cards supporting DSR and hosts requesting
+	 * DSR configuration
+	 */
+	if (card->csd.dsr_imp && host->dsr_req)
+		mmc_set_dsr(host);
+
+	/*
 	 * Select card, as all following commands rely on that.
 	 */
 	if (!mmc_host_is_spi(host)) {
@@ -1308,7 +1388,7 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 	 * If enhanced_area_en is TRUE, host needs to enable ERASE_GRP_DEF
 	 * bit.  This bit will be lost every time after a reset or power off.
 	 */
-	if (card->ext_csd.enhanced_area_en ||
+	if (card->ext_csd.partition_setting_completed ||
 	    (card->ext_csd.rev >= 3 && (host->caps2 & MMC_CAP2_HC_ERASE_SZ))) {
 		err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
 				 EXT_CSD_ERASE_GROUP_DEF, 1,
@@ -1408,8 +1488,8 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 		if (err && err != -EBADMSG)
 			goto free_card;
 		if (err) {
-			pr_warning("%s: Enabling HPI failed\n",
-				   mmc_hostname(card->host));
+			pr_warn("%s: Enabling HPI failed\n",
+				mmc_hostname(card->host));
 			err = 0;
 		} else
 			card->ext_csd.hpi_en = 1;
@@ -1430,9 +1510,8 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
 		 * Only if no error, cache is turned on successfully.
 		 */
 		if (err) {
-			pr_warning("%s: Cache is supported, "
-					"but failed to turn on (%d)\n",
-					mmc_hostname(card->host), err);
+			pr_warn("%s: Cache is supported, but failed to turn on (%d)\n",
+				mmc_hostname(card->host), err);
 			card->ext_csd.cache_ctrl = 0;
 			err = 0;
 		} else {
diff --git a/drivers/mmc/core/mmc_ops.c b/drivers/mmc/core/mmc_ops.c
index f51b5ba3bbea..7911e0510a1d 100644
--- a/drivers/mmc/core/mmc_ops.c
+++ b/drivers/mmc/core/mmc_ops.c
@@ -93,6 +93,26 @@ int mmc_deselect_cards(struct mmc_host *host)
 	return _mmc_select_card(host, NULL);
 }
 
+/*
+ * Write the value specified in the device tree or board code into the optional
+ * 16 bit Driver Stage Register. This can be used to tune raise/fall times and
+ * drive strength of the DAT and CMD outputs. The actual meaning of a given
+ * value is hardware dependant.
+ * The presence of the DSR register can be determined from the CSD register,
+ * bit 76.
+ */
+int mmc_set_dsr(struct mmc_host *host)
+{
+	struct mmc_command cmd = {0};
+
+	cmd.opcode = MMC_SET_DSR;
+
+	cmd.arg = (host->dsr << 16) | 0xffff;
+	cmd.flags = MMC_RSP_NONE | MMC_CMD_AC;
+
+	return mmc_wait_for_cmd(host, &cmd, MMC_CMD_RETRIES);
+}
+
 int mmc_go_idle(struct mmc_host *host)
 {
 	int err;
@@ -629,8 +649,8 @@ int mmc_send_hpi_cmd(struct mmc_card *card, u32 *status)
 	int err;
 
 	if (!card->ext_csd.hpi) {
-		pr_warning("%s: Card didn't support HPI command\n",
-			   mmc_hostname(card->host));
+		pr_warn("%s: Card didn't support HPI command\n",
+			mmc_hostname(card->host));
 		return -EINVAL;
 	}
 
diff --git a/drivers/mmc/core/mmc_ops.h b/drivers/mmc/core/mmc_ops.h
index 80ae9f4e0293..390dac665b2a 100644
--- a/drivers/mmc/core/mmc_ops.h
+++ b/drivers/mmc/core/mmc_ops.h
@@ -14,6 +14,7 @@
 
 int mmc_select_card(struct mmc_card *card);
 int mmc_deselect_cards(struct mmc_host *host);
+int mmc_set_dsr(struct mmc_host *host);
 int mmc_go_idle(struct mmc_host *host);
 int mmc_send_op_cond(struct mmc_host *host, u32 ocr, u32 *rocr);
 int mmc_all_send_cid(struct mmc_host *host, u32 *cid);
diff --git a/drivers/mmc/core/sd.c b/drivers/mmc/core/sd.c
index 0c44510bf717..d90a6de7901d 100644
--- a/drivers/mmc/core/sd.c
+++ b/drivers/mmc/core/sd.c
@@ -127,6 +127,7 @@ static int mmc_decode_csd(struct mmc_card *card)
 		csd->read_partial = UNSTUFF_BITS(resp, 79, 1);
 		csd->write_misalign = UNSTUFF_BITS(resp, 78, 1);
 		csd->read_misalign = UNSTUFF_BITS(resp, 77, 1);
+		csd->dsr_imp = UNSTUFF_BITS(resp, 76, 1);
 		csd->r2w_factor = UNSTUFF_BITS(resp, 26, 3);
 		csd->write_blkbits = UNSTUFF_BITS(resp, 22, 4);
 		csd->write_partial = UNSTUFF_BITS(resp, 21, 1);
@@ -228,8 +229,8 @@ static int mmc_read_ssr(struct mmc_card *card)
 	u32 *ssr;
 
 	if (!(card->csd.cmdclass & CCC_APP_SPEC)) {
-		pr_warning("%s: card lacks mandatory SD Status "
-			"function.\n", mmc_hostname(card->host));
+		pr_warn("%s: card lacks mandatory SD Status function\n",
+			mmc_hostname(card->host));
 		return 0;
 	}
 
@@ -239,8 +240,8 @@ static int mmc_read_ssr(struct mmc_card *card)
 
 	err = mmc_app_sd_status(card, ssr);
 	if (err) {
-		pr_warning("%s: problem reading SD Status "
-			"register.\n", mmc_hostname(card->host));
+		pr_warn("%s: problem reading SD Status register\n",
+			mmc_hostname(card->host));
 		err = 0;
 		goto out;
 	}
@@ -264,8 +265,8 @@ static int mmc_read_ssr(struct mmc_card *card)
 				card->ssr.erase_offset = eo * 1000;
 			}
 		} else {
-			pr_warning("%s: SD Status: Invalid Allocation Unit size.\n",
-				   mmc_hostname(card->host));
+			pr_warn("%s: SD Status: Invalid Allocation Unit size\n",
+				mmc_hostname(card->host));
 		}
 	}
 out:
@@ -285,8 +286,7 @@ static int mmc_read_switch(struct mmc_card *card)
 		return 0;
 
 	if (!(card->csd.cmdclass & CCC_SWITCH)) {
-		pr_warning("%s: card lacks mandatory switch "
-			"function, performance might suffer.\n",
+		pr_warn("%s: card lacks mandatory switch function, performance might suffer\n",
 			mmc_hostname(card->host));
 		return 0;
 	}
@@ -315,7 +315,7 @@ static int mmc_read_switch(struct mmc_card *card)
 		if (err != -EINVAL && err != -ENOSYS && err != -EFAULT)
 			goto out;
 
-		pr_warning("%s: problem reading Bus Speed modes.\n",
+		pr_warn("%s: problem reading Bus Speed modes\n",
 			mmc_hostname(card->host));
 		err = 0;
 
@@ -371,8 +371,7 @@ int mmc_sd_switch_hs(struct mmc_card *card)
 		goto out;
 
 	if ((status[16] & 0xF) != 1) {
-		pr_warning("%s: Problem switching card "
-			"into high-speed mode!\n",
+		pr_warn("%s: Problem switching card into high-speed mode!\n",
 			mmc_hostname(card->host));
 		err = 0;
 	} else {
@@ -439,7 +438,7 @@ static int sd_select_driver_type(struct mmc_card *card, u8 *status)
 		return err;
 
 	if ((status[15] & 0xF) != drive_strength) {
-		pr_warning("%s: Problem setting drive strength!\n",
+		pr_warn("%s: Problem setting drive strength!\n",
 			mmc_hostname(card->host));
 		return 0;
 	}
@@ -517,7 +516,7 @@ static int sd_set_bus_speed_mode(struct mmc_card *card, u8 *status)
 		return err;
 
 	if ((status[16] & 0xF) != card->sd_bus_speed)
-		pr_warning("%s: Problem setting bus speed mode!\n",
+		pr_warn("%s: Problem setting bus speed mode!\n",
 			mmc_hostname(card->host));
 	else {
 		mmc_set_timing(card->host, timing);
@@ -597,7 +596,7 @@ static int sd_set_current_limit(struct mmc_card *card, u8 *status)
 			return err;
 
 		if (((status[15] >> 4) & 0x0F) != current_limit)
-			pr_warning("%s: Problem setting current limit!\n",
+			pr_warn("%s: Problem setting current limit!\n",
 				mmc_hostname(card->host));
 
 	}
@@ -726,8 +725,7 @@ int mmc_sd_get_cid(struct mmc_host *host, u32 ocr, u32 *cid, u32 *rocr)
 try_again:
 	if (!retries) {
 		ocr &= ~SD_OCR_S18R;
-		pr_warning("%s: Skipping voltage switch\n",
-			mmc_hostname(host));
+		pr_warn("%s: Skipping voltage switch\n", mmc_hostname(host));
 	}
 
 	/*
@@ -871,9 +869,7 @@ int mmc_sd_setup_card(struct mmc_host *host, struct mmc_card *card,
 		}
 
 		if (ro < 0) {
-			pr_warning("%s: host does not "
-				"support reading read-only "
-				"switch. assuming write-enable.\n",
+			pr_warn("%s: host does not support reading read-only switch, assuming write-enable\n",
 				mmc_hostname(host));
 		} else if (ro > 0) {
 			mmc_card_set_readonly(card);
@@ -954,6 +950,13 @@ static int mmc_sd_init_card(struct mmc_host *host, u32 ocr,
 	}
 
 	/*
+	 * handling only for cards supporting DSR and hosts requesting
+	 * DSR configuration
+	 */
+	if (card->csd.dsr_imp && host->dsr_req)
+		mmc_set_dsr(host);
+
+	/*
 	 * Select card, as all following commands rely on that.
 	 */
 	if (!mmc_host_is_spi(host)) {
diff --git a/drivers/mmc/core/sdio.c b/drivers/mmc/core/sdio.c
index e636d9e99e4a..2439e717655b 100644
--- a/drivers/mmc/core/sdio.c
+++ b/drivers/mmc/core/sdio.c
@@ -216,8 +216,8 @@ static int sdio_enable_wide(struct mmc_card *card)
 		return ret;
 
 	if ((ctrl & SDIO_BUS_WIDTH_MASK) == SDIO_BUS_WIDTH_RESERVED)
-		pr_warning("%s: SDIO_CCCR_IF is invalid: 0x%02x\n",
-			   mmc_hostname(card->host), ctrl);
+		pr_warn("%s: SDIO_CCCR_IF is invalid: 0x%02x\n",
+			mmc_hostname(card->host), ctrl);
 
 	/* set as 4-bit bus width */
 	ctrl &= ~SDIO_BUS_WIDTH_MASK;
@@ -605,8 +605,7 @@ static int mmc_sdio_init_card(struct mmc_host *host, u32 ocr,
 
 try_again:
 	if (!retries) {
-		pr_warning("%s: Skipping voltage switch\n",
-				mmc_hostname(host));
+		pr_warn("%s: Skipping voltage switch\n", mmc_hostname(host));
 		ocr &= ~R4_18V_PRESENT;
 	}
 
@@ -992,8 +991,16 @@ static int mmc_sdio_resume(struct mmc_host *host)
 		}
 	}
 
-	if (!err && host->sdio_irqs)
-		wake_up_process(host->sdio_irq_thread);
+	if (!err && host->sdio_irqs) {
+		if (!(host->caps2 & MMC_CAP2_SDIO_IRQ_NOTHREAD)) {
+			wake_up_process(host->sdio_irq_thread);
+		} else if (host->caps & MMC_CAP_SDIO_IRQ) {
+			mmc_host_clk_hold(host);
+			host->ops->enable_sdio_irq(host, 1);
+			mmc_host_clk_release(host);
+		}
+	}
+
 	mmc_release_host(host);
 
 	host->pm_flags &= ~MMC_PM_KEEP_POWER;
diff --git a/drivers/mmc/core/sdio_bus.c b/drivers/mmc/core/sdio_bus.c
index 4fa8fef9147f..6da97b170563 100644
--- a/drivers/mmc/core/sdio_bus.c
+++ b/drivers/mmc/core/sdio_bus.c
@@ -16,6 +16,7 @@
 #include <linux/export.h>
 #include <linux/slab.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_domain.h>
 #include <linux/acpi.h>
 
 #include <linux/mmc/card.h>
@@ -177,8 +178,8 @@ static int sdio_bus_remove(struct device *dev)
 	drv->remove(func);
 
 	if (func->irq_handler) {
-		pr_warning("WARNING: driver %s did not remove "
-			"its interrupt handler!\n", drv->name);
+		pr_warn("WARNING: driver %s did not remove its interrupt handler!\n",
+			drv->name);
 		sdio_claim_host(func);
 		sdio_release_irq(func);
 		sdio_release_host(func);
@@ -315,7 +316,7 @@ int sdio_add_func(struct sdio_func *func)
 	ret = device_add(&func->dev);
 	if (ret == 0) {
 		sdio_func_set_present(func);
-		acpi_dev_pm_attach(&func->dev, false);
+		dev_pm_domain_attach(&func->dev, false);
 	}
 
 	return ret;
@@ -332,7 +333,7 @@ void sdio_remove_func(struct sdio_func *func)
 	if (!sdio_func_present(func))
 		return;
 
-	acpi_dev_pm_detach(&func->dev, false);
+	dev_pm_domain_detach(&func->dev, false);
 	device_del(&func->dev);
 	put_device(&func->dev);
 }
diff --git a/drivers/mmc/core/sdio_irq.c b/drivers/mmc/core/sdio_irq.c
index 5cc13c8d35bb..09cc67d028f0 100644
--- a/drivers/mmc/core/sdio_irq.c
+++ b/drivers/mmc/core/sdio_irq.c
@@ -69,16 +69,15 @@ static int process_sdio_pending_irqs(struct mmc_host *host)
 		if (pending & (1 << i)) {
 			func = card->sdio_func[i - 1];
 			if (!func) {
-				pr_warning("%s: pending IRQ for "
-					"non-existent function\n",
+				pr_warn("%s: pending IRQ for non-existent function\n",
 					mmc_card_id(card));
 				ret = -EINVAL;
 			} else if (func->irq_handler) {
 				func->irq_handler(func);
 				count++;
 			} else {
-				pr_warning("%s: pending IRQ with no handler\n",
-				       sdio_func_id(func));
+				pr_warn("%s: pending IRQ with no handler\n",
+					sdio_func_id(func));
 				ret = -EINVAL;
 			}
 		}
@@ -208,7 +207,7 @@ static int sdio_card_irq_get(struct mmc_card *card)
 				host->sdio_irqs--;
 				return err;
 			}
-		} else {
+		} else if (host->caps & MMC_CAP_SDIO_IRQ) {
 			mmc_host_clk_hold(host);
 			host->ops->enable_sdio_irq(host, 1);
 			mmc_host_clk_release(host);
@@ -229,7 +228,7 @@ static int sdio_card_irq_put(struct mmc_card *card)
 		if (!(host->caps2 & MMC_CAP2_SDIO_IRQ_NOTHREAD)) {
 			atomic_set(&host->sdio_irq_thread_abort, 1);
 			kthread_stop(host->sdio_irq_thread);
-		} else {
+		} else if (host->caps & MMC_CAP_SDIO_IRQ) {
 			mmc_host_clk_hold(host);
 			host->ops->enable_sdio_irq(host, 0);
 			mmc_host_clk_release(host);
diff --git a/drivers/mmc/core/slot-gpio.c b/drivers/mmc/core/slot-gpio.c
index 5f89cb83d5f0..69bbf2adb329 100644
--- a/drivers/mmc/core/slot-gpio.c
+++ b/drivers/mmc/core/slot-gpio.c
@@ -221,8 +221,6 @@ int mmc_gpio_request_cd(struct mmc_host *host, unsigned int gpio,
 	ctx->override_cd_active_level = true;
 	ctx->cd_gpio = gpio_to_desc(gpio);
 
-	mmc_gpiod_request_cd_irq(host);
-
 	return 0;
 }
 EXPORT_SYMBOL(mmc_gpio_request_cd);
@@ -283,6 +281,8 @@ EXPORT_SYMBOL(mmc_gpio_free_cd);
  * @idx: index of the GPIO to obtain in the consumer
  * @override_active_level: ignore %GPIO_ACTIVE_LOW flag
  * @debounce: debounce time in microseconds
+ * @gpio_invert: will return whether the GPIO line is inverted or not, set
+ * to NULL to ignore
  *
  * Use this function in place of mmc_gpio_request_cd() to use the GPIO
  * descriptor API.  Note that it is paired with mmc_gpiod_free_cd() not
@@ -293,7 +293,7 @@ EXPORT_SYMBOL(mmc_gpio_free_cd);
  */
 int mmc_gpiod_request_cd(struct mmc_host *host, const char *con_id,
 			 unsigned int idx, bool override_active_level,
-			 unsigned int debounce)
+			 unsigned int debounce, bool *gpio_invert)
 {
 	struct mmc_gpio *ctx;
 	struct gpio_desc *desc;
@@ -308,20 +308,19 @@ int mmc_gpiod_request_cd(struct mmc_host *host, const char *con_id,
 	if (!con_id)
 		con_id = ctx->cd_label;
 
-	desc = devm_gpiod_get_index(host->parent, con_id, idx);
+	desc = devm_gpiod_get_index(host->parent, con_id, idx, GPIOD_IN);
 	if (IS_ERR(desc))
 		return PTR_ERR(desc);
 
-	ret = gpiod_direction_input(desc);
-	if (ret < 0)
-		return ret;
-
 	if (debounce) {
 		ret = gpiod_set_debounce(desc, debounce);
 		if (ret < 0)
 			return ret;
 	}
 
+	if (gpio_invert)
+		*gpio_invert = !gpiod_is_active_low(desc);
+
 	ctx->override_cd_active_level = override_active_level;
 	ctx->cd_gpio = desc;
 
@@ -330,6 +329,59 @@ int mmc_gpiod_request_cd(struct mmc_host *host, const char *con_id,
 EXPORT_SYMBOL(mmc_gpiod_request_cd);
 
 /**
+ * mmc_gpiod_request_ro - request a gpio descriptor for write protection
+ * @host: mmc host
+ * @con_id: function within the GPIO consumer
+ * @idx: index of the GPIO to obtain in the consumer
+ * @override_active_level: ignore %GPIO_ACTIVE_LOW flag
+ * @debounce: debounce time in microseconds
+ * @gpio_invert: will return whether the GPIO line is inverted or not,
+ * set to NULL to ignore
+ *
+ * Use this function in place of mmc_gpio_request_ro() to use the GPIO
+ * descriptor API.  Note that it is paired with mmc_gpiod_free_ro() not
+ * mmc_gpio_free_ro().
+ *
+ * Returns zero on success, else an error.
+ */
+int mmc_gpiod_request_ro(struct mmc_host *host, const char *con_id,
+			 unsigned int idx, bool override_active_level,
+			 unsigned int debounce, bool *gpio_invert)
+{
+	struct mmc_gpio *ctx;
+	struct gpio_desc *desc;
+	int ret;
+
+	ret = mmc_gpio_alloc(host);
+	if (ret < 0)
+		return ret;
+
+	ctx = host->slot.handler_priv;
+
+	if (!con_id)
+		con_id = ctx->ro_label;
+
+	desc = devm_gpiod_get_index(host->parent, con_id, idx, GPIOD_IN);
+	if (IS_ERR(desc))
+		return PTR_ERR(desc);
+
+	if (debounce) {
+		ret = gpiod_set_debounce(desc, debounce);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (gpio_invert)
+		*gpio_invert = !gpiod_is_active_low(desc);
+
+	ctx->override_ro_active_level = override_active_level;
+	ctx->ro_gpio = desc;
+
+	return 0;
+}
+EXPORT_SYMBOL(mmc_gpiod_request_ro);
+
+/**
  * mmc_gpiod_free_cd - free the card-detection gpio descriptor
  * @host: mmc host
  *
@@ -348,7 +400,7 @@ void mmc_gpiod_free_cd(struct mmc_host *host)
 		host->slot.cd_irq = -EINVAL;
 	}
 
-	devm_gpiod_put(&host->class_dev, ctx->cd_gpio);
+	devm_gpiod_put(host->parent, ctx->cd_gpio);
 
 	ctx->cd_gpio = NULL;
 }
diff --git a/drivers/mmc/host/Kconfig b/drivers/mmc/host/Kconfig
index 451135822464..13860656104b 100644
--- a/drivers/mmc/host/Kconfig
+++ b/drivers/mmc/host/Kconfig
@@ -14,6 +14,17 @@ config MMC_ARMMMCI
 
 	  If unsure, say N.
 
+config MMC_QCOM_DML
+	tristate "Qualcomm Data Mover for SD Card Controller"
+	depends on MMC_ARMMMCI && QCOM_BAM_DMA
+	default y
+	help
+	  This selects the Qualcomm Data Mover lite/local on SD Card controller.
+	  This option will enable the dma to work correctly, if you are using
+	  Qcom SOCs and MMC, you would probably need this option to get DMA working.
+
+	  if unsure, say N.
+
 config MMC_PXA
 	tristate "Intel PXA25x/26x/27x Multimedia Card Interface support"
 	depends on ARCH_PXA
@@ -568,7 +579,8 @@ config SDH_BFIN_MISSING_CMD_PULLUP_WORKAROUND
 
 config MMC_DW
 	tristate "Synopsys DesignWare Memory Card Interface"
-	depends on ARC || ARM
+	depends on HAS_DMA
+	depends on ARC || ARM || MIPS || COMPILE_TEST
 	help
 	  This selects support for the Synopsys DesignWare Mobile Storage IP
 	  block, this provides host support for SD and MMC interfaces, in both
@@ -626,6 +638,15 @@ config MMC_DW_PCI
 
 	  If unsure, say N.
 
+config MMC_DW_ROCKCHIP
+	tristate "Rockchip specific extensions for Synopsys DW Memory Card Interface"
+	depends on MMC_DW && ARCH_ROCKCHIP
+	select MMC_DW_PLTFM
+	help
+	  This selects support for Rockchip SoC specific extensions to the
+	  Synopsys DesignWare Memory Card Interface driver. Select this option
+	  for platforms based on RK3066, RK3188 and RK3288 SoC's.
+
 config MMC_SH_MMCIF
 	tristate "SuperH Internal MMCIF support"
 	depends on MMC_BLOCK && HAS_DMA
diff --git a/drivers/mmc/host/Makefile b/drivers/mmc/host/Makefile
index f211eede8db5..b09ecfb88269 100644
--- a/drivers/mmc/host/Makefile
+++ b/drivers/mmc/host/Makefile
@@ -3,6 +3,7 @@
 #
 
 obj-$(CONFIG_MMC_ARMMMCI)	+= mmci.o
+obj-$(CONFIG_MMC_QCOM_DML)	+= mmci_qcom_dml.o
 obj-$(CONFIG_MMC_PXA)		+= pxamci.o
 obj-$(CONFIG_MMC_MXC)		+= mxcmmc.o
 obj-$(CONFIG_MMC_MXS)		+= mxs-mmc.o
@@ -45,6 +46,7 @@ obj-$(CONFIG_MMC_DW_PLTFM)	+= dw_mmc-pltfm.o
 obj-$(CONFIG_MMC_DW_EXYNOS)	+= dw_mmc-exynos.o
 obj-$(CONFIG_MMC_DW_K3)		+= dw_mmc-k3.o
 obj-$(CONFIG_MMC_DW_PCI)	+= dw_mmc-pci.o
+obj-$(CONFIG_MMC_DW_ROCKCHIP)	+= dw_mmc-rockchip.o
 obj-$(CONFIG_MMC_SH_MMCIF)	+= sh_mmcif.o
 obj-$(CONFIG_MMC_JZ4740)	+= jz4740_mmc.o
 obj-$(CONFIG_MMC_VUB300)	+= vub300.o
diff --git a/drivers/mmc/host/atmel-mci.c b/drivers/mmc/host/atmel-mci.c
index bb585d940901..77250d4b1979 100644
--- a/drivers/mmc/host/atmel-mci.c
+++ b/drivers/mmc/host/atmel-mci.c
@@ -17,6 +17,7 @@
 #include <linux/gpio.h>
 #include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/ioport.h>
 #include <linux/module.h>
 #include <linux/of.h>
@@ -2195,7 +2196,8 @@ static int __init atmci_init_slot(struct atmel_mci *host,
 	/* Assume card is present initially */
 	set_bit(ATMCI_CARD_PRESENT, &slot->flags);
 	if (gpio_is_valid(slot->detect_pin)) {
-		if (gpio_request(slot->detect_pin, "mmc_detect")) {
+		if (devm_gpio_request(&host->pdev->dev, slot->detect_pin,
+				      "mmc_detect")) {
 			dev_dbg(&mmc->class_dev, "no detect pin available\n");
 			slot->detect_pin = -EBUSY;
 		} else if (gpio_get_value(slot->detect_pin) ^
@@ -2208,7 +2210,8 @@ static int __init atmci_init_slot(struct atmel_mci *host,
 		mmc->caps |= MMC_CAP_NEEDS_POLL;
 
 	if (gpio_is_valid(slot->wp_pin)) {
-		if (gpio_request(slot->wp_pin, "mmc_wp")) {
+		if (devm_gpio_request(&host->pdev->dev, slot->wp_pin,
+				      "mmc_wp")) {
 			dev_dbg(&mmc->class_dev, "no WP pin available\n");
 			slot->wp_pin = -EBUSY;
 		}
@@ -2232,7 +2235,6 @@ static int __init atmci_init_slot(struct atmel_mci *host,
 			dev_dbg(&mmc->class_dev,
 				"could not request IRQ %d for detect pin\n",
 				gpio_to_irq(slot->detect_pin));
-			gpio_free(slot->detect_pin);
 			slot->detect_pin = -EBUSY;
 		}
 	}
@@ -2242,7 +2244,7 @@ static int __init atmci_init_slot(struct atmel_mci *host,
 	return 0;
 }
 
-static void __exit atmci_cleanup_slot(struct atmel_mci_slot *slot,
+static void atmci_cleanup_slot(struct atmel_mci_slot *slot,
 		unsigned int id)
 {
 	/* Debugfs stuff is cleaned up by mmc core */
@@ -2257,10 +2259,7 @@ static void __exit atmci_cleanup_slot(struct atmel_mci_slot *slot,
 
 		free_irq(gpio_to_irq(pin), slot);
 		del_timer_sync(&slot->detect_timer);
-		gpio_free(pin);
 	}
-	if (gpio_is_valid(slot->wp_pin))
-		gpio_free(slot->wp_pin);
 
 	slot->host->slot[id] = NULL;
 	mmc_free_host(slot->mmc);
@@ -2344,6 +2343,7 @@ static void __init atmci_get_cap(struct atmel_mci *host)
 
 	/* keep only major version number */
 	switch (version & 0xf00) {
+	case 0x600:
 	case 0x500:
 		host->caps.has_odd_clk_div = 1;
 	case 0x400:
@@ -2377,7 +2377,7 @@ static int __init atmci_probe(struct platform_device *pdev)
 	struct resource			*regs;
 	unsigned int			nr_slots;
 	int				irq;
-	int				ret;
+	int				ret, i;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!regs)
@@ -2395,7 +2395,7 @@ static int __init atmci_probe(struct platform_device *pdev)
 	if (irq < 0)
 		return irq;
 
-	host = kzalloc(sizeof(struct atmel_mci), GFP_KERNEL);
+	host = devm_kzalloc(&pdev->dev, sizeof(*host), GFP_KERNEL);
 	if (!host)
 		return -ENOMEM;
 
@@ -2403,20 +2403,18 @@ static int __init atmci_probe(struct platform_device *pdev)
 	spin_lock_init(&host->lock);
 	INIT_LIST_HEAD(&host->queue);
 
-	host->mck = clk_get(&pdev->dev, "mci_clk");
-	if (IS_ERR(host->mck)) {
-		ret = PTR_ERR(host->mck);
-		goto err_clk_get;
-	}
+	host->mck = devm_clk_get(&pdev->dev, "mci_clk");
+	if (IS_ERR(host->mck))
+		return PTR_ERR(host->mck);
 
-	ret = -ENOMEM;
-	host->regs = ioremap(regs->start, resource_size(regs));
+	host->regs = devm_ioremap(&pdev->dev, regs->start, resource_size(regs));
 	if (!host->regs)
-		goto err_ioremap;
+		return -ENOMEM;
 
 	ret = clk_prepare_enable(host->mck);
 	if (ret)
-		goto err_request_irq;
+		return ret;
+
 	atmci_writel(host, ATMCI_CR, ATMCI_CR_SWRST);
 	host->bus_hz = clk_get_rate(host->mck);
 	clk_disable_unprepare(host->mck);
@@ -2427,7 +2425,7 @@ static int __init atmci_probe(struct platform_device *pdev)
 
 	ret = request_irq(irq, atmci_interrupt, 0, dev_name(&pdev->dev), host);
 	if (ret)
-		goto err_request_irq;
+		return ret;
 
 	/* Get MCI capabilities and set operations according to it */
 	atmci_get_cap(host);
@@ -2485,7 +2483,7 @@ static int __init atmci_probe(struct platform_device *pdev)
 		if (!host->buffer) {
 			ret = -ENOMEM;
 			dev_err(&pdev->dev, "buffer allocation failed\n");
-			goto err_init_slot;
+			goto err_dma_alloc;
 		}
 	}
 
@@ -2495,16 +2493,16 @@ static int __init atmci_probe(struct platform_device *pdev)
 
 	return 0;
 
+err_dma_alloc:
+	for (i = 0; i < ATMCI_MAX_NR_SLOTS; i++) {
+		if (host->slot[i])
+			atmci_cleanup_slot(host->slot[i], i);
+	}
 err_init_slot:
+	del_timer_sync(&host->timer);
 	if (host->dma.chan)
 		dma_release_channel(host->dma.chan);
 	free_irq(irq, host);
-err_request_irq:
-	iounmap(host->regs);
-err_ioremap:
-	clk_put(host->mck);
-err_clk_get:
-	kfree(host);
 	return ret;
 }
 
@@ -2528,14 +2526,11 @@ static int __exit atmci_remove(struct platform_device *pdev)
 	atmci_readl(host, ATMCI_SR);
 	clk_disable_unprepare(host->mck);
 
+	del_timer_sync(&host->timer);
 	if (host->dma.chan)
 		dma_release_channel(host->dma.chan);
 
 	free_irq(platform_get_irq(pdev, 0), host);
-	iounmap(host->regs);
-
-	clk_put(host->mck);
-	kfree(host);
 
 	return 0;
 }
diff --git a/drivers/mmc/host/au1xmmc.c b/drivers/mmc/host/au1xmmc.c
index 9c9f6af29251..725f6a6fd89b 100644
--- a/drivers/mmc/host/au1xmmc.c
+++ b/drivers/mmc/host/au1xmmc.c
@@ -1028,9 +1028,12 @@ static int au1xmmc_probe(struct platform_device *pdev)
 	host->clk = clk_get(&pdev->dev, ALCHEMY_PERIPH_CLK);
 	if (IS_ERR(host->clk)) {
 		dev_err(&pdev->dev, "cannot find clock\n");
+		ret = PTR_ERR(host->clk);
 		goto out_irq;
 	}
-	if (clk_prepare_enable(host->clk)) {
+
+	ret = clk_prepare_enable(host->clk);
+	if (ret) {
 		dev_err(&pdev->dev, "cannot enable clock\n");
 		goto out_clk;
 	}
diff --git a/drivers/mmc/host/dw_mmc-pci.c b/drivers/mmc/host/dw_mmc-pci.c
index 6ada1b36685b..4c69fbd29811 100644
--- a/drivers/mmc/host/dw_mmc-pci.c
+++ b/drivers/mmc/host/dw_mmc-pci.c
@@ -95,9 +95,6 @@ static int dw_mci_pci_resume(struct device *dev)
 
 	return dw_mci_resume(host);
 }
-#else
-#define dw_mci_pci_suspend	NULL
-#define dw_mci_pci_resume	NULL
 #endif /* CONFIG_PM_SLEEP */
 
 static SIMPLE_DEV_PM_OPS(dw_mci_pci_pmops, dw_mci_pci_suspend, dw_mci_pci_resume);
diff --git a/drivers/mmc/host/dw_mmc-pltfm.c b/drivers/mmc/host/dw_mmc-pltfm.c
index d4a47a9f5584..8b6572162ed9 100644
--- a/drivers/mmc/host/dw_mmc-pltfm.c
+++ b/drivers/mmc/host/dw_mmc-pltfm.c
@@ -21,6 +21,7 @@
 #include <linux/mmc/mmc.h>
 #include <linux/mmc/dw_mmc.h>
 #include <linux/of.h>
+#include <linux/clk.h>
 
 #include "dw_mmc.h"
 #include "dw_mmc-pltfm.h"
@@ -30,10 +31,6 @@ static void dw_mci_pltfm_prepare_command(struct dw_mci *host, u32 *cmdr)
 	*cmdr |= SDMMC_CMD_USE_HOLD_REG;
 }
 
-static const struct dw_mci_drv_data rockchip_drv_data = {
-	.prepare_command	= dw_mci_pltfm_prepare_command,
-};
-
 static const struct dw_mci_drv_data socfpga_drv_data = {
 	.prepare_command	= dw_mci_pltfm_prepare_command,
 };
@@ -84,9 +81,6 @@ static int dw_mci_pltfm_resume(struct device *dev)
 
 	return dw_mci_resume(host);
 }
-#else
-#define dw_mci_pltfm_suspend	NULL
-#define dw_mci_pltfm_resume	NULL
 #endif /* CONFIG_PM_SLEEP */
 
 SIMPLE_DEV_PM_OPS(dw_mci_pltfm_pmops, dw_mci_pltfm_suspend, dw_mci_pltfm_resume);
@@ -94,8 +88,6 @@ EXPORT_SYMBOL_GPL(dw_mci_pltfm_pmops);
 
 static const struct of_device_id dw_mci_pltfm_match[] = {
 	{ .compatible = "snps,dw-mshc", },
-	{ .compatible = "rockchip,rk2928-dw-mshc",
-		.data = &rockchip_drv_data },
 	{ .compatible = "altr,socfpga-dw-mshc",
 		.data = &socfpga_drv_data },
 	{},
diff --git a/drivers/mmc/host/dw_mmc-rockchip.c b/drivers/mmc/host/dw_mmc-rockchip.c
new file mode 100644
index 000000000000..f0c2cb1a210d
--- /dev/null
+++ b/drivers/mmc/host/dw_mmc-rockchip.c
@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2014, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/dw_mmc.h>
+#include <linux/of_address.h>
+
+#include "dw_mmc.h"
+#include "dw_mmc-pltfm.h"
+
+#define RK3288_CLKGEN_DIV       2
+
+static void dw_mci_rockchip_prepare_command(struct dw_mci *host, u32 *cmdr)
+{
+	*cmdr |= SDMMC_CMD_USE_HOLD_REG;
+}
+
+static int dw_mci_rk3288_setup_clock(struct dw_mci *host)
+{
+	host->bus_hz /= RK3288_CLKGEN_DIV;
+
+	return 0;
+}
+
+static void dw_mci_rk3288_set_ios(struct dw_mci *host, struct mmc_ios *ios)
+{
+	int ret;
+	unsigned int cclkin;
+	u32 bus_hz;
+
+	/*
+	 * cclkin: source clock of mmc controller
+	 * bus_hz: card interface clock generated by CLKGEN
+	 * bus_hz = cclkin / RK3288_CLKGEN_DIV
+	 * ios->clock = (div == 0) ? bus_hz : (bus_hz / (2 * div))
+	 *
+	 * Note: div can only be 0 or 1
+	 *       if DDR50 8bit mode(only emmc work in 8bit mode),
+	 *       div must be set 1
+	 */
+	if (ios->bus_width == MMC_BUS_WIDTH_8 &&
+	    ios->timing == MMC_TIMING_MMC_DDR52)
+		cclkin = 2 * ios->clock * RK3288_CLKGEN_DIV;
+	else
+		cclkin = ios->clock * RK3288_CLKGEN_DIV;
+
+	ret = clk_set_rate(host->ciu_clk, cclkin);
+	if (ret)
+		dev_warn(host->dev, "failed to set rate %uHz\n", ios->clock);
+
+	bus_hz = clk_get_rate(host->ciu_clk) / RK3288_CLKGEN_DIV;
+	if (bus_hz != host->bus_hz) {
+		host->bus_hz = bus_hz;
+		/* force dw_mci_setup_bus() */
+		host->current_speed = 0;
+	}
+}
+
+static const struct dw_mci_drv_data rk2928_drv_data = {
+	.prepare_command        = dw_mci_rockchip_prepare_command,
+};
+
+static const struct dw_mci_drv_data rk3288_drv_data = {
+	.prepare_command        = dw_mci_rockchip_prepare_command,
+	.set_ios		= dw_mci_rk3288_set_ios,
+	.setup_clock    = dw_mci_rk3288_setup_clock,
+};
+
+static const struct of_device_id dw_mci_rockchip_match[] = {
+	{ .compatible = "rockchip,rk2928-dw-mshc",
+		.data = &rk2928_drv_data },
+	{ .compatible = "rockchip,rk3288-dw-mshc",
+		.data = &rk3288_drv_data },
+	{},
+};
+MODULE_DEVICE_TABLE(of, dw_mci_rockchip_match);
+
+static int dw_mci_rockchip_probe(struct platform_device *pdev)
+{
+	const struct dw_mci_drv_data *drv_data;
+	const struct of_device_id *match;
+
+	if (!pdev->dev.of_node)
+		return -ENODEV;
+
+	match = of_match_node(dw_mci_rockchip_match, pdev->dev.of_node);
+	drv_data = match->data;
+
+	return dw_mci_pltfm_register(pdev, drv_data);
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int dw_mci_rockchip_suspend(struct device *dev)
+{
+	struct dw_mci *host = dev_get_drvdata(dev);
+
+	return dw_mci_suspend(host);
+}
+
+static int dw_mci_rockchip_resume(struct device *dev)
+{
+	struct dw_mci *host = dev_get_drvdata(dev);
+
+	return dw_mci_resume(host);
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static SIMPLE_DEV_PM_OPS(dw_mci_rockchip_pmops,
+			 dw_mci_rockchip_suspend,
+			 dw_mci_rockchip_resume);
+
+static struct platform_driver dw_mci_rockchip_pltfm_driver = {
+	.probe		= dw_mci_rockchip_probe,
+	.remove		= __exit_p(dw_mci_pltfm_remove),
+	.driver		= {
+		.name		= "dwmmc_rockchip",
+		.of_match_table	= dw_mci_rockchip_match,
+		.pm		= &dw_mci_rockchip_pmops,
+	},
+};
+
+module_platform_driver(dw_mci_rockchip_pltfm_driver);
+
+MODULE_AUTHOR("Addy Ke <addy.ke@rock-chips.com>");
+MODULE_DESCRIPTION("Rockchip Specific DW-MSHC Driver Extension");
+MODULE_ALIAS("platform:dwmmc-rockchip");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mmc/host/dw_mmc.c b/drivers/mmc/host/dw_mmc.c
index 8f216edbdf08..69f0cc68d5b2 100644
--- a/drivers/mmc/host/dw_mmc.c
+++ b/drivers/mmc/host/dw_mmc.c
@@ -29,6 +29,7 @@
 #include <linux/irq.h>
 #include <linux/mmc/host.h>
 #include <linux/mmc/mmc.h>
+#include <linux/mmc/sd.h>
 #include <linux/mmc/sdio.h>
 #include <linux/mmc/dw_mmc.h>
 #include <linux/bitops.h>
@@ -81,36 +82,6 @@ struct idmac_desc {
 };
 #endif /* CONFIG_MMC_DW_IDMAC */
 
-static const u8 tuning_blk_pattern_4bit[] = {
-	0xff, 0x0f, 0xff, 0x00, 0xff, 0xcc, 0xc3, 0xcc,
-	0xc3, 0x3c, 0xcc, 0xff, 0xfe, 0xff, 0xfe, 0xef,
-	0xff, 0xdf, 0xff, 0xdd, 0xff, 0xfb, 0xff, 0xfb,
-	0xbf, 0xff, 0x7f, 0xff, 0x77, 0xf7, 0xbd, 0xef,
-	0xff, 0xf0, 0xff, 0xf0, 0x0f, 0xfc, 0xcc, 0x3c,
-	0xcc, 0x33, 0xcc, 0xcf, 0xff, 0xef, 0xff, 0xee,
-	0xff, 0xfd, 0xff, 0xfd, 0xdf, 0xff, 0xbf, 0xff,
-	0xbb, 0xff, 0xf7, 0xff, 0xf7, 0x7f, 0x7b, 0xde,
-};
-
-static const u8 tuning_blk_pattern_8bit[] = {
-	0xff, 0xff, 0x00, 0xff, 0xff, 0xff, 0x00, 0x00,
-	0xff, 0xff, 0xcc, 0xcc, 0xcc, 0x33, 0xcc, 0xcc,
-	0xcc, 0x33, 0x33, 0xcc, 0xcc, 0xcc, 0xff, 0xff,
-	0xff, 0xee, 0xff, 0xff, 0xff, 0xee, 0xee, 0xff,
-	0xff, 0xff, 0xdd, 0xff, 0xff, 0xff, 0xdd, 0xdd,
-	0xff, 0xff, 0xff, 0xbb, 0xff, 0xff, 0xff, 0xbb,
-	0xbb, 0xff, 0xff, 0xff, 0x77, 0xff, 0xff, 0xff,
-	0x77, 0x77, 0xff, 0x77, 0xbb, 0xdd, 0xee, 0xff,
-	0xff, 0xff, 0xff, 0x00, 0xff, 0xff, 0xff, 0x00,
-	0x00, 0xff, 0xff, 0xcc, 0xcc, 0xcc, 0x33, 0xcc,
-	0xcc, 0xcc, 0x33, 0x33, 0xcc, 0xcc, 0xcc, 0xff,
-	0xff, 0xff, 0xee, 0xff, 0xff, 0xff, 0xee, 0xee,
-	0xff, 0xff, 0xff, 0xdd, 0xff, 0xff, 0xff, 0xdd,
-	0xdd, 0xff, 0xff, 0xff, 0xbb, 0xff, 0xff, 0xff,
-	0xbb, 0xbb, 0xff, 0xff, 0xff, 0x77, 0xff, 0xff,
-	0xff, 0x77, 0x77, 0xff, 0x77, 0xbb, 0xdd, 0xee,
-};
-
 static bool dw_mci_reset(struct dw_mci *host);
 
 #if defined(CONFIG_DEBUG_FS)
@@ -234,10 +205,13 @@ err:
 }
 #endif /* defined(CONFIG_DEBUG_FS) */
 
+static void mci_send_cmd(struct dw_mci_slot *slot, u32 cmd, u32 arg);
+
 static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
 {
 	struct mmc_data	*data;
 	struct dw_mci_slot *slot = mmc_priv(mmc);
+	struct dw_mci *host = slot->host;
 	const struct dw_mci_drv_data *drv_data = slot->host->drv_data;
 	u32 cmdr;
 	cmd->error = -EINPROGRESS;
@@ -253,6 +227,34 @@ static u32 dw_mci_prepare_command(struct mmc_host *mmc, struct mmc_command *cmd)
 	else if (cmd->opcode != MMC_SEND_STATUS && cmd->data)
 		cmdr |= SDMMC_CMD_PRV_DAT_WAIT;
 
+	if (cmd->opcode == SD_SWITCH_VOLTAGE) {
+		u32 clk_en_a;
+
+		/* Special bit makes CMD11 not die */
+		cmdr |= SDMMC_CMD_VOLT_SWITCH;
+
+		/* Change state to continue to handle CMD11 weirdness */
+		WARN_ON(slot->host->state != STATE_SENDING_CMD);
+		slot->host->state = STATE_SENDING_CMD11;
+
+		/*
+		 * We need to disable low power mode (automatic clock stop)
+		 * while doing voltage switch so we don't confuse the card,
+		 * since stopping the clock is a specific part of the UHS
+		 * voltage change dance.
+		 *
+		 * Note that low power mode (SDMMC_CLKEN_LOW_PWR) will be
+		 * unconditionally turned back on in dw_mci_setup_bus() if it's
+		 * ever called with a non-zero clock.  That shouldn't happen
+		 * until the voltage change is all done.
+		 */
+		clk_en_a = mci_readl(host, CLKENA);
+		clk_en_a &= ~(SDMMC_CLKEN_LOW_PWR << slot->id);
+		mci_writel(host, CLKENA, clk_en_a);
+		mci_send_cmd(slot, SDMMC_CMD_UPD_CLK |
+			     SDMMC_CMD_PRV_DAT_WAIT, 0);
+	}
+
 	if (cmd->flags & MMC_RSP_PRESENT) {
 		/* We expect a response, so set this bit */
 		cmdr |= SDMMC_CMD_RESP_EXP;
@@ -775,11 +777,15 @@ static void dw_mci_setup_bus(struct dw_mci_slot *slot, bool force_clkinit)
 	unsigned int clock = slot->clock;
 	u32 div;
 	u32 clk_en_a;
+	u32 sdmmc_cmd_bits = SDMMC_CMD_UPD_CLK | SDMMC_CMD_PRV_DAT_WAIT;
+
+	/* We must continue to set bit 28 in CMD until the change is complete */
+	if (host->state == STATE_WAITING_CMD11_DONE)
+		sdmmc_cmd_bits |= SDMMC_CMD_VOLT_SWITCH;
 
 	if (!clock) {
 		mci_writel(host, CLKENA, 0);
-		mci_send_cmd(slot,
-			     SDMMC_CMD_UPD_CLK | SDMMC_CMD_PRV_DAT_WAIT, 0);
+		mci_send_cmd(slot, sdmmc_cmd_bits, 0);
 	} else if (clock != host->current_speed || force_clkinit) {
 		div = host->bus_hz / clock;
 		if (host->bus_hz % clock && host->bus_hz > clock)
@@ -803,15 +809,13 @@ static void dw_mci_setup_bus(struct dw_mci_slot *slot, bool force_clkinit)
 		mci_writel(host, CLKSRC, 0);
 
 		/* inform CIU */
-		mci_send_cmd(slot,
-			     SDMMC_CMD_UPD_CLK | SDMMC_CMD_PRV_DAT_WAIT, 0);
+		mci_send_cmd(slot, sdmmc_cmd_bits, 0);
 
 		/* set clock to desired speed */
 		mci_writel(host, CLKDIV, div);
 
 		/* inform CIU */
-		mci_send_cmd(slot,
-			     SDMMC_CMD_UPD_CLK | SDMMC_CMD_PRV_DAT_WAIT, 0);
+		mci_send_cmd(slot, sdmmc_cmd_bits, 0);
 
 		/* enable clock; only low power if no SDIO */
 		clk_en_a = SDMMC_CLKEN_ENABLE << slot->id;
@@ -820,8 +824,7 @@ static void dw_mci_setup_bus(struct dw_mci_slot *slot, bool force_clkinit)
 		mci_writel(host, CLKENA, clk_en_a);
 
 		/* inform CIU */
-		mci_send_cmd(slot,
-			     SDMMC_CMD_UPD_CLK | SDMMC_CMD_PRV_DAT_WAIT, 0);
+		mci_send_cmd(slot, sdmmc_cmd_bits, 0);
 
 		/* keep the clock with reflecting clock dividor */
 		slot->__clk_old = clock << div;
@@ -897,6 +900,17 @@ static void dw_mci_queue_request(struct dw_mci *host, struct dw_mci_slot *slot,
 
 	slot->mrq = mrq;
 
+	if (host->state == STATE_WAITING_CMD11_DONE) {
+		dev_warn(&slot->mmc->class_dev,
+			 "Voltage change didn't complete\n");
+		/*
+		 * this case isn't expected to happen, so we can
+		 * either crash here or just try to continue on
+		 * in the closest possible state
+		 */
+		host->state = STATE_IDLE;
+	}
+
 	if (host->state == STATE_IDLE) {
 		host->state = STATE_SENDING_CMD;
 		dw_mci_start_request(host, slot);
@@ -936,6 +950,7 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	struct dw_mci_slot *slot = mmc_priv(mmc);
 	const struct dw_mci_drv_data *drv_data = slot->host->drv_data;
 	u32 regs;
+	int ret;
 
 	switch (ios->bus_width) {
 	case MMC_BUS_WIDTH_4:
@@ -972,14 +987,43 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	/* Slot specific timing and width adjustment */
 	dw_mci_setup_bus(slot, false);
 
+	if (slot->host->state == STATE_WAITING_CMD11_DONE && ios->clock != 0)
+		slot->host->state = STATE_IDLE;
+
 	switch (ios->power_mode) {
 	case MMC_POWER_UP:
+		if (!IS_ERR(mmc->supply.vmmc)) {
+			ret = mmc_regulator_set_ocr(mmc, mmc->supply.vmmc,
+					ios->vdd);
+			if (ret) {
+				dev_err(slot->host->dev,
+					"failed to enable vmmc regulator\n");
+				/*return, if failed turn on vmmc*/
+				return;
+			}
+		}
+		if (!IS_ERR(mmc->supply.vqmmc) && !slot->host->vqmmc_enabled) {
+			ret = regulator_enable(mmc->supply.vqmmc);
+			if (ret < 0)
+				dev_err(slot->host->dev,
+					"failed to enable vqmmc regulator\n");
+			else
+				slot->host->vqmmc_enabled = true;
+		}
 		set_bit(DW_MMC_CARD_NEED_INIT, &slot->flags);
 		regs = mci_readl(slot->host, PWREN);
 		regs |= (1 << slot->id);
 		mci_writel(slot->host, PWREN, regs);
 		break;
 	case MMC_POWER_OFF:
+		if (!IS_ERR(mmc->supply.vmmc))
+			mmc_regulator_set_ocr(mmc, mmc->supply.vmmc, 0);
+
+		if (!IS_ERR(mmc->supply.vqmmc) && slot->host->vqmmc_enabled) {
+			regulator_disable(mmc->supply.vqmmc);
+			slot->host->vqmmc_enabled = false;
+		}
+
 		regs = mci_readl(slot->host, PWREN);
 		regs &= ~(1 << slot->id);
 		mci_writel(slot->host, PWREN, regs);
@@ -989,6 +1033,59 @@ static void dw_mci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	}
 }
 
+static int dw_mci_card_busy(struct mmc_host *mmc)
+{
+	struct dw_mci_slot *slot = mmc_priv(mmc);
+	u32 status;
+
+	/*
+	 * Check the busy bit which is low when DAT[3:0]
+	 * (the data lines) are 0000
+	 */
+	status = mci_readl(slot->host, STATUS);
+
+	return !!(status & SDMMC_STATUS_BUSY);
+}
+
+static int dw_mci_switch_voltage(struct mmc_host *mmc, struct mmc_ios *ios)
+{
+	struct dw_mci_slot *slot = mmc_priv(mmc);
+	struct dw_mci *host = slot->host;
+	u32 uhs;
+	u32 v18 = SDMMC_UHS_18V << slot->id;
+	int min_uv, max_uv;
+	int ret;
+
+	/*
+	 * Program the voltage.  Note that some instances of dw_mmc may use
+	 * the UHS_REG for this.  For other instances (like exynos) the UHS_REG
+	 * does no harm but you need to set the regulator directly.  Try both.
+	 */
+	uhs = mci_readl(host, UHS_REG);
+	if (ios->signal_voltage == MMC_SIGNAL_VOLTAGE_330) {
+		min_uv = 2700000;
+		max_uv = 3600000;
+		uhs &= ~v18;
+	} else {
+		min_uv = 1700000;
+		max_uv = 1950000;
+		uhs |= v18;
+	}
+	if (!IS_ERR(mmc->supply.vqmmc)) {
+		ret = regulator_set_voltage(mmc->supply.vqmmc, min_uv, max_uv);
+
+		if (ret) {
+			dev_err(&mmc->class_dev,
+					 "Regulator set error %d: %d - %d\n",
+					 ret, min_uv, max_uv);
+			return ret;
+		}
+	}
+	mci_writel(host, UHS_REG, uhs);
+
+	return 0;
+}
+
 static int dw_mci_get_ro(struct mmc_host *mmc)
 {
 	int read_only;
@@ -1131,6 +1228,9 @@ static const struct mmc_host_ops dw_mci_ops = {
 	.get_cd			= dw_mci_get_cd,
 	.enable_sdio_irq	= dw_mci_enable_sdio_irq,
 	.execute_tuning		= dw_mci_execute_tuning,
+	.card_busy		= dw_mci_card_busy,
+	.start_signal_voltage_switch = dw_mci_switch_voltage,
+
 };
 
 static void dw_mci_request_end(struct dw_mci *host, struct mmc_request *mrq)
@@ -1154,7 +1254,11 @@ static void dw_mci_request_end(struct dw_mci *host, struct mmc_request *mrq)
 		dw_mci_start_request(host, slot);
 	} else {
 		dev_vdbg(host->dev, "list empty\n");
-		host->state = STATE_IDLE;
+
+		if (host->state == STATE_SENDING_CMD11)
+			host->state = STATE_WAITING_CMD11_DONE;
+		else
+			host->state = STATE_IDLE;
 	}
 
 	spin_unlock(&host->lock);
@@ -1265,8 +1369,10 @@ static void dw_mci_tasklet_func(unsigned long priv)
 
 		switch (state) {
 		case STATE_IDLE:
+		case STATE_WAITING_CMD11_DONE:
 			break;
 
+		case STATE_SENDING_CMD11:
 		case STATE_SENDING_CMD:
 			if (!test_and_clear_bit(EVENT_CMD_COMPLETE,
 						&host->pending_events))
@@ -1299,6 +1405,14 @@ static void dw_mci_tasklet_func(unsigned long priv)
 			/* fall through */
 
 		case STATE_SENDING_DATA:
+			/*
+			 * We could get a data error and never a transfer
+			 * complete so we'd better check for it here.
+			 *
+			 * Note that we don't really care if we also got a
+			 * transfer complete; stopping the DMA and sending an
+			 * abort won't hurt.
+			 */
 			if (test_and_clear_bit(EVENT_DATA_ERROR,
 					       &host->pending_events)) {
 				dw_mci_stop_dma(host);
@@ -1312,7 +1426,29 @@ static void dw_mci_tasklet_func(unsigned long priv)
 				break;
 
 			set_bit(EVENT_XFER_COMPLETE, &host->completed_events);
+
+			/*
+			 * Handle an EVENT_DATA_ERROR that might have shown up
+			 * before the transfer completed.  This might not have
+			 * been caught by the check above because the interrupt
+			 * could have gone off between the previous check and
+			 * the check for transfer complete.
+			 *
+			 * Technically this ought not be needed assuming we
+			 * get a DATA_COMPLETE eventually (we'll notice the
+			 * error and end the request), but it shouldn't hurt.
+			 *
+			 * This has the advantage of sending the stop command.
+			 */
+			if (test_and_clear_bit(EVENT_DATA_ERROR,
+					       &host->pending_events)) {
+				dw_mci_stop_dma(host);
+				send_stop_abort(host, data);
+				state = STATE_DATA_ERROR;
+				break;
+			}
 			prev_state = state = STATE_DATA_BUSY;
+
 			/* fall through */
 
 		case STATE_DATA_BUSY:
@@ -1335,6 +1471,22 @@ static void dw_mci_tasklet_func(unsigned long priv)
 				/* stop command for open-ended transfer*/
 				if (data->stop)
 					send_stop_abort(host, data);
+			} else {
+				/*
+				 * If we don't have a command complete now we'll
+				 * never get one since we just reset everything;
+				 * better end the request.
+				 *
+				 * If we do have a command complete we'll fall
+				 * through to the SENDING_STOP command and
+				 * everything will be peachy keen.
+				 */
+				if (!test_bit(EVENT_CMD_COMPLETE,
+					      &host->pending_events)) {
+					host->cmd = NULL;
+					dw_mci_request_end(host, mrq);
+					goto unlock;
+				}
 			}
 
 			/*
@@ -1821,6 +1973,14 @@ static irqreturn_t dw_mci_interrupt(int irq, void *dev_id)
 	}
 
 	if (pending) {
+		/* Check volt switch first, since it can look like an error */
+		if ((host->state == STATE_SENDING_CMD11) &&
+		    (pending & SDMMC_INT_VOLT_SWITCH)) {
+			mci_writel(host, RINTSTS, SDMMC_INT_VOLT_SWITCH);
+			pending &= ~SDMMC_INT_VOLT_SWITCH;
+			dw_mci_cmd_interrupt(host, pending);
+		}
+
 		if (pending & DW_MCI_CMD_ERROR_FLAGS) {
 			mci_writel(host, RINTSTS, DW_MCI_CMD_ERROR_FLAGS);
 			host->cmd_status = pending;
@@ -1926,7 +2086,9 @@ static void dw_mci_work_routine_card(struct work_struct *work)
 
 					switch (host->state) {
 					case STATE_IDLE:
+					case STATE_WAITING_CMD11_DONE:
 						break;
+					case STATE_SENDING_CMD11:
 					case STATE_SENDING_CMD:
 						mrq->cmd->error = -ENOMEDIUM;
 						if (!mrq->data)
@@ -2028,10 +2190,6 @@ static int dw_mci_of_get_slot_quirks(struct device *dev, u8 slot)
 {
 	return 0;
 }
-static struct device_node *dw_mci_of_find_slot_node(struct device *dev, u8 slot)
-{
-	return NULL;
-}
 #endif /* CONFIG_OF */
 
 static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
@@ -2064,7 +2222,13 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 		mmc->f_max = freq[1];
 	}
 
-	mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
+	/*if there are external regulators, get them*/
+	ret = mmc_regulator_get_supply(mmc);
+	if (ret == -EPROBE_DEFER)
+		goto err_host_allocated;
+
+	if (!mmc->ocr_avail)
+		mmc->ocr_avail = MMC_VDD_32_33 | MMC_VDD_33_34;
 
 	if (host->pdata->caps)
 		mmc->caps = host->pdata->caps;
@@ -2085,7 +2249,9 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 	if (host->pdata->caps2)
 		mmc->caps2 = host->pdata->caps2;
 
-	mmc_of_parse(mmc);
+	ret = mmc_of_parse(mmc);
+	if (ret)
+		goto err_host_allocated;
 
 	if (host->pdata->blk_settings) {
 		mmc->max_segs = host->pdata->blk_settings->max_segs;
@@ -2117,7 +2283,7 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 
 	ret = mmc_add_host(mmc);
 	if (ret)
-		goto err_setup_bus;
+		goto err_host_allocated;
 
 #if defined(CONFIG_DEBUG_FS)
 	dw_mci_init_debugfs(slot);
@@ -2128,9 +2294,9 @@ static int dw_mci_init_slot(struct dw_mci *host, unsigned int id)
 
 	return 0;
 
-err_setup_bus:
+err_host_allocated:
 	mmc_free_host(mmc);
-	return -EINVAL;
+	return ret;
 }
 
 static void dw_mci_cleanup_slot(struct dw_mci_slot *slot, unsigned int id)
@@ -2423,24 +2589,6 @@ int dw_mci_probe(struct dw_mci *host)
 		}
 	}
 
-	host->vmmc = devm_regulator_get_optional(host->dev, "vmmc");
-	if (IS_ERR(host->vmmc)) {
-		ret = PTR_ERR(host->vmmc);
-		if (ret == -EPROBE_DEFER)
-			goto err_clk_ciu;
-
-		dev_info(host->dev, "no vmmc regulator found: %d\n", ret);
-		host->vmmc = NULL;
-	} else {
-		ret = regulator_enable(host->vmmc);
-		if (ret) {
-			if (ret != -EPROBE_DEFER)
-				dev_err(host->dev,
-					"regulator_enable fail: %d\n", ret);
-			goto err_clk_ciu;
-		}
-	}
-
 	host->quirks = host->pdata->quirks;
 
 	spin_lock_init(&host->lock);
@@ -2584,8 +2732,6 @@ err_workqueue:
 err_dmaunmap:
 	if (host->use_dma && host->dma_ops->exit)
 		host->dma_ops->exit(host);
-	if (host->vmmc)
-		regulator_disable(host->vmmc);
 
 err_clk_ciu:
 	if (!IS_ERR(host->ciu_clk))
@@ -2621,9 +2767,6 @@ void dw_mci_remove(struct dw_mci *host)
 	if (host->use_dma && host->dma_ops->exit)
 		host->dma_ops->exit(host);
 
-	if (host->vmmc)
-		regulator_disable(host->vmmc);
-
 	if (!IS_ERR(host->ciu_clk))
 		clk_disable_unprepare(host->ciu_clk);
 
@@ -2640,9 +2783,6 @@ EXPORT_SYMBOL(dw_mci_remove);
  */
 int dw_mci_suspend(struct dw_mci *host)
 {
-	if (host->vmmc)
-		regulator_disable(host->vmmc);
-
 	return 0;
 }
 EXPORT_SYMBOL(dw_mci_suspend);
@@ -2651,15 +2791,6 @@ int dw_mci_resume(struct dw_mci *host)
 {
 	int i, ret;
 
-	if (host->vmmc) {
-		ret = regulator_enable(host->vmmc);
-		if (ret) {
-			dev_err(host->dev,
-				"failed to enable regulator: %d\n", ret);
-			return ret;
-		}
-	}
-
 	if (!dw_mci_ctrl_reset(host, SDMMC_CTRL_ALL_RESET_FLAGS)) {
 		ret = -ENODEV;
 		return ret;
diff --git a/drivers/mmc/host/dw_mmc.h b/drivers/mmc/host/dw_mmc.h
index 08fd956d81f3..01b99e8a9190 100644
--- a/drivers/mmc/host/dw_mmc.h
+++ b/drivers/mmc/host/dw_mmc.h
@@ -99,6 +99,7 @@
 #define SDMMC_INT_HLE			BIT(12)
 #define SDMMC_INT_FRUN			BIT(11)
 #define SDMMC_INT_HTO			BIT(10)
+#define SDMMC_INT_VOLT_SWITCH		BIT(10) /* overloads bit 10! */
 #define SDMMC_INT_DRTO			BIT(9)
 #define SDMMC_INT_RTO			BIT(8)
 #define SDMMC_INT_DCRC			BIT(7)
@@ -113,6 +114,7 @@
 /* Command register defines */
 #define SDMMC_CMD_START			BIT(31)
 #define SDMMC_CMD_USE_HOLD_REG	BIT(29)
+#define SDMMC_CMD_VOLT_SWITCH		BIT(28)
 #define SDMMC_CMD_CCS_EXP		BIT(23)
 #define SDMMC_CMD_CEATA_RD		BIT(22)
 #define SDMMC_CMD_UPD_CLK		BIT(21)
@@ -130,6 +132,7 @@
 /* Status register defines */
 #define SDMMC_GET_FCNT(x)		(((x)>>17) & 0x1FFF)
 #define SDMMC_STATUS_DMA_REQ		BIT(31)
+#define SDMMC_STATUS_BUSY		BIT(9)
 /* FIFOTH register defines */
 #define SDMMC_SET_FIFOTH(m, r, t)	(((m) & 0x7) << 28 | \
 					 ((r) & 0xFFF) << 16 | \
@@ -150,7 +153,7 @@
 #define SDMMC_GET_VERID(x)		((x) & 0xFFFF)
 /* Card read threshold */
 #define SDMMC_SET_RD_THLD(v, x)		(((v) & 0x1FFF) << 16 | (x))
-
+#define SDMMC_UHS_18V			BIT(0)
 /* All ctrl reset bits */
 #define SDMMC_CTRL_ALL_RESET_FLAGS \
 	(SDMMC_CTRL_RESET | SDMMC_CTRL_FIFO_RESET | SDMMC_CTRL_DMA_RESET)
diff --git a/drivers/mmc/host/jz4740_mmc.c b/drivers/mmc/host/jz4740_mmc.c
index 537d6c7a5ae4..76e8bce6f46e 100644
--- a/drivers/mmc/host/jz4740_mmc.c
+++ b/drivers/mmc/host/jz4740_mmc.c
@@ -30,7 +30,9 @@
 #include <asm/mach-jz4740/gpio.h>
 #include <asm/cacheflush.h>
 #include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
 
+#include <asm/mach-jz4740/dma.h>
 #include <asm/mach-jz4740/jz4740_mmc.h>
 
 #define JZ_REG_MMC_STRPCL	0x00
@@ -112,6 +114,11 @@ enum jz4740_mmc_state {
 	JZ4740_MMC_STATE_DONE,
 };
 
+struct jz4740_mmc_host_next {
+	int sg_len;
+	s32 cookie;
+};
+
 struct jz4740_mmc_host {
 	struct mmc_host *mmc;
 	struct platform_device *pdev;
@@ -122,6 +129,7 @@ struct jz4740_mmc_host {
 	int card_detect_irq;
 
 	void __iomem *base;
+	struct resource *mem_res;
 	struct mmc_request *req;
 	struct mmc_command *cmd;
 
@@ -136,8 +144,220 @@ struct jz4740_mmc_host {
 	struct timer_list timeout_timer;
 	struct sg_mapping_iter miter;
 	enum jz4740_mmc_state state;
+
+	/* DMA support */
+	struct dma_chan *dma_rx;
+	struct dma_chan *dma_tx;
+	struct jz4740_mmc_host_next next_data;
+	bool use_dma;
+	int sg_len;
+
+/* The DMA trigger level is 8 words, that is to say, the DMA read
+ * trigger is when data words in MSC_RXFIFO is >= 8 and the DMA write
+ * trigger is when data words in MSC_TXFIFO is < 8.
+ */
+#define JZ4740_MMC_FIFO_HALF_SIZE 8
 };
 
+/*----------------------------------------------------------------------------*/
+/* DMA infrastructure */
+
+static void jz4740_mmc_release_dma_channels(struct jz4740_mmc_host *host)
+{
+	if (!host->use_dma)
+		return;
+
+	dma_release_channel(host->dma_tx);
+	dma_release_channel(host->dma_rx);
+}
+
+static int jz4740_mmc_acquire_dma_channels(struct jz4740_mmc_host *host)
+{
+	dma_cap_mask_t mask;
+
+	dma_cap_zero(mask);
+	dma_cap_set(DMA_SLAVE, mask);
+
+	host->dma_tx = dma_request_channel(mask, NULL, host);
+	if (!host->dma_tx) {
+		dev_err(mmc_dev(host->mmc), "Failed to get dma_tx channel\n");
+		return -ENODEV;
+	}
+
+	host->dma_rx = dma_request_channel(mask, NULL, host);
+	if (!host->dma_rx) {
+		dev_err(mmc_dev(host->mmc), "Failed to get dma_rx channel\n");
+		goto free_master_write;
+	}
+
+	/* Initialize DMA pre request cookie */
+	host->next_data.cookie = 1;
+
+	return 0;
+
+free_master_write:
+	dma_release_channel(host->dma_tx);
+	return -ENODEV;
+}
+
+static inline int jz4740_mmc_get_dma_dir(struct mmc_data *data)
+{
+	return (data->flags & MMC_DATA_READ) ? DMA_FROM_DEVICE : DMA_TO_DEVICE;
+}
+
+static inline struct dma_chan *jz4740_mmc_get_dma_chan(struct jz4740_mmc_host *host,
+						       struct mmc_data *data)
+{
+	return (data->flags & MMC_DATA_READ) ? host->dma_rx : host->dma_tx;
+}
+
+static void jz4740_mmc_dma_unmap(struct jz4740_mmc_host *host,
+				 struct mmc_data *data)
+{
+	struct dma_chan *chan = jz4740_mmc_get_dma_chan(host, data);
+	enum dma_data_direction dir = jz4740_mmc_get_dma_dir(data);
+
+	dma_unmap_sg(chan->device->dev, data->sg, data->sg_len, dir);
+}
+
+/* Prepares DMA data for current/next transfer, returns non-zero on failure */
+static int jz4740_mmc_prepare_dma_data(struct jz4740_mmc_host *host,
+				       struct mmc_data *data,
+				       struct jz4740_mmc_host_next *next,
+				       struct dma_chan *chan)
+{
+	struct jz4740_mmc_host_next *next_data = &host->next_data;
+	enum dma_data_direction dir = jz4740_mmc_get_dma_dir(data);
+	int sg_len;
+
+	if (!next && data->host_cookie &&
+	    data->host_cookie != host->next_data.cookie) {
+		dev_warn(mmc_dev(host->mmc),
+			 "[%s] invalid cookie: data->host_cookie %d host->next_data.cookie %d\n",
+			 __func__,
+			 data->host_cookie,
+			 host->next_data.cookie);
+		data->host_cookie = 0;
+	}
+
+	/* Check if next job is already prepared */
+	if (next || data->host_cookie != host->next_data.cookie) {
+		sg_len = dma_map_sg(chan->device->dev,
+				    data->sg,
+				    data->sg_len,
+				    dir);
+
+	} else {
+		sg_len = next_data->sg_len;
+		next_data->sg_len = 0;
+	}
+
+	if (sg_len <= 0) {
+		dev_err(mmc_dev(host->mmc),
+			"Failed to map scatterlist for DMA operation\n");
+		return -EINVAL;
+	}
+
+	if (next) {
+		next->sg_len = sg_len;
+		data->host_cookie = ++next->cookie < 0 ? 1 : next->cookie;
+	} else
+		host->sg_len = sg_len;
+
+	return 0;
+}
+
+static int jz4740_mmc_start_dma_transfer(struct jz4740_mmc_host *host,
+					 struct mmc_data *data)
+{
+	int ret;
+	struct dma_chan *chan;
+	struct dma_async_tx_descriptor *desc;
+	struct dma_slave_config conf = {
+		.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES,
+		.src_maxburst = JZ4740_MMC_FIFO_HALF_SIZE,
+		.dst_maxburst = JZ4740_MMC_FIFO_HALF_SIZE,
+	};
+
+	if (data->flags & MMC_DATA_WRITE) {
+		conf.direction = DMA_MEM_TO_DEV;
+		conf.dst_addr = host->mem_res->start + JZ_REG_MMC_TXFIFO;
+		conf.slave_id = JZ4740_DMA_TYPE_MMC_TRANSMIT;
+		chan = host->dma_tx;
+	} else {
+		conf.direction = DMA_DEV_TO_MEM;
+		conf.src_addr = host->mem_res->start + JZ_REG_MMC_RXFIFO;
+		conf.slave_id = JZ4740_DMA_TYPE_MMC_RECEIVE;
+		chan = host->dma_rx;
+	}
+
+	ret = jz4740_mmc_prepare_dma_data(host, data, NULL, chan);
+	if (ret)
+		return ret;
+
+	dmaengine_slave_config(chan, &conf);
+	desc = dmaengine_prep_slave_sg(chan,
+				       data->sg,
+				       host->sg_len,
+				       conf.direction,
+				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+	if (!desc) {
+		dev_err(mmc_dev(host->mmc),
+			"Failed to allocate DMA %s descriptor",
+			 conf.direction == DMA_MEM_TO_DEV ? "TX" : "RX");
+		goto dma_unmap;
+	}
+
+	dmaengine_submit(desc);
+	dma_async_issue_pending(chan);
+
+	return 0;
+
+dma_unmap:
+	jz4740_mmc_dma_unmap(host, data);
+	return -ENOMEM;
+}
+
+static void jz4740_mmc_pre_request(struct mmc_host *mmc,
+				   struct mmc_request *mrq,
+				   bool is_first_req)
+{
+	struct jz4740_mmc_host *host = mmc_priv(mmc);
+	struct mmc_data *data = mrq->data;
+	struct jz4740_mmc_host_next *next_data = &host->next_data;
+
+	BUG_ON(data->host_cookie);
+
+	if (host->use_dma) {
+		struct dma_chan *chan = jz4740_mmc_get_dma_chan(host, data);
+
+		if (jz4740_mmc_prepare_dma_data(host, data, next_data, chan))
+			data->host_cookie = 0;
+	}
+}
+
+static void jz4740_mmc_post_request(struct mmc_host *mmc,
+				    struct mmc_request *mrq,
+				    int err)
+{
+	struct jz4740_mmc_host *host = mmc_priv(mmc);
+	struct mmc_data *data = mrq->data;
+
+	if (host->use_dma && data->host_cookie) {
+		jz4740_mmc_dma_unmap(host, data);
+		data->host_cookie = 0;
+	}
+
+	if (err) {
+		struct dma_chan *chan = jz4740_mmc_get_dma_chan(host, data);
+
+		dmaengine_terminate_all(chan);
+	}
+}
+
+/*----------------------------------------------------------------------------*/
+
 static void jz4740_mmc_set_irq_enabled(struct jz4740_mmc_host *host,
 	unsigned int irq, bool enabled)
 {
@@ -442,6 +662,8 @@ static void jz4740_mmc_send_command(struct jz4740_mmc_host *host,
 			cmdat |= JZ_MMC_CMDAT_WRITE;
 		if (cmd->data->flags & MMC_DATA_STREAM)
 			cmdat |= JZ_MMC_CMDAT_STREAM;
+		if (host->use_dma)
+			cmdat |= JZ_MMC_CMDAT_DMA_EN;
 
 		writew(cmd->data->blksz, host->base + JZ_REG_MMC_BLKLEN);
 		writew(cmd->data->blocks, host->base + JZ_REG_MMC_NOB);
@@ -474,6 +696,7 @@ static irqreturn_t jz_mmc_irq_worker(int irq, void *devid)
 	struct jz4740_mmc_host *host = (struct jz4740_mmc_host *)devid;
 	struct mmc_command *cmd = host->req->cmd;
 	struct mmc_request *req = host->req;
+	struct mmc_data *data = cmd->data;
 	bool timeout = false;
 
 	if (cmd->error)
@@ -484,23 +707,37 @@ static irqreturn_t jz_mmc_irq_worker(int irq, void *devid)
 		if (cmd->flags & MMC_RSP_PRESENT)
 			jz4740_mmc_read_response(host, cmd);
 
-		if (!cmd->data)
+		if (!data)
 			break;
 
 		jz_mmc_prepare_data_transfer(host);
 
 	case JZ4740_MMC_STATE_TRANSFER_DATA:
-		if (cmd->data->flags & MMC_DATA_READ)
-			timeout = jz4740_mmc_read_data(host, cmd->data);
+		if (host->use_dma) {
+			/* Use DMA if enabled.
+			 * Data transfer direction is defined later by
+			 * relying on data flags in
+			 * jz4740_mmc_prepare_dma_data() and
+			 * jz4740_mmc_start_dma_transfer().
+			 */
+			timeout = jz4740_mmc_start_dma_transfer(host, data);
+			data->bytes_xfered = data->blocks * data->blksz;
+		} else if (data->flags & MMC_DATA_READ)
+			/* Use PIO if DMA is not enabled.
+			 * Data transfer direction was defined before
+			 * by relying on data flags in
+			 * jz_mmc_prepare_data_transfer().
+			 */
+			timeout = jz4740_mmc_read_data(host, data);
 		else
-			timeout = jz4740_mmc_write_data(host, cmd->data);
+			timeout = jz4740_mmc_write_data(host, data);
 
 		if (unlikely(timeout)) {
 			host->state = JZ4740_MMC_STATE_TRANSFER_DATA;
 			break;
 		}
 
-		jz4740_mmc_transfer_check_state(host, cmd->data);
+		jz4740_mmc_transfer_check_state(host, data);
 
 		timeout = jz4740_mmc_poll_irq(host, JZ_MMC_IRQ_DATA_TRAN_DONE);
 		if (unlikely(timeout)) {
@@ -664,6 +901,8 @@ static void jz4740_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
 
 static const struct mmc_host_ops jz4740_mmc_ops = {
 	.request	= jz4740_mmc_request,
+	.pre_req	= jz4740_mmc_pre_request,
+	.post_req	= jz4740_mmc_post_request,
 	.set_ios	= jz4740_mmc_set_ios,
 	.get_ro		= mmc_gpio_get_ro,
 	.get_cd		= mmc_gpio_get_cd,
@@ -757,7 +996,6 @@ static int jz4740_mmc_probe(struct platform_device* pdev)
 	struct mmc_host *mmc;
 	struct jz4740_mmc_host *host;
 	struct jz4740_mmc_platform_data *pdata;
-	struct resource *res;
 
 	pdata = pdev->dev.platform_data;
 
@@ -784,10 +1022,11 @@ static int jz4740_mmc_probe(struct platform_device* pdev)
 		goto err_free_host;
 	}
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	host->base = devm_ioremap_resource(&pdev->dev, res);
+	host->mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	host->base = devm_ioremap_resource(&pdev->dev, host->mem_res);
 	if (IS_ERR(host->base)) {
 		ret = PTR_ERR(host->base);
+		dev_err(&pdev->dev, "Failed to ioremap base memory\n");
 		goto err_free_host;
 	}
 
@@ -834,6 +1073,10 @@ static int jz4740_mmc_probe(struct platform_device* pdev)
 	/* It is not important when it times out, it just needs to timeout. */
 	set_timer_slack(&host->timeout_timer, HZ);
 
+	host->use_dma = true;
+	if (host->use_dma && jz4740_mmc_acquire_dma_channels(host) != 0)
+		host->use_dma = false;
+
 	platform_set_drvdata(pdev, host);
 	ret = mmc_add_host(mmc);
 
@@ -843,6 +1086,10 @@ static int jz4740_mmc_probe(struct platform_device* pdev)
 	}
 	dev_info(&pdev->dev, "JZ SD/MMC card driver registered\n");
 
+	dev_info(&pdev->dev, "Using %s, %d-bit mode\n",
+		 host->use_dma ? "DMA" : "PIO",
+		 (mmc->caps & MMC_CAP_4_BIT_DATA) ? 4 : 1);
+
 	return 0;
 
 err_free_irq:
@@ -850,6 +1097,8 @@ err_free_irq:
 err_free_gpios:
 	jz4740_mmc_free_gpios(pdev);
 err_gpio_bulk_free:
+	if (host->use_dma)
+		jz4740_mmc_release_dma_channels(host);
 	jz_gpio_bulk_free(jz4740_mmc_pins, jz4740_mmc_num_pins(host));
 err_free_host:
 	mmc_free_host(mmc);
@@ -872,6 +1121,9 @@ static int jz4740_mmc_remove(struct platform_device *pdev)
 	jz4740_mmc_free_gpios(pdev);
 	jz_gpio_bulk_free(jz4740_mmc_pins, jz4740_mmc_num_pins(host));
 
+	if (host->use_dma)
+		jz4740_mmc_release_dma_channels(host);
+
 	mmc_free_host(host->mmc);
 
 	return 0;
@@ -909,7 +1161,6 @@ static struct platform_driver jz4740_mmc_driver = {
 	.remove = jz4740_mmc_remove,
 	.driver = {
 		.name = "jz4740-mmc",
-		.owner = THIS_MODULE,
 		.pm = JZ4740_MMC_PM_OPS,
 	},
 };
diff --git a/drivers/mmc/host/mmc_spi.c b/drivers/mmc/host/mmc_spi.c
index cc8d4a6099cd..e4a07546f8b6 100644
--- a/drivers/mmc/host/mmc_spi.c
+++ b/drivers/mmc/host/mmc_spi.c
@@ -1436,6 +1436,7 @@ static int mmc_spi_probe(struct spi_device *spi)
 					     host->pdata->cd_debounce);
 		if (status != 0)
 			goto fail_add_host;
+		mmc_gpiod_request_cd_irq(mmc);
 	}
 
 	if (host->pdata && host->pdata->flags & MMC_SPI_USE_RO_GPIO) {
diff --git a/drivers/mmc/host/mmci.c b/drivers/mmc/host/mmci.c
index e4d470704150..43af791e2e45 100644
--- a/drivers/mmc/host/mmci.c
+++ b/drivers/mmc/host/mmci.c
@@ -43,6 +43,7 @@
 #include <asm/sizes.h>
 
 #include "mmci.h"
+#include "mmci_qcom_dml.h"
 
 #define DRIVER_NAME "mmci-pl18x"
 
@@ -60,12 +61,13 @@ static unsigned int fmax = 515633;
  * @fifohalfsize: number of bytes that can be written when MCI_TXFIFOHALFEMPTY
  *		  is asserted (likewise for RX)
  * @data_cmd_enable: enable value for data commands.
- * @sdio: variant supports SDIO
+ * @st_sdio: enable ST specific SDIO logic
  * @st_clkdiv: true if using a ST-specific clock divider algorithm
  * @datactrl_mask_ddrmode: ddr mode mask in datactrl register.
  * @blksz_datactrl16: true if Block size is at b16..b30 position in datactrl register
  * @blksz_datactrl4: true if Block size is at b4..b16 position in datactrl
  *		     register
+ * @datactrl_mask_sdio: SDIO enable mask in datactrl register
  * @pwrreg_powerup: power up value for MMCIPOWER register
  * @f_max: maximum clk frequency supported by the controller.
  * @signal_direction: input/out direction of bus signals can be indicated
@@ -74,6 +76,7 @@ static unsigned int fmax = 515633;
  * @pwrreg_nopower: bits in MMCIPOWER don't controls ext. power supply
  * @explicit_mclk_control: enable explicit mclk control in driver.
  * @qcom_fifo: enables qcom specific fifo pio read logic.
+ * @qcom_dml: enables qcom specific dma glue for dma transfers.
  * @reversed_irq_handling: handle data irq before cmd irq.
  */
 struct variant_data {
@@ -86,7 +89,8 @@ struct variant_data {
 	unsigned int		fifohalfsize;
 	unsigned int		data_cmd_enable;
 	unsigned int		datactrl_mask_ddrmode;
-	bool			sdio;
+	unsigned int		datactrl_mask_sdio;
+	bool			st_sdio;
 	bool			st_clkdiv;
 	bool			blksz_datactrl16;
 	bool			blksz_datactrl4;
@@ -98,6 +102,7 @@ struct variant_data {
 	bool			pwrreg_nopower;
 	bool			explicit_mclk_control;
 	bool			qcom_fifo;
+	bool			qcom_dml;
 	bool			reversed_irq_handling;
 };
 
@@ -133,7 +138,8 @@ static struct variant_data variant_u300 = {
 	.clkreg_enable		= MCI_ST_U300_HWFCEN,
 	.clkreg_8bit_bus_enable = MCI_ST_8BIT_BUS,
 	.datalength_bits	= 16,
-	.sdio			= true,
+	.datactrl_mask_sdio	= MCI_ST_DPSM_SDIOEN,
+	.st_sdio			= true,
 	.pwrreg_powerup		= MCI_PWR_ON,
 	.f_max			= 100000000,
 	.signal_direction	= true,
@@ -146,7 +152,8 @@ static struct variant_data variant_nomadik = {
 	.fifohalfsize		= 8 * 4,
 	.clkreg			= MCI_CLK_ENABLE,
 	.datalength_bits	= 24,
-	.sdio			= true,
+	.datactrl_mask_sdio	= MCI_ST_DPSM_SDIOEN,
+	.st_sdio		= true,
 	.st_clkdiv		= true,
 	.pwrreg_powerup		= MCI_PWR_ON,
 	.f_max			= 100000000,
@@ -163,7 +170,8 @@ static struct variant_data variant_ux500 = {
 	.clkreg_8bit_bus_enable = MCI_ST_8BIT_BUS,
 	.clkreg_neg_edge_enable	= MCI_ST_UX500_NEG_EDGE,
 	.datalength_bits	= 24,
-	.sdio			= true,
+	.datactrl_mask_sdio	= MCI_ST_DPSM_SDIOEN,
+	.st_sdio		= true,
 	.st_clkdiv		= true,
 	.pwrreg_powerup		= MCI_PWR_ON,
 	.f_max			= 100000000,
@@ -182,7 +190,8 @@ static struct variant_data variant_ux500v2 = {
 	.clkreg_neg_edge_enable	= MCI_ST_UX500_NEG_EDGE,
 	.datactrl_mask_ddrmode	= MCI_ST_DPSM_DDRMODE,
 	.datalength_bits	= 24,
-	.sdio			= true,
+	.datactrl_mask_sdio	= MCI_ST_DPSM_SDIOEN,
+	.st_sdio		= true,
 	.st_clkdiv		= true,
 	.blksz_datactrl16	= true,
 	.pwrreg_powerup		= MCI_PWR_ON,
@@ -208,6 +217,7 @@ static struct variant_data variant_qcom = {
 	.f_max			= 208000000,
 	.explicit_mclk_control	= true,
 	.qcom_fifo		= true,
+	.qcom_dml		= true,
 };
 
 static int mmci_card_busy(struct mmc_host *mmc)
@@ -421,6 +431,7 @@ static void mmci_dma_setup(struct mmci_host *host)
 {
 	const char *rxname, *txname;
 	dma_cap_mask_t mask;
+	struct variant_data *variant = host->variant;
 
 	host->dma_rx_channel = dma_request_slave_channel(mmc_dev(host->mmc), "rx");
 	host->dma_tx_channel = dma_request_slave_channel(mmc_dev(host->mmc), "tx");
@@ -471,6 +482,10 @@ static void mmci_dma_setup(struct mmci_host *host)
 		if (max_seg_size < host->mmc->max_seg_size)
 			host->mmc->max_seg_size = max_seg_size;
 	}
+
+	if (variant->qcom_dml && host->dma_rx_channel && host->dma_tx_channel)
+		if (dml_hw_init(host, host->mmc->parent->of_node))
+			variant->qcom_dml = false;
 }
 
 /*
@@ -572,6 +587,7 @@ static int __mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
 	struct dma_async_tx_descriptor *desc;
 	enum dma_data_direction buffer_dirn;
 	int nr_sg;
+	unsigned long flags = DMA_CTRL_ACK;
 
 	if (data->flags & MMC_DATA_READ) {
 		conf.direction = DMA_DEV_TO_MEM;
@@ -596,9 +612,12 @@ static int __mmci_dma_prep_data(struct mmci_host *host, struct mmc_data *data,
 	if (nr_sg == 0)
 		return -EINVAL;
 
+	if (host->variant->qcom_dml)
+		flags |= DMA_PREP_INTERRUPT;
+
 	dmaengine_slave_config(chan, &conf);
 	desc = dmaengine_prep_slave_sg(chan, data->sg, nr_sg,
-					    conf.direction, DMA_CTRL_ACK);
+					    conf.direction, flags);
 	if (!desc)
 		goto unmap_exit;
 
@@ -647,6 +666,9 @@ static int mmci_dma_start_data(struct mmci_host *host, unsigned int datactrl)
 	dmaengine_submit(host->dma_desc_current);
 	dma_async_issue_pending(host->dma_current);
 
+	if (host->variant->qcom_dml)
+		dml_start_xfer(host, data);
+
 	datactrl |= MCI_DPSM_DMAENABLE;
 
 	/* Trigger the DMA transfer */
@@ -792,32 +814,26 @@ static void mmci_start_data(struct mmci_host *host, struct mmc_data *data)
 	if (data->flags & MMC_DATA_READ)
 		datactrl |= MCI_DPSM_DIRECTION;
 
-	/* The ST Micro variants has a special bit to enable SDIO */
-	if (variant->sdio && host->mmc->card)
-		if (mmc_card_sdio(host->mmc->card)) {
-			/*
-			 * The ST Micro variants has a special bit
-			 * to enable SDIO.
-			 */
-			u32 clk;
+	if (host->mmc->card && mmc_card_sdio(host->mmc->card)) {
+		u32 clk;
 
-			datactrl |= MCI_ST_DPSM_SDIOEN;
+		datactrl |= variant->datactrl_mask_sdio;
 
-			/*
-			 * The ST Micro variant for SDIO small write transfers
-			 * needs to have clock H/W flow control disabled,
-			 * otherwise the transfer will not start. The threshold
-			 * depends on the rate of MCLK.
-			 */
-			if (data->flags & MMC_DATA_WRITE &&
-			    (host->size < 8 ||
-			     (host->size <= 8 && host->mclk > 50000000)))
-				clk = host->clk_reg & ~variant->clkreg_enable;
-			else
-				clk = host->clk_reg | variant->clkreg_enable;
+		/*
+		 * The ST Micro variant for SDIO small write transfers
+		 * needs to have clock H/W flow control disabled,
+		 * otherwise the transfer will not start. The threshold
+		 * depends on the rate of MCLK.
+		 */
+		if (variant->st_sdio && data->flags & MMC_DATA_WRITE &&
+		    (host->size < 8 ||
+		     (host->size <= 8 && host->mclk > 50000000)))
+			clk = host->clk_reg & ~variant->clkreg_enable;
+		else
+			clk = host->clk_reg | variant->clkreg_enable;
 
-			mmci_write_clkreg(host, clk);
-		}
+		mmci_write_clkreg(host, clk);
+	}
 
 	if (host->mmc->ios.timing == MMC_TIMING_UHS_DDR50 ||
 	    host->mmc->ios.timing == MMC_TIMING_MMC_DDR52)
@@ -1658,16 +1674,35 @@ static int mmci_probe(struct amba_device *dev,
 	writel(0, host->base + MMCIMASK1);
 	writel(0xfff, host->base + MMCICLEAR);
 
-	/* If DT, cd/wp gpios must be supplied through it. */
-	if (!np && gpio_is_valid(plat->gpio_cd)) {
-		ret = mmc_gpio_request_cd(mmc, plat->gpio_cd, 0);
-		if (ret)
-			goto clk_disable;
-	}
-	if (!np && gpio_is_valid(plat->gpio_wp)) {
-		ret = mmc_gpio_request_ro(mmc, plat->gpio_wp);
-		if (ret)
-			goto clk_disable;
+	/*
+	 * If:
+	 * - not using DT but using a descriptor table, or
+	 * - using a table of descriptors ALONGSIDE DT, or
+	 * look up these descriptors named "cd" and "wp" right here, fail
+	 * silently of these do not exist and proceed to try platform data
+	 */
+	if (!np) {
+		ret = mmc_gpiod_request_cd(mmc, "cd", 0, false, 0, NULL);
+		if (ret < 0) {
+			if (ret == -EPROBE_DEFER)
+				goto clk_disable;
+			else if (gpio_is_valid(plat->gpio_cd)) {
+				ret = mmc_gpio_request_cd(mmc, plat->gpio_cd, 0);
+				if (ret)
+					goto clk_disable;
+			}
+		}
+
+		ret = mmc_gpiod_request_ro(mmc, "wp", 0, false, 0, NULL);
+		if (ret < 0) {
+			if (ret == -EPROBE_DEFER)
+				goto clk_disable;
+			else if (gpio_is_valid(plat->gpio_wp)) {
+				ret = mmc_gpio_request_ro(mmc, plat->gpio_wp);
+				if (ret)
+					goto clk_disable;
+			}
+		}
 	}
 
 	ret = devm_request_irq(&dev->dev, dev->irq[0], mmci_irq, IRQF_SHARED,
diff --git a/drivers/mmc/host/mmci_qcom_dml.c b/drivers/mmc/host/mmci_qcom_dml.c
new file mode 100644
index 000000000000..2b7fc3764803
--- /dev/null
+++ b/drivers/mmc/host/mmci_qcom_dml.c
@@ -0,0 +1,177 @@
+/*
+ *
+ * Copyright (c) 2011, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/of.h>
+#include <linux/of_dma.h>
+#include <linux/bitops.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/card.h>
+#include "mmci.h"
+
+/* Registers */
+#define DML_CONFIG			0x00
+#define PRODUCER_CRCI_MSK		GENMASK(1, 0)
+#define PRODUCER_CRCI_DISABLE		0
+#define PRODUCER_CRCI_X_SEL		BIT(0)
+#define PRODUCER_CRCI_Y_SEL		BIT(1)
+#define CONSUMER_CRCI_MSK		GENMASK(3, 2)
+#define CONSUMER_CRCI_DISABLE		0
+#define CONSUMER_CRCI_X_SEL		BIT(2)
+#define CONSUMER_CRCI_Y_SEL		BIT(3)
+#define PRODUCER_TRANS_END_EN		BIT(4)
+#define BYPASS				BIT(16)
+#define DIRECT_MODE			BIT(17)
+#define INFINITE_CONS_TRANS		BIT(18)
+
+#define DML_SW_RESET			0x08
+#define DML_PRODUCER_START		0x0c
+#define DML_CONSUMER_START		0x10
+#define DML_PRODUCER_PIPE_LOGICAL_SIZE	0x14
+#define DML_CONSUMER_PIPE_LOGICAL_SIZE	0x18
+#define DML_PIPE_ID			0x1c
+#define PRODUCER_PIPE_ID_SHFT		0
+#define PRODUCER_PIPE_ID_MSK		GENMASK(4, 0)
+#define CONSUMER_PIPE_ID_SHFT		16
+#define CONSUMER_PIPE_ID_MSK		GENMASK(20, 16)
+
+#define DML_PRODUCER_BAM_BLOCK_SIZE	0x24
+#define DML_PRODUCER_BAM_TRANS_SIZE	0x28
+
+/* other definitions */
+#define PRODUCER_PIPE_LOGICAL_SIZE	4096
+#define CONSUMER_PIPE_LOGICAL_SIZE	4096
+
+#define DML_OFFSET			0x800
+
+void dml_start_xfer(struct mmci_host *host, struct mmc_data *data)
+{
+	u32 config;
+	void __iomem *base = host->base + DML_OFFSET;
+
+	if (data->flags & MMC_DATA_READ) {
+		/* Read operation: configure DML for producer operation */
+		/* Set producer CRCI-x and disable consumer CRCI */
+		config = readl_relaxed(base + DML_CONFIG);
+		config = (config & ~PRODUCER_CRCI_MSK) | PRODUCER_CRCI_X_SEL;
+		config = (config & ~CONSUMER_CRCI_MSK) | CONSUMER_CRCI_DISABLE;
+		writel_relaxed(config, base + DML_CONFIG);
+
+		/* Set the Producer BAM block size */
+		writel_relaxed(data->blksz, base + DML_PRODUCER_BAM_BLOCK_SIZE);
+
+		/* Set Producer BAM Transaction size */
+		writel_relaxed(data->blocks * data->blksz,
+			       base + DML_PRODUCER_BAM_TRANS_SIZE);
+		/* Set Producer Transaction End bit */
+		config = readl_relaxed(base + DML_CONFIG);
+		config |= PRODUCER_TRANS_END_EN;
+		writel_relaxed(config, base + DML_CONFIG);
+		/* Trigger producer */
+		writel_relaxed(1, base + DML_PRODUCER_START);
+	} else {
+		/* Write operation: configure DML for consumer operation */
+		/* Set consumer CRCI-x and disable producer CRCI*/
+		config = readl_relaxed(base + DML_CONFIG);
+		config = (config & ~CONSUMER_CRCI_MSK) | CONSUMER_CRCI_X_SEL;
+		config = (config & ~PRODUCER_CRCI_MSK) | PRODUCER_CRCI_DISABLE;
+		writel_relaxed(config, base + DML_CONFIG);
+		/* Clear Producer Transaction End bit */
+		config = readl_relaxed(base + DML_CONFIG);
+		config &= ~PRODUCER_TRANS_END_EN;
+		writel_relaxed(config, base + DML_CONFIG);
+		/* Trigger consumer */
+		writel_relaxed(1, base + DML_CONSUMER_START);
+	}
+
+	/* make sure the dml is configured before dma is triggered */
+	wmb();
+}
+
+static int of_get_dml_pipe_index(struct device_node *np, const char *name)
+{
+	int index;
+	struct of_phandle_args	dma_spec;
+
+	index = of_property_match_string(np, "dma-names", name);
+
+	if (index < 0)
+		return -ENODEV;
+
+	if (of_parse_phandle_with_args(np, "dmas", "#dma-cells", index,
+				       &dma_spec))
+		return -ENODEV;
+
+	if (dma_spec.args_count)
+		return dma_spec.args[0];
+
+	return -ENODEV;
+}
+
+/* Initialize the dml hardware connected to SD Card controller */
+int dml_hw_init(struct mmci_host *host, struct device_node *np)
+{
+	u32 config;
+	void __iomem *base;
+	int consumer_id, producer_id;
+
+	consumer_id = of_get_dml_pipe_index(np, "tx");
+	producer_id = of_get_dml_pipe_index(np, "rx");
+
+	if (producer_id < 0 || consumer_id < 0)
+		return -ENODEV;
+
+	base = host->base + DML_OFFSET;
+
+	/* Reset the DML block */
+	writel_relaxed(1, base + DML_SW_RESET);
+
+	/* Disable the producer and consumer CRCI */
+	config = (PRODUCER_CRCI_DISABLE | CONSUMER_CRCI_DISABLE);
+	/*
+	 * Disable the bypass mode. Bypass mode will only be used
+	 * if data transfer is to happen in PIO mode and don't
+	 * want the BAM interface to connect with SDCC-DML.
+	 */
+	config &= ~BYPASS;
+	/*
+	 * Disable direct mode as we don't DML to MASTER the AHB bus.
+	 * BAM connected with DML should MASTER the AHB bus.
+	 */
+	config &= ~DIRECT_MODE;
+	/*
+	 * Disable infinite mode transfer as we won't be doing any
+	 * infinite size data transfers. All data transfer will be
+	 * of finite data size.
+	 */
+	config &= ~INFINITE_CONS_TRANS;
+	writel_relaxed(config, base + DML_CONFIG);
+
+	/*
+	 * Initialize the logical BAM pipe size for producer
+	 * and consumer.
+	 */
+	writel_relaxed(PRODUCER_PIPE_LOGICAL_SIZE,
+		       base + DML_PRODUCER_PIPE_LOGICAL_SIZE);
+	writel_relaxed(CONSUMER_PIPE_LOGICAL_SIZE,
+		       base + DML_CONSUMER_PIPE_LOGICAL_SIZE);
+
+	/* Initialize Producer/consumer pipe id */
+	writel_relaxed(producer_id | (consumer_id << CONSUMER_PIPE_ID_SHFT),
+		       base + DML_PIPE_ID);
+
+	/* Make sure dml intialization is finished */
+	mb();
+
+	return 0;
+}
diff --git a/drivers/mmc/host/mmci_qcom_dml.h b/drivers/mmc/host/mmci_qcom_dml.h
new file mode 100644
index 000000000000..6e405d09d534
--- /dev/null
+++ b/drivers/mmc/host/mmci_qcom_dml.h
@@ -0,0 +1,31 @@
+/*
+ *
+ * Copyright (c) 2011, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#ifndef __MMC_QCOM_DML_H__
+#define __MMC_QCOM_DML_H__
+
+#ifdef CONFIG_MMC_QCOM_DML
+int dml_hw_init(struct mmci_host *host, struct device_node *np);
+void dml_start_xfer(struct mmci_host *host, struct mmc_data *data);
+#else
+static inline int dml_hw_init(struct mmci_host *host, struct device_node *np)
+{
+	return -ENOSYS;
+}
+static inline void dml_start_xfer(struct mmci_host *host, struct mmc_data *data)
+{
+}
+#endif /* CONFIG_MMC_QCOM_DML */
+
+#endif /* __MMC_QCOM_DML_H__ */
diff --git a/drivers/mmc/host/moxart-mmc.c b/drivers/mmc/host/moxart-mmc.c
index b4b1efbf6c16..f3e18d08e852 100644
--- a/drivers/mmc/host/moxart-mmc.c
+++ b/drivers/mmc/host/moxart-mmc.c
@@ -717,7 +717,6 @@ static struct platform_driver moxart_mmc_driver = {
 	.remove     = moxart_remove,
 	.driver     = {
 		.name		= "mmc-moxart",
-		.owner		= THIS_MODULE,
 		.of_match_table	= moxart_mmc_match,
 	},
 };
diff --git a/drivers/mmc/host/mxcmmc.c b/drivers/mmc/host/mxcmmc.c
index ed1cb93c3784..ad111422ad55 100644
--- a/drivers/mmc/host/mxcmmc.c
+++ b/drivers/mmc/host/mxcmmc.c
@@ -1238,7 +1238,6 @@ static struct platform_driver mxcmci_driver = {
 	.id_table	= mxcmci_devtype,
 	.driver		= {
 		.name		= DRIVER_NAME,
-		.owner		= THIS_MODULE,
 		.pm	= &mxcmci_pm_ops,
 		.of_match_table	= mxcmci_of_match,
 	}
diff --git a/drivers/mmc/host/mxs-mmc.c b/drivers/mmc/host/mxs-mmc.c
index 140885a5a4e7..cd74e5143c36 100644
--- a/drivers/mmc/host/mxs-mmc.c
+++ b/drivers/mmc/host/mxs-mmc.c
@@ -735,7 +735,6 @@ static struct platform_driver mxs_mmc_driver = {
 	.id_table	= mxs_ssp_ids,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_PM
 		.pm	= &mxs_mmc_pm_ops,
 #endif
diff --git a/drivers/mmc/host/omap.c b/drivers/mmc/host/omap.c
index 81974ecdfcbc..68dd6c79c378 100644
--- a/drivers/mmc/host/omap.c
+++ b/drivers/mmc/host/omap.c
@@ -1494,7 +1494,6 @@ static struct platform_driver mmc_omap_driver = {
 	.remove		= mmc_omap_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(mmc_omap_match),
 	},
 };
diff --git a/drivers/mmc/host/omap_hsmmc.c b/drivers/mmc/host/omap_hsmmc.c
index 965672663ef0..df27bb4fc098 100644
--- a/drivers/mmc/host/omap_hsmmc.c
+++ b/drivers/mmc/host/omap_hsmmc.c
@@ -1829,7 +1829,17 @@ static int omap_hsmmc_disable_fclk(struct mmc_host *mmc)
 	return 0;
 }
 
-static const struct mmc_host_ops omap_hsmmc_ops = {
+static int omap_hsmmc_multi_io_quirk(struct mmc_card *card,
+				     unsigned int direction, int blk_size)
+{
+	/* This controller can't do multiblock reads due to hw bugs */
+	if (direction == MMC_DATA_READ)
+		return 1;
+
+	return blk_size;
+}
+
+static struct mmc_host_ops omap_hsmmc_ops = {
 	.enable = omap_hsmmc_enable_fclk,
 	.disable = omap_hsmmc_disable_fclk,
 	.post_req = omap_hsmmc_post_req,
@@ -2101,7 +2111,7 @@ static int omap_hsmmc_probe(struct platform_device *pdev)
 
 	if (host->pdata->controller_flags & OMAP_HSMMC_BROKEN_MULTIBLOCK_READ) {
 		dev_info(&pdev->dev, "multiblock reads disabled due to 35xx erratum 2.1.1.128; MMC read performance may suffer\n");
-		mmc->caps2 |= MMC_CAP2_NO_MULTI_READ;
+		omap_hsmmc_ops.multi_io_quirk = omap_hsmmc_multi_io_quirk;
 	}
 
 	pm_runtime_enable(host->dev);
@@ -2489,7 +2499,6 @@ static struct platform_driver omap_hsmmc_driver = {
 	.remove		= omap_hsmmc_remove,
 	.driver		= {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &omap_hsmmc_dev_pm_ops,
 		.of_match_table = of_match_ptr(omap_mmc_of_match),
 	},
diff --git a/drivers/mmc/host/pxamci.c b/drivers/mmc/host/pxamci.c
index 32fe11323f39..1b6d0bfe35f5 100644
--- a/drivers/mmc/host/pxamci.c
+++ b/drivers/mmc/host/pxamci.c
@@ -474,7 +474,7 @@ static void pxamci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		unsigned int clk = rate / ios->clock;
 
 		if (host->clkrt == CLKRT_OFF)
-			clk_enable(host->clk);
+			clk_prepare_enable(host->clk);
 
 		if (ios->clock == 26000000) {
 			/* to support 26MHz */
@@ -501,7 +501,7 @@ static void pxamci_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 		pxamci_stop_clock(host);
 		if (host->clkrt != CLKRT_OFF) {
 			host->clkrt = CLKRT_OFF;
-			clk_disable(host->clk);
+			clk_disable_unprepare(host->clk);
 		}
 	}
 
@@ -885,7 +885,6 @@ static struct platform_driver pxamci_driver = {
 	.remove		= pxamci_remove,
 	.driver		= {
 		.name	= DRIVER_NAME,
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(pxa_mmc_dt_ids),
 	},
 };
diff --git a/drivers/mmc/host/rtsx_pci_sdmmc.c b/drivers/mmc/host/rtsx_pci_sdmmc.c
index dfde4a210238..c70b602f8f1e 100644
--- a/drivers/mmc/host/rtsx_pci_sdmmc.c
+++ b/drivers/mmc/host/rtsx_pci_sdmmc.c
@@ -412,6 +412,13 @@ static void sd_send_cmd_get_rsp(struct realtek_pci_sdmmc *host,
 	}
 
 	if (rsp_type == SD_RSP_TYPE_R2) {
+		/*
+		 * The controller offloads the last byte {CRC-7, end bit 1'b1}
+		 * of response type R2. Assign dummy CRC, 0, and end bit to the
+		 * byte(ptr[16], goes into the LSB of resp[3] later).
+		 */
+		ptr[16] = 1;
+
 		for (i = 0; i < 4; i++) {
 			cmd->resp[i] = get_unaligned_be32(ptr + 1 + i * 4);
 			dev_dbg(sdmmc_dev(host), "cmd->resp[%d] = 0x%08x\n",
@@ -1292,6 +1299,7 @@ static void realtek_init_host(struct realtek_pci_sdmmc *host)
 	mmc->caps = MMC_CAP_4_BIT_DATA | MMC_CAP_SD_HIGHSPEED |
 		MMC_CAP_MMC_HIGHSPEED | MMC_CAP_BUS_WIDTH_TEST |
 		MMC_CAP_UHS_SDR12 | MMC_CAP_UHS_SDR25;
+	mmc->caps2 = MMC_CAP2_NO_PRESCAN_POWERUP | MMC_CAP2_FULL_PWR_CYCLE;
 	mmc->max_current_330 = 400;
 	mmc->max_current_180 = 800;
 	mmc->ops = &realtek_pci_sdmmc_ops;
@@ -1416,7 +1424,6 @@ static struct platform_driver rtsx_pci_sdmmc_driver = {
 	.remove		= rtsx_pci_sdmmc_drv_remove,
 	.id_table       = rtsx_pci_sdmmc_ids,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= DRV_NAME_RTSX_PCI_SDMMC,
 	},
 };
diff --git a/drivers/mmc/host/rtsx_usb_sdmmc.c b/drivers/mmc/host/rtsx_usb_sdmmc.c
index 5d3766e792f0..88af827e086b 100644
--- a/drivers/mmc/host/rtsx_usb_sdmmc.c
+++ b/drivers/mmc/host/rtsx_usb_sdmmc.c
@@ -435,6 +435,13 @@ static void sd_send_cmd_get_rsp(struct rtsx_usb_sdmmc *host,
 	}
 
 	if (rsp_type == SD_RSP_TYPE_R2) {
+		/*
+		 * The controller offloads the last byte {CRC-7, end bit 1'b1}
+		 * of response type R2. Assign dummy CRC, 0, and end bit to the
+		 * byte(ptr[16], goes into the LSB of resp[3] later).
+		 */
+		ptr[16] = 1;
+
 		for (i = 0; i < 4; i++) {
 			cmd->resp[i] = get_unaligned_be32(ptr + 1 + i * 4);
 			dev_dbg(sdmmc_dev(host), "cmd->resp[%d] = 0x%08x\n",
@@ -1329,6 +1336,7 @@ static void rtsx_usb_init_host(struct rtsx_usb_sdmmc *host)
 		MMC_CAP_MMC_HIGHSPEED | MMC_CAP_BUS_WIDTH_TEST |
 		MMC_CAP_UHS_SDR12 | MMC_CAP_UHS_SDR25 | MMC_CAP_UHS_SDR50 |
 		MMC_CAP_NEEDS_POLL;
+	mmc->caps2 = MMC_CAP2_NO_PRESCAN_POWERUP | MMC_CAP2_FULL_PWR_CYCLE;
 
 	mmc->max_current_330 = 400;
 	mmc->max_current_180 = 800;
@@ -1445,7 +1453,6 @@ static struct platform_driver rtsx_usb_sdmmc_driver = {
 	.remove		= rtsx_usb_sdmmc_drv_remove,
 	.id_table       = rtsx_usb_sdmmc_ids,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "rtsx_usb_sdmmc",
 	},
 };
diff --git a/drivers/mmc/host/s3cmci.c b/drivers/mmc/host/s3cmci.c
index e5516a226362..94cddf381ba3 100644
--- a/drivers/mmc/host/s3cmci.c
+++ b/drivers/mmc/host/s3cmci.c
@@ -985,7 +985,8 @@ static int s3cmci_setup_data(struct s3cmci_host *host, struct mmc_data *data)
 		 * one block being transferred. */
 
 		if (data->blocks > 1) {
-			pr_warning("%s: can't do non-word sized block transfers (blksz %d)\n", __func__, data->blksz);
+			pr_warn("%s: can't do non-word sized block transfers (blksz %d)\n",
+				__func__, data->blksz);
 			return -EINVAL;
 		}
 	}
@@ -1874,7 +1875,6 @@ MODULE_DEVICE_TABLE(platform, s3cmci_driver_ids);
 static struct platform_driver s3cmci_driver = {
 	.driver	= {
 		.name	= "s3c-sdi",
-		.owner	= THIS_MODULE,
 	},
 	.id_table	= s3cmci_driver_ids,
 	.probe		= s3cmci_probe,
diff --git a/drivers/mmc/host/sdhci-acpi.c b/drivers/mmc/host/sdhci-acpi.c
index 8c5337002c51..9cccc0e89b04 100644
--- a/drivers/mmc/host/sdhci-acpi.c
+++ b/drivers/mmc/host/sdhci-acpi.c
@@ -67,6 +67,8 @@ struct sdhci_acpi_slot {
 	unsigned int	caps2;
 	mmc_pm_flag_t	pm_caps;
 	unsigned int	flags;
+	int (*probe_slot)(struct platform_device *, const char *, const char *);
+	int (*remove_slot)(struct platform_device *);
 };
 
 struct sdhci_acpi_host {
@@ -122,13 +124,67 @@ static const struct sdhci_acpi_chip sdhci_acpi_chip_int = {
 	.ops = &sdhci_acpi_ops_int,
 };
 
+static int sdhci_acpi_emmc_probe_slot(struct platform_device *pdev,
+				      const char *hid, const char *uid)
+{
+	struct sdhci_acpi_host *c = platform_get_drvdata(pdev);
+	struct sdhci_host *host;
+
+	if (!c || !c->host)
+		return 0;
+
+	host = c->host;
+
+	/* Platform specific code during emmc proble slot goes here */
+
+	if (hid && uid && !strcmp(hid, "80860F14") && !strcmp(uid, "1") &&
+	    sdhci_readl(host, SDHCI_CAPABILITIES) == 0x446cc8b2 &&
+	    sdhci_readl(host, SDHCI_CAPABILITIES_1) == 0x00000807)
+		host->timeout_clk = 1000; /* 1000 kHz i.e. 1 MHz */
+
+	return 0;
+}
+
+static int sdhci_acpi_sdio_probe_slot(struct platform_device *pdev,
+				      const char *hid, const char *uid)
+{
+	struct sdhci_acpi_host *c = platform_get_drvdata(pdev);
+	struct sdhci_host *host;
+
+	if (!c || !c->host)
+		return 0;
+
+	host = c->host;
+
+	/* Platform specific code during emmc proble slot goes here */
+
+	return 0;
+}
+
+static int sdhci_acpi_sd_probe_slot(struct platform_device *pdev,
+				    const char *hid, const char *uid)
+{
+	struct sdhci_acpi_host *c = platform_get_drvdata(pdev);
+	struct sdhci_host *host;
+
+	if (!c || !c->host || !c->slot)
+		return 0;
+
+	host = c->host;
+
+	/* Platform specific code during emmc proble slot goes here */
+
+	return 0;
+}
+
 static const struct sdhci_acpi_slot sdhci_acpi_slot_int_emmc = {
 	.chip    = &sdhci_acpi_chip_int,
 	.caps    = MMC_CAP_8_BIT_DATA | MMC_CAP_NONREMOVABLE |
 		   MMC_CAP_HW_RESET | MMC_CAP_1_8V_DDR,
 	.caps2   = MMC_CAP2_HC_ERASE_SZ,
 	.flags   = SDHCI_ACPI_RUNTIME_PM,
-	.quirks2 = SDHCI_QUIRK2_PRESET_VALUE_BROKEN,
+	.quirks2 = SDHCI_QUIRK2_PRESET_VALUE_BROKEN | SDHCI_QUIRK2_STOP_WITH_TC,
+	.probe_slot	= sdhci_acpi_emmc_probe_slot,
 };
 
 static const struct sdhci_acpi_slot sdhci_acpi_slot_int_sdio = {
@@ -137,12 +193,15 @@ static const struct sdhci_acpi_slot sdhci_acpi_slot_int_sdio = {
 	.caps    = MMC_CAP_NONREMOVABLE | MMC_CAP_POWER_OFF_CARD,
 	.flags   = SDHCI_ACPI_RUNTIME_PM,
 	.pm_caps = MMC_PM_KEEP_POWER,
+	.probe_slot	= sdhci_acpi_sdio_probe_slot,
 };
 
 static const struct sdhci_acpi_slot sdhci_acpi_slot_int_sd = {
 	.flags   = SDHCI_ACPI_SD_CD | SDHCI_ACPI_SD_CD_OVERRIDE_LEVEL |
 		   SDHCI_ACPI_RUNTIME_PM,
-	.quirks2 = SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON,
+	.quirks2 = SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON |
+		   SDHCI_QUIRK2_STOP_WITH_TC,
+	.probe_slot	= sdhci_acpi_sd_probe_slot,
 };
 
 struct sdhci_acpi_uid_slot {
@@ -156,6 +215,7 @@ static const struct sdhci_acpi_uid_slot sdhci_acpi_uids[] = {
 	{ "80860F14" , "3" , &sdhci_acpi_slot_int_sd   },
 	{ "80860F16" , NULL, &sdhci_acpi_slot_int_sd   },
 	{ "INT33BB"  , "2" , &sdhci_acpi_slot_int_sdio },
+	{ "INT33BB"  , "3" , &sdhci_acpi_slot_int_sd },
 	{ "INT33C6"  , NULL, &sdhci_acpi_slot_int_sdio },
 	{ "INT3436"  , NULL, &sdhci_acpi_slot_int_sdio },
 	{ "PNP0D40"  },
@@ -173,8 +233,8 @@ static const struct acpi_device_id sdhci_acpi_ids[] = {
 };
 MODULE_DEVICE_TABLE(acpi, sdhci_acpi_ids);
 
-static const struct sdhci_acpi_slot *sdhci_acpi_get_slot_by_ids(const char *hid,
-								const char *uid)
+static const struct sdhci_acpi_slot *sdhci_acpi_get_slot(const char *hid,
+							 const char *uid)
 {
 	const struct sdhci_acpi_uid_slot *u;
 
@@ -189,24 +249,6 @@ static const struct sdhci_acpi_slot *sdhci_acpi_get_slot_by_ids(const char *hid,
 	return NULL;
 }
 
-static const struct sdhci_acpi_slot *sdhci_acpi_get_slot(acpi_handle handle,
-							 const char *hid)
-{
-	const struct sdhci_acpi_slot *slot;
-	struct acpi_device_info *info;
-	const char *uid = NULL;
-	acpi_status status;
-
-	status = acpi_get_object_info(handle, &info);
-	if (!ACPI_FAILURE(status) && (info->valid & ACPI_VALID_UID))
-		uid = info->unique_id.string;
-
-	slot = sdhci_acpi_get_slot_by_ids(hid, uid);
-
-	kfree(info);
-	return slot;
-}
-
 static int sdhci_acpi_probe(struct platform_device *pdev)
 {
 	struct device *dev = &pdev->dev;
@@ -217,6 +259,7 @@ static int sdhci_acpi_probe(struct platform_device *pdev)
 	struct resource *iomem;
 	resource_size_t len;
 	const char *hid;
+	const char *uid;
 	int err;
 
 	if (acpi_bus_get_device(handle, &device))
@@ -226,6 +269,7 @@ static int sdhci_acpi_probe(struct platform_device *pdev)
 		return -ENODEV;
 
 	hid = acpi_device_hid(device);
+	uid = device->pnp.unique_id;
 
 	iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (!iomem)
@@ -244,7 +288,7 @@ static int sdhci_acpi_probe(struct platform_device *pdev)
 
 	c = sdhci_priv(host);
 	c->host = host;
-	c->slot = sdhci_acpi_get_slot(handle, hid);
+	c->slot = sdhci_acpi_get_slot(hid, uid);
 	c->pdev = pdev;
 	c->use_runtime_pm = sdhci_acpi_flag(c, SDHCI_ACPI_RUNTIME_PM);
 
@@ -277,6 +321,11 @@ static int sdhci_acpi_probe(struct platform_device *pdev)
 	}
 
 	if (c->slot) {
+		if (c->slot->probe_slot) {
+			err = c->slot->probe_slot(pdev, hid, uid);
+			if (err)
+				goto err_free;
+		}
 		if (c->slot->chip) {
 			host->ops            = c->slot->chip->ops;
 			host->quirks        |= c->slot->chip->quirks;
@@ -297,7 +346,7 @@ static int sdhci_acpi_probe(struct platform_device *pdev)
 	if (sdhci_acpi_flag(c, SDHCI_ACPI_SD_CD)) {
 		bool v = sdhci_acpi_flag(c, SDHCI_ACPI_SD_CD_OVERRIDE_LEVEL);
 
-		if (mmc_gpiod_request_cd(host->mmc, NULL, 0, v, 0)) {
+		if (mmc_gpiod_request_cd(host->mmc, NULL, 0, v, 0, NULL)) {
 			dev_warn(dev, "failed to setup card detect gpio\n");
 			c->use_runtime_pm = false;
 		}
@@ -334,6 +383,9 @@ static int sdhci_acpi_remove(struct platform_device *pdev)
 		pm_runtime_put_noidle(dev);
 	}
 
+	if (c->slot && c->slot->remove_slot)
+		c->slot->remove_slot(pdev);
+
 	dead = (sdhci_readl(c->host, SDHCI_INT_STATUS) == ~0);
 	sdhci_remove_host(c->host, dead);
 	sdhci_free_host(c->host);
@@ -385,20 +437,13 @@ static int sdhci_acpi_runtime_idle(struct device *dev)
 	return 0;
 }
 
-#else
-
-#define sdhci_acpi_runtime_suspend	NULL
-#define sdhci_acpi_runtime_resume	NULL
-#define sdhci_acpi_runtime_idle		NULL
-
 #endif
 
 static const struct dev_pm_ops sdhci_acpi_pm_ops = {
 	.suspend		= sdhci_acpi_suspend,
 	.resume			= sdhci_acpi_resume,
-	.runtime_suspend	= sdhci_acpi_runtime_suspend,
-	.runtime_resume		= sdhci_acpi_runtime_resume,
-	.runtime_idle		= sdhci_acpi_runtime_idle,
+	SET_RUNTIME_PM_OPS(sdhci_acpi_runtime_suspend,
+			sdhci_acpi_runtime_resume, sdhci_acpi_runtime_idle)
 };
 
 static struct platform_driver sdhci_acpi_driver = {
diff --git a/drivers/mmc/host/sdhci-bcm-kona.c b/drivers/mmc/host/sdhci-bcm-kona.c
index dd780c315a63..e7e4fbdcbfe0 100644
--- a/drivers/mmc/host/sdhci-bcm-kona.c
+++ b/drivers/mmc/host/sdhci-bcm-kona.c
@@ -225,7 +225,7 @@ static struct sdhci_pltfm_data sdhci_pltfm_data_kona = {
 		SDHCI_QUIRK_CAP_CLOCK_BASE_BROKEN,
 };
 
-static struct __initconst of_device_id sdhci_bcm_kona_of_match[] = {
+static const struct of_device_id sdhci_bcm_kona_of_match[] = {
 	{ .compatible = "brcm,kona-sdhci"},
 	{ .compatible = "bcm,kona-sdhci"}, /* deprecated name */
 	{}
@@ -359,7 +359,6 @@ static int sdhci_bcm_kona_remove(struct platform_device *pdev)
 static struct platform_driver sdhci_bcm_kona_driver = {
 	.driver		= {
 		.name	= "sdhci-kona",
-		.owner	= THIS_MODULE,
 		.pm	= SDHCI_PLTFM_PMOPS,
 		.of_match_table = sdhci_bcm_kona_of_match,
 	},
diff --git a/drivers/mmc/host/sdhci-bcm2835.c b/drivers/mmc/host/sdhci-bcm2835.c
index 46af9a439d7b..439d259fdf1d 100644
--- a/drivers/mmc/host/sdhci-bcm2835.c
+++ b/drivers/mmc/host/sdhci-bcm2835.c
@@ -194,7 +194,6 @@ MODULE_DEVICE_TABLE(of, bcm2835_sdhci_of_match);
 static struct platform_driver bcm2835_sdhci_driver = {
 	.driver = {
 		.name = "sdhci-bcm2835",
-		.owner = THIS_MODULE,
 		.of_match_table = bcm2835_sdhci_of_match,
 		.pm = SDHCI_PLTFM_PMOPS,
 	},
diff --git a/drivers/mmc/host/sdhci-cns3xxx.c b/drivers/mmc/host/sdhci-cns3xxx.c
index 14b74075589a..a7935a8d0922 100644
--- a/drivers/mmc/host/sdhci-cns3xxx.c
+++ b/drivers/mmc/host/sdhci-cns3xxx.c
@@ -106,7 +106,6 @@ static int sdhci_cns3xxx_remove(struct platform_device *pdev)
 static struct platform_driver sdhci_cns3xxx_driver = {
 	.driver		= {
 		.name	= "sdhci-cns3xxx",
-		.owner	= THIS_MODULE,
 		.pm	= SDHCI_PLTFM_PMOPS,
 	},
 	.probe		= sdhci_cns3xxx_probe,
diff --git a/drivers/mmc/host/sdhci-dove.c b/drivers/mmc/host/sdhci-dove.c
index e6278ec007d7..ca969d271a27 100644
--- a/drivers/mmc/host/sdhci-dove.c
+++ b/drivers/mmc/host/sdhci-dove.c
@@ -146,7 +146,6 @@ MODULE_DEVICE_TABLE(of, sdhci_dove_of_match_table);
 static struct platform_driver sdhci_dove_driver = {
 	.driver		= {
 		.name	= "sdhci-dove",
-		.owner	= THIS_MODULE,
 		.pm	= SDHCI_PLTFM_PMOPS,
 		.of_match_table = sdhci_dove_of_match_table,
 	},
diff --git a/drivers/mmc/host/sdhci-esdhc-imx.c b/drivers/mmc/host/sdhci-esdhc-imx.c
index ccec0e32590f..587ee0edeb57 100644
--- a/drivers/mmc/host/sdhci-esdhc-imx.c
+++ b/drivers/mmc/host/sdhci-esdhc-imx.c
@@ -880,6 +880,24 @@ static void esdhc_reset(struct sdhci_host *host, u8 mask)
 	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 }
 
+static unsigned int esdhc_get_max_timeout_count(struct sdhci_host *host)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct pltfm_imx_data *imx_data = pltfm_host->priv;
+
+	return esdhc_is_usdhc(imx_data) ? 1 << 28 : 1 << 27;
+}
+
+static void esdhc_set_timeout(struct sdhci_host *host, struct mmc_command *cmd)
+{
+	struct sdhci_pltfm_host *pltfm_host = sdhci_priv(host);
+	struct pltfm_imx_data *imx_data = pltfm_host->priv;
+
+	/* use maximum timeout counter */
+	sdhci_writeb(host, esdhc_is_usdhc(imx_data) ? 0xF : 0xE,
+			SDHCI_TIMEOUT_CONTROL);
+}
+
 static struct sdhci_ops sdhci_esdhc_ops = {
 	.read_l = esdhc_readl_le,
 	.read_w = esdhc_readw_le,
@@ -889,7 +907,9 @@ static struct sdhci_ops sdhci_esdhc_ops = {
 	.set_clock = esdhc_pltfm_set_clock,
 	.get_max_clock = esdhc_pltfm_get_max_clock,
 	.get_min_clock = esdhc_pltfm_get_min_clock,
+	.get_max_timeout_count = esdhc_get_max_timeout_count,
 	.get_ro = esdhc_pltfm_get_ro,
+	.set_timeout = esdhc_set_timeout,
 	.set_bus_width = esdhc_pltfm_set_bus_width,
 	.set_uhs_signaling = esdhc_set_uhs_signaling,
 	.reset = esdhc_reset,
@@ -1207,7 +1227,6 @@ static const struct dev_pm_ops sdhci_esdhc_pmops = {
 static struct platform_driver sdhci_esdhc_imx_driver = {
 	.driver		= {
 		.name	= "sdhci-esdhc-imx",
-		.owner	= THIS_MODULE,
 		.of_match_table = imx_esdhc_dt_ids,
 		.pm	= &sdhci_esdhc_pmops,
 	},
diff --git a/drivers/mmc/host/sdhci-msm.c b/drivers/mmc/host/sdhci-msm.c
index 1a6661ed6205..30804385af6d 100644
--- a/drivers/mmc/host/sdhci-msm.c
+++ b/drivers/mmc/host/sdhci-msm.c
@@ -46,24 +46,6 @@
 #define CMUX_SHIFT_PHASE_SHIFT	24
 #define CMUX_SHIFT_PHASE_MASK	(7 << CMUX_SHIFT_PHASE_SHIFT)
 
-static const u32 tuning_block_64[] = {
-	0x00ff0fff, 0xccc3ccff, 0xffcc3cc3, 0xeffefffe,
-	0xddffdfff, 0xfbfffbff, 0xff7fffbf, 0xefbdf777,
-	0xf0fff0ff, 0x3cccfc0f, 0xcfcc33cc, 0xeeffefff,
-	0xfdfffdff, 0xffbfffdf, 0xfff7ffbb, 0xde7b7ff7
-};
-
-static const u32 tuning_block_128[] = {
-	0xff00ffff, 0x0000ffff, 0xccccffff, 0xcccc33cc,
-	0xcc3333cc, 0xffffcccc, 0xffffeeff, 0xffeeeeff,
-	0xffddffff, 0xddddffff, 0xbbffffff, 0xbbffffff,
-	0xffffffbb, 0xffffff77, 0x77ff7777, 0xffeeddbb,
-	0x00ffffff, 0x00ffffff, 0xccffff00, 0xcc33cccc,
-	0x3333cccc, 0xffcccccc, 0xffeeffff, 0xeeeeffff,
-	0xddffffff, 0xddffffff, 0xffffffdd, 0xffffffbb,
-	0xffffbbbb, 0xffff77ff, 0xff7777ff, 0xeeddbb77
-};
-
 struct sdhci_msm_host {
 	struct platform_device *pdev;
 	void __iomem *core_mem;	/* MSM SDCC mapped address */
@@ -358,8 +340,8 @@ static int sdhci_msm_execute_tuning(struct sdhci_host *host, u32 opcode)
 {
 	int tuning_seq_cnt = 3;
 	u8 phase, *data_buf, tuned_phases[16], tuned_phase_cnt = 0;
-	const u32 *tuning_block_pattern = tuning_block_64;
-	int size = sizeof(tuning_block_64);	/* Pattern size in bytes */
+	const u8 *tuning_block_pattern = tuning_blk_pattern_4bit;
+	int size = sizeof(tuning_blk_pattern_4bit);
 	int rc;
 	struct mmc_host *mmc = host->mmc;
 	struct mmc_ios ios = host->mmc->ios;
@@ -375,8 +357,8 @@ static int sdhci_msm_execute_tuning(struct sdhci_host *host, u32 opcode)
 
 	if ((opcode == MMC_SEND_TUNING_BLOCK_HS200) &&
 	    (mmc->ios.bus_width == MMC_BUS_WIDTH_8)) {
-		tuning_block_pattern = tuning_block_128;
-		size = sizeof(tuning_block_128);
+		tuning_block_pattern = tuning_blk_pattern_8bit;
+		size = sizeof(tuning_blk_pattern_8bit);
 	}
 
 	data_buf = kmalloc(size, GFP_KERNEL);
@@ -610,7 +592,6 @@ static struct platform_driver sdhci_msm_driver = {
 	.remove = sdhci_msm_remove,
 	.driver = {
 		   .name = "sdhci_msm",
-		   .owner = THIS_MODULE,
 		   .of_match_table = sdhci_msm_dt_match,
 	},
 };
diff --git a/drivers/mmc/host/sdhci-of-arasan.c b/drivers/mmc/host/sdhci-of-arasan.c
index 5bd1092310f2..981d66e5c023 100644
--- a/drivers/mmc/host/sdhci-of-arasan.c
+++ b/drivers/mmc/host/sdhci-of-arasan.c
@@ -213,7 +213,6 @@ MODULE_DEVICE_TABLE(of, sdhci_arasan_of_match);
 static struct platform_driver sdhci_arasan_driver = {
 	.driver = {
 		.name = "sdhci-arasan",
-		.owner = THIS_MODULE,
 		.of_match_table = sdhci_arasan_of_match,
 		.pm = &sdhci_arasan_dev_pm_ops,
 	},
diff --git a/drivers/mmc/host/sdhci-of-esdhc.c b/drivers/mmc/host/sdhci-of-esdhc.c
index 8be4dcfb49a0..8872c85c63d4 100644
--- a/drivers/mmc/host/sdhci-of-esdhc.c
+++ b/drivers/mmc/host/sdhci-of-esdhc.c
@@ -388,7 +388,6 @@ MODULE_DEVICE_TABLE(of, sdhci_esdhc_of_match);
 static struct platform_driver sdhci_esdhc_driver = {
 	.driver = {
 		.name = "sdhci-esdhc",
-		.owner = THIS_MODULE,
 		.of_match_table = sdhci_esdhc_of_match,
 		.pm = ESDHC_PMOPS,
 	},
diff --git a/drivers/mmc/host/sdhci-of-hlwd.c b/drivers/mmc/host/sdhci-of-hlwd.c
index b341661369a2..be479279a1d5 100644
--- a/drivers/mmc/host/sdhci-of-hlwd.c
+++ b/drivers/mmc/host/sdhci-of-hlwd.c
@@ -89,7 +89,6 @@ MODULE_DEVICE_TABLE(of, sdhci_hlwd_of_match);
 static struct platform_driver sdhci_hlwd_driver = {
 	.driver = {
 		.name = "sdhci-hlwd",
-		.owner = THIS_MODULE,
 		.of_match_table = sdhci_hlwd_of_match,
 		.pm = SDHCI_PLTFM_PMOPS,
 	},
diff --git a/drivers/mmc/host/sdhci-pci.c b/drivers/mmc/host/sdhci-pci.c
index c3a1debc9289..61192973e7cb 100644
--- a/drivers/mmc/host/sdhci-pci.c
+++ b/drivers/mmc/host/sdhci-pci.c
@@ -24,6 +24,7 @@
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/pm_runtime.h>
+#include <linux/mmc/slot-gpio.h>
 #include <linux/mmc/sdhci-pci-data.h>
 
 #include "sdhci.h"
@@ -271,6 +272,8 @@ static int byt_emmc_probe_slot(struct sdhci_pci_slot *slot)
 				 MMC_CAP_HW_RESET | MMC_CAP_1_8V_DDR;
 	slot->host->mmc->caps2 |= MMC_CAP2_HC_ERASE_SZ;
 	slot->hw_reset = sdhci_pci_int_hw_reset;
+	if (slot->chip->pdev->device == PCI_DEVICE_ID_INTEL_BSW_EMMC)
+		slot->host->timeout_clk = 1000; /* 1000 kHz i.e. 1 MHz */
 	return 0;
 }
 
@@ -280,22 +283,35 @@ static int byt_sdio_probe_slot(struct sdhci_pci_slot *slot)
 	return 0;
 }
 
+static int byt_sd_probe_slot(struct sdhci_pci_slot *slot)
+{
+	slot->cd_con_id = NULL;
+	slot->cd_idx = 0;
+	slot->cd_override_level = true;
+	return 0;
+}
+
 static const struct sdhci_pci_fixes sdhci_intel_byt_emmc = {
 	.allow_runtime_pm = true,
 	.probe_slot	= byt_emmc_probe_slot,
-	.quirks2	= SDHCI_QUIRK2_PRESET_VALUE_BROKEN,
+	.quirks2	= SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
+			  SDHCI_QUIRK2_STOP_WITH_TC,
 };
 
 static const struct sdhci_pci_fixes sdhci_intel_byt_sdio = {
-	.quirks2	= SDHCI_QUIRK2_HOST_OFF_CARD_ON,
+	.quirks2	= SDHCI_QUIRK2_HOST_OFF_CARD_ON |
+			SDHCI_QUIRK2_PRESET_VALUE_BROKEN,
 	.allow_runtime_pm = true,
 	.probe_slot	= byt_sdio_probe_slot,
 };
 
 static const struct sdhci_pci_fixes sdhci_intel_byt_sd = {
-	.quirks2	= SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON,
+	.quirks2	= SDHCI_QUIRK2_CARD_ON_NEEDS_BUS_ON |
+			  SDHCI_QUIRK2_PRESET_VALUE_BROKEN |
+			  SDHCI_QUIRK2_STOP_WITH_TC,
 	.allow_runtime_pm = true,
 	.own_cd_for_runtime_pm = true,
+	.probe_slot	= byt_sd_probe_slot,
 };
 
 /* Define Host controllers for Intel Merrifield platform */
@@ -317,7 +333,9 @@ static int intel_mrfl_mmc_probe_slot(struct sdhci_pci_slot *slot)
 
 static const struct sdhci_pci_fixes sdhci_intel_mrfl_mmc = {
 	.quirks		= SDHCI_QUIRK_NO_ENDATTR_IN_NOPDESC,
-	.quirks2	= SDHCI_QUIRK2_BROKEN_HS200,
+	.quirks2	= SDHCI_QUIRK2_BROKEN_HS200 |
+			SDHCI_QUIRK2_PRESET_VALUE_BROKEN,
+	.allow_runtime_pm = true,
 	.probe_slot	= intel_mrfl_mmc_probe_slot,
 };
 
@@ -876,6 +894,29 @@ static const struct pci_device_id pci_ids[] = {
 		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
 	},
 
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_BSW_EMMC,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_emmc,
+	},
+
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_BSW_SDIO,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sdio,
+	},
+
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_BSW_SD,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.driver_data	= (kernel_ulong_t)&sdhci_intel_byt_sd,
+	},
 
 	{
 		.vendor		= PCI_VENDOR_ID_INTEL,
@@ -1269,20 +1310,13 @@ static int sdhci_pci_runtime_idle(struct device *dev)
 	return 0;
 }
 
-#else
-
-#define sdhci_pci_runtime_suspend	NULL
-#define sdhci_pci_runtime_resume	NULL
-#define sdhci_pci_runtime_idle		NULL
-
 #endif
 
 static const struct dev_pm_ops sdhci_pci_pm_ops = {
 	.suspend = sdhci_pci_suspend,
 	.resume = sdhci_pci_resume,
-	.runtime_suspend = sdhci_pci_runtime_suspend,
-	.runtime_resume = sdhci_pci_runtime_resume,
-	.runtime_idle = sdhci_pci_runtime_idle,
+	SET_RUNTIME_PM_OPS(sdhci_pci_runtime_suspend,
+			sdhci_pci_runtime_resume, sdhci_pci_runtime_idle)
 };
 
 /*****************************************************************************\
@@ -1332,6 +1366,7 @@ static struct sdhci_pci_slot *sdhci_pci_probe_slot(
 	slot->pci_bar = bar;
 	slot->rst_n_gpio = -EINVAL;
 	slot->cd_gpio = -EINVAL;
+	slot->cd_idx = -1;
 
 	/* Retrieve platform data if there is any */
 	if (*sdhci_pci_get_data)
@@ -1390,6 +1425,13 @@ static struct sdhci_pci_slot *sdhci_pci_probe_slot(
 	host->mmc->slotno = slotno;
 	host->mmc->caps2 |= MMC_CAP2_NO_PRESCAN_POWERUP;
 
+	if (slot->cd_idx >= 0 &&
+	    mmc_gpiod_request_cd(host->mmc, slot->cd_con_id, slot->cd_idx,
+				 slot->cd_override_level, 0, NULL)) {
+		dev_warn(&pdev->dev, "failed to setup card detect gpio\n");
+		slot->cd_idx = -1;
+	}
+
 	ret = sdhci_add_host(host);
 	if (ret)
 		goto remove;
@@ -1402,7 +1444,7 @@ static struct sdhci_pci_slot *sdhci_pci_probe_slot(
 	 * Note sdhci_pci_add_own_cd() sets slot->cd_gpio to -EINVAL on failure.
 	 */
 	if (chip->fixes && chip->fixes->own_cd_for_runtime_pm &&
-	    !gpio_is_valid(slot->cd_gpio))
+	    !gpio_is_valid(slot->cd_gpio) && slot->cd_idx < 0)
 		chip->allow_runtime_pm = false;
 
 	return slot;
diff --git a/drivers/mmc/host/sdhci-pci.h b/drivers/mmc/host/sdhci-pci.h
index c101477ef3be..d57c3d169914 100644
--- a/drivers/mmc/host/sdhci-pci.h
+++ b/drivers/mmc/host/sdhci-pci.h
@@ -11,6 +11,9 @@
 #define PCI_DEVICE_ID_INTEL_BYT_SDIO	0x0f15
 #define PCI_DEVICE_ID_INTEL_BYT_SD	0x0f16
 #define PCI_DEVICE_ID_INTEL_BYT_EMMC2	0x0f50
+#define PCI_DEVICE_ID_INTEL_BSW_EMMC	0x2294
+#define PCI_DEVICE_ID_INTEL_BSW_SDIO	0x2295
+#define PCI_DEVICE_ID_INTEL_BSW_SD	0x2296
 #define PCI_DEVICE_ID_INTEL_MRFL_MMC	0x1190
 #define PCI_DEVICE_ID_INTEL_CLV_SDIO0	0x08f9
 #define PCI_DEVICE_ID_INTEL_CLV_SDIO1	0x08fa
@@ -61,6 +64,10 @@ struct sdhci_pci_slot {
 	int			cd_gpio;
 	int			cd_irq;
 
+	char			*cd_con_id;
+	int			cd_idx;
+	bool			cd_override_level;
+
 	void (*hw_reset)(struct sdhci_host *host);
 };
 
diff --git a/drivers/mmc/host/sdhci-pltfm.c b/drivers/mmc/host/sdhci-pltfm.c
index 7e834fb78f42..c5b01d6bb85d 100644
--- a/drivers/mmc/host/sdhci-pltfm.c
+++ b/drivers/mmc/host/sdhci-pltfm.c
@@ -123,7 +123,6 @@ struct sdhci_host *sdhci_pltfm_init(struct platform_device *pdev,
 				    size_t priv_size)
 {
 	struct sdhci_host *host;
-	struct device_node *np = pdev->dev.of_node;
 	struct resource *iomem;
 	int ret;
 
@@ -136,13 +135,8 @@ struct sdhci_host *sdhci_pltfm_init(struct platform_device *pdev,
 	if (resource_size(iomem) < 0x100)
 		dev_err(&pdev->dev, "Invalid iomem size!\n");
 
-	/* Some PCI-based MFD need the parent here */
-	if (pdev->dev.parent != &platform_bus && !np)
-		host = sdhci_alloc_host(pdev->dev.parent,
-			sizeof(struct sdhci_pltfm_host) + priv_size);
-	else
-		host = sdhci_alloc_host(&pdev->dev,
-			sizeof(struct sdhci_pltfm_host) + priv_size);
+	host = sdhci_alloc_host(&pdev->dev,
+		sizeof(struct sdhci_pltfm_host) + priv_size);
 
 	if (IS_ERR(host)) {
 		ret = PTR_ERR(host);
diff --git a/drivers/mmc/host/sdhci-pxav2.c b/drivers/mmc/host/sdhci-pxav2.c
index 3c0f3c0a1cc8..b4c23e983baf 100644
--- a/drivers/mmc/host/sdhci-pxav2.c
+++ b/drivers/mmc/host/sdhci-pxav2.c
@@ -261,7 +261,6 @@ static int sdhci_pxav2_remove(struct platform_device *pdev)
 static struct platform_driver sdhci_pxav2_driver = {
 	.driver		= {
 		.name	= "sdhci-pxav2",
-		.owner	= THIS_MODULE,
 #ifdef CONFIG_OF
 		.of_match_table = sdhci_pxav2_of_match,
 #endif
diff --git a/drivers/mmc/host/sdhci-pxav3.c b/drivers/mmc/host/sdhci-pxav3.c
index 6f842fb8e6b8..5036d7d39529 100644
--- a/drivers/mmc/host/sdhci-pxav3.c
+++ b/drivers/mmc/host/sdhci-pxav3.c
@@ -224,12 +224,11 @@ static void pxav3_set_uhs_signaling(struct sdhci_host *host, unsigned int uhs)
 
 static const struct sdhci_ops pxav3_sdhci_ops = {
 	.set_clock = sdhci_set_clock,
-	.set_uhs_signaling = pxav3_set_uhs_signaling,
 	.platform_send_init_74_clocks = pxav3_gen_init_74_clocks,
 	.get_max_clock = sdhci_pltfm_clk_get_max_clock,
 	.set_bus_width = sdhci_set_bus_width,
 	.reset = pxav3_reset,
-	.set_uhs_signaling = sdhci_set_uhs_signaling,
+	.set_uhs_signaling = pxav3_set_uhs_signaling,
 };
 
 static struct sdhci_pltfm_data sdhci_pxav3_pdata = {
@@ -381,11 +380,11 @@ static int sdhci_pxav3_probe(struct platform_device *pdev)
 
 	return 0;
 
-err_of_parse:
-err_cd_req:
 err_add_host:
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
+err_of_parse:
+err_cd_req:
 	clk_disable_unprepare(clk);
 err_clk_get:
 err_mbus_win:
@@ -492,7 +491,6 @@ static struct platform_driver sdhci_pxav3_driver = {
 #ifdef CONFIG_OF
 		.of_match_table = sdhci_pxav3_of_match,
 #endif
-		.owner	= THIS_MODULE,
 		.pm	= SDHCI_PXAV3_PMOPS,
 	},
 	.probe		= sdhci_pxav3_probe,
diff --git a/drivers/mmc/host/sdhci-s3c.c b/drivers/mmc/host/sdhci-s3c.c
index fa5954a05449..0ce6eb17deaf 100644
--- a/drivers/mmc/host/sdhci-s3c.c
+++ b/drivers/mmc/host/sdhci-s3c.c
@@ -606,8 +606,6 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 	ret = sdhci_add_host(host);
 	if (ret) {
 		dev_err(dev, "sdhci_add_host() failed\n");
-		pm_runtime_forbid(&pdev->dev);
-		pm_runtime_get_noresume(&pdev->dev);
 		goto err_req_regs;
 	}
 
@@ -618,6 +616,8 @@ static int sdhci_s3c_probe(struct platform_device *pdev)
 	return 0;
 
  err_req_regs:
+	pm_runtime_disable(&pdev->dev);
+
  err_no_busclks:
 	clk_disable_unprepare(sc->clk_io);
 
@@ -747,7 +747,6 @@ static struct platform_driver sdhci_s3c_driver = {
 	.remove		= sdhci_s3c_remove,
 	.id_table	= sdhci_s3c_driver_ids,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "s3c-sdhci",
 		.of_match_table = of_match_ptr(sdhci_s3c_dt_match),
 		.pm	= SDHCI_S3C_PMOPS,
diff --git a/drivers/mmc/host/sdhci-sirf.c b/drivers/mmc/host/sdhci-sirf.c
index 17004531d089..dd29d47c07aa 100644
--- a/drivers/mmc/host/sdhci-sirf.c
+++ b/drivers/mmc/host/sdhci-sirf.c
@@ -15,6 +15,8 @@
 #include <linux/mmc/slot-gpio.h>
 #include "sdhci-pltfm.h"
 
+#define SDHCI_SIRF_8BITBUS BIT(3)
+
 struct sdhci_sirf_priv {
 	struct clk *clk;
 	int gpio_cd;
@@ -27,10 +29,30 @@ static unsigned int sdhci_sirf_get_max_clk(struct sdhci_host *host)
 	return clk_get_rate(priv->clk);
 }
 
+static void sdhci_sirf_set_bus_width(struct sdhci_host *host, int width)
+{
+	u8 ctrl;
+
+	ctrl = sdhci_readb(host, SDHCI_HOST_CONTROL);
+	ctrl &= ~(SDHCI_CTRL_4BITBUS | SDHCI_SIRF_8BITBUS);
+
+	/*
+	 * CSR atlas7 and prima2 SD host version is not 3.0
+	 * 8bit-width enable bit of CSR SD hosts is 3,
+	 * while stardard hosts use bit 5
+	 */
+	if (width == MMC_BUS_WIDTH_8)
+		ctrl |= SDHCI_SIRF_8BITBUS;
+	else if (width == MMC_BUS_WIDTH_4)
+		ctrl |= SDHCI_CTRL_4BITBUS;
+
+	sdhci_writeb(host, ctrl, SDHCI_HOST_CONTROL);
+}
+
 static struct sdhci_ops sdhci_sirf_ops = {
 	.set_clock = sdhci_set_clock,
 	.get_max_clock	= sdhci_sirf_get_max_clk,
-	.set_bus_width = sdhci_set_bus_width,
+	.set_bus_width = sdhci_sirf_set_bus_width,
 	.reset = sdhci_reset,
 	.set_uhs_signaling = sdhci_set_uhs_signaling,
 };
@@ -94,6 +116,7 @@ static int sdhci_sirf_probe(struct platform_device *pdev)
 				ret);
 			goto err_request_cd;
 		}
+		mmc_gpiod_request_cd_irq(host->mmc);
 	}
 
 	return 0;
@@ -167,7 +190,6 @@ MODULE_DEVICE_TABLE(of, sdhci_sirf_of_match);
 static struct platform_driver sdhci_sirf_driver = {
 	.driver		= {
 		.name	= "sdhci-sirf",
-		.owner	= THIS_MODULE,
 		.of_match_table = sdhci_sirf_of_match,
 #ifdef CONFIG_PM_SLEEP
 		.pm	= &sdhci_sirf_pm_ops,
diff --git a/drivers/mmc/host/sdhci-spear.c b/drivers/mmc/host/sdhci-spear.c
index 9d535c7336ef..22e58268545f 100644
--- a/drivers/mmc/host/sdhci-spear.c
+++ b/drivers/mmc/host/sdhci-spear.c
@@ -230,7 +230,6 @@ MODULE_DEVICE_TABLE(of, sdhci_spear_id_table);
 static struct platform_driver sdhci_driver = {
 	.driver = {
 		.name	= "sdhci",
-		.owner	= THIS_MODULE,
 		.pm	= &sdhci_pm_ops,
 		.of_match_table = of_match_ptr(sdhci_spear_id_table),
 	},
diff --git a/drivers/mmc/host/sdhci-tegra.c b/drivers/mmc/host/sdhci-tegra.c
index 33100d10d176..59797106af93 100644
--- a/drivers/mmc/host/sdhci-tegra.c
+++ b/drivers/mmc/host/sdhci-tegra.c
@@ -318,7 +318,6 @@ static int sdhci_tegra_remove(struct platform_device *pdev)
 static struct platform_driver sdhci_tegra_driver = {
 	.driver		= {
 		.name	= "sdhci-tegra",
-		.owner	= THIS_MODULE,
 		.of_match_table = sdhci_tegra_dt_match,
 		.pm	= SDHCI_PLTFM_PMOPS,
 	},
diff --git a/drivers/mmc/host/sdhci.c b/drivers/mmc/host/sdhci.c
index 37b2a9ae52ef..ada1a3ea3a87 100644
--- a/drivers/mmc/host/sdhci.c
+++ b/drivers/mmc/host/sdhci.c
@@ -707,19 +707,28 @@ static void sdhci_set_transfer_irqs(struct sdhci_host *host)
 	sdhci_writel(host, host->ier, SDHCI_SIGNAL_ENABLE);
 }
 
-static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_command *cmd)
+static void sdhci_set_timeout(struct sdhci_host *host, struct mmc_command *cmd)
 {
 	u8 count;
+
+	if (host->ops->set_timeout) {
+		host->ops->set_timeout(host, cmd);
+	} else {
+		count = sdhci_calc_timeout(host, cmd);
+		sdhci_writeb(host, count, SDHCI_TIMEOUT_CONTROL);
+	}
+}
+
+static void sdhci_prepare_data(struct sdhci_host *host, struct mmc_command *cmd)
+{
 	u8 ctrl;
 	struct mmc_data *data = cmd->data;
 	int ret;
 
 	WARN_ON(host->data);
 
-	if (data || (cmd->flags & MMC_RSP_BUSY)) {
-		count = sdhci_calc_timeout(host, cmd);
-		sdhci_writeb(host, count, SDHCI_TIMEOUT_CONTROL);
-	}
+	if (data || (cmd->flags & MMC_RSP_BUSY))
+		sdhci_set_timeout(host, cmd);
 
 	if (!data)
 		return;
@@ -1007,6 +1016,7 @@ void sdhci_send_command(struct sdhci_host *host, struct mmc_command *cmd)
 	mod_timer(&host->timer, timeout);
 
 	host->cmd = cmd;
+	host->busy_handle = 0;
 
 	sdhci_prepare_data(host, cmd);
 
@@ -1194,7 +1204,6 @@ void sdhci_set_clock(struct sdhci_host *host, unsigned int clock)
 clock_set:
 	if (real_div)
 		host->mmc->actual_clock = (host->max_clk * clk_mul) / real_div;
-
 	clk |= (div & SDHCI_DIV_MASK) << SDHCI_DIVIDER_SHIFT;
 	clk |= ((div & SDHCI_DIV_HI_MASK) >> SDHCI_DIV_MASK_LEN)
 		<< SDHCI_DIVIDER_HI_SHIFT;
@@ -1357,11 +1366,12 @@ static void sdhci_request(struct mmc_host *mmc, struct mmc_request *mrq)
 		present_state = sdhci_readl(host, SDHCI_PRESENT_STATE);
 		/*
 		 * Check if the re-tuning timer has already expired and there
-		 * is no on-going data transfer. If so, we need to execute
-		 * tuning procedure before sending command.
+		 * is no on-going data transfer and DAT0 is not busy. If so,
+		 * we need to execute tuning procedure before sending command.
 		 */
 		if ((host->flags & SDHCI_NEEDS_RETUNING) &&
-		    !(present_state & (SDHCI_DOING_WRITE | SDHCI_DOING_READ))) {
+		    !(present_state & (SDHCI_DOING_WRITE | SDHCI_DOING_READ)) &&
+		    (present_state & SDHCI_DATA_0_LVL_MASK)) {
 			if (mmc->card) {
 				/* eMMC uses cmd21 but sd and sdio use cmd19 */
 				tuning_opcode =
@@ -1471,6 +1481,18 @@ static void sdhci_do_set_ios(struct sdhci_host *host, struct mmc_ios *ios)
 	if (!ios->clock || ios->clock != host->clock) {
 		host->ops->set_clock(host, ios->clock);
 		host->clock = ios->clock;
+
+		if (host->quirks & SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK &&
+		    host->clock) {
+			host->timeout_clk = host->mmc->actual_clock ?
+						host->mmc->actual_clock / 1000 :
+						host->clock / 1000;
+			host->mmc->max_busy_timeout =
+				host->ops->get_max_timeout_count ?
+				host->ops->get_max_timeout_count(host) :
+				1 << 27;
+			host->mmc->max_busy_timeout /= host->timeout_clk;
+		}
 	}
 
 	sdhci_set_power(host, ios->power_mode, ios->vdd);
@@ -1733,8 +1755,8 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 			ret = regulator_set_voltage(mmc->supply.vqmmc, 2700000,
 						    3600000);
 			if (ret) {
-				pr_warning("%s: Switching to 3.3V signalling voltage "
-						" failed\n", mmc_hostname(mmc));
+				pr_warn("%s: Switching to 3.3V signalling voltage failed\n",
+					mmc_hostname(mmc));
 				return -EIO;
 			}
 		}
@@ -1746,8 +1768,8 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 		if (!(ctrl & SDHCI_CTRL_VDD_180))
 			return 0;
 
-		pr_warning("%s: 3.3V regulator output did not became stable\n",
-				mmc_hostname(mmc));
+		pr_warn("%s: 3.3V regulator output did not became stable\n",
+			mmc_hostname(mmc));
 
 		return -EAGAIN;
 	case MMC_SIGNAL_VOLTAGE_180:
@@ -1755,8 +1777,8 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 			ret = regulator_set_voltage(mmc->supply.vqmmc,
 					1700000, 1950000);
 			if (ret) {
-				pr_warning("%s: Switching to 1.8V signalling voltage "
-						" failed\n", mmc_hostname(mmc));
+				pr_warn("%s: Switching to 1.8V signalling voltage failed\n",
+					mmc_hostname(mmc));
 				return -EIO;
 			}
 		}
@@ -1773,8 +1795,8 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 		if (ctrl & SDHCI_CTRL_VDD_180)
 			return 0;
 
-		pr_warning("%s: 1.8V regulator output did not became stable\n",
-				mmc_hostname(mmc));
+		pr_warn("%s: 1.8V regulator output did not became stable\n",
+			mmc_hostname(mmc));
 
 		return -EAGAIN;
 	case MMC_SIGNAL_VOLTAGE_120:
@@ -1782,8 +1804,8 @@ static int sdhci_do_start_signal_voltage_switch(struct sdhci_host *host,
 			ret = regulator_set_voltage(mmc->supply.vqmmc, 1100000,
 						    1300000);
 			if (ret) {
-				pr_warning("%s: Switching to 1.2V signalling voltage "
-						" failed\n", mmc_hostname(mmc));
+				pr_warn("%s: Switching to 1.2V signalling voltage failed\n",
+					mmc_hostname(mmc));
 				return -EIO;
 			}
 		}
@@ -2203,7 +2225,7 @@ static void sdhci_tuning_timer(unsigned long data)
  *                                                                           *
 \*****************************************************************************/
 
-static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask)
+static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask, u32 *mask)
 {
 	BUG_ON(intmask == 0);
 
@@ -2241,11 +2263,18 @@ static void sdhci_cmd_irq(struct sdhci_host *host, u32 intmask)
 		if (host->cmd->data)
 			DBG("Cannot wait for busy signal when also "
 				"doing a data transfer");
-		else if (!(host->quirks & SDHCI_QUIRK_NO_BUSY_IRQ))
+		else if (!(host->quirks & SDHCI_QUIRK_NO_BUSY_IRQ)
+				&& !host->busy_handle) {
+			/* Mark that command complete before busy is ended */
+			host->busy_handle = 1;
 			return;
+		}
 
 		/* The controller does not support the end-of-busy IRQ,
 		 * fall through and take the SDHCI_INT_RESPONSE */
+	} else if ((host->quirks2 & SDHCI_QUIRK2_STOP_WITH_TC) &&
+		   host->cmd->opcode == MMC_STOP_TRANSMISSION && !host->data) {
+		*mask &= ~SDHCI_INT_DATA_END;
 	}
 
 	if (intmask & SDHCI_INT_RESPONSE)
@@ -2304,8 +2333,21 @@ static void sdhci_data_irq(struct sdhci_host *host, u32 intmask)
 		 * above in sdhci_cmd_irq().
 		 */
 		if (host->cmd && (host->cmd->flags & MMC_RSP_BUSY)) {
+			if (intmask & SDHCI_INT_DATA_TIMEOUT) {
+				host->cmd->error = -ETIMEDOUT;
+				tasklet_schedule(&host->finish_tasklet);
+				return;
+			}
 			if (intmask & SDHCI_INT_DATA_END) {
-				sdhci_finish_command(host);
+				/*
+				 * Some cards handle busy-end interrupt
+				 * before the command completed, so make
+				 * sure we do things in the proper order.
+				 */
+				if (host->busy_handle)
+					sdhci_finish_command(host);
+				else
+					host->busy_handle = 1;
 				return;
 			}
 		}
@@ -2442,7 +2484,8 @@ static irqreturn_t sdhci_irq(int irq, void *dev_id)
 		}
 
 		if (intmask & SDHCI_INT_CMD_MASK)
-			sdhci_cmd_irq(host, intmask & SDHCI_INT_CMD_MASK);
+			sdhci_cmd_irq(host, intmask & SDHCI_INT_CMD_MASK,
+				      &intmask);
 
 		if (intmask & SDHCI_INT_DATA_MASK)
 			sdhci_data_irq(host, intmask & SDHCI_INT_DATA_MASK);
@@ -2534,7 +2577,7 @@ void sdhci_enable_irq_wakeups(struct sdhci_host *host)
 }
 EXPORT_SYMBOL_GPL(sdhci_enable_irq_wakeups);
 
-void sdhci_disable_irq_wakeups(struct sdhci_host *host)
+static void sdhci_disable_irq_wakeups(struct sdhci_host *host)
 {
 	u8 val;
 	u8 mask = SDHCI_WAKE_ON_INSERT | SDHCI_WAKE_ON_REMOVE
@@ -2544,7 +2587,6 @@ void sdhci_disable_irq_wakeups(struct sdhci_host *host)
 	val &= ~mask;
 	sdhci_writeb(host, val, SDHCI_WAKE_UP_CONTROL);
 }
-EXPORT_SYMBOL_GPL(sdhci_disable_irq_wakeups);
 
 int sdhci_suspend_host(struct sdhci_host *host)
 {
@@ -2749,6 +2791,7 @@ int sdhci_add_host(struct sdhci_host *host)
 	u32 caps[2] = {0, 0};
 	u32 max_current_caps;
 	unsigned int ocr_avail;
+	unsigned int override_timeout_clk;
 	int ret;
 
 	WARN_ON(host == NULL);
@@ -2762,6 +2805,8 @@ int sdhci_add_host(struct sdhci_host *host)
 	if (debug_quirks2)
 		host->quirks2 = debug_quirks2;
 
+	override_timeout_clk = host->timeout_clk;
+
 	sdhci_do_reset(host, SDHCI_RESET_ALL);
 
 	host->version = sdhci_readw(host, SDHCI_HOST_VERSION);
@@ -2807,8 +2852,7 @@ int sdhci_add_host(struct sdhci_host *host)
 	if (host->flags & (SDHCI_USE_SDMA | SDHCI_USE_ADMA)) {
 		if (host->ops->enable_dma) {
 			if (host->ops->enable_dma(host)) {
-				pr_warning("%s: No suitable DMA "
-					"available. Falling back to PIO.\n",
+				pr_warn("%s: No suitable DMA available - falling back to PIO\n",
 					mmc_hostname(mmc));
 				host->flags &=
 					~(SDHCI_USE_SDMA | SDHCI_USE_ADMA);
@@ -2830,15 +2874,14 @@ int sdhci_add_host(struct sdhci_host *host)
 			dma_free_coherent(mmc_dev(mmc), ADMA_SIZE,
 					  host->adma_desc, host->adma_addr);
 			kfree(host->align_buffer);
-			pr_warning("%s: Unable to allocate ADMA "
-				"buffers. Falling back to standard DMA.\n",
+			pr_warn("%s: Unable to allocate ADMA buffers - falling back to standard DMA\n",
 				mmc_hostname(mmc));
 			host->flags &= ~SDHCI_USE_ADMA;
 			host->adma_desc = NULL;
 			host->align_buffer = NULL;
 		} else if (host->adma_addr & 3) {
-			pr_warning("%s: unable to allocate aligned ADMA descriptor\n",
-				   mmc_hostname(mmc));
+			pr_warn("%s: unable to allocate aligned ADMA descriptor\n",
+				mmc_hostname(mmc));
 			host->flags &= ~SDHCI_USE_ADMA;
 			dma_free_coherent(mmc_dev(mmc), ADMA_SIZE,
 					  host->adma_desc, host->adma_addr);
@@ -2908,25 +2951,30 @@ int sdhci_add_host(struct sdhci_host *host)
 	} else
 		mmc->f_min = host->max_clk / SDHCI_MAX_DIV_SPEC_200;
 
-	host->timeout_clk =
-		(caps[0] & SDHCI_TIMEOUT_CLK_MASK) >> SDHCI_TIMEOUT_CLK_SHIFT;
-	if (host->timeout_clk == 0) {
-		if (host->ops->get_timeout_clock) {
-			host->timeout_clk = host->ops->get_timeout_clock(host);
-		} else if (!(host->quirks &
-				SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK)) {
-			pr_err("%s: Hardware doesn't specify timeout clock "
-			       "frequency.\n", mmc_hostname(mmc));
-			return -ENODEV;
+	if (!(host->quirks & SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK)) {
+		host->timeout_clk = (caps[0] & SDHCI_TIMEOUT_CLK_MASK) >>
+					SDHCI_TIMEOUT_CLK_SHIFT;
+		if (host->timeout_clk == 0) {
+			if (host->ops->get_timeout_clock) {
+				host->timeout_clk =
+					host->ops->get_timeout_clock(host);
+			} else {
+				pr_err("%s: Hardware doesn't specify timeout clock frequency.\n",
+					mmc_hostname(mmc));
+				return -ENODEV;
+			}
 		}
-	}
-	if (caps[0] & SDHCI_TIMEOUT_CLK_UNIT)
-		host->timeout_clk *= 1000;
 
-	if (host->quirks & SDHCI_QUIRK_DATA_TIMEOUT_USES_SDCLK)
-		host->timeout_clk = mmc->f_max / 1000;
+		if (caps[0] & SDHCI_TIMEOUT_CLK_UNIT)
+			host->timeout_clk *= 1000;
 
-	mmc->max_busy_timeout = (1 << 27) / host->timeout_clk;
+		mmc->max_busy_timeout = host->ops->get_max_timeout_count ?
+			host->ops->get_max_timeout_count(host) : 1 << 27;
+		mmc->max_busy_timeout /= host->timeout_clk;
+	}
+
+	if (override_timeout_clk)
+		host->timeout_clk = override_timeout_clk;
 
 	mmc->caps |= MMC_CAP_SDIO_IRQ | MMC_CAP_ERASE | MMC_CAP_CMD23;
 	mmc->caps2 |= MMC_CAP2_SDIO_IRQ_NOTHREAD;
@@ -2998,8 +3046,13 @@ int sdhci_add_host(struct sdhci_host *host)
 		/* SD3.0: SDR104 is supported so (for eMMC) the caps2
 		 * field can be promoted to support HS200.
 		 */
-		if (!(host->quirks2 & SDHCI_QUIRK2_BROKEN_HS200))
+		if (!(host->quirks2 & SDHCI_QUIRK2_BROKEN_HS200)) {
 			mmc->caps2 |= MMC_CAP2_HS200;
+			if (IS_ERR(mmc->supply.vqmmc) ||
+					!regulator_is_supported_voltage
+					(mmc->supply.vqmmc, 1100000, 1300000))
+				mmc->caps2 &= ~MMC_CAP2_HS200_1_2V_SDR;
+		}
 	} else if (caps[1] & SDHCI_SUPPORT_SDR50)
 		mmc->caps |= MMC_CAP_UHS_SDR50;
 
@@ -3049,7 +3102,7 @@ int sdhci_add_host(struct sdhci_host *host)
 	 */
 	max_current_caps = sdhci_readl(host, SDHCI_MAX_CURRENT);
 	if (!max_current_caps && !IS_ERR(mmc->supply.vmmc)) {
-		u32 curr = regulator_get_current_limit(mmc->supply.vmmc);
+		int curr = regulator_get_current_limit(mmc->supply.vmmc);
 		if (curr > 0) {
 
 			/* convert to SDHCI_MAX_CURRENT format */
@@ -3158,8 +3211,8 @@ int sdhci_add_host(struct sdhci_host *host)
 		mmc->max_blk_size = (caps[0] & SDHCI_MAX_BLOCK_MASK) >>
 				SDHCI_MAX_BLOCK_SHIFT;
 		if (mmc->max_blk_size >= 3) {
-			pr_warning("%s: Invalid maximum block size, "
-				"assuming 512 bytes\n", mmc_hostname(mmc));
+			pr_warn("%s: Invalid maximum block size, assuming 512 bytes\n",
+				mmc_hostname(mmc));
 			mmc->max_blk_size = 0;
 		}
 	}
diff --git a/drivers/mmc/host/sdhci.h b/drivers/mmc/host/sdhci.h
index 4a5cd5e3fa3e..31896a779d4e 100644
--- a/drivers/mmc/host/sdhci.h
+++ b/drivers/mmc/host/sdhci.h
@@ -72,6 +72,7 @@
 #define  SDHCI_WRITE_PROTECT	0x00080000
 #define  SDHCI_DATA_LVL_MASK	0x00F00000
 #define   SDHCI_DATA_LVL_SHIFT	20
+#define   SDHCI_DATA_0_LVL_MASK	0x00100000
 
 #define SDHCI_HOST_CONTROL	0x28
 #define  SDHCI_CTRL_LED		0x01
@@ -281,6 +282,9 @@ struct sdhci_ops {
 	unsigned int	(*get_max_clock)(struct sdhci_host *host);
 	unsigned int	(*get_min_clock)(struct sdhci_host *host);
 	unsigned int	(*get_timeout_clock)(struct sdhci_host *host);
+	unsigned int	(*get_max_timeout_count)(struct sdhci_host *host);
+	void		(*set_timeout)(struct sdhci_host *host,
+				       struct mmc_command *cmd);
 	void		(*set_bus_width)(struct sdhci_host *host, int width);
 	void (*platform_send_init_74_clocks)(struct sdhci_host *host,
 					     u8 power_mode);
diff --git a/drivers/mmc/host/sh_mmcif.c b/drivers/mmc/host/sh_mmcif.c
index d11708c815d7..7d9d6a321521 100644
--- a/drivers/mmc/host/sh_mmcif.c
+++ b/drivers/mmc/host/sh_mmcif.c
@@ -1553,7 +1553,6 @@ static struct platform_driver sh_mmcif_driver = {
 	.driver		= {
 		.name	= DRIVER_NAME,
 		.pm	= &sh_mmcif_dev_pm_ops,
-		.owner	= THIS_MODULE,
 		.of_match_table = mmcif_of_match,
 	},
 };
diff --git a/drivers/mmc/host/sh_mobile_sdhi.c b/drivers/mmc/host/sh_mobile_sdhi.c
index 91058dabd11a..a2e81a1ea6af 100644
--- a/drivers/mmc/host/sh_mobile_sdhi.c
+++ b/drivers/mmc/host/sh_mobile_sdhi.c
@@ -39,6 +39,7 @@ struct sh_mobile_sdhi_of_data {
 	unsigned long tmio_flags;
 	unsigned long capabilities;
 	unsigned long capabilities2;
+	dma_addr_t dma_rx_offset;
 };
 
 static const struct sh_mobile_sdhi_of_data sh_mobile_sdhi_of_cfg[] = {
@@ -48,14 +49,16 @@ static const struct sh_mobile_sdhi_of_data sh_mobile_sdhi_of_cfg[] = {
 };
 
 static const struct sh_mobile_sdhi_of_data of_rcar_gen1_compatible = {
-	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE,
+	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
+			  TMIO_MMC_CLK_ACTUAL,
 	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
 };
 
 static const struct sh_mobile_sdhi_of_data of_rcar_gen2_compatible = {
-	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE,
+	.tmio_flags	= TMIO_MMC_HAS_IDLE_WAIT | TMIO_MMC_WRPROTECT_DISABLE |
+			  TMIO_MMC_CLK_ACTUAL,
 	.capabilities	= MMC_CAP_SD_HIGHSPEED | MMC_CAP_SDIO_IRQ,
-	.capabilities2	= MMC_CAP2_NO_MULTI_READ,
+	.dma_rx_offset	= 0x2000,
 };
 
 static const struct of_device_id sh_mobile_sdhi_of_match[] = {
@@ -68,6 +71,9 @@ static const struct of_device_id sh_mobile_sdhi_of_match[] = {
 	{ .compatible = "renesas,sdhi-r8a7779", .data = &of_rcar_gen1_compatible, },
 	{ .compatible = "renesas,sdhi-r8a7790", .data = &of_rcar_gen2_compatible, },
 	{ .compatible = "renesas,sdhi-r8a7791", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7792", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7793", .data = &of_rcar_gen2_compatible, },
+	{ .compatible = "renesas,sdhi-r8a7794", .data = &of_rcar_gen2_compatible, },
 	{},
 };
 MODULE_DEVICE_TABLE(of, sh_mobile_sdhi_of_match);
@@ -132,6 +138,24 @@ static int sh_mobile_sdhi_write16_hook(struct tmio_mmc_host *host, int addr)
 	return 0;
 }
 
+static int sh_mobile_sdhi_multi_io_quirk(struct mmc_card *card,
+					 unsigned int direction, int blk_size)
+{
+	/*
+	 * In Renesas controllers, when performing a
+	 * multiple block read of one or two blocks,
+	 * depending on the timing with which the
+	 * response register is read, the response
+	 * value may not be read properly.
+	 * Use single block read for this HW bug
+	 */
+	if ((direction == MMC_DATA_READ) &&
+	    blk_size == 2)
+		return 1;
+
+	return blk_size;
+}
+
 static void sh_mobile_sdhi_cd_wakeup(const struct platform_device *pdev)
 {
 	mmc_detect_change(platform_get_drvdata(pdev), msecs_to_jiffies(100));
@@ -187,6 +211,7 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 	mmc_data->clk_disable = sh_mobile_sdhi_clk_disable;
 	mmc_data->capabilities = MMC_CAP_MMC_HIGHSPEED;
 	mmc_data->write16_hook = sh_mobile_sdhi_write16_hook;
+	mmc_data->multi_io_quirk = sh_mobile_sdhi_multi_io_quirk;
 	if (p) {
 		mmc_data->flags = p->tmio_flags;
 		mmc_data->ocr_mask = p->tmio_ocr_mask;
@@ -223,11 +248,27 @@ static int sh_mobile_sdhi_probe(struct platform_device *pdev)
 	 */
 	mmc_data->flags |= TMIO_MMC_SDIO_IRQ;
 
+	/*
+	 * All SDHI have CMD12 controll bit
+	 */
+	mmc_data->flags |= TMIO_MMC_HAVE_CMD12_CTRL;
+
+	/*
+	 * All SDHI need SDIO_INFO1 reserved bit
+	 */
+	mmc_data->flags |= TMIO_MMC_SDIO_STATUS_QUIRK;
+
+	/*
+	 * All SDHI have DMA control register
+	 */
+	mmc_data->flags |= TMIO_MMC_HAVE_CTL_DMA_REG;
+
 	if (of_id && of_id->data) {
 		const struct sh_mobile_sdhi_of_data *of_data = of_id->data;
 		mmc_data->flags |= of_data->tmio_flags;
 		mmc_data->capabilities |= of_data->capabilities;
 		mmc_data->capabilities2 |= of_data->capabilities2;
+		dma_priv->dma_rx_offset = of_data->dma_rx_offset;
 	}
 
 	/* SD control register space size is 0x100, 0x200 for bus_shift=1 */
@@ -332,8 +373,9 @@ static int sh_mobile_sdhi_remove(struct platform_device *pdev)
 }
 
 static const struct dev_pm_ops tmio_mmc_dev_pm_ops = {
-	SET_SYSTEM_SLEEP_PM_OPS(tmio_mmc_host_suspend, tmio_mmc_host_resume)
-	SET_RUNTIME_PM_OPS(tmio_mmc_host_runtime_suspend,
+	SET_SYSTEM_SLEEP_PM_OPS(pm_runtime_force_suspend,
+			pm_runtime_force_resume)
+	SET_PM_RUNTIME_PM_OPS(tmio_mmc_host_runtime_suspend,
 			tmio_mmc_host_runtime_resume,
 			NULL)
 };
@@ -341,7 +383,6 @@ static const struct dev_pm_ops tmio_mmc_dev_pm_ops = {
 static struct platform_driver sh_mobile_sdhi_driver = {
 	.driver		= {
 		.name	= "sh_mobile_sdhi",
-		.owner	= THIS_MODULE,
 		.pm	= &tmio_mmc_dev_pm_ops,
 		.of_match_table = sh_mobile_sdhi_of_match,
 	},
diff --git a/drivers/mmc/host/sunxi-mmc.c b/drivers/mmc/host/sunxi-mmc.c
index 024f67c98cdc..d1663b3c4143 100644
--- a/drivers/mmc/host/sunxi-mmc.c
+++ b/drivers/mmc/host/sunxi-mmc.c
@@ -990,7 +990,8 @@ static int sunxi_mmc_probe(struct platform_device *pdev)
 	/* 400kHz ~ 50MHz */
 	mmc->f_min		=   400000;
 	mmc->f_max		= 50000000;
-	mmc->caps	       |= MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED;
+	mmc->caps	       |= MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED |
+				  MMC_CAP_ERASE;
 
 	ret = mmc_of_parse(mmc);
 	if (ret)
@@ -1035,7 +1036,6 @@ static int sunxi_mmc_remove(struct platform_device *pdev)
 static struct platform_driver sunxi_mmc_driver = {
 	.driver = {
 		.name	= "sunxi-mmc",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(sunxi_mmc_of_match),
 	},
 	.probe		= sunxi_mmc_probe,
diff --git a/drivers/mmc/host/tifm_sd.c b/drivers/mmc/host/tifm_sd.c
index d1760ebcac03..93c4b40df90a 100644
--- a/drivers/mmc/host/tifm_sd.c
+++ b/drivers/mmc/host/tifm_sd.c
@@ -952,8 +952,8 @@ static int tifm_sd_probe(struct tifm_dev *sock)
 
 	if (!(TIFM_SOCK_STATE_OCCUPIED
 	      & readl(sock->addr + SOCK_PRESENT_STATE))) {
-		pr_warning("%s : card gone, unexpectedly\n",
-		       dev_name(&sock->dev));
+		pr_warn("%s : card gone, unexpectedly\n",
+			dev_name(&sock->dev));
 		return rc;
 	}
 
diff --git a/drivers/mmc/host/tmio_mmc.c b/drivers/mmc/host/tmio_mmc.c
index cfad844730d8..659028ddb8b1 100644
--- a/drivers/mmc/host/tmio_mmc.c
+++ b/drivers/mmc/host/tmio_mmc.c
@@ -30,7 +30,7 @@ static int tmio_mmc_suspend(struct device *dev)
 	const struct mfd_cell *cell = mfd_get_cell(pdev);
 	int ret;
 
-	ret = tmio_mmc_host_suspend(dev);
+	ret = pm_runtime_force_suspend(dev);
 
 	/* Tell MFD core it can disable us now.*/
 	if (!ret && cell->disable)
@@ -50,7 +50,7 @@ static int tmio_mmc_resume(struct device *dev)
 		ret = cell->resume(pdev);
 
 	if (!ret)
-		ret = tmio_mmc_host_resume(dev);
+		ret = pm_runtime_force_resume(dev);
 
 	return ret;
 }
@@ -135,6 +135,9 @@ static int tmio_mmc_remove(struct platform_device *pdev)
 
 static const struct dev_pm_ops tmio_mmc_dev_pm_ops = {
 	SET_SYSTEM_SLEEP_PM_OPS(tmio_mmc_suspend, tmio_mmc_resume)
+	SET_PM_RUNTIME_PM_OPS(tmio_mmc_host_runtime_suspend,
+			tmio_mmc_host_runtime_resume,
+			NULL)
 };
 
 static struct platform_driver tmio_mmc_driver = {
diff --git a/drivers/mmc/host/tmio_mmc.h b/drivers/mmc/host/tmio_mmc.h
index 100ffe0b2faf..a34ecbe1c1ad 100644
--- a/drivers/mmc/host/tmio_mmc.h
+++ b/drivers/mmc/host/tmio_mmc.h
@@ -40,22 +40,6 @@
 
 struct tmio_mmc_data;
 
-/*
- * We differentiate between the following 3 power states:
- * 1. card slot powered off, controller stopped. This is used, when either there
- *    is no card in the slot, or the card really has to be powered down.
- * 2. card slot powered on, controller stopped. This is used, when a card is in
- *    the slot, but no activity is currently taking place. This is a power-
- *    saving mode with card-state preserved. This state can be entered, e.g.
- *    when MMC clock-gating is used.
- * 3. card slot powered on, controller running. This is the actual active state.
- */
-enum tmio_mmc_power {
-	TMIO_MMC_OFF_STOP,	/* card power off, controller stopped */
-	TMIO_MMC_ON_STOP,	/* card power on, controller stopped */
-	TMIO_MMC_ON_RUN,	/* card power on, controller running */
-};
-
 struct tmio_mmc_host {
 	void __iomem *ctl;
 	struct mmc_command      *cmd;
@@ -63,9 +47,6 @@ struct tmio_mmc_host {
 	struct mmc_data         *data;
 	struct mmc_host         *mmc;
 
-	/* Controller and card power state */
-	enum tmio_mmc_power	power;
-
 	/* Callbacks for clock / power control */
 	void (*set_pwr)(struct platform_device *host, int state);
 	void (*set_clk_div)(struct platform_device *host, int state);
@@ -92,15 +73,16 @@ struct tmio_mmc_host {
 	struct delayed_work	delayed_reset_work;
 	struct work_struct	done;
 
-	/* Cache IRQ mask */
+	/* Cache */
 	u32			sdcard_irq_mask;
 	u32			sdio_irq_mask;
+	unsigned int		clk_cache;
 
 	spinlock_t		lock;		/* protect host private data */
 	unsigned long		last_req_ts;
 	struct mutex		ios_lock;	/* protect set_ios() context */
 	bool			native_hotplug;
-	bool			resuming;
+	bool			sdio_irq_enabled;
 };
 
 int tmio_mmc_host_probe(struct tmio_mmc_host **host,
@@ -162,12 +144,7 @@ static inline void tmio_mmc_abort_dma(struct tmio_mmc_host *host)
 }
 #endif
 
-#ifdef CONFIG_PM_SLEEP
-int tmio_mmc_host_suspend(struct device *dev);
-int tmio_mmc_host_resume(struct device *dev);
-#endif
-
-#ifdef CONFIG_PM_RUNTIME
+#ifdef CONFIG_PM
 int tmio_mmc_host_runtime_suspend(struct device *dev);
 int tmio_mmc_host_runtime_resume(struct device *dev);
 #endif
diff --git a/drivers/mmc/host/tmio_mmc_dma.c b/drivers/mmc/host/tmio_mmc_dma.c
index eb8f1d5c34b1..7d077388b9eb 100644
--- a/drivers/mmc/host/tmio_mmc_dma.c
+++ b/drivers/mmc/host/tmio_mmc_dma.c
@@ -28,10 +28,8 @@ void tmio_mmc_enable_dma(struct tmio_mmc_host *host, bool enable)
 	if (!host->chan_tx || !host->chan_rx)
 		return;
 
-#if defined(CONFIG_SUPERH) || defined(CONFIG_ARCH_SHMOBILE)
-	/* Switch DMA mode on or off - SuperH specific? */
-	sd_ctrl_write16(host, CTL_DMA_ENABLE, enable ? 2 : 0);
-#endif
+	if (host->pdata->flags & TMIO_MMC_HAVE_CTL_DMA_REG)
+		sd_ctrl_write16(host, CTL_DMA_ENABLE, enable ? 2 : 0);
 }
 
 void tmio_mmc_abort_dma(struct tmio_mmc_host *host)
@@ -312,7 +310,7 @@ void tmio_mmc_request_dma(struct tmio_mmc_host *host, struct tmio_mmc_data *pdat
 		if (pdata->dma->chan_priv_rx)
 			cfg.slave_id = pdata->dma->slave_id_rx;
 		cfg.direction = DMA_DEV_TO_MEM;
-		cfg.src_addr = cfg.dst_addr;
+		cfg.src_addr = cfg.dst_addr + pdata->dma->dma_rx_offset;
 		cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
 		cfg.dst_addr = 0;
 		ret = dmaengine_slave_config(host->chan_rx, &cfg);
diff --git a/drivers/mmc/host/tmio_mmc_pio.c b/drivers/mmc/host/tmio_mmc_pio.c
index faf0924e71cb..250bf8c9f998 100644
--- a/drivers/mmc/host/tmio_mmc_pio.c
+++ b/drivers/mmc/host/tmio_mmc_pio.c
@@ -44,6 +44,7 @@
 #include <linux/pm_qos.h>
 #include <linux/pm_runtime.h>
 #include <linux/regulator/consumer.h>
+#include <linux/mmc/sdio.h>
 #include <linux/scatterlist.h>
 #include <linux/spinlock.h>
 #include <linux/workqueue.h>
@@ -129,19 +130,28 @@ static void tmio_mmc_enable_sdio_irq(struct mmc_host *mmc, int enable)
 {
 	struct tmio_mmc_host *host = mmc_priv(mmc);
 
-	if (enable) {
+	if (enable && !host->sdio_irq_enabled) {
+		/* Keep device active while SDIO irq is enabled */
+		pm_runtime_get_sync(mmc_dev(mmc));
+		host->sdio_irq_enabled = true;
+
 		host->sdio_irq_mask = TMIO_SDIO_MASK_ALL &
 					~TMIO_SDIO_STAT_IOIRQ;
 		sd_ctrl_write16(host, CTL_TRANSACTION_CTL, 0x0001);
 		sd_ctrl_write16(host, CTL_SDIO_IRQ_MASK, host->sdio_irq_mask);
-	} else {
+	} else if (!enable && host->sdio_irq_enabled) {
 		host->sdio_irq_mask = TMIO_SDIO_MASK_ALL;
 		sd_ctrl_write16(host, CTL_SDIO_IRQ_MASK, host->sdio_irq_mask);
 		sd_ctrl_write16(host, CTL_TRANSACTION_CTL, 0x0000);
+
+		host->sdio_irq_enabled = false;
+		pm_runtime_mark_last_busy(mmc_dev(mmc));
+		pm_runtime_put_autosuspend(mmc_dev(mmc));
 	}
 }
 
-static void tmio_mmc_set_clock(struct tmio_mmc_host *host, int new_clock)
+static void tmio_mmc_set_clock(struct tmio_mmc_host *host,
+				unsigned int new_clock)
 {
 	u32 clk = 0, clock;
 
@@ -149,7 +159,11 @@ static void tmio_mmc_set_clock(struct tmio_mmc_host *host, int new_clock)
 		for (clock = host->mmc->f_min, clk = 0x80000080;
 			new_clock >= (clock<<1); clk >>= 1)
 			clock <<= 1;
-		clk |= 0x100;
+
+		/* 1/1 clock is option */
+		if ((host->pdata->flags & TMIO_MMC_CLK_ACTUAL) &&
+		    ((clk >> 22) & 0x1))
+			clk |= 0xff;
 	}
 
 	if (host->set_clk_div)
@@ -245,6 +259,9 @@ static void tmio_mmc_reset_work(struct work_struct *work)
 
 	tmio_mmc_abort_dma(host);
 	mmc_request_done(host->mmc, mrq);
+
+	pm_runtime_mark_last_busy(mmc_dev(host->mmc));
+	pm_runtime_put_autosuspend(mmc_dev(host->mmc));
 }
 
 /* called with host->lock held, interrupts disabled */
@@ -274,6 +291,9 @@ static void tmio_mmc_finish_request(struct tmio_mmc_host *host)
 		tmio_mmc_abort_dma(host);
 
 	mmc_request_done(host->mmc, mrq);
+
+	pm_runtime_mark_last_busy(mmc_dev(host->mmc));
+	pm_runtime_put_autosuspend(mmc_dev(host->mmc));
 }
 
 static void tmio_mmc_done_work(struct work_struct *work)
@@ -295,6 +315,7 @@ static void tmio_mmc_done_work(struct work_struct *work)
 #define TRANSFER_READ  0x1000
 #define TRANSFER_MULTI 0x2000
 #define SECURITY_CMD   0x4000
+#define NO_CMD12_ISSUE 0x4000 /* TMIO_MMC_HAVE_CMD12_CTRL */
 
 static int tmio_mmc_start_command(struct tmio_mmc_host *host, struct mmc_command *cmd)
 {
@@ -331,6 +352,14 @@ static int tmio_mmc_start_command(struct tmio_mmc_host *host, struct mmc_command
 		if (data->blocks > 1) {
 			sd_ctrl_write16(host, CTL_STOP_INTERNAL_ACTION, 0x100);
 			c |= TRANSFER_MULTI;
+
+			/*
+			 * Disable auto CMD12 at IO_RW_EXTENDED when
+			 * multiple block transfer
+			 */
+			if ((host->pdata->flags & TMIO_MMC_HAVE_CMD12_CTRL) &&
+			    (cmd->opcode == SD_IO_RW_EXTENDED))
+				c |= NO_CMD12_ISSUE;
 		}
 		if (data->flags & MMC_DATA_READ)
 			c |= TRANSFER_READ;
@@ -347,6 +376,40 @@ static int tmio_mmc_start_command(struct tmio_mmc_host *host, struct mmc_command
 	return 0;
 }
 
+static void tmio_mmc_transfer_data(struct tmio_mmc_host *host,
+				   unsigned short *buf,
+				   unsigned int count)
+{
+	int is_read = host->data->flags & MMC_DATA_READ;
+	u8  *buf8;
+
+	/*
+	 * Transfer the data
+	 */
+	if (is_read)
+		sd_ctrl_read16_rep(host, CTL_SD_DATA_PORT, buf, count >> 1);
+	else
+		sd_ctrl_write16_rep(host, CTL_SD_DATA_PORT, buf, count >> 1);
+
+	/* if count was even number */
+	if (!(count & 0x1))
+		return;
+
+	/* if count was odd number */
+	buf8 = (u8 *)(buf + (count >> 1));
+
+	/*
+	 * FIXME
+	 *
+	 * driver and this function are assuming that
+	 * it is used as little endian
+	 */
+	if (is_read)
+		*buf8 = sd_ctrl_read16(host, CTL_SD_DATA_PORT) & 0xff;
+	else
+		sd_ctrl_write16(host, CTL_SD_DATA_PORT, *buf8);
+}
+
 /*
  * This chip always returns (at least?) as much data as you ask for.
  * I'm unsure what happens if you ask for less than a block. This should be
@@ -379,10 +442,7 @@ static void tmio_mmc_pio_irq(struct tmio_mmc_host *host)
 		 count, host->sg_off, data->flags);
 
 	/* Transfer the data */
-	if (data->flags & MMC_DATA_READ)
-		sd_ctrl_read16_rep(host, CTL_SD_DATA_PORT, buf, count >> 1);
-	else
-		sd_ctrl_write16_rep(host, CTL_SD_DATA_PORT, buf, count >> 1);
+	tmio_mmc_transfer_data(host, buf, count);
 
 	host->sg_off += count;
 
@@ -465,6 +525,9 @@ static void tmio_mmc_data_irq(struct tmio_mmc_host *host)
 		goto out;
 
 	if (host->chan_tx && (data->flags & MMC_DATA_WRITE) && !host->force_pio) {
+		u32 status = sd_ctrl_read32(host, CTL_STATUS);
+		bool done = false;
+
 		/*
 		 * Has all data been written out yet? Testing on SuperH showed,
 		 * that in most cases the first interrupt comes already with the
@@ -473,7 +536,15 @@ static void tmio_mmc_data_irq(struct tmio_mmc_host *host)
 		 * DATAEND interrupt with the BUSY bit set, in this cases
 		 * waiting for one more interrupt fixes the problem.
 		 */
-		if (!(sd_ctrl_read32(host, CTL_STATUS) & TMIO_STAT_CMD_BUSY)) {
+		if (host->pdata->flags & TMIO_MMC_HAS_IDLE_WAIT) {
+			if (status & TMIO_STAT_ILL_FUNC)
+				done = true;
+		} else {
+			if (!(status & TMIO_STAT_CMD_BUSY))
+				done = true;
+		}
+
+		if (done) {
 			tmio_mmc_disable_mmc_irqs(host, TMIO_STAT_DATAEND);
 			tasklet_schedule(&host->dma_complete);
 		}
@@ -557,6 +628,9 @@ static void tmio_mmc_card_irq_status(struct tmio_mmc_host *host,
 
 	pr_debug_status(*status);
 	pr_debug_status(*ireg);
+
+	/* Clear the status except the interrupt status */
+	sd_ctrl_write32(host, CTL_STATUS, TMIO_MASK_IRQ);
 }
 
 static bool __tmio_mmc_card_detect_irq(struct tmio_mmc_host *host,
@@ -637,6 +711,7 @@ irqreturn_t tmio_mmc_sdio_irq(int irq, void *devid)
 	struct mmc_host *mmc = host->mmc;
 	struct tmio_mmc_data *pdata = host->pdata;
 	unsigned int ireg, status;
+	unsigned int sdio_status;
 
 	if (!(pdata->flags & TMIO_MMC_SDIO_IRQ))
 		return IRQ_HANDLED;
@@ -644,7 +719,11 @@ irqreturn_t tmio_mmc_sdio_irq(int irq, void *devid)
 	status = sd_ctrl_read16(host, CTL_SDIO_STATUS);
 	ireg = status & TMIO_SDIO_MASK_ALL & ~host->sdcard_irq_mask;
 
-	sd_ctrl_write16(host, CTL_SDIO_STATUS, status & ~TMIO_SDIO_MASK_ALL);
+	sdio_status = status & ~TMIO_SDIO_MASK_ALL;
+	if (pdata->flags & TMIO_MMC_SDIO_STATUS_QUIRK)
+		sdio_status |= 6;
+
+	sd_ctrl_write16(host, CTL_SDIO_STATUS, sdio_status);
 
 	if (mmc->caps & MMC_CAP_SDIO_IRQ && ireg & TMIO_SDIO_STAT_IOIRQ)
 		mmc_signal_sdio_irq(mmc);
@@ -728,6 +807,8 @@ static void tmio_mmc_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 	spin_unlock_irqrestore(&host->lock, flags);
 
+	pm_runtime_get_sync(mmc_dev(mmc));
+
 	if (mrq->data) {
 		ret = tmio_mmc_start_data(host, mrq->data);
 		if (ret)
@@ -746,11 +827,14 @@ fail:
 	host->mrq = NULL;
 	mrq->cmd->error = ret;
 	mmc_request_done(mmc, mrq);
+
+	pm_runtime_mark_last_busy(mmc_dev(mmc));
+	pm_runtime_put_autosuspend(mmc_dev(mmc));
 }
 
-static int tmio_mmc_clk_update(struct mmc_host *mmc)
+static int tmio_mmc_clk_update(struct tmio_mmc_host *host)
 {
-	struct tmio_mmc_host *host = mmc_priv(mmc);
+	struct mmc_host *mmc = host->mmc;
 	struct tmio_mmc_data *pdata = host->pdata;
 	int ret;
 
@@ -812,6 +896,19 @@ static void tmio_mmc_power_off(struct tmio_mmc_host *host)
 		host->set_pwr(host->pdev, 0);
 }
 
+static void tmio_mmc_set_bus_width(struct tmio_mmc_host *host,
+				unsigned char bus_width)
+{
+	switch (bus_width) {
+	case MMC_BUS_WIDTH_1:
+		sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x80e0);
+		break;
+	case MMC_BUS_WIDTH_4:
+		sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x00e0);
+		break;
+	}
+}
+
 /* Set MMC clock / power.
  * Note: This controller uses a simple divider scheme therefore it cannot
  * run a MMC card at full speed (20MHz). The max clock is 24MHz on SD, but as
@@ -824,6 +921,8 @@ static void tmio_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 	struct device *dev = &host->pdev->dev;
 	unsigned long flags;
 
+	pm_runtime_get_sync(mmc_dev(mmc));
+
 	mutex_lock(&host->ios_lock);
 
 	spin_lock_irqsave(&host->lock, flags);
@@ -850,60 +949,22 @@ static void tmio_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 
 	spin_unlock_irqrestore(&host->lock, flags);
 
-	/*
-	 * host->power toggles between false and true in both cases - either
-	 * or not the controller can be runtime-suspended during inactivity.
-	 * But if the controller has to be kept on, the runtime-pm usage_count
-	 * is kept positive, so no suspending actually takes place.
-	 */
-	if (ios->power_mode == MMC_POWER_ON && ios->clock) {
-		if (host->power != TMIO_MMC_ON_RUN) {
-			tmio_mmc_clk_update(mmc);
-			pm_runtime_get_sync(dev);
-			if (host->resuming) {
-				tmio_mmc_reset(host);
-				host->resuming = false;
-			}
-		}
-		if (host->power == TMIO_MMC_OFF_STOP)
-			tmio_mmc_reset(host);
+	switch (ios->power_mode) {
+	case MMC_POWER_OFF:
+		tmio_mmc_power_off(host);
+		tmio_mmc_clk_stop(host);
+		break;
+	case MMC_POWER_UP:
 		tmio_mmc_set_clock(host, ios->clock);
-		if (host->power == TMIO_MMC_OFF_STOP)
-			/* power up SD card and the bus */
-			tmio_mmc_power_on(host, ios->vdd);
-		host->power = TMIO_MMC_ON_RUN;
-		/* start bus clock */
+		tmio_mmc_power_on(host, ios->vdd);
 		tmio_mmc_clk_start(host);
-	} else if (ios->power_mode != MMC_POWER_UP) {
-		struct tmio_mmc_data *pdata = host->pdata;
-		unsigned int old_power = host->power;
-
-		if (old_power != TMIO_MMC_OFF_STOP) {
-			if (ios->power_mode == MMC_POWER_OFF) {
-				tmio_mmc_power_off(host);
-				host->power = TMIO_MMC_OFF_STOP;
-			} else {
-				host->power = TMIO_MMC_ON_STOP;
-			}
-		}
-
-		if (old_power == TMIO_MMC_ON_RUN) {
-			tmio_mmc_clk_stop(host);
-			pm_runtime_put(dev);
-			if (pdata->clk_disable)
-				pdata->clk_disable(host->pdev);
-		}
-	}
-
-	if (host->power != TMIO_MMC_OFF_STOP) {
-		switch (ios->bus_width) {
-		case MMC_BUS_WIDTH_1:
-			sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x80e0);
+		tmio_mmc_set_bus_width(host, ios->bus_width);
 		break;
-		case MMC_BUS_WIDTH_4:
-			sd_ctrl_write16(host, CTL_SD_MEM_CARD_OPT, 0x00e0);
+	case MMC_POWER_ON:
+		tmio_mmc_set_clock(host, ios->clock);
+		tmio_mmc_clk_start(host);
+		tmio_mmc_set_bus_width(host, ios->bus_width);
 		break;
-		}
 	}
 
 	/* Let things settle. delay taken from winCE driver */
@@ -915,7 +976,12 @@ static void tmio_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 			ios->clock, ios->power_mode);
 	host->mrq = NULL;
 
+	host->clk_cache = ios->clock;
+
 	mutex_unlock(&host->ios_lock);
+
+	pm_runtime_mark_last_busy(mmc_dev(mmc));
+	pm_runtime_put_autosuspend(mmc_dev(mmc));
 }
 
 static int tmio_mmc_get_ro(struct mmc_host *mmc)
@@ -926,8 +992,25 @@ static int tmio_mmc_get_ro(struct mmc_host *mmc)
 	if (ret >= 0)
 		return ret;
 
-	return !((pdata->flags & TMIO_MMC_WRPROTECT_DISABLE) ||
-		 (sd_ctrl_read32(host, CTL_STATUS) & TMIO_STAT_WRPROTECT));
+	pm_runtime_get_sync(mmc_dev(mmc));
+	ret = !((pdata->flags & TMIO_MMC_WRPROTECT_DISABLE) ||
+		(sd_ctrl_read32(host, CTL_STATUS) & TMIO_STAT_WRPROTECT));
+	pm_runtime_mark_last_busy(mmc_dev(mmc));
+	pm_runtime_put_autosuspend(mmc_dev(mmc));
+
+	return ret;
+}
+
+static int tmio_multi_io_quirk(struct mmc_card *card,
+			       unsigned int direction, int blk_size)
+{
+	struct tmio_mmc_host *host = mmc_priv(card->host);
+	struct tmio_mmc_data *pdata = host->pdata;
+
+	if (pdata->multi_io_quirk)
+		return pdata->multi_io_quirk(card, direction, blk_size);
+
+	return blk_size;
 }
 
 static const struct mmc_host_ops tmio_mmc_ops = {
@@ -936,6 +1019,7 @@ static const struct mmc_host_ops tmio_mmc_ops = {
 	.get_ro         = tmio_mmc_get_ro,
 	.get_cd		= mmc_gpio_get_cd,
 	.enable_sdio_irq = tmio_mmc_enable_sdio_irq,
+	.multi_io_quirk	= tmio_multi_io_quirk,
 };
 
 static int tmio_mmc_init_ocr(struct tmio_mmc_host *host)
@@ -1032,28 +1116,23 @@ int tmio_mmc_host_probe(struct tmio_mmc_host **host,
 				  mmc->caps & MMC_CAP_NONREMOVABLE ||
 				  mmc->slot.cd_irq >= 0);
 
-	_host->power = TMIO_MMC_OFF_STOP;
-	pm_runtime_enable(&pdev->dev);
-	ret = pm_runtime_resume(&pdev->dev);
-	if (ret < 0)
-		goto pm_disable;
-
-	if (tmio_mmc_clk_update(mmc) < 0) {
+	if (tmio_mmc_clk_update(_host) < 0) {
 		mmc->f_max = pdata->hclk;
 		mmc->f_min = mmc->f_max / 512;
 	}
 
 	/*
-	 * There are 4 different scenarios for the card detection:
-	 *  1) an external gpio irq handles the cd (best for power savings)
-	 *  2) internal sdhi irq handles the cd
-	 *  3) a worker thread polls the sdhi - indicated by MMC_CAP_NEEDS_POLL
-	 *  4) the medium is non-removable - indicated by MMC_CAP_NONREMOVABLE
-	 *
-	 *  While we increment the runtime PM counter for all scenarios when
-	 *  the mmc core activates us by calling an appropriate set_ios(), we
-	 *  must additionally ensure that in case 2) the tmio mmc hardware stays
-	 *  powered on during runtime for the card detection to work.
+	 * Check the sanity of mmc->f_min to prevent tmio_mmc_set_clock() from
+	 * looping forever...
+	 */
+	if (mmc->f_min == 0) {
+		ret = -EINVAL;
+		goto host_free;
+	}
+
+	/*
+	 * While using internal tmio hardware logic for card detection, we need
+	 * to ensure it stays powered for it to work.
 	 */
 	if (_host->native_hotplug)
 		pm_runtime_get_noresume(&pdev->dev);
@@ -1074,8 +1153,12 @@ int tmio_mmc_host_probe(struct tmio_mmc_host **host,
 
 	_host->sdcard_irq_mask &= ~irq_mask;
 
-	if (pdata->flags & TMIO_MMC_SDIO_IRQ)
-		tmio_mmc_enable_sdio_irq(mmc, 0);
+	_host->sdio_irq_enabled = false;
+	if (pdata->flags & TMIO_MMC_SDIO_IRQ) {
+		_host->sdio_irq_mask = TMIO_SDIO_MASK_ALL;
+		sd_ctrl_write16(_host, CTL_SDIO_IRQ_MASK, _host->sdio_irq_mask);
+		sd_ctrl_write16(_host, CTL_TRANSACTION_CTL, 0x0000);
+	}
 
 	spin_lock_init(&_host->lock);
 	mutex_init(&_host->ios_lock);
@@ -1087,9 +1170,12 @@ int tmio_mmc_host_probe(struct tmio_mmc_host **host,
 	/* See if we also get DMA */
 	tmio_mmc_request_dma(_host, pdata);
 
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 50);
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
 	ret = mmc_add_host(mmc);
-	if (pdata->clk_disable)
-		pdata->clk_disable(pdev);
 	if (ret < 0) {
 		tmio_mmc_host_remove(_host);
 		return ret;
@@ -1103,15 +1189,13 @@ int tmio_mmc_host_probe(struct tmio_mmc_host **host,
 			tmio_mmc_host_remove(_host);
 			return ret;
 		}
+		mmc_gpiod_request_cd_irq(mmc);
 	}
 
 	*host = _host;
 
 	return 0;
 
-pm_disable:
-	pm_runtime_disable(&pdev->dev);
-	iounmap(_host->ctl);
 host_free:
 	mmc_free_host(mmc);
 
@@ -1142,34 +1226,20 @@ void tmio_mmc_host_remove(struct tmio_mmc_host *host)
 }
 EXPORT_SYMBOL(tmio_mmc_host_remove);
 
-#ifdef CONFIG_PM_SLEEP
-int tmio_mmc_host_suspend(struct device *dev)
+#ifdef CONFIG_PM
+int tmio_mmc_host_runtime_suspend(struct device *dev)
 {
 	struct mmc_host *mmc = dev_get_drvdata(dev);
 	struct tmio_mmc_host *host = mmc_priv(mmc);
 
 	tmio_mmc_disable_mmc_irqs(host, TMIO_MASK_ALL);
-	return 0;
-}
-EXPORT_SYMBOL(tmio_mmc_host_suspend);
-
-int tmio_mmc_host_resume(struct device *dev)
-{
-	struct mmc_host *mmc = dev_get_drvdata(dev);
-	struct tmio_mmc_host *host = mmc_priv(mmc);
 
-	tmio_mmc_enable_dma(host, true);
+	if (host->clk_cache)
+		tmio_mmc_clk_stop(host);
 
-	/* The MMC core will perform the complete set up */
-	host->resuming = true;
-	return 0;
-}
-EXPORT_SYMBOL(tmio_mmc_host_resume);
-#endif
+	if (host->pdata->clk_disable)
+		host->pdata->clk_disable(host->pdev);
 
-#ifdef CONFIG_PM_RUNTIME
-int tmio_mmc_host_runtime_suspend(struct device *dev)
-{
 	return 0;
 }
 EXPORT_SYMBOL(tmio_mmc_host_runtime_suspend);
@@ -1179,6 +1249,14 @@ int tmio_mmc_host_runtime_resume(struct device *dev)
 	struct mmc_host *mmc = dev_get_drvdata(dev);
 	struct tmio_mmc_host *host = mmc_priv(mmc);
 
+	tmio_mmc_reset(host);
+	tmio_mmc_clk_update(host);
+
+	if (host->clk_cache) {
+		tmio_mmc_set_clock(host, host->clk_cache);
+		tmio_mmc_clk_start(host);
+	}
+
 	tmio_mmc_enable_dma(host, true);
 
 	return 0;
diff --git a/drivers/mmc/host/wbsd.c b/drivers/mmc/host/wbsd.c
index 1defd5ed3236..9a6dfb0c4ecc 100644
--- a/drivers/mmc/host/wbsd.c
+++ b/drivers/mmc/host/wbsd.c
@@ -803,8 +803,7 @@ static void wbsd_request(struct mmc_host *mmc, struct mmc_request *mrq)
 
 		default:
 #ifdef CONFIG_MMC_DEBUG
-			pr_warning("%s: Data command %d is not "
-				"supported by this controller.\n",
+			pr_warn("%s: Data command %d is not supported by this controller\n",
 				mmc_hostname(host->mmc), cmd->opcode);
 #endif
 			cmd->error = -EINVAL;
@@ -1429,8 +1428,8 @@ free:
 	free_dma(dma);
 
 err:
-	pr_warning(DRIVER_NAME ": Unable to allocate DMA %d. "
-		"Falling back on FIFO.\n", dma);
+	pr_warn(DRIVER_NAME ": Unable to allocate DMA %d - falling back on FIFO\n",
+		dma);
 }
 
 static void wbsd_release_dma(struct wbsd_host *host)
@@ -1664,9 +1663,7 @@ static int wbsd_init(struct device *dev, int base, int irq, int dma,
 	ret = wbsd_scan(host);
 	if (ret) {
 		if (pnp && (ret == -ENODEV)) {
-			pr_warning(DRIVER_NAME
-				": Unable to confirm device presence. You may "
-				"experience lock-ups.\n");
+			pr_warn(DRIVER_NAME ": Unable to confirm device presence - you may experience lock-ups\n");
 		} else {
 			wbsd_free_mmc(dev);
 			return ret;
@@ -1688,10 +1685,7 @@ static int wbsd_init(struct device *dev, int base, int irq, int dma,
 	 */
 	if (pnp) {
 		if ((host->config != 0) && !wbsd_chip_validate(host)) {
-			pr_warning(DRIVER_NAME
-				": PnP active but chip not configured! "
-				"You probably have a buggy BIOS. "
-				"Configuring chip manually.\n");
+			pr_warn(DRIVER_NAME ": PnP active but chip not configured! You probably have a buggy BIOS. Configuring chip manually.\n");
 			wbsd_chip_config(host);
 		}
 	} else
@@ -1884,10 +1878,7 @@ static int wbsd_pnp_resume(struct pnp_dev *pnp_dev)
 	 */
 	if (host->config != 0) {
 		if (!wbsd_chip_validate(host)) {
-			pr_warning(DRIVER_NAME
-				": PnP active but chip not configured! "
-				"You probably have a buggy BIOS. "
-				"Configuring chip manually.\n");
+			pr_warn(DRIVER_NAME ": PnP active but chip not configured! You probably have a buggy BIOS. Configuring chip manually.\n");
 			wbsd_chip_config(host);
 		}
 	}
diff --git a/drivers/mtd/bcm47xxpart.c b/drivers/mtd/bcm47xxpart.c
index adfa74c1bc45..8057f52a45b7 100644
--- a/drivers/mtd/bcm47xxpart.c
+++ b/drivers/mtd/bcm47xxpart.c
@@ -199,6 +199,17 @@ static int bcm47xxpart_parse(struct mtd_info *master,
 			continue;
 		}
 
+		/*
+		 * New (ARM?) devices may have NVRAM in some middle block. Last
+		 * block will be checked later, so skip it.
+		 */
+		if (offset != master->size - blocksize &&
+		    buf[0x000 / 4] == NVRAM_HEADER) {
+			bcm47xxpart_add_part(&parts[curr_part++], "nvram",
+					     offset, 0);
+			continue;
+		}
+
 		/* Read middle of the block */
 		if (mtd_read(master, offset + 0x8000, 0x4,
 			     &bytes_read, (uint8_t *)buf) < 0) {
diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index 5a4bfe33112a..c50d8cf0f60d 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -1434,6 +1434,10 @@ static int cfi_amdstd_otp_walk(struct mtd_info *mtd, loff_t from, size_t len,
 
 				mutex_lock(&chip->mutex);
 				ret = get_chip(map, chip, base, FL_LOCKING);
+				if (ret) {
+					mutex_unlock(&chip->mutex);
+					return ret;
+				}
 
 				/* Enter lock register command */
 				cfi_send_gen_cmd(0xAA, cfi->addr_unlock1,
@@ -2029,6 +2033,8 @@ static int cfi_amdstd_panic_wait(struct map_info *map, struct flchip *chip,
 
 			udelay(1);
 		}
+
+		retries--;
 	}
 
 	/* the chip never became ready */
diff --git a/drivers/mtd/devices/Makefile b/drivers/mtd/devices/Makefile
index c68868f60588..f0b0e611d1d6 100644
--- a/drivers/mtd/devices/Makefile
+++ b/drivers/mtd/devices/Makefile
@@ -12,7 +12,6 @@ obj-$(CONFIG_MTD_LART)		+= lart.o
 obj-$(CONFIG_MTD_BLOCK2MTD)	+= block2mtd.o
 obj-$(CONFIG_MTD_DATAFLASH)	+= mtd_dataflash.o
 obj-$(CONFIG_MTD_M25P80)	+= m25p80.o
-obj-$(CONFIG_MTD_NAND_OMAP_BCH)	+= elm.o
 obj-$(CONFIG_MTD_SPEAR_SMI)	+= spear_smi.o
 obj-$(CONFIG_MTD_SST25L)	+= sst25l.o
 obj-$(CONFIG_MTD_BCM47XXSFLASH)	+= bcm47xxsflash.o
diff --git a/drivers/mtd/devices/docg3.c b/drivers/mtd/devices/docg3.c
index 91a169c44b39..21cc4b66feaa 100644
--- a/drivers/mtd/devices/docg3.c
+++ b/drivers/mtd/devices/docg3.c
@@ -1697,16 +1697,16 @@ static int dbg_asicmode_show(struct seq_file *s, void *p)
 
 	switch (mode) {
 	case DOC_ASICMODE_RESET:
-		pos += seq_printf(s, "reset");
+		pos += seq_puts(s, "reset");
 		break;
 	case DOC_ASICMODE_NORMAL:
-		pos += seq_printf(s, "normal");
+		pos += seq_puts(s, "normal");
 		break;
 	case DOC_ASICMODE_POWERDOWN:
-		pos += seq_printf(s, "powerdown");
+		pos += seq_puts(s, "powerdown");
 		break;
 	}
-	pos += seq_printf(s, ")\n");
+	pos += seq_puts(s, ")\n");
 	return pos;
 }
 DEBUGFS_RO_ATTR(asic_mode, dbg_asicmode_show);
@@ -1745,22 +1745,22 @@ static int dbg_protection_show(struct seq_file *s, void *p)
 	pos += seq_printf(s, "Protection = 0x%02x (",
 			 protect);
 	if (protect & DOC_PROTECT_FOUNDRY_OTP_LOCK)
-		pos += seq_printf(s, "FOUNDRY_OTP_LOCK,");
+		pos += seq_puts(s, "FOUNDRY_OTP_LOCK,");
 	if (protect & DOC_PROTECT_CUSTOMER_OTP_LOCK)
-		pos += seq_printf(s, "CUSTOMER_OTP_LOCK,");
+		pos += seq_puts(s, "CUSTOMER_OTP_LOCK,");
 	if (protect & DOC_PROTECT_LOCK_INPUT)
-		pos += seq_printf(s, "LOCK_INPUT,");
+		pos += seq_puts(s, "LOCK_INPUT,");
 	if (protect & DOC_PROTECT_STICKY_LOCK)
-		pos += seq_printf(s, "STICKY_LOCK,");
+		pos += seq_puts(s, "STICKY_LOCK,");
 	if (protect & DOC_PROTECT_PROTECTION_ENABLED)
-		pos += seq_printf(s, "PROTECTION ON,");
+		pos += seq_puts(s, "PROTECTION ON,");
 	if (protect & DOC_PROTECT_IPL_DOWNLOAD_LOCK)
-		pos += seq_printf(s, "IPL_DOWNLOAD_LOCK,");
+		pos += seq_puts(s, "IPL_DOWNLOAD_LOCK,");
 	if (protect & DOC_PROTECT_PROTECTION_ERROR)
-		pos += seq_printf(s, "PROTECT_ERR,");
+		pos += seq_puts(s, "PROTECT_ERR,");
 	else
-		pos += seq_printf(s, "NO_PROTECT_ERR");
-	pos += seq_printf(s, ")\n");
+		pos += seq_puts(s, "NO_PROTECT_ERR");
+	pos += seq_puts(s, ")\n");
 
 	pos += seq_printf(s, "DPS0 = 0x%02x : "
 			 "Protected area [0x%x - 0x%x] : OTP=%d, READ=%d, "
diff --git a/drivers/mtd/devices/m25p80.c b/drivers/mtd/devices/m25p80.c
index ed7e0a1bed3c..dcda6287228d 100644
--- a/drivers/mtd/devices/m25p80.c
+++ b/drivers/mtd/devices/m25p80.c
@@ -193,11 +193,14 @@ static int m25p_probe(struct spi_device *spi)
 {
 	struct mtd_part_parser_data	ppdata;
 	struct flash_platform_data	*data;
+	const struct spi_device_id *id = NULL;
 	struct m25p *flash;
 	struct spi_nor *nor;
 	enum read_mode mode = SPI_NOR_NORMAL;
 	int ret;
 
+	data = dev_get_platdata(&spi->dev);
+
 	flash = devm_kzalloc(&spi->dev, sizeof(*flash), GFP_KERNEL);
 	if (!flash)
 		return -ENOMEM;
@@ -223,11 +226,26 @@ static int m25p_probe(struct spi_device *spi)
 		mode = SPI_NOR_QUAD;
 	else if (spi->mode & SPI_RX_DUAL)
 		mode = SPI_NOR_DUAL;
-	ret = spi_nor_scan(nor, spi_get_device_id(spi), mode);
+
+	if (data && data->name)
+		flash->mtd.name = data->name;
+
+	/* For some (historical?) reason many platforms provide two different
+	 * names in flash_platform_data: "name" and "type". Quite often name is
+	 * set to "m25p80" and then "type" provides a real chip name.
+	 * If that's the case, respect "type" and ignore a "name".
+	 */
+	if (data && data->type)
+		id = spi_nor_match_id(data->type);
+
+	/* If we didn't get name from platform, simply use "modalias". */
+	if (!id)
+		id = spi_get_device_id(spi);
+
+	ret = spi_nor_scan(nor, id, mode);
 	if (ret)
 		return ret;
 
-	data = dev_get_platdata(&spi->dev);
 	ppdata.of_node = spi->dev.of_node;
 
 	return mtd_device_parse_register(&flash->mtd, NULL, &ppdata,
diff --git a/drivers/mtd/maps/Kconfig b/drivers/mtd/maps/Kconfig
index 21b2874a303b..ba801d2c6dcc 100644
--- a/drivers/mtd/maps/Kconfig
+++ b/drivers/mtd/maps/Kconfig
@@ -249,7 +249,7 @@ config MTD_CFI_FLAGADM
 
 config MTD_SOLUTIONENGINE
 	tristate "CFI Flash device mapped on Hitachi SolutionEngine"
-	depends on SUPERH && SOLUTION_ENGINE && MTD_CFI && MTD_REDBOOT_PARTS
+	depends on SOLUTION_ENGINE && MTD_CFI && MTD_REDBOOT_PARTS
 	help
 	  This enables access to the flash chips on the Hitachi SolutionEngine and
 	  similar boards. Say 'Y' if you are building a kernel for such a board.
diff --git a/drivers/mtd/maps/gpio-addr-flash.c b/drivers/mtd/maps/gpio-addr-flash.c
index a4c477b9fdd6..2fb346091af2 100644
--- a/drivers/mtd/maps/gpio-addr-flash.c
+++ b/drivers/mtd/maps/gpio-addr-flash.c
@@ -99,22 +99,28 @@ static map_word gf_read(struct map_info *map, unsigned long ofs)
  *	@from: flash offset to copy from
  *	@len:  how much to copy
  *
- * We rely on the MTD layer to chunk up copies such that a single request here
- * will not cross a window size.  This allows us to only wiggle the GPIOs once
- * before falling back to a normal memcpy.  Reading the higher layer code shows
- * that this is indeed the case, but add a BUG_ON() to future proof.
+ * The "from" region may straddle more than one window, so toggle the GPIOs for
+ * each window region before reading its data.
  */
 static void gf_copy_from(struct map_info *map, void *to, unsigned long from, ssize_t len)
 {
 	struct async_state *state = gf_map_info_to_state(map);
 
-	gf_set_gpios(state, from);
+	int this_len;
 
-	/* BUG if operation crosses the win_size */
-	BUG_ON(!((from + len) % state->win_size <= (from + len)));
+	while (len) {
+		if ((from % state->win_size) + len > state->win_size)
+			this_len = state->win_size - (from % state->win_size);
+		else
+			this_len = len;
 
-	/* operation does not cross the win_size, so one shot it */
-	memcpy_fromio(to, map->virt + (from % state->win_size), len);
+		gf_set_gpios(state, from);
+		memcpy_fromio(to, map->virt + (from % state->win_size),
+			 this_len);
+		len -= this_len;
+		from += this_len;
+		to += this_len;
+	}
 }
 
 /**
@@ -147,13 +153,21 @@ static void gf_copy_to(struct map_info *map, unsigned long to,
 {
 	struct async_state *state = gf_map_info_to_state(map);
 
-	gf_set_gpios(state, to);
+	int this_len;
+
+	while (len) {
+		if ((to % state->win_size) + len > state->win_size)
+			this_len = state->win_size - (to % state->win_size);
+		else
+			this_len = len;
 
-	/* BUG if operation crosses the win_size */
-	BUG_ON(!((to + len) % state->win_size <= (to + len)));
+		gf_set_gpios(state, to);
+		memcpy_toio(map->virt + (to % state->win_size), from, len);
 
-	/* operation does not cross the win_size, so one shot it */
-	memcpy_toio(map->virt + (to % state->win_size), from, len);
+		len -= this_len;
+		to += this_len;
+		from += this_len;
+	}
 }
 
 static const char * const part_probe_types[] = {
diff --git a/drivers/mtd/maps/pcmciamtd.c b/drivers/mtd/maps/pcmciamtd.c
index a3cfad392ed6..af747af5eee9 100644
--- a/drivers/mtd/maps/pcmciamtd.c
+++ b/drivers/mtd/maps/pcmciamtd.c
@@ -89,7 +89,7 @@ static caddr_t remap_window(struct map_info *map, unsigned long to)
 
 	if (!pcmcia_dev_present(dev->p_dev)) {
 		pr_debug("device removed\n");
-		return 0;
+		return NULL;
 	}
 
 	offset = to & ~(dev->win_size-1);
diff --git a/drivers/mtd/maps/physmap_of.c b/drivers/mtd/maps/physmap_of.c
index 217c25d7381b..c1d21cb501ca 100644
--- a/drivers/mtd/maps/physmap_of.c
+++ b/drivers/mtd/maps/physmap_of.c
@@ -103,7 +103,7 @@ static struct mtd_info *obsolete_probe(struct platform_device *dev,
 		if (strcmp(of_probe, "ROM") != 0)
 			dev_warn(&dev->dev, "obsolete_probe: don't know probe "
 				 "type '%s', mapping as rom\n", of_probe);
-		return do_map_probe("mtd_rom", map);
+		return do_map_probe("map_rom", map);
 	}
 }
 
@@ -340,6 +340,10 @@ static struct of_device_id of_flash_match[] = {
 		.data           = (void *)"map_ram",
 	},
 	{
+		.compatible     = "mtd-rom",
+		.data           = (void *)"map_rom",
+	},
+	{
 		.type		= "rom",
 		.compatible	= "direct-mapped"
 	},
diff --git a/drivers/mtd/mtd_blkdevs.c b/drivers/mtd/mtd_blkdevs.c
index 43e30992a369..d08229eb44d8 100644
--- a/drivers/mtd/mtd_blkdevs.c
+++ b/drivers/mtd/mtd_blkdevs.c
@@ -417,6 +417,7 @@ int add_mtd_blktrans_dev(struct mtd_blktrans_dev *new)
 	blk_queue_logical_block_size(new->rq, tr->blksize);
 
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, new->rq);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, new->rq);
 
 	if (tr->discard) {
 		queue_flag_set_unlocked(QUEUE_FLAG_DISCARD, new->rq);
diff --git a/drivers/mtd/mtdchar.c b/drivers/mtd/mtdchar.c
index a0f54e80670c..53563955931b 100644
--- a/drivers/mtd/mtdchar.c
+++ b/drivers/mtd/mtdchar.c
@@ -549,6 +549,9 @@ static int mtdchar_blkpg_ioctl(struct mtd_info *mtd,
 		if (mtd_is_partition(mtd))
 			return -EINVAL;
 
+		/* Sanitize user input */
+		p.devname[BLKPG_DEVNAMELTH - 1] = '\0';
+
 		return mtd_add_partition(mtd, p.devname, p.start, p.length);
 
 	case BLKPG_DEL_PARTITION:
diff --git a/drivers/mtd/mtdcore.c b/drivers/mtd/mtdcore.c
index e4831b4159db..4c611871d7e6 100644
--- a/drivers/mtd/mtdcore.c
+++ b/drivers/mtd/mtdcore.c
@@ -105,12 +105,11 @@ static LIST_HEAD(mtd_notifiers);
  */
 static void mtd_release(struct device *dev)
 {
-	struct mtd_info __maybe_unused *mtd = dev_get_drvdata(dev);
+	struct mtd_info *mtd = dev_get_drvdata(dev);
 	dev_t index = MTD_DEVT(mtd->index);
 
-	/* remove /dev/mtdXro node if needed */
-	if (index)
-		device_destroy(&mtd_class, index + 1);
+	/* remove /dev/mtdXro node */
+	device_destroy(&mtd_class, index + 1);
 }
 
 static int mtd_cls_suspend(struct device *dev, pm_message_t state)
@@ -442,10 +441,8 @@ int add_mtd_device(struct mtd_info *mtd)
 	if (device_register(&mtd->dev) != 0)
 		goto fail_added;
 
-	if (MTD_DEVT(i))
-		device_create(&mtd_class, mtd->dev.parent,
-			      MTD_DEVT(i) + 1,
-			      NULL, "mtd%dro", i);
+	device_create(&mtd_class, mtd->dev.parent, MTD_DEVT(i) + 1, NULL,
+		      "mtd%dro", i);
 
 	pr_debug("mtd: Giving out device %d to %s\n", i, mtd->name);
 	/* No need to get a refcount on the module containing
@@ -778,7 +775,7 @@ EXPORT_SYMBOL_GPL(__put_mtd_device);
  */
 int mtd_erase(struct mtd_info *mtd, struct erase_info *instr)
 {
-	if (instr->addr > mtd->size || instr->len > mtd->size - instr->addr)
+	if (instr->addr >= mtd->size || instr->len > mtd->size - instr->addr)
 		return -EINVAL;
 	if (!(mtd->flags & MTD_WRITEABLE))
 		return -EROFS;
@@ -804,7 +801,7 @@ int mtd_point(struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen,
 		*phys = 0;
 	if (!mtd->_point)
 		return -EOPNOTSUPP;
-	if (from < 0 || from > mtd->size || len > mtd->size - from)
+	if (from < 0 || from >= mtd->size || len > mtd->size - from)
 		return -EINVAL;
 	if (!len)
 		return 0;
@@ -817,7 +814,7 @@ int mtd_unpoint(struct mtd_info *mtd, loff_t from, size_t len)
 {
 	if (!mtd->_point)
 		return -EOPNOTSUPP;
-	if (from < 0 || from > mtd->size || len > mtd->size - from)
+	if (from < 0 || from >= mtd->size || len > mtd->size - from)
 		return -EINVAL;
 	if (!len)
 		return 0;
@@ -835,7 +832,7 @@ unsigned long mtd_get_unmapped_area(struct mtd_info *mtd, unsigned long len,
 {
 	if (!mtd->_get_unmapped_area)
 		return -EOPNOTSUPP;
-	if (offset > mtd->size || len > mtd->size - offset)
+	if (offset >= mtd->size || len > mtd->size - offset)
 		return -EINVAL;
 	return mtd->_get_unmapped_area(mtd, len, offset, flags);
 }
@@ -846,7 +843,7 @@ int mtd_read(struct mtd_info *mtd, loff_t from, size_t len, size_t *retlen,
 {
 	int ret_code;
 	*retlen = 0;
-	if (from < 0 || from > mtd->size || len > mtd->size - from)
+	if (from < 0 || from >= mtd->size || len > mtd->size - from)
 		return -EINVAL;
 	if (!len)
 		return 0;
@@ -869,7 +866,7 @@ int mtd_write(struct mtd_info *mtd, loff_t to, size_t len, size_t *retlen,
 	      const u_char *buf)
 {
 	*retlen = 0;
-	if (to < 0 || to > mtd->size || len > mtd->size - to)
+	if (to < 0 || to >= mtd->size || len > mtd->size - to)
 		return -EINVAL;
 	if (!mtd->_write || !(mtd->flags & MTD_WRITEABLE))
 		return -EROFS;
@@ -892,7 +889,7 @@ int mtd_panic_write(struct mtd_info *mtd, loff_t to, size_t len, size_t *retlen,
 	*retlen = 0;
 	if (!mtd->_panic_write)
 		return -EOPNOTSUPP;
-	if (to < 0 || to > mtd->size || len > mtd->size - to)
+	if (to < 0 || to >= mtd->size || len > mtd->size - to)
 		return -EINVAL;
 	if (!(mtd->flags & MTD_WRITEABLE))
 		return -EROFS;
@@ -1011,7 +1008,7 @@ int mtd_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
 	if (!mtd->_lock)
 		return -EOPNOTSUPP;
-	if (ofs < 0 || ofs > mtd->size || len > mtd->size - ofs)
+	if (ofs < 0 || ofs >= mtd->size || len > mtd->size - ofs)
 		return -EINVAL;
 	if (!len)
 		return 0;
@@ -1023,7 +1020,7 @@ int mtd_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
 	if (!mtd->_unlock)
 		return -EOPNOTSUPP;
-	if (ofs < 0 || ofs > mtd->size || len > mtd->size - ofs)
+	if (ofs < 0 || ofs >= mtd->size || len > mtd->size - ofs)
 		return -EINVAL;
 	if (!len)
 		return 0;
@@ -1035,7 +1032,7 @@ int mtd_is_locked(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 {
 	if (!mtd->_is_locked)
 		return -EOPNOTSUPP;
-	if (ofs < 0 || ofs > mtd->size || len > mtd->size - ofs)
+	if (ofs < 0 || ofs >= mtd->size || len > mtd->size - ofs)
 		return -EINVAL;
 	if (!len)
 		return 0;
@@ -1045,7 +1042,7 @@ EXPORT_SYMBOL_GPL(mtd_is_locked);
 
 int mtd_block_isreserved(struct mtd_info *mtd, loff_t ofs)
 {
-	if (ofs < 0 || ofs > mtd->size)
+	if (ofs < 0 || ofs >= mtd->size)
 		return -EINVAL;
 	if (!mtd->_block_isreserved)
 		return 0;
@@ -1055,7 +1052,7 @@ EXPORT_SYMBOL_GPL(mtd_block_isreserved);
 
 int mtd_block_isbad(struct mtd_info *mtd, loff_t ofs)
 {
-	if (ofs < 0 || ofs > mtd->size)
+	if (ofs < 0 || ofs >= mtd->size)
 		return -EINVAL;
 	if (!mtd->_block_isbad)
 		return 0;
@@ -1067,7 +1064,7 @@ int mtd_block_markbad(struct mtd_info *mtd, loff_t ofs)
 {
 	if (!mtd->_block_markbad)
 		return -EOPNOTSUPP;
-	if (ofs < 0 || ofs > mtd->size)
+	if (ofs < 0 || ofs >= mtd->size)
 		return -EINVAL;
 	if (!(mtd->flags & MTD_WRITEABLE))
 		return -EROFS;
diff --git a/drivers/mtd/mtdswap.c b/drivers/mtd/mtdswap.c
index 8b33b26eb12b..fc8b3d16cce7 100644
--- a/drivers/mtd/mtdswap.c
+++ b/drivers/mtd/mtdswap.c
@@ -145,7 +145,7 @@ struct mtdswap_dev {
 struct mtdswap_oobdata {
 	__le16 magic;
 	__le32 count;
-} __attribute__((packed));
+} __packed;
 
 #define MTDSWAP_MAGIC_CLEAN	0x2095
 #define MTDSWAP_MAGIC_DIRTY	(MTDSWAP_MAGIC_CLEAN + 1)
@@ -1287,7 +1287,7 @@ static int mtdswap_show(struct seq_file *s, void *data)
 
 	seq_printf(s, "total erasures: %lu\n", sum);
 
-	seq_printf(s, "\n");
+	seq_puts(s, "\n");
 
 	seq_printf(s, "mtdswap_readsect count: %llu\n", d->sect_read_count);
 	seq_printf(s, "mtdswap_writesect count: %llu\n", d->sect_write_count);
@@ -1296,7 +1296,7 @@ static int mtdswap_show(struct seq_file *s, void *data)
 	seq_printf(s, "mtd write count: %llu\n", d->mtd_write_count);
 	seq_printf(s, "discarded pages count: %llu\n", d->discard_page_count);
 
-	seq_printf(s, "\n");
+	seq_puts(s, "\n");
 	seq_printf(s, "total pages: %u\n", pages);
 	seq_printf(s, "pages mapped: %u\n", mapped);
 
@@ -1474,7 +1474,7 @@ static void mtdswap_add_mtd(struct mtd_blktrans_ops *tr, struct mtd_info *mtd)
 	}
 
 	eblocks = mtd_div_by_eb(use_size, mtd);
-	use_size = eblocks * mtd->erasesize;
+	use_size = (uint64_t)eblocks * mtd->erasesize;
 	bad_blocks = mtdswap_badblocks(mtd, use_size);
 	eavailable = eblocks - bad_blocks;
 
diff --git a/drivers/mtd/nand/Kconfig b/drivers/mtd/nand/Kconfig
index f1cf503517fd..dd10646982ae 100644
--- a/drivers/mtd/nand/Kconfig
+++ b/drivers/mtd/nand/Kconfig
@@ -96,7 +96,7 @@ config MTD_NAND_OMAP2
 
 config MTD_NAND_OMAP_BCH
 	depends on MTD_NAND_OMAP2
-	tristate "Support hardware based BCH error correction"
+	bool "Support hardware based BCH error correction"
 	default n
 	select BCH
 	help
@@ -104,7 +104,10 @@ config MTD_NAND_OMAP_BCH
 	  locate and correct errors when using BCH ECC scheme. This offloads
 	  the cpu from doing ECC error searching and correction. However some
 	  legacy OMAP families like OMAP2xxx, OMAP3xxx do not have ELM engine
-	  so they should not enable this config symbol.
+	  so this is optional for them.
+
+config MTD_NAND_OMAP_BCH_BUILD
+	def_tristate MTD_NAND_OMAP2 && MTD_NAND_OMAP_BCH
 
 config MTD_NAND_IDS
 	tristate
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index a035e7cc6d46..9c847e469ca7 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -27,6 +27,7 @@ obj-$(CONFIG_MTD_NAND_NDFC)		+= ndfc.o
 obj-$(CONFIG_MTD_NAND_ATMEL)		+= atmel_nand.o
 obj-$(CONFIG_MTD_NAND_GPIO)		+= gpio.o
 obj-$(CONFIG_MTD_NAND_OMAP2) 		+= omap2.o
+obj-$(CONFIG_MTD_NAND_OMAP_BCH_BUILD)	+= omap_elm.o
 obj-$(CONFIG_MTD_NAND_CM_X270)		+= cmx270_nand.o
 obj-$(CONFIG_MTD_NAND_PXA3xx)		+= pxa3xx_nand.o
 obj-$(CONFIG_MTD_NAND_TMIO)		+= tmio_nand.o
diff --git a/drivers/mtd/nand/atmel_nand.c b/drivers/mtd/nand/atmel_nand.c
index e321c564ff05..19d1e9d17bf9 100644
--- a/drivers/mtd/nand/atmel_nand.c
+++ b/drivers/mtd/nand/atmel_nand.c
@@ -27,6 +27,7 @@
  *
  */
 
+#include <linux/clk.h>
 #include <linux/dma-mapping.h>
 #include <linux/slab.h>
 #include <linux/module.h>
@@ -96,6 +97,8 @@ struct atmel_nfc {
 	bool			use_nfc_sram;
 	bool			write_by_sram;
 
+	struct clk		*clk;
+
 	bool			is_initialized;
 	struct completion	comp_ready;
 	struct completion	comp_cmd_done;
@@ -128,8 +131,6 @@ struct atmel_nand_host {
 	u32			pmecc_lookup_table_offset_512;
 	u32			pmecc_lookup_table_offset_1024;
 
-	int			pmecc_bytes_per_sector;
-	int			pmecc_sector_number;
 	int			pmecc_degree;	/* Degree of remainders */
 	int			pmecc_cw_len;	/* Length of codeword */
 
@@ -841,7 +842,7 @@ static void pmecc_correct_data(struct mtd_info *mtd, uint8_t *buf, uint8_t *ecc,
 				pos, bit_pos, err_byte, *(buf + byte_pos));
 		} else {
 			/* Bit flip in OOB area */
-			tmp = sector_num * host->pmecc_bytes_per_sector
+			tmp = sector_num * nand_chip->ecc.bytes
 					+ (byte_pos - sector_size);
 			err_byte = ecc[tmp];
 			ecc[tmp] ^= (1 << bit_pos);
@@ -874,7 +875,7 @@ static int pmecc_correction(struct mtd_info *mtd, u32 pmecc_stat, uint8_t *buf,
 	return 0;
 
 normal_check:
-	for (i = 0; i < host->pmecc_sector_number; i++) {
+	for (i = 0; i < nand_chip->ecc.steps; i++) {
 		err_nbr = 0;
 		if (pmecc_stat & 0x1) {
 			buf_pos = buf + i * host->pmecc_sector_size;
@@ -890,7 +891,7 @@ normal_check:
 				return -EIO;
 			} else {
 				pmecc_correct_data(mtd, buf_pos, ecc, i,
-					host->pmecc_bytes_per_sector, err_nbr);
+					nand_chip->ecc.bytes, err_nbr);
 				mtd->ecc_stats.corrected += err_nbr;
 				total_err += err_nbr;
 			}
@@ -984,11 +985,11 @@ static int atmel_nand_pmecc_write_page(struct mtd_info *mtd,
 		cpu_relax();
 	}
 
-	for (i = 0; i < host->pmecc_sector_number; i++) {
-		for (j = 0; j < host->pmecc_bytes_per_sector; j++) {
+	for (i = 0; i < chip->ecc.steps; i++) {
+		for (j = 0; j < chip->ecc.bytes; j++) {
 			int pos;
 
-			pos = i * host->pmecc_bytes_per_sector + j;
+			pos = i * chip->ecc.bytes + j;
 			chip->oob_poi[eccpos[pos]] =
 				pmecc_readb_ecc_relaxed(host->ecc, i, j);
 		}
@@ -1031,7 +1032,7 @@ static void atmel_pmecc_core_init(struct mtd_info *mtd)
 	else if (host->pmecc_sector_size == 1024)
 		val |= PMECC_CFG_SECTOR1024;
 
-	switch (host->pmecc_sector_number) {
+	switch (nand_chip->ecc.steps) {
 	case 1:
 		val |= PMECC_CFG_PAGE_1SECTOR;
 		break;
@@ -1148,7 +1149,6 @@ static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 
 	host->ecc = devm_ioremap_resource(&pdev->dev, regs);
 	if (IS_ERR(host->ecc)) {
-		dev_err(host->dev, "ioremap failed\n");
 		err_no = PTR_ERR(host->ecc);
 		goto err;
 	}
@@ -1156,8 +1156,6 @@ static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 	regs_pmerr = platform_get_resource(pdev, IORESOURCE_MEM, 2);
 	host->pmerrloc_base = devm_ioremap_resource(&pdev->dev, regs_pmerr);
 	if (IS_ERR(host->pmerrloc_base)) {
-		dev_err(host->dev,
-			"Can not get I/O resource for PMECC ERRLOC controller!\n");
 		err_no = PTR_ERR(host->pmerrloc_base);
 		goto err;
 	}
@@ -1165,7 +1163,6 @@ static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 	regs_rom = platform_get_resource(pdev, IORESOURCE_MEM, 3);
 	host->pmecc_rom_base = devm_ioremap_resource(&pdev->dev, regs_rom);
 	if (IS_ERR(host->pmecc_rom_base)) {
-		dev_err(host->dev, "Can not get I/O resource for ROM!\n");
 		err_no = PTR_ERR(host->pmecc_rom_base);
 		goto err;
 	}
@@ -1174,22 +1171,29 @@ static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 
 	/* set ECC page size and oob layout */
 	switch (mtd->writesize) {
+	case 512:
+	case 1024:
 	case 2048:
+	case 4096:
+	case 8192:
+		if (sector_size > mtd->writesize) {
+			dev_err(host->dev, "pmecc sector size is bigger than the page size!\n");
+			err_no = -EINVAL;
+			goto err;
+		}
+
 		host->pmecc_degree = (sector_size == 512) ?
 			PMECC_GF_DIMENSION_13 : PMECC_GF_DIMENSION_14;
 		host->pmecc_cw_len = (1 << host->pmecc_degree) - 1;
-		host->pmecc_sector_number = mtd->writesize / sector_size;
-		host->pmecc_bytes_per_sector = pmecc_get_ecc_bytes(
-			cap, sector_size);
 		host->pmecc_alpha_to = pmecc_get_alpha_to(host);
 		host->pmecc_index_of = host->pmecc_rom_base +
 			host->pmecc_lookup_table_offset;
 
-		nand_chip->ecc.steps = host->pmecc_sector_number;
 		nand_chip->ecc.strength = cap;
-		nand_chip->ecc.bytes = host->pmecc_bytes_per_sector;
-		nand_chip->ecc.total = host->pmecc_bytes_per_sector *
-				       host->pmecc_sector_number;
+		nand_chip->ecc.bytes = pmecc_get_ecc_bytes(cap, sector_size);
+		nand_chip->ecc.steps = mtd->writesize / sector_size;
+		nand_chip->ecc.total = nand_chip->ecc.bytes *
+			nand_chip->ecc.steps;
 		if (nand_chip->ecc.total > mtd->oobsize - 2) {
 			dev_err(host->dev, "No room for ECC bytes\n");
 			err_no = -EINVAL;
@@ -1201,13 +1205,9 @@ static int atmel_pmecc_nand_init_params(struct platform_device *pdev,
 
 		nand_chip->ecc.layout = &atmel_pmecc_oobinfo;
 		break;
-	case 512:
-	case 1024:
-	case 4096:
-		/* TODO */
+	default:
 		dev_warn(host->dev,
 			"Unsupported page size for PMECC, use Software ECC\n");
-	default:
 		/* page size not handled by HW ECC */
 		/* switching back to soft ECC */
 		nand_chip->ecc.mode = NAND_ECC_SOFT;
@@ -1530,10 +1530,8 @@ static int atmel_hw_nand_init_params(struct platform_device *pdev,
 	}
 
 	host->ecc = devm_ioremap_resource(&pdev->dev, regs);
-	if (IS_ERR(host->ecc)) {
-		dev_err(host->dev, "ioremap failed\n");
+	if (IS_ERR(host->ecc))
 		return PTR_ERR(host->ecc);
-	}
 
 	/* ECC is calculated for the whole page (1 step) */
 	nand_chip->ecc.size = mtd->writesize;
@@ -1907,15 +1905,7 @@ static int nfc_sram_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	if (offset || (data_len < mtd->writesize))
 		return -EINVAL;
 
-	cfg = nfc_readl(host->nfc->hsmc_regs, CFG);
 	len = mtd->writesize;
-
-	if (unlikely(raw)) {
-		len += mtd->oobsize;
-		nfc_writel(host->nfc->hsmc_regs, CFG, cfg | NFC_CFG_WSPARE);
-	} else
-		nfc_writel(host->nfc->hsmc_regs, CFG, cfg & ~NFC_CFG_WSPARE);
-
 	/* Copy page data to sram that will write to nand via NFC */
 	if (use_dma) {
 		if (atmel_nand_dma_op(mtd, (void *)buf, len, 0) != 0)
@@ -1925,6 +1915,15 @@ static int nfc_sram_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 		memcpy32_toio(sram, buf, len);
 	}
 
+	cfg = nfc_readl(host->nfc->hsmc_regs, CFG);
+	if (unlikely(raw) && oob_required) {
+		memcpy32_toio(sram + len, chip->oob_poi, mtd->oobsize);
+		len += mtd->oobsize;
+		nfc_writel(host->nfc->hsmc_regs, CFG, cfg | NFC_CFG_WSPARE);
+	} else {
+		nfc_writel(host->nfc->hsmc_regs, CFG, cfg & ~NFC_CFG_WSPARE);
+	}
+
 	if (chip->ecc.mode == NAND_ECC_HW && host->has_pmecc)
 		/*
 		 * When use NFC sram, need set up PMECC before send
@@ -2040,7 +2039,6 @@ static int atmel_nand_probe(struct platform_device *pdev)
 	mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	host->io_base = devm_ioremap_resource(&pdev->dev, mem);
 	if (IS_ERR(host->io_base)) {
-		dev_err(&pdev->dev, "atmel_nand: ioremap resource failed\n");
 		res = PTR_ERR(host->io_base);
 		goto err_nand_ioremap;
 	}
@@ -2099,7 +2097,7 @@ static int atmel_nand_probe(struct platform_device *pdev)
 	}
 
 	nand_chip->ecc.mode = host->board.ecc_mode;
-	nand_chip->chip_delay = 20;		/* 20us command delay time */
+	nand_chip->chip_delay = 40;		/* 40us command delay time */
 
 	if (host->board.bus_width_16)	/* 16-bit bus width */
 		nand_chip->options |= NAND_BUSWIDTH_16;
@@ -2248,6 +2246,7 @@ static int atmel_nand_nfc_probe(struct platform_device *pdev)
 {
 	struct atmel_nfc *nfc = &nand_nfc;
 	struct resource *nfc_cmd_regs, *nfc_hsmc_regs, *nfc_sram;
+	int ret;
 
 	nfc_cmd_regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	nfc->base_cmd_regs = devm_ioremap_resource(&pdev->dev, nfc_cmd_regs);
@@ -2279,8 +2278,28 @@ static int atmel_nand_nfc_probe(struct platform_device *pdev)
 	nfc_writel(nfc->hsmc_regs, IDR, 0xffffffff);
 	nfc_readl(nfc->hsmc_regs, SR);	/* clear the NFC_SR */
 
+	nfc->clk = devm_clk_get(&pdev->dev, NULL);
+	if (!IS_ERR(nfc->clk)) {
+		ret = clk_prepare_enable(nfc->clk);
+		if (ret)
+			return ret;
+	} else {
+		dev_warn(&pdev->dev, "NFC clock missing, update your Device Tree");
+	}
+
 	nfc->is_initialized = true;
 	dev_info(&pdev->dev, "NFC is probed.\n");
+
+	return 0;
+}
+
+static int atmel_nand_nfc_remove(struct platform_device *pdev)
+{
+	struct atmel_nfc *nfc = &nand_nfc;
+
+	if (!IS_ERR(nfc->clk))
+		clk_disable_unprepare(nfc->clk);
+
 	return 0;
 }
 
@@ -2297,6 +2316,7 @@ static struct platform_driver atmel_nand_nfc_driver = {
 		.of_match_table = of_match_ptr(atmel_nand_nfc_match),
 	},
 	.probe = atmel_nand_nfc_probe,
+	.remove = atmel_nand_nfc_remove,
 };
 
 static struct platform_driver atmel_nand_driver = {
diff --git a/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c b/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
index b2ab373c9eef..592befc7ffa1 100644
--- a/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
+++ b/drivers/mtd/nand/bcm47xxnflash/ops_bcm4706.c
@@ -14,6 +14,7 @@
 #include <linux/module.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>
+#include <linux/delay.h>
 #include <linux/bcma/bcma.h>
 
 /* Broadcom uses 1'000'000 but it seems to be too many. Tests on WNDR4500 has
@@ -23,6 +24,8 @@
 #define NFLASH_SECTOR_SIZE		512
 
 #define NCTL_CMD0			0x00010000
+#define NCTL_COL			0x00020000	/* Update column with value from BCMA_CC_NFLASH_COL_ADDR */
+#define NCTL_ROW			0x00040000	/* Update row (page) with value from BCMA_CC_NFLASH_ROW_ADDR */
 #define NCTL_CMD1W			0x00080000
 #define NCTL_READ			0x00100000
 #define NCTL_WRITE			0x00200000
@@ -109,7 +112,7 @@ static void bcm47xxnflash_ops_bcm4706_read(struct mtd_info *mtd, uint8_t *buf,
 				b47n->curr_page_addr);
 
 		/* Prepare to read */
-		ctlcode = NCTL_CSA | NCTL_CMD1W | 0x00040000 | 0x00020000 |
+		ctlcode = NCTL_CSA | NCTL_CMD1W | NCTL_ROW | NCTL_COL |
 			  NCTL_CMD0;
 		ctlcode |= NAND_CMD_READSTART << 8;
 		if (bcm47xxnflash_ops_bcm4706_ctl_cmd(b47n->cc, ctlcode))
@@ -167,6 +170,26 @@ static void bcm47xxnflash_ops_bcm4706_write(struct mtd_info *mtd,
  * NAND chip ops
  **************************************************/
 
+static void bcm47xxnflash_ops_bcm4706_cmd_ctrl(struct mtd_info *mtd, int cmd,
+					       unsigned int ctrl)
+{
+	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
+	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+	u32 code = 0;
+
+	if (cmd == NAND_CMD_NONE)
+		return;
+
+	if (cmd & NAND_CTRL_CLE)
+		code = cmd | NCTL_CMD0;
+
+	/* nCS is not needed for reset command */
+	if (cmd != NAND_CMD_RESET)
+		code |= NCTL_CSA;
+
+	bcm47xxnflash_ops_bcm4706_ctl_cmd(b47n->cc, code);
+}
+
 /* Default nand_select_chip calls cmd_ctrl, which is not used in BCM4706 */
 static void bcm47xxnflash_ops_bcm4706_select_chip(struct mtd_info *mtd,
 						  int chip)
@@ -174,6 +197,14 @@ static void bcm47xxnflash_ops_bcm4706_select_chip(struct mtd_info *mtd,
 	return;
 }
 
+static int bcm47xxnflash_ops_bcm4706_dev_ready(struct mtd_info *mtd)
+{
+	struct nand_chip *nand_chip = (struct nand_chip *)mtd->priv;
+	struct bcm47xxnflash *b47n = (struct bcm47xxnflash *)nand_chip->priv;
+
+	return !!(bcma_cc_read32(b47n->cc, BCMA_CC_NFLASH_CTL) & NCTL_READY);
+}
+
 /*
  * Default nand_command and nand_command_lp don't match BCM4706 hardware layout.
  * For example, reading chip id is performed in a non-standard way.
@@ -198,7 +229,10 @@ static void bcm47xxnflash_ops_bcm4706_cmdfunc(struct mtd_info *mtd,
 
 	switch (command) {
 	case NAND_CMD_RESET:
-		pr_warn("Chip reset not implemented yet\n");
+		nand_chip->cmd_ctrl(mtd, command, NAND_CTRL_CLE);
+
+		ndelay(100);
+		nand_wait_ready(mtd);
 		break;
 	case NAND_CMD_READID:
 		ctlcode = NCTL_CSA | 0x01000000 | NCTL_CMD1W | NCTL_CMD0;
@@ -242,7 +276,7 @@ static void bcm47xxnflash_ops_bcm4706_cmdfunc(struct mtd_info *mtd,
 	case NAND_CMD_ERASE1:
 		bcma_cc_write32(cc, BCMA_CC_NFLASH_ROW_ADDR,
 				b47n->curr_page_addr);
-		ctlcode = 0x00040000 | NCTL_CMD1W | NCTL_CMD0 |
+		ctlcode = NCTL_ROW | NCTL_CMD1W | NCTL_CMD0 |
 			  NAND_CMD_ERASE1 | (NAND_CMD_ERASE2 << 8);
 		if (bcm47xxnflash_ops_bcm4706_ctl_cmd(cc, ctlcode))
 			pr_err("ERASE1 failed\n");
@@ -257,13 +291,13 @@ static void bcm47xxnflash_ops_bcm4706_cmdfunc(struct mtd_info *mtd,
 				b47n->curr_page_addr);
 
 		/* Prepare to write */
-		ctlcode = 0x40000000 | 0x00040000 | 0x00020000 | 0x00010000;
+		ctlcode = 0x40000000 | NCTL_ROW | NCTL_COL | NCTL_CMD0;
 		ctlcode |= NAND_CMD_SEQIN;
 		if (bcm47xxnflash_ops_bcm4706_ctl_cmd(cc, ctlcode))
 			pr_err("SEQIN failed\n");
 		break;
 	case NAND_CMD_PAGEPROG:
-		if (bcm47xxnflash_ops_bcm4706_ctl_cmd(cc, 0x00010000 |
+		if (bcm47xxnflash_ops_bcm4706_ctl_cmd(cc, NCTL_CMD0 |
 							  NAND_CMD_PAGEPROG))
 			pr_err("PAGEPROG failed\n");
 		if (bcm47xxnflash_ops_bcm4706_poll(cc))
@@ -341,6 +375,7 @@ static void bcm47xxnflash_ops_bcm4706_write_buf(struct mtd_info *mtd,
 
 int bcm47xxnflash_ops_bcm4706_init(struct bcm47xxnflash *b47n)
 {
+	struct nand_chip *nand_chip = (struct nand_chip *)&b47n->nand_chip;
 	int err;
 	u32 freq;
 	u16 clock;
@@ -351,10 +386,14 @@ int bcm47xxnflash_ops_bcm4706_init(struct bcm47xxnflash *b47n)
 	u32 val;
 
 	b47n->nand_chip.select_chip = bcm47xxnflash_ops_bcm4706_select_chip;
+	nand_chip->cmd_ctrl = bcm47xxnflash_ops_bcm4706_cmd_ctrl;
+	nand_chip->dev_ready = bcm47xxnflash_ops_bcm4706_dev_ready;
 	b47n->nand_chip.cmdfunc = bcm47xxnflash_ops_bcm4706_cmdfunc;
 	b47n->nand_chip.read_byte = bcm47xxnflash_ops_bcm4706_read_byte;
 	b47n->nand_chip.read_buf = bcm47xxnflash_ops_bcm4706_read_buf;
 	b47n->nand_chip.write_buf = bcm47xxnflash_ops_bcm4706_write_buf;
+
+	nand_chip->chip_delay = 50;
 	b47n->nand_chip.bbt_options = NAND_BBT_USE_FLASH;
 	b47n->nand_chip.ecc.mode = NAND_ECC_NONE; /* TODO: implement ECC */
 
@@ -364,11 +403,13 @@ int bcm47xxnflash_ops_bcm4706_init(struct bcm47xxnflash *b47n)
 
 	/* Configure wait counters */
 	if (b47n->cc->status & BCMA_CC_CHIPST_4706_PKG_OPTION) {
-		freq = 100000000;
+		/* 400 MHz */
+		freq = 400000000 / 4;
 	} else {
 		freq = bcma_chipco_pll_read(b47n->cc, 4);
-		freq = (freq * 0xFFF) >> 3;
-		freq = (freq * 25000000) >> 3;
+		freq = (freq & 0xFFF) >> 3;
+		/* Fixed reference clock 25 MHz and m = 2 */
+		freq = (freq * 25000000 / 2) / 4;
 	}
 	clock = freq / 1000000;
 	w0 = bcm47xxnflash_ops_bcm4706_ns_to_cycle(15, clock);
diff --git a/drivers/mtd/nand/denali.c b/drivers/mtd/nand/denali.c
index 0b071a3136a2..b3b7ca1bafb8 100644
--- a/drivers/mtd/nand/denali.c
+++ b/drivers/mtd/nand/denali.c
@@ -29,20 +29,23 @@
 
 MODULE_LICENSE("GPL");
 
-/* We define a module parameter that allows the user to override
+/*
+ * We define a module parameter that allows the user to override
  * the hardware and decide what timing mode should be used.
  */
 #define NAND_DEFAULT_TIMINGS	-1
 
 static int onfi_timing_mode = NAND_DEFAULT_TIMINGS;
 module_param(onfi_timing_mode, int, S_IRUGO);
-MODULE_PARM_DESC(onfi_timing_mode, "Overrides default ONFI setting."
-			" -1 indicates use default timings");
+MODULE_PARM_DESC(onfi_timing_mode,
+	   "Overrides default ONFI setting. -1 indicates use default timings");
 
 #define DENALI_NAND_NAME    "denali-nand"
 
-/* We define a macro here that combines all interrupts this driver uses into
- * a single constant value, for convenience. */
+/*
+ * We define a macro here that combines all interrupts this driver uses into
+ * a single constant value, for convenience.
+ */
 #define DENALI_IRQ_ALL	(INTR_STATUS__DMA_CMD_COMP | \
 			INTR_STATUS__ECC_TRANSACTION_DONE | \
 			INTR_STATUS__ECC_ERR | \
@@ -54,26 +57,34 @@ MODULE_PARM_DESC(onfi_timing_mode, "Overrides default ONFI setting."
 			INTR_STATUS__RST_COMP | \
 			INTR_STATUS__ERASE_COMP)
 
-/* indicates whether or not the internal value for the flash bank is
- * valid or not */
+/*
+ * indicates whether or not the internal value for the flash bank is
+ * valid or not
+ */
 #define CHIP_SELECT_INVALID	-1
 
 #define SUPPORT_8BITECC		1
 
-/* This macro divides two integers and rounds fractional values up
- * to the nearest integer value. */
+/*
+ * This macro divides two integers and rounds fractional values up
+ * to the nearest integer value.
+ */
 #define CEIL_DIV(X, Y) (((X)%(Y)) ? ((X)/(Y)+1) : ((X)/(Y)))
 
-/* this macro allows us to convert from an MTD structure to our own
+/*
+ * this macro allows us to convert from an MTD structure to our own
  * device context (denali) structure.
  */
 #define mtd_to_denali(m) container_of(m, struct denali_nand_info, mtd)
 
-/* These constants are defined by the driver to enable common driver
- * configuration options. */
+/*
+ * These constants are defined by the driver to enable common driver
+ * configuration options.
+ */
 #define SPARE_ACCESS		0x41
 #define MAIN_ACCESS		0x42
 #define MAIN_SPARE_ACCESS	0x43
+#define PIPELINE_ACCESS		0x2000
 
 #define DENALI_READ	0
 #define DENALI_WRITE	0x100
@@ -83,8 +94,10 @@ MODULE_PARM_DESC(onfi_timing_mode, "Overrides default ONFI setting."
 #define ADDR_CYCLE	1
 #define STATUS_CYCLE	2
 
-/* this is a helper macro that allows us to
- * format the bank into the proper bits for the controller */
+/*
+ * this is a helper macro that allows us to
+ * format the bank into the proper bits for the controller
+ */
 #define BANK(x) ((x) << 24)
 
 /* forward declarations */
@@ -95,12 +108,12 @@ static void denali_irq_enable(struct denali_nand_info *denali,
 							uint32_t int_mask);
 static uint32_t read_interrupt_status(struct denali_nand_info *denali);
 
-/* Certain operations for the denali NAND controller use
- * an indexed mode to read/write data. The operation is
- * performed by writing the address value of the command
- * to the device memory followed by the data. This function
+/*
+ * Certain operations for the denali NAND controller use an indexed mode to
+ * read/write data. The operation is performed by writing the address value
+ * of the command to the device memory followed by the data. This function
  * abstracts this common operation.
-*/
+ */
 static void index_addr(struct denali_nand_info *denali,
 				uint32_t address, uint32_t data)
 {
@@ -116,8 +129,10 @@ static void index_addr_read_data(struct denali_nand_info *denali,
 	*pdata = ioread32(denali->flash_mem + 0x10);
 }
 
-/* We need to buffer some data for some of the NAND core routines.
- * The operations manage buffering that data. */
+/*
+ * We need to buffer some data for some of the NAND core routines.
+ * The operations manage buffering that data.
+ */
 static void reset_buf(struct denali_nand_info *denali)
 {
 	denali->buf.head = denali->buf.tail = 0;
@@ -131,7 +146,7 @@ static void write_byte_to_buf(struct denali_nand_info *denali, uint8_t byte)
 /* reads the status of the device */
 static void read_status(struct denali_nand_info *denali)
 {
-	uint32_t cmd = 0x0;
+	uint32_t cmd;
 
 	/* initialize the data buffer to store status */
 	reset_buf(denali);
@@ -146,9 +161,8 @@ static void read_status(struct denali_nand_info *denali)
 /* resets a specific device connected to the core */
 static void reset_bank(struct denali_nand_info *denali)
 {
-	uint32_t irq_status = 0;
-	uint32_t irq_mask = INTR_STATUS__RST_COMP |
-			    INTR_STATUS__TIME_OUT;
+	uint32_t irq_status;
+	uint32_t irq_mask = INTR_STATUS__RST_COMP | INTR_STATUS__TIME_OUT;
 
 	clear_interrupts(denali);
 
@@ -163,19 +177,18 @@ static void reset_bank(struct denali_nand_info *denali)
 /* Reset the flash controller */
 static uint16_t denali_nand_reset(struct denali_nand_info *denali)
 {
-	uint32_t i;
+	int i;
 
 	dev_dbg(denali->dev, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
+		__FILE__, __LINE__, __func__);
 
-	for (i = 0 ; i < denali->max_banks; i++)
+	for (i = 0; i < denali->max_banks; i++)
 		iowrite32(INTR_STATUS__RST_COMP | INTR_STATUS__TIME_OUT,
 		denali->flash_reg + INTR_STATUS(i));
 
-	for (i = 0 ; i < denali->max_banks; i++) {
+	for (i = 0; i < denali->max_banks; i++) {
 		iowrite32(1 << i, denali->flash_reg + DEVICE_RESET);
-		while (!(ioread32(denali->flash_reg +
-				INTR_STATUS(i)) &
+		while (!(ioread32(denali->flash_reg + INTR_STATUS(i)) &
 			(INTR_STATUS__RST_COMP | INTR_STATUS__TIME_OUT)))
 			cpu_relax();
 		if (ioread32(denali->flash_reg + INTR_STATUS(i)) &
@@ -186,12 +199,13 @@ static uint16_t denali_nand_reset(struct denali_nand_info *denali)
 
 	for (i = 0; i < denali->max_banks; i++)
 		iowrite32(INTR_STATUS__RST_COMP | INTR_STATUS__TIME_OUT,
-			denali->flash_reg + INTR_STATUS(i));
+			  denali->flash_reg + INTR_STATUS(i));
 
 	return PASS;
 }
 
-/* this routine calculates the ONFI timing values for a given mode and
+/*
+ * this routine calculates the ONFI timing values for a given mode and
  * programs the clocking register accordingly. The mode is determined by
  * the get_onfi_nand_para routine.
  */
@@ -219,7 +233,7 @@ static void nand_onfi_timing_set(struct denali_nand_info *denali,
 	uint16_t addr_2_data, re_2_we, re_2_re, we_2_re, cs_cnt;
 
 	dev_dbg(denali->dev, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
+		__FILE__, __LINE__, __func__);
 
 	en_lo = CEIL_DIV(Trp[mode], CLK_X);
 	en_hi = CEIL_DIV(Treh[mode], CLK_X);
@@ -239,9 +253,8 @@ static void nand_onfi_timing_set(struct denali_nand_info *denali,
 
 		data_invalid_rloh = (en_lo + en_hi) * CLK_X + Trloh[mode];
 
-		data_invalid =
-		    data_invalid_rhoh <
-		    data_invalid_rloh ? data_invalid_rhoh : data_invalid_rloh;
+		data_invalid = data_invalid_rhoh < data_invalid_rloh ?
+					data_invalid_rhoh : data_invalid_rloh;
 
 		dv_window = data_invalid - Trea[mode];
 
@@ -251,12 +264,12 @@ static void nand_onfi_timing_set(struct denali_nand_info *denali,
 
 	acc_clks = CEIL_DIV(Trea[mode], CLK_X);
 
-	while (((acc_clks * CLK_X) - Trea[mode]) < 3)
+	while (acc_clks * CLK_X - Trea[mode] < 3)
 		acc_clks++;
 
-	if ((data_invalid - acc_clks * CLK_X) < 2)
+	if (data_invalid - acc_clks * CLK_X < 2)
 		dev_warn(denali->dev, "%s, Line %d: Warning!\n",
-			__FILE__, __LINE__);
+			 __FILE__, __LINE__);
 
 	addr_2_data = CEIL_DIV(Tadl[mode], CLK_X);
 	re_2_we = CEIL_DIV(Trhw[mode], CLK_X);
@@ -269,7 +282,7 @@ static void nand_onfi_timing_set(struct denali_nand_info *denali,
 		cs_cnt = 1;
 
 	if (Tcea[mode]) {
-		while (((cs_cnt * CLK_X) + Trea[mode]) < Tcea[mode])
+		while (cs_cnt * CLK_X + Trea[mode] < Tcea[mode])
 			cs_cnt++;
 	}
 
@@ -279,8 +292,8 @@ static void nand_onfi_timing_set(struct denali_nand_info *denali,
 #endif
 
 	/* Sighting 3462430: Temporary hack for MT29F128G08CJABAWP:B */
-	if ((ioread32(denali->flash_reg + MANUFACTURER_ID) == 0) &&
-		(ioread32(denali->flash_reg + DEVICE_ID) == 0x88))
+	if (ioread32(denali->flash_reg + MANUFACTURER_ID) == 0 &&
+		ioread32(denali->flash_reg + DEVICE_ID) == 0x88)
 		acc_clks = 6;
 
 	iowrite32(acc_clks, denali->flash_reg + ACC_CLKS);
@@ -297,9 +310,11 @@ static void nand_onfi_timing_set(struct denali_nand_info *denali,
 static uint16_t get_onfi_nand_para(struct denali_nand_info *denali)
 {
 	int i;
-	/* we needn't to do a reset here because driver has already
+
+	/*
+	 * we needn't to do a reset here because driver has already
 	 * reset all the banks before
-	 * */
+	 */
 	if (!(ioread32(denali->flash_reg + ONFI_TIMING_MODE) &
 		ONFI_TIMING_MODE__VALUE))
 		return FAIL;
@@ -312,8 +327,10 @@ static uint16_t get_onfi_nand_para(struct denali_nand_info *denali)
 
 	nand_onfi_timing_set(denali, i);
 
-	/* By now, all the ONFI devices we know support the page cache */
-	/* rw feature. So here we enable the pipeline_rw_ahead feature */
+	/*
+	 * By now, all the ONFI devices we know support the page cache
+	 * rw feature. So here we enable the pipeline_rw_ahead feature
+	 */
 	/* iowrite32(1, denali->flash_reg + CACHE_WRITE_ENABLE); */
 	/* iowrite32(1, denali->flash_reg + CACHE_READ_ENABLE);  */
 
@@ -339,8 +356,10 @@ static void get_toshiba_nand_para(struct denali_nand_info *denali)
 {
 	uint32_t tmp;
 
-	/* Workaround to fix a controller bug which reports a wrong */
-	/* spare area size for some kind of Toshiba NAND device */
+	/*
+	 * Workaround to fix a controller bug which reports a wrong
+	 * spare area size for some kind of Toshiba NAND device
+	 */
 	if ((ioread32(denali->flash_reg + DEVICE_MAIN_AREA_SIZE) == 4096) &&
 		(ioread32(denali->flash_reg + DEVICE_SPARE_AREA_SIZE) == 64)) {
 		iowrite32(216, denali->flash_reg + DEVICE_SPARE_AREA_SIZE);
@@ -384,13 +403,14 @@ static void get_hynix_nand_para(struct denali_nand_info *denali,
 		break;
 	default:
 		dev_warn(denali->dev,
-			"Spectra: Unknown Hynix NAND (Device ID: 0x%x)."
-			"Will use default parameter values instead.\n",
-			device_id);
+			 "Spectra: Unknown Hynix NAND (Device ID: 0x%x).\n"
+			 "Will use default parameter values instead.\n",
+			 device_id);
 	}
 }
 
-/* determines how many NAND chips are connected to the controller. Note for
+/*
+ * determines how many NAND chips are connected to the controller. Note for
  * Intel CE4100 devices we don't support more than one device.
  */
 static void find_valid_banks(struct denali_nand_info *denali)
@@ -400,10 +420,9 @@ static void find_valid_banks(struct denali_nand_info *denali)
 
 	denali->total_used_banks = 1;
 	for (i = 0; i < denali->max_banks; i++) {
-		index_addr(denali, (uint32_t)(MODE_11 | (i << 24) | 0), 0x90);
-		index_addr(denali, (uint32_t)(MODE_11 | (i << 24) | 1), 0);
-		index_addr_read_data(denali,
-				(uint32_t)(MODE_11 | (i << 24) | 2), &id[i]);
+		index_addr(denali, MODE_11 | (i << 24) | 0, 0x90);
+		index_addr(denali, MODE_11 | (i << 24) | 1, 0);
+		index_addr_read_data(denali, MODE_11 | (i << 24) | 2, &id[i]);
 
 		dev_dbg(denali->dev,
 			"Return 1st ID for bank[%d]: %x\n", i, id[i]);
@@ -420,14 +439,14 @@ static void find_valid_banks(struct denali_nand_info *denali)
 	}
 
 	if (denali->platform == INTEL_CE4100) {
-		/* Platform limitations of the CE4100 device limit
+		/*
+		 * Platform limitations of the CE4100 device limit
 		 * users to a single chip solution for NAND.
 		 * Multichip support is not enabled.
 		 */
 		if (denali->total_used_banks != 1) {
 			dev_err(denali->dev,
-					"Sorry, Intel CE4100 only supports "
-					"a single NAND device.\n");
+				"Sorry, Intel CE4100 only supports a single NAND device.\n");
 			BUG();
 		}
 	}
@@ -448,12 +467,13 @@ static void detect_max_banks(struct denali_nand_info *denali)
 
 static void detect_partition_feature(struct denali_nand_info *denali)
 {
-	/* For MRST platform, denali->fwblks represent the
+	/*
+	 * For MRST platform, denali->fwblks represent the
 	 * number of blocks firmware is taken,
 	 * FW is in protect partition and MTD driver has no
 	 * permission to access it. So let driver know how many
 	 * blocks it can't touch.
-	 * */
+	 */
 	if (ioread32(denali->flash_reg + FEATURES) & FEATURES__PARTITION) {
 		if ((ioread32(denali->flash_reg + PERM_SRC_ID(1)) &
 			PERM_SRC_ID__SRCID) == SPECTRA_PARTITION_ID) {
@@ -464,30 +484,32 @@ static void detect_partition_feature(struct denali_nand_info *denali)
 			    +
 			    (ioread32(denali->flash_reg + MIN_BLK_ADDR(1)) &
 			    MIN_BLK_ADDR__VALUE);
-		} else
+		} else {
 			denali->fwblks = SPECTRA_START_BLOCK;
-	} else
+		}
+	} else {
 		denali->fwblks = SPECTRA_START_BLOCK;
+	}
 }
 
 static uint16_t denali_nand_timing_set(struct denali_nand_info *denali)
 {
 	uint16_t status = PASS;
 	uint32_t id_bytes[8], addr;
-	uint8_t i, maf_id, device_id;
+	uint8_t maf_id, device_id;
+	int i;
 
-	dev_dbg(denali->dev,
-			"%s, Line %d, Function: %s\n",
+	dev_dbg(denali->dev, "%s, Line %d, Function: %s\n",
 			__FILE__, __LINE__, __func__);
 
-	/* Use read id method to get device ID and other
-	 * params. For some NAND chips, controller can't
-	 * report the correct device ID by reading from
-	 * DEVICE_ID register
-	 * */
-	addr = (uint32_t)MODE_11 | BANK(denali->flash_bank);
-	index_addr(denali, (uint32_t)addr | 0, 0x90);
-	index_addr(denali, (uint32_t)addr | 1, 0);
+	/*
+	 * Use read id method to get device ID and other params.
+	 * For some NAND chips, controller can't report the correct
+	 * device ID by reading from DEVICE_ID register
+	 */
+	addr = MODE_11 | BANK(denali->flash_bank);
+	index_addr(denali, addr | 0, 0x90);
+	index_addr(denali, addr | 1, 0);
 	for (i = 0; i < 8; i++)
 		index_addr_read_data(denali, addr | 2, &id_bytes[i]);
 	maf_id = id_bytes[0];
@@ -506,7 +528,7 @@ static uint16_t denali_nand_timing_set(struct denali_nand_info *denali)
 	}
 
 	dev_info(denali->dev,
-			"Dump timing register values:"
+			"Dump timing register values:\n"
 			"acc_clks: %d, re_2_we: %d, re_2_re: %d\n"
 			"we_2_re: %d, addr_2_data: %d, rdwr_en_lo_cnt: %d\n"
 			"rdwr_en_hi_cnt: %d, cs_setup_cnt: %d\n",
@@ -523,7 +545,8 @@ static uint16_t denali_nand_timing_set(struct denali_nand_info *denali)
 
 	detect_partition_feature(denali);
 
-	/* If the user specified to override the default timings
+	/*
+	 * If the user specified to override the default timings
 	 * with a specific ONFI mode, we apply those changes here.
 	 */
 	if (onfi_timing_mode != NAND_DEFAULT_TIMINGS)
@@ -536,7 +559,7 @@ static void denali_set_intr_modes(struct denali_nand_info *denali,
 					uint16_t INT_ENABLE)
 {
 	dev_dbg(denali->dev, "%s, Line %d, Function: %s\n",
-		       __FILE__, __LINE__, __func__);
+		__FILE__, __LINE__, __func__);
 
 	if (INT_ENABLE)
 		iowrite32(1, denali->flash_reg + GLOBAL_INT_ENABLE);
@@ -544,17 +567,18 @@ static void denali_set_intr_modes(struct denali_nand_info *denali,
 		iowrite32(0, denali->flash_reg + GLOBAL_INT_ENABLE);
 }
 
-/* validation function to verify that the controlling software is making
+/*
+ * validation function to verify that the controlling software is making
  * a valid request
  */
 static inline bool is_flash_bank_valid(int flash_bank)
 {
-	return (flash_bank >= 0 && flash_bank < 4);
+	return flash_bank >= 0 && flash_bank < 4;
 }
 
 static void denali_irq_init(struct denali_nand_info *denali)
 {
-	uint32_t int_mask = 0;
+	uint32_t int_mask;
 	int i;
 
 	/* Disable global interrupts */
@@ -584,7 +608,8 @@ static void denali_irq_enable(struct denali_nand_info *denali,
 		iowrite32(int_mask, denali->flash_reg + INTR_EN(i));
 }
 
-/* This function only returns when an interrupt that this driver cares about
+/*
+ * This function only returns when an interrupt that this driver cares about
  * occurs. This is to reduce the overhead of servicing interrupts
  */
 static inline uint32_t denali_irq_detected(struct denali_nand_info *denali)
@@ -596,7 +621,7 @@ static inline uint32_t denali_irq_detected(struct denali_nand_info *denali)
 static inline void clear_interrupt(struct denali_nand_info *denali,
 							uint32_t irq_mask)
 {
-	uint32_t intr_status_reg = 0;
+	uint32_t intr_status_reg;
 
 	intr_status_reg = INTR_STATUS(denali->flash_bank);
 
@@ -605,7 +630,8 @@ static inline void clear_interrupt(struct denali_nand_info *denali,
 
 static void clear_interrupts(struct denali_nand_info *denali)
 {
-	uint32_t status = 0x0;
+	uint32_t status;
+
 	spin_lock_irq(&denali->irq_lock);
 
 	status = read_interrupt_status(denali);
@@ -617,38 +643,40 @@ static void clear_interrupts(struct denali_nand_info *denali)
 
 static uint32_t read_interrupt_status(struct denali_nand_info *denali)
 {
-	uint32_t intr_status_reg = 0;
+	uint32_t intr_status_reg;
 
 	intr_status_reg = INTR_STATUS(denali->flash_bank);
 
 	return ioread32(denali->flash_reg + intr_status_reg);
 }
 
-/* This is the interrupt service routine. It handles all interrupts
- * sent to this device. Note that on CE4100, this is a shared
- * interrupt.
+/*
+ * This is the interrupt service routine. It handles all interrupts
+ * sent to this device. Note that on CE4100, this is a shared interrupt.
  */
 static irqreturn_t denali_isr(int irq, void *dev_id)
 {
 	struct denali_nand_info *denali = dev_id;
-	uint32_t irq_status = 0x0;
+	uint32_t irq_status;
 	irqreturn_t result = IRQ_NONE;
 
 	spin_lock(&denali->irq_lock);
 
-	/* check to see if a valid NAND chip has
-	 * been selected.
-	 */
+	/* check to see if a valid NAND chip has been selected. */
 	if (is_flash_bank_valid(denali->flash_bank)) {
-		/* check to see if controller generated
-		 * the interrupt, since this is a shared interrupt */
+		/*
+		 * check to see if controller generated the interrupt,
+		 * since this is a shared interrupt
+		 */
 		irq_status = denali_irq_detected(denali);
 		if (irq_status != 0) {
 			/* handle interrupt */
 			/* first acknowledge it */
 			clear_interrupt(denali, irq_status);
-			/* store the status in the device context for someone
-			   to read */
+			/*
+			 * store the status in the device context for someone
+			 * to read
+			 */
 			denali->irq_status |= irq_status;
 			/* notify anyone who cares that it happened */
 			complete(&denali->complete);
@@ -663,9 +691,8 @@ static irqreturn_t denali_isr(int irq, void *dev_id)
 
 static uint32_t wait_for_irq(struct denali_nand_info *denali, uint32_t irq_mask)
 {
-	unsigned long comp_res = 0;
-	uint32_t intr_status = 0;
-	bool retry = false;
+	unsigned long comp_res;
+	uint32_t intr_status;
 	unsigned long timeout = msecs_to_jiffies(1000);
 
 	do {
@@ -679,12 +706,13 @@ static uint32_t wait_for_irq(struct denali_nand_info *denali, uint32_t irq_mask)
 			spin_unlock_irq(&denali->irq_lock);
 			/* our interrupt was detected */
 			break;
-		} else {
-			/* these are not the interrupts you are looking for -
-			 * need to wait again */
-			spin_unlock_irq(&denali->irq_lock);
-			retry = true;
 		}
+
+		/*
+		 * these are not the interrupts you are looking for -
+		 * need to wait again
+		 */
+		spin_unlock_irq(&denali->irq_lock);
 	} while (comp_res != 0);
 
 	if (comp_res == 0) {
@@ -697,12 +725,14 @@ static uint32_t wait_for_irq(struct denali_nand_info *denali, uint32_t irq_mask)
 	return intr_status;
 }
 
-/* This helper function setups the registers for ECC and whether or not
- * the spare area will be transferred. */
+/*
+ * This helper function setups the registers for ECC and whether or not
+ * the spare area will be transferred.
+ */
 static void setup_ecc_for_xfer(struct denali_nand_info *denali, bool ecc_en,
 				bool transfer_spare)
 {
-	int ecc_en_flag = 0, transfer_spare_flag = 0;
+	int ecc_en_flag, transfer_spare_flag;
 
 	/* set ECC, transfer spare bits if needed */
 	ecc_en_flag = ecc_en ? ECC_ENABLE__FLAG : 0;
@@ -710,22 +740,20 @@ static void setup_ecc_for_xfer(struct denali_nand_info *denali, bool ecc_en,
 
 	/* Enable spare area/ECC per user's request. */
 	iowrite32(ecc_en_flag, denali->flash_reg + ECC_ENABLE);
-	iowrite32(transfer_spare_flag,
-			denali->flash_reg + TRANSFER_SPARE_REG);
+	iowrite32(transfer_spare_flag, denali->flash_reg + TRANSFER_SPARE_REG);
 }
 
-/* sends a pipeline command operation to the controller. See the Denali NAND
+/*
+ * sends a pipeline command operation to the controller. See the Denali NAND
  * controller's user guide for more information (section 4.2.3.6).
  */
 static int denali_send_pipeline_cmd(struct denali_nand_info *denali,
-							bool ecc_en,
-							bool transfer_spare,
-							int access_type,
-							int op)
+				    bool ecc_en, bool transfer_spare,
+				    int access_type, int op)
 {
 	int status = PASS;
-	uint32_t addr = 0x0, cmd = 0x0, page_count = 1, irq_status = 0,
-		 irq_mask = 0;
+	uint32_t page_count = 1;
+	uint32_t addr, cmd, irq_status, irq_mask;
 
 	if (op == DENALI_READ)
 		irq_mask = INTR_STATUS__LOAD_COMP;
@@ -736,7 +764,6 @@ static int denali_send_pipeline_cmd(struct denali_nand_info *denali,
 
 	setup_ecc_for_xfer(denali, ecc_en, transfer_spare);
 
-	/* clear interrupts */
 	clear_interrupts(denali);
 
 	addr = BANK(denali->flash_bank) | denali->page;
@@ -747,37 +774,38 @@ static int denali_send_pipeline_cmd(struct denali_nand_info *denali,
 	} else if (op == DENALI_WRITE && access_type == SPARE_ACCESS) {
 		/* read spare area */
 		cmd = MODE_10 | addr;
-		index_addr(denali, (uint32_t)cmd, access_type);
+		index_addr(denali, cmd, access_type);
 
 		cmd = MODE_01 | addr;
 		iowrite32(cmd, denali->flash_mem);
 	} else if (op == DENALI_READ) {
 		/* setup page read request for access type */
 		cmd = MODE_10 | addr;
-		index_addr(denali, (uint32_t)cmd, access_type);
+		index_addr(denali, cmd, access_type);
 
-		/* page 33 of the NAND controller spec indicates we should not
-		   use the pipeline commands in Spare area only mode. So we
-		   don't.
+		/*
+		 * page 33 of the NAND controller spec indicates we should not
+		 * use the pipeline commands in Spare area only mode.
+		 * So we don't.
 		 */
 		if (access_type == SPARE_ACCESS) {
 			cmd = MODE_01 | addr;
 			iowrite32(cmd, denali->flash_mem);
 		} else {
-			index_addr(denali, (uint32_t)cmd,
-					0x2000 | op | page_count);
+			index_addr(denali, cmd,
+					PIPELINE_ACCESS | op | page_count);
 
-			/* wait for command to be accepted
+			/*
+			 * wait for command to be accepted
 			 * can always use status0 bit as the
-			 * mask is identical for each
-			 * bank. */
+			 * mask is identical for each bank.
+			 */
 			irq_status = wait_for_irq(denali, irq_mask);
 
 			if (irq_status == 0) {
 				dev_err(denali->dev,
-						"cmd, page, addr on timeout "
-						"(0x%x, 0x%x, 0x%x)\n",
-						cmd, denali->page, addr);
+					"cmd, page, addr on timeout (0x%x, 0x%x, 0x%x)\n",
+					cmd, denali->page, addr);
 				status = FAIL;
 			} else {
 				cmd = MODE_01 | addr;
@@ -790,51 +818,51 @@ static int denali_send_pipeline_cmd(struct denali_nand_info *denali,
 
 /* helper function that simply writes a buffer to the flash */
 static int write_data_to_flash_mem(struct denali_nand_info *denali,
-							const uint8_t *buf,
-							int len)
+				   const uint8_t *buf, int len)
 {
-	uint32_t i = 0, *buf32;
+	uint32_t *buf32;
+	int i;
 
-	/* verify that the len is a multiple of 4. see comment in
-	 * read_data_from_flash_mem() */
+	/*
+	 * verify that the len is a multiple of 4.
+	 * see comment in read_data_from_flash_mem()
+	 */
 	BUG_ON((len % 4) != 0);
 
 	/* write the data to the flash memory */
 	buf32 = (uint32_t *)buf;
 	for (i = 0; i < len / 4; i++)
 		iowrite32(*buf32++, denali->flash_mem + 0x10);
-	return i*4; /* intent is to return the number of bytes read */
+	return i * 4; /* intent is to return the number of bytes read */
 }
 
 /* helper function that simply reads a buffer from the flash */
 static int read_data_from_flash_mem(struct denali_nand_info *denali,
-								uint8_t *buf,
-								int len)
+				    uint8_t *buf, int len)
 {
-	uint32_t i = 0, *buf32;
-
-	/* we assume that len will be a multiple of 4, if not
-	 * it would be nice to know about it ASAP rather than
-	 * have random failures...
-	 * This assumption is based on the fact that this
-	 * function is designed to be used to read flash pages,
-	 * which are typically multiples of 4...
-	 */
+	uint32_t *buf32;
+	int i;
 
+	/*
+	 * we assume that len will be a multiple of 4, if not it would be nice
+	 * to know about it ASAP rather than have random failures...
+	 * This assumption is based on the fact that this function is designed
+	 * to be used to read flash pages, which are typically multiples of 4.
+	 */
 	BUG_ON((len % 4) != 0);
 
 	/* transfer the data from the flash */
 	buf32 = (uint32_t *)buf;
 	for (i = 0; i < len / 4; i++)
 		*buf32++ = ioread32(denali->flash_mem + 0x10);
-	return i*4; /* intent is to return the number of bytes read */
+	return i * 4; /* intent is to return the number of bytes read */
 }
 
 /* writes OOB data to the device */
 static int write_oob_data(struct mtd_info *mtd, uint8_t *buf, int page)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
-	uint32_t irq_status = 0;
+	uint32_t irq_status;
 	uint32_t irq_mask = INTR_STATUS__PROGRAM_COMP |
 						INTR_STATUS__PROGRAM_FAIL;
 	int status = 0;
@@ -863,8 +891,8 @@ static int write_oob_data(struct mtd_info *mtd, uint8_t *buf, int page)
 static void read_oob_data(struct mtd_info *mtd, uint8_t *buf, int page)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
-	uint32_t irq_mask = INTR_STATUS__LOAD_COMP,
-			 irq_status = 0, addr = 0x0, cmd = 0x0;
+	uint32_t irq_mask = INTR_STATUS__LOAD_COMP;
+	uint32_t irq_status, addr, cmd;
 
 	denali->page = page;
 
@@ -872,16 +900,19 @@ static void read_oob_data(struct mtd_info *mtd, uint8_t *buf, int page)
 							DENALI_READ) == PASS) {
 		read_data_from_flash_mem(denali, buf, mtd->oobsize);
 
-		/* wait for command to be accepted
-		 * can always use status0 bit as the mask is identical for each
-		 * bank. */
+		/*
+		 * wait for command to be accepted
+		 * can always use status0 bit as the
+		 * mask is identical for each bank.
+		 */
 		irq_status = wait_for_irq(denali, irq_mask);
 
 		if (irq_status == 0)
 			dev_err(denali->dev, "page on OOB timeout %d\n",
 					denali->page);
 
-		/* We set the device back to MAIN_ACCESS here as I observed
+		/*
+		 * We set the device back to MAIN_ACCESS here as I observed
 		 * instability with the controller if you do a block erase
 		 * and the last transaction was a SPARE_ACCESS. Block erase
 		 * is reliable (according to the MTD test infrastructure)
@@ -889,16 +920,18 @@ static void read_oob_data(struct mtd_info *mtd, uint8_t *buf, int page)
 		 */
 		addr = BANK(denali->flash_bank) | denali->page;
 		cmd = MODE_10 | addr;
-		index_addr(denali, (uint32_t)cmd, MAIN_ACCESS);
+		index_addr(denali, cmd, MAIN_ACCESS);
 	}
 }
 
-/* this function examines buffers to see if they contain data that
+/*
+ * this function examines buffers to see if they contain data that
  * indicate that the buffer is part of an erased region of flash.
  */
 static bool is_erased(uint8_t *buf, int len)
 {
-	int i = 0;
+	int i;
+
 	for (i = 0; i < len; i++)
 		if (buf[i] != 0xFF)
 			return false;
@@ -921,9 +954,8 @@ static bool handle_ecc(struct denali_nand_info *denali, uint8_t *buf,
 
 	if (irq_status & INTR_STATUS__ECC_ERR) {
 		/* read the ECC errors. we'll ignore them for now */
-		uint32_t err_address = 0, err_correction_info = 0;
-		uint32_t err_byte = 0, err_sector = 0, err_device = 0;
-		uint32_t err_correction_value = 0;
+		uint32_t err_address, err_correction_info, err_byte,
+			 err_sector, err_device, err_correction_value;
 		denali_set_intr_modes(denali, false);
 
 		do {
@@ -939,15 +971,17 @@ static bool handle_ecc(struct denali_nand_info *denali, uint8_t *buf,
 			err_device = ECC_ERR_DEVICE(err_correction_info);
 
 			if (ECC_ERROR_CORRECTABLE(err_correction_info)) {
-				/* If err_byte is larger than ECC_SECTOR_SIZE,
+				/*
+				 * If err_byte is larger than ECC_SECTOR_SIZE,
 				 * means error happened in OOB, so we ignore
 				 * it. It's no need for us to correct it
 				 * err_device is represented the NAND error
 				 * bits are happened in if there are more
 				 * than one NAND connected.
-				 * */
+				 */
 				if (err_byte < ECC_SECTOR_SIZE) {
 					int offset;
+
 					offset = (err_sector *
 							ECC_SECTOR_SIZE +
 							err_byte) *
@@ -959,17 +993,19 @@ static bool handle_ecc(struct denali_nand_info *denali, uint8_t *buf,
 					bitflips++;
 				}
 			} else {
-				/* if the error is not correctable, need to
+				/*
+				 * if the error is not correctable, need to
 				 * look at the page to see if it is an erased
 				 * page. if so, then it's not a real ECC error
-				 * */
+				 */
 				check_erased_page = true;
 			}
 		} while (!ECC_LAST_ERR(err_correction_info));
-		/* Once handle all ecc errors, controller will triger
+		/*
+		 * Once handle all ecc errors, controller will triger
 		 * a ECC_TRANSACTION_DONE interrupt, so here just wait
 		 * for a while for this interrupt
-		 * */
+		 */
 		while (!(read_interrupt_status(denali) &
 				INTR_STATUS__ECC_TRANSACTION_DONE))
 			cpu_relax();
@@ -983,21 +1019,16 @@ static bool handle_ecc(struct denali_nand_info *denali, uint8_t *buf,
 /* programs the controller to either enable/disable DMA transfers */
 static void denali_enable_dma(struct denali_nand_info *denali, bool en)
 {
-	uint32_t reg_val = 0x0;
-
-	if (en)
-		reg_val = DMA_ENABLE__FLAG;
-
-	iowrite32(reg_val, denali->flash_reg + DMA_ENABLE);
+	iowrite32(en ? DMA_ENABLE__FLAG : 0, denali->flash_reg + DMA_ENABLE);
 	ioread32(denali->flash_reg + DMA_ENABLE);
 }
 
 /* setups the HW to perform the data DMA */
 static void denali_setup_dma(struct denali_nand_info *denali, int op)
 {
-	uint32_t mode = 0x0;
+	uint32_t mode;
 	const int page_count = 1;
-	dma_addr_t addr = denali->buf.dma_buf;
+	uint32_t addr = denali->buf.dma_buf;
 
 	mode = MODE_10 | BANK(denali->flash_bank);
 
@@ -1007,31 +1038,31 @@ static void denali_setup_dma(struct denali_nand_info *denali, int op)
 	index_addr(denali, mode | denali->page, 0x2000 | op | page_count);
 
 	/* 2. set memory high address bits 23:8 */
-	index_addr(denali, mode | ((uint16_t)(addr >> 16) << 8), 0x2200);
+	index_addr(denali, mode | ((addr >> 16) << 8), 0x2200);
 
 	/* 3. set memory low address bits 23:8 */
-	index_addr(denali, mode | ((uint16_t)addr << 8), 0x2300);
+	index_addr(denali, mode | ((addr & 0xff) << 8), 0x2300);
 
-	/* 4.  interrupt when complete, burst len = 64 bytes*/
+	/* 4. interrupt when complete, burst len = 64 bytes */
 	index_addr(denali, mode | 0x14000, 0x2400);
 }
 
-/* writes a page. user specifies type, and this function handles the
- * configuration details. */
+/*
+ * writes a page. user specifies type, and this function handles the
+ * configuration details.
+ */
 static int write_page(struct mtd_info *mtd, struct nand_chip *chip,
 			const uint8_t *buf, bool raw_xfer)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
-
 	dma_addr_t addr = denali->buf.dma_buf;
 	size_t size = denali->mtd.writesize + denali->mtd.oobsize;
-
-	uint32_t irq_status = 0;
+	uint32_t irq_status;
 	uint32_t irq_mask = INTR_STATUS__DMA_CMD_COMP |
 						INTR_STATUS__PROGRAM_FAIL;
 
-	/* if it is a raw xfer, we want to disable ecc, and send
-	 * the spare area.
+	/*
+	 * if it is a raw xfer, we want to disable ecc and send the spare area.
 	 * !raw_xfer - enable ecc
 	 * raw_xfer - transfer spare
 	 */
@@ -1058,12 +1089,9 @@ static int write_page(struct mtd_info *mtd, struct nand_chip *chip,
 	irq_status = wait_for_irq(denali, irq_mask);
 
 	if (irq_status == 0) {
-		dev_err(denali->dev,
-				"timeout on write_page (type = %d)\n",
-				raw_xfer);
-		denali->status =
-			(irq_status & INTR_STATUS__PROGRAM_FAIL) ?
-			NAND_STATUS_FAIL : PASS;
+		dev_err(denali->dev, "timeout on write_page (type = %d)\n",
+			raw_xfer);
+		denali->status = NAND_STATUS_FAIL;
 	}
 
 	denali_enable_dma(denali, false);
@@ -1074,27 +1102,33 @@ static int write_page(struct mtd_info *mtd, struct nand_chip *chip,
 
 /* NAND core entry points */
 
-/* this is the callback that the NAND core calls to write a page. Since
+/*
+ * this is the callback that the NAND core calls to write a page. Since
  * writing a page with ECC or without is similar, all the work is done
  * by write_page above.
- * */
+ */
 static int denali_write_page(struct mtd_info *mtd, struct nand_chip *chip,
 				const uint8_t *buf, int oob_required)
 {
-	/* for regular page writes, we let HW handle all the ECC
-	 * data written to the device. */
+	/*
+	 * for regular page writes, we let HW handle all the ECC
+	 * data written to the device.
+	 */
 	return write_page(mtd, chip, buf, false);
 }
 
-/* This is the callback that the NAND core calls to write a page without ECC.
+/*
+ * This is the callback that the NAND core calls to write a page without ECC.
  * raw access is similar to ECC page writes, so all the work is done in the
  * write_page() function above.
  */
 static int denali_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 					const uint8_t *buf, int oob_required)
 {
-	/* for raw page writes, we want to disable ECC and simply write
-	   whatever data is in the buffer. */
+	/*
+	 * for raw page writes, we want to disable ECC and simply write
+	 * whatever data is in the buffer.
+	 */
 	return write_page(mtd, chip, buf, true);
 }
 
@@ -1121,15 +1155,15 @@ static int denali_read_page(struct mtd_info *mtd, struct nand_chip *chip,
 	dma_addr_t addr = denali->buf.dma_buf;
 	size_t size = denali->mtd.writesize + denali->mtd.oobsize;
 
-	uint32_t irq_status = 0;
+	uint32_t irq_status;
 	uint32_t irq_mask = INTR_STATUS__ECC_TRANSACTION_DONE |
 			    INTR_STATUS__ECC_ERR;
 	bool check_erased_page = false;
 
 	if (page != denali->page) {
-		dev_err(denali->dev, "IN %s: page %d is not"
-				" equal to denali->page %d, investigate!!",
-				__func__, page, denali->page);
+		dev_err(denali->dev,
+			"IN %s: page %d is not equal to denali->page %d",
+			__func__, page, denali->page);
 		BUG();
 	}
 
@@ -1169,17 +1203,14 @@ static int denali_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 				uint8_t *buf, int oob_required, int page)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
-
 	dma_addr_t addr = denali->buf.dma_buf;
 	size_t size = denali->mtd.writesize + denali->mtd.oobsize;
-
-	uint32_t irq_status = 0;
 	uint32_t irq_mask = INTR_STATUS__DMA_CMD_COMP;
 
 	if (page != denali->page) {
-		dev_err(denali->dev, "IN %s: page %d is not"
-				" equal to denali->page %d, investigate!!",
-				__func__, page, denali->page);
+		dev_err(denali->dev,
+			"IN %s: page %d is not equal to denali->page %d",
+			__func__, page, denali->page);
 		BUG();
 	}
 
@@ -1192,7 +1223,7 @@ static int denali_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 	denali_setup_dma(denali, DENALI_READ);
 
 	/* wait for operation to complete */
-	irq_status = wait_for_irq(denali, irq_mask);
+	wait_for_irq(denali, irq_mask);
 
 	dma_sync_single_for_cpu(denali->dev, addr, size, DMA_FROM_DEVICE);
 
@@ -1228,6 +1259,7 @@ static int denali_waitfunc(struct mtd_info *mtd, struct nand_chip *chip)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 	int status = denali->status;
+
 	denali->status = 0;
 
 	return status;
@@ -1237,20 +1269,19 @@ static int denali_erase(struct mtd_info *mtd, int page)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
 
-	uint32_t cmd = 0x0, irq_status = 0;
+	uint32_t cmd, irq_status;
 
-	/* clear interrupts */
 	clear_interrupts(denali);
 
 	/* setup page read request for access type */
 	cmd = MODE_10 | BANK(denali->flash_bank) | page;
-	index_addr(denali, (uint32_t)cmd, 0x1);
+	index_addr(denali, cmd, 0x1);
 
 	/* wait for erase to complete or failure to occur */
 	irq_status = wait_for_irq(denali, INTR_STATUS__ERASE_COMP |
 					INTR_STATUS__ERASE_FAIL);
 
-	return (irq_status & INTR_STATUS__ERASE_FAIL) ? NAND_STATUS_FAIL : PASS;
+	return irq_status & INTR_STATUS__ERASE_FAIL ? NAND_STATUS_FAIL : PASS;
 }
 
 static void denali_cmdfunc(struct mtd_info *mtd, unsigned int cmd, int col,
@@ -1269,17 +1300,16 @@ static void denali_cmdfunc(struct mtd_info *mtd, unsigned int cmd, int col,
 	case NAND_CMD_READID:
 	case NAND_CMD_PARAM:
 		reset_buf(denali);
-		/*sometimes ManufactureId read from register is not right
+		/*
+		 * sometimes ManufactureId read from register is not right
 		 * e.g. some of Micron MT29F32G08QAA MLC NAND chips
 		 * So here we send READID cmd to NAND insteand
-		 * */
-		addr = (uint32_t)MODE_11 | BANK(denali->flash_bank);
-		index_addr(denali, (uint32_t)addr | 0, 0x90);
-		index_addr(denali, (uint32_t)addr | 1, 0);
+		 */
+		addr = MODE_11 | BANK(denali->flash_bank);
+		index_addr(denali, addr | 0, 0x90);
+		index_addr(denali, addr | 1, 0);
 		for (i = 0; i < 8; i++) {
-			index_addr_read_data(denali,
-						(uint32_t)addr | 2,
-						&id);
+			index_addr_read_data(denali, addr | 2, &id);
 			write_byte_to_buf(denali, id);
 		}
 		break;
@@ -1304,8 +1334,8 @@ static int denali_ecc_calculate(struct mtd_info *mtd, const uint8_t *data,
 				uint8_t *ecc_code)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
-	dev_err(denali->dev,
-			"denali_ecc_calculate called unexpectedly\n");
+
+	dev_err(denali->dev, "denali_ecc_calculate called unexpectedly\n");
 	BUG();
 	return -EIO;
 }
@@ -1314,8 +1344,8 @@ static int denali_ecc_correct(struct mtd_info *mtd, uint8_t *data,
 				uint8_t *read_ecc, uint8_t *calc_ecc)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
-	dev_err(denali->dev,
-			"denali_ecc_correct called unexpectedly\n");
+
+	dev_err(denali->dev, "denali_ecc_correct called unexpectedly\n");
 	BUG();
 	return -EIO;
 }
@@ -1323,8 +1353,8 @@ static int denali_ecc_correct(struct mtd_info *mtd, uint8_t *data,
 static void denali_ecc_hwctl(struct mtd_info *mtd, int mode)
 {
 	struct denali_nand_info *denali = mtd_to_denali(mtd);
-	dev_err(denali->dev,
-			"denali_ecc_hwctl called unexpectedly\n");
+
+	dev_err(denali->dev, "denali_ecc_hwctl called unexpectedly\n");
 	BUG();
 }
 /* end NAND core entry points */
@@ -1332,11 +1362,12 @@ static void denali_ecc_hwctl(struct mtd_info *mtd, int mode)
 /* Initialization code to bring the device up to a known good state */
 static void denali_hw_init(struct denali_nand_info *denali)
 {
-	/* tell driver how many bit controller will skip before
+	/*
+	 * tell driver how many bit controller will skip before
 	 * writing ECC code in OOB, this register may be already
 	 * set by firmware. So we read this value out.
 	 * if this value is 0, just let it be.
-	 * */
+	 */
 	denali->bbtskipbytes = ioread32(denali->flash_reg +
 						SPARE_AREA_SKIP_BYTES);
 	detect_max_banks(denali);
@@ -1354,10 +1385,11 @@ static void denali_hw_init(struct denali_nand_info *denali)
 	denali_irq_init(denali);
 }
 
-/* Althogh controller spec said SLC ECC is forceb to be 4bit,
+/*
+ * Althogh controller spec said SLC ECC is forceb to be 4bit,
  * but denali controller in MRST only support 15bit and 8bit ECC
  * correction
- * */
+ */
 #define ECC_8BITS	14
 static struct nand_ecclayout nand_8bit_oob = {
 	.eccbytes = 14,
@@ -1397,13 +1429,16 @@ static void denali_drv_init(struct denali_nand_info *denali)
 	denali->idx = 0;
 
 	/* setup interrupt handler */
-	/* the completion object will be used to notify
-	 * the callee that the interrupt is done */
+	/*
+	 * the completion object will be used to notify
+	 * the callee that the interrupt is done
+	 */
 	init_completion(&denali->complete);
 
-	/* the spinlock will be used to synchronize the ISR
-	 * with any element that might be access shared
-	 * data (interrupt status) */
+	/*
+	 * the spinlock will be used to synchronize the ISR with any
+	 * element that might be access shared data (interrupt status)
+	 */
 	spin_lock_init(&denali->irq_lock);
 
 	/* indicate that MTD has not selected a valid bank yet */
@@ -1418,7 +1453,8 @@ int denali_init(struct denali_nand_info *denali)
 	int ret;
 
 	if (denali->platform == INTEL_CE4100) {
-		/* Due to a silicon limitation, we can only support
+		/*
+		 * Due to a silicon limitation, we can only support
 		 * ONFI timing mode 1 and below.
 		 */
 		if (onfi_timing_mode < -1 || onfi_timing_mode > 1) {
@@ -1437,8 +1473,10 @@ int denali_init(struct denali_nand_info *denali)
 	denali_hw_init(denali);
 	denali_drv_init(denali);
 
-	/* denali_isr register is done after all the hardware
-	 * initilization is finished*/
+	/*
+	 * denali_isr register is done after all the hardware
+	 * initilization is finished
+	 */
 	if (request_irq(denali->irq, denali_isr, IRQF_SHARED,
 			DENALI_NAND_NAME, denali)) {
 		pr_err("Spectra: Unable to allocate IRQ\n");
@@ -1457,9 +1495,11 @@ int denali_init(struct denali_nand_info *denali)
 	denali->nand.read_byte = denali_read_byte;
 	denali->nand.waitfunc = denali_waitfunc;
 
-	/* scan for NAND devices attached to the controller
+	/*
+	 * scan for NAND devices attached to the controller
 	 * this is the first stage in a two step process to register
-	 * with the nand subsystem */
+	 * with the nand subsystem
+	 */
 	if (nand_scan_ident(&denali->mtd, denali->max_banks, NULL)) {
 		ret = -ENXIO;
 		goto failed_req_irq;
@@ -1491,10 +1531,10 @@ int denali_init(struct denali_nand_info *denali)
 		goto failed_req_irq;
 	}
 
-	/* support for multi nand
-	 * MTD known nothing about multi nand,
-	 * so we should tell it the real pagesize
-	 * and anything necessery
+	/*
+	 * support for multi nand
+	 * MTD known nothing about multi nand, so we should tell it
+	 * the real pagesize and anything necessery
 	 */
 	denali->devnum = ioread32(denali->flash_reg + DEVICES_CONNECTED);
 	denali->nand.chipsize <<= (denali->devnum - 1);
@@ -1510,9 +1550,11 @@ int denali_init(struct denali_nand_info *denali)
 	denali->mtd.size = denali->nand.numchips * denali->nand.chipsize;
 	denali->bbtskipbytes *= denali->devnum;
 
-	/* second stage of the NAND scan
+	/*
+	 * second stage of the NAND scan
 	 * this stage requires information regarding ECC and
-	 * bad block management. */
+	 * bad block management.
+	 */
 
 	/* Bad block management */
 	denali->nand.bbt_td = &bbt_main_descr;
@@ -1523,7 +1565,8 @@ int denali_init(struct denali_nand_info *denali)
 	denali->nand.options |= NAND_SKIP_BBTSCAN;
 	denali->nand.ecc.mode = NAND_ECC_HW_SYNDROME;
 
-	/* Denali Controller only support 15bit and 8bit ECC in MRST,
+	/*
+	 * Denali Controller only support 15bit and 8bit ECC in MRST,
 	 * so just let controller do 15bit ECC for MLC and 8bit ECC for
 	 * SLC if possible.
 	 * */
@@ -1539,8 +1582,7 @@ int denali_init(struct denali_nand_info *denali)
 	} else if (denali->mtd.oobsize < (denali->bbtskipbytes +
 			ECC_8BITS * (denali->mtd.writesize /
 			ECC_SECTOR_SIZE))) {
-		pr_err("Your NAND chip OOB is not large enough to \
-				contain 8bit ECC correction codes");
+		pr_err("Your NAND chip OOB is not large enough to contain 8bit ECC correction codes");
 		goto failed_req_irq;
 	} else {
 		denali->nand.ecc.strength = 8;
@@ -1559,18 +1601,19 @@ int denali_init(struct denali_nand_info *denali)
 		denali->mtd.oobsize - denali->nand.ecc.layout->eccbytes -
 		denali->bbtskipbytes;
 
-	/* Let driver know the total blocks number and
-	 * how many blocks contained by each nand chip.
-	 * blksperchip will help driver to know how many
-	 * blocks is taken by FW.
-	 * */
-	denali->totalblks = denali->mtd.size >>
-				denali->nand.phys_erase_shift;
+	/*
+	 * Let driver know the total blocks number and how many blocks
+	 * contained by each nand chip. blksperchip will help driver to
+	 * know how many blocks is taken by FW.
+	 */
+	denali->totalblks = denali->mtd.size >> denali->nand.phys_erase_shift;
 	denali->blksperchip = denali->totalblks / denali->nand.numchips;
 
-	/* These functions are required by the NAND core framework, otherwise,
+	/*
+	 * These functions are required by the NAND core framework, otherwise,
 	 * the NAND core will assert. However, we don't need them, so we'll stub
-	 * them out. */
+	 * them out.
+	 */
 	denali->nand.ecc.calculate = denali_ecc_calculate;
 	denali->nand.ecc.correct = denali_ecc_correct;
 	denali->nand.ecc.hwctl = denali_ecc_hwctl;
@@ -1610,7 +1653,7 @@ void denali_remove(struct denali_nand_info *denali)
 {
 	denali_irq_cleanup(denali->irq, denali);
 	dma_unmap_single(denali->dev, denali->buf.dma_buf,
-			denali->mtd.writesize + denali->mtd.oobsize,
-			DMA_BIDIRECTIONAL);
+			 denali->mtd.writesize + denali->mtd.oobsize,
+			 DMA_BIDIRECTIONAL);
 }
 EXPORT_SYMBOL(denali_remove);
diff --git a/drivers/mtd/nand/denali.h b/drivers/mtd/nand/denali.h
index 966817462421..145bf88930e8 100644
--- a/drivers/mtd/nand/denali.h
+++ b/drivers/mtd/nand/denali.h
@@ -17,6 +17,9 @@
  *
  */
 
+#ifndef __DENALI_H__
+#define __DENALI_H__
+
 #include <linux/mtd/nand.h>
 
 #define DEVICE_RESET				0x0
@@ -400,28 +403,6 @@
 #define ONFI_BLOOM_TIME         1
 #define MODE5_WORKAROUND        0
 
-/* lld_nand.h */
-/*
- * NAND Flash Controller Device Driver
- * Copyright (c) 2009, Intel Corporation and its suppliers.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
- *
- */
-
-#ifndef _LLD_NAND_
-#define _LLD_NAND_
 
 #define MODE_00    0x00000000
 #define MODE_01    0x04000000
@@ -499,4 +480,4 @@ struct denali_nand_info {
 extern int denali_init(struct denali_nand_info *denali);
 extern void denali_remove(struct denali_nand_info *denali);
 
-#endif /*_LLD_NAND_*/
+#endif /* __DENALI_H__ */
diff --git a/drivers/mtd/nand/fsmc_nand.c b/drivers/mtd/nand/fsmc_nand.c
index 1550692973dc..7a915870d9d6 100644
--- a/drivers/mtd/nand/fsmc_nand.c
+++ b/drivers/mtd/nand/fsmc_nand.c
@@ -605,7 +605,7 @@ static int dma_xfer(struct fsmc_nand_data *host, void *buffer, int len,
 	wait_for_completion_timeout(&host->dma_access_complete,
 				msecs_to_jiffies(3000));
 	if (ret <= 0) {
-		chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(chan);
 		dev_err(host->dev, "wait_for_completion_timeout\n");
 		if (!ret)
 			ret = -ETIMEDOUT;
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index d8cdf06343fb..5b5c62712814 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -982,6 +982,15 @@ int nand_unlock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 
 	chip->select_chip(mtd, chipnr);
 
+	/*
+	 * Reset the chip.
+	 * If we want to check the WP through READ STATUS and check the bit 7
+	 * we must reset the chip
+	 * some operation can also clear the bit 7 of status register
+	 * eg. erase/program a locked block
+	 */
+	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+
 	/* Check, if it is write protected */
 	if (nand_check_wp(mtd)) {
 		pr_debug("%s: device is write protected!\n",
@@ -1032,6 +1041,15 @@ int nand_lock(struct mtd_info *mtd, loff_t ofs, uint64_t len)
 
 	chip->select_chip(mtd, chipnr);
 
+	/*
+	 * Reset the chip.
+	 * If we want to check the WP through READ STATUS and check the bit 7
+	 * we must reset the chip
+	 * some operation can also clear the bit 7 of status register
+	 * eg. erase/program a locked block
+	 */
+	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+
 	/* Check, if it is write protected */
 	if (nand_check_wp(mtd)) {
 		pr_debug("%s: device is write protected!\n",
@@ -2391,8 +2409,8 @@ static int nand_do_write_ops(struct mtd_info *mtd, loff_t to,
 	blockmask = (1 << (chip->phys_erase_shift - chip->page_shift)) - 1;
 
 	/* Invalidate the page cache, when we write to the cached page */
-	if (to <= (chip->pagebuf << chip->page_shift) &&
-	    (chip->pagebuf << chip->page_shift) < (to + ops->len))
+	if (to <= ((loff_t)chip->pagebuf << chip->page_shift) &&
+	    ((loff_t)chip->pagebuf << chip->page_shift) < (to + ops->len))
 		chip->pagebuf = -1;
 
 	/* Don't allow multipage oob writes with offset */
@@ -3576,6 +3594,8 @@ static bool find_full_id_nand(struct mtd_info *mtd, struct nand_chip *chip,
 		chip->options |= type->options;
 		chip->ecc_strength_ds = NAND_ECC_STRENGTH(type);
 		chip->ecc_step_ds = NAND_ECC_STEP(type);
+		chip->onfi_timing_mode_default =
+					type->onfi_timing_mode_default;
 
 		*busw = type->options & NAND_BUSWIDTH_16;
 
@@ -3918,8 +3938,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 	case NAND_ECC_HW_OOB_FIRST:
 		/* Similar to NAND_ECC_HW, but a separate read_page handle */
 		if (!ecc->calculate || !ecc->correct || !ecc->hwctl) {
-			pr_warn("No ECC functions supplied; "
-				   "hardware ECC not possible\n");
+			pr_warn("No ECC functions supplied; hardware ECC not possible\n");
 			BUG();
 		}
 		if (!ecc->read_page)
@@ -3950,8 +3969,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 		     ecc->read_page == nand_read_page_hwecc ||
 		     !ecc->write_page ||
 		     ecc->write_page == nand_write_page_hwecc)) {
-			pr_warn("No ECC functions supplied; "
-				   "hardware ECC not possible\n");
+			pr_warn("No ECC functions supplied; hardware ECC not possible\n");
 			BUG();
 		}
 		/* Use standard syndrome read/write page function? */
@@ -3975,9 +3993,8 @@ int nand_scan_tail(struct mtd_info *mtd)
 			}
 			break;
 		}
-		pr_warn("%d byte HW ECC not possible on "
-			   "%d byte page size, fallback to SW ECC\n",
-			   ecc->size, mtd->writesize);
+		pr_warn("%d byte HW ECC not possible on %d byte page size, fallback to SW ECC\n",
+			ecc->size, mtd->writesize);
 		ecc->mode = NAND_ECC_SOFT;
 
 	case NAND_ECC_SOFT:
@@ -4030,8 +4047,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 		break;
 
 	case NAND_ECC_NONE:
-		pr_warn("NAND_ECC_NONE selected by board driver. "
-			   "This is not recommended!\n");
+		pr_warn("NAND_ECC_NONE selected by board driver. This is not recommended!\n");
 		ecc->read_page = nand_read_page_raw;
 		ecc->write_page = nand_write_page_raw;
 		ecc->read_oob = nand_read_oob_std;
diff --git a/drivers/mtd/nand/nand_bbt.c b/drivers/mtd/nand/nand_bbt.c
index 443fa82cde6a..9bb8453d224e 100644
--- a/drivers/mtd/nand/nand_bbt.c
+++ b/drivers/mtd/nand/nand_bbt.c
@@ -201,12 +201,12 @@ static int read_bbt(struct mtd_info *mtd, uint8_t *buf, int page, int num,
 		res = mtd_read(mtd, from, len, &retlen, buf);
 		if (res < 0) {
 			if (mtd_is_eccerr(res)) {
-				pr_info("nand_bbt: ECC error in BBT at "
-					"0x%012llx\n", from & ~mtd->writesize);
+				pr_info("nand_bbt: ECC error in BBT at 0x%012llx\n",
+					from & ~mtd->writesize);
 				return res;
 			} else if (mtd_is_bitflip(res)) {
-				pr_info("nand_bbt: corrected error in BBT at "
-					"0x%012llx\n", from & ~mtd->writesize);
+				pr_info("nand_bbt: corrected error in BBT at 0x%012llx\n",
+					from & ~mtd->writesize);
 				ret = res;
 			} else {
 				pr_info("nand_bbt: error reading BBT\n");
@@ -580,8 +580,8 @@ static int search_bbt(struct mtd_info *mtd, uint8_t *buf, struct nand_bbt_descr
 		if (td->pages[i] == -1)
 			pr_warn("Bad block table not found for chip %d\n", i);
 		else
-			pr_info("Bad block table found at page %d, version "
-				 "0x%02X\n", td->pages[i], td->version[i]);
+			pr_info("Bad block table found at page %d, version 0x%02X\n",
+				td->pages[i], td->version[i]);
 	}
 	return 0;
 }
@@ -725,12 +725,10 @@ static int write_bbt(struct mtd_info *mtd, uint8_t *buf,
 			res = mtd_read(mtd, to, len, &retlen, buf);
 			if (res < 0) {
 				if (retlen != len) {
-					pr_info("nand_bbt: error reading block "
-						"for writing the bad block table\n");
+					pr_info("nand_bbt: error reading block for writing the bad block table\n");
 					return res;
 				}
-				pr_warn("nand_bbt: ECC error while reading "
-					"block for writing bad block table\n");
+				pr_warn("nand_bbt: ECC error while reading block for writing bad block table\n");
 			}
 			/* Read oob data */
 			ops.ooblen = (len >> this->page_shift) * mtd->oobsize;
@@ -1338,9 +1336,8 @@ int nand_isbad_bbt(struct mtd_info *mtd, loff_t offs, int allowbbt)
 	block = (int)(offs >> this->bbt_erase_shift);
 	res = bbt_get_entry(this, block);
 
-	pr_debug("nand_isbad_bbt(): bbt info for offs 0x%08x: "
-			"(block %d) 0x%02x\n",
-			(unsigned int)offs, block, res);
+	pr_debug("nand_isbad_bbt(): bbt info for offs 0x%08x: (block %d) 0x%02x\n",
+		 (unsigned int)offs, block, res);
 
 	switch (res) {
 	case BBT_BLOCK_GOOD:
diff --git a/drivers/mtd/nand/nand_ids.c b/drivers/mtd/nand/nand_ids.c
index 3d7c89fc1031..fbde89105245 100644
--- a/drivers/mtd/nand/nand_ids.c
+++ b/drivers/mtd/nand/nand_ids.c
@@ -46,6 +46,10 @@ struct nand_flash_dev nand_flash_ids[] = {
 	{"SDTNRGAMA 64G 3.3V 8-bit",
 		{ .id = {0x45, 0xde, 0x94, 0x93, 0x76, 0x50} },
 		  SZ_16K, SZ_8K, SZ_4M, 0, 6, 1280, NAND_ECC_INFO(40, SZ_1K) },
+	{"H27UCG8T2ATR-BC 64G 3.3V 8-bit",
+		{ .id = {0xad, 0xde, 0x94, 0xda, 0x74, 0xc4} },
+		  SZ_8K, SZ_8K, SZ_2M, 0, 6, 640, NAND_ECC_INFO(40, SZ_1K),
+		  4 },
 
 	LEGACY_ID_NAND("NAND 4MiB 5V 8-bit",   0x6B, 4, SZ_8K, SP_OPTIONS),
 	LEGACY_ID_NAND("NAND 4MiB 3,3V 8-bit", 0xE3, 4, SZ_8K, SP_OPTIONS),
diff --git a/drivers/mtd/nand/nand_timings.c b/drivers/mtd/nand/nand_timings.c
index 8b36253420fa..e81470a8ac67 100644
--- a/drivers/mtd/nand/nand_timings.c
+++ b/drivers/mtd/nand/nand_timings.c
@@ -42,7 +42,7 @@ static const struct nand_sdr_timings onfi_sdr_timings[] = {
 		.tRHZ_max = 200000,
 		.tRLOH_min = 0,
 		.tRP_min = 50000,
-		.tRST_max = 250000000000,
+		.tRST_max = 250000000000ULL,
 		.tWB_max = 200000,
 		.tRR_min = 40000,
 		.tWC_min = 100000,
diff --git a/drivers/mtd/nand/nandsim.c b/drivers/mtd/nand/nandsim.c
index 4f0d83648e5a..7dc1dd28d896 100644
--- a/drivers/mtd/nand/nandsim.c
+++ b/drivers/mtd/nand/nandsim.c
@@ -827,7 +827,7 @@ static int parse_badblocks(struct nandsim *ns, struct mtd_info *mtd)
 			NS_ERR("invalid badblocks.\n");
 			return -EINVAL;
 		}
-		offset = erase_block_no * ns->geom.secsz;
+		offset = (loff_t)erase_block_no * ns->geom.secsz;
 		if (mtd_block_markbad(mtd, offset)) {
 			NS_ERR("invalid badblocks.\n");
 			return -EINVAL;
diff --git a/drivers/mtd/nand/ndfc.c b/drivers/mtd/nand/ndfc.c
index 69eaba690a99..253a644da76a 100644
--- a/drivers/mtd/nand/ndfc.c
+++ b/drivers/mtd/nand/ndfc.c
@@ -203,7 +203,8 @@ static int ndfc_probe(struct platform_device *ofdev)
 	struct ndfc_controller *ndfc;
 	const __be32 *reg;
 	u32 ccr;
-	int err, len, cs;
+	u32 cs;
+	int err, len;
 
 	/* Read the reg property to get the chip select */
 	reg = of_get_property(ofdev->dev.of_node, "reg", &len);
diff --git a/drivers/mtd/nand/omap2.c b/drivers/mtd/nand/omap2.c
index f0ed92e210a1..3b357e920a0c 100644
--- a/drivers/mtd/nand/omap2.c
+++ b/drivers/mtd/nand/omap2.c
@@ -136,7 +136,6 @@
 
 #define BADBLOCK_MARKER_LENGTH		2
 
-#ifdef CONFIG_MTD_NAND_OMAP_BCH
 static u_char bch16_vector[] = {0xf5, 0x24, 0x1c, 0xd0, 0x61, 0xb3, 0xf1, 0x55,
 				0x2e, 0x2c, 0x86, 0xa3, 0xed, 0x36, 0x1b, 0x78,
 				0x48, 0x76, 0xa9, 0x3b, 0x97, 0xd1, 0x7a, 0x93,
@@ -144,7 +143,6 @@ static u_char bch16_vector[] = {0xf5, 0x24, 0x1c, 0xd0, 0x61, 0xb3, 0xf1, 0x55,
 static u_char bch8_vector[] = {0xf3, 0xdb, 0x14, 0x16, 0x8b, 0xd2, 0xbe, 0xcc,
 	0xac, 0x6b, 0xff, 0x99, 0x7b};
 static u_char bch4_vector[] = {0x00, 0x6b, 0x31, 0xdd, 0x41, 0xbc, 0x10};
-#endif
 
 /* oob info generated runtime depending on ecc algorithm and layout selected */
 static struct nand_ecclayout omap_oobinfo;
@@ -931,7 +929,7 @@ static int omap_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 	u32 val;
 
 	val = readl(info->reg.gpmc_ecc_config);
-	if (((val >> ECC_CONFIG_CS_SHIFT)  & ~CS_MASK) != info->gpmc_cs)
+	if (((val >> ECC_CONFIG_CS_SHIFT) & CS_MASK) != info->gpmc_cs)
 		return -EINVAL;
 
 	/* read ecc result */
@@ -1292,7 +1290,6 @@ static int __maybe_unused omap_calculate_ecc_bch(struct mtd_info *mtd,
 	return 0;
 }
 
-#ifdef CONFIG_MTD_NAND_OMAP_BCH
 /**
  * erased_sector_bitflips - count bit flips
  * @data:	data sector buffer
@@ -1378,7 +1375,7 @@ static int omap_elm_correct_data(struct mtd_info *mtd, u_char *data,
 		erased_ecc_vec = bch16_vector;
 		break;
 	default:
-		pr_err("invalid driver configuration\n");
+		dev_err(&info->pdev->dev, "invalid driver configuration\n");
 		return -EINVAL;
 	}
 
@@ -1449,7 +1446,8 @@ static int omap_elm_correct_data(struct mtd_info *mtd, u_char *data,
 	err = 0;
 	for (i = 0; i < eccsteps; i++) {
 		if (err_vec[i].error_uncorrectable) {
-			pr_err("nand: uncorrectable bit-flips found\n");
+			dev_err(&info->pdev->dev,
+				"uncorrectable bit-flips found\n");
 			err = -EBADMSG;
 		} else if (err_vec[i].error_reported) {
 			for (j = 0; j < err_vec[i].error_count; j++) {
@@ -1486,8 +1484,9 @@ static int omap_elm_correct_data(struct mtd_info *mtd, u_char *data,
 							1 << bit_pos;
 					}
 				} else {
-					pr_err("invalid bit-flip @ %d:%d\n",
-							 byte_pos, bit_pos);
+					dev_err(&info->pdev->dev,
+						"invalid bit-flip @ %d:%d\n",
+						byte_pos, bit_pos);
 					err = -EBADMSG;
 				}
 			}
@@ -1593,33 +1592,71 @@ static int omap_read_page_bch(struct mtd_info *mtd, struct nand_chip *chip,
 /**
  * is_elm_present - checks for presence of ELM module by scanning DT nodes
  * @omap_nand_info: NAND device structure containing platform data
- * @bch_type: 0x0=BCH4, 0x1=BCH8, 0x2=BCH16
  */
-static int is_elm_present(struct omap_nand_info *info,
-			struct device_node *elm_node, enum bch_ecc bch_type)
+static bool is_elm_present(struct omap_nand_info *info,
+			   struct device_node *elm_node)
 {
 	struct platform_device *pdev;
-	struct nand_ecc_ctrl *ecc = &info->nand.ecc;
-	int err;
+
 	/* check whether elm-id is passed via DT */
 	if (!elm_node) {
-		pr_err("nand: error: ELM DT node not found\n");
-		return -ENODEV;
+		dev_err(&info->pdev->dev, "ELM devicetree node not found\n");
+		return false;
 	}
 	pdev = of_find_device_by_node(elm_node);
 	/* check whether ELM device is registered */
 	if (!pdev) {
-		pr_err("nand: error: ELM device not found\n");
-		return -ENODEV;
+		dev_err(&info->pdev->dev, "ELM device not found\n");
+		return false;
 	}
 	/* ELM module available, now configure it */
 	info->elm_dev = &pdev->dev;
-	err = elm_config(info->elm_dev, bch_type,
-		(info->mtd.writesize / ecc->size), ecc->size, ecc->bytes);
+	return true;
+}
 
-	return err;
+static bool omap2_nand_ecc_check(struct omap_nand_info *info,
+				 struct omap_nand_platform_data	*pdata)
+{
+	bool ecc_needs_bch, ecc_needs_omap_bch, ecc_needs_elm;
+
+	switch (info->ecc_opt) {
+	case OMAP_ECC_BCH4_CODE_HW_DETECTION_SW:
+	case OMAP_ECC_BCH8_CODE_HW_DETECTION_SW:
+		ecc_needs_omap_bch = false;
+		ecc_needs_bch = true;
+		ecc_needs_elm = false;
+		break;
+	case OMAP_ECC_BCH4_CODE_HW:
+	case OMAP_ECC_BCH8_CODE_HW:
+	case OMAP_ECC_BCH16_CODE_HW:
+		ecc_needs_omap_bch = true;
+		ecc_needs_bch = false;
+		ecc_needs_elm = true;
+		break;
+	default:
+		ecc_needs_omap_bch = false;
+		ecc_needs_bch = false;
+		ecc_needs_elm = false;
+		break;
+	}
+
+	if (ecc_needs_bch && !IS_ENABLED(CONFIG_MTD_NAND_ECC_BCH)) {
+		dev_err(&info->pdev->dev,
+			"CONFIG_MTD_NAND_ECC_BCH not enabled\n");
+		return false;
+	}
+	if (ecc_needs_omap_bch && !IS_ENABLED(CONFIG_MTD_NAND_OMAP_BCH)) {
+		dev_err(&info->pdev->dev,
+			"CONFIG_MTD_NAND_OMAP_BCH not enabled\n");
+		return false;
+	}
+	if (ecc_needs_elm && !is_elm_present(info, pdata->elm_of_node)) {
+		dev_err(&info->pdev->dev, "ELM not available\n");
+		return false;
+	}
+
+	return true;
 }
-#endif /* CONFIG_MTD_NAND_ECC_BCH */
 
 static int omap_nand_probe(struct platform_device *pdev)
 {
@@ -1663,7 +1700,6 @@ static int omap_nand_probe(struct platform_device *pdev)
 	mtd->owner		= THIS_MODULE;
 	nand_chip		= &info->nand;
 	nand_chip->ecc.priv	= NULL;
-	nand_chip->options	|= NAND_SKIP_BBTSCAN;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	nand_chip->IO_ADDR_R = devm_ioremap_resource(&pdev->dev, res);
@@ -1692,17 +1728,22 @@ static int omap_nand_probe(struct platform_device *pdev)
 		nand_chip->chip_delay = 50;
 	}
 
+	if (pdata->flash_bbt)
+		nand_chip->bbt_options |= NAND_BBT_USE_FLASH | NAND_BBT_NO_OOB;
+	else
+		nand_chip->options |= NAND_SKIP_BBTSCAN;
+
 	/* scan NAND device connected to chip controller */
 	nand_chip->options |= pdata->devsize & NAND_BUSWIDTH_16;
 	if (nand_scan_ident(mtd, 1, NULL)) {
-		pr_err("nand device scan failed, may be bus-width mismatch\n");
+		dev_err(&info->pdev->dev, "scan failed, may be bus-width mismatch\n");
 		err = -ENXIO;
 		goto return_error;
 	}
 
 	/* check for small page devices */
 	if ((mtd->oobsize < 64) && (pdata->ecc_opt != OMAP_ECC_HAM1_CODE_HW)) {
-		pr_err("small page devices are not supported\n");
+		dev_err(&info->pdev->dev, "small page devices are not supported\n");
 		err = -EINVAL;
 		goto return_error;
 	}
@@ -1793,10 +1834,18 @@ static int omap_nand_probe(struct platform_device *pdev)
 		goto return_error;
 	}
 
+	if (!omap2_nand_ecc_check(info, pdata)) {
+		err = -EINVAL;
+		goto return_error;
+	}
+
 	/* populate MTD interface based on ECC scheme */
-	nand_chip->ecc.layout	= &omap_oobinfo;
 	ecclayout		= &omap_oobinfo;
 	switch (info->ecc_opt) {
+	case OMAP_ECC_HAM1_CODE_SW:
+		nand_chip->ecc.mode = NAND_ECC_SOFT;
+		break;
+
 	case OMAP_ECC_HAM1_CODE_HW:
 		pr_info("nand: using OMAP_ECC_HAM1_CODE_HW\n");
 		nand_chip->ecc.mode             = NAND_ECC_HW;
@@ -1822,7 +1871,6 @@ static int omap_nand_probe(struct platform_device *pdev)
 		break;
 
 	case OMAP_ECC_BCH4_CODE_HW_DETECTION_SW:
-#ifdef CONFIG_MTD_NAND_ECC_BCH
 		pr_info("nand: using OMAP_ECC_BCH4_CODE_HW_DETECTION_SW\n");
 		nand_chip->ecc.mode		= NAND_ECC_HW;
 		nand_chip->ecc.size		= 512;
@@ -1848,20 +1896,15 @@ static int omap_nand_probe(struct platform_device *pdev)
 		nand_chip->ecc.priv		= nand_bch_init(mtd,
 							nand_chip->ecc.size,
 							nand_chip->ecc.bytes,
-							&nand_chip->ecc.layout);
+							&ecclayout);
 		if (!nand_chip->ecc.priv) {
-			pr_err("nand: error: unable to use s/w BCH library\n");
+			dev_err(&info->pdev->dev, "unable to use BCH library\n");
 			err = -EINVAL;
+			goto return_error;
 		}
 		break;
-#else
-		pr_err("nand: error: CONFIG_MTD_NAND_ECC_BCH not enabled\n");
-		err = -EINVAL;
-		goto return_error;
-#endif
 
 	case OMAP_ECC_BCH4_CODE_HW:
-#ifdef CONFIG_MTD_NAND_OMAP_BCH
 		pr_info("nand: using OMAP_ECC_BCH4_CODE_HW ECC scheme\n");
 		nand_chip->ecc.mode		= NAND_ECC_HW;
 		nand_chip->ecc.size		= 512;
@@ -1883,21 +1926,15 @@ static int omap_nand_probe(struct platform_device *pdev)
 		/* reserved marker already included in ecclayout->eccbytes */
 		ecclayout->oobfree->offset	=
 				ecclayout->eccpos[ecclayout->eccbytes - 1] + 1;
-		/* This ECC scheme requires ELM H/W block */
-		if (is_elm_present(info, pdata->elm_of_node, BCH4_ECC) < 0) {
-			pr_err("nand: error: could not initialize ELM\n");
-			err = -ENODEV;
+
+		err = elm_config(info->elm_dev, BCH4_ECC,
+				 info->mtd.writesize / nand_chip->ecc.size,
+				 nand_chip->ecc.size, nand_chip->ecc.bytes);
+		if (err < 0)
 			goto return_error;
-		}
 		break;
-#else
-		pr_err("nand: error: CONFIG_MTD_NAND_OMAP_BCH not enabled\n");
-		err = -EINVAL;
-		goto return_error;
-#endif
 
 	case OMAP_ECC_BCH8_CODE_HW_DETECTION_SW:
-#ifdef CONFIG_MTD_NAND_ECC_BCH
 		pr_info("nand: using OMAP_ECC_BCH8_CODE_HW_DETECTION_SW\n");
 		nand_chip->ecc.mode		= NAND_ECC_HW;
 		nand_chip->ecc.size		= 512;
@@ -1923,21 +1960,15 @@ static int omap_nand_probe(struct platform_device *pdev)
 		nand_chip->ecc.priv		= nand_bch_init(mtd,
 							nand_chip->ecc.size,
 							nand_chip->ecc.bytes,
-							&nand_chip->ecc.layout);
+							&ecclayout);
 		if (!nand_chip->ecc.priv) {
-			pr_err("nand: error: unable to use s/w BCH library\n");
+			dev_err(&info->pdev->dev, "unable to use BCH library\n");
 			err = -EINVAL;
 			goto return_error;
 		}
 		break;
-#else
-		pr_err("nand: error: CONFIG_MTD_NAND_ECC_BCH not enabled\n");
-		err = -EINVAL;
-		goto return_error;
-#endif
 
 	case OMAP_ECC_BCH8_CODE_HW:
-#ifdef CONFIG_MTD_NAND_OMAP_BCH
 		pr_info("nand: using OMAP_ECC_BCH8_CODE_HW ECC scheme\n");
 		nand_chip->ecc.mode		= NAND_ECC_HW;
 		nand_chip->ecc.size		= 512;
@@ -1949,12 +1980,13 @@ static int omap_nand_probe(struct platform_device *pdev)
 		nand_chip->ecc.calculate	= omap_calculate_ecc_bch;
 		nand_chip->ecc.read_page	= omap_read_page_bch;
 		nand_chip->ecc.write_page	= omap_write_page_bch;
-		/* This ECC scheme requires ELM H/W block */
-		err = is_elm_present(info, pdata->elm_of_node, BCH8_ECC);
-		if (err < 0) {
-			pr_err("nand: error: could not initialize ELM\n");
+
+		err = elm_config(info->elm_dev, BCH8_ECC,
+				 info->mtd.writesize / nand_chip->ecc.size,
+				 nand_chip->ecc.size, nand_chip->ecc.bytes);
+		if (err < 0)
 			goto return_error;
-		}
+
 		/* define ECC layout */
 		ecclayout->eccbytes		= nand_chip->ecc.bytes *
 							(mtd->writesize /
@@ -1966,14 +1998,8 @@ static int omap_nand_probe(struct platform_device *pdev)
 		ecclayout->oobfree->offset	=
 				ecclayout->eccpos[ecclayout->eccbytes - 1] + 1;
 		break;
-#else
-		pr_err("nand: error: CONFIG_MTD_NAND_OMAP_BCH not enabled\n");
-		err = -EINVAL;
-		goto return_error;
-#endif
 
 	case OMAP_ECC_BCH16_CODE_HW:
-#ifdef CONFIG_MTD_NAND_OMAP_BCH
 		pr_info("using OMAP_ECC_BCH16_CODE_HW ECC scheme\n");
 		nand_chip->ecc.mode		= NAND_ECC_HW;
 		nand_chip->ecc.size		= 512;
@@ -1984,12 +2010,13 @@ static int omap_nand_probe(struct platform_device *pdev)
 		nand_chip->ecc.calculate	= omap_calculate_ecc_bch;
 		nand_chip->ecc.read_page	= omap_read_page_bch;
 		nand_chip->ecc.write_page	= omap_write_page_bch;
-		/* This ECC scheme requires ELM H/W block */
-		err = is_elm_present(info, pdata->elm_of_node, BCH16_ECC);
-		if (err < 0) {
-			pr_err("ELM is required for this ECC scheme\n");
+
+		err = elm_config(info->elm_dev, BCH16_ECC,
+				 info->mtd.writesize / nand_chip->ecc.size,
+				 nand_chip->ecc.size, nand_chip->ecc.bytes);
+		if (err < 0)
 			goto return_error;
-		}
+
 		/* define ECC layout */
 		ecclayout->eccbytes		= nand_chip->ecc.bytes *
 							(mtd->writesize /
@@ -2001,27 +2028,28 @@ static int omap_nand_probe(struct platform_device *pdev)
 		ecclayout->oobfree->offset	=
 				ecclayout->eccpos[ecclayout->eccbytes - 1] + 1;
 		break;
-#else
-		pr_err("nand: error: CONFIG_MTD_NAND_OMAP_BCH not enabled\n");
-		err = -EINVAL;
-		goto return_error;
-#endif
 	default:
-		pr_err("nand: error: invalid or unsupported ECC scheme\n");
+		dev_err(&info->pdev->dev, "invalid or unsupported ECC scheme\n");
 		err = -EINVAL;
 		goto return_error;
 	}
 
+	if (info->ecc_opt == OMAP_ECC_HAM1_CODE_SW)
+		goto scan_tail;
+
 	/* all OOB bytes from oobfree->offset till end off OOB are free */
 	ecclayout->oobfree->length = mtd->oobsize - ecclayout->oobfree->offset;
 	/* check if NAND device's OOB is enough to store ECC signatures */
 	if (mtd->oobsize < (ecclayout->eccbytes + BADBLOCK_MARKER_LENGTH)) {
-		pr_err("not enough OOB bytes required = %d, available=%d\n",
-					   ecclayout->eccbytes, mtd->oobsize);
+		dev_err(&info->pdev->dev,
+			"not enough OOB bytes required = %d, available=%d\n",
+			ecclayout->eccbytes, mtd->oobsize);
 		err = -EINVAL;
 		goto return_error;
 	}
+	nand_chip->ecc.layout = ecclayout;
 
+scan_tail:
 	/* second phase scan */
 	if (nand_scan_tail(mtd)) {
 		err = -ENXIO;
diff --git a/drivers/mtd/devices/elm.c b/drivers/mtd/nand/omap_elm.c
index b4f61c7fc161..b4f61c7fc161 100644
--- a/drivers/mtd/devices/elm.c
+++ b/drivers/mtd/nand/omap_elm.c
diff --git a/drivers/mtd/nand/sh_flctl.c b/drivers/mtd/nand/sh_flctl.c
index c0670237e7a2..0ed7c603298f 100644
--- a/drivers/mtd/nand/sh_flctl.c
+++ b/drivers/mtd/nand/sh_flctl.c
@@ -395,7 +395,7 @@ static int flctl_dma_fifo0_transfer(struct sh_flctl *flctl, unsigned long *buf,
 				msecs_to_jiffies(3000));
 
 	if (ret <= 0) {
-		chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(chan);
 		dev_err(&flctl->pdev->dev, "wait_for_completion_timeout\n");
 	}
 
diff --git a/drivers/mtd/nand/sm_common.h b/drivers/mtd/nand/sm_common.h
index 00f4a83359b2..d3e028e58b0f 100644
--- a/drivers/mtd/nand/sm_common.h
+++ b/drivers/mtd/nand/sm_common.h
@@ -18,7 +18,7 @@ struct sm_oob {
 	uint8_t ecc2[3];
 	uint8_t lba_copy2[2];
 	uint8_t ecc1[3];
-} __attribute__((packed));
+} __packed;
 
 
 /* one sector is always 512 bytes, but it can consist of two nand pages */
diff --git a/drivers/mtd/sm_ftl.c b/drivers/mtd/sm_ftl.c
index cf49c22673b9..c23184a47fc4 100644
--- a/drivers/mtd/sm_ftl.c
+++ b/drivers/mtd/sm_ftl.c
@@ -1058,7 +1058,7 @@ static int sm_write(struct mtd_blktrans_dev *dev,
 {
 	struct sm_ftl *ftl = dev->priv;
 	struct ftl_zone *zone;
-	int error, zone_num, block, boffset;
+	int error = 0, zone_num, block, boffset;
 
 	BUG_ON(ftl->readonly);
 	sm_break_offset(ftl, sec_no << 9, &zone_num, &block, &boffset);
diff --git a/drivers/mtd/spi-nor/Kconfig b/drivers/mtd/spi-nor/Kconfig
index f8acfa4310ef..64a4f0edabc7 100644
--- a/drivers/mtd/spi-nor/Kconfig
+++ b/drivers/mtd/spi-nor/Kconfig
@@ -7,6 +7,20 @@ menuconfig MTD_SPI_NOR
 
 if MTD_SPI_NOR
 
+config MTD_SPI_NOR_USE_4K_SECTORS
+	bool "Use small 4096 B erase sectors"
+	default y
+	help
+	  Many flash memories support erasing small (4096 B) sectors. Depending
+	  on the usage this feature may provide performance gain in comparison
+	  to erasing whole blocks (32/64 KiB).
+	  Changing a small part of the flash's contents is usually faster with
+	  small sectors. On the other hand erasing should be faster when using
+	  64 KiB block instead of 16 × 4 KiB sectors.
+
+	  Please note that some tools/drivers/filesystems may not work with
+	  4096 B erase size (e.g. UBIFS requires 15 KiB as a minimum).
+
 config SPI_FSL_QUADSPI
 	tristate "Freescale Quad SPI controller"
 	depends on ARCH_MXC
diff --git a/drivers/mtd/spi-nor/spi-nor.c b/drivers/mtd/spi-nor/spi-nor.c
index b5ad6bebf5e7..ae16aa2f6885 100644
--- a/drivers/mtd/spi-nor/spi-nor.c
+++ b/drivers/mtd/spi-nor/spi-nor.c
@@ -611,6 +611,7 @@ const struct spi_device_id spi_nor_ids[] = {
 	{ "m25px32-s0", INFO(0x207316,  0, 64 * 1024, 64, SECT_4K) },
 	{ "m25px32-s1", INFO(0x206316,  0, 64 * 1024, 64, SECT_4K) },
 	{ "m25px64",    INFO(0x207117,  0, 64 * 1024, 128, 0) },
+	{ "m25px80",    INFO(0x207114,  0, 64 * 1024, 16, 0) },
 
 	/* Winbond -- w25x "blocks" are 64K, "sectors" are 4KiB */
 	{ "w25x10", INFO(0xef3011, 0, 64 * 1024,  2,  SECT_4K) },
@@ -623,7 +624,6 @@ const struct spi_device_id spi_nor_ids[] = {
 	{ "w25q32dw", INFO(0xef6016, 0, 64 * 1024,  64, SECT_4K) },
 	{ "w25x64", INFO(0xef3017, 0, 64 * 1024, 128, SECT_4K) },
 	{ "w25q64", INFO(0xef4017, 0, 64 * 1024, 128, SECT_4K) },
-	{ "w25q128", INFO(0xef4018, 0, 64 * 1024, 256, SECT_4K) },
 	{ "w25q80", INFO(0xef5014, 0, 64 * 1024,  16, SECT_4K) },
 	{ "w25q80bl", INFO(0xef4014, 0, 64 * 1024,  16, SECT_4K) },
 	{ "w25q128", INFO(0xef4018, 0, 64 * 1024, 256, SECT_4K) },
@@ -671,11 +671,6 @@ static const struct spi_device_id *spi_nor_read_id(struct spi_nor *nor)
 	return ERR_PTR(-ENODEV);
 }
 
-static const struct spi_device_id *jedec_probe(struct spi_nor *nor)
-{
-	return nor->read_id(nor);
-}
-
 static int spi_nor_read(struct mtd_info *mtd, loff_t from, size_t len,
 			size_t *retlen, u_char *buf)
 {
@@ -920,7 +915,6 @@ int spi_nor_scan(struct spi_nor *nor, const struct spi_device_id *id,
 			enum read_mode mode)
 {
 	struct flash_info		*info;
-	struct flash_platform_data	*data;
 	struct device *dev = nor->dev;
 	struct mtd_info *mtd = nor->mtd;
 	struct device_node *np = dev->of_node;
@@ -931,34 +925,12 @@ int spi_nor_scan(struct spi_nor *nor, const struct spi_device_id *id,
 	if (ret)
 		return ret;
 
-	/* Platform data helps sort out which chip type we have, as
-	 * well as how this board partitions it.  If we don't have
-	 * a chip ID, try the JEDEC id commands; they'll work for most
-	 * newer chips, even if we don't recognize the particular chip.
-	 */
-	data = dev_get_platdata(dev);
-	if (data && data->type) {
-		const struct spi_device_id *plat_id;
-
-		for (i = 0; i < ARRAY_SIZE(spi_nor_ids) - 1; i++) {
-			plat_id = &spi_nor_ids[i];
-			if (strcmp(data->type, plat_id->name))
-				continue;
-			break;
-		}
-
-		if (i < ARRAY_SIZE(spi_nor_ids) - 1)
-			id = plat_id;
-		else
-			dev_warn(dev, "unrecognized id %s\n", data->type);
-	}
-
 	info = (void *)id->driver_data;
 
 	if (info->jedec_id) {
 		const struct spi_device_id *jid;
 
-		jid = jedec_probe(nor);
+		jid = nor->read_id(nor);
 		if (IS_ERR(jid)) {
 			return PTR_ERR(jid);
 		} else if (jid != id) {
@@ -990,11 +962,8 @@ int spi_nor_scan(struct spi_nor *nor, const struct spi_device_id *id,
 		write_sr(nor, 0);
 	}
 
-	if (data && data->name)
-		mtd->name = data->name;
-	else
+	if (!mtd->name)
 		mtd->name = dev_name(dev);
-
 	mtd->type = MTD_NORFLASH;
 	mtd->writesize = 1;
 	mtd->flags = MTD_CAP_NORFLASH;
@@ -1018,6 +987,7 @@ int spi_nor_scan(struct spi_nor *nor, const struct spi_device_id *id,
 	    nor->wait_till_ready == spi_nor_wait_till_ready)
 		nor->wait_till_ready = spi_nor_wait_till_fsr_ready;
 
+#ifdef CONFIG_MTD_SPI_NOR_USE_4K_SECTORS
 	/* prefer "small sector" erase if possible */
 	if (info->flags & SECT_4K) {
 		nor->erase_opcode = SPINOR_OP_BE_4K;
@@ -1025,7 +995,9 @@ int spi_nor_scan(struct spi_nor *nor, const struct spi_device_id *id,
 	} else if (info->flags & SECT_4K_PMC) {
 		nor->erase_opcode = SPINOR_OP_BE_4K_PMC;
 		mtd->erasesize = 4096;
-	} else {
+	} else
+#endif
+	{
 		nor->erase_opcode = SPINOR_OP_SE;
 		mtd->erasesize = info->sector_size;
 	}
diff --git a/drivers/mtd/tests/mtd_test.c b/drivers/mtd/tests/mtd_test.c
index 111ee46a7428..34736bbcc07b 100644
--- a/drivers/mtd/tests/mtd_test.c
+++ b/drivers/mtd/tests/mtd_test.c
@@ -10,7 +10,7 @@ int mtdtest_erase_eraseblock(struct mtd_info *mtd, unsigned int ebnum)
 {
 	int err;
 	struct erase_info ei;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	memset(&ei, 0, sizeof(struct erase_info));
 	ei.mtd  = mtd;
@@ -33,7 +33,7 @@ int mtdtest_erase_eraseblock(struct mtd_info *mtd, unsigned int ebnum)
 static int is_block_bad(struct mtd_info *mtd, unsigned int ebnum)
 {
 	int ret;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	ret = mtd_block_isbad(mtd, addr);
 	if (ret)
diff --git a/drivers/mtd/tests/nandbiterrs.c b/drivers/mtd/tests/nandbiterrs.c
index 6f976159611f..273f7e553954 100644
--- a/drivers/mtd/tests/nandbiterrs.c
+++ b/drivers/mtd/tests/nandbiterrs.c
@@ -364,7 +364,7 @@ static int __init mtd_nandbiterrs_init(void)
 
 	pr_info("Device uses %d subpages of %d bytes\n", subcount, subsize);
 
-	offset     = page_offset * mtd->writesize;
+	offset     = (loff_t)page_offset * mtd->writesize;
 	eraseblock = mtd_div_by_eb(offset, mtd);
 
 	pr_info("Using page=%u, offset=%llu, eraseblock=%u\n",
diff --git a/drivers/mtd/tests/oobtest.c b/drivers/mtd/tests/oobtest.c
index f19ab1acde1f..dc4f9602b97e 100644
--- a/drivers/mtd/tests/oobtest.c
+++ b/drivers/mtd/tests/oobtest.c
@@ -120,7 +120,7 @@ static int verify_eraseblock(int ebnum)
 	int i;
 	struct mtd_oob_ops ops;
 	int err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	prandom_bytes_state(&rnd_state, writebuf, use_len_max * pgcnt);
 	for (i = 0; i < pgcnt; ++i, addr += mtd->writesize) {
@@ -214,7 +214,7 @@ static int verify_eraseblock_in_one_go(int ebnum)
 {
 	struct mtd_oob_ops ops;
 	int err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 	size_t len = mtd->ecclayout->oobavail * pgcnt;
 
 	prandom_bytes_state(&rnd_state, writebuf, len);
@@ -568,7 +568,7 @@ static int __init mtd_oobtest_init(void)
 		size_t sz = mtd->ecclayout->oobavail;
 		if (bbt[i] || bbt[i + 1])
 			continue;
-		addr = (i + 1) * mtd->erasesize - mtd->writesize;
+		addr = (loff_t)(i + 1) * mtd->erasesize - mtd->writesize;
 		prandom_bytes_state(&rnd_state, writebuf, sz * cnt);
 		for (pg = 0; pg < cnt; ++pg) {
 			ops.mode      = MTD_OPS_AUTO_OOB;
@@ -598,7 +598,7 @@ static int __init mtd_oobtest_init(void)
 			continue;
 		prandom_bytes_state(&rnd_state, writebuf,
 					mtd->ecclayout->oobavail * 2);
-		addr = (i + 1) * mtd->erasesize - mtd->writesize;
+		addr = (loff_t)(i + 1) * mtd->erasesize - mtd->writesize;
 		ops.mode      = MTD_OPS_AUTO_OOB;
 		ops.len       = 0;
 		ops.retlen    = 0;
diff --git a/drivers/mtd/tests/pagetest.c b/drivers/mtd/tests/pagetest.c
index ed2d3f656fd2..88296e888e9d 100644
--- a/drivers/mtd/tests/pagetest.c
+++ b/drivers/mtd/tests/pagetest.c
@@ -52,7 +52,7 @@ static struct rnd_state rnd_state;
 
 static int write_eraseblock(int ebnum)
 {
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	prandom_bytes_state(&rnd_state, writebuf, mtd->erasesize);
 	cond_resched();
@@ -64,7 +64,7 @@ static int verify_eraseblock(int ebnum)
 	uint32_t j;
 	int err = 0, i;
 	loff_t addr0, addrn;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	addr0 = 0;
 	for (i = 0; i < ebcnt && bbt[i]; ++i)
diff --git a/drivers/mtd/tests/readtest.c b/drivers/mtd/tests/readtest.c
index 626e66d0f7e7..a54cf1511114 100644
--- a/drivers/mtd/tests/readtest.c
+++ b/drivers/mtd/tests/readtest.c
@@ -47,7 +47,7 @@ static int pgcnt;
 static int read_eraseblock_by_page(int ebnum)
 {
 	int i, ret, err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 	void *buf = iobuf;
 	void *oobbuf = iobuf1;
 
diff --git a/drivers/mtd/tests/speedtest.c b/drivers/mtd/tests/speedtest.c
index 87ff6a29f84e..5ee9f7021020 100644
--- a/drivers/mtd/tests/speedtest.c
+++ b/drivers/mtd/tests/speedtest.c
@@ -55,7 +55,7 @@ static int multiblock_erase(int ebnum, int blocks)
 {
 	int err;
 	struct erase_info ei;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	memset(&ei, 0, sizeof(struct erase_info));
 	ei.mtd  = mtd;
@@ -80,7 +80,7 @@ static int multiblock_erase(int ebnum, int blocks)
 
 static int write_eraseblock(int ebnum)
 {
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	return mtdtest_write(mtd, addr, mtd->erasesize, iobuf);
 }
@@ -88,7 +88,7 @@ static int write_eraseblock(int ebnum)
 static int write_eraseblock_by_page(int ebnum)
 {
 	int i, err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < pgcnt; i++) {
@@ -106,7 +106,7 @@ static int write_eraseblock_by_2pages(int ebnum)
 {
 	size_t sz = pgsize * 2;
 	int i, n = pgcnt / 2, err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < n; i++) {
@@ -124,7 +124,7 @@ static int write_eraseblock_by_2pages(int ebnum)
 
 static int read_eraseblock(int ebnum)
 {
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	return mtdtest_read(mtd, addr, mtd->erasesize, iobuf);
 }
@@ -132,7 +132,7 @@ static int read_eraseblock(int ebnum)
 static int read_eraseblock_by_page(int ebnum)
 {
 	int i, err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < pgcnt; i++) {
@@ -150,7 +150,7 @@ static int read_eraseblock_by_2pages(int ebnum)
 {
 	size_t sz = pgsize * 2;
 	int i, n = pgcnt / 2, err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 	void *buf = iobuf;
 
 	for (i = 0; i < n; i++) {
diff --git a/drivers/mtd/tests/subpagetest.c b/drivers/mtd/tests/subpagetest.c
index a876371ad410..7b59ef522d5e 100644
--- a/drivers/mtd/tests/subpagetest.c
+++ b/drivers/mtd/tests/subpagetest.c
@@ -57,7 +57,7 @@ static int write_eraseblock(int ebnum)
 {
 	size_t written;
 	int err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	prandom_bytes_state(&rnd_state, writebuf, subpgsize);
 	err = mtd_write(mtd, addr, subpgsize, &written, writebuf);
@@ -92,7 +92,7 @@ static int write_eraseblock2(int ebnum)
 {
 	size_t written;
 	int err = 0, k;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	for (k = 1; k < 33; ++k) {
 		if (addr + (subpgsize * k) > (ebnum + 1) * mtd->erasesize)
@@ -131,7 +131,7 @@ static int verify_eraseblock(int ebnum)
 {
 	size_t read;
 	int err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	prandom_bytes_state(&rnd_state, writebuf, subpgsize);
 	clear_data(readbuf, subpgsize);
@@ -192,7 +192,7 @@ static int verify_eraseblock2(int ebnum)
 {
 	size_t read;
 	int err = 0, k;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	for (k = 1; k < 33; ++k) {
 		if (addr + (subpgsize * k) > (ebnum + 1) * mtd->erasesize)
@@ -227,7 +227,7 @@ static int verify_eraseblock_ff(int ebnum)
 	uint32_t j;
 	size_t read;
 	int err = 0;
-	loff_t addr = ebnum * mtd->erasesize;
+	loff_t addr = (loff_t)ebnum * mtd->erasesize;
 
 	memset(writebuf, 0xff, subpgsize);
 	for (j = 0; j < mtd->erasesize / subpgsize; ++j) {
diff --git a/drivers/mtd/ubi/block.c b/drivers/mtd/ubi/block.c
index 33c64955d4d7..8876c7d3d712 100644
--- a/drivers/mtd/ubi/block.c
+++ b/drivers/mtd/ubi/block.c
@@ -188,8 +188,9 @@ static int ubiblock_read_to_buf(struct ubiblock *dev, char *buffer,
 
 	ret = ubi_read(dev->desc, leb, buffer, offset, len);
 	if (ret) {
-		ubi_err("%s ubi_read error %d",
-			dev->gd->disk_name, ret);
+		ubi_err("%s: error %d while reading from LEB %d (offset %d, "
+		        "length %d)", dev->gd->disk_name, ret, leb, offset,
+			len);
 		return ret;
 	}
 	return 0;
@@ -378,7 +379,7 @@ int ubiblock_create(struct ubi_volume_info *vi)
 {
 	struct ubiblock *dev;
 	struct gendisk *gd;
-	u64 disk_capacity = ((u64)vi->size * vi->usable_leb_size) >> 9;
+	u64 disk_capacity = vi->used_bytes >> 9;
 	int ret;
 
 	if ((sector_t)disk_capacity != disk_capacity)
@@ -502,13 +503,8 @@ int ubiblock_remove(struct ubi_volume_info *vi)
 static int ubiblock_resize(struct ubi_volume_info *vi)
 {
 	struct ubiblock *dev;
-	u64 disk_capacity = ((u64)vi->size * vi->usable_leb_size) >> 9;
+	u64 disk_capacity = vi->used_bytes >> 9;
 
-	if ((sector_t)disk_capacity != disk_capacity) {
-		ubi_warn("%s: the volume is too big, cannot resize (%d LEBs)",
-			 dev->gd->disk_name, vi->size);
-		return -EFBIG;
-	}
 	/*
 	 * Need to lock the device list until we stop using the device,
 	 * otherwise the device struct might get released in
@@ -520,10 +516,20 @@ static int ubiblock_resize(struct ubi_volume_info *vi)
 		mutex_unlock(&devices_mutex);
 		return -ENODEV;
 	}
+	if ((sector_t)disk_capacity != disk_capacity) {
+		mutex_unlock(&devices_mutex);
+		ubi_warn("%s: the volume is too big (%d LEBs), cannot resize",
+			 dev->gd->disk_name, vi->size);
+		return -EFBIG;
+	}
 
 	mutex_lock(&dev->dev_mutex);
-	set_capacity(dev->gd, disk_capacity);
-	ubi_msg("%s resized to %d LEBs", dev->gd->disk_name, vi->size);
+
+	if (get_capacity(dev->gd) != disk_capacity) {
+		set_capacity(dev->gd, disk_capacity);
+		ubi_msg("%s resized to %lld bytes", dev->gd->disk_name,
+			vi->used_bytes);
+	}
 	mutex_unlock(&dev->dev_mutex);
 	mutex_unlock(&devices_mutex);
 	return 0;
@@ -547,6 +553,14 @@ static int ubiblock_notify(struct notifier_block *nb,
 	case UBI_VOLUME_RESIZED:
 		ubiblock_resize(&nt->vi);
 		break;
+	case UBI_VOLUME_UPDATED:
+		/*
+		 * If the volume is static, a content update might mean the
+		 * size (i.e. used_bytes) was also changed.
+		 */
+		if (nt->vi.vol_type == UBI_STATIC_VOLUME)
+			ubiblock_resize(&nt->vi);
+		break;
 	default:
 		break;
 	}
diff --git a/drivers/mtd/ubi/cdev.c b/drivers/mtd/ubi/cdev.c
index 7646220ca6e2..59de69a24e40 100644
--- a/drivers/mtd/ubi/cdev.c
+++ b/drivers/mtd/ubi/cdev.c
@@ -425,8 +425,10 @@ static long vol_cdev_ioctl(struct file *file, unsigned int cmd,
 			break;
 
 		err = ubi_start_update(ubi, vol, bytes);
-		if (bytes == 0)
+		if (bytes == 0) {
+			ubi_volume_notify(ubi, vol, UBI_VOLUME_UPDATED);
 			revoke_exclusive(desc, UBI_READWRITE);
+		}
 		break;
 	}
 
@@ -699,7 +701,7 @@ static int rename_volumes(struct ubi_device *ubi,
 		req->ents[i].name[req->ents[i].name_len] = '\0';
 		n = strlen(req->ents[i].name);
 		if (n != req->ents[i].name_len)
-			err = -EINVAL;
+			return -EINVAL;
 	}
 
 	/* Make sure volume IDs and names are unique */
diff --git a/drivers/mtd/ubi/eba.c b/drivers/mtd/ubi/eba.c
index 0e11671dadc4..2402d3b50171 100644
--- a/drivers/mtd/ubi/eba.c
+++ b/drivers/mtd/ubi/eba.c
@@ -441,10 +441,9 @@ retry:
 
 	err = ubi_io_read_data(ubi, buf, pnum, offset, len);
 	if (err) {
-		if (err == UBI_IO_BITFLIPS) {
+		if (err == UBI_IO_BITFLIPS)
 			scrub = 1;
-			err = 0;
-		} else if (mtd_is_eccerr(err)) {
+		else if (mtd_is_eccerr(err)) {
 			if (vol->vol_type == UBI_DYNAMIC_VOLUME)
 				goto out_unlock;
 			scrub = 1;
diff --git a/drivers/mtd/ubi/fastmap.c b/drivers/mtd/ubi/fastmap.c
index 0431b46d9fd9..cfd5b5e90156 100644
--- a/drivers/mtd/ubi/fastmap.c
+++ b/drivers/mtd/ubi/fastmap.c
@@ -24,7 +24,8 @@ size_t ubi_calc_fm_size(struct ubi_device *ubi)
 {
 	size_t size;
 
-	size = sizeof(struct ubi_fm_hdr) + \
+	size = sizeof(struct ubi_fm_sb) + \
+		sizeof(struct ubi_fm_hdr) + \
 		sizeof(struct ubi_fm_scan_pool) + \
 		sizeof(struct ubi_fm_scan_pool) + \
 		(ubi->peb_count * sizeof(struct ubi_fm_ec)) + \
@@ -330,6 +331,7 @@ static int process_pool_aeb(struct ubi_device *ubi, struct ubi_attach_info *ai,
 		av = tmp_av;
 	else {
 		ubi_err("orphaned volume in fastmap pool!");
+		kmem_cache_free(ai->aeb_slab_cache, new_aeb);
 		return UBI_BAD_FASTMAP;
 	}
 
diff --git a/drivers/mtd/ubi/ubi.h b/drivers/mtd/ubi/ubi.h
index 7bf416329c19..320fc38fa2a1 100644
--- a/drivers/mtd/ubi/ubi.h
+++ b/drivers/mtd/ubi/ubi.h
@@ -439,7 +439,8 @@ struct ubi_debug_info {
  *	     @move_to, @move_to_put @erase_pending, @wl_scheduled, @works,
  *	     @erroneous, and @erroneous_peb_count fields
  * @move_mutex: serializes eraseblock moves
- * @work_sem: synchronizes the WL worker with use tasks
+ * @work_sem: used to wait for all the scheduled works to finish and prevent
+ * new works from being submitted
  * @wl_scheduled: non-zero if the wear-leveling was scheduled
  * @lookuptbl: a table to quickly find a &struct ubi_wl_entry object for any
  *             physical eraseblock
@@ -713,14 +714,15 @@ struct ubi_attach_info {
  * @torture: if the physical eraseblock has to be tortured
  * @anchor: produce a anchor PEB to by used by fastmap
  *
- * The @func pointer points to the worker function. If the @cancel argument is
- * not zero, the worker has to free the resources and exit immediately. The
- * worker has to return zero in case of success and a negative error code in
+ * The @func pointer points to the worker function. If the @shutdown argument is
+ * not zero, the worker has to free the resources and exit immediately as the
+ * WL sub-system is shutting down.
+ * The worker has to return zero in case of success and a negative error code in
  * case of failure.
  */
 struct ubi_work {
 	struct list_head list;
-	int (*func)(struct ubi_device *ubi, struct ubi_work *wrk, int cancel);
+	int (*func)(struct ubi_device *ubi, struct ubi_work *wrk, int shutdown);
 	/* The below fields are only relevant to erasure works */
 	struct ubi_wl_entry *e;
 	int vol_id;
diff --git a/drivers/mtd/ubi/wl.c b/drivers/mtd/ubi/wl.c
index 20f491713145..6654f191868e 100644
--- a/drivers/mtd/ubi/wl.c
+++ b/drivers/mtd/ubi/wl.c
@@ -272,7 +272,7 @@ static int produce_free_peb(struct ubi_device *ubi)
 {
 	int err;
 
-	while (!ubi->free.rb_node) {
+	while (!ubi->free.rb_node && ubi->works_count) {
 		spin_unlock(&ubi->wl_lock);
 
 		dbg_wl("do one work synchronously");
@@ -835,7 +835,7 @@ repeat:
  * @wrk: the work to schedule
  *
  * This function adds a work defined by @wrk to the tail of the pending works
- * list. Can only be used of ubi->work_sem is already held in read mode!
+ * list. Can only be used if ubi->work_sem is already held in read mode!
  */
 static void __schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
 {
@@ -864,7 +864,7 @@ static void schedule_ubi_work(struct ubi_device *ubi, struct ubi_work *wrk)
 }
 
 static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
-			int cancel);
+			int shutdown);
 
 #ifdef CONFIG_MTD_UBI_FASTMAP
 /**
@@ -990,14 +990,15 @@ int ubi_wl_put_fm_peb(struct ubi_device *ubi, struct ubi_wl_entry *fm_e,
  * wear_leveling_worker - wear-leveling worker function.
  * @ubi: UBI device description object
  * @wrk: the work object
- * @cancel: non-zero if the worker has to free memory and exit
+ * @shutdown: non-zero if the worker has to free memory and exit
+ * because the WL-subsystem is shutting down
  *
  * This function copies a more worn out physical eraseblock to a less worn out
  * one. Returns zero in case of success and a negative error code in case of
  * failure.
  */
 static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
-				int cancel)
+				int shutdown)
 {
 	int err, scrubbing = 0, torture = 0, protect = 0, erroneous = 0;
 	int vol_id = -1, uninitialized_var(lnum);
@@ -1008,7 +1009,7 @@ static int wear_leveling_worker(struct ubi_device *ubi, struct ubi_work *wrk,
 	struct ubi_vid_hdr *vid_hdr;
 
 	kfree(wrk);
-	if (cancel)
+	if (shutdown)
 		return 0;
 
 	vid_hdr = ubi_zalloc_vid_hdr(ubi, GFP_NOFS);
@@ -1407,7 +1408,8 @@ int ubi_ensure_anchor_pebs(struct ubi_device *ubi)
  * erase_worker - physical eraseblock erase worker function.
  * @ubi: UBI device description object
  * @wl_wrk: the work object
- * @cancel: non-zero if the worker has to free memory and exit
+ * @shutdown: non-zero if the worker has to free memory and exit
+ * because the WL sub-system is shutting down
  *
  * This function erases a physical eraseblock and perform torture testing if
  * needed. It also takes care about marking the physical eraseblock bad if
@@ -1415,7 +1417,7 @@ int ubi_ensure_anchor_pebs(struct ubi_device *ubi)
  * failure.
  */
 static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
-			int cancel)
+			int shutdown)
 {
 	struct ubi_wl_entry *e = wl_wrk->e;
 	int pnum = e->pnum;
@@ -1423,7 +1425,7 @@ static int erase_worker(struct ubi_device *ubi, struct ubi_work *wl_wrk,
 	int lnum = wl_wrk->lnum;
 	int err, available_consumed = 0;
 
-	if (cancel) {
+	if (shutdown) {
 		dbg_wl("cancel erasure of PEB %d EC %d", pnum, e->ec);
 		kfree(wl_wrk);
 		kmem_cache_free(ubi_wl_entry_slab, e);
@@ -1845,10 +1847,10 @@ int ubi_thread(void *u)
 }
 
 /**
- * cancel_pending - cancel all pending works.
+ * shutdown_work - shutdown all pending works.
  * @ubi: UBI device description object
  */
-static void cancel_pending(struct ubi_device *ubi)
+static void shutdown_work(struct ubi_device *ubi)
 {
 	while (!list_empty(&ubi->works)) {
 		struct ubi_work *wrk;
@@ -1997,7 +1999,7 @@ int ubi_wl_init(struct ubi_device *ubi, struct ubi_attach_info *ai)
 	return 0;
 
 out_free:
-	cancel_pending(ubi);
+	shutdown_work(ubi);
 	tree_destroy(&ubi->used);
 	tree_destroy(&ubi->free);
 	tree_destroy(&ubi->scrub);
@@ -2029,7 +2031,7 @@ static void protection_queue_destroy(struct ubi_device *ubi)
 void ubi_wl_close(struct ubi_device *ubi)
 {
 	dbg_wl("close the WL sub-system");
-	cancel_pending(ubi);
+	shutdown_work(ubi);
 	protection_queue_destroy(ubi);
 	tree_destroy(&ubi->used);
 	tree_destroy(&ubi->erroneous);
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index c6f6f69f8961..4706386b7d34 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -147,7 +147,6 @@ config MACVTAP
 config VXLAN
        tristate "Virtual eXtensible Local Area Network (VXLAN)"
        depends on INET
-       select NET_IP_TUNNEL
        select NET_UDP_TUNNEL
        ---help---
 	  This allows one to create vxlan virtual interfaces that provide
diff --git a/drivers/net/appletalk/ipddp.c b/drivers/net/appletalk/ipddp.c
index 10d0dba572c2..e90c6a7333d7 100644
--- a/drivers/net/appletalk/ipddp.c
+++ b/drivers/net/appletalk/ipddp.c
@@ -74,7 +74,7 @@ static struct net_device * __init ipddp_init(void)
 	if (!dev)
 		return ERR_PTR(-ENOMEM);
 
-	dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(dev);
 	strcpy(dev->name, "ipddp%d");
 
 	if (version_printed++ == 0)
diff --git a/drivers/net/arcnet/arcnet.c b/drivers/net/arcnet/arcnet.c
index 3b790de6c976..09de683c167e 100644
--- a/drivers/net/arcnet/arcnet.c
+++ b/drivers/net/arcnet/arcnet.c
@@ -777,7 +777,7 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 			ACOMMAND(CFLAGScmd | RESETclear);
 		AINTMASK(0);
 		spin_unlock(&lp->lock);
-		return IRQ_HANDLED;
+		return retval;
 	}
 
 	BUGMSG(D_DURING, "in arcnet_inthandler (status=%Xh, intmask=%Xh)\n",
diff --git a/drivers/net/arcnet/com20020-pci.c b/drivers/net/arcnet/com20020-pci.c
index 7bb292e59559..6c99ff0b0bdd 100644
--- a/drivers/net/arcnet/com20020-pci.c
+++ b/drivers/net/arcnet/com20020-pci.c
@@ -38,6 +38,7 @@
 #include <linux/pci.h>
 #include <linux/arcdevice.h>
 #include <linux/com20020.h>
+#include <linux/list.h>
 
 #include <asm/io.h>
 
@@ -61,115 +62,317 @@ module_param(clockp, int, 0);
 module_param(clockm, int, 0);
 MODULE_LICENSE("GPL");
 
+static void com20020pci_remove(struct pci_dev *pdev);
+
 static int com20020pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
+	struct com20020_pci_card_info *ci;
 	struct net_device *dev;
 	struct arcnet_local *lp;
-	int ioaddr, err;
+	struct com20020_priv *priv;
+	int i, ioaddr, ret;
+	struct resource *r;
 
 	if (pci_enable_device(pdev))
 		return -EIO;
-	dev = alloc_arcdev(device);
-	if (!dev)
-		return -ENOMEM;
 
-	dev->netdev_ops = &com20020_netdev_ops;
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct com20020_priv),
+			    GFP_KERNEL);
+	ci = (struct com20020_pci_card_info *)id->driver_data;
+	priv->ci = ci;
 
-	lp = netdev_priv(dev);
+	INIT_LIST_HEAD(&priv->list_dev);
 
-	pci_set_drvdata(pdev, dev);
 
-	// SOHARD needs PCI base addr 4
-	if (pdev->vendor==0x10B5) {
-		BUGMSG(D_NORMAL, "SOHARD\n");
-		ioaddr = pci_resource_start(pdev, 4);
-	}
-	else {
-		BUGMSG(D_NORMAL, "Contemporary Controls\n");
-		ioaddr = pci_resource_start(pdev, 2);
-	}
+	for (i = 0; i < ci->devcount; i++) {
+		struct com20020_pci_channel_map *cm = &ci->chan_map_tbl[i];
+		struct com20020_dev *card;
 
-	if (!request_region(ioaddr, ARCNET_TOTAL_SIZE, "com20020-pci")) {
-		BUGMSG(D_INIT, "IO region %xh-%xh already allocated.\n",
-		       ioaddr, ioaddr + ARCNET_TOTAL_SIZE - 1);
-		err = -EBUSY;
-		goto out_dev;
-	}
+		dev = alloc_arcdev(device);
+		if (!dev) {
+			ret = -ENOMEM;
+			goto out_port;
+		}
 
-	// Dummy access after Reset
-	// ARCNET controller needs this access to detect bustype
-	outb(0x00,ioaddr+1);
-	inb(ioaddr+1);
-
-	dev->base_addr = ioaddr;
-	dev->irq = pdev->irq;
-	dev->dev_addr[0] = node;
-	lp->card_name = "PCI COM20020";
-	lp->card_flags = id->driver_data;
-	lp->backplane = backplane;
-	lp->clockp = clockp & 7;
-	lp->clockm = clockm & 3;
-	lp->timeout = timeout;
-	lp->hw.owner = THIS_MODULE;
-
-	if (ASTATUS() == 0xFF) {
-		BUGMSG(D_NORMAL, "IO address %Xh was reported by PCI BIOS, "
-		       "but seems empty!\n", ioaddr);
-		err = -EIO;
-		goto out_port;
-	}
-	if (com20020_check(dev)) {
-		err = -EIO;
-		goto out_port;
+		dev->netdev_ops = &com20020_netdev_ops;
+
+		lp = netdev_priv(dev);
+
+		BUGMSG(D_NORMAL, "%s Controls\n", ci->name);
+		ioaddr = pci_resource_start(pdev, cm->bar) + cm->offset;
+
+		r = devm_request_region(&pdev->dev, ioaddr, cm->size,
+					"com20020-pci");
+		if (!r) {
+			pr_err("IO region %xh-%xh already allocated.\n",
+			       ioaddr, ioaddr + cm->size - 1);
+			ret = -EBUSY;
+			goto out_port;
+		}
+
+		/* Dummy access after Reset
+		 * ARCNET controller needs
+		 * this access to detect bustype
+		 */
+		outb(0x00, ioaddr + 1);
+		inb(ioaddr + 1);
+
+		dev->base_addr = ioaddr;
+		dev->dev_addr[0] = node;
+		dev->irq = pdev->irq;
+		lp->card_name = "PCI COM20020";
+		lp->card_flags = ci->flags;
+		lp->backplane = backplane;
+		lp->clockp = clockp & 7;
+		lp->clockm = clockm & 3;
+		lp->timeout = timeout;
+		lp->hw.owner = THIS_MODULE;
+
+		if (ASTATUS() == 0xFF) {
+			pr_err("IO address %Xh is empty!\n", ioaddr);
+			ret = -EIO;
+			goto out_port;
+		}
+		if (com20020_check(dev)) {
+			ret = -EIO;
+			goto out_port;
+		}
+
+		card = devm_kzalloc(&pdev->dev, sizeof(struct com20020_dev),
+				    GFP_KERNEL);
+		if (!card) {
+			pr_err("%s out of memory!\n", __func__);
+			return -ENOMEM;
+		}
+
+		card->index = i;
+		card->pci_priv = priv;
+		card->dev = dev;
+
+		dev_set_drvdata(&dev->dev, card);
+
+		ret = com20020_found(dev, IRQF_SHARED);
+		if (ret)
+			goto out_port;
+
+		list_add(&card->list, &priv->list_dev);
 	}
 
-	if ((err = com20020_found(dev, IRQF_SHARED)) != 0)
-	        goto out_port;
+	pci_set_drvdata(pdev, priv);
 
 	return 0;
 
 out_port:
-	release_region(ioaddr, ARCNET_TOTAL_SIZE);
-out_dev:
-	free_netdev(dev);
-	return err;
+	com20020pci_remove(pdev);
+	return ret;
 }
 
 static void com20020pci_remove(struct pci_dev *pdev)
 {
-	struct net_device *dev = pci_get_drvdata(pdev);
-	unregister_netdev(dev);
-	free_irq(dev->irq, dev);
-	release_region(dev->base_addr, ARCNET_TOTAL_SIZE);
-	free_netdev(dev);
+	struct com20020_dev *card, *tmpcard;
+	struct com20020_priv *priv;
+
+	priv = pci_get_drvdata(pdev);
+
+	list_for_each_entry_safe(card, tmpcard, &priv->list_dev, list) {
+		struct net_device *dev = card->dev;
+
+		unregister_netdev(dev);
+		free_irq(dev->irq, dev);
+		free_netdev(dev);
+	}
 }
 
+static struct com20020_pci_card_info card_info_10mbit = {
+	.name = "ARC-PCI",
+	.devcount = 1,
+	.chan_map_tbl = {
+		{ 2, 0x00, 0x08 },
+	},
+	.flags = ARC_CAN_10MBIT,
+};
+
+static struct com20020_pci_card_info card_info_5mbit = {
+	.name = "ARC-PCI",
+	.devcount = 1,
+	.chan_map_tbl = {
+		{ 2, 0x00, 0x08 },
+	},
+	.flags = ARC_IS_5MBIT,
+};
+
+static struct com20020_pci_card_info card_info_sohard = {
+	.name = "PLX-PCI",
+	.devcount = 1,
+	/* SOHARD needs PCI base addr 4 */
+	.chan_map_tbl = {
+		{4, 0x00, 0x08},
+	},
+	.flags = ARC_CAN_10MBIT,
+};
+
+static struct com20020_pci_card_info card_info_eae = {
+	.name = "EAE PLX-PCI",
+	.devcount = 2,
+	.chan_map_tbl = {
+		{ 2, 0x00, 0x08 },
+		{ 2, 0x08, 0x08 }
+	},
+	.flags = ARC_CAN_10MBIT,
+};
+
 static const struct pci_device_id com20020pci_id_table[] = {
-	{ 0x1571, 0xa001, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa002, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa003, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa004, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa005, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa006, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa007, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa008, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 },
-	{ 0x1571, 0xa009, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_IS_5MBIT },
-	{ 0x1571, 0xa00a, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_IS_5MBIT },
-	{ 0x1571, 0xa00b, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_IS_5MBIT },
-	{ 0x1571, 0xa00c, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_IS_5MBIT },
-	{ 0x1571, 0xa00d, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_IS_5MBIT },
-	{ 0x1571, 0xa00e, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_IS_5MBIT },
-	{ 0x1571, 0xa201, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{ 0x1571, 0xa202, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{ 0x1571, 0xa203, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{ 0x1571, 0xa204, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{ 0x1571, 0xa205, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{ 0x1571, 0xa206, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{ 0x10B5, 0x9030, 0x10B5,     0x2978,     0, 0, ARC_CAN_10MBIT },
-	{ 0x10B5, 0x9050, 0x10B5,     0x2273,     0, 0, ARC_CAN_10MBIT },
-	{ 0x14BA, 0x6000, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{ 0x10B5, 0x2200, PCI_ANY_ID, PCI_ANY_ID, 0, 0, ARC_CAN_10MBIT },
-	{0,}
+	{
+		0x1571, 0xa001,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0,
+	},
+	{
+		0x1571, 0xa002,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0,
+	},
+	{
+		0x1571, 0xa003,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0
+	},
+	{
+		0x1571, 0xa004,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0,
+	},
+	{
+		0x1571, 0xa005,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0
+	},
+	{
+		0x1571, 0xa006,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0
+	},
+	{
+		0x1571, 0xa007,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0
+	},
+	{
+		0x1571, 0xa008,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		0
+	},
+	{
+		0x1571, 0xa009,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_5mbit
+	},
+	{
+		0x1571, 0xa00a,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_5mbit
+	},
+	{
+		0x1571, 0xa00b,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_5mbit
+	},
+	{
+		0x1571, 0xa00c,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_5mbit
+	},
+	{
+		0x1571, 0xa00d,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_5mbit
+	},
+	{
+		0x1571, 0xa00e,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_5mbit
+	},
+	{
+		0x1571, 0xa201,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{
+		0x1571, 0xa202,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{
+		0x1571, 0xa203,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{
+		0x1571, 0xa204,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{
+		0x1571, 0xa205,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{
+		0x1571, 0xa206,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{
+		0x10B5, 0x9030,
+		0x10B5, 0x2978,
+		0, 0,
+		(kernel_ulong_t)&card_info_sohard
+	},
+	{
+		0x10B5, 0x9050,
+		0x10B5, 0x2273,
+		0, 0,
+		(kernel_ulong_t)&card_info_sohard
+	},
+	{
+		0x10B5, 0x9050,
+		0x10B5, 0x3292,
+		0, 0,
+		(kernel_ulong_t)&card_info_eae
+	},
+	{
+		0x14BA, 0x6000,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{
+		0x10B5, 0x2200,
+		PCI_ANY_ID, PCI_ANY_ID,
+		0, 0,
+		(kernel_ulong_t)&card_info_10mbit
+	},
+	{ 0, }
 };
 
 MODULE_DEVICE_TABLE(pci, com20020pci_id_table);
diff --git a/drivers/net/arcnet/com20020.c b/drivers/net/arcnet/com20020.c
index 7b96c5f47e8d..1a8437842fbc 100644
--- a/drivers/net/arcnet/com20020.c
+++ b/drivers/net/arcnet/com20020.c
@@ -149,11 +149,25 @@ int com20020_check(struct net_device *dev)
 	return 0;
 }
 
+static int com20020_set_hwaddr(struct net_device *dev, void *addr)
+{
+	int ioaddr = dev->base_addr;
+	struct arcnet_local *lp = netdev_priv(dev);
+	struct sockaddr *hwaddr = addr;
+
+	memcpy(dev->dev_addr, hwaddr->sa_data, 1);
+	SET_SUBADR(SUB_NODE);
+	outb(dev->dev_addr[0], _XREG);
+
+	return 0;
+}
+
 const struct net_device_ops com20020_netdev_ops = {
 	.ndo_open	= arcnet_open,
 	.ndo_stop	= arcnet_close,
 	.ndo_start_xmit = arcnet_send_packet,
 	.ndo_tx_timeout = arcnet_timeout,
+	.ndo_set_mac_address = com20020_set_hwaddr,
 	.ndo_set_rx_mode = com20020_set_mc_list,
 };
 
diff --git a/drivers/net/arcnet/com20020_cs.c b/drivers/net/arcnet/com20020_cs.c
index 1a790a20210d..057d9582132a 100644
--- a/drivers/net/arcnet/com20020_cs.c
+++ b/drivers/net/arcnet/com20020_cs.c
@@ -112,10 +112,6 @@ static void com20020_detach(struct pcmcia_device *p_dev);
 
 /*====================================================================*/
 
-struct com20020_dev {
-    struct net_device       *dev;
-};
-
 static int com20020_probe(struct pcmcia_device *p_dev)
 {
     struct com20020_dev *info;
diff --git a/drivers/net/bonding/bond_3ad.c b/drivers/net/bonding/bond_3ad.c
index ee2c73a9de39..2110215f3528 100644
--- a/drivers/net/bonding/bond_3ad.c
+++ b/drivers/net/bonding/bond_3ad.c
@@ -102,17 +102,20 @@ static const u8 lacpdu_mcast_addr[ETH_ALEN] = MULTICAST_LACPDU_ADDR;
 /* ================= main 802.3ad protocol functions ================== */
 static int ad_lacpdu_send(struct port *port);
 static int ad_marker_send(struct port *port, struct bond_marker *marker);
-static void ad_mux_machine(struct port *port);
+static void ad_mux_machine(struct port *port, bool *update_slave_arr);
 static void ad_rx_machine(struct lacpdu *lacpdu, struct port *port);
 static void ad_tx_machine(struct port *port);
 static void ad_periodic_machine(struct port *port);
-static void ad_port_selection_logic(struct port *port);
-static void ad_agg_selection_logic(struct aggregator *aggregator);
+static void ad_port_selection_logic(struct port *port, bool *update_slave_arr);
+static void ad_agg_selection_logic(struct aggregator *aggregator,
+				   bool *update_slave_arr);
 static void ad_clear_agg(struct aggregator *aggregator);
 static void ad_initialize_agg(struct aggregator *aggregator);
 static void ad_initialize_port(struct port *port, int lacp_fast);
-static void ad_enable_collecting_distributing(struct port *port);
-static void ad_disable_collecting_distributing(struct port *port);
+static void ad_enable_collecting_distributing(struct port *port,
+					      bool *update_slave_arr);
+static void ad_disable_collecting_distributing(struct port *port,
+					       bool *update_slave_arr);
 static void ad_marker_info_received(struct bond_marker *marker_info,
 				    struct port *port);
 static void ad_marker_response_received(struct bond_marker *marker,
@@ -234,24 +237,6 @@ static inline int __check_agg_selection_timer(struct port *port)
 }
 
 /**
- * __get_state_machine_lock - lock the port's state machines
- * @port: the port we're looking at
- */
-static inline void __get_state_machine_lock(struct port *port)
-{
-	spin_lock_bh(&(SLAVE_AD_INFO(port->slave)->state_machine_lock));
-}
-
-/**
- * __release_state_machine_lock - unlock the port's state machines
- * @port: the port we're looking at
- */
-static inline void __release_state_machine_lock(struct port *port)
-{
-	spin_unlock_bh(&(SLAVE_AD_INFO(port->slave)->state_machine_lock));
-}
-
-/**
  * __get_link_speed - get a port's speed
  * @port: the port we're looking at
  *
@@ -315,15 +300,14 @@ static u16 __get_link_speed(struct port *port)
 static u8 __get_duplex(struct port *port)
 {
 	struct slave *slave = port->slave;
-
 	u8 retval;
 
 	/* handling a special case: when the configuration starts with
 	 * link down, it sets the duplex to 0.
 	 */
-	if (slave->link != BOND_LINK_UP)
+	if (slave->link != BOND_LINK_UP) {
 		retval = 0x0;
-	else {
+	} else {
 		switch (slave->duplex) {
 		case DUPLEX_FULL:
 			retval = 0x1;
@@ -341,16 +325,6 @@ static u8 __get_duplex(struct port *port)
 	return retval;
 }
 
-/**
- * __initialize_port_locks - initialize a port's STATE machine spinlock
- * @port: the slave of the port we're looking at
- */
-static inline void __initialize_port_locks(struct slave *slave)
-{
-	/* make sure it isn't called twice */
-	spin_lock_init(&(SLAVE_AD_INFO(slave)->state_machine_lock));
-}
-
 /* Conversions */
 
 /**
@@ -825,8 +799,9 @@ static int ad_marker_send(struct port *port, struct bond_marker *marker)
 /**
  * ad_mux_machine - handle a port's mux state machine
  * @port: the port we're looking at
+ * @update_slave_arr: Does slave array need update?
  */
-static void ad_mux_machine(struct port *port)
+static void ad_mux_machine(struct port *port, bool *update_slave_arr)
 {
 	mux_states_t last_state;
 
@@ -930,7 +905,8 @@ static void ad_mux_machine(struct port *port)
 		switch (port->sm_mux_state) {
 		case AD_MUX_DETACHED:
 			port->actor_oper_port_state &= ~AD_STATE_SYNCHRONIZATION;
-			ad_disable_collecting_distributing(port);
+			ad_disable_collecting_distributing(port,
+							   update_slave_arr);
 			port->actor_oper_port_state &= ~AD_STATE_COLLECTING;
 			port->actor_oper_port_state &= ~AD_STATE_DISTRIBUTING;
 			port->ntt = true;
@@ -942,13 +918,15 @@ static void ad_mux_machine(struct port *port)
 			port->actor_oper_port_state |= AD_STATE_SYNCHRONIZATION;
 			port->actor_oper_port_state &= ~AD_STATE_COLLECTING;
 			port->actor_oper_port_state &= ~AD_STATE_DISTRIBUTING;
-			ad_disable_collecting_distributing(port);
+			ad_disable_collecting_distributing(port,
+							   update_slave_arr);
 			port->ntt = true;
 			break;
 		case AD_MUX_COLLECTING_DISTRIBUTING:
 			port->actor_oper_port_state |= AD_STATE_COLLECTING;
 			port->actor_oper_port_state |= AD_STATE_DISTRIBUTING;
-			ad_enable_collecting_distributing(port);
+			ad_enable_collecting_distributing(port,
+							  update_slave_arr);
 			port->ntt = true;
 			break;
 		default:
@@ -1216,12 +1194,13 @@ static void ad_periodic_machine(struct port *port)
 /**
  * ad_port_selection_logic - select aggregation groups
  * @port: the port we're looking at
+ * @update_slave_arr: Does slave array need update?
  *
  * Select aggregation groups, and assign each port for it's aggregetor. The
  * selection logic is called in the inititalization (after all the handshkes),
  * and after every lacpdu receive (if selected is off).
  */
-static void ad_port_selection_logic(struct port *port)
+static void ad_port_selection_logic(struct port *port, bool *update_slave_arr)
 {
 	struct aggregator *aggregator, *free_aggregator = NULL, *temp_aggregator;
 	struct port *last_port = NULL, *curr_port;
@@ -1376,7 +1355,7 @@ static void ad_port_selection_logic(struct port *port)
 			      __agg_ports_are_ready(port->aggregator));
 
 	aggregator = __get_first_agg(port);
-	ad_agg_selection_logic(aggregator);
+	ad_agg_selection_logic(aggregator, update_slave_arr);
 }
 
 /* Decide if "agg" is a better choice for the new active aggregator that
@@ -1464,6 +1443,7 @@ static int agg_device_up(const struct aggregator *agg)
 /**
  * ad_agg_selection_logic - select an aggregation group for a team
  * @aggregator: the aggregator we're looking at
+ * @update_slave_arr: Does slave array need update?
  *
  * It is assumed that only one aggregator may be selected for a team.
  *
@@ -1486,7 +1466,8 @@ static int agg_device_up(const struct aggregator *agg)
  * __get_active_agg() won't work correctly. This function should be better
  * called with the bond itself, and retrieve the first agg from it.
  */
-static void ad_agg_selection_logic(struct aggregator *agg)
+static void ad_agg_selection_logic(struct aggregator *agg,
+				   bool *update_slave_arr)
 {
 	struct aggregator *best, *active, *origin;
 	struct bonding *bond = agg->slave->bond;
@@ -1579,6 +1560,8 @@ static void ad_agg_selection_logic(struct aggregator *agg)
 				__disable_port(port);
 			}
 		}
+		/* Slave array needs update. */
+		*update_slave_arr = true;
 	}
 
 	/* if the selected aggregator is of join individuals
@@ -1707,24 +1690,30 @@ static void ad_initialize_port(struct port *port, int lacp_fast)
 /**
  * ad_enable_collecting_distributing - enable a port's transmit/receive
  * @port: the port we're looking at
+ * @update_slave_arr: Does slave array need update?
  *
  * Enable @port if it's in an active aggregator
  */
-static void ad_enable_collecting_distributing(struct port *port)
+static void ad_enable_collecting_distributing(struct port *port,
+					      bool *update_slave_arr)
 {
 	if (port->aggregator->is_active) {
 		pr_debug("Enabling port %d(LAG %d)\n",
 			 port->actor_port_number,
 			 port->aggregator->aggregator_identifier);
 		__enable_port(port);
+		/* Slave array needs update */
+		*update_slave_arr = true;
 	}
 }
 
 /**
  * ad_disable_collecting_distributing - disable a port's transmit/receive
  * @port: the port we're looking at
+ * @update_slave_arr: Does slave array need update?
  */
-static void ad_disable_collecting_distributing(struct port *port)
+static void ad_disable_collecting_distributing(struct port *port,
+					       bool *update_slave_arr)
 {
 	if (port->aggregator &&
 	    !MAC_ADDRESS_EQUAL(&(port->aggregator->partner_system),
@@ -1733,6 +1722,8 @@ static void ad_disable_collecting_distributing(struct port *port)
 			 port->actor_port_number,
 			 port->aggregator->aggregator_identifier);
 		__disable_port(port);
+		/* Slave array needs an update */
+		*update_slave_arr = true;
 	}
 }
 
@@ -1843,7 +1834,6 @@ void bond_3ad_bind_slave(struct slave *slave)
 
 		ad_initialize_port(port, bond->params.lacp_fast);
 
-		__initialize_port_locks(slave);
 		port->slave = slave;
 		port->actor_port_number = SLAVE_AD_INFO(slave)->id;
 		/* key is determined according to the link speed, duplex and user key(which
@@ -1898,7 +1888,10 @@ void bond_3ad_unbind_slave(struct slave *slave)
 	struct bonding *bond = slave->bond;
 	struct slave *slave_iter;
 	struct list_head *iter;
+	bool dummy_slave_update; /* Ignore this value as caller updates array */
 
+	/* Sync against bond_3ad_state_machine_handler() */
+	spin_lock_bh(&bond->mode_lock);
 	aggregator = &(SLAVE_AD_INFO(slave)->aggregator);
 	port = &(SLAVE_AD_INFO(slave)->port);
 
@@ -1906,7 +1899,7 @@ void bond_3ad_unbind_slave(struct slave *slave)
 	if (!port->slave) {
 		netdev_warn(bond->dev, "Trying to unbind an uninitialized port on %s\n",
 			    slave->dev->name);
-		return;
+		goto out;
 	}
 
 	netdev_dbg(bond->dev, "Unbinding Link Aggregation Group %d\n",
@@ -1979,7 +1972,8 @@ void bond_3ad_unbind_slave(struct slave *slave)
 				ad_clear_agg(aggregator);
 
 				if (select_new_active_agg)
-					ad_agg_selection_logic(__get_first_agg(port));
+					ad_agg_selection_logic(__get_first_agg(port),
+							       &dummy_slave_update);
 			} else {
 				netdev_warn(bond->dev, "unbinding aggregator, and could not find a new aggregator for its ports\n");
 			}
@@ -1994,7 +1988,8 @@ void bond_3ad_unbind_slave(struct slave *slave)
 				/* select new active aggregator */
 				temp_aggregator = __get_first_agg(port);
 				if (temp_aggregator)
-					ad_agg_selection_logic(temp_aggregator);
+					ad_agg_selection_logic(temp_aggregator,
+							       &dummy_slave_update);
 			}
 		}
 	}
@@ -2024,7 +2019,8 @@ void bond_3ad_unbind_slave(struct slave *slave)
 					if (select_new_active_agg) {
 						netdev_info(bond->dev, "Removing an active aggregator\n");
 						/* select new active aggregator */
-						ad_agg_selection_logic(__get_first_agg(port));
+						ad_agg_selection_logic(__get_first_agg(port),
+							               &dummy_slave_update);
 					}
 				}
 				break;
@@ -2032,6 +2028,9 @@ void bond_3ad_unbind_slave(struct slave *slave)
 		}
 	}
 	port->slave = NULL;
+
+out:
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 /**
@@ -2056,8 +2055,13 @@ void bond_3ad_state_machine_handler(struct work_struct *work)
 	struct slave *slave;
 	struct port *port;
 	bool should_notify_rtnl = BOND_SLAVE_NOTIFY_LATER;
+	bool update_slave_arr = false;
 
-	read_lock(&bond->lock);
+	/* Lock to protect data accessed by all (e.g., port->sm_vars) and
+	 * against running with bond_3ad_unbind_slave. ad_rx_machine may run
+	 * concurrently due to incoming LACPDU as well.
+	 */
+	spin_lock_bh(&bond->mode_lock);
 	rcu_read_lock();
 
 	/* check if there are any slaves */
@@ -2079,7 +2083,7 @@ void bond_3ad_state_machine_handler(struct work_struct *work)
 			}
 
 			aggregator = __get_first_agg(port);
-			ad_agg_selection_logic(aggregator);
+			ad_agg_selection_logic(aggregator, &update_slave_arr);
 		}
 		bond_3ad_set_carrier(bond);
 	}
@@ -2093,23 +2097,15 @@ void bond_3ad_state_machine_handler(struct work_struct *work)
 			goto re_arm;
 		}
 
-		/* Lock around state machines to protect data accessed
-		 * by all (e.g., port->sm_vars).  ad_rx_machine may run
-		 * concurrently due to incoming LACPDU.
-		 */
-		__get_state_machine_lock(port);
-
 		ad_rx_machine(NULL, port);
 		ad_periodic_machine(port);
-		ad_port_selection_logic(port);
-		ad_mux_machine(port);
+		ad_port_selection_logic(port, &update_slave_arr);
+		ad_mux_machine(port, &update_slave_arr);
 		ad_tx_machine(port);
 
 		/* turn off the BEGIN bit, since we already handled it */
 		if (port->sm_vars & AD_PORT_BEGIN)
 			port->sm_vars &= ~AD_PORT_BEGIN;
-
-		__release_state_machine_lock(port);
 	}
 
 re_arm:
@@ -2120,7 +2116,10 @@ re_arm:
 		}
 	}
 	rcu_read_unlock();
-	read_unlock(&bond->lock);
+	spin_unlock_bh(&bond->mode_lock);
+
+	if (update_slave_arr)
+		bond_slave_arr_work_rearm(bond, 0);
 
 	if (should_notify_rtnl && rtnl_trylock()) {
 		bond_slave_state_notify(bond);
@@ -2161,9 +2160,9 @@ static int bond_3ad_rx_indication(struct lacpdu *lacpdu, struct slave *slave,
 			netdev_dbg(slave->bond->dev, "Received LACPDU on port %d\n",
 				   port->actor_port_number);
 			/* Protect against concurrent state machines */
-			__get_state_machine_lock(port);
+			spin_lock(&slave->bond->mode_lock);
 			ad_rx_machine(lacpdu, port);
-			__release_state_machine_lock(port);
+			spin_unlock(&slave->bond->mode_lock);
 			break;
 
 		case AD_TYPE_MARKER:
@@ -2213,7 +2212,7 @@ void bond_3ad_adapter_speed_changed(struct slave *slave)
 		return;
 	}
 
-	__get_state_machine_lock(port);
+	spin_lock_bh(&slave->bond->mode_lock);
 
 	port->actor_admin_port_key &= ~AD_SPEED_KEY_BITS;
 	port->actor_oper_port_key = port->actor_admin_port_key |=
@@ -2224,7 +2223,7 @@ void bond_3ad_adapter_speed_changed(struct slave *slave)
 	 */
 	port->sm_vars |= AD_PORT_BEGIN;
 
-	__release_state_machine_lock(port);
+	spin_unlock_bh(&slave->bond->mode_lock);
 }
 
 /**
@@ -2246,7 +2245,7 @@ void bond_3ad_adapter_duplex_changed(struct slave *slave)
 		return;
 	}
 
-	__get_state_machine_lock(port);
+	spin_lock_bh(&slave->bond->mode_lock);
 
 	port->actor_admin_port_key &= ~AD_DUPLEX_KEY_BITS;
 	port->actor_oper_port_key = port->actor_admin_port_key |=
@@ -2257,7 +2256,7 @@ void bond_3ad_adapter_duplex_changed(struct slave *slave)
 	 */
 	port->sm_vars |= AD_PORT_BEGIN;
 
-	__release_state_machine_lock(port);
+	spin_unlock_bh(&slave->bond->mode_lock);
 }
 
 /**
@@ -2280,7 +2279,7 @@ void bond_3ad_handle_link_change(struct slave *slave, char link)
 		return;
 	}
 
-	__get_state_machine_lock(port);
+	spin_lock_bh(&slave->bond->mode_lock);
 	/* on link down we are zeroing duplex and speed since
 	 * some of the adaptors(ce1000.lan) report full duplex/speed
 	 * instead of N/A(duplex) / 0(speed).
@@ -2311,7 +2310,12 @@ void bond_3ad_handle_link_change(struct slave *slave, char link)
 	 */
 	port->sm_vars |= AD_PORT_BEGIN;
 
-	__release_state_machine_lock(port);
+	spin_unlock_bh(&slave->bond->mode_lock);
+
+	/* RTNL is held and mode_lock is released so it's safe
+	 * to update slave_array here.
+	 */
+	bond_update_slave_arr(slave->bond, NULL);
 }
 
 /**
@@ -2395,7 +2399,6 @@ int __bond_3ad_get_active_agg_info(struct bonding *bond,
 	return 0;
 }
 
-/* Wrapper used to hold bond->lock so no slave manipulation can occur */
 int bond_3ad_get_active_agg_info(struct bonding *bond, struct ad_info *ad_info)
 {
 	int ret;
@@ -2407,90 +2410,19 @@ int bond_3ad_get_active_agg_info(struct bonding *bond, struct ad_info *ad_info)
 	return ret;
 }
 
-int bond_3ad_xmit_xor(struct sk_buff *skb, struct net_device *dev)
-{
-	struct bonding *bond = netdev_priv(dev);
-	struct slave *slave, *first_ok_slave;
-	struct aggregator *agg;
-	struct ad_info ad_info;
-	struct list_head *iter;
-	int slaves_in_agg;
-	int slave_agg_no;
-	int agg_id;
-
-	if (__bond_3ad_get_active_agg_info(bond, &ad_info)) {
-		netdev_dbg(dev, "__bond_3ad_get_active_agg_info failed\n");
-		goto err_free;
-	}
-
-	slaves_in_agg = ad_info.ports;
-	agg_id = ad_info.aggregator_id;
-
-	if (slaves_in_agg == 0) {
-		netdev_dbg(dev, "active aggregator is empty\n");
-		goto err_free;
-	}
-
-	slave_agg_no = bond_xmit_hash(bond, skb) % slaves_in_agg;
-	first_ok_slave = NULL;
-
-	bond_for_each_slave_rcu(bond, slave, iter) {
-		agg = SLAVE_AD_INFO(slave)->port.aggregator;
-		if (!agg || agg->aggregator_identifier != agg_id)
-			continue;
-
-		if (slave_agg_no >= 0) {
-			if (!first_ok_slave && bond_slave_can_tx(slave))
-				first_ok_slave = slave;
-			slave_agg_no--;
-			continue;
-		}
-
-		if (bond_slave_can_tx(slave)) {
-			bond_dev_queue_xmit(bond, skb, slave->dev);
-			goto out;
-		}
-	}
-
-	if (slave_agg_no >= 0) {
-		netdev_err(dev, "Couldn't find a slave to tx on for aggregator ID %d\n",
-			   agg_id);
-		goto err_free;
-	}
-
-	/* we couldn't find any suitable slave after the agg_no, so use the
-	 * first suitable found, if found.
-	 */
-	if (first_ok_slave)
-		bond_dev_queue_xmit(bond, skb, first_ok_slave->dev);
-	else
-		goto err_free;
-
-out:
-	return NETDEV_TX_OK;
-err_free:
-	/* no suitable interface, frame not sent */
-	dev_kfree_skb_any(skb);
-	goto out;
-}
-
 int bond_3ad_lacpdu_recv(const struct sk_buff *skb, struct bonding *bond,
 			 struct slave *slave)
 {
-	int ret = RX_HANDLER_ANOTHER;
 	struct lacpdu *lacpdu, _lacpdu;
 
 	if (skb->protocol != PKT_TYPE_LACPDU)
-		return ret;
+		return RX_HANDLER_ANOTHER;
 
 	lacpdu = skb_header_pointer(skb, 0, sizeof(_lacpdu), &_lacpdu);
 	if (!lacpdu)
-		return ret;
+		return RX_HANDLER_ANOTHER;
 
-	read_lock(&bond->lock);
-	ret = bond_3ad_rx_indication(lacpdu, slave, skb->len);
-	read_unlock(&bond->lock);
-	return ret;
+	return bond_3ad_rx_indication(lacpdu, slave, skb->len);
 }
 
 /**
@@ -2500,7 +2432,7 @@ int bond_3ad_lacpdu_recv(const struct sk_buff *skb, struct bonding *bond,
  * When modify lacp_rate parameter via sysfs,
  * update actor_oper_port_state of each port.
  *
- * Hold slave->state_machine_lock,
+ * Hold bond->mode_lock,
  * so we can modify port->actor_oper_port_state,
  * no matter bond is up or down.
  */
@@ -2512,13 +2444,13 @@ void bond_3ad_update_lacp_rate(struct bonding *bond)
 	int lacp_fast;
 
 	lacp_fast = bond->params.lacp_fast;
+	spin_lock_bh(&bond->mode_lock);
 	bond_for_each_slave(bond, slave, iter) {
 		port = &(SLAVE_AD_INFO(slave)->port);
-		__get_state_machine_lock(port);
 		if (lacp_fast)
 			port->actor_oper_port_state |= AD_STATE_LACP_TIMEOUT;
 		else
 			port->actor_oper_port_state &= ~AD_STATE_LACP_TIMEOUT;
-		__release_state_machine_lock(port);
 	}
+	spin_unlock_bh(&bond->mode_lock);
 }
diff --git a/drivers/net/bonding/bond_3ad.h b/drivers/net/bonding/bond_3ad.h
index bb03b1df2f3e..c5f14ac63f3e 100644
--- a/drivers/net/bonding/bond_3ad.h
+++ b/drivers/net/bonding/bond_3ad.h
@@ -259,7 +259,6 @@ struct ad_bond_info {
 struct ad_slave_info {
 	struct aggregator aggregator;	/* 802.3ad aggregator structure */
 	struct port port;		/* 802.3ad port structure */
-	spinlock_t state_machine_lock;	/* mutex state machines vs. incoming LACPDU */
 	u16 id;
 };
 
diff --git a/drivers/net/bonding/bond_alb.c b/drivers/net/bonding/bond_alb.c
index 95dd1f58c260..d2eadab787c5 100644
--- a/drivers/net/bonding/bond_alb.c
+++ b/drivers/net/bonding/bond_alb.c
@@ -100,27 +100,6 @@ static inline u8 _simple_hash(const u8 *hash_start, int hash_size)
 
 /*********************** tlb specific functions ***************************/
 
-static inline void _lock_tx_hashtbl_bh(struct bonding *bond)
-{
-	spin_lock_bh(&(BOND_ALB_INFO(bond).tx_hashtbl_lock));
-}
-
-static inline void _unlock_tx_hashtbl_bh(struct bonding *bond)
-{
-	spin_unlock_bh(&(BOND_ALB_INFO(bond).tx_hashtbl_lock));
-}
-
-static inline void _lock_tx_hashtbl(struct bonding *bond)
-{
-	spin_lock(&(BOND_ALB_INFO(bond).tx_hashtbl_lock));
-}
-
-static inline void _unlock_tx_hashtbl(struct bonding *bond)
-{
-	spin_unlock(&(BOND_ALB_INFO(bond).tx_hashtbl_lock));
-}
-
-/* Caller must hold tx_hashtbl lock */
 static inline void tlb_init_table_entry(struct tlb_client_info *entry, int save_load)
 {
 	if (save_load) {
@@ -140,7 +119,6 @@ static inline void tlb_init_slave(struct slave *slave)
 	SLAVE_TLB_INFO(slave).head = TLB_NULL_INDEX;
 }
 
-/* Caller must hold bond lock for read, BH disabled */
 static void __tlb_clear_slave(struct bonding *bond, struct slave *slave,
 			 int save_load)
 {
@@ -163,13 +141,12 @@ static void __tlb_clear_slave(struct bonding *bond, struct slave *slave,
 	tlb_init_slave(slave);
 }
 
-/* Caller must hold bond lock for read */
 static void tlb_clear_slave(struct bonding *bond, struct slave *slave,
 			 int save_load)
 {
-	_lock_tx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 	__tlb_clear_slave(bond, slave, save_load);
-	_unlock_tx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 /* Must be called before starting the monitor timer */
@@ -184,14 +161,14 @@ static int tlb_initialize(struct bonding *bond)
 	if (!new_hashtbl)
 		return -1;
 
-	_lock_tx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	bond_info->tx_hashtbl = new_hashtbl;
 
 	for (i = 0; i < TLB_HASH_TABLE_SIZE; i++)
 		tlb_init_table_entry(&bond_info->tx_hashtbl[i], 0);
 
-	_unlock_tx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 
 	return 0;
 }
@@ -200,18 +177,13 @@ static int tlb_initialize(struct bonding *bond)
 static void tlb_deinitialize(struct bonding *bond)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
-	struct tlb_up_slave *arr;
 
-	_lock_tx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	kfree(bond_info->tx_hashtbl);
 	bond_info->tx_hashtbl = NULL;
 
-	_unlock_tx_hashtbl_bh(bond);
-
-	arr = rtnl_dereference(bond_info->slave_arr);
-	if (arr)
-		kfree_rcu(arr, rcu);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 static long long compute_gap(struct slave *slave)
@@ -220,7 +192,6 @@ static long long compute_gap(struct slave *slave)
 	       (s64) (SLAVE_TLB_INFO(slave).load << 3); /* Bytes to bits */
 }
 
-/* Caller must hold bond lock for read */
 static struct slave *tlb_get_least_loaded_slave(struct bonding *bond)
 {
 	struct slave *slave, *least_loaded;
@@ -281,42 +252,23 @@ static struct slave *__tlb_choose_channel(struct bonding *bond, u32 hash_index,
 	return assigned_slave;
 }
 
-/* Caller must hold bond lock for read */
 static struct slave *tlb_choose_channel(struct bonding *bond, u32 hash_index,
 					u32 skb_len)
 {
 	struct slave *tx_slave;
-	/*
-	 * We don't need to disable softirq here, becase
+
+	/* We don't need to disable softirq here, becase
 	 * tlb_choose_channel() is only called by bond_alb_xmit()
 	 * which already has softirq disabled.
 	 */
-	_lock_tx_hashtbl(bond);
+	spin_lock(&bond->mode_lock);
 	tx_slave = __tlb_choose_channel(bond, hash_index, skb_len);
-	_unlock_tx_hashtbl(bond);
+	spin_unlock(&bond->mode_lock);
+
 	return tx_slave;
 }
 
 /*********************** rlb specific functions ***************************/
-static inline void _lock_rx_hashtbl_bh(struct bonding *bond)
-{
-	spin_lock_bh(&(BOND_ALB_INFO(bond).rx_hashtbl_lock));
-}
-
-static inline void _unlock_rx_hashtbl_bh(struct bonding *bond)
-{
-	spin_unlock_bh(&(BOND_ALB_INFO(bond).rx_hashtbl_lock));
-}
-
-static inline void _lock_rx_hashtbl(struct bonding *bond)
-{
-	spin_lock(&(BOND_ALB_INFO(bond).rx_hashtbl_lock));
-}
-
-static inline void _unlock_rx_hashtbl(struct bonding *bond)
-{
-	spin_unlock(&(BOND_ALB_INFO(bond).rx_hashtbl_lock));
-}
 
 /* when an ARP REPLY is received from a client update its info
  * in the rx_hashtbl
@@ -327,7 +279,7 @@ static void rlb_update_entry_from_arp(struct bonding *bond, struct arp_pkt *arp)
 	struct rlb_client_info *client_info;
 	u32 hash_index;
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	hash_index = _simple_hash((u8 *)&(arp->ip_src), sizeof(arp->ip_src));
 	client_info = &(bond_info->rx_hashtbl[hash_index]);
@@ -342,7 +294,7 @@ static void rlb_update_entry_from_arp(struct bonding *bond, struct arp_pkt *arp)
 		bond_info->rx_ntt = 1;
 	}
 
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 static int rlb_arp_recv(const struct sk_buff *skb, struct bonding *bond,
@@ -378,15 +330,15 @@ out:
 	return RX_HANDLER_ANOTHER;
 }
 
-/* Caller must hold bond lock for read */
-static struct slave *rlb_next_rx_slave(struct bonding *bond)
+/* Caller must hold rcu_read_lock() */
+static struct slave *__rlb_next_rx_slave(struct bonding *bond)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
 	struct slave *before = NULL, *rx_slave = NULL, *slave;
 	struct list_head *iter;
 	bool found = false;
 
-	bond_for_each_slave(bond, slave, iter) {
+	bond_for_each_slave_rcu(bond, slave, iter) {
 		if (!bond_slave_can_tx(slave))
 			continue;
 		if (!found) {
@@ -411,35 +363,16 @@ static struct slave *rlb_next_rx_slave(struct bonding *bond)
 	return rx_slave;
 }
 
-/* Caller must hold rcu_read_lock() for read */
-static struct slave *__rlb_next_rx_slave(struct bonding *bond)
+/* Caller must hold RTNL, rcu_read_lock is obtained only to silence checkers */
+static struct slave *rlb_next_rx_slave(struct bonding *bond)
 {
-	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
-	struct slave *before = NULL, *rx_slave = NULL, *slave;
-	struct list_head *iter;
-	bool found = false;
+	struct slave *rx_slave;
 
-	bond_for_each_slave_rcu(bond, slave, iter) {
-		if (!bond_slave_can_tx(slave))
-			continue;
-		if (!found) {
-			if (!before || before->speed < slave->speed)
-				before = slave;
-		} else {
-			if (!rx_slave || rx_slave->speed < slave->speed)
-				rx_slave = slave;
-		}
-		if (slave == bond_info->rx_slave)
-			found = true;
-	}
-	/* we didn't find anything after the current or we have something
-	 * better before and up to the current slave
-	 */
-	if (!rx_slave || (before && rx_slave->speed < before->speed))
-		rx_slave = before;
+	ASSERT_RTNL();
 
-	if (rx_slave)
-		bond_info->rx_slave = rx_slave;
+	rcu_read_lock();
+	rx_slave = __rlb_next_rx_slave(bond);
+	rcu_read_unlock();
 
 	return rx_slave;
 }
@@ -447,11 +380,11 @@ static struct slave *__rlb_next_rx_slave(struct bonding *bond)
 /* teach the switch the mac of a disabled slave
  * on the primary for fault tolerance
  *
- * Caller must hold bond->curr_slave_lock for write or bond lock for write
+ * Caller must hold RTNL
  */
 static void rlb_teach_disabled_mac_on_primary(struct bonding *bond, u8 addr[])
 {
-	struct slave *curr_active = bond_deref_active_protected(bond);
+	struct slave *curr_active = rtnl_dereference(bond->curr_active_slave);
 
 	if (!curr_active)
 		return;
@@ -479,7 +412,7 @@ static void rlb_clear_slave(struct bonding *bond, struct slave *slave)
 	u32 index, next_index;
 
 	/* clear slave from rx_hashtbl */
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	rx_hash_table = bond_info->rx_hashtbl;
 	index = bond_info->rx_hashtbl_used_head;
@@ -510,14 +443,10 @@ static void rlb_clear_slave(struct bonding *bond, struct slave *slave)
 		}
 	}
 
-	_unlock_rx_hashtbl_bh(bond);
-
-	write_lock_bh(&bond->curr_slave_lock);
+	spin_unlock_bh(&bond->mode_lock);
 
-	if (slave != bond_deref_active_protected(bond))
+	if (slave != rtnl_dereference(bond->curr_active_slave))
 		rlb_teach_disabled_mac_on_primary(bond, slave->dev->dev_addr);
-
-	write_unlock_bh(&bond->curr_slave_lock);
 }
 
 static void rlb_update_client(struct rlb_client_info *client_info)
@@ -565,7 +494,7 @@ static void rlb_update_rx_clients(struct bonding *bond)
 	struct rlb_client_info *client_info;
 	u32 hash_index;
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	hash_index = bond_info->rx_hashtbl_used_head;
 	for (; hash_index != RLB_NULL_INDEX;
@@ -583,7 +512,7 @@ static void rlb_update_rx_clients(struct bonding *bond)
 	 */
 	bond_info->rlb_update_delay_counter = RLB_UPDATE_DELAY;
 
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 /* The slave was assigned a new mac address - update the clients */
@@ -594,7 +523,7 @@ static void rlb_req_update_slave_clients(struct bonding *bond, struct slave *sla
 	int ntt = 0;
 	u32 hash_index;
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	hash_index = bond_info->rx_hashtbl_used_head;
 	for (; hash_index != RLB_NULL_INDEX;
@@ -615,7 +544,7 @@ static void rlb_req_update_slave_clients(struct bonding *bond, struct slave *sla
 		bond_info->rlb_update_retry_counter = RLB_UPDATE_RETRY;
 	}
 
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 /* mark all clients using src_ip to be updated */
@@ -625,7 +554,7 @@ static void rlb_req_update_subnet_clients(struct bonding *bond, __be32 src_ip)
 	struct rlb_client_info *client_info;
 	u32 hash_index;
 
-	_lock_rx_hashtbl(bond);
+	spin_lock(&bond->mode_lock);
 
 	hash_index = bond_info->rx_hashtbl_used_head;
 	for (; hash_index != RLB_NULL_INDEX;
@@ -636,7 +565,7 @@ static void rlb_req_update_subnet_clients(struct bonding *bond, __be32 src_ip)
 			netdev_err(bond->dev, "found a client with no channel in the client's hash table\n");
 			continue;
 		}
-		/*update all clients using this src_ip, that are not assigned
+		/* update all clients using this src_ip, that are not assigned
 		 * to the team's address (curr_active_slave) and have a known
 		 * unicast mac address.
 		 */
@@ -649,10 +578,9 @@ static void rlb_req_update_subnet_clients(struct bonding *bond, __be32 src_ip)
 		}
 	}
 
-	_unlock_rx_hashtbl(bond);
+	spin_unlock(&bond->mode_lock);
 }
 
-/* Caller must hold both bond and ptr locks for read */
 static struct slave *rlb_choose_channel(struct sk_buff *skb, struct bonding *bond)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
@@ -661,7 +589,7 @@ static struct slave *rlb_choose_channel(struct sk_buff *skb, struct bonding *bon
 	struct rlb_client_info *client_info;
 	u32 hash_index = 0;
 
-	_lock_rx_hashtbl(bond);
+	spin_lock(&bond->mode_lock);
 
 	curr_active_slave = rcu_dereference(bond->curr_active_slave);
 
@@ -680,7 +608,7 @@ static struct slave *rlb_choose_channel(struct sk_buff *skb, struct bonding *bon
 
 			assigned_slave = client_info->slave;
 			if (assigned_slave) {
-				_unlock_rx_hashtbl(bond);
+				spin_unlock(&bond->mode_lock);
 				return assigned_slave;
 			}
 		} else {
@@ -742,7 +670,7 @@ static struct slave *rlb_choose_channel(struct sk_buff *skb, struct bonding *bon
 		}
 	}
 
-	_unlock_rx_hashtbl(bond);
+	spin_unlock(&bond->mode_lock);
 
 	return assigned_slave;
 }
@@ -763,9 +691,7 @@ static struct slave *rlb_arp_xmit(struct sk_buff *skb, struct bonding *bond)
 		return NULL;
 
 	if (arp->op_code == htons(ARPOP_REPLY)) {
-		/* the arp must be sent on the selected
-		* rx channel
-		*/
+		/* the arp must be sent on the selected rx channel */
 		tx_slave = rlb_choose_channel(skb, bond);
 		if (tx_slave)
 			ether_addr_copy(arp->mac_src, tx_slave->dev->dev_addr);
@@ -795,7 +721,6 @@ static struct slave *rlb_arp_xmit(struct sk_buff *skb, struct bonding *bond)
 	return tx_slave;
 }
 
-/* Caller must hold bond lock for read */
 static void rlb_rebalance(struct bonding *bond)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
@@ -804,7 +729,7 @@ static void rlb_rebalance(struct bonding *bond)
 	int ntt;
 	u32 hash_index;
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	ntt = 0;
 	hash_index = bond_info->rx_hashtbl_used_head;
@@ -822,10 +747,10 @@ static void rlb_rebalance(struct bonding *bond)
 	/* update the team's flag only after the whole iteration */
 	if (ntt)
 		bond_info->rx_ntt = 1;
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
-/* Caller must hold rx_hashtbl lock */
+/* Caller must hold mode_lock */
 static void rlb_init_table_entry_dst(struct rlb_client_info *entry)
 {
 	entry->used_next = RLB_NULL_INDEX;
@@ -913,15 +838,16 @@ static void rlb_src_link(struct bonding *bond, u32 ip_src_hash, u32 ip_dst_hash)
 	bond_info->rx_hashtbl[ip_src_hash].src_first = ip_dst_hash;
 }
 
-/* deletes all rx_hashtbl entries with  arp->ip_src if their mac_src does
- * not match arp->mac_src */
+/* deletes all rx_hashtbl entries with arp->ip_src if their mac_src does
+ * not match arp->mac_src
+ */
 static void rlb_purge_src_ip(struct bonding *bond, struct arp_pkt *arp)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
 	u32 ip_src_hash = _simple_hash((u8 *)&(arp->ip_src), sizeof(arp->ip_src));
 	u32 index;
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	index = bond_info->rx_hashtbl[ip_src_hash].src_first;
 	while (index != RLB_NULL_INDEX) {
@@ -932,7 +858,7 @@ static void rlb_purge_src_ip(struct bonding *bond, struct arp_pkt *arp)
 				rlb_delete_table_entry(bond, index);
 		index = next_index;
 	}
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 static int rlb_initialize(struct bonding *bond)
@@ -946,7 +872,7 @@ static int rlb_initialize(struct bonding *bond)
 	if (!new_hashtbl)
 		return -1;
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	bond_info->rx_hashtbl = new_hashtbl;
 
@@ -955,7 +881,7 @@ static int rlb_initialize(struct bonding *bond)
 	for (i = 0; i < RLB_HASH_TABLE_SIZE; i++)
 		rlb_init_table_entry(bond_info->rx_hashtbl + i);
 
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 
 	/* register to receive ARPs */
 	bond->recv_probe = rlb_arp_recv;
@@ -967,13 +893,13 @@ static void rlb_deinitialize(struct bonding *bond)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	kfree(bond_info->rx_hashtbl);
 	bond_info->rx_hashtbl = NULL;
 	bond_info->rx_hashtbl_used_head = RLB_NULL_INDEX;
 
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 static void rlb_clear_vlan(struct bonding *bond, unsigned short vlan_id)
@@ -981,7 +907,7 @@ static void rlb_clear_vlan(struct bonding *bond, unsigned short vlan_id)
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
 	u32 curr_index;
 
-	_lock_rx_hashtbl_bh(bond);
+	spin_lock_bh(&bond->mode_lock);
 
 	curr_index = bond_info->rx_hashtbl_used_head;
 	while (curr_index != RLB_NULL_INDEX) {
@@ -994,7 +920,7 @@ static void rlb_clear_vlan(struct bonding *bond, unsigned short vlan_id)
 		curr_index = next_index;
 	}
 
-	_unlock_rx_hashtbl_bh(bond);
+	spin_unlock_bh(&bond->mode_lock);
 }
 
 /*********************** tlb/rlb shared functions *********************/
@@ -1091,8 +1017,9 @@ static int alb_set_slave_mac_addr(struct slave *slave, u8 addr[])
 		return 0;
 	}
 
-	/* for rlb each slave must have a unique hw mac addresses so that */
-	/* each slave will receive packets destined to a different mac */
+	/* for rlb each slave must have a unique hw mac addresses so that
+	 * each slave will receive packets destined to a different mac
+	 */
 	memcpy(s_addr.sa_data, addr, dev->addr_len);
 	s_addr.sa_family = dev->type;
 	if (dev_set_mac_address(dev, &s_addr)) {
@@ -1103,13 +1030,10 @@ static int alb_set_slave_mac_addr(struct slave *slave, u8 addr[])
 	return 0;
 }
 
-/*
- * Swap MAC addresses between two slaves.
+/* Swap MAC addresses between two slaves.
  *
  * Called with RTNL held, and no other locks.
- *
  */
-
 static void alb_swap_mac_addr(struct slave *slave1, struct slave *slave2)
 {
 	u8 tmp_mac_addr[ETH_ALEN];
@@ -1120,8 +1044,7 @@ static void alb_swap_mac_addr(struct slave *slave1, struct slave *slave2)
 
 }
 
-/*
- * Send learning packets after MAC address swap.
+/* Send learning packets after MAC address swap.
  *
  * Called with RTNL and no other locks
  */
@@ -1194,7 +1117,6 @@ static void alb_change_hw_addr_on_detach(struct bonding *bond, struct slave *sla
 		found_slave = bond_slave_has_mac(bond, slave->perm_hwaddr);
 
 		if (found_slave) {
-			/* locking: needs RTNL and nothing else */
 			alb_swap_mac_addr(slave, found_slave);
 			alb_fasten_mac_swap(bond, slave, found_slave);
 		}
@@ -1243,7 +1165,8 @@ static int alb_handle_addr_collision_on_attach(struct bonding *bond, struct slav
 			return 0;
 
 		/* Try setting slave mac to bond address and fall-through
-		   to code handling that situation below... */
+		 * to code handling that situation below...
+		 */
 		alb_set_slave_mac_addr(slave, bond->dev->dev_addr);
 	}
 
@@ -1351,7 +1274,6 @@ int bond_alb_initialize(struct bonding *bond, int rlb_enabled)
 
 	if (rlb_enabled) {
 		bond->alb_info.rlb_enabled = 1;
-		/* initialize rlb */
 		res = rlb_initialize(bond);
 		if (res) {
 			tlb_deinitialize(bond);
@@ -1375,7 +1297,7 @@ void bond_alb_deinitialize(struct bonding *bond)
 }
 
 static int bond_do_alb_xmit(struct sk_buff *skb, struct bonding *bond,
-		struct slave *tx_slave)
+			    struct slave *tx_slave)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
 	struct ethhdr *eth_data = eth_hdr(skb);
@@ -1388,7 +1310,7 @@ static int bond_do_alb_xmit(struct sk_buff *skb, struct bonding *bond,
 	}
 
 	if (tx_slave && bond_slave_can_tx(tx_slave)) {
-		if (tx_slave != rcu_dereference(bond->curr_active_slave)) {
+		if (tx_slave != rcu_access_pointer(bond->curr_active_slave)) {
 			ether_addr_copy(eth_data->h_source,
 					tx_slave->dev->dev_addr);
 		}
@@ -1398,9 +1320,9 @@ static int bond_do_alb_xmit(struct sk_buff *skb, struct bonding *bond,
 	}
 
 	if (tx_slave && bond->params.tlb_dynamic_lb) {
-		_lock_tx_hashtbl(bond);
+		spin_lock(&bond->mode_lock);
 		__tlb_clear_slave(bond, tx_slave, 0);
-		_unlock_tx_hashtbl(bond);
+		spin_unlock(&bond->mode_lock);
 	}
 
 	/* no suitable interface, frame not sent */
@@ -1409,39 +1331,9 @@ out:
 	return NETDEV_TX_OK;
 }
 
-static int bond_tlb_update_slave_arr(struct bonding *bond,
-				     struct slave *skipslave)
-{
-	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
-	struct slave *tx_slave;
-	struct list_head *iter;
-	struct tlb_up_slave *new_arr, *old_arr;
-
-	new_arr = kzalloc(offsetof(struct tlb_up_slave, arr[bond->slave_cnt]),
-			  GFP_ATOMIC);
-	if (!new_arr)
-		return -ENOMEM;
-
-	bond_for_each_slave(bond, tx_slave, iter) {
-		if (!bond_slave_can_tx(tx_slave))
-			continue;
-		if (skipslave == tx_slave)
-			continue;
-		new_arr->arr[new_arr->count++] = tx_slave;
-	}
-
-	old_arr = rtnl_dereference(bond_info->slave_arr);
-	rcu_assign_pointer(bond_info->slave_arr, new_arr);
-	if (old_arr)
-		kfree_rcu(old_arr, rcu);
-
-	return 0;
-}
-
 int bond_tlb_xmit(struct sk_buff *skb, struct net_device *bond_dev)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
-	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
 	struct ethhdr *eth_data;
 	struct slave *tx_slave = NULL;
 	u32 hash_index;
@@ -1462,12 +1354,14 @@ int bond_tlb_xmit(struct sk_buff *skb, struct net_device *bond_dev)
 							      hash_index & 0xFF,
 							      skb->len);
 			} else {
-				struct tlb_up_slave *slaves;
+				struct bond_up_slave *slaves;
+				unsigned int count;
 
-				slaves = rcu_dereference(bond_info->slave_arr);
-				if (slaves && slaves->count)
+				slaves = rcu_dereference(bond->slave_arr);
+				count = slaves ? ACCESS_ONCE(slaves->count) : 0;
+				if (likely(count))
 					tx_slave = slaves->arr[hash_index %
-							       slaves->count];
+							       count];
 			}
 			break;
 		}
@@ -1595,13 +1489,6 @@ void bond_alb_monitor(struct work_struct *work)
 	if (bond_info->lp_counter >= BOND_ALB_LP_TICKS(bond)) {
 		bool strict_match;
 
-		/* change of curr_active_slave involves swapping of mac addresses.
-		 * in order to avoid this swapping from happening while
-		 * sending the learning packets, the curr_slave_lock must be held for
-		 * read.
-		 */
-		read_lock(&bond->curr_slave_lock);
-
 		bond_for_each_slave_rcu(bond, slave, iter) {
 			/* If updating current_active, use all currently
 			 * user mac addreses (!strict_match).  Otherwise, only
@@ -1613,17 +1500,11 @@ void bond_alb_monitor(struct work_struct *work)
 			alb_send_learning_packets(slave, slave->dev->dev_addr,
 						  strict_match);
 		}
-
-		read_unlock(&bond->curr_slave_lock);
-
 		bond_info->lp_counter = 0;
 	}
 
 	/* rebalance tx traffic */
 	if (bond_info->tx_rebalance_counter >= BOND_TLB_REBALANCE_TICKS) {
-
-		read_lock(&bond->curr_slave_lock);
-
 		bond_for_each_slave_rcu(bond, slave, iter) {
 			tlb_clear_slave(bond, slave, 1);
 			if (slave == rcu_access_pointer(bond->curr_active_slave)) {
@@ -1633,19 +1514,14 @@ void bond_alb_monitor(struct work_struct *work)
 				bond_info->unbalanced_load = 0;
 			}
 		}
-
-		read_unlock(&bond->curr_slave_lock);
-
 		bond_info->tx_rebalance_counter = 0;
 	}
 
-	/* handle rlb stuff */
 	if (bond_info->rlb_enabled) {
 		if (bond_info->primary_is_promisc &&
 		    (++bond_info->rlb_promisc_timeout_counter >= RLB_PROMISC_TIMEOUT)) {
 
-			/*
-			 * dev_set_promiscuity requires rtnl and
+			/* dev_set_promiscuity requires rtnl and
 			 * nothing else.  Avoid race with bond_close.
 			 */
 			rcu_read_unlock();
@@ -1715,8 +1591,7 @@ int bond_alb_init_slave(struct bonding *bond, struct slave *slave)
 	return 0;
 }
 
-/*
- * Remove slave from tlb and rlb hash tables, and fix up MAC addresses
+/* Remove slave from tlb and rlb hash tables, and fix up MAC addresses
  * if necessary.
  *
  * Caller must hold RTNL and no other locks
@@ -1733,13 +1608,8 @@ void bond_alb_deinit_slave(struct bonding *bond, struct slave *slave)
 		rlb_clear_slave(bond, slave);
 	}
 
-	if (bond_is_nondyn_tlb(bond))
-		if (bond_tlb_update_slave_arr(bond, slave))
-			pr_err("Failed to build slave-array for TLB mode.\n");
-
 }
 
-/* Caller must hold bond lock for read */
 void bond_alb_handle_link_change(struct bonding *bond, struct slave *slave, char link)
 {
 	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
@@ -1762,7 +1632,7 @@ void bond_alb_handle_link_change(struct bonding *bond, struct slave *slave, char
 	}
 
 	if (bond_is_nondyn_tlb(bond)) {
-		if (bond_tlb_update_slave_arr(bond, NULL))
+		if (bond_update_slave_arr(bond, NULL))
 			pr_err("Failed to build slave-array for TLB mode.\n");
 	}
 }
@@ -1775,22 +1645,14 @@ void bond_alb_handle_link_change(struct bonding *bond, struct slave *slave, char
  * Set the bond->curr_active_slave to @new_slave and handle
  * mac address swapping and promiscuity changes as needed.
  *
- * If new_slave is NULL, caller must hold curr_slave_lock or
- * bond->lock for write.
- *
- * If new_slave is not NULL, caller must hold RTNL, curr_slave_lock
- * for write.  Processing here may sleep, so no other locks may be held.
+ * Caller must hold RTNL
  */
 void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave)
-	__releases(&bond->curr_slave_lock)
-	__acquires(&bond->curr_slave_lock)
 {
 	struct slave *swap_slave;
 	struct slave *curr_active;
 
-	curr_active = rcu_dereference_protected(bond->curr_active_slave,
-						!new_slave ||
-						lockdep_is_held(&bond->curr_slave_lock));
+	curr_active = rtnl_dereference(bond->curr_active_slave);
 	if (curr_active == new_slave)
 		return;
 
@@ -1812,8 +1674,7 @@ void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave
 	if (!swap_slave)
 		swap_slave = bond_slave_has_mac(bond, bond->dev->dev_addr);
 
-	/*
-	 * Arrange for swap_slave and new_slave to temporarily be
+	/* Arrange for swap_slave and new_slave to temporarily be
 	 * ignored so we can mess with their MAC addresses without
 	 * fear of interference from transmit activity.
 	 */
@@ -1821,10 +1682,6 @@ void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave
 		tlb_clear_slave(bond, swap_slave, 1);
 	tlb_clear_slave(bond, new_slave, 1);
 
-	write_unlock_bh(&bond->curr_slave_lock);
-
-	ASSERT_RTNL();
-
 	/* in TLB mode, the slave might flip down/up with the old dev_addr,
 	 * and thus filter bond->dev_addr's packets, so force bond's mac
 	 */
@@ -1853,16 +1710,10 @@ void bond_alb_handle_active_change(struct bonding *bond, struct slave *new_slave
 		alb_send_learning_packets(new_slave, bond->dev->dev_addr,
 					  false);
 	}
-
-	write_lock_bh(&bond->curr_slave_lock);
 }
 
-/*
- * Called with RTNL
- */
+/* Called with RTNL */
 int bond_alb_set_mac_address(struct net_device *bond_dev, void *addr)
-	__acquires(&bond->lock)
-	__releases(&bond->lock)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 	struct sockaddr *sa = addr;
@@ -1895,14 +1746,12 @@ int bond_alb_set_mac_address(struct net_device *bond_dev, void *addr)
 	} else {
 		alb_set_slave_mac_addr(curr_active, bond_dev->dev_addr);
 
-		read_lock(&bond->lock);
 		alb_send_learning_packets(curr_active,
 					  bond_dev->dev_addr, false);
 		if (bond->alb_info.rlb_enabled) {
 			/* inform clients mac address has changed */
 			rlb_req_update_slave_clients(bond, curr_active);
 		}
-		read_unlock(&bond->lock);
 	}
 
 	return 0;
diff --git a/drivers/net/bonding/bond_alb.h b/drivers/net/bonding/bond_alb.h
index aaeac61d03cf..1ad473b4ade5 100644
--- a/drivers/net/bonding/bond_alb.h
+++ b/drivers/net/bonding/bond_alb.h
@@ -139,24 +139,14 @@ struct tlb_slave_info {
 			 */
 };
 
-struct tlb_up_slave {
-	unsigned int	count;
-	struct rcu_head rcu;
-	struct slave	*arr[0];
-};
-
 struct alb_bond_info {
 	struct tlb_client_info	*tx_hashtbl; /* Dynamically allocated */
-	spinlock_t		tx_hashtbl_lock;
 	u32			unbalanced_load;
 	int			tx_rebalance_counter;
 	int			lp_counter;
-	/* -------- non-dynamic tlb mode only ---------*/
-	struct tlb_up_slave __rcu *slave_arr;	  /* Up slaves */
 	/* -------- rlb parameters -------- */
 	int rlb_enabled;
 	struct rlb_client_info	*rx_hashtbl;	/* Receive hash table */
-	spinlock_t		rx_hashtbl_lock;
 	u32			rx_hashtbl_used_head;
 	u8			rx_ntt;	/* flag - need to transmit
 					 * to all rx clients
diff --git a/drivers/net/bonding/bond_debugfs.c b/drivers/net/bonding/bond_debugfs.c
index 280971b227ea..8f99082f90eb 100644
--- a/drivers/net/bonding/bond_debugfs.c
+++ b/drivers/net/bonding/bond_debugfs.c
@@ -13,9 +13,7 @@
 
 static struct dentry *bonding_debug_root;
 
-/*
- *  Show RLB hash table
- */
+/* Show RLB hash table */
 static int bond_debug_rlb_hash_show(struct seq_file *m, void *v)
 {
 	struct bonding *bond = m->private;
@@ -29,7 +27,7 @@ static int bond_debug_rlb_hash_show(struct seq_file *m, void *v)
 	seq_printf(m, "SourceIP        DestinationIP   "
 			"Destination MAC   DEV\n");
 
-	spin_lock_bh(&(BOND_ALB_INFO(bond).rx_hashtbl_lock));
+	spin_lock_bh(&bond->mode_lock);
 
 	hash_index = bond_info->rx_hashtbl_used_head;
 	for (; hash_index != RLB_NULL_INDEX;
@@ -42,7 +40,7 @@ static int bond_debug_rlb_hash_show(struct seq_file *m, void *v)
 			client_info->slave->dev->name);
 	}
 
-	spin_unlock_bh(&(BOND_ALB_INFO(bond).rx_hashtbl_lock));
+	spin_unlock_bh(&bond->mode_lock);
 
 	return 0;
 }
diff --git a/drivers/net/bonding/bond_main.c b/drivers/net/bonding/bond_main.c
index f0f5eab0fab1..c9ac06cfe6b7 100644
--- a/drivers/net/bonding/bond_main.c
+++ b/drivers/net/bonding/bond_main.c
@@ -175,7 +175,7 @@ MODULE_PARM_DESC(fail_over_mac, "For active-backup, do not set all slaves to "
 				"the same MAC; 0 for none (default), "
 				"1 for active, 2 for follow");
 module_param(all_slaves_active, int, 0);
-MODULE_PARM_DESC(all_slaves_active, "Keep all frames received on an interface"
+MODULE_PARM_DESC(all_slaves_active, "Keep all frames received on an interface "
 				     "by setting active flag for all slaves; "
 				     "0 for never (default), 1 for always.");
 module_param(resend_igmp, int, 0);
@@ -208,6 +208,9 @@ static int lacp_fast;
 
 static int bond_init(struct net_device *bond_dev);
 static void bond_uninit(struct net_device *bond_dev);
+static struct rtnl_link_stats64 *bond_get_stats(struct net_device *bond_dev,
+						struct rtnl_link_stats64 *stats);
+static void bond_slave_arr_handler(struct work_struct *work);
 
 /*---------------------------- General routines -----------------------------*/
 
@@ -253,8 +256,7 @@ void bond_dev_queue_xmit(struct bonding *bond, struct sk_buff *skb,
 		dev_queue_xmit(skb);
 }
 
-/*
- * In the following 2 functions, bond_vlan_rx_add_vid and bond_vlan_rx_kill_vid,
+/* In the following 2 functions, bond_vlan_rx_add_vid and bond_vlan_rx_kill_vid,
  * We don't protect the slave list iteration with a lock because:
  * a. This operation is performed in IOCTL context,
  * b. The operation is protected by the RTNL semaphore in the 8021q code,
@@ -326,8 +328,7 @@ static int bond_vlan_rx_kill_vid(struct net_device *bond_dev,
 
 /*------------------------------- Link status -------------------------------*/
 
-/*
- * Set the carrier state for the master according to the state of its
+/* Set the carrier state for the master according to the state of its
  * slaves.  If any slaves are up, the master is up.  In 802.3ad mode,
  * do special 802.3ad magic.
  *
@@ -362,8 +363,7 @@ down:
 	return 0;
 }
 
-/*
- * Get link speed and duplex from the slave's base driver
+/* Get link speed and duplex from the slave's base driver
  * using ethtool. If for some reason the call fails or the
  * values are invalid, set speed and duplex to -1,
  * and return.
@@ -416,8 +416,7 @@ const char *bond_slave_link_status(s8 link)
 	}
 }
 
-/*
- * if <dev> supports MII link status reporting, check its link status.
+/* if <dev> supports MII link status reporting, check its link status.
  *
  * We either do MII/ETHTOOL ioctls, or check netif_carrier_ok(),
  * depending upon the setting of the use_carrier parameter.
@@ -454,14 +453,14 @@ static int bond_check_dev_link(struct bonding *bond,
 	/* Ethtool can't be used, fallback to MII ioctls. */
 	ioctl = slave_ops->ndo_do_ioctl;
 	if (ioctl) {
-		/* TODO: set pointer to correct ioctl on a per team member */
-		/*       bases to make this more efficient. that is, once  */
-		/*       we determine the correct ioctl, we will always    */
-		/*       call it and not the others for that team          */
-		/*       member.                                           */
-
-		/*
-		 * We cannot assume that SIOCGMIIPHY will also read a
+		/* TODO: set pointer to correct ioctl on a per team member
+		 *       bases to make this more efficient. that is, once
+		 *       we determine the correct ioctl, we will always
+		 *       call it and not the others for that team
+		 *       member.
+		 */
+
+		/* We cannot assume that SIOCGMIIPHY will also read a
 		 * register; not all network drivers (e.g., e100)
 		 * support that.
 		 */
@@ -476,8 +475,7 @@ static int bond_check_dev_link(struct bonding *bond,
 		}
 	}
 
-	/*
-	 * If reporting, report that either there's no dev->do_ioctl,
+	/* If reporting, report that either there's no dev->do_ioctl,
 	 * or both SIOCGMIIREG and get_link failed (meaning that we
 	 * cannot report link status).  If not reporting, pretend
 	 * we're ok.
@@ -487,9 +485,7 @@ static int bond_check_dev_link(struct bonding *bond,
 
 /*----------------------------- Multicast list ------------------------------*/
 
-/*
- * Push the promiscuity flag down to appropriate slaves
- */
+/* Push the promiscuity flag down to appropriate slaves */
 static int bond_set_promiscuity(struct bonding *bond, int inc)
 {
 	struct list_head *iter;
@@ -512,9 +508,7 @@ static int bond_set_promiscuity(struct bonding *bond, int inc)
 	return err;
 }
 
-/*
- * Push the allmulti flag down to all slaves
- */
+/* Push the allmulti flag down to all slaves */
 static int bond_set_allmulti(struct bonding *bond, int inc)
 {
 	struct list_head *iter;
@@ -537,8 +531,7 @@ static int bond_set_allmulti(struct bonding *bond, int inc)
 	return err;
 }
 
-/*
- * Retrieve the list of registered multicast addresses for the bonding
+/* Retrieve the list of registered multicast addresses for the bonding
  * device and retransmit an IGMP JOIN request to the current active
  * slave.
  */
@@ -560,8 +553,7 @@ static void bond_resend_igmp_join_requests_delayed(struct work_struct *work)
 	rtnl_unlock();
 }
 
-/* Flush bond's hardware addresses from slave
- */
+/* Flush bond's hardware addresses from slave */
 static void bond_hw_addr_flush(struct net_device *bond_dev,
 			       struct net_device *slave_dev)
 {
@@ -588,8 +580,6 @@ static void bond_hw_addr_flush(struct net_device *bond_dev,
 static void bond_hw_addr_swap(struct bonding *bond, struct slave *new_active,
 			      struct slave *old_active)
 {
-	ASSERT_RTNL();
-
 	if (old_active) {
 		if (bond->dev->flags & IFF_PROMISC)
 			dev_set_promiscuity(old_active->dev, -1);
@@ -632,18 +622,15 @@ static void bond_set_dev_addr(struct net_device *bond_dev,
 	call_netdevice_notifiers(NETDEV_CHANGEADDR, bond_dev);
 }
 
-/*
- * bond_do_fail_over_mac
+/* bond_do_fail_over_mac
  *
  * Perform special MAC address swapping for fail_over_mac settings
  *
- * Called with RTNL, curr_slave_lock for write_bh.
+ * Called with RTNL
  */
 static void bond_do_fail_over_mac(struct bonding *bond,
 				  struct slave *new_active,
 				  struct slave *old_active)
-	__releases(&bond->curr_slave_lock)
-	__acquires(&bond->curr_slave_lock)
 {
 	u8 tmp_mac[ETH_ALEN];
 	struct sockaddr saddr;
@@ -651,23 +638,17 @@ static void bond_do_fail_over_mac(struct bonding *bond,
 
 	switch (bond->params.fail_over_mac) {
 	case BOND_FOM_ACTIVE:
-		if (new_active) {
-			write_unlock_bh(&bond->curr_slave_lock);
+		if (new_active)
 			bond_set_dev_addr(bond->dev, new_active->dev);
-			write_lock_bh(&bond->curr_slave_lock);
-		}
 		break;
 	case BOND_FOM_FOLLOW:
-		/*
-		 * if new_active && old_active, swap them
+		/* if new_active && old_active, swap them
 		 * if just old_active, do nothing (going to no active slave)
 		 * if just new_active, set new_active to bond's MAC
 		 */
 		if (!new_active)
 			return;
 
-		write_unlock_bh(&bond->curr_slave_lock);
-
 		if (old_active) {
 			ether_addr_copy(tmp_mac, new_active->dev->dev_addr);
 			ether_addr_copy(saddr.sa_data,
@@ -696,7 +677,6 @@ static void bond_do_fail_over_mac(struct bonding *bond,
 			netdev_err(bond->dev, "Error %d setting MAC of slave %s\n",
 				   -rv, new_active->dev->name);
 out:
-		write_lock_bh(&bond->curr_slave_lock);
 		break;
 	default:
 		netdev_err(bond->dev, "bond_do_fail_over_mac impossible: bad policy %d\n",
@@ -708,8 +688,8 @@ out:
 
 static bool bond_should_change_active(struct bonding *bond)
 {
-	struct slave *prim = bond->primary_slave;
-	struct slave *curr = bond_deref_active_protected(bond);
+	struct slave *prim = rtnl_dereference(bond->primary_slave);
+	struct slave *curr = rtnl_dereference(bond->curr_active_slave);
 
 	if (!prim || !curr || curr->link != BOND_LINK_UP)
 		return true;
@@ -732,13 +712,14 @@ static bool bond_should_change_active(struct bonding *bond)
  */
 static struct slave *bond_find_best_slave(struct bonding *bond)
 {
-	struct slave *slave, *bestslave = NULL;
+	struct slave *slave, *bestslave = NULL, *primary;
 	struct list_head *iter;
 	int mintime = bond->params.updelay;
 
-	if (bond->primary_slave && bond->primary_slave->link == BOND_LINK_UP &&
+	primary = rtnl_dereference(bond->primary_slave);
+	if (primary && primary->link == BOND_LINK_UP &&
 	    bond_should_change_active(bond))
-		return bond->primary_slave;
+		return primary;
 
 	bond_for_each_slave(bond, slave, iter) {
 		if (slave->link == BOND_LINK_UP)
@@ -784,15 +765,15 @@ static bool bond_should_notify_peers(struct bonding *bond)
  * because it is apparently the best available slave we have, even though its
  * updelay hasn't timed out yet.
  *
- * If new_active is not NULL, caller must hold curr_slave_lock for write_bh.
+ * Caller must hold RTNL.
  */
 void bond_change_active_slave(struct bonding *bond, struct slave *new_active)
 {
 	struct slave *old_active;
 
-	old_active = rcu_dereference_protected(bond->curr_active_slave,
-					       !new_active ||
-					       lockdep_is_held(&bond->curr_slave_lock));
+	ASSERT_RTNL();
+
+	old_active = rtnl_dereference(bond->curr_active_slave);
 
 	if (old_active == new_active)
 		return;
@@ -860,21 +841,18 @@ void bond_change_active_slave(struct bonding *bond, struct slave *new_active)
 					bond_should_notify_peers(bond);
 			}
 
-			write_unlock_bh(&bond->curr_slave_lock);
-
 			call_netdevice_notifiers(NETDEV_BONDING_FAILOVER, bond->dev);
 			if (should_notify_peers)
 				call_netdevice_notifiers(NETDEV_NOTIFY_PEERS,
 							 bond->dev);
-
-			write_lock_bh(&bond->curr_slave_lock);
 		}
 	}
 
 	/* resend IGMP joins since active slave has changed or
 	 * all were sent on curr_active_slave.
 	 * resend only if bond is brought up with the affected
-	 * bonding modes and the retransmission is enabled */
+	 * bonding modes and the retransmission is enabled
+	 */
 	if (netif_running(bond->dev) && (bond->params.resend_igmp > 0) &&
 	    ((bond_uses_primary(bond) && new_active) ||
 	     BOND_MODE(bond) == BOND_MODE_ROUNDROBIN)) {
@@ -892,15 +870,17 @@ void bond_change_active_slave(struct bonding *bond, struct slave *new_active)
  * - The primary_slave has got its link back.
  * - A slave has got its link back and there's no old curr_active_slave.
  *
- * Caller must hold curr_slave_lock for write_bh.
+ * Caller must hold RTNL.
  */
 void bond_select_active_slave(struct bonding *bond)
 {
 	struct slave *best_slave;
 	int rv;
 
+	ASSERT_RTNL();
+
 	best_slave = bond_find_best_slave(bond);
-	if (best_slave != bond_deref_active_protected(bond)) {
+	if (best_slave != rtnl_dereference(bond->curr_active_slave)) {
 		bond_change_active_slave(bond, best_slave);
 		rv = bond_set_carrier(bond);
 		if (!rv)
@@ -1022,7 +1002,8 @@ static netdev_features_t bond_fix_features(struct net_device *dev,
 
 static void bond_compute_features(struct bonding *bond)
 {
-	unsigned int flags, dst_release_flag = IFF_XMIT_DST_RELEASE;
+	unsigned int dst_release_flag = IFF_XMIT_DST_RELEASE |
+					IFF_XMIT_DST_RELEASE_PERM;
 	netdev_features_t vlan_features = BOND_VLAN_FEATURES;
 	netdev_features_t enc_features  = BOND_ENC_FEATURES;
 	struct net_device *bond_dev = bond->dev;
@@ -1058,8 +1039,10 @@ done:
 	bond_dev->gso_max_segs = gso_max_segs;
 	netif_set_gso_max_size(bond_dev, gso_max_size);
 
-	flags = bond_dev->priv_flags & ~IFF_XMIT_DST_RELEASE;
-	bond_dev->priv_flags = flags | dst_release_flag;
+	bond_dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
+	if ((bond_dev->priv_flags & IFF_XMIT_DST_RELEASE_PERM) &&
+	    dst_release_flag == (IFF_XMIT_DST_RELEASE | IFF_XMIT_DST_RELEASE_PERM))
+		bond_dev->priv_flags |= IFF_XMIT_DST_RELEASE;
 
 	netdev_change_features(bond_dev);
 }
@@ -1240,8 +1223,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 			   slave_dev->name);
 	}
 
-	/*
-	 * Old ifenslave binaries are no longer supported.  These can
+	/* Old ifenslave binaries are no longer supported.  These can
 	 * be identified with moderate accuracy by the state of the slave:
 	 * the current ifenslave will set the interface down prior to
 	 * enslaving it; the old ifenslave will not.
@@ -1313,7 +1295,8 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 	call_netdevice_notifiers(NETDEV_JOIN, slave_dev);
 
 	/* If this is the first slave, then we need to set the master's hardware
-	 * address to be the same as the slave's. */
+	 * address to be the same as the slave's.
+	 */
 	if (!bond_has_slaves(bond) &&
 	    bond->dev->addr_assign_type == NET_ADDR_RANDOM)
 		bond_set_dev_addr(bond->dev, slave_dev);
@@ -1326,8 +1309,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 
 	new_slave->bond = bond;
 	new_slave->dev = slave_dev;
-	/*
-	 * Set the new_slave's queue_id to be zero.  Queue ID mapping
+	/* Set the new_slave's queue_id to be zero.  Queue ID mapping
 	 * is set via sysfs or module option if desired.
 	 */
 	new_slave->queue_id = 0;
@@ -1340,8 +1322,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 		goto err_free;
 	}
 
-	/*
-	 * Save slave's original ("permanent") mac address for modes
+	/* Save slave's original ("permanent") mac address for modes
 	 * that need it, and for restoring it upon release, and then
 	 * set it to the master's address
 	 */
@@ -1349,8 +1330,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 
 	if (!bond->params.fail_over_mac ||
 	    BOND_MODE(bond) != BOND_MODE_ACTIVEBACKUP) {
-		/*
-		 * Set slave to master's mac address.  The application already
+		/* Set slave to master's mac address.  The application already
 		 * set the master's mac address to that of the first slave
 		 */
 		memcpy(addr.sa_data, bond_dev->dev_addr, bond_dev->addr_len);
@@ -1370,6 +1350,8 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 	}
 
 	slave_dev->priv_flags |= IFF_BONDING;
+	/* initialize slave stats */
+	dev_get_stats(new_slave->dev, &new_slave->slave_stats);
 
 	if (bond_is_lb(bond)) {
 		/* bond_alb_init_slave() must be called before all other stages since
@@ -1436,8 +1418,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 		link_reporting = bond_check_dev_link(bond, slave_dev, 1);
 
 		if ((link_reporting == -1) && !bond->params.arp_interval) {
-			/*
-			 * miimon is set but a bonded network driver
+			/* miimon is set but a bonded network driver
 			 * does not support ETHTOOL/MII and
 			 * arp_interval is not set.  Note: if
 			 * use_carrier is enabled, we will never go
@@ -1482,7 +1463,7 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 	if (bond_uses_primary(bond) && bond->params.primary[0]) {
 		/* if there is a primary slave, remember it */
 		if (strcmp(bond->params.primary, new_slave->dev->name) == 0) {
-			bond->primary_slave = new_slave;
+			rcu_assign_pointer(bond->primary_slave, new_slave);
 			bond->force_primary = true;
 		}
 	}
@@ -1570,12 +1551,13 @@ int bond_enslave(struct net_device *bond_dev, struct net_device *slave_dev)
 
 	if (bond_uses_primary(bond)) {
 		block_netpoll_tx();
-		write_lock_bh(&bond->curr_slave_lock);
 		bond_select_active_slave(bond);
-		write_unlock_bh(&bond->curr_slave_lock);
 		unblock_netpoll_tx();
 	}
 
+	if (bond_mode_uses_xmit_hash(bond))
+		bond_update_slave_arr(bond, NULL);
+
 	netdev_info(bond_dev, "Enslaving %s as %s interface with %s link\n",
 		    slave_dev->name,
 		    bond_is_active_slave(new_slave) ? "an active" : "a backup",
@@ -1596,16 +1578,16 @@ err_detach:
 		bond_hw_addr_flush(bond_dev, slave_dev);
 
 	vlan_vids_del_by_dev(slave_dev, bond_dev);
-	if (bond->primary_slave == new_slave)
-		bond->primary_slave = NULL;
+	if (rcu_access_pointer(bond->primary_slave) == new_slave)
+		RCU_INIT_POINTER(bond->primary_slave, NULL);
 	if (rcu_access_pointer(bond->curr_active_slave) == new_slave) {
 		block_netpoll_tx();
-		write_lock_bh(&bond->curr_slave_lock);
 		bond_change_active_slave(bond, NULL);
 		bond_select_active_slave(bond);
-		write_unlock_bh(&bond->curr_slave_lock);
 		unblock_netpoll_tx();
 	}
+	/* either primary_slave or curr_active_slave might've changed */
+	synchronize_rcu();
 	slave_disable_netpoll(new_slave);
 
 err_close:
@@ -1639,10 +1621,9 @@ err_undo_flags:
 	return res;
 }
 
-/*
- * Try to release the slave device <slave> from the bond device <master>
+/* Try to release the slave device <slave> from the bond device <master>
  * It is legal to access curr_active_slave without a lock because all the function
- * is write-locked. If "all" is true it means that the function is being called
+ * is RTNL-locked. If "all" is true it means that the function is being called
  * while destroying a bond interface and all slaves are being released.
  *
  * The rules for slave state should be:
@@ -1682,18 +1663,20 @@ static int __bond_release_one(struct net_device *bond_dev,
 
 	bond_sysfs_slave_del(slave);
 
+	/* recompute stats just before removing the slave */
+	bond_get_stats(bond->dev, &bond->bond_stats);
+
 	bond_upper_dev_unlink(bond_dev, slave_dev);
 	/* unregister rx_handler early so bond_handle_frame wouldn't be called
 	 * for this slave anymore.
 	 */
 	netdev_rx_handler_unregister(slave_dev);
-	write_lock_bh(&bond->lock);
 
-	/* Inform AD package of unbinding of slave. */
 	if (BOND_MODE(bond) == BOND_MODE_8023AD)
 		bond_3ad_unbind_slave(slave);
 
-	write_unlock_bh(&bond->lock);
+	if (bond_mode_uses_xmit_hash(bond))
+		bond_update_slave_arr(bond, slave);
 
 	netdev_info(bond_dev, "Releasing %s interface %s\n",
 		    bond_is_active_slave(slave) ? "active" : "backup",
@@ -1712,14 +1695,11 @@ static int __bond_release_one(struct net_device *bond_dev,
 				    bond_dev->name, slave_dev->name);
 	}
 
-	if (bond->primary_slave == slave)
-		bond->primary_slave = NULL;
+	if (rtnl_dereference(bond->primary_slave) == slave)
+		RCU_INIT_POINTER(bond->primary_slave, NULL);
 
-	if (oldcurrent == slave) {
-		write_lock_bh(&bond->curr_slave_lock);
+	if (oldcurrent == slave)
 		bond_change_active_slave(bond, NULL);
-		write_unlock_bh(&bond->curr_slave_lock);
-	}
 
 	if (bond_is_lb(bond)) {
 		/* Must be called only after the slave has been
@@ -1733,16 +1713,11 @@ static int __bond_release_one(struct net_device *bond_dev,
 	if (all) {
 		RCU_INIT_POINTER(bond->curr_active_slave, NULL);
 	} else if (oldcurrent == slave) {
-		/*
-		 * Note that we hold RTNL over this sequence, so there
+		/* Note that we hold RTNL over this sequence, so there
 		 * is no concern that another slave add/remove event
 		 * will interfere.
 		 */
-		write_lock_bh(&bond->curr_slave_lock);
-
 		bond_select_active_slave(bond);
-
-		write_unlock_bh(&bond->curr_slave_lock);
 	}
 
 	if (!bond_has_slaves(bond)) {
@@ -1765,10 +1740,9 @@ static int __bond_release_one(struct net_device *bond_dev,
 		netdev_info(bond_dev, "last VLAN challenged slave %s left bond %s - VLAN blocking is removed\n",
 			    slave_dev->name, bond_dev->name);
 
-	/* must do this from outside any spinlocks */
 	vlan_vids_del_by_dev(slave_dev, bond_dev);
 
-	/* If the mode uses primary, then this cases was handled above by
+	/* If the mode uses primary, then this case was handled above by
 	 * bond_change_active_slave(..., NULL)
 	 */
 	if (!bond_uses_primary(bond)) {
@@ -1808,7 +1782,7 @@ static int __bond_release_one(struct net_device *bond_dev,
 
 	bond_free_slave(slave);
 
-	return 0;  /* deletion OK */
+	return 0;
 }
 
 /* A wrapper used because of ndo_del_link */
@@ -1817,10 +1791,9 @@ int bond_release(struct net_device *bond_dev, struct net_device *slave_dev)
 	return __bond_release_one(bond_dev, slave_dev, false);
 }
 
-/*
-* First release a slave and then destroy the bond if no more slaves are left.
-* Must be under rtnl_lock when this function is called.
-*/
+/* First release a slave and then destroy the bond if no more slaves are left.
+ * Must be under rtnl_lock when this function is called.
+ */
 static int  bond_release_and_destroy(struct net_device *bond_dev,
 				     struct net_device *slave_dev)
 {
@@ -1843,7 +1816,6 @@ static int bond_info_query(struct net_device *bond_dev, struct ifbond *info)
 
 	info->bond_mode = BOND_MODE(bond);
 	info->miimon = bond->params.miimon;
-
 	info->num_slaves = bond->slave_cnt;
 
 	return 0;
@@ -1906,9 +1878,7 @@ static int bond_miimon_inspect(struct bonding *bond)
 			/*FALLTHRU*/
 		case BOND_LINK_FAIL:
 			if (link_state) {
-				/*
-				 * recovered before downdelay expired
-				 */
+				/* recovered before downdelay expired */
 				slave->link = BOND_LINK_UP;
 				slave->last_link_up = jiffies;
 				netdev_info(bond->dev, "link status up again after %d ms for interface %s\n",
@@ -1974,7 +1944,7 @@ static int bond_miimon_inspect(struct bonding *bond)
 static void bond_miimon_commit(struct bonding *bond)
 {
 	struct list_head *iter;
-	struct slave *slave;
+	struct slave *slave, *primary;
 
 	bond_for_each_slave(bond, slave, iter) {
 		switch (slave->new_link) {
@@ -1985,13 +1955,14 @@ static void bond_miimon_commit(struct bonding *bond)
 			slave->link = BOND_LINK_UP;
 			slave->last_link_up = jiffies;
 
+			primary = rtnl_dereference(bond->primary_slave);
 			if (BOND_MODE(bond) == BOND_MODE_8023AD) {
 				/* prevent it from being the active one */
 				bond_set_backup_slave(slave);
 			} else if (BOND_MODE(bond) != BOND_MODE_ACTIVEBACKUP) {
 				/* make it immediately active */
 				bond_set_active_slave(slave);
-			} else if (slave != bond->primary_slave) {
+			} else if (slave != primary) {
 				/* prevent it from being the active one */
 				bond_set_backup_slave(slave);
 			}
@@ -2009,8 +1980,10 @@ static void bond_miimon_commit(struct bonding *bond)
 				bond_alb_handle_link_change(bond, slave,
 							    BOND_LINK_UP);
 
-			if (!bond->curr_active_slave ||
-			    (slave == bond->primary_slave))
+			if (BOND_MODE(bond) == BOND_MODE_XOR)
+				bond_update_slave_arr(bond, NULL);
+
+			if (!bond->curr_active_slave || slave == primary)
 				goto do_failover;
 
 			continue;
@@ -2037,6 +2010,9 @@ static void bond_miimon_commit(struct bonding *bond)
 				bond_alb_handle_link_change(bond, slave,
 							    BOND_LINK_DOWN);
 
+			if (BOND_MODE(bond) == BOND_MODE_XOR)
+				bond_update_slave_arr(bond, NULL);
+
 			if (slave == rcu_access_pointer(bond->curr_active_slave))
 				goto do_failover;
 
@@ -2051,19 +2027,15 @@ static void bond_miimon_commit(struct bonding *bond)
 		}
 
 do_failover:
-		ASSERT_RTNL();
 		block_netpoll_tx();
-		write_lock_bh(&bond->curr_slave_lock);
 		bond_select_active_slave(bond);
-		write_unlock_bh(&bond->curr_slave_lock);
 		unblock_netpoll_tx();
 	}
 
 	bond_set_carrier(bond);
 }
 
-/*
- * bond_mii_monitor
+/* bond_mii_monitor
  *
  * Really a wrapper that splits the mii monitor into two phases: an
  * inspection, then (if inspection indicates something needs to be done)
@@ -2135,8 +2107,7 @@ static bool bond_has_this_ip(struct bonding *bond, __be32 ip)
 	return ret;
 }
 
-/*
- * We go to the (large) trouble of VLAN tagging ARP frames because
+/* We go to the (large) trouble of VLAN tagging ARP frames because
  * switches in VLAN mode (especially if ports are configured as
  * "native" to a VLAN) might not pass non-tagged frames.
  */
@@ -2363,8 +2334,7 @@ int bond_arp_rcv(const struct sk_buff *skb, struct bonding *bond,
 
 	curr_active_slave = rcu_dereference(bond->curr_active_slave);
 
-	/*
-	 * Backup slaves won't see the ARP reply, but do come through
+	/* Backup slaves won't see the ARP reply, but do come through
 	 * here for each ARP probe (so we swap the sip/tip to validate
 	 * the probe).  In a "redundant switch, common router" type of
 	 * configuration, the ARP probe will (hopefully) travel from
@@ -2404,8 +2374,7 @@ static bool bond_time_in_interval(struct bonding *bond, unsigned long last_act,
 			     last_act + mod * delta_in_ticks + delta_in_ticks/2);
 }
 
-/*
- * this function is called regularly to monitor each slave's link
+/* This function is called regularly to monitor each slave's link
  * ensuring that traffic is being sent and received when arp monitoring
  * is used in load-balancing mode. if the adapter has been dormant, then an
  * arp is transmitted to generate traffic. see activebackup_arp_monitor for
@@ -2500,16 +2469,11 @@ static void bond_loadbalance_arp_mon(struct work_struct *work)
 
 		if (slave_state_changed) {
 			bond_slave_state_change(bond);
+			if (BOND_MODE(bond) == BOND_MODE_XOR)
+				bond_update_slave_arr(bond, NULL);
 		} else if (do_failover) {
-			/* the bond_select_active_slave must hold RTNL
-			 * and curr_slave_lock for write.
-			 */
 			block_netpoll_tx();
-			write_lock_bh(&bond->curr_slave_lock);
-
 			bond_select_active_slave(bond);
-
-			write_unlock_bh(&bond->curr_slave_lock);
 			unblock_netpoll_tx();
 		}
 		rtnl_unlock();
@@ -2521,13 +2485,12 @@ re_arm:
 				   msecs_to_jiffies(bond->params.arp_interval));
 }
 
-/*
- * Called to inspect slaves for active-backup mode ARP monitor link state
+/* Called to inspect slaves for active-backup mode ARP monitor link state
  * changes.  Sets new_link in slaves to specify what action should take
  * place for the slave.  Returns 0 if no changes are found, >0 if changes
  * to link states must be committed.
  *
- * Called with rcu_read_lock hold.
+ * Called with rcu_read_lock held.
  */
 static int bond_ab_arp_inspect(struct bonding *bond)
 {
@@ -2548,16 +2511,14 @@ static int bond_ab_arp_inspect(struct bonding *bond)
 			continue;
 		}
 
-		/*
-		 * Give slaves 2*delta after being enslaved or made
+		/* Give slaves 2*delta after being enslaved or made
 		 * active.  This avoids bouncing, as the last receive
 		 * times need a full ARP monitor cycle to be updated.
 		 */
 		if (bond_time_in_interval(bond, slave->last_link_up, 2))
 			continue;
 
-		/*
-		 * Backup slave is down if:
+		/* Backup slave is down if:
 		 * - No current_arp_slave AND
 		 * - more than 3*delta since last receive AND
 		 * - the bond has an IP address
@@ -2576,8 +2537,7 @@ static int bond_ab_arp_inspect(struct bonding *bond)
 			commit++;
 		}
 
-		/*
-		 * Active slave is down if:
+		/* Active slave is down if:
 		 * - more than 2*delta since transmitting OR
 		 * - (more than 2*delta since receive AND
 		 *    the bond has an IP address)
@@ -2594,8 +2554,7 @@ static int bond_ab_arp_inspect(struct bonding *bond)
 	return commit;
 }
 
-/*
- * Called to commit link state changes noted by inspection step of
+/* Called to commit link state changes noted by inspection step of
  * active-backup mode ARP monitor.
  *
  * Called with RTNL hold.
@@ -2631,7 +2590,7 @@ static void bond_ab_arp_commit(struct bonding *bond)
 					    slave->dev->name);
 
 				if (!rtnl_dereference(bond->curr_active_slave) ||
-				    (slave == bond->primary_slave))
+				    slave == rtnl_dereference(bond->primary_slave))
 					goto do_failover;
 
 			}
@@ -2663,21 +2622,17 @@ static void bond_ab_arp_commit(struct bonding *bond)
 		}
 
 do_failover:
-		ASSERT_RTNL();
 		block_netpoll_tx();
-		write_lock_bh(&bond->curr_slave_lock);
 		bond_select_active_slave(bond);
-		write_unlock_bh(&bond->curr_slave_lock);
 		unblock_netpoll_tx();
 	}
 
 	bond_set_carrier(bond);
 }
 
-/*
- * Send ARP probes for active-backup mode ARP monitor.
+/* Send ARP probes for active-backup mode ARP monitor.
  *
- * Called with rcu_read_lock hold.
+ * Called with rcu_read_lock held.
  */
 static bool bond_ab_arp_probe(struct bonding *bond)
 {
@@ -2817,9 +2772,7 @@ re_arm:
 
 /*-------------------------- netdev event handling --------------------------*/
 
-/*
- * Change device name
- */
+/* Change device name */
 static int bond_event_changename(struct bonding *bond)
 {
 	bond_remove_proc_entry(bond);
@@ -2858,7 +2811,7 @@ static int bond_master_netdev_event(unsigned long event,
 static int bond_slave_netdev_event(unsigned long event,
 				   struct net_device *slave_dev)
 {
-	struct slave *slave = bond_slave_get_rtnl(slave_dev);
+	struct slave *slave = bond_slave_get_rtnl(slave_dev), *primary;
 	struct bonding *bond;
 	struct net_device *bond_dev;
 	u32 old_speed;
@@ -2872,6 +2825,7 @@ static int bond_slave_netdev_event(unsigned long event,
 		return NOTIFY_DONE;
 	bond_dev = slave->bond->dev;
 	bond = slave->bond;
+	primary = rtnl_dereference(bond->primary_slave);
 
 	switch (event) {
 	case NETDEV_UNREGISTER:
@@ -2893,15 +2847,23 @@ static int bond_slave_netdev_event(unsigned long event,
 			if (old_duplex != slave->duplex)
 				bond_3ad_adapter_duplex_changed(slave);
 		}
+		/* Refresh slave-array if applicable!
+		 * If the setup does not use miimon or arpmon (mode-specific!),
+		 * then these events will not cause the slave-array to be
+		 * refreshed. This will cause xmit to use a slave that is not
+		 * usable. Avoid such situation by refeshing the array at these
+		 * events. If these (miimon/arpmon) parameters are configured
+		 * then array gets refreshed twice and that should be fine!
+		 */
+		if (bond_mode_uses_xmit_hash(bond))
+			bond_update_slave_arr(bond, NULL);
 		break;
 	case NETDEV_DOWN:
-		/*
-		 * ... Or is it this?
-		 */
+		if (bond_mode_uses_xmit_hash(bond))
+			bond_update_slave_arr(bond, NULL);
 		break;
 	case NETDEV_CHANGEMTU:
-		/*
-		 * TODO: Should slaves be allowed to
+		/* TODO: Should slaves be allowed to
 		 * independently alter their MTU?  For
 		 * an active-backup bond, slaves need
 		 * not be the same type of device, so
@@ -2919,23 +2881,21 @@ static int bond_slave_netdev_event(unsigned long event,
 		    !bond->params.primary[0])
 			break;
 
-		if (slave == bond->primary_slave) {
+		if (slave == primary) {
 			/* slave's name changed - he's no longer primary */
-			bond->primary_slave = NULL;
+			RCU_INIT_POINTER(bond->primary_slave, NULL);
 		} else if (!strcmp(slave_dev->name, bond->params.primary)) {
 			/* we have a new primary slave */
-			bond->primary_slave = slave;
+			rcu_assign_pointer(bond->primary_slave, slave);
 		} else { /* we didn't change primary - exit */
 			break;
 		}
 
 		netdev_info(bond->dev, "Primary slave changed to %s, reselecting active slave\n",
-			    bond->primary_slave ? slave_dev->name : "none");
+			    primary ? slave_dev->name : "none");
 
 		block_netpoll_tx();
-		write_lock_bh(&bond->curr_slave_lock);
 		bond_select_active_slave(bond);
-		write_unlock_bh(&bond->curr_slave_lock);
 		unblock_netpoll_tx();
 		break;
 	case NETDEV_FEAT_CHANGE:
@@ -2952,8 +2912,7 @@ static int bond_slave_netdev_event(unsigned long event,
 	return NOTIFY_DONE;
 }
 
-/*
- * bond_netdev_event: handle netdev notifier chain events.
+/* bond_netdev_event: handle netdev notifier chain events.
  *
  * This function receives events for the netdev chain.  The caller (an
  * ioctl handler calling blocking_notifier_call_chain) holds the necessary
@@ -3081,6 +3040,7 @@ static void bond_work_init_all(struct bonding *bond)
 	else
 		INIT_DELAYED_WORK(&bond->arp_work, bond_loadbalance_arp_mon);
 	INIT_DELAYED_WORK(&bond->ad_work, bond_3ad_state_machine_handler);
+	INIT_DELAYED_WORK(&bond->slave_arr_work, bond_slave_arr_handler);
 }
 
 static void bond_work_cancel_all(struct bonding *bond)
@@ -3090,6 +3050,7 @@ static void bond_work_cancel_all(struct bonding *bond)
 	cancel_delayed_work_sync(&bond->alb_work);
 	cancel_delayed_work_sync(&bond->ad_work);
 	cancel_delayed_work_sync(&bond->mcast_work);
+	cancel_delayed_work_sync(&bond->slave_arr_work);
 }
 
 static int bond_open(struct net_device *bond_dev)
@@ -3099,9 +3060,7 @@ static int bond_open(struct net_device *bond_dev)
 	struct slave *slave;
 
 	/* reset slave->backup and slave->inactive */
-	read_lock(&bond->lock);
 	if (bond_has_slaves(bond)) {
-		read_lock(&bond->curr_slave_lock);
 		bond_for_each_slave(bond, slave, iter) {
 			if (bond_uses_primary(bond) &&
 			    slave != rcu_access_pointer(bond->curr_active_slave)) {
@@ -3112,9 +3071,7 @@ static int bond_open(struct net_device *bond_dev)
 							    BOND_SLAVE_NOTIFY_NOW);
 			}
 		}
-		read_unlock(&bond->curr_slave_lock);
 	}
-	read_unlock(&bond->lock);
 
 	bond_work_init_all(bond);
 
@@ -3143,6 +3100,9 @@ static int bond_open(struct net_device *bond_dev)
 		bond_3ad_initiate_agg_selection(bond, 1);
 	}
 
+	if (bond_mode_uses_xmit_hash(bond))
+		bond_update_slave_arr(bond, NULL);
+
 	return 0;
 }
 
@@ -3167,40 +3127,43 @@ static struct rtnl_link_stats64 *bond_get_stats(struct net_device *bond_dev,
 	struct list_head *iter;
 	struct slave *slave;
 
-	memset(stats, 0, sizeof(*stats));
+	memcpy(stats, &bond->bond_stats, sizeof(*stats));
 
-	read_lock_bh(&bond->lock);
 	bond_for_each_slave(bond, slave, iter) {
 		const struct rtnl_link_stats64 *sstats =
 			dev_get_stats(slave->dev, &temp);
+		struct rtnl_link_stats64 *pstats = &slave->slave_stats;
+
+		stats->rx_packets +=  sstats->rx_packets - pstats->rx_packets;
+		stats->rx_bytes += sstats->rx_bytes - pstats->rx_bytes;
+		stats->rx_errors += sstats->rx_errors - pstats->rx_errors;
+		stats->rx_dropped += sstats->rx_dropped - pstats->rx_dropped;
 
-		stats->rx_packets += sstats->rx_packets;
-		stats->rx_bytes += sstats->rx_bytes;
-		stats->rx_errors += sstats->rx_errors;
-		stats->rx_dropped += sstats->rx_dropped;
+		stats->tx_packets += sstats->tx_packets - pstats->tx_packets;;
+		stats->tx_bytes += sstats->tx_bytes - pstats->tx_bytes;
+		stats->tx_errors += sstats->tx_errors - pstats->tx_errors;
+		stats->tx_dropped += sstats->tx_dropped - pstats->tx_dropped;
 
-		stats->tx_packets += sstats->tx_packets;
-		stats->tx_bytes += sstats->tx_bytes;
-		stats->tx_errors += sstats->tx_errors;
-		stats->tx_dropped += sstats->tx_dropped;
+		stats->multicast += sstats->multicast - pstats->multicast;
+		stats->collisions += sstats->collisions - pstats->collisions;
 
-		stats->multicast += sstats->multicast;
-		stats->collisions += sstats->collisions;
+		stats->rx_length_errors += sstats->rx_length_errors - pstats->rx_length_errors;
+		stats->rx_over_errors += sstats->rx_over_errors - pstats->rx_over_errors;
+		stats->rx_crc_errors += sstats->rx_crc_errors - pstats->rx_crc_errors;
+		stats->rx_frame_errors += sstats->rx_frame_errors - pstats->rx_frame_errors;
+		stats->rx_fifo_errors += sstats->rx_fifo_errors - pstats->rx_fifo_errors;
+		stats->rx_missed_errors += sstats->rx_missed_errors - pstats->rx_missed_errors;
 
-		stats->rx_length_errors += sstats->rx_length_errors;
-		stats->rx_over_errors += sstats->rx_over_errors;
-		stats->rx_crc_errors += sstats->rx_crc_errors;
-		stats->rx_frame_errors += sstats->rx_frame_errors;
-		stats->rx_fifo_errors += sstats->rx_fifo_errors;
-		stats->rx_missed_errors += sstats->rx_missed_errors;
+		stats->tx_aborted_errors += sstats->tx_aborted_errors - pstats->tx_aborted_errors;
+		stats->tx_carrier_errors += sstats->tx_carrier_errors - pstats->tx_carrier_errors;
+		stats->tx_fifo_errors += sstats->tx_fifo_errors - pstats->tx_fifo_errors;
+		stats->tx_heartbeat_errors += sstats->tx_heartbeat_errors - pstats->tx_heartbeat_errors;
+		stats->tx_window_errors += sstats->tx_window_errors - pstats->tx_window_errors;
 
-		stats->tx_aborted_errors += sstats->tx_aborted_errors;
-		stats->tx_carrier_errors += sstats->tx_carrier_errors;
-		stats->tx_fifo_errors += sstats->tx_fifo_errors;
-		stats->tx_heartbeat_errors += sstats->tx_heartbeat_errors;
-		stats->tx_window_errors += sstats->tx_window_errors;
+		/* save off the slave stats for the next run */
+		memcpy(pstats, sstats, sizeof(*sstats));
 	}
-	read_unlock_bh(&bond->lock);
+	memcpy(&bond->bond_stats, stats, sizeof(*stats));
 
 	return stats;
 }
@@ -3229,24 +3192,17 @@ static int bond_do_ioctl(struct net_device *bond_dev, struct ifreq *ifr, int cmd
 		mii->phy_id = 0;
 		/* Fall Through */
 	case SIOCGMIIREG:
-		/*
-		 * We do this again just in case we were called by SIOCGMIIREG
+		/* We do this again just in case we were called by SIOCGMIIREG
 		 * instead of SIOCGMIIPHY.
 		 */
 		mii = if_mii(ifr);
 		if (!mii)
 			return -EINVAL;
 
-
 		if (mii->reg_num == 1) {
 			mii->val_out = 0;
-			read_lock(&bond->lock);
-			read_lock(&bond->curr_slave_lock);
 			if (netif_carrier_ok(bond->dev))
 				mii->val_out = BMSR_LSTATUS;
-
-			read_unlock(&bond->curr_slave_lock);
-			read_unlock(&bond->lock);
 		}
 
 		return 0;
@@ -3277,7 +3233,6 @@ static int bond_do_ioctl(struct net_device *bond_dev, struct ifreq *ifr, int cmd
 
 		return res;
 	default:
-		/* Go on */
 		break;
 	}
 
@@ -3339,7 +3294,6 @@ static void bond_set_rx_mode(struct net_device *bond_dev)
 	struct list_head *iter;
 	struct slave *slave;
 
-
 	rcu_read_lock();
 	if (bond_uses_primary(bond)) {
 		slave = rcu_dereference(bond->curr_active_slave);
@@ -3377,8 +3331,7 @@ static int bond_neigh_init(struct neighbour *n)
 	if (ret)
 		return ret;
 
-	/*
-	 * Assign slave's neigh_cleanup to neighbour in case cleanup is called
+	/* Assign slave's neigh_cleanup to neighbour in case cleanup is called
 	 * after the last slave has been detached.  Assumes that all slaves
 	 * utilize the same neigh_cleanup (true at this writing as only user
 	 * is ipoib).
@@ -3391,8 +3344,7 @@ static int bond_neigh_init(struct neighbour *n)
 	return parms.neigh_setup(n);
 }
 
-/*
- * The bonding ndo_neigh_setup is called at init time beofre any
+/* The bonding ndo_neigh_setup is called at init time beofre any
  * slave exists. So we must declare proxy setup function which will
  * be used at run time to resolve the actual slave neigh param setup.
  *
@@ -3410,9 +3362,7 @@ static int bond_neigh_setup(struct net_device *dev,
 	return 0;
 }
 
-/*
- * Change the MTU of all of a master's slaves to match the master
- */
+/* Change the MTU of all of a master's slaves to match the master */
 static int bond_change_mtu(struct net_device *bond_dev, int new_mtu)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
@@ -3422,21 +3372,6 @@ static int bond_change_mtu(struct net_device *bond_dev, int new_mtu)
 
 	netdev_dbg(bond_dev, "bond=%p, new_mtu=%d\n", bond, new_mtu);
 
-	/* Can't hold bond->lock with bh disabled here since
-	 * some base drivers panic. On the other hand we can't
-	 * hold bond->lock without bh disabled because we'll
-	 * deadlock. The only solution is to rely on the fact
-	 * that we're under rtnl_lock here, and the slaves
-	 * list won't change. This doesn't solve the problem
-	 * of setting the slave's MTU while it is
-	 * transmitting, but the assumption is that the base
-	 * driver can handle that.
-	 *
-	 * TODO: figure out a way to safely iterate the slaves
-	 * list, but without holding a lock around the actual
-	 * call to the base driver.
-	 */
-
 	bond_for_each_slave(bond, slave, iter) {
 		netdev_dbg(bond_dev, "s %p c_m %p\n",
 			   slave, slave->dev->netdev_ops->ndo_change_mtu);
@@ -3480,8 +3415,7 @@ unwind:
 	return res;
 }
 
-/*
- * Change HW address
+/* Change HW address
  *
  * Note that many devices must be down to change the HW address, and
  * downing the master releases all slaves.  We can make bonds full of
@@ -3511,21 +3445,6 @@ static int bond_set_mac_address(struct net_device *bond_dev, void *addr)
 	if (!is_valid_ether_addr(sa->sa_data))
 		return -EADDRNOTAVAIL;
 
-	/* Can't hold bond->lock with bh disabled here since
-	 * some base drivers panic. On the other hand we can't
-	 * hold bond->lock without bh disabled because we'll
-	 * deadlock. The only solution is to rely on the fact
-	 * that we're under rtnl_lock here, and the slaves
-	 * list won't change. This doesn't solve the problem
-	 * of setting the slave's hw address while it is
-	 * transmitting, but the assumption is that the base
-	 * driver can handle that.
-	 *
-	 * TODO: figure out a way to safely iterate the slaves
-	 * list, but without holding a lock around the actual
-	 * call to the base driver.
-	 */
-
 	bond_for_each_slave(bond, slave, iter) {
 		netdev_dbg(bond_dev, "slave %p %s\n", slave, slave->dev->name);
 		res = dev_set_mac_address(slave->dev, addr);
@@ -3654,20 +3573,25 @@ static int bond_xmit_roundrobin(struct sk_buff *skb, struct net_device *bond_dev
 	 */
 	if (iph->protocol == IPPROTO_IGMP && skb->protocol == htons(ETH_P_IP)) {
 		slave = rcu_dereference(bond->curr_active_slave);
-		if (slave && bond_slave_can_tx(slave))
+		if (slave)
 			bond_dev_queue_xmit(bond, skb, slave->dev);
 		else
 			bond_xmit_slave_id(bond, skb, 0);
 	} else {
-		slave_id = bond_rr_gen_slave_id(bond);
-		bond_xmit_slave_id(bond, skb, slave_id % bond->slave_cnt);
+		int slave_cnt = ACCESS_ONCE(bond->slave_cnt);
+
+		if (likely(slave_cnt)) {
+			slave_id = bond_rr_gen_slave_id(bond);
+			bond_xmit_slave_id(bond, skb, slave_id % slave_cnt);
+		} else {
+			dev_kfree_skb_any(skb);
+		}
 	}
 
 	return NETDEV_TX_OK;
 }
 
-/*
- * in active-backup mode, we know that bond->curr_active_slave is always valid if
+/* In active-backup mode, we know that bond->curr_active_slave is always valid if
  * the bond has a usable interface.
  */
 static int bond_xmit_activebackup(struct sk_buff *skb, struct net_device *bond_dev)
@@ -3684,15 +3608,148 @@ static int bond_xmit_activebackup(struct sk_buff *skb, struct net_device *bond_d
 	return NETDEV_TX_OK;
 }
 
-/* In bond_xmit_xor() , we determine the output device by using a pre-
- * determined xmit_hash_policy(), If the selected device is not enabled,
- * find the next active slave.
+/* Use this to update slave_array when (a) it's not appropriate to update
+ * slave_array right away (note that update_slave_array() may sleep)
+ * and / or (b) RTNL is not held.
  */
-static int bond_xmit_xor(struct sk_buff *skb, struct net_device *bond_dev)
+void bond_slave_arr_work_rearm(struct bonding *bond, unsigned long delay)
 {
-	struct bonding *bond = netdev_priv(bond_dev);
+	queue_delayed_work(bond->wq, &bond->slave_arr_work, delay);
+}
+
+/* Slave array work handler. Holds only RTNL */
+static void bond_slave_arr_handler(struct work_struct *work)
+{
+	struct bonding *bond = container_of(work, struct bonding,
+					    slave_arr_work.work);
+	int ret;
+
+	if (!rtnl_trylock())
+		goto err;
+
+	ret = bond_update_slave_arr(bond, NULL);
+	rtnl_unlock();
+	if (ret) {
+		pr_warn_ratelimited("Failed to update slave array from WT\n");
+		goto err;
+	}
+	return;
+
+err:
+	bond_slave_arr_work_rearm(bond, 1);
+}
+
+/* Build the usable slaves array in control path for modes that use xmit-hash
+ * to determine the slave interface -
+ * (a) BOND_MODE_8023AD
+ * (b) BOND_MODE_XOR
+ * (c) BOND_MODE_TLB && tlb_dynamic_lb == 0
+ *
+ * The caller is expected to hold RTNL only and NO other lock!
+ */
+int bond_update_slave_arr(struct bonding *bond, struct slave *skipslave)
+{
+	struct slave *slave;
+	struct list_head *iter;
+	struct bond_up_slave *new_arr, *old_arr;
+	int slaves_in_agg;
+	int agg_id = 0;
+	int ret = 0;
 
-	bond_xmit_slave_id(bond, skb, bond_xmit_hash(bond, skb) % bond->slave_cnt);
+#ifdef CONFIG_LOCKDEP
+	WARN_ON(lockdep_is_held(&bond->mode_lock));
+#endif
+
+	new_arr = kzalloc(offsetof(struct bond_up_slave, arr[bond->slave_cnt]),
+			  GFP_KERNEL);
+	if (!new_arr) {
+		ret = -ENOMEM;
+		pr_err("Failed to build slave-array.\n");
+		goto out;
+	}
+	if (BOND_MODE(bond) == BOND_MODE_8023AD) {
+		struct ad_info ad_info;
+
+		if (bond_3ad_get_active_agg_info(bond, &ad_info)) {
+			pr_debug("bond_3ad_get_active_agg_info failed\n");
+			kfree_rcu(new_arr, rcu);
+			/* No active aggragator means it's not safe to use
+			 * the previous array.
+			 */
+			old_arr = rtnl_dereference(bond->slave_arr);
+			if (old_arr) {
+				RCU_INIT_POINTER(bond->slave_arr, NULL);
+				kfree_rcu(old_arr, rcu);
+			}
+			goto out;
+		}
+		slaves_in_agg = ad_info.ports;
+		agg_id = ad_info.aggregator_id;
+	}
+	bond_for_each_slave(bond, slave, iter) {
+		if (BOND_MODE(bond) == BOND_MODE_8023AD) {
+			struct aggregator *agg;
+
+			agg = SLAVE_AD_INFO(slave)->port.aggregator;
+			if (!agg || agg->aggregator_identifier != agg_id)
+				continue;
+		}
+		if (!bond_slave_can_tx(slave))
+			continue;
+		if (skipslave == slave)
+			continue;
+		new_arr->arr[new_arr->count++] = slave;
+	}
+
+	old_arr = rtnl_dereference(bond->slave_arr);
+	rcu_assign_pointer(bond->slave_arr, new_arr);
+	if (old_arr)
+		kfree_rcu(old_arr, rcu);
+out:
+	if (ret != 0 && skipslave) {
+		int idx;
+
+		/* Rare situation where caller has asked to skip a specific
+		 * slave but allocation failed (most likely!). BTW this is
+		 * only possible when the call is initiated from
+		 * __bond_release_one(). In this situation; overwrite the
+		 * skipslave entry in the array with the last entry from the
+		 * array to avoid a situation where the xmit path may choose
+		 * this to-be-skipped slave to send a packet out.
+		 */
+		old_arr = rtnl_dereference(bond->slave_arr);
+		for (idx = 0; idx < old_arr->count; idx++) {
+			if (skipslave == old_arr->arr[idx]) {
+				old_arr->arr[idx] =
+				    old_arr->arr[old_arr->count-1];
+				old_arr->count--;
+				break;
+			}
+		}
+	}
+	return ret;
+}
+
+/* Use this Xmit function for 3AD as well as XOR modes. The current
+ * usable slave array is formed in the control path. The xmit function
+ * just calculates hash and sends the packet out.
+ */
+int bond_3ad_xor_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	struct bonding *bond = netdev_priv(dev);
+	struct slave *slave;
+	struct bond_up_slave *slaves;
+	unsigned int count;
+
+	slaves = rcu_dereference(bond->slave_arr);
+	count = slaves ? ACCESS_ONCE(slaves->count) : 0;
+	if (likely(count)) {
+		slave = slaves->arr[bond_xmit_hash(bond, skb) % count];
+		bond_dev_queue_xmit(bond, skb, slave->dev);
+	} else {
+		dev_kfree_skb_any(skb);
+		atomic_long_inc(&dev->tx_dropped);
+	}
 
 	return NETDEV_TX_OK;
 }
@@ -3715,7 +3772,6 @@ static int bond_xmit_broadcast(struct sk_buff *skb, struct net_device *bond_dev)
 						    bond_dev->name, __func__);
 				continue;
 			}
-			/* bond_dev_queue_xmit always returns 0 */
 			bond_dev_queue_xmit(bond, skb2, slave->dev);
 		}
 	}
@@ -3729,9 +3785,7 @@ static int bond_xmit_broadcast(struct sk_buff *skb, struct net_device *bond_dev)
 
 /*------------------------- Device initialization ---------------------------*/
 
-/*
- * Lookup the slave that corresponds to a qid
- */
+/* Lookup the slave that corresponds to a qid */
 static inline int bond_slave_override(struct bonding *bond,
 				      struct sk_buff *skb)
 {
@@ -3760,17 +3814,14 @@ static inline int bond_slave_override(struct bonding *bond,
 static u16 bond_select_queue(struct net_device *dev, struct sk_buff *skb,
 			     void *accel_priv, select_queue_fallback_t fallback)
 {
-	/*
-	 * This helper function exists to help dev_pick_tx get the correct
+	/* This helper function exists to help dev_pick_tx get the correct
 	 * destination queue.  Using a helper function skips a call to
 	 * skb_tx_hash and will put the skbs in the queue we expect on their
 	 * way down to the bonding driver.
 	 */
 	u16 txq = skb_rx_queue_recorded(skb) ? skb_get_rx_queue(skb) : 0;
 
-	/*
-	 * Save the original txq to restore before passing to the driver
-	 */
+	/* Save the original txq to restore before passing to the driver */
 	qdisc_skb_cb(skb)->slave_dev_queue_mapping = skb->queue_mapping;
 
 	if (unlikely(txq >= dev->real_num_tx_queues)) {
@@ -3794,12 +3845,11 @@ static netdev_tx_t __bond_start_xmit(struct sk_buff *skb, struct net_device *dev
 		return bond_xmit_roundrobin(skb, dev);
 	case BOND_MODE_ACTIVEBACKUP:
 		return bond_xmit_activebackup(skb, dev);
+	case BOND_MODE_8023AD:
 	case BOND_MODE_XOR:
-		return bond_xmit_xor(skb, dev);
+		return bond_3ad_xor_xmit(skb, dev);
 	case BOND_MODE_BROADCAST:
 		return bond_xmit_broadcast(skb, dev);
-	case BOND_MODE_8023AD:
-		return bond_3ad_xmit_xor(skb, dev);
 	case BOND_MODE_ALB:
 		return bond_alb_xmit(skb, dev);
 	case BOND_MODE_TLB:
@@ -3818,8 +3868,7 @@ static netdev_tx_t bond_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct bonding *bond = netdev_priv(dev);
 	netdev_tx_t ret = NETDEV_TX_OK;
 
-	/*
-	 * If we risk deadlock from transmitting this in the
+	/* If we risk deadlock from transmitting this in the
 	 * netpoll path, tell netpoll to queue the frame for later tx
 	 */
 	if (unlikely(is_netpoll_tx_blocked(dev)))
@@ -3851,7 +3900,6 @@ static int bond_ethtool_get_settings(struct net_device *bond_dev,
 	 * the true receive or transmit bandwidth (not all modes are symmetric)
 	 * this is an accurate maximum.
 	 */
-	read_lock(&bond->lock);
 	bond_for_each_slave(bond, slave, iter) {
 		if (bond_slave_can_tx(slave)) {
 			if (slave->speed != SPEED_UNKNOWN)
@@ -3862,7 +3910,6 @@ static int bond_ethtool_get_settings(struct net_device *bond_dev,
 		}
 	}
 	ethtool_cmd_speed_set(ecmd, speed ? : SPEED_UNKNOWN);
-	read_unlock(&bond->lock);
 
 	return 0;
 }
@@ -3924,9 +3971,7 @@ void bond_setup(struct net_device *bond_dev)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 
-	/* initialize rwlocks */
-	rwlock_init(&bond->lock);
-	rwlock_init(&bond->curr_slave_lock);
+	spin_lock_init(&bond->mode_lock);
 	bond->params = bonding_defaults;
 
 	/* Initialize pointers */
@@ -3947,8 +3992,7 @@ void bond_setup(struct net_device *bond_dev)
 	bond_dev->priv_flags |= IFF_BONDING | IFF_UNICAST_FLT;
 	bond_dev->priv_flags &= ~(IFF_XMIT_DST_RELEASE | IFF_TX_SKB_SHARING);
 
-	/* don't acquire bond device's netif_tx_lock when
-	 * transmitting */
+	/* don't acquire bond device's netif_tx_lock when transmitting */
 	bond_dev->features |= NETIF_F_LLTX;
 
 	/* By default, we declare the bond to be fully
@@ -3971,15 +4015,15 @@ void bond_setup(struct net_device *bond_dev)
 	bond_dev->features |= bond_dev->hw_features;
 }
 
-/*
-* Destroy a bonding device.
-* Must be under rtnl_lock when this function is called.
-*/
+/* Destroy a bonding device.
+ * Must be under rtnl_lock when this function is called.
+ */
 static void bond_uninit(struct net_device *bond_dev)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 	struct list_head *iter;
 	struct slave *slave;
+	struct bond_up_slave *arr;
 
 	bond_netpoll_cleanup(bond_dev);
 
@@ -3988,6 +4032,12 @@ static void bond_uninit(struct net_device *bond_dev)
 		__bond_release_one(bond_dev, slave->dev, true);
 	netdev_info(bond_dev, "Released all slaves\n");
 
+	arr = rtnl_dereference(bond->slave_arr);
+	if (arr) {
+		RCU_INIT_POINTER(bond->slave_arr, NULL);
+		kfree_rcu(arr, rcu);
+	}
+
 	list_del(&bond->bond_list);
 
 	bond_debug_unregister(bond);
@@ -4002,9 +4052,7 @@ static int bond_check_params(struct bond_params *params)
 	const struct bond_opt_value *valptr;
 	int arp_all_targets_value;
 
-	/*
-	 * Convert string parameters.
-	 */
+	/* Convert string parameters. */
 	if (mode) {
 		bond_opt_initstr(&newval, mode);
 		valptr = bond_opt_parse(bond_opt_get(BOND_OPT_MODE), &newval);
@@ -4181,9 +4229,9 @@ static int bond_check_params(struct bond_params *params)
 
 	for (arp_ip_count = 0, i = 0;
 	     (arp_ip_count < BOND_MAX_ARP_TARGETS) && arp_ip_target[i]; i++) {
-		/* not complete check, but should be good enough to
-		   catch mistakes */
 		__be32 ip;
+
+		/* not a complete check, but good enough to catch mistakes */
 		if (!in4_pton(arp_ip_target[i], -1, (u8 *)&ip, -1, NULL) ||
 		    !bond_is_ip_target_ok(ip)) {
 			pr_warn("Warning: bad arp_ip_target module parameter (%s), ARP monitoring will not be performed\n",
@@ -4366,26 +4414,14 @@ static void bond_set_lockdep_class(struct net_device *dev)
 	dev->qdisc_tx_busylock = &bonding_tx_busylock_key;
 }
 
-/*
- * Called from registration process
- */
+/* Called from registration process */
 static int bond_init(struct net_device *bond_dev)
 {
 	struct bonding *bond = netdev_priv(bond_dev);
 	struct bond_net *bn = net_generic(dev_net(bond_dev), bond_net_id);
-	struct alb_bond_info *bond_info = &(BOND_ALB_INFO(bond));
 
 	netdev_dbg(bond_dev, "Begin bond_init\n");
 
-	/*
-	 * Initialize locks that may be required during
-	 * en/deslave operations.  All of the bond_open work
-	 * (of which this is part) should really be moved to
-	 * a phase prior to dev_open
-	 */
-	spin_lock_init(&(bond_info->tx_hashtbl_lock));
-	spin_lock_init(&(bond_info->rx_hashtbl_lock));
-
 	bond->wq = create_singlethread_workqueue(bond_dev->name);
 	if (!bond->wq)
 		return -ENOMEM;
@@ -4532,9 +4568,7 @@ static void __exit bonding_exit(void)
 	unregister_pernet_subsys(&bond_net_ops);
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
-	/*
-	 * Make sure we don't have an imbalance on our netpoll blocking
-	 */
+	/* Make sure we don't have an imbalance on our netpoll blocking */
 	WARN_ON(atomic_read(&netpoll_block_tx));
 #endif
 }
diff --git a/drivers/net/bonding/bond_netlink.c b/drivers/net/bonding/bond_netlink.c
index d163e112f04c..c13d83e15ace 100644
--- a/drivers/net/bonding/bond_netlink.c
+++ b/drivers/net/bonding/bond_netlink.c
@@ -96,6 +96,10 @@ static const struct nla_policy bond_policy[IFLA_BOND_MAX + 1] = {
 	[IFLA_BOND_AD_INFO]		= { .type = NLA_NESTED },
 };
 
+static const struct nla_policy bond_slave_policy[IFLA_BOND_SLAVE_MAX + 1] = {
+	[IFLA_BOND_SLAVE_QUEUE_ID]	= { .type = NLA_U16 },
+};
+
 static int bond_validate(struct nlattr *tb[], struct nlattr *data[])
 {
 	if (tb[IFLA_ADDRESS]) {
@@ -107,6 +111,33 @@ static int bond_validate(struct nlattr *tb[], struct nlattr *data[])
 	return 0;
 }
 
+static int bond_slave_changelink(struct net_device *bond_dev,
+				 struct net_device *slave_dev,
+				 struct nlattr *tb[], struct nlattr *data[])
+{
+	struct bonding *bond = netdev_priv(bond_dev);
+	struct bond_opt_value newval;
+	int err;
+
+	if (!data)
+		return 0;
+
+	if (data[IFLA_BOND_SLAVE_QUEUE_ID]) {
+		u16 queue_id = nla_get_u16(data[IFLA_BOND_SLAVE_QUEUE_ID]);
+		char queue_id_str[IFNAMSIZ + 7];
+
+		/* queue_id option setting expects slave_name:queue_id */
+		snprintf(queue_id_str, sizeof(queue_id_str), "%s:%u\n",
+			 slave_dev->name, queue_id);
+		bond_opt_initstr(&newval, queue_id_str);
+		err = __bond_opt_set(bond, BOND_OPT_QUEUE_ID, &newval);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
 static int bond_changelink(struct net_device *bond_dev,
 			   struct nlattr *tb[], struct nlattr *data[])
 {
@@ -412,6 +443,7 @@ static int bond_fill_info(struct sk_buff *skb,
 	unsigned int packets_per_slave;
 	int ifindex, i, targets_added;
 	struct nlattr *targets;
+	struct slave *primary;
 
 	if (nla_put_u8(skb, IFLA_BOND_MODE, BOND_MODE(bond)))
 		goto nla_put_failure;
@@ -461,9 +493,9 @@ static int bond_fill_info(struct sk_buff *skb,
 			bond->params.arp_all_targets))
 		goto nla_put_failure;
 
-	if (bond->primary_slave &&
-	    nla_put_u32(skb, IFLA_BOND_PRIMARY,
-			bond->primary_slave->dev->ifindex))
+	primary = rtnl_dereference(bond->primary_slave);
+	if (primary &&
+	    nla_put_u32(skb, IFLA_BOND_PRIMARY, primary->dev->ifindex))
 		goto nla_put_failure;
 
 	if (nla_put_u8(skb, IFLA_BOND_PRIMARY_RESELECT,
@@ -562,6 +594,9 @@ struct rtnl_link_ops bond_link_ops __read_mostly = {
 	.get_num_tx_queues	= bond_get_num_tx_queues,
 	.get_num_rx_queues	= bond_get_num_tx_queues, /* Use the same number
 							     as for TX queues */
+	.slave_maxtype		= IFLA_BOND_SLAVE_MAX,
+	.slave_policy		= bond_slave_policy,
+	.slave_changelink	= bond_slave_changelink,
 	.get_slave_size		= bond_get_slave_size,
 	.fill_slave_info	= bond_fill_slave_info,
 };
diff --git a/drivers/net/bonding/bond_options.c b/drivers/net/bonding/bond_options.c
index dc73463c2c23..b62697f4a3de 100644
--- a/drivers/net/bonding/bond_options.c
+++ b/drivers/net/bonding/bond_options.c
@@ -625,6 +625,8 @@ int __bond_opt_set(struct bonding *bond,
 out:
 	if (ret)
 		bond_opt_error_interpret(bond, opt, ret, val);
+	else
+		call_netdevice_notifiers(NETDEV_CHANGEINFODATA, bond->dev);
 
 	return ret;
 }
@@ -732,15 +734,13 @@ static int bond_option_active_slave_set(struct bonding *bond,
 	}
 
 	block_netpoll_tx();
-	write_lock_bh(&bond->curr_slave_lock);
-
 	/* check to see if we are clearing active */
 	if (!slave_dev) {
 		netdev_info(bond->dev, "Clearing current active slave\n");
 		RCU_INIT_POINTER(bond->curr_active_slave, NULL);
 		bond_select_active_slave(bond);
 	} else {
-		struct slave *old_active = bond_deref_active_protected(bond);
+		struct slave *old_active = rtnl_dereference(bond->curr_active_slave);
 		struct slave *new_active = bond_slave_get_rtnl(slave_dev);
 
 		BUG_ON(!new_active);
@@ -763,8 +763,6 @@ static int bond_option_active_slave_set(struct bonding *bond,
 			}
 		}
 	}
-
-	write_unlock_bh(&bond->curr_slave_lock);
 	unblock_netpoll_tx();
 
 	return ret;
@@ -953,14 +951,7 @@ static int _bond_option_arp_ip_target_add(struct bonding *bond, __be32 target)
 
 static int bond_option_arp_ip_target_add(struct bonding *bond, __be32 target)
 {
-	int ret;
-
-	/* not to race with bond_arp_rcv */
-	write_lock_bh(&bond->lock);
-	ret = _bond_option_arp_ip_target_add(bond, target);
-	write_unlock_bh(&bond->lock);
-
-	return ret;
+	return _bond_option_arp_ip_target_add(bond, target);
 }
 
 static int bond_option_arp_ip_target_rem(struct bonding *bond, __be32 target)
@@ -989,9 +980,6 @@ static int bond_option_arp_ip_target_rem(struct bonding *bond, __be32 target)
 
 	netdev_info(bond->dev, "Removing ARP target %pI4\n", &target);
 
-	/* not to race with bond_arp_rcv */
-	write_lock_bh(&bond->lock);
-
 	bond_for_each_slave(bond, slave, iter) {
 		targets_rx = slave->target_last_arp_rx;
 		for (i = ind; (i < BOND_MAX_ARP_TARGETS-1) && targets[i+1]; i++)
@@ -1002,8 +990,6 @@ static int bond_option_arp_ip_target_rem(struct bonding *bond, __be32 target)
 		targets[i] = targets[i+1];
 	targets[i] = 0;
 
-	write_unlock_bh(&bond->lock);
-
 	return 0;
 }
 
@@ -1011,11 +997,8 @@ void bond_option_arp_ip_targets_clear(struct bonding *bond)
 {
 	int i;
 
-	/* not to race with bond_arp_rcv */
-	write_lock_bh(&bond->lock);
 	for (i = 0; i < BOND_MAX_ARP_TARGETS; i++)
 		_bond_options_arp_ip_target_set(bond, i, 0, 0);
-	write_unlock_bh(&bond->lock);
 }
 
 static int bond_option_arp_ip_targets_set(struct bonding *bond,
@@ -1079,8 +1062,6 @@ static int bond_option_primary_set(struct bonding *bond,
 	struct slave *slave;
 
 	block_netpoll_tx();
-	read_lock(&bond->lock);
-	write_lock_bh(&bond->curr_slave_lock);
 
 	p = strchr(primary, '\n');
 	if (p)
@@ -1088,7 +1069,7 @@ static int bond_option_primary_set(struct bonding *bond,
 	/* check to see if we are clearing primary */
 	if (!strlen(primary)) {
 		netdev_info(bond->dev, "Setting primary slave to None\n");
-		bond->primary_slave = NULL;
+		RCU_INIT_POINTER(bond->primary_slave, NULL);
 		memset(bond->params.primary, 0, sizeof(bond->params.primary));
 		bond_select_active_slave(bond);
 		goto out;
@@ -1098,16 +1079,16 @@ static int bond_option_primary_set(struct bonding *bond,
 		if (strncmp(slave->dev->name, primary, IFNAMSIZ) == 0) {
 			netdev_info(bond->dev, "Setting %s as primary slave\n",
 				    slave->dev->name);
-			bond->primary_slave = slave;
+			rcu_assign_pointer(bond->primary_slave, slave);
 			strcpy(bond->params.primary, slave->dev->name);
 			bond_select_active_slave(bond);
 			goto out;
 		}
 	}
 
-	if (bond->primary_slave) {
+	if (rtnl_dereference(bond->primary_slave)) {
 		netdev_info(bond->dev, "Setting primary slave to None\n");
-		bond->primary_slave = NULL;
+		RCU_INIT_POINTER(bond->primary_slave, NULL);
 		bond_select_active_slave(bond);
 	}
 	strncpy(bond->params.primary, primary, IFNAMSIZ);
@@ -1117,8 +1098,6 @@ static int bond_option_primary_set(struct bonding *bond,
 		    primary, bond->dev->name);
 
 out:
-	write_unlock_bh(&bond->curr_slave_lock);
-	read_unlock(&bond->lock);
 	unblock_netpoll_tx();
 
 	return 0;
@@ -1132,9 +1111,7 @@ static int bond_option_primary_reselect_set(struct bonding *bond,
 	bond->params.primary_reselect = newval->value;
 
 	block_netpoll_tx();
-	write_lock_bh(&bond->curr_slave_lock);
 	bond_select_active_slave(bond);
-	write_unlock_bh(&bond->curr_slave_lock);
 	unblock_netpoll_tx();
 
 	return 0;
diff --git a/drivers/net/bonding/bond_procfs.c b/drivers/net/bonding/bond_procfs.c
index de62c0385dfb..a3948f8d1e53 100644
--- a/drivers/net/bonding/bond_procfs.c
+++ b/drivers/net/bonding/bond_procfs.c
@@ -7,21 +7,18 @@
 
 static void *bond_info_seq_start(struct seq_file *seq, loff_t *pos)
 	__acquires(RCU)
-	__acquires(&bond->lock)
 {
 	struct bonding *bond = seq->private;
 	struct list_head *iter;
 	struct slave *slave;
 	loff_t off = 0;
 
-	/* make sure the bond won't be taken away */
 	rcu_read_lock();
-	read_lock(&bond->lock);
 
 	if (*pos == 0)
 		return SEQ_START_TOKEN;
 
-	bond_for_each_slave(bond, slave, iter)
+	bond_for_each_slave_rcu(bond, slave, iter)
 		if (++off == *pos)
 			return slave;
 
@@ -37,12 +34,9 @@ static void *bond_info_seq_next(struct seq_file *seq, void *v, loff_t *pos)
 
 	++*pos;
 	if (v == SEQ_START_TOKEN)
-		return bond_first_slave(bond);
+		return bond_first_slave_rcu(bond);
 
-	if (bond_is_last_slave(bond, v))
-		return NULL;
-
-	bond_for_each_slave(bond, slave, iter) {
+	bond_for_each_slave_rcu(bond, slave, iter) {
 		if (found)
 			return slave;
 		if (slave == v)
@@ -53,12 +47,8 @@ static void *bond_info_seq_next(struct seq_file *seq, void *v, loff_t *pos)
 }
 
 static void bond_info_seq_stop(struct seq_file *seq, void *v)
-	__releases(&bond->lock)
 	__releases(RCU)
 {
-	struct bonding *bond = seq->private;
-
-	read_unlock(&bond->lock);
 	rcu_read_unlock();
 }
 
@@ -66,7 +56,7 @@ static void bond_info_show_master(struct seq_file *seq)
 {
 	struct bonding *bond = seq->private;
 	const struct bond_opt_value *optval;
-	struct slave *curr;
+	struct slave *curr, *primary;
 	int i;
 
 	curr = rcu_dereference(bond->curr_active_slave);
@@ -83,8 +73,7 @@ static void bond_info_show_master(struct seq_file *seq)
 
 	seq_printf(seq, "\n");
 
-	if (BOND_MODE(bond) == BOND_MODE_XOR ||
-		BOND_MODE(bond) == BOND_MODE_8023AD) {
+	if (bond_mode_uses_xmit_hash(bond)) {
 		optval = bond_opt_get_val(BOND_OPT_XMIT_HASH,
 					  bond->params.xmit_policy);
 		seq_printf(seq, "Transmit Hash Policy: %s (%d)\n",
@@ -92,10 +81,10 @@ static void bond_info_show_master(struct seq_file *seq)
 	}
 
 	if (bond_uses_primary(bond)) {
+		primary = rcu_dereference(bond->primary_slave);
 		seq_printf(seq, "Primary Slave: %s",
-			   (bond->primary_slave) ?
-			   bond->primary_slave->dev->name : "None");
-		if (bond->primary_slave) {
+			   primary ? primary->dev->name : "None");
+		if (primary) {
 			optval = bond_opt_get_val(BOND_OPT_PRIMARY_RESELECT,
 						  bond->params.primary_reselect);
 			seq_printf(seq, " (primary_reselect %s)",
diff --git a/drivers/net/bonding/bond_sysfs.c b/drivers/net/bonding/bond_sysfs.c
index 98db8edd9c75..8ffbafd500fd 100644
--- a/drivers/net/bonding/bond_sysfs.c
+++ b/drivers/net/bonding/bond_sysfs.c
@@ -91,7 +91,6 @@ static struct net_device *bond_get_by_name(struct bond_net *bn, const char *ifna
  * creates and deletes entire bonds.
  *
  * The class parameter is ignored.
- *
  */
 static ssize_t bonding_store_bonds(struct class *cls,
 				   struct class_attribute *attr,
@@ -425,11 +424,15 @@ static ssize_t bonding_show_primary(struct device *d,
 				    struct device_attribute *attr,
 				    char *buf)
 {
-	int count = 0;
 	struct bonding *bond = to_bond(d);
+	struct slave *primary;
+	int count = 0;
 
-	if (bond->primary_slave)
-		count = sprintf(buf, "%s\n", bond->primary_slave->dev->name);
+	rcu_read_lock();
+	primary = rcu_dereference(bond->primary_slave);
+	if (primary)
+		count = sprintf(buf, "%s\n", primary->dev->name);
+	rcu_read_unlock();
 
 	return count;
 }
diff --git a/drivers/net/bonding/bond_sysfs_slave.c b/drivers/net/bonding/bond_sysfs_slave.c
index 5cd532ca1cfe..b01b0ce4d1be 100644
--- a/drivers/net/bonding/bond_sysfs_slave.c
+++ b/drivers/net/bonding/bond_sysfs_slave.c
@@ -36,7 +36,7 @@ static ssize_t state_show(struct slave *slave, char *buf)
 	case BOND_STATE_BACKUP:
 		return sprintf(buf, "backup\n");
 	default:
-		return sprintf(buf, "UNKONWN\n");
+		return sprintf(buf, "UNKNOWN\n");
 	}
 }
 static SLAVE_ATTR_RO(state);
diff --git a/drivers/net/bonding/bonding.h b/drivers/net/bonding/bonding.h
index aace510d08d1..10920f0686e2 100644
--- a/drivers/net/bonding/bonding.h
+++ b/drivers/net/bonding/bonding.h
@@ -24,6 +24,7 @@
 #include <linux/inetdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/reciprocal_div.h>
+#include <linux/if_link.h>
 
 #include "bond_3ad.h"
 #include "bond_alb.h"
@@ -83,7 +84,7 @@
  * @pos:	current slave
  * @iter:	list_head * iterator
  *
- * Caller must hold bond->lock
+ * Caller must hold RTNL
  */
 #define bond_for_each_slave(bond, pos, iter) \
 	netdev_for_each_lower_private((bond)->dev, pos, iter)
@@ -175,6 +176,13 @@ struct slave {
 	struct netpoll *np;
 #endif
 	struct kobject kobj;
+	struct rtnl_link_stats64 slave_stats;
+};
+
+struct bond_up_slave {
+	unsigned int	count;
+	struct rcu_head rcu;
+	struct slave	*arr[0];
 };
 
 /*
@@ -184,24 +192,26 @@ struct slave {
 
 /*
  * Here are the locking policies for the two bonding locks:
- *
- * 1) Get bond->lock when reading/writing slave list.
- * 2) Get bond->curr_slave_lock when reading/writing bond->curr_active_slave.
- *    (It is unnecessary when the write-lock is put with bond->lock.)
- * 3) When we lock with bond->curr_slave_lock, we must lock with bond->lock
- *    beforehand.
+ * Get rcu_read_lock when reading or RTNL when writing slave list.
  */
 struct bonding {
 	struct   net_device *dev; /* first - useful for panic debug */
 	struct   slave __rcu *curr_active_slave;
 	struct   slave __rcu *current_arp_slave;
-	struct   slave *primary_slave;
+	struct   slave __rcu *primary_slave;
+	struct   bond_up_slave __rcu *slave_arr; /* Array of usable slaves */
 	bool     force_primary;
 	s32      slave_cnt; /* never change this value outside the attach/detach wrappers */
 	int     (*recv_probe)(const struct sk_buff *, struct bonding *,
 			      struct slave *);
-	rwlock_t lock;
-	rwlock_t curr_slave_lock;
+	/* mode_lock is used for mode-specific locking needs, currently used by:
+	 * 3ad mode (4) - protect against running bond_3ad_unbind_slave() and
+	 *                bond_3ad_state_machine_handler() concurrently and also
+	 *                the access to the state machine shared variables.
+	 * TLB mode (5) - to sync the use and modifications of its hash table
+	 * ALB mode (6) - to sync the use and modifications of its hash table
+	 */
+	spinlock_t mode_lock;
 	u8	 send_peer_notif;
 	u8       igmp_retrans;
 #ifdef CONFIG_PROC_FS
@@ -219,10 +229,12 @@ struct bonding {
 	struct   delayed_work alb_work;
 	struct   delayed_work ad_work;
 	struct   delayed_work mcast_work;
+	struct   delayed_work slave_arr_work;
 #ifdef CONFIG_DEBUG_FS
 	/* debugging support via debugfs */
 	struct	 dentry *debug_dir;
 #endif /* CONFIG_DEBUG_FS */
+	struct rtnl_link_stats64 bond_stats;
 };
 
 #define bond_slave_get_rcu(dev) \
@@ -231,10 +243,6 @@ struct bonding {
 #define bond_slave_get_rtnl(dev) \
 	((struct slave *) rtnl_dereference(dev->rx_handler_data))
 
-#define bond_deref_active_protected(bond)				   \
-	rcu_dereference_protected(bond->curr_active_slave,		   \
-				  lockdep_is_held(&bond->curr_slave_lock))
-
 struct bond_vlan_tag {
 	__be16		vlan_proto;
 	unsigned short	vlan_id;
@@ -274,6 +282,13 @@ static inline bool bond_is_nondyn_tlb(const struct bonding *bond)
 	       (bond->params.tlb_dynamic_lb == 0);
 }
 
+static inline bool bond_mode_uses_xmit_hash(const struct bonding *bond)
+{
+	return (BOND_MODE(bond) == BOND_MODE_8023AD ||
+		BOND_MODE(bond) == BOND_MODE_XOR ||
+		bond_is_nondyn_tlb(bond));
+}
+
 static inline bool bond_mode_uses_arp(int mode)
 {
 	return mode != BOND_MODE_8023AD && mode != BOND_MODE_TLB &&
@@ -527,6 +542,8 @@ const char *bond_slave_link_status(s8 link);
 struct bond_vlan_tag *bond_verify_device_path(struct net_device *start_dev,
 					      struct net_device *end_dev,
 					      int level);
+int bond_update_slave_arr(struct bonding *bond, struct slave *skipslave);
+void bond_slave_arr_work_rearm(struct bonding *bond, unsigned long delay);
 
 #ifdef CONFIG_PROC_FS
 void bond_create_proc_entry(struct bonding *bond);
diff --git a/drivers/net/can/Kconfig b/drivers/net/can/Kconfig
index 41688229c570..98d73aab52fe 100644
--- a/drivers/net/can/Kconfig
+++ b/drivers/net/can/Kconfig
@@ -65,7 +65,7 @@ config CAN_LEDS
 
 config CAN_AT91
 	tristate "Atmel AT91 onchip CAN controller"
-	depends on ARCH_AT91 || COMPILE_TEST
+	depends on (ARCH_AT91 || COMPILE_TEST) && HAS_IOMEM
 	---help---
 	  This is a driver for the SoC CAN controller in Atmel's AT91SAM9263
 	  and AT91SAM9X5 processors.
@@ -143,6 +143,8 @@ source "drivers/net/can/sja1000/Kconfig"
 
 source "drivers/net/can/c_can/Kconfig"
 
+source "drivers/net/can/m_can/Kconfig"
+
 source "drivers/net/can/cc770/Kconfig"
 
 source "drivers/net/can/spi/Kconfig"
diff --git a/drivers/net/can/Makefile b/drivers/net/can/Makefile
index 1697f22353a9..fc9304143f44 100644
--- a/drivers/net/can/Makefile
+++ b/drivers/net/can/Makefile
@@ -17,6 +17,7 @@ obj-y				+= softing/
 obj-$(CONFIG_CAN_SJA1000)	+= sja1000/
 obj-$(CONFIG_CAN_MSCAN)		+= mscan/
 obj-$(CONFIG_CAN_C_CAN)		+= c_can/
+obj-$(CONFIG_CAN_M_CAN)		+= m_can/
 obj-$(CONFIG_CAN_CC770)		+= cc770/
 obj-$(CONFIG_CAN_AT91)		+= at91_can.o
 obj-$(CONFIG_CAN_TI_HECC)	+= ti_hecc.o
@@ -28,4 +29,4 @@ obj-$(CONFIG_CAN_GRCAN)		+= grcan.o
 obj-$(CONFIG_CAN_RCAR)		+= rcar_can.o
 obj-$(CONFIG_CAN_XILINXCAN)	+= xilinx_can.o
 
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
+subdir-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/at91_can.c b/drivers/net/can/at91_can.c
index f07fa89b5fd5..05e1aa090add 100644
--- a/drivers/net/can/at91_can.c
+++ b/drivers/net/can/at91_can.c
@@ -1123,7 +1123,9 @@ static int at91_open(struct net_device *dev)
 	struct at91_priv *priv = netdev_priv(dev);
 	int err;
 
-	clk_enable(priv->clk);
+	err = clk_prepare_enable(priv->clk);
+	if (err)
+		return err;
 
 	/* check or determine and set bittime */
 	err = open_candev(dev);
@@ -1149,7 +1151,7 @@ static int at91_open(struct net_device *dev)
  out_close:
 	close_candev(dev);
  out:
-	clk_disable(priv->clk);
+	clk_disable_unprepare(priv->clk);
 
 	return err;
 }
@@ -1166,7 +1168,7 @@ static int at91_close(struct net_device *dev)
 	at91_chip_stop(dev, CAN_STATE_STOPPED);
 
 	free_irq(dev->irq, dev);
-	clk_disable(priv->clk);
+	clk_disable_unprepare(priv->clk);
 
 	close_candev(dev);
 
diff --git a/drivers/net/can/c_can/Makefile b/drivers/net/can/c_can/Makefile
index ad1cc842170a..9fdc678b5b37 100644
--- a/drivers/net/can/c_can/Makefile
+++ b/drivers/net/can/c_can/Makefile
@@ -5,5 +5,3 @@
 obj-$(CONFIG_CAN_C_CAN) += c_can.o
 obj-$(CONFIG_CAN_C_CAN_PLATFORM) += c_can_platform.o
 obj-$(CONFIG_CAN_C_CAN_PCI) += c_can_pci.o
-
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/c_can/c_can_platform.c b/drivers/net/can/c_can/c_can_platform.c
index 5dede6e64376..fb279d6ae484 100644
--- a/drivers/net/can/c_can/c_can_platform.c
+++ b/drivers/net/can/c_can/c_can_platform.c
@@ -97,14 +97,14 @@ static void c_can_hw_raminit_ti(const struct c_can_priv *priv, bool enable)
 	ctrl |= CAN_RAMINIT_DONE_MASK(priv->instance);
 	writel(ctrl, priv->raminit_ctrlreg);
 	ctrl &= ~CAN_RAMINIT_DONE_MASK(priv->instance);
-	c_can_hw_raminit_wait_ti(priv, ctrl, mask);
+	c_can_hw_raminit_wait_ti(priv, mask, ctrl);
 
 	if (enable) {
 		/* Set start bit and wait for the done bit. */
 		ctrl |= CAN_RAMINIT_START_MASK(priv->instance);
 		writel(ctrl, priv->raminit_ctrlreg);
 		ctrl |= CAN_RAMINIT_DONE_MASK(priv->instance);
-		c_can_hw_raminit_wait_ti(priv, ctrl, mask);
+		c_can_hw_raminit_wait_ti(priv, mask, ctrl);
 	}
 	spin_unlock(&raminit_lock);
 }
@@ -280,7 +280,7 @@ static int c_can_plat_probe(struct platform_device *pdev)
 
 		priv->raminit_ctrlreg = devm_ioremap(&pdev->dev, res->start,
 						     resource_size(res));
-		if (IS_ERR(priv->raminit_ctrlreg) || priv->instance < 0)
+		if (!priv->raminit_ctrlreg || priv->instance < 0)
 			dev_info(&pdev->dev, "control memory is not used for raminit\n");
 		else
 			priv->raminit = c_can_hw_raminit_ti;
diff --git a/drivers/net/can/cc770/Makefile b/drivers/net/can/cc770/Makefile
index 9fb8321b33eb..8657f879ae19 100644
--- a/drivers/net/can/cc770/Makefile
+++ b/drivers/net/can/cc770/Makefile
@@ -5,5 +5,3 @@
 obj-$(CONFIG_CAN_CC770) += cc770.o
 obj-$(CONFIG_CAN_CC770_ISA) += cc770_isa.o
 obj-$(CONFIG_CAN_CC770_PLATFORM) += cc770_platform.o
-
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/dev.c b/drivers/net/can/dev.c
index 9f91fcba43f8..02492d241e4c 100644
--- a/drivers/net/can/dev.c
+++ b/drivers/net/can/dev.c
@@ -103,11 +103,11 @@ static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt,
 			      const struct can_bittiming_const *btc)
 {
 	struct can_priv *priv = netdev_priv(dev);
-	long rate, best_rate = 0;
 	long best_error = 1000000000, error = 0;
 	int best_tseg = 0, best_brp = 0, brp = 0;
 	int tsegall, tseg = 0, tseg1 = 0, tseg2 = 0;
 	int spt_error = 1000, spt = 0, sampl_pt;
+	long rate;
 	u64 v64;
 
 	/* Use CIA recommended sample points */
@@ -152,7 +152,6 @@ static int can_calc_bittiming(struct net_device *dev, struct can_bittiming *bt,
 		}
 		best_tseg = tseg / 2;
 		best_brp = brp;
-		best_rate = rate;
 		if (error == 0)
 			break;
 	}
diff --git a/drivers/net/can/flexcan.c b/drivers/net/can/flexcan.c
index f425ec2c7839..60f86bd0434a 100644
--- a/drivers/net/can/flexcan.c
+++ b/drivers/net/can/flexcan.c
@@ -62,7 +62,7 @@
 #define FLEXCAN_MCR_BCC			BIT(16)
 #define FLEXCAN_MCR_LPRIO_EN		BIT(13)
 #define FLEXCAN_MCR_AEN			BIT(12)
-#define FLEXCAN_MCR_MAXMB(x)		((x) & 0x1f)
+#define FLEXCAN_MCR_MAXMB(x)		((x) & 0x7f)
 #define FLEXCAN_MCR_IDAM_A		(0 << 8)
 #define FLEXCAN_MCR_IDAM_B		(1 << 8)
 #define FLEXCAN_MCR_IDAM_C		(2 << 8)
@@ -92,6 +92,27 @@
 #define FLEXCAN_CTRL_ERR_ALL \
 	(FLEXCAN_CTRL_ERR_BUS | FLEXCAN_CTRL_ERR_STATE)
 
+/* FLEXCAN control register 2 (CTRL2) bits */
+#define FLEXCAN_CRL2_ECRWRE		BIT(29)
+#define FLEXCAN_CRL2_WRMFRZ		BIT(28)
+#define FLEXCAN_CRL2_RFFN(x)		(((x) & 0x0f) << 24)
+#define FLEXCAN_CRL2_TASD(x)		(((x) & 0x1f) << 19)
+#define FLEXCAN_CRL2_MRP		BIT(18)
+#define FLEXCAN_CRL2_RRS		BIT(17)
+#define FLEXCAN_CRL2_EACEN		BIT(16)
+
+/* FLEXCAN memory error control register (MECR) bits */
+#define FLEXCAN_MECR_ECRWRDIS		BIT(31)
+#define FLEXCAN_MECR_HANCEI_MSK		BIT(19)
+#define FLEXCAN_MECR_FANCEI_MSK		BIT(18)
+#define FLEXCAN_MECR_CEI_MSK		BIT(16)
+#define FLEXCAN_MECR_HAERRIE		BIT(15)
+#define FLEXCAN_MECR_FAERRIE		BIT(14)
+#define FLEXCAN_MECR_EXTERRIE		BIT(13)
+#define FLEXCAN_MECR_RERRDIS		BIT(9)
+#define FLEXCAN_MECR_ECCDIS		BIT(8)
+#define FLEXCAN_MECR_NCEFAFRZ		BIT(7)
+
 /* FLEXCAN error and status register (ESR) bits */
 #define FLEXCAN_ESR_TWRN_INT		BIT(17)
 #define FLEXCAN_ESR_RWRN_INT		BIT(16)
@@ -125,7 +146,9 @@
 	 FLEXCAN_ESR_BOFF_INT | FLEXCAN_ESR_ERR_INT)
 
 /* FLEXCAN interrupt flag register (IFLAG) bits */
-#define FLEXCAN_TX_BUF_ID		8
+/* Errata ERR005829 step7: Reserve first valid MB */
+#define FLEXCAN_TX_BUF_RESERVED		8
+#define FLEXCAN_TX_BUF_ID		9
 #define FLEXCAN_IFLAG_BUF(x)		BIT(x)
 #define FLEXCAN_IFLAG_RX_FIFO_OVERFLOW	BIT(7)
 #define FLEXCAN_IFLAG_RX_FIFO_WARN	BIT(6)
@@ -136,6 +159,17 @@
 
 /* FLEXCAN message buffers */
 #define FLEXCAN_MB_CNT_CODE(x)		(((x) & 0xf) << 24)
+#define FLEXCAN_MB_CODE_RX_INACTIVE	(0x0 << 24)
+#define FLEXCAN_MB_CODE_RX_EMPTY	(0x4 << 24)
+#define FLEXCAN_MB_CODE_RX_FULL		(0x2 << 24)
+#define FLEXCAN_MB_CODE_RX_OVERRRUN	(0x6 << 24)
+#define FLEXCAN_MB_CODE_RX_RANSWER	(0xa << 24)
+
+#define FLEXCAN_MB_CODE_TX_INACTIVE	(0x8 << 24)
+#define FLEXCAN_MB_CODE_TX_ABORT	(0x9 << 24)
+#define FLEXCAN_MB_CODE_TX_DATA		(0xc << 24)
+#define FLEXCAN_MB_CODE_TX_TANSWER	(0xe << 24)
+
 #define FLEXCAN_MB_CNT_SRR		BIT(22)
 #define FLEXCAN_MB_CNT_IDE		BIT(21)
 #define FLEXCAN_MB_CNT_RTR		BIT(20)
@@ -150,18 +184,20 @@
  * FLEXCAN hardware feature flags
  *
  * Below is some version info we got:
- *    SOC   Version   IP-Version  Glitch-  [TR]WRN_INT
- *                                Filter?   connected?
- *   MX25  FlexCAN2  03.00.00.00     no         no
- *   MX28  FlexCAN2  03.00.04.00    yes        yes
- *   MX35  FlexCAN2  03.00.00.00     no         no
- *   MX53  FlexCAN2  03.00.00.00    yes         no
- *   MX6s  FlexCAN3  10.00.12.00    yes        yes
+ *    SOC   Version   IP-Version  Glitch-  [TR]WRN_INT  Memory err
+ *                                Filter?   connected?  detection
+ *   MX25  FlexCAN2  03.00.00.00     no         no         no
+ *   MX28  FlexCAN2  03.00.04.00    yes        yes         no
+ *   MX35  FlexCAN2  03.00.00.00     no         no         no
+ *   MX53  FlexCAN2  03.00.00.00    yes         no         no
+ *   MX6s  FlexCAN3  10.00.12.00    yes        yes         no
+ *   VF610 FlexCAN3  ?               no        yes        yes
  *
  * Some SOCs do not have the RX_WARN & TX_WARN interrupt line connected.
  */
 #define FLEXCAN_HAS_V10_FEATURES	BIT(1) /* For core version >= 10 */
 #define FLEXCAN_HAS_BROKEN_ERR_STATE	BIT(2) /* [TR]WRN_INT not connected */
+#define FLEXCAN_HAS_MECR_FEATURES	BIT(3) /* Memory error detection */
 
 /* Structure of the message buffer */
 struct flexcan_mb {
@@ -192,8 +228,17 @@ struct flexcan_regs {
 	u32 crcr;		/* 0x44 */
 	u32 rxfgmask;		/* 0x48 */
 	u32 rxfir;		/* 0x4c */
-	u32 _reserved3[12];
-	struct flexcan_mb cantxfg[64];
+	u32 _reserved3[12];	/* 0x50 */
+	struct flexcan_mb cantxfg[64];	/* 0x80 */
+	u32 _reserved4[408];
+	u32 mecr;		/* 0xae0 */
+	u32 erriar;		/* 0xae4 */
+	u32 erridpr;		/* 0xae8 */
+	u32 errippr;		/* 0xaec */
+	u32 rerrar;		/* 0xaf0 */
+	u32 rerrdr;		/* 0xaf4 */
+	u32 rerrsynr;		/* 0xaf8 */
+	u32 errsr;		/* 0xafc */
 };
 
 struct flexcan_devtype_data {
@@ -223,6 +268,9 @@ static struct flexcan_devtype_data fsl_imx28_devtype_data;
 static struct flexcan_devtype_data fsl_imx6q_devtype_data = {
 	.features = FLEXCAN_HAS_V10_FEATURES,
 };
+static struct flexcan_devtype_data fsl_vf610_devtype_data = {
+	.features = FLEXCAN_HAS_V10_FEATURES | FLEXCAN_HAS_MECR_FEATURES,
+};
 
 static const struct can_bittiming_const flexcan_bittiming_const = {
 	.name = DRV_NAME,
@@ -298,7 +346,7 @@ static int flexcan_chip_enable(struct flexcan_priv *priv)
 	flexcan_write(reg, &regs->mcr);
 
 	while (timeout-- && (flexcan_read(&regs->mcr) & FLEXCAN_MCR_LPM_ACK))
-		usleep_range(10, 20);
+		udelay(10);
 
 	if (flexcan_read(&regs->mcr) & FLEXCAN_MCR_LPM_ACK)
 		return -ETIMEDOUT;
@@ -317,7 +365,7 @@ static int flexcan_chip_disable(struct flexcan_priv *priv)
 	flexcan_write(reg, &regs->mcr);
 
 	while (timeout-- && !(flexcan_read(&regs->mcr) & FLEXCAN_MCR_LPM_ACK))
-		usleep_range(10, 20);
+		udelay(10);
 
 	if (!(flexcan_read(&regs->mcr) & FLEXCAN_MCR_LPM_ACK))
 		return -ETIMEDOUT;
@@ -336,7 +384,7 @@ static int flexcan_chip_freeze(struct flexcan_priv *priv)
 	flexcan_write(reg, &regs->mcr);
 
 	while (timeout-- && !(flexcan_read(&regs->mcr) & FLEXCAN_MCR_FRZ_ACK))
-		usleep_range(100, 200);
+		udelay(100);
 
 	if (!(flexcan_read(&regs->mcr) & FLEXCAN_MCR_FRZ_ACK))
 		return -ETIMEDOUT;
@@ -355,7 +403,7 @@ static int flexcan_chip_unfreeze(struct flexcan_priv *priv)
 	flexcan_write(reg, &regs->mcr);
 
 	while (timeout-- && (flexcan_read(&regs->mcr) & FLEXCAN_MCR_FRZ_ACK))
-		usleep_range(10, 20);
+		udelay(10);
 
 	if (flexcan_read(&regs->mcr) & FLEXCAN_MCR_FRZ_ACK)
 		return -ETIMEDOUT;
@@ -370,7 +418,7 @@ static int flexcan_chip_softreset(struct flexcan_priv *priv)
 
 	flexcan_write(FLEXCAN_MCR_SOFTRST, &regs->mcr);
 	while (timeout-- && (flexcan_read(&regs->mcr) & FLEXCAN_MCR_SOFTRST))
-		usleep_range(10, 20);
+		udelay(10);
 
 	if (flexcan_read(&regs->mcr) & FLEXCAN_MCR_SOFTRST)
 		return -ETIMEDOUT;
@@ -378,8 +426,9 @@ static int flexcan_chip_softreset(struct flexcan_priv *priv)
 	return 0;
 }
 
-static int flexcan_get_berr_counter(const struct net_device *dev,
-				    struct can_berr_counter *bec)
+
+static int __flexcan_get_berr_counter(const struct net_device *dev,
+				      struct can_berr_counter *bec)
 {
 	const struct flexcan_priv *priv = netdev_priv(dev);
 	struct flexcan_regs __iomem *regs = priv->base;
@@ -391,6 +440,29 @@ static int flexcan_get_berr_counter(const struct net_device *dev,
 	return 0;
 }
 
+static int flexcan_get_berr_counter(const struct net_device *dev,
+				    struct can_berr_counter *bec)
+{
+	const struct flexcan_priv *priv = netdev_priv(dev);
+	int err;
+
+	err = clk_prepare_enable(priv->clk_ipg);
+	if (err)
+		return err;
+
+	err = clk_prepare_enable(priv->clk_per);
+	if (err)
+		goto out_disable_ipg;
+
+	err = __flexcan_get_berr_counter(dev, bec);
+
+	clk_disable_unprepare(priv->clk_per);
+ out_disable_ipg:
+	clk_disable_unprepare(priv->clk_ipg);
+
+	return err;
+}
+
 static int flexcan_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	const struct flexcan_priv *priv = netdev_priv(dev);
@@ -428,6 +500,14 @@ static int flexcan_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	flexcan_write(can_id, &regs->cantxfg[FLEXCAN_TX_BUF_ID].can_id);
 	flexcan_write(ctrl, &regs->cantxfg[FLEXCAN_TX_BUF_ID].can_ctrl);
 
+	/* Errata ERR005829 step8:
+	 * Write twice INACTIVE(0x8) code to first MB.
+	 */
+	flexcan_write(FLEXCAN_MB_CODE_TX_INACTIVE,
+		      &regs->cantxfg[FLEXCAN_TX_BUF_RESERVED].can_ctrl);
+	flexcan_write(FLEXCAN_MB_CODE_TX_INACTIVE,
+		      &regs->cantxfg[FLEXCAN_TX_BUF_RESERVED].can_ctrl);
+
 	return NETDEV_TX_OK;
 }
 
@@ -503,7 +583,7 @@ static void do_state(struct net_device *dev,
 	struct flexcan_priv *priv = netdev_priv(dev);
 	struct can_berr_counter bec;
 
-	flexcan_get_berr_counter(dev, &bec);
+	__flexcan_get_berr_counter(dev, &bec);
 
 	switch (priv->can.state) {
 	case CAN_STATE_ERROR_ACTIVE:
@@ -549,6 +629,13 @@ static void do_state(struct net_device *dev,
 
 	/* process state changes depending on the new state */
 	switch (new_state) {
+	case CAN_STATE_ERROR_WARNING:
+		netdev_dbg(dev, "Error Warning\n");
+		cf->can_id |= CAN_ERR_CRTL;
+		cf->data[1] = (bec.txerr > bec.rxerr) ?
+			CAN_ERR_CRTL_TX_WARNING :
+			CAN_ERR_CRTL_RX_WARNING;
+		break;
 	case CAN_STATE_ERROR_ACTIVE:
 		netdev_dbg(dev, "Error Active\n");
 		cf->can_id |= CAN_ERR_PROT;
@@ -737,6 +824,9 @@ static irqreturn_t flexcan_irq(int irq, void *dev_id)
 		stats->tx_bytes += can_get_echo_skb(dev, 0);
 		stats->tx_packets++;
 		can_led_event(dev, CAN_LED_EVENT_TX);
+		/* after sending a RTR frame mailbox is in RX mode */
+		flexcan_write(FLEXCAN_MB_CODE_TX_INACTIVE,
+			      &regs->cantxfg[FLEXCAN_TX_BUF_ID].can_ctrl);
 		flexcan_write((1 << FLEXCAN_TX_BUF_ID), &regs->iflag1);
 		netif_wake_queue(dev);
 	}
@@ -792,8 +882,8 @@ static int flexcan_chip_start(struct net_device *dev)
 {
 	struct flexcan_priv *priv = netdev_priv(dev);
 	struct flexcan_regs __iomem *regs = priv->base;
-	int err;
-	u32 reg_mcr, reg_ctrl;
+	u32 reg_mcr, reg_ctrl, reg_crl2, reg_mecr;
+	int err, i;
 
 	/* enable module */
 	err = flexcan_chip_enable(priv);
@@ -852,14 +942,26 @@ static int flexcan_chip_start(struct net_device *dev)
 	if (priv->devtype_data->features & FLEXCAN_HAS_BROKEN_ERR_STATE ||
 	    priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING)
 		reg_ctrl |= FLEXCAN_CTRL_ERR_MSK;
+	else
+		reg_ctrl &= ~FLEXCAN_CTRL_ERR_MSK;
 
 	/* save for later use */
 	priv->reg_ctrl_default = reg_ctrl;
 	netdev_dbg(dev, "%s: writing ctrl=0x%08x", __func__, reg_ctrl);
 	flexcan_write(reg_ctrl, &regs->ctrl);
 
-	/* Abort any pending TX, mark Mailbox as INACTIVE */
-	flexcan_write(FLEXCAN_MB_CNT_CODE(0x4),
+	/* clear and invalidate all mailboxes first */
+	for (i = FLEXCAN_TX_BUF_ID; i < ARRAY_SIZE(regs->cantxfg); i++) {
+		flexcan_write(FLEXCAN_MB_CODE_RX_INACTIVE,
+			      &regs->cantxfg[i].can_ctrl);
+	}
+
+	/* Errata ERR005829: mark first TX mailbox as INACTIVE */
+	flexcan_write(FLEXCAN_MB_CODE_TX_INACTIVE,
+		      &regs->cantxfg[FLEXCAN_TX_BUF_RESERVED].can_ctrl);
+
+	/* mark TX mailbox as INACTIVE */
+	flexcan_write(FLEXCAN_MB_CODE_TX_INACTIVE,
 		      &regs->cantxfg[FLEXCAN_TX_BUF_ID].can_ctrl);
 
 	/* acceptance mask/acceptance code (accept everything) */
@@ -870,6 +972,31 @@ static int flexcan_chip_start(struct net_device *dev)
 	if (priv->devtype_data->features & FLEXCAN_HAS_V10_FEATURES)
 		flexcan_write(0x0, &regs->rxfgmask);
 
+	/*
+	 * On Vybrid, disable memory error detection interrupts
+	 * and freeze mode.
+	 * This also works around errata e5295 which generates
+	 * false positive memory errors and put the device in
+	 * freeze mode.
+	 */
+	if (priv->devtype_data->features & FLEXCAN_HAS_MECR_FEATURES) {
+		/*
+		 * Follow the protocol as described in "Detection
+		 * and Correction of Memory Errors" to write to
+		 * MECR register
+		 */
+		reg_crl2 = flexcan_read(&regs->crl2);
+		reg_crl2 |= FLEXCAN_CRL2_ECRWRE;
+		flexcan_write(reg_crl2, &regs->crl2);
+
+		reg_mecr = flexcan_read(&regs->mecr);
+		reg_mecr &= ~FLEXCAN_MECR_ECRWRDIS;
+		flexcan_write(reg_mecr, &regs->mecr);
+		reg_mecr &= ~(FLEXCAN_MECR_NCEFAFRZ | FLEXCAN_MECR_HANCEI_MSK |
+				FLEXCAN_MECR_FANCEI_MSK);
+		flexcan_write(reg_mecr, &regs->mecr);
+	}
+
 	err = flexcan_transceiver_enable(priv);
 	if (err)
 		goto out_chip_disable;
@@ -1080,6 +1207,7 @@ static const struct of_device_id flexcan_of_match[] = {
 	{ .compatible = "fsl,imx6q-flexcan", .data = &fsl_imx6q_devtype_data, },
 	{ .compatible = "fsl,imx28-flexcan", .data = &fsl_imx28_devtype_data, },
 	{ .compatible = "fsl,p1010-flexcan", .data = &fsl_p1010_devtype_data, },
+	{ .compatible = "fsl,vf610-flexcan", .data = &fsl_vf610_devtype_data, },
 	{ /* sentinel */ },
 };
 MODULE_DEVICE_TABLE(of, flexcan_of_match);
diff --git a/drivers/net/can/m_can/Kconfig b/drivers/net/can/m_can/Kconfig
new file mode 100644
index 000000000000..fca5482c09ac
--- /dev/null
+++ b/drivers/net/can/m_can/Kconfig
@@ -0,0 +1,4 @@
+config CAN_M_CAN
+	tristate "Bosch M_CAN devices"
+	---help---
+	  Say Y here if you want to support for Bosch M_CAN controller.
diff --git a/drivers/net/can/m_can/Makefile b/drivers/net/can/m_can/Makefile
new file mode 100644
index 000000000000..8bbd7f24f5be
--- /dev/null
+++ b/drivers/net/can/m_can/Makefile
@@ -0,0 +1,5 @@
+#
+#  Makefile for the Bosch M_CAN controller driver.
+#
+
+obj-$(CONFIG_CAN_M_CAN) += m_can.o
diff --git a/drivers/net/can/m_can/m_can.c b/drivers/net/can/m_can/m_can.c
new file mode 100644
index 000000000000..10d571eaed85
--- /dev/null
+++ b/drivers/net/can/m_can/m_can.c
@@ -0,0 +1,1202 @@
+/*
+ * CAN bus driver for Bosch M_CAN controller
+ *
+ * Copyright (C) 2014 Freescale Semiconductor, Inc.
+ *	Dong Aisheng <b29396@freescale.com>
+ *
+ * Bosch M_CAN user manual can be obtained from:
+ * http://www.bosch-semiconductors.de/media/pdf_1/ipmodules_1/m_can/
+ * mcan_users_manual_v302.pdf
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2. This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+
+#include <linux/can/dev.h>
+
+/* napi related */
+#define M_CAN_NAPI_WEIGHT	64
+
+/* message ram configuration data length */
+#define MRAM_CFG_LEN	8
+
+/* registers definition */
+enum m_can_reg {
+	M_CAN_CREL	= 0x0,
+	M_CAN_ENDN	= 0x4,
+	M_CAN_CUST	= 0x8,
+	M_CAN_FBTP	= 0xc,
+	M_CAN_TEST	= 0x10,
+	M_CAN_RWD	= 0x14,
+	M_CAN_CCCR	= 0x18,
+	M_CAN_BTP	= 0x1c,
+	M_CAN_TSCC	= 0x20,
+	M_CAN_TSCV	= 0x24,
+	M_CAN_TOCC	= 0x28,
+	M_CAN_TOCV	= 0x2c,
+	M_CAN_ECR	= 0x40,
+	M_CAN_PSR	= 0x44,
+	M_CAN_IR	= 0x50,
+	M_CAN_IE	= 0x54,
+	M_CAN_ILS	= 0x58,
+	M_CAN_ILE	= 0x5c,
+	M_CAN_GFC	= 0x80,
+	M_CAN_SIDFC	= 0x84,
+	M_CAN_XIDFC	= 0x88,
+	M_CAN_XIDAM	= 0x90,
+	M_CAN_HPMS	= 0x94,
+	M_CAN_NDAT1	= 0x98,
+	M_CAN_NDAT2	= 0x9c,
+	M_CAN_RXF0C	= 0xa0,
+	M_CAN_RXF0S	= 0xa4,
+	M_CAN_RXF0A	= 0xa8,
+	M_CAN_RXBC	= 0xac,
+	M_CAN_RXF1C	= 0xb0,
+	M_CAN_RXF1S	= 0xb4,
+	M_CAN_RXF1A	= 0xb8,
+	M_CAN_RXESC	= 0xbc,
+	M_CAN_TXBC	= 0xc0,
+	M_CAN_TXFQS	= 0xc4,
+	M_CAN_TXESC	= 0xc8,
+	M_CAN_TXBRP	= 0xcc,
+	M_CAN_TXBAR	= 0xd0,
+	M_CAN_TXBCR	= 0xd4,
+	M_CAN_TXBTO	= 0xd8,
+	M_CAN_TXBCF	= 0xdc,
+	M_CAN_TXBTIE	= 0xe0,
+	M_CAN_TXBCIE	= 0xe4,
+	M_CAN_TXEFC	= 0xf0,
+	M_CAN_TXEFS	= 0xf4,
+	M_CAN_TXEFA	= 0xf8,
+};
+
+/* m_can lec values */
+enum m_can_lec_type {
+	LEC_NO_ERROR = 0,
+	LEC_STUFF_ERROR,
+	LEC_FORM_ERROR,
+	LEC_ACK_ERROR,
+	LEC_BIT1_ERROR,
+	LEC_BIT0_ERROR,
+	LEC_CRC_ERROR,
+	LEC_UNUSED,
+};
+
+enum m_can_mram_cfg {
+	MRAM_SIDF = 0,
+	MRAM_XIDF,
+	MRAM_RXF0,
+	MRAM_RXF1,
+	MRAM_RXB,
+	MRAM_TXE,
+	MRAM_TXB,
+	MRAM_CFG_NUM,
+};
+
+/* Test Register (TEST) */
+#define TEST_LBCK	BIT(4)
+
+/* CC Control Register(CCCR) */
+#define CCCR_TEST	BIT(7)
+#define CCCR_MON	BIT(5)
+#define CCCR_CCE	BIT(1)
+#define CCCR_INIT	BIT(0)
+
+/* Bit Timing & Prescaler Register (BTP) */
+#define BTR_BRP_MASK		0x3ff
+#define BTR_BRP_SHIFT		16
+#define BTR_TSEG1_SHIFT		8
+#define BTR_TSEG1_MASK		(0x3f << BTR_TSEG1_SHIFT)
+#define BTR_TSEG2_SHIFT		4
+#define BTR_TSEG2_MASK		(0xf << BTR_TSEG2_SHIFT)
+#define BTR_SJW_SHIFT		0
+#define BTR_SJW_MASK		0xf
+
+/* Error Counter Register(ECR) */
+#define ECR_RP			BIT(15)
+#define ECR_REC_SHIFT		8
+#define ECR_REC_MASK		(0x7f << ECR_REC_SHIFT)
+#define ECR_TEC_SHIFT		0
+#define ECR_TEC_MASK		0xff
+
+/* Protocol Status Register(PSR) */
+#define PSR_BO		BIT(7)
+#define PSR_EW		BIT(6)
+#define PSR_EP		BIT(5)
+#define PSR_LEC_MASK	0x7
+
+/* Interrupt Register(IR) */
+#define IR_ALL_INT	0xffffffff
+#define IR_STE		BIT(31)
+#define IR_FOE		BIT(30)
+#define IR_ACKE		BIT(29)
+#define IR_BE		BIT(28)
+#define IR_CRCE		BIT(27)
+#define IR_WDI		BIT(26)
+#define IR_BO		BIT(25)
+#define IR_EW		BIT(24)
+#define IR_EP		BIT(23)
+#define IR_ELO		BIT(22)
+#define IR_BEU		BIT(21)
+#define IR_BEC		BIT(20)
+#define IR_DRX		BIT(19)
+#define IR_TOO		BIT(18)
+#define IR_MRAF		BIT(17)
+#define IR_TSW		BIT(16)
+#define IR_TEFL		BIT(15)
+#define IR_TEFF		BIT(14)
+#define IR_TEFW		BIT(13)
+#define IR_TEFN		BIT(12)
+#define IR_TFE		BIT(11)
+#define IR_TCF		BIT(10)
+#define IR_TC		BIT(9)
+#define IR_HPM		BIT(8)
+#define IR_RF1L		BIT(7)
+#define IR_RF1F		BIT(6)
+#define IR_RF1W		BIT(5)
+#define IR_RF1N		BIT(4)
+#define IR_RF0L		BIT(3)
+#define IR_RF0F		BIT(2)
+#define IR_RF0W		BIT(1)
+#define IR_RF0N		BIT(0)
+#define IR_ERR_STATE	(IR_BO | IR_EW | IR_EP)
+#define IR_ERR_LEC	(IR_STE	| IR_FOE | IR_ACKE | IR_BE | IR_CRCE)
+#define IR_ERR_BUS	(IR_ERR_LEC | IR_WDI | IR_ELO | IR_BEU | \
+			 IR_BEC | IR_TOO | IR_MRAF | IR_TSW | IR_TEFL | \
+			 IR_RF1L | IR_RF0L)
+#define IR_ERR_ALL	(IR_ERR_STATE | IR_ERR_BUS)
+
+/* Interrupt Line Select (ILS) */
+#define ILS_ALL_INT0	0x0
+#define ILS_ALL_INT1	0xFFFFFFFF
+
+/* Interrupt Line Enable (ILE) */
+#define ILE_EINT0	BIT(0)
+#define ILE_EINT1	BIT(1)
+
+/* Rx FIFO 0/1 Configuration (RXF0C/RXF1C) */
+#define RXFC_FWM_OFF	24
+#define RXFC_FWM_MASK	0x7f
+#define RXFC_FWM_1	(1 << RXFC_FWM_OFF)
+#define RXFC_FS_OFF	16
+#define RXFC_FS_MASK	0x7f
+
+/* Rx FIFO 0/1 Status (RXF0S/RXF1S) */
+#define RXFS_RFL	BIT(25)
+#define RXFS_FF		BIT(24)
+#define RXFS_FPI_OFF	16
+#define RXFS_FPI_MASK	0x3f0000
+#define RXFS_FGI_OFF	8
+#define RXFS_FGI_MASK	0x3f00
+#define RXFS_FFL_MASK	0x7f
+
+/* Rx Buffer / FIFO Element Size Configuration (RXESC) */
+#define M_CAN_RXESC_8BYTES	0x0
+
+/* Tx Buffer Configuration(TXBC) */
+#define TXBC_NDTB_OFF		16
+#define TXBC_NDTB_MASK		0x3f
+
+/* Tx Buffer Element Size Configuration(TXESC) */
+#define TXESC_TBDS_8BYTES	0x0
+
+/* Tx Event FIFO Con.guration (TXEFC) */
+#define TXEFC_EFS_OFF		16
+#define TXEFC_EFS_MASK		0x3f
+
+/* Message RAM Configuration (in bytes) */
+#define SIDF_ELEMENT_SIZE	4
+#define XIDF_ELEMENT_SIZE	8
+#define RXF0_ELEMENT_SIZE	16
+#define RXF1_ELEMENT_SIZE	16
+#define RXB_ELEMENT_SIZE	16
+#define TXE_ELEMENT_SIZE	8
+#define TXB_ELEMENT_SIZE	16
+
+/* Message RAM Elements */
+#define M_CAN_FIFO_ID		0x0
+#define M_CAN_FIFO_DLC		0x4
+#define M_CAN_FIFO_DATA(n)	(0x8 + ((n) << 2))
+
+/* Rx Buffer Element */
+#define RX_BUF_ESI		BIT(31)
+#define RX_BUF_XTD		BIT(30)
+#define RX_BUF_RTR		BIT(29)
+
+/* Tx Buffer Element */
+#define TX_BUF_XTD		BIT(30)
+#define TX_BUF_RTR		BIT(29)
+
+/* address offset and element number for each FIFO/Buffer in the Message RAM */
+struct mram_cfg {
+	u16 off;
+	u8  num;
+};
+
+/* m_can private data structure */
+struct m_can_priv {
+	struct can_priv can;	/* must be the first member */
+	struct napi_struct napi;
+	struct net_device *dev;
+	struct device *device;
+	struct clk *hclk;
+	struct clk *cclk;
+	void __iomem *base;
+	u32 irqstatus;
+
+	/* message ram configuration */
+	void __iomem *mram_base;
+	struct mram_cfg mcfg[MRAM_CFG_NUM];
+};
+
+static inline u32 m_can_read(const struct m_can_priv *priv, enum m_can_reg reg)
+{
+	return readl(priv->base + reg);
+}
+
+static inline void m_can_write(const struct m_can_priv *priv,
+			       enum m_can_reg reg, u32 val)
+{
+	writel(val, priv->base + reg);
+}
+
+static inline u32 m_can_fifo_read(const struct m_can_priv *priv,
+				  u32 fgi, unsigned int offset)
+{
+	return readl(priv->mram_base + priv->mcfg[MRAM_RXF0].off +
+		     fgi * RXF0_ELEMENT_SIZE + offset);
+}
+
+static inline void m_can_fifo_write(const struct m_can_priv *priv,
+				    u32 fpi, unsigned int offset, u32 val)
+{
+	return writel(val, priv->mram_base + priv->mcfg[MRAM_TXB].off +
+		      fpi * TXB_ELEMENT_SIZE + offset);
+}
+
+static inline void m_can_config_endisable(const struct m_can_priv *priv,
+					  bool enable)
+{
+	u32 cccr = m_can_read(priv, M_CAN_CCCR);
+	u32 timeout = 10;
+	u32 val = 0;
+
+	if (enable) {
+		/* enable m_can configuration */
+		m_can_write(priv, M_CAN_CCCR, cccr | CCCR_INIT);
+		/* CCCR.CCE can only be set/reset while CCCR.INIT = '1' */
+		m_can_write(priv, M_CAN_CCCR, cccr | CCCR_INIT | CCCR_CCE);
+	} else {
+		m_can_write(priv, M_CAN_CCCR, cccr & ~(CCCR_INIT | CCCR_CCE));
+	}
+
+	/* there's a delay for module initialization */
+	if (enable)
+		val = CCCR_INIT | CCCR_CCE;
+
+	while ((m_can_read(priv, M_CAN_CCCR) & (CCCR_INIT | CCCR_CCE)) != val) {
+		if (timeout == 0) {
+			netdev_warn(priv->dev, "Failed to init module\n");
+			return;
+		}
+		timeout--;
+		udelay(1);
+	}
+}
+
+static inline void m_can_enable_all_interrupts(const struct m_can_priv *priv)
+{
+	m_can_write(priv, M_CAN_ILE, ILE_EINT0 | ILE_EINT1);
+}
+
+static inline void m_can_disable_all_interrupts(const struct m_can_priv *priv)
+{
+	m_can_write(priv, M_CAN_ILE, 0x0);
+}
+
+static void m_can_read_fifo(const struct net_device *dev, struct can_frame *cf,
+			    u32 rxfs)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	u32 id, fgi;
+
+	/* calculate the fifo get index for where to read data */
+	fgi = (rxfs & RXFS_FGI_MASK) >> RXFS_FGI_OFF;
+	id = m_can_fifo_read(priv, fgi, M_CAN_FIFO_ID);
+	if (id & RX_BUF_XTD)
+		cf->can_id = (id & CAN_EFF_MASK) | CAN_EFF_FLAG;
+	else
+		cf->can_id = (id >> 18) & CAN_SFF_MASK;
+
+	if (id & RX_BUF_RTR) {
+		cf->can_id |= CAN_RTR_FLAG;
+	} else {
+		id = m_can_fifo_read(priv, fgi, M_CAN_FIFO_DLC);
+		cf->can_dlc = get_can_dlc((id >> 16) & 0x0F);
+		*(u32 *)(cf->data + 0) = m_can_fifo_read(priv, fgi,
+							 M_CAN_FIFO_DATA(0));
+		*(u32 *)(cf->data + 4) = m_can_fifo_read(priv, fgi,
+							 M_CAN_FIFO_DATA(1));
+	}
+
+	/* acknowledge rx fifo 0 */
+	m_can_write(priv, M_CAN_RXF0A, fgi);
+}
+
+static int m_can_do_rx_poll(struct net_device *dev, int quota)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+	u32 pkts = 0;
+	u32 rxfs;
+
+	rxfs = m_can_read(priv, M_CAN_RXF0S);
+	if (!(rxfs & RXFS_FFL_MASK)) {
+		netdev_dbg(dev, "no messages in fifo0\n");
+		return 0;
+	}
+
+	while ((rxfs & RXFS_FFL_MASK) && (quota > 0)) {
+		if (rxfs & RXFS_RFL)
+			netdev_warn(dev, "Rx FIFO 0 Message Lost\n");
+
+		skb = alloc_can_skb(dev, &frame);
+		if (!skb) {
+			stats->rx_dropped++;
+			return pkts;
+		}
+
+		m_can_read_fifo(dev, frame, rxfs);
+
+		stats->rx_packets++;
+		stats->rx_bytes += frame->can_dlc;
+
+		netif_receive_skb(skb);
+
+		quota--;
+		pkts++;
+		rxfs = m_can_read(priv, M_CAN_RXF0S);
+	}
+
+	if (pkts)
+		can_led_event(dev, CAN_LED_EVENT_RX);
+
+	return pkts;
+}
+
+static int m_can_handle_lost_msg(struct net_device *dev)
+{
+	struct net_device_stats *stats = &dev->stats;
+	struct sk_buff *skb;
+	struct can_frame *frame;
+
+	netdev_err(dev, "msg lost in rxf0\n");
+
+	stats->rx_errors++;
+	stats->rx_over_errors++;
+
+	skb = alloc_can_err_skb(dev, &frame);
+	if (unlikely(!skb))
+		return 0;
+
+	frame->can_id |= CAN_ERR_CRTL;
+	frame->data[1] = CAN_ERR_CRTL_RX_OVERFLOW;
+
+	netif_receive_skb(skb);
+
+	return 1;
+}
+
+static int m_can_handle_lec_err(struct net_device *dev,
+				enum m_can_lec_type lec_type)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf;
+	struct sk_buff *skb;
+
+	priv->can.can_stats.bus_error++;
+	stats->rx_errors++;
+
+	/* propagate the error condition to the CAN stack */
+	skb = alloc_can_err_skb(dev, &cf);
+	if (unlikely(!skb))
+		return 0;
+
+	/* check for 'last error code' which tells us the
+	 * type of the last error to occur on the CAN bus
+	 */
+	cf->can_id |= CAN_ERR_PROT | CAN_ERR_BUSERROR;
+	cf->data[2] |= CAN_ERR_PROT_UNSPEC;
+
+	switch (lec_type) {
+	case LEC_STUFF_ERROR:
+		netdev_dbg(dev, "stuff error\n");
+		cf->data[2] |= CAN_ERR_PROT_STUFF;
+		break;
+	case LEC_FORM_ERROR:
+		netdev_dbg(dev, "form error\n");
+		cf->data[2] |= CAN_ERR_PROT_FORM;
+		break;
+	case LEC_ACK_ERROR:
+		netdev_dbg(dev, "ack error\n");
+		cf->data[3] |= (CAN_ERR_PROT_LOC_ACK |
+				CAN_ERR_PROT_LOC_ACK_DEL);
+		break;
+	case LEC_BIT1_ERROR:
+		netdev_dbg(dev, "bit1 error\n");
+		cf->data[2] |= CAN_ERR_PROT_BIT1;
+		break;
+	case LEC_BIT0_ERROR:
+		netdev_dbg(dev, "bit0 error\n");
+		cf->data[2] |= CAN_ERR_PROT_BIT0;
+		break;
+	case LEC_CRC_ERROR:
+		netdev_dbg(dev, "CRC error\n");
+		cf->data[3] |= (CAN_ERR_PROT_LOC_CRC_SEQ |
+				CAN_ERR_PROT_LOC_CRC_DEL);
+		break;
+	default:
+		break;
+	}
+
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+	netif_receive_skb(skb);
+
+	return 1;
+}
+
+static int m_can_get_berr_counter(const struct net_device *dev,
+				  struct can_berr_counter *bec)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	unsigned int ecr;
+	int err;
+
+	err = clk_prepare_enable(priv->hclk);
+	if (err)
+		return err;
+
+	err = clk_prepare_enable(priv->cclk);
+	if (err) {
+		clk_disable_unprepare(priv->hclk);
+		return err;
+	}
+
+	ecr = m_can_read(priv, M_CAN_ECR);
+	bec->rxerr = (ecr & ECR_REC_MASK) >> ECR_REC_SHIFT;
+	bec->txerr = ecr & ECR_TEC_MASK;
+
+	clk_disable_unprepare(priv->cclk);
+	clk_disable_unprepare(priv->hclk);
+
+	return 0;
+}
+
+static int m_can_handle_state_change(struct net_device *dev,
+				     enum can_state new_state)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	struct can_frame *cf;
+	struct sk_buff *skb;
+	struct can_berr_counter bec;
+	unsigned int ecr;
+
+	switch (new_state) {
+	case CAN_STATE_ERROR_ACTIVE:
+		/* error warning state */
+		priv->can.can_stats.error_warning++;
+		priv->can.state = CAN_STATE_ERROR_WARNING;
+		break;
+	case CAN_STATE_ERROR_PASSIVE:
+		/* error passive state */
+		priv->can.can_stats.error_passive++;
+		priv->can.state = CAN_STATE_ERROR_PASSIVE;
+		break;
+	case CAN_STATE_BUS_OFF:
+		/* bus-off state */
+		priv->can.state = CAN_STATE_BUS_OFF;
+		m_can_disable_all_interrupts(priv);
+		can_bus_off(dev);
+		break;
+	default:
+		break;
+	}
+
+	/* propagate the error condition to the CAN stack */
+	skb = alloc_can_err_skb(dev, &cf);
+	if (unlikely(!skb))
+		return 0;
+
+	m_can_get_berr_counter(dev, &bec);
+
+	switch (new_state) {
+	case CAN_STATE_ERROR_ACTIVE:
+		/* error warning state */
+		cf->can_id |= CAN_ERR_CRTL;
+		cf->data[1] = (bec.txerr > bec.rxerr) ?
+			CAN_ERR_CRTL_TX_WARNING :
+			CAN_ERR_CRTL_RX_WARNING;
+		cf->data[6] = bec.txerr;
+		cf->data[7] = bec.rxerr;
+		break;
+	case CAN_STATE_ERROR_PASSIVE:
+		/* error passive state */
+		cf->can_id |= CAN_ERR_CRTL;
+		ecr = m_can_read(priv, M_CAN_ECR);
+		if (ecr & ECR_RP)
+			cf->data[1] |= CAN_ERR_CRTL_RX_PASSIVE;
+		if (bec.txerr > 127)
+			cf->data[1] |= CAN_ERR_CRTL_TX_PASSIVE;
+		cf->data[6] = bec.txerr;
+		cf->data[7] = bec.rxerr;
+		break;
+	case CAN_STATE_BUS_OFF:
+		/* bus-off state */
+		cf->can_id |= CAN_ERR_BUSOFF;
+		break;
+	default:
+		break;
+	}
+
+	stats->rx_packets++;
+	stats->rx_bytes += cf->can_dlc;
+	netif_receive_skb(skb);
+
+	return 1;
+}
+
+static int m_can_handle_state_errors(struct net_device *dev, u32 psr)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	int work_done = 0;
+
+	if ((psr & PSR_EW) &&
+	    (priv->can.state != CAN_STATE_ERROR_WARNING)) {
+		netdev_dbg(dev, "entered error warning state\n");
+		work_done += m_can_handle_state_change(dev,
+						       CAN_STATE_ERROR_WARNING);
+	}
+
+	if ((psr & PSR_EP) &&
+	    (priv->can.state != CAN_STATE_ERROR_PASSIVE)) {
+		netdev_dbg(dev, "entered error warning state\n");
+		work_done += m_can_handle_state_change(dev,
+						       CAN_STATE_ERROR_PASSIVE);
+	}
+
+	if ((psr & PSR_BO) &&
+	    (priv->can.state != CAN_STATE_BUS_OFF)) {
+		netdev_dbg(dev, "entered error warning state\n");
+		work_done += m_can_handle_state_change(dev,
+						       CAN_STATE_BUS_OFF);
+	}
+
+	return work_done;
+}
+
+static void m_can_handle_other_err(struct net_device *dev, u32 irqstatus)
+{
+	if (irqstatus & IR_WDI)
+		netdev_err(dev, "Message RAM Watchdog event due to missing READY\n");
+	if (irqstatus & IR_BEU)
+		netdev_err(dev, "Error Logging Overflow\n");
+	if (irqstatus & IR_BEU)
+		netdev_err(dev, "Bit Error Uncorrected\n");
+	if (irqstatus & IR_BEC)
+		netdev_err(dev, "Bit Error Corrected\n");
+	if (irqstatus & IR_TOO)
+		netdev_err(dev, "Timeout reached\n");
+	if (irqstatus & IR_MRAF)
+		netdev_err(dev, "Message RAM access failure occurred\n");
+}
+
+static inline bool is_lec_err(u32 psr)
+{
+	psr &= LEC_UNUSED;
+
+	return psr && (psr != LEC_UNUSED);
+}
+
+static int m_can_handle_bus_errors(struct net_device *dev, u32 irqstatus,
+				   u32 psr)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	int work_done = 0;
+
+	if (irqstatus & IR_RF0L)
+		work_done += m_can_handle_lost_msg(dev);
+
+	/* handle lec errors on the bus */
+	if ((priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING) &&
+	    is_lec_err(psr))
+		work_done += m_can_handle_lec_err(dev, psr & LEC_UNUSED);
+
+	/* other unproccessed error interrupts */
+	m_can_handle_other_err(dev, irqstatus);
+
+	return work_done;
+}
+
+static int m_can_poll(struct napi_struct *napi, int quota)
+{
+	struct net_device *dev = napi->dev;
+	struct m_can_priv *priv = netdev_priv(dev);
+	int work_done = 0;
+	u32 irqstatus, psr;
+
+	irqstatus = priv->irqstatus | m_can_read(priv, M_CAN_IR);
+	if (!irqstatus)
+		goto end;
+
+	psr = m_can_read(priv, M_CAN_PSR);
+	if (irqstatus & IR_ERR_STATE)
+		work_done += m_can_handle_state_errors(dev, psr);
+
+	if (irqstatus & IR_ERR_BUS)
+		work_done += m_can_handle_bus_errors(dev, irqstatus, psr);
+
+	if (irqstatus & IR_RF0N)
+		work_done += m_can_do_rx_poll(dev, (quota - work_done));
+
+	if (work_done < quota) {
+		napi_complete(napi);
+		m_can_enable_all_interrupts(priv);
+	}
+
+end:
+	return work_done;
+}
+
+static irqreturn_t m_can_isr(int irq, void *dev_id)
+{
+	struct net_device *dev = (struct net_device *)dev_id;
+	struct m_can_priv *priv = netdev_priv(dev);
+	struct net_device_stats *stats = &dev->stats;
+	u32 ir;
+
+	ir = m_can_read(priv, M_CAN_IR);
+	if (!ir)
+		return IRQ_NONE;
+
+	/* ACK all irqs */
+	if (ir & IR_ALL_INT)
+		m_can_write(priv, M_CAN_IR, ir);
+
+	/* schedule NAPI in case of
+	 * - rx IRQ
+	 * - state change IRQ
+	 * - bus error IRQ and bus error reporting
+	 */
+	if ((ir & IR_RF0N) || (ir & IR_ERR_ALL)) {
+		priv->irqstatus = ir;
+		m_can_disable_all_interrupts(priv);
+		napi_schedule(&priv->napi);
+	}
+
+	/* transmission complete interrupt */
+	if (ir & IR_TC) {
+		stats->tx_bytes += can_get_echo_skb(dev, 0);
+		stats->tx_packets++;
+		can_led_event(dev, CAN_LED_EVENT_TX);
+		netif_wake_queue(dev);
+	}
+
+	return IRQ_HANDLED;
+}
+
+static const struct can_bittiming_const m_can_bittiming_const = {
+	.name = KBUILD_MODNAME,
+	.tseg1_min = 2,		/* Time segment 1 = prop_seg + phase_seg1 */
+	.tseg1_max = 64,
+	.tseg2_min = 1,		/* Time segment 2 = phase_seg2 */
+	.tseg2_max = 16,
+	.sjw_max = 16,
+	.brp_min = 1,
+	.brp_max = 1024,
+	.brp_inc = 1,
+};
+
+static int m_can_set_bittiming(struct net_device *dev)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	const struct can_bittiming *bt = &priv->can.bittiming;
+	u16 brp, sjw, tseg1, tseg2;
+	u32 reg_btp;
+
+	brp = bt->brp - 1;
+	sjw = bt->sjw - 1;
+	tseg1 = bt->prop_seg + bt->phase_seg1 - 1;
+	tseg2 = bt->phase_seg2 - 1;
+	reg_btp = (brp << BTR_BRP_SHIFT) | (sjw << BTR_SJW_SHIFT) |
+			(tseg1 << BTR_TSEG1_SHIFT) | (tseg2 << BTR_TSEG2_SHIFT);
+	m_can_write(priv, M_CAN_BTP, reg_btp);
+	netdev_dbg(dev, "setting BTP 0x%x\n", reg_btp);
+
+	return 0;
+}
+
+/* Configure M_CAN chip:
+ * - set rx buffer/fifo element size
+ * - configure rx fifo
+ * - accept non-matching frame into fifo 0
+ * - configure tx buffer
+ * - configure mode
+ * - setup bittiming
+ */
+static void m_can_chip_config(struct net_device *dev)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	u32 cccr, test;
+
+	m_can_config_endisable(priv, true);
+
+	/* RX Buffer/FIFO Element Size 8 bytes data field */
+	m_can_write(priv, M_CAN_RXESC, M_CAN_RXESC_8BYTES);
+
+	/* Accept Non-matching Frames Into FIFO 0 */
+	m_can_write(priv, M_CAN_GFC, 0x0);
+
+	/* only support one Tx Buffer currently */
+	m_can_write(priv, M_CAN_TXBC, (1 << TXBC_NDTB_OFF) |
+		    priv->mcfg[MRAM_TXB].off);
+
+	/* only support 8 bytes firstly */
+	m_can_write(priv, M_CAN_TXESC, TXESC_TBDS_8BYTES);
+
+	m_can_write(priv, M_CAN_TXEFC, (1 << TXEFC_EFS_OFF) |
+		    priv->mcfg[MRAM_TXE].off);
+
+	/* rx fifo configuration, blocking mode, fifo size 1 */
+	m_can_write(priv, M_CAN_RXF0C,
+		    (priv->mcfg[MRAM_RXF0].num << RXFC_FS_OFF) |
+		    RXFC_FWM_1 | priv->mcfg[MRAM_RXF0].off);
+
+	m_can_write(priv, M_CAN_RXF1C,
+		    (priv->mcfg[MRAM_RXF1].num << RXFC_FS_OFF) |
+		    RXFC_FWM_1 | priv->mcfg[MRAM_RXF1].off);
+
+	cccr = m_can_read(priv, M_CAN_CCCR);
+	cccr &= ~(CCCR_TEST | CCCR_MON);
+	test = m_can_read(priv, M_CAN_TEST);
+	test &= ~TEST_LBCK;
+
+	if (priv->can.ctrlmode & CAN_CTRLMODE_LISTENONLY)
+		cccr |= CCCR_MON;
+
+	if (priv->can.ctrlmode & CAN_CTRLMODE_LOOPBACK) {
+		cccr |= CCCR_TEST;
+		test |= TEST_LBCK;
+	}
+
+	m_can_write(priv, M_CAN_CCCR, cccr);
+	m_can_write(priv, M_CAN_TEST, test);
+
+	/* enable interrupts */
+	m_can_write(priv, M_CAN_IR, IR_ALL_INT);
+	if (!(priv->can.ctrlmode & CAN_CTRLMODE_BERR_REPORTING))
+		m_can_write(priv, M_CAN_IE, IR_ALL_INT & ~IR_ERR_LEC);
+	else
+		m_can_write(priv, M_CAN_IE, IR_ALL_INT);
+
+	/* route all interrupts to INT0 */
+	m_can_write(priv, M_CAN_ILS, ILS_ALL_INT0);
+
+	/* set bittiming params */
+	m_can_set_bittiming(dev);
+
+	m_can_config_endisable(priv, false);
+}
+
+static void m_can_start(struct net_device *dev)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+
+	/* basic m_can configuration */
+	m_can_chip_config(dev);
+
+	priv->can.state = CAN_STATE_ERROR_ACTIVE;
+
+	m_can_enable_all_interrupts(priv);
+}
+
+static int m_can_set_mode(struct net_device *dev, enum can_mode mode)
+{
+	switch (mode) {
+	case CAN_MODE_START:
+		m_can_start(dev);
+		netif_wake_queue(dev);
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static void free_m_can_dev(struct net_device *dev)
+{
+	free_candev(dev);
+}
+
+static struct net_device *alloc_m_can_dev(void)
+{
+	struct net_device *dev;
+	struct m_can_priv *priv;
+
+	dev = alloc_candev(sizeof(*priv), 1);
+	if (!dev)
+		return NULL;
+
+	priv = netdev_priv(dev);
+	netif_napi_add(dev, &priv->napi, m_can_poll, M_CAN_NAPI_WEIGHT);
+
+	priv->dev = dev;
+	priv->can.bittiming_const = &m_can_bittiming_const;
+	priv->can.do_set_mode = m_can_set_mode;
+	priv->can.do_get_berr_counter = m_can_get_berr_counter;
+	priv->can.ctrlmode_supported = CAN_CTRLMODE_LOOPBACK |
+					CAN_CTRLMODE_LISTENONLY |
+					CAN_CTRLMODE_BERR_REPORTING;
+
+	return dev;
+}
+
+static int m_can_open(struct net_device *dev)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	int err;
+
+	err = clk_prepare_enable(priv->hclk);
+	if (err)
+		return err;
+
+	err = clk_prepare_enable(priv->cclk);
+	if (err)
+		goto exit_disable_hclk;
+
+	/* open the can device */
+	err = open_candev(dev);
+	if (err) {
+		netdev_err(dev, "failed to open can device\n");
+		goto exit_disable_cclk;
+	}
+
+	/* register interrupt handler */
+	err = request_irq(dev->irq, m_can_isr, IRQF_SHARED, dev->name,
+			  dev);
+	if (err < 0) {
+		netdev_err(dev, "failed to request interrupt\n");
+		goto exit_irq_fail;
+	}
+
+	/* start the m_can controller */
+	m_can_start(dev);
+
+	can_led_event(dev, CAN_LED_EVENT_OPEN);
+	napi_enable(&priv->napi);
+	netif_start_queue(dev);
+
+	return 0;
+
+exit_irq_fail:
+	close_candev(dev);
+exit_disable_cclk:
+	clk_disable_unprepare(priv->cclk);
+exit_disable_hclk:
+	clk_disable_unprepare(priv->hclk);
+	return err;
+}
+
+static void m_can_stop(struct net_device *dev)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+
+	/* disable all interrupts */
+	m_can_disable_all_interrupts(priv);
+
+	clk_disable_unprepare(priv->hclk);
+	clk_disable_unprepare(priv->cclk);
+
+	/* set the state as STOPPED */
+	priv->can.state = CAN_STATE_STOPPED;
+}
+
+static int m_can_close(struct net_device *dev)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+
+	netif_stop_queue(dev);
+	napi_disable(&priv->napi);
+	m_can_stop(dev);
+	free_irq(dev->irq, dev);
+	close_candev(dev);
+	can_led_event(dev, CAN_LED_EVENT_STOP);
+
+	return 0;
+}
+
+static netdev_tx_t m_can_start_xmit(struct sk_buff *skb,
+				    struct net_device *dev)
+{
+	struct m_can_priv *priv = netdev_priv(dev);
+	struct can_frame *cf = (struct can_frame *)skb->data;
+	u32 id;
+
+	if (can_dropped_invalid_skb(dev, skb))
+		return NETDEV_TX_OK;
+
+	netif_stop_queue(dev);
+
+	if (cf->can_id & CAN_EFF_FLAG) {
+		id = cf->can_id & CAN_EFF_MASK;
+		id |= TX_BUF_XTD;
+	} else {
+		id = ((cf->can_id & CAN_SFF_MASK) << 18);
+	}
+
+	if (cf->can_id & CAN_RTR_FLAG)
+		id |= TX_BUF_RTR;
+
+	/* message ram configuration */
+	m_can_fifo_write(priv, 0, M_CAN_FIFO_ID, id);
+	m_can_fifo_write(priv, 0, M_CAN_FIFO_DLC, cf->can_dlc << 16);
+	m_can_fifo_write(priv, 0, M_CAN_FIFO_DATA(0), *(u32 *)(cf->data + 0));
+	m_can_fifo_write(priv, 0, M_CAN_FIFO_DATA(1), *(u32 *)(cf->data + 4));
+	can_put_echo_skb(skb, dev, 0);
+
+	/* enable first TX buffer to start transfer  */
+	m_can_write(priv, M_CAN_TXBTIE, 0x1);
+	m_can_write(priv, M_CAN_TXBAR, 0x1);
+
+	return NETDEV_TX_OK;
+}
+
+static const struct net_device_ops m_can_netdev_ops = {
+	.ndo_open = m_can_open,
+	.ndo_stop = m_can_close,
+	.ndo_start_xmit = m_can_start_xmit,
+};
+
+static int register_m_can_dev(struct net_device *dev)
+{
+	dev->flags |= IFF_ECHO;	/* we support local echo */
+	dev->netdev_ops = &m_can_netdev_ops;
+
+	return register_candev(dev);
+}
+
+static int m_can_of_parse_mram(struct platform_device *pdev,
+			       struct m_can_priv *priv)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct resource *res;
+	void __iomem *addr;
+	u32 out_val[MRAM_CFG_LEN];
+	int ret;
+
+	/* message ram could be shared */
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "message_ram");
+	if (!res)
+		return -ENODEV;
+
+	addr = devm_ioremap(&pdev->dev, res->start, resource_size(res));
+	if (!addr)
+		return -ENOMEM;
+
+	/* get message ram configuration */
+	ret = of_property_read_u32_array(np, "bosch,mram-cfg",
+					 out_val, sizeof(out_val) / 4);
+	if (ret) {
+		dev_err(&pdev->dev, "can not get message ram configuration\n");
+		return -ENODEV;
+	}
+
+	priv->mram_base = addr;
+	priv->mcfg[MRAM_SIDF].off = out_val[0];
+	priv->mcfg[MRAM_SIDF].num = out_val[1];
+	priv->mcfg[MRAM_XIDF].off = priv->mcfg[MRAM_SIDF].off +
+			priv->mcfg[MRAM_SIDF].num * SIDF_ELEMENT_SIZE;
+	priv->mcfg[MRAM_XIDF].num = out_val[2];
+	priv->mcfg[MRAM_RXF0].off = priv->mcfg[MRAM_XIDF].off +
+			priv->mcfg[MRAM_XIDF].num * XIDF_ELEMENT_SIZE;
+	priv->mcfg[MRAM_RXF0].num = out_val[3] & RXFC_FS_MASK;
+	priv->mcfg[MRAM_RXF1].off = priv->mcfg[MRAM_RXF0].off +
+			priv->mcfg[MRAM_RXF0].num * RXF0_ELEMENT_SIZE;
+	priv->mcfg[MRAM_RXF1].num = out_val[4] & RXFC_FS_MASK;
+	priv->mcfg[MRAM_RXB].off = priv->mcfg[MRAM_RXF1].off +
+			priv->mcfg[MRAM_RXF1].num * RXF1_ELEMENT_SIZE;
+	priv->mcfg[MRAM_RXB].num = out_val[5];
+	priv->mcfg[MRAM_TXE].off = priv->mcfg[MRAM_RXB].off +
+			priv->mcfg[MRAM_RXB].num * RXB_ELEMENT_SIZE;
+	priv->mcfg[MRAM_TXE].num = out_val[6];
+	priv->mcfg[MRAM_TXB].off = priv->mcfg[MRAM_TXE].off +
+			priv->mcfg[MRAM_TXE].num * TXE_ELEMENT_SIZE;
+	priv->mcfg[MRAM_TXB].num = out_val[7] & TXBC_NDTB_MASK;
+
+	dev_dbg(&pdev->dev, "mram_base %p sidf 0x%x %d xidf 0x%x %d rxf0 0x%x %d rxf1 0x%x %d rxb 0x%x %d txe 0x%x %d txb 0x%x %d\n",
+		priv->mram_base,
+		priv->mcfg[MRAM_SIDF].off, priv->mcfg[MRAM_SIDF].num,
+		priv->mcfg[MRAM_XIDF].off, priv->mcfg[MRAM_XIDF].num,
+		priv->mcfg[MRAM_RXF0].off, priv->mcfg[MRAM_RXF0].num,
+		priv->mcfg[MRAM_RXF1].off, priv->mcfg[MRAM_RXF1].num,
+		priv->mcfg[MRAM_RXB].off, priv->mcfg[MRAM_RXB].num,
+		priv->mcfg[MRAM_TXE].off, priv->mcfg[MRAM_TXE].num,
+		priv->mcfg[MRAM_TXB].off, priv->mcfg[MRAM_TXB].num);
+
+	return 0;
+}
+
+static int m_can_plat_probe(struct platform_device *pdev)
+{
+	struct net_device *dev;
+	struct m_can_priv *priv;
+	struct resource *res;
+	void __iomem *addr;
+	struct clk *hclk, *cclk;
+	int irq, ret;
+
+	hclk = devm_clk_get(&pdev->dev, "hclk");
+	cclk = devm_clk_get(&pdev->dev, "cclk");
+	if (IS_ERR(hclk) || IS_ERR(cclk)) {
+		dev_err(&pdev->dev, "no clock find\n");
+		return -ENODEV;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "m_can");
+	addr = devm_ioremap_resource(&pdev->dev, res);
+	irq = platform_get_irq_byname(pdev, "int0");
+	if (IS_ERR(addr) || irq < 0)
+		return -EINVAL;
+
+	/* allocate the m_can device */
+	dev = alloc_m_can_dev();
+	if (!dev)
+		return -ENOMEM;
+
+	priv = netdev_priv(dev);
+	dev->irq = irq;
+	priv->base = addr;
+	priv->device = &pdev->dev;
+	priv->hclk = hclk;
+	priv->cclk = cclk;
+	priv->can.clock.freq = clk_get_rate(cclk);
+
+	ret = m_can_of_parse_mram(pdev, priv);
+	if (ret)
+		goto failed_free_dev;
+
+	platform_set_drvdata(pdev, dev);
+	SET_NETDEV_DEV(dev, &pdev->dev);
+
+	ret = register_m_can_dev(dev);
+	if (ret) {
+		dev_err(&pdev->dev, "registering %s failed (err=%d)\n",
+			KBUILD_MODNAME, ret);
+		goto failed_free_dev;
+	}
+
+	devm_can_led_init(dev);
+
+	dev_info(&pdev->dev, "%s device registered (regs=%p, irq=%d)\n",
+		 KBUILD_MODNAME, priv->base, dev->irq);
+
+	return 0;
+
+failed_free_dev:
+	free_m_can_dev(dev);
+	return ret;
+}
+
+static __maybe_unused int m_can_suspend(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
+	struct m_can_priv *priv = netdev_priv(ndev);
+
+	if (netif_running(ndev)) {
+		netif_stop_queue(ndev);
+		netif_device_detach(ndev);
+	}
+
+	/* TODO: enter low power */
+
+	priv->can.state = CAN_STATE_SLEEPING;
+
+	return 0;
+}
+
+static __maybe_unused int m_can_resume(struct device *dev)
+{
+	struct net_device *ndev = dev_get_drvdata(dev);
+	struct m_can_priv *priv = netdev_priv(ndev);
+
+	/* TODO: exit low power */
+
+	priv->can.state = CAN_STATE_ERROR_ACTIVE;
+
+	if (netif_running(ndev)) {
+		netif_device_attach(ndev);
+		netif_start_queue(ndev);
+	}
+
+	return 0;
+}
+
+static void unregister_m_can_dev(struct net_device *dev)
+{
+	unregister_candev(dev);
+}
+
+static int m_can_plat_remove(struct platform_device *pdev)
+{
+	struct net_device *dev = platform_get_drvdata(pdev);
+
+	unregister_m_can_dev(dev);
+	platform_set_drvdata(pdev, NULL);
+
+	free_m_can_dev(dev);
+
+	return 0;
+}
+
+static const struct dev_pm_ops m_can_pmops = {
+	SET_SYSTEM_SLEEP_PM_OPS(m_can_suspend, m_can_resume)
+};
+
+static const struct of_device_id m_can_of_table[] = {
+	{ .compatible = "bosch,m_can", .data = NULL },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, m_can_of_table);
+
+static struct platform_driver m_can_plat_driver = {
+	.driver = {
+		.name = KBUILD_MODNAME,
+		.of_match_table = m_can_of_table,
+		.pm     = &m_can_pmops,
+	},
+	.probe = m_can_plat_probe,
+	.remove = m_can_plat_remove,
+};
+
+module_platform_driver(m_can_plat_driver);
+
+MODULE_AUTHOR("Dong Aisheng <b29396@freescale.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("CAN bus driver for Bosch M_CAN controller");
diff --git a/drivers/net/can/mscan/Makefile b/drivers/net/can/mscan/Makefile
index c9fab17cd8b4..58903b45f5fb 100644
--- a/drivers/net/can/mscan/Makefile
+++ b/drivers/net/can/mscan/Makefile
@@ -1,5 +1,3 @@
 
 obj-$(CONFIG_CAN_MPC5XXX)	+= mscan-mpc5xxx.o
 mscan-mpc5xxx-objs		:= mscan.o mpc5xxx_can.o
-
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/rcar_can.c b/drivers/net/can/rcar_can.c
index 5268d216ecfa..1abe133d1594 100644
--- a/drivers/net/can/rcar_can.c
+++ b/drivers/net/can/rcar_can.c
@@ -20,6 +20,7 @@
 #include <linux/can/dev.h>
 #include <linux/clk.h>
 #include <linux/can/platform/rcar_can.h>
+#include <linux/of.h>
 
 #define RCAR_CAN_DRV_NAME	"rcar_can"
 
@@ -87,6 +88,7 @@ struct rcar_can_priv {
 	struct napi_struct napi;
 	struct rcar_can_regs __iomem *regs;
 	struct clk *clk;
+	struct clk *can_clk;
 	u8 tx_dlc[RCAR_CAN_FIFO_DEPTH];
 	u32 tx_head;
 	u32 tx_tail;
@@ -505,14 +507,20 @@ static int rcar_can_open(struct net_device *ndev)
 
 	err = clk_prepare_enable(priv->clk);
 	if (err) {
-		netdev_err(ndev, "clk_prepare_enable() failed, error %d\n",
+		netdev_err(ndev, "failed to enable periperal clock, error %d\n",
 			   err);
 		goto out;
 	}
+	err = clk_prepare_enable(priv->can_clk);
+	if (err) {
+		netdev_err(ndev, "failed to enable CAN clock, error %d\n",
+			   err);
+		goto out_clock;
+	}
 	err = open_candev(ndev);
 	if (err) {
 		netdev_err(ndev, "open_candev() failed, error %d\n", err);
-		goto out_clock;
+		goto out_can_clock;
 	}
 	napi_enable(&priv->napi);
 	err = request_irq(ndev->irq, rcar_can_interrupt, 0, ndev->name, ndev);
@@ -527,6 +535,8 @@ static int rcar_can_open(struct net_device *ndev)
 out_close:
 	napi_disable(&priv->napi);
 	close_candev(ndev);
+out_can_clock:
+	clk_disable_unprepare(priv->can_clk);
 out_clock:
 	clk_disable_unprepare(priv->clk);
 out:
@@ -565,6 +575,7 @@ static int rcar_can_close(struct net_device *ndev)
 	rcar_can_stop(ndev);
 	free_irq(ndev->irq, ndev);
 	napi_disable(&priv->napi);
+	clk_disable_unprepare(priv->can_clk);
 	clk_disable_unprepare(priv->clk);
 	close_candev(ndev);
 	can_led_event(ndev, CAN_LED_EVENT_STOP);
@@ -715,6 +726,12 @@ static int rcar_can_get_berr_counter(const struct net_device *dev,
 	return 0;
 }
 
+static const char * const clock_names[] = {
+	[CLKR_CLKP1]	= "clkp1",
+	[CLKR_CLKP2]	= "clkp2",
+	[CLKR_CLKEXT]	= "can_clk",
+};
+
 static int rcar_can_probe(struct platform_device *pdev)
 {
 	struct rcar_can_platform_data *pdata;
@@ -722,13 +739,20 @@ static int rcar_can_probe(struct platform_device *pdev)
 	struct net_device *ndev;
 	struct resource *mem;
 	void __iomem *addr;
+	u32 clock_select = CLKR_CLKP1;
 	int err = -ENODEV;
 	int irq;
 
-	pdata = dev_get_platdata(&pdev->dev);
-	if (!pdata) {
-		dev_err(&pdev->dev, "No platform data provided!\n");
-		goto fail;
+	if (pdev->dev.of_node) {
+		of_property_read_u32(pdev->dev.of_node,
+				     "renesas,can-clock-select", &clock_select);
+	} else {
+		pdata = dev_get_platdata(&pdev->dev);
+		if (!pdata) {
+			dev_err(&pdev->dev, "No platform data provided!\n");
+			goto fail;
+		}
+		clock_select = pdata->clock_select;
 	}
 
 	irq = platform_get_irq(pdev, 0);
@@ -753,10 +777,22 @@ static int rcar_can_probe(struct platform_device *pdev)
 
 	priv = netdev_priv(ndev);
 
-	priv->clk = devm_clk_get(&pdev->dev, NULL);
+	priv->clk = devm_clk_get(&pdev->dev, "clkp1");
 	if (IS_ERR(priv->clk)) {
 		err = PTR_ERR(priv->clk);
-		dev_err(&pdev->dev, "cannot get clock: %d\n", err);
+		dev_err(&pdev->dev, "cannot get peripheral clock: %d\n", err);
+		goto fail_clk;
+	}
+
+	if (clock_select >= ARRAY_SIZE(clock_names)) {
+		err = -EINVAL;
+		dev_err(&pdev->dev, "invalid CAN clock selected\n");
+		goto fail_clk;
+	}
+	priv->can_clk = devm_clk_get(&pdev->dev, clock_names[clock_select]);
+	if (IS_ERR(priv->can_clk)) {
+		err = PTR_ERR(priv->can_clk);
+		dev_err(&pdev->dev, "cannot get CAN clock: %d\n", err);
 		goto fail_clk;
 	}
 
@@ -765,8 +801,8 @@ static int rcar_can_probe(struct platform_device *pdev)
 	ndev->flags |= IFF_ECHO;
 	priv->ndev = ndev;
 	priv->regs = addr;
-	priv->clock_select = pdata->clock_select;
-	priv->can.clock.freq = clk_get_rate(priv->clk);
+	priv->clock_select = clock_select;
+	priv->can.clock.freq = clk_get_rate(priv->can_clk);
 	priv->can.bittiming_const = &rcar_can_bittiming_const;
 	priv->can.do_set_mode = rcar_can_do_set_mode;
 	priv->can.do_get_berr_counter = rcar_can_get_berr_counter;
@@ -858,10 +894,20 @@ static int __maybe_unused rcar_can_resume(struct device *dev)
 
 static SIMPLE_DEV_PM_OPS(rcar_can_pm_ops, rcar_can_suspend, rcar_can_resume);
 
+static const struct of_device_id rcar_can_of_table[] __maybe_unused = {
+	{ .compatible = "renesas,can-r8a7778" },
+	{ .compatible = "renesas,can-r8a7779" },
+	{ .compatible = "renesas,can-r8a7790" },
+	{ .compatible = "renesas,can-r8a7791" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rcar_can_of_table);
+
 static struct platform_driver rcar_can_driver = {
 	.driver = {
 		.name = RCAR_CAN_DRV_NAME,
 		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(rcar_can_of_table),
 		.pm = &rcar_can_pm_ops,
 	},
 	.probe = rcar_can_probe,
diff --git a/drivers/net/can/sja1000/Makefile b/drivers/net/can/sja1000/Makefile
index 531d5fcc97e5..be11ddd11b87 100644
--- a/drivers/net/can/sja1000/Makefile
+++ b/drivers/net/can/sja1000/Makefile
@@ -12,5 +12,3 @@ obj-$(CONFIG_CAN_PEAK_PCMCIA) += peak_pcmcia.o
 obj-$(CONFIG_CAN_PEAK_PCI) += peak_pci.o
 obj-$(CONFIG_CAN_PLX_PCI) += plx_pci.o
 obj-$(CONFIG_CAN_TSCAN1) += tscan1.o
-
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/sja1000/peak_pci.c b/drivers/net/can/sja1000/peak_pci.c
index 7a85590fefb9..e5fac368068a 100644
--- a/drivers/net/can/sja1000/peak_pci.c
+++ b/drivers/net/can/sja1000/peak_pci.c
@@ -70,6 +70,8 @@ struct peak_pci_chan {
 #define PEAK_PC_104P_DEVICE_ID	0x0006	/* PCAN-PC/104+ cards */
 #define PEAK_PCI_104E_DEVICE_ID	0x0007	/* PCAN-PCI/104 Express cards */
 #define PEAK_MPCIE_DEVICE_ID	0x0008	/* The miniPCIe slot cards */
+#define PEAK_PCIE_OEM_ID	0x0009	/* PCAN-PCI Express OEM */
+#define PEAK_PCIEC34_DEVICE_ID	0x000A	/* PCAN-PCI Express 34 (one channel) */
 
 #define PEAK_PCI_CHAN_MAX	4
 
@@ -87,6 +89,7 @@ static const struct pci_device_id peak_pci_tbl[] = {
 	{PEAK_PCI_VENDOR_ID, PEAK_CPCI_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
 #ifdef CONFIG_CAN_PEAK_PCIEC
 	{PEAK_PCI_VENDOR_ID, PEAK_PCIEC_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
+	{PEAK_PCI_VENDOR_ID, PEAK_PCIEC34_DEVICE_ID, PCI_ANY_ID, PCI_ANY_ID,},
 #endif
 	{0,}
 };
@@ -653,7 +656,8 @@ static int peak_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		 * This must be done *before* register_sja1000dev() but
 		 * *after* devices linkage
 		 */
-		if (pdev->device == PEAK_PCIEC_DEVICE_ID) {
+		if (pdev->device == PEAK_PCIEC_DEVICE_ID ||
+		    pdev->device == PEAK_PCIEC34_DEVICE_ID) {
 			err = peak_pciec_probe(pdev, dev);
 			if (err) {
 				dev_err(&pdev->dev,
diff --git a/drivers/net/can/sja1000/sja1000.c b/drivers/net/can/sja1000/sja1000.c
index d1692154ed1b..b27ac6074afb 100644
--- a/drivers/net/can/sja1000/sja1000.c
+++ b/drivers/net/can/sja1000/sja1000.c
@@ -172,6 +172,35 @@ static void set_normal_mode(struct net_device *dev)
 	netdev_err(dev, "setting SJA1000 into normal mode failed!\n");
 }
 
+/*
+ * initialize SJA1000 chip:
+ *   - reset chip
+ *   - set output mode
+ *   - set baudrate
+ *   - enable interrupts
+ *   - start operating mode
+ */
+static void chipset_init(struct net_device *dev)
+{
+	struct sja1000_priv *priv = netdev_priv(dev);
+
+	/* set clock divider and output control register */
+	priv->write_reg(priv, SJA1000_CDR, priv->cdr | CDR_PELICAN);
+
+	/* set acceptance filter (accept all) */
+	priv->write_reg(priv, SJA1000_ACCC0, 0x00);
+	priv->write_reg(priv, SJA1000_ACCC1, 0x00);
+	priv->write_reg(priv, SJA1000_ACCC2, 0x00);
+	priv->write_reg(priv, SJA1000_ACCC3, 0x00);
+
+	priv->write_reg(priv, SJA1000_ACCM0, 0xFF);
+	priv->write_reg(priv, SJA1000_ACCM1, 0xFF);
+	priv->write_reg(priv, SJA1000_ACCM2, 0xFF);
+	priv->write_reg(priv, SJA1000_ACCM3, 0xFF);
+
+	priv->write_reg(priv, SJA1000_OCR, priv->ocr | OCR_MODE_NORMAL);
+}
+
 static void sja1000_start(struct net_device *dev)
 {
 	struct sja1000_priv *priv = netdev_priv(dev);
@@ -180,6 +209,10 @@ static void sja1000_start(struct net_device *dev)
 	if (priv->can.state != CAN_STATE_STOPPED)
 		set_reset_mode(dev);
 
+	/* Initialize chip if uninitialized at this stage */
+	if (!(priv->read_reg(priv, SJA1000_CDR) & CDR_PELICAN))
+		chipset_init(dev);
+
 	/* Clear error counters and error code capture */
 	priv->write_reg(priv, SJA1000_TXERR, 0x0);
 	priv->write_reg(priv, SJA1000_RXERR, 0x0);
@@ -237,35 +270,6 @@ static int sja1000_get_berr_counter(const struct net_device *dev,
 }
 
 /*
- * initialize SJA1000 chip:
- *   - reset chip
- *   - set output mode
- *   - set baudrate
- *   - enable interrupts
- *   - start operating mode
- */
-static void chipset_init(struct net_device *dev)
-{
-	struct sja1000_priv *priv = netdev_priv(dev);
-
-	/* set clock divider and output control register */
-	priv->write_reg(priv, SJA1000_CDR, priv->cdr | CDR_PELICAN);
-
-	/* set acceptance filter (accept all) */
-	priv->write_reg(priv, SJA1000_ACCC0, 0x00);
-	priv->write_reg(priv, SJA1000_ACCC1, 0x00);
-	priv->write_reg(priv, SJA1000_ACCC2, 0x00);
-	priv->write_reg(priv, SJA1000_ACCC3, 0x00);
-
-	priv->write_reg(priv, SJA1000_ACCM0, 0xFF);
-	priv->write_reg(priv, SJA1000_ACCM1, 0xFF);
-	priv->write_reg(priv, SJA1000_ACCM2, 0xFF);
-	priv->write_reg(priv, SJA1000_ACCM3, 0xFF);
-
-	priv->write_reg(priv, SJA1000_OCR, priv->ocr | OCR_MODE_NORMAL);
-}
-
-/*
  * transmit a CAN message
  * message layout in the sk_buff should be like this:
  * xx xx xx xx	 ff	 ll   00 11 22 33 44 55 66 77
diff --git a/drivers/net/can/softing/Makefile b/drivers/net/can/softing/Makefile
index c5e5016c742e..a23da492dad5 100644
--- a/drivers/net/can/softing/Makefile
+++ b/drivers/net/can/softing/Makefile
@@ -2,5 +2,3 @@
 softing-y := softing_main.o softing_fw.o
 obj-$(CONFIG_CAN_SOFTING) += softing.o
 obj-$(CONFIG_CAN_SOFTING_CS) += softing_cs.o
-
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/spi/Makefile b/drivers/net/can/spi/Makefile
index 90bcacffbc65..0e86040cdd8c 100644
--- a/drivers/net/can/spi/Makefile
+++ b/drivers/net/can/spi/Makefile
@@ -4,5 +4,3 @@
 
 
 obj-$(CONFIG_CAN_MCP251X)	+= mcp251x.o
-
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/can/spi/mcp251x.c b/drivers/net/can/spi/mcp251x.c
index 5df239e68812..c66d699640a9 100644
--- a/drivers/net/can/spi/mcp251x.c
+++ b/drivers/net/can/spi/mcp251x.c
@@ -1107,10 +1107,10 @@ static int mcp251x_can_probe(struct spi_device *spi)
 		 * Minimum coherent DMA allocation is PAGE_SIZE, so allocate
 		 * that much and share it between Tx and Rx DMA buffers.
 		 */
-		priv->spi_tx_buf = dma_alloc_coherent(&spi->dev,
-						      PAGE_SIZE,
-						      &priv->spi_tx_dma,
-						      GFP_DMA);
+		priv->spi_tx_buf = dmam_alloc_coherent(&spi->dev,
+						       PAGE_SIZE,
+						       &priv->spi_tx_dma,
+						       GFP_DMA);
 
 		if (priv->spi_tx_buf) {
 			priv->spi_rx_buf = (priv->spi_tx_buf + (PAGE_SIZE / 2));
@@ -1156,9 +1156,6 @@ static int mcp251x_can_probe(struct spi_device *spi)
 	return 0;
 
 error_probe:
-	if (mcp251x_enable_dma)
-		dma_free_coherent(&spi->dev, PAGE_SIZE,
-				  priv->spi_tx_buf, priv->spi_tx_dma);
 	mcp251x_power_enable(priv->power, 0);
 
 out_clk:
@@ -1178,11 +1175,6 @@ static int mcp251x_can_remove(struct spi_device *spi)
 
 	unregister_candev(net);
 
-	if (mcp251x_enable_dma) {
-		dma_free_coherent(&spi->dev, PAGE_SIZE,
-				  priv->spi_tx_buf, priv->spi_tx_dma);
-	}
-
 	mcp251x_power_enable(priv->power, 0);
 
 	if (!IS_ERR(priv->clk))
diff --git a/drivers/net/can/usb/Makefile b/drivers/net/can/usb/Makefile
index 7b9a393b1ac8..a64cf983fb87 100644
--- a/drivers/net/can/usb/Makefile
+++ b/drivers/net/can/usb/Makefile
@@ -8,5 +8,3 @@ obj-$(CONFIG_CAN_GS_USB) += gs_usb.o
 obj-$(CONFIG_CAN_KVASER_USB) += kvaser_usb.o
 obj-$(CONFIG_CAN_PEAK_USB) += peak_usb/
 obj-$(CONFIG_CAN_8DEV_USB) += usb_8dev.o
-
-ccflags-$(CONFIG_CAN_DEBUG_DEVICES) := -DDEBUG
diff --git a/drivers/net/dsa/Kconfig b/drivers/net/dsa/Kconfig
index b8fe808b7957..9234d808cbb3 100644
--- a/drivers/net/dsa/Kconfig
+++ b/drivers/net/dsa/Kconfig
@@ -36,4 +36,25 @@ config NET_DSA_MV88E6123_61_65
 	  This enables support for the Marvell 88E6123/6161/6165
 	  ethernet switch chips.
 
+config NET_DSA_MV88E6171
+	tristate "Marvell 88E6171 ethernet switch chip support"
+	select NET_DSA
+	select NET_DSA_MV88E6XXX
+	select NET_DSA_TAG_EDSA
+	---help---
+	  This enables support for the Marvell 88E6171 ethernet switch
+	  chip.
+
+config NET_DSA_BCM_SF2
+	tristate "Broadcom Starfighter 2 Ethernet switch support"
+	depends on HAS_IOMEM
+	select NET_DSA
+	select NET_DSA_TAG_BRCM
+	select FIXED_PHY if NET_DSA_BCM_SF2=y
+	select BCM7XXX_PHY
+	select MDIO_BCM_UNIMAC
+	---help---
+	  This enables support for the Broadcom Starfighter 2 Ethernet
+	  switch chips.
+
 endmenu
diff --git a/drivers/net/dsa/Makefile b/drivers/net/dsa/Makefile
index f3bda05536cc..23a90de9830e 100644
--- a/drivers/net/dsa/Makefile
+++ b/drivers/net/dsa/Makefile
@@ -7,3 +7,7 @@ endif
 ifdef CONFIG_NET_DSA_MV88E6131
 mv88e6xxx_drv-y += mv88e6131.o
 endif
+ifdef CONFIG_NET_DSA_MV88E6171
+mv88e6xxx_drv-y += mv88e6171.o
+endif
+obj-$(CONFIG_NET_DSA_BCM_SF2)	+= bcm_sf2.o
diff --git a/drivers/net/dsa/bcm_sf2.c b/drivers/net/dsa/bcm_sf2.c
new file mode 100644
index 000000000000..b9625968daac
--- /dev/null
+++ b/drivers/net/dsa/bcm_sf2.c
@@ -0,0 +1,887 @@
+/*
+ * Broadcom Starfighter 2 DSA switch driver
+ *
+ * Copyright (C) 2014, Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/phy.h>
+#include <linux/phy_fixed.h>
+#include <linux/mii.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_address.h>
+#include <net/dsa.h>
+#include <linux/ethtool.h>
+
+#include "bcm_sf2.h"
+#include "bcm_sf2_regs.h"
+
+/* String, offset, and register size in bytes if different from 4 bytes */
+static const struct bcm_sf2_hw_stats bcm_sf2_mib[] = {
+	{ "TxOctets",		0x000, 8	},
+	{ "TxDropPkts",		0x020		},
+	{ "TxQPKTQ0",		0x030		},
+	{ "TxBroadcastPkts",	0x040		},
+	{ "TxMulticastPkts",	0x050		},
+	{ "TxUnicastPKts",	0x060		},
+	{ "TxCollisions",	0x070		},
+	{ "TxSingleCollision",	0x080		},
+	{ "TxMultipleCollision", 0x090		},
+	{ "TxDeferredCollision", 0x0a0		},
+	{ "TxLateCollision",	0x0b0		},
+	{ "TxExcessiveCollision", 0x0c0		},
+	{ "TxFrameInDisc",	0x0d0		},
+	{ "TxPausePkts",	0x0e0		},
+	{ "TxQPKTQ1",		0x0f0		},
+	{ "TxQPKTQ2",		0x100		},
+	{ "TxQPKTQ3",		0x110		},
+	{ "TxQPKTQ4",		0x120		},
+	{ "TxQPKTQ5",		0x130		},
+	{ "RxOctets",		0x140, 8	},
+	{ "RxUndersizePkts",	0x160		},
+	{ "RxPausePkts",	0x170		},
+	{ "RxPkts64Octets",	0x180		},
+	{ "RxPkts65to127Octets", 0x190		},
+	{ "RxPkts128to255Octets", 0x1a0		},
+	{ "RxPkts256to511Octets", 0x1b0		},
+	{ "RxPkts512to1023Octets", 0x1c0	},
+	{ "RxPkts1024toMaxPktsOctets", 0x1d0	},
+	{ "RxOversizePkts",	0x1e0		},
+	{ "RxJabbers",		0x1f0		},
+	{ "RxAlignmentErrors",	0x200		},
+	{ "RxFCSErrors",	0x210		},
+	{ "RxGoodOctets",	0x220, 8	},
+	{ "RxDropPkts",		0x240		},
+	{ "RxUnicastPkts",	0x250		},
+	{ "RxMulticastPkts",	0x260		},
+	{ "RxBroadcastPkts",	0x270		},
+	{ "RxSAChanges",	0x280		},
+	{ "RxFragments",	0x290		},
+	{ "RxJumboPkt",		0x2a0		},
+	{ "RxSymblErr",		0x2b0		},
+	{ "InRangeErrCount",	0x2c0		},
+	{ "OutRangeErrCount",	0x2d0		},
+	{ "EEELpiEvent",	0x2e0		},
+	{ "EEELpiDuration",	0x2f0		},
+	{ "RxDiscard",		0x300, 8	},
+	{ "TxQPKTQ6",		0x320		},
+	{ "TxQPKTQ7",		0x330		},
+	{ "TxPkts64Octets",	0x340		},
+	{ "TxPkts65to127Octets", 0x350		},
+	{ "TxPkts128to255Octets", 0x360		},
+	{ "TxPkts256to511Ocets", 0x370		},
+	{ "TxPkts512to1023Ocets", 0x380		},
+	{ "TxPkts1024toMaxPktOcets", 0x390	},
+};
+
+#define BCM_SF2_STATS_SIZE	ARRAY_SIZE(bcm_sf2_mib)
+
+static void bcm_sf2_sw_get_strings(struct dsa_switch *ds,
+				   int port, uint8_t *data)
+{
+	unsigned int i;
+
+	for (i = 0; i < BCM_SF2_STATS_SIZE; i++)
+		memcpy(data + i * ETH_GSTRING_LEN,
+		       bcm_sf2_mib[i].string, ETH_GSTRING_LEN);
+}
+
+static void bcm_sf2_sw_get_ethtool_stats(struct dsa_switch *ds,
+					 int port, uint64_t *data)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	const struct bcm_sf2_hw_stats *s;
+	unsigned int i;
+	u64 val = 0;
+	u32 offset;
+
+	mutex_lock(&priv->stats_mutex);
+
+	/* Now fetch the per-port counters */
+	for (i = 0; i < BCM_SF2_STATS_SIZE; i++) {
+		s = &bcm_sf2_mib[i];
+
+		/* Do a latched 64-bit read if needed */
+		offset = s->reg + CORE_P_MIB_OFFSET(port);
+		if (s->sizeof_stat == 8)
+			val = core_readq(priv, offset);
+		else
+			val = core_readl(priv, offset);
+
+		data[i] = (u64)val;
+	}
+
+	mutex_unlock(&priv->stats_mutex);
+}
+
+static int bcm_sf2_sw_get_sset_count(struct dsa_switch *ds)
+{
+	return BCM_SF2_STATS_SIZE;
+}
+
+static char *bcm_sf2_sw_probe(struct device *host_dev, int sw_addr)
+{
+	return "Broadcom Starfighter 2";
+}
+
+static void bcm_sf2_imp_vlan_setup(struct dsa_switch *ds, int cpu_port)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	unsigned int i;
+	u32 reg;
+
+	/* Enable the IMP Port to be in the same VLAN as the other ports
+	 * on a per-port basis such that we only have Port i and IMP in
+	 * the same VLAN.
+	 */
+	for (i = 0; i < priv->hw_params.num_ports; i++) {
+		if (!((1 << i) & ds->phys_port_mask))
+			continue;
+
+		reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(i));
+		reg |= (1 << cpu_port);
+		core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(i));
+	}
+}
+
+static void bcm_sf2_imp_setup(struct dsa_switch *ds, int port)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	u32 reg, val;
+
+	/* Enable the port memories */
+	reg = core_readl(priv, CORE_MEM_PSM_VDD_CTRL);
+	reg &= ~P_TXQ_PSM_VDD(port);
+	core_writel(priv, reg, CORE_MEM_PSM_VDD_CTRL);
+
+	/* Enable Broadcast, Multicast, Unicast forwarding to IMP port */
+	reg = core_readl(priv, CORE_IMP_CTL);
+	reg |= (RX_BCST_EN | RX_MCST_EN | RX_UCST_EN);
+	reg &= ~(RX_DIS | TX_DIS);
+	core_writel(priv, reg, CORE_IMP_CTL);
+
+	/* Enable forwarding */
+	core_writel(priv, SW_FWDG_EN, CORE_SWMODE);
+
+	/* Enable IMP port in dumb mode */
+	reg = core_readl(priv, CORE_SWITCH_CTRL);
+	reg |= MII_DUMB_FWDG_EN;
+	core_writel(priv, reg, CORE_SWITCH_CTRL);
+
+	/* Resolve which bit controls the Broadcom tag */
+	switch (port) {
+	case 8:
+		val = BRCM_HDR_EN_P8;
+		break;
+	case 7:
+		val = BRCM_HDR_EN_P7;
+		break;
+	case 5:
+		val = BRCM_HDR_EN_P5;
+		break;
+	default:
+		val = 0;
+		break;
+	}
+
+	/* Enable Broadcom tags for IMP port */
+	reg = core_readl(priv, CORE_BRCM_HDR_CTRL);
+	reg |= val;
+	core_writel(priv, reg, CORE_BRCM_HDR_CTRL);
+
+	/* Enable reception Broadcom tag for CPU TX (switch RX) to
+	 * allow us to tag outgoing frames
+	 */
+	reg = core_readl(priv, CORE_BRCM_HDR_RX_DIS);
+	reg &= ~(1 << port);
+	core_writel(priv, reg, CORE_BRCM_HDR_RX_DIS);
+
+	/* Enable transmission of Broadcom tags from the switch (CPU RX) to
+	 * allow delivering frames to the per-port net_devices
+	 */
+	reg = core_readl(priv, CORE_BRCM_HDR_TX_DIS);
+	reg &= ~(1 << port);
+	core_writel(priv, reg, CORE_BRCM_HDR_TX_DIS);
+
+	/* Force link status for IMP port */
+	reg = core_readl(priv, CORE_STS_OVERRIDE_IMP);
+	reg |= (MII_SW_OR | LINK_STS);
+	core_writel(priv, reg, CORE_STS_OVERRIDE_IMP);
+}
+
+static void bcm_sf2_eee_enable_set(struct dsa_switch *ds, int port, bool enable)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	u32 reg;
+
+	reg = core_readl(priv, CORE_EEE_EN_CTRL);
+	if (enable)
+		reg |= 1 << port;
+	else
+		reg &= ~(1 << port);
+	core_writel(priv, reg, CORE_EEE_EN_CTRL);
+}
+
+static int bcm_sf2_port_setup(struct dsa_switch *ds, int port,
+			      struct phy_device *phy)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	s8 cpu_port = ds->dst[ds->index].cpu_port;
+	u32 reg;
+
+	/* Clear the memory power down */
+	reg = core_readl(priv, CORE_MEM_PSM_VDD_CTRL);
+	reg &= ~P_TXQ_PSM_VDD(port);
+	core_writel(priv, reg, CORE_MEM_PSM_VDD_CTRL);
+
+	/* Clear the Rx and Tx disable bits and set to no spanning tree */
+	core_writel(priv, 0, CORE_G_PCTL_PORT(port));
+
+	/* Enable port 7 interrupts to get notified */
+	if (port == 7)
+		intrl2_1_mask_clear(priv, P_IRQ_MASK(P7_IRQ_OFF));
+
+	/* Set this port, and only this one to be in the default VLAN */
+	reg = core_readl(priv, CORE_PORT_VLAN_CTL_PORT(port));
+	reg &= ~PORT_VLAN_CTRL_MASK;
+	reg |= (1 << port);
+	core_writel(priv, reg, CORE_PORT_VLAN_CTL_PORT(port));
+
+	bcm_sf2_imp_vlan_setup(ds, cpu_port);
+
+	/* If EEE was enabled, restore it */
+	if (priv->port_sts[port].eee.eee_enabled)
+		bcm_sf2_eee_enable_set(ds, port, true);
+
+	return 0;
+}
+
+static void bcm_sf2_port_disable(struct dsa_switch *ds, int port,
+				 struct phy_device *phy)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	u32 off, reg;
+
+	if (priv->wol_ports_mask & (1 << port))
+		return;
+
+	if (port == 7) {
+		intrl2_1_mask_set(priv, P_IRQ_MASK(P7_IRQ_OFF));
+		intrl2_1_writel(priv, P_IRQ_MASK(P7_IRQ_OFF), INTRL2_CPU_CLEAR);
+	}
+
+	if (dsa_is_cpu_port(ds, port))
+		off = CORE_IMP_CTL;
+	else
+		off = CORE_G_PCTL_PORT(port);
+
+	reg = core_readl(priv, off);
+	reg |= RX_DIS | TX_DIS;
+	core_writel(priv, reg, off);
+
+	/* Power down the port memory */
+	reg = core_readl(priv, CORE_MEM_PSM_VDD_CTRL);
+	reg |= P_TXQ_PSM_VDD(port);
+	core_writel(priv, reg, CORE_MEM_PSM_VDD_CTRL);
+}
+
+/* Returns 0 if EEE was not enabled, or 1 otherwise
+ */
+static int bcm_sf2_eee_init(struct dsa_switch *ds, int port,
+			    struct phy_device *phy)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct ethtool_eee *p = &priv->port_sts[port].eee;
+	int ret;
+
+	p->supported = (SUPPORTED_1000baseT_Full | SUPPORTED_100baseT_Full);
+
+	ret = phy_init_eee(phy, 0);
+	if (ret)
+		return 0;
+
+	bcm_sf2_eee_enable_set(ds, port, true);
+
+	return 1;
+}
+
+static int bcm_sf2_sw_get_eee(struct dsa_switch *ds, int port,
+			      struct ethtool_eee *e)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct ethtool_eee *p = &priv->port_sts[port].eee;
+	u32 reg;
+
+	reg = core_readl(priv, CORE_EEE_LPI_INDICATE);
+	e->eee_enabled = p->eee_enabled;
+	e->eee_active = !!(reg & (1 << port));
+
+	return 0;
+}
+
+static int bcm_sf2_sw_set_eee(struct dsa_switch *ds, int port,
+			      struct phy_device *phydev,
+			      struct ethtool_eee *e)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct ethtool_eee *p = &priv->port_sts[port].eee;
+
+	p->eee_enabled = e->eee_enabled;
+
+	if (!p->eee_enabled) {
+		bcm_sf2_eee_enable_set(ds, port, false);
+	} else {
+		p->eee_enabled = bcm_sf2_eee_init(ds, port, phydev);
+		if (!p->eee_enabled)
+			return -EOPNOTSUPP;
+	}
+
+	return 0;
+}
+
+static irqreturn_t bcm_sf2_switch_0_isr(int irq, void *dev_id)
+{
+	struct bcm_sf2_priv *priv = dev_id;
+
+	priv->irq0_stat = intrl2_0_readl(priv, INTRL2_CPU_STATUS) &
+				~priv->irq0_mask;
+	intrl2_0_writel(priv, priv->irq0_stat, INTRL2_CPU_CLEAR);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t bcm_sf2_switch_1_isr(int irq, void *dev_id)
+{
+	struct bcm_sf2_priv *priv = dev_id;
+
+	priv->irq1_stat = intrl2_1_readl(priv, INTRL2_CPU_STATUS) &
+				~priv->irq1_mask;
+	intrl2_1_writel(priv, priv->irq1_stat, INTRL2_CPU_CLEAR);
+
+	if (priv->irq1_stat & P_LINK_UP_IRQ(P7_IRQ_OFF))
+		priv->port_sts[7].link = 1;
+	if (priv->irq1_stat & P_LINK_DOWN_IRQ(P7_IRQ_OFF))
+		priv->port_sts[7].link = 0;
+
+	return IRQ_HANDLED;
+}
+
+static int bcm_sf2_sw_setup(struct dsa_switch *ds)
+{
+	const char *reg_names[BCM_SF2_REGS_NUM] = BCM_SF2_REGS_NAME;
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct device_node *dn;
+	void __iomem **base;
+	unsigned int port;
+	unsigned int i;
+	u32 reg, rev;
+	int ret;
+
+	spin_lock_init(&priv->indir_lock);
+	mutex_init(&priv->stats_mutex);
+
+	/* All the interesting properties are at the parent device_node
+	 * level
+	 */
+	dn = ds->pd->of_node->parent;
+
+	priv->irq0 = irq_of_parse_and_map(dn, 0);
+	priv->irq1 = irq_of_parse_and_map(dn, 1);
+
+	base = &priv->core;
+	for (i = 0; i < BCM_SF2_REGS_NUM; i++) {
+		*base = of_iomap(dn, i);
+		if (*base == NULL) {
+			pr_err("unable to find register: %s\n", reg_names[i]);
+			return -ENODEV;
+		}
+		base++;
+	}
+
+	/* Disable all interrupts and request them */
+	intrl2_0_writel(priv, 0xffffffff, INTRL2_CPU_MASK_SET);
+	intrl2_0_writel(priv, 0xffffffff, INTRL2_CPU_CLEAR);
+	intrl2_0_writel(priv, 0, INTRL2_CPU_MASK_CLEAR);
+	intrl2_1_writel(priv, 0xffffffff, INTRL2_CPU_MASK_SET);
+	intrl2_1_writel(priv, 0xffffffff, INTRL2_CPU_CLEAR);
+	intrl2_1_writel(priv, 0, INTRL2_CPU_MASK_CLEAR);
+
+	ret = request_irq(priv->irq0, bcm_sf2_switch_0_isr, 0,
+			  "switch_0", priv);
+	if (ret < 0) {
+		pr_err("failed to request switch_0 IRQ\n");
+		goto out_unmap;
+	}
+
+	ret = request_irq(priv->irq1, bcm_sf2_switch_1_isr, 0,
+			  "switch_1", priv);
+	if (ret < 0) {
+		pr_err("failed to request switch_1 IRQ\n");
+		goto out_free_irq0;
+	}
+
+	/* Reset the MIB counters */
+	reg = core_readl(priv, CORE_GMNCFGCFG);
+	reg |= RST_MIB_CNT;
+	core_writel(priv, reg, CORE_GMNCFGCFG);
+	reg &= ~RST_MIB_CNT;
+	core_writel(priv, reg, CORE_GMNCFGCFG);
+
+	/* Get the maximum number of ports for this switch */
+	priv->hw_params.num_ports = core_readl(priv, CORE_IMP0_PRT_ID) + 1;
+	if (priv->hw_params.num_ports > DSA_MAX_PORTS)
+		priv->hw_params.num_ports = DSA_MAX_PORTS;
+
+	/* Assume a single GPHY setup if we can't read that property */
+	if (of_property_read_u32(dn, "brcm,num-gphy",
+				 &priv->hw_params.num_gphy))
+		priv->hw_params.num_gphy = 1;
+
+	/* Enable all valid ports and disable those unused */
+	for (port = 0; port < priv->hw_params.num_ports; port++) {
+		/* IMP port receives special treatment */
+		if ((1 << port) & ds->phys_port_mask)
+			bcm_sf2_port_setup(ds, port, NULL);
+		else if (dsa_is_cpu_port(ds, port))
+			bcm_sf2_imp_setup(ds, port);
+		else
+			bcm_sf2_port_disable(ds, port, NULL);
+	}
+
+	/* Include the pseudo-PHY address and the broadcast PHY address to
+	 * divert reads towards our workaround
+	 */
+	ds->phys_mii_mask |= ((1 << 30) | (1 << 0));
+
+	rev = reg_readl(priv, REG_SWITCH_REVISION);
+	priv->hw_params.top_rev = (rev >> SWITCH_TOP_REV_SHIFT) &
+					SWITCH_TOP_REV_MASK;
+	priv->hw_params.core_rev = (rev & SF2_REV_MASK);
+
+	rev = reg_readl(priv, REG_PHY_REVISION);
+	priv->hw_params.gphy_rev = rev & PHY_REVISION_MASK;
+
+	pr_info("Starfighter 2 top: %x.%02x, core: %x.%02x base: 0x%p, IRQs: %d, %d\n",
+		priv->hw_params.top_rev >> 8, priv->hw_params.top_rev & 0xff,
+		priv->hw_params.core_rev >> 8, priv->hw_params.core_rev & 0xff,
+		priv->core, priv->irq0, priv->irq1);
+
+	return 0;
+
+out_free_irq0:
+	free_irq(priv->irq0, priv);
+out_unmap:
+	base = &priv->core;
+	for (i = 0; i < BCM_SF2_REGS_NUM; i++) {
+		iounmap(*base);
+		base++;
+	}
+	return ret;
+}
+
+static int bcm_sf2_sw_set_addr(struct dsa_switch *ds, u8 *addr)
+{
+	return 0;
+}
+
+static u32 bcm_sf2_sw_get_phy_flags(struct dsa_switch *ds, int port)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+
+	/* The BCM7xxx PHY driver expects to find the integrated PHY revision
+	 * in bits 15:8 and the patch level in bits 7:0 which is exactly what
+	 * the REG_PHY_REVISION register layout is.
+	 */
+
+	return priv->hw_params.gphy_rev;
+}
+
+static int bcm_sf2_sw_indir_rw(struct dsa_switch *ds, int op, int addr,
+			       int regnum, u16 val)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	int ret = 0;
+	u32 reg;
+
+	reg = reg_readl(priv, REG_SWITCH_CNTRL);
+	reg |= MDIO_MASTER_SEL;
+	reg_writel(priv, reg, REG_SWITCH_CNTRL);
+
+	/* Page << 8 | offset */
+	reg = 0x70;
+	reg <<= 2;
+	core_writel(priv, addr, reg);
+
+	/* Page << 8 | offset */
+	reg = 0x80 << 8 | regnum << 1;
+	reg <<= 2;
+
+	if (op)
+		ret = core_readl(priv, reg);
+	else
+		core_writel(priv, val, reg);
+
+	reg = reg_readl(priv, REG_SWITCH_CNTRL);
+	reg &= ~MDIO_MASTER_SEL;
+	reg_writel(priv, reg, REG_SWITCH_CNTRL);
+
+	return ret & 0xffff;
+}
+
+static int bcm_sf2_sw_phy_read(struct dsa_switch *ds, int addr, int regnum)
+{
+	/* Intercept reads from the MDIO broadcast address or Broadcom
+	 * pseudo-PHY address
+	 */
+	switch (addr) {
+	case 0:
+	case 30:
+		return bcm_sf2_sw_indir_rw(ds, 1, addr, regnum, 0);
+	default:
+		return 0xffff;
+	}
+}
+
+static int bcm_sf2_sw_phy_write(struct dsa_switch *ds, int addr, int regnum,
+				u16 val)
+{
+	/* Intercept writes to the MDIO broadcast address or Broadcom
+	 * pseudo-PHY address
+	 */
+	switch (addr) {
+	case 0:
+	case 30:
+		bcm_sf2_sw_indir_rw(ds, 0, addr, regnum, val);
+		break;
+	}
+
+	return 0;
+}
+
+static void bcm_sf2_sw_adjust_link(struct dsa_switch *ds, int port,
+				   struct phy_device *phydev)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	u32 id_mode_dis = 0, port_mode;
+	const char *str = NULL;
+	u32 reg;
+
+	switch (phydev->interface) {
+	case PHY_INTERFACE_MODE_RGMII:
+		str = "RGMII (no delay)";
+		id_mode_dis = 1;
+	case PHY_INTERFACE_MODE_RGMII_TXID:
+		if (!str)
+			str = "RGMII (TX delay)";
+		port_mode = EXT_GPHY;
+		break;
+	case PHY_INTERFACE_MODE_MII:
+		str = "MII";
+		port_mode = EXT_EPHY;
+		break;
+	case PHY_INTERFACE_MODE_REVMII:
+		str = "Reverse MII";
+		port_mode = EXT_REVMII;
+		break;
+	default:
+		/* All other PHYs: internal and MoCA */
+		goto force_link;
+	}
+
+	/* If the link is down, just disable the interface to conserve power */
+	if (!phydev->link) {
+		reg = reg_readl(priv, REG_RGMII_CNTRL_P(port));
+		reg &= ~RGMII_MODE_EN;
+		reg_writel(priv, reg, REG_RGMII_CNTRL_P(port));
+		goto force_link;
+	}
+
+	/* Clear id_mode_dis bit, and the existing port mode, but
+	 * make sure we enable the RGMII block for data to pass
+	 */
+	reg = reg_readl(priv, REG_RGMII_CNTRL_P(port));
+	reg &= ~ID_MODE_DIS;
+	reg &= ~(PORT_MODE_MASK << PORT_MODE_SHIFT);
+	reg &= ~(RX_PAUSE_EN | TX_PAUSE_EN);
+
+	reg |= port_mode | RGMII_MODE_EN;
+	if (id_mode_dis)
+		reg |= ID_MODE_DIS;
+
+	if (phydev->pause) {
+		if (phydev->asym_pause)
+			reg |= TX_PAUSE_EN;
+		reg |= RX_PAUSE_EN;
+	}
+
+	reg_writel(priv, reg, REG_RGMII_CNTRL_P(port));
+
+	pr_info("Port %d configured for %s\n", port, str);
+
+force_link:
+	/* Force link settings detected from the PHY */
+	reg = SW_OVERRIDE;
+	switch (phydev->speed) {
+	case SPEED_1000:
+		reg |= SPDSTS_1000 << SPEED_SHIFT;
+		break;
+	case SPEED_100:
+		reg |= SPDSTS_100 << SPEED_SHIFT;
+		break;
+	}
+
+	if (phydev->link)
+		reg |= LINK_STS;
+	if (phydev->duplex == DUPLEX_FULL)
+		reg |= DUPLX_MODE;
+
+	core_writel(priv, reg, CORE_STS_OVERRIDE_GMIIP_PORT(port));
+}
+
+static void bcm_sf2_sw_fixed_link_update(struct dsa_switch *ds, int port,
+					 struct fixed_phy_status *status)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	u32 link, duplex, pause, speed;
+	u32 reg;
+
+	link = core_readl(priv, CORE_LNKSTS);
+	duplex = core_readl(priv, CORE_DUPSTS);
+	pause = core_readl(priv, CORE_PAUSESTS);
+	speed = core_readl(priv, CORE_SPDSTS);
+
+	speed >>= (port * SPDSTS_SHIFT);
+	speed &= SPDSTS_MASK;
+
+	status->link = 0;
+
+	/* Port 7 is special as we do not get link status from CORE_LNKSTS,
+	 * which means that we need to force the link at the port override
+	 * level to get the data to flow. We do use what the interrupt handler
+	 * did determine before.
+	 */
+	if (port == 7) {
+		status->link = priv->port_sts[port].link;
+		reg = core_readl(priv, CORE_STS_OVERRIDE_GMIIP_PORT(7));
+		reg |= SW_OVERRIDE;
+		if (status->link)
+			reg |= LINK_STS;
+		else
+			reg &= ~LINK_STS;
+		core_writel(priv, reg, CORE_STS_OVERRIDE_GMIIP_PORT(7));
+		status->duplex = 1;
+	} else {
+		status->link = !!(link & (1 << port));
+		status->duplex = !!(duplex & (1 << port));
+	}
+
+	switch (speed) {
+	case SPDSTS_10:
+		status->speed = SPEED_10;
+		break;
+	case SPDSTS_100:
+		status->speed = SPEED_100;
+		break;
+	case SPDSTS_1000:
+		status->speed = SPEED_1000;
+		break;
+	}
+
+	if ((pause & (1 << port)) &&
+	    (pause & (1 << (port + PAUSESTS_TX_PAUSE_SHIFT)))) {
+		status->asym_pause = 1;
+		status->pause = 1;
+	}
+
+	if (pause & (1 << port))
+		status->pause = 1;
+}
+
+static int bcm_sf2_sw_suspend(struct dsa_switch *ds)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	unsigned int port;
+
+	intrl2_0_writel(priv, 0xffffffff, INTRL2_CPU_MASK_SET);
+	intrl2_0_writel(priv, 0xffffffff, INTRL2_CPU_CLEAR);
+	intrl2_0_writel(priv, 0, INTRL2_CPU_MASK_CLEAR);
+	intrl2_1_writel(priv, 0xffffffff, INTRL2_CPU_MASK_SET);
+	intrl2_1_writel(priv, 0xffffffff, INTRL2_CPU_CLEAR);
+	intrl2_1_writel(priv, 0, INTRL2_CPU_MASK_CLEAR);
+
+	/* Disable all ports physically present including the IMP
+	 * port, the other ones have already been disabled during
+	 * bcm_sf2_sw_setup
+	 */
+	for (port = 0; port < DSA_MAX_PORTS; port++) {
+		if ((1 << port) & ds->phys_port_mask ||
+		    dsa_is_cpu_port(ds, port))
+			bcm_sf2_port_disable(ds, port, NULL);
+	}
+
+	return 0;
+}
+
+static int bcm_sf2_sw_rst(struct bcm_sf2_priv *priv)
+{
+	unsigned int timeout = 1000;
+	u32 reg;
+
+	reg = core_readl(priv, CORE_WATCHDOG_CTRL);
+	reg |= SOFTWARE_RESET | EN_CHIP_RST | EN_SW_RESET;
+	core_writel(priv, reg, CORE_WATCHDOG_CTRL);
+
+	do {
+		reg = core_readl(priv, CORE_WATCHDOG_CTRL);
+		if (!(reg & SOFTWARE_RESET))
+			break;
+
+		usleep_range(1000, 2000);
+	} while (timeout-- > 0);
+
+	if (timeout == 0)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int bcm_sf2_sw_resume(struct dsa_switch *ds)
+{
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	unsigned int port;
+	u32 reg;
+	int ret;
+
+	ret = bcm_sf2_sw_rst(priv);
+	if (ret) {
+		pr_err("%s: failed to software reset switch\n", __func__);
+		return ret;
+	}
+
+	/* Reinitialize the single GPHY */
+	if (priv->hw_params.num_gphy == 1) {
+		reg = reg_readl(priv, REG_SPHY_CNTRL);
+		reg |= PHY_RESET;
+		reg &= ~(EXT_PWR_DOWN | IDDQ_BIAS);
+		reg_writel(priv, reg, REG_SPHY_CNTRL);
+		udelay(21);
+		reg = reg_readl(priv, REG_SPHY_CNTRL);
+		reg &= ~PHY_RESET;
+		reg_writel(priv, reg, REG_SPHY_CNTRL);
+	}
+
+	for (port = 0; port < DSA_MAX_PORTS; port++) {
+		if ((1 << port) & ds->phys_port_mask)
+			bcm_sf2_port_setup(ds, port, NULL);
+		else if (dsa_is_cpu_port(ds, port))
+			bcm_sf2_imp_setup(ds, port);
+	}
+
+	return 0;
+}
+
+static void bcm_sf2_sw_get_wol(struct dsa_switch *ds, int port,
+			       struct ethtool_wolinfo *wol)
+{
+	struct net_device *p = ds->dst[ds->index].master_netdev;
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	struct ethtool_wolinfo pwol;
+
+	/* Get the parent device WoL settings */
+	p->ethtool_ops->get_wol(p, &pwol);
+
+	/* Advertise the parent device supported settings */
+	wol->supported = pwol.supported;
+	memset(&wol->sopass, 0, sizeof(wol->sopass));
+
+	if (pwol.wolopts & WAKE_MAGICSECURE)
+		memcpy(&wol->sopass, pwol.sopass, sizeof(wol->sopass));
+
+	if (priv->wol_ports_mask & (1 << port))
+		wol->wolopts = pwol.wolopts;
+	else
+		wol->wolopts = 0;
+}
+
+static int bcm_sf2_sw_set_wol(struct dsa_switch *ds, int port,
+			      struct ethtool_wolinfo *wol)
+{
+	struct net_device *p = ds->dst[ds->index].master_netdev;
+	struct bcm_sf2_priv *priv = ds_to_priv(ds);
+	s8 cpu_port = ds->dst[ds->index].cpu_port;
+	struct ethtool_wolinfo pwol;
+
+	p->ethtool_ops->get_wol(p, &pwol);
+	if (wol->wolopts & ~pwol.supported)
+		return -EINVAL;
+
+	if (wol->wolopts)
+		priv->wol_ports_mask |= (1 << port);
+	else
+		priv->wol_ports_mask &= ~(1 << port);
+
+	/* If we have at least one port enabled, make sure the CPU port
+	 * is also enabled. If the CPU port is the last one enabled, we disable
+	 * it since this configuration does not make sense.
+	 */
+	if (priv->wol_ports_mask && priv->wol_ports_mask != (1 << cpu_port))
+		priv->wol_ports_mask |= (1 << cpu_port);
+	else
+		priv->wol_ports_mask &= ~(1 << cpu_port);
+
+	return p->ethtool_ops->set_wol(p, wol);
+}
+
+static struct dsa_switch_driver bcm_sf2_switch_driver = {
+	.tag_protocol		= DSA_TAG_PROTO_BRCM,
+	.priv_size		= sizeof(struct bcm_sf2_priv),
+	.probe			= bcm_sf2_sw_probe,
+	.setup			= bcm_sf2_sw_setup,
+	.set_addr		= bcm_sf2_sw_set_addr,
+	.get_phy_flags		= bcm_sf2_sw_get_phy_flags,
+	.phy_read		= bcm_sf2_sw_phy_read,
+	.phy_write		= bcm_sf2_sw_phy_write,
+	.get_strings		= bcm_sf2_sw_get_strings,
+	.get_ethtool_stats	= bcm_sf2_sw_get_ethtool_stats,
+	.get_sset_count		= bcm_sf2_sw_get_sset_count,
+	.adjust_link		= bcm_sf2_sw_adjust_link,
+	.fixed_link_update	= bcm_sf2_sw_fixed_link_update,
+	.suspend		= bcm_sf2_sw_suspend,
+	.resume			= bcm_sf2_sw_resume,
+	.get_wol		= bcm_sf2_sw_get_wol,
+	.set_wol		= bcm_sf2_sw_set_wol,
+	.port_enable		= bcm_sf2_port_setup,
+	.port_disable		= bcm_sf2_port_disable,
+	.get_eee		= bcm_sf2_sw_get_eee,
+	.set_eee		= bcm_sf2_sw_set_eee,
+};
+
+static int __init bcm_sf2_init(void)
+{
+	register_switch_driver(&bcm_sf2_switch_driver);
+
+	return 0;
+}
+module_init(bcm_sf2_init);
+
+static void __exit bcm_sf2_exit(void)
+{
+	unregister_switch_driver(&bcm_sf2_switch_driver);
+}
+module_exit(bcm_sf2_exit);
+
+MODULE_AUTHOR("Broadcom Corporation");
+MODULE_DESCRIPTION("Driver for Broadcom Starfighter 2 ethernet switch chip");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:brcm-sf2");
diff --git a/drivers/net/dsa/bcm_sf2.h b/drivers/net/dsa/bcm_sf2.h
new file mode 100644
index 000000000000..ee9f650d5026
--- /dev/null
+++ b/drivers/net/dsa/bcm_sf2.h
@@ -0,0 +1,147 @@
+/*
+ * Broadcom Starfighter2 private context
+ *
+ * Copyright (C) 2014, Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef __BCM_SF2_H
+#define __BCM_SF2_H
+
+#include <linux/platform_device.h>
+#include <linux/kernel.h>
+#include <linux/io.h>
+#include <linux/spinlock.h>
+#include <linux/mutex.h>
+#include <linux/mii.h>
+#include <linux/ethtool.h>
+
+#include <net/dsa.h>
+
+#include "bcm_sf2_regs.h"
+
+struct bcm_sf2_hw_params {
+	u16	top_rev;
+	u16	core_rev;
+	u16	gphy_rev;
+	u32	num_gphy;
+	u8	num_acb_queue;
+	u8	num_rgmii;
+	u8	num_ports;
+	u8	fcb_pause_override:1;
+	u8	acb_packets_inflight:1;
+};
+
+#define BCM_SF2_REGS_NAME {\
+	"core", "reg", "intrl2_0", "intrl2_1", "fcb", "acb" \
+}
+
+#define BCM_SF2_REGS_NUM	6
+
+struct bcm_sf2_port_status {
+	unsigned int link;
+
+	struct ethtool_eee eee;
+};
+
+struct bcm_sf2_priv {
+	/* Base registers, keep those in order with BCM_SF2_REGS_NAME */
+	void __iomem			*core;
+	void __iomem			*reg;
+	void __iomem			*intrl2_0;
+	void __iomem			*intrl2_1;
+	void __iomem			*fcb;
+	void __iomem			*acb;
+
+	/* spinlock protecting access to the indirect registers */
+	spinlock_t			indir_lock;
+
+	int				irq0;
+	int				irq1;
+	u32				irq0_stat;
+	u32				irq0_mask;
+	u32				irq1_stat;
+	u32				irq1_mask;
+
+	/* Mutex protecting access to the MIB counters */
+	struct mutex			stats_mutex;
+
+	struct bcm_sf2_hw_params	hw_params;
+
+	struct bcm_sf2_port_status	port_sts[DSA_MAX_PORTS];
+
+	/* Mask of ports enabled for Wake-on-LAN */
+	u32				wol_ports_mask;
+};
+
+struct bcm_sf2_hw_stats {
+	const char	*string;
+	u16		reg;
+	u8		sizeof_stat;
+};
+
+#define SF2_IO_MACRO(name) \
+static inline u32 name##_readl(struct bcm_sf2_priv *priv, u32 off)	\
+{									\
+	return __raw_readl(priv->name + off);				\
+}									\
+static inline void name##_writel(struct bcm_sf2_priv *priv,		\
+				  u32 val, u32 off)			\
+{									\
+	__raw_writel(val, priv->name + off);				\
+}									\
+
+/* Accesses to 64-bits register requires us to latch the hi/lo pairs
+ * using the REG_DIR_DATA_{READ,WRITE} ancillary registers. The 'indir_lock'
+ * spinlock is automatically grabbed and released to provide relative
+ * atomiticy with latched reads/writes.
+ */
+#define SF2_IO64_MACRO(name) \
+static inline u64 name##_readq(struct bcm_sf2_priv *priv, u32 off)	\
+{									\
+	u32 indir, dir;							\
+	spin_lock(&priv->indir_lock);					\
+	indir = reg_readl(priv, REG_DIR_DATA_READ);			\
+	dir = __raw_readl(priv->name + off);				\
+	spin_unlock(&priv->indir_lock);					\
+	return (u64)indir << 32 | dir;					\
+}									\
+static inline void name##_writeq(struct bcm_sf2_priv *priv, u32 off,	\
+							u64 val)	\
+{									\
+	spin_lock(&priv->indir_lock);					\
+	reg_writel(priv, upper_32_bits(val), REG_DIR_DATA_WRITE);	\
+	__raw_writel(lower_32_bits(val), priv->name + off);		\
+	spin_unlock(&priv->indir_lock);					\
+}
+
+#define SWITCH_INTR_L2(which)						\
+static inline void intrl2_##which##_mask_clear(struct bcm_sf2_priv *priv, \
+						u32 mask)		\
+{									\
+	intrl2_##which##_writel(priv, mask, INTRL2_CPU_MASK_CLEAR);	\
+	priv->irq##which##_mask &= ~(mask);				\
+}									\
+static inline void intrl2_##which##_mask_set(struct bcm_sf2_priv *priv, \
+						u32 mask)		\
+{									\
+	intrl2_## which##_writel(priv, mask, INTRL2_CPU_MASK_SET);	\
+	priv->irq##which##_mask |= (mask);				\
+}									\
+
+SF2_IO_MACRO(core);
+SF2_IO_MACRO(reg);
+SF2_IO64_MACRO(core);
+SF2_IO_MACRO(intrl2_0);
+SF2_IO_MACRO(intrl2_1);
+SF2_IO_MACRO(fcb);
+SF2_IO_MACRO(acb);
+
+SWITCH_INTR_L2(0);
+SWITCH_INTR_L2(1);
+
+#endif /* __BCM_SF2_H */
diff --git a/drivers/net/dsa/bcm_sf2_regs.h b/drivers/net/dsa/bcm_sf2_regs.h
new file mode 100644
index 000000000000..1bb49cb699ab
--- /dev/null
+++ b/drivers/net/dsa/bcm_sf2_regs.h
@@ -0,0 +1,231 @@
+/*
+ * Broadcom Starfighter 2 switch register defines
+ *
+ * Copyright (C) 2014, Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+#ifndef __BCM_SF2_REGS_H
+#define __BCM_SF2_REGS_H
+
+/* Register set relative to 'REG' */
+#define REG_SWITCH_CNTRL		0x00
+#define  MDIO_MASTER_SEL		(1 << 0)
+
+#define REG_SWITCH_STATUS		0x04
+#define REG_DIR_DATA_WRITE		0x08
+#define REG_DIR_DATA_READ		0x0C
+
+#define REG_SWITCH_REVISION		0x18
+#define  SF2_REV_MASK			0xffff
+#define  SWITCH_TOP_REV_SHIFT		16
+#define  SWITCH_TOP_REV_MASK		0xffff
+
+#define REG_PHY_REVISION		0x1C
+#define  PHY_REVISION_MASK		0xffff
+
+#define REG_SPHY_CNTRL			0x2C
+#define  IDDQ_BIAS			(1 << 0)
+#define  EXT_PWR_DOWN			(1 << 1)
+#define  FORCE_DLL_EN			(1 << 2)
+#define  IDDQ_GLOBAL_PWR		(1 << 3)
+#define  CK25_DIS			(1 << 4)
+#define  PHY_RESET			(1 << 5)
+#define  PHY_PHYAD_SHIFT		8
+#define  PHY_PHYAD_MASK			0x1F
+
+#define REG_RGMII_0_BASE		0x34
+#define REG_RGMII_CNTRL			0x00
+#define REG_RGMII_IB_STATUS		0x04
+#define REG_RGMII_RX_CLOCK_DELAY_CNTRL	0x08
+#define REG_RGMII_CNTRL_SIZE		0x0C
+#define REG_RGMII_CNTRL_P(x)		(REG_RGMII_0_BASE + \
+					((x) * REG_RGMII_CNTRL_SIZE))
+/* Relative to REG_RGMII_CNTRL */
+#define  RGMII_MODE_EN			(1 << 0)
+#define  ID_MODE_DIS			(1 << 1)
+#define  PORT_MODE_SHIFT		2
+#define  INT_EPHY			(0 << PORT_MODE_SHIFT)
+#define  INT_GPHY			(1 << PORT_MODE_SHIFT)
+#define  EXT_EPHY			(2 << PORT_MODE_SHIFT)
+#define  EXT_GPHY			(3 << PORT_MODE_SHIFT)
+#define  EXT_REVMII			(4 << PORT_MODE_SHIFT)
+#define  PORT_MODE_MASK			0x7
+#define  RVMII_REF_SEL			(1 << 5)
+#define  RX_PAUSE_EN			(1 << 6)
+#define  TX_PAUSE_EN			(1 << 7)
+#define  TX_CLK_STOP_EN			(1 << 8)
+#define  LPI_COUNT_SHIFT		9
+#define  LPI_COUNT_MASK			0x3F
+
+/* Register set relative to 'INTRL2_0' and 'INTRL2_1' */
+#define INTRL2_CPU_STATUS		0x00
+#define INTRL2_CPU_SET			0x04
+#define INTRL2_CPU_CLEAR		0x08
+#define INTRL2_CPU_MASK_STATUS		0x0c
+#define INTRL2_CPU_MASK_SET		0x10
+#define INTRL2_CPU_MASK_CLEAR		0x14
+
+/* Shared INTRL2_0 and INTRL2_ interrupt sources macros */
+#define P_LINK_UP_IRQ(x)		(1 << (0 + (x)))
+#define P_LINK_DOWN_IRQ(x)		(1 << (1 + (x)))
+#define P_ENERGY_ON_IRQ(x)		(1 << (2 + (x)))
+#define P_ENERGY_OFF_IRQ(x)		(1 << (3 + (x)))
+#define P_GPHY_IRQ(x)			(1 << (4 + (x)))
+#define P_NUM_IRQ			5
+#define P_IRQ_MASK(x)			(P_LINK_UP_IRQ((x)) | \
+					 P_LINK_DOWN_IRQ((x)) | \
+					 P_ENERGY_ON_IRQ((x)) | \
+					 P_ENERGY_OFF_IRQ((x)) | \
+					 P_GPHY_IRQ((x)))
+
+/* INTRL2_0 interrupt sources */
+#define P0_IRQ_OFF			0
+#define MEM_DOUBLE_IRQ			(1 << 5)
+#define EEE_LPI_IRQ			(1 << 6)
+#define P5_CPU_WAKE_IRQ			(1 << 7)
+#define P8_CPU_WAKE_IRQ			(1 << 8)
+#define P7_CPU_WAKE_IRQ			(1 << 9)
+#define IEEE1588_IRQ			(1 << 10)
+#define MDIO_ERR_IRQ			(1 << 11)
+#define MDIO_DONE_IRQ			(1 << 12)
+#define GISB_ERR_IRQ			(1 << 13)
+#define UBUS_ERR_IRQ			(1 << 14)
+#define FAILOVER_ON_IRQ			(1 << 15)
+#define FAILOVER_OFF_IRQ		(1 << 16)
+#define TCAM_SOFT_ERR_IRQ		(1 << 17)
+
+/* INTRL2_1 interrupt sources */
+#define P7_IRQ_OFF			0
+#define P_IRQ_OFF(x)			((6 - (x)) * P_NUM_IRQ)
+
+/* Register set relative to 'CORE' */
+#define CORE_G_PCTL_PORT0		0x00000
+#define CORE_G_PCTL_PORT(x)		(CORE_G_PCTL_PORT0 + (x * 0x4))
+#define CORE_IMP_CTL			0x00020
+#define  RX_DIS				(1 << 0)
+#define  TX_DIS				(1 << 1)
+#define  RX_BCST_EN			(1 << 2)
+#define  RX_MCST_EN			(1 << 3)
+#define  RX_UCST_EN			(1 << 4)
+#define  G_MISTP_STATE_SHIFT		5
+#define  G_MISTP_NO_STP			(0 << G_MISTP_STATE_SHIFT)
+#define  G_MISTP_DIS_STATE		(1 << G_MISTP_STATE_SHIFT)
+#define  G_MISTP_BLOCK_STATE		(2 << G_MISTP_STATE_SHIFT)
+#define  G_MISTP_LISTEN_STATE		(3 << G_MISTP_STATE_SHIFT)
+#define  G_MISTP_LEARN_STATE		(4 << G_MISTP_STATE_SHIFT)
+#define  G_MISTP_FWD_STATE		(5 << G_MISTP_STATE_SHIFT)
+#define  G_MISTP_STATE_MASK		0x7
+
+#define CORE_SWMODE			0x0002c
+#define  SW_FWDG_MODE			(1 << 0)
+#define  SW_FWDG_EN			(1 << 1)
+#define  RTRY_LMT_DIS			(1 << 2)
+
+#define CORE_STS_OVERRIDE_IMP		0x00038
+#define  GMII_SPEED_UP_2G		(1 << 6)
+#define  MII_SW_OR			(1 << 7)
+
+#define CORE_NEW_CTRL			0x00084
+#define  IP_MC				(1 << 0)
+#define  OUTRANGEERR_DISCARD		(1 << 1)
+#define  INRANGEERR_DISCARD		(1 << 2)
+#define  CABLE_DIAG_LEN			(1 << 3)
+#define  OVERRIDE_AUTO_PD_WAR		(1 << 4)
+#define  EN_AUTO_PD_WAR			(1 << 5)
+#define  UC_FWD_EN			(1 << 6)
+#define  MC_FWD_EN			(1 << 7)
+
+#define CORE_SWITCH_CTRL		0x00088
+#define  MII_DUMB_FWDG_EN		(1 << 6)
+
+#define CORE_SFT_LRN_CTRL		0x000f8
+#define  SW_LEARN_CNTL(x)		(1 << (x))
+
+#define CORE_STS_OVERRIDE_GMIIP_PORT(x)	(0x160 + (x) * 4)
+#define  LINK_STS			(1 << 0)
+#define  DUPLX_MODE			(1 << 1)
+#define  SPEED_SHIFT			2
+#define  SPEED_MASK			0x3
+#define  RXFLOW_CNTL			(1 << 4)
+#define  TXFLOW_CNTL			(1 << 5)
+#define  SW_OVERRIDE			(1 << 6)
+
+#define CORE_WATCHDOG_CTRL		0x001e4
+#define  SOFTWARE_RESET			(1 << 7)
+#define  EN_CHIP_RST			(1 << 6)
+#define  EN_SW_RESET			(1 << 4)
+
+#define CORE_LNKSTS			0x00400
+#define  LNK_STS_MASK			0x1ff
+
+#define CORE_SPDSTS			0x00410
+#define  SPDSTS_10			0
+#define  SPDSTS_100			1
+#define  SPDSTS_1000			2
+#define  SPDSTS_SHIFT			2
+#define  SPDSTS_MASK			0x3
+
+#define CORE_DUPSTS			0x00420
+#define  CORE_DUPSTS_MASK		0x1ff
+
+#define CORE_PAUSESTS			0x00428
+#define  PAUSESTS_TX_PAUSE_SHIFT	9
+
+#define CORE_GMNCFGCFG			0x0800
+#define  RST_MIB_CNT			(1 << 0)
+#define  RXBPDU_EN			(1 << 1)
+
+#define CORE_IMP0_PRT_ID		0x0804
+
+#define CORE_BRCM_HDR_CTRL		0x0080c
+#define  BRCM_HDR_EN_P8			(1 << 0)
+#define  BRCM_HDR_EN_P5			(1 << 1)
+#define  BRCM_HDR_EN_P7			(1 << 2)
+
+#define CORE_BRCM_HDR_CTRL2		0x0828
+
+#define CORE_HL_PRTC_CTRL		0x0940
+#define  ARP_EN				(1 << 0)
+#define  RARP_EN			(1 << 1)
+#define  DHCP_EN			(1 << 2)
+#define  ICMPV4_EN			(1 << 3)
+#define  ICMPV6_EN			(1 << 4)
+#define  ICMPV6_FWD_MODE		(1 << 5)
+#define  IGMP_DIP_EN			(1 << 8)
+#define  IGMP_RPTLVE_EN			(1 << 9)
+#define  IGMP_RTPLVE_FWD_MODE		(1 << 10)
+#define  IGMP_QRY_EN			(1 << 11)
+#define  IGMP_QRY_FWD_MODE		(1 << 12)
+#define  IGMP_UKN_EN			(1 << 13)
+#define  IGMP_UKN_FWD_MODE		(1 << 14)
+#define  MLD_RPTDONE_EN			(1 << 15)
+#define  MLD_RPTDONE_FWD_MODE		(1 << 16)
+#define  MLD_QRY_EN			(1 << 17)
+#define  MLD_QRY_FWD_MODE		(1 << 18)
+
+#define CORE_RST_MIB_CNT_EN		0x0950
+
+#define CORE_BRCM_HDR_RX_DIS		0x0980
+#define CORE_BRCM_HDR_TX_DIS		0x0988
+
+#define CORE_MEM_PSM_VDD_CTRL		0x2380
+#define  P_TXQ_PSM_VDD_SHIFT		2
+#define  P_TXQ_PSM_VDD_MASK		0x3
+#define  P_TXQ_PSM_VDD(x)		(P_TXQ_PSM_VDD_MASK << \
+					((x) * P_TXQ_PSM_VDD_SHIFT))
+
+#define	CORE_P0_MIB_OFFSET		0x8000
+#define P_MIB_SIZE			0x400
+#define CORE_P_MIB_OFFSET(x)		(CORE_P0_MIB_OFFSET + (x) * P_MIB_SIZE)
+
+#define CORE_PORT_VLAN_CTL_PORT(x)	(0xc400 + ((x) * 0x8))
+#define  PORT_VLAN_CTRL_MASK		0x1ff
+
+#define CORE_EEE_EN_CTRL		0x24800
+#define CORE_EEE_LPI_INDICATE		0x24810
+
+#endif /* __BCM_SF2_REGS_H */
diff --git a/drivers/net/dsa/mv88e6060.c b/drivers/net/dsa/mv88e6060.c
index 7a54ec04b418..05b0ca3bf71d 100644
--- a/drivers/net/dsa/mv88e6060.c
+++ b/drivers/net/dsa/mv88e6060.c
@@ -21,7 +21,12 @@
 
 static int reg_read(struct dsa_switch *ds, int addr, int reg)
 {
-	return mdiobus_read(ds->master_mii_bus, ds->pd->sw_addr + addr, reg);
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
+
+	if (bus == NULL)
+		return -EINVAL;
+
+	return mdiobus_read(bus, ds->pd->sw_addr + addr, reg);
 }
 
 #define REG_READ(addr, reg)					\
@@ -37,8 +42,12 @@ static int reg_read(struct dsa_switch *ds, int addr, int reg)
 
 static int reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 {
-	return mdiobus_write(ds->master_mii_bus, ds->pd->sw_addr + addr,
-			     reg, val);
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
+
+	if (bus == NULL)
+		return -EINVAL;
+
+	return mdiobus_write(bus, ds->pd->sw_addr + addr, reg, val);
 }
 
 #define REG_WRITE(addr, reg, val)				\
@@ -50,10 +59,14 @@ static int reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 			return __ret;				\
 	})
 
-static char *mv88e6060_probe(struct mii_bus *bus, int sw_addr)
+static char *mv88e6060_probe(struct device *host_dev, int sw_addr)
 {
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
 	int ret;
 
+	if (bus == NULL)
+		return NULL;
+
 	ret = mdiobus_read(bus, sw_addr + REG_PORT(0), 0x03);
 	if (ret >= 0) {
 		ret &= 0xfff0;
@@ -258,7 +271,7 @@ static void mv88e6060_poll_link(struct dsa_switch *ds)
 }
 
 static struct dsa_switch_driver mv88e6060_switch_driver = {
-	.tag_protocol	= htons(ETH_P_TRAILER),
+	.tag_protocol	= DSA_TAG_PROTO_TRAILER,
 	.probe		= mv88e6060_probe,
 	.setup		= mv88e6060_setup,
 	.set_addr	= mv88e6060_set_addr,
diff --git a/drivers/net/dsa/mv88e6123_61_65.c b/drivers/net/dsa/mv88e6123_61_65.c
index 69c42513dd72..a332c53ff955 100644
--- a/drivers/net/dsa/mv88e6123_61_65.c
+++ b/drivers/net/dsa/mv88e6123_61_65.c
@@ -17,10 +17,14 @@
 #include <net/dsa.h>
 #include "mv88e6xxx.h"
 
-static char *mv88e6123_61_65_probe(struct mii_bus *bus, int sw_addr)
+static char *mv88e6123_61_65_probe(struct device *host_dev, int sw_addr)
 {
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
 	int ret;
 
+	if (bus == NULL)
+		return NULL;
+
 	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
 	if (ret >= 0) {
 		if (ret == 0x1212)
@@ -207,7 +211,7 @@ static int mv88e6123_61_65_setup_port(struct dsa_switch *ds, int p)
 	 */
 	val = 0x0433;
 	if (dsa_is_cpu_port(ds, p)) {
-		if (ds->dst->tag_protocol == htons(ETH_P_EDSA))
+		if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
 			val |= 0x3300;
 		else
 			val |= 0x0100;
@@ -391,7 +395,7 @@ static int mv88e6123_61_65_get_sset_count(struct dsa_switch *ds)
 }
 
 struct dsa_switch_driver mv88e6123_61_65_switch_driver = {
-	.tag_protocol		= cpu_to_be16(ETH_P_EDSA),
+	.tag_protocol		= DSA_TAG_PROTO_EDSA,
 	.priv_size		= sizeof(struct mv88e6xxx_priv_state),
 	.probe			= mv88e6123_61_65_probe,
 	.setup			= mv88e6123_61_65_setup,
diff --git a/drivers/net/dsa/mv88e6131.c b/drivers/net/dsa/mv88e6131.c
index 953bc6a49e59..244c735014fa 100644
--- a/drivers/net/dsa/mv88e6131.c
+++ b/drivers/net/dsa/mv88e6131.c
@@ -22,10 +22,14 @@
 #define ID_6095		0x0950
 #define ID_6131		0x1060
 
-static char *mv88e6131_probe(struct mii_bus *bus, int sw_addr)
+static char *mv88e6131_probe(struct device *host_dev, int sw_addr)
 {
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
 	int ret;
 
+	if (bus == NULL)
+		return NULL;
+
 	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
 	if (ret >= 0) {
 		ret &= 0xfff0;
@@ -379,7 +383,7 @@ static int mv88e6131_get_sset_count(struct dsa_switch *ds)
 }
 
 struct dsa_switch_driver mv88e6131_switch_driver = {
-	.tag_protocol		= cpu_to_be16(ETH_P_DSA),
+	.tag_protocol		= DSA_TAG_PROTO_DSA,
 	.priv_size		= sizeof(struct mv88e6xxx_priv_state),
 	.probe			= mv88e6131_probe,
 	.setup			= mv88e6131_setup,
diff --git a/drivers/net/dsa/mv88e6171.c b/drivers/net/dsa/mv88e6171.c
new file mode 100644
index 000000000000..1020a7af67cf
--- /dev/null
+++ b/drivers/net/dsa/mv88e6171.c
@@ -0,0 +1,411 @@
+/* net/dsa/mv88e6171.c - Marvell 88e6171 switch chip support
+ * Copyright (c) 2008-2009 Marvell Semiconductor
+ * Copyright (c) 2014 Claudio Leite <leitec@staticky.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/jiffies.h>
+#include <linux/list.h>
+#include <linux/module.h>
+#include <linux/netdevice.h>
+#include <linux/phy.h>
+#include <net/dsa.h>
+#include "mv88e6xxx.h"
+
+static char *mv88e6171_probe(struct device *host_dev, int sw_addr)
+{
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(host_dev);
+	int ret;
+
+	if (bus == NULL)
+		return NULL;
+
+	ret = __mv88e6xxx_reg_read(bus, sw_addr, REG_PORT(0), 0x03);
+	if (ret >= 0) {
+		if ((ret & 0xfff0) == 0x1710)
+			return "Marvell 88E6171";
+	}
+
+	return NULL;
+}
+
+static int mv88e6171_switch_reset(struct dsa_switch *ds)
+{
+	int i;
+	int ret;
+	unsigned long timeout;
+
+	/* Set all ports to the disabled state. */
+	for (i = 0; i < 8; i++) {
+		ret = REG_READ(REG_PORT(i), 0x04);
+		REG_WRITE(REG_PORT(i), 0x04, ret & 0xfffc);
+	}
+
+	/* Wait for transmit queues to drain. */
+	usleep_range(2000, 4000);
+
+	/* Reset the switch. */
+	REG_WRITE(REG_GLOBAL, 0x04, 0xc400);
+
+	/* Wait up to one second for reset to complete. */
+	timeout = jiffies + 1 * HZ;
+	while (time_before(jiffies, timeout)) {
+		ret = REG_READ(REG_GLOBAL, 0x00);
+		if ((ret & 0xc800) == 0xc800)
+			break;
+
+		usleep_range(1000, 2000);
+	}
+	if (time_after(jiffies, timeout))
+		return -ETIMEDOUT;
+
+	/* Enable ports not under DSA, e.g. WAN port */
+	for (i = 0; i < 8; i++) {
+		if (dsa_is_cpu_port(ds, i) || ds->phys_port_mask & (1 << i))
+			continue;
+
+		ret = REG_READ(REG_PORT(i), 0x04);
+		REG_WRITE(REG_PORT(i), 0x04, ret | 0x03);
+	}
+
+	return 0;
+}
+
+static int mv88e6171_setup_global(struct dsa_switch *ds)
+{
+	int ret;
+	int i;
+
+	/* Disable the PHY polling unit (since there won't be any
+	 * external PHYs to poll), don't discard packets with
+	 * excessive collisions, and mask all interrupt sources.
+	 */
+	REG_WRITE(REG_GLOBAL, 0x04, 0x0000);
+
+	/* Set the default address aging time to 5 minutes, and
+	 * enable address learn messages to be sent to all message
+	 * ports.
+	 */
+	REG_WRITE(REG_GLOBAL, 0x0a, 0x0148);
+
+	/* Configure the priority mapping registers. */
+	ret = mv88e6xxx_config_prio(ds);
+	if (ret < 0)
+		return ret;
+
+	/* Configure the upstream port, and configure the upstream
+	 * port as the port to which ingress and egress monitor frames
+	 * are to be sent.
+	 */
+	if (REG_READ(REG_PORT(0), 0x03) == 0x1710)
+		REG_WRITE(REG_GLOBAL, 0x1a, (dsa_upstream_port(ds) * 0x1111));
+	else
+		REG_WRITE(REG_GLOBAL, 0x1a, (dsa_upstream_port(ds) * 0x1110));
+
+	/* Disable remote management for now, and set the switch's
+	 * DSA device number.
+	 */
+	REG_WRITE(REG_GLOBAL, 0x1c, ds->index & 0x1f);
+
+	/* Send all frames with destination addresses matching
+	 * 01:80:c2:00:00:2x to the CPU port.
+	 */
+	REG_WRITE(REG_GLOBAL2, 0x02, 0xffff);
+
+	/* Send all frames with destination addresses matching
+	 * 01:80:c2:00:00:0x to the CPU port.
+	 */
+	REG_WRITE(REG_GLOBAL2, 0x03, 0xffff);
+
+	/* Disable the loopback filter, disable flow control
+	 * messages, disable flood broadcast override, disable
+	 * removing of provider tags, disable ATU age violation
+	 * interrupts, disable tag flow control, force flow
+	 * control priority to the highest, and send all special
+	 * multicast frames to the CPU at the highest priority.
+	 */
+	REG_WRITE(REG_GLOBAL2, 0x05, 0x00ff);
+
+	/* Program the DSA routing table. */
+	for (i = 0; i < 32; i++) {
+		int nexthop;
+
+		nexthop = 0x1f;
+		if (i != ds->index && i < ds->dst->pd->nr_chips)
+			nexthop = ds->pd->rtable[i] & 0x1f;
+
+		REG_WRITE(REG_GLOBAL2, 0x06, 0x8000 | (i << 8) | nexthop);
+	}
+
+	/* Clear all trunk masks. */
+	for (i = 0; i < 8; i++)
+		REG_WRITE(REG_GLOBAL2, 0x07, 0x8000 | (i << 12) | 0xff);
+
+	/* Clear all trunk mappings. */
+	for (i = 0; i < 16; i++)
+		REG_WRITE(REG_GLOBAL2, 0x08, 0x8000 | (i << 11));
+
+	/* Disable ingress rate limiting by resetting all ingress
+	 * rate limit registers to their initial state.
+	 */
+	for (i = 0; i < 6; i++)
+		REG_WRITE(REG_GLOBAL2, 0x09, 0x9000 | (i << 8));
+
+	/* Initialise cross-chip port VLAN table to reset defaults. */
+	REG_WRITE(REG_GLOBAL2, 0x0b, 0x9000);
+
+	/* Clear the priority override table. */
+	for (i = 0; i < 16; i++)
+		REG_WRITE(REG_GLOBAL2, 0x0f, 0x8000 | (i << 8));
+
+	/* @@@ initialise AVB (22/23) watchdog (27) sdet (29) registers */
+
+	return 0;
+}
+
+static int mv88e6171_setup_port(struct dsa_switch *ds, int p)
+{
+	int addr = REG_PORT(p);
+	u16 val;
+
+	/* MAC Forcing register: don't force link, speed, duplex
+	 * or flow control state to any particular values on physical
+	 * ports, but force the CPU port and all DSA ports to 1000 Mb/s
+	 * full duplex.
+	 */
+	val = REG_READ(addr, 0x01);
+	if (dsa_is_cpu_port(ds, p) || ds->dsa_port_mask & (1 << p))
+		REG_WRITE(addr, 0x01, val | 0x003e);
+	else
+		REG_WRITE(addr, 0x01, val | 0x0003);
+
+	/* Do not limit the period of time that this port can be
+	 * paused for by the remote end or the period of time that
+	 * this port can pause the remote end.
+	 */
+	REG_WRITE(addr, 0x02, 0x0000);
+
+	/* Port Control: disable Drop-on-Unlock, disable Drop-on-Lock,
+	 * disable Header mode, enable IGMP/MLD snooping, disable VLAN
+	 * tunneling, determine priority by looking at 802.1p and IP
+	 * priority fields (IP prio has precedence), and set STP state
+	 * to Forwarding.
+	 *
+	 * If this is the CPU link, use DSA or EDSA tagging depending
+	 * on which tagging mode was configured.
+	 *
+	 * If this is a link to another switch, use DSA tagging mode.
+	 *
+	 * If this is the upstream port for this switch, enable
+	 * forwarding of unknown unicasts and multicasts.
+	 */
+	val = 0x0433;
+	if (dsa_is_cpu_port(ds, p)) {
+		if (ds->dst->tag_protocol == DSA_TAG_PROTO_EDSA)
+			val |= 0x3300;
+		else
+			val |= 0x0100;
+	}
+	if (ds->dsa_port_mask & (1 << p))
+		val |= 0x0100;
+	if (p == dsa_upstream_port(ds))
+		val |= 0x000c;
+	REG_WRITE(addr, 0x04, val);
+
+	/* Port Control 1: disable trunking.  Also, if this is the
+	 * CPU port, enable learn messages to be sent to this port.
+	 */
+	REG_WRITE(addr, 0x05, dsa_is_cpu_port(ds, p) ? 0x8000 : 0x0000);
+
+	/* Port based VLAN map: give each port its own address
+	 * database, allow the CPU port to talk to each of the 'real'
+	 * ports, and allow each of the 'real' ports to only talk to
+	 * the upstream port.
+	 */
+	val = (p & 0xf) << 12;
+	if (dsa_is_cpu_port(ds, p))
+		val |= ds->phys_port_mask;
+	else
+		val |= 1 << dsa_upstream_port(ds);
+	REG_WRITE(addr, 0x06, val);
+
+	/* Default VLAN ID and priority: don't set a default VLAN
+	 * ID, and set the default packet priority to zero.
+	 */
+	REG_WRITE(addr, 0x07, 0x0000);
+
+	/* Port Control 2: don't force a good FCS, set the maximum
+	 * frame size to 10240 bytes, don't let the switch add or
+	 * strip 802.1q tags, don't discard tagged or untagged frames
+	 * on this port, do a destination address lookup on all
+	 * received packets as usual, disable ARP mirroring and don't
+	 * send a copy of all transmitted/received frames on this port
+	 * to the CPU.
+	 */
+	REG_WRITE(addr, 0x08, 0x2080);
+
+	/* Egress rate control: disable egress rate control. */
+	REG_WRITE(addr, 0x09, 0x0001);
+
+	/* Egress rate control 2: disable egress rate control. */
+	REG_WRITE(addr, 0x0a, 0x0000);
+
+	/* Port Association Vector: when learning source addresses
+	 * of packets, add the address to the address database using
+	 * a port bitmap that has only the bit for this port set and
+	 * the other bits clear.
+	 */
+	REG_WRITE(addr, 0x0b, 1 << p);
+
+	/* Port ATU control: disable limiting the number of address
+	 * database entries that this port is allowed to use.
+	 */
+	REG_WRITE(addr, 0x0c, 0x0000);
+
+	/* Priority Override: disable DA, SA and VTU priority override. */
+	REG_WRITE(addr, 0x0d, 0x0000);
+
+	/* Port Ethertype: use the Ethertype DSA Ethertype value. */
+	REG_WRITE(addr, 0x0f, ETH_P_EDSA);
+
+	/* Tag Remap: use an identity 802.1p prio -> switch prio
+	 * mapping.
+	 */
+	REG_WRITE(addr, 0x18, 0x3210);
+
+	/* Tag Remap 2: use an identity 802.1p prio -> switch prio
+	 * mapping.
+	 */
+	REG_WRITE(addr, 0x19, 0x7654);
+
+	return 0;
+}
+
+static int mv88e6171_setup(struct dsa_switch *ds)
+{
+	struct mv88e6xxx_priv_state *ps = (void *)(ds + 1);
+	int i;
+	int ret;
+
+	mutex_init(&ps->smi_mutex);
+	mutex_init(&ps->stats_mutex);
+
+	ret = mv88e6171_switch_reset(ds);
+	if (ret < 0)
+		return ret;
+
+	/* @@@ initialise vtu and atu */
+
+	ret = mv88e6171_setup_global(ds);
+	if (ret < 0)
+		return ret;
+
+	for (i = 0; i < 8; i++) {
+		if (!(dsa_is_cpu_port(ds, i) || ds->phys_port_mask & (1 << i)))
+			continue;
+
+		ret = mv88e6171_setup_port(ds, i);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int mv88e6171_port_to_phy_addr(int port)
+{
+	if (port >= 0 && port <= 4)
+		return port;
+	return -1;
+}
+
+static int
+mv88e6171_phy_read(struct dsa_switch *ds, int port, int regnum)
+{
+	int addr = mv88e6171_port_to_phy_addr(port);
+
+	return mv88e6xxx_phy_read(ds, addr, regnum);
+}
+
+static int
+mv88e6171_phy_write(struct dsa_switch *ds,
+		    int port, int regnum, u16 val)
+{
+	int addr = mv88e6171_port_to_phy_addr(port);
+
+	return mv88e6xxx_phy_write(ds, addr, regnum, val);
+}
+
+static struct mv88e6xxx_hw_stat mv88e6171_hw_stats[] = {
+	{ "in_good_octets", 8, 0x00, },
+	{ "in_bad_octets", 4, 0x02, },
+	{ "in_unicast", 4, 0x04, },
+	{ "in_broadcasts", 4, 0x06, },
+	{ "in_multicasts", 4, 0x07, },
+	{ "in_pause", 4, 0x16, },
+	{ "in_undersize", 4, 0x18, },
+	{ "in_fragments", 4, 0x19, },
+	{ "in_oversize", 4, 0x1a, },
+	{ "in_jabber", 4, 0x1b, },
+	{ "in_rx_error", 4, 0x1c, },
+	{ "in_fcs_error", 4, 0x1d, },
+	{ "out_octets", 8, 0x0e, },
+	{ "out_unicast", 4, 0x10, },
+	{ "out_broadcasts", 4, 0x13, },
+	{ "out_multicasts", 4, 0x12, },
+	{ "out_pause", 4, 0x15, },
+	{ "excessive", 4, 0x11, },
+	{ "collisions", 4, 0x1e, },
+	{ "deferred", 4, 0x05, },
+	{ "single", 4, 0x14, },
+	{ "multiple", 4, 0x17, },
+	{ "out_fcs_error", 4, 0x03, },
+	{ "late", 4, 0x1f, },
+	{ "hist_64bytes", 4, 0x08, },
+	{ "hist_65_127bytes", 4, 0x09, },
+	{ "hist_128_255bytes", 4, 0x0a, },
+	{ "hist_256_511bytes", 4, 0x0b, },
+	{ "hist_512_1023bytes", 4, 0x0c, },
+	{ "hist_1024_max_bytes", 4, 0x0d, },
+};
+
+static void
+mv88e6171_get_strings(struct dsa_switch *ds, int port, uint8_t *data)
+{
+	mv88e6xxx_get_strings(ds, ARRAY_SIZE(mv88e6171_hw_stats),
+			      mv88e6171_hw_stats, port, data);
+}
+
+static void
+mv88e6171_get_ethtool_stats(struct dsa_switch *ds,
+			    int port, uint64_t *data)
+{
+	mv88e6xxx_get_ethtool_stats(ds, ARRAY_SIZE(mv88e6171_hw_stats),
+				    mv88e6171_hw_stats, port, data);
+}
+
+static int mv88e6171_get_sset_count(struct dsa_switch *ds)
+{
+	return ARRAY_SIZE(mv88e6171_hw_stats);
+}
+
+struct dsa_switch_driver mv88e6171_switch_driver = {
+	.tag_protocol		= DSA_TAG_PROTO_DSA,
+	.priv_size		= sizeof(struct mv88e6xxx_priv_state),
+	.probe			= mv88e6171_probe,
+	.setup			= mv88e6171_setup,
+	.set_addr		= mv88e6xxx_set_addr_indirect,
+	.phy_read		= mv88e6171_phy_read,
+	.phy_write		= mv88e6171_phy_write,
+	.poll_link		= mv88e6xxx_poll_link,
+	.get_strings		= mv88e6171_get_strings,
+	.get_ethtool_stats	= mv88e6171_get_ethtool_stats,
+	.get_sset_count		= mv88e6171_get_sset_count,
+};
+
+MODULE_ALIAS("platform:mv88e6171");
diff --git a/drivers/net/dsa/mv88e6xxx.c b/drivers/net/dsa/mv88e6xxx.c
index 9ce2146346b6..a6c90cf5634d 100644
--- a/drivers/net/dsa/mv88e6xxx.c
+++ b/drivers/net/dsa/mv88e6xxx.c
@@ -75,11 +75,14 @@ int __mv88e6xxx_reg_read(struct mii_bus *bus, int sw_addr, int addr, int reg)
 int mv88e6xxx_reg_read(struct dsa_switch *ds, int addr, int reg)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
 	int ret;
 
+	if (bus == NULL)
+		return -EINVAL;
+
 	mutex_lock(&ps->smi_mutex);
-	ret = __mv88e6xxx_reg_read(ds->master_mii_bus,
-				   ds->pd->sw_addr, addr, reg);
+	ret = __mv88e6xxx_reg_read(bus, ds->pd->sw_addr, addr, reg);
 	mutex_unlock(&ps->smi_mutex);
 
 	return ret;
@@ -119,11 +122,14 @@ int __mv88e6xxx_reg_write(struct mii_bus *bus, int sw_addr, int addr,
 int mv88e6xxx_reg_write(struct dsa_switch *ds, int addr, int reg, u16 val)
 {
 	struct mv88e6xxx_priv_state *ps = ds_to_priv(ds);
+	struct mii_bus *bus = dsa_host_dev_to_mii_bus(ds->master_dev);
 	int ret;
 
+	if (bus == NULL)
+		return -EINVAL;
+
 	mutex_lock(&ps->smi_mutex);
-	ret = __mv88e6xxx_reg_write(ds->master_mii_bus,
-				    ds->pd->sw_addr, addr, reg, val);
+	ret = __mv88e6xxx_reg_write(bus, ds->pd->sw_addr, addr, reg, val);
 	mutex_unlock(&ps->smi_mutex);
 
 	return ret;
@@ -501,12 +507,18 @@ static int __init mv88e6xxx_init(void)
 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6123_61_65)
 	register_switch_driver(&mv88e6123_61_65_switch_driver);
 #endif
+#if IS_ENABLED(CONFIG_NET_DSA_MV88E6171)
+	register_switch_driver(&mv88e6171_switch_driver);
+#endif
 	return 0;
 }
 module_init(mv88e6xxx_init);
 
 static void __exit mv88e6xxx_cleanup(void)
 {
+#if IS_ENABLED(CONFIG_NET_DSA_MV88E6171)
+	unregister_switch_driver(&mv88e6171_switch_driver);
+#endif
 #if IS_ENABLED(CONFIG_NET_DSA_MV88E6123_61_65)
 	unregister_switch_driver(&mv88e6123_61_65_switch_driver);
 #endif
diff --git a/drivers/net/dsa/mv88e6xxx.h b/drivers/net/dsa/mv88e6xxx.h
index 911ede58dd12..5e5145ad9525 100644
--- a/drivers/net/dsa/mv88e6xxx.h
+++ b/drivers/net/dsa/mv88e6xxx.h
@@ -70,6 +70,7 @@ void mv88e6xxx_get_ethtool_stats(struct dsa_switch *ds,
 
 extern struct dsa_switch_driver mv88e6131_switch_driver;
 extern struct dsa_switch_driver mv88e6123_61_65_switch_driver;
+extern struct dsa_switch_driver mv88e6171_switch_driver;
 
 #define REG_READ(addr, reg)						\
 	({								\
diff --git a/drivers/net/eql.c b/drivers/net/eql.c
index 957e5c0cede3..a10ad74cc8d2 100644
--- a/drivers/net/eql.c
+++ b/drivers/net/eql.c
@@ -199,7 +199,7 @@ static void __init eql_setup(struct net_device *dev)
 
 	dev->type       	= ARPHRD_SLIP;
 	dev->tx_queue_len 	= 5;		/* Hands them off fast */
-	dev->priv_flags	       &= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(dev);
 }
 
 static int eql_open(struct net_device *dev)
diff --git a/drivers/net/ethernet/3com/3c509.c b/drivers/net/ethernet/3com/3c509.c
index a968654b631d..4547a1b8b958 100644
--- a/drivers/net/ethernet/3com/3c509.c
+++ b/drivers/net/ethernet/3com/3c509.c
@@ -695,9 +695,9 @@ el3_tx_timeout (struct net_device *dev)
 	int ioaddr = dev->base_addr;
 
 	/* Transmitter timeout, serious problems. */
-	pr_warning("%s: transmit timed out, Tx_status %2.2x status %4.4x Tx FIFO room %d.\n",
-		   dev->name, inb(ioaddr + TX_STATUS), inw(ioaddr + EL3_STATUS),
-		   inw(ioaddr + TX_FREE));
+	pr_warn("%s: transmit timed out, Tx_status %2.2x status %4.4x Tx FIFO room %d\n",
+		dev->name, inb(ioaddr + TX_STATUS), inw(ioaddr + EL3_STATUS),
+		inw(ioaddr + TX_FREE));
 	dev->stats.tx_errors++;
 	dev->trans_start = jiffies; /* prevent tx timeout */
 	/* Issue TX_RESET and TX_START commands. */
diff --git a/drivers/net/ethernet/3com/3c515.c b/drivers/net/ethernet/3com/3c515.c
index 94c656f5a05d..942fb0d5aace 100644
--- a/drivers/net/ethernet/3com/3c515.c
+++ b/drivers/net/ethernet/3com/3c515.c
@@ -515,7 +515,7 @@ static struct net_device *corkscrew_scan(int unit)
 			if (pnp_device_attach(idev) < 0)
 				continue;
 			if (pnp_activate_dev(idev) < 0) {
-				pr_warning("pnp activate failed (out of resources?)\n");
+				pr_warn("pnp activate failed (out of resources?)\n");
 				pnp_device_detach(idev);
 				continue;
 			}
@@ -659,7 +659,7 @@ static int corkscrew_setup(struct net_device *dev, int ioaddr,
 	pr_cont(", IRQ %d\n", dev->irq);
 	/* Tell them about an invalid IRQ. */
 	if (corkscrew_debug && (dev->irq <= 0 || dev->irq > 15))
-		pr_warning(" *** Warning: this IRQ is unlikely to work! ***\n");
+		pr_warn(" *** Warning: this IRQ is unlikely to work! ***\n");
 
 	{
 		static const char * const ram_split[] = {
@@ -967,13 +967,13 @@ static void corkscrew_timeout(struct net_device *dev)
 	struct corkscrew_private *vp = netdev_priv(dev);
 	int ioaddr = dev->base_addr;
 
-	pr_warning("%s: transmit timed out, tx_status %2.2x status %4.4x.\n",
-	       dev->name, inb(ioaddr + TxStatus),
-	       inw(ioaddr + EL3_STATUS));
+	pr_warn("%s: transmit timed out, tx_status %2.2x status %4.4x\n",
+		dev->name, inb(ioaddr + TxStatus),
+		inw(ioaddr + EL3_STATUS));
 	/* Slight code bloat to be user friendly. */
 	if ((inb(ioaddr + TxStatus) & 0x88) == 0x88)
-		pr_warning("%s: Transmitter encountered 16 collisions --"
-		       " network cable problem?\n", dev->name);
+		pr_warn("%s: Transmitter encountered 16 collisions -- network cable problem?\n",
+			dev->name);
 #ifndef final_version
 	pr_debug("  Flags; bus-master %d, full %d; dirty %d current %d.\n",
 	       vp->full_bus_master_tx, vp->tx_full, vp->dirty_tx,
@@ -1382,13 +1382,10 @@ static int boomerang_rx(struct net_device *dev)
 				temp = skb_put(skb, pkt_len);
 				/* Remove this checking code for final release. */
 				if (isa_bus_to_virt(vp->rx_ring[entry].addr) != temp)
-					pr_warning("%s: Warning -- the skbuff addresses do not match"
-					     " in boomerang_rx: %p vs. %p / %p.\n",
-					     dev->name,
-					     isa_bus_to_virt(vp->
-							 rx_ring[entry].
-							 addr), skb->head,
-					     temp);
+					pr_warn("%s: Warning -- the skbuff addresses do not match in boomerang_rx: %p vs. %p / %p\n",
+						dev->name,
+						isa_bus_to_virt(vp->rx_ring[entry].addr),
+						skb->head, temp);
 				rx_nocopy++;
 			}
 			skb->protocol = eth_type_trans(skb, dev);
diff --git a/drivers/net/ethernet/3com/3c59x.c b/drivers/net/ethernet/3com/3c59x.c
index 059c7414e303..41095ebad97f 100644
--- a/drivers/net/ethernet/3com/3c59x.c
+++ b/drivers/net/ethernet/3com/3c59x.c
@@ -1310,8 +1310,8 @@ static int vortex_probe1(struct device *gendev, void __iomem *ioaddr, int irq,
 		pr_cont(", IRQ %d\n", dev->irq);
 	/* Tell them about an invalid IRQ. */
 	if (dev->irq <= 0 || dev->irq >= nr_irqs)
-		pr_warning(" *** Warning: IRQ %d is unlikely to work! ***\n",
-			   dev->irq);
+		pr_warn(" *** Warning: IRQ %d is unlikely to work! ***\n",
+			dev->irq);
 
 	step = (window_read8(vp, 4, Wn4_NetDiag) & 0x1e) >> 1;
 	if (print_info) {
@@ -1425,7 +1425,7 @@ static int vortex_probe1(struct device *gendev, void __iomem *ioaddr, int irq,
 		}
 		mii_preamble_required--;
 		if (phy_idx == 0) {
-			pr_warning("  ***WARNING*** No MII transceivers found!\n");
+			pr_warn("  ***WARNING*** No MII transceivers found!\n");
 			vp->phys[0] = 24;
 		} else {
 			vp->advertising = mdio_read(dev, vp->phys[0], MII_ADVERTISE);
@@ -1566,8 +1566,7 @@ vortex_up(struct net_device *dev)
 			pci_restore_state(VORTEX_PCI(vp));
 		err = pci_enable_device(VORTEX_PCI(vp));
 		if (err) {
-			pr_warning("%s: Could not enable device\n",
-				dev->name);
+			pr_warn("%s: Could not enable device\n", dev->name);
 			goto err_out;
 		}
 	}
@@ -2007,8 +2006,8 @@ vortex_error(struct net_device *dev, int status)
 		/* This occurs when we have the wrong media type! */
 		if (DoneDidThat == 0  &&
 			ioread16(ioaddr + EL3_STATUS) & StatsFull) {
-			pr_warning("%s: Updating statistics failed, disabling "
-				   "stats as an interrupt source.\n", dev->name);
+			pr_warn("%s: Updating statistics failed, disabling stats as an interrupt source\n",
+				dev->name);
 			iowrite16(SetIntrEnb |
 				  (window_read16(vp, 5, 10) & ~StatsFull),
 				  ioaddr + EL3_CMD);
@@ -2129,6 +2128,7 @@ boomerang_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	int entry = vp->cur_tx % TX_RING_SIZE;
 	struct boom_tx_desc *prev_entry = &vp->tx_ring[(vp->cur_tx-1) % TX_RING_SIZE];
 	unsigned long flags;
+	dma_addr_t dma_addr;
 
 	if (vortex_debug > 6) {
 		pr_debug("boomerang_start_xmit()\n");
@@ -2147,8 +2147,8 @@ boomerang_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (vp->cur_tx - vp->dirty_tx >= TX_RING_SIZE) {
 		if (vortex_debug > 0)
-			pr_warning("%s: BUG! Tx Ring full, refusing to send buffer.\n",
-				   dev->name);
+			pr_warn("%s: BUG! Tx Ring full, refusing to send buffer\n",
+				dev->name);
 		netif_stop_queue(dev);
 		return NETDEV_TX_BUSY;
 	}
@@ -2163,24 +2163,48 @@ boomerang_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			vp->tx_ring[entry].status = cpu_to_le32(skb->len | TxIntrUploaded | AddTCPChksum | AddUDPChksum);
 
 	if (!skb_shinfo(skb)->nr_frags) {
-		vp->tx_ring[entry].frag[0].addr = cpu_to_le32(pci_map_single(VORTEX_PCI(vp), skb->data,
-										skb->len, PCI_DMA_TODEVICE));
+		dma_addr = pci_map_single(VORTEX_PCI(vp), skb->data, skb->len,
+					  PCI_DMA_TODEVICE);
+		if (dma_mapping_error(&VORTEX_PCI(vp)->dev, dma_addr))
+			goto out_dma_err;
+
+		vp->tx_ring[entry].frag[0].addr = cpu_to_le32(dma_addr);
 		vp->tx_ring[entry].frag[0].length = cpu_to_le32(skb->len | LAST_FRAG);
 	} else {
 		int i;
 
-		vp->tx_ring[entry].frag[0].addr = cpu_to_le32(pci_map_single(VORTEX_PCI(vp), skb->data,
-										skb_headlen(skb), PCI_DMA_TODEVICE));
+		dma_addr = pci_map_single(VORTEX_PCI(vp), skb->data,
+					  skb_headlen(skb), PCI_DMA_TODEVICE);
+		if (dma_mapping_error(&VORTEX_PCI(vp)->dev, dma_addr))
+			goto out_dma_err;
+
+		vp->tx_ring[entry].frag[0].addr = cpu_to_le32(dma_addr);
 		vp->tx_ring[entry].frag[0].length = cpu_to_le32(skb_headlen(skb));
 
 		for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 			skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
 
+			dma_addr = skb_frag_dma_map(&VORTEX_PCI(vp)->dev, frag,
+						    0,
+						    frag->size,
+						    DMA_TO_DEVICE);
+			if (dma_mapping_error(&VORTEX_PCI(vp)->dev, dma_addr)) {
+				for(i = i-1; i >= 0; i--)
+					dma_unmap_page(&VORTEX_PCI(vp)->dev,
+						       le32_to_cpu(vp->tx_ring[entry].frag[i+1].addr),
+						       le32_to_cpu(vp->tx_ring[entry].frag[i+1].length),
+						       DMA_TO_DEVICE);
+
+				pci_unmap_single(VORTEX_PCI(vp),
+						 le32_to_cpu(vp->tx_ring[entry].frag[0].addr),
+						 le32_to_cpu(vp->tx_ring[entry].frag[0].length),
+						 PCI_DMA_TODEVICE);
+
+				goto out_dma_err;
+			}
+
 			vp->tx_ring[entry].frag[i+1].addr =
-					cpu_to_le32(pci_map_single(
-						VORTEX_PCI(vp),
-						(void *)skb_frag_address(frag),
-						skb_frag_size(frag), PCI_DMA_TODEVICE));
+						cpu_to_le32(dma_addr);
 
 			if (i == skb_shinfo(skb)->nr_frags-1)
 					vp->tx_ring[entry].frag[i+1].length = cpu_to_le32(skb_frag_size(frag)|LAST_FRAG);
@@ -2189,7 +2213,10 @@ boomerang_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		}
 	}
 #else
-	vp->tx_ring[entry].addr = cpu_to_le32(pci_map_single(VORTEX_PCI(vp), skb->data, skb->len, PCI_DMA_TODEVICE));
+	dma_addr = pci_map_single(VORTEX_PCI(vp), skb->data, skb->len, PCI_DMA_TODEVICE);
+	if (dma_mapping_error(&VORTEX_PCI(vp)->dev, dma_addr))
+		goto out_dma_err;
+	vp->tx_ring[entry].addr = cpu_to_le32(dma_addr);
 	vp->tx_ring[entry].length = cpu_to_le32(skb->len | LAST_FRAG);
 	vp->tx_ring[entry].status = cpu_to_le32(skb->len | TxIntrUploaded);
 #endif
@@ -2217,7 +2244,11 @@ boomerang_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	skb_tx_timestamp(skb);
 	iowrite16(DownUnstall, ioaddr + EL3_CMD);
 	spin_unlock_irqrestore(&vp->lock, flags);
+out:
 	return NETDEV_TX_OK;
+out_dma_err:
+	dev_err(&VORTEX_PCI(vp)->dev, "Error mapping dma buffer\n");
+	goto out;
 }
 
 /* The interrupt handler does all of the Rx thread work and cleans up
@@ -2311,7 +2342,7 @@ vortex_interrupt(int irq, void *dev_id)
 		}
 
 		if (--work_done < 0) {
-			pr_warning("%s: Too much work in interrupt, status %4.4x.\n",
+			pr_warn("%s: Too much work in interrupt, status %4.4x\n",
 				dev->name, status);
 			/* Disable all pending interrupts. */
 			do {
@@ -2444,7 +2475,7 @@ boomerang_interrupt(int irq, void *dev_id)
 			vortex_error(dev, status);
 
 		if (--work_done < 0) {
-			pr_warning("%s: Too much work in interrupt, status %4.4x.\n",
+			pr_warn("%s: Too much work in interrupt, status %4.4x\n",
 				dev->name, status);
 			/* Disable all pending interrupts. */
 			do {
@@ -2620,7 +2651,8 @@ boomerang_rx(struct net_device *dev)
 			if (skb == NULL) {
 				static unsigned long last_jif;
 				if (time_after(jiffies, last_jif + 10 * HZ)) {
-					pr_warning("%s: memory shortage\n", dev->name);
+					pr_warn("%s: memory shortage\n",
+						dev->name);
 					last_jif = jiffies;
 				}
 				if ((vp->cur_rx - vp->dirty_rx) == RX_RING_SIZE)
@@ -2719,7 +2751,8 @@ vortex_close(struct net_device *dev)
 	if (vp->rx_csumhits &&
 	    (vp->drv_flags & HAS_HWCKSM) == 0 &&
 	    (vp->card_idx >= MAX_UNITS || hw_checksums[vp->card_idx] == -1)) {
-		pr_warning("%s supports hardware checksums, and we're not using them!\n", dev->name);
+		pr_warn("%s supports hardware checksums, and we're not using them!\n",
+			dev->name);
 	}
 #endif
 
diff --git a/drivers/net/ethernet/Kconfig b/drivers/net/ethernet/Kconfig
index dc7406c81c45..1ed1fbba5d58 100644
--- a/drivers/net/ethernet/Kconfig
+++ b/drivers/net/ethernet/Kconfig
@@ -20,6 +20,7 @@ config SUNGEM_PHY
 source "drivers/net/ethernet/3com/Kconfig"
 source "drivers/net/ethernet/adaptec/Kconfig"
 source "drivers/net/ethernet/aeroflex/Kconfig"
+source "drivers/net/ethernet/agere/Kconfig"
 source "drivers/net/ethernet/allwinner/Kconfig"
 source "drivers/net/ethernet/alteon/Kconfig"
 source "drivers/net/ethernet/altera/Kconfig"
@@ -150,6 +151,7 @@ config ETHOC
 source "drivers/net/ethernet/packetengines/Kconfig"
 source "drivers/net/ethernet/pasemi/Kconfig"
 source "drivers/net/ethernet/qlogic/Kconfig"
+source "drivers/net/ethernet/qualcomm/Kconfig"
 source "drivers/net/ethernet/realtek/Kconfig"
 source "drivers/net/ethernet/renesas/Kconfig"
 source "drivers/net/ethernet/rdc/Kconfig"
diff --git a/drivers/net/ethernet/Makefile b/drivers/net/ethernet/Makefile
index 224a01877149..6e0b629e9859 100644
--- a/drivers/net/ethernet/Makefile
+++ b/drivers/net/ethernet/Makefile
@@ -6,6 +6,7 @@ obj-$(CONFIG_NET_VENDOR_3COM) += 3com/
 obj-$(CONFIG_NET_VENDOR_8390) += 8390/
 obj-$(CONFIG_NET_VENDOR_ADAPTEC) += adaptec/
 obj-$(CONFIG_GRETH) += aeroflex/
+obj-$(CONFIG_NET_VENDOR_AGERE) += agere/
 obj-$(CONFIG_NET_VENDOR_ALLWINNER) += allwinner/
 obj-$(CONFIG_NET_VENDOR_ALTEON) += alteon/
 obj-$(CONFIG_ALTERA_TSE) += altera/
@@ -60,6 +61,7 @@ obj-$(CONFIG_ETHOC) += ethoc.o
 obj-$(CONFIG_NET_PACKET_ENGINE) += packetengines/
 obj-$(CONFIG_NET_VENDOR_PASEMI) += pasemi/
 obj-$(CONFIG_NET_VENDOR_QLOGIC) += qlogic/
+obj-$(CONFIG_NET_VENDOR_QUALCOMM) += qualcomm/
 obj-$(CONFIG_NET_VENDOR_REALTEK) += realtek/
 obj-$(CONFIG_SH_ETH) += renesas/
 obj-$(CONFIG_NET_VENDOR_RDC) += rdc/
diff --git a/drivers/net/ethernet/adi/bfin_mac.c b/drivers/net/ethernet/adi/bfin_mac.c
index afa66847e10b..8ed4d3408ef6 100644
--- a/drivers/net/ethernet/adi/bfin_mac.c
+++ b/drivers/net/ethernet/adi/bfin_mac.c
@@ -1692,9 +1692,6 @@ static int bfin_mac_probe(struct platform_device *pdev)
 	lp->vlan1_mask = ETH_P_8021Q | mii_bus_data->vlan1_mask;
 	lp->vlan2_mask = ETH_P_8021Q | mii_bus_data->vlan2_mask;
 
-	/* Fill in the fields of the device structure with ethernet values. */
-	ether_setup(ndev);
-
 	ndev->netdev_ops = &bfin_mac_netdev_ops;
 	ndev->ethtool_ops = &bfin_mac_ethtool_ops;
 
diff --git a/drivers/net/ethernet/aeroflex/greth.c b/drivers/net/ethernet/aeroflex/greth.c
index 23578dfee249..3005155e412b 100644
--- a/drivers/net/ethernet/aeroflex/greth.c
+++ b/drivers/net/ethernet/aeroflex/greth.c
@@ -123,6 +123,12 @@ static inline void greth_enable_tx(struct greth_private *greth)
 	GRETH_REGORIN(greth->regs->control, GRETH_TXEN);
 }
 
+static inline void greth_enable_tx_and_irq(struct greth_private *greth)
+{
+	wmb(); /* BDs must been written to memory before enabling TX */
+	GRETH_REGORIN(greth->regs->control, GRETH_TXEN | GRETH_TXI);
+}
+
 static inline void greth_disable_tx(struct greth_private *greth)
 {
 	GRETH_REGANDIN(greth->regs->control, ~GRETH_TXEN);
@@ -447,29 +453,30 @@ out:
 	return err;
 }
 
+static inline u16 greth_num_free_bds(u16 tx_last, u16 tx_next)
+{
+	if (tx_next < tx_last)
+		return (tx_last - tx_next) - 1;
+	else
+		return GRETH_TXBD_NUM - (tx_next - tx_last) - 1;
+}
 
 static netdev_tx_t
 greth_start_xmit_gbit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct greth_private *greth = netdev_priv(dev);
 	struct greth_bd *bdp;
-	u32 status = 0, dma_addr, ctrl;
+	u32 status, dma_addr;
 	int curr_tx, nr_frags, i, err = NETDEV_TX_OK;
 	unsigned long flags;
+	u16 tx_last;
 
 	nr_frags = skb_shinfo(skb)->nr_frags;
+	tx_last = greth->tx_last;
+	rmb(); /* tx_last is updated by the poll task */
 
-	/* Clean TX Ring */
-	greth_clean_tx_gbit(dev);
-
-	if (greth->tx_free < nr_frags + 1) {
-		spin_lock_irqsave(&greth->devlock, flags);/*save from poll/irq*/
-		ctrl = GRETH_REGLOAD(greth->regs->control);
-		/* Enable TX IRQ only if not already in poll() routine */
-		if (ctrl & GRETH_RXI)
-			GRETH_REGSAVE(greth->regs->control, ctrl | GRETH_TXI);
+	if (greth_num_free_bds(tx_last, greth->tx_next) < nr_frags + 1) {
 		netif_stop_queue(dev);
-		spin_unlock_irqrestore(&greth->devlock, flags);
 		err = NETDEV_TX_BUSY;
 		goto out;
 	}
@@ -488,6 +495,8 @@ greth_start_xmit_gbit(struct sk_buff *skb, struct net_device *dev)
 	/* Linear buf */
 	if (nr_frags != 0)
 		status = GRETH_TXBD_MORE;
+	else
+		status = GRETH_BD_IE;
 
 	if (skb->ip_summed == CHECKSUM_PARTIAL)
 		status |= GRETH_TXBD_CSALL;
@@ -545,14 +554,12 @@ greth_start_xmit_gbit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Enable the descriptor chain by enabling the first descriptor */
 	bdp = greth->tx_bd_base + greth->tx_next;
-	greth_write_bd(&bdp->stat, greth_read_bd(&bdp->stat) | GRETH_BD_EN);
-	greth->tx_next = curr_tx;
-	greth->tx_free -= nr_frags + 1;
-
-	wmb();
+	greth_write_bd(&bdp->stat,
+		       greth_read_bd(&bdp->stat) | GRETH_BD_EN);
 
 	spin_lock_irqsave(&greth->devlock, flags); /*save from poll/irq*/
-	greth_enable_tx(greth);
+	greth->tx_next = curr_tx;
+	greth_enable_tx_and_irq(greth);
 	spin_unlock_irqrestore(&greth->devlock, flags);
 
 	return NETDEV_TX_OK;
@@ -648,7 +655,6 @@ static void greth_clean_tx(struct net_device *dev)
 	if (greth->tx_free > 0) {
 		netif_wake_queue(dev);
 	}
-
 }
 
 static inline void greth_update_tx_stats(struct net_device *dev, u32 stat)
@@ -670,20 +676,22 @@ static void greth_clean_tx_gbit(struct net_device *dev)
 {
 	struct greth_private *greth;
 	struct greth_bd *bdp, *bdp_last_frag;
-	struct sk_buff *skb;
+	struct sk_buff *skb = NULL;
 	u32 stat;
 	int nr_frags, i;
+	u16 tx_last;
 
 	greth = netdev_priv(dev);
+	tx_last = greth->tx_last;
 
-	while (greth->tx_free < GRETH_TXBD_NUM) {
+	while (tx_last != greth->tx_next) {
 
-		skb = greth->tx_skbuff[greth->tx_last];
+		skb = greth->tx_skbuff[tx_last];
 
 		nr_frags = skb_shinfo(skb)->nr_frags;
 
 		/* We only clean fully completed SKBs */
-		bdp_last_frag = greth->tx_bd_base + SKIP_TX(greth->tx_last, nr_frags);
+		bdp_last_frag = greth->tx_bd_base + SKIP_TX(tx_last, nr_frags);
 
 		GRETH_REGSAVE(greth->regs->status, GRETH_INT_TE | GRETH_INT_TX);
 		mb();
@@ -692,14 +700,14 @@ static void greth_clean_tx_gbit(struct net_device *dev)
 		if (stat & GRETH_BD_EN)
 			break;
 
-		greth->tx_skbuff[greth->tx_last] = NULL;
+		greth->tx_skbuff[tx_last] = NULL;
 
 		greth_update_tx_stats(dev, stat);
 		dev->stats.tx_bytes += skb->len;
 
-		bdp = greth->tx_bd_base + greth->tx_last;
+		bdp = greth->tx_bd_base + tx_last;
 
-		greth->tx_last = NEXT_TX(greth->tx_last);
+		tx_last = NEXT_TX(tx_last);
 
 		dma_unmap_single(greth->dev,
 				 greth_read_bd(&bdp->addr),
@@ -708,21 +716,26 @@ static void greth_clean_tx_gbit(struct net_device *dev)
 
 		for (i = 0; i < nr_frags; i++) {
 			skb_frag_t *frag = &skb_shinfo(skb)->frags[i];
-			bdp = greth->tx_bd_base + greth->tx_last;
+			bdp = greth->tx_bd_base + tx_last;
 
 			dma_unmap_page(greth->dev,
 				       greth_read_bd(&bdp->addr),
 				       skb_frag_size(frag),
 				       DMA_TO_DEVICE);
 
-			greth->tx_last = NEXT_TX(greth->tx_last);
+			tx_last = NEXT_TX(tx_last);
 		}
-		greth->tx_free += nr_frags+1;
 		dev_kfree_skb(skb);
 	}
+	if (skb) { /* skb is set only if the above while loop was entered */
+		wmb();
+		greth->tx_last = tx_last;
 
-	if (netif_queue_stopped(dev) && (greth->tx_free > (MAX_SKB_FRAGS+1)))
-		netif_wake_queue(dev);
+		if (netif_queue_stopped(dev) &&
+		    (greth_num_free_bds(tx_last, greth->tx_next) >
+		    (MAX_SKB_FRAGS+1)))
+			netif_wake_queue(dev);
+	}
 }
 
 static int greth_rx(struct net_device *dev, int limit)
@@ -965,16 +978,12 @@ static int greth_poll(struct napi_struct *napi, int budget)
 	greth = container_of(napi, struct greth_private, napi);
 
 restart_txrx_poll:
-	if (netif_queue_stopped(greth->netdev)) {
-		if (greth->gbit_mac)
-			greth_clean_tx_gbit(greth->netdev);
-		else
-			greth_clean_tx(greth->netdev);
-	}
-
 	if (greth->gbit_mac) {
+		greth_clean_tx_gbit(greth->netdev);
 		work_done += greth_rx_gbit(greth->netdev, budget - work_done);
 	} else {
+		if (netif_queue_stopped(greth->netdev))
+			greth_clean_tx(greth->netdev);
 		work_done += greth_rx(greth->netdev, budget - work_done);
 	}
 
@@ -983,7 +992,8 @@ restart_txrx_poll:
 		spin_lock_irqsave(&greth->devlock, flags);
 
 		ctrl = GRETH_REGLOAD(greth->regs->control);
-		if (netif_queue_stopped(greth->netdev)) {
+		if ((greth->gbit_mac && (greth->tx_last != greth->tx_next)) ||
+		    (!greth->gbit_mac && netif_queue_stopped(greth->netdev))) {
 			GRETH_REGSAVE(greth->regs->control,
 					ctrl | GRETH_TXI | GRETH_RXI);
 			mask = GRETH_INT_RX | GRETH_INT_RE |
diff --git a/drivers/net/ethernet/aeroflex/greth.h b/drivers/net/ethernet/aeroflex/greth.h
index 232a622a85b7..ae16ac94daf8 100644
--- a/drivers/net/ethernet/aeroflex/greth.h
+++ b/drivers/net/ethernet/aeroflex/greth.h
@@ -107,7 +107,7 @@ struct greth_private {
 
 	u16 tx_next;
 	u16 tx_last;
-	u16 tx_free;
+	u16 tx_free; /* only used on 10/100Mbit */
 	u16 rx_cur;
 
 	struct greth_regs *regs;	/* Address of controller registers. */
diff --git a/drivers/net/ethernet/agere/Kconfig b/drivers/net/ethernet/agere/Kconfig
new file mode 100644
index 000000000000..63e805de619e
--- /dev/null
+++ b/drivers/net/ethernet/agere/Kconfig
@@ -0,0 +1,31 @@
+#
+# Agere device configuration
+#
+
+config NET_VENDOR_AGERE
+	bool "Agere devices"
+	default y
+	depends on PCI
+	---help---
+	  If you have a network (Ethernet) card belonging to this class, say Y
+	  and read the Ethernet-HOWTO, available from
+	  <http://www.tldp.org/docs.html#howto>.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about Agere devices. If you say Y, you will be asked
+	  for your specific card in the following questions.
+
+if NET_VENDOR_AGERE
+
+config ET131X
+	tristate "Agere ET-1310 Gigabit Ethernet support"
+	depends on PCI
+	select PHYLIB
+	---help---
+	  This driver supports Agere ET-1310 ethernet adapters.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called et131x.
+
+endif # NET_VENDOR_AGERE
diff --git a/drivers/staging/et131x/Makefile b/drivers/net/ethernet/agere/Makefile
index 027ff9453fe1..027ff9453fe1 100644
--- a/drivers/staging/et131x/Makefile
+++ b/drivers/net/ethernet/agere/Makefile
diff --git a/drivers/staging/et131x/et131x.c b/drivers/net/ethernet/agere/et131x.c
index 8bf1eb485163..384dc163851b 100644
--- a/drivers/staging/et131x/et131x.c
+++ b/drivers/net/ethernet/agere/et131x.c
@@ -134,10 +134,6 @@ MODULE_DESCRIPTION("10/100/1000 Base-T Ethernet Driver for the ET1310 by Agere S
 #define ET131X_TX_TIMEOUT	(1 * HZ)
 #define NIC_SEND_HANG_THRESHOLD	0
 
-/* MP_TCB flags */
-#define FMP_DEST_MULTI			0x00000001
-#define FMP_DEST_BROAD			0x00000002
-
 /* MP_ADAPTER flags */
 #define FMP_ADAPTER_INTERRUPT_IN_USE	0x00000008
 
@@ -179,7 +175,7 @@ MODULE_DESCRIPTION("10/100/1000 Base-T Ethernet Driver for the ET1310 by Agere S
 #define NIC_DEFAULT_NUM_RFD	1024
 #define NUM_FBRS		2
 
-#define NUM_PACKETS_HANDLED	256
+#define MAX_PACKETS_HANDLED	256
 
 #define ALCATEL_MULTICAST_PKT	0x01000000
 #define ALCATEL_BROADCAST_PKT	0x02000000
@@ -233,7 +229,6 @@ struct fbr_desc {
  * 26-27: ri			Ring Index
  * 28-31: reserved
  */
-
 struct pkt_stat_desc {
 	u32 word0;
 	u32 word1;
@@ -293,12 +288,11 @@ struct rx_ring {
 	void *ps_ring_virtaddr;
 	dma_addr_t ps_ring_physaddr;
 	u32 local_psr_full;
-	u32 psr_num_entries;
+	u32 psr_entries;
 
 	struct rx_status_block *rx_status_block;
 	dma_addr_t rx_status_bus;
 
-	/* RECV */
 	struct list_head recv_list;
 	u32 num_ready_recv;
 
@@ -333,7 +327,6 @@ struct rx_ring {
  * 13: TCP checksum assist
  * 14: UDP checksum assist
  */
-
 #define TXDESC_FLAG_LASTPKT		0x0001
 #define TXDESC_FLAG_FIRSTPKT		0x0002
 #define TXDESC_FLAG_INTPROC		0x0004
@@ -353,7 +346,6 @@ struct tx_desc {
 /* TCB (Transmit Control Block: Host Side) */
 struct tcb {
 	struct tcb *next;	/* Next entry in ring */
-	u32 flags;		/* Our flags for the packet */
 	u32 count;		/* Used to spot stuck/lost packets */
 	u32 stale;		/* Used to spot stuck/lost packets */
 	struct sk_buff *skb;	/* Network skb we are tied to */
@@ -370,12 +362,7 @@ struct tx_ring {
 	struct tcb *tcb_qhead;
 	struct tcb *tcb_qtail;
 
-	/* list of TCBs that are currently being sent.  NOTE that access to all
-	 * three of these (including used) are controlled via the
-	 * TCBSendQLock.  This lock should be secured prior to incementing /
-	 * decrementing used, or any queue manipulation on send_head /
-	 * tail
-	 */
+	/* list of TCBs that are currently being sent. */
 	struct tcb *send_head;
 	struct tcb *send_tail;
 	int used;
@@ -427,23 +414,10 @@ struct rfd {
 
 /* Struct to define some device statistics */
 struct ce_stats {
-	/* MIB II variables
-	 *
-	 * NOTE: atomic_t types are only guaranteed to store 24-bits; if we
-	 * MUST have 32, then we'll need another way to perform atomic
-	 * operations
-	 */
-	u32		unicast_pkts_rcvd;
-	atomic_t	unicast_pkts_xmtd;
 	u32		multicast_pkts_rcvd;
-	atomic_t	multicast_pkts_xmtd;
-	u32		broadcast_pkts_rcvd;
-	atomic_t	broadcast_pkts_xmtd;
 	u32		rcvd_pkts_dropped;
 
-	/* Tx Statistics. */
 	u32		tx_underflows;
-
 	u32		tx_collisions;
 	u32		tx_excessive_collisions;
 	u32		tx_first_collisions;
@@ -451,16 +425,13 @@ struct ce_stats {
 	u32		tx_max_pkt_errs;
 	u32		tx_deferred;
 
-	/* Rx Statistics. */
 	u32		rx_overflows;
-
 	u32		rx_length_errs;
 	u32		rx_align_errs;
 	u32		rx_crc_errs;
 	u32		rx_code_violations;
 	u32		rx_other_errs;
 
-	u32		synchronous_iterations;
 	u32		interrupt_status;
 };
 
@@ -470,7 +441,7 @@ struct et131x_adapter {
 	struct pci_dev *pdev;
 	struct mii_bus *mii_bus;
 	struct phy_device *phydev;
-	struct work_struct task;
+	struct napi_struct napi;
 
 	/* Flags that indicate current state of the adapter */
 	u32 flags;
@@ -484,13 +455,9 @@ struct et131x_adapter {
 	bool has_eeprom;
 	u8 eeprom_data[2];
 
-	/* Spinlocks */
-	spinlock_t tcb_send_qlock;
-	spinlock_t tcb_ready_qlock;
-	spinlock_t send_hw_lock;
-
-	spinlock_t rcv_lock;
-	spinlock_t fbr_lock;
+	spinlock_t tcb_send_qlock; /* protects the tx_ring send tcb list */
+	spinlock_t tcb_ready_qlock; /* protects the tx_ring ready tcb list */
+	spinlock_t rcv_lock; /* protects the rx_ring receive list */
 
 	/* Packet Filter and look ahead size */
 	u32 packet_filter;
@@ -507,7 +474,7 @@ struct et131x_adapter {
 	u32 registry_jumbo_packet;	/* Max supported ethernet packet size */
 
 	/* Derived from the registry: */
-	u8 flowcontrol;		/* flow control validated by the far-end */
+	u8 flow;		/* flow control validated by the far-end */
 
 	/* Minimize init-time */
 	struct timer_list error_timer;
@@ -517,20 +484,12 @@ struct et131x_adapter {
 	 */
 	u8 boot_coma;
 
-	/* Next two used to save power information at power down. This
-	 * information will be used during power up to set up parts of Power
-	 * Management in JAGCore
-	 */
-	u16 pdown_speed;
-	u8 pdown_duplex;
-
 	/* Tx Memory Variables */
 	struct tx_ring tx_ring;
 
 	/* Rx Memory Variables */
 	struct rx_ring rx_ring;
 
-	/* Stats */
 	struct ce_stats stats;
 };
 
@@ -544,9 +503,7 @@ static int eeprom_wait_ready(struct pci_dev *pdev, u32 *status)
 	 *    Subsequent operations need only to check that bits 1:0 are equal
 	 *    to 1 prior to starting a single byte read/write
 	 */
-
 	for (i = 0; i < MAX_NUM_REGISTER_POLLS; i++) {
-		/* Read registers grouped in DWORD1 */
 		if (pci_read_config_dword(pdev, LBCIF_DWORD1_GROUP, &reg))
 			return -EIO;
 
@@ -560,20 +517,12 @@ static int eeprom_wait_ready(struct pci_dev *pdev, u32 *status)
 	return -ETIMEDOUT;
 }
 
-/* eeprom_write - Write a byte to the ET1310's EEPROM
- * @adapter: pointer to our private adapter structure
- * @addr: the address to write
- * @data: the value to write
- *
- * Returns 1 for a successful write.
- */
 static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 {
 	struct pci_dev *pdev = adapter->pdev;
 	int index = 0;
 	int retries;
 	int err = 0;
-	int i2c_wack = 0;
 	int writeok = 0;
 	u32 status;
 	u32 val = 0;
@@ -585,7 +534,6 @@ static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 	 * All inputs are disabled during this write cycle and the EEPROM will
 	 * not respond to any access until the internal write is complete.
 	 */
-
 	err = eeprom_wait_ready(pdev, NULL);
 	if (err < 0)
 		return err;
@@ -596,15 +544,12 @@ static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 	  *    byte addressing).
 	  */
 	if (pci_write_config_byte(pdev, LBCIF_CONTROL_REGISTER,
-			LBCIF_CONTROL_LBCIF_ENABLE | LBCIF_CONTROL_I2C_WRITE))
+				  LBCIF_CONTROL_LBCIF_ENABLE |
+					LBCIF_CONTROL_I2C_WRITE))
 		return -EIO;
 
-	i2c_wack = 1;
-
 	/* Prepare EEPROM address for Step 3 */
-
 	for (retries = 0; retries < MAX_NUM_WRITE_RETRIES; retries++) {
-		/* Write the address to the LBCIF Address Register */
 		if (pci_write_config_dword(pdev, LBCIF_ADDRESS_REGISTER, addr))
 			break;
 		/* Write the data to the LBCIF Data Register (the I2C write
@@ -627,8 +572,8 @@ static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 		 * an error has occurred.Don't break here if we are revision
 		 * 1, this is so we do a blind write for load bug.
 		 */
-		if ((status & LBCIF_STATUS_GENERAL_ERROR)
-			&& adapter->pdev->revision == 0)
+		if ((status & LBCIF_STATUS_GENERAL_ERROR) &&
+		    adapter->pdev->revision == 0)
 			break;
 
 		/* Check bit 2 of the LBCIF Status Register.  If equal to 1 an
@@ -652,13 +597,11 @@ static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 		break;
 	}
 
-	/* Set bit 6 of the LBCIF Control Register = 0.
-	 */
 	udelay(10);
 
-	while (i2c_wack) {
+	while (1) {
 		if (pci_write_config_byte(pdev, LBCIF_CONTROL_REGISTER,
-			LBCIF_CONTROL_LBCIF_ENABLE))
+					  LBCIF_CONTROL_LBCIF_ENABLE))
 			writeok = 0;
 
 		/* Do read until internal ACK_ERROR goes away meaning write
@@ -670,7 +613,8 @@ static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 					       addr);
 			do {
 				pci_read_config_dword(pdev,
-					LBCIF_DATA_REGISTER, &val);
+						      LBCIF_DATA_REGISTER,
+						      &val);
 			} while ((val & 0x00010000) == 0);
 		} while (val & 0x00040000);
 
@@ -681,15 +625,6 @@ static int eeprom_write(struct et131x_adapter *adapter, u32 addr, u8 data)
 	return writeok ? 0 : -EIO;
 }
 
-/* eeprom_read - Read a byte from the ET1310's EEPROM
- * @adapter: pointer to our private adapter structure
- * @addr: the address from which to read
- * @pdata: a pointer to a byte in which to store the value of the read
- * @eeprom_id: the ID of the EEPROM
- * @addrmode: how the EEPROM is to be accessed
- *
- * Returns 1 for a successful read
- */
 static int eeprom_read(struct et131x_adapter *adapter, u32 addr, u8 *pdata)
 {
 	struct pci_dev *pdev = adapter->pdev;
@@ -699,7 +634,6 @@ static int eeprom_read(struct et131x_adapter *adapter, u32 addr, u8 *pdata)
 	/* A single byte read is similar to the single byte write, with the
 	 * exception of the data flow:
 	 */
-
 	err = eeprom_wait_ready(pdev, NULL);
 	if (err < 0)
 		return err;
@@ -727,9 +661,7 @@ static int eeprom_read(struct et131x_adapter *adapter, u32 addr, u8 *pdata)
 	 * Register.
 	 */
 	*pdata = err;
-	/* Check bit 2 of the LBCIF Status Register.  If = 1,
-	 * then an error has occurred.
-	 */
+
 	return (status & LBCIF_STATUS_ACK_ERROR) ? -EIO : 0;
 }
 
@@ -738,9 +670,6 @@ static int et131x_init_eeprom(struct et131x_adapter *adapter)
 	struct pci_dev *pdev = adapter->pdev;
 	u8 eestatus;
 
-	/* We first need to check the EEPROM Status code located at offset
-	 * 0xB2 of config space
-	 */
 	pci_read_config_byte(pdev, ET1310_PCI_EEPROM_STATUS, &eestatus);
 
 	/* THIS IS A WORKAROUND:
@@ -751,7 +680,7 @@ static int et131x_init_eeprom(struct et131x_adapter *adapter)
 	 */
 	if (pci_read_config_byte(pdev, ET1310_PCI_EEPROM_STATUS, &eestatus)) {
 		dev_err(&pdev->dev,
-		       "Could not read PCI config space for EEPROM Status\n");
+			"Could not read PCI config space for EEPROM Status\n");
 		return -EIO;
 	}
 
@@ -775,7 +704,8 @@ static int et131x_init_eeprom(struct et131x_adapter *adapter)
 		}
 		if (pdev->revision  != 0x01 || write_failed) {
 			dev_err(&pdev->dev,
-			    "Fatal EEPROM Status Error - 0x%04x\n", eestatus);
+				"Fatal EEPROM Status Error - 0x%04x\n",
+				eestatus);
 
 			/* This error could mean that there was an error
 			 * reading the eeprom or that the eeprom doesn't exist.
@@ -790,7 +720,7 @@ static int et131x_init_eeprom(struct et131x_adapter *adapter)
 	adapter->has_eeprom = 1;
 
 	/* Read the EEPROM for information regarding LED behavior. Refer to
-	 * ET1310_phy.c, et131x_xcvr_init(), for its use.
+	 * et131x_xcvr_init() for its use.
 	 */
 	eeprom_read(adapter, 0x70, &adapter->eeprom_data[0]);
 	eeprom_read(adapter, 0x71, &adapter->eeprom_data[1]);
@@ -802,9 +732,6 @@ static int et131x_init_eeprom(struct et131x_adapter *adapter)
 	return 0;
 }
 
-/* et131x_rx_dma_enable - re-start of Rx_DMA on the ET1310.
- * @adapter: pointer to our adapter structure
- */
 static void et131x_rx_dma_enable(struct et131x_adapter *adapter)
 {
 	/* Setup the receive dma configuration register for normal operation */
@@ -833,15 +760,12 @@ static void et131x_rx_dma_enable(struct et131x_adapter *adapter)
 		csr = readl(&adapter->regs->rxdma.csr);
 		if (csr & ET_RXDMA_CSR_HALT_STATUS) {
 			dev_err(&adapter->pdev->dev,
-			    "RX Dma failed to exit halt state.  CSR 0x%08x\n",
+				"RX Dma failed to exit halt state. CSR 0x%08x\n",
 				csr);
 		}
 	}
 }
 
-/* et131x_rx_dma_disable - Stop of Rx_DMA on the ET1310
- * @adapter: pointer to our adapter structure
- */
 static void et131x_rx_dma_disable(struct et131x_adapter *adapter)
 {
 	u32 csr;
@@ -854,23 +778,18 @@ static void et131x_rx_dma_disable(struct et131x_adapter *adapter)
 		csr = readl(&adapter->regs->rxdma.csr);
 		if (!(csr & ET_RXDMA_CSR_HALT_STATUS))
 			dev_err(&adapter->pdev->dev,
-			      "RX Dma failed to enter halt state. CSR 0x%08x\n",
-			      csr);
+				"RX Dma failed to enter halt state. CSR 0x%08x\n",
+				csr);
 	}
 }
 
-/* et131x_tx_dma_enable - re-start of Tx_DMA on the ET1310.
- * @adapter: pointer to our adapter structure
- *
- * Mainly used after a return to the D0 (full-power) state from a lower state.
- */
 static void et131x_tx_dma_enable(struct et131x_adapter *adapter)
 {
 	/* Setup the transmit dma configuration register for normal
 	 * operation
 	 */
-	writel(ET_TXDMA_SNGL_EPKT|(PARM_DMA_CACHE_DEF << ET_TXDMA_CACHE_SHIFT),
-					&adapter->regs->txdma.csr);
+	writel(ET_TXDMA_SNGL_EPKT | (PARM_DMA_CACHE_DEF << ET_TXDMA_CACHE_SHIFT),
+	       &adapter->regs->txdma.csr);
 }
 
 static inline void add_10bit(u32 *v, int n)
@@ -883,9 +802,6 @@ static inline void add_12bit(u32 *v, int n)
 	*v = INDEX12(*v + n) | (*v & ET_DMA12_WRAP);
 }
 
-/* et1310_config_mac_regs1 - Initialize the first part of MAC regs
- * @adapter: pointer to our adapter structure
- */
 static void et1310_config_mac_regs1(struct et131x_adapter *adapter)
 {
 	struct mac_regs __iomem *macregs = &adapter->regs->mac;
@@ -913,7 +829,6 @@ static void et1310_config_mac_regs1(struct et131x_adapter *adapter)
 	/* Next lets configure the MAC Interface Control register */
 	writel(0, &macregs->if_ctrl);
 
-	/* Let's move on to setting up the mii management configuration */
 	writel(ET_MAC_MIIMGMT_CLK_RST, &macregs->mii_mgmt_cfg);
 
 	/* Next lets configure the MAC Station Address register.  These
@@ -945,9 +860,6 @@ static void et1310_config_mac_regs1(struct et131x_adapter *adapter)
 	writel(0, &macregs->cfg1);
 }
 
-/* et1310_config_mac_regs2 - Initialize the second part of MAC regs
- * @adapter: pointer to our adapter structure
- */
 static void et1310_config_mac_regs2(struct et131x_adapter *adapter)
 {
 	int32_t delay = 0;
@@ -967,20 +879,17 @@ static void et1310_config_mac_regs2(struct et131x_adapter *adapter)
 	cfg2 &= ~ET_MAC_CFG2_IFMODE_MASK;
 	if (phydev->speed == SPEED_1000) {
 		cfg2 |= ET_MAC_CFG2_IFMODE_1000;
-		/* Phy mode bit */
 		ifctrl &= ~ET_MAC_IFCTRL_PHYMODE;
 	} else {
 		cfg2 |= ET_MAC_CFG2_IFMODE_100;
 		ifctrl |= ET_MAC_IFCTRL_PHYMODE;
 	}
 
-	/* We need to enable Rx/Tx */
 	cfg1 |= ET_MAC_CFG1_RX_ENABLE | ET_MAC_CFG1_TX_ENABLE |
 							ET_MAC_CFG1_TX_FLOW;
-	/* Initialize loop back to off */
+
 	cfg1 &= ~(ET_MAC_CFG1_LOOPBACK | ET_MAC_CFG1_RX_FLOW);
-	if (adapter->flowcontrol == FLOW_RXONLY ||
-				adapter->flowcontrol == FLOW_BOTH)
+	if (adapter->flow == FLOW_RXONLY || adapter->flow == FLOW_BOTH)
 		cfg1 |= ET_MAC_CFG1_RX_FLOW;
 	writel(cfg1, &mac->cfg1);
 
@@ -995,7 +904,6 @@ static void et1310_config_mac_regs2(struct et131x_adapter *adapter)
 	cfg2 &= ~ET_MAC_CFG2_IFMODE_HUGE_FRAME;
 	cfg2 &= ~ET_MAC_CFG2_IFMODE_FULL_DPLX;
 
-	/* Turn on duplex if needed */
 	if (phydev->duplex == DUPLEX_FULL)
 		cfg2 |= ET_MAC_CFG2_IFMODE_FULL_DPLX;
 
@@ -1014,26 +922,19 @@ static void et1310_config_mac_regs2(struct et131x_adapter *adapter)
 
 	if (delay == 100) {
 		dev_warn(&adapter->pdev->dev,
-		    "Syncd bits did not respond correctly cfg1 word 0x%08x\n",
-			cfg1);
+			 "Syncd bits did not respond correctly cfg1 word 0x%08x\n",
+			 cfg1);
 	}
 
-	/* Enable txmac */
 	ctl |= ET_TX_CTRL_TXMAC_ENABLE | ET_TX_CTRL_FC_DISABLE;
 	writel(ctl, &adapter->regs->txmac.ctl);
 
-	/* Ready to start the RXDMA/TXDMA engine */
 	if (adapter->flags & FMP_ADAPTER_LOWER_POWER) {
 		et131x_rx_dma_enable(adapter);
 		et131x_tx_dma_enable(adapter);
 	}
 }
 
-/* et1310_in_phy_coma - check if the device is in phy coma
- * @adapter: pointer to our adapter structure
- *
- * Returns 0 if the device is not in phy coma, 1 if it is in phy coma
- */
 static int et1310_in_phy_coma(struct et131x_adapter *adapter)
 {
 	u32 pmcsr = readl(&adapter->regs->global.pm_csr);
@@ -1138,6 +1039,7 @@ static void et1310_config_rxmac_regs(struct et131x_adapter *adapter)
 	u32 sa_lo;
 	u32 sa_hi = 0;
 	u32 pf_ctrl = 0;
+	u32 __iomem *wolw;
 
 	/* Disable the MAC while it is being configured (also disable WOL) */
 	writel(0x8, &rxmac->ctrl);
@@ -1151,30 +1053,8 @@ static void et1310_config_rxmac_regs(struct et131x_adapter *adapter)
 	 * its default Values of 0x00000000 because there are not WOL masks
 	 * as of this time.
 	 */
-	writel(0, &rxmac->mask0_word0);
-	writel(0, &rxmac->mask0_word1);
-	writel(0, &rxmac->mask0_word2);
-	writel(0, &rxmac->mask0_word3);
-
-	writel(0, &rxmac->mask1_word0);
-	writel(0, &rxmac->mask1_word1);
-	writel(0, &rxmac->mask1_word2);
-	writel(0, &rxmac->mask1_word3);
-
-	writel(0, &rxmac->mask2_word0);
-	writel(0, &rxmac->mask2_word1);
-	writel(0, &rxmac->mask2_word2);
-	writel(0, &rxmac->mask2_word3);
-
-	writel(0, &rxmac->mask3_word0);
-	writel(0, &rxmac->mask3_word1);
-	writel(0, &rxmac->mask3_word2);
-	writel(0, &rxmac->mask3_word3);
-
-	writel(0, &rxmac->mask4_word0);
-	writel(0, &rxmac->mask4_word1);
-	writel(0, &rxmac->mask4_word2);
-	writel(0, &rxmac->mask4_word3);
+	for (wolw = &rxmac->mask0_word0; wolw <= &rxmac->mask4_word3; wolw++)
+		writel(0, wolw);
 
 	/* Lets setup the WOL Source Address */
 	sa_lo = (adapter->addr[2] << ET_RX_WOL_LO_SA3_SHIFT) |
@@ -1183,7 +1063,7 @@ static void et1310_config_rxmac_regs(struct et131x_adapter *adapter)
 		 adapter->addr[5];
 	writel(sa_lo, &rxmac->sa_lo);
 
-	sa_hi = (u32) (adapter->addr[0] << ET_RX_WOL_HI_SA1_SHIFT) |
+	sa_hi = (u32)(adapter->addr[0] << ET_RX_WOL_HI_SA1_SHIFT) |
 		       adapter->addr[1];
 	writel(sa_hi, &rxmac->sa_hi);
 
@@ -1225,13 +1105,8 @@ static void et1310_config_rxmac_regs(struct et131x_adapter *adapter)
 	else
 		writel(0, &rxmac->mcif_ctrl_max_seg);
 
-	/* Initialize the MCIF water marks */
 	writel(0, &rxmac->mcif_water_mark);
-
-	/*  Initialize the MIF control */
 	writel(0, &rxmac->mif_ctrl);
-
-	/* Initialize the Space Available Register */
 	writel(0, &rxmac->space_avail);
 
 	/* Initialize the the mif_ctrl register
@@ -1270,7 +1145,7 @@ static void et1310_config_txmac_regs(struct et131x_adapter *adapter)
 	 * cfpt - control frame pause timer set to 64 (0x40)
 	 * cfep - control frame extended pause timer set to 0x0
 	 */
-	if (adapter->flowcontrol == FLOW_NONE)
+	if (adapter->flow == FLOW_NONE)
 		writel(0, &txmac->cf_param);
 	else
 		writel(0x40, &txmac->cf_param);
@@ -1278,60 +1153,13 @@ static void et1310_config_txmac_regs(struct et131x_adapter *adapter)
 
 static void et1310_config_macstat_regs(struct et131x_adapter *adapter)
 {
-	struct macstat_regs __iomem *macstat =
-		&adapter->regs->macstat;
+	struct macstat_regs __iomem *macstat = &adapter->regs->macstat;
+	u32 __iomem *reg;
 
-	/* Next we need to initialize all the macstat registers to zero on
-	 * the device.
-	 */
-	writel(0, &macstat->txrx_0_64_byte_frames);
-	writel(0, &macstat->txrx_65_127_byte_frames);
-	writel(0, &macstat->txrx_128_255_byte_frames);
-	writel(0, &macstat->txrx_256_511_byte_frames);
-	writel(0, &macstat->txrx_512_1023_byte_frames);
-	writel(0, &macstat->txrx_1024_1518_byte_frames);
-	writel(0, &macstat->txrx_1519_1522_gvln_frames);
-
-	writel(0, &macstat->rx_bytes);
-	writel(0, &macstat->rx_packets);
-	writel(0, &macstat->rx_fcs_errs);
-	writel(0, &macstat->rx_multicast_packets);
-	writel(0, &macstat->rx_broadcast_packets);
-	writel(0, &macstat->rx_control_frames);
-	writel(0, &macstat->rx_pause_frames);
-	writel(0, &macstat->rx_unknown_opcodes);
-	writel(0, &macstat->rx_align_errs);
-	writel(0, &macstat->rx_frame_len_errs);
-	writel(0, &macstat->rx_code_errs);
-	writel(0, &macstat->rx_carrier_sense_errs);
-	writel(0, &macstat->rx_undersize_packets);
-	writel(0, &macstat->rx_oversize_packets);
-	writel(0, &macstat->rx_fragment_packets);
-	writel(0, &macstat->rx_jabbers);
-	writel(0, &macstat->rx_drops);
-
-	writel(0, &macstat->tx_bytes);
-	writel(0, &macstat->tx_packets);
-	writel(0, &macstat->tx_multicast_packets);
-	writel(0, &macstat->tx_broadcast_packets);
-	writel(0, &macstat->tx_pause_frames);
-	writel(0, &macstat->tx_deferred);
-	writel(0, &macstat->tx_excessive_deferred);
-	writel(0, &macstat->tx_single_collisions);
-	writel(0, &macstat->tx_multiple_collisions);
-	writel(0, &macstat->tx_late_collisions);
-	writel(0, &macstat->tx_excessive_collisions);
-	writel(0, &macstat->tx_total_collisions);
-	writel(0, &macstat->tx_pause_honored_frames);
-	writel(0, &macstat->tx_drops);
-	writel(0, &macstat->tx_jabbers);
-	writel(0, &macstat->tx_fcs_errs);
-	writel(0, &macstat->tx_control_frames);
-	writel(0, &macstat->tx_oversize_frames);
-	writel(0, &macstat->tx_undersize_frames);
-	writel(0, &macstat->tx_fragments);
-	writel(0, &macstat->carry_reg1);
-	writel(0, &macstat->carry_reg2);
+	/* initialize all the macstat registers to zero on the device  */
+	for (reg = &macstat->txrx_0_64_byte_frames;
+	     reg <= &macstat->carry_reg2; reg++)
+		writel(0, reg);
 
 	/* Unmask any counters that we want to track the overflow of.
 	 * Initially this will be all counters.  It may become clear later
@@ -1341,14 +1169,8 @@ static void et1310_config_macstat_regs(struct et131x_adapter *adapter)
 	writel(0xFFFE7E8B, &macstat->carry_reg2_mask);
 }
 
-/* et131x_phy_mii_read - Read from the PHY through the MII Interface on the MAC
- * @adapter: pointer to our private adapter structure
- * @addr: the address of the transceiver
- * @reg: the register to read
- * @value: pointer to a 16-bit value in which the value will be stored
- */
 static int et131x_phy_mii_read(struct et131x_adapter *adapter, u8 addr,
-	      u8 reg, u16 *value)
+			       u8 reg, u16 *value)
 {
 	struct mac_regs __iomem *mac = &adapter->regs->mac;
 	int status = 0;
@@ -1380,9 +1202,9 @@ static int et131x_phy_mii_read(struct et131x_adapter *adapter, u8 addr,
 	/* If we hit the max delay, we could not read the register */
 	if (delay == 50) {
 		dev_warn(&adapter->pdev->dev,
-			    "reg 0x%08x could not be read\n", reg);
+			 "reg 0x%08x could not be read\n", reg);
 		dev_warn(&adapter->pdev->dev, "status is  0x%08x\n",
-			    mii_indicator);
+			 mii_indicator);
 
 		status = -EIO;
 		goto out;
@@ -1416,27 +1238,16 @@ static int et131x_mii_read(struct et131x_adapter *adapter, u8 reg, u16 *value)
 	return et131x_phy_mii_read(adapter, phydev->addr, reg, value);
 }
 
-/* et131x_mii_write - Write to a PHY reg through the MII interface of the MAC
- * @adapter: pointer to our private adapter structure
- * @reg: the register to read
- * @value: 16-bit value to write
- */
-static int et131x_mii_write(struct et131x_adapter *adapter, u8 reg, u16 value)
+static int et131x_mii_write(struct et131x_adapter *adapter, u8 addr, u8 reg,
+			    u16 value)
 {
 	struct mac_regs __iomem *mac = &adapter->regs->mac;
-	struct phy_device *phydev = adapter->phydev;
 	int status = 0;
-	u8 addr;
 	u32 delay = 0;
 	u32 mii_addr;
 	u32 mii_cmd;
 	u32 mii_indicator;
 
-	if (!phydev)
-		return -EIO;
-
-	addr = phydev->addr;
-
 	/* Save a local copy of the registers we are dealing with so we can
 	 * set them back
 	 */
@@ -1463,11 +1274,11 @@ static int et131x_mii_write(struct et131x_adapter *adapter, u8 reg, u16 value)
 		u16 tmp;
 
 		dev_warn(&adapter->pdev->dev,
-		    "reg 0x%08x could not be written", reg);
+			 "reg 0x%08x could not be written", reg);
 		dev_warn(&adapter->pdev->dev, "status is  0x%08x\n",
-			    mii_indicator);
+			 mii_indicator);
 		dev_warn(&adapter->pdev->dev, "command is  0x%08x\n",
-			    readl(&mac->mii_mgmt_cmd));
+			 readl(&mac->mii_mgmt_cmd));
 
 		et131x_mii_read(adapter, reg, &tmp);
 
@@ -1493,7 +1304,6 @@ static void et1310_phy_read_mii_bit(struct et131x_adapter *adapter,
 	u16 reg;
 	u16 mask = 1 << bitnum;
 
-	/* Read the requested register */
 	et131x_mii_read(adapter, regnum, &reg);
 
 	*value = (reg & mask) >> bitnum;
@@ -1504,7 +1314,7 @@ static void et1310_config_flow_control(struct et131x_adapter *adapter)
 	struct phy_device *phydev = adapter->phydev;
 
 	if (phydev->duplex == DUPLEX_HALF) {
-		adapter->flowcontrol = FLOW_NONE;
+		adapter->flow = FLOW_NONE;
 	} else {
 		char remote_pause, remote_async_pause;
 
@@ -1512,19 +1322,19 @@ static void et1310_config_flow_control(struct et131x_adapter *adapter)
 		et1310_phy_read_mii_bit(adapter, 5, 11, &remote_async_pause);
 
 		if (remote_pause && remote_async_pause) {
-			adapter->flowcontrol = adapter->wanted_flow;
+			adapter->flow = adapter->wanted_flow;
 		} else if (remote_pause && !remote_async_pause) {
 			if (adapter->wanted_flow == FLOW_BOTH)
-				adapter->flowcontrol = FLOW_BOTH;
+				adapter->flow = FLOW_BOTH;
 			else
-				adapter->flowcontrol = FLOW_NONE;
+				adapter->flow = FLOW_NONE;
 		} else if (!remote_pause && !remote_async_pause) {
-			adapter->flowcontrol = FLOW_NONE;
+			adapter->flow = FLOW_NONE;
 		} else {
 			if (adapter->wanted_flow == FLOW_BOTH)
-				adapter->flowcontrol = FLOW_RXONLY;
+				adapter->flow = FLOW_RXONLY;
 			else
-				adapter->flowcontrol = FLOW_NONE;
+				adapter->flow = FLOW_NONE;
 		}
 	}
 }
@@ -1621,8 +1431,8 @@ static int et131x_mdio_read(struct mii_bus *bus, int phy_addr, int reg)
 
 	if (ret < 0)
 		return ret;
-	else
-		return value;
+
+	return value;
 }
 
 static int et131x_mdio_write(struct mii_bus *bus, int phy_addr,
@@ -1631,17 +1441,7 @@ static int et131x_mdio_write(struct mii_bus *bus, int phy_addr,
 	struct net_device *netdev = bus->priv;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
-	return et131x_mii_write(adapter, reg, value);
-}
-
-static int et131x_mdio_reset(struct mii_bus *bus)
-{
-	struct net_device *netdev = bus->priv;
-	struct et131x_adapter *adapter = netdev_priv(netdev);
-
-	et131x_mii_write(adapter, MII_BMCR, BMCR_RESET);
-
-	return 0;
+	return et131x_mii_write(adapter, phy_addr, reg, value);
 }
 
 /*	et1310_phy_power_switch	-	PHY power control
@@ -1656,18 +1456,20 @@ static int et131x_mdio_reset(struct mii_bus *bus)
 static void et1310_phy_power_switch(struct et131x_adapter *adapter, bool down)
 {
 	u16 data;
+	struct  phy_device *phydev = adapter->phydev;
 
 	et131x_mii_read(adapter, MII_BMCR, &data);
 	data &= ~BMCR_PDOWN;
 	if (down)
 		data |= BMCR_PDOWN;
-	et131x_mii_write(adapter, MII_BMCR, data);
+	et131x_mii_write(adapter, phydev->addr, MII_BMCR, data);
 }
 
 /* et131x_xcvr_init - Init the phy if we are setting it into force mode */
 static void et131x_xcvr_init(struct et131x_adapter *adapter)
 {
 	u16 lcr2;
+	struct  phy_device *phydev = adapter->phydev;
 
 	/* Set the LED behavior such that LED 1 indicates speed (off =
 	 * 10Mbits, blink = 100Mbits, on = 1000Mbits) and LED 2 indicates
@@ -1688,14 +1490,11 @@ static void et131x_xcvr_init(struct et131x_adapter *adapter)
 		else
 			lcr2 |= (LED_VAL_LINKON << LED_TXRX_SHIFT);
 
-		et131x_mii_write(adapter, PHY_LED_2, lcr2);
+		et131x_mii_write(adapter, phydev->addr, PHY_LED_2, lcr2);
 	}
 }
 
-/* et131x_configure_global_regs	- configure JAGCore global regs
- *
- * Used to configure the global registers on the JAGCore
- */
+/* et131x_configure_global_regs	- configure JAGCore global regs */
 static void et131x_configure_global_regs(struct et131x_adapter *adapter)
 {
 	struct global_regs __iomem *regs = &adapter->regs->global;
@@ -1728,7 +1527,6 @@ static void et131x_configure_global_regs(struct et131x_adapter *adapter)
 	/* Initialize the loopback register. Disable all loopbacks. */
 	writel(0, &regs->loopback);
 
-	/* MSI Register */
 	writel(0, &regs->msi_config);
 
 	/* By default, disable the watchdog timer.  It will be enabled when
@@ -1748,7 +1546,6 @@ static void et131x_config_rx_dma_regs(struct et131x_adapter *adapter)
 	unsigned long flags;
 	u8 id;
 
-	/* Halt RXDMA to perform the reconfigure.  */
 	et131x_rx_dma_disable(adapter);
 
 	/* Load the completion writeback physical address */
@@ -1757,12 +1554,10 @@ static void et131x_config_rx_dma_regs(struct et131x_adapter *adapter)
 
 	memset(rx_local->rx_status_block, 0, sizeof(struct rx_status_block));
 
-	/* Set the address and parameters of the packet status ring into the
-	 * 1310's registers
-	 */
+	/* Set the address and parameters of the packet status ring */
 	writel(upper_32_bits(rx_local->ps_ring_physaddr), &rx_dma->psr_base_hi);
 	writel(lower_32_bits(rx_local->ps_ring_physaddr), &rx_dma->psr_base_lo);
-	writel(rx_local->psr_num_entries - 1, &rx_dma->psr_num_des);
+	writel(rx_local->psr_entries - 1, &rx_dma->psr_num_des);
 	writel(0, &rx_dma->psr_full_offset);
 
 	psr_num_des = readl(&rx_dma->psr_num_des) & ET_RXDMA_PSR_NUM_DES_MASK;
@@ -1805,9 +1600,7 @@ static void et131x_config_rx_dma_regs(struct et131x_adapter *adapter)
 			fbr_entry++;
 		}
 
-		/* Set the address and parameters of Free buffer ring 1 and 0
-		 * into the 1310's registers
-		 */
+		/* Set the address and parameters of Free buffer ring 1 and 0 */
 		writel(upper_32_bits(fbr->ring_physaddr), base_hi);
 		writel(lower_32_bits(fbr->ring_physaddr), base_lo);
 		writel(fbr->num_entries - 1, num_des);
@@ -1868,9 +1661,7 @@ static void et131x_config_tx_dma_regs(struct et131x_adapter *adapter)
 /* et131x_adapter_setup - Set the adapter up as per cassini+ documentation */
 static void et131x_adapter_setup(struct et131x_adapter *adapter)
 {
-	/* Configure the JAGCore */
 	et131x_configure_global_regs(adapter);
-
 	et1310_config_mac_regs1(adapter);
 
 	/* Configure the MMC registers */
@@ -1909,18 +1700,11 @@ static void et131x_soft_reset(struct et131x_adapter *adapter)
 	writel(0, &adapter->regs->mac.cfg1);
 }
 
-/*	et131x_enable_interrupts	-	enable interrupt
- *
- *	Enable the appropriate interrupts on the ET131x according to our
- *	configuration
- */
 static void et131x_enable_interrupts(struct et131x_adapter *adapter)
 {
 	u32 mask;
 
-	/* Enable all global interrupts */
-	if (adapter->flowcontrol == FLOW_TXONLY ||
-	    adapter->flowcontrol == FLOW_BOTH)
+	if (adapter->flow == FLOW_TXONLY || adapter->flow == FLOW_BOTH)
 		mask = INT_MASK_ENABLE;
 	else
 		mask = INT_MASK_ENABLE_NO_FLOW;
@@ -1928,61 +1712,46 @@ static void et131x_enable_interrupts(struct et131x_adapter *adapter)
 	writel(mask, &adapter->regs->global.int_mask);
 }
 
-/*	et131x_disable_interrupts	-	interrupt disable
- *
- *	Block all interrupts from the et131x device at the device itself
- */
 static void et131x_disable_interrupts(struct et131x_adapter *adapter)
 {
-	/* Disable all global interrupts */
 	writel(INT_MASK_DISABLE, &adapter->regs->global.int_mask);
 }
 
-/* et131x_tx_dma_disable - Stop of Tx_DMA on the ET1310 */
 static void et131x_tx_dma_disable(struct et131x_adapter *adapter)
 {
 	/* Setup the transmit dma configuration register */
 	writel(ET_TXDMA_CSR_HALT | ET_TXDMA_SNGL_EPKT,
-					&adapter->regs->txdma.csr);
+	       &adapter->regs->txdma.csr);
 }
 
-/* et131x_enable_txrx - Enable tx/rx queues */
 static void et131x_enable_txrx(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
-	/* Enable the Tx and Rx DMA engines (if not already enabled) */
 	et131x_rx_dma_enable(adapter);
 	et131x_tx_dma_enable(adapter);
 
-	/* Enable device interrupts */
 	if (adapter->flags & FMP_ADAPTER_INTERRUPT_IN_USE)
 		et131x_enable_interrupts(adapter);
 
-	/* We're ready to move some data, so start the queue */
 	netif_start_queue(netdev);
 }
 
-/* et131x_disable_txrx - Disable tx/rx queues */
 static void et131x_disable_txrx(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
-	/* First thing is to stop the queue */
 	netif_stop_queue(netdev);
 
-	/* Stop the Tx and Rx DMA engines */
 	et131x_rx_dma_disable(adapter);
 	et131x_tx_dma_disable(adapter);
 
-	/* Disable device interrupts */
 	et131x_disable_interrupts(adapter);
 }
 
-/* et131x_init_send - Initialize send data structures */
 static void et131x_init_send(struct et131x_adapter *adapter)
 {
-	u32 ct;
+	int i;
 	struct tx_ring *tx_ring = &adapter->tx_ring;
 	struct tcb *tcb = tx_ring->tcb_ring;
 
@@ -1990,14 +1759,11 @@ static void et131x_init_send(struct et131x_adapter *adapter)
 
 	memset(tcb, 0, sizeof(struct tcb) * NUM_TCB);
 
-	/* Go through and set up each TCB */
-	for (ct = 0; ct++ < NUM_TCB; tcb++)
-		/* Set the link pointer in HW TCB to the next TCB in the
-		 * chain
-		 */
+	for (i = 0; i < NUM_TCB; i++) {
 		tcb->next = tcb + 1;
+		tcb++;
+	}
 
-	/* Set the  tail pointer */
 	tcb--;
 	tx_ring->tcb_qtail = tcb;
 	tcb->next = NULL;
@@ -2006,7 +1772,7 @@ static void et131x_init_send(struct et131x_adapter *adapter)
 	tx_ring->send_tail = NULL;
 }
 
-/* et1310_enable_phy_coma - called when network cable is unplugged
+/* et1310_enable_phy_coma
  *
  * driver receive an phy status change interrupt while in D0 and check that
  * phy_status is down.
@@ -2015,33 +1781,15 @@ static void et131x_init_send(struct et131x_adapter *adapter)
  *          -- set gigE PHY in Coma mode
  *          -- wake on phy_interrupt; Perform software reset JAGCore,
  *             re-initialize jagcore and gigE PHY
- *
- *      Add D0-ASPM-PhyLinkDown Support:
- *          -- while in D0, when there is a phy_interrupt indicating phy link
- *             down status, call the MPSetPhyComa routine to enter this active
- *             state power saving mode
- *          -- while in D0-ASPM-PhyLinkDown mode, when there is a phy_interrupt
- *       indicating linkup status, call the MPDisablePhyComa routine to
- *             restore JAGCore and gigE PHY
  */
 static void et1310_enable_phy_coma(struct et131x_adapter *adapter)
 {
-	unsigned long flags;
-	u32 pmcsr;
-
-	pmcsr = readl(&adapter->regs->global.pm_csr);
-
-	/* Save the GbE PHY speed and duplex modes. Need to restore this
-	 * when cable is plugged back in
-	 */
+	u32 pmcsr = readl(&adapter->regs->global.pm_csr);
 
 	/* Stop sending packets. */
-	spin_lock_irqsave(&adapter->send_hw_lock, flags);
 	adapter->flags |= FMP_ADAPTER_LOWER_POWER;
-	spin_unlock_irqrestore(&adapter->send_hw_lock, flags);
 
 	/* Wait for outstanding Receive packets */
-
 	et131x_disable_txrx(adapter->netdev);
 
 	/* Gate off JAGCore 3 clock domains */
@@ -2053,7 +1801,6 @@ static void et1310_enable_phy_coma(struct et131x_adapter *adapter)
 	writel(pmcsr, &adapter->regs->global.pm_csr);
 }
 
-/* et1310_disable_phy_coma - Disable the Phy Coma Mode */
 static void et1310_disable_phy_coma(struct et131x_adapter *adapter)
 {
 	u32 pmcsr;
@@ -2078,7 +1825,6 @@ static void et1310_disable_phy_coma(struct et131x_adapter *adapter)
 	 */
 	et131x_soft_reset(adapter);
 
-	/* setup et1310 as per the documentation ?? */
 	et131x_adapter_setup(adapter);
 
 	/* Allow Tx to restart */
@@ -2117,16 +1863,16 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 	u8 id;
 	u32 i, j;
 	u32 bufsize;
-	u32 pktstat_ringsize;
+	u32 psr_size;
 	u32 fbr_chunksize;
 	struct rx_ring *rx_ring = &adapter->rx_ring;
 	struct fbr_lookup *fbr;
 
 	/* Alloc memory for the lookup table */
-	rx_ring->fbr[0] = kmalloc(sizeof(struct fbr_lookup), GFP_KERNEL);
+	rx_ring->fbr[0] = kzalloc(sizeof(*fbr), GFP_KERNEL);
 	if (rx_ring->fbr[0] == NULL)
 		return -ENOMEM;
-	rx_ring->fbr[1] = kmalloc(sizeof(struct fbr_lookup), GFP_KERNEL);
+	rx_ring->fbr[1] = kzalloc(sizeof(*fbr), GFP_KERNEL);
 	if (rx_ring->fbr[1] == NULL)
 		return -ENOMEM;
 
@@ -2147,7 +1893,6 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 	 * rings. Also, FBR1 remains a constant size - when it's size doubles
 	 * the number of entries halves.  FBR0 increases in size, however.
 	 */
-
 	if (adapter->registry_jumbo_packet < 2048) {
 		rx_ring->fbr[0]->buffsize = 256;
 		rx_ring->fbr[0]->num_entries = 512;
@@ -2165,8 +1910,8 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 		rx_ring->fbr[1]->num_entries = 128;
 	}
 
-	rx_ring->psr_num_entries = rx_ring->fbr[0]->num_entries +
-				   rx_ring->fbr[1]->num_entries;
+	rx_ring->psr_entries = rx_ring->fbr[0]->num_entries +
+			       rx_ring->fbr[1]->num_entries;
 
 	for (id = 0; id < NUM_FBRS; id++) {
 		fbr = rx_ring->fbr[id];
@@ -2178,7 +1923,8 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 							GFP_KERNEL);
 		if (!fbr->ring_virtaddr) {
 			dev_err(&adapter->pdev->dev,
-			   "Cannot alloc memory for Free Buffer Ring %d\n", id);
+				"Cannot alloc memory for Free Buffer Ring %d\n",
+				id);
 			return -ENOMEM;
 		}
 	}
@@ -2188,7 +1934,7 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 		fbr_chunksize = (FBR_CHUNKS * fbr->buffsize);
 
 		for (i = 0; i < fbr->num_entries / FBR_CHUNKS; i++) {
-			dma_addr_t fbr_tmp_physaddr;
+			dma_addr_t fbr_physaddr;
 
 			fbr->mem_virtaddrs[i] = dma_alloc_coherent(
 					&adapter->pdev->dev, fbr_chunksize,
@@ -2202,51 +1948,41 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 			}
 
 			/* See NOTE in "Save Physical Address" comment above */
-			fbr_tmp_physaddr = fbr->mem_physaddrs[i];
+			fbr_physaddr = fbr->mem_physaddrs[i];
 
 			for (j = 0; j < FBR_CHUNKS; j++) {
-				u32 index = (i * FBR_CHUNKS) + j;
+				u32 k = (i * FBR_CHUNKS) + j;
 
 				/* Save the Virtual address of this index for
 				 * quick access later
 				 */
-				fbr->virt[index] = (u8 *)fbr->mem_virtaddrs[i] +
+				fbr->virt[k] = (u8 *)fbr->mem_virtaddrs[i] +
 						   (j * fbr->buffsize);
 
 				/* now store the physical address in the
 				 * descriptor so the device can access it
 				 */
-				fbr->bus_high[index] =
-						upper_32_bits(fbr_tmp_physaddr);
-				fbr->bus_low[index] =
-						lower_32_bits(fbr_tmp_physaddr);
-
-				fbr_tmp_physaddr += fbr->buffsize;
+				fbr->bus_high[k] = upper_32_bits(fbr_physaddr);
+				fbr->bus_low[k] = lower_32_bits(fbr_physaddr);
+				fbr_physaddr += fbr->buffsize;
 			}
 		}
 	}
 
 	/* Allocate an area of memory for FIFO of Packet Status ring entries */
-	pktstat_ringsize =
-		sizeof(struct pkt_stat_desc) * rx_ring->psr_num_entries;
+	psr_size = sizeof(struct pkt_stat_desc) * rx_ring->psr_entries;
 
 	rx_ring->ps_ring_virtaddr = dma_alloc_coherent(&adapter->pdev->dev,
-						  pktstat_ringsize,
+						  psr_size,
 						  &rx_ring->ps_ring_physaddr,
 						  GFP_KERNEL);
 
 	if (!rx_ring->ps_ring_virtaddr) {
 		dev_err(&adapter->pdev->dev,
-			  "Cannot alloc memory for Packet Status Ring\n");
+			"Cannot alloc memory for Packet Status Ring\n");
 		return -ENOMEM;
 	}
 
-	/* NOTE : dma_alloc_coherent(), used above to alloc DMA regions,
-	 * ALWAYS returns SAC (32-bit) addresses. If DAC (64-bit) addresses
-	 * are ever returned, make sure the high part is retrieved here before
-	 * storing the adjusted address.
-	 */
-
 	/* Allocate an area of memory for writeback of status information */
 	rx_ring->rx_status_block = dma_alloc_coherent(&adapter->pdev->dev,
 					    sizeof(struct rx_status_block),
@@ -2254,7 +1990,7 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 					    GFP_KERNEL);
 	if (!rx_ring->rx_status_block) {
 		dev_err(&adapter->pdev->dev,
-			  "Cannot alloc memory for Status Block\n");
+			"Cannot alloc memory for Status Block\n");
 		return -ENOMEM;
 	}
 	rx_ring->num_rfd = NIC_DEFAULT_NUM_RFD;
@@ -2266,13 +2002,12 @@ static int et131x_rx_dma_memory_alloc(struct et131x_adapter *adapter)
 	return 0;
 }
 
-/* et131x_rx_dma_memory_free - Free all memory allocated within this module */
 static void et131x_rx_dma_memory_free(struct et131x_adapter *adapter)
 {
 	u8 id;
-	u32 index;
+	u32 ii;
 	u32 bufsize;
-	u32 pktstat_ringsize;
+	u32 psr_size;
 	struct rfd *rfd;
 	struct rx_ring *rx_ring = &adapter->rx_ring;
 	struct fbr_lookup *fbr;
@@ -2297,18 +2032,16 @@ static void et131x_rx_dma_memory_free(struct et131x_adapter *adapter)
 			continue;
 
 		/* First the packet memory */
-		for (index = 0;
-		     index < fbr->num_entries / FBR_CHUNKS;
-		     index++) {
-			if (fbr->mem_virtaddrs[index]) {
+		for (ii = 0; ii < fbr->num_entries / FBR_CHUNKS; ii++) {
+			if (fbr->mem_virtaddrs[ii]) {
 				bufsize = fbr->buffsize * FBR_CHUNKS;
 
 				dma_free_coherent(&adapter->pdev->dev,
 						  bufsize,
-						  fbr->mem_virtaddrs[index],
-						  fbr->mem_physaddrs[index]);
+						  fbr->mem_virtaddrs[ii],
+						  fbr->mem_physaddrs[ii]);
 
-				fbr->mem_virtaddrs[index] = NULL;
+				fbr->mem_virtaddrs[ii] = NULL;
 			}
 		}
 
@@ -2324,12 +2057,11 @@ static void et131x_rx_dma_memory_free(struct et131x_adapter *adapter)
 
 	/* Free Packet Status Ring */
 	if (rx_ring->ps_ring_virtaddr) {
-		pktstat_ringsize = sizeof(struct pkt_stat_desc) *
-					rx_ring->psr_num_entries;
+		psr_size = sizeof(struct pkt_stat_desc) * rx_ring->psr_entries;
 
-		dma_free_coherent(&adapter->pdev->dev, pktstat_ringsize,
-				    rx_ring->ps_ring_virtaddr,
-				    rx_ring->ps_ring_physaddr);
+		dma_free_coherent(&adapter->pdev->dev, psr_size,
+				  rx_ring->ps_ring_virtaddr,
+				  rx_ring->ps_ring_physaddr);
 
 		rx_ring->ps_ring_virtaddr = NULL;
 	}
@@ -2337,8 +2069,9 @@ static void et131x_rx_dma_memory_free(struct et131x_adapter *adapter)
 	/* Free area of memory for the writeback of status information */
 	if (rx_ring->rx_status_block) {
 		dma_free_coherent(&adapter->pdev->dev,
-			sizeof(struct rx_status_block),
-			rx_ring->rx_status_block, rx_ring->rx_status_bus);
+				  sizeof(struct rx_status_block),
+				  rx_ring->rx_status_block,
+				  rx_ring->rx_status_bus);
 		rx_ring->rx_status_block = NULL;
 	}
 
@@ -2359,7 +2092,7 @@ static int et131x_init_recv(struct et131x_adapter *adapter)
 
 	/* Setup each RFD */
 	for (rfdct = 0; rfdct < rx_ring->num_rfd; rfdct++) {
-		rfd = kzalloc(sizeof(struct rfd), GFP_ATOMIC | GFP_DMA);
+		rfd = kzalloc(sizeof(*rfd), GFP_ATOMIC | GFP_DMA);
 		if (!rfd)
 			return -ENOMEM;
 
@@ -2389,10 +2122,7 @@ static void et131x_set_rx_dma_timer(struct et131x_adapter *adapter)
 	}
 }
 
-/* NICReturnRFD - Recycle a RFD and put it back onto the receive list
- * @adapter: pointer to our adapter
- * @rfd: pointer to the RFD
- */
+/* nic_return_rfd - Recycle a RFD and put it back onto the receive list */
 static void nic_return_rfd(struct et131x_adapter *adapter, struct rfd *rfd)
 {
 	struct rx_ring *rx_local = &adapter->rx_ring;
@@ -2410,8 +2140,6 @@ static void nic_return_rfd(struct et131x_adapter *adapter, struct rfd *rfd)
 		u32 __iomem *offset;
 		struct fbr_desc *next;
 
-		spin_lock_irqsave(&adapter->fbr_lock, flags);
-
 		if (ring_index == 0)
 			offset = &rx_dma->fbr0_full_offset;
 		else
@@ -2431,11 +2159,9 @@ static void nic_return_rfd(struct et131x_adapter *adapter, struct rfd *rfd)
 		free_buff_ring = bump_free_buff_ring(&fbr->local_full,
 						     fbr->num_entries - 1);
 		writel(free_buff_ring, offset);
-
-		spin_unlock_irqrestore(&adapter->fbr_lock, flags);
 	} else {
 		dev_err(&adapter->pdev->dev,
-			  "%s illegal Buffer Index returned\n", __func__);
+			"%s illegal Buffer Index returned\n", __func__);
 	}
 
 	/* The processing on this RFD is done, so put it back on the tail of
@@ -2451,8 +2177,6 @@ static void nic_return_rfd(struct et131x_adapter *adapter, struct rfd *rfd)
 
 /* nic_rx_pkts - Checks the hardware for available packets
  *
- * Returns rfd, a pointer to our MPRFD.
- *
  * Checks the hardware for available packets, using completion ring
  * If packets are available, it gets an RFD from the recv_list, attaches
  * the packet to it, puts the RFD in the RecvPendList, and also returns
@@ -2464,8 +2188,6 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 	struct rx_status_block *status;
 	struct pkt_stat_desc *psr;
 	struct rfd *rfd;
-	u32 i;
-	u8 *buf;
 	unsigned long flags;
 	struct list_head *element;
 	u8 ring_index;
@@ -2481,14 +2203,14 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 	 * Status Ring, and also the two Free Buffer rings.
 	 */
 	status = rx_local->rx_status_block;
-	word1 = status->word1 >> 16;	/* Get the useful bits */
+	word1 = status->word1 >> 16;
 
 	/* Check the PSR and wrap bits do not match */
 	if ((word1 & 0x1FFF) == (rx_local->local_psr_full & 0x1FFF))
 		return NULL; /* Looks like this ring is not updated yet */
 
 	/* The packet status ring indicates that data is available. */
-	psr = (struct pkt_stat_desc *) (rx_local->ps_ring_virtaddr) +
+	psr = (struct pkt_stat_desc *)(rx_local->ps_ring_virtaddr) +
 			(rx_local->local_psr_full & 0xFFF);
 
 	/* Grab any information that is required once the PSR is advanced,
@@ -2503,8 +2225,7 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 	/* Indicate that we have used this PSR entry. */
 	/* FIXME wrap 12 */
 	add_12bit(&rx_local->local_psr_full, 1);
-	if (
-	  (rx_local->local_psr_full & 0xFFF) > rx_local->psr_num_entries - 1) {
+	if ((rx_local->local_psr_full & 0xFFF) > rx_local->psr_entries - 1) {
 		/* Clear psr full and toggle the wrap bit */
 		rx_local->local_psr_full &=  ~0xFFF;
 		rx_local->local_psr_full ^= 0x1000;
@@ -2545,76 +2266,18 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 	 */
 	if (len < (NIC_MIN_PACKET_SIZE + 4)) {
 		adapter->stats.rx_other_errs++;
-		len = 0;
-	}
-
-	if (len == 0) {
 		rfd->len = 0;
 		goto out;
 	}
 
-	/* Determine if this is a multicast packet coming in */
-	if ((word0 & ALCATEL_MULTICAST_PKT) &&
-	    !(word0 & ALCATEL_BROADCAST_PKT)) {
-		/* Promiscuous mode and Multicast mode are not mutually
-		 * exclusive as was first thought. I guess Promiscuous is just
-		 * considered a super-set of the other filters. Generally filter
-		 * is 0x2b when in promiscuous mode.
-		 */
-		if ((adapter->packet_filter & ET131X_PACKET_TYPE_MULTICAST)
-		   && !(adapter->packet_filter & ET131X_PACKET_TYPE_PROMISCUOUS)
-		   && !(adapter->packet_filter &
-					ET131X_PACKET_TYPE_ALL_MULTICAST)) {
-			buf = fbr->virt[buff_index];
-
-			/* Loop through our list to see if the destination
-			 * address of this packet matches one in our list.
-			 */
-			for (i = 0; i < adapter->multicast_addr_count; i++) {
-				if (buf[0] == adapter->multicast_list[i][0]
-				 && buf[1] == adapter->multicast_list[i][1]
-				 && buf[2] == adapter->multicast_list[i][2]
-				 && buf[3] == adapter->multicast_list[i][3]
-				 && buf[4] == adapter->multicast_list[i][4]
-				 && buf[5] == adapter->multicast_list[i][5]) {
-					break;
-				}
-			}
-
-			/* If our index is equal to the number of Multicast
-			 * address we have, then this means we did not find this
-			 * packet's matching address in our list. Set the len to
-			 * zero, so we free our RFD when we return from this
-			 * function.
-			 */
-			if (i == adapter->multicast_addr_count)
-				len = 0;
-		}
-
-		if (len > 0)
-			adapter->stats.multicast_pkts_rcvd++;
-	} else if (word0 & ALCATEL_BROADCAST_PKT) {
-		adapter->stats.broadcast_pkts_rcvd++;
-	} else {
-		/* Not sure what this counter measures in promiscuous mode.
-		 * Perhaps we should check the MAC address to see if it is
-		 * directed to us in promiscuous mode.
-		 */
-		adapter->stats.unicast_pkts_rcvd++;
-	}
-
-	if (!len) {
-		rfd->len = 0;
-		goto out;
-	}
+	if ((word0 & ALCATEL_MULTICAST_PKT) && !(word0 & ALCATEL_BROADCAST_PKT))
+		adapter->stats.multicast_pkts_rcvd++;
 
 	rfd->len = len;
 
 	skb = dev_alloc_skb(rfd->len + 2);
-	if (!skb) {
-		dev_err(&adapter->pdev->dev, "Couldn't alloc an SKB for Rx\n");
+	if (!skb)
 		return NULL;
-	}
 
 	adapter->netdev->stats.rx_bytes += rfd->len;
 
@@ -2622,26 +2285,26 @@ static struct rfd *nic_rx_pkts(struct et131x_adapter *adapter)
 
 	skb->protocol = eth_type_trans(skb, adapter->netdev);
 	skb->ip_summed = CHECKSUM_NONE;
-	netif_rx_ni(skb);
+	netif_receive_skb(skb);
 
 out:
 	nic_return_rfd(adapter, rfd);
 	return rfd;
 }
 
-/* et131x_handle_recv_interrupt - Interrupt handler for receive processing
- *
- * Assumption, Rcv spinlock has been acquired.
- */
-static void et131x_handle_recv_interrupt(struct et131x_adapter *adapter)
+static int et131x_handle_recv_pkts(struct et131x_adapter *adapter, int budget)
 {
 	struct rfd *rfd = NULL;
-	u32 count = 0;
+	int count = 0;
+	int limit = budget;
 	bool done = true;
 	struct rx_ring *rx_ring = &adapter->rx_ring;
 
+	if (budget > MAX_PACKETS_HANDLED)
+		limit = MAX_PACKETS_HANDLED;
+
 	/* Process up to available RFD's */
-	while (count < NUM_PACKETS_HANDLED) {
+	while (count < limit) {
 		if (list_empty(&rx_ring->recv_list)) {
 			WARN_ON(rx_ring->num_ready_recv != 0);
 			done = false;
@@ -2663,23 +2326,24 @@ static void et131x_handle_recv_interrupt(struct et131x_adapter *adapter)
 		    rfd->len == 0)
 			continue;
 
-		/* Increment the number of packets we received */
 		adapter->netdev->stats.rx_packets++;
 
-		/* Set the status on the packet, either resources or success */
 		if (rx_ring->num_ready_recv < RFD_LOW_WATER_MARK)
 			dev_warn(&adapter->pdev->dev, "RFD's are running out\n");
 
 		count++;
 	}
 
-	if (count == NUM_PACKETS_HANDLED || !done) {
+	if (count == limit || !done) {
 		rx_ring->unfinished_receives = true;
 		writel(PARM_TX_TIME_INT_DEF * NANO_IN_A_MICRO,
 		       &adapter->regs->global.watchdog_timer);
-	} else
+	} else {
 		/* Watchdog timer will disable itself if appropriate. */
 		rx_ring->unfinished_receives = false;
+	}
+
+	return count;
 }
 
 /* et131x_tx_dma_memory_alloc
@@ -2712,14 +2376,6 @@ static int et131x_tx_dma_memory_alloc(struct et131x_adapter *adapter)
 		return -ENOMEM;
 	}
 
-	/* Save physical address
-	 *
-	 * NOTE: dma_alloc_coherent(), used above to alloc DMA regions,
-	 * ALWAYS returns SAC (32-bit) addresses. If DAC (64-bit) addresses
-	 * are ever returned, make sure the high part is retrieved here before
-	 * storing the adjusted address.
-	 */
-	/* Allocate memory for the Tx status block */
 	tx_ring->tx_status = dma_alloc_coherent(&adapter->pdev->dev,
 						    sizeof(u32),
 						    &tx_ring->tx_status_pa,
@@ -2732,7 +2388,6 @@ static int et131x_tx_dma_memory_alloc(struct et131x_adapter *adapter)
 	return 0;
 }
 
-/* et131x_tx_dma_memory_free - Free all memory allocated within this module */
 static void et131x_tx_dma_memory_free(struct et131x_adapter *adapter)
 {
 	int desc_size = 0;
@@ -2761,20 +2416,16 @@ static void et131x_tx_dma_memory_free(struct et131x_adapter *adapter)
 	kfree(tx_ring->tcb_ring);
 }
 
-/* nic_send_packet - NIC specific send handler for version B silicon.
- * @adapter: pointer to our adapter
- * @tcb: pointer to struct tcb
- */
+/* nic_send_packet - NIC specific send handler for version B silicon. */
 static int nic_send_packet(struct et131x_adapter *adapter, struct tcb *tcb)
 {
 	u32 i;
-	struct tx_desc desc[24];	/* 24 x 16 byte */
+	struct tx_desc desc[24];
 	u32 frag = 0;
 	u32 thiscopy, remainder;
 	struct sk_buff *skb = tcb->skb;
 	u32 nr_frags = skb_shinfo(skb)->nr_frags + 1;
 	struct skb_frag_struct *frags = &skb_shinfo(skb)->frags[0];
-	unsigned long flags;
 	struct phy_device *phydev = adapter->phydev;
 	dma_addr_t dma_addr;
 	struct tx_ring *tx_ring = &adapter->tx_ring;
@@ -2782,10 +2433,6 @@ static int nic_send_packet(struct et131x_adapter *adapter, struct tcb *tcb)
 	/* Part of the optimizations of this send routine restrict us to
 	 * sending 24 fragments at a pass.  In practice we should never see
 	 * more than 5 fragments.
-	 *
-	 * NOTE: The older version of this function (below) can handle any
-	 * number of fragments. If needed, we can call this function,
-	 * although it is less efficient.
 	 */
 
 	/* nr_frags should be no more than 18. */
@@ -2822,19 +2469,19 @@ static int nic_send_packet(struct et131x_adapter *adapter, struct tcb *tcb)
 			} else {
 				desc[frag].len_vlan = skb_headlen(skb) / 2;
 				dma_addr = dma_map_single(&adapter->pdev->dev,
-							 skb->data,
-							 (skb_headlen(skb) / 2),
-							 DMA_TO_DEVICE);
+							  skb->data,
+							  skb_headlen(skb) / 2,
+							  DMA_TO_DEVICE);
 				desc[frag].addr_lo = lower_32_bits(dma_addr);
 				desc[frag].addr_hi = upper_32_bits(dma_addr);
 				frag++;
 
 				desc[frag].len_vlan = skb_headlen(skb) / 2;
 				dma_addr = dma_map_single(&adapter->pdev->dev,
-							 skb->data +
-							 (skb_headlen(skb) / 2),
-							 (skb_headlen(skb) / 2),
-							 DMA_TO_DEVICE);
+							  skb->data +
+							  skb_headlen(skb) / 2,
+							  skb_headlen(skb) / 2,
+							  DMA_TO_DEVICE);
 				desc[frag].addr_lo = lower_32_bits(dma_addr);
 				desc[frag].addr_hi = upper_32_bits(dma_addr);
 				frag++;
@@ -2861,17 +2508,16 @@ static int nic_send_packet(struct et131x_adapter *adapter, struct tcb *tcb)
 		} else { /* Last element */
 			desc[frag - 1].flags = TXDESC_FLAG_LASTPKT;
 		}
-	} else
+	} else {
 		desc[frag - 1].flags =
 				    TXDESC_FLAG_INTPROC | TXDESC_FLAG_LASTPKT;
+	}
 
 	desc[0].flags |= TXDESC_FLAG_FIRSTPKT;
 
 	tcb->index_start = tx_ring->send_idx;
 	tcb->stale = 0;
 
-	spin_lock_irqsave(&adapter->send_hw_lock, flags);
-
 	thiscopy = NUM_DESC_PER_RING_TX - INDEX10(tx_ring->send_idx);
 
 	if (thiscopy >= frag) {
@@ -2888,7 +2534,7 @@ static int nic_send_packet(struct et131x_adapter *adapter, struct tcb *tcb)
 	add_10bit(&tx_ring->send_idx, thiscopy);
 
 	if (INDEX10(tx_ring->send_idx) == 0 ||
-		  INDEX10(tx_ring->send_idx) == NUM_DESC_PER_RING_TX) {
+	    INDEX10(tx_ring->send_idx) == NUM_DESC_PER_RING_TX) {
 		tx_ring->send_idx &= ~ET_DMA10_MASK;
 		tx_ring->send_idx ^= ET_DMA10_WRAP;
 	}
@@ -2906,8 +2552,9 @@ static int nic_send_packet(struct et131x_adapter *adapter, struct tcb *tcb)
 			tcb->index = NUM_DESC_PER_RING_TX - 1;
 		else
 			tcb->index = ET_DMA10_WRAP|(NUM_DESC_PER_RING_TX - 1);
-	} else
+	} else {
 		tcb->index = tx_ring->send_idx - 1;
+	}
 
 	spin_lock(&adapter->tcb_send_qlock);
 
@@ -2934,20 +2581,13 @@ static int nic_send_packet(struct et131x_adapter *adapter, struct tcb *tcb)
 		writel(PARM_TX_TIME_INT_DEF * NANO_IN_A_MICRO,
 		       &adapter->regs->global.watchdog_timer);
 	}
-	spin_unlock_irqrestore(&adapter->send_hw_lock, flags);
-
 	return 0;
 }
 
-/* send_packet - Do the work to send a packet
- *
- * Assumption: Send spinlock has been acquired
- */
 static int send_packet(struct sk_buff *skb, struct et131x_adapter *adapter)
 {
 	int status;
 	struct tcb *tcb;
-	u16 *shbufva;
 	unsigned long flags;
 	struct tx_ring *tx_ring = &adapter->tx_ring;
 
@@ -2955,7 +2595,6 @@ static int send_packet(struct sk_buff *skb, struct et131x_adapter *adapter)
 	if (skb->len < ETH_HLEN)
 		return -EIO;
 
-	/* Get a TCB for this packet */
 	spin_lock_irqsave(&adapter->tcb_ready_qlock, flags);
 
 	tcb = tx_ring->tcb_qhead;
@@ -2973,20 +2612,8 @@ static int send_packet(struct sk_buff *skb, struct et131x_adapter *adapter)
 	spin_unlock_irqrestore(&adapter->tcb_ready_qlock, flags);
 
 	tcb->skb = skb;
-
-	if (skb->data != NULL && skb_headlen(skb) >= 6) {
-		shbufva = (u16 *) skb->data;
-
-		if ((shbufva[0] == 0xffff) &&
-		    (shbufva[1] == 0xffff) && (shbufva[2] == 0xffff))
-			tcb->flags |= FMP_DEST_BROAD;
-		else if ((shbufva[0] & 0x3) == 0x0001)
-			tcb->flags |=  FMP_DEST_MULTI;
-	}
-
 	tcb->next = NULL;
 
-	/* Call the NIC specific send handler. */
 	status = nic_send_packet(adapter, tcb);
 
 	if (status != 0) {
@@ -3006,60 +2633,9 @@ static int send_packet(struct sk_buff *skb, struct et131x_adapter *adapter)
 	return 0;
 }
 
-/* et131x_send_packets - This function is called by the OS to send packets */
-static int et131x_send_packets(struct sk_buff *skb, struct net_device *netdev)
-{
-	int status = 0;
-	struct et131x_adapter *adapter = netdev_priv(netdev);
-	struct tx_ring *tx_ring = &adapter->tx_ring;
-
-	/* Send these packets
-	 *
-	 * NOTE: The Linux Tx entry point is only given one packet at a time
-	 * to Tx, so the PacketCount and it's array used makes no sense here
-	 */
-
-	/* TCB is not available */
-	if (tx_ring->used >= NUM_TCB) {
-		/* NOTE: If there's an error on send, no need to queue the
-		 * packet under Linux; if we just send an error up to the
-		 * netif layer, it will resend the skb to us.
-		 */
-		status = -ENOMEM;
-	} else {
-		/* We need to see if the link is up; if it's not, make the
-		 * netif layer think we're good and drop the packet
-		 */
-		if ((adapter->flags & FMP_ADAPTER_FAIL_SEND_MASK) ||
-					!netif_carrier_ok(netdev)) {
-			dev_kfree_skb_any(skb);
-			skb = NULL;
-
-			adapter->netdev->stats.tx_dropped++;
-		} else {
-			status = send_packet(skb, adapter);
-			if (status != 0 && status != -ENOMEM) {
-				/* On any other error, make netif think we're
-				 * OK and drop the packet
-				 */
-				dev_kfree_skb_any(skb);
-				skb = NULL;
-				adapter->netdev->stats.tx_dropped++;
-			}
-		}
-	}
-	return status;
-}
-
-/* free_send_packet - Recycle a struct tcb
- * @adapter: pointer to our adapter
- * @tcb: pointer to struct tcb
- *
- * Complete the packet if necessary
- * Assumption - Send spinlock has been acquired
- */
+/* free_send_packet - Recycle a struct tcb */
 static inline void free_send_packet(struct et131x_adapter *adapter,
-						struct tcb *tcb)
+				    struct tcb *tcb)
 {
 	unsigned long flags;
 	struct tx_desc *desc = NULL;
@@ -3067,13 +2643,6 @@ static inline void free_send_packet(struct et131x_adapter *adapter,
 	struct tx_ring *tx_ring = &adapter->tx_ring;
 	u64  dma_addr;
 
-	if (tcb->flags & FMP_DEST_BROAD)
-		atomic_inc(&adapter->stats.broadcast_pkts_xmtd);
-	else if (tcb->flags & FMP_DEST_MULTI)
-		atomic_inc(&adapter->stats.multicast_pkts_xmtd);
-	else
-		atomic_inc(&adapter->stats.unicast_pkts_xmtd);
-
 	if (tcb->skb) {
 		stats->tx_bytes += tcb->skb->len;
 
@@ -3112,8 +2681,7 @@ static inline void free_send_packet(struct et131x_adapter *adapter,
 
 	if (tx_ring->tcb_qtail)
 		tx_ring->tcb_qtail->next = tcb;
-	else
-		/* Apparently ready Q is empty. */
+	else /* Apparently ready Q is empty. */
 		tx_ring->tcb_qhead = tcb;
 
 	tx_ring->tcb_qtail = tcb;
@@ -3122,10 +2690,7 @@ static inline void free_send_packet(struct et131x_adapter *adapter,
 	WARN_ON(tx_ring->used < 0);
 }
 
-/* et131x_free_busy_send_packets - Free and complete the stopped active sends
- *
- * Assumption - Send spinlock has been acquired
- */
+/* et131x_free_busy_send_packets - Free and complete the stopped active sends */
 static void et131x_free_busy_send_packets(struct et131x_adapter *adapter)
 {
 	struct tcb *tcb;
@@ -3165,14 +2730,12 @@ static void et131x_free_busy_send_packets(struct et131x_adapter *adapter)
 	tx_ring->used = 0;
 }
 
-/* et131x_handle_send_interrupt - Interrupt handler for sending processing
+/* et131x_handle_send_pkts
  *
  * Re-claim the send resources, complete sends and get more to send from
  * the send wait queue.
- *
- * Assumption - Send spinlock has been acquired
  */
-static void et131x_handle_send_interrupt(struct et131x_adapter *adapter)
+static void et131x_handle_send_pkts(struct et131x_adapter *adapter)
 {
 	unsigned long flags;
 	u32 serviced;
@@ -3206,8 +2769,8 @@ static void et131x_handle_send_interrupt(struct et131x_adapter *adapter)
 		tcb = tx_ring->send_head;
 	}
 	while (tcb &&
-	       !((serviced ^ tcb->index) & ET_DMA10_WRAP)
-	       && index > (tcb->index & ET_DMA10_MASK)) {
+	       !((serviced ^ tcb->index) & ET_DMA10_WRAP) &&
+	       index > (tcb->index & ET_DMA10_MASK)) {
 		tx_ring->used--;
 		tx_ring->send_head = tcb->next;
 		if (tcb->next == NULL)
@@ -3424,7 +2987,7 @@ static struct ethtool_ops et131x_ethtool_ops = {
 	.get_link	= ethtool_op_get_link,
 };
 
-/* et131x_hwaddr_init - set up the MAC Address on the ET1310 */
+/* et131x_hwaddr_init - set up the MAC Address */
 static void et131x_hwaddr_init(struct et131x_adapter *adapter)
 {
 	/* If have our default mac from init and no mac address from
@@ -3441,23 +3004,16 @@ static void et131x_hwaddr_init(struct et131x_adapter *adapter)
 		 * working with so we need to copy the current
 		 * address into the permanent address
 		 */
-		memcpy(adapter->rom_addr,
-			adapter->addr, ETH_ALEN);
+		ether_addr_copy(adapter->rom_addr, adapter->addr);
 	} else {
 		/* We do not have an override address, so set the
 		 * current address to the permanent address and add
 		 * it to the device
 		 */
-		memcpy(adapter->addr,
-		       adapter->rom_addr, ETH_ALEN);
+		ether_addr_copy(adapter->addr, adapter->rom_addr);
 	}
 }
 
-/* et131x_pci_init	 - initial PCI setup
- *
- * Perform the initial setup of PCI registers and if possible initialise
- * the MAC address. At this point the I/O registers have yet to be mapped
- */
 static int et131x_pci_init(struct et131x_adapter *adapter,
 			   struct pci_dev *pdev)
 {
@@ -3473,8 +3029,6 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 		goto err_out;
 	}
 
-	/* Let's set up the PORT LOGIC Register. */
-
 	/* Program the Ack/Nak latency and replay timers */
 	max_payload = pdev->pcie_mpss;
 
@@ -3483,15 +3037,15 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 		static const u16 replay[2] = { 0x1E0, 0x2ED };
 
 		if (pci_write_config_word(pdev, ET1310_PCI_ACK_NACK,
-					       acknak[max_payload])) {
+					  acknak[max_payload])) {
 			dev_err(&pdev->dev,
-			  "Could not write PCI config space for ACK/NAK\n");
+				"Could not write PCI config space for ACK/NAK\n");
 			goto err_out;
 		}
 		if (pci_write_config_word(pdev, ET1310_PCI_REPLAY,
-					       replay[max_payload])) {
+					  replay[max_payload])) {
 			dev_err(&pdev->dev,
-			  "Could not write PCI config space for Replay Timer\n");
+				"Could not write PCI config space for Replay Timer\n");
 			goto err_out;
 		}
 	}
@@ -3501,7 +3055,7 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 	 */
 	if (pci_write_config_byte(pdev, ET1310_PCI_L0L1LATENCY, 0x11)) {
 		dev_err(&pdev->dev,
-		  "Could not write PCI config space for Latency Timers\n");
+			"Could not write PCI config space for Latency Timers\n");
 		goto err_out;
 	}
 
@@ -3522,7 +3076,7 @@ static int et131x_pci_init(struct et131x_adapter *adapter,
 
 	for (i = 0; i < ETH_ALEN; i++) {
 		if (pci_read_config_byte(pdev, ET1310_PCI_MAC_ADDRESS + i,
-					adapter->rom_addr + i)) {
+					 adapter->rom_addr + i)) {
 			dev_err(&pdev->dev, "Could not read PCI config space for MAC address\n");
 			goto err_out;
 		}
@@ -3543,7 +3097,7 @@ err_out:
  */
 static void et131x_error_timer_handler(unsigned long data)
 {
-	struct et131x_adapter *adapter = (struct et131x_adapter *) data;
+	struct et131x_adapter *adapter = (struct et131x_adapter *)data;
 	struct phy_device *phydev = adapter->phydev;
 
 	if (et1310_in_phy_coma(adapter)) {
@@ -3576,38 +3130,32 @@ static void et131x_error_timer_handler(unsigned long data)
 	mod_timer(&adapter->error_timer, jiffies + TX_ERROR_PERIOD * HZ / 1000);
 }
 
-/* et131x_adapter_memory_free - Free all memory allocated for use by Tx & Rx */
 static void et131x_adapter_memory_free(struct et131x_adapter *adapter)
 {
 	et131x_tx_dma_memory_free(adapter);
 	et131x_rx_dma_memory_free(adapter);
 }
 
-/* et131x_adapter_memory_alloc
- * Allocate all the memory blocks for send, receive and others.
- */
 static int et131x_adapter_memory_alloc(struct et131x_adapter *adapter)
 {
 	int status;
 
-	/* Allocate memory for the Tx Ring */
 	status = et131x_tx_dma_memory_alloc(adapter);
 	if (status) {
 		dev_err(&adapter->pdev->dev,
-			  "et131x_tx_dma_memory_alloc FAILED\n");
+			"et131x_tx_dma_memory_alloc FAILED\n");
 		et131x_tx_dma_memory_free(adapter);
 		return status;
 	}
-	/* Receive buffer memory allocation */
+
 	status = et131x_rx_dma_memory_alloc(adapter);
 	if (status) {
 		dev_err(&adapter->pdev->dev,
-			  "et131x_rx_dma_memory_alloc FAILED\n");
+			"et131x_rx_dma_memory_alloc FAILED\n");
 		et131x_adapter_memory_free(adapter);
 		return status;
 	}
 
-	/* Init receive data structures */
 	status = et131x_init_recv(adapter);
 	if (status) {
 		dev_err(&adapter->pdev->dev, "et131x_init_recv FAILED\n");
@@ -3642,15 +3190,16 @@ static void et131x_adjust_link(struct net_device *netdev)
 			u16 register18;
 
 			et131x_mii_read(adapter, PHY_MPHY_CONTROL_REG,
-					 &register18);
-			et131x_mii_write(adapter, PHY_MPHY_CONTROL_REG,
+					&register18);
+			et131x_mii_write(adapter, phydev->addr,
+					 PHY_MPHY_CONTROL_REG,
 					 register18 | 0x4);
-			et131x_mii_write(adapter, PHY_INDEX_REG,
+			et131x_mii_write(adapter, phydev->addr, PHY_INDEX_REG,
 					 register18 | 0x8402);
-			et131x_mii_write(adapter, PHY_DATA_REG,
+			et131x_mii_write(adapter, phydev->addr, PHY_DATA_REG,
 					 register18 | 511);
-			et131x_mii_write(adapter, PHY_MPHY_CONTROL_REG,
-					 register18);
+			et131x_mii_write(adapter, phydev->addr,
+					 PHY_MPHY_CONTROL_REG, register18);
 		}
 
 		et1310_config_flow_control(adapter);
@@ -3662,7 +3211,8 @@ static void et131x_adjust_link(struct net_device *netdev)
 			et131x_mii_read(adapter, PHY_CONFIG, &reg);
 			reg &= ~ET_PHY_CONFIG_TX_FIFO_DEPTH;
 			reg |= ET_PHY_CONFIG_FIFO_DEPTH_32;
-			et131x_mii_write(adapter, PHY_CONFIG, reg);
+			et131x_mii_write(adapter, phydev->addr, PHY_CONFIG,
+					 reg);
 		}
 
 		et131x_set_rx_dma_timer(adapter);
@@ -3674,21 +3224,19 @@ static void et131x_adjust_link(struct net_device *netdev)
 			u16 register18;
 
 			et131x_mii_read(adapter, PHY_MPHY_CONTROL_REG,
-					 &register18);
-			et131x_mii_write(adapter, PHY_MPHY_CONTROL_REG,
+					&register18);
+			et131x_mii_write(adapter, phydev->addr,
+					 PHY_MPHY_CONTROL_REG,
 					 register18 | 0x4);
-			et131x_mii_write(adapter, PHY_INDEX_REG,
-					 register18 | 0x8402);
-			et131x_mii_write(adapter, PHY_DATA_REG,
-					 register18 | 511);
-			et131x_mii_write(adapter, PHY_MPHY_CONTROL_REG,
-					 register18);
+			et131x_mii_write(adapter, phydev->addr,
+					 PHY_INDEX_REG, register18 | 0x8402);
+			et131x_mii_write(adapter, phydev->addr,
+					 PHY_DATA_REG, register18 | 511);
+			et131x_mii_write(adapter, phydev->addr,
+					 PHY_MPHY_CONTROL_REG, register18);
 		}
 
-		/* Free the packets being actively sent & stopped */
 		et131x_free_busy_send_packets(adapter);
-
-		/* Re-initialize the send structures */
 		et131x_init_send(adapter);
 
 		/* Bring the device back to the state it was during
@@ -3698,10 +3246,8 @@ static void et131x_adjust_link(struct net_device *netdev)
 		 */
 		et131x_soft_reset(adapter);
 
-		/* Setup ET1310 as per the documentation */
 		et131x_adapter_setup(adapter);
 
-		/* perform reset of tx/rx */
 		et131x_disable_txrx(netdev);
 		et131x_enable_txrx(netdev);
 	}
@@ -3726,18 +3272,20 @@ static int et131x_mii_probe(struct net_device *netdev)
 		return PTR_ERR(phydev);
 	}
 
-	phydev->supported &= (SUPPORTED_10baseT_Half
-				| SUPPORTED_10baseT_Full
-				| SUPPORTED_100baseT_Half
-				| SUPPORTED_100baseT_Full
-				| SUPPORTED_Autoneg
-				| SUPPORTED_MII
-				| SUPPORTED_TP);
+	phydev->supported &= (SUPPORTED_10baseT_Half |
+			      SUPPORTED_10baseT_Full |
+			      SUPPORTED_100baseT_Half |
+			      SUPPORTED_100baseT_Full |
+			      SUPPORTED_Autoneg |
+			      SUPPORTED_MII |
+			      SUPPORTED_TP);
 
 	if (adapter->pdev->device != ET131X_PCI_DEVICE_ID_FAST)
-		phydev->supported |= SUPPORTED_1000baseT_Full;
+		phydev->supported |= SUPPORTED_1000baseT_Half |
+				     SUPPORTED_1000baseT_Full;
 
 	phydev->advertising = phydev->supported;
+	phydev->autoneg = AUTONEG_ENABLE;
 	adapter->phydev = phydev;
 
 	dev_info(&adapter->pdev->dev,
@@ -3747,11 +3295,6 @@ static int et131x_mii_probe(struct net_device *netdev)
 	return 0;
 }
 
-/* et131x_adapter_init
- *
- * Initialize the data structures for the et131x_adapter object and link
- * them together with the platform provided device structures.
- */
 static struct et131x_adapter *et131x_adapter_init(struct net_device *netdev,
 						  struct pci_dev *pdev)
 {
@@ -3759,41 +3302,30 @@ static struct et131x_adapter *et131x_adapter_init(struct net_device *netdev,
 
 	struct et131x_adapter *adapter;
 
-	/* Allocate private adapter struct and copy in relevant information */
 	adapter = netdev_priv(netdev);
 	adapter->pdev = pci_dev_get(pdev);
 	adapter->netdev = netdev;
 
-	/* Initialize spinlocks here */
 	spin_lock_init(&adapter->tcb_send_qlock);
 	spin_lock_init(&adapter->tcb_ready_qlock);
-	spin_lock_init(&adapter->send_hw_lock);
 	spin_lock_init(&adapter->rcv_lock);
-	spin_lock_init(&adapter->fbr_lock);
 
 	adapter->registry_jumbo_packet = 1514;	/* 1514-9216 */
 
-	/* Set the MAC address to a default */
 	ether_addr_copy(adapter->addr, default_mac);
 
 	return adapter;
 }
 
-/* et131x_pci_remove
- *
- * Registered in the pci_driver structure, this function is called when the
- * PCI subsystem detects that a PCI device which matches the information
- * contained in the pci_device_id table has been removed.
- */
 static void et131x_pci_remove(struct pci_dev *pdev)
 {
 	struct net_device *netdev = pci_get_drvdata(pdev);
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
 	unregister_netdev(netdev);
+	netif_napi_del(&adapter->napi);
 	phy_disconnect(adapter->phydev);
 	mdiobus_unregister(adapter->mii_bus);
-	cancel_work_sync(&adapter->task);
 	kfree(adapter->mii_bus->irq);
 	mdiobus_free(adapter->mii_bus);
 
@@ -3806,7 +3338,6 @@ static void et131x_pci_remove(struct pci_dev *pdev)
 	pci_disable_device(pdev);
 }
 
-/* et131x_up - Bring up a device for use.  */
 static void et131x_up(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -3815,7 +3346,6 @@ static void et131x_up(struct net_device *netdev)
 	phy_start(adapter->phydev);
 }
 
-/* et131x_down - Bring down the device */
 static void et131x_down(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -3855,51 +3385,35 @@ static int et131x_resume(struct device *dev)
 
 	return 0;
 }
+#endif
 
 static SIMPLE_DEV_PM_OPS(et131x_pm_ops, et131x_suspend, et131x_resume);
-#define ET131X_PM_OPS (&et131x_pm_ops)
-#else
-#define ET131X_PM_OPS NULL
-#endif
 
-/* et131x_isr - The Interrupt Service Routine for the driver.
- * @irq: the IRQ on which the interrupt was received.
- * @dev_id: device-specific info (here a pointer to a net_device struct)
- *
- * Returns a value indicating if the interrupt was handled.
- */
 static irqreturn_t et131x_isr(int irq, void *dev_id)
 {
 	bool handled = true;
-	struct net_device *netdev = (struct net_device *)dev_id;
+	bool enable_interrupts = true;
+	struct net_device *netdev = dev_id;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
+	struct address_map __iomem *iomem = adapter->regs;
 	struct rx_ring *rx_ring = &adapter->rx_ring;
 	struct tx_ring *tx_ring = &adapter->tx_ring;
 	u32 status;
 
 	if (!netif_device_present(netdev)) {
 		handled = false;
+		enable_interrupts = false;
 		goto out;
 	}
 
-	/* If the adapter is in low power state, then it should not
-	 * recognize any interrupt
-	 */
-
-	/* Disable Device Interrupts */
 	et131x_disable_interrupts(adapter);
 
-	/* Get a copy of the value in the interrupt status register
-	 * so we can process the interrupting section
-	 */
 	status = readl(&adapter->regs->global.int_status);
 
-	if (adapter->flowcontrol == FLOW_TXONLY ||
-	    adapter->flowcontrol == FLOW_BOTH) {
+	if (adapter->flow == FLOW_TXONLY || adapter->flow == FLOW_BOTH)
 		status &= ~INT_MASK_ENABLE;
-	} else {
+	else
 		status &= ~INT_MASK_ENABLE_NO_FLOW;
-	}
 
 	/* Make sure this is our interrupt */
 	if (!status) {
@@ -3909,7 +3423,6 @@ static irqreturn_t et131x_isr(int irq, void *dev_id)
 	}
 
 	/* This is our interrupt, so process accordingly */
-
 	if (status & ET_INTR_WATCHDOG) {
 		struct tcb *tcb = tx_ring->send_head;
 
@@ -3925,71 +3438,25 @@ static irqreturn_t et131x_isr(int irq, void *dev_id)
 		status &= ~ET_INTR_WATCHDOG;
 	}
 
-	if (!status) {
-		/* This interrupt has in some way been "handled" by
-		 * the ISR. Either it was a spurious Rx interrupt, or
-		 * it was a Tx interrupt that has been filtered by
-		 * the ISR.
-		 */
-		et131x_enable_interrupts(adapter);
-		goto out;
+	if (status & (ET_INTR_RXDMA_XFR_DONE | ET_INTR_TXDMA_ISR)) {
+		enable_interrupts = false;
+		napi_schedule(&adapter->napi);
 	}
 
-	/* We need to save the interrupt status value for use in our
-	 * DPC. We will clear the software copy of that in that
-	 * routine.
-	 */
-	adapter->stats.interrupt_status = status;
-
-	/* Schedule the ISR handler as a bottom-half task in the
-	 * kernel's tq_immediate queue, and mark the queue for
-	 * execution
-	 */
-	schedule_work(&adapter->task);
-out:
-	return IRQ_RETVAL(handled);
-}
-
-/* et131x_isr_handler - The ISR handler
- *
- * scheduled to run in a deferred context by the ISR. This is where the ISR's
- * work actually gets done.
- */
-static void et131x_isr_handler(struct work_struct *work)
-{
-	struct et131x_adapter *adapter =
-		container_of(work, struct et131x_adapter, task);
-	u32 status = adapter->stats.interrupt_status;
-	struct address_map __iomem *iomem = adapter->regs;
-
-	/* These first two are by far the most common.  Once handled, we clear
-	 * their two bits in the status word.  If the word is now zero, we
-	 * exit.
-	 */
-	/* Handle all the completed Transmit interrupts */
-	if (status & ET_INTR_TXDMA_ISR)
-		et131x_handle_send_interrupt(adapter);
-
-	/* Handle all the completed Receives interrupts */
-	if (status & ET_INTR_RXDMA_XFR_DONE)
-		et131x_handle_recv_interrupt(adapter);
-
-	status &= ~(ET_INTR_TXDMA_ERR | ET_INTR_RXDMA_XFR_DONE);
+	status &= ~(ET_INTR_TXDMA_ISR | ET_INTR_RXDMA_XFR_DONE);
 
 	if (!status)
 		goto out;
 
-	/* Handle the TXDMA Error interrupt */
 	if (status & ET_INTR_TXDMA_ERR) {
 		/* Following read also clears the register (COR) */
 		u32 txdma_err = readl(&iomem->txdma.tx_dma_error);
 
 		dev_warn(&adapter->pdev->dev,
-			    "TXDMA_ERR interrupt, error = %d\n",
-			    txdma_err);
+			 "TXDMA_ERR interrupt, error = %d\n",
+			 txdma_err);
 	}
 
-	/* Handle Free Buffer Ring 0 and 1 Low interrupt */
 	if (status & (ET_INTR_RXDMA_FB_R0_LOW | ET_INTR_RXDMA_FB_R1_LOW)) {
 		/* This indicates the number of unused buffers in RXDMA free
 		 * buffer ring 0 is <= the limit you programmed. Free buffer
@@ -4006,8 +3473,7 @@ static void et131x_isr_handler(struct work_struct *work)
 		/*  If the user has flow control on, then we will
 		 * send a pause packet, otherwise just exit
 		 */
-		if (adapter->flowcontrol == FLOW_TXONLY ||
-		    adapter->flowcontrol == FLOW_BOTH) {
+		if (adapter->flow == FLOW_TXONLY || adapter->flow == FLOW_BOTH) {
 			u32 pm_csr;
 
 			/* Tell the device to send a pause packet via the back
@@ -4030,7 +3496,6 @@ static void et131x_isr_handler(struct work_struct *work)
 		 */
 	}
 
-	/* Handle RXDMA Error Interrupt */
 	if (status & ET_INTR_RXDMA_ERR) {
 		/* The rxdma_error interrupt is sent when a time-out on a
 		 * request issued by the JAGCore has occurred or a completion is
@@ -4048,22 +3513,19 @@ static void et131x_isr_handler(struct work_struct *work)
 		 */
 		/* TRAP();*/
 
-		dev_warn(&adapter->pdev->dev,
-			    "RxDMA_ERR interrupt, error %x\n",
-			    readl(&iomem->txmac.tx_test));
+		dev_warn(&adapter->pdev->dev, "RxDMA_ERR interrupt, error %x\n",
+			 readl(&iomem->txmac.tx_test));
 	}
 
 	/* Handle the Wake on LAN Event */
 	if (status & ET_INTR_WOL) {
 		/* This is a secondary interrupt for wake on LAN. The driver
 		 * should never see this, if it does, something serious is
-		 * wrong. We will TRAP the message when we are in DBG mode,
-		 * otherwise we will ignore it.
+		 * wrong.
 		 */
 		dev_err(&adapter->pdev->dev, "WAKE_ON_LAN interrupt\n");
 	}
 
-	/* Let's move on to the TxMac */
 	if (status & ET_INTR_TXMAC) {
 		u32 err = readl(&iomem->txmac.err);
 
@@ -4075,8 +3537,7 @@ static void et131x_isr_handler(struct work_struct *work)
 		 * occur. In a nutshell, the whole Tx path will have to be reset
 		 * and re-configured afterwards.
 		 */
-		dev_warn(&adapter->pdev->dev,
-			 "TXMAC interrupt, error 0x%08x\n",
+		dev_warn(&adapter->pdev->dev, "TXMAC interrupt, error 0x%08x\n",
 			 err);
 
 		/* If we are debugging, we want to see this error, otherwise we
@@ -4084,14 +3545,11 @@ static void et131x_isr_handler(struct work_struct *work)
 		 */
 	}
 
-	/* Handle RXMAC Interrupt */
 	if (status & ET_INTR_RXMAC) {
 		/* These interrupts are catastrophic to the device, what we need
 		 * to do is disable the interrupts and set the flag to cause us
 		 * to reset so we can solve this issue.
 		 */
-		/* MP_SET_FLAG( adapter, FMP_ADAPTER_HARDWARE_ERROR); */
-
 		dev_warn(&adapter->pdev->dev,
 			 "RXMAC interrupt, error 0x%08x.  Requesting reset\n",
 			 readl(&iomem->rxmac.err_reg));
@@ -4106,7 +3564,6 @@ static void et131x_isr_handler(struct work_struct *work)
 		 */
 	}
 
-	/* Handle MAC_STAT Interrupt */
 	if (status & ET_INTR_MAC_STAT) {
 		/* This means at least one of the un-masked counters in the
 		 * MAC_STAT block has rolled over. Use this to maintain the top,
@@ -4115,7 +3572,6 @@ static void et131x_isr_handler(struct work_struct *work)
 		et1310_handle_macstat_interrupt(adapter);
 	}
 
-	/* Handle SLV Timeout Interrupt */
 	if (status & ET_INTR_SLV_TIMEOUT) {
 		/* This means a timeout has occurred on a read or write request
 		 * to one of the JAGCore registers. The Global Resources block
@@ -4124,8 +3580,28 @@ static void et131x_isr_handler(struct work_struct *work)
 		 * addressed module is in a power-down state and can't respond.
 		 */
 	}
+
 out:
-	et131x_enable_interrupts(adapter);
+	if (enable_interrupts)
+		et131x_enable_interrupts(adapter);
+
+	return IRQ_RETVAL(handled);
+}
+
+static int et131x_poll(struct napi_struct *napi, int budget)
+{
+	struct et131x_adapter *adapter =
+		container_of(napi, struct et131x_adapter, napi);
+	int work_done = et131x_handle_recv_pkts(adapter, budget);
+
+	et131x_handle_send_pkts(adapter);
+
+	if (work_done < budget) {
+		napi_complete(&adapter->napi);
+		et131x_enable_interrupts(adapter);
+	}
+
+	return work_done;
 }
 
 /* et131x_stats - Return the current device statistics  */
@@ -4147,17 +3623,9 @@ static struct net_device_stats *et131x_stats(struct net_device *netdev)
 	stats->rx_length_errors = devstat->rx_length_errs;
 	stats->rx_over_errors = devstat->rx_overflows;
 	stats->rx_crc_errors = devstat->rx_crc_errs;
+	stats->rx_dropped = devstat->rcvd_pkts_dropped;
 
-	/* NOTE: These stats don't have corresponding values in CE_STATS,
-	 * so we're going to have to update these directly from within the
-	 * TX/RX code
-	 */
-	/* stats->rx_bytes            = 20; devstat->; */
-	/* stats->tx_bytes            = 20;  devstat->; */
-	/* stats->rx_dropped          = devstat->; */
-	/* stats->tx_dropped          = devstat->; */
-
-	/*  NOTE: Not used, can't find analogous statistics */
+	/* NOTE: Not used, can't find analogous statistics */
 	/* stats->rx_frame_errors     = devstat->; */
 	/* stats->rx_fifo_errors      = devstat->; */
 	/* stats->rx_missed_errors    = devstat->; */
@@ -4170,7 +3638,6 @@ static struct net_device_stats *et131x_stats(struct net_device *netdev)
 	return stats;
 }
 
-/* et131x_open - Open the device for use.  */
 static int et131x_open(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -4194,17 +3661,19 @@ static int et131x_open(struct net_device *netdev)
 
 	adapter->flags |= FMP_ADAPTER_INTERRUPT_IN_USE;
 
+	napi_enable(&adapter->napi);
+
 	et131x_up(netdev);
 
 	return result;
 }
 
-/* et131x_close - Close the device */
 static int et131x_close(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
 	et131x_down(netdev);
+	napi_disable(&adapter->napi);
 
 	adapter->flags &= ~FMP_ADAPTER_INTERRUPT_IN_USE;
 	free_irq(adapter->pdev->irq, netdev);
@@ -4213,11 +3682,6 @@ static int et131x_close(struct net_device *netdev)
 	return del_timer_sync(&adapter->error_timer);
 }
 
-/* et131x_ioctl - The I/O Control handler for the driver
- * @netdev: device on which the control request is being made
- * @reqbuf: a pointer to the IOCTL request buffer
- * @cmd: the IOCTL command code
- */
 static int et131x_ioctl(struct net_device *netdev, struct ifreq *reqbuf,
 			int cmd)
 {
@@ -4229,11 +3693,7 @@ static int et131x_ioctl(struct net_device *netdev, struct ifreq *reqbuf,
 	return phy_mii_ioctl(adapter->phydev, reqbuf, cmd);
 }
 
-/* et131x_set_packet_filter - Configures the Rx Packet filtering on the device
- * @adapter: pointer to our private adapter structure
- *
- * FIXME: lot of dups with MAC code
- */
+/* et131x_set_packet_filter - Configures the Rx Packet filtering */
 static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 {
 	int filter = adapter->packet_filter;
@@ -4243,9 +3703,7 @@ static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 	ctrl = readl(&adapter->regs->rxmac.ctrl);
 	pf_ctrl = readl(&adapter->regs->rxmac.pf_ctrl);
 
-	/* Default to disabled packet filtering.  Enable it in the individual
-	 * case statements that require the device to filter something
-	 */
+	/* Default to disabled packet filtering */
 	ctrl |= 0x04;
 
 	/* Set us to be in promiscuous mode so we receive everything, this
@@ -4277,8 +3735,9 @@ static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 		if (filter & ET131X_PACKET_TYPE_BROADCAST) {
 			pf_ctrl |= 1;	/* Broadcast filter bit */
 			ctrl &= ~0x04;
-		} else
+		} else {
 			pf_ctrl &= ~1;
+		}
 
 		/* Setup the receive mac configuration registers - Packet
 		 * Filter control + the enable / disable for packet filter
@@ -4290,7 +3749,6 @@ static int et131x_set_packet_filter(struct et131x_adapter *adapter)
 	return 0;
 }
 
-/* et131x_multicast - The handler to configure multicasting on the interface */
 static void et131x_multicast(struct net_device *netdev)
 {
 	struct et131x_adapter *adapter = netdev_priv(netdev);
@@ -4314,30 +3772,28 @@ static void et131x_multicast(struct net_device *netdev)
 	/* Check the net_device flags and set the device independent flags
 	 * accordingly
 	 */
-
 	if (netdev->flags & IFF_PROMISC)
 		adapter->packet_filter |= ET131X_PACKET_TYPE_PROMISCUOUS;
 	else
 		adapter->packet_filter &= ~ET131X_PACKET_TYPE_PROMISCUOUS;
 
-	if (netdev->flags & IFF_ALLMULTI)
-		adapter->packet_filter |= ET131X_PACKET_TYPE_ALL_MULTICAST;
-
-	if (netdev_mc_count(netdev) > NIC_MAX_MCAST_LIST)
+	if ((netdev->flags & IFF_ALLMULTI) ||
+	    (netdev_mc_count(netdev) > NIC_MAX_MCAST_LIST))
 		adapter->packet_filter |= ET131X_PACKET_TYPE_ALL_MULTICAST;
 
 	if (netdev_mc_count(netdev) < 1) {
 		adapter->packet_filter &= ~ET131X_PACKET_TYPE_ALL_MULTICAST;
 		adapter->packet_filter &= ~ET131X_PACKET_TYPE_MULTICAST;
-	} else
+	} else {
 		adapter->packet_filter |= ET131X_PACKET_TYPE_MULTICAST;
+	}
 
 	/* Set values in the private adapter struct */
 	i = 0;
 	netdev_for_each_mc_addr(ha, netdev) {
 		if (i == NIC_MAX_MCAST_LIST)
 			break;
-		memcpy(adapter->multicast_list[i++], ha->addr, ETH_ALEN);
+		ether_addr_copy(adapter->multicast_list[i++], ha->addr);
 	}
 	adapter->multicast_addr_count = i;
 
@@ -4351,10 +3807,8 @@ static void et131x_multicast(struct net_device *netdev)
 		et131x_set_packet_filter(adapter);
 }
 
-/* et131x_tx - The handler to tx a packet on the device */
-static int et131x_tx(struct sk_buff *skb, struct net_device *netdev)
+static netdev_tx_t et131x_tx(struct sk_buff *skb, struct net_device *netdev)
 {
-	int status = 0;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 	struct tx_ring *tx_ring = &adapter->tx_ring;
 
@@ -4365,17 +3819,23 @@ static int et131x_tx(struct sk_buff *skb, struct net_device *netdev)
 	/* Save the timestamp for the TX timeout watchdog */
 	netdev->trans_start = jiffies;
 
-	/* Call the device-specific data Tx routine */
-	status = et131x_send_packets(skb, netdev);
+	/* TCB is not available */
+	if (tx_ring->used >= NUM_TCB)
+		goto drop_err;
 
-	/* Check status and manage the netif queue if necessary */
-	if (status != 0) {
-		if (status == -ENOMEM)
-			status = NETDEV_TX_BUSY;
-		else
-			status = NETDEV_TX_OK;
-	}
-	return status;
+	if ((adapter->flags & FMP_ADAPTER_FAIL_SEND_MASK) ||
+	    !netif_carrier_ok(netdev))
+		goto drop_err;
+
+	if (send_packet(skb, adapter))
+		goto drop_err;
+
+	return NETDEV_TX_OK;
+
+drop_err:
+	dev_kfree_skb_any(skb);
+	adapter->netdev->stats.tx_dropped++;
+	return NETDEV_TX_OK;
 }
 
 /* et131x_tx_timeout - Timeout handler
@@ -4409,124 +3869,57 @@ static void et131x_tx_timeout(struct net_device *netdev)
 
 	/* Is send stuck? */
 	spin_lock_irqsave(&adapter->tcb_send_qlock, flags);
-
 	tcb = tx_ring->send_head;
+	spin_unlock_irqrestore(&adapter->tcb_send_qlock, flags);
 
-	if (tcb != NULL) {
+	if (tcb) {
 		tcb->count++;
 
 		if (tcb->count > NIC_SEND_HANG_THRESHOLD) {
-			spin_unlock_irqrestore(&adapter->tcb_send_qlock,
-					       flags);
-
 			dev_warn(&adapter->pdev->dev,
-				"Send stuck - reset.  tcb->WrIndex %x, flags 0x%08x\n",
-				tcb->index,
-				tcb->flags);
+				 "Send stuck - reset. tcb->WrIndex %x\n",
+				 tcb->index);
 
 			adapter->netdev->stats.tx_errors++;
 
 			/* perform reset of tx/rx */
 			et131x_disable_txrx(netdev);
 			et131x_enable_txrx(netdev);
-			return;
 		}
 	}
-
-	spin_unlock_irqrestore(&adapter->tcb_send_qlock, flags);
 }
 
-/* et131x_change_mtu - The handler called to change the MTU for the device */
 static int et131x_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	int result = 0;
 	struct et131x_adapter *adapter = netdev_priv(netdev);
 
-	/* Make sure the requested MTU is valid */
 	if (new_mtu < 64 || new_mtu > 9216)
 		return -EINVAL;
 
 	et131x_disable_txrx(netdev);
-	et131x_handle_send_interrupt(adapter);
-	et131x_handle_recv_interrupt(adapter);
 
-	/* Set the new MTU */
 	netdev->mtu = new_mtu;
 
-	/* Free Rx DMA memory */
 	et131x_adapter_memory_free(adapter);
 
 	/* Set the config parameter for Jumbo Packet support */
 	adapter->registry_jumbo_packet = new_mtu + 14;
 	et131x_soft_reset(adapter);
 
-	/* Alloc and init Rx DMA memory */
 	result = et131x_adapter_memory_alloc(adapter);
 	if (result != 0) {
 		dev_warn(&adapter->pdev->dev,
-			"Change MTU failed; couldn't re-alloc DMA memory\n");
-		return result;
-	}
-
-	et131x_init_send(adapter);
-
-	et131x_hwaddr_init(adapter);
-	memcpy(netdev->dev_addr, adapter->addr, ETH_ALEN);
-
-	/* Init the device with the new settings */
-	et131x_adapter_setup(adapter);
-
-	et131x_enable_txrx(netdev);
-
-	return result;
-}
-
-/* et131x_set_mac_addr - handler to change the MAC address for the device */
-static int et131x_set_mac_addr(struct net_device *netdev, void *new_mac)
-{
-	int result = 0;
-	struct et131x_adapter *adapter = netdev_priv(netdev);
-	struct sockaddr *address = new_mac;
-
-	if (adapter == NULL)
-		return -ENODEV;
-
-	/* Make sure the requested MAC is valid */
-	if (!is_valid_ether_addr(address->sa_data))
-		return -EADDRNOTAVAIL;
-
-	et131x_disable_txrx(netdev);
-	et131x_handle_send_interrupt(adapter);
-	et131x_handle_recv_interrupt(adapter);
-
-	/* Set the new MAC */
-	/* netdev->set_mac_address  = &new_mac; */
-
-	memcpy(netdev->dev_addr, address->sa_data, netdev->addr_len);
-
-	netdev_info(netdev, "Setting MAC address to %pM\n",
-		    netdev->dev_addr);
-
-	/* Free Rx DMA memory */
-	et131x_adapter_memory_free(adapter);
-
-	et131x_soft_reset(adapter);
-
-	/* Alloc and init Rx DMA memory */
-	result = et131x_adapter_memory_alloc(adapter);
-	if (result != 0) {
-		dev_err(&adapter->pdev->dev,
-			"Change MAC failed; couldn't re-alloc DMA memory\n");
+			 "Change MTU failed; couldn't re-alloc DMA memory\n");
 		return result;
 	}
 
 	et131x_init_send(adapter);
-
 	et131x_hwaddr_init(adapter);
+	ether_addr_copy(netdev->dev_addr, adapter->addr);
 
 	/* Init the device with the new settings */
 	et131x_adapter_setup(adapter);
-
 	et131x_enable_txrx(netdev);
 
 	return result;
@@ -4539,21 +3932,12 @@ static const struct net_device_ops et131x_netdev_ops = {
 	.ndo_set_rx_mode	= et131x_multicast,
 	.ndo_tx_timeout		= et131x_tx_timeout,
 	.ndo_change_mtu		= et131x_change_mtu,
-	.ndo_set_mac_address	= et131x_set_mac_addr,
+	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_get_stats		= et131x_stats,
 	.ndo_do_ioctl		= et131x_ioctl,
 };
 
-/* et131x_pci_setup - Perform device initialization
- * @pdev: a pointer to the device's pci_dev structure
- * @ent: this device's entry in the pci_device_id table
- *
- * Registered in the pci_driver structure, this function is called when the
- * PCI subsystem finds a new PCI device which matches the information
- * contained in the pci_device_id table. This routine is the equivalent to
- * a device insertion routine.
- */
 static int et131x_pci_setup(struct pci_dev *pdev,
 			    const struct pci_device_id *ent)
 {
@@ -4591,7 +3975,6 @@ static int et131x_pci_setup(struct pci_dev *pdev,
 		goto err_release_res;
 	}
 
-	/* Allocate netdev and private adapter structs */
 	netdev = alloc_etherdev(sizeof(struct et131x_adapter));
 	if (!netdev) {
 		dev_err(&pdev->dev, "Couldn't alloc netdev struct\n");
@@ -4622,35 +4005,23 @@ static int et131x_pci_setup(struct pci_dev *pdev,
 	/* If Phy COMA mode was enabled when we went down, disable it here. */
 	writel(ET_PMCSR_INIT,  &adapter->regs->global.pm_csr);
 
-	/* Issue a global reset to the et1310 */
 	et131x_soft_reset(adapter);
-
-	/* Disable all interrupts (paranoid) */
 	et131x_disable_interrupts(adapter);
 
-	/* Allocate DMA memory */
 	rc = et131x_adapter_memory_alloc(adapter);
 	if (rc < 0) {
 		dev_err(&pdev->dev, "Could not alloc adapter memory (DMA)\n");
 		goto err_iounmap;
 	}
 
-	/* Init send data structures */
 	et131x_init_send(adapter);
 
-	/* Set up the task structure for the ISR's deferred handler */
-	INIT_WORK(&adapter->task, et131x_isr_handler);
-
-	/* Copy address into the net_device struct */
-	memcpy(netdev->dev_addr, adapter->addr, ETH_ALEN);
+	netif_napi_add(netdev, &adapter->napi, et131x_poll, 64);
 
-	/* Init variable for counting how long we do not have link status */
-	adapter->boot_coma = 0;
-	et1310_disable_phy_coma(adapter);
+	ether_addr_copy(netdev->dev_addr, adapter->addr);
 
 	rc = -ENOMEM;
 
-	/* Setup the mii_bus struct */
 	adapter->mii_bus = mdiobus_alloc();
 	if (!adapter->mii_bus) {
 		dev_err(&pdev->dev, "Alloc of mii_bus struct failed\n");
@@ -4659,11 +4030,10 @@ static int et131x_pci_setup(struct pci_dev *pdev,
 
 	adapter->mii_bus->name = "et131x_eth_mii";
 	snprintf(adapter->mii_bus->id, MII_BUS_ID_SIZE, "%x",
-		(adapter->pdev->bus->number << 8) | adapter->pdev->devfn);
+		 (adapter->pdev->bus->number << 8) | adapter->pdev->devfn);
 	adapter->mii_bus->priv = netdev;
 	adapter->mii_bus->read = et131x_mdio_read;
 	adapter->mii_bus->write = et131x_mdio_write;
-	adapter->mii_bus->reset = et131x_mdio_reset;
 	adapter->mii_bus->irq = kmalloc_array(PHY_MAX_ADDR, sizeof(int),
 					      GFP_KERNEL);
 	if (!adapter->mii_bus->irq)
@@ -4684,9 +4054,12 @@ static int et131x_pci_setup(struct pci_dev *pdev,
 		goto err_mdio_unregister;
 	}
 
-	/* Setup et1310 as per the documentation */
 	et131x_adapter_setup(adapter);
 
+	/* Init variable for counting how long we do not have link status */
+	adapter->boot_coma = 0;
+	et1310_disable_phy_coma(adapter);
+
 	/* We can enable interrupts now
 	 *
 	 *  NOTE - Because registration of interrupt handler is done in the
@@ -4694,7 +4067,6 @@ static int et131x_pci_setup(struct pci_dev *pdev,
 	 *         point
 	 */
 
-	/* Register the net_device struct with the Linux network layer */
 	rc = register_netdev(netdev);
 	if (rc < 0) {
 		dev_err(&pdev->dev, "register_netdev() failed\n");
@@ -4734,7 +4106,7 @@ err_disable:
 static const struct pci_device_id et131x_pci_table[] = {
 	{ PCI_VDEVICE(ATT, ET131X_PCI_DEVICE_ID_GIG), 0UL},
 	{ PCI_VDEVICE(ATT, ET131X_PCI_DEVICE_ID_FAST), 0UL},
-	{0,}
+	{ 0,}
 };
 MODULE_DEVICE_TABLE(pci, et131x_pci_table);
 
@@ -4743,7 +4115,7 @@ static struct pci_driver et131x_driver = {
 	.id_table	= et131x_pci_table,
 	.probe		= et131x_pci_setup,
 	.remove		= et131x_pci_remove,
-	.driver.pm	= ET131X_PM_OPS,
+	.driver.pm	= &et131x_pm_ops,
 };
 
 module_pci_driver(et131x_driver);
diff --git a/drivers/staging/et131x/et131x.h b/drivers/net/ethernet/agere/et131x.h
index 1318439db13e..be9a11c02526 100644
--- a/drivers/staging/et131x/et131x.h
+++ b/drivers/net/ethernet/agere/et131x.h
@@ -1,5 +1,4 @@
-/*
- * Copyright © 2005 Agere Systems Inc.
+/* Copyright © 2005 Agere Systems Inc.
  * All rights reserved.
  *   http://www.agere.com
  *
@@ -78,9 +77,7 @@
 #define LBCIF_STATUS_EEPROM_PRESENT     0x80
 
 /* START OF GLOBAL REGISTER ADDRESS MAP */
-
-/*
- * 10bit registers
+/* 10bit registers
  *
  * Tx queue start address reg in global address map at address 0x0000
  * tx queue end address reg in global address map at address 0x0004
@@ -88,8 +85,7 @@
  * rx queue end address reg in global address map at address 0x000C
  */
 
-/*
- * structure for power management control status reg in global address map
+/* structure for power management control status reg in global address map
  * located at address 0x0010
  *	jagcore_rx_rdy	bit 9
  *	jagcore_tx_rdy	bit 8
@@ -102,14 +98,11 @@
  *	jagcore_tx_en	bit 1
  *	gigephy_en	bit 0
  */
-
 #define ET_PM_PHY_SW_COMA		0x40
 #define ET_PMCSR_INIT			0x38
 
-/*
- * Interrupt status reg at address 0x0018
+/* Interrupt status reg at address 0x0018
  */
-
 #define	ET_INTR_TXDMA_ISR	0x00000008
 #define ET_INTR_TXDMA_ERR	0x00000010
 #define ET_INTR_RXDMA_XFR_DONE	0x00000020
@@ -125,16 +118,14 @@
 #define ET_INTR_MAC_STAT	0x00080000
 #define ET_INTR_SLV_TIMEOUT	0x00100000
 
-/*
- * Interrupt mask register at address 0x001C
+/* Interrupt mask register at address 0x001C
  * Interrupt alias clear mask reg at address 0x0020
  * Interrupt status alias reg at address 0x0024
  *
  * Same masks as above
  */
 
-/*
- * Software reset reg at address 0x0028
+/* Software reset reg at address 0x0028
  * 0:	txdma_sw_reset
  * 1:	rxdma_sw_reset
  * 2:	txmac_sw_reset
@@ -144,29 +135,22 @@
  * 6:	mmc_sw_reset
  *31:	selfclr_disable
  */
-
 #define ET_RESET_ALL	0x007F
 
-/*
- * SLV Timer reg at address 0x002C (low 24 bits)
+/* SLV Timer reg at address 0x002C (low 24 bits)
  */
 
-/*
- * MSI Configuration reg at address 0x0030
+/* MSI Configuration reg at address 0x0030
  */
-
 #define ET_MSI_VECTOR	0x0000001F
 #define ET_MSI_TC	0x00070000
 
-/*
- * Loopback reg located at address 0x0034
+/* Loopback reg located at address 0x0034
  */
-
 #define ET_LOOP_MAC	0x00000001
 #define ET_LOOP_DMA	0x00000002
 
-/*
- * GLOBAL Module of JAGCore Address Mapping
+/* GLOBAL Module of JAGCore Address Mapping
  * Located at address 0x0000
  */
 struct global_regs {				/* Location: */
@@ -187,13 +171,9 @@ struct global_regs {				/* Location: */
 	u32 watchdog_timer;			/*  0x0038 */
 };
 
-
 /* START OF TXDMA REGISTER ADDRESS MAP */
-
-/*
- * txdma control status reg at address 0x1000
+/* txdma control status reg at address 0x1000
  */
-
 #define ET_TXDMA_CSR_HALT	0x00000001
 #define ET_TXDMA_DROP_TLP	0x00000002
 #define ET_TXDMA_CACHE_THRS	0x000000F0
@@ -201,26 +181,22 @@ struct global_regs {				/* Location: */
 #define ET_TXDMA_SNGL_EPKT	0x00000100
 #define ET_TXDMA_CLASS		0x00001E00
 
-/*
- * structure for txdma packet ring base address hi reg in txdma address map
+/* structure for txdma packet ring base address hi reg in txdma address map
  * located at address 0x1004
  * Defined earlier (u32)
  */
 
-/*
- * structure for txdma packet ring base address low reg in txdma address map
+/* structure for txdma packet ring base address low reg in txdma address map
  * located at address 0x1008
  * Defined earlier (u32)
  */
 
-/*
- * structure for txdma packet ring number of descriptor reg in txdma address
+/* structure for txdma packet ring number of descriptor reg in txdma address
  * map.  Located at address 0x100C
  *
  * 31-10: unused
  * 9-0: pr ndes
  */
-
 #define ET_DMA12_MASK		0x0FFF	/* 12 bit mask for DMA12W types */
 #define ET_DMA12_WRAP		0x1000
 #define ET_DMA10_MASK		0x03FF	/* 10 bit mask for DMA10W types */
@@ -232,8 +208,7 @@ struct global_regs {				/* Location: */
 #define INDEX10(x)	((x) & ET_DMA10_MASK)
 #define INDEX4(x)	((x) & ET_DMA4_MASK)
 
-/*
- * 10bit DMA with wrap
+/* 10bit DMA with wrap
  * txdma tx queue write address reg in txdma address map at 0x1010
  * txdma tx queue write address external reg in txdma address map at 0x1014
  * txdma tx queue read address reg in txdma address map at 0x1018
@@ -259,8 +234,7 @@ struct global_regs {				/* Location: */
  * 9: WrbkRewind
  */
 
-/*
- * Tx DMA Module of JAGCore Address Mapping
+/* Tx DMA Module of JAGCore Address Mapping
  * Located at address 0x1000
  */
 struct txdma_regs {			/* Location: */
@@ -294,11 +268,8 @@ struct txdma_regs {			/* Location: */
 
 /* END OF TXDMA REGISTER ADDRESS MAP */
 
-
 /* START OF RXDMA REGISTER ADDRESS MAP */
-
-/*
- * structure for control status reg in rxdma address map
+/* structure for control status reg in rxdma address map
  * Located at address 0x2000
  *
  * CSR
@@ -318,7 +289,6 @@ struct txdma_regs {			/* Location: */
  * 17: halt_status
  * 18-31: unused
  */
-
 #define ET_RXDMA_CSR_HALT		0x0001
 #define ET_RXDMA_CSR_FBR0_SIZE_LO	0x0100
 #define ET_RXDMA_CSR_FBR0_SIZE_HI	0x0200
@@ -328,76 +298,64 @@ struct txdma_regs {			/* Location: */
 #define ET_RXDMA_CSR_FBR1_ENABLE	0x2000
 #define ET_RXDMA_CSR_HALT_STATUS	0x00020000
 
-/*
- * structure for dma writeback lo reg in rxdma address map
+/* structure for dma writeback lo reg in rxdma address map
  * located at address 0x2004
  * Defined earlier (u32)
  */
 
-/*
- * structure for dma writeback hi reg in rxdma address map
+/* structure for dma writeback hi reg in rxdma address map
  * located at address 0x2008
  * Defined earlier (u32)
  */
 
-/*
- * structure for number of packets done reg in rxdma address map
+/* structure for number of packets done reg in rxdma address map
  * located at address 0x200C
  *
  * 31-8: unused
  * 7-0: num done
  */
 
-/*
- * structure for max packet time reg in rxdma address map
+/* structure for max packet time reg in rxdma address map
  * located at address 0x2010
  *
  * 31-18: unused
  * 17-0: time done
  */
 
-/*
- * structure for rx queue read address reg in rxdma address map
+/* structure for rx queue read address reg in rxdma address map
  * located at address 0x2014
  * Defined earlier (u32)
  */
 
-/*
- * structure for rx queue read address external reg in rxdma address map
+/* structure for rx queue read address external reg in rxdma address map
  * located at address 0x2018
  * Defined earlier (u32)
  */
 
-/*
- * structure for rx queue write address reg in rxdma address map
+/* structure for rx queue write address reg in rxdma address map
  * located at address 0x201C
  * Defined earlier (u32)
  */
 
-/*
- * structure for packet status ring base address lo reg in rxdma address map
+/* structure for packet status ring base address lo reg in rxdma address map
  * located at address 0x2020
  * Defined earlier (u32)
  */
 
-/*
- * structure for packet status ring base address hi reg in rxdma address map
+/* structure for packet status ring base address hi reg in rxdma address map
  * located at address 0x2024
  * Defined earlier (u32)
  */
 
-/*
- * structure for packet status ring number of descriptors reg in rxdma address
+/* structure for packet status ring number of descriptors reg in rxdma address
  * map.  Located at address 0x2028
  *
  * 31-12: unused
  * 11-0: psr ndes
  */
-
 #define ET_RXDMA_PSR_NUM_DES_MASK	0xFFF
 
-/*
- * structure for packet status ring available offset reg in rxdma address map
+/* structure for packet status ring available offset reg in rxdma address map
  * located at address 0x202C
  *
  * 31-13: unused
@@ -405,8 +363,7 @@ struct txdma_regs {			/* Location: */
  * 11-0: psr avail
  */
 
-/*
- * structure for packet status ring full offset reg in rxdma address map
+/* structure for packet status ring full offset reg in rxdma address map
  * located at address 0x2030
  *
  * 31-13: unused
@@ -414,108 +371,92 @@ struct txdma_regs {			/* Location: */
  * 11-0: psr full
  */
 
-/*
- * structure for packet status ring access index reg in rxdma address map
+/* structure for packet status ring access index reg in rxdma address map
  * located at address 0x2034
  *
  * 31-5: unused
  * 4-0: psr_ai
  */
 
-/*
- * structure for packet status ring minimum descriptors reg in rxdma address
+/* structure for packet status ring minimum descriptors reg in rxdma address
  * map.  Located at address 0x2038
  *
  * 31-12: unused
  * 11-0: psr_min
  */
 
-/*
- * structure for free buffer ring base lo address reg in rxdma address map
+/* structure for free buffer ring base lo address reg in rxdma address map
  * located at address 0x203C
  * Defined earlier (u32)
  */
 
-/*
- * structure for free buffer ring base hi address reg in rxdma address map
+/* structure for free buffer ring base hi address reg in rxdma address map
  * located at address 0x2040
  * Defined earlier (u32)
  */
 
-/*
- * structure for free buffer ring number of descriptors reg in rxdma address
+/* structure for free buffer ring number of descriptors reg in rxdma address
  * map.  Located at address 0x2044
  *
  * 31-10: unused
  * 9-0: fbr ndesc
  */
 
-/*
- * structure for free buffer ring 0 available offset reg in rxdma address map
+/* structure for free buffer ring 0 available offset reg in rxdma address map
  * located at address 0x2048
  * Defined earlier (u32)
  */
 
-/*
- * structure for free buffer ring 0 full offset reg in rxdma address map
+/* structure for free buffer ring 0 full offset reg in rxdma address map
  * located at address 0x204C
  * Defined earlier (u32)
  */
 
-/*
- * structure for free buffer cache 0 full offset reg in rxdma address map
+/* structure for free buffer cache 0 full offset reg in rxdma address map
  * located at address 0x2050
  *
  * 31-5: unused
  * 4-0: fbc rdi
  */
 
-/*
- * structure for free buffer ring 0 minimum descriptor reg in rxdma address map
+/* structure for free buffer ring 0 minimum descriptor reg in rxdma address map
  * located at address 0x2054
  *
  * 31-10: unused
  * 9-0: fbr min
  */
 
-/*
- * structure for free buffer ring 1 base address lo reg in rxdma address map
+/* structure for free buffer ring 1 base address lo reg in rxdma address map
  * located at address 0x2058 - 0x205C
  * Defined earlier (RXDMA_FBR_BASE_LO_t and RXDMA_FBR_BASE_HI_t)
  */
 
-/*
- * structure for free buffer ring 1 number of descriptors reg in rxdma address
+/* structure for free buffer ring 1 number of descriptors reg in rxdma address
  * map.  Located at address 0x2060
  * Defined earlier (RXDMA_FBR_NUM_DES_t)
  */
 
-/*
- * structure for free buffer ring 1 available offset reg in rxdma address map
+/* structure for free buffer ring 1 available offset reg in rxdma address map
  * located at address 0x2064
  * Defined Earlier (RXDMA_FBR_AVAIL_OFFSET_t)
  */
 
-/*
- * structure for free buffer ring 1 full offset reg in rxdma address map
+/* structure for free buffer ring 1 full offset reg in rxdma address map
  * located at address 0x2068
  * Defined Earlier (RXDMA_FBR_FULL_OFFSET_t)
  */
 
-/*
- * structure for free buffer cache 1 read index reg in rxdma address map
+/* structure for free buffer cache 1 read index reg in rxdma address map
  * located at address 0x206C
  * Defined Earlier (RXDMA_FBC_RD_INDEX_t)
  */
 
-/*
- * structure for free buffer ring 1 minimum descriptor reg in rxdma address map
+/* structure for free buffer ring 1 minimum descriptor reg in rxdma address map
  * located at address 0x2070
  * Defined Earlier (RXDMA_FBR_MIN_DES_t)
  */
 
-/*
- * Rx DMA Module of JAGCore Address Mapping
+/* Rx DMA Module of JAGCore Address Mapping
  * Located at address 0x2000
  */
 struct rxdma_regs {					/* Location: */
@@ -552,11 +493,8 @@ struct rxdma_regs {					/* Location: */
 
 /* END OF RXDMA REGISTER ADDRESS MAP */
 
-
 /* START OF TXMAC REGISTER ADDRESS MAP */
-
-/*
- * structure for control reg in txmac address map
+/* structure for control reg in txmac address map
  * located at address 0x3000
  *
  * bits
@@ -570,12 +508,10 @@ struct rxdma_regs {					/* Location: */
  * 1: mif_disable
  * 0: txmac_en
  */
-
 #define ET_TX_CTRL_FC_DISABLE	0x0008
 #define ET_TX_CTRL_TXMAC_ENABLE	0x0001
 
-/*
- * structure for shadow pointer reg in txmac address map
+/* structure for shadow pointer reg in txmac address map
  * located at address 0x3004
  * 31-27: reserved
  * 26-16: txq rd ptr
@@ -583,8 +519,7 @@ struct rxdma_regs {					/* Location: */
  * 10-0: txq wr ptr
  */
 
-/*
- * structure for error count reg in txmac address map
+/* structure for error count reg in txmac address map
  * located at address 0x3008
  *
  * 31-12: unused
@@ -593,22 +528,19 @@ struct rxdma_regs {					/* Location: */
  * 3-0: fifo_underrun
  */
 
-/*
- * structure for max fill reg in txmac address map
+/* structure for max fill reg in txmac address map
  * located at address 0x300C
  * 31-12: unused
  * 11-0: max fill
  */
 
-/*
- * structure for cf parameter reg in txmac address map
+/* structure for cf parameter reg in txmac address map
  * located at address 0x3010
  * 31-16: cfep
  * 15-0: cfpt
  */
 
-/*
- * structure for tx test reg in txmac address map
+/* structure for tx test reg in txmac address map
  * located at address 0x3014
  * 31-17: unused
  * 16: reserved
@@ -617,8 +549,7 @@ struct rxdma_regs {					/* Location: */
  * 10-0: txq test pointer
  */
 
-/*
- * structure for error reg in txmac address map
+/* structure for error reg in txmac address map
  * located at address 0x3018
  *
  * 31-9: unused
@@ -632,8 +563,7 @@ struct rxdma_regs {					/* Location: */
  * 0: seg0_err
  */
 
-/*
- * structure for error interrupt reg in txmac address map
+/* structure for error interrupt reg in txmac address map
  * located at address 0x301C
  *
  * 31-9: unused
@@ -647,8 +577,7 @@ struct rxdma_regs {					/* Location: */
  * 0: seg0_err
  */
 
-/*
- * structure for error interrupt reg in txmac address map
+/* structure for error interrupt reg in txmac address map
  * located at address 0x3020
  *
  * 31-2: unused
@@ -656,8 +585,7 @@ struct rxdma_regs {					/* Location: */
  * 0: bp_xonxoff
  */
 
-/*
- * Tx MAC Module of JAGCore Address Mapping
+/* Tx MAC Module of JAGCore Address Mapping
  */
 struct txmac_regs {			/* Location: */
 	u32 ctl;			/*  0x3000 */
@@ -675,8 +603,7 @@ struct txmac_regs {			/* Location: */
 
 /* START OF RXMAC REGISTER ADDRESS MAP */
 
-/*
- * structure for rxmac control reg in rxmac address map
+/* structure for rxmac control reg in rxmac address map
  * located at address 0x4000
  *
  * 31-7: reserved
@@ -688,12 +615,10 @@ struct txmac_regs {			/* Location: */
  * 1: mcif_disable
  * 0: rxmac_en
  */
-
 #define ET_RX_CTRL_WOL_DISABLE	0x0008
 #define ET_RX_CTRL_RXMAC_ENABLE	0x0001
 
-/*
- * structure for Wake On Lan Control and CRC 0 reg in rxmac address map
+/* structure for Wake On Lan Control and CRC 0 reg in rxmac address map
  * located at address 0x4004
  * 31-16: crc
  * 15-12: reserved
@@ -707,24 +632,21 @@ struct txmac_regs {			/* Location: */
  * 4-0: valid_crc 4-0
  */
 
-/*
- * structure for CRC 1 and CRC 2 reg in rxmac address map
+/* structure for CRC 1 and CRC 2 reg in rxmac address map
  * located at address 0x4008
  *
  * 31-16: crc2
  * 15-0: crc1
  */
 
-/*
- * structure for CRC 3 and CRC 4 reg in rxmac address map
+/* structure for CRC 3 and CRC 4 reg in rxmac address map
  * located at address 0x400C
  *
  * 31-16: crc4
  * 15-0: crc3
  */
 
-/*
- * structure for Wake On Lan Source Address Lo reg in rxmac address map
+/* structure for Wake On Lan Source Address Lo reg in rxmac address map
  * located at address 0x4010
  *
  * 31-24: sa3
@@ -732,30 +654,25 @@ struct txmac_regs {			/* Location: */
  * 15-8: sa5
  * 7-0: sa6
  */
-
 #define ET_RX_WOL_LO_SA3_SHIFT 24
 #define ET_RX_WOL_LO_SA4_SHIFT 16
 #define ET_RX_WOL_LO_SA5_SHIFT 8
 
-/*
- * structure for Wake On Lan Source Address Hi reg in rxmac address map
+/* structure for Wake On Lan Source Address Hi reg in rxmac address map
  * located at address 0x4014
  *
  * 31-16: reserved
  * 15-8: sa1
  * 7-0: sa2
  */
-
 #define ET_RX_WOL_HI_SA1_SHIFT 8
 
-/*
- * structure for Wake On Lan mask reg in rxmac address map
+/* structure for Wake On Lan mask reg in rxmac address map
  * located at address 0x4018 - 0x4064
  * Defined earlier (u32)
  */
 
-/*
- * structure for Unicast Packet Filter Address 1 reg in rxmac address map
+/* structure for Unicast Packet Filter Address 1 reg in rxmac address map
  * located at address 0x4068
  *
  * 31-24: addr1_3
@@ -763,13 +680,11 @@ struct txmac_regs {			/* Location: */
  * 15-8: addr1_5
  * 7-0: addr1_6
  */
-
 #define ET_RX_UNI_PF_ADDR1_3_SHIFT 24
 #define ET_RX_UNI_PF_ADDR1_4_SHIFT 16
 #define ET_RX_UNI_PF_ADDR1_5_SHIFT 8
 
-/*
- * structure for Unicast Packet Filter Address 2 reg in rxmac address map
+/* structure for Unicast Packet Filter Address 2 reg in rxmac address map
  * located at address 0x406C
  *
  * 31-24: addr2_3
@@ -777,13 +692,11 @@ struct txmac_regs {			/* Location: */
  * 15-8: addr2_5
  * 7-0: addr2_6
  */
-
 #define ET_RX_UNI_PF_ADDR2_3_SHIFT 24
 #define ET_RX_UNI_PF_ADDR2_4_SHIFT 16
 #define ET_RX_UNI_PF_ADDR2_5_SHIFT 8
 
-/*
- * structure for Unicast Packet Filter Address 1 & 2 reg in rxmac address map
+/* structure for Unicast Packet Filter Address 1 & 2 reg in rxmac address map
  * located at address 0x4070
  *
  * 31-24: addr2_1
@@ -791,19 +704,16 @@ struct txmac_regs {			/* Location: */
  * 15-8: addr1_1
  * 7-0: addr1_2
  */
-
 #define ET_RX_UNI_PF_ADDR2_1_SHIFT 24
 #define ET_RX_UNI_PF_ADDR2_2_SHIFT 16
 #define ET_RX_UNI_PF_ADDR1_1_SHIFT 8
 
-/*
- * structure for Multicast Hash reg in rxmac address map
+/* structure for Multicast Hash reg in rxmac address map
  * located at address 0x4074 - 0x4080
  * Defined earlier (u32)
  */
 
-/*
- * structure for Packet Filter Control reg in rxmac address map
+/* structure for Packet Filter Control reg in rxmac address map
  * located at address 0x4084
  *
  * 31-23: unused
@@ -814,15 +724,13 @@ struct txmac_regs {			/* Location: */
  * 1: filter_multi_en
  * 0: filter_broad_en
  */
-
 #define ET_RX_PFCTRL_MIN_PKT_SZ_SHIFT		16
 #define ET_RX_PFCTRL_FRAG_FILTER_ENABLE		0x0008
 #define ET_RX_PFCTRL_UNICST_FILTER_ENABLE	0x0004
 #define ET_RX_PFCTRL_MLTCST_FILTER_ENABLE	0x0002
 #define ET_RX_PFCTRL_BRDCST_FILTER_ENABLE	0x0001
 
-/*
- * structure for Memory Controller Interface Control Max Segment reg in rxmac
+/* structure for Memory Controller Interface Control Max Segment reg in rxmac
  * address map.  Located at address 0x4088
  *
  * 31-10: reserved
@@ -830,13 +738,11 @@ struct txmac_regs {			/* Location: */
  * 1: fc_en
  * 0: seg_en
  */
-
 #define ET_RX_MCIF_CTRL_MAX_SEG_SIZE_SHIFT	2
 #define ET_RX_MCIF_CTRL_MAX_SEG_FC_ENABLE	0x0002
 #define ET_RX_MCIF_CTRL_MAX_SEG_ENABLE		0x0001
 
-/*
- * structure for Memory Controller Interface Water Mark reg in rxmac address
+/* structure for Memory Controller Interface Water Mark reg in rxmac address
  * map.  Located at address 0x408C
  *
  * 31-26: unused
@@ -845,8 +751,7 @@ struct txmac_regs {			/* Location: */
  * 9-0: mark_lo
  */
 
-/*
- * structure for Rx Queue Dialog reg in rxmac address map.
+/* structure for Rx Queue Dialog reg in rxmac address map.
  * located at address 0x4090
  *
  * 31-26: reserved
@@ -855,8 +760,7 @@ struct txmac_regs {			/* Location: */
  * 9-0: wr_ptr
  */
 
-/*
- * structure for space available reg in rxmac address map.
+/* structure for space available reg in rxmac address map.
  * located at address 0x4094
  *
  * 31-17: reserved
@@ -865,8 +769,7 @@ struct txmac_regs {			/* Location: */
  * 9-0: space_avail
  */
 
-/*
- * structure for management interface reg in rxmac address map.
+/* structure for management interface reg in rxmac address map.
  * located at address 0x4098
  *
  * 31-18: reserved
@@ -874,8 +777,7 @@ struct txmac_regs {			/* Location: */
  * 16-0: drop_pkt_mask
  */
 
-/*
- * structure for Error reg in rxmac address map.
+/* structure for Error reg in rxmac address map.
  * located at address 0x409C
  *
  * 31-4: unused
@@ -885,8 +787,7 @@ struct txmac_regs {			/* Location: */
  * 0: mcif
  */
 
-/*
- * Rx MAC Module of JAGCore Address Mapping
+/* Rx MAC Module of JAGCore Address Mapping
  */
 struct rxmac_regs {					/* Location: */
 	u32 ctrl;					/*  0x4000 */
@@ -935,9 +836,7 @@ struct rxmac_regs {					/* Location: */
 /* END OF RXMAC REGISTER ADDRESS MAP */
 
 /* START OF MAC REGISTER ADDRESS MAP */
-
-/*
- * structure for configuration #1 reg in mac address map.
+/* structure for configuration #1 reg in mac address map.
  * located at address 0x5000
  *
  * 31: soft reset
@@ -957,7 +856,6 @@ struct rxmac_regs {					/* Location: */
  * 1: syncd tx en
  * 0: tx enable
  */
-
 #define ET_MAC_CFG1_SOFT_RESET		0x80000000
 #define ET_MAC_CFG1_SIM_RESET		0x40000000
 #define ET_MAC_CFG1_RESET_RXMC		0x00080000
@@ -971,8 +869,7 @@ struct rxmac_regs {					/* Location: */
 #define ET_MAC_CFG1_TX_ENABLE		0x00000001
 #define ET_MAC_CFG1_WAIT		0x0000000A	/* RX & TX syncd */
 
-/*
- * structure for configuration #2 reg in mac address map.
+/* structure for configuration #2 reg in mac address map.
  * located at address 0x5004
  * 31-16: reserved
  * 15-12: preamble
@@ -986,7 +883,6 @@ struct rxmac_regs {					/* Location: */
  * 1: crc enable
  * 0: full duplex
  */
-
 #define ET_MAC_CFG2_PREAMBLE_SHIFT	12
 #define ET_MAC_CFG2_IFMODE_MASK		0x0300
 #define ET_MAC_CFG2_IFMODE_1000		0x0200
@@ -997,8 +893,7 @@ struct rxmac_regs {					/* Location: */
 #define ET_MAC_CFG2_IFMODE_CRC_ENABLE	0x0002
 #define ET_MAC_CFG2_IFMODE_FULL_DPLX	0x0001
 
-/*
- * structure for Interpacket gap reg in mac address map.
+/* structure for Interpacket gap reg in mac address map.
  * located at address 0x5008
  *
  * 31: reserved
@@ -1021,25 +916,21 @@ struct rxmac_regs {					/* Location: */
  * 9-0: collision window
  */
 
-/*
- * structure for Maximum Frame Length reg in mac address map.
+/* structure for Maximum Frame Length reg in mac address map.
  * located at address 0x5010: bits 0-15 hold the length.
  */
 
-/*
- * structure for Reserve 1 reg in mac address map.
+/* structure for Reserve 1 reg in mac address map.
  * located at address 0x5014 - 0x5018
  * Defined earlier (u32)
  */
 
-/*
- * structure for Test reg in mac address map.
+/* structure for Test reg in mac address map.
  * located at address 0x501C
  * test: bits 0-2, rest unused
  */
 
-/*
- * structure for MII Management Configuration reg in mac address map.
+/* structure for MII Management Configuration reg in mac address map.
  * located at address 0x5020
  *
  * 31: reset MII mgmt
@@ -1049,57 +940,47 @@ struct rxmac_regs {					/* Location: */
  * 3: undefined
  * 2-0: mgmt clock reset
  */
-
 #define ET_MAC_MIIMGMT_CLK_RST	0x0007
 
-/*
- * structure for MII Management Command reg in mac address map.
+/* structure for MII Management Command reg in mac address map.
  * located at address 0x5024
  * bit 1: scan cycle
  * bit 0: read cycle
  */
 
-/*
- * structure for MII Management Address reg in mac address map.
+/* structure for MII Management Address reg in mac address map.
  * located at address 0x5028
  * 31-13: reserved
  * 12-8: phy addr
  * 7-5: reserved
  * 4-0: register
  */
-
 #define ET_MAC_MII_ADDR(phy, reg)	((phy) << 8 | (reg))
 
-/*
- * structure for MII Management Control reg in mac address map.
+/* structure for MII Management Control reg in mac address map.
  * located at address 0x502C
  * 31-16: reserved
  * 15-0: phy control
  */
 
-/*
- * structure for MII Management Status reg in mac address map.
+/* structure for MII Management Status reg in mac address map.
  * located at address 0x5030
  * 31-16: reserved
  * 15-0: phy control
  */
-
 #define ET_MAC_MIIMGMT_STAT_PHYCRTL_MASK 0xFFFF
 
-/*
- * structure for MII Management Indicators reg in mac address map.
+/* structure for MII Management Indicators reg in mac address map.
  * located at address 0x5034
  * 31-3: reserved
  * 2: not valid
  * 1: scanning
  * 0: busy
  */
-
 #define ET_MAC_MGMT_BUSY	0x00000001	/* busy */
 #define ET_MAC_MGMT_WAIT	0x00000005	/* busy | not valid */
 
-/*
- * structure for Interface Control reg in mac address map.
+/* structure for Interface Control reg in mac address map.
  * located at address 0x5038
  *
  * 31: reset if module
@@ -1120,12 +1001,10 @@ struct rxmac_regs {					/* Location: */
  * 6-1: reserved
  * 0: enable jabber protection
  */
-
 #define ET_MAC_IFCTRL_GHDMODE	(1 << 26)
 #define ET_MAC_IFCTRL_PHYMODE	(1 << 24)
 
-/*
- * structure for Interface Status reg in mac address map.
+/* structure for Interface Status reg in mac address map.
  * located at address 0x503C
  *
  * 31-10: reserved
@@ -1141,8 +1020,7 @@ struct rxmac_regs {					/* Location: */
  * 0: pe10t_jabber
  */
 
-/*
- * structure for Mac Station Address, Part 1 reg in mac address map.
+/* structure for Mac Station Address, Part 1 reg in mac address map.
  * located at address 0x5040
  *
  * 31-24: Octet6
@@ -1150,25 +1028,21 @@ struct rxmac_regs {					/* Location: */
  * 15-8: Octet4
  * 7-0: Octet3
  */
-
 #define ET_MAC_STATION_ADDR1_OC6_SHIFT 24
 #define ET_MAC_STATION_ADDR1_OC5_SHIFT 16
 #define ET_MAC_STATION_ADDR1_OC4_SHIFT 8
 
-/*
- * structure for Mac Station Address, Part 2 reg in mac address map.
+/* structure for Mac Station Address, Part 2 reg in mac address map.
  * located at address 0x5044
  *
  * 31-24: Octet2
  * 23-16: Octet1
  * 15-0: reserved
  */
-
 #define ET_MAC_STATION_ADDR2_OC2_SHIFT 24
 #define ET_MAC_STATION_ADDR2_OC1_SHIFT 16
 
-/*
- * MAC Module of JAGCore Address Mapping
+/* MAC Module of JAGCore Address Mapping
  */
 struct mac_regs {					/* Location: */
 	u32 cfg1;					/*  0x5000 */
@@ -1194,9 +1068,7 @@ struct mac_regs {					/* Location: */
 /* END OF MAC REGISTER ADDRESS MAP */
 
 /* START OF MAC STAT REGISTER ADDRESS MAP */
-
-/*
- * structure for Carry Register One and it's Mask Register reg located in mac
+/* structure for Carry Register One and it's Mask Register reg located in mac
  * stat address map address 0x6130 and 0x6138.
  *
  * 31: tr64
@@ -1226,8 +1098,7 @@ struct mac_regs {					/* Location: */
  * 0: rdrp
  */
 
-/*
- * structure for Carry Register Two Mask Register reg in mac stat address map.
+/* structure for Carry Register Two Mask Register reg in mac stat address map.
  * located at address 0x613C
  *
  * 31-20: unused
@@ -1253,166 +1124,68 @@ struct mac_regs {					/* Location: */
  * 0: tdrp
  */
 
-/*
- * MAC STATS Module of JAGCore Address Mapping
+/* MAC STATS Module of JAGCore Address Mapping
  */
 struct macstat_regs {			/* Location: */
 	u32 pad[32];			/*  0x6000 - 607C */
 
-	/* Tx/Rx 0-64 Byte Frame Counter */
+	/* counters */
 	u32 txrx_0_64_byte_frames;	/*  0x6080 */
-
-	/* Tx/Rx 65-127 Byte Frame Counter */
 	u32 txrx_65_127_byte_frames;	/*  0x6084 */
-
-	/* Tx/Rx 128-255 Byte Frame Counter */
 	u32 txrx_128_255_byte_frames;	/*  0x6088 */
-
-	/* Tx/Rx 256-511 Byte Frame Counter */
 	u32 txrx_256_511_byte_frames;	/*  0x608C */
-
-	/* Tx/Rx 512-1023 Byte Frame Counter */
 	u32 txrx_512_1023_byte_frames;	/*  0x6090 */
-
-	/* Tx/Rx 1024-1518 Byte Frame Counter */
 	u32 txrx_1024_1518_byte_frames;	/*  0x6094 */
-
-	/* Tx/Rx 1519-1522 Byte Good VLAN Frame Count */
 	u32 txrx_1519_1522_gvln_frames;	/*  0x6098 */
-
-	/* Rx Byte Counter */
 	u32 rx_bytes;			/*  0x609C */
-
-	/* Rx Packet Counter */
 	u32 rx_packets;			/*  0x60A0 */
-
-	/* Rx FCS Error Counter */
 	u32 rx_fcs_errs;		/*  0x60A4 */
-
-	/* Rx Multicast Packet Counter */
 	u32 rx_multicast_packets;	/*  0x60A8 */
-
-	/* Rx Broadcast Packet Counter */
 	u32 rx_broadcast_packets;	/*  0x60AC */
-
-	/* Rx Control Frame Packet Counter */
 	u32 rx_control_frames;		/*  0x60B0 */
-
-	/* Rx Pause Frame Packet Counter */
 	u32 rx_pause_frames;		/*  0x60B4 */
-
-	/* Rx Unknown OP Code Counter */
 	u32 rx_unknown_opcodes;		/*  0x60B8 */
-
-	/* Rx Alignment Error Counter */
 	u32 rx_align_errs;		/*  0x60BC */
-
-	/* Rx Frame Length Error Counter */
 	u32 rx_frame_len_errs;		/*  0x60C0 */
-
-	/* Rx Code Error Counter */
 	u32 rx_code_errs;		/*  0x60C4 */
-
-	/* Rx Carrier Sense Error Counter */
 	u32 rx_carrier_sense_errs;	/*  0x60C8 */
-
-	/* Rx Undersize Packet Counter */
 	u32 rx_undersize_packets;	/*  0x60CC */
-
-	/* Rx Oversize Packet Counter */
 	u32 rx_oversize_packets;	/*  0x60D0 */
-
-	/* Rx Fragment Counter */
 	u32 rx_fragment_packets;	/*  0x60D4 */
-
-	/* Rx Jabber Counter */
 	u32 rx_jabbers;			/*  0x60D8 */
-
-	/* Rx Drop */
 	u32 rx_drops;			/*  0x60DC */
-
-	/* Tx Byte Counter */
 	u32 tx_bytes;			/*  0x60E0 */
-
-	/* Tx Packet Counter */
 	u32 tx_packets;			/*  0x60E4 */
-
-	/* Tx Multicast Packet Counter */
 	u32 tx_multicast_packets;	/*  0x60E8 */
-
-	/* Tx Broadcast Packet Counter */
 	u32 tx_broadcast_packets;	/*  0x60EC */
-
-	/* Tx Pause Control Frame Counter */
 	u32 tx_pause_frames;		/*  0x60F0 */
-
-	/* Tx Deferral Packet Counter */
 	u32 tx_deferred;		/*  0x60F4 */
-
-	/* Tx Excessive Deferral Packet Counter */
 	u32 tx_excessive_deferred;	/*  0x60F8 */
-
-	/* Tx Single Collision Packet Counter */
 	u32 tx_single_collisions;	/*  0x60FC */
-
-	/* Tx Multiple Collision Packet Counter */
 	u32 tx_multiple_collisions;	/*  0x6100 */
-
-	/* Tx Late Collision Packet Counter */
 	u32 tx_late_collisions;		/*  0x6104 */
-
-	/* Tx Excessive Collision Packet Counter */
 	u32 tx_excessive_collisions;	/*  0x6108 */
-
-	/* Tx Total Collision Packet Counter */
 	u32 tx_total_collisions;	/*  0x610C */
-
-	/* Tx Pause Frame Honored Counter */
 	u32 tx_pause_honored_frames;	/*  0x6110 */
-
-	/* Tx Drop Frame Counter */
 	u32 tx_drops;			/*  0x6114 */
-
-	/* Tx Jabber Frame Counter */
 	u32 tx_jabbers;			/*  0x6118 */
-
-	/* Tx FCS Error Counter */
 	u32 tx_fcs_errs;		/*  0x611C */
-
-	/* Tx Control Frame Counter */
 	u32 tx_control_frames;		/*  0x6120 */
-
-	/* Tx Oversize Frame Counter */
 	u32 tx_oversize_frames;		/*  0x6124 */
-
-	/* Tx Undersize Frame Counter */
 	u32 tx_undersize_frames;	/*  0x6128 */
-
-	/* Tx Fragments Frame Counter */
 	u32 tx_fragments;		/*  0x612C */
-
-	/* Carry Register One Register */
 	u32 carry_reg1;			/*  0x6130 */
-
-	/* Carry Register Two Register */
 	u32 carry_reg2;			/*  0x6134 */
-
-	/* Carry Register One Mask Register */
 	u32 carry_reg1_mask;		/*  0x6138 */
-
-	/* Carry Register Two Mask Register */
 	u32 carry_reg2_mask;		/*  0x613C */
 };
 
 /* END OF MAC STAT REGISTER ADDRESS MAP */
 
 /* START OF MMC REGISTER ADDRESS MAP */
-
-/*
- * Main Memory Controller Control reg in mmc address map.
+/* Main Memory Controller Control reg in mmc address map.
  * located at address 0x7000
  */
-
 #define ET_MMC_ENABLE		1
 #define ET_MMC_ARB_DISABLE	2
 #define ET_MMC_RXMAC_DISABLE	4
@@ -1421,23 +1194,19 @@ struct macstat_regs {			/* Location: */
 #define ET_MMC_RXDMA_DISABLE	32
 #define ET_MMC_FORCE_CE		64
 
-/*
- * Main Memory Controller Host Memory Access Address reg in mmc
+/* Main Memory Controller Host Memory Access Address reg in mmc
  * address map.  Located at address 0x7004. Top 16 bits hold the address bits
  */
-
 #define ET_SRAM_REQ_ACCESS	1
 #define ET_SRAM_WR_ACCESS	2
 #define ET_SRAM_IS_CTRL		4
 
-/*
- * structure for Main Memory Controller Host Memory Access Data reg in mmc
+/* structure for Main Memory Controller Host Memory Access Data reg in mmc
  * address map.  Located at address 0x7008 - 0x7014
  * Defined earlier (u32)
  */
 
-/*
- * Memory Control Module of JAGCore Address Mapping
+/* Memory Control Module of JAGCore Address Mapping
  */
 struct mmc_regs {		/* Location: */
 	u32 mmc_ctrl;		/*  0x7000 */
@@ -1450,9 +1219,7 @@ struct mmc_regs {		/* Location: */
 
 /* END OF MMC REGISTER ADDRESS MAP */
 
-
-/*
- * JAGCore Address Mapping
+/* JAGCore Address Mapping
  */
 struct address_map {
 	struct global_regs global;
@@ -1481,16 +1248,13 @@ struct address_map {
 	u8 unused_mmc[4096 - sizeof(struct mmc_regs)];
 	/* unused section of address map */
 	u8 unused_[1015808];
-
 	u8 unused_exp_rom[4096];	/* MGS-size TBD */
 	u8 unused__[524288];	/* unused section of address map */
 };
 
-/*
- * Defines for generic MII registers 0x00 -> 0x0F can be found in
+/* Defines for generic MII registers 0x00 -> 0x0F can be found in
  * include/linux/mii.h
  */
-
 /* some defines for modem registers that seem to be 'reserved' */
 #define PHY_INDEX_REG              0x10
 #define PHY_DATA_REG               0x11
@@ -1550,7 +1314,6 @@ struct address_map {
  *	3:	reserved
  *	2-0:	mac_if_mode
  */
-
 #define ET_PHY_CONFIG_TX_FIFO_DEPTH	0x3000
 
 #define ET_PHY_CONFIG_FIFO_DEPTH_8	0x0000
diff --git a/drivers/net/ethernet/allwinner/sun4i-emac.c b/drivers/net/ethernet/allwinner/sun4i-emac.c
index 29b9f082475d..1fcd5568a352 100644
--- a/drivers/net/ethernet/allwinner/sun4i-emac.c
+++ b/drivers/net/ethernet/allwinner/sun4i-emac.c
@@ -878,8 +878,6 @@ static int emac_probe(struct platform_device *pdev)
 	emac_powerup(ndev);
 	emac_reset(db);
 
-	ether_setup(ndev);
-
 	ndev->netdev_ops = &emac_netdev_ops;
 	ndev->watchdog_timeo = msecs_to_jiffies(watchdog);
 	ndev->ethtool_ops = &emac_ethtool_ops;
diff --git a/drivers/net/ethernet/altera/altera_tse_main.c b/drivers/net/ethernet/altera/altera_tse_main.c
index 7330681574d2..4efc4355d345 100644
--- a/drivers/net/ethernet/altera/altera_tse_main.c
+++ b/drivers/net/ethernet/altera/altera_tse_main.c
@@ -728,6 +728,44 @@ static struct phy_device *connect_local_phy(struct net_device *dev)
 	return phydev;
 }
 
+static int altera_tse_phy_get_addr_mdio_create(struct net_device *dev)
+{
+	struct altera_tse_private *priv = netdev_priv(dev);
+	struct device_node *np = priv->device->of_node;
+	int ret = 0;
+
+	priv->phy_iface = of_get_phy_mode(np);
+
+	/* Avoid get phy addr and create mdio if no phy is present */
+	if (!priv->phy_iface)
+		return 0;
+
+	/* try to get PHY address from device tree, use PHY autodetection if
+	 * no valid address is given
+	 */
+
+	if (of_property_read_u32(priv->device->of_node, "phy-addr",
+			 &priv->phy_addr)) {
+		priv->phy_addr = POLL_PHY;
+	}
+
+	if (!((priv->phy_addr == POLL_PHY) ||
+		  ((priv->phy_addr >= 0) && (priv->phy_addr < PHY_MAX_ADDR)))) {
+		netdev_err(dev, "invalid phy-addr specified %d\n",
+			priv->phy_addr);
+		return -ENODEV;
+	}
+
+	/* Create/attach to MDIO bus */
+	ret = altera_tse_mdio_create(dev,
+					 atomic_add_return(1, &instance_count));
+
+	if (ret)
+		return -ENODEV;
+
+	return 0;
+}
+
 /* Initialize driver's PHY state, and attach to the PHY
  */
 static int init_phy(struct net_device *dev)
@@ -736,6 +774,10 @@ static int init_phy(struct net_device *dev)
 	struct phy_device *phydev;
 	struct device_node *phynode;
 
+	/* Avoid init phy in case of no phy present */
+	if (!priv->phy_iface)
+		return 0;
+
 	priv->oldlink = 0;
 	priv->oldspeed = 0;
 	priv->oldduplex = -1;
@@ -1231,7 +1273,6 @@ static int altera_tse_probe(struct platform_device *pdev)
 	struct resource *dma_res;
 	struct altera_tse_private *priv;
 	const unsigned char *macaddr;
-	struct device_node *np = pdev->dev.of_node;
 	void __iomem *descmap;
 	const struct of_device_id *of_id = NULL;
 
@@ -1408,32 +1449,13 @@ static int altera_tse_probe(struct platform_device *pdev)
 	else
 		eth_hw_addr_random(ndev);
 
-	priv->phy_iface = of_get_phy_mode(np);
-
-	/* try to get PHY address from device tree, use PHY autodetection if
-	 * no valid address is given
-	 */
-	if (of_property_read_u32(pdev->dev.of_node, "phy-addr",
-				 &priv->phy_addr)) {
-		priv->phy_addr = POLL_PHY;
-	}
-
-	if (!((priv->phy_addr == POLL_PHY) ||
-	      ((priv->phy_addr >= 0) && (priv->phy_addr < PHY_MAX_ADDR)))) {
-		dev_err(&pdev->dev, "invalid phy-addr specified %d\n",
-			priv->phy_addr);
-		goto err_free_netdev;
-	}
-
-	/* Create/attach to MDIO bus */
-	ret = altera_tse_mdio_create(ndev,
-				     atomic_add_return(1, &instance_count));
+	/* get phy addr and create mdio */
+	ret = altera_tse_phy_get_addr_mdio_create(ndev);
 
 	if (ret)
 		goto err_free_netdev;
 
 	/* initialize netdev */
-	ether_setup(ndev);
 	ndev->mem_start = control_port->start;
 	ndev->mem_end = control_port->end;
 	ndev->netdev_ops = &altera_tse_netdev_ops;
diff --git a/drivers/net/ethernet/amd/au1000_eth.c b/drivers/net/ethernet/amd/au1000_eth.c
index 31c48a7ac2b6..6c323f4f457b 100644
--- a/drivers/net/ethernet/amd/au1000_eth.c
+++ b/drivers/net/ethernet/amd/au1000_eth.c
@@ -1140,7 +1140,6 @@ static const struct net_device_ops au1000_netdev_ops = {
 
 static int au1000_probe(struct platform_device *pdev)
 {
-	static unsigned version_printed;
 	struct au1000_private *aup = NULL;
 	struct au1000_eth_platform_data *pd;
 	struct net_device *dev = NULL;
@@ -1371,9 +1370,8 @@ static int au1000_probe(struct platform_device *pdev)
 
 	netdev_info(dev, "Au1xx0 Ethernet found at 0x%lx, irq %d\n",
 			(unsigned long)base->start, irq);
-	if (version_printed++ == 0)
-		pr_info("%s version %s %s\n",
-					DRV_NAME, DRV_VERSION, DRV_AUTHOR);
+
+	pr_info_once("%s version %s %s\n", DRV_NAME, DRV_VERSION, DRV_AUTHOR);
 
 	return 0;
 
diff --git a/drivers/net/ethernet/amd/nmclan_cs.c b/drivers/net/ethernet/amd/nmclan_cs.c
index abf3b1581c82..5b22764ba88d 100644
--- a/drivers/net/ethernet/amd/nmclan_cs.c
+++ b/drivers/net/ethernet/amd/nmclan_cs.c
@@ -621,7 +621,7 @@ static int nmclan_config(struct pcmcia_device *link)
   ret = pcmcia_request_io(link);
   if (ret)
 	  goto failed;
-  ret = pcmcia_request_exclusive_irq(link, mace_interrupt);
+  ret = pcmcia_request_irq(link, mace_interrupt);
   if (ret)
 	  goto failed;
   ret = pcmcia_enable_device(link);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-common.h b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
index cc25a3a9e7cf..caade30820d5 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-common.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-common.h
@@ -271,7 +271,6 @@
 #define DMA_PBL_X8_DISABLE		0x00
 #define DMA_PBL_X8_ENABLE		0x01
 
-
 /* MAC register offsets */
 #define MAC_TCR				0x0000
 #define MAC_RCR				0x0004
@@ -792,7 +791,6 @@
 #define MTL_Q_DISABLED			0x00
 #define MTL_Q_ENABLED			0x02
 
-
 /* MTL traffic class register offsets
  *   Multiple traffic classes can be active.  The first class has registers
  *   that begin at 0x1100.  Each subsequent queue has registers that
@@ -815,7 +813,6 @@
 #define MTL_TSA_SP			0x00
 #define MTL_TSA_ETS			0x02
 
-
 /* PCS MMD select register offset
  *  The MMD select register is used for accessing PCS registers
  *  when the underlying APB3 interface is using indirect addressing.
@@ -825,7 +822,6 @@
  */
 #define PCS_MMD_SELECT			0xff
 
-
 /* Descriptor/Packet entry bit positions and sizes */
 #define RX_PACKET_ERRORS_CRC_INDEX		2
 #define RX_PACKET_ERRORS_CRC_WIDTH		1
@@ -929,7 +925,6 @@
 #define MDIO_AN_COMP_STAT		0x0030
 #endif
 
-
 /* Bit setting and getting macros
  *  The get macro will extract the current bit field value from within
  *  the variable
@@ -957,7 +952,6 @@ do {									\
 			      ((0x1 << (_width)) - 1)) << (_index)));	\
 } while (0)
 
-
 /* Bit setting and getting macros based on register fields
  *  The get macro uses the bit field definitions formed using the input
  *  names to extract the current bit field value from within the
@@ -986,7 +980,6 @@ do {									\
 		 _prefix##_##_field##_INDEX,				\
 		 _prefix##_##_field##_WIDTH, (_val))
 
-
 /* Macros for reading or writing registers
  *  The ioread macros will get bit fields or full values using the
  *  register definitions formed using the input names
@@ -1014,7 +1007,6 @@ do {									\
 	XGMAC_IOWRITE((_pdata), _reg, reg_val);				\
 } while (0)
 
-
 /* Macros for reading or writing MTL queue or traffic class registers
  *  Similar to the standard read and write macros except that the
  *  base register value is calculated by the queue or traffic class number
@@ -1041,7 +1033,6 @@ do {									\
 	XGMAC_MTL_IOWRITE((_pdata), (_n), _reg, reg_val);		\
 } while (0)
 
-
 /* Macros for reading or writing DMA channel registers
  *  Similar to the standard read and write macros except that the
  *  base register value is obtained from the ring
@@ -1066,7 +1057,6 @@ do {									\
 	XGMAC_DMA_IOWRITE((_channel), _reg, reg_val);			\
 } while (0)
 
-
 /* Macros for building, reading or writing register values or bits
  * within the register values of XPCS registers.
  */
@@ -1076,7 +1066,6 @@ do {									\
 #define XPCS_IOREAD(_pdata, _off)					\
 	ioread32((_pdata)->xpcs_regs + (_off))
 
-
 /* Macros for building, reading or writing register values or bits
  * using MDIO.  Different from above because of the use of standardized
  * Linux include values.  No shifting is performed with the bit
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c b/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c
index 7d6a49b24321..8a50b01c2686 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-dcb.c
@@ -120,7 +120,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 static int xgbe_dcb_ieee_getets(struct net_device *netdev,
 				struct ieee_ets *ets)
 {
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-debugfs.c b/drivers/net/ethernet/amd/xgbe/xgbe-debugfs.c
index 346592dca33c..76479d04b903 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-debugfs.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-debugfs.c
@@ -121,7 +121,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 static ssize_t xgbe_common_read(char __user *buffer, size_t count,
 				loff_t *ppos, unsigned int value)
 {
@@ -272,8 +271,8 @@ static ssize_t xpcs_reg_value_read(struct file *filp, char __user *buffer,
 	struct xgbe_prv_data *pdata = filp->private_data;
 	unsigned int value;
 
-	value = pdata->hw_if.read_mmd_regs(pdata, pdata->debugfs_xpcs_mmd,
-					   pdata->debugfs_xpcs_reg);
+	value = XMDIO_READ(pdata, pdata->debugfs_xpcs_mmd,
+			   pdata->debugfs_xpcs_reg);
 
 	return xgbe_common_read(buffer, count, ppos, value);
 }
@@ -290,8 +289,8 @@ static ssize_t xpcs_reg_value_write(struct file *filp,
 	if (len < 0)
 		return len;
 
-	pdata->hw_if.write_mmd_regs(pdata, pdata->debugfs_xpcs_mmd,
-				    pdata->debugfs_xpcs_reg, value);
+	XMDIO_WRITE(pdata, pdata->debugfs_xpcs_mmd, pdata->debugfs_xpcs_reg,
+		    value);
 
 	return len;
 }
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-desc.c b/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
index 1c5d62e8dab6..6fc5da01437d 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-desc.c
@@ -117,7 +117,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 static void xgbe_unmap_skb(struct xgbe_prv_data *, struct xgbe_ring_data *);
 
 static void xgbe_free_ring(struct xgbe_prv_data *pdata,
@@ -524,11 +523,8 @@ static void xgbe_realloc_skb(struct xgbe_channel *channel)
 
 		/* Allocate skb & assign to each rdesc */
 		skb = dev_alloc_skb(pdata->rx_buf_size);
-		if (skb == NULL) {
-			netdev_alert(pdata->netdev,
-				     "failed to allocate skb\n");
+		if (skb == NULL)
 			break;
-		}
 		skb_dma = dma_map_single(pdata->dev, skb->data,
 					 pdata->rx_buf_size, DMA_FROM_DEVICE);
 		if (dma_mapping_error(pdata->dev, skb_dma)) {
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
index edaca4496264..9da3a03e8c07 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-dev.c
@@ -122,7 +122,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 static unsigned int xgbe_usec_to_riwt(struct xgbe_prv_data *pdata,
 				      unsigned int usec)
 {
@@ -348,7 +347,7 @@ static int xgbe_disable_tx_flow_control(struct xgbe_prv_data *pdata)
 
 	/* Clear MAC flow control */
 	max_q_count = XGMAC_MAX_FLOW_CONTROL_QUEUES;
-	q_count = min_t(unsigned int, pdata->rx_q_count, max_q_count);
+	q_count = min_t(unsigned int, pdata->tx_q_count, max_q_count);
 	reg = MAC_Q0TFCR;
 	for (i = 0; i < q_count; i++) {
 		reg_val = XGMAC_IOREAD(pdata, reg);
@@ -373,7 +372,7 @@ static int xgbe_enable_tx_flow_control(struct xgbe_prv_data *pdata)
 
 	/* Set MAC flow control */
 	max_q_count = XGMAC_MAX_FLOW_CONTROL_QUEUES;
-	q_count = min_t(unsigned int, pdata->rx_q_count, max_q_count);
+	q_count = min_t(unsigned int, pdata->tx_q_count, max_q_count);
 	reg = MAC_Q0TFCR;
 	for (i = 0; i < q_count; i++) {
 		reg_val = XGMAC_IOREAD(pdata, reg);
@@ -509,8 +508,8 @@ static void xgbe_enable_mac_interrupts(struct xgbe_prv_data *pdata)
 	XGMAC_IOWRITE(pdata, MAC_IER, mac_ier);
 
 	/* Enable all counter interrupts */
-	XGMAC_IOWRITE_BITS(pdata, MMC_RIER, ALL_INTERRUPTS, 0xff);
-	XGMAC_IOWRITE_BITS(pdata, MMC_TIER, ALL_INTERRUPTS, 0xff);
+	XGMAC_IOWRITE_BITS(pdata, MMC_RIER, ALL_INTERRUPTS, 0xffffffff);
+	XGMAC_IOWRITE_BITS(pdata, MMC_TIER, ALL_INTERRUPTS, 0xffffffff);
 }
 
 static int xgbe_set_gmii_speed(struct xgbe_prv_data *pdata)
@@ -1633,6 +1632,9 @@ static int xgbe_flush_tx_queues(struct xgbe_prv_data *pdata)
 {
 	unsigned int i, count;
 
+	if (XGMAC_GET_BITS(pdata->hw_feat.version, MAC_VR, SNPSVER) < 0x21)
+		return 0;
+
 	for (i = 0; i < pdata->tx_q_count; i++)
 		XGMAC_MTL_IOWRITE_BITS(pdata, i, MTL_Q_TQOMR, FTQ, 1);
 
@@ -1703,8 +1705,8 @@ static void xgbe_config_mtl_mode(struct xgbe_prv_data *pdata)
 	XGMAC_IOWRITE_BITS(pdata, MTL_OMR, RAA, MTL_RAA_SP);
 }
 
-static unsigned int xgbe_calculate_per_queue_fifo(unsigned long fifo_size,
-						  unsigned char queue_count)
+static unsigned int xgbe_calculate_per_queue_fifo(unsigned int fifo_size,
+						  unsigned int queue_count)
 {
 	unsigned int q_fifo_size = 0;
 	enum xgbe_mtl_fifo_size p_fifo = XGMAC_MTL_FIFO_SIZE_256;
@@ -1748,6 +1750,10 @@ static unsigned int xgbe_calculate_per_queue_fifo(unsigned long fifo_size,
 		q_fifo_size = XGBE_FIFO_SIZE_KB(256);
 		break;
 	}
+
+	/* The configured value is not the actual amount of fifo RAM */
+	q_fifo_size = min_t(unsigned int, XGBE_FIFO_MAX, q_fifo_size);
+
 	q_fifo_size = q_fifo_size / queue_count;
 
 	/* Set the queue fifo size programmable value */
@@ -1947,6 +1953,32 @@ static void xgbe_config_vlan_support(struct xgbe_prv_data *pdata)
 		xgbe_disable_rx_vlan_stripping(pdata);
 }
 
+static u64 xgbe_mmc_read(struct xgbe_prv_data *pdata, unsigned int reg_lo)
+{
+	bool read_hi;
+	u64 val;
+
+	switch (reg_lo) {
+	/* These registers are always 64 bit */
+	case MMC_TXOCTETCOUNT_GB_LO:
+	case MMC_TXOCTETCOUNT_G_LO:
+	case MMC_RXOCTETCOUNT_GB_LO:
+	case MMC_RXOCTETCOUNT_G_LO:
+		read_hi = true;
+		break;
+
+	default:
+		read_hi = false;
+	};
+
+	val = XGMAC_IOREAD(pdata, reg_lo);
+
+	if (read_hi)
+		val |= ((u64)XGMAC_IOREAD(pdata, reg_lo + 4) << 32);
+
+	return val;
+}
+
 static void xgbe_tx_mmc_int(struct xgbe_prv_data *pdata)
 {
 	struct xgbe_mmc_stats *stats = &pdata->mmc_stats;
@@ -1954,75 +1986,75 @@ static void xgbe_tx_mmc_int(struct xgbe_prv_data *pdata)
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXOCTETCOUNT_GB))
 		stats->txoctetcount_gb +=
-			XGMAC_IOREAD(pdata, MMC_TXOCTETCOUNT_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TXOCTETCOUNT_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXFRAMECOUNT_GB))
 		stats->txframecount_gb +=
-			XGMAC_IOREAD(pdata, MMC_TXFRAMECOUNT_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TXFRAMECOUNT_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXBROADCASTFRAMES_G))
 		stats->txbroadcastframes_g +=
-			XGMAC_IOREAD(pdata, MMC_TXBROADCASTFRAMES_G_LO);
+			xgbe_mmc_read(pdata, MMC_TXBROADCASTFRAMES_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXMULTICASTFRAMES_G))
 		stats->txmulticastframes_g +=
-			XGMAC_IOREAD(pdata, MMC_TXMULTICASTFRAMES_G_LO);
+			xgbe_mmc_read(pdata, MMC_TXMULTICASTFRAMES_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TX64OCTETS_GB))
 		stats->tx64octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_TX64OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TX64OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TX65TO127OCTETS_GB))
 		stats->tx65to127octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_TX65TO127OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TX65TO127OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TX128TO255OCTETS_GB))
 		stats->tx128to255octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_TX128TO255OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TX128TO255OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TX256TO511OCTETS_GB))
 		stats->tx256to511octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_TX256TO511OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TX256TO511OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TX512TO1023OCTETS_GB))
 		stats->tx512to1023octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_TX512TO1023OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TX512TO1023OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TX1024TOMAXOCTETS_GB))
 		stats->tx1024tomaxoctets_gb +=
-			XGMAC_IOREAD(pdata, MMC_TX1024TOMAXOCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TX1024TOMAXOCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXUNICASTFRAMES_GB))
 		stats->txunicastframes_gb +=
-			XGMAC_IOREAD(pdata, MMC_TXUNICASTFRAMES_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TXUNICASTFRAMES_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXMULTICASTFRAMES_GB))
 		stats->txmulticastframes_gb +=
-			XGMAC_IOREAD(pdata, MMC_TXMULTICASTFRAMES_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TXMULTICASTFRAMES_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXBROADCASTFRAMES_GB))
 		stats->txbroadcastframes_g +=
-			XGMAC_IOREAD(pdata, MMC_TXBROADCASTFRAMES_GB_LO);
+			xgbe_mmc_read(pdata, MMC_TXBROADCASTFRAMES_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXUNDERFLOWERROR))
 		stats->txunderflowerror +=
-			XGMAC_IOREAD(pdata, MMC_TXUNDERFLOWERROR_LO);
+			xgbe_mmc_read(pdata, MMC_TXUNDERFLOWERROR_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXOCTETCOUNT_G))
 		stats->txoctetcount_g +=
-			XGMAC_IOREAD(pdata, MMC_TXOCTETCOUNT_G_LO);
+			xgbe_mmc_read(pdata, MMC_TXOCTETCOUNT_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXFRAMECOUNT_G))
 		stats->txframecount_g +=
-			XGMAC_IOREAD(pdata, MMC_TXFRAMECOUNT_G_LO);
+			xgbe_mmc_read(pdata, MMC_TXFRAMECOUNT_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXPAUSEFRAMES))
 		stats->txpauseframes +=
-			XGMAC_IOREAD(pdata, MMC_TXPAUSEFRAMES_LO);
+			xgbe_mmc_read(pdata, MMC_TXPAUSEFRAMES_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_TISR, TXVLANFRAMES_G))
 		stats->txvlanframes_g +=
-			XGMAC_IOREAD(pdata, MMC_TXVLANFRAMES_G_LO);
+			xgbe_mmc_read(pdata, MMC_TXVLANFRAMES_G_LO);
 }
 
 static void xgbe_rx_mmc_int(struct xgbe_prv_data *pdata)
@@ -2032,95 +2064,95 @@ static void xgbe_rx_mmc_int(struct xgbe_prv_data *pdata)
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXFRAMECOUNT_GB))
 		stats->rxframecount_gb +=
-			XGMAC_IOREAD(pdata, MMC_RXFRAMECOUNT_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RXFRAMECOUNT_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXOCTETCOUNT_GB))
 		stats->rxoctetcount_gb +=
-			XGMAC_IOREAD(pdata, MMC_RXOCTETCOUNT_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RXOCTETCOUNT_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXOCTETCOUNT_G))
 		stats->rxoctetcount_g +=
-			XGMAC_IOREAD(pdata, MMC_RXOCTETCOUNT_G_LO);
+			xgbe_mmc_read(pdata, MMC_RXOCTETCOUNT_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXBROADCASTFRAMES_G))
 		stats->rxbroadcastframes_g +=
-			XGMAC_IOREAD(pdata, MMC_RXBROADCASTFRAMES_G_LO);
+			xgbe_mmc_read(pdata, MMC_RXBROADCASTFRAMES_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXMULTICASTFRAMES_G))
 		stats->rxmulticastframes_g +=
-			XGMAC_IOREAD(pdata, MMC_RXMULTICASTFRAMES_G_LO);
+			xgbe_mmc_read(pdata, MMC_RXMULTICASTFRAMES_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXCRCERROR))
 		stats->rxcrcerror +=
-			XGMAC_IOREAD(pdata, MMC_RXCRCERROR_LO);
+			xgbe_mmc_read(pdata, MMC_RXCRCERROR_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXRUNTERROR))
 		stats->rxrunterror +=
-			XGMAC_IOREAD(pdata, MMC_RXRUNTERROR);
+			xgbe_mmc_read(pdata, MMC_RXRUNTERROR);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXJABBERERROR))
 		stats->rxjabbererror +=
-			XGMAC_IOREAD(pdata, MMC_RXJABBERERROR);
+			xgbe_mmc_read(pdata, MMC_RXJABBERERROR);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXUNDERSIZE_G))
 		stats->rxundersize_g +=
-			XGMAC_IOREAD(pdata, MMC_RXUNDERSIZE_G);
+			xgbe_mmc_read(pdata, MMC_RXUNDERSIZE_G);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXOVERSIZE_G))
 		stats->rxoversize_g +=
-			XGMAC_IOREAD(pdata, MMC_RXOVERSIZE_G);
+			xgbe_mmc_read(pdata, MMC_RXOVERSIZE_G);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RX64OCTETS_GB))
 		stats->rx64octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_RX64OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RX64OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RX65TO127OCTETS_GB))
 		stats->rx65to127octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_RX65TO127OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RX65TO127OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RX128TO255OCTETS_GB))
 		stats->rx128to255octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_RX128TO255OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RX128TO255OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RX256TO511OCTETS_GB))
 		stats->rx256to511octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_RX256TO511OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RX256TO511OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RX512TO1023OCTETS_GB))
 		stats->rx512to1023octets_gb +=
-			XGMAC_IOREAD(pdata, MMC_RX512TO1023OCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RX512TO1023OCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RX1024TOMAXOCTETS_GB))
 		stats->rx1024tomaxoctets_gb +=
-			XGMAC_IOREAD(pdata, MMC_RX1024TOMAXOCTETS_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RX1024TOMAXOCTETS_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXUNICASTFRAMES_G))
 		stats->rxunicastframes_g +=
-			XGMAC_IOREAD(pdata, MMC_RXUNICASTFRAMES_G_LO);
+			xgbe_mmc_read(pdata, MMC_RXUNICASTFRAMES_G_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXLENGTHERROR))
 		stats->rxlengtherror +=
-			XGMAC_IOREAD(pdata, MMC_RXLENGTHERROR_LO);
+			xgbe_mmc_read(pdata, MMC_RXLENGTHERROR_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXOUTOFRANGETYPE))
 		stats->rxoutofrangetype +=
-			XGMAC_IOREAD(pdata, MMC_RXOUTOFRANGETYPE_LO);
+			xgbe_mmc_read(pdata, MMC_RXOUTOFRANGETYPE_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXPAUSEFRAMES))
 		stats->rxpauseframes +=
-			XGMAC_IOREAD(pdata, MMC_RXPAUSEFRAMES_LO);
+			xgbe_mmc_read(pdata, MMC_RXPAUSEFRAMES_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXFIFOOVERFLOW))
 		stats->rxfifooverflow +=
-			XGMAC_IOREAD(pdata, MMC_RXFIFOOVERFLOW_LO);
+			xgbe_mmc_read(pdata, MMC_RXFIFOOVERFLOW_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXVLANFRAMES_GB))
 		stats->rxvlanframes_gb +=
-			XGMAC_IOREAD(pdata, MMC_RXVLANFRAMES_GB_LO);
+			xgbe_mmc_read(pdata, MMC_RXVLANFRAMES_GB_LO);
 
 	if (XGMAC_GET_BITS(mmc_isr, MMC_RISR, RXWATCHDOGERROR))
 		stats->rxwatchdogerror +=
-			XGMAC_IOREAD(pdata, MMC_RXWATCHDOGERROR);
+			xgbe_mmc_read(pdata, MMC_RXWATCHDOGERROR);
 }
 
 static void xgbe_read_mmc_stats(struct xgbe_prv_data *pdata)
@@ -2131,127 +2163,127 @@ static void xgbe_read_mmc_stats(struct xgbe_prv_data *pdata)
 	XGMAC_IOWRITE_BITS(pdata, MMC_CR, MCF, 1);
 
 	stats->txoctetcount_gb +=
-		XGMAC_IOREAD(pdata, MMC_TXOCTETCOUNT_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TXOCTETCOUNT_GB_LO);
 
 	stats->txframecount_gb +=
-		XGMAC_IOREAD(pdata, MMC_TXFRAMECOUNT_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TXFRAMECOUNT_GB_LO);
 
 	stats->txbroadcastframes_g +=
-		XGMAC_IOREAD(pdata, MMC_TXBROADCASTFRAMES_G_LO);
+		xgbe_mmc_read(pdata, MMC_TXBROADCASTFRAMES_G_LO);
 
 	stats->txmulticastframes_g +=
-		XGMAC_IOREAD(pdata, MMC_TXMULTICASTFRAMES_G_LO);
+		xgbe_mmc_read(pdata, MMC_TXMULTICASTFRAMES_G_LO);
 
 	stats->tx64octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_TX64OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TX64OCTETS_GB_LO);
 
 	stats->tx65to127octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_TX65TO127OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TX65TO127OCTETS_GB_LO);
 
 	stats->tx128to255octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_TX128TO255OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TX128TO255OCTETS_GB_LO);
 
 	stats->tx256to511octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_TX256TO511OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TX256TO511OCTETS_GB_LO);
 
 	stats->tx512to1023octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_TX512TO1023OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TX512TO1023OCTETS_GB_LO);
 
 	stats->tx1024tomaxoctets_gb +=
-		XGMAC_IOREAD(pdata, MMC_TX1024TOMAXOCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TX1024TOMAXOCTETS_GB_LO);
 
 	stats->txunicastframes_gb +=
-		XGMAC_IOREAD(pdata, MMC_TXUNICASTFRAMES_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TXUNICASTFRAMES_GB_LO);
 
 	stats->txmulticastframes_gb +=
-		XGMAC_IOREAD(pdata, MMC_TXMULTICASTFRAMES_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TXMULTICASTFRAMES_GB_LO);
 
 	stats->txbroadcastframes_g +=
-		XGMAC_IOREAD(pdata, MMC_TXBROADCASTFRAMES_GB_LO);
+		xgbe_mmc_read(pdata, MMC_TXBROADCASTFRAMES_GB_LO);
 
 	stats->txunderflowerror +=
-		XGMAC_IOREAD(pdata, MMC_TXUNDERFLOWERROR_LO);
+		xgbe_mmc_read(pdata, MMC_TXUNDERFLOWERROR_LO);
 
 	stats->txoctetcount_g +=
-		XGMAC_IOREAD(pdata, MMC_TXOCTETCOUNT_G_LO);
+		xgbe_mmc_read(pdata, MMC_TXOCTETCOUNT_G_LO);
 
 	stats->txframecount_g +=
-		XGMAC_IOREAD(pdata, MMC_TXFRAMECOUNT_G_LO);
+		xgbe_mmc_read(pdata, MMC_TXFRAMECOUNT_G_LO);
 
 	stats->txpauseframes +=
-		XGMAC_IOREAD(pdata, MMC_TXPAUSEFRAMES_LO);
+		xgbe_mmc_read(pdata, MMC_TXPAUSEFRAMES_LO);
 
 	stats->txvlanframes_g +=
-		XGMAC_IOREAD(pdata, MMC_TXVLANFRAMES_G_LO);
+		xgbe_mmc_read(pdata, MMC_TXVLANFRAMES_G_LO);
 
 	stats->rxframecount_gb +=
-		XGMAC_IOREAD(pdata, MMC_RXFRAMECOUNT_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RXFRAMECOUNT_GB_LO);
 
 	stats->rxoctetcount_gb +=
-		XGMAC_IOREAD(pdata, MMC_RXOCTETCOUNT_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RXOCTETCOUNT_GB_LO);
 
 	stats->rxoctetcount_g +=
-		XGMAC_IOREAD(pdata, MMC_RXOCTETCOUNT_G_LO);
+		xgbe_mmc_read(pdata, MMC_RXOCTETCOUNT_G_LO);
 
 	stats->rxbroadcastframes_g +=
-		XGMAC_IOREAD(pdata, MMC_RXBROADCASTFRAMES_G_LO);
+		xgbe_mmc_read(pdata, MMC_RXBROADCASTFRAMES_G_LO);
 
 	stats->rxmulticastframes_g +=
-		XGMAC_IOREAD(pdata, MMC_RXMULTICASTFRAMES_G_LO);
+		xgbe_mmc_read(pdata, MMC_RXMULTICASTFRAMES_G_LO);
 
 	stats->rxcrcerror +=
-		XGMAC_IOREAD(pdata, MMC_RXCRCERROR_LO);
+		xgbe_mmc_read(pdata, MMC_RXCRCERROR_LO);
 
 	stats->rxrunterror +=
-		XGMAC_IOREAD(pdata, MMC_RXRUNTERROR);
+		xgbe_mmc_read(pdata, MMC_RXRUNTERROR);
 
 	stats->rxjabbererror +=
-		XGMAC_IOREAD(pdata, MMC_RXJABBERERROR);
+		xgbe_mmc_read(pdata, MMC_RXJABBERERROR);
 
 	stats->rxundersize_g +=
-		XGMAC_IOREAD(pdata, MMC_RXUNDERSIZE_G);
+		xgbe_mmc_read(pdata, MMC_RXUNDERSIZE_G);
 
 	stats->rxoversize_g +=
-		XGMAC_IOREAD(pdata, MMC_RXOVERSIZE_G);
+		xgbe_mmc_read(pdata, MMC_RXOVERSIZE_G);
 
 	stats->rx64octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_RX64OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RX64OCTETS_GB_LO);
 
 	stats->rx65to127octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_RX65TO127OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RX65TO127OCTETS_GB_LO);
 
 	stats->rx128to255octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_RX128TO255OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RX128TO255OCTETS_GB_LO);
 
 	stats->rx256to511octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_RX256TO511OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RX256TO511OCTETS_GB_LO);
 
 	stats->rx512to1023octets_gb +=
-		XGMAC_IOREAD(pdata, MMC_RX512TO1023OCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RX512TO1023OCTETS_GB_LO);
 
 	stats->rx1024tomaxoctets_gb +=
-		XGMAC_IOREAD(pdata, MMC_RX1024TOMAXOCTETS_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RX1024TOMAXOCTETS_GB_LO);
 
 	stats->rxunicastframes_g +=
-		XGMAC_IOREAD(pdata, MMC_RXUNICASTFRAMES_G_LO);
+		xgbe_mmc_read(pdata, MMC_RXUNICASTFRAMES_G_LO);
 
 	stats->rxlengtherror +=
-		XGMAC_IOREAD(pdata, MMC_RXLENGTHERROR_LO);
+		xgbe_mmc_read(pdata, MMC_RXLENGTHERROR_LO);
 
 	stats->rxoutofrangetype +=
-		XGMAC_IOREAD(pdata, MMC_RXOUTOFRANGETYPE_LO);
+		xgbe_mmc_read(pdata, MMC_RXOUTOFRANGETYPE_LO);
 
 	stats->rxpauseframes +=
-		XGMAC_IOREAD(pdata, MMC_RXPAUSEFRAMES_LO);
+		xgbe_mmc_read(pdata, MMC_RXPAUSEFRAMES_LO);
 
 	stats->rxfifooverflow +=
-		XGMAC_IOREAD(pdata, MMC_RXFIFOOVERFLOW_LO);
+		xgbe_mmc_read(pdata, MMC_RXFIFOOVERFLOW_LO);
 
 	stats->rxvlanframes_gb +=
-		XGMAC_IOREAD(pdata, MMC_RXVLANFRAMES_GB_LO);
+		xgbe_mmc_read(pdata, MMC_RXVLANFRAMES_GB_LO);
 
 	stats->rxwatchdogerror +=
-		XGMAC_IOREAD(pdata, MMC_RXWATCHDOGERROR);
+		xgbe_mmc_read(pdata, MMC_RXWATCHDOGERROR);
 
 	/* Un-freeze counters */
 	XGMAC_IOWRITE_BITS(pdata, MMC_CR, MCF, 0);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
index dc84f7193c2d..29554992215a 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-drv.c
@@ -126,7 +126,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 static int xgbe_poll(struct napi_struct *, int);
 static void xgbe_set_rx_mode(struct net_device *);
 
@@ -361,6 +360,8 @@ void xgbe_get_all_hw_features(struct xgbe_prv_data *pdata)
 
 	memset(hw_feat, 0, sizeof(*hw_feat));
 
+	hw_feat->version = XGMAC_IOREAD(pdata, MAC_VR);
+
 	/* Hardware feature register 0 */
 	hw_feat->gmii        = XGMAC_GET_BITS(mac_hfr0, MAC_HWF0R, GMIISEL);
 	hw_feat->vlhash      = XGMAC_GET_BITS(mac_hfr0, MAC_HWF0R, VLHASH);
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
index a076aca138a1..49508ec98b72 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ethtool.c
@@ -121,7 +121,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 struct xgbe_stats {
 	char stat_string[ETH_GSTRING_LEN];
 	int stat_size;
@@ -173,6 +172,7 @@ static const struct xgbe_stats xgbe_gstring_stats[] = {
 	XGMAC_MMC_STAT("rx_watchdog_errors", rxwatchdogerror),
 	XGMAC_MMC_STAT("rx_pause_frames", rxpauseframes),
 };
+
 #define XGBE_STATS_COUNT	ARRAY_SIZE(xgbe_gstring_stats)
 
 static void xgbe_get_strings(struct net_device *netdev, u32 stringset, u8 *data)
@@ -361,15 +361,16 @@ static void xgbe_get_drvinfo(struct net_device *netdev,
 			     struct ethtool_drvinfo *drvinfo)
 {
 	struct xgbe_prv_data *pdata = netdev_priv(netdev);
+	struct xgbe_hw_features *hw_feat = &pdata->hw_feat;
 
 	strlcpy(drvinfo->driver, XGBE_DRV_NAME, sizeof(drvinfo->driver));
 	strlcpy(drvinfo->version, XGBE_DRV_VERSION, sizeof(drvinfo->version));
 	strlcpy(drvinfo->bus_info, dev_name(pdata->dev),
 		sizeof(drvinfo->bus_info));
 	snprintf(drvinfo->fw_version, sizeof(drvinfo->fw_version), "%d.%d.%d",
-		 XGMAC_IOREAD_BITS(pdata, MAC_VR, USERVER),
-		 XGMAC_IOREAD_BITS(pdata, MAC_VR, DEVID),
-		 XGMAC_IOREAD_BITS(pdata, MAC_VR, SNPSVER));
+		 XGMAC_GET_BITS(hw_feat->version, MAC_VR, USERVER),
+		 XGMAC_GET_BITS(hw_feat->version, MAC_VR, DEVID),
+		 XGMAC_GET_BITS(hw_feat->version, MAC_VR, SNPSVER));
 	drvinfo->n_stats = XGBE_STATS_COUNT;
 }
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-main.c b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
index 8aa6a9353f7b..f5a8fa03921a 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-main.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-main.c
@@ -128,7 +128,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 MODULE_AUTHOR("Tom Lendacky <thomas.lendacky@amd.com>");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(XGBE_DRV_VERSION);
@@ -172,7 +171,7 @@ static struct xgbe_channel *xgbe_alloc_rings(struct xgbe_prv_data *pdata)
 		}
 
 		if (i < pdata->rx_ring_count) {
-			spin_lock_init(&tx_ring->lock);
+			spin_lock_init(&rx_ring->lock);
 			channel->rx_ring = rx_ring++;
 		}
 
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c b/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
index 6d2221e023f4..363b210560f3 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-mdio.c
@@ -123,7 +123,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 static int xgbe_mdio_read(struct mii_bus *mii, int prtad, int mmd_reg)
 {
 	struct xgbe_prv_data *pdata = mii->priv;
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
index 37e64cfa5718..a1bf9d1cdae1 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
+++ b/drivers/net/ethernet/amd/xgbe/xgbe-ptp.c
@@ -122,7 +122,6 @@
 #include "xgbe.h"
 #include "xgbe-common.h"
 
-
 static cycle_t xgbe_cc_read(const struct cyclecounter *cc)
 {
 	struct xgbe_prv_data *pdata = container_of(cc,
diff --git a/drivers/net/ethernet/amd/xgbe/xgbe.h b/drivers/net/ethernet/amd/xgbe/xgbe.h
index 07bf70a82908..789957d43a13 100644
--- a/drivers/net/ethernet/amd/xgbe/xgbe.h
+++ b/drivers/net/ethernet/amd/xgbe/xgbe.h
@@ -128,7 +128,6 @@
 #include <linux/net_tstamp.h>
 #include <net/dcbnl.h>
 
-
 #define XGBE_DRV_NAME		"amd-xgbe"
 #define XGBE_DRV_VERSION	"1.0.0-a"
 #define XGBE_DRV_DESC		"AMD 10 Gigabit Ethernet Driver"
@@ -183,6 +182,7 @@
 #define XGMAC_DRIVER_CONTEXT	1
 #define XGMAC_IOCTL_CONTEXT	2
 
+#define XGBE_FIFO_MAX		81920
 #define XGBE_FIFO_SIZE_B(x)	(x)
 #define XGBE_FIFO_SIZE_KB(x)	(x * 1024)
 
@@ -198,7 +198,6 @@
 	((_ring)->rdata +					\
 	 ((_idx) & ((_ring)->rdesc_count - 1)))
 
-
 /* Default coalescing parameters */
 #define XGMAC_INIT_DMA_TX_USECS		50
 #define XGMAC_INIT_DMA_TX_FRAMES	25
@@ -526,6 +525,9 @@ struct xgbe_desc_if {
  * or configurations are present in the device.
  */
 struct xgbe_hw_features {
+	/* HW Version */
+	unsigned int version;
+
 	/* HW Feature Register0 */
 	unsigned int gmii;		/* 1000 Mbps support */
 	unsigned int vlhash;		/* VLAN Hash Filter */
diff --git a/drivers/net/ethernet/apm/xgene/Kconfig b/drivers/net/ethernet/apm/xgene/Kconfig
index 616dff6d3f5f..f4054d242f3c 100644
--- a/drivers/net/ethernet/apm/xgene/Kconfig
+++ b/drivers/net/ethernet/apm/xgene/Kconfig
@@ -1,5 +1,6 @@
 config NET_XGENE
 	tristate "APM X-Gene SoC Ethernet Driver"
+	depends on HAS_DMA
 	select PHYLIB
 	help
 	  This is the Ethernet driver for the on-chip ethernet interface on the
diff --git a/drivers/net/ethernet/apm/xgene/Makefile b/drivers/net/ethernet/apm/xgene/Makefile
index c643e8a0a0dc..68be565548c0 100644
--- a/drivers/net/ethernet/apm/xgene/Makefile
+++ b/drivers/net/ethernet/apm/xgene/Makefile
@@ -2,5 +2,6 @@
 # Makefile for APM X-Gene Ethernet Driver.
 #
 
-xgene-enet-objs := xgene_enet_hw.o xgene_enet_main.o xgene_enet_ethtool.o
+xgene-enet-objs := xgene_enet_hw.o xgene_enet_sgmac.o xgene_enet_xgmac.o \
+		   xgene_enet_main.o xgene_enet_ethtool.o
 obj-$(CONFIG_NET_XGENE) += xgene-enet.o
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c b/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
index 63f2aa54a594..416d6ebfc2ce 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_ethtool.c
@@ -59,10 +59,31 @@ static int xgene_get_settings(struct net_device *ndev, struct ethtool_cmd *cmd)
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
 	struct phy_device *phydev = pdata->phy_dev;
 
-	if (phydev == NULL)
-		return -ENODEV;
+	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII) {
+		if (phydev == NULL)
+			return -ENODEV;
+
+		return phy_ethtool_gset(phydev, cmd);
+	} else if (pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
+		cmd->supported = SUPPORTED_1000baseT_Full |
+				 SUPPORTED_Autoneg | SUPPORTED_MII;
+		cmd->advertising = cmd->supported;
+		ethtool_cmd_speed_set(cmd, SPEED_1000);
+		cmd->duplex = DUPLEX_FULL;
+		cmd->port = PORT_MII;
+		cmd->transceiver = XCVR_INTERNAL;
+		cmd->autoneg = AUTONEG_ENABLE;
+	} else {
+		cmd->supported = SUPPORTED_10000baseT_Full | SUPPORTED_FIBRE;
+		cmd->advertising = cmd->supported;
+		ethtool_cmd_speed_set(cmd, SPEED_10000);
+		cmd->duplex = DUPLEX_FULL;
+		cmd->port = PORT_FIBRE;
+		cmd->transceiver = XCVR_INTERNAL;
+		cmd->autoneg = AUTONEG_DISABLE;
+	}
 
-	return phy_ethtool_gset(phydev, cmd);
+	return 0;
 }
 
 static int xgene_set_settings(struct net_device *ndev, struct ethtool_cmd *cmd)
@@ -70,10 +91,14 @@ static int xgene_set_settings(struct net_device *ndev, struct ethtool_cmd *cmd)
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
 	struct phy_device *phydev = pdata->phy_dev;
 
-	if (phydev == NULL)
-		return -ENODEV;
+	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII) {
+		if (phydev == NULL)
+			return -ENODEV;
+
+		return phy_ethtool_sset(phydev, cmd);
+	}
 
-	return phy_ethtool_sset(phydev, cmd);
+	return -EINVAL;
 }
 
 static void xgene_get_strings(struct net_device *ndev, u32 stringset, u8 *data)
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
index 812d8d65159b..63ea1941e973 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.c
@@ -402,7 +402,7 @@ static int xgene_mii_phy_read(struct xgene_enet_pdata *pdata,
 	return data;
 }
 
-void xgene_gmac_set_mac_addr(struct xgene_enet_pdata *pdata)
+static void xgene_gmac_set_mac_addr(struct xgene_enet_pdata *pdata)
 {
 	u32 addr0, addr1;
 	u8 *dev_addr = pdata->ndev->dev_addr;
@@ -410,7 +410,6 @@ void xgene_gmac_set_mac_addr(struct xgene_enet_pdata *pdata)
 	addr0 = (dev_addr[3] << 24) | (dev_addr[2] << 16) |
 		(dev_addr[1] << 8) | dev_addr[0];
 	addr1 = (dev_addr[5] << 24) | (dev_addr[4] << 16);
-	addr1 |= pdata->phy_addr & 0xFFFF;
 
 	xgene_enet_wr_mcx_mac(pdata, STATION_ADDR0_ADDR, addr0);
 	xgene_enet_wr_mcx_mac(pdata, STATION_ADDR1_ADDR, addr1);
@@ -436,13 +435,13 @@ static int xgene_enet_ecc_init(struct xgene_enet_pdata *pdata)
 	return 0;
 }
 
-void xgene_gmac_reset(struct xgene_enet_pdata *pdata)
+static void xgene_gmac_reset(struct xgene_enet_pdata *pdata)
 {
 	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, SOFT_RESET1);
 	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, 0);
 }
 
-void xgene_gmac_init(struct xgene_enet_pdata *pdata, int speed)
+static void xgene_gmac_init(struct xgene_enet_pdata *pdata)
 {
 	u32 value, mc2;
 	u32 intf_ctl, rgmii;
@@ -456,7 +455,7 @@ void xgene_gmac_init(struct xgene_enet_pdata *pdata, int speed)
 	xgene_enet_rd_mcx_mac(pdata, INTERFACE_CONTROL_ADDR, &intf_ctl);
 	xgene_enet_rd_csr(pdata, RGMII_REG_0_ADDR, &rgmii);
 
-	switch (speed) {
+	switch (pdata->phy_speed) {
 	case SPEED_10:
 		ENET_INTERFACE_MODE2_SET(&mc2, 1);
 		CFG_MACMODE_SET(&icm0, 0);
@@ -525,8 +524,8 @@ static void xgene_enet_config_ring_if_assoc(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIQMLITEFPQASSOC_ADDR, val);
 }
 
-void xgene_enet_cle_bypass(struct xgene_enet_pdata *pdata,
-			   u32 dst_ring_num, u16 bufpool_id)
+static void xgene_enet_cle_bypass(struct xgene_enet_pdata *pdata,
+				  u32 dst_ring_num, u16 bufpool_id)
 {
 	u32 cb;
 	u32 fpsel;
@@ -544,7 +543,7 @@ void xgene_enet_cle_bypass(struct xgene_enet_pdata *pdata,
 	xgene_enet_wr_csr(pdata, CLE_BYPASS_REG1_0_ADDR, cb);
 }
 
-void xgene_gmac_rx_enable(struct xgene_enet_pdata *pdata)
+static void xgene_gmac_rx_enable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
@@ -552,7 +551,7 @@ void xgene_gmac_rx_enable(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data | RX_EN);
 }
 
-void xgene_gmac_tx_enable(struct xgene_enet_pdata *pdata)
+static void xgene_gmac_tx_enable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
@@ -560,7 +559,7 @@ void xgene_gmac_tx_enable(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data | TX_EN);
 }
 
-void xgene_gmac_rx_disable(struct xgene_enet_pdata *pdata)
+static void xgene_gmac_rx_disable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
@@ -568,7 +567,7 @@ void xgene_gmac_rx_disable(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data & ~RX_EN);
 }
 
-void xgene_gmac_tx_disable(struct xgene_enet_pdata *pdata)
+static void xgene_gmac_tx_disable(struct xgene_enet_pdata *pdata)
 {
 	u32 data;
 
@@ -576,7 +575,7 @@ void xgene_gmac_tx_disable(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mcx_mac(pdata, MAC_CONFIG_1_ADDR, data & ~TX_EN);
 }
 
-void xgene_enet_reset(struct xgene_enet_pdata *pdata)
+static void xgene_enet_reset(struct xgene_enet_pdata *pdata)
 {
 	u32 val;
 
@@ -593,7 +592,7 @@ void xgene_enet_reset(struct xgene_enet_pdata *pdata)
 	xgene_enet_wr_mcx_mac(pdata, MII_MGMT_CONFIG_ADDR, val);
 }
 
-void xgene_gport_shutdown(struct xgene_enet_pdata *pdata)
+static void xgene_gport_shutdown(struct xgene_enet_pdata *pdata)
 {
 	clk_disable_unprepare(pdata->clk);
 }
@@ -627,10 +626,10 @@ static void xgene_enet_adjust_link(struct net_device *ndev)
 
 	if (phydev->link) {
 		if (pdata->phy_speed != phydev->speed) {
-			xgene_gmac_init(pdata, phydev->speed);
+			pdata->phy_speed = phydev->speed;
+			xgene_gmac_init(pdata);
 			xgene_gmac_rx_enable(pdata);
 			xgene_gmac_tx_enable(pdata);
-			pdata->phy_speed = phydev->speed;
 			phy_print_status(phydev);
 		}
 	} else {
@@ -726,3 +725,19 @@ void xgene_enet_mdio_remove(struct xgene_enet_pdata *pdata)
 	mdiobus_free(pdata->mdio_bus);
 	pdata->mdio_bus = NULL;
 }
+
+struct xgene_mac_ops xgene_gmac_ops = {
+	.init = xgene_gmac_init,
+	.reset = xgene_gmac_reset,
+	.rx_enable = xgene_gmac_rx_enable,
+	.tx_enable = xgene_gmac_tx_enable,
+	.rx_disable = xgene_gmac_rx_disable,
+	.tx_disable = xgene_gmac_tx_disable,
+	.set_mac_addr = xgene_gmac_set_mac_addr,
+};
+
+struct xgene_port_ops xgene_gport_ops = {
+	.reset = xgene_enet_reset,
+	.cle_bypass = xgene_enet_cle_bypass,
+	.shutdown = xgene_gport_shutdown,
+};
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
index 371e7a5b2507..38558584080e 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_hw.h
@@ -42,6 +42,12 @@ static inline u32 xgene_get_bits(u32 val, u32 start, u32 end)
 	return (val & GENMASK(end, start)) >> start;
 }
 
+enum xgene_enet_rm {
+	RM0,
+	RM1,
+	RM3 = 3
+};
+
 #define CSR_RING_ID		0x0008
 #define OVERWRITE		BIT(31)
 #define IS_BUFFER_POOL		BIT(20)
@@ -52,7 +58,6 @@ static inline u32 xgene_get_bits(u32 val, u32 start, u32 end)
 #define CSR_RING_WR_BASE	0x0070
 #define NUM_RING_CONFIG		5
 #define BUFPOOL_MODE		3
-#define RM3			3
 #define INC_DEC_CMD_ADDR	0x002c
 #define UDP_HDR_SIZE		2
 #define BUF_LEN_CODE_2K		0x5000
@@ -94,11 +99,9 @@ static inline u32 xgene_get_bits(u32 val, u32 start, u32 end)
 
 #define BLOCK_ETH_CSR_OFFSET		0x2000
 #define BLOCK_ETH_RING_IF_OFFSET	0x9000
-#define BLOCK_ETH_CLKRST_CSR_OFFSET	0xC000
 #define BLOCK_ETH_DIAG_CSR_OFFSET	0xD000
 
 #define BLOCK_ETH_MAC_OFFSET		0x0000
-#define BLOCK_ETH_STATS_OFFSET		0x0014
 #define BLOCK_ETH_MAC_CSR_OFFSET	0x2800
 
 #define MAC_ADDR_REG_OFFSET		0x00
@@ -107,12 +110,6 @@ static inline u32 xgene_get_bits(u32 val, u32 start, u32 end)
 #define MAC_READ_REG_OFFSET		0x0c
 #define MAC_COMMAND_DONE_REG_OFFSET	0x10
 
-#define STAT_ADDR_REG_OFFSET		0x00
-#define STAT_COMMAND_REG_OFFSET		0x04
-#define STAT_WRITE_REG_OFFSET		0x08
-#define STAT_READ_REG_OFFSET		0x0c
-#define STAT_COMMAND_DONE_REG_OFFSET	0x10
-
 #define MII_MGMT_CONFIG_ADDR		0x20
 #define MII_MGMT_COMMAND_ADDR		0x24
 #define MII_MGMT_ADDRESS_ADDR		0x28
@@ -147,6 +144,8 @@ static inline u32 xgene_get_bits(u32 val, u32 start, u32 end)
 #define CFG_CLE_FPSEL0_SET(dst, val)		xgene_set_bits(dst, val, 16, 4)
 #define CFG_MACMODE_SET(dst, val)		xgene_set_bits(dst, val, 18, 2)
 #define CFG_WAITASYNCRD_SET(dst, val)		xgene_set_bits(dst, val, 0, 16)
+#define CFG_CLE_DSTQID0(val)		(val & GENMASK(11, 0))
+#define CFG_CLE_FPSEL0(val)		((val << 16) & GENMASK(19, 16))
 #define ICM_CONFIG0_REG_0_ADDR		0x0400
 #define ICM_CONFIG2_REG_0_ADDR		0x0410
 #define RX_DV_GATE_REG_0_ADDR		0x05fc
@@ -183,7 +182,6 @@ static inline u32 xgene_get_bits(u32 val, u32 start, u32 end)
 #define TUND_ADDR			0x4a
 
 #define TSO_IPPROTO_TCP			1
-#define	FULL_DUPLEX			2
 
 #define USERINFO_POS			0
 #define USERINFO_LEN			32
@@ -318,20 +316,10 @@ void xgene_enet_parse_error(struct xgene_enet_desc_ring *ring,
 			    struct xgene_enet_pdata *pdata,
 			    enum xgene_enet_err_code status);
 
-void xgene_enet_reset(struct xgene_enet_pdata *priv);
-void xgene_gmac_reset(struct xgene_enet_pdata *priv);
-void xgene_gmac_init(struct xgene_enet_pdata *priv, int speed);
-void xgene_gmac_tx_enable(struct xgene_enet_pdata *priv);
-void xgene_gmac_rx_enable(struct xgene_enet_pdata *priv);
-void xgene_gmac_tx_disable(struct xgene_enet_pdata *priv);
-void xgene_gmac_rx_disable(struct xgene_enet_pdata *priv);
-void xgene_gmac_set_mac_addr(struct xgene_enet_pdata *pdata);
-void xgene_enet_cle_bypass(struct xgene_enet_pdata *pdata,
-			   u32 dst_ring_num, u16 bufpool_id);
-void xgene_gport_shutdown(struct xgene_enet_pdata *priv);
-void xgene_gmac_get_tx_stats(struct xgene_enet_pdata *pdata);
-
 int xgene_enet_mdio_config(struct xgene_enet_pdata *pdata);
 void xgene_enet_mdio_remove(struct xgene_enet_pdata *pdata);
 
+extern struct xgene_mac_ops xgene_gmac_ops;
+extern struct xgene_port_ops xgene_gport_ops;
+
 #endif /* __XGENE_ENET_HW_H__ */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
index e1a8f4e19983..3c208cc6f6bb 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.c
@@ -21,6 +21,8 @@
 
 #include "xgene_enet_main.h"
 #include "xgene_enet_hw.h"
+#include "xgene_enet_sgmac.h"
+#include "xgene_enet_xgmac.h"
 
 static void xgene_enet_init_bufpool(struct xgene_enet_desc_ring *buf_pool)
 {
@@ -390,7 +392,7 @@ static int xgene_enet_process_ring(struct xgene_enet_desc_ring *ring,
 		}
 	}
 
-	return budget;
+	return count;
 }
 
 static int xgene_enet_napi(struct napi_struct *napi, const int budget)
@@ -413,7 +415,7 @@ static void xgene_enet_timeout(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
 
-	xgene_gmac_reset(pdata);
+	pdata->mac_ops->reset(pdata);
 }
 
 static int xgene_enet_register_irq(struct net_device *ndev)
@@ -445,18 +447,21 @@ static void xgene_enet_free_irq(struct net_device *ndev)
 static int xgene_enet_open(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
+	struct xgene_mac_ops *mac_ops = pdata->mac_ops;
 	int ret;
 
-	xgene_gmac_tx_enable(pdata);
-	xgene_gmac_rx_enable(pdata);
+	mac_ops->tx_enable(pdata);
+	mac_ops->rx_enable(pdata);
 
 	ret = xgene_enet_register_irq(ndev);
 	if (ret)
 		return ret;
 	napi_enable(&pdata->rx_ring->napi);
 
-	if (pdata->phy_dev)
+	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
 		phy_start(pdata->phy_dev);
+	else
+		schedule_delayed_work(&pdata->link_work, PHY_POLL_LINK_OFF);
 
 	netif_start_queue(ndev);
 
@@ -466,18 +471,21 @@ static int xgene_enet_open(struct net_device *ndev)
 static int xgene_enet_close(struct net_device *ndev)
 {
 	struct xgene_enet_pdata *pdata = netdev_priv(ndev);
+	struct xgene_mac_ops *mac_ops = pdata->mac_ops;
 
 	netif_stop_queue(ndev);
 
-	if (pdata->phy_dev)
+	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
 		phy_stop(pdata->phy_dev);
+	else
+		cancel_delayed_work_sync(&pdata->link_work);
 
 	napi_disable(&pdata->rx_ring->napi);
 	xgene_enet_free_irq(ndev);
 	xgene_enet_process_ring(pdata->rx_ring, -1);
 
-	xgene_gmac_tx_disable(pdata);
-	xgene_gmac_rx_disable(pdata);
+	mac_ops->tx_disable(pdata);
+	mac_ops->rx_disable(pdata);
 
 	return 0;
 }
@@ -563,15 +571,21 @@ static void xgene_enet_free_desc_rings(struct xgene_enet_pdata *pdata)
 	struct xgene_enet_desc_ring *ring;
 
 	ring = pdata->tx_ring;
-	if (ring && ring->cp_ring && ring->cp_ring->cp_skb)
-		devm_kfree(dev, ring->cp_ring->cp_skb);
-	xgene_enet_free_desc_ring(ring);
+	if (ring) {
+		if (ring->cp_ring && ring->cp_ring->cp_skb)
+			devm_kfree(dev, ring->cp_ring->cp_skb);
+		xgene_enet_free_desc_ring(ring);
+	}
 
 	ring = pdata->rx_ring;
-	if (ring && ring->buf_pool && ring->buf_pool->rx_skb)
-		devm_kfree(dev, ring->buf_pool->rx_skb);
-	xgene_enet_free_desc_ring(ring->buf_pool);
-	xgene_enet_free_desc_ring(ring);
+	if (ring) {
+		if (ring->buf_pool) {
+			if (ring->buf_pool->rx_skb)
+				devm_kfree(dev, ring->buf_pool->rx_skb);
+			xgene_enet_free_desc_ring(ring->buf_pool);
+		}
+		xgene_enet_free_desc_ring(ring);
+	}
 }
 
 static struct xgene_enet_desc_ring *xgene_enet_create_desc_ring(
@@ -607,7 +621,6 @@ static struct xgene_enet_desc_ring *xgene_enet_create_desc_ring(
 
 	ring->cmd_base = pdata->ring_cmd_addr + (ring->num << 6);
 	ring->cmd = ring->cmd_base + INC_DEC_CMD_ADDR;
-	pdata->rm = RM3;
 	ring = xgene_enet_setup_ring(ring);
 	netdev_dbg(ndev, "ring info: num=%d  size=%d  id=%d  slots=%d\n",
 		   ring->num, ring->size, ring->id, ring->slots);
@@ -718,7 +731,7 @@ static int xgene_enet_set_mac_address(struct net_device *ndev, void *addr)
 	ret = eth_mac_addr(ndev, addr);
 	if (ret)
 		return ret;
-	xgene_gmac_set_mac_addr(pdata);
+	pdata->mac_ops->set_mac_addr(pdata);
 
 	return ret;
 }
@@ -797,8 +810,14 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 
 	pdata->phy_mode = of_get_phy_mode(pdev->dev.of_node);
 	if (pdata->phy_mode < 0) {
-		dev_err(dev, "Incorrect phy-connection-type in DTS\n");
-		return -EINVAL;
+		dev_err(dev, "Unable to get phy-connection-type\n");
+		return pdata->phy_mode;
+	}
+	if (pdata->phy_mode != PHY_INTERFACE_MODE_RGMII &&
+	    pdata->phy_mode != PHY_INTERFACE_MODE_SGMII &&
+	    pdata->phy_mode != PHY_INTERFACE_MODE_XGMII) {
+		dev_err(dev, "Incorrect phy-connection-type specified\n");
+		return -ENODEV;
 	}
 
 	pdata->clk = devm_clk_get(&pdev->dev, NULL);
@@ -813,12 +832,17 @@ static int xgene_enet_get_resources(struct xgene_enet_pdata *pdata)
 	pdata->eth_csr_addr = base_addr + BLOCK_ETH_CSR_OFFSET;
 	pdata->eth_ring_if_addr = base_addr + BLOCK_ETH_RING_IF_OFFSET;
 	pdata->eth_diag_csr_addr = base_addr + BLOCK_ETH_DIAG_CSR_OFFSET;
-	pdata->mcx_mac_addr = base_addr + BLOCK_ETH_MAC_OFFSET;
-	pdata->mcx_stats_addr = base_addr + BLOCK_ETH_STATS_OFFSET;
-	pdata->mcx_mac_csr_addr = base_addr + BLOCK_ETH_MAC_CSR_OFFSET;
+	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII ||
+	    pdata->phy_mode == PHY_INTERFACE_MODE_SGMII) {
+		pdata->mcx_mac_addr = base_addr + BLOCK_ETH_MAC_OFFSET;
+		pdata->mcx_mac_csr_addr = base_addr + BLOCK_ETH_MAC_CSR_OFFSET;
+	} else {
+		pdata->mcx_mac_addr = base_addr + BLOCK_AXG_MAC_OFFSET;
+		pdata->mcx_mac_csr_addr = base_addr + BLOCK_AXG_MAC_CSR_OFFSET;
+	}
 	pdata->rx_buff_cnt = NUM_PKT_BUF;
 
-	return ret;
+	return 0;
 }
 
 static int xgene_enet_init_hw(struct xgene_enet_pdata *pdata)
@@ -828,8 +852,7 @@ static int xgene_enet_init_hw(struct xgene_enet_pdata *pdata)
 	u16 dst_ring_num;
 	int ret;
 
-	xgene_gmac_tx_disable(pdata);
-	xgene_gmac_rx_disable(pdata);
+	pdata->port_ops->reset(pdata);
 
 	ret = xgene_enet_create_desc_rings(ndev);
 	if (ret) {
@@ -847,17 +870,40 @@ static int xgene_enet_init_hw(struct xgene_enet_pdata *pdata)
 	}
 
 	dst_ring_num = xgene_enet_dst_ring_num(pdata->rx_ring);
-	xgene_enet_cle_bypass(pdata, dst_ring_num, buf_pool->id);
+	pdata->port_ops->cle_bypass(pdata, dst_ring_num, buf_pool->id);
+	pdata->mac_ops->init(pdata);
 
 	return ret;
 }
 
+static void xgene_enet_setup_ops(struct xgene_enet_pdata *pdata)
+{
+	switch (pdata->phy_mode) {
+	case PHY_INTERFACE_MODE_RGMII:
+		pdata->mac_ops = &xgene_gmac_ops;
+		pdata->port_ops = &xgene_gport_ops;
+		pdata->rm = RM3;
+		break;
+	case PHY_INTERFACE_MODE_SGMII:
+		pdata->mac_ops = &xgene_sgmac_ops;
+		pdata->port_ops = &xgene_sgport_ops;
+		pdata->rm = RM1;
+		break;
+	default:
+		pdata->mac_ops = &xgene_xgmac_ops;
+		pdata->port_ops = &xgene_xgport_ops;
+		pdata->rm = RM0;
+		break;
+	}
+}
+
 static int xgene_enet_probe(struct platform_device *pdev)
 {
 	struct net_device *ndev;
 	struct xgene_enet_pdata *pdata;
 	struct device *dev = &pdev->dev;
 	struct napi_struct *napi;
+	struct xgene_mac_ops *mac_ops;
 	int ret;
 
 	ndev = alloc_etherdev(sizeof(struct xgene_enet_pdata));
@@ -880,8 +926,7 @@ static int xgene_enet_probe(struct platform_device *pdev)
 	if (ret)
 		goto err;
 
-	xgene_enet_reset(pdata);
-	xgene_gmac_init(pdata, SPEED_1000);
+	xgene_enet_setup_ops(pdata);
 
 	ret = register_netdev(ndev);
 	if (ret) {
@@ -901,7 +946,11 @@ static int xgene_enet_probe(struct platform_device *pdev)
 
 	napi = &pdata->rx_ring->napi;
 	netif_napi_add(ndev, napi, xgene_enet_napi, NAPI_POLL_WEIGHT);
-	ret = xgene_enet_mdio_config(pdata);
+	mac_ops = pdata->mac_ops;
+	if (pdata->phy_mode == PHY_INTERFACE_MODE_RGMII)
+		ret = xgene_enet_mdio_config(pdata);
+	else
+		INIT_DELAYED_WORK(&pdata->link_work, mac_ops->link_state);
 
 	return ret;
 err:
@@ -912,19 +961,21 @@ err:
 static int xgene_enet_remove(struct platform_device *pdev)
 {
 	struct xgene_enet_pdata *pdata;
+	struct xgene_mac_ops *mac_ops;
 	struct net_device *ndev;
 
 	pdata = platform_get_drvdata(pdev);
+	mac_ops = pdata->mac_ops;
 	ndev = pdata->ndev;
 
-	xgene_gmac_rx_disable(pdata);
-	xgene_gmac_tx_disable(pdata);
+	mac_ops->rx_disable(pdata);
+	mac_ops->tx_disable(pdata);
 
 	netif_napi_del(&pdata->rx_ring->napi);
 	xgene_enet_mdio_remove(pdata);
 	xgene_enet_delete_desc_rings(pdata);
 	unregister_netdev(ndev);
-	xgene_gport_shutdown(pdata);
+	pdata->port_ops->shutdown(pdata);
 	free_netdev(ndev);
 
 	return 0;
@@ -950,5 +1001,6 @@ module_platform_driver(xgene_enet_driver);
 
 MODULE_DESCRIPTION("APM X-Gene SoC Ethernet driver");
 MODULE_VERSION(XGENE_DRV_VERSION);
+MODULE_AUTHOR("Iyappan Subramanian <isubramanian@apm.com>");
 MODULE_AUTHOR("Keyur Chudgar <kchudgar@apm.com>");
 MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
index 0815866986b0..874e5a01161f 100644
--- a/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_main.h
@@ -39,6 +39,9 @@
 #define NUM_PKT_BUF	64
 #define NUM_BUFPOOL	32
 
+#define PHY_POLL_LINK_ON	(10 * HZ)
+#define PHY_POLL_LINK_OFF	(PHY_POLL_LINK_ON / 5)
+
 /* software context of a descriptor ring */
 struct xgene_enet_desc_ring {
 	struct net_device *ndev;
@@ -68,6 +71,24 @@ struct xgene_enet_desc_ring {
 	};
 };
 
+struct xgene_mac_ops {
+	void (*init)(struct xgene_enet_pdata *pdata);
+	void (*reset)(struct xgene_enet_pdata *pdata);
+	void (*tx_enable)(struct xgene_enet_pdata *pdata);
+	void (*rx_enable)(struct xgene_enet_pdata *pdata);
+	void (*tx_disable)(struct xgene_enet_pdata *pdata);
+	void (*rx_disable)(struct xgene_enet_pdata *pdata);
+	void (*set_mac_addr)(struct xgene_enet_pdata *pdata);
+	void (*link_state)(struct work_struct *work);
+};
+
+struct xgene_port_ops {
+	void (*reset)(struct xgene_enet_pdata *pdata);
+	void (*cle_bypass)(struct xgene_enet_pdata *pdata,
+			   u32 dst_ring_num, u16 bufpool_id);
+	void (*shutdown)(struct xgene_enet_pdata *pdata);
+};
+
 /* ethernet private data */
 struct xgene_enet_pdata {
 	struct net_device *ndev;
@@ -88,16 +109,23 @@ struct xgene_enet_pdata {
 	void __iomem *eth_ring_if_addr;
 	void __iomem *eth_diag_csr_addr;
 	void __iomem *mcx_mac_addr;
-	void __iomem *mcx_stats_addr;
 	void __iomem *mcx_mac_csr_addr;
 	void __iomem *base_addr;
 	void __iomem *ring_csr_addr;
 	void __iomem *ring_cmd_addr;
-	u32 phy_addr;
 	int phy_mode;
-	u32 speed;
-	u16 rm;
+	enum xgene_enet_rm rm;
 	struct rtnl_link_stats64 stats;
+	struct xgene_mac_ops *mac_ops;
+	struct xgene_port_ops *port_ops;
+	struct delayed_work link_work;
+};
+
+struct xgene_indirect_ctl {
+	void __iomem *addr;
+	void __iomem *ctl;
+	void __iomem *cmd;
+	void __iomem *cmd_done;
 };
 
 /* Set the specified value into a bit-field defined by its starting position
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
new file mode 100644
index 000000000000..e6d24c210198
--- /dev/null
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.c
@@ -0,0 +1,389 @@
+/* Applied Micro X-Gene SoC Ethernet Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Authors: Iyappan Subramanian <isubramanian@apm.com>
+ *	    Keyur Chudgar <kchudgar@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "xgene_enet_main.h"
+#include "xgene_enet_hw.h"
+#include "xgene_enet_sgmac.h"
+
+static void xgene_enet_wr_csr(struct xgene_enet_pdata *p, u32 offset, u32 val)
+{
+	iowrite32(val, p->eth_csr_addr + offset);
+}
+
+static void xgene_enet_wr_ring_if(struct xgene_enet_pdata *p,
+				  u32 offset, u32 val)
+{
+	iowrite32(val, p->eth_ring_if_addr + offset);
+}
+
+static void xgene_enet_wr_diag_csr(struct xgene_enet_pdata *p,
+				   u32 offset, u32 val)
+{
+	iowrite32(val, p->eth_diag_csr_addr + offset);
+}
+
+static bool xgene_enet_wr_indirect(struct xgene_indirect_ctl *ctl,
+				   u32 wr_addr, u32 wr_data)
+{
+	int i;
+
+	iowrite32(wr_addr, ctl->addr);
+	iowrite32(wr_data, ctl->ctl);
+	iowrite32(XGENE_ENET_WR_CMD, ctl->cmd);
+
+	/* wait for write command to complete */
+	for (i = 0; i < 10; i++) {
+		if (ioread32(ctl->cmd_done)) {
+			iowrite32(0, ctl->cmd);
+			return true;
+		}
+		udelay(1);
+	}
+
+	return false;
+}
+
+static void xgene_enet_wr_mac(struct xgene_enet_pdata *p,
+			      u32 wr_addr, u32 wr_data)
+{
+	struct xgene_indirect_ctl ctl = {
+		.addr = p->mcx_mac_addr + MAC_ADDR_REG_OFFSET,
+		.ctl = p->mcx_mac_addr + MAC_WRITE_REG_OFFSET,
+		.cmd = p->mcx_mac_addr + MAC_COMMAND_REG_OFFSET,
+		.cmd_done = p->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET
+	};
+
+	if (!xgene_enet_wr_indirect(&ctl, wr_addr, wr_data))
+		netdev_err(p->ndev, "mac write failed, addr: %04x\n", wr_addr);
+}
+
+static u32 xgene_enet_rd_csr(struct xgene_enet_pdata *p, u32 offset)
+{
+	return ioread32(p->eth_csr_addr + offset);
+}
+
+static u32 xgene_enet_rd_diag_csr(struct xgene_enet_pdata *p, u32 offset)
+{
+	return ioread32(p->eth_diag_csr_addr + offset);
+}
+
+static u32 xgene_enet_rd_indirect(struct xgene_indirect_ctl *ctl, u32 rd_addr)
+{
+	u32 rd_data;
+	int i;
+
+	iowrite32(rd_addr, ctl->addr);
+	iowrite32(XGENE_ENET_RD_CMD, ctl->cmd);
+
+	/* wait for read command to complete */
+	for (i = 0; i < 10; i++) {
+		if (ioread32(ctl->cmd_done)) {
+			rd_data = ioread32(ctl->ctl);
+			iowrite32(0, ctl->cmd);
+
+			return rd_data;
+		}
+		udelay(1);
+	}
+
+	pr_err("%s: mac read failed, addr: %04x\n", __func__, rd_addr);
+
+	return 0;
+}
+
+static u32 xgene_enet_rd_mac(struct xgene_enet_pdata *p, u32 rd_addr)
+{
+	struct xgene_indirect_ctl ctl = {
+		.addr = p->mcx_mac_addr + MAC_ADDR_REG_OFFSET,
+		.ctl = p->mcx_mac_addr + MAC_READ_REG_OFFSET,
+		.cmd = p->mcx_mac_addr + MAC_COMMAND_REG_OFFSET,
+		.cmd_done = p->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET
+	};
+
+	return xgene_enet_rd_indirect(&ctl, rd_addr);
+}
+
+static int xgene_enet_ecc_init(struct xgene_enet_pdata *p)
+{
+	struct net_device *ndev = p->ndev;
+	u32 data;
+	int i;
+
+	xgene_enet_wr_diag_csr(p, ENET_CFG_MEM_RAM_SHUTDOWN_ADDR, 0);
+	for (i = 0; i < 10 && data != ~0U ; i++) {
+		usleep_range(100, 110);
+		data = xgene_enet_rd_diag_csr(p, ENET_BLOCK_MEM_RDY_ADDR);
+	}
+
+	if (data != ~0U) {
+		netdev_err(ndev, "Failed to release memory from shutdown\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void xgene_enet_config_ring_if_assoc(struct xgene_enet_pdata *p)
+{
+	u32 val = 0xffffffff;
+
+	xgene_enet_wr_ring_if(p, ENET_CFGSSQMIWQASSOC_ADDR, val);
+	xgene_enet_wr_ring_if(p, ENET_CFGSSQMIFPQASSOC_ADDR, val);
+}
+
+static void xgene_mii_phy_write(struct xgene_enet_pdata *p, u8 phy_id,
+				u32 reg, u16 data)
+{
+	u32 addr, wr_data, done;
+	int i;
+
+	addr = PHY_ADDR(phy_id) | REG_ADDR(reg);
+	xgene_enet_wr_mac(p, MII_MGMT_ADDRESS_ADDR, addr);
+
+	wr_data = PHY_CONTROL(data);
+	xgene_enet_wr_mac(p, MII_MGMT_CONTROL_ADDR, wr_data);
+
+	for (i = 0; i < 10; i++) {
+		done = xgene_enet_rd_mac(p, MII_MGMT_INDICATORS_ADDR);
+		if (!(done & BUSY_MASK))
+			return;
+		usleep_range(10, 20);
+	}
+
+	netdev_err(p->ndev, "MII_MGMT write failed\n");
+}
+
+static u32 xgene_mii_phy_read(struct xgene_enet_pdata *p, u8 phy_id, u32 reg)
+{
+	u32 addr, data, done;
+	int i;
+
+	addr = PHY_ADDR(phy_id) | REG_ADDR(reg);
+	xgene_enet_wr_mac(p, MII_MGMT_ADDRESS_ADDR, addr);
+	xgene_enet_wr_mac(p, MII_MGMT_COMMAND_ADDR, READ_CYCLE_MASK);
+
+	for (i = 0; i < 10; i++) {
+		done = xgene_enet_rd_mac(p, MII_MGMT_INDICATORS_ADDR);
+		if (!(done & BUSY_MASK)) {
+			data = xgene_enet_rd_mac(p, MII_MGMT_STATUS_ADDR);
+			xgene_enet_wr_mac(p, MII_MGMT_COMMAND_ADDR, 0);
+
+			return data;
+		}
+		usleep_range(10, 20);
+	}
+
+	netdev_err(p->ndev, "MII_MGMT read failed\n");
+
+	return 0;
+}
+
+static void xgene_sgmac_reset(struct xgene_enet_pdata *p)
+{
+	xgene_enet_wr_mac(p, MAC_CONFIG_1_ADDR, SOFT_RESET1);
+	xgene_enet_wr_mac(p, MAC_CONFIG_1_ADDR, 0);
+}
+
+static void xgene_sgmac_set_mac_addr(struct xgene_enet_pdata *p)
+{
+	u32 addr0, addr1;
+	u8 *dev_addr = p->ndev->dev_addr;
+
+	addr0 = (dev_addr[3] << 24) | (dev_addr[2] << 16) |
+		(dev_addr[1] << 8) | dev_addr[0];
+	xgene_enet_wr_mac(p, STATION_ADDR0_ADDR, addr0);
+
+	addr1 = xgene_enet_rd_mac(p, STATION_ADDR1_ADDR);
+	addr1 |= (dev_addr[5] << 24) | (dev_addr[4] << 16);
+	xgene_enet_wr_mac(p, STATION_ADDR1_ADDR, addr1);
+}
+
+static u32 xgene_enet_link_status(struct xgene_enet_pdata *p)
+{
+	u32 data;
+
+	data = xgene_mii_phy_read(p, INT_PHY_ADDR,
+				  SGMII_BASE_PAGE_ABILITY_ADDR >> 2);
+
+	return data & LINK_UP;
+}
+
+static void xgene_sgmac_init(struct xgene_enet_pdata *p)
+{
+	u32 data, loop = 10;
+
+	xgene_sgmac_reset(p);
+
+	/* Enable auto-negotiation */
+	xgene_mii_phy_write(p, INT_PHY_ADDR, SGMII_CONTROL_ADDR >> 2, 0x1000);
+	xgene_mii_phy_write(p, INT_PHY_ADDR, SGMII_TBI_CONTROL_ADDR >> 2, 0);
+
+	while (loop--) {
+		data = xgene_mii_phy_read(p, INT_PHY_ADDR,
+					  SGMII_STATUS_ADDR >> 2);
+		if ((data & AUTO_NEG_COMPLETE) && (data & LINK_STATUS))
+			break;
+		usleep_range(10, 20);
+	}
+	if (!(data & AUTO_NEG_COMPLETE) || !(data & LINK_STATUS))
+		netdev_err(p->ndev, "Auto-negotiation failed\n");
+
+	data = xgene_enet_rd_mac(p, MAC_CONFIG_2_ADDR);
+	ENET_INTERFACE_MODE2_SET(&data, 2);
+	xgene_enet_wr_mac(p, MAC_CONFIG_2_ADDR, data | FULL_DUPLEX2);
+	xgene_enet_wr_mac(p, INTERFACE_CONTROL_ADDR, ENET_GHD_MODE);
+
+	data = xgene_enet_rd_csr(p, ENET_SPARE_CFG_REG_ADDR);
+	data |= MPA_IDLE_WITH_QMI_EMPTY;
+	xgene_enet_wr_csr(p, ENET_SPARE_CFG_REG_ADDR, data);
+
+	xgene_sgmac_set_mac_addr(p);
+
+	data = xgene_enet_rd_csr(p, DEBUG_REG_ADDR);
+	data |= CFG_BYPASS_UNISEC_TX | CFG_BYPASS_UNISEC_RX;
+	xgene_enet_wr_csr(p, DEBUG_REG_ADDR, data);
+
+	/* Adjust MDC clock frequency */
+	data = xgene_enet_rd_mac(p, MII_MGMT_CONFIG_ADDR);
+	MGMT_CLOCK_SEL_SET(&data, 7);
+	xgene_enet_wr_mac(p, MII_MGMT_CONFIG_ADDR, data);
+
+	/* Enable drop if bufpool not available */
+	data = xgene_enet_rd_csr(p, RSIF_CONFIG_REG_ADDR);
+	data |= CFG_RSIF_FPBUFF_TIMEOUT_EN;
+	xgene_enet_wr_csr(p, RSIF_CONFIG_REG_ADDR, data);
+
+	/* Rtype should be copied from FP */
+	xgene_enet_wr_csr(p, RSIF_RAM_DBG_REG0_ADDR, 0);
+
+	/* Bypass traffic gating */
+	xgene_enet_wr_csr(p, CFG_LINK_AGGR_RESUME_0_ADDR, TX_PORT0);
+	xgene_enet_wr_csr(p, CFG_BYPASS_ADDR, RESUME_TX);
+	xgene_enet_wr_csr(p, SG_RX_DV_GATE_REG_0_ADDR, RESUME_RX0);
+}
+
+static void xgene_sgmac_rxtx(struct xgene_enet_pdata *p, u32 bits, bool set)
+{
+	u32 data;
+
+	data = xgene_enet_rd_mac(p, MAC_CONFIG_1_ADDR);
+
+	if (set)
+		data |= bits;
+	else
+		data &= ~bits;
+
+	xgene_enet_wr_mac(p, MAC_CONFIG_1_ADDR, data);
+}
+
+static void xgene_sgmac_rx_enable(struct xgene_enet_pdata *p)
+{
+	xgene_sgmac_rxtx(p, RX_EN, true);
+}
+
+static void xgene_sgmac_tx_enable(struct xgene_enet_pdata *p)
+{
+	xgene_sgmac_rxtx(p, TX_EN, true);
+}
+
+static void xgene_sgmac_rx_disable(struct xgene_enet_pdata *p)
+{
+	xgene_sgmac_rxtx(p, RX_EN, false);
+}
+
+static void xgene_sgmac_tx_disable(struct xgene_enet_pdata *p)
+{
+	xgene_sgmac_rxtx(p, TX_EN, false);
+}
+
+static void xgene_enet_reset(struct xgene_enet_pdata *p)
+{
+	clk_prepare_enable(p->clk);
+	clk_disable_unprepare(p->clk);
+	clk_prepare_enable(p->clk);
+
+	xgene_enet_ecc_init(p);
+	xgene_enet_config_ring_if_assoc(p);
+}
+
+static void xgene_enet_cle_bypass(struct xgene_enet_pdata *p,
+				  u32 dst_ring_num, u16 bufpool_id)
+{
+	u32 data, fpsel;
+
+	data = CFG_CLE_BYPASS_EN0;
+	xgene_enet_wr_csr(p, CLE_BYPASS_REG0_0_ADDR, data);
+
+	fpsel = xgene_enet_ring_bufnum(bufpool_id) - 0x20;
+	data = CFG_CLE_DSTQID0(dst_ring_num) | CFG_CLE_FPSEL0(fpsel);
+	xgene_enet_wr_csr(p, CLE_BYPASS_REG1_0_ADDR, data);
+}
+
+static void xgene_enet_shutdown(struct xgene_enet_pdata *p)
+{
+	clk_disable_unprepare(p->clk);
+}
+
+static void xgene_enet_link_state(struct work_struct *work)
+{
+	struct xgene_enet_pdata *p = container_of(to_delayed_work(work),
+				     struct xgene_enet_pdata, link_work);
+	struct net_device *ndev = p->ndev;
+	u32 link, poll_interval;
+
+	link = xgene_enet_link_status(p);
+	if (link) {
+		if (!netif_carrier_ok(ndev)) {
+			netif_carrier_on(ndev);
+			xgene_sgmac_init(p);
+			xgene_sgmac_rx_enable(p);
+			xgene_sgmac_tx_enable(p);
+			netdev_info(ndev, "Link is Up - 1Gbps\n");
+		}
+		poll_interval = PHY_POLL_LINK_ON;
+	} else {
+		if (netif_carrier_ok(ndev)) {
+			xgene_sgmac_rx_disable(p);
+			xgene_sgmac_tx_disable(p);
+			netif_carrier_off(ndev);
+			netdev_info(ndev, "Link is Down\n");
+		}
+		poll_interval = PHY_POLL_LINK_OFF;
+	}
+
+	schedule_delayed_work(&p->link_work, poll_interval);
+}
+
+struct xgene_mac_ops xgene_sgmac_ops = {
+	.init		= xgene_sgmac_init,
+	.reset		= xgene_sgmac_reset,
+	.rx_enable	= xgene_sgmac_rx_enable,
+	.tx_enable	= xgene_sgmac_tx_enable,
+	.rx_disable	= xgene_sgmac_rx_disable,
+	.tx_disable	= xgene_sgmac_tx_disable,
+	.set_mac_addr	= xgene_sgmac_set_mac_addr,
+	.link_state	= xgene_enet_link_state
+};
+
+struct xgene_port_ops xgene_sgport_ops = {
+	.reset		= xgene_enet_reset,
+	.cle_bypass	= xgene_enet_cle_bypass,
+	.shutdown	= xgene_enet_shutdown
+};
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.h b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.h
new file mode 100644
index 000000000000..de432465009c
--- /dev/null
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_sgmac.h
@@ -0,0 +1,41 @@
+/* Applied Micro X-Gene SoC Ethernet Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Authors: Iyappan Subramanian <isubramanian@apm.com>
+ *	    Keyur Chudgar <kchudgar@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __XGENE_ENET_SGMAC_H__
+#define __XGENE_ENET_SGMAC_H__
+
+#define PHY_ADDR(src)		(((src)<<8) & GENMASK(12, 8))
+#define REG_ADDR(src)		((src) & GENMASK(4, 0))
+#define PHY_CONTROL(src)	((src) & GENMASK(15, 0))
+#define INT_PHY_ADDR			0x1e
+#define SGMII_TBI_CONTROL_ADDR		0x44
+#define SGMII_CONTROL_ADDR		0x00
+#define SGMII_STATUS_ADDR		0x04
+#define SGMII_BASE_PAGE_ABILITY_ADDR	0x14
+#define AUTO_NEG_COMPLETE		BIT(5)
+#define LINK_STATUS			BIT(2)
+#define LINK_UP				BIT(15)
+#define MPA_IDLE_WITH_QMI_EMPTY		BIT(12)
+#define SG_RX_DV_GATE_REG_0_ADDR	0x0dfc
+
+extern struct xgene_mac_ops xgene_sgmac_ops;
+extern struct xgene_port_ops xgene_sgport_ops;
+
+#endif  /* __XGENE_ENET_SGMAC_H__ */
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
new file mode 100644
index 000000000000..67d07206b3c7
--- /dev/null
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.c
@@ -0,0 +1,332 @@
+/* Applied Micro X-Gene SoC Ethernet Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Authors: Iyappan Subramanian <isubramanian@apm.com>
+ *	    Keyur Chudgar <kchudgar@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "xgene_enet_main.h"
+#include "xgene_enet_hw.h"
+#include "xgene_enet_xgmac.h"
+
+static void xgene_enet_wr_csr(struct xgene_enet_pdata *pdata,
+			      u32 offset, u32 val)
+{
+	void __iomem *addr = pdata->eth_csr_addr + offset;
+
+	iowrite32(val, addr);
+}
+
+static void xgene_enet_wr_ring_if(struct xgene_enet_pdata *pdata,
+				  u32 offset, u32 val)
+{
+	void __iomem *addr = pdata->eth_ring_if_addr + offset;
+
+	iowrite32(val, addr);
+}
+
+static void xgene_enet_wr_diag_csr(struct xgene_enet_pdata *pdata,
+				   u32 offset, u32 val)
+{
+	void __iomem *addr = pdata->eth_diag_csr_addr + offset;
+
+	iowrite32(val, addr);
+}
+
+static bool xgene_enet_wr_indirect(void __iomem *addr, void __iomem *wr,
+				   void __iomem *cmd, void __iomem *cmd_done,
+				   u32 wr_addr, u32 wr_data)
+{
+	u32 done;
+	u8 wait = 10;
+
+	iowrite32(wr_addr, addr);
+	iowrite32(wr_data, wr);
+	iowrite32(XGENE_ENET_WR_CMD, cmd);
+
+	/* wait for write command to complete */
+	while (!(done = ioread32(cmd_done)) && wait--)
+		udelay(1);
+
+	if (!done)
+		return false;
+
+	iowrite32(0, cmd);
+
+	return true;
+}
+
+static void xgene_enet_wr_mac(struct xgene_enet_pdata *pdata,
+			      u32 wr_addr, u32 wr_data)
+{
+	void __iomem *addr, *wr, *cmd, *cmd_done;
+
+	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
+	wr = pdata->mcx_mac_addr + MAC_WRITE_REG_OFFSET;
+	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
+	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
+
+	if (!xgene_enet_wr_indirect(addr, wr, cmd, cmd_done, wr_addr, wr_data))
+		netdev_err(pdata->ndev, "MCX mac write failed, addr: %04x\n",
+			   wr_addr);
+}
+
+static void xgene_enet_rd_csr(struct xgene_enet_pdata *pdata,
+			      u32 offset, u32 *val)
+{
+	void __iomem *addr = pdata->eth_csr_addr + offset;
+
+	*val = ioread32(addr);
+}
+
+static void xgene_enet_rd_diag_csr(struct xgene_enet_pdata *pdata,
+				   u32 offset, u32 *val)
+{
+	void __iomem *addr = pdata->eth_diag_csr_addr + offset;
+
+	*val = ioread32(addr);
+}
+
+static bool xgene_enet_rd_indirect(void __iomem *addr, void __iomem *rd,
+				   void __iomem *cmd, void __iomem *cmd_done,
+				   u32 rd_addr, u32 *rd_data)
+{
+	u32 done;
+	u8 wait = 10;
+
+	iowrite32(rd_addr, addr);
+	iowrite32(XGENE_ENET_RD_CMD, cmd);
+
+	/* wait for read command to complete */
+	while (!(done = ioread32(cmd_done)) && wait--)
+		udelay(1);
+
+	if (!done)
+		return false;
+
+	*rd_data = ioread32(rd);
+	iowrite32(0, cmd);
+
+	return true;
+}
+
+static void xgene_enet_rd_mac(struct xgene_enet_pdata *pdata,
+			      u32 rd_addr, u32 *rd_data)
+{
+	void __iomem *addr, *rd, *cmd, *cmd_done;
+
+	addr = pdata->mcx_mac_addr + MAC_ADDR_REG_OFFSET;
+	rd = pdata->mcx_mac_addr + MAC_READ_REG_OFFSET;
+	cmd = pdata->mcx_mac_addr + MAC_COMMAND_REG_OFFSET;
+	cmd_done = pdata->mcx_mac_addr + MAC_COMMAND_DONE_REG_OFFSET;
+
+	if (!xgene_enet_rd_indirect(addr, rd, cmd, cmd_done, rd_addr, rd_data))
+		netdev_err(pdata->ndev, "MCX mac read failed, addr: %04x\n",
+			   rd_addr);
+}
+
+static int xgene_enet_ecc_init(struct xgene_enet_pdata *pdata)
+{
+	struct net_device *ndev = pdata->ndev;
+	u32 data;
+	u8 wait = 10;
+
+	xgene_enet_wr_diag_csr(pdata, ENET_CFG_MEM_RAM_SHUTDOWN_ADDR, 0x0);
+	do {
+		usleep_range(100, 110);
+		xgene_enet_rd_diag_csr(pdata, ENET_BLOCK_MEM_RDY_ADDR, &data);
+	} while ((data != 0xffffffff) && wait--);
+
+	if (data != 0xffffffff) {
+		netdev_err(ndev, "Failed to release memory from shutdown\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static void xgene_enet_config_ring_if_assoc(struct xgene_enet_pdata *pdata)
+{
+	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIWQASSOC_ADDR, 0);
+	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIFPQASSOC_ADDR, 0);
+	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIQMLITEWQASSOC_ADDR, 0);
+	xgene_enet_wr_ring_if(pdata, ENET_CFGSSQMIQMLITEFPQASSOC_ADDR, 0);
+}
+
+static void xgene_xgmac_reset(struct xgene_enet_pdata *pdata)
+{
+	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_0, HSTMACRST);
+	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_0, 0);
+}
+
+static void xgene_xgmac_set_mac_addr(struct xgene_enet_pdata *pdata)
+{
+	u32 addr0, addr1;
+	u8 *dev_addr = pdata->ndev->dev_addr;
+
+	addr0 = (dev_addr[3] << 24) | (dev_addr[2] << 16) |
+		(dev_addr[1] << 8) | dev_addr[0];
+	addr1 = (dev_addr[5] << 24) | (dev_addr[4] << 16);
+
+	xgene_enet_wr_mac(pdata, HSTMACADR_LSW_ADDR, addr0);
+	xgene_enet_wr_mac(pdata, HSTMACADR_MSW_ADDR, addr1);
+}
+
+static u32 xgene_enet_link_status(struct xgene_enet_pdata *pdata)
+{
+	u32 data;
+
+	xgene_enet_rd_csr(pdata, XG_LINK_STATUS_ADDR, &data);
+
+	return data;
+}
+
+static void xgene_xgmac_init(struct xgene_enet_pdata *pdata)
+{
+	u32 data;
+
+	xgene_xgmac_reset(pdata);
+
+	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	data |= HSTPPEN;
+	data &= ~HSTLENCHK;
+	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data);
+
+	xgene_enet_wr_mac(pdata, HSTMAXFRAME_LENGTH_ADDR, 0x06000600);
+	xgene_xgmac_set_mac_addr(pdata);
+
+	xgene_enet_rd_csr(pdata, XG_RSIF_CONFIG_REG_ADDR, &data);
+	data |= CFG_RSIF_FPBUFF_TIMEOUT_EN;
+	xgene_enet_wr_csr(pdata, XG_RSIF_CONFIG_REG_ADDR, data);
+
+	xgene_enet_wr_csr(pdata, XG_CFG_BYPASS_ADDR, RESUME_TX);
+	xgene_enet_wr_csr(pdata, XGENET_RX_DV_GATE_REG_0_ADDR, 0);
+	xgene_enet_rd_csr(pdata, XG_ENET_SPARE_CFG_REG_ADDR, &data);
+	data |= BIT(12);
+	xgene_enet_wr_csr(pdata, XG_ENET_SPARE_CFG_REG_ADDR, data);
+	xgene_enet_wr_csr(pdata, XG_ENET_SPARE_CFG_REG_1_ADDR, 0x82);
+}
+
+static void xgene_xgmac_rx_enable(struct xgene_enet_pdata *pdata)
+{
+	u32 data;
+
+	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data | HSTRFEN);
+}
+
+static void xgene_xgmac_tx_enable(struct xgene_enet_pdata *pdata)
+{
+	u32 data;
+
+	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data | HSTTFEN);
+}
+
+static void xgene_xgmac_rx_disable(struct xgene_enet_pdata *pdata)
+{
+	u32 data;
+
+	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data & ~HSTRFEN);
+}
+
+static void xgene_xgmac_tx_disable(struct xgene_enet_pdata *pdata)
+{
+	u32 data;
+
+	xgene_enet_rd_mac(pdata, AXGMAC_CONFIG_1, &data);
+	xgene_enet_wr_mac(pdata, AXGMAC_CONFIG_1, data & ~HSTTFEN);
+}
+
+static void xgene_enet_reset(struct xgene_enet_pdata *pdata)
+{
+	clk_prepare_enable(pdata->clk);
+	clk_disable_unprepare(pdata->clk);
+	clk_prepare_enable(pdata->clk);
+
+	xgene_enet_ecc_init(pdata);
+	xgene_enet_config_ring_if_assoc(pdata);
+}
+
+static void xgene_enet_xgcle_bypass(struct xgene_enet_pdata *pdata,
+				    u32 dst_ring_num, u16 bufpool_id)
+{
+	u32 cb, fpsel;
+
+	xgene_enet_rd_csr(pdata, XCLE_BYPASS_REG0_ADDR, &cb);
+	cb |= CFG_CLE_BYPASS_EN0;
+	CFG_CLE_IP_PROTOCOL0_SET(&cb, 3);
+	xgene_enet_wr_csr(pdata, XCLE_BYPASS_REG0_ADDR, cb);
+
+	fpsel = xgene_enet_ring_bufnum(bufpool_id) - 0x20;
+	xgene_enet_rd_csr(pdata, XCLE_BYPASS_REG1_ADDR, &cb);
+	CFG_CLE_DSTQID0_SET(&cb, dst_ring_num);
+	CFG_CLE_FPSEL0_SET(&cb, fpsel);
+	xgene_enet_wr_csr(pdata, XCLE_BYPASS_REG1_ADDR, cb);
+}
+
+static void xgene_enet_shutdown(struct xgene_enet_pdata *pdata)
+{
+	clk_disable_unprepare(pdata->clk);
+}
+
+static void xgene_enet_link_state(struct work_struct *work)
+{
+	struct xgene_enet_pdata *pdata = container_of(to_delayed_work(work),
+					 struct xgene_enet_pdata, link_work);
+	struct net_device *ndev = pdata->ndev;
+	u32 link_status, poll_interval;
+
+	link_status = xgene_enet_link_status(pdata);
+	if (link_status) {
+		if (!netif_carrier_ok(ndev)) {
+			netif_carrier_on(ndev);
+			xgene_xgmac_init(pdata);
+			xgene_xgmac_rx_enable(pdata);
+			xgene_xgmac_tx_enable(pdata);
+			netdev_info(ndev, "Link is Up - 10Gbps\n");
+		}
+		poll_interval = PHY_POLL_LINK_ON;
+	} else {
+		if (netif_carrier_ok(ndev)) {
+			xgene_xgmac_rx_disable(pdata);
+			xgene_xgmac_tx_disable(pdata);
+			netif_carrier_off(ndev);
+			netdev_info(ndev, "Link is Down\n");
+		}
+		poll_interval = PHY_POLL_LINK_OFF;
+	}
+
+	schedule_delayed_work(&pdata->link_work, poll_interval);
+}
+
+struct xgene_mac_ops xgene_xgmac_ops = {
+	.init = xgene_xgmac_init,
+	.reset = xgene_xgmac_reset,
+	.rx_enable = xgene_xgmac_rx_enable,
+	.tx_enable = xgene_xgmac_tx_enable,
+	.rx_disable = xgene_xgmac_rx_disable,
+	.tx_disable = xgene_xgmac_tx_disable,
+	.set_mac_addr = xgene_xgmac_set_mac_addr,
+	.link_state = xgene_enet_link_state
+};
+
+struct xgene_port_ops xgene_xgport_ops = {
+	.reset = xgene_enet_reset,
+	.cle_bypass = xgene_enet_xgcle_bypass,
+	.shutdown = xgene_enet_shutdown,
+};
diff --git a/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
new file mode 100644
index 000000000000..5a5296a6d1df
--- /dev/null
+++ b/drivers/net/ethernet/apm/xgene/xgene_enet_xgmac.h
@@ -0,0 +1,53 @@
+/* Applied Micro X-Gene SoC Ethernet Driver
+ *
+ * Copyright (c) 2014, Applied Micro Circuits Corporation
+ * Authors: Iyappan Subramanian <isubramanian@apm.com>
+ *	    Keyur Chudgar <kchudgar@apm.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef __XGENE_ENET_XGMAC_H__
+#define __XGENE_ENET_XGMAC_H__
+
+#define BLOCK_AXG_MAC_OFFSET		0x0800
+#define BLOCK_AXG_MAC_CSR_OFFSET	0x2000
+
+#define AXGMAC_CONFIG_0			0x0000
+#define AXGMAC_CONFIG_1			0x0004
+#define HSTMACRST			BIT(31)
+#define HSTTCTLEN			BIT(31)
+#define HSTTFEN				BIT(30)
+#define HSTRCTLEN			BIT(29)
+#define HSTRFEN				BIT(28)
+#define HSTPPEN				BIT(7)
+#define HSTDRPLT64			BIT(5)
+#define HSTLENCHK			BIT(3)
+#define HSTMACADR_LSW_ADDR		0x0010
+#define HSTMACADR_MSW_ADDR		0x0014
+#define HSTMAXFRAME_LENGTH_ADDR		0x0020
+
+#define XG_RSIF_CONFIG_REG_ADDR		0x00a0
+#define XCLE_BYPASS_REG0_ADDR           0x0160
+#define XCLE_BYPASS_REG1_ADDR           0x0164
+#define XG_CFG_BYPASS_ADDR		0x0204
+#define XG_LINK_STATUS_ADDR		0x0228
+#define XG_ENET_SPARE_CFG_REG_ADDR	0x040c
+#define XG_ENET_SPARE_CFG_REG_1_ADDR	0x0410
+#define XGENET_RX_DV_GATE_REG_0_ADDR	0x0804
+
+extern struct xgene_mac_ops xgene_xgmac_ops;
+extern struct xgene_port_ops xgene_xgport_ops;
+
+#endif /* __XGENE_ENET_XGMAC_H__ */
diff --git a/drivers/net/ethernet/arc/Kconfig b/drivers/net/ethernet/arc/Kconfig
index 514c57fd26f1..8e262e2b39b6 100644
--- a/drivers/net/ethernet/arc/Kconfig
+++ b/drivers/net/ethernet/arc/Kconfig
@@ -17,10 +17,14 @@ config NET_VENDOR_ARC
 
 if NET_VENDOR_ARC
 
-config ARC_EMAC
-	tristate "ARC EMAC support"
+config ARC_EMAC_CORE
+	tristate
 	select MII
 	select PHYLIB
+
+config ARC_EMAC
+	tristate "ARC EMAC support"
+	select ARC_EMAC_CORE
 	depends on OF_IRQ
 	depends on OF_NET
 	---help---
@@ -28,4 +32,14 @@ config ARC_EMAC
 	  non-standard on-chip ethernet device ARC EMAC 10/100 is used.
 	  Say Y here if you have such a board.  If unsure, say N.
 
+config EMAC_ROCKCHIP
+	tristate "Rockchip EMAC support"
+	select ARC_EMAC_CORE
+	depends on OF_IRQ && OF_NET && REGULATOR
+	---help---
+	  Support for Rockchip RK3066/RK3188 EMAC ethernet controllers.
+	  This selects Rockchip SoC glue layer support for the
+	  emac device driver. This driver is used for RK3066/RK3188
+	  EMAC ethernet controller.
+
 endif # NET_VENDOR_ARC
diff --git a/drivers/net/ethernet/arc/Makefile b/drivers/net/ethernet/arc/Makefile
index 00c8657637d5..79108af553fb 100644
--- a/drivers/net/ethernet/arc/Makefile
+++ b/drivers/net/ethernet/arc/Makefile
@@ -3,4 +3,6 @@
 #
 
 arc_emac-objs := emac_main.o emac_mdio.o
-obj-$(CONFIG_ARC_EMAC) += arc_emac.o
+obj-$(CONFIG_ARC_EMAC_CORE) += arc_emac.o
+obj-$(CONFIG_ARC_EMAC) += emac_arc.o
+obj-$(CONFIG_EMAC_ROCKCHIP) += emac_rockchip.o
diff --git a/drivers/net/ethernet/arc/emac.h b/drivers/net/ethernet/arc/emac.h
index 36cc9bd07c47..dae1ac300a49 100644
--- a/drivers/net/ethernet/arc/emac.h
+++ b/drivers/net/ethernet/arc/emac.h
@@ -123,6 +123,10 @@ struct buffer_state {
  * @speed:	PHY's last set speed.
  */
 struct arc_emac_priv {
+	const char *drv_name;
+	const char *drv_version;
+	void (*set_mac_speed)(void *priv, unsigned int speed);
+
 	/* Devices */
 	struct device *dev;
 	struct phy_device *phy_dev;
@@ -204,7 +208,9 @@ static inline void arc_reg_clr(struct arc_emac_priv *priv, int reg, int mask)
 	arc_reg_set(priv, reg, value & ~mask);
 }
 
-int arc_mdio_probe(struct platform_device *pdev, struct arc_emac_priv *priv);
+int arc_mdio_probe(struct arc_emac_priv *priv);
 int arc_mdio_remove(struct arc_emac_priv *priv);
+int arc_emac_probe(struct net_device *ndev, int interface);
+int arc_emac_remove(struct net_device *ndev);
 
 #endif /* ARC_EMAC_H */
diff --git a/drivers/net/ethernet/arc/emac_arc.c b/drivers/net/ethernet/arc/emac_arc.c
new file mode 100644
index 000000000000..f9cb99bfb511
--- /dev/null
+++ b/drivers/net/ethernet/arc/emac_arc.c
@@ -0,0 +1,95 @@
+/**
+ * emac_arc.c - ARC EMAC specific glue layer
+ *
+ * Copyright (C) 2014 Romain Perier
+ *
+ * Romain Perier  <romain.perier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/module.h>
+#include <linux/of_net.h>
+#include <linux/platform_device.h>
+
+#include "emac.h"
+
+#define DRV_NAME    "emac_arc"
+#define DRV_VERSION "1.0"
+
+static int emac_arc_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct net_device *ndev;
+	struct arc_emac_priv *priv;
+	int interface, err;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	ndev = alloc_etherdev(sizeof(struct arc_emac_priv));
+	if (!ndev)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, ndev);
+	SET_NETDEV_DEV(ndev, dev);
+
+	priv = netdev_priv(ndev);
+	priv->drv_name = DRV_NAME;
+	priv->drv_version = DRV_VERSION;
+
+	interface = of_get_phy_mode(dev->of_node);
+	if (interface < 0)
+		interface = PHY_INTERFACE_MODE_MII;
+
+	priv->clk = devm_clk_get(dev, "hclk");
+	if (IS_ERR(priv->clk)) {
+		dev_err(dev, "failed to retrieve host clock from device tree\n");
+		err = -EINVAL;
+		goto out_netdev;
+	}
+
+	err = arc_emac_probe(ndev, interface);
+out_netdev:
+	if (err)
+		free_netdev(ndev);
+	return err;
+}
+
+static int emac_arc_remove(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	int err;
+
+	err = arc_emac_remove(ndev);
+	free_netdev(ndev);
+	return err;
+}
+
+static const struct of_device_id emac_arc_dt_ids[] = {
+	{ .compatible = "snps,arc-emac" },
+	{ /* Sentinel */ }
+};
+
+static struct platform_driver emac_arc_driver = {
+	.probe = emac_arc_probe,
+	.remove = emac_arc_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table  = emac_arc_dt_ids,
+	},
+};
+
+module_platform_driver(emac_arc_driver);
+
+MODULE_AUTHOR("Romain Perier <romain.perier@gmail.com>");
+MODULE_DESCRIPTION("ARC EMAC platform driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/arc/emac_main.c b/drivers/net/ethernet/arc/emac_main.c
index fe5cfeace6e3..abe1eabc0171 100644
--- a/drivers/net/ethernet/arc/emac_main.c
+++ b/drivers/net/ethernet/arc/emac_main.c
@@ -26,8 +26,17 @@
 
 #include "emac.h"
 
-#define DRV_NAME	"arc_emac"
-#define DRV_VERSION	"1.0"
+
+/**
+ * arc_emac_tx_avail - Return the number of available slots in the tx ring.
+ * @priv: Pointer to ARC EMAC private data structure.
+ *
+ * returns: the number of slots available for transmission in tx the ring.
+ */
+static inline int arc_emac_tx_avail(struct arc_emac_priv *priv)
+{
+	return (priv->txbd_dirty + TX_BD_NUM - priv->txbd_curr - 1) % TX_BD_NUM;
+}
 
 /**
  * arc_emac_adjust_link - Adjust the PHY link duplex.
@@ -50,6 +59,8 @@ static void arc_emac_adjust_link(struct net_device *ndev)
 	if (priv->speed != phy_dev->speed) {
 		priv->speed = phy_dev->speed;
 		state_changed = 1;
+		if (priv->set_mac_speed)
+			priv->set_mac_speed(priv, priv->speed);
 	}
 
 	if (priv->duplex != phy_dev->duplex) {
@@ -120,8 +131,10 @@ static int arc_emac_set_settings(struct net_device *ndev,
 static void arc_emac_get_drvinfo(struct net_device *ndev,
 				 struct ethtool_drvinfo *info)
 {
-	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
-	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
+	struct arc_emac_priv *priv = netdev_priv(ndev);
+
+	strlcpy(info->driver, priv->drv_name, sizeof(info->driver));
+	strlcpy(info->version, priv->drv_version, sizeof(info->version));
 }
 
 static const struct ethtool_ops arc_emac_ethtool_ops = {
@@ -180,10 +193,15 @@ static void arc_emac_tx_clean(struct net_device *ndev)
 		txbd->info = 0;
 
 		*txbd_dirty = (*txbd_dirty + 1) % TX_BD_NUM;
-
-		if (netif_queue_stopped(ndev))
-			netif_wake_queue(ndev);
 	}
+
+	/* Ensure that txbd_dirty is visible to tx() before checking
+	 * for queue stopped.
+	 */
+	smp_mb();
+
+	if (netif_queue_stopped(ndev) && arc_emac_tx_avail(priv))
+		netif_wake_queue(ndev);
 }
 
 /**
@@ -298,7 +316,7 @@ static int arc_emac_poll(struct napi_struct *napi, int budget)
 	work_done = arc_emac_rx(ndev, budget);
 	if (work_done < budget) {
 		napi_complete(napi);
-		arc_reg_or(priv, R_ENABLE, RXINT_MASK);
+		arc_reg_or(priv, R_ENABLE, RXINT_MASK | TXINT_MASK);
 	}
 
 	return work_done;
@@ -327,9 +345,9 @@ static irqreturn_t arc_emac_intr(int irq, void *dev_instance)
 	/* Reset all flags except "MDIO complete" */
 	arc_reg_set(priv, R_STATUS, status);
 
-	if (status & RXINT_MASK) {
+	if (status & (RXINT_MASK | TXINT_MASK)) {
 		if (likely(napi_schedule_prep(&priv->napi))) {
-			arc_reg_clr(priv, R_ENABLE, RXINT_MASK);
+			arc_reg_clr(priv, R_ENABLE, RXINT_MASK | TXINT_MASK);
 			__napi_schedule(&priv->napi);
 		}
 	}
@@ -440,7 +458,7 @@ static int arc_emac_open(struct net_device *ndev)
 	arc_reg_set(priv, R_TX_RING, (unsigned int)priv->txbd_dma);
 
 	/* Enable interrupts */
-	arc_reg_set(priv, R_ENABLE, RXINT_MASK | ERR_MASK);
+	arc_reg_set(priv, R_ENABLE, RXINT_MASK | TXINT_MASK | ERR_MASK);
 
 	/* Set CONTROL */
 	arc_reg_set(priv, R_CTRL,
@@ -511,7 +529,7 @@ static int arc_emac_stop(struct net_device *ndev)
 	netif_stop_queue(ndev);
 
 	/* Disable interrupts */
-	arc_reg_clr(priv, R_ENABLE, RXINT_MASK | ERR_MASK);
+	arc_reg_clr(priv, R_ENABLE, RXINT_MASK | TXINT_MASK | ERR_MASK);
 
 	/* Disable EMAC */
 	arc_reg_clr(priv, R_CTRL, EN_MASK);
@@ -574,11 +592,9 @@ static int arc_emac_tx(struct sk_buff *skb, struct net_device *ndev)
 
 	len = max_t(unsigned int, ETH_ZLEN, skb->len);
 
-	/* EMAC still holds this buffer in its possession.
-	 * CPU must not modify this buffer descriptor
-	 */
-	if (unlikely((le32_to_cpu(*info) & OWN_MASK) == FOR_EMAC)) {
+	if (unlikely(!arc_emac_tx_avail(priv))) {
 		netif_stop_queue(ndev);
+		netdev_err(ndev, "BUG! Tx Ring full when queue awake!\n");
 		return NETDEV_TX_BUSY;
 	}
 
@@ -607,12 +623,19 @@ static int arc_emac_tx(struct sk_buff *skb, struct net_device *ndev)
 	/* Increment index to point to the next BD */
 	*txbd_curr = (*txbd_curr + 1) % TX_BD_NUM;
 
-	/* Get "info" of the next BD */
-	info = &priv->txbd[*txbd_curr].info;
+	/* Ensure that tx_clean() sees the new txbd_curr before
+	 * checking the queue status. This prevents an unneeded wake
+	 * of the queue in tx_clean().
+	 */
+	smp_mb();
 
-	/* Check if if Tx BD ring is full - next BD is still owned by EMAC */
-	if (unlikely((le32_to_cpu(*info) & OWN_MASK) == FOR_EMAC))
+	if (!arc_emac_tx_avail(priv)) {
 		netif_stop_queue(ndev);
+		/* Refresh tx_dirty */
+		smp_mb();
+		if (arc_emac_tx_avail(priv))
+			netif_start_queue(ndev);
+	}
 
 	arc_reg_set(priv, R_STATUS, TXPL_MASK);
 
@@ -671,46 +694,38 @@ static const struct net_device_ops arc_emac_netdev_ops = {
 #endif
 };
 
-static int arc_emac_probe(struct platform_device *pdev)
+int arc_emac_probe(struct net_device *ndev, int interface)
 {
+	struct device *dev = ndev->dev.parent;
 	struct resource res_regs;
 	struct device_node *phy_node;
 	struct arc_emac_priv *priv;
-	struct net_device *ndev;
 	const char *mac_addr;
 	unsigned int id, clock_frequency, irq;
 	int err;
 
-	if (!pdev->dev.of_node)
-		return -ENODEV;
 
 	/* Get PHY from device tree */
-	phy_node = of_parse_phandle(pdev->dev.of_node, "phy", 0);
+	phy_node = of_parse_phandle(dev->of_node, "phy", 0);
 	if (!phy_node) {
-		dev_err(&pdev->dev, "failed to retrieve phy description from device tree\n");
+		dev_err(dev, "failed to retrieve phy description from device tree\n");
 		return -ENODEV;
 	}
 
 	/* Get EMAC registers base address from device tree */
-	err = of_address_to_resource(pdev->dev.of_node, 0, &res_regs);
+	err = of_address_to_resource(dev->of_node, 0, &res_regs);
 	if (err) {
-		dev_err(&pdev->dev, "failed to retrieve registers base from device tree\n");
+		dev_err(dev, "failed to retrieve registers base from device tree\n");
 		return -ENODEV;
 	}
 
 	/* Get IRQ from device tree */
-	irq = irq_of_parse_and_map(pdev->dev.of_node, 0);
+	irq = irq_of_parse_and_map(dev->of_node, 0);
 	if (!irq) {
-		dev_err(&pdev->dev, "failed to retrieve <irq> value from device tree\n");
+		dev_err(dev, "failed to retrieve <irq> value from device tree\n");
 		return -ENODEV;
 	}
 
-	ndev = alloc_etherdev(sizeof(struct arc_emac_priv));
-	if (!ndev)
-		return -ENOMEM;
-
-	platform_set_drvdata(pdev, ndev);
-	SET_NETDEV_DEV(ndev, &pdev->dev);
 
 	ndev->netdev_ops = &arc_emac_netdev_ops;
 	ndev->ethtool_ops = &arc_emac_ethtool_ops;
@@ -719,60 +734,57 @@ static int arc_emac_probe(struct platform_device *pdev)
 	ndev->flags &= ~IFF_MULTICAST;
 
 	priv = netdev_priv(ndev);
-	priv->dev = &pdev->dev;
+	priv->dev = dev;
 
-	priv->regs = devm_ioremap_resource(&pdev->dev, &res_regs);
+	priv->regs = devm_ioremap_resource(dev, &res_regs);
 	if (IS_ERR(priv->regs)) {
-		err = PTR_ERR(priv->regs);
-		goto out_netdev;
+		return PTR_ERR(priv->regs);
 	}
-	dev_dbg(&pdev->dev, "Registers base address is 0x%p\n", priv->regs);
+	dev_dbg(dev, "Registers base address is 0x%p\n", priv->regs);
 
-	priv->clk = of_clk_get(pdev->dev.of_node, 0);
-	if (IS_ERR(priv->clk)) {
-		/* Get CPU clock frequency from device tree */
-		if (of_property_read_u32(pdev->dev.of_node, "clock-frequency",
-					&clock_frequency)) {
-			dev_err(&pdev->dev, "failed to retrieve <clock-frequency> from device tree\n");
-			err = -EINVAL;
-			goto out_netdev;
-		}
-	} else {
+	if (priv->clk) {
 		err = clk_prepare_enable(priv->clk);
 		if (err) {
-			dev_err(&pdev->dev, "failed to enable clock\n");
-			goto out_clkget;
+			dev_err(dev, "failed to enable clock\n");
+			return err;
 		}
 
 		clock_frequency = clk_get_rate(priv->clk);
+	} else {
+		/* Get CPU clock frequency from device tree */
+		if (of_property_read_u32(dev->of_node, "clock-frequency",
+					 &clock_frequency)) {
+			dev_err(dev, "failed to retrieve <clock-frequency> from device tree\n");
+			return -EINVAL;
+		}
 	}
 
 	id = arc_reg_get(priv, R_ID);
 
 	/* Check for EMAC revision 5 or 7, magic number */
 	if (!(id == 0x0005fd02 || id == 0x0007fd02)) {
-		dev_err(&pdev->dev, "ARC EMAC not detected, id=0x%x\n", id);
+		dev_err(dev, "ARC EMAC not detected, id=0x%x\n", id);
 		err = -ENODEV;
 		goto out_clken;
 	}
-	dev_info(&pdev->dev, "ARC EMAC detected with id: 0x%x\n", id);
+	dev_info(dev, "ARC EMAC detected with id: 0x%x\n", id);
 
 	/* Set poll rate so that it polls every 1 ms */
 	arc_reg_set(priv, R_POLLRATE, clock_frequency / 1000000);
 
 	ndev->irq = irq;
-	dev_info(&pdev->dev, "IRQ is %d\n", ndev->irq);
+	dev_info(dev, "IRQ is %d\n", ndev->irq);
 
 	/* Register interrupt handler for device */
-	err = devm_request_irq(&pdev->dev, ndev->irq, arc_emac_intr, 0,
+	err = devm_request_irq(dev, ndev->irq, arc_emac_intr, 0,
 			       ndev->name, ndev);
 	if (err) {
-		dev_err(&pdev->dev, "could not allocate IRQ\n");
+		dev_err(dev, "could not allocate IRQ\n");
 		goto out_clken;
 	}
 
 	/* Get MAC address from device tree */
-	mac_addr = of_get_mac_address(pdev->dev.of_node);
+	mac_addr = of_get_mac_address(dev->of_node);
 
 	if (mac_addr)
 		memcpy(ndev->dev_addr, mac_addr, ETH_ALEN);
@@ -780,14 +792,14 @@ static int arc_emac_probe(struct platform_device *pdev)
 		eth_hw_addr_random(ndev);
 
 	arc_emac_set_address_internal(ndev);
-	dev_info(&pdev->dev, "MAC address is now %pM\n", ndev->dev_addr);
+	dev_info(dev, "MAC address is now %pM\n", ndev->dev_addr);
 
 	/* Do 1 allocation instead of 2 separate ones for Rx and Tx BD rings */
-	priv->rxbd = dmam_alloc_coherent(&pdev->dev, RX_RING_SZ + TX_RING_SZ,
+	priv->rxbd = dmam_alloc_coherent(dev, RX_RING_SZ + TX_RING_SZ,
 					 &priv->rxbd_dma, GFP_KERNEL);
 
 	if (!priv->rxbd) {
-		dev_err(&pdev->dev, "failed to allocate data buffers\n");
+		dev_err(dev, "failed to allocate data buffers\n");
 		err = -ENOMEM;
 		goto out_clken;
 	}
@@ -795,31 +807,31 @@ static int arc_emac_probe(struct platform_device *pdev)
 	priv->txbd = priv->rxbd + RX_BD_NUM;
 
 	priv->txbd_dma = priv->rxbd_dma + RX_RING_SZ;
-	dev_dbg(&pdev->dev, "EMAC Device addr: Rx Ring [0x%x], Tx Ring[%x]\n",
+	dev_dbg(dev, "EMAC Device addr: Rx Ring [0x%x], Tx Ring[%x]\n",
 		(unsigned int)priv->rxbd_dma, (unsigned int)priv->txbd_dma);
 
-	err = arc_mdio_probe(pdev, priv);
+	err = arc_mdio_probe(priv);
 	if (err) {
-		dev_err(&pdev->dev, "failed to probe MII bus\n");
+		dev_err(dev, "failed to probe MII bus\n");
 		goto out_clken;
 	}
 
 	priv->phy_dev = of_phy_connect(ndev, phy_node, arc_emac_adjust_link, 0,
-				       PHY_INTERFACE_MODE_MII);
+				       interface);
 	if (!priv->phy_dev) {
-		dev_err(&pdev->dev, "of_phy_connect() failed\n");
+		dev_err(dev, "of_phy_connect() failed\n");
 		err = -ENODEV;
 		goto out_mdio;
 	}
 
-	dev_info(&pdev->dev, "connected to %s phy with id 0x%x\n",
+	dev_info(dev, "connected to %s phy with id 0x%x\n",
 		 priv->phy_dev->drv->name, priv->phy_dev->phy_id);
 
 	netif_napi_add(ndev, &priv->napi, arc_emac_poll, ARC_EMAC_NAPI_WEIGHT);
 
 	err = register_netdev(ndev);
 	if (err) {
-		dev_err(&pdev->dev, "failed to register network device\n");
+		dev_err(dev, "failed to register network device\n");
 		goto out_netif_api;
 	}
 
@@ -832,19 +844,14 @@ out_netif_api:
 out_mdio:
 	arc_mdio_remove(priv);
 out_clken:
-	if (!IS_ERR(priv->clk))
+	if (priv->clk)
 		clk_disable_unprepare(priv->clk);
-out_clkget:
-	if (!IS_ERR(priv->clk))
-		clk_put(priv->clk);
-out_netdev:
-	free_netdev(ndev);
 	return err;
 }
+EXPORT_SYMBOL_GPL(arc_emac_probe);
 
-static int arc_emac_remove(struct platform_device *pdev)
+int arc_emac_remove(struct net_device *ndev)
 {
-	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct arc_emac_priv *priv = netdev_priv(ndev);
 
 	phy_disconnect(priv->phy_dev);
@@ -855,31 +862,12 @@ static int arc_emac_remove(struct platform_device *pdev)
 
 	if (!IS_ERR(priv->clk)) {
 		clk_disable_unprepare(priv->clk);
-		clk_put(priv->clk);
 	}
 
-	free_netdev(ndev);
 
 	return 0;
 }
-
-static const struct of_device_id arc_emac_dt_ids[] = {
-	{ .compatible = "snps,arc-emac" },
-	{ /* Sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, arc_emac_dt_ids);
-
-static struct platform_driver arc_emac_driver = {
-	.probe = arc_emac_probe,
-	.remove = arc_emac_remove,
-	.driver = {
-		.name = DRV_NAME,
-		.owner = THIS_MODULE,
-		.of_match_table  = arc_emac_dt_ids,
-		},
-};
-
-module_platform_driver(arc_emac_driver);
+EXPORT_SYMBOL_GPL(arc_emac_remove);
 
 MODULE_AUTHOR("Alexey Brodkin <abrodkin@synopsys.com>");
 MODULE_DESCRIPTION("ARC EMAC driver");
diff --git a/drivers/net/ethernet/arc/emac_mdio.c b/drivers/net/ethernet/arc/emac_mdio.c
index 26ba2423f33a..d5ee986936da 100644
--- a/drivers/net/ethernet/arc/emac_mdio.c
+++ b/drivers/net/ethernet/arc/emac_mdio.c
@@ -100,7 +100,6 @@ static int arc_mdio_write(struct mii_bus *bus, int phy_addr,
 
 /**
  * arc_mdio_probe - MDIO probe function.
- * @pdev:	Pointer to platform device.
  * @priv:	Pointer to ARC EMAC private data structure.
  *
  * returns:	0 on success, -ENOMEM when mdiobus_alloc
@@ -108,7 +107,7 @@ static int arc_mdio_write(struct mii_bus *bus, int phy_addr,
  *
  * Sets up and registers the MDIO interface.
  */
-int arc_mdio_probe(struct platform_device *pdev, struct arc_emac_priv *priv)
+int arc_mdio_probe(struct arc_emac_priv *priv)
 {
 	struct mii_bus *bus;
 	int error;
@@ -124,9 +123,9 @@ int arc_mdio_probe(struct platform_device *pdev, struct arc_emac_priv *priv)
 	bus->read = &arc_mdio_read;
 	bus->write = &arc_mdio_write;
 
-	snprintf(bus->id, MII_BUS_ID_SIZE, "%s", pdev->name);
+	snprintf(bus->id, MII_BUS_ID_SIZE, "%s", bus->name);
 
-	error = of_mdiobus_register(bus, pdev->dev.of_node);
+	error = of_mdiobus_register(bus, priv->dev->of_node);
 	if (error) {
 		dev_err(priv->dev, "cannot register MDIO bus %s\n", bus->name);
 		mdiobus_free(bus);
diff --git a/drivers/net/ethernet/arc/emac_rockchip.c b/drivers/net/ethernet/arc/emac_rockchip.c
new file mode 100644
index 000000000000..c31c7407b753
--- /dev/null
+++ b/drivers/net/ethernet/arc/emac_rockchip.c
@@ -0,0 +1,229 @@
+/**
+ * emac-rockchip.c - Rockchip EMAC specific glue layer
+ *
+ * Copyright (C) 2014 Romain Perier <romain.perier@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of_net.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#include "emac.h"
+
+#define DRV_NAME        "rockchip_emac"
+#define DRV_VERSION     "1.0"
+
+#define GRF_MODE_MII		(1UL << 0)
+#define GRF_MODE_RMII		(0UL << 0)
+#define GRF_SPEED_10M		(0UL << 1)
+#define GRF_SPEED_100M		(1UL << 1)
+#define GRF_SPEED_ENABLE_BIT	(1UL << 17)
+#define GRF_MODE_ENABLE_BIT	(1UL << 16)
+
+struct emac_rockchip_soc_data {
+	int grf_offset;
+};
+
+struct rockchip_priv_data {
+	struct arc_emac_priv emac;
+	struct regmap *grf;
+	const struct emac_rockchip_soc_data *soc_data;
+	struct regulator *regulator;
+	struct clk *refclk;
+};
+
+static void emac_rockchip_set_mac_speed(void *priv, unsigned int speed)
+{
+	struct rockchip_priv_data *emac = priv;
+	u32 data;
+	int err = 0;
+
+	/* write-enable bits */
+	data = GRF_SPEED_ENABLE_BIT;
+
+	switch(speed) {
+	case 10:
+		data |= GRF_SPEED_10M;
+		break;
+	case 100:
+		data |= GRF_SPEED_100M;
+		break;
+	default:
+		pr_err("speed %u not supported\n", speed);
+		return;
+	}
+
+	err = regmap_write(emac->grf, emac->soc_data->grf_offset, data);
+	if (err)
+		pr_err("unable to apply speed %u to grf (%d)\n", speed, err);
+}
+
+static const struct emac_rockchip_soc_data emac_rockchip_dt_data[] = {
+	{ .grf_offset = 0x154 }, /* rk3066 */
+	{ .grf_offset = 0x0a4 }, /* rk3188 */
+};
+
+static const struct of_device_id emac_rockchip_dt_ids[] = {
+	{ .compatible = "rockchip,rk3066-emac", .data = &emac_rockchip_dt_data[0] },
+	{ .compatible = "rockchip,rk3188-emac", .data = &emac_rockchip_dt_data[1] },
+	{ /* Sentinel */ }
+};
+
+MODULE_DEVICE_TABLE(of, emac_rockchip_dt_ids);
+
+static int emac_rockchip_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct net_device *ndev;
+	struct rockchip_priv_data *priv;
+	const struct of_device_id *match;
+	u32 data;
+	int err, interface;
+
+	if (!pdev->dev.of_node)
+		return -ENODEV;
+
+	ndev = alloc_etherdev(sizeof(struct rockchip_priv_data));
+	if (!ndev)
+		return -ENOMEM;
+	platform_set_drvdata(pdev, ndev);
+	SET_NETDEV_DEV(ndev, dev);
+
+	priv = netdev_priv(ndev);
+	priv->emac.drv_name = DRV_NAME;
+	priv->emac.drv_version = DRV_VERSION;
+	priv->emac.set_mac_speed = emac_rockchip_set_mac_speed;
+
+	interface = of_get_phy_mode(dev->of_node);
+
+	/* RK3066 and RK3188 SoCs only support RMII */
+	if (interface != PHY_INTERFACE_MODE_RMII) {
+		dev_err(dev, "unsupported phy interface mode %d\n", interface);
+		err = -ENOTSUPP;
+		goto out_netdev;
+	}
+
+	priv->grf = syscon_regmap_lookup_by_phandle(dev->of_node, "rockchip,grf");
+	if (IS_ERR(priv->grf)) {
+		dev_err(dev, "failed to retrieve global register file (%ld)\n", PTR_ERR(priv->grf));
+		err = PTR_ERR(priv->grf);
+		goto out_netdev;
+	}
+
+	match = of_match_node(emac_rockchip_dt_ids, dev->of_node);
+	priv->soc_data = match->data;
+
+	priv->emac.clk = devm_clk_get(dev, "hclk");
+	if (IS_ERR(priv->emac.clk)) {
+		dev_err(dev, "failed to retrieve host clock (%ld)\n", PTR_ERR(priv->emac.clk));
+		err = PTR_ERR(priv->emac.clk);
+		goto out_netdev;
+	}
+
+	priv->refclk = devm_clk_get(dev, "macref");
+	if (IS_ERR(priv->refclk)) {
+		dev_err(dev, "failed to retrieve reference clock (%ld)\n", PTR_ERR(priv->refclk));
+		err = PTR_ERR(priv->refclk);
+		goto out_netdev;
+	}
+
+	err = clk_prepare_enable(priv->refclk);
+	if (err) {
+		dev_err(dev, "failed to enable reference clock (%d)\n", err);
+		goto out_netdev;
+	}
+
+	/* Optional regulator for PHY */
+	priv->regulator = devm_regulator_get_optional(dev, "phy");
+	if (IS_ERR(priv->regulator)) {
+		if (PTR_ERR(priv->regulator) == -EPROBE_DEFER)
+			return -EPROBE_DEFER;
+		dev_err(dev, "no regulator found\n");
+		priv->regulator = NULL;
+	}
+
+	if (priv->regulator) {
+		err = regulator_enable(priv->regulator);
+		if (err) {
+			dev_err(dev, "failed to enable phy-supply (%d)\n", err);
+			goto out_clk_disable;
+		}
+	}
+
+	err = arc_emac_probe(ndev, interface);
+	if (err)
+		goto out_regulator_disable;
+
+	/* write-enable bits */
+	data = GRF_MODE_ENABLE_BIT | GRF_SPEED_ENABLE_BIT;
+
+	data |= GRF_SPEED_100M;
+	data |= GRF_MODE_RMII;
+
+	err = regmap_write(priv->grf, priv->soc_data->grf_offset, data);
+	if (err) {
+		dev_err(dev, "unable to apply initial settings to grf (%d)\n", err);
+		goto out_regulator_disable;
+	}
+
+	/* RMII interface needs always a rate of 50MHz */
+	err = clk_set_rate(priv->refclk, 50000000);
+	if (err)
+		dev_err(dev, "failed to change reference clock rate (%d)\n", err);
+	return 0;
+
+out_regulator_disable:
+	if (priv->regulator)
+		regulator_disable(priv->regulator);
+out_clk_disable:
+	clk_disable_unprepare(priv->refclk);
+out_netdev:
+	free_netdev(ndev);
+	return err;
+}
+
+static int emac_rockchip_remove(struct platform_device *pdev)
+{
+	struct net_device *ndev = platform_get_drvdata(pdev);
+	struct rockchip_priv_data *priv = netdev_priv(ndev);
+	int err;
+
+	err = arc_emac_remove(ndev);
+
+	clk_disable_unprepare(priv->refclk);
+
+	if (priv->regulator)
+		regulator_disable(priv->regulator);
+
+	free_netdev(ndev);
+	return err;
+}
+
+static struct platform_driver emac_rockchip_driver = {
+	.probe = emac_rockchip_probe,
+	.remove = emac_rockchip_remove,
+	.driver = {
+		.name = DRV_NAME,
+		.of_match_table  = emac_rockchip_dt_ids,
+	},
+};
+
+module_platform_driver(emac_rockchip_driver);
+
+MODULE_AUTHOR("Romain Perier <romain.perier@gmail.com>");
+MODULE_DESCRIPTION("Rockchip EMAC platform driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/ethernet/broadcom/Kconfig b/drivers/net/ethernet/broadcom/Kconfig
index 7dcfb19a31c8..c3e260c21734 100644
--- a/drivers/net/ethernet/broadcom/Kconfig
+++ b/drivers/net/ethernet/broadcom/Kconfig
@@ -84,7 +84,7 @@ config BNX2
 
 config CNIC
 	tristate "QLogic CNIC support"
-	depends on PCI
+	depends on PCI && (IPV6 || IPV6=n)
 	select BNX2
 	select UIO
 	---help---
@@ -122,6 +122,7 @@ config TIGON3
 config BNX2X
 	tristate "Broadcom NetXtremeII 10Gb support"
 	depends on PCI
+	select PTP_1588_CLOCK
 	select FW_LOADER
 	select ZLIB_INFLATE
 	select LIBCRC32C
diff --git a/drivers/net/ethernet/broadcom/b44.c b/drivers/net/ethernet/broadcom/b44.c
index 4a7028d65912..416620fa8fac 100644
--- a/drivers/net/ethernet/broadcom/b44.c
+++ b/drivers/net/ethernet/broadcom/b44.c
@@ -427,7 +427,7 @@ static void b44_wap54g10_workaround(struct b44 *bp)
 	}
 	return;
 error:
-	pr_warning("PHY: cannot reset MII transceiver isolate bit\n");
+	pr_warn("PHY: cannot reset MII transceiver isolate bit\n");
 }
 #else
 static inline void b44_wap54g10_workaround(struct b44 *bp)
@@ -1697,7 +1697,7 @@ static struct rtnl_link_stats64 *b44_get_stats64(struct net_device *dev,
 				     hwstat->tx_underruns +
 				     hwstat->tx_excessive_cols +
 				     hwstat->tx_late_cols);
-		nstat->multicast  = hwstat->tx_multicast_pkts;
+		nstat->multicast  = hwstat->rx_multicast_pkts;
 		nstat->collisions = hwstat->tx_total_cols;
 
 		nstat->rx_length_errors = (hwstat->rx_oversize_pkts +
diff --git a/drivers/net/ethernet/broadcom/bcmsysport.c b/drivers/net/ethernet/broadcom/bcmsysport.c
index 6f4e18644bd4..9ae36979bdee 100644
--- a/drivers/net/ethernet/broadcom/bcmsysport.c
+++ b/drivers/net/ethernet/broadcom/bcmsysport.c
@@ -139,6 +139,15 @@ static int bcm_sysport_set_rx_csum(struct net_device *dev,
 	else
 		reg &= ~RXCHK_SKIP_FCS;
 
+	/* If Broadcom tags are enabled (e.g: using a switch), make
+	 * sure we tell the RXCHK hardware to expect a 4-bytes Broadcom
+	 * tag after the Ethernet MAC Source Address.
+	 */
+	if (netdev_uses_dsa(dev))
+		reg |= RXCHK_BRCM_TAG_EN;
+	else
+		reg &= ~RXCHK_BRCM_TAG_EN;
+
 	rxchk_writel(priv, reg, RXCHK_CONTROL);
 
 	return 0;
@@ -427,7 +436,8 @@ static int bcm_sysport_set_wol(struct net_device *dev,
 	/* Flag the device and relevant IRQ as wakeup capable */
 	if (wol->wolopts) {
 		device_set_wakeup_enable(kdev, 1);
-		enable_irq_wake(priv->wol_irq);
+		if (priv->wol_irq_disabled)
+			enable_irq_wake(priv->wol_irq);
 		priv->wol_irq_disabled = 0;
 	} else {
 		device_set_wakeup_enable(kdev, 0);
@@ -534,6 +544,25 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 	while ((processed < to_process) && (processed < budget)) {
 		cb = &priv->rx_cbs[priv->rx_read_ptr];
 		skb = cb->skb;
+
+		processed++;
+		priv->rx_read_ptr++;
+
+		if (priv->rx_read_ptr == priv->num_rx_bds)
+			priv->rx_read_ptr = 0;
+
+		/* We do not have a backing SKB, so we do not a corresponding
+		 * DMA mapping for this incoming packet since
+		 * bcm_sysport_rx_refill always either has both skb and mapping
+		 * or none.
+		 */
+		if (unlikely(!skb)) {
+			netif_err(priv, rx_err, ndev, "out of memory!\n");
+			ndev->stats.rx_dropped++;
+			ndev->stats.rx_errors++;
+			goto refill;
+		}
+
 		dma_unmap_single(kdev, dma_unmap_addr(cb, dma_addr),
 				 RX_BUF_LENGTH, DMA_FROM_DEVICE);
 
@@ -543,23 +572,11 @@ static unsigned int bcm_sysport_desc_rx(struct bcm_sysport_priv *priv,
 		status = (rsb->rx_status_len >> DESC_STATUS_SHIFT) &
 			  DESC_STATUS_MASK;
 
-		processed++;
-		priv->rx_read_ptr++;
-		if (priv->rx_read_ptr == priv->num_rx_bds)
-			priv->rx_read_ptr = 0;
-
 		netif_dbg(priv, rx_status, ndev,
 			  "p=%d, c=%d, rd_ptr=%d, len=%d, flag=0x%04x\n",
 			  p_index, priv->rx_c_index, priv->rx_read_ptr,
 			  len, status);
 
-		if (unlikely(!skb)) {
-			netif_err(priv, rx_err, ndev, "out of memory!\n");
-			ndev->stats.rx_dropped++;
-			ndev->stats.rx_errors++;
-			goto refill;
-		}
-
 		if (unlikely(!(status & DESC_EOP) || !(status & DESC_SOP))) {
 			netif_err(priv, rx_status, ndev, "fragmented packet!\n");
 			ndev->stats.rx_dropped++;
@@ -841,7 +858,8 @@ static irqreturn_t bcm_sysport_wol_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int bcm_sysport_insert_tsb(struct sk_buff *skb, struct net_device *dev)
+static struct sk_buff *bcm_sysport_insert_tsb(struct sk_buff *skb,
+					      struct net_device *dev)
 {
 	struct sk_buff *nskb;
 	struct bcm_tsb *tsb;
@@ -857,7 +875,7 @@ static int bcm_sysport_insert_tsb(struct sk_buff *skb, struct net_device *dev)
 		if (!nskb) {
 			dev->stats.tx_errors++;
 			dev->stats.tx_dropped++;
-			return -ENOMEM;
+			return NULL;
 		}
 		skb = nskb;
 	}
@@ -876,7 +894,7 @@ static int bcm_sysport_insert_tsb(struct sk_buff *skb, struct net_device *dev)
 			ip_proto = ipv6_hdr(skb)->nexthdr;
 			break;
 		default:
-			return 0;
+			return skb;
 		}
 
 		/* Get the checksum offset and the L4 (transport) offset */
@@ -895,7 +913,7 @@ static int bcm_sysport_insert_tsb(struct sk_buff *skb, struct net_device *dev)
 		tsb->l4_ptr_dest_map = csum_info;
 	}
 
-	return 0;
+	return skb;
 }
 
 static netdev_tx_t bcm_sysport_xmit(struct sk_buff *skb,
@@ -929,8 +947,8 @@ static netdev_tx_t bcm_sysport_xmit(struct sk_buff *skb,
 
 	/* Insert TSB and checksum infos */
 	if (priv->tsb_en) {
-		ret = bcm_sysport_insert_tsb(skb, dev);
-		if (ret) {
+		skb = bcm_sysport_insert_tsb(skb, dev);
+		if (!skb) {
 			ret = NETDEV_TX_OK;
 			goto out;
 		}
@@ -1062,16 +1080,19 @@ static void bcm_sysport_adj_link(struct net_device *dev)
 	if (!phydev->pause)
 		cmd_bits |= CMD_RX_PAUSE_IGNORE | CMD_TX_PAUSE_IGNORE;
 
-	if (changed) {
+	if (!changed)
+		return;
+
+	if (phydev->link) {
 		reg = umac_readl(priv, UMAC_CMD);
 		reg &= ~((CMD_SPEED_MASK << CMD_SPEED_SHIFT) |
 			CMD_HD_EN | CMD_RX_PAUSE_IGNORE |
 			CMD_TX_PAUSE_IGNORE);
 		reg |= cmd_bits;
 		umac_writel(priv, reg, UMAC_CMD);
-
-		phy_print_status(priv->phydev);
 	}
+
+	phy_print_status(priv->phydev);
 }
 
 static int bcm_sysport_init_tx_ring(struct bcm_sysport_priv *priv,
diff --git a/drivers/net/ethernet/broadcom/bnx2.c b/drivers/net/ethernet/broadcom/bnx2.c
index 2fee73b878c2..823d01c5684c 100644
--- a/drivers/net/ethernet/broadcom/bnx2.c
+++ b/drivers/net/ethernet/broadcom/bnx2.c
@@ -3236,8 +3236,9 @@ bnx2_rx_int(struct bnx2 *bp, struct bnx2_napi *bnapi, int budget)
 
 		skb->protocol = eth_type_trans(skb, bp->dev);
 
-		if ((len > (bp->dev->mtu + ETH_HLEN)) &&
-			(ntohs(skb->protocol) != 0x8100)) {
+		if (len > (bp->dev->mtu + ETH_HLEN) &&
+		    skb->protocol != htons(0x8100) &&
+		    skb->protocol != htons(ETH_P_8021AD)) {
 
 			dev_kfree_skb(skb);
 			goto next_rx;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
index d777fae86988..c3a6072134f5 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x.h
@@ -20,13 +20,17 @@
 #include <linux/types.h>
 #include <linux/pci_regs.h>
 
+#include <linux/ptp_clock_kernel.h>
+#include <linux/net_tstamp.h>
+#include <linux/clocksource.h>
+
 /* compilation time flags */
 
 /* define this to make the driver freeze on error to allow getting debug info
  * (you will need to reboot afterwards) */
 /* #define BNX2X_STOP_ON_ERROR */
 
-#define DRV_MODULE_VERSION      "1.78.19-0"
+#define DRV_MODULE_VERSION      "1.710.51-0"
 #define DRV_MODULE_RELDATE      "2014/02/10"
 #define BNX2X_BC_VER            0x040200
 
@@ -70,6 +74,7 @@ enum bnx2x_int_mode {
 #define BNX2X_MSG_SP			0x0100000 /* was: NETIF_MSG_INTR */
 #define BNX2X_MSG_FP			0x0200000 /* was: NETIF_MSG_INTR */
 #define BNX2X_MSG_IOV			0x0800000
+#define BNX2X_MSG_PTP			0x1000000
 #define BNX2X_MSG_IDLE			0x2000000 /* used for idle check*/
 #define BNX2X_MSG_ETHTOOL		0x4000000
 #define BNX2X_MSG_DCB			0x8000000
@@ -1443,6 +1448,12 @@ struct bnx2x_fp_stats {
 	struct bnx2x_eth_q_stats_old eth_q_stats_old;
 };
 
+enum {
+	SUB_MF_MODE_UNKNOWN = 0,
+	SUB_MF_MODE_UFP,
+	SUB_MF_MODE_NPAR1_DOT_5,
+};
+
 struct bnx2x {
 	/* Fields used in the tx and intr/napi performance paths
 	 * are grouped together in the beginning of the structure
@@ -1587,10 +1598,11 @@ struct bnx2x {
 #define USING_SINGLE_MSIX_FLAG		(1 << 20)
 #define BC_SUPPORTS_DCBX_MSG_NON_PMF	(1 << 21)
 #define IS_VF_FLAG			(1 << 22)
-#define INTERRUPTS_ENABLED_FLAG		(1 << 23)
-#define BC_SUPPORTS_RMMOD_CMD		(1 << 24)
-#define HAS_PHYS_PORT_ID		(1 << 25)
-#define AER_ENABLED			(1 << 26)
+#define BC_SUPPORTS_RMMOD_CMD		(1 << 23)
+#define HAS_PHYS_PORT_ID		(1 << 24)
+#define AER_ENABLED			(1 << 25)
+#define PTP_SUPPORTED			(1 << 26)
+#define TX_TIMESTAMPING_EN		(1 << 27)
 
 #define BP_NOMCP(bp)			((bp)->flags & NO_MCP_FLAG)
 
@@ -1653,6 +1665,9 @@ struct bnx2x {
 #define IS_MF_SI(bp)		(bp->mf_mode == MULTI_FUNCTION_SI)
 #define IS_MF_SD(bp)		(bp->mf_mode == MULTI_FUNCTION_SD)
 #define IS_MF_AFEX(bp)		(bp->mf_mode == MULTI_FUNCTION_AFEX)
+	u8			mf_sub_mode;
+#define IS_MF_UFP(bp)		(IS_MF_SD(bp) && \
+				 bp->mf_sub_mode == SUB_MF_MODE_UFP)
 
 	u8			wol;
 
@@ -1684,13 +1699,9 @@ struct bnx2x {
 #define BNX2X_STATE_ERROR		0xf000
 
 #define BNX2X_MAX_PRIORITY		8
-#define BNX2X_MAX_ENTRIES_PER_PRI	16
-#define BNX2X_MAX_COS			3
-#define BNX2X_MAX_TX_COS		2
 	int			num_queues;
 	uint			num_ethernet_queues;
 	uint			num_cnic_queues;
-	int			num_napi_queues;
 	int			disable_tpa;
 
 	u32			rx_mode;
@@ -1933,6 +1944,19 @@ struct bnx2x {
 
 	u8					phys_port_id[ETH_ALEN];
 
+	/* PTP related context */
+	struct ptp_clock *ptp_clock;
+	struct ptp_clock_info ptp_clock_info;
+	struct work_struct ptp_task;
+	struct cyclecounter cyclecounter;
+	struct timecounter timecounter;
+	bool timecounter_init_done;
+	struct sk_buff *ptp_tx_skb;
+	unsigned long ptp_tx_start;
+	bool hwtstamp_ioctl_called;
+	u16 tx_type;
+	u16 rx_filter;
+
 	struct bnx2x_link_report_data		vf_link_vars;
 };
 
@@ -2346,7 +2370,7 @@ void bnx2x_igu_clear_sb_gen(struct bnx2x *bp, u8 func, u8 idu_sb_id,
 #define ATTN_HARD_WIRED_MASK		0xff00
 #define ATTENTION_ID			4
 
-#define IS_MF_STORAGE_ONLY(bp) (IS_MF_STORAGE_SD(bp) || \
+#define IS_MF_STORAGE_ONLY(bp) (IS_MF_STORAGE_PERSONALITY_ONLY(bp) || \
 				 IS_MF_FCOE_AFEX(bp))
 
 /* stuff added to make the code fit 80Col */
@@ -2522,14 +2546,44 @@ void bnx2x_notify_link_changed(struct bnx2x *bp);
 
 #define IS_MF_ISCSI_SD(bp) (IS_MF_SD(bp) && BNX2X_IS_MF_SD_PROTOCOL_ISCSI(bp))
 #define IS_MF_FCOE_SD(bp) (IS_MF_SD(bp) && BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp))
+#define IS_MF_ISCSI_SI(bp) (IS_MF_SI(bp) && BNX2X_IS_MF_EXT_PROTOCOL_ISCSI(bp))
+
+#define IS_MF_ISCSI_ONLY(bp)    (IS_MF_ISCSI_SD(bp) ||  IS_MF_ISCSI_SI(bp))
+
+#define BNX2X_MF_EXT_PROTOCOL_MASK					\
+				(MACP_FUNC_CFG_FLAGS_ETHERNET |		\
+				 MACP_FUNC_CFG_FLAGS_ISCSI_OFFLOAD |	\
+				 MACP_FUNC_CFG_FLAGS_FCOE_OFFLOAD)
+
+#define BNX2X_MF_EXT_PROT(bp)	((bp)->mf_ext_config &			\
+				 BNX2X_MF_EXT_PROTOCOL_MASK)
 
-#define BNX2X_MF_EXT_PROTOCOL_FCOE(bp)  ((bp)->mf_ext_config & \
-					 MACP_FUNC_CFG_FLAGS_FCOE_OFFLOAD)
+#define BNX2X_HAS_MF_EXT_PROTOCOL_FCOE(bp)				\
+		(BNX2X_MF_EXT_PROT(bp) & MACP_FUNC_CFG_FLAGS_FCOE_OFFLOAD)
+
+#define BNX2X_IS_MF_EXT_PROTOCOL_FCOE(bp)				\
+		(BNX2X_MF_EXT_PROT(bp) == MACP_FUNC_CFG_FLAGS_FCOE_OFFLOAD)
+
+#define BNX2X_IS_MF_EXT_PROTOCOL_ISCSI(bp)				\
+		(BNX2X_MF_EXT_PROT(bp) == MACP_FUNC_CFG_FLAGS_ISCSI_OFFLOAD)
+
+#define IS_MF_FCOE_AFEX(bp)						\
+		(IS_MF_AFEX(bp) && BNX2X_IS_MF_EXT_PROTOCOL_FCOE(bp))
+
+#define IS_MF_SD_STORAGE_PERSONALITY_ONLY(bp)				\
+				(IS_MF_SD(bp) &&			\
+				 (BNX2X_IS_MF_SD_PROTOCOL_ISCSI(bp) ||	\
+				  BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp)))
+
+#define IS_MF_SI_STORAGE_PERSONALITY_ONLY(bp)				\
+				(IS_MF_SI(bp) &&			\
+				 (BNX2X_IS_MF_EXT_PROTOCOL_ISCSI(bp) ||	\
+				  BNX2X_IS_MF_EXT_PROTOCOL_FCOE(bp)))
+
+#define IS_MF_STORAGE_PERSONALITY_ONLY(bp)				\
+			(IS_MF_SD_STORAGE_PERSONALITY_ONLY(bp) ||	\
+			 IS_MF_SI_STORAGE_PERSONALITY_ONLY(bp))
 
-#define IS_MF_FCOE_AFEX(bp) (IS_MF_AFEX(bp) && BNX2X_MF_EXT_PROTOCOL_FCOE(bp))
-#define IS_MF_STORAGE_SD(bp) (IS_MF_SD(bp) && \
-				(BNX2X_IS_MF_SD_PROTOCOL_ISCSI(bp) || \
-				 BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp)))
 
 #define SET_FLAG(value, mask, flag) \
 	do {\
@@ -2559,4 +2613,11 @@ void bnx2x_update_mng_version(struct bnx2x *bp);
 
 #define E1H_MAX_MF_SB_COUNT (HC_SB_MAX_SB_E1X/(E1HVN_MAX * PORT_MAX))
 
+void bnx2x_init_ptp(struct bnx2x *bp);
+int bnx2x_configure_ptp_filters(struct bnx2x *bp);
+void bnx2x_set_rx_ts(struct bnx2x *bp, struct sk_buff *skb);
+
+#define BNX2X_MAX_PHC_DRIFT 31000000
+#define BNX2X_PTP_TX_TIMEOUT
+
 #endif /* bnx2x.h */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
index 4e6c82e20224..40beef5bca88 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.c
@@ -21,6 +21,7 @@
 #include <linux/if_vlan.h>
 #include <linux/interrupt.h>
 #include <linux/ip.h>
+#include <linux/crash_dump.h>
 #include <net/tcp.h>
 #include <net/ipv6.h>
 #include <net/ip6_checksum.h>
@@ -64,7 +65,7 @@ static int bnx2x_calc_num_queues(struct bnx2x *bp)
 	int nq = bnx2x_num_queues ? : netif_get_num_default_rss_queues();
 
 	/* Reduce memory usage in kdump environment by using only one queue */
-	if (reset_devices)
+	if (is_kdump_kernel())
 		nq = 1;
 
 	nq = clamp(nq, 1, BNX2X_MAX_QUEUES(bp));
@@ -483,11 +484,7 @@ static void bnx2x_tpa_start(struct bnx2x_fastpath *fp, u16 queue,
 
 #ifdef BNX2X_STOP_ON_ERROR
 	fp->tpa_queue_used |= (1 << queue);
-#ifdef _ASM_GENERIC_INT_L64_H
-	DP(NETIF_MSG_RX_STATUS, "fp->tpa_queue_used = 0x%lx\n",
-#else
 	DP(NETIF_MSG_RX_STATUS, "fp->tpa_queue_used = 0x%llx\n",
-#endif
 	   fp->tpa_queue_used);
 #endif
 }
@@ -1067,6 +1064,11 @@ reuse_rx:
 
 		skb_record_rx_queue(skb, fp->rx_queue);
 
+		/* Check if this packet was timestamped */
+		if (unlikely(cqe->fast_path_cqe.type_error_flags &
+			     (1 << ETH_FAST_PATH_RX_CQE_PTP_PKT_SHIFT)))
+			bnx2x_set_rx_ts(bp, skb);
+
 		if (le16_to_cpu(cqe_fp->pars_flags.flags) &
 		    PARSING_FLAGS_VLAN)
 			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
@@ -1936,7 +1938,7 @@ void bnx2x_set_num_queues(struct bnx2x *bp)
 	bp->num_ethernet_queues = bnx2x_calc_num_queues(bp);
 
 	/* override in STORAGE SD modes */
-	if (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp))
+	if (IS_MF_STORAGE_ONLY(bp))
 		bp->num_ethernet_queues = 1;
 
 	/* Add special queues */
@@ -2082,6 +2084,10 @@ int bnx2x_rss(struct bnx2x *bp, struct bnx2x_rss_config_obj *rss_obj,
 			__set_bit(BNX2X_RSS_IPV4_UDP, &params.rss_flags);
 		if (rss_obj->udp_rss_v6)
 			__set_bit(BNX2X_RSS_IPV6_UDP, &params.rss_flags);
+
+		if (!CHIP_IS_E1x(bp))
+			/* valid only for TUNN_MODE_GRE tunnel mode */
+			__set_bit(BNX2X_RSS_GRE_INNER_HDRS, &params.rss_flags);
 	} else {
 		__set_bit(BNX2X_RSS_MODE_DISABLED, &params.rss_flags);
 	}
@@ -2804,7 +2810,11 @@ int bnx2x_nic_load(struct bnx2x *bp, int load_mode)
 	/* Initialize Rx filter. */
 	bnx2x_set_rx_mode_inner(bp);
 
-	/* Start the Tx */
+	if (bp->flags & PTP_SUPPORTED) {
+		bnx2x_init_ptp(bp);
+		bnx2x_configure_ptp_filters(bp);
+	}
+	/* Start Tx */
 	switch (load_mode) {
 	case LOAD_NORMAL:
 		/* Tx queue should be only re-enabled */
@@ -3441,26 +3451,6 @@ exit_lbl:
 }
 #endif
 
-static void bnx2x_set_pbd_gso_e2(struct sk_buff *skb, u32 *parsing_data,
-				 u32 xmit_type)
-{
-	struct ipv6hdr *ipv6;
-
-	*parsing_data |= (skb_shinfo(skb)->gso_size <<
-			      ETH_TX_PARSE_BD_E2_LSO_MSS_SHIFT) &
-			      ETH_TX_PARSE_BD_E2_LSO_MSS;
-
-	if (xmit_type & XMIT_GSO_ENC_V6)
-		ipv6 = inner_ipv6_hdr(skb);
-	else if (xmit_type & XMIT_GSO_V6)
-		ipv6 = ipv6_hdr(skb);
-	else
-		ipv6 = NULL;
-
-	if (ipv6 && ipv6->nexthdr == NEXTHDR_IPV6)
-		*parsing_data |= ETH_TX_PARSE_BD_E2_IPV6_WITH_EXT_HDR;
-}
-
 /**
  * bnx2x_set_pbd_gso - update PBD in GSO case.
  *
@@ -3470,7 +3460,6 @@ static void bnx2x_set_pbd_gso_e2(struct sk_buff *skb, u32 *parsing_data,
  */
 static void bnx2x_set_pbd_gso(struct sk_buff *skb,
 			      struct eth_tx_parse_bd_e1x *pbd,
-			      struct eth_tx_start_bd *tx_start_bd,
 			      u32 xmit_type)
 {
 	pbd->lso_mss = cpu_to_le16(skb_shinfo(skb)->gso_size);
@@ -3483,9 +3472,6 @@ static void bnx2x_set_pbd_gso(struct sk_buff *skb,
 			bswab16(~csum_tcpudp_magic(ip_hdr(skb)->saddr,
 						   ip_hdr(skb)->daddr,
 						   0, IPPROTO_TCP, 0));
-
-		/* GSO on 57710/57711 needs FW to calculate IP checksum */
-		tx_start_bd->bd_flags.as_bitfield |= ETH_TX_BD_FLAGS_IP_CSUM;
 	} else {
 		pbd->tcp_pseudo_csum =
 			bswab16(~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
@@ -3657,18 +3643,23 @@ static void bnx2x_update_pbds_gso_enc(struct sk_buff *skb,
 			   (__force u32)iph->tot_len -
 			   (__force u32)iph->frag_off;
 
+		outerip_len = iph->ihl << 1;
+
 		pbd2->fw_ip_csum_wo_len_flags_frag =
 			bswab16(csum_fold((__force __wsum)csum));
 	} else {
 		pbd2->fw_ip_hdr_to_payload_w =
 			hlen_w - ((sizeof(struct ipv6hdr)) >> 1);
+		pbd_e2->data.tunnel_data.flags |=
+			ETH_TUNNEL_DATA_IP_HDR_TYPE_OUTER;
 	}
 
 	pbd2->tcp_send_seq = bswab32(inner_tcp_hdr(skb)->seq);
 
 	pbd2->tcp_flags = pbd_tcp_flags(inner_tcp_hdr(skb));
 
-	if (xmit_type & XMIT_GSO_V4) {
+	/* inner IP header info */
+	if (xmit_type & XMIT_CSUM_ENC_V4) {
 		pbd2->hw_ip_id = bswab16(inner_ip_hdr(skb)->id);
 
 		pbd_e2->data.tunnel_data.pseudo_csum =
@@ -3676,8 +3667,6 @@ static void bnx2x_update_pbds_gso_enc(struct sk_buff *skb,
 					inner_ip_hdr(skb)->saddr,
 					inner_ip_hdr(skb)->daddr,
 					0, IPPROTO_TCP, 0));
-
-		outerip_len = ip_hdr(skb)->ihl << 1;
 	} else {
 		pbd_e2->data.tunnel_data.pseudo_csum =
 			bswab16(~csum_ipv6_magic(
@@ -3690,8 +3679,6 @@ static void bnx2x_update_pbds_gso_enc(struct sk_buff *skb,
 
 	*global_data |=
 		outerip_off |
-		(!!(xmit_type & XMIT_CSUM_V6) <<
-			ETH_TX_PARSE_2ND_BD_IP_HDR_TYPE_OUTER_SHIFT) |
 		(outerip_len <<
 			ETH_TX_PARSE_2ND_BD_IP_HDR_LEN_OUTER_W_SHIFT) |
 		((skb->protocol == cpu_to_be16(ETH_P_8021Q)) <<
@@ -3703,6 +3690,23 @@ static void bnx2x_update_pbds_gso_enc(struct sk_buff *skb,
 	}
 }
 
+static inline void bnx2x_set_ipv6_ext_e2(struct sk_buff *skb, u32 *parsing_data,
+					 u32 xmit_type)
+{
+	struct ipv6hdr *ipv6;
+
+	if (!(xmit_type & (XMIT_GSO_ENC_V6 | XMIT_GSO_V6)))
+		return;
+
+	if (xmit_type & XMIT_GSO_ENC_V6)
+		ipv6 = inner_ipv6_hdr(skb);
+	else /* XMIT_GSO_V6 */
+		ipv6 = ipv6_hdr(skb);
+
+	if (ipv6->nexthdr == NEXTHDR_IPV6)
+		*parsing_data |= ETH_TX_PARSE_BD_E2_IPV6_WITH_EXT_HDR;
+}
+
 /* called with netif_tx_lock
  * bnx2x_tx_int() runs without netif_tx_lock unless it needs to call
  * netif_wake_queue()
@@ -3835,6 +3839,20 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	tx_start_bd->bd_flags.as_bitfield = ETH_TX_BD_FLAGS_START_BD;
 
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
+		if (!(bp->flags & TX_TIMESTAMPING_EN)) {
+			BNX2X_ERR("Tx timestamping was not enabled, this packet will not be timestamped\n");
+		} else if (bp->ptp_tx_skb) {
+			BNX2X_ERR("The device supports only a single outstanding packet to timestamp, this packet will not be timestamped\n");
+		} else {
+			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+			/* schedule check for Tx timestamp */
+			bp->ptp_tx_skb = skb_get(skb);
+			bp->ptp_tx_start = jiffies;
+			schedule_work(&bp->ptp_task);
+		}
+	}
+
 	/* header nbd: indirectly zero other flags! */
 	tx_start_bd->general_data = 1 << ETH_TX_START_BD_HDR_NBDS_SHIFT;
 
@@ -3856,12 +3874,16 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		/* when transmitting in a vf, start bd must hold the ethertype
 		 * for fw to enforce it
 		 */
+#ifndef BNX2X_STOP_ON_ERROR
 		if (IS_VF(bp))
+#endif
 			tx_start_bd->vlan_or_ethertype =
 				cpu_to_le16(ntohs(eth->h_proto));
+#ifndef BNX2X_STOP_ON_ERROR
 		else
 			/* used by FW for packet accounting */
 			tx_start_bd->vlan_or_ethertype = cpu_to_le16(pkt_prod);
+#endif
 	}
 
 	nbd = 2; /* start_bd + pbd + frags (updated when pages are mapped) */
@@ -3919,6 +3941,7 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 						     xmit_type);
 		}
 
+		bnx2x_set_ipv6_ext_e2(skb, &pbd_e2_parsing_data, xmit_type);
 		/* Add the macs to the parsing BD if this is a vf or if
 		 * Tx Switching is enabled.
 		 */
@@ -3933,11 +3956,22 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 					      &pbd_e2->data.mac_addr.dst_mid,
 					      &pbd_e2->data.mac_addr.dst_lo,
 					      eth->h_dest);
-		} else if (bp->flags & TX_SWITCHING) {
-			bnx2x_set_fw_mac_addr(&pbd_e2->data.mac_addr.dst_hi,
-					      &pbd_e2->data.mac_addr.dst_mid,
-					      &pbd_e2->data.mac_addr.dst_lo,
-					      eth->h_dest);
+		} else {
+			if (bp->flags & TX_SWITCHING)
+				bnx2x_set_fw_mac_addr(
+						&pbd_e2->data.mac_addr.dst_hi,
+						&pbd_e2->data.mac_addr.dst_mid,
+						&pbd_e2->data.mac_addr.dst_lo,
+						eth->h_dest);
+#ifdef BNX2X_STOP_ON_ERROR
+			/* Enforce security is always set in Stop on Error -
+			 * source mac should be present in the parsing BD
+			 */
+			bnx2x_set_fw_mac_addr(&pbd_e2->data.mac_addr.src_hi,
+					      &pbd_e2->data.mac_addr.src_mid,
+					      &pbd_e2->data.mac_addr.src_lo,
+					      eth->h_source);
+#endif
 		}
 
 		SET_FLAG(pbd_e2_parsing_data,
@@ -3984,10 +4018,12 @@ netdev_tx_t bnx2x_start_xmit(struct sk_buff *skb, struct net_device *dev)
 						 bd_prod);
 		}
 		if (!CHIP_IS_E1x(bp))
-			bnx2x_set_pbd_gso_e2(skb, &pbd_e2_parsing_data,
-					     xmit_type);
+			pbd_e2_parsing_data |=
+				(skb_shinfo(skb)->gso_size <<
+				 ETH_TX_PARSE_BD_E2_LSO_MSS_SHIFT) &
+				 ETH_TX_PARSE_BD_E2_LSO_MSS;
 		else
-			bnx2x_set_pbd_gso(skb, pbd_e1x, first_bd, xmit_type);
+			bnx2x_set_pbd_gso(skb, pbd_e1x, xmit_type);
 	}
 
 	/* Set the PBD's parsing_data field if not zero
@@ -4195,14 +4231,13 @@ int bnx2x_change_mac_addr(struct net_device *dev, void *p)
 	struct bnx2x *bp = netdev_priv(dev);
 	int rc = 0;
 
-	if (!bnx2x_is_valid_ether_addr(bp, addr->sa_data)) {
+	if (!is_valid_ether_addr(addr->sa_data)) {
 		BNX2X_ERR("Requested MAC address is not valid\n");
 		return -EINVAL;
 	}
 
-	if ((IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp)) &&
-	    !is_zero_ether_addr(addr->sa_data)) {
-		BNX2X_ERR("Can't configure non-zero address on iSCSI or FCoE functions in MF-SD mode\n");
+	if (IS_MF_STORAGE_ONLY(bp)) {
+		BNX2X_ERR("Can't change address on STORAGE ONLY function\n");
 		return -EINVAL;
 	}
 
@@ -4381,8 +4416,7 @@ static int bnx2x_alloc_fp_mem_at(struct bnx2x *bp, int index)
 	u8 cos;
 	int rx_ring_size = 0;
 
-	if (!bp->rx_ring_size &&
-	    (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp))) {
+	if (!bp->rx_ring_size && IS_MF_STORAGE_ONLY(bp)) {
 		rx_ring_size = MIN_RX_SIZE_NONTPA;
 		bp->rx_ring_size = rx_ring_size;
 	} else if (!bp->rx_ring_size) {
@@ -4775,11 +4809,15 @@ netdev_features_t bnx2x_fix_features(struct net_device *dev,
 	struct bnx2x *bp = netdev_priv(dev);
 
 	/* TPA requires Rx CSUM offloading */
-	if (!(features & NETIF_F_RXCSUM) || bp->disable_tpa) {
+	if (!(features & NETIF_F_RXCSUM)) {
 		features &= ~NETIF_F_LRO;
 		features &= ~NETIF_F_GRO;
 	}
 
+	/* Note: do not disable SW GRO in kernel when HW GRO is off */
+	if (bp->disable_tpa)
+		features &= ~NETIF_F_LRO;
+
 	return features;
 }
 
@@ -4818,6 +4856,10 @@ int bnx2x_set_features(struct net_device *dev, netdev_features_t features)
 	if ((changes & GRO_ENABLE_FLAG) && (flags & TPA_ENABLE_FLAG))
 		changes &= ~GRO_ENABLE_FLAG;
 
+	/* if GRO is changed while HW TPA is off, don't force a reload */
+	if ((changes & GRO_ENABLE_FLAG) && bp->disable_tpa)
+		changes &= ~GRO_ENABLE_FLAG;
+
 	if (changes)
 		bnx2x_reload = true;
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
index 571427c7226b..adcacda7af7b 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_cmn.h
@@ -932,8 +932,15 @@ static inline int bnx2x_func_start(struct bnx2x *bp)
 	else /* CHIP_IS_E1X */
 		start_params->network_cos_mode = FW_WRR;
 
-	start_params->gre_tunnel_mode = L2GRE_TUNNEL;
-	start_params->gre_tunnel_rss = GRE_INNER_HEADERS_RSS;
+	start_params->tunnel_mode	= TUNN_MODE_GRE;
+	start_params->gre_tunnel_type	= IPGRE_TUNNEL;
+	start_params->inner_gre_rss_en	= 1;
+
+	if (IS_MF_UFP(bp) && BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp)) {
+		start_params->class_fail_ethtype = ETH_P_FIP;
+		start_params->class_fail = 1;
+		start_params->no_added_tags = 1;
+	}
 
 	return bnx2x_func_state_change(bp, &func_params);
 }
@@ -1297,15 +1304,7 @@ static inline void bnx2x_update_drv_flags(struct bnx2x *bp, u32 flags, u32 set)
 	}
 }
 
-static inline bool bnx2x_is_valid_ether_addr(struct bnx2x *bp, u8 *addr)
-{
-	if (is_valid_ether_addr(addr) ||
-	    (is_zero_ether_addr(addr) &&
-	     (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp))))
-		return true;
 
-	return false;
-}
 
 /**
  * bnx2x_fill_fw_str - Fill buffer with FW version string
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
index fb26bc4c42a1..6e4294ed1fc9 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dcb.c
@@ -2092,7 +2092,6 @@ static void bnx2x_dcbnl_get_pfc_cfg(struct net_device *netdev, int prio,
 static u8 bnx2x_dcbnl_set_all(struct net_device *netdev)
 {
 	struct bnx2x *bp = netdev_priv(netdev);
-	int rc = 0;
 
 	DP(BNX2X_MSG_DCB, "SET-ALL\n");
 
@@ -2110,9 +2109,7 @@ static u8 bnx2x_dcbnl_set_all(struct net_device *netdev)
 				       1);
 		bnx2x_dcbx_init(bp, true);
 	}
-	DP(BNX2X_MSG_DCB, "set_dcbx_params done (%d)\n", rc);
-	if (rc)
-		return 1;
+	DP(BNX2X_MSG_DCB, "set_dcbx_params done\n");
 
 	return 0;
 }
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dump.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dump.h
index 12eb4baee9f6..741aa130c19f 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dump.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_dump.h
@@ -40,7 +40,7 @@ struct	dump_header {
 	u32 dump_meta_data; /* OR of CHIP and PATH. */
 };
 
-#define BNX2X_DUMP_VERSION 0x50acff01
+#define  BNX2X_DUMP_VERSION 0x61111111
 struct reg_addr {
 	u32 addr;
 	u32 size;
@@ -1464,7 +1464,6 @@ static const struct reg_addr reg_addrs[] = {
 	{ 0x180398, 1, 0x1c, 0x924},
 	{ 0x1803a0, 5, 0x1c, 0x924},
 	{ 0x1803b4, 2, 0x18, 0x924},
-	{ 0x180400, 256, 0x3, 0xfff},
 	{ 0x181000, 4, 0x1f, 0x93c},
 	{ 0x181010, 1020, 0x1f, 0x38},
 	{ 0x182000, 4, 0x18, 0x924},
@@ -1576,7 +1575,6 @@ static const struct reg_addr reg_addrs[] = {
 	{ 0x200398, 1, 0x1c, 0x924},
 	{ 0x2003a0, 1, 0x1c, 0x924},
 	{ 0x2003a8, 2, 0x1c, 0x924},
-	{ 0x200400, 256, 0x3, 0xfff},
 	{ 0x202000, 4, 0x1f, 0x1927},
 	{ 0x202010, 2044, 0x1f, 0x1007},
 	{ 0x204000, 4, 0x18, 0x924},
@@ -1688,7 +1686,6 @@ static const struct reg_addr reg_addrs[] = {
 	{ 0x280398, 1, 0x1c, 0x924},
 	{ 0x2803a0, 1, 0x1c, 0x924},
 	{ 0x2803a8, 2, 0x1c, 0x924},
-	{ 0x280400, 256, 0x3, 0xfff},
 	{ 0x282000, 4, 0x1f, 0x9e4},
 	{ 0x282010, 2044, 0x1f, 0x1c0},
 	{ 0x284000, 4, 0x18, 0x924},
@@ -1800,7 +1797,6 @@ static const struct reg_addr reg_addrs[] = {
 	{ 0x300398, 1, 0x1c, 0x924},
 	{ 0x3003a0, 1, 0x1c, 0x924},
 	{ 0x3003a8, 2, 0x1c, 0x924},
-	{ 0x300400, 256, 0x3, 0xfff},
 	{ 0x302000, 4, 0x1f, 0xf24},
 	{ 0x302010, 2044, 0x1f, 0xe00},
 	{ 0x304000, 4, 0x18, 0x924},
@@ -2206,10 +2202,10 @@ static const struct wreg_addr wreg_addr_e3b0 = {
 	0x1b0c00, 128, 2, read_reg_e3b0, 0x1f, 0x1fff};
 
 static const unsigned int dump_num_registers[NUM_CHIPS][NUM_PRESETS] = {
-	{20782, 18567, 27975, 19729, 18311, 27719, 20836, 32391, 41799, 20812,
-	 26247, 35655, 19074},
-	{32774, 19297, 33277, 31721, 19041, 33021, 32828, 33121, 47101, 32804,
-	 26977, 40957, 35895},
+	{19758, 17543, 26951, 18705, 17287, 26695, 19812, 31367, 40775, 19788,
+	 25223, 34631, 19074},
+	{31750, 18273, 32253, 30697, 18017, 31997, 31804, 32097, 46077, 31780,
+	 25953, 39933, 35895},
 	{36527, 17928, 33697, 35474, 18700, 34466, 36581, 31752, 47521, 36557,
 	 25608, 41377, 43903},
 	{45239, 17936, 34387, 44186, 18708, 35156, 45293, 31760, 48211, 45269,
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
index 92fee842f954..1edc931b1458 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_ethtool.c
@@ -1852,7 +1852,7 @@ static int bnx2x_set_ringparam(struct net_device *dev,
 	if ((ering->rx_pending > MAX_RX_AVAIL) ||
 	    (ering->rx_pending < (bp->disable_tpa ? MIN_RX_SIZE_NONTPA :
 						    MIN_RX_SIZE_TPA)) ||
-	    (ering->tx_pending > (IS_MF_FCOE_AFEX(bp) ? 0 : MAX_TX_AVAIL)) ||
+	    (ering->tx_pending > (IS_MF_STORAGE_ONLY(bp) ? 0 : MAX_TX_AVAIL)) ||
 	    (ering->tx_pending <= MAX_SKB_FRAGS + 4)) {
 		DP(BNX2X_MSG_ETHTOOL, "Command parameters not supported\n");
 		return -EINVAL;
@@ -3481,6 +3481,46 @@ static int bnx2x_set_channels(struct net_device *dev,
 	return bnx2x_nic_load(bp, LOAD_NORMAL);
 }
 
+static int bnx2x_get_ts_info(struct net_device *dev,
+			     struct ethtool_ts_info *info)
+{
+	struct bnx2x *bp = netdev_priv(dev);
+
+	if (bp->flags & PTP_SUPPORTED) {
+		info->so_timestamping = SOF_TIMESTAMPING_TX_SOFTWARE |
+					SOF_TIMESTAMPING_RX_SOFTWARE |
+					SOF_TIMESTAMPING_SOFTWARE |
+					SOF_TIMESTAMPING_TX_HARDWARE |
+					SOF_TIMESTAMPING_RX_HARDWARE |
+					SOF_TIMESTAMPING_RAW_HARDWARE;
+
+		if (bp->ptp_clock)
+			info->phc_index = ptp_clock_index(bp->ptp_clock);
+		else
+			info->phc_index = -1;
+
+		info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
+				   (1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT) |
+				   (1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC) |
+				   (1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_L2_SYNC) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_EVENT) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_SYNC) |
+				   (1 << HWTSTAMP_FILTER_PTP_V2_DELAY_REQ);
+
+		info->tx_types = (1 << HWTSTAMP_TX_OFF)|(1 << HWTSTAMP_TX_ON);
+
+		return 0;
+	}
+
+	return ethtool_op_get_ts_info(dev, info);
+}
+
 static const struct ethtool_ops bnx2x_ethtool_ops = {
 	.get_settings		= bnx2x_get_settings,
 	.set_settings		= bnx2x_set_settings,
@@ -3522,7 +3562,7 @@ static const struct ethtool_ops bnx2x_ethtool_ops = {
 	.get_module_eeprom	= bnx2x_get_module_eeprom,
 	.get_eee		= bnx2x_get_eee,
 	.set_eee		= bnx2x_set_eee,
-	.get_ts_info		= ethtool_op_get_ts_info,
+	.get_ts_info		= bnx2x_get_ts_info,
 };
 
 static const struct ethtool_ops bnx2x_vf_ethtool_ops = {
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
index 95dc36543548..7636e3c18771 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_fw_defs.h
@@ -10,170 +10,170 @@
 #ifndef BNX2X_FW_DEFS_H
 #define BNX2X_FW_DEFS_H
 
-#define CSTORM_ASSERT_LIST_INDEX_OFFSET	(IRO[148].base)
+#define CSTORM_ASSERT_LIST_INDEX_OFFSET	(IRO[152].base)
 #define CSTORM_ASSERT_LIST_OFFSET(assertListEntry) \
-	(IRO[147].base + ((assertListEntry) * IRO[147].m1))
+	(IRO[151].base + ((assertListEntry) * IRO[151].m1))
 #define CSTORM_EVENT_RING_DATA_OFFSET(pfId) \
-	(IRO[153].base + (((pfId)>>1) * IRO[153].m1) + (((pfId)&1) * \
-	IRO[153].m2))
+	(IRO[157].base + (((pfId)>>1) * IRO[157].m1) + (((pfId)&1) * \
+	IRO[157].m2))
 #define CSTORM_EVENT_RING_PROD_OFFSET(pfId) \
-	(IRO[154].base + (((pfId)>>1) * IRO[154].m1) + (((pfId)&1) * \
-	IRO[154].m2))
+	(IRO[158].base + (((pfId)>>1) * IRO[158].m1) + (((pfId)&1) * \
+	IRO[158].m2))
 #define CSTORM_FINAL_CLEANUP_COMPLETE_OFFSET(funcId) \
-	(IRO[159].base + ((funcId) * IRO[159].m1))
+	(IRO[163].base + ((funcId) * IRO[163].m1))
 #define CSTORM_FUNC_EN_OFFSET(funcId) \
-	(IRO[149].base + ((funcId) * IRO[149].m1))
+	(IRO[153].base + ((funcId) * IRO[153].m1))
 #define CSTORM_HC_SYNC_LINE_INDEX_E1X_OFFSET(hcIndex, sbId) \
-	(IRO[139].base + ((hcIndex) * IRO[139].m1) + ((sbId) * IRO[139].m2))
+	(IRO[143].base + ((hcIndex) * IRO[143].m1) + ((sbId) * IRO[143].m2))
 #define CSTORM_HC_SYNC_LINE_INDEX_E2_OFFSET(hcIndex, sbId) \
-	(IRO[138].base + (((hcIndex)>>2) * IRO[138].m1) + (((hcIndex)&3) \
-	* IRO[138].m2) + ((sbId) * IRO[138].m3))
-#define CSTORM_IGU_MODE_OFFSET (IRO[157].base)
+	(IRO[142].base + (((hcIndex)>>2) * IRO[142].m1) + (((hcIndex)&3) \
+	* IRO[142].m2) + ((sbId) * IRO[142].m3))
+#define CSTORM_IGU_MODE_OFFSET (IRO[161].base)
 #define CSTORM_ISCSI_CQ_SIZE_OFFSET(pfId) \
-	(IRO[317].base + ((pfId) * IRO[317].m1))
+	(IRO[323].base + ((pfId) * IRO[323].m1))
 #define CSTORM_ISCSI_CQ_SQN_SIZE_OFFSET(pfId) \
-	(IRO[318].base + ((pfId) * IRO[318].m1))
+	(IRO[324].base + ((pfId) * IRO[324].m1))
 #define CSTORM_ISCSI_EQ_CONS_OFFSET(pfId, iscsiEqId) \
-	(IRO[310].base + ((pfId) * IRO[310].m1) + ((iscsiEqId) * IRO[310].m2))
+	(IRO[316].base + ((pfId) * IRO[316].m1) + ((iscsiEqId) * IRO[316].m2))
 #define CSTORM_ISCSI_EQ_NEXT_EQE_ADDR_OFFSET(pfId, iscsiEqId) \
-	(IRO[312].base + ((pfId) * IRO[312].m1) + ((iscsiEqId) * IRO[312].m2))
+	(IRO[318].base + ((pfId) * IRO[318].m1) + ((iscsiEqId) * IRO[318].m2))
 #define CSTORM_ISCSI_EQ_NEXT_PAGE_ADDR_OFFSET(pfId, iscsiEqId) \
-	(IRO[311].base + ((pfId) * IRO[311].m1) + ((iscsiEqId) * IRO[311].m2))
+	(IRO[317].base + ((pfId) * IRO[317].m1) + ((iscsiEqId) * IRO[317].m2))
 #define CSTORM_ISCSI_EQ_NEXT_PAGE_ADDR_VALID_OFFSET(pfId, iscsiEqId) \
-	(IRO[313].base + ((pfId) * IRO[313].m1) + ((iscsiEqId) * IRO[313].m2))
+	(IRO[319].base + ((pfId) * IRO[319].m1) + ((iscsiEqId) * IRO[319].m2))
 #define CSTORM_ISCSI_EQ_PROD_OFFSET(pfId, iscsiEqId) \
-	(IRO[309].base + ((pfId) * IRO[309].m1) + ((iscsiEqId) * IRO[309].m2))
-#define CSTORM_ISCSI_EQ_SB_INDEX_OFFSET(pfId, iscsiEqId) \
 	(IRO[315].base + ((pfId) * IRO[315].m1) + ((iscsiEqId) * IRO[315].m2))
+#define CSTORM_ISCSI_EQ_SB_INDEX_OFFSET(pfId, iscsiEqId) \
+	(IRO[321].base + ((pfId) * IRO[321].m1) + ((iscsiEqId) * IRO[321].m2))
 #define CSTORM_ISCSI_EQ_SB_NUM_OFFSET(pfId, iscsiEqId) \
-	(IRO[314].base + ((pfId) * IRO[314].m1) + ((iscsiEqId) * IRO[314].m2))
+	(IRO[320].base + ((pfId) * IRO[320].m1) + ((iscsiEqId) * IRO[320].m2))
 #define CSTORM_ISCSI_HQ_SIZE_OFFSET(pfId) \
-	(IRO[316].base + ((pfId) * IRO[316].m1))
+	(IRO[322].base + ((pfId) * IRO[322].m1))
 #define CSTORM_ISCSI_NUM_OF_TASKS_OFFSET(pfId) \
-	(IRO[308].base + ((pfId) * IRO[308].m1))
+	(IRO[314].base + ((pfId) * IRO[314].m1))
 #define CSTORM_ISCSI_PAGE_SIZE_LOG_OFFSET(pfId) \
-	(IRO[307].base + ((pfId) * IRO[307].m1))
+	(IRO[313].base + ((pfId) * IRO[313].m1))
 #define CSTORM_ISCSI_PAGE_SIZE_OFFSET(pfId) \
-	(IRO[306].base + ((pfId) * IRO[306].m1))
+	(IRO[312].base + ((pfId) * IRO[312].m1))
 #define CSTORM_RECORD_SLOW_PATH_OFFSET(funcId) \
-	(IRO[151].base + ((funcId) * IRO[151].m1))
+	(IRO[155].base + ((funcId) * IRO[155].m1))
 #define CSTORM_SP_STATUS_BLOCK_DATA_OFFSET(pfId) \
-	(IRO[142].base + ((pfId) * IRO[142].m1))
+	(IRO[146].base + ((pfId) * IRO[146].m1))
 #define CSTORM_SP_STATUS_BLOCK_DATA_STATE_OFFSET(pfId) \
-	(IRO[143].base + ((pfId) * IRO[143].m1))
+	(IRO[147].base + ((pfId) * IRO[147].m1))
 #define CSTORM_SP_STATUS_BLOCK_OFFSET(pfId) \
-	(IRO[141].base + ((pfId) * IRO[141].m1))
-#define CSTORM_SP_STATUS_BLOCK_SIZE (IRO[141].size)
+	(IRO[145].base + ((pfId) * IRO[145].m1))
+#define CSTORM_SP_STATUS_BLOCK_SIZE (IRO[145].size)
 #define CSTORM_SP_SYNC_BLOCK_OFFSET(pfId) \
-	(IRO[144].base + ((pfId) * IRO[144].m1))
-#define CSTORM_SP_SYNC_BLOCK_SIZE (IRO[144].size)
+	(IRO[148].base + ((pfId) * IRO[148].m1))
+#define CSTORM_SP_SYNC_BLOCK_SIZE (IRO[148].size)
 #define CSTORM_STATUS_BLOCK_DATA_FLAGS_OFFSET(sbId, hcIndex) \
-	(IRO[136].base + ((sbId) * IRO[136].m1) + ((hcIndex) * IRO[136].m2))
+	(IRO[140].base + ((sbId) * IRO[140].m1) + ((hcIndex) * IRO[140].m2))
 #define CSTORM_STATUS_BLOCK_DATA_OFFSET(sbId) \
-	(IRO[133].base + ((sbId) * IRO[133].m1))
+	(IRO[137].base + ((sbId) * IRO[137].m1))
 #define CSTORM_STATUS_BLOCK_DATA_STATE_OFFSET(sbId) \
-	(IRO[134].base + ((sbId) * IRO[134].m1))
+	(IRO[138].base + ((sbId) * IRO[138].m1))
 #define CSTORM_STATUS_BLOCK_DATA_TIMEOUT_OFFSET(sbId, hcIndex) \
-	(IRO[135].base + ((sbId) * IRO[135].m1) + ((hcIndex) * IRO[135].m2))
+	(IRO[139].base + ((sbId) * IRO[139].m1) + ((hcIndex) * IRO[139].m2))
 #define CSTORM_STATUS_BLOCK_OFFSET(sbId) \
-	(IRO[132].base + ((sbId) * IRO[132].m1))
-#define CSTORM_STATUS_BLOCK_SIZE (IRO[132].size)
+	(IRO[136].base + ((sbId) * IRO[136].m1))
+#define CSTORM_STATUS_BLOCK_SIZE (IRO[136].size)
 #define CSTORM_SYNC_BLOCK_OFFSET(sbId) \
-	(IRO[137].base + ((sbId) * IRO[137].m1))
-#define CSTORM_SYNC_BLOCK_SIZE (IRO[137].size)
+	(IRO[141].base + ((sbId) * IRO[141].m1))
+#define CSTORM_SYNC_BLOCK_SIZE (IRO[141].size)
 #define CSTORM_VF_PF_CHANNEL_STATE_OFFSET(vfId) \
-	(IRO[155].base + ((vfId) * IRO[155].m1))
+	(IRO[159].base + ((vfId) * IRO[159].m1))
 #define CSTORM_VF_PF_CHANNEL_VALID_OFFSET(vfId) \
-	(IRO[156].base + ((vfId) * IRO[156].m1))
+	(IRO[160].base + ((vfId) * IRO[160].m1))
 #define CSTORM_VF_TO_PF_OFFSET(funcId) \
-	(IRO[150].base + ((funcId) * IRO[150].m1))
+	(IRO[154].base + ((funcId) * IRO[154].m1))
 #define TSTORM_APPROXIMATE_MATCH_MULTICAST_FILTERING_OFFSET(pfId) \
-	(IRO[203].base + ((pfId) * IRO[203].m1))
+	(IRO[207].base + ((pfId) * IRO[207].m1))
 #define TSTORM_ASSERT_LIST_INDEX_OFFSET	(IRO[102].base)
 #define TSTORM_ASSERT_LIST_OFFSET(assertListEntry) \
 	(IRO[101].base + ((assertListEntry) * IRO[101].m1))
 #define TSTORM_FUNCTION_COMMON_CONFIG_OFFSET(pfId) \
-	(IRO[201].base + ((pfId) * IRO[201].m1))
+	(IRO[205].base + ((pfId) * IRO[205].m1))
 #define TSTORM_FUNC_EN_OFFSET(funcId) \
-	(IRO[103].base + ((funcId) * IRO[103].m1))
+	(IRO[107].base + ((funcId) * IRO[107].m1))
 #define TSTORM_ISCSI_ERROR_BITMAP_OFFSET(pfId) \
-	(IRO[272].base + ((pfId) * IRO[272].m1))
+	(IRO[278].base + ((pfId) * IRO[278].m1))
 #define TSTORM_ISCSI_L2_ISCSI_OOO_CID_TABLE_OFFSET(pfId) \
-	(IRO[273].base + ((pfId) * IRO[273].m1))
+	(IRO[279].base + ((pfId) * IRO[279].m1))
 #define TSTORM_ISCSI_L2_ISCSI_OOO_CLIENT_ID_TABLE_OFFSET(pfId) \
-	(IRO[274].base + ((pfId) * IRO[274].m1))
+	(IRO[280].base + ((pfId) * IRO[280].m1))
 #define TSTORM_ISCSI_L2_ISCSI_OOO_PROD_OFFSET(pfId) \
-	(IRO[275].base + ((pfId) * IRO[275].m1))
+	(IRO[281].base + ((pfId) * IRO[281].m1))
 #define TSTORM_ISCSI_NUM_OF_TASKS_OFFSET(pfId) \
-	(IRO[271].base + ((pfId) * IRO[271].m1))
+	(IRO[277].base + ((pfId) * IRO[277].m1))
 #define TSTORM_ISCSI_PAGE_SIZE_LOG_OFFSET(pfId) \
-	(IRO[270].base + ((pfId) * IRO[270].m1))
+	(IRO[276].base + ((pfId) * IRO[276].m1))
 #define TSTORM_ISCSI_PAGE_SIZE_OFFSET(pfId) \
-	(IRO[269].base + ((pfId) * IRO[269].m1))
+	(IRO[275].base + ((pfId) * IRO[275].m1))
 #define TSTORM_ISCSI_RQ_SIZE_OFFSET(pfId) \
-	(IRO[268].base + ((pfId) * IRO[268].m1))
+	(IRO[274].base + ((pfId) * IRO[274].m1))
 #define TSTORM_ISCSI_TCP_LOCAL_ADV_WND_OFFSET(pfId) \
-	(IRO[278].base + ((pfId) * IRO[278].m1))
+	(IRO[284].base + ((pfId) * IRO[284].m1))
 #define TSTORM_ISCSI_TCP_VARS_FLAGS_OFFSET(pfId) \
-	(IRO[264].base + ((pfId) * IRO[264].m1))
+	(IRO[270].base + ((pfId) * IRO[270].m1))
 #define TSTORM_ISCSI_TCP_VARS_LSB_LOCAL_MAC_ADDR_OFFSET(pfId) \
-	(IRO[265].base + ((pfId) * IRO[265].m1))
+	(IRO[271].base + ((pfId) * IRO[271].m1))
 #define TSTORM_ISCSI_TCP_VARS_MID_LOCAL_MAC_ADDR_OFFSET(pfId) \
-	(IRO[266].base + ((pfId) * IRO[266].m1))
+	(IRO[272].base + ((pfId) * IRO[272].m1))
 #define TSTORM_ISCSI_TCP_VARS_MSB_LOCAL_MAC_ADDR_OFFSET(pfId) \
-	(IRO[267].base + ((pfId) * IRO[267].m1))
+	(IRO[273].base + ((pfId) * IRO[273].m1))
 #define TSTORM_MAC_FILTER_CONFIG_OFFSET(pfId) \
-	(IRO[202].base + ((pfId) * IRO[202].m1))
+	(IRO[206].base + ((pfId) * IRO[206].m1))
 #define TSTORM_RECORD_SLOW_PATH_OFFSET(funcId) \
-	(IRO[105].base + ((funcId) * IRO[105].m1))
+	(IRO[109].base + ((funcId) * IRO[109].m1))
 #define TSTORM_TCP_MAX_CWND_OFFSET(pfId) \
-	(IRO[217].base + ((pfId) * IRO[217].m1))
+	(IRO[223].base + ((pfId) * IRO[223].m1))
 #define TSTORM_VF_TO_PF_OFFSET(funcId) \
-	(IRO[104].base + ((funcId) * IRO[104].m1))
-#define USTORM_AGG_DATA_OFFSET (IRO[206].base)
-#define USTORM_AGG_DATA_SIZE (IRO[206].size)
-#define USTORM_ASSERT_LIST_INDEX_OFFSET	(IRO[177].base)
+	(IRO[108].base + ((funcId) * IRO[108].m1))
+#define USTORM_AGG_DATA_OFFSET (IRO[212].base)
+#define USTORM_AGG_DATA_SIZE (IRO[212].size)
+#define USTORM_ASSERT_LIST_INDEX_OFFSET	(IRO[181].base)
 #define USTORM_ASSERT_LIST_OFFSET(assertListEntry) \
-	(IRO[176].base + ((assertListEntry) * IRO[176].m1))
+	(IRO[180].base + ((assertListEntry) * IRO[180].m1))
 #define USTORM_ETH_PAUSE_ENABLED_OFFSET(portId) \
-	(IRO[183].base + ((portId) * IRO[183].m1))
+	(IRO[187].base + ((portId) * IRO[187].m1))
 #define USTORM_FCOE_EQ_PROD_OFFSET(pfId) \
-	(IRO[319].base + ((pfId) * IRO[319].m1))
+	(IRO[325].base + ((pfId) * IRO[325].m1))
 #define USTORM_FUNC_EN_OFFSET(funcId) \
-	(IRO[178].base + ((funcId) * IRO[178].m1))
+	(IRO[182].base + ((funcId) * IRO[182].m1))
 #define USTORM_ISCSI_CQ_SIZE_OFFSET(pfId) \
-	(IRO[283].base + ((pfId) * IRO[283].m1))
+	(IRO[289].base + ((pfId) * IRO[289].m1))
 #define USTORM_ISCSI_CQ_SQN_SIZE_OFFSET(pfId) \
-	(IRO[284].base + ((pfId) * IRO[284].m1))
+	(IRO[290].base + ((pfId) * IRO[290].m1))
 #define USTORM_ISCSI_ERROR_BITMAP_OFFSET(pfId) \
-	(IRO[288].base + ((pfId) * IRO[288].m1))
+	(IRO[294].base + ((pfId) * IRO[294].m1))
 #define USTORM_ISCSI_GLOBAL_BUF_PHYS_ADDR_OFFSET(pfId) \
-	(IRO[285].base + ((pfId) * IRO[285].m1))
+	(IRO[291].base + ((pfId) * IRO[291].m1))
 #define USTORM_ISCSI_NUM_OF_TASKS_OFFSET(pfId) \
-	(IRO[281].base + ((pfId) * IRO[281].m1))
+	(IRO[287].base + ((pfId) * IRO[287].m1))
 #define USTORM_ISCSI_PAGE_SIZE_LOG_OFFSET(pfId) \
-	(IRO[280].base + ((pfId) * IRO[280].m1))
+	(IRO[286].base + ((pfId) * IRO[286].m1))
 #define USTORM_ISCSI_PAGE_SIZE_OFFSET(pfId) \
-	(IRO[279].base + ((pfId) * IRO[279].m1))
+	(IRO[285].base + ((pfId) * IRO[285].m1))
 #define USTORM_ISCSI_R2TQ_SIZE_OFFSET(pfId) \
-	(IRO[282].base + ((pfId) * IRO[282].m1))
+	(IRO[288].base + ((pfId) * IRO[288].m1))
 #define USTORM_ISCSI_RQ_BUFFER_SIZE_OFFSET(pfId) \
-	(IRO[286].base + ((pfId) * IRO[286].m1))
+	(IRO[292].base + ((pfId) * IRO[292].m1))
 #define USTORM_ISCSI_RQ_SIZE_OFFSET(pfId) \
-	(IRO[287].base + ((pfId) * IRO[287].m1))
+	(IRO[293].base + ((pfId) * IRO[293].m1))
 #define USTORM_MEM_WORKAROUND_ADDRESS_OFFSET(pfId) \
-	(IRO[182].base + ((pfId) * IRO[182].m1))
+	(IRO[186].base + ((pfId) * IRO[186].m1))
 #define USTORM_RECORD_SLOW_PATH_OFFSET(funcId) \
-	(IRO[180].base + ((funcId) * IRO[180].m1))
+	(IRO[184].base + ((funcId) * IRO[184].m1))
 #define USTORM_RX_PRODS_E1X_OFFSET(portId, clientId) \
-	(IRO[209].base + ((portId) * IRO[209].m1) + ((clientId) * \
-	IRO[209].m2))
+	(IRO[215].base + ((portId) * IRO[215].m1) + ((clientId) * \
+	IRO[215].m2))
 #define USTORM_RX_PRODS_E2_OFFSET(qzoneId) \
-	(IRO[210].base + ((qzoneId) * IRO[210].m1))
-#define USTORM_TPA_BTR_OFFSET (IRO[207].base)
-#define USTORM_TPA_BTR_SIZE (IRO[207].size)
+	(IRO[216].base + ((qzoneId) * IRO[216].m1))
+#define USTORM_TPA_BTR_OFFSET (IRO[213].base)
+#define USTORM_TPA_BTR_SIZE (IRO[213].size)
 #define USTORM_VF_TO_PF_OFFSET(funcId) \
-	(IRO[179].base + ((funcId) * IRO[179].m1))
+	(IRO[183].base + ((funcId) * IRO[183].m1))
 #define XSTORM_AGG_INT_FINAL_CLEANUP_COMP_TYPE (IRO[67].base)
 #define XSTORM_AGG_INT_FINAL_CLEANUP_INDEX (IRO[66].base)
 #define XSTORM_ASSERT_LIST_INDEX_OFFSET	(IRO[51].base)
@@ -186,39 +186,39 @@
 #define XSTORM_FUNC_EN_OFFSET(funcId) \
 	(IRO[47].base + ((funcId) * IRO[47].m1))
 #define XSTORM_ISCSI_HQ_SIZE_OFFSET(pfId) \
-	(IRO[296].base + ((pfId) * IRO[296].m1))
+	(IRO[302].base + ((pfId) * IRO[302].m1))
 #define XSTORM_ISCSI_LOCAL_MAC_ADDR0_OFFSET(pfId) \
-	(IRO[299].base + ((pfId) * IRO[299].m1))
+	(IRO[305].base + ((pfId) * IRO[305].m1))
 #define XSTORM_ISCSI_LOCAL_MAC_ADDR1_OFFSET(pfId) \
-	(IRO[300].base + ((pfId) * IRO[300].m1))
+	(IRO[306].base + ((pfId) * IRO[306].m1))
 #define XSTORM_ISCSI_LOCAL_MAC_ADDR2_OFFSET(pfId) \
-	(IRO[301].base + ((pfId) * IRO[301].m1))
+	(IRO[307].base + ((pfId) * IRO[307].m1))
 #define XSTORM_ISCSI_LOCAL_MAC_ADDR3_OFFSET(pfId) \
-	(IRO[302].base + ((pfId) * IRO[302].m1))
+	(IRO[308].base + ((pfId) * IRO[308].m1))
 #define XSTORM_ISCSI_LOCAL_MAC_ADDR4_OFFSET(pfId) \
-	(IRO[303].base + ((pfId) * IRO[303].m1))
+	(IRO[309].base + ((pfId) * IRO[309].m1))
 #define XSTORM_ISCSI_LOCAL_MAC_ADDR5_OFFSET(pfId) \
-	(IRO[304].base + ((pfId) * IRO[304].m1))
+	(IRO[310].base + ((pfId) * IRO[310].m1))
 #define XSTORM_ISCSI_LOCAL_VLAN_OFFSET(pfId) \
-	(IRO[305].base + ((pfId) * IRO[305].m1))
+	(IRO[311].base + ((pfId) * IRO[311].m1))
 #define XSTORM_ISCSI_NUM_OF_TASKS_OFFSET(pfId) \
-	(IRO[295].base + ((pfId) * IRO[295].m1))
+	(IRO[301].base + ((pfId) * IRO[301].m1))
 #define XSTORM_ISCSI_PAGE_SIZE_LOG_OFFSET(pfId) \
-	(IRO[294].base + ((pfId) * IRO[294].m1))
+	(IRO[300].base + ((pfId) * IRO[300].m1))
 #define XSTORM_ISCSI_PAGE_SIZE_OFFSET(pfId) \
-	(IRO[293].base + ((pfId) * IRO[293].m1))
+	(IRO[299].base + ((pfId) * IRO[299].m1))
 #define XSTORM_ISCSI_R2TQ_SIZE_OFFSET(pfId) \
-	(IRO[298].base + ((pfId) * IRO[298].m1))
+	(IRO[304].base + ((pfId) * IRO[304].m1))
 #define XSTORM_ISCSI_SQ_SIZE_OFFSET(pfId) \
-	(IRO[297].base + ((pfId) * IRO[297].m1))
+	(IRO[303].base + ((pfId) * IRO[303].m1))
 #define XSTORM_ISCSI_TCP_VARS_ADV_WND_SCL_OFFSET(pfId) \
-	(IRO[292].base + ((pfId) * IRO[292].m1))
+	(IRO[298].base + ((pfId) * IRO[298].m1))
 #define XSTORM_ISCSI_TCP_VARS_FLAGS_OFFSET(pfId) \
-	(IRO[291].base + ((pfId) * IRO[291].m1))
+	(IRO[297].base + ((pfId) * IRO[297].m1))
 #define XSTORM_ISCSI_TCP_VARS_TOS_OFFSET(pfId) \
-	(IRO[290].base + ((pfId) * IRO[290].m1))
+	(IRO[296].base + ((pfId) * IRO[296].m1))
 #define XSTORM_ISCSI_TCP_VARS_TTL_OFFSET(pfId) \
-	(IRO[289].base + ((pfId) * IRO[289].m1))
+	(IRO[295].base + ((pfId) * IRO[295].m1))
 #define XSTORM_RATE_SHAPING_PER_VN_VARS_OFFSET(pfId) \
 	(IRO[44].base + ((pfId) * IRO[44].m1))
 #define XSTORM_RECORD_SLOW_PATH_OFFSET(funcId) \
@@ -231,16 +231,19 @@
 #define XSTORM_SPQ_PROD_OFFSET(funcId) \
 	(IRO[31].base + ((funcId) * IRO[31].m1))
 #define XSTORM_TCP_GLOBAL_DEL_ACK_COUNTER_ENABLED_OFFSET(portId) \
-	(IRO[211].base + ((portId) * IRO[211].m1))
+	(IRO[217].base + ((portId) * IRO[217].m1))
 #define XSTORM_TCP_GLOBAL_DEL_ACK_COUNTER_MAX_COUNT_OFFSET(portId) \
-	(IRO[212].base + ((portId) * IRO[212].m1))
+	(IRO[218].base + ((portId) * IRO[218].m1))
 #define XSTORM_TCP_TX_SWS_TIMER_VAL_OFFSET(pfId) \
-	(IRO[214].base + (((pfId)>>1) * IRO[214].m1) + (((pfId)&1) * \
-	IRO[214].m2))
+	(IRO[220].base + (((pfId)>>1) * IRO[220].m1) + (((pfId)&1) * \
+	IRO[220].m2))
 #define XSTORM_VF_TO_PF_OFFSET(funcId) \
 	(IRO[48].base + ((funcId) * IRO[48].m1))
 #define COMMON_ASM_INVALID_ASSERT_OPCODE 0x0
 
+/* eth hsi version */
+#define ETH_FP_HSI_VERSION (ETH_FP_HSI_VER_2)
+
 /* Ethernet Ring parameters */
 #define X_ETH_LOCAL_RING_SIZE 13
 #define FIRST_BD_IN_PKT	0
@@ -356,6 +359,7 @@
 #define XSEMI_CLK1_RESUL_CHIP (1e-3)
 
 #define SDM_TIMER_TICK_RESUL_CHIP (4 * (1e-6))
+#define TSDM_TIMER_TICK_RESUL_CHIP (1 * (1e-6))
 
 /**** END DEFINES FOR TIMERS/CLOCKS RESOLUTIONS ****/
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
index 5ba8af50c84f..583591d52497 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_hsi.h
@@ -280,17 +280,11 @@ struct shared_hw_cfg {			 /* NVRAM Offset */
 		#define SHARED_HW_CFG_MDC_MDIO_ACCESS2_BOTH          0x60000000
 		#define SHARED_HW_CFG_MDC_MDIO_ACCESS2_SWAPPED       0x80000000
 
-
-	u32 power_dissipated;			/* 0x11c */
-	#define SHARED_HW_CFG_POWER_MGNT_SCALE_MASK         0x00ff0000
-		#define SHARED_HW_CFG_POWER_MGNT_SCALE_SHIFT         16
-		#define SHARED_HW_CFG_POWER_MGNT_UNKNOWN_SCALE       0x00000000
-		#define SHARED_HW_CFG_POWER_MGNT_DOT_1_WATT          0x00010000
-		#define SHARED_HW_CFG_POWER_MGNT_DOT_01_WATT         0x00020000
-		#define SHARED_HW_CFG_POWER_MGNT_DOT_001_WATT        0x00030000
-
-	#define SHARED_HW_CFG_POWER_DIS_CMN_MASK            0xff000000
-	#define SHARED_HW_CFG_POWER_DIS_CMN_SHIFT                    24
+	u32 config_3;				/* 0x11C */
+	#define SHARED_HW_CFG_EXTENDED_MF_MODE_MASK         0x00000F00
+		#define SHARED_HW_CFG_EXTENDED_MF_MODE_SHIFT              8
+		#define SHARED_HW_CFG_EXTENDED_MF_MODE_NPAR1_DOT_5        0x00000000
+		#define SHARED_HW_CFG_EXTENDED_MF_MODE_NPAR2_DOT_0        0x00000100
 
 	u32 ump_nc_si_config;			/* 0x120 */
 	#define SHARED_HW_CFG_UMP_NC_SI_MII_MODE_MASK       0x00000003
@@ -859,6 +853,8 @@ struct shared_feat_cfg {		 /* NVRAM Offset */
 		#define SHARED_FEAT_CFG_FORCE_SF_MODE_SPIO4          0x00000200
 		#define SHARED_FEAT_CFG_FORCE_SF_MODE_SWITCH_INDEPT  0x00000300
 		#define SHARED_FEAT_CFG_FORCE_SF_MODE_AFEX_MODE      0x00000400
+		#define SHARED_FEAT_CFG_FORCE_SF_MODE_UFP_MODE       0x00000600
+		#define SHARED_FEAT_CFG_FORCE_SF_MODE_EXTENDED_MODE  0x00000700
 
 	/* The interval in seconds between sending LLDP packets. Set to zero
 	   to disable the feature */
@@ -1268,6 +1264,10 @@ struct drv_func_mb {
 	#define DRV_MSG_CODE_GET_UPGRADE_KEY            0x81000000
 	#define DRV_MSG_CODE_GET_MANUF_KEY              0x82000000
 	#define DRV_MSG_CODE_LOAD_L2B_PRAM              0x90000000
+	#define DRV_MSG_CODE_OEM_OK			0x00010000
+	#define DRV_MSG_CODE_OEM_FAILURE		0x00020000
+	#define DRV_MSG_CODE_OEM_UPDATE_SVID_OK		0x00030000
+	#define DRV_MSG_CODE_OEM_UPDATE_SVID_FAILURE	0x00040000
 	/*
 	 * The optic module verification command requires bootcode
 	 * v5.0.6 or later, te specific optic module verification command
@@ -1423,6 +1423,12 @@ struct drv_func_mb {
 	#define DRV_STATUS_SET_MF_BW                    0x00000004
 	#define DRV_STATUS_LINK_EVENT                   0x00000008
 
+	#define DRV_STATUS_OEM_EVENT_MASK               0x00000070
+	#define DRV_STATUS_OEM_DISABLE_ENABLE_PF        0x00000010
+	#define DRV_STATUS_OEM_BANDWIDTH_ALLOCATION     0x00000020
+
+	#define DRV_STATUS_OEM_UPDATE_SVID              0x00000080
+
 	#define DRV_STATUS_DCC_EVENT_MASK               0x0000ff00
 	#define DRV_STATUS_DCC_DISABLE_ENABLE_PF        0x00000100
 	#define DRV_STATUS_DCC_BANDWIDTH_ALLOCATION     0x00000200
@@ -2233,7 +2239,12 @@ struct shmem2_region {
 	u32 reserved3;				/* Offset 0x14C */
 	u32 reserved4;				/* Offset 0x150 */
 	u32 link_attr_sync[PORT_MAX];		/* Offset 0x154 */
-	#define LINK_ATTR_SYNC_KR2_ENABLE	(1<<0)
+	#define LINK_ATTR_SYNC_KR2_ENABLE	0x00000001
+	#define LINK_SFP_EEPROM_COMP_CODE_MASK	0x0000ff00
+	#define LINK_SFP_EEPROM_COMP_CODE_SHIFT		 8
+	#define LINK_SFP_EEPROM_COMP_CODE_SR	0x00001000
+	#define LINK_SFP_EEPROM_COMP_CODE_LR	0x00002000
+	#define LINK_SFP_EEPROM_COMP_CODE_LRM	0x00004000
 
 	u32 reserved5[2];
 	u32 reserved6[PORT_MAX];
@@ -2876,8 +2887,8 @@ struct afex_stats {
 };
 
 #define BCM_5710_FW_MAJOR_VERSION			7
-#define BCM_5710_FW_MINOR_VERSION			8
-#define BCM_5710_FW_REVISION_VERSION		19
+#define BCM_5710_FW_MINOR_VERSION			10
+#define BCM_5710_FW_REVISION_VERSION		51
 #define BCM_5710_FW_ENGINEERING_VERSION		0
 #define BCM_5710_FW_COMPILE_FLAGS			1
 
@@ -3446,6 +3457,7 @@ enum classify_rule {
 	CLASSIFY_RULE_OPCODE_MAC,
 	CLASSIFY_RULE_OPCODE_VLAN,
 	CLASSIFY_RULE_OPCODE_PAIR,
+	CLASSIFY_RULE_OPCODE_VXLAN,
 	MAX_CLASSIFY_RULE
 };
 
@@ -3475,7 +3487,8 @@ struct client_init_general_data {
 	u8 func_id;
 	u8 cos;
 	u8 traffic_type;
-	u32 reserved0;
+	u8 fp_hsi_ver;
+	u8 reserved0[3];
 };
 
 
@@ -3545,7 +3558,9 @@ struct client_init_rx_data {
 	__le16 rx_cos_mask;
 	__le16 silent_vlan_value;
 	__le16 silent_vlan_mask;
-	__le32 reserved6[2];
+	u8 handle_ptp_pkts_flg;
+	u8 reserved6[3];
+	__le32 reserved7;
 };
 
 /*
@@ -3576,7 +3591,7 @@ struct client_init_tx_data {
 	u8 tunnel_lso_inc_ip_id;
 	u8 refuse_outband_vlan_flg;
 	u8 tunnel_non_lso_pcsum_location;
-	u8 reserved1;
+	u8 tunnel_non_lso_outer_ip_csum_location;
 };
 
 /*
@@ -3614,7 +3629,9 @@ struct client_update_ramrod_data {
 	u8 refuse_outband_vlan_change_flg;
 	u8 tx_switching_flg;
 	u8 tx_switching_change_flg;
-	__le32 reserved1;
+	u8 handle_ptp_pkts_flg;
+	u8 handle_ptp_pkts_change_flg;
+	__le16 reserved1;
 	__le32 echo;
 };
 
@@ -3634,6 +3651,11 @@ struct double_regpair {
 	u32 regpair1_hi;
 };
 
+/* 2nd parse bd type used in ethernet tx BDs */
+enum eth_2nd_parse_bd_type {
+	ETH_2ND_PARSE_BD_TYPE_LSO_TUNNEL,
+	MAX_ETH_2ND_PARSE_BD_TYPE
+};
 
 /*
  * Ethernet address typesm used in ethernet tx BDs
@@ -3719,12 +3741,25 @@ struct eth_classify_vlan_cmd {
 };
 
 /*
+ * Command for adding/removing a VXLAN classification rule
+ */
+struct eth_classify_vxlan_cmd {
+	struct eth_classify_cmd_header header;
+	__le32 vni;
+	__le16 inner_mac_lsb;
+	__le16 inner_mac_mid;
+	__le16 inner_mac_msb;
+	__le16 reserved1;
+};
+
+/*
  * union for eth classification rule
  */
 union eth_classify_rule_cmd {
 	struct eth_classify_mac_cmd mac;
 	struct eth_classify_vlan_cmd vlan;
 	struct eth_classify_pair_cmd pair;
+	struct eth_classify_vxlan_cmd vxlan;
 };
 
 /*
@@ -3830,8 +3865,10 @@ struct eth_fast_path_rx_cqe {
 #define ETH_FAST_PATH_RX_CQE_IP_BAD_XSUM_FLG_SHIFT 4
 #define ETH_FAST_PATH_RX_CQE_L4_BAD_XSUM_FLG (0x1<<5)
 #define ETH_FAST_PATH_RX_CQE_L4_BAD_XSUM_FLG_SHIFT 5
-#define ETH_FAST_PATH_RX_CQE_RESERVED0 (0x3<<6)
-#define ETH_FAST_PATH_RX_CQE_RESERVED0_SHIFT 6
+#define ETH_FAST_PATH_RX_CQE_PTP_PKT (0x1<<6)
+#define ETH_FAST_PATH_RX_CQE_PTP_PKT_SHIFT 6
+#define ETH_FAST_PATH_RX_CQE_RESERVED0 (0x1<<7)
+#define ETH_FAST_PATH_RX_CQE_RESERVED0_SHIFT 7
 	u8 status_flags;
 #define ETH_FAST_PATH_RX_CQE_RSS_HASH_TYPE (0x7<<0)
 #define ETH_FAST_PATH_RX_CQE_RSS_HASH_TYPE_SHIFT 0
@@ -3902,6 +3939,13 @@ struct eth_filter_rules_ramrod_data {
 	struct eth_filter_rules_cmd rules[FILTER_RULES_COUNT];
 };
 
+/* Hsi version */
+enum eth_fp_hsi_ver {
+	ETH_FP_HSI_VER_0,
+	ETH_FP_HSI_VER_1,
+	ETH_FP_HSI_VER_2,
+	MAX_ETH_FP_HSI_VER
+};
 
 /*
  * parameters for eth classification configuration ramrod
@@ -3950,29 +3994,17 @@ struct eth_mac_addresses {
 
 /* tunneling related data */
 struct eth_tunnel_data {
-#if defined(__BIG_ENDIAN)
-	__le16 dst_mid;
-	__le16 dst_lo;
-#elif defined(__LITTLE_ENDIAN)
 	__le16 dst_lo;
 	__le16 dst_mid;
-#endif
-#if defined(__BIG_ENDIAN)
-	__le16 reserved0;
-	__le16 dst_hi;
-#elif defined(__LITTLE_ENDIAN)
 	__le16 dst_hi;
-	__le16 reserved0;
-#endif
-#if defined(__BIG_ENDIAN)
-	u8 reserved1;
-	u8 ip_hdr_start_inner_w;
-	__le16 pseudo_csum;
-#elif defined(__LITTLE_ENDIAN)
+	__le16 fw_ip_hdr_csum;
 	__le16 pseudo_csum;
 	u8 ip_hdr_start_inner_w;
-	u8 reserved1;
-#endif
+	u8 flags;
+#define ETH_TUNNEL_DATA_IP_HDR_TYPE_OUTER (0x1<<0)
+#define ETH_TUNNEL_DATA_IP_HDR_TYPE_OUTER_SHIFT 0
+#define ETH_TUNNEL_DATA_RESERVED (0x7F<<1)
+#define ETH_TUNNEL_DATA_RESERVED_SHIFT 1
 };
 
 /* union for mac addresses and for tunneling data.
@@ -4059,31 +4091,41 @@ enum eth_rss_mode {
  */
 struct eth_rss_update_ramrod_data {
 	u8 rss_engine_id;
-	u8 capabilities;
+	u8 rss_mode;
+	__le16 capabilities;
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_CAPABILITY (0x1<<0)
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_CAPABILITY_SHIFT 0
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_TCP_CAPABILITY (0x1<<1)
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_TCP_CAPABILITY_SHIFT 1
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_UDP_CAPABILITY (0x1<<2)
 #define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_UDP_CAPABILITY_SHIFT 2
-#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_CAPABILITY (0x1<<3)
-#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_CAPABILITY_SHIFT 3
-#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY (0x1<<4)
-#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY_SHIFT 4
-#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY (0x1<<5)
-#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY_SHIFT 5
-#define ETH_RSS_UPDATE_RAMROD_DATA_EN_5_TUPLE_CAPABILITY (0x1<<6)
-#define ETH_RSS_UPDATE_RAMROD_DATA_EN_5_TUPLE_CAPABILITY_SHIFT 6
-#define ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY (0x1<<7)
-#define ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY_SHIFT 7
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_VXLAN_CAPABILITY (0x1<<3)
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV4_VXLAN_CAPABILITY_SHIFT 3
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_CAPABILITY (0x1<<4)
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_CAPABILITY_SHIFT 4
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY (0x1<<5)
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY_SHIFT 5
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY (0x1<<6)
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY_SHIFT 6
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_VXLAN_CAPABILITY (0x1<<7)
+#define ETH_RSS_UPDATE_RAMROD_DATA_IPV6_VXLAN_CAPABILITY_SHIFT 7
+#define ETH_RSS_UPDATE_RAMROD_DATA_EN_5_TUPLE_CAPABILITY (0x1<<8)
+#define ETH_RSS_UPDATE_RAMROD_DATA_EN_5_TUPLE_CAPABILITY_SHIFT 8
+#define ETH_RSS_UPDATE_RAMROD_DATA_NVGRE_KEY_ENTROPY_CAPABILITY (0x1<<9)
+#define ETH_RSS_UPDATE_RAMROD_DATA_NVGRE_KEY_ENTROPY_CAPABILITY_SHIFT 9
+#define ETH_RSS_UPDATE_RAMROD_DATA_GRE_INNER_HDRS_CAPABILITY (0x1<<10)
+#define ETH_RSS_UPDATE_RAMROD_DATA_GRE_INNER_HDRS_CAPABILITY_SHIFT 10
+#define ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY (0x1<<11)
+#define ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY_SHIFT 11
+#define ETH_RSS_UPDATE_RAMROD_DATA_RESERVED (0xF<<12)
+#define ETH_RSS_UPDATE_RAMROD_DATA_RESERVED_SHIFT 12
 	u8 rss_result_mask;
-	u8 rss_mode;
-	__le16 udp_4tuple_dst_port_mask;
-	__le16 udp_4tuple_dst_port_value;
+	u8 reserved3;
+	__le16 reserved4;
 	u8 indirection_table[T_ETH_INDIRECTION_TABLE_SIZE];
 	__le32 rss_key[T_ETH_RSS_KEY];
 	__le32 echo;
-	__le32 reserved3;
+	__le32 reserved5;
 };
 
 
@@ -4255,10 +4297,10 @@ enum eth_tunnel_lso_inc_ip_id {
 /* In case tunnel exist and L4 checksum offload,
  * the pseudo checksum location, on packet or on BD.
  */
-enum eth_tunnel_non_lso_pcsum_location {
-	PCSUM_ON_PKT,
-	PCSUM_ON_BD,
-	MAX_ETH_TUNNEL_NON_LSO_PCSUM_LOCATION
+enum eth_tunnel_non_lso_csum_location {
+	CSUM_ON_PKT,
+	CSUM_ON_BD,
+	MAX_ETH_TUNNEL_NON_LSO_CSUM_LOCATION
 };
 
 /*
@@ -4305,8 +4347,10 @@ struct eth_tx_start_bd {
 	__le16 vlan_or_ethertype;
 	struct eth_tx_bd_flags bd_flags;
 	u8 general_data;
-#define ETH_TX_START_BD_HDR_NBDS (0xF<<0)
+#define ETH_TX_START_BD_HDR_NBDS (0x7<<0)
 #define ETH_TX_START_BD_HDR_NBDS_SHIFT 0
+#define ETH_TX_START_BD_NO_ADDED_TAGS (0x1<<3)
+#define ETH_TX_START_BD_NO_ADDED_TAGS_SHIFT 3
 #define ETH_TX_START_BD_FORCE_VLAN_MODE (0x1<<4)
 #define ETH_TX_START_BD_FORCE_VLAN_MODE_SHIFT 4
 #define ETH_TX_START_BD_PARSE_NBDS (0x3<<5)
@@ -4382,8 +4426,8 @@ struct eth_tx_parse_2nd_bd {
 	__le16 global_data;
 #define ETH_TX_PARSE_2ND_BD_IP_HDR_START_OUTER_W (0xF<<0)
 #define ETH_TX_PARSE_2ND_BD_IP_HDR_START_OUTER_W_SHIFT 0
-#define ETH_TX_PARSE_2ND_BD_IP_HDR_TYPE_OUTER (0x1<<4)
-#define ETH_TX_PARSE_2ND_BD_IP_HDR_TYPE_OUTER_SHIFT 4
+#define ETH_TX_PARSE_2ND_BD_RESERVED0 (0x1<<4)
+#define ETH_TX_PARSE_2ND_BD_RESERVED0_SHIFT 4
 #define ETH_TX_PARSE_2ND_BD_LLC_SNAP_EN (0x1<<5)
 #define ETH_TX_PARSE_2ND_BD_LLC_SNAP_EN_SHIFT 5
 #define ETH_TX_PARSE_2ND_BD_NS_FLG (0x1<<6)
@@ -4392,9 +4436,14 @@ struct eth_tx_parse_2nd_bd {
 #define ETH_TX_PARSE_2ND_BD_TUNNEL_UDP_EXIST_SHIFT 7
 #define ETH_TX_PARSE_2ND_BD_IP_HDR_LEN_OUTER_W (0x1F<<8)
 #define ETH_TX_PARSE_2ND_BD_IP_HDR_LEN_OUTER_W_SHIFT 8
-#define ETH_TX_PARSE_2ND_BD_RESERVED0 (0x7<<13)
-#define ETH_TX_PARSE_2ND_BD_RESERVED0_SHIFT 13
-	__le16 reserved1;
+#define ETH_TX_PARSE_2ND_BD_RESERVED1 (0x7<<13)
+#define ETH_TX_PARSE_2ND_BD_RESERVED1_SHIFT 13
+	u8 bd_type;
+#define ETH_TX_PARSE_2ND_BD_TYPE (0xF<<0)
+#define ETH_TX_PARSE_2ND_BD_TYPE_SHIFT 0
+#define ETH_TX_PARSE_2ND_BD_RESERVED2 (0xF<<4)
+#define ETH_TX_PARSE_2ND_BD_RESERVED2_SHIFT 4
+	u8 reserved3;
 	u8 tcp_flags;
 #define ETH_TX_PARSE_2ND_BD_FIN_FLG (0x1<<0)
 #define ETH_TX_PARSE_2ND_BD_FIN_FLG_SHIFT 0
@@ -4412,7 +4461,7 @@ struct eth_tx_parse_2nd_bd {
 #define ETH_TX_PARSE_2ND_BD_ECE_FLG_SHIFT 6
 #define ETH_TX_PARSE_2ND_BD_CWR_FLG (0x1<<7)
 #define ETH_TX_PARSE_2ND_BD_CWR_FLG_SHIFT 7
-	u8 reserved2;
+	u8 reserved4;
 	u8 tunnel_udp_hdr_start_w;
 	u8 fw_ip_hdr_to_payload_w;
 	__le16 fw_ip_csum_wo_len_flags_frag;
@@ -5200,10 +5249,18 @@ struct function_start_data {
 	u8 path_id;
 	u8 network_cos_mode;
 	u8 dmae_cmd_id;
-	u8 gre_tunnel_mode;
-	u8 gre_tunnel_rss;
-	u8 nvgre_clss_en;
-	__le16 reserved1[2];
+	u8 tunnel_mode;
+	u8 gre_tunnel_type;
+	u8 tunn_clss_en;
+	u8 inner_gre_rss_en;
+	u8 sd_accept_mf_clss_fail;
+	__le16 vxlan_dst_port;
+	__le16 sd_accept_mf_clss_fail_ethtype;
+	__le16 sd_vlan_eth_type;
+	u8 sd_vlan_force_pri_flg;
+	u8 sd_vlan_force_pri_val;
+	u8 sd_accept_mf_clss_fail_match_ethtype;
+	u8 no_added_tags;
 };
 
 struct function_update_data {
@@ -5220,12 +5277,20 @@ struct function_update_data {
 	u8 tx_switch_suspend_change_flg;
 	u8 tx_switch_suspend;
 	u8 echo;
+	u8 update_tunn_cfg_flg;
+	u8 tunnel_mode;
+	u8 gre_tunnel_type;
+	u8 tunn_clss_en;
+	u8 inner_gre_rss_en;
+	__le16 vxlan_dst_port;
+	u8 sd_vlan_force_pri_change_flg;
+	u8 sd_vlan_force_pri_flg;
+	u8 sd_vlan_force_pri_val;
+	u8 sd_vlan_tag_change_flg;
+	u8 sd_vlan_eth_type_change_flg;
 	u8 reserved1;
-	u8 update_gre_cfg_flg;
-	u8 gre_tunnel_mode;
-	u8 gre_tunnel_rss;
-	u8 nvgre_clss_en;
-	u32 reserved3;
+	__le16 sd_vlan_tag;
+	__le16 sd_vlan_eth_type;
 };
 
 /*
@@ -5254,17 +5319,9 @@ struct fw_version {
 #define __FW_VERSION_RESERVED_SHIFT 4
 };
 
-/* GRE RSS Mode */
-enum gre_rss_mode {
-	GRE_OUTER_HEADERS_RSS,
-	GRE_INNER_HEADERS_RSS,
-	NVGRE_KEY_ENTROPY_RSS,
-	MAX_GRE_RSS_MODE
-};
 
 /* GRE Tunnel Mode */
 enum gre_tunnel_type {
-	NO_GRE_TUNNEL,
 	NVGRE_TUNNEL,
 	L2GRE_TUNNEL,
 	IPGRE_TUNNEL,
@@ -5437,6 +5494,7 @@ enum ip_ver {
  * Malicious VF error ID
  */
 enum malicious_vf_error_id {
+	MALICIOUS_VF_NO_ERROR,
 	VF_PF_CHANNEL_NOT_READY,
 	ETH_ILLEGAL_BD_LENGTHS,
 	ETH_PACKET_TOO_SHORT,
@@ -5597,6 +5655,16 @@ struct protocol_common_spe {
 	union protocol_common_specific_data data;
 };
 
+/* The data for the Set Timesync Ramrod */
+struct set_timesync_ramrod_data {
+	u8 drift_adjust_cmd;
+	u8 offset_cmd;
+	u8 add_sub_drift_adjust_value;
+	u8 drift_adjust_value;
+	u32 drift_adjust_period;
+	struct regpair offset_delta;
+};
+
 /*
  * The send queue element
  */
@@ -5719,10 +5787,38 @@ struct tstorm_vf_zone_data {
 	struct regpair reserved;
 };
 
+/* Add or Subtract Value for Set Timesync Ramrod */
+enum ts_add_sub_value {
+	TS_SUB_VALUE,
+	TS_ADD_VALUE,
+	MAX_TS_ADD_SUB_VALUE
+};
 
-/*
- * zone A per-queue data
- */
+/* Drift-Adjust Commands for Set Timesync Ramrod */
+enum ts_drift_adjust_cmd {
+	TS_DRIFT_ADJUST_KEEP,
+	TS_DRIFT_ADJUST_SET,
+	TS_DRIFT_ADJUST_RESET,
+	MAX_TS_DRIFT_ADJUST_CMD
+};
+
+/* Offset Commands for Set Timesync Ramrod */
+enum ts_offset_cmd {
+	TS_OFFSET_KEEP,
+	TS_OFFSET_INC,
+	TS_OFFSET_DEC,
+	MAX_TS_OFFSET_CMD
+};
+
+/* Tunnel Mode */
+enum tunnel_mode {
+	TUNN_MODE_NONE,
+	TUNN_MODE_VXLAN,
+	TUNN_MODE_GRE,
+	MAX_TUNNEL_MODE
+};
+
+ /* zone A per-queue data */
 struct ustorm_queue_zone_data {
 	struct ustorm_eth_rx_producers eth_rx_producers;
 	struct regpair reserved[3];
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
index 53fb4fa61b40..549549eaf580 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.c
@@ -154,15 +154,22 @@ typedef int (*read_sfp_module_eeprom_func_p)(struct bnx2x_phy *phy,
 			 LINK_STATUS_LINK_PARTNER_ASYMMETRIC_PAUSE)
 
 #define SFP_EEPROM_CON_TYPE_ADDR		0x2
+	#define SFP_EEPROM_CON_TYPE_VAL_UNKNOWN	0x0
 	#define SFP_EEPROM_CON_TYPE_VAL_LC	0x7
 	#define SFP_EEPROM_CON_TYPE_VAL_COPPER	0x21
 	#define SFP_EEPROM_CON_TYPE_VAL_RJ45	0x22
 
 
-#define SFP_EEPROM_COMP_CODE_ADDR		0x3
-	#define SFP_EEPROM_COMP_CODE_SR_MASK	(1<<4)
-	#define SFP_EEPROM_COMP_CODE_LR_MASK	(1<<5)
-	#define SFP_EEPROM_COMP_CODE_LRM_MASK	(1<<6)
+#define SFP_EEPROM_10G_COMP_CODE_ADDR		0x3
+	#define SFP_EEPROM_10G_COMP_CODE_SR_MASK	(1<<4)
+	#define SFP_EEPROM_10G_COMP_CODE_LR_MASK	(1<<5)
+	#define SFP_EEPROM_10G_COMP_CODE_LRM_MASK	(1<<6)
+
+#define SFP_EEPROM_1G_COMP_CODE_ADDR		0x6
+	#define SFP_EEPROM_1G_COMP_CODE_SX	(1<<0)
+	#define SFP_EEPROM_1G_COMP_CODE_LX	(1<<1)
+	#define SFP_EEPROM_1G_COMP_CODE_CX	(1<<2)
+	#define SFP_EEPROM_1G_COMP_CODE_BASE_T	(1<<3)
 
 #define SFP_EEPROM_FC_TX_TECH_ADDR		0x8
 	#define SFP_EEPROM_FC_TX_TECH_BITMASK_COPPER_PASSIVE 0x4
@@ -3633,8 +3640,8 @@ static void bnx2x_warpcore_enable_AN_KR2(struct bnx2x_phy *phy,
 				 reg_set[i].val);
 
 	/* Start KR2 work-around timer which handles BCM8073 link-parner */
-	vars->link_attr_sync |= LINK_ATTR_SYNC_KR2_ENABLE;
-	bnx2x_update_link_attr(params, vars->link_attr_sync);
+	params->link_attr_sync |= LINK_ATTR_SYNC_KR2_ENABLE;
+	bnx2x_update_link_attr(params, params->link_attr_sync);
 }
 
 static void bnx2x_disable_kr2(struct link_params *params,
@@ -3666,8 +3673,8 @@ static void bnx2x_disable_kr2(struct link_params *params,
 	for (i = 0; i < ARRAY_SIZE(reg_set); i++)
 		bnx2x_cl45_write(bp, phy, reg_set[i].devad, reg_set[i].reg,
 				 reg_set[i].val);
-	vars->link_attr_sync &= ~LINK_ATTR_SYNC_KR2_ENABLE;
-	bnx2x_update_link_attr(params, vars->link_attr_sync);
+	params->link_attr_sync &= ~LINK_ATTR_SYNC_KR2_ENABLE;
+	bnx2x_update_link_attr(params, params->link_attr_sync);
 
 	vars->check_kr2_recovery_cnt = CHECK_KR2_RECOVERY_CNT;
 }
@@ -4810,7 +4817,7 @@ void bnx2x_link_status_update(struct link_params *params,
 					~FEATURE_CONFIG_PFC_ENABLED;
 
 	if (SHMEM2_HAS(bp, link_attr_sync))
-		vars->link_attr_sync = SHMEM2_RD(bp,
+		params->link_attr_sync = SHMEM2_RD(bp,
 						 link_attr_sync[params->port]);
 
 	DP(NETIF_MSG_LINK, "link_status 0x%x  phy_link_up %x int_mask 0x%x\n",
@@ -8057,21 +8064,24 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 {
 	struct bnx2x *bp = params->bp;
 	u32 sync_offset = 0, phy_idx, media_types;
-	u8 gport, val[2], check_limiting_mode = 0;
+	u8 val[SFP_EEPROM_FC_TX_TECH_ADDR + 1], check_limiting_mode = 0;
 	*edc_mode = EDC_MODE_LIMITING;
 	phy->media_type = ETH_PHY_UNSPECIFIED;
 	/* First check for copper cable */
 	if (bnx2x_read_sfp_module_eeprom(phy,
 					 params,
 					 I2C_DEV_ADDR_A0,
-					 SFP_EEPROM_CON_TYPE_ADDR,
-					 2,
+					 0,
+					 SFP_EEPROM_FC_TX_TECH_ADDR + 1,
 					 (u8 *)val) != 0) {
 		DP(NETIF_MSG_LINK, "Failed to read from SFP+ module EEPROM\n");
 		return -EINVAL;
 	}
-
-	switch (val[0]) {
+	params->link_attr_sync &= ~LINK_SFP_EEPROM_COMP_CODE_MASK;
+	params->link_attr_sync |= val[SFP_EEPROM_10G_COMP_CODE_ADDR] <<
+		LINK_SFP_EEPROM_COMP_CODE_SHIFT;
+	bnx2x_update_link_attr(params, params->link_attr_sync);
+	switch (val[SFP_EEPROM_CON_TYPE_ADDR]) {
 	case SFP_EEPROM_CON_TYPE_VAL_COPPER:
 	{
 		u8 copper_module_type;
@@ -8079,17 +8089,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 		/* Check if its active cable (includes SFP+ module)
 		 * of passive cable
 		 */
-		if (bnx2x_read_sfp_module_eeprom(phy,
-					       params,
-					       I2C_DEV_ADDR_A0,
-					       SFP_EEPROM_FC_TX_TECH_ADDR,
-					       1,
-					       &copper_module_type) != 0) {
-			DP(NETIF_MSG_LINK,
-				"Failed to read copper-cable-type"
-				" from SFP+ EEPROM\n");
-			return -EINVAL;
-		}
+		copper_module_type = val[SFP_EEPROM_FC_TX_TECH_ADDR];
 
 		if (copper_module_type &
 		    SFP_EEPROM_FC_TX_TECH_BITMASK_COPPER_ACTIVE) {
@@ -8115,16 +8115,18 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 		}
 		break;
 	}
+	case SFP_EEPROM_CON_TYPE_VAL_UNKNOWN:
 	case SFP_EEPROM_CON_TYPE_VAL_LC:
 	case SFP_EEPROM_CON_TYPE_VAL_RJ45:
 		check_limiting_mode = 1;
-		if ((val[1] & (SFP_EEPROM_COMP_CODE_SR_MASK |
-			       SFP_EEPROM_COMP_CODE_LR_MASK |
-			       SFP_EEPROM_COMP_CODE_LRM_MASK)) == 0) {
+		if ((val[SFP_EEPROM_10G_COMP_CODE_ADDR] &
+		     (SFP_EEPROM_10G_COMP_CODE_SR_MASK |
+		      SFP_EEPROM_10G_COMP_CODE_LR_MASK |
+		      SFP_EEPROM_10G_COMP_CODE_LRM_MASK)) == 0) {
 			DP(NETIF_MSG_LINK, "1G SFP module detected\n");
-			gport = params->port;
 			phy->media_type = ETH_PHY_SFP_1G_FIBER;
 			if (phy->req_line_speed != SPEED_1000) {
+				u8 gport = params->port;
 				phy->req_line_speed = SPEED_1000;
 				if (!CHIP_IS_E1x(bp)) {
 					gport = BP_PATH(bp) +
@@ -8134,6 +8136,12 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 					   "Warning: Link speed was forced to 1000Mbps. Current SFP module in port %d is not compliant with 10G Ethernet\n",
 					   gport);
 			}
+			if (val[SFP_EEPROM_1G_COMP_CODE_ADDR] &
+			    SFP_EEPROM_1G_COMP_CODE_BASE_T) {
+				bnx2x_sfp_set_transmitter(params, phy, 0);
+				msleep(40);
+				bnx2x_sfp_set_transmitter(params, phy, 1);
+			}
 		} else {
 			int idx, cfg_idx = 0;
 			DP(NETIF_MSG_LINK, "10G Optic module detected\n");
@@ -8149,7 +8157,7 @@ static int bnx2x_get_edc_mode(struct bnx2x_phy *phy,
 		break;
 	default:
 		DP(NETIF_MSG_LINK, "Unable to determine module type 0x%x !!!\n",
-			 val[0]);
+			 val[SFP_EEPROM_CON_TYPE_ADDR]);
 		return -EINVAL;
 	}
 	sync_offset = params->shmem_base +
@@ -13507,7 +13515,7 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 
 	sigdet = bnx2x_warpcore_get_sigdet(phy, params);
 	if (!sigdet) {
-		if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
+		if (!(params->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
 			bnx2x_kr2_recovery(params, vars, phy);
 			DP(NETIF_MSG_LINK, "No sigdet\n");
 		}
@@ -13525,7 +13533,7 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 
 	/* CL73 has not begun yet */
 	if (base_page == 0) {
-		if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
+		if (!(params->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
 			bnx2x_kr2_recovery(params, vars, phy);
 			DP(NETIF_MSG_LINK, "No BP\n");
 		}
@@ -13541,7 +13549,7 @@ static void bnx2x_check_kr2_wa(struct link_params *params,
 			    ((next_page & 0xe0) == 0x20))));
 
 	/* In case KR2 is already disabled, check if we need to re-enable it */
-	if (!(vars->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
+	if (!(params->link_attr_sync & LINK_ATTR_SYNC_KR2_ENABLE)) {
 		if (!not_kr2_device) {
 			DP(NETIF_MSG_LINK, "BP=0x%x, NP=0x%x\n", base_page,
 			   next_page);
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
index 389f5f8cb0a3..d9cce4c3899b 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_link.h
@@ -323,6 +323,9 @@ struct link_params {
 #define LINK_FLAGS_INT_DISABLED		(1<<0)
 #define PHY_INITIALIZED		(1<<1)
 	u32 lfa_base;
+
+	/* The same definitions as the shmem2 parameter */
+	u32 link_attr_sync;
 };
 
 /* Output parameters */
@@ -364,8 +367,6 @@ struct link_vars {
 	u8 rx_tx_asic_rst;
 	u8 turn_to_run_wc_rt;
 	u16 rsrv2;
-	/* The same definitions as the shmem2 parameter */
-	u32 link_attr_sync;
 };
 
 /***********************************************************/
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
index c13364b6cc19..74fbf9ea7bd8 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_main.c
@@ -41,6 +41,7 @@
 #include <linux/ethtool.h>
 #include <linux/mii.h>
 #include <linux/if_vlan.h>
+#include <linux/crash_dump.h>
 #include <net/ip.h>
 #include <net/ipv6.h>
 #include <net/tcp.h>
@@ -63,7 +64,6 @@
 #include "bnx2x_vfpf.h"
 #include "bnx2x_dcb.h"
 #include "bnx2x_sp.h"
-
 #include <linux/firmware.h>
 #include "bnx2x_fw_file_hdr.h"
 /* FW files */
@@ -290,6 +290,8 @@ static int bnx2x_set_storm_rx_mode(struct bnx2x *bp);
 * General service functions
 ****************************************************************************/
 
+static int bnx2x_hwtstamp_ioctl(struct bnx2x *bp, struct ifreq *ifr);
+
 static void __storm_memset_dma_mapping(struct bnx2x *bp,
 				       u32 addr, dma_addr_t mapping)
 {
@@ -523,6 +525,7 @@ int bnx2x_issue_dmae_with_comp(struct bnx2x *bp, struct dmae_command *dmae,
 	 * as long as this code is called both from syscall context and
 	 * from ndo_set_rx_mode() flow that may be called from BH.
 	 */
+
 	spin_lock_bh(&bp->dmae_lock);
 
 	/* reset completion */
@@ -551,7 +554,9 @@ int bnx2x_issue_dmae_with_comp(struct bnx2x *bp, struct dmae_command *dmae,
 	}
 
 unlock:
+
 	spin_unlock_bh(&bp->dmae_lock);
+
 	return rc;
 }
 
@@ -646,119 +651,98 @@ static void bnx2x_write_dmae_phys_len(struct bnx2x *bp, dma_addr_t phys_addr,
 	bnx2x_write_dmae(bp, phys_addr + offset, addr + offset, len);
 }
 
+enum storms {
+	   XSTORM,
+	   TSTORM,
+	   CSTORM,
+	   USTORM,
+	   MAX_STORMS
+};
+
+#define STORMS_NUM 4
+#define REGS_IN_ENTRY 4
+
+static inline int bnx2x_get_assert_list_entry(struct bnx2x *bp,
+					      enum storms storm,
+					      int entry)
+{
+	switch (storm) {
+	case XSTORM:
+		return XSTORM_ASSERT_LIST_OFFSET(entry);
+	case TSTORM:
+		return TSTORM_ASSERT_LIST_OFFSET(entry);
+	case CSTORM:
+		return CSTORM_ASSERT_LIST_OFFSET(entry);
+	case USTORM:
+		return USTORM_ASSERT_LIST_OFFSET(entry);
+	case MAX_STORMS:
+	default:
+		BNX2X_ERR("unknown storm\n");
+	}
+	return -EINVAL;
+}
+
 static int bnx2x_mc_assert(struct bnx2x *bp)
 {
 	char last_idx;
-	int i, rc = 0;
-	u32 row0, row1, row2, row3;
-
-	/* XSTORM */
-	last_idx = REG_RD8(bp, BAR_XSTRORM_INTMEM +
-			   XSTORM_ASSERT_LIST_INDEX_OFFSET);
-	if (last_idx)
-		BNX2X_ERR("XSTORM_ASSERT_LIST_INDEX 0x%x\n", last_idx);
-
-	/* print the asserts */
-	for (i = 0; i < STROM_ASSERT_ARRAY_SIZE; i++) {
-
-		row0 = REG_RD(bp, BAR_XSTRORM_INTMEM +
-			      XSTORM_ASSERT_LIST_OFFSET(i));
-		row1 = REG_RD(bp, BAR_XSTRORM_INTMEM +
-			      XSTORM_ASSERT_LIST_OFFSET(i) + 4);
-		row2 = REG_RD(bp, BAR_XSTRORM_INTMEM +
-			      XSTORM_ASSERT_LIST_OFFSET(i) + 8);
-		row3 = REG_RD(bp, BAR_XSTRORM_INTMEM +
-			      XSTORM_ASSERT_LIST_OFFSET(i) + 12);
-
-		if (row0 != COMMON_ASM_INVALID_ASSERT_OPCODE) {
-			BNX2X_ERR("XSTORM_ASSERT_INDEX 0x%x = 0x%08x 0x%08x 0x%08x 0x%08x\n",
-				  i, row3, row2, row1, row0);
-			rc++;
-		} else {
-			break;
-		}
-	}
-
-	/* TSTORM */
-	last_idx = REG_RD8(bp, BAR_TSTRORM_INTMEM +
-			   TSTORM_ASSERT_LIST_INDEX_OFFSET);
-	if (last_idx)
-		BNX2X_ERR("TSTORM_ASSERT_LIST_INDEX 0x%x\n", last_idx);
-
-	/* print the asserts */
-	for (i = 0; i < STROM_ASSERT_ARRAY_SIZE; i++) {
-
-		row0 = REG_RD(bp, BAR_TSTRORM_INTMEM +
-			      TSTORM_ASSERT_LIST_OFFSET(i));
-		row1 = REG_RD(bp, BAR_TSTRORM_INTMEM +
-			      TSTORM_ASSERT_LIST_OFFSET(i) + 4);
-		row2 = REG_RD(bp, BAR_TSTRORM_INTMEM +
-			      TSTORM_ASSERT_LIST_OFFSET(i) + 8);
-		row3 = REG_RD(bp, BAR_TSTRORM_INTMEM +
-			      TSTORM_ASSERT_LIST_OFFSET(i) + 12);
-
-		if (row0 != COMMON_ASM_INVALID_ASSERT_OPCODE) {
-			BNX2X_ERR("TSTORM_ASSERT_INDEX 0x%x = 0x%08x 0x%08x 0x%08x 0x%08x\n",
-				  i, row3, row2, row1, row0);
-			rc++;
-		} else {
-			break;
-		}
-	}
+	int i, j, rc = 0;
+	enum storms storm;
+	u32 regs[REGS_IN_ENTRY];
+	u32 bar_storm_intmem[STORMS_NUM] = {
+		BAR_XSTRORM_INTMEM,
+		BAR_TSTRORM_INTMEM,
+		BAR_CSTRORM_INTMEM,
+		BAR_USTRORM_INTMEM
+	};
+	u32 storm_assert_list_index[STORMS_NUM] = {
+		XSTORM_ASSERT_LIST_INDEX_OFFSET,
+		TSTORM_ASSERT_LIST_INDEX_OFFSET,
+		CSTORM_ASSERT_LIST_INDEX_OFFSET,
+		USTORM_ASSERT_LIST_INDEX_OFFSET
+	};
+	char *storms_string[STORMS_NUM] = {
+		"XSTORM",
+		"TSTORM",
+		"CSTORM",
+		"USTORM"
+	};
 
-	/* CSTORM */
-	last_idx = REG_RD8(bp, BAR_CSTRORM_INTMEM +
-			   CSTORM_ASSERT_LIST_INDEX_OFFSET);
-	if (last_idx)
-		BNX2X_ERR("CSTORM_ASSERT_LIST_INDEX 0x%x\n", last_idx);
-
-	/* print the asserts */
-	for (i = 0; i < STROM_ASSERT_ARRAY_SIZE; i++) {
-
-		row0 = REG_RD(bp, BAR_CSTRORM_INTMEM +
-			      CSTORM_ASSERT_LIST_OFFSET(i));
-		row1 = REG_RD(bp, BAR_CSTRORM_INTMEM +
-			      CSTORM_ASSERT_LIST_OFFSET(i) + 4);
-		row2 = REG_RD(bp, BAR_CSTRORM_INTMEM +
-			      CSTORM_ASSERT_LIST_OFFSET(i) + 8);
-		row3 = REG_RD(bp, BAR_CSTRORM_INTMEM +
-			      CSTORM_ASSERT_LIST_OFFSET(i) + 12);
-
-		if (row0 != COMMON_ASM_INVALID_ASSERT_OPCODE) {
-			BNX2X_ERR("CSTORM_ASSERT_INDEX 0x%x = 0x%08x 0x%08x 0x%08x 0x%08x\n",
-				  i, row3, row2, row1, row0);
-			rc++;
-		} else {
-			break;
+	for (storm = XSTORM; storm < MAX_STORMS; storm++) {
+		last_idx = REG_RD8(bp, bar_storm_intmem[storm] +
+				   storm_assert_list_index[storm]);
+		if (last_idx)
+			BNX2X_ERR("%s_ASSERT_LIST_INDEX 0x%x\n",
+				  storms_string[storm], last_idx);
+
+		/* print the asserts */
+		for (i = 0; i < STROM_ASSERT_ARRAY_SIZE; i++) {
+			/* read a single assert entry */
+			for (j = 0; j < REGS_IN_ENTRY; j++)
+				regs[j] = REG_RD(bp, bar_storm_intmem[storm] +
+					  bnx2x_get_assert_list_entry(bp,
+								      storm,
+								      i) +
+					  sizeof(u32) * j);
+
+			/* log entry if it contains a valid assert */
+			if (regs[0] != COMMON_ASM_INVALID_ASSERT_OPCODE) {
+				BNX2X_ERR("%s_ASSERT_INDEX 0x%x = 0x%08x 0x%08x 0x%08x 0x%08x\n",
+					  storms_string[storm], i, regs[3],
+					  regs[2], regs[1], regs[0]);
+				rc++;
+			} else {
+				break;
+			}
 		}
 	}
 
-	/* USTORM */
-	last_idx = REG_RD8(bp, BAR_USTRORM_INTMEM +
-			   USTORM_ASSERT_LIST_INDEX_OFFSET);
-	if (last_idx)
-		BNX2X_ERR("USTORM_ASSERT_LIST_INDEX 0x%x\n", last_idx);
-
-	/* print the asserts */
-	for (i = 0; i < STROM_ASSERT_ARRAY_SIZE; i++) {
-
-		row0 = REG_RD(bp, BAR_USTRORM_INTMEM +
-			      USTORM_ASSERT_LIST_OFFSET(i));
-		row1 = REG_RD(bp, BAR_USTRORM_INTMEM +
-			      USTORM_ASSERT_LIST_OFFSET(i) + 4);
-		row2 = REG_RD(bp, BAR_USTRORM_INTMEM +
-			      USTORM_ASSERT_LIST_OFFSET(i) + 8);
-		row3 = REG_RD(bp, BAR_USTRORM_INTMEM +
-			      USTORM_ASSERT_LIST_OFFSET(i) + 12);
-
-		if (row0 != COMMON_ASM_INVALID_ASSERT_OPCODE) {
-			BNX2X_ERR("USTORM_ASSERT_INDEX 0x%x = 0x%08x 0x%08x 0x%08x 0x%08x\n",
-				  i, row3, row2, row1, row0);
-			rc++;
-		} else {
-			break;
-		}
-	}
+	BNX2X_ERR("Chip Revision: %s, FW Version: %d_%d_%d\n",
+		  CHIP_IS_E1(bp) ? "everest1" :
+		  CHIP_IS_E1H(bp) ? "everest1h" :
+		  CHIP_IS_E2(bp) ? "everest2" : "everest3",
+		  BCM_5710_FW_MAJOR_VERSION,
+		  BCM_5710_FW_MINOR_VERSION,
+		  BCM_5710_FW_REVISION_VERSION);
 
 	return rc;
 }
@@ -983,6 +967,12 @@ void bnx2x_panic_dump(struct bnx2x *bp, bool disable_int)
 		u32 *sb_data_p;
 		struct bnx2x_fp_txdata txdata;
 
+		if (!bp->fp)
+			break;
+
+		if (!fp->rx_cons_sb)
+			continue;
+
 		/* Rx */
 		BNX2X_ERR("fp%d: rx_bd_prod(0x%x)  rx_bd_cons(0x%x)  rx_comp_prod(0x%x)  rx_comp_cons(0x%x)  *rx_cons_sb(0x%x)\n",
 			  i, fp->rx_bd_prod, fp->rx_bd_cons,
@@ -995,7 +985,14 @@ void bnx2x_panic_dump(struct bnx2x *bp, bool disable_int)
 		/* Tx */
 		for_each_cos_in_tx_queue(fp, cos)
 		{
+			if (!fp->txdata_ptr[cos])
+				break;
+
 			txdata = *fp->txdata_ptr[cos];
+
+			if (!txdata.tx_cons_sb)
+				continue;
+
 			BNX2X_ERR("fp%d: tx_pkt_prod(0x%x)  tx_pkt_cons(0x%x)  tx_bd_prod(0x%x)  tx_bd_cons(0x%x)  *tx_cons_sb(0x%x)\n",
 				  i, txdata.tx_pkt_prod,
 				  txdata.tx_pkt_cons, txdata.tx_bd_prod,
@@ -1097,6 +1094,12 @@ void bnx2x_panic_dump(struct bnx2x *bp, bool disable_int)
 	for_each_valid_rx_queue(bp, i) {
 		struct bnx2x_fastpath *fp = &bp->fp[i];
 
+		if (!bp->fp)
+			break;
+
+		if (!fp->rx_cons_sb)
+			continue;
+
 		start = RX_BD(le16_to_cpu(*fp->rx_cons_sb) - 10);
 		end = RX_BD(le16_to_cpu(*fp->rx_cons_sb) + 503);
 		for (j = start; j != end; j = RX_BD(j + 1)) {
@@ -1130,9 +1133,19 @@ void bnx2x_panic_dump(struct bnx2x *bp, bool disable_int)
 	/* Tx */
 	for_each_valid_tx_queue(bp, i) {
 		struct bnx2x_fastpath *fp = &bp->fp[i];
+
+		if (!bp->fp)
+			break;
+
 		for_each_cos_in_tx_queue(fp, cos) {
 			struct bnx2x_fp_txdata *txdata = fp->txdata_ptr[cos];
 
+			if (!fp->txdata_ptr[cos])
+				break;
+
+			if (!txdata->tx_cons_sb)
+				continue;
+
 			start = TX_BD(le16_to_cpu(*txdata->tx_cons_sb) - 10);
 			end = TX_BD(le16_to_cpu(*txdata->tx_cons_sb) + 245);
 			for (j = start; j != end; j = TX_BD(j + 1)) {
@@ -2071,8 +2084,6 @@ int bnx2x_get_gpio(struct bnx2x *bp, int gpio_num, u8 port)
 	else
 		value = 0;
 
-	DP(NETIF_MSG_LINK, "pin %d  value 0x%x\n", gpio_num, value);
-
 	return value;
 }
 
@@ -2894,6 +2905,57 @@ static void bnx2x_handle_afex_cmd(struct bnx2x *bp, u32 cmd)
 	}
 }
 
+static void bnx2x_handle_update_svid_cmd(struct bnx2x *bp)
+{
+	struct bnx2x_func_switch_update_params *switch_update_params;
+	struct bnx2x_func_state_params func_params;
+
+	memset(&func_params, 0, sizeof(struct bnx2x_func_state_params));
+	switch_update_params = &func_params.params.switch_update;
+	func_params.f_obj = &bp->func_obj;
+	func_params.cmd = BNX2X_F_CMD_SWITCH_UPDATE;
+
+	if (IS_MF_UFP(bp)) {
+		int func = BP_ABS_FUNC(bp);
+		u32 val;
+
+		/* Re-learn the S-tag from shmem */
+		val = MF_CFG_RD(bp, func_mf_config[func].e1hov_tag) &
+				FUNC_MF_CFG_E1HOV_TAG_MASK;
+		if (val != FUNC_MF_CFG_E1HOV_TAG_DEFAULT) {
+			bp->mf_ov = val;
+		} else {
+			BNX2X_ERR("Got an SVID event, but no tag is configured in shmem\n");
+			goto fail;
+		}
+
+		/* Configure new S-tag in LLH */
+		REG_WR(bp, NIG_REG_LLH0_FUNC_VLAN_ID + BP_PORT(bp) * 8,
+		       bp->mf_ov);
+
+		/* Send Ramrod to update FW of change */
+		__set_bit(BNX2X_F_UPDATE_SD_VLAN_TAG_CHNG,
+			  &switch_update_params->changes);
+		switch_update_params->vlan = bp->mf_ov;
+
+		if (bnx2x_func_state_change(bp, &func_params) < 0) {
+			BNX2X_ERR("Failed to configure FW of S-tag Change to %02x\n",
+				  bp->mf_ov);
+			goto fail;
+		}
+
+		DP(BNX2X_MSG_MCP, "Configured S-tag %02x\n", bp->mf_ov);
+
+		bnx2x_fw_command(bp, DRV_MSG_CODE_OEM_UPDATE_SVID_OK, 0);
+
+		return;
+	}
+
+	/* not supported by SW yet */
+fail:
+	bnx2x_fw_command(bp, DRV_MSG_CODE_OEM_UPDATE_SVID_FAILURE, 0);
+}
+
 static void bnx2x_pmf_update(struct bnx2x *bp)
 {
 	int port = BP_PORT(bp);
@@ -3286,7 +3348,8 @@ static void bnx2x_e1h_enable(struct bnx2x *bp)
 {
 	int port = BP_PORT(bp);
 
-	REG_WR(bp, NIG_REG_LLH0_FUNC_EN + port*8, 1);
+	if (!(IS_MF_UFP(bp) && BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp)))
+		REG_WR(bp, NIG_REG_LLH0_FUNC_EN + port * 8, 1);
 
 	/* Tx queue should be only re-enabled */
 	netif_tx_wake_all_queues(bp->dev);
@@ -3641,14 +3704,30 @@ out:
 	   ethver, iscsiver, fcoever);
 }
 
-static void bnx2x_dcc_event(struct bnx2x *bp, u32 dcc_event)
+static void bnx2x_oem_event(struct bnx2x *bp, u32 event)
 {
-	DP(BNX2X_MSG_MCP, "dcc_event 0x%x\n", dcc_event);
+	u32 cmd_ok, cmd_fail;
 
-	if (dcc_event & DRV_STATUS_DCC_DISABLE_ENABLE_PF) {
+	/* sanity */
+	if (event & DRV_STATUS_DCC_EVENT_MASK &&
+	    event & DRV_STATUS_OEM_EVENT_MASK) {
+		BNX2X_ERR("Received simultaneous events %08x\n", event);
+		return;
+	}
 
-		/*
-		 * This is the only place besides the function initialization
+	if (event & DRV_STATUS_DCC_EVENT_MASK) {
+		cmd_fail = DRV_MSG_CODE_DCC_FAILURE;
+		cmd_ok = DRV_MSG_CODE_DCC_OK;
+	} else /* if (event & DRV_STATUS_OEM_EVENT_MASK) */ {
+		cmd_fail = DRV_MSG_CODE_OEM_FAILURE;
+		cmd_ok = DRV_MSG_CODE_OEM_OK;
+	}
+
+	DP(BNX2X_MSG_MCP, "oem_event 0x%x\n", event);
+
+	if (event & (DRV_STATUS_DCC_DISABLE_ENABLE_PF |
+		     DRV_STATUS_OEM_DISABLE_ENABLE_PF)) {
+		/* This is the only place besides the function initialization
 		 * where the bp->flags can change so it is done without any
 		 * locks
 		 */
@@ -3663,18 +3742,22 @@ static void bnx2x_dcc_event(struct bnx2x *bp, u32 dcc_event)
 
 			bnx2x_e1h_enable(bp);
 		}
-		dcc_event &= ~DRV_STATUS_DCC_DISABLE_ENABLE_PF;
+		event &= ~(DRV_STATUS_DCC_DISABLE_ENABLE_PF |
+			   DRV_STATUS_OEM_DISABLE_ENABLE_PF);
 	}
-	if (dcc_event & DRV_STATUS_DCC_BANDWIDTH_ALLOCATION) {
+
+	if (event & (DRV_STATUS_DCC_BANDWIDTH_ALLOCATION |
+		     DRV_STATUS_OEM_BANDWIDTH_ALLOCATION)) {
 		bnx2x_config_mf_bw(bp);
-		dcc_event &= ~DRV_STATUS_DCC_BANDWIDTH_ALLOCATION;
+		event &= ~(DRV_STATUS_DCC_BANDWIDTH_ALLOCATION |
+			   DRV_STATUS_OEM_BANDWIDTH_ALLOCATION);
 	}
 
 	/* Report results to MCP */
-	if (dcc_event)
-		bnx2x_fw_command(bp, DRV_MSG_CODE_DCC_FAILURE, 0);
+	if (event)
+		bnx2x_fw_command(bp, cmd_fail, 0);
 	else
-		bnx2x_fw_command(bp, DRV_MSG_CODE_DCC_OK, 0);
+		bnx2x_fw_command(bp, cmd_ok, 0);
 }
 
 /* must be called under the spq lock */
@@ -4156,9 +4239,12 @@ static void bnx2x_attn_int_deasserted3(struct bnx2x *bp, u32 attn)
 					func_mf_config[BP_ABS_FUNC(bp)].config);
 			val = SHMEM_RD(bp,
 				       func_mb[BP_FW_MB_IDX(bp)].drv_status);
-			if (val & DRV_STATUS_DCC_EVENT_MASK)
-				bnx2x_dcc_event(bp,
-					    (val & DRV_STATUS_DCC_EVENT_MASK));
+
+			if (val & (DRV_STATUS_DCC_EVENT_MASK |
+				   DRV_STATUS_OEM_EVENT_MASK))
+				bnx2x_oem_event(bp,
+					(val & (DRV_STATUS_DCC_EVENT_MASK |
+						DRV_STATUS_OEM_EVENT_MASK)));
 
 			if (val & DRV_STATUS_SET_MF_BW)
 				bnx2x_set_mf_bw(bp);
@@ -4184,6 +4270,10 @@ static void bnx2x_attn_int_deasserted3(struct bnx2x *bp, u32 attn)
 					val & DRV_STATUS_AFEX_EVENT_MASK);
 			if (val & DRV_STATUS_EEE_NEGOTIATION_RESULTS)
 				bnx2x_handle_eee_event(bp);
+
+			if (val & DRV_STATUS_OEM_UPDATE_SVID)
+				bnx2x_handle_update_svid_cmd(bp);
+
 			if (bp->link_vars.periodic_flags &
 			    PERIODIC_FLAGS_LINK_EVENT) {
 				/*  sync with link */
@@ -4678,7 +4768,7 @@ static bool bnx2x_check_blocks_with_parity2(struct bnx2x *bp, u32 sig,
 	for (i = 0; sig; i++) {
 		cur_bit = (0x1UL << i);
 		if (sig & cur_bit) {
-			res |= true; /* Each bit is real error! */
+			res = true; /* Each bit is real error! */
 			if (print) {
 				switch (cur_bit) {
 				case AEU_INPUTS_ATTN_BITS_CSEMI_PARITY_ERROR:
@@ -4757,21 +4847,21 @@ static bool bnx2x_check_blocks_with_parity3(struct bnx2x *bp, u32 sig,
 					_print_next_block((*par_num)++,
 							  "MCP ROM");
 				*global = true;
-				res |= true;
+				res = true;
 				break;
 			case AEU_INPUTS_ATTN_BITS_MCP_LATCHED_UMP_RX_PARITY:
 				if (print)
 					_print_next_block((*par_num)++,
 							  "MCP UMP RX");
 				*global = true;
-				res |= true;
+				res = true;
 				break;
 			case AEU_INPUTS_ATTN_BITS_MCP_LATCHED_UMP_TX_PARITY:
 				if (print)
 					_print_next_block((*par_num)++,
 							  "MCP UMP TX");
 				*global = true;
-				res |= true;
+				res = true;
 				break;
 			case AEU_INPUTS_ATTN_BITS_MCP_LATCHED_SCPAD_PARITY:
 				if (print)
@@ -4803,7 +4893,7 @@ static bool bnx2x_check_blocks_with_parity4(struct bnx2x *bp, u32 sig,
 	for (i = 0; sig; i++) {
 		cur_bit = (0x1UL << i);
 		if (sig & cur_bit) {
-			res |= true; /* Each bit is real error! */
+			res = true; /* Each bit is real error! */
 			if (print) {
 				switch (cur_bit) {
 				case AEU_INPUTS_ATTN_BITS_PGLUE_PARITY_ERROR:
@@ -5452,6 +5542,14 @@ static void bnx2x_eq_int(struct bnx2x *bp)
 				break;
 
 			goto next_spqe;
+
+		case EVENT_RING_OPCODE_SET_TIMESYNC:
+			DP(BNX2X_MSG_SP | BNX2X_MSG_PTP,
+			   "got set_timesync ramrod completion\n");
+			if (f_obj->complete_cmd(bp, f_obj,
+						BNX2X_F_CMD_SET_TIMESYNC))
+				break;
+			goto next_spqe;
 		}
 
 		switch (opcode | bp->state) {
@@ -6102,7 +6200,7 @@ static int bnx2x_fill_accept_flags(struct bnx2x *bp, u32 rx_mode,
 	}
 
 	/* Set ACCEPT_ANY_VLAN as we do not enable filtering by VLAN */
-	if (bp->rx_mode != BNX2X_RX_MODE_NONE) {
+	if (rx_mode != BNX2X_RX_MODE_NONE) {
 		__set_bit(BNX2X_ACCEPT_ANY_VLAN, rx_accept_flags);
 		__set_bit(BNX2X_ACCEPT_ANY_VLAN, tx_accept_flags);
 	}
@@ -6849,6 +6947,37 @@ static void bnx2x__common_init_phy(struct bnx2x *bp)
 	bnx2x_release_phy_lock(bp);
 }
 
+static void bnx2x_config_endianity(struct bnx2x *bp, u32 val)
+{
+	REG_WR(bp, PXP2_REG_RQ_QM_ENDIAN_M, val);
+	REG_WR(bp, PXP2_REG_RQ_TM_ENDIAN_M, val);
+	REG_WR(bp, PXP2_REG_RQ_SRC_ENDIAN_M, val);
+	REG_WR(bp, PXP2_REG_RQ_CDU_ENDIAN_M, val);
+	REG_WR(bp, PXP2_REG_RQ_DBG_ENDIAN_M, val);
+
+	/* make sure this value is 0 */
+	REG_WR(bp, PXP2_REG_RQ_HC_ENDIAN_M, 0);
+
+	REG_WR(bp, PXP2_REG_RD_QM_SWAP_MODE, val);
+	REG_WR(bp, PXP2_REG_RD_TM_SWAP_MODE, val);
+	REG_WR(bp, PXP2_REG_RD_SRC_SWAP_MODE, val);
+	REG_WR(bp, PXP2_REG_RD_CDURD_SWAP_MODE, val);
+}
+
+static void bnx2x_set_endianity(struct bnx2x *bp)
+{
+#ifdef __BIG_ENDIAN
+	bnx2x_config_endianity(bp, 1);
+#else
+	bnx2x_config_endianity(bp, 0);
+#endif
+}
+
+static void bnx2x_reset_endianity(struct bnx2x *bp)
+{
+	bnx2x_config_endianity(bp, 0);
+}
+
 /**
  * bnx2x_init_hw_common - initialize the HW at the COMMON phase.
  *
@@ -6915,23 +7044,7 @@ static int bnx2x_init_hw_common(struct bnx2x *bp)
 
 	bnx2x_init_block(bp, BLOCK_PXP2, PHASE_COMMON);
 	bnx2x_init_pxp(bp);
-
-#ifdef __BIG_ENDIAN
-	REG_WR(bp, PXP2_REG_RQ_QM_ENDIAN_M, 1);
-	REG_WR(bp, PXP2_REG_RQ_TM_ENDIAN_M, 1);
-	REG_WR(bp, PXP2_REG_RQ_SRC_ENDIAN_M, 1);
-	REG_WR(bp, PXP2_REG_RQ_CDU_ENDIAN_M, 1);
-	REG_WR(bp, PXP2_REG_RQ_DBG_ENDIAN_M, 1);
-	/* make sure this value is 0 */
-	REG_WR(bp, PXP2_REG_RQ_HC_ENDIAN_M, 0);
-
-/*	REG_WR(bp, PXP2_REG_RD_PBF_SWAP_MODE, 1); */
-	REG_WR(bp, PXP2_REG_RD_QM_SWAP_MODE, 1);
-	REG_WR(bp, PXP2_REG_RD_TM_SWAP_MODE, 1);
-	REG_WR(bp, PXP2_REG_RD_SRC_SWAP_MODE, 1);
-	REG_WR(bp, PXP2_REG_RD_CDURD_SWAP_MODE, 1);
-#endif
-
+	bnx2x_set_endianity(bp);
 	bnx2x_ilt_init_page_size(bp, INITOP_SET);
 
 	if (CHIP_REV_IS_FPGA(bp) && CHIP_IS_E1H(bp))
@@ -7647,7 +7760,11 @@ static inline int bnx2x_func_switch_update(struct bnx2x *bp, int suspend)
 	func_params.cmd = BNX2X_F_CMD_SWITCH_UPDATE;
 
 	/* Function parameters */
-	switch_update_params->suspend = suspend;
+	__set_bit(BNX2X_F_UPDATE_TX_SWITCH_SUSPEND_CHNG,
+		  &switch_update_params->changes);
+	if (suspend)
+		__set_bit(BNX2X_F_UPDATE_TX_SWITCH_SUSPEND,
+			  &switch_update_params->changes);
 
 	rc = bnx2x_func_state_change(bp, &func_params);
 
@@ -7892,8 +8009,11 @@ static int bnx2x_init_hw_func(struct bnx2x *bp)
 		REG_WR(bp, CFC_REG_WEAK_ENABLE_PF, 1);
 
 	if (IS_MF(bp)) {
-		REG_WR(bp, NIG_REG_LLH0_FUNC_EN + port*8, 1);
-		REG_WR(bp, NIG_REG_LLH0_FUNC_VLAN_ID + port*8, bp->mf_ov);
+		if (!(IS_MF_UFP(bp) && BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp))) {
+			REG_WR(bp, NIG_REG_LLH0_FUNC_EN + port * 8, 1);
+			REG_WR(bp, NIG_REG_LLH0_FUNC_VLAN_ID + port * 8,
+			       bp->mf_ov);
+		}
 	}
 
 	bnx2x_init_block(bp, BLOCK_MISC_AEU, init_phase);
@@ -8285,13 +8405,6 @@ int bnx2x_del_all_macs(struct bnx2x *bp,
 
 int bnx2x_set_eth_mac(struct bnx2x *bp, bool set)
 {
-	if (is_zero_ether_addr(bp->dev->dev_addr) &&
-	    (IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp))) {
-		DP(NETIF_MSG_IFUP | NETIF_MSG_IFDOWN,
-		   "Ignoring Zero MAC for STORAGE SD mode\n");
-		return 0;
-	}
-
 	if (IS_PF(bp)) {
 		unsigned long ramrod_flags = 0;
 
@@ -9010,7 +9123,7 @@ static int bnx2x_func_wait_started(struct bnx2x *bp)
 		struct bnx2x_func_state_params func_params = {NULL};
 
 		DP(NETIF_MSG_IFDOWN,
-		   "Hmmm... Unexpected function state! Forcing STARTED-->TX_ST0PPED-->STARTED\n");
+		   "Hmmm... Unexpected function state! Forcing STARTED-->TX_STOPPED-->STARTED\n");
 
 		func_params.f_obj = &bp->func_obj;
 		__set_bit(RAMROD_DRV_CLR_ONLY,
@@ -9029,6 +9142,48 @@ static int bnx2x_func_wait_started(struct bnx2x *bp)
 	return 0;
 }
 
+static void bnx2x_disable_ptp(struct bnx2x *bp)
+{
+	int port = BP_PORT(bp);
+
+	/* Disable sending PTP packets to host */
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_TO_HOST :
+	       NIG_REG_P0_LLH_PTP_TO_HOST, 0x0);
+
+	/* Reset PTP event detection rules */
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_PARAM_MASK :
+	       NIG_REG_P0_LLH_PTP_PARAM_MASK, 0x7FF);
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_RULE_MASK :
+	       NIG_REG_P0_LLH_PTP_RULE_MASK, 0x3FFF);
+	REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_PARAM_MASK :
+	       NIG_REG_P0_TLLH_PTP_PARAM_MASK, 0x7FF);
+	REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_RULE_MASK :
+	       NIG_REG_P0_TLLH_PTP_RULE_MASK, 0x3FFF);
+
+	/* Disable the PTP feature */
+	REG_WR(bp, port ? NIG_REG_P1_PTP_EN :
+	       NIG_REG_P0_PTP_EN, 0x0);
+}
+
+/* Called during unload, to stop PTP-related stuff */
+void bnx2x_stop_ptp(struct bnx2x *bp)
+{
+	/* Cancel PTP work queue. Should be done after the Tx queues are
+	 * drained to prevent additional scheduling.
+	 */
+	cancel_work_sync(&bp->ptp_task);
+
+	if (bp->ptp_tx_skb) {
+		dev_kfree_skb_any(bp->ptp_tx_skb);
+		bp->ptp_tx_skb = NULL;
+	}
+
+	/* Disable PTP in HW */
+	bnx2x_disable_ptp(bp);
+
+	DP(BNX2X_MSG_PTP, "PTP stop ended successfully\n");
+}
+
 void bnx2x_chip_cleanup(struct bnx2x *bp, int unload_mode, bool keep_link)
 {
 	int port = BP_PORT(bp);
@@ -9147,6 +9302,13 @@ unload_error:
 #endif
 	}
 
+	/* stop_ptp should be after the Tx queues are drained to prevent
+	 * scheduling to the cancelled PTP work queue. It should also be after
+	 * function stop ramrod is sent, since as part of this ramrod FW access
+	 * PTP registers.
+	 */
+	bnx2x_stop_ptp(bp);
+
 	/* Disable HW interrupts, NAPI */
 	bnx2x_netif_stop(bp, 1);
 	/* Delete all NAPI objects */
@@ -10052,6 +10214,8 @@ static void bnx2x_prev_unload_close_mac(struct bnx2x *bp,
 }
 
 #define BNX2X_PREV_UNDI_PROD_ADDR(p) (BAR_TSTRORM_INTMEM + 0x1508 + ((p) << 4))
+#define BNX2X_PREV_UNDI_PROD_ADDR_H(f) (BAR_TSTRORM_INTMEM + \
+					0x1848 + ((f) << 4))
 #define BNX2X_PREV_UNDI_RCQ(val)	((val) & 0xffff)
 #define BNX2X_PREV_UNDI_BD(val)		((val) >> 16 & 0xffff)
 #define BNX2X_PREV_UNDI_PROD(rcq, bd)	((bd) << 16 | (rcq))
@@ -10059,8 +10223,6 @@ static void bnx2x_prev_unload_close_mac(struct bnx2x *bp,
 #define BCM_5710_UNDI_FW_MF_MAJOR	(0x07)
 #define BCM_5710_UNDI_FW_MF_MINOR	(0x08)
 #define BCM_5710_UNDI_FW_MF_VERS	(0x05)
-#define BNX2X_PREV_UNDI_MF_PORT(p) (BAR_TSTRORM_INTMEM + 0x150c + ((p) << 4))
-#define BNX2X_PREV_UNDI_MF_FUNC(f) (BAR_TSTRORM_INTMEM + 0x184c + ((f) << 4))
 
 static bool bnx2x_prev_is_after_undi(struct bnx2x *bp)
 {
@@ -10079,72 +10241,25 @@ static bool bnx2x_prev_is_after_undi(struct bnx2x *bp)
 	return false;
 }
 
-static bool bnx2x_prev_unload_undi_fw_supports_mf(struct bnx2x *bp)
-{
-	u8 major, minor, version;
-	u32 fw;
-
-	/* Must check that FW is loaded */
-	if (!(REG_RD(bp, MISC_REG_RESET_REG_1) &
-	     MISC_REGISTERS_RESET_REG_1_RST_XSEM)) {
-		BNX2X_DEV_INFO("XSEM is reset - UNDI MF FW is not loaded\n");
-		return false;
-	}
-
-	/* Read Currently loaded FW version */
-	fw = REG_RD(bp, XSEM_REG_PRAM);
-	major = fw & 0xff;
-	minor = (fw >> 0x8) & 0xff;
-	version = (fw >> 0x10) & 0xff;
-	BNX2X_DEV_INFO("Loaded FW: 0x%08x: Major 0x%02x Minor 0x%02x Version 0x%02x\n",
-		       fw, major, minor, version);
-
-	if (major > BCM_5710_UNDI_FW_MF_MAJOR)
-		return true;
-
-	if ((major == BCM_5710_UNDI_FW_MF_MAJOR) &&
-	    (minor > BCM_5710_UNDI_FW_MF_MINOR))
-		return true;
-
-	if ((major == BCM_5710_UNDI_FW_MF_MAJOR) &&
-	    (minor == BCM_5710_UNDI_FW_MF_MINOR) &&
-	    (version >= BCM_5710_UNDI_FW_MF_VERS))
-		return true;
-
-	return false;
-}
-
-static void bnx2x_prev_unload_undi_mf(struct bnx2x *bp)
-{
-	int i;
-
-	/* Due to legacy (FW) code, the first function on each engine has a
-	 * different offset macro from the rest of the functions.
-	 * Setting this for all 8 functions is harmless regardless of whether
-	 * this is actually a multi-function device.
-	 */
-	for (i = 0; i < 2; i++)
-		REG_WR(bp, BNX2X_PREV_UNDI_MF_PORT(i), 1);
-
-	for (i = 2; i < 8; i++)
-		REG_WR(bp, BNX2X_PREV_UNDI_MF_FUNC(i - 2), 1);
-
-	BNX2X_DEV_INFO("UNDI FW (MF) set to discard\n");
-}
-
-static void bnx2x_prev_unload_undi_inc(struct bnx2x *bp, u8 port, u8 inc)
+static void bnx2x_prev_unload_undi_inc(struct bnx2x *bp, u8 inc)
 {
 	u16 rcq, bd;
-	u32 tmp_reg = REG_RD(bp, BNX2X_PREV_UNDI_PROD_ADDR(port));
+	u32 addr, tmp_reg;
 
+	if (BP_FUNC(bp) < 2)
+		addr = BNX2X_PREV_UNDI_PROD_ADDR(BP_PORT(bp));
+	else
+		addr = BNX2X_PREV_UNDI_PROD_ADDR_H(BP_FUNC(bp) - 2);
+
+	tmp_reg = REG_RD(bp, addr);
 	rcq = BNX2X_PREV_UNDI_RCQ(tmp_reg) + inc;
 	bd = BNX2X_PREV_UNDI_BD(tmp_reg) + inc;
 
 	tmp_reg = BNX2X_PREV_UNDI_PROD(rcq, bd);
-	REG_WR(bp, BNX2X_PREV_UNDI_PROD_ADDR(port), tmp_reg);
+	REG_WR(bp, addr, tmp_reg);
 
-	BNX2X_DEV_INFO("UNDI producer [%d] rings bd -> 0x%04x, rcq -> 0x%04x\n",
-		       port, bd, rcq);
+	BNX2X_DEV_INFO("UNDI producer [%d/%d][%08x] rings bd -> 0x%04x, rcq -> 0x%04x\n",
+		       BP_PORT(bp), BP_FUNC(bp), addr, bd, rcq);
 }
 
 static int bnx2x_prev_mcp_done(struct bnx2x *bp)
@@ -10383,7 +10498,6 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 	/* Reset should be performed after BRB is emptied */
 	if (reset_reg & MISC_REGISTERS_RESET_REG_1_RST_BRB1) {
 		u32 timer_count = 1000;
-		bool need_write = true;
 
 		/* Close the MAC Rx to prevent BRB from filling up */
 		bnx2x_prev_unload_close_mac(bp, &mac_vals);
@@ -10420,20 +10534,10 @@ static int bnx2x_prev_unload_common(struct bnx2x *bp)
 			else
 				timer_count--;
 
-			/* New UNDI FW supports MF and contains better
-			 * cleaning methods - might be redundant but harmless.
-			 */
-			if (bnx2x_prev_unload_undi_fw_supports_mf(bp)) {
-				if (need_write) {
-					bnx2x_prev_unload_undi_mf(bp);
-					need_write = false;
-				}
-			} else if (prev_undi) {
-				/* If UNDI resides in memory,
-				 * manually increment it
-				 */
-				bnx2x_prev_unload_undi_inc(bp, BP_PORT(bp), 1);
-			}
+			/* If UNDI resides in memory, manually increment it */
+			if (prev_undi)
+				bnx2x_prev_unload_undi_inc(bp, 1);
+
 			udelay(10);
 		}
 
@@ -11326,15 +11430,14 @@ static void bnx2x_get_fcoe_info(struct bnx2x *bp)
 				 dev_info.port_hw_config[port].
 				 fcoe_wwn_node_name_lower);
 	} else if (!IS_MF_SD(bp)) {
-		/*
-		 * Read the WWN info only if the FCoE feature is enabled for
+		/* Read the WWN info only if the FCoE feature is enabled for
 		 * this function.
 		 */
-		if (BNX2X_MF_EXT_PROTOCOL_FCOE(bp) && !CHIP_IS_E1x(bp))
+		if (BNX2X_HAS_MF_EXT_PROTOCOL_FCOE(bp))
+			bnx2x_get_ext_wwn_info(bp, func);
+	} else {
+		if (BNX2X_IS_MF_SD_PROTOCOL_FCOE(bp) && !CHIP_IS_E1x(bp))
 			bnx2x_get_ext_wwn_info(bp, func);
-
-	} else if (IS_MF_FCOE_SD(bp) && !CHIP_IS_E1x(bp)) {
-		bnx2x_get_ext_wwn_info(bp, func);
 	}
 
 	BNX2X_DEV_INFO("max_fcoe_conn 0x%x\n", bp->cnic_eth_dev.max_fcoe_conn);
@@ -11372,7 +11475,7 @@ static void bnx2x_get_cnic_mac_hwinfo(struct bnx2x *bp)
 		 * In non SD mode features configuration comes from struct
 		 * func_ext_config.
 		 */
-		if (!IS_MF_SD(bp) && !CHIP_IS_E1x(bp)) {
+		if (!IS_MF_SD(bp)) {
 			u32 cfg = MF_CFG_RD(bp, func_ext_config[func].func_cfg);
 			if (cfg & MACP_FUNC_CFG_FLAGS_ISCSI_OFFLOAD) {
 				val2 = MF_CFG_RD(bp, func_ext_config[func].
@@ -11491,7 +11594,7 @@ static void bnx2x_get_mac_hwinfo(struct bnx2x *bp)
 
 	memcpy(bp->link_params.mac_addr, bp->dev->dev_addr, ETH_ALEN);
 
-	if (!bnx2x_is_valid_ether_addr(bp, bp->dev->dev_addr))
+	if (!is_valid_ether_addr(bp->dev->dev_addr))
 		dev_err(&bp->pdev->dev,
 			"bad Ethernet MAC address configuration: %pM\n"
 			"change it manually before bringing up the appropriate network interface\n",
@@ -11521,11 +11624,27 @@ static bool bnx2x_get_dropless_info(struct bnx2x *bp)
 	return cfg;
 }
 
+static void validate_set_si_mode(struct bnx2x *bp)
+{
+	u8 func = BP_ABS_FUNC(bp);
+	u32 val;
+
+	val = MF_CFG_RD(bp, func_mf_config[func].mac_upper);
+
+	/* check for legal mac (upper bytes) */
+	if (val != 0xffff) {
+		bp->mf_mode = MULTI_FUNCTION_SI;
+		bp->mf_config[BP_VN(bp)] =
+			MF_CFG_RD(bp, func_mf_config[func].config);
+	} else
+		BNX2X_DEV_INFO("illegal MAC address for SI\n");
+}
+
 static int bnx2x_get_hwinfo(struct bnx2x *bp)
 {
 	int /*abs*/func = BP_ABS_FUNC(bp);
 	int vn;
-	u32 val = 0;
+	u32 val = 0, val2 = 0;
 	int rc = 0;
 
 	bnx2x_get_common_hwinfo(bp);
@@ -11605,6 +11724,7 @@ static int bnx2x_get_hwinfo(struct bnx2x *bp)
 
 	bp->mf_ov = 0;
 	bp->mf_mode = 0;
+	bp->mf_sub_mode = 0;
 	vn = BP_VN(bp);
 
 	if (!CHIP_IS_E1(bp) && !BP_NOMCP(bp)) {
@@ -11634,15 +11754,7 @@ static int bnx2x_get_hwinfo(struct bnx2x *bp)
 
 			switch (val) {
 			case SHARED_FEAT_CFG_FORCE_SF_MODE_SWITCH_INDEPT:
-				val = MF_CFG_RD(bp, func_mf_config[func].
-						mac_upper);
-				/* check for legal mac (upper bytes)*/
-				if (val != 0xffff) {
-					bp->mf_mode = MULTI_FUNCTION_SI;
-					bp->mf_config[vn] = MF_CFG_RD(bp,
-						   func_mf_config[func].config);
-				} else
-					BNX2X_DEV_INFO("illegal MAC address for SI\n");
+				validate_set_si_mode(bp);
 				break;
 			case SHARED_FEAT_CFG_FORCE_SF_MODE_AFEX_MODE:
 				if ((!CHIP_IS_E1x(bp)) &&
@@ -11670,9 +11782,33 @@ static int bnx2x_get_hwinfo(struct bnx2x *bp)
 				} else
 					BNX2X_DEV_INFO("illegal OV for SD\n");
 				break;
+			case SHARED_FEAT_CFG_FORCE_SF_MODE_UFP_MODE:
+				bp->mf_mode = MULTI_FUNCTION_SD;
+				bp->mf_sub_mode = SUB_MF_MODE_UFP;
+				bp->mf_config[vn] =
+					MF_CFG_RD(bp,
+						  func_mf_config[func].config);
+				break;
 			case SHARED_FEAT_CFG_FORCE_SF_MODE_FORCED_SF:
 				bp->mf_config[vn] = 0;
 				break;
+			case SHARED_FEAT_CFG_FORCE_SF_MODE_EXTENDED_MODE:
+				val2 = SHMEM_RD(bp,
+					dev_info.shared_hw_config.config_3);
+				val2 &= SHARED_HW_CFG_EXTENDED_MF_MODE_MASK;
+				switch (val2) {
+				case SHARED_HW_CFG_EXTENDED_MF_MODE_NPAR1_DOT_5:
+					validate_set_si_mode(bp);
+					bp->mf_sub_mode =
+							SUB_MF_MODE_NPAR1_DOT_5;
+					break;
+				default:
+					/* Unknown configuration */
+					bp->mf_config[vn] = 0;
+					BNX2X_DEV_INFO("unknown extended MF mode 0x%x\n",
+						       val);
+				}
+				break;
 			default:
 				/* Unknown configuration: reset mf_config */
 				bp->mf_config[vn] = 0;
@@ -11693,6 +11829,11 @@ static int bnx2x_get_hwinfo(struct bnx2x *bp)
 
 				BNX2X_DEV_INFO("MF OV for func %d is %d (0x%04x)\n",
 					       func, bp->mf_ov, bp->mf_ov);
+			} else if (bp->mf_sub_mode == SUB_MF_MODE_UFP) {
+				dev_err(&bp->pdev->dev,
+					"Unexpected - no valid MF OV for func %d in UFP mode\n",
+					func);
+				bp->path_has_ovlan = true;
 			} else {
 				dev_err(&bp->pdev->dev,
 					"No valid MF OV for func %d, aborting\n",
@@ -11941,9 +12082,9 @@ static int bnx2x_init_bp(struct bnx2x *bp)
 		dev_err(&bp->pdev->dev, "MCP disabled, must load devices in order!\n");
 
 	bp->disable_tpa = disable_tpa;
-	bp->disable_tpa |= IS_MF_STORAGE_SD(bp) || IS_MF_FCOE_AFEX(bp);
+	bp->disable_tpa |= !!IS_MF_STORAGE_ONLY(bp);
 	/* Reduce memory usage in kdump environment by disabling TPA */
-	bp->disable_tpa |= reset_devices;
+	bp->disable_tpa |= is_kdump_kernel();
 
 	/* Set TPA flags */
 	if (bp->disable_tpa) {
@@ -11961,7 +12102,7 @@ static int bnx2x_init_bp(struct bnx2x *bp)
 
 	bp->mrrs = mrrs;
 
-	bp->tx_ring_size = IS_MF_FCOE_AFEX(bp) ? 0 : MAX_TX_AVAIL;
+	bp->tx_ring_size = IS_MF_STORAGE_ONLY(bp) ? 0 : MAX_TX_AVAIL;
 	if (IS_VF(bp))
 		bp->rx_ring_size = MAX_RX_AVAIL;
 
@@ -12019,6 +12160,9 @@ static int bnx2x_init_bp(struct bnx2x *bp)
 
 	bp->dump_preset_idx = 1;
 
+	if (CHIP_IS_E3B0(bp))
+		bp->flags |= PTP_SUPPORTED;
+
 	return rc;
 }
 
@@ -12278,7 +12422,7 @@ void bnx2x_set_rx_mode_inner(struct bnx2x *bp)
 
 	bp->rx_mode = rx_mode;
 	/* handle ISCSI SD mode */
-	if (IS_MF_ISCSI_SD(bp))
+	if (IS_MF_ISCSI_ONLY(bp))
 		bp->rx_mode = BNX2X_RX_MODE_NONE;
 
 	/* Schedule the rx_mode command */
@@ -12351,13 +12495,17 @@ static int bnx2x_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 	struct bnx2x *bp = netdev_priv(dev);
 	struct mii_ioctl_data *mdio = if_mii(ifr);
 
-	DP(NETIF_MSG_LINK, "ioctl: phy id 0x%x, reg 0x%x, val_in 0x%x\n",
-	   mdio->phy_id, mdio->reg_num, mdio->val_in);
-
 	if (!netif_running(dev))
 		return -EAGAIN;
 
-	return mdio_mii_ioctl(&bp->mdio, mdio, cmd);
+	switch (cmd) {
+	case SIOCSHWTSTAMP:
+		return bnx2x_hwtstamp_ioctl(bp, ifr);
+	default:
+		DP(NETIF_MSG_LINK, "ioctl: phy id 0x%x, reg 0x%x, val_in 0x%x\n",
+		   mdio->phy_id, mdio->reg_num, mdio->val_in);
+		return mdio_mii_ioctl(&bp->mdio, mdio, cmd);
+	}
 }
 
 #ifdef CONFIG_NET_POLL_CONTROLLER
@@ -12381,7 +12529,7 @@ static int bnx2x_validate_addr(struct net_device *dev)
 	if (IS_VF(bp))
 		bnx2x_sample_bulletin(bp);
 
-	if (!bnx2x_is_valid_ether_addr(bp, dev->dev_addr)) {
+	if (!is_valid_ether_addr(dev->dev_addr)) {
 		BNX2X_ERR("Non-valid Ethernet address\n");
 		return -EADDRNOTAVAIL;
 	}
@@ -13001,6 +13149,191 @@ static int set_is_vf(int chip_id)
 	}
 }
 
+/* nig_tsgen registers relative address */
+#define tsgen_ctrl 0x0
+#define tsgen_freecount 0x10
+#define tsgen_synctime_t0 0x20
+#define tsgen_offset_t0 0x28
+#define tsgen_drift_t0 0x30
+#define tsgen_synctime_t1 0x58
+#define tsgen_offset_t1 0x60
+#define tsgen_drift_t1 0x68
+
+/* FW workaround for setting drift */
+static int bnx2x_send_update_drift_ramrod(struct bnx2x *bp, int drift_dir,
+					  int best_val, int best_period)
+{
+	struct bnx2x_func_state_params func_params = {NULL};
+	struct bnx2x_func_set_timesync_params *set_timesync_params =
+		&func_params.params.set_timesync;
+
+	/* Prepare parameters for function state transitions */
+	__set_bit(RAMROD_COMP_WAIT, &func_params.ramrod_flags);
+	__set_bit(RAMROD_RETRY, &func_params.ramrod_flags);
+
+	func_params.f_obj = &bp->func_obj;
+	func_params.cmd = BNX2X_F_CMD_SET_TIMESYNC;
+
+	/* Function parameters */
+	set_timesync_params->drift_adjust_cmd = TS_DRIFT_ADJUST_SET;
+	set_timesync_params->offset_cmd = TS_OFFSET_KEEP;
+	set_timesync_params->add_sub_drift_adjust_value =
+		drift_dir ? TS_ADD_VALUE : TS_SUB_VALUE;
+	set_timesync_params->drift_adjust_value = best_val;
+	set_timesync_params->drift_adjust_period = best_period;
+
+	return bnx2x_func_state_change(bp, &func_params);
+}
+
+static int bnx2x_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
+	int rc;
+	int drift_dir = 1;
+	int val, period, period1, period2, dif, dif1, dif2;
+	int best_dif = BNX2X_MAX_PHC_DRIFT, best_period = 0, best_val = 0;
+
+	DP(BNX2X_MSG_PTP, "PTP adjfreq called, ppb = %d\n", ppb);
+
+	if (!netif_running(bp->dev)) {
+		DP(BNX2X_MSG_PTP,
+		   "PTP adjfreq called while the interface is down\n");
+		return -EFAULT;
+	}
+
+	if (ppb < 0) {
+		ppb = -ppb;
+		drift_dir = 0;
+	}
+
+	if (ppb == 0) {
+		best_val = 1;
+		best_period = 0x1FFFFFF;
+	} else if (ppb >= BNX2X_MAX_PHC_DRIFT) {
+		best_val = 31;
+		best_period = 1;
+	} else {
+		/* Changed not to allow val = 8, 16, 24 as these values
+		 * are not supported in workaround.
+		 */
+		for (val = 0; val <= 31; val++) {
+			if ((val & 0x7) == 0)
+				continue;
+			period1 = val * 1000000 / ppb;
+			period2 = period1 + 1;
+			if (period1 != 0)
+				dif1 = ppb - (val * 1000000 / period1);
+			else
+				dif1 = BNX2X_MAX_PHC_DRIFT;
+			if (dif1 < 0)
+				dif1 = -dif1;
+			dif2 = ppb - (val * 1000000 / period2);
+			if (dif2 < 0)
+				dif2 = -dif2;
+			dif = (dif1 < dif2) ? dif1 : dif2;
+			period = (dif1 < dif2) ? period1 : period2;
+			if (dif < best_dif) {
+				best_dif = dif;
+				best_val = val;
+				best_period = period;
+			}
+		}
+	}
+
+	rc = bnx2x_send_update_drift_ramrod(bp, drift_dir, best_val,
+					    best_period);
+	if (rc) {
+		BNX2X_ERR("Failed to set drift\n");
+		return -EFAULT;
+	}
+
+	DP(BNX2X_MSG_PTP, "Configrued val = %d, period = %d\n", best_val,
+	   best_period);
+
+	return 0;
+}
+
+static int bnx2x_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
+	u64 now;
+
+	DP(BNX2X_MSG_PTP, "PTP adjtime called, delta = %llx\n", delta);
+
+	now = timecounter_read(&bp->timecounter);
+	now += delta;
+	/* Re-init the timecounter */
+	timecounter_init(&bp->timecounter, &bp->cyclecounter, now);
+
+	return 0;
+}
+
+static int bnx2x_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
+	u64 ns;
+	u32 remainder;
+
+	ns = timecounter_read(&bp->timecounter);
+
+	DP(BNX2X_MSG_PTP, "PTP gettime called, ns = %llu\n", ns);
+
+	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
+	ts->tv_nsec = remainder;
+
+	return 0;
+}
+
+static int bnx2x_ptp_settime(struct ptp_clock_info *ptp,
+			     const struct timespec *ts)
+{
+	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
+	u64 ns;
+
+	ns = ts->tv_sec * 1000000000ULL;
+	ns += ts->tv_nsec;
+
+	DP(BNX2X_MSG_PTP, "PTP settime called, ns = %llu\n", ns);
+
+	/* Re-init the timecounter */
+	timecounter_init(&bp->timecounter, &bp->cyclecounter, ns);
+
+	return 0;
+}
+
+/* Enable (or disable) ancillary features of the phc subsystem */
+static int bnx2x_ptp_enable(struct ptp_clock_info *ptp,
+			    struct ptp_clock_request *rq, int on)
+{
+	struct bnx2x *bp = container_of(ptp, struct bnx2x, ptp_clock_info);
+
+	BNX2X_ERR("PHC ancillary features are not supported\n");
+	return -ENOTSUPP;
+}
+
+void bnx2x_register_phc(struct bnx2x *bp)
+{
+	/* Fill the ptp_clock_info struct and register PTP clock*/
+	bp->ptp_clock_info.owner = THIS_MODULE;
+	snprintf(bp->ptp_clock_info.name, 16, "%s", bp->dev->name);
+	bp->ptp_clock_info.max_adj = BNX2X_MAX_PHC_DRIFT; /* In PPB */
+	bp->ptp_clock_info.n_alarm = 0;
+	bp->ptp_clock_info.n_ext_ts = 0;
+	bp->ptp_clock_info.n_per_out = 0;
+	bp->ptp_clock_info.pps = 0;
+	bp->ptp_clock_info.adjfreq = bnx2x_ptp_adjfreq;
+	bp->ptp_clock_info.adjtime = bnx2x_ptp_adjtime;
+	bp->ptp_clock_info.gettime = bnx2x_ptp_gettime;
+	bp->ptp_clock_info.settime = bnx2x_ptp_settime;
+	bp->ptp_clock_info.enable = bnx2x_ptp_enable;
+
+	bp->ptp_clock = ptp_clock_register(&bp->ptp_clock_info, &bp->pdev->dev);
+	if (IS_ERR(bp->ptp_clock)) {
+		bp->ptp_clock = NULL;
+		BNX2X_ERR("PTP clock registeration failed\n");
+	}
+}
+
 static int bnx2x_init_one(struct pci_dev *pdev,
 				    const struct pci_device_id *ent)
 {
@@ -13172,6 +13505,8 @@ static int bnx2x_init_one(struct pci_dev *pdev,
 		       "Unknown",
 		       dev->base_addr, bp->pdev->irq, dev->dev_addr);
 
+	bnx2x_register_phc(bp);
+
 	return 0;
 
 init_one_exit:
@@ -13198,6 +13533,11 @@ static void __bnx2x_remove(struct pci_dev *pdev,
 			   struct bnx2x *bp,
 			   bool remove_netdev)
 {
+	if (bp->ptp_clock) {
+		ptp_clock_unregister(bp->ptp_clock);
+		bp->ptp_clock = NULL;
+	}
+
 	/* Delete storage MAC address */
 	if (!NO_FCOE(bp)) {
 		rtnl_lock();
@@ -13227,9 +13567,15 @@ static void __bnx2x_remove(struct pci_dev *pdev,
 	bnx2x_iov_remove_one(bp);
 
 	/* Power on: we can't let PCI layer write to us while we are in D3 */
-	if (IS_PF(bp))
+	if (IS_PF(bp)) {
 		bnx2x_set_power_state(bp, PCI_D0);
 
+		/* Set endianity registers to reset values in case next driver
+		 * boots in different endianty environment.
+		 */
+		bnx2x_reset_endianity(bp);
+	}
+
 	/* Disable MSI/MSI-X */
 	bnx2x_disable_msi(bp);
 
@@ -14173,3 +14519,332 @@ int bnx2x_pretend_func(struct bnx2x *bp, u16 pretend_func_val)
 	REG_RD(bp, pretend_reg);
 	return 0;
 }
+
+static void bnx2x_ptp_task(struct work_struct *work)
+{
+	struct bnx2x *bp = container_of(work, struct bnx2x, ptp_task);
+	int port = BP_PORT(bp);
+	u32 val_seq;
+	u64 timestamp, ns;
+	struct skb_shared_hwtstamps shhwtstamps;
+
+	/* Read Tx timestamp registers */
+	val_seq = REG_RD(bp, port ? NIG_REG_P1_TLLH_PTP_BUF_SEQID :
+			 NIG_REG_P0_TLLH_PTP_BUF_SEQID);
+	if (val_seq & 0x10000) {
+		/* There is a valid timestamp value */
+		timestamp = REG_RD(bp, port ? NIG_REG_P1_TLLH_PTP_BUF_TS_MSB :
+				   NIG_REG_P0_TLLH_PTP_BUF_TS_MSB);
+		timestamp <<= 32;
+		timestamp |= REG_RD(bp, port ? NIG_REG_P1_TLLH_PTP_BUF_TS_LSB :
+				    NIG_REG_P0_TLLH_PTP_BUF_TS_LSB);
+		/* Reset timestamp register to allow new timestamp */
+		REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_BUF_SEQID :
+		       NIG_REG_P0_TLLH_PTP_BUF_SEQID, 0x10000);
+		ns = timecounter_cyc2time(&bp->timecounter, timestamp);
+
+		memset(&shhwtstamps, 0, sizeof(shhwtstamps));
+		shhwtstamps.hwtstamp = ns_to_ktime(ns);
+		skb_tstamp_tx(bp->ptp_tx_skb, &shhwtstamps);
+		dev_kfree_skb_any(bp->ptp_tx_skb);
+		bp->ptp_tx_skb = NULL;
+
+		DP(BNX2X_MSG_PTP, "Tx timestamp, timestamp cycles = %llu, ns = %llu\n",
+		   timestamp, ns);
+	} else {
+		DP(BNX2X_MSG_PTP, "There is no valid Tx timestamp yet\n");
+		/* Reschedule to keep checking for a valid timestamp value */
+		schedule_work(&bp->ptp_task);
+	}
+}
+
+void bnx2x_set_rx_ts(struct bnx2x *bp, struct sk_buff *skb)
+{
+	int port = BP_PORT(bp);
+	u64 timestamp, ns;
+
+	timestamp = REG_RD(bp, port ? NIG_REG_P1_LLH_PTP_HOST_BUF_TS_MSB :
+			    NIG_REG_P0_LLH_PTP_HOST_BUF_TS_MSB);
+	timestamp <<= 32;
+	timestamp |= REG_RD(bp, port ? NIG_REG_P1_LLH_PTP_HOST_BUF_TS_LSB :
+			    NIG_REG_P0_LLH_PTP_HOST_BUF_TS_LSB);
+
+	/* Reset timestamp register to allow new timestamp */
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_HOST_BUF_SEQID :
+	       NIG_REG_P0_LLH_PTP_HOST_BUF_SEQID, 0x10000);
+
+	ns = timecounter_cyc2time(&bp->timecounter, timestamp);
+
+	skb_hwtstamps(skb)->hwtstamp = ns_to_ktime(ns);
+
+	DP(BNX2X_MSG_PTP, "Rx timestamp, timestamp cycles = %llu, ns = %llu\n",
+	   timestamp, ns);
+}
+
+/* Read the PHC */
+static cycle_t bnx2x_cyclecounter_read(const struct cyclecounter *cc)
+{
+	struct bnx2x *bp = container_of(cc, struct bnx2x, cyclecounter);
+	int port = BP_PORT(bp);
+	u32 wb_data[2];
+	u64 phc_cycles;
+
+	REG_RD_DMAE(bp, port ? NIG_REG_TIMESYNC_GEN_REG + tsgen_synctime_t1 :
+		    NIG_REG_TIMESYNC_GEN_REG + tsgen_synctime_t0, wb_data, 2);
+	phc_cycles = wb_data[1];
+	phc_cycles = (phc_cycles << 32) + wb_data[0];
+
+	DP(BNX2X_MSG_PTP, "PHC read cycles = %llu\n", phc_cycles);
+
+	return phc_cycles;
+}
+
+static void bnx2x_init_cyclecounter(struct bnx2x *bp)
+{
+	memset(&bp->cyclecounter, 0, sizeof(bp->cyclecounter));
+	bp->cyclecounter.read = bnx2x_cyclecounter_read;
+	bp->cyclecounter.mask = CLOCKSOURCE_MASK(64);
+	bp->cyclecounter.shift = 1;
+	bp->cyclecounter.mult = 1;
+}
+
+static int bnx2x_send_reset_timesync_ramrod(struct bnx2x *bp)
+{
+	struct bnx2x_func_state_params func_params = {NULL};
+	struct bnx2x_func_set_timesync_params *set_timesync_params =
+		&func_params.params.set_timesync;
+
+	/* Prepare parameters for function state transitions */
+	__set_bit(RAMROD_COMP_WAIT, &func_params.ramrod_flags);
+	__set_bit(RAMROD_RETRY, &func_params.ramrod_flags);
+
+	func_params.f_obj = &bp->func_obj;
+	func_params.cmd = BNX2X_F_CMD_SET_TIMESYNC;
+
+	/* Function parameters */
+	set_timesync_params->drift_adjust_cmd = TS_DRIFT_ADJUST_RESET;
+	set_timesync_params->offset_cmd = TS_OFFSET_KEEP;
+
+	return bnx2x_func_state_change(bp, &func_params);
+}
+
+int bnx2x_enable_ptp_packets(struct bnx2x *bp)
+{
+	struct bnx2x_queue_state_params q_params;
+	int rc, i;
+
+	/* send queue update ramrod to enable PTP packets */
+	memset(&q_params, 0, sizeof(q_params));
+	__set_bit(RAMROD_COMP_WAIT, &q_params.ramrod_flags);
+	q_params.cmd = BNX2X_Q_CMD_UPDATE;
+	__set_bit(BNX2X_Q_UPDATE_PTP_PKTS_CHNG,
+		  &q_params.params.update.update_flags);
+	__set_bit(BNX2X_Q_UPDATE_PTP_PKTS,
+		  &q_params.params.update.update_flags);
+
+	/* send the ramrod on all the queues of the PF */
+	for_each_eth_queue(bp, i) {
+		struct bnx2x_fastpath *fp = &bp->fp[i];
+
+		/* Set the appropriate Queue object */
+		q_params.q_obj = &bnx2x_sp_obj(bp, fp).q_obj;
+
+		/* Update the Queue state */
+		rc = bnx2x_queue_state_change(bp, &q_params);
+		if (rc) {
+			BNX2X_ERR("Failed to enable PTP packets\n");
+			return rc;
+		}
+	}
+
+	return 0;
+}
+
+int bnx2x_configure_ptp_filters(struct bnx2x *bp)
+{
+	int port = BP_PORT(bp);
+	int rc;
+
+	if (!bp->hwtstamp_ioctl_called)
+		return 0;
+
+	switch (bp->tx_type) {
+	case HWTSTAMP_TX_ON:
+		bp->flags |= TX_TIMESTAMPING_EN;
+		REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_PARAM_MASK :
+		       NIG_REG_P0_TLLH_PTP_PARAM_MASK, 0x6AA);
+		REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_RULE_MASK :
+		       NIG_REG_P0_TLLH_PTP_RULE_MASK, 0x3EEE);
+		break;
+	case HWTSTAMP_TX_ONESTEP_SYNC:
+		BNX2X_ERR("One-step timestamping is not supported\n");
+		return -ERANGE;
+	}
+
+	switch (bp->rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		break;
+	case HWTSTAMP_FILTER_ALL:
+	case HWTSTAMP_FILTER_SOME:
+		bp->rx_filter = HWTSTAMP_FILTER_NONE;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+		bp->rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
+		/* Initialize PTP detection for UDP/IPv4 events */
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_PARAM_MASK :
+		       NIG_REG_P0_LLH_PTP_PARAM_MASK, 0x7EE);
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_RULE_MASK :
+		       NIG_REG_P0_LLH_PTP_RULE_MASK, 0x3FFE);
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+		bp->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
+		/* Initialize PTP detection for UDP/IPv4 or UDP/IPv6 events */
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_PARAM_MASK :
+		       NIG_REG_P0_LLH_PTP_PARAM_MASK, 0x7EA);
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_RULE_MASK :
+		       NIG_REG_P0_LLH_PTP_RULE_MASK, 0x3FEE);
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+		bp->rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
+		/* Initialize PTP detection L2 events */
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_PARAM_MASK :
+		       NIG_REG_P0_LLH_PTP_PARAM_MASK, 0x6BF);
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_RULE_MASK :
+		       NIG_REG_P0_LLH_PTP_RULE_MASK, 0x3EFF);
+
+		break;
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+		bp->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+		/* Initialize PTP detection L2, UDP/IPv4 or UDP/IPv6 events */
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_PARAM_MASK :
+		       NIG_REG_P0_LLH_PTP_PARAM_MASK, 0x6AA);
+		REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_RULE_MASK :
+		       NIG_REG_P0_LLH_PTP_RULE_MASK, 0x3EEE);
+		break;
+	}
+
+	/* Indicate to FW that this PF expects recorded PTP packets */
+	rc = bnx2x_enable_ptp_packets(bp);
+	if (rc)
+		return rc;
+
+	/* Enable sending PTP packets to host */
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_TO_HOST :
+	       NIG_REG_P0_LLH_PTP_TO_HOST, 0x1);
+
+	return 0;
+}
+
+static int bnx2x_hwtstamp_ioctl(struct bnx2x *bp, struct ifreq *ifr)
+{
+	struct hwtstamp_config config;
+	int rc;
+
+	DP(BNX2X_MSG_PTP, "HWTSTAMP IOCTL called\n");
+
+	if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+		return -EFAULT;
+
+	DP(BNX2X_MSG_PTP, "Requested tx_type: %d, requested rx_filters = %d\n",
+	   config.tx_type, config.rx_filter);
+
+	if (config.flags) {
+		BNX2X_ERR("config.flags is reserved for future use\n");
+		return -EINVAL;
+	}
+
+	bp->hwtstamp_ioctl_called = 1;
+	bp->tx_type = config.tx_type;
+	bp->rx_filter = config.rx_filter;
+
+	rc = bnx2x_configure_ptp_filters(bp);
+	if (rc)
+		return rc;
+
+	config.rx_filter = bp->rx_filter;
+
+	return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+		-EFAULT : 0;
+}
+
+/* Configrues HW for PTP */
+static int bnx2x_configure_ptp(struct bnx2x *bp)
+{
+	int rc, port = BP_PORT(bp);
+	u32 wb_data[2];
+
+	/* Reset PTP event detection rules - will be configured in the IOCTL */
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_PARAM_MASK :
+	       NIG_REG_P0_LLH_PTP_PARAM_MASK, 0x7FF);
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_RULE_MASK :
+	       NIG_REG_P0_LLH_PTP_RULE_MASK, 0x3FFF);
+	REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_PARAM_MASK :
+	       NIG_REG_P0_TLLH_PTP_PARAM_MASK, 0x7FF);
+	REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_RULE_MASK :
+	       NIG_REG_P0_TLLH_PTP_RULE_MASK, 0x3FFF);
+
+	/* Disable PTP packets to host - will be configured in the IOCTL*/
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_TO_HOST :
+	       NIG_REG_P0_LLH_PTP_TO_HOST, 0x0);
+
+	/* Enable the PTP feature */
+	REG_WR(bp, port ? NIG_REG_P1_PTP_EN :
+	       NIG_REG_P0_PTP_EN, 0x3F);
+
+	/* Enable the free-running counter */
+	wb_data[0] = 0;
+	wb_data[1] = 0;
+	REG_WR_DMAE(bp, NIG_REG_TIMESYNC_GEN_REG + tsgen_ctrl, wb_data, 2);
+
+	/* Reset drift register (offset register is not reset) */
+	rc = bnx2x_send_reset_timesync_ramrod(bp);
+	if (rc) {
+		BNX2X_ERR("Failed to reset PHC drift register\n");
+		return -EFAULT;
+	}
+
+	/* Reset possibly old timestamps */
+	REG_WR(bp, port ? NIG_REG_P1_LLH_PTP_HOST_BUF_SEQID :
+	       NIG_REG_P0_LLH_PTP_HOST_BUF_SEQID, 0x10000);
+	REG_WR(bp, port ? NIG_REG_P1_TLLH_PTP_BUF_SEQID :
+	       NIG_REG_P0_TLLH_PTP_BUF_SEQID, 0x10000);
+
+	return 0;
+}
+
+/* Called during load, to initialize PTP-related stuff */
+void bnx2x_init_ptp(struct bnx2x *bp)
+{
+	int rc;
+
+	/* Configure PTP in HW */
+	rc = bnx2x_configure_ptp(bp);
+	if (rc) {
+		BNX2X_ERR("Stopping PTP initialization\n");
+		return;
+	}
+
+	/* Init work queue for Tx timestamping */
+	INIT_WORK(&bp->ptp_task, bnx2x_ptp_task);
+
+	/* Init cyclecounter and timecounter. This is done only in the first
+	 * load. If done in every load, PTP application will fail when doing
+	 * unload / load (e.g. MTU change) while it is running.
+	 */
+	if (!bp->timecounter_init_done) {
+		bnx2x_init_cyclecounter(bp);
+		timecounter_init(&bp->timecounter, &bp->cyclecounter,
+				 ktime_to_ns(ktime_get_real()));
+		bp->timecounter_init_done = 1;
+	}
+
+	DP(BNX2X_MSG_PTP, "PTP initialization ended successfully\n");
+}
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
index 2beb5430b876..b0779d773343 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_reg.h
@@ -2182,6 +2182,45 @@
 #define NIG_REG_P0_HWPFC_ENABLE				 0x18078
 #define NIG_REG_P0_LLH_FUNC_MEM2				 0x18480
 #define NIG_REG_P0_LLH_FUNC_MEM2_ENABLE			 0x18440
+/* [RW 17] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * the host. Bits [15:0] return the sequence ID of the packet. Bit 16
+ * indicates the validity of the data in the buffer. Writing a 1 to bit 16
+ * will clear the buffer.
+ */
+#define NIG_REG_P0_LLH_PTP_HOST_BUF_SEQID			 0x1875c
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * the host. This location returns the lower 32 bits of timestamp value.
+ */
+#define NIG_REG_P0_LLH_PTP_HOST_BUF_TS_LSB			 0x18754
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * the host. This location returns the upper 32 bits of timestamp value.
+ */
+#define NIG_REG_P0_LLH_PTP_HOST_BUF_TS_MSB			 0x18758
+/* [RW 11] Mask register for the various parameters used in determining PTP
+ * packet presence. Set each bit to 1 to mask out the particular parameter.
+ * 0-IPv4 DA 0 of 224.0.1.129. 1-IPv4 DA 1 of 224.0.0.107. 2-IPv6 DA 0 of
+ * 0xFF0*:0:0:0:0:0:0:181. 3-IPv6 DA 1 of 0xFF02:0:0:0:0:0:0:6B. 4-UDP
+ * destination port 0 of 319. 5-UDP destination port 1 of 320. 6-MAC
+ * Ethertype 0 of 0x88F7. 7-configurable MAC Ethertype 1. 8-MAC DA 0 of
+ * 0x01-1B-19-00-00-00. 9-MAC DA 1 of 0x01-80-C2-00-00-0E. 10-configurable
+ * MAC DA 2. The reset default is set to mask out all parameters.
+ */
+#define NIG_REG_P0_LLH_PTP_PARAM_MASK				 0x187a0
+/* [RW 14] Mask regiser for the rules used in detecting PTP packets. Set
+ * each bit to 1 to mask out that particular rule. 0-{IPv4 DA 0; UDP DP 0} .
+ * 1-{IPv4 DA 0; UDP DP 1} . 2-{IPv4 DA 1; UDP DP 0} . 3-{IPv4 DA 1; UDP DP
+ * 1} . 4-{IPv6 DA 0; UDP DP 0} . 5-{IPv6 DA 0; UDP DP 1} . 6-{IPv6 DA 1;
+ * UDP DP 0} . 7-{IPv6 DA 1; UDP DP 1} . 8-{MAC DA 0; Ethertype 0} . 9-{MAC
+ * DA 1; Ethertype 0} . 10-{MAC DA 0; Ethertype 1} . 11-{MAC DA 1; Ethertype
+ * 1} . 12-{MAC DA 2; Ethertype 0} . 13-{MAC DA 2; Ethertype 1} . The reset
+ * default is to mask out all of the rules. Note that rules 0-3 are for IPv4
+ * packets only and require that the packet is IPv4 for the rules to match.
+ * Note that rules 4-7 are for IPv6 packets only and require that the packet
+ * is IPv6 for the rules to match.
+ */
+#define NIG_REG_P0_LLH_PTP_RULE_MASK				 0x187a4
+/* [RW 1] Set to 1 to enable PTP packets to be forwarded to the host. */
+#define NIG_REG_P0_LLH_PTP_TO_HOST				 0x187ac
 /* [RW 1] Input enable for RX MAC interface. */
 #define NIG_REG_P0_MAC_IN_EN					 0x185ac
 /* [RW 1] Output enable for TX MAC interface */
@@ -2194,6 +2233,17 @@
  * priority field is extracted from the outer-most VLAN in receive packet.
  * Only COS 0 and COS 1 are supported in E2. */
 #define NIG_REG_P0_PKT_PRIORITY_TO_COS				 0x18054
+/* [RW 6] Enable for TimeSync feature. Bits [2:0] are for RX side. Bits
+ * [5:3] are for TX side. Bit 0 enables TimeSync on RX side. Bit 1 enables
+ * V1 frame format in timesync event detection on RX side. Bit 2 enables V2
+ * frame format in timesync event detection on RX side. Bit 3 enables
+ * TimeSync on TX side. Bit 4 enables V1 frame format in timesync event
+ * detection on TX side. Bit 5 enables V2 frame format in timesync event
+ * detection on TX side. Note that for HW to detect PTP packet and extract
+ * data from the packet, at least one of the version bits of that traffic
+ * direction has to be enabled.
+ */
+#define NIG_REG_P0_PTP_EN					 0x18788
 /* [RW 16] Bit-map indicating which SAFC/PFC priorities to map to COS 0. A
  * priority is mapped to COS 0 when the corresponding mask bit is 1. More
  * than one bit may be set; allowing multiple priorities to be mapped to one
@@ -2300,7 +2350,46 @@
  * Ethernet header. */
 #define NIG_REG_P1_HDRS_AFTER_BASIC				 0x1818c
 #define NIG_REG_P1_LLH_FUNC_MEM2				 0x184c0
-#define NIG_REG_P1_LLH_FUNC_MEM2_ENABLE			 0x18460
+#define NIG_REG_P1_LLH_FUNC_MEM2_ENABLE			 0x18460a
+/* [RW 17] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * the host. Bits [15:0] return the sequence ID of the packet. Bit 16
+ * indicates the validity of the data in the buffer. Writing a 1 to bit 16
+ * will clear the buffer.
+ */
+#define NIG_REG_P1_LLH_PTP_HOST_BUF_SEQID			 0x18774
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * the host. This location returns the lower 32 bits of timestamp value.
+ */
+#define NIG_REG_P1_LLH_PTP_HOST_BUF_TS_LSB			 0x1876c
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * the host. This location returns the upper 32 bits of timestamp value.
+ */
+#define NIG_REG_P1_LLH_PTP_HOST_BUF_TS_MSB			 0x18770
+/* [RW 11] Mask register for the various parameters used in determining PTP
+ * packet presence. Set each bit to 1 to mask out the particular parameter.
+ * 0-IPv4 DA 0 of 224.0.1.129. 1-IPv4 DA 1 of 224.0.0.107. 2-IPv6 DA 0 of
+ * 0xFF0*:0:0:0:0:0:0:181. 3-IPv6 DA 1 of 0xFF02:0:0:0:0:0:0:6B. 4-UDP
+ * destination port 0 of 319. 5-UDP destination port 1 of 320. 6-MAC
+ * Ethertype 0 of 0x88F7. 7-configurable MAC Ethertype 1. 8-MAC DA 0 of
+ * 0x01-1B-19-00-00-00. 9-MAC DA 1 of 0x01-80-C2-00-00-0E. 10-configurable
+ * MAC DA 2. The reset default is set to mask out all parameters.
+ */
+#define NIG_REG_P1_LLH_PTP_PARAM_MASK				 0x187c8
+/* [RW 14] Mask regiser for the rules used in detecting PTP packets. Set
+ * each bit to 1 to mask out that particular rule. 0-{IPv4 DA 0; UDP DP 0} .
+ * 1-{IPv4 DA 0; UDP DP 1} . 2-{IPv4 DA 1; UDP DP 0} . 3-{IPv4 DA 1; UDP DP
+ * 1} . 4-{IPv6 DA 0; UDP DP 0} . 5-{IPv6 DA 0; UDP DP 1} . 6-{IPv6 DA 1;
+ * UDP DP 0} . 7-{IPv6 DA 1; UDP DP 1} . 8-{MAC DA 0; Ethertype 0} . 9-{MAC
+ * DA 1; Ethertype 0} . 10-{MAC DA 0; Ethertype 1} . 11-{MAC DA 1; Ethertype
+ * 1} . 12-{MAC DA 2; Ethertype 0} . 13-{MAC DA 2; Ethertype 1} . The reset
+ * default is to mask out all of the rules. Note that rules 0-3 are for IPv4
+ * packets only and require that the packet is IPv4 for the rules to match.
+ * Note that rules 4-7 are for IPv6 packets only and require that the packet
+ * is IPv6 for the rules to match.
+ */
+#define NIG_REG_P1_LLH_PTP_RULE_MASK				 0x187cc
+/* [RW 1] Set to 1 to enable PTP packets to be forwarded to the host. */
+#define NIG_REG_P1_LLH_PTP_TO_HOST				 0x187d4
 /* [RW 32] Specify the client number to be assigned to each priority of the
  * strict priority arbiter. This register specifies bits 31:0 of the 36-bit
  * value. Priority 0 is the highest priority. Bits [3:0] are for priority 0
@@ -2342,6 +2431,17 @@
  * priority field is extracted from the outer-most VLAN in receive packet.
  * Only COS 0 and COS 1 are supported in E2. */
 #define NIG_REG_P1_PKT_PRIORITY_TO_COS				 0x181a8
+/* [RW 6] Enable for TimeSync feature. Bits [2:0] are for RX side. Bits
+ * [5:3] are for TX side. Bit 0 enables TimeSync on RX side. Bit 1 enables
+ * V1 frame format in timesync event detection on RX side. Bit 2 enables V2
+ * frame format in timesync event detection on RX side. Bit 3 enables
+ * TimeSync on TX side. Bit 4 enables V1 frame format in timesync event
+ * detection on TX side. Bit 5 enables V2 frame format in timesync event
+ * detection on TX side. Note that for HW to detect PTP packet and extract
+ * data from the packet, at least one of the version bits of that traffic
+ * direction has to be enabled.
+ */
+#define NIG_REG_P1_PTP_EN					 0x187b0
 /* [RW 16] Bit-map indicating which SAFC/PFC priorities to map to COS 0. A
  * priority is mapped to COS 0 when the corresponding mask bit is 1. More
  * than one bit may be set; allowing multiple priorities to be mapped to one
@@ -2361,6 +2461,78 @@
 #define NIG_REG_P1_RX_MACFIFO_EMPTY				 0x1858c
 /* [R 1] TLLH FIFO is empty. */
 #define NIG_REG_P1_TLLH_FIFO_EMPTY				 0x18338
+/* [RW 19] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * TX side. Bits [15:0] reflect the sequence ID of the packet. Bit 16
+ * indicates the validity of the data in the buffer. Bit 17 indicates that
+ * the sequence ID is valid and it is waiting for the TX timestamp value.
+ * Bit 18 indicates whether the timestamp is from a SW request (value of 1)
+ * or HW request (value of 0). Writing a 1 to bit 16 will clear the buffer.
+ */
+#define NIG_REG_P0_TLLH_PTP_BUF_SEQID				 0x187e0
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * MCP. This location returns the lower 32 bits of timestamp value.
+ */
+#define NIG_REG_P0_TLLH_PTP_BUF_TS_LSB				 0x187d8
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * MCP. This location returns the upper 32 bits of timestamp value.
+ */
+#define NIG_REG_P0_TLLH_PTP_BUF_TS_MSB				 0x187dc
+/* [RW 11] Mask register for the various parameters used in determining PTP
+ * packet presence. Set each bit to 1 to mask out the particular parameter.
+ * 0-IPv4 DA 0 of 224.0.1.129. 1-IPv4 DA 1 of 224.0.0.107. 2-IPv6 DA 0 of
+ * 0xFF0*:0:0:0:0:0:0:181. 3-IPv6 DA 1 of 0xFF02:0:0:0:0:0:0:6B. 4-UDP
+ * destination port 0 of 319. 5-UDP destination port 1 of 320. 6-MAC
+ * Ethertype 0 of 0x88F7. 7-configurable MAC Ethertype 1. 8-MAC DA 0 of
+ * 0x01-1B-19-00-00-00. 9-MAC DA 1 of 0x01-80-C2-00-00-0E. 10-configurable
+ * MAC DA 2. The reset default is set to mask out all parameters.
+ */
+#define NIG_REG_P0_TLLH_PTP_PARAM_MASK				 0x187f0
+/* [RW 14] Mask regiser for the rules used in detecting PTP packets. Set
+ * each bit to 1 to mask out that particular rule. 0-{IPv4 DA 0; UDP DP 0} .
+ * 1-{IPv4 DA 0; UDP DP 1} . 2-{IPv4 DA 1; UDP DP 0} . 3-{IPv4 DA 1; UDP DP
+ * 1} . 4-{IPv6 DA 0; UDP DP 0} . 5-{IPv6 DA 0; UDP DP 1} . 6-{IPv6 DA 1;
+ * UDP DP 0} . 7-{IPv6 DA 1; UDP DP 1} . 8-{MAC DA 0; Ethertype 0} . 9-{MAC
+ * DA 1; Ethertype 0} . 10-{MAC DA 0; Ethertype 1} . 11-{MAC DA 1; Ethertype
+ * 1} . 12-{MAC DA 2; Ethertype 0} . 13-{MAC DA 2; Ethertype 1} . The reset
+ * default is to mask out all of the rules.
+ */
+#define NIG_REG_P0_TLLH_PTP_RULE_MASK				 0x187f4
+/* [RW 19] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * TX side. Bits [15:0] reflect the sequence ID of the packet. Bit 16
+ * indicates the validity of the data in the buffer. Bit 17 indicates that
+ * the sequence ID is valid and it is waiting for the TX timestamp value.
+ * Bit 18 indicates whether the timestamp is from a SW request (value of 1)
+ * or HW request (value of 0). Writing a 1 to bit 16 will clear the buffer.
+ */
+#define NIG_REG_P1_TLLH_PTP_BUF_SEQID				 0x187ec
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * MCP. This location returns the lower 32 bits of timestamp value.
+ */
+#define NIG_REG_P1_TLLH_PTP_BUF_TS_LSB				 0x187e4
+/* [R 32] Packet TimeSync information that is buffered in 1-deep FIFOs for
+ * MCP. This location returns the upper 32 bits of timestamp value.
+ */
+#define NIG_REG_P1_TLLH_PTP_BUF_TS_MSB				 0x187e8
+/* [RW 11] Mask register for the various parameters used in determining PTP
+ * packet presence. Set each bit to 1 to mask out the particular parameter.
+ * 0-IPv4 DA 0 of 224.0.1.129. 1-IPv4 DA 1 of 224.0.0.107. 2-IPv6 DA 0 of
+ * 0xFF0*:0:0:0:0:0:0:181. 3-IPv6 DA 1 of 0xFF02:0:0:0:0:0:0:6B. 4-UDP
+ * destination port 0 of 319. 5-UDP destination port 1 of 320. 6-MAC
+ * Ethertype 0 of 0x88F7. 7-configurable MAC Ethertype 1. 8-MAC DA 0 of
+ * 0x01-1B-19-00-00-00. 9-MAC DA 1 of 0x01-80-C2-00-00-0E. 10-configurable
+ * MAC DA 2. The reset default is set to mask out all parameters.
+ */
+#define NIG_REG_P1_TLLH_PTP_PARAM_MASK				 0x187f8
+/* [RW 14] Mask regiser for the rules used in detecting PTP packets. Set
+ * each bit to 1 to mask out that particular rule. 0-{IPv4 DA 0; UDP DP 0} .
+ * 1-{IPv4 DA 0; UDP DP 1} . 2-{IPv4 DA 1; UDP DP 0} . 3-{IPv4 DA 1; UDP DP
+ * 1} . 4-{IPv6 DA 0; UDP DP 0} . 5-{IPv6 DA 0; UDP DP 1} . 6-{IPv6 DA 1;
+ * UDP DP 0} . 7-{IPv6 DA 1; UDP DP 1} . 8-{MAC DA 0; Ethertype 0} . 9-{MAC
+ * DA 1; Ethertype 0} . 10-{MAC DA 0; Ethertype 1} . 11-{MAC DA 1; Ethertype
+ * 1} . 12-{MAC DA 2; Ethertype 0} . 13-{MAC DA 2; Ethertype 1} . The reset
+ * default is to mask out all of the rules.
+ */
+#define NIG_REG_P1_TLLH_PTP_RULE_MASK				 0x187fc
 /* [RW 32] Specify which of the credit registers the client is to be mapped
  * to. This register specifies bits 31:0 of the 36-bit value. Bits[3:0] are
  * for client 0; bits [35:32] are for client 8. For clients that are not
@@ -2513,6 +2685,10 @@
    swap is equal to SPIO pin that inputs from ifmux_serdes_swap. If 1 then
    ort swap is equal to ~nig_registers_port_swap.port_swap */
 #define NIG_REG_STRAP_OVERRIDE					 0x10398
+/* [WB 64] Addresses for TimeSync related registers in the timesync
+ * generator sub-module.
+ */
+#define NIG_REG_TIMESYNC_GEN_REG				 0x18800
 /* [RW 1] output enable for RX_XCM0 IF */
 #define NIG_REG_XCM0_OUT_EN					 0x100f0
 /* [RW 1] output enable for RX_XCM1 IF */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
index b1936044767a..7bc2924a7e24 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.c
@@ -4019,6 +4019,7 @@ static int bnx2x_setup_rss(struct bnx2x *bp,
 	struct bnx2x_raw_obj *r = &o->raw;
 	struct eth_rss_update_ramrod_data *data =
 		(struct eth_rss_update_ramrod_data *)(r->rdata);
+	u16 caps = 0;
 	u8 rss_mode = 0;
 	int rc;
 
@@ -4042,28 +4043,34 @@ static int bnx2x_setup_rss(struct bnx2x *bp,
 
 	/* RSS capabilities */
 	if (test_bit(BNX2X_RSS_IPV4, &p->rss_flags))
-		data->capabilities |=
-			ETH_RSS_UPDATE_RAMROD_DATA_IPV4_CAPABILITY;
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_IPV4_CAPABILITY;
 
 	if (test_bit(BNX2X_RSS_IPV4_TCP, &p->rss_flags))
-		data->capabilities |=
-			ETH_RSS_UPDATE_RAMROD_DATA_IPV4_TCP_CAPABILITY;
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_IPV4_TCP_CAPABILITY;
 
 	if (test_bit(BNX2X_RSS_IPV4_UDP, &p->rss_flags))
-		data->capabilities |=
-			ETH_RSS_UPDATE_RAMROD_DATA_IPV4_UDP_CAPABILITY;
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_IPV4_UDP_CAPABILITY;
 
 	if (test_bit(BNX2X_RSS_IPV6, &p->rss_flags))
-		data->capabilities |=
-			ETH_RSS_UPDATE_RAMROD_DATA_IPV6_CAPABILITY;
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_IPV6_CAPABILITY;
 
 	if (test_bit(BNX2X_RSS_IPV6_TCP, &p->rss_flags))
-		data->capabilities |=
-			ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY;
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_IPV6_TCP_CAPABILITY;
 
 	if (test_bit(BNX2X_RSS_IPV6_UDP, &p->rss_flags))
-		data->capabilities |=
-			ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY;
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_IPV6_UDP_CAPABILITY;
+
+	if (test_bit(BNX2X_RSS_GRE_INNER_HDRS, &p->rss_flags))
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_GRE_INNER_HDRS_CAPABILITY;
+
+	/* RSS keys */
+	if (test_bit(BNX2X_RSS_SET_SRCH, &p->rss_flags)) {
+		memcpy(&data->rss_key[0], &p->rss_key[0],
+		       sizeof(data->rss_key));
+		caps |= ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY;
+	}
+
+	data->capabilities = cpu_to_le16(caps);
 
 	/* Hashing mask */
 	data->rss_result_mask = p->rss_result_mask;
@@ -4084,13 +4091,6 @@ static int bnx2x_setup_rss(struct bnx2x *bp,
 	if (netif_msg_ifup(bp))
 		bnx2x_debug_print_ind_table(bp, p);
 
-	/* RSS keys */
-	if (test_bit(BNX2X_RSS_SET_SRCH, &p->rss_flags)) {
-		memcpy(&data->rss_key[0], &p->rss_key[0],
-		       sizeof(data->rss_key));
-		data->capabilities |= ETH_RSS_UPDATE_RAMROD_DATA_UPDATE_RSS_KEY;
-	}
-
 	/* No need for an explicit memory barrier here as long as we
 	 * ensure the ordering of writing to the SPQ element
 	 * and updating of the SPQ producer which involves a memory
@@ -4336,6 +4336,8 @@ static void bnx2x_q_fill_init_general_data(struct bnx2x *bp,
 		test_bit(BNX2X_Q_FLG_FCOE, flags) ?
 		LLFC_TRAFFIC_TYPE_FCOE : LLFC_TRAFFIC_TYPE_NW;
 
+	gen_data->fp_hsi_ver = ETH_FP_HSI_VERSION;
+
 	DP(BNX2X_MSG_SP, "flags: active %d, cos %d, stats en %d\n",
 	   gen_data->activate_flg, gen_data->cos, gen_data->statistics_en_flg);
 }
@@ -4357,12 +4359,13 @@ static void bnx2x_q_fill_init_tx_data(struct bnx2x_queue_sp_obj *o,
 		test_bit(BNX2X_Q_FLG_ANTI_SPOOF, flags);
 	tx_data->force_default_pri_flg =
 		test_bit(BNX2X_Q_FLG_FORCE_DEFAULT_PRI, flags);
-
+	tx_data->refuse_outband_vlan_flg =
+		test_bit(BNX2X_Q_FLG_REFUSE_OUTBAND_VLAN, flags);
 	tx_data->tunnel_lso_inc_ip_id =
 		test_bit(BNX2X_Q_FLG_TUN_INC_INNER_IP_ID, flags);
 	tx_data->tunnel_non_lso_pcsum_location =
-		test_bit(BNX2X_Q_FLG_PCSUM_ON_PKT, flags) ? PCSUM_ON_PKT :
-								  PCSUM_ON_BD;
+		test_bit(BNX2X_Q_FLG_PCSUM_ON_PKT, flags) ? CSUM_ON_PKT :
+							    CSUM_ON_BD;
 
 	tx_data->tx_status_block_id = params->fw_sb_id;
 	tx_data->tx_sb_index_number = params->sb_cq_index;
@@ -4722,6 +4725,12 @@ static void bnx2x_q_fill_update_data(struct bnx2x *bp,
 	data->tx_switching_change_flg =
 		test_bit(BNX2X_Q_UPDATE_TX_SWITCHING_CHNG,
 			 &params->update_flags);
+
+	/* PTP */
+	data->handle_ptp_pkts_flg =
+		test_bit(BNX2X_Q_UPDATE_PTP_PKTS, &params->update_flags);
+	data->handle_ptp_pkts_change_flg =
+		test_bit(BNX2X_Q_UPDATE_PTP_PKTS_CHNG, &params->update_flags);
 }
 
 static inline int bnx2x_q_send_update(struct bnx2x *bp,
@@ -5376,6 +5385,10 @@ static int bnx2x_func_chk_transition(struct bnx2x *bp,
 			 (!test_bit(BNX2X_F_CMD_STOP, &o->pending)))
 			next_state = BNX2X_F_STATE_STARTED;
 
+		else if ((cmd == BNX2X_F_CMD_SET_TIMESYNC) &&
+			 (!test_bit(BNX2X_F_CMD_STOP, &o->pending)))
+			next_state = BNX2X_F_STATE_STARTED;
+
 		else if (cmd == BNX2X_F_CMD_TX_STOP)
 			next_state = BNX2X_F_STATE_TX_STOPPED;
 
@@ -5385,6 +5398,10 @@ static int bnx2x_func_chk_transition(struct bnx2x *bp,
 		    (!test_bit(BNX2X_F_CMD_STOP, &o->pending)))
 			next_state = BNX2X_F_STATE_TX_STOPPED;
 
+		else if ((cmd == BNX2X_F_CMD_SET_TIMESYNC) &&
+			 (!test_bit(BNX2X_F_CMD_STOP, &o->pending)))
+			next_state = BNX2X_F_STATE_TX_STOPPED;
+
 		else if (cmd == BNX2X_F_CMD_TX_START)
 			next_state = BNX2X_F_STATE_STARTED;
 
@@ -5652,9 +5669,27 @@ static inline int bnx2x_func_send_start(struct bnx2x *bp,
 	rdata->sd_vlan_tag	= cpu_to_le16(start_params->sd_vlan_tag);
 	rdata->path_id		= BP_PATH(bp);
 	rdata->network_cos_mode	= start_params->network_cos_mode;
-	rdata->gre_tunnel_mode	= start_params->gre_tunnel_mode;
-	rdata->gre_tunnel_rss	= start_params->gre_tunnel_rss;
+	rdata->tunnel_mode	= start_params->tunnel_mode;
+	rdata->gre_tunnel_type	= start_params->gre_tunnel_type;
+	rdata->inner_gre_rss_en = start_params->inner_gre_rss_en;
+	rdata->vxlan_dst_port	= cpu_to_le16(4789);
+	rdata->sd_accept_mf_clss_fail = start_params->class_fail;
+	if (start_params->class_fail_ethtype) {
+		rdata->sd_accept_mf_clss_fail_match_ethtype = 1;
+		rdata->sd_accept_mf_clss_fail_ethtype =
+			cpu_to_le16(start_params->class_fail_ethtype);
+	}
 
+	rdata->sd_vlan_force_pri_flg = start_params->sd_vlan_force_pri;
+	rdata->sd_vlan_force_pri_val = start_params->sd_vlan_force_pri_val;
+	if (start_params->sd_vlan_eth_type)
+		rdata->sd_vlan_eth_type =
+			cpu_to_le16(start_params->sd_vlan_eth_type);
+	else
+		rdata->sd_vlan_eth_type =
+			cpu_to_le16(0x8100);
+
+	rdata->no_added_tags = start_params->no_added_tags;
 	/* No need for an explicit memory barrier here as long we would
 	 * need to ensure the ordering of writing to the SPQ element
 	 * and updating of the SPQ producer which involves a memory
@@ -5680,8 +5715,52 @@ static inline int bnx2x_func_send_switch_update(struct bnx2x *bp,
 	memset(rdata, 0, sizeof(*rdata));
 
 	/* Fill the ramrod data with provided parameters */
-	rdata->tx_switch_suspend_change_flg = 1;
-	rdata->tx_switch_suspend = switch_update_params->suspend;
+	if (test_bit(BNX2X_F_UPDATE_TX_SWITCH_SUSPEND_CHNG,
+		     &switch_update_params->changes)) {
+		rdata->tx_switch_suspend_change_flg = 1;
+		rdata->tx_switch_suspend =
+			test_bit(BNX2X_F_UPDATE_TX_SWITCH_SUSPEND,
+				 &switch_update_params->changes);
+	}
+
+	if (test_bit(BNX2X_F_UPDATE_SD_VLAN_TAG_CHNG,
+		     &switch_update_params->changes)) {
+		rdata->sd_vlan_tag_change_flg = 1;
+		rdata->sd_vlan_tag =
+			cpu_to_le16(switch_update_params->vlan);
+	}
+
+	if (test_bit(BNX2X_F_UPDATE_SD_VLAN_ETH_TYPE_CHNG,
+		     &switch_update_params->changes)) {
+		rdata->sd_vlan_eth_type_change_flg = 1;
+		rdata->sd_vlan_eth_type =
+			cpu_to_le16(switch_update_params->vlan_eth_type);
+	}
+
+	if (test_bit(BNX2X_F_UPDATE_VLAN_FORCE_PRIO_CHNG,
+		     &switch_update_params->changes)) {
+		rdata->sd_vlan_force_pri_change_flg = 1;
+		if (test_bit(BNX2X_F_UPDATE_VLAN_FORCE_PRIO_FLAG,
+			     &switch_update_params->changes))
+			rdata->sd_vlan_force_pri_flg = 1;
+		rdata->sd_vlan_force_pri_flg =
+			switch_update_params->vlan_force_prio;
+	}
+
+	if (test_bit(BNX2X_F_UPDATE_TUNNEL_CFG_CHNG,
+		     &switch_update_params->changes)) {
+		rdata->update_tunn_cfg_flg = 1;
+		if (test_bit(BNX2X_F_UPDATE_TUNNEL_CLSS_EN,
+			     &switch_update_params->changes))
+			rdata->tunn_clss_en = 1;
+		if (test_bit(BNX2X_F_UPDATE_TUNNEL_INNER_GRE_RSS_EN,
+			     &switch_update_params->changes))
+			rdata->inner_gre_rss_en = 1;
+		rdata->tunnel_mode = switch_update_params->tunnel_mode;
+		rdata->gre_tunnel_type = switch_update_params->gre_tunnel_type;
+		rdata->vxlan_dst_port = cpu_to_le16(4789);
+	}
+
 	rdata->echo = SWITCH_UPDATE;
 
 	/* No need for an explicit memory barrier here as long as we
@@ -5817,6 +5896,42 @@ static inline int bnx2x_func_send_tx_start(struct bnx2x *bp,
 			     U64_LO(data_mapping), NONE_CONNECTION_TYPE);
 }
 
+static inline
+int bnx2x_func_send_set_timesync(struct bnx2x *bp,
+				 struct bnx2x_func_state_params *params)
+{
+	struct bnx2x_func_sp_obj *o = params->f_obj;
+	struct set_timesync_ramrod_data *rdata =
+		(struct set_timesync_ramrod_data *)o->rdata;
+	dma_addr_t data_mapping = o->rdata_mapping;
+	struct bnx2x_func_set_timesync_params *set_timesync_params =
+		&params->params.set_timesync;
+
+	memset(rdata, 0, sizeof(*rdata));
+
+	/* Fill the ramrod data with provided parameters */
+	rdata->drift_adjust_cmd = set_timesync_params->drift_adjust_cmd;
+	rdata->offset_cmd = set_timesync_params->offset_cmd;
+	rdata->add_sub_drift_adjust_value =
+		set_timesync_params->add_sub_drift_adjust_value;
+	rdata->drift_adjust_value = set_timesync_params->drift_adjust_value;
+	rdata->drift_adjust_period = set_timesync_params->drift_adjust_period;
+	rdata->offset_delta.lo =
+		cpu_to_le32(U64_LO(set_timesync_params->offset_delta));
+	rdata->offset_delta.hi =
+		cpu_to_le32(U64_HI(set_timesync_params->offset_delta));
+
+	DP(BNX2X_MSG_SP, "Set timesync command params: drift_cmd = %d, offset_cmd = %d, add_sub_drift = %d, drift_val = %d, drift_period = %d, offset_lo = %d, offset_hi = %d\n",
+	   rdata->drift_adjust_cmd, rdata->offset_cmd,
+	   rdata->add_sub_drift_adjust_value, rdata->drift_adjust_value,
+	   rdata->drift_adjust_period, rdata->offset_delta.lo,
+	   rdata->offset_delta.hi);
+
+	return bnx2x_sp_post(bp, RAMROD_CMD_ID_COMMON_SET_TIMESYNC, 0,
+			     U64_HI(data_mapping),
+			     U64_LO(data_mapping), NONE_CONNECTION_TYPE);
+}
+
 static int bnx2x_func_send_cmd(struct bnx2x *bp,
 			       struct bnx2x_func_state_params *params)
 {
@@ -5839,6 +5954,8 @@ static int bnx2x_func_send_cmd(struct bnx2x *bp,
 		return bnx2x_func_send_tx_start(bp, params);
 	case BNX2X_F_CMD_SWITCH_UPDATE:
 		return bnx2x_func_send_switch_update(bp, params);
+	case BNX2X_F_CMD_SET_TIMESYNC:
+		return bnx2x_func_send_set_timesync(bp, params);
 	default:
 		BNX2X_ERR("Unknown command: %d\n", params->cmd);
 		return -EINVAL;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
index 718ecd294661..e97275f456c0 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sp.h
@@ -711,6 +711,7 @@ enum {
 	BNX2X_RSS_IPV6,
 	BNX2X_RSS_IPV6_TCP,
 	BNX2X_RSS_IPV6_UDP,
+	BNX2X_RSS_GRE_INNER_HDRS,
 };
 
 struct bnx2x_config_rss_params {
@@ -769,7 +770,9 @@ enum {
 	BNX2X_Q_UPDATE_SILENT_VLAN_REM_CHNG,
 	BNX2X_Q_UPDATE_SILENT_VLAN_REM,
 	BNX2X_Q_UPDATE_TX_SWITCHING_CHNG,
-	BNX2X_Q_UPDATE_TX_SWITCHING
+	BNX2X_Q_UPDATE_TX_SWITCHING,
+	BNX2X_Q_UPDATE_PTP_PKTS_CHNG,
+	BNX2X_Q_UPDATE_PTP_PKTS,
 };
 
 /* Allowed Queue states */
@@ -831,6 +834,7 @@ enum {
 	BNX2X_Q_FLG_ANTI_SPOOF,
 	BNX2X_Q_FLG_SILENT_VLAN_REM,
 	BNX2X_Q_FLG_FORCE_DEFAULT_PRI,
+	BNX2X_Q_FLG_REFUSE_OUTBAND_VLAN,
 	BNX2X_Q_FLG_PCSUM_ON_PKT,
 	BNX2X_Q_FLG_TUN_INC_INNER_IP_ID
 };
@@ -851,6 +855,10 @@ enum bnx2x_q_type {
 #define BNX2X_MULTI_TX_COS			3 /* Maximum possible */
 
 #define MAC_PAD (ALIGN(ETH_ALEN, sizeof(u32)) - ETH_ALEN)
+/* DMAE channel to be used by FW for timesync workaroun. A driver that sends
+ * timesync-related ramrods must not use this DMAE command ID.
+ */
+#define FW_DMAE_CMD_ID 6
 
 struct bnx2x_queue_init_params {
 	struct {
@@ -1085,6 +1093,20 @@ struct bnx2x_queue_sp_obj {
 };
 
 /********************** Function state update *********************************/
+
+/* UPDATE command options */
+enum {
+	BNX2X_F_UPDATE_TX_SWITCH_SUSPEND_CHNG,
+	BNX2X_F_UPDATE_TX_SWITCH_SUSPEND,
+	BNX2X_F_UPDATE_SD_VLAN_TAG_CHNG,
+	BNX2X_F_UPDATE_SD_VLAN_ETH_TYPE_CHNG,
+	BNX2X_F_UPDATE_VLAN_FORCE_PRIO_CHNG,
+	BNX2X_F_UPDATE_VLAN_FORCE_PRIO_FLAG,
+	BNX2X_F_UPDATE_TUNNEL_CFG_CHNG,
+	BNX2X_F_UPDATE_TUNNEL_CLSS_EN,
+	BNX2X_F_UPDATE_TUNNEL_INNER_GRE_RSS_EN,
+};
+
 /* Allowed Function states */
 enum bnx2x_func_state {
 	BNX2X_F_STATE_RESET,
@@ -1105,6 +1127,7 @@ enum bnx2x_func_cmd {
 	BNX2X_F_CMD_TX_STOP,
 	BNX2X_F_CMD_TX_START,
 	BNX2X_F_CMD_SWITCH_UPDATE,
+	BNX2X_F_CMD_SET_TIMESYNC,
 	BNX2X_F_CMD_MAX,
 };
 
@@ -1146,18 +1169,44 @@ struct bnx2x_func_start_params {
 	/* Function cos mode */
 	u8 network_cos_mode;
 
-	/* NVGRE classification enablement */
-	u8 nvgre_clss_en;
+	/* TUNN_MODE_NONE/TUNN_MODE_VXLAN/TUNN_MODE_GRE */
+	u8 tunnel_mode;
 
-	/* NO_GRE_TUNNEL/NVGRE_TUNNEL/L2GRE_TUNNEL/IPGRE_TUNNEL */
-	u8 gre_tunnel_mode;
+	/* tunneling classification enablement */
+	u8 tunn_clss_en;
+
+	/* NVGRE_TUNNEL/L2GRE_TUNNEL/IPGRE_TUNNEL */
+	u8 gre_tunnel_type;
+
+	/* Enables Inner GRE RSS on the function, depends on the client RSS
+	 * capailities
+	 */
+	u8 inner_gre_rss_en;
+
+	/* Allows accepting of packets failing MF classification, possibly
+	 * only matching a given ethertype
+	 */
+	u8 class_fail;
+	u16 class_fail_ethtype;
 
-	/* GRE_OUTER_HEADERS_RSS/GRE_INNER_HEADERS_RSS/NVGRE_KEY_ENTROPY_RSS */
-	u8 gre_tunnel_rss;
+	/* Override priority of output packets */
+	u8 sd_vlan_force_pri;
+	u8 sd_vlan_force_pri_val;
+
+	/* Replace vlan's ethertype */
+	u16 sd_vlan_eth_type;
+
+	/* Prevent inner vlans from being added by FW */
+	u8 no_added_tags;
 };
 
 struct bnx2x_func_switch_update_params {
-	u8 suspend;
+	unsigned long changes; /* BNX2X_F_UPDATE_XX bits */
+	u16 vlan;
+	u16 vlan_eth_type;
+	u8 vlan_force_prio;
+	u8 tunnel_mode;
+	u8 gre_tunnel_type;
 };
 
 struct bnx2x_func_afex_update_params {
@@ -1172,6 +1221,7 @@ struct bnx2x_func_afex_viflists_params {
 	u8 afex_vif_list_command;
 	u8 func_to_clear;
 };
+
 struct bnx2x_func_tx_start_params {
 	struct priority_cos traffic_type_to_priority_cos[MAX_TRAFFIC_TYPES];
 	u8 dcb_enabled;
@@ -1179,6 +1229,24 @@ struct bnx2x_func_tx_start_params {
 	u8 dont_add_pri_0_en;
 };
 
+struct bnx2x_func_set_timesync_params {
+	/* Reset, set or keep the current drift value */
+	u8 drift_adjust_cmd;
+
+	/* Dec, inc or keep the current offset */
+	u8 offset_cmd;
+
+	/* Drift value direction */
+	u8 add_sub_drift_adjust_value;
+
+	/* Drift, period and offset values to be used according to the commands
+	 * above.
+	 */
+	u8 drift_adjust_value;
+	u32 drift_adjust_period;
+	u64 offset_delta;
+};
+
 struct bnx2x_func_state_params {
 	struct bnx2x_func_sp_obj *f_obj;
 
@@ -1197,6 +1265,7 @@ struct bnx2x_func_state_params {
 		struct bnx2x_func_afex_update_params afex_update;
 		struct bnx2x_func_afex_viflists_params afex_viflists;
 		struct bnx2x_func_tx_start_params tx_start;
+		struct bnx2x_func_set_timesync_params set_timesync;
 	} params;
 };
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
index 662310c5f4e9..c88b20af87df 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.c
@@ -1125,7 +1125,7 @@ static int bnx2x_ari_enabled(struct pci_dev *dev)
 	return dev->bus->self && dev->bus->self->ari_enabled;
 }
 
-static void
+static int
 bnx2x_get_vf_igu_cam_info(struct bnx2x *bp)
 {
 	int sb_id;
@@ -1150,6 +1150,7 @@ bnx2x_get_vf_igu_cam_info(struct bnx2x *bp)
 		   GET_FIELD((val), IGU_REG_MAPPING_MEMORY_VECTOR));
 	}
 	DP(BNX2X_MSG_IOV, "vf_sbs_pool is %d\n", BP_VFDB(bp)->vf_sbs_pool);
+	return BP_VFDB(bp)->vf_sbs_pool;
 }
 
 static void __bnx2x_iov_free_vfdb(struct bnx2x *bp)
@@ -1314,15 +1315,17 @@ int bnx2x_iov_init_one(struct bnx2x *bp, int int_mode_param,
 	}
 
 	/* re-read the IGU CAM for VFs - index and abs_vfid must be set */
-	bnx2x_get_vf_igu_cam_info(bp);
+	if (!bnx2x_get_vf_igu_cam_info(bp)) {
+		BNX2X_ERR("No entries in IGU CAM for vfs\n");
+		err = -EINVAL;
+		goto failed;
+	}
 
 	/* allocate the queue arrays for all VFs */
 	bp->vfdb->vfqs = kzalloc(
 		BNX2X_MAX_NUM_VF_QUEUES * sizeof(struct bnx2x_vf_queue),
 		GFP_KERNEL);
 
-	DP(BNX2X_MSG_IOV, "bp->vfdb->vfqs was %p\n", bp->vfdb->vfqs);
-
 	if (!bp->vfdb->vfqs) {
 		BNX2X_ERR("failed to allocate vf queue array\n");
 		err = -ENOMEM;
@@ -1349,9 +1352,7 @@ void bnx2x_iov_remove_one(struct bnx2x *bp)
 	if (!IS_SRIOV(bp))
 		return;
 
-	DP(BNX2X_MSG_IOV, "about to call disable sriov\n");
-	pci_disable_sriov(bp->pdev);
-	DP(BNX2X_MSG_IOV, "sriov disabled\n");
+	bnx2x_disable_sriov(bp);
 
 	/* disable access to all VFs */
 	for (vf_idx = 0; vf_idx < bp->vfdb->sriov.total; vf_idx++) {
@@ -1985,21 +1986,6 @@ void bnx2x_iov_adjust_stats_req(struct bnx2x *bp)
 	bp->fw_stats_req->hdr.cmd_num = bp->fw_stats_num + stats_count;
 }
 
-static inline
-struct bnx2x_virtf *__vf_from_stat_id(struct bnx2x *bp, u8 stat_id)
-{
-	int i;
-	struct bnx2x_virtf *vf = NULL;
-
-	for_each_vf(bp, i) {
-		vf = BP_VF(bp, i);
-		if (stat_id >= vf->igu_base_id &&
-		    stat_id < vf->igu_base_id + vf_sb_count(vf))
-			break;
-	}
-	return vf;
-}
-
 /* VF API helpers */
 static void bnx2x_vf_qtbl_set_q(struct bnx2x *bp, u8 abs_vfid, u8 qid,
 				u8 enable)
@@ -2362,12 +2348,6 @@ int bnx2x_vf_release(struct bnx2x *bp, struct bnx2x_virtf *vf)
 	return rc;
 }
 
-static inline void bnx2x_vf_get_sbdf(struct bnx2x *bp,
-			      struct bnx2x_virtf *vf, u32 *sbdf)
-{
-	*sbdf = vf->devfn | (vf->bus << 8);
-}
-
 void bnx2x_lock_vf_pf_channel(struct bnx2x *bp, struct bnx2x_virtf *vf,
 			      enum channel_tlvs tlv)
 {
@@ -2416,7 +2396,7 @@ void bnx2x_unlock_vf_pf_channel(struct bnx2x *bp, struct bnx2x_virtf *vf,
 
 	/* log the unlock */
 	DP(BNX2X_MSG_IOV, "VF[%d]: vf pf channel unlocked by %d\n",
-	   vf->abs_vfid, vf->op_current);
+	   vf->abs_vfid, current_tlv);
 }
 
 static int bnx2x_set_pf_tx_switching(struct bnx2x *bp, bool enable)
@@ -2501,7 +2481,7 @@ int bnx2x_sriov_configure(struct pci_dev *dev, int num_vfs_param)
 	bp->requested_nr_virtfn = num_vfs_param;
 	if (num_vfs_param == 0) {
 		bnx2x_set_pf_tx_switching(bp, false);
-		pci_disable_sriov(dev);
+		bnx2x_disable_sriov(bp);
 		return 0;
 	} else {
 		return bnx2x_enable_sriov(bp);
@@ -2614,6 +2594,12 @@ void bnx2x_pf_set_vfs_vlan(struct bnx2x *bp)
 
 void bnx2x_disable_sriov(struct bnx2x *bp)
 {
+	if (pci_vfs_assigned(bp->pdev)) {
+		DP(BNX2X_MSG_IOV,
+		   "Unloading driver while VFs are assigned - VFs will not be deallocated\n");
+		return;
+	}
+
 	pci_disable_sriov(bp->pdev);
 }
 
@@ -2628,7 +2614,7 @@ static int bnx2x_vf_op_prep(struct bnx2x *bp, int vfidx,
 	}
 
 	if (!IS_SRIOV(bp)) {
-		BNX2X_ERR("sriov is disabled - can't utilize iov-realted functionality\n");
+		BNX2X_ERR("sriov is disabled - can't utilize iov-related functionality\n");
 		return -EINVAL;
 	}
 
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
index ca1055f3d8af..01bafa4ac045 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_sriov.h
@@ -299,7 +299,8 @@ struct bnx2x_vfdb {
 #define BP_VFDB(bp)		((bp)->vfdb)
 	/* vf array */
 	struct bnx2x_virtf	*vfs;
-#define BP_VF(bp, idx)		(&((bp)->vfdb->vfs[idx]))
+#define BP_VF(bp, idx)		((BP_VFDB(bp) && (bp)->vfdb->vfs) ? \
+					&((bp)->vfdb->vfs[idx]) : NULL)
 #define bnx2x_vf(bp, idx, var)	((bp)->vfdb->vfs[idx].var)
 
 	/* queue array - for all vfs */
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
index ca47665f94bf..d1608297c773 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_stats.c
@@ -137,7 +137,7 @@ static void bnx2x_storm_stats_post(struct bnx2x *bp)
 			cpu_to_le16(bp->stats_counter++);
 
 		DP(BNX2X_MSG_STATS, "Sending statistics ramrod %d\n",
-			bp->fw_stats_req->hdr.drv_stats_counter);
+		   le16_to_cpu(bp->fw_stats_req->hdr.drv_stats_counter));
 
 		/* adjust the ramrod to include VF queues statistics */
 		bnx2x_iov_adjust_stats_req(bp);
@@ -200,7 +200,7 @@ static void bnx2x_hw_stats_post(struct bnx2x *bp)
 	}
 }
 
-static int bnx2x_stats_comp(struct bnx2x *bp)
+static void bnx2x_stats_comp(struct bnx2x *bp)
 {
 	u32 *stats_comp = bnx2x_sp(bp, stats_comp);
 	int cnt = 10;
@@ -214,7 +214,6 @@ static int bnx2x_stats_comp(struct bnx2x *bp)
 		cnt--;
 		usleep_range(1000, 2000);
 	}
-	return 1;
 }
 
 /*
@@ -1630,6 +1629,11 @@ void bnx2x_stats_init(struct bnx2x *bp)
 	int /*abs*/port = BP_PORT(bp);
 	int mb_idx = BP_FW_MB_IDX(bp);
 
+	if (IS_VF(bp)) {
+		bnx2x_memset_stats(bp);
+		return;
+	}
+
 	bp->stats_pending = 0;
 	bp->executer_idx = 0;
 	bp->stats_counter = 0;
diff --git a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
index 54e0427a9ee6..b1d9c44aa56c 100644
--- a/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
+++ b/drivers/net/ethernet/broadcom/bnx2x/bnx2x_vfpf.c
@@ -583,7 +583,6 @@ int bnx2x_vfpf_setup_q(struct bnx2x *bp, struct bnx2x_fastpath *fp,
 	flags |= VFPF_QUEUE_FLG_STATS;
 	flags |= VFPF_QUEUE_FLG_CACHE_ALIGN;
 	flags |= VFPF_QUEUE_FLG_VLAN;
-	DP(NETIF_MSG_IFUP, "vlan removal enabled\n");
 
 	/* Common */
 	req->vf_qid = fp_idx;
@@ -952,14 +951,6 @@ static void storm_memset_vf_mbx_valid(struct bnx2x *bp, u16 abs_fid)
 	REG_WR8(bp, addr, 1);
 }
 
-static inline void bnx2x_set_vf_mbxs_valid(struct bnx2x *bp)
-{
-	int i;
-
-	for_each_vf(bp, i)
-		storm_memset_vf_mbx_valid(bp, bnx2x_vf(bp, i, abs_vfid));
-}
-
 /* enable vf_pf mailbox (aka vf-pf-channel) */
 void bnx2x_vf_enable_mbx(struct bnx2x *bp, u8 abs_vfid)
 {
diff --git a/drivers/net/ethernet/broadcom/cnic.c b/drivers/net/ethernet/broadcom/cnic.c
index 27861a6c7ca5..23f23c97c2ad 100644
--- a/drivers/net/ethernet/broadcom/cnic.c
+++ b/drivers/net/ethernet/broadcom/cnic.c
@@ -31,7 +31,7 @@
 #include <linux/if_vlan.h>
 #include <linux/prefetch.h>
 #include <linux/random.h>
-#if defined(CONFIG_VLAN_8021Q) || defined(CONFIG_VLAN_8021Q_MODULE)
+#if IS_ENABLED(CONFIG_VLAN_8021Q)
 #define BCM_VLAN 1
 #endif
 #include <net/ip.h>
@@ -383,7 +383,7 @@ static int cnic_iscsi_nl_msg_recv(struct cnic_dev *dev, u32 msg_type,
 			break;
 
 		rcu_read_lock();
-		if (!rcu_dereference(cp->ulp_ops[CNIC_ULP_L4])) {
+		if (!rcu_access_pointer(cp->ulp_ops[CNIC_ULP_L4])) {
 			rc = -ENODEV;
 			rcu_read_unlock();
 			break;
@@ -527,7 +527,7 @@ int cnic_unregister_driver(int ulp_type)
 	list_for_each_entry(dev, &cnic_dev_list, list) {
 		struct cnic_local *cp = dev->cnic_priv;
 
-		if (rcu_dereference(cp->ulp_ops[ulp_type])) {
+		if (rcu_access_pointer(cp->ulp_ops[ulp_type])) {
 			pr_err("%s: Type %d still has devices registered\n",
 			       __func__, ulp_type);
 			read_unlock(&cnic_dev_lock);
@@ -575,7 +575,7 @@ static int cnic_register_device(struct cnic_dev *dev, int ulp_type,
 		mutex_unlock(&cnic_lock);
 		return -EAGAIN;
 	}
-	if (rcu_dereference(cp->ulp_ops[ulp_type])) {
+	if (rcu_access_pointer(cp->ulp_ops[ulp_type])) {
 		pr_err("%s: Type %d has already been registered to this device\n",
 		       __func__, ulp_type);
 		mutex_unlock(&cnic_lock);
@@ -3685,7 +3685,7 @@ static int cnic_get_v4_route(struct sockaddr_in *dst_addr,
 static int cnic_get_v6_route(struct sockaddr_in6 *dst_addr,
 			     struct dst_entry **dst)
 {
-#if defined(CONFIG_IPV6) || (defined(CONFIG_IPV6_MODULE) && defined(MODULE))
+#if IS_ENABLED(CONFIG_IPV6)
 	struct flowi6 fl6;
 
 	memset(&fl6, 0, sizeof(fl6));
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.c b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
index 3f9d4de8173c..fdc9ec09e453 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.c
@@ -191,8 +191,9 @@ enum dma_reg {
 	DMA_STATUS,
 	DMA_SCB_BURST_SIZE,
 	DMA_ARB_CTRL,
-	DMA_PRIORITY,
-	DMA_RING_PRIORITY,
+	DMA_PRIORITY_0,
+	DMA_PRIORITY_1,
+	DMA_PRIORITY_2,
 };
 
 static const u8 bcmgenet_dma_regs_v3plus[] = {
@@ -201,8 +202,9 @@ static const u8 bcmgenet_dma_regs_v3plus[] = {
 	[DMA_STATUS]		= 0x08,
 	[DMA_SCB_BURST_SIZE]	= 0x0C,
 	[DMA_ARB_CTRL]		= 0x2C,
-	[DMA_PRIORITY]		= 0x30,
-	[DMA_RING_PRIORITY]	= 0x38,
+	[DMA_PRIORITY_0]	= 0x30,
+	[DMA_PRIORITY_1]	= 0x34,
+	[DMA_PRIORITY_2]	= 0x38,
 };
 
 static const u8 bcmgenet_dma_regs_v2[] = {
@@ -211,8 +213,9 @@ static const u8 bcmgenet_dma_regs_v2[] = {
 	[DMA_STATUS]		= 0x08,
 	[DMA_SCB_BURST_SIZE]	= 0x0C,
 	[DMA_ARB_CTRL]		= 0x30,
-	[DMA_PRIORITY]		= 0x34,
-	[DMA_RING_PRIORITY]	= 0x3C,
+	[DMA_PRIORITY_0]	= 0x34,
+	[DMA_PRIORITY_1]	= 0x38,
+	[DMA_PRIORITY_2]	= 0x3C,
 };
 
 static const u8 bcmgenet_dma_regs_v1[] = {
@@ -220,8 +223,9 @@ static const u8 bcmgenet_dma_regs_v1[] = {
 	[DMA_STATUS]		= 0x04,
 	[DMA_SCB_BURST_SIZE]	= 0x0C,
 	[DMA_ARB_CTRL]		= 0x30,
-	[DMA_PRIORITY]		= 0x34,
-	[DMA_RING_PRIORITY]	= 0x3C,
+	[DMA_PRIORITY_0]	= 0x34,
+	[DMA_PRIORITY_1]	= 0x38,
+	[DMA_PRIORITY_2]	= 0x3C,
 };
 
 /* Set at runtime once bcmgenet version is known */
@@ -875,6 +879,7 @@ static void __bcmgenet_tx_reclaim(struct net_device *dev,
 	int last_tx_cn, last_c_index, num_tx_bds;
 	struct enet_cb *tx_cb_ptr;
 	struct netdev_queue *txq;
+	unsigned int bds_compl;
 	unsigned int c_index;
 
 	/* Compute how many buffers are transmitted since last xmit call */
@@ -899,7 +904,9 @@ static void __bcmgenet_tx_reclaim(struct net_device *dev,
 	/* Reclaim transmitted buffers */
 	while (last_tx_cn-- > 0) {
 		tx_cb_ptr = ring->cbs + last_c_index;
+		bds_compl = 0;
 		if (tx_cb_ptr->skb) {
+			bds_compl = skb_shinfo(tx_cb_ptr->skb)->nr_frags + 1;
 			dev->stats.tx_bytes += tx_cb_ptr->skb->len;
 			dma_unmap_single(&dev->dev,
 					 dma_unmap_addr(tx_cb_ptr, dma_addr),
@@ -916,7 +923,7 @@ static void __bcmgenet_tx_reclaim(struct net_device *dev,
 			dma_unmap_addr_set(tx_cb_ptr, dma_addr, 0);
 		}
 		dev->stats.tx_packets++;
-		ring->free_bds += 1;
+		ring->free_bds += bds_compl;
 
 		last_c_index++;
 		last_c_index &= (num_tx_bds - 1);
@@ -1051,7 +1058,8 @@ static int bcmgenet_xmit_frag(struct net_device *dev,
 /* Reallocate the SKB to put enough headroom in front of it and insert
  * the transmit checksum offsets in the descriptors
  */
-static int bcmgenet_put_tx_csum(struct net_device *dev, struct sk_buff *skb)
+static struct sk_buff *bcmgenet_put_tx_csum(struct net_device *dev,
+					    struct sk_buff *skb)
 {
 	struct status_64 *status = NULL;
 	struct sk_buff *new_skb;
@@ -1069,7 +1077,7 @@ static int bcmgenet_put_tx_csum(struct net_device *dev, struct sk_buff *skb)
 		if (!new_skb) {
 			dev->stats.tx_errors++;
 			dev->stats.tx_dropped++;
-			return -ENOMEM;
+			return NULL;
 		}
 		skb = new_skb;
 	}
@@ -1087,7 +1095,7 @@ static int bcmgenet_put_tx_csum(struct net_device *dev, struct sk_buff *skb)
 			ip_proto = ipv6_hdr(skb)->nexthdr;
 			break;
 		default:
-			return 0;
+			return skb;
 		}
 
 		offset = skb_checksum_start_offset(skb) - sizeof(*status);
@@ -1108,7 +1116,7 @@ static int bcmgenet_put_tx_csum(struct net_device *dev, struct sk_buff *skb)
 		status->tx_csum_info = tx_csum_info;
 	}
 
-	return 0;
+	return skb;
 }
 
 static netdev_tx_t bcmgenet_xmit(struct sk_buff *skb, struct net_device *dev)
@@ -1155,8 +1163,8 @@ static netdev_tx_t bcmgenet_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* set the SKB transmit checksum */
 	if (priv->desc_64b_en) {
-		ret = bcmgenet_put_tx_csum(dev, skb);
-		if (ret) {
+		skb = bcmgenet_put_tx_csum(dev, skb);
+		if (!skb) {
 			ret = NETDEV_TX_OK;
 			goto out;
 		}
@@ -1274,12 +1282,24 @@ static unsigned int bcmgenet_desc_rx(struct bcmgenet_priv *priv,
 
 	while ((rxpktprocessed < rxpkttoprocess) &&
 	       (rxpktprocessed < budget)) {
+		cb = &priv->rx_cbs[priv->rx_read_ptr];
+		skb = cb->skb;
+
+		/* We do not have a backing SKB, so we do not have a
+		 * corresponding DMA mapping for this incoming packet since
+		 * bcmgenet_rx_refill always either has both skb and mapping or
+		 * none.
+		 */
+		if (unlikely(!skb)) {
+			dev->stats.rx_dropped++;
+			dev->stats.rx_errors++;
+			goto refill;
+		}
+
 		/* Unmap the packet contents such that we can use the
 		 * RSV from the 64 bytes descriptor when enabled and save
 		 * a 32-bits register read
 		 */
-		cb = &priv->rx_cbs[priv->rx_read_ptr];
-		skb = cb->skb;
 		dma_unmap_single(&dev->dev, dma_unmap_addr(cb, dma_addr),
 				 priv->rx_buf_len, DMA_FROM_DEVICE);
 
@@ -1307,18 +1327,6 @@ static unsigned int bcmgenet_desc_rx(struct bcmgenet_priv *priv,
 			  __func__, p_index, priv->rx_c_index,
 			  priv->rx_read_ptr, dma_length_status);
 
-		rxpktprocessed++;
-
-		priv->rx_read_ptr++;
-		priv->rx_read_ptr &= (priv->num_rx_bds - 1);
-
-		/* out of memory, just drop packets at the hardware level */
-		if (unlikely(!skb)) {
-			dev->stats.rx_dropped++;
-			dev->stats.rx_errors++;
-			goto refill;
-		}
-
 		if (unlikely(!(dma_flag & DMA_EOP) || !(dma_flag & DMA_SOP))) {
 			netif_err(priv, rx_status, dev,
 				  "dropping fragmented packet!\n");
@@ -1391,6 +1399,10 @@ refill:
 		err = bcmgenet_rx_refill(priv, cb);
 		if (err)
 			netif_err(priv, rx_err, dev, "Rx refill failed\n");
+
+		rxpktprocessed++;
+		priv->rx_read_ptr++;
+		priv->rx_read_ptr &= (priv->num_rx_bds - 1);
 	}
 
 	return rxpktprocessed;
@@ -1687,7 +1699,8 @@ static void bcmgenet_init_multiq(struct net_device *dev)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 	unsigned int i, dma_enable;
-	u32 reg, dma_ctrl, ring_cfg = 0, dma_priority = 0;
+	u32 reg, dma_ctrl, ring_cfg = 0;
+	u32 dma_priority[3] = {0, 0, 0};
 
 	if (!netif_is_multiqueue(dev)) {
 		netdev_warn(dev, "called with non multi queue aware HW\n");
@@ -1712,22 +1725,25 @@ static void bcmgenet_init_multiq(struct net_device *dev)
 
 		/* Configure ring as descriptor ring and setup priority */
 		ring_cfg |= 1 << i;
-		dma_priority |= ((GENET_Q0_PRIORITY + i) <<
-				(GENET_MAX_MQ_CNT + 1) * i);
 		dma_ctrl |= 1 << (i + DMA_RING_BUF_EN_SHIFT);
+
+		dma_priority[DMA_PRIO_REG_INDEX(i)] |=
+			((GENET_Q0_PRIORITY + i) << DMA_PRIO_REG_SHIFT(i));
 	}
 
+	/* Set ring 16 priority and program the hardware registers */
+	dma_priority[DMA_PRIO_REG_INDEX(DESC_INDEX)] |=
+		((GENET_Q0_PRIORITY + priv->hw_params->tx_queues) <<
+		 DMA_PRIO_REG_SHIFT(DESC_INDEX));
+	bcmgenet_tdma_writel(priv, dma_priority[0], DMA_PRIORITY_0);
+	bcmgenet_tdma_writel(priv, dma_priority[1], DMA_PRIORITY_1);
+	bcmgenet_tdma_writel(priv, dma_priority[2], DMA_PRIORITY_2);
+
 	/* Enable rings */
 	reg = bcmgenet_tdma_readl(priv, DMA_RING_CFG);
 	reg |= ring_cfg;
 	bcmgenet_tdma_writel(priv, reg, DMA_RING_CFG);
 
-	/* Use configured rings priority and set ring #16 priority */
-	reg = bcmgenet_tdma_readl(priv, DMA_RING_PRIORITY);
-	reg |= ((GENET_Q0_PRIORITY + priv->hw_params->tx_queues) << 20);
-	reg |= dma_priority;
-	bcmgenet_tdma_writel(priv, reg, DMA_PRIORITY);
-
 	/* Configure ring as descriptor ring and re-enable DMA if enabled */
 	reg = bcmgenet_tdma_readl(priv, DMA_CTRL);
 	reg |= dma_ctrl;
@@ -1736,13 +1752,63 @@ static void bcmgenet_init_multiq(struct net_device *dev)
 	bcmgenet_tdma_writel(priv, reg, DMA_CTRL);
 }
 
+static int bcmgenet_dma_teardown(struct bcmgenet_priv *priv)
+{
+	int ret = 0;
+	int timeout = 0;
+	u32 reg;
+
+	/* Disable TDMA to stop add more frames in TX DMA */
+	reg = bcmgenet_tdma_readl(priv, DMA_CTRL);
+	reg &= ~DMA_EN;
+	bcmgenet_tdma_writel(priv, reg, DMA_CTRL);
+
+	/* Check TDMA status register to confirm TDMA is disabled */
+	while (timeout++ < DMA_TIMEOUT_VAL) {
+		reg = bcmgenet_tdma_readl(priv, DMA_STATUS);
+		if (reg & DMA_DISABLED)
+			break;
+
+		udelay(1);
+	}
+
+	if (timeout == DMA_TIMEOUT_VAL) {
+		netdev_warn(priv->dev, "Timed out while disabling TX DMA\n");
+		ret = -ETIMEDOUT;
+	}
+
+	/* Wait 10ms for packet drain in both tx and rx dma */
+	usleep_range(10000, 20000);
+
+	/* Disable RDMA */
+	reg = bcmgenet_rdma_readl(priv, DMA_CTRL);
+	reg &= ~DMA_EN;
+	bcmgenet_rdma_writel(priv, reg, DMA_CTRL);
+
+	timeout = 0;
+	/* Check RDMA status register to confirm RDMA is disabled */
+	while (timeout++ < DMA_TIMEOUT_VAL) {
+		reg = bcmgenet_rdma_readl(priv, DMA_STATUS);
+		if (reg & DMA_DISABLED)
+			break;
+
+		udelay(1);
+	}
+
+	if (timeout == DMA_TIMEOUT_VAL) {
+		netdev_warn(priv->dev, "Timed out while disabling RX DMA\n");
+		ret = -ETIMEDOUT;
+	}
+
+	return ret;
+}
+
 static void bcmgenet_fini_dma(struct bcmgenet_priv *priv)
 {
 	int i;
 
 	/* disable DMA */
-	bcmgenet_rdma_writel(priv, 0, DMA_CTRL);
-	bcmgenet_tdma_writel(priv, 0, DMA_CTRL);
+	bcmgenet_dma_teardown(priv);
 
 	for (i = 0; i < priv->num_tx_bds; i++) {
 		if (priv->tx_cbs[i].skb != NULL) {
@@ -1959,19 +2025,6 @@ static void bcmgenet_set_hw_addr(struct bcmgenet_priv *priv,
 	bcmgenet_umac_writel(priv, (addr[4] << 8) | addr[5], UMAC_MAC1);
 }
 
-static int bcmgenet_wol_resume(struct bcmgenet_priv *priv)
-{
-	/* From WOL-enabled suspend, switch to regular clock */
-	if (priv->wolopts)
-		clk_disable_unprepare(priv->clk_wol);
-
-	phy_init_hw(priv->phydev);
-	/* Speed settings must be restored */
-	bcmgenet_mii_config(priv->dev);
-
-	return 0;
-}
-
 /* Returns a reusable dma control register value */
 static u32 bcmgenet_dma_disable(struct bcmgenet_priv *priv)
 {
@@ -2101,57 +2154,6 @@ err_clk_disable:
 	return ret;
 }
 
-static int bcmgenet_dma_teardown(struct bcmgenet_priv *priv)
-{
-	int ret = 0;
-	int timeout = 0;
-	u32 reg;
-
-	/* Disable TDMA to stop add more frames in TX DMA */
-	reg = bcmgenet_tdma_readl(priv, DMA_CTRL);
-	reg &= ~DMA_EN;
-	bcmgenet_tdma_writel(priv, reg, DMA_CTRL);
-
-	/* Check TDMA status register to confirm TDMA is disabled */
-	while (timeout++ < DMA_TIMEOUT_VAL) {
-		reg = bcmgenet_tdma_readl(priv, DMA_STATUS);
-		if (reg & DMA_DISABLED)
-			break;
-
-		udelay(1);
-	}
-
-	if (timeout == DMA_TIMEOUT_VAL) {
-		netdev_warn(priv->dev, "Timed out while disabling TX DMA\n");
-		ret = -ETIMEDOUT;
-	}
-
-	/* Wait 10ms for packet drain in both tx and rx dma */
-	usleep_range(10000, 20000);
-
-	/* Disable RDMA */
-	reg = bcmgenet_rdma_readl(priv, DMA_CTRL);
-	reg &= ~DMA_EN;
-	bcmgenet_rdma_writel(priv, reg, DMA_CTRL);
-
-	timeout = 0;
-	/* Check RDMA status register to confirm RDMA is disabled */
-	while (timeout++ < DMA_TIMEOUT_VAL) {
-		reg = bcmgenet_rdma_readl(priv, DMA_STATUS);
-		if (reg & DMA_DISABLED)
-			break;
-
-		udelay(1);
-	}
-
-	if (timeout == DMA_TIMEOUT_VAL) {
-		netdev_warn(priv->dev, "Timed out while disabling RX DMA\n");
-		ret = -ETIMEDOUT;
-	}
-
-	return ret;
-}
-
 static void bcmgenet_netif_stop(struct net_device *dev)
 {
 	struct bcmgenet_priv *priv = netdev_priv(dev);
@@ -2167,9 +2169,10 @@ static void bcmgenet_netif_stop(struct net_device *dev)
 	 */
 	cancel_work_sync(&priv->bcmgenet_irq_work);
 
-	priv->old_pause = -1;
 	priv->old_link = -1;
+	priv->old_speed = -1;
 	priv->old_duplex = -1;
+	priv->old_pause = -1;
 }
 
 static int bcmgenet_close(struct net_device *dev)
@@ -2432,6 +2435,13 @@ static void bcmgenet_set_hw_params(struct bcmgenet_priv *priv)
 	dev_info(&priv->pdev->dev, "GENET " GENET_VER_FMT,
 		 major, (reg >> 16) & 0x0f, reg & 0xffff);
 
+	/* Store the integrated PHY revision for the MDIO probing function
+	 * to pass this information to the PHY driver. The PHY driver expects
+	 * to find the PHY major revision in bits 15:8 while the GENET register
+	 * stores that information in bits 7:0, account for that.
+	 */
+	priv->gphy_rev = (reg & 0xffff) << 8;
+
 #ifdef CONFIG_PHYS_ADDR_T_64BIT
 	if (!(params->flags & GENET_HAS_40BITS))
 		pr_warn("GENET does not support 40-bits PA\n");
@@ -2669,9 +2679,13 @@ static int bcmgenet_resume(struct device *d)
 	if (ret)
 		goto out_clk_disable;
 
-	ret = bcmgenet_wol_resume(priv);
-	if (ret)
-		goto out_clk_disable;
+	/* From WOL-enabled suspend, switch to regular clock */
+	if (priv->wolopts)
+		clk_disable_unprepare(priv->clk_wol);
+
+	phy_init_hw(priv->phydev);
+	/* Speed settings must be restored */
+	bcmgenet_mii_config(priv->dev);
 
 	/* disable ethernet MAC while updating its registers */
 	umac_enable_set(priv, CMD_TX_EN | CMD_RX_EN, false);
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet.h b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
index c862d0666771..dbf524ea3b19 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet.h
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet.h
@@ -401,6 +401,8 @@ struct bcmgenet_mib_counters {
 #define DMA_ARBITER_MODE_MASK		0x03
 #define DMA_RING_BUF_PRIORITY_MASK	0x1F
 #define DMA_RING_BUF_PRIORITY_SHIFT	5
+#define DMA_PRIO_REG_INDEX(q)		((q) / 6)
+#define DMA_PRIO_REG_SHIFT(q)		(((q) % 6) * DMA_RING_BUF_PRIORITY_SHIFT)
 #define DMA_RATE_ADJ_MASK		0xFF
 
 /* Tx/Rx Dma Descriptor common bits*/
@@ -545,10 +547,12 @@ struct bcmgenet_priv {
 	struct phy_device *phydev;
 	struct device_node *phy_dn;
 	struct mii_bus *mii_bus;
+	u16 gphy_rev;
 
 	/* PHY device variables */
-	int old_duplex;
 	int old_link;
+	int old_speed;
+	int old_duplex;
 	int old_pause;
 	phy_interface_t phy_interface;
 	int phy_addr;
diff --git a/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c b/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c
index b82b7e4e06b2..149a0d70c108 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmgenet_wol.c
@@ -86,7 +86,9 @@ int bcmgenet_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 	/* Flag the device and relevant IRQ as wakeup capable */
 	if (wol->wolopts) {
 		device_set_wakeup_enable(kdev, 1);
-		enable_irq_wake(priv->wol_irq);
+		/* Avoid unbalanced enable_irq_wake calls */
+		if (priv->wol_irq_disabled)
+			enable_irq_wake(priv->wol_irq);
 		priv->wol_irq_disabled = false;
 	} else {
 		device_set_wakeup_enable(kdev, 0);
diff --git a/drivers/net/ethernet/broadcom/genet/bcmmii.c b/drivers/net/ethernet/broadcom/genet/bcmmii.c
index c88f7ae99636..9ff799a9f801 100644
--- a/drivers/net/ethernet/broadcom/genet/bcmmii.c
+++ b/drivers/net/ethernet/broadcom/genet/bcmmii.c
@@ -82,24 +82,33 @@ static void bcmgenet_mii_setup(struct net_device *dev)
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 	struct phy_device *phydev = priv->phydev;
 	u32 reg, cmd_bits = 0;
-	unsigned int status_changed = 0;
+	bool status_changed = false;
 
 	if (priv->old_link != phydev->link) {
-		status_changed = 1;
+		status_changed = true;
 		priv->old_link = phydev->link;
 	}
 
 	if (phydev->link) {
-		/* program UMAC and RGMII block based on established link
-		 * speed, pause, and duplex.
-		 * the speed set in umac->cmd tell RGMII block which clock
-		 * 25MHz(100Mbps)/125MHz(1Gbps) to use for transmit.
-		 * receive clock is provided by PHY.
-		 */
-		reg = bcmgenet_ext_readl(priv, EXT_RGMII_OOB_CTRL);
-		reg &= ~OOB_DISABLE;
-		reg |= RGMII_LINK;
-		bcmgenet_ext_writel(priv, reg, EXT_RGMII_OOB_CTRL);
+		/* check speed/duplex/pause changes */
+		if (priv->old_speed != phydev->speed) {
+			status_changed = true;
+			priv->old_speed = phydev->speed;
+		}
+
+		if (priv->old_duplex != phydev->duplex) {
+			status_changed = true;
+			priv->old_duplex = phydev->duplex;
+		}
+
+		if (priv->old_pause != phydev->pause) {
+			status_changed = true;
+			priv->old_pause = phydev->pause;
+		}
+
+		/* done if nothing has changed */
+		if (!status_changed)
+			return;
 
 		/* speed */
 		if (phydev->speed == SPEED_1000)
@@ -110,36 +119,39 @@ static void bcmgenet_mii_setup(struct net_device *dev)
 			cmd_bits = UMAC_SPEED_10;
 		cmd_bits <<= CMD_SPEED_SHIFT;
 
-		if (priv->old_duplex != phydev->duplex) {
-			status_changed = 1;
-			priv->old_duplex = phydev->duplex;
-		}
-
 		/* duplex */
 		if (phydev->duplex != DUPLEX_FULL)
 			cmd_bits |= CMD_HD_EN;
 
-		if (priv->old_pause != phydev->pause) {
-			status_changed = 1;
-			priv->old_pause = phydev->pause;
-		}
-
 		/* pause capability */
 		if (!phydev->pause)
 			cmd_bits |= CMD_RX_PAUSE_IGNORE | CMD_TX_PAUSE_IGNORE;
-	}
 
-	if (!status_changed)
-		return;
+		/*
+		 * Program UMAC and RGMII block based on established
+		 * link speed, duplex, and pause. The speed set in
+		 * umac->cmd tell RGMII block which clock to use for
+		 * transmit -- 25MHz(100Mbps) or 125MHz(1Gbps).
+		 * Receive clock is provided by the PHY.
+		 */
+		reg = bcmgenet_ext_readl(priv, EXT_RGMII_OOB_CTRL);
+		reg &= ~OOB_DISABLE;
+		reg |= RGMII_LINK;
+		bcmgenet_ext_writel(priv, reg, EXT_RGMII_OOB_CTRL);
 
-	if (phydev->link) {
 		reg = bcmgenet_umac_readl(priv, UMAC_CMD);
 		reg &= ~((CMD_SPEED_MASK << CMD_SPEED_SHIFT) |
 			       CMD_HD_EN |
 			       CMD_RX_PAUSE_IGNORE | CMD_TX_PAUSE_IGNORE);
 		reg |= cmd_bits;
 		bcmgenet_umac_writel(priv, reg, UMAC_CMD);
+	} else {
+		/* done if nothing has changed */
+		if (!status_changed)
+			return;
 
+		/* needed for MoCA fixed PHY to reflect correct link status */
+		netif_carrier_off(dev);
 	}
 
 	phy_print_status(phydev);
@@ -296,7 +308,7 @@ static int bcmgenet_mii_probe(struct net_device *dev)
 	struct bcmgenet_priv *priv = netdev_priv(dev);
 	struct device_node *dn = priv->pdev->dev.of_node;
 	struct phy_device *phydev;
-	unsigned int phy_flags;
+	u32 phy_flags;
 	int ret;
 
 	if (priv->phydev) {
@@ -315,16 +327,22 @@ static int bcmgenet_mii_probe(struct net_device *dev)
 		priv->phy_dn = of_node_get(dn);
 	}
 
-	phydev = of_phy_connect(dev, priv->phy_dn, bcmgenet_mii_setup, 0,
-				priv->phy_interface);
+	/* Communicate the integrated PHY revision */
+	phy_flags = priv->gphy_rev;
+
+	/* Initialize link state variables that bcmgenet_mii_setup() uses */
+	priv->old_link = -1;
+	priv->old_speed = -1;
+	priv->old_duplex = -1;
+	priv->old_pause = -1;
+
+	phydev = of_phy_connect(dev, priv->phy_dn, bcmgenet_mii_setup,
+				phy_flags, priv->phy_interface);
 	if (!phydev) {
 		pr_err("could not attach to PHY\n");
 		return -ENODEV;
 	}
 
-	priv->old_link = -1;
-	priv->old_duplex = -1;
-	priv->old_pause = -1;
 	priv->phydev = phydev;
 
 	/* Configure port multiplexer based on what the probed PHY device since
@@ -338,15 +356,6 @@ static int bcmgenet_mii_probe(struct net_device *dev)
 		return ret;
 	}
 
-	phy_flags = PHY_BRCM_100MBPS_WAR;
-
-	/* workarounds are only needed for 100Mpbs PHYs, and
-	 * never on GENET V1 hardware
-	 */
-	if ((phydev->supported & PHY_GBIT_FEATURES) || GENET_IS_V1(priv))
-		phy_flags = 0;
-
-	phydev->dev_flags |= phy_flags;
 	phydev->advertising = phydev->supported;
 
 	/* The internal PHY has its link interrupts routed to the
diff --git a/drivers/net/ethernet/broadcom/tg3.c b/drivers/net/ethernet/broadcom/tg3.c
index 3ac5d23454a8..dbb41c1923e6 100644
--- a/drivers/net/ethernet/broadcom/tg3.c
+++ b/drivers/net/ethernet/broadcom/tg3.c
@@ -6918,7 +6918,8 @@ static int tg3_rx(struct tg3_napi *tnapi, int budget)
 		skb->protocol = eth_type_trans(skb, tp->dev);
 
 		if (len > (tp->dev->mtu + ETH_HLEN) &&
-		    skb->protocol != htons(ETH_P_8021Q)) {
+		    skb->protocol != htons(ETH_P_8021Q) &&
+		    skb->protocol != htons(ETH_P_8021AD)) {
 			dev_kfree_skb_any(skb);
 			goto drop_it_no_recycle;
 		}
@@ -7914,8 +7915,6 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	entry = tnapi->tx_prod;
 	base_flags = 0;
-	if (skb->ip_summed == CHECKSUM_PARTIAL)
-		base_flags |= TXD_FLAG_TCPUDP_CSUM;
 
 	mss = skb_shinfo(skb)->gso_size;
 	if (mss) {
@@ -7929,6 +7928,13 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 		hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb) - ETH_HLEN;
 
+		/* HW/FW can not correctly segment packets that have been
+		 * vlan encapsulated.
+		 */
+		if (skb->protocol == htons(ETH_P_8021Q) ||
+		    skb->protocol == htons(ETH_P_8021AD))
+			return tg3_tso_bug(tp, tnapi, txq, skb);
+
 		if (!skb_is_gso_v6(skb)) {
 			if (unlikely((ETH_HLEN + hdr_len) > 80) &&
 			    tg3_flag(tp, TSO_BUG))
@@ -7979,6 +7985,17 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 				base_flags |= tsflags << 12;
 			}
 		}
+	} else if (skb->ip_summed == CHECKSUM_PARTIAL) {
+		/* HW/FW can not correctly checksum packets that have been
+		 * vlan encapsulated.
+		 */
+		if (skb->protocol == htons(ETH_P_8021Q) ||
+		    skb->protocol == htons(ETH_P_8021AD)) {
+			if (skb_checksum_help(skb))
+				goto drop;
+		} else  {
+			base_flags |= TXD_FLAG_TCPUDP_CSUM;
+		}
 	}
 
 	if (tg3_flag(tp, USE_JUMBO_BDFLAG) &&
@@ -8082,9 +8099,6 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	/* Sync BD data before updating mailbox */
 	wmb();
 
-	/* Packets are ready, update Tx producer idx local and on card. */
-	tw32_tx_mbox(tnapi->prodmbox, entry);
-
 	tnapi->tx_prod = entry;
 	if (unlikely(tg3_tx_avail(tnapi) <= (MAX_SKB_FRAGS + 1))) {
 		netif_tx_stop_queue(txq);
@@ -8099,7 +8113,12 @@ static netdev_tx_t tg3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 			netif_tx_wake_queue(txq);
 	}
 
-	mmiowb();
+	if (!skb->xmit_more || netif_xmit_stopped(txq)) {
+		/* Packets are ready, update Tx producer idx on card. */
+		tw32_tx_mbox(tnapi->prodmbox, entry);
+		mmiowb();
+	}
+
 	return NETDEV_TX_OK;
 
 dma_error:
@@ -11617,6 +11636,12 @@ static int tg3_open(struct net_device *dev)
 	struct tg3 *tp = netdev_priv(dev);
 	int err;
 
+	if (tp->pcierr_recovery) {
+		netdev_err(dev, "Failed to open device. PCI error recovery "
+			   "in progress\n");
+		return -EAGAIN;
+	}
+
 	if (tp->fw_needed) {
 		err = tg3_request_firmware(tp);
 		if (tg3_asic_rev(tp) == ASIC_REV_57766) {
@@ -11674,6 +11699,12 @@ static int tg3_close(struct net_device *dev)
 {
 	struct tg3 *tp = netdev_priv(dev);
 
+	if (tp->pcierr_recovery) {
+		netdev_err(dev, "Failed to close device. PCI error recovery "
+			   "in progress\n");
+		return -EAGAIN;
+	}
+
 	tg3_ptp_fini(tp);
 
 	tg3_stop(tp);
@@ -17561,6 +17592,7 @@ static int tg3_init_one(struct pci_dev *pdev,
 	tp->rx_mode = TG3_DEF_RX_MODE;
 	tp->tx_mode = TG3_DEF_TX_MODE;
 	tp->irq_sync = 1;
+	tp->pcierr_recovery = false;
 
 	if (tg3_debug > 0)
 		tp->msg_enable = tg3_debug;
@@ -18071,6 +18103,8 @@ static pci_ers_result_t tg3_io_error_detected(struct pci_dev *pdev,
 
 	rtnl_lock();
 
+	tp->pcierr_recovery = true;
+
 	/* We probably don't have netdev yet */
 	if (!netdev || !netif_running(netdev))
 		goto done;
@@ -18195,6 +18229,7 @@ static void tg3_io_resume(struct pci_dev *pdev)
 	tg3_phy_start(tp);
 
 done:
+	tp->pcierr_recovery = false;
 	rtnl_unlock();
 }
 
diff --git a/drivers/net/ethernet/broadcom/tg3.h b/drivers/net/ethernet/broadcom/tg3.h
index 461accaf0aa4..31c9f8295953 100644
--- a/drivers/net/ethernet/broadcom/tg3.h
+++ b/drivers/net/ethernet/broadcom/tg3.h
@@ -3407,6 +3407,7 @@ struct tg3 {
 
 	struct device			*hwmon_dev;
 	bool				link_up;
+	bool				pcierr_recovery;
 };
 
 /* Accessor macros for chip and asic attributes
diff --git a/drivers/net/ethernet/brocade/bna/bna_enet.c b/drivers/net/ethernet/brocade/bna/bna_enet.c
index 13f9636cdba7..903466ef41c0 100644
--- a/drivers/net/ethernet/brocade/bna/bna_enet.c
+++ b/drivers/net/ethernet/brocade/bna/bna_enet.c
@@ -107,7 +107,8 @@ bna_bfi_ethport_admin_rsp(struct bna_ethport *ethport,
 {
 	struct bfi_enet_enable_req *admin_req =
 		&ethport->bfi_enet_cmd.admin_req;
-	struct bfi_enet_rsp *rsp = (struct bfi_enet_rsp *)msghdr;
+	struct bfi_enet_rsp *rsp =
+		container_of(msghdr, struct bfi_enet_rsp, mh);
 
 	switch (admin_req->enable) {
 	case BNA_STATUS_T_ENABLED:
@@ -133,7 +134,8 @@ bna_bfi_ethport_lpbk_rsp(struct bna_ethport *ethport,
 {
 	struct bfi_enet_diag_lb_req *diag_lb_req =
 		&ethport->bfi_enet_cmd.lpbk_req;
-	struct bfi_enet_rsp *rsp = (struct bfi_enet_rsp *)msghdr;
+	struct bfi_enet_rsp *rsp =
+		container_of(msghdr, struct bfi_enet_rsp, mh);
 
 	switch (diag_lb_req->enable) {
 	case BNA_STATUS_T_ENABLED:
@@ -161,7 +163,8 @@ static void
 bna_bfi_attr_get_rsp(struct bna_ioceth *ioceth,
 			struct bfi_msgq_mhdr *msghdr)
 {
-	struct bfi_enet_attr_rsp *rsp = (struct bfi_enet_attr_rsp *)msghdr;
+	struct bfi_enet_attr_rsp *rsp =
+		container_of(msghdr, struct bfi_enet_attr_rsp, mh);
 
 	/**
 	 * Store only if not set earlier, since BNAD can override the HW
diff --git a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
index 85e63546abe3..5fac411c52f4 100644
--- a/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
+++ b/drivers/net/ethernet/brocade/bna/bna_tx_rx.c
@@ -715,7 +715,7 @@ bna_bfi_rxf_ucast_set_rsp(struct bna_rxf *rxf,
 			struct bfi_msgq_mhdr *msghdr)
 {
 	struct bfi_enet_rsp *rsp =
-		(struct bfi_enet_rsp *)msghdr;
+		container_of(msghdr, struct bfi_enet_rsp, mh);
 
 	if (rsp->error) {
 		/* Clear ucast from cache */
@@ -732,7 +732,7 @@ bna_bfi_rxf_mcast_add_rsp(struct bna_rxf *rxf,
 	struct bfi_enet_mcast_add_req *req =
 		&rxf->bfi_enet_cmd.mcast_add_req;
 	struct bfi_enet_mcast_add_rsp *rsp =
-		(struct bfi_enet_mcast_add_rsp *)msghdr;
+		container_of(msghdr, struct bfi_enet_mcast_add_rsp, mh);
 
 	bna_rxf_mchandle_attach(rxf, (u8 *)&req->mac_addr,
 		ntohs(rsp->handle));
@@ -3410,7 +3410,7 @@ bna_bfi_tx_enet_start(struct bna_tx *tx)
 
 	cfg_req->tx_cfg.vlan_mode = BFI_ENET_TX_VLAN_WI;
 	cfg_req->tx_cfg.vlan_id = htons((u16)tx->txf_vlan_id);
-	cfg_req->tx_cfg.admit_tagged_frame = BNA_STATUS_T_DISABLED;
+	cfg_req->tx_cfg.admit_tagged_frame = BNA_STATUS_T_ENABLED;
 	cfg_req->tx_cfg.apply_vlan_filter = BNA_STATUS_T_DISABLED;
 
 	bfa_msgq_cmd_set(&tx->msgq_cmd, NULL, NULL,
diff --git a/drivers/net/ethernet/brocade/bna/bnad.c b/drivers/net/ethernet/brocade/bna/bnad.c
index ff8cae5e2535..c3861de9dc81 100644
--- a/drivers/net/ethernet/brocade/bna/bnad.c
+++ b/drivers/net/ethernet/brocade/bna/bnad.c
@@ -552,6 +552,7 @@ bnad_cq_setup_skb_frags(struct bna_rcb *rcb, struct sk_buff *skb,
 
 		len = (vec == nvecs) ?
 			last_fraglen : unmap->vector.len;
+		skb->truesize += unmap->vector.len;
 		totlen += len;
 
 		skb_fill_page_desc(skb, skb_shinfo(skb)->nr_frags,
@@ -563,7 +564,6 @@ bnad_cq_setup_skb_frags(struct bna_rcb *rcb, struct sk_buff *skb,
 
 	skb->len += totlen;
 	skb->data_len += totlen;
-	skb->truesize += totlen;
 }
 
 static inline void
@@ -2506,7 +2506,7 @@ bnad_tso_prepare(struct bnad *bnad, struct sk_buff *skb)
 	 * For TSO, the TCP checksum field is seeded with pseudo-header sum
 	 * excluding the length field.
 	 */
-	if (skb->protocol == htons(ETH_P_IP)) {
+	if (vlan_get_protocol(skb) == htons(ETH_P_IP)) {
 		struct iphdr *iph = ip_hdr(skb);
 
 		/* Do we really need these? */
@@ -2864,18 +2864,19 @@ bnad_txq_wi_prepare(struct bnad *bnad, struct bna_tcb *tcb,
 		txqent->hdr.wi.opcode =	htons(BNA_TXQ_WI_SEND);
 		txqent->hdr.wi.lso_mss = 0;
 
-		if (unlikely(skb->len > (bnad->netdev->mtu + ETH_HLEN))) {
+		if (unlikely(skb->len > (bnad->netdev->mtu + VLAN_ETH_HLEN))) {
 			BNAD_UPDATE_CTR(bnad, tx_skb_non_tso_too_long);
 			return -EINVAL;
 		}
 
 		if (skb->ip_summed == CHECKSUM_PARTIAL) {
+			__be16 net_proto = vlan_get_protocol(skb);
 			u8 proto = 0;
 
-			if (skb->protocol == htons(ETH_P_IP))
+			if (net_proto == htons(ETH_P_IP))
 				proto = ip_hdr(skb)->protocol;
 #ifdef NETIF_F_IPV6_CSUM
-			else if (skb->protocol == htons(ETH_P_IPV6)) {
+			else if (net_proto == htons(ETH_P_IPV6)) {
 				/* nexthdr may not be TCP immediately. */
 				proto = ipv6_hdr(skb)->nexthdr;
 			}
diff --git a/drivers/net/ethernet/cadence/at91_ether.c b/drivers/net/ethernet/cadence/at91_ether.c
index 4a79edaf3885..4a24b9a6ad75 100644
--- a/drivers/net/ethernet/cadence/at91_ether.c
+++ b/drivers/net/ethernet/cadence/at91_ether.c
@@ -351,7 +351,6 @@ static int __init at91ether_probe(struct platform_device *pdev)
 	if (res)
 		goto err_disable_clock;
 
-	ether_setup(dev);
 	dev->netdev_ops = &at91ether_netdev_ops;
 	dev->ethtool_ops = &macb_ethtool_ops;
 	platform_set_drvdata(pdev, dev);
diff --git a/drivers/net/ethernet/cadence/macb.c b/drivers/net/ethernet/cadence/macb.c
index ca5d7798b265..4d9fc0509af6 100644
--- a/drivers/net/ethernet/cadence/macb.c
+++ b/drivers/net/ethernet/cadence/macb.c
@@ -30,7 +30,6 @@
 #include <linux/of_device.h>
 #include <linux/of_mdio.h>
 #include <linux/of_net.h>
-#include <linux/pinctrl/consumer.h>
 
 #include "macb.h"
 
@@ -2071,7 +2070,6 @@ static int __init macb_probe(struct platform_device *pdev)
 	struct phy_device *phydev;
 	u32 config;
 	int err = -ENXIO;
-	struct pinctrl *pinctrl;
 	const char *mac;
 
 	regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
@@ -2080,15 +2078,6 @@ static int __init macb_probe(struct platform_device *pdev)
 		goto err_out;
 	}
 
-	pinctrl = devm_pinctrl_get_select_default(&pdev->dev);
-	if (IS_ERR(pinctrl)) {
-		err = PTR_ERR(pinctrl);
-		if (err == -EPROBE_DEFER)
-			goto err_out;
-
-		dev_warn(&pdev->dev, "No pinctrl provided\n");
-	}
-
 	err = -ENOMEM;
 	dev = alloc_etherdev(sizeof(*bp));
 	if (!dev)
@@ -2241,9 +2230,9 @@ static int __init macb_probe(struct platform_device *pdev)
 
 	netif_carrier_off(dev);
 
-	netdev_info(dev, "Cadence %s at 0x%08lx irq %d (%pM)\n",
-		    macb_is_gem(bp) ? "GEM" : "MACB", dev->base_addr,
-		    dev->irq, dev->dev_addr);
+	netdev_info(dev, "Cadence %s rev 0x%08x at 0x%08lx irq %d (%pM)\n",
+		    macb_is_gem(bp) ? "GEM" : "MACB", macb_readl(bp, MID),
+		    dev->base_addr, dev->irq, dev->dev_addr);
 
 	phydev = bp->phy_dev;
 	netdev_info(dev, "attached PHY driver [%s] (mii_bus:phy_addr=%s, irq=%d)\n",
diff --git a/drivers/net/ethernet/calxeda/Kconfig b/drivers/net/ethernet/calxeda/Kconfig
index 184a063bed5f..07d2201530d2 100644
--- a/drivers/net/ethernet/calxeda/Kconfig
+++ b/drivers/net/ethernet/calxeda/Kconfig
@@ -1,6 +1,7 @@
 config NET_CALXEDA_XGMAC
 	tristate "Calxeda 1G/10G XGMAC Ethernet driver"
 	depends on HAS_IOMEM && HAS_DMA
+	depends on ARCH_HIGHBANK || COMPILE_TEST
 	select CRC32
 	help
 	  This is the driver for the XGMAC Ethernet IP block found on Calxeda
diff --git a/drivers/net/ethernet/calxeda/xgmac.c b/drivers/net/ethernet/calxeda/xgmac.c
index 25d6b2a10e4e..47bfea24b9e1 100644
--- a/drivers/net/ethernet/calxeda/xgmac.c
+++ b/drivers/net/ethernet/calxeda/xgmac.c
@@ -1735,7 +1735,6 @@ static int xgmac_probe(struct platform_device *pdev)
 	SET_NETDEV_DEV(ndev, &pdev->dev);
 	priv = netdev_priv(ndev);
 	platform_set_drvdata(pdev, ndev);
-	ether_setup(ndev);
 	ndev->netdev_ops = &xgmac_netdev_ops;
 	ndev->ethtool_ops = &xgmac_ethtool_ops;
 	spin_lock_init(&priv->stats_lock);
diff --git a/drivers/net/ethernet/chelsio/Kconfig b/drivers/net/ethernet/chelsio/Kconfig
index c3ce9df0041a..ac6473f75eb9 100644
--- a/drivers/net/ethernet/chelsio/Kconfig
+++ b/drivers/net/ethernet/chelsio/Kconfig
@@ -68,7 +68,7 @@ config CHELSIO_T3
 
 config CHELSIO_T4
 	tristate "Chelsio Communications T4/T5 Ethernet support"
-	depends on PCI
+	depends on PCI && (IPV6 || IPV6=n)
 	select FW_LOADER
 	select MDIO
 	---help---
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
index d57282172ea5..3c481b260745 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4.h
@@ -431,6 +431,7 @@ struct sge_fl {                     /* SGE free-buffer queue state */
 	struct rx_sw_desc *sdesc;   /* address of SW Rx descriptor ring */
 	__be64 *desc;               /* address of HW Rx descriptor ring */
 	dma_addr_t addr;            /* bus address of HW ring start */
+	u64 udb;                    /* BAR2 offset of User Doorbell area */
 };
 
 /* A packet gather list */
@@ -451,6 +452,7 @@ struct sge_rspq {                   /* state for an SGE response queue */
 	u8 gen;                     /* current generation bit */
 	u8 intr_params;             /* interrupt holdoff parameters */
 	u8 next_intr_params;        /* holdoff params for next interrupt */
+	u8 adaptive_rx;
 	u8 pktcnt_idx;              /* interrupt packet threshold */
 	u8 uld;                     /* ULD handling this queue */
 	u8 idx;                     /* queue index within its group */
@@ -459,6 +461,7 @@ struct sge_rspq {                   /* state for an SGE response queue */
 	u16 abs_id;                 /* absolute SGE id for the response q */
 	__be64 *desc;               /* address of HW response ring */
 	dma_addr_t phys_addr;       /* physical address of the ring */
+	u64 udb;                    /* BAR2 offset of User Doorbell area */
 	unsigned int iqe_len;       /* entry size */
 	unsigned int size;          /* capacity of response queue */
 	struct adapter *adap;
@@ -516,7 +519,7 @@ struct sge_txq {
 	int db_disabled;
 	unsigned short db_pidx;
 	unsigned short db_pidx_inc;
-	u64 udb;
+	u64 udb;                    /* BAR2 offset of User Doorbell area */
 };
 
 struct sge_eth_txq {                /* state for an SGE Ethernet Tx queue */
@@ -652,6 +655,7 @@ struct adapter {
 	struct tid_info tids;
 	void **tid_release_head;
 	spinlock_t tid_release_lock;
+	struct workqueue_struct *workq;
 	struct work_struct tid_release_task;
 	struct work_struct db_full_task;
 	struct work_struct db_drop_task;
@@ -964,7 +968,7 @@ void t4_intr_enable(struct adapter *adapter);
 void t4_intr_disable(struct adapter *adapter);
 int t4_slow_intr_handler(struct adapter *adapter);
 
-int t4_wait_dev_ready(struct adapter *adap);
+int t4_wait_dev_ready(void __iomem *regs);
 int t4_link_start(struct adapter *adap, unsigned int mbox, unsigned int port,
 		  struct link_config *lc);
 int t4_restart_aneg(struct adapter *adap, unsigned int mbox, unsigned int port);
@@ -982,6 +986,8 @@ static inline int t4_memory_write(struct adapter *adap, int mtype, u32 addr,
 int t4_seeprom_wp(struct adapter *adapter, bool enable);
 int get_vpd_params(struct adapter *adapter, struct vpd_params *p);
 int t4_load_fw(struct adapter *adapter, const u8 *fw_data, unsigned int size);
+int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
+		  const u8 *fw_data, unsigned int size, int force);
 unsigned int t4_flash_cfg_addr(struct adapter *adapter);
 int t4_get_fw_version(struct adapter *adapter, u32 *vers);
 int t4_get_tp_version(struct adapter *adapter, u32 *vers);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
index 1afee70ce856..3f60070f2519 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/cxgb4_main.c
@@ -283,6 +283,9 @@ static const struct pci_device_id cxgb4_pci_tbl[] = {
 	CH_DEVICE(0x5083, 4),
 	CH_DEVICE(0x5084, 4),
 	CH_DEVICE(0x5085, 4),
+	CH_DEVICE(0x5086, 4),
+	CH_DEVICE(0x5087, 4),
+	CH_DEVICE(0x5088, 4),
 	CH_DEVICE(0x5401, 4),
 	CH_DEVICE(0x5402, 4),
 	CH_DEVICE(0x5403, 4),
@@ -310,6 +313,9 @@ static const struct pci_device_id cxgb4_pci_tbl[] = {
 	CH_DEVICE(0x5483, 4),
 	CH_DEVICE(0x5484, 4),
 	CH_DEVICE(0x5485, 4),
+	CH_DEVICE(0x5486, 4),
+	CH_DEVICE(0x5487, 4),
+	CH_DEVICE(0x5488, 4),
 	{ 0, }
 };
 
@@ -643,8 +649,6 @@ static int set_rxmode(struct net_device *dev, int mtu, bool sleep_ok)
 	return ret;
 }
 
-static struct workqueue_struct *workq;
-
 /**
  *	link_start - enable a port
  *	@dev: the port to enable
@@ -1255,7 +1259,9 @@ freeout:	t4_free_sge_resources(adap);
 			goto freeout;
 	}
 
-	t4_write_reg(adap, MPS_TRC_RSS_CONTROL,
+	t4_write_reg(adap, is_t4(adap->params.chip) ?
+				MPS_TRC_RSS_CONTROL :
+				MPS_T5_TRC_RSS_CONTROL,
 		     RSSCONTROL(netdev2pinfo(adap->port[0])->tx_chan) |
 		     QUEUENUMBER(s->ethrxq[0].rspq.abs_id));
 	return 0;
@@ -1763,7 +1769,8 @@ static void get_regs(struct net_device *dev, struct ethtool_regs *regs,
 		0xd004, 0xd03c,
 		0xdfc0, 0xdfe0,
 		0xe000, 0xea7c,
-		0xf000, 0x11190,
+		0xf000, 0x11110,
+		0x11118, 0x11190,
 		0x19040, 0x1906c,
 		0x19078, 0x19080,
 		0x1908c, 0x19124,
@@ -1970,7 +1977,8 @@ static void get_regs(struct net_device *dev, struct ethtool_regs *regs,
 		0xd004, 0xd03c,
 		0xdfc0, 0xdfe0,
 		0xe000, 0x11088,
-		0x1109c, 0x1117c,
+		0x1109c, 0x11110,
+		0x11118, 0x1117c,
 		0x11190, 0x11204,
 		0x19040, 0x1906c,
 		0x19078, 0x19080,
@@ -2745,8 +2753,31 @@ static int set_rx_intr_params(struct net_device *dev,
 	return 0;
 }
 
+static int set_adaptive_rx_setting(struct net_device *dev, int adaptive_rx)
+{
+	int i;
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
+
+	for (i = 0; i < pi->nqsets; i++, q++)
+		q->rspq.adaptive_rx = adaptive_rx;
+
+	return 0;
+}
+
+static int get_adaptive_rx_setting(struct net_device *dev)
+{
+	struct port_info *pi = netdev_priv(dev);
+	struct adapter *adap = pi->adapter;
+	struct sge_eth_rxq *q = &adap->sge.ethrxq[pi->first_qset];
+
+	return q->rspq.adaptive_rx;
+}
+
 static int set_coalesce(struct net_device *dev, struct ethtool_coalesce *c)
 {
+	set_adaptive_rx_setting(dev, c->use_adaptive_rx_coalesce);
 	return set_rx_intr_params(dev, c->rx_coalesce_usecs,
 				  c->rx_max_coalesced_frames);
 }
@@ -2760,6 +2791,7 @@ static int get_coalesce(struct net_device *dev, struct ethtool_coalesce *c)
 	c->rx_coalesce_usecs = qtimer_val(adap, rq);
 	c->rx_max_coalesced_frames = (rq->intr_params & QINTR_CNT_EN) ?
 		adap->sge.counter_val[rq->pktcnt_idx] : 0;
+	c->use_adaptive_rx_coalesce = get_adaptive_rx_setting(dev);
 	return 0;
 }
 
@@ -2897,16 +2929,26 @@ static int set_flash(struct net_device *netdev, struct ethtool_flash *ef)
 	int ret;
 	const struct firmware *fw;
 	struct adapter *adap = netdev2adap(netdev);
+	unsigned int mbox = FW_PCIE_FW_MASTER_MASK + 1;
 
 	ef->data[sizeof(ef->data) - 1] = '\0';
 	ret = request_firmware(&fw, ef->data, adap->pdev_dev);
 	if (ret < 0)
 		return ret;
 
-	ret = t4_load_fw(adap, fw->data, fw->size);
+	/* If the adapter has been fully initialized then we'll go ahead and
+	 * try to get the firmware's cooperation in upgrading to the new
+	 * firmware image otherwise we'll try to do the entire job from the
+	 * host ... and we always "force" the operation in this path.
+	 */
+	if (adap->flags & FULL_INIT_DONE)
+		mbox = adap->mbox;
+
+	ret = t4_fw_upgrade(adap, mbox, fw->data, fw->size, 1);
 	release_firmware(fw);
 	if (!ret)
-		dev_info(adap->pdev_dev, "loaded firmware %s\n", ef->data);
+		dev_info(adap->pdev_dev, "loaded firmware %s,"
+			 " reload cxgb4 driver\n", ef->data);
 	return ret;
 }
 
@@ -3340,7 +3382,7 @@ static void cxgb4_queue_tid_release(struct tid_info *t, unsigned int chan,
 	adap->tid_release_head = (void **)((uintptr_t)p | chan);
 	if (!adap->tid_release_task_busy) {
 		adap->tid_release_task_busy = true;
-		queue_work(workq, &adap->tid_release_task);
+		queue_work(adap->workq, &adap->tid_release_task);
 	}
 	spin_unlock_bh(&adap->tid_release_lock);
 }
@@ -4140,7 +4182,7 @@ void t4_db_full(struct adapter *adap)
 		notify_rdma_uld(adap, CXGB4_CONTROL_DB_FULL);
 		t4_set_reg_field(adap, SGE_INT_ENABLE3,
 				 DBFIFO_HP_INT | DBFIFO_LP_INT, 0);
-		queue_work(workq, &adap->db_full_task);
+		queue_work(adap->workq, &adap->db_full_task);
 	}
 }
 
@@ -4150,7 +4192,7 @@ void t4_db_dropped(struct adapter *adap)
 		disable_dbs(adap);
 		notify_rdma_uld(adap, CXGB4_CONTROL_DB_FULL);
 	}
-	queue_work(workq, &adap->db_drop_task);
+	queue_work(adap->workq, &adap->db_drop_task);
 }
 
 static void uld_attach(struct adapter *adap, unsigned int uld)
@@ -4327,6 +4369,7 @@ EXPORT_SYMBOL(cxgb4_unregister_uld);
  * success (true) if it belongs otherwise failure (false).
  * Called with rcu_read_lock() held.
  */
+#if IS_ENABLED(CONFIG_IPV6)
 static bool cxgb4_netdev(const struct net_device *netdev)
 {
 	struct adapter *adap;
@@ -4388,7 +4431,6 @@ static int cxgb4_inet6addr_handler(struct notifier_block *this,
 		 * bond. We need to find such different adapters and add clip
 		 * in all of them only once.
 		 */
-		read_lock(&bond->lock);
 		bond_for_each_slave(bond, slave, iter) {
 			if (!first_pdev) {
 				ret = clip_add(slave->dev, ifa, event);
@@ -4402,7 +4444,6 @@ static int cxgb4_inet6addr_handler(struct notifier_block *this,
 				   to_pci_dev(slave->dev->dev.parent))
 					ret = clip_add(slave->dev, ifa, event);
 		}
-		read_unlock(&bond->lock);
 	} else
 		ret = clip_add(ifa->idev->dev, ifa, event);
 
@@ -4450,6 +4491,13 @@ static int update_root_dev_clip(struct net_device *dev)
 		return ret;
 
 	/* Parse all bond and vlan devices layered on top of the physical dev */
+	root_dev = netdev_master_upper_dev_get_rcu(dev);
+	if (root_dev) {
+		ret = update_dev_clip(root_dev, dev);
+		if (ret)
+			return ret;
+	}
+
 	for (i = 0; i < VLAN_N_VID; i++) {
 		root_dev = __vlan_find_dev_deep_rcu(dev, htons(ETH_P_8021Q), i);
 		if (!root_dev)
@@ -4482,6 +4530,7 @@ static void update_clip(const struct adapter *adap)
 	}
 	rcu_read_unlock();
 }
+#endif /* IS_ENABLED(CONFIG_IPV6) */
 
 /**
  *	cxgb_up - enable the adapter
@@ -4528,7 +4577,9 @@ static int cxgb_up(struct adapter *adap)
 	t4_intr_enable(adap);
 	adap->flags |= FULL_INIT_DONE;
 	notify_ulds(adap, CXGB4_STATE_UP);
+#if IS_ENABLED(CONFIG_IPV6)
 	update_clip(adap);
+#endif
  out:
 	return err;
  irq_err:
@@ -5957,7 +6008,8 @@ static int adap_init0(struct adapter *adap)
 		params[3] = FW_PARAM_PFVF(CQ_END);
 		params[4] = FW_PARAM_PFVF(OCQ_START);
 		params[5] = FW_PARAM_PFVF(OCQ_END);
-		ret = t4_query_params(adap, 0, 0, 0, 6, params, val);
+		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 6, params,
+				      val);
 		if (ret < 0)
 			goto bye;
 		adap->vres.qp.start = val[0];
@@ -5969,7 +6021,8 @@ static int adap_init0(struct adapter *adap)
 
 		params[0] = FW_PARAM_DEV(MAXORDIRD_QP);
 		params[1] = FW_PARAM_DEV(MAXIRD_ADAPTER);
-		ret = t4_query_params(adap, 0, 0, 0, 2, params, val);
+		ret = t4_query_params(adap, adap->mbox, adap->fn, 0, 2, params,
+				      val);
 		if (ret < 0) {
 			adap->params.max_ordird_qp = 8;
 			adap->params.max_ird_adapter = 32 * adap->tids.ntids;
@@ -6105,7 +6158,7 @@ static pci_ers_result_t eeh_slot_reset(struct pci_dev *pdev)
 	pci_save_state(pdev);
 	pci_cleanup_aer_uncorrect_error_status(pdev);
 
-	if (t4_wait_dev_ready(adap) < 0)
+	if (t4_wait_dev_ready(adap->regs) < 0)
 		return PCI_ERS_RESULT_DISCONNECT;
 	if (t4_fw_hello(adap, adap->fn, adap->fn, MASTER_MUST, NULL) < 0)
 		return PCI_ERS_RESULT_DISCONNECT;
@@ -6474,6 +6527,7 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct port_info *pi;
 	bool highdma = false;
 	struct adapter *adapter = NULL;
+	void __iomem *regs;
 
 	printk_once(KERN_INFO "%s - version %s\n", DRV_DESC, DRV_VERSION);
 
@@ -6490,19 +6544,39 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out_release_regions;
 	}
 
+	regs = pci_ioremap_bar(pdev, 0);
+	if (!regs) {
+		dev_err(&pdev->dev, "cannot map device registers\n");
+		err = -ENOMEM;
+		goto out_disable_device;
+	}
+
+	err = t4_wait_dev_ready(regs);
+	if (err < 0)
+		goto out_unmap_bar0;
+
+	/* We control everything through one PF */
+	func = SOURCEPF_GET(readl(regs + PL_WHOAMI));
+	if (func != ent->driver_data) {
+		iounmap(regs);
+		pci_disable_device(pdev);
+		pci_save_state(pdev);        /* to restore SR-IOV later */
+		goto sriov;
+	}
+
 	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(64))) {
 		highdma = true;
 		err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
 		if (err) {
 			dev_err(&pdev->dev, "unable to obtain 64-bit DMA for "
 				"coherent allocations\n");
-			goto out_disable_device;
+			goto out_unmap_bar0;
 		}
 	} else {
 		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (err) {
 			dev_err(&pdev->dev, "no usable DMA configuration\n");
-			goto out_disable_device;
+			goto out_unmap_bar0;
 		}
 	}
 
@@ -6514,26 +6588,19 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	adapter = kzalloc(sizeof(*adapter), GFP_KERNEL);
 	if (!adapter) {
 		err = -ENOMEM;
-		goto out_disable_device;
+		goto out_unmap_bar0;
 	}
 
-	/* PCI device has been enabled */
-	adapter->flags |= DEV_ENABLED;
-
-	adapter->regs = pci_ioremap_bar(pdev, 0);
-	if (!adapter->regs) {
-		dev_err(&pdev->dev, "cannot map device registers\n");
+	adapter->workq = create_singlethread_workqueue("cxgb4");
+	if (!adapter->workq) {
 		err = -ENOMEM;
 		goto out_free_adapter;
 	}
 
-	/* We control everything through one PF */
-	func = SOURCEPF_GET(readl(adapter->regs + PL_WHOAMI));
-	if (func != ent->driver_data) {
-		pci_save_state(pdev);        /* to restore SR-IOV later */
-		goto sriov;
-	}
+	/* PCI device has been enabled */
+	adapter->flags |= DEV_ENABLED;
 
+	adapter->regs = regs;
 	adapter->pdev = pdev;
 	adapter->pdev_dev = &pdev->dev;
 	adapter->mbox = func;
@@ -6550,7 +6617,8 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	err = t4_prep_adapter(adapter);
 	if (err)
-		goto out_unmap_bar0;
+		goto out_free_adapter;
+
 
 	if (!is_t4(adapter->params.chip)) {
 		s_qpp = QUEUESPERPAGEPF1 * adapter->fn;
@@ -6567,14 +6635,14 @@ static int init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			dev_err(&pdev->dev,
 				"Incorrect number of egress queues per page\n");
 			err = -EINVAL;
-			goto out_unmap_bar0;
+			goto out_free_adapter;
 		}
 		adapter->bar2 = ioremap_wc(pci_resource_start(pdev, 2),
 		pci_resource_len(pdev, 2));
 		if (!adapter->bar2) {
 			dev_err(&pdev->dev, "cannot map device bar2 region\n");
 			err = -ENOMEM;
-			goto out_unmap_bar0;
+			goto out_free_adapter;
 		}
 	}
 
@@ -6712,10 +6780,13 @@ sriov:
  out_unmap_bar:
 	if (!is_t4(adapter->params.chip))
 		iounmap(adapter->bar2);
- out_unmap_bar0:
-	iounmap(adapter->regs);
  out_free_adapter:
+	if (adapter->workq)
+		destroy_workqueue(adapter->workq);
+
 	kfree(adapter);
+ out_unmap_bar0:
+	iounmap(regs);
  out_disable_device:
 	pci_disable_pcie_error_reporting(pdev);
 	pci_disable_device(pdev);
@@ -6736,6 +6807,11 @@ static void remove_one(struct pci_dev *pdev)
 	if (adapter) {
 		int i;
 
+		/* Tear down per-adapter Work Queue first since it can contain
+		 * references to our adapter data structure.
+		 */
+		destroy_workqueue(adapter->workq);
+
 		if (is_offload(adapter))
 			detach_ulds(adapter);
 
@@ -6788,33 +6864,29 @@ static int __init cxgb4_init_module(void)
 {
 	int ret;
 
-	workq = create_singlethread_workqueue("cxgb4");
-	if (!workq)
-		return -ENOMEM;
-
 	/* Debugfs support is optional, just warn if this fails */
 	cxgb4_debugfs_root = debugfs_create_dir(KBUILD_MODNAME, NULL);
 	if (!cxgb4_debugfs_root)
 		pr_warn("could not create debugfs entry, continuing\n");
 
 	ret = pci_register_driver(&cxgb4_driver);
-	if (ret < 0) {
+	if (ret < 0)
 		debugfs_remove(cxgb4_debugfs_root);
-		destroy_workqueue(workq);
-	}
 
+#if IS_ENABLED(CONFIG_IPV6)
 	register_inet6addr_notifier(&cxgb4_inet6addr_notifier);
+#endif
 
 	return ret;
 }
 
 static void __exit cxgb4_cleanup_module(void)
 {
+#if IS_ENABLED(CONFIG_IPV6)
 	unregister_inet6addr_notifier(&cxgb4_inet6addr_notifier);
+#endif
 	pci_unregister_driver(&cxgb4_driver);
 	debugfs_remove(cxgb4_debugfs_root);  /* NULL ok */
-	flush_workqueue(workq);
-	destroy_workqueue(workq);
 }
 
 module_init(cxgb4_init_module);
diff --git a/drivers/net/ethernet/chelsio/cxgb4/sge.c b/drivers/net/ethernet/chelsio/cxgb4/sge.c
index b0bba32d69d5..5e1b314e11af 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/sge.c
@@ -203,6 +203,9 @@ enum {
 	RX_LARGE_MTU_BUF = 0x3,   /* large MTU buffer */
 };
 
+static int timer_pkt_quota[] = {1, 1, 2, 3, 4, 5};
+#define MIN_NAPI_WORK  1
+
 static inline dma_addr_t get_buf_addr(const struct rx_sw_desc *d)
 {
 	return d->dma_addr & ~(dma_addr_t)RX_BUF_FLAGS;
@@ -521,9 +524,23 @@ static inline void ring_fl_db(struct adapter *adap, struct sge_fl *q)
 		val = PIDX(q->pend_cred / 8);
 		if (!is_t4(adap->params.chip))
 			val |= DBTYPE(1);
+		val |= DBPRIO(1);
 		wmb();
-		t4_write_reg(adap, MYPF_REG(SGE_PF_KDOORBELL), DBPRIO(1) |
-			     QID(q->cntxt_id) | val);
+
+		/* If we're on T4, use the old doorbell mechanism; otherwise
+		 * use the new BAR2 mechanism.
+		 */
+		if (is_t4(adap->params.chip)) {
+			t4_write_reg(adap, MYPF_REG(SGE_PF_KDOORBELL),
+				     val | QID(q->cntxt_id));
+		} else {
+			writel(val,  adap->bar2 + q->udb + SGE_UDB_KDOORBELL);
+
+			/* This Write memory Barrier will force the write to
+			 * the User Doorbell area to be flushed.
+			 */
+			wmb();
+		}
 		q->pend_cred &= 7;
 	}
 }
@@ -833,13 +850,14 @@ static void write_sgl(const struct sk_buff *skb, struct sge_txq *q,
 		*end = 0;
 }
 
-/* This function copies 64 byte coalesced work request to
- * memory mapped BAR2 space(user space writes).
- * For coalesced WR SGE, fetches data from the FIFO instead of from Host.
+/* This function copies a tx_desc struct to memory mapped BAR2 space(user space
+ * writes). For coalesced WR SGE, fetches data from the FIFO instead of from
+ * Host.
  */
-static void cxgb_pio_copy(u64 __iomem *dst, u64 *src)
+static void cxgb_pio_copy(u64 __iomem *dst, struct tx_desc *desc)
 {
-	int count = 8;
+	int count = sizeof(*desc) / sizeof(u64);
+	u64 *src = (u64 *)desc;
 
 	while (count) {
 		writeq(*src, dst);
@@ -859,30 +877,63 @@ static void cxgb_pio_copy(u64 __iomem *dst, u64 *src)
  */
 static inline void ring_tx_db(struct adapter *adap, struct sge_txq *q, int n)
 {
-	unsigned int *wr, index;
-	unsigned long flags;
-
 	wmb();            /* write descriptors before telling HW */
-	spin_lock_irqsave(&q->db_lock, flags);
-	if (!q->db_disabled) {
-		if (is_t4(adap->params.chip)) {
+
+	if (is_t4(adap->params.chip)) {
+		u32 val = PIDX(n);
+		unsigned long flags;
+
+		/* For T4 we need to participate in the Doorbell Recovery
+		 * mechanism.
+		 */
+		spin_lock_irqsave(&q->db_lock, flags);
+		if (!q->db_disabled)
 			t4_write_reg(adap, MYPF_REG(SGE_PF_KDOORBELL),
-				     QID(q->cntxt_id) | PIDX(n));
+				     QID(q->cntxt_id) | val);
+		else
+			q->db_pidx_inc += n;
+		q->db_pidx = q->pidx;
+		spin_unlock_irqrestore(&q->db_lock, flags);
+	} else {
+		u32 val = PIDX_T5(n);
+
+		/* T4 and later chips share the same PIDX field offset within
+		 * the doorbell, but T5 and later shrank the field in order to
+		 * gain a bit for Doorbell Priority.  The field was absurdly
+		 * large in the first place (14 bits) so we just use the T5
+		 * and later limits and warn if a Queue ID is too large.
+		 */
+		WARN_ON(val & DBPRIO(1));
+
+		/* For T5 and later we use the Write-Combine mapped BAR2 User
+		 * Doorbell mechanism.  If we're only writing a single TX
+		 * Descriptor and TX Write Combining hasn't been disabled, we
+		 * can use the Write Combining Gather Buffer; otherwise we use
+		 * the simple doorbell.
+		 */
+		if (n == 1) {
+			int index = (q->pidx
+				     ? (q->pidx - 1)
+				     : (q->size - 1));
+
+			cxgb_pio_copy(adap->bar2 + q->udb + SGE_UDB_WCDOORBELL,
+				      q->desc + index);
 		} else {
-			if (n == 1) {
-				index = q->pidx ? (q->pidx - 1) : (q->size - 1);
-				wr = (unsigned int *)&q->desc[index];
-				cxgb_pio_copy((u64 __iomem *)
-					      (adap->bar2 + q->udb + 64),
-					      (u64 *)wr);
-			} else
-				writel(n,  adap->bar2 + q->udb + 8);
-			wmb();
+			writel(val,  adap->bar2 + q->udb + SGE_UDB_KDOORBELL);
 		}
-	} else
-		q->db_pidx_inc += n;
-	q->db_pidx = q->pidx;
-	spin_unlock_irqrestore(&q->db_lock, flags);
+
+		/* This Write Memory Barrier will force the write to the User
+		 * Doorbell area to be flushed.  This is needed to prevent
+		 * writes on different CPUs for the same queue from hitting
+		 * the adapter out of order.  This is required when some Work
+		 * Requests take the Write Combine Gather Buffer path (user
+		 * doorbell area offset [SGE_UDB_WCDOORBELL..+63]) and some
+		 * take the traditional path where we simply increment the
+		 * PIDX (User Doorbell area SGE_UDB_KDOORBELL) and have the
+		 * hardware DMA read the actual Work Request.
+		 */
+		wmb();
+	}
 }
 
 /**
@@ -1072,7 +1123,10 @@ out_free:	dev_kfree_skb_any(skb);
 		lso->c.ipid_ofst = htons(0);
 		lso->c.mss = htons(ssi->gso_size);
 		lso->c.seqno_offset = htonl(0);
-		lso->c.len = htonl(skb->len);
+		if (is_t4(adap->params.chip))
+			lso->c.len = htonl(skb->len);
+		else
+			lso->c.len = htonl(LSO_T5_XFER_SIZE(skb->len));
 		cpl = (void *)(lso + 1);
 		cntrl = TXPKT_CSUM_TYPE(v6 ? TX_CSUM_TCPIP6 : TX_CSUM_TCPIP) |
 			TXPKT_IPHDR_LEN(l3hdr_len) |
@@ -1916,16 +1970,40 @@ static int napi_rx_handler(struct napi_struct *napi, int budget)
 	unsigned int params;
 	struct sge_rspq *q = container_of(napi, struct sge_rspq, napi);
 	int work_done = process_responses(q, budget);
+	u32 val;
 
 	if (likely(work_done < budget)) {
+		int timer_index;
+
 		napi_complete(napi);
-		params = q->next_intr_params;
-		q->next_intr_params = q->intr_params;
+		timer_index = QINTR_TIMER_IDX_GET(q->next_intr_params);
+
+		if (q->adaptive_rx) {
+			if (work_done > max(timer_pkt_quota[timer_index],
+					    MIN_NAPI_WORK))
+				timer_index = (timer_index + 1);
+			else
+				timer_index = timer_index - 1;
+
+			timer_index = clamp(timer_index, 0, SGE_TIMERREGS - 1);
+			q->next_intr_params = QINTR_TIMER_IDX(timer_index) |
+							      V_QINTR_CNT_EN;
+			params = q->next_intr_params;
+		} else {
+			params = q->next_intr_params;
+			q->next_intr_params = q->intr_params;
+		}
 	} else
 		params = QINTR_TIMER_IDX(7);
 
-	t4_write_reg(q->adap, MYPF_REG(SGE_PF_GTS), CIDXINC(work_done) |
-		     INGRESSQID((u32)q->cntxt_id) | SEINTARM(params));
+	val = CIDXINC(work_done) | SEINTARM(params);
+	if (is_t4(q->adap->params.chip)) {
+		t4_write_reg(q->adap, MYPF_REG(SGE_PF_GTS),
+			     val | INGRESSQID((u32)q->cntxt_id));
+	} else {
+		writel(val, q->adap->bar2 + q->udb + SGE_UDB_GTS);
+		wmb();
+	}
 	return work_done;
 }
 
@@ -1949,6 +2027,7 @@ static unsigned int process_intrq(struct adapter *adap)
 	unsigned int credits;
 	const struct rsp_ctrl *rc;
 	struct sge_rspq *q = &adap->sge.intrq;
+	u32 val;
 
 	spin_lock(&adap->sge.intrq_lock);
 	for (credits = 0; ; credits++) {
@@ -1967,8 +2046,14 @@ static unsigned int process_intrq(struct adapter *adap)
 		rspq_next(q);
 	}
 
-	t4_write_reg(adap, MYPF_REG(SGE_PF_GTS), CIDXINC(credits) |
-		     INGRESSQID(q->cntxt_id) | SEINTARM(q->intr_params));
+	val =  CIDXINC(credits) | SEINTARM(q->intr_params);
+	if (is_t4(adap->params.chip)) {
+		t4_write_reg(adap, MYPF_REG(SGE_PF_GTS),
+			     val | INGRESSQID(q->cntxt_id));
+	} else {
+		writel(val, adap->bar2 + q->udb + SGE_UDB_GTS);
+		wmb();
+	}
 	spin_unlock(&adap->sge.intrq_lock);
 	return credits;
 }
@@ -2149,6 +2234,51 @@ static void sge_tx_timer_cb(unsigned long data)
 	mod_timer(&s->tx_timer, jiffies + (budget ? TX_QCHECK_PERIOD : 2));
 }
 
+/**
+ *      udb_address - return the BAR2 User Doorbell address for a Queue
+ *      @adap: the adapter
+ *      @cntxt_id: the Queue Context ID
+ *      @qpp: Queues Per Page (for all PFs)
+ *
+ *      Returns the BAR2 address of the user Doorbell associated with the
+ *      indicated Queue Context ID.  Note that this is only applicable
+ *      for T5 and later.
+ */
+static u64 udb_address(struct adapter *adap, unsigned int cntxt_id,
+		       unsigned int qpp)
+{
+	u64 udb;
+	unsigned int s_qpp;
+	unsigned short udb_density;
+	unsigned long qpshift;
+	int page;
+
+	BUG_ON(is_t4(adap->params.chip));
+
+	s_qpp = (QUEUESPERPAGEPF0 +
+		(QUEUESPERPAGEPF1 - QUEUESPERPAGEPF0) * adap->fn);
+	udb_density = 1 << ((qpp >> s_qpp) & QUEUESPERPAGEPF0_MASK);
+	qpshift = PAGE_SHIFT - ilog2(udb_density);
+	udb = (u64)cntxt_id << qpshift;
+	udb &= PAGE_MASK;
+	page = udb / PAGE_SIZE;
+	udb += (cntxt_id - (page * udb_density)) * SGE_UDB_SIZE;
+
+	return udb;
+}
+
+static u64 udb_address_eq(struct adapter *adap, unsigned int cntxt_id)
+{
+	return udb_address(adap, cntxt_id,
+			   t4_read_reg(adap, SGE_EGRESS_QUEUES_PER_PAGE_PF));
+}
+
+static u64 udb_address_iq(struct adapter *adap, unsigned int cntxt_id)
+{
+	return udb_address(adap, cntxt_id,
+			   t4_read_reg(adap, SGE_INGRESS_QUEUES_PER_PAGE_PF));
+}
+
 int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 		     struct net_device *dev, int intr_idx,
 		     struct sge_fl *fl, rspq_handler_t hnd)
@@ -2214,6 +2344,8 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 	iq->next_intr_params = iq->intr_params;
 	iq->cntxt_id = ntohs(c.iqid);
 	iq->abs_id = ntohs(c.physiqid);
+	if (!is_t4(adap->params.chip))
+		iq->udb = udb_address_iq(adap, iq->cntxt_id);
 	iq->size--;                           /* subtract status entry */
 	iq->netdev = dev;
 	iq->handler = hnd;
@@ -2229,6 +2361,12 @@ int t4_sge_alloc_rxq(struct adapter *adap, struct sge_rspq *iq, bool fwevtq,
 		fl->pidx = fl->cidx = 0;
 		fl->alloc_failed = fl->large_alloc_failed = fl->starving = 0;
 		adap->sge.egr_map[fl->cntxt_id - adap->sge.egr_start] = fl;
+
+		/* Note, we must initialize the Free List User Doorbell
+		 * address before refilling the Free List!
+		 */
+		if (!is_t4(adap->params.chip))
+			fl->udb = udb_address_eq(adap, fl->cntxt_id);
 		refill_fl(adap, fl, fl_cap(fl), GFP_KERNEL);
 	}
 	return 0;
@@ -2254,21 +2392,8 @@ err:
 static void init_txq(struct adapter *adap, struct sge_txq *q, unsigned int id)
 {
 	q->cntxt_id = id;
-	if (!is_t4(adap->params.chip)) {
-		unsigned int s_qpp;
-		unsigned short udb_density;
-		unsigned long qpshift;
-		int page;
-
-		s_qpp = QUEUESPERPAGEPF1 * adap->fn;
-		udb_density = 1 << QUEUESPERPAGEPF0_GET((t4_read_reg(adap,
-				SGE_EGRESS_QUEUES_PER_PAGE_PF) >> s_qpp));
-		qpshift = PAGE_SHIFT - ilog2(udb_density);
-		q->udb = q->cntxt_id << qpshift;
-		q->udb &= PAGE_MASK;
-		page = q->udb / PAGE_SIZE;
-		q->udb += (q->cntxt_id - (page * udb_density)) * 128;
-	}
+	if (!is_t4(adap->params.chip))
+		q->udb = udb_address_eq(adap, q->cntxt_id);
 
 	q->in_use = 0;
 	q->cidx = q->pidx = 0;
@@ -2303,7 +2428,8 @@ int t4_sge_alloc_eth_txq(struct adapter *adap, struct sge_eth_txq *txq,
 			    FW_EQ_ETH_CMD_PFN(adap->fn) | FW_EQ_ETH_CMD_VFN(0));
 	c.alloc_to_len16 = htonl(FW_EQ_ETH_CMD_ALLOC |
 				 FW_EQ_ETH_CMD_EQSTART | FW_LEN16(c));
-	c.viid_pkd = htonl(FW_EQ_ETH_CMD_VIID(pi->viid));
+	c.viid_pkd = htonl(FW_EQ_ETH_CMD_AUTOEQUEQE |
+			   FW_EQ_ETH_CMD_VIID(pi->viid));
 	c.fetchszm_to_iqid = htonl(FW_EQ_ETH_CMD_HOSTFCMODE(2) |
 				   FW_EQ_ETH_CMD_PCIECHN(pi->tx_chan) |
 				   FW_EQ_ETH_CMD_FETCHRO(1) |
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
index a853133d8db8..a9d9d74e4f09 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.c
@@ -37,8 +37,6 @@
 #include "t4_regs.h"
 #include "t4fw_api.h"
 
-static int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
-			 const u8 *fw_data, unsigned int size, int force);
 /**
  *	t4_wait_op_done_val - wait until an operation is completed
  *	@adapter: the adapter performing the operation
@@ -168,6 +166,34 @@ void t4_hw_pci_read_cfg4(struct adapter *adap, int reg, u32 *val)
 }
 
 /*
+ * t4_report_fw_error - report firmware error
+ * @adap: the adapter
+ *
+ * The adapter firmware can indicate error conditions to the host.
+ * If the firmware has indicated an error, print out the reason for
+ * the firmware error.
+ */
+static void t4_report_fw_error(struct adapter *adap)
+{
+	static const char *const reason[] = {
+		"Crash",                        /* PCIE_FW_EVAL_CRASH */
+		"During Device Preparation",    /* PCIE_FW_EVAL_PREP */
+		"During Device Configuration",  /* PCIE_FW_EVAL_CONF */
+		"During Device Initialization", /* PCIE_FW_EVAL_INIT */
+		"Unexpected Event",             /* PCIE_FW_EVAL_UNEXPECTEDEVENT */
+		"Insufficient Airflow",         /* PCIE_FW_EVAL_OVERHEAT */
+		"Device Shutdown",              /* PCIE_FW_EVAL_DEVICESHUTDOWN */
+		"Reserved",                     /* reserved */
+	};
+	u32 pcie_fw;
+
+	pcie_fw = t4_read_reg(adap, MA_PCIE_FW);
+	if (pcie_fw & FW_PCIE_FW_ERR)
+		dev_err(adap->pdev_dev, "Firmware reports adapter error: %s\n",
+			reason[FW_PCIE_FW_EVAL_GET(pcie_fw)]);
+}
+
+/*
  * Get the reply to a mailbox command and store it in @rpl in big-endian order.
  */
 static void get_mbox_rpl(struct adapter *adap, __be64 *rpl, int nflit,
@@ -300,6 +326,7 @@ int t4_wr_mbox_meat(struct adapter *adap, int mbox, const void *cmd, int size,
 	dump_mbox(adap, mbox, data_reg);
 	dev_err(adap->pdev_dev, "command %#x in mailbox %d timed out\n",
 		*(const u8 *)cmd, mbox);
+	t4_report_fw_error(adap);
 	return -ETIMEDOUT;
 }
 
@@ -566,6 +593,7 @@ int t4_memory_rw(struct adapter *adap, int win, int mtype, u32 addr,
 #define VPD_BASE           0x400
 #define VPD_BASE_OLD       0
 #define VPD_LEN            1024
+#define CHELSIO_VPD_UNIQUE_ID 0x82
 
 /**
  *	t4_seeprom_wp - enable/disable EEPROM write protection
@@ -603,7 +631,14 @@ int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
 	ret = pci_read_vpd(adapter->pdev, VPD_BASE, sizeof(u32), vpd);
 	if (ret < 0)
 		goto out;
-	addr = *vpd == 0x82 ? VPD_BASE : VPD_BASE_OLD;
+
+	/* The VPD shall have a unique identifier specified by the PCI SIG.
+	 * For chelsio adapters, the identifier is 0x82. The first byte of a VPD
+	 * shall be CHELSIO_VPD_UNIQUE_ID (0x82). The VPD programming software
+	 * is expected to automatically put this entry at the
+	 * beginning of the VPD.
+	 */
+	addr = *vpd == CHELSIO_VPD_UNIQUE_ID ? VPD_BASE : VPD_BASE_OLD;
 
 	ret = pci_read_vpd(adapter->pdev, addr, VPD_LEN, vpd);
 	if (ret < 0)
@@ -667,6 +702,7 @@ int get_vpd_params(struct adapter *adapter, struct vpd_params *p)
 	i = pci_vpd_info_field_size(vpd + sn - PCI_VPD_INFO_FLD_HDR_SIZE);
 	memcpy(p->sn, vpd + sn, min(i, SERNUM_LEN));
 	strim(p->sn);
+	i = pci_vpd_info_field_size(vpd + pn - PCI_VPD_INFO_FLD_HDR_SIZE);
 	memcpy(p->pn, vpd + pn, min(i, PN_LEN));
 	strim(p->pn);
 
@@ -1061,6 +1097,9 @@ static int t4_flash_erase_sectors(struct adapter *adapter, int start, int end)
 {
 	int ret = 0;
 
+	if (end >= adapter->params.sf_nsec)
+		return -EINVAL;
+
 	while (start <= end) {
 		if ((ret = sf1_write(adapter, 1, 0, 1, SF_WR_ENABLE)) != 0 ||
 		    (ret = sf1_write(adapter, 4, 0, 1,
@@ -1394,15 +1433,18 @@ static void pcie_intr_handler(struct adapter *adapter)
 
 	int fat;
 
-	fat = t4_handle_intr_status(adapter,
-				    PCIE_CORE_UTL_SYSTEM_BUS_AGENT_STATUS,
-				    sysbus_intr_info) +
-	      t4_handle_intr_status(adapter,
-				    PCIE_CORE_UTL_PCI_EXPRESS_PORT_STATUS,
-				    pcie_port_intr_info) +
-	      t4_handle_intr_status(adapter, PCIE_INT_CAUSE,
-				    is_t4(adapter->params.chip) ?
-				    pcie_intr_info : t5_pcie_intr_info);
+	if (is_t4(adapter->params.chip))
+		fat = t4_handle_intr_status(adapter,
+					    PCIE_CORE_UTL_SYSTEM_BUS_AGENT_STATUS,
+					    sysbus_intr_info) +
+			t4_handle_intr_status(adapter,
+					      PCIE_CORE_UTL_PCI_EXPRESS_PORT_STATUS,
+					      pcie_port_intr_info) +
+			t4_handle_intr_status(adapter, PCIE_INT_CAUSE,
+					      pcie_intr_info);
+	else
+		fat = t4_handle_intr_status(adapter, PCIE_INT_CAUSE,
+					    t5_pcie_intr_info);
 
 	if (fat)
 		t4_fatal_err(adapter);
@@ -1521,6 +1563,9 @@ static void cim_intr_handler(struct adapter *adapter)
 
 	int fat;
 
+	if (t4_read_reg(adapter, MA_PCIE_FW) & FW_PCIE_FW_ERR)
+		t4_report_fw_error(adapter);
+
 	fat = t4_handle_intr_status(adapter, CIM_HOST_INT_CAUSE,
 				    cim_intr_info) +
 	      t4_handle_intr_status(adapter, CIM_HOST_UPACC_INT_CAUSE,
@@ -1768,10 +1813,16 @@ static void ma_intr_handler(struct adapter *adap)
 {
 	u32 v, status = t4_read_reg(adap, MA_INT_CAUSE);
 
-	if (status & MEM_PERR_INT_CAUSE)
+	if (status & MEM_PERR_INT_CAUSE) {
 		dev_alert(adap->pdev_dev,
 			  "MA parity error, parity status %#x\n",
 			  t4_read_reg(adap, MA_PARITY_ERROR_STATUS));
+		if (is_t5(adap->params.chip))
+			dev_alert(adap->pdev_dev,
+				  "MA parity error, parity status %#x\n",
+				  t4_read_reg(adap,
+					      MA_PARITY_ERROR_STATUS2));
+	}
 	if (status & MEM_WRAP_INT_CAUSE) {
 		v = t4_read_reg(adap, MA_INT_WRAP_STATUS);
 		dev_alert(adap->pdev_dev, "MA address wrap-around error by "
@@ -2733,12 +2784,16 @@ retry:
 	/*
 	 * Issue the HELLO command to the firmware.  If it's not successful
 	 * but indicates that we got a "busy" or "timeout" condition, retry
-	 * the HELLO until we exhaust our retry limit.
+	 * the HELLO until we exhaust our retry limit.  If we do exceed our
+	 * retry limit, check to see if the firmware left us any error
+	 * information and report that if so.
 	 */
 	ret = t4_wr_mbox(adap, mbox, &c, sizeof(c), &c);
 	if (ret < 0) {
 		if ((ret == -EBUSY || ret == -ETIMEDOUT) && retries-- > 0)
 			goto retry;
+		if (t4_read_reg(adap, MA_PCIE_FW) & FW_PCIE_FW_ERR)
+			t4_report_fw_error(adap);
 		return ret;
 	}
 
@@ -3019,8 +3074,8 @@ static int t4_fw_restart(struct adapter *adap, unsigned int mbox, int reset)
  *	positive errno indicates that the adapter is ~probably~ intact, a
  *	negative errno indicates that things are looking bad ...
  */
-static int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
-			 const u8 *fw_data, unsigned int size, int force)
+int t4_fw_upgrade(struct adapter *adap, unsigned int mbox,
+		  const u8 *fw_data, unsigned int size, int force)
 {
 	const struct fw_hdr *fw_hdr = (const struct fw_hdr *)fw_data;
 	int reset, ret;
@@ -3742,6 +3797,7 @@ int t4_handle_fw_rpl(struct adapter *adap, const __be64 *rpl)
 			lc->link_ok = link_ok;
 			lc->speed = speed;
 			lc->fc = fc;
+			lc->supported = be16_to_cpu(p->u.info.pcap);
 			t4_os_link_changed(adap, port, link_ok);
 		}
 		if (mod != pi->mod_type) {
@@ -3787,16 +3843,35 @@ static void init_link_config(struct link_config *lc, unsigned int caps)
 	}
 }
 
-int t4_wait_dev_ready(struct adapter *adap)
+#define CIM_PF_NOACCESS 0xeeeeeeee
+
+int t4_wait_dev_ready(void __iomem *regs)
 {
-	if (t4_read_reg(adap, PL_WHOAMI) != 0xffffffff)
+	u32 whoami;
+
+	whoami = readl(regs + PL_WHOAMI);
+	if (whoami != 0xffffffff && whoami != CIM_PF_NOACCESS)
 		return 0;
+
 	msleep(500);
-	return t4_read_reg(adap, PL_WHOAMI) != 0xffffffff ? 0 : -EIO;
+	whoami = readl(regs + PL_WHOAMI);
+	return (whoami != 0xffffffff && whoami != CIM_PF_NOACCESS ? 0 : -EIO);
 }
 
+struct flash_desc {
+	u32 vendor_and_model_id;
+	u32 size_mb;
+};
+
 static int get_flash_params(struct adapter *adap)
 {
+	/* Table for non-Numonix supported flash parts.  Numonix parts are left
+	 * to the preexisting code.  All flash parts have 64KB sectors.
+	 */
+	static struct flash_desc supported_flash[] = {
+		{ 0x150201, 4 << 20 },       /* Spansion 4MB S25FL032P */
+	};
+
 	int ret;
 	u32 info;
 
@@ -3807,6 +3882,14 @@ static int get_flash_params(struct adapter *adap)
 	if (ret)
 		return ret;
 
+	for (ret = 0; ret < ARRAY_SIZE(supported_flash); ++ret)
+		if (supported_flash[ret].vendor_and_model_id == info) {
+			adap->params.sf_size = supported_flash[ret].size_mb;
+			adap->params.sf_nsec =
+				adap->params.sf_size / SF_SEC_SIZE;
+			return 0;
+		}
+
 	if ((info & 0xff) != 0x20)             /* not a Numonix flash */
 		return -EINVAL;
 	info >>= 16;                           /* log2 of size */
@@ -3819,6 +3902,10 @@ static int get_flash_params(struct adapter *adap)
 	adap->params.sf_size = 1 << info;
 	adap->params.sf_fw_start =
 		t4_read_reg(adap, CIM_BOOT_CFG) & BOOTADDR_MASK;
+
+	if (adap->params.sf_size < FLASH_MIN_SIZE)
+		dev_warn(adap->pdev_dev, "WARNING!!! FLASH size %#x < %#x!!!\n",
+			 adap->params.sf_size, FLASH_MIN_SIZE);
 	return 0;
 }
 
@@ -3837,10 +3924,6 @@ int t4_prep_adapter(struct adapter *adapter)
 	uint16_t device_id;
 	u32 pl_rev;
 
-	ret = t4_wait_dev_ready(adapter);
-	if (ret < 0)
-		return ret;
-
 	get_pci_mode(adapter, &adapter->params.pci);
 	pl_rev = G_REV(t4_read_reg(adapter, PL_REV));
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
index 35e3d8e32881..c19a90e7f7d1 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_hw.h
@@ -135,6 +135,7 @@ struct rsp_ctrl {
 #define RSPD_GEN(x)  ((x) >> 7)
 #define RSPD_TYPE(x) (((x) >> 4) & 3)
 
+#define V_QINTR_CNT_EN	   0x0
 #define QINTR_CNT_EN       0x1
 #define QINTR_TIMER_IDX(x) ((x) << 1)
 #define QINTR_TIMER_IDX_GET(x) (((x) >> 1) & 0x7)
@@ -175,7 +176,7 @@ enum {
 	 * Location of firmware image in FLASH.
 	 */
 	FLASH_FW_START_SEC = 8,
-	FLASH_FW_NSECS = 8,
+	FLASH_FW_NSECS = 16,
 	FLASH_FW_START = FLASH_START(FLASH_FW_START_SEC),
 	FLASH_FW_MAX_SIZE = FLASH_MAX_SIZE(FLASH_FW_NSECS),
 
@@ -206,6 +207,12 @@ enum {
 	FLASH_CFG_START = FLASH_START(FLASH_CFG_START_SEC),
 	FLASH_CFG_MAX_SIZE = FLASH_MAX_SIZE(FLASH_CFG_NSECS),
 
+	/* We don't support FLASH devices which can't support the full
+	 * standard set of sections which we need for normal
+	 * operations.
+	 */
+	FLASH_MIN_SIZE = FLASH_CFG_START + FLASH_CFG_MAX_SIZE,
+
 	FLASH_FPGA_CFG_START_SEC = 15,
 	FLASH_FPGA_CFG_START = FLASH_START(FLASH_FPGA_CFG_START_SEC),
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
index 52e08103f221..5f4db2398c71 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_msg.h
@@ -527,6 +527,7 @@ struct cpl_tx_pkt_lso_core {
 #define LSO_LAST_SLICE    (1 << 22)
 #define LSO_FIRST_SLICE   (1 << 23)
 #define LSO_OPCODE(x)     ((x) << 24)
+#define LSO_T5_XFER_SIZE(x) ((x) << 0)
 	__be16 ipid_ofst;
 	__be16 mss;
 	__be32 seqno_offset;
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
index e3146e83df20..a1024db5dc13 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4_regs.h
@@ -72,11 +72,11 @@
 #define  PIDX_MASK   0x00003fffU
 #define  PIDX_SHIFT  0
 #define  PIDX(x)     ((x) << PIDX_SHIFT)
-#define  S_PIDX_T5   0
-#define  M_PIDX_T5   0x1fffU
-#define  PIDX_T5(x)  (((x) >> S_PIDX_T5) & M_PIDX_T5)
+#define  PIDX_SHIFT_T5   0
+#define  PIDX_T5(x)  ((x) << PIDX_SHIFT_T5)
 
 
+#define SGE_TIMERREGS	6
 #define SGE_PF_GTS 0x4
 #define  INGRESSQID_MASK   0xffff0000U
 #define  INGRESSQID_SHIFT  16
@@ -157,8 +157,27 @@
 #define  QUEUESPERPAGEPF0_MASK   0x0000000fU
 #define  QUEUESPERPAGEPF0_GET(x) ((x) & QUEUESPERPAGEPF0_MASK)
 
+#define QUEUESPERPAGEPF0    0
 #define QUEUESPERPAGEPF1    4
 
+/* T5 and later support a new BAR2-based doorbell mechanism for Egress Queues.
+ * The User Doorbells are each 128 bytes in length with a Simple Doorbell at
+ * offsets 8x and a Write Combining single 64-byte Egress Queue Unit
+ * (X_IDXSIZE_UNIT) Gather Buffer interface at offset 64.  For Ingress Queues,
+ * we have a Going To Sleep register at offsets 8x+4.
+ *
+ * As noted above, we have many instances of the Simple Doorbell and Going To
+ * Sleep registers at offsets 8x and 8x+4, respectively.  We want to use a
+ * non-64-byte aligned offset for the Simple Doorbell in order to attempt to
+ * avoid buffering of the writes to the Simple Doorbell and we want to use a
+ * non-contiguous offset for the Going To Sleep writes in order to avoid
+ * possible combining between them.
+ */
+#define SGE_UDB_SIZE            128
+#define SGE_UDB_KDOORBELL       8
+#define SGE_UDB_GTS             20
+#define SGE_UDB_WCDOORBELL      64
+
 #define SGE_INT_CAUSE1 0x1024
 #define SGE_INT_CAUSE2 0x1030
 #define SGE_INT_CAUSE3 0x103c
@@ -511,6 +530,7 @@
 #define  MEM_WRAP_CLIENT_NUM_GET(x) (((x) & MEM_WRAP_CLIENT_NUM_MASK) >> MEM_WRAP_CLIENT_NUM_SHIFT)
 #define MA_PCIE_FW 0x30b8
 #define MA_PARITY_ERROR_STATUS 0x77f4
+#define MA_PARITY_ERROR_STATUS2 0x7804
 
 #define MA_EXT_MEMORY1_BAR 0x7808
 #define EDC_0_BASE_ADDR 0x7900
@@ -959,6 +979,7 @@
 #define  TRCMULTIFILTER     0x00000001U
 
 #define MPS_TRC_RSS_CONTROL 0x9808
+#define MPS_T5_TRC_RSS_CONTROL 0xa00c
 #define  RSSCONTROL_MASK    0x00ff0000U
 #define  RSSCONTROL_SHIFT   16
 #define  RSSCONTROL(x)      ((x) << RSSCONTROL_SHIFT)
diff --git a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
index 0549170d7e2e..3409756a85b9 100644
--- a/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
+++ b/drivers/net/ethernet/chelsio/cxgb4/t4fw_api.h
@@ -1227,6 +1227,7 @@ struct fw_eq_eth_cmd {
 #define FW_EQ_ETH_CMD_CIDXFTHRESH(x) ((x) << 16)
 #define FW_EQ_ETH_CMD_EQSIZE(x) ((x) << 0)
 
+#define FW_EQ_ETH_CMD_AUTOEQUEQE (1U << 30)
 #define FW_EQ_ETH_CMD_VIID(x) ((x) << 16)
 
 struct fw_eq_ctrl_cmd {
@@ -2227,6 +2228,10 @@ struct fw_debug_cmd {
 #define FW_PCIE_FW_MASTER(x)     ((x) << FW_PCIE_FW_MASTER_SHIFT)
 #define FW_PCIE_FW_MASTER_GET(x) (((x) >> FW_PCIE_FW_MASTER_SHIFT) & \
 				 FW_PCIE_FW_MASTER_MASK)
+#define FW_PCIE_FW_EVAL_MASK   0x7
+#define FW_PCIE_FW_EVAL_SHIFT  24
+#define FW_PCIE_FW_EVAL_GET(x) (((x) >> FW_PCIE_FW_EVAL_SHIFT) & \
+				 FW_PCIE_FW_EVAL_MASK)
 
 struct fw_hdr {
 	u8 ver;
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
index 2102a4c91737..bfa398d91826 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/cxgb4vf_main.c
@@ -163,15 +163,19 @@ void t4vf_os_link_changed(struct adapter *adapter, int pidx, int link_ok)
 		netif_carrier_on(dev);
 
 		switch (pi->link_cfg.speed) {
-		case SPEED_10000:
+		case 40000:
+			s = "40Gbps";
+			break;
+
+		case 10000:
 			s = "10Gbps";
 			break;
 
-		case SPEED_1000:
+		case 1000:
 			s = "1000Mbps";
 			break;
 
-		case SPEED_100:
+		case 100:
 			s = "100Mbps";
 			break;
 
@@ -2351,7 +2355,7 @@ static void cfg_queues(struct adapter *adapter)
 		struct port_info *pi = adap2pinfo(adapter, pidx);
 
 		pi->first_qset = qidx;
-		pi->nqsets = is_10g_port(&pi->link_cfg) ? q10g : 1;
+		pi->nqsets = is_x_10g_port(&pi->link_cfg) ? q10g : 1;
 		qidx += pi->nqsets;
 	}
 	s->ethqsets = qidx;
@@ -2907,61 +2911,62 @@ static void cxgb4vf_pci_shutdown(struct pci_dev *pdev)
 /*
  * PCI Device registration data structures.
  */
-#define CH_DEVICE(devid, idx) \
-	{ PCI_VENDOR_ID_CHELSIO, devid, PCI_ANY_ID, PCI_ANY_ID, 0, 0, idx }
+#define CH_DEVICE(devid) \
+	{ PCI_VENDOR_ID_CHELSIO, devid, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0 }
 
 static const struct pci_device_id cxgb4vf_pci_tbl[] = {
-	CH_DEVICE(0xb000, 0),	/* PE10K FPGA */
-	CH_DEVICE(0x4800, 0),	/* T440-dbg */
-	CH_DEVICE(0x4801, 0),	/* T420-cr */
-	CH_DEVICE(0x4802, 0),	/* T422-cr */
-	CH_DEVICE(0x4803, 0),	/* T440-cr */
-	CH_DEVICE(0x4804, 0),	/* T420-bch */
-	CH_DEVICE(0x4805, 0),   /* T440-bch */
-	CH_DEVICE(0x4806, 0),	/* T460-ch */
-	CH_DEVICE(0x4807, 0),	/* T420-so */
-	CH_DEVICE(0x4808, 0),	/* T420-cx */
-	CH_DEVICE(0x4809, 0),	/* T420-bt */
-	CH_DEVICE(0x480a, 0),   /* T404-bt */
-	CH_DEVICE(0x480d, 0),   /* T480-cr */
-	CH_DEVICE(0x480e, 0),   /* T440-lp-cr */
-	CH_DEVICE(0x4880, 0),
-	CH_DEVICE(0x4880, 1),
-	CH_DEVICE(0x4880, 2),
-	CH_DEVICE(0x4880, 3),
-	CH_DEVICE(0x4880, 4),
-	CH_DEVICE(0x4880, 5),
-	CH_DEVICE(0x4880, 6),
-	CH_DEVICE(0x4880, 7),
-	CH_DEVICE(0x4880, 8),
-	CH_DEVICE(0x5800, 0),	/* T580-dbg */
-	CH_DEVICE(0x5801, 0),	/* T520-cr */
-	CH_DEVICE(0x5802, 0),	/* T522-cr */
-	CH_DEVICE(0x5803, 0),	/* T540-cr */
-	CH_DEVICE(0x5804, 0),	/* T520-bch */
-	CH_DEVICE(0x5805, 0),   /* T540-bch */
-	CH_DEVICE(0x5806, 0),	/* T540-ch */
-	CH_DEVICE(0x5807, 0),	/* T520-so */
-	CH_DEVICE(0x5808, 0),	/* T520-cx */
-	CH_DEVICE(0x5809, 0),	/* T520-bt */
-	CH_DEVICE(0x580a, 0),   /* T504-bt */
-	CH_DEVICE(0x580b, 0),   /* T520-sr */
-	CH_DEVICE(0x580c, 0),   /* T504-bt */
-	CH_DEVICE(0x580d, 0),   /* T580-cr */
-	CH_DEVICE(0x580e, 0),   /* T540-lp-cr */
-	CH_DEVICE(0x580f, 0),   /* Amsterdam */
-	CH_DEVICE(0x5810, 0),   /* T580-lp-cr */
-	CH_DEVICE(0x5811, 0),   /* T520-lp-cr */
-	CH_DEVICE(0x5812, 0),   /* T560-cr */
-	CH_DEVICE(0x5813, 0),   /* T580-cr */
-	CH_DEVICE(0x5814, 0),   /* T580-so-cr */
-	CH_DEVICE(0x5815, 0),   /* T502-bt */
-	CH_DEVICE(0x5880, 0),
-	CH_DEVICE(0x5881, 0),
-	CH_DEVICE(0x5882, 0),
-	CH_DEVICE(0x5883, 0),
-	CH_DEVICE(0x5884, 0),
-	CH_DEVICE(0x5885, 0),
+	CH_DEVICE(0xb000),	/* PE10K FPGA */
+	CH_DEVICE(0x4801),	/* T420-cr */
+	CH_DEVICE(0x4802),	/* T422-cr */
+	CH_DEVICE(0x4803),	/* T440-cr */
+	CH_DEVICE(0x4804),	/* T420-bch */
+	CH_DEVICE(0x4805),	/* T440-bch */
+	CH_DEVICE(0x4806),	/* T460-ch */
+	CH_DEVICE(0x4807),	/* T420-so */
+	CH_DEVICE(0x4808),	/* T420-cx */
+	CH_DEVICE(0x4809),	/* T420-bt */
+	CH_DEVICE(0x480a),	/* T404-bt */
+	CH_DEVICE(0x480d),	/* T480-cr */
+	CH_DEVICE(0x480e),	/* T440-lp-cr */
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x4880),
+	CH_DEVICE(0x5801),	/* T520-cr */
+	CH_DEVICE(0x5802),	/* T522-cr */
+	CH_DEVICE(0x5803),	/* T540-cr */
+	CH_DEVICE(0x5804),	/* T520-bch */
+	CH_DEVICE(0x5805),	/* T540-bch */
+	CH_DEVICE(0x5806),	/* T540-ch */
+	CH_DEVICE(0x5807),	/* T520-so */
+	CH_DEVICE(0x5808),	/* T520-cx */
+	CH_DEVICE(0x5809),	/* T520-bt */
+	CH_DEVICE(0x580a),	/* T504-bt */
+	CH_DEVICE(0x580b),	/* T520-sr */
+	CH_DEVICE(0x580c),	/* T504-bt */
+	CH_DEVICE(0x580d),	/* T580-cr */
+	CH_DEVICE(0x580e),	/* T540-lp-cr */
+	CH_DEVICE(0x580f),	/* Amsterdam */
+	CH_DEVICE(0x5810),	/* T580-lp-cr */
+	CH_DEVICE(0x5811),	/* T520-lp-cr */
+	CH_DEVICE(0x5812),	/* T560-cr */
+	CH_DEVICE(0x5813),	/* T580-cr */
+	CH_DEVICE(0x5814),	/* T580-so-cr */
+	CH_DEVICE(0x5815),	/* T502-bt */
+	CH_DEVICE(0x5880),
+	CH_DEVICE(0x5881),
+	CH_DEVICE(0x5882),
+	CH_DEVICE(0x5883),
+	CH_DEVICE(0x5884),
+	CH_DEVICE(0x5885),
+	CH_DEVICE(0x5886),
+	CH_DEVICE(0x5887),
+	CH_DEVICE(0x5888),
 	{ 0, }
 };
 
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
index bdfa80ca5e31..85036e6b42c4 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/sge.c
@@ -1208,7 +1208,10 @@ int t4vf_eth_xmit(struct sk_buff *skb, struct net_device *dev)
 		lso->ipid_ofst = cpu_to_be16(0);
 		lso->mss = cpu_to_be16(ssi->gso_size);
 		lso->seqno_offset = cpu_to_be32(0);
-		lso->len = cpu_to_be32(skb->len);
+		if (is_t4(adapter->params.chip))
+			lso->len = cpu_to_be32(skb->len);
+		else
+			lso->len = cpu_to_be32(LSO_T5_XFER_SIZE(skb->len));
 
 		/*
 		 * Set up TX Packet CPL pointer, control word and perform
@@ -2250,7 +2253,8 @@ int t4vf_sge_alloc_eth_txq(struct adapter *adapter, struct sge_eth_txq *txq,
 	cmd.alloc_to_len16 = cpu_to_be32(FW_EQ_ETH_CMD_ALLOC |
 					 FW_EQ_ETH_CMD_EQSTART |
 					 FW_LEN16(cmd));
-	cmd.viid_pkd = cpu_to_be32(FW_EQ_ETH_CMD_VIID(pi->viid));
+	cmd.viid_pkd = cpu_to_be32(FW_EQ_ETH_CMD_AUTOEQUEQE |
+				   FW_EQ_ETH_CMD_VIID(pi->viid));
 	cmd.fetchszm_to_iqid =
 		cpu_to_be32(FW_EQ_ETH_CMD_HOSTFCMODE(SGE_HOSTFCMODE_STPG) |
 			    FW_EQ_ETH_CMD_PCIECHN(pi->port_id) |
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
index f412d0fa0850..95df61dcb4ce 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_common.h
@@ -228,6 +228,12 @@ static inline bool is_10g_port(const struct link_config *lc)
 	return (lc->supported & SUPPORTED_10000baseT_Full) != 0;
 }
 
+static inline bool is_x_10g_port(const struct link_config *lc)
+{
+	return (lc->supported & FW_PORT_CAP_SPEED_10G) != 0 ||
+		(lc->supported & FW_PORT_CAP_SPEED_40G) != 0;
+}
+
 static inline unsigned int core_ticks_per_usec(const struct adapter *adapter)
 {
 	return adapter->params.vpd.cclk / 1000;
diff --git a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
index 25dfeb8f28ed..e984fdc48ba2 100644
--- a/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
+++ b/drivers/net/ethernet/chelsio/cxgb4vf/t4vf_hw.c
@@ -327,6 +327,8 @@ int t4vf_port_init(struct adapter *adapter, int pidx)
 		v |= SUPPORTED_1000baseT_Full;
 	if (word & FW_PORT_CAP_SPEED_10G)
 		v |= SUPPORTED_10000baseT_Full;
+	if (word & FW_PORT_CAP_SPEED_40G)
+		v |= SUPPORTED_40000baseSR4_Full;
 	if (word & FW_PORT_CAP_ANEG)
 		v |= SUPPORTED_Autoneg;
 	init_link_config(&pi->link_cfg, v);
@@ -1352,11 +1354,13 @@ int t4vf_handle_fw_rpl(struct adapter *adapter, const __be64 *rpl)
 		if (word & FW_PORT_CMD_TXPAUSE)
 			fc |= PAUSE_TX;
 		if (word & FW_PORT_CMD_LSPEED(FW_PORT_CAP_SPEED_100M))
-			speed = SPEED_100;
+			speed = 100;
 		else if (word & FW_PORT_CMD_LSPEED(FW_PORT_CAP_SPEED_1G))
-			speed = SPEED_1000;
+			speed = 1000;
 		else if (word & FW_PORT_CMD_LSPEED(FW_PORT_CAP_SPEED_10G))
-			speed = SPEED_10000;
+			speed = 10000;
+		else if (word & FW_PORT_CMD_LSPEED(FW_PORT_CAP_SPEED_40G))
+			speed = 40000;
 
 		/*
 		 * Scan all of our "ports" (Virtual Interfaces) looking for
diff --git a/drivers/net/ethernet/cisco/enic/enic.h b/drivers/net/ethernet/cisco/enic/enic.h
index 962510f391df..5ba5ad071bb6 100644
--- a/drivers/net/ethernet/cisco/enic/enic.h
+++ b/drivers/net/ethernet/cisco/enic/enic.h
@@ -186,6 +186,7 @@ struct enic {
 	____cacheline_aligned struct vnic_cq cq[ENIC_CQ_MAX];
 	unsigned int cq_count;
 	struct enic_rfs_flw_tbl rfs_h;
+	u32 rx_copybreak;
 };
 
 static inline struct device *enic_get_dev(struct enic *enic)
diff --git a/drivers/net/ethernet/cisco/enic/enic_ethtool.c b/drivers/net/ethernet/cisco/enic/enic_ethtool.c
index 523c9ceb04c0..85173d620758 100644
--- a/drivers/net/ethernet/cisco/enic/enic_ethtool.c
+++ b/drivers/net/ethernet/cisco/enic/enic_ethtool.c
@@ -379,6 +379,43 @@ static int enic_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
 	return ret;
 }
 
+static int enic_get_tunable(struct net_device *dev,
+			    const struct ethtool_tunable *tuna, void *data)
+{
+	struct enic *enic = netdev_priv(dev);
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		*(u32 *)data = enic->rx_copybreak;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int enic_set_tunable(struct net_device *dev,
+			    const struct ethtool_tunable *tuna,
+			    const void *data)
+{
+	struct enic *enic = netdev_priv(dev);
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		enic->rx_copybreak = *(u32 *)data;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
 static const struct ethtool_ops enic_ethtool_ops = {
 	.get_settings = enic_get_settings,
 	.get_drvinfo = enic_get_drvinfo,
@@ -391,6 +428,8 @@ static const struct ethtool_ops enic_ethtool_ops = {
 	.get_coalesce = enic_get_coalesce,
 	.set_coalesce = enic_set_coalesce,
 	.get_rxnfc = enic_get_rxnfc,
+	.get_tunable = enic_get_tunable,
+	.set_tunable = enic_set_tunable,
 };
 
 void enic_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/ethernet/cisco/enic/enic_main.c b/drivers/net/ethernet/cisco/enic/enic_main.c
index c8832bc1c5f7..929bfe70080a 100644
--- a/drivers/net/ethernet/cisco/enic/enic_main.c
+++ b/drivers/net/ethernet/cisco/enic/enic_main.c
@@ -66,6 +66,8 @@
 #define PCI_DEVICE_ID_CISCO_VIC_ENET_DYN     0x0044  /* enet dynamic vnic */
 #define PCI_DEVICE_ID_CISCO_VIC_ENET_VF      0x0071  /* enet SRIOV VF */
 
+#define RX_COPYBREAK_DEFAULT		256
+
 /* Supported devices */
 static const struct pci_device_id enic_id_table[] = {
 	{ PCI_VDEVICE(CISCO, PCI_DEVICE_ID_CISCO_VIC_ENET) },
@@ -924,6 +926,7 @@ static void enic_free_rq_buf(struct vnic_rq *rq, struct vnic_rq_buf *buf)
 	pci_unmap_single(enic->pdev, buf->dma_addr,
 		buf->len, PCI_DMA_FROMDEVICE);
 	dev_kfree_skb_any(buf->os_buf);
+	buf->os_buf = NULL;
 }
 
 static int enic_rq_alloc_buf(struct vnic_rq *rq)
@@ -934,7 +937,24 @@ static int enic_rq_alloc_buf(struct vnic_rq *rq)
 	unsigned int len = netdev->mtu + VLAN_ETH_HLEN;
 	unsigned int os_buf_index = 0;
 	dma_addr_t dma_addr;
+	struct vnic_rq_buf *buf = rq->to_use;
+
+	if (buf->os_buf) {
+		buf = buf->next;
+		rq->to_use = buf;
+		rq->ring.desc_avail--;
+		if ((buf->index & VNIC_RQ_RETURN_RATE) == 0) {
+			/* Adding write memory barrier prevents compiler and/or
+			 * CPU reordering, thus avoiding descriptor posting
+			 * before descriptor is initialized. Otherwise, hardware
+			 * can read stale descriptor fields.
+			 */
+			wmb();
+			iowrite32(buf->index, &rq->ctrl->posted_index);
+		}
 
+		return 0;
+	}
 	skb = netdev_alloc_skb_ip_align(netdev, len);
 	if (!skb)
 		return -ENOMEM;
@@ -957,6 +977,25 @@ static void enic_intr_update_pkt_size(struct vnic_rx_bytes_counter *pkt_size,
 		pkt_size->small_pkt_bytes_cnt += pkt_len;
 }
 
+static bool enic_rxcopybreak(struct net_device *netdev, struct sk_buff **skb,
+			     struct vnic_rq_buf *buf, u16 len)
+{
+	struct enic *enic = netdev_priv(netdev);
+	struct sk_buff *new_skb;
+
+	if (len > enic->rx_copybreak)
+		return false;
+	new_skb = netdev_alloc_skb_ip_align(netdev, len);
+	if (!new_skb)
+		return false;
+	pci_dma_sync_single_for_cpu(enic->pdev, buf->dma_addr, len,
+				    DMA_FROM_DEVICE);
+	memcpy(new_skb->data, (*skb)->data, len);
+	*skb = new_skb;
+
+	return true;
+}
+
 static void enic_rq_indicate_buf(struct vnic_rq *rq,
 	struct cq_desc *cq_desc, struct vnic_rq_buf *buf,
 	int skipped, void *opaque)
@@ -978,9 +1017,6 @@ static void enic_rq_indicate_buf(struct vnic_rq *rq,
 		return;
 
 	skb = buf->os_buf;
-	prefetch(skb->data - NET_IP_ALIGN);
-	pci_unmap_single(enic->pdev, buf->dma_addr,
-		buf->len, PCI_DMA_FROMDEVICE);
 
 	cq_enet_rq_desc_dec((struct cq_enet_rq_desc *)cq_desc,
 		&type, &color, &q_number, &completed_index,
@@ -1011,6 +1047,13 @@ static void enic_rq_indicate_buf(struct vnic_rq *rq,
 		/* Good receive
 		 */
 
+		if (!enic_rxcopybreak(netdev, &skb, buf, bytes_written)) {
+			buf->os_buf = NULL;
+			pci_unmap_single(enic->pdev, buf->dma_addr, buf->len,
+					 PCI_DMA_FROMDEVICE);
+		}
+		prefetch(skb->data - NET_IP_ALIGN);
+
 		skb_put(skb, bytes_written);
 		skb->protocol = eth_type_trans(skb, netdev);
 		skb_record_rx_queue(skb, q_number);
@@ -2531,6 +2574,7 @@ static int enic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		dev_err(dev, "Cannot register net device, aborting\n");
 		goto err_out_dev_deinit;
 	}
+	enic->rx_copybreak = RX_COPYBREAK_DEFAULT;
 
 	return 0;
 
diff --git a/drivers/net/ethernet/cisco/enic/vnic_dev.c b/drivers/net/ethernet/cisco/enic/vnic_dev.c
index 37472ce4fac3..62f7b7baf93c 100644
--- a/drivers/net/ethernet/cisco/enic/vnic_dev.c
+++ b/drivers/net/ethernet/cisco/enic/vnic_dev.c
@@ -847,8 +847,7 @@ int vnic_dev_intr_coal_timer_info(struct vnic_dev *vdev)
 	 */
 	if ((err == ERR_ECMDUNKNOWN) ||
 		(!err && !(vdev->args[0] && vdev->args[1] && vdev->args[2]))) {
-		pr_warning("Using default conversion factor for "
-			"interrupt coalesce timer\n");
+		pr_warn("Using default conversion factor for interrupt coalesce timer\n");
 		vnic_dev_intr_coal_timer_info_default(vdev);
 		return 0;
 	}
diff --git a/drivers/net/ethernet/davicom/dm9000.c b/drivers/net/ethernet/davicom/dm9000.c
index 9b33057a9477..f3ba840cbf7b 100644
--- a/drivers/net/ethernet/davicom/dm9000.c
+++ b/drivers/net/ethernet/davicom/dm9000.c
@@ -1399,7 +1399,7 @@ static struct dm9000_plat_data *dm9000_parse_dt(struct device *dev)
 	const void *mac_addr;
 
 	if (!IS_ENABLED(CONFIG_OF) || !np)
-		return NULL;
+		return ERR_PTR(-ENXIO);
 
 	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
 	if (!pdata)
@@ -1613,9 +1613,6 @@ dm9000_probe(struct platform_device *pdev)
 
 	/* from this point we assume that we have found a DM9000 */
 
-	/* driver system function */
-	ether_setup(ndev);
-
 	ndev->netdev_ops	= &dm9000_netdev_ops;
 	ndev->watchdog_timeo	= msecs_to_jiffies(watchdog);
 	ndev->ethtool_ops	= &dm9000_ethtool_ops;
diff --git a/drivers/net/ethernet/dec/tulip/dmfe.c b/drivers/net/ethernet/dec/tulip/dmfe.c
index 322213d901d5..c8205606c775 100644
--- a/drivers/net/ethernet/dec/tulip/dmfe.c
+++ b/drivers/net/ethernet/dec/tulip/dmfe.c
@@ -328,10 +328,10 @@ static void allocate_rx_buffer(struct net_device *);
 static void update_cr6(u32, void __iomem *);
 static void send_filter_frame(struct DEVICE *);
 static void dm9132_id_table(struct DEVICE *);
-static u16 phy_read(void __iomem *, u8, u8, u32);
-static void phy_write(void __iomem *, u8, u8, u16, u32);
-static void phy_write_1bit(void __iomem *, u32);
-static u16 phy_read_1bit(void __iomem *);
+static u16 dmfe_phy_read(void __iomem *, u8, u8, u32);
+static void dmfe_phy_write(void __iomem *, u8, u8, u16, u32);
+static void dmfe_phy_write_1bit(void __iomem *, u32);
+static u16 dmfe_phy_read_1bit(void __iomem *);
 static u8 dmfe_sense_speed(struct dmfe_board_info *);
 static void dmfe_process_mode(struct dmfe_board_info *);
 static void dmfe_timer(unsigned long);
@@ -770,7 +770,7 @@ static int dmfe_stop(struct DEVICE *dev)
 	/* Reset & stop DM910X board */
 	dw32(DCR0, DM910X_RESET);
 	udelay(100);
-	phy_write(ioaddr, db->phy_addr, 0, 0x8000, db->chip_id);
+	dmfe_phy_write(ioaddr, db->phy_addr, 0, 0x8000, db->chip_id);
 
 	/* free interrupt */
 	free_irq(db->pdev->irq, dev);
@@ -1154,7 +1154,7 @@ static void dmfe_timer(unsigned long data)
 		if (db->chip_type && (db->chip_id==PCI_DM9102_ID)) {
 			db->cr6_data &= ~0x40000;
 			update_cr6(db->cr6_data, ioaddr);
-			phy_write(ioaddr, db->phy_addr, 0, 0x1000, db->chip_id);
+			dmfe_phy_write(ioaddr, db->phy_addr, 0, 0x1000, db->chip_id);
 			db->cr6_data |= 0x40000;
 			update_cr6(db->cr6_data, ioaddr);
 			db->timer.expires = DMFE_TIMER_WUT + HZ * 2;
@@ -1230,9 +1230,9 @@ static void dmfe_timer(unsigned long data)
 	*/
 
 	/* need a dummy read because of PHY's register latch*/
-	phy_read (db->ioaddr, db->phy_addr, 1, db->chip_id);
-	link_ok_phy = (phy_read (db->ioaddr,
-		       db->phy_addr, 1, db->chip_id) & 0x4) ? 1 : 0;
+	dmfe_phy_read (db->ioaddr, db->phy_addr, 1, db->chip_id);
+	link_ok_phy = (dmfe_phy_read (db->ioaddr,
+				      db->phy_addr, 1, db->chip_id) & 0x4) ? 1 : 0;
 
 	if (link_ok_phy != link_ok) {
 		DMFE_DBUG (0, "PHY and chip report different link status", 0);
@@ -1247,8 +1247,8 @@ static void dmfe_timer(unsigned long data)
 		/* For Force 10/100M Half/Full mode: Enable Auto-Nego mode */
 		/* AUTO or force 1M Homerun/Longrun don't need */
 		if ( !(db->media_mode & 0x38) )
-			phy_write(db->ioaddr, db->phy_addr,
-				  0, 0x1000, db->chip_id);
+			dmfe_phy_write(db->ioaddr, db->phy_addr,
+				       0, 0x1000, db->chip_id);
 
 		/* AUTO mode, if INT phyxcer link failed, select EXT device */
 		if (db->media_mode & DMFE_AUTO) {
@@ -1649,16 +1649,16 @@ static u8 dmfe_sense_speed(struct dmfe_board_info *db)
 	/* CR6 bit18=0, select 10/100M */
 	update_cr6(db->cr6_data & ~0x40000, ioaddr);
 
-	phy_mode = phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
-	phy_mode = phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
+	phy_mode = dmfe_phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
+	phy_mode = dmfe_phy_read(db->ioaddr, db->phy_addr, 1, db->chip_id);
 
 	if ( (phy_mode & 0x24) == 0x24 ) {
 		if (db->chip_id == PCI_DM9132_ID)	/* DM9132 */
-			phy_mode = phy_read(db->ioaddr,
-				    db->phy_addr, 7, db->chip_id) & 0xf000;
+			phy_mode = dmfe_phy_read(db->ioaddr,
+						 db->phy_addr, 7, db->chip_id) & 0xf000;
 		else 				/* DM9102/DM9102A */
-			phy_mode = phy_read(db->ioaddr,
-				    db->phy_addr, 17, db->chip_id) & 0xf000;
+			phy_mode = dmfe_phy_read(db->ioaddr,
+						 db->phy_addr, 17, db->chip_id) & 0xf000;
 		switch (phy_mode) {
 		case 0x1000: db->op_mode = DMFE_10MHF; break;
 		case 0x2000: db->op_mode = DMFE_10MFD; break;
@@ -1695,15 +1695,15 @@ static void dmfe_set_phyxcer(struct dmfe_board_info *db)
 
 	/* DM9009 Chip: Phyxcer reg18 bit12=0 */
 	if (db->chip_id == PCI_DM9009_ID) {
-		phy_reg = phy_read(db->ioaddr,
-				   db->phy_addr, 18, db->chip_id) & ~0x1000;
+		phy_reg = dmfe_phy_read(db->ioaddr,
+					db->phy_addr, 18, db->chip_id) & ~0x1000;
 
-		phy_write(db->ioaddr,
-			  db->phy_addr, 18, phy_reg, db->chip_id);
+		dmfe_phy_write(db->ioaddr,
+			       db->phy_addr, 18, phy_reg, db->chip_id);
 	}
 
 	/* Phyxcer capability setting */
-	phy_reg = phy_read(db->ioaddr, db->phy_addr, 4, db->chip_id) & ~0x01e0;
+	phy_reg = dmfe_phy_read(db->ioaddr, db->phy_addr, 4, db->chip_id) & ~0x01e0;
 
 	if (db->media_mode & DMFE_AUTO) {
 		/* AUTO Mode */
@@ -1724,13 +1724,13 @@ static void dmfe_set_phyxcer(struct dmfe_board_info *db)
 		phy_reg|=db->PHY_reg4;
 		db->media_mode|=DMFE_AUTO;
 	}
-	phy_write(db->ioaddr, db->phy_addr, 4, phy_reg, db->chip_id);
+	dmfe_phy_write(db->ioaddr, db->phy_addr, 4, phy_reg, db->chip_id);
 
  	/* Restart Auto-Negotiation */
 	if ( db->chip_type && (db->chip_id == PCI_DM9102_ID) )
-		phy_write(db->ioaddr, db->phy_addr, 0, 0x1800, db->chip_id);
+		dmfe_phy_write(db->ioaddr, db->phy_addr, 0, 0x1800, db->chip_id);
 	if ( !db->chip_type )
-		phy_write(db->ioaddr, db->phy_addr, 0, 0x1200, db->chip_id);
+		dmfe_phy_write(db->ioaddr, db->phy_addr, 0, 0x1200, db->chip_id);
 }
 
 
@@ -1762,7 +1762,7 @@ static void dmfe_process_mode(struct dmfe_board_info *db)
 	/* 10/100M phyxcer force mode need */
 	if ( !(db->media_mode & 0x18)) {
 		/* Forece Mode */
-		phy_reg = phy_read(db->ioaddr, db->phy_addr, 6, db->chip_id);
+		phy_reg = dmfe_phy_read(db->ioaddr, db->phy_addr, 6, db->chip_id);
 		if ( !(phy_reg & 0x1) ) {
 			/* parter without N-Way capability */
 			phy_reg = 0x0;
@@ -1772,12 +1772,12 @@ static void dmfe_process_mode(struct dmfe_board_info *db)
 			case DMFE_100MHF: phy_reg = 0x2000; break;
 			case DMFE_100MFD: phy_reg = 0x2100; break;
 			}
-			phy_write(db->ioaddr,
-				  db->phy_addr, 0, phy_reg, db->chip_id);
+			dmfe_phy_write(db->ioaddr,
+				       db->phy_addr, 0, phy_reg, db->chip_id);
        			if ( db->chip_type && (db->chip_id == PCI_DM9102_ID) )
 				mdelay(20);
-			phy_write(db->ioaddr,
-				  db->phy_addr, 0, phy_reg, db->chip_id);
+			dmfe_phy_write(db->ioaddr,
+				       db->phy_addr, 0, phy_reg, db->chip_id);
 		}
 	}
 }
@@ -1787,8 +1787,8 @@ static void dmfe_process_mode(struct dmfe_board_info *db)
  *	Write a word to Phy register
  */
 
-static void phy_write(void __iomem *ioaddr, u8 phy_addr, u8 offset,
-		      u16 phy_data, u32 chip_id)
+static void dmfe_phy_write(void __iomem *ioaddr, u8 phy_addr, u8 offset,
+			   u16 phy_data, u32 chip_id)
 {
 	u16 i;
 
@@ -1799,34 +1799,34 @@ static void phy_write(void __iomem *ioaddr, u8 phy_addr, u8 offset,
 
 		/* Send 33 synchronization clock to Phy controller */
 		for (i = 0; i < 35; i++)
-			phy_write_1bit(ioaddr, PHY_DATA_1);
+			dmfe_phy_write_1bit(ioaddr, PHY_DATA_1);
 
 		/* Send start command(01) to Phy */
-		phy_write_1bit(ioaddr, PHY_DATA_0);
-		phy_write_1bit(ioaddr, PHY_DATA_1);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_0);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_1);
 
 		/* Send write command(01) to Phy */
-		phy_write_1bit(ioaddr, PHY_DATA_0);
-		phy_write_1bit(ioaddr, PHY_DATA_1);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_0);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_1);
 
 		/* Send Phy address */
 		for (i = 0x10; i > 0; i = i >> 1)
-			phy_write_1bit(ioaddr,
-				       phy_addr & i ? PHY_DATA_1 : PHY_DATA_0);
+			dmfe_phy_write_1bit(ioaddr,
+					    phy_addr & i ? PHY_DATA_1 : PHY_DATA_0);
 
 		/* Send register address */
 		for (i = 0x10; i > 0; i = i >> 1)
-			phy_write_1bit(ioaddr,
-				       offset & i ? PHY_DATA_1 : PHY_DATA_0);
+			dmfe_phy_write_1bit(ioaddr,
+					    offset & i ? PHY_DATA_1 : PHY_DATA_0);
 
 		/* written trasnition */
-		phy_write_1bit(ioaddr, PHY_DATA_1);
-		phy_write_1bit(ioaddr, PHY_DATA_0);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_1);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_0);
 
 		/* Write a word data to PHY controller */
 		for ( i = 0x8000; i > 0; i >>= 1)
-			phy_write_1bit(ioaddr,
-				       phy_data & i ? PHY_DATA_1 : PHY_DATA_0);
+			dmfe_phy_write_1bit(ioaddr,
+					    phy_data & i ? PHY_DATA_1 : PHY_DATA_0);
 	}
 }
 
@@ -1835,7 +1835,7 @@ static void phy_write(void __iomem *ioaddr, u8 phy_addr, u8 offset,
  *	Read a word data from phy register
  */
 
-static u16 phy_read(void __iomem *ioaddr, u8 phy_addr, u8 offset, u32 chip_id)
+static u16 dmfe_phy_read(void __iomem *ioaddr, u8 phy_addr, u8 offset, u32 chip_id)
 {
 	int i;
 	u16 phy_data;
@@ -1848,33 +1848,33 @@ static u16 phy_read(void __iomem *ioaddr, u8 phy_addr, u8 offset, u32 chip_id)
 
 		/* Send 33 synchronization clock to Phy controller */
 		for (i = 0; i < 35; i++)
-			phy_write_1bit(ioaddr, PHY_DATA_1);
+			dmfe_phy_write_1bit(ioaddr, PHY_DATA_1);
 
 		/* Send start command(01) to Phy */
-		phy_write_1bit(ioaddr, PHY_DATA_0);
-		phy_write_1bit(ioaddr, PHY_DATA_1);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_0);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_1);
 
 		/* Send read command(10) to Phy */
-		phy_write_1bit(ioaddr, PHY_DATA_1);
-		phy_write_1bit(ioaddr, PHY_DATA_0);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_1);
+		dmfe_phy_write_1bit(ioaddr, PHY_DATA_0);
 
 		/* Send Phy address */
 		for (i = 0x10; i > 0; i = i >> 1)
-			phy_write_1bit(ioaddr,
-				       phy_addr & i ? PHY_DATA_1 : PHY_DATA_0);
+			dmfe_phy_write_1bit(ioaddr,
+					    phy_addr & i ? PHY_DATA_1 : PHY_DATA_0);
 
 		/* Send register address */
 		for (i = 0x10; i > 0; i = i >> 1)
-			phy_write_1bit(ioaddr,
-				       offset & i ? PHY_DATA_1 : PHY_DATA_0);
+			dmfe_phy_write_1bit(ioaddr,
+					    offset & i ? PHY_DATA_1 : PHY_DATA_0);
 
 		/* Skip transition state */
-		phy_read_1bit(ioaddr);
+		dmfe_phy_read_1bit(ioaddr);
 
 		/* read 16bit data */
 		for (phy_data = 0, i = 0; i < 16; i++) {
 			phy_data <<= 1;
-			phy_data |= phy_read_1bit(ioaddr);
+			phy_data |= dmfe_phy_read_1bit(ioaddr);
 		}
 	}
 
@@ -1886,7 +1886,7 @@ static u16 phy_read(void __iomem *ioaddr, u8 phy_addr, u8 offset, u32 chip_id)
  *	Write one bit data to Phy Controller
  */
 
-static void phy_write_1bit(void __iomem *ioaddr, u32 phy_data)
+static void dmfe_phy_write_1bit(void __iomem *ioaddr, u32 phy_data)
 {
 	dw32(DCR9, phy_data);		/* MII Clock Low */
 	udelay(1);
@@ -1901,7 +1901,7 @@ static void phy_write_1bit(void __iomem *ioaddr, u32 phy_data)
  *	Read one bit phy data from PHY controller
  */
 
-static u16 phy_read_1bit(void __iomem *ioaddr)
+static u16 dmfe_phy_read_1bit(void __iomem *ioaddr)
 {
 	u16 phy_data;
 
@@ -1995,11 +1995,11 @@ static void dmfe_parse_srom(struct dmfe_board_info * db)
 	/* Check DM9801 or DM9802 present or not */
 	db->HPNA_present = 0;
 	update_cr6(db->cr6_data | 0x40000, db->ioaddr);
-	tmp_reg = phy_read(db->ioaddr, db->phy_addr, 3, db->chip_id);
+	tmp_reg = dmfe_phy_read(db->ioaddr, db->phy_addr, 3, db->chip_id);
 	if ( ( tmp_reg & 0xfff0 ) == 0xb900 ) {
 		/* DM9801 or DM9802 present */
 		db->HPNA_timer = 8;
-		if ( phy_read(db->ioaddr, db->phy_addr, 31, db->chip_id) == 0x4404) {
+		if ( dmfe_phy_read(db->ioaddr, db->phy_addr, 31, db->chip_id) == 0x4404) {
 			/* DM9801 HomeRun */
 			db->HPNA_present = 1;
 			dmfe_program_DM9801(db, tmp_reg);
@@ -2025,29 +2025,29 @@ static void dmfe_program_DM9801(struct dmfe_board_info * db, int HPNA_rev)
 	switch(HPNA_rev) {
 	case 0xb900: /* DM9801 E3 */
 		db->HPNA_command |= 0x1000;
-		reg25 = phy_read(db->ioaddr, db->phy_addr, 24, db->chip_id);
+		reg25 = dmfe_phy_read(db->ioaddr, db->phy_addr, 24, db->chip_id);
 		reg25 = ( (reg25 + HPNA_NoiseFloor) & 0xff) | 0xf000;
-		reg17 = phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id);
+		reg17 = dmfe_phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id);
 		break;
 	case 0xb901: /* DM9801 E4 */
-		reg25 = phy_read(db->ioaddr, db->phy_addr, 25, db->chip_id);
+		reg25 = dmfe_phy_read(db->ioaddr, db->phy_addr, 25, db->chip_id);
 		reg25 = (reg25 & 0xff00) + HPNA_NoiseFloor;
-		reg17 = phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id);
+		reg17 = dmfe_phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id);
 		reg17 = (reg17 & 0xfff0) + HPNA_NoiseFloor + 3;
 		break;
 	case 0xb902: /* DM9801 E5 */
 	case 0xb903: /* DM9801 E6 */
 	default:
 		db->HPNA_command |= 0x1000;
-		reg25 = phy_read(db->ioaddr, db->phy_addr, 25, db->chip_id);
+		reg25 = dmfe_phy_read(db->ioaddr, db->phy_addr, 25, db->chip_id);
 		reg25 = (reg25 & 0xff00) + HPNA_NoiseFloor - 5;
-		reg17 = phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id);
+		reg17 = dmfe_phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id);
 		reg17 = (reg17 & 0xfff0) + HPNA_NoiseFloor;
 		break;
 	}
-	phy_write(db->ioaddr, db->phy_addr, 16, db->HPNA_command, db->chip_id);
-	phy_write(db->ioaddr, db->phy_addr, 17, reg17, db->chip_id);
-	phy_write(db->ioaddr, db->phy_addr, 25, reg25, db->chip_id);
+	dmfe_phy_write(db->ioaddr, db->phy_addr, 16, db->HPNA_command, db->chip_id);
+	dmfe_phy_write(db->ioaddr, db->phy_addr, 17, reg17, db->chip_id);
+	dmfe_phy_write(db->ioaddr, db->phy_addr, 25, reg25, db->chip_id);
 }
 
 
@@ -2060,10 +2060,10 @@ static void dmfe_program_DM9802(struct dmfe_board_info * db)
 	uint phy_reg;
 
 	if ( !HPNA_NoiseFloor ) HPNA_NoiseFloor = DM9802_NOISE_FLOOR;
-	phy_write(db->ioaddr, db->phy_addr, 16, db->HPNA_command, db->chip_id);
-	phy_reg = phy_read(db->ioaddr, db->phy_addr, 25, db->chip_id);
+	dmfe_phy_write(db->ioaddr, db->phy_addr, 16, db->HPNA_command, db->chip_id);
+	phy_reg = dmfe_phy_read(db->ioaddr, db->phy_addr, 25, db->chip_id);
 	phy_reg = ( phy_reg & 0xff00) + HPNA_NoiseFloor;
-	phy_write(db->ioaddr, db->phy_addr, 25, phy_reg, db->chip_id);
+	dmfe_phy_write(db->ioaddr, db->phy_addr, 25, phy_reg, db->chip_id);
 }
 
 
@@ -2077,7 +2077,7 @@ static void dmfe_HPNA_remote_cmd_chk(struct dmfe_board_info * db)
 	uint phy_reg;
 
 	/* Got remote device status */
-	phy_reg = phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id) & 0x60;
+	phy_reg = dmfe_phy_read(db->ioaddr, db->phy_addr, 17, db->chip_id) & 0x60;
 	switch(phy_reg) {
 	case 0x00: phy_reg = 0x0a00;break; /* LP/LS */
 	case 0x20: phy_reg = 0x0900;break; /* LP/HS */
@@ -2087,8 +2087,8 @@ static void dmfe_HPNA_remote_cmd_chk(struct dmfe_board_info * db)
 
 	/* Check remote device status match our setting ot not */
 	if ( phy_reg != (db->HPNA_command & 0x0f00) ) {
-		phy_write(db->ioaddr, db->phy_addr, 16, db->HPNA_command,
-			  db->chip_id);
+		dmfe_phy_write(db->ioaddr, db->phy_addr, 16, db->HPNA_command,
+			       db->chip_id);
 		db->HPNA_timer=8;
 	} else
 		db->HPNA_timer=600;	/* Match, every 10 minutes, check */
diff --git a/drivers/net/ethernet/ec_bhf.c b/drivers/net/ethernet/ec_bhf.c
index 056b44b93477..d1017509b08a 100644
--- a/drivers/net/ethernet/ec_bhf.c
+++ b/drivers/net/ethernet/ec_bhf.c
@@ -1,5 +1,5 @@
  /*
- * drivers/net/ethernet/beckhoff/ec_bhf.c
+ * drivers/net/ethernet/ec_bhf.c
  *
  * Copyright (C) 2014 Darek Marcinkiewicz <reksio@newterm.pl>
  *
@@ -18,9 +18,6 @@
  * Those can be found on Bechhoff CX50xx industrial PCs.
  */
 
-#if 0
-#define DEBUG
-#endif
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
@@ -74,6 +71,8 @@
 
 #define DMA_WINDOW_SIZE_MASK	0xfffffffc
 
+#define ETHERCAT_MASTER_ID	0x14
+
 static struct pci_device_id ids[] = {
 	{ PCI_DEVICE(0x15ec, 0x5000), },
 	{ 0, }
@@ -131,7 +130,6 @@ struct bhf_dma {
 
 struct ec_bhf_priv {
 	struct net_device *net_dev;
-
 	struct pci_dev *dev;
 
 	void __iomem *io;
@@ -162,32 +160,6 @@ struct ec_bhf_priv {
 
 #define PRIV_TO_DEV(priv) (&(priv)->dev->dev)
 
-#define ETHERCAT_MASTER_ID	0x14
-
-static void ec_bhf_print_status(struct ec_bhf_priv *priv)
-{
-	struct device *dev = PRIV_TO_DEV(priv);
-
-	dev_dbg(dev, "Frame error counter: %d\n",
-		ioread8(priv->mac_io + MAC_FRAME_ERR_CNT));
-	dev_dbg(dev, "RX error counter: %d\n",
-		ioread8(priv->mac_io + MAC_RX_ERR_CNT));
-	dev_dbg(dev, "CRC error counter: %d\n",
-		ioread8(priv->mac_io + MAC_CRC_ERR_CNT));
-	dev_dbg(dev, "TX frame counter: %d\n",
-		ioread32(priv->mac_io + MAC_TX_FRAME_CNT));
-	dev_dbg(dev, "RX frame counter: %d\n",
-		ioread32(priv->mac_io + MAC_RX_FRAME_CNT));
-	dev_dbg(dev, "TX fifo level: %d\n",
-		ioread8(priv->mac_io + MAC_TX_FIFO_LVL));
-	dev_dbg(dev, "Dropped frames: %d\n",
-		ioread8(priv->mac_io + MAC_DROPPED_FRMS));
-	dev_dbg(dev, "Connected with CCAT slot: %d\n",
-		ioread8(priv->mac_io + MAC_CONNECTED_CCAT_FLAG));
-	dev_dbg(dev, "Link status: %d\n",
-		ioread8(priv->mii_io + MII_LINK_STATUS));
-}
-
 static void ec_bhf_reset(struct ec_bhf_priv *priv)
 {
 	iowrite8(0, priv->mac_io + MAC_FRAME_ERR_CNT);
@@ -210,8 +182,6 @@ static void ec_bhf_send_packet(struct ec_bhf_priv *priv, struct tx_desc *desc)
 	u32 addr = (u8 *)desc - priv->tx_buf.buf;
 
 	iowrite32((ALIGN(len, 8) << 24) | addr, priv->fifo_io + FIFO_TX_REG);
-
-	dev_dbg(PRIV_TO_DEV(priv), "Done sending packet\n");
 }
 
 static int ec_bhf_desc_sent(struct tx_desc *desc)
@@ -244,7 +214,6 @@ static void ec_bhf_add_rx_desc(struct ec_bhf_priv *priv, struct rx_desc *desc)
 static void ec_bhf_process_rx(struct ec_bhf_priv *priv)
 {
 	struct rx_desc *desc = &priv->rx_descs[priv->rx_dnext];
-	struct device *dev = PRIV_TO_DEV(priv);
 
 	while (ec_bhf_pkt_received(desc)) {
 		int pkt_size = (le16_to_cpu(desc->header.len) &
@@ -253,20 +222,16 @@ static void ec_bhf_process_rx(struct ec_bhf_priv *priv)
 		struct sk_buff *skb;
 
 		skb = netdev_alloc_skb_ip_align(priv->net_dev, pkt_size);
-		dev_dbg(dev, "Received packet, size: %d\n", pkt_size);
-
 		if (skb) {
 			memcpy(skb_put(skb, pkt_size), data, pkt_size);
 			skb->protocol = eth_type_trans(skb, priv->net_dev);
-			dev_dbg(dev, "Protocol type: %x\n", skb->protocol);
-
 			priv->stat_rx_bytes += pkt_size;
 
 			netif_rx(skb);
 		} else {
-			dev_err_ratelimited(dev,
-				"Couldn't allocate a skb_buff for a packet of size %u\n",
-				pkt_size);
+			dev_err_ratelimited(PRIV_TO_DEV(priv),
+					    "Couldn't allocate a skb_buff for a packet of size %u\n",
+					    pkt_size);
 		}
 
 		desc->header.recv = 0;
@@ -276,7 +241,6 @@ static void ec_bhf_process_rx(struct ec_bhf_priv *priv)
 		priv->rx_dnext = (priv->rx_dnext + 1) % priv->rx_dcount;
 		desc = &priv->rx_descs[priv->rx_dnext];
 	}
-
 }
 
 static enum hrtimer_restart ec_bhf_timer_fun(struct hrtimer *timer)
@@ -299,14 +263,7 @@ static int ec_bhf_setup_offsets(struct ec_bhf_priv *priv)
 	unsigned block_count, i;
 	void __iomem *ec_info;
 
-	dev_dbg(dev, "Info block:\n");
-	dev_dbg(dev, "Type of function: %x\n", (unsigned)ioread16(priv->io));
-	dev_dbg(dev, "Revision of function: %x\n",
-		(unsigned)ioread16(priv->io + INFO_BLOCK_REV));
-
 	block_count = ioread8(priv->io + INFO_BLOCK_BLK_CNT);
-	dev_dbg(dev, "Number of function blocks: %x\n", block_count);
-
 	for (i = 0; i < block_count; i++) {
 		u16 type = ioread16(priv->io + i * INFO_BLOCK_SIZE +
 				    INFO_BLOCK_TYPE);
@@ -317,29 +274,17 @@ static int ec_bhf_setup_offsets(struct ec_bhf_priv *priv)
 		dev_err(dev, "EtherCAT master with DMA block not found\n");
 		return -ENODEV;
 	}
-	dev_dbg(dev, "EtherCAT master with DMA block found at pos: %d\n", i);
 
 	ec_info = priv->io + i * INFO_BLOCK_SIZE;
-	dev_dbg(dev, "EtherCAT master revision: %d\n",
-		ioread16(ec_info + INFO_BLOCK_REV));
 
 	priv->tx_dma_chan = ioread8(ec_info + INFO_BLOCK_TX_CHAN);
-	dev_dbg(dev, "EtherCAT master tx dma channel: %d\n",
-		priv->tx_dma_chan);
-
 	priv->rx_dma_chan = ioread8(ec_info + INFO_BLOCK_RX_CHAN);
-	dev_dbg(dev, "EtherCAT master rx dma channel: %d\n",
-		 priv->rx_dma_chan);
 
 	priv->ec_io = priv->io + ioread32(ec_info + INFO_BLOCK_OFFSET);
 	priv->mii_io = priv->ec_io + ioread32(priv->ec_io + EC_MII_OFFSET);
 	priv->fifo_io = priv->ec_io + ioread32(priv->ec_io + EC_FIFO_OFFSET);
 	priv->mac_io = priv->ec_io + ioread32(priv->ec_io + EC_MAC_OFFSET);
 
-	dev_dbg(dev,
-		"EtherCAT block addres: %p, fifo address: %p, mii address: %p, mac address: %p\n",
-		priv->ec_io, priv->fifo_io, priv->mii_io, priv->mac_io);
-
 	return 0;
 }
 
@@ -350,8 +295,6 @@ static netdev_tx_t ec_bhf_start_xmit(struct sk_buff *skb,
 	struct tx_desc *desc;
 	unsigned len;
 
-	dev_dbg(PRIV_TO_DEV(priv), "Starting xmit\n");
-
 	desc = &priv->tx_descs[priv->tx_dnext];
 
 	skb_copy_and_csum_dev(skb, desc->data);
@@ -366,15 +309,12 @@ static netdev_tx_t ec_bhf_start_xmit(struct sk_buff *skb,
 	priv->tx_dnext = (priv->tx_dnext + 1) % priv->tx_dcount;
 
 	if (!ec_bhf_desc_sent(&priv->tx_descs[priv->tx_dnext])) {
-		/* Make sure that update updates to tx_dnext are perceived
+		/* Make sure that updates to tx_dnext are perceived
 		 * by timer routine.
 		 */
 		smp_wmb();
 
 		netif_stop_queue(net_dev);
-
-		dev_dbg(PRIV_TO_DEV(priv), "Stopping netif queue\n");
-		ec_bhf_print_status(priv);
 	}
 
 	priv->stat_tx_bytes += len;
@@ -397,7 +337,6 @@ static int ec_bhf_alloc_dma_mem(struct ec_bhf_priv *priv,
 
 	mask = ioread32(priv->dma_io + offset);
 	mask &= DMA_WINDOW_SIZE_MASK;
-	dev_dbg(dev, "Read mask %x for channel %d\n", mask, channel);
 
 	/* We want to allocate a chunk of memory that is:
 	 * - aligned to the mask we just read
@@ -408,12 +347,10 @@ static int ec_bhf_alloc_dma_mem(struct ec_bhf_priv *priv,
 	buf->len = min_t(int, ~mask + 1, size);
 	buf->alloc_len = 2 * buf->len;
 
-	dev_dbg(dev, "Allocating %d bytes for channel %d",
-		(int)buf->alloc_len, channel);
 	buf->alloc = dma_alloc_coherent(dev, buf->alloc_len, &buf->alloc_phys,
 					GFP_KERNEL);
 	if (buf->alloc == NULL) {
-		dev_info(dev, "Failed to allocate buffer\n");
+		dev_err(dev, "Failed to allocate buffer\n");
 		return -ENOMEM;
 	}
 
@@ -422,8 +359,6 @@ static int ec_bhf_alloc_dma_mem(struct ec_bhf_priv *priv,
 
 	iowrite32(0, priv->dma_io + offset + 4);
 	iowrite32(buf->buf_phys, priv->dma_io + offset);
-	dev_dbg(dev, "Buffer: %x and read from dev: %x",
-		(unsigned)buf->buf_phys, ioread32(priv->dma_io + offset));
 
 	return 0;
 }
@@ -433,7 +368,7 @@ static void ec_bhf_setup_tx_descs(struct ec_bhf_priv *priv)
 	int i = 0;
 
 	priv->tx_dcount = priv->tx_buf.len / sizeof(struct tx_desc);
-	priv->tx_descs = (struct tx_desc *) priv->tx_buf.buf;
+	priv->tx_descs = (struct tx_desc *)priv->tx_buf.buf;
 	priv->tx_dnext = 0;
 
 	for (i = 0; i < priv->tx_dcount; i++)
@@ -445,7 +380,7 @@ static void ec_bhf_setup_rx_descs(struct ec_bhf_priv *priv)
 	int i;
 
 	priv->rx_dcount = priv->rx_buf.len / sizeof(struct rx_desc);
-	priv->rx_descs = (struct rx_desc *) priv->rx_buf.buf;
+	priv->rx_descs = (struct rx_desc *)priv->rx_buf.buf;
 	priv->rx_dnext = 0;
 
 	for (i = 0; i < priv->rx_dcount; i++) {
@@ -469,8 +404,6 @@ static int ec_bhf_open(struct net_device *net_dev)
 	struct device *dev = PRIV_TO_DEV(priv);
 	int err = 0;
 
-	dev_info(dev, "Opening device\n");
-
 	ec_bhf_reset(priv);
 
 	err = ec_bhf_alloc_dma_mem(priv, &priv->rx_buf, priv->rx_dma_chan,
@@ -481,20 +414,13 @@ static int ec_bhf_open(struct net_device *net_dev)
 	}
 	ec_bhf_setup_rx_descs(priv);
 
-	dev_info(dev, "RX buffer allocated, address: %x\n",
-		 (unsigned)priv->rx_buf.buf_phys);
-
 	err = ec_bhf_alloc_dma_mem(priv, &priv->tx_buf, priv->tx_dma_chan,
 				   FIFO_SIZE * sizeof(struct tx_desc));
 	if (err) {
 		dev_err(dev, "Failed to allocate tx buffer\n");
 		goto error_rx_free;
 	}
-	dev_dbg(dev, "TX buffer allocated, addres: %x\n",
-		(unsigned)priv->tx_buf.buf_phys);
-
 	iowrite8(0, priv->mii_io + MII_MAC_FILT_FLAG);
-
 	ec_bhf_setup_tx_descs(priv);
 
 	netif_start_queue(net_dev);
@@ -504,10 +430,6 @@ static int ec_bhf_open(struct net_device *net_dev)
 	hrtimer_start(&priv->hrtimer, ktime_set(0, polling_frequency),
 		      HRTIMER_MODE_REL);
 
-	dev_info(PRIV_TO_DEV(priv), "Device open\n");
-
-	ec_bhf_print_status(priv);
-
 	return 0;
 
 error_rx_free:
@@ -640,9 +562,6 @@ static int ec_bhf_probe(struct pci_dev *dev, const struct pci_device_id *id)
 
 	memcpy_fromio(net_dev->dev_addr, priv->mii_io + MII_MAC_ADDR, 6);
 
-	dev_dbg(&dev->dev, "CX5020 Ethercat master address: %pM\n",
-		net_dev->dev_addr);
-
 	err = register_netdev(net_dev);
 	if (err < 0)
 		goto err_free_net_dev;
diff --git a/drivers/net/ethernet/emulex/benet/be.h b/drivers/net/ethernet/emulex/benet/be.h
index 43e08d0bc3d3..9a2d75235e89 100644
--- a/drivers/net/ethernet/emulex/benet/be.h
+++ b/drivers/net/ethernet/emulex/benet/be.h
@@ -86,6 +86,8 @@ static inline char *nic_name(struct pci_dev *pdev)
 
 #define BE_MAX_JUMBO_FRAME_SIZE	9018
 #define BE_MIN_MTU		256
+#define BE_MAX_MTU              (BE_MAX_JUMBO_FRAME_SIZE -	\
+				 (ETH_HLEN + ETH_FCS_LEN))
 
 #define BE_NUM_VLANS_SUPPORTED	64
 #define BE_MAX_EQD		128u
@@ -112,7 +114,6 @@ static inline char *nic_name(struct pci_dev *pdev)
 #define MAX_ROCE_EQS		5
 #define MAX_MSIX_VECTORS	32
 #define MIN_MSIX_VECTORS	1
-#define BE_TX_BUDGET		256
 #define BE_NAPI_WEIGHT		64
 #define MAX_RX_POST		BE_NAPI_WEIGHT /* Frags posted at a time */
 #define RX_FRAGS_REFILL_WM	(RX_Q_LEN - MAX_RX_POST)
@@ -198,7 +199,6 @@ struct be_eq_obj {
 
 	u8 idx;			/* array index */
 	u8 msix_idx;
-	u16 tx_budget;
 	u16 spurious_intr;
 	struct napi_struct napi;
 	struct be_adapter *adapter;
@@ -248,6 +248,13 @@ struct be_tx_stats {
 	ulong tx_jiffies;
 	u32 tx_stops;
 	u32 tx_drv_drops;	/* pkts dropped by driver */
+	/* the error counters are described in be_ethtool.c */
+	u32 tx_hdr_parse_err;
+	u32 tx_dma_err;
+	u32 tx_tso_err;
+	u32 tx_spoof_check_err;
+	u32 tx_qinq_err;
+	u32 tx_internal_parity_err;
 	struct u64_stats_sync sync;
 	struct u64_stats_sync sync_compl;
 };
@@ -316,6 +323,7 @@ struct be_rx_obj {
 struct be_drv_stats {
 	u32 be_on_die_temperature;
 	u32 eth_red_drops;
+	u32 dma_map_errors;
 	u32 rx_drops_no_pbuf;
 	u32 rx_drops_no_txpb;
 	u32 rx_drops_no_erx_descr;
@@ -399,9 +407,9 @@ struct phy_info {
 	u16 auto_speeds_supported;
 	u16 fixed_speeds_supported;
 	int link_speed;
-	u32 dac_cable_len;
 	u32 advertising;
 	u32 supported;
+	u8 cable_type;
 };
 
 struct be_resources {
@@ -613,6 +621,10 @@ extern const struct ethtool_ops be_ethtool_ops;
 	for (i = eqo->idx, rxo = &adapter->rx_obj[i]; i < adapter->num_rx_qs;\
 		 i += adapter->num_evt_qs, rxo += adapter->num_evt_qs)
 
+#define for_all_tx_queues_on_eq(adapter, eqo, txo, i)			\
+	for (i = eqo->idx, txo = &adapter->tx_obj[i]; i < adapter->num_tx_qs;\
+		i += adapter->num_evt_qs, txo += adapter->num_evt_qs)
+
 #define is_mcc_eqo(eqo)			(eqo->idx == 0)
 #define mcc_eqo(adapter)		(&adapter->eq_obj[0])
 
@@ -661,6 +673,18 @@ static inline u32 amap_get(void *ptr, u32 dw_offset, u32 mask, u32 offset)
 			amap_mask(sizeof(((_struct *)0)->field)),	\
 			AMAP_BIT_OFFSET(_struct, field))
 
+#define GET_RX_COMPL_V0_BITS(field, ptr)				\
+		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, field, ptr)
+
+#define GET_RX_COMPL_V1_BITS(field, ptr)				\
+		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, field, ptr)
+
+#define GET_TX_COMPL_BITS(field, ptr)					\
+		AMAP_GET_BITS(struct amap_eth_tx_compl, field, ptr)
+
+#define SET_TX_WRB_HDR_BITS(field, ptr, val)				\
+		AMAP_SET_BITS(struct amap_eth_hdr_wrb, field, ptr, val)
+
 #define be_dws_cpu_to_le(wrb, len)	swap_dws(wrb, len)
 #define be_dws_le_to_cpu(wrb, len)	swap_dws(wrb, len)
 static inline void swap_dws(void *wrb, int len)
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.c b/drivers/net/ethernet/emulex/benet/be_cmds.c
index 4370ec1952ac..fead5c65a4f0 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.c
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.c
@@ -209,7 +209,6 @@ static int be_mcc_compl_process(struct be_adapter *adapter,
 
 	if (base_status != MCC_STATUS_SUCCESS &&
 	    !be_skip_err_log(opcode, base_status, addl_status)) {
-
 		if (base_status == MCC_STATUS_UNAUTHORIZED_REQUEST) {
 			dev_warn(&adapter->pdev->dev,
 				 "VF is not privileged to issue opcode %d-%d\n",
@@ -309,8 +308,6 @@ static void be_async_grp5_evt_process(struct be_adapter *adapter,
 		be_async_grp5_pvid_state_process(adapter, compl);
 		break;
 	default:
-		dev_warn(&adapter->pdev->dev, "Unknown grp5 event 0x%x!\n",
-			 event_type);
 		break;
 	}
 }
@@ -319,7 +316,7 @@ static void be_async_dbg_evt_process(struct be_adapter *adapter,
 				     struct be_mcc_compl *cmp)
 {
 	u8 event_type = 0;
-	struct be_async_event_qnq *evt = (struct be_async_event_qnq *) cmp;
+	struct be_async_event_qnq *evt = (struct be_async_event_qnq *)cmp;
 
 	event_type = (cmp->flags >> ASYNC_EVENT_TYPE_SHIFT) &
 			ASYNC_EVENT_TYPE_MASK;
@@ -595,6 +592,7 @@ static int lancer_wait_ready(struct be_adapter *adapter)
 static bool lancer_provisioning_error(struct be_adapter *adapter)
 {
 	u32 sliport_status = 0, sliport_err1 = 0, sliport_err2 = 0;
+
 	sliport_status = ioread32(adapter->db + SLIPORT_STATUS_OFFSET);
 	if (sliport_status & SLIPORT_STATUS_ERR_MASK) {
 		sliport_err1 = ioread32(adapter->db + SLIPORT_ERROR1_OFFSET);
@@ -677,7 +675,6 @@ int be_fw_wait_ready(struct be_adapter *adapter)
 	return -1;
 }
 
-
 static inline struct be_sge *nonembedded_sgl(struct be_mcc_wrb *wrb)
 {
 	return &wrb->payload.sgl[0];
@@ -924,6 +921,7 @@ int be_cmd_eq_create(struct be_adapter *adapter, struct be_eq_obj *eqo)
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_eq_create *resp = embedded_payload(wrb);
+
 		eqo->q.id = le16_to_cpu(resp->eq_id);
 		eqo->msix_idx =
 			(ver == 2) ? le16_to_cpu(resp->msix_idx) : eqo->idx;
@@ -958,7 +956,7 @@ int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
 	if (permanent) {
 		req->permanent = 1;
 	} else {
-		req->if_id = cpu_to_le16((u16) if_handle);
+		req->if_id = cpu_to_le16((u16)if_handle);
 		req->pmac_id = cpu_to_le32(pmac_id);
 		req->permanent = 0;
 	}
@@ -966,6 +964,7 @@ int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_mac_query *resp = embedded_payload(wrb);
+
 		memcpy(mac_addr, resp->mac.addr, ETH_ALEN);
 	}
 
@@ -1002,6 +1001,7 @@ int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_pmac_add *resp = embedded_payload(wrb);
+
 		*pmac_id = le32_to_cpu(resp->pmac_id);
 	}
 
@@ -1034,7 +1034,8 @@ int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, int pmac_id, u32 dom)
 	req = embedded_payload(wrb);
 
 	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
-		OPCODE_COMMON_NTWK_PMAC_DEL, sizeof(*req), wrb, NULL);
+			       OPCODE_COMMON_NTWK_PMAC_DEL, sizeof(*req),
+			       wrb, NULL);
 
 	req->hdr.domain = dom;
 	req->if_id = cpu_to_le32(if_id);
@@ -1106,6 +1107,7 @@ int be_cmd_cq_create(struct be_adapter *adapter, struct be_queue_info *cq,
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_cq_create *resp = embedded_payload(wrb);
+
 		cq->id = le16_to_cpu(resp->cq_id);
 		cq->created = true;
 	}
@@ -1118,6 +1120,7 @@ int be_cmd_cq_create(struct be_adapter *adapter, struct be_queue_info *cq,
 static u32 be_encoded_q_len(int q_len)
 {
 	u32 len_encoded = fls(q_len); /* log2(len) + 1 */
+
 	if (len_encoded == 16)
 		len_encoded = 0;
 	return len_encoded;
@@ -1173,6 +1176,7 @@ static int be_cmd_mccq_ext_create(struct be_adapter *adapter,
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_mcc_create *resp = embedded_payload(wrb);
+
 		mccq->id = le16_to_cpu(resp->id);
 		mccq->created = true;
 	}
@@ -1216,6 +1220,7 @@ static int be_cmd_mccq_org_create(struct be_adapter *adapter,
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_mcc_create *resp = embedded_payload(wrb);
+
 		mccq->id = le16_to_cpu(resp->id);
 		mccq->created = true;
 	}
@@ -1274,6 +1279,7 @@ int be_cmd_txq_create(struct be_adapter *adapter, struct be_tx_obj *txo)
 	status = be_cmd_notify_wait(adapter, &wrb);
 	if (!status) {
 		struct be_cmd_resp_eth_tx_create *resp = embedded_payload(&wrb);
+
 		txq->id = le16_to_cpu(resp->cid);
 		if (ver == 2)
 			txo->db_offset = le32_to_cpu(resp->db_offset);
@@ -1318,6 +1324,7 @@ int be_cmd_rxq_create(struct be_adapter *adapter,
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_eth_rx_create *resp = embedded_payload(wrb);
+
 		rxq->id = le16_to_cpu(resp->id);
 		rxq->created = true;
 		*rss_id = resp->rss_id;
@@ -1431,6 +1438,7 @@ int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags, u32 en_flags,
 	status = be_cmd_notify_wait(adapter, &wrb);
 	if (!status) {
 		struct be_cmd_resp_if_create *resp = embedded_payload(&wrb);
+
 		*if_handle = le32_to_cpu(resp->interface_id);
 
 		/* Hack to retrieve VF's pmac-id on BE3 */
@@ -1514,7 +1522,6 @@ err:
 int lancer_cmd_get_pport_stats(struct be_adapter *adapter,
 			       struct be_dma_mem *nonemb_cmd)
 {
-
 	struct be_mcc_wrb *wrb;
 	struct lancer_cmd_req_pport_stats *req;
 	int status = 0;
@@ -1605,6 +1612,7 @@ int be_cmd_link_status_query(struct be_adapter *adapter, u16 *link_speed,
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_link_status *resp = embedded_payload(wrb);
+
 		if (link_speed) {
 			*link_speed = resp->link_speed ?
 				      le16_to_cpu(resp->link_speed) * 10 :
@@ -1672,6 +1680,7 @@ int be_cmd_get_reg_len(struct be_adapter *adapter, u32 *log_size)
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_get_fat *resp = embedded_payload(wrb);
+
 		if (log_size && resp->log_size)
 			*log_size = le32_to_cpu(resp->log_size) -
 					sizeof(u32);
@@ -1681,17 +1690,17 @@ err:
 	return status;
 }
 
-void be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
+int be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
 {
 	struct be_dma_mem get_fat_cmd;
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_fat *req;
 	u32 offset = 0, total_size, buf_size,
 				log_offset = sizeof(u32), payload_len;
-	int status;
+	int status = 0;
 
 	if (buf_len == 0)
-		return;
+		return -EIO;
 
 	total_size = buf_len;
 
@@ -1700,10 +1709,9 @@ void be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
 					      get_fat_cmd.size,
 					      &get_fat_cmd.dma);
 	if (!get_fat_cmd.va) {
-		status = -ENOMEM;
 		dev_err(&adapter->pdev->dev,
-		"Memory allocation failure while retrieving FAT data\n");
-		return;
+			"Memory allocation failure while reading FAT data\n");
+		return -ENOMEM;
 	}
 
 	spin_lock_bh(&adapter->mcc_lock);
@@ -1732,6 +1740,7 @@ void be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf)
 		status = be_mcc_notify_wait(adapter);
 		if (!status) {
 			struct be_cmd_resp_get_fat *resp = get_fat_cmd.va;
+
 			memcpy(buf + offset,
 			       resp->data_buffer,
 			       le32_to_cpu(resp->read_log_length));
@@ -1746,6 +1755,7 @@ err:
 	pci_free_consistent(adapter->pdev, get_fat_cmd.size,
 			    get_fat_cmd.va, get_fat_cmd.dma);
 	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
 }
 
 /* Uses synchronous mcc */
@@ -1771,8 +1781,11 @@ int be_cmd_get_fw_ver(struct be_adapter *adapter)
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_get_fw_version *resp = embedded_payload(wrb);
-		strcpy(adapter->fw_ver, resp->firmware_version_string);
-		strcpy(adapter->fw_on_flash, resp->fw_on_flash_version_string);
+
+		strlcpy(adapter->fw_ver, resp->firmware_version_string,
+			sizeof(adapter->fw_ver));
+		strlcpy(adapter->fw_on_flash, resp->fw_on_flash_version_string,
+			sizeof(adapter->fw_on_flash));
 	}
 err:
 	spin_unlock_bh(&adapter->mcc_lock);
@@ -1782,8 +1795,8 @@ err:
 /* set the EQ delay interval of an EQ to specified value
  * Uses async mcc
  */
-int be_cmd_modify_eqd(struct be_adapter *adapter, struct be_set_eqd *set_eqd,
-		      int num)
+static int __be_cmd_modify_eqd(struct be_adapter *adapter,
+			       struct be_set_eqd *set_eqd, int num)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_modify_eq_delay *req;
@@ -1816,6 +1829,25 @@ err:
 	return status;
 }
 
+int be_cmd_modify_eqd(struct be_adapter *adapter, struct be_set_eqd *set_eqd,
+		      int num)
+{
+	int num_eqs, i = 0;
+
+	if (lancer_chip(adapter) && num > 8) {
+		while (num) {
+			num_eqs = min(num, 8);
+			__be_cmd_modify_eqd(adapter, &set_eqd[i], num_eqs);
+			i += num_eqs;
+			num -= num_eqs;
+		}
+	} else {
+		__be_cmd_modify_eqd(adapter, set_eqd, num);
+	}
+
+	return 0;
+}
+
 /* Uses sycnhronous mcc */
 int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
 		       u32 num)
@@ -1879,8 +1911,8 @@ int be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 value)
 					    BE_IF_FLAGS_VLAN_PROMISCUOUS |
 					    BE_IF_FLAGS_MCAST_PROMISCUOUS);
 	} else if (flags & IFF_ALLMULTI) {
-		req->if_flags_mask = req->if_flags =
-				cpu_to_le32(BE_IF_FLAGS_MCAST_PROMISCUOUS);
+		req->if_flags_mask = cpu_to_le32(BE_IF_FLAGS_MCAST_PROMISCUOUS);
+		req->if_flags =	cpu_to_le32(BE_IF_FLAGS_MCAST_PROMISCUOUS);
 	} else if (flags & BE_FLAGS_VLAN_PROMISC) {
 		req->if_flags_mask = cpu_to_le32(BE_IF_FLAGS_VLAN_PROMISCUOUS);
 
@@ -1891,8 +1923,8 @@ int be_cmd_rx_filter(struct be_adapter *adapter, u32 flags, u32 value)
 		struct netdev_hw_addr *ha;
 		int i = 0;
 
-		req->if_flags_mask = req->if_flags =
-				cpu_to_le32(BE_IF_FLAGS_MULTICAST);
+		req->if_flags_mask = cpu_to_le32(BE_IF_FLAGS_MULTICAST);
+		req->if_flags =	cpu_to_le32(BE_IF_FLAGS_MULTICAST);
 
 		/* Reset mcast promisc mode if already set by setting mask
 		 * and not setting flags field
@@ -1947,6 +1979,7 @@ int be_cmd_set_flow_control(struct be_adapter *adapter, u32 tx_fc, u32 rx_fc)
 			       OPCODE_COMMON_SET_FLOW_CONTROL, sizeof(*req),
 			       wrb, NULL);
 
+	req->hdr.version = 1;
 	req->tx_flow_control = cpu_to_le16((u16)tx_fc);
 	req->rx_flow_control = cpu_to_le16((u16)rx_fc);
 
@@ -1954,6 +1987,10 @@ int be_cmd_set_flow_control(struct be_adapter *adapter, u32 tx_fc, u32 rx_fc)
 
 err:
 	spin_unlock_bh(&adapter->mcc_lock);
+
+	if (base_status(status) == MCC_STATUS_FEATURE_NOT_SUPPORTED)
+		return  -EOPNOTSUPP;
+
 	return status;
 }
 
@@ -1985,6 +2022,7 @@ int be_cmd_get_flow_control(struct be_adapter *adapter, u32 *tx_fc, u32 *rx_fc)
 	if (!status) {
 		struct be_cmd_resp_get_flow_control *resp =
 						embedded_payload(wrb);
+
 		*tx_fc = le16_to_cpu(resp->tx_flow_control);
 		*rx_fc = le16_to_cpu(resp->rx_flow_control);
 	}
@@ -2014,10 +2052,14 @@ int be_cmd_query_fw_cfg(struct be_adapter *adapter)
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_query_fw_cfg *resp = embedded_payload(wrb);
+
 		adapter->port_num = le32_to_cpu(resp->phys_port);
 		adapter->function_mode = le32_to_cpu(resp->function_mode);
 		adapter->function_caps = le32_to_cpu(resp->function_caps);
 		adapter->asic_rev = le32_to_cpu(resp->asic_revision) & 0xFF;
+		dev_info(&adapter->pdev->dev,
+			 "FW config: function_mode=0x%x, function_caps=0x%x\n",
+			 adapter->function_mode, adapter->function_caps);
 	}
 
 	mutex_unlock(&adapter->mbox_lock);
@@ -2159,6 +2201,7 @@ int be_cmd_get_beacon_state(struct be_adapter *adapter, u8 port_num, u32 *state)
 	if (!status) {
 		struct be_cmd_resp_get_beacon_state *resp =
 						embedded_payload(wrb);
+
 		*state = resp->beacon_state;
 	}
 
@@ -2167,6 +2210,53 @@ err:
 	return status;
 }
 
+/* Uses sync mcc */
+int be_cmd_read_port_transceiver_data(struct be_adapter *adapter,
+				      u8 page_num, u8 *data)
+{
+	struct be_dma_mem cmd;
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_port_type *req;
+	int status;
+
+	if (page_num > TR_PAGE_A2)
+		return -EINVAL;
+
+	cmd.size = sizeof(struct be_cmd_resp_port_type);
+	cmd.va = pci_alloc_consistent(adapter->pdev, cmd.size, &cmd.dma);
+	if (!cmd.va) {
+		dev_err(&adapter->pdev->dev, "Memory allocation failed\n");
+		return -ENOMEM;
+	}
+	memset(cmd.va, 0, cmd.size);
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = cmd.va;
+
+	be_wrb_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+			       OPCODE_COMMON_READ_TRANSRECV_DATA,
+			       cmd.size, wrb, &cmd);
+
+	req->port = cpu_to_le32(adapter->hba_port_num);
+	req->page_num = cpu_to_le32(page_num);
+	status = be_mcc_notify_wait(adapter);
+	if (!status) {
+		struct be_cmd_resp_port_type *resp = cmd.va;
+
+		memcpy(data, resp->page_data, PAGE_DATA_LEN);
+	}
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	pci_free_consistent(adapter->pdev, cmd.size, cmd.va, cmd.dma);
+	return status;
+}
+
 int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 			    u32 data_size, u32 data_offset,
 			    const char *obj_name, u32 *data_written,
@@ -2207,7 +2297,7 @@ int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
 
 	be_dws_cpu_to_le(ctxt, sizeof(req->context));
 	req->write_offset = cpu_to_le32(data_offset);
-	strcpy(req->object_name, obj_name);
+	strlcpy(req->object_name, obj_name, sizeof(req->object_name));
 	req->descriptor_count = cpu_to_le32(1);
 	req->buf_len = cpu_to_le32(data_size);
 	req->addr_low = cpu_to_le32((cmd->dma +
@@ -2240,6 +2330,31 @@ err_unlock:
 	return status;
 }
 
+int be_cmd_query_cable_type(struct be_adapter *adapter)
+{
+	u8 page_data[PAGE_DATA_LEN];
+	int status;
+
+	status = be_cmd_read_port_transceiver_data(adapter, TR_PAGE_A0,
+						   page_data);
+	if (!status) {
+		switch (adapter->phy.interface_type) {
+		case PHY_TYPE_QSFP:
+			adapter->phy.cable_type =
+				page_data[QSFP_PLUS_CABLE_TYPE_OFFSET];
+			break;
+		case PHY_TYPE_SFP_PLUS_10GB:
+			adapter->phy.cable_type =
+				page_data[SFP_PLUS_CABLE_TYPE_OFFSET];
+			break;
+		default:
+			adapter->phy.cable_type = 0;
+			break;
+		}
+	}
+	return status;
+}
+
 int lancer_cmd_delete_object(struct be_adapter *adapter, const char *obj_name)
 {
 	struct lancer_cmd_req_delete_object *req;
@@ -2260,7 +2375,7 @@ int lancer_cmd_delete_object(struct be_adapter *adapter, const char *obj_name)
 			       OPCODE_COMMON_DELETE_OBJECT,
 			       sizeof(*req), wrb, NULL);
 
-	strcpy(req->object_name, obj_name);
+	strlcpy(req->object_name, obj_name, sizeof(req->object_name));
 
 	status = be_mcc_notify_wait(adapter);
 err:
@@ -2357,7 +2472,7 @@ err_unlock:
 }
 
 int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc,
-			  u16 optype, int offset)
+			 u16 optype, int offset)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_read_flash_crc *req;
@@ -2528,9 +2643,10 @@ int be_cmd_ddr_dma_test(struct be_adapter *adapter, u64 pattern,
 
 	if (!status) {
 		struct be_cmd_resp_ddrdma_test *resp;
+
 		resp = cmd->va;
 		if ((memcmp(resp->rcv_buff, req->snd_buff, byte_cnt) != 0) ||
-				resp->snd_err) {
+		    resp->snd_err) {
 			status = -1;
 		}
 	}
@@ -2603,6 +2719,7 @@ int be_cmd_get_phy_info(struct be_adapter *adapter)
 	if (!status) {
 		struct be_phy_info *resp_phy_info =
 				cmd.va + sizeof(struct be_cmd_req_hdr);
+
 		adapter->phy.phy_type = le16_to_cpu(resp_phy_info->phy_type);
 		adapter->phy.interface_type =
 			le16_to_cpu(resp_phy_info->interface_type);
@@ -2732,6 +2849,7 @@ int be_cmd_req_native_mode(struct be_adapter *adapter)
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_set_func_cap *resp = embedded_payload(wrb);
+
 		adapter->be3_native = le32_to_cpu(resp->cap_flags) &
 					CAPABILITY_BE3_NATIVE_ERX_API;
 		if (!adapter->be3_native)
@@ -2771,6 +2889,7 @@ int be_cmd_get_fn_privileges(struct be_adapter *adapter, u32 *privilege,
 	if (!status) {
 		struct be_cmd_resp_get_fn_privileges *resp =
 						embedded_payload(wrb);
+
 		*privilege = le32_to_cpu(resp->privilege_mask);
 
 		/* In UMC mode FW does not return right privileges.
@@ -2918,7 +3037,6 @@ out:
 int be_cmd_get_active_mac(struct be_adapter *adapter, u32 curr_pmac_id,
 			  u8 *mac, u32 if_handle, bool active, u32 domain)
 {
-
 	if (!active)
 		be_cmd_get_mac_from_list(adapter, mac, &active, &curr_pmac_id,
 					 if_handle, domain);
@@ -3102,6 +3220,7 @@ int be_cmd_get_hsw_config(struct be_adapter *adapter, u16 *pvid,
 	if (!status) {
 		struct be_cmd_resp_get_hsw_config *resp =
 						embedded_payload(wrb);
+
 		be_dws_le_to_cpu(&resp->context, sizeof(resp->context));
 		vid = AMAP_GET_BITS(struct amap_get_hsw_resp_context,
 				    pvid, &resp->context);
@@ -3161,7 +3280,8 @@ int be_cmd_get_acpi_wol_cap(struct be_adapter *adapter)
 	status = be_mbox_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_acpi_wol_magic_config_v1 *resp;
-		resp = (struct be_cmd_resp_acpi_wol_magic_config_v1 *) cmd.va;
+
+		resp = (struct be_cmd_resp_acpi_wol_magic_config_v1 *)cmd.va;
 
 		adapter->wol_cap = resp->wol_settings;
 		if (adapter->wol_cap & BE_WOL_CAP)
@@ -3197,6 +3317,7 @@ int be_cmd_set_fw_log_level(struct be_adapter *adapter, u32 level)
 			(extfat_cmd.va + sizeof(struct be_cmd_resp_hdr));
 	for (i = 0; i < le32_to_cpu(cfgs->num_modules); i++) {
 		u32 num_modes = le32_to_cpu(cfgs->module[i].num_modes);
+
 		for (j = 0; j < num_modes; j++) {
 			if (cfgs->module[i].trace_lvl[j].mode == MODE_UART)
 				cfgs->module[i].trace_lvl[j].dbg_lvl =
@@ -3233,6 +3354,7 @@ int be_cmd_get_fw_log_level(struct be_adapter *adapter)
 	if (!status) {
 		cfgs = (struct be_fat_conf_params *)(extfat_cmd.va +
 						sizeof(struct be_cmd_resp_hdr));
+
 		for (j = 0; j < le32_to_cpu(cfgs->module[0].num_modes); j++) {
 			if (cfgs->module[0].trace_lvl[j].mode == MODE_UART)
 				level = cfgs->module[0].trace_lvl[j].dbg_lvl;
@@ -3329,6 +3451,7 @@ int be_cmd_query_port_name(struct be_adapter *adapter, u8 *port_name)
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_get_port_name *resp = embedded_payload(wrb);
+
 		*port_name = resp->port_name[adapter->hba_port_num];
 	} else {
 		*port_name = adapter->hba_port_num + '0';
@@ -3952,6 +4075,7 @@ int be_cmd_get_active_profile(struct be_adapter *adapter, u16 *profile_id)
 	if (!status) {
 		struct be_cmd_resp_get_active_profile *resp =
 							embedded_payload(wrb);
+
 		*profile_id = le16_to_cpu(resp->active_profile_id);
 	}
 
@@ -4004,7 +4128,7 @@ int be_roce_mcc_cmd(void *netdev_handle, void *wrb_payload,
 {
 	struct be_adapter *adapter = netdev_priv(netdev_handle);
 	struct be_mcc_wrb *wrb;
-	struct be_cmd_req_hdr *hdr = (struct be_cmd_req_hdr *) wrb_payload;
+	struct be_cmd_req_hdr *hdr = (struct be_cmd_req_hdr *)wrb_payload;
 	struct be_cmd_req_hdr *req;
 	struct be_cmd_resp_hdr *resp;
 	int status;
diff --git a/drivers/net/ethernet/emulex/benet/be_cmds.h b/drivers/net/ethernet/emulex/benet/be_cmds.h
index 5284b825bba2..eb5085d6794f 100644
--- a/drivers/net/ethernet/emulex/benet/be_cmds.h
+++ b/drivers/net/ethernet/emulex/benet/be_cmds.h
@@ -57,7 +57,8 @@ enum mcc_base_status {
 	MCC_STATUS_ILLEGAL_FIELD = 3,
 	MCC_STATUS_INSUFFICIENT_BUFFER = 4,
 	MCC_STATUS_UNAUTHORIZED_REQUEST = 5,
-	MCC_STATUS_NOT_SUPPORTED = 66
+	MCC_STATUS_NOT_SUPPORTED = 66,
+	MCC_STATUS_FEATURE_NOT_SUPPORTED = 68
 };
 
 /* Additional status */
@@ -1004,8 +1005,8 @@ struct be_cmd_resp_link_status {
 /*    Identifies the type of port attached to NIC     */
 struct be_cmd_req_port_type {
 	struct be_cmd_req_hdr hdr;
-	u32 page_num;
-	u32 port;
+	__le32 page_num;
+	__le32 port;
 };
 
 enum {
@@ -1013,28 +1014,23 @@ enum {
 	TR_PAGE_A2 = 0xa2
 };
 
+/* From SFF-8436 QSFP+ spec */
+#define	QSFP_PLUS_CABLE_TYPE_OFFSET	0x83
+#define	QSFP_PLUS_CR4_CABLE		0x8
+#define	QSFP_PLUS_SR4_CABLE		0x4
+#define	QSFP_PLUS_LR4_CABLE		0x2
+
+/* From SFF-8472 spec */
+#define	SFP_PLUS_SFF_8472_COMP		0x5E
+#define	SFP_PLUS_CABLE_TYPE_OFFSET	0x8
+#define	SFP_PLUS_COPPER_CABLE		0x4
+
+#define PAGE_DATA_LEN   256
 struct be_cmd_resp_port_type {
 	struct be_cmd_resp_hdr hdr;
 	u32 page_num;
 	u32 port;
-	struct data {
-		u8 identifier;
-		u8 identifier_ext;
-		u8 connector;
-		u8 transceiver[8];
-		u8 rsvd0[3];
-		u8 length_km;
-		u8 length_hm;
-		u8 length_om1;
-		u8 length_om2;
-		u8 length_cu;
-		u8 length_cu_m;
-		u8 vendor_name[16];
-		u8 rsvd;
-		u8 vendor_oui[3];
-		u8 vendor_pn[16];
-		u8 vendor_rev[4];
-	} data;
+	u8  page_data[PAGE_DATA_LEN];
 };
 
 /******************** Get FW Version *******************/
@@ -1367,6 +1363,9 @@ enum {
 	PHY_TYPE_BASET_1GB,
 	PHY_TYPE_BASEX_1GB,
 	PHY_TYPE_SGMII,
+	PHY_TYPE_QSFP,
+	PHY_TYPE_KR4_40GB,
+	PHY_TYPE_KR2_20GB,
 	PHY_TYPE_DISABLED = 255
 };
 
@@ -1375,6 +1374,8 @@ enum {
 #define BE_SUPPORTED_SPEED_100MBPS	2
 #define BE_SUPPORTED_SPEED_1GBPS	4
 #define BE_SUPPORTED_SPEED_10GBPS	8
+#define BE_SUPPORTED_SPEED_20GBPS	0x10
+#define BE_SUPPORTED_SPEED_40GBPS	0x20
 
 #define BE_AN_EN			0x2
 #define BE_PAUSE_SYM_EN			0x80
@@ -2066,6 +2067,9 @@ int be_cmd_set_beacon_state(struct be_adapter *adapter, u8 port_num, u8 beacon,
 			    u8 status, u8 state);
 int be_cmd_get_beacon_state(struct be_adapter *adapter, u8 port_num,
 			    u32 *state);
+int be_cmd_read_port_transceiver_data(struct be_adapter *adapter,
+				      u8 page_num, u8 *data);
+int be_cmd_query_cable_type(struct be_adapter *adapter);
 int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
 			  u32 flash_oper, u32 flash_opcode, u32 buf_size);
 int lancer_cmd_write_object(struct be_adapter *adapter, struct be_dma_mem *cmd,
@@ -2101,7 +2105,7 @@ int be_cmd_get_die_temperature(struct be_adapter *adapter);
 int be_cmd_get_cntl_attributes(struct be_adapter *adapter);
 int be_cmd_req_native_mode(struct be_adapter *adapter);
 int be_cmd_get_reg_len(struct be_adapter *adapter, u32 *log_size);
-void be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf);
+int be_cmd_get_regs(struct be_adapter *adapter, u32 buf_len, void *buf);
 int be_cmd_get_fn_privileges(struct be_adapter *adapter, u32 *privilege,
 			     u32 domain);
 int be_cmd_set_fn_privileges(struct be_adapter *adapter, u32 privileges,
diff --git a/drivers/net/ethernet/emulex/benet/be_ethtool.c b/drivers/net/ethernet/emulex/benet/be_ethtool.c
index 0cd3311409a8..e42a791c1835 100644
--- a/drivers/net/ethernet/emulex/benet/be_ethtool.c
+++ b/drivers/net/ethernet/emulex/benet/be_ethtool.c
@@ -78,6 +78,11 @@ static const struct be_ethtool_stat et_stats[] = {
 	 * fifo must never overflow.
 	 */
 	{DRVSTAT_INFO(rxpp_fifo_overflow_drop)},
+	/* Received packets dropped when the RX block runs out of space in
+	 * one of its input FIFOs. This could happen due a long burst of
+	 * minimum-sized (64b) frames in the receive path.
+	 * This counter may also be erroneously incremented rarely.
+	 */
 	{DRVSTAT_INFO(rx_input_fifo_overflow_drop)},
 	{DRVSTAT_INFO(rx_ip_checksum_errs)},
 	{DRVSTAT_INFO(rx_tcp_checksum_errs)},
@@ -114,6 +119,8 @@ static const struct be_ethtool_stat et_stats[] = {
 	 * is more than 9018 bytes
 	 */
 	{DRVSTAT_INFO(rx_drops_mtu)},
+	/* Number of dma mapping errors */
+	{DRVSTAT_INFO(dma_map_errors)},
 	/* Number of packets dropped due to random early drop function */
 	{DRVSTAT_INFO(eth_red_drops)},
 	{DRVSTAT_INFO(be_on_die_temperature)},
@@ -123,6 +130,7 @@ static const struct be_ethtool_stat et_stats[] = {
 	{DRVSTAT_INFO(roce_drops_payload_len)},
 	{DRVSTAT_INFO(roce_drops_crc)}
 };
+
 #define ETHTOOL_STATS_NUM ARRAY_SIZE(et_stats)
 
 /* Stats related to multi RX queues: get_stats routine assumes bytes, pkts
@@ -145,6 +153,7 @@ static const struct be_ethtool_stat et_rx_stats[] = {
 	 */
 	{DRVSTAT_RX_INFO(rx_drops_no_frags)}
 };
+
 #define ETHTOOL_RXSTATS_NUM (ARRAY_SIZE(et_rx_stats))
 
 /* Stats related to multi TX queues: get_stats routine assumes compl is the
@@ -152,6 +161,34 @@ static const struct be_ethtool_stat et_rx_stats[] = {
  */
 static const struct be_ethtool_stat et_tx_stats[] = {
 	{DRVSTAT_TX_INFO(tx_compl)}, /* If moving this member see above note */
+	/* This counter is incremented when the HW encounters an error while
+	 * parsing the packet header of an outgoing TX request. This counter is
+	 * applicable only for BE2, BE3 and Skyhawk based adapters.
+	 */
+	{DRVSTAT_TX_INFO(tx_hdr_parse_err)},
+	/* This counter is incremented when an error occurs in the DMA
+	 * operation associated with the TX request from the host to the device.
+	 */
+	{DRVSTAT_TX_INFO(tx_dma_err)},
+	/* This counter is incremented when MAC or VLAN spoof checking is
+	 * enabled on the interface and the TX request fails the spoof check
+	 * in HW.
+	 */
+	{DRVSTAT_TX_INFO(tx_spoof_check_err)},
+	/* This counter is incremented when the HW encounters an error while
+	 * performing TSO offload. This counter is applicable only for Lancer
+	 * adapters.
+	 */
+	{DRVSTAT_TX_INFO(tx_tso_err)},
+	/* This counter is incremented when the HW detects Q-in-Q style VLAN
+	 * tagging in a packet and such tagging is not expected on the outgoing
+	 * interface. This counter is applicable only for Lancer adapters.
+	 */
+	{DRVSTAT_TX_INFO(tx_qinq_err)},
+	/* This counter is incremented when the HW detects parity errors in the
+	 * packet data. This counter is applicable only for Lancer adapters.
+	 */
+	{DRVSTAT_TX_INFO(tx_internal_parity_err)},
 	{DRVSTAT_TX_INFO(tx_bytes)},
 	{DRVSTAT_TX_INFO(tx_pkts)},
 	/* Number of skbs queued for trasmission by the driver */
@@ -165,6 +202,7 @@ static const struct be_ethtool_stat et_tx_stats[] = {
 	/* Pkts dropped in the driver's transmit path */
 	{DRVSTAT_TX_INFO(tx_drv_drops)}
 };
+
 #define ETHTOOL_TXSTATS_NUM (ARRAY_SIZE(et_tx_stats))
 
 static const char et_self_tests[][ETH_GSTRING_LEN] = {
@@ -239,7 +277,7 @@ static int lancer_cmd_read_file(struct be_adapter *adapter, u8 *file_name,
 
 	while ((total_read_len < buf_len) && !eof) {
 		chunk_size = min_t(u32, (buf_len - total_read_len),
-				LANCER_READ_FILE_CHUNK);
+				   LANCER_READ_FILE_CHUNK);
 		chunk_size = ALIGN(chunk_size, 4);
 		status = lancer_cmd_read_object(adapter, &read_cmd, chunk_size,
 						total_read_len, file_name,
@@ -298,7 +336,6 @@ static int be_get_coalesce(struct net_device *netdev,
 	struct be_adapter *adapter = netdev_priv(netdev);
 	struct be_aic_obj *aic = &adapter->aic_obj[0];
 
-
 	et->rx_coalesce_usecs = aic->prev_eqd;
 	et->rx_coalesce_usecs_high = aic->max_eqd;
 	et->rx_coalesce_usecs_low = aic->min_eqd;
@@ -440,18 +477,27 @@ static int be_get_sset_count(struct net_device *netdev, int stringset)
 	}
 }
 
-static u32 be_get_port_type(u32 phy_type, u32 dac_cable_len)
+static u32 be_get_port_type(struct be_adapter *adapter)
 {
 	u32 port;
 
-	switch (phy_type) {
+	switch (adapter->phy.interface_type) {
 	case PHY_TYPE_BASET_1GB:
 	case PHY_TYPE_BASEX_1GB:
 	case PHY_TYPE_SGMII:
 		port = PORT_TP;
 		break;
 	case PHY_TYPE_SFP_PLUS_10GB:
-		port = dac_cable_len ? PORT_DA : PORT_FIBRE;
+		if (adapter->phy.cable_type & SFP_PLUS_COPPER_CABLE)
+			port = PORT_DA;
+		else
+			port = PORT_FIBRE;
+		break;
+	case PHY_TYPE_QSFP:
+		if (adapter->phy.cable_type & QSFP_PLUS_CR4_CABLE)
+			port = PORT_DA;
+		else
+			port = PORT_FIBRE;
 		break;
 	case PHY_TYPE_XFP_10GB:
 	case PHY_TYPE_SFP_1GB:
@@ -467,11 +513,11 @@ static u32 be_get_port_type(u32 phy_type, u32 dac_cable_len)
 	return port;
 }
 
-static u32 convert_to_et_setting(u32 if_type, u32 if_speeds)
+static u32 convert_to_et_setting(struct be_adapter *adapter, u32 if_speeds)
 {
 	u32 val = 0;
 
-	switch (if_type) {
+	switch (adapter->phy.interface_type) {
 	case PHY_TYPE_BASET_1GB:
 	case PHY_TYPE_BASEX_1GB:
 	case PHY_TYPE_SGMII:
@@ -490,10 +536,38 @@ static u32 convert_to_et_setting(u32 if_type, u32 if_speeds)
 		if (if_speeds & BE_SUPPORTED_SPEED_10GBPS)
 			val |= SUPPORTED_10000baseKX4_Full;
 		break;
+	case PHY_TYPE_KR2_20GB:
+		val |= SUPPORTED_Backplane;
+		if (if_speeds & BE_SUPPORTED_SPEED_10GBPS)
+			val |= SUPPORTED_10000baseKR_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_20GBPS)
+			val |= SUPPORTED_20000baseKR2_Full;
+		break;
 	case PHY_TYPE_KR_10GB:
 		val |= SUPPORTED_Backplane |
 				SUPPORTED_10000baseKR_Full;
 		break;
+	case PHY_TYPE_KR4_40GB:
+		val |= SUPPORTED_Backplane;
+		if (if_speeds & BE_SUPPORTED_SPEED_10GBPS)
+			val |= SUPPORTED_10000baseKR_Full;
+		if (if_speeds & BE_SUPPORTED_SPEED_40GBPS)
+			val |= SUPPORTED_40000baseKR4_Full;
+		break;
+	case PHY_TYPE_QSFP:
+		if (if_speeds & BE_SUPPORTED_SPEED_40GBPS) {
+			switch (adapter->phy.cable_type) {
+			case QSFP_PLUS_CR4_CABLE:
+				val |= SUPPORTED_40000baseCR4_Full;
+				break;
+			case QSFP_PLUS_LR4_CABLE:
+				val |= SUPPORTED_40000baseLR4_Full;
+				break;
+			default:
+				val |= SUPPORTED_40000baseSR4_Full;
+				break;
+			}
+		}
 	case PHY_TYPE_SFP_PLUS_10GB:
 	case PHY_TYPE_XFP_10GB:
 	case PHY_TYPE_SFP_1GB:
@@ -534,8 +608,6 @@ static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 	int status;
 	u32 auto_speeds;
 	u32 fixed_speeds;
-	u32 dac_cable_len;
-	u16 interface_type;
 
 	if (adapter->phy.link_speed < 0) {
 		status = be_cmd_link_status_query(adapter, &link_speed,
@@ -546,21 +618,19 @@ static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 
 		status = be_cmd_get_phy_info(adapter);
 		if (!status) {
-			interface_type = adapter->phy.interface_type;
 			auto_speeds = adapter->phy.auto_speeds_supported;
 			fixed_speeds = adapter->phy.fixed_speeds_supported;
-			dac_cable_len = adapter->phy.dac_cable_len;
+
+			be_cmd_query_cable_type(adapter);
 
 			ecmd->supported =
-				convert_to_et_setting(interface_type,
+				convert_to_et_setting(adapter,
 						      auto_speeds |
 						      fixed_speeds);
 			ecmd->advertising =
-				convert_to_et_setting(interface_type,
-						      auto_speeds);
+				convert_to_et_setting(adapter, auto_speeds);
 
-			ecmd->port = be_get_port_type(interface_type,
-						      dac_cable_len);
+			ecmd->port = be_get_port_type(adapter);
 
 			if (adapter->phy.auto_speeds_supported) {
 				ecmd->supported |= SUPPORTED_Autoneg;
@@ -614,8 +684,10 @@ static void be_get_ringparam(struct net_device *netdev,
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
 
-	ring->rx_max_pending = ring->rx_pending = adapter->rx_obj[0].q.len;
-	ring->tx_max_pending = ring->tx_pending = adapter->tx_obj[0].q.len;
+	ring->rx_max_pending = adapter->rx_obj[0].q.len;
+	ring->rx_pending = adapter->rx_obj[0].q.len;
+	ring->tx_max_pending = adapter->tx_obj[0].q.len;
+	ring->tx_pending = adapter->tx_obj[0].q.len;
 }
 
 static void
@@ -641,7 +713,7 @@ be_set_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
 	status = be_cmd_set_flow_control(adapter,
 					 adapter->tx_fc, adapter->rx_fc);
 	if (status)
-		dev_warn(&adapter->pdev->dev, "Pause param set failed.\n");
+		dev_warn(&adapter->pdev->dev, "Pause param set failed\n");
 
 	return be_cmd_status(status);
 }
@@ -907,8 +979,6 @@ static void be_set_msg_level(struct net_device *netdev, u32 level)
 						FW_LOG_LEVEL_DEFAULT :
 						FW_LOG_LEVEL_FATAL);
 	adapter->msg_enable = level;
-
-	return;
 }
 
 static u64 be_get_rss_hash_opts(struct be_adapter *adapter, u64 flow_type)
@@ -1127,6 +1197,7 @@ static int be_set_rxfh(struct net_device *netdev, const u32 *indir,
 
 	if (indir) {
 		struct be_rx_obj *rxo;
+
 		for (i = 0; i < RSS_INDIR_TABLE_LEN; i++) {
 			j = indir[i];
 			rxo = &adapter->rx_obj[j];
@@ -1142,8 +1213,8 @@ static int be_set_rxfh(struct net_device *netdev, const u32 *indir,
 		hkey =  adapter->rss_info.rss_hkey;
 
 	rc = be_cmd_rss_config(adapter, rsstable,
-			adapter->rss_info.rss_flags,
-			RSS_INDIR_TABLE_LEN, hkey);
+			       adapter->rss_info.rss_flags,
+			       RSS_INDIR_TABLE_LEN, hkey);
 	if (rc) {
 		adapter->rss_info.rss_flags = RSS_ENABLE_NONE;
 		return -EIO;
@@ -1154,6 +1225,58 @@ static int be_set_rxfh(struct net_device *netdev, const u32 *indir,
 	return 0;
 }
 
+static int be_get_module_info(struct net_device *netdev,
+			      struct ethtool_modinfo *modinfo)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	u8 page_data[PAGE_DATA_LEN];
+	int status;
+
+	if (!check_privilege(adapter, MAX_PRIVILEGES))
+		return -EOPNOTSUPP;
+
+	status = be_cmd_read_port_transceiver_data(adapter, TR_PAGE_A0,
+						   page_data);
+	if (!status) {
+		if (!page_data[SFP_PLUS_SFF_8472_COMP]) {
+			modinfo->type = ETH_MODULE_SFF_8079;
+			modinfo->eeprom_len = PAGE_DATA_LEN;
+		} else {
+			modinfo->type = ETH_MODULE_SFF_8472;
+			modinfo->eeprom_len = 2 * PAGE_DATA_LEN;
+		}
+	}
+	return be_cmd_status(status);
+}
+
+static int be_get_module_eeprom(struct net_device *netdev,
+				struct ethtool_eeprom *eeprom, u8 *data)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	int status;
+
+	if (!check_privilege(adapter, MAX_PRIVILEGES))
+		return -EOPNOTSUPP;
+
+	status = be_cmd_read_port_transceiver_data(adapter, TR_PAGE_A0,
+						   data);
+	if (status)
+		goto err;
+
+	if (eeprom->offset + eeprom->len > PAGE_DATA_LEN) {
+		status = be_cmd_read_port_transceiver_data(adapter,
+							   TR_PAGE_A2,
+							   data +
+							   PAGE_DATA_LEN);
+		if (status)
+			goto err;
+	}
+	if (eeprom->offset)
+		memcpy(data, data + eeprom->offset, eeprom->len);
+err:
+	return be_cmd_status(status);
+}
+
 const struct ethtool_ops be_ethtool_ops = {
 	.get_settings = be_get_settings,
 	.get_drvinfo = be_get_drvinfo,
@@ -1185,5 +1308,7 @@ const struct ethtool_ops be_ethtool_ops = {
 	.get_rxfh = be_get_rxfh,
 	.set_rxfh = be_set_rxfh,
 	.get_channels = be_get_channels,
-	.set_channels = be_set_channels
+	.set_channels = be_set_channels,
+	.get_module_info = be_get_module_info,
+	.get_module_eeprom = be_get_module_eeprom
 };
diff --git a/drivers/net/ethernet/emulex/benet/be_hw.h b/drivers/net/ethernet/emulex/benet/be_hw.h
index 8840c64aaeca..295ee0835ba0 100644
--- a/drivers/net/ethernet/emulex/benet/be_hw.h
+++ b/drivers/net/ethernet/emulex/benet/be_hw.h
@@ -315,6 +315,18 @@ struct be_eth_hdr_wrb {
 	u32 dw[4];
 };
 
+/********* Tx Compl Status Encoding *********/
+#define BE_TX_COMP_HDR_PARSE_ERR	0x2
+#define BE_TX_COMP_NDMA_ERR		0x3
+#define BE_TX_COMP_ACL_ERR		0x5
+
+#define LANCER_TX_COMP_LSO_ERR			0x1
+#define LANCER_TX_COMP_HSW_DROP_MAC_ERR		0x3
+#define LANCER_TX_COMP_HSW_DROP_VLAN_ERR	0x5
+#define LANCER_TX_COMP_QINQ_ERR			0x7
+#define LANCER_TX_COMP_PARITY_ERR		0xb
+#define LANCER_TX_COMP_DMA_ERR			0xd
+
 /* TX Compl Queue Descriptor */
 
 /* Pseudo amap definition for eth_tx_compl in which each bit of the
diff --git a/drivers/net/ethernet/emulex/benet/be_main.c b/drivers/net/ethernet/emulex/benet/be_main.c
index 93ff8ef39352..9a18e7930b31 100644
--- a/drivers/net/ethernet/emulex/benet/be_main.c
+++ b/drivers/net/ethernet/emulex/benet/be_main.c
@@ -86,6 +86,7 @@ static const char * const ue_status_low_desc[] = {
 	"JTAG ",
 	"MPU_INTPEND "
 };
+
 /* UE Status High CSR */
 static const char * const ue_status_hi_desc[] = {
 	"LPCMEMHOST",
@@ -122,10 +123,10 @@ static const char * const ue_status_hi_desc[] = {
 	"Unknown"
 };
 
-
 static void be_queue_free(struct be_adapter *adapter, struct be_queue_info *q)
 {
 	struct be_dma_mem *mem = &q->dma_mem;
+
 	if (mem->va) {
 		dma_free_coherent(&adapter->pdev->dev, mem->size, mem->va,
 				  mem->dma);
@@ -187,6 +188,7 @@ static void be_intr_set(struct be_adapter *adapter, bool enable)
 static void be_rxq_notify(struct be_adapter *adapter, u16 qid, u16 posted)
 {
 	u32 val = 0;
+
 	val |= qid & DB_RQ_RING_ID_MASK;
 	val |= posted << DB_RQ_NUM_POSTED_SHIFT;
 
@@ -198,6 +200,7 @@ static void be_txq_notify(struct be_adapter *adapter, struct be_tx_obj *txo,
 			  u16 posted)
 {
 	u32 val = 0;
+
 	val |= txo->q.id & DB_TXULP_RING_ID_MASK;
 	val |= (posted & DB_TXULP_NUM_POSTED_MASK) << DB_TXULP_NUM_POSTED_SHIFT;
 
@@ -209,6 +212,7 @@ static void be_eq_notify(struct be_adapter *adapter, u16 qid,
 			 bool arm, bool clear_int, u16 num_popped)
 {
 	u32 val = 0;
+
 	val |= qid & DB_EQ_RING_ID_MASK;
 	val |= ((qid & DB_EQ_RING_ID_EXT_MASK) << DB_EQ_RING_ID_EXT_MASK_SHIFT);
 
@@ -227,6 +231,7 @@ static void be_eq_notify(struct be_adapter *adapter, u16 qid,
 void be_cq_notify(struct be_adapter *adapter, u16 qid, bool arm, u16 num_popped)
 {
 	u32 val = 0;
+
 	val |= qid & DB_CQ_RING_ID_MASK;
 	val |= ((qid & DB_CQ_RING_ID_EXT_MASK) <<
 			DB_CQ_RING_ID_EXT_MASK_SHIFT);
@@ -488,7 +493,6 @@ static void populate_be_v2_stats(struct be_adapter *adapter)
 
 static void populate_lancer_stats(struct be_adapter *adapter)
 {
-
 	struct be_drv_stats *drvs = &adapter->drv_stats;
 	struct lancer_pport_stats *pport_stats = pport_stats_from_cmd(adapter);
 
@@ -588,6 +592,7 @@ static struct rtnl_link_stats64 *be_get_stats64(struct net_device *netdev,
 
 	for_all_rx_queues(adapter, rxo, i) {
 		const struct be_rx_stats *rx_stats = rx_stats(rxo);
+
 		do {
 			start = u64_stats_fetch_begin_irq(&rx_stats->sync);
 			pkts = rx_stats(rxo)->rx_pkts;
@@ -602,6 +607,7 @@ static struct rtnl_link_stats64 *be_get_stats64(struct net_device *netdev,
 
 	for_all_tx_queues(adapter, txo, i) {
 		const struct be_tx_stats *tx_stats = tx_stats(txo);
+
 		do {
 			start = u64_stats_fetch_begin_irq(&tx_stats->sync);
 			pkts = tx_stats(txo)->tx_pkts;
@@ -738,38 +744,37 @@ static void wrb_fill_hdr(struct be_adapter *adapter, struct be_eth_hdr_wrb *hdr,
 
 	memset(hdr, 0, sizeof(*hdr));
 
-	AMAP_SET_BITS(struct amap_eth_hdr_wrb, crc, hdr, 1);
+	SET_TX_WRB_HDR_BITS(crc, hdr, 1);
 
 	if (skb_is_gso(skb)) {
-		AMAP_SET_BITS(struct amap_eth_hdr_wrb, lso, hdr, 1);
-		AMAP_SET_BITS(struct amap_eth_hdr_wrb, lso_mss,
-			hdr, skb_shinfo(skb)->gso_size);
+		SET_TX_WRB_HDR_BITS(lso, hdr, 1);
+		SET_TX_WRB_HDR_BITS(lso_mss, hdr, skb_shinfo(skb)->gso_size);
 		if (skb_is_gso_v6(skb) && !lancer_chip(adapter))
-			AMAP_SET_BITS(struct amap_eth_hdr_wrb, lso6, hdr, 1);
+			SET_TX_WRB_HDR_BITS(lso6, hdr, 1);
 	} else if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		if (skb->encapsulation) {
-			AMAP_SET_BITS(struct amap_eth_hdr_wrb, ipcs, hdr, 1);
+			SET_TX_WRB_HDR_BITS(ipcs, hdr, 1);
 			proto = skb_inner_ip_proto(skb);
 		} else {
 			proto = skb_ip_proto(skb);
 		}
 		if (proto == IPPROTO_TCP)
-			AMAP_SET_BITS(struct amap_eth_hdr_wrb, tcpcs, hdr, 1);
+			SET_TX_WRB_HDR_BITS(tcpcs, hdr, 1);
 		else if (proto == IPPROTO_UDP)
-			AMAP_SET_BITS(struct amap_eth_hdr_wrb, udpcs, hdr, 1);
+			SET_TX_WRB_HDR_BITS(udpcs, hdr, 1);
 	}
 
 	if (vlan_tx_tag_present(skb)) {
-		AMAP_SET_BITS(struct amap_eth_hdr_wrb, vlan, hdr, 1);
+		SET_TX_WRB_HDR_BITS(vlan, hdr, 1);
 		vlan_tag = be_get_tx_vlan_tag(adapter, skb);
-		AMAP_SET_BITS(struct amap_eth_hdr_wrb, vlan_tag, hdr, vlan_tag);
+		SET_TX_WRB_HDR_BITS(vlan_tag, hdr, vlan_tag);
 	}
 
 	/* To skip HW VLAN tagging: evt = 1, compl = 0 */
-	AMAP_SET_BITS(struct amap_eth_hdr_wrb, complete, hdr, !skip_hw_vlan);
-	AMAP_SET_BITS(struct amap_eth_hdr_wrb, event, hdr, 1);
-	AMAP_SET_BITS(struct amap_eth_hdr_wrb, num_wrb, hdr, wrb_cnt);
-	AMAP_SET_BITS(struct amap_eth_hdr_wrb, len, hdr, len);
+	SET_TX_WRB_HDR_BITS(complete, hdr, !skip_hw_vlan);
+	SET_TX_WRB_HDR_BITS(event, hdr, 1);
+	SET_TX_WRB_HDR_BITS(num_wrb, hdr, wrb_cnt);
+	SET_TX_WRB_HDR_BITS(len, hdr, len);
 }
 
 static void unmap_tx_frag(struct device *dev, struct be_eth_wrb *wrb,
@@ -808,6 +813,7 @@ static int make_tx_wrbs(struct be_adapter *adapter, struct be_queue_info *txq,
 
 	if (skb->len > skb->data_len) {
 		int len = skb_headlen(skb);
+
 		busaddr = dma_map_single(dev, skb->data, len, DMA_TO_DEVICE);
 		if (dma_mapping_error(dev, busaddr))
 			goto dma_err;
@@ -821,6 +827,7 @@ static int make_tx_wrbs(struct be_adapter *adapter, struct be_queue_info *txq,
 
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 		const struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[i];
+
 		busaddr = skb_frag_dma_map(dev, frag, 0,
 					   skb_frag_size(frag), DMA_TO_DEVICE);
 		if (dma_mapping_error(dev, busaddr))
@@ -850,6 +857,7 @@ dma_err:
 		unmap_tx_frag(dev, wrb, map_single);
 		map_single = false;
 		copied -= wrb->frag_len;
+		adapter->drv_stats.dma_map_errors++;
 		queue_head_inc(txq);
 	}
 	return 0;
@@ -910,7 +918,7 @@ static bool be_ipv6_exthdr_check(struct sk_buff *skb)
 		if (ip6h->nexthdr != NEXTHDR_TCP &&
 		    ip6h->nexthdr != NEXTHDR_UDP) {
 			struct ipv6_opt_hdr *ehdr =
-				(struct ipv6_opt_hdr *) (skb->data + offset);
+				(struct ipv6_opt_hdr *)(skb->data + offset);
 
 			/* offending pkt: 2nd byte following IPv6 hdr is 0xff */
 			if (ehdr->hdrlen == 0xff)
@@ -974,8 +982,8 @@ static struct sk_buff *be_lancer_xmit_workarounds(struct be_adapter *adapter,
 	 * skip HW tagging is not enabled by FW.
 	 */
 	if (unlikely(be_ipv6_tx_stall_chk(adapter, skb) &&
-	    (adapter->pvid || adapter->qnq_vid) &&
-	    !qnq_async_evt_rcvd(adapter)))
+		     (adapter->pvid || adapter->qnq_vid) &&
+		     !qnq_async_evt_rcvd(adapter)))
 		goto tx_drop;
 
 	/* Manual VLAN tag insertion to prevent:
@@ -1073,15 +1081,15 @@ static netdev_tx_t be_xmit(struct sk_buff *skb, struct net_device *netdev)
 static int be_change_mtu(struct net_device *netdev, int new_mtu)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
-	if (new_mtu < BE_MIN_MTU ||
-	    new_mtu > (BE_MAX_JUMBO_FRAME_SIZE - (ETH_HLEN + ETH_FCS_LEN))) {
-		dev_info(&adapter->pdev->dev,
-			 "MTU must be between %d and %d bytes\n",
-			 BE_MIN_MTU,
-			 (BE_MAX_JUMBO_FRAME_SIZE - (ETH_HLEN + ETH_FCS_LEN)));
+	struct device *dev = &adapter->pdev->dev;
+
+	if (new_mtu < BE_MIN_MTU || new_mtu > BE_MAX_MTU) {
+		dev_info(dev, "MTU must be between %d and %d bytes\n",
+			 BE_MIN_MTU, BE_MAX_MTU);
 		return -EINVAL;
 	}
-	dev_info(&adapter->pdev->dev, "MTU changed from %d to %d bytes\n",
+
+	dev_info(dev, "MTU changed from %d to %d bytes\n",
 		 netdev->mtu, new_mtu);
 	netdev->mtu = new_mtu;
 	return 0;
@@ -1093,6 +1101,7 @@ static int be_change_mtu(struct net_device *netdev, int new_mtu)
  */
 static int be_vid_config(struct be_adapter *adapter)
 {
+	struct device *dev = &adapter->pdev->dev;
 	u16 vids[BE_NUM_VLANS_SUPPORTED];
 	u16 num = 0, i = 0;
 	int status = 0;
@@ -1114,16 +1123,15 @@ static int be_vid_config(struct be_adapter *adapter)
 		if (addl_status(status) ==
 				MCC_ADDL_STATUS_INSUFFICIENT_RESOURCES)
 			goto set_vlan_promisc;
-		dev_err(&adapter->pdev->dev,
-			"Setting HW VLAN filtering failed.\n");
+		dev_err(dev, "Setting HW VLAN filtering failed\n");
 	} else {
 		if (adapter->flags & BE_FLAGS_VLAN_PROMISC) {
 			/* hw VLAN filtering re-enabled. */
 			status = be_cmd_rx_filter(adapter,
 						  BE_FLAGS_VLAN_PROMISC, OFF);
 			if (!status) {
-				dev_info(&adapter->pdev->dev,
-					 "Disabling VLAN Promiscuous mode.\n");
+				dev_info(dev,
+					 "Disabling VLAN Promiscuous mode\n");
 				adapter->flags &= ~BE_FLAGS_VLAN_PROMISC;
 			}
 		}
@@ -1137,11 +1145,10 @@ set_vlan_promisc:
 
 	status = be_cmd_rx_filter(adapter, BE_FLAGS_VLAN_PROMISC, ON);
 	if (!status) {
-		dev_info(&adapter->pdev->dev, "Enable VLAN Promiscuous mode\n");
+		dev_info(dev, "Enable VLAN Promiscuous mode\n");
 		adapter->flags |= BE_FLAGS_VLAN_PROMISC;
 	} else
-		dev_err(&adapter->pdev->dev,
-			"Failed to enable VLAN Promiscuous mode.\n");
+		dev_err(dev, "Failed to enable VLAN Promiscuous mode\n");
 	return status;
 }
 
@@ -1417,6 +1424,7 @@ err:
 		max_tx_rate, vf);
 	return be_cmd_status(status);
 }
+
 static int be_set_vf_link_state(struct net_device *netdev, int vf,
 				int link_state)
 {
@@ -1482,7 +1490,6 @@ static void be_eqd_update(struct be_adapter *adapter)
 			tx_pkts = txo->stats.tx_reqs;
 		} while (u64_stats_fetch_retry_irq(&txo->stats.sync, start));
 
-
 		/* Skip, if wrapped around or first calculation */
 		now = jiffies;
 		if (!aic->jiffies || time_before(now, aic->jiffies) ||
@@ -1683,7 +1690,7 @@ static void be_rx_compl_process(struct be_rx_obj *rxo, struct napi_struct *napi,
 	if (netdev->features & NETIF_F_RXHASH)
 		skb_set_hash(skb, rxcp->rss_hash, PKT_HASH_TYPE_L3);
 
-	skb->encapsulation = rxcp->tunneled;
+	skb->csum_level = rxcp->tunneled;
 	skb_mark_napi_id(skb, napi);
 
 	if (rxcp->vlanf)
@@ -1741,7 +1748,7 @@ static void be_rx_compl_process_gro(struct be_rx_obj *rxo,
 	if (adapter->netdev->features & NETIF_F_RXHASH)
 		skb_set_hash(skb, rxcp->rss_hash, PKT_HASH_TYPE_L3);
 
-	skb->encapsulation = rxcp->tunneled;
+	skb->csum_level = rxcp->tunneled;
 	skb_mark_napi_id(skb, napi);
 
 	if (rxcp->vlanf)
@@ -1753,65 +1760,46 @@ static void be_rx_compl_process_gro(struct be_rx_obj *rxo,
 static void be_parse_rx_compl_v1(struct be_eth_rx_compl *compl,
 				 struct be_rx_compl_info *rxcp)
 {
-	rxcp->pkt_size =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, pktsize, compl);
-	rxcp->vlanf = AMAP_GET_BITS(struct amap_eth_rx_compl_v1, vtp, compl);
-	rxcp->err = AMAP_GET_BITS(struct amap_eth_rx_compl_v1, err, compl);
-	rxcp->tcpf = AMAP_GET_BITS(struct amap_eth_rx_compl_v1, tcpf, compl);
-	rxcp->udpf = AMAP_GET_BITS(struct amap_eth_rx_compl_v1, udpf, compl);
-	rxcp->ip_csum =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, ipcksm, compl);
-	rxcp->l4_csum =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, l4_cksm, compl);
-	rxcp->ipv6 =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, ip_version, compl);
-	rxcp->num_rcvd =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, numfrags, compl);
-	rxcp->pkt_type =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, cast_enc, compl);
-	rxcp->rss_hash =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, rsshash, compl);
+	rxcp->pkt_size = GET_RX_COMPL_V1_BITS(pktsize, compl);
+	rxcp->vlanf = GET_RX_COMPL_V1_BITS(vtp, compl);
+	rxcp->err = GET_RX_COMPL_V1_BITS(err, compl);
+	rxcp->tcpf = GET_RX_COMPL_V1_BITS(tcpf, compl);
+	rxcp->udpf = GET_RX_COMPL_V1_BITS(udpf, compl);
+	rxcp->ip_csum = GET_RX_COMPL_V1_BITS(ipcksm, compl);
+	rxcp->l4_csum = GET_RX_COMPL_V1_BITS(l4_cksm, compl);
+	rxcp->ipv6 = GET_RX_COMPL_V1_BITS(ip_version, compl);
+	rxcp->num_rcvd = GET_RX_COMPL_V1_BITS(numfrags, compl);
+	rxcp->pkt_type = GET_RX_COMPL_V1_BITS(cast_enc, compl);
+	rxcp->rss_hash = GET_RX_COMPL_V1_BITS(rsshash, compl);
 	if (rxcp->vlanf) {
-		rxcp->qnq = AMAP_GET_BITS(struct amap_eth_rx_compl_v1, qnq,
-					  compl);
-		rxcp->vlan_tag = AMAP_GET_BITS(struct amap_eth_rx_compl_v1,
-					       vlan_tag, compl);
+		rxcp->qnq = GET_RX_COMPL_V1_BITS(qnq, compl);
+		rxcp->vlan_tag = GET_RX_COMPL_V1_BITS(vlan_tag, compl);
 	}
-	rxcp->port = AMAP_GET_BITS(struct amap_eth_rx_compl_v1, port, compl);
+	rxcp->port = GET_RX_COMPL_V1_BITS(port, compl);
 	rxcp->tunneled =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v1, tunneled, compl);
+		GET_RX_COMPL_V1_BITS(tunneled, compl);
 }
 
 static void be_parse_rx_compl_v0(struct be_eth_rx_compl *compl,
 				 struct be_rx_compl_info *rxcp)
 {
-	rxcp->pkt_size =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, pktsize, compl);
-	rxcp->vlanf = AMAP_GET_BITS(struct amap_eth_rx_compl_v0, vtp, compl);
-	rxcp->err = AMAP_GET_BITS(struct amap_eth_rx_compl_v0, err, compl);
-	rxcp->tcpf = AMAP_GET_BITS(struct amap_eth_rx_compl_v0, tcpf, compl);
-	rxcp->udpf = AMAP_GET_BITS(struct amap_eth_rx_compl_v0, udpf, compl);
-	rxcp->ip_csum =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, ipcksm, compl);
-	rxcp->l4_csum =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, l4_cksm, compl);
-	rxcp->ipv6 =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, ip_version, compl);
-	rxcp->num_rcvd =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, numfrags, compl);
-	rxcp->pkt_type =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, cast_enc, compl);
-	rxcp->rss_hash =
-		AMAP_GET_BITS(struct amap_eth_rx_compl_v0, rsshash, compl);
+	rxcp->pkt_size = GET_RX_COMPL_V0_BITS(pktsize, compl);
+	rxcp->vlanf = GET_RX_COMPL_V0_BITS(vtp, compl);
+	rxcp->err = GET_RX_COMPL_V0_BITS(err, compl);
+	rxcp->tcpf = GET_RX_COMPL_V0_BITS(tcpf, compl);
+	rxcp->udpf = GET_RX_COMPL_V0_BITS(udpf, compl);
+	rxcp->ip_csum = GET_RX_COMPL_V0_BITS(ipcksm, compl);
+	rxcp->l4_csum = GET_RX_COMPL_V0_BITS(l4_cksm, compl);
+	rxcp->ipv6 = GET_RX_COMPL_V0_BITS(ip_version, compl);
+	rxcp->num_rcvd = GET_RX_COMPL_V0_BITS(numfrags, compl);
+	rxcp->pkt_type = GET_RX_COMPL_V0_BITS(cast_enc, compl);
+	rxcp->rss_hash = GET_RX_COMPL_V0_BITS(rsshash, compl);
 	if (rxcp->vlanf) {
-		rxcp->qnq = AMAP_GET_BITS(struct amap_eth_rx_compl_v0, qnq,
-					  compl);
-		rxcp->vlan_tag = AMAP_GET_BITS(struct amap_eth_rx_compl_v0,
-					       vlan_tag, compl);
+		rxcp->qnq = GET_RX_COMPL_V0_BITS(qnq, compl);
+		rxcp->vlan_tag = GET_RX_COMPL_V0_BITS(vlan_tag, compl);
 	}
-	rxcp->port = AMAP_GET_BITS(struct amap_eth_rx_compl_v0, port, compl);
-	rxcp->ip_frag = AMAP_GET_BITS(struct amap_eth_rx_compl_v0,
-				      ip_frag, compl);
+	rxcp->port = GET_RX_COMPL_V0_BITS(port, compl);
+	rxcp->ip_frag = GET_RX_COMPL_V0_BITS(ip_frag, compl);
 }
 
 static struct be_rx_compl_info *be_rx_compl_get(struct be_rx_obj *rxo)
@@ -1872,7 +1860,7 @@ static inline struct page *be_alloc_pages(u32 size, gfp_t gfp)
  * Allocate a page, split it to fragments of size rx_frag_size and post as
  * receive buffers to BE
  */
-static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp)
+static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp, u32 frags_needed)
 {
 	struct be_adapter *adapter = rxo->adapter;
 	struct be_rx_page_info *page_info = NULL, *prev_page_info = NULL;
@@ -1881,10 +1869,10 @@ static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp)
 	struct device *dev = &adapter->pdev->dev;
 	struct be_eth_rx_d *rxd;
 	u64 page_dmaaddr = 0, frag_dmaaddr;
-	u32 posted, page_offset = 0;
+	u32 posted, page_offset = 0, notify = 0;
 
 	page_info = &rxo->page_info_tbl[rxq->head];
-	for (posted = 0; posted < MAX_RX_POST && !page_info->page; posted++) {
+	for (posted = 0; posted < frags_needed && !page_info->page; posted++) {
 		if (!pagep) {
 			pagep = be_alloc_pages(adapter->big_page_size, gfp);
 			if (unlikely(!pagep)) {
@@ -1897,7 +1885,7 @@ static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp)
 			if (dma_mapping_error(dev, page_dmaaddr)) {
 				put_page(pagep);
 				pagep = NULL;
-				rx_stats(rxo)->rx_post_fail++;
+				adapter->drv_stats.dma_map_errors++;
 				break;
 			}
 			page_offset = 0;
@@ -1940,7 +1928,11 @@ static void be_post_rx_frags(struct be_rx_obj *rxo, gfp_t gfp)
 		atomic_add(posted, &rxq->used);
 		if (rxo->rx_post_starved)
 			rxo->rx_post_starved = false;
-		be_rxq_notify(adapter, rxq->id, posted);
+		do {
+			notify = min(256u, posted);
+			be_rxq_notify(adapter, rxq->id, notify);
+			posted -= notify;
+		} while (posted);
 	} else if (atomic_read(&rxq->used) == 0) {
 		/* Let be_worker replenish when memory is available */
 		rxo->rx_post_starved = true;
@@ -1991,7 +1983,7 @@ static u16 be_tx_compl_process(struct be_adapter *adapter,
 		queue_tail_inc(txq);
 	} while (cur_index != last_index);
 
-	dev_kfree_skb_any(sent_skb);
+	dev_consume_skb_any(sent_skb);
 	return num_wrbs;
 }
 
@@ -2069,7 +2061,8 @@ static void be_rx_cq_clean(struct be_rx_obj *rxo)
 		memset(page_info, 0, sizeof(*page_info));
 	}
 	BUG_ON(atomic_read(&rxq->used));
-	rxq->tail = rxq->head = 0;
+	rxq->tail = 0;
+	rxq->head = 0;
 }
 
 static void be_tx_compl_clean(struct be_adapter *adapter)
@@ -2091,9 +2084,7 @@ static void be_tx_compl_clean(struct be_adapter *adapter)
 			num_wrbs = 0;
 			txq = &txo->q;
 			while ((txcp = be_tx_compl_get(&txo->cq))) {
-				end_idx =
-					AMAP_GET_BITS(struct amap_eth_tx_compl,
-						      wrb_index, txcp);
+				end_idx = GET_TX_COMPL_BITS(wrb_index, txcp);
 				num_wrbs += be_tx_compl_process(adapter, txo,
 								end_idx);
 				cmpl++;
@@ -2164,7 +2155,6 @@ static int be_evt_queues_create(struct be_adapter *adapter)
 		napi_hash_add(&eqo->napi);
 		aic = &adapter->aic_obj[i];
 		eqo->adapter = adapter;
-		eqo->tx_budget = BE_TX_BUDGET;
 		eqo->idx = i;
 		aic->max_eqd = BE_MAX_EQD;
 		aic->enable = true;
@@ -2394,6 +2384,7 @@ static int be_process_rx(struct be_rx_obj *rxo, struct napi_struct *napi,
 	struct be_queue_info *rx_cq = &rxo->cq;
 	struct be_rx_compl_info *rxcp;
 	u32 work_done;
+	u32 frags_consumed = 0;
 
 	for (work_done = 0; work_done < budget; work_done++) {
 		rxcp = be_rx_compl_get(rxo);
@@ -2426,6 +2417,7 @@ static int be_process_rx(struct be_rx_obj *rxo, struct napi_struct *napi,
 			be_rx_compl_process(rxo, napi, rxcp);
 
 loop_continue:
+		frags_consumed += rxcp->num_rcvd;
 		be_rx_stats_update(rxo, rxcp);
 	}
 
@@ -2437,26 +2429,71 @@ loop_continue:
 		 */
 		if (atomic_read(&rxo->q.used) < RX_FRAGS_REFILL_WM &&
 		    !rxo->rx_post_starved)
-			be_post_rx_frags(rxo, GFP_ATOMIC);
+			be_post_rx_frags(rxo, GFP_ATOMIC,
+					 max_t(u32, MAX_RX_POST,
+					       frags_consumed));
 	}
 
 	return work_done;
 }
 
-static bool be_process_tx(struct be_adapter *adapter, struct be_tx_obj *txo,
-			  int budget, int idx)
+static inline void be_update_tx_err(struct be_tx_obj *txo, u32 status)
+{
+	switch (status) {
+	case BE_TX_COMP_HDR_PARSE_ERR:
+		tx_stats(txo)->tx_hdr_parse_err++;
+		break;
+	case BE_TX_COMP_NDMA_ERR:
+		tx_stats(txo)->tx_dma_err++;
+		break;
+	case BE_TX_COMP_ACL_ERR:
+		tx_stats(txo)->tx_spoof_check_err++;
+		break;
+	}
+}
+
+static inline void lancer_update_tx_err(struct be_tx_obj *txo, u32 status)
+{
+	switch (status) {
+	case LANCER_TX_COMP_LSO_ERR:
+		tx_stats(txo)->tx_tso_err++;
+		break;
+	case LANCER_TX_COMP_HSW_DROP_MAC_ERR:
+	case LANCER_TX_COMP_HSW_DROP_VLAN_ERR:
+		tx_stats(txo)->tx_spoof_check_err++;
+		break;
+	case LANCER_TX_COMP_QINQ_ERR:
+		tx_stats(txo)->tx_qinq_err++;
+		break;
+	case LANCER_TX_COMP_PARITY_ERR:
+		tx_stats(txo)->tx_internal_parity_err++;
+		break;
+	case LANCER_TX_COMP_DMA_ERR:
+		tx_stats(txo)->tx_dma_err++;
+		break;
+	}
+}
+
+static void be_process_tx(struct be_adapter *adapter, struct be_tx_obj *txo,
+			  int idx)
 {
 	struct be_eth_tx_compl *txcp;
-	int num_wrbs = 0, work_done;
+	int num_wrbs = 0, work_done = 0;
+	u32 compl_status;
+	u16 last_idx;
 
-	for (work_done = 0; work_done < budget; work_done++) {
-		txcp = be_tx_compl_get(&txo->cq);
-		if (!txcp)
-			break;
-		num_wrbs += be_tx_compl_process(adapter, txo,
-						AMAP_GET_BITS(struct
-							      amap_eth_tx_compl,
-							      wrb_index, txcp));
+	while ((txcp = be_tx_compl_get(&txo->cq))) {
+		last_idx = GET_TX_COMPL_BITS(wrb_index, txcp);
+		num_wrbs += be_tx_compl_process(adapter, txo, last_idx);
+		work_done++;
+
+		compl_status = GET_TX_COMPL_BITS(status, txcp);
+		if (compl_status) {
+			if (lancer_chip(adapter))
+				lancer_update_tx_err(txo, compl_status);
+			else
+				be_update_tx_err(txo, compl_status);
+		}
 	}
 
 	if (work_done) {
@@ -2474,7 +2511,6 @@ static bool be_process_tx(struct be_adapter *adapter, struct be_tx_obj *txo,
 		tx_stats(txo)->tx_compl += work_done;
 		u64_stats_update_end(&tx_stats(txo)->sync_compl);
 	}
-	return (work_done < budget); /* Done */
 }
 
 int be_poll(struct napi_struct *napi, int budget)
@@ -2483,17 +2519,12 @@ int be_poll(struct napi_struct *napi, int budget)
 	struct be_adapter *adapter = eqo->adapter;
 	int max_work = 0, work, i, num_evts;
 	struct be_rx_obj *rxo;
-	bool tx_done;
+	struct be_tx_obj *txo;
 
 	num_evts = events_get(eqo);
 
-	/* Process all TXQs serviced by this EQ */
-	for (i = eqo->idx; i < adapter->num_tx_qs; i += adapter->num_evt_qs) {
-		tx_done = be_process_tx(adapter, &adapter->tx_obj[i],
-					eqo->tx_budget, i);
-		if (!tx_done)
-			max_work = budget;
-	}
+	for_all_tx_queues_on_eq(adapter, eqo, txo, i)
+		be_process_tx(adapter, txo, i);
 
 	if (be_lock_napi(eqo)) {
 		/* This loop will iterate twice for EQ0 in which
@@ -2882,7 +2913,7 @@ static int be_rx_qs_create(struct be_adapter *adapter)
 
 	/* First time posting */
 	for_all_rx_queues(adapter, rxo, i)
-		be_post_rx_frags(rxo, GFP_KERNEL);
+		be_post_rx_frags(rxo, GFP_KERNEL, MAX_RX_POST);
 	return 0;
 }
 
@@ -3309,10 +3340,20 @@ static void BEx_get_resources(struct be_adapter *adapter,
 	 */
 	if (BE2_chip(adapter) || use_sriov ||  (adapter->port_num > 1) ||
 	    !be_physfn(adapter) || (be_is_mc(adapter) &&
-	    !(adapter->function_caps & BE_FUNCTION_CAPS_RSS)))
+	    !(adapter->function_caps & BE_FUNCTION_CAPS_RSS))) {
 		res->max_tx_qs = 1;
-	else
+	} else if (adapter->function_caps & BE_FUNCTION_CAPS_SUPER_NIC) {
+		struct be_resources super_nic_res = {0};
+
+		/* On a SuperNIC profile, the driver needs to use the
+		 * GET_PROFILE_CONFIG cmd to query the per-function TXQ limits
+		 */
+		be_cmd_get_profile_config(adapter, &super_nic_res, 0);
+		/* Some old versions of BE3 FW don't report max_tx_qs value */
+		res->max_tx_qs = super_nic_res.max_tx_qs ? : BE3_MAX_TX_QS;
+	} else {
 		res->max_tx_qs = BE3_MAX_TX_QS;
+	}
 
 	if ((adapter->function_caps & BE_FUNCTION_CAPS_RSS) &&
 	    !use_sriov && be_physfn(adapter))
@@ -3362,7 +3403,7 @@ static int be_get_sriov_config(struct be_adapter *adapter)
 
 	if (!be_max_vfs(adapter)) {
 		if (num_vfs)
-			dev_warn(dev, "device doesn't support SRIOV\n");
+			dev_warn(dev, "SRIOV is disabled. Ignoring num_vfs\n");
 		adapter->num_vfs = 0;
 		return 0;
 	}
@@ -3413,16 +3454,16 @@ static int be_get_resources(struct be_adapter *adapter)
 		if (be_roce_supported(adapter))
 			res.max_evt_qs /= 2;
 		adapter->res = res;
-
-		dev_info(dev, "Max: txqs %d, rxqs %d, rss %d, eqs %d, vfs %d\n",
-			 be_max_txqs(adapter), be_max_rxqs(adapter),
-			 be_max_rss(adapter), be_max_eqs(adapter),
-			 be_max_vfs(adapter));
-		dev_info(dev, "Max: uc-macs %d, mc-macs %d, vlans %d\n",
-			 be_max_uc(adapter), be_max_mc(adapter),
-			 be_max_vlans(adapter));
 	}
 
+	dev_info(dev, "Max: txqs %d, rxqs %d, rss %d, eqs %d, vfs %d\n",
+		 be_max_txqs(adapter), be_max_rxqs(adapter),
+		 be_max_rss(adapter), be_max_eqs(adapter),
+		 be_max_vfs(adapter));
+	dev_info(dev, "Max: uc-macs %d, mc-macs %d, vlans %d\n",
+		 be_max_uc(adapter), be_max_mc(adapter),
+		 be_max_vlans(adapter));
+
 	return 0;
 }
 
@@ -3633,9 +3674,10 @@ static int be_setup(struct be_adapter *adapter)
 		goto err;
 
 	be_cmd_get_fw_ver(adapter);
+	dev_info(dev, "FW version is %s\n", adapter->fw_ver);
 
 	if (BE2_chip(adapter) && fw_major_num(adapter->fw_ver) < 4) {
-		dev_err(dev, "Firmware on card is old(%s), IRQs may not work.",
+		dev_err(dev, "Firmware on card is old(%s), IRQs may not work",
 			adapter->fw_ver);
 		dev_err(dev, "Please upgrade firmware to version >= 4.0\n");
 	}
@@ -3683,8 +3725,6 @@ static void be_netpoll(struct net_device *netdev)
 		be_eq_notify(eqo->adapter, eqo->q.id, false, true, 0);
 		napi_schedule(&eqo->napi);
 	}
-
-	return;
 }
 #endif
 
@@ -4052,6 +4092,7 @@ static int lancer_fw_download(struct be_adapter *adapter,
 {
 #define LANCER_FW_DOWNLOAD_CHUNK      (32 * 1024)
 #define LANCER_FW_DOWNLOAD_LOCATION   "/prg"
+	struct device *dev = &adapter->pdev->dev;
 	struct be_dma_mem flash_cmd;
 	const u8 *data_ptr = NULL;
 	u8 *dest_image_ptr = NULL;
@@ -4064,21 +4105,16 @@ static int lancer_fw_download(struct be_adapter *adapter,
 	u8 change_status;
 
 	if (!IS_ALIGNED(fw->size, sizeof(u32))) {
-		dev_err(&adapter->pdev->dev,
-			"FW Image not properly aligned. "
-			"Length must be 4 byte aligned.\n");
-		status = -EINVAL;
-		goto lancer_fw_exit;
+		dev_err(dev, "FW image size should be multiple of 4\n");
+		return -EINVAL;
 	}
 
 	flash_cmd.size = sizeof(struct lancer_cmd_req_write_object)
 				+ LANCER_FW_DOWNLOAD_CHUNK;
-	flash_cmd.va = dma_alloc_coherent(&adapter->pdev->dev, flash_cmd.size,
+	flash_cmd.va = dma_alloc_coherent(dev, flash_cmd.size,
 					  &flash_cmd.dma, GFP_KERNEL);
-	if (!flash_cmd.va) {
-		status = -ENOMEM;
-		goto lancer_fw_exit;
-	}
+	if (!flash_cmd.va)
+		return -ENOMEM;
 
 	dest_image_ptr = flash_cmd.va +
 				sizeof(struct lancer_cmd_req_write_object);
@@ -4113,35 +4149,27 @@ static int lancer_fw_download(struct be_adapter *adapter,
 						 &add_status);
 	}
 
-	dma_free_coherent(&adapter->pdev->dev, flash_cmd.size, flash_cmd.va,
-			  flash_cmd.dma);
+	dma_free_coherent(dev, flash_cmd.size, flash_cmd.va, flash_cmd.dma);
 	if (status) {
-		dev_err(&adapter->pdev->dev,
-			"Firmware load error. "
-			"Status code: 0x%x Additional Status: 0x%x\n",
-			status, add_status);
-		goto lancer_fw_exit;
+		dev_err(dev, "Firmware load error\n");
+		return be_cmd_status(status);
 	}
 
+	dev_info(dev, "Firmware flashed successfully\n");
+
 	if (change_status == LANCER_FW_RESET_NEEDED) {
-		dev_info(&adapter->pdev->dev,
-			 "Resetting adapter to activate new FW\n");
+		dev_info(dev, "Resetting adapter to activate new FW\n");
 		status = lancer_physdev_ctrl(adapter,
 					     PHYSDEV_CONTROL_FW_RESET_MASK);
 		if (status) {
-			dev_err(&adapter->pdev->dev,
-				"Adapter busy for FW reset.\n"
-				"New FW will not be active.\n");
-			goto lancer_fw_exit;
+			dev_err(dev, "Adapter busy, could not reset FW\n");
+			dev_err(dev, "Reboot server to activate new FW\n");
 		}
 	} else if (change_status != LANCER_NO_RESET_NEEDED) {
-		dev_err(&adapter->pdev->dev,
-			"System reboot required for new FW to be active\n");
+		dev_info(dev, "Reboot server to activate new FW\n");
 	}
 
-	dev_info(&adapter->pdev->dev, "Firmware flashed successfully\n");
-lancer_fw_exit:
-	return status;
+	return 0;
 }
 
 #define UFI_TYPE2		2
@@ -4374,7 +4402,6 @@ static void be_add_vxlan_port(struct net_device *netdev, sa_family_t sa_family,
 	return;
 err:
 	be_disable_vxlan_offloads(adapter);
-	return;
 }
 
 static void be_del_vxlan_port(struct net_device *netdev, sa_family_t sa_family,
@@ -4506,6 +4533,7 @@ static int be_map_pci_bars(struct be_adapter *adapter)
 	return 0;
 
 pci_map_err:
+	dev_err(&adapter->pdev->dev, "Error in mapping PCI BARs\n");
 	be_unmap_pci_bars(adapter);
 	return -ENOMEM;
 }
@@ -4713,7 +4741,6 @@ static void be_func_recovery_task(struct work_struct *work)
 	be_detect_error(adapter);
 
 	if (adapter->hw_error && lancer_chip(adapter)) {
-
 		rtnl_lock();
 		netif_device_detach(adapter->netdev);
 		rtnl_unlock();
@@ -4750,7 +4777,7 @@ static void be_worker(struct work_struct *work)
 	if (!adapter->stats_cmd_sent) {
 		if (lancer_chip(adapter))
 			lancer_cmd_get_pport_stats(adapter,
-						&adapter->stats_cmd);
+						   &adapter->stats_cmd);
 		else
 			be_cmd_get_stats(adapter, &adapter->stats_cmd);
 	}
@@ -4764,7 +4791,7 @@ static void be_worker(struct work_struct *work)
 		 * allocation failures.
 		 */
 		if (rxo->rx_post_starved)
-			be_post_rx_frags(rxo, GFP_KERNEL);
+			be_post_rx_frags(rxo, GFP_KERNEL, MAX_RX_POST);
 	}
 
 	be_eqd_update(adapter);
@@ -4822,6 +4849,8 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 	struct net_device *netdev;
 	char port_name;
 
+	dev_info(&pdev->dev, "%s version is %s\n", DRV_NAME, DRV_VER);
+
 	status = pci_enable_device(pdev);
 	if (status)
 		goto do_none;
@@ -4853,11 +4882,9 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 		}
 	}
 
-	if (be_physfn(adapter)) {
-		status = pci_enable_pcie_error_reporting(pdev);
-		if (!status)
-			dev_info(&pdev->dev, "PCIe error reporting enabled\n");
-	}
+	status = pci_enable_pcie_error_reporting(pdev);
+	if (!status)
+		dev_info(&pdev->dev, "PCIe error reporting enabled\n");
 
 	status = be_ctrl_init(adapter);
 	if (status)
@@ -4897,7 +4924,8 @@ static int be_probe(struct pci_dev *pdev, const struct pci_device_id *pdev_id)
 
 	INIT_DELAYED_WORK(&adapter->work, be_worker);
 	INIT_DELAYED_WORK(&adapter->func_recovery_work, be_func_recovery_task);
-	adapter->rx_fc = adapter->tx_fc = true;
+	adapter->rx_fc = true;
+	adapter->tx_fc = true;
 
 	status = be_setup(adapter);
 	if (status)
diff --git a/drivers/net/ethernet/emulex/benet/be_roce.c b/drivers/net/ethernet/emulex/benet/be_roce.c
index ef4672dc7357..132866433a25 100644
--- a/drivers/net/ethernet/emulex/benet/be_roce.c
+++ b/drivers/net/ethernet/emulex/benet/be_roce.c
@@ -174,6 +174,7 @@ int be_roce_register_driver(struct ocrdma_driver *drv)
 	ocrdma_drv = drv;
 	list_for_each_entry(dev, &be_adapter_list, entry) {
 		struct net_device *netdev;
+
 		_be_roce_dev_add(dev);
 		netdev = dev->netdev;
 		if (netif_running(netdev) && netif_oper_up(netdev))
diff --git a/drivers/net/ethernet/ethoc.c b/drivers/net/ethernet/ethoc.c
index f3658bdb64cc..0bc6c102f3ac 100644
--- a/drivers/net/ethernet/ethoc.c
+++ b/drivers/net/ethernet/ethoc.c
@@ -1222,8 +1222,6 @@ static int ethoc_probe(struct platform_device *pdev)
 		goto error;
 	}
 
-	ether_setup(netdev);
-
 	/* setup the net_device structure */
 	netdev->netdev_ops = &ethoc_netdev_ops;
 	netdev->watchdog_timeo = ETHOC_TIMEOUT;
diff --git a/drivers/net/ethernet/freescale/fec.h b/drivers/net/ethernet/freescale/fec.h
index 9f7fa644a397..9af296a1ca99 100644
--- a/drivers/net/ethernet/freescale/fec.h
+++ b/drivers/net/ethernet/freescale/fec.h
@@ -27,8 +27,8 @@
  */
 #define FEC_IEVENT		0x004 /* Interrupt event reg */
 #define FEC_IMASK		0x008 /* Interrupt mask reg */
-#define FEC_R_DES_ACTIVE	0x010 /* Receive descriptor reg */
-#define FEC_X_DES_ACTIVE	0x014 /* Transmit descriptor reg */
+#define FEC_R_DES_ACTIVE_0	0x010 /* Receive descriptor reg */
+#define FEC_X_DES_ACTIVE_0	0x014 /* Transmit descriptor reg */
 #define FEC_ECNTRL		0x024 /* Ethernet control reg */
 #define FEC_MII_DATA		0x040 /* MII manage frame reg */
 #define FEC_MII_SPEED		0x044 /* MII speed control reg */
@@ -38,6 +38,12 @@
 #define FEC_ADDR_LOW		0x0e4 /* Low 32bits MAC address */
 #define FEC_ADDR_HIGH		0x0e8 /* High 16bits MAC address */
 #define FEC_OPD			0x0ec /* Opcode + Pause duration */
+#define FEC_TXIC0		0xF0  /* Tx Interrupt Coalescing for ring 0 */
+#define FEC_TXIC1		0xF4  /* Tx Interrupt Coalescing for ring 1 */
+#define FEC_TXIC2		0xF8  /* Tx Interrupt Coalescing for ring 2 */
+#define FEC_RXIC0		0x100 /* Rx Interrupt Coalescing for ring 0 */
+#define FEC_RXIC1		0x104 /* Rx Interrupt Coalescing for ring 1 */
+#define FEC_RXIC2		0x108 /* Rx Interrupt Coalescing for ring 2 */
 #define FEC_HASH_TABLE_HIGH	0x118 /* High 32bits hash table */
 #define FEC_HASH_TABLE_LOW	0x11c /* Low 32bits hash table */
 #define FEC_GRP_HASH_TABLE_HIGH	0x120 /* High 32bits hash table */
@@ -45,14 +51,27 @@
 #define FEC_X_WMRK		0x144 /* FIFO transmit water mark */
 #define FEC_R_BOUND		0x14c /* FIFO receive bound reg */
 #define FEC_R_FSTART		0x150 /* FIFO receive start reg */
-#define FEC_R_DES_START		0x180 /* Receive descriptor ring */
-#define FEC_X_DES_START		0x184 /* Transmit descriptor ring */
+#define FEC_R_DES_START_1	0x160 /* Receive descriptor ring 1 */
+#define FEC_X_DES_START_1	0x164 /* Transmit descriptor ring 1 */
+#define FEC_R_DES_START_2	0x16c /* Receive descriptor ring 2 */
+#define FEC_X_DES_START_2	0x170 /* Transmit descriptor ring 2 */
+#define FEC_R_DES_START_0	0x180 /* Receive descriptor ring */
+#define FEC_X_DES_START_0	0x184 /* Transmit descriptor ring */
 #define FEC_R_BUFF_SIZE		0x188 /* Maximum receive buff size */
 #define FEC_R_FIFO_RSFL		0x190 /* Receive FIFO section full threshold */
 #define FEC_R_FIFO_RSEM		0x194 /* Receive FIFO section empty threshold */
 #define FEC_R_FIFO_RAEM		0x198 /* Receive FIFO almost empty threshold */
 #define FEC_R_FIFO_RAFL		0x19c /* Receive FIFO almost full threshold */
 #define FEC_RACC		0x1C4 /* Receive Accelerator function */
+#define FEC_RCMR_1		0x1c8 /* Receive classification match ring 1 */
+#define FEC_RCMR_2		0x1cc /* Receive classification match ring 2 */
+#define FEC_DMA_CFG_1		0x1d8 /* DMA class configuration for ring 1 */
+#define FEC_DMA_CFG_2		0x1dc /* DMA class Configuration for ring 2 */
+#define FEC_R_DES_ACTIVE_1	0x1e0 /* Rx descriptor active for ring 1 */
+#define FEC_X_DES_ACTIVE_1	0x1e4 /* Tx descriptor active for ring 1 */
+#define FEC_R_DES_ACTIVE_2	0x1e8 /* Rx descriptor active for ring 2 */
+#define FEC_X_DES_ACTIVE_2	0x1ec /* Tx descriptor active for ring 2 */
+#define FEC_QOS_SCHEME		0x1f0 /* Set multi queues Qos scheme */
 #define FEC_MIIGSK_CFGR		0x300 /* MIIGSK Configuration reg */
 #define FEC_MIIGSK_ENR		0x308 /* MIIGSK Enable reg */
 
@@ -121,8 +140,12 @@
 #define FEC_IEVENT		0x004 /* Interrupt even reg */
 #define FEC_IMASK		0x008 /* Interrupt mask reg */
 #define FEC_IVEC		0x00c /* Interrupt vec status reg */
-#define FEC_R_DES_ACTIVE	0x010 /* Receive descriptor reg */
-#define FEC_X_DES_ACTIVE	0x014 /* Transmit descriptor reg */
+#define FEC_R_DES_ACTIVE_0	0x010 /* Receive descriptor reg */
+#define FEC_R_DES_ACTIVE_1	FEC_R_DES_ACTIVE_0
+#define FEC_R_DES_ACTIVE_2	FEC_R_DES_ACTIVE_0
+#define FEC_X_DES_ACTIVE_0	0x014 /* Transmit descriptor reg */
+#define FEC_X_DES_ACTIVE_1	FEC_X_DES_ACTIVE_0
+#define FEC_X_DES_ACTIVE_2	FEC_X_DES_ACTIVE_0
 #define FEC_MII_DATA		0x040 /* MII manage frame reg */
 #define FEC_MII_SPEED		0x044 /* MII speed control reg */
 #define FEC_R_BOUND		0x08c /* FIFO receive bound reg */
@@ -136,11 +159,27 @@
 #define FEC_ADDR_HIGH		0x3c4 /* High 16bits MAC address */
 #define FEC_GRP_HASH_TABLE_HIGH	0x3c8 /* High 32bits hash table */
 #define FEC_GRP_HASH_TABLE_LOW	0x3cc /* Low 32bits hash table */
-#define FEC_R_DES_START		0x3d0 /* Receive descriptor ring */
-#define FEC_X_DES_START		0x3d4 /* Transmit descriptor ring */
+#define FEC_R_DES_START_0	0x3d0 /* Receive descriptor ring */
+#define FEC_R_DES_START_1	FEC_R_DES_START_0
+#define FEC_R_DES_START_2	FEC_R_DES_START_0
+#define FEC_X_DES_START_0	0x3d4 /* Transmit descriptor ring */
+#define FEC_X_DES_START_1	FEC_X_DES_START_0
+#define FEC_X_DES_START_2	FEC_X_DES_START_0
 #define FEC_R_BUFF_SIZE		0x3d8 /* Maximum receive buff size */
 #define FEC_FIFO_RAM		0x400 /* FIFO RAM buffer */
-
+/* Not existed in real chip
+ * Just for pass build.
+ */
+#define FEC_RCMR_1		0xFFF
+#define FEC_RCMR_2		0xFFF
+#define FEC_DMA_CFG_1		0xFFF
+#define FEC_DMA_CFG_2		0xFFF
+#define FEC_TXIC0		0xFFF
+#define FEC_TXIC1		0xFFF
+#define FEC_TXIC2		0xFFF
+#define FEC_RXIC0		0xFFF
+#define FEC_RXIC1		0xFFF
+#define FEC_RXIC2		0xFFF
 #endif /* CONFIG_M5272 */
 
 
@@ -233,6 +272,44 @@ struct bufdesc_ex {
 /* This device has up to three irqs on some platforms */
 #define FEC_IRQ_NUM		3
 
+/* Maximum number of queues supported
+ * ENET with AVB IP can support up to 3 independent tx queues and rx queues.
+ * User can point the queue number that is less than or equal to 3.
+ */
+#define FEC_ENET_MAX_TX_QS	3
+#define FEC_ENET_MAX_RX_QS	3
+
+#define FEC_R_DES_START(X)	((X == 1) ? FEC_R_DES_START_1 : \
+				((X == 2) ? \
+					FEC_R_DES_START_2 : FEC_R_DES_START_0))
+#define FEC_X_DES_START(X)	((X == 1) ? FEC_X_DES_START_1 : \
+				((X == 2) ? \
+					FEC_X_DES_START_2 : FEC_X_DES_START_0))
+#define FEC_R_DES_ACTIVE(X)	((X == 1) ? FEC_R_DES_ACTIVE_1 : \
+				((X == 2) ? \
+				   FEC_R_DES_ACTIVE_2 : FEC_R_DES_ACTIVE_0))
+#define FEC_X_DES_ACTIVE(X)	((X == 1) ? FEC_X_DES_ACTIVE_1 : \
+				((X == 2) ? \
+				   FEC_X_DES_ACTIVE_2 : FEC_X_DES_ACTIVE_0))
+
+#define FEC_DMA_CFG(X)		((X == 2) ? FEC_DMA_CFG_2 : FEC_DMA_CFG_1)
+
+#define DMA_CLASS_EN		(1 << 16)
+#define FEC_RCMR(X)		((X == 2) ? FEC_RCMR_2 : FEC_RCMR_1)
+#define IDLE_SLOPE_MASK		0xFFFF
+#define IDLE_SLOPE_1		0x200 /* BW fraction: 0.5 */
+#define IDLE_SLOPE_2		0x200 /* BW fraction: 0.5 */
+#define IDLE_SLOPE(X)		((X == 1) ? (IDLE_SLOPE_1 & IDLE_SLOPE_MASK) : \
+				(IDLE_SLOPE_2 & IDLE_SLOPE_MASK))
+#define RCMR_MATCHEN            (0x1 << 16)
+#define RCMR_CMP_CFG(v, n)	((v & 0x7) <<  (n << 2))
+#define RCMR_CMP_1		(RCMR_CMP_CFG(0, 0) | RCMR_CMP_CFG(1, 1) | \
+				RCMR_CMP_CFG(2, 2) | RCMR_CMP_CFG(3, 3))
+#define RCMR_CMP_2		(RCMR_CMP_CFG(4, 0) | RCMR_CMP_CFG(5, 1) | \
+				RCMR_CMP_CFG(6, 2) | RCMR_CMP_CFG(7, 3))
+#define RCMR_CMP(X)		((X == 1) ? RCMR_CMP_1 : RCMR_CMP_2)
+#define FEC_TX_BD_FTYPE(X)	((X & 0xF) << 20)
+
 /* The number of Tx and Rx buffers.  These are allocated from the page
  * pool.  The code may assume these are power of two, so it it best
  * to keep them that size.
@@ -240,7 +317,7 @@ struct bufdesc_ex {
  * the skbuffer directly.
  */
 
-#define FEC_ENET_RX_PAGES	8
+#define FEC_ENET_RX_PAGES	256
 #define FEC_ENET_RX_FRSIZE	2048
 #define FEC_ENET_RX_FRPPG	(PAGE_SIZE / FEC_ENET_RX_FRSIZE)
 #define RX_RING_SIZE		(FEC_ENET_RX_FRPPG * FEC_ENET_RX_PAGES)
@@ -256,6 +333,119 @@ struct bufdesc_ex {
 #define FLAG_RX_CSUM_ENABLED	(BD_ENET_RX_ICE | BD_ENET_RX_PCR)
 #define FLAG_RX_CSUM_ERROR	(BD_ENET_RX_ICE | BD_ENET_RX_PCR)
 
+/* Interrupt events/masks. */
+#define FEC_ENET_HBERR  ((uint)0x80000000)      /* Heartbeat error */
+#define FEC_ENET_BABR   ((uint)0x40000000)      /* Babbling receiver */
+#define FEC_ENET_BABT   ((uint)0x20000000)      /* Babbling transmitter */
+#define FEC_ENET_GRA    ((uint)0x10000000)      /* Graceful stop complete */
+#define FEC_ENET_TXF_0	((uint)0x08000000)	/* Full frame transmitted */
+#define FEC_ENET_TXF_1	((uint)0x00000008)	/* Full frame transmitted */
+#define FEC_ENET_TXF_2	((uint)0x00000080)	/* Full frame transmitted */
+#define FEC_ENET_TXB    ((uint)0x04000000)      /* A buffer was transmitted */
+#define FEC_ENET_RXF_0	((uint)0x02000000)	/* Full frame received */
+#define FEC_ENET_RXF_1	((uint)0x00000002)	/* Full frame received */
+#define FEC_ENET_RXF_2	((uint)0x00000020)	/* Full frame received */
+#define FEC_ENET_RXB    ((uint)0x01000000)      /* A buffer was received */
+#define FEC_ENET_MII    ((uint)0x00800000)      /* MII interrupt */
+#define FEC_ENET_EBERR  ((uint)0x00400000)      /* SDMA bus error */
+#define FEC_ENET_TXF	(FEC_ENET_TXF_0 | FEC_ENET_TXF_1 | FEC_ENET_TXF_2)
+#define FEC_ENET_RXF	(FEC_ENET_RXF_0 | FEC_ENET_RXF_1 | FEC_ENET_RXF_2)
+#define FEC_ENET_TS_AVAIL       ((uint)0x00010000)
+#define FEC_ENET_TS_TIMER       ((uint)0x00008000)
+
+#define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII | FEC_ENET_TS_TIMER)
+#define FEC_RX_DISABLED_IMASK (FEC_DEFAULT_IMASK & (~FEC_ENET_RXF))
+
+/* ENET interrupt coalescing macro define */
+#define FEC_ITR_CLK_SEL		(0x1 << 30)
+#define FEC_ITR_EN		(0x1 << 31)
+#define FEC_ITR_ICFT(X)		((X & 0xFF) << 20)
+#define FEC_ITR_ICTT(X)		((X) & 0xFFFF)
+#define FEC_ITR_ICFT_DEFAULT	200  /* Set 200 frame count threshold */
+#define FEC_ITR_ICTT_DEFAULT	1000 /* Set 1000us timer threshold */
+
+#define FEC_VLAN_TAG_LEN       0x04
+#define FEC_ETHTYPE_LEN                0x02
+
+/* Controller is ENET-MAC */
+#define FEC_QUIRK_ENET_MAC		(1 << 0)
+/* Controller needs driver to swap frame */
+#define FEC_QUIRK_SWAP_FRAME		(1 << 1)
+/* Controller uses gasket */
+#define FEC_QUIRK_USE_GASKET		(1 << 2)
+/* Controller has GBIT support */
+#define FEC_QUIRK_HAS_GBIT		(1 << 3)
+/* Controller has extend desc buffer */
+#define FEC_QUIRK_HAS_BUFDESC_EX	(1 << 4)
+/* Controller has hardware checksum support */
+#define FEC_QUIRK_HAS_CSUM		(1 << 5)
+/* Controller has hardware vlan support */
+#define FEC_QUIRK_HAS_VLAN		(1 << 6)
+/* ENET IP errata ERR006358
+ *
+ * If the ready bit in the transmit buffer descriptor (TxBD[R]) is previously
+ * detected as not set during a prior frame transmission, then the
+ * ENET_TDAR[TDAR] bit is cleared at a later time, even if additional TxBDs
+ * were added to the ring and the ENET_TDAR[TDAR] bit is set. This results in
+ * frames not being transmitted until there is a 0-to-1 transition on
+ * ENET_TDAR[TDAR].
+ */
+#define FEC_QUIRK_ERR006358            (1 << 7)
+/* ENET IP hw AVB
+ *
+ * i.MX6SX ENET IP add Audio Video Bridging (AVB) feature support.
+ * - Two class indicators on receive with configurable priority
+ * - Two class indicators and line speed timer on transmit allowing
+ *   implementation class credit based shapers externally
+ * - Additional DMA registers provisioned to allow managing up to 3
+ *   independent rings
+ */
+#define FEC_QUIRK_HAS_AVB		(1 << 8)
+/* There is a TDAR race condition for mutliQ when the software sets TDAR
+ * and the UDMA clears TDAR simultaneously or in a small window (2-4 cycles).
+ * This will cause the udma_tx and udma_tx_arbiter state machines to hang.
+ * The issue exist at i.MX6SX enet IP.
+ */
+#define FEC_QUIRK_ERR007885		(1 << 9)
+/* ENET Block Guide/ Chapter for the iMX6SX (PELE) address one issue:
+ * After set ENET_ATCR[Capture], there need some time cycles before the counter
+ * value is capture in the register clock domain.
+ * The wait-time-cycles is at least 6 clock cycles of the slower clock between
+ * the register clock and the 1588 clock. The 1588 ts_clk is fixed to 25Mhz,
+ * register clock is 66Mhz, so the wait-time-cycles must be greater than 240ns
+ * (40ns * 6).
+ */
+#define FEC_QUIRK_BUG_CAPTURE		(1 << 10)
+
+struct fec_enet_priv_tx_q {
+	int index;
+	unsigned char *tx_bounce[TX_RING_SIZE];
+	struct  sk_buff *tx_skbuff[TX_RING_SIZE];
+
+	dma_addr_t	bd_dma;
+	struct bufdesc	*tx_bd_base;
+	uint tx_ring_size;
+
+	unsigned short tx_stop_threshold;
+	unsigned short tx_wake_threshold;
+
+	struct bufdesc	*cur_tx;
+	struct bufdesc	*dirty_tx;
+	char *tso_hdrs;
+	dma_addr_t tso_hdrs_dma;
+};
+
+struct fec_enet_priv_rx_q {
+	int index;
+	struct  sk_buff *rx_skbuff[RX_RING_SIZE];
+
+	dma_addr_t	bd_dma;
+	struct bufdesc	*rx_bd_base;
+	uint rx_ring_size;
+
+	struct bufdesc	*cur_rx;
+};
+
 /* The FEC buffer descriptors track the ring buffers.  The rx_bd_base and
  * tx_bd_base always point to the base of the buffer descriptors.  The
  * cur_rx and cur_tx point to the currently available buffer.
@@ -272,33 +462,28 @@ struct fec_enet_private {
 
 	struct clk *clk_ipg;
 	struct clk *clk_ahb;
+	struct clk *clk_ref;
 	struct clk *clk_enet_out;
 	struct clk *clk_ptp;
 
+	bool ptp_clk_on;
+	struct mutex ptp_clk_mutex;
+	unsigned int num_tx_queues;
+	unsigned int num_rx_queues;
+
 	/* The saved address of a sent-in-place packet/buffer, for skfree(). */
-	unsigned char *tx_bounce[TX_RING_SIZE];
-	struct	sk_buff *tx_skbuff[TX_RING_SIZE];
-	struct	sk_buff *rx_skbuff[RX_RING_SIZE];
+	struct fec_enet_priv_tx_q *tx_queue[FEC_ENET_MAX_TX_QS];
+	struct fec_enet_priv_rx_q *rx_queue[FEC_ENET_MAX_RX_QS];
 
-	/* CPM dual port RAM relative addresses */
-	dma_addr_t	bd_dma;
-	/* Address of Rx and Tx buffers */
-	struct bufdesc	*rx_bd_base;
-	struct bufdesc	*tx_bd_base;
-	/* The next free ring entry */
-	struct bufdesc	*cur_rx, *cur_tx;
-	/* The ring entries to be free()ed */
-	struct bufdesc	*dirty_tx;
+	unsigned int total_tx_ring_size;
+	unsigned int total_rx_ring_size;
 
-	unsigned short bufdesc_size;
-	unsigned short tx_ring_size;
-	unsigned short rx_ring_size;
-	unsigned short tx_stop_threshold;
-	unsigned short tx_wake_threshold;
+	unsigned long work_tx;
+	unsigned long work_rx;
+	unsigned long work_ts;
+	unsigned long work_mdio;
 
-	/* Software TSO */
-	char *tso_hdrs;
-	dma_addr_t tso_hdrs_dma;
+	unsigned short bufdesc_size;
 
 	struct	platform_device *pdev;
 
@@ -335,14 +520,36 @@ struct fec_enet_private {
 	u32 cycle_speed;
 	int hwts_rx_en;
 	int hwts_tx_en;
-	struct timer_list time_keep;
+	struct delayed_work time_keep;
 	struct regulator *reg_phy;
+
+	unsigned int tx_align;
+	unsigned int rx_align;
+
+	/* hw interrupt coalesce */
+	unsigned int rx_pkts_itr;
+	unsigned int rx_time_itr;
+	unsigned int tx_pkts_itr;
+	unsigned int tx_time_itr;
+	unsigned int itr_clk_rate;
+
+	u32 rx_copybreak;
+
+	/* ptp clock period in ns*/
+	unsigned int ptp_inc;
+
+	/* pps  */
+	int pps_channel;
+	unsigned int reload_period;
+	int pps_enable;
+	unsigned int next_counter;
 };
 
 void fec_ptp_init(struct platform_device *pdev);
 void fec_ptp_start_cyclecounter(struct net_device *ndev);
 int fec_ptp_set(struct net_device *ndev, struct ifreq *ifr);
 int fec_ptp_get(struct net_device *ndev, struct ifreq *ifr);
+uint fec_ptp_check_pps_event(struct fec_enet_private *fep);
 
 /****************************************************************************/
 #endif /* FEC_H */
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 4f87dffcb9b2..81b96cf87574 100644
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -57,21 +57,19 @@
 #include <linux/regulator/consumer.h>
 #include <linux/if_vlan.h>
 #include <linux/pinctrl/consumer.h>
+#include <linux/prefetch.h>
 
 #include <asm/cacheflush.h>
 
 #include "fec.h"
 
 static void set_multicast_list(struct net_device *ndev);
-
-#if defined(CONFIG_ARM)
-#define FEC_ALIGNMENT	0xf
-#else
-#define FEC_ALIGNMENT	0x3
-#endif
+static void fec_enet_itr_coal_init(struct net_device *ndev);
 
 #define DRIVER_NAME	"fec"
 
+#define FEC_ENET_GET_QUQUE(_x) ((_x == 0) ? 1 : ((_x == 1) ? 2 : 0))
+
 /* Pause frame feild and FIFO threshold */
 #define FEC_ENET_FCE	(1 << 5)
 #define FEC_ENET_RSEM_V	0x84
@@ -80,31 +78,6 @@ static void set_multicast_list(struct net_device *ndev);
 #define FEC_ENET_RAFL_V	0x8
 #define FEC_ENET_OPD_V	0xFFF0
 
-/* Controller is ENET-MAC */
-#define FEC_QUIRK_ENET_MAC		(1 << 0)
-/* Controller needs driver to swap frame */
-#define FEC_QUIRK_SWAP_FRAME		(1 << 1)
-/* Controller uses gasket */
-#define FEC_QUIRK_USE_GASKET		(1 << 2)
-/* Controller has GBIT support */
-#define FEC_QUIRK_HAS_GBIT		(1 << 3)
-/* Controller has extend desc buffer */
-#define FEC_QUIRK_HAS_BUFDESC_EX	(1 << 4)
-/* Controller has hardware checksum support */
-#define FEC_QUIRK_HAS_CSUM		(1 << 5)
-/* Controller has hardware vlan support */
-#define FEC_QUIRK_HAS_VLAN		(1 << 6)
-/* ENET IP errata ERR006358
- *
- * If the ready bit in the transmit buffer descriptor (TxBD[R]) is previously
- * detected as not set during a prior frame transmission, then the
- * ENET_TDAR[TDAR] bit is cleared at a later time, even if additional TxBDs
- * were added to the ring and the ENET_TDAR[TDAR] bit is set. This results in
- * frames not being transmitted until there is a 0-to-1 transition on
- * ENET_TDAR[TDAR].
- */
-#define FEC_QUIRK_ERR006358            (1 << 7)
-
 static struct platform_device_id fec_devtype[] = {
 	{
 		/* keep it for coldfire */
@@ -128,6 +101,12 @@ static struct platform_device_id fec_devtype[] = {
 		.name = "mvf600-fec",
 		.driver_data = FEC_QUIRK_ENET_MAC,
 	}, {
+		.name = "imx6sx-fec",
+		.driver_data = FEC_QUIRK_ENET_MAC | FEC_QUIRK_HAS_GBIT |
+				FEC_QUIRK_HAS_BUFDESC_EX | FEC_QUIRK_HAS_CSUM |
+				FEC_QUIRK_HAS_VLAN | FEC_QUIRK_HAS_AVB |
+				FEC_QUIRK_ERR007885 | FEC_QUIRK_BUG_CAPTURE,
+	}, {
 		/* sentinel */
 	}
 };
@@ -139,6 +118,7 @@ enum imx_fec_type {
 	IMX28_FEC,
 	IMX6Q_FEC,
 	MVF600_FEC,
+	IMX6SX_FEC,
 };
 
 static const struct of_device_id fec_dt_ids[] = {
@@ -147,6 +127,7 @@ static const struct of_device_id fec_dt_ids[] = {
 	{ .compatible = "fsl,imx28-fec", .data = &fec_devtype[IMX28_FEC], },
 	{ .compatible = "fsl,imx6q-fec", .data = &fec_devtype[IMX6Q_FEC], },
 	{ .compatible = "fsl,mvf600-fec", .data = &fec_devtype[MVF600_FEC], },
+	{ .compatible = "fsl,imx6sx-fec", .data = &fec_devtype[IMX6SX_FEC], },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, fec_dt_ids);
@@ -175,21 +156,6 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #endif
 #endif /* CONFIG_M5272 */
 
-/* Interrupt events/masks. */
-#define FEC_ENET_HBERR	((uint)0x80000000)	/* Heartbeat error */
-#define FEC_ENET_BABR	((uint)0x40000000)	/* Babbling receiver */
-#define FEC_ENET_BABT	((uint)0x20000000)	/* Babbling transmitter */
-#define FEC_ENET_GRA	((uint)0x10000000)	/* Graceful stop complete */
-#define FEC_ENET_TXF	((uint)0x08000000)	/* Full frame transmitted */
-#define FEC_ENET_TXB	((uint)0x04000000)	/* A buffer was transmitted */
-#define FEC_ENET_RXF	((uint)0x02000000)	/* Full frame received */
-#define FEC_ENET_RXB	((uint)0x01000000)	/* A buffer was received */
-#define FEC_ENET_MII	((uint)0x00800000)	/* MII interrupt */
-#define FEC_ENET_EBERR	((uint)0x00400000)	/* SDMA bus error */
-
-#define FEC_DEFAULT_IMASK (FEC_ENET_TXF | FEC_ENET_RXF | FEC_ENET_MII)
-#define FEC_RX_DISABLED_IMASK (FEC_DEFAULT_IMASK & (~FEC_ENET_RXF))
-
 /* The FEC stores dest/src/type/vlan, data, and checksum for receive packets.
  */
 #define PKT_MAXBUF_SIZE		1522
@@ -230,6 +196,8 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 #define FEC_PAUSE_FLAG_AUTONEG	0x1
 #define FEC_PAUSE_FLAG_ENABLE	0x2
 
+#define COPYBREAK_DEFAULT	256
+
 #define TSO_HEADER_SIZE		128
 /* Max number of allowed TCP segments for software TSO */
 #define FEC_MAX_TSO_SEGS	100
@@ -242,22 +210,26 @@ MODULE_PARM_DESC(macaddr, "FEC Ethernet MAC address");
 static int mii_cnt;
 
 static inline
-struct bufdesc *fec_enet_get_nextdesc(struct bufdesc *bdp, struct fec_enet_private *fep)
+struct bufdesc *fec_enet_get_nextdesc(struct bufdesc *bdp,
+				      struct fec_enet_private *fep,
+				      int queue_id)
 {
 	struct bufdesc *new_bd = bdp + 1;
 	struct bufdesc_ex *ex_new_bd = (struct bufdesc_ex *)bdp + 1;
+	struct fec_enet_priv_tx_q *txq = fep->tx_queue[queue_id];
+	struct fec_enet_priv_rx_q *rxq = fep->rx_queue[queue_id];
 	struct bufdesc_ex *ex_base;
 	struct bufdesc *base;
 	int ring_size;
 
-	if (bdp >= fep->tx_bd_base) {
-		base = fep->tx_bd_base;
-		ring_size = fep->tx_ring_size;
-		ex_base = (struct bufdesc_ex *)fep->tx_bd_base;
+	if (bdp >= txq->tx_bd_base) {
+		base = txq->tx_bd_base;
+		ring_size = txq->tx_ring_size;
+		ex_base = (struct bufdesc_ex *)txq->tx_bd_base;
 	} else {
-		base = fep->rx_bd_base;
-		ring_size = fep->rx_ring_size;
-		ex_base = (struct bufdesc_ex *)fep->rx_bd_base;
+		base = rxq->rx_bd_base;
+		ring_size = rxq->rx_ring_size;
+		ex_base = (struct bufdesc_ex *)rxq->rx_bd_base;
 	}
 
 	if (fep->bufdesc_ex)
@@ -269,22 +241,26 @@ struct bufdesc *fec_enet_get_nextdesc(struct bufdesc *bdp, struct fec_enet_priva
 }
 
 static inline
-struct bufdesc *fec_enet_get_prevdesc(struct bufdesc *bdp, struct fec_enet_private *fep)
+struct bufdesc *fec_enet_get_prevdesc(struct bufdesc *bdp,
+				      struct fec_enet_private *fep,
+				      int queue_id)
 {
 	struct bufdesc *new_bd = bdp - 1;
 	struct bufdesc_ex *ex_new_bd = (struct bufdesc_ex *)bdp - 1;
+	struct fec_enet_priv_tx_q *txq = fep->tx_queue[queue_id];
+	struct fec_enet_priv_rx_q *rxq = fep->rx_queue[queue_id];
 	struct bufdesc_ex *ex_base;
 	struct bufdesc *base;
 	int ring_size;
 
-	if (bdp >= fep->tx_bd_base) {
-		base = fep->tx_bd_base;
-		ring_size = fep->tx_ring_size;
-		ex_base = (struct bufdesc_ex *)fep->tx_bd_base;
+	if (bdp >= txq->tx_bd_base) {
+		base = txq->tx_bd_base;
+		ring_size = txq->tx_ring_size;
+		ex_base = (struct bufdesc_ex *)txq->tx_bd_base;
 	} else {
-		base = fep->rx_bd_base;
-		ring_size = fep->rx_ring_size;
-		ex_base = (struct bufdesc_ex *)fep->rx_bd_base;
+		base = rxq->rx_bd_base;
+		ring_size = rxq->rx_ring_size;
+		ex_base = (struct bufdesc_ex *)rxq->rx_bd_base;
 	}
 
 	if (fep->bufdesc_ex)
@@ -300,14 +276,15 @@ static int fec_enet_get_bd_index(struct bufdesc *base, struct bufdesc *bdp,
 	return ((const char *)bdp - (const char *)base) / fep->bufdesc_size;
 }
 
-static int fec_enet_get_free_txdesc_num(struct fec_enet_private *fep)
+static int fec_enet_get_free_txdesc_num(struct fec_enet_private *fep,
+					struct fec_enet_priv_tx_q *txq)
 {
 	int entries;
 
-	entries = ((const char *)fep->dirty_tx -
-			(const char *)fep->cur_tx) / fep->bufdesc_size - 1;
+	entries = ((const char *)txq->dirty_tx -
+			(const char *)txq->cur_tx) / fep->bufdesc_size - 1;
 
-	return entries > 0 ? entries : entries + fep->tx_ring_size;
+	return entries > 0 ? entries : entries + txq->tx_ring_size;
 }
 
 static void *swap_buffer(void *bufaddr, int len)
@@ -324,22 +301,26 @@ static void *swap_buffer(void *bufaddr, int len)
 static void fec_dump(struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
-	struct bufdesc *bdp = fep->tx_bd_base;
-	unsigned int index = 0;
+	struct bufdesc *bdp;
+	struct fec_enet_priv_tx_q *txq;
+	int index = 0;
 
 	netdev_info(ndev, "TX ring dump\n");
 	pr_info("Nr     SC     addr       len  SKB\n");
 
+	txq = fep->tx_queue[0];
+	bdp = txq->tx_bd_base;
+
 	do {
 		pr_info("%3u %c%c 0x%04x 0x%08lx %4u %p\n",
 			index,
-			bdp == fep->cur_tx ? 'S' : ' ',
-			bdp == fep->dirty_tx ? 'H' : ' ',
+			bdp == txq->cur_tx ? 'S' : ' ',
+			bdp == txq->dirty_tx ? 'H' : ' ',
 			bdp->cbd_sc, bdp->cbd_bufaddr, bdp->cbd_datlen,
-			fep->tx_skbuff[index]);
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+			txq->tx_skbuff[index]);
+		bdp = fec_enet_get_nextdesc(bdp, fep, 0);
 		index++;
-	} while (bdp != fep->tx_bd_base);
+	} while (bdp != txq->tx_bd_base);
 }
 
 static inline bool is_ipv4_pkt(struct sk_buff *skb)
@@ -365,14 +346,17 @@ fec_enet_clear_csum(struct sk_buff *skb, struct net_device *ndev)
 }
 
 static int
-fec_enet_txq_submit_frag_skb(struct sk_buff *skb, struct net_device *ndev)
+fec_enet_txq_submit_frag_skb(struct fec_enet_priv_tx_q *txq,
+			     struct sk_buff *skb,
+			     struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
-	struct bufdesc *bdp = fep->cur_tx;
+	struct bufdesc *bdp = txq->cur_tx;
 	struct bufdesc_ex *ebdp;
 	int nr_frags = skb_shinfo(skb)->nr_frags;
+	unsigned short queue = skb_get_queue_mapping(skb);
 	int frag, frag_len;
 	unsigned short status;
 	unsigned int estatus = 0;
@@ -384,7 +368,7 @@ fec_enet_txq_submit_frag_skb(struct sk_buff *skb, struct net_device *ndev)
 
 	for (frag = 0; frag < nr_frags; frag++) {
 		this_frag = &skb_shinfo(skb)->frags[frag];
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
 		ebdp = (struct bufdesc_ex *)bdp;
 
 		status = bdp->cbd_sc;
@@ -404,6 +388,8 @@ fec_enet_txq_submit_frag_skb(struct sk_buff *skb, struct net_device *ndev)
 		}
 
 		if (fep->bufdesc_ex) {
+			if (id_entry->driver_data & FEC_QUIRK_HAS_AVB)
+				estatus |= FEC_TX_BD_FTYPE(queue);
 			if (skb->ip_summed == CHECKSUM_PARTIAL)
 				estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 			ebdp->cbd_bdu = 0;
@@ -412,11 +398,11 @@ fec_enet_txq_submit_frag_skb(struct sk_buff *skb, struct net_device *ndev)
 
 		bufaddr = page_address(this_frag->page.p) + this_frag->page_offset;
 
-		index = fec_enet_get_bd_index(fep->tx_bd_base, bdp, fep);
-		if (((unsigned long) bufaddr) & FEC_ALIGNMENT ||
+		index = fec_enet_get_bd_index(txq->tx_bd_base, bdp, fep);
+		if (((unsigned long) bufaddr) & fep->tx_align ||
 			id_entry->driver_data & FEC_QUIRK_SWAP_FRAME) {
-			memcpy(fep->tx_bounce[index], bufaddr, frag_len);
-			bufaddr = fep->tx_bounce[index];
+			memcpy(txq->tx_bounce[index], bufaddr, frag_len);
+			bufaddr = txq->tx_bounce[index];
 
 			if (id_entry->driver_data & FEC_QUIRK_SWAP_FRAME)
 				swap_buffer(bufaddr, frag_len);
@@ -436,21 +422,22 @@ fec_enet_txq_submit_frag_skb(struct sk_buff *skb, struct net_device *ndev)
 		bdp->cbd_sc = status;
 	}
 
-	fep->cur_tx = bdp;
+	txq->cur_tx = bdp;
 
 	return 0;
 
 dma_mapping_error:
-	bdp = fep->cur_tx;
+	bdp = txq->cur_tx;
 	for (i = 0; i < frag; i++) {
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
 		dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
 				bdp->cbd_datlen, DMA_TO_DEVICE);
 	}
 	return NETDEV_TX_OK;
 }
 
-static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
+static int fec_enet_txq_submit_skb(struct fec_enet_priv_tx_q *txq,
+				   struct sk_buff *skb, struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
@@ -461,12 +448,13 @@ static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
 	dma_addr_t addr;
 	unsigned short status;
 	unsigned short buflen;
+	unsigned short queue;
 	unsigned int estatus = 0;
 	unsigned int index;
 	int entries_free;
 	int ret;
 
-	entries_free = fec_enet_get_free_txdesc_num(fep);
+	entries_free = fec_enet_get_free_txdesc_num(fep, txq);
 	if (entries_free < MAX_SKB_FRAGS + 1) {
 		dev_kfree_skb_any(skb);
 		if (net_ratelimit())
@@ -481,7 +469,7 @@ static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
 	}
 
 	/* Fill in a Tx ring entry */
-	bdp = fep->cur_tx;
+	bdp = txq->cur_tx;
 	status = bdp->cbd_sc;
 	status &= ~BD_ENET_TX_STATS;
 
@@ -489,11 +477,12 @@ static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
 	bufaddr = skb->data;
 	buflen = skb_headlen(skb);
 
-	index = fec_enet_get_bd_index(fep->tx_bd_base, bdp, fep);
-	if (((unsigned long) bufaddr) & FEC_ALIGNMENT ||
+	queue = skb_get_queue_mapping(skb);
+	index = fec_enet_get_bd_index(txq->tx_bd_base, bdp, fep);
+	if (((unsigned long) bufaddr) & fep->tx_align ||
 		id_entry->driver_data & FEC_QUIRK_SWAP_FRAME) {
-		memcpy(fep->tx_bounce[index], skb->data, buflen);
-		bufaddr = fep->tx_bounce[index];
+		memcpy(txq->tx_bounce[index], skb->data, buflen);
+		bufaddr = txq->tx_bounce[index];
 
 		if (id_entry->driver_data & FEC_QUIRK_SWAP_FRAME)
 			swap_buffer(bufaddr, buflen);
@@ -509,7 +498,7 @@ static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
 	}
 
 	if (nr_frags) {
-		ret = fec_enet_txq_submit_frag_skb(skb, ndev);
+		ret = fec_enet_txq_submit_frag_skb(txq, skb, ndev);
 		if (ret)
 			return ret;
 	} else {
@@ -530,6 +519,9 @@ static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
 			fep->hwts_tx_en))
 			skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 
+		if (id_entry->driver_data & FEC_QUIRK_HAS_AVB)
+			estatus |= FEC_TX_BD_FTYPE(queue);
+
 		if (skb->ip_summed == CHECKSUM_PARTIAL)
 			estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 
@@ -537,10 +529,10 @@ static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
 		ebdp->cbd_esc = estatus;
 	}
 
-	last_bdp = fep->cur_tx;
-	index = fec_enet_get_bd_index(fep->tx_bd_base, last_bdp, fep);
+	last_bdp = txq->cur_tx;
+	index = fec_enet_get_bd_index(txq->tx_bd_base, last_bdp, fep);
 	/* Save skb pointer */
-	fep->tx_skbuff[index] = skb;
+	txq->tx_skbuff[index] = skb;
 
 	bdp->cbd_datlen = buflen;
 	bdp->cbd_bufaddr = addr;
@@ -552,27 +544,29 @@ static int fec_enet_txq_submit_skb(struct sk_buff *skb, struct net_device *ndev)
 	bdp->cbd_sc = status;
 
 	/* If this was the last BD in the ring, start at the beginning again. */
-	bdp = fec_enet_get_nextdesc(last_bdp, fep);
+	bdp = fec_enet_get_nextdesc(last_bdp, fep, queue);
 
 	skb_tx_timestamp(skb);
 
-	fep->cur_tx = bdp;
+	txq->cur_tx = bdp;
 
 	/* Trigger transmission start */
-	writel(0, fep->hwp + FEC_X_DES_ACTIVE);
+	writel(0, fep->hwp + FEC_X_DES_ACTIVE(queue));
 
 	return 0;
 }
 
 static int
-fec_enet_txq_put_data_tso(struct sk_buff *skb, struct net_device *ndev,
-			struct bufdesc *bdp, int index, char *data,
-			int size, bool last_tcp, bool is_last)
+fec_enet_txq_put_data_tso(struct fec_enet_priv_tx_q *txq, struct sk_buff *skb,
+			  struct net_device *ndev,
+			  struct bufdesc *bdp, int index, char *data,
+			  int size, bool last_tcp, bool is_last)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
-	struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
+	struct bufdesc_ex *ebdp = container_of(bdp, struct bufdesc_ex, desc);
+	unsigned short queue = skb_get_queue_mapping(skb);
 	unsigned short status;
 	unsigned int estatus = 0;
 	dma_addr_t addr;
@@ -582,10 +576,10 @@ fec_enet_txq_put_data_tso(struct sk_buff *skb, struct net_device *ndev,
 
 	status |= (BD_ENET_TX_TC | BD_ENET_TX_READY);
 
-	if (((unsigned long) data) & FEC_ALIGNMENT ||
+	if (((unsigned long) data) & fep->tx_align ||
 		id_entry->driver_data & FEC_QUIRK_SWAP_FRAME) {
-		memcpy(fep->tx_bounce[index], data, size);
-		data = fep->tx_bounce[index];
+		memcpy(txq->tx_bounce[index], data, size);
+		data = txq->tx_bounce[index];
 
 		if (id_entry->driver_data & FEC_QUIRK_SWAP_FRAME)
 			swap_buffer(data, size);
@@ -603,6 +597,8 @@ fec_enet_txq_put_data_tso(struct sk_buff *skb, struct net_device *ndev,
 	bdp->cbd_bufaddr = addr;
 
 	if (fep->bufdesc_ex) {
+		if (id_entry->driver_data & FEC_QUIRK_HAS_AVB)
+			estatus |= FEC_TX_BD_FTYPE(queue);
 		if (skb->ip_summed == CHECKSUM_PARTIAL)
 			estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 		ebdp->cbd_bdu = 0;
@@ -624,14 +620,16 @@ fec_enet_txq_put_data_tso(struct sk_buff *skb, struct net_device *ndev,
 }
 
 static int
-fec_enet_txq_put_hdr_tso(struct sk_buff *skb, struct net_device *ndev,
-			struct bufdesc *bdp, int index)
+fec_enet_txq_put_hdr_tso(struct fec_enet_priv_tx_q *txq,
+			 struct sk_buff *skb, struct net_device *ndev,
+			 struct bufdesc *bdp, int index)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
 	int hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
-	struct bufdesc_ex *ebdp = (struct bufdesc_ex *)bdp;
+	struct bufdesc_ex *ebdp = container_of(bdp, struct bufdesc_ex, desc);
+	unsigned short queue = skb_get_queue_mapping(skb);
 	void *bufaddr;
 	unsigned long dmabuf;
 	unsigned short status;
@@ -641,12 +639,12 @@ fec_enet_txq_put_hdr_tso(struct sk_buff *skb, struct net_device *ndev,
 	status &= ~BD_ENET_TX_STATS;
 	status |= (BD_ENET_TX_TC | BD_ENET_TX_READY);
 
-	bufaddr = fep->tso_hdrs + index * TSO_HEADER_SIZE;
-	dmabuf = fep->tso_hdrs_dma + index * TSO_HEADER_SIZE;
-	if (((unsigned long) bufaddr) & FEC_ALIGNMENT ||
+	bufaddr = txq->tso_hdrs + index * TSO_HEADER_SIZE;
+	dmabuf = txq->tso_hdrs_dma + index * TSO_HEADER_SIZE;
+	if (((unsigned long)bufaddr) & fep->tx_align ||
 		id_entry->driver_data & FEC_QUIRK_SWAP_FRAME) {
-		memcpy(fep->tx_bounce[index], skb->data, hdr_len);
-		bufaddr = fep->tx_bounce[index];
+		memcpy(txq->tx_bounce[index], skb->data, hdr_len);
+		bufaddr = txq->tx_bounce[index];
 
 		if (id_entry->driver_data & FEC_QUIRK_SWAP_FRAME)
 			swap_buffer(bufaddr, hdr_len);
@@ -665,6 +663,8 @@ fec_enet_txq_put_hdr_tso(struct sk_buff *skb, struct net_device *ndev,
 	bdp->cbd_datlen = hdr_len;
 
 	if (fep->bufdesc_ex) {
+		if (id_entry->driver_data & FEC_QUIRK_HAS_AVB)
+			estatus |= FEC_TX_BD_FTYPE(queue);
 		if (skb->ip_summed == CHECKSUM_PARTIAL)
 			estatus |= BD_ENET_TX_PINS | BD_ENET_TX_IINS;
 		ebdp->cbd_bdu = 0;
@@ -676,17 +676,22 @@ fec_enet_txq_put_hdr_tso(struct sk_buff *skb, struct net_device *ndev,
 	return 0;
 }
 
-static int fec_enet_txq_submit_tso(struct sk_buff *skb, struct net_device *ndev)
+static int fec_enet_txq_submit_tso(struct fec_enet_priv_tx_q *txq,
+				   struct sk_buff *skb,
+				   struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	int hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
 	int total_len, data_left;
-	struct bufdesc *bdp = fep->cur_tx;
+	struct bufdesc *bdp = txq->cur_tx;
+	unsigned short queue = skb_get_queue_mapping(skb);
 	struct tso_t tso;
 	unsigned int index = 0;
 	int ret;
+	const struct platform_device_id *id_entry =
+				platform_get_device_id(fep->pdev);
 
-	if (tso_count_descs(skb) >= fec_enet_get_free_txdesc_num(fep)) {
+	if (tso_count_descs(skb) >= fec_enet_get_free_txdesc_num(fep, txq)) {
 		dev_kfree_skb_any(skb);
 		if (net_ratelimit())
 			netdev_err(ndev, "NOT enough BD for TSO!\n");
@@ -706,14 +711,14 @@ static int fec_enet_txq_submit_tso(struct sk_buff *skb, struct net_device *ndev)
 	while (total_len > 0) {
 		char *hdr;
 
-		index = fec_enet_get_bd_index(fep->tx_bd_base, bdp, fep);
+		index = fec_enet_get_bd_index(txq->tx_bd_base, bdp, fep);
 		data_left = min_t(int, skb_shinfo(skb)->gso_size, total_len);
 		total_len -= data_left;
 
 		/* prepare packet headers: MAC + IP + TCP */
-		hdr = fep->tso_hdrs + index * TSO_HEADER_SIZE;
+		hdr = txq->tso_hdrs + index * TSO_HEADER_SIZE;
 		tso_build_hdr(skb, hdr, &tso, data_left, total_len == 0);
-		ret = fec_enet_txq_put_hdr_tso(skb, ndev, bdp, index);
+		ret = fec_enet_txq_put_hdr_tso(txq, skb, ndev, bdp, index);
 		if (ret)
 			goto err_release;
 
@@ -721,10 +726,13 @@ static int fec_enet_txq_submit_tso(struct sk_buff *skb, struct net_device *ndev)
 			int size;
 
 			size = min_t(int, tso.size, data_left);
-			bdp = fec_enet_get_nextdesc(bdp, fep);
-			index = fec_enet_get_bd_index(fep->tx_bd_base, bdp, fep);
-			ret = fec_enet_txq_put_data_tso(skb, ndev, bdp, index, tso.data,
-							size, size == data_left,
+			bdp = fec_enet_get_nextdesc(bdp, fep, queue);
+			index = fec_enet_get_bd_index(txq->tx_bd_base,
+						      bdp, fep);
+			ret = fec_enet_txq_put_data_tso(txq, skb, ndev,
+							bdp, index,
+							tso.data, size,
+							size == data_left,
 							total_len == 0);
 			if (ret)
 				goto err_release;
@@ -733,17 +741,22 @@ static int fec_enet_txq_submit_tso(struct sk_buff *skb, struct net_device *ndev)
 			tso_build_data(skb, &tso, size);
 		}
 
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
 	}
 
 	/* Save skb pointer */
-	fep->tx_skbuff[index] = skb;
+	txq->tx_skbuff[index] = skb;
 
 	skb_tx_timestamp(skb);
-	fep->cur_tx = bdp;
+	txq->cur_tx = bdp;
 
 	/* Trigger transmission start */
-	writel(0, fep->hwp + FEC_X_DES_ACTIVE);
+	if (!(id_entry->driver_data & FEC_QUIRK_ERR007885) ||
+	    !readl(fep->hwp + FEC_X_DES_ACTIVE(queue)) ||
+	    !readl(fep->hwp + FEC_X_DES_ACTIVE(queue)) ||
+	    !readl(fep->hwp + FEC_X_DES_ACTIVE(queue)) ||
+	    !readl(fep->hwp + FEC_X_DES_ACTIVE(queue)))
+		writel(0, fep->hwp + FEC_X_DES_ACTIVE(queue));
 
 	return 0;
 
@@ -757,18 +770,25 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	int entries_free;
+	unsigned short queue;
+	struct fec_enet_priv_tx_q *txq;
+	struct netdev_queue *nq;
 	int ret;
 
+	queue = skb_get_queue_mapping(skb);
+	txq = fep->tx_queue[queue];
+	nq = netdev_get_tx_queue(ndev, queue);
+
 	if (skb_is_gso(skb))
-		ret = fec_enet_txq_submit_tso(skb, ndev);
+		ret = fec_enet_txq_submit_tso(txq, skb, ndev);
 	else
-		ret = fec_enet_txq_submit_skb(skb, ndev);
+		ret = fec_enet_txq_submit_skb(txq, skb, ndev);
 	if (ret)
 		return ret;
 
-	entries_free = fec_enet_get_free_txdesc_num(fep);
-	if (entries_free <= fep->tx_stop_threshold)
-		netif_stop_queue(ndev);
+	entries_free = fec_enet_get_free_txdesc_num(fep, txq);
+	if (entries_free <= txq->tx_stop_threshold)
+		netif_tx_stop_queue(nq);
 
 	return NETDEV_TX_OK;
 }
@@ -778,46 +798,111 @@ fec_enet_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 static void fec_enet_bd_init(struct net_device *dev)
 {
 	struct fec_enet_private *fep = netdev_priv(dev);
+	struct fec_enet_priv_tx_q *txq;
+	struct fec_enet_priv_rx_q *rxq;
 	struct bufdesc *bdp;
 	unsigned int i;
+	unsigned int q;
 
-	/* Initialize the receive buffer descriptors. */
-	bdp = fep->rx_bd_base;
-	for (i = 0; i < fep->rx_ring_size; i++) {
+	for (q = 0; q < fep->num_rx_queues; q++) {
+		/* Initialize the receive buffer descriptors. */
+		rxq = fep->rx_queue[q];
+		bdp = rxq->rx_bd_base;
 
-		/* Initialize the BD for every fragment in the page. */
-		if (bdp->cbd_bufaddr)
-			bdp->cbd_sc = BD_ENET_RX_EMPTY;
-		else
+		for (i = 0; i < rxq->rx_ring_size; i++) {
+
+			/* Initialize the BD for every fragment in the page. */
+			if (bdp->cbd_bufaddr)
+				bdp->cbd_sc = BD_ENET_RX_EMPTY;
+			else
+				bdp->cbd_sc = 0;
+			bdp = fec_enet_get_nextdesc(bdp, fep, q);
+		}
+
+		/* Set the last buffer to wrap */
+		bdp = fec_enet_get_prevdesc(bdp, fep, q);
+		bdp->cbd_sc |= BD_SC_WRAP;
+
+		rxq->cur_rx = rxq->rx_bd_base;
+	}
+
+	for (q = 0; q < fep->num_tx_queues; q++) {
+		/* ...and the same for transmit */
+		txq = fep->tx_queue[q];
+		bdp = txq->tx_bd_base;
+		txq->cur_tx = bdp;
+
+		for (i = 0; i < txq->tx_ring_size; i++) {
+			/* Initialize the BD for every fragment in the page. */
 			bdp->cbd_sc = 0;
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+			if (txq->tx_skbuff[i]) {
+				dev_kfree_skb_any(txq->tx_skbuff[i]);
+				txq->tx_skbuff[i] = NULL;
+			}
+			bdp->cbd_bufaddr = 0;
+			bdp = fec_enet_get_nextdesc(bdp, fep, q);
+		}
+
+		/* Set the last buffer to wrap */
+		bdp = fec_enet_get_prevdesc(bdp, fep, q);
+		bdp->cbd_sc |= BD_SC_WRAP;
+		txq->dirty_tx = bdp;
 	}
+}
 
-	/* Set the last buffer to wrap */
-	bdp = fec_enet_get_prevdesc(bdp, fep);
-	bdp->cbd_sc |= BD_SC_WRAP;
+static void fec_enet_active_rxring(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	int i;
 
-	fep->cur_rx = fep->rx_bd_base;
+	for (i = 0; i < fep->num_rx_queues; i++)
+		writel(0, fep->hwp + FEC_R_DES_ACTIVE(i));
+}
 
-	/* ...and the same for transmit */
-	bdp = fep->tx_bd_base;
-	fep->cur_tx = bdp;
-	for (i = 0; i < fep->tx_ring_size; i++) {
+static void fec_enet_enable_ring(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	struct fec_enet_priv_tx_q *txq;
+	struct fec_enet_priv_rx_q *rxq;
+	int i;
 
-		/* Initialize the BD for every fragment in the page. */
-		bdp->cbd_sc = 0;
-		if (fep->tx_skbuff[i]) {
-			dev_kfree_skb_any(fep->tx_skbuff[i]);
-			fep->tx_skbuff[i] = NULL;
-		}
-		bdp->cbd_bufaddr = 0;
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+	for (i = 0; i < fep->num_rx_queues; i++) {
+		rxq = fep->rx_queue[i];
+		writel(rxq->bd_dma, fep->hwp + FEC_R_DES_START(i));
+
+		/* enable DMA1/2 */
+		if (i)
+			writel(RCMR_MATCHEN | RCMR_CMP(i),
+			       fep->hwp + FEC_RCMR(i));
 	}
 
-	/* Set the last buffer to wrap */
-	bdp = fec_enet_get_prevdesc(bdp, fep);
-	bdp->cbd_sc |= BD_SC_WRAP;
-	fep->dirty_tx = bdp;
+	for (i = 0; i < fep->num_tx_queues; i++) {
+		txq = fep->tx_queue[i];
+		writel(txq->bd_dma, fep->hwp + FEC_X_DES_START(i));
+
+		/* enable DMA1/2 */
+		if (i)
+			writel(DMA_CLASS_EN | IDLE_SLOPE(i),
+			       fep->hwp + FEC_DMA_CFG(i));
+	}
+}
+
+static void fec_enet_reset_skb(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	struct fec_enet_priv_tx_q *txq;
+	int i, j;
+
+	for (i = 0; i < fep->num_tx_queues; i++) {
+		txq = fep->tx_queue[i];
+
+		for (j = 0; j < txq->tx_ring_size; j++) {
+			if (txq->tx_skbuff[j]) {
+				dev_kfree_skb_any(txq->tx_skbuff[j]);
+				txq->tx_skbuff[j] = NULL;
+			}
+		}
+	}
 }
 
 /*
@@ -831,15 +916,21 @@ fec_restart(struct net_device *ndev)
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
-	int i;
 	u32 val;
 	u32 temp_mac[2];
 	u32 rcntl = OPT_FRAME_SIZE | 0x04;
 	u32 ecntl = 0x2; /* ETHEREN */
 
-	/* Whack a reset.  We should wait for this. */
-	writel(1, fep->hwp + FEC_ECNTRL);
-	udelay(10);
+	/* Whack a reset.  We should wait for this.
+	 * For i.MX6SX SOC, enet use AXI bus, we use disable MAC
+	 * instead of reset MAC itself.
+	 */
+	if (id_entry && id_entry->driver_data & FEC_QUIRK_HAS_AVB) {
+		writel(0, fep->hwp + FEC_ECNTRL);
+	} else {
+		writel(1, fep->hwp + FEC_ECNTRL);
+		udelay(10);
+	}
 
 	/*
 	 * enet-mac reset will reset mac address registers too,
@@ -859,22 +950,10 @@ fec_restart(struct net_device *ndev)
 
 	fec_enet_bd_init(ndev);
 
-	/* Set receive and transmit descriptor base. */
-	writel(fep->bd_dma, fep->hwp + FEC_R_DES_START);
-	if (fep->bufdesc_ex)
-		writel((unsigned long)fep->bd_dma + sizeof(struct bufdesc_ex)
-			* fep->rx_ring_size, fep->hwp + FEC_X_DES_START);
-	else
-		writel((unsigned long)fep->bd_dma + sizeof(struct bufdesc)
-			* fep->rx_ring_size,	fep->hwp + FEC_X_DES_START);
-
+	fec_enet_enable_ring(ndev);
 
-	for (i = 0; i <= TX_RING_MOD_MASK; i++) {
-		if (fep->tx_skbuff[i]) {
-			dev_kfree_skb_any(fep->tx_skbuff[i]);
-			fep->tx_skbuff[i] = NULL;
-		}
-	}
+	/* Reset tx SKB buffers. */
+	fec_enet_reset_skb(ndev);
 
 	/* Enable MII mode */
 	if (fep->full_duplex == DUPLEX_FULL) {
@@ -996,13 +1075,17 @@ fec_restart(struct net_device *ndev)
 
 	/* And last, enable the transmit and receive processing */
 	writel(ecntl, fep->hwp + FEC_ECNTRL);
-	writel(0, fep->hwp + FEC_R_DES_ACTIVE);
+	fec_enet_active_rxring(ndev);
 
 	if (fep->bufdesc_ex)
 		fec_ptp_start_cyclecounter(ndev);
 
 	/* Enable interrupts we wish to service */
 	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
+
+	/* Init the interrupt coalescing */
+	fec_enet_itr_coal_init(ndev);
+
 }
 
 static void
@@ -1021,9 +1104,16 @@ fec_stop(struct net_device *ndev)
 			netdev_err(ndev, "Graceful transmit stop did not complete!\n");
 	}
 
-	/* Whack a reset.  We should wait for this. */
-	writel(1, fep->hwp + FEC_ECNTRL);
-	udelay(10);
+	/* Whack a reset.  We should wait for this.
+	 * For i.MX6SX SOC, enet use AXI bus, we use disable MAC
+	 * instead of reset MAC itself.
+	 */
+	if (id_entry && id_entry->driver_data & FEC_QUIRK_HAS_AVB) {
+		writel(0, fep->hwp + FEC_ECNTRL);
+	} else {
+		writel(1, fep->hwp + FEC_ECNTRL);
+		udelay(10);
+	}
 	writel(fep->phy_speed, fep->hwp + FEC_MII_SPEED);
 	writel(FEC_DEFAULT_IMASK, fep->hwp + FEC_IMASK);
 
@@ -1081,37 +1171,45 @@ fec_enet_hwtstamp(struct fec_enet_private *fep, unsigned ts,
 }
 
 static void
-fec_enet_tx(struct net_device *ndev)
+fec_enet_tx_queue(struct net_device *ndev, u16 queue_id)
 {
 	struct	fec_enet_private *fep;
 	struct bufdesc *bdp;
 	unsigned short status;
 	struct	sk_buff	*skb;
+	struct fec_enet_priv_tx_q *txq;
+	struct netdev_queue *nq;
 	int	index = 0;
 	int	entries_free;
 
 	fep = netdev_priv(ndev);
-	bdp = fep->dirty_tx;
+
+	queue_id = FEC_ENET_GET_QUQUE(queue_id);
+
+	txq = fep->tx_queue[queue_id];
+	/* get next bdp of dirty_tx */
+	nq = netdev_get_tx_queue(ndev, queue_id);
+	bdp = txq->dirty_tx;
 
 	/* get next bdp of dirty_tx */
-	bdp = fec_enet_get_nextdesc(bdp, fep);
+	bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
 
 	while (((status = bdp->cbd_sc) & BD_ENET_TX_READY) == 0) {
 
 		/* current queue is empty */
-		if (bdp == fep->cur_tx)
+		if (bdp == txq->cur_tx)
 			break;
 
-		index = fec_enet_get_bd_index(fep->tx_bd_base, bdp, fep);
+		index = fec_enet_get_bd_index(txq->tx_bd_base, bdp, fep);
 
-		skb = fep->tx_skbuff[index];
-		fep->tx_skbuff[index] = NULL;
-		if (!IS_TSO_HEADER(fep, bdp->cbd_bufaddr))
+		skb = txq->tx_skbuff[index];
+		txq->tx_skbuff[index] = NULL;
+		if (!IS_TSO_HEADER(txq, bdp->cbd_bufaddr))
 			dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
 					bdp->cbd_datlen, DMA_TO_DEVICE);
 		bdp->cbd_bufaddr = 0;
 		if (!skb) {
-			bdp = fec_enet_get_nextdesc(bdp, fep);
+			bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
 			continue;
 		}
 
@@ -1153,23 +1251,81 @@ fec_enet_tx(struct net_device *ndev)
 		/* Free the sk buffer associated with this last transmit */
 		dev_kfree_skb_any(skb);
 
-		fep->dirty_tx = bdp;
+		txq->dirty_tx = bdp;
 
 		/* Update pointer to next buffer descriptor to be transmitted */
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+		bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
 
 		/* Since we have freed up a buffer, the ring is no longer full
 		 */
 		if (netif_queue_stopped(ndev)) {
-			entries_free = fec_enet_get_free_txdesc_num(fep);
-			if (entries_free >= fep->tx_wake_threshold)
-				netif_wake_queue(ndev);
+			entries_free = fec_enet_get_free_txdesc_num(fep, txq);
+			if (entries_free >= txq->tx_wake_threshold)
+				netif_tx_wake_queue(nq);
 		}
 	}
 
 	/* ERR006538: Keep the transmitter going */
-	if (bdp != fep->cur_tx && readl(fep->hwp + FEC_X_DES_ACTIVE) == 0)
-		writel(0, fep->hwp + FEC_X_DES_ACTIVE);
+	if (bdp != txq->cur_tx &&
+	    readl(fep->hwp + FEC_X_DES_ACTIVE(queue_id)) == 0)
+		writel(0, fep->hwp + FEC_X_DES_ACTIVE(queue_id));
+}
+
+static void
+fec_enet_tx(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	u16 queue_id;
+	/* First process class A queue, then Class B and Best Effort queue */
+	for_each_set_bit(queue_id, &fep->work_tx, FEC_ENET_MAX_TX_QS) {
+		clear_bit(queue_id, &fep->work_tx);
+		fec_enet_tx_queue(ndev, queue_id);
+	}
+	return;
+}
+
+static int
+fec_enet_new_rxbdp(struct net_device *ndev, struct bufdesc *bdp, struct sk_buff *skb)
+{
+	struct  fec_enet_private *fep = netdev_priv(ndev);
+	int off;
+
+	off = ((unsigned long)skb->data) & fep->rx_align;
+	if (off)
+		skb_reserve(skb, fep->rx_align + 1 - off);
+
+	bdp->cbd_bufaddr = dma_map_single(&fep->pdev->dev, skb->data,
+					  FEC_ENET_RX_FRSIZE - fep->rx_align,
+					  DMA_FROM_DEVICE);
+	if (dma_mapping_error(&fep->pdev->dev, bdp->cbd_bufaddr)) {
+		if (net_ratelimit())
+			netdev_err(ndev, "Rx DMA memory map failed\n");
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static bool fec_enet_copybreak(struct net_device *ndev, struct sk_buff **skb,
+			       struct bufdesc *bdp, u32 length)
+{
+	struct  fec_enet_private *fep = netdev_priv(ndev);
+	struct sk_buff *new_skb;
+
+	if (length > fep->rx_copybreak)
+		return false;
+
+	new_skb = netdev_alloc_skb(ndev, length);
+	if (!new_skb)
+		return false;
+
+	dma_sync_single_for_cpu(&fep->pdev->dev, bdp->cbd_bufaddr,
+				FEC_ENET_RX_FRSIZE - fep->rx_align,
+				DMA_FROM_DEVICE);
+	memcpy(new_skb->data, (*skb)->data, length);
+	*skb = new_skb;
+
+	return true;
 }
 
 /* During a receive, the cur_rx points to the current incoming buffer.
@@ -1178,14 +1334,16 @@ fec_enet_tx(struct net_device *ndev)
  * effectively tossing the packet.
  */
 static int
-fec_enet_rx(struct net_device *ndev, int budget)
+fec_enet_rx_queue(struct net_device *ndev, int budget, u16 queue_id)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
+	struct fec_enet_priv_rx_q *rxq;
 	struct bufdesc *bdp;
 	unsigned short status;
-	struct	sk_buff	*skb;
+	struct  sk_buff *skb_new = NULL;
+	struct  sk_buff *skb;
 	ushort	pkt_len;
 	__u8 *data;
 	int	pkt_received = 0;
@@ -1193,15 +1351,18 @@ fec_enet_rx(struct net_device *ndev, int budget)
 	bool	vlan_packet_rcvd = false;
 	u16	vlan_tag;
 	int	index = 0;
+	bool	is_copybreak;
 
 #ifdef CONFIG_M532x
 	flush_cache_all();
 #endif
+	queue_id = FEC_ENET_GET_QUQUE(queue_id);
+	rxq = fep->rx_queue[queue_id];
 
 	/* First, grab all of the stats for the incoming packet.
 	 * These get messed up if we get called due to a busy condition.
 	 */
-	bdp = fep->cur_rx;
+	bdp = rxq->cur_rx;
 
 	while (!((status = bdp->cbd_sc) & BD_ENET_RX_EMPTY)) {
 
@@ -1215,7 +1376,6 @@ fec_enet_rx(struct net_device *ndev, int budget)
 		if ((status & BD_ENET_RX_LAST) == 0)
 			netdev_err(ndev, "rcv is not +last\n");
 
-		writel(FEC_ENET_RXF, fep->hwp + FEC_IEVENT);
 
 		/* Check for errors. */
 		if (status & (BD_ENET_RX_LG | BD_ENET_RX_SH | BD_ENET_RX_NO |
@@ -1248,11 +1408,28 @@ fec_enet_rx(struct net_device *ndev, int budget)
 		pkt_len = bdp->cbd_datlen;
 		ndev->stats.rx_bytes += pkt_len;
 
-		index = fec_enet_get_bd_index(fep->rx_bd_base, bdp, fep);
-		data = fep->rx_skbuff[index]->data;
-		dma_sync_single_for_cpu(&fep->pdev->dev, bdp->cbd_bufaddr,
-					FEC_ENET_RX_FRSIZE, DMA_FROM_DEVICE);
+		index = fec_enet_get_bd_index(rxq->rx_bd_base, bdp, fep);
+		skb = rxq->rx_skbuff[index];
+
+		/* The packet length includes FCS, but we don't want to
+		 * include that when passing upstream as it messes up
+		 * bridging applications.
+		 */
+		is_copybreak = fec_enet_copybreak(ndev, &skb, bdp, pkt_len - 4);
+		if (!is_copybreak) {
+			skb_new = netdev_alloc_skb(ndev, FEC_ENET_RX_FRSIZE);
+			if (unlikely(!skb_new)) {
+				ndev->stats.rx_dropped++;
+				goto rx_processing_done;
+			}
+			dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
+					 FEC_ENET_RX_FRSIZE - fep->rx_align,
+					 DMA_FROM_DEVICE);
+		}
 
+		prefetch(skb->data - NET_IP_ALIGN);
+		skb_put(skb, pkt_len - 4);
+		data = skb->data;
 		if (id_entry->driver_data & FEC_QUIRK_SWAP_FRAME)
 			swap_buffer(data, pkt_len);
 
@@ -1264,66 +1441,53 @@ fec_enet_rx(struct net_device *ndev, int budget)
 		/* If this is a VLAN packet remove the VLAN Tag */
 		vlan_packet_rcvd = false;
 		if ((ndev->features & NETIF_F_HW_VLAN_CTAG_RX) &&
-		    fep->bufdesc_ex && (ebdp->cbd_esc & BD_ENET_RX_VLAN)) {
+			fep->bufdesc_ex && (ebdp->cbd_esc & BD_ENET_RX_VLAN)) {
 			/* Push and remove the vlan tag */
 			struct vlan_hdr *vlan_header =
 					(struct vlan_hdr *) (data + ETH_HLEN);
 			vlan_tag = ntohs(vlan_header->h_vlan_TCI);
-			pkt_len -= VLAN_HLEN;
 
 			vlan_packet_rcvd = true;
+
+			skb_copy_to_linear_data_offset(skb, VLAN_HLEN,
+						       data, (2 * ETH_ALEN));
+			skb_pull(skb, VLAN_HLEN);
 		}
 
-		/* This does 16 byte alignment, exactly what we need.
-		 * The packet length includes FCS, but we don't want to
-		 * include that when passing upstream as it messes up
-		 * bridging applications.
-		 */
-		skb = netdev_alloc_skb(ndev, pkt_len - 4 + NET_IP_ALIGN);
+		skb->protocol = eth_type_trans(skb, ndev);
 
-		if (unlikely(!skb)) {
-			ndev->stats.rx_dropped++;
-		} else {
-			int payload_offset = (2 * ETH_ALEN);
-			skb_reserve(skb, NET_IP_ALIGN);
-			skb_put(skb, pkt_len - 4);	/* Make room */
-
-			/* Extract the frame data without the VLAN header. */
-			skb_copy_to_linear_data(skb, data, (2 * ETH_ALEN));
-			if (vlan_packet_rcvd)
-				payload_offset = (2 * ETH_ALEN) + VLAN_HLEN;
-			skb_copy_to_linear_data_offset(skb, (2 * ETH_ALEN),
-						       data + payload_offset,
-						       pkt_len - 4 - (2 * ETH_ALEN));
-
-			skb->protocol = eth_type_trans(skb, ndev);
-
-			/* Get receive timestamp from the skb */
-			if (fep->hwts_rx_en && fep->bufdesc_ex)
-				fec_enet_hwtstamp(fep, ebdp->ts,
-						  skb_hwtstamps(skb));
-
-			if (fep->bufdesc_ex &&
-			    (fep->csum_flags & FLAG_RX_CSUM_ENABLED)) {
-				if (!(ebdp->cbd_esc & FLAG_RX_CSUM_ERROR)) {
-					/* don't check it */
-					skb->ip_summed = CHECKSUM_UNNECESSARY;
-				} else {
-					skb_checksum_none_assert(skb);
-				}
+		/* Get receive timestamp from the skb */
+		if (fep->hwts_rx_en && fep->bufdesc_ex)
+			fec_enet_hwtstamp(fep, ebdp->ts,
+					  skb_hwtstamps(skb));
+
+		if (fep->bufdesc_ex &&
+		    (fep->csum_flags & FLAG_RX_CSUM_ENABLED)) {
+			if (!(ebdp->cbd_esc & FLAG_RX_CSUM_ERROR)) {
+				/* don't check it */
+				skb->ip_summed = CHECKSUM_UNNECESSARY;
+			} else {
+				skb_checksum_none_assert(skb);
 			}
+		}
 
-			/* Handle received VLAN packets */
-			if (vlan_packet_rcvd)
-				__vlan_hwaccel_put_tag(skb,
-						       htons(ETH_P_8021Q),
-						       vlan_tag);
+		/* Handle received VLAN packets */
+		if (vlan_packet_rcvd)
+			__vlan_hwaccel_put_tag(skb,
+					       htons(ETH_P_8021Q),
+					       vlan_tag);
 
-			napi_gro_receive(&fep->napi, skb);
+		napi_gro_receive(&fep->napi, skb);
+
+		if (is_copybreak) {
+			dma_sync_single_for_device(&fep->pdev->dev, bdp->cbd_bufaddr,
+						   FEC_ENET_RX_FRSIZE - fep->rx_align,
+						   DMA_FROM_DEVICE);
+		} else {
+			rxq->rx_skbuff[index] = skb_new;
+			fec_enet_new_rxbdp(ndev, bdp, skb_new);
 		}
 
-		dma_sync_single_for_device(&fep->pdev->dev, bdp->cbd_bufaddr,
-					FEC_ENET_RX_FRSIZE, DMA_FROM_DEVICE);
 rx_processing_done:
 		/* Clear the status flags for this buffer */
 		status &= ~BD_ENET_RX_STATS;
@@ -1341,19 +1505,56 @@ rx_processing_done:
 		}
 
 		/* Update BD pointer to next entry */
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+		bdp = fec_enet_get_nextdesc(bdp, fep, queue_id);
 
 		/* Doing this here will keep the FEC running while we process
 		 * incoming frames.  On a heavily loaded network, we should be
 		 * able to keep up at the expense of system resources.
 		 */
-		writel(0, fep->hwp + FEC_R_DES_ACTIVE);
+		writel(0, fep->hwp + FEC_R_DES_ACTIVE(queue_id));
 	}
-	fep->cur_rx = bdp;
+	rxq->cur_rx = bdp;
+	return pkt_received;
+}
 
+static int
+fec_enet_rx(struct net_device *ndev, int budget)
+{
+	int     pkt_received = 0;
+	u16	queue_id;
+	struct fec_enet_private *fep = netdev_priv(ndev);
+
+	for_each_set_bit(queue_id, &fep->work_rx, FEC_ENET_MAX_RX_QS) {
+		clear_bit(queue_id, &fep->work_rx);
+		pkt_received += fec_enet_rx_queue(ndev,
+					budget - pkt_received, queue_id);
+	}
 	return pkt_received;
 }
 
+static bool
+fec_enet_collect_events(struct fec_enet_private *fep, uint int_events)
+{
+	if (int_events == 0)
+		return false;
+
+	if (int_events & FEC_ENET_RXF)
+		fep->work_rx |= (1 << 2);
+	if (int_events & FEC_ENET_RXF_1)
+		fep->work_rx |= (1 << 0);
+	if (int_events & FEC_ENET_RXF_2)
+		fep->work_rx |= (1 << 1);
+
+	if (int_events & FEC_ENET_TXF)
+		fep->work_tx |= (1 << 2);
+	if (int_events & FEC_ENET_TXF_1)
+		fep->work_tx |= (1 << 0);
+	if (int_events & FEC_ENET_TXF_2)
+		fep->work_tx |= (1 << 1);
+
+	return true;
+}
+
 static irqreturn_t
 fec_enet_interrupt(int irq, void *dev_id)
 {
@@ -1365,6 +1566,7 @@ fec_enet_interrupt(int irq, void *dev_id)
 
 	int_events = readl(fep->hwp + FEC_IEVENT);
 	writel(int_events & ~napi_mask, fep->hwp + FEC_IEVENT);
+	fec_enet_collect_events(fep, int_events);
 
 	if (int_events & napi_mask) {
 		ret = IRQ_HANDLED;
@@ -1379,6 +1581,8 @@ fec_enet_interrupt(int irq, void *dev_id)
 		complete(&fep->mdio_done);
 	}
 
+	fec_ptp_check_pps_event(fep);
+
 	return ret;
 }
 
@@ -1611,20 +1815,41 @@ static int fec_enet_clk_enable(struct net_device *ndev, bool enable)
 				goto failed_clk_enet_out;
 		}
 		if (fep->clk_ptp) {
+			mutex_lock(&fep->ptp_clk_mutex);
 			ret = clk_prepare_enable(fep->clk_ptp);
-			if (ret)
+			if (ret) {
+				mutex_unlock(&fep->ptp_clk_mutex);
 				goto failed_clk_ptp;
+			} else {
+				fep->ptp_clk_on = true;
+			}
+			mutex_unlock(&fep->ptp_clk_mutex);
+		}
+		if (fep->clk_ref) {
+			ret = clk_prepare_enable(fep->clk_ref);
+			if (ret)
+				goto failed_clk_ref;
 		}
 	} else {
 		clk_disable_unprepare(fep->clk_ahb);
 		clk_disable_unprepare(fep->clk_ipg);
 		if (fep->clk_enet_out)
 			clk_disable_unprepare(fep->clk_enet_out);
-		if (fep->clk_ptp)
+		if (fep->clk_ptp) {
+			mutex_lock(&fep->ptp_clk_mutex);
 			clk_disable_unprepare(fep->clk_ptp);
+			fep->ptp_clk_on = false;
+			mutex_unlock(&fep->ptp_clk_mutex);
+		}
+		if (fep->clk_ref)
+			clk_disable_unprepare(fep->clk_ref);
 	}
 
 	return 0;
+
+failed_clk_ref:
+	if (fep->clk_ref)
+		clk_disable_unprepare(fep->clk_ref);
 failed_clk_ptp:
 	if (fep->clk_enet_out)
 		clk_disable_unprepare(fep->clk_enet_out);
@@ -1664,13 +1889,13 @@ static int fec_enet_mii_probe(struct net_device *ndev)
 				continue;
 			if (dev_id--)
 				continue;
-			strncpy(mdio_bus_id, fep->mii_bus->id, MII_BUS_ID_SIZE);
+			strlcpy(mdio_bus_id, fep->mii_bus->id, MII_BUS_ID_SIZE);
 			break;
 		}
 
 		if (phy_id >= PHY_MAX_ADDR) {
 			netdev_info(ndev, "no PHY, assuming direct connection to switch\n");
-			strncpy(mdio_bus_id, "fixed-0", MII_BUS_ID_SIZE);
+			strlcpy(mdio_bus_id, "fixed-0", MII_BUS_ID_SIZE);
 			phy_id = 0;
 		}
 
@@ -2052,12 +2277,179 @@ static int fec_enet_nway_reset(struct net_device *dev)
 	return genphy_restart_aneg(phydev);
 }
 
+/* ITR clock source is enet system clock (clk_ahb).
+ * TCTT unit is cycle_ns * 64 cycle
+ * So, the ICTT value = X us / (cycle_ns * 64)
+ */
+static int fec_enet_us_to_itr_clock(struct net_device *ndev, int us)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+
+	return us * (fep->itr_clk_rate / 64000) / 1000;
+}
+
+/* Set threshold for interrupt coalescing */
+static void fec_enet_itr_coal_set(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	const struct platform_device_id *id_entry =
+				platform_get_device_id(fep->pdev);
+	int rx_itr, tx_itr;
+
+	if (!(id_entry->driver_data & FEC_QUIRK_HAS_AVB))
+		return;
+
+	/* Must be greater than zero to avoid unpredictable behavior */
+	if (!fep->rx_time_itr || !fep->rx_pkts_itr ||
+	    !fep->tx_time_itr || !fep->tx_pkts_itr)
+		return;
+
+	/* Select enet system clock as Interrupt Coalescing
+	 * timer Clock Source
+	 */
+	rx_itr = FEC_ITR_CLK_SEL;
+	tx_itr = FEC_ITR_CLK_SEL;
+
+	/* set ICFT and ICTT */
+	rx_itr |= FEC_ITR_ICFT(fep->rx_pkts_itr);
+	rx_itr |= FEC_ITR_ICTT(fec_enet_us_to_itr_clock(ndev, fep->rx_time_itr));
+	tx_itr |= FEC_ITR_ICFT(fep->tx_pkts_itr);
+	tx_itr |= FEC_ITR_ICTT(fec_enet_us_to_itr_clock(ndev, fep->tx_time_itr));
+
+	rx_itr |= FEC_ITR_EN;
+	tx_itr |= FEC_ITR_EN;
+
+	writel(tx_itr, fep->hwp + FEC_TXIC0);
+	writel(rx_itr, fep->hwp + FEC_RXIC0);
+	writel(tx_itr, fep->hwp + FEC_TXIC1);
+	writel(rx_itr, fep->hwp + FEC_RXIC1);
+	writel(tx_itr, fep->hwp + FEC_TXIC2);
+	writel(rx_itr, fep->hwp + FEC_RXIC2);
+}
+
+static int
+fec_enet_get_coalesce(struct net_device *ndev, struct ethtool_coalesce *ec)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	const struct platform_device_id *id_entry =
+				platform_get_device_id(fep->pdev);
+
+	if (!(id_entry->driver_data & FEC_QUIRK_HAS_AVB))
+		return -EOPNOTSUPP;
+
+	ec->rx_coalesce_usecs = fep->rx_time_itr;
+	ec->rx_max_coalesced_frames = fep->rx_pkts_itr;
+
+	ec->tx_coalesce_usecs = fep->tx_time_itr;
+	ec->tx_max_coalesced_frames = fep->tx_pkts_itr;
+
+	return 0;
+}
+
+static int
+fec_enet_set_coalesce(struct net_device *ndev, struct ethtool_coalesce *ec)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	const struct platform_device_id *id_entry =
+				platform_get_device_id(fep->pdev);
+
+	unsigned int cycle;
+
+	if (!(id_entry->driver_data & FEC_QUIRK_HAS_AVB))
+		return -EOPNOTSUPP;
+
+	if (ec->rx_max_coalesced_frames > 255) {
+		pr_err("Rx coalesced frames exceed hardware limiation");
+		return -EINVAL;
+	}
+
+	if (ec->tx_max_coalesced_frames > 255) {
+		pr_err("Tx coalesced frame exceed hardware limiation");
+		return -EINVAL;
+	}
+
+	cycle = fec_enet_us_to_itr_clock(ndev, fep->rx_time_itr);
+	if (cycle > 0xFFFF) {
+		pr_err("Rx coalesed usec exceeed hardware limiation");
+		return -EINVAL;
+	}
+
+	cycle = fec_enet_us_to_itr_clock(ndev, fep->tx_time_itr);
+	if (cycle > 0xFFFF) {
+		pr_err("Rx coalesed usec exceeed hardware limiation");
+		return -EINVAL;
+	}
+
+	fep->rx_time_itr = ec->rx_coalesce_usecs;
+	fep->rx_pkts_itr = ec->rx_max_coalesced_frames;
+
+	fep->tx_time_itr = ec->tx_coalesce_usecs;
+	fep->tx_pkts_itr = ec->tx_max_coalesced_frames;
+
+	fec_enet_itr_coal_set(ndev);
+
+	return 0;
+}
+
+static void fec_enet_itr_coal_init(struct net_device *ndev)
+{
+	struct ethtool_coalesce ec;
+
+	ec.rx_coalesce_usecs = FEC_ITR_ICTT_DEFAULT;
+	ec.rx_max_coalesced_frames = FEC_ITR_ICFT_DEFAULT;
+
+	ec.tx_coalesce_usecs = FEC_ITR_ICTT_DEFAULT;
+	ec.tx_max_coalesced_frames = FEC_ITR_ICFT_DEFAULT;
+
+	fec_enet_set_coalesce(ndev, &ec);
+}
+
+static int fec_enet_get_tunable(struct net_device *netdev,
+				const struct ethtool_tunable *tuna,
+				void *data)
+{
+	struct fec_enet_private *fep = netdev_priv(netdev);
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		*(u32 *)data = fep->rx_copybreak;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int fec_enet_set_tunable(struct net_device *netdev,
+				const struct ethtool_tunable *tuna,
+				const void *data)
+{
+	struct fec_enet_private *fep = netdev_priv(netdev);
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_RX_COPYBREAK:
+		fep->rx_copybreak = *(u32 *)data;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
 static const struct ethtool_ops fec_enet_ethtool_ops = {
 	.get_settings		= fec_enet_get_settings,
 	.set_settings		= fec_enet_set_settings,
 	.get_drvinfo		= fec_enet_get_drvinfo,
 	.nway_reset		= fec_enet_nway_reset,
 	.get_link		= ethtool_op_get_link,
+	.get_coalesce		= fec_enet_get_coalesce,
+	.set_coalesce		= fec_enet_set_coalesce,
 #ifndef CONFIG_M5272
 	.get_pauseparam		= fec_enet_get_pauseparam,
 	.set_pauseparam		= fec_enet_set_pauseparam,
@@ -2066,6 +2458,8 @@ static const struct ethtool_ops fec_enet_ethtool_ops = {
 	.get_sset_count		= fec_enet_get_sset_count,
 #endif
 	.get_ts_info		= fec_enet_get_ts_info,
+	.get_tunable		= fec_enet_get_tunable,
+	.set_tunable		= fec_enet_set_tunable,
 };
 
 static int fec_enet_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
@@ -2095,55 +2489,136 @@ static void fec_enet_free_buffers(struct net_device *ndev)
 	unsigned int i;
 	struct sk_buff *skb;
 	struct bufdesc	*bdp;
+	struct fec_enet_priv_tx_q *txq;
+	struct fec_enet_priv_rx_q *rxq;
+	unsigned int q;
+
+	for (q = 0; q < fep->num_rx_queues; q++) {
+		rxq = fep->rx_queue[q];
+		bdp = rxq->rx_bd_base;
+		for (i = 0; i < rxq->rx_ring_size; i++) {
+			skb = rxq->rx_skbuff[i];
+			rxq->rx_skbuff[i] = NULL;
+			if (skb) {
+				dma_unmap_single(&fep->pdev->dev,
+						 bdp->cbd_bufaddr,
+						 FEC_ENET_RX_FRSIZE - fep->rx_align,
+						 DMA_FROM_DEVICE);
+				dev_kfree_skb(skb);
+			}
+			bdp = fec_enet_get_nextdesc(bdp, fep, q);
+		}
+	}
 
-	bdp = fep->rx_bd_base;
-	for (i = 0; i < fep->rx_ring_size; i++) {
-		skb = fep->rx_skbuff[i];
-		fep->rx_skbuff[i] = NULL;
-		if (skb) {
-			dma_unmap_single(&fep->pdev->dev, bdp->cbd_bufaddr,
-					FEC_ENET_RX_FRSIZE, DMA_FROM_DEVICE);
+	for (q = 0; q < fep->num_tx_queues; q++) {
+		txq = fep->tx_queue[q];
+		bdp = txq->tx_bd_base;
+		for (i = 0; i < txq->tx_ring_size; i++) {
+			kfree(txq->tx_bounce[i]);
+			txq->tx_bounce[i] = NULL;
+			skb = txq->tx_skbuff[i];
+			txq->tx_skbuff[i] = NULL;
 			dev_kfree_skb(skb);
 		}
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+	}
+}
+
+static void fec_enet_free_queue(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	int i;
+	struct fec_enet_priv_tx_q *txq;
+
+	for (i = 0; i < fep->num_tx_queues; i++)
+		if (fep->tx_queue[i] && fep->tx_queue[i]->tso_hdrs) {
+			txq = fep->tx_queue[i];
+			dma_free_coherent(NULL,
+					  txq->tx_ring_size * TSO_HEADER_SIZE,
+					  txq->tso_hdrs,
+					  txq->tso_hdrs_dma);
+		}
+
+	for (i = 0; i < fep->num_rx_queues; i++)
+		if (fep->rx_queue[i])
+			kfree(fep->rx_queue[i]);
+
+	for (i = 0; i < fep->num_tx_queues; i++)
+		if (fep->tx_queue[i])
+			kfree(fep->tx_queue[i]);
+}
+
+static int fec_enet_alloc_queue(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	int i;
+	int ret = 0;
+	struct fec_enet_priv_tx_q *txq;
+
+	for (i = 0; i < fep->num_tx_queues; i++) {
+		txq = kzalloc(sizeof(*txq), GFP_KERNEL);
+		if (!txq) {
+			ret = -ENOMEM;
+			goto alloc_failed;
+		}
+
+		fep->tx_queue[i] = txq;
+		txq->tx_ring_size = TX_RING_SIZE;
+		fep->total_tx_ring_size += fep->tx_queue[i]->tx_ring_size;
+
+		txq->tx_stop_threshold = FEC_MAX_SKB_DESCS;
+		txq->tx_wake_threshold =
+				(txq->tx_ring_size - txq->tx_stop_threshold) / 2;
+
+		txq->tso_hdrs = dma_alloc_coherent(NULL,
+					txq->tx_ring_size * TSO_HEADER_SIZE,
+					&txq->tso_hdrs_dma,
+					GFP_KERNEL);
+		if (!txq->tso_hdrs) {
+			ret = -ENOMEM;
+			goto alloc_failed;
+		}
 	}
 
-	bdp = fep->tx_bd_base;
-	for (i = 0; i < fep->tx_ring_size; i++) {
-		kfree(fep->tx_bounce[i]);
-		fep->tx_bounce[i] = NULL;
-		skb = fep->tx_skbuff[i];
-		fep->tx_skbuff[i] = NULL;
-		dev_kfree_skb(skb);
+	for (i = 0; i < fep->num_rx_queues; i++) {
+		fep->rx_queue[i] = kzalloc(sizeof(*fep->rx_queue[i]),
+					   GFP_KERNEL);
+		if (!fep->rx_queue[i]) {
+			ret = -ENOMEM;
+			goto alloc_failed;
+		}
+
+		fep->rx_queue[i]->rx_ring_size = RX_RING_SIZE;
+		fep->total_rx_ring_size += fep->rx_queue[i]->rx_ring_size;
 	}
+	return ret;
+
+alloc_failed:
+	fec_enet_free_queue(ndev);
+	return ret;
 }
 
-static int fec_enet_alloc_buffers(struct net_device *ndev)
+static int
+fec_enet_alloc_rxq_buffers(struct net_device *ndev, unsigned int queue)
 {
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	unsigned int i;
 	struct sk_buff *skb;
 	struct bufdesc	*bdp;
+	struct fec_enet_priv_rx_q *rxq;
 
-	bdp = fep->rx_bd_base;
-	for (i = 0; i < fep->rx_ring_size; i++) {
-		dma_addr_t addr;
-
+	rxq = fep->rx_queue[queue];
+	bdp = rxq->rx_bd_base;
+	for (i = 0; i < rxq->rx_ring_size; i++) {
 		skb = netdev_alloc_skb(ndev, FEC_ENET_RX_FRSIZE);
 		if (!skb)
 			goto err_alloc;
 
-		addr = dma_map_single(&fep->pdev->dev, skb->data,
-				FEC_ENET_RX_FRSIZE, DMA_FROM_DEVICE);
-		if (dma_mapping_error(&fep->pdev->dev, addr)) {
+		if (fec_enet_new_rxbdp(ndev, bdp, skb)) {
 			dev_kfree_skb(skb);
-			if (net_ratelimit())
-				netdev_err(ndev, "Rx DMA memory map failed\n");
 			goto err_alloc;
 		}
 
-		fep->rx_skbuff[i] = skb;
-		bdp->cbd_bufaddr = addr;
+		rxq->rx_skbuff[i] = skb;
 		bdp->cbd_sc = BD_ENET_RX_EMPTY;
 
 		if (fep->bufdesc_ex) {
@@ -2151,17 +2626,32 @@ static int fec_enet_alloc_buffers(struct net_device *ndev)
 			ebdp->cbd_esc = BD_ENET_RX_INT;
 		}
 
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
 	}
 
 	/* Set the last buffer to wrap. */
-	bdp = fec_enet_get_prevdesc(bdp, fep);
+	bdp = fec_enet_get_prevdesc(bdp, fep, queue);
 	bdp->cbd_sc |= BD_SC_WRAP;
+	return 0;
+
+ err_alloc:
+	fec_enet_free_buffers(ndev);
+	return -ENOMEM;
+}
 
-	bdp = fep->tx_bd_base;
-	for (i = 0; i < fep->tx_ring_size; i++) {
-		fep->tx_bounce[i] = kmalloc(FEC_ENET_TX_FRSIZE, GFP_KERNEL);
-		if (!fep->tx_bounce[i])
+static int
+fec_enet_alloc_txq_buffers(struct net_device *ndev, unsigned int queue)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	unsigned int i;
+	struct bufdesc  *bdp;
+	struct fec_enet_priv_tx_q *txq;
+
+	txq = fep->tx_queue[queue];
+	bdp = txq->tx_bd_base;
+	for (i = 0; i < txq->tx_ring_size; i++) {
+		txq->tx_bounce[i] = kmalloc(FEC_ENET_TX_FRSIZE, GFP_KERNEL);
+		if (!txq->tx_bounce[i])
 			goto err_alloc;
 
 		bdp->cbd_sc = 0;
@@ -2172,11 +2662,11 @@ static int fec_enet_alloc_buffers(struct net_device *ndev)
 			ebdp->cbd_esc = BD_ENET_TX_INT;
 		}
 
-		bdp = fec_enet_get_nextdesc(bdp, fep);
+		bdp = fec_enet_get_nextdesc(bdp, fep, queue);
 	}
 
 	/* Set the last buffer to wrap. */
-	bdp = fec_enet_get_prevdesc(bdp, fep);
+	bdp = fec_enet_get_prevdesc(bdp, fep, queue);
 	bdp->cbd_sc |= BD_SC_WRAP;
 
 	return 0;
@@ -2186,6 +2676,21 @@ static int fec_enet_alloc_buffers(struct net_device *ndev)
 	return -ENOMEM;
 }
 
+static int fec_enet_alloc_buffers(struct net_device *ndev)
+{
+	struct fec_enet_private *fep = netdev_priv(ndev);
+	unsigned int i;
+
+	for (i = 0; i < fep->num_rx_queues; i++)
+		if (fec_enet_alloc_rxq_buffers(ndev, i))
+			return -ENOMEM;
+
+	for (i = 0; i < fep->num_tx_queues; i++)
+		if (fec_enet_alloc_txq_buffers(ndev, i))
+			return -ENOMEM;
+	return 0;
+}
+
 static int
 fec_enet_open(struct net_device *ndev)
 {
@@ -2203,20 +2708,26 @@ fec_enet_open(struct net_device *ndev)
 
 	ret = fec_enet_alloc_buffers(ndev);
 	if (ret)
-		return ret;
+		goto err_enet_alloc;
 
 	/* Probe and connect to PHY when open the interface */
 	ret = fec_enet_mii_probe(ndev);
-	if (ret) {
-		fec_enet_free_buffers(ndev);
-		return ret;
-	}
+	if (ret)
+		goto err_enet_mii_probe;
 
 	fec_restart(ndev);
 	napi_enable(&fep->napi);
 	phy_start(fep->phy_dev);
-	netif_start_queue(ndev);
+	netif_tx_start_all_queues(ndev);
+
 	return 0;
+
+err_enet_mii_probe:
+	fec_enet_free_buffers(ndev);
+err_enet_alloc:
+	fec_enet_clk_enable(ndev, false);
+	pinctrl_pm_select_sleep_state(&fep->pdev->dev);
+	return ret;
 }
 
 static int
@@ -2362,20 +2873,12 @@ static void fec_poll_controller(struct net_device *dev)
 #endif
 
 #define FEATURES_NEED_QUIESCE NETIF_F_RXCSUM
-
-static int fec_set_features(struct net_device *netdev,
+static inline void fec_enet_set_netdev_features(struct net_device *netdev,
 	netdev_features_t features)
 {
 	struct fec_enet_private *fep = netdev_priv(netdev);
 	netdev_features_t changed = features ^ netdev->features;
 
-	/* Quiesce the device if necessary */
-	if (netif_running(netdev) && changed & FEATURES_NEED_QUIESCE) {
-		napi_disable(&fep->napi);
-		netif_tx_lock_bh(netdev);
-		fec_stop(netdev);
-	}
-
 	netdev->features = features;
 
 	/* Receive checksum has been changed */
@@ -2385,13 +2888,25 @@ static int fec_set_features(struct net_device *netdev,
 		else
 			fep->csum_flags &= ~FLAG_RX_CSUM_ENABLED;
 	}
+}
+
+static int fec_set_features(struct net_device *netdev,
+	netdev_features_t features)
+{
+	struct fec_enet_private *fep = netdev_priv(netdev);
+	netdev_features_t changed = features ^ netdev->features;
 
-	/* Resume the device after updates */
 	if (netif_running(netdev) && changed & FEATURES_NEED_QUIESCE) {
+		napi_disable(&fep->napi);
+		netif_tx_lock_bh(netdev);
+		fec_stop(netdev);
+		fec_enet_set_netdev_features(netdev, features);
 		fec_restart(netdev);
-		netif_wake_queue(netdev);
+		netif_tx_wake_all_queues(netdev);
 		netif_tx_unlock_bh(netdev);
 		napi_enable(&fep->napi);
+	} else {
+		fec_enet_set_netdev_features(netdev, features);
 	}
 
 	return 0;
@@ -2422,39 +2937,38 @@ static int fec_enet_init(struct net_device *ndev)
 	struct fec_enet_private *fep = netdev_priv(ndev);
 	const struct platform_device_id *id_entry =
 				platform_get_device_id(fep->pdev);
+	struct fec_enet_priv_tx_q *txq;
+	struct fec_enet_priv_rx_q *rxq;
 	struct bufdesc *cbd_base;
+	dma_addr_t bd_dma;
 	int bd_size;
+	unsigned int i;
 
-	/* init the tx & rx ring size */
-	fep->tx_ring_size = TX_RING_SIZE;
-	fep->rx_ring_size = RX_RING_SIZE;
+#if defined(CONFIG_ARM)
+	fep->rx_align = 0xf;
+	fep->tx_align = 0xf;
+#else
+	fep->rx_align = 0x3;
+	fep->tx_align = 0x3;
+#endif
 
-	fep->tx_stop_threshold = FEC_MAX_SKB_DESCS;
-	fep->tx_wake_threshold = (fep->tx_ring_size - fep->tx_stop_threshold) / 2;
+	fec_enet_alloc_queue(ndev);
 
 	if (fep->bufdesc_ex)
 		fep->bufdesc_size = sizeof(struct bufdesc_ex);
 	else
 		fep->bufdesc_size = sizeof(struct bufdesc);
-	bd_size = (fep->tx_ring_size + fep->rx_ring_size) *
+	bd_size = (fep->total_tx_ring_size + fep->total_rx_ring_size) *
 			fep->bufdesc_size;
 
 	/* Allocate memory for buffer descriptors. */
-	cbd_base = dma_alloc_coherent(NULL, bd_size, &fep->bd_dma,
+	cbd_base = dma_alloc_coherent(NULL, bd_size, &bd_dma,
 				      GFP_KERNEL);
-	if (!cbd_base)
-		return -ENOMEM;
-
-	fep->tso_hdrs = dma_alloc_coherent(NULL, fep->tx_ring_size * TSO_HEADER_SIZE,
-						&fep->tso_hdrs_dma, GFP_KERNEL);
-	if (!fep->tso_hdrs) {
-		dma_free_coherent(NULL, bd_size, cbd_base, fep->bd_dma);
+	if (!cbd_base) {
 		return -ENOMEM;
 	}
 
-	memset(cbd_base, 0, PAGE_SIZE);
-
-	fep->netdev = ndev;
+	memset(cbd_base, 0, bd_size);
 
 	/* Get the Ethernet address */
 	fec_get_mac(ndev);
@@ -2462,12 +2976,36 @@ static int fec_enet_init(struct net_device *ndev)
 	fec_set_mac_address(ndev, NULL);
 
 	/* Set receive and transmit descriptor base. */
-	fep->rx_bd_base = cbd_base;
-	if (fep->bufdesc_ex)
-		fep->tx_bd_base = (struct bufdesc *)
-			(((struct bufdesc_ex *)cbd_base) + fep->rx_ring_size);
-	else
-		fep->tx_bd_base = cbd_base + fep->rx_ring_size;
+	for (i = 0; i < fep->num_rx_queues; i++) {
+		rxq = fep->rx_queue[i];
+		rxq->index = i;
+		rxq->rx_bd_base = (struct bufdesc *)cbd_base;
+		rxq->bd_dma = bd_dma;
+		if (fep->bufdesc_ex) {
+			bd_dma += sizeof(struct bufdesc_ex) * rxq->rx_ring_size;
+			cbd_base = (struct bufdesc *)
+				(((struct bufdesc_ex *)cbd_base) + rxq->rx_ring_size);
+		} else {
+			bd_dma += sizeof(struct bufdesc) * rxq->rx_ring_size;
+			cbd_base += rxq->rx_ring_size;
+		}
+	}
+
+	for (i = 0; i < fep->num_tx_queues; i++) {
+		txq = fep->tx_queue[i];
+		txq->index = i;
+		txq->tx_bd_base = (struct bufdesc *)cbd_base;
+		txq->bd_dma = bd_dma;
+		if (fep->bufdesc_ex) {
+			bd_dma += sizeof(struct bufdesc_ex) * txq->tx_ring_size;
+			cbd_base = (struct bufdesc *)
+			 (((struct bufdesc_ex *)cbd_base) + txq->tx_ring_size);
+		} else {
+			bd_dma += sizeof(struct bufdesc) * txq->tx_ring_size;
+			cbd_base += txq->tx_ring_size;
+		}
+	}
+
 
 	/* The FEC Ethernet specific entries in the device structure */
 	ndev->watchdog_timeo = TX_TIMEOUT;
@@ -2490,6 +3028,11 @@ static int fec_enet_init(struct net_device *ndev)
 		fep->csum_flags |= FLAG_RX_CSUM_ENABLED;
 	}
 
+	if (id_entry->driver_data & FEC_QUIRK_HAS_AVB) {
+		fep->tx_align = 0;
+		fep->rx_align = 0x3f;
+	}
+
 	ndev->hw_features = ndev->features;
 
 	fec_restart(ndev);
@@ -2535,6 +3078,42 @@ static void fec_reset_phy(struct platform_device *pdev)
 }
 #endif /* CONFIG_OF */
 
+static void
+fec_enet_get_queue_num(struct platform_device *pdev, int *num_tx, int *num_rx)
+{
+	struct device_node *np = pdev->dev.of_node;
+	int err;
+
+	*num_tx = *num_rx = 1;
+
+	if (!np || !of_device_is_available(np))
+		return;
+
+	/* parse the num of tx and rx queues */
+	err = of_property_read_u32(np, "fsl,num-tx-queues", num_tx);
+	if (err)
+		*num_tx = 1;
+
+	err = of_property_read_u32(np, "fsl,num-rx-queues", num_rx);
+	if (err)
+		*num_rx = 1;
+
+	if (*num_tx < 1 || *num_tx > FEC_ENET_MAX_TX_QS) {
+		dev_warn(&pdev->dev, "Invalid num_tx(=%d), fall back to 1\n",
+			 *num_tx);
+		*num_tx = 1;
+		return;
+	}
+
+	if (*num_rx < 1 || *num_rx > FEC_ENET_MAX_RX_QS) {
+		dev_warn(&pdev->dev, "Invalid num_rx(=%d), fall back to 1\n",
+			 *num_rx);
+		*num_rx = 1;
+		return;
+	}
+
+}
+
 static int
 fec_probe(struct platform_device *pdev)
 {
@@ -2546,13 +3125,18 @@ fec_probe(struct platform_device *pdev)
 	const struct of_device_id *of_id;
 	static int dev_id;
 	struct device_node *np = pdev->dev.of_node, *phy_node;
+	int num_tx_qs;
+	int num_rx_qs;
 
 	of_id = of_match_device(fec_dt_ids, &pdev->dev);
 	if (of_id)
 		pdev->id_entry = of_id->data;
 
+	fec_enet_get_queue_num(pdev, &num_tx_qs, &num_rx_qs);
+
 	/* Init network device */
-	ndev = alloc_etherdev(sizeof(struct fec_enet_private));
+	ndev = alloc_etherdev_mqs(sizeof(struct fec_enet_private),
+				  num_tx_qs, num_rx_qs);
 	if (!ndev)
 		return -ENOMEM;
 
@@ -2561,6 +3145,9 @@ fec_probe(struct platform_device *pdev)
 	/* setup board info structure */
 	fep = netdev_priv(ndev);
 
+	fep->num_rx_queues = num_rx_qs;
+	fep->num_tx_queues = num_tx_qs;
+
 #if !defined(CONFIG_M5272)
 	/* default enable pause frame auto negotiation */
 	if (pdev->id_entry &&
@@ -2620,11 +3207,21 @@ fec_probe(struct platform_device *pdev)
 		goto failed_clk;
 	}
 
+	fep->itr_clk_rate = clk_get_rate(fep->clk_ahb);
+
 	/* enet_out is optional, depends on board */
 	fep->clk_enet_out = devm_clk_get(&pdev->dev, "enet_out");
 	if (IS_ERR(fep->clk_enet_out))
 		fep->clk_enet_out = NULL;
 
+	fep->ptp_clk_on = false;
+	mutex_init(&fep->ptp_clk_mutex);
+
+	/* clk_ref is optional, depends on board */
+	fep->clk_ref = devm_clk_get(&pdev->dev, "enet_clk_ref");
+	if (IS_ERR(fep->clk_ref))
+		fep->clk_ref = NULL;
+
 	fep->clk_ptp = devm_clk_get(&pdev->dev, "ptp");
 	fep->bufdesc_ex =
 		pdev->id_entry->driver_data & FEC_QUIRK_HAS_BUFDESC_EX;
@@ -2672,6 +3269,7 @@ fec_probe(struct platform_device *pdev)
 			goto failed_irq;
 	}
 
+	init_completion(&fep->mdio_done);
 	ret = fec_enet_mii_init(pdev);
 	if (ret)
 		goto failed_mii_init;
@@ -2688,6 +3286,7 @@ fec_probe(struct platform_device *pdev)
 	if (fep->bufdesc_ex && fep->ptp_clock)
 		netdev_info(ndev, "registered PHC device %d\n", fep->dev_id);
 
+	fep->rx_copybreak = COPYBREAK_DEFAULT;
 	INIT_WORK(&fep->tx_timeout_work, fec_enet_timeout_work);
 	return 0;
 
@@ -2715,10 +3314,10 @@ fec_drv_remove(struct platform_device *pdev)
 	struct net_device *ndev = platform_get_drvdata(pdev);
 	struct fec_enet_private *fep = netdev_priv(ndev);
 
+	cancel_delayed_work_sync(&fep->time_keep);
 	cancel_work_sync(&fep->tx_timeout_work);
 	unregister_netdev(ndev);
 	fec_enet_mii_remove(fep);
-	del_timer_sync(&fep->time_keep);
 	if (fep->reg_phy)
 		regulator_disable(fep->reg_phy);
 	if (fep->ptp_clock)
diff --git a/drivers/net/ethernet/freescale/fec_ptp.c b/drivers/net/ethernet/freescale/fec_ptp.c
index 82386b29914a..992c8c3db553 100644
--- a/drivers/net/ethernet/freescale/fec_ptp.c
+++ b/drivers/net/ethernet/freescale/fec_ptp.c
@@ -61,6 +61,24 @@
 #define FEC_T_INC_CORR_MASK             0x00007f00
 #define FEC_T_INC_CORR_OFFSET           8
 
+#define FEC_T_CTRL_PINPER		0x00000080
+#define FEC_T_TF0_MASK			0x00000001
+#define FEC_T_TF0_OFFSET		0
+#define FEC_T_TF1_MASK			0x00000002
+#define FEC_T_TF1_OFFSET		1
+#define FEC_T_TF2_MASK			0x00000004
+#define FEC_T_TF2_OFFSET		2
+#define FEC_T_TF3_MASK			0x00000008
+#define FEC_T_TF3_OFFSET		3
+#define FEC_T_TDRE_MASK			0x00000001
+#define FEC_T_TDRE_OFFSET		0
+#define FEC_T_TMODE_MASK		0x0000003C
+#define FEC_T_TMODE_OFFSET		2
+#define FEC_T_TIE_MASK			0x00000040
+#define FEC_T_TIE_OFFSET		6
+#define FEC_T_TF_MASK			0x00000080
+#define FEC_T_TF_OFFSET			7
+
 #define FEC_ATIME_CTRL		0x400
 #define FEC_ATIME		0x404
 #define FEC_ATIME_EVT_OFFSET	0x408
@@ -69,7 +87,143 @@
 #define FEC_ATIME_INC		0x414
 #define FEC_TS_TIMESTAMP	0x418
 
+#define FEC_TGSR		0x604
+#define FEC_TCSR(n)		(0x608 + n * 0x08)
+#define FEC_TCCR(n)		(0x60C + n * 0x08)
+#define MAX_TIMER_CHANNEL	3
+#define FEC_TMODE_TOGGLE	0x05
+#define FEC_HIGH_PULSE		0x0F
+
 #define FEC_CC_MULT	(1 << 31)
+#define FEC_COUNTER_PERIOD	(1 << 31)
+#define PPS_OUPUT_RELOAD_PERIOD	NSEC_PER_SEC
+#define FEC_CHANNLE_0		0
+#define DEFAULT_PPS_CHANNEL	FEC_CHANNLE_0
+
+/**
+ * fec_ptp_enable_pps
+ * @fep: the fec_enet_private structure handle
+ * @enable: enable the channel pps output
+ *
+ * This function enble the PPS ouput on the timer channel.
+ */
+static int fec_ptp_enable_pps(struct fec_enet_private *fep, uint enable)
+{
+	unsigned long flags;
+	u32 val, tempval;
+	int inc;
+	struct timespec ts;
+	u64 ns;
+	u32 remainder;
+	val = 0;
+
+	if (!(fep->hwts_tx_en || fep->hwts_rx_en)) {
+		dev_err(&fep->pdev->dev, "No ptp stack is running\n");
+		return -EINVAL;
+	}
+
+	if (fep->pps_enable == enable)
+		return 0;
+
+	fep->pps_channel = DEFAULT_PPS_CHANNEL;
+	fep->reload_period = PPS_OUPUT_RELOAD_PERIOD;
+	inc = fep->ptp_inc;
+
+	spin_lock_irqsave(&fep->tmreg_lock, flags);
+
+	if (enable) {
+		/* clear capture or output compare interrupt status if have.
+		 */
+		writel(FEC_T_TF_MASK, fep->hwp + FEC_TCSR(fep->pps_channel));
+
+		/* It is recommended to doulbe check the TMODE field in the
+		 * TCSR register to be cleared before the first compare counter
+		 * is written into TCCR register. Just add a double check.
+		 */
+		val = readl(fep->hwp + FEC_TCSR(fep->pps_channel));
+		do {
+			val &= ~(FEC_T_TMODE_MASK);
+			writel(val, fep->hwp + FEC_TCSR(fep->pps_channel));
+			val = readl(fep->hwp + FEC_TCSR(fep->pps_channel));
+		} while (val & FEC_T_TMODE_MASK);
+
+		/* Dummy read counter to update the counter */
+		timecounter_read(&fep->tc);
+		/* We want to find the first compare event in the next
+		 * second point. So we need to know what the ptp time
+		 * is now and how many nanoseconds is ahead to get next second.
+		 * The remaining nanosecond ahead before the next second would be
+		 * NSEC_PER_SEC - ts.tv_nsec. Add the remaining nanoseconds
+		 * to current timer would be next second.
+		 */
+		tempval = readl(fep->hwp + FEC_ATIME_CTRL);
+		tempval |= FEC_T_CTRL_CAPTURE;
+		writel(tempval, fep->hwp + FEC_ATIME_CTRL);
+
+		tempval = readl(fep->hwp + FEC_ATIME);
+		/* Convert the ptp local counter to 1588 timestamp */
+		ns = timecounter_cyc2time(&fep->tc, tempval);
+		ts.tv_sec = div_u64_rem(ns, 1000000000ULL, &remainder);
+		ts.tv_nsec = remainder;
+
+		/* The tempval is  less than 3 seconds, and  so val is less than
+		 * 4 seconds. No overflow for 32bit calculation.
+		 */
+		val = NSEC_PER_SEC - (u32)ts.tv_nsec + tempval;
+
+		/* Need to consider the situation that the current time is
+		 * very close to the second point, which means NSEC_PER_SEC
+		 * - ts.tv_nsec is close to be zero(For example 20ns); Since the timer
+		 * is still running when we calculate the first compare event, it is
+		 * possible that the remaining nanoseonds run out before the compare
+		 * counter is calculated and written into TCCR register. To avoid
+		 * this possibility, we will set the compare event to be the next
+		 * of next second. The current setting is 31-bit timer and wrap
+		 * around over 2 seconds. So it is okay to set the next of next
+		 * seond for the timer.
+		 */
+		val += NSEC_PER_SEC;
+
+		/* We add (2 * NSEC_PER_SEC - (u32)ts.tv_nsec) to current
+		 * ptp counter, which maybe cause 32-bit wrap. Since the
+		 * (NSEC_PER_SEC - (u32)ts.tv_nsec) is less than 2 second.
+		 * We can ensure the wrap will not cause issue. If the offset
+		 * is bigger than fep->cc.mask would be a error.
+		 */
+		val &= fep->cc.mask;
+		writel(val, fep->hwp + FEC_TCCR(fep->pps_channel));
+
+		/* Calculate the second the compare event timestamp */
+		fep->next_counter = (val + fep->reload_period) & fep->cc.mask;
+
+		/* * Enable compare event when overflow */
+		val = readl(fep->hwp + FEC_ATIME_CTRL);
+		val |= FEC_T_CTRL_PINPER;
+		writel(val, fep->hwp + FEC_ATIME_CTRL);
+
+		/* Compare channel setting. */
+		val = readl(fep->hwp + FEC_TCSR(fep->pps_channel));
+		val |= (1 << FEC_T_TF_OFFSET | 1 << FEC_T_TIE_OFFSET);
+		val &= ~(1 << FEC_T_TDRE_OFFSET);
+		val &= ~(FEC_T_TMODE_MASK);
+		val |= (FEC_HIGH_PULSE << FEC_T_TMODE_OFFSET);
+		writel(val, fep->hwp + FEC_TCSR(fep->pps_channel));
+
+		/* Write the second compare event timestamp and calculate
+		 * the third timestamp. Refer the TCCR register detail in the spec.
+		 */
+		writel(fep->next_counter, fep->hwp + FEC_TCCR(fep->pps_channel));
+		fep->next_counter = (fep->next_counter + fep->reload_period) & fep->cc.mask;
+	} else {
+		writel(0, fep->hwp + FEC_TCSR(fep->pps_channel));
+	}
+
+	fep->pps_enable = enable;
+	spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+
+	return 0;
+}
+
 /**
  * fec_ptp_read - read raw cycle counter (to be used by time counter)
  * @cc: the cyclecounter structure
@@ -82,12 +236,17 @@ static cycle_t fec_ptp_read(const struct cyclecounter *cc)
 {
 	struct fec_enet_private *fep =
 		container_of(cc, struct fec_enet_private, cc);
+	const struct platform_device_id *id_entry =
+		platform_get_device_id(fep->pdev);
 	u32 tempval;
 
 	tempval = readl(fep->hwp + FEC_ATIME_CTRL);
 	tempval |= FEC_T_CTRL_CAPTURE;
 	writel(tempval, fep->hwp + FEC_ATIME_CTRL);
 
+	if (id_entry->driver_data & FEC_QUIRK_BUG_CAPTURE)
+		udelay(1);
+
 	return readl(fep->hwp + FEC_ATIME);
 }
 
@@ -113,14 +272,15 @@ void fec_ptp_start_cyclecounter(struct net_device *ndev)
 	/* 1ns counter */
 	writel(inc << FEC_T_INC_OFFSET, fep->hwp + FEC_ATIME_INC);
 
-	/* use free running count */
-	writel(0, fep->hwp + FEC_ATIME_EVT_PERIOD);
+	/* use 31-bit timer counter */
+	writel(FEC_COUNTER_PERIOD, fep->hwp + FEC_ATIME_EVT_PERIOD);
 
-	writel(FEC_T_CTRL_ENABLE, fep->hwp + FEC_ATIME_CTRL);
+	writel(FEC_T_CTRL_ENABLE | FEC_T_CTRL_PERIOD_RST,
+		fep->hwp + FEC_ATIME_CTRL);
 
 	memset(&fep->cc, 0, sizeof(fep->cc));
 	fep->cc.read = fec_ptp_read;
-	fep->cc.mask = CLOCKSOURCE_MASK(32);
+	fep->cc.mask = CLOCKSOURCE_MASK(31);
 	fep->cc.shift = 31;
 	fep->cc.mult = FEC_CC_MULT;
 
@@ -143,32 +303,59 @@ void fec_ptp_start_cyclecounter(struct net_device *ndev)
  */
 static int fec_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
 {
-	u64 diff;
 	unsigned long flags;
 	int neg_adj = 0;
-	u32 mult = FEC_CC_MULT;
+	u32 i, tmp;
+	u32 corr_inc, corr_period;
+	u32 corr_ns;
+	u64 lhs, rhs;
 
 	struct fec_enet_private *fep =
 	    container_of(ptp, struct fec_enet_private, ptp_caps);
 
+	if (ppb == 0)
+		return 0;
+
 	if (ppb < 0) {
 		ppb = -ppb;
 		neg_adj = 1;
 	}
 
-	diff = mult;
-	diff *= ppb;
-	diff = div_u64(diff, 1000000000ULL);
+	/* In theory, corr_inc/corr_period = ppb/NSEC_PER_SEC;
+	 * Try to find the corr_inc  between 1 to fep->ptp_inc to
+	 * meet adjustment requirement.
+	 */
+	lhs = NSEC_PER_SEC;
+	rhs = (u64)ppb * (u64)fep->ptp_inc;
+	for (i = 1; i <= fep->ptp_inc; i++) {
+		if (lhs >= rhs) {
+			corr_inc = i;
+			corr_period = div_u64(lhs, rhs);
+			break;
+		}
+		lhs += NSEC_PER_SEC;
+	}
+	/* Not found? Set it to high value - double speed
+	 * correct in every clock step.
+	 */
+	if (i > fep->ptp_inc) {
+		corr_inc = fep->ptp_inc;
+		corr_period = 1;
+	}
+
+	if (neg_adj)
+		corr_ns = fep->ptp_inc - corr_inc;
+	else
+		corr_ns = fep->ptp_inc + corr_inc;
 
 	spin_lock_irqsave(&fep->tmreg_lock, flags);
-	/*
-	 * dummy read to set cycle_last in tc to now.
-	 * So use adjusted mult to calculate when next call
-	 * timercounter_read.
-	 */
-	timecounter_read(&fep->tc);
 
-	fep->cc.mult = neg_adj ? mult - diff : mult + diff;
+	tmp = readl(fep->hwp + FEC_ATIME_INC) & FEC_T_INC_MASK;
+	tmp |= corr_ns << FEC_T_INC_CORR_OFFSET;
+	writel(tmp, fep->hwp + FEC_ATIME_INC);
+	writel(corr_period, fep->hwp + FEC_ATIME_CORR);
+	/* dummy read to update the timer. */
+	timecounter_read(&fep->tc);
 
 	spin_unlock_irqrestore(&fep->tmreg_lock, flags);
 
@@ -188,12 +375,19 @@ static int fec_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
 	    container_of(ptp, struct fec_enet_private, ptp_caps);
 	unsigned long flags;
 	u64 now;
+	u32 counter;
 
 	spin_lock_irqsave(&fep->tmreg_lock, flags);
 
 	now = timecounter_read(&fep->tc);
 	now += delta;
 
+	/* Get the timer value based on adjusted timestamp.
+	 * Update the counter with the masked value.
+	 */
+	counter = now & fep->cc.mask;
+	writel(counter, fep->hwp + FEC_ATIME);
+
 	/* reset the timecounter */
 	timecounter_init(&fep->tc, &fep->cc, now);
 
@@ -244,13 +438,27 @@ static int fec_ptp_settime(struct ptp_clock_info *ptp,
 
 	u64 ns;
 	unsigned long flags;
+	u32 counter;
+
+	mutex_lock(&fep->ptp_clk_mutex);
+	/* Check the ptp clock */
+	if (!fep->ptp_clk_on) {
+		mutex_unlock(&fep->ptp_clk_mutex);
+		return -EINVAL;
+	}
 
 	ns = ts->tv_sec * 1000000000ULL;
 	ns += ts->tv_nsec;
+	/* Get the timer value based on timestamp.
+	 * Update the counter with the masked value.
+	 */
+	counter = ns & fep->cc.mask;
 
 	spin_lock_irqsave(&fep->tmreg_lock, flags);
+	writel(counter, fep->hwp + FEC_ATIME);
 	timecounter_init(&fep->tc, &fep->cc, ns);
 	spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+	mutex_unlock(&fep->ptp_clk_mutex);
 	return 0;
 }
 
@@ -264,6 +472,15 @@ static int fec_ptp_settime(struct ptp_clock_info *ptp,
 static int fec_ptp_enable(struct ptp_clock_info *ptp,
 			  struct ptp_clock_request *rq, int on)
 {
+	struct fec_enet_private *fep =
+	    container_of(ptp, struct fec_enet_private, ptp_caps);
+	int ret = 0;
+
+	if (rq->type == PTP_CLK_REQ_PPS) {
+		ret = fec_ptp_enable_pps(fep, on);
+
+		return ret;
+	}
 	return -EOPNOTSUPP;
 }
 
@@ -338,17 +555,22 @@ int fec_ptp_get(struct net_device *ndev, struct ifreq *ifr)
  * fec_time_keep - call timecounter_read every second to avoid timer overrun
  *                 because ENET just support 32bit counter, will timeout in 4s
  */
-static void fec_time_keep(unsigned long _data)
+static void fec_time_keep(struct work_struct *work)
 {
-	struct fec_enet_private *fep = (struct fec_enet_private *)_data;
+	struct delayed_work *dwork = to_delayed_work(work);
+	struct fec_enet_private *fep = container_of(dwork, struct fec_enet_private, time_keep);
 	u64 ns;
 	unsigned long flags;
 
-	spin_lock_irqsave(&fep->tmreg_lock, flags);
-	ns = timecounter_read(&fep->tc);
-	spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+	mutex_lock(&fep->ptp_clk_mutex);
+	if (fep->ptp_clk_on) {
+		spin_lock_irqsave(&fep->tmreg_lock, flags);
+		ns = timecounter_read(&fep->tc);
+		spin_unlock_irqrestore(&fep->tmreg_lock, flags);
+	}
+	mutex_unlock(&fep->ptp_clk_mutex);
 
-	mod_timer(&fep->time_keep, jiffies + HZ);
+	schedule_delayed_work(&fep->time_keep, HZ);
 }
 
 /**
@@ -373,7 +595,7 @@ void fec_ptp_init(struct platform_device *pdev)
 	fep->ptp_caps.n_ext_ts = 0;
 	fep->ptp_caps.n_per_out = 0;
 	fep->ptp_caps.n_pins = 0;
-	fep->ptp_caps.pps = 0;
+	fep->ptp_caps.pps = 1;
 	fep->ptp_caps.adjfreq = fec_ptp_adjfreq;
 	fep->ptp_caps.adjtime = fec_ptp_adjtime;
 	fep->ptp_caps.gettime = fec_ptp_gettime;
@@ -381,20 +603,52 @@ void fec_ptp_init(struct platform_device *pdev)
 	fep->ptp_caps.enable = fec_ptp_enable;
 
 	fep->cycle_speed = clk_get_rate(fep->clk_ptp);
+	fep->ptp_inc = NSEC_PER_SEC / fep->cycle_speed;
 
 	spin_lock_init(&fep->tmreg_lock);
 
 	fec_ptp_start_cyclecounter(ndev);
 
-	init_timer(&fep->time_keep);
-	fep->time_keep.data = (unsigned long)fep;
-	fep->time_keep.function = fec_time_keep;
-	fep->time_keep.expires = jiffies + HZ;
-	add_timer(&fep->time_keep);
+	INIT_DELAYED_WORK(&fep->time_keep, fec_time_keep);
 
 	fep->ptp_clock = ptp_clock_register(&fep->ptp_caps, &pdev->dev);
 	if (IS_ERR(fep->ptp_clock)) {
 		fep->ptp_clock = NULL;
 		pr_err("ptp_clock_register failed\n");
 	}
+
+	schedule_delayed_work(&fep->time_keep, HZ);
+}
+
+/**
+ * fec_ptp_check_pps_event
+ * @fep: the fec_enet_private structure handle
+ *
+ * This function check the pps event and reload the timer compare counter.
+ */
+uint fec_ptp_check_pps_event(struct fec_enet_private *fep)
+{
+	u32 val;
+	u8 channel = fep->pps_channel;
+	struct ptp_clock_event event;
+
+	val = readl(fep->hwp + FEC_TCSR(channel));
+	if (val & FEC_T_TF_MASK) {
+		/* Write the next next compare(not the next according the spec)
+		 * value to the register
+		 */
+		writel(fep->next_counter, fep->hwp + FEC_TCCR(channel));
+		do {
+			writel(val, fep->hwp + FEC_TCSR(channel));
+		} while (readl(fep->hwp + FEC_TCSR(channel)) & FEC_T_TF_MASK);
+
+		/* Update the counter; */
+		fep->next_counter = (fep->next_counter + fep->reload_period) & fep->cc.mask;
+
+		event.type = PTP_CLOCK_PPS;
+		ptp_clock_event(fep->ptp_clock, &event);
+		return 1;
+	}
+
+	return 0;
 }
diff --git a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
index 748fd24d3d9e..c92c3b7876ca 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet-main.c
@@ -215,139 +215,23 @@ static int fs_enet_rx_napi(struct napi_struct *napi, int budget)
 	return received;
 }
 
-/* non NAPI receive function */
-static int fs_enet_rx_non_napi(struct net_device *dev)
+static int fs_enet_tx_napi(struct napi_struct *napi, int budget)
 {
-	struct fs_enet_private *fep = netdev_priv(dev);
-	const struct fs_platform_info *fpi = fep->fpi;
-	cbd_t __iomem *bdp;
-	struct sk_buff *skb, *skbn, *skbt;
-	int received = 0;
-	u16 pkt_len, sc;
-	int curidx;
-	/*
-	 * First, grab all of the stats for the incoming packet.
-	 * These get messed up if we get called due to a busy condition.
-	 */
-	bdp = fep->cur_rx;
-
-	while (((sc = CBDR_SC(bdp)) & BD_ENET_RX_EMPTY) == 0) {
-
-		curidx = bdp - fep->rx_bd_base;
-
-		/*
-		 * Since we have allocated space to hold a complete frame,
-		 * the last indicator should be set.
-		 */
-		if ((sc & BD_ENET_RX_LAST) == 0)
-			dev_warn(fep->dev, "rcv is not +last\n");
-
-		/*
-		 * Check for errors.
-		 */
-		if (sc & (BD_ENET_RX_LG | BD_ENET_RX_SH | BD_ENET_RX_CL |
-			  BD_ENET_RX_NO | BD_ENET_RX_CR | BD_ENET_RX_OV)) {
-			fep->stats.rx_errors++;
-			/* Frame too long or too short. */
-			if (sc & (BD_ENET_RX_LG | BD_ENET_RX_SH))
-				fep->stats.rx_length_errors++;
-			/* Frame alignment */
-			if (sc & (BD_ENET_RX_NO | BD_ENET_RX_CL))
-				fep->stats.rx_frame_errors++;
-			/* CRC Error */
-			if (sc & BD_ENET_RX_CR)
-				fep->stats.rx_crc_errors++;
-			/* FIFO overrun */
-			if (sc & BD_ENET_RX_OV)
-				fep->stats.rx_crc_errors++;
-
-			skb = fep->rx_skbuff[curidx];
-
-			dma_unmap_single(fep->dev, CBDR_BUFADDR(bdp),
-				L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
-				DMA_FROM_DEVICE);
-
-			skbn = skb;
-
-		} else {
-
-			skb = fep->rx_skbuff[curidx];
-
-			dma_unmap_single(fep->dev, CBDR_BUFADDR(bdp),
-				L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
-				DMA_FROM_DEVICE);
-
-			/*
-			 * Process the incoming frame.
-			 */
-			fep->stats.rx_packets++;
-			pkt_len = CBDR_DATLEN(bdp) - 4;	/* remove CRC */
-			fep->stats.rx_bytes += pkt_len + 4;
-
-			if (pkt_len <= fpi->rx_copybreak) {
-				/* +2 to make IP header L1 cache aligned */
-				skbn = netdev_alloc_skb(dev, pkt_len + 2);
-				if (skbn != NULL) {
-					skb_reserve(skbn, 2);	/* align IP header */
-					skb_copy_from_linear_data(skb,
-						      skbn->data, pkt_len);
-					/* swap */
-					skbt = skb;
-					skb = skbn;
-					skbn = skbt;
-				}
-			} else {
-				skbn = netdev_alloc_skb(dev, ENET_RX_FRSIZE);
-
-				if (skbn)
-					skb_align(skbn, ENET_RX_ALIGN);
-			}
-
-			if (skbn != NULL) {
-				skb_put(skb, pkt_len);	/* Make room */
-				skb->protocol = eth_type_trans(skb, dev);
-				received++;
-				netif_rx(skb);
-			} else {
-				fep->stats.rx_dropped++;
-				skbn = skb;
-			}
-		}
-
-		fep->rx_skbuff[curidx] = skbn;
-		CBDW_BUFADDR(bdp, dma_map_single(fep->dev, skbn->data,
-			     L1_CACHE_ALIGN(PKT_MAXBUF_SIZE),
-			     DMA_FROM_DEVICE));
-		CBDW_DATLEN(bdp, 0);
-		CBDW_SC(bdp, (sc & ~BD_ENET_RX_STATS) | BD_ENET_RX_EMPTY);
-
-		/*
-		 * Update BD pointer to next entry.
-		 */
-		if ((sc & BD_ENET_RX_WRAP) == 0)
-			bdp++;
-		else
-			bdp = fep->rx_bd_base;
-
-		(*fep->ops->rx_bd_done)(dev);
-	}
-
-	fep->cur_rx = bdp;
-
-	return 0;
-}
-
-static void fs_enet_tx(struct net_device *dev)
-{
-	struct fs_enet_private *fep = netdev_priv(dev);
+	struct fs_enet_private *fep = container_of(napi, struct fs_enet_private,
+						   napi_tx);
+	struct net_device *dev = fep->ndev;
 	cbd_t __iomem *bdp;
 	struct sk_buff *skb;
 	int dirtyidx, do_wake, do_restart;
 	u16 sc;
+	int has_tx_work = 0;
 
 	spin_lock(&fep->tx_lock);
 	bdp = fep->dirty_tx;
 
+	/* clear TX status bits for napi*/
+	(*fep->ops->napi_clear_tx_event)(dev);
+
 	do_wake = do_restart = 0;
 	while (((sc = CBDR_SC(bdp)) & BD_ENET_TX_READY) == 0) {
 		dirtyidx = bdp - fep->tx_bd_base;
@@ -400,7 +284,7 @@ static void fs_enet_tx(struct net_device *dev)
 		/*
 		 * Free the sk buffer associated with this last transmit.
 		 */
-		dev_kfree_skb_irq(skb);
+		dev_kfree_skb(skb);
 		fep->tx_skbuff[dirtyidx] = NULL;
 
 		/*
@@ -417,6 +301,7 @@ static void fs_enet_tx(struct net_device *dev)
 		 */
 		if (!fep->tx_free++)
 			do_wake = 1;
+		has_tx_work = 1;
 	}
 
 	fep->dirty_tx = bdp;
@@ -424,10 +309,19 @@ static void fs_enet_tx(struct net_device *dev)
 	if (do_restart)
 		(*fep->ops->tx_restart)(dev);
 
+	if (!has_tx_work) {
+		napi_complete(napi);
+		(*fep->ops->napi_enable_tx)(dev);
+	}
+
 	spin_unlock(&fep->tx_lock);
 
 	if (do_wake)
 		netif_wake_queue(dev);
+
+	if (has_tx_work)
+		return budget;
+	return 0;
 }
 
 /*
@@ -453,8 +347,7 @@ fs_enet_interrupt(int irq, void *dev_id)
 		nr++;
 
 		int_clr_events = int_events;
-		if (fpi->use_napi)
-			int_clr_events &= ~fep->ev_napi_rx;
+		int_clr_events &= ~fep->ev_napi_rx;
 
 		(*fep->ops->clear_int_events)(dev, int_clr_events);
 
@@ -462,23 +355,28 @@ fs_enet_interrupt(int irq, void *dev_id)
 			(*fep->ops->ev_error)(dev, int_events);
 
 		if (int_events & fep->ev_rx) {
-			if (!fpi->use_napi)
-				fs_enet_rx_non_napi(dev);
-			else {
-				napi_ok = napi_schedule_prep(&fep->napi);
-
-				(*fep->ops->napi_disable_rx)(dev);
-				(*fep->ops->clear_int_events)(dev, fep->ev_napi_rx);
-
-				/* NOTE: it is possible for FCCs in NAPI mode    */
-				/* to submit a spurious interrupt while in poll  */
-				if (napi_ok)
-					__napi_schedule(&fep->napi);
-			}
+			napi_ok = napi_schedule_prep(&fep->napi);
+
+			(*fep->ops->napi_disable_rx)(dev);
+			(*fep->ops->clear_int_events)(dev, fep->ev_napi_rx);
+
+			/* NOTE: it is possible for FCCs in NAPI mode    */
+			/* to submit a spurious interrupt while in poll  */
+			if (napi_ok)
+				__napi_schedule(&fep->napi);
 		}
 
-		if (int_events & fep->ev_tx)
-			fs_enet_tx(dev);
+		if (int_events & fep->ev_tx) {
+			napi_ok = napi_schedule_prep(&fep->napi_tx);
+
+			(*fep->ops->napi_disable_tx)(dev);
+			(*fep->ops->clear_int_events)(dev, fep->ev_napi_tx);
+
+			/* NOTE: it is possible for FCCs in NAPI mode    */
+			/* to submit a spurious interrupt while in poll  */
+			if (napi_ok)
+				__napi_schedule(&fep->napi_tx);
+		}
 	}
 
 	handled = nr > 0;
@@ -611,7 +509,6 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	cbd_t __iomem *bdp;
 	int curidx;
 	u16 sc;
-	unsigned long flags;
 
 #ifdef CONFIG_FS_ENET_MPC5121_FEC
 	if (((unsigned long)skb->data) & 0x3) {
@@ -626,7 +523,7 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		}
 	}
 #endif
-	spin_lock_irqsave(&fep->tx_lock, flags);
+	spin_lock(&fep->tx_lock);
 
 	/*
 	 * Fill in a Tx ring entry
@@ -635,7 +532,7 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (!fep->tx_free || (CBDR_SC(bdp) & BD_ENET_TX_READY)) {
 		netif_stop_queue(dev);
-		spin_unlock_irqrestore(&fep->tx_lock, flags);
+		spin_unlock(&fep->tx_lock);
 
 		/*
 		 * Ooops.  All transmit buffers are full.  Bail out.
@@ -691,7 +588,7 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	(*fep->ops->tx_kickstart)(dev);
 
-	spin_unlock_irqrestore(&fep->tx_lock, flags);
+	spin_unlock(&fep->tx_lock);
 
 	return NETDEV_TX_OK;
 }
@@ -811,24 +708,24 @@ static int fs_enet_open(struct net_device *dev)
 	/* not doing this, will cause a crash in fs_enet_rx_napi */
 	fs_init_bds(fep->ndev);
 
-	if (fep->fpi->use_napi)
-		napi_enable(&fep->napi);
+	napi_enable(&fep->napi);
+	napi_enable(&fep->napi_tx);
 
 	/* Install our interrupt handler. */
 	r = request_irq(fep->interrupt, fs_enet_interrupt, IRQF_SHARED,
 			"fs_enet-mac", dev);
 	if (r != 0) {
 		dev_err(fep->dev, "Could not allocate FS_ENET IRQ!");
-		if (fep->fpi->use_napi)
-			napi_disable(&fep->napi);
+		napi_disable(&fep->napi);
+		napi_disable(&fep->napi_tx);
 		return -EINVAL;
 	}
 
 	err = fs_init_phy(dev);
 	if (err) {
 		free_irq(fep->interrupt, dev);
-		if (fep->fpi->use_napi)
-			napi_disable(&fep->napi);
+		napi_disable(&fep->napi);
+		napi_disable(&fep->napi_tx);
 		return err;
 	}
 	phy_start(fep->phydev);
@@ -845,8 +742,8 @@ static int fs_enet_close(struct net_device *dev)
 
 	netif_stop_queue(dev);
 	netif_carrier_off(dev);
-	if (fep->fpi->use_napi)
-		napi_disable(&fep->napi);
+	napi_disable(&fep->napi);
+	napi_disable(&fep->napi_tx);
 	phy_stop(fep->phydev);
 
 	spin_lock_irqsave(&fep->lock, flags);
@@ -1022,7 +919,6 @@ static int fs_enet_probe(struct platform_device *ofdev)
 	fpi->rx_ring = 32;
 	fpi->tx_ring = 32;
 	fpi->rx_copybreak = 240;
-	fpi->use_napi = 1;
 	fpi->napi_weight = 17;
 	fpi->phy_node = of_parse_phandle(ofdev->dev.of_node, "phy-handle", 0);
 	if (!fpi->phy_node && of_phy_is_fixed_link(ofdev->dev.of_node)) {
@@ -1102,9 +998,8 @@ static int fs_enet_probe(struct platform_device *ofdev)
 
 	ndev->netdev_ops = &fs_enet_netdev_ops;
 	ndev->watchdog_timeo = 2 * HZ;
-	if (fpi->use_napi)
-		netif_napi_add(ndev, &fep->napi, fs_enet_rx_napi,
-		               fpi->napi_weight);
+	netif_napi_add(ndev, &fep->napi, fs_enet_rx_napi, fpi->napi_weight);
+	netif_napi_add(ndev, &fep->napi_tx, fs_enet_tx_napi, 2);
 
 	ndev->ethtool_ops = &fs_ethtool_ops;
 
diff --git a/drivers/net/ethernet/freescale/fs_enet/fs_enet.h b/drivers/net/ethernet/freescale/fs_enet/fs_enet.h
index 1ece4b1a689e..3a4b49e0e717 100644
--- a/drivers/net/ethernet/freescale/fs_enet/fs_enet.h
+++ b/drivers/net/ethernet/freescale/fs_enet/fs_enet.h
@@ -84,6 +84,9 @@ struct fs_ops {
 	void (*napi_clear_rx_event)(struct net_device *dev);
 	void (*napi_enable_rx)(struct net_device *dev);
 	void (*napi_disable_rx)(struct net_device *dev);
+	void (*napi_clear_tx_event)(struct net_device *dev);
+	void (*napi_enable_tx)(struct net_device *dev);
+	void (*napi_disable_tx)(struct net_device *dev);
 	void (*rx_bd_done)(struct net_device *dev);
 	void (*tx_kickstart)(struct net_device *dev);
 	u32 (*get_int_events)(struct net_device *dev);
@@ -119,6 +122,7 @@ struct phy_info {
 
 struct fs_enet_private {
 	struct napi_struct napi;
+	struct napi_struct napi_tx;
 	struct device *dev;	/* pointer back to the device (must be initialized first) */
 	struct net_device *ndev;
 	spinlock_t lock;	/* during all ops except TX pckt processing */
@@ -149,6 +153,7 @@ struct fs_enet_private {
 
 	/* event masks */
 	u32 ev_napi_rx;		/* mask of NAPI rx events */
+	u32 ev_napi_tx;		/* mask of NAPI rx events */
 	u32 ev_rx;		/* rx event mask          */
 	u32 ev_tx;		/* tx event mask          */
 	u32 ev_err;		/* error event mask       */
@@ -191,8 +196,8 @@ void fs_cleanup_bds(struct net_device *dev);
 
 #define DRV_MODULE_NAME		"fs_enet"
 #define PFX DRV_MODULE_NAME	": "
-#define DRV_MODULE_VERSION	"1.0"
-#define DRV_MODULE_RELDATE	"Aug 8, 2005"
+#define DRV_MODULE_VERSION	"1.1"
+#define DRV_MODULE_RELDATE	"Sep 22, 2014"
 
 /***************************************************************************/
 
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c b/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
index f5383abbf399..08f5b911d96b 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-fcc.c
@@ -125,6 +125,7 @@ out:
 }
 
 #define FCC_NAPI_RX_EVENT_MSK	(FCC_ENET_RXF | FCC_ENET_RXB)
+#define FCC_NAPI_TX_EVENT_MSK	(FCC_ENET_TXB)
 #define FCC_RX_EVENT		(FCC_ENET_RXF)
 #define FCC_TX_EVENT		(FCC_ENET_TXB)
 #define FCC_ERR_EVENT_MSK	(FCC_ENET_TXE)
@@ -137,6 +138,7 @@ static int setup_data(struct net_device *dev)
 		return -EINVAL;
 
 	fep->ev_napi_rx = FCC_NAPI_RX_EVENT_MSK;
+	fep->ev_napi_tx = FCC_NAPI_TX_EVENT_MSK;
 	fep->ev_rx = FCC_RX_EVENT;
 	fep->ev_tx = FCC_TX_EVENT;
 	fep->ev_err = FCC_ERR_EVENT_MSK;
@@ -446,6 +448,30 @@ static void napi_disable_rx(struct net_device *dev)
 	C16(fccp, fcc_fccm, FCC_NAPI_RX_EVENT_MSK);
 }
 
+static void napi_clear_tx_event(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	fcc_t __iomem *fccp = fep->fcc.fccp;
+
+	W16(fccp, fcc_fcce, FCC_NAPI_TX_EVENT_MSK);
+}
+
+static void napi_enable_tx(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	fcc_t __iomem *fccp = fep->fcc.fccp;
+
+	S16(fccp, fcc_fccm, FCC_NAPI_TX_EVENT_MSK);
+}
+
+static void napi_disable_tx(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	fcc_t __iomem *fccp = fep->fcc.fccp;
+
+	C16(fccp, fcc_fccm, FCC_NAPI_TX_EVENT_MSK);
+}
+
 static void rx_bd_done(struct net_device *dev)
 {
 	/* nothing */
@@ -572,6 +598,9 @@ const struct fs_ops fs_fcc_ops = {
 	.napi_clear_rx_event	= napi_clear_rx_event,
 	.napi_enable_rx		= napi_enable_rx,
 	.napi_disable_rx	= napi_disable_rx,
+	.napi_clear_tx_event	= napi_clear_tx_event,
+	.napi_enable_tx		= napi_enable_tx,
+	.napi_disable_tx	= napi_disable_tx,
 	.rx_bd_done		= rx_bd_done,
 	.tx_kickstart		= tx_kickstart,
 	.get_int_events		= get_int_events,
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-fec.c b/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
index 1eedfba2ad3c..3d4e08be1709 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-fec.c
@@ -110,6 +110,7 @@ static int do_pd_setup(struct fs_enet_private *fep)
 }
 
 #define FEC_NAPI_RX_EVENT_MSK	(FEC_ENET_RXF | FEC_ENET_RXB)
+#define FEC_NAPI_TX_EVENT_MSK	(FEC_ENET_TXF | FEC_ENET_TXB)
 #define FEC_RX_EVENT		(FEC_ENET_RXF)
 #define FEC_TX_EVENT		(FEC_ENET_TXF)
 #define FEC_ERR_EVENT_MSK	(FEC_ENET_HBERR | FEC_ENET_BABR | \
@@ -126,6 +127,7 @@ static int setup_data(struct net_device *dev)
 	fep->fec.htlo = 0;
 
 	fep->ev_napi_rx = FEC_NAPI_RX_EVENT_MSK;
+	fep->ev_napi_tx = FEC_NAPI_TX_EVENT_MSK;
 	fep->ev_rx = FEC_RX_EVENT;
 	fep->ev_tx = FEC_TX_EVENT;
 	fep->ev_err = FEC_ERR_EVENT_MSK;
@@ -415,6 +417,30 @@ static void napi_disable_rx(struct net_device *dev)
 	FC(fecp, imask, FEC_NAPI_RX_EVENT_MSK);
 }
 
+static void napi_clear_tx_event(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	struct fec __iomem *fecp = fep->fec.fecp;
+
+	FW(fecp, ievent, FEC_NAPI_TX_EVENT_MSK);
+}
+
+static void napi_enable_tx(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	struct fec __iomem *fecp = fep->fec.fecp;
+
+	FS(fecp, imask, FEC_NAPI_TX_EVENT_MSK);
+}
+
+static void napi_disable_tx(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	struct fec __iomem *fecp = fep->fec.fecp;
+
+	FC(fecp, imask, FEC_NAPI_TX_EVENT_MSK);
+}
+
 static void rx_bd_done(struct net_device *dev)
 {
 	struct fs_enet_private *fep = netdev_priv(dev);
@@ -487,6 +513,9 @@ const struct fs_ops fs_fec_ops = {
 	.napi_clear_rx_event	= napi_clear_rx_event,
 	.napi_enable_rx		= napi_enable_rx,
 	.napi_disable_rx	= napi_disable_rx,
+	.napi_clear_tx_event	= napi_clear_tx_event,
+	.napi_enable_tx		= napi_enable_tx,
+	.napi_disable_tx	= napi_disable_tx,
 	.rx_bd_done		= rx_bd_done,
 	.tx_kickstart		= tx_kickstart,
 	.get_int_events		= get_int_events,
diff --git a/drivers/net/ethernet/freescale/fs_enet/mac-scc.c b/drivers/net/ethernet/freescale/fs_enet/mac-scc.c
index 90b3b19b7cd3..f30411f0701f 100644
--- a/drivers/net/ethernet/freescale/fs_enet/mac-scc.c
+++ b/drivers/net/ethernet/freescale/fs_enet/mac-scc.c
@@ -116,6 +116,7 @@ static int do_pd_setup(struct fs_enet_private *fep)
 }
 
 #define SCC_NAPI_RX_EVENT_MSK	(SCCE_ENET_RXF | SCCE_ENET_RXB)
+#define SCC_NAPI_TX_EVENT_MSK	(SCCE_ENET_TXB)
 #define SCC_RX_EVENT		(SCCE_ENET_RXF)
 #define SCC_TX_EVENT		(SCCE_ENET_TXB)
 #define SCC_ERR_EVENT_MSK	(SCCE_ENET_TXE | SCCE_ENET_BSY)
@@ -130,6 +131,7 @@ static int setup_data(struct net_device *dev)
 	fep->scc.htlo = 0;
 
 	fep->ev_napi_rx = SCC_NAPI_RX_EVENT_MSK;
+	fep->ev_napi_tx = SCC_NAPI_TX_EVENT_MSK;
 	fep->ev_rx = SCC_RX_EVENT;
 	fep->ev_tx = SCC_TX_EVENT | SCCE_ENET_TXE;
 	fep->ev_err = SCC_ERR_EVENT_MSK;
@@ -398,6 +400,30 @@ static void napi_disable_rx(struct net_device *dev)
 	C16(sccp, scc_sccm, SCC_NAPI_RX_EVENT_MSK);
 }
 
+static void napi_clear_tx_event(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	scc_t __iomem *sccp = fep->scc.sccp;
+
+	W16(sccp, scc_scce, SCC_NAPI_TX_EVENT_MSK);
+}
+
+static void napi_enable_tx(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	scc_t __iomem *sccp = fep->scc.sccp;
+
+	S16(sccp, scc_sccm, SCC_NAPI_TX_EVENT_MSK);
+}
+
+static void napi_disable_tx(struct net_device *dev)
+{
+	struct fs_enet_private *fep = netdev_priv(dev);
+	scc_t __iomem *sccp = fep->scc.sccp;
+
+	C16(sccp, scc_sccm, SCC_NAPI_TX_EVENT_MSK);
+}
+
 static void rx_bd_done(struct net_device *dev)
 {
 	/* nothing */
@@ -471,6 +497,9 @@ const struct fs_ops fs_scc_ops = {
 	.napi_clear_rx_event	= napi_clear_rx_event,
 	.napi_enable_rx		= napi_enable_rx,
 	.napi_disable_rx	= napi_disable_rx,
+	.napi_clear_tx_event	= napi_clear_tx_event,
+	.napi_enable_tx		= napi_enable_tx,
+	.napi_disable_tx	= napi_disable_tx,
 	.rx_bd_done		= rx_bd_done,
 	.tx_kickstart		= tx_kickstart,
 	.get_int_events		= get_int_events,
diff --git a/drivers/net/ethernet/freescale/fsl_pq_mdio.c b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
index 583e71ab7f51..964c6bf37710 100644
--- a/drivers/net/ethernet/freescale/fsl_pq_mdio.c
+++ b/drivers/net/ethernet/freescale/fsl_pq_mdio.c
@@ -28,7 +28,9 @@
 #include <linux/of_device.h>
 
 #include <asm/io.h>
+#if IS_ENABLED(CONFIG_UCC_GETH)
 #include <asm/ucc.h>	/* for ucc_set_qe_mux_mii_mng() */
+#endif
 
 #include "gianfar.h"
 
@@ -102,19 +104,22 @@ static int fsl_pq_mdio_write(struct mii_bus *bus, int mii_id, int regnum,
 {
 	struct fsl_pq_mdio_priv *priv = bus->priv;
 	struct fsl_pq_mii __iomem *regs = priv->regs;
-	u32 status;
+	unsigned int timeout;
 
 	/* Set the PHY address and the register address we want to write */
-	out_be32(&regs->miimadd, (mii_id << 8) | regnum);
+	iowrite32be((mii_id << 8) | regnum, &regs->miimadd);
 
 	/* Write out the value we want */
-	out_be32(&regs->miimcon, value);
+	iowrite32be(value, &regs->miimcon);
 
 	/* Wait for the transaction to finish */
-	status = spin_event_timeout(!(in_be32(&regs->miimind) &	MIIMIND_BUSY),
-				    MII_TIMEOUT, 0);
+	timeout = MII_TIMEOUT;
+	while ((ioread32be(&regs->miimind) & MIIMIND_BUSY) && timeout) {
+		cpu_relax();
+		timeout--;
+	}
 
-	return status ? 0 : -ETIMEDOUT;
+	return timeout ? 0 : -ETIMEDOUT;
 }
 
 /*
@@ -131,25 +136,29 @@ static int fsl_pq_mdio_read(struct mii_bus *bus, int mii_id, int regnum)
 {
 	struct fsl_pq_mdio_priv *priv = bus->priv;
 	struct fsl_pq_mii __iomem *regs = priv->regs;
-	u32 status;
+	unsigned int timeout;
 	u16 value;
 
 	/* Set the PHY address and the register address we want to read */
-	out_be32(&regs->miimadd, (mii_id << 8) | regnum);
+	iowrite32be((mii_id << 8) | regnum, &regs->miimadd);
 
 	/* Clear miimcom, and then initiate a read */
-	out_be32(&regs->miimcom, 0);
-	out_be32(&regs->miimcom, MII_READ_COMMAND);
+	iowrite32be(0, &regs->miimcom);
+	iowrite32be(MII_READ_COMMAND, &regs->miimcom);
 
 	/* Wait for the transaction to finish, normally less than 100us */
-	status = spin_event_timeout(!(in_be32(&regs->miimind) &
-				    (MIIMIND_NOTVALID | MIIMIND_BUSY)),
-				    MII_TIMEOUT, 0);
-	if (!status)
+	timeout = MII_TIMEOUT;
+	while ((ioread32be(&regs->miimind) &
+	       (MIIMIND_NOTVALID | MIIMIND_BUSY)) && timeout) {
+		cpu_relax();
+		timeout--;
+	}
+
+	if (!timeout)
 		return -ETIMEDOUT;
 
 	/* Grab the value of the register from miimstat */
-	value = in_be32(&regs->miimstat);
+	value = ioread32be(&regs->miimstat);
 
 	dev_dbg(&bus->dev, "read %04x from address %x/%x\n", value, mii_id, regnum);
 	return value;
@@ -160,23 +169,26 @@ static int fsl_pq_mdio_reset(struct mii_bus *bus)
 {
 	struct fsl_pq_mdio_priv *priv = bus->priv;
 	struct fsl_pq_mii __iomem *regs = priv->regs;
-	u32 status;
+	unsigned int timeout;
 
 	mutex_lock(&bus->mdio_lock);
 
 	/* Reset the management interface */
-	out_be32(&regs->miimcfg, MIIMCFG_RESET);
+	iowrite32be(MIIMCFG_RESET, &regs->miimcfg);
 
 	/* Setup the MII Mgmt clock speed */
-	out_be32(&regs->miimcfg, MIIMCFG_INIT_VALUE);
+	iowrite32be(MIIMCFG_INIT_VALUE, &regs->miimcfg);
 
 	/* Wait until the bus is free */
-	status = spin_event_timeout(!(in_be32(&regs->miimind) &	MIIMIND_BUSY),
-				    MII_TIMEOUT, 0);
+	timeout = MII_TIMEOUT;
+	while ((ioread32be(&regs->miimind) & MIIMIND_BUSY) && timeout) {
+		cpu_relax();
+		timeout--;
+	}
 
 	mutex_unlock(&bus->mdio_lock);
 
-	if (!status) {
+	if (!timeout) {
 		dev_err(&bus->dev, "timeout waiting for MII bus\n");
 		return -EBUSY;
 	}
@@ -433,7 +445,7 @@ static int fsl_pq_mdio_probe(struct platform_device *pdev)
 
 			tbipa = data->get_tbipa(priv->map);
 
-			out_be32(tbipa, be32_to_cpup(prop));
+			iowrite32be(be32_to_cpup(prop), tbipa);
 		}
 	}
 
diff --git a/drivers/net/ethernet/freescale/gianfar.c b/drivers/net/ethernet/freescale/gianfar.c
index fb29d049f4e1..4fdf0aa16978 100644
--- a/drivers/net/ethernet/freescale/gianfar.c
+++ b/drivers/net/ethernet/freescale/gianfar.c
@@ -88,8 +88,10 @@
 #include <linux/net_tstamp.h>
 
 #include <asm/io.h>
+#ifdef CONFIG_PPC
 #include <asm/reg.h>
 #include <asm/mpc85xx.h>
+#endif
 #include <asm/irq.h>
 #include <asm/uaccess.h>
 #include <linux/module.h>
@@ -100,6 +102,8 @@
 #include <linux/phy_fixed.h>
 #include <linux/of.h>
 #include <linux/of_net.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
 
 #include "gianfar.h"
 
@@ -161,7 +165,7 @@ static void gfar_init_rxbdp(struct gfar_priv_rx_q *rx_queue, struct rxbd8 *bdp,
 	if (bdp == rx_queue->rx_bd_base + rx_queue->rx_ring_size - 1)
 		lstatus |= BD_LFLAG(RXBD_WRAP);
 
-	eieio();
+	gfar_wmb();
 
 	bdp->lstatus = lstatus;
 }
@@ -334,7 +338,7 @@ static void gfar_init_tx_rx_base(struct gfar_private *priv)
 
 static void gfar_rx_buff_size_config(struct gfar_private *priv)
 {
-	int frame_size = priv->ndev->mtu + ETH_HLEN;
+	int frame_size = priv->ndev->mtu + ETH_HLEN + ETH_FCS_LEN;
 
 	/* set this when rx hw offload (TOE) functions are being used */
 	priv->uses_rxfcb = 0;
@@ -1061,6 +1065,7 @@ static void gfar_init_filer_table(struct gfar_private *priv)
 	}
 }
 
+#ifdef CONFIG_PPC
 static void __gfar_detect_errata_83xx(struct gfar_private *priv)
 {
 	unsigned int pvr = mfspr(SPRN_PVR);
@@ -1093,6 +1098,7 @@ static void __gfar_detect_errata_85xx(struct gfar_private *priv)
 	    ((SVR_SOC_VER(svr) == SVR_P2010) && (SVR_REV(svr) < 0x20)))
 		priv->errata |= GFAR_ERRATA_76; /* aka eTSEC 20 */
 }
+#endif
 
 static void gfar_detect_errata(struct gfar_private *priv)
 {
@@ -1101,10 +1107,12 @@ static void gfar_detect_errata(struct gfar_private *priv)
 	/* no plans to fix */
 	priv->errata |= GFAR_ERRATA_A002;
 
+#ifdef CONFIG_PPC
 	if (pvr_version_is(PVR_VER_E500V1) || pvr_version_is(PVR_VER_E500V2))
 		__gfar_detect_errata_85xx(priv);
 	else /* non-mpc85xx parts, i.e. e300 core based */
 		__gfar_detect_errata_83xx(priv);
+#endif
 
 	if (priv->errata)
 		dev_info(dev, "enabled errata workarounds, flags: 0x%x\n",
@@ -1754,26 +1762,32 @@ static void gfar_halt_nodisable(struct gfar_private *priv)
 {
 	struct gfar __iomem *regs = priv->gfargrp[0].regs;
 	u32 tempval;
+	unsigned int timeout;
+	int stopped;
 
 	gfar_ints_disable(priv);
 
+	if (gfar_is_dma_stopped(priv))
+		return;
+
 	/* Stop the DMA, and wait for it to stop */
 	tempval = gfar_read(&regs->dmactrl);
-	if ((tempval & (DMACTRL_GRS | DMACTRL_GTS)) !=
-	    (DMACTRL_GRS | DMACTRL_GTS)) {
-		int ret;
-
-		tempval |= (DMACTRL_GRS | DMACTRL_GTS);
-		gfar_write(&regs->dmactrl, tempval);
+	tempval |= (DMACTRL_GRS | DMACTRL_GTS);
+	gfar_write(&regs->dmactrl, tempval);
 
-		do {
-			ret = spin_event_timeout(((gfar_read(&regs->ievent) &
-				 (IEVENT_GRSC | IEVENT_GTSC)) ==
-				 (IEVENT_GRSC | IEVENT_GTSC)), 1000000, 0);
-			if (!ret && !(gfar_read(&regs->ievent) & IEVENT_GRSC))
-				ret = __gfar_is_rx_idle(priv);
-		} while (!ret);
+retry:
+	timeout = 1000;
+	while (!(stopped = gfar_is_dma_stopped(priv)) && timeout) {
+		cpu_relax();
+		timeout--;
 	}
+
+	if (!timeout)
+		stopped = gfar_is_dma_stopped(priv);
+
+	if (!stopped && !gfar_is_rx_dma_stopped(priv) &&
+	    !__gfar_is_rx_idle(priv))
+		goto retry;
 }
 
 /* Halt the receive and transmit queues */
@@ -2357,18 +2371,11 @@ static int gfar_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 */
 	spin_lock_irqsave(&tx_queue->txlock, flags);
 
-	/* The powerpc-specific eieio() is used, as wmb() has too strong
-	 * semantics (it requires synchronization between cacheable and
-	 * uncacheable mappings, which eieio doesn't provide and which we
-	 * don't need), thus requiring a more expensive sync instruction.  At
-	 * some point, the set of architecture-independent barrier functions
-	 * should be expanded to include weaker barriers.
-	 */
-	eieio();
+	gfar_wmb();
 
 	txbdp_start->lstatus = lstatus;
 
-	eieio(); /* force lstatus write before tx_skbuff */
+	gfar_wmb(); /* force lstatus write before tx_skbuff */
 
 	tx_queue->tx_skbuff[tx_queue->skb_curtx] = skb;
 
@@ -3240,22 +3247,21 @@ static void gfar_set_mac_for_addr(struct net_device *dev, int num,
 {
 	struct gfar_private *priv = netdev_priv(dev);
 	struct gfar __iomem *regs = priv->gfargrp[0].regs;
-	int idx;
-	char tmpbuf[ETH_ALEN];
 	u32 tempval;
 	u32 __iomem *macptr = &regs->macstnaddr1;
 
 	macptr += num*2;
 
-	/* Now copy it into the mac registers backwards, cuz
-	 * little endian is silly
+	/* For a station address of 0x12345678ABCD in transmission
+	 * order (BE), MACnADDR1 is set to 0xCDAB7856 and
+	 * MACnADDR2 is set to 0x34120000.
 	 */
-	for (idx = 0; idx < ETH_ALEN; idx++)
-		tmpbuf[ETH_ALEN - 1 - idx] = addr[idx];
+	tempval = (addr[5] << 24) | (addr[4] << 16) |
+		  (addr[3] << 8)  |  addr[2];
 
-	gfar_write(macptr, *((u32 *) (tmpbuf)));
+	gfar_write(macptr, tempval);
 
-	tempval = *((u32 *) (tmpbuf + 4));
+	tempval = (addr[1] << 24) | (addr[0] << 16);
 
 	gfar_write(macptr+1, tempval);
 }
diff --git a/drivers/net/ethernet/freescale/gianfar.h b/drivers/net/ethernet/freescale/gianfar.h
index 84632c569f2c..2805cfbf1765 100644
--- a/drivers/net/ethernet/freescale/gianfar.h
+++ b/drivers/net/ethernet/freescale/gianfar.h
@@ -1226,6 +1226,37 @@ static inline void gfar_write_isrg(struct gfar_private *priv)
 	}
 }
 
+static inline int gfar_is_dma_stopped(struct gfar_private *priv)
+{
+	struct gfar __iomem *regs = priv->gfargrp[0].regs;
+
+	return ((gfar_read(&regs->ievent) & (IEVENT_GRSC | IEVENT_GTSC)) ==
+	       (IEVENT_GRSC | IEVENT_GTSC));
+}
+
+static inline int gfar_is_rx_dma_stopped(struct gfar_private *priv)
+{
+	struct gfar __iomem *regs = priv->gfargrp[0].regs;
+
+	return gfar_read(&regs->ievent) & IEVENT_GRSC;
+}
+
+static inline void gfar_wmb(void)
+{
+#if defined(CONFIG_PPC)
+	/* The powerpc-specific eieio() is used, as wmb() has too strong
+	 * semantics (it requires synchronization between cacheable and
+	 * uncacheable mappings, which eieio() doesn't provide and which we
+	 * don't need), thus requiring a more expensive sync instruction.  At
+	 * some point, the set of architecture-independent barrier functions
+	 * should be expanded to include weaker barriers.
+	 */
+	eieio();
+#else
+	wmb(); /* order write acesses for BD (or FCB) fields */
+#endif
+}
+
 irqreturn_t gfar_receive(int irq, void *dev_id);
 int startup_gfar(struct net_device *dev);
 void stop_gfar(struct net_device *dev);
diff --git a/drivers/net/ethernet/hp/hp100.c b/drivers/net/ethernet/hp/hp100.c
index ed7916f6fbcf..76a6e0c77d69 100644
--- a/drivers/net/ethernet/hp/hp100.c
+++ b/drivers/net/ethernet/hp/hp100.c
@@ -1627,7 +1627,7 @@ static void hp100_clean_txring(struct net_device *dev)
 #endif
 		/* Conversion to new PCI API : NOP */
 		pci_unmap_single(lp->pci_dev, (dma_addr_t) lp->txrhead->pdl[1], lp->txrhead->pdl[2], PCI_DMA_TODEVICE);
-		dev_kfree_skb_any(lp->txrhead->skb);
+		dev_consume_skb_any(lp->txrhead->skb);
 		lp->txrhead->skb = NULL;
 		lp->txrhead = lp->txrhead->next;
 		lp->txrcommit--;
@@ -1745,7 +1745,7 @@ static netdev_tx_t hp100_start_xmit(struct sk_buff *skb,
 	hp100_ints_on();
 	spin_unlock_irqrestore(&lp->lock, flags);
 
-	dev_kfree_skb_any(skb);
+	dev_consume_skb_any(skb);
 
 #ifdef HP100_DEBUG_TX
 	printk("hp100: %s: start_xmit: end\n", dev->name);
diff --git a/drivers/net/ethernet/ibm/ehea/ehea_main.c b/drivers/net/ethernet/ibm/ehea/ehea_main.c
index a0b418e007a0..566b17db135a 100644
--- a/drivers/net/ethernet/ibm/ehea/ehea_main.c
+++ b/drivers/net/ethernet/ibm/ehea/ehea_main.c
@@ -1994,7 +1994,7 @@ static void xmit_common(struct sk_buff *skb, struct ehea_swqe *swqe)
 {
 	swqe->tx_control |= EHEA_SWQE_IMM_DATA_PRESENT | EHEA_SWQE_CRC;
 
-	if (skb->protocol != htons(ETH_P_IP))
+	if (vlan_get_protocol(skb) != htons(ETH_P_IP))
 		return;
 
 	if (skb->ip_summed == CHECKSUM_PARTIAL)
diff --git a/drivers/net/ethernet/ibm/ibmveth.c b/drivers/net/ethernet/ibm/ibmveth.c
index c9127562bd22..21978cc019e7 100644
--- a/drivers/net/ethernet/ibm/ibmveth.c
+++ b/drivers/net/ethernet/ibm/ibmveth.c
@@ -292,6 +292,18 @@ failure:
 	atomic_add(buffers_added, &(pool->available));
 }
 
+/*
+ * The final 8 bytes of the buffer list is a counter of frames dropped
+ * because there was not a buffer in the buffer list capable of holding
+ * the frame.
+ */
+static void ibmveth_update_rx_no_buffer(struct ibmveth_adapter *adapter)
+{
+	__be64 *p = adapter->buffer_list_addr + 4096 - 8;
+
+	adapter->rx_no_buffer = be64_to_cpup(p);
+}
+
 /* replenish routine */
 static void ibmveth_replenish_task(struct ibmveth_adapter *adapter)
 {
@@ -307,8 +319,7 @@ static void ibmveth_replenish_task(struct ibmveth_adapter *adapter)
 			ibmveth_replenish_buffer_pool(adapter, pool);
 	}
 
-	adapter->rx_no_buffer = *(u64 *)(((char*)adapter->buffer_list_addr) +
-						4096 - 8);
+	ibmveth_update_rx_no_buffer(adapter);
 }
 
 /* empty and free ana buffer pool - also used to do cleanup in error paths */
@@ -698,8 +709,7 @@ static int ibmveth_close(struct net_device *netdev)
 
 	free_irq(netdev->irq, netdev);
 
-	adapter->rx_no_buffer = *(u64 *)(((char *)adapter->buffer_list_addr) +
-						4096 - 8);
+	ibmveth_update_rx_no_buffer(adapter);
 
 	ibmveth_cleanup(adapter);
 
diff --git a/drivers/net/ethernet/intel/Kconfig b/drivers/net/ethernet/intel/Kconfig
index bb9f0ba9d164..5b8300a32bf5 100644
--- a/drivers/net/ethernet/intel/Kconfig
+++ b/drivers/net/ethernet/intel/Kconfig
@@ -300,4 +300,35 @@ config I40EVF
 	  will be called i40evf.  MSI-X interrupt support is required
 	  for this driver to work correctly.
 
+config FM10K
+	tristate "Intel(R) FM10000 Ethernet Switch Host Interface Support"
+	default n
+	depends on PCI_MSI
+	select PTP_1588_CLOCK
+	---help---
+	  This driver supports Intel(R) FM10000 Ethernet Switch Host
+	  Interface.  For more information on how to identify your adapter,
+	  go to the Adapter & Driver ID Guide at:
+
+	  <http://support.intel.com/support/network/sb/CS-008441.htm>
+
+	  For general information and support, go to the Intel support
+	  website at:
+
+	  <http://support.intel.com>
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called fm10k.  MSI-X interrupt support is required
+
+config FM10K_VXLAN
+	bool "Virtual eXtensible Local Area Network Support"
+	default n
+	depends on FM10K && VXLAN && !(FM10K=y && VXLAN=m)
+	---help---
+	  This allows one to create VXLAN virtual interfaces that provide
+	  Layer 2 Networks over Layer 3 Networks. VXLAN is often used
+	  to tunnel virtual network infrastructure in virtualized environments.
+	  Say Y here if you want to use Virtual eXtensible Local Area Network
+	  (VXLAN) in the driver.
+
 endif # NET_VENDOR_INTEL
diff --git a/drivers/net/ethernet/intel/Makefile b/drivers/net/ethernet/intel/Makefile
index cdbbca8a3755..5ea764d85ec3 100644
--- a/drivers/net/ethernet/intel/Makefile
+++ b/drivers/net/ethernet/intel/Makefile
@@ -12,3 +12,4 @@ obj-$(CONFIG_IXGBEVF) += ixgbevf/
 obj-$(CONFIG_I40E) += i40e/
 obj-$(CONFIG_IXGB) += ixgb/
 obj-$(CONFIG_I40EVF) += i40evf/
+obj-$(CONFIG_FM10K) += fm10k/
diff --git a/drivers/net/ethernet/intel/e1000/e1000.h b/drivers/net/ethernet/intel/e1000/e1000.h
index 10a0f221b183..69707108d23c 100644
--- a/drivers/net/ethernet/intel/e1000/e1000.h
+++ b/drivers/net/ethernet/intel/e1000/e1000.h
@@ -148,16 +148,23 @@ struct e1000_adapter;
 /* wrapper around a pointer to a socket buffer,
  * so a DMA handle can be stored along with the buffer
  */
-struct e1000_buffer {
+struct e1000_tx_buffer {
 	struct sk_buff *skb;
 	dma_addr_t dma;
-	struct page *page;
 	unsigned long time_stamp;
 	u16 length;
 	u16 next_to_watch;
-	unsigned int segs;
+	bool mapped_as_page;
+	unsigned short segs;
 	unsigned int bytecount;
-	u16 mapped_as_page;
+};
+
+struct e1000_rx_buffer {
+	union {
+		struct page *page; /* jumbo: alloc_page */
+		u8 *data; /* else, netdev_alloc_frag */
+	} rxbuf;
+	dma_addr_t dma;
 };
 
 struct e1000_tx_ring {
@@ -174,7 +181,7 @@ struct e1000_tx_ring {
 	/* next descriptor to check for DD status bit */
 	unsigned int next_to_clean;
 	/* array of buffer information structs */
-	struct e1000_buffer *buffer_info;
+	struct e1000_tx_buffer *buffer_info;
 
 	u16 tdh;
 	u16 tdt;
@@ -195,7 +202,7 @@ struct e1000_rx_ring {
 	/* next descriptor to check for DD status bit */
 	unsigned int next_to_clean;
 	/* array of buffer information structs */
-	struct e1000_buffer *buffer_info;
+	struct e1000_rx_buffer *buffer_info;
 	struct sk_buff *rx_skb_top;
 
 	/* cpu for rx queue */
diff --git a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
index cca5bca44e73..b691eb4f6376 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_ethtool.c
@@ -1,35 +1,30 @@
 /*******************************************************************************
-
-  Intel PRO/1000 Linux driver
-  Copyright(c) 1999 - 2006 Intel Corporation.
-
-  This program is free software; you can redistribute it and/or modify it
-  under the terms and conditions of the GNU General Public License,
-  version 2, as published by the Free Software Foundation.
-
-  This program is distributed in the hope it will be useful, but WITHOUT
-  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
-  more details.
-
-  You should have received a copy of the GNU General Public License along with
-  this program; if not, write to the Free Software Foundation, Inc.,
-  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
-
-  The full GNU General Public License is included in this distribution in
-  the file called "COPYING".
-
-  Contact Information:
-  Linux NICS <linux.nics@intel.com>
-  e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
-  Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
-
-*******************************************************************************/
+ * Intel PRO/1000 Linux driver
+ * Copyright(c) 1999 - 2006 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * Linux NICS <linux.nics@intel.com>
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ ******************************************************************************/
 
 /* ethtool support for e1000 */
 
 #include "e1000.h"
-#include <asm/uaccess.h>
+#include <linux/uaccess.h>
 
 enum {NETDEV_STATS, E1000_STATS};
 
@@ -42,7 +37,7 @@ struct e1000_stats {
 
 #define E1000_STAT(m)		E1000_STATS, \
 				sizeof(((struct e1000_adapter *)0)->m), \
-		      		offsetof(struct e1000_adapter, m)
+				offsetof(struct e1000_adapter, m)
 #define E1000_NETDEV_STAT(m)	NETDEV_STATS, \
 				sizeof(((struct net_device *)0)->m), \
 				offsetof(struct net_device, m)
@@ -104,6 +99,7 @@ static const char e1000_gstrings_test[][ETH_GSTRING_LEN] = {
 	"Interrupt test (offline)", "Loopback test  (offline)",
 	"Link test   (on/offline)"
 };
+
 #define E1000_TEST_LEN	ARRAY_SIZE(e1000_gstrings_test)
 
 static int e1000_get_settings(struct net_device *netdev,
@@ -113,7 +109,6 @@ static int e1000_get_settings(struct net_device *netdev,
 	struct e1000_hw *hw = &adapter->hw;
 
 	if (hw->media_type == e1000_media_type_copper) {
-
 		ecmd->supported = (SUPPORTED_10baseT_Half |
 				   SUPPORTED_10baseT_Full |
 				   SUPPORTED_100baseT_Half |
@@ -155,9 +150,8 @@ static int e1000_get_settings(struct net_device *netdev,
 	}
 
 	if (er32(STATUS) & E1000_STATUS_LU) {
-
 		e1000_get_speed_and_duplex(hw, &adapter->link_speed,
-		                                   &adapter->link_duplex);
+					   &adapter->link_duplex);
 		ethtool_cmd_speed_set(ecmd, adapter->link_speed);
 
 		/* unfortunately FULL_DUPLEX != DUPLEX_FULL
@@ -247,9 +241,9 @@ static int e1000_set_settings(struct net_device *netdev,
 	if (netif_running(adapter->netdev)) {
 		e1000_down(adapter);
 		e1000_up(adapter);
-	} else
+	} else {
 		e1000_reset(adapter);
-
+	}
 	clear_bit(__E1000_RESETTING, &adapter->flags);
 	return 0;
 }
@@ -279,11 +273,11 @@ static void e1000_get_pauseparam(struct net_device *netdev,
 	pause->autoneg =
 		(adapter->fc_autoneg ? AUTONEG_ENABLE : AUTONEG_DISABLE);
 
-	if (hw->fc == E1000_FC_RX_PAUSE)
+	if (hw->fc == E1000_FC_RX_PAUSE) {
 		pause->rx_pause = 1;
-	else if (hw->fc == E1000_FC_TX_PAUSE)
+	} else if (hw->fc == E1000_FC_TX_PAUSE) {
 		pause->tx_pause = 1;
-	else if (hw->fc == E1000_FC_FULL) {
+	} else if (hw->fc == E1000_FC_FULL) {
 		pause->rx_pause = 1;
 		pause->tx_pause = 1;
 	}
@@ -316,8 +310,9 @@ static int e1000_set_pauseparam(struct net_device *netdev,
 		if (netif_running(adapter->netdev)) {
 			e1000_down(adapter);
 			e1000_up(adapter);
-		} else
+		} else {
 			e1000_reset(adapter);
+		}
 	} else
 		retval = ((hw->media_type == e1000_media_type_fiber) ?
 			  e1000_setup_link(hw) : e1000_force_mac_fc(hw));
@@ -329,12 +324,14 @@ static int e1000_set_pauseparam(struct net_device *netdev,
 static u32 e1000_get_msglevel(struct net_device *netdev)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
+
 	return adapter->msg_enable;
 }
 
 static void e1000_set_msglevel(struct net_device *netdev, u32 data)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
+
 	adapter->msg_enable = data;
 }
 
@@ -526,7 +523,7 @@ static int e1000_set_eeprom(struct net_device *netdev,
 		 * only the first byte of the word is being modified
 		 */
 		ret_val = e1000_read_eeprom(hw, last_word, 1,
-		                  &eeprom_buff[last_word - first_word]);
+					    &eeprom_buff[last_word - first_word]);
 	}
 
 	/* Device's eeprom is always little-endian, word addressable */
@@ -618,13 +615,12 @@ static int e1000_set_ringparam(struct net_device *netdev,
 	adapter->tx_ring = txdr;
 	adapter->rx_ring = rxdr;
 
-	rxdr->count = max(ring->rx_pending,(u32)E1000_MIN_RXD);
-	rxdr->count = min(rxdr->count,(u32)(mac_type < e1000_82544 ?
+	rxdr->count = max(ring->rx_pending, (u32)E1000_MIN_RXD);
+	rxdr->count = min(rxdr->count, (u32)(mac_type < e1000_82544 ?
 			  E1000_MAX_RXD : E1000_MAX_82544_RXD));
 	rxdr->count = ALIGN(rxdr->count, REQ_RX_DESCRIPTOR_MULTIPLE);
-
-	txdr->count = max(ring->tx_pending,(u32)E1000_MIN_TXD);
-	txdr->count = min(txdr->count,(u32)(mac_type < e1000_82544 ?
+	txdr->count = max(ring->tx_pending, (u32)E1000_MIN_TXD);
+	txdr->count = min(txdr->count, (u32)(mac_type < e1000_82544 ?
 			  E1000_MAX_TXD : E1000_MAX_82544_TXD));
 	txdr->count = ALIGN(txdr->count, REQ_TX_DESCRIPTOR_MULTIPLE);
 
@@ -680,8 +676,9 @@ static bool reg_pattern_test(struct e1000_adapter *adapter, u64 *data, int reg,
 			     u32 mask, u32 write)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	static const u32 test[] =
-		{0x5A5A5A5A, 0xA5A5A5A5, 0x00000000, 0xFFFFFFFF};
+	static const u32 test[] = {
+		0x5A5A5A5A, 0xA5A5A5A5, 0x00000000, 0xFFFFFFFF
+	};
 	u8 __iomem *address = hw->hw_addr + reg;
 	u32 read;
 	int i;
@@ -793,8 +790,8 @@ static int e1000_reg_test(struct e1000_adapter *adapter, u64 *data)
 		REG_PATTERN_TEST(TIDV, 0x0000FFFF, 0x0000FFFF);
 		value = E1000_RAR_ENTRIES;
 		for (i = 0; i < value; i++) {
-			REG_PATTERN_TEST(RA + (((i << 1) + 1) << 2), 0x8003FFFF,
-			                 0xFFFFFFFF);
+			REG_PATTERN_TEST(RA + (((i << 1) + 1) << 2),
+					 0x8003FFFF, 0xFFFFFFFF);
 		}
 	} else {
 		REG_SET_AND_CHECK(RCTL, 0xFFFFFFFF, 0x01FFFFFF);
@@ -877,7 +874,6 @@ static int e1000_intr_test(struct e1000_adapter *adapter, u64 *data)
 
 	/* Test each interrupt */
 	for (; i < 10; i++) {
-
 		/* Interrupt to test */
 		mask = 1 << i;
 
@@ -972,10 +968,9 @@ static void e1000_free_desc_rings(struct e1000_adapter *adapter)
 			if (rxdr->buffer_info[i].dma)
 				dma_unmap_single(&pdev->dev,
 						 rxdr->buffer_info[i].dma,
-						 rxdr->buffer_info[i].length,
+						 E1000_RXBUFFER_2048,
 						 DMA_FROM_DEVICE);
-			if (rxdr->buffer_info[i].skb)
-				dev_kfree_skb(rxdr->buffer_info[i].skb);
+			kfree(rxdr->buffer_info[i].rxbuf.data);
 		}
 	}
 
@@ -1010,7 +1005,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	if (!txdr->count)
 		txdr->count = E1000_DEFAULT_TXD;
 
-	txdr->buffer_info = kcalloc(txdr->count, sizeof(struct e1000_buffer),
+	txdr->buffer_info = kcalloc(txdr->count, sizeof(struct e1000_tx_buffer),
 				    GFP_KERNEL);
 	if (!txdr->buffer_info) {
 		ret_val = 1;
@@ -1069,7 +1064,7 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 	if (!rxdr->count)
 		rxdr->count = E1000_DEFAULT_RXD;
 
-	rxdr->buffer_info = kcalloc(rxdr->count, sizeof(struct e1000_buffer),
+	rxdr->buffer_info = kcalloc(rxdr->count, sizeof(struct e1000_rx_buffer),
 				    GFP_KERNEL);
 	if (!rxdr->buffer_info) {
 		ret_val = 5;
@@ -1099,25 +1094,25 @@ static int e1000_setup_desc_rings(struct e1000_adapter *adapter)
 
 	for (i = 0; i < rxdr->count; i++) {
 		struct e1000_rx_desc *rx_desc = E1000_RX_DESC(*rxdr, i);
-		struct sk_buff *skb;
+		u8 *buf;
 
-		skb = alloc_skb(E1000_RXBUFFER_2048 + NET_IP_ALIGN, GFP_KERNEL);
-		if (!skb) {
+		buf = kzalloc(E1000_RXBUFFER_2048 + NET_SKB_PAD + NET_IP_ALIGN,
+			      GFP_KERNEL);
+		if (!buf) {
 			ret_val = 7;
 			goto err_nomem;
 		}
-		skb_reserve(skb, NET_IP_ALIGN);
-		rxdr->buffer_info[i].skb = skb;
-		rxdr->buffer_info[i].length = E1000_RXBUFFER_2048;
+		rxdr->buffer_info[i].rxbuf.data = buf;
+
 		rxdr->buffer_info[i].dma =
-			dma_map_single(&pdev->dev, skb->data,
+			dma_map_single(&pdev->dev,
+				       buf + NET_SKB_PAD + NET_IP_ALIGN,
 				       E1000_RXBUFFER_2048, DMA_FROM_DEVICE);
 		if (dma_mapping_error(&pdev->dev, rxdr->buffer_info[i].dma)) {
 			ret_val = 8;
 			goto err_nomem;
 		}
 		rx_desc->buffer_addr = cpu_to_le64(rxdr->buffer_info[i].dma);
-		memset(skb->data, 0x00, skb->len);
 	}
 
 	return 0;
@@ -1149,8 +1144,7 @@ static void e1000_phy_reset_clk_and_crs(struct e1000_adapter *adapter)
 	 */
 	e1000_read_phy_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL, &phy_reg);
 	phy_reg |= M88E1000_EPSCR_TX_CLK_25;
-	e1000_write_phy_reg(hw,
-		M88E1000_EXT_PHY_SPEC_CTRL, phy_reg);
+	e1000_write_phy_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL, phy_reg);
 
 	/* In addition, because of the s/w reset above, we need to enable
 	 * CRS on TX.  This must be set for both full and half duplex
@@ -1158,8 +1152,7 @@ static void e1000_phy_reset_clk_and_crs(struct e1000_adapter *adapter)
 	 */
 	e1000_read_phy_reg(hw, M88E1000_PHY_SPEC_CTRL, &phy_reg);
 	phy_reg |= M88E1000_PSCR_ASSERT_CRS_ON_TX;
-	e1000_write_phy_reg(hw,
-		M88E1000_PHY_SPEC_CTRL, phy_reg);
+	e1000_write_phy_reg(hw, M88E1000_PHY_SPEC_CTRL, phy_reg);
 }
 
 static int e1000_nonintegrated_phy_loopback(struct e1000_adapter *adapter)
@@ -1216,7 +1209,7 @@ static int e1000_nonintegrated_phy_loopback(struct e1000_adapter *adapter)
 	/* Check Phy Configuration */
 	e1000_read_phy_reg(hw, PHY_CTRL, &phy_reg);
 	if (phy_reg != 0x4100)
-		 return 9;
+		return 9;
 
 	e1000_read_phy_reg(hw, M88E1000_EXT_PHY_SPEC_CTRL, &phy_reg);
 	if (phy_reg != 0x0070)
@@ -1261,7 +1254,7 @@ static int e1000_integrated_phy_loopback(struct e1000_adapter *adapter)
 			E1000_CTRL_FD); /* Force Duplex to FULL */
 
 	if (hw->media_type == e1000_media_type_copper &&
-	   hw->phy_type == e1000_phy_m88)
+	    hw->phy_type == e1000_phy_m88)
 		ctrl_reg |= E1000_CTRL_ILOS; /* Invert Loss of Signal */
 	else {
 		/* Set the ILOS bit on the fiber Nic is half
@@ -1299,7 +1292,7 @@ static int e1000_set_phy_loopback(struct e1000_adapter *adapter)
 			 * attempt this 10 times.
 			 */
 			while (e1000_nonintegrated_phy_loopback(adapter) &&
-			      count++ < 10);
+			       count++ < 10);
 			if (count < 11)
 				return 0;
 		}
@@ -1348,8 +1341,9 @@ static int e1000_setup_loopback_test(struct e1000_adapter *adapter)
 			ew32(RCTL, rctl);
 			return 0;
 		}
-	} else if (hw->media_type == e1000_media_type_copper)
+	} else if (hw->media_type == e1000_media_type_copper) {
 		return e1000_set_phy_loopback(adapter);
+	}
 
 	return 7;
 }
@@ -1391,13 +1385,13 @@ static void e1000_create_lbtest_frame(struct sk_buff *skb,
 	memset(&skb->data[frame_size / 2 + 12], 0xAF, 1);
 }
 
-static int e1000_check_lbtest_frame(struct sk_buff *skb,
+static int e1000_check_lbtest_frame(const unsigned char *data,
 				    unsigned int frame_size)
 {
 	frame_size &= ~1;
-	if (*(skb->data + 3) == 0xFF) {
-		if ((*(skb->data + frame_size / 2 + 10) == 0xBE) &&
-		   (*(skb->data + frame_size / 2 + 12) == 0xAF)) {
+	if (*(data + 3) == 0xFF) {
+		if ((*(data + frame_size / 2 + 10) == 0xBE) &&
+		    (*(data + frame_size / 2 + 12) == 0xAF)) {
 			return 0;
 		}
 	}
@@ -1410,7 +1404,7 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 	struct e1000_tx_ring *txdr = &adapter->test_tx_ring;
 	struct e1000_rx_ring *rxdr = &adapter->test_rx_ring;
 	struct pci_dev *pdev = adapter->pdev;
-	int i, j, k, l, lc, good_cnt, ret_val=0;
+	int i, j, k, l, lc, good_cnt, ret_val = 0;
 	unsigned long time;
 
 	ew32(RDT, rxdr->count - 1);
@@ -1429,12 +1423,13 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 	for (j = 0; j <= lc; j++) { /* loop count loop */
 		for (i = 0; i < 64; i++) { /* send the packets */
 			e1000_create_lbtest_frame(txdr->buffer_info[i].skb,
-					1024);
+						  1024);
 			dma_sync_single_for_device(&pdev->dev,
 						   txdr->buffer_info[k].dma,
 						   txdr->buffer_info[k].length,
 						   DMA_TO_DEVICE);
-			if (unlikely(++k == txdr->count)) k = 0;
+			if (unlikely(++k == txdr->count))
+				k = 0;
 		}
 		ew32(TDT, k);
 		E1000_WRITE_FLUSH();
@@ -1444,15 +1439,17 @@ static int e1000_run_loopback_test(struct e1000_adapter *adapter)
 		do { /* receive the sent packets */
 			dma_sync_single_for_cpu(&pdev->dev,
 						rxdr->buffer_info[l].dma,
-						rxdr->buffer_info[l].length,
+						E1000_RXBUFFER_2048,
 						DMA_FROM_DEVICE);
 
 			ret_val = e1000_check_lbtest_frame(
-					rxdr->buffer_info[l].skb,
+					rxdr->buffer_info[l].rxbuf.data +
+					NET_SKB_PAD + NET_IP_ALIGN,
 					1024);
 			if (!ret_val)
 				good_cnt++;
-			if (unlikely(++l == rxdr->count)) l = 0;
+			if (unlikely(++l == rxdr->count))
+				l = 0;
 			/* time + 20 msecs (200 msecs on 2.4) is more than
 			 * enough time to complete the receives, if it's
 			 * exceeded, break and error off
@@ -1494,6 +1491,7 @@ static int e1000_link_test(struct e1000_adapter *adapter, u64 *data)
 	*data = 0;
 	if (hw->media_type == e1000_media_type_internal_serdes) {
 		int i = 0;
+
 		hw->serdes_has_link = false;
 
 		/* On some blade server designs, link establishment
@@ -1512,9 +1510,8 @@ static int e1000_link_test(struct e1000_adapter *adapter, u64 *data)
 		if (hw->autoneg)  /* if auto_neg is set wait for it */
 			msleep(4000);
 
-		if (!(er32(STATUS) & E1000_STATUS_LU)) {
+		if (!(er32(STATUS) & E1000_STATUS_LU))
 			*data = 1;
-		}
 	}
 	return *data;
 }
@@ -1665,8 +1662,7 @@ static void e1000_get_wol(struct net_device *netdev,
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 
-	wol->supported = WAKE_UCAST | WAKE_MCAST |
-	                 WAKE_BCAST | WAKE_MAGIC;
+	wol->supported = WAKE_UCAST | WAKE_MCAST | WAKE_BCAST | WAKE_MAGIC;
 	wol->wolopts = 0;
 
 	/* this function will set ->supported = 0 and return 1 if wol is not
@@ -1819,6 +1815,7 @@ static int e1000_set_coalesce(struct net_device *netdev,
 static int e1000_nway_reset(struct net_device *netdev)
 {
 	struct e1000_adapter *adapter = netdev_priv(netdev);
+
 	if (netif_running(netdev))
 		e1000_reinit_locked(adapter);
 	return 0;
@@ -1830,22 +1827,29 @@ static void e1000_get_ethtool_stats(struct net_device *netdev,
 	struct e1000_adapter *adapter = netdev_priv(netdev);
 	int i;
 	char *p = NULL;
+	const struct e1000_stats *stat = e1000_gstrings_stats;
 
 	e1000_update_stats(adapter);
 	for (i = 0; i < E1000_GLOBAL_STATS_LEN; i++) {
-		switch (e1000_gstrings_stats[i].type) {
+		switch (stat->type) {
 		case NETDEV_STATS:
-			p = (char *) netdev +
-					e1000_gstrings_stats[i].stat_offset;
+			p = (char *)netdev + stat->stat_offset;
 			break;
 		case E1000_STATS:
-			p = (char *) adapter +
-					e1000_gstrings_stats[i].stat_offset;
+			p = (char *)adapter + stat->stat_offset;
+			break;
+		default:
+			WARN_ONCE(1, "Invalid E1000 stat type: %u index %d\n",
+				  stat->type, i);
 			break;
 		}
 
-		data[i] = (e1000_gstrings_stats[i].sizeof_stat ==
-			sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
+		if (stat->sizeof_stat == sizeof(u64))
+			data[i] = *(u64 *)p;
+		else
+			data[i] = *(u32 *)p;
+
+		stat++;
 	}
 /* BUG_ON(i != E1000_STATS_LEN); */
 }
@@ -1858,8 +1862,7 @@ static void e1000_get_strings(struct net_device *netdev, u32 stringset,
 
 	switch (stringset) {
 	case ETH_SS_TEST:
-		memcpy(data, *e1000_gstrings_test,
-			sizeof(e1000_gstrings_test));
+		memcpy(data, e1000_gstrings_test, sizeof(e1000_gstrings_test));
 		break;
 	case ETH_SS_STATS:
 		for (i = 0; i < E1000_GLOBAL_STATS_LEN; i++) {
diff --git a/drivers/net/ethernet/intel/e1000/e1000_hw.c b/drivers/net/ethernet/intel/e1000/e1000_hw.c
index 1acf5034db10..45c8c864104e 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_hw.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_hw.c
@@ -4837,84 +4837,6 @@ void e1000_update_adaptive(struct e1000_hw *hw)
 }
 
 /**
- * e1000_tbi_adjust_stats
- * @hw: Struct containing variables accessed by shared code
- * @frame_len: The length of the frame in question
- * @mac_addr: The Ethernet destination address of the frame in question
- *
- * Adjusts the statistic counters when a frame is accepted by TBI_ACCEPT
- */
-void e1000_tbi_adjust_stats(struct e1000_hw *hw, struct e1000_hw_stats *stats,
-			    u32 frame_len, u8 *mac_addr)
-{
-	u64 carry_bit;
-
-	/* First adjust the frame length. */
-	frame_len--;
-	/* We need to adjust the statistics counters, since the hardware
-	 * counters overcount this packet as a CRC error and undercount
-	 * the packet as a good packet
-	 */
-	/* This packet should not be counted as a CRC error. */
-	stats->crcerrs--;
-	/* This packet does count as a Good Packet Received. */
-	stats->gprc++;
-
-	/* Adjust the Good Octets received counters */
-	carry_bit = 0x80000000 & stats->gorcl;
-	stats->gorcl += frame_len;
-	/* If the high bit of Gorcl (the low 32 bits of the Good Octets
-	 * Received Count) was one before the addition,
-	 * AND it is zero after, then we lost the carry out,
-	 * need to add one to Gorch (Good Octets Received Count High).
-	 * This could be simplified if all environments supported
-	 * 64-bit integers.
-	 */
-	if (carry_bit && ((stats->gorcl & 0x80000000) == 0))
-		stats->gorch++;
-	/* Is this a broadcast or multicast?  Check broadcast first,
-	 * since the test for a multicast frame will test positive on
-	 * a broadcast frame.
-	 */
-	if (is_broadcast_ether_addr(mac_addr))
-		/* Broadcast packet */
-		stats->bprc++;
-	else if (is_multicast_ether_addr(mac_addr))
-		/* Multicast packet */
-		stats->mprc++;
-
-	if (frame_len == hw->max_frame_size) {
-		/* In this case, the hardware has overcounted the number of
-		 * oversize frames.
-		 */
-		if (stats->roc > 0)
-			stats->roc--;
-	}
-
-	/* Adjust the bin counters when the extra byte put the frame in the
-	 * wrong bin. Remember that the frame_len was adjusted above.
-	 */
-	if (frame_len == 64) {
-		stats->prc64++;
-		stats->prc127--;
-	} else if (frame_len == 127) {
-		stats->prc127++;
-		stats->prc255--;
-	} else if (frame_len == 255) {
-		stats->prc255++;
-		stats->prc511--;
-	} else if (frame_len == 511) {
-		stats->prc511++;
-		stats->prc1023--;
-	} else if (frame_len == 1023) {
-		stats->prc1023++;
-		stats->prc1522--;
-	} else if (frame_len == 1522) {
-		stats->prc1522++;
-	}
-}
-
-/**
  * e1000_get_bus_info
  * @hw: Struct containing variables accessed by shared code
  *
diff --git a/drivers/net/ethernet/intel/e1000/e1000_hw.h b/drivers/net/ethernet/intel/e1000/e1000_hw.h
index 11578c8978db..5cf7268cc4e1 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_hw.h
+++ b/drivers/net/ethernet/intel/e1000/e1000_hw.h
@@ -393,8 +393,6 @@ s32 e1000_blink_led_start(struct e1000_hw *hw);
 /* Everything else */
 void e1000_reset_adaptive(struct e1000_hw *hw);
 void e1000_update_adaptive(struct e1000_hw *hw);
-void e1000_tbi_adjust_stats(struct e1000_hw *hw, struct e1000_hw_stats *stats,
-			    u32 frame_len, u8 * mac_addr);
 void e1000_get_bus_info(struct e1000_hw *hw);
 void e1000_pci_set_mwi(struct e1000_hw *hw);
 void e1000_pci_clear_mwi(struct e1000_hw *hw);
diff --git a/drivers/net/ethernet/intel/e1000/e1000_main.c b/drivers/net/ethernet/intel/e1000/e1000_main.c
index cbc330b301cd..5f6aded512f5 100644
--- a/drivers/net/ethernet/intel/e1000/e1000_main.c
+++ b/drivers/net/ethernet/intel/e1000/e1000_main.c
@@ -1497,7 +1497,7 @@ static int e1000_setup_tx_resources(struct e1000_adapter *adapter,
 	struct pci_dev *pdev = adapter->pdev;
 	int size;
 
-	size = sizeof(struct e1000_buffer) * txdr->count;
+	size = sizeof(struct e1000_tx_buffer) * txdr->count;
 	txdr->buffer_info = vzalloc(size);
 	if (!txdr->buffer_info)
 		return -ENOMEM;
@@ -1687,7 +1687,7 @@ static int e1000_setup_rx_resources(struct e1000_adapter *adapter,
 	struct pci_dev *pdev = adapter->pdev;
 	int size, desc_len;
 
-	size = sizeof(struct e1000_buffer) * rxdr->count;
+	size = sizeof(struct e1000_rx_buffer) * rxdr->count;
 	rxdr->buffer_info = vzalloc(size);
 	if (!rxdr->buffer_info)
 		return -ENOMEM;
@@ -1947,8 +1947,9 @@ void e1000_free_all_tx_resources(struct e1000_adapter *adapter)
 		e1000_free_tx_resources(adapter, &adapter->tx_ring[i]);
 }
 
-static void e1000_unmap_and_free_tx_resource(struct e1000_adapter *adapter,
-					     struct e1000_buffer *buffer_info)
+static void
+e1000_unmap_and_free_tx_resource(struct e1000_adapter *adapter,
+				 struct e1000_tx_buffer *buffer_info)
 {
 	if (buffer_info->dma) {
 		if (buffer_info->mapped_as_page)
@@ -1977,7 +1978,7 @@ static void e1000_clean_tx_ring(struct e1000_adapter *adapter,
 				struct e1000_tx_ring *tx_ring)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	struct e1000_buffer *buffer_info;
+	struct e1000_tx_buffer *buffer_info;
 	unsigned long size;
 	unsigned int i;
 
@@ -1989,7 +1990,7 @@ static void e1000_clean_tx_ring(struct e1000_adapter *adapter,
 	}
 
 	netdev_reset_queue(adapter->netdev);
-	size = sizeof(struct e1000_buffer) * tx_ring->count;
+	size = sizeof(struct e1000_tx_buffer) * tx_ring->count;
 	memset(tx_ring->buffer_info, 0, size);
 
 	/* Zero out the descriptor ring */
@@ -2053,6 +2054,28 @@ void e1000_free_all_rx_resources(struct e1000_adapter *adapter)
 		e1000_free_rx_resources(adapter, &adapter->rx_ring[i]);
 }
 
+#define E1000_HEADROOM (NET_SKB_PAD + NET_IP_ALIGN)
+static unsigned int e1000_frag_len(const struct e1000_adapter *a)
+{
+	return SKB_DATA_ALIGN(a->rx_buffer_len + E1000_HEADROOM) +
+		SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
+}
+
+static void *e1000_alloc_frag(const struct e1000_adapter *a)
+{
+	unsigned int len = e1000_frag_len(a);
+	u8 *data = netdev_alloc_frag(len);
+
+	if (likely(data))
+		data += E1000_HEADROOM;
+	return data;
+}
+
+static void e1000_free_frag(const void *data)
+{
+	put_page(virt_to_head_page(data));
+}
+
 /**
  * e1000_clean_rx_ring - Free Rx Buffers per Queue
  * @adapter: board private structure
@@ -2062,44 +2085,42 @@ static void e1000_clean_rx_ring(struct e1000_adapter *adapter,
 				struct e1000_rx_ring *rx_ring)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	struct e1000_buffer *buffer_info;
+	struct e1000_rx_buffer *buffer_info;
 	struct pci_dev *pdev = adapter->pdev;
 	unsigned long size;
 	unsigned int i;
 
-	/* Free all the Rx ring sk_buffs */
+	/* Free all the Rx netfrags */
 	for (i = 0; i < rx_ring->count; i++) {
 		buffer_info = &rx_ring->buffer_info[i];
-		if (buffer_info->dma &&
-		    adapter->clean_rx == e1000_clean_rx_irq) {
-			dma_unmap_single(&pdev->dev, buffer_info->dma,
-			                 buffer_info->length,
-					 DMA_FROM_DEVICE);
-		} else if (buffer_info->dma &&
-		           adapter->clean_rx == e1000_clean_jumbo_rx_irq) {
-			dma_unmap_page(&pdev->dev, buffer_info->dma,
-				       buffer_info->length,
-				       DMA_FROM_DEVICE);
+		if (adapter->clean_rx == e1000_clean_rx_irq) {
+			if (buffer_info->dma)
+				dma_unmap_single(&pdev->dev, buffer_info->dma,
+						 adapter->rx_buffer_len,
+						 DMA_FROM_DEVICE);
+			if (buffer_info->rxbuf.data) {
+				e1000_free_frag(buffer_info->rxbuf.data);
+				buffer_info->rxbuf.data = NULL;
+			}
+		} else if (adapter->clean_rx == e1000_clean_jumbo_rx_irq) {
+			if (buffer_info->dma)
+				dma_unmap_page(&pdev->dev, buffer_info->dma,
+					       adapter->rx_buffer_len,
+					       DMA_FROM_DEVICE);
+			if (buffer_info->rxbuf.page) {
+				put_page(buffer_info->rxbuf.page);
+				buffer_info->rxbuf.page = NULL;
+			}
 		}
 
 		buffer_info->dma = 0;
-		if (buffer_info->page) {
-			put_page(buffer_info->page);
-			buffer_info->page = NULL;
-		}
-		if (buffer_info->skb) {
-			dev_kfree_skb(buffer_info->skb);
-			buffer_info->skb = NULL;
-		}
 	}
 
 	/* there also may be some cached data from a chained receive */
-	if (rx_ring->rx_skb_top) {
-		dev_kfree_skb(rx_ring->rx_skb_top);
-		rx_ring->rx_skb_top = NULL;
-	}
+	napi_free_frags(&adapter->napi);
+	rx_ring->rx_skb_top = NULL;
 
-	size = sizeof(struct e1000_buffer) * rx_ring->count;
+	size = sizeof(struct e1000_rx_buffer) * rx_ring->count;
 	memset(rx_ring->buffer_info, 0, size);
 
 	/* Zero out the descriptor ring */
@@ -2674,10 +2695,11 @@ set_itr_now:
 #define E1000_TX_FLAGS_VLAN_SHIFT	16
 
 static int e1000_tso(struct e1000_adapter *adapter,
-		     struct e1000_tx_ring *tx_ring, struct sk_buff *skb)
+		     struct e1000_tx_ring *tx_ring, struct sk_buff *skb,
+		     __be16 protocol)
 {
 	struct e1000_context_desc *context_desc;
-	struct e1000_buffer *buffer_info;
+	struct e1000_tx_buffer *buffer_info;
 	unsigned int i;
 	u32 cmd_length = 0;
 	u16 ipcse = 0, tucse, mss;
@@ -2692,7 +2714,7 @@ static int e1000_tso(struct e1000_adapter *adapter,
 
 		hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
 		mss = skb_shinfo(skb)->gso_size;
-		if (skb->protocol == htons(ETH_P_IP)) {
+		if (protocol == htons(ETH_P_IP)) {
 			struct iphdr *iph = ip_hdr(skb);
 			iph->tot_len = 0;
 			iph->check = 0;
@@ -2702,7 +2724,7 @@ static int e1000_tso(struct e1000_adapter *adapter,
 								 0);
 			cmd_length = E1000_TXD_CMD_IP;
 			ipcse = skb_transport_offset(skb) - 1;
-		} else if (skb->protocol == htons(ETH_P_IPV6)) {
+		} else if (skb_is_gso_v6(skb)) {
 			ipv6_hdr(skb)->payload_len = 0;
 			tcp_hdr(skb)->check =
 				~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
@@ -2745,10 +2767,11 @@ static int e1000_tso(struct e1000_adapter *adapter,
 }
 
 static bool e1000_tx_csum(struct e1000_adapter *adapter,
-			  struct e1000_tx_ring *tx_ring, struct sk_buff *skb)
+			  struct e1000_tx_ring *tx_ring, struct sk_buff *skb,
+			  __be16 protocol)
 {
 	struct e1000_context_desc *context_desc;
-	struct e1000_buffer *buffer_info;
+	struct e1000_tx_buffer *buffer_info;
 	unsigned int i;
 	u8 css;
 	u32 cmd_len = E1000_TXD_CMD_DEXT;
@@ -2756,7 +2779,7 @@ static bool e1000_tx_csum(struct e1000_adapter *adapter,
 	if (skb->ip_summed != CHECKSUM_PARTIAL)
 		return false;
 
-	switch (skb->protocol) {
+	switch (protocol) {
 	case cpu_to_be16(ETH_P_IP):
 		if (ip_hdr(skb)->protocol == IPPROTO_TCP)
 			cmd_len |= E1000_TXD_CMD_TCP;
@@ -2807,7 +2830,7 @@ static int e1000_tx_map(struct e1000_adapter *adapter,
 {
 	struct e1000_hw *hw = &adapter->hw;
 	struct pci_dev *pdev = adapter->pdev;
-	struct e1000_buffer *buffer_info;
+	struct e1000_tx_buffer *buffer_info;
 	unsigned int len = skb_headlen(skb);
 	unsigned int offset = 0, size, count = 0, i;
 	unsigned int f, bytecount, segs;
@@ -2953,7 +2976,7 @@ static void e1000_tx_queue(struct e1000_adapter *adapter,
 {
 	struct e1000_hw *hw = &adapter->hw;
 	struct e1000_tx_desc *tx_desc = NULL;
-	struct e1000_buffer *buffer_info;
+	struct e1000_tx_buffer *buffer_info;
 	u32 txd_upper = 0, txd_lower = E1000_TXD_CMD_IFCS;
 	unsigned int i;
 
@@ -3097,6 +3120,7 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 	int count = 0;
 	int tso;
 	unsigned int f;
+	__be16 protocol = vlan_get_protocol(skb);
 
 	/* This goes back to the question of how to logically map a Tx queue
 	 * to a flow.  Right now, performance is impacted slightly negatively
@@ -3210,7 +3234,7 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 
 	first = tx_ring->next_to_use;
 
-	tso = e1000_tso(adapter, tx_ring, skb);
+	tso = e1000_tso(adapter, tx_ring, skb, protocol);
 	if (tso < 0) {
 		dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
@@ -3220,10 +3244,10 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 		if (likely(hw->mac_type != e1000_82544))
 			tx_ring->last_tx_tso = true;
 		tx_flags |= E1000_TX_FLAGS_TSO;
-	} else if (likely(e1000_tx_csum(adapter, tx_ring, skb)))
+	} else if (likely(e1000_tx_csum(adapter, tx_ring, skb, protocol)))
 		tx_flags |= E1000_TX_FLAGS_CSUM;
 
-	if (likely(skb->protocol == htons(ETH_P_IP)))
+	if (protocol == htons(ETH_P_IP))
 		tx_flags |= E1000_TX_FLAGS_IPV4;
 
 	if (unlikely(skb->no_fcs))
@@ -3370,7 +3394,7 @@ static void e1000_dump(struct e1000_adapter *adapter)
 
 	for (i = 0; tx_ring->desc && (i < tx_ring->count); i++) {
 		struct e1000_tx_desc *tx_desc = E1000_TX_DESC(*tx_ring, i);
-		struct e1000_buffer *buffer_info = &tx_ring->buffer_info[i];
+		struct e1000_tx_buffer *buffer_info = &tx_ring->buffer_info[i];
 		struct my_u { __le64 a; __le64 b; };
 		struct my_u *u = (struct my_u *)tx_desc;
 		const char *type;
@@ -3412,7 +3436,7 @@ rx_ring_summary:
 
 	for (i = 0; rx_ring->desc && (i < rx_ring->count); i++) {
 		struct e1000_rx_desc *rx_desc = E1000_RX_DESC(*rx_ring, i);
-		struct e1000_buffer *buffer_info = &rx_ring->buffer_info[i];
+		struct e1000_rx_buffer *buffer_info = &rx_ring->buffer_info[i];
 		struct my_u { __le64 a; __le64 b; };
 		struct my_u *u = (struct my_u *)rx_desc;
 		const char *type;
@@ -3426,7 +3450,7 @@ rx_ring_summary:
 
 		pr_info("R[0x%03X]     %016llX %016llX %016llX %p %s\n",
 			i, le64_to_cpu(u->a), le64_to_cpu(u->b),
-			(u64)buffer_info->dma, buffer_info->skb, type);
+			(u64)buffer_info->dma, buffer_info->rxbuf.data, type);
 	} /* for */
 
 	/* dump the descriptor caches */
@@ -3808,7 +3832,7 @@ static bool e1000_clean_tx_irq(struct e1000_adapter *adapter,
 	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
 	struct e1000_tx_desc *tx_desc, *eop_desc;
-	struct e1000_buffer *buffer_info;
+	struct e1000_tx_buffer *buffer_info;
 	unsigned int i, eop;
 	unsigned int count = 0;
 	unsigned int total_tx_bytes=0, total_tx_packets=0;
@@ -3946,12 +3970,12 @@ static void e1000_rx_checksum(struct e1000_adapter *adapter, u32 status_err,
 }
 
 /**
- * e1000_consume_page - helper function
+ * e1000_consume_page - helper function for jumbo Rx path
  **/
-static void e1000_consume_page(struct e1000_buffer *bi, struct sk_buff *skb,
+static void e1000_consume_page(struct e1000_rx_buffer *bi, struct sk_buff *skb,
 			       u16 length)
 {
-	bi->page = NULL;
+	bi->rxbuf.page = NULL;
 	skb->len += length;
 	skb->data_len += length;
 	skb->truesize += PAGE_SIZE;
@@ -3978,6 +4002,113 @@ static void e1000_receive_skb(struct e1000_adapter *adapter, u8 status,
 }
 
 /**
+ * e1000_tbi_adjust_stats
+ * @hw: Struct containing variables accessed by shared code
+ * @frame_len: The length of the frame in question
+ * @mac_addr: The Ethernet destination address of the frame in question
+ *
+ * Adjusts the statistic counters when a frame is accepted by TBI_ACCEPT
+ */
+static void e1000_tbi_adjust_stats(struct e1000_hw *hw,
+				   struct e1000_hw_stats *stats,
+				   u32 frame_len, const u8 *mac_addr)
+{
+	u64 carry_bit;
+
+	/* First adjust the frame length. */
+	frame_len--;
+	/* We need to adjust the statistics counters, since the hardware
+	 * counters overcount this packet as a CRC error and undercount
+	 * the packet as a good packet
+	 */
+	/* This packet should not be counted as a CRC error. */
+	stats->crcerrs--;
+	/* This packet does count as a Good Packet Received. */
+	stats->gprc++;
+
+	/* Adjust the Good Octets received counters */
+	carry_bit = 0x80000000 & stats->gorcl;
+	stats->gorcl += frame_len;
+	/* If the high bit of Gorcl (the low 32 bits of the Good Octets
+	 * Received Count) was one before the addition,
+	 * AND it is zero after, then we lost the carry out,
+	 * need to add one to Gorch (Good Octets Received Count High).
+	 * This could be simplified if all environments supported
+	 * 64-bit integers.
+	 */
+	if (carry_bit && ((stats->gorcl & 0x80000000) == 0))
+		stats->gorch++;
+	/* Is this a broadcast or multicast?  Check broadcast first,
+	 * since the test for a multicast frame will test positive on
+	 * a broadcast frame.
+	 */
+	if (is_broadcast_ether_addr(mac_addr))
+		stats->bprc++;
+	else if (is_multicast_ether_addr(mac_addr))
+		stats->mprc++;
+
+	if (frame_len == hw->max_frame_size) {
+		/* In this case, the hardware has overcounted the number of
+		 * oversize frames.
+		 */
+		if (stats->roc > 0)
+			stats->roc--;
+	}
+
+	/* Adjust the bin counters when the extra byte put the frame in the
+	 * wrong bin. Remember that the frame_len was adjusted above.
+	 */
+	if (frame_len == 64) {
+		stats->prc64++;
+		stats->prc127--;
+	} else if (frame_len == 127) {
+		stats->prc127++;
+		stats->prc255--;
+	} else if (frame_len == 255) {
+		stats->prc255++;
+		stats->prc511--;
+	} else if (frame_len == 511) {
+		stats->prc511++;
+		stats->prc1023--;
+	} else if (frame_len == 1023) {
+		stats->prc1023++;
+		stats->prc1522--;
+	} else if (frame_len == 1522) {
+		stats->prc1522++;
+	}
+}
+
+static bool e1000_tbi_should_accept(struct e1000_adapter *adapter,
+				    u8 status, u8 errors,
+				    u32 length, const u8 *data)
+{
+	struct e1000_hw *hw = &adapter->hw;
+	u8 last_byte = *(data + length - 1);
+
+	if (TBI_ACCEPT(hw, status, errors, length, last_byte)) {
+		unsigned long irq_flags;
+
+		spin_lock_irqsave(&adapter->stats_lock, irq_flags);
+		e1000_tbi_adjust_stats(hw, &adapter->stats, length, data);
+		spin_unlock_irqrestore(&adapter->stats_lock, irq_flags);
+
+		return true;
+	}
+
+	return false;
+}
+
+static struct sk_buff *e1000_alloc_rx_skb(struct e1000_adapter *adapter,
+					  unsigned int bufsz)
+{
+	struct sk_buff *skb = netdev_alloc_skb_ip_align(adapter->netdev, bufsz);
+
+	if (unlikely(!skb))
+		adapter->alloc_rx_buff_failed++;
+	return skb;
+}
+
+/**
  * e1000_clean_jumbo_rx_irq - Send received data up the network stack; legacy
  * @adapter: board private structure
  * @rx_ring: ring to clean
@@ -3991,12 +4122,10 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 				     struct e1000_rx_ring *rx_ring,
 				     int *work_done, int work_to_do)
 {
-	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
 	struct pci_dev *pdev = adapter->pdev;
 	struct e1000_rx_desc *rx_desc, *next_rxd;
-	struct e1000_buffer *buffer_info, *next_buffer;
-	unsigned long irq_flags;
+	struct e1000_rx_buffer *buffer_info, *next_buffer;
 	u32 length;
 	unsigned int i;
 	int cleaned_count = 0;
@@ -4017,8 +4146,6 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 		rmb(); /* read descriptor and rx_buffer_info after status DD */
 
 		status = rx_desc->status;
-		skb = buffer_info->skb;
-		buffer_info->skb = NULL;
 
 		if (++i == rx_ring->count) i = 0;
 		next_rxd = E1000_RX_DESC(*rx_ring, i);
@@ -4029,7 +4156,7 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 		cleaned = true;
 		cleaned_count++;
 		dma_unmap_page(&pdev->dev, buffer_info->dma,
-			       buffer_info->length, DMA_FROM_DEVICE);
+			       adapter->rx_buffer_len, DMA_FROM_DEVICE);
 		buffer_info->dma = 0;
 
 		length = le16_to_cpu(rx_desc->length);
@@ -4037,25 +4164,15 @@ static bool e1000_clean_jumbo_rx_irq(struct e1000_adapter *adapter,
 		/* errors is only valid for DD + EOP descriptors */
 		if (unlikely((status & E1000_RXD_STAT_EOP) &&
 		    (rx_desc->errors & E1000_RXD_ERR_FRAME_ERR_MASK))) {
-			u8 *mapped;
-			u8 last_byte;
-
-			mapped = page_address(buffer_info->page);
-			last_byte = *(mapped + length - 1);
-			if (TBI_ACCEPT(hw, status, rx_desc->errors, length,
-				       last_byte)) {
-				spin_lock_irqsave(&adapter->stats_lock,
-						  irq_flags);
-				e1000_tbi_adjust_stats(hw, &adapter->stats,
-						       length, mapped);
-				spin_unlock_irqrestore(&adapter->stats_lock,
-						       irq_flags);
+			u8 *mapped = page_address(buffer_info->rxbuf.page);
+
+			if (e1000_tbi_should_accept(adapter, status,
+						    rx_desc->errors,
+						    length, mapped)) {
 				length--;
+			} else if (netdev->features & NETIF_F_RXALL) {
+				goto process_skb;
 			} else {
-				if (netdev->features & NETIF_F_RXALL)
-					goto process_skb;
-				/* recycle both page and skb */
-				buffer_info->skb = skb;
 				/* an error means any chain goes out the window
 				 * too
 				 */
@@ -4072,16 +4189,18 @@ process_skb:
 			/* this descriptor is only the beginning (or middle) */
 			if (!rxtop) {
 				/* this is the beginning of a chain */
-				rxtop = skb;
-				skb_fill_page_desc(rxtop, 0, buffer_info->page,
+				rxtop = napi_get_frags(&adapter->napi);
+				if (!rxtop)
+					break;
+
+				skb_fill_page_desc(rxtop, 0,
+						   buffer_info->rxbuf.page,
 						   0, length);
 			} else {
 				/* this is the middle of a chain */
 				skb_fill_page_desc(rxtop,
 				    skb_shinfo(rxtop)->nr_frags,
-				    buffer_info->page, 0, length);
-				/* re-use the skb, only consumed the page */
-				buffer_info->skb = skb;
+				    buffer_info->rxbuf.page, 0, length);
 			}
 			e1000_consume_page(buffer_info, rxtop, length);
 			goto next_desc;
@@ -4090,32 +4209,51 @@ process_skb:
 				/* end of the chain */
 				skb_fill_page_desc(rxtop,
 				    skb_shinfo(rxtop)->nr_frags,
-				    buffer_info->page, 0, length);
-				/* re-use the current skb, we only consumed the
-				 * page
-				 */
-				buffer_info->skb = skb;
+				    buffer_info->rxbuf.page, 0, length);
 				skb = rxtop;
 				rxtop = NULL;
 				e1000_consume_page(buffer_info, skb, length);
 			} else {
+				struct page *p;
 				/* no chain, got EOP, this buf is the packet
 				 * copybreak to save the put_page/alloc_page
 				 */
-				if (length <= copybreak &&
-				    skb_tailroom(skb) >= length) {
+				p = buffer_info->rxbuf.page;
+				if (length <= copybreak) {
 					u8 *vaddr;
-					vaddr = kmap_atomic(buffer_info->page);
+
+					if (likely(!(netdev->features & NETIF_F_RXFCS)))
+						length -= 4;
+					skb = e1000_alloc_rx_skb(adapter,
+								 length);
+					if (!skb)
+						break;
+
+					vaddr = kmap_atomic(p);
 					memcpy(skb_tail_pointer(skb), vaddr,
 					       length);
 					kunmap_atomic(vaddr);
 					/* re-use the page, so don't erase
-					 * buffer_info->page
+					 * buffer_info->rxbuf.page
 					 */
 					skb_put(skb, length);
+					e1000_rx_checksum(adapter,
+							  status | rx_desc->errors << 24,
+							  le16_to_cpu(rx_desc->csum), skb);
+
+					total_rx_bytes += skb->len;
+					total_rx_packets++;
+
+					e1000_receive_skb(adapter, status,
+							  rx_desc->special, skb);
+					goto next_desc;
 				} else {
-					skb_fill_page_desc(skb, 0,
-							   buffer_info->page, 0,
+					skb = napi_get_frags(&adapter->napi);
+					if (!skb) {
+						adapter->alloc_rx_buff_failed++;
+						break;
+					}
+					skb_fill_page_desc(skb, 0, p, 0,
 							   length);
 					e1000_consume_page(buffer_info, skb,
 							   length);
@@ -4134,14 +4272,14 @@ process_skb:
 			pskb_trim(skb, skb->len - 4);
 		total_rx_packets++;
 
-		/* eth type trans needs skb->data to point to something */
-		if (!pskb_may_pull(skb, ETH_HLEN)) {
-			e_err(drv, "pskb_may_pull failed.\n");
-			dev_kfree_skb(skb);
-			goto next_desc;
+		if (status & E1000_RXD_STAT_VP) {
+			__le16 vlan = rx_desc->special;
+			u16 vid = le16_to_cpu(vlan) & E1000_RXD_SPC_VLAN_MASK;
+
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
 		}
 
-		e1000_receive_skb(adapter, status, rx_desc->special, skb);
+		napi_gro_frags(&adapter->napi);
 
 next_desc:
 		rx_desc->status = 0;
@@ -4172,25 +4310,25 @@ next_desc:
 /* this should improve performance for small packets with large amounts
  * of reassembly being done in the stack
  */
-static void e1000_check_copybreak(struct net_device *netdev,
-				 struct e1000_buffer *buffer_info,
-				 u32 length, struct sk_buff **skb)
+static struct sk_buff *e1000_copybreak(struct e1000_adapter *adapter,
+				       struct e1000_rx_buffer *buffer_info,
+				       u32 length, const void *data)
 {
-	struct sk_buff *new_skb;
+	struct sk_buff *skb;
 
 	if (length > copybreak)
-		return;
+		return NULL;
 
-	new_skb = netdev_alloc_skb_ip_align(netdev, length);
-	if (!new_skb)
-		return;
+	skb = e1000_alloc_rx_skb(adapter, length);
+	if (!skb)
+		return NULL;
+
+	dma_sync_single_for_cpu(&adapter->pdev->dev, buffer_info->dma,
+				length, DMA_FROM_DEVICE);
 
-	skb_copy_to_linear_data_offset(new_skb, -NET_IP_ALIGN,
-				       (*skb)->data - NET_IP_ALIGN,
-				       length + NET_IP_ALIGN);
-	/* save the skb in buffer_info as good */
-	buffer_info->skb = *skb;
-	*skb = new_skb;
+	memcpy(skb_put(skb, length), data, length);
+
+	return skb;
 }
 
 /**
@@ -4204,12 +4342,10 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 			       struct e1000_rx_ring *rx_ring,
 			       int *work_done, int work_to_do)
 {
-	struct e1000_hw *hw = &adapter->hw;
 	struct net_device *netdev = adapter->netdev;
 	struct pci_dev *pdev = adapter->pdev;
 	struct e1000_rx_desc *rx_desc, *next_rxd;
-	struct e1000_buffer *buffer_info, *next_buffer;
-	unsigned long flags;
+	struct e1000_rx_buffer *buffer_info, *next_buffer;
 	u32 length;
 	unsigned int i;
 	int cleaned_count = 0;
@@ -4222,6 +4358,7 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 
 	while (rx_desc->status & E1000_RXD_STAT_DD) {
 		struct sk_buff *skb;
+		u8 *data;
 		u8 status;
 
 		if (*work_done >= work_to_do)
@@ -4230,10 +4367,27 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 		rmb(); /* read descriptor and rx_buffer_info after status DD */
 
 		status = rx_desc->status;
-		skb = buffer_info->skb;
-		buffer_info->skb = NULL;
+		length = le16_to_cpu(rx_desc->length);
+
+		data = buffer_info->rxbuf.data;
+		prefetch(data);
+		skb = e1000_copybreak(adapter, buffer_info, length, data);
+		if (!skb) {
+			unsigned int frag_len = e1000_frag_len(adapter);
+
+			skb = build_skb(data - E1000_HEADROOM, frag_len);
+			if (!skb) {
+				adapter->alloc_rx_buff_failed++;
+				break;
+			}
 
-		prefetch(skb->data - NET_IP_ALIGN);
+			skb_reserve(skb, E1000_HEADROOM);
+			dma_unmap_single(&pdev->dev, buffer_info->dma,
+					 adapter->rx_buffer_len,
+					 DMA_FROM_DEVICE);
+			buffer_info->dma = 0;
+			buffer_info->rxbuf.data = NULL;
+		}
 
 		if (++i == rx_ring->count) i = 0;
 		next_rxd = E1000_RX_DESC(*rx_ring, i);
@@ -4243,11 +4397,7 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 
 		cleaned = true;
 		cleaned_count++;
-		dma_unmap_single(&pdev->dev, buffer_info->dma,
-				 buffer_info->length, DMA_FROM_DEVICE);
-		buffer_info->dma = 0;
 
-		length = le16_to_cpu(rx_desc->length);
 		/* !EOP means multiple descriptors were used to store a single
 		 * packet, if thats the case we need to toss it.  In fact, we
 		 * to toss every packet with the EOP bit clear and the next
@@ -4259,29 +4409,22 @@ static bool e1000_clean_rx_irq(struct e1000_adapter *adapter,
 
 		if (adapter->discarding) {
 			/* All receives must fit into a single buffer */
-			e_dbg("Receive packet consumed multiple buffers\n");
-			/* recycle */
-			buffer_info->skb = skb;
+			netdev_dbg(netdev, "Receive packet consumed multiple buffers\n");
+			dev_kfree_skb(skb);
 			if (status & E1000_RXD_STAT_EOP)
 				adapter->discarding = false;
 			goto next_desc;
 		}
 
 		if (unlikely(rx_desc->errors & E1000_RXD_ERR_FRAME_ERR_MASK)) {
-			u8 last_byte = *(skb->data + length - 1);
-			if (TBI_ACCEPT(hw, status, rx_desc->errors, length,
-				       last_byte)) {
-				spin_lock_irqsave(&adapter->stats_lock, flags);
-				e1000_tbi_adjust_stats(hw, &adapter->stats,
-						       length, skb->data);
-				spin_unlock_irqrestore(&adapter->stats_lock,
-						       flags);
+			if (e1000_tbi_should_accept(adapter, status,
+						    rx_desc->errors,
+						    length, data)) {
 				length--;
+			} else if (netdev->features & NETIF_F_RXALL) {
+				goto process_skb;
 			} else {
-				if (netdev->features & NETIF_F_RXALL)
-					goto process_skb;
-				/* recycle */
-				buffer_info->skb = skb;
+				dev_kfree_skb(skb);
 				goto next_desc;
 			}
 		}
@@ -4296,9 +4439,10 @@ process_skb:
 			 */
 			length -= 4;
 
-		e1000_check_copybreak(netdev, buffer_info, length, &skb);
-
-		skb_put(skb, length);
+		if (buffer_info->rxbuf.data == NULL)
+			skb_put(skb, length);
+		else /* copybreak skb */
+			skb_trim(skb, length);
 
 		/* Receive Checksum Offload */
 		e1000_rx_checksum(adapter,
@@ -4344,38 +4488,19 @@ static void
 e1000_alloc_jumbo_rx_buffers(struct e1000_adapter *adapter,
 			     struct e1000_rx_ring *rx_ring, int cleaned_count)
 {
-	struct net_device *netdev = adapter->netdev;
 	struct pci_dev *pdev = adapter->pdev;
 	struct e1000_rx_desc *rx_desc;
-	struct e1000_buffer *buffer_info;
-	struct sk_buff *skb;
+	struct e1000_rx_buffer *buffer_info;
 	unsigned int i;
-	unsigned int bufsz = 256 - 16 /*for skb_reserve */ ;
 
 	i = rx_ring->next_to_use;
 	buffer_info = &rx_ring->buffer_info[i];
 
 	while (cleaned_count--) {
-		skb = buffer_info->skb;
-		if (skb) {
-			skb_trim(skb, 0);
-			goto check_page;
-		}
-
-		skb = netdev_alloc_skb_ip_align(netdev, bufsz);
-		if (unlikely(!skb)) {
-			/* Better luck next round */
-			adapter->alloc_rx_buff_failed++;
-			break;
-		}
-
-		buffer_info->skb = skb;
-		buffer_info->length = adapter->rx_buffer_len;
-check_page:
 		/* allocate a new page if necessary */
-		if (!buffer_info->page) {
-			buffer_info->page = alloc_page(GFP_ATOMIC);
-			if (unlikely(!buffer_info->page)) {
+		if (!buffer_info->rxbuf.page) {
+			buffer_info->rxbuf.page = alloc_page(GFP_ATOMIC);
+			if (unlikely(!buffer_info->rxbuf.page)) {
 				adapter->alloc_rx_buff_failed++;
 				break;
 			}
@@ -4383,17 +4508,15 @@ check_page:
 
 		if (!buffer_info->dma) {
 			buffer_info->dma = dma_map_page(&pdev->dev,
-							buffer_info->page, 0,
-							buffer_info->length,
+							buffer_info->rxbuf.page, 0,
+							adapter->rx_buffer_len,
 							DMA_FROM_DEVICE);
 			if (dma_mapping_error(&pdev->dev, buffer_info->dma)) {
-				put_page(buffer_info->page);
-				dev_kfree_skb(skb);
-				buffer_info->page = NULL;
-				buffer_info->skb = NULL;
+				put_page(buffer_info->rxbuf.page);
+				buffer_info->rxbuf.page = NULL;
 				buffer_info->dma = 0;
 				adapter->alloc_rx_buff_failed++;
-				break; /* while !buffer_info->skb */
+				break;
 			}
 		}
 
@@ -4429,11 +4552,9 @@ static void e1000_alloc_rx_buffers(struct e1000_adapter *adapter,
 				   int cleaned_count)
 {
 	struct e1000_hw *hw = &adapter->hw;
-	struct net_device *netdev = adapter->netdev;
 	struct pci_dev *pdev = adapter->pdev;
 	struct e1000_rx_desc *rx_desc;
-	struct e1000_buffer *buffer_info;
-	struct sk_buff *skb;
+	struct e1000_rx_buffer *buffer_info;
 	unsigned int i;
 	unsigned int bufsz = adapter->rx_buffer_len;
 
@@ -4441,57 +4562,52 @@ static void e1000_alloc_rx_buffers(struct e1000_adapter *adapter,
 	buffer_info = &rx_ring->buffer_info[i];
 
 	while (cleaned_count--) {
-		skb = buffer_info->skb;
-		if (skb) {
-			skb_trim(skb, 0);
-			goto map_skb;
-		}
+		void *data;
 
-		skb = netdev_alloc_skb_ip_align(netdev, bufsz);
-		if (unlikely(!skb)) {
+		if (buffer_info->rxbuf.data)
+			goto skip;
+
+		data = e1000_alloc_frag(adapter);
+		if (!data) {
 			/* Better luck next round */
 			adapter->alloc_rx_buff_failed++;
 			break;
 		}
 
 		/* Fix for errata 23, can't cross 64kB boundary */
-		if (!e1000_check_64k_bound(adapter, skb->data, bufsz)) {
-			struct sk_buff *oldskb = skb;
+		if (!e1000_check_64k_bound(adapter, data, bufsz)) {
+			void *olddata = data;
 			e_err(rx_err, "skb align check failed: %u bytes at "
-			      "%p\n", bufsz, skb->data);
+			      "%p\n", bufsz, data);
 			/* Try again, without freeing the previous */
-			skb = netdev_alloc_skb_ip_align(netdev, bufsz);
+			data = e1000_alloc_frag(adapter);
 			/* Failed allocation, critical failure */
-			if (!skb) {
-				dev_kfree_skb(oldskb);
+			if (!data) {
+				e1000_free_frag(olddata);
 				adapter->alloc_rx_buff_failed++;
 				break;
 			}
 
-			if (!e1000_check_64k_bound(adapter, skb->data, bufsz)) {
+			if (!e1000_check_64k_bound(adapter, data, bufsz)) {
 				/* give up */
-				dev_kfree_skb(skb);
-				dev_kfree_skb(oldskb);
+				e1000_free_frag(data);
+				e1000_free_frag(olddata);
 				adapter->alloc_rx_buff_failed++;
-				break; /* while !buffer_info->skb */
+				break;
 			}
 
 			/* Use new allocation */
-			dev_kfree_skb(oldskb);
+			e1000_free_frag(olddata);
 		}
-		buffer_info->skb = skb;
-		buffer_info->length = adapter->rx_buffer_len;
-map_skb:
 		buffer_info->dma = dma_map_single(&pdev->dev,
-						  skb->data,
-						  buffer_info->length,
+						  data,
+						  adapter->rx_buffer_len,
 						  DMA_FROM_DEVICE);
 		if (dma_mapping_error(&pdev->dev, buffer_info->dma)) {
-			dev_kfree_skb(skb);
-			buffer_info->skb = NULL;
+			e1000_free_frag(data);
 			buffer_info->dma = 0;
 			adapter->alloc_rx_buff_failed++;
-			break; /* while !buffer_info->skb */
+			break;
 		}
 
 		/* XXX if it was allocated cleanly it will never map to a
@@ -4505,17 +4621,20 @@ map_skb:
 			e_err(rx_err, "dma align check failed: %u bytes at "
 			      "%p\n", adapter->rx_buffer_len,
 			      (void *)(unsigned long)buffer_info->dma);
-			dev_kfree_skb(skb);
-			buffer_info->skb = NULL;
 
 			dma_unmap_single(&pdev->dev, buffer_info->dma,
 					 adapter->rx_buffer_len,
 					 DMA_FROM_DEVICE);
+
+			e1000_free_frag(data);
+			buffer_info->rxbuf.data = NULL;
 			buffer_info->dma = 0;
 
 			adapter->alloc_rx_buff_failed++;
-			break; /* while !buffer_info->skb */
+			break;
 		}
+		buffer_info->rxbuf.data = data;
+ skip:
 		rx_desc = E1000_RX_DESC(*rx_ring, i);
 		rx_desc->buffer_addr = cpu_to_le64(buffer_info->dma);
 
diff --git a/drivers/net/ethernet/intel/e1000e/netdev.c b/drivers/net/ethernet/intel/e1000e/netdev.c
index 65c3aef2bd36..247335d2c7ec 100644
--- a/drivers/net/ethernet/intel/e1000e/netdev.c
+++ b/drivers/net/ethernet/intel/e1000e/netdev.c
@@ -5164,7 +5164,8 @@ link_up:
 #define E1000_TX_FLAGS_VLAN_MASK	0xffff0000
 #define E1000_TX_FLAGS_VLAN_SHIFT	16
 
-static int e1000_tso(struct e1000_ring *tx_ring, struct sk_buff *skb)
+static int e1000_tso(struct e1000_ring *tx_ring, struct sk_buff *skb,
+		     __be16 protocol)
 {
 	struct e1000_context_desc *context_desc;
 	struct e1000_buffer *buffer_info;
@@ -5183,7 +5184,7 @@ static int e1000_tso(struct e1000_ring *tx_ring, struct sk_buff *skb)
 
 	hdr_len = skb_transport_offset(skb) + tcp_hdrlen(skb);
 	mss = skb_shinfo(skb)->gso_size;
-	if (skb->protocol == htons(ETH_P_IP)) {
+	if (protocol == htons(ETH_P_IP)) {
 		struct iphdr *iph = ip_hdr(skb);
 		iph->tot_len = 0;
 		iph->check = 0;
@@ -5231,7 +5232,8 @@ static int e1000_tso(struct e1000_ring *tx_ring, struct sk_buff *skb)
 	return 1;
 }
 
-static bool e1000_tx_csum(struct e1000_ring *tx_ring, struct sk_buff *skb)
+static bool e1000_tx_csum(struct e1000_ring *tx_ring, struct sk_buff *skb,
+			  __be16 protocol)
 {
 	struct e1000_adapter *adapter = tx_ring->adapter;
 	struct e1000_context_desc *context_desc;
@@ -5239,16 +5241,10 @@ static bool e1000_tx_csum(struct e1000_ring *tx_ring, struct sk_buff *skb)
 	unsigned int i;
 	u8 css;
 	u32 cmd_len = E1000_TXD_CMD_DEXT;
-	__be16 protocol;
 
 	if (skb->ip_summed != CHECKSUM_PARTIAL)
 		return false;
 
-	if (skb->protocol == cpu_to_be16(ETH_P_8021Q))
-		protocol = vlan_eth_hdr(skb)->h_vlan_encapsulated_proto;
-	else
-		protocol = skb->protocol;
-
 	switch (protocol) {
 	case cpu_to_be16(ETH_P_IP):
 		if (ip_hdr(skb)->protocol == IPPROTO_TCP)
@@ -5546,6 +5542,7 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 	int count = 0;
 	int tso;
 	unsigned int f;
+	__be16 protocol = vlan_get_protocol(skb);
 
 	if (test_bit(__E1000_DOWN, &adapter->state)) {
 		dev_kfree_skb_any(skb);
@@ -5620,7 +5617,7 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 
 	first = tx_ring->next_to_use;
 
-	tso = e1000_tso(tx_ring, skb);
+	tso = e1000_tso(tx_ring, skb, protocol);
 	if (tso < 0) {
 		dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
@@ -5628,14 +5625,14 @@ static netdev_tx_t e1000_xmit_frame(struct sk_buff *skb,
 
 	if (tso)
 		tx_flags |= E1000_TX_FLAGS_TSO;
-	else if (e1000_tx_csum(tx_ring, skb))
+	else if (e1000_tx_csum(tx_ring, skb, protocol))
 		tx_flags |= E1000_TX_FLAGS_CSUM;
 
 	/* Old method was to assume IPv4 packet by default if TSO was enabled.
 	 * 82571 hardware supports TSO capabilities for IPv6 as well...
 	 * no longer assume, we must.
 	 */
-	if (skb->protocol == htons(ETH_P_IP))
+	if (protocol == htons(ETH_P_IP))
 		tx_flags |= E1000_TX_FLAGS_IPV4;
 
 	if (unlikely(skb->no_fcs))
diff --git a/drivers/net/ethernet/intel/fm10k/Makefile b/drivers/net/ethernet/intel/fm10k/Makefile
new file mode 100644
index 000000000000..08859dd220a8
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/Makefile
@@ -0,0 +1,33 @@
+################################################################################
+#
+# Intel Ethernet Switch Host Interface Driver
+# Copyright(c) 2013 - 2014 Intel Corporation.
+#
+# This program is free software; you can redistribute it and/or modify it
+# under the terms and conditions of the GNU General Public License,
+# version 2, as published by the Free Software Foundation.
+#
+# This program is distributed in the hope it will be useful, but WITHOUT
+# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+# FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+# more details.
+#
+# The full GNU General Public License is included in this distribution in
+# the file called "COPYING".
+#
+# Contact Information:
+# e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+# Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+#
+################################################################################
+
+#
+# Makefile for the Intel(R) FM10000 Ethernet Switch Host Interface driver
+#
+
+obj-$(CONFIG_FM10K) += fm10k.o
+
+fm10k-objs := fm10k_main.o fm10k_common.o fm10k_pci.o \
+	      fm10k_netdev.o fm10k_ethtool.o fm10k_pf.o fm10k_vf.o \
+	      fm10k_mbx.o fm10k_iov.o fm10k_tlv.o \
+	      fm10k_debugfs.o fm10k_ptp.o fm10k_dcbnl.o
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k.h b/drivers/net/ethernet/intel/fm10k/fm10k.h
new file mode 100644
index 000000000000..42eb4344a9dc
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k.h
@@ -0,0 +1,530 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifndef _FM10K_H_
+#define _FM10K_H_
+
+#include <linux/types.h>
+#include <linux/etherdevice.h>
+#include <linux/rtnetlink.h>
+#include <linux/if_vlan.h>
+#include <linux/pci.h>
+#include <linux/net_tstamp.h>
+#include <linux/clocksource.h>
+#include <linux/ptp_clock_kernel.h>
+
+#include "fm10k_pf.h"
+#include "fm10k_vf.h"
+
+#define FM10K_MAX_JUMBO_FRAME_SIZE	15358	/* Maximum supported size 15K */
+
+#define MAX_QUEUES	FM10K_MAX_QUEUES_PF
+
+#define FM10K_MIN_RXD		 128
+#define FM10K_MAX_RXD		4096
+#define FM10K_DEFAULT_RXD	 256
+
+#define FM10K_MIN_TXD		 128
+#define FM10K_MAX_TXD		4096
+#define FM10K_DEFAULT_TXD	 256
+#define FM10K_DEFAULT_TX_WORK	 256
+
+#define FM10K_RXBUFFER_256	  256
+#define FM10K_RX_HDR_LEN	FM10K_RXBUFFER_256
+#define FM10K_RXBUFFER_2048	 2048
+#define FM10K_RX_BUFSZ		FM10K_RXBUFFER_2048
+
+/* How many Rx Buffers do we bundle into one write to the hardware ? */
+#define FM10K_RX_BUFFER_WRITE	16	/* Must be power of 2 */
+
+#define FM10K_MAX_STATIONS	63
+struct fm10k_l2_accel {
+	int size;
+	u16 count;
+	u16 dglort;
+	struct rcu_head rcu;
+	struct net_device *macvlan[0];
+};
+
+enum fm10k_ring_state_t {
+	__FM10K_TX_DETECT_HANG,
+	__FM10K_HANG_CHECK_ARMED,
+};
+
+#define check_for_tx_hang(ring) \
+	test_bit(__FM10K_TX_DETECT_HANG, &(ring)->state)
+#define set_check_for_tx_hang(ring) \
+	set_bit(__FM10K_TX_DETECT_HANG, &(ring)->state)
+#define clear_check_for_tx_hang(ring) \
+	clear_bit(__FM10K_TX_DETECT_HANG, &(ring)->state)
+
+struct fm10k_tx_buffer {
+	struct fm10k_tx_desc *next_to_watch;
+	struct sk_buff *skb;
+	unsigned int bytecount;
+	u16 gso_segs;
+	u16 tx_flags;
+	DEFINE_DMA_UNMAP_ADDR(dma);
+	DEFINE_DMA_UNMAP_LEN(len);
+};
+
+struct fm10k_rx_buffer {
+	dma_addr_t dma;
+	struct page *page;
+	u32 page_offset;
+};
+
+struct fm10k_queue_stats {
+	u64 packets;
+	u64 bytes;
+};
+
+struct fm10k_tx_queue_stats {
+	u64 restart_queue;
+	u64 csum_err;
+	u64 tx_busy;
+	u64 tx_done_old;
+};
+
+struct fm10k_rx_queue_stats {
+	u64 alloc_failed;
+	u64 csum_err;
+	u64 errors;
+};
+
+struct fm10k_ring {
+	struct fm10k_q_vector *q_vector;/* backpointer to host q_vector */
+	struct net_device *netdev;	/* netdev ring belongs to */
+	struct device *dev;		/* device for DMA mapping */
+	struct fm10k_l2_accel __rcu *l2_accel;	/* L2 acceleration list */
+	void *desc;			/* descriptor ring memory */
+	union {
+		struct fm10k_tx_buffer *tx_buffer;
+		struct fm10k_rx_buffer *rx_buffer;
+	};
+	u32 __iomem *tail;
+	unsigned long state;
+	dma_addr_t dma;			/* phys. address of descriptor ring */
+	unsigned int size;		/* length in bytes */
+
+	u8 queue_index;			/* needed for queue management */
+	u8 reg_idx;			/* holds the special value that gets
+					 * the hardware register offset
+					 * associated with this ring, which is
+					 * different for DCB and RSS modes
+					 */
+	u8 qos_pc;			/* priority class of queue */
+	u16 vid;			/* default vlan ID of queue */
+	u16 count;			/* amount of descriptors */
+
+	u16 next_to_alloc;
+	u16 next_to_use;
+	u16 next_to_clean;
+
+	struct fm10k_queue_stats stats;
+	struct u64_stats_sync syncp;
+	union {
+		/* Tx */
+		struct fm10k_tx_queue_stats tx_stats;
+		/* Rx */
+		struct {
+			struct fm10k_rx_queue_stats rx_stats;
+			struct sk_buff *skb;
+		};
+	};
+} ____cacheline_internodealigned_in_smp;
+
+struct fm10k_ring_container {
+	struct fm10k_ring *ring;	/* pointer to linked list of rings */
+	unsigned int total_bytes;	/* total bytes processed this int */
+	unsigned int total_packets;	/* total packets processed this int */
+	u16 work_limit;			/* total work allowed per interrupt */
+	u16 itr;			/* interrupt throttle rate value */
+	u8 count;			/* total number of rings in vector */
+};
+
+#define FM10K_ITR_MAX		0x0FFF	/* maximum value for ITR */
+#define FM10K_ITR_10K		100	/* 100us */
+#define FM10K_ITR_20K		50	/* 50us */
+#define FM10K_ITR_ADAPTIVE	0x8000	/* adaptive interrupt moderation flag */
+
+#define FM10K_ITR_ENABLE	(FM10K_ITR_AUTOMASK | FM10K_ITR_MASK_CLEAR)
+
+static inline struct netdev_queue *txring_txq(const struct fm10k_ring *ring)
+{
+	return &ring->netdev->_tx[ring->queue_index];
+}
+
+/* iterator for handling rings in ring container */
+#define fm10k_for_each_ring(pos, head) \
+	for (pos = &(head).ring[(head).count]; (--pos) >= (head).ring;)
+
+#define MAX_Q_VECTORS 256
+#define MIN_Q_VECTORS	1
+enum fm10k_non_q_vectors {
+	FM10K_MBX_VECTOR,
+#define NON_Q_VECTORS_VF NON_Q_VECTORS_PF
+	NON_Q_VECTORS_PF
+};
+
+#define NON_Q_VECTORS(hw)	(((hw)->mac.type == fm10k_mac_pf) ? \
+						NON_Q_VECTORS_PF : \
+						NON_Q_VECTORS_VF)
+#define MIN_MSIX_COUNT(hw)	(MIN_Q_VECTORS + NON_Q_VECTORS(hw))
+
+struct fm10k_q_vector {
+	struct fm10k_intfc *interface;
+	u32 __iomem *itr;	/* pointer to ITR register for this vector */
+	u16 v_idx;		/* index of q_vector within interface array */
+	struct fm10k_ring_container rx, tx;
+
+	struct napi_struct napi;
+	char name[IFNAMSIZ + 9];
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dbg_q_vector;
+#endif /* CONFIG_DEBUG_FS */
+	struct rcu_head rcu;	/* to avoid race with update stats on free */
+
+	/* for dynamic allocation of rings associated with this q_vector */
+	struct fm10k_ring ring[0] ____cacheline_internodealigned_in_smp;
+};
+
+enum fm10k_ring_f_enum {
+	RING_F_RSS,
+	RING_F_QOS,
+	RING_F_ARRAY_SIZE  /* must be last in enum set */
+};
+
+struct fm10k_ring_feature {
+	u16 limit;	/* upper limit on feature indices */
+	u16 indices;	/* current value of indices */
+	u16 mask;	/* Mask used for feature to ring mapping */
+	u16 offset;	/* offset to start of feature */
+};
+
+struct fm10k_iov_data {
+	unsigned int		num_vfs;
+	unsigned int		next_vf_mbx;
+	struct rcu_head		rcu;
+	struct fm10k_vf_info	vf_info[0];
+};
+
+#define fm10k_vxlan_port_for_each(vp, intfc) \
+	list_for_each_entry(vp, &(intfc)->vxlan_port, list)
+struct fm10k_vxlan_port {
+	struct list_head	list;
+	sa_family_t		sa_family;
+	__be16			port;
+};
+
+struct fm10k_intfc {
+	unsigned long active_vlans[BITS_TO_LONGS(VLAN_N_VID)];
+	struct net_device *netdev;
+	struct fm10k_l2_accel *l2_accel; /* pointer to L2 acceleration list */
+	struct pci_dev *pdev;
+	unsigned long state;
+
+	u32 flags;
+#define FM10K_FLAG_RESET_REQUESTED		(u32)(1 << 0)
+#define FM10K_FLAG_RSS_FIELD_IPV4_UDP		(u32)(1 << 1)
+#define FM10K_FLAG_RSS_FIELD_IPV6_UDP		(u32)(1 << 2)
+#define FM10K_FLAG_RX_TS_ENABLED		(u32)(1 << 3)
+#define FM10K_FLAG_SWPRI_CONFIG			(u32)(1 << 4)
+	int xcast_mode;
+
+	/* Tx fast path data */
+	int num_tx_queues;
+	u16 tx_itr;
+
+	/* Rx fast path data */
+	int num_rx_queues;
+	u16 rx_itr;
+
+	/* TX */
+	struct fm10k_ring *tx_ring[MAX_QUEUES] ____cacheline_aligned_in_smp;
+
+	u64 restart_queue;
+	u64 tx_busy;
+	u64 tx_csum_errors;
+	u64 alloc_failed;
+	u64 rx_csum_errors;
+	u64 rx_errors;
+
+	u64 tx_bytes_nic;
+	u64 tx_packets_nic;
+	u64 rx_bytes_nic;
+	u64 rx_packets_nic;
+	u64 rx_drops_nic;
+	u64 rx_overrun_pf;
+	u64 rx_overrun_vf;
+	u32 tx_timeout_count;
+
+	/* RX */
+	struct fm10k_ring *rx_ring[MAX_QUEUES];
+
+	/* Queueing vectors */
+	struct fm10k_q_vector *q_vector[MAX_Q_VECTORS];
+	struct msix_entry *msix_entries;
+	int num_q_vectors;	/* current number of q_vectors for device */
+	struct fm10k_ring_feature ring_feature[RING_F_ARRAY_SIZE];
+
+	/* SR-IOV information management structure */
+	struct fm10k_iov_data *iov_data;
+
+	struct fm10k_hw_stats stats;
+	struct fm10k_hw hw;
+	u32 __iomem *uc_addr;
+	u32 __iomem *sw_addr;
+	u16 msg_enable;
+	u16 tx_ring_count;
+	u16 rx_ring_count;
+	struct timer_list service_timer;
+	struct work_struct service_task;
+	unsigned long next_stats_update;
+	unsigned long next_tx_hang_check;
+	unsigned long last_reset;
+	unsigned long link_down_event;
+	bool host_ready;
+
+	u32 reta[FM10K_RETA_SIZE];
+	u32 rssrk[FM10K_RSSRK_SIZE];
+
+	/* VXLAN port tracking information */
+	struct list_head vxlan_port;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *dbg_intfc;
+
+#endif /* CONFIG_DEBUG_FS */
+	struct ptp_clock_info ptp_caps;
+	struct ptp_clock *ptp_clock;
+
+	struct sk_buff_head ts_tx_skb_queue;
+	u32 tx_hwtstamp_timeouts;
+
+	struct hwtstamp_config ts_config;
+	/* We are unable to actually adjust the clock beyond the frequency
+	 * value.  Once the clock is started there is no resetting it.  As
+	 * such we maintain a separate offset from the actual hardware clock
+	 * to allow for offset adjustment.
+	 */
+	s64 ptp_adjust;
+	rwlock_t systime_lock;
+#ifdef CONFIG_DCB
+	u8 pfc_en;
+#endif
+	u8 rx_pause;
+
+	/* GLORT resources in use by PF */
+	u16 glort;
+	u16 glort_count;
+
+	/* VLAN ID for updating multicast/unicast lists */
+	u16 vid;
+};
+
+enum fm10k_state_t {
+	__FM10K_RESETTING,
+	__FM10K_DOWN,
+	__FM10K_SERVICE_SCHED,
+	__FM10K_SERVICE_DISABLE,
+	__FM10K_MBX_LOCK,
+	__FM10K_LINK_DOWN,
+};
+
+static inline void fm10k_mbx_lock(struct fm10k_intfc *interface)
+{
+	/* busy loop if we cannot obtain the lock as some calls
+	 * such as ndo_set_rx_mode may be made in atomic context
+	 */
+	while (test_and_set_bit(__FM10K_MBX_LOCK, &interface->state))
+		udelay(20);
+}
+
+static inline void fm10k_mbx_unlock(struct fm10k_intfc *interface)
+{
+	/* flush memory to make sure state is correct */
+	smp_mb__before_atomic();
+	clear_bit(__FM10K_MBX_LOCK, &interface->state);
+}
+
+static inline int fm10k_mbx_trylock(struct fm10k_intfc *interface)
+{
+	return !test_and_set_bit(__FM10K_MBX_LOCK, &interface->state);
+}
+
+/* fm10k_test_staterr - test bits in Rx descriptor status and error fields */
+static inline __le32 fm10k_test_staterr(union fm10k_rx_desc *rx_desc,
+					const u32 stat_err_bits)
+{
+	return rx_desc->d.staterr & cpu_to_le32(stat_err_bits);
+}
+
+/* fm10k_desc_unused - calculate if we have unused descriptors */
+static inline u16 fm10k_desc_unused(struct fm10k_ring *ring)
+{
+	s16 unused = ring->next_to_clean - ring->next_to_use - 1;
+
+	return likely(unused < 0) ? unused + ring->count : unused;
+}
+
+#define FM10K_TX_DESC(R, i)	\
+	(&(((struct fm10k_tx_desc *)((R)->desc))[i]))
+#define FM10K_RX_DESC(R, i)	\
+	 (&(((union fm10k_rx_desc *)((R)->desc))[i]))
+
+#define FM10K_MAX_TXD_PWR	14
+#define FM10K_MAX_DATA_PER_TXD	(1 << FM10K_MAX_TXD_PWR)
+
+/* Tx Descriptors needed, worst case */
+#define TXD_USE_COUNT(S)	DIV_ROUND_UP((S), FM10K_MAX_DATA_PER_TXD)
+#define DESC_NEEDED	(MAX_SKB_FRAGS + 4)
+
+enum fm10k_tx_flags {
+	/* Tx offload flags */
+	FM10K_TX_FLAGS_CSUM	= 0x01,
+};
+
+/* This structure is stored as little endian values as that is the native
+ * format of the Rx descriptor.  The ordering of these fields is reversed
+ * from the actual ftag header to allow for a single bswap to take care
+ * of placing all of the values in network order
+ */
+union fm10k_ftag_info {
+	__le64 ftag;
+	struct {
+		/* dglort and sglort combined into a single 32bit desc read */
+		__le32 glort;
+		/* upper 16 bits of vlan are reserved 0 for swpri_type_user */
+		__le32 vlan;
+	} d;
+	struct {
+		__le16 dglort;
+		__le16 sglort;
+		__le16 vlan;
+		__le16 swpri_type_user;
+	} w;
+};
+
+struct fm10k_cb {
+	union {
+		__le64 tstamp;
+		unsigned long ts_tx_timeout;
+	};
+	union fm10k_ftag_info fi;
+};
+
+#define FM10K_CB(skb) ((struct fm10k_cb *)(skb)->cb)
+
+/* main */
+extern char fm10k_driver_name[];
+extern const char fm10k_driver_version[];
+int fm10k_init_queueing_scheme(struct fm10k_intfc *interface);
+void fm10k_clear_queueing_scheme(struct fm10k_intfc *interface);
+netdev_tx_t fm10k_xmit_frame_ring(struct sk_buff *skb,
+				  struct fm10k_ring *tx_ring);
+void fm10k_tx_timeout_reset(struct fm10k_intfc *interface);
+bool fm10k_check_tx_hang(struct fm10k_ring *tx_ring);
+void fm10k_alloc_rx_buffers(struct fm10k_ring *rx_ring, u16 cleaned_count);
+
+/* PCI */
+void fm10k_mbx_free_irq(struct fm10k_intfc *);
+int fm10k_mbx_request_irq(struct fm10k_intfc *);
+void fm10k_qv_free_irq(struct fm10k_intfc *interface);
+int fm10k_qv_request_irq(struct fm10k_intfc *interface);
+int fm10k_register_pci_driver(void);
+void fm10k_unregister_pci_driver(void);
+void fm10k_up(struct fm10k_intfc *interface);
+void fm10k_down(struct fm10k_intfc *interface);
+void fm10k_update_stats(struct fm10k_intfc *interface);
+void fm10k_service_event_schedule(struct fm10k_intfc *interface);
+void fm10k_update_rx_drop_en(struct fm10k_intfc *interface);
+
+/* Netdev */
+struct net_device *fm10k_alloc_netdev(void);
+int fm10k_setup_rx_resources(struct fm10k_ring *);
+int fm10k_setup_tx_resources(struct fm10k_ring *);
+void fm10k_free_rx_resources(struct fm10k_ring *);
+void fm10k_free_tx_resources(struct fm10k_ring *);
+void fm10k_clean_all_rx_rings(struct fm10k_intfc *);
+void fm10k_clean_all_tx_rings(struct fm10k_intfc *);
+void fm10k_unmap_and_free_tx_resource(struct fm10k_ring *,
+				      struct fm10k_tx_buffer *);
+void fm10k_restore_rx_state(struct fm10k_intfc *);
+void fm10k_reset_rx_state(struct fm10k_intfc *);
+int fm10k_setup_tc(struct net_device *dev, u8 tc);
+int fm10k_open(struct net_device *netdev);
+int fm10k_close(struct net_device *netdev);
+
+/* Ethtool */
+void fm10k_set_ethtool_ops(struct net_device *dev);
+
+/* IOV */
+s32 fm10k_iov_event(struct fm10k_intfc *interface);
+s32 fm10k_iov_mbx(struct fm10k_intfc *interface);
+void fm10k_iov_suspend(struct pci_dev *pdev);
+int fm10k_iov_resume(struct pci_dev *pdev);
+void fm10k_iov_disable(struct pci_dev *pdev);
+int fm10k_iov_configure(struct pci_dev *pdev, int num_vfs);
+s32 fm10k_iov_update_pvid(struct fm10k_intfc *interface, u16 glort, u16 pvid);
+int fm10k_ndo_set_vf_mac(struct net_device *netdev, int vf_idx, u8 *mac);
+int fm10k_ndo_set_vf_vlan(struct net_device *netdev,
+			  int vf_idx, u16 vid, u8 qos);
+int fm10k_ndo_set_vf_bw(struct net_device *netdev, int vf_idx, int rate,
+			int unused);
+int fm10k_ndo_get_vf_config(struct net_device *netdev,
+			    int vf_idx, struct ifla_vf_info *ivi);
+
+/* DebugFS */
+#ifdef CONFIG_DEBUG_FS
+void fm10k_dbg_q_vector_init(struct fm10k_q_vector *q_vector);
+void fm10k_dbg_q_vector_exit(struct fm10k_q_vector *q_vector);
+void fm10k_dbg_intfc_init(struct fm10k_intfc *interface);
+void fm10k_dbg_intfc_exit(struct fm10k_intfc *interface);
+void fm10k_dbg_init(void);
+void fm10k_dbg_exit(void);
+#else
+static inline void fm10k_dbg_q_vector_init(struct fm10k_q_vector *q_vector) {}
+static inline void fm10k_dbg_q_vector_exit(struct fm10k_q_vector *q_vector) {}
+static inline void fm10k_dbg_intfc_init(struct fm10k_intfc *interface) {}
+static inline void fm10k_dbg_intfc_exit(struct fm10k_intfc *interface) {}
+static inline void fm10k_dbg_init(void) {}
+static inline void fm10k_dbg_exit(void) {}
+#endif /* CONFIG_DEBUG_FS */
+
+/* Time Stamping */
+void fm10k_systime_to_hwtstamp(struct fm10k_intfc *interface,
+			       struct skb_shared_hwtstamps *hwtstamp,
+			       u64 systime);
+void fm10k_ts_tx_enqueue(struct fm10k_intfc *interface, struct sk_buff *skb);
+void fm10k_ts_tx_hwtstamp(struct fm10k_intfc *interface, __le16 dglort,
+			  u64 systime);
+void fm10k_ts_reset(struct fm10k_intfc *interface);
+void fm10k_ts_init(struct fm10k_intfc *interface);
+void fm10k_ts_tx_subtask(struct fm10k_intfc *interface);
+void fm10k_ptp_register(struct fm10k_intfc *interface);
+void fm10k_ptp_unregister(struct fm10k_intfc *interface);
+int fm10k_get_ts_config(struct net_device *netdev, struct ifreq *ifr);
+int fm10k_set_ts_config(struct net_device *netdev, struct ifreq *ifr);
+
+/* DCB */
+void fm10k_dcbnl_set_ops(struct net_device *dev);
+#endif /* _FM10K_H_ */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_common.c b/drivers/net/ethernet/intel/fm10k/fm10k_common.c
new file mode 100644
index 000000000000..bf19dccd4288
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_common.c
@@ -0,0 +1,534 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k_common.h"
+
+/**
+ *  fm10k_get_bus_info_generic - Generic set PCI bus info
+ *  @hw: pointer to hardware structure
+ *
+ *  Gets the PCI bus info (speed, width, type) then calls helper function to
+ *  store this data within the fm10k_hw structure.
+ **/
+s32 fm10k_get_bus_info_generic(struct fm10k_hw *hw)
+{
+	u16 link_cap, link_status, device_cap, device_control;
+
+	/* Get the maximum link width and speed from PCIe config space */
+	link_cap = fm10k_read_pci_cfg_word(hw, FM10K_PCIE_LINK_CAP);
+
+	switch (link_cap & FM10K_PCIE_LINK_WIDTH) {
+	case FM10K_PCIE_LINK_WIDTH_1:
+		hw->bus_caps.width = fm10k_bus_width_pcie_x1;
+		break;
+	case FM10K_PCIE_LINK_WIDTH_2:
+		hw->bus_caps.width = fm10k_bus_width_pcie_x2;
+		break;
+	case FM10K_PCIE_LINK_WIDTH_4:
+		hw->bus_caps.width = fm10k_bus_width_pcie_x4;
+		break;
+	case FM10K_PCIE_LINK_WIDTH_8:
+		hw->bus_caps.width = fm10k_bus_width_pcie_x8;
+		break;
+	default:
+		hw->bus_caps.width = fm10k_bus_width_unknown;
+		break;
+	}
+
+	switch (link_cap & FM10K_PCIE_LINK_SPEED) {
+	case FM10K_PCIE_LINK_SPEED_2500:
+		hw->bus_caps.speed = fm10k_bus_speed_2500;
+		break;
+	case FM10K_PCIE_LINK_SPEED_5000:
+		hw->bus_caps.speed = fm10k_bus_speed_5000;
+		break;
+	case FM10K_PCIE_LINK_SPEED_8000:
+		hw->bus_caps.speed = fm10k_bus_speed_8000;
+		break;
+	default:
+		hw->bus_caps.speed = fm10k_bus_speed_unknown;
+		break;
+	}
+
+	/* Get the PCIe maximum payload size for the PCIe function */
+	device_cap = fm10k_read_pci_cfg_word(hw, FM10K_PCIE_DEV_CAP);
+
+	switch (device_cap & FM10K_PCIE_DEV_CAP_PAYLOAD) {
+	case FM10K_PCIE_DEV_CAP_PAYLOAD_128:
+		hw->bus_caps.payload = fm10k_bus_payload_128;
+		break;
+	case FM10K_PCIE_DEV_CAP_PAYLOAD_256:
+		hw->bus_caps.payload = fm10k_bus_payload_256;
+		break;
+	case FM10K_PCIE_DEV_CAP_PAYLOAD_512:
+		hw->bus_caps.payload = fm10k_bus_payload_512;
+		break;
+	default:
+		hw->bus_caps.payload = fm10k_bus_payload_unknown;
+		break;
+	}
+
+	/* Get the negotiated link width and speed from PCIe config space */
+	link_status = fm10k_read_pci_cfg_word(hw, FM10K_PCIE_LINK_STATUS);
+
+	switch (link_status & FM10K_PCIE_LINK_WIDTH) {
+	case FM10K_PCIE_LINK_WIDTH_1:
+		hw->bus.width = fm10k_bus_width_pcie_x1;
+		break;
+	case FM10K_PCIE_LINK_WIDTH_2:
+		hw->bus.width = fm10k_bus_width_pcie_x2;
+		break;
+	case FM10K_PCIE_LINK_WIDTH_4:
+		hw->bus.width = fm10k_bus_width_pcie_x4;
+		break;
+	case FM10K_PCIE_LINK_WIDTH_8:
+		hw->bus.width = fm10k_bus_width_pcie_x8;
+		break;
+	default:
+		hw->bus.width = fm10k_bus_width_unknown;
+		break;
+	}
+
+	switch (link_status & FM10K_PCIE_LINK_SPEED) {
+	case FM10K_PCIE_LINK_SPEED_2500:
+		hw->bus.speed = fm10k_bus_speed_2500;
+		break;
+	case FM10K_PCIE_LINK_SPEED_5000:
+		hw->bus.speed = fm10k_bus_speed_5000;
+		break;
+	case FM10K_PCIE_LINK_SPEED_8000:
+		hw->bus.speed = fm10k_bus_speed_8000;
+		break;
+	default:
+		hw->bus.speed = fm10k_bus_speed_unknown;
+		break;
+	}
+
+	/* Get the negotiated PCIe maximum payload size for the PCIe function */
+	device_control = fm10k_read_pci_cfg_word(hw, FM10K_PCIE_DEV_CTRL);
+
+	switch (device_control & FM10K_PCIE_DEV_CTRL_PAYLOAD) {
+	case FM10K_PCIE_DEV_CTRL_PAYLOAD_128:
+		hw->bus.payload = fm10k_bus_payload_128;
+		break;
+	case FM10K_PCIE_DEV_CTRL_PAYLOAD_256:
+		hw->bus.payload = fm10k_bus_payload_256;
+		break;
+	case FM10K_PCIE_DEV_CTRL_PAYLOAD_512:
+		hw->bus.payload = fm10k_bus_payload_512;
+		break;
+	default:
+		hw->bus.payload = fm10k_bus_payload_unknown;
+		break;
+	}
+
+	return 0;
+}
+
+static u16 fm10k_get_pcie_msix_count_generic(struct fm10k_hw *hw)
+{
+	u16 msix_count;
+
+	/* read in value from MSI-X capability register */
+	msix_count = fm10k_read_pci_cfg_word(hw, FM10K_PCI_MSIX_MSG_CTRL);
+	msix_count &= FM10K_PCI_MSIX_MSG_CTRL_TBL_SZ_MASK;
+
+	/* MSI-X count is zero-based in HW */
+	msix_count++;
+
+	if (msix_count > FM10K_MAX_MSIX_VECTORS)
+		msix_count = FM10K_MAX_MSIX_VECTORS;
+
+	return msix_count;
+}
+
+/**
+ *  fm10k_get_invariants_generic - Inits constant values
+ *  @hw: pointer to the hardware structure
+ *
+ *  Initialize the common invariants for the device.
+ **/
+s32 fm10k_get_invariants_generic(struct fm10k_hw *hw)
+{
+	struct fm10k_mac_info *mac = &hw->mac;
+
+	/* initialize GLORT state to avoid any false hits */
+	mac->dglort_map = FM10K_DGLORTMAP_NONE;
+
+	/* record maximum number of MSI-X vectors */
+	mac->max_msix_vectors = fm10k_get_pcie_msix_count_generic(hw);
+
+	return 0;
+}
+
+/**
+ *  fm10k_start_hw_generic - Prepare hardware for Tx/Rx
+ *  @hw: pointer to hardware structure
+ *
+ *  This function sets the Tx ready flag to indicate that the Tx path has
+ *  been initialized.
+ **/
+s32 fm10k_start_hw_generic(struct fm10k_hw *hw)
+{
+	/* set flag indicating we are beginning Tx */
+	hw->mac.tx_ready = true;
+
+	return 0;
+}
+
+/**
+ *  fm10k_disable_queues_generic - Stop Tx/Rx queues
+ *  @hw: pointer to hardware structure
+ *  @q_cnt: number of queues to be disabled
+ *
+ **/
+s32 fm10k_disable_queues_generic(struct fm10k_hw *hw, u16 q_cnt)
+{
+	u32 reg;
+	u16 i, time;
+
+	/* clear tx_ready to prevent any false hits for reset */
+	hw->mac.tx_ready = false;
+
+	/* clear the enable bit for all rings */
+	for (i = 0; i < q_cnt; i++) {
+		reg = fm10k_read_reg(hw, FM10K_TXDCTL(i));
+		fm10k_write_reg(hw, FM10K_TXDCTL(i),
+				reg & ~FM10K_TXDCTL_ENABLE);
+		reg = fm10k_read_reg(hw, FM10K_RXQCTL(i));
+		fm10k_write_reg(hw, FM10K_RXQCTL(i),
+				reg & ~FM10K_RXQCTL_ENABLE);
+	}
+
+	fm10k_write_flush(hw);
+	udelay(1);
+
+	/* loop through all queues to verify that they are all disabled */
+	for (i = 0, time = FM10K_QUEUE_DISABLE_TIMEOUT; time;) {
+		/* if we are at end of rings all rings are disabled */
+		if (i == q_cnt)
+			return 0;
+
+		/* if queue enables cleared, then move to next ring pair */
+		reg = fm10k_read_reg(hw, FM10K_TXDCTL(i));
+		if (!~reg || !(reg & FM10K_TXDCTL_ENABLE)) {
+			reg = fm10k_read_reg(hw, FM10K_RXQCTL(i));
+			if (!~reg || !(reg & FM10K_RXQCTL_ENABLE)) {
+				i++;
+				continue;
+			}
+		}
+
+		/* decrement time and wait 1 usec */
+		time--;
+		if (time)
+			udelay(1);
+	}
+
+	return FM10K_ERR_REQUESTS_PENDING;
+}
+
+/**
+ *  fm10k_stop_hw_generic - Stop Tx/Rx units
+ *  @hw: pointer to hardware structure
+ *
+ **/
+s32 fm10k_stop_hw_generic(struct fm10k_hw *hw)
+{
+	return fm10k_disable_queues_generic(hw, hw->mac.max_queues);
+}
+
+/**
+ *  fm10k_read_hw_stats_32b - Reads value of 32-bit registers
+ *  @hw: pointer to the hardware structure
+ *  @addr: address of register containing a 32-bit value
+ *
+ *  Function reads the content of the register and returns the delta
+ *  between the base and the current value.
+ *  **/
+u32 fm10k_read_hw_stats_32b(struct fm10k_hw *hw, u32 addr,
+			    struct fm10k_hw_stat *stat)
+{
+	u32 delta = fm10k_read_reg(hw, addr) - stat->base_l;
+
+	if (FM10K_REMOVED(hw->hw_addr))
+		stat->base_h = 0;
+
+	return delta;
+}
+
+/**
+ *  fm10k_read_hw_stats_48b - Reads value of 48-bit registers
+ *  @hw: pointer to the hardware structure
+ *  @addr: address of register containing the lower 32-bit value
+ *
+ *  Function reads the content of 2 registers, combined to represent a 48-bit
+ *  statistical value. Extra processing is required to handle overflowing.
+ *  Finally, a delta value is returned representing the difference between the
+ *  values stored in registers and values stored in the statistic counters.
+ *  **/
+static u64 fm10k_read_hw_stats_48b(struct fm10k_hw *hw, u32 addr,
+				   struct fm10k_hw_stat *stat)
+{
+	u32 count_l;
+	u32 count_h;
+	u32 count_tmp;
+	u64 delta;
+
+	count_h = fm10k_read_reg(hw, addr + 1);
+
+	/* Check for overflow */
+	do {
+		count_tmp = count_h;
+		count_l = fm10k_read_reg(hw, addr);
+		count_h = fm10k_read_reg(hw, addr + 1);
+	} while (count_h != count_tmp);
+
+	delta = ((u64)(count_h - stat->base_h) << 32) + count_l;
+	delta -= stat->base_l;
+
+	return delta & FM10K_48_BIT_MASK;
+}
+
+/**
+ *  fm10k_update_hw_base_48b - Updates 48-bit statistic base value
+ *  @stat: pointer to the hardware statistic structure
+ *  @delta: value to be updated into the hardware statistic structure
+ *
+ *  Function receives a value and determines if an update is required based on
+ *  a delta calculation. Only the base value will be updated.
+ **/
+static void fm10k_update_hw_base_48b(struct fm10k_hw_stat *stat, u64 delta)
+{
+	if (!delta)
+		return;
+
+	/* update lower 32 bits */
+	delta += stat->base_l;
+	stat->base_l = (u32)delta;
+
+	/* update upper 32 bits */
+	stat->base_h += (u32)(delta >> 32);
+}
+
+/**
+ *  fm10k_update_hw_stats_tx_q - Updates TX queue statistics counters
+ *  @hw: pointer to the hardware structure
+ *  @q: pointer to the ring of hardware statistics queue
+ *  @idx: index pointing to the start of the ring iteration
+ *
+ *  Function updates the TX queue statistics counters that are related to the
+ *  hardware.
+ **/
+static void fm10k_update_hw_stats_tx_q(struct fm10k_hw *hw,
+				       struct fm10k_hw_stats_q *q,
+				       u32 idx)
+{
+	u32 id_tx, id_tx_prev, tx_packets;
+	u64 tx_bytes = 0;
+
+	/* Retrieve TX Owner Data */
+	id_tx = fm10k_read_reg(hw, FM10K_TXQCTL(idx));
+
+	/* Process TX Ring */
+	do {
+		tx_packets = fm10k_read_hw_stats_32b(hw, FM10K_QPTC(idx),
+						     &q->tx_packets);
+
+		if (tx_packets)
+			tx_bytes = fm10k_read_hw_stats_48b(hw,
+							   FM10K_QBTC_L(idx),
+							   &q->tx_bytes);
+
+		/* Re-Check Owner Data */
+		id_tx_prev = id_tx;
+		id_tx = fm10k_read_reg(hw, FM10K_TXQCTL(idx));
+	} while ((id_tx ^ id_tx_prev) & FM10K_TXQCTL_ID_MASK);
+
+	/* drop non-ID bits and set VALID ID bit */
+	id_tx &= FM10K_TXQCTL_ID_MASK;
+	id_tx |= FM10K_STAT_VALID;
+
+	/* update packet counts */
+	if (q->tx_stats_idx == id_tx) {
+		q->tx_packets.count += tx_packets;
+		q->tx_bytes.count += tx_bytes;
+	}
+
+	/* update bases and record ID */
+	fm10k_update_hw_base_32b(&q->tx_packets, tx_packets);
+	fm10k_update_hw_base_48b(&q->tx_bytes, tx_bytes);
+
+	q->tx_stats_idx = id_tx;
+}
+
+/**
+ *  fm10k_update_hw_stats_rx_q - Updates RX queue statistics counters
+ *  @hw: pointer to the hardware structure
+ *  @q: pointer to the ring of hardware statistics queue
+ *  @idx: index pointing to the start of the ring iteration
+ *
+ *  Function updates the RX queue statistics counters that are related to the
+ *  hardware.
+ **/
+static void fm10k_update_hw_stats_rx_q(struct fm10k_hw *hw,
+				       struct fm10k_hw_stats_q *q,
+				       u32 idx)
+{
+	u32 id_rx, id_rx_prev, rx_packets, rx_drops;
+	u64 rx_bytes = 0;
+
+	/* Retrieve RX Owner Data */
+	id_rx = fm10k_read_reg(hw, FM10K_RXQCTL(idx));
+
+	/* Process RX Ring*/
+	do {
+		rx_drops = fm10k_read_hw_stats_32b(hw, FM10K_QPRDC(idx),
+						   &q->rx_drops);
+
+		rx_packets = fm10k_read_hw_stats_32b(hw, FM10K_QPRC(idx),
+						     &q->rx_packets);
+
+		if (rx_packets)
+			rx_bytes = fm10k_read_hw_stats_48b(hw,
+							   FM10K_QBRC_L(idx),
+							   &q->rx_bytes);
+
+		/* Re-Check Owner Data */
+		id_rx_prev = id_rx;
+		id_rx = fm10k_read_reg(hw, FM10K_RXQCTL(idx));
+	} while ((id_rx ^ id_rx_prev) & FM10K_RXQCTL_ID_MASK);
+
+	/* drop non-ID bits and set VALID ID bit */
+	id_rx &= FM10K_RXQCTL_ID_MASK;
+	id_rx |= FM10K_STAT_VALID;
+
+	/* update packet counts */
+	if (q->rx_stats_idx == id_rx) {
+		q->rx_drops.count += rx_drops;
+		q->rx_packets.count += rx_packets;
+		q->rx_bytes.count += rx_bytes;
+	}
+
+	/* update bases and record ID */
+	fm10k_update_hw_base_32b(&q->rx_drops, rx_drops);
+	fm10k_update_hw_base_32b(&q->rx_packets, rx_packets);
+	fm10k_update_hw_base_48b(&q->rx_bytes, rx_bytes);
+
+	q->rx_stats_idx = id_rx;
+}
+
+/**
+ *  fm10k_update_hw_stats_q - Updates queue statistics counters
+ *  @hw: pointer to the hardware structure
+ *  @q: pointer to the ring of hardware statistics queue
+ *  @idx: index pointing to the start of the ring iteration
+ *  @count: number of queues to iterate over
+ *
+ *  Function updates the queue statistics counters that are related to the
+ *  hardware.
+ **/
+void fm10k_update_hw_stats_q(struct fm10k_hw *hw, struct fm10k_hw_stats_q *q,
+			     u32 idx, u32 count)
+{
+	u32 i;
+
+	for (i = 0; i < count; i++, idx++, q++) {
+		fm10k_update_hw_stats_tx_q(hw, q, idx);
+		fm10k_update_hw_stats_rx_q(hw, q, idx);
+	}
+}
+
+/**
+ *  fm10k_unbind_hw_stats_q - Unbind the queue counters from their queues
+ *  @hw: pointer to the hardware structure
+ *  @q: pointer to the ring of hardware statistics queue
+ *  @idx: index pointing to the start of the ring iteration
+ *  @count: number of queues to iterate over
+ *
+ *  Function invalidates the index values for the queues so any updates that
+ *  may have happened are ignored and the base for the queue stats is reset.
+ **/
+
+void fm10k_unbind_hw_stats_q(struct fm10k_hw_stats_q *q, u32 idx, u32 count)
+{
+	u32 i;
+
+	for (i = 0; i < count; i++, idx++, q++) {
+		q->rx_stats_idx = 0;
+		q->tx_stats_idx = 0;
+	}
+}
+
+/**
+ *  fm10k_get_host_state_generic - Returns the state of the host
+ *  @hw: pointer to hardware structure
+ *  @host_ready: pointer to boolean value that will record host state
+ *
+ *  This function will check the health of the mailbox and Tx queue 0
+ *  in order to determine if we should report that the link is up or not.
+ **/
+s32 fm10k_get_host_state_generic(struct fm10k_hw *hw, bool *host_ready)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	struct fm10k_mac_info *mac = &hw->mac;
+	s32 ret_val = 0;
+	u32 txdctl = fm10k_read_reg(hw, FM10K_TXDCTL(0));
+
+	/* process upstream mailbox in case interrupts were disabled */
+	mbx->ops.process(hw, mbx);
+
+	/* If Tx is no longer enabled link should come down */
+	if (!(~txdctl) || !(txdctl & FM10K_TXDCTL_ENABLE))
+		mac->get_host_state = true;
+
+	/* exit if not checking for link, or link cannot be changed */
+	if (!mac->get_host_state || !(~txdctl))
+		goto out;
+
+	/* if we somehow dropped the Tx enable we should reset */
+	if (hw->mac.tx_ready && !(txdctl & FM10K_TXDCTL_ENABLE)) {
+		ret_val = FM10K_ERR_RESET_REQUESTED;
+		goto out;
+	}
+
+	/* if Mailbox timed out we should request reset */
+	if (!mbx->timeout) {
+		ret_val = FM10K_ERR_RESET_REQUESTED;
+		goto out;
+	}
+
+	/* verify Mailbox is still valid */
+	if (!mbx->ops.tx_ready(mbx, FM10K_VFMBX_MSG_MTU))
+		goto out;
+
+	/* interface cannot receive traffic without logical ports */
+	if (mac->dglort_map == FM10K_DGLORTMAP_NONE)
+		goto out;
+
+	/* if we passed all the tests above then the switch is ready and we no
+	 * longer need to check for link
+	 */
+	mac->get_host_state = false;
+
+out:
+	*host_ready = !mac->get_host_state;
+	return ret_val;
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_common.h b/drivers/net/ethernet/intel/fm10k/fm10k_common.h
new file mode 100644
index 000000000000..45e4e5b1f20a
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_common.h
@@ -0,0 +1,65 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifndef _FM10K_COMMON_H_
+#define _FM10K_COMMON_H_
+
+#include "fm10k_type.h"
+
+#define FM10K_REMOVED(hw_addr) unlikely(!(hw_addr))
+
+/* PCI configuration read */
+u16 fm10k_read_pci_cfg_word(struct fm10k_hw *hw, u32 reg);
+
+/* read operations, indexed using DWORDS */
+u32 fm10k_read_reg(struct fm10k_hw *hw, int reg);
+
+/* write operations, indexed using DWORDS */
+#define fm10k_write_reg(hw, reg, val) \
+do { \
+	u32 __iomem *hw_addr = ACCESS_ONCE((hw)->hw_addr); \
+	if (!FM10K_REMOVED(hw_addr)) \
+		writel((val), &hw_addr[(reg)]); \
+} while (0)
+
+/* Switch register write operations, index using DWORDS */
+#define fm10k_write_sw_reg(hw, reg, val) \
+do { \
+	u32 __iomem *sw_addr = ACCESS_ONCE((hw)->sw_addr); \
+	if (!FM10K_REMOVED(sw_addr)) \
+		writel((val), &sw_addr[(reg)]); \
+} while (0)
+
+/* read ctrl register which has no clear on read fields as PCIe flush */
+#define fm10k_write_flush(hw) fm10k_read_reg((hw), FM10K_CTRL)
+s32 fm10k_get_bus_info_generic(struct fm10k_hw *hw);
+s32 fm10k_get_invariants_generic(struct fm10k_hw *hw);
+s32 fm10k_disable_queues_generic(struct fm10k_hw *hw, u16 q_cnt);
+s32 fm10k_start_hw_generic(struct fm10k_hw *hw);
+s32 fm10k_stop_hw_generic(struct fm10k_hw *hw);
+u32 fm10k_read_hw_stats_32b(struct fm10k_hw *hw, u32 addr,
+			    struct fm10k_hw_stat *stat);
+#define fm10k_update_hw_base_32b(stat, delta) ((stat)->base_l += (delta))
+void fm10k_update_hw_stats_q(struct fm10k_hw *hw, struct fm10k_hw_stats_q *q,
+			     u32 idx, u32 count);
+#define fm10k_unbind_hw_stats_32b(s) ((s)->base_h = 0)
+void fm10k_unbind_hw_stats_q(struct fm10k_hw_stats_q *q, u32 idx, u32 count);
+s32 fm10k_get_host_state_generic(struct fm10k_hw *hw, bool *host_ready);
+#endif /* _FM10K_COMMON_H_ */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c b/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
new file mode 100644
index 000000000000..212a92dad222
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_dcbnl.c
@@ -0,0 +1,174 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k.h"
+
+#ifdef CONFIG_DCB
+/**
+ * fm10k_dcbnl_ieee_getets - get the ETS configuration for the device
+ * @dev: netdev interface for the device
+ * @ets: ETS structure to push configuration to
+ **/
+static int fm10k_dcbnl_ieee_getets(struct net_device *dev, struct ieee_ets *ets)
+{
+	int i;
+
+	/* we support 8 TCs in all modes */
+	ets->ets_cap = IEEE_8021QAZ_MAX_TCS;
+	ets->cbs = 0;
+
+	/* we only support strict priority and cannot do traffic shaping */
+	memset(ets->tc_tx_bw, 0, sizeof(ets->tc_tx_bw));
+	memset(ets->tc_rx_bw, 0, sizeof(ets->tc_rx_bw));
+	memset(ets->tc_tsa, IEEE_8021QAZ_TSA_STRICT, sizeof(ets->tc_tsa));
+
+	/* populate the prio map based on the netdev */
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
+		ets->prio_tc[i] = netdev_get_prio_tc_map(dev, i);
+
+	return 0;
+}
+
+/**
+ * fm10k_dcbnl_ieee_setets - set the ETS configuration for the device
+ * @dev: netdev interface for the device
+ * @ets: ETS structure to pull configuration from
+ **/
+static int fm10k_dcbnl_ieee_setets(struct net_device *dev, struct ieee_ets *ets)
+{
+	u8 num_tc = 0;
+	int i, err;
+
+	/* verify type and determine num_tcs needed */
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++) {
+		if (ets->tc_tx_bw[i] || ets->tc_rx_bw[i])
+			return -EINVAL;
+		if (ets->tc_tsa[i] != IEEE_8021QAZ_TSA_STRICT)
+			return -EINVAL;
+		if (ets->prio_tc[i] > num_tc)
+			num_tc = ets->prio_tc[i];
+	}
+
+	/* if requested TC is greater than 0 then num_tcs is max + 1 */
+	if (num_tc)
+		num_tc++;
+
+	if (num_tc > IEEE_8021QAZ_MAX_TCS)
+		return -EINVAL;
+
+	/* update TC hardware mapping if necessary */
+	if (num_tc != netdev_get_num_tc(dev)) {
+		err = fm10k_setup_tc(dev, num_tc);
+		if (err)
+			return err;
+	}
+
+	/* update priority mapping */
+	for (i = 0; i < IEEE_8021QAZ_MAX_TCS; i++)
+		netdev_set_prio_tc_map(dev, i, ets->prio_tc[i]);
+
+	return 0;
+}
+
+/**
+ * fm10k_dcbnl_ieee_getpfc - get the PFC configuration for the device
+ * @dev: netdev interface for the device
+ * @pfc: PFC structure to push configuration to
+ **/
+static int fm10k_dcbnl_ieee_getpfc(struct net_device *dev, struct ieee_pfc *pfc)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+
+	/* record flow control max count and state of TCs */
+	pfc->pfc_cap = IEEE_8021QAZ_MAX_TCS;
+	pfc->pfc_en = interface->pfc_en;
+
+	return 0;
+}
+
+/**
+ * fm10k_dcbnl_ieee_setpfc - set the PFC configuration for the device
+ * @dev: netdev interface for the device
+ * @pfc: PFC structure to pull configuration from
+ **/
+static int fm10k_dcbnl_ieee_setpfc(struct net_device *dev, struct ieee_pfc *pfc)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+
+	/* record PFC configuration to interface */
+	interface->pfc_en = pfc->pfc_en;
+
+	/* if we are running update the drop_en state for all queues */
+	if (netif_running(dev))
+		fm10k_update_rx_drop_en(interface);
+
+	return 0;
+}
+
+/**
+ * fm10k_dcbnl_ieee_getdcbx - get the DCBX configuration for the device
+ * @dev: netdev interface for the device
+ *
+ * Returns that we support only IEEE DCB for this interface
+ **/
+static u8 fm10k_dcbnl_getdcbx(struct net_device *dev)
+{
+	return DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE;
+}
+
+/**
+ * fm10k_dcbnl_ieee_setdcbx - get the DCBX configuration for the device
+ * @dev: netdev interface for the device
+ * @mode: new mode for this device
+ *
+ * Returns error on attempt to enable anything but IEEE DCB for this interface
+ **/
+static u8 fm10k_dcbnl_setdcbx(struct net_device *dev, u8 mode)
+{
+	return (mode != (DCB_CAP_DCBX_HOST | DCB_CAP_DCBX_VER_IEEE)) ? 1 : 0;
+}
+
+static const struct dcbnl_rtnl_ops fm10k_dcbnl_ops = {
+	.ieee_getets	= fm10k_dcbnl_ieee_getets,
+	.ieee_setets	= fm10k_dcbnl_ieee_setets,
+	.ieee_getpfc	= fm10k_dcbnl_ieee_getpfc,
+	.ieee_setpfc	= fm10k_dcbnl_ieee_setpfc,
+
+	.getdcbx	= fm10k_dcbnl_getdcbx,
+	.setdcbx	= fm10k_dcbnl_setdcbx,
+};
+
+#endif /* CONFIG_DCB */
+/**
+ * fm10k_dcbnl_set_ops - Configures dcbnl ops pointer for netdev
+ * @dev: netdev interface for the device
+ *
+ * Enables PF for DCB by assigning DCBNL ops pointer.
+ **/
+void fm10k_dcbnl_set_ops(struct net_device *dev)
+{
+#ifdef CONFIG_DCB
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+
+	if (hw->mac.type == fm10k_mac_pf)
+		dev->dcbnl_ops = &fm10k_dcbnl_ops;
+#endif /* CONFIG_DCB */
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c b/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
new file mode 100644
index 000000000000..4327f86218b9
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_debugfs.c
@@ -0,0 +1,259 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifdef CONFIG_DEBUG_FS
+
+#include "fm10k.h"
+
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+static struct dentry *dbg_root;
+
+/* Descriptor Seq Functions */
+
+static void *fm10k_dbg_desc_seq_start(struct seq_file *s, loff_t *pos)
+{
+	struct fm10k_ring *ring = s->private;
+
+	return (*pos < ring->count) ? pos : NULL;
+}
+
+static void *fm10k_dbg_desc_seq_next(struct seq_file *s, void *v, loff_t *pos)
+{
+	struct fm10k_ring *ring = s->private;
+
+	return (++(*pos) < ring->count) ? pos : NULL;
+}
+
+static void fm10k_dbg_desc_seq_stop(struct seq_file *s, void *v)
+{
+	/* Do nothing. */
+}
+
+static void fm10k_dbg_desc_break(struct seq_file *s, int i)
+{
+	while (i--)
+		seq_puts(s, "-");
+
+	seq_puts(s, "\n");
+}
+
+static int fm10k_dbg_tx_desc_seq_show(struct seq_file *s, void *v)
+{
+	struct fm10k_ring *ring = s->private;
+	int i = *(loff_t *)v;
+	static const char tx_desc_hdr[] =
+		"DES BUFFER_ADDRESS     LENGTH VLAN   MSS    HDRLEN FLAGS\n";
+
+	/* Generate header */
+	if (!i) {
+		seq_printf(s, tx_desc_hdr);
+		fm10k_dbg_desc_break(s, sizeof(tx_desc_hdr) - 1);
+	}
+
+	/* Validate descriptor allocation */
+	if (!ring->desc) {
+		seq_printf(s, "%03X Descriptor ring not allocated.\n", i);
+	} else {
+		struct fm10k_tx_desc *txd = FM10K_TX_DESC(ring, i);
+
+		seq_printf(s, "%03X %#018llx %#06x %#06x %#06x %#06x %#04x\n",
+			   i, txd->buffer_addr, txd->buflen, txd->vlan,
+			   txd->mss, txd->hdrlen, txd->flags);
+	}
+
+	return 0;
+}
+
+static int fm10k_dbg_rx_desc_seq_show(struct seq_file *s, void *v)
+{
+	struct fm10k_ring *ring = s->private;
+	int i = *(loff_t *)v;
+	static const char rx_desc_hdr[] =
+	"DES DATA       RSS        STATERR    LENGTH VLAN   DGLORT SGLORT TIMESTAMP\n";
+
+	/* Generate header */
+	if (!i) {
+		seq_printf(s, rx_desc_hdr);
+		fm10k_dbg_desc_break(s, sizeof(rx_desc_hdr) - 1);
+	}
+
+	/* Validate descriptor allocation */
+	if (!ring->desc) {
+		seq_printf(s, "%03X Descriptor ring not allocated.\n", i);
+	} else {
+		union fm10k_rx_desc *rxd = FM10K_RX_DESC(ring, i);
+
+		seq_printf(s,
+			   "%03X %#010x %#010x %#010x %#06x %#06x %#06x %#06x %#018llx\n",
+			   i, rxd->d.data, rxd->d.rss, rxd->d.staterr,
+			   rxd->w.length, rxd->w.vlan, rxd->w.dglort,
+			   rxd->w.sglort, rxd->q.timestamp);
+	}
+
+	return 0;
+}
+
+static const struct seq_operations fm10k_dbg_tx_desc_seq_ops = {
+	.start = fm10k_dbg_desc_seq_start,
+	.next  = fm10k_dbg_desc_seq_next,
+	.stop  = fm10k_dbg_desc_seq_stop,
+	.show  = fm10k_dbg_tx_desc_seq_show,
+};
+
+static const struct seq_operations fm10k_dbg_rx_desc_seq_ops = {
+	.start = fm10k_dbg_desc_seq_start,
+	.next  = fm10k_dbg_desc_seq_next,
+	.stop  = fm10k_dbg_desc_seq_stop,
+	.show  = fm10k_dbg_rx_desc_seq_show,
+};
+
+static int fm10k_dbg_desc_open(struct inode *inode, struct file *filep)
+{
+	struct fm10k_ring *ring = inode->i_private;
+	struct fm10k_q_vector *q_vector = ring->q_vector;
+	const struct seq_operations *desc_seq_ops;
+	int err;
+
+	if (ring < q_vector->rx.ring)
+		desc_seq_ops = &fm10k_dbg_tx_desc_seq_ops;
+	else
+		desc_seq_ops = &fm10k_dbg_rx_desc_seq_ops;
+
+	err = seq_open(filep, desc_seq_ops);
+	if (err)
+		return err;
+
+	((struct seq_file *)filep->private_data)->private = ring;
+
+	return 0;
+}
+
+static const struct file_operations fm10k_dbg_desc_fops = {
+	.owner   = THIS_MODULE,
+	.open    = fm10k_dbg_desc_open,
+	.read    = seq_read,
+	.llseek  = seq_lseek,
+	.release = seq_release,
+};
+
+/**
+ * fm10k_dbg_q_vector_init - setup debugfs for the q_vectors
+ * @q_vector: q_vector to allocate directories for
+ *
+ * A folder is created for each q_vector found. In each q_vector
+ * folder, a debugfs file is created for each tx and rx ring
+ * allocated to the q_vector.
+ **/
+void fm10k_dbg_q_vector_init(struct fm10k_q_vector *q_vector)
+{
+	struct fm10k_intfc *interface = q_vector->interface;
+	char name[16];
+	int i;
+
+	if (!interface->dbg_intfc)
+		return;
+
+	/* Generate a folder for each q_vector */
+	sprintf(name, "q_vector.%03d", q_vector->v_idx);
+
+	q_vector->dbg_q_vector = debugfs_create_dir(name, interface->dbg_intfc);
+	if (!q_vector->dbg_q_vector)
+		return;
+
+	/* Generate a file for each rx ring in the q_vector */
+	for (i = 0; i < q_vector->tx.count; i++) {
+		struct fm10k_ring *ring = &q_vector->tx.ring[i];
+
+		sprintf(name, "tx_ring.%03d", ring->queue_index);
+
+		debugfs_create_file(name, 0600,
+				    q_vector->dbg_q_vector, ring,
+				    &fm10k_dbg_desc_fops);
+	}
+
+	/* Generate a file for each rx ring in the q_vector */
+	for (i = 0; i < q_vector->rx.count; i++) {
+		struct fm10k_ring *ring = &q_vector->rx.ring[i];
+
+		sprintf(name, "rx_ring.%03d", ring->queue_index);
+
+		debugfs_create_file(name, 0600,
+				    q_vector->dbg_q_vector, ring,
+				    &fm10k_dbg_desc_fops);
+	}
+}
+
+/**
+ * fm10k_dbg_free_q_vector_dir - setup debugfs for the q_vectors
+ * @q_vector: q_vector to allocate directories for
+ **/
+void fm10k_dbg_q_vector_exit(struct fm10k_q_vector *q_vector)
+{
+	struct fm10k_intfc *interface = q_vector->interface;
+
+	if (interface->dbg_intfc)
+		debugfs_remove_recursive(q_vector->dbg_q_vector);
+	q_vector->dbg_q_vector = NULL;
+}
+
+/**
+ * fm10k_dbg_intfc_init - setup the debugfs directory for the intferface
+ * @interface: the interface that is starting up
+ **/
+
+void fm10k_dbg_intfc_init(struct fm10k_intfc *interface)
+{
+	const char *name = pci_name(interface->pdev);
+
+	if (dbg_root)
+		interface->dbg_intfc = debugfs_create_dir(name, dbg_root);
+}
+
+/**
+ * fm10k_dbg_intfc_exit - clean out the interface's debugfs entries
+ * @interface: the interface that is stopping
+ **/
+void fm10k_dbg_intfc_exit(struct fm10k_intfc *interface)
+{
+	if (dbg_root)
+		debugfs_remove_recursive(interface->dbg_intfc);
+	interface->dbg_intfc = NULL;
+}
+
+/**
+ * fm10k_dbg_init - start up debugfs for the driver
+ **/
+void fm10k_dbg_init(void)
+{
+	dbg_root = debugfs_create_dir(fm10k_driver_name, NULL);
+}
+
+/**
+ * fm10k_dbg_exit - clean out the driver's debugfs entries
+ **/
+void fm10k_dbg_exit(void)
+{
+	debugfs_remove_recursive(dbg_root);
+	dbg_root = NULL;
+}
+
+#endif /* CONFIG_DEBUG_FS */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
new file mode 100644
index 000000000000..2d04464e6aa3
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ethtool.c
@@ -0,0 +1,1071 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include <linux/vmalloc.h>
+
+#include "fm10k.h"
+
+struct fm10k_stats {
+	char stat_string[ETH_GSTRING_LEN];
+	int sizeof_stat;
+	int stat_offset;
+};
+
+#define FM10K_NETDEV_STAT(_net_stat) { \
+	.stat_string = #_net_stat, \
+	.sizeof_stat = FIELD_SIZEOF(struct net_device_stats, _net_stat), \
+	.stat_offset = offsetof(struct net_device_stats, _net_stat) \
+}
+
+static const struct fm10k_stats fm10k_gstrings_net_stats[] = {
+	FM10K_NETDEV_STAT(tx_packets),
+	FM10K_NETDEV_STAT(tx_bytes),
+	FM10K_NETDEV_STAT(tx_errors),
+	FM10K_NETDEV_STAT(rx_packets),
+	FM10K_NETDEV_STAT(rx_bytes),
+	FM10K_NETDEV_STAT(rx_errors),
+	FM10K_NETDEV_STAT(rx_dropped),
+
+	/* detailed Rx errors */
+	FM10K_NETDEV_STAT(rx_length_errors),
+	FM10K_NETDEV_STAT(rx_crc_errors),
+	FM10K_NETDEV_STAT(rx_fifo_errors),
+};
+
+#define FM10K_NETDEV_STATS_LEN	ARRAY_SIZE(fm10k_gstrings_net_stats)
+
+#define FM10K_STAT(_name, _stat) { \
+	.stat_string = _name, \
+	.sizeof_stat = FIELD_SIZEOF(struct fm10k_intfc, _stat), \
+	.stat_offset = offsetof(struct fm10k_intfc, _stat) \
+}
+
+static const struct fm10k_stats fm10k_gstrings_stats[] = {
+	FM10K_STAT("tx_restart_queue", restart_queue),
+	FM10K_STAT("tx_busy", tx_busy),
+	FM10K_STAT("tx_csum_errors", tx_csum_errors),
+	FM10K_STAT("rx_alloc_failed", alloc_failed),
+	FM10K_STAT("rx_csum_errors", rx_csum_errors),
+	FM10K_STAT("rx_errors", rx_errors),
+
+	FM10K_STAT("tx_packets_nic", tx_packets_nic),
+	FM10K_STAT("tx_bytes_nic", tx_bytes_nic),
+	FM10K_STAT("rx_packets_nic", rx_packets_nic),
+	FM10K_STAT("rx_bytes_nic", rx_bytes_nic),
+	FM10K_STAT("rx_drops_nic", rx_drops_nic),
+	FM10K_STAT("rx_overrun_pf", rx_overrun_pf),
+	FM10K_STAT("rx_overrun_vf", rx_overrun_vf),
+
+	FM10K_STAT("timeout", stats.timeout.count),
+	FM10K_STAT("ur", stats.ur.count),
+	FM10K_STAT("ca", stats.ca.count),
+	FM10K_STAT("um", stats.um.count),
+	FM10K_STAT("xec", stats.xec.count),
+	FM10K_STAT("vlan_drop", stats.vlan_drop.count),
+	FM10K_STAT("loopback_drop", stats.loopback_drop.count),
+	FM10K_STAT("nodesc_drop", stats.nodesc_drop.count),
+
+	FM10K_STAT("swapi_status", hw.swapi.status),
+	FM10K_STAT("mac_rules_used", hw.swapi.mac.used),
+	FM10K_STAT("mac_rules_avail", hw.swapi.mac.avail),
+
+	FM10K_STAT("mbx_tx_busy", hw.mbx.tx_busy),
+	FM10K_STAT("mbx_tx_dropped", hw.mbx.tx_dropped),
+	FM10K_STAT("mbx_tx_messages", hw.mbx.tx_messages),
+	FM10K_STAT("mbx_tx_dwords", hw.mbx.tx_dwords),
+	FM10K_STAT("mbx_rx_messages", hw.mbx.rx_messages),
+	FM10K_STAT("mbx_rx_dwords", hw.mbx.rx_dwords),
+	FM10K_STAT("mbx_rx_parse_err", hw.mbx.rx_parse_err),
+
+	FM10K_STAT("tx_hwtstamp_timeouts", tx_hwtstamp_timeouts),
+};
+
+#define FM10K_GLOBAL_STATS_LEN ARRAY_SIZE(fm10k_gstrings_stats)
+
+#define FM10K_QUEUE_STATS_LEN \
+	  (MAX_QUEUES * 2 * (sizeof(struct fm10k_queue_stats) / sizeof(u64)))
+
+#define FM10K_STATS_LEN (FM10K_GLOBAL_STATS_LEN + \
+			 FM10K_NETDEV_STATS_LEN + \
+			 FM10K_QUEUE_STATS_LEN)
+
+static const char fm10k_gstrings_test[][ETH_GSTRING_LEN] = {
+	"Mailbox test (on/offline)"
+};
+
+#define FM10K_TEST_LEN (sizeof(fm10k_gstrings_test) / ETH_GSTRING_LEN)
+
+enum fm10k_self_test_types {
+	FM10K_TEST_MBX,
+	FM10K_TEST_MAX = FM10K_TEST_LEN
+};
+
+static void fm10k_get_strings(struct net_device *dev, u32 stringset,
+			      u8 *data)
+{
+	char *p = (char *)data;
+	int i;
+
+	switch (stringset) {
+	case ETH_SS_TEST:
+		memcpy(data, *fm10k_gstrings_test,
+		       FM10K_TEST_LEN * ETH_GSTRING_LEN);
+		break;
+	case ETH_SS_STATS:
+		for (i = 0; i < FM10K_NETDEV_STATS_LEN; i++) {
+			memcpy(p, fm10k_gstrings_net_stats[i].stat_string,
+			       ETH_GSTRING_LEN);
+			p += ETH_GSTRING_LEN;
+		}
+		for (i = 0; i < FM10K_GLOBAL_STATS_LEN; i++) {
+			memcpy(p, fm10k_gstrings_stats[i].stat_string,
+			       ETH_GSTRING_LEN);
+			p += ETH_GSTRING_LEN;
+		}
+
+		for (i = 0; i < MAX_QUEUES; i++) {
+			sprintf(p, "tx_queue_%u_packets", i);
+			p += ETH_GSTRING_LEN;
+			sprintf(p, "tx_queue_%u_bytes", i);
+			p += ETH_GSTRING_LEN;
+			sprintf(p, "rx_queue_%u_packets", i);
+			p += ETH_GSTRING_LEN;
+			sprintf(p, "rx_queue_%u_bytes", i);
+			p += ETH_GSTRING_LEN;
+		}
+		break;
+	}
+}
+
+static int fm10k_get_sset_count(struct net_device *dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_TEST:
+		return FM10K_TEST_LEN;
+	case ETH_SS_STATS:
+		return FM10K_STATS_LEN;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void fm10k_get_ethtool_stats(struct net_device *netdev,
+				    struct ethtool_stats *stats, u64 *data)
+{
+	const int stat_count = sizeof(struct fm10k_queue_stats) / sizeof(u64);
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct net_device_stats *net_stats = &netdev->stats;
+	char *p;
+	int i, j;
+
+	fm10k_update_stats(interface);
+
+	for (i = 0; i < FM10K_NETDEV_STATS_LEN; i++) {
+		p = (char *)net_stats + fm10k_gstrings_net_stats[i].stat_offset;
+		*(data++) = (fm10k_gstrings_net_stats[i].sizeof_stat ==
+			sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
+	}
+
+	for (i = 0; i < FM10K_GLOBAL_STATS_LEN; i++) {
+		p = (char *)interface + fm10k_gstrings_stats[i].stat_offset;
+		*(data++) = (fm10k_gstrings_stats[i].sizeof_stat ==
+			sizeof(u64)) ? *(u64 *)p : *(u32 *)p;
+	}
+
+	for (i = 0; i < MAX_QUEUES; i++) {
+		struct fm10k_ring *ring;
+		u64 *queue_stat;
+
+		ring = interface->tx_ring[i];
+		if (ring)
+			queue_stat = (u64 *)&ring->stats;
+		for (j = 0; j < stat_count; j++)
+			*(data++) = ring ? queue_stat[j] : 0;
+
+		ring = interface->rx_ring[i];
+		if (ring)
+			queue_stat = (u64 *)&ring->stats;
+		for (j = 0; j < stat_count; j++)
+			*(data++) = ring ? queue_stat[j] : 0;
+	}
+}
+
+/* If function below adds more registers this define needs to be updated */
+#define FM10K_REGS_LEN_Q 29
+
+static void fm10k_get_reg_q(struct fm10k_hw *hw, u32 *buff, int i)
+{
+	int idx = 0;
+
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RDBAL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RDBAH(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RDLEN(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TPH_RXCTRL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RDH(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RDT(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RXQCTL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RXDCTL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_RXINT(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_SRRCTL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_QPRC(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_QPRDC(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_QBRC_L(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_QBRC_H(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TDBAL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TDBAH(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TDLEN(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TPH_TXCTRL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TDH(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TDT(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TXDCTL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TXQCTL(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TXINT(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_QPTC(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_QBTC_L(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_QBTC_H(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TQDLOC(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_TX_SGLORT(i));
+	buff[idx++] = fm10k_read_reg(hw, FM10K_PFVTCTL(i));
+
+	BUG_ON(idx != FM10K_REGS_LEN_Q);
+}
+
+/* If function above adds more registers this define needs to be updated */
+#define FM10K_REGS_LEN_VSI 43
+
+static void fm10k_get_reg_vsi(struct fm10k_hw *hw, u32 *buff, int i)
+{
+	int idx = 0, j;
+
+	buff[idx++] = fm10k_read_reg(hw, FM10K_MRQC(i));
+	for (j = 0; j < 10; j++)
+		buff[idx++] = fm10k_read_reg(hw, FM10K_RSSRK(i, j));
+	for (j = 0; j < 32; j++)
+		buff[idx++] = fm10k_read_reg(hw, FM10K_RETA(i, j));
+
+	BUG_ON(idx != FM10K_REGS_LEN_VSI);
+}
+
+static void fm10k_get_regs(struct net_device *netdev,
+			   struct ethtool_regs *regs, void *p)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_hw *hw = &interface->hw;
+	u32 *buff = p;
+	u16 i;
+
+	regs->version = (1 << 24) | (hw->revision_id << 16) | hw->device_id;
+
+	switch (hw->mac.type) {
+	case fm10k_mac_pf:
+		/* General PF Registers */
+		*(buff++) = fm10k_read_reg(hw, FM10K_CTRL);
+		*(buff++) = fm10k_read_reg(hw, FM10K_CTRL_EXT);
+		*(buff++) = fm10k_read_reg(hw, FM10K_GCR);
+		*(buff++) = fm10k_read_reg(hw, FM10K_GCR_EXT);
+
+		for (i = 0; i < 8; i++) {
+			*(buff++) = fm10k_read_reg(hw, FM10K_DGLORTMAP(i));
+			*(buff++) = fm10k_read_reg(hw, FM10K_DGLORTDEC(i));
+		}
+
+		for (i = 0; i < 65; i++) {
+			fm10k_get_reg_vsi(hw, buff, i);
+			buff += FM10K_REGS_LEN_VSI;
+		}
+
+		*(buff++) = fm10k_read_reg(hw, FM10K_DMA_CTRL);
+		*(buff++) = fm10k_read_reg(hw, FM10K_DMA_CTRL2);
+
+		for (i = 0; i < FM10K_MAX_QUEUES_PF; i++) {
+			fm10k_get_reg_q(hw, buff, i);
+			buff += FM10K_REGS_LEN_Q;
+		}
+
+		*(buff++) = fm10k_read_reg(hw, FM10K_TPH_CTRL);
+
+		for (i = 0; i < 8; i++)
+			*(buff++) = fm10k_read_reg(hw, FM10K_INT_MAP(i));
+
+		/* Interrupt Throttling Registers */
+		for (i = 0; i < 130; i++)
+			*(buff++) = fm10k_read_reg(hw, FM10K_ITR(i));
+
+		break;
+	case fm10k_mac_vf:
+		/* General VF registers */
+		*(buff++) = fm10k_read_reg(hw, FM10K_VFCTRL);
+		*(buff++) = fm10k_read_reg(hw, FM10K_VFINT_MAP);
+		*(buff++) = fm10k_read_reg(hw, FM10K_VFSYSTIME);
+
+		/* Interrupt Throttling Registers */
+		for (i = 0; i < 8; i++)
+			*(buff++) = fm10k_read_reg(hw, FM10K_VFITR(i));
+
+		fm10k_get_reg_vsi(hw, buff, 0);
+		buff += FM10K_REGS_LEN_VSI;
+
+		for (i = 0; i < FM10K_MAX_QUEUES_POOL; i++) {
+			if (i < hw->mac.max_queues)
+				fm10k_get_reg_q(hw, buff, i);
+			else
+				memset(buff, 0, sizeof(u32) * FM10K_REGS_LEN_Q);
+			buff += FM10K_REGS_LEN_Q;
+		}
+
+		break;
+	default:
+		return;
+	}
+}
+
+/* If function above adds more registers these define need to be updated */
+#define FM10K_REGS_LEN_PF \
+(162 + (65 * FM10K_REGS_LEN_VSI) + (FM10K_MAX_QUEUES_PF * FM10K_REGS_LEN_Q))
+#define FM10K_REGS_LEN_VF \
+(11 + FM10K_REGS_LEN_VSI + (FM10K_MAX_QUEUES_POOL * FM10K_REGS_LEN_Q))
+
+static int fm10k_get_regs_len(struct net_device *netdev)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_hw *hw = &interface->hw;
+
+	switch (hw->mac.type) {
+	case fm10k_mac_pf:
+		return FM10K_REGS_LEN_PF * sizeof(u32);
+	case fm10k_mac_vf:
+		return FM10K_REGS_LEN_VF * sizeof(u32);
+	default:
+		return 0;
+	}
+}
+
+static void fm10k_get_drvinfo(struct net_device *dev,
+			      struct ethtool_drvinfo *info)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+
+	strncpy(info->driver, fm10k_driver_name,
+		sizeof(info->driver) - 1);
+	strncpy(info->version, fm10k_driver_version,
+		sizeof(info->version) - 1);
+	strncpy(info->bus_info, pci_name(interface->pdev),
+		sizeof(info->bus_info) - 1);
+
+	info->n_stats = FM10K_STATS_LEN;
+
+	info->regdump_len = fm10k_get_regs_len(dev);
+}
+
+static void fm10k_get_pauseparam(struct net_device *dev,
+				 struct ethtool_pauseparam *pause)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+
+	/* record fixed values for autoneg and tx pause */
+	pause->autoneg = 0;
+	pause->tx_pause = 1;
+
+	pause->rx_pause = interface->rx_pause ? 1 : 0;
+}
+
+static int fm10k_set_pauseparam(struct net_device *dev,
+				struct ethtool_pauseparam *pause)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+
+	if (pause->autoneg || !pause->tx_pause)
+		return -EINVAL;
+
+	/* we can only support pause on the PF to avoid head-of-line blocking */
+	if (hw->mac.type == fm10k_mac_pf)
+		interface->rx_pause = pause->rx_pause ? ~0 : 0;
+	else if (pause->rx_pause)
+		return -EINVAL;
+
+	if (netif_running(dev))
+		fm10k_update_rx_drop_en(interface);
+
+	return 0;
+}
+
+static u32 fm10k_get_msglevel(struct net_device *netdev)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+
+	return interface->msg_enable;
+}
+
+static void fm10k_set_msglevel(struct net_device *netdev, u32 data)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+
+	interface->msg_enable = data;
+}
+
+static void fm10k_get_ringparam(struct net_device *netdev,
+				struct ethtool_ringparam *ring)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+
+	ring->rx_max_pending = FM10K_MAX_RXD;
+	ring->tx_max_pending = FM10K_MAX_TXD;
+	ring->rx_mini_max_pending = 0;
+	ring->rx_jumbo_max_pending = 0;
+	ring->rx_pending = interface->rx_ring_count;
+	ring->tx_pending = interface->tx_ring_count;
+	ring->rx_mini_pending = 0;
+	ring->rx_jumbo_pending = 0;
+}
+
+static int fm10k_set_ringparam(struct net_device *netdev,
+			       struct ethtool_ringparam *ring)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_ring *temp_ring;
+	int i, err = 0;
+	u32 new_rx_count, new_tx_count;
+
+	if ((ring->rx_mini_pending) || (ring->rx_jumbo_pending))
+		return -EINVAL;
+
+	new_tx_count = clamp_t(u32, ring->tx_pending,
+			       FM10K_MIN_TXD, FM10K_MAX_TXD);
+	new_tx_count = ALIGN(new_tx_count, FM10K_REQ_TX_DESCRIPTOR_MULTIPLE);
+
+	new_rx_count = clamp_t(u32, ring->rx_pending,
+			       FM10K_MIN_RXD, FM10K_MAX_RXD);
+	new_rx_count = ALIGN(new_rx_count, FM10K_REQ_RX_DESCRIPTOR_MULTIPLE);
+
+	if ((new_tx_count == interface->tx_ring_count) &&
+	    (new_rx_count == interface->rx_ring_count)) {
+		/* nothing to do */
+		return 0;
+	}
+
+	while (test_and_set_bit(__FM10K_RESETTING, &interface->state))
+		usleep_range(1000, 2000);
+
+	if (!netif_running(interface->netdev)) {
+		for (i = 0; i < interface->num_tx_queues; i++)
+			interface->tx_ring[i]->count = new_tx_count;
+		for (i = 0; i < interface->num_rx_queues; i++)
+			interface->rx_ring[i]->count = new_rx_count;
+		interface->tx_ring_count = new_tx_count;
+		interface->rx_ring_count = new_rx_count;
+		goto clear_reset;
+	}
+
+	/* allocate temporary buffer to store rings in */
+	i = max_t(int, interface->num_tx_queues, interface->num_rx_queues);
+	temp_ring = vmalloc(i * sizeof(struct fm10k_ring));
+
+	if (!temp_ring) {
+		err = -ENOMEM;
+		goto clear_reset;
+	}
+
+	fm10k_down(interface);
+
+	/* Setup new Tx resources and free the old Tx resources in that order.
+	 * We can then assign the new resources to the rings via a memcpy.
+	 * The advantage to this approach is that we are guaranteed to still
+	 * have resources even in the case of an allocation failure.
+	 */
+	if (new_tx_count != interface->tx_ring_count) {
+		for (i = 0; i < interface->num_tx_queues; i++) {
+			memcpy(&temp_ring[i], interface->tx_ring[i],
+			       sizeof(struct fm10k_ring));
+
+			temp_ring[i].count = new_tx_count;
+			err = fm10k_setup_tx_resources(&temp_ring[i]);
+			if (err) {
+				while (i) {
+					i--;
+					fm10k_free_tx_resources(&temp_ring[i]);
+				}
+				goto err_setup;
+			}
+		}
+
+		for (i = 0; i < interface->num_tx_queues; i++) {
+			fm10k_free_tx_resources(interface->tx_ring[i]);
+
+			memcpy(interface->tx_ring[i], &temp_ring[i],
+			       sizeof(struct fm10k_ring));
+		}
+
+		interface->tx_ring_count = new_tx_count;
+	}
+
+	/* Repeat the process for the Rx rings if needed */
+	if (new_rx_count != interface->rx_ring_count) {
+		for (i = 0; i < interface->num_rx_queues; i++) {
+			memcpy(&temp_ring[i], interface->rx_ring[i],
+			       sizeof(struct fm10k_ring));
+
+			temp_ring[i].count = new_rx_count;
+			err = fm10k_setup_rx_resources(&temp_ring[i]);
+			if (err) {
+				while (i) {
+					i--;
+					fm10k_free_rx_resources(&temp_ring[i]);
+				}
+				goto err_setup;
+			}
+		}
+
+		for (i = 0; i < interface->num_rx_queues; i++) {
+			fm10k_free_rx_resources(interface->rx_ring[i]);
+
+			memcpy(interface->rx_ring[i], &temp_ring[i],
+			       sizeof(struct fm10k_ring));
+		}
+
+		interface->rx_ring_count = new_rx_count;
+	}
+
+err_setup:
+	fm10k_up(interface);
+	vfree(temp_ring);
+clear_reset:
+	clear_bit(__FM10K_RESETTING, &interface->state);
+	return err;
+}
+
+static int fm10k_get_coalesce(struct net_device *dev,
+			      struct ethtool_coalesce *ec)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+
+	ec->use_adaptive_tx_coalesce =
+		!!(interface->tx_itr & FM10K_ITR_ADAPTIVE);
+	ec->tx_coalesce_usecs = interface->tx_itr & ~FM10K_ITR_ADAPTIVE;
+
+	ec->use_adaptive_rx_coalesce =
+		!!(interface->rx_itr & FM10K_ITR_ADAPTIVE);
+	ec->rx_coalesce_usecs = interface->rx_itr & ~FM10K_ITR_ADAPTIVE;
+
+	return 0;
+}
+
+static int fm10k_set_coalesce(struct net_device *dev,
+			      struct ethtool_coalesce *ec)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_q_vector *qv;
+	u16 tx_itr, rx_itr;
+	int i;
+
+	/* verify limits */
+	if ((ec->rx_coalesce_usecs > FM10K_ITR_MAX) ||
+	    (ec->tx_coalesce_usecs > FM10K_ITR_MAX))
+		return -EINVAL;
+
+	/* record settings */
+	tx_itr = ec->tx_coalesce_usecs;
+	rx_itr = ec->rx_coalesce_usecs;
+
+	/* set initial values for adaptive ITR */
+	if (ec->use_adaptive_tx_coalesce)
+		tx_itr = FM10K_ITR_ADAPTIVE | FM10K_ITR_10K;
+
+	if (ec->use_adaptive_rx_coalesce)
+		rx_itr = FM10K_ITR_ADAPTIVE | FM10K_ITR_20K;
+
+	/* update interface */
+	interface->tx_itr = tx_itr;
+	interface->rx_itr = rx_itr;
+
+	/* update q_vectors */
+	for (i = 0; i < interface->num_q_vectors; i++) {
+		qv = interface->q_vector[i];
+		qv->tx.itr = tx_itr;
+		qv->rx.itr = rx_itr;
+	}
+
+	return 0;
+}
+
+static int fm10k_get_rss_hash_opts(struct fm10k_intfc *interface,
+				   struct ethtool_rxnfc *cmd)
+{
+	cmd->data = 0;
+
+	/* Report default options for RSS on fm10k */
+	switch (cmd->flow_type) {
+	case TCP_V4_FLOW:
+	case TCP_V6_FLOW:
+		cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		/* fall through */
+	case UDP_V4_FLOW:
+		if (interface->flags & FM10K_FLAG_RSS_FIELD_IPV4_UDP)
+			cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		/* fall through */
+	case SCTP_V4_FLOW:
+	case SCTP_V6_FLOW:
+	case AH_ESP_V4_FLOW:
+	case AH_ESP_V6_FLOW:
+	case AH_V4_FLOW:
+	case AH_V6_FLOW:
+	case ESP_V4_FLOW:
+	case ESP_V6_FLOW:
+	case IPV4_FLOW:
+	case IPV6_FLOW:
+		cmd->data |= RXH_IP_SRC | RXH_IP_DST;
+		break;
+	case UDP_V6_FLOW:
+		if (interface->flags & FM10K_FLAG_RSS_FIELD_IPV6_UDP)
+			cmd->data |= RXH_L4_B_0_1 | RXH_L4_B_2_3;
+		cmd->data |= RXH_IP_SRC | RXH_IP_DST;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fm10k_get_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd,
+			   u32 *rule_locs)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	int ret = -EOPNOTSUPP;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_GRXRINGS:
+		cmd->data = interface->num_rx_queues;
+		ret = 0;
+		break;
+	case ETHTOOL_GRXFH:
+		ret = fm10k_get_rss_hash_opts(interface, cmd);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+#define UDP_RSS_FLAGS (FM10K_FLAG_RSS_FIELD_IPV4_UDP | \
+		       FM10K_FLAG_RSS_FIELD_IPV6_UDP)
+static int fm10k_set_rss_hash_opt(struct fm10k_intfc *interface,
+				  struct ethtool_rxnfc *nfc)
+{
+	u32 flags = interface->flags;
+
+	/* RSS does not support anything other than hashing
+	 * to queues on src and dst IPs and ports
+	 */
+	if (nfc->data & ~(RXH_IP_SRC | RXH_IP_DST |
+			  RXH_L4_B_0_1 | RXH_L4_B_2_3))
+		return -EINVAL;
+
+	switch (nfc->flow_type) {
+	case TCP_V4_FLOW:
+	case TCP_V6_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST) ||
+		    !(nfc->data & RXH_L4_B_0_1) ||
+		    !(nfc->data & RXH_L4_B_2_3))
+			return -EINVAL;
+		break;
+	case UDP_V4_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST))
+			return -EINVAL;
+		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+		case 0:
+			flags &= ~FM10K_FLAG_RSS_FIELD_IPV4_UDP;
+			break;
+		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+			flags |= FM10K_FLAG_RSS_FIELD_IPV4_UDP;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case UDP_V6_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST))
+			return -EINVAL;
+		switch (nfc->data & (RXH_L4_B_0_1 | RXH_L4_B_2_3)) {
+		case 0:
+			flags &= ~FM10K_FLAG_RSS_FIELD_IPV6_UDP;
+			break;
+		case (RXH_L4_B_0_1 | RXH_L4_B_2_3):
+			flags |= FM10K_FLAG_RSS_FIELD_IPV6_UDP;
+			break;
+		default:
+			return -EINVAL;
+		}
+		break;
+	case AH_ESP_V4_FLOW:
+	case AH_V4_FLOW:
+	case ESP_V4_FLOW:
+	case SCTP_V4_FLOW:
+	case AH_ESP_V6_FLOW:
+	case AH_V6_FLOW:
+	case ESP_V6_FLOW:
+	case SCTP_V6_FLOW:
+		if (!(nfc->data & RXH_IP_SRC) ||
+		    !(nfc->data & RXH_IP_DST) ||
+		    (nfc->data & RXH_L4_B_0_1) ||
+		    (nfc->data & RXH_L4_B_2_3))
+			return -EINVAL;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* if we changed something we need to update flags */
+	if (flags != interface->flags) {
+		struct fm10k_hw *hw = &interface->hw;
+		u32 mrqc;
+
+		if ((flags & UDP_RSS_FLAGS) &&
+		    !(interface->flags & UDP_RSS_FLAGS))
+			netif_warn(interface, drv, interface->netdev,
+				   "enabling UDP RSS: fragmented packets may arrive out of order to the stack above\n");
+
+		interface->flags = flags;
+
+		/* Perform hash on these packet types */
+		mrqc = FM10K_MRQC_IPV4 |
+		       FM10K_MRQC_TCP_IPV4 |
+		       FM10K_MRQC_IPV6 |
+		       FM10K_MRQC_TCP_IPV6;
+
+		if (flags & FM10K_FLAG_RSS_FIELD_IPV4_UDP)
+			mrqc |= FM10K_MRQC_UDP_IPV4;
+		if (flags & FM10K_FLAG_RSS_FIELD_IPV6_UDP)
+			mrqc |= FM10K_MRQC_UDP_IPV6;
+
+		fm10k_write_reg(hw, FM10K_MRQC(0), mrqc);
+	}
+
+	return 0;
+}
+
+static int fm10k_set_rxnfc(struct net_device *dev, struct ethtool_rxnfc *cmd)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	int ret = -EOPNOTSUPP;
+
+	switch (cmd->cmd) {
+	case ETHTOOL_SRXFH:
+		ret = fm10k_set_rss_hash_opt(interface, cmd);
+		break;
+	default:
+		break;
+	}
+
+	return ret;
+}
+
+static int fm10k_mbx_test(struct fm10k_intfc *interface, u64 *data)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 attr_flag, test_msg[6];
+	unsigned long timeout;
+	int err;
+
+	/* For now this is a VF only feature */
+	if (hw->mac.type != fm10k_mac_vf)
+		return 0;
+
+	/* loop through both nested and unnested attribute types */
+	for (attr_flag = (1 << FM10K_TEST_MSG_UNSET);
+	     attr_flag < (1 << (2 * FM10K_TEST_MSG_NESTED));
+	     attr_flag += attr_flag) {
+		/* generate message to be tested */
+		fm10k_tlv_msg_test_create(test_msg, attr_flag);
+
+		fm10k_mbx_lock(interface);
+		mbx->test_result = FM10K_NOT_IMPLEMENTED;
+		err = mbx->ops.enqueue_tx(hw, mbx, test_msg);
+		fm10k_mbx_unlock(interface);
+
+		/* wait up to 1 second for response */
+		timeout = jiffies + HZ;
+		do {
+			if (err < 0)
+				goto err_out;
+
+			usleep_range(500, 1000);
+
+			fm10k_mbx_lock(interface);
+			mbx->ops.process(hw, mbx);
+			fm10k_mbx_unlock(interface);
+
+			err = mbx->test_result;
+			if (!err)
+				break;
+		} while (time_is_after_jiffies(timeout));
+
+		/* reporting errors */
+		if (err)
+			goto err_out;
+	}
+
+err_out:
+	*data = err < 0 ? (attr_flag) : (err > 0);
+	return err;
+}
+
+static void fm10k_self_test(struct net_device *dev,
+			    struct ethtool_test *eth_test, u64 *data)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+
+	memset(data, 0, sizeof(*data) * FM10K_TEST_LEN);
+
+	if (FM10K_REMOVED(hw)) {
+		netif_err(interface, drv, dev,
+			  "Interface removed - test blocked\n");
+		eth_test->flags |= ETH_TEST_FL_FAILED;
+		return;
+	}
+
+	if (fm10k_mbx_test(interface, &data[FM10K_TEST_MBX]))
+		eth_test->flags |= ETH_TEST_FL_FAILED;
+}
+
+static u32 fm10k_get_reta_size(struct net_device *netdev)
+{
+	return FM10K_RETA_SIZE * FM10K_RETA_ENTRIES_PER_REG;
+}
+
+static int fm10k_get_reta(struct net_device *netdev, u32 *indir)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	int i;
+
+	if (!indir)
+		return 0;
+
+	for (i = 0; i < FM10K_RETA_SIZE; i++, indir += 4) {
+		u32 reta = interface->reta[i];
+
+		indir[0] = (reta << 24) >> 24;
+		indir[1] = (reta << 16) >> 24;
+		indir[2] = (reta <<  8) >> 24;
+		indir[3] = (reta) >> 24;
+	}
+
+	return 0;
+}
+
+static int fm10k_set_reta(struct net_device *netdev, const u32 *indir)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_hw *hw = &interface->hw;
+	int i;
+	u16 rss_i;
+
+	if (!indir)
+		return 0;
+
+	/* Verify user input. */
+	rss_i = interface->ring_feature[RING_F_RSS].indices;
+	for (i = fm10k_get_reta_size(netdev); i--;) {
+		if (indir[i] < rss_i)
+			continue;
+		return -EINVAL;
+	}
+
+	/* record entries to reta table */
+	for (i = 0; i < FM10K_RETA_SIZE; i++, indir += 4) {
+		u32 reta = indir[0] |
+			   (indir[1] << 8) |
+			   (indir[2] << 16) |
+			   (indir[3] << 24);
+
+		if (interface->reta[i] == reta)
+			continue;
+
+		interface->reta[i] = reta;
+		fm10k_write_reg(hw, FM10K_RETA(0, i), reta);
+	}
+
+	return 0;
+}
+
+static u32 fm10k_get_rssrk_size(struct net_device *netdev)
+{
+	return FM10K_RSSRK_SIZE * FM10K_RSSRK_ENTRIES_PER_REG;
+}
+
+static int fm10k_get_rssh(struct net_device *netdev, u32 *indir, u8 *key)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	int i, err;
+
+	err = fm10k_get_reta(netdev, indir);
+	if (err || !key)
+		return err;
+
+	for (i = 0; i < FM10K_RSSRK_SIZE; i++, key += 4)
+		*(__le32 *)key = cpu_to_le32(interface->rssrk[i]);
+
+	return 0;
+}
+
+static int fm10k_set_rssh(struct net_device *netdev, const u32 *indir,
+			  const u8 *key)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_hw *hw = &interface->hw;
+	int i, err;
+
+	err = fm10k_set_reta(netdev, indir);
+	if (err || !key)
+		return err;
+
+	for (i = 0; i < FM10K_RSSRK_SIZE; i++, key += 4) {
+		u32 rssrk = le32_to_cpu(*(__le32 *)key);
+
+		if (interface->rssrk[i] == rssrk)
+			continue;
+
+		interface->rssrk[i] = rssrk;
+		fm10k_write_reg(hw, FM10K_RSSRK(0, i), rssrk);
+	}
+
+	return 0;
+}
+
+static unsigned int fm10k_max_channels(struct net_device *dev)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	unsigned int max_combined = interface->hw.mac.max_queues;
+	u8 tcs = netdev_get_num_tc(dev);
+
+	/* For QoS report channels per traffic class */
+	if (tcs > 1)
+		max_combined = 1 << (fls(max_combined / tcs) - 1);
+
+	return max_combined;
+}
+
+static void fm10k_get_channels(struct net_device *dev,
+			       struct ethtool_channels *ch)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+
+	/* report maximum channels */
+	ch->max_combined = fm10k_max_channels(dev);
+
+	/* report info for other vector */
+	ch->max_other = NON_Q_VECTORS(hw);
+	ch->other_count = ch->max_other;
+
+	/* record RSS queues */
+	ch->combined_count = interface->ring_feature[RING_F_RSS].indices;
+}
+
+static int fm10k_set_channels(struct net_device *dev,
+			      struct ethtool_channels *ch)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	unsigned int count = ch->combined_count;
+	struct fm10k_hw *hw = &interface->hw;
+
+	/* verify they are not requesting separate vectors */
+	if (!count || ch->rx_count || ch->tx_count)
+		return -EINVAL;
+
+	/* verify other_count has not changed */
+	if (ch->other_count != NON_Q_VECTORS(hw))
+		return -EINVAL;
+
+	/* verify the number of channels does not exceed hardware limits */
+	if (count > fm10k_max_channels(dev))
+		return -EINVAL;
+
+	interface->ring_feature[RING_F_RSS].limit = count;
+
+	/* use setup TC to update any traffic class queue mapping */
+	return fm10k_setup_tc(dev, netdev_get_num_tc(dev));
+}
+
+static int fm10k_get_ts_info(struct net_device *dev,
+			   struct ethtool_ts_info *info)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+
+	info->so_timestamping =
+		SOF_TIMESTAMPING_TX_SOFTWARE |
+		SOF_TIMESTAMPING_RX_SOFTWARE |
+		SOF_TIMESTAMPING_SOFTWARE |
+		SOF_TIMESTAMPING_TX_HARDWARE |
+		SOF_TIMESTAMPING_RX_HARDWARE |
+		SOF_TIMESTAMPING_RAW_HARDWARE;
+
+	if (interface->ptp_clock)
+		info->phc_index = ptp_clock_index(interface->ptp_clock);
+	else
+		info->phc_index = -1;
+
+	info->tx_types = (1 << HWTSTAMP_TX_OFF) |
+			 (1 << HWTSTAMP_TX_ON);
+
+	info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
+			   (1 << HWTSTAMP_FILTER_ALL);
+
+	return 0;
+}
+
+static const struct ethtool_ops fm10k_ethtool_ops = {
+	.get_strings		= fm10k_get_strings,
+	.get_sset_count		= fm10k_get_sset_count,
+	.get_ethtool_stats      = fm10k_get_ethtool_stats,
+	.get_drvinfo		= fm10k_get_drvinfo,
+	.get_link		= ethtool_op_get_link,
+	.get_pauseparam		= fm10k_get_pauseparam,
+	.set_pauseparam		= fm10k_set_pauseparam,
+	.get_msglevel		= fm10k_get_msglevel,
+	.set_msglevel		= fm10k_set_msglevel,
+	.get_ringparam		= fm10k_get_ringparam,
+	.set_ringparam		= fm10k_set_ringparam,
+	.get_coalesce		= fm10k_get_coalesce,
+	.set_coalesce		= fm10k_set_coalesce,
+	.get_rxnfc		= fm10k_get_rxnfc,
+	.set_rxnfc		= fm10k_set_rxnfc,
+	.get_regs               = fm10k_get_regs,
+	.get_regs_len           = fm10k_get_regs_len,
+	.self_test		= fm10k_self_test,
+	.get_rxfh_indir_size	= fm10k_get_reta_size,
+	.get_rxfh_key_size	= fm10k_get_rssrk_size,
+	.get_rxfh		= fm10k_get_rssh,
+	.set_rxfh		= fm10k_set_rssh,
+	.get_channels		= fm10k_get_channels,
+	.set_channels		= fm10k_set_channels,
+	.get_ts_info            = fm10k_get_ts_info,
+};
+
+void fm10k_set_ethtool_ops(struct net_device *dev)
+{
+	dev->ethtool_ops = &fm10k_ethtool_ops;
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_iov.c b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
new file mode 100644
index 000000000000..060190864238
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_iov.c
@@ -0,0 +1,536 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k.h"
+#include "fm10k_vf.h"
+#include "fm10k_pf.h"
+
+static s32 fm10k_iov_msg_error(struct fm10k_hw *hw, u32 **results,
+			       struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_vf_info *vf_info = (struct fm10k_vf_info *)mbx;
+	struct fm10k_intfc *interface = hw->back;
+	struct pci_dev *pdev = interface->pdev;
+
+	dev_err(&pdev->dev, "Unknown message ID %u on VF %d\n",
+		**results & FM10K_TLV_ID_MASK, vf_info->vf_idx);
+
+	return fm10k_tlv_msg_error(hw, results, mbx);
+}
+
+static const struct fm10k_msg_data iov_mbx_data[] = {
+	FM10K_TLV_MSG_TEST_HANDLER(fm10k_tlv_msg_test),
+	FM10K_VF_MSG_MSIX_HANDLER(fm10k_iov_msg_msix_pf),
+	FM10K_VF_MSG_MAC_VLAN_HANDLER(fm10k_iov_msg_mac_vlan_pf),
+	FM10K_VF_MSG_LPORT_STATE_HANDLER(fm10k_iov_msg_lport_state_pf),
+	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_iov_msg_error),
+};
+
+s32 fm10k_iov_event(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_iov_data *iov_data;
+	s64 mbicr, vflre;
+	int i;
+
+	/* if there is no iov_data then there is no mailboxes to process */
+	if (!ACCESS_ONCE(interface->iov_data))
+		return 0;
+
+	rcu_read_lock();
+
+	iov_data = interface->iov_data;
+
+	/* check again now that we are in the RCU block */
+	if (!iov_data)
+		goto read_unlock;
+
+	if (!(fm10k_read_reg(hw, FM10K_EICR) & FM10K_EICR_VFLR))
+		goto process_mbx;
+
+	/* read VFLRE to determine if any VFs have been reset */
+	do {
+		vflre = fm10k_read_reg(hw, FM10K_PFVFLRE(0));
+		vflre <<= 32;
+		vflre |= fm10k_read_reg(hw, FM10K_PFVFLRE(1));
+		vflre = (vflre << 32) | (vflre >> 32);
+		vflre |= fm10k_read_reg(hw, FM10K_PFVFLRE(0));
+
+		i = iov_data->num_vfs;
+
+		for (vflre <<= 64 - i; vflre && i--; vflre += vflre) {
+			struct fm10k_vf_info *vf_info = &iov_data->vf_info[i];
+
+			if (vflre >= 0)
+				continue;
+
+			hw->iov.ops.reset_resources(hw, vf_info);
+			vf_info->mbx.ops.connect(hw, &vf_info->mbx);
+		}
+	} while (i != iov_data->num_vfs);
+
+process_mbx:
+	/* read MBICR to determine which VFs require attention */
+	mbicr = fm10k_read_reg(hw, FM10K_MBICR(1));
+	mbicr <<= 32;
+	mbicr |= fm10k_read_reg(hw, FM10K_MBICR(0));
+
+	i = iov_data->next_vf_mbx ? : iov_data->num_vfs;
+
+	for (mbicr <<= 64 - i; i--; mbicr += mbicr) {
+		struct fm10k_mbx_info *mbx = &iov_data->vf_info[i].mbx;
+
+		if (mbicr >= 0)
+			continue;
+
+		if (!hw->mbx.ops.tx_ready(&hw->mbx, FM10K_VFMBX_MSG_MTU))
+			break;
+
+		mbx->ops.process(hw, mbx);
+	}
+
+	if (i >= 0) {
+		iov_data->next_vf_mbx = i + 1;
+	} else if (iov_data->next_vf_mbx) {
+		iov_data->next_vf_mbx = 0;
+		goto process_mbx;
+	}
+read_unlock:
+	rcu_read_unlock();
+
+	return 0;
+}
+
+s32 fm10k_iov_mbx(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_iov_data *iov_data;
+	int i;
+
+	/* if there is no iov_data then there is no mailboxes to process */
+	if (!ACCESS_ONCE(interface->iov_data))
+		return 0;
+
+	rcu_read_lock();
+
+	iov_data = interface->iov_data;
+
+	/* check again now that we are in the RCU block */
+	if (!iov_data)
+		goto read_unlock;
+
+	/* lock the mailbox for transmit and receive */
+	fm10k_mbx_lock(interface);
+
+process_mbx:
+	for (i = iov_data->next_vf_mbx ? : iov_data->num_vfs; i--;) {
+		struct fm10k_vf_info *vf_info = &iov_data->vf_info[i];
+		struct fm10k_mbx_info *mbx = &vf_info->mbx;
+		u16 glort = vf_info->glort;
+
+		/* verify port mapping is valid, if not reset port */
+		if (vf_info->vf_flags && !fm10k_glort_valid_pf(hw, glort))
+			hw->iov.ops.reset_lport(hw, vf_info);
+
+		/* reset VFs that have mailbox timed out */
+		if (!mbx->timeout) {
+			hw->iov.ops.reset_resources(hw, vf_info);
+			mbx->ops.connect(hw, mbx);
+		}
+
+		/* no work pending, then just continue */
+		if (mbx->ops.tx_complete(mbx) && !mbx->ops.rx_ready(mbx))
+			continue;
+
+		/* guarantee we have free space in the SM mailbox */
+		if (!hw->mbx.ops.tx_ready(&hw->mbx, FM10K_VFMBX_MSG_MTU))
+			break;
+
+		/* cleanup mailbox and process received messages */
+		mbx->ops.process(hw, mbx);
+	}
+
+	if (i >= 0) {
+		iov_data->next_vf_mbx = i + 1;
+	} else if (iov_data->next_vf_mbx) {
+		iov_data->next_vf_mbx = 0;
+		goto process_mbx;
+	}
+
+	/* free the lock */
+	fm10k_mbx_unlock(interface);
+
+read_unlock:
+	rcu_read_unlock();
+
+	return 0;
+}
+
+void fm10k_iov_suspend(struct pci_dev *pdev)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_hw *hw = &interface->hw;
+	int num_vfs, i;
+
+	/* pull out num_vfs from iov_data */
+	num_vfs = iov_data ? iov_data->num_vfs : 0;
+
+	/* shut down queue mapping for VFs */
+	fm10k_write_reg(hw, FM10K_DGLORTMAP(fm10k_dglort_vf_rss),
+			FM10K_DGLORTMAP_NONE);
+
+	/* Stop any active VFs and reset their resources */
+	for (i = 0; i < num_vfs; i++) {
+		struct fm10k_vf_info *vf_info = &iov_data->vf_info[i];
+
+		hw->iov.ops.reset_resources(hw, vf_info);
+		hw->iov.ops.reset_lport(hw, vf_info);
+	}
+}
+
+int fm10k_iov_resume(struct pci_dev *pdev)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_dglort_cfg dglort = { 0 };
+	struct fm10k_hw *hw = &interface->hw;
+	int num_vfs, i;
+
+	/* pull out num_vfs from iov_data */
+	num_vfs = iov_data ? iov_data->num_vfs : 0;
+
+	/* return error if iov_data is not already populated */
+	if (!iov_data)
+		return -ENOMEM;
+
+	/* allocate hardware resources for the VFs */
+	hw->iov.ops.assign_resources(hw, num_vfs, num_vfs);
+
+	/* configure DGLORT mapping for RSS */
+	dglort.glort = hw->mac.dglort_map & FM10K_DGLORTMAP_NONE;
+	dglort.idx = fm10k_dglort_vf_rss;
+	dglort.inner_rss = 1;
+	dglort.rss_l = fls(fm10k_queues_per_pool(hw) - 1);
+	dglort.queue_b = fm10k_vf_queue_index(hw, 0);
+	dglort.vsi_l = fls(hw->iov.total_vfs - 1);
+	dglort.vsi_b = 1;
+
+	hw->mac.ops.configure_dglort_map(hw, &dglort);
+
+	/* assign resources to the device */
+	for (i = 0; i < num_vfs; i++) {
+		struct fm10k_vf_info *vf_info = &iov_data->vf_info[i];
+
+		/* allocate all but the last GLORT to the VFs */
+		if (i == ((~hw->mac.dglort_map) >> FM10K_DGLORTMAP_MASK_SHIFT))
+			break;
+
+		/* assign GLORT to VF, and restrict it to multicast */
+		hw->iov.ops.set_lport(hw, vf_info, i,
+				      FM10K_VF_FLAG_MULTI_CAPABLE);
+
+		/* assign our default vid to the VF following reset */
+		vf_info->sw_vid = hw->mac.default_vid;
+
+		/* mailbox is disconnected so we don't send a message */
+		hw->iov.ops.assign_default_mac_vlan(hw, vf_info);
+
+		/* now we are ready so we can connect */
+		vf_info->mbx.ops.connect(hw, &vf_info->mbx);
+	}
+
+	return 0;
+}
+
+s32 fm10k_iov_update_pvid(struct fm10k_intfc *interface, u16 glort, u16 pvid)
+{
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_vf_info *vf_info;
+	u16 vf_idx = (glort - hw->mac.dglort_map) & FM10K_DGLORTMAP_NONE;
+
+	/* no IOV support, not our message to process */
+	if (!iov_data)
+		return FM10K_ERR_PARAM;
+
+	/* glort outside our range, not our message to process */
+	if (vf_idx >= iov_data->num_vfs)
+		return FM10K_ERR_PARAM;
+
+	/* determine if an update has occured and if so notify the VF */
+	vf_info = &iov_data->vf_info[vf_idx];
+	if (vf_info->sw_vid != pvid) {
+		vf_info->sw_vid = pvid;
+		hw->iov.ops.assign_default_mac_vlan(hw, vf_info);
+	}
+
+	return 0;
+}
+
+static void fm10k_iov_free_data(struct pci_dev *pdev)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+
+	if (!interface->iov_data)
+		return;
+
+	/* reclaim hardware resources */
+	fm10k_iov_suspend(pdev);
+
+	/* drop iov_data from interface */
+	kfree_rcu(interface->iov_data, rcu);
+	interface->iov_data = NULL;
+}
+
+static s32 fm10k_iov_alloc_data(struct pci_dev *pdev, int num_vfs)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_hw *hw = &interface->hw;
+	size_t size;
+	int i, err;
+
+	/* return error if iov_data is already populated */
+	if (iov_data)
+		return -EBUSY;
+
+	/* The PF should always be able to assign resources */
+	if (!hw->iov.ops.assign_resources)
+		return -ENODEV;
+
+	/* nothing to do if no VFs are requested */
+	if (!num_vfs)
+		return 0;
+
+	/* allocate memory for VF storage */
+	size = offsetof(struct fm10k_iov_data, vf_info[num_vfs]);
+	iov_data = kzalloc(size, GFP_KERNEL);
+	if (!iov_data)
+		return -ENOMEM;
+
+	/* record number of VFs */
+	iov_data->num_vfs = num_vfs;
+
+	/* loop through vf_info structures initializing each entry */
+	for (i = 0; i < num_vfs; i++) {
+		struct fm10k_vf_info *vf_info = &iov_data->vf_info[i];
+
+		/* Record VF VSI value */
+		vf_info->vsi = i + 1;
+		vf_info->vf_idx = i;
+
+		/* initialize mailbox memory */
+		err = fm10k_pfvf_mbx_init(hw, &vf_info->mbx, iov_mbx_data, i);
+		if (err) {
+			dev_err(&pdev->dev,
+				"Unable to initialize SR-IOV mailbox\n");
+			kfree(iov_data);
+			return err;
+		}
+	}
+
+	/* assign iov_data to interface */
+	interface->iov_data = iov_data;
+
+	/* allocate hardware resources for the VFs */
+	fm10k_iov_resume(pdev);
+
+	return 0;
+}
+
+void fm10k_iov_disable(struct pci_dev *pdev)
+{
+	if (pci_num_vf(pdev) && pci_vfs_assigned(pdev))
+		dev_err(&pdev->dev,
+			"Cannot disable SR-IOV while VFs are assigned\n");
+	else
+		pci_disable_sriov(pdev);
+
+	fm10k_iov_free_data(pdev);
+}
+
+static void fm10k_disable_aer_comp_abort(struct pci_dev *pdev)
+{
+	u32 err_sev;
+	int pos;
+
+	pos = pci_find_ext_capability(pdev, PCI_EXT_CAP_ID_ERR);
+	if (!pos)
+		return;
+
+	pci_read_config_dword(pdev, pos + PCI_ERR_UNCOR_SEVER, &err_sev);
+	err_sev &= ~PCI_ERR_UNC_COMP_ABORT;
+	pci_write_config_dword(pdev, pos + PCI_ERR_UNCOR_SEVER, err_sev);
+}
+
+int fm10k_iov_configure(struct pci_dev *pdev, int num_vfs)
+{
+	int current_vfs = pci_num_vf(pdev);
+	int err = 0;
+
+	if (current_vfs && pci_vfs_assigned(pdev)) {
+		dev_err(&pdev->dev,
+			"Cannot modify SR-IOV while VFs are assigned\n");
+		num_vfs = current_vfs;
+	} else {
+		pci_disable_sriov(pdev);
+		fm10k_iov_free_data(pdev);
+	}
+
+	/* allocate resources for the VFs */
+	err = fm10k_iov_alloc_data(pdev, num_vfs);
+	if (err)
+		return err;
+
+	/* allocate VFs if not already allocated */
+	if (num_vfs && (num_vfs != current_vfs)) {
+		/* Disable completer abort error reporting as
+		 * the VFs can trigger this any time they read a queue
+		 * that they don't own.
+		 */
+		fm10k_disable_aer_comp_abort(pdev);
+
+		err = pci_enable_sriov(pdev, num_vfs);
+		if (err) {
+			dev_err(&pdev->dev,
+				"Enable PCI SR-IOV failed: %d\n", err);
+			return err;
+		}
+	}
+
+	return num_vfs;
+}
+
+int fm10k_ndo_set_vf_mac(struct net_device *netdev, int vf_idx, u8 *mac)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_vf_info *vf_info;
+
+	/* verify SR-IOV is active and that vf idx is valid */
+	if (!iov_data || vf_idx >= iov_data->num_vfs)
+		return -EINVAL;
+
+	/* verify MAC addr is valid */
+	if (!is_zero_ether_addr(mac) && !is_valid_ether_addr(mac))
+		return -EINVAL;
+
+	/* record new MAC address */
+	vf_info = &iov_data->vf_info[vf_idx];
+	ether_addr_copy(vf_info->mac, mac);
+
+	/* assigning the MAC will send a mailbox message so lock is needed */
+	fm10k_mbx_lock(interface);
+
+	/* assign MAC address to VF */
+	hw->iov.ops.assign_default_mac_vlan(hw, vf_info);
+
+	fm10k_mbx_unlock(interface);
+
+	return 0;
+}
+
+int fm10k_ndo_set_vf_vlan(struct net_device *netdev, int vf_idx, u16 vid,
+			  u8 qos)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_vf_info *vf_info;
+
+	/* verify SR-IOV is active and that vf idx is valid */
+	if (!iov_data || vf_idx >= iov_data->num_vfs)
+		return -EINVAL;
+
+	/* QOS is unsupported and VLAN IDs accepted range 0-4094 */
+	if (qos || (vid > (VLAN_VID_MASK - 1)))
+		return -EINVAL;
+
+	vf_info = &iov_data->vf_info[vf_idx];
+
+	/* exit if there is nothing to do */
+	if (vf_info->pf_vid == vid)
+		return 0;
+
+	/* record default VLAN ID for VF */
+	vf_info->pf_vid = vid;
+
+	/* assigning the VLAN will send a mailbox message so lock is needed */
+	fm10k_mbx_lock(interface);
+
+	/* Clear the VLAN table for the VF */
+	hw->mac.ops.update_vlan(hw, FM10K_VLAN_ALL, vf_info->vsi, false);
+
+	/* Update VF assignment and trigger reset */
+	hw->iov.ops.assign_default_mac_vlan(hw, vf_info);
+
+	fm10k_mbx_unlock(interface);
+
+	return 0;
+}
+
+int fm10k_ndo_set_vf_bw(struct net_device *netdev, int vf_idx, int unused,
+			int rate)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_hw *hw = &interface->hw;
+
+	/* verify SR-IOV is active and that vf idx is valid */
+	if (!iov_data || vf_idx >= iov_data->num_vfs)
+		return -EINVAL;
+
+	/* rate limit cannot be less than 10Mbs or greater than link speed */
+	if (rate && ((rate < FM10K_VF_TC_MIN) || rate > FM10K_VF_TC_MAX))
+		return -EINVAL;
+
+	/* store values */
+	iov_data->vf_info[vf_idx].rate = rate;
+
+	/* update hardware configuration */
+	hw->iov.ops.configure_tc(hw, vf_idx, rate);
+
+	return 0;
+}
+
+int fm10k_ndo_get_vf_config(struct net_device *netdev,
+			    int vf_idx, struct ifla_vf_info *ivi)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_iov_data *iov_data = interface->iov_data;
+	struct fm10k_vf_info *vf_info;
+
+	/* verify SR-IOV is active and that vf idx is valid */
+	if (!iov_data || vf_idx >= iov_data->num_vfs)
+		return -EINVAL;
+
+	vf_info = &iov_data->vf_info[vf_idx];
+
+	ivi->vf = vf_idx;
+	ivi->max_tx_rate = vf_info->rate;
+	ivi->min_tx_rate = 0;
+	ether_addr_copy(ivi->mac, vf_info->mac);
+	ivi->vlan = vf_info->pf_vid;
+	ivi->qos = 0;
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_main.c b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
new file mode 100644
index 000000000000..e645af412e76
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_main.c
@@ -0,0 +1,1981 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <net/ipv6.h>
+#include <net/ip.h>
+#include <net/tcp.h>
+#include <linux/if_macvlan.h>
+#include <linux/prefetch.h>
+
+#include "fm10k.h"
+
+#define DRV_VERSION	"0.12.2-k"
+const char fm10k_driver_version[] = DRV_VERSION;
+char fm10k_driver_name[] = "fm10k";
+static const char fm10k_driver_string[] =
+	"Intel(R) Ethernet Switch Host Interface Driver";
+static const char fm10k_copyright[] =
+	"Copyright (c) 2013 Intel Corporation.";
+
+MODULE_AUTHOR("Intel Corporation, <linux.nics@intel.com>");
+MODULE_DESCRIPTION("Intel(R) Ethernet Switch Host Interface Driver");
+MODULE_LICENSE("GPL");
+MODULE_VERSION(DRV_VERSION);
+
+/**
+ * fm10k_init_module - Driver Registration Routine
+ *
+ * fm10k_init_module is the first routine called when the driver is
+ * loaded.  All it does is register with the PCI subsystem.
+ **/
+static int __init fm10k_init_module(void)
+{
+	pr_info("%s - version %s\n", fm10k_driver_string, fm10k_driver_version);
+	pr_info("%s\n", fm10k_copyright);
+
+	fm10k_dbg_init();
+
+	return fm10k_register_pci_driver();
+}
+module_init(fm10k_init_module);
+
+/**
+ * fm10k_exit_module - Driver Exit Cleanup Routine
+ *
+ * fm10k_exit_module is called just before the driver is removed
+ * from memory.
+ **/
+static void __exit fm10k_exit_module(void)
+{
+	fm10k_unregister_pci_driver();
+
+	fm10k_dbg_exit();
+}
+module_exit(fm10k_exit_module);
+
+static bool fm10k_alloc_mapped_page(struct fm10k_ring *rx_ring,
+				    struct fm10k_rx_buffer *bi)
+{
+	struct page *page = bi->page;
+	dma_addr_t dma;
+
+	/* Only page will be NULL if buffer was consumed */
+	if (likely(page))
+		return true;
+
+	/* alloc new page for storage */
+	page = alloc_page(GFP_ATOMIC | __GFP_COLD);
+	if (unlikely(!page)) {
+		rx_ring->rx_stats.alloc_failed++;
+		return false;
+	}
+
+	/* map page for use */
+	dma = dma_map_page(rx_ring->dev, page, 0, PAGE_SIZE, DMA_FROM_DEVICE);
+
+	/* if mapping failed free memory back to system since
+	 * there isn't much point in holding memory we can't use
+	 */
+	if (dma_mapping_error(rx_ring->dev, dma)) {
+		__free_page(page);
+		bi->page = NULL;
+
+		rx_ring->rx_stats.alloc_failed++;
+		return false;
+	}
+
+	bi->dma = dma;
+	bi->page = page;
+	bi->page_offset = 0;
+
+	return true;
+}
+
+/**
+ * fm10k_alloc_rx_buffers - Replace used receive buffers
+ * @rx_ring: ring to place buffers on
+ * @cleaned_count: number of buffers to replace
+ **/
+void fm10k_alloc_rx_buffers(struct fm10k_ring *rx_ring, u16 cleaned_count)
+{
+	union fm10k_rx_desc *rx_desc;
+	struct fm10k_rx_buffer *bi;
+	u16 i = rx_ring->next_to_use;
+
+	/* nothing to do */
+	if (!cleaned_count)
+		return;
+
+	rx_desc = FM10K_RX_DESC(rx_ring, i);
+	bi = &rx_ring->rx_buffer[i];
+	i -= rx_ring->count;
+
+	do {
+		if (!fm10k_alloc_mapped_page(rx_ring, bi))
+			break;
+
+		/* Refresh the desc even if buffer_addrs didn't change
+		 * because each write-back erases this info.
+		 */
+		rx_desc->q.pkt_addr = cpu_to_le64(bi->dma + bi->page_offset);
+
+		rx_desc++;
+		bi++;
+		i++;
+		if (unlikely(!i)) {
+			rx_desc = FM10K_RX_DESC(rx_ring, 0);
+			bi = rx_ring->rx_buffer;
+			i -= rx_ring->count;
+		}
+
+		/* clear the hdr_addr for the next_to_use descriptor */
+		rx_desc->q.hdr_addr = 0;
+
+		cleaned_count--;
+	} while (cleaned_count);
+
+	i += rx_ring->count;
+
+	if (rx_ring->next_to_use != i) {
+		/* record the next descriptor to use */
+		rx_ring->next_to_use = i;
+
+		/* update next to alloc since we have filled the ring */
+		rx_ring->next_to_alloc = i;
+
+		/* Force memory writes to complete before letting h/w
+		 * know there are new descriptors to fetch.  (Only
+		 * applicable for weak-ordered memory model archs,
+		 * such as IA-64).
+		 */
+		wmb();
+
+		/* notify hardware of new descriptors */
+		writel(i, rx_ring->tail);
+	}
+}
+
+/**
+ * fm10k_reuse_rx_page - page flip buffer and store it back on the ring
+ * @rx_ring: rx descriptor ring to store buffers on
+ * @old_buff: donor buffer to have page reused
+ *
+ * Synchronizes page for reuse by the interface
+ **/
+static void fm10k_reuse_rx_page(struct fm10k_ring *rx_ring,
+				struct fm10k_rx_buffer *old_buff)
+{
+	struct fm10k_rx_buffer *new_buff;
+	u16 nta = rx_ring->next_to_alloc;
+
+	new_buff = &rx_ring->rx_buffer[nta];
+
+	/* update, and store next to alloc */
+	nta++;
+	rx_ring->next_to_alloc = (nta < rx_ring->count) ? nta : 0;
+
+	/* transfer page from old buffer to new buffer */
+	memcpy(new_buff, old_buff, sizeof(struct fm10k_rx_buffer));
+
+	/* sync the buffer for use by the device */
+	dma_sync_single_range_for_device(rx_ring->dev, old_buff->dma,
+					 old_buff->page_offset,
+					 FM10K_RX_BUFSZ,
+					 DMA_FROM_DEVICE);
+}
+
+static bool fm10k_can_reuse_rx_page(struct fm10k_rx_buffer *rx_buffer,
+				    struct page *page,
+				    unsigned int truesize)
+{
+	/* avoid re-using remote pages */
+	if (unlikely(page_to_nid(page) != numa_mem_id()))
+		return false;
+
+#if (PAGE_SIZE < 8192)
+	/* if we are only owner of page we can reuse it */
+	if (unlikely(page_count(page) != 1))
+		return false;
+
+	/* flip page offset to other buffer */
+	rx_buffer->page_offset ^= FM10K_RX_BUFSZ;
+
+	/* Even if we own the page, we are not allowed to use atomic_set()
+	 * This would break get_page_unless_zero() users.
+	 */
+	atomic_inc(&page->_count);
+#else
+	/* move offset up to the next cache line */
+	rx_buffer->page_offset += truesize;
+
+	if (rx_buffer->page_offset > (PAGE_SIZE - FM10K_RX_BUFSZ))
+		return false;
+
+	/* bump ref count on page before it is given to the stack */
+	get_page(page);
+#endif
+
+	return true;
+}
+
+/**
+ * fm10k_add_rx_frag - Add contents of Rx buffer to sk_buff
+ * @rx_ring: rx descriptor ring to transact packets on
+ * @rx_buffer: buffer containing page to add
+ * @rx_desc: descriptor containing length of buffer written by hardware
+ * @skb: sk_buff to place the data into
+ *
+ * This function will add the data contained in rx_buffer->page to the skb.
+ * This is done either through a direct copy if the data in the buffer is
+ * less than the skb header size, otherwise it will just attach the page as
+ * a frag to the skb.
+ *
+ * The function will then update the page offset if necessary and return
+ * true if the buffer can be reused by the interface.
+ **/
+static bool fm10k_add_rx_frag(struct fm10k_ring *rx_ring,
+			      struct fm10k_rx_buffer *rx_buffer,
+			      union fm10k_rx_desc *rx_desc,
+			      struct sk_buff *skb)
+{
+	struct page *page = rx_buffer->page;
+	unsigned int size = le16_to_cpu(rx_desc->w.length);
+#if (PAGE_SIZE < 8192)
+	unsigned int truesize = FM10K_RX_BUFSZ;
+#else
+	unsigned int truesize = ALIGN(size, L1_CACHE_BYTES);
+#endif
+
+	if ((size <= FM10K_RX_HDR_LEN) && !skb_is_nonlinear(skb)) {
+		unsigned char *va = page_address(page) + rx_buffer->page_offset;
+
+		memcpy(__skb_put(skb, size), va, ALIGN(size, sizeof(long)));
+
+		/* we can reuse buffer as-is, just make sure it is local */
+		if (likely(page_to_nid(page) == numa_mem_id()))
+			return true;
+
+		/* this page cannot be reused so discard it */
+		put_page(page);
+		return false;
+	}
+
+	skb_add_rx_frag(skb, skb_shinfo(skb)->nr_frags, page,
+			rx_buffer->page_offset, size, truesize);
+
+	return fm10k_can_reuse_rx_page(rx_buffer, page, truesize);
+}
+
+static struct sk_buff *fm10k_fetch_rx_buffer(struct fm10k_ring *rx_ring,
+					     union fm10k_rx_desc *rx_desc,
+					     struct sk_buff *skb)
+{
+	struct fm10k_rx_buffer *rx_buffer;
+	struct page *page;
+
+	rx_buffer = &rx_ring->rx_buffer[rx_ring->next_to_clean];
+
+	page = rx_buffer->page;
+	prefetchw(page);
+
+	if (likely(!skb)) {
+		void *page_addr = page_address(page) +
+				  rx_buffer->page_offset;
+
+		/* prefetch first cache line of first page */
+		prefetch(page_addr);
+#if L1_CACHE_BYTES < 128
+		prefetch(page_addr + L1_CACHE_BYTES);
+#endif
+
+		/* allocate a skb to store the frags */
+		skb = netdev_alloc_skb_ip_align(rx_ring->netdev,
+						FM10K_RX_HDR_LEN);
+		if (unlikely(!skb)) {
+			rx_ring->rx_stats.alloc_failed++;
+			return NULL;
+		}
+
+		/* we will be copying header into skb->data in
+		 * pskb_may_pull so it is in our interest to prefetch
+		 * it now to avoid a possible cache miss
+		 */
+		prefetchw(skb->data);
+	}
+
+	/* we are reusing so sync this buffer for CPU use */
+	dma_sync_single_range_for_cpu(rx_ring->dev,
+				      rx_buffer->dma,
+				      rx_buffer->page_offset,
+				      FM10K_RX_BUFSZ,
+				      DMA_FROM_DEVICE);
+
+	/* pull page into skb */
+	if (fm10k_add_rx_frag(rx_ring, rx_buffer, rx_desc, skb)) {
+		/* hand second half of page back to the ring */
+		fm10k_reuse_rx_page(rx_ring, rx_buffer);
+	} else {
+		/* we are not reusing the buffer so unmap it */
+		dma_unmap_page(rx_ring->dev, rx_buffer->dma,
+			       PAGE_SIZE, DMA_FROM_DEVICE);
+	}
+
+	/* clear contents of rx_buffer */
+	rx_buffer->page = NULL;
+
+	return skb;
+}
+
+static inline void fm10k_rx_checksum(struct fm10k_ring *ring,
+				     union fm10k_rx_desc *rx_desc,
+				     struct sk_buff *skb)
+{
+	skb_checksum_none_assert(skb);
+
+	/* Rx checksum disabled via ethtool */
+	if (!(ring->netdev->features & NETIF_F_RXCSUM))
+		return;
+
+	/* TCP/UDP checksum error bit is set */
+	if (fm10k_test_staterr(rx_desc,
+			       FM10K_RXD_STATUS_L4E |
+			       FM10K_RXD_STATUS_L4E2 |
+			       FM10K_RXD_STATUS_IPE |
+			       FM10K_RXD_STATUS_IPE2)) {
+		ring->rx_stats.csum_err++;
+		return;
+	}
+
+	/* It must be a TCP or UDP packet with a valid checksum */
+	if (fm10k_test_staterr(rx_desc, FM10K_RXD_STATUS_L4CS2))
+		skb->encapsulation = true;
+	else if (!fm10k_test_staterr(rx_desc, FM10K_RXD_STATUS_L4CS))
+		return;
+
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+}
+
+#define FM10K_RSS_L4_TYPES_MASK \
+	((1ul << FM10K_RSSTYPE_IPV4_TCP) | \
+	 (1ul << FM10K_RSSTYPE_IPV4_UDP) | \
+	 (1ul << FM10K_RSSTYPE_IPV6_TCP) | \
+	 (1ul << FM10K_RSSTYPE_IPV6_UDP))
+
+static inline void fm10k_rx_hash(struct fm10k_ring *ring,
+				 union fm10k_rx_desc *rx_desc,
+				 struct sk_buff *skb)
+{
+	u16 rss_type;
+
+	if (!(ring->netdev->features & NETIF_F_RXHASH))
+		return;
+
+	rss_type = le16_to_cpu(rx_desc->w.pkt_info) & FM10K_RXD_RSSTYPE_MASK;
+	if (!rss_type)
+		return;
+
+	skb_set_hash(skb, le32_to_cpu(rx_desc->d.rss),
+		     (FM10K_RSS_L4_TYPES_MASK & (1ul << rss_type)) ?
+		     PKT_HASH_TYPE_L4 : PKT_HASH_TYPE_L3);
+}
+
+static void fm10k_rx_hwtstamp(struct fm10k_ring *rx_ring,
+			      union fm10k_rx_desc *rx_desc,
+			      struct sk_buff *skb)
+{
+	struct fm10k_intfc *interface = rx_ring->q_vector->interface;
+
+	FM10K_CB(skb)->tstamp = rx_desc->q.timestamp;
+
+	if (unlikely(interface->flags & FM10K_FLAG_RX_TS_ENABLED))
+		fm10k_systime_to_hwtstamp(interface, skb_hwtstamps(skb),
+					  le64_to_cpu(rx_desc->q.timestamp));
+}
+
+static void fm10k_type_trans(struct fm10k_ring *rx_ring,
+			     union fm10k_rx_desc *rx_desc,
+			     struct sk_buff *skb)
+{
+	struct net_device *dev = rx_ring->netdev;
+	struct fm10k_l2_accel *l2_accel = rcu_dereference_bh(rx_ring->l2_accel);
+
+	/* check to see if DGLORT belongs to a MACVLAN */
+	if (l2_accel) {
+		u16 idx = le16_to_cpu(FM10K_CB(skb)->fi.w.dglort) - 1;
+
+		idx -= l2_accel->dglort;
+		if (idx < l2_accel->size && l2_accel->macvlan[idx])
+			dev = l2_accel->macvlan[idx];
+		else
+			l2_accel = NULL;
+	}
+
+	skb->protocol = eth_type_trans(skb, dev);
+
+	if (!l2_accel)
+		return;
+
+	/* update MACVLAN statistics */
+	macvlan_count_rx(netdev_priv(dev), skb->len + ETH_HLEN, 1,
+			 !!(rx_desc->w.hdr_info &
+			    cpu_to_le16(FM10K_RXD_HDR_INFO_XC_MASK)));
+}
+
+/**
+ * fm10k_process_skb_fields - Populate skb header fields from Rx descriptor
+ * @rx_ring: rx descriptor ring packet is being transacted on
+ * @rx_desc: pointer to the EOP Rx descriptor
+ * @skb: pointer to current skb being populated
+ *
+ * This function checks the ring, descriptor, and packet information in
+ * order to populate the hash, checksum, VLAN, timestamp, protocol, and
+ * other fields within the skb.
+ **/
+static unsigned int fm10k_process_skb_fields(struct fm10k_ring *rx_ring,
+					     union fm10k_rx_desc *rx_desc,
+					     struct sk_buff *skb)
+{
+	unsigned int len = skb->len;
+
+	fm10k_rx_hash(rx_ring, rx_desc, skb);
+
+	fm10k_rx_checksum(rx_ring, rx_desc, skb);
+
+	fm10k_rx_hwtstamp(rx_ring, rx_desc, skb);
+
+	FM10K_CB(skb)->fi.w.vlan = rx_desc->w.vlan;
+
+	skb_record_rx_queue(skb, rx_ring->queue_index);
+
+	FM10K_CB(skb)->fi.d.glort = rx_desc->d.glort;
+
+	if (rx_desc->w.vlan) {
+		u16 vid = le16_to_cpu(rx_desc->w.vlan);
+
+		if (vid != rx_ring->vid)
+			__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q), vid);
+	}
+
+	fm10k_type_trans(rx_ring, rx_desc, skb);
+
+	return len;
+}
+
+/**
+ * fm10k_is_non_eop - process handling of non-EOP buffers
+ * @rx_ring: Rx ring being processed
+ * @rx_desc: Rx descriptor for current buffer
+ *
+ * This function updates next to clean.  If the buffer is an EOP buffer
+ * this function exits returning false, otherwise it will place the
+ * sk_buff in the next buffer to be chained and return true indicating
+ * that this is in fact a non-EOP buffer.
+ **/
+static bool fm10k_is_non_eop(struct fm10k_ring *rx_ring,
+			     union fm10k_rx_desc *rx_desc)
+{
+	u32 ntc = rx_ring->next_to_clean + 1;
+
+	/* fetch, update, and store next to clean */
+	ntc = (ntc < rx_ring->count) ? ntc : 0;
+	rx_ring->next_to_clean = ntc;
+
+	prefetch(FM10K_RX_DESC(rx_ring, ntc));
+
+	if (likely(fm10k_test_staterr(rx_desc, FM10K_RXD_STATUS_EOP)))
+		return false;
+
+	return true;
+}
+
+/**
+ * fm10k_pull_tail - fm10k specific version of skb_pull_tail
+ * @rx_ring: rx descriptor ring packet is being transacted on
+ * @rx_desc: pointer to the EOP Rx descriptor
+ * @skb: pointer to current skb being adjusted
+ *
+ * This function is an fm10k specific version of __pskb_pull_tail.  The
+ * main difference between this version and the original function is that
+ * this function can make several assumptions about the state of things
+ * that allow for significant optimizations versus the standard function.
+ * As a result we can do things like drop a frag and maintain an accurate
+ * truesize for the skb.
+ */
+static void fm10k_pull_tail(struct fm10k_ring *rx_ring,
+			    union fm10k_rx_desc *rx_desc,
+			    struct sk_buff *skb)
+{
+	struct skb_frag_struct *frag = &skb_shinfo(skb)->frags[0];
+	unsigned char *va;
+	unsigned int pull_len;
+
+	/* it is valid to use page_address instead of kmap since we are
+	 * working with pages allocated out of the lomem pool per
+	 * alloc_page(GFP_ATOMIC)
+	 */
+	va = skb_frag_address(frag);
+
+	/* we need the header to contain the greater of either ETH_HLEN or
+	 * 60 bytes if the skb->len is less than 60 for skb_pad.
+	 */
+	pull_len = eth_get_headlen(va, FM10K_RX_HDR_LEN);
+
+	/* align pull length to size of long to optimize memcpy performance */
+	skb_copy_to_linear_data(skb, va, ALIGN(pull_len, sizeof(long)));
+
+	/* update all of the pointers */
+	skb_frag_size_sub(frag, pull_len);
+	frag->page_offset += pull_len;
+	skb->data_len -= pull_len;
+	skb->tail += pull_len;
+}
+
+/**
+ * fm10k_cleanup_headers - Correct corrupted or empty headers
+ * @rx_ring: rx descriptor ring packet is being transacted on
+ * @rx_desc: pointer to the EOP Rx descriptor
+ * @skb: pointer to current skb being fixed
+ *
+ * Address the case where we are pulling data in on pages only
+ * and as such no data is present in the skb header.
+ *
+ * In addition if skb is not at least 60 bytes we need to pad it so that
+ * it is large enough to qualify as a valid Ethernet frame.
+ *
+ * Returns true if an error was encountered and skb was freed.
+ **/
+static bool fm10k_cleanup_headers(struct fm10k_ring *rx_ring,
+				  union fm10k_rx_desc *rx_desc,
+				  struct sk_buff *skb)
+{
+	if (unlikely((fm10k_test_staterr(rx_desc,
+					 FM10K_RXD_STATUS_RXE)))) {
+		dev_kfree_skb_any(skb);
+		rx_ring->rx_stats.errors++;
+		return true;
+	}
+
+	/* place header in linear portion of buffer */
+	if (skb_is_nonlinear(skb))
+		fm10k_pull_tail(rx_ring, rx_desc, skb);
+
+	/* if skb_pad returns an error the skb was freed */
+	if (unlikely(skb->len < 60)) {
+		int pad_len = 60 - skb->len;
+
+		if (skb_pad(skb, pad_len))
+			return true;
+		__skb_put(skb, pad_len);
+	}
+
+	return false;
+}
+
+/**
+ * fm10k_receive_skb - helper function to handle rx indications
+ * @q_vector: structure containing interrupt and ring information
+ * @skb: packet to send up
+ **/
+static void fm10k_receive_skb(struct fm10k_q_vector *q_vector,
+			      struct sk_buff *skb)
+{
+	napi_gro_receive(&q_vector->napi, skb);
+}
+
+static bool fm10k_clean_rx_irq(struct fm10k_q_vector *q_vector,
+			       struct fm10k_ring *rx_ring,
+			       int budget)
+{
+	struct sk_buff *skb = rx_ring->skb;
+	unsigned int total_bytes = 0, total_packets = 0;
+	u16 cleaned_count = fm10k_desc_unused(rx_ring);
+
+	do {
+		union fm10k_rx_desc *rx_desc;
+
+		/* return some buffers to hardware, one at a time is too slow */
+		if (cleaned_count >= FM10K_RX_BUFFER_WRITE) {
+			fm10k_alloc_rx_buffers(rx_ring, cleaned_count);
+			cleaned_count = 0;
+		}
+
+		rx_desc = FM10K_RX_DESC(rx_ring, rx_ring->next_to_clean);
+
+		if (!fm10k_test_staterr(rx_desc, FM10K_RXD_STATUS_DD))
+			break;
+
+		/* This memory barrier is needed to keep us from reading
+		 * any other fields out of the rx_desc until we know the
+		 * RXD_STATUS_DD bit is set
+		 */
+		rmb();
+
+		/* retrieve a buffer from the ring */
+		skb = fm10k_fetch_rx_buffer(rx_ring, rx_desc, skb);
+
+		/* exit if we failed to retrieve a buffer */
+		if (!skb)
+			break;
+
+		cleaned_count++;
+
+		/* fetch next buffer in frame if non-eop */
+		if (fm10k_is_non_eop(rx_ring, rx_desc))
+			continue;
+
+		/* verify the packet layout is correct */
+		if (fm10k_cleanup_headers(rx_ring, rx_desc, skb)) {
+			skb = NULL;
+			continue;
+		}
+
+		/* populate checksum, timestamp, VLAN, and protocol */
+		total_bytes += fm10k_process_skb_fields(rx_ring, rx_desc, skb);
+
+		fm10k_receive_skb(q_vector, skb);
+
+		/* reset skb pointer */
+		skb = NULL;
+
+		/* update budget accounting */
+		total_packets++;
+	} while (likely(total_packets < budget));
+
+	/* place incomplete frames back on ring for completion */
+	rx_ring->skb = skb;
+
+	u64_stats_update_begin(&rx_ring->syncp);
+	rx_ring->stats.packets += total_packets;
+	rx_ring->stats.bytes += total_bytes;
+	u64_stats_update_end(&rx_ring->syncp);
+	q_vector->rx.total_packets += total_packets;
+	q_vector->rx.total_bytes += total_bytes;
+
+	return total_packets < budget;
+}
+
+#define VXLAN_HLEN (sizeof(struct udphdr) + 8)
+static struct ethhdr *fm10k_port_is_vxlan(struct sk_buff *skb)
+{
+	struct fm10k_intfc *interface = netdev_priv(skb->dev);
+	struct fm10k_vxlan_port *vxlan_port;
+
+	/* we can only offload a vxlan if we recognize it as such */
+	vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
+					      struct fm10k_vxlan_port, list);
+
+	if (!vxlan_port)
+		return NULL;
+	if (vxlan_port->port != udp_hdr(skb)->dest)
+		return NULL;
+
+	/* return offset of udp_hdr plus 8 bytes for VXLAN header */
+	return (struct ethhdr *)(skb_transport_header(skb) + VXLAN_HLEN);
+}
+
+#define FM10K_NVGRE_RESERVED0_FLAGS htons(0x9FFF)
+#define NVGRE_TNI htons(0x2000)
+struct fm10k_nvgre_hdr {
+	__be16 flags;
+	__be16 proto;
+	__be32 tni;
+};
+
+static struct ethhdr *fm10k_gre_is_nvgre(struct sk_buff *skb)
+{
+	struct fm10k_nvgre_hdr *nvgre_hdr;
+	int hlen = ip_hdrlen(skb);
+
+	/* currently only IPv4 is supported due to hlen above */
+	if (vlan_get_protocol(skb) != htons(ETH_P_IP))
+		return NULL;
+
+	/* our transport header should be NVGRE */
+	nvgre_hdr = (struct fm10k_nvgre_hdr *)(skb_network_header(skb) + hlen);
+
+	/* verify all reserved flags are 0 */
+	if (nvgre_hdr->flags & FM10K_NVGRE_RESERVED0_FLAGS)
+		return NULL;
+
+	/* verify protocol is transparent Ethernet bridging */
+	if (nvgre_hdr->proto != htons(ETH_P_TEB))
+		return NULL;
+
+	/* report start of ethernet header */
+	if (nvgre_hdr->flags & NVGRE_TNI)
+		return (struct ethhdr *)(nvgre_hdr + 1);
+
+	return (struct ethhdr *)(&nvgre_hdr->tni);
+}
+
+static __be16 fm10k_tx_encap_offload(struct sk_buff *skb)
+{
+	struct ethhdr *eth_hdr;
+	u8 l4_hdr = 0;
+
+	switch (vlan_get_protocol(skb)) {
+	case htons(ETH_P_IP):
+		l4_hdr = ip_hdr(skb)->protocol;
+		break;
+	case htons(ETH_P_IPV6):
+		l4_hdr = ipv6_hdr(skb)->nexthdr;
+		break;
+	default:
+		return 0;
+	}
+
+	switch (l4_hdr) {
+	case IPPROTO_UDP:
+		eth_hdr = fm10k_port_is_vxlan(skb);
+		break;
+	case IPPROTO_GRE:
+		eth_hdr = fm10k_gre_is_nvgre(skb);
+		break;
+	default:
+		return 0;
+	}
+
+	if (!eth_hdr)
+		return 0;
+
+	switch (eth_hdr->h_proto) {
+	case htons(ETH_P_IP):
+	case htons(ETH_P_IPV6):
+		break;
+	default:
+		return 0;
+	}
+
+	return eth_hdr->h_proto;
+}
+
+static int fm10k_tso(struct fm10k_ring *tx_ring,
+		     struct fm10k_tx_buffer *first)
+{
+	struct sk_buff *skb = first->skb;
+	struct fm10k_tx_desc *tx_desc;
+	unsigned char *th;
+	u8 hdrlen;
+
+	if (skb->ip_summed != CHECKSUM_PARTIAL)
+		return 0;
+
+	if (!skb_is_gso(skb))
+		return 0;
+
+	/* compute header lengths */
+	if (skb->encapsulation) {
+		if (!fm10k_tx_encap_offload(skb))
+			goto err_vxlan;
+		th = skb_inner_transport_header(skb);
+	} else {
+		th = skb_transport_header(skb);
+	}
+
+	/* compute offset from SOF to transport header and add header len */
+	hdrlen = (th - skb->data) + (((struct tcphdr *)th)->doff << 2);
+
+	first->tx_flags |= FM10K_TX_FLAGS_CSUM;
+
+	/* update gso size and bytecount with header size */
+	first->gso_segs = skb_shinfo(skb)->gso_segs;
+	first->bytecount += (first->gso_segs - 1) * hdrlen;
+
+	/* populate Tx descriptor header size and mss */
+	tx_desc = FM10K_TX_DESC(tx_ring, tx_ring->next_to_use);
+	tx_desc->hdrlen = hdrlen;
+	tx_desc->mss = cpu_to_le16(skb_shinfo(skb)->gso_size);
+
+	return 1;
+err_vxlan:
+	tx_ring->netdev->features &= ~NETIF_F_GSO_UDP_TUNNEL;
+	if (!net_ratelimit())
+		netdev_err(tx_ring->netdev,
+			   "TSO requested for unsupported tunnel, disabling offload\n");
+	return -1;
+}
+
+static void fm10k_tx_csum(struct fm10k_ring *tx_ring,
+			  struct fm10k_tx_buffer *first)
+{
+	struct sk_buff *skb = first->skb;
+	struct fm10k_tx_desc *tx_desc;
+	union {
+		struct iphdr *ipv4;
+		struct ipv6hdr *ipv6;
+		u8 *raw;
+	} network_hdr;
+	__be16 protocol;
+	u8 l4_hdr = 0;
+
+	if (skb->ip_summed != CHECKSUM_PARTIAL)
+		goto no_csum;
+
+	if (skb->encapsulation) {
+		protocol = fm10k_tx_encap_offload(skb);
+		if (!protocol) {
+			if (skb_checksum_help(skb)) {
+				dev_warn(tx_ring->dev,
+					 "failed to offload encap csum!\n");
+				tx_ring->tx_stats.csum_err++;
+			}
+			goto no_csum;
+		}
+		network_hdr.raw = skb_inner_network_header(skb);
+	} else {
+		protocol = vlan_get_protocol(skb);
+		network_hdr.raw = skb_network_header(skb);
+	}
+
+	switch (protocol) {
+	case htons(ETH_P_IP):
+		l4_hdr = network_hdr.ipv4->protocol;
+		break;
+	case htons(ETH_P_IPV6):
+		l4_hdr = network_hdr.ipv6->nexthdr;
+		break;
+	default:
+		if (unlikely(net_ratelimit())) {
+			dev_warn(tx_ring->dev,
+				 "partial checksum but ip version=%x!\n",
+				 protocol);
+		}
+		tx_ring->tx_stats.csum_err++;
+		goto no_csum;
+	}
+
+	switch (l4_hdr) {
+	case IPPROTO_TCP:
+	case IPPROTO_UDP:
+		break;
+	case IPPROTO_GRE:
+		if (skb->encapsulation)
+			break;
+	default:
+		if (unlikely(net_ratelimit())) {
+			dev_warn(tx_ring->dev,
+				 "partial checksum but l4 proto=%x!\n",
+				 l4_hdr);
+		}
+		tx_ring->tx_stats.csum_err++;
+		goto no_csum;
+	}
+
+	/* update TX checksum flag */
+	first->tx_flags |= FM10K_TX_FLAGS_CSUM;
+
+no_csum:
+	/* populate Tx descriptor header size and mss */
+	tx_desc = FM10K_TX_DESC(tx_ring, tx_ring->next_to_use);
+	tx_desc->hdrlen = 0;
+	tx_desc->mss = 0;
+}
+
+#define FM10K_SET_FLAG(_input, _flag, _result) \
+	((_flag <= _result) ? \
+	 ((u32)(_input & _flag) * (_result / _flag)) : \
+	 ((u32)(_input & _flag) / (_flag / _result)))
+
+static u8 fm10k_tx_desc_flags(struct sk_buff *skb, u32 tx_flags)
+{
+	/* set type for advanced descriptor with frame checksum insertion */
+	u32 desc_flags = 0;
+
+	/* set timestamping bits */
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
+	    likely(skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS))
+			desc_flags |= FM10K_TXD_FLAG_TIME;
+
+	/* set checksum offload bits */
+	desc_flags |= FM10K_SET_FLAG(tx_flags, FM10K_TX_FLAGS_CSUM,
+				     FM10K_TXD_FLAG_CSUM);
+
+	return desc_flags;
+}
+
+static bool fm10k_tx_desc_push(struct fm10k_ring *tx_ring,
+			       struct fm10k_tx_desc *tx_desc, u16 i,
+			       dma_addr_t dma, unsigned int size, u8 desc_flags)
+{
+	/* set RS and INT for last frame in a cache line */
+	if ((++i & (FM10K_TXD_WB_FIFO_SIZE - 1)) == 0)
+		desc_flags |= FM10K_TXD_FLAG_RS | FM10K_TXD_FLAG_INT;
+
+	/* record values to descriptor */
+	tx_desc->buffer_addr = cpu_to_le64(dma);
+	tx_desc->flags = desc_flags;
+	tx_desc->buflen = cpu_to_le16(size);
+
+	/* return true if we just wrapped the ring */
+	return i == tx_ring->count;
+}
+
+static int __fm10k_maybe_stop_tx(struct fm10k_ring *tx_ring, u16 size)
+{
+	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
+
+	smp_mb();
+
+	/* We need to check again in a case another CPU has just
+	 * made room available. */
+	if (likely(fm10k_desc_unused(tx_ring) < size))
+		return -EBUSY;
+
+	/* A reprieve! - use start_queue because it doesn't call schedule */
+	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
+	++tx_ring->tx_stats.restart_queue;
+	return 0;
+}
+
+static inline int fm10k_maybe_stop_tx(struct fm10k_ring *tx_ring, u16 size)
+{
+	if (likely(fm10k_desc_unused(tx_ring) >= size))
+		return 0;
+	return __fm10k_maybe_stop_tx(tx_ring, size);
+}
+
+static void fm10k_tx_map(struct fm10k_ring *tx_ring,
+			 struct fm10k_tx_buffer *first)
+{
+	struct sk_buff *skb = first->skb;
+	struct fm10k_tx_buffer *tx_buffer;
+	struct fm10k_tx_desc *tx_desc;
+	struct skb_frag_struct *frag;
+	unsigned char *data;
+	dma_addr_t dma;
+	unsigned int data_len, size;
+	u32 tx_flags = first->tx_flags;
+	u16 i = tx_ring->next_to_use;
+	u8 flags = fm10k_tx_desc_flags(skb, tx_flags);
+
+	tx_desc = FM10K_TX_DESC(tx_ring, i);
+
+	/* add HW VLAN tag */
+	if (vlan_tx_tag_present(skb))
+		tx_desc->vlan = cpu_to_le16(vlan_tx_tag_get(skb));
+	else
+		tx_desc->vlan = 0;
+
+	size = skb_headlen(skb);
+	data = skb->data;
+
+	dma = dma_map_single(tx_ring->dev, data, size, DMA_TO_DEVICE);
+
+	data_len = skb->data_len;
+	tx_buffer = first;
+
+	for (frag = &skb_shinfo(skb)->frags[0];; frag++) {
+		if (dma_mapping_error(tx_ring->dev, dma))
+			goto dma_error;
+
+		/* record length, and DMA address */
+		dma_unmap_len_set(tx_buffer, len, size);
+		dma_unmap_addr_set(tx_buffer, dma, dma);
+
+		while (unlikely(size > FM10K_MAX_DATA_PER_TXD)) {
+			if (fm10k_tx_desc_push(tx_ring, tx_desc++, i++, dma,
+					       FM10K_MAX_DATA_PER_TXD, flags)) {
+				tx_desc = FM10K_TX_DESC(tx_ring, 0);
+				i = 0;
+			}
+
+			dma += FM10K_MAX_DATA_PER_TXD;
+			size -= FM10K_MAX_DATA_PER_TXD;
+		}
+
+		if (likely(!data_len))
+			break;
+
+		if (fm10k_tx_desc_push(tx_ring, tx_desc++, i++,
+				       dma, size, flags)) {
+			tx_desc = FM10K_TX_DESC(tx_ring, 0);
+			i = 0;
+		}
+
+		size = skb_frag_size(frag);
+		data_len -= size;
+
+		dma = skb_frag_dma_map(tx_ring->dev, frag, 0, size,
+				       DMA_TO_DEVICE);
+
+		tx_buffer = &tx_ring->tx_buffer[i];
+	}
+
+	/* write last descriptor with LAST bit set */
+	flags |= FM10K_TXD_FLAG_LAST;
+
+	if (fm10k_tx_desc_push(tx_ring, tx_desc, i++, dma, size, flags))
+		i = 0;
+
+	/* record bytecount for BQL */
+	netdev_tx_sent_queue(txring_txq(tx_ring), first->bytecount);
+
+	/* record SW timestamp if HW timestamp is not available */
+	skb_tx_timestamp(first->skb);
+
+	/* Force memory writes to complete before letting h/w know there
+	 * are new descriptors to fetch.  (Only applicable for weak-ordered
+	 * memory model archs, such as IA-64).
+	 *
+	 * We also need this memory barrier to make certain all of the
+	 * status bits have been updated before next_to_watch is written.
+	 */
+	wmb();
+
+	/* set next_to_watch value indicating a packet is present */
+	first->next_to_watch = tx_desc;
+
+	tx_ring->next_to_use = i;
+
+	/* Make sure there is space in the ring for the next send. */
+	fm10k_maybe_stop_tx(tx_ring, DESC_NEEDED);
+
+	/* notify HW of packet */
+	if (netif_xmit_stopped(txring_txq(tx_ring)) || !skb->xmit_more) {
+		writel(i, tx_ring->tail);
+
+		/* we need this if more than one processor can write to our tail
+		 * at a time, it synchronizes IO on IA64/Altix systems
+		 */
+		mmiowb();
+	}
+
+	return;
+dma_error:
+	dev_err(tx_ring->dev, "TX DMA map failed\n");
+
+	/* clear dma mappings for failed tx_buffer map */
+	for (;;) {
+		tx_buffer = &tx_ring->tx_buffer[i];
+		fm10k_unmap_and_free_tx_resource(tx_ring, tx_buffer);
+		if (tx_buffer == first)
+			break;
+		if (i == 0)
+			i = tx_ring->count;
+		i--;
+	}
+
+	tx_ring->next_to_use = i;
+}
+
+netdev_tx_t fm10k_xmit_frame_ring(struct sk_buff *skb,
+				  struct fm10k_ring *tx_ring)
+{
+	struct fm10k_tx_buffer *first;
+	int tso;
+	u32 tx_flags = 0;
+#if PAGE_SIZE > FM10K_MAX_DATA_PER_TXD
+	unsigned short f;
+#endif
+	u16 count = TXD_USE_COUNT(skb_headlen(skb));
+
+	/* need: 1 descriptor per page * PAGE_SIZE/FM10K_MAX_DATA_PER_TXD,
+	 *       + 1 desc for skb_headlen/FM10K_MAX_DATA_PER_TXD,
+	 *       + 2 desc gap to keep tail from touching head
+	 * otherwise try next time
+	 */
+#if PAGE_SIZE > FM10K_MAX_DATA_PER_TXD
+	for (f = 0; f < skb_shinfo(skb)->nr_frags; f++)
+		count += TXD_USE_COUNT(skb_shinfo(skb)->frags[f].size);
+#else
+	count += skb_shinfo(skb)->nr_frags;
+#endif
+	if (fm10k_maybe_stop_tx(tx_ring, count + 3)) {
+		tx_ring->tx_stats.tx_busy++;
+		return NETDEV_TX_BUSY;
+	}
+
+	/* record the location of the first descriptor for this packet */
+	first = &tx_ring->tx_buffer[tx_ring->next_to_use];
+	first->skb = skb;
+	first->bytecount = max_t(unsigned int, skb->len, ETH_ZLEN);
+	first->gso_segs = 1;
+
+	/* record initial flags and protocol */
+	first->tx_flags = tx_flags;
+
+	tso = fm10k_tso(tx_ring, first);
+	if (tso < 0)
+		goto out_drop;
+	else if (!tso)
+		fm10k_tx_csum(tx_ring, first);
+
+	fm10k_tx_map(tx_ring, first);
+
+	return NETDEV_TX_OK;
+
+out_drop:
+	dev_kfree_skb_any(first->skb);
+	first->skb = NULL;
+
+	return NETDEV_TX_OK;
+}
+
+static u64 fm10k_get_tx_completed(struct fm10k_ring *ring)
+{
+	return ring->stats.packets;
+}
+
+static u64 fm10k_get_tx_pending(struct fm10k_ring *ring)
+{
+	/* use SW head and tail until we have real hardware */
+	u32 head = ring->next_to_clean;
+	u32 tail = ring->next_to_use;
+
+	return ((head <= tail) ? tail : tail + ring->count) - head;
+}
+
+bool fm10k_check_tx_hang(struct fm10k_ring *tx_ring)
+{
+	u32 tx_done = fm10k_get_tx_completed(tx_ring);
+	u32 tx_done_old = tx_ring->tx_stats.tx_done_old;
+	u32 tx_pending = fm10k_get_tx_pending(tx_ring);
+
+	clear_check_for_tx_hang(tx_ring);
+
+	/* Check for a hung queue, but be thorough. This verifies
+	 * that a transmit has been completed since the previous
+	 * check AND there is at least one packet pending. By
+	 * requiring this to fail twice we avoid races with
+	 * clearing the ARMED bit and conditions where we
+	 * run the check_tx_hang logic with a transmit completion
+	 * pending but without time to complete it yet.
+	 */
+	if (!tx_pending || (tx_done_old != tx_done)) {
+		/* update completed stats and continue */
+		tx_ring->tx_stats.tx_done_old = tx_done;
+		/* reset the countdown */
+		clear_bit(__FM10K_HANG_CHECK_ARMED, &tx_ring->state);
+
+		return false;
+	}
+
+	/* make sure it is true for two checks in a row */
+	return test_and_set_bit(__FM10K_HANG_CHECK_ARMED, &tx_ring->state);
+}
+
+/**
+ * fm10k_tx_timeout_reset - initiate reset due to Tx timeout
+ * @interface: driver private struct
+ **/
+void fm10k_tx_timeout_reset(struct fm10k_intfc *interface)
+{
+	/* Do the reset outside of interrupt context */
+	if (!test_bit(__FM10K_DOWN, &interface->state)) {
+		netdev_err(interface->netdev, "Reset interface\n");
+		interface->tx_timeout_count++;
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+		fm10k_service_event_schedule(interface);
+	}
+}
+
+/**
+ * fm10k_clean_tx_irq - Reclaim resources after transmit completes
+ * @q_vector: structure containing interrupt and ring information
+ * @tx_ring: tx ring to clean
+ **/
+static bool fm10k_clean_tx_irq(struct fm10k_q_vector *q_vector,
+			       struct fm10k_ring *tx_ring)
+{
+	struct fm10k_intfc *interface = q_vector->interface;
+	struct fm10k_tx_buffer *tx_buffer;
+	struct fm10k_tx_desc *tx_desc;
+	unsigned int total_bytes = 0, total_packets = 0;
+	unsigned int budget = q_vector->tx.work_limit;
+	unsigned int i = tx_ring->next_to_clean;
+
+	if (test_bit(__FM10K_DOWN, &interface->state))
+		return true;
+
+	tx_buffer = &tx_ring->tx_buffer[i];
+	tx_desc = FM10K_TX_DESC(tx_ring, i);
+	i -= tx_ring->count;
+
+	do {
+		struct fm10k_tx_desc *eop_desc = tx_buffer->next_to_watch;
+
+		/* if next_to_watch is not set then there is no work pending */
+		if (!eop_desc)
+			break;
+
+		/* prevent any other reads prior to eop_desc */
+		read_barrier_depends();
+
+		/* if DD is not set pending work has not been completed */
+		if (!(eop_desc->flags & FM10K_TXD_FLAG_DONE))
+			break;
+
+		/* clear next_to_watch to prevent false hangs */
+		tx_buffer->next_to_watch = NULL;
+
+		/* update the statistics for this packet */
+		total_bytes += tx_buffer->bytecount;
+		total_packets += tx_buffer->gso_segs;
+
+		/* free the skb */
+		dev_consume_skb_any(tx_buffer->skb);
+
+		/* unmap skb header data */
+		dma_unmap_single(tx_ring->dev,
+				 dma_unmap_addr(tx_buffer, dma),
+				 dma_unmap_len(tx_buffer, len),
+				 DMA_TO_DEVICE);
+
+		/* clear tx_buffer data */
+		tx_buffer->skb = NULL;
+		dma_unmap_len_set(tx_buffer, len, 0);
+
+		/* unmap remaining buffers */
+		while (tx_desc != eop_desc) {
+			tx_buffer++;
+			tx_desc++;
+			i++;
+			if (unlikely(!i)) {
+				i -= tx_ring->count;
+				tx_buffer = tx_ring->tx_buffer;
+				tx_desc = FM10K_TX_DESC(tx_ring, 0);
+			}
+
+			/* unmap any remaining paged data */
+			if (dma_unmap_len(tx_buffer, len)) {
+				dma_unmap_page(tx_ring->dev,
+					       dma_unmap_addr(tx_buffer, dma),
+					       dma_unmap_len(tx_buffer, len),
+					       DMA_TO_DEVICE);
+				dma_unmap_len_set(tx_buffer, len, 0);
+			}
+		}
+
+		/* move us one more past the eop_desc for start of next pkt */
+		tx_buffer++;
+		tx_desc++;
+		i++;
+		if (unlikely(!i)) {
+			i -= tx_ring->count;
+			tx_buffer = tx_ring->tx_buffer;
+			tx_desc = FM10K_TX_DESC(tx_ring, 0);
+		}
+
+		/* issue prefetch for next Tx descriptor */
+		prefetch(tx_desc);
+
+		/* update budget accounting */
+		budget--;
+	} while (likely(budget));
+
+	i += tx_ring->count;
+	tx_ring->next_to_clean = i;
+	u64_stats_update_begin(&tx_ring->syncp);
+	tx_ring->stats.bytes += total_bytes;
+	tx_ring->stats.packets += total_packets;
+	u64_stats_update_end(&tx_ring->syncp);
+	q_vector->tx.total_bytes += total_bytes;
+	q_vector->tx.total_packets += total_packets;
+
+	if (check_for_tx_hang(tx_ring) && fm10k_check_tx_hang(tx_ring)) {
+		/* schedule immediate reset if we believe we hung */
+		struct fm10k_hw *hw = &interface->hw;
+
+		netif_err(interface, drv, tx_ring->netdev,
+			  "Detected Tx Unit Hang\n"
+			  "  Tx Queue             <%d>\n"
+			  "  TDH, TDT             <%x>, <%x>\n"
+			  "  next_to_use          <%x>\n"
+			  "  next_to_clean        <%x>\n",
+			  tx_ring->queue_index,
+			  fm10k_read_reg(hw, FM10K_TDH(tx_ring->reg_idx)),
+			  fm10k_read_reg(hw, FM10K_TDT(tx_ring->reg_idx)),
+			  tx_ring->next_to_use, i);
+
+		netif_stop_subqueue(tx_ring->netdev,
+				    tx_ring->queue_index);
+
+		netif_info(interface, probe, tx_ring->netdev,
+			   "tx hang %d detected on queue %d, resetting interface\n",
+			   interface->tx_timeout_count + 1,
+			   tx_ring->queue_index);
+
+		fm10k_tx_timeout_reset(interface);
+
+		/* the netdev is about to reset, no point in enabling stuff */
+		return true;
+	}
+
+	/* notify netdev of completed buffers */
+	netdev_tx_completed_queue(txring_txq(tx_ring),
+				  total_packets, total_bytes);
+
+#define TX_WAKE_THRESHOLD min_t(u16, FM10K_MIN_TXD - 1, DESC_NEEDED * 2)
+	if (unlikely(total_packets && netif_carrier_ok(tx_ring->netdev) &&
+		     (fm10k_desc_unused(tx_ring) >= TX_WAKE_THRESHOLD))) {
+		/* Make sure that anybody stopping the queue after this
+		 * sees the new next_to_clean.
+		 */
+		smp_mb();
+		if (__netif_subqueue_stopped(tx_ring->netdev,
+					     tx_ring->queue_index) &&
+		    !test_bit(__FM10K_DOWN, &interface->state)) {
+			netif_wake_subqueue(tx_ring->netdev,
+					    tx_ring->queue_index);
+			++tx_ring->tx_stats.restart_queue;
+		}
+	}
+
+	return !!budget;
+}
+
+/**
+ * fm10k_update_itr - update the dynamic ITR value based on packet size
+ *
+ *      Stores a new ITR value based on strictly on packet size.  The
+ *      divisors and thresholds used by this function were determined based
+ *      on theoretical maximum wire speed and testing data, in order to
+ *      minimize response time while increasing bulk throughput.
+ *
+ * @ring_container: Container for rings to have ITR updated
+ **/
+static void fm10k_update_itr(struct fm10k_ring_container *ring_container)
+{
+	unsigned int avg_wire_size, packets;
+
+	/* Only update ITR if we are using adaptive setting */
+	if (!(ring_container->itr & FM10K_ITR_ADAPTIVE))
+		goto clear_counts;
+
+	packets = ring_container->total_packets;
+	if (!packets)
+		goto clear_counts;
+
+	avg_wire_size = ring_container->total_bytes / packets;
+
+	/* Add 24 bytes to size to account for CRC, preamble, and gap */
+	avg_wire_size += 24;
+
+	/* Don't starve jumbo frames */
+	if (avg_wire_size > 3000)
+		avg_wire_size = 3000;
+
+	/* Give a little boost to mid-size frames */
+	if ((avg_wire_size > 300) && (avg_wire_size < 1200))
+		avg_wire_size /= 3;
+	else
+		avg_wire_size /= 2;
+
+	/* write back value and retain adaptive flag */
+	ring_container->itr = avg_wire_size | FM10K_ITR_ADAPTIVE;
+
+clear_counts:
+	ring_container->total_bytes = 0;
+	ring_container->total_packets = 0;
+}
+
+static void fm10k_qv_enable(struct fm10k_q_vector *q_vector)
+{
+	/* Enable auto-mask and clear the current mask */
+	u32 itr = FM10K_ITR_ENABLE;
+
+	/* Update Tx ITR */
+	fm10k_update_itr(&q_vector->tx);
+
+	/* Update Rx ITR */
+	fm10k_update_itr(&q_vector->rx);
+
+	/* Store Tx itr in timer slot 0 */
+	itr |= (q_vector->tx.itr & FM10K_ITR_MAX);
+
+	/* Shift Rx itr to timer slot 1 */
+	itr |= (q_vector->rx.itr & FM10K_ITR_MAX) << FM10K_ITR_INTERVAL1_SHIFT;
+
+	/* Write the final value to the ITR register */
+	writel(itr, q_vector->itr);
+}
+
+static int fm10k_poll(struct napi_struct *napi, int budget)
+{
+	struct fm10k_q_vector *q_vector =
+			       container_of(napi, struct fm10k_q_vector, napi);
+	struct fm10k_ring *ring;
+	int per_ring_budget;
+	bool clean_complete = true;
+
+	fm10k_for_each_ring(ring, q_vector->tx)
+		clean_complete &= fm10k_clean_tx_irq(q_vector, ring);
+
+	/* attempt to distribute budget to each queue fairly, but don't
+	 * allow the budget to go below 1 because we'll exit polling
+	 */
+	if (q_vector->rx.count > 1)
+		per_ring_budget = max(budget/q_vector->rx.count, 1);
+	else
+		per_ring_budget = budget;
+
+	fm10k_for_each_ring(ring, q_vector->rx)
+		clean_complete &= fm10k_clean_rx_irq(q_vector, ring,
+						     per_ring_budget);
+
+	/* If all work not completed, return budget and keep polling */
+	if (!clean_complete)
+		return budget;
+
+	/* all work done, exit the polling mode */
+	napi_complete(napi);
+
+	/* re-enable the q_vector */
+	fm10k_qv_enable(q_vector);
+
+	return 0;
+}
+
+/**
+ * fm10k_set_qos_queues: Allocate queues for a QOS-enabled device
+ * @interface: board private structure to initialize
+ *
+ * When QoS (Quality of Service) is enabled, allocate queues for
+ * each traffic class.  If multiqueue isn't available,then abort QoS
+ * initialization.
+ *
+ * This function handles all combinations of Qos and RSS.
+ *
+ **/
+static bool fm10k_set_qos_queues(struct fm10k_intfc *interface)
+{
+	struct net_device *dev = interface->netdev;
+	struct fm10k_ring_feature *f;
+	int rss_i, i;
+	int pcs;
+
+	/* Map queue offset and counts onto allocated tx queues */
+	pcs = netdev_get_num_tc(dev);
+
+	if (pcs <= 1)
+		return false;
+
+	/* set QoS mask and indices */
+	f = &interface->ring_feature[RING_F_QOS];
+	f->indices = pcs;
+	f->mask = (1 << fls(pcs - 1)) - 1;
+
+	/* determine the upper limit for our current DCB mode */
+	rss_i = interface->hw.mac.max_queues / pcs;
+	rss_i = 1 << (fls(rss_i) - 1);
+
+	/* set RSS mask and indices */
+	f = &interface->ring_feature[RING_F_RSS];
+	rss_i = min_t(u16, rss_i, f->limit);
+	f->indices = rss_i;
+	f->mask = (1 << fls(rss_i - 1)) - 1;
+
+	/* configure pause class to queue mapping */
+	for (i = 0; i < pcs; i++)
+		netdev_set_tc_queue(dev, i, rss_i, rss_i * i);
+
+	interface->num_rx_queues = rss_i * pcs;
+	interface->num_tx_queues = rss_i * pcs;
+
+	return true;
+}
+
+/**
+ * fm10k_set_rss_queues: Allocate queues for RSS
+ * @interface: board private structure to initialize
+ *
+ * This is our "base" multiqueue mode.  RSS (Receive Side Scaling) will try
+ * to allocate one Rx queue per CPU, and if available, one Tx queue per CPU.
+ *
+ **/
+static bool fm10k_set_rss_queues(struct fm10k_intfc *interface)
+{
+	struct fm10k_ring_feature *f;
+	u16 rss_i;
+
+	f = &interface->ring_feature[RING_F_RSS];
+	rss_i = min_t(u16, interface->hw.mac.max_queues, f->limit);
+
+	/* record indices and power of 2 mask for RSS */
+	f->indices = rss_i;
+	f->mask = (1 << fls(rss_i - 1)) - 1;
+
+	interface->num_rx_queues = rss_i;
+	interface->num_tx_queues = rss_i;
+
+	return true;
+}
+
+/**
+ * fm10k_set_num_queues: Allocate queues for device, feature dependent
+ * @interface: board private structure to initialize
+ *
+ * This is the top level queue allocation routine.  The order here is very
+ * important, starting with the "most" number of features turned on at once,
+ * and ending with the smallest set of features.  This way large combinations
+ * can be allocated if they're turned on, and smaller combinations are the
+ * fallthrough conditions.
+ *
+ **/
+static void fm10k_set_num_queues(struct fm10k_intfc *interface)
+{
+	/* Start with base case */
+	interface->num_rx_queues = 1;
+	interface->num_tx_queues = 1;
+
+	if (fm10k_set_qos_queues(interface))
+		return;
+
+	fm10k_set_rss_queues(interface);
+}
+
+/**
+ * fm10k_alloc_q_vector - Allocate memory for a single interrupt vector
+ * @interface: board private structure to initialize
+ * @v_count: q_vectors allocated on interface, used for ring interleaving
+ * @v_idx: index of vector in interface struct
+ * @txr_count: total number of Tx rings to allocate
+ * @txr_idx: index of first Tx ring to allocate
+ * @rxr_count: total number of Rx rings to allocate
+ * @rxr_idx: index of first Rx ring to allocate
+ *
+ * We allocate one q_vector.  If allocation fails we return -ENOMEM.
+ **/
+static int fm10k_alloc_q_vector(struct fm10k_intfc *interface,
+				unsigned int v_count, unsigned int v_idx,
+				unsigned int txr_count, unsigned int txr_idx,
+				unsigned int rxr_count, unsigned int rxr_idx)
+{
+	struct fm10k_q_vector *q_vector;
+	struct fm10k_ring *ring;
+	int ring_count, size;
+
+	ring_count = txr_count + rxr_count;
+	size = sizeof(struct fm10k_q_vector) +
+	       (sizeof(struct fm10k_ring) * ring_count);
+
+	/* allocate q_vector and rings */
+	q_vector = kzalloc(size, GFP_KERNEL);
+	if (!q_vector)
+		return -ENOMEM;
+
+	/* initialize NAPI */
+	netif_napi_add(interface->netdev, &q_vector->napi,
+		       fm10k_poll, NAPI_POLL_WEIGHT);
+
+	/* tie q_vector and interface together */
+	interface->q_vector[v_idx] = q_vector;
+	q_vector->interface = interface;
+	q_vector->v_idx = v_idx;
+
+	/* initialize pointer to rings */
+	ring = q_vector->ring;
+
+	/* save Tx ring container info */
+	q_vector->tx.ring = ring;
+	q_vector->tx.work_limit = FM10K_DEFAULT_TX_WORK;
+	q_vector->tx.itr = interface->tx_itr;
+	q_vector->tx.count = txr_count;
+
+	while (txr_count) {
+		/* assign generic ring traits */
+		ring->dev = &interface->pdev->dev;
+		ring->netdev = interface->netdev;
+
+		/* configure backlink on ring */
+		ring->q_vector = q_vector;
+
+		/* apply Tx specific ring traits */
+		ring->count = interface->tx_ring_count;
+		ring->queue_index = txr_idx;
+
+		/* assign ring to interface */
+		interface->tx_ring[txr_idx] = ring;
+
+		/* update count and index */
+		txr_count--;
+		txr_idx += v_count;
+
+		/* push pointer to next ring */
+		ring++;
+	}
+
+	/* save Rx ring container info */
+	q_vector->rx.ring = ring;
+	q_vector->rx.itr = interface->rx_itr;
+	q_vector->rx.count = rxr_count;
+
+	while (rxr_count) {
+		/* assign generic ring traits */
+		ring->dev = &interface->pdev->dev;
+		ring->netdev = interface->netdev;
+		rcu_assign_pointer(ring->l2_accel, interface->l2_accel);
+
+		/* configure backlink on ring */
+		ring->q_vector = q_vector;
+
+		/* apply Rx specific ring traits */
+		ring->count = interface->rx_ring_count;
+		ring->queue_index = rxr_idx;
+
+		/* assign ring to interface */
+		interface->rx_ring[rxr_idx] = ring;
+
+		/* update count and index */
+		rxr_count--;
+		rxr_idx += v_count;
+
+		/* push pointer to next ring */
+		ring++;
+	}
+
+	fm10k_dbg_q_vector_init(q_vector);
+
+	return 0;
+}
+
+/**
+ * fm10k_free_q_vector - Free memory allocated for specific interrupt vector
+ * @interface: board private structure to initialize
+ * @v_idx: Index of vector to be freed
+ *
+ * This function frees the memory allocated to the q_vector.  In addition if
+ * NAPI is enabled it will delete any references to the NAPI struct prior
+ * to freeing the q_vector.
+ **/
+static void fm10k_free_q_vector(struct fm10k_intfc *interface, int v_idx)
+{
+	struct fm10k_q_vector *q_vector = interface->q_vector[v_idx];
+	struct fm10k_ring *ring;
+
+	fm10k_dbg_q_vector_exit(q_vector);
+
+	fm10k_for_each_ring(ring, q_vector->tx)
+		interface->tx_ring[ring->queue_index] = NULL;
+
+	fm10k_for_each_ring(ring, q_vector->rx)
+		interface->rx_ring[ring->queue_index] = NULL;
+
+	interface->q_vector[v_idx] = NULL;
+	netif_napi_del(&q_vector->napi);
+	kfree_rcu(q_vector, rcu);
+}
+
+/**
+ * fm10k_alloc_q_vectors - Allocate memory for interrupt vectors
+ * @interface: board private structure to initialize
+ *
+ * We allocate one q_vector per queue interrupt.  If allocation fails we
+ * return -ENOMEM.
+ **/
+static int fm10k_alloc_q_vectors(struct fm10k_intfc *interface)
+{
+	unsigned int q_vectors = interface->num_q_vectors;
+	unsigned int rxr_remaining = interface->num_rx_queues;
+	unsigned int txr_remaining = interface->num_tx_queues;
+	unsigned int rxr_idx = 0, txr_idx = 0, v_idx = 0;
+	int err;
+
+	if (q_vectors >= (rxr_remaining + txr_remaining)) {
+		for (; rxr_remaining; v_idx++) {
+			err = fm10k_alloc_q_vector(interface, q_vectors, v_idx,
+						   0, 0, 1, rxr_idx);
+			if (err)
+				goto err_out;
+
+			/* update counts and index */
+			rxr_remaining--;
+			rxr_idx++;
+		}
+	}
+
+	for (; v_idx < q_vectors; v_idx++) {
+		int rqpv = DIV_ROUND_UP(rxr_remaining, q_vectors - v_idx);
+		int tqpv = DIV_ROUND_UP(txr_remaining, q_vectors - v_idx);
+
+		err = fm10k_alloc_q_vector(interface, q_vectors, v_idx,
+					   tqpv, txr_idx,
+					   rqpv, rxr_idx);
+
+		if (err)
+			goto err_out;
+
+		/* update counts and index */
+		rxr_remaining -= rqpv;
+		txr_remaining -= tqpv;
+		rxr_idx++;
+		txr_idx++;
+	}
+
+	return 0;
+
+err_out:
+	interface->num_tx_queues = 0;
+	interface->num_rx_queues = 0;
+	interface->num_q_vectors = 0;
+
+	while (v_idx--)
+		fm10k_free_q_vector(interface, v_idx);
+
+	return -ENOMEM;
+}
+
+/**
+ * fm10k_free_q_vectors - Free memory allocated for interrupt vectors
+ * @interface: board private structure to initialize
+ *
+ * This function frees the memory allocated to the q_vectors.  In addition if
+ * NAPI is enabled it will delete any references to the NAPI struct prior
+ * to freeing the q_vector.
+ **/
+static void fm10k_free_q_vectors(struct fm10k_intfc *interface)
+{
+	int v_idx = interface->num_q_vectors;
+
+	interface->num_tx_queues = 0;
+	interface->num_rx_queues = 0;
+	interface->num_q_vectors = 0;
+
+	while (v_idx--)
+		fm10k_free_q_vector(interface, v_idx);
+}
+
+/**
+ * f10k_reset_msix_capability - reset MSI-X capability
+ * @interface: board private structure to initialize
+ *
+ * Reset the MSI-X capability back to its starting state
+ **/
+static void fm10k_reset_msix_capability(struct fm10k_intfc *interface)
+{
+	pci_disable_msix(interface->pdev);
+	kfree(interface->msix_entries);
+	interface->msix_entries = NULL;
+}
+
+/**
+ * f10k_init_msix_capability - configure MSI-X capability
+ * @interface: board private structure to initialize
+ *
+ * Attempt to configure the interrupts using the best available
+ * capabilities of the hardware and the kernel.
+ **/
+static int fm10k_init_msix_capability(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	int v_budget, vector;
+
+	/* It's easy to be greedy for MSI-X vectors, but it really
+	 * doesn't do us much good if we have a lot more vectors
+	 * than CPU's.  So let's be conservative and only ask for
+	 * (roughly) the same number of vectors as there are CPU's.
+	 * the default is to use pairs of vectors
+	 */
+	v_budget = max(interface->num_rx_queues, interface->num_tx_queues);
+	v_budget = min_t(u16, v_budget, num_online_cpus());
+
+	/* account for vectors not related to queues */
+	v_budget += NON_Q_VECTORS(hw);
+
+	/* At the same time, hardware can only support a maximum of
+	 * hw.mac->max_msix_vectors vectors.  With features
+	 * such as RSS and VMDq, we can easily surpass the number of Rx and Tx
+	 * descriptor queues supported by our device.  Thus, we cap it off in
+	 * those rare cases where the cpu count also exceeds our vector limit.
+	 */
+	v_budget = min_t(int, v_budget, hw->mac.max_msix_vectors);
+
+	/* A failure in MSI-X entry allocation is fatal. */
+	interface->msix_entries = kcalloc(v_budget, sizeof(struct msix_entry),
+					  GFP_KERNEL);
+	if (!interface->msix_entries)
+		return -ENOMEM;
+
+	/* populate entry values */
+	for (vector = 0; vector < v_budget; vector++)
+		interface->msix_entries[vector].entry = vector;
+
+	/* Attempt to enable MSI-X with requested value */
+	v_budget = pci_enable_msix_range(interface->pdev,
+					 interface->msix_entries,
+					 MIN_MSIX_COUNT(hw),
+					 v_budget);
+	if (v_budget < 0) {
+		kfree(interface->msix_entries);
+		interface->msix_entries = NULL;
+		return -ENOMEM;
+	}
+
+	/* record the number of queues available for q_vectors */
+	interface->num_q_vectors = v_budget - NON_Q_VECTORS(hw);
+
+	return 0;
+}
+
+/**
+ * fm10k_cache_ring_qos - Descriptor ring to register mapping for QoS
+ * @interface: Interface structure continaining rings and devices
+ *
+ * Cache the descriptor ring offsets for Qos
+ **/
+static bool fm10k_cache_ring_qos(struct fm10k_intfc *interface)
+{
+	struct net_device *dev = interface->netdev;
+	int pc, offset, rss_i, i, q_idx;
+	u16 pc_stride = interface->ring_feature[RING_F_QOS].mask + 1;
+	u8 num_pcs = netdev_get_num_tc(dev);
+
+	if (num_pcs <= 1)
+		return false;
+
+	rss_i = interface->ring_feature[RING_F_RSS].indices;
+
+	for (pc = 0, offset = 0; pc < num_pcs; pc++, offset += rss_i) {
+		q_idx = pc;
+		for (i = 0; i < rss_i; i++) {
+			interface->tx_ring[offset + i]->reg_idx = q_idx;
+			interface->tx_ring[offset + i]->qos_pc = pc;
+			interface->rx_ring[offset + i]->reg_idx = q_idx;
+			interface->rx_ring[offset + i]->qos_pc = pc;
+			q_idx += pc_stride;
+		}
+	}
+
+	return true;
+}
+
+/**
+ * fm10k_cache_ring_rss - Descriptor ring to register mapping for RSS
+ * @interface: Interface structure continaining rings and devices
+ *
+ * Cache the descriptor ring offsets for RSS
+ **/
+static void fm10k_cache_ring_rss(struct fm10k_intfc *interface)
+{
+	int i;
+
+	for (i = 0; i < interface->num_rx_queues; i++)
+		interface->rx_ring[i]->reg_idx = i;
+
+	for (i = 0; i < interface->num_tx_queues; i++)
+		interface->tx_ring[i]->reg_idx = i;
+}
+
+/**
+ * fm10k_assign_rings - Map rings to network devices
+ * @interface: Interface structure containing rings and devices
+ *
+ * This function is meant to go though and configure both the network
+ * devices so that they contain rings, and configure the rings so that
+ * they function with their network devices.
+ **/
+static void fm10k_assign_rings(struct fm10k_intfc *interface)
+{
+	if (fm10k_cache_ring_qos(interface))
+		return;
+
+	fm10k_cache_ring_rss(interface);
+}
+
+static void fm10k_init_reta(struct fm10k_intfc *interface)
+{
+	u16 i, rss_i = interface->ring_feature[RING_F_RSS].indices;
+	u32 reta, base;
+
+	/* If the netdev is initialized we have to maintain table if possible */
+	if (interface->netdev->reg_state) {
+		for (i = FM10K_RETA_SIZE; i--;) {
+			reta = interface->reta[i];
+			if ((((reta << 24) >> 24) < rss_i) &&
+			    (((reta << 16) >> 24) < rss_i) &&
+			    (((reta <<  8) >> 24) < rss_i) &&
+			    (((reta)       >> 24) < rss_i))
+				continue;
+			goto repopulate_reta;
+		}
+
+		/* do nothing if all of the elements are in bounds */
+		return;
+	}
+
+repopulate_reta:
+	/* Populate the redirection table 4 entries at a time.  To do this
+	 * we are generating the results for n and n+2 and then interleaving
+	 * those with the results with n+1 and n+3.
+	 */
+	for (i = FM10K_RETA_SIZE; i--;) {
+		/* first pass generates n and n+2 */
+		base = ((i * 0x00040004) + 0x00020000) * rss_i;
+		reta = (base & 0x3F803F80) >> 7;
+
+		/* second pass generates n+1 and n+3 */
+		base += 0x00010001 * rss_i;
+		reta |= (base & 0x3F803F80) << 1;
+
+		interface->reta[i] = reta;
+	}
+}
+
+/**
+ * fm10k_init_queueing_scheme - Determine proper queueing scheme
+ * @interface: board private structure to initialize
+ *
+ * We determine which queueing scheme to use based on...
+ * - Hardware queue count (num_*_queues)
+ *   - defined by miscellaneous hardware support/features (RSS, etc.)
+ **/
+int fm10k_init_queueing_scheme(struct fm10k_intfc *interface)
+{
+	int err;
+
+	/* Number of supported queues */
+	fm10k_set_num_queues(interface);
+
+	/* Configure MSI-X capability */
+	err = fm10k_init_msix_capability(interface);
+	if (err) {
+		dev_err(&interface->pdev->dev,
+			"Unable to initialize MSI-X capability\n");
+		return err;
+	}
+
+	/* Allocate memory for queues */
+	err = fm10k_alloc_q_vectors(interface);
+	if (err)
+		return err;
+
+	/* Map rings to devices, and map devices to physical queues */
+	fm10k_assign_rings(interface);
+
+	/* Initialize RSS redirection table */
+	fm10k_init_reta(interface);
+
+	return 0;
+}
+
+/**
+ * fm10k_clear_queueing_scheme - Clear the current queueing scheme settings
+ * @interface: board private structure to clear queueing scheme on
+ *
+ * We go through and clear queueing specific resources and reset the structure
+ * to pre-load conditions
+ **/
+void fm10k_clear_queueing_scheme(struct fm10k_intfc *interface)
+{
+	fm10k_free_q_vectors(interface);
+	fm10k_reset_msix_capability(interface);
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
new file mode 100644
index 000000000000..14a4ea795c01
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.c
@@ -0,0 +1,2125 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k_common.h"
+
+/**
+ *  fm10k_fifo_init - Initialize a message FIFO
+ *  @fifo: pointer to FIFO
+ *  @buffer: pointer to memory to be used to store FIFO
+ *  @size: maximum message size to store in FIFO, must be 2^n - 1
+ **/
+static void fm10k_fifo_init(struct fm10k_mbx_fifo *fifo, u32 *buffer, u16 size)
+{
+	fifo->buffer = buffer;
+	fifo->size = size;
+	fifo->head = 0;
+	fifo->tail = 0;
+}
+
+/**
+ *  fm10k_fifo_used - Retrieve used space in FIFO
+ *  @fifo: pointer to FIFO
+ *
+ *  This function returns the number of DWORDs used in the FIFO
+ **/
+static u16 fm10k_fifo_used(struct fm10k_mbx_fifo *fifo)
+{
+	return fifo->tail - fifo->head;
+}
+
+/**
+ *  fm10k_fifo_unused - Retrieve unused space in FIFO
+ *  @fifo: pointer to FIFO
+ *
+ *  This function returns the number of unused DWORDs in the FIFO
+ **/
+static u16 fm10k_fifo_unused(struct fm10k_mbx_fifo *fifo)
+{
+	return fifo->size + fifo->head - fifo->tail;
+}
+
+/**
+ *  fm10k_fifo_empty - Test to verify if fifo is empty
+ *  @fifo: pointer to FIFO
+ *
+ *  This function returns true if the FIFO is empty, else false
+ **/
+static bool fm10k_fifo_empty(struct fm10k_mbx_fifo *fifo)
+{
+	return fifo->head == fifo->tail;
+}
+
+/**
+ *  fm10k_fifo_head_offset - returns indices of head with given offset
+ *  @fifo: pointer to FIFO
+ *  @offset: offset to add to head
+ *
+ *  This function returns the indicies into the fifo based on head + offset
+ **/
+static u16 fm10k_fifo_head_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
+{
+	return (fifo->head + offset) & (fifo->size - 1);
+}
+
+/**
+ *  fm10k_fifo_tail_offset - returns indices of tail with given offset
+ *  @fifo: pointer to FIFO
+ *  @offset: offset to add to tail
+ *
+ *  This function returns the indicies into the fifo based on tail + offset
+ **/
+static u16 fm10k_fifo_tail_offset(struct fm10k_mbx_fifo *fifo, u16 offset)
+{
+	return (fifo->tail + offset) & (fifo->size - 1);
+}
+
+/**
+ *  fm10k_fifo_head_len - Retrieve length of first message in FIFO
+ *  @fifo: pointer to FIFO
+ *
+ *  This function returns the size of the first message in the FIFO
+ **/
+static u16 fm10k_fifo_head_len(struct fm10k_mbx_fifo *fifo)
+{
+	u32 *head = fifo->buffer + fm10k_fifo_head_offset(fifo, 0);
+
+	/* verify there is at least 1 DWORD in the fifo so *head is valid */
+	if (fm10k_fifo_empty(fifo))
+		return 0;
+
+	/* retieve the message length */
+	return FM10K_TLV_DWORD_LEN(*head);
+}
+
+/**
+ *  fm10k_fifo_head_drop - Drop the first message in FIFO
+ *  @fifo: pointer to FIFO
+ *
+ *  This function returns the size of the message dropped from the FIFO
+ **/
+static u16 fm10k_fifo_head_drop(struct fm10k_mbx_fifo *fifo)
+{
+	u16 len = fm10k_fifo_head_len(fifo);
+
+	/* update head so it is at the start of next frame */
+	fifo->head += len;
+
+	return len;
+}
+
+/**
+ *  fm10k_mbx_index_len - Convert a head/tail index into a length value
+ *  @mbx: pointer to mailbox
+ *  @head: head index
+ *  @tail: head index
+ *
+ *  This function takes the head and tail index and determines the length
+ *  of the data indicated by this pair.
+ **/
+static u16 fm10k_mbx_index_len(struct fm10k_mbx_info *mbx, u16 head, u16 tail)
+{
+	u16 len = tail - head;
+
+	/* we wrapped so subtract 2, one for index 0, one for all 1s index */
+	if (len > tail)
+		len -= 2;
+
+	return len & ((mbx->mbmem_len << 1) - 1);
+}
+
+/**
+ *  fm10k_mbx_tail_add - Determine new tail value with added offset
+ *  @mbx: pointer to mailbox
+ *  @offset: length to add to head offset
+ *
+ *  This function takes the local tail index and recomputes it for
+ *  a given length added as an offset.
+ **/
+static u16 fm10k_mbx_tail_add(struct fm10k_mbx_info *mbx, u16 offset)
+{
+	u16 tail = (mbx->tail + offset + 1) & ((mbx->mbmem_len << 1) - 1);
+
+	/* add/sub 1 because we cannot have offset 0 or all 1s */
+	return (tail > mbx->tail) ? --tail : ++tail;
+}
+
+/**
+ *  fm10k_mbx_tail_sub - Determine new tail value with subtracted offset
+ *  @mbx: pointer to mailbox
+ *  @offset: length to add to head offset
+ *
+ *  This function takes the local tail index and recomputes it for
+ *  a given length added as an offset.
+ **/
+static u16 fm10k_mbx_tail_sub(struct fm10k_mbx_info *mbx, u16 offset)
+{
+	u16 tail = (mbx->tail - offset - 1) & ((mbx->mbmem_len << 1) - 1);
+
+	/* sub/add 1 because we cannot have offset 0 or all 1s */
+	return (tail < mbx->tail) ? ++tail : --tail;
+}
+
+/**
+ *  fm10k_mbx_head_add - Determine new head value with added offset
+ *  @mbx: pointer to mailbox
+ *  @offset: length to add to head offset
+ *
+ *  This function takes the local head index and recomputes it for
+ *  a given length added as an offset.
+ **/
+static u16 fm10k_mbx_head_add(struct fm10k_mbx_info *mbx, u16 offset)
+{
+	u16 head = (mbx->head + offset + 1) & ((mbx->mbmem_len << 1) - 1);
+
+	/* add/sub 1 because we cannot have offset 0 or all 1s */
+	return (head > mbx->head) ? --head : ++head;
+}
+
+/**
+ *  fm10k_mbx_head_sub - Determine new head value with subtracted offset
+ *  @mbx: pointer to mailbox
+ *  @offset: length to add to head offset
+ *
+ *  This function takes the local head index and recomputes it for
+ *  a given length added as an offset.
+ **/
+static u16 fm10k_mbx_head_sub(struct fm10k_mbx_info *mbx, u16 offset)
+{
+	u16 head = (mbx->head - offset - 1) & ((mbx->mbmem_len << 1) - 1);
+
+	/* sub/add 1 because we cannot have offset 0 or all 1s */
+	return (head < mbx->head) ? ++head : --head;
+}
+
+/**
+ *  fm10k_mbx_pushed_tail_len - Retrieve the length of message being pushed
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will return the length of the message currently being
+ *  pushed onto the tail of the Rx queue.
+ **/
+static u16 fm10k_mbx_pushed_tail_len(struct fm10k_mbx_info *mbx)
+{
+	u32 *tail = mbx->rx.buffer + fm10k_fifo_tail_offset(&mbx->rx, 0);
+
+	/* pushed tail is only valid if pushed is set */
+	if (!mbx->pushed)
+		return 0;
+
+	return FM10K_TLV_DWORD_LEN(*tail);
+}
+
+/**
+ *  fm10k_fifo_write_copy - pulls data off of msg and places it in fifo
+ *  @fifo: pointer to FIFO
+ *  @msg: message array to populate
+ *  @tail_offset: additional offset to add to tail pointer
+ *  @len: length of FIFO to copy into message header
+ *
+ *  This function will take a message and copy it into a section of the
+ *  FIFO.  In order to get something into a location other than just
+ *  the tail you can use tail_offset to adjust the pointer.
+ **/
+static void fm10k_fifo_write_copy(struct fm10k_mbx_fifo *fifo,
+				  const u32 *msg, u16 tail_offset, u16 len)
+{
+	u16 end = fm10k_fifo_tail_offset(fifo, tail_offset);
+	u32 *tail = fifo->buffer + end;
+
+	/* track when we should cross the end of the FIFO */
+	end = fifo->size - end;
+
+	/* copy end of message before start of message */
+	if (end < len)
+		memcpy(fifo->buffer, msg + end, (len - end) << 2);
+	else
+		end = len;
+
+	/* Copy remaining message into Tx FIFO */
+	memcpy(tail, msg, end << 2);
+}
+
+/**
+ *  fm10k_fifo_enqueue - Enqueues the message to the tail of the FIFO
+ *  @fifo: pointer to FIFO
+ *  @msg: message array to read
+ *
+ *  This function enqueues a message up to the size specified by the length
+ *  contained in the first DWORD of the message and will place at the tail
+ *  of the FIFO.  It will return 0 on success, or a negative value on error.
+ **/
+static s32 fm10k_fifo_enqueue(struct fm10k_mbx_fifo *fifo, const u32 *msg)
+{
+	u16 len = FM10K_TLV_DWORD_LEN(*msg);
+
+	/* verify parameters */
+	if (len > fifo->size)
+		return FM10K_MBX_ERR_SIZE;
+
+	/* verify there is room for the message */
+	if (len > fm10k_fifo_unused(fifo))
+		return FM10K_MBX_ERR_NO_SPACE;
+
+	/* Copy message into FIFO */
+	fm10k_fifo_write_copy(fifo, msg, 0, len);
+
+	/* memory barrier to guarantee FIFO is written before tail update */
+	wmb();
+
+	/* Update Tx FIFO tail */
+	fifo->tail += len;
+
+	return 0;
+}
+
+/**
+ *  fm10k_mbx_validate_msg_size - Validate incoming message based on size
+ *  @mbx: pointer to mailbox
+ *  @len: length of data pushed onto buffer
+ *
+ *  This function analyzes the frame and will return a non-zero value when
+ *  the start of a message larger than the mailbox is detected.
+ **/
+static u16 fm10k_mbx_validate_msg_size(struct fm10k_mbx_info *mbx, u16 len)
+{
+	struct fm10k_mbx_fifo *fifo = &mbx->rx;
+	u16 total_len = 0, msg_len;
+	u32 *msg;
+
+	/* length should include previous amounts pushed */
+	len += mbx->pushed;
+
+	/* offset in message is based off of current message size */
+	do {
+		msg = fifo->buffer + fm10k_fifo_tail_offset(fifo, total_len);
+		msg_len = FM10K_TLV_DWORD_LEN(*msg);
+		total_len += msg_len;
+	} while (total_len < len);
+
+	/* message extends out of pushed section, but fits in FIFO */
+	if ((len < total_len) && (msg_len <= mbx->rx.size))
+		return 0;
+
+	/* return length of invalid section */
+	return (len < total_len) ? len : (len - total_len);
+}
+
+/**
+ *  fm10k_mbx_write_copy - pulls data off of Tx FIFO and places it in mbmem
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will take a seciton of the Rx FIFO and copy it into the
+		mbx->tail--;
+ *  mailbox memory.  The offset in mbmem is based on the lower bits of the
+ *  tail and len determines the length to copy.
+ **/
+static void fm10k_mbx_write_copy(struct fm10k_hw *hw,
+				 struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_mbx_fifo *fifo = &mbx->tx;
+	u32 mbmem = mbx->mbmem_reg;
+	u32 *head = fifo->buffer;
+	u16 end, len, tail, mask;
+
+	if (!mbx->tail_len)
+		return;
+
+	/* determine data length and mbmem tail index */
+	mask = mbx->mbmem_len - 1;
+	len = mbx->tail_len;
+	tail = fm10k_mbx_tail_sub(mbx, len);
+	if (tail > mask)
+		tail++;
+
+	/* determine offset in the ring */
+	end = fm10k_fifo_head_offset(fifo, mbx->pulled);
+	head += end;
+
+	/* memory barrier to guarantee data is ready to be read */
+	rmb();
+
+	/* Copy message from Tx FIFO */
+	for (end = fifo->size - end; len; head = fifo->buffer) {
+		do {
+			/* adjust tail to match offset for FIFO */
+			tail &= mask;
+			if (!tail)
+				tail++;
+
+			/* write message to hardware FIFO */
+			fm10k_write_reg(hw, mbmem + tail++, *(head++));
+		} while (--len && --end);
+	}
+}
+
+/**
+ *  fm10k_mbx_pull_head - Pulls data off of head of Tx FIFO
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *  @head: acknowledgement number last received
+ *
+ *  This function will push the tail index forward based on the remote
+ *  head index.  It will then pull up to mbmem_len DWORDs off of the
+ *  head of the FIFO and will place it in the MBMEM registers
+ *  associated with the mailbox.
+ **/
+static void fm10k_mbx_pull_head(struct fm10k_hw *hw,
+				struct fm10k_mbx_info *mbx, u16 head)
+{
+	u16 mbmem_len, len, ack = fm10k_mbx_index_len(mbx, head, mbx->tail);
+	struct fm10k_mbx_fifo *fifo = &mbx->tx;
+
+	/* update number of bytes pulled and update bytes in transit */
+	mbx->pulled += mbx->tail_len - ack;
+
+	/* determine length of data to pull, reserve space for mbmem header */
+	mbmem_len = mbx->mbmem_len - 1;
+	len = fm10k_fifo_used(fifo) - mbx->pulled;
+	if (len > mbmem_len)
+		len = mbmem_len;
+
+	/* update tail and record number of bytes in transit */
+	mbx->tail = fm10k_mbx_tail_add(mbx, len - ack);
+	mbx->tail_len = len;
+
+	/* drop pulled messages from the FIFO */
+	for (len = fm10k_fifo_head_len(fifo);
+	     len && (mbx->pulled >= len);
+	     len = fm10k_fifo_head_len(fifo)) {
+		mbx->pulled -= fm10k_fifo_head_drop(fifo);
+		mbx->tx_messages++;
+		mbx->tx_dwords += len;
+	}
+
+	/* Copy message out from the Tx FIFO */
+	fm10k_mbx_write_copy(hw, mbx);
+}
+
+/**
+ *  fm10k_mbx_read_copy - pulls data off of mbmem and places it in Rx FIFO
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will take a seciton of the mailbox memory and copy it
+ *  into the Rx FIFO.  The offset is based on the lower bits of the
+ *  head and len determines the length to copy.
+ **/
+static void fm10k_mbx_read_copy(struct fm10k_hw *hw,
+				struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_mbx_fifo *fifo = &mbx->rx;
+	u32 mbmem = mbx->mbmem_reg ^ mbx->mbmem_len;
+	u32 *tail = fifo->buffer;
+	u16 end, len, head;
+
+	/* determine data length and mbmem head index */
+	len = mbx->head_len;
+	head = fm10k_mbx_head_sub(mbx, len);
+	if (head >= mbx->mbmem_len)
+		head++;
+
+	/* determine offset in the ring */
+	end = fm10k_fifo_tail_offset(fifo, mbx->pushed);
+	tail += end;
+
+	/* Copy message into Rx FIFO */
+	for (end = fifo->size - end; len; tail = fifo->buffer) {
+		do {
+			/* adjust head to match offset for FIFO */
+			head &= mbx->mbmem_len - 1;
+			if (!head)
+				head++;
+
+			/* read message from hardware FIFO */
+			*(tail++) = fm10k_read_reg(hw, mbmem + head++);
+		} while (--len && --end);
+	}
+
+	/* memory barrier to guarantee FIFO is written before tail update */
+	wmb();
+}
+
+/**
+ *  fm10k_mbx_push_tail - Pushes up to 15 DWORDs on to tail of FIFO
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *  @tail: tail index of message
+ *
+ *  This function will first validate the tail index and size for the
+ *  incoming message.  It then updates the acknowlegment number and
+ *  copies the data into the FIFO.  It will return the number of messages
+ *  dequeued on success and a negative value on error.
+ **/
+static s32 fm10k_mbx_push_tail(struct fm10k_hw *hw,
+			       struct fm10k_mbx_info *mbx,
+			       u16 tail)
+{
+	struct fm10k_mbx_fifo *fifo = &mbx->rx;
+	u16 len, seq = fm10k_mbx_index_len(mbx, mbx->head, tail);
+
+	/* determine length of data to push */
+	len = fm10k_fifo_unused(fifo) - mbx->pushed;
+	if (len > seq)
+		len = seq;
+
+	/* update head and record bytes received */
+	mbx->head = fm10k_mbx_head_add(mbx, len);
+	mbx->head_len = len;
+
+	/* nothing to do if there is no data */
+	if (!len)
+		return 0;
+
+	/* Copy msg into Rx FIFO */
+	fm10k_mbx_read_copy(hw, mbx);
+
+	/* determine if there are any invalid lengths in message */
+	if (fm10k_mbx_validate_msg_size(mbx, len))
+		return FM10K_MBX_ERR_SIZE;
+
+	/* Update pushed */
+	mbx->pushed += len;
+
+	/* flush any completed messages */
+	for (len = fm10k_mbx_pushed_tail_len(mbx);
+	     len && (mbx->pushed >= len);
+	     len = fm10k_mbx_pushed_tail_len(mbx)) {
+		fifo->tail += len;
+		mbx->pushed -= len;
+		mbx->rx_messages++;
+		mbx->rx_dwords += len;
+	}
+
+	return 0;
+}
+
+/* pre-generated data for generating the CRC based on the poly 0xAC9A. */
+static const u16 fm10k_crc_16b_table[256] = {
+	0x0000, 0x7956, 0xF2AC, 0x8BFA, 0xBC6D, 0xC53B, 0x4EC1, 0x3797,
+	0x21EF, 0x58B9, 0xD343, 0xAA15, 0x9D82, 0xE4D4, 0x6F2E, 0x1678,
+	0x43DE, 0x3A88, 0xB172, 0xC824, 0xFFB3, 0x86E5, 0x0D1F, 0x7449,
+	0x6231, 0x1B67, 0x909D, 0xE9CB, 0xDE5C, 0xA70A, 0x2CF0, 0x55A6,
+	0x87BC, 0xFEEA, 0x7510, 0x0C46, 0x3BD1, 0x4287, 0xC97D, 0xB02B,
+	0xA653, 0xDF05, 0x54FF, 0x2DA9, 0x1A3E, 0x6368, 0xE892, 0x91C4,
+	0xC462, 0xBD34, 0x36CE, 0x4F98, 0x780F, 0x0159, 0x8AA3, 0xF3F5,
+	0xE58D, 0x9CDB, 0x1721, 0x6E77, 0x59E0, 0x20B6, 0xAB4C, 0xD21A,
+	0x564D, 0x2F1B, 0xA4E1, 0xDDB7, 0xEA20, 0x9376, 0x188C, 0x61DA,
+	0x77A2, 0x0EF4, 0x850E, 0xFC58, 0xCBCF, 0xB299, 0x3963, 0x4035,
+	0x1593, 0x6CC5, 0xE73F, 0x9E69, 0xA9FE, 0xD0A8, 0x5B52, 0x2204,
+	0x347C, 0x4D2A, 0xC6D0, 0xBF86, 0x8811, 0xF147, 0x7ABD, 0x03EB,
+	0xD1F1, 0xA8A7, 0x235D, 0x5A0B, 0x6D9C, 0x14CA, 0x9F30, 0xE666,
+	0xF01E, 0x8948, 0x02B2, 0x7BE4, 0x4C73, 0x3525, 0xBEDF, 0xC789,
+	0x922F, 0xEB79, 0x6083, 0x19D5, 0x2E42, 0x5714, 0xDCEE, 0xA5B8,
+	0xB3C0, 0xCA96, 0x416C, 0x383A, 0x0FAD, 0x76FB, 0xFD01, 0x8457,
+	0xAC9A, 0xD5CC, 0x5E36, 0x2760, 0x10F7, 0x69A1, 0xE25B, 0x9B0D,
+	0x8D75, 0xF423, 0x7FD9, 0x068F, 0x3118, 0x484E, 0xC3B4, 0xBAE2,
+	0xEF44, 0x9612, 0x1DE8, 0x64BE, 0x5329, 0x2A7F, 0xA185, 0xD8D3,
+	0xCEAB, 0xB7FD, 0x3C07, 0x4551, 0x72C6, 0x0B90, 0x806A, 0xF93C,
+	0x2B26, 0x5270, 0xD98A, 0xA0DC, 0x974B, 0xEE1D, 0x65E7, 0x1CB1,
+	0x0AC9, 0x739F, 0xF865, 0x8133, 0xB6A4, 0xCFF2, 0x4408, 0x3D5E,
+	0x68F8, 0x11AE, 0x9A54, 0xE302, 0xD495, 0xADC3, 0x2639, 0x5F6F,
+	0x4917, 0x3041, 0xBBBB, 0xC2ED, 0xF57A, 0x8C2C, 0x07D6, 0x7E80,
+	0xFAD7, 0x8381, 0x087B, 0x712D, 0x46BA, 0x3FEC, 0xB416, 0xCD40,
+	0xDB38, 0xA26E, 0x2994, 0x50C2, 0x6755, 0x1E03, 0x95F9, 0xECAF,
+	0xB909, 0xC05F, 0x4BA5, 0x32F3, 0x0564, 0x7C32, 0xF7C8, 0x8E9E,
+	0x98E6, 0xE1B0, 0x6A4A, 0x131C, 0x248B, 0x5DDD, 0xD627, 0xAF71,
+	0x7D6B, 0x043D, 0x8FC7, 0xF691, 0xC106, 0xB850, 0x33AA, 0x4AFC,
+	0x5C84, 0x25D2, 0xAE28, 0xD77E, 0xE0E9, 0x99BF, 0x1245, 0x6B13,
+	0x3EB5, 0x47E3, 0xCC19, 0xB54F, 0x82D8, 0xFB8E, 0x7074, 0x0922,
+	0x1F5A, 0x660C, 0xEDF6, 0x94A0, 0xA337, 0xDA61, 0x519B, 0x28CD };
+
+/**
+ *  fm10k_crc_16b - Generate a 16 bit CRC for a region of 16 bit data
+ *  @data: pointer to data to process
+ *  @seed: seed value for CRC
+ *  @len: length measured in 16 bits words
+ *
+ *  This function will generate a CRC based on the polynomial 0xAC9A and
+ *  whatever value is stored in the seed variable.  Note that this
+ *  value inverts the local seed and the result in order to capture all
+ *  leading and trailing zeros.
+ */
+static u16 fm10k_crc_16b(const u32 *data, u16 seed, u16 len)
+{
+	u32 result = seed;
+
+	while (len--) {
+		result ^= *(data++);
+		result = (result >> 8) ^ fm10k_crc_16b_table[result & 0xFF];
+		result = (result >> 8) ^ fm10k_crc_16b_table[result & 0xFF];
+
+		if (!(len--))
+			break;
+
+		result = (result >> 8) ^ fm10k_crc_16b_table[result & 0xFF];
+		result = (result >> 8) ^ fm10k_crc_16b_table[result & 0xFF];
+	}
+
+	return (u16)result;
+}
+
+/**
+ *  fm10k_fifo_crc - generate a CRC based off of FIFO data
+ *  @fifo: pointer to FIFO
+ *  @offset: offset point for start of FIFO
+ *  @len: number of DWORDS words to process
+ *  @seed: seed value for CRC
+ *
+ *  This function generates a CRC for some region of the FIFO
+ **/
+static u16 fm10k_fifo_crc(struct fm10k_mbx_fifo *fifo, u16 offset,
+			  u16 len, u16 seed)
+{
+	u32 *data = fifo->buffer + offset;
+
+	/* track when we should cross the end of the FIFO */
+	offset = fifo->size - offset;
+
+	/* if we are in 2 blocks process the end of the FIFO first */
+	if (offset < len) {
+		seed = fm10k_crc_16b(data, seed, offset * 2);
+		data = fifo->buffer;
+		len -= offset;
+	}
+
+	/* process any remaining bits */
+	return fm10k_crc_16b(data, seed, len * 2);
+}
+
+/**
+ *  fm10k_mbx_update_local_crc - Update the local CRC for outgoing data
+ *  @mbx: pointer to mailbox
+ *  @head: head index provided by remote mailbox
+ *
+ *  This function will generate the CRC for all data from the end of the
+ *  last head update to the current one.  It uses the result of the
+ *  previous CRC as the seed for this update.  The result is stored in
+ *  mbx->local.
+ **/
+static void fm10k_mbx_update_local_crc(struct fm10k_mbx_info *mbx, u16 head)
+{
+	u16 len = mbx->tail_len - fm10k_mbx_index_len(mbx, head, mbx->tail);
+
+	/* determine the offset for the start of the region to be pulled */
+	head = fm10k_fifo_head_offset(&mbx->tx, mbx->pulled);
+
+	/* update local CRC to include all of the pulled data */
+	mbx->local = fm10k_fifo_crc(&mbx->tx, head, len, mbx->local);
+}
+
+/**
+ *  fm10k_mbx_verify_remote_crc - Verify the CRC is correct for current data
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will take all data that has been provided from the remote
+ *  end and generate a CRC for it.  This is stored in mbx->remote.  The
+ *  CRC for the header is then computed and if the result is non-zero this
+ *  is an error and we signal an error dropping all data and resetting the
+ *  connection.
+ */
+static s32 fm10k_mbx_verify_remote_crc(struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_mbx_fifo *fifo = &mbx->rx;
+	u16 len = mbx->head_len;
+	u16 offset = fm10k_fifo_tail_offset(fifo, mbx->pushed) - len;
+	u16 crc;
+
+	/* update the remote CRC if new data has been received */
+	if (len)
+		mbx->remote = fm10k_fifo_crc(fifo, offset, len, mbx->remote);
+
+	/* process the full header as we have to validate the CRC */
+	crc = fm10k_crc_16b(&mbx->mbx_hdr, mbx->remote, 1);
+
+	/* notify other end if we have a problem */
+	return crc ? FM10K_MBX_ERR_CRC : 0;
+}
+
+/**
+ *  fm10k_mbx_rx_ready - Indicates that a message is ready in the Rx FIFO
+ *  @mbx: pointer to mailbox
+ *
+ *  This function returns true if there is a message in the Rx FIFO to dequeue.
+ **/
+static bool fm10k_mbx_rx_ready(struct fm10k_mbx_info *mbx)
+{
+	u16 msg_size = fm10k_fifo_head_len(&mbx->rx);
+
+	return msg_size && (fm10k_fifo_used(&mbx->rx) >= msg_size);
+}
+
+/**
+ *  fm10k_mbx_tx_ready - Indicates that the mailbox is in state ready for Tx
+ *  @mbx: pointer to mailbox
+ *  @len: verify free space is >= this value
+ *
+ *  This function returns true if the mailbox is in a state ready to transmit.
+ **/
+static bool fm10k_mbx_tx_ready(struct fm10k_mbx_info *mbx, u16 len)
+{
+	u16 fifo_unused = fm10k_fifo_unused(&mbx->tx);
+
+	return (mbx->state == FM10K_STATE_OPEN) && (fifo_unused >= len);
+}
+
+/**
+ *  fm10k_mbx_tx_complete - Indicates that the Tx FIFO has been emptied
+ *  @mbx: pointer to mailbox
+ *
+ *  This function returns true if the Tx FIFO is empty.
+ **/
+static bool fm10k_mbx_tx_complete(struct fm10k_mbx_info *mbx)
+{
+	return fm10k_fifo_empty(&mbx->tx);
+}
+
+/**
+ *  fm10k_mbx_deqeueue_rx - Dequeues the message from the head in the Rx FIFO
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function dequeues messages and hands them off to the tlv parser.
+ *  It will return the number of messages processed when called.
+ **/
+static u16 fm10k_mbx_dequeue_rx(struct fm10k_hw *hw,
+				struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_mbx_fifo *fifo = &mbx->rx;
+	s32 err;
+	u16 cnt;
+
+	/* parse Rx messages out of the Rx FIFO to empty it */
+	for (cnt = 0; !fm10k_fifo_empty(fifo); cnt++) {
+		err = fm10k_tlv_msg_parse(hw, fifo->buffer + fifo->head,
+					  mbx, mbx->msg_data);
+		if (err < 0)
+			mbx->rx_parse_err++;
+
+		fm10k_fifo_head_drop(fifo);
+	}
+
+	/* shift remaining bytes back to start of FIFO */
+	memmove(fifo->buffer, fifo->buffer + fifo->tail, mbx->pushed << 2);
+
+	/* shift head and tail based on the memory we moved */
+	fifo->tail -= fifo->head;
+	fifo->head = 0;
+
+	return cnt;
+}
+
+/**
+ *  fm10k_mbx_enqueue_tx - Enqueues the message to the tail of the Tx FIFO
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *  @msg: message array to read
+ *
+ *  This function enqueues a message up to the size specified by the length
+ *  contained in the first DWORD of the message and will place at the tail
+ *  of the FIFO.  It will return 0 on success, or a negative value on error.
+ **/
+static s32 fm10k_mbx_enqueue_tx(struct fm10k_hw *hw,
+				struct fm10k_mbx_info *mbx, const u32 *msg)
+{
+	u32 countdown = mbx->timeout;
+	s32 err;
+
+	switch (mbx->state) {
+	case FM10K_STATE_CLOSED:
+	case FM10K_STATE_DISCONNECT:
+		return FM10K_MBX_ERR_NO_MBX;
+	default:
+		break;
+	}
+
+	/* enqueue the message on the Tx FIFO */
+	err = fm10k_fifo_enqueue(&mbx->tx, msg);
+
+	/* if it failed give the FIFO a chance to drain */
+	while (err && countdown) {
+		countdown--;
+		udelay(mbx->udelay);
+		mbx->ops.process(hw, mbx);
+		err = fm10k_fifo_enqueue(&mbx->tx, msg);
+	}
+
+	/* if we failed trhead the error */
+	if (err) {
+		mbx->timeout = 0;
+		mbx->tx_busy++;
+	}
+
+	/* begin processing message, ignore errors as this is just meant
+	 * to start the mailbox flow so we are not concerned if there
+	 * is a bad error, or the mailbox is already busy with a request
+	 */
+	if (!mbx->tail_len)
+		mbx->ops.process(hw, mbx);
+
+	return 0;
+}
+
+/**
+ *  fm10k_mbx_read - Copies the mbmem to local message buffer
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function copies the message from the mbmem to the message array
+ **/
+static s32 fm10k_mbx_read(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx)
+{
+	/* only allow one reader in here at a time */
+	if (mbx->mbx_hdr)
+		return FM10K_MBX_ERR_BUSY;
+
+	/* read to capture initial interrupt bits */
+	if (fm10k_read_reg(hw, mbx->mbx_reg) & FM10K_MBX_REQ_INTERRUPT)
+		mbx->mbx_lock = FM10K_MBX_ACK;
+
+	/* write back interrupt bits to clear */
+	fm10k_write_reg(hw, mbx->mbx_reg,
+			FM10K_MBX_REQ_INTERRUPT | FM10K_MBX_ACK_INTERRUPT);
+
+	/* read remote header */
+	mbx->mbx_hdr = fm10k_read_reg(hw, mbx->mbmem_reg ^ mbx->mbmem_len);
+
+	return 0;
+}
+
+/**
+ *  fm10k_mbx_write - Copies the local message buffer to mbmem
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function copies the message from the the message array to mbmem
+ **/
+static void fm10k_mbx_write(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx)
+{
+	u32 mbmem = mbx->mbmem_reg;
+
+	/* write new msg header to notify recepient of change */
+	fm10k_write_reg(hw, mbmem, mbx->mbx_hdr);
+
+	/* write mailbox to sent interrupt */
+	if (mbx->mbx_lock)
+		fm10k_write_reg(hw, mbx->mbx_reg, mbx->mbx_lock);
+
+	/* we no longer are using the header so free it */
+	mbx->mbx_hdr = 0;
+	mbx->mbx_lock = 0;
+}
+
+/**
+ *  fm10k_mbx_create_connect_hdr - Generate a connect mailbox header
+ *  @mbx: pointer to mailbox
+ *
+ *  This function returns a connection mailbox header
+ **/
+static void fm10k_mbx_create_connect_hdr(struct fm10k_mbx_info *mbx)
+{
+	mbx->mbx_lock |= FM10K_MBX_REQ;
+
+	mbx->mbx_hdr = FM10K_MSG_HDR_FIELD_SET(FM10K_MSG_CONNECT, TYPE) |
+		       FM10K_MSG_HDR_FIELD_SET(mbx->head, HEAD) |
+		       FM10K_MSG_HDR_FIELD_SET(mbx->rx.size - 1, CONNECT_SIZE);
+}
+
+/**
+ *  fm10k_mbx_create_data_hdr - Generate a data mailbox header
+ *  @mbx: pointer to mailbox
+ *
+ *  This function returns a data mailbox header
+ **/
+static void fm10k_mbx_create_data_hdr(struct fm10k_mbx_info *mbx)
+{
+	u32 hdr = FM10K_MSG_HDR_FIELD_SET(FM10K_MSG_DATA, TYPE) |
+		  FM10K_MSG_HDR_FIELD_SET(mbx->tail, TAIL) |
+		  FM10K_MSG_HDR_FIELD_SET(mbx->head, HEAD);
+	struct fm10k_mbx_fifo *fifo = &mbx->tx;
+	u16 crc;
+
+	if (mbx->tail_len)
+		mbx->mbx_lock |= FM10K_MBX_REQ;
+
+	/* generate CRC for data in flight and header */
+	crc = fm10k_fifo_crc(fifo, fm10k_fifo_head_offset(fifo, mbx->pulled),
+			     mbx->tail_len, mbx->local);
+	crc = fm10k_crc_16b(&hdr, crc, 1);
+
+	/* load header to memory to be written */
+	mbx->mbx_hdr = hdr | FM10K_MSG_HDR_FIELD_SET(crc, CRC);
+}
+
+/**
+ *  fm10k_mbx_create_disconnect_hdr - Generate a disconnect mailbox header
+ *  @mbx: pointer to mailbox
+ *
+ *  This function returns a disconnect mailbox header
+ **/
+static void fm10k_mbx_create_disconnect_hdr(struct fm10k_mbx_info *mbx)
+{
+	u32 hdr = FM10K_MSG_HDR_FIELD_SET(FM10K_MSG_DISCONNECT, TYPE) |
+		  FM10K_MSG_HDR_FIELD_SET(mbx->tail, TAIL) |
+		  FM10K_MSG_HDR_FIELD_SET(mbx->head, HEAD);
+	u16 crc = fm10k_crc_16b(&hdr, mbx->local, 1);
+
+	mbx->mbx_lock |= FM10K_MBX_ACK;
+
+	/* load header to memory to be written */
+	mbx->mbx_hdr = hdr | FM10K_MSG_HDR_FIELD_SET(crc, CRC);
+}
+
+/**
+ *  fm10k_mbx_create_error_msg - Generate a error message
+ *  @mbx: pointer to mailbox
+ *  @err: local error encountered
+ *
+ *  This function will interpret the error provided by err, and based on
+ *  that it may shift the message by 1 DWORD and then place an error header
+ *  at the start of the message.
+ **/
+static void fm10k_mbx_create_error_msg(struct fm10k_mbx_info *mbx, s32 err)
+{
+	/* only generate an error message for these types */
+	switch (err) {
+	case FM10K_MBX_ERR_TAIL:
+	case FM10K_MBX_ERR_HEAD:
+	case FM10K_MBX_ERR_TYPE:
+	case FM10K_MBX_ERR_SIZE:
+	case FM10K_MBX_ERR_RSVD0:
+	case FM10K_MBX_ERR_CRC:
+		break;
+	default:
+		return;
+	}
+
+	mbx->mbx_lock |= FM10K_MBX_REQ;
+
+	mbx->mbx_hdr = FM10K_MSG_HDR_FIELD_SET(FM10K_MSG_ERROR, TYPE) |
+		       FM10K_MSG_HDR_FIELD_SET(err, ERR_NO) |
+		       FM10K_MSG_HDR_FIELD_SET(mbx->head, HEAD);
+}
+
+/**
+ *  fm10k_mbx_validate_msg_hdr - Validate common fields in the message header
+ *  @mbx: pointer to mailbox
+ *  @msg: message array to read
+ *
+ *  This function will parse up the fields in the mailbox header and return
+ *  an error if the header contains any of a number of invalid configurations
+ *  including unrecognized type, invalid route, or a malformed message.
+ **/
+static s32 fm10k_mbx_validate_msg_hdr(struct fm10k_mbx_info *mbx)
+{
+	u16 type, rsvd0, head, tail, size;
+	const u32 *hdr = &mbx->mbx_hdr;
+
+	type = FM10K_MSG_HDR_FIELD_GET(*hdr, TYPE);
+	rsvd0 = FM10K_MSG_HDR_FIELD_GET(*hdr, RSVD0);
+	tail = FM10K_MSG_HDR_FIELD_GET(*hdr, TAIL);
+	head = FM10K_MSG_HDR_FIELD_GET(*hdr, HEAD);
+	size = FM10K_MSG_HDR_FIELD_GET(*hdr, CONNECT_SIZE);
+
+	if (rsvd0)
+		return FM10K_MBX_ERR_RSVD0;
+
+	switch (type) {
+	case FM10K_MSG_DISCONNECT:
+		/* validate that all data has been received */
+		if (tail != mbx->head)
+			return FM10K_MBX_ERR_TAIL;
+
+		/* fall through */
+	case FM10K_MSG_DATA:
+		/* validate that head is moving correctly */
+		if (!head || (head == FM10K_MSG_HDR_MASK(HEAD)))
+			return FM10K_MBX_ERR_HEAD;
+		if (fm10k_mbx_index_len(mbx, head, mbx->tail) > mbx->tail_len)
+			return FM10K_MBX_ERR_HEAD;
+
+		/* validate that tail is moving correctly */
+		if (!tail || (tail == FM10K_MSG_HDR_MASK(TAIL)))
+			return FM10K_MBX_ERR_TAIL;
+		if (fm10k_mbx_index_len(mbx, mbx->head, tail) < mbx->mbmem_len)
+			break;
+
+		return FM10K_MBX_ERR_TAIL;
+	case FM10K_MSG_CONNECT:
+		/* validate size is in range and is power of 2 mask */
+		if ((size < FM10K_VFMBX_MSG_MTU) || (size & (size + 1)))
+			return FM10K_MBX_ERR_SIZE;
+
+		/* fall through */
+	case FM10K_MSG_ERROR:
+		if (!head || (head == FM10K_MSG_HDR_MASK(HEAD)))
+			return FM10K_MBX_ERR_HEAD;
+		/* neither create nor error include a tail offset */
+		if (tail)
+			return FM10K_MBX_ERR_TAIL;
+
+		break;
+	default:
+		return FM10K_MBX_ERR_TYPE;
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_mbx_create_reply - Generate reply based on state and remote head
+ *  @mbx: pointer to mailbox
+ *  @head: acknowledgement number
+ *
+ *  This function will generate an outgoing message based on the current
+ *  mailbox state and the remote fifo head.  It will return the length
+ *  of the outgoing message excluding header on success, and a negative value
+ *  on error.
+ **/
+static s32 fm10k_mbx_create_reply(struct fm10k_hw *hw,
+				  struct fm10k_mbx_info *mbx, u16 head)
+{
+	switch (mbx->state) {
+	case FM10K_STATE_OPEN:
+	case FM10K_STATE_DISCONNECT:
+		/* update our checksum for the outgoing data */
+		fm10k_mbx_update_local_crc(mbx, head);
+
+		/* as long as other end recognizes us keep sending data */
+		fm10k_mbx_pull_head(hw, mbx, head);
+
+		/* generate new header based on data */
+		if (mbx->tail_len || (mbx->state == FM10K_STATE_OPEN))
+			fm10k_mbx_create_data_hdr(mbx);
+		else
+			fm10k_mbx_create_disconnect_hdr(mbx);
+		break;
+	case FM10K_STATE_CONNECT:
+		/* send disconnect even if we aren't connected */
+		fm10k_mbx_create_connect_hdr(mbx);
+		break;
+	case FM10K_STATE_CLOSED:
+		/* generate new header based on data */
+		fm10k_mbx_create_disconnect_hdr(mbx);
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_mbx_reset_work- Reset internal pointers for any pending work
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will reset all internal pointers so any work in progress
+ *  is dropped.  This call should occur every time we transition from the
+ *  open state to the connect state.
+ **/
+static void fm10k_mbx_reset_work(struct fm10k_mbx_info *mbx)
+{
+	/* reset our outgoing max size back to Rx limits */
+	mbx->max_size = mbx->rx.size - 1;
+
+	/* just do a quick resysnc to start of message */
+	mbx->pushed = 0;
+	mbx->pulled = 0;
+	mbx->tail_len = 0;
+	mbx->head_len = 0;
+	mbx->rx.tail = 0;
+	mbx->rx.head = 0;
+}
+
+/**
+ *  fm10k_mbx_update_max_size - Update the max_size and drop any large messages
+ *  @mbx: pointer to mailbox
+ *  @size: new value for max_size
+ *
+ *  This function will update the max_size value and drop any outgoing messages
+ *  from the head of the Tx FIFO that are larger than max_size.
+ **/
+static void fm10k_mbx_update_max_size(struct fm10k_mbx_info *mbx, u16 size)
+{
+	u16 len;
+
+	mbx->max_size = size;
+
+	/* flush any oversized messages from the queue */
+	for (len = fm10k_fifo_head_len(&mbx->tx);
+	     len > size;
+	     len = fm10k_fifo_head_len(&mbx->tx)) {
+		fm10k_fifo_head_drop(&mbx->tx);
+		mbx->tx_dropped++;
+	}
+}
+
+/**
+ *  fm10k_mbx_connect_reset - Reset following request for reset
+ *  @mbx: pointer to mailbox
+ *
+ *  This function resets the mailbox to either a disconnected state
+ *  or a connect state depending on the current mailbox state
+ **/
+static void fm10k_mbx_connect_reset(struct fm10k_mbx_info *mbx)
+{
+	/* just do a quick resysnc to start of frame */
+	fm10k_mbx_reset_work(mbx);
+
+	/* reset CRC seeds */
+	mbx->local = FM10K_MBX_CRC_SEED;
+	mbx->remote = FM10K_MBX_CRC_SEED;
+
+	/* we cannot exit connect until the size is good */
+	if (mbx->state == FM10K_STATE_OPEN)
+		mbx->state = FM10K_STATE_CONNECT;
+	else
+		mbx->state = FM10K_STATE_CLOSED;
+}
+
+/**
+ *  fm10k_mbx_process_connect - Process connect header
+ *  @mbx: pointer to mailbox
+ *  @msg: message array to process
+ *
+ *  This function will read an incoming connect header and reply with the
+ *  appropriate message.  It will return a value indicating the number of
+ *  data DWORDs on success, or will return a negative value on failure.
+ **/
+static s32 fm10k_mbx_process_connect(struct fm10k_hw *hw,
+				     struct fm10k_mbx_info *mbx)
+{
+	const enum fm10k_mbx_state state = mbx->state;
+	const u32 *hdr = &mbx->mbx_hdr;
+	u16 size, head;
+
+	/* we will need to pull all of the fields for verification */
+	size = FM10K_MSG_HDR_FIELD_GET(*hdr, CONNECT_SIZE);
+	head = FM10K_MSG_HDR_FIELD_GET(*hdr, HEAD);
+
+	switch (state) {
+	case FM10K_STATE_DISCONNECT:
+	case FM10K_STATE_OPEN:
+		/* reset any in-progress work */
+		fm10k_mbx_connect_reset(mbx);
+		break;
+	case FM10K_STATE_CONNECT:
+		/* we cannot exit connect until the size is good */
+		if (size > mbx->rx.size) {
+			mbx->max_size = mbx->rx.size - 1;
+		} else {
+			/* record the remote system requesting connection */
+			mbx->state = FM10K_STATE_OPEN;
+
+			fm10k_mbx_update_max_size(mbx, size);
+		}
+		break;
+	default:
+		break;
+	}
+
+	/* align our tail index to remote head index */
+	mbx->tail = head;
+
+	return fm10k_mbx_create_reply(hw, mbx, head);
+}
+
+/**
+ *  fm10k_mbx_process_data - Process data header
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will read an incoming data header and reply with the
+ *  appropriate message.  It will return a value indicating the number of
+ *  data DWORDs on success, or will return a negative value on failure.
+ **/
+static s32 fm10k_mbx_process_data(struct fm10k_hw *hw,
+				  struct fm10k_mbx_info *mbx)
+{
+	const u32 *hdr = &mbx->mbx_hdr;
+	u16 head, tail;
+	s32 err;
+
+	/* we will need to pull all of the fields for verification */
+	head = FM10K_MSG_HDR_FIELD_GET(*hdr, HEAD);
+	tail = FM10K_MSG_HDR_FIELD_GET(*hdr, TAIL);
+
+	/* if we are in connect just update our data and go */
+	if (mbx->state == FM10K_STATE_CONNECT) {
+		mbx->tail = head;
+		mbx->state = FM10K_STATE_OPEN;
+	}
+
+	/* abort on message size errors */
+	err = fm10k_mbx_push_tail(hw, mbx, tail);
+	if (err < 0)
+		return err;
+
+	/* verify the checksum on the incoming data */
+	err = fm10k_mbx_verify_remote_crc(mbx);
+	if (err)
+		return err;
+
+	/* process messages if we have received any */
+	fm10k_mbx_dequeue_rx(hw, mbx);
+
+	return fm10k_mbx_create_reply(hw, mbx, head);
+}
+
+/**
+ *  fm10k_mbx_process_disconnect - Process disconnect header
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will read an incoming disconnect header and reply with the
+ *  appropriate message.  It will return a value indicating the number of
+ *  data DWORDs on success, or will return a negative value on failure.
+ **/
+static s32 fm10k_mbx_process_disconnect(struct fm10k_hw *hw,
+					struct fm10k_mbx_info *mbx)
+{
+	const enum fm10k_mbx_state state = mbx->state;
+	const u32 *hdr = &mbx->mbx_hdr;
+	u16 head, tail;
+	s32 err;
+
+	/* we will need to pull all of the fields for verification */
+	head = FM10K_MSG_HDR_FIELD_GET(*hdr, HEAD);
+	tail = FM10K_MSG_HDR_FIELD_GET(*hdr, TAIL);
+
+	/* We should not be receiving disconnect if Rx is incomplete */
+	if (mbx->pushed)
+		return FM10K_MBX_ERR_TAIL;
+
+	/* we have already verified mbx->head == tail so we know this is 0 */
+	mbx->head_len = 0;
+
+	/* verify the checksum on the incoming header is correct */
+	err = fm10k_mbx_verify_remote_crc(mbx);
+	if (err)
+		return err;
+
+	switch (state) {
+	case FM10K_STATE_DISCONNECT:
+	case FM10K_STATE_OPEN:
+		/* state doesn't change if we still have work to do */
+		if (!fm10k_mbx_tx_complete(mbx))
+			break;
+
+		/* verify the head indicates we completed all transmits */
+		if (head != mbx->tail)
+			return FM10K_MBX_ERR_HEAD;
+
+		/* reset any in-progress work */
+		fm10k_mbx_connect_reset(mbx);
+		break;
+	default:
+		break;
+	}
+
+	return fm10k_mbx_create_reply(hw, mbx, head);
+}
+
+/**
+ *  fm10k_mbx_process_error - Process error header
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will read an incoming error header and reply with the
+ *  appropriate message.  It will return a value indicating the number of
+ *  data DWORDs on success, or will return a negative value on failure.
+ **/
+static s32 fm10k_mbx_process_error(struct fm10k_hw *hw,
+				   struct fm10k_mbx_info *mbx)
+{
+	const u32 *hdr = &mbx->mbx_hdr;
+	s32 err_no;
+	u16 head;
+
+	/* we will need to pull all of the fields for verification */
+	head = FM10K_MSG_HDR_FIELD_GET(*hdr, HEAD);
+
+	/* we only have lower 10 bits of error number os add upper bits */
+	err_no = FM10K_MSG_HDR_FIELD_GET(*hdr, ERR_NO);
+	err_no |= ~FM10K_MSG_HDR_MASK(ERR_NO);
+
+	switch (mbx->state) {
+	case FM10K_STATE_OPEN:
+	case FM10K_STATE_DISCONNECT:
+		/* flush any uncompleted work */
+		fm10k_mbx_reset_work(mbx);
+
+		/* reset CRC seeds */
+		mbx->local = FM10K_MBX_CRC_SEED;
+		mbx->remote = FM10K_MBX_CRC_SEED;
+
+		/* reset tail index and size to prepare for reconnect */
+		mbx->tail = head;
+
+		/* if open then reset max_size and go back to connect */
+		if (mbx->state == FM10K_STATE_OPEN) {
+			mbx->state = FM10K_STATE_CONNECT;
+			break;
+		}
+
+		/* send a connect message to get data flowing again */
+		fm10k_mbx_create_connect_hdr(mbx);
+		return 0;
+	default:
+		break;
+	}
+
+	return fm10k_mbx_create_reply(hw, mbx, mbx->tail);
+}
+
+/**
+ *  fm10k_mbx_process - Process mailbox interrupt
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will process incoming mailbox events and generate mailbox
+ *  replies.  It will return a value indicating the number of DWORDs
+ *  transmitted excluding header on success or a negative value on error.
+ **/
+static s32 fm10k_mbx_process(struct fm10k_hw *hw,
+			     struct fm10k_mbx_info *mbx)
+{
+	s32 err;
+
+	/* we do not read mailbox if closed */
+	if (mbx->state == FM10K_STATE_CLOSED)
+		return 0;
+
+	/* copy data from mailbox */
+	err = fm10k_mbx_read(hw, mbx);
+	if (err)
+		return err;
+
+	/* validate type, source, and destination */
+	err = fm10k_mbx_validate_msg_hdr(mbx);
+	if (err < 0)
+		goto msg_err;
+
+	switch (FM10K_MSG_HDR_FIELD_GET(mbx->mbx_hdr, TYPE)) {
+	case FM10K_MSG_CONNECT:
+		err = fm10k_mbx_process_connect(hw, mbx);
+		break;
+	case FM10K_MSG_DATA:
+		err = fm10k_mbx_process_data(hw, mbx);
+		break;
+	case FM10K_MSG_DISCONNECT:
+		err = fm10k_mbx_process_disconnect(hw, mbx);
+		break;
+	case FM10K_MSG_ERROR:
+		err = fm10k_mbx_process_error(hw, mbx);
+		break;
+	default:
+		err = FM10K_MBX_ERR_TYPE;
+		break;
+	}
+
+msg_err:
+	/* notify partner of errors on our end */
+	if (err < 0)
+		fm10k_mbx_create_error_msg(mbx, err);
+
+	/* copy data from mailbox */
+	fm10k_mbx_write(hw, mbx);
+
+	return err;
+}
+
+/**
+ *  fm10k_mbx_disconnect - Shutdown mailbox connection
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will shut down the mailbox.  It places the mailbox first
+ *  in the disconnect state, it then allows up to a predefined timeout for
+ *  the mailbox to transition to close on its own.  If this does not occur
+ *  then the mailbox will be forced into the closed state.
+ *
+ *  Any mailbox transactions not completed before calling this function
+ *  are not guaranteed to complete and may be dropped.
+ **/
+static void fm10k_mbx_disconnect(struct fm10k_hw *hw,
+				 struct fm10k_mbx_info *mbx)
+{
+	int timeout = mbx->timeout ? FM10K_MBX_DISCONNECT_TIMEOUT : 0;
+
+	/* Place mbx in ready to disconnect state */
+	mbx->state = FM10K_STATE_DISCONNECT;
+
+	/* trigger interrupt to start shutdown process */
+	fm10k_write_reg(hw, mbx->mbx_reg, FM10K_MBX_REQ |
+					  FM10K_MBX_INTERRUPT_DISABLE);
+	do {
+		udelay(FM10K_MBX_POLL_DELAY);
+		mbx->ops.process(hw, mbx);
+		timeout -= FM10K_MBX_POLL_DELAY;
+	} while ((timeout > 0) && (mbx->state != FM10K_STATE_CLOSED));
+
+	/* in case we didn't close just force the mailbox into shutdown */
+	fm10k_mbx_connect_reset(mbx);
+	fm10k_mbx_update_max_size(mbx, 0);
+
+	fm10k_write_reg(hw, mbx->mbmem_reg, 0);
+}
+
+/**
+ *  fm10k_mbx_connect - Start mailbox connection
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will initiate a mailbox connection.  It will populate the
+ *  mailbox with a broadcast connect message and then initialize the lock.
+ *  This is safe since the connect message is a single DWORD so the mailbox
+ *  transaction is guaranteed to be atomic.
+ *
+ *  This function will return an error if the mailbox has not been initiated
+ *  or is currently in use.
+ **/
+static s32 fm10k_mbx_connect(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx)
+{
+	/* we cannot connect an uninitialized mailbox */
+	if (!mbx->rx.buffer)
+		return FM10K_MBX_ERR_NO_SPACE;
+
+	/* we cannot connect an already connected mailbox */
+	if (mbx->state != FM10K_STATE_CLOSED)
+		return FM10K_MBX_ERR_BUSY;
+
+	/* mailbox timeout can now become active */
+	mbx->timeout = FM10K_MBX_INIT_TIMEOUT;
+
+	/* Place mbx in ready to connect state */
+	mbx->state = FM10K_STATE_CONNECT;
+
+	/* initialize header of remote mailbox */
+	fm10k_mbx_create_disconnect_hdr(mbx);
+	fm10k_write_reg(hw, mbx->mbmem_reg ^ mbx->mbmem_len, mbx->mbx_hdr);
+
+	/* enable interrupt and notify other party of new message */
+	mbx->mbx_lock = FM10K_MBX_REQ_INTERRUPT | FM10K_MBX_ACK_INTERRUPT |
+			FM10K_MBX_INTERRUPT_ENABLE;
+
+	/* generate and load connect header into mailbox */
+	fm10k_mbx_create_connect_hdr(mbx);
+	fm10k_mbx_write(hw, mbx);
+
+	return 0;
+}
+
+/**
+ *  fm10k_mbx_validate_handlers - Validate layout of message parsing data
+ *  @msg_data: handlers for mailbox events
+ *
+ *  This function validates the layout of the message parsing data.  This
+ *  should be mostly static, but it is important to catch any errors that
+ *  are made when constructing the parsers.
+ **/
+static s32 fm10k_mbx_validate_handlers(const struct fm10k_msg_data *msg_data)
+{
+	const struct fm10k_tlv_attr *attr;
+	unsigned int id;
+
+	/* Allow NULL mailboxes that transmit but don't receive */
+	if (!msg_data)
+		return 0;
+
+	while (msg_data->id != FM10K_TLV_ERROR) {
+		/* all messages should have a function handler */
+		if (!msg_data->func)
+			return FM10K_ERR_PARAM;
+
+		/* parser is optional */
+		attr = msg_data->attr;
+		if (attr) {
+			while (attr->id != FM10K_TLV_ERROR) {
+				id = attr->id;
+				attr++;
+				/* ID should always be increasing */
+				if (id >= attr->id)
+					return FM10K_ERR_PARAM;
+				/* ID should fit in results array */
+				if (id >= FM10K_TLV_RESULTS_MAX)
+					return FM10K_ERR_PARAM;
+			}
+
+			/* verify terminator is in the list */
+			if (attr->id != FM10K_TLV_ERROR)
+				return FM10K_ERR_PARAM;
+		}
+
+		id = msg_data->id;
+		msg_data++;
+		/* ID should always be increasing */
+		if (id >= msg_data->id)
+			return FM10K_ERR_PARAM;
+	}
+
+	/* verify terminator is in the list */
+	if ((msg_data->id != FM10K_TLV_ERROR) || !msg_data->func)
+		return FM10K_ERR_PARAM;
+
+	return 0;
+}
+
+/**
+ *  fm10k_mbx_register_handlers - Register a set of handler ops for mailbox
+ *  @mbx: pointer to mailbox
+ *  @msg_data: handlers for mailbox events
+ *
+ *  This function associates a set of message handling ops with a mailbox.
+ **/
+static s32 fm10k_mbx_register_handlers(struct fm10k_mbx_info *mbx,
+				       const struct fm10k_msg_data *msg_data)
+{
+	/* validate layout of handlers before assigning them */
+	if (fm10k_mbx_validate_handlers(msg_data))
+		return FM10K_ERR_PARAM;
+
+	/* initialize the message handlers */
+	mbx->msg_data = msg_data;
+
+	return 0;
+}
+
+/**
+ *  fm10k_pfvf_mbx_init - Initialize mailbox memory for PF/VF mailbox
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *  @msg_data: handlers for mailbox events
+ *  @id: ID reference for PF as it supports up to 64 PF/VF mailboxes
+ *
+ *  This function initializes the mailbox for use.  It will split the
+ *  buffer provided an use that th populate both the Tx and Rx FIFO by
+ *  evenly splitting it.  In order to allow for easy masking of head/tail
+ *  the value reported in size must be a power of 2 and is reported in
+ *  DWORDs, not bytes.  Any invalid values will cause the mailbox to return
+ *  error.
+ **/
+s32 fm10k_pfvf_mbx_init(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx,
+			const struct fm10k_msg_data *msg_data, u8 id)
+{
+	/* initialize registers */
+	switch (hw->mac.type) {
+	case fm10k_mac_vf:
+		mbx->mbx_reg = FM10K_VFMBX;
+		mbx->mbmem_reg = FM10K_VFMBMEM(FM10K_VFMBMEM_VF_XOR);
+		break;
+	case fm10k_mac_pf:
+		/* there are only 64 VF <-> PF mailboxes */
+		if (id < 64) {
+			mbx->mbx_reg = FM10K_MBX(id);
+			mbx->mbmem_reg = FM10K_MBMEM_VF(id, 0);
+			break;
+		}
+		/* fallthough */
+	default:
+		return FM10K_MBX_ERR_NO_MBX;
+	}
+
+	/* start out in closed state */
+	mbx->state = FM10K_STATE_CLOSED;
+
+	/* validate layout of handlers before assigning them */
+	if (fm10k_mbx_validate_handlers(msg_data))
+		return FM10K_ERR_PARAM;
+
+	/* initialize the message handlers */
+	mbx->msg_data = msg_data;
+
+	/* start mailbox as timed out and let the reset_hw call
+	 * set the timeout value to begin communications
+	 */
+	mbx->timeout = 0;
+	mbx->udelay = FM10K_MBX_INIT_DELAY;
+
+	/* initalize tail and head */
+	mbx->tail = 1;
+	mbx->head = 1;
+
+	/* initialize CRC seeds */
+	mbx->local = FM10K_MBX_CRC_SEED;
+	mbx->remote = FM10K_MBX_CRC_SEED;
+
+	/* Split buffer for use by Tx/Rx FIFOs */
+	mbx->max_size = FM10K_MBX_MSG_MAX_SIZE;
+	mbx->mbmem_len = FM10K_VFMBMEM_VF_XOR;
+
+	/* initialize the FIFOs, sizes are in 4 byte increments */
+	fm10k_fifo_init(&mbx->tx, mbx->buffer, FM10K_MBX_TX_BUFFER_SIZE);
+	fm10k_fifo_init(&mbx->rx, &mbx->buffer[FM10K_MBX_TX_BUFFER_SIZE],
+			FM10K_MBX_RX_BUFFER_SIZE);
+
+	/* initialize function pointers */
+	mbx->ops.connect = fm10k_mbx_connect;
+	mbx->ops.disconnect = fm10k_mbx_disconnect;
+	mbx->ops.rx_ready = fm10k_mbx_rx_ready;
+	mbx->ops.tx_ready = fm10k_mbx_tx_ready;
+	mbx->ops.tx_complete = fm10k_mbx_tx_complete;
+	mbx->ops.enqueue_tx = fm10k_mbx_enqueue_tx;
+	mbx->ops.process = fm10k_mbx_process;
+	mbx->ops.register_handlers = fm10k_mbx_register_handlers;
+
+	return 0;
+}
+
+/**
+ *  fm10k_sm_mbx_create_data_hdr - Generate a mailbox header for local FIFO
+ *  @mbx: pointer to mailbox
+ *
+ *  This function returns a connection mailbox header
+ **/
+static void fm10k_sm_mbx_create_data_hdr(struct fm10k_mbx_info *mbx)
+{
+	if (mbx->tail_len)
+		mbx->mbx_lock |= FM10K_MBX_REQ;
+
+	mbx->mbx_hdr = FM10K_MSG_HDR_FIELD_SET(mbx->tail, SM_TAIL) |
+		       FM10K_MSG_HDR_FIELD_SET(mbx->remote, SM_VER) |
+		       FM10K_MSG_HDR_FIELD_SET(mbx->head, SM_HEAD);
+}
+
+/**
+ *  fm10k_sm_mbx_create_connect_hdr - Generate a mailbox header for local FIFO
+ *  @mbx: pointer to mailbox
+ *  @err: error flags to report if any
+ *
+ *  This function returns a connection mailbox header
+ **/
+static void fm10k_sm_mbx_create_connect_hdr(struct fm10k_mbx_info *mbx, u8 err)
+{
+	if (mbx->local)
+		mbx->mbx_lock |= FM10K_MBX_REQ;
+
+	mbx->mbx_hdr = FM10K_MSG_HDR_FIELD_SET(mbx->tail, SM_TAIL) |
+		       FM10K_MSG_HDR_FIELD_SET(mbx->remote, SM_VER) |
+		       FM10K_MSG_HDR_FIELD_SET(mbx->head, SM_HEAD) |
+		       FM10K_MSG_HDR_FIELD_SET(err, SM_ERR);
+}
+
+/**
+ *  fm10k_sm_mbx_connect_reset - Reset following request for reset
+ *  @mbx: pointer to mailbox
+ *
+ *  This function resets the mailbox to a just connected state
+ **/
+static void fm10k_sm_mbx_connect_reset(struct fm10k_mbx_info *mbx)
+{
+	/* flush any uncompleted work */
+	fm10k_mbx_reset_work(mbx);
+
+	/* set local version to max and remote version to 0 */
+	mbx->local = FM10K_SM_MBX_VERSION;
+	mbx->remote = 0;
+
+	/* initalize tail and head */
+	mbx->tail = 1;
+	mbx->head = 1;
+
+	/* reset state back to connect */
+	mbx->state = FM10K_STATE_CONNECT;
+}
+
+/**
+ *  fm10k_sm_mbx_connect - Start switch manager mailbox connection
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will initiate a mailbox connection with the switch
+ *  manager.  To do this it will first disconnect the mailbox, and then
+ *  reconnect it in order to complete a reset of the mailbox.
+ *
+ *  This function will return an error if the mailbox has not been initiated
+ *  or is currently in use.
+ **/
+static s32 fm10k_sm_mbx_connect(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx)
+{
+	/* we cannot connect an uninitialized mailbox */
+	if (!mbx->rx.buffer)
+		return FM10K_MBX_ERR_NO_SPACE;
+
+	/* we cannot connect an already connected mailbox */
+	if (mbx->state != FM10K_STATE_CLOSED)
+		return FM10K_MBX_ERR_BUSY;
+
+	/* mailbox timeout can now become active */
+	mbx->timeout = FM10K_MBX_INIT_TIMEOUT;
+
+	/* Place mbx in ready to connect state */
+	mbx->state = FM10K_STATE_CONNECT;
+	mbx->max_size = FM10K_MBX_MSG_MAX_SIZE;
+
+	/* reset interface back to connect */
+	fm10k_sm_mbx_connect_reset(mbx);
+
+	/* enable interrupt and notify other party of new message */
+	mbx->mbx_lock = FM10K_MBX_REQ_INTERRUPT | FM10K_MBX_ACK_INTERRUPT |
+			FM10K_MBX_INTERRUPT_ENABLE;
+
+	/* generate and load connect header into mailbox */
+	fm10k_sm_mbx_create_connect_hdr(mbx, 0);
+	fm10k_mbx_write(hw, mbx);
+
+	/* enable interrupt and notify other party of new message */
+
+	return 0;
+}
+
+/**
+ *  fm10k_sm_mbx_disconnect - Shutdown mailbox connection
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will shut down the mailbox.  It places the mailbox first
+ *  in the disconnect state, it then allows up to a predefined timeout for
+ *  the mailbox to transition to close on its own.  If this does not occur
+ *  then the mailbox will be forced into the closed state.
+ *
+ *  Any mailbox transactions not completed before calling this function
+ *  are not guaranteed to complete and may be dropped.
+ **/
+static void fm10k_sm_mbx_disconnect(struct fm10k_hw *hw,
+				    struct fm10k_mbx_info *mbx)
+{
+	int timeout = mbx->timeout ? FM10K_MBX_DISCONNECT_TIMEOUT : 0;
+
+	/* Place mbx in ready to disconnect state */
+	mbx->state = FM10K_STATE_DISCONNECT;
+
+	/* trigger interrupt to start shutdown process */
+	fm10k_write_reg(hw, mbx->mbx_reg, FM10K_MBX_REQ |
+					  FM10K_MBX_INTERRUPT_DISABLE);
+	do {
+		udelay(FM10K_MBX_POLL_DELAY);
+		mbx->ops.process(hw, mbx);
+		timeout -= FM10K_MBX_POLL_DELAY;
+	} while ((timeout > 0) && (mbx->state != FM10K_STATE_CLOSED));
+
+	/* in case we didn't close just force the mailbox into shutdown */
+	mbx->state = FM10K_STATE_CLOSED;
+	mbx->remote = 0;
+	fm10k_mbx_reset_work(mbx);
+	fm10k_mbx_update_max_size(mbx, 0);
+
+	fm10k_write_reg(hw, mbx->mbmem_reg, 0);
+}
+
+/**
+ *  fm10k_mbx_validate_fifo_hdr - Validate fields in the remote FIFO header
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will parse up the fields in the mailbox header and return
+ *  an error if the header contains any of a number of invalid configurations
+ *  including unrecognized offsets or version numbers.
+ **/
+static s32 fm10k_sm_mbx_validate_fifo_hdr(struct fm10k_mbx_info *mbx)
+{
+	const u32 *hdr = &mbx->mbx_hdr;
+	u16 tail, head, ver;
+
+	tail = FM10K_MSG_HDR_FIELD_GET(*hdr, SM_TAIL);
+	ver = FM10K_MSG_HDR_FIELD_GET(*hdr, SM_VER);
+	head = FM10K_MSG_HDR_FIELD_GET(*hdr, SM_HEAD);
+
+	switch (ver) {
+	case 0:
+		break;
+	case FM10K_SM_MBX_VERSION:
+		if (!head || head > FM10K_SM_MBX_FIFO_LEN)
+			return FM10K_MBX_ERR_HEAD;
+		if (!tail || tail > FM10K_SM_MBX_FIFO_LEN)
+			return FM10K_MBX_ERR_TAIL;
+		if (mbx->tail < head)
+			head += mbx->mbmem_len - 1;
+		if (tail < mbx->head)
+			tail += mbx->mbmem_len - 1;
+		if (fm10k_mbx_index_len(mbx, head, mbx->tail) > mbx->tail_len)
+			return FM10K_MBX_ERR_HEAD;
+		if (fm10k_mbx_index_len(mbx, mbx->head, tail) < mbx->mbmem_len)
+			break;
+		return FM10K_MBX_ERR_TAIL;
+	default:
+		return FM10K_MBX_ERR_SRC;
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_sm_mbx_process_error - Process header with error flag set
+ *  @mbx: pointer to mailbox
+ *
+ *  This function is meant to respond to a request where the error flag
+ *  is set.  As a result we will terminate a connection if one is present
+ *  and fall back into the reset state with a connection header of version
+ *  0 (RESET).
+ **/
+static void fm10k_sm_mbx_process_error(struct fm10k_mbx_info *mbx)
+{
+	const enum fm10k_mbx_state state = mbx->state;
+
+	switch (state) {
+	case FM10K_STATE_DISCONNECT:
+		/* if there is an error just disconnect */
+		mbx->remote = 0;
+		break;
+	case FM10K_STATE_OPEN:
+		/* flush any uncompleted work */
+		fm10k_sm_mbx_connect_reset(mbx);
+		break;
+	case FM10K_STATE_CONNECT:
+		/* try connnecting at lower version */
+		if (mbx->remote) {
+			while (mbx->local > 1)
+				mbx->local--;
+			mbx->remote = 0;
+		}
+		break;
+	default:
+		break;
+	}
+
+	fm10k_sm_mbx_create_connect_hdr(mbx, 0);
+}
+
+/**
+ *  fm10k_sm_mbx_create_error_message - Process an error in FIFO hdr
+ *  @mbx: pointer to mailbox
+ *  @err: local error encountered
+ *
+ *  This function will interpret the error provided by err, and based on
+ *  that it may set the error bit in the local message header
+ **/
+static void fm10k_sm_mbx_create_error_msg(struct fm10k_mbx_info *mbx, s32 err)
+{
+	/* only generate an error message for these types */
+	switch (err) {
+	case FM10K_MBX_ERR_TAIL:
+	case FM10K_MBX_ERR_HEAD:
+	case FM10K_MBX_ERR_SRC:
+	case FM10K_MBX_ERR_SIZE:
+	case FM10K_MBX_ERR_RSVD0:
+		break;
+	default:
+		return;
+	}
+
+	/* process it as though we received an error, and send error reply */
+	fm10k_sm_mbx_process_error(mbx);
+	fm10k_sm_mbx_create_connect_hdr(mbx, 1);
+}
+
+/**
+ *  fm10k_sm_mbx_receive - Take message from Rx mailbox FIFO and put it in Rx
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will dequeue one message from the Rx switch manager mailbox
+ *  FIFO and place it in the Rx mailbox FIFO for processing by software.
+ **/
+static s32 fm10k_sm_mbx_receive(struct fm10k_hw *hw,
+				struct fm10k_mbx_info *mbx,
+				u16 tail)
+{
+	/* reduce length by 1 to convert to a mask */
+	u16 mbmem_len = mbx->mbmem_len - 1;
+	s32 err;
+
+	/* push tail in front of head */
+	if (tail < mbx->head)
+		tail += mbmem_len;
+
+	/* copy data to the Rx FIFO */
+	err = fm10k_mbx_push_tail(hw, mbx, tail);
+	if (err < 0)
+		return err;
+
+	/* process messages if we have received any */
+	fm10k_mbx_dequeue_rx(hw, mbx);
+
+	/* guarantee head aligns with the end of the last message */
+	mbx->head = fm10k_mbx_head_sub(mbx, mbx->pushed);
+	mbx->pushed = 0;
+
+	/* clear any extra bits left over since index adds 1 extra bit */
+	if (mbx->head > mbmem_len)
+		mbx->head -= mbmem_len;
+
+	return err;
+}
+
+/**
+ *  fm10k_sm_mbx_transmit - Take message from Tx and put it in Tx mailbox FIFO
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will dequeue one message from the Tx mailbox FIFO and place
+ *  it in the Tx switch manager mailbox FIFO for processing by hardware.
+ **/
+static void fm10k_sm_mbx_transmit(struct fm10k_hw *hw,
+				  struct fm10k_mbx_info *mbx, u16 head)
+{
+	struct fm10k_mbx_fifo *fifo = &mbx->tx;
+	/* reduce length by 1 to convert to a mask */
+	u16 mbmem_len = mbx->mbmem_len - 1;
+	u16 tail_len, len = 0;
+	u32 *msg;
+
+	/* push head behind tail */
+	if (mbx->tail < head)
+		head += mbmem_len;
+
+	fm10k_mbx_pull_head(hw, mbx, head);
+
+	/* determine msg aligned offset for end of buffer */
+	do {
+		msg = fifo->buffer + fm10k_fifo_head_offset(fifo, len);
+		tail_len = len;
+		len += FM10K_TLV_DWORD_LEN(*msg);
+	} while ((len <= mbx->tail_len) && (len < mbmem_len));
+
+	/* guarantee we stop on a message boundary */
+	if (mbx->tail_len > tail_len) {
+		mbx->tail = fm10k_mbx_tail_sub(mbx, mbx->tail_len - tail_len);
+		mbx->tail_len = tail_len;
+	}
+
+	/* clear any extra bits left over since index adds 1 extra bit */
+	if (mbx->tail > mbmem_len)
+		mbx->tail -= mbmem_len;
+}
+
+/**
+ *  fm10k_sm_mbx_create_reply - Generate reply based on state and remote head
+ *  @mbx: pointer to mailbox
+ *  @head: acknowledgement number
+ *
+ *  This function will generate an outgoing message based on the current
+ *  mailbox state and the remote fifo head.  It will return the length
+ *  of the outgoing message excluding header on success, and a negative value
+ *  on error.
+ **/
+static void fm10k_sm_mbx_create_reply(struct fm10k_hw *hw,
+				      struct fm10k_mbx_info *mbx, u16 head)
+{
+	switch (mbx->state) {
+	case FM10K_STATE_OPEN:
+	case FM10K_STATE_DISCONNECT:
+		/* flush out Tx data */
+		fm10k_sm_mbx_transmit(hw, mbx, head);
+
+		/* generate new header based on data */
+		if (mbx->tail_len || (mbx->state == FM10K_STATE_OPEN)) {
+			fm10k_sm_mbx_create_data_hdr(mbx);
+		} else {
+			mbx->remote = 0;
+			fm10k_sm_mbx_create_connect_hdr(mbx, 0);
+		}
+		break;
+	case FM10K_STATE_CONNECT:
+	case FM10K_STATE_CLOSED:
+		fm10k_sm_mbx_create_connect_hdr(mbx, 0);
+		break;
+	default:
+		break;
+	}
+}
+
+/**
+ *  fm10k_sm_mbx_process_reset - Process header with version == 0 (RESET)
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function is meant to respond to a request where the version data
+ *  is set to 0.  As such we will either terminate the connection or go
+ *  into the connect state in order to re-establish the connection.  This
+ *  function can also be used to respond to an error as the connection
+ *  resetting would also be a means of dealing with errors.
+ **/
+static void fm10k_sm_mbx_process_reset(struct fm10k_hw *hw,
+				       struct fm10k_mbx_info *mbx)
+{
+	const enum fm10k_mbx_state state = mbx->state;
+
+	switch (state) {
+	case FM10K_STATE_DISCONNECT:
+		/* drop remote connections and disconnect */
+		mbx->state = FM10K_STATE_CLOSED;
+		mbx->remote = 0;
+		mbx->local = 0;
+		break;
+	case FM10K_STATE_OPEN:
+		/* flush any incomplete work */
+		fm10k_sm_mbx_connect_reset(mbx);
+		break;
+	case FM10K_STATE_CONNECT:
+		/* Update remote value to match local value */
+		mbx->remote = mbx->local;
+	default:
+		break;
+	}
+
+	fm10k_sm_mbx_create_reply(hw, mbx, mbx->tail);
+}
+
+/**
+ *  fm10k_sm_mbx_process_version_1 - Process header with version == 1
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function is meant to process messages received when the remote
+ *  mailbox is active.
+ **/
+static s32 fm10k_sm_mbx_process_version_1(struct fm10k_hw *hw,
+					  struct fm10k_mbx_info *mbx)
+{
+	const u32 *hdr = &mbx->mbx_hdr;
+	u16 head, tail;
+	s32 len;
+
+	/* pull all fields needed for verification */
+	tail = FM10K_MSG_HDR_FIELD_GET(*hdr, SM_TAIL);
+	head = FM10K_MSG_HDR_FIELD_GET(*hdr, SM_HEAD);
+
+	/* if we are in connect and wanting version 1 then start up and go */
+	if (mbx->state == FM10K_STATE_CONNECT) {
+		if (!mbx->remote)
+			goto send_reply;
+		if (mbx->remote != 1)
+			return FM10K_MBX_ERR_SRC;
+
+		mbx->state = FM10K_STATE_OPEN;
+	}
+
+	do {
+		/* abort on message size errors */
+		len = fm10k_sm_mbx_receive(hw, mbx, tail);
+		if (len < 0)
+			return len;
+
+		/* continue until we have flushed the Rx FIFO */
+	} while (len);
+
+send_reply:
+	fm10k_sm_mbx_create_reply(hw, mbx, head);
+
+	return 0;
+}
+
+/**
+ *  fm10k_sm_mbx_process - Process mailbox switch mailbox interrupt
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *
+ *  This function will process incoming mailbox events and generate mailbox
+ *  replies.  It will return a value indicating the number of DWORDs
+ *  transmitted excluding header on success or a negative value on error.
+ **/
+static s32 fm10k_sm_mbx_process(struct fm10k_hw *hw,
+				struct fm10k_mbx_info *mbx)
+{
+	s32 err;
+
+	/* we do not read mailbox if closed */
+	if (mbx->state == FM10K_STATE_CLOSED)
+		return 0;
+
+	/* retrieve data from switch manager */
+	err = fm10k_mbx_read(hw, mbx);
+	if (err)
+		return err;
+
+	err = fm10k_sm_mbx_validate_fifo_hdr(mbx);
+	if (err < 0)
+		goto fifo_err;
+
+	if (FM10K_MSG_HDR_FIELD_GET(mbx->mbx_hdr, SM_ERR)) {
+		fm10k_sm_mbx_process_error(mbx);
+		goto fifo_err;
+	}
+
+	switch (FM10K_MSG_HDR_FIELD_GET(mbx->mbx_hdr, SM_VER)) {
+	case 0:
+		fm10k_sm_mbx_process_reset(hw, mbx);
+		break;
+	case FM10K_SM_MBX_VERSION:
+		err = fm10k_sm_mbx_process_version_1(hw, mbx);
+		break;
+	}
+
+fifo_err:
+	if (err < 0)
+		fm10k_sm_mbx_create_error_msg(mbx, err);
+
+	/* report data to switch manager */
+	fm10k_mbx_write(hw, mbx);
+
+	return err;
+}
+
+/**
+ *  fm10k_sm_mbx_init - Initialize mailbox memory for PF/SM mailbox
+ *  @hw: pointer to hardware structure
+ *  @mbx: pointer to mailbox
+ *  @msg_data: handlers for mailbox events
+ *
+ *  This function for now is used to stub out the PF/SM mailbox
+ **/
+s32 fm10k_sm_mbx_init(struct fm10k_hw *hw, struct fm10k_mbx_info *mbx,
+		      const struct fm10k_msg_data *msg_data)
+{
+	mbx->mbx_reg = FM10K_GMBX;
+	mbx->mbmem_reg = FM10K_MBMEM_PF(0);
+	/* start out in closed state */
+	mbx->state = FM10K_STATE_CLOSED;
+
+	/* validate layout of handlers before assigning them */
+	if (fm10k_mbx_validate_handlers(msg_data))
+		return FM10K_ERR_PARAM;
+
+	/* initialize the message handlers */
+	mbx->msg_data = msg_data;
+
+	/* start mailbox as timed out and let the reset_hw call
+	 * set the timeout value to begin communications
+	 */
+	mbx->timeout = 0;
+	mbx->udelay = FM10K_MBX_INIT_DELAY;
+
+	/* Split buffer for use by Tx/Rx FIFOs */
+	mbx->max_size = FM10K_MBX_MSG_MAX_SIZE;
+	mbx->mbmem_len = FM10K_MBMEM_PF_XOR;
+
+	/* initialize the FIFOs, sizes are in 4 byte increments */
+	fm10k_fifo_init(&mbx->tx, mbx->buffer, FM10K_MBX_TX_BUFFER_SIZE);
+	fm10k_fifo_init(&mbx->rx, &mbx->buffer[FM10K_MBX_TX_BUFFER_SIZE],
+			FM10K_MBX_RX_BUFFER_SIZE);
+
+	/* initialize function pointers */
+	mbx->ops.connect = fm10k_sm_mbx_connect;
+	mbx->ops.disconnect = fm10k_sm_mbx_disconnect;
+	mbx->ops.rx_ready = fm10k_mbx_rx_ready;
+	mbx->ops.tx_ready = fm10k_mbx_tx_ready;
+	mbx->ops.tx_complete = fm10k_mbx_tx_complete;
+	mbx->ops.enqueue_tx = fm10k_mbx_enqueue_tx;
+	mbx->ops.process = fm10k_sm_mbx_process;
+	mbx->ops.register_handlers = fm10k_mbx_register_handlers;
+
+	return 0;
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_mbx.h b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.h
new file mode 100644
index 000000000000..0419a7f0035e
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_mbx.h
@@ -0,0 +1,307 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifndef _FM10K_MBX_H_
+#define _FM10K_MBX_H_
+
+/* forward declaration */
+struct fm10k_mbx_info;
+
+#include "fm10k_type.h"
+#include "fm10k_tlv.h"
+
+/* PF Mailbox Registers */
+#define FM10K_MBMEM(_n)		((_n) + 0x18000)
+#define FM10K_MBMEM_VF(_n, _m)	(((_n) * 0x10) + (_m) + 0x18000)
+#define FM10K_MBMEM_SM(_n)	((_n) + 0x18400)
+#define FM10K_MBMEM_PF(_n)	((_n) + 0x18600)
+/* XOR provides means of switching from Tx to Rx FIFO */
+#define FM10K_MBMEM_PF_XOR	(FM10K_MBMEM_SM(0) ^ FM10K_MBMEM_PF(0))
+#define FM10K_MBX(_n)		((_n) + 0x18800)
+#define FM10K_MBX_REQ				0x00000002
+#define FM10K_MBX_ACK				0x00000004
+#define FM10K_MBX_REQ_INTERRUPT			0x00000008
+#define FM10K_MBX_ACK_INTERRUPT			0x00000010
+#define FM10K_MBX_INTERRUPT_ENABLE		0x00000020
+#define FM10K_MBX_INTERRUPT_DISABLE		0x00000040
+#define FM10K_MBICR(_n)		((_n) + 0x18840)
+#define FM10K_GMBX		0x18842
+
+/* VF Mailbox Registers */
+#define FM10K_VFMBX		0x00010
+#define FM10K_VFMBMEM(_n)	((_n) + 0x00020)
+#define FM10K_VFMBMEM_LEN	16
+#define FM10K_VFMBMEM_VF_XOR	(FM10K_VFMBMEM_LEN / 2)
+
+/* Delays/timeouts */
+#define FM10K_MBX_DISCONNECT_TIMEOUT		500
+#define FM10K_MBX_POLL_DELAY			19
+#define FM10K_MBX_INT_DELAY			20
+
+/* PF/VF Mailbox state machine
+ *
+ * +----------+	    connect()	+----------+
+ * |  CLOSED  | --------------> |  CONNECT |
+ * +----------+			+----------+
+ *   ^				  ^	 |
+ *   | rcv:	      rcv:	  |	 | rcv:
+ *   |  Connect	       Disconnect |	 |  Connect
+ *   |  Disconnect     Error	  |	 |  Data
+ *   |				  |	 |
+ *   |				  |	 V
+ * +----------+   disconnect()	+----------+
+ * |DISCONNECT| <-------------- |   OPEN   |
+ * +----------+			+----------+
+ *
+ * The diagram above describes the PF/VF mailbox state machine.  There
+ * are four main states to this machine.
+ * Closed: This state represents a mailbox that is in a standby state
+ *	   with interrupts disabled.  In this state the mailbox should not
+ *	   read the mailbox or write any data.  The only means of exiting
+ *	   this state is for the system to make the connect() call for the
+ *	   mailbox, it will then transition to the connect state.
+ * Connect: In this state the mailbox is seeking a connection.  It will
+ *	    post a connect message with no specified destination and will
+ *	    wait for a reply from the other side of the mailbox.  This state
+ *	    is exited when either a connect with the local mailbox as the
+ *	    destination is received or when a data message is received with
+ *	    a valid sequence number.
+ * Open: In this state the mailbox is able to transfer data between the local
+ *       entity and the remote.  It will fall back to connect in the event of
+ *       receiving either an error message, or a disconnect message.  It will
+ *       transition to disconnect on a call to disconnect();
+ * Disconnect: In this state the mailbox is attempting to gracefully terminate
+ *	       the connection.  It will do so at the first point where it knows
+ *	       that the remote endpoint is either done sending, or when the
+ *	       remote endpoint has fallen back into connect.
+ */
+enum fm10k_mbx_state {
+	FM10K_STATE_CLOSED,
+	FM10K_STATE_CONNECT,
+	FM10K_STATE_OPEN,
+	FM10K_STATE_DISCONNECT,
+};
+
+/* PF/VF Mailbox header format
+ *    3			  2		      1			  0
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |        Size/Err_no/CRC        | Rsvd0 | Head  | Tail  | Type  |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * The layout above describes the format for the header used in the PF/VF
+ * mailbox.  The header is broken out into the following fields:
+ * Type: There are 4 supported message types
+ *		0x8: Data header - used to transport message data
+ *		0xC: Connect header - used to establish connection
+ *		0xD: Disconnect header - used to tear down a connection
+ *		0xE: Error header - used to address message exceptions
+ * Tail: Tail index for local FIFO
+ *		Tail index actually consists of two parts.  The MSB of
+ *		the head is a loop tracker, it is 0 on an even numbered
+ *		loop through the FIFO, and 1 on the odd numbered loops.
+ *		To get the actual mailbox offset based on the tail it
+ *		is necessary to add bit 3 to bit 0 and clear bit 3.  This
+ *		gives us a valid range of 0x1 - 0xE.
+ * Head: Head index for remote FIFO
+ *		Head index follows the same format as the tail index.
+ * Rsvd0: Reserved 0 portion of the mailbox header
+ * CRC: Running CRC for all data since connect plus current message header
+ * Size: Maximum message size - Applies only to connect headers
+ *		The maximum message size is provided during connect to avoid
+ *		jamming the mailbox with messages that do not fit.
+ * Err_no: Error number - Applies only to error headers
+ *		The error number provides a indication of the type of error
+ *		experienced.
+ */
+
+/* macros for retriving and setting header values */
+#define FM10K_MSG_HDR_MASK(name) \
+	((0x1u << FM10K_MSG_##name##_SIZE) - 1)
+#define FM10K_MSG_HDR_FIELD_SET(value, name) \
+	(((u32)(value) & FM10K_MSG_HDR_MASK(name)) << FM10K_MSG_##name##_SHIFT)
+#define FM10K_MSG_HDR_FIELD_GET(value, name) \
+	((u16)((value) >> FM10K_MSG_##name##_SHIFT) & FM10K_MSG_HDR_MASK(name))
+
+/* offsets shared between all headers */
+#define FM10K_MSG_TYPE_SHIFT			0
+#define FM10K_MSG_TYPE_SIZE			4
+#define FM10K_MSG_TAIL_SHIFT			4
+#define FM10K_MSG_TAIL_SIZE			4
+#define FM10K_MSG_HEAD_SHIFT			8
+#define FM10K_MSG_HEAD_SIZE			4
+#define FM10K_MSG_RSVD0_SHIFT			12
+#define FM10K_MSG_RSVD0_SIZE			4
+
+/* offsets for data/disconnect headers */
+#define FM10K_MSG_CRC_SHIFT			16
+#define FM10K_MSG_CRC_SIZE			16
+
+/* offsets for connect headers */
+#define FM10K_MSG_CONNECT_SIZE_SHIFT		16
+#define FM10K_MSG_CONNECT_SIZE_SIZE		16
+
+/* offsets for error headers */
+#define FM10K_MSG_ERR_NO_SHIFT			16
+#define FM10K_MSG_ERR_NO_SIZE			16
+
+enum fm10k_msg_type {
+	FM10K_MSG_DATA			= 0x8,
+	FM10K_MSG_CONNECT		= 0xC,
+	FM10K_MSG_DISCONNECT		= 0xD,
+	FM10K_MSG_ERROR			= 0xE,
+};
+
+/* HNI/SM Mailbox FIFO format
+ *    3                   2                   1                   0
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-------+-----------------------+-------+-----------------------+
+ * | Error |      Remote Head      |Version|      Local Tail       |
+ * +-------+-----------------------+-------+-----------------------+
+ * |                                                               |
+ * .                        Local FIFO Data                        .
+ * .                                                               .
+ * +-------+-----------------------+-------+-----------------------+
+ *
+ * The layout above describes the format for the FIFOs used by the host
+ * network interface and the switch manager to communicate messages back
+ * and forth.  Both the HNI and the switch maintain one such FIFO.  The
+ * layout in memory has the switch manager FIFO followed immediately by
+ * the HNI FIFO.  For this reason I am using just the pointer to the
+ * HNI FIFO in the mailbox ops as the offset between the two is fixed.
+ *
+ * The header for the FIFO is broken out into the following fields:
+ * Local Tail:  Offset into FIFO region for next DWORD to write.
+ * Version:  Version info for mailbox, only values of 0/1 are supported.
+ * Remote Head:  Offset into remote FIFO to indicate how much we have read.
+ * Error: Error indication, values TBD.
+ */
+
+/* version number for switch manager mailboxes */
+#define FM10K_SM_MBX_VERSION		1
+#define FM10K_SM_MBX_FIFO_LEN		(FM10K_MBMEM_PF_XOR - 1)
+
+/* offsets shared between all SM FIFO headers */
+#define FM10K_MSG_SM_TAIL_SHIFT			0
+#define FM10K_MSG_SM_TAIL_SIZE			12
+#define FM10K_MSG_SM_VER_SHIFT			12
+#define FM10K_MSG_SM_VER_SIZE			4
+#define FM10K_MSG_SM_HEAD_SHIFT			16
+#define FM10K_MSG_SM_HEAD_SIZE			12
+#define FM10K_MSG_SM_ERR_SHIFT			28
+#define FM10K_MSG_SM_ERR_SIZE			4
+
+/* All error messages returned by mailbox functions
+ * The value -511 is 0xFE01 in hex.  The idea is to order the errors
+ * from 0xFE01 - 0xFEFF so error codes are easily visible in the mailbox
+ * messages.  This also helps to avoid error number collisions as Linux
+ * doesn't appear to use error numbers 256 - 511.
+ */
+#define FM10K_MBX_ERR(_n) ((_n) - 512)
+#define FM10K_MBX_ERR_NO_MBX		FM10K_MBX_ERR(0x01)
+#define FM10K_MBX_ERR_NO_SPACE		FM10K_MBX_ERR(0x03)
+#define FM10K_MBX_ERR_TAIL		FM10K_MBX_ERR(0x05)
+#define FM10K_MBX_ERR_HEAD		FM10K_MBX_ERR(0x06)
+#define FM10K_MBX_ERR_SRC		FM10K_MBX_ERR(0x08)
+#define FM10K_MBX_ERR_TYPE		FM10K_MBX_ERR(0x09)
+#define FM10K_MBX_ERR_SIZE		FM10K_MBX_ERR(0x0B)
+#define FM10K_MBX_ERR_BUSY		FM10K_MBX_ERR(0x0C)
+#define FM10K_MBX_ERR_RSVD0		FM10K_MBX_ERR(0x0E)
+#define FM10K_MBX_ERR_CRC		FM10K_MBX_ERR(0x0F)
+
+#define FM10K_MBX_CRC_SEED		0xFFFF
+
+struct fm10k_mbx_ops {
+	s32 (*connect)(struct fm10k_hw *, struct fm10k_mbx_info *);
+	void (*disconnect)(struct fm10k_hw *, struct fm10k_mbx_info *);
+	bool (*rx_ready)(struct fm10k_mbx_info *);
+	bool (*tx_ready)(struct fm10k_mbx_info *, u16);
+	bool (*tx_complete)(struct fm10k_mbx_info *);
+	s32 (*enqueue_tx)(struct fm10k_hw *, struct fm10k_mbx_info *,
+			  const u32 *);
+	s32 (*process)(struct fm10k_hw *, struct fm10k_mbx_info *);
+	s32 (*register_handlers)(struct fm10k_mbx_info *,
+				 const struct fm10k_msg_data *);
+};
+
+struct fm10k_mbx_fifo {
+	u32 *buffer;
+	u16 head;
+	u16 tail;
+	u16 size;
+};
+
+/* size of buffer to be stored in mailbox for FIFOs */
+#define FM10K_MBX_TX_BUFFER_SIZE	512
+#define FM10K_MBX_RX_BUFFER_SIZE	128
+#define FM10K_MBX_BUFFER_SIZE \
+	(FM10K_MBX_TX_BUFFER_SIZE + FM10K_MBX_RX_BUFFER_SIZE)
+
+/* minimum and maximum message size in dwords */
+#define FM10K_MBX_MSG_MAX_SIZE \
+	((FM10K_MBX_TX_BUFFER_SIZE - 1) & (FM10K_MBX_RX_BUFFER_SIZE - 1))
+#define FM10K_VFMBX_MSG_MTU	((FM10K_VFMBMEM_LEN / 2) - 1)
+
+#define FM10K_MBX_INIT_TIMEOUT	2000 /* number of retries on mailbox */
+#define FM10K_MBX_INIT_DELAY	500  /* microseconds between retries */
+
+struct fm10k_mbx_info {
+	/* function pointers for mailbox operations */
+	struct fm10k_mbx_ops ops;
+	const struct fm10k_msg_data *msg_data;
+
+	/* message FIFOs */
+	struct fm10k_mbx_fifo rx;
+	struct fm10k_mbx_fifo tx;
+
+	/* delay for handling timeouts */
+	u32 timeout;
+	u32 udelay;
+
+	/* mailbox state info */
+	u32 mbx_reg, mbmem_reg, mbx_lock, mbx_hdr;
+	u16 max_size, mbmem_len;
+	u16 tail, tail_len, pulled;
+	u16 head, head_len, pushed;
+	u16 local, remote;
+	enum fm10k_mbx_state state;
+
+	/* result of last mailbox test */
+	s32 test_result;
+
+	/* statistics */
+	u64 tx_busy;
+	u64 tx_dropped;
+	u64 tx_messages;
+	u64 tx_dwords;
+	u64 rx_messages;
+	u64 rx_dwords;
+	u64 rx_parse_err;
+
+	/* Buffer to store messages */
+	u32 buffer[FM10K_MBX_BUFFER_SIZE];
+};
+
+s32 fm10k_pfvf_mbx_init(struct fm10k_hw *, struct fm10k_mbx_info *,
+			const struct fm10k_msg_data *, u8);
+s32 fm10k_sm_mbx_init(struct fm10k_hw *, struct fm10k_mbx_info *,
+		      const struct fm10k_msg_data *);
+
+#endif /* _FM10K_MBX_H_ */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
new file mode 100644
index 000000000000..8811364b91cb
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_netdev.c
@@ -0,0 +1,1436 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k.h"
+#include <linux/vmalloc.h>
+#if IS_ENABLED(CONFIG_FM10K_VXLAN)
+#include <net/vxlan.h>
+#endif /* CONFIG_FM10K_VXLAN */
+
+/**
+ * fm10k_setup_tx_resources - allocate Tx resources (Descriptors)
+ * @tx_ring:    tx descriptor ring (for a specific queue) to setup
+ *
+ * Return 0 on success, negative on failure
+ **/
+int fm10k_setup_tx_resources(struct fm10k_ring *tx_ring)
+{
+	struct device *dev = tx_ring->dev;
+	int size;
+
+	size = sizeof(struct fm10k_tx_buffer) * tx_ring->count;
+
+	tx_ring->tx_buffer = vzalloc(size);
+	if (!tx_ring->tx_buffer)
+		goto err;
+
+	u64_stats_init(&tx_ring->syncp);
+
+	/* round up to nearest 4K */
+	tx_ring->size = tx_ring->count * sizeof(struct fm10k_tx_desc);
+	tx_ring->size = ALIGN(tx_ring->size, 4096);
+
+	tx_ring->desc = dma_alloc_coherent(dev, tx_ring->size,
+					   &tx_ring->dma, GFP_KERNEL);
+	if (!tx_ring->desc)
+		goto err;
+
+	return 0;
+
+err:
+	vfree(tx_ring->tx_buffer);
+	tx_ring->tx_buffer = NULL;
+	return -ENOMEM;
+}
+
+/**
+ * fm10k_setup_all_tx_resources - allocate all queues Tx resources
+ * @interface: board private structure
+ *
+ * If this function returns with an error, then it's possible one or
+ * more of the rings is populated (while the rest are not).  It is the
+ * callers duty to clean those orphaned rings.
+ *
+ * Return 0 on success, negative on failure
+ **/
+static int fm10k_setup_all_tx_resources(struct fm10k_intfc *interface)
+{
+	int i, err = 0;
+
+	for (i = 0; i < interface->num_tx_queues; i++) {
+		err = fm10k_setup_tx_resources(interface->tx_ring[i]);
+		if (!err)
+			continue;
+
+		netif_err(interface, probe, interface->netdev,
+			  "Allocation for Tx Queue %u failed\n", i);
+		goto err_setup_tx;
+	}
+
+	return 0;
+err_setup_tx:
+	/* rewind the index freeing the rings as we go */
+	while (i--)
+		fm10k_free_tx_resources(interface->tx_ring[i]);
+	return err;
+}
+
+/**
+ * fm10k_setup_rx_resources - allocate Rx resources (Descriptors)
+ * @rx_ring:    rx descriptor ring (for a specific queue) to setup
+ *
+ * Returns 0 on success, negative on failure
+ **/
+int fm10k_setup_rx_resources(struct fm10k_ring *rx_ring)
+{
+	struct device *dev = rx_ring->dev;
+	int size;
+
+	size = sizeof(struct fm10k_rx_buffer) * rx_ring->count;
+
+	rx_ring->rx_buffer = vzalloc(size);
+	if (!rx_ring->rx_buffer)
+		goto err;
+
+	u64_stats_init(&rx_ring->syncp);
+
+	/* Round up to nearest 4K */
+	rx_ring->size = rx_ring->count * sizeof(union fm10k_rx_desc);
+	rx_ring->size = ALIGN(rx_ring->size, 4096);
+
+	rx_ring->desc = dma_alloc_coherent(dev, rx_ring->size,
+					   &rx_ring->dma, GFP_KERNEL);
+	if (!rx_ring->desc)
+		goto err;
+
+	return 0;
+err:
+	vfree(rx_ring->rx_buffer);
+	rx_ring->rx_buffer = NULL;
+	return -ENOMEM;
+}
+
+/**
+ * fm10k_setup_all_rx_resources - allocate all queues Rx resources
+ * @interface: board private structure
+ *
+ * If this function returns with an error, then it's possible one or
+ * more of the rings is populated (while the rest are not).  It is the
+ * callers duty to clean those orphaned rings.
+ *
+ * Return 0 on success, negative on failure
+ **/
+static int fm10k_setup_all_rx_resources(struct fm10k_intfc *interface)
+{
+	int i, err = 0;
+
+	for (i = 0; i < interface->num_rx_queues; i++) {
+		err = fm10k_setup_rx_resources(interface->rx_ring[i]);
+		if (!err)
+			continue;
+
+		netif_err(interface, probe, interface->netdev,
+			  "Allocation for Rx Queue %u failed\n", i);
+		goto err_setup_rx;
+	}
+
+	return 0;
+err_setup_rx:
+	/* rewind the index freeing the rings as we go */
+	while (i--)
+		fm10k_free_rx_resources(interface->rx_ring[i]);
+	return err;
+}
+
+void fm10k_unmap_and_free_tx_resource(struct fm10k_ring *ring,
+				      struct fm10k_tx_buffer *tx_buffer)
+{
+	if (tx_buffer->skb) {
+		dev_kfree_skb_any(tx_buffer->skb);
+		if (dma_unmap_len(tx_buffer, len))
+			dma_unmap_single(ring->dev,
+					 dma_unmap_addr(tx_buffer, dma),
+					 dma_unmap_len(tx_buffer, len),
+					 DMA_TO_DEVICE);
+	} else if (dma_unmap_len(tx_buffer, len)) {
+		dma_unmap_page(ring->dev,
+			       dma_unmap_addr(tx_buffer, dma),
+			       dma_unmap_len(tx_buffer, len),
+			       DMA_TO_DEVICE);
+	}
+	tx_buffer->next_to_watch = NULL;
+	tx_buffer->skb = NULL;
+	dma_unmap_len_set(tx_buffer, len, 0);
+	/* tx_buffer must be completely set up in the transmit path */
+}
+
+/**
+ * fm10k_clean_tx_ring - Free Tx Buffers
+ * @tx_ring: ring to be cleaned
+ **/
+static void fm10k_clean_tx_ring(struct fm10k_ring *tx_ring)
+{
+	struct fm10k_tx_buffer *tx_buffer;
+	unsigned long size;
+	u16 i;
+
+	/* ring already cleared, nothing to do */
+	if (!tx_ring->tx_buffer)
+		return;
+
+	/* Free all the Tx ring sk_buffs */
+	for (i = 0; i < tx_ring->count; i++) {
+		tx_buffer = &tx_ring->tx_buffer[i];
+		fm10k_unmap_and_free_tx_resource(tx_ring, tx_buffer);
+	}
+
+	/* reset BQL values */
+	netdev_tx_reset_queue(txring_txq(tx_ring));
+
+	size = sizeof(struct fm10k_tx_buffer) * tx_ring->count;
+	memset(tx_ring->tx_buffer, 0, size);
+
+	/* Zero out the descriptor ring */
+	memset(tx_ring->desc, 0, tx_ring->size);
+}
+
+/**
+ * fm10k_free_tx_resources - Free Tx Resources per Queue
+ * @tx_ring: Tx descriptor ring for a specific queue
+ *
+ * Free all transmit software resources
+ **/
+void fm10k_free_tx_resources(struct fm10k_ring *tx_ring)
+{
+	fm10k_clean_tx_ring(tx_ring);
+
+	vfree(tx_ring->tx_buffer);
+	tx_ring->tx_buffer = NULL;
+
+	/* if not set, then don't free */
+	if (!tx_ring->desc)
+		return;
+
+	dma_free_coherent(tx_ring->dev, tx_ring->size,
+			  tx_ring->desc, tx_ring->dma);
+	tx_ring->desc = NULL;
+}
+
+/**
+ * fm10k_clean_all_tx_rings - Free Tx Buffers for all queues
+ * @interface: board private structure
+ **/
+void fm10k_clean_all_tx_rings(struct fm10k_intfc *interface)
+{
+	int i;
+
+	for (i = 0; i < interface->num_tx_queues; i++)
+		fm10k_clean_tx_ring(interface->tx_ring[i]);
+
+	/* remove any stale timestamp buffers and free them */
+	skb_queue_purge(&interface->ts_tx_skb_queue);
+}
+
+/**
+ * fm10k_free_all_tx_resources - Free Tx Resources for All Queues
+ * @interface: board private structure
+ *
+ * Free all transmit software resources
+ **/
+static void fm10k_free_all_tx_resources(struct fm10k_intfc *interface)
+{
+	int i = interface->num_tx_queues;
+
+	while (i--)
+		fm10k_free_tx_resources(interface->tx_ring[i]);
+}
+
+/**
+ * fm10k_clean_rx_ring - Free Rx Buffers per Queue
+ * @rx_ring: ring to free buffers from
+ **/
+static void fm10k_clean_rx_ring(struct fm10k_ring *rx_ring)
+{
+	unsigned long size;
+	u16 i;
+
+	if (!rx_ring->rx_buffer)
+		return;
+
+	if (rx_ring->skb)
+		dev_kfree_skb(rx_ring->skb);
+	rx_ring->skb = NULL;
+
+	/* Free all the Rx ring sk_buffs */
+	for (i = 0; i < rx_ring->count; i++) {
+		struct fm10k_rx_buffer *buffer = &rx_ring->rx_buffer[i];
+		/* clean-up will only set page pointer to NULL */
+		if (!buffer->page)
+			continue;
+
+		dma_unmap_page(rx_ring->dev, buffer->dma,
+			       PAGE_SIZE, DMA_FROM_DEVICE);
+		__free_page(buffer->page);
+
+		buffer->page = NULL;
+	}
+
+	size = sizeof(struct fm10k_rx_buffer) * rx_ring->count;
+	memset(rx_ring->rx_buffer, 0, size);
+
+	/* Zero out the descriptor ring */
+	memset(rx_ring->desc, 0, rx_ring->size);
+
+	rx_ring->next_to_alloc = 0;
+	rx_ring->next_to_clean = 0;
+	rx_ring->next_to_use = 0;
+}
+
+/**
+ * fm10k_free_rx_resources - Free Rx Resources
+ * @rx_ring: ring to clean the resources from
+ *
+ * Free all receive software resources
+ **/
+void fm10k_free_rx_resources(struct fm10k_ring *rx_ring)
+{
+	fm10k_clean_rx_ring(rx_ring);
+
+	vfree(rx_ring->rx_buffer);
+	rx_ring->rx_buffer = NULL;
+
+	/* if not set, then don't free */
+	if (!rx_ring->desc)
+		return;
+
+	dma_free_coherent(rx_ring->dev, rx_ring->size,
+			  rx_ring->desc, rx_ring->dma);
+
+	rx_ring->desc = NULL;
+}
+
+/**
+ * fm10k_clean_all_rx_rings - Free Rx Buffers for all queues
+ * @interface: board private structure
+ **/
+void fm10k_clean_all_rx_rings(struct fm10k_intfc *interface)
+{
+	int i;
+
+	for (i = 0; i < interface->num_rx_queues; i++)
+		fm10k_clean_rx_ring(interface->rx_ring[i]);
+}
+
+/**
+ * fm10k_free_all_rx_resources - Free Rx Resources for All Queues
+ * @interface: board private structure
+ *
+ * Free all receive software resources
+ **/
+static void fm10k_free_all_rx_resources(struct fm10k_intfc *interface)
+{
+	int i = interface->num_rx_queues;
+
+	while (i--)
+		fm10k_free_rx_resources(interface->rx_ring[i]);
+}
+
+/**
+ * fm10k_request_glort_range - Request GLORTs for use in configuring rules
+ * @interface: board private structure
+ *
+ * This function allocates a range of glorts for this inteface to use.
+ **/
+static void fm10k_request_glort_range(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	u16 mask = (~hw->mac.dglort_map) >> FM10K_DGLORTMAP_MASK_SHIFT;
+
+	/* establish GLORT base */
+	interface->glort = hw->mac.dglort_map & FM10K_DGLORTMAP_NONE;
+	interface->glort_count = 0;
+
+	/* nothing we can do until mask is allocated */
+	if (hw->mac.dglort_map == FM10K_DGLORTMAP_NONE)
+		return;
+
+	/* we support 3 possible GLORT configurations.
+	 * 1: VFs consume all but the last 1
+	 * 2: VFs and PF split glorts with possible gap between
+	 * 3: VFs allocated first 64, all others belong to PF
+	 */
+	if (mask <= hw->iov.total_vfs) {
+		interface->glort_count = 1;
+		interface->glort += mask;
+	} else if (mask < 64) {
+		interface->glort_count = (mask + 1) / 2;
+		interface->glort += interface->glort_count;
+	} else {
+		interface->glort_count = mask - 63;
+		interface->glort += 64;
+	}
+}
+
+/**
+ * fm10k_del_vxlan_port_all
+ * @interface: board private structure
+ *
+ * This function frees the entire vxlan_port list
+ **/
+static void fm10k_del_vxlan_port_all(struct fm10k_intfc *interface)
+{
+	struct fm10k_vxlan_port *vxlan_port;
+
+	/* flush all entries from list */
+	vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
+					      struct fm10k_vxlan_port, list);
+	while (vxlan_port) {
+		list_del(&vxlan_port->list);
+		kfree(vxlan_port);
+		vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
+						      struct fm10k_vxlan_port,
+						      list);
+	}
+}
+
+/**
+ * fm10k_restore_vxlan_port
+ * @interface: board private structure
+ *
+ * This function restores the value in the tunnel_cfg register after reset
+ **/
+static void fm10k_restore_vxlan_port(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_vxlan_port *vxlan_port;
+
+	/* only the PF supports configuring tunnels */
+	if (hw->mac.type != fm10k_mac_pf)
+		return;
+
+	vxlan_port = list_first_entry_or_null(&interface->vxlan_port,
+					      struct fm10k_vxlan_port, list);
+
+	/* restore tunnel configuration register */
+	fm10k_write_reg(hw, FM10K_TUNNEL_CFG,
+			(vxlan_port ? ntohs(vxlan_port->port) : 0) |
+			(ETH_P_TEB << FM10K_TUNNEL_CFG_NVGRE_SHIFT));
+}
+
+/**
+ * fm10k_add_vxlan_port
+ * @netdev: network interface device structure
+ * @sa_family: Address family of new port
+ * @port: port number used for VXLAN
+ *
+ * This funciton is called when a new VXLAN interface has added a new port
+ * number to the range that is currently in use for VXLAN.  The new port
+ * number is always added to the tail so that the port number list should
+ * match the order in which the ports were allocated.  The head of the list
+ * is always used as the VXLAN port number for offloads.
+ **/
+static void fm10k_add_vxlan_port(struct net_device *dev,
+				 sa_family_t sa_family, __be16 port) {
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_vxlan_port *vxlan_port;
+
+	/* only the PF supports configuring tunnels */
+	if (interface->hw.mac.type != fm10k_mac_pf)
+		return;
+
+	/* existing ports are pulled out so our new entry is always last */
+	fm10k_vxlan_port_for_each(vxlan_port, interface) {
+		if ((vxlan_port->port == port) &&
+		    (vxlan_port->sa_family == sa_family)) {
+			list_del(&vxlan_port->list);
+			goto insert_tail;
+		}
+	}
+
+	/* allocate memory to track ports */
+	vxlan_port = kmalloc(sizeof(*vxlan_port), GFP_ATOMIC);
+	if (!vxlan_port)
+		return;
+	vxlan_port->port = port;
+	vxlan_port->sa_family = sa_family;
+
+insert_tail:
+	/* add new port value to list */
+	list_add_tail(&vxlan_port->list, &interface->vxlan_port);
+
+	fm10k_restore_vxlan_port(interface);
+}
+
+/**
+ * fm10k_del_vxlan_port
+ * @netdev: network interface device structure
+ * @sa_family: Address family of freed port
+ * @port: port number used for VXLAN
+ *
+ * This funciton is called when a new VXLAN interface has freed a port
+ * number from the range that is currently in use for VXLAN.  The freed
+ * port is removed from the list and the new head is used to determine
+ * the port number for offloads.
+ **/
+static void fm10k_del_vxlan_port(struct net_device *dev,
+				 sa_family_t sa_family, __be16 port) {
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_vxlan_port *vxlan_port;
+
+	if (interface->hw.mac.type != fm10k_mac_pf)
+		return;
+
+	/* find the port in the list and free it */
+	fm10k_vxlan_port_for_each(vxlan_port, interface) {
+		if ((vxlan_port->port == port) &&
+		    (vxlan_port->sa_family == sa_family)) {
+			list_del(&vxlan_port->list);
+			kfree(vxlan_port);
+			break;
+		}
+	}
+
+	fm10k_restore_vxlan_port(interface);
+}
+
+/**
+ * fm10k_open - Called when a network interface is made active
+ * @netdev: network interface device structure
+ *
+ * Returns 0 on success, negative value on failure
+ *
+ * The open entry point is called when a network interface is made
+ * active by the system (IFF_UP).  At this point all resources needed
+ * for transmit and receive operations are allocated, the interrupt
+ * handler is registered with the OS, the watchdog timer is started,
+ * and the stack is notified that the interface is ready.
+ **/
+int fm10k_open(struct net_device *netdev)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	int err;
+
+	/* allocate transmit descriptors */
+	err = fm10k_setup_all_tx_resources(interface);
+	if (err)
+		goto err_setup_tx;
+
+	/* allocate receive descriptors */
+	err = fm10k_setup_all_rx_resources(interface);
+	if (err)
+		goto err_setup_rx;
+
+	/* allocate interrupt resources */
+	err = fm10k_qv_request_irq(interface);
+	if (err)
+		goto err_req_irq;
+
+	/* setup GLORT assignment for this port */
+	fm10k_request_glort_range(interface);
+
+	/* Notify the stack of the actual queue counts */
+	err = netif_set_real_num_tx_queues(netdev,
+					   interface->num_tx_queues);
+	if (err)
+		goto err_set_queues;
+
+	err = netif_set_real_num_rx_queues(netdev,
+					   interface->num_rx_queues);
+	if (err)
+		goto err_set_queues;
+
+#if IS_ENABLED(CONFIG_FM10K_VXLAN)
+	/* update VXLAN port configuration */
+	vxlan_get_rx_port(netdev);
+
+#endif
+	fm10k_up(interface);
+
+	return 0;
+
+err_set_queues:
+	fm10k_qv_free_irq(interface);
+err_req_irq:
+	fm10k_free_all_rx_resources(interface);
+err_setup_rx:
+	fm10k_free_all_tx_resources(interface);
+err_setup_tx:
+	return err;
+}
+
+/**
+ * fm10k_close - Disables a network interface
+ * @netdev: network interface device structure
+ *
+ * Returns 0, this is not allowed to fail
+ *
+ * The close entry point is called when an interface is de-activated
+ * by the OS.  The hardware is still under the drivers control, but
+ * needs to be disabled.  A global MAC reset is issued to stop the
+ * hardware, and all transmit and receive resources are freed.
+ **/
+int fm10k_close(struct net_device *netdev)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+
+	fm10k_down(interface);
+
+	fm10k_qv_free_irq(interface);
+
+	fm10k_del_vxlan_port_all(interface);
+
+	fm10k_free_all_tx_resources(interface);
+	fm10k_free_all_rx_resources(interface);
+
+	return 0;
+}
+
+static netdev_tx_t fm10k_xmit_frame(struct sk_buff *skb, struct net_device *dev)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	unsigned int r_idx = skb->queue_mapping;
+	int err;
+
+	if ((skb->protocol ==  htons(ETH_P_8021Q)) &&
+	    !vlan_tx_tag_present(skb)) {
+		/* FM10K only supports hardware tagging, any tags in frame
+		 * are considered 2nd level or "outer" tags
+		 */
+		struct vlan_hdr *vhdr;
+		__be16 proto;
+
+		/* make sure skb is not shared */
+		skb = skb_share_check(skb, GFP_ATOMIC);
+		if (!skb)
+			return NETDEV_TX_OK;
+
+		/* make sure there is enough room to move the ethernet header */
+		if (unlikely(!pskb_may_pull(skb, VLAN_ETH_HLEN)))
+			return NETDEV_TX_OK;
+
+		/* verify the skb head is not shared */
+		err = skb_cow_head(skb, 0);
+		if (err)
+			return NETDEV_TX_OK;
+
+		/* locate vlan header */
+		vhdr = (struct vlan_hdr *)(skb->data + ETH_HLEN);
+
+		/* pull the 2 key pieces of data out of it */
+		__vlan_hwaccel_put_tag(skb,
+				       htons(ETH_P_8021Q),
+				       ntohs(vhdr->h_vlan_TCI));
+		proto = vhdr->h_vlan_encapsulated_proto;
+		skb->protocol = (ntohs(proto) >= 1536) ? proto :
+							 htons(ETH_P_802_2);
+
+		/* squash it by moving the ethernet addresses up 4 bytes */
+		memmove(skb->data + VLAN_HLEN, skb->data, 12);
+		__skb_pull(skb, VLAN_HLEN);
+		skb_reset_mac_header(skb);
+	}
+
+	/* The minimum packet size for a single buffer is 17B so pad the skb
+	 * in order to meet this minimum size requirement.
+	 */
+	if (unlikely(skb->len < 17)) {
+		int pad_len = 17 - skb->len;
+
+		if (skb_pad(skb, pad_len))
+			return NETDEV_TX_OK;
+		__skb_put(skb, pad_len);
+	}
+
+	/* prepare packet for hardware time stamping */
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP))
+		fm10k_ts_tx_enqueue(interface, skb);
+
+	if (r_idx >= interface->num_tx_queues)
+		r_idx %= interface->num_tx_queues;
+
+	err = fm10k_xmit_frame_ring(skb, interface->tx_ring[r_idx]);
+
+	return err;
+}
+
+static int fm10k_change_mtu(struct net_device *dev, int new_mtu)
+{
+	if (new_mtu < 68 || new_mtu > FM10K_MAX_JUMBO_FRAME_SIZE)
+		return -EINVAL;
+
+	dev->mtu = new_mtu;
+
+	return 0;
+}
+
+/**
+ * fm10k_tx_timeout - Respond to a Tx Hang
+ * @netdev: network interface device structure
+ **/
+static void fm10k_tx_timeout(struct net_device *netdev)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	bool real_tx_hang = false;
+	int i;
+
+#define TX_TIMEO_LIMIT 16000
+	for (i = 0; i < interface->num_tx_queues; i++) {
+		struct fm10k_ring *tx_ring = interface->tx_ring[i];
+
+		if (check_for_tx_hang(tx_ring) && fm10k_check_tx_hang(tx_ring))
+			real_tx_hang = true;
+	}
+
+	if (real_tx_hang) {
+		fm10k_tx_timeout_reset(interface);
+	} else {
+		netif_info(interface, drv, netdev,
+			   "Fake Tx hang detected with timeout of %d seconds\n",
+			   netdev->watchdog_timeo/HZ);
+
+		/* fake Tx hang - increase the kernel timeout */
+		if (netdev->watchdog_timeo < TX_TIMEO_LIMIT)
+			netdev->watchdog_timeo *= 2;
+	}
+}
+
+static int fm10k_uc_vlan_unsync(struct net_device *netdev,
+				const unsigned char *uc_addr)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_hw *hw = &interface->hw;
+	u16 glort = interface->glort;
+	u16 vid = interface->vid;
+	bool set = !!(vid / VLAN_N_VID);
+	int err;
+
+	/* drop any leading bits on the VLAN ID */
+	vid &= VLAN_N_VID - 1;
+
+	err = hw->mac.ops.update_uc_addr(hw, glort, uc_addr, vid, set, 0);
+	if (err)
+		return err;
+
+	/* return non-zero value as we are only doing a partial sync/unsync */
+	return 1;
+}
+
+static int fm10k_mc_vlan_unsync(struct net_device *netdev,
+				const unsigned char *mc_addr)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_hw *hw = &interface->hw;
+	u16 glort = interface->glort;
+	u16 vid = interface->vid;
+	bool set = !!(vid / VLAN_N_VID);
+	int err;
+
+	/* drop any leading bits on the VLAN ID */
+	vid &= VLAN_N_VID - 1;
+
+	err = hw->mac.ops.update_mc_addr(hw, glort, mc_addr, vid, set);
+	if (err)
+		return err;
+
+	/* return non-zero value as we are only doing a partial sync/unsync */
+	return 1;
+}
+
+static int fm10k_update_vid(struct net_device *netdev, u16 vid, bool set)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_hw *hw = &interface->hw;
+	s32 err;
+
+	/* updates do not apply to VLAN 0 */
+	if (!vid)
+		return 0;
+
+	if (vid >= VLAN_N_VID)
+		return -EINVAL;
+
+	/* Verify we have permission to add VLANs */
+	if (hw->mac.vlan_override)
+		return -EACCES;
+
+	/* if default VLAN is already present do nothing */
+	if (vid == hw->mac.default_vid)
+		return -EBUSY;
+
+	/* update active_vlans bitmask */
+	set_bit(vid, interface->active_vlans);
+	if (!set)
+		clear_bit(vid, interface->active_vlans);
+
+	fm10k_mbx_lock(interface);
+
+	/* only need to update the VLAN if not in promiscous mode */
+	if (!(netdev->flags & IFF_PROMISC)) {
+		err = hw->mac.ops.update_vlan(hw, vid, 0, set);
+		if (err)
+			goto err_out;
+	}
+
+	/* update our base MAC address */
+	err = hw->mac.ops.update_uc_addr(hw, interface->glort, hw->mac.addr,
+					 vid, set, 0);
+	if (err)
+		goto err_out;
+
+	/* set vid prior to syncing/unsyncing the VLAN */
+	interface->vid = vid + (set ? VLAN_N_VID : 0);
+
+	/* Update the unicast and multicast address list to add/drop VLAN */
+	__dev_uc_unsync(netdev, fm10k_uc_vlan_unsync);
+	__dev_mc_unsync(netdev, fm10k_mc_vlan_unsync);
+
+err_out:
+	fm10k_mbx_unlock(interface);
+
+	return err;
+}
+
+static int fm10k_vlan_rx_add_vid(struct net_device *netdev,
+				 __always_unused __be16 proto, u16 vid)
+{
+	/* update VLAN and address table based on changes */
+	return fm10k_update_vid(netdev, vid, true);
+}
+
+static int fm10k_vlan_rx_kill_vid(struct net_device *netdev,
+				  __always_unused __be16 proto, u16 vid)
+{
+	/* update VLAN and address table based on changes */
+	return fm10k_update_vid(netdev, vid, false);
+}
+
+static u16 fm10k_find_next_vlan(struct fm10k_intfc *interface, u16 vid)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	u16 default_vid = hw->mac.default_vid;
+	u16 vid_limit = vid < default_vid ? default_vid : VLAN_N_VID;
+
+	vid = find_next_bit(interface->active_vlans, vid_limit, ++vid);
+
+	return vid;
+}
+
+static void fm10k_clear_unused_vlans(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	u32 vid, prev_vid;
+
+	/* loop through and find any gaps in the table */
+	for (vid = 0, prev_vid = 0;
+	     prev_vid < VLAN_N_VID;
+	     prev_vid = vid + 1, vid = fm10k_find_next_vlan(interface, vid)) {
+		if (prev_vid == vid)
+			continue;
+
+		/* send request to clear multiple bits at a time */
+		prev_vid += (vid - prev_vid - 1) << FM10K_VLAN_LENGTH_SHIFT;
+		hw->mac.ops.update_vlan(hw, prev_vid, 0, false);
+	}
+}
+
+static int __fm10k_uc_sync(struct net_device *dev,
+			   const unsigned char *addr, bool sync)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+	u16 vid, glort = interface->glort;
+	s32 err;
+
+	if (!is_valid_ether_addr(addr))
+		return -EADDRNOTAVAIL;
+
+	/* update table with current entries */
+	for (vid = hw->mac.default_vid ? fm10k_find_next_vlan(interface, 0) : 0;
+	     vid < VLAN_N_VID;
+	     vid = fm10k_find_next_vlan(interface, vid)) {
+		err = hw->mac.ops.update_uc_addr(hw, glort, addr,
+						  vid, sync, 0);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int fm10k_uc_sync(struct net_device *dev,
+			 const unsigned char *addr)
+{
+	return __fm10k_uc_sync(dev, addr, true);
+}
+
+static int fm10k_uc_unsync(struct net_device *dev,
+			   const unsigned char *addr)
+{
+	return __fm10k_uc_sync(dev, addr, false);
+}
+
+static int fm10k_set_mac(struct net_device *dev, void *p)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+	struct sockaddr *addr = p;
+	s32 err = 0;
+
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
+	if (dev->flags & IFF_UP) {
+		/* setting MAC address requires mailbox */
+		fm10k_mbx_lock(interface);
+
+		err = fm10k_uc_sync(dev, addr->sa_data);
+		if (!err)
+			fm10k_uc_unsync(dev, hw->mac.addr);
+
+		fm10k_mbx_unlock(interface);
+	}
+
+	if (!err) {
+		ether_addr_copy(dev->dev_addr, addr->sa_data);
+		ether_addr_copy(hw->mac.addr, addr->sa_data);
+		dev->addr_assign_type &= ~NET_ADDR_RANDOM;
+	}
+
+	/* if we had a mailbox error suggest trying again */
+	return err ? -EAGAIN : 0;
+}
+
+static int __fm10k_mc_sync(struct net_device *dev,
+			   const unsigned char *addr, bool sync)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+	u16 vid, glort = interface->glort;
+	s32 err;
+
+	if (!is_multicast_ether_addr(addr))
+		return -EADDRNOTAVAIL;
+
+	/* update table with current entries */
+	for (vid = hw->mac.default_vid ? fm10k_find_next_vlan(interface, 0) : 0;
+	     vid < VLAN_N_VID;
+	     vid = fm10k_find_next_vlan(interface, vid)) {
+		err = hw->mac.ops.update_mc_addr(hw, glort, addr, vid, sync);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+static int fm10k_mc_sync(struct net_device *dev,
+			 const unsigned char *addr)
+{
+	return __fm10k_mc_sync(dev, addr, true);
+}
+
+static int fm10k_mc_unsync(struct net_device *dev,
+			   const unsigned char *addr)
+{
+	return __fm10k_mc_sync(dev, addr, false);
+}
+
+static void fm10k_set_rx_mode(struct net_device *dev)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_hw *hw = &interface->hw;
+	int xcast_mode;
+
+	/* no need to update the harwdare if we are not running */
+	if (!(dev->flags & IFF_UP))
+		return;
+
+	/* determine new mode based on flags */
+	xcast_mode = (dev->flags & IFF_PROMISC) ? FM10K_XCAST_MODE_PROMISC :
+		     (dev->flags & IFF_ALLMULTI) ? FM10K_XCAST_MODE_ALLMULTI :
+		     (dev->flags & (IFF_BROADCAST | IFF_MULTICAST)) ?
+		     FM10K_XCAST_MODE_MULTI : FM10K_XCAST_MODE_NONE;
+
+	fm10k_mbx_lock(interface);
+
+	/* syncronize all of the addresses */
+	if (xcast_mode != FM10K_XCAST_MODE_PROMISC) {
+		__dev_uc_sync(dev, fm10k_uc_sync, fm10k_uc_unsync);
+		if (xcast_mode != FM10K_XCAST_MODE_ALLMULTI)
+			__dev_mc_sync(dev, fm10k_mc_sync, fm10k_mc_unsync);
+	}
+
+	/* if we aren't changing modes there is nothing to do */
+	if (interface->xcast_mode != xcast_mode) {
+		/* update VLAN table */
+		if (xcast_mode == FM10K_XCAST_MODE_PROMISC)
+			hw->mac.ops.update_vlan(hw, FM10K_VLAN_ALL, 0, true);
+		if (interface->xcast_mode == FM10K_XCAST_MODE_PROMISC)
+			fm10k_clear_unused_vlans(interface);
+
+		/* update xcast mode */
+		hw->mac.ops.update_xcast_mode(hw, interface->glort, xcast_mode);
+
+		/* record updated xcast mode state */
+		interface->xcast_mode = xcast_mode;
+	}
+
+	fm10k_mbx_unlock(interface);
+}
+
+void fm10k_restore_rx_state(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	int xcast_mode;
+	u16 vid, glort;
+
+	/* restore our address if perm_addr is set */
+	if (hw->mac.type == fm10k_mac_vf) {
+		if (is_valid_ether_addr(hw->mac.perm_addr)) {
+			ether_addr_copy(hw->mac.addr, hw->mac.perm_addr);
+			ether_addr_copy(netdev->perm_addr, hw->mac.perm_addr);
+			ether_addr_copy(netdev->dev_addr, hw->mac.perm_addr);
+			netdev->addr_assign_type &= ~NET_ADDR_RANDOM;
+		}
+
+		if (hw->mac.vlan_override)
+			netdev->features &= ~NETIF_F_HW_VLAN_CTAG_RX;
+		else
+			netdev->features |= NETIF_F_HW_VLAN_CTAG_RX;
+	}
+
+	/* record glort for this interface */
+	glort = interface->glort;
+
+	/* convert interface flags to xcast mode */
+	if (netdev->flags & IFF_PROMISC)
+		xcast_mode = FM10K_XCAST_MODE_PROMISC;
+	else if (netdev->flags & IFF_ALLMULTI)
+		xcast_mode = FM10K_XCAST_MODE_ALLMULTI;
+	else if (netdev->flags & (IFF_BROADCAST | IFF_MULTICAST))
+		xcast_mode = FM10K_XCAST_MODE_MULTI;
+	else
+		xcast_mode = FM10K_XCAST_MODE_NONE;
+
+	fm10k_mbx_lock(interface);
+
+	/* Enable logical port */
+	hw->mac.ops.update_lport_state(hw, glort, interface->glort_count, true);
+
+	/* update VLAN table */
+	hw->mac.ops.update_vlan(hw, FM10K_VLAN_ALL, 0,
+				xcast_mode == FM10K_XCAST_MODE_PROMISC);
+
+	/* Add filter for VLAN 0 */
+	hw->mac.ops.update_vlan(hw, 0, 0, true);
+
+	/* update table with current entries */
+	for (vid = hw->mac.default_vid ? fm10k_find_next_vlan(interface, 0) : 0;
+	     vid < VLAN_N_VID;
+	     vid = fm10k_find_next_vlan(interface, vid)) {
+		hw->mac.ops.update_vlan(hw, vid, 0, true);
+		hw->mac.ops.update_uc_addr(hw, glort, hw->mac.addr,
+					   vid, true, 0);
+	}
+
+	/* syncronize all of the addresses */
+	if (xcast_mode != FM10K_XCAST_MODE_PROMISC) {
+		__dev_uc_sync(netdev, fm10k_uc_sync, fm10k_uc_unsync);
+		if (xcast_mode != FM10K_XCAST_MODE_ALLMULTI)
+			__dev_mc_sync(netdev, fm10k_mc_sync, fm10k_mc_unsync);
+	}
+
+	/* update xcast mode */
+	hw->mac.ops.update_xcast_mode(hw, glort, xcast_mode);
+
+	fm10k_mbx_unlock(interface);
+
+	/* record updated xcast mode state */
+	interface->xcast_mode = xcast_mode;
+
+	/* Restore tunnel configuration */
+	fm10k_restore_vxlan_port(interface);
+}
+
+void fm10k_reset_rx_state(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+
+	fm10k_mbx_lock(interface);
+
+	/* clear the logical port state on lower device */
+	hw->mac.ops.update_lport_state(hw, interface->glort,
+				       interface->glort_count, false);
+
+	fm10k_mbx_unlock(interface);
+
+	/* reset flags to default state */
+	interface->xcast_mode = FM10K_XCAST_MODE_NONE;
+
+	/* clear the sync flag since the lport has been dropped */
+	__dev_uc_unsync(netdev, NULL);
+	__dev_mc_unsync(netdev, NULL);
+}
+
+/**
+ * fm10k_get_stats64 - Get System Network Statistics
+ * @netdev: network interface device structure
+ * @stats: storage space for 64bit statistics
+ *
+ * Returns 64bit statistics, for use in the ndo_get_stats64 callback. This
+ * function replaces fm10k_get_stats for kernels which support it.
+ */
+static struct rtnl_link_stats64 *fm10k_get_stats64(struct net_device *netdev,
+						   struct rtnl_link_stats64 *stats)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct fm10k_ring *ring;
+	unsigned int start, i;
+	u64 bytes, packets;
+
+	rcu_read_lock();
+
+	for (i = 0; i < interface->num_rx_queues; i++) {
+		ring = ACCESS_ONCE(interface->rx_ring[i]);
+
+		if (!ring)
+			continue;
+
+		do {
+			start = u64_stats_fetch_begin_irq(&ring->syncp);
+			packets = ring->stats.packets;
+			bytes   = ring->stats.bytes;
+		} while (u64_stats_fetch_retry_irq(&ring->syncp, start));
+
+		stats->rx_packets += packets;
+		stats->rx_bytes   += bytes;
+	}
+
+	for (i = 0; i < interface->num_tx_queues; i++) {
+		ring = ACCESS_ONCE(interface->rx_ring[i]);
+
+		if (!ring)
+			continue;
+
+		do {
+			start = u64_stats_fetch_begin_irq(&ring->syncp);
+			packets = ring->stats.packets;
+			bytes   = ring->stats.bytes;
+		} while (u64_stats_fetch_retry_irq(&ring->syncp, start));
+
+		stats->tx_packets += packets;
+		stats->tx_bytes   += bytes;
+	}
+
+	rcu_read_unlock();
+
+	/* following stats updated by fm10k_service_task() */
+	stats->rx_missed_errors	= netdev->stats.rx_missed_errors;
+
+	return stats;
+}
+
+int fm10k_setup_tc(struct net_device *dev, u8 tc)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+
+	/* Currently only the PF supports priority classes */
+	if (tc && (interface->hw.mac.type != fm10k_mac_pf))
+		return -EINVAL;
+
+	/* Hardware supports up to 8 traffic classes */
+	if (tc > 8)
+		return -EINVAL;
+
+	/* Hardware has to reinitialize queues to match packet
+	 * buffer alignment. Unfortunately, the hardware is not
+	 * flexible enough to do this dynamically.
+	 */
+	if (netif_running(dev))
+		fm10k_close(dev);
+
+	fm10k_mbx_free_irq(interface);
+
+	fm10k_clear_queueing_scheme(interface);
+
+	/* we expect the prio_tc map to be repopulated later */
+	netdev_reset_tc(dev);
+	netdev_set_num_tc(dev, tc);
+
+	fm10k_init_queueing_scheme(interface);
+
+	fm10k_mbx_request_irq(interface);
+
+	if (netif_running(dev))
+		fm10k_open(dev);
+
+	/* flag to indicate SWPRI has yet to be updated */
+	interface->flags |= FM10K_FLAG_SWPRI_CONFIG;
+
+	return 0;
+}
+
+static int fm10k_ioctl(struct net_device *netdev, struct ifreq *ifr, int cmd)
+{
+	switch (cmd) {
+	case SIOCGHWTSTAMP:
+		return fm10k_get_ts_config(netdev, ifr);
+	case SIOCSHWTSTAMP:
+		return fm10k_set_ts_config(netdev, ifr);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+static void fm10k_assign_l2_accel(struct fm10k_intfc *interface,
+				  struct fm10k_l2_accel *l2_accel)
+{
+	struct fm10k_ring *ring;
+	int i;
+
+	for (i = 0; i < interface->num_rx_queues; i++) {
+		ring = interface->rx_ring[i];
+		rcu_assign_pointer(ring->l2_accel, l2_accel);
+	}
+
+	interface->l2_accel = l2_accel;
+}
+
+static void *fm10k_dfwd_add_station(struct net_device *dev,
+				    struct net_device *sdev)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_l2_accel *l2_accel = interface->l2_accel;
+	struct fm10k_l2_accel *old_l2_accel = NULL;
+	struct fm10k_dglort_cfg dglort = { 0 };
+	struct fm10k_hw *hw = &interface->hw;
+	int size = 0, i;
+	u16 glort;
+
+	/* allocate l2 accel structure if it is not available */
+	if (!l2_accel) {
+		/* verify there is enough free GLORTs to support l2_accel */
+		if (interface->glort_count < 7)
+			return ERR_PTR(-EBUSY);
+
+		size = offsetof(struct fm10k_l2_accel, macvlan[7]);
+		l2_accel = kzalloc(size, GFP_KERNEL);
+		if (!l2_accel)
+			return ERR_PTR(-ENOMEM);
+
+		l2_accel->size = 7;
+		l2_accel->dglort = interface->glort;
+
+		/* update pointers */
+		fm10k_assign_l2_accel(interface, l2_accel);
+	/* do not expand if we are at our limit */
+	} else if ((l2_accel->count == FM10K_MAX_STATIONS) ||
+		   (l2_accel->count == (interface->glort_count - 1))) {
+		return ERR_PTR(-EBUSY);
+	/* expand if we have hit the size limit */
+	} else if (l2_accel->count == l2_accel->size) {
+		old_l2_accel = l2_accel;
+		size = offsetof(struct fm10k_l2_accel,
+				macvlan[(l2_accel->size * 2) + 1]);
+		l2_accel = kzalloc(size, GFP_KERNEL);
+		if (!l2_accel)
+			return ERR_PTR(-ENOMEM);
+
+		memcpy(l2_accel, old_l2_accel,
+		       offsetof(struct fm10k_l2_accel,
+				macvlan[old_l2_accel->size]));
+
+		l2_accel->size = (old_l2_accel->size * 2) + 1;
+
+		/* update pointers */
+		fm10k_assign_l2_accel(interface, l2_accel);
+		kfree_rcu(old_l2_accel, rcu);
+	}
+
+	/* add macvlan to accel table, and record GLORT for position */
+	for (i = 0; i < l2_accel->size; i++) {
+		if (!l2_accel->macvlan[i])
+			break;
+	}
+
+	/* record station */
+	l2_accel->macvlan[i] = sdev;
+	l2_accel->count++;
+
+	/* configure default DGLORT mapping for RSS/DCB */
+	dglort.idx = fm10k_dglort_pf_rss;
+	dglort.inner_rss = 1;
+	dglort.rss_l = fls(interface->ring_feature[RING_F_RSS].mask);
+	dglort.pc_l = fls(interface->ring_feature[RING_F_QOS].mask);
+	dglort.glort = interface->glort;
+	dglort.shared_l = fls(l2_accel->size);
+	hw->mac.ops.configure_dglort_map(hw, &dglort);
+
+	/* Add rules for this specific dglort to the switch */
+	fm10k_mbx_lock(interface);
+
+	glort = l2_accel->dglort + 1 + i;
+	hw->mac.ops.update_xcast_mode(hw, glort, FM10K_XCAST_MODE_MULTI);
+	hw->mac.ops.update_uc_addr(hw, glort, sdev->dev_addr, 0, true, 0);
+
+	fm10k_mbx_unlock(interface);
+
+	return sdev;
+}
+
+static void fm10k_dfwd_del_station(struct net_device *dev, void *priv)
+{
+	struct fm10k_intfc *interface = netdev_priv(dev);
+	struct fm10k_l2_accel *l2_accel = ACCESS_ONCE(interface->l2_accel);
+	struct fm10k_dglort_cfg dglort = { 0 };
+	struct fm10k_hw *hw = &interface->hw;
+	struct net_device *sdev = priv;
+	int i;
+	u16 glort;
+
+	if (!l2_accel)
+		return;
+
+	/* search table for matching interface */
+	for (i = 0; i < l2_accel->size; i++) {
+		if (l2_accel->macvlan[i] == sdev)
+			break;
+	}
+
+	/* exit if macvlan not found */
+	if (i == l2_accel->size)
+		return;
+
+	/* Remove any rules specific to this dglort */
+	fm10k_mbx_lock(interface);
+
+	glort = l2_accel->dglort + 1 + i;
+	hw->mac.ops.update_xcast_mode(hw, glort, FM10K_XCAST_MODE_NONE);
+	hw->mac.ops.update_uc_addr(hw, glort, sdev->dev_addr, 0, false, 0);
+
+	fm10k_mbx_unlock(interface);
+
+	/* record removal */
+	l2_accel->macvlan[i] = NULL;
+	l2_accel->count--;
+
+	/* configure default DGLORT mapping for RSS/DCB */
+	dglort.idx = fm10k_dglort_pf_rss;
+	dglort.inner_rss = 1;
+	dglort.rss_l = fls(interface->ring_feature[RING_F_RSS].mask);
+	dglort.pc_l = fls(interface->ring_feature[RING_F_QOS].mask);
+	dglort.glort = interface->glort;
+	if (l2_accel)
+		dglort.shared_l = fls(l2_accel->size);
+	hw->mac.ops.configure_dglort_map(hw, &dglort);
+
+	/* If table is empty remove it */
+	if (l2_accel->count == 0) {
+		fm10k_assign_l2_accel(interface, NULL);
+		kfree_rcu(l2_accel, rcu);
+	}
+}
+
+static const struct net_device_ops fm10k_netdev_ops = {
+	.ndo_open		= fm10k_open,
+	.ndo_stop		= fm10k_close,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_start_xmit		= fm10k_xmit_frame,
+	.ndo_set_mac_address	= fm10k_set_mac,
+	.ndo_change_mtu		= fm10k_change_mtu,
+	.ndo_tx_timeout		= fm10k_tx_timeout,
+	.ndo_vlan_rx_add_vid	= fm10k_vlan_rx_add_vid,
+	.ndo_vlan_rx_kill_vid	= fm10k_vlan_rx_kill_vid,
+	.ndo_set_rx_mode	= fm10k_set_rx_mode,
+	.ndo_get_stats64	= fm10k_get_stats64,
+	.ndo_setup_tc		= fm10k_setup_tc,
+	.ndo_set_vf_mac		= fm10k_ndo_set_vf_mac,
+	.ndo_set_vf_vlan	= fm10k_ndo_set_vf_vlan,
+	.ndo_set_vf_rate	= fm10k_ndo_set_vf_bw,
+	.ndo_get_vf_config	= fm10k_ndo_get_vf_config,
+	.ndo_add_vxlan_port	= fm10k_add_vxlan_port,
+	.ndo_del_vxlan_port	= fm10k_del_vxlan_port,
+	.ndo_do_ioctl		= fm10k_ioctl,
+	.ndo_dfwd_add_station	= fm10k_dfwd_add_station,
+	.ndo_dfwd_del_station	= fm10k_dfwd_del_station,
+};
+
+#define DEFAULT_DEBUG_LEVEL_SHIFT 3
+
+struct net_device *fm10k_alloc_netdev(void)
+{
+	struct fm10k_intfc *interface;
+	struct net_device *dev;
+
+	dev = alloc_etherdev_mq(sizeof(struct fm10k_intfc), MAX_QUEUES);
+	if (!dev)
+		return NULL;
+
+	/* set net device and ethtool ops */
+	dev->netdev_ops = &fm10k_netdev_ops;
+	fm10k_set_ethtool_ops(dev);
+
+	/* configure default debug level */
+	interface = netdev_priv(dev);
+	interface->msg_enable = (1 << DEFAULT_DEBUG_LEVEL_SHIFT) - 1;
+
+	/* configure default features */
+	dev->features |= NETIF_F_IP_CSUM |
+			 NETIF_F_IPV6_CSUM |
+			 NETIF_F_SG |
+			 NETIF_F_TSO |
+			 NETIF_F_TSO6 |
+			 NETIF_F_TSO_ECN |
+			 NETIF_F_GSO_UDP_TUNNEL |
+			 NETIF_F_RXHASH |
+			 NETIF_F_RXCSUM;
+
+	/* all features defined to this point should be changeable */
+	dev->hw_features |= dev->features;
+
+	/* allow user to enable L2 forwarding acceleration */
+	dev->hw_features |= NETIF_F_HW_L2FW_DOFFLOAD;
+
+	/* configure VLAN features */
+	dev->vlan_features |= dev->features;
+
+	/* configure tunnel offloads */
+	dev->hw_enc_features = NETIF_F_IP_CSUM |
+			       NETIF_F_TSO |
+			       NETIF_F_TSO6 |
+			       NETIF_F_TSO_ECN |
+			       NETIF_F_GSO_UDP_TUNNEL |
+			       NETIF_F_IPV6_CSUM |
+			       NETIF_F_SG;
+
+	/* we want to leave these both on as we cannot disable VLAN tag
+	 * insertion or stripping on the hardware since it is contained
+	 * in the FTAG and not in the frame itself.
+	 */
+	dev->features |= NETIF_F_HW_VLAN_CTAG_TX |
+			 NETIF_F_HW_VLAN_CTAG_RX |
+			 NETIF_F_HW_VLAN_CTAG_FILTER;
+
+	dev->priv_flags |= IFF_UNICAST_FLT;
+
+	return dev;
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pci.c b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
new file mode 100644
index 000000000000..a0cb74ab3dc6
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pci.c
@@ -0,0 +1,2167 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include <linux/module.h>
+#include <linux/aer.h>
+
+#include "fm10k.h"
+
+static const struct fm10k_info *fm10k_info_tbl[] = {
+	[fm10k_device_pf] = &fm10k_pf_info,
+	[fm10k_device_vf] = &fm10k_vf_info,
+};
+
+/**
+ * fm10k_pci_tbl - PCI Device ID Table
+ *
+ * Wildcard entries (PCI_ANY_ID) should come last
+ * Last entry must be all 0s
+ *
+ * { Vendor ID, Device ID, SubVendor ID, SubDevice ID,
+ *   Class, Class Mask, private data (not used) }
+ */
+static const struct pci_device_id fm10k_pci_tbl[] = {
+	{ PCI_VDEVICE(INTEL, FM10K_DEV_ID_PF), fm10k_device_pf },
+	{ PCI_VDEVICE(INTEL, FM10K_DEV_ID_VF), fm10k_device_vf },
+	/* required last entry */
+	{ 0, }
+};
+MODULE_DEVICE_TABLE(pci, fm10k_pci_tbl);
+
+u16 fm10k_read_pci_cfg_word(struct fm10k_hw *hw, u32 reg)
+{
+	struct fm10k_intfc *interface = hw->back;
+	u16 value = 0;
+
+	if (FM10K_REMOVED(hw->hw_addr))
+		return ~value;
+
+	pci_read_config_word(interface->pdev, reg, &value);
+	if (value == 0xFFFF)
+		fm10k_write_flush(hw);
+
+	return value;
+}
+
+u32 fm10k_read_reg(struct fm10k_hw *hw, int reg)
+{
+	u32 __iomem *hw_addr = ACCESS_ONCE(hw->hw_addr);
+	u32 value = 0;
+
+	if (FM10K_REMOVED(hw_addr))
+		return ~value;
+
+	value = readl(&hw_addr[reg]);
+	if (!(~value) && (!reg || !(~readl(hw_addr)))) {
+		struct fm10k_intfc *interface = hw->back;
+		struct net_device *netdev = interface->netdev;
+
+		hw->hw_addr = NULL;
+		netif_device_detach(netdev);
+		netdev_err(netdev, "PCIe link lost, device now detached\n");
+	}
+
+	return value;
+}
+
+static int fm10k_hw_ready(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+
+	fm10k_write_flush(hw);
+
+	return FM10K_REMOVED(hw->hw_addr) ? -ENODEV : 0;
+}
+
+void fm10k_service_event_schedule(struct fm10k_intfc *interface)
+{
+	if (!test_bit(__FM10K_SERVICE_DISABLE, &interface->state) &&
+	    !test_and_set_bit(__FM10K_SERVICE_SCHED, &interface->state))
+		schedule_work(&interface->service_task);
+}
+
+static void fm10k_service_event_complete(struct fm10k_intfc *interface)
+{
+	BUG_ON(!test_bit(__FM10K_SERVICE_SCHED, &interface->state));
+
+	/* flush memory to make sure state is correct before next watchog */
+	smp_mb__before_atomic();
+	clear_bit(__FM10K_SERVICE_SCHED, &interface->state);
+}
+
+/**
+ * fm10k_service_timer - Timer Call-back
+ * @data: pointer to interface cast into an unsigned long
+ **/
+static void fm10k_service_timer(unsigned long data)
+{
+	struct fm10k_intfc *interface = (struct fm10k_intfc *)data;
+
+	/* Reset the timer */
+	mod_timer(&interface->service_timer, (HZ * 2) + jiffies);
+
+	fm10k_service_event_schedule(interface);
+}
+
+static void fm10k_detach_subtask(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+
+	/* do nothing if device is still present or hw_addr is set */
+	if (netif_device_present(netdev) || interface->hw.hw_addr)
+		return;
+
+	rtnl_lock();
+
+	if (netif_running(netdev))
+		dev_close(netdev);
+
+	rtnl_unlock();
+}
+
+static void fm10k_reinit(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	int err;
+
+	WARN_ON(in_interrupt());
+
+	/* put off any impending NetWatchDogTimeout */
+	netdev->trans_start = jiffies;
+
+	while (test_and_set_bit(__FM10K_RESETTING, &interface->state))
+		usleep_range(1000, 2000);
+
+	rtnl_lock();
+
+	fm10k_iov_suspend(interface->pdev);
+
+	if (netif_running(netdev))
+		fm10k_close(netdev);
+
+	fm10k_mbx_free_irq(interface);
+
+	/* delay any future reset requests */
+	interface->last_reset = jiffies + (10 * HZ);
+
+	/* reset and initialize the hardware so it is in a known state */
+	err = hw->mac.ops.reset_hw(hw) ? : hw->mac.ops.init_hw(hw);
+	if (err)
+		dev_err(&interface->pdev->dev, "init_hw failed: %d\n", err);
+
+	/* reassociate interrupts */
+	fm10k_mbx_request_irq(interface);
+
+	/* reset clock */
+	fm10k_ts_reset(interface);
+
+	if (netif_running(netdev))
+		fm10k_open(netdev);
+
+	fm10k_iov_resume(interface->pdev);
+
+	rtnl_unlock();
+
+	clear_bit(__FM10K_RESETTING, &interface->state);
+}
+
+static void fm10k_reset_subtask(struct fm10k_intfc *interface)
+{
+	if (!(interface->flags & FM10K_FLAG_RESET_REQUESTED))
+		return;
+
+	interface->flags &= ~FM10K_FLAG_RESET_REQUESTED;
+
+	netdev_err(interface->netdev, "Reset interface\n");
+	interface->tx_timeout_count++;
+
+	fm10k_reinit(interface);
+}
+
+/**
+ * fm10k_configure_swpri_map - Configure Receive SWPRI to PC mapping
+ * @interface: board private structure
+ *
+ * Configure the SWPRI to PC mapping for the port.
+ **/
+static void fm10k_configure_swpri_map(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	int i;
+
+	/* clear flag indicating update is needed */
+	interface->flags &= ~FM10K_FLAG_SWPRI_CONFIG;
+
+	/* these registers are only available on the PF */
+	if (hw->mac.type != fm10k_mac_pf)
+		return;
+
+	/* configure SWPRI to PC map */
+	for (i = 0; i < FM10K_SWPRI_MAX; i++)
+		fm10k_write_reg(hw, FM10K_SWPRI_MAP(i),
+				netdev_get_prio_tc_map(netdev, i));
+}
+
+/**
+ * fm10k_watchdog_update_host_state - Update the link status based on host.
+ * @interface: board private structure
+ **/
+static void fm10k_watchdog_update_host_state(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	s32 err;
+
+	if (test_bit(__FM10K_LINK_DOWN, &interface->state)) {
+		interface->host_ready = false;
+		if (time_is_after_jiffies(interface->link_down_event))
+			return;
+		clear_bit(__FM10K_LINK_DOWN, &interface->state);
+	}
+
+	if (interface->flags & FM10K_FLAG_SWPRI_CONFIG) {
+		if (rtnl_trylock()) {
+			fm10k_configure_swpri_map(interface);
+			rtnl_unlock();
+		}
+	}
+
+	/* lock the mailbox for transmit and receive */
+	fm10k_mbx_lock(interface);
+
+	err = hw->mac.ops.get_host_state(hw, &interface->host_ready);
+	if (err && time_is_before_jiffies(interface->last_reset))
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+
+	/* free the lock */
+	fm10k_mbx_unlock(interface);
+}
+
+/**
+ * fm10k_mbx_subtask - Process upstream and downstream mailboxes
+ * @interface: board private structure
+ *
+ * This function will process both the upstream and downstream mailboxes.
+ * It is necessary for us to hold the rtnl_lock while doing this as the
+ * mailbox accesses are protected by this lock.
+ **/
+static void fm10k_mbx_subtask(struct fm10k_intfc *interface)
+{
+	/* process upstream mailbox and update device state */
+	fm10k_watchdog_update_host_state(interface);
+
+	/* process downstream mailboxes */
+	fm10k_iov_mbx(interface);
+}
+
+/**
+ * fm10k_watchdog_host_is_ready - Update netdev status based on host ready
+ * @interface: board private structure
+ **/
+static void fm10k_watchdog_host_is_ready(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+
+	/* only continue if link state is currently down */
+	if (netif_carrier_ok(netdev))
+		return;
+
+	netif_info(interface, drv, netdev, "NIC Link is up\n");
+
+	netif_carrier_on(netdev);
+	netif_tx_wake_all_queues(netdev);
+}
+
+/**
+ * fm10k_watchdog_host_not_ready - Update netdev status based on host not ready
+ * @interface: board private structure
+ **/
+static void fm10k_watchdog_host_not_ready(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+
+	/* only continue if link state is currently up */
+	if (!netif_carrier_ok(netdev))
+		return;
+
+	netif_info(interface, drv, netdev, "NIC Link is down\n");
+
+	netif_carrier_off(netdev);
+	netif_tx_stop_all_queues(netdev);
+}
+
+/**
+ * fm10k_update_stats - Update the board statistics counters.
+ * @interface: board private structure
+ **/
+void fm10k_update_stats(struct fm10k_intfc *interface)
+{
+	struct net_device_stats *net_stats = &interface->netdev->stats;
+	struct fm10k_hw *hw = &interface->hw;
+	u64 rx_errors = 0, rx_csum_errors = 0, tx_csum_errors = 0;
+	u64 restart_queue = 0, tx_busy = 0, alloc_failed = 0;
+	u64 rx_bytes_nic = 0, rx_pkts_nic = 0, rx_drops_nic = 0;
+	u64 tx_bytes_nic = 0, tx_pkts_nic = 0;
+	u64 bytes, pkts;
+	int i;
+
+	/* do not allow stats update via service task for next second */
+	interface->next_stats_update = jiffies + HZ;
+
+	/* gather some stats to the interface struct that are per queue */
+	for (bytes = 0, pkts = 0, i = 0; i < interface->num_tx_queues; i++) {
+		struct fm10k_ring *tx_ring = interface->tx_ring[i];
+
+		restart_queue += tx_ring->tx_stats.restart_queue;
+		tx_busy += tx_ring->tx_stats.tx_busy;
+		tx_csum_errors += tx_ring->tx_stats.csum_err;
+		bytes += tx_ring->stats.bytes;
+		pkts += tx_ring->stats.packets;
+	}
+
+	interface->restart_queue = restart_queue;
+	interface->tx_busy = tx_busy;
+	net_stats->tx_bytes = bytes;
+	net_stats->tx_packets = pkts;
+	interface->tx_csum_errors = tx_csum_errors;
+	/* gather some stats to the interface struct that are per queue */
+	for (bytes = 0, pkts = 0, i = 0; i < interface->num_rx_queues; i++) {
+		struct fm10k_ring *rx_ring = interface->rx_ring[i];
+
+		bytes += rx_ring->stats.bytes;
+		pkts += rx_ring->stats.packets;
+		alloc_failed += rx_ring->rx_stats.alloc_failed;
+		rx_csum_errors += rx_ring->rx_stats.csum_err;
+		rx_errors += rx_ring->rx_stats.errors;
+	}
+
+	net_stats->rx_bytes = bytes;
+	net_stats->rx_packets = pkts;
+	interface->alloc_failed = alloc_failed;
+	interface->rx_csum_errors = rx_csum_errors;
+	interface->rx_errors = rx_errors;
+
+	hw->mac.ops.update_hw_stats(hw, &interface->stats);
+
+	for (i = 0; i < FM10K_MAX_QUEUES_PF; i++) {
+		struct fm10k_hw_stats_q *q = &interface->stats.q[i];
+
+		tx_bytes_nic += q->tx_bytes.count;
+		tx_pkts_nic += q->tx_packets.count;
+		rx_bytes_nic += q->rx_bytes.count;
+		rx_pkts_nic += q->rx_packets.count;
+		rx_drops_nic += q->rx_drops.count;
+	}
+
+	interface->tx_bytes_nic = tx_bytes_nic;
+	interface->tx_packets_nic = tx_pkts_nic;
+	interface->rx_bytes_nic = rx_bytes_nic;
+	interface->rx_packets_nic = rx_pkts_nic;
+	interface->rx_drops_nic = rx_drops_nic;
+
+	/* Fill out the OS statistics structure */
+	net_stats->rx_errors = interface->stats.xec.count;
+	net_stats->rx_dropped = interface->stats.nodesc_drop.count;
+}
+
+/**
+ * fm10k_watchdog_flush_tx - flush queues on host not ready
+ * @interface - pointer to the device interface structure
+ **/
+static void fm10k_watchdog_flush_tx(struct fm10k_intfc *interface)
+{
+	int some_tx_pending = 0;
+	int i;
+
+	/* nothing to do if carrier is up */
+	if (netif_carrier_ok(interface->netdev))
+		return;
+
+	for (i = 0; i < interface->num_tx_queues; i++) {
+		struct fm10k_ring *tx_ring = interface->tx_ring[i];
+
+		if (tx_ring->next_to_use != tx_ring->next_to_clean) {
+			some_tx_pending = 1;
+			break;
+		}
+	}
+
+	/* We've lost link, so the controller stops DMA, but we've got
+	 * queued Tx work that's never going to get done, so reset
+	 * controller to flush Tx.
+	 */
+	if (some_tx_pending)
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+}
+
+/**
+ * fm10k_watchdog_subtask - check and bring link up
+ * @interface - pointer to the device interface structure
+ **/
+static void fm10k_watchdog_subtask(struct fm10k_intfc *interface)
+{
+	/* if interface is down do nothing */
+	if (test_bit(__FM10K_DOWN, &interface->state) ||
+	    test_bit(__FM10K_RESETTING, &interface->state))
+		return;
+
+	if (interface->host_ready)
+		fm10k_watchdog_host_is_ready(interface);
+	else
+		fm10k_watchdog_host_not_ready(interface);
+
+	/* update stats only once every second */
+	if (time_is_before_jiffies(interface->next_stats_update))
+		fm10k_update_stats(interface);
+
+	/* flush any uncompleted work */
+	fm10k_watchdog_flush_tx(interface);
+}
+
+/**
+ * fm10k_check_hang_subtask - check for hung queues and dropped interrupts
+ * @interface - pointer to the device interface structure
+ *
+ * This function serves two purposes.  First it strobes the interrupt lines
+ * in order to make certain interrupts are occurring.  Secondly it sets the
+ * bits needed to check for TX hangs.  As a result we should immediately
+ * determine if a hang has occurred.
+ */
+static void fm10k_check_hang_subtask(struct fm10k_intfc *interface)
+{
+	int i;
+
+	/* If we're down or resetting, just bail */
+	if (test_bit(__FM10K_DOWN, &interface->state) ||
+	    test_bit(__FM10K_RESETTING, &interface->state))
+		return;
+
+	/* rate limit tx hang checks to only once every 2 seconds */
+	if (time_is_after_eq_jiffies(interface->next_tx_hang_check))
+		return;
+	interface->next_tx_hang_check = jiffies + (2 * HZ);
+
+	if (netif_carrier_ok(interface->netdev)) {
+		/* Force detection of hung controller */
+		for (i = 0; i < interface->num_tx_queues; i++)
+			set_check_for_tx_hang(interface->tx_ring[i]);
+
+		/* Rearm all in-use q_vectors for immediate firing */
+		for (i = 0; i < interface->num_q_vectors; i++) {
+			struct fm10k_q_vector *qv = interface->q_vector[i];
+
+			if (!qv->tx.count && !qv->rx.count)
+				continue;
+			writel(FM10K_ITR_ENABLE | FM10K_ITR_PENDING2, qv->itr);
+		}
+	}
+}
+
+/**
+ * fm10k_service_task - manages and runs subtasks
+ * @work: pointer to work_struct containing our data
+ **/
+static void fm10k_service_task(struct work_struct *work)
+{
+	struct fm10k_intfc *interface;
+
+	interface = container_of(work, struct fm10k_intfc, service_task);
+
+	/* tasks always capable of running, but must be rtnl protected */
+	fm10k_mbx_subtask(interface);
+	fm10k_detach_subtask(interface);
+	fm10k_reset_subtask(interface);
+
+	/* tasks only run when interface is up */
+	fm10k_watchdog_subtask(interface);
+	fm10k_check_hang_subtask(interface);
+	fm10k_ts_tx_subtask(interface);
+
+	/* release lock on service events to allow scheduling next event */
+	fm10k_service_event_complete(interface);
+}
+
+/**
+ * fm10k_configure_tx_ring - Configure Tx ring after Reset
+ * @interface: board private structure
+ * @ring: structure containing ring specific data
+ *
+ * Configure the Tx descriptor ring after a reset.
+ **/
+static void fm10k_configure_tx_ring(struct fm10k_intfc *interface,
+				    struct fm10k_ring *ring)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	u64 tdba = ring->dma;
+	u32 size = ring->count * sizeof(struct fm10k_tx_desc);
+	u32 txint = FM10K_INT_MAP_DISABLE;
+	u32 txdctl = FM10K_TXDCTL_ENABLE | (1 << FM10K_TXDCTL_MAX_TIME_SHIFT);
+	u8 reg_idx = ring->reg_idx;
+
+	/* disable queue to avoid issues while updating state */
+	fm10k_write_reg(hw, FM10K_TXDCTL(reg_idx), 0);
+	fm10k_write_flush(hw);
+
+	/* possible poll here to verify ring resources have been cleaned */
+
+	/* set location and size for descriptor ring */
+	fm10k_write_reg(hw, FM10K_TDBAL(reg_idx), tdba & DMA_BIT_MASK(32));
+	fm10k_write_reg(hw, FM10K_TDBAH(reg_idx), tdba >> 32);
+	fm10k_write_reg(hw, FM10K_TDLEN(reg_idx), size);
+
+	/* reset head and tail pointers */
+	fm10k_write_reg(hw, FM10K_TDH(reg_idx), 0);
+	fm10k_write_reg(hw, FM10K_TDT(reg_idx), 0);
+
+	/* store tail pointer */
+	ring->tail = &interface->uc_addr[FM10K_TDT(reg_idx)];
+
+	/* reset ntu and ntc to place SW in sync with hardwdare */
+	ring->next_to_clean = 0;
+	ring->next_to_use = 0;
+
+	/* Map interrupt */
+	if (ring->q_vector) {
+		txint = ring->q_vector->v_idx + NON_Q_VECTORS(hw);
+		txint |= FM10K_INT_MAP_TIMER0;
+	}
+
+	fm10k_write_reg(hw, FM10K_TXINT(reg_idx), txint);
+
+	/* enable use of FTAG bit in Tx descriptor, register is RO for VF */
+	fm10k_write_reg(hw, FM10K_PFVTCTL(reg_idx),
+			FM10K_PFVTCTL_FTAG_DESC_ENABLE);
+
+	/* enable queue */
+	fm10k_write_reg(hw, FM10K_TXDCTL(reg_idx), txdctl);
+}
+
+/**
+ * fm10k_enable_tx_ring - Verify Tx ring is enabled after configuration
+ * @interface: board private structure
+ * @ring: structure containing ring specific data
+ *
+ * Verify the Tx descriptor ring is ready for transmit.
+ **/
+static void fm10k_enable_tx_ring(struct fm10k_intfc *interface,
+				 struct fm10k_ring *ring)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	int wait_loop = 10;
+	u32 txdctl;
+	u8 reg_idx = ring->reg_idx;
+
+	/* if we are already enabled just exit */
+	if (fm10k_read_reg(hw, FM10K_TXDCTL(reg_idx)) & FM10K_TXDCTL_ENABLE)
+		return;
+
+	/* poll to verify queue is enabled */
+	do {
+		usleep_range(1000, 2000);
+		txdctl = fm10k_read_reg(hw, FM10K_TXDCTL(reg_idx));
+	} while (!(txdctl & FM10K_TXDCTL_ENABLE) && --wait_loop);
+	if (!wait_loop)
+		netif_err(interface, drv, interface->netdev,
+			  "Could not enable Tx Queue %d\n", reg_idx);
+}
+
+/**
+ * fm10k_configure_tx - Configure Transmit Unit after Reset
+ * @interface: board private structure
+ *
+ * Configure the Tx unit of the MAC after a reset.
+ **/
+static void fm10k_configure_tx(struct fm10k_intfc *interface)
+{
+	int i;
+
+	/* Setup the HW Tx Head and Tail descriptor pointers */
+	for (i = 0; i < interface->num_tx_queues; i++)
+		fm10k_configure_tx_ring(interface, interface->tx_ring[i]);
+
+	/* poll here to verify that Tx rings are now enabled */
+	for (i = 0; i < interface->num_tx_queues; i++)
+		fm10k_enable_tx_ring(interface, interface->tx_ring[i]);
+}
+
+/**
+ * fm10k_configure_rx_ring - Configure Rx ring after Reset
+ * @interface: board private structure
+ * @ring: structure containing ring specific data
+ *
+ * Configure the Rx descriptor ring after a reset.
+ **/
+static void fm10k_configure_rx_ring(struct fm10k_intfc *interface,
+				    struct fm10k_ring *ring)
+{
+	u64 rdba = ring->dma;
+	struct fm10k_hw *hw = &interface->hw;
+	u32 size = ring->count * sizeof(union fm10k_rx_desc);
+	u32 rxqctl = FM10K_RXQCTL_ENABLE | FM10K_RXQCTL_PF;
+	u32 rxdctl = FM10K_RXDCTL_WRITE_BACK_MIN_DELAY;
+	u32 srrctl = FM10K_SRRCTL_BUFFER_CHAINING_EN;
+	u32 rxint = FM10K_INT_MAP_DISABLE;
+	u8 rx_pause = interface->rx_pause;
+	u8 reg_idx = ring->reg_idx;
+
+	/* disable queue to avoid issues while updating state */
+	fm10k_write_reg(hw, FM10K_RXQCTL(reg_idx), 0);
+	fm10k_write_flush(hw);
+
+	/* possible poll here to verify ring resources have been cleaned */
+
+	/* set location and size for descriptor ring */
+	fm10k_write_reg(hw, FM10K_RDBAL(reg_idx), rdba & DMA_BIT_MASK(32));
+	fm10k_write_reg(hw, FM10K_RDBAH(reg_idx), rdba >> 32);
+	fm10k_write_reg(hw, FM10K_RDLEN(reg_idx), size);
+
+	/* reset head and tail pointers */
+	fm10k_write_reg(hw, FM10K_RDH(reg_idx), 0);
+	fm10k_write_reg(hw, FM10K_RDT(reg_idx), 0);
+
+	/* store tail pointer */
+	ring->tail = &interface->uc_addr[FM10K_RDT(reg_idx)];
+
+	/* reset ntu and ntc to place SW in sync with hardwdare */
+	ring->next_to_clean = 0;
+	ring->next_to_use = 0;
+	ring->next_to_alloc = 0;
+
+	/* Configure the Rx buffer size for one buff without split */
+	srrctl |= FM10K_RX_BUFSZ >> FM10K_SRRCTL_BSIZEPKT_SHIFT;
+
+	/* Configure the Rx ring to supress loopback packets */
+	srrctl |= FM10K_SRRCTL_LOOPBACK_SUPPRESS;
+	fm10k_write_reg(hw, FM10K_SRRCTL(reg_idx), srrctl);
+
+	/* Enable drop on empty */
+#ifdef CONFIG_DCB
+	if (interface->pfc_en)
+		rx_pause = interface->pfc_en;
+#endif
+	if (!(rx_pause & (1 << ring->qos_pc)))
+		rxdctl |= FM10K_RXDCTL_DROP_ON_EMPTY;
+
+	fm10k_write_reg(hw, FM10K_RXDCTL(reg_idx), rxdctl);
+
+	/* assign default VLAN to queue */
+	ring->vid = hw->mac.default_vid;
+
+	/* Map interrupt */
+	if (ring->q_vector) {
+		rxint = ring->q_vector->v_idx + NON_Q_VECTORS(hw);
+		rxint |= FM10K_INT_MAP_TIMER1;
+	}
+
+	fm10k_write_reg(hw, FM10K_RXINT(reg_idx), rxint);
+
+	/* enable queue */
+	fm10k_write_reg(hw, FM10K_RXQCTL(reg_idx), rxqctl);
+
+	/* place buffers on ring for receive data */
+	fm10k_alloc_rx_buffers(ring, fm10k_desc_unused(ring));
+}
+
+/**
+ * fm10k_update_rx_drop_en - Configures the drop enable bits for Rx rings
+ * @interface: board private structure
+ *
+ * Configure the drop enable bits for the Rx rings.
+ **/
+void fm10k_update_rx_drop_en(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	u8 rx_pause = interface->rx_pause;
+	int i;
+
+#ifdef CONFIG_DCB
+	if (interface->pfc_en)
+		rx_pause = interface->pfc_en;
+
+#endif
+	for (i = 0; i < interface->num_rx_queues; i++) {
+		struct fm10k_ring *ring = interface->rx_ring[i];
+		u32 rxdctl = FM10K_RXDCTL_WRITE_BACK_MIN_DELAY;
+		u8 reg_idx = ring->reg_idx;
+
+		if (!(rx_pause & (1 << ring->qos_pc)))
+			rxdctl |= FM10K_RXDCTL_DROP_ON_EMPTY;
+
+		fm10k_write_reg(hw, FM10K_RXDCTL(reg_idx), rxdctl);
+	}
+}
+
+/**
+ * fm10k_configure_dglort - Configure Receive DGLORT after reset
+ * @interface: board private structure
+ *
+ * Configure the DGLORT description and RSS tables.
+ **/
+static void fm10k_configure_dglort(struct fm10k_intfc *interface)
+{
+	struct fm10k_dglort_cfg dglort = { 0 };
+	struct fm10k_hw *hw = &interface->hw;
+	int i;
+	u32 mrqc;
+
+	/* Fill out hash function seeds */
+	for (i = 0; i < FM10K_RSSRK_SIZE; i++)
+		fm10k_write_reg(hw, FM10K_RSSRK(0, i), interface->rssrk[i]);
+
+	/* Write RETA table to hardware */
+	for (i = 0; i < FM10K_RETA_SIZE; i++)
+		fm10k_write_reg(hw, FM10K_RETA(0, i), interface->reta[i]);
+
+	/* Generate RSS hash based on packet types, TCP/UDP
+	 * port numbers and/or IPv4/v6 src and dst addresses
+	 */
+	mrqc = FM10K_MRQC_IPV4 |
+	       FM10K_MRQC_TCP_IPV4 |
+	       FM10K_MRQC_IPV6 |
+	       FM10K_MRQC_TCP_IPV6;
+
+	if (interface->flags & FM10K_FLAG_RSS_FIELD_IPV4_UDP)
+		mrqc |= FM10K_MRQC_UDP_IPV4;
+	if (interface->flags & FM10K_FLAG_RSS_FIELD_IPV6_UDP)
+		mrqc |= FM10K_MRQC_UDP_IPV6;
+
+	fm10k_write_reg(hw, FM10K_MRQC(0), mrqc);
+
+	/* configure default DGLORT mapping for RSS/DCB */
+	dglort.inner_rss = 1;
+	dglort.rss_l = fls(interface->ring_feature[RING_F_RSS].mask);
+	dglort.pc_l = fls(interface->ring_feature[RING_F_QOS].mask);
+	hw->mac.ops.configure_dglort_map(hw, &dglort);
+
+	/* assign GLORT per queue for queue mapped testing */
+	if (interface->glort_count > 64) {
+		memset(&dglort, 0, sizeof(dglort));
+		dglort.inner_rss = 1;
+		dglort.glort = interface->glort + 64;
+		dglort.idx = fm10k_dglort_pf_queue;
+		dglort.queue_l = fls(interface->num_rx_queues - 1);
+		hw->mac.ops.configure_dglort_map(hw, &dglort);
+	}
+
+	/* assign glort value for RSS/DCB specific to this interface */
+	memset(&dglort, 0, sizeof(dglort));
+	dglort.inner_rss = 1;
+	dglort.glort = interface->glort;
+	dglort.rss_l = fls(interface->ring_feature[RING_F_RSS].mask);
+	dglort.pc_l = fls(interface->ring_feature[RING_F_QOS].mask);
+	/* configure DGLORT mapping for RSS/DCB */
+	dglort.idx = fm10k_dglort_pf_rss;
+	if (interface->l2_accel)
+		dglort.shared_l = fls(interface->l2_accel->size);
+	hw->mac.ops.configure_dglort_map(hw, &dglort);
+}
+
+/**
+ * fm10k_configure_rx - Configure Receive Unit after Reset
+ * @interface: board private structure
+ *
+ * Configure the Rx unit of the MAC after a reset.
+ **/
+static void fm10k_configure_rx(struct fm10k_intfc *interface)
+{
+	int i;
+
+	/* Configure SWPRI to PC map */
+	fm10k_configure_swpri_map(interface);
+
+	/* Configure RSS and DGLORT map */
+	fm10k_configure_dglort(interface);
+
+	/* Setup the HW Rx Head and Tail descriptor pointers */
+	for (i = 0; i < interface->num_rx_queues; i++)
+		fm10k_configure_rx_ring(interface, interface->rx_ring[i]);
+
+	/* possible poll here to verify that Rx rings are now enabled */
+}
+
+static void fm10k_napi_enable_all(struct fm10k_intfc *interface)
+{
+	struct fm10k_q_vector *q_vector;
+	int q_idx;
+
+	for (q_idx = 0; q_idx < interface->num_q_vectors; q_idx++) {
+		q_vector = interface->q_vector[q_idx];
+		napi_enable(&q_vector->napi);
+	}
+}
+
+static irqreturn_t fm10k_msix_clean_rings(int irq, void *data)
+{
+	struct fm10k_q_vector *q_vector = data;
+
+	if (q_vector->rx.count || q_vector->tx.count)
+		napi_schedule(&q_vector->napi);
+
+	return IRQ_HANDLED;
+}
+
+static irqreturn_t fm10k_msix_mbx_vf(int irq, void *data)
+{
+	struct fm10k_intfc *interface = data;
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+
+	/* re-enable mailbox interrupt and indicate 20us delay */
+	fm10k_write_reg(hw, FM10K_VFITR(FM10K_MBX_VECTOR),
+			FM10K_ITR_ENABLE | FM10K_MBX_INT_DELAY);
+
+	/* service upstream mailbox */
+	if (fm10k_mbx_trylock(interface)) {
+		mbx->ops.process(hw, mbx);
+		fm10k_mbx_unlock(interface);
+	}
+
+	hw->mac.get_host_state = 1;
+	fm10k_service_event_schedule(interface);
+
+	return IRQ_HANDLED;
+}
+
+#define FM10K_ERR_MSG(type) case (type): error = #type; break
+static void fm10k_print_fault(struct fm10k_intfc *interface, int type,
+			      struct fm10k_fault *fault)
+{
+	struct pci_dev *pdev = interface->pdev;
+	char *error;
+
+	switch (type) {
+	case FM10K_PCA_FAULT:
+		switch (fault->type) {
+		default:
+			error = "Unknown PCA error";
+			break;
+		FM10K_ERR_MSG(PCA_NO_FAULT);
+		FM10K_ERR_MSG(PCA_UNMAPPED_ADDR);
+		FM10K_ERR_MSG(PCA_BAD_QACCESS_PF);
+		FM10K_ERR_MSG(PCA_BAD_QACCESS_VF);
+		FM10K_ERR_MSG(PCA_MALICIOUS_REQ);
+		FM10K_ERR_MSG(PCA_POISONED_TLP);
+		FM10K_ERR_MSG(PCA_TLP_ABORT);
+		}
+		break;
+	case FM10K_THI_FAULT:
+		switch (fault->type) {
+		default:
+			error = "Unknown THI error";
+			break;
+		FM10K_ERR_MSG(THI_NO_FAULT);
+		FM10K_ERR_MSG(THI_MAL_DIS_Q_FAULT);
+		}
+		break;
+	case FM10K_FUM_FAULT:
+		switch (fault->type) {
+		default:
+			error = "Unknown FUM error";
+			break;
+		FM10K_ERR_MSG(FUM_NO_FAULT);
+		FM10K_ERR_MSG(FUM_UNMAPPED_ADDR);
+		FM10K_ERR_MSG(FUM_BAD_VF_QACCESS);
+		FM10K_ERR_MSG(FUM_ADD_DECODE_ERR);
+		FM10K_ERR_MSG(FUM_RO_ERROR);
+		FM10K_ERR_MSG(FUM_QPRC_CRC_ERROR);
+		FM10K_ERR_MSG(FUM_CSR_TIMEOUT);
+		FM10K_ERR_MSG(FUM_INVALID_TYPE);
+		FM10K_ERR_MSG(FUM_INVALID_LENGTH);
+		FM10K_ERR_MSG(FUM_INVALID_BE);
+		FM10K_ERR_MSG(FUM_INVALID_ALIGN);
+		}
+		break;
+	default:
+		error = "Undocumented fault";
+		break;
+	}
+
+	dev_warn(&pdev->dev,
+		 "%s Address: 0x%llx SpecInfo: 0x%x Func: %02x.%0x\n",
+		 error, fault->address, fault->specinfo,
+		 PCI_SLOT(fault->func), PCI_FUNC(fault->func));
+}
+
+static void fm10k_report_fault(struct fm10k_intfc *interface, u32 eicr)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_fault fault = { 0 };
+	int type, err;
+
+	for (eicr &= FM10K_EICR_FAULT_MASK, type = FM10K_PCA_FAULT;
+	     eicr;
+	     eicr >>= 1, type += FM10K_FAULT_SIZE) {
+		/* only check if there is an error reported */
+		if (!(eicr & 0x1))
+			continue;
+
+		/* retrieve fault info */
+		err = hw->mac.ops.get_fault(hw, type, &fault);
+		if (err) {
+			dev_err(&interface->pdev->dev,
+				"error reading fault\n");
+			continue;
+		}
+
+		fm10k_print_fault(interface, type, &fault);
+	}
+}
+
+static void fm10k_reset_drop_on_empty(struct fm10k_intfc *interface, u32 eicr)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	const u32 rxdctl = FM10K_RXDCTL_WRITE_BACK_MIN_DELAY;
+	u32 maxholdq;
+	int q;
+
+	if (!(eicr & FM10K_EICR_MAXHOLDTIME))
+		return;
+
+	maxholdq = fm10k_read_reg(hw, FM10K_MAXHOLDQ(7));
+	if (maxholdq)
+		fm10k_write_reg(hw, FM10K_MAXHOLDQ(7), maxholdq);
+	for (q = 255;;) {
+		if (maxholdq & (1 << 31)) {
+			if (q < FM10K_MAX_QUEUES_PF) {
+				interface->rx_overrun_pf++;
+				fm10k_write_reg(hw, FM10K_RXDCTL(q), rxdctl);
+			} else {
+				interface->rx_overrun_vf++;
+			}
+		}
+
+		maxholdq *= 2;
+		if (!maxholdq)
+			q &= ~(32 - 1);
+
+		if (!q)
+			break;
+
+		if (q-- % 32)
+			continue;
+
+		maxholdq = fm10k_read_reg(hw, FM10K_MAXHOLDQ(q / 32));
+		if (maxholdq)
+			fm10k_write_reg(hw, FM10K_MAXHOLDQ(q / 32), maxholdq);
+	}
+}
+
+static irqreturn_t fm10k_msix_mbx_pf(int irq, void *data)
+{
+	struct fm10k_intfc *interface = data;
+	struct fm10k_hw *hw = &interface->hw;
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 eicr;
+
+	/* unmask any set bits related to this interrupt */
+	eicr = fm10k_read_reg(hw, FM10K_EICR);
+	fm10k_write_reg(hw, FM10K_EICR, eicr & (FM10K_EICR_MAILBOX |
+						FM10K_EICR_SWITCHREADY |
+						FM10K_EICR_SWITCHNOTREADY));
+
+	/* report any faults found to the message log */
+	fm10k_report_fault(interface, eicr);
+
+	/* reset any queues disabled due to receiver overrun */
+	fm10k_reset_drop_on_empty(interface, eicr);
+
+	/* service mailboxes */
+	if (fm10k_mbx_trylock(interface)) {
+		mbx->ops.process(hw, mbx);
+		fm10k_iov_event(interface);
+		fm10k_mbx_unlock(interface);
+	}
+
+	/* if switch toggled state we should reset GLORTs */
+	if (eicr & FM10K_EICR_SWITCHNOTREADY) {
+		/* force link down for at least 4 seconds */
+		interface->link_down_event = jiffies + (4 * HZ);
+		set_bit(__FM10K_LINK_DOWN, &interface->state);
+
+		/* reset dglort_map back to no config */
+		hw->mac.dglort_map = FM10K_DGLORTMAP_NONE;
+	}
+
+	/* we should validate host state after interrupt event */
+	hw->mac.get_host_state = 1;
+	fm10k_service_event_schedule(interface);
+
+	/* re-enable mailbox interrupt and indicate 20us delay */
+	fm10k_write_reg(hw, FM10K_ITR(FM10K_MBX_VECTOR),
+			FM10K_ITR_ENABLE | FM10K_MBX_INT_DELAY);
+
+	return IRQ_HANDLED;
+}
+
+void fm10k_mbx_free_irq(struct fm10k_intfc *interface)
+{
+	struct msix_entry *entry = &interface->msix_entries[FM10K_MBX_VECTOR];
+	struct fm10k_hw *hw = &interface->hw;
+	int itr_reg;
+
+	/* disconnect the mailbox */
+	hw->mbx.ops.disconnect(hw, &hw->mbx);
+
+	/* disable Mailbox cause */
+	if (hw->mac.type == fm10k_mac_pf) {
+		fm10k_write_reg(hw, FM10K_EIMR,
+				FM10K_EIMR_DISABLE(PCA_FAULT) |
+				FM10K_EIMR_DISABLE(FUM_FAULT) |
+				FM10K_EIMR_DISABLE(MAILBOX) |
+				FM10K_EIMR_DISABLE(SWITCHREADY) |
+				FM10K_EIMR_DISABLE(SWITCHNOTREADY) |
+				FM10K_EIMR_DISABLE(SRAMERROR) |
+				FM10K_EIMR_DISABLE(VFLR) |
+				FM10K_EIMR_DISABLE(MAXHOLDTIME));
+		itr_reg = FM10K_ITR(FM10K_MBX_VECTOR);
+	} else {
+		itr_reg = FM10K_VFITR(FM10K_MBX_VECTOR);
+	}
+
+	fm10k_write_reg(hw, itr_reg, FM10K_ITR_MASK_SET);
+
+	free_irq(entry->vector, interface);
+}
+
+static s32 fm10k_mbx_mac_addr(struct fm10k_hw *hw, u32 **results,
+			      struct fm10k_mbx_info *mbx)
+{
+	bool vlan_override = hw->mac.vlan_override;
+	u16 default_vid = hw->mac.default_vid;
+	struct fm10k_intfc *interface;
+	s32 err;
+
+	err = fm10k_msg_mac_vlan_vf(hw, results, mbx);
+	if (err)
+		return err;
+
+	interface = container_of(hw, struct fm10k_intfc, hw);
+
+	/* MAC was changed so we need reset */
+	if (is_valid_ether_addr(hw->mac.perm_addr) &&
+	    memcmp(hw->mac.perm_addr, hw->mac.addr, ETH_ALEN))
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+
+	/* VLAN override was changed, or default VLAN changed */
+	if ((vlan_override != hw->mac.vlan_override) ||
+	    (default_vid != hw->mac.default_vid))
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+
+	return 0;
+}
+
+static s32 fm10k_1588_msg_vf(struct fm10k_hw *hw, u32 **results,
+			     struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_intfc *interface;
+	u64 timestamp;
+	s32 err;
+
+	err = fm10k_tlv_attr_get_u64(results[FM10K_1588_MSG_TIMESTAMP],
+				     &timestamp);
+	if (err)
+		return err;
+
+	interface = container_of(hw, struct fm10k_intfc, hw);
+
+	fm10k_ts_tx_hwtstamp(interface, 0, timestamp);
+
+	return 0;
+}
+
+/* generic error handler for mailbox issues */
+static s32 fm10k_mbx_error(struct fm10k_hw *hw, u32 **results,
+			   struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_intfc *interface;
+	struct pci_dev *pdev;
+
+	interface = container_of(hw, struct fm10k_intfc, hw);
+	pdev = interface->pdev;
+
+	dev_err(&pdev->dev, "Unknown message ID %u\n",
+		**results & FM10K_TLV_ID_MASK);
+
+	return 0;
+}
+
+static const struct fm10k_msg_data vf_mbx_data[] = {
+	FM10K_TLV_MSG_TEST_HANDLER(fm10k_tlv_msg_test),
+	FM10K_VF_MSG_MAC_VLAN_HANDLER(fm10k_mbx_mac_addr),
+	FM10K_VF_MSG_LPORT_STATE_HANDLER(fm10k_msg_lport_state_vf),
+	FM10K_VF_MSG_1588_HANDLER(fm10k_1588_msg_vf),
+	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_mbx_error),
+};
+
+static int fm10k_mbx_request_irq_vf(struct fm10k_intfc *interface)
+{
+	struct msix_entry *entry = &interface->msix_entries[FM10K_MBX_VECTOR];
+	struct net_device *dev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	int err;
+
+	/* Use timer0 for interrupt moderation on the mailbox */
+	u32 itr = FM10K_INT_MAP_TIMER0 | entry->entry;
+
+	/* register mailbox handlers */
+	err = hw->mbx.ops.register_handlers(&hw->mbx, vf_mbx_data);
+	if (err)
+		return err;
+
+	/* request the IRQ */
+	err = request_irq(entry->vector, fm10k_msix_mbx_vf, 0,
+			  dev->name, interface);
+	if (err) {
+		netif_err(interface, probe, dev,
+			  "request_irq for msix_mbx failed: %d\n", err);
+		return err;
+	}
+
+	/* map all of the interrupt sources */
+	fm10k_write_reg(hw, FM10K_VFINT_MAP, itr);
+
+	/* enable interrupt */
+	fm10k_write_reg(hw, FM10K_VFITR(entry->entry), FM10K_ITR_ENABLE);
+
+	return 0;
+}
+
+static s32 fm10k_lport_map(struct fm10k_hw *hw, u32 **results,
+			   struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_intfc *interface;
+	u32 dglort_map = hw->mac.dglort_map;
+	s32 err;
+
+	err = fm10k_msg_lport_map_pf(hw, results, mbx);
+	if (err)
+		return err;
+
+	interface = container_of(hw, struct fm10k_intfc, hw);
+
+	/* we need to reset if port count was just updated */
+	if (dglort_map != hw->mac.dglort_map)
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+
+	return 0;
+}
+
+static s32 fm10k_update_pvid(struct fm10k_hw *hw, u32 **results,
+			     struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_intfc *interface;
+	u16 glort, pvid;
+	u32 pvid_update;
+	s32 err;
+
+	err = fm10k_tlv_attr_get_u32(results[FM10K_PF_ATTR_ID_UPDATE_PVID],
+				     &pvid_update);
+	if (err)
+		return err;
+
+	/* extract values from the pvid update */
+	glort = FM10K_MSG_HDR_FIELD_GET(pvid_update, UPDATE_PVID_GLORT);
+	pvid = FM10K_MSG_HDR_FIELD_GET(pvid_update, UPDATE_PVID_PVID);
+
+	/* if glort is not valid return error */
+	if (!fm10k_glort_valid_pf(hw, glort))
+		return FM10K_ERR_PARAM;
+
+	/* verify VID is valid */
+	if (pvid >= FM10K_VLAN_TABLE_VID_MAX)
+		return FM10K_ERR_PARAM;
+
+	interface = container_of(hw, struct fm10k_intfc, hw);
+
+	/* check to see if this belongs to one of the VFs */
+	err = fm10k_iov_update_pvid(interface, glort, pvid);
+	if (!err)
+		return 0;
+
+	/* we need to reset if default VLAN was just updated */
+	if (pvid != hw->mac.default_vid)
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+
+	hw->mac.default_vid = pvid;
+
+	return 0;
+}
+
+static s32 fm10k_1588_msg_pf(struct fm10k_hw *hw, u32 **results,
+			     struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_swapi_1588_timestamp timestamp;
+	struct fm10k_iov_data *iov_data;
+	struct fm10k_intfc *interface;
+	u16 sglort, vf_idx;
+	s32 err;
+
+	err = fm10k_tlv_attr_get_le_struct(
+				results[FM10K_PF_ATTR_ID_1588_TIMESTAMP],
+				&timestamp, sizeof(timestamp));
+	if (err)
+		return err;
+
+	interface = container_of(hw, struct fm10k_intfc, hw);
+
+	if (timestamp.dglort) {
+		fm10k_ts_tx_hwtstamp(interface, timestamp.dglort,
+				     le64_to_cpu(timestamp.egress));
+		return 0;
+	}
+
+	/* either dglort or sglort must be set */
+	if (!timestamp.sglort)
+		return FM10K_ERR_PARAM;
+
+	/* verify GLORT is at least one of the ones we own */
+	sglort = le16_to_cpu(timestamp.sglort);
+	if (!fm10k_glort_valid_pf(hw, sglort))
+		return FM10K_ERR_PARAM;
+
+	if (sglort == interface->glort) {
+		fm10k_ts_tx_hwtstamp(interface, 0,
+				     le64_to_cpu(timestamp.ingress));
+		return 0;
+	}
+
+	/* if there is no iov_data then there is no mailboxes to process */
+	if (!ACCESS_ONCE(interface->iov_data))
+		return FM10K_ERR_PARAM;
+
+	rcu_read_lock();
+
+	/* notify VF if this timestamp belongs to it */
+	iov_data = interface->iov_data;
+	vf_idx = (hw->mac.dglort_map & FM10K_DGLORTMAP_NONE) - sglort;
+
+	if (!iov_data || vf_idx >= iov_data->num_vfs) {
+		err = FM10K_ERR_PARAM;
+		goto err_unlock;
+	}
+
+	err = hw->iov.ops.report_timestamp(hw, &iov_data->vf_info[vf_idx],
+					   le64_to_cpu(timestamp.ingress));
+
+err_unlock:
+	rcu_read_unlock();
+
+	return err;
+}
+
+static const struct fm10k_msg_data pf_mbx_data[] = {
+	FM10K_PF_MSG_ERR_HANDLER(XCAST_MODES, fm10k_msg_err_pf),
+	FM10K_PF_MSG_ERR_HANDLER(UPDATE_MAC_FWD_RULE, fm10k_msg_err_pf),
+	FM10K_PF_MSG_LPORT_MAP_HANDLER(fm10k_lport_map),
+	FM10K_PF_MSG_ERR_HANDLER(LPORT_CREATE, fm10k_msg_err_pf),
+	FM10K_PF_MSG_ERR_HANDLER(LPORT_DELETE, fm10k_msg_err_pf),
+	FM10K_PF_MSG_UPDATE_PVID_HANDLER(fm10k_update_pvid),
+	FM10K_PF_MSG_1588_TIMESTAMP_HANDLER(fm10k_1588_msg_pf),
+	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_mbx_error),
+};
+
+static int fm10k_mbx_request_irq_pf(struct fm10k_intfc *interface)
+{
+	struct msix_entry *entry = &interface->msix_entries[FM10K_MBX_VECTOR];
+	struct net_device *dev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	int err;
+
+	/* Use timer0 for interrupt moderation on the mailbox */
+	u32 mbx_itr = FM10K_INT_MAP_TIMER0 | entry->entry;
+	u32 other_itr = FM10K_INT_MAP_IMMEDIATE | entry->entry;
+
+	/* register mailbox handlers */
+	err = hw->mbx.ops.register_handlers(&hw->mbx, pf_mbx_data);
+	if (err)
+		return err;
+
+	/* request the IRQ */
+	err = request_irq(entry->vector, fm10k_msix_mbx_pf, 0,
+			  dev->name, interface);
+	if (err) {
+		netif_err(interface, probe, dev,
+			  "request_irq for msix_mbx failed: %d\n", err);
+		return err;
+	}
+
+	/* Enable interrupts w/ no moderation for "other" interrupts */
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_PCIeFault), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_SwitchUpDown), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_SRAM), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_MaxHoldTime), other_itr);
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_VFLR), other_itr);
+
+	/* Enable interrupts w/ moderation for mailbox */
+	fm10k_write_reg(hw, FM10K_INT_MAP(fm10k_int_Mailbox), mbx_itr);
+
+	/* Enable individual interrupt causes */
+	fm10k_write_reg(hw, FM10K_EIMR, FM10K_EIMR_ENABLE(PCA_FAULT) |
+					FM10K_EIMR_ENABLE(FUM_FAULT) |
+					FM10K_EIMR_ENABLE(MAILBOX) |
+					FM10K_EIMR_ENABLE(SWITCHREADY) |
+					FM10K_EIMR_ENABLE(SWITCHNOTREADY) |
+					FM10K_EIMR_ENABLE(SRAMERROR) |
+					FM10K_EIMR_ENABLE(VFLR) |
+					FM10K_EIMR_ENABLE(MAXHOLDTIME));
+
+	/* enable interrupt */
+	fm10k_write_reg(hw, FM10K_ITR(entry->entry), FM10K_ITR_ENABLE);
+
+	return 0;
+}
+
+int fm10k_mbx_request_irq(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+	int err;
+
+	/* enable Mailbox cause */
+	if (hw->mac.type == fm10k_mac_pf)
+		err = fm10k_mbx_request_irq_pf(interface);
+	else
+		err = fm10k_mbx_request_irq_vf(interface);
+
+	/* connect mailbox */
+	if (!err)
+		err = hw->mbx.ops.connect(hw, &hw->mbx);
+
+	return err;
+}
+
+/**
+ * fm10k_qv_free_irq - release interrupts associated with queue vectors
+ * @interface: board private structure
+ *
+ * Release all interrupts associated with this interface
+ **/
+void fm10k_qv_free_irq(struct fm10k_intfc *interface)
+{
+	int vector = interface->num_q_vectors;
+	struct fm10k_hw *hw = &interface->hw;
+	struct msix_entry *entry;
+
+	entry = &interface->msix_entries[NON_Q_VECTORS(hw) + vector];
+
+	while (vector) {
+		struct fm10k_q_vector *q_vector;
+
+		vector--;
+		entry--;
+		q_vector = interface->q_vector[vector];
+
+		if (!q_vector->tx.count && !q_vector->rx.count)
+			continue;
+
+		/* disable interrupts */
+
+		writel(FM10K_ITR_MASK_SET, q_vector->itr);
+
+		free_irq(entry->vector, q_vector);
+	}
+}
+
+/**
+ * fm10k_qv_request_irq - initialize interrupts for queue vectors
+ * @interface: board private structure
+ *
+ * Attempts to configure interrupts using the best available
+ * capabilities of the hardware and kernel.
+ **/
+int fm10k_qv_request_irq(struct fm10k_intfc *interface)
+{
+	struct net_device *dev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	struct msix_entry *entry;
+	int ri = 0, ti = 0;
+	int vector, err;
+
+	entry = &interface->msix_entries[NON_Q_VECTORS(hw)];
+
+	for (vector = 0; vector < interface->num_q_vectors; vector++) {
+		struct fm10k_q_vector *q_vector = interface->q_vector[vector];
+
+		/* name the vector */
+		if (q_vector->tx.count && q_vector->rx.count) {
+			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
+				 "%s-TxRx-%d", dev->name, ri++);
+			ti++;
+		} else if (q_vector->rx.count) {
+			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
+				 "%s-rx-%d", dev->name, ri++);
+		} else if (q_vector->tx.count) {
+			snprintf(q_vector->name, sizeof(q_vector->name) - 1,
+				 "%s-tx-%d", dev->name, ti++);
+		} else {
+			/* skip this unused q_vector */
+			continue;
+		}
+
+		/* Assign ITR register to q_vector */
+		q_vector->itr = (hw->mac.type == fm10k_mac_pf) ?
+				&interface->uc_addr[FM10K_ITR(entry->entry)] :
+				&interface->uc_addr[FM10K_VFITR(entry->entry)];
+
+		/* request the IRQ */
+		err = request_irq(entry->vector, &fm10k_msix_clean_rings, 0,
+				  q_vector->name, q_vector);
+		if (err) {
+			netif_err(interface, probe, dev,
+				  "request_irq failed for MSIX interrupt Error: %d\n",
+				  err);
+			goto err_out;
+		}
+
+		/* Enable q_vector */
+		writel(FM10K_ITR_ENABLE, q_vector->itr);
+
+		entry++;
+	}
+
+	return 0;
+
+err_out:
+	/* wind through the ring freeing all entries and vectors */
+	while (vector) {
+		struct fm10k_q_vector *q_vector;
+
+		entry--;
+		vector--;
+		q_vector = interface->q_vector[vector];
+
+		if (!q_vector->tx.count && !q_vector->rx.count)
+			continue;
+
+		/* disable interrupts */
+
+		writel(FM10K_ITR_MASK_SET, q_vector->itr);
+
+		free_irq(entry->vector, q_vector);
+	}
+
+	return err;
+}
+
+void fm10k_up(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+
+	/* Enable Tx/Rx DMA */
+	hw->mac.ops.start_hw(hw);
+
+	/* configure Tx descriptor rings */
+	fm10k_configure_tx(interface);
+
+	/* configure Rx descriptor rings */
+	fm10k_configure_rx(interface);
+
+	/* configure interrupts */
+	hw->mac.ops.update_int_moderator(hw);
+
+	/* clear down bit to indicate we are ready to go */
+	clear_bit(__FM10K_DOWN, &interface->state);
+
+	/* enable polling cleanups */
+	fm10k_napi_enable_all(interface);
+
+	/* re-establish Rx filters */
+	fm10k_restore_rx_state(interface);
+
+	/* enable transmits */
+	netif_tx_start_all_queues(interface->netdev);
+
+	/* kick off the service timer */
+	hw->mac.get_host_state = 1;
+	mod_timer(&interface->service_timer, jiffies);
+}
+
+static void fm10k_napi_disable_all(struct fm10k_intfc *interface)
+{
+	struct fm10k_q_vector *q_vector;
+	int q_idx;
+
+	for (q_idx = 0; q_idx < interface->num_q_vectors; q_idx++) {
+		q_vector = interface->q_vector[q_idx];
+		napi_disable(&q_vector->napi);
+	}
+}
+
+void fm10k_down(struct fm10k_intfc *interface)
+{
+	struct net_device *netdev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+
+	/* signal that we are down to the interrupt handler and service task */
+	set_bit(__FM10K_DOWN, &interface->state);
+
+	/* call carrier off first to avoid false dev_watchdog timeouts */
+	netif_carrier_off(netdev);
+
+	/* disable transmits */
+	netif_tx_stop_all_queues(netdev);
+	netif_tx_disable(netdev);
+
+	/* reset Rx filters */
+	fm10k_reset_rx_state(interface);
+
+	/* allow 10ms for device to quiesce */
+	usleep_range(10000, 20000);
+
+	/* disable polling routines */
+	fm10k_napi_disable_all(interface);
+
+	del_timer_sync(&interface->service_timer);
+
+	/* capture stats one last time before stopping interface */
+	fm10k_update_stats(interface);
+
+	/* Disable DMA engine for Tx/Rx */
+	hw->mac.ops.stop_hw(hw);
+
+	/* free any buffers still on the rings */
+	fm10k_clean_all_tx_rings(interface);
+}
+
+/**
+ * fm10k_sw_init - Initialize general software structures
+ * @interface: host interface private structure to initialize
+ *
+ * fm10k_sw_init initializes the interface private data structure.
+ * Fields are initialized based on PCI device information and
+ * OS network device settings (MTU size).
+ **/
+static int fm10k_sw_init(struct fm10k_intfc *interface,
+			 const struct pci_device_id *ent)
+{
+	static const u32 seed[FM10K_RSSRK_SIZE] = { 0xda565a6d, 0xc20e5b25,
+						    0x3d256741, 0xb08fa343,
+						    0xcb2bcad0, 0xb4307bae,
+						    0xa32dcb77, 0x0cf23080,
+						    0x3bb7426a, 0xfa01acbe };
+	const struct fm10k_info *fi = fm10k_info_tbl[ent->driver_data];
+	struct fm10k_hw *hw = &interface->hw;
+	struct pci_dev *pdev = interface->pdev;
+	struct net_device *netdev = interface->netdev;
+	unsigned int rss;
+	int err;
+
+	/* initialize back pointer */
+	hw->back = interface;
+	hw->hw_addr = interface->uc_addr;
+
+	/* PCI config space info */
+	hw->vendor_id = pdev->vendor;
+	hw->device_id = pdev->device;
+	hw->revision_id = pdev->revision;
+	hw->subsystem_vendor_id = pdev->subsystem_vendor;
+	hw->subsystem_device_id = pdev->subsystem_device;
+
+	/* Setup hw api */
+	memcpy(&hw->mac.ops, fi->mac_ops, sizeof(hw->mac.ops));
+	hw->mac.type = fi->mac;
+
+	/* Setup IOV handlers */
+	if (fi->iov_ops)
+		memcpy(&hw->iov.ops, fi->iov_ops, sizeof(hw->iov.ops));
+
+	/* Set common capability flags and settings */
+	rss = min_t(int, FM10K_MAX_RSS_INDICES, num_online_cpus());
+	interface->ring_feature[RING_F_RSS].limit = rss;
+	fi->get_invariants(hw);
+
+	/* pick up the PCIe bus settings for reporting later */
+	if (hw->mac.ops.get_bus_info)
+		hw->mac.ops.get_bus_info(hw);
+
+	/* limit the usable DMA range */
+	if (hw->mac.ops.set_dma_mask)
+		hw->mac.ops.set_dma_mask(hw, dma_get_mask(&pdev->dev));
+
+	/* update netdev with DMA restrictions */
+	if (dma_get_mask(&pdev->dev) > DMA_BIT_MASK(32)) {
+		netdev->features |= NETIF_F_HIGHDMA;
+		netdev->vlan_features |= NETIF_F_HIGHDMA;
+	}
+
+	/* delay any future reset requests */
+	interface->last_reset = jiffies + (10 * HZ);
+
+	/* reset and initialize the hardware so it is in a known state */
+	err = hw->mac.ops.reset_hw(hw) ? : hw->mac.ops.init_hw(hw);
+	if (err) {
+		dev_err(&pdev->dev, "init_hw failed: %d\n", err);
+		return err;
+	}
+
+	/* initialize hardware statistics */
+	hw->mac.ops.update_hw_stats(hw, &interface->stats);
+
+	/* Set upper limit on IOV VFs that can be allocated */
+	pci_sriov_set_totalvfs(pdev, hw->iov.total_vfs);
+
+	/* Start with random Ethernet address */
+	eth_random_addr(hw->mac.addr);
+
+	/* Initialize MAC address from hardware */
+	err = hw->mac.ops.read_mac_addr(hw);
+	if (err) {
+		dev_warn(&pdev->dev,
+			 "Failed to obtain MAC address defaulting to random\n");
+		/* tag address assignment as random */
+		netdev->addr_assign_type |= NET_ADDR_RANDOM;
+	}
+
+	memcpy(netdev->dev_addr, hw->mac.addr, netdev->addr_len);
+	memcpy(netdev->perm_addr, hw->mac.addr, netdev->addr_len);
+
+	if (!is_valid_ether_addr(netdev->perm_addr)) {
+		dev_err(&pdev->dev, "Invalid MAC Address\n");
+		return -EIO;
+	}
+
+	/* assign BAR 4 resources for use with PTP */
+	if (fm10k_read_reg(hw, FM10K_CTRL) & FM10K_CTRL_BAR4_ALLOWED)
+		interface->sw_addr = ioremap(pci_resource_start(pdev, 4),
+					     pci_resource_len(pdev, 4));
+	hw->sw_addr = interface->sw_addr;
+
+	/* Only the PF can support VXLAN and NVGRE offloads */
+	if (hw->mac.type != fm10k_mac_pf) {
+		netdev->hw_enc_features = 0;
+		netdev->features &= ~NETIF_F_GSO_UDP_TUNNEL;
+		netdev->hw_features &= ~NETIF_F_GSO_UDP_TUNNEL;
+	}
+
+	/* initialize DCBNL interface */
+	fm10k_dcbnl_set_ops(netdev);
+
+	/* Initialize service timer and service task */
+	set_bit(__FM10K_SERVICE_DISABLE, &interface->state);
+	setup_timer(&interface->service_timer, &fm10k_service_timer,
+		    (unsigned long)interface);
+	INIT_WORK(&interface->service_task, fm10k_service_task);
+
+	/* Intitialize timestamp data */
+	fm10k_ts_init(interface);
+
+	/* set default ring sizes */
+	interface->tx_ring_count = FM10K_DEFAULT_TXD;
+	interface->rx_ring_count = FM10K_DEFAULT_RXD;
+
+	/* set default interrupt moderation */
+	interface->tx_itr = FM10K_ITR_10K;
+	interface->rx_itr = FM10K_ITR_ADAPTIVE | FM10K_ITR_20K;
+
+	/* initialize vxlan_port list */
+	INIT_LIST_HEAD(&interface->vxlan_port);
+
+	/* initialize RSS key */
+	memcpy(interface->rssrk, seed, sizeof(seed));
+
+	/* Start off interface as being down */
+	set_bit(__FM10K_DOWN, &interface->state);
+
+	return 0;
+}
+
+static void fm10k_slot_warn(struct fm10k_intfc *interface)
+{
+	struct device *dev = &interface->pdev->dev;
+	struct fm10k_hw *hw = &interface->hw;
+
+	if (hw->mac.ops.is_slot_appropriate(hw))
+		return;
+
+	dev_warn(dev,
+		 "For optimal performance, a %s %s slot is recommended.\n",
+		 (hw->bus_caps.width == fm10k_bus_width_pcie_x1 ? "x1" :
+		  hw->bus_caps.width == fm10k_bus_width_pcie_x4 ? "x4" :
+		  "x8"),
+		 (hw->bus_caps.speed == fm10k_bus_speed_2500 ? "2.5GT/s" :
+		  hw->bus_caps.speed == fm10k_bus_speed_5000 ? "5.0GT/s" :
+		  "8.0GT/s"));
+	dev_warn(dev,
+		 "A slot with more lanes and/or higher speed is suggested.\n");
+}
+
+/**
+ * fm10k_probe - Device Initialization Routine
+ * @pdev: PCI device information struct
+ * @ent: entry in fm10k_pci_tbl
+ *
+ * Returns 0 on success, negative on failure
+ *
+ * fm10k_probe initializes an interface identified by a pci_dev structure.
+ * The OS initialization, configuring of the interface private structure,
+ * and a hardware reset occur.
+ **/
+static int fm10k_probe(struct pci_dev *pdev,
+		       const struct pci_device_id *ent)
+{
+	struct net_device *netdev;
+	struct fm10k_intfc *interface;
+	struct fm10k_hw *hw;
+	int err;
+	u64 dma_mask;
+
+	err = pci_enable_device_mem(pdev);
+	if (err)
+		return err;
+
+	/* By default fm10k only supports a 48 bit DMA mask */
+	dma_mask = DMA_BIT_MASK(48) | dma_get_required_mask(&pdev->dev);
+
+	if ((dma_mask <= DMA_BIT_MASK(32)) ||
+	    dma_set_mask_and_coherent(&pdev->dev, dma_mask)) {
+		dma_mask &= DMA_BIT_MASK(32);
+
+		err = dma_set_mask_and_coherent(&pdev->dev, DMA_BIT_MASK(32));
+		err = dma_set_mask(&pdev->dev, DMA_BIT_MASK(32));
+		if (err) {
+			err = dma_set_coherent_mask(&pdev->dev,
+						    DMA_BIT_MASK(32));
+			if (err) {
+				dev_err(&pdev->dev,
+					"No usable DMA configuration, aborting\n");
+				goto err_dma;
+			}
+		}
+	}
+
+	err = pci_request_selected_regions(pdev,
+					   pci_select_bars(pdev,
+							   IORESOURCE_MEM),
+					   fm10k_driver_name);
+	if (err) {
+		dev_err(&pdev->dev,
+			"pci_request_selected_regions failed 0x%x\n", err);
+		goto err_pci_reg;
+	}
+
+	pci_enable_pcie_error_reporting(pdev);
+
+	pci_set_master(pdev);
+	pci_save_state(pdev);
+
+	netdev = fm10k_alloc_netdev();
+	if (!netdev) {
+		err = -ENOMEM;
+		goto err_alloc_netdev;
+	}
+
+	SET_NETDEV_DEV(netdev, &pdev->dev);
+
+	interface = netdev_priv(netdev);
+	pci_set_drvdata(pdev, interface);
+
+	interface->netdev = netdev;
+	interface->pdev = pdev;
+	hw = &interface->hw;
+
+	interface->uc_addr = ioremap(pci_resource_start(pdev, 0),
+				     FM10K_UC_ADDR_SIZE);
+	if (!interface->uc_addr) {
+		err = -EIO;
+		goto err_ioremap;
+	}
+
+	err = fm10k_sw_init(interface, ent);
+	if (err)
+		goto err_sw_init;
+
+	/* enable debugfs support */
+	fm10k_dbg_intfc_init(interface);
+
+	err = fm10k_init_queueing_scheme(interface);
+	if (err)
+		goto err_sw_init;
+
+	err = fm10k_mbx_request_irq(interface);
+	if (err)
+		goto err_mbx_interrupt;
+
+	/* final check of hardware state before registering the interface */
+	err = fm10k_hw_ready(interface);
+	if (err)
+		goto err_register;
+
+	err = register_netdev(netdev);
+	if (err)
+		goto err_register;
+
+	/* carrier off reporting is important to ethtool even BEFORE open */
+	netif_carrier_off(netdev);
+
+	/* stop all the transmit queues from transmitting until link is up */
+	netif_tx_stop_all_queues(netdev);
+
+	/* Register PTP interface */
+	fm10k_ptp_register(interface);
+
+	/* print bus type/speed/width info */
+	dev_info(&pdev->dev, "(PCI Express:%s Width: %s Payload: %s)\n",
+		 (hw->bus.speed == fm10k_bus_speed_8000 ? "8.0GT/s" :
+		  hw->bus.speed == fm10k_bus_speed_5000 ? "5.0GT/s" :
+		  hw->bus.speed == fm10k_bus_speed_2500 ? "2.5GT/s" :
+		  "Unknown"),
+		 (hw->bus.width == fm10k_bus_width_pcie_x8 ? "x8" :
+		  hw->bus.width == fm10k_bus_width_pcie_x4 ? "x4" :
+		  hw->bus.width == fm10k_bus_width_pcie_x1 ? "x1" :
+		  "Unknown"),
+		 (hw->bus.payload == fm10k_bus_payload_128 ? "128B" :
+		  hw->bus.payload == fm10k_bus_payload_256 ? "256B" :
+		  hw->bus.payload == fm10k_bus_payload_512 ? "512B" :
+		  "Unknown"));
+
+	/* print warning for non-optimal configurations */
+	fm10k_slot_warn(interface);
+
+	/* enable SR-IOV after registering netdev to enforce PF/VF ordering */
+	fm10k_iov_configure(pdev, 0);
+
+	/* clear the service task disable bit to allow service task to start */
+	clear_bit(__FM10K_SERVICE_DISABLE, &interface->state);
+
+	return 0;
+
+err_register:
+	fm10k_mbx_free_irq(interface);
+err_mbx_interrupt:
+	fm10k_clear_queueing_scheme(interface);
+err_sw_init:
+	if (interface->sw_addr)
+		iounmap(interface->sw_addr);
+	iounmap(interface->uc_addr);
+err_ioremap:
+	free_netdev(netdev);
+err_alloc_netdev:
+	pci_release_selected_regions(pdev,
+				     pci_select_bars(pdev, IORESOURCE_MEM));
+err_pci_reg:
+err_dma:
+	pci_disable_device(pdev);
+	return err;
+}
+
+/**
+ * fm10k_remove - Device Removal Routine
+ * @pdev: PCI device information struct
+ *
+ * fm10k_remove is called by the PCI subsystem to alert the driver
+ * that it should release a PCI device.  The could be caused by a
+ * Hot-Plug event, or because the driver is going to be removed from
+ * memory.
+ **/
+static void fm10k_remove(struct pci_dev *pdev)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct net_device *netdev = interface->netdev;
+
+	set_bit(__FM10K_SERVICE_DISABLE, &interface->state);
+	cancel_work_sync(&interface->service_task);
+
+	/* free netdev, this may bounce the interrupts due to setup_tc */
+	if (netdev->reg_state == NETREG_REGISTERED)
+		unregister_netdev(netdev);
+
+	/* cleanup timestamp handling */
+	fm10k_ptp_unregister(interface);
+
+	/* release VFs */
+	fm10k_iov_disable(pdev);
+
+	/* disable mailbox interrupt */
+	fm10k_mbx_free_irq(interface);
+
+	/* free interrupts */
+	fm10k_clear_queueing_scheme(interface);
+
+	/* remove any debugfs interfaces */
+	fm10k_dbg_intfc_exit(interface);
+
+	if (interface->sw_addr)
+		iounmap(interface->sw_addr);
+	iounmap(interface->uc_addr);
+
+	free_netdev(netdev);
+
+	pci_release_selected_regions(pdev,
+				     pci_select_bars(pdev, IORESOURCE_MEM));
+
+	pci_disable_pcie_error_reporting(pdev);
+
+	pci_disable_device(pdev);
+}
+
+#ifdef CONFIG_PM
+/**
+ * fm10k_resume - Restore device to pre-sleep state
+ * @pdev: PCI device information struct
+ *
+ * fm10k_resume is called after the system has powered back up from a sleep
+ * state and is ready to resume operation.  This function is meant to restore
+ * the device back to its pre-sleep state.
+ **/
+static int fm10k_resume(struct pci_dev *pdev)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct net_device *netdev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	u32 err;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+
+	/* pci_restore_state clears dev->state_saved so call
+	 * pci_save_state to restore it.
+	 */
+	pci_save_state(pdev);
+
+	err = pci_enable_device_mem(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "Cannot enable PCI device from suspend\n");
+		return err;
+	}
+	pci_set_master(pdev);
+
+	pci_wake_from_d3(pdev, false);
+
+	/* refresh hw_addr in case it was dropped */
+	hw->hw_addr = interface->uc_addr;
+
+	/* reset hardware to known state */
+	err = hw->mac.ops.init_hw(&interface->hw);
+	if (err)
+		return err;
+
+	/* reset statistics starting values */
+	hw->mac.ops.rebind_hw_stats(hw, &interface->stats);
+
+	/* reset clock */
+	fm10k_ts_reset(interface);
+
+	rtnl_lock();
+
+	err = fm10k_init_queueing_scheme(interface);
+	if (!err) {
+		fm10k_mbx_request_irq(interface);
+		if (netif_running(netdev))
+			err = fm10k_open(netdev);
+	}
+
+	rtnl_unlock();
+
+	if (err)
+		return err;
+
+	/* restore SR-IOV interface */
+	fm10k_iov_resume(pdev);
+
+	netif_device_attach(netdev);
+
+	return 0;
+}
+
+/**
+ * fm10k_suspend - Prepare the device for a system sleep state
+ * @pdev: PCI device information struct
+ *
+ * fm10k_suspend is meant to shutdown the device prior to the system entering
+ * a sleep state.  The fm10k hardware does not support wake on lan so the
+ * driver simply needs to shut down the device so it is in a low power state.
+ **/
+static int fm10k_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct net_device *netdev = interface->netdev;
+	int err = 0;
+
+	netif_device_detach(netdev);
+
+	fm10k_iov_suspend(pdev);
+
+	rtnl_lock();
+
+	if (netif_running(netdev))
+		fm10k_close(netdev);
+
+	fm10k_mbx_free_irq(interface);
+
+	fm10k_clear_queueing_scheme(interface);
+
+	rtnl_unlock();
+
+	err = pci_save_state(pdev);
+	if (err)
+		return err;
+
+	pci_disable_device(pdev);
+	pci_wake_from_d3(pdev, false);
+	pci_set_power_state(pdev, PCI_D3hot);
+
+	return 0;
+}
+
+#endif /* CONFIG_PM */
+/**
+ * fm10k_io_error_detected - called when PCI error is detected
+ * @pdev: Pointer to PCI device
+ * @state: The current pci connection state
+ *
+ * This function is called after a PCI bus error affecting
+ * this device has been detected.
+ */
+static pci_ers_result_t fm10k_io_error_detected(struct pci_dev *pdev,
+						pci_channel_state_t state)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct net_device *netdev = interface->netdev;
+
+	netif_device_detach(netdev);
+
+	if (state == pci_channel_io_perm_failure)
+		return PCI_ERS_RESULT_DISCONNECT;
+
+	if (netif_running(netdev))
+		fm10k_close(netdev);
+
+	fm10k_mbx_free_irq(interface);
+
+	pci_disable_device(pdev);
+
+	/* Request a slot reset. */
+	return PCI_ERS_RESULT_NEED_RESET;
+}
+
+/**
+ * fm10k_io_slot_reset - called after the pci bus has been reset.
+ * @pdev: Pointer to PCI device
+ *
+ * Restart the card from scratch, as if from a cold-boot.
+ */
+static pci_ers_result_t fm10k_io_slot_reset(struct pci_dev *pdev)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	pci_ers_result_t result;
+
+	if (pci_enable_device_mem(pdev)) {
+		dev_err(&pdev->dev,
+			"Cannot re-enable PCI device after reset.\n");
+		result = PCI_ERS_RESULT_DISCONNECT;
+	} else {
+		pci_set_master(pdev);
+		pci_restore_state(pdev);
+
+		/* After second error pci->state_saved is false, this
+		 * resets it so EEH doesn't break.
+		 */
+		pci_save_state(pdev);
+
+		pci_wake_from_d3(pdev, false);
+
+		/* refresh hw_addr in case it was dropped */
+		interface->hw.hw_addr = interface->uc_addr;
+
+		interface->flags |= FM10K_FLAG_RESET_REQUESTED;
+		fm10k_service_event_schedule(interface);
+
+		result = PCI_ERS_RESULT_RECOVERED;
+	}
+
+	pci_cleanup_aer_uncorrect_error_status(pdev);
+
+	return result;
+}
+
+/**
+ * fm10k_io_resume - called when traffic can start flowing again.
+ * @pdev: Pointer to PCI device
+ *
+ * This callback is called when the error recovery driver tells us that
+ * its OK to resume normal operation.
+ */
+static void fm10k_io_resume(struct pci_dev *pdev)
+{
+	struct fm10k_intfc *interface = pci_get_drvdata(pdev);
+	struct net_device *netdev = interface->netdev;
+	struct fm10k_hw *hw = &interface->hw;
+	int err = 0;
+
+	/* reset hardware to known state */
+	hw->mac.ops.init_hw(&interface->hw);
+
+	/* reset statistics starting values */
+	hw->mac.ops.rebind_hw_stats(hw, &interface->stats);
+
+	/* reassociate interrupts */
+	fm10k_mbx_request_irq(interface);
+
+	/* reset clock */
+	fm10k_ts_reset(interface);
+
+	if (netif_running(netdev))
+		err = fm10k_open(netdev);
+
+	/* final check of hardware state before registering the interface */
+	err = err ? : fm10k_hw_ready(interface);
+
+	if (!err)
+		netif_device_attach(netdev);
+}
+
+static const struct pci_error_handlers fm10k_err_handler = {
+	.error_detected = fm10k_io_error_detected,
+	.slot_reset = fm10k_io_slot_reset,
+	.resume = fm10k_io_resume,
+};
+
+static struct pci_driver fm10k_driver = {
+	.name			= fm10k_driver_name,
+	.id_table		= fm10k_pci_tbl,
+	.probe			= fm10k_probe,
+	.remove			= fm10k_remove,
+#ifdef CONFIG_PM
+	.suspend		= fm10k_suspend,
+	.resume			= fm10k_resume,
+#endif
+	.sriov_configure	= fm10k_iov_configure,
+	.err_handler		= &fm10k_err_handler
+};
+
+/**
+ * fm10k_register_pci_driver - register driver interface
+ *
+ * This funciton is called on module load in order to register the driver.
+ **/
+int fm10k_register_pci_driver(void)
+{
+	return pci_register_driver(&fm10k_driver);
+}
+
+/**
+ * fm10k_unregister_pci_driver - unregister driver interface
+ *
+ * This funciton is called on module unload in order to remove the driver.
+ **/
+void fm10k_unregister_pci_driver(void)
+{
+	pci_unregister_driver(&fm10k_driver);
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.c b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
new file mode 100644
index 000000000000..275423d4f777
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.c
@@ -0,0 +1,1880 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k_pf.h"
+#include "fm10k_vf.h"
+
+/**
+ *  fm10k_reset_hw_pf - PF hardware reset
+ *  @hw: pointer to hardware structure
+ *
+ *  This function should return the hardware to a state similar to the
+ *  one it is in after being powered on.
+ **/
+static s32 fm10k_reset_hw_pf(struct fm10k_hw *hw)
+{
+	s32 err;
+	u32 reg;
+	u16 i;
+
+	/* Disable interrupts */
+	fm10k_write_reg(hw, FM10K_EIMR, FM10K_EIMR_DISABLE(ALL));
+
+	/* Lock ITR2 reg 0 into itself and disable interrupt moderation */
+	fm10k_write_reg(hw, FM10K_ITR2(0), 0);
+	fm10k_write_reg(hw, FM10K_INT_CTRL, 0);
+
+	/* We assume here Tx and Rx queue 0 are owned by the PF */
+
+	/* Shut off VF access to their queues forcing them to queue 0 */
+	for (i = 0; i < FM10K_TQMAP_TABLE_SIZE; i++) {
+		fm10k_write_reg(hw, FM10K_TQMAP(i), 0);
+		fm10k_write_reg(hw, FM10K_RQMAP(i), 0);
+	}
+
+	/* shut down all rings */
+	err = fm10k_disable_queues_generic(hw, FM10K_MAX_QUEUES);
+	if (err)
+		return err;
+
+	/* Verify that DMA is no longer active */
+	reg = fm10k_read_reg(hw, FM10K_DMA_CTRL);
+	if (reg & (FM10K_DMA_CTRL_TX_ACTIVE | FM10K_DMA_CTRL_RX_ACTIVE))
+		return FM10K_ERR_DMA_PENDING;
+
+	/* Inititate data path reset */
+	reg |= FM10K_DMA_CTRL_DATAPATH_RESET;
+	fm10k_write_reg(hw, FM10K_DMA_CTRL, reg);
+
+	/* Flush write and allow 100us for reset to complete */
+	fm10k_write_flush(hw);
+	udelay(FM10K_RESET_TIMEOUT);
+
+	/* Verify we made it out of reset */
+	reg = fm10k_read_reg(hw, FM10K_IP);
+	if (!(reg & FM10K_IP_NOTINRESET))
+		err = FM10K_ERR_RESET_FAILED;
+
+	return err;
+}
+
+/**
+ *  fm10k_is_ari_hierarchy_pf - Indicate ARI hierarchy support
+ *  @hw: pointer to hardware structure
+ *
+ *  Looks at the ARI hierarchy bit to determine whether ARI is supported or not.
+ **/
+static bool fm10k_is_ari_hierarchy_pf(struct fm10k_hw *hw)
+{
+	u16 sriov_ctrl = fm10k_read_pci_cfg_word(hw, FM10K_PCIE_SRIOV_CTRL);
+
+	return !!(sriov_ctrl & FM10K_PCIE_SRIOV_CTRL_VFARI);
+}
+
+/**
+ *  fm10k_init_hw_pf - PF hardware initialization
+ *  @hw: pointer to hardware structure
+ *
+ **/
+static s32 fm10k_init_hw_pf(struct fm10k_hw *hw)
+{
+	u32 dma_ctrl, txqctl;
+	u16 i;
+
+	/* Establish default VSI as valid */
+	fm10k_write_reg(hw, FM10K_DGLORTDEC(fm10k_dglort_default), 0);
+	fm10k_write_reg(hw, FM10K_DGLORTMAP(fm10k_dglort_default),
+			FM10K_DGLORTMAP_ANY);
+
+	/* Invalidate all other GLORT entries */
+	for (i = 1; i < FM10K_DGLORT_COUNT; i++)
+		fm10k_write_reg(hw, FM10K_DGLORTMAP(i), FM10K_DGLORTMAP_NONE);
+
+	/* reset ITR2(0) to point to itself */
+	fm10k_write_reg(hw, FM10K_ITR2(0), 0);
+
+	/* reset VF ITR2(0) to point to 0 avoid PF registers */
+	fm10k_write_reg(hw, FM10K_ITR2(FM10K_ITR_REG_COUNT_PF), 0);
+
+	/* loop through all PF ITR2 registers pointing them to the previous */
+	for (i = 1; i < FM10K_ITR_REG_COUNT_PF; i++)
+		fm10k_write_reg(hw, FM10K_ITR2(i), i - 1);
+
+	/* Enable interrupt moderator if not already enabled */
+	fm10k_write_reg(hw, FM10K_INT_CTRL, FM10K_INT_CTRL_ENABLEMODERATOR);
+
+	/* compute the default txqctl configuration */
+	txqctl = FM10K_TXQCTL_PF | FM10K_TXQCTL_UNLIMITED_BW |
+		 (hw->mac.default_vid << FM10K_TXQCTL_VID_SHIFT);
+
+	for (i = 0; i < FM10K_MAX_QUEUES; i++) {
+		/* configure rings for 256 Queue / 32 Descriptor cache mode */
+		fm10k_write_reg(hw, FM10K_TQDLOC(i),
+				(i * FM10K_TQDLOC_BASE_32_DESC) |
+				FM10K_TQDLOC_SIZE_32_DESC);
+		fm10k_write_reg(hw, FM10K_TXQCTL(i), txqctl);
+
+		/* configure rings to provide TPH processing hints */
+		fm10k_write_reg(hw, FM10K_TPH_TXCTRL(i),
+				FM10K_TPH_TXCTRL_DESC_TPHEN |
+				FM10K_TPH_TXCTRL_DESC_RROEN |
+				FM10K_TPH_TXCTRL_DESC_WROEN |
+				FM10K_TPH_TXCTRL_DATA_RROEN);
+		fm10k_write_reg(hw, FM10K_TPH_RXCTRL(i),
+				FM10K_TPH_RXCTRL_DESC_TPHEN |
+				FM10K_TPH_RXCTRL_DESC_RROEN |
+				FM10K_TPH_RXCTRL_DATA_WROEN |
+				FM10K_TPH_RXCTRL_HDR_WROEN);
+	}
+
+	/* set max hold interval to align with 1.024 usec in all modes */
+	switch (hw->bus.speed) {
+	case fm10k_bus_speed_2500:
+		dma_ctrl = FM10K_DMA_CTRL_MAX_HOLD_1US_GEN1;
+		break;
+	case fm10k_bus_speed_5000:
+		dma_ctrl = FM10K_DMA_CTRL_MAX_HOLD_1US_GEN2;
+		break;
+	case fm10k_bus_speed_8000:
+		dma_ctrl = FM10K_DMA_CTRL_MAX_HOLD_1US_GEN3;
+		break;
+	default:
+		dma_ctrl = 0;
+		break;
+	}
+
+	/* Configure TSO flags */
+	fm10k_write_reg(hw, FM10K_DTXTCPFLGL, FM10K_TSO_FLAGS_LOW);
+	fm10k_write_reg(hw, FM10K_DTXTCPFLGH, FM10K_TSO_FLAGS_HI);
+
+	/* Enable DMA engine
+	 * Set Rx Descriptor size to 32
+	 * Set Minimum MSS to 64
+	 * Set Maximum number of Rx queues to 256 / 32 Descriptor
+	 */
+	dma_ctrl |= FM10K_DMA_CTRL_TX_ENABLE | FM10K_DMA_CTRL_RX_ENABLE |
+		    FM10K_DMA_CTRL_RX_DESC_SIZE | FM10K_DMA_CTRL_MINMSS_64 |
+		    FM10K_DMA_CTRL_32_DESC;
+
+	fm10k_write_reg(hw, FM10K_DMA_CTRL, dma_ctrl);
+
+	/* record maximum queue count, we limit ourselves to 128 */
+	hw->mac.max_queues = FM10K_MAX_QUEUES_PF;
+
+	/* We support either 64 VFs or 7 VFs depending on if we have ARI */
+	hw->iov.total_vfs = fm10k_is_ari_hierarchy_pf(hw) ? 64 : 7;
+
+	return 0;
+}
+
+/**
+ *  fm10k_is_slot_appropriate_pf - Indicate appropriate slot for this SKU
+ *  @hw: pointer to hardware structure
+ *
+ *  Looks at the PCIe bus info to confirm whether or not this slot can support
+ *  the necessary bandwidth for this device.
+ **/
+static bool fm10k_is_slot_appropriate_pf(struct fm10k_hw *hw)
+{
+	return (hw->bus.speed == hw->bus_caps.speed) &&
+	       (hw->bus.width == hw->bus_caps.width);
+}
+
+/**
+ *  fm10k_update_vlan_pf - Update status of VLAN ID in VLAN filter table
+ *  @hw: pointer to hardware structure
+ *  @vid: VLAN ID to add to table
+ *  @vsi: Index indicating VF ID or PF ID in table
+ *  @set: Indicates if this is a set or clear operation
+ *
+ *  This function adds or removes the corresponding VLAN ID from the VLAN
+ *  filter table for the corresponding function.  In addition to the
+ *  standard set/clear that supports one bit a multi-bit write is
+ *  supported to set 64 bits at a time.
+ **/
+static s32 fm10k_update_vlan_pf(struct fm10k_hw *hw, u32 vid, u8 vsi, bool set)
+{
+	u32 vlan_table, reg, mask, bit, len;
+
+	/* verify the VSI index is valid */
+	if (vsi > FM10K_VLAN_TABLE_VSI_MAX)
+		return FM10K_ERR_PARAM;
+
+	/* VLAN multi-bit write:
+	 * The multi-bit write has several parts to it.
+	 *    3			  2		      1			  0
+	 *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+	 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	 * | RSVD0 |         Length        |C|RSVD0|        VLAN ID        |
+	 * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+	 *
+	 * VLAN ID: Vlan Starting value
+	 * RSVD0: Reserved section, must be 0
+	 * C: Flag field, 0 is set, 1 is clear (Used in VF VLAN message)
+	 * Length: Number of times to repeat the bit being set
+	 */
+	len = vid >> 16;
+	vid = (vid << 17) >> 17;
+
+	/* verify the reserved 0 fields are 0 */
+	if (len >= FM10K_VLAN_TABLE_VID_MAX ||
+	    vid >= FM10K_VLAN_TABLE_VID_MAX)
+		return FM10K_ERR_PARAM;
+
+	/* Loop through the table updating all required VLANs */
+	for (reg = FM10K_VLAN_TABLE(vsi, vid / 32), bit = vid % 32;
+	     len < FM10K_VLAN_TABLE_VID_MAX;
+	     len -= 32 - bit, reg++, bit = 0) {
+		/* record the initial state of the register */
+		vlan_table = fm10k_read_reg(hw, reg);
+
+		/* truncate mask if we are at the start or end of the run */
+		mask = (~(u32)0 >> ((len < 31) ? 31 - len : 0)) << bit;
+
+		/* make necessary modifications to the register */
+		mask &= set ? ~vlan_table : vlan_table;
+		if (mask)
+			fm10k_write_reg(hw, reg, vlan_table ^ mask);
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_read_mac_addr_pf - Read device MAC address
+ *  @hw: pointer to the HW structure
+ *
+ *  Reads the device MAC address from the SM_AREA and stores the value.
+ **/
+static s32 fm10k_read_mac_addr_pf(struct fm10k_hw *hw)
+{
+	u8 perm_addr[ETH_ALEN];
+	u32 serial_num;
+	int i;
+
+	serial_num = fm10k_read_reg(hw, FM10K_SM_AREA(1));
+
+	/* last byte should be all 1's */
+	if ((~serial_num) << 24)
+		return  FM10K_ERR_INVALID_MAC_ADDR;
+
+	perm_addr[0] = (u8)(serial_num >> 24);
+	perm_addr[1] = (u8)(serial_num >> 16);
+	perm_addr[2] = (u8)(serial_num >> 8);
+
+	serial_num = fm10k_read_reg(hw, FM10K_SM_AREA(0));
+
+	/* first byte should be all 1's */
+	if ((~serial_num) >> 24)
+		return  FM10K_ERR_INVALID_MAC_ADDR;
+
+	perm_addr[3] = (u8)(serial_num >> 16);
+	perm_addr[4] = (u8)(serial_num >> 8);
+	perm_addr[5] = (u8)(serial_num);
+
+	for (i = 0; i < ETH_ALEN; i++) {
+		hw->mac.perm_addr[i] = perm_addr[i];
+		hw->mac.addr[i] = perm_addr[i];
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_glort_valid_pf - Validate that the provided glort is valid
+ *  @hw: pointer to the HW structure
+ *  @glort: base glort to be validated
+ *
+ *  This function will return an error if the provided glort is invalid
+ **/
+bool fm10k_glort_valid_pf(struct fm10k_hw *hw, u16 glort)
+{
+	glort &= hw->mac.dglort_map >> FM10K_DGLORTMAP_MASK_SHIFT;
+
+	return glort == (hw->mac.dglort_map & FM10K_DGLORTMAP_NONE);
+}
+
+/**
+ *  fm10k_update_uc_addr_pf - Update device unicast addresss
+ *  @hw: pointer to the HW structure
+ *  @glort: base resource tag for this request
+ *  @mac: MAC address to add/remove from table
+ *  @vid: VLAN ID to add/remove from table
+ *  @add: Indicates if this is an add or remove operation
+ *  @flags: flags field to indicate add and secure
+ *
+ *  This function generates a message to the Switch API requesting
+ *  that the given logical port add/remove the given L2 MAC/VLAN address.
+ **/
+static s32 fm10k_update_xc_addr_pf(struct fm10k_hw *hw, u16 glort,
+				   const u8 *mac, u16 vid, bool add, u8 flags)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	struct fm10k_mac_update mac_update;
+	u32 msg[5];
+
+	/* if glort is not valid return error */
+	if (!fm10k_glort_valid_pf(hw, glort))
+		return FM10K_ERR_PARAM;
+
+	/* drop upper 4 bits of VLAN ID */
+	vid = (vid << 4) >> 4;
+
+	/* record fields */
+	mac_update.mac_lower = cpu_to_le32(((u32)mac[2] << 24) |
+						 ((u32)mac[3] << 16) |
+						 ((u32)mac[4] << 8) |
+						 ((u32)mac[5]));
+	mac_update.mac_upper = cpu_to_le16(((u32)mac[0] << 8) |
+						 ((u32)mac[1]));
+	mac_update.vlan = cpu_to_le16(vid);
+	mac_update.glort = cpu_to_le16(glort);
+	mac_update.action = add ? 0 : 1;
+	mac_update.flags = flags;
+
+	/* populate mac_update fields */
+	fm10k_tlv_msg_init(msg, FM10K_PF_MSG_ID_UPDATE_MAC_FWD_RULE);
+	fm10k_tlv_attr_put_le_struct(msg, FM10K_PF_ATTR_ID_MAC_UPDATE,
+				     &mac_update, sizeof(mac_update));
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_update_uc_addr_pf - Update device unicast addresss
+ *  @hw: pointer to the HW structure
+ *  @glort: base resource tag for this request
+ *  @mac: MAC address to add/remove from table
+ *  @vid: VLAN ID to add/remove from table
+ *  @add: Indicates if this is an add or remove operation
+ *  @flags: flags field to indicate add and secure
+ *
+ *  This function is used to add or remove unicast addresses for
+ *  the PF.
+ **/
+static s32 fm10k_update_uc_addr_pf(struct fm10k_hw *hw, u16 glort,
+				   const u8 *mac, u16 vid, bool add, u8 flags)
+{
+	/* verify MAC address is valid */
+	if (!is_valid_ether_addr(mac))
+		return FM10K_ERR_PARAM;
+
+	return fm10k_update_xc_addr_pf(hw, glort, mac, vid, add, flags);
+}
+
+/**
+ *  fm10k_update_mc_addr_pf - Update device multicast addresses
+ *  @hw: pointer to the HW structure
+ *  @glort: base resource tag for this request
+ *  @mac: MAC address to add/remove from table
+ *  @vid: VLAN ID to add/remove from table
+ *  @add: Indicates if this is an add or remove operation
+ *
+ *  This function is used to add or remove multicast MAC addresses for
+ *  the PF.
+ **/
+static s32 fm10k_update_mc_addr_pf(struct fm10k_hw *hw, u16 glort,
+				   const u8 *mac, u16 vid, bool add)
+{
+	/* verify multicast address is valid */
+	if (!is_multicast_ether_addr(mac))
+		return FM10K_ERR_PARAM;
+
+	return fm10k_update_xc_addr_pf(hw, glort, mac, vid, add, 0);
+}
+
+/**
+ *  fm10k_update_xcast_mode_pf - Request update of multicast mode
+ *  @hw: pointer to hardware structure
+ *  @glort: base resource tag for this request
+ *  @mode: integer value indicating mode being requested
+ *
+ *  This function will attempt to request a higher mode for the port
+ *  so that it can enable either multicast, multicast promiscuous, or
+ *  promiscuous mode of operation.
+ **/
+static s32 fm10k_update_xcast_mode_pf(struct fm10k_hw *hw, u16 glort, u8 mode)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[3], xcast_mode;
+
+	if (mode > FM10K_XCAST_MODE_NONE)
+		return FM10K_ERR_PARAM;
+	/* if glort is not valid return error */
+	if (!fm10k_glort_valid_pf(hw, glort))
+		return FM10K_ERR_PARAM;
+
+	/* write xcast mode as a single u32 value,
+	 * lower 16 bits: glort
+	 * upper 16 bits: mode
+	 */
+	xcast_mode = ((u32)mode << 16) | glort;
+
+	/* generate message requesting to change xcast mode */
+	fm10k_tlv_msg_init(msg, FM10K_PF_MSG_ID_XCAST_MODES);
+	fm10k_tlv_attr_put_u32(msg, FM10K_PF_ATTR_ID_XCAST_MODE, xcast_mode);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_update_int_moderator_pf - Update interrupt moderator linked list
+ *  @hw: pointer to hardware structure
+ *
+ *  This function walks through the MSI-X vector table to determine the
+ *  number of active interrupts and based on that information updates the
+ *  interrupt moderator linked list.
+ **/
+static void fm10k_update_int_moderator_pf(struct fm10k_hw *hw)
+{
+	u32 i;
+
+	/* Disable interrupt moderator */
+	fm10k_write_reg(hw, FM10K_INT_CTRL, 0);
+
+	/* loop through PF from last to first looking enabled vectors */
+	for (i = FM10K_ITR_REG_COUNT_PF - 1; i; i--) {
+		if (!fm10k_read_reg(hw, FM10K_MSIX_VECTOR_MASK(i)))
+			break;
+	}
+
+	/* always reset VFITR2[0] to point to last enabled PF vector*/
+	fm10k_write_reg(hw, FM10K_ITR2(FM10K_ITR_REG_COUNT_PF), i);
+
+	/* reset ITR2[0] to point to last enabled PF vector */
+	if (!hw->iov.num_vfs)
+		fm10k_write_reg(hw, FM10K_ITR2(0), i);
+
+	/* Enable interrupt moderator */
+	fm10k_write_reg(hw, FM10K_INT_CTRL, FM10K_INT_CTRL_ENABLEMODERATOR);
+}
+
+/**
+ *  fm10k_update_lport_state_pf - Notify the switch of a change in port state
+ *  @hw: pointer to the HW structure
+ *  @glort: base resource tag for this request
+ *  @count: number of logical ports being updated
+ *  @enable: boolean value indicating enable or disable
+ *
+ *  This function is used to add/remove a logical port from the switch.
+ **/
+static s32 fm10k_update_lport_state_pf(struct fm10k_hw *hw, u16 glort,
+				       u16 count, bool enable)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[3], lport_msg;
+
+	/* do nothing if we are being asked to create or destroy 0 ports */
+	if (!count)
+		return 0;
+
+	/* if glort is not valid return error */
+	if (!fm10k_glort_valid_pf(hw, glort))
+		return FM10K_ERR_PARAM;
+
+	/* construct the lport message from the 2 pieces of data we have */
+	lport_msg = ((u32)count << 16) | glort;
+
+	/* generate lport create/delete message */
+	fm10k_tlv_msg_init(msg, enable ? FM10K_PF_MSG_ID_LPORT_CREATE :
+					 FM10K_PF_MSG_ID_LPORT_DELETE);
+	fm10k_tlv_attr_put_u32(msg, FM10K_PF_ATTR_ID_PORT, lport_msg);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_configure_dglort_map_pf - Configures GLORT entry and queues
+ *  @hw: pointer to hardware structure
+ *  @dglort: pointer to dglort configuration structure
+ *
+ *  Reads the configuration structure contained in dglort_cfg and uses
+ *  that information to then populate a DGLORTMAP/DEC entry and the queues
+ *  to which it has been assigned.
+ **/
+static s32 fm10k_configure_dglort_map_pf(struct fm10k_hw *hw,
+					 struct fm10k_dglort_cfg *dglort)
+{
+	u16 glort, queue_count, vsi_count, pc_count;
+	u16 vsi, queue, pc, q_idx;
+	u32 txqctl, dglortdec, dglortmap;
+
+	/* verify the dglort pointer */
+	if (!dglort)
+		return FM10K_ERR_PARAM;
+
+	/* verify the dglort values */
+	if ((dglort->idx > 7) || (dglort->rss_l > 7) || (dglort->pc_l > 3) ||
+	    (dglort->vsi_l > 6) || (dglort->vsi_b > 64) ||
+	    (dglort->queue_l > 8) || (dglort->queue_b >= 256))
+		return FM10K_ERR_PARAM;
+
+	/* determine count of VSIs and queues */
+	queue_count = 1 << (dglort->rss_l + dglort->pc_l);
+	vsi_count = 1 << (dglort->vsi_l + dglort->queue_l);
+	glort = dglort->glort;
+	q_idx = dglort->queue_b;
+
+	/* configure SGLORT for queues */
+	for (vsi = 0; vsi < vsi_count; vsi++, glort++) {
+		for (queue = 0; queue < queue_count; queue++, q_idx++) {
+			if (q_idx >= FM10K_MAX_QUEUES)
+				break;
+
+			fm10k_write_reg(hw, FM10K_TX_SGLORT(q_idx), glort);
+			fm10k_write_reg(hw, FM10K_RX_SGLORT(q_idx), glort);
+		}
+	}
+
+	/* determine count of PCs and queues */
+	queue_count = 1 << (dglort->queue_l + dglort->rss_l + dglort->vsi_l);
+	pc_count = 1 << dglort->pc_l;
+
+	/* configure PC for Tx queues */
+	for (pc = 0; pc < pc_count; pc++) {
+		q_idx = pc + dglort->queue_b;
+		for (queue = 0; queue < queue_count; queue++) {
+			if (q_idx >= FM10K_MAX_QUEUES)
+				break;
+
+			txqctl = fm10k_read_reg(hw, FM10K_TXQCTL(q_idx));
+			txqctl &= ~FM10K_TXQCTL_PC_MASK;
+			txqctl |= pc << FM10K_TXQCTL_PC_SHIFT;
+			fm10k_write_reg(hw, FM10K_TXQCTL(q_idx), txqctl);
+
+			q_idx += pc_count;
+		}
+	}
+
+	/* configure DGLORTDEC */
+	dglortdec = ((u32)(dglort->rss_l) << FM10K_DGLORTDEC_RSSLENGTH_SHIFT) |
+		    ((u32)(dglort->queue_b) << FM10K_DGLORTDEC_QBASE_SHIFT) |
+		    ((u32)(dglort->pc_l) << FM10K_DGLORTDEC_PCLENGTH_SHIFT) |
+		    ((u32)(dglort->vsi_b) << FM10K_DGLORTDEC_VSIBASE_SHIFT) |
+		    ((u32)(dglort->vsi_l) << FM10K_DGLORTDEC_VSILENGTH_SHIFT) |
+		    ((u32)(dglort->queue_l));
+	if (dglort->inner_rss)
+		dglortdec |=  FM10K_DGLORTDEC_INNERRSS_ENABLE;
+
+	/* configure DGLORTMAP */
+	dglortmap = (dglort->idx == fm10k_dglort_default) ?
+			FM10K_DGLORTMAP_ANY : FM10K_DGLORTMAP_ZERO;
+	dglortmap <<= dglort->vsi_l + dglort->queue_l + dglort->shared_l;
+	dglortmap |= dglort->glort;
+
+	/* write values to hardware */
+	fm10k_write_reg(hw, FM10K_DGLORTDEC(dglort->idx), dglortdec);
+	fm10k_write_reg(hw, FM10K_DGLORTMAP(dglort->idx), dglortmap);
+
+	return 0;
+}
+
+u16 fm10k_queues_per_pool(struct fm10k_hw *hw)
+{
+	u16 num_pools = hw->iov.num_pools;
+
+	return (num_pools > 32) ? 2 : (num_pools > 16) ? 4 : (num_pools > 8) ?
+	       8 : FM10K_MAX_QUEUES_POOL;
+}
+
+u16 fm10k_vf_queue_index(struct fm10k_hw *hw, u16 vf_idx)
+{
+	u16 num_vfs = hw->iov.num_vfs;
+	u16 vf_q_idx = FM10K_MAX_QUEUES;
+
+	vf_q_idx -= fm10k_queues_per_pool(hw) * (num_vfs - vf_idx);
+
+	return vf_q_idx;
+}
+
+static u16 fm10k_vectors_per_pool(struct fm10k_hw *hw)
+{
+	u16 num_pools = hw->iov.num_pools;
+
+	return (num_pools > 32) ? 8 : (num_pools > 16) ? 16 :
+	       FM10K_MAX_VECTORS_POOL;
+}
+
+static u16 fm10k_vf_vector_index(struct fm10k_hw *hw, u16 vf_idx)
+{
+	u16 vf_v_idx = FM10K_MAX_VECTORS_PF;
+
+	vf_v_idx += fm10k_vectors_per_pool(hw) * vf_idx;
+
+	return vf_v_idx;
+}
+
+/**
+ *  fm10k_iov_assign_resources_pf - Assign pool resources for virtualization
+ *  @hw: pointer to the HW structure
+ *  @num_vfs: number of VFs to be allocated
+ *  @num_pools: number of virtualization pools to be allocated
+ *
+ *  Allocates queues and traffic classes to virtualization entities to prepare
+ *  the PF for SR-IOV and VMDq
+ **/
+static s32 fm10k_iov_assign_resources_pf(struct fm10k_hw *hw, u16 num_vfs,
+					 u16 num_pools)
+{
+	u16 qmap_stride, qpp, vpp, vf_q_idx, vf_q_idx0, qmap_idx;
+	u32 vid = hw->mac.default_vid << FM10K_TXQCTL_VID_SHIFT;
+	int i, j;
+
+	/* hardware only supports up to 64 pools */
+	if (num_pools > 64)
+		return FM10K_ERR_PARAM;
+
+	/* the number of VFs cannot exceed the number of pools */
+	if ((num_vfs > num_pools) || (num_vfs > hw->iov.total_vfs))
+		return FM10K_ERR_PARAM;
+
+	/* record number of virtualization entities */
+	hw->iov.num_vfs = num_vfs;
+	hw->iov.num_pools = num_pools;
+
+	/* determine qmap offsets and counts */
+	qmap_stride = (num_vfs > 8) ? 32 : 256;
+	qpp = fm10k_queues_per_pool(hw);
+	vpp = fm10k_vectors_per_pool(hw);
+
+	/* calculate starting index for queues */
+	vf_q_idx = fm10k_vf_queue_index(hw, 0);
+	qmap_idx = 0;
+
+	/* establish TCs with -1 credits and no quanta to prevent transmit */
+	for (i = 0; i < num_vfs; i++) {
+		fm10k_write_reg(hw, FM10K_TC_MAXCREDIT(i), 0);
+		fm10k_write_reg(hw, FM10K_TC_RATE(i), 0);
+		fm10k_write_reg(hw, FM10K_TC_CREDIT(i),
+				FM10K_TC_CREDIT_CREDIT_MASK);
+	}
+
+	/* zero out all mbmem registers */
+	for (i = FM10K_VFMBMEM_LEN * num_vfs; i--;)
+		fm10k_write_reg(hw, FM10K_MBMEM(i), 0);
+
+	/* clear event notification of VF FLR */
+	fm10k_write_reg(hw, FM10K_PFVFLREC(0), ~0);
+	fm10k_write_reg(hw, FM10K_PFVFLREC(1), ~0);
+
+	/* loop through unallocated rings assigning them back to PF */
+	for (i = FM10K_MAX_QUEUES_PF; i < vf_q_idx; i++) {
+		fm10k_write_reg(hw, FM10K_TXDCTL(i), 0);
+		fm10k_write_reg(hw, FM10K_TXQCTL(i), FM10K_TXQCTL_PF | vid);
+		fm10k_write_reg(hw, FM10K_RXQCTL(i), FM10K_RXQCTL_PF);
+	}
+
+	/* PF should have already updated VFITR2[0] */
+
+	/* update all ITR registers to flow to VFITR2[0] */
+	for (i = FM10K_ITR_REG_COUNT_PF + 1; i < FM10K_ITR_REG_COUNT; i++) {
+		if (!(i & (vpp - 1)))
+			fm10k_write_reg(hw, FM10K_ITR2(i), i - vpp);
+		else
+			fm10k_write_reg(hw, FM10K_ITR2(i), i - 1);
+	}
+
+	/* update PF ITR2[0] to reference the last vector */
+	fm10k_write_reg(hw, FM10K_ITR2(0),
+			fm10k_vf_vector_index(hw, num_vfs - 1));
+
+	/* loop through rings populating rings and TCs */
+	for (i = 0; i < num_vfs; i++) {
+		/* record index for VF queue 0 for use in end of loop */
+		vf_q_idx0 = vf_q_idx;
+
+		for (j = 0; j < qpp; j++, qmap_idx++, vf_q_idx++) {
+			/* assign VF and locked TC to queues */
+			fm10k_write_reg(hw, FM10K_TXDCTL(vf_q_idx), 0);
+			fm10k_write_reg(hw, FM10K_TXQCTL(vf_q_idx),
+					(i << FM10K_TXQCTL_TC_SHIFT) | i |
+					FM10K_TXQCTL_VF | vid);
+			fm10k_write_reg(hw, FM10K_RXDCTL(vf_q_idx),
+					FM10K_RXDCTL_WRITE_BACK_MIN_DELAY |
+					FM10K_RXDCTL_DROP_ON_EMPTY);
+			fm10k_write_reg(hw, FM10K_RXQCTL(vf_q_idx),
+					FM10K_RXQCTL_VF |
+					(i << FM10K_RXQCTL_VF_SHIFT));
+
+			/* map queue pair to VF */
+			fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), vf_q_idx);
+			fm10k_write_reg(hw, FM10K_RQMAP(qmap_idx), vf_q_idx);
+		}
+
+		/* repeat the first ring for all of the remaining VF rings */
+		for (; j < qmap_stride; j++, qmap_idx++) {
+			fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), vf_q_idx0);
+			fm10k_write_reg(hw, FM10K_RQMAP(qmap_idx), vf_q_idx0);
+		}
+	}
+
+	/* loop through remaining indexes assigning all to queue 0 */
+	while (qmap_idx < FM10K_TQMAP_TABLE_SIZE) {
+		fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), 0);
+		fm10k_write_reg(hw, FM10K_RQMAP(qmap_idx), 0);
+		qmap_idx++;
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_iov_configure_tc_pf - Configure the shaping group for VF
+ *  @hw: pointer to the HW structure
+ *  @vf_idx: index of VF receiving GLORT
+ *  @rate: Rate indicated in Mb/s
+ *
+ *  Configured the TC for a given VF to allow only up to a given number
+ *  of Mb/s of outgoing Tx throughput.
+ **/
+static s32 fm10k_iov_configure_tc_pf(struct fm10k_hw *hw, u16 vf_idx, int rate)
+{
+	/* configure defaults */
+	u32 interval = FM10K_TC_RATE_INTERVAL_4US_GEN3;
+	u32 tc_rate = FM10K_TC_RATE_QUANTA_MASK;
+
+	/* verify vf is in range */
+	if (vf_idx >= hw->iov.num_vfs)
+		return FM10K_ERR_PARAM;
+
+	/* set interval to align with 4.096 usec in all modes */
+	switch (hw->bus.speed) {
+	case fm10k_bus_speed_2500:
+		interval = FM10K_TC_RATE_INTERVAL_4US_GEN1;
+		break;
+	case fm10k_bus_speed_5000:
+		interval = FM10K_TC_RATE_INTERVAL_4US_GEN2;
+		break;
+	default:
+		break;
+	}
+
+	if (rate) {
+		if (rate > FM10K_VF_TC_MAX || rate < FM10K_VF_TC_MIN)
+			return FM10K_ERR_PARAM;
+
+		/* The quanta is measured in Bytes per 4.096 or 8.192 usec
+		 * The rate is provided in Mbits per second
+		 * To tralslate from rate to quanta we need to multiply the
+		 * rate by 8.192 usec and divide by 8 bits/byte.  To avoid
+		 * dealing with floating point we can round the values up
+		 * to the nearest whole number ratio which gives us 128 / 125.
+		 */
+		tc_rate = (rate * 128) / 125;
+
+		/* try to keep the rate limiting accurate by increasing
+		 * the number of credits and interval for rates less than 4Gb/s
+		 */
+		if (rate < 4000)
+			interval <<= 1;
+		else
+			tc_rate >>= 1;
+	}
+
+	/* update rate limiter with new values */
+	fm10k_write_reg(hw, FM10K_TC_RATE(vf_idx), tc_rate | interval);
+	fm10k_write_reg(hw, FM10K_TC_MAXCREDIT(vf_idx), FM10K_TC_MAXCREDIT_64K);
+	fm10k_write_reg(hw, FM10K_TC_CREDIT(vf_idx), FM10K_TC_MAXCREDIT_64K);
+
+	return 0;
+}
+
+/**
+ *  fm10k_iov_assign_int_moderator_pf - Add VF interrupts to moderator list
+ *  @hw: pointer to the HW structure
+ *  @vf_idx: index of VF receiving GLORT
+ *
+ *  Update the interrupt moderator linked list to include any MSI-X
+ *  interrupts which the VF has enabled in the MSI-X vector table.
+ **/
+static s32 fm10k_iov_assign_int_moderator_pf(struct fm10k_hw *hw, u16 vf_idx)
+{
+	u16 vf_v_idx, vf_v_limit, i;
+
+	/* verify vf is in range */
+	if (vf_idx >= hw->iov.num_vfs)
+		return FM10K_ERR_PARAM;
+
+	/* determine vector offset and count*/
+	vf_v_idx = fm10k_vf_vector_index(hw, vf_idx);
+	vf_v_limit = vf_v_idx + fm10k_vectors_per_pool(hw);
+
+	/* search for first vector that is not masked */
+	for (i = vf_v_limit - 1; i > vf_v_idx; i--) {
+		if (!fm10k_read_reg(hw, FM10K_MSIX_VECTOR_MASK(i)))
+			break;
+	}
+
+	/* reset linked list so it now includes our active vectors */
+	if (vf_idx == (hw->iov.num_vfs - 1))
+		fm10k_write_reg(hw, FM10K_ITR2(0), i);
+	else
+		fm10k_write_reg(hw, FM10K_ITR2(vf_v_limit), i);
+
+	return 0;
+}
+
+/**
+ *  fm10k_iov_assign_default_mac_vlan_pf - Assign a MAC and VLAN to VF
+ *  @hw: pointer to the HW structure
+ *  @vf_info: pointer to VF information structure
+ *
+ *  Assign a MAC address and default VLAN to a VF and notify it of the update
+ **/
+static s32 fm10k_iov_assign_default_mac_vlan_pf(struct fm10k_hw *hw,
+						struct fm10k_vf_info *vf_info)
+{
+	u16 qmap_stride, queues_per_pool, vf_q_idx, timeout, qmap_idx, i;
+	u32 msg[4], txdctl, txqctl, tdbal = 0, tdbah = 0;
+	s32 err = 0;
+	u16 vf_idx, vf_vid;
+
+	/* verify vf is in range */
+	if (!vf_info || vf_info->vf_idx >= hw->iov.num_vfs)
+		return FM10K_ERR_PARAM;
+
+	/* determine qmap offsets and counts */
+	qmap_stride = (hw->iov.num_vfs > 8) ? 32 : 256;
+	queues_per_pool = fm10k_queues_per_pool(hw);
+
+	/* calculate starting index for queues */
+	vf_idx = vf_info->vf_idx;
+	vf_q_idx = fm10k_vf_queue_index(hw, vf_idx);
+	qmap_idx = qmap_stride * vf_idx;
+
+	/* MAP Tx queue back to 0 temporarily, and disable it */
+	fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), 0);
+	fm10k_write_reg(hw, FM10K_TXDCTL(vf_q_idx), 0);
+
+	/* determine correct default VLAN ID */
+	if (vf_info->pf_vid)
+		vf_vid = vf_info->pf_vid | FM10K_VLAN_CLEAR;
+	else
+		vf_vid = vf_info->sw_vid;
+
+	/* generate MAC_ADDR request */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_MAC_VLAN);
+	fm10k_tlv_attr_put_mac_vlan(msg, FM10K_MAC_VLAN_MSG_DEFAULT_MAC,
+				    vf_info->mac, vf_vid);
+
+	/* load onto outgoing mailbox, ignore any errors on enqueue */
+	if (vf_info->mbx.ops.enqueue_tx)
+		vf_info->mbx.ops.enqueue_tx(hw, &vf_info->mbx, msg);
+
+	/* verify ring has disabled before modifying base address registers */
+	txdctl = fm10k_read_reg(hw, FM10K_TXDCTL(vf_q_idx));
+	for (timeout = 0; txdctl & FM10K_TXDCTL_ENABLE; timeout++) {
+		/* limit ourselves to a 1ms timeout */
+		if (timeout == 10) {
+			err = FM10K_ERR_DMA_PENDING;
+			goto err_out;
+		}
+
+		usleep_range(100, 200);
+		txdctl = fm10k_read_reg(hw, FM10K_TXDCTL(vf_q_idx));
+	}
+
+	/* Update base address registers to contain MAC address */
+	if (is_valid_ether_addr(vf_info->mac)) {
+		tdbal = (((u32)vf_info->mac[3]) << 24) |
+			(((u32)vf_info->mac[4]) << 16) |
+			(((u32)vf_info->mac[5]) << 8);
+
+		tdbah = (((u32)0xFF)	        << 24) |
+			(((u32)vf_info->mac[0]) << 16) |
+			(((u32)vf_info->mac[1]) << 8) |
+			((u32)vf_info->mac[2]);
+	}
+
+	/* Record the base address into queue 0 */
+	fm10k_write_reg(hw, FM10K_TDBAL(vf_q_idx), tdbal);
+	fm10k_write_reg(hw, FM10K_TDBAH(vf_q_idx), tdbah);
+
+err_out:
+	/* configure Queue control register */
+	txqctl = ((u32)vf_vid << FM10K_TXQCTL_VID_SHIFT) &
+		 FM10K_TXQCTL_VID_MASK;
+	txqctl |= (vf_idx << FM10K_TXQCTL_TC_SHIFT) |
+		  FM10K_TXQCTL_VF | vf_idx;
+
+	/* assign VID */
+	for (i = 0; i < queues_per_pool; i++)
+		fm10k_write_reg(hw, FM10K_TXQCTL(vf_q_idx + i), txqctl);
+
+	/* restore the queue back to VF ownership */
+	fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx), vf_q_idx);
+	return err;
+}
+
+/**
+ *  fm10k_iov_reset_resources_pf - Reassign queues and interrupts to a VF
+ *  @hw: pointer to the HW structure
+ *  @vf_info: pointer to VF information structure
+ *
+ *  Reassign the interrupts and queues to a VF following an FLR
+ **/
+static s32 fm10k_iov_reset_resources_pf(struct fm10k_hw *hw,
+					struct fm10k_vf_info *vf_info)
+{
+	u16 qmap_stride, queues_per_pool, vf_q_idx, qmap_idx;
+	u32 tdbal = 0, tdbah = 0, txqctl, rxqctl;
+	u16 vf_v_idx, vf_v_limit, vf_vid;
+	u8 vf_idx = vf_info->vf_idx;
+	int i;
+
+	/* verify vf is in range */
+	if (vf_idx >= hw->iov.num_vfs)
+		return FM10K_ERR_PARAM;
+
+	/* clear event notification of VF FLR */
+	fm10k_write_reg(hw, FM10K_PFVFLREC(vf_idx / 32), 1 << (vf_idx % 32));
+
+	/* force timeout and then disconnect the mailbox */
+	vf_info->mbx.timeout = 0;
+	if (vf_info->mbx.ops.disconnect)
+		vf_info->mbx.ops.disconnect(hw, &vf_info->mbx);
+
+	/* determine vector offset and count*/
+	vf_v_idx = fm10k_vf_vector_index(hw, vf_idx);
+	vf_v_limit = vf_v_idx + fm10k_vectors_per_pool(hw);
+
+	/* determine qmap offsets and counts */
+	qmap_stride = (hw->iov.num_vfs > 8) ? 32 : 256;
+	queues_per_pool = fm10k_queues_per_pool(hw);
+	qmap_idx = qmap_stride * vf_idx;
+
+	/* make all the queues inaccessible to the VF */
+	for (i = qmap_idx; i < (qmap_idx + qmap_stride); i++) {
+		fm10k_write_reg(hw, FM10K_TQMAP(i), 0);
+		fm10k_write_reg(hw, FM10K_RQMAP(i), 0);
+	}
+
+	/* calculate starting index for queues */
+	vf_q_idx = fm10k_vf_queue_index(hw, vf_idx);
+
+	/* determine correct default VLAN ID */
+	if (vf_info->pf_vid)
+		vf_vid = vf_info->pf_vid;
+	else
+		vf_vid = vf_info->sw_vid;
+
+	/* configure Queue control register */
+	txqctl = ((u32)vf_vid << FM10K_TXQCTL_VID_SHIFT) |
+		 (vf_idx << FM10K_TXQCTL_TC_SHIFT) |
+		 FM10K_TXQCTL_VF | vf_idx;
+	rxqctl = FM10K_RXQCTL_VF | (vf_idx << FM10K_RXQCTL_VF_SHIFT);
+
+	/* stop further DMA and reset queue ownership back to VF */
+	for (i = vf_q_idx; i < (queues_per_pool + vf_q_idx); i++) {
+		fm10k_write_reg(hw, FM10K_TXDCTL(i), 0);
+		fm10k_write_reg(hw, FM10K_TXQCTL(i), txqctl);
+		fm10k_write_reg(hw, FM10K_RXDCTL(i),
+				FM10K_RXDCTL_WRITE_BACK_MIN_DELAY |
+				FM10K_RXDCTL_DROP_ON_EMPTY);
+		fm10k_write_reg(hw, FM10K_RXQCTL(i), rxqctl);
+	}
+
+	/* reset TC with -1 credits and no quanta to prevent transmit */
+	fm10k_write_reg(hw, FM10K_TC_MAXCREDIT(vf_idx), 0);
+	fm10k_write_reg(hw, FM10K_TC_RATE(vf_idx), 0);
+	fm10k_write_reg(hw, FM10K_TC_CREDIT(vf_idx),
+			FM10K_TC_CREDIT_CREDIT_MASK);
+
+	/* update our first entry in the table based on previous VF */
+	if (!vf_idx)
+		hw->mac.ops.update_int_moderator(hw);
+	else
+		hw->iov.ops.assign_int_moderator(hw, vf_idx - 1);
+
+	/* reset linked list so it now includes our active vectors */
+	if (vf_idx == (hw->iov.num_vfs - 1))
+		fm10k_write_reg(hw, FM10K_ITR2(0), vf_v_idx);
+	else
+		fm10k_write_reg(hw, FM10K_ITR2(vf_v_limit), vf_v_idx);
+
+	/* link remaining vectors so that next points to previous */
+	for (vf_v_idx++; vf_v_idx < vf_v_limit; vf_v_idx++)
+		fm10k_write_reg(hw, FM10K_ITR2(vf_v_idx), vf_v_idx - 1);
+
+	/* zero out MBMEM, VLAN_TABLE, RETA, RSSRK, and MRQC registers */
+	for (i = FM10K_VFMBMEM_LEN; i--;)
+		fm10k_write_reg(hw, FM10K_MBMEM_VF(vf_idx, i), 0);
+	for (i = FM10K_VLAN_TABLE_SIZE; i--;)
+		fm10k_write_reg(hw, FM10K_VLAN_TABLE(vf_info->vsi, i), 0);
+	for (i = FM10K_RETA_SIZE; i--;)
+		fm10k_write_reg(hw, FM10K_RETA(vf_info->vsi, i), 0);
+	for (i = FM10K_RSSRK_SIZE; i--;)
+		fm10k_write_reg(hw, FM10K_RSSRK(vf_info->vsi, i), 0);
+	fm10k_write_reg(hw, FM10K_MRQC(vf_info->vsi), 0);
+
+	/* Update base address registers to contain MAC address */
+	if (is_valid_ether_addr(vf_info->mac)) {
+		tdbal = (((u32)vf_info->mac[3]) << 24) |
+			(((u32)vf_info->mac[4]) << 16) |
+			(((u32)vf_info->mac[5]) << 8);
+		tdbah = (((u32)0xFF)	   << 24) |
+			(((u32)vf_info->mac[0]) << 16) |
+			(((u32)vf_info->mac[1]) << 8) |
+			((u32)vf_info->mac[2]);
+	}
+
+	/* map queue pairs back to VF from last to first*/
+	for (i = queues_per_pool; i--;) {
+		fm10k_write_reg(hw, FM10K_TDBAL(vf_q_idx + i), tdbal);
+		fm10k_write_reg(hw, FM10K_TDBAH(vf_q_idx + i), tdbah);
+		fm10k_write_reg(hw, FM10K_TQMAP(qmap_idx + i), vf_q_idx + i);
+		fm10k_write_reg(hw, FM10K_RQMAP(qmap_idx + i), vf_q_idx + i);
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_iov_set_lport_pf - Assign and enable a logical port for a given VF
+ *  @hw: pointer to hardware structure
+ *  @vf_info: pointer to VF information structure
+ *  @lport_idx: Logical port offset from the hardware glort
+ *  @flags: Set of capability flags to extend port beyond basic functionality
+ *
+ *  This function allows enabling a VF port by assigning it a GLORT and
+ *  setting the flags so that it can enable an Rx mode.
+ **/
+static s32 fm10k_iov_set_lport_pf(struct fm10k_hw *hw,
+				  struct fm10k_vf_info *vf_info,
+				  u16 lport_idx, u8 flags)
+{
+	u16 glort = (hw->mac.dglort_map + lport_idx) & FM10K_DGLORTMAP_NONE;
+
+	/* if glort is not valid return error */
+	if (!fm10k_glort_valid_pf(hw, glort))
+		return FM10K_ERR_PARAM;
+
+	vf_info->vf_flags = flags | FM10K_VF_FLAG_NONE_CAPABLE;
+	vf_info->glort = glort;
+
+	return 0;
+}
+
+/**
+ *  fm10k_iov_reset_lport_pf - Disable a logical port for a given VF
+ *  @hw: pointer to hardware structure
+ *  @vf_info: pointer to VF information structure
+ *
+ *  This function disables a VF port by stripping it of a GLORT and
+ *  setting the flags so that it cannot enable any Rx mode.
+ **/
+static void fm10k_iov_reset_lport_pf(struct fm10k_hw *hw,
+				     struct fm10k_vf_info *vf_info)
+{
+	u32 msg[1];
+
+	/* need to disable the port if it is already enabled */
+	if (FM10K_VF_FLAG_ENABLED(vf_info)) {
+		/* notify switch that this port has been disabled */
+		fm10k_update_lport_state_pf(hw, vf_info->glort, 1, false);
+
+		/* generate port state response to notify VF it is not ready */
+		fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_LPORT_STATE);
+		vf_info->mbx.ops.enqueue_tx(hw, &vf_info->mbx, msg);
+	}
+
+	/* clear flags and glort if it exists */
+	vf_info->vf_flags = 0;
+	vf_info->glort = 0;
+}
+
+/**
+ *  fm10k_iov_update_stats_pf - Updates hardware related statistics for VFs
+ *  @hw: pointer to hardware structure
+ *  @q: stats for all queues of a VF
+ *  @vf_idx: index of VF
+ *
+ *  This function collects queue stats for VFs.
+ **/
+static void fm10k_iov_update_stats_pf(struct fm10k_hw *hw,
+				      struct fm10k_hw_stats_q *q,
+				      u16 vf_idx)
+{
+	u32 idx, qpp;
+
+	/* get stats for all of the queues */
+	qpp = fm10k_queues_per_pool(hw);
+	idx = fm10k_vf_queue_index(hw, vf_idx);
+	fm10k_update_hw_stats_q(hw, q, idx, qpp);
+}
+
+static s32 fm10k_iov_report_timestamp_pf(struct fm10k_hw *hw,
+					 struct fm10k_vf_info *vf_info,
+					 u64 timestamp)
+{
+	u32 msg[4];
+
+	/* generate port state response to notify VF it is not ready */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_1588);
+	fm10k_tlv_attr_put_u64(msg, FM10K_1588_MSG_TIMESTAMP, timestamp);
+
+	return vf_info->mbx.ops.enqueue_tx(hw, &vf_info->mbx, msg);
+}
+
+/**
+ *  fm10k_iov_msg_msix_pf - Message handler for MSI-X request from VF
+ *  @hw: Pointer to hardware structure
+ *  @results: Pointer array to message, results[0] is pointer to message
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This function is a default handler for MSI-X requests from the VF.  The
+ *  assumption is that in this case it is acceptable to just directly
+ *  hand off the message form the VF to the underlying shared code.
+ **/
+s32 fm10k_iov_msg_msix_pf(struct fm10k_hw *hw, u32 **results,
+			  struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_vf_info *vf_info = (struct fm10k_vf_info *)mbx;
+	u8 vf_idx = vf_info->vf_idx;
+
+	return hw->iov.ops.assign_int_moderator(hw, vf_idx);
+}
+
+/**
+ *  fm10k_iov_msg_mac_vlan_pf - Message handler for MAC/VLAN request from VF
+ *  @hw: Pointer to hardware structure
+ *  @results: Pointer array to message, results[0] is pointer to message
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This function is a default handler for MAC/VLAN requests from the VF.
+ *  The assumption is that in this case it is acceptable to just directly
+ *  hand off the message form the VF to the underlying shared code.
+ **/
+s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *hw, u32 **results,
+			      struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_vf_info *vf_info = (struct fm10k_vf_info *)mbx;
+	int err = 0;
+	u8 mac[ETH_ALEN];
+	u32 *result;
+	u16 vlan;
+	u32 vid;
+
+	/* we shouldn't be updating rules on a disabled interface */
+	if (!FM10K_VF_FLAG_ENABLED(vf_info))
+		err = FM10K_ERR_PARAM;
+
+	if (!err && !!results[FM10K_MAC_VLAN_MSG_VLAN]) {
+		result = results[FM10K_MAC_VLAN_MSG_VLAN];
+
+		/* record VLAN id requested */
+		err = fm10k_tlv_attr_get_u32(result, &vid);
+		if (err)
+			return err;
+
+		/* if VLAN ID is 0, set the default VLAN ID instead of 0 */
+		if (!vid || (vid == FM10K_VLAN_CLEAR)) {
+			if (vf_info->pf_vid)
+				vid |= vf_info->pf_vid;
+			else
+				vid |= vf_info->sw_vid;
+		} else if (vid != vf_info->pf_vid) {
+			return FM10K_ERR_PARAM;
+		}
+
+		/* update VSI info for VF in regards to VLAN table */
+		err = hw->mac.ops.update_vlan(hw, vid, vf_info->vsi,
+					      !(vid & FM10K_VLAN_CLEAR));
+	}
+
+	if (!err && !!results[FM10K_MAC_VLAN_MSG_MAC]) {
+		result = results[FM10K_MAC_VLAN_MSG_MAC];
+
+		/* record unicast MAC address requested */
+		err = fm10k_tlv_attr_get_mac_vlan(result, mac, &vlan);
+		if (err)
+			return err;
+
+		/* block attempts to set MAC for a locked device */
+		if (is_valid_ether_addr(vf_info->mac) &&
+		    memcmp(mac, vf_info->mac, ETH_ALEN))
+			return FM10K_ERR_PARAM;
+
+		/* if VLAN ID is 0, set the default VLAN ID instead of 0 */
+		if (!vlan || (vlan == FM10K_VLAN_CLEAR)) {
+			if (vf_info->pf_vid)
+				vlan |= vf_info->pf_vid;
+			else
+				vlan |= vf_info->sw_vid;
+		} else if (vf_info->pf_vid) {
+			return FM10K_ERR_PARAM;
+		}
+
+		/* notify switch of request for new unicast address */
+		err = hw->mac.ops.update_uc_addr(hw, vf_info->glort, mac, vlan,
+						 !(vlan & FM10K_VLAN_CLEAR), 0);
+	}
+
+	if (!err && !!results[FM10K_MAC_VLAN_MSG_MULTICAST]) {
+		result = results[FM10K_MAC_VLAN_MSG_MULTICAST];
+
+		/* record multicast MAC address requested */
+		err = fm10k_tlv_attr_get_mac_vlan(result, mac, &vlan);
+		if (err)
+			return err;
+
+		/* verify that the VF is allowed to request multicast */
+		if (!(vf_info->vf_flags & FM10K_VF_FLAG_MULTI_ENABLED))
+			return FM10K_ERR_PARAM;
+
+		/* if VLAN ID is 0, set the default VLAN ID instead of 0 */
+		if (!vlan || (vlan == FM10K_VLAN_CLEAR)) {
+			if (vf_info->pf_vid)
+				vlan |= vf_info->pf_vid;
+			else
+				vlan |= vf_info->sw_vid;
+		} else if (vf_info->pf_vid) {
+			return FM10K_ERR_PARAM;
+		}
+
+		/* notify switch of request for new multicast address */
+		err = hw->mac.ops.update_mc_addr(hw, vf_info->glort, mac,
+						 !(vlan & FM10K_VLAN_CLEAR), 0);
+	}
+
+	return err;
+}
+
+/**
+ *  fm10k_iov_supported_xcast_mode_pf - Determine best match for xcast mode
+ *  @vf_info: VF info structure containing capability flags
+ *  @mode: Requested xcast mode
+ *
+ *  This function outputs the mode that most closely matches the requested
+ *  mode.  If not modes match it will request we disable the port
+ **/
+static u8 fm10k_iov_supported_xcast_mode_pf(struct fm10k_vf_info *vf_info,
+					    u8 mode)
+{
+	u8 vf_flags = vf_info->vf_flags;
+
+	/* match up mode to capabilities as best as possible */
+	switch (mode) {
+	case FM10K_XCAST_MODE_PROMISC:
+		if (vf_flags & FM10K_VF_FLAG_PROMISC_CAPABLE)
+			return FM10K_XCAST_MODE_PROMISC;
+		/* fallthough */
+	case FM10K_XCAST_MODE_ALLMULTI:
+		if (vf_flags & FM10K_VF_FLAG_ALLMULTI_CAPABLE)
+			return FM10K_XCAST_MODE_ALLMULTI;
+		/* fallthough */
+	case FM10K_XCAST_MODE_MULTI:
+		if (vf_flags & FM10K_VF_FLAG_MULTI_CAPABLE)
+			return FM10K_XCAST_MODE_MULTI;
+		/* fallthough */
+	case FM10K_XCAST_MODE_NONE:
+		if (vf_flags & FM10K_VF_FLAG_NONE_CAPABLE)
+			return FM10K_XCAST_MODE_NONE;
+		/* fallthough */
+	default:
+		break;
+	}
+
+	/* disable interface as it should not be able to request any */
+	return FM10K_XCAST_MODE_DISABLE;
+}
+
+/**
+ *  fm10k_iov_msg_lport_state_pf - Message handler for port state requests
+ *  @hw: Pointer to hardware structure
+ *  @results: Pointer array to message, results[0] is pointer to message
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This function is a default handler for port state requests.  The port
+ *  state requests for now are basic and consist of enabling or disabling
+ *  the port.
+ **/
+s32 fm10k_iov_msg_lport_state_pf(struct fm10k_hw *hw, u32 **results,
+				 struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_vf_info *vf_info = (struct fm10k_vf_info *)mbx;
+	u32 *result;
+	s32 err = 0;
+	u32 msg[2];
+	u8 mode = 0;
+
+	/* verify VF is allowed to enable even minimal mode */
+	if (!(vf_info->vf_flags & FM10K_VF_FLAG_NONE_CAPABLE))
+		return FM10K_ERR_PARAM;
+
+	if (!!results[FM10K_LPORT_STATE_MSG_XCAST_MODE]) {
+		result = results[FM10K_LPORT_STATE_MSG_XCAST_MODE];
+
+		/* XCAST mode update requested */
+		err = fm10k_tlv_attr_get_u8(result, &mode);
+		if (err)
+			return FM10K_ERR_PARAM;
+
+		/* prep for possible demotion depending on capabilities */
+		mode = fm10k_iov_supported_xcast_mode_pf(vf_info, mode);
+
+		/* if mode is not currently enabled, enable it */
+		if (!(FM10K_VF_FLAG_ENABLED(vf_info) & (1 << mode)))
+			fm10k_update_xcast_mode_pf(hw, vf_info->glort, mode);
+
+		/* swap mode back to a bit flag */
+		mode = FM10K_VF_FLAG_SET_MODE(mode);
+	} else if (!results[FM10K_LPORT_STATE_MSG_DISABLE]) {
+		/* need to disable the port if it is already enabled */
+		if (FM10K_VF_FLAG_ENABLED(vf_info))
+			err = fm10k_update_lport_state_pf(hw, vf_info->glort,
+							  1, false);
+
+		/* when enabling the port we should reset the rate limiters */
+		hw->iov.ops.configure_tc(hw, vf_info->vf_idx, vf_info->rate);
+
+		/* set mode for minimal functionality */
+		mode = FM10K_VF_FLAG_SET_MODE_NONE;
+
+		/* generate port state response to notify VF it is ready */
+		fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_LPORT_STATE);
+		fm10k_tlv_attr_put_bool(msg, FM10K_LPORT_STATE_MSG_READY);
+		mbx->ops.enqueue_tx(hw, mbx, msg);
+	}
+
+	/* if enable state toggled note the update */
+	if (!err && (!FM10K_VF_FLAG_ENABLED(vf_info) != !mode))
+		err = fm10k_update_lport_state_pf(hw, vf_info->glort, 1,
+						  !!mode);
+
+	/* if state change succeeded, then update our stored state */
+	mode |= FM10K_VF_FLAG_CAPABLE(vf_info);
+	if (!err)
+		vf_info->vf_flags = mode;
+
+	return err;
+}
+
+const struct fm10k_msg_data fm10k_iov_msg_data_pf[] = {
+	FM10K_TLV_MSG_TEST_HANDLER(fm10k_tlv_msg_test),
+	FM10K_VF_MSG_MSIX_HANDLER(fm10k_iov_msg_msix_pf),
+	FM10K_VF_MSG_MAC_VLAN_HANDLER(fm10k_iov_msg_mac_vlan_pf),
+	FM10K_VF_MSG_LPORT_STATE_HANDLER(fm10k_iov_msg_lport_state_pf),
+	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_tlv_msg_error),
+};
+
+/**
+ *  fm10k_update_stats_hw_pf - Updates hardware related statistics of PF
+ *  @hw: pointer to hardware structure
+ *  @stats: pointer to the stats structure to update
+ *
+ *  This function collects and aggregates global and per queue hardware
+ *  statistics.
+ **/
+static void fm10k_update_hw_stats_pf(struct fm10k_hw *hw,
+				     struct fm10k_hw_stats *stats)
+{
+	u32 timeout, ur, ca, um, xec, vlan_drop, loopback_drop, nodesc_drop;
+	u32 id, id_prev;
+
+	/* Use Tx queue 0 as a canary to detect a reset */
+	id = fm10k_read_reg(hw, FM10K_TXQCTL(0));
+
+	/* Read Global Statistics */
+	do {
+		timeout = fm10k_read_hw_stats_32b(hw, FM10K_STATS_TIMEOUT,
+						  &stats->timeout);
+		ur = fm10k_read_hw_stats_32b(hw, FM10K_STATS_UR, &stats->ur);
+		ca = fm10k_read_hw_stats_32b(hw, FM10K_STATS_CA, &stats->ca);
+		um = fm10k_read_hw_stats_32b(hw, FM10K_STATS_UM, &stats->um);
+		xec = fm10k_read_hw_stats_32b(hw, FM10K_STATS_XEC, &stats->xec);
+		vlan_drop = fm10k_read_hw_stats_32b(hw, FM10K_STATS_VLAN_DROP,
+						    &stats->vlan_drop);
+		loopback_drop = fm10k_read_hw_stats_32b(hw,
+							FM10K_STATS_LOOPBACK_DROP,
+						     &stats->loopback_drop);
+		nodesc_drop = fm10k_read_hw_stats_32b(hw,
+						      FM10K_STATS_NODESC_DROP,
+						      &stats->nodesc_drop);
+
+		/* if value has not changed then we have consistent data */
+		id_prev = id;
+		id = fm10k_read_reg(hw, FM10K_TXQCTL(0));
+	} while ((id ^ id_prev) & FM10K_TXQCTL_ID_MASK);
+
+	/* drop non-ID bits and set VALID ID bit */
+	id &= FM10K_TXQCTL_ID_MASK;
+	id |= FM10K_STAT_VALID;
+
+	/* Update Global Statistics */
+	if (stats->stats_idx == id) {
+		stats->timeout.count += timeout;
+		stats->ur.count += ur;
+		stats->ca.count += ca;
+		stats->um.count += um;
+		stats->xec.count += xec;
+		stats->vlan_drop.count += vlan_drop;
+		stats->loopback_drop.count += loopback_drop;
+		stats->nodesc_drop.count += nodesc_drop;
+	}
+
+	/* Update bases and record current PF id */
+	fm10k_update_hw_base_32b(&stats->timeout, timeout);
+	fm10k_update_hw_base_32b(&stats->ur, ur);
+	fm10k_update_hw_base_32b(&stats->ca, ca);
+	fm10k_update_hw_base_32b(&stats->um, um);
+	fm10k_update_hw_base_32b(&stats->xec, xec);
+	fm10k_update_hw_base_32b(&stats->vlan_drop, vlan_drop);
+	fm10k_update_hw_base_32b(&stats->loopback_drop, loopback_drop);
+	fm10k_update_hw_base_32b(&stats->nodesc_drop, nodesc_drop);
+	stats->stats_idx = id;
+
+	/* Update Queue Statistics */
+	fm10k_update_hw_stats_q(hw, stats->q, 0, hw->mac.max_queues);
+}
+
+/**
+ *  fm10k_rebind_hw_stats_pf - Resets base for hardware statistics of PF
+ *  @hw: pointer to hardware structure
+ *  @stats: pointer to the stats structure to update
+ *
+ *  This function resets the base for global and per queue hardware
+ *  statistics.
+ **/
+static void fm10k_rebind_hw_stats_pf(struct fm10k_hw *hw,
+				     struct fm10k_hw_stats *stats)
+{
+	/* Unbind Global Statistics */
+	fm10k_unbind_hw_stats_32b(&stats->timeout);
+	fm10k_unbind_hw_stats_32b(&stats->ur);
+	fm10k_unbind_hw_stats_32b(&stats->ca);
+	fm10k_unbind_hw_stats_32b(&stats->um);
+	fm10k_unbind_hw_stats_32b(&stats->xec);
+	fm10k_unbind_hw_stats_32b(&stats->vlan_drop);
+	fm10k_unbind_hw_stats_32b(&stats->loopback_drop);
+	fm10k_unbind_hw_stats_32b(&stats->nodesc_drop);
+
+	/* Unbind Queue Statistics */
+	fm10k_unbind_hw_stats_q(stats->q, 0, hw->mac.max_queues);
+
+	/* Reinitialize bases for all stats */
+	fm10k_update_hw_stats_pf(hw, stats);
+}
+
+/**
+ *  fm10k_set_dma_mask_pf - Configures PhyAddrSpace to limit DMA to system
+ *  @hw: pointer to hardware structure
+ *  @dma_mask: 64 bit DMA mask required for platform
+ *
+ *  This function sets the PHYADDR.PhyAddrSpace bits for the endpoint in order
+ *  to limit the access to memory beyond what is physically in the system.
+ **/
+static void fm10k_set_dma_mask_pf(struct fm10k_hw *hw, u64 dma_mask)
+{
+	/* we need to write the upper 32 bits of DMA mask to PhyAddrSpace */
+	u32 phyaddr = (u32)(dma_mask >> 32);
+
+	fm10k_write_reg(hw, FM10K_PHYADDR, phyaddr);
+}
+
+/**
+ *  fm10k_get_fault_pf - Record a fault in one of the interface units
+ *  @hw: pointer to hardware structure
+ *  @type: pointer to fault type register offset
+ *  @fault: pointer to memory location to record the fault
+ *
+ *  Record the fault register contents to the fault data structure and
+ *  clear the entry from the register.
+ *
+ *  Returns ERR_PARAM if invalid register is specified or no error is present.
+ **/
+static s32 fm10k_get_fault_pf(struct fm10k_hw *hw, int type,
+			      struct fm10k_fault *fault)
+{
+	u32 func;
+
+	/* verify the fault register is in range and is aligned */
+	switch (type) {
+	case FM10K_PCA_FAULT:
+	case FM10K_THI_FAULT:
+	case FM10K_FUM_FAULT:
+		break;
+	default:
+		return FM10K_ERR_PARAM;
+	}
+
+	/* only service faults that are valid */
+	func = fm10k_read_reg(hw, type + FM10K_FAULT_FUNC);
+	if (!(func & FM10K_FAULT_FUNC_VALID))
+		return FM10K_ERR_PARAM;
+
+	/* read remaining fields */
+	fault->address = fm10k_read_reg(hw, type + FM10K_FAULT_ADDR_HI);
+	fault->address <<= 32;
+	fault->address = fm10k_read_reg(hw, type + FM10K_FAULT_ADDR_LO);
+	fault->specinfo = fm10k_read_reg(hw, type + FM10K_FAULT_SPECINFO);
+
+	/* clear valid bit to allow for next error */
+	fm10k_write_reg(hw, type + FM10K_FAULT_FUNC, FM10K_FAULT_FUNC_VALID);
+
+	/* Record which function triggered the error */
+	if (func & FM10K_FAULT_FUNC_PF)
+		fault->func = 0;
+	else
+		fault->func = 1 + ((func & FM10K_FAULT_FUNC_VF_MASK) >>
+				   FM10K_FAULT_FUNC_VF_SHIFT);
+
+	/* record fault type */
+	fault->type = func & FM10K_FAULT_FUNC_TYPE_MASK;
+
+	return 0;
+}
+
+/**
+ *  fm10k_request_lport_map_pf - Request LPORT map from the switch API
+ *  @hw: pointer to hardware structure
+ *
+ **/
+static s32 fm10k_request_lport_map_pf(struct fm10k_hw *hw)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[1];
+
+	/* issue request asking for LPORT map */
+	fm10k_tlv_msg_init(msg, FM10K_PF_MSG_ID_LPORT_MAP);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_get_host_state_pf - Returns the state of the switch and mailbox
+ *  @hw: pointer to hardware structure
+ *  @switch_ready: pointer to boolean value that will record switch state
+ *
+ *  This funciton will check the DMA_CTRL2 register and mailbox in order
+ *  to determine if the switch is ready for the PF to begin requesting
+ *  addresses and mapping traffic to the local interface.
+ **/
+static s32 fm10k_get_host_state_pf(struct fm10k_hw *hw, bool *switch_ready)
+{
+	s32 ret_val = 0;
+	u32 dma_ctrl2;
+
+	/* verify the switch is ready for interraction */
+	dma_ctrl2 = fm10k_read_reg(hw, FM10K_DMA_CTRL2);
+	if (!(dma_ctrl2 & FM10K_DMA_CTRL2_SWITCH_READY))
+		goto out;
+
+	/* retrieve generic host state info */
+	ret_val = fm10k_get_host_state_generic(hw, switch_ready);
+	if (ret_val)
+		goto out;
+
+	/* interface cannot receive traffic without logical ports */
+	if (hw->mac.dglort_map == FM10K_DGLORTMAP_NONE)
+		ret_val = fm10k_request_lport_map_pf(hw);
+
+out:
+	return ret_val;
+}
+
+/* This structure defines the attibutes to be parsed below */
+const struct fm10k_tlv_attr fm10k_lport_map_msg_attr[] = {
+	FM10K_TLV_ATTR_U32(FM10K_PF_ATTR_ID_LPORT_MAP),
+	FM10K_TLV_ATTR_LAST
+};
+
+/**
+ *  fm10k_msg_lport_map_pf - Message handler for lport_map message from SM
+ *  @hw: Pointer to hardware structure
+ *  @results: pointer array containing parsed data
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This handler configures the lport mapping based on the reply from the
+ *  switch API.
+ **/
+s32 fm10k_msg_lport_map_pf(struct fm10k_hw *hw, u32 **results,
+			   struct fm10k_mbx_info *mbx)
+{
+	u16 glort, mask;
+	u32 dglort_map;
+	s32 err;
+
+	err = fm10k_tlv_attr_get_u32(results[FM10K_PF_ATTR_ID_LPORT_MAP],
+				     &dglort_map);
+	if (err)
+		return err;
+
+	/* extract values out of the header */
+	glort = FM10K_MSG_HDR_FIELD_GET(dglort_map, LPORT_MAP_GLORT);
+	mask = FM10K_MSG_HDR_FIELD_GET(dglort_map, LPORT_MAP_MASK);
+
+	/* verify mask is set and none of the masked bits in glort are set */
+	if (!mask || (glort & ~mask))
+		return FM10K_ERR_PARAM;
+
+	/* verify the mask is contiguous, and that it is 1's followed by 0's */
+	if (((~(mask - 1) & mask) + mask) & FM10K_DGLORTMAP_NONE)
+		return FM10K_ERR_PARAM;
+
+	/* record the glort, mask, and port count */
+	hw->mac.dglort_map = dglort_map;
+
+	return 0;
+}
+
+const struct fm10k_tlv_attr fm10k_update_pvid_msg_attr[] = {
+	FM10K_TLV_ATTR_U32(FM10K_PF_ATTR_ID_UPDATE_PVID),
+	FM10K_TLV_ATTR_LAST
+};
+
+/**
+ *  fm10k_msg_update_pvid_pf - Message handler for port VLAN message from SM
+ *  @hw: Pointer to hardware structure
+ *  @results: pointer array containing parsed data
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This handler configures the default VLAN for the PF
+ **/
+s32 fm10k_msg_update_pvid_pf(struct fm10k_hw *hw, u32 **results,
+			     struct fm10k_mbx_info *mbx)
+{
+	u16 glort, pvid;
+	u32 pvid_update;
+	s32 err;
+
+	err = fm10k_tlv_attr_get_u32(results[FM10K_PF_ATTR_ID_UPDATE_PVID],
+				     &pvid_update);
+	if (err)
+		return err;
+
+	/* extract values from the pvid update */
+	glort = FM10K_MSG_HDR_FIELD_GET(pvid_update, UPDATE_PVID_GLORT);
+	pvid = FM10K_MSG_HDR_FIELD_GET(pvid_update, UPDATE_PVID_PVID);
+
+	/* if glort is not valid return error */
+	if (!fm10k_glort_valid_pf(hw, glort))
+		return FM10K_ERR_PARAM;
+
+	/* verify VID is valid */
+	if (pvid >= FM10K_VLAN_TABLE_VID_MAX)
+		return FM10K_ERR_PARAM;
+
+	/* record the port VLAN ID value */
+	hw->mac.default_vid = pvid;
+
+	return 0;
+}
+
+/**
+ *  fm10k_record_global_table_data - Move global table data to swapi table info
+ *  @from: pointer to source table data structure
+ *  @to: pointer to destination table info structure
+ *
+ *  This function is will copy table_data to the table_info contained in
+ *  the hw struct.
+ **/
+static void fm10k_record_global_table_data(struct fm10k_global_table_data *from,
+					   struct fm10k_swapi_table_info *to)
+{
+	/* convert from le32 struct to CPU byte ordered values */
+	to->used = le32_to_cpu(from->used);
+	to->avail = le32_to_cpu(from->avail);
+}
+
+const struct fm10k_tlv_attr fm10k_err_msg_attr[] = {
+	FM10K_TLV_ATTR_LE_STRUCT(FM10K_PF_ATTR_ID_ERR,
+				 sizeof(struct fm10k_swapi_error)),
+	FM10K_TLV_ATTR_LAST
+};
+
+/**
+ *  fm10k_msg_err_pf - Message handler for error reply
+ *  @hw: Pointer to hardware structure
+ *  @results: pointer array containing parsed data
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This handler will capture the data for any error replies to previous
+ *  messages that the PF has sent.
+ **/
+s32 fm10k_msg_err_pf(struct fm10k_hw *hw, u32 **results,
+		     struct fm10k_mbx_info *mbx)
+{
+	struct fm10k_swapi_error err_msg;
+	s32 err;
+
+	/* extract structure from message */
+	err = fm10k_tlv_attr_get_le_struct(results[FM10K_PF_ATTR_ID_ERR],
+					   &err_msg, sizeof(err_msg));
+	if (err)
+		return err;
+
+	/* record table status */
+	fm10k_record_global_table_data(&err_msg.mac, &hw->swapi.mac);
+	fm10k_record_global_table_data(&err_msg.nexthop, &hw->swapi.nexthop);
+	fm10k_record_global_table_data(&err_msg.ffu, &hw->swapi.ffu);
+
+	/* record SW API status value */
+	hw->swapi.status = le32_to_cpu(err_msg.status);
+
+	return 0;
+}
+
+const struct fm10k_tlv_attr fm10k_1588_timestamp_msg_attr[] = {
+	FM10K_TLV_ATTR_LE_STRUCT(FM10K_PF_ATTR_ID_1588_TIMESTAMP,
+				 sizeof(struct fm10k_swapi_1588_timestamp)),
+	FM10K_TLV_ATTR_LAST
+};
+
+/* currently there is no shared 1588 timestamp handler */
+
+/**
+ *  fm10k_adjust_systime_pf - Adjust systime frequency
+ *  @hw: pointer to hardware structure
+ *  @ppb: adjustment rate in parts per billion
+ *
+ *  This function will adjust the SYSTIME_CFG register contained in BAR 4
+ *  if this function is supported for BAR 4 access.  The adjustment amount
+ *  is based on the parts per billion value provided and adjusted to a
+ *  value based on parts per 2^48 clock cycles.
+ *
+ *  If adjustment is not supported or the requested value is too large
+ *  we will return an error.
+ **/
+static s32 fm10k_adjust_systime_pf(struct fm10k_hw *hw, s32 ppb)
+{
+	u64 systime_adjust;
+
+	/* if sw_addr is not set we don't have switch register access */
+	if (!hw->sw_addr)
+		return ppb ? FM10K_ERR_PARAM : 0;
+
+	/* we must convert the value from parts per billion to parts per
+	 * 2^48 cycles.  In addition I have opted to only use the 30 most
+	 * significant bits of the adjustment value as the 8 least
+	 * significant bits are located in another register and represent
+	 * a value significantly less than a part per billion, the result
+	 * of dropping the 8 least significant bits is that the adjustment
+	 * value is effectively multiplied by 2^8 when we write it.
+	 *
+	 * As a result of all this the math for this breaks down as follows:
+	 *	ppb / 10^9 == adjust * 2^8 / 2^48
+	 * If we solve this for adjust, and simplify it comes out as:
+	 *	ppb * 2^31 / 5^9 == adjust
+	 */
+	systime_adjust = (ppb < 0) ? -ppb : ppb;
+	systime_adjust <<= 31;
+	do_div(systime_adjust, 1953125);
+
+	/* verify the requested adjustment value is in range */
+	if (systime_adjust > FM10K_SW_SYSTIME_ADJUST_MASK)
+		return FM10K_ERR_PARAM;
+
+	if (ppb < 0)
+		systime_adjust |= FM10K_SW_SYSTIME_ADJUST_DIR_NEGATIVE;
+
+	fm10k_write_sw_reg(hw, FM10K_SW_SYSTIME_ADJUST, (u32)systime_adjust);
+
+	return 0;
+}
+
+/**
+ *  fm10k_read_systime_pf - Reads value of systime registers
+ *  @hw: pointer to the hardware structure
+ *
+ *  Function reads the content of 2 registers, combined to represent a 64 bit
+ *  value measured in nanosecods.  In order to guarantee the value is accurate
+ *  we check the 32 most significant bits both before and after reading the
+ *  32 least significant bits to verify they didn't change as we were reading
+ *  the registers.
+ **/
+static u64 fm10k_read_systime_pf(struct fm10k_hw *hw)
+{
+	u32 systime_l, systime_h, systime_tmp;
+
+	systime_h = fm10k_read_reg(hw, FM10K_SYSTIME + 1);
+
+	do {
+		systime_tmp = systime_h;
+		systime_l = fm10k_read_reg(hw, FM10K_SYSTIME);
+		systime_h = fm10k_read_reg(hw, FM10K_SYSTIME + 1);
+	} while (systime_tmp != systime_h);
+
+	return ((u64)systime_h << 32) | systime_l;
+}
+
+static const struct fm10k_msg_data fm10k_msg_data_pf[] = {
+	FM10K_PF_MSG_ERR_HANDLER(XCAST_MODES, fm10k_msg_err_pf),
+	FM10K_PF_MSG_ERR_HANDLER(UPDATE_MAC_FWD_RULE, fm10k_msg_err_pf),
+	FM10K_PF_MSG_LPORT_MAP_HANDLER(fm10k_msg_lport_map_pf),
+	FM10K_PF_MSG_ERR_HANDLER(LPORT_CREATE, fm10k_msg_err_pf),
+	FM10K_PF_MSG_ERR_HANDLER(LPORT_DELETE, fm10k_msg_err_pf),
+	FM10K_PF_MSG_UPDATE_PVID_HANDLER(fm10k_msg_update_pvid_pf),
+	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_tlv_msg_error),
+};
+
+static struct fm10k_mac_ops mac_ops_pf = {
+	.get_bus_info		= &fm10k_get_bus_info_generic,
+	.reset_hw		= &fm10k_reset_hw_pf,
+	.init_hw		= &fm10k_init_hw_pf,
+	.start_hw		= &fm10k_start_hw_generic,
+	.stop_hw		= &fm10k_stop_hw_generic,
+	.is_slot_appropriate	= &fm10k_is_slot_appropriate_pf,
+	.update_vlan		= &fm10k_update_vlan_pf,
+	.read_mac_addr		= &fm10k_read_mac_addr_pf,
+	.update_uc_addr		= &fm10k_update_uc_addr_pf,
+	.update_mc_addr		= &fm10k_update_mc_addr_pf,
+	.update_xcast_mode	= &fm10k_update_xcast_mode_pf,
+	.update_int_moderator	= &fm10k_update_int_moderator_pf,
+	.update_lport_state	= &fm10k_update_lport_state_pf,
+	.update_hw_stats	= &fm10k_update_hw_stats_pf,
+	.rebind_hw_stats	= &fm10k_rebind_hw_stats_pf,
+	.configure_dglort_map	= &fm10k_configure_dglort_map_pf,
+	.set_dma_mask		= &fm10k_set_dma_mask_pf,
+	.get_fault		= &fm10k_get_fault_pf,
+	.get_host_state		= &fm10k_get_host_state_pf,
+	.adjust_systime		= &fm10k_adjust_systime_pf,
+	.read_systime		= &fm10k_read_systime_pf,
+};
+
+static struct fm10k_iov_ops iov_ops_pf = {
+	.assign_resources		= &fm10k_iov_assign_resources_pf,
+	.configure_tc			= &fm10k_iov_configure_tc_pf,
+	.assign_int_moderator		= &fm10k_iov_assign_int_moderator_pf,
+	.assign_default_mac_vlan	= fm10k_iov_assign_default_mac_vlan_pf,
+	.reset_resources		= &fm10k_iov_reset_resources_pf,
+	.set_lport			= &fm10k_iov_set_lport_pf,
+	.reset_lport			= &fm10k_iov_reset_lport_pf,
+	.update_stats			= &fm10k_iov_update_stats_pf,
+	.report_timestamp		= &fm10k_iov_report_timestamp_pf,
+};
+
+static s32 fm10k_get_invariants_pf(struct fm10k_hw *hw)
+{
+	fm10k_get_invariants_generic(hw);
+
+	return fm10k_sm_mbx_init(hw, &hw->mbx, fm10k_msg_data_pf);
+}
+
+struct fm10k_info fm10k_pf_info = {
+	.mac		= fm10k_mac_pf,
+	.get_invariants	= &fm10k_get_invariants_pf,
+	.mac_ops	= &mac_ops_pf,
+	.iov_ops	= &iov_ops_pf,
+};
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_pf.h b/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
new file mode 100644
index 000000000000..7ab1db4fff32
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_pf.h
@@ -0,0 +1,135 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifndef _FM10K_PF_H_
+#define _FM10K_PF_H_
+
+#include "fm10k_type.h"
+#include "fm10k_common.h"
+
+bool fm10k_glort_valid_pf(struct fm10k_hw *hw, u16 glort);
+u16 fm10k_queues_per_pool(struct fm10k_hw *hw);
+u16 fm10k_vf_queue_index(struct fm10k_hw *hw, u16 vf_idx);
+
+enum fm10k_pf_tlv_msg_id_v1 {
+	FM10K_PF_MSG_ID_TEST			= 0x000, /* msg ID reserved */
+	FM10K_PF_MSG_ID_XCAST_MODES		= 0x001,
+	FM10K_PF_MSG_ID_UPDATE_MAC_FWD_RULE	= 0x002,
+	FM10K_PF_MSG_ID_LPORT_MAP		= 0x100,
+	FM10K_PF_MSG_ID_LPORT_CREATE		= 0x200,
+	FM10K_PF_MSG_ID_LPORT_DELETE		= 0x201,
+	FM10K_PF_MSG_ID_CONFIG			= 0x300,
+	FM10K_PF_MSG_ID_UPDATE_PVID		= 0x400,
+	FM10K_PF_MSG_ID_CREATE_FLOW_TABLE	= 0x501,
+	FM10K_PF_MSG_ID_DELETE_FLOW_TABLE	= 0x502,
+	FM10K_PF_MSG_ID_UPDATE_FLOW		= 0x503,
+	FM10K_PF_MSG_ID_DELETE_FLOW		= 0x504,
+	FM10K_PF_MSG_ID_SET_FLOW_STATE		= 0x505,
+	FM10K_PF_MSG_ID_GET_1588_INFO		= 0x506,
+	FM10K_PF_MSG_ID_1588_TIMESTAMP		= 0x701,
+};
+
+enum fm10k_pf_tlv_attr_id_v1 {
+	FM10K_PF_ATTR_ID_ERR			= 0x00,
+	FM10K_PF_ATTR_ID_LPORT_MAP		= 0x01,
+	FM10K_PF_ATTR_ID_XCAST_MODE		= 0x02,
+	FM10K_PF_ATTR_ID_MAC_UPDATE		= 0x03,
+	FM10K_PF_ATTR_ID_VLAN_UPDATE		= 0x04,
+	FM10K_PF_ATTR_ID_CONFIG			= 0x05,
+	FM10K_PF_ATTR_ID_CREATE_FLOW_TABLE	= 0x06,
+	FM10K_PF_ATTR_ID_DELETE_FLOW_TABLE	= 0x07,
+	FM10K_PF_ATTR_ID_UPDATE_FLOW		= 0x08,
+	FM10K_PF_ATTR_ID_FLOW_STATE		= 0x09,
+	FM10K_PF_ATTR_ID_FLOW_HANDLE		= 0x0A,
+	FM10K_PF_ATTR_ID_DELETE_FLOW		= 0x0B,
+	FM10K_PF_ATTR_ID_PORT			= 0x0C,
+	FM10K_PF_ATTR_ID_UPDATE_PVID		= 0x0D,
+	FM10K_PF_ATTR_ID_1588_TIMESTAMP		= 0x10,
+};
+
+#define FM10K_MSG_LPORT_MAP_GLORT_SHIFT	0
+#define FM10K_MSG_LPORT_MAP_GLORT_SIZE	16
+#define FM10K_MSG_LPORT_MAP_MASK_SHIFT	16
+#define FM10K_MSG_LPORT_MAP_MASK_SIZE	16
+
+#define FM10K_MSG_UPDATE_PVID_GLORT_SHIFT	0
+#define FM10K_MSG_UPDATE_PVID_GLORT_SIZE	16
+#define FM10K_MSG_UPDATE_PVID_PVID_SHIFT	16
+#define FM10K_MSG_UPDATE_PVID_PVID_SIZE		16
+
+struct fm10k_mac_update {
+	__le32	mac_lower;
+	__le16	mac_upper;
+	__le16	vlan;
+	__le16	glort;
+	u8	flags;
+	u8	action;
+};
+
+struct fm10k_global_table_data {
+	__le32	used;
+	__le32	avail;
+};
+
+struct fm10k_swapi_error {
+	__le32				status;
+	struct fm10k_global_table_data	mac;
+	struct fm10k_global_table_data	nexthop;
+	struct fm10k_global_table_data	ffu;
+};
+
+struct fm10k_swapi_1588_timestamp {
+	__le64 egress;
+	__le64 ingress;
+	__le16 dglort;
+	__le16 sglort;
+};
+
+s32 fm10k_msg_lport_map_pf(struct fm10k_hw *, u32 **, struct fm10k_mbx_info *);
+extern const struct fm10k_tlv_attr fm10k_lport_map_msg_attr[];
+#define FM10K_PF_MSG_LPORT_MAP_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_PF_MSG_ID_LPORT_MAP, \
+			  fm10k_lport_map_msg_attr, func)
+s32 fm10k_msg_update_pvid_pf(struct fm10k_hw *, u32 **,
+			     struct fm10k_mbx_info *);
+extern const struct fm10k_tlv_attr fm10k_update_pvid_msg_attr[];
+#define FM10K_PF_MSG_UPDATE_PVID_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_PF_MSG_ID_UPDATE_PVID, \
+			  fm10k_update_pvid_msg_attr, func)
+
+s32 fm10k_msg_err_pf(struct fm10k_hw *, u32 **, struct fm10k_mbx_info *);
+extern const struct fm10k_tlv_attr fm10k_err_msg_attr[];
+#define FM10K_PF_MSG_ERR_HANDLER(msg, func) \
+	FM10K_MSG_HANDLER(FM10K_PF_MSG_ID_##msg, fm10k_err_msg_attr, func)
+
+extern const struct fm10k_tlv_attr fm10k_1588_timestamp_msg_attr[];
+#define FM10K_PF_MSG_1588_TIMESTAMP_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_PF_MSG_ID_1588_TIMESTAMP, \
+			  fm10k_1588_timestamp_msg_attr, func)
+
+s32 fm10k_iov_msg_msix_pf(struct fm10k_hw *, u32 **, struct fm10k_mbx_info *);
+s32 fm10k_iov_msg_mac_vlan_pf(struct fm10k_hw *, u32 **,
+			      struct fm10k_mbx_info *);
+s32 fm10k_iov_msg_lport_state_pf(struct fm10k_hw *, u32 **,
+				 struct fm10k_mbx_info *);
+extern const struct fm10k_msg_data fm10k_iov_msg_data_pf[];
+
+extern struct fm10k_info fm10k_pf_info;
+#endif /* _FM10K_PF_H */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c b/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
new file mode 100644
index 000000000000..7822809436a3
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_ptp.c
@@ -0,0 +1,463 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include <linux/ptp_classify.h>
+#include <linux/ptp_clock_kernel.h>
+
+#include "fm10k.h"
+
+#define FM10K_TS_TX_TIMEOUT		(HZ * 15)
+
+void fm10k_systime_to_hwtstamp(struct fm10k_intfc *interface,
+			       struct skb_shared_hwtstamps *hwtstamp,
+			       u64 systime)
+{
+	unsigned long flags;
+
+	read_lock_irqsave(&interface->systime_lock, flags);
+	systime += interface->ptp_adjust;
+	read_unlock_irqrestore(&interface->systime_lock, flags);
+
+	hwtstamp->hwtstamp = ns_to_ktime(systime);
+}
+
+static struct sk_buff *fm10k_ts_tx_skb(struct fm10k_intfc *interface,
+				       __le16 dglort)
+{
+	struct sk_buff_head *list = &interface->ts_tx_skb_queue;
+	struct sk_buff *skb;
+
+	skb_queue_walk(list, skb) {
+		if (FM10K_CB(skb)->fi.w.dglort == dglort)
+			return skb;
+	}
+
+	return NULL;
+}
+
+void fm10k_ts_tx_enqueue(struct fm10k_intfc *interface, struct sk_buff *skb)
+{
+	struct sk_buff_head *list = &interface->ts_tx_skb_queue;
+	struct sk_buff *clone;
+	unsigned long flags;
+	__le16 dglort;
+
+	/* create clone for us to return on the Tx path */
+	clone = skb_clone_sk(skb);
+	if (!clone)
+		return;
+
+	FM10K_CB(clone)->ts_tx_timeout = jiffies + FM10K_TS_TX_TIMEOUT;
+	dglort = FM10K_CB(clone)->fi.w.dglort;
+
+	spin_lock_irqsave(&list->lock, flags);
+
+	/* attempt to locate any buffers with the same dglort,
+	 * if none are present then insert skb in tail of list
+	 */
+	skb = fm10k_ts_tx_skb(interface, FM10K_CB(clone)->fi.w.dglort);
+	if (!skb)
+		__skb_queue_tail(list, clone);
+
+	spin_unlock_irqrestore(&list->lock, flags);
+
+	/* if list is already has one then we just free the clone */
+	if (skb)
+		kfree_skb(skb);
+	else
+		skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
+}
+
+void fm10k_ts_tx_hwtstamp(struct fm10k_intfc *interface, __le16 dglort,
+			  u64 systime)
+{
+	struct skb_shared_hwtstamps shhwtstamps;
+	struct sk_buff_head *list = &interface->ts_tx_skb_queue;
+	struct sk_buff *skb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&list->lock, flags);
+
+	/* attempt to locate and pull the sk_buff out of the list */
+	skb = fm10k_ts_tx_skb(interface, dglort);
+	if (skb)
+		__skb_unlink(skb, list);
+
+	spin_unlock_irqrestore(&list->lock, flags);
+
+	/* if not found do nothing */
+	if (!skb)
+		return;
+
+	/* timestamp the sk_buff and return it to the socket */
+	fm10k_systime_to_hwtstamp(interface, &shhwtstamps, systime);
+	skb_complete_tx_timestamp(skb, &shhwtstamps);
+}
+
+void fm10k_ts_tx_subtask(struct fm10k_intfc *interface)
+{
+	struct sk_buff_head *list = &interface->ts_tx_skb_queue;
+	struct sk_buff *skb, *tmp;
+	unsigned long flags;
+
+	/* If we're down or resetting, just bail */
+	if (test_bit(__FM10K_DOWN, &interface->state) ||
+	    test_bit(__FM10K_RESETTING, &interface->state))
+		return;
+
+	spin_lock_irqsave(&list->lock, flags);
+
+	/* walk though the list and flush any expired timestamp packets */
+	skb_queue_walk_safe(list, skb, tmp) {
+		if (!time_is_after_jiffies(FM10K_CB(skb)->ts_tx_timeout))
+			continue;
+		__skb_unlink(skb, list);
+		kfree_skb(skb);
+		interface->tx_hwtstamp_timeouts++;
+	}
+
+	spin_unlock_irqrestore(&list->lock, flags);
+}
+
+static u64 fm10k_systime_read(struct fm10k_intfc *interface)
+{
+	struct fm10k_hw *hw = &interface->hw;
+
+	return hw->mac.ops.read_systime(hw);
+}
+
+void fm10k_ts_reset(struct fm10k_intfc *interface)
+{
+	s64 ns = ktime_to_ns(ktime_get_real());
+	unsigned long flags;
+
+	/* reinitialize the clock */
+	write_lock_irqsave(&interface->systime_lock, flags);
+	interface->ptp_adjust = fm10k_systime_read(interface) - ns;
+	write_unlock_irqrestore(&interface->systime_lock, flags);
+}
+
+void fm10k_ts_init(struct fm10k_intfc *interface)
+{
+	/* Initialize lock protecting systime access */
+	rwlock_init(&interface->systime_lock);
+
+	/* Initialize skb queue for pending timestamp requests */
+	skb_queue_head_init(&interface->ts_tx_skb_queue);
+
+	/* reset the clock to current kernel time */
+	fm10k_ts_reset(interface);
+}
+
+/**
+ * fm10k_get_ts_config - get current hardware timestamping configuration
+ * @netdev: network interface device structure
+ * @ifreq: ioctl data
+ *
+ * This function returns the current timestamping settings. Rather than
+ * attempt to deconstruct registers to fill in the values, simply keep a copy
+ * of the old settings around, and return a copy when requested.
+ */
+int fm10k_get_ts_config(struct net_device *netdev, struct ifreq *ifr)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct hwtstamp_config *config = &interface->ts_config;
+
+	return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
+		-EFAULT : 0;
+}
+
+/**
+ * fm10k_set_ts_config - control hardware time stamping
+ * @netdev: network interface device structure
+ * @ifreq: ioctl data
+ *
+ * Outgoing time stamping can be enabled and disabled. Play nice and
+ * disable it when requested, although it shouldn't cause any overhead
+ * when no packet needs it. At most one packet in the queue may be
+ * marked for time stamping, otherwise it would be impossible to tell
+ * for sure to which packet the hardware time stamp belongs.
+ *
+ * Incoming time stamping has to be configured via the hardware
+ * filters. Not all combinations are supported, in particular event
+ * type has to be specified. Matching the kind of event packet is
+ * not supported, with the exception of "all V2 events regardless of
+ * level 2 or 4".
+ *
+ * Since hardware always timestamps Path delay packets when timestamping V2
+ * packets, regardless of the type specified in the register, only use V2
+ * Event mode. This more accurately tells the user what the hardware is going
+ * to do anyways.
+ */
+int fm10k_set_ts_config(struct net_device *netdev, struct ifreq *ifr)
+{
+	struct fm10k_intfc *interface = netdev_priv(netdev);
+	struct hwtstamp_config ts_config;
+
+	if (copy_from_user(&ts_config, ifr->ifr_data, sizeof(ts_config)))
+		return -EFAULT;
+
+	/* reserved for future extensions */
+	if (ts_config.flags)
+		return -EINVAL;
+
+	switch (ts_config.tx_type) {
+	case HWTSTAMP_TX_OFF:
+		break;
+	case HWTSTAMP_TX_ON:
+		/* we likely need some check here to see if this is supported */
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	switch (ts_config.rx_filter) {
+	case HWTSTAMP_FILTER_NONE:
+		interface->flags &= ~FM10K_FLAG_RX_TS_ENABLED;
+		break;
+	case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+	case HWTSTAMP_FILTER_PTP_V2_EVENT:
+	case HWTSTAMP_FILTER_PTP_V2_SYNC:
+	case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+	case HWTSTAMP_FILTER_ALL:
+		interface->flags |= FM10K_FLAG_RX_TS_ENABLED;
+		ts_config.rx_filter = HWTSTAMP_FILTER_ALL;
+		break;
+	default:
+		return -ERANGE;
+	}
+
+	/* save these settings for future reference */
+	interface->ts_config = ts_config;
+
+	return copy_to_user(ifr->ifr_data, &ts_config, sizeof(ts_config)) ?
+		-EFAULT : 0;
+}
+
+static int fm10k_ptp_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
+{
+	struct fm10k_intfc *interface;
+	struct fm10k_hw *hw;
+	int err;
+
+	interface = container_of(ptp, struct fm10k_intfc, ptp_caps);
+	hw = &interface->hw;
+
+	err = hw->mac.ops.adjust_systime(hw, ppb);
+
+	/* the only error we should see is if the value is out of range */
+	return (err == FM10K_ERR_PARAM) ? -ERANGE : err;
+}
+
+static int fm10k_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct fm10k_intfc *interface;
+	unsigned long flags;
+
+	interface = container_of(ptp, struct fm10k_intfc, ptp_caps);
+
+	write_lock_irqsave(&interface->systime_lock, flags);
+	interface->ptp_adjust += delta;
+	write_unlock_irqrestore(&interface->systime_lock, flags);
+
+	return 0;
+}
+
+static int fm10k_ptp_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
+{
+	struct fm10k_intfc *interface;
+	unsigned long flags;
+	u64 now;
+
+	interface = container_of(ptp, struct fm10k_intfc, ptp_caps);
+
+	read_lock_irqsave(&interface->systime_lock, flags);
+	now = fm10k_systime_read(interface) + interface->ptp_adjust;
+	read_unlock_irqrestore(&interface->systime_lock, flags);
+
+	*ts = ns_to_timespec(now);
+
+	return 0;
+}
+
+static int fm10k_ptp_settime(struct ptp_clock_info *ptp,
+			     const struct timespec *ts)
+{
+	struct fm10k_intfc *interface;
+	unsigned long flags;
+	u64 ns = timespec_to_ns(ts);
+
+	interface = container_of(ptp, struct fm10k_intfc, ptp_caps);
+
+	write_lock_irqsave(&interface->systime_lock, flags);
+	interface->ptp_adjust = fm10k_systime_read(interface) - ns;
+	write_unlock_irqrestore(&interface->systime_lock, flags);
+
+	return 0;
+}
+
+static int fm10k_ptp_enable(struct ptp_clock_info *ptp,
+			    struct ptp_clock_request *rq, int on)
+{
+	struct ptp_clock_time *t = &rq->perout.period;
+	struct fm10k_intfc *interface;
+	struct fm10k_hw *hw;
+	u64 period;
+	u32 step;
+
+	/* we can only support periodic output */
+	if (rq->type != PTP_CLK_REQ_PEROUT)
+		return -EINVAL;
+
+	/* verify the requested channel is there */
+	if (rq->perout.index >= ptp->n_per_out)
+		return -EINVAL;
+
+	/* we cannot enforce start time as there is no
+	 * mechanism for that in the hardware, we can only control
+	 * the period.
+	 */
+
+	/* we cannot support periods greater than 4 seconds due to reg limit */
+	if (t->sec > 4 || t->sec < 0)
+		return -ERANGE;
+
+	interface = container_of(ptp, struct fm10k_intfc, ptp_caps);
+	hw = &interface->hw;
+
+	/* we simply cannot support the operation if we don't have BAR4 */
+	if (!hw->sw_addr)
+		return -ENOTSUPP;
+
+	/* convert to unsigned 64b ns, verify we can put it in a 32b register */
+	period = t->sec * 1000000000LL + t->nsec;
+
+	/* determine the minimum size for period */
+	step = 2 * (fm10k_read_reg(hw, FM10K_SYSTIME_CFG) &
+		    FM10K_SYSTIME_CFG_STEP_MASK);
+
+	/* verify the value is in range supported by hardware */
+	if ((period && (period < step)) || (period > U32_MAX))
+		return -ERANGE;
+
+	/* notify hardware of request to being sending pulses */
+	fm10k_write_sw_reg(hw, FM10K_SW_SYSTIME_PULSE(rq->perout.index),
+			   (u32)period);
+
+	return 0;
+}
+
+static struct ptp_pin_desc fm10k_ptp_pd[2] = {
+	{
+		.name = "IEEE1588_PULSE0",
+		.index = 0,
+		.func = PTP_PF_PEROUT,
+		.chan = 0
+	},
+	{
+		.name = "IEEE1588_PULSE1",
+		.index = 1,
+		.func = PTP_PF_PEROUT,
+		.chan = 1
+	}
+};
+
+static int fm10k_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
+			    enum ptp_pin_function func, unsigned int chan)
+{
+	/* verify the requested pin is there */
+	if (pin >= ptp->n_pins || !ptp->pin_config)
+		return -EINVAL;
+
+	/* enforce locked channels, no changing them */
+	if (chan != ptp->pin_config[pin].chan)
+		return -EINVAL;
+
+	/* we want to keep the functions locked as well */
+	if (func != ptp->pin_config[pin].func)
+		return -EINVAL;
+
+	return 0;
+}
+
+void fm10k_ptp_register(struct fm10k_intfc *interface)
+{
+	struct ptp_clock_info *ptp_caps = &interface->ptp_caps;
+	struct device *dev = &interface->pdev->dev;
+	struct ptp_clock *ptp_clock;
+
+	snprintf(ptp_caps->name, sizeof(ptp_caps->name),
+		 "%s", interface->netdev->name);
+	ptp_caps->owner		= THIS_MODULE;
+	/* This math is simply the inverse of the math in
+	 * fm10k_adjust_systime_pf applied to an adjustment value
+	 * of 2^30 - 1 which is the maximum value of the register:
+	 * 	max_ppb == ((2^30 - 1) * 5^9) / 2^31
+	 */
+	ptp_caps->max_adj	= 976562;
+	ptp_caps->adjfreq	= fm10k_ptp_adjfreq;
+	ptp_caps->adjtime	= fm10k_ptp_adjtime;
+	ptp_caps->gettime	= fm10k_ptp_gettime;
+	ptp_caps->settime	= fm10k_ptp_settime;
+
+	/* provide pins if BAR4 is accessible */
+	if (interface->sw_addr) {
+		/* enable periodic outputs */
+		ptp_caps->n_per_out = 2;
+		ptp_caps->enable	= fm10k_ptp_enable;
+
+		/* enable clock pins */
+		ptp_caps->verify	= fm10k_ptp_verify;
+		ptp_caps->n_pins = 2;
+		ptp_caps->pin_config = fm10k_ptp_pd;
+	}
+
+	ptp_clock = ptp_clock_register(ptp_caps, dev);
+	if (IS_ERR(ptp_clock)) {
+		ptp_clock = NULL;
+		dev_err(dev, "ptp_clock_register failed\n");
+	} else {
+		dev_info(dev, "registered PHC device %s\n", ptp_caps->name);
+	}
+
+	interface->ptp_clock = ptp_clock;
+}
+
+void fm10k_ptp_unregister(struct fm10k_intfc *interface)
+{
+	struct ptp_clock *ptp_clock = interface->ptp_clock;
+	struct device *dev = &interface->pdev->dev;
+
+	if (!ptp_clock)
+		return;
+
+	interface->ptp_clock = NULL;
+
+	ptp_clock_unregister(ptp_clock);
+	dev_info(dev, "removed PHC %s\n", interface->ptp_caps.name);
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
new file mode 100644
index 000000000000..fd0a05f011a8
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.c
@@ -0,0 +1,863 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k_tlv.h"
+
+/**
+ *  fm10k_tlv_msg_init - Initialize message block for TLV data storage
+ *  @msg: Pointer to message block
+ *  @msg_id: Message ID indicating message type
+ *
+ *  This function return success if provided with a valid message pointer
+ **/
+s32 fm10k_tlv_msg_init(u32 *msg, u16 msg_id)
+{
+	/* verify pointer is not NULL */
+	if (!msg)
+		return FM10K_ERR_PARAM;
+
+	*msg = (FM10K_TLV_FLAGS_MSG << FM10K_TLV_FLAGS_SHIFT) | msg_id;
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_put_null_string - Place null terminated string on message
+ *  @msg: Pointer to message block
+ *  @attr_id: Attribute ID
+ *  @string: Pointer to string to be stored in attribute
+ *
+ *  This function will reorder a string to be CPU endian and store it in
+ *  the attribute buffer.  It will return success if provided with a valid
+ *  pointers.
+ **/
+s32 fm10k_tlv_attr_put_null_string(u32 *msg, u16 attr_id,
+				   const unsigned char *string)
+{
+	u32 attr_data = 0, len = 0;
+	u32 *attr;
+
+	/* verify pointers are not NULL */
+	if (!string || !msg)
+		return FM10K_ERR_PARAM;
+
+	attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
+
+	/* copy string into local variable and then write to msg */
+	do {
+		/* write data to message */
+		if (len && !(len % 4)) {
+			attr[len / 4] = attr_data;
+			attr_data = 0;
+		}
+
+		/* record character to offset location */
+		attr_data |= (u32)(*string) << (8 * (len % 4));
+		len++;
+
+		/* test for NULL and then increment */
+	} while (*(string++));
+
+	/* write last piece of data to message */
+	attr[(len + 3) / 4] = attr_data;
+
+	/* record attribute header, update message length */
+	len <<= FM10K_TLV_LEN_SHIFT;
+	attr[0] = len | attr_id;
+
+	/* add header length to length */
+	len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
+	*msg += FM10K_TLV_LEN_ALIGN(len);
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_get_null_string - Get null terminated string from attribute
+ *  @attr: Pointer to attribute
+ *  @string: Pointer to location of destination string
+ *
+ *  This function pulls the string back out of the attribute and will place
+ *  it in the array pointed by by string.  It will return success if provided
+ *  with a valid pointers.
+ **/
+s32 fm10k_tlv_attr_get_null_string(u32 *attr, unsigned char *string)
+{
+	u32 len;
+
+	/* verify pointers are not NULL */
+	if (!string || !attr)
+		return FM10K_ERR_PARAM;
+
+	len = *attr >> FM10K_TLV_LEN_SHIFT;
+	attr++;
+
+	while (len--)
+		string[len] = (u8)(attr[len / 4] >> (8 * (len % 4)));
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_put_mac_vlan - Store MAC/VLAN attribute in message
+ *  @msg: Pointer to message block
+ *  @attr_id: Attribute ID
+ *  @mac_addr: MAC address to be stored
+ *
+ *  This function will reorder a MAC address to be CPU endian and store it
+ *  in the attribute buffer.  It will return success if provided with a
+ *  valid pointers.
+ **/
+s32 fm10k_tlv_attr_put_mac_vlan(u32 *msg, u16 attr_id,
+				const u8 *mac_addr, u16 vlan)
+{
+	u32 len = ETH_ALEN << FM10K_TLV_LEN_SHIFT;
+	u32 *attr;
+
+	/* verify pointers are not NULL */
+	if (!msg || !mac_addr)
+		return FM10K_ERR_PARAM;
+
+	attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
+
+	/* record attribute header, update message length */
+	attr[0] = len | attr_id;
+
+	/* copy value into local variable and then write to msg */
+	attr[1] = le32_to_cpu(*(const __le32 *)&mac_addr[0]);
+	attr[2] = le16_to_cpu(*(const __le16 *)&mac_addr[4]);
+	attr[2] |= (u32)vlan << 16;
+
+	/* add header length to length */
+	len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
+	*msg += FM10K_TLV_LEN_ALIGN(len);
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_get_mac_vlan - Get MAC/VLAN stored in attribute
+ *  @attr: Pointer to attribute
+ *  @attr_id: Attribute ID
+ *  @mac_addr: location of buffer to store MAC address
+ *
+ *  This function pulls the MAC address back out of the attribute and will
+ *  place it in the array pointed by by mac_addr.  It will return success
+ *  if provided with a valid pointers.
+ **/
+s32 fm10k_tlv_attr_get_mac_vlan(u32 *attr, u8 *mac_addr, u16 *vlan)
+{
+	/* verify pointers are not NULL */
+	if (!mac_addr || !attr)
+		return FM10K_ERR_PARAM;
+
+	*(__le32 *)&mac_addr[0] = cpu_to_le32(attr[1]);
+	*(__le16 *)&mac_addr[4] = cpu_to_le16((u16)(attr[2]));
+	*vlan = (u16)(attr[2] >> 16);
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_put_bool - Add header indicating value "true"
+ *  @msg: Pointer to message block
+ *  @attr_id: Attribute ID
+ *
+ *  This function will simply add an attribute header, the fact
+ *  that the header is here means the attribute value is true, else
+ *  it is false.  The function will return success if provided with a
+ *  valid pointers.
+ **/
+s32 fm10k_tlv_attr_put_bool(u32 *msg, u16 attr_id)
+{
+	/* verify pointers are not NULL */
+	if (!msg)
+		return FM10K_ERR_PARAM;
+
+	/* record attribute header */
+	msg[FM10K_TLV_DWORD_LEN(*msg)] = attr_id;
+
+	/* add header length to length */
+	*msg += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_put_value - Store integer value attribute in message
+ *  @msg: Pointer to message block
+ *  @attr_id: Attribute ID
+ *  @value: Value to be written
+ *  @len: Size of value
+ *
+ *  This function will place an integer value of up to 8 bytes in size
+ *  in a message attribute.  The function will return success provided
+ *  that msg is a valid pointer, and len is 1, 2, 4, or 8.
+ **/
+s32 fm10k_tlv_attr_put_value(u32 *msg, u16 attr_id, s64 value, u32 len)
+{
+	u32 *attr;
+
+	/* verify non-null msg and len is 1, 2, 4, or 8 */
+	if (!msg || !len || len > 8 || (len & (len - 1)))
+		return FM10K_ERR_PARAM;
+
+	attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
+
+	if (len < 4) {
+		attr[1] = (u32)value & ((0x1ul << (8 * len)) - 1);
+	} else {
+		attr[1] = (u32)value;
+		if (len > 4)
+			attr[2] = (u32)(value >> 32);
+	}
+
+	/* record attribute header, update message length */
+	len <<= FM10K_TLV_LEN_SHIFT;
+	attr[0] = len | attr_id;
+
+	/* add header length to length */
+	len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
+	*msg += FM10K_TLV_LEN_ALIGN(len);
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_get_value - Get integer value stored in attribute
+ *  @attr: Pointer to attribute
+ *  @value: Pointer to destination buffer
+ *  @len: Size of value
+ *
+ *  This function will place an integer value of up to 8 bytes in size
+ *  in the offset pointed to by value.  The function will return success
+ *  provided that pointers are valid and the len value matches the
+ *  attribute length.
+ **/
+s32 fm10k_tlv_attr_get_value(u32 *attr, void *value, u32 len)
+{
+	/* verify pointers are not NULL */
+	if (!attr || !value)
+		return FM10K_ERR_PARAM;
+
+	if ((*attr >> FM10K_TLV_LEN_SHIFT) != len)
+		return FM10K_ERR_PARAM;
+
+	if (len == 8)
+		*(u64 *)value = ((u64)attr[2] << 32) | attr[1];
+	else if (len == 4)
+		*(u32 *)value = attr[1];
+	else if (len == 2)
+		*(u16 *)value = (u16)attr[1];
+	else
+		*(u8 *)value = (u8)attr[1];
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_put_le_struct - Store little endian structure in message
+ *  @msg: Pointer to message block
+ *  @attr_id: Attribute ID
+ *  @le_struct: Pointer to structure to be written
+ *  @len: Size of le_struct
+ *
+ *  This function will place a little endian structure value in a message
+ *  attribute.  The function will return success provided that all pointers
+ *  are valid and length is a non-zero multiple of 4.
+ **/
+s32 fm10k_tlv_attr_put_le_struct(u32 *msg, u16 attr_id,
+				 const void *le_struct, u32 len)
+{
+	const __le32 *le32_ptr = (const __le32 *)le_struct;
+	u32 *attr;
+	u32 i;
+
+	/* verify non-null msg and len is in 32 bit words */
+	if (!msg || !len || (len % 4))
+		return FM10K_ERR_PARAM;
+
+	attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
+
+	/* copy le32 structure into host byte order at 32b boundaries */
+	for (i = 0; i < (len / 4); i++)
+		attr[i + 1] = le32_to_cpu(le32_ptr[i]);
+
+	/* record attribute header, update message length */
+	len <<= FM10K_TLV_LEN_SHIFT;
+	attr[0] = len | attr_id;
+
+	/* add header length to length */
+	len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
+	*msg += FM10K_TLV_LEN_ALIGN(len);
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_get_le_struct - Get little endian struct form attribute
+ *  @attr: Pointer to attribute
+ *  @le_struct: Pointer to structure to be written
+ *  @len: Size of structure
+ *
+ *  This function will place a little endian structure in the buffer
+ *  pointed to by le_struct.  The function will return success
+ *  provided that pointers are valid and the len value matches the
+ *  attribute length.
+ **/
+s32 fm10k_tlv_attr_get_le_struct(u32 *attr, void *le_struct, u32 len)
+{
+	__le32 *le32_ptr = (__le32 *)le_struct;
+	u32 i;
+
+	/* verify pointers are not NULL */
+	if (!le_struct || !attr)
+		return FM10K_ERR_PARAM;
+
+	if ((*attr >> FM10K_TLV_LEN_SHIFT) != len)
+		return FM10K_ERR_PARAM;
+
+	attr++;
+
+	for (i = 0; len; i++, len -= 4)
+		le32_ptr[i] = cpu_to_le32(attr[i]);
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_nest_start - Start a set of nested attributes
+ *  @msg: Pointer to message block
+ *  @attr_id: Attribute ID
+ *
+ *  This function will mark off a new nested region for encapsulating
+ *  a given set of attributes.  The idea is if you wish to place a secondary
+ *  structure within the message this mechanism allows for that.  The
+ *  function will return NULL on failure, and a pointer to the start
+ *  of the nested attributes on success.
+ **/
+u32 *fm10k_tlv_attr_nest_start(u32 *msg, u16 attr_id)
+{
+	u32 *attr;
+
+	/* verify pointer is not NULL */
+	if (!msg)
+		return NULL;
+
+	attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
+
+	attr[0] = attr_id;
+
+	/* return pointer to nest header */
+	return attr;
+}
+
+/**
+ *  fm10k_tlv_attr_nest_start - Start a set of nested attributes
+ *  @msg: Pointer to message block
+ *
+ *  This function closes off an existing set of nested attributes.  The
+ *  message pointer should be pointing to the parent of the nest.  So in
+ *  the case of a nest within the nest this would be the outer nest pointer.
+ *  This function will return success provided all pointers are valid.
+ **/
+s32 fm10k_tlv_attr_nest_stop(u32 *msg)
+{
+	u32 *attr;
+	u32 len;
+
+	/* verify pointer is not NULL */
+	if (!msg)
+		return FM10K_ERR_PARAM;
+
+	/* locate the nested header and retrieve its length */
+	attr = &msg[FM10K_TLV_DWORD_LEN(*msg)];
+	len = (attr[0] >> FM10K_TLV_LEN_SHIFT) << FM10K_TLV_LEN_SHIFT;
+
+	/* only include nest if data was added to it */
+	if (len) {
+		len += FM10K_TLV_HDR_LEN << FM10K_TLV_LEN_SHIFT;
+		*msg += len;
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_validate - Validate attribute metadata
+ *  @attr: Pointer to attribute
+ *  @tlv_attr: Type and length info for attribute
+ *
+ *  This function does some basic validation of the input TLV.  It
+ *  verifies the length, and in the case of null terminated strings
+ *  it verifies that the last byte is null.  The function will
+ *  return FM10K_ERR_PARAM if any attribute is malformed, otherwise
+ *  it returns 0.
+ **/
+static s32 fm10k_tlv_attr_validate(u32 *attr,
+				   const struct fm10k_tlv_attr *tlv_attr)
+{
+	u32 attr_id = *attr & FM10K_TLV_ID_MASK;
+	u16 len = *attr >> FM10K_TLV_LEN_SHIFT;
+
+	/* verify this is an attribute and not a message */
+	if (*attr & (FM10K_TLV_FLAGS_MSG << FM10K_TLV_FLAGS_SHIFT))
+		return FM10K_ERR_PARAM;
+
+	/* search through the list of attributes to find a matching ID */
+	while (tlv_attr->id < attr_id)
+		tlv_attr++;
+
+	/* if didn't find a match then we should exit */
+	if (tlv_attr->id != attr_id)
+		return FM10K_NOT_IMPLEMENTED;
+
+	/* move to start of attribute data */
+	attr++;
+
+	switch (tlv_attr->type) {
+	case FM10K_TLV_NULL_STRING:
+		if (!len ||
+		    (attr[(len - 1) / 4] & (0xFF << (8 * ((len - 1) % 4)))))
+			return FM10K_ERR_PARAM;
+		if (len > tlv_attr->len)
+			return FM10K_ERR_PARAM;
+		break;
+	case FM10K_TLV_MAC_ADDR:
+		if (len != ETH_ALEN)
+			return FM10K_ERR_PARAM;
+		break;
+	case FM10K_TLV_BOOL:
+		if (len)
+			return FM10K_ERR_PARAM;
+		break;
+	case FM10K_TLV_UNSIGNED:
+	case FM10K_TLV_SIGNED:
+		if (len != tlv_attr->len)
+			return FM10K_ERR_PARAM;
+		break;
+	case FM10K_TLV_LE_STRUCT:
+		/* struct must be 4 byte aligned */
+		if ((len % 4) || len != tlv_attr->len)
+			return FM10K_ERR_PARAM;
+		break;
+	case FM10K_TLV_NESTED:
+		/* nested attributes must be 4 byte aligned */
+		if (len % 4)
+			return FM10K_ERR_PARAM;
+		break;
+	default:
+		/* attribute id is mapped to bad value */
+		return FM10K_ERR_PARAM;
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_attr_parse - Parses stream of attribute data
+ *  @attr: Pointer to attribute list
+ *  @results: Pointer array to store pointers to attributes
+ *  @tlv_attr: Type and length info for attributes
+ *
+ *  This function validates a stream of attributes and parses them
+ *  up into an array of pointers stored in results.  The function will
+ *  return FM10K_ERR_PARAM on any input or message error,
+ *  FM10K_NOT_IMPLEMENTED for any attribute that is outside of the array
+ *  and 0 on success.
+ **/
+s32 fm10k_tlv_attr_parse(u32 *attr, u32 **results,
+			 const struct fm10k_tlv_attr *tlv_attr)
+{
+	u32 i, attr_id, offset = 0;
+	s32 err = 0;
+	u16 len;
+
+	/* verify pointers are not NULL */
+	if (!attr || !results)
+		return FM10K_ERR_PARAM;
+
+	/* initialize results to NULL */
+	for (i = 0; i < FM10K_TLV_RESULTS_MAX; i++)
+		results[i] = NULL;
+
+	/* pull length from the message header */
+	len = *attr >> FM10K_TLV_LEN_SHIFT;
+
+	/* no attributes to parse if there is no length */
+	if (!len)
+		return 0;
+
+	/* no attributes to parse, just raw data, message becomes attribute */
+	if (!tlv_attr) {
+		results[0] = attr;
+		return 0;
+	}
+
+	/* move to start of attribute data */
+	attr++;
+
+	/* run through list parsing all attributes */
+	while (offset < len) {
+		attr_id = *attr & FM10K_TLV_ID_MASK;
+
+		if (attr_id < FM10K_TLV_RESULTS_MAX)
+			err = fm10k_tlv_attr_validate(attr, tlv_attr);
+		else
+			err = FM10K_NOT_IMPLEMENTED;
+
+		if (err < 0)
+			return err;
+		if (!err)
+			results[attr_id] = attr;
+
+		/* update offset */
+		offset += FM10K_TLV_DWORD_LEN(*attr) * 4;
+
+		/* move to next attribute */
+		attr = &attr[FM10K_TLV_DWORD_LEN(*attr)];
+	}
+
+	/* we should find ourselves at the end of the list */
+	if (offset != len)
+		return FM10K_ERR_PARAM;
+
+	return 0;
+}
+
+/**
+ *  fm10k_tlv_msg_parse - Parses message header and calls function handler
+ *  @hw: Pointer to hardware structure
+ *  @msg: Pointer to message
+ *  @mbx: Pointer to mailbox information structure
+ *  @func: Function array containing list of message handling functions
+ *
+ *  This function should be the first function called upon receiving a
+ *  message.  The handler will identify the message type and call the correct
+ *  handler for the given message.  It will return the value from the function
+ *  call on a recognized message type, otherwise it will return
+ *  FM10K_NOT_IMPLEMENTED on an unrecognized type.
+ **/
+s32 fm10k_tlv_msg_parse(struct fm10k_hw *hw, u32 *msg,
+			struct fm10k_mbx_info *mbx,
+			const struct fm10k_msg_data *data)
+{
+	u32 *results[FM10K_TLV_RESULTS_MAX];
+	u32 msg_id;
+	s32 err;
+
+	/* verify pointer is not NULL */
+	if (!msg || !data)
+		return FM10K_ERR_PARAM;
+
+	/* verify this is a message and not an attribute */
+	if (!(*msg & (FM10K_TLV_FLAGS_MSG << FM10K_TLV_FLAGS_SHIFT)))
+		return FM10K_ERR_PARAM;
+
+	/* grab message ID */
+	msg_id = *msg & FM10K_TLV_ID_MASK;
+
+	while (data->id < msg_id)
+		data++;
+
+	/* if we didn't find it then pass it up as an error */
+	if (data->id != msg_id) {
+		while (data->id != FM10K_TLV_ERROR)
+			data++;
+	}
+
+	/* parse the attributes into the results list */
+	err = fm10k_tlv_attr_parse(msg, results, data->attr);
+	if (err < 0)
+		return err;
+
+	return data->func(hw, results, mbx);
+}
+
+/**
+ *  fm10k_tlv_msg_error - Default handler for unrecognized TLV message IDs
+ *  @hw: Pointer to hardware structure
+ *  @results: Pointer array to message, results[0] is pointer to message
+ *  @mbx: Unused mailbox pointer
+ *
+ *  This function is a default handler for unrecognized messages.  At a
+ *  a minimum it just indicates that the message requested was
+ *  unimplemented.
+ **/
+s32 fm10k_tlv_msg_error(struct fm10k_hw *hw, u32 **results,
+			struct fm10k_mbx_info *mbx)
+{
+	return FM10K_NOT_IMPLEMENTED;
+}
+
+static const unsigned char test_str[] =	"fm10k";
+static const unsigned char test_mac[ETH_ALEN] = { 0x12, 0x34, 0x56,
+						  0x78, 0x9a, 0xbc };
+static const u16 test_vlan = 0x0FED;
+static const u64 test_u64 = 0xfedcba9876543210ull;
+static const u32 test_u32 = 0x87654321;
+static const u16 test_u16 = 0x8765;
+static const u8  test_u8  = 0x87;
+static const s64 test_s64 = -0x123456789abcdef0ll;
+static const s32 test_s32 = -0x1235678;
+static const s16 test_s16 = -0x1234;
+static const s8  test_s8  = -0x12;
+static const __le32 test_le[2] = { cpu_to_le32(0x12345678),
+				   cpu_to_le32(0x9abcdef0)};
+
+/* The message below is meant to be used as a test message to demonstrate
+ * how to use the TLV interface and to test the types.  Normally this code
+ * be compiled out by stripping the code wrapped in FM10K_TLV_TEST_MSG
+ */
+const struct fm10k_tlv_attr fm10k_tlv_msg_test_attr[] = {
+	FM10K_TLV_ATTR_NULL_STRING(FM10K_TEST_MSG_STRING, 80),
+	FM10K_TLV_ATTR_MAC_ADDR(FM10K_TEST_MSG_MAC_ADDR),
+	FM10K_TLV_ATTR_U8(FM10K_TEST_MSG_U8),
+	FM10K_TLV_ATTR_U16(FM10K_TEST_MSG_U16),
+	FM10K_TLV_ATTR_U32(FM10K_TEST_MSG_U32),
+	FM10K_TLV_ATTR_U64(FM10K_TEST_MSG_U64),
+	FM10K_TLV_ATTR_S8(FM10K_TEST_MSG_S8),
+	FM10K_TLV_ATTR_S16(FM10K_TEST_MSG_S16),
+	FM10K_TLV_ATTR_S32(FM10K_TEST_MSG_S32),
+	FM10K_TLV_ATTR_S64(FM10K_TEST_MSG_S64),
+	FM10K_TLV_ATTR_LE_STRUCT(FM10K_TEST_MSG_LE_STRUCT, 8),
+	FM10K_TLV_ATTR_NESTED(FM10K_TEST_MSG_NESTED),
+	FM10K_TLV_ATTR_S32(FM10K_TEST_MSG_RESULT),
+	FM10K_TLV_ATTR_LAST
+};
+
+/**
+ *  fm10k_tlv_msg_test_generate_data - Stuff message with data
+ *  @msg: Pointer to message
+ *  @attr_flags: List of flags indicating what attributes to add
+ *
+ *  This function is meant to load a message buffer with attribute data
+ **/
+static void fm10k_tlv_msg_test_generate_data(u32 *msg, u32 attr_flags)
+{
+	if (attr_flags & (1 << FM10K_TEST_MSG_STRING))
+		fm10k_tlv_attr_put_null_string(msg, FM10K_TEST_MSG_STRING,
+					       test_str);
+	if (attr_flags & (1 << FM10K_TEST_MSG_MAC_ADDR))
+		fm10k_tlv_attr_put_mac_vlan(msg, FM10K_TEST_MSG_MAC_ADDR,
+					    test_mac, test_vlan);
+	if (attr_flags & (1 << FM10K_TEST_MSG_U8))
+		fm10k_tlv_attr_put_u8(msg, FM10K_TEST_MSG_U8,  test_u8);
+	if (attr_flags & (1 << FM10K_TEST_MSG_U16))
+		fm10k_tlv_attr_put_u16(msg, FM10K_TEST_MSG_U16, test_u16);
+	if (attr_flags & (1 << FM10K_TEST_MSG_U32))
+		fm10k_tlv_attr_put_u32(msg, FM10K_TEST_MSG_U32, test_u32);
+	if (attr_flags & (1 << FM10K_TEST_MSG_U64))
+		fm10k_tlv_attr_put_u64(msg, FM10K_TEST_MSG_U64, test_u64);
+	if (attr_flags & (1 << FM10K_TEST_MSG_S8))
+		fm10k_tlv_attr_put_s8(msg, FM10K_TEST_MSG_S8,  test_s8);
+	if (attr_flags & (1 << FM10K_TEST_MSG_S16))
+		fm10k_tlv_attr_put_s16(msg, FM10K_TEST_MSG_S16, test_s16);
+	if (attr_flags & (1 << FM10K_TEST_MSG_S32))
+		fm10k_tlv_attr_put_s32(msg, FM10K_TEST_MSG_S32, test_s32);
+	if (attr_flags & (1 << FM10K_TEST_MSG_S64))
+		fm10k_tlv_attr_put_s64(msg, FM10K_TEST_MSG_S64, test_s64);
+	if (attr_flags & (1 << FM10K_TEST_MSG_LE_STRUCT))
+		fm10k_tlv_attr_put_le_struct(msg, FM10K_TEST_MSG_LE_STRUCT,
+					     test_le, 8);
+}
+
+/**
+ *  fm10k_tlv_msg_test_create - Create a test message testing all attributes
+ *  @msg: Pointer to message
+ *  @attr_flags: List of flags indicating what attributes to add
+ *
+ *  This function is meant to load a message buffer with all attribute types
+ *  including a nested attribute.
+ **/
+void fm10k_tlv_msg_test_create(u32 *msg, u32 attr_flags)
+{
+	u32 *nest = NULL;
+
+	fm10k_tlv_msg_init(msg, FM10K_TLV_MSG_ID_TEST);
+
+	fm10k_tlv_msg_test_generate_data(msg, attr_flags);
+
+	/* check for nested attributes */
+	attr_flags >>= FM10K_TEST_MSG_NESTED;
+
+	if (attr_flags) {
+		nest = fm10k_tlv_attr_nest_start(msg, FM10K_TEST_MSG_NESTED);
+
+		fm10k_tlv_msg_test_generate_data(nest, attr_flags);
+
+		fm10k_tlv_attr_nest_stop(msg);
+	}
+}
+
+/**
+ *  fm10k_tlv_msg_test - Validate all results on test message receive
+ *  @hw: Pointer to hardware structure
+ *  @results: Pointer array to attributes in the mesage
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This function does a check to verify all attributes match what the test
+ *  message placed in the message buffer.  It is the default handler
+ *  for TLV test messages.
+ **/
+s32 fm10k_tlv_msg_test(struct fm10k_hw *hw, u32 **results,
+		       struct fm10k_mbx_info *mbx)
+{
+	u32 *nest_results[FM10K_TLV_RESULTS_MAX];
+	unsigned char result_str[80];
+	unsigned char result_mac[ETH_ALEN];
+	s32 err = 0;
+	__le32 result_le[2];
+	u16 result_vlan;
+	u64 result_u64;
+	u32 result_u32;
+	u16 result_u16;
+	u8  result_u8;
+	s64 result_s64;
+	s32 result_s32;
+	s16 result_s16;
+	s8  result_s8;
+	u32 reply[3];
+
+	/* retrieve results of a previous test */
+	if (!!results[FM10K_TEST_MSG_RESULT])
+		return fm10k_tlv_attr_get_s32(results[FM10K_TEST_MSG_RESULT],
+					      &mbx->test_result);
+
+parse_nested:
+	if (!!results[FM10K_TEST_MSG_STRING]) {
+		err = fm10k_tlv_attr_get_null_string(
+					results[FM10K_TEST_MSG_STRING],
+					result_str);
+		if (!err && memcmp(test_str, result_str, sizeof(test_str)))
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_MAC_ADDR]) {
+		err = fm10k_tlv_attr_get_mac_vlan(
+					results[FM10K_TEST_MSG_MAC_ADDR],
+					result_mac, &result_vlan);
+		if (!err && memcmp(test_mac, result_mac, ETH_ALEN))
+			err = FM10K_ERR_INVALID_VALUE;
+		if (!err && test_vlan != result_vlan)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_U8]) {
+		err = fm10k_tlv_attr_get_u8(results[FM10K_TEST_MSG_U8],
+					    &result_u8);
+		if (!err && test_u8 != result_u8)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_U16]) {
+		err = fm10k_tlv_attr_get_u16(results[FM10K_TEST_MSG_U16],
+					     &result_u16);
+		if (!err && test_u16 != result_u16)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_U32]) {
+		err = fm10k_tlv_attr_get_u32(results[FM10K_TEST_MSG_U32],
+					     &result_u32);
+		if (!err && test_u32 != result_u32)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_U64]) {
+		err = fm10k_tlv_attr_get_u64(results[FM10K_TEST_MSG_U64],
+					     &result_u64);
+		if (!err && test_u64 != result_u64)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_S8]) {
+		err = fm10k_tlv_attr_get_s8(results[FM10K_TEST_MSG_S8],
+					    &result_s8);
+		if (!err && test_s8 != result_s8)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_S16]) {
+		err = fm10k_tlv_attr_get_s16(results[FM10K_TEST_MSG_S16],
+					     &result_s16);
+		if (!err && test_s16 != result_s16)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_S32]) {
+		err = fm10k_tlv_attr_get_s32(results[FM10K_TEST_MSG_S32],
+					     &result_s32);
+		if (!err && test_s32 != result_s32)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_S64]) {
+		err = fm10k_tlv_attr_get_s64(results[FM10K_TEST_MSG_S64],
+					     &result_s64);
+		if (!err && test_s64 != result_s64)
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+	if (!!results[FM10K_TEST_MSG_LE_STRUCT]) {
+		err = fm10k_tlv_attr_get_le_struct(
+					results[FM10K_TEST_MSG_LE_STRUCT],
+					result_le,
+					sizeof(result_le));
+		if (!err && memcmp(test_le, result_le, sizeof(test_le)))
+			err = FM10K_ERR_INVALID_VALUE;
+		if (err)
+			goto report_result;
+	}
+
+	if (!!results[FM10K_TEST_MSG_NESTED]) {
+		/* clear any pointers */
+		memset(nest_results, 0, sizeof(nest_results));
+
+		/* parse the nested attributes into the nest results list */
+		err = fm10k_tlv_attr_parse(results[FM10K_TEST_MSG_NESTED],
+					   nest_results,
+					   fm10k_tlv_msg_test_attr);
+		if (err)
+			goto report_result;
+
+		/* loop back through to the start */
+		results = nest_results;
+		goto parse_nested;
+	}
+
+report_result:
+	/* generate reply with test result */
+	fm10k_tlv_msg_init(reply, FM10K_TLV_MSG_ID_TEST);
+	fm10k_tlv_attr_put_s32(reply, FM10K_TEST_MSG_RESULT, err);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, reply);
+}
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_tlv.h b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.h
new file mode 100644
index 000000000000..7e045e8bf1eb
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_tlv.h
@@ -0,0 +1,186 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifndef _FM10K_TLV_H_
+#define _FM10K_TLV_H_
+
+/* forward declaration */
+struct fm10k_msg_data;
+
+#include "fm10k_type.h"
+
+/* Message / Argument header format
+ *    3			  2		      1			  0
+ *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * |	     Length	   | Flags |	      Type / ID		   |
+ * +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * The message header format described here is used for messages that are
+ * passed between the PF and the VF.  To allow for messages larger then
+ * mailbox size we will provide a message with the above header and it
+ * will be segmented and transported to the mailbox to the other side where
+ * it is reassembled.  It contains the following fields:
+ * Len: Length of the message in bytes excluding the message header
+ * Flags: TBD
+ * Rule: These will be the message/argument types we pass
+ */
+/* message data header */
+#define FM10K_TLV_ID_SHIFT		0
+#define FM10K_TLV_ID_SIZE		16
+#define FM10K_TLV_ID_MASK		((1u << FM10K_TLV_ID_SIZE) - 1)
+#define FM10K_TLV_FLAGS_SHIFT		16
+#define FM10K_TLV_FLAGS_MSG		0x1
+#define FM10K_TLV_FLAGS_SIZE		4
+#define FM10K_TLV_LEN_SHIFT		20
+#define FM10K_TLV_LEN_SIZE		12
+
+#define FM10K_TLV_HDR_LEN		4ul
+#define FM10K_TLV_LEN_ALIGN_MASK \
+	((FM10K_TLV_HDR_LEN - 1) << FM10K_TLV_LEN_SHIFT)
+#define FM10K_TLV_LEN_ALIGN(tlv) \
+	(((tlv) + FM10K_TLV_LEN_ALIGN_MASK) & ~FM10K_TLV_LEN_ALIGN_MASK)
+#define FM10K_TLV_DWORD_LEN(tlv) \
+	((u16)((FM10K_TLV_LEN_ALIGN(tlv)) >> (FM10K_TLV_LEN_SHIFT + 2)) + 1)
+
+#define FM10K_TLV_RESULTS_MAX		32
+
+enum fm10k_tlv_type {
+	FM10K_TLV_NULL_STRING,
+	FM10K_TLV_MAC_ADDR,
+	FM10K_TLV_BOOL,
+	FM10K_TLV_UNSIGNED,
+	FM10K_TLV_SIGNED,
+	FM10K_TLV_LE_STRUCT,
+	FM10K_TLV_NESTED,
+	FM10K_TLV_MAX_TYPE
+};
+
+#define FM10K_TLV_ERROR (~0u)
+
+struct fm10k_tlv_attr {
+	unsigned int		id;
+	enum fm10k_tlv_type	type;
+	u16			len;
+};
+
+#define FM10K_TLV_ATTR_NULL_STRING(id, len) { id, FM10K_TLV_NULL_STRING, len }
+#define FM10K_TLV_ATTR_MAC_ADDR(id)	    { id, FM10K_TLV_MAC_ADDR, 6 }
+#define FM10K_TLV_ATTR_BOOL(id)		    { id, FM10K_TLV_BOOL, 0 }
+#define FM10K_TLV_ATTR_U8(id)		    { id, FM10K_TLV_UNSIGNED, 1 }
+#define FM10K_TLV_ATTR_U16(id)		    { id, FM10K_TLV_UNSIGNED, 2 }
+#define FM10K_TLV_ATTR_U32(id)		    { id, FM10K_TLV_UNSIGNED, 4 }
+#define FM10K_TLV_ATTR_U64(id)		    { id, FM10K_TLV_UNSIGNED, 8 }
+#define FM10K_TLV_ATTR_S8(id)		    { id, FM10K_TLV_SIGNED, 1 }
+#define FM10K_TLV_ATTR_S16(id)		    { id, FM10K_TLV_SIGNED, 2 }
+#define FM10K_TLV_ATTR_S32(id)		    { id, FM10K_TLV_SIGNED, 4 }
+#define FM10K_TLV_ATTR_S64(id)		    { id, FM10K_TLV_SIGNED, 8 }
+#define FM10K_TLV_ATTR_LE_STRUCT(id, len)   { id, FM10K_TLV_LE_STRUCT, len }
+#define FM10K_TLV_ATTR_NESTED(id)	    { id, FM10K_TLV_NESTED }
+#define FM10K_TLV_ATTR_LAST		    { FM10K_TLV_ERROR }
+
+struct fm10k_msg_data {
+	unsigned int		    id;
+	const struct fm10k_tlv_attr *attr;
+	s32			    (*func)(struct fm10k_hw *, u32 **,
+					    struct fm10k_mbx_info *);
+};
+
+#define FM10K_MSG_HANDLER(id, attr, func) { id, attr, func }
+
+s32 fm10k_tlv_msg_init(u32 *, u16);
+s32 fm10k_tlv_attr_put_null_string(u32 *, u16, const unsigned char *);
+s32 fm10k_tlv_attr_get_null_string(u32 *, unsigned char *);
+s32 fm10k_tlv_attr_put_mac_vlan(u32 *, u16, const u8 *, u16);
+s32 fm10k_tlv_attr_get_mac_vlan(u32 *, u8 *, u16 *);
+s32 fm10k_tlv_attr_put_bool(u32 *, u16);
+s32 fm10k_tlv_attr_put_value(u32 *, u16, s64, u32);
+#define fm10k_tlv_attr_put_u8(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 1)
+#define fm10k_tlv_attr_put_u16(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 2)
+#define fm10k_tlv_attr_put_u32(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 4)
+#define fm10k_tlv_attr_put_u64(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 8)
+#define fm10k_tlv_attr_put_s8(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 1)
+#define fm10k_tlv_attr_put_s16(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 2)
+#define fm10k_tlv_attr_put_s32(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 4)
+#define fm10k_tlv_attr_put_s64(msg, attr_id, val) \
+		fm10k_tlv_attr_put_value(msg, attr_id, val, 8)
+s32 fm10k_tlv_attr_get_value(u32 *, void *, u32);
+#define fm10k_tlv_attr_get_u8(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(u8))
+#define fm10k_tlv_attr_get_u16(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(u16))
+#define fm10k_tlv_attr_get_u32(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(u32))
+#define fm10k_tlv_attr_get_u64(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(u64))
+#define fm10k_tlv_attr_get_s8(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(s8))
+#define fm10k_tlv_attr_get_s16(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(s16))
+#define fm10k_tlv_attr_get_s32(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(s32))
+#define fm10k_tlv_attr_get_s64(attr, ptr) \
+		fm10k_tlv_attr_get_value(attr, ptr, sizeof(s64))
+s32 fm10k_tlv_attr_put_le_struct(u32 *, u16, const void *, u32);
+s32 fm10k_tlv_attr_get_le_struct(u32 *, void *, u32);
+u32 *fm10k_tlv_attr_nest_start(u32 *, u16);
+s32 fm10k_tlv_attr_nest_stop(u32 *);
+s32 fm10k_tlv_attr_parse(u32 *, u32 **, const struct fm10k_tlv_attr *);
+s32 fm10k_tlv_msg_parse(struct fm10k_hw *, u32 *, struct fm10k_mbx_info *,
+			const struct fm10k_msg_data *);
+s32 fm10k_tlv_msg_error(struct fm10k_hw *hw, u32 **results,
+			struct fm10k_mbx_info *);
+
+#define FM10K_TLV_MSG_ID_TEST	0
+
+enum fm10k_tlv_test_attr_id {
+	FM10K_TEST_MSG_UNSET,
+	FM10K_TEST_MSG_STRING,
+	FM10K_TEST_MSG_MAC_ADDR,
+	FM10K_TEST_MSG_U8,
+	FM10K_TEST_MSG_U16,
+	FM10K_TEST_MSG_U32,
+	FM10K_TEST_MSG_U64,
+	FM10K_TEST_MSG_S8,
+	FM10K_TEST_MSG_S16,
+	FM10K_TEST_MSG_S32,
+	FM10K_TEST_MSG_S64,
+	FM10K_TEST_MSG_LE_STRUCT,
+	FM10K_TEST_MSG_NESTED,
+	FM10K_TEST_MSG_RESULT,
+	FM10K_TEST_MSG_MAX
+};
+
+extern const struct fm10k_tlv_attr fm10k_tlv_msg_test_attr[];
+void fm10k_tlv_msg_test_create(u32 *, u32);
+s32 fm10k_tlv_msg_test(struct fm10k_hw *, u32 **, struct fm10k_mbx_info *);
+
+#define FM10K_TLV_MSG_TEST_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_TLV_MSG_ID_TEST, fm10k_tlv_msg_test_attr, func)
+#define FM10K_TLV_MSG_ERROR_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_TLV_ERROR, NULL, func)
+#endif /* _FM10K_MSG_H_ */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_type.h b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
new file mode 100644
index 000000000000..280296f29154
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_type.h
@@ -0,0 +1,770 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifndef _FM10K_TYPE_H_
+#define _FM10K_TYPE_H_
+
+/* forward declaration */
+struct fm10k_hw;
+
+#include <linux/types.h>
+#include <asm/byteorder.h>
+#include <linux/etherdevice.h>
+
+#include "fm10k_mbx.h"
+
+#define FM10K_DEV_ID_PF			0x15A4
+#define FM10K_DEV_ID_VF			0x15A5
+
+#define FM10K_MAX_QUEUES		256
+#define FM10K_MAX_QUEUES_PF		128
+#define FM10K_MAX_QUEUES_POOL		16
+
+#define FM10K_48_BIT_MASK		0x0000FFFFFFFFFFFFull
+#define FM10K_STAT_VALID		0x80000000
+
+/* PCI Bus Info */
+#define FM10K_PCIE_LINK_CAP		0x7C
+#define FM10K_PCIE_LINK_STATUS		0x82
+#define FM10K_PCIE_LINK_WIDTH		0x3F0
+#define FM10K_PCIE_LINK_WIDTH_1		0x10
+#define FM10K_PCIE_LINK_WIDTH_2		0x20
+#define FM10K_PCIE_LINK_WIDTH_4		0x40
+#define FM10K_PCIE_LINK_WIDTH_8		0x80
+#define FM10K_PCIE_LINK_SPEED		0xF
+#define FM10K_PCIE_LINK_SPEED_2500	0x1
+#define FM10K_PCIE_LINK_SPEED_5000	0x2
+#define FM10K_PCIE_LINK_SPEED_8000	0x3
+
+/* PCIe payload size */
+#define FM10K_PCIE_DEV_CAP			0x74
+#define FM10K_PCIE_DEV_CAP_PAYLOAD		0x07
+#define FM10K_PCIE_DEV_CAP_PAYLOAD_128		0x00
+#define FM10K_PCIE_DEV_CAP_PAYLOAD_256		0x01
+#define FM10K_PCIE_DEV_CAP_PAYLOAD_512		0x02
+#define FM10K_PCIE_DEV_CTRL			0x78
+#define FM10K_PCIE_DEV_CTRL_PAYLOAD		0xE0
+#define FM10K_PCIE_DEV_CTRL_PAYLOAD_128		0x00
+#define FM10K_PCIE_DEV_CTRL_PAYLOAD_256		0x20
+#define FM10K_PCIE_DEV_CTRL_PAYLOAD_512		0x40
+
+/* PCIe MSI-X Capability info */
+#define FM10K_PCI_MSIX_MSG_CTRL			0xB2
+#define FM10K_PCI_MSIX_MSG_CTRL_TBL_SZ_MASK	0x7FF
+#define FM10K_MAX_MSIX_VECTORS			256
+#define FM10K_MAX_VECTORS_PF			256
+#define FM10K_MAX_VECTORS_POOL			32
+
+/* PCIe SR-IOV Info */
+#define FM10K_PCIE_SRIOV_CTRL			0x190
+#define FM10K_PCIE_SRIOV_CTRL_VFARI		0x10
+
+#define FM10K_ERR_PARAM				-2
+#define FM10K_ERR_REQUESTS_PENDING		-4
+#define FM10K_ERR_RESET_REQUESTED		-5
+#define FM10K_ERR_DMA_PENDING			-6
+#define FM10K_ERR_RESET_FAILED			-7
+#define FM10K_ERR_INVALID_MAC_ADDR		-8
+#define FM10K_ERR_INVALID_VALUE			-9
+#define FM10K_NOT_IMPLEMENTED			0x7FFFFFFF
+
+/* Start of PF registers */
+#define FM10K_CTRL		0x0000
+#define FM10K_CTRL_BAR4_ALLOWED			0x00000004
+
+#define FM10K_CTRL_EXT		0x0001
+#define FM10K_GCR		0x0003
+#define FM10K_GCR_EXT		0x0005
+
+/* Interrupt control registers */
+#define FM10K_EICR		0x0006
+#define FM10K_EICR_FAULT_MASK			0x0000003F
+#define FM10K_EICR_MAILBOX			0x00000040
+#define FM10K_EICR_SWITCHREADY			0x00000080
+#define FM10K_EICR_SWITCHNOTREADY		0x00000100
+#define FM10K_EICR_SWITCHINTERRUPT		0x00000200
+#define FM10K_EICR_VFLR				0x00000800
+#define FM10K_EICR_MAXHOLDTIME			0x00001000
+#define FM10K_EIMR		0x0007
+#define FM10K_EIMR_PCA_FAULT			0x00000001
+#define FM10K_EIMR_THI_FAULT			0x00000010
+#define FM10K_EIMR_FUM_FAULT			0x00000400
+#define FM10K_EIMR_MAILBOX			0x00001000
+#define FM10K_EIMR_SWITCHREADY			0x00004000
+#define FM10K_EIMR_SWITCHNOTREADY		0x00010000
+#define FM10K_EIMR_SWITCHINTERRUPT		0x00040000
+#define FM10K_EIMR_SRAMERROR			0x00100000
+#define FM10K_EIMR_VFLR				0x00400000
+#define FM10K_EIMR_MAXHOLDTIME			0x01000000
+#define FM10K_EIMR_ALL				0x55555555
+#define FM10K_EIMR_DISABLE(NAME)		((FM10K_EIMR_ ## NAME) << 0)
+#define FM10K_EIMR_ENABLE(NAME)			((FM10K_EIMR_ ## NAME) << 1)
+#define FM10K_FAULT_ADDR_LO		0x0
+#define FM10K_FAULT_ADDR_HI		0x1
+#define FM10K_FAULT_SPECINFO		0x2
+#define FM10K_FAULT_FUNC		0x3
+#define FM10K_FAULT_SIZE		0x4
+#define FM10K_FAULT_FUNC_VALID			0x00008000
+#define FM10K_FAULT_FUNC_PF			0x00004000
+#define FM10K_FAULT_FUNC_VF_MASK		0x00003F00
+#define FM10K_FAULT_FUNC_VF_SHIFT		8
+#define FM10K_FAULT_FUNC_TYPE_MASK		0x000000FF
+
+#define FM10K_PCA_FAULT		0x0008
+#define FM10K_THI_FAULT		0x0010
+#define FM10K_FUM_FAULT		0x001C
+
+/* Rx queue timeout indicator */
+#define FM10K_MAXHOLDQ(_n)	((_n) + 0x0020)
+
+/* Switch Manager info */
+#define FM10K_SM_AREA(_n)	((_n) + 0x0028)
+
+/* GLORT mapping registers */
+#define FM10K_DGLORTMAP(_n)	((_n) + 0x0030)
+#define FM10K_DGLORT_COUNT			8
+#define FM10K_DGLORTMAP_MASK_SHIFT		16
+#define FM10K_DGLORTMAP_ANY			0x00000000
+#define FM10K_DGLORTMAP_NONE			0x0000FFFF
+#define FM10K_DGLORTMAP_ZERO			0xFFFF0000
+#define FM10K_DGLORTDEC(_n)	((_n) + 0x0038)
+#define FM10K_DGLORTDEC_VSILENGTH_SHIFT		4
+#define FM10K_DGLORTDEC_VSIBASE_SHIFT		7
+#define FM10K_DGLORTDEC_PCLENGTH_SHIFT		14
+#define FM10K_DGLORTDEC_QBASE_SHIFT		16
+#define FM10K_DGLORTDEC_RSSLENGTH_SHIFT		24
+#define FM10K_DGLORTDEC_INNERRSS_ENABLE		0x08000000
+#define FM10K_TUNNEL_CFG	0x0040
+#define FM10K_TUNNEL_CFG_NVGRE_SHIFT		16
+#define FM10K_SWPRI_MAP(_n)	((_n) + 0x0050)
+#define FM10K_SWPRI_MAX		16
+#define FM10K_RSSRK(_n, _m)	(((_n) * 0x10) + (_m) + 0x0800)
+#define FM10K_RSSRK_SIZE	10
+#define FM10K_RSSRK_ENTRIES_PER_REG		4
+#define FM10K_RETA(_n, _m)	(((_n) * 0x20) + (_m) + 0x1000)
+#define FM10K_RETA_SIZE		32
+#define FM10K_RETA_ENTRIES_PER_REG		4
+#define FM10K_MAX_RSS_INDICES	128
+
+/* Rate limiting registers */
+#define FM10K_TC_CREDIT(_n)	((_n) + 0x2000)
+#define FM10K_TC_CREDIT_CREDIT_MASK		0x001FFFFF
+#define FM10K_TC_MAXCREDIT(_n)	((_n) + 0x2040)
+#define FM10K_TC_MAXCREDIT_64K			0x00010000
+#define FM10K_TC_RATE(_n)	((_n) + 0x2080)
+#define FM10K_TC_RATE_QUANTA_MASK		0x0000FFFF
+#define FM10K_TC_RATE_INTERVAL_4US_GEN1		0x00020000
+#define FM10K_TC_RATE_INTERVAL_4US_GEN2		0x00040000
+#define FM10K_TC_RATE_INTERVAL_4US_GEN3		0x00080000
+
+/* DMA control registers */
+#define FM10K_DMA_CTRL		0x20C3
+#define FM10K_DMA_CTRL_TX_ENABLE		0x00000001
+#define FM10K_DMA_CTRL_TX_ACTIVE		0x00000008
+#define FM10K_DMA_CTRL_RX_ENABLE		0x00000010
+#define FM10K_DMA_CTRL_RX_ACTIVE		0x00000080
+#define FM10K_DMA_CTRL_RX_DESC_SIZE		0x00000100
+#define FM10K_DMA_CTRL_MINMSS_64		0x00008000
+#define FM10K_DMA_CTRL_MAX_HOLD_1US_GEN3	0x04800000
+#define FM10K_DMA_CTRL_MAX_HOLD_1US_GEN2	0x04000000
+#define FM10K_DMA_CTRL_MAX_HOLD_1US_GEN1	0x03800000
+#define FM10K_DMA_CTRL_DATAPATH_RESET		0x20000000
+#define FM10K_DMA_CTRL_32_DESC			0x00000000
+
+#define FM10K_DMA_CTRL2		0x20C4
+#define FM10K_DMA_CTRL2_SWITCH_READY		0x00002000
+
+/* TSO flags configuration
+ * First packet contains all flags except for fin and psh
+ * Middle packet contains only urg and ack
+ * Last packet contains urg, ack, fin, and psh
+ */
+#define FM10K_TSO_FLAGS_LOW		0x00300FF6
+#define FM10K_TSO_FLAGS_HI		0x00000039
+#define FM10K_DTXTCPFLGL	0x20C5
+#define FM10K_DTXTCPFLGH	0x20C6
+
+#define FM10K_TPH_CTRL		0x20C7
+#define FM10K_MRQC(_n)		((_n) + 0x2100)
+#define FM10K_MRQC_TCP_IPV4			0x00000001
+#define FM10K_MRQC_IPV4				0x00000002
+#define FM10K_MRQC_IPV6				0x00000010
+#define FM10K_MRQC_TCP_IPV6			0x00000020
+#define FM10K_MRQC_UDP_IPV4			0x00000040
+#define FM10K_MRQC_UDP_IPV6			0x00000080
+
+#define FM10K_TQMAP(_n)		((_n) + 0x2800)
+#define FM10K_TQMAP_TABLE_SIZE			2048
+#define FM10K_RQMAP(_n)		((_n) + 0x3000)
+
+/* Hardware Statistics */
+#define FM10K_STATS_TIMEOUT		0x3800
+#define FM10K_STATS_UR			0x3801
+#define FM10K_STATS_CA			0x3802
+#define FM10K_STATS_UM			0x3803
+#define FM10K_STATS_XEC			0x3804
+#define FM10K_STATS_VLAN_DROP		0x3805
+#define FM10K_STATS_LOOPBACK_DROP	0x3806
+#define FM10K_STATS_NODESC_DROP		0x3807
+
+/* Timesync registers */
+#define FM10K_SYSTIME		0x3814
+#define FM10K_SYSTIME_CFG	0x3818
+#define FM10K_SYSTIME_CFG_STEP_MASK		0x0000000F
+
+/* PCIe state registers */
+#define FM10K_PHYADDR		0x381C
+
+/* Rx ring registers */
+#define FM10K_RDBAL(_n)		((0x40 * (_n)) + 0x4000)
+#define FM10K_RDBAH(_n)		((0x40 * (_n)) + 0x4001)
+#define FM10K_RDLEN(_n)		((0x40 * (_n)) + 0x4002)
+#define FM10K_TPH_RXCTRL(_n)	((0x40 * (_n)) + 0x4003)
+#define FM10K_TPH_RXCTRL_DESC_TPHEN		0x00000020
+#define FM10K_TPH_RXCTRL_DESC_RROEN		0x00000200
+#define FM10K_TPH_RXCTRL_DATA_WROEN		0x00002000
+#define FM10K_TPH_RXCTRL_HDR_WROEN		0x00008000
+#define FM10K_RDH(_n)		((0x40 * (_n)) + 0x4004)
+#define FM10K_RDT(_n)		((0x40 * (_n)) + 0x4005)
+#define FM10K_RXQCTL(_n)	((0x40 * (_n)) + 0x4006)
+#define FM10K_RXQCTL_ENABLE			0x00000001
+#define FM10K_RXQCTL_PF				0x000000FC
+#define FM10K_RXQCTL_VF_SHIFT			2
+#define FM10K_RXQCTL_VF				0x00000100
+#define FM10K_RXQCTL_ID_MASK	(FM10K_RXQCTL_PF | FM10K_RXQCTL_VF)
+#define FM10K_RXDCTL(_n)	((0x40 * (_n)) + 0x4007)
+#define FM10K_RXDCTL_WRITE_BACK_MIN_DELAY	0x00000001
+#define FM10K_RXDCTL_DROP_ON_EMPTY		0x00000200
+#define FM10K_RXINT(_n)		((0x40 * (_n)) + 0x4008)
+#define FM10K_SRRCTL(_n)	((0x40 * (_n)) + 0x4009)
+#define FM10K_SRRCTL_BSIZEPKT_SHIFT		8 /* shift _right_ */
+#define FM10K_SRRCTL_LOOPBACK_SUPPRESS		0x40000000
+#define FM10K_SRRCTL_BUFFER_CHAINING_EN		0x80000000
+
+/* Rx Statistics */
+#define FM10K_QPRC(_n)		((0x40 * (_n)) + 0x400A)
+#define FM10K_QPRDC(_n)		((0x40 * (_n)) + 0x400B)
+#define FM10K_QBRC_L(_n)	((0x40 * (_n)) + 0x400C)
+#define FM10K_QBRC_H(_n)	((0x40 * (_n)) + 0x400D)
+
+/* Rx GLORT register */
+#define FM10K_RX_SGLORT(_n)		((0x40 * (_n)) + 0x400E)
+
+/* Tx ring registers */
+#define FM10K_TDBAL(_n)		((0x40 * (_n)) + 0x8000)
+#define FM10K_TDBAH(_n)		((0x40 * (_n)) + 0x8001)
+#define FM10K_TDLEN(_n)		((0x40 * (_n)) + 0x8002)
+#define FM10K_TPH_TXCTRL(_n)	((0x40 * (_n)) + 0x8003)
+#define FM10K_TPH_TXCTRL_DESC_TPHEN		0x00000020
+#define FM10K_TPH_TXCTRL_DESC_RROEN		0x00000200
+#define FM10K_TPH_TXCTRL_DESC_WROEN		0x00000800
+#define FM10K_TPH_TXCTRL_DATA_RROEN		0x00002000
+#define FM10K_TDH(_n)		((0x40 * (_n)) + 0x8004)
+#define FM10K_TDT(_n)		((0x40 * (_n)) + 0x8005)
+#define FM10K_TXDCTL(_n)	((0x40 * (_n)) + 0x8006)
+#define FM10K_TXDCTL_ENABLE			0x00004000
+#define FM10K_TXDCTL_MAX_TIME_SHIFT		16
+#define FM10K_TXQCTL(_n)	((0x40 * (_n)) + 0x8007)
+#define FM10K_TXQCTL_PF				0x0000003F
+#define FM10K_TXQCTL_VF				0x00000040
+#define FM10K_TXQCTL_ID_MASK	(FM10K_TXQCTL_PF | FM10K_TXQCTL_VF)
+#define FM10K_TXQCTL_PC_SHIFT			7
+#define FM10K_TXQCTL_PC_MASK			0x00000380
+#define FM10K_TXQCTL_TC_SHIFT			10
+#define FM10K_TXQCTL_VID_SHIFT			16
+#define FM10K_TXQCTL_VID_MASK			0x0FFF0000
+#define FM10K_TXQCTL_UNLIMITED_BW		0x10000000
+#define FM10K_TXINT(_n)		((0x40 * (_n)) + 0x8008)
+
+/* Tx Statistics */
+#define FM10K_QPTC(_n)		((0x40 * (_n)) + 0x8009)
+#define FM10K_QBTC_L(_n)	((0x40 * (_n)) + 0x800A)
+#define FM10K_QBTC_H(_n)	((0x40 * (_n)) + 0x800B)
+
+/* Tx Push registers */
+#define FM10K_TQDLOC(_n)	((0x40 * (_n)) + 0x800C)
+#define FM10K_TQDLOC_BASE_32_DESC		0x08
+#define FM10K_TQDLOC_SIZE_32_DESC		0x00050000
+
+/* Tx GLORT registers */
+#define FM10K_TX_SGLORT(_n)	((0x40 * (_n)) + 0x800D)
+#define FM10K_PFVTCTL(_n)	((0x40 * (_n)) + 0x800E)
+#define FM10K_PFVTCTL_FTAG_DESC_ENABLE		0x00000001
+
+/* Interrupt moderation and control registers */
+#define FM10K_INT_MAP(_n)	((_n) + 0x10080)
+#define FM10K_INT_MAP_TIMER0			0x00000000
+#define FM10K_INT_MAP_TIMER1			0x00000100
+#define FM10K_INT_MAP_IMMEDIATE			0x00000200
+#define FM10K_INT_MAP_DISABLE			0x00000300
+#define FM10K_MSIX_VECTOR_MASK(_n)	((0x4 * (_n)) + 0x11003)
+#define FM10K_INT_CTRL		0x12000
+#define FM10K_INT_CTRL_ENABLEMODERATOR		0x00000400
+#define FM10K_ITR(_n)		((_n) + 0x12400)
+#define FM10K_ITR_INTERVAL1_SHIFT		12
+#define FM10K_ITR_PENDING2			0x10000000
+#define FM10K_ITR_AUTOMASK			0x20000000
+#define FM10K_ITR_MASK_SET			0x40000000
+#define FM10K_ITR_MASK_CLEAR			0x80000000
+#define FM10K_ITR2(_n)		((0x2 * (_n)) + 0x12800)
+#define FM10K_ITR_REG_COUNT			768
+#define FM10K_ITR_REG_COUNT_PF			256
+
+/* Switch manager interrupt registers */
+#define FM10K_IP		0x13000
+#define FM10K_IP_NOTINRESET			0x00000100
+
+/* VLAN registers */
+#define FM10K_VLAN_TABLE(_n, _m)	((0x80 * (_n)) + (_m) + 0x14000)
+#define FM10K_VLAN_TABLE_SIZE			128
+
+/* VLAN specific message offsets */
+#define FM10K_VLAN_TABLE_VID_MAX		4096
+#define FM10K_VLAN_TABLE_VSI_MAX		64
+#define FM10K_VLAN_LENGTH_SHIFT			16
+#define FM10K_VLAN_CLEAR			(1 << 15)
+#define FM10K_VLAN_ALL \
+	((FM10K_VLAN_TABLE_VID_MAX - 1) << FM10K_VLAN_LENGTH_SHIFT)
+
+/* VF FLR event notification registers */
+#define FM10K_PFVFLRE(_n)	((0x1 * (_n)) + 0x18844)
+#define FM10K_PFVFLREC(_n)	((0x1 * (_n)) + 0x18846)
+
+/* Defines for size of uncacheable memories */
+#define FM10K_UC_ADDR_START	0x000000	/* start of standard regs */
+#define FM10K_UC_ADDR_END	0x100000	/* end of standard regs */
+#define FM10K_UC_ADDR_SIZE	(FM10K_UC_ADDR_END - FM10K_UC_ADDR_START)
+
+/* Define timeouts for resets and disables */
+#define FM10K_QUEUE_DISABLE_TIMEOUT		100
+#define FM10K_RESET_TIMEOUT			100
+
+/* VF registers */
+#define FM10K_VFCTRL		0x00000
+#define FM10K_VFCTRL_RST			0x00000008
+#define FM10K_VFINT_MAP		0x00030
+#define FM10K_VFSYSTIME		0x00040
+#define FM10K_VFITR(_n)		((_n) + 0x00060)
+
+/* Registers contained in BAR 4 for Switch management */
+#define FM10K_SW_SYSTIME_ADJUST	0x0224D
+#define FM10K_SW_SYSTIME_ADJUST_MASK		0x3FFFFFFF
+#define FM10K_SW_SYSTIME_ADJUST_DIR_NEGATIVE	0x80000000
+#define FM10K_SW_SYSTIME_PULSE(_n)	((_n) + 0x02252)
+
+enum fm10k_int_source {
+	fm10k_int_Mailbox	= 0,
+	fm10k_int_PCIeFault	= 1,
+	fm10k_int_SwitchUpDown	= 2,
+	fm10k_int_SwitchEvent	= 3,
+	fm10k_int_SRAM		= 4,
+	fm10k_int_VFLR		= 5,
+	fm10k_int_MaxHoldTime	= 6,
+	fm10k_int_sources_max_pf
+};
+
+/* PCIe bus speeds */
+enum fm10k_bus_speed {
+	fm10k_bus_speed_unknown	= 0,
+	fm10k_bus_speed_2500	= 2500,
+	fm10k_bus_speed_5000	= 5000,
+	fm10k_bus_speed_8000	= 8000,
+	fm10k_bus_speed_reserved
+};
+
+/* PCIe bus widths */
+enum fm10k_bus_width {
+	fm10k_bus_width_unknown	= 0,
+	fm10k_bus_width_pcie_x1	= 1,
+	fm10k_bus_width_pcie_x2	= 2,
+	fm10k_bus_width_pcie_x4	= 4,
+	fm10k_bus_width_pcie_x8	= 8,
+	fm10k_bus_width_reserved
+};
+
+/* PCIe payload sizes */
+enum fm10k_bus_payload {
+	fm10k_bus_payload_unknown = 0,
+	fm10k_bus_payload_128	  = 1,
+	fm10k_bus_payload_256	  = 2,
+	fm10k_bus_payload_512	  = 3,
+	fm10k_bus_payload_reserved
+};
+
+/* Bus parameters */
+struct fm10k_bus_info {
+	enum fm10k_bus_speed speed;
+	enum fm10k_bus_width width;
+	enum fm10k_bus_payload payload;
+};
+
+/* Statistics related declarations */
+struct fm10k_hw_stat {
+	u64 count;
+	u32 base_l;
+	u32 base_h;
+};
+
+struct fm10k_hw_stats_q {
+	struct fm10k_hw_stat tx_bytes;
+	struct fm10k_hw_stat tx_packets;
+#define tx_stats_idx	tx_packets.base_h
+	struct fm10k_hw_stat rx_bytes;
+	struct fm10k_hw_stat rx_packets;
+#define rx_stats_idx	rx_packets.base_h
+	struct fm10k_hw_stat rx_drops;
+};
+
+struct fm10k_hw_stats {
+	struct fm10k_hw_stat	timeout;
+#define stats_idx	timeout.base_h
+	struct fm10k_hw_stat	ur;
+	struct fm10k_hw_stat	ca;
+	struct fm10k_hw_stat	um;
+	struct fm10k_hw_stat	xec;
+	struct fm10k_hw_stat	vlan_drop;
+	struct fm10k_hw_stat	loopback_drop;
+	struct fm10k_hw_stat	nodesc_drop;
+	struct fm10k_hw_stats_q q[FM10K_MAX_QUEUES_PF];
+};
+
+/* Establish DGLORT feature priority */
+enum fm10k_dglortdec_idx {
+	fm10k_dglort_default	= 0,
+	fm10k_dglort_vf_rsvd0	= 1,
+	fm10k_dglort_vf_rss	= 2,
+	fm10k_dglort_pf_rsvd0	= 3,
+	fm10k_dglort_pf_queue	= 4,
+	fm10k_dglort_pf_vsi	= 5,
+	fm10k_dglort_pf_rsvd1	= 6,
+	fm10k_dglort_pf_rss	= 7
+};
+
+struct fm10k_dglort_cfg {
+	u16 glort;	/* GLORT base */
+	u16 queue_b;	/* Base value for queue */
+	u8  vsi_b;	/* Base value for VSI */
+	u8  idx;	/* index of DGLORTDEC entry */
+	u8  rss_l;	/* RSS indices */
+	u8  pc_l;	/* Priority Class indices */
+	u8  vsi_l;	/* Number of bits from GLORT used to determine VSI */
+	u8  queue_l;	/* Number of bits from GLORT used to determine queue */
+	u8  shared_l;	/* Ignored bits from GLORT resulting in shared VSI */
+	u8  inner_rss;	/* Boolean value if inner header is used for RSS */
+};
+
+enum fm10k_pca_fault {
+	PCA_NO_FAULT,
+	PCA_UNMAPPED_ADDR,
+	PCA_BAD_QACCESS_PF,
+	PCA_BAD_QACCESS_VF,
+	PCA_MALICIOUS_REQ,
+	PCA_POISONED_TLP,
+	PCA_TLP_ABORT,
+	__PCA_MAX
+};
+
+enum fm10k_thi_fault {
+	THI_NO_FAULT,
+	THI_MAL_DIS_Q_FAULT,
+	__THI_MAX
+};
+
+enum fm10k_fum_fault {
+	FUM_NO_FAULT,
+	FUM_UNMAPPED_ADDR,
+	FUM_POISONED_TLP,
+	FUM_BAD_VF_QACCESS,
+	FUM_ADD_DECODE_ERR,
+	FUM_RO_ERROR,
+	FUM_QPRC_CRC_ERROR,
+	FUM_CSR_TIMEOUT,
+	FUM_INVALID_TYPE,
+	FUM_INVALID_LENGTH,
+	FUM_INVALID_BE,
+	FUM_INVALID_ALIGN,
+	__FUM_MAX
+};
+
+struct fm10k_fault {
+	u64 address;	/* Address at the time fault was detected */
+	u32 specinfo;	/* Extra info on this fault (fault dependent) */
+	u8 type;	/* Fault value dependent on subunit */
+	u8 func;	/* Function number of the fault */
+};
+
+struct fm10k_mac_ops {
+	/* basic bring-up and tear-down */
+	s32 (*reset_hw)(struct fm10k_hw *);
+	s32 (*init_hw)(struct fm10k_hw *);
+	s32 (*start_hw)(struct fm10k_hw *);
+	s32 (*stop_hw)(struct fm10k_hw *);
+	s32 (*get_bus_info)(struct fm10k_hw *);
+	s32 (*get_host_state)(struct fm10k_hw *, bool *);
+	bool (*is_slot_appropriate)(struct fm10k_hw *);
+	s32 (*update_vlan)(struct fm10k_hw *, u32, u8, bool);
+	s32 (*read_mac_addr)(struct fm10k_hw *);
+	s32 (*update_uc_addr)(struct fm10k_hw *, u16, const u8 *,
+			      u16, bool, u8);
+	s32 (*update_mc_addr)(struct fm10k_hw *, u16, const u8 *, u16, bool);
+	s32 (*update_xcast_mode)(struct fm10k_hw *, u16, u8);
+	void (*update_int_moderator)(struct fm10k_hw *);
+	s32  (*update_lport_state)(struct fm10k_hw *, u16, u16, bool);
+	void (*update_hw_stats)(struct fm10k_hw *, struct fm10k_hw_stats *);
+	void (*rebind_hw_stats)(struct fm10k_hw *, struct fm10k_hw_stats *);
+	s32 (*configure_dglort_map)(struct fm10k_hw *,
+				    struct fm10k_dglort_cfg *);
+	void (*set_dma_mask)(struct fm10k_hw *, u64);
+	s32 (*get_fault)(struct fm10k_hw *, int, struct fm10k_fault *);
+	void (*request_lport_map)(struct fm10k_hw *);
+	s32 (*adjust_systime)(struct fm10k_hw *, s32 ppb);
+	u64 (*read_systime)(struct fm10k_hw *);
+};
+
+enum fm10k_mac_type {
+	fm10k_mac_unknown = 0,
+	fm10k_mac_pf,
+	fm10k_mac_vf,
+	fm10k_num_macs
+};
+
+struct fm10k_mac_info {
+	struct fm10k_mac_ops ops;
+	enum fm10k_mac_type type;
+	u8 addr[ETH_ALEN];
+	u8 perm_addr[ETH_ALEN];
+	u16 default_vid;
+	u16 max_msix_vectors;
+	u16 max_queues;
+	bool vlan_override;
+	bool get_host_state;
+	bool tx_ready;
+	u32 dglort_map;
+};
+
+struct fm10k_swapi_table_info {
+	u32 used;
+	u32 avail;
+};
+
+struct fm10k_swapi_info {
+	u32 status;
+	struct fm10k_swapi_table_info mac;
+	struct fm10k_swapi_table_info nexthop;
+	struct fm10k_swapi_table_info ffu;
+};
+
+enum fm10k_xcast_modes {
+	FM10K_XCAST_MODE_ALLMULTI	= 0,
+	FM10K_XCAST_MODE_MULTI		= 1,
+	FM10K_XCAST_MODE_PROMISC	= 2,
+	FM10K_XCAST_MODE_NONE		= 3,
+	FM10K_XCAST_MODE_DISABLE	= 4
+};
+
+#define FM10K_VF_TC_MAX		100000	/* 100,000 Mb/s aka 100Gb/s */
+#define FM10K_VF_TC_MIN		1	/* 1 Mb/s is the slowest rate */
+
+struct fm10k_vf_info {
+	/* mbx must be first field in struct unless all default IOV message
+	 * handlers are redone as the assumption is that vf_info starts
+	 * at the same offset as the mailbox
+	 */
+	struct fm10k_mbx_info	mbx;		/* PF side of VF mailbox */
+	int			rate;		/* Tx BW cap as defined by OS */
+	u16			glort;		/* resource tag for this VF */
+	u16			sw_vid;		/* Switch API assigned VLAN */
+	u16			pf_vid;		/* PF assigned Default VLAN */
+	u8			mac[ETH_ALEN];	/* PF Default MAC address */
+	u8			vsi;		/* VSI idenfifier */
+	u8			vf_idx;		/* which VF this is */
+	u8			vf_flags;	/* flags indicating what modes
+						 * are supported for the port
+						 */
+};
+
+#define FM10K_VF_FLAG_ALLMULTI_CAPABLE	((u8)1 << FM10K_XCAST_MODE_ALLMULTI)
+#define FM10K_VF_FLAG_MULTI_CAPABLE	((u8)1 << FM10K_XCAST_MODE_MULTI)
+#define FM10K_VF_FLAG_PROMISC_CAPABLE	((u8)1 << FM10K_XCAST_MODE_PROMISC)
+#define FM10K_VF_FLAG_NONE_CAPABLE	((u8)1 << FM10K_XCAST_MODE_NONE)
+#define FM10K_VF_FLAG_CAPABLE(vf_info)	((vf_info)->vf_flags & (u8)0xF)
+#define FM10K_VF_FLAG_ENABLED(vf_info)	((vf_info)->vf_flags >> 4)
+#define FM10K_VF_FLAG_SET_MODE(mode)	((u8)0x10 << (mode))
+#define FM10K_VF_FLAG_SET_MODE_NONE \
+	FM10K_VF_FLAG_SET_MODE(FM10K_XCAST_MODE_NONE)
+#define FM10K_VF_FLAG_MULTI_ENABLED \
+	(FM10K_VF_FLAG_SET_MODE(FM10K_XCAST_MODE_ALLMULTI) | \
+	 FM10K_VF_FLAG_SET_MODE(FM10K_XCAST_MODE_MULTI) | \
+	 FM10K_VF_FLAG_SET_MODE(FM10K_XCAST_MODE_PROMISC))
+
+struct fm10k_iov_ops {
+	/* IOV related bring-up and tear-down */
+	s32 (*assign_resources)(struct fm10k_hw *, u16, u16);
+	s32 (*configure_tc)(struct fm10k_hw *, u16, int);
+	s32 (*assign_int_moderator)(struct fm10k_hw *, u16);
+	s32 (*assign_default_mac_vlan)(struct fm10k_hw *,
+				       struct fm10k_vf_info *);
+	s32 (*reset_resources)(struct fm10k_hw *,
+			       struct fm10k_vf_info *);
+	s32 (*set_lport)(struct fm10k_hw *, struct fm10k_vf_info *, u16, u8);
+	void (*reset_lport)(struct fm10k_hw *, struct fm10k_vf_info *);
+	void (*update_stats)(struct fm10k_hw *, struct fm10k_hw_stats_q *, u16);
+	s32 (*report_timestamp)(struct fm10k_hw *, struct fm10k_vf_info *, u64);
+};
+
+struct fm10k_iov_info {
+	struct fm10k_iov_ops ops;
+	u16 total_vfs;
+	u16 num_vfs;
+	u16 num_pools;
+};
+
+enum fm10k_devices {
+	fm10k_device_pf,
+	fm10k_device_vf,
+};
+
+struct fm10k_info {
+	enum fm10k_mac_type	mac;
+	s32			(*get_invariants)(struct fm10k_hw *);
+	struct fm10k_mac_ops	*mac_ops;
+	struct fm10k_iov_ops	*iov_ops;
+};
+
+struct fm10k_hw {
+	u32 __iomem *hw_addr;
+	u32 __iomem *sw_addr;
+	void *back;
+	struct fm10k_mac_info mac;
+	struct fm10k_bus_info bus;
+	struct fm10k_bus_info bus_caps;
+	struct fm10k_iov_info iov;
+	struct fm10k_mbx_info mbx;
+	struct fm10k_swapi_info swapi;
+	u16 device_id;
+	u16 vendor_id;
+	u16 subsystem_device_id;
+	u16 subsystem_vendor_id;
+	u8 revision_id;
+};
+
+/* Number of Transmit and Receive Descriptors must be a multiple of 8 */
+#define FM10K_REQ_TX_DESCRIPTOR_MULTIPLE	8
+#define FM10K_REQ_RX_DESCRIPTOR_MULTIPLE	8
+
+/* Transmit Descriptor */
+struct fm10k_tx_desc {
+	__le64 buffer_addr;	/* Address of the descriptor's data buffer */
+	__le16 buflen;		/* Length of data to be DMAed */
+	__le16 vlan;		/* VLAN_ID and VPRI to be inserted in FTAG */
+	__le16 mss;		/* MSS for segmentation offload */
+	u8 hdrlen;		/* Header size for segmentation offload */
+	u8 flags;		/* Status and offload request flags */
+};
+
+/* Transmit Descriptor Cache Structure */
+struct fm10k_tx_desc_cache {
+	struct fm10k_tx_desc tx_desc[256];
+};
+
+#define FM10K_TXD_FLAG_INT	0x01
+#define FM10K_TXD_FLAG_TIME	0x02
+#define FM10K_TXD_FLAG_CSUM	0x04
+#define FM10K_TXD_FLAG_FTAG	0x10
+#define FM10K_TXD_FLAG_RS	0x20
+#define FM10K_TXD_FLAG_LAST	0x40
+#define FM10K_TXD_FLAG_DONE	0x80
+
+/* These macros are meant to enable optimal placement of the RS and INT
+ * bits.  It will point us to the last descriptor in the cache for either the
+ * start of the packet, or the end of the packet.  If the index is actually
+ * at the start of the FIFO it will point to the offset for the last index
+ * in the FIFO to prevent an unnecessary write.
+ */
+#define FM10K_TXD_WB_FIFO_SIZE	4
+
+/* Receive Descriptor - 32B */
+union fm10k_rx_desc {
+	struct {
+		__le64 pkt_addr; /* Packet buffer address */
+		__le64 hdr_addr; /* Header buffer address */
+		__le64 reserved; /* Empty space, RSS hash */
+		__le64 timestamp;
+	} q; /* Read, Writeback, 64b quad-words */
+	struct {
+		__le32 data; /* RSS and header data */
+		__le32 rss;  /* RSS Hash */
+		__le32 staterr;
+		__le32 vlan_len;
+		__le32 glort; /* sglort/dglort */
+	} d; /* Writeback, 32b double-words */
+	struct {
+		__le16 pkt_info; /* RSS, Pkt type */
+		__le16 hdr_info; /* Splithdr, hdrlen, xC */
+		__le16 rss_lower;
+		__le16 rss_upper;
+		__le16 status; /* status/error */
+		__le16 csum_err; /* checksum or extended error value */
+		__le16 length; /* Packet length */
+		__le16 vlan; /* VLAN tag */
+		__le16 dglort;
+		__le16 sglort;
+	} w; /* Writeback, 16b words */
+};
+
+#define FM10K_RXD_RSSTYPE_MASK		0x000F
+enum fm10k_rdesc_rss_type {
+	FM10K_RSSTYPE_NONE	= 0x0,
+	FM10K_RSSTYPE_IPV4_TCP	= 0x1,
+	FM10K_RSSTYPE_IPV4	= 0x2,
+	FM10K_RSSTYPE_IPV6_TCP	= 0x3,
+	/* Reserved 0x4 */
+	FM10K_RSSTYPE_IPV6	= 0x5,
+	/* Reserved 0x6 */
+	FM10K_RSSTYPE_IPV4_UDP	= 0x7,
+	FM10K_RSSTYPE_IPV6_UDP	= 0x8
+	/* Reserved 0x9 - 0xF */
+};
+
+#define FM10K_RXD_HDR_INFO_XC_MASK	0x0006
+enum fm10k_rxdesc_xc {
+	FM10K_XC_UNICAST	= 0x0,
+	FM10K_XC_MULTICAST	= 0x4,
+	FM10K_XC_BROADCAST	= 0x6
+};
+
+#define FM10K_RXD_STATUS_DD		0x0001 /* Descriptor done */
+#define FM10K_RXD_STATUS_EOP		0x0002 /* End of packet */
+#define FM10K_RXD_STATUS_L4CS		0x0010 /* Indicates an L4 csum */
+#define FM10K_RXD_STATUS_L4CS2		0x0040 /* Inner header L4 csum */
+#define FM10K_RXD_STATUS_L4E2		0x0800 /* Inner header L4 csum err */
+#define FM10K_RXD_STATUS_IPE2		0x1000 /* Inner header IPv4 csum err */
+#define FM10K_RXD_STATUS_RXE		0x2000 /* Generic Rx error */
+#define FM10K_RXD_STATUS_L4E		0x4000 /* L4 csum error */
+#define FM10K_RXD_STATUS_IPE		0x8000 /* IPv4 csum error */
+
+struct fm10k_ftag {
+	__be16 swpri_type_user;
+	__be16 vlan;
+	__be16 sglort;
+	__be16 dglort;
+};
+
+#endif /* _FM10K_TYPE_H */
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_vf.c b/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
new file mode 100644
index 000000000000..f0aa0f97b4a9
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_vf.c
@@ -0,0 +1,578 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#include "fm10k_vf.h"
+
+/**
+ *  fm10k_stop_hw_vf - Stop Tx/Rx units
+ *  @hw: pointer to hardware structure
+ *
+ **/
+static s32 fm10k_stop_hw_vf(struct fm10k_hw *hw)
+{
+	u8 *perm_addr = hw->mac.perm_addr;
+	u32 bal = 0, bah = 0;
+	s32 err;
+	u16 i;
+
+	/* we need to disable the queues before taking further steps */
+	err = fm10k_stop_hw_generic(hw);
+	if (err)
+		return err;
+
+	/* If permenant address is set then we need to restore it */
+	if (is_valid_ether_addr(perm_addr)) {
+		bal = (((u32)perm_addr[3]) << 24) |
+		      (((u32)perm_addr[4]) << 16) |
+		      (((u32)perm_addr[5]) << 8);
+		bah = (((u32)0xFF)	   << 24) |
+		      (((u32)perm_addr[0]) << 16) |
+		      (((u32)perm_addr[1]) << 8) |
+		       ((u32)perm_addr[2]);
+	}
+
+	/* The queues have already been disabled so we just need to
+	 * update their base address registers
+	 */
+	for (i = 0; i < hw->mac.max_queues; i++) {
+		fm10k_write_reg(hw, FM10K_TDBAL(i), bal);
+		fm10k_write_reg(hw, FM10K_TDBAH(i), bah);
+		fm10k_write_reg(hw, FM10K_RDBAL(i), bal);
+		fm10k_write_reg(hw, FM10K_RDBAH(i), bah);
+	}
+
+	return 0;
+}
+
+/**
+ *  fm10k_reset_hw_vf - VF hardware reset
+ *  @hw: pointer to hardware structure
+ *
+ *  This function should return the hardare to a state similar to the
+ *  one it is in after just being initialized.
+ **/
+static s32 fm10k_reset_hw_vf(struct fm10k_hw *hw)
+{
+	s32 err;
+
+	/* shut down queues we own and reset DMA configuration */
+	err = fm10k_stop_hw_vf(hw);
+	if (err)
+		return err;
+
+	/* Inititate VF reset */
+	fm10k_write_reg(hw, FM10K_VFCTRL, FM10K_VFCTRL_RST);
+
+	/* Flush write and allow 100us for reset to complete */
+	fm10k_write_flush(hw);
+	udelay(FM10K_RESET_TIMEOUT);
+
+	/* Clear reset bit and verify it was cleared */
+	fm10k_write_reg(hw, FM10K_VFCTRL, 0);
+	if (fm10k_read_reg(hw, FM10K_VFCTRL) & FM10K_VFCTRL_RST)
+		err = FM10K_ERR_RESET_FAILED;
+
+	return err;
+}
+
+/**
+ *  fm10k_init_hw_vf - VF hardware initialization
+ *  @hw: pointer to hardware structure
+ *
+ **/
+static s32 fm10k_init_hw_vf(struct fm10k_hw *hw)
+{
+	u32 tqdloc, tqdloc0 = ~fm10k_read_reg(hw, FM10K_TQDLOC(0));
+	s32 err;
+	u16 i;
+
+	/* assume we always have at least 1 queue */
+	for (i = 1; tqdloc0 && (i < FM10K_MAX_QUEUES_POOL); i++) {
+		/* verify the Descriptor cache offsets are increasing */
+		tqdloc = ~fm10k_read_reg(hw, FM10K_TQDLOC(i));
+		if (!tqdloc || (tqdloc == tqdloc0))
+			break;
+
+		/* check to verify the PF doesn't own any of our queues */
+		if (!~fm10k_read_reg(hw, FM10K_TXQCTL(i)) ||
+		    !~fm10k_read_reg(hw, FM10K_RXQCTL(i)))
+			break;
+	}
+
+	/* shut down queues we own and reset DMA configuration */
+	err = fm10k_disable_queues_generic(hw, i);
+	if (err)
+		return err;
+
+	/* record maximum queue count */
+	hw->mac.max_queues = i;
+
+	return 0;
+}
+
+/**
+ *  fm10k_is_slot_appropriate_vf - Indicate appropriate slot for this SKU
+ *  @hw: pointer to hardware structure
+ *
+ *  Looks at the PCIe bus info to confirm whether or not this slot can support
+ *  the necessary bandwidth for this device. Since the VF has no control over
+ *  the "slot" it is in, always indicate that the slot is appropriate.
+ **/
+static bool fm10k_is_slot_appropriate_vf(struct fm10k_hw *hw)
+{
+	return true;
+}
+
+/* This structure defines the attibutes to be parsed below */
+const struct fm10k_tlv_attr fm10k_mac_vlan_msg_attr[] = {
+	FM10K_TLV_ATTR_U32(FM10K_MAC_VLAN_MSG_VLAN),
+	FM10K_TLV_ATTR_BOOL(FM10K_MAC_VLAN_MSG_SET),
+	FM10K_TLV_ATTR_MAC_ADDR(FM10K_MAC_VLAN_MSG_MAC),
+	FM10K_TLV_ATTR_MAC_ADDR(FM10K_MAC_VLAN_MSG_DEFAULT_MAC),
+	FM10K_TLV_ATTR_MAC_ADDR(FM10K_MAC_VLAN_MSG_MULTICAST),
+	FM10K_TLV_ATTR_LAST
+};
+
+/**
+ *  fm10k_update_vlan_vf - Update status of VLAN ID in VLAN filter table
+ *  @hw: pointer to hardware structure
+ *  @vid: VLAN ID to add to table
+ *  @vsi: Reserved, should always be 0
+ *  @set: Indicates if this is a set or clear operation
+ *
+ *  This function adds or removes the corresponding VLAN ID from the VLAN
+ *  filter table for this VF.
+ **/
+static s32 fm10k_update_vlan_vf(struct fm10k_hw *hw, u32 vid, u8 vsi, bool set)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[4];
+
+	/* verify the index is not set */
+	if (vsi)
+		return FM10K_ERR_PARAM;
+
+	/* verify upper 4 bits of vid and length are 0 */
+	if ((vid << 16 | vid) >> 28)
+		return FM10K_ERR_PARAM;
+
+	/* encode set bit into the VLAN ID */
+	if (!set)
+		vid |= FM10K_VLAN_CLEAR;
+
+	/* generate VLAN request */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_MAC_VLAN);
+	fm10k_tlv_attr_put_u32(msg, FM10K_MAC_VLAN_MSG_VLAN, vid);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_msg_mac_vlan_vf - Read device MAC address from mailbox message
+ *  @hw: pointer to the HW structure
+ *  @results: Attributes for message
+ *  @mbx: unused mailbox data
+ *
+ *  This function should determine the MAC address for the VF
+ **/
+s32 fm10k_msg_mac_vlan_vf(struct fm10k_hw *hw, u32 **results,
+			  struct fm10k_mbx_info *mbx)
+{
+	u8 perm_addr[ETH_ALEN];
+	u16 vid;
+	s32 err;
+
+	/* record MAC address requested */
+	err = fm10k_tlv_attr_get_mac_vlan(
+					results[FM10K_MAC_VLAN_MSG_DEFAULT_MAC],
+					perm_addr, &vid);
+	if (err)
+		return err;
+
+	ether_addr_copy(hw->mac.perm_addr, perm_addr);
+	hw->mac.default_vid = vid & (FM10K_VLAN_TABLE_VID_MAX - 1);
+	hw->mac.vlan_override = !!(vid & FM10K_VLAN_CLEAR);
+
+	return 0;
+}
+
+/**
+ *  fm10k_read_mac_addr_vf - Read device MAC address
+ *  @hw: pointer to the HW structure
+ *
+ *  This function should determine the MAC address for the VF
+ **/
+static s32 fm10k_read_mac_addr_vf(struct fm10k_hw *hw)
+{
+	u8 perm_addr[ETH_ALEN];
+	u32 base_addr;
+
+	base_addr = fm10k_read_reg(hw, FM10K_TDBAL(0));
+
+	/* last byte should be 0 */
+	if (base_addr << 24)
+		return  FM10K_ERR_INVALID_MAC_ADDR;
+
+	perm_addr[3] = (u8)(base_addr >> 24);
+	perm_addr[4] = (u8)(base_addr >> 16);
+	perm_addr[5] = (u8)(base_addr >> 8);
+
+	base_addr = fm10k_read_reg(hw, FM10K_TDBAH(0));
+
+	/* first byte should be all 1's */
+	if ((~base_addr) >> 24)
+		return  FM10K_ERR_INVALID_MAC_ADDR;
+
+	perm_addr[0] = (u8)(base_addr >> 16);
+	perm_addr[1] = (u8)(base_addr >> 8);
+	perm_addr[2] = (u8)(base_addr);
+
+	ether_addr_copy(hw->mac.perm_addr, perm_addr);
+	ether_addr_copy(hw->mac.addr, perm_addr);
+
+	return 0;
+}
+
+/**
+ *  fm10k_update_uc_addr_vf - Update device unicast address
+ *  @hw: pointer to the HW structure
+ *  @glort: unused
+ *  @mac: MAC address to add/remove from table
+ *  @vid: VLAN ID to add/remove from table
+ *  @add: Indicates if this is an add or remove operation
+ *  @flags: flags field to indicate add and secure - unused
+ *
+ *  This function is used to add or remove unicast MAC addresses for
+ *  the VF.
+ **/
+static s32 fm10k_update_uc_addr_vf(struct fm10k_hw *hw, u16 glort,
+				   const u8 *mac, u16 vid, bool add, u8 flags)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[7];
+
+	/* verify VLAN ID is valid */
+	if (vid >= FM10K_VLAN_TABLE_VID_MAX)
+		return FM10K_ERR_PARAM;
+
+	/* verify MAC address is valid */
+	if (!is_valid_ether_addr(mac))
+		return FM10K_ERR_PARAM;
+
+	/* verify we are not locked down on the MAC address */
+	if (is_valid_ether_addr(hw->mac.perm_addr) &&
+	    memcmp(hw->mac.perm_addr, mac, ETH_ALEN))
+		return FM10K_ERR_PARAM;
+
+	/* add bit to notify us if this is a set of clear operation */
+	if (!add)
+		vid |= FM10K_VLAN_CLEAR;
+
+	/* generate VLAN request */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_MAC_VLAN);
+	fm10k_tlv_attr_put_mac_vlan(msg, FM10K_MAC_VLAN_MSG_MAC, mac, vid);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_update_mc_addr_vf - Update device multicast address
+ *  @hw: pointer to the HW structure
+ *  @glort: unused
+ *  @mac: MAC address to add/remove from table
+ *  @vid: VLAN ID to add/remove from table
+ *  @add: Indicates if this is an add or remove operation
+ *
+ *  This function is used to add or remove multicast MAC addresses for
+ *  the VF.
+ **/
+static s32 fm10k_update_mc_addr_vf(struct fm10k_hw *hw, u16 glort,
+				   const u8 *mac, u16 vid, bool add)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[7];
+
+	/* verify VLAN ID is valid */
+	if (vid >= FM10K_VLAN_TABLE_VID_MAX)
+		return FM10K_ERR_PARAM;
+
+	/* verify multicast address is valid */
+	if (!is_multicast_ether_addr(mac))
+		return FM10K_ERR_PARAM;
+
+	/* add bit to notify us if this is a set of clear operation */
+	if (!add)
+		vid |= FM10K_VLAN_CLEAR;
+
+	/* generate VLAN request */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_MAC_VLAN);
+	fm10k_tlv_attr_put_mac_vlan(msg, FM10K_MAC_VLAN_MSG_MULTICAST,
+				    mac, vid);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_update_int_moderator_vf - Request update of interrupt moderator list
+ *  @hw: pointer to hardware structure
+ *
+ *  This function will issue a request to the PF to rescan our MSI-X table
+ *  and to update the interrupt moderator linked list.
+ **/
+static void fm10k_update_int_moderator_vf(struct fm10k_hw *hw)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[1];
+
+	/* generate MSI-X request */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_MSIX);
+
+	/* load onto outgoing mailbox */
+	mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/* This structure defines the attibutes to be parsed below */
+const struct fm10k_tlv_attr fm10k_lport_state_msg_attr[] = {
+	FM10K_TLV_ATTR_BOOL(FM10K_LPORT_STATE_MSG_DISABLE),
+	FM10K_TLV_ATTR_U8(FM10K_LPORT_STATE_MSG_XCAST_MODE),
+	FM10K_TLV_ATTR_BOOL(FM10K_LPORT_STATE_MSG_READY),
+	FM10K_TLV_ATTR_LAST
+};
+
+/**
+ *  fm10k_msg_lport_state_vf - Message handler for lport_state message from PF
+ *  @hw: Pointer to hardware structure
+ *  @results: pointer array containing parsed data
+ *  @mbx: Pointer to mailbox information structure
+ *
+ *  This handler is meant to capture the indication from the PF that we
+ *  are ready to bring up the interface.
+ **/
+s32 fm10k_msg_lport_state_vf(struct fm10k_hw *hw, u32 **results,
+			     struct fm10k_mbx_info *mbx)
+{
+	hw->mac.dglort_map = !results[FM10K_LPORT_STATE_MSG_READY] ?
+			     FM10K_DGLORTMAP_NONE : FM10K_DGLORTMAP_ZERO;
+
+	return 0;
+}
+
+/**
+ *  fm10k_update_lport_state_vf - Update device state in lower device
+ *  @hw: pointer to the HW structure
+ *  @glort: unused
+ *  @count: number of logical ports to enable - unused (always 1)
+ *  @enable: boolean value indicating if this is an enable or disable request
+ *
+ *  Notify the lower device of a state change.  If the lower device is
+ *  enabled we can add filters, if it is disabled all filters for this
+ *  logical port are flushed.
+ **/
+static s32 fm10k_update_lport_state_vf(struct fm10k_hw *hw, u16 glort,
+				       u16 count, bool enable)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[2];
+
+	/* reset glort mask 0 as we have to wait to be enabled */
+	hw->mac.dglort_map = FM10K_DGLORTMAP_NONE;
+
+	/* generate port state request */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_LPORT_STATE);
+	if (!enable)
+		fm10k_tlv_attr_put_bool(msg, FM10K_LPORT_STATE_MSG_DISABLE);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+/**
+ *  fm10k_update_xcast_mode_vf - Request update of multicast mode
+ *  @hw: pointer to hardware structure
+ *  @glort: unused
+ *  @mode: integer value indicating mode being requested
+ *
+ *  This function will attempt to request a higher mode for the port
+ *  so that it can enable either multicast, multicast promiscuous, or
+ *  promiscuous mode of operation.
+ **/
+static s32 fm10k_update_xcast_mode_vf(struct fm10k_hw *hw, u16 glort, u8 mode)
+{
+	struct fm10k_mbx_info *mbx = &hw->mbx;
+	u32 msg[3];
+
+	if (mode > FM10K_XCAST_MODE_NONE)
+		return FM10K_ERR_PARAM;
+	/* generate message requesting to change xcast mode */
+	fm10k_tlv_msg_init(msg, FM10K_VF_MSG_ID_LPORT_STATE);
+	fm10k_tlv_attr_put_u8(msg, FM10K_LPORT_STATE_MSG_XCAST_MODE, mode);
+
+	/* load onto outgoing mailbox */
+	return mbx->ops.enqueue_tx(hw, mbx, msg);
+}
+
+const struct fm10k_tlv_attr fm10k_1588_msg_attr[] = {
+	FM10K_TLV_ATTR_U64(FM10K_1588_MSG_TIMESTAMP),
+	FM10K_TLV_ATTR_LAST
+};
+
+/* currently there is no shared 1588 timestamp handler */
+
+/**
+ *  fm10k_update_hw_stats_vf - Updates hardware related statistics of VF
+ *  @hw: pointer to hardware structure
+ *  @stats: pointer to statistics structure
+ *
+ *  This function collects and aggregates per queue hardware statistics.
+ **/
+static void fm10k_update_hw_stats_vf(struct fm10k_hw *hw,
+				     struct fm10k_hw_stats *stats)
+{
+	fm10k_update_hw_stats_q(hw, stats->q, 0, hw->mac.max_queues);
+}
+
+/**
+ *  fm10k_rebind_hw_stats_vf - Resets base for hardware statistics of VF
+ *  @hw: pointer to hardware structure
+ *  @stats: pointer to the stats structure to update
+ *
+ *  This function resets the base for queue hardware statistics.
+ **/
+static void fm10k_rebind_hw_stats_vf(struct fm10k_hw *hw,
+				     struct fm10k_hw_stats *stats)
+{
+	/* Unbind Queue Statistics */
+	fm10k_unbind_hw_stats_q(stats->q, 0, hw->mac.max_queues);
+
+	/* Reinitialize bases for all stats */
+	fm10k_update_hw_stats_vf(hw, stats);
+}
+
+/**
+ *  fm10k_configure_dglort_map_vf - Configures GLORT entry and queues
+ *  @hw: pointer to hardware structure
+ *  @dglort: pointer to dglort configuration structure
+ *
+ *  Reads the configuration structure contained in dglort_cfg and uses
+ *  that information to then populate a DGLORTMAP/DEC entry and the queues
+ *  to which it has been assigned.
+ **/
+static s32 fm10k_configure_dglort_map_vf(struct fm10k_hw *hw,
+					 struct fm10k_dglort_cfg *dglort)
+{
+	/* verify the dglort pointer */
+	if (!dglort)
+		return FM10K_ERR_PARAM;
+
+	/* stub for now until we determine correct message for this */
+
+	return 0;
+}
+
+/**
+ *  fm10k_adjust_systime_vf - Adjust systime frequency
+ *  @hw: pointer to hardware structure
+ *  @ppb: adjustment rate in parts per billion
+ *
+ *  This function takes an adjustment rate in parts per billion and will
+ *  verify that this value is 0 as the VF cannot support adjusting the
+ *  systime clock.
+ *
+ *  If the ppb value is non-zero the return is ERR_PARAM else success
+ **/
+static s32 fm10k_adjust_systime_vf(struct fm10k_hw *hw, s32 ppb)
+{
+	/* The VF cannot adjust the clock frequency, however it should
+	 * already have a syntonic clock with whichever host interface is
+	 * running as the master for the host interface clock domain so
+	 * there should be not frequency adjustment necessary.
+	 */
+	return ppb ? FM10K_ERR_PARAM : 0;
+}
+
+/**
+ *  fm10k_read_systime_vf - Reads value of systime registers
+ *  @hw: pointer to the hardware structure
+ *
+ *  Function reads the content of 2 registers, combined to represent a 64 bit
+ *  value measured in nanosecods.  In order to guarantee the value is accurate
+ *  we check the 32 most significant bits both before and after reading the
+ *  32 least significant bits to verify they didn't change as we were reading
+ *  the registers.
+ **/
+static u64 fm10k_read_systime_vf(struct fm10k_hw *hw)
+{
+	u32 systime_l, systime_h, systime_tmp;
+
+	systime_h = fm10k_read_reg(hw, FM10K_VFSYSTIME + 1);
+
+	do {
+		systime_tmp = systime_h;
+		systime_l = fm10k_read_reg(hw, FM10K_VFSYSTIME);
+		systime_h = fm10k_read_reg(hw, FM10K_VFSYSTIME + 1);
+	} while (systime_tmp != systime_h);
+
+	return ((u64)systime_h << 32) | systime_l;
+}
+
+static const struct fm10k_msg_data fm10k_msg_data_vf[] = {
+	FM10K_TLV_MSG_TEST_HANDLER(fm10k_tlv_msg_test),
+	FM10K_VF_MSG_MAC_VLAN_HANDLER(fm10k_msg_mac_vlan_vf),
+	FM10K_VF_MSG_LPORT_STATE_HANDLER(fm10k_msg_lport_state_vf),
+	FM10K_TLV_MSG_ERROR_HANDLER(fm10k_tlv_msg_error),
+};
+
+static struct fm10k_mac_ops mac_ops_vf = {
+	.get_bus_info		= &fm10k_get_bus_info_generic,
+	.reset_hw		= &fm10k_reset_hw_vf,
+	.init_hw		= &fm10k_init_hw_vf,
+	.start_hw		= &fm10k_start_hw_generic,
+	.stop_hw		= &fm10k_stop_hw_vf,
+	.is_slot_appropriate	= &fm10k_is_slot_appropriate_vf,
+	.update_vlan		= &fm10k_update_vlan_vf,
+	.read_mac_addr		= &fm10k_read_mac_addr_vf,
+	.update_uc_addr		= &fm10k_update_uc_addr_vf,
+	.update_mc_addr		= &fm10k_update_mc_addr_vf,
+	.update_xcast_mode	= &fm10k_update_xcast_mode_vf,
+	.update_int_moderator	= &fm10k_update_int_moderator_vf,
+	.update_lport_state	= &fm10k_update_lport_state_vf,
+	.update_hw_stats	= &fm10k_update_hw_stats_vf,
+	.rebind_hw_stats	= &fm10k_rebind_hw_stats_vf,
+	.configure_dglort_map	= &fm10k_configure_dglort_map_vf,
+	.get_host_state		= &fm10k_get_host_state_generic,
+	.adjust_systime		= &fm10k_adjust_systime_vf,
+	.read_systime		= &fm10k_read_systime_vf,
+};
+
+static s32 fm10k_get_invariants_vf(struct fm10k_hw *hw)
+{
+	fm10k_get_invariants_generic(hw);
+
+	return fm10k_pfvf_mbx_init(hw, &hw->mbx, fm10k_msg_data_vf, 0);
+}
+
+struct fm10k_info fm10k_vf_info = {
+	.mac		= fm10k_mac_vf,
+	.get_invariants	= &fm10k_get_invariants_vf,
+	.mac_ops	= &mac_ops_vf,
+};
diff --git a/drivers/net/ethernet/intel/fm10k/fm10k_vf.h b/drivers/net/ethernet/intel/fm10k/fm10k_vf.h
new file mode 100644
index 000000000000..06a99d794c99
--- /dev/null
+++ b/drivers/net/ethernet/intel/fm10k/fm10k_vf.h
@@ -0,0 +1,78 @@
+/* Intel Ethernet Switch Host Interface Driver
+ * Copyright(c) 2013 - 2014 Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in
+ * the file called "COPYING".
+ *
+ * Contact Information:
+ * e1000-devel Mailing List <e1000-devel@lists.sourceforge.net>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ */
+
+#ifndef _FM10K_VF_H_
+#define _FM10K_VF_H_
+
+#include "fm10k_type.h"
+#include "fm10k_common.h"
+
+enum fm10k_vf_tlv_msg_id {
+	FM10K_VF_MSG_ID_TEST = 0,	/* msg ID reserved for testing */
+	FM10K_VF_MSG_ID_MSIX,
+	FM10K_VF_MSG_ID_MAC_VLAN,
+	FM10K_VF_MSG_ID_LPORT_STATE,
+	FM10K_VF_MSG_ID_1588,
+	FM10K_VF_MSG_ID_MAX,
+};
+
+enum fm10k_tlv_mac_vlan_attr_id {
+	FM10K_MAC_VLAN_MSG_VLAN,
+	FM10K_MAC_VLAN_MSG_SET,
+	FM10K_MAC_VLAN_MSG_MAC,
+	FM10K_MAC_VLAN_MSG_DEFAULT_MAC,
+	FM10K_MAC_VLAN_MSG_MULTICAST,
+	FM10K_MAC_VLAN_MSG_ID_MAX
+};
+
+enum fm10k_tlv_lport_state_attr_id {
+	FM10K_LPORT_STATE_MSG_DISABLE,
+	FM10K_LPORT_STATE_MSG_XCAST_MODE,
+	FM10K_LPORT_STATE_MSG_READY,
+	FM10K_LPORT_STATE_MSG_MAX
+};
+
+enum fm10k_tlv_1588_attr_id {
+	FM10K_1588_MSG_TIMESTAMP,
+	FM10K_1588_MSG_MAX
+};
+
+#define FM10K_VF_MSG_MSIX_HANDLER(func) \
+	 FM10K_MSG_HANDLER(FM10K_VF_MSG_ID_MSIX, NULL, func)
+
+s32 fm10k_msg_mac_vlan_vf(struct fm10k_hw *, u32 **, struct fm10k_mbx_info *);
+extern const struct fm10k_tlv_attr fm10k_mac_vlan_msg_attr[];
+#define FM10K_VF_MSG_MAC_VLAN_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_VF_MSG_ID_MAC_VLAN, \
+			  fm10k_mac_vlan_msg_attr, func)
+
+s32 fm10k_msg_lport_state_vf(struct fm10k_hw *, u32 **,
+			     struct fm10k_mbx_info *);
+extern const struct fm10k_tlv_attr fm10k_lport_state_msg_attr[];
+#define FM10K_VF_MSG_LPORT_STATE_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_VF_MSG_ID_LPORT_STATE, \
+			  fm10k_lport_state_msg_attr, func)
+
+extern const struct fm10k_tlv_attr fm10k_1588_msg_attr[];
+#define FM10K_VF_MSG_1588_HANDLER(func) \
+	FM10K_MSG_HANDLER(FM10K_VF_MSG_ID_1588, fm10k_1588_msg_attr, func)
+
+extern struct fm10k_info fm10k_vf_info;
+#endif /* _FM10K_VF_H */
diff --git a/drivers/net/ethernet/intel/i40e/i40e.h b/drivers/net/ethernet/intel/i40e/i40e.h
index 801da392a20e..f1e33f896439 100644
--- a/drivers/net/ethernet/intel/i40e/i40e.h
+++ b/drivers/net/ethernet/intel/i40e/i40e.h
@@ -144,6 +144,8 @@ enum i40e_state_t {
 	__I40E_PTP_TX_IN_PROGRESS,
 	__I40E_BAD_EEPROM,
 	__I40E_DOWN_REQUESTED,
+	__I40E_FD_FLUSH_REQUESTED,
+	__I40E_RESET_FAILED,
 };
 
 enum i40e_interrupt_policy {
@@ -250,6 +252,11 @@ struct i40e_pf {
 	u16 fdir_pf_active_filters;
 	u16 fd_sb_cnt_idx;
 	u16 fd_atr_cnt_idx;
+	unsigned long fd_flush_timestamp;
+	u32 fd_flush_cnt;
+	u32 fd_add_err;
+	u32 fd_atr_cnt;
+	u32 fd_tcp_rule;
 
 #ifdef CONFIG_I40E_VXLAN
 	__be16  vxlan_ports[I40E_MAX_PF_UDP_OFFLOAD_PORTS];
@@ -310,6 +317,7 @@ struct i40e_pf {
 	u32 tx_timeout_count;
 	u32 tx_timeout_recovery_level;
 	unsigned long tx_timeout_last_recovery;
+	u32 tx_sluggish_count;
 	u32 hw_csum_rx_error;
 	u32 led_status;
 	u16 corer_count; /* Core reset count */
@@ -608,6 +616,7 @@ int i40e_add_del_fdir(struct i40e_vsi *vsi,
 void i40e_fdir_check_and_reenable(struct i40e_pf *pf);
 int i40e_get_current_fd_count(struct i40e_pf *pf);
 int i40e_get_cur_guaranteed_fd_count(struct i40e_pf *pf);
+int i40e_get_current_atr_cnt(struct i40e_pf *pf);
 bool i40e_set_ntuple(struct i40e_pf *pf, netdev_features_t features);
 void i40e_set_ethtool_ops(struct net_device *netdev);
 struct i40e_mac_filter *i40e_add_filter(struct i40e_vsi *vsi,
diff --git a/drivers/net/ethernet/intel/i40e/i40e_adminq.c b/drivers/net/ethernet/intel/i40e/i40e_adminq.c
index b29c157b1f57..72f5d25a222f 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_adminq.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_adminq.c
@@ -840,7 +840,8 @@ i40e_status i40e_asq_send_command(struct i40e_hw *hw,
 
 	/* bump the tail */
 	i40e_debug(hw, I40E_DEBUG_AQ_MESSAGE, "AQTX: desc and buffer:\n");
-	i40e_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc_on_ring, buff);
+	i40e_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc_on_ring,
+		      buff, buff_size);
 	(hw->aq.asq.next_to_use)++;
 	if (hw->aq.asq.next_to_use == hw->aq.asq.count)
 		hw->aq.asq.next_to_use = 0;
@@ -891,7 +892,7 @@ i40e_status i40e_asq_send_command(struct i40e_hw *hw,
 
 	i40e_debug(hw, I40E_DEBUG_AQ_MESSAGE,
 		   "AQTX: desc and buffer writeback:\n");
-	i40e_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, buff);
+	i40e_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, buff, buff_size);
 
 	/* update the error if time out occurred */
 	if ((!cmd_completed) &&
@@ -987,7 +988,8 @@ i40e_status i40e_clean_arq_element(struct i40e_hw *hw,
 		       e->msg_size);
 
 	i40e_debug(hw, I40E_DEBUG_AQ_MESSAGE, "AQRX: desc and buffer:\n");
-	i40e_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, e->msg_buf);
+	i40e_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, e->msg_buf,
+		      hw->aq.arq_buf_size);
 
 	/* Restore the original datalen and buffer address in the desc,
 	 * FW updates datalen to indicate the event message
diff --git a/drivers/net/ethernet/intel/i40e/i40e_common.c b/drivers/net/ethernet/intel/i40e/i40e_common.c
index df43e7c6777c..30056b25d94e 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_common.c
@@ -75,13 +75,15 @@ static i40e_status i40e_set_mac_type(struct i40e_hw *hw)
  * @mask: debug mask
  * @desc: pointer to admin queue descriptor
  * @buffer: pointer to command buffer
+ * @buf_len: max length of buffer
  *
  * Dumps debug log about adminq command with descriptor contents.
  **/
 void i40e_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
-		   void *buffer)
+		   void *buffer, u16 buf_len)
 {
 	struct i40e_aq_desc *aq_desc = (struct i40e_aq_desc *)desc;
+	u16 len = le16_to_cpu(aq_desc->datalen);
 	u8 *aq_buffer = (u8 *)buffer;
 	u32 data[4];
 	u32 i = 0;
@@ -105,7 +107,9 @@ void i40e_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
 	if ((buffer != NULL) && (aq_desc->datalen != 0)) {
 		memset(data, 0, sizeof(data));
 		i40e_debug(hw, mask, "AQ CMD Buffer:\n");
-		for (i = 0; i < le16_to_cpu(aq_desc->datalen); i++) {
+		if (buf_len < len)
+			len = buf_len;
+		for (i = 0; i < len; i++) {
 			data[((i % 16) / 4)] |=
 				((u32)aq_buffer[i]) << (8 * (i % 4));
 			if ((i % 16) == 15) {
@@ -748,6 +752,8 @@ static enum i40e_media_type i40e_get_media_type(struct i40e_hw *hw)
 	switch (hw->phy.link_info.phy_type) {
 	case I40E_PHY_TYPE_10GBASE_SR:
 	case I40E_PHY_TYPE_10GBASE_LR:
+	case I40E_PHY_TYPE_1000BASE_SX:
+	case I40E_PHY_TYPE_1000BASE_LX:
 	case I40E_PHY_TYPE_40GBASE_SR4:
 	case I40E_PHY_TYPE_40GBASE_LR4:
 		media = I40E_MEDIA_TYPE_FIBER;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
index 5a0cabeb35ed..7067f4b9159c 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_debugfs.c
@@ -1356,6 +1356,9 @@ static ssize_t i40e_dbg_command_write(struct file *filp,
 				 "emp reset count: %d\n", pf->empr_count);
 			dev_info(&pf->pdev->dev,
 				 "pf reset count: %d\n", pf->pfr_count);
+			dev_info(&pf->pdev->dev,
+				 "pf tx sluggish count: %d\n",
+				 pf->tx_sluggish_count);
 		} else if (strncmp(&cmd_buf[5], "port", 4) == 0) {
 			struct i40e_aqc_query_port_ets_config_resp *bw_data;
 			struct i40e_dcbx_config *cfg =
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
index e8ba7470700a..1dda467ae1ac 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ethtool.c
@@ -145,6 +145,7 @@ static struct i40e_stats i40e_gstrings_stats[] = {
 	I40E_PF_STAT("rx_jabber", stats.rx_jabber),
 	I40E_PF_STAT("VF_admin_queue_requests", vf_aq_requests),
 	I40E_PF_STAT("rx_hwtstamp_cleared", rx_hwtstamp_cleared),
+	I40E_PF_STAT("fdir_flush_cnt", fd_flush_cnt),
 	I40E_PF_STAT("fdir_atr_match", stats.fd_atr_match),
 	I40E_PF_STAT("fdir_sb_match", stats.fd_sb_match),
 
@@ -312,7 +313,10 @@ static int i40e_get_settings(struct net_device *netdev,
 		break;
 	case I40E_PHY_TYPE_10GBASE_SR:
 	case I40E_PHY_TYPE_10GBASE_LR:
+	case I40E_PHY_TYPE_1000BASE_SX:
+	case I40E_PHY_TYPE_1000BASE_LX:
 		ecmd->supported = SUPPORTED_10000baseT_Full;
+		ecmd->supported |= SUPPORTED_1000baseT_Full;
 		break;
 	case I40E_PHY_TYPE_10GBASE_CR1_CU:
 	case I40E_PHY_TYPE_10GBASE_CR1:
@@ -351,7 +355,8 @@ static int i40e_get_settings(struct net_device *netdev,
 		break;
 	default:
 		/* if we got here and link is up something bad is afoot */
-		WARN_ON(link_up);
+		netdev_info(netdev, "WARNING: Link is up but PHY type 0x%x is not recognized.\n",
+			    hw_link_info->phy_type);
 	}
 
 no_valid_phy_type:
@@ -461,7 +466,8 @@ static int i40e_set_settings(struct net_device *netdev,
 
 	if (hw->phy.media_type != I40E_MEDIA_TYPE_BASET &&
 	    hw->phy.media_type != I40E_MEDIA_TYPE_FIBER &&
-	    hw->phy.media_type != I40E_MEDIA_TYPE_BACKPLANE)
+	    hw->phy.media_type != I40E_MEDIA_TYPE_BACKPLANE &&
+	    hw->phy.link_info.link_info & I40E_AQ_LINK_UP)
 		return -EOPNOTSUPP;
 
 	/* get our own copy of the bits to check against */
@@ -492,11 +498,10 @@ static int i40e_set_settings(struct net_device *netdev,
 	if (status)
 		return -EAGAIN;
 
-	/* Copy link_speed and abilities to config in case they are not
+	/* Copy abilities to config in case autoneg is not
 	 * set below
 	 */
 	memset(&config, 0, sizeof(struct i40e_aq_set_phy_config));
-	config.link_speed = abilities.link_speed;
 	config.abilities = abilities.abilities;
 
 	/* Check autoneg */
@@ -533,42 +538,38 @@ static int i40e_set_settings(struct net_device *netdev,
 		return -EINVAL;
 
 	if (advertise & ADVERTISED_100baseT_Full)
-		if (!(abilities.link_speed & I40E_LINK_SPEED_100MB)) {
-			config.link_speed |= I40E_LINK_SPEED_100MB;
-			change = true;
-		}
+		config.link_speed |= I40E_LINK_SPEED_100MB;
 	if (advertise & ADVERTISED_1000baseT_Full ||
 	    advertise & ADVERTISED_1000baseKX_Full)
-		if (!(abilities.link_speed & I40E_LINK_SPEED_1GB)) {
-			config.link_speed |= I40E_LINK_SPEED_1GB;
-			change = true;
-		}
+		config.link_speed |= I40E_LINK_SPEED_1GB;
 	if (advertise & ADVERTISED_10000baseT_Full ||
 	    advertise & ADVERTISED_10000baseKX4_Full ||
 	    advertise & ADVERTISED_10000baseKR_Full)
-		if (!(abilities.link_speed & I40E_LINK_SPEED_10GB)) {
-			config.link_speed |= I40E_LINK_SPEED_10GB;
-			change = true;
-		}
+		config.link_speed |= I40E_LINK_SPEED_10GB;
 	if (advertise & ADVERTISED_40000baseKR4_Full ||
 	    advertise & ADVERTISED_40000baseCR4_Full ||
 	    advertise & ADVERTISED_40000baseSR4_Full ||
 	    advertise & ADVERTISED_40000baseLR4_Full)
-		if (!(abilities.link_speed & I40E_LINK_SPEED_40GB)) {
-			config.link_speed |= I40E_LINK_SPEED_40GB;
-			change = true;
-		}
+		config.link_speed |= I40E_LINK_SPEED_40GB;
 
-	if (change) {
+	if (change || (abilities.link_speed != config.link_speed)) {
 		/* copy over the rest of the abilities */
 		config.phy_type = abilities.phy_type;
 		config.eee_capability = abilities.eee_capability;
 		config.eeer = abilities.eeer_val;
 		config.low_power_ctrl = abilities.d3_lpan;
 
-		/* If link is up set link and an so changes take effect */
-		if (hw->phy.link_info.link_info & I40E_AQ_LINK_UP)
-			config.abilities |= I40E_AQ_PHY_ENABLE_ATOMIC_LINK;
+		/* set link and auto negotiation so changes take effect */
+		config.abilities |= I40E_AQ_PHY_ENABLE_ATOMIC_LINK;
+		/* If link is up put link down */
+		if (hw->phy.link_info.link_info & I40E_AQ_LINK_UP) {
+			/* Tell the OS link is going down, the link will go
+			 * back up when fw says it is ready asynchronously
+			 */
+			netdev_info(netdev, "PHY settings change requested, NIC Link is going down.\n");
+			netif_carrier_off(netdev);
+			netif_tx_stop_all_queues(netdev);
+		}
 
 		/* make the aq call */
 		status = i40e_aq_set_phy_config(hw, &config, NULL);
@@ -685,6 +686,13 @@ static int i40e_set_pauseparam(struct net_device *netdev,
 	else
 		 return -EINVAL;
 
+	/* Tell the OS link is going down, the link will go back up when fw
+	 * says it is ready asynchronously
+	 */
+	netdev_info(netdev, "Flow control settings change requested, NIC Link is going down.\n");
+	netif_carrier_off(netdev);
+	netif_tx_stop_all_queues(netdev);
+
 	/* Set the fc mode and only restart an if link is up*/
 	status = i40e_set_fc(hw, &aq_failures, link_up);
 
@@ -1977,6 +1985,13 @@ static int i40e_del_fdir_entry(struct i40e_vsi *vsi,
 	struct i40e_pf *pf = vsi->back;
 	int ret = 0;
 
+	if (test_bit(__I40E_RESET_RECOVERY_PENDING, &pf->state) ||
+	    test_bit(__I40E_RESET_INTR_RECEIVED, &pf->state))
+		return -EBUSY;
+
+	if (test_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state))
+		return -EBUSY;
+
 	ret = i40e_update_ethtool_fdir_entry(vsi, NULL, fsp->location, cmd);
 
 	i40e_fdir_check_and_reenable(pf);
@@ -2010,6 +2025,13 @@ static int i40e_add_fdir_ethtool(struct i40e_vsi *vsi,
 	if (pf->auto_disable_flags & I40E_FLAG_FD_SB_ENABLED)
 		return -ENOSPC;
 
+	if (test_bit(__I40E_RESET_RECOVERY_PENDING, &pf->state) ||
+	    test_bit(__I40E_RESET_INTR_RECEIVED, &pf->state))
+		return -EBUSY;
+
+	if (test_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state))
+		return -EBUSY;
+
 	fsp = (struct ethtool_rx_flow_spec *)&cmd->fs;
 
 	if (fsp->location >= (pf->hw.func_caps.fd_filters_best_effort +
diff --git a/drivers/net/ethernet/intel/i40e/i40e_main.c b/drivers/net/ethernet/intel/i40e/i40e_main.c
index eddec6ba095b..ed5f1c15fb0f 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_main.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_main.c
@@ -37,9 +37,9 @@ static const char i40e_driver_string[] =
 
 #define DRV_KERN "-k"
 
-#define DRV_VERSION_MAJOR 0
-#define DRV_VERSION_MINOR 4
-#define DRV_VERSION_BUILD 21
+#define DRV_VERSION_MAJOR 1
+#define DRV_VERSION_MINOR 0
+#define DRV_VERSION_BUILD 11
 #define DRV_VERSION __stringify(DRV_VERSION_MAJOR) "." \
 	     __stringify(DRV_VERSION_MINOR) "." \
 	     __stringify(DRV_VERSION_BUILD)    DRV_KERN
@@ -1239,8 +1239,11 @@ struct i40e_mac_filter *i40e_put_mac_in_vlan(struct i40e_vsi *vsi, u8 *macaddr,
  * i40e_rm_default_mac_filter - Remove the default MAC filter set by NVM
  * @vsi: the PF Main VSI - inappropriate for any other VSI
  * @macaddr: the MAC address
+ *
+ * Some older firmware configurations set up a default promiscuous VLAN
+ * filter that needs to be removed.
  **/
-static void i40e_rm_default_mac_filter(struct i40e_vsi *vsi, u8 *macaddr)
+static int i40e_rm_default_mac_filter(struct i40e_vsi *vsi, u8 *macaddr)
 {
 	struct i40e_aqc_remove_macvlan_element_data element;
 	struct i40e_pf *pf = vsi->back;
@@ -1248,15 +1251,18 @@ static void i40e_rm_default_mac_filter(struct i40e_vsi *vsi, u8 *macaddr)
 
 	/* Only appropriate for the PF main VSI */
 	if (vsi->type != I40E_VSI_MAIN)
-		return;
+		return -EINVAL;
 
+	memset(&element, 0, sizeof(element));
 	ether_addr_copy(element.mac_addr, macaddr);
 	element.vlan_tag = 0;
 	element.flags = I40E_AQC_MACVLAN_DEL_PERFECT_MATCH |
 			I40E_AQC_MACVLAN_DEL_IGNORE_VLAN;
 	aq_ret = i40e_aq_remove_macvlan(&pf->hw, vsi->seid, &element, 1, NULL);
 	if (aq_ret)
-		dev_err(&pf->pdev->dev, "Could not remove default MAC-VLAN\n");
+		return -ENOENT;
+
+	return 0;
 }
 
 /**
@@ -1385,18 +1391,30 @@ static int i40e_set_mac(struct net_device *netdev, void *p)
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
 	struct i40e_vsi *vsi = np->vsi;
+	struct i40e_pf *pf = vsi->back;
+	struct i40e_hw *hw = &pf->hw;
 	struct sockaddr *addr = p;
 	struct i40e_mac_filter *f;
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
-	netdev_info(netdev, "set mac address=%pM\n", addr->sa_data);
+	if (ether_addr_equal(netdev->dev_addr, addr->sa_data)) {
+		netdev_info(netdev, "already using mac address %pM\n",
+			    addr->sa_data);
+		return 0;
+	}
 
 	if (test_bit(__I40E_DOWN, &vsi->back->state) ||
 	    test_bit(__I40E_RESET_RECOVERY_PENDING, &vsi->back->state))
 		return -EADDRNOTAVAIL;
 
+	if (ether_addr_equal(hw->mac.addr, addr->sa_data))
+		netdev_info(netdev, "returning to hw mac address %pM\n",
+			    hw->mac.addr);
+	else
+		netdev_info(netdev, "set new mac address %pM\n", addr->sa_data);
+
 	if (vsi->type == I40E_VSI_MAIN) {
 		i40e_status ret;
 		ret = i40e_aq_mac_address_write(&vsi->back->hw,
@@ -1410,25 +1428,34 @@ static int i40e_set_mac(struct net_device *netdev, void *p)
 		}
 	}
 
-	f = i40e_find_mac(vsi, addr->sa_data, false, true);
-	if (!f) {
-		/* In order to be sure to not drop any packets, add the
-		 * new address first then delete the old one.
-		 */
-		f = i40e_add_filter(vsi, addr->sa_data, I40E_VLAN_ANY,
-				    false, false);
-		if (!f)
-			return -ENOMEM;
+	if (ether_addr_equal(netdev->dev_addr, hw->mac.addr)) {
+		struct i40e_aqc_remove_macvlan_element_data element;
 
-		i40e_sync_vsi_filters(vsi);
+		memset(&element, 0, sizeof(element));
+		ether_addr_copy(element.mac_addr, netdev->dev_addr);
+		element.flags = I40E_AQC_MACVLAN_DEL_PERFECT_MATCH;
+		i40e_aq_remove_macvlan(&pf->hw, vsi->seid, &element, 1, NULL);
+	} else {
 		i40e_del_filter(vsi, netdev->dev_addr, I40E_VLAN_ANY,
 				false, false);
-		i40e_sync_vsi_filters(vsi);
 	}
 
-	f->is_laa = true;
-	if (!ether_addr_equal(netdev->dev_addr, addr->sa_data))
-		ether_addr_copy(netdev->dev_addr, addr->sa_data);
+	if (ether_addr_equal(addr->sa_data, hw->mac.addr)) {
+		struct i40e_aqc_add_macvlan_element_data element;
+
+		memset(&element, 0, sizeof(element));
+		ether_addr_copy(element.mac_addr, hw->mac.addr);
+		element.flags = cpu_to_le16(I40E_AQC_MACVLAN_ADD_PERFECT_MATCH);
+		i40e_aq_add_macvlan(&pf->hw, vsi->seid, &element, 1, NULL);
+	} else {
+		f = i40e_add_filter(vsi, addr->sa_data, I40E_VLAN_ANY,
+				    false, false);
+		if (f)
+			f->is_laa = true;
+	}
+
+	i40e_sync_vsi_filters(vsi);
+	ether_addr_copy(netdev->dev_addr, addr->sa_data);
 
 	return 0;
 }
@@ -1796,9 +1823,8 @@ int i40e_sync_vsi_filters(struct i40e_vsi *vsi)
 		kfree(add_list);
 		add_list = NULL;
 
-		if (add_happened && (!aq_ret)) {
-			/* do nothing */;
-		} else if (add_happened && (aq_ret)) {
+		if (add_happened && aq_ret &&
+		    pf->hw.aq.asq_last_status != I40E_AQ_RC_EINVAL) {
 			dev_info(&pf->pdev->dev,
 				 "add filter failed, err %d, aq_err %d\n",
 				 aq_ret, pf->hw.aq.asq_last_status);
@@ -4480,11 +4506,26 @@ static int i40e_up_complete(struct i40e_vsi *vsi)
 		netif_carrier_on(vsi->netdev);
 	} else if (vsi->netdev) {
 		i40e_print_link_message(vsi, false);
+		/* need to check for qualified module here*/
+		if ((pf->hw.phy.link_info.link_info &
+			I40E_AQ_MEDIA_AVAILABLE) &&
+		    (!(pf->hw.phy.link_info.an_info &
+			I40E_AQ_QUALIFIED_MODULE)))
+			netdev_err(vsi->netdev,
+				   "the driver failed to link because an unqualified module was detected.");
 	}
 
 	/* replay FDIR SB filters */
-	if (vsi->type == I40E_VSI_FDIR)
+	if (vsi->type == I40E_VSI_FDIR) {
+		/* reset fd counters */
+		pf->fd_add_err = pf->fd_atr_cnt = 0;
+		if (pf->fd_tcp_rule > 0) {
+			pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
+			dev_info(&pf->pdev->dev, "Forcing ATR off, sideband rules for TCP/IPv4 exist\n");
+			pf->fd_tcp_rule = 0;
+		}
 		i40e_fdir_filter_restore(vsi);
+	}
 	i40e_service_event_schedule(pf);
 
 	return 0;
@@ -5125,6 +5166,7 @@ int i40e_get_current_fd_count(struct i40e_pf *pf)
 		      I40E_PFQF_FDSTAT_BEST_CNT_SHIFT);
 	return fcnt_prog;
 }
+
 /**
  * i40e_fdir_check_and_reenable - Function to reenabe FD ATR or SB if disabled
  * @pf: board private structure
@@ -5133,15 +5175,17 @@ void i40e_fdir_check_and_reenable(struct i40e_pf *pf)
 {
 	u32 fcnt_prog, fcnt_avail;
 
+	if (test_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state))
+		return;
+
 	/* Check if, FD SB or ATR was auto disabled and if there is enough room
 	 * to re-enable
 	 */
-	if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
-	    (pf->flags & I40E_FLAG_FD_SB_ENABLED))
-		return;
 	fcnt_prog = i40e_get_cur_guaranteed_fd_count(pf);
 	fcnt_avail = pf->fdir_pf_filter_count;
-	if (fcnt_prog < (fcnt_avail - I40E_FDIR_BUFFER_HEAD_ROOM)) {
+	if ((fcnt_prog < (fcnt_avail - I40E_FDIR_BUFFER_HEAD_ROOM)) ||
+	    (pf->fd_add_err == 0) ||
+	    (i40e_get_current_atr_cnt(pf) < pf->fd_atr_cnt)) {
 		if ((pf->flags & I40E_FLAG_FD_SB_ENABLED) &&
 		    (pf->auto_disable_flags & I40E_FLAG_FD_SB_ENABLED)) {
 			pf->auto_disable_flags &= ~I40E_FLAG_FD_SB_ENABLED;
@@ -5158,23 +5202,84 @@ void i40e_fdir_check_and_reenable(struct i40e_pf *pf)
 	}
 }
 
+#define I40E_MIN_FD_FLUSH_INTERVAL 10
+/**
+ * i40e_fdir_flush_and_replay - Function to flush all FD filters and replay SB
+ * @pf: board private structure
+ **/
+static void i40e_fdir_flush_and_replay(struct i40e_pf *pf)
+{
+	int flush_wait_retry = 50;
+	int reg;
+
+	if (time_after(jiffies, pf->fd_flush_timestamp +
+				(I40E_MIN_FD_FLUSH_INTERVAL * HZ))) {
+		set_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
+		pf->fd_flush_timestamp = jiffies;
+		pf->auto_disable_flags |= I40E_FLAG_FD_SB_ENABLED;
+		pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
+		/* flush all filters */
+		wr32(&pf->hw, I40E_PFQF_CTL_1,
+		     I40E_PFQF_CTL_1_CLEARFDTABLE_MASK);
+		i40e_flush(&pf->hw);
+		pf->fd_flush_cnt++;
+		pf->fd_add_err = 0;
+		do {
+			/* Check FD flush status every 5-6msec */
+			usleep_range(5000, 6000);
+			reg = rd32(&pf->hw, I40E_PFQF_CTL_1);
+			if (!(reg & I40E_PFQF_CTL_1_CLEARFDTABLE_MASK))
+				break;
+		} while (flush_wait_retry--);
+		if (reg & I40E_PFQF_CTL_1_CLEARFDTABLE_MASK) {
+			dev_warn(&pf->pdev->dev, "FD table did not flush, needs more time\n");
+		} else {
+			/* replay sideband filters */
+			i40e_fdir_filter_restore(pf->vsi[pf->lan_vsi]);
+
+			pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
+			pf->auto_disable_flags &= ~I40E_FLAG_FD_ATR_ENABLED;
+			pf->auto_disable_flags &= ~I40E_FLAG_FD_SB_ENABLED;
+			clear_bit(__I40E_FD_FLUSH_REQUESTED, &pf->state);
+			dev_info(&pf->pdev->dev, "FD Filter table flushed and FD-SB replayed.\n");
+		}
+	}
+}
+
+/**
+ * i40e_get_current_atr_count - Get the count of total FD ATR filters programmed
+ * @pf: board private structure
+ **/
+int i40e_get_current_atr_cnt(struct i40e_pf *pf)
+{
+	return i40e_get_current_fd_count(pf) - pf->fdir_pf_active_filters;
+}
+
+/* We can see up to 256 filter programming desc in transit if the filters are
+ * being applied really fast; before we see the first
+ * filter miss error on Rx queue 0. Accumulating enough error messages before
+ * reacting will make sure we don't cause flush too often.
+ */
+#define I40E_MAX_FD_PROGRAM_ERROR 256
+
 /**
  * i40e_fdir_reinit_subtask - Worker thread to reinit FDIR filter table
  * @pf: board private structure
  **/
 static void i40e_fdir_reinit_subtask(struct i40e_pf *pf)
 {
-	if (!(pf->flags & I40E_FLAG_FDIR_REQUIRES_REINIT))
-		return;
 
 	/* if interface is down do nothing */
 	if (test_bit(__I40E_DOWN, &pf->state))
 		return;
+
+	if ((pf->fd_add_err >= I40E_MAX_FD_PROGRAM_ERROR) &&
+	    (i40e_get_current_atr_cnt(pf) >= pf->fd_atr_cnt) &&
+	    (i40e_get_current_atr_cnt(pf) > pf->fdir_pf_filter_count))
+		i40e_fdir_flush_and_replay(pf);
+
 	i40e_fdir_check_and_reenable(pf);
 
-	if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
-	    (pf->flags & I40E_FLAG_FD_SB_ENABLED))
-		pf->flags &= ~I40E_FLAG_FDIR_REQUIRES_REINIT;
 }
 
 /**
@@ -5184,7 +5289,7 @@ static void i40e_fdir_reinit_subtask(struct i40e_pf *pf)
  **/
 static void i40e_vsi_link_event(struct i40e_vsi *vsi, bool link_up)
 {
-	if (!vsi)
+	if (!vsi || test_bit(__I40E_DOWN, &vsi->state))
 		return;
 
 	switch (vsi->type) {
@@ -5420,6 +5525,13 @@ static void i40e_handle_link_event(struct i40e_pf *pf,
 	memcpy(&pf->hw.phy.link_info_old, hw_link_info,
 	       sizeof(pf->hw.phy.link_info_old));
 
+	/* check for unqualified module, if link is down */
+	if ((status->link_info & I40E_AQ_MEDIA_AVAILABLE) &&
+	    (!(status->an_info & I40E_AQ_QUALIFIED_MODULE)) &&
+	    (!(status->link_info & I40E_AQ_LINK_UP)))
+		dev_err(&pf->pdev->dev,
+			"The driver failed to link because an unqualified module was detected.\n");
+
 	/* update link status */
 	hw_link_info->phy_type = (enum i40e_aq_phy_type)status->phy_type;
 	hw_link_info->link_speed = (enum i40e_aq_link_speed)status->link_speed;
@@ -5456,6 +5568,10 @@ static void i40e_clean_adminq_subtask(struct i40e_pf *pf)
 	u32 oldval;
 	u32 val;
 
+	/* Do not run clean AQ when PF reset fails */
+	if (test_bit(__I40E_RESET_FAILED, &pf->state))
+		return;
+
 	/* check for error indications */
 	val = rd32(&pf->hw, pf->hw.aq.arq.len);
 	oldval = val;
@@ -5861,19 +5977,20 @@ static void i40e_reset_and_rebuild(struct i40e_pf *pf, bool reinit)
 	ret = i40e_pf_reset(hw);
 	if (ret) {
 		dev_info(&pf->pdev->dev, "PF reset failed, %d\n", ret);
-		goto end_core_reset;
+		set_bit(__I40E_RESET_FAILED, &pf->state);
+		goto clear_recovery;
 	}
 	pf->pfr_count++;
 
 	if (test_bit(__I40E_DOWN, &pf->state))
-		goto end_core_reset;
+		goto clear_recovery;
 	dev_dbg(&pf->pdev->dev, "Rebuilding internal switch\n");
 
 	/* rebuild the basics for the AdminQ, HMC, and initial HW switch */
 	ret = i40e_init_adminq(&pf->hw);
 	if (ret) {
 		dev_info(&pf->pdev->dev, "Rebuild AdminQ failed, %d\n", ret);
-		goto end_core_reset;
+		goto clear_recovery;
 	}
 
 	/* re-verify the eeprom if we just had an EMP reset */
@@ -5991,6 +6108,8 @@ static void i40e_reset_and_rebuild(struct i40e_pf *pf, bool reinit)
 	i40e_send_version(pf);
 
 end_core_reset:
+	clear_bit(__I40E_RESET_FAILED, &pf->state);
+clear_recovery:
 	clear_bit(__I40E_RESET_RECOVERY_PENDING, &pf->state);
 }
 
@@ -6036,9 +6155,9 @@ static void i40e_handle_mdd_event(struct i40e_pf *pf)
 				I40E_GL_MDET_TX_EVENT_SHIFT;
 		u8 queue = (reg & I40E_GL_MDET_TX_QUEUE_MASK) >>
 				I40E_GL_MDET_TX_QUEUE_SHIFT;
-		dev_info(&pf->pdev->dev,
-			 "Malicious Driver Detection event 0x%02x on TX queue %d pf number 0x%02x vf number 0x%02x\n",
-			 event, queue, pf_num, vf_num);
+		if (netif_msg_tx_err(pf))
+			dev_info(&pf->pdev->dev, "Malicious Driver Detection event 0x%02x on TX queue %d pf number 0x%02x vf number 0x%02x\n",
+				 event, queue, pf_num, vf_num);
 		wr32(hw, I40E_GL_MDET_TX, 0xffffffff);
 		mdd_detected = true;
 	}
@@ -6050,9 +6169,9 @@ static void i40e_handle_mdd_event(struct i40e_pf *pf)
 				I40E_GL_MDET_RX_EVENT_SHIFT;
 		u8 queue = (reg & I40E_GL_MDET_RX_QUEUE_MASK) >>
 				I40E_GL_MDET_RX_QUEUE_SHIFT;
-		dev_info(&pf->pdev->dev,
-			 "Malicious Driver Detection event 0x%02x on RX queue %d of function 0x%02x\n",
-			 event, queue, func);
+		if (netif_msg_rx_err(pf))
+			dev_info(&pf->pdev->dev, "Malicious Driver Detection event 0x%02x on RX queue %d of function 0x%02x\n",
+				 event, queue, func);
 		wr32(hw, I40E_GL_MDET_RX, 0xffffffff);
 		mdd_detected = true;
 	}
@@ -6061,17 +6180,13 @@ static void i40e_handle_mdd_event(struct i40e_pf *pf)
 		reg = rd32(hw, I40E_PF_MDET_TX);
 		if (reg & I40E_PF_MDET_TX_VALID_MASK) {
 			wr32(hw, I40E_PF_MDET_TX, 0xFFFF);
-			dev_info(&pf->pdev->dev,
-				 "MDD TX event is for this function 0x%08x, requesting PF reset.\n",
-				 reg);
+			dev_info(&pf->pdev->dev, "TX driver issue detected, PF reset issued\n");
 			pf_mdd_detected = true;
 		}
 		reg = rd32(hw, I40E_PF_MDET_RX);
 		if (reg & I40E_PF_MDET_RX_VALID_MASK) {
 			wr32(hw, I40E_PF_MDET_RX, 0xFFFF);
-			dev_info(&pf->pdev->dev,
-				 "MDD RX event is for this function 0x%08x, requesting PF reset.\n",
-				 reg);
+			dev_info(&pf->pdev->dev, "RX driver issue detected, PF reset issued\n");
 			pf_mdd_detected = true;
 		}
 		/* Queue belongs to the PF, initiate a reset */
@@ -6088,14 +6203,16 @@ static void i40e_handle_mdd_event(struct i40e_pf *pf)
 		if (reg & I40E_VP_MDET_TX_VALID_MASK) {
 			wr32(hw, I40E_VP_MDET_TX(i), 0xFFFF);
 			vf->num_mdd_events++;
-			dev_info(&pf->pdev->dev, "MDD TX event on VF %d\n", i);
+			dev_info(&pf->pdev->dev, "TX driver issue detected on VF %d\n",
+				 i);
 		}
 
 		reg = rd32(hw, I40E_VP_MDET_RX(i));
 		if (reg & I40E_VP_MDET_RX_VALID_MASK) {
 			wr32(hw, I40E_VP_MDET_RX(i), 0xFFFF);
 			vf->num_mdd_events++;
-			dev_info(&pf->pdev->dev, "MDD RX event on VF %d\n", i);
+			dev_info(&pf->pdev->dev, "RX driver issue detected on VF %d\n",
+				 i);
 		}
 
 		if (vf->num_mdd_events > I40E_DEFAULT_NUM_MDD_EVENTS_ALLOWED) {
@@ -7086,6 +7203,11 @@ bool i40e_set_ntuple(struct i40e_pf *pf, netdev_features_t features)
 		}
 		pf->flags &= ~I40E_FLAG_FD_SB_ENABLED;
 		pf->auto_disable_flags &= ~I40E_FLAG_FD_SB_ENABLED;
+		/* reset fd counters */
+		pf->fd_add_err = pf->fd_atr_cnt = pf->fd_tcp_rule = 0;
+		pf->fdir_pf_active_filters = 0;
+		pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
+		dev_info(&pf->pdev->dev, "ATR re-enabled.\n");
 		/* if ATR was auto disabled it can be re-enabled. */
 		if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
 		    (pf->auto_disable_flags & I40E_FLAG_FD_ATR_ENABLED))
@@ -7352,7 +7474,7 @@ static const struct net_device_ops i40e_netdev_ops = {
 	.ndo_set_vf_rate	= i40e_ndo_set_vf_bw,
 	.ndo_get_vf_config	= i40e_ndo_get_vf_config,
 	.ndo_set_vf_link_state	= i40e_ndo_set_vf_link_state,
-	.ndo_set_vf_spoofchk	= i40e_ndo_set_vf_spoofck,
+	.ndo_set_vf_spoofchk	= i40e_ndo_set_vf_spoofchk,
 #ifdef CONFIG_I40E_VXLAN
 	.ndo_add_vxlan_port	= i40e_add_vxlan_port,
 	.ndo_del_vxlan_port	= i40e_del_vxlan_port,
@@ -7421,14 +7543,14 @@ static int i40e_config_netdev(struct i40e_vsi *vsi)
 	if (vsi->type == I40E_VSI_MAIN) {
 		SET_NETDEV_DEV(netdev, &pf->pdev->dev);
 		ether_addr_copy(mac_addr, hw->mac.perm_addr);
-		/* The following two steps are necessary to prevent reception
-		 * of tagged packets - by default the NVM loads a MAC-VLAN
-		 * filter that will accept any tagged packet.  This is to
-		 * prevent that during normal operations until a specific
-		 * VLAN tag filter has been set.
+		/* The following steps are necessary to prevent reception
+		 * of tagged packets - some older NVM configurations load a
+		 * default a MAC-VLAN filter that accepts any tagged packet
+		 * which must be replaced by a normal filter.
 		 */
-		i40e_rm_default_mac_filter(vsi, mac_addr);
-		i40e_add_filter(vsi, mac_addr, I40E_VLAN_ANY, false, true);
+		if (!i40e_rm_default_mac_filter(vsi, mac_addr))
+			i40e_add_filter(vsi, mac_addr,
+					I40E_VLAN_ANY, false, true);
 	} else {
 		/* relate the VSI_VMDQ name to the VSI_MAIN name */
 		snprintf(netdev->name, IFNAMSIZ, "%sv%%d",
@@ -7644,7 +7766,22 @@ static int i40e_add_vsi(struct i40e_vsi *vsi)
 		f_count++;
 
 		if (f->is_laa && vsi->type == I40E_VSI_MAIN) {
-			i40e_aq_mac_address_write(&vsi->back->hw,
+			struct i40e_aqc_remove_macvlan_element_data element;
+
+			memset(&element, 0, sizeof(element));
+			ether_addr_copy(element.mac_addr, f->macaddr);
+			element.flags = I40E_AQC_MACVLAN_DEL_PERFECT_MATCH;
+			ret = i40e_aq_remove_macvlan(hw, vsi->seid,
+						     &element, 1, NULL);
+			if (ret) {
+				/* some older FW has a different default */
+				element.flags |=
+					       I40E_AQC_MACVLAN_DEL_IGNORE_VLAN;
+				i40e_aq_remove_macvlan(hw, vsi->seid,
+						       &element, 1, NULL);
+			}
+
+			i40e_aq_mac_address_write(hw,
 						  I40E_AQC_WRITE_TYPE_LAA_WOL,
 						  f->macaddr, NULL);
 		}
diff --git a/drivers/net/ethernet/intel/i40e/i40e_prototype.h b/drivers/net/ethernet/intel/i40e/i40e_prototype.h
index 949a9a01778b..0988b5c1fe87 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_prototype.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_prototype.h
@@ -52,10 +52,8 @@ i40e_status i40e_asq_send_command(struct i40e_hw *hw,
 				struct i40e_asq_cmd_details *cmd_details);
 
 /* debug function for adminq */
-void i40e_debug_aq(struct i40e_hw *hw,
-		   enum i40e_debug_mask mask,
-		   void *desc,
-		   void *buffer);
+void i40e_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask,
+		   void *desc, void *buffer, u16 buf_len);
 
 void i40e_idle_aq(struct i40e_hw *hw);
 bool i40e_check_asq_alive(struct i40e_hw *hw);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_ptp.c b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
index bb7fe98b3a6c..537b6216971d 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_ptp.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_ptp.c
@@ -247,7 +247,7 @@ void i40e_ptp_rx_hang(struct i40e_vsi *vsi)
 	u32 prttsyn_stat;
 	int n;
 
-	if (pf->flags & I40E_FLAG_PTP)
+	if (!(pf->flags & I40E_FLAG_PTP))
 		return;
 
 	prttsyn_stat = rd32(hw, I40E_PRTTSYN_STAT_1);
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.c b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
index a51aa37b7b5a..3195d82e4942 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.c
@@ -224,15 +224,19 @@ static int i40e_add_del_fdir_udpv4(struct i40e_vsi *vsi,
 	ret = i40e_program_fdir_filter(fd_data, raw_packet, pf, add);
 	if (ret) {
 		dev_info(&pf->pdev->dev,
-			 "Filter command send failed for PCTYPE %d (ret = %d)\n",
-			 fd_data->pctype, ret);
+			 "PCTYPE:%d, Filter command send failed for fd_id:%d (ret = %d)\n",
+			 fd_data->pctype, fd_data->fd_id, ret);
 		err = true;
 	} else {
-		dev_info(&pf->pdev->dev,
-			 "Filter OK for PCTYPE %d (ret = %d)\n",
-			 fd_data->pctype, ret);
+		if (add)
+			dev_info(&pf->pdev->dev,
+				 "Filter OK for PCTYPE %d loc = %d\n",
+				 fd_data->pctype, fd_data->fd_id);
+		else
+			dev_info(&pf->pdev->dev,
+				 "Filter deleted for PCTYPE %d loc = %d\n",
+				 fd_data->pctype, fd_data->fd_id);
 	}
-
 	return err ? -EOPNOTSUPP : 0;
 }
 
@@ -276,10 +280,18 @@ static int i40e_add_del_fdir_tcpv4(struct i40e_vsi *vsi,
 	tcp->source = fd_data->src_port;
 
 	if (add) {
+		pf->fd_tcp_rule++;
 		if (pf->flags & I40E_FLAG_FD_ATR_ENABLED) {
 			dev_info(&pf->pdev->dev, "Forcing ATR off, sideband rules for TCP/IPv4 flow being applied\n");
 			pf->flags &= ~I40E_FLAG_FD_ATR_ENABLED;
 		}
+	} else {
+		pf->fd_tcp_rule = (pf->fd_tcp_rule > 0) ?
+				  (pf->fd_tcp_rule - 1) : 0;
+		if (pf->fd_tcp_rule == 0) {
+			pf->flags |= I40E_FLAG_FD_ATR_ENABLED;
+			dev_info(&pf->pdev->dev, "ATR re-enabled due to no sideband TCP/IPv4 rules\n");
+		}
 	}
 
 	fd_data->pctype = I40E_FILTER_PCTYPE_NONF_IPV4_TCP;
@@ -287,12 +299,17 @@ static int i40e_add_del_fdir_tcpv4(struct i40e_vsi *vsi,
 
 	if (ret) {
 		dev_info(&pf->pdev->dev,
-			 "Filter command send failed for PCTYPE %d (ret = %d)\n",
-			 fd_data->pctype, ret);
+			 "PCTYPE:%d, Filter command send failed for fd_id:%d (ret = %d)\n",
+			 fd_data->pctype, fd_data->fd_id, ret);
 		err = true;
 	} else {
-		dev_info(&pf->pdev->dev, "Filter OK for PCTYPE %d (ret = %d)\n",
-			 fd_data->pctype, ret);
+		if (add)
+			dev_info(&pf->pdev->dev, "Filter OK for PCTYPE %d loc = %d)\n",
+				 fd_data->pctype, fd_data->fd_id);
+		else
+			dev_info(&pf->pdev->dev,
+				 "Filter deleted for PCTYPE %d loc = %d\n",
+				 fd_data->pctype, fd_data->fd_id);
 	}
 
 	return err ? -EOPNOTSUPP : 0;
@@ -355,13 +372,18 @@ static int i40e_add_del_fdir_ipv4(struct i40e_vsi *vsi,
 
 		if (ret) {
 			dev_info(&pf->pdev->dev,
-				 "Filter command send failed for PCTYPE %d (ret = %d)\n",
-				 fd_data->pctype, ret);
+				 "PCTYPE:%d, Filter command send failed for fd_id:%d (ret = %d)\n",
+				 fd_data->pctype, fd_data->fd_id, ret);
 			err = true;
 		} else {
-			dev_info(&pf->pdev->dev,
-				 "Filter OK for PCTYPE %d (ret = %d)\n",
-				 fd_data->pctype, ret);
+			if (add)
+				dev_info(&pf->pdev->dev,
+					 "Filter OK for PCTYPE %d loc = %d\n",
+					 fd_data->pctype, fd_data->fd_id);
+			else
+				dev_info(&pf->pdev->dev,
+					 "Filter deleted for PCTYPE %d loc = %d\n",
+					 fd_data->pctype, fd_data->fd_id);
 		}
 	}
 
@@ -443,8 +465,14 @@ static void i40e_fd_handle_status(struct i40e_ring *rx_ring,
 		I40E_RX_PROG_STATUS_DESC_QW1_ERROR_SHIFT;
 
 	if (error == (0x1 << I40E_RX_PROG_STATUS_DESC_FD_TBL_FULL_SHIFT)) {
-		dev_warn(&pdev->dev, "ntuple filter loc = %d, could not be added\n",
-			 rx_desc->wb.qword0.hi_dword.fd_id);
+		if ((rx_desc->wb.qword0.hi_dword.fd_id != 0) ||
+		    (I40E_DEBUG_FD & pf->hw.debug_mask))
+			dev_warn(&pdev->dev, "ntuple filter loc = %d, could not be added\n",
+				 rx_desc->wb.qword0.hi_dword.fd_id);
+
+		pf->fd_add_err++;
+		/* store the current atr filter count */
+		pf->fd_atr_cnt = i40e_get_current_atr_cnt(pf);
 
 		/* filter programming failed most likely due to table full */
 		fcnt_prog = i40e_get_cur_guaranteed_fd_count(pf);
@@ -454,29 +482,21 @@ static void i40e_fd_handle_status(struct i40e_ring *rx_ring,
 		 * FD ATR/SB and then re-enable it when there is room.
 		 */
 		if (fcnt_prog >= (fcnt_avail - I40E_FDIR_BUFFER_FULL_MARGIN)) {
-			/* Turn off ATR first */
-			if ((pf->flags & I40E_FLAG_FD_ATR_ENABLED) &&
+			if ((pf->flags & I40E_FLAG_FD_SB_ENABLED) &&
 			    !(pf->auto_disable_flags &
-			      I40E_FLAG_FD_ATR_ENABLED)) {
-				dev_warn(&pdev->dev, "FD filter space full, ATR for further flows will be turned off\n");
-				pf->auto_disable_flags |=
-						       I40E_FLAG_FD_ATR_ENABLED;
-				pf->flags |= I40E_FLAG_FDIR_REQUIRES_REINIT;
-			} else if ((pf->flags & I40E_FLAG_FD_SB_ENABLED) &&
-				   !(pf->auto_disable_flags &
 				     I40E_FLAG_FD_SB_ENABLED)) {
 				dev_warn(&pdev->dev, "FD filter space full, new ntuple rules will not be added\n");
 				pf->auto_disable_flags |=
 							I40E_FLAG_FD_SB_ENABLED;
-				pf->flags |= I40E_FLAG_FDIR_REQUIRES_REINIT;
 			}
 		} else {
-			dev_info(&pdev->dev, "FD filter programming error\n");
+			dev_info(&pdev->dev,
+				"FD filter programming failed due to incorrect filter parameters\n");
 		}
 	} else if (error ==
 			  (0x1 << I40E_RX_PROG_STATUS_DESC_NO_FD_ENTRY_SHIFT)) {
 		if (I40E_DEBUG_FD & pf->hw.debug_mask)
-			dev_info(&pdev->dev, "ntuple filter loc = %d, could not be removed\n",
+			dev_info(&pdev->dev, "ntuple filter fd_id = %d, could not be removed\n",
 				 rx_desc->wb.qword0.hi_dword.fd_id);
 	}
 }
@@ -587,6 +607,7 @@ static u32 i40e_get_tx_pending(struct i40e_ring *ring)
 static bool i40e_check_tx_hang(struct i40e_ring *tx_ring)
 {
 	u32 tx_pending = i40e_get_tx_pending(tx_ring);
+	struct i40e_pf *pf = tx_ring->vsi->back;
 	bool ret = false;
 
 	clear_check_for_tx_hang(tx_ring);
@@ -603,10 +624,17 @@ static bool i40e_check_tx_hang(struct i40e_ring *tx_ring)
 	 * pending but without time to complete it yet.
 	 */
 	if ((tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) &&
-	    tx_pending) {
+	    (tx_pending >= I40E_MIN_DESC_PENDING)) {
 		/* make sure it is true for two checks in a row */
 		ret = test_and_set_bit(__I40E_HANG_CHECK_ARMED,
 				       &tx_ring->state);
+	} else if ((tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) &&
+		   (tx_pending < I40E_MIN_DESC_PENDING) &&
+		   (tx_pending > 0)) {
+		if (I40E_DEBUG_FLOW & pf->hw.debug_mask)
+			dev_info(tx_ring->dev, "HW needs some more descs to do a cacheline flush. tx_pending %d, queue %d",
+				 tx_pending, tx_ring->queue_index);
+		pf->tx_sluggish_count++;
 	} else {
 		/* update completed stats and disarm the hang check */
 		tx_ring->tx_stats.tx_done_old = tx_ring->stats.packets;
@@ -674,7 +702,7 @@ static bool i40e_clean_tx_irq(struct i40e_ring *tx_ring, int budget)
 		total_packets += tx_buf->gso_segs;
 
 		/* free the skb */
-		dev_kfree_skb_any(tx_buf->skb);
+		dev_consume_skb_any(tx_buf->skb);
 
 		/* unmap skb header data */
 		dma_unmap_single(tx_ring->dev,
@@ -1213,7 +1241,6 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	ipv6_tunnel = (rx_ptype > I40E_RX_PTYPE_GRENAT6_MAC_PAY3) &&
 		      (rx_ptype < I40E_RX_PTYPE_GRENAT6_MACVLAN_IPV6_ICMP_PAY4);
 
-	skb->encapsulation = ipv4_tunnel || ipv6_tunnel;
 	skb->ip_summed = CHECKSUM_NONE;
 
 	/* Rx csum enabled and ip headers found? */
@@ -1287,6 +1314,7 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	}
 
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb->csum_level = ipv4_tunnel || ipv6_tunnel;
 
 	return;
 
@@ -2025,6 +2053,47 @@ static void i40e_create_tx_ctx(struct i40e_ring *tx_ring,
 }
 
 /**
+ * __i40e_maybe_stop_tx - 2nd level check for tx stop conditions
+ * @tx_ring: the ring to be checked
+ * @size:    the size buffer we want to assure is available
+ *
+ * Returns -EBUSY if a stop is needed, else 0
+ **/
+static inline int __i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
+{
+	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
+	/* Memory barrier before checking head and tail */
+	smp_mb();
+
+	/* Check again in a case another CPU has just made room available. */
+	if (likely(I40E_DESC_UNUSED(tx_ring) < size))
+		return -EBUSY;
+
+	/* A reprieve! - use start_queue because it doesn't call schedule */
+	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
+	++tx_ring->tx_stats.restart_queue;
+	return 0;
+}
+
+/**
+ * i40e_maybe_stop_tx - 1st level check for tx stop conditions
+ * @tx_ring: the ring to be checked
+ * @size:    the size buffer we want to assure is available
+ *
+ * Returns 0 if stop is not needed
+ **/
+#ifdef I40E_FCOE
+int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
+#else
+static int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
+#endif
+{
+	if (likely(I40E_DESC_UNUSED(tx_ring) >= size))
+		return 0;
+	return __i40e_maybe_stop_tx(tx_ring, size);
+}
+
+/**
  * i40e_tx_map - Build the Tx descriptor
  * @tx_ring:  ring to send buffer on
  * @skb:      send buffer
@@ -2167,8 +2236,12 @@ static void i40e_tx_map(struct i40e_ring *tx_ring, struct sk_buff *skb,
 
 	tx_ring->next_to_use = i;
 
+	i40e_maybe_stop_tx(tx_ring, DESC_NEEDED);
 	/* notify HW of packet */
-	writel(i, tx_ring->tail);
+	if (!skb->xmit_more ||
+	    netif_xmit_stopped(netdev_get_tx_queue(tx_ring->netdev,
+						   tx_ring->queue_index)))
+		writel(i, tx_ring->tail);
 
 	return;
 
@@ -2190,47 +2263,6 @@ dma_error:
 }
 
 /**
- * __i40e_maybe_stop_tx - 2nd level check for tx stop conditions
- * @tx_ring: the ring to be checked
- * @size:    the size buffer we want to assure is available
- *
- * Returns -EBUSY if a stop is needed, else 0
- **/
-static inline int __i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
-{
-	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
-	/* Memory barrier before checking head and tail */
-	smp_mb();
-
-	/* Check again in a case another CPU has just made room available. */
-	if (likely(I40E_DESC_UNUSED(tx_ring) < size))
-		return -EBUSY;
-
-	/* A reprieve! - use start_queue because it doesn't call schedule */
-	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
-	++tx_ring->tx_stats.restart_queue;
-	return 0;
-}
-
-/**
- * i40e_maybe_stop_tx - 1st level check for tx stop conditions
- * @tx_ring: the ring to be checked
- * @size:    the size buffer we want to assure is available
- *
- * Returns 0 if stop is not needed
- **/
-#ifdef I40E_FCOE
-int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
-#else
-static int i40e_maybe_stop_tx(struct i40e_ring *tx_ring, int size)
-#endif
-{
-	if (likely(I40E_DESC_UNUSED(tx_ring) >= size))
-		return 0;
-	return __i40e_maybe_stop_tx(tx_ring, size);
-}
-
-/**
  * i40e_xmit_descriptor_count - calculate number of tx descriptors needed
  * @skb:     send buffer
  * @tx_ring: ring to send buffer on
@@ -2295,7 +2327,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 		goto out_drop;
 
 	/* obtain protocol of skb */
-	protocol = skb->protocol;
+	protocol = vlan_get_protocol(skb);
 
 	/* record the location of the first descriptor for this packet */
 	first = &tx_ring->tx_bi[tx_ring->next_to_use];
@@ -2344,8 +2376,6 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 	i40e_tx_map(tx_ring, skb, first, tx_flags, hdr_len,
 		    td_cmd, td_offset);
 
-	i40e_maybe_stop_tx(tx_ring, DESC_NEEDED);
-
 	return NETDEV_TX_OK;
 
 out_drop:
diff --git a/drivers/net/ethernet/intel/i40e/i40e_txrx.h b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
index 73f4fa425697..d7a625a6a14f 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_txrx.h
@@ -121,6 +121,7 @@ enum i40e_dyn_idx_t {
 /* Tx Descriptors needed, worst case */
 #define TXD_USE_COUNT(S) DIV_ROUND_UP((S), I40E_MAX_DATA_PER_TXD)
 #define DESC_NEEDED (MAX_SKB_FRAGS + 4)
+#define I40E_MIN_DESC_PENDING	4
 
 #define I40E_TX_FLAGS_CSUM		(u32)(1)
 #define I40E_TX_FLAGS_HW_VLAN		(u32)(1 << 1)
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
index 89672551dce9..4eeed267e4b7 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.c
@@ -73,7 +73,7 @@ static inline bool i40e_vc_isvalid_queue_id(struct i40e_vf *vf, u8 vsi_id,
 {
 	struct i40e_pf *pf = vf->pf;
 
-	return qid < pf->vsi[vsi_id]->num_queue_pairs;
+	return qid < pf->vsi[vsi_id]->alloc_queue_pairs;
 }
 
 /**
@@ -350,6 +350,7 @@ static int i40e_config_vsi_rx_queue(struct i40e_vf *vf, u16 vsi_idx,
 	rx_ctx.lrxqthresh = 2;
 	rx_ctx.crcstrip = 1;
 	rx_ctx.prefena = 1;
+	rx_ctx.l2tsel = 1;
 
 	/* clear the context in the HMC */
 	ret = i40e_clear_lan_rx_queue_context(hw, pf_queue_id);
@@ -468,7 +469,7 @@ static void i40e_enable_vf_mappings(struct i40e_vf *vf)
 	wr32(hw, I40E_VPLAN_MAPENA(vf->vf_id), reg);
 
 	/* map PF queues to VF queues */
-	for (j = 0; j < pf->vsi[vf->lan_vsi_index]->num_queue_pairs; j++) {
+	for (j = 0; j < pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs; j++) {
 		u16 qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_index, j);
 		reg = (qid & I40E_VPLAN_QTABLE_QINDEX_MASK);
 		wr32(hw, I40E_VPLAN_QTABLE(total_queue_pairs, vf->vf_id), reg);
@@ -477,7 +478,7 @@ static void i40e_enable_vf_mappings(struct i40e_vf *vf)
 
 	/* map PF queues to VSI */
 	for (j = 0; j < 7; j++) {
-		if (j * 2 >= pf->vsi[vf->lan_vsi_index]->num_queue_pairs) {
+		if (j * 2 >= pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs) {
 			reg = 0x07FF07FF;	/* unused */
 		} else {
 			u16 qid = i40e_vc_get_pf_queue_id(vf, vf->lan_vsi_index,
@@ -584,7 +585,7 @@ static int i40e_alloc_vf_res(struct i40e_vf *vf)
 	ret = i40e_alloc_vsi_res(vf, I40E_VSI_SRIOV);
 	if (ret)
 		goto error_alloc;
-	total_queue_pairs += pf->vsi[vf->lan_vsi_index]->num_queue_pairs;
+	total_queue_pairs += pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs;
 	set_bit(I40E_VIRTCHNL_VF_CAP_PRIVILEGE, &vf->vf_caps);
 
 	/* store the total qps number for the runtime
@@ -706,35 +707,6 @@ complete_reset:
 	wr32(hw, I40E_VFGEN_RSTAT1(vf->vf_id), I40E_VFR_VFACTIVE);
 	i40e_flush(hw);
 }
-
-/**
- * i40e_vfs_are_assigned
- * @pf: pointer to the pf structure
- *
- * Determine if any VFs are assigned to VMs
- **/
-static bool i40e_vfs_are_assigned(struct i40e_pf *pf)
-{
-	struct pci_dev *pdev = pf->pdev;
-	struct pci_dev *vfdev;
-
-	/* loop through all the VFs to see if we own any that are assigned */
-	vfdev = pci_get_device(PCI_VENDOR_ID_INTEL, I40E_DEV_ID_VF , NULL);
-	while (vfdev) {
-		/* if we don't own it we don't care */
-		if (vfdev->is_virtfn && pci_physfn(vfdev) == pdev) {
-			/* if it is assigned we cannot release it */
-			if (vfdev->dev_flags & PCI_DEV_FLAGS_ASSIGNED)
-				return true;
-		}
-
-		vfdev = pci_get_device(PCI_VENDOR_ID_INTEL,
-				       I40E_DEV_ID_VF,
-				       vfdev);
-	}
-
-	return false;
-}
 #ifdef CONFIG_PCI_IOV
 
 /**
@@ -842,7 +814,7 @@ void i40e_free_vfs(struct i40e_pf *pf)
 	 * assigned. Setting the number of VFs to 0 through sysfs is caught
 	 * before this function ever gets called.
 	 */
-	if (!i40e_vfs_are_assigned(pf)) {
+	if (!pci_vfs_assigned(pf->pdev)) {
 		pci_disable_sriov(pf->pdev);
 		/* Acknowledge VFLR for all VFS. Without this, VFs will fail to
 		 * work correctly when SR-IOV gets re-enabled.
@@ -979,7 +951,7 @@ int i40e_pci_sriov_configure(struct pci_dev *pdev, int num_vfs)
 	if (num_vfs)
 		return i40e_pci_sriov_enable(pdev, num_vfs);
 
-	if (!i40e_vfs_are_assigned(pf)) {
+	if (!pci_vfs_assigned(pf->pdev)) {
 		i40e_free_vfs(pf);
 	} else {
 		dev_warn(&pdev->dev, "Unable to free VFs because some are assigned to VMs.\n");
@@ -1003,11 +975,19 @@ int i40e_pci_sriov_configure(struct pci_dev *pdev, int num_vfs)
 static int i40e_vc_send_msg_to_vf(struct i40e_vf *vf, u32 v_opcode,
 				  u32 v_retval, u8 *msg, u16 msglen)
 {
-	struct i40e_pf *pf = vf->pf;
-	struct i40e_hw *hw = &pf->hw;
-	int abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
+	struct i40e_pf *pf;
+	struct i40e_hw *hw;
+	int abs_vf_id;
 	i40e_status aq_ret;
 
+	/* validate the request */
+	if (!vf || vf->vf_id >= vf->pf->num_alloc_vfs)
+		return -EINVAL;
+
+	pf = vf->pf;
+	hw = &pf->hw;
+	abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
+
 	/* single place to detect unsuccessful return values */
 	if (v_retval) {
 		vf->num_invalid_msgs++;
@@ -1115,7 +1095,7 @@ static int i40e_vc_get_vf_resources_msg(struct i40e_vf *vf)
 		vfres->vsi_res[i].vsi_id = vf->lan_vsi_index;
 		vfres->vsi_res[i].vsi_type = I40E_VSI_SRIOV;
 		vfres->vsi_res[i].num_queue_pairs =
-		    pf->vsi[vf->lan_vsi_index]->num_queue_pairs;
+		    pf->vsi[vf->lan_vsi_index]->alloc_queue_pairs;
 		memcpy(vfres->vsi_res[i].default_mac_addr,
 		       vf->default_lan_addr.addr, ETH_ALEN);
 		i++;
@@ -1201,6 +1181,7 @@ static int i40e_vc_config_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 	struct i40e_virtchnl_vsi_queue_config_info *qci =
 	    (struct i40e_virtchnl_vsi_queue_config_info *)msg;
 	struct i40e_virtchnl_queue_pair_info *qpi;
+	struct i40e_pf *pf = vf->pf;
 	u16 vsi_id, vsi_queue_id;
 	i40e_status aq_ret = 0;
 	int i;
@@ -1234,6 +1215,8 @@ static int i40e_vc_config_queues_msg(struct i40e_vf *vf, u8 *msg, u16 msglen)
 			goto error_param;
 		}
 	}
+	/* set vsi num_queue_pairs in use to num configured by vf */
+	pf->vsi[vf->lan_vsi_index]->num_queue_pairs = qci->num_queue_pairs;
 
 error_param:
 	/* send the response to the vf */
@@ -1928,17 +1911,20 @@ static void i40e_vc_vf_broadcast(struct i40e_pf *pf,
 {
 	struct i40e_hw *hw = &pf->hw;
 	struct i40e_vf *vf = pf->vf;
-	int abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
 	int i;
 
-	for (i = 0; i < pf->num_alloc_vfs; i++) {
+	for (i = 0; i < pf->num_alloc_vfs; i++, vf++) {
+		int abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
+		/* Not all vfs are enabled so skip the ones that are not */
+		if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states) &&
+		    !test_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states))
+			continue;
+
 		/* Ignore return value on purpose - a given VF may fail, but
 		 * we need to keep going and send to all of them
 		 */
 		i40e_aq_send_msg_to_vf(hw, abs_vf_id, v_opcode, v_retval,
 				       msg, msglen, NULL);
-		vf++;
-		abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
 	}
 }
 
@@ -1954,12 +1940,12 @@ void i40e_vc_notify_link_state(struct i40e_pf *pf)
 	struct i40e_hw *hw = &pf->hw;
 	struct i40e_vf *vf = pf->vf;
 	struct i40e_link_status *ls = &pf->hw.phy.link_info;
-	int abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
 	int i;
 
 	pfe.event = I40E_VIRTCHNL_EVENT_LINK_CHANGE;
 	pfe.severity = I40E_PF_EVENT_SEVERITY_INFO;
-	for (i = 0; i < pf->num_alloc_vfs; i++) {
+	for (i = 0; i < pf->num_alloc_vfs; i++, vf++) {
+		int abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
 		if (vf->link_forced) {
 			pfe.event_data.link_event.link_status = vf->link_up;
 			pfe.event_data.link_event.link_speed =
@@ -1972,8 +1958,6 @@ void i40e_vc_notify_link_state(struct i40e_pf *pf)
 		i40e_aq_send_msg_to_vf(hw, abs_vf_id, I40E_VIRTCHNL_OP_EVENT,
 				       0, (u8 *)&pfe, sizeof(pfe),
 				       NULL);
-		vf++;
-		abs_vf_id = vf->vf_id + hw->func_caps.vf_base_id;
 	}
 }
 
@@ -2002,7 +1986,18 @@ void i40e_vc_notify_reset(struct i40e_pf *pf)
 void i40e_vc_notify_vf_reset(struct i40e_vf *vf)
 {
 	struct i40e_virtchnl_pf_event pfe;
-	int abs_vf_id = vf->vf_id + vf->pf->hw.func_caps.vf_base_id;
+	int abs_vf_id;
+
+	/* validate the request */
+	if (!vf || vf->vf_id >= vf->pf->num_alloc_vfs)
+		return;
+
+	/* verify if the VF is in either init or active before proceeding */
+	if (!test_bit(I40E_VF_STAT_INIT, &vf->vf_states) &&
+	    !test_bit(I40E_VF_STAT_ACTIVE, &vf->vf_states))
+		return;
+
+	abs_vf_id = vf->vf_id + vf->pf->hw.func_caps.vf_base_id;
 
 	pfe.event = I40E_VIRTCHNL_EVENT_RESET_IMPENDING;
 	pfe.severity = I40E_PF_EVENT_SEVERITY_CERTAIN_DOOM;
@@ -2074,7 +2069,6 @@ int i40e_ndo_set_vf_mac(struct net_device *netdev, int vf_id, u8 *mac)
 	/* Force the VF driver stop so it has to reload with new MAC address */
 	i40e_vc_disable_vf(pf, vf);
 	dev_info(&pf->pdev->dev, "Reload the VF driver to make this change effective.\n");
-	ret = 0;
 
 error_param:
 	return ret;
@@ -2399,7 +2393,7 @@ error_out:
  *
  * Enable or disable VF spoof checking
  **/
-int i40e_ndo_set_vf_spoofck(struct net_device *netdev, int vf_id, bool enable)
+int i40e_ndo_set_vf_spoofchk(struct net_device *netdev, int vf_id, bool enable)
 {
 	struct i40e_netdev_priv *np = netdev_priv(netdev);
 	struct i40e_vsi *vsi = np->vsi;
diff --git a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
index 63e7e0d81ad2..0adc61e1052d 100644
--- a/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
+++ b/drivers/net/ethernet/intel/i40e/i40e_virtchnl_pf.h
@@ -122,7 +122,7 @@ int i40e_ndo_set_vf_bw(struct net_device *netdev, int vf_id, int min_tx_rate,
 int i40e_ndo_get_vf_config(struct net_device *netdev,
 			   int vf_id, struct ifla_vf_info *ivi);
 int i40e_ndo_set_vf_link_state(struct net_device *netdev, int vf_id, int link);
-int i40e_ndo_set_vf_spoofck(struct net_device *netdev, int vf_id, bool enable);
+int i40e_ndo_set_vf_spoofchk(struct net_device *netdev, int vf_id, bool enable);
 
 void i40e_vc_notify_link_state(struct i40e_pf *pf);
 void i40e_vc_notify_reset(struct i40e_pf *pf);
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_adminq.c b/drivers/net/ethernet/intel/i40evf/i40e_adminq.c
index 003006033614..f206be917842 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_adminq.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_adminq.c
@@ -788,7 +788,8 @@ i40e_status i40evf_asq_send_command(struct i40e_hw *hw,
 
 	/* bump the tail */
 	i40e_debug(hw, I40E_DEBUG_AQ_MESSAGE, "AQTX: desc and buffer:\n");
-	i40evf_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc_on_ring, buff);
+	i40evf_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc_on_ring,
+			buff, buff_size);
 	(hw->aq.asq.next_to_use)++;
 	if (hw->aq.asq.next_to_use == hw->aq.asq.count)
 		hw->aq.asq.next_to_use = 0;
@@ -842,7 +843,8 @@ i40e_status i40evf_asq_send_command(struct i40e_hw *hw,
 
 	i40e_debug(hw, I40E_DEBUG_AQ_MESSAGE,
 		   "AQTX: desc and buffer writeback:\n");
-	i40evf_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, buff);
+	i40evf_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, buff,
+			buff_size);
 
 	/* update the error if time out occurred */
 	if ((!cmd_completed) &&
@@ -938,7 +940,8 @@ i40e_status i40evf_clean_arq_element(struct i40e_hw *hw,
 		hw->aq.nvm_busy = false;
 
 	i40e_debug(hw, I40E_DEBUG_AQ_MESSAGE, "AQRX: desc and buffer:\n");
-	i40evf_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, e->msg_buf);
+	i40evf_debug_aq(hw, I40E_DEBUG_AQ_COMMAND, (void *)desc, e->msg_buf,
+			hw->aq.arq_buf_size);
 
 	/* Restore the original datalen and buffer address in the desc,
 	 * FW updates datalen to indicate the event message
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_common.c b/drivers/net/ethernet/intel/i40evf/i40e_common.c
index 4ea90bf239bb..952560551964 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_common.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_common.c
@@ -75,13 +75,15 @@ i40e_status i40e_set_mac_type(struct i40e_hw *hw)
  * @mask: debug mask
  * @desc: pointer to admin queue descriptor
  * @buffer: pointer to command buffer
+ * @buf_len: max length of buffer
  *
  * Dumps debug log about adminq command with descriptor contents.
  **/
 void i40evf_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
-		   void *buffer)
+		   void *buffer, u16 buf_len)
 {
 	struct i40e_aq_desc *aq_desc = (struct i40e_aq_desc *)desc;
+	u16 len = le16_to_cpu(aq_desc->datalen);
 	u8 *aq_buffer = (u8 *)buffer;
 	u32 data[4];
 	u32 i = 0;
@@ -105,7 +107,9 @@ void i40evf_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask, void *desc,
 	if ((buffer != NULL) && (aq_desc->datalen != 0)) {
 		memset(data, 0, sizeof(data));
 		i40e_debug(hw, mask, "AQ CMD Buffer:\n");
-		for (i = 0; i < le16_to_cpu(aq_desc->datalen); i++) {
+		if (buf_len < len)
+			len = buf_len;
+		for (i = 0; i < len; i++) {
 			data[((i % 16) / 4)] |=
 				((u32)aq_buffer[i]) << (8 * (i % 4));
 			if ((i % 16) == 15) {
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_prototype.h b/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
index 849edcc2e398..9173834825ac 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_prototype.h
@@ -53,10 +53,8 @@ i40e_status i40evf_asq_send_command(struct i40e_hw *hw,
 bool i40evf_asq_done(struct i40e_hw *hw);
 
 /* debug function for adminq */
-void i40evf_debug_aq(struct i40e_hw *hw,
-		   enum i40e_debug_mask mask,
-		   void *desc,
-		   void *buffer);
+void i40evf_debug_aq(struct i40e_hw *hw, enum i40e_debug_mask mask,
+		     void *desc, void *buffer, u16 buf_len);
 
 void i40e_idle_aq(struct i40e_hw *hw);
 void i40evf_resume_aq(struct i40e_hw *hw);
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
index 79bf96ca6489..04c7c1557a0c 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.c
@@ -163,11 +163,13 @@ static bool i40e_check_tx_hang(struct i40e_ring *tx_ring)
 	 * pending but without time to complete it yet.
 	 */
 	if ((tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) &&
-	    tx_pending) {
+	    (tx_pending >= I40E_MIN_DESC_PENDING)) {
 		/* make sure it is true for two checks in a row */
 		ret = test_and_set_bit(__I40E_HANG_CHECK_ARMED,
 				       &tx_ring->state);
-	} else {
+	} else if (!(tx_ring->tx_stats.tx_done_old == tx_ring->stats.packets) ||
+		   !(tx_pending < I40E_MIN_DESC_PENDING) ||
+		   !(tx_pending > 0)) {
 		/* update completed stats and disarm the hang check */
 		tx_ring->tx_stats.tx_done_old = tx_ring->stats.packets;
 		clear_bit(__I40E_HANG_CHECK_ARMED, &tx_ring->state);
@@ -744,7 +746,6 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	ipv6_tunnel = (rx_ptype > I40E_RX_PTYPE_GRENAT6_MAC_PAY3) &&
 		      (rx_ptype < I40E_RX_PTYPE_GRENAT6_MACVLAN_IPV6_ICMP_PAY4);
 
-	skb->encapsulation = ipv4_tunnel || ipv6_tunnel;
 	skb->ip_summed = CHECKSUM_NONE;
 
 	/* Rx csum enabled and ip headers found? */
@@ -818,6 +819,7 @@ static inline void i40e_rx_checksum(struct i40e_vsi *vsi,
 	}
 
 	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb->csum_level = ipv4_tunnel || ipv6_tunnel;
 
 	return;
 
@@ -1597,7 +1599,7 @@ static netdev_tx_t i40e_xmit_frame_ring(struct sk_buff *skb,
 		goto out_drop;
 
 	/* obtain protocol of skb */
-	protocol = skb->protocol;
+	protocol = vlan_get_protocol(skb);
 
 	/* record the location of the first descriptor for this packet */
 	first = &tx_ring->tx_bi[tx_ring->next_to_use];
diff --git a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
index 8bc6858163b0..f6dcf9dd9290 100644
--- a/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
+++ b/drivers/net/ethernet/intel/i40evf/i40e_txrx.h
@@ -121,6 +121,7 @@ enum i40e_dyn_idx_t {
 /* Tx Descriptors needed, worst case */
 #define TXD_USE_COUNT(S) DIV_ROUND_UP((S), I40E_MAX_DATA_PER_TXD)
 #define DESC_NEEDED (MAX_SKB_FRAGS + 4)
+#define I40E_MIN_DESC_PENDING	4
 
 #define I40E_TX_FLAGS_CSUM		(u32)(1)
 #define I40E_TX_FLAGS_HW_VLAN		(u32)(1 << 1)
diff --git a/drivers/net/ethernet/intel/i40evf/i40evf_main.c b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
index 38429fae4fcf..c51bc7a33bc5 100644
--- a/drivers/net/ethernet/intel/i40evf/i40evf_main.c
+++ b/drivers/net/ethernet/intel/i40evf/i40evf_main.c
@@ -36,7 +36,7 @@ char i40evf_driver_name[] = "i40evf";
 static const char i40evf_driver_string[] =
 	"Intel(R) XL710/X710 Virtual Function Network Driver";
 
-#define DRV_VERSION "0.9.40"
+#define DRV_VERSION "1.0.5"
 const char i40evf_driver_version[] = DRV_VERSION;
 static const char i40evf_copyright[] =
 	"Copyright (c) 2013 - 2014 Intel Corporation.";
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.c b/drivers/net/ethernet/intel/igb/e1000_82575.c
index 236a6183a865..051ea94bdcd3 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.c
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.c
@@ -2548,11 +2548,13 @@ s32 igb_read_emi_reg(struct e1000_hw *hw, u16 addr, u16 *data)
 /**
  *  igb_set_eee_i350 - Enable/disable EEE support
  *  @hw: pointer to the HW structure
+ *  @adv1G: boolean flag enabling 1G EEE advertisement
+ *  @adv100m: boolean flag enabling 100M EEE advertisement
  *
  *  Enable/disable EEE based on setting in dev_spec structure.
  *
  **/
-s32 igb_set_eee_i350(struct e1000_hw *hw)
+s32 igb_set_eee_i350(struct e1000_hw *hw, bool adv1G, bool adv100M)
 {
 	u32 ipcnfg, eeer;
 
@@ -2566,7 +2568,16 @@ s32 igb_set_eee_i350(struct e1000_hw *hw)
 	if (!(hw->dev_spec._82575.eee_disable)) {
 		u32 eee_su = rd32(E1000_EEE_SU);
 
-		ipcnfg |= (E1000_IPCNFG_EEE_1G_AN | E1000_IPCNFG_EEE_100M_AN);
+		if (adv100M)
+			ipcnfg |= E1000_IPCNFG_EEE_100M_AN;
+		else
+			ipcnfg &= ~E1000_IPCNFG_EEE_100M_AN;
+
+		if (adv1G)
+			ipcnfg |= E1000_IPCNFG_EEE_1G_AN;
+		else
+			ipcnfg &= ~E1000_IPCNFG_EEE_1G_AN;
+
 		eeer |= (E1000_EEER_TX_LPI_EN | E1000_EEER_RX_LPI_EN |
 			E1000_EEER_LPI_FC);
 
@@ -2593,11 +2604,13 @@ out:
 /**
  *  igb_set_eee_i354 - Enable/disable EEE support
  *  @hw: pointer to the HW structure
+ *  @adv1G: boolean flag enabling 1G EEE advertisement
+ *  @adv100m: boolean flag enabling 100M EEE advertisement
  *
  *  Enable/disable EEE legacy mode based on setting in dev_spec structure.
  *
  **/
-s32 igb_set_eee_i354(struct e1000_hw *hw)
+s32 igb_set_eee_i354(struct e1000_hw *hw, bool adv1G, bool adv100M)
 {
 	struct e1000_phy_info *phy = &hw->phy;
 	s32 ret_val = 0;
@@ -2636,8 +2649,16 @@ s32 igb_set_eee_i354(struct e1000_hw *hw)
 		if (ret_val)
 			goto out;
 
-		phy_data |= E1000_EEE_ADV_100_SUPPORTED |
-			    E1000_EEE_ADV_1000_SUPPORTED;
+		if (adv100M)
+			phy_data |= E1000_EEE_ADV_100_SUPPORTED;
+		else
+			phy_data &= ~E1000_EEE_ADV_100_SUPPORTED;
+
+		if (adv1G)
+			phy_data |= E1000_EEE_ADV_1000_SUPPORTED;
+		else
+			phy_data &= ~E1000_EEE_ADV_1000_SUPPORTED;
+
 		ret_val = igb_write_xmdio_reg(hw, E1000_EEE_ADV_ADDR_I354,
 						E1000_EEE_ADV_DEV_I354,
 						phy_data);
diff --git a/drivers/net/ethernet/intel/igb/e1000_82575.h b/drivers/net/ethernet/intel/igb/e1000_82575.h
index b407c55738fa..2154aea7aa7e 100644
--- a/drivers/net/ethernet/intel/igb/e1000_82575.h
+++ b/drivers/net/ethernet/intel/igb/e1000_82575.h
@@ -263,8 +263,8 @@ void igb_vmdq_set_loopback_pf(struct e1000_hw *, bool);
 void igb_vmdq_set_replication_pf(struct e1000_hw *, bool);
 u16 igb_rxpbs_adjust_82580(u32 data);
 s32 igb_read_emi_reg(struct e1000_hw *, u16 addr, u16 *data);
-s32 igb_set_eee_i350(struct e1000_hw *);
-s32 igb_set_eee_i354(struct e1000_hw *);
+s32 igb_set_eee_i350(struct e1000_hw *, bool adv1G, bool adv100M);
+s32 igb_set_eee_i354(struct e1000_hw *, bool adv1G, bool adv100M);
 s32 igb_get_eee_status_i354(struct e1000_hw *hw, bool *status);
 
 #define E1000_I2C_THERMAL_SENSOR_ADDR	0xF8
diff --git a/drivers/net/ethernet/intel/igb/e1000_hw.h b/drivers/net/ethernet/intel/igb/e1000_hw.h
index ce55ea5d750c..2003b3756ba2 100644
--- a/drivers/net/ethernet/intel/igb/e1000_hw.h
+++ b/drivers/net/ethernet/intel/igb/e1000_hw.h
@@ -265,11 +265,6 @@ struct e1000_hw_stats {
 	u64 b2ogprc;
 };
 
-struct e1000_phy_stats {
-	u32 idle_errors;
-	u32 receive_errors;
-};
-
 struct e1000_host_mng_dhcp_cookie {
 	u32 signature;
 	u8  status;
diff --git a/drivers/net/ethernet/intel/igb/igb.h b/drivers/net/ethernet/intel/igb/igb.h
index 06102d1f7c03..82d891e183b1 100644
--- a/drivers/net/ethernet/intel/igb/igb.h
+++ b/drivers/net/ethernet/intel/igb/igb.h
@@ -403,7 +403,6 @@ struct igb_adapter {
 	struct e1000_hw hw;
 	struct e1000_hw_stats stats;
 	struct e1000_phy_info phy_info;
-	struct e1000_phy_stats phy_stats;
 
 	u32 test_icr;
 	struct igb_ring test_tx_ring;
diff --git a/drivers/net/ethernet/intel/igb/igb_ethtool.c b/drivers/net/ethernet/intel/igb/igb_ethtool.c
index c737d1f40838..02cfd3b14762 100644
--- a/drivers/net/ethernet/intel/igb/igb_ethtool.c
+++ b/drivers/net/ethernet/intel/igb/igb_ethtool.c
@@ -2675,6 +2675,7 @@ static int igb_set_eee(struct net_device *netdev,
 	struct igb_adapter *adapter = netdev_priv(netdev);
 	struct e1000_hw *hw = &adapter->hw;
 	struct ethtool_eee eee_curr;
+	bool adv1g_eee = true, adv100m_eee = true;
 	s32 ret_val;
 
 	if ((hw->mac.type < e1000_i350) ||
@@ -2701,12 +2702,14 @@ static int igb_set_eee(struct net_device *netdev,
 			return -EINVAL;
 		}
 
-		if (edata->advertised &
-		    ~(ADVERTISE_100_FULL | ADVERTISE_1000_FULL)) {
+		if (!edata->advertised || (edata->advertised &
+		    ~(ADVERTISE_100_FULL | ADVERTISE_1000_FULL))) {
 			dev_err(&adapter->pdev->dev,
-				"EEE Advertisement supports only 100Tx and or 100T full duplex\n");
+				"EEE Advertisement supports only 100Tx and/or 100T full duplex\n");
 			return -EINVAL;
 		}
+		adv100m_eee = !!(edata->advertised & ADVERTISE_100_FULL);
+		adv1g_eee = !!(edata->advertised & ADVERTISE_1000_FULL);
 
 	} else if (!edata->eee_enabled) {
 		dev_err(&adapter->pdev->dev,
@@ -2718,10 +2721,6 @@ static int igb_set_eee(struct net_device *netdev,
 	if (hw->dev_spec._82575.eee_disable != !edata->eee_enabled) {
 		hw->dev_spec._82575.eee_disable = !edata->eee_enabled;
 		adapter->flags |= IGB_FLAG_EEE;
-		if (hw->mac.type == e1000_i350)
-			igb_set_eee_i350(hw);
-		else
-			igb_set_eee_i354(hw);
 
 		/* reset link */
 		if (netif_running(netdev))
@@ -2730,6 +2729,17 @@ static int igb_set_eee(struct net_device *netdev,
 			igb_reset(adapter);
 	}
 
+	if (hw->mac.type == e1000_i354)
+		ret_val = igb_set_eee_i354(hw, adv1g_eee, adv100m_eee);
+	else
+		ret_val = igb_set_eee_i350(hw, adv1g_eee, adv100m_eee);
+
+	if (ret_val) {
+		dev_err(&adapter->pdev->dev,
+			"Problem setting EEE advertisement options\n");
+		return -EINVAL;
+	}
+
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/intel/igb/igb_main.c b/drivers/net/ethernet/intel/igb/igb_main.c
index cb14bbdfb056..a21b14495ebd 100644
--- a/drivers/net/ethernet/intel/igb/igb_main.c
+++ b/drivers/net/ethernet/intel/igb/igb_main.c
@@ -58,7 +58,7 @@
 
 #define MAJ 5
 #define MIN 2
-#define BUILD 13
+#define BUILD 15
 #define DRV_VERSION __stringify(MAJ) "." __stringify(MIN) "." \
 __stringify(BUILD) "-k"
 char igb_driver_name[] = "igb";
@@ -2012,10 +2012,10 @@ void igb_reset(struct igb_adapter *adapter)
 		case e1000_i350:
 		case e1000_i210:
 		case e1000_i211:
-			igb_set_eee_i350(hw);
+			igb_set_eee_i350(hw, true, true);
 			break;
 		case e1000_i354:
-			igb_set_eee_i354(hw);
+			igb_set_eee_i354(hw, true, true);
 			break;
 		default:
 			break;
@@ -2619,7 +2619,7 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		case e1000_i210:
 		case e1000_i211:
 			/* Enable EEE for internal copper PHY devices */
-			err = igb_set_eee_i350(hw);
+			err = igb_set_eee_i350(hw, true, true);
 			if ((!err) &&
 			    (!hw->dev_spec._82575.eee_disable)) {
 				adapter->eee_advert =
@@ -2630,7 +2630,7 @@ static int igb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		case e1000_i354:
 			if ((rd32(E1000_CTRL_EXT) &
 			    E1000_CTRL_EXT_LINK_MODE_SGMII)) {
-				err = igb_set_eee_i354(hw);
+				err = igb_set_eee_i354(hw, true, true);
 				if ((!err) &&
 					(!hw->dev_spec._82575.eee_disable)) {
 					adapter->eee_advert =
@@ -4813,6 +4813,41 @@ static void igb_tx_olinfo_status(struct igb_ring *tx_ring,
 	tx_desc->read.olinfo_status = cpu_to_le32(olinfo_status);
 }
 
+static int __igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
+{
+	struct net_device *netdev = tx_ring->netdev;
+
+	netif_stop_subqueue(netdev, tx_ring->queue_index);
+
+	/* Herbert's original patch had:
+	 *  smp_mb__after_netif_stop_queue();
+	 * but since that doesn't exist yet, just open code it.
+	 */
+	smp_mb();
+
+	/* We need to check again in a case another CPU has just
+	 * made room available.
+	 */
+	if (igb_desc_unused(tx_ring) < size)
+		return -EBUSY;
+
+	/* A reprieve! */
+	netif_wake_subqueue(netdev, tx_ring->queue_index);
+
+	u64_stats_update_begin(&tx_ring->tx_syncp2);
+	tx_ring->tx_stats.restart_queue2++;
+	u64_stats_update_end(&tx_ring->tx_syncp2);
+
+	return 0;
+}
+
+static inline int igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
+{
+	if (igb_desc_unused(tx_ring) >= size)
+		return 0;
+	return __igb_maybe_stop_tx(tx_ring, size);
+}
+
 static void igb_tx_map(struct igb_ring *tx_ring,
 		       struct igb_tx_buffer *first,
 		       const u8 hdr_len)
@@ -4915,13 +4950,17 @@ static void igb_tx_map(struct igb_ring *tx_ring,
 
 	tx_ring->next_to_use = i;
 
-	writel(i, tx_ring->tail);
+	/* Make sure there is space in the ring for the next send. */
+	igb_maybe_stop_tx(tx_ring, DESC_NEEDED);
 
-	/* we need this if more than one processor can write to our tail
-	 * at a time, it synchronizes IO on IA64/Altix systems
-	 */
-	mmiowb();
+	if (netif_xmit_stopped(txring_txq(tx_ring)) || !skb->xmit_more) {
+		writel(i, tx_ring->tail);
 
+		/* we need this if more than one processor can write to our tail
+		 * at a time, it synchronizes IO on IA64/Altix systems
+		 */
+		mmiowb();
+	}
 	return;
 
 dma_error:
@@ -4941,41 +4980,6 @@ dma_error:
 	tx_ring->next_to_use = i;
 }
 
-static int __igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
-{
-	struct net_device *netdev = tx_ring->netdev;
-
-	netif_stop_subqueue(netdev, tx_ring->queue_index);
-
-	/* Herbert's original patch had:
-	 *  smp_mb__after_netif_stop_queue();
-	 * but since that doesn't exist yet, just open code it.
-	 */
-	smp_mb();
-
-	/* We need to check again in a case another CPU has just
-	 * made room available.
-	 */
-	if (igb_desc_unused(tx_ring) < size)
-		return -EBUSY;
-
-	/* A reprieve! */
-	netif_wake_subqueue(netdev, tx_ring->queue_index);
-
-	u64_stats_update_begin(&tx_ring->tx_syncp2);
-	tx_ring->tx_stats.restart_queue2++;
-	u64_stats_update_end(&tx_ring->tx_syncp2);
-
-	return 0;
-}
-
-static inline int igb_maybe_stop_tx(struct igb_ring *tx_ring, const u16 size)
-{
-	if (igb_desc_unused(tx_ring) >= size)
-		return 0;
-	return __igb_maybe_stop_tx(tx_ring, size);
-}
-
 netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 				struct igb_ring *tx_ring)
 {
@@ -5046,9 +5050,6 @@ netdev_tx_t igb_xmit_frame_ring(struct sk_buff *skb,
 
 	igb_tx_map(tx_ring, first, hdr_len);
 
-	/* Make sure there is space in the ring for the next send. */
-	igb_maybe_stop_tx(tx_ring, DESC_NEEDED);
-
 	return NETDEV_TX_OK;
 
 out_drop:
@@ -5205,14 +5206,11 @@ void igb_update_stats(struct igb_adapter *adapter,
 	struct e1000_hw *hw = &adapter->hw;
 	struct pci_dev *pdev = adapter->pdev;
 	u32 reg, mpc;
-	u16 phy_tmp;
 	int i;
 	u64 bytes, packets;
 	unsigned int start;
 	u64 _bytes, _packets;
 
-#define PHY_IDLE_ERROR_COUNT_MASK 0x00FF
-
 	/* Prevent stats update while adapter is being reset, or if the pci
 	 * connection is down.
 	 */
@@ -5373,15 +5371,6 @@ void igb_update_stats(struct igb_adapter *adapter,
 
 	/* Tx Dropped needs to be maintained elsewhere */
 
-	/* Phy Stats */
-	if (hw->phy.media_type == e1000_media_type_copper) {
-		if ((adapter->link_speed == SPEED_1000) &&
-		   (!igb_read_phy_reg(hw, PHY_1000T_STATUS, &phy_tmp))) {
-			phy_tmp &= PHY_IDLE_ERROR_COUNT_MASK;
-			adapter->phy_stats.idle_errors += phy_tmp;
-		}
-	}
-
 	/* Management Stats */
 	adapter->stats.mgptc += rd32(E1000_MGTPTC);
 	adapter->stats.mgprc += rd32(E1000_MGTPRC);
@@ -6385,7 +6374,7 @@ static bool igb_clean_tx_irq(struct igb_q_vector *q_vector)
 		total_packets += tx_buffer->gso_segs;
 
 		/* free the skb */
-		dev_kfree_skb_any(tx_buffer->skb);
+		dev_consume_skb_any(tx_buffer->skb);
 
 		/* unmap skb header data */
 		dma_unmap_single(tx_ring->dev,
@@ -6556,11 +6545,10 @@ static bool igb_can_reuse_rx_page(struct igb_rx_buffer *rx_buffer,
 	/* flip page offset to other buffer */
 	rx_buffer->page_offset ^= IGB_RX_BUFSZ;
 
-	/* since we are the only owner of the page and we need to
-	 * increment it, just set the value to 2 in order to avoid
-	 * an unnecessary locked operation
+	/* Even if we own the page, we are not allowed to use atomic_set()
+	 * This would break get_page_unless_zero() users.
 	 */
-	atomic_set(&page->_count, 2);
+	atomic_inc(&page->_count);
 #else
 	/* move offset up to the next cache line */
 	rx_buffer->page_offset += truesize;
@@ -6768,113 +6756,6 @@ static bool igb_is_non_eop(struct igb_ring *rx_ring,
 }
 
 /**
- *  igb_get_headlen - determine size of header for LRO/GRO
- *  @data: pointer to the start of the headers
- *  @max_len: total length of section to find headers in
- *
- *  This function is meant to determine the length of headers that will
- *  be recognized by hardware for LRO, and GRO offloads.  The main
- *  motivation of doing this is to only perform one pull for IPv4 TCP
- *  packets so that we can do basic things like calculating the gso_size
- *  based on the average data per packet.
- **/
-static unsigned int igb_get_headlen(unsigned char *data,
-				    unsigned int max_len)
-{
-	union {
-		unsigned char *network;
-		/* l2 headers */
-		struct ethhdr *eth;
-		struct vlan_hdr *vlan;
-		/* l3 headers */
-		struct iphdr *ipv4;
-		struct ipv6hdr *ipv6;
-	} hdr;
-	__be16 protocol;
-	u8 nexthdr = 0;	/* default to not TCP */
-	u8 hlen;
-
-	/* this should never happen, but better safe than sorry */
-	if (max_len < ETH_HLEN)
-		return max_len;
-
-	/* initialize network frame pointer */
-	hdr.network = data;
-
-	/* set first protocol and move network header forward */
-	protocol = hdr.eth->h_proto;
-	hdr.network += ETH_HLEN;
-
-	/* handle any vlan tag if present */
-	if (protocol == htons(ETH_P_8021Q)) {
-		if ((hdr.network - data) > (max_len - VLAN_HLEN))
-			return max_len;
-
-		protocol = hdr.vlan->h_vlan_encapsulated_proto;
-		hdr.network += VLAN_HLEN;
-	}
-
-	/* handle L3 protocols */
-	if (protocol == htons(ETH_P_IP)) {
-		if ((hdr.network - data) > (max_len - sizeof(struct iphdr)))
-			return max_len;
-
-		/* access ihl as a u8 to avoid unaligned access on ia64 */
-		hlen = (hdr.network[0] & 0x0F) << 2;
-
-		/* verify hlen meets minimum size requirements */
-		if (hlen < sizeof(struct iphdr))
-			return hdr.network - data;
-
-		/* record next protocol if header is present */
-		if (!(hdr.ipv4->frag_off & htons(IP_OFFSET)))
-			nexthdr = hdr.ipv4->protocol;
-	} else if (protocol == htons(ETH_P_IPV6)) {
-		if ((hdr.network - data) > (max_len - sizeof(struct ipv6hdr)))
-			return max_len;
-
-		/* record next protocol */
-		nexthdr = hdr.ipv6->nexthdr;
-		hlen = sizeof(struct ipv6hdr);
-	} else {
-		return hdr.network - data;
-	}
-
-	/* relocate pointer to start of L4 header */
-	hdr.network += hlen;
-
-	/* finally sort out TCP */
-	if (nexthdr == IPPROTO_TCP) {
-		if ((hdr.network - data) > (max_len - sizeof(struct tcphdr)))
-			return max_len;
-
-		/* access doff as a u8 to avoid unaligned access on ia64 */
-		hlen = (hdr.network[12] & 0xF0) >> 2;
-
-		/* verify hlen meets minimum size requirements */
-		if (hlen < sizeof(struct tcphdr))
-			return hdr.network - data;
-
-		hdr.network += hlen;
-	} else if (nexthdr == IPPROTO_UDP) {
-		if ((hdr.network - data) > (max_len - sizeof(struct udphdr)))
-			return max_len;
-
-		hdr.network += sizeof(struct udphdr);
-	}
-
-	/* If everything has gone correctly hdr.network should be the
-	 * data section of the packet and will be the end of the header.
-	 * If not then it probably represents the end of the last recognized
-	 * header.
-	 */
-	if ((hdr.network - data) < max_len)
-		return hdr.network - data;
-	else
-		return max_len;
-}
-
-/**
  *  igb_pull_tail - igb specific version of skb_pull_tail
  *  @rx_ring: rx descriptor ring packet is being transacted on
  *  @rx_desc: pointer to the EOP Rx descriptor
@@ -6918,7 +6799,7 @@ static void igb_pull_tail(struct igb_ring *rx_ring,
 	/* we need the header to contain the greater of either ETH_HLEN or
 	 * 60 bytes if the skb->len is less than 60 for skb_pad.
 	 */
-	pull_len = igb_get_headlen(va, IGB_RX_HDR_LEN);
+	pull_len = eth_get_headlen(va, IGB_RX_HDR_LEN);
 
 	/* align pull length to size of long to optimize memcpy performance */
 	skb_copy_to_linear_data(skb, va, ALIGN(pull_len, sizeof(long)));
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe.h b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
index ac9f2148cdc5..5032a602d5c9 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe.h
@@ -307,7 +307,6 @@ enum ixgbe_ring_f_enum {
 #define MAX_RX_QUEUES (IXGBE_MAX_FDIR_INDICES + 1)
 #define MAX_TX_QUEUES (IXGBE_MAX_FDIR_INDICES + 1)
 #define IXGBE_MAX_L2A_QUEUES 4
-#define IXGBE_MAX_L2A_QUEUES 4
 #define IXGBE_BAD_L2A_QUEUE 3
 #define IXGBE_MAX_MACVLANS	31
 #define IXGBE_MAX_DCBMACVLANS	8
@@ -386,119 +385,87 @@ struct ixgbe_q_vector {
 	char name[IFNAMSIZ + 9];
 
 #ifdef CONFIG_NET_RX_BUSY_POLL
-	unsigned int state;
-#define IXGBE_QV_STATE_IDLE        0
-#define IXGBE_QV_STATE_NAPI	   1     /* NAPI owns this QV */
-#define IXGBE_QV_STATE_POLL	   2     /* poll owns this QV */
-#define IXGBE_QV_STATE_DISABLED	   4     /* QV is disabled */
-#define IXGBE_QV_OWNED (IXGBE_QV_STATE_NAPI | IXGBE_QV_STATE_POLL)
-#define IXGBE_QV_LOCKED (IXGBE_QV_OWNED | IXGBE_QV_STATE_DISABLED)
-#define IXGBE_QV_STATE_NAPI_YIELD  8     /* NAPI yielded this QV */
-#define IXGBE_QV_STATE_POLL_YIELD  16    /* poll yielded this QV */
-#define IXGBE_QV_YIELD (IXGBE_QV_STATE_NAPI_YIELD | IXGBE_QV_STATE_POLL_YIELD)
-#define IXGBE_QV_USER_PEND (IXGBE_QV_STATE_POLL | IXGBE_QV_STATE_POLL_YIELD)
-	spinlock_t lock;
+	atomic_t state;
 #endif  /* CONFIG_NET_RX_BUSY_POLL */
 
 	/* for dynamic allocation of rings associated with this q_vector */
 	struct ixgbe_ring ring[0] ____cacheline_internodealigned_in_smp;
 };
+
 #ifdef CONFIG_NET_RX_BUSY_POLL
+enum ixgbe_qv_state_t {
+	IXGBE_QV_STATE_IDLE = 0,
+	IXGBE_QV_STATE_NAPI,
+	IXGBE_QV_STATE_POLL,
+	IXGBE_QV_STATE_DISABLE
+};
+
 static inline void ixgbe_qv_init_lock(struct ixgbe_q_vector *q_vector)
 {
-
-	spin_lock_init(&q_vector->lock);
-	q_vector->state = IXGBE_QV_STATE_IDLE;
+	/* reset state to idle */
+	atomic_set(&q_vector->state, IXGBE_QV_STATE_IDLE);
 }
 
 /* called from the device poll routine to get ownership of a q_vector */
 static inline bool ixgbe_qv_lock_napi(struct ixgbe_q_vector *q_vector)
 {
-	int rc = true;
-	spin_lock_bh(&q_vector->lock);
-	if (q_vector->state & IXGBE_QV_LOCKED) {
-		WARN_ON(q_vector->state & IXGBE_QV_STATE_NAPI);
-		q_vector->state |= IXGBE_QV_STATE_NAPI_YIELD;
-		rc = false;
+	int rc = atomic_cmpxchg(&q_vector->state, IXGBE_QV_STATE_IDLE,
+				IXGBE_QV_STATE_NAPI);
 #ifdef BP_EXTENDED_STATS
+	if (rc != IXGBE_QV_STATE_IDLE)
 		q_vector->tx.ring->stats.yields++;
 #endif
-	} else {
-		/* we don't care if someone yielded */
-		q_vector->state = IXGBE_QV_STATE_NAPI;
-	}
-	spin_unlock_bh(&q_vector->lock);
-	return rc;
+
+	return rc == IXGBE_QV_STATE_IDLE;
 }
 
 /* returns true is someone tried to get the qv while napi had it */
-static inline bool ixgbe_qv_unlock_napi(struct ixgbe_q_vector *q_vector)
+static inline void ixgbe_qv_unlock_napi(struct ixgbe_q_vector *q_vector)
 {
-	int rc = false;
-	spin_lock_bh(&q_vector->lock);
-	WARN_ON(q_vector->state & (IXGBE_QV_STATE_POLL |
-			       IXGBE_QV_STATE_NAPI_YIELD));
-
-	if (q_vector->state & IXGBE_QV_STATE_POLL_YIELD)
-		rc = true;
-	/* will reset state to idle, unless QV is disabled */
-	q_vector->state &= IXGBE_QV_STATE_DISABLED;
-	spin_unlock_bh(&q_vector->lock);
-	return rc;
+	WARN_ON(atomic_read(&q_vector->state) != IXGBE_QV_STATE_NAPI);
+
+	/* flush any outstanding Rx frames */
+	if (q_vector->napi.gro_list)
+		napi_gro_flush(&q_vector->napi, false);
+
+	/* reset state to idle */
+	atomic_set(&q_vector->state, IXGBE_QV_STATE_IDLE);
 }
 
 /* called from ixgbe_low_latency_poll() */
 static inline bool ixgbe_qv_lock_poll(struct ixgbe_q_vector *q_vector)
 {
-	int rc = true;
-	spin_lock_bh(&q_vector->lock);
-	if ((q_vector->state & IXGBE_QV_LOCKED)) {
-		q_vector->state |= IXGBE_QV_STATE_POLL_YIELD;
-		rc = false;
+	int rc = atomic_cmpxchg(&q_vector->state, IXGBE_QV_STATE_IDLE,
+				IXGBE_QV_STATE_POLL);
 #ifdef BP_EXTENDED_STATS
-		q_vector->rx.ring->stats.yields++;
+	if (rc != IXGBE_QV_STATE_IDLE)
+		q_vector->tx.ring->stats.yields++;
 #endif
-	} else {
-		/* preserve yield marks */
-		q_vector->state |= IXGBE_QV_STATE_POLL;
-	}
-	spin_unlock_bh(&q_vector->lock);
-	return rc;
+	return rc == IXGBE_QV_STATE_IDLE;
 }
 
 /* returns true if someone tried to get the qv while it was locked */
-static inline bool ixgbe_qv_unlock_poll(struct ixgbe_q_vector *q_vector)
+static inline void ixgbe_qv_unlock_poll(struct ixgbe_q_vector *q_vector)
 {
-	int rc = false;
-	spin_lock_bh(&q_vector->lock);
-	WARN_ON(q_vector->state & (IXGBE_QV_STATE_NAPI));
-
-	if (q_vector->state & IXGBE_QV_STATE_POLL_YIELD)
-		rc = true;
-	/* will reset state to idle, unless QV is disabled */
-	q_vector->state &= IXGBE_QV_STATE_DISABLED;
-	spin_unlock_bh(&q_vector->lock);
-	return rc;
+	WARN_ON(atomic_read(&q_vector->state) != IXGBE_QV_STATE_POLL);
+
+	/* reset state to idle */
+	atomic_set(&q_vector->state, IXGBE_QV_STATE_IDLE);
 }
 
 /* true if a socket is polling, even if it did not get the lock */
 static inline bool ixgbe_qv_busy_polling(struct ixgbe_q_vector *q_vector)
 {
-	WARN_ON(!(q_vector->state & IXGBE_QV_OWNED));
-	return q_vector->state & IXGBE_QV_USER_PEND;
+	return atomic_read(&q_vector->state) == IXGBE_QV_STATE_POLL;
 }
 
 /* false if QV is currently owned */
 static inline bool ixgbe_qv_disable(struct ixgbe_q_vector *q_vector)
 {
-	int rc = true;
-	spin_lock_bh(&q_vector->lock);
-	if (q_vector->state & IXGBE_QV_OWNED)
-		rc = false;
-	q_vector->state |= IXGBE_QV_STATE_DISABLED;
-	spin_unlock_bh(&q_vector->lock);
-
-	return rc;
+	int rc = atomic_cmpxchg(&q_vector->state, IXGBE_QV_STATE_IDLE,
+				IXGBE_QV_STATE_DISABLE);
+
+	return rc == IXGBE_QV_STATE_IDLE;
 }
 
 #else /* CONFIG_NET_RX_BUSY_POLL */
@@ -643,9 +610,7 @@ struct ixgbe_adapter {
 	 * thus the additional *_CAPABLE flags.
 	 */
 	u32 flags;
-#define IXGBE_FLAG_MSI_CAPABLE                  (u32)(1 << 0)
 #define IXGBE_FLAG_MSI_ENABLED                  (u32)(1 << 1)
-#define IXGBE_FLAG_MSIX_CAPABLE                 (u32)(1 << 2)
 #define IXGBE_FLAG_MSIX_ENABLED                 (u32)(1 << 3)
 #define IXGBE_FLAG_RX_1BUF_CAPABLE              (u32)(1 << 4)
 #define IXGBE_FLAG_RX_PS_CAPABLE                (u32)(1 << 5)
@@ -760,8 +725,6 @@ struct ixgbe_adapter {
 	u8 __iomem *io_addr; /* Mainly for iounmap use */
 	u32 wol;
 
-	u16 bd_number;
-
 	u16 eeprom_verh;
 	u16 eeprom_verl;
 	u16 eeprom_cap;
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
index e4100b5737b6..3ce4a258f945 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_ethtool.c
@@ -1303,7 +1303,7 @@ static const struct ixgbe_reg_test reg_test_82599[] = {
 	{ IXGBE_RAL(0), 16, TABLE64_TEST_LO, 0xFFFFFFFF, 0xFFFFFFFF },
 	{ IXGBE_RAL(0), 16, TABLE64_TEST_HI, 0x8001FFFF, 0x800CFFFF },
 	{ IXGBE_MTA(0), 128, TABLE32_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ 0, 0, 0, 0 }
+	{ .reg = 0 }
 };
 
 /* default 82598 register test */
@@ -1331,7 +1331,7 @@ static const struct ixgbe_reg_test reg_test_82598[] = {
 	{ IXGBE_RAL(0), 16, TABLE64_TEST_LO, 0xFFFFFFFF, 0xFFFFFFFF },
 	{ IXGBE_RAL(0), 16, TABLE64_TEST_HI, 0x800CFFFF, 0x800CFFFF },
 	{ IXGBE_MTA(0), 128, TABLE32_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
-	{ 0, 0, 0, 0 }
+	{ .reg = 0 }
 };
 
 static bool reg_pattern_test(struct ixgbe_adapter *adapter, u64 *data, int reg,
@@ -2267,7 +2267,6 @@ static int ixgbe_set_coalesce(struct net_device *netdev,
 	if (adapter->q_vector[0]->tx.count && adapter->q_vector[0]->rx.count)
 		adapter->tx_itr_setting = adapter->rx_itr_setting;
 
-#if IS_ENABLED(CONFIG_BQL)
 	/* detect ITR changes that require update of TXDCTL.WTHRESH */
 	if ((adapter->tx_itr_setting != 1) &&
 	    (adapter->tx_itr_setting < IXGBE_100K_ITR)) {
@@ -2279,7 +2278,7 @@ static int ixgbe_set_coalesce(struct net_device *netdev,
 		    (tx_itr_prev < IXGBE_100K_ITR))
 			need_reset = true;
 	}
-#endif
+
 	/* check the old value and enable RSC if necessary */
 	need_reset |= ixgbe_update_rsc(adapter);
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
index 2d9451e39686..ce40c77381e9 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_lib.c
@@ -696,46 +696,83 @@ static void ixgbe_set_num_queues(struct ixgbe_adapter *adapter)
 	ixgbe_set_rss_queues(adapter);
 }
 
-static void ixgbe_acquire_msix_vectors(struct ixgbe_adapter *adapter,
-				       int vectors)
+/**
+ * ixgbe_acquire_msix_vectors - acquire MSI-X vectors
+ * @adapter: board private structure
+ *
+ * Attempts to acquire a suitable range of MSI-X vector interrupts. Will
+ * return a negative error code if unable to acquire MSI-X vectors for any
+ * reason.
+ */
+static int ixgbe_acquire_msix_vectors(struct ixgbe_adapter *adapter)
 {
-	int vector_threshold;
+	struct ixgbe_hw *hw = &adapter->hw;
+	int i, vectors, vector_threshold;
+
+	/* We start by asking for one vector per queue pair */
+	vectors = max(adapter->num_rx_queues, adapter->num_tx_queues);
 
-	/* We'll want at least 2 (vector_threshold):
-	 * 1) TxQ[0] + RxQ[0] handler
-	 * 2) Other (Link Status Change, etc.)
+	/* It is easy to be greedy for MSI-X vectors. However, it really
+	 * doesn't do much good if we have a lot more vectors than CPUs. We'll
+	 * be somewhat conservative and only ask for (roughly) the same number
+	 * of vectors as there are CPUs.
 	 */
-	vector_threshold = MIN_MSIX_COUNT;
+	vectors = min_t(int, vectors, num_online_cpus());
 
-	/*
-	 * The more we get, the more we will assign to Tx/Rx Cleanup
-	 * for the separate queues...where Rx Cleanup >= Tx Cleanup.
-	 * Right now, we simply care about how many we'll get; we'll
-	 * set them up later while requesting irq's.
+	/* Some vectors are necessary for non-queue interrupts */
+	vectors += NON_Q_VECTORS;
+
+	/* Hardware can only support a maximum of hw.mac->max_msix_vectors.
+	 * With features such as RSS and VMDq, we can easily surpass the
+	 * number of Rx and Tx descriptor queues supported by our device.
+	 * Thus, we cap the maximum in the rare cases where the CPU count also
+	 * exceeds our vector limit
 	 */
+	vectors = min_t(int, vectors, hw->mac.max_msix_vectors);
+
+	/* We want a minimum of two MSI-X vectors for (1) a TxQ[0] + RxQ[0]
+	 * handler, and (2) an Other (Link Status Change, etc.) handler.
+	 */
+	vector_threshold = MIN_MSIX_COUNT;
+
+	adapter->msix_entries = kcalloc(vectors,
+					sizeof(struct msix_entry),
+					GFP_KERNEL);
+	if (!adapter->msix_entries)
+		return -ENOMEM;
+
+	for (i = 0; i < vectors; i++)
+		adapter->msix_entries[i].entry = i;
+
 	vectors = pci_enable_msix_range(adapter->pdev, adapter->msix_entries,
 					vector_threshold, vectors);
 
 	if (vectors < 0) {
-		/* Can't allocate enough MSI-X interrupts?  Oh well.
-		 * This just means we'll go with either a single MSI
-		 * vector or fall back to legacy interrupts.
+		/* A negative count of allocated vectors indicates an error in
+		 * acquiring within the specified range of MSI-X vectors
 		 */
-		netif_printk(adapter, hw, KERN_DEBUG, adapter->netdev,
-			     "Unable to allocate MSI-X interrupts\n");
+		e_dev_warn("Failed to allocate MSI-X interrupts. Err: %d\n",
+			   vectors);
+
 		adapter->flags &= ~IXGBE_FLAG_MSIX_ENABLED;
 		kfree(adapter->msix_entries);
 		adapter->msix_entries = NULL;
-	} else {
-		adapter->flags |= IXGBE_FLAG_MSIX_ENABLED; /* Woot! */
-		/*
-		 * Adjust for only the vectors we'll use, which is minimum
-		 * of max_msix_q_vectors + NON_Q_VECTORS, or the number of
-		 * vectors we were allocated.
-		 */
-		vectors -= NON_Q_VECTORS;
-		adapter->num_q_vectors = min(vectors, adapter->max_q_vectors);
+
+		return vectors;
 	}
+
+	/* we successfully allocated some number of vectors within our
+	 * requested range.
+	 */
+	adapter->flags |= IXGBE_FLAG_MSIX_ENABLED;
+
+	/* Adjust for only the vectors we'll use, which is minimum
+	 * of max_q_vectors, or the number of vectors we were allocated.
+	 */
+	vectors -= NON_Q_VECTORS;
+	adapter->num_q_vectors = min_t(int, vectors, adapter->max_q_vectors);
+
+	return 0;
 }
 
 static void ixgbe_add_ring(struct ixgbe_ring *ring,
@@ -807,6 +844,11 @@ static int ixgbe_alloc_q_vector(struct ixgbe_adapter *adapter,
 		       ixgbe_poll, 64);
 	napi_hash_add(&q_vector->napi);
 
+#ifdef CONFIG_NET_RX_BUSY_POLL
+	/* initialize busy poll */
+	atomic_set(&q_vector->state, IXGBE_QV_STATE_DISABLE);
+
+#endif
 	/* tie q_vector and adapter together */
 	adapter->q_vector[v_idx] = q_vector;
 	q_vector->adapter = adapter;
@@ -1049,46 +1091,20 @@ static void ixgbe_reset_interrupt_capability(struct ixgbe_adapter *adapter)
  **/
 static void ixgbe_set_interrupt_capability(struct ixgbe_adapter *adapter)
 {
-	struct ixgbe_hw *hw = &adapter->hw;
-	int vector, v_budget, err;
+	int err;
 
-	/*
-	 * It's easy to be greedy for MSI-X vectors, but it really
-	 * doesn't do us much good if we have a lot more vectors
-	 * than CPU's.  So let's be conservative and only ask for
-	 * (roughly) the same number of vectors as there are CPU's.
-	 * The default is to use pairs of vectors.
-	 */
-	v_budget = max(adapter->num_rx_queues, adapter->num_tx_queues);
-	v_budget = min_t(int, v_budget, num_online_cpus());
-	v_budget += NON_Q_VECTORS;
+	/* We will try to get MSI-X interrupts first */
+	if (!ixgbe_acquire_msix_vectors(adapter))
+		return;
 
-	/*
-	 * At the same time, hardware can only support a maximum of
-	 * hw.mac->max_msix_vectors vectors.  With features
-	 * such as RSS and VMDq, we can easily surpass the number of Rx and Tx
-	 * descriptor queues supported by our device.  Thus, we cap it off in
-	 * those rare cases where the cpu count also exceeds our vector limit.
+	/* At this point, we do not have MSI-X capabilities. We need to
+	 * reconfigure or disable various features which require MSI-X
+	 * capability.
 	 */
-	v_budget = min_t(int, v_budget, hw->mac.max_msix_vectors);
-
-	/* A failure in MSI-X entry allocation isn't fatal, but it does
-	 * mean we disable MSI-X capabilities of the adapter. */
-	adapter->msix_entries = kcalloc(v_budget,
-					sizeof(struct msix_entry), GFP_KERNEL);
-	if (adapter->msix_entries) {
-		for (vector = 0; vector < v_budget; vector++)
-			adapter->msix_entries[vector].entry = vector;
-
-		ixgbe_acquire_msix_vectors(adapter, v_budget);
-
-		if (adapter->flags & IXGBE_FLAG_MSIX_ENABLED)
-			return;
-	}
 
-	/* disable DCB if number of TCs exceeds 1 */
+	/* Disable DCB unless we only have a single traffic class */
 	if (netdev_get_num_tc(adapter->netdev) > 1) {
-		e_err(probe, "num TCs exceeds number of queues - disabling DCB\n");
+		e_dev_warn("Number of DCB TCs exceeds number of available queues. Disabling DCB support.\n");
 		netdev_reset_tc(adapter->netdev);
 
 		if (adapter->hw.mac.type == ixgbe_mac_82598EB)
@@ -1098,26 +1114,30 @@ static void ixgbe_set_interrupt_capability(struct ixgbe_adapter *adapter)
 		adapter->temp_dcb_cfg.pfc_mode_enable = false;
 		adapter->dcb_cfg.pfc_mode_enable = false;
 	}
+
 	adapter->dcb_cfg.num_tcs.pg_tcs = 1;
 	adapter->dcb_cfg.num_tcs.pfc_tcs = 1;
 
-	/* disable SR-IOV */
+	/* Disable SR-IOV support */
+	e_dev_warn("Disabling SR-IOV support\n");
 	ixgbe_disable_sriov(adapter);
 
-	/* disable RSS */
+	/* Disable RSS */
+	e_dev_warn("Disabling RSS support\n");
 	adapter->ring_feature[RING_F_RSS].limit = 1;
 
+	/* recalculate number of queues now that many features have been
+	 * changed or disabled.
+	 */
 	ixgbe_set_num_queues(adapter);
 	adapter->num_q_vectors = 1;
 
 	err = pci_enable_msi(adapter->pdev);
-	if (err) {
-		netif_printk(adapter, hw, KERN_DEBUG, adapter->netdev,
-			     "Unable to allocate MSI interrupt, falling back to legacy.  Error: %d\n",
-			     err);
-		return;
-	}
-	adapter->flags |= IXGBE_FLAG_MSI_ENABLED;
+	if (err)
+		e_dev_warn("Failed to allocate MSI interrupt, falling back to legacy. Error: %d\n",
+			   err);
+	else
+		adapter->flags |= IXGBE_FLAG_MSI_ENABLED;
 }
 
 /**
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
index 87bd53fdd209..fec5212d4337 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_main.c
@@ -440,7 +440,7 @@ static const struct ixgbe_reg_info ixgbe_reg_info_tbl[] = {
 	{IXGBE_TXDCTL(0), "TXDCTL"},
 
 	/* List Terminator */
-	{}
+	{ .name = NULL }
 };
 
 
@@ -1094,7 +1094,7 @@ static bool ixgbe_clean_tx_irq(struct ixgbe_q_vector *q_vector,
 		total_packets += tx_buffer->gso_segs;
 
 		/* free the skb */
-		dev_kfree_skb_any(tx_buffer->skb);
+		dev_consume_skb_any(tx_buffer->skb);
 
 		/* unmap skb header data */
 		dma_unmap_single(tx_ring->dev,
@@ -1521,120 +1521,6 @@ void ixgbe_alloc_rx_buffers(struct ixgbe_ring *rx_ring, u16 cleaned_count)
 		ixgbe_release_rx_desc(rx_ring, i);
 }
 
-/**
- * ixgbe_get_headlen - determine size of header for RSC/LRO/GRO/FCOE
- * @data: pointer to the start of the headers
- * @max_len: total length of section to find headers in
- *
- * This function is meant to determine the length of headers that will
- * be recognized by hardware for LRO, GRO, and RSC offloads.  The main
- * motivation of doing this is to only perform one pull for IPv4 TCP
- * packets so that we can do basic things like calculating the gso_size
- * based on the average data per packet.
- **/
-static unsigned int ixgbe_get_headlen(unsigned char *data,
-				      unsigned int max_len)
-{
-	union {
-		unsigned char *network;
-		/* l2 headers */
-		struct ethhdr *eth;
-		struct vlan_hdr *vlan;
-		/* l3 headers */
-		struct iphdr *ipv4;
-		struct ipv6hdr *ipv6;
-	} hdr;
-	__be16 protocol;
-	u8 nexthdr = 0;	/* default to not TCP */
-	u8 hlen;
-
-	/* this should never happen, but better safe than sorry */
-	if (max_len < ETH_HLEN)
-		return max_len;
-
-	/* initialize network frame pointer */
-	hdr.network = data;
-
-	/* set first protocol and move network header forward */
-	protocol = hdr.eth->h_proto;
-	hdr.network += ETH_HLEN;
-
-	/* handle any vlan tag if present */
-	if (protocol == htons(ETH_P_8021Q)) {
-		if ((hdr.network - data) > (max_len - VLAN_HLEN))
-			return max_len;
-
-		protocol = hdr.vlan->h_vlan_encapsulated_proto;
-		hdr.network += VLAN_HLEN;
-	}
-
-	/* handle L3 protocols */
-	if (protocol == htons(ETH_P_IP)) {
-		if ((hdr.network - data) > (max_len - sizeof(struct iphdr)))
-			return max_len;
-
-		/* access ihl as a u8 to avoid unaligned access on ia64 */
-		hlen = (hdr.network[0] & 0x0F) << 2;
-
-		/* verify hlen meets minimum size requirements */
-		if (hlen < sizeof(struct iphdr))
-			return hdr.network - data;
-
-		/* record next protocol if header is present */
-		if (!(hdr.ipv4->frag_off & htons(IP_OFFSET)))
-			nexthdr = hdr.ipv4->protocol;
-	} else if (protocol == htons(ETH_P_IPV6)) {
-		if ((hdr.network - data) > (max_len - sizeof(struct ipv6hdr)))
-			return max_len;
-
-		/* record next protocol */
-		nexthdr = hdr.ipv6->nexthdr;
-		hlen = sizeof(struct ipv6hdr);
-#ifdef IXGBE_FCOE
-	} else if (protocol == htons(ETH_P_FCOE)) {
-		if ((hdr.network - data) > (max_len - FCOE_HEADER_LEN))
-			return max_len;
-		hlen = FCOE_HEADER_LEN;
-#endif
-	} else {
-		return hdr.network - data;
-	}
-
-	/* relocate pointer to start of L4 header */
-	hdr.network += hlen;
-
-	/* finally sort out TCP/UDP */
-	if (nexthdr == IPPROTO_TCP) {
-		if ((hdr.network - data) > (max_len - sizeof(struct tcphdr)))
-			return max_len;
-
-		/* access doff as a u8 to avoid unaligned access on ia64 */
-		hlen = (hdr.network[12] & 0xF0) >> 2;
-
-		/* verify hlen meets minimum size requirements */
-		if (hlen < sizeof(struct tcphdr))
-			return hdr.network - data;
-
-		hdr.network += hlen;
-	} else if (nexthdr == IPPROTO_UDP) {
-		if ((hdr.network - data) > (max_len - sizeof(struct udphdr)))
-			return max_len;
-
-		hdr.network += sizeof(struct udphdr);
-	}
-
-	/*
-	 * If everything has gone correctly hdr.network should be the
-	 * data section of the packet and will be the end of the header.
-	 * If not then it probably represents the end of the last recognized
-	 * header.
-	 */
-	if ((hdr.network - data) < max_len)
-		return hdr.network - data;
-	else
-		return max_len;
-}
-
 static void ixgbe_set_rsc_gso_size(struct ixgbe_ring *ring,
 				   struct sk_buff *skb)
 {
@@ -1793,7 +1679,7 @@ static void ixgbe_pull_tail(struct ixgbe_ring *rx_ring,
 	 * we need the header to contain the greater of either ETH_HLEN or
 	 * 60 bytes if the skb->len is less than 60 for skb_pad.
 	 */
-	pull_len = ixgbe_get_headlen(va, IXGBE_RX_HDR_SIZE);
+	pull_len = eth_get_headlen(va, IXGBE_RX_HDR_SIZE);
 
 	/* align pull length to size of long to optimize memcpy performance */
 	skb_copy_to_linear_data(skb, va, ALIGN(pull_len, sizeof(long)));
@@ -1979,12 +1865,10 @@ static bool ixgbe_add_rx_frag(struct ixgbe_ring *rx_ring,
 	/* flip page offset to other buffer */
 	rx_buffer->page_offset ^= truesize;
 
-	/*
-	 * since we are the only owner of the page and we need to
-	 * increment it, just set the value to 2 in order to avoid
-	 * an unecessary locked operation
+	/* Even if we own the page, we are not allowed to use atomic_set()
+	 * This would break get_page_unless_zero() users.
 	 */
-	atomic_set(&page->_count, 2);
+	atomic_inc(&page->_count);
 #else
 	/* move offset up to the next cache line */
 	rx_buffer->page_offset += truesize;
@@ -2191,9 +2075,6 @@ static int ixgbe_clean_rx_irq(struct ixgbe_q_vector *q_vector,
 	q_vector->rx.total_packets += total_rx_packets;
 	q_vector->rx.total_bytes += total_rx_bytes;
 
-	if (cleaned_count)
-		ixgbe_alloc_rx_buffers(rx_ring, cleaned_count);
-
 	return total_rx_packets;
 }
 
@@ -3099,11 +2980,7 @@ void ixgbe_configure_tx_ring(struct ixgbe_adapter *adapter,
 	 * to or less than the number of on chip descriptors, which is
 	 * currently 40.
 	 */
-#if IS_ENABLED(CONFIG_BQL)
 	if (!ring->q_vector || (ring->q_vector->itr < IXGBE_100K_ITR))
-#else
-	if (!ring->q_vector || (ring->q_vector->itr < 8))
-#endif
 		txdctl |= (1 << 16);	/* WTHRESH = 1 */
 	else
 		txdctl |= (8 << 16);	/* WTHRESH = 8 */
@@ -5300,15 +5177,15 @@ int ixgbe_setup_tx_resources(struct ixgbe_ring *tx_ring)
 {
 	struct device *dev = tx_ring->dev;
 	int orig_node = dev_to_node(dev);
-	int numa_node = -1;
+	int ring_node = -1;
 	int size;
 
 	size = sizeof(struct ixgbe_tx_buffer) * tx_ring->count;
 
 	if (tx_ring->q_vector)
-		numa_node = tx_ring->q_vector->numa_node;
+		ring_node = tx_ring->q_vector->numa_node;
 
-	tx_ring->tx_buffer_info = vzalloc_node(size, numa_node);
+	tx_ring->tx_buffer_info = vzalloc_node(size, ring_node);
 	if (!tx_ring->tx_buffer_info)
 		tx_ring->tx_buffer_info = vzalloc(size);
 	if (!tx_ring->tx_buffer_info)
@@ -5320,7 +5197,7 @@ int ixgbe_setup_tx_resources(struct ixgbe_ring *tx_ring)
 	tx_ring->size = tx_ring->count * sizeof(union ixgbe_adv_tx_desc);
 	tx_ring->size = ALIGN(tx_ring->size, 4096);
 
-	set_dev_node(dev, numa_node);
+	set_dev_node(dev, ring_node);
 	tx_ring->desc = dma_alloc_coherent(dev,
 					   tx_ring->size,
 					   &tx_ring->dma,
@@ -5384,15 +5261,15 @@ int ixgbe_setup_rx_resources(struct ixgbe_ring *rx_ring)
 {
 	struct device *dev = rx_ring->dev;
 	int orig_node = dev_to_node(dev);
-	int numa_node = -1;
+	int ring_node = -1;
 	int size;
 
 	size = sizeof(struct ixgbe_rx_buffer) * rx_ring->count;
 
 	if (rx_ring->q_vector)
-		numa_node = rx_ring->q_vector->numa_node;
+		ring_node = rx_ring->q_vector->numa_node;
 
-	rx_ring->rx_buffer_info = vzalloc_node(size, numa_node);
+	rx_ring->rx_buffer_info = vzalloc_node(size, ring_node);
 	if (!rx_ring->rx_buffer_info)
 		rx_ring->rx_buffer_info = vzalloc(size);
 	if (!rx_ring->rx_buffer_info)
@@ -5404,7 +5281,7 @@ int ixgbe_setup_rx_resources(struct ixgbe_ring *rx_ring)
 	rx_ring->size = rx_ring->count * sizeof(union ixgbe_adv_rx_desc);
 	rx_ring->size = ALIGN(rx_ring->size, 4096);
 
-	set_dev_node(dev, numa_node);
+	set_dev_node(dev, ring_node);
 	rx_ring->desc = dma_alloc_coherent(dev,
 					   rx_ring->size,
 					   &rx_ring->dma,
@@ -6319,25 +6196,55 @@ static void ixgbe_watchdog_link_is_down(struct ixgbe_adapter *adapter)
 	ixgbe_ping_all_vfs(adapter);
 }
 
+static bool ixgbe_ring_tx_pending(struct ixgbe_adapter *adapter)
+{
+	int i;
+
+	for (i = 0; i < adapter->num_tx_queues; i++) {
+		struct ixgbe_ring *tx_ring = adapter->tx_ring[i];
+
+		if (tx_ring->next_to_use != tx_ring->next_to_clean)
+			return true;
+	}
+
+	return false;
+}
+
+static bool ixgbe_vf_tx_pending(struct ixgbe_adapter *adapter)
+{
+	struct ixgbe_hw *hw = &adapter->hw;
+	struct ixgbe_ring_feature *vmdq = &adapter->ring_feature[RING_F_VMDQ];
+	u32 q_per_pool = __ALIGN_MASK(1, ~vmdq->mask);
+
+	int i, j;
+
+	if (!adapter->num_vfs)
+		return false;
+
+	for (i = 0; i < adapter->num_vfs; i++) {
+		for (j = 0; j < q_per_pool; j++) {
+			u32 h, t;
+
+			h = IXGBE_READ_REG(hw, IXGBE_PVFTDHN(q_per_pool, i, j));
+			t = IXGBE_READ_REG(hw, IXGBE_PVFTDTN(q_per_pool, i, j));
+
+			if (h != t)
+				return true;
+		}
+	}
+
+	return false;
+}
+
 /**
  * ixgbe_watchdog_flush_tx - flush queues on link down
  * @adapter: pointer to the device adapter structure
  **/
 static void ixgbe_watchdog_flush_tx(struct ixgbe_adapter *adapter)
 {
-	int i;
-	int some_tx_pending = 0;
-
 	if (!netif_carrier_ok(adapter->netdev)) {
-		for (i = 0; i < adapter->num_tx_queues; i++) {
-			struct ixgbe_ring *tx_ring = adapter->tx_ring[i];
-			if (tx_ring->next_to_use != tx_ring->next_to_clean) {
-				some_tx_pending = 1;
-				break;
-			}
-		}
-
-		if (some_tx_pending) {
+		if (ixgbe_ring_tx_pending(adapter) ||
+		    ixgbe_vf_tx_pending(adapter)) {
 			/* We've lost link, so the controller stops DMA,
 			 * but we've got queued Tx work that's never going
 			 * to get done, so reset controller to flush Tx.
@@ -6837,6 +6744,36 @@ static void ixgbe_tx_olinfo_status(union ixgbe_adv_tx_desc *tx_desc,
 	tx_desc->read.olinfo_status = cpu_to_le32(olinfo_status);
 }
 
+static int __ixgbe_maybe_stop_tx(struct ixgbe_ring *tx_ring, u16 size)
+{
+	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
+
+	/* Herbert's original patch had:
+	 *  smp_mb__after_netif_stop_queue();
+	 * but since that doesn't exist yet, just open code it.
+	 */
+	smp_mb();
+
+	/* We need to check again in a case another CPU has just
+	 * made room available.
+	 */
+	if (likely(ixgbe_desc_unused(tx_ring) < size))
+		return -EBUSY;
+
+	/* A reprieve! - use start_queue because it doesn't call schedule */
+	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
+	++tx_ring->tx_stats.restart_queue;
+	return 0;
+}
+
+static inline int ixgbe_maybe_stop_tx(struct ixgbe_ring *tx_ring, u16 size)
+{
+	if (likely(ixgbe_desc_unused(tx_ring) >= size))
+		return 0;
+
+	return __ixgbe_maybe_stop_tx(tx_ring, size);
+}
+
 #define IXGBE_TXD_CMD (IXGBE_TXD_CMD_EOP | \
 		       IXGBE_TXD_CMD_RS)
 
@@ -6958,8 +6895,12 @@ static void ixgbe_tx_map(struct ixgbe_ring *tx_ring,
 
 	tx_ring->next_to_use = i;
 
-	/* notify HW of packet */
-	ixgbe_write_tail(tx_ring, i);
+	ixgbe_maybe_stop_tx(tx_ring, DESC_NEEDED);
+
+	if (netif_xmit_stopped(txring_txq(tx_ring)) || !skb->xmit_more) {
+		/* notify HW of packet */
+		ixgbe_write_tail(tx_ring, i);
+	}
 
 	return;
 dma_error:
@@ -7067,32 +7008,6 @@ static void ixgbe_atr(struct ixgbe_ring *ring,
 					      input, common, ring->queue_index);
 }
 
-static int __ixgbe_maybe_stop_tx(struct ixgbe_ring *tx_ring, u16 size)
-{
-	netif_stop_subqueue(tx_ring->netdev, tx_ring->queue_index);
-	/* Herbert's original patch had:
-	 *  smp_mb__after_netif_stop_queue();
-	 * but since that doesn't exist yet, just open code it. */
-	smp_mb();
-
-	/* We need to check again in a case another CPU has just
-	 * made room available. */
-	if (likely(ixgbe_desc_unused(tx_ring) < size))
-		return -EBUSY;
-
-	/* A reprieve! - use start_queue because it doesn't call schedule */
-	netif_start_subqueue(tx_ring->netdev, tx_ring->queue_index);
-	++tx_ring->tx_stats.restart_queue;
-	return 0;
-}
-
-static inline int ixgbe_maybe_stop_tx(struct ixgbe_ring *tx_ring, u16 size)
-{
-	if (likely(ixgbe_desc_unused(tx_ring) >= size))
-		return 0;
-	return __ixgbe_maybe_stop_tx(tx_ring, size);
-}
-
 static u16 ixgbe_select_queue(struct net_device *dev, struct sk_buff *skb,
 			      void *accel_priv, select_queue_fallback_t fallback)
 {
@@ -7187,9 +7102,10 @@ netdev_tx_t ixgbe_xmit_frame_ring(struct sk_buff *skb,
 		tx_flags |= IXGBE_TX_FLAGS_SW_VLAN;
 	}
 
-	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
-		     !test_and_set_bit_lock(__IXGBE_PTP_TX_IN_PROGRESS,
-					    &adapter->state))) {
+	if (unlikely(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) &&
+	    adapter->ptp_clock &&
+	    !test_and_set_bit_lock(__IXGBE_PTP_TX_IN_PROGRESS,
+				   &adapter->state)) {
 		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
 		tx_flags |= IXGBE_TX_FLAGS_TSTAMP;
 
@@ -7261,8 +7177,6 @@ xmit_fcoe:
 #endif /* IXGBE_FCOE */
 	ixgbe_tx_map(tx_ring, first, hdr_len);
 
-	ixgbe_maybe_stop_tx(tx_ring, DESC_NEEDED);
-
 	return NETDEV_TX_OK;
 
 out_drop:
@@ -7735,39 +7649,13 @@ static int ixgbe_ndo_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 			     const unsigned char *addr,
 			     u16 flags)
 {
-	struct ixgbe_adapter *adapter = netdev_priv(dev);
-	int err;
-
-	if (!(adapter->flags & IXGBE_FLAG_SRIOV_ENABLED))
-		return ndo_dflt_fdb_add(ndm, tb, dev, addr, flags);
-
-	/* Hardware does not support aging addresses so if a
-	 * ndm_state is given only allow permanent addresses
-	 */
-	if (ndm->ndm_state && !(ndm->ndm_state & NUD_PERMANENT)) {
-		pr_info("%s: FDB only supports static addresses\n",
-			ixgbe_driver_name);
-		return -EINVAL;
-	}
-
+	/* guarantee we can provide a unique filter for the unicast address */
 	if (is_unicast_ether_addr(addr) || is_link_local_ether_addr(addr)) {
-		u32 rar_uc_entries = IXGBE_MAX_PF_MACVLANS;
-
-		if (netdev_uc_count(dev) < rar_uc_entries)
-			err = dev_uc_add_excl(dev, addr);
-		else
-			err = -ENOMEM;
-	} else if (is_multicast_ether_addr(addr)) {
-		err = dev_mc_add_excl(dev, addr);
-	} else {
-		err = -EINVAL;
+		if (IXGBE_MAX_PF_MACVLANS <= netdev_uc_count(dev))
+			return -ENOMEM;
 	}
 
-	/* Only return duplicate errors if NLM_F_EXCL is set */
-	if (err == -EEXIST && !(flags & NLM_F_EXCL))
-		err = 0;
-
-	return err;
+	return ndo_dflt_fdb_add(ndm, tb, dev, addr, flags);
 }
 
 static int ixgbe_ndo_bridge_setlink(struct net_device *dev,
@@ -7830,9 +7718,17 @@ static void *ixgbe_fwd_add(struct net_device *pdev, struct net_device *vdev)
 {
 	struct ixgbe_fwd_adapter *fwd_adapter = NULL;
 	struct ixgbe_adapter *adapter = netdev_priv(pdev);
+	int used_pools = adapter->num_vfs + adapter->num_rx_pools;
 	unsigned int limit;
 	int pool, err;
 
+	/* Hardware has a limited number of available pools. Each VF, and the
+	 * PF require a pool. Check to ensure we don't attempt to use more
+	 * then the available number of pools.
+	 */
+	if (used_pools >= IXGBE_MAX_VF_FUNCTIONS)
+		return ERR_PTR(-EINVAL);
+
 #ifdef CONFIG_RPS
 	if (vdev->num_rx_queues != vdev->num_tx_queues) {
 		netdev_info(pdev, "%s: Only supports a single queue count for TX and RX\n",
@@ -8080,7 +7976,6 @@ static int ixgbe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct ixgbe_adapter *adapter = NULL;
 	struct ixgbe_hw *hw;
 	const struct ixgbe_info *ii = ixgbe_info_tbl[ent->driver_data];
-	static int cards_found;
 	int i, err, pci_using_dac, expected_gts;
 	unsigned int indices = MAX_TX_QUEUES;
 	u8 part_str[IXGBE_PBANUM_LENGTH];
@@ -8166,8 +8061,6 @@ static int ixgbe_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	netdev->watchdog_timeo = 5 * HZ;
 	strlcpy(netdev->name, pci_name(pdev), sizeof(netdev->name));
 
-	adapter->bd_number = cards_found;
-
 	/* Setup hw api */
 	memcpy(&hw->mac.ops, ii->mac_ops, sizeof(hw->mac.ops));
 	hw->mac.type  = ii->mac;
@@ -8451,7 +8344,6 @@ skip_sriov:
 	ixgbe_add_sanmac_netdev(netdev);
 
 	e_dev_info("%s\n", ixgbe_default_device_descr);
-	cards_found++;
 
 #ifdef CONFIG_IXGBE_HWMON
 	if (ixgbe_sysfs_init(adapter))
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
index 11f02ea78c4a..d47b19f27c35 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_phy.c
@@ -445,8 +445,6 @@ s32 ixgbe_write_phy_reg_generic(struct ixgbe_hw *hw, u32 reg_addr,
 s32 ixgbe_setup_phy_link_generic(struct ixgbe_hw *hw)
 {
 	s32 status = 0;
-	u32 time_out;
-	u32 max_time_out = 10;
 	u16 autoneg_reg = IXGBE_MII_AUTONEG_REG;
 	bool autoneg = false;
 	ixgbe_link_speed speed;
@@ -514,25 +512,6 @@ s32 ixgbe_setup_phy_link_generic(struct ixgbe_hw *hw)
 	hw->phy.ops.write_reg(hw, MDIO_CTRL1,
 			      MDIO_MMD_AN, autoneg_reg);
 
-	/* Wait for autonegotiation to finish */
-	for (time_out = 0; time_out < max_time_out; time_out++) {
-		udelay(10);
-		/* Restart PHY autonegotiation and wait for completion */
-		status = hw->phy.ops.read_reg(hw, MDIO_STAT1,
-					      MDIO_MMD_AN,
-					      &autoneg_reg);
-
-		autoneg_reg &= MDIO_AN_STAT1_COMPLETE;
-		if (autoneg_reg == MDIO_AN_STAT1_COMPLETE) {
-			break;
-		}
-	}
-
-	if (time_out == max_time_out) {
-		hw_dbg(hw, "ixgbe_setup_phy_link_generic: time out\n");
-		return IXGBE_ERR_LINK_SETUP;
-	}
-
 	return status;
 }
 
@@ -657,8 +636,6 @@ s32 ixgbe_check_phy_link_tnx(struct ixgbe_hw *hw, ixgbe_link_speed *speed,
 s32 ixgbe_setup_phy_link_tnx(struct ixgbe_hw *hw)
 {
 	s32 status;
-	u32 time_out;
-	u32 max_time_out = 10;
 	u16 autoneg_reg = IXGBE_MII_AUTONEG_REG;
 	bool autoneg = false;
 	ixgbe_link_speed speed;
@@ -724,24 +701,6 @@ s32 ixgbe_setup_phy_link_tnx(struct ixgbe_hw *hw)
 	hw->phy.ops.write_reg(hw, MDIO_CTRL1,
 			      MDIO_MMD_AN, autoneg_reg);
 
-	/* Wait for autonegotiation to finish */
-	for (time_out = 0; time_out < max_time_out; time_out++) {
-		udelay(10);
-		/* Restart PHY autonegotiation and wait for completion */
-		status = hw->phy.ops.read_reg(hw, MDIO_STAT1,
-					      MDIO_MMD_AN,
-					      &autoneg_reg);
-
-		autoneg_reg &= MDIO_AN_STAT1_COMPLETE;
-		if (autoneg_reg == MDIO_AN_STAT1_COMPLETE)
-			break;
-	}
-
-	if (time_out == max_time_out) {
-		hw_dbg(hw, "ixgbe_setup_phy_link_tnx: time out\n");
-		return IXGBE_ERR_LINK_SETUP;
-	}
-
 	return status;
 }
 
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
index c14d4d89672f..97c85b859536 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_sriov.c
@@ -250,13 +250,15 @@ static int ixgbe_pci_sriov_enable(struct pci_dev *dev, int num_vfs)
 	if (err)
 		return err;
 
-	/* While the SR-IOV capability structure reports total VFs to be
-	 * 64 we limit the actual number that can be allocated to 63 so
-	 * that some transmit/receive resources can be reserved to the
-	 * PF.  The PCI bus driver already checks for other values out of
-	 * range.
+	/* While the SR-IOV capability structure reports total VFs to be 64,
+	 * we have to limit the actual number allocated based on two factors.
+	 * First, we reserve some transmit/receive resources for the PF.
+	 * Second, VMDQ also uses the same pools that SR-IOV does. We need to
+	 * account for this, so that we don't accidentally allocate more VFs
+	 * than we have available pools. The PCI bus driver already checks for
+	 * other values out of range.
 	 */
-	if (num_vfs > IXGBE_MAX_VFS_DRV_LIMIT)
+	if ((num_vfs + adapter->num_rx_pools) > IXGBE_MAX_VF_FUNCTIONS)
 		return -EPERM;
 
 	adapter->num_vfs = num_vfs;
@@ -1259,6 +1261,9 @@ int ixgbe_ndo_set_vf_spoofchk(struct net_device *netdev, int vf, bool setting)
 	struct ixgbe_hw *hw = &adapter->hw;
 	u32 regval;
 
+	if (vf >= adapter->num_vfs)
+		return -EINVAL;
+
 	adapter->vfinfo[vf].spoofchk_enabled = setting;
 
 	regval = IXGBE_READ_REG(hw, IXGBE_PFVFSPOOF(vf_target_reg));
diff --git a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
index e6b07c2a01fe..dfd55d83bc03 100644
--- a/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
+++ b/drivers/net/ethernet/intel/ixgbe/ixgbe_type.h
@@ -2194,6 +2194,8 @@ enum {
 #define IXGBE_VFLRE(_i)		((((_i) & 1) ? 0x001C0 : 0x00600))
 #define IXGBE_VFLREC(_i)		(0x00700 + ((_i) * 4))
 /* Translated register #defines */
+#define IXGBE_PVFTDH(P)		(0x06010 + (0x40 * (P)))
+#define IXGBE_PVFTDT(P)		(0x06018 + (0x40 * (P)))
 #define IXGBE_PVFTDWBAL(P)	(0x06038 + (0x40 * (P)))
 #define IXGBE_PVFTDWBAH(P)	(0x0603C + (0x40 * (P)))
 
@@ -2202,6 +2204,11 @@ enum {
 #define IXGBE_PVFTDWBAHn(q_per_pool, vf_number, vf_q_index) \
 		(IXGBE_PVFTDWBAH((q_per_pool)*(vf_number) + (vf_q_index)))
 
+#define IXGBE_PVFTDHN(q_per_pool, vf_number, vf_q_index) \
+		(IXGBE_PVFTDH((q_per_pool)*(vf_number) + (vf_q_index)))
+#define IXGBE_PVFTDTN(q_per_pool, vf_number, vf_q_index) \
+		(IXGBE_PVFTDT((q_per_pool)*(vf_number) + (vf_q_index)))
+
 enum ixgbe_fdir_pballoc_type {
 	IXGBE_FDIR_PBALLOC_NONE = 0,
 	IXGBE_FDIR_PBALLOC_64K  = 1,
diff --git a/drivers/net/ethernet/intel/ixgbevf/ethtool.c b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
index d420f124633f..cc0e5b7ff041 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ethtool.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ethtool.c
@@ -523,7 +523,7 @@ static const struct ixgbevf_reg_test reg_test_vf[] = {
 	{ IXGBE_VFTDBAL(0), 2, PATTERN_TEST, 0xFFFFFF80, 0xFFFFFFFF },
 	{ IXGBE_VFTDBAH(0), 2, PATTERN_TEST, 0xFFFFFFFF, 0xFFFFFFFF },
 	{ IXGBE_VFTDLEN(0), 2, PATTERN_TEST, 0x000FFF80, 0x000FFF80 },
-	{ 0, 0, 0, 0 }
+	{ .reg = 0 }
 };
 
 static const u32 register_test_patterns[] = {
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
index a0a1de9ce238..ba96cb5b886d 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf.h
@@ -385,7 +385,6 @@ struct ixgbevf_adapter {
 	/* structs defined in ixgbe_vf.h */
 	struct ixgbe_hw hw;
 	u16 msg_enable;
-	u16 bd_number;
 	/* Interrupt Throttle Rate */
 	u32 eitr_param;
 
diff --git a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
index c22a00c3621a..030a219c85e3 100644
--- a/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
+++ b/drivers/net/ethernet/intel/ixgbevf/ixgbevf_main.c
@@ -3464,7 +3464,6 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	struct ixgbevf_adapter *adapter = NULL;
 	struct ixgbe_hw *hw = NULL;
 	const struct ixgbevf_info *ii = ixgbevf_info_tbl[ent->driver_data];
-	static int cards_found;
 	int err, pci_using_dac;
 
 	err = pci_enable_device(pdev);
@@ -3525,8 +3524,6 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	ixgbevf_assign_netdev_ops(netdev);
 
-	adapter->bd_number = cards_found;
-
 	/* Setup hw api */
 	memcpy(&hw->mac.ops, ii->mac_ops, sizeof(hw->mac.ops));
 	hw->mac.type  = ii->mac;
@@ -3601,7 +3598,6 @@ static int ixgbevf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	hw_dbg(hw, "MAC: %d\n", hw->mac.type);
 
 	hw_dbg(hw, "Intel(R) 82599 Virtual Function\n");
-	cards_found++;
 	return 0;
 
 err_register:
diff --git a/drivers/net/ethernet/intel/ixgbevf/vf.c b/drivers/net/ethernet/intel/ixgbevf/vf.c
index 4d44d64ae387..9cddd56d02c3 100644
--- a/drivers/net/ethernet/intel/ixgbevf/vf.c
+++ b/drivers/net/ethernet/intel/ixgbevf/vf.c
@@ -434,6 +434,21 @@ static s32 ixgbevf_check_mac_link_vf(struct ixgbe_hw *hw,
 	if (!(links_reg & IXGBE_LINKS_UP))
 		goto out;
 
+	/* for SFP+ modules and DA cables on 82599 it can take up to 500usecs
+	 * before the link status is correct
+	 */
+	if (mac->type == ixgbe_mac_82599_vf) {
+		int i;
+
+		for (i = 0; i < 5; i++) {
+			udelay(100);
+			links_reg = IXGBE_READ_REG(hw, IXGBE_VFLINKS);
+
+			if (!(links_reg & IXGBE_LINKS_UP))
+				goto out;
+		}
+	}
+
 	switch (links_reg & IXGBE_LINKS_SPEED_82599) {
 	case IXGBE_LINKS_SPEED_10G_82599:
 		*speed = IXGBE_LINK_SPEED_10GB_FULL;
diff --git a/drivers/net/ethernet/lantiq_etop.c b/drivers/net/ethernet/lantiq_etop.c
index fd4b6aecf6ee..2dad4d5047ba 100644
--- a/drivers/net/ethernet/lantiq_etop.c
+++ b/drivers/net/ethernet/lantiq_etop.c
@@ -633,7 +633,6 @@ ltq_etop_init(struct net_device *dev)
 	int err;
 	bool random_mac = false;
 
-	ether_setup(dev);
 	dev->watchdog_timeo = 10 * HZ;
 	err = ltq_etop_hw_init(dev);
 	if (err)
diff --git a/drivers/net/ethernet/marvell/Kconfig b/drivers/net/ethernet/marvell/Kconfig
index 1b4fc7c639e6..d323a695dfbc 100644
--- a/drivers/net/ethernet/marvell/Kconfig
+++ b/drivers/net/ethernet/marvell/Kconfig
@@ -64,7 +64,8 @@ config MVPP2
 
 config PXA168_ETH
 	tristate "Marvell pxa168 ethernet support"
-	depends on CPU_PXA168
+	depends on HAS_IOMEM && HAS_DMA
+	depends on CPU_PXA168 || ARCH_BERLIN || COMPILE_TEST
 	select PHYLIB
 	---help---
 	  This driver supports the pxa168 Ethernet ports.
diff --git a/drivers/net/ethernet/marvell/mvneta.c b/drivers/net/ethernet/marvell/mvneta.c
index c9f1d1b7ef37..ade067de1689 100644
--- a/drivers/net/ethernet/marvell/mvneta.c
+++ b/drivers/net/ethernet/marvell/mvneta.c
@@ -20,6 +20,7 @@
 #include <linux/mbus.h>
 #include <linux/module.h>
 #include <linux/interrupt.h>
+#include <linux/if_vlan.h>
 #include <net/ip.h>
 #include <net/ipv6.h>
 #include <linux/io.h>
@@ -1371,15 +1372,16 @@ static u32 mvneta_skb_tx_csum(struct mvneta_port *pp, struct sk_buff *skb)
 {
 	if (skb->ip_summed == CHECKSUM_PARTIAL) {
 		int ip_hdr_len = 0;
+		__be16 l3_proto = vlan_get_protocol(skb);
 		u8 l4_proto;
 
-		if (skb->protocol == htons(ETH_P_IP)) {
+		if (l3_proto == htons(ETH_P_IP)) {
 			struct iphdr *ip4h = ip_hdr(skb);
 
 			/* Calculate IPv4 checksum and L4 checksum */
 			ip_hdr_len = ip4h->ihl;
 			l4_proto = ip4h->protocol;
-		} else if (skb->protocol == htons(ETH_P_IPV6)) {
+		} else if (l3_proto == htons(ETH_P_IPV6)) {
 			struct ipv6hdr *ip6h = ipv6_hdr(skb);
 
 			/* Read l4_protocol from one of IPv6 extra headers */
@@ -1390,7 +1392,7 @@ static u32 mvneta_skb_tx_csum(struct mvneta_port *pp, struct sk_buff *skb)
 			return MVNETA_TX_L4_CSUM_NOT;
 
 		return mvneta_txq_desc_csum(skb_network_offset(skb),
-				skb->protocol, ip_hdr_len, l4_proto);
+					    l3_proto, ip_hdr_len, l4_proto);
 	}
 
 	return MVNETA_TX_L4_CSUM_NOT;
diff --git a/drivers/net/ethernet/marvell/pxa168_eth.c b/drivers/net/ethernet/marvell/pxa168_eth.c
index 8f5aa7c62b18..c3b209cd0660 100644
--- a/drivers/net/ethernet/marvell/pxa168_eth.c
+++ b/drivers/net/ethernet/marvell/pxa168_eth.c
@@ -22,27 +22,30 @@
  * along with this program; if not, see <http://www.gnu.org/licenses/>.
  */
 
-#include <linux/dma-mapping.h>
-#include <linux/in.h>
-#include <linux/ip.h>
-#include <linux/tcp.h>
-#include <linux/udp.h>
-#include <linux/etherdevice.h>
 #include <linux/bitops.h>
+#include <linux/clk.h>
 #include <linux/delay.h>
+#include <linux/dma-mapping.h>
+#include <linux/etherdevice.h>
 #include <linux/ethtool.h>
-#include <linux/platform_device.h>
-#include <linux/module.h>
+#include <linux/in.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ip.h>
 #include <linux/kernel.h>
-#include <linux/workqueue.h>
-#include <linux/clk.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_net.h>
 #include <linux/phy.h>
-#include <linux/io.h>
-#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/pxa168_eth.h>
+#include <linux/tcp.h>
 #include <linux/types.h>
+#include <linux/udp.h>
+#include <linux/workqueue.h>
+
 #include <asm/pgtable.h>
 #include <asm/cacheflush.h>
-#include <linux/pxa168_eth.h>
 
 #define DRIVER_NAME	"pxa168-eth"
 #define DRIVER_VERSION	"0.3"
@@ -58,6 +61,8 @@
 #define PORT_COMMAND		0x0410
 #define PORT_STATUS		0x0418
 #define HTPR			0x0428
+#define MAC_ADDR_LOW		0x0430
+#define MAC_ADDR_HIGH		0x0438
 #define SDMA_CONFIG		0x0440
 #define SDMA_CMD		0x0448
 #define INT_CAUSE		0x0450
@@ -161,7 +166,7 @@
 /* Bit definitions for Port status */
 #define PORT_SPEED_100		(1 << 0)
 #define FULL_DUPLEX		(1 << 1)
-#define FLOW_CONTROL_ENABLED	(1 << 2)
+#define FLOW_CONTROL_DISABLED	(1 << 2)
 #define LINK_UP			(1 << 3)
 
 /* Bit definitions for work to be done */
@@ -191,6 +196,7 @@ struct tx_desc {
 
 struct pxa168_eth_private {
 	int port_num;		/* User Ethernet port number    */
+	int phy_addr;
 
 	int rx_resource_err;	/* Rx ring resource error flag */
 
@@ -296,7 +302,7 @@ static void abort_dma(struct pxa168_eth_private *pep)
 	} while (max_retries-- > 0 && delay <= 0);
 
 	if (max_retries <= 0)
-		printk(KERN_ERR "%s : DMA Stuck\n", __func__);
+		netdev_err(pep->dev, "%s : DMA Stuck\n", __func__);
 }
 
 static int ethernet_phy_get(struct pxa168_eth_private *pep)
@@ -507,9 +513,10 @@ static int add_del_hash_entry(struct pxa168_eth_private *pep,
 
 	if (i == HOP_NUMBER) {
 		if (!del) {
-			printk(KERN_INFO "%s: table section is full, need to "
-					"move to 16kB implementation?\n",
-					 __FILE__);
+			netdev_info(pep->dev,
+				    "%s: table section is full, need to "
+				    "move to 16kB implementation?\n",
+				    __FILE__);
 			return -ENOSPC;
 		} else
 			return 0;
@@ -600,16 +607,42 @@ static void pxa168_eth_set_rx_mode(struct net_device *dev)
 		update_hash_table_mac_address(pep, NULL, ha->addr);
 }
 
+static void pxa168_eth_get_mac_address(struct net_device *dev,
+				       unsigned char *addr)
+{
+	struct pxa168_eth_private *pep = netdev_priv(dev);
+	unsigned int mac_h = rdl(pep, MAC_ADDR_HIGH);
+	unsigned int mac_l = rdl(pep, MAC_ADDR_LOW);
+
+	addr[0] = (mac_h >> 24) & 0xff;
+	addr[1] = (mac_h >> 16) & 0xff;
+	addr[2] = (mac_h >> 8) & 0xff;
+	addr[3] = mac_h & 0xff;
+	addr[4] = (mac_l >> 8) & 0xff;
+	addr[5] = mac_l & 0xff;
+}
+
 static int pxa168_eth_set_mac_address(struct net_device *dev, void *addr)
 {
 	struct sockaddr *sa = addr;
 	struct pxa168_eth_private *pep = netdev_priv(dev);
 	unsigned char oldMac[ETH_ALEN];
+	u32 mac_h, mac_l;
 
 	if (!is_valid_ether_addr(sa->sa_data))
 		return -EADDRNOTAVAIL;
 	memcpy(oldMac, dev->dev_addr, ETH_ALEN);
 	memcpy(dev->dev_addr, sa->sa_data, ETH_ALEN);
+
+	mac_h = dev->dev_addr[0] << 24;
+	mac_h |= dev->dev_addr[1] << 16;
+	mac_h |= dev->dev_addr[2] << 8;
+	mac_h |= dev->dev_addr[3];
+	mac_l = dev->dev_addr[4] << 8;
+	mac_l |= dev->dev_addr[5];
+	wrl(pep, MAC_ADDR_HIGH, mac_h);
+	wrl(pep, MAC_ADDR_LOW, mac_l);
+
 	netif_addr_lock_bh(dev);
 	update_hash_table_mac_address(pep, oldMac, dev->dev_addr);
 	netif_addr_unlock_bh(dev);
@@ -726,7 +759,7 @@ static int txq_reclaim(struct net_device *dev, int force)
 
 		if (cmd_sts & TX_ERROR) {
 			if (net_ratelimit())
-				printk(KERN_ERR "%s: Error in TX\n", dev->name);
+				netdev_err(dev, "Error in TX\n");
 			dev->stats.tx_errors++;
 		}
 		dma_unmap_single(NULL, addr, count, DMA_TO_DEVICE);
@@ -743,8 +776,7 @@ static void pxa168_eth_tx_timeout(struct net_device *dev)
 {
 	struct pxa168_eth_private *pep = netdev_priv(dev);
 
-	printk(KERN_INFO "%s: TX timeout  desc_count %d\n",
-	       dev->name, pep->tx_desc_count);
+	netdev_info(dev, "TX timeout  desc_count %d\n", pep->tx_desc_count);
 
 	schedule_work(&pep->tx_timeout_task);
 }
@@ -814,9 +846,8 @@ static int rxq_process(struct net_device *dev, int budget)
 			if ((cmd_sts & (RX_FIRST_DESC | RX_LAST_DESC)) !=
 			    (RX_FIRST_DESC | RX_LAST_DESC)) {
 				if (net_ratelimit())
-					printk(KERN_ERR
-					       "%s: Rx pkt on multiple desc\n",
-					       dev->name);
+					netdev_err(dev,
+						   "Rx pkt on multiple desc\n");
 			}
 			if (cmd_sts & RX_ERROR)
 				stats->rx_errors++;
@@ -871,7 +902,7 @@ static void handle_link_event(struct pxa168_eth_private *pep)
 	port_status = rdl(pep, PORT_STATUS);
 	if (!(port_status & LINK_UP)) {
 		if (netif_carrier_ok(dev)) {
-			printk(KERN_INFO "%s: link down\n", dev->name);
+			netdev_info(dev, "link down\n");
 			netif_carrier_off(dev);
 			txq_reclaim(dev, 1);
 		}
@@ -883,10 +914,9 @@ static void handle_link_event(struct pxa168_eth_private *pep)
 		speed = 10;
 
 	duplex = (port_status & FULL_DUPLEX) ? 1 : 0;
-	fc = (port_status & FLOW_CONTROL_ENABLED) ? 1 : 0;
-	printk(KERN_INFO "%s: link up, %d Mb/s, %s duplex, "
-	       "flow control %sabled\n", dev->name,
-	       speed, duplex ? "full" : "half", fc ? "en" : "dis");
+	fc = (port_status & FLOW_CONTROL_DISABLED) ? 0 : 1;
+	netdev_info(dev, "link up, %d Mb/s, %s duplex, flow control %sabled\n",
+		    speed, duplex ? "full" : "half", fc ? "en" : "dis");
 	if (!netif_carrier_ok(dev))
 		netif_carrier_on(dev);
 }
@@ -1039,9 +1069,8 @@ static void rxq_deinit(struct net_device *dev)
 		}
 	}
 	if (pep->rx_desc_count)
-		printk(KERN_ERR
-		       "Error in freeing Rx Ring. %d skb's still\n",
-		       pep->rx_desc_count);
+		netdev_err(dev, "Error in freeing Rx Ring. %d skb's still\n",
+			   pep->rx_desc_count);
 	/* Free RX ring */
 	if (pep->p_rx_desc_area)
 		dma_free_coherent(pep->dev->dev.parent, pep->rx_desc_area_size,
@@ -1280,15 +1309,15 @@ static int pxa168_smi_read(struct mii_bus *bus, int phy_addr, int regnum)
 	int val;
 
 	if (smi_wait_ready(pep)) {
-		printk(KERN_WARNING "pxa168_eth: SMI bus busy timeout\n");
+		netdev_warn(pep->dev, "pxa168_eth: SMI bus busy timeout\n");
 		return -ETIMEDOUT;
 	}
 	wrl(pep, SMI, (phy_addr << 16) | (regnum << 21) | SMI_OP_R);
 	/* now wait for the data to be valid */
 	for (i = 0; !((val = rdl(pep, SMI)) & SMI_R_VALID); i++) {
 		if (i == PHY_WAIT_ITERATIONS) {
-			printk(KERN_WARNING
-				"pxa168_eth: SMI bus read not valid\n");
+			netdev_warn(pep->dev,
+				    "pxa168_eth: SMI bus read not valid\n");
 			return -ENODEV;
 		}
 		msleep(10);
@@ -1303,7 +1332,7 @@ static int pxa168_smi_write(struct mii_bus *bus, int phy_addr, int regnum,
 	struct pxa168_eth_private *pep = bus->priv;
 
 	if (smi_wait_ready(pep)) {
-		printk(KERN_WARNING "pxa168_eth: SMI bus busy timeout\n");
+		netdev_warn(pep->dev, "pxa168_eth: SMI bus busy timeout\n");
 		return -ETIMEDOUT;
 	}
 
@@ -1311,7 +1340,7 @@ static int pxa168_smi_write(struct mii_bus *bus, int phy_addr, int regnum,
 	    SMI_OP_W | (value & 0xffff));
 
 	if (smi_wait_ready(pep)) {
-		printk(KERN_ERR "pxa168_eth: SMI bus busy timeout\n");
+		netdev_err(pep->dev, "pxa168_eth: SMI bus busy timeout\n");
 		return -ETIMEDOUT;
 	}
 
@@ -1361,24 +1390,25 @@ static struct phy_device *phy_scan(struct pxa168_eth_private *pep, int phy_addr)
 	return phydev;
 }
 
-static void phy_init(struct pxa168_eth_private *pep, int speed, int duplex)
+static void phy_init(struct pxa168_eth_private *pep)
 {
 	struct phy_device *phy = pep->phy;
 
 	phy_attach(pep->dev, dev_name(&phy->dev), PHY_INTERFACE_MODE_MII);
 
-	if (speed == 0) {
+	if (pep->pd && pep->pd->speed != 0) {
+		phy->autoneg = AUTONEG_DISABLE;
+		phy->advertising = 0;
+		phy->speed = pep->pd->speed;
+		phy->duplex = pep->pd->duplex;
+	} else {
 		phy->autoneg = AUTONEG_ENABLE;
 		phy->speed = 0;
 		phy->duplex = 0;
 		phy->supported &= PHY_BASIC_FEATURES;
 		phy->advertising = phy->supported | ADVERTISED_Autoneg;
-	} else {
-		phy->autoneg = AUTONEG_DISABLE;
-		phy->advertising = 0;
-		phy->speed = speed;
-		phy->duplex = duplex;
 	}
+
 	phy_start_aneg(phy);
 }
 
@@ -1386,11 +1416,13 @@ static int ethernet_phy_setup(struct net_device *dev)
 {
 	struct pxa168_eth_private *pep = netdev_priv(dev);
 
-	if (pep->pd->init)
+	if (pep->pd && pep->pd->init)
 		pep->pd->init();
-	pep->phy = phy_scan(pep, pep->pd->phy_addr & 0x1f);
+
+	pep->phy = phy_scan(pep, pep->phy_addr & 0x1f);
 	if (pep->phy != NULL)
-		phy_init(pep, pep->pd->speed, pep->pd->duplex);
+		phy_init(pep);
+
 	update_hash_table_mac_address(pep, NULL, dev->dev_addr);
 
 	return 0;
@@ -1425,23 +1457,23 @@ static void pxa168_get_drvinfo(struct net_device *dev,
 }
 
 static const struct ethtool_ops pxa168_ethtool_ops = {
-	.get_settings = pxa168_get_settings,
-	.set_settings = pxa168_set_settings,
-	.get_drvinfo = pxa168_get_drvinfo,
-	.get_link = ethtool_op_get_link,
-	.get_ts_info = ethtool_op_get_ts_info,
+	.get_settings	= pxa168_get_settings,
+	.set_settings	= pxa168_set_settings,
+	.get_drvinfo	= pxa168_get_drvinfo,
+	.get_link	= ethtool_op_get_link,
+	.get_ts_info	= ethtool_op_get_ts_info,
 };
 
 static const struct net_device_ops pxa168_eth_netdev_ops = {
-	.ndo_open = pxa168_eth_open,
-	.ndo_stop = pxa168_eth_stop,
-	.ndo_start_xmit = pxa168_eth_start_xmit,
-	.ndo_set_rx_mode = pxa168_eth_set_rx_mode,
-	.ndo_set_mac_address = pxa168_eth_set_mac_address,
-	.ndo_validate_addr = eth_validate_addr,
-	.ndo_do_ioctl = pxa168_eth_do_ioctl,
-	.ndo_change_mtu = pxa168_eth_change_mtu,
-	.ndo_tx_timeout = pxa168_eth_tx_timeout,
+	.ndo_open		= pxa168_eth_open,
+	.ndo_stop		= pxa168_eth_stop,
+	.ndo_start_xmit		= pxa168_eth_start_xmit,
+	.ndo_set_rx_mode	= pxa168_eth_set_rx_mode,
+	.ndo_set_mac_address	= pxa168_eth_set_mac_address,
+	.ndo_validate_addr	= eth_validate_addr,
+	.ndo_do_ioctl		= pxa168_eth_do_ioctl,
+	.ndo_change_mtu		= pxa168_eth_change_mtu,
+	.ndo_tx_timeout		= pxa168_eth_tx_timeout,
 };
 
 static int pxa168_eth_probe(struct platform_device *pdev)
@@ -1450,17 +1482,18 @@ static int pxa168_eth_probe(struct platform_device *pdev)
 	struct net_device *dev = NULL;
 	struct resource *res;
 	struct clk *clk;
+	struct device_node *np;
+	const unsigned char *mac_addr = NULL;
 	int err;
 
 	printk(KERN_NOTICE "PXA168 10/100 Ethernet Driver\n");
 
-	clk = clk_get(&pdev->dev, "MFUCLK");
+	clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(clk)) {
-		printk(KERN_ERR "%s: Fast Ethernet failed to get clock\n",
-			DRIVER_NAME);
+		dev_err(&pdev->dev, "Fast Ethernet failed to get clock\n");
 		return -ENODEV;
 	}
-	clk_enable(clk);
+	clk_prepare_enable(clk);
 
 	dev = alloc_etherdev(sizeof(struct pxa168_eth_private));
 	if (!dev) {
@@ -1477,8 +1510,8 @@ static int pxa168_eth_probe(struct platform_device *pdev)
 		err = -ENODEV;
 		goto err_netdev;
 	}
-	pep->base = ioremap(res->start, resource_size(res));
-	if (pep->base == NULL) {
+	pep->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(pep->base)) {
 		err = -ENOMEM;
 		goto err_netdev;
 	}
@@ -1492,19 +1525,42 @@ static int pxa168_eth_probe(struct platform_device *pdev)
 
 	INIT_WORK(&pep->tx_timeout_task, pxa168_eth_tx_timeout_task);
 
-	printk(KERN_INFO "%s:Using random mac address\n", DRIVER_NAME);
-	eth_hw_addr_random(dev);
+	if (pdev->dev.of_node)
+		mac_addr = of_get_mac_address(pdev->dev.of_node);
 
-	pep->pd = dev_get_platdata(&pdev->dev);
-	pep->rx_ring_size = NUM_RX_DESCS;
-	if (pep->pd->rx_queue_size)
-		pep->rx_ring_size = pep->pd->rx_queue_size;
+	if (mac_addr && is_valid_ether_addr(mac_addr)) {
+		ether_addr_copy(dev->dev_addr, mac_addr);
+	} else {
+		/* try reading the mac address, if set by the bootloader */
+		pxa168_eth_get_mac_address(dev, dev->dev_addr);
+		if (!is_valid_ether_addr(dev->dev_addr)) {
+			dev_info(&pdev->dev, "Using random mac address\n");
+			eth_hw_addr_random(dev);
+		}
+	}
 
+	pep->rx_ring_size = NUM_RX_DESCS;
 	pep->tx_ring_size = NUM_TX_DESCS;
-	if (pep->pd->tx_queue_size)
-		pep->tx_ring_size = pep->pd->tx_queue_size;
 
-	pep->port_num = pep->pd->port_number;
+	pep->pd = dev_get_platdata(&pdev->dev);
+	if (pep->pd) {
+		if (pep->pd->rx_queue_size)
+			pep->rx_ring_size = pep->pd->rx_queue_size;
+
+		if (pep->pd->tx_queue_size)
+			pep->tx_ring_size = pep->pd->tx_queue_size;
+
+		pep->port_num = pep->pd->port_number;
+		pep->phy_addr = pep->pd->phy_addr;
+	} else if (pdev->dev.of_node) {
+		of_property_read_u32(pdev->dev.of_node, "port-id",
+				     &pep->port_num);
+
+		np = of_parse_phandle(pdev->dev.of_node, "phy-handle", 0);
+		if (np)
+			of_property_read_u32(np, "reg", &pep->phy_addr);
+	}
+
 	/* Hardware supports only 3 ports */
 	BUG_ON(pep->port_num > 2);
 	netif_napi_add(dev, &pep->napi, pxa168_rx_poll, pep->rx_ring_size);
@@ -1605,6 +1661,12 @@ static int pxa168_eth_suspend(struct platform_device *pdev, pm_message_t state)
 #define pxa168_eth_suspend NULL
 #endif
 
+static const struct of_device_id pxa168_eth_of_match[] = {
+	{ .compatible = "marvell,pxa168-eth" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, pxa168_eth_of_match);
+
 static struct platform_driver pxa168_eth_driver = {
 	.probe = pxa168_eth_probe,
 	.remove = pxa168_eth_remove,
@@ -1612,8 +1674,9 @@ static struct platform_driver pxa168_eth_driver = {
 	.resume = pxa168_eth_resume,
 	.suspend = pxa168_eth_suspend,
 	.driver = {
-		   .name = DRIVER_NAME,
-		   },
+		.name		= DRIVER_NAME,
+		.of_match_table	= of_match_ptr(pxa168_eth_of_match),
+	},
 };
 
 module_platform_driver(pxa168_eth_driver);
diff --git a/drivers/net/ethernet/marvell/skge.c b/drivers/net/ethernet/marvell/skge.c
index 24b242277ea1..264eab7d3b26 100644
--- a/drivers/net/ethernet/marvell/skge.c
+++ b/drivers/net/ethernet/marvell/skge.c
@@ -1107,7 +1107,7 @@ static u16 xm_phy_read(struct skge_hw *hw, int port, u16 reg)
 {
 	u16 v = 0;
 	if (__xm_phy_read(hw, port, reg, &v))
-		pr_warning("%s: phy read timed out\n", hw->dev[port]->name);
+		pr_warn("%s: phy read timed out\n", hw->dev[port]->name);
 	return v;
 }
 
@@ -1903,7 +1903,7 @@ static int gm_phy_write(struct skge_hw *hw, int port, u16 reg, u16 val)
 			return 0;
 	}
 
-	pr_warning("%s: phy write timeout\n", hw->dev[port]->name);
+	pr_warn("%s: phy write timeout\n", hw->dev[port]->name);
 	return -EIO;
 }
 
@@ -1931,7 +1931,7 @@ static u16 gm_phy_read(struct skge_hw *hw, int port, u16 reg)
 {
 	u16 v = 0;
 	if (__gm_phy_read(hw, port, reg, &v))
-		pr_warning("%s: phy read timeout\n", hw->dev[port]->name);
+		pr_warn("%s: phy read timeout\n", hw->dev[port]->name);
 	return v;
 }
 
diff --git a/drivers/net/ethernet/marvell/sky2.c b/drivers/net/ethernet/marvell/sky2.c
index dba48a5ce7ab..bd3366267039 100644
--- a/drivers/net/ethernet/marvell/sky2.c
+++ b/drivers/net/ethernet/marvell/sky2.c
@@ -2814,7 +2814,7 @@ static int sky2_status_intr(struct sky2_hw *hw, int to_do, u16 idx)
 
 		default:
 			if (net_ratelimit())
-				pr_warning("unknown status opcode 0x%x\n", opcode);
+				pr_warn("unknown status opcode 0x%x\n", opcode);
 		}
 	} while (hw->st_idx != idx);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/cmd.c b/drivers/net/ethernet/mellanox/mlx4/cmd.c
index 65a4a0f88ea0..b16e1b95566f 100644
--- a/drivers/net/ethernet/mellanox/mlx4/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx4/cmd.c
@@ -580,8 +580,18 @@ static int mlx4_cmd_wait(struct mlx4_dev *dev, u64 in_param, u64 *out_param,
 
 	err = context->result;
 	if (err) {
-		mlx4_err(dev, "command 0x%x failed: fw status = 0x%x\n",
-			 op, context->fw_status);
+		/* Since we do not want to have this error message always
+		 * displayed at driver start when there are ConnectX2 HCAs
+		 * on the host, we deprecate the error message for this
+		 * specific command/input_mod/opcode_mod/fw-status to be debug.
+		 */
+		if (op == MLX4_CMD_SET_PORT && in_modifier == 1 &&
+		    op_modifier == 0 && context->fw_status == CMD_STAT_BAD_SIZE)
+			mlx4_dbg(dev, "command 0x%x failed: fw status = 0x%x\n",
+				 op, context->fw_status);
+		else
+			mlx4_err(dev, "command 0x%x failed: fw status = 0x%x\n",
+				 op, context->fw_status);
 		goto out;
 	}
 
@@ -1695,7 +1705,7 @@ static int mlx4_master_activate_admin_state(struct mlx4_priv *priv, int slave)
 			if (err) {
 				vp_oper->vlan_idx = NO_INDX;
 				mlx4_warn(&priv->dev,
-					  "No vlan resorces slave %d, port %d\n",
+					  "No vlan resources slave %d, port %d\n",
 					  slave, port);
 				return err;
 			}
@@ -1711,7 +1721,7 @@ static int mlx4_master_activate_admin_state(struct mlx4_priv *priv, int slave)
 				err = vp_oper->mac_idx;
 				vp_oper->mac_idx = NO_INDX;
 				mlx4_warn(&priv->dev,
-					  "No mac resorces slave %d, port %d\n",
+					  "No mac resources slave %d, port %d\n",
 					  slave, port);
 				return err;
 			}
@@ -2389,6 +2399,22 @@ struct mlx4_slaves_pport mlx4_phys_to_slaves_pport_actv(
 }
 EXPORT_SYMBOL_GPL(mlx4_phys_to_slaves_pport_actv);
 
+static int mlx4_slaves_closest_port(struct mlx4_dev *dev, int slave, int port)
+{
+	struct mlx4_active_ports actv_ports = mlx4_get_active_ports(dev, slave);
+	int min_port = find_first_bit(actv_ports.ports, dev->caps.num_ports)
+			+ 1;
+	int max_port = min_port +
+		bitmap_weight(actv_ports.ports, dev->caps.num_ports);
+
+	if (port < min_port)
+		port = min_port;
+	else if (port >= max_port)
+		port = max_port - 1;
+
+	return port;
+}
+
 int mlx4_set_vf_mac(struct mlx4_dev *dev, int port, int vf, u64 mac)
 {
 	struct mlx4_priv *priv = mlx4_priv(dev);
@@ -2402,6 +2428,7 @@ int mlx4_set_vf_mac(struct mlx4_dev *dev, int port, int vf, u64 mac)
 	if (slave < 0)
 		return -EINVAL;
 
+	port = mlx4_slaves_closest_port(dev, slave, port);
 	s_info = &priv->mfunc.master.vf_admin[slave].vport[port];
 	s_info->mac = mac;
 	mlx4_info(dev, "default mac on vf %d port %d to %llX will take afect only after vf restart\n",
@@ -2428,6 +2455,7 @@ int mlx4_set_vf_vlan(struct mlx4_dev *dev, int port, int vf, u16 vlan, u8 qos)
 	if (slave < 0)
 		return -EINVAL;
 
+	port = mlx4_slaves_closest_port(dev, slave, port);
 	vf_admin = &priv->mfunc.master.vf_admin[slave].vport[port];
 
 	if ((0 == vlan) && (0 == qos))
@@ -2455,6 +2483,7 @@ bool mlx4_get_slave_default_vlan(struct mlx4_dev *dev, int port, int slave,
 	struct mlx4_priv *priv;
 
 	priv = mlx4_priv(dev);
+	port = mlx4_slaves_closest_port(dev, slave, port);
 	vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
 
 	if (MLX4_VGT != vp_oper->state.default_vlan) {
@@ -2482,6 +2511,7 @@ int mlx4_set_vf_spoofchk(struct mlx4_dev *dev, int port, int vf, bool setting)
 	if (slave < 0)
 		return -EINVAL;
 
+	port = mlx4_slaves_closest_port(dev, slave, port);
 	s_info = &priv->mfunc.master.vf_admin[slave].vport[port];
 	s_info->spoofchk = setting;
 
@@ -2535,6 +2565,7 @@ int mlx4_set_vf_link_state(struct mlx4_dev *dev, int port, int vf, int link_stat
 	if (slave < 0)
 		return -EINVAL;
 
+	port = mlx4_slaves_closest_port(dev, slave, port);
 	switch (link_state) {
 	case IFLA_VF_LINK_STATE_AUTO:
 		/* get current link state */
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
index e22f24f784fc..ae83da9cd18a 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_ethtool.c
@@ -112,6 +112,7 @@ static const char main_strings[][ETH_GSTRING_LEN] = {
 
 	/* port statistics */
 	"tso_packets",
+	"xmit_more",
 	"queue_stopped", "wake_queue", "tx_timeout", "rx_alloc_failed",
 	"rx_csum_good", "rx_csum_none", "tx_chksum_offload",
 
@@ -487,6 +488,9 @@ static int mlx4_en_set_pauseparam(struct net_device *dev,
 	struct mlx4_en_dev *mdev = priv->mdev;
 	int err;
 
+	if (pause->autoneg)
+		return -EINVAL;
+
 	priv->prof->tx_pause = pause->tx_pause != 0;
 	priv->prof->rx_pause = pause->rx_pause != 0;
 	err = mlx4_SET_PORT_general(mdev->dev, priv->port,
@@ -1263,6 +1267,48 @@ static u32 mlx4_en_get_priv_flags(struct net_device *dev)
 	return priv->pflags;
 }
 
+static int mlx4_en_get_tunable(struct net_device *dev,
+			       const struct ethtool_tunable *tuna,
+			       void *data)
+{
+	const struct mlx4_en_priv *priv = netdev_priv(dev);
+	int ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_TX_COPYBREAK:
+		*(u32 *)data = priv->prof->inline_thold;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
+static int mlx4_en_set_tunable(struct net_device *dev,
+			       const struct ethtool_tunable *tuna,
+			       const void *data)
+{
+	struct mlx4_en_priv *priv = netdev_priv(dev);
+	int val, ret = 0;
+
+	switch (tuna->id) {
+	case ETHTOOL_TX_COPYBREAK:
+		val = *(u32 *)data;
+		if (val < MIN_PKT_LEN || val > MAX_INLINE)
+			ret = -EINVAL;
+		else
+			priv->prof->inline_thold = val;
+		break;
+	default:
+		ret = -EINVAL;
+		break;
+	}
+
+	return ret;
+}
+
 
 const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.get_drvinfo = mlx4_en_get_drvinfo,
@@ -1293,6 +1339,8 @@ const struct ethtool_ops mlx4_en_ethtool_ops = {
 	.get_ts_info = mlx4_en_get_ts_info,
 	.set_priv_flags = mlx4_en_set_priv_flags,
 	.get_priv_flags = mlx4_en_get_priv_flags,
+	.get_tunable		= mlx4_en_get_tunable,
+	.set_tunable		= mlx4_en_set_tunable,
 };
 
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_main.c b/drivers/net/ethernet/mellanox/mlx4/en_main.c
index 3626fdf4cb5d..2091ae88615d 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_main.c
@@ -78,27 +78,24 @@ MLX4_EN_PARM_INT(inline_thold, MAX_INLINE,
 #define MAX_PFC_TX     0xff
 #define MAX_PFC_RX     0xff
 
-int en_print(const char *level, const struct mlx4_en_priv *priv,
-	     const char *format, ...)
+void en_print(const char *level, const struct mlx4_en_priv *priv,
+	      const char *format, ...)
 {
 	va_list args;
 	struct va_format vaf;
-	int i;
 
 	va_start(args, format);
 
 	vaf.fmt = format;
 	vaf.va = &args;
 	if (priv->registered)
-		i = printk("%s%s: %s: %pV",
-			   level, DRV_NAME, priv->dev->name, &vaf);
+		printk("%s%s: %s: %pV",
+		       level, DRV_NAME, priv->dev->name, &vaf);
 	else
-		i = printk("%s%s: %s: Port %d: %pV",
-			   level, DRV_NAME, dev_name(&priv->mdev->pdev->dev),
-			   priv->port, &vaf);
+		printk("%s%s: %s: Port %d: %pV",
+		       level, DRV_NAME, dev_name(&priv->mdev->pdev->dev),
+		       priv->port, &vaf);
 	va_end(args);
-
-	return i;
 }
 
 void mlx4_en_update_loopback_state(struct net_device *dev,
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
index bb536aa613f4..f3032fec8fce 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_netdev.c
@@ -474,39 +474,12 @@ static int mlx4_en_tunnel_steer_add(struct mlx4_en_priv *priv, unsigned char *ad
 				    int qpn, u64 *reg_id)
 {
 	int err;
-	struct mlx4_spec_list spec_eth_outer = { {NULL} };
-	struct mlx4_spec_list spec_vxlan     = { {NULL} };
-	struct mlx4_spec_list spec_eth_inner = { {NULL} };
-
-	struct mlx4_net_trans_rule rule = {
-		.queue_mode = MLX4_NET_TRANS_Q_FIFO,
-		.exclusive = 0,
-		.allow_loopback = 1,
-		.promisc_mode = MLX4_FS_REGULAR,
-		.priority = MLX4_DOMAIN_NIC,
-	};
-
-	__be64 mac_mask = cpu_to_be64(MLX4_MAC_MASK << 16);
 
 	if (priv->mdev->dev->caps.tunnel_offload_mode != MLX4_TUNNEL_OFFLOAD_MODE_VXLAN)
 		return 0; /* do nothing */
 
-	rule.port = priv->port;
-	rule.qpn = qpn;
-	INIT_LIST_HEAD(&rule.list);
-
-	spec_eth_outer.id = MLX4_NET_TRANS_RULE_ID_ETH;
-	memcpy(spec_eth_outer.eth.dst_mac, addr, ETH_ALEN);
-	memcpy(spec_eth_outer.eth.dst_mac_msk, &mac_mask, ETH_ALEN);
-
-	spec_vxlan.id = MLX4_NET_TRANS_RULE_ID_VXLAN;    /* any vxlan header */
-	spec_eth_inner.id = MLX4_NET_TRANS_RULE_ID_ETH;	 /* any inner eth header */
-
-	list_add_tail(&spec_eth_outer.list, &rule.list);
-	list_add_tail(&spec_vxlan.list,     &rule.list);
-	list_add_tail(&spec_eth_inner.list, &rule.list);
-
-	err = mlx4_flow_attach(priv->mdev->dev, &rule, reg_id);
+	err = mlx4_tunnel_steer_add(priv->mdev->dev, addr, priv->port, qpn,
+				    MLX4_DOMAIN_NIC, reg_id);
 	if (err) {
 		en_err(priv, "failed to add vxlan steering rule, err %d\n", err);
 		return err;
@@ -2486,6 +2459,7 @@ int mlx4_en_init_netdev(struct mlx4_en_dev *mdev, int port,
 	}
 	priv->rx_ring_num = prof->rx_ring_num;
 	priv->cqe_factor = (mdev->dev->caps.cqe_size == 64) ? 1 : 0;
+	priv->cqe_size = mdev->dev->caps.cqe_size;
 	priv->mac_index = -1;
 	priv->msg_enable = MLX4_EN_MSG_LEVEL;
 	spin_lock_init(&priv->stats_lock);
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_port.c b/drivers/net/ethernet/mellanox/mlx4/en_port.c
index c2cfb05e7290..0a0261d128b9 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_port.c
@@ -150,14 +150,19 @@ int mlx4_en_DUMP_ETH_STATS(struct mlx4_en_dev *mdev, u8 port, u8 reset)
 	priv->port_stats.tx_chksum_offload = 0;
 	priv->port_stats.queue_stopped = 0;
 	priv->port_stats.wake_queue = 0;
+	priv->port_stats.tso_packets = 0;
+	priv->port_stats.xmit_more = 0;
 
 	for (i = 0; i < priv->tx_ring_num; i++) {
-		stats->tx_packets += priv->tx_ring[i]->packets;
-		stats->tx_bytes += priv->tx_ring[i]->bytes;
-		priv->port_stats.tx_chksum_offload += priv->tx_ring[i]->tx_csum;
-		priv->port_stats.queue_stopped +=
-			priv->tx_ring[i]->queue_stopped;
-		priv->port_stats.wake_queue += priv->tx_ring[i]->wake_queue;
+		const struct mlx4_en_tx_ring *ring = priv->tx_ring[i];
+
+		stats->tx_packets += ring->packets;
+		stats->tx_bytes += ring->bytes;
+		priv->port_stats.tx_chksum_offload += ring->tx_csum;
+		priv->port_stats.queue_stopped     += ring->queue_stopped;
+		priv->port_stats.wake_queue        += ring->wake_queue;
+		priv->port_stats.tso_packets       += ring->tso_packets;
+		priv->port_stats.xmit_more         += ring->xmit_more;
 	}
 
 	stats->rx_errors = be64_to_cpu(mlx4_en_stats->PCS) +
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_rx.c b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
index 9c909d23f14c..01660c595f5c 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_rx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_rx.c
@@ -76,10 +76,10 @@ static int mlx4_alloc_pages(struct mlx4_en_priv *priv,
 	page_alloc->dma = dma;
 	page_alloc->page_offset = frag_info->frag_align;
 	/* Not doing get_page() for each frag is a big win
-	 * on asymetric workloads.
+	 * on asymetric workloads. Note we can not use atomic_set().
 	 */
-	atomic_set(&page->_count,
-		   page_alloc->page_size / frag_info->frag_stride);
+	atomic_add(page_alloc->page_size / frag_info->frag_stride - 1,
+		   &page->_count);
 	return 0;
 }
 
@@ -588,6 +588,8 @@ static struct sk_buff *mlx4_en_rx_skb(struct mlx4_en_priv *priv,
 		skb_copy_to_linear_data(skb, va, length);
 		skb->tail += length;
 	} else {
+		unsigned int pull_len;
+
 		/* Move relevant fragments to skb */
 		used_frags = mlx4_en_complete_rx_desc(priv, rx_desc, frags,
 							skb, length);
@@ -597,16 +599,17 @@ static struct sk_buff *mlx4_en_rx_skb(struct mlx4_en_priv *priv,
 		}
 		skb_shinfo(skb)->nr_frags = used_frags;
 
+		pull_len = eth_get_headlen(va, SMALL_PACKET_SIZE);
 		/* Copy headers into the skb linear buffer */
-		memcpy(skb->data, va, HEADER_COPY_SIZE);
-		skb->tail += HEADER_COPY_SIZE;
+		memcpy(skb->data, va, pull_len);
+		skb->tail += pull_len;
 
 		/* Skip headers in first fragment */
-		skb_shinfo(skb)->frags[0].page_offset += HEADER_COPY_SIZE;
+		skb_shinfo(skb)->frags[0].page_offset += pull_len;
 
 		/* Adjust size of first fragment */
-		skb_frag_size_sub(&skb_shinfo(skb)->frags[0], HEADER_COPY_SIZE);
-		skb->data_len = length - HEADER_COPY_SIZE;
+		skb_frag_size_sub(&skb_shinfo(skb)->frags[0], pull_len);
+		skb->data_len = length - pull_len;
 	}
 	return skb;
 }
@@ -668,7 +671,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 	 * descriptor offset can be deduced from the CQE index instead of
 	 * reading 'cqe->index' */
 	index = cq->mcq.cons_index & ring->size_mask;
-	cqe = &cq->buf[(index << factor) + factor];
+	cqe = mlx4_en_get_cqe(cq->buf, index, priv->cqe_size) + factor;
 
 	/* Process all completed CQEs */
 	while (XNOR(cqe->owner_sr_opcode & MLX4_CQE_OWNER_MASK,
@@ -769,7 +772,7 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 					gro_skb->ip_summed = CHECKSUM_UNNECESSARY;
 
 					if (l2_tunnel)
-						gro_skb->encapsulation = 1;
+						gro_skb->csum_level = 1;
 					if ((cqe->vlan_my_qpn &
 					    cpu_to_be32(MLX4_CQE_VLAN_PRESENT_MASK)) &&
 					    (dev->features & NETIF_F_HW_VLAN_CTAG_RX)) {
@@ -823,8 +826,8 @@ int mlx4_en_process_rx_cq(struct net_device *dev, struct mlx4_en_cq *cq, int bud
 		skb->protocol = eth_type_trans(skb, dev);
 		skb_record_rx_queue(skb, cq->ring);
 
-		if (l2_tunnel)
-			skb->encapsulation = 1;
+		if (l2_tunnel && ip_summed == CHECKSUM_UNNECESSARY)
+			skb->csum_level = 1;
 
 		if (dev->features & NETIF_F_RXHASH)
 			skb_set_hash(skb,
@@ -855,7 +858,7 @@ next:
 
 		++cq->mcq.cons_index;
 		index = (cq->mcq.cons_index) & ring->size_mask;
-		cqe = &cq->buf[(index << factor) + factor];
+		cqe = mlx4_en_get_cqe(cq->buf, index, priv->cqe_size) + factor;
 		if (++polled == budget)
 			goto out;
 	}
diff --git a/drivers/net/ethernet/mellanox/mlx4/en_tx.c b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
index dae3da6d8dd0..34c137878545 100644
--- a/drivers/net/ethernet/mellanox/mlx4/en_tx.c
+++ b/drivers/net/ethernet/mellanox/mlx4/en_tx.c
@@ -37,6 +37,7 @@
 #include <linux/mlx4/qp.h>
 #include <linux/skbuff.h>
 #include <linux/if_vlan.h>
+#include <linux/prefetch.h>
 #include <linux/vmalloc.h>
 #include <linux/tcp.h>
 #include <linux/ip.h>
@@ -65,10 +66,9 @@ int mlx4_en_create_tx_ring(struct mlx4_en_priv *priv,
 	ring->size = size;
 	ring->size_mask = size - 1;
 	ring->stride = stride;
-	ring->inline_thold = priv->prof->inline_thold;
 
 	tmp = size * sizeof(struct mlx4_en_tx_info);
-	ring->tx_info = vmalloc_node(tmp, node);
+	ring->tx_info = kmalloc_node(tmp, GFP_KERNEL | __GFP_NOWARN, node);
 	if (!ring->tx_info) {
 		ring->tx_info = vmalloc(tmp);
 		if (!ring->tx_info) {
@@ -151,7 +151,7 @@ err_bounce:
 	kfree(ring->bounce_buf);
 	ring->bounce_buf = NULL;
 err_info:
-	vfree(ring->tx_info);
+	kvfree(ring->tx_info);
 	ring->tx_info = NULL;
 err_ring:
 	kfree(ring);
@@ -174,7 +174,7 @@ void mlx4_en_destroy_tx_ring(struct mlx4_en_priv *priv,
 	mlx4_free_hwq_res(mdev->dev, &ring->wqres, ring->buf_size);
 	kfree(ring->bounce_buf);
 	ring->bounce_buf = NULL;
-	vfree(ring->tx_info);
+	kvfree(ring->tx_info);
 	ring->tx_info = NULL;
 	kfree(ring);
 	*pring = NULL;
@@ -191,12 +191,12 @@ int mlx4_en_activate_tx_ring(struct mlx4_en_priv *priv,
 	ring->prod = 0;
 	ring->cons = 0xffffffff;
 	ring->last_nr_txbb = 1;
-	ring->poll_cnt = 0;
 	memset(ring->tx_info, 0, ring->size * sizeof(struct mlx4_en_tx_info));
 	memset(ring->buf, 0, ring->buf_size);
 
 	ring->qp_state = MLX4_QP_STATE_RST;
-	ring->doorbell_qpn = ring->qp.qpn << 8;
+	ring->doorbell_qpn = cpu_to_be32(ring->qp.qpn << 8);
+	ring->mr_key = cpu_to_be32(mdev->mr.key);
 
 	mlx4_en_fill_qp_context(priv, ring->size, ring->stride, 1, 0, ring->qpn,
 				ring->cqn, user_prio, &ring->context);
@@ -259,38 +259,45 @@ static u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 				struct mlx4_en_tx_ring *ring,
 				int index, u8 owner, u64 timestamp)
 {
-	struct mlx4_en_dev *mdev = priv->mdev;
 	struct mlx4_en_tx_info *tx_info = &ring->tx_info[index];
 	struct mlx4_en_tx_desc *tx_desc = ring->buf + index * TXBB_SIZE;
 	struct mlx4_wqe_data_seg *data = (void *) tx_desc + tx_info->data_offset;
-	struct sk_buff *skb = tx_info->skb;
-	struct skb_frag_struct *frag;
 	void *end = ring->buf + ring->buf_size;
-	int frags = skb_shinfo(skb)->nr_frags;
+	struct sk_buff *skb = tx_info->skb;
+	int nr_maps = tx_info->nr_maps;
 	int i;
-	struct skb_shared_hwtstamps hwts;
 
-	if (timestamp) {
-		mlx4_en_fill_hwtstamps(mdev, &hwts, timestamp);
+	/* We do not touch skb here, so prefetch skb->users location
+	 * to speedup consume_skb()
+	 */
+	prefetchw(&skb->users);
+
+	if (unlikely(timestamp)) {
+		struct skb_shared_hwtstamps hwts;
+
+		mlx4_en_fill_hwtstamps(priv->mdev, &hwts, timestamp);
 		skb_tstamp_tx(skb, &hwts);
 	}
 
 	/* Optimize the common case when there are no wraparounds */
 	if (likely((void *) tx_desc + tx_info->nr_txbb * TXBB_SIZE <= end)) {
 		if (!tx_info->inl) {
-			if (tx_info->linear) {
+			if (tx_info->linear)
 				dma_unmap_single(priv->ddev,
-					(dma_addr_t) be64_to_cpu(data->addr),
-					 be32_to_cpu(data->byte_count),
-					 PCI_DMA_TODEVICE);
-				++data;
-			}
-
-			for (i = 0; i < frags; i++) {
-				frag = &skb_shinfo(skb)->frags[i];
+						tx_info->map0_dma,
+						tx_info->map0_byte_count,
+						PCI_DMA_TODEVICE);
+			else
 				dma_unmap_page(priv->ddev,
-					(dma_addr_t) be64_to_cpu(data[i].addr),
-					skb_frag_size(frag), PCI_DMA_TODEVICE);
+					       tx_info->map0_dma,
+					       tx_info->map0_byte_count,
+					       PCI_DMA_TODEVICE);
+			for (i = 1; i < nr_maps; i++) {
+				data++;
+				dma_unmap_page(priv->ddev,
+					(dma_addr_t)be64_to_cpu(data->addr),
+					be32_to_cpu(data->byte_count),
+					PCI_DMA_TODEVICE);
 			}
 		}
 	} else {
@@ -299,27 +306,29 @@ static u32 mlx4_en_free_tx_desc(struct mlx4_en_priv *priv,
 				data = ring->buf + ((void *)data - end);
 			}
 
-			if (tx_info->linear) {
+			if (tx_info->linear)
 				dma_unmap_single(priv->ddev,
-					(dma_addr_t) be64_to_cpu(data->addr),
-					 be32_to_cpu(data->byte_count),
-					 PCI_DMA_TODEVICE);
-				++data;
-			}
-
-			for (i = 0; i < frags; i++) {
+						tx_info->map0_dma,
+						tx_info->map0_byte_count,
+						PCI_DMA_TODEVICE);
+			else
+				dma_unmap_page(priv->ddev,
+					       tx_info->map0_dma,
+					       tx_info->map0_byte_count,
+					       PCI_DMA_TODEVICE);
+			for (i = 1; i < nr_maps; i++) {
+				data++;
 				/* Check for wraparound before unmapping */
 				if ((void *) data >= end)
 					data = ring->buf;
-				frag = &skb_shinfo(skb)->frags[i];
 				dma_unmap_page(priv->ddev,
-					(dma_addr_t) be64_to_cpu(data->addr),
-					 skb_frag_size(frag), PCI_DMA_TODEVICE);
-				++data;
+					(dma_addr_t)be64_to_cpu(data->addr),
+					be32_to_cpu(data->byte_count),
+					PCI_DMA_TODEVICE);
 			}
 		}
 	}
-	dev_kfree_skb_any(skb);
+	dev_consume_skb_any(skb);
 	return tx_info->nr_txbb;
 }
 
@@ -377,13 +386,19 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 	u64 timestamp = 0;
 	int done = 0;
 	int budget = priv->tx_work_limit;
+	u32 last_nr_txbb;
+	u32 ring_cons;
 
 	if (!priv->port_up)
 		return true;
 
+	netdev_txq_bql_complete_prefetchw(ring->tx_queue);
+
 	index = cons_index & size_mask;
-	cqe = &buf[(index << factor) + factor];
-	ring_index = ring->cons & size_mask;
+	cqe = mlx4_en_get_cqe(buf, index, priv->cqe_size) + factor;
+	last_nr_txbb = ACCESS_ONCE(ring->last_nr_txbb);
+	ring_cons = ACCESS_ONCE(ring->cons);
+	ring_index = ring_cons & size_mask;
 	stamp_index = ring_index;
 
 	/* Process all completed CQEs */
@@ -408,19 +423,19 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 		new_index = be16_to_cpu(cqe->wqe_index) & size_mask;
 
 		do {
-			txbbs_skipped += ring->last_nr_txbb;
-			ring_index = (ring_index + ring->last_nr_txbb) & size_mask;
+			txbbs_skipped += last_nr_txbb;
+			ring_index = (ring_index + last_nr_txbb) & size_mask;
 			if (ring->tx_info[ring_index].ts_requested)
 				timestamp = mlx4_en_get_cqe_ts(cqe);
 
 			/* free next descriptor */
-			ring->last_nr_txbb = mlx4_en_free_tx_desc(
+			last_nr_txbb = mlx4_en_free_tx_desc(
 					priv, ring, ring_index,
-					!!((ring->cons + txbbs_skipped) &
+					!!((ring_cons + txbbs_skipped) &
 					ring->size), timestamp);
 
 			mlx4_en_stamp_wqe(priv, ring, stamp_index,
-					  !!((ring->cons + txbbs_stamp) &
+					  !!((ring_cons + txbbs_stamp) &
 						ring->size));
 			stamp_index = ring_index;
 			txbbs_stamp = txbbs_skipped;
@@ -430,7 +445,7 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 
 		++cons_index;
 		index = cons_index & size_mask;
-		cqe = &buf[(index << factor) + factor];
+		cqe = mlx4_en_get_cqe(buf, index, priv->cqe_size) + factor;
 	}
 
 
@@ -441,7 +456,11 @@ static bool mlx4_en_process_tx_cq(struct net_device *dev,
 	mcq->cons_index = cons_index;
 	mlx4_cq_set_ci(mcq);
 	wmb();
-	ring->cons += txbbs_skipped;
+
+	/* we want to dirty this cache line once */
+	ACCESS_ONCE(ring->last_nr_txbb) = last_nr_txbb;
+	ACCESS_ONCE(ring->cons) = ring_cons + txbbs_skipped;
+
 	netdev_tx_completed_queue(ring->tx_queue, packets, bytes);
 
 	/*
@@ -512,30 +531,35 @@ static struct mlx4_en_tx_desc *mlx4_en_bounce_to_desc(struct mlx4_en_priv *priv,
 	return ring->buf + index * TXBB_SIZE;
 }
 
-static int is_inline(int inline_thold, struct sk_buff *skb, void **pfrag)
+/* Decide if skb can be inlined in tx descriptor to avoid dma mapping
+ *
+ * It seems strange we do not simply use skb_copy_bits().
+ * This would allow to inline all skbs iff skb->len <= inline_thold
+ *
+ * Note that caller already checked skb was not a gso packet
+ */
+static bool is_inline(int inline_thold, const struct sk_buff *skb,
+		      const struct skb_shared_info *shinfo,
+		      void **pfrag)
 {
 	void *ptr;
 
-	if (inline_thold && !skb_is_gso(skb) && skb->len <= inline_thold) {
-		if (skb_shinfo(skb)->nr_frags == 1) {
-			ptr = skb_frag_address_safe(&skb_shinfo(skb)->frags[0]);
-			if (unlikely(!ptr))
-				return 0;
-
-			if (pfrag)
-				*pfrag = ptr;
+	if (skb->len > inline_thold || !inline_thold)
+		return false;
 
-			return 1;
-		} else if (unlikely(skb_shinfo(skb)->nr_frags))
-			return 0;
-		else
-			return 1;
+	if (shinfo->nr_frags == 1) {
+		ptr = skb_frag_address_safe(&shinfo->frags[0]);
+		if (unlikely(!ptr))
+			return false;
+		*pfrag = ptr;
+		return true;
 	}
-
-	return 0;
+	if (shinfo->nr_frags)
+		return false;
+	return true;
 }
 
-static int inline_size(struct sk_buff *skb)
+static int inline_size(const struct sk_buff *skb)
 {
 	if (skb->len + CTRL_SIZE + sizeof(struct mlx4_wqe_inline_seg)
 	    <= MLX4_INLINE_ALIGN)
@@ -546,18 +570,23 @@ static int inline_size(struct sk_buff *skb)
 			     sizeof(struct mlx4_wqe_inline_seg), 16);
 }
 
-static int get_real_size(struct sk_buff *skb, struct net_device *dev,
-			 int *lso_header_size)
+static int get_real_size(const struct sk_buff *skb,
+			 const struct skb_shared_info *shinfo,
+			 struct net_device *dev,
+			 int *lso_header_size,
+			 bool *inline_ok,
+			 void **pfrag)
 {
 	struct mlx4_en_priv *priv = netdev_priv(dev);
 	int real_size;
 
-	if (skb_is_gso(skb)) {
+	if (shinfo->gso_size) {
+		*inline_ok = false;
 		if (skb->encapsulation)
 			*lso_header_size = (skb_inner_transport_header(skb) - skb->data) + inner_tcp_hdrlen(skb);
 		else
 			*lso_header_size = skb_transport_offset(skb) + tcp_hdrlen(skb);
-		real_size = CTRL_SIZE + skb_shinfo(skb)->nr_frags * DS_SIZE +
+		real_size = CTRL_SIZE + shinfo->nr_frags * DS_SIZE +
 			ALIGN(*lso_header_size + 4, DS_SIZE);
 		if (unlikely(*lso_header_size != skb_headlen(skb))) {
 			/* We add a segment for the skb linear buffer only if
@@ -572,20 +601,28 @@ static int get_real_size(struct sk_buff *skb, struct net_device *dev,
 		}
 	} else {
 		*lso_header_size = 0;
-		if (!is_inline(priv->prof->inline_thold, skb, NULL))
-			real_size = CTRL_SIZE + (skb_shinfo(skb)->nr_frags + 1) * DS_SIZE;
-		else
+		*inline_ok = is_inline(priv->prof->inline_thold, skb,
+				       shinfo, pfrag);
+
+		if (*inline_ok)
 			real_size = inline_size(skb);
+		else
+			real_size = CTRL_SIZE +
+				    (shinfo->nr_frags + 1) * DS_SIZE;
 	}
 
 	return real_size;
 }
 
-static void build_inline_wqe(struct mlx4_en_tx_desc *tx_desc, struct sk_buff *skb,
-			     int real_size, u16 *vlan_tag, int tx_ind, void *fragptr)
+static void build_inline_wqe(struct mlx4_en_tx_desc *tx_desc,
+			     const struct sk_buff *skb,
+			     const struct skb_shared_info *shinfo,
+			     int real_size, u16 *vlan_tag,
+			     int tx_ind, void *fragptr)
 {
 	struct mlx4_wqe_inline_seg *inl = &tx_desc->inl;
 	int spc = MLX4_INLINE_ALIGN - CTRL_SIZE - sizeof *inl;
+	unsigned int hlen = skb_headlen(skb);
 
 	if (skb->len <= spc) {
 		if (likely(skb->len >= MIN_PKT_LEN)) {
@@ -595,19 +632,19 @@ static void build_inline_wqe(struct mlx4_en_tx_desc *tx_desc, struct sk_buff *sk
 			memset(((void *)(inl + 1)) + skb->len, 0,
 			       MIN_PKT_LEN - skb->len);
 		}
-		skb_copy_from_linear_data(skb, inl + 1, skb_headlen(skb));
-		if (skb_shinfo(skb)->nr_frags)
-			memcpy(((void *)(inl + 1)) + skb_headlen(skb), fragptr,
-			       skb_frag_size(&skb_shinfo(skb)->frags[0]));
+		skb_copy_from_linear_data(skb, inl + 1, hlen);
+		if (shinfo->nr_frags)
+			memcpy(((void *)(inl + 1)) + hlen, fragptr,
+			       skb_frag_size(&shinfo->frags[0]));
 
 	} else {
 		inl->byte_count = cpu_to_be32(1 << 31 | spc);
-		if (skb_headlen(skb) <= spc) {
-			skb_copy_from_linear_data(skb, inl + 1, skb_headlen(skb));
-			if (skb_headlen(skb) < spc) {
-				memcpy(((void *)(inl + 1)) + skb_headlen(skb),
-					fragptr, spc - skb_headlen(skb));
-				fragptr +=  spc - skb_headlen(skb);
+		if (hlen <= spc) {
+			skb_copy_from_linear_data(skb, inl + 1, hlen);
+			if (hlen < spc) {
+				memcpy(((void *)(inl + 1)) + hlen,
+				       fragptr, spc - hlen);
+				fragptr +=  spc - hlen;
 			}
 			inl = (void *) (inl + 1) + spc;
 			memcpy(((void *)(inl + 1)), fragptr, skb->len - spc);
@@ -615,10 +652,11 @@ static void build_inline_wqe(struct mlx4_en_tx_desc *tx_desc, struct sk_buff *sk
 			skb_copy_from_linear_data(skb, inl + 1, spc);
 			inl = (void *) (inl + 1) + spc;
 			skb_copy_from_linear_data_offset(skb, spc, inl + 1,
-					skb_headlen(skb) - spc);
-			if (skb_shinfo(skb)->nr_frags)
-				memcpy(((void *)(inl + 1)) + skb_headlen(skb) - spc,
-					fragptr, skb_frag_size(&skb_shinfo(skb)->frags[0]));
+							 hlen - spc);
+			if (shinfo->nr_frags)
+				memcpy(((void *)(inl + 1)) + hlen - spc,
+				       fragptr,
+				       skb_frag_size(&shinfo->frags[0]));
 		}
 
 		wmb();
@@ -642,15 +680,16 @@ u16 mlx4_en_select_queue(struct net_device *dev, struct sk_buff *skb,
 	return fallback(dev, skb) % rings_p_up + up * rings_p_up;
 }
 
-static void mlx4_bf_copy(void __iomem *dst, unsigned long *src, unsigned bytecnt)
+static void mlx4_bf_copy(void __iomem *dst, const void *src,
+			 unsigned int bytecnt)
 {
 	__iowrite64_copy(dst, src, bytecnt / 8);
 }
 
 netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 {
+	struct skb_shared_info *shinfo = skb_shinfo(skb);
 	struct mlx4_en_priv *priv = netdev_priv(dev);
-	struct mlx4_en_dev *mdev = priv->mdev;
 	struct device *ddev = priv->ddev;
 	struct mlx4_en_tx_ring *ring;
 	struct mlx4_en_tx_desc *tx_desc;
@@ -663,15 +702,26 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	u32 index, bf_index;
 	__be32 op_own;
 	u16 vlan_tag = 0;
-	int i;
+	int i_frag;
 	int lso_header_size;
-	void *fragptr;
+	void *fragptr = NULL;
 	bool bounce = false;
+	bool send_doorbell;
+	bool stop_queue;
+	bool inline_ok;
+	u32 ring_cons;
 
 	if (!priv->port_up)
 		goto tx_drop;
 
-	real_size = get_real_size(skb, dev, &lso_header_size);
+	tx_ind = skb_get_queue_mapping(skb);
+	ring = priv->tx_ring[tx_ind];
+
+	/* fetch ring->cons far ahead before needing it to avoid stall */
+	ring_cons = ACCESS_ONCE(ring->cons);
+
+	real_size = get_real_size(skb, shinfo, dev, &lso_header_size,
+				  &inline_ok, &fragptr);
 	if (unlikely(!real_size))
 		goto tx_drop;
 
@@ -684,38 +734,15 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto tx_drop;
 	}
 
-	tx_ind = skb->queue_mapping;
-	ring = priv->tx_ring[tx_ind];
 	if (vlan_tx_tag_present(skb))
 		vlan_tag = vlan_tx_tag_get(skb);
 
-	/* Check available TXBBs And 2K spare for prefetch */
-	if (unlikely(((int)(ring->prod - ring->cons)) >
-		     ring->size - HEADROOM - MAX_DESC_TXBBS)) {
-		/* every full Tx ring stops queue */
-		netif_tx_stop_queue(ring->tx_queue);
-		ring->queue_stopped++;
 
-		/* If queue was emptied after the if, and before the
-		 * stop_queue - need to wake the queue, or else it will remain
-		 * stopped forever.
-		 * Need a memory barrier to make sure ring->cons was not
-		 * updated before queue was stopped.
-		 */
-		wmb();
-
-		if (unlikely(((int)(ring->prod - ring->cons)) <=
-			     ring->size - HEADROOM - MAX_DESC_TXBBS)) {
-			netif_tx_wake_queue(ring->tx_queue);
-			ring->wake_queue++;
-		} else {
-			return NETDEV_TX_BUSY;
-		}
-	}
+	netdev_txq_bql_enqueue_prefetchw(ring->tx_queue);
 
 	/* Track current inflight packets for performance analysis */
 	AVG_PERF_COUNTER(priv->pstats.inflight_avg,
-			 (u32) (ring->prod - ring->cons - 1));
+			 (u32)(ring->prod - ring_cons - 1));
 
 	/* Packet is good - grab an index and transmit it */
 	index = ring->prod & ring->size_mask;
@@ -735,46 +762,48 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	tx_info->skb = skb;
 	tx_info->nr_txbb = nr_txbb;
 
+	data = &tx_desc->data;
 	if (lso_header_size)
 		data = ((void *)&tx_desc->lso + ALIGN(lso_header_size + 4,
 						      DS_SIZE));
-	else
-		data = &tx_desc->data;
 
 	/* valid only for none inline segments */
 	tx_info->data_offset = (void *)data - (void *)tx_desc;
 
+	tx_info->inl = inline_ok;
+
 	tx_info->linear = (lso_header_size < skb_headlen(skb) &&
-			   !is_inline(ring->inline_thold, skb, NULL)) ? 1 : 0;
+			   !inline_ok) ? 1 : 0;
 
-	data += skb_shinfo(skb)->nr_frags + tx_info->linear - 1;
+	tx_info->nr_maps = shinfo->nr_frags + tx_info->linear;
+	data += tx_info->nr_maps - 1;
 
-	if (is_inline(ring->inline_thold, skb, &fragptr)) {
-		tx_info->inl = 1;
-	} else {
-		/* Map fragments */
-		for (i = skb_shinfo(skb)->nr_frags - 1; i >= 0; i--) {
-			struct skb_frag_struct *frag;
-			dma_addr_t dma;
+	if (!tx_info->inl) {
+		dma_addr_t dma = 0;
+		u32 byte_count = 0;
+
+		/* Map fragments if any */
+		for (i_frag = shinfo->nr_frags - 1; i_frag >= 0; i_frag--) {
+			const struct skb_frag_struct *frag;
 
-			frag = &skb_shinfo(skb)->frags[i];
+			frag = &shinfo->frags[i_frag];
+			byte_count = skb_frag_size(frag);
 			dma = skb_frag_dma_map(ddev, frag,
-					       0, skb_frag_size(frag),
+					       0, byte_count,
 					       DMA_TO_DEVICE);
 			if (dma_mapping_error(ddev, dma))
 				goto tx_drop_unmap;
 
 			data->addr = cpu_to_be64(dma);
-			data->lkey = cpu_to_be32(mdev->mr.key);
+			data->lkey = ring->mr_key;
 			wmb();
-			data->byte_count = cpu_to_be32(skb_frag_size(frag));
+			data->byte_count = cpu_to_be32(byte_count);
 			--data;
 		}
 
-		/* Map linear part */
+		/* Map linear part if needed */
 		if (tx_info->linear) {
-			u32 byte_count = skb_headlen(skb) - lso_header_size;
-			dma_addr_t dma;
+			byte_count = skb_headlen(skb) - lso_header_size;
 
 			dma = dma_map_single(ddev, skb->data +
 					     lso_header_size, byte_count,
@@ -783,29 +812,28 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 				goto tx_drop_unmap;
 
 			data->addr = cpu_to_be64(dma);
-			data->lkey = cpu_to_be32(mdev->mr.key);
+			data->lkey = ring->mr_key;
 			wmb();
 			data->byte_count = cpu_to_be32(byte_count);
 		}
-		tx_info->inl = 0;
+		/* tx completion can avoid cache line miss for common cases */
+		tx_info->map0_dma = dma;
+		tx_info->map0_byte_count = byte_count;
 	}
 
 	/*
 	 * For timestamping add flag to skb_shinfo and
 	 * set flag for further reference
 	 */
-	if (ring->hwtstamp_tx_type == HWTSTAMP_TX_ON &&
-	    skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
-		skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+	tx_info->ts_requested = 0;
+	if (unlikely(ring->hwtstamp_tx_type == HWTSTAMP_TX_ON &&
+		     shinfo->tx_flags & SKBTX_HW_TSTAMP)) {
+		shinfo->tx_flags |= SKBTX_IN_PROGRESS;
 		tx_info->ts_requested = 1;
 	}
 
 	/* Prepare ctrl segement apart opcode+ownership, which depends on
 	 * whether LSO is used */
-	tx_desc->ctrl.vlan_tag = cpu_to_be16(vlan_tag);
-	tx_desc->ctrl.ins_vlan = MLX4_WQE_CTRL_INS_VLAN *
-		!!vlan_tx_tag_present(skb);
-	tx_desc->ctrl.fence_size = (real_size / 16) & 0x3f;
 	tx_desc->ctrl.srcrb_flags = priv->ctrl_flags;
 	if (likely(skb->ip_summed == CHECKSUM_PARTIAL)) {
 		tx_desc->ctrl.srcrb_flags |= cpu_to_be32(MLX4_WQE_CTRL_IP_CSUM |
@@ -826,6 +854,8 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	/* Handle LSO (TSO) packets */
 	if (lso_header_size) {
+		int i;
+
 		/* Mark opcode as LSO */
 		op_own = cpu_to_be32(MLX4_OPCODE_LSO | (1 << 6)) |
 			((ring->prod & ring->size) ?
@@ -833,15 +863,16 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 
 		/* Fill in the LSO prefix */
 		tx_desc->lso.mss_hdr_size = cpu_to_be32(
-			skb_shinfo(skb)->gso_size << 16 | lso_header_size);
+			shinfo->gso_size << 16 | lso_header_size);
 
 		/* Copy headers;
 		 * note that we already verified that it is linear */
 		memcpy(tx_desc->lso.header, skb->data, lso_header_size);
 
-		priv->port_stats.tso_packets++;
-		i = ((skb->len - lso_header_size) / skb_shinfo(skb)->gso_size) +
-			!!((skb->len - lso_header_size) % skb_shinfo(skb)->gso_size);
+		ring->tso_packets++;
+
+		i = ((skb->len - lso_header_size) / shinfo->gso_size) +
+			!!((skb->len - lso_header_size) % shinfo->gso_size);
 		tx_info->nr_bytes = skb->len + (i - 1) * lso_header_size;
 		ring->packets += i;
 	} else {
@@ -851,16 +882,14 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 			 cpu_to_be32(MLX4_EN_BIT_DESC_OWN) : 0);
 		tx_info->nr_bytes = max_t(unsigned int, skb->len, ETH_ZLEN);
 		ring->packets++;
-
 	}
 	ring->bytes += tx_info->nr_bytes;
 	netdev_tx_sent_queue(ring->tx_queue, tx_info->nr_bytes);
 	AVG_PERF_COUNTER(priv->pstats.tx_pktsz_avg, skb->len);
 
-	if (tx_info->inl) {
-		build_inline_wqe(tx_desc, skb, real_size, &vlan_tag, tx_ind, fragptr);
-		tx_info->inl = 1;
-	}
+	if (tx_info->inl)
+		build_inline_wqe(tx_desc, skb, shinfo, real_size, &vlan_tag,
+				 tx_ind, fragptr);
 
 	if (skb->encapsulation) {
 		struct iphdr *ipv4 = (struct iphdr *)skb_inner_network_header(skb);
@@ -873,44 +902,85 @@ netdev_tx_t mlx4_en_xmit(struct sk_buff *skb, struct net_device *dev)
 	ring->prod += nr_txbb;
 
 	/* If we used a bounce buffer then copy descriptor back into place */
-	if (bounce)
+	if (unlikely(bounce))
 		tx_desc = mlx4_en_bounce_to_desc(priv, ring, index, desc_size);
 
 	skb_tx_timestamp(skb);
 
-	if (ring->bf_enabled && desc_size <= MAX_BF && !bounce && !vlan_tx_tag_present(skb)) {
-		tx_desc->ctrl.bf_qpn |= cpu_to_be32(ring->doorbell_qpn);
+	/* Check available TXBBs And 2K spare for prefetch */
+	stop_queue = (int)(ring->prod - ring_cons) >
+		      ring->size - HEADROOM - MAX_DESC_TXBBS;
+	if (unlikely(stop_queue)) {
+		netif_tx_stop_queue(ring->tx_queue);
+		ring->queue_stopped++;
+	}
+	send_doorbell = !skb->xmit_more || netif_xmit_stopped(ring->tx_queue);
+
+	real_size = (real_size / 16) & 0x3f;
+
+	if (ring->bf_enabled && desc_size <= MAX_BF && !bounce &&
+	    !vlan_tx_tag_present(skb) && send_doorbell) {
+		tx_desc->ctrl.bf_qpn = ring->doorbell_qpn |
+				       cpu_to_be32(real_size);
 
 		op_own |= htonl((bf_index & 0xffff) << 8);
-		/* Ensure new descirptor hits memory
-		* before setting ownership of this descriptor to HW */
+		/* Ensure new descriptor hits memory
+		 * before setting ownership of this descriptor to HW
+		 */
 		wmb();
 		tx_desc->ctrl.owner_opcode = op_own;
 
 		wmb();
 
-		mlx4_bf_copy(ring->bf.reg + ring->bf.offset, (unsigned long *) &tx_desc->ctrl,
-		     desc_size);
+		mlx4_bf_copy(ring->bf.reg + ring->bf.offset, &tx_desc->ctrl,
+			     desc_size);
 
 		wmb();
 
 		ring->bf.offset ^= ring->bf.buf_size;
 	} else {
-		/* Ensure new descirptor hits memory
-		* before setting ownership of this descriptor to HW */
+		tx_desc->ctrl.vlan_tag = cpu_to_be16(vlan_tag);
+		tx_desc->ctrl.ins_vlan = MLX4_WQE_CTRL_INS_VLAN *
+			!!vlan_tx_tag_present(skb);
+		tx_desc->ctrl.fence_size = real_size;
+
+		/* Ensure new descriptor hits memory
+		 * before setting ownership of this descriptor to HW
+		 */
 		wmb();
 		tx_desc->ctrl.owner_opcode = op_own;
-		wmb();
-		iowrite32be(ring->doorbell_qpn, ring->bf.uar->map + MLX4_SEND_DOORBELL);
+		if (send_doorbell) {
+			wmb();
+			iowrite32(ring->doorbell_qpn,
+				  ring->bf.uar->map + MLX4_SEND_DOORBELL);
+		} else {
+			ring->xmit_more++;
+		}
 	}
 
+	if (unlikely(stop_queue)) {
+		/* If queue was emptied after the if (stop_queue) , and before
+		 * the netif_tx_stop_queue() - need to wake the queue,
+		 * or else it will remain stopped forever.
+		 * Need a memory barrier to make sure ring->cons was not
+		 * updated before queue was stopped.
+		 */
+		smp_rmb();
+
+		ring_cons = ACCESS_ONCE(ring->cons);
+		if (unlikely(((int)(ring->prod - ring_cons)) <=
+			     ring->size - HEADROOM - MAX_DESC_TXBBS)) {
+			netif_tx_wake_queue(ring->tx_queue);
+			ring->wake_queue++;
+		}
+	}
 	return NETDEV_TX_OK;
 
 tx_drop_unmap:
 	en_err(priv, "DMA mapping error\n");
 
-	for (i++; i < skb_shinfo(skb)->nr_frags; i++) {
-		data++;
+	while (++i_frag < shinfo->nr_frags) {
+		++data;
 		dma_unmap_page(ddev, (dma_addr_t) be64_to_cpu(data->addr),
 			       be32_to_cpu(data->byte_count),
 			       PCI_DMA_TODEVICE);
diff --git a/drivers/net/ethernet/mellanox/mlx4/eq.c b/drivers/net/ethernet/mellanox/mlx4/eq.c
index 2a004b347e1d..a49c9d11d8a5 100644
--- a/drivers/net/ethernet/mellanox/mlx4/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx4/eq.c
@@ -101,21 +101,24 @@ static void eq_set_ci(struct mlx4_eq *eq, int req_not)
 	mb();
 }
 
-static struct mlx4_eqe *get_eqe(struct mlx4_eq *eq, u32 entry, u8 eqe_factor)
+static struct mlx4_eqe *get_eqe(struct mlx4_eq *eq, u32 entry, u8 eqe_factor,
+				u8 eqe_size)
 {
 	/* (entry & (eq->nent - 1)) gives us a cyclic array */
-	unsigned long offset = (entry & (eq->nent - 1)) * (MLX4_EQ_ENTRY_SIZE << eqe_factor);
-	/* CX3 is capable of extending the EQE from 32 to 64 bytes.
-	 * When this feature is enabled, the first (in the lower addresses)
+	unsigned long offset = (entry & (eq->nent - 1)) * eqe_size;
+	/* CX3 is capable of extending the EQE from 32 to 64 bytes with
+	 * strides of 64B,128B and 256B.
+	 * When 64B EQE is used, the first (in the lower addresses)
 	 * 32 bytes in the 64 byte EQE are reserved and the next 32 bytes
 	 * contain the legacy EQE information.
+	 * In all other cases, the first 32B contains the legacy EQE info.
 	 */
 	return eq->page_list[offset / PAGE_SIZE].buf + (offset + (eqe_factor ? MLX4_EQ_ENTRY_SIZE : 0)) % PAGE_SIZE;
 }
 
-static struct mlx4_eqe *next_eqe_sw(struct mlx4_eq *eq, u8 eqe_factor)
+static struct mlx4_eqe *next_eqe_sw(struct mlx4_eq *eq, u8 eqe_factor, u8 size)
 {
-	struct mlx4_eqe *eqe = get_eqe(eq, eq->cons_index, eqe_factor);
+	struct mlx4_eqe *eqe = get_eqe(eq, eq->cons_index, eqe_factor, size);
 	return !!(eqe->owner & 0x80) ^ !!(eq->cons_index & eq->nent) ? NULL : eqe;
 }
 
@@ -459,8 +462,9 @@ static int mlx4_eq_int(struct mlx4_dev *dev, struct mlx4_eq *eq)
 	enum slave_port_gen_event gen_event;
 	unsigned long flags;
 	struct mlx4_vport_state *s_info;
+	int eqe_size = dev->caps.eqe_size;
 
-	while ((eqe = next_eqe_sw(eq, dev->caps.eqe_factor))) {
+	while ((eqe = next_eqe_sw(eq, dev->caps.eqe_factor, eqe_size))) {
 		/*
 		 * Make sure we read EQ entry contents after we've
 		 * checked the ownership bit.
@@ -894,8 +898,10 @@ static int mlx4_create_eq(struct mlx4_dev *dev, int nent,
 
 	eq->dev   = dev;
 	eq->nent  = roundup_pow_of_two(max(nent, 2));
-	/* CX3 is capable of extending the CQE/EQE from 32 to 64 bytes */
-	npages = PAGE_ALIGN(eq->nent * (MLX4_EQ_ENTRY_SIZE << dev->caps.eqe_factor)) / PAGE_SIZE;
+	/* CX3 is capable of extending the CQE/EQE from 32 to 64 bytes, with
+	 * strides of 64B,128B and 256B.
+	 */
+	npages = PAGE_ALIGN(eq->nent * dev->caps.eqe_size) / PAGE_SIZE;
 
 	eq->page_list = kmalloc(npages * sizeof *eq->page_list,
 				GFP_KERNEL);
@@ -997,8 +1003,10 @@ static void mlx4_free_eq(struct mlx4_dev *dev,
 	struct mlx4_cmd_mailbox *mailbox;
 	int err;
 	int i;
-	/* CX3 is capable of extending the CQE/EQE from 32 to 64 bytes */
-	int npages = PAGE_ALIGN((MLX4_EQ_ENTRY_SIZE << dev->caps.eqe_factor) * eq->nent) / PAGE_SIZE;
+	/* CX3 is capable of extending the CQE/EQE from 32 to 64 bytes, with
+	 * strides of 64B,128B and 256B
+	 */
+	int npages = PAGE_ALIGN(dev->caps.eqe_size  * eq->nent) / PAGE_SIZE;
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
 	if (IS_ERR(mailbox))
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.c b/drivers/net/ethernet/mellanox/mlx4/fw.c
index 494753e44ae3..2e88a235e26b 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.c
@@ -137,7 +137,9 @@ static void dump_dev_cap_flags2(struct mlx4_dev *dev, u64 flags)
 		[8] = "Dynamic QP updates support",
 		[9] = "Device managed flow steering IPoIB support",
 		[10] = "TCP/IP offloads/flow-steering for VXLAN support",
-		[11] = "MAD DEMUX (Secure-Host) support"
+		[11] = "MAD DEMUX (Secure-Host) support",
+		[12] = "Large cache line (>64B) CQE stride support",
+		[13] = "Large cache line (>64B) EQE stride support"
 	};
 	int i;
 
@@ -557,6 +559,7 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 #define QUERY_DEV_CAP_FLOW_STEERING_IPOIB_OFFSET	0x74
 #define QUERY_DEV_CAP_FLOW_STEERING_RANGE_EN_OFFSET	0x76
 #define QUERY_DEV_CAP_FLOW_STEERING_MAX_QP_OFFSET	0x77
+#define QUERY_DEV_CAP_CQ_EQ_CACHE_LINE_STRIDE	0x7a
 #define QUERY_DEV_CAP_RDMARC_ENTRY_SZ_OFFSET	0x80
 #define QUERY_DEV_CAP_QPC_ENTRY_SZ_OFFSET	0x82
 #define QUERY_DEV_CAP_AUX_ENTRY_SZ_OFFSET	0x84
@@ -733,6 +736,11 @@ int mlx4_QUERY_DEV_CAP(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	dev_cap->max_rq_sg = field;
 	MLX4_GET(size, outbox, QUERY_DEV_CAP_MAX_DESC_SZ_RQ_OFFSET);
 	dev_cap->max_rq_desc_sz = size;
+	MLX4_GET(field, outbox, QUERY_DEV_CAP_CQ_EQ_CACHE_LINE_STRIDE);
+	if (field & (1 << 6))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_CQE_STRIDE;
+	if (field & (1 << 7))
+		dev_cap->flags2 |= MLX4_DEV_CAP_FLAG2_EQE_STRIDE;
 
 	MLX4_GET(dev_cap->bmme_flags, outbox,
 		 QUERY_DEV_CAP_BMME_FLAGS_OFFSET);
@@ -974,8 +982,13 @@ int mlx4_QUERY_PORT_wrapper(struct mlx4_dev *dev, int slave,
 	if (port < 0)
 		return -EINVAL;
 
-	vhcr->in_modifier = (vhcr->in_modifier & ~0xFF) |
-			    (port & 0xFF);
+	/* Protect against untrusted guests: enforce that this is the
+	 * QUERY_PORT general query.
+	 */
+	if (vhcr->op_modifier || vhcr->in_modifier & ~0xFF)
+		return -EINVAL;
+
+	vhcr->in_modifier = port;
 
 	err = mlx4_cmd_box(dev, 0, outbox->dma, vhcr->in_modifier, 0,
 			   MLX4_CMD_QUERY_PORT, MLX4_CMD_TIME_CLASS_B,
@@ -1376,6 +1389,7 @@ int mlx4_INIT_HCA(struct mlx4_dev *dev, struct mlx4_init_hca_param *param)
 #define	 INIT_HCA_CQC_BASE_OFFSET	 (INIT_HCA_QPC_OFFSET + 0x30)
 #define	 INIT_HCA_LOG_CQ_OFFSET		 (INIT_HCA_QPC_OFFSET + 0x37)
 #define	 INIT_HCA_EQE_CQE_OFFSETS	 (INIT_HCA_QPC_OFFSET + 0x38)
+#define	 INIT_HCA_EQE_CQE_STRIDE_OFFSET  (INIT_HCA_QPC_OFFSET + 0x3b)
 #define	 INIT_HCA_ALTC_BASE_OFFSET	 (INIT_HCA_QPC_OFFSET + 0x40)
 #define	 INIT_HCA_AUXC_BASE_OFFSET	 (INIT_HCA_QPC_OFFSET + 0x50)
 #define	 INIT_HCA_EQC_BASE_OFFSET	 (INIT_HCA_QPC_OFFSET + 0x60)
@@ -1452,11 +1466,25 @@ int mlx4_INIT_HCA(struct mlx4_dev *dev, struct mlx4_init_hca_param *param)
 	if (dev->caps.flags & MLX4_DEV_CAP_FLAG_64B_CQE) {
 		*(inbox + INIT_HCA_EQE_CQE_OFFSETS / 4) |= cpu_to_be32(1 << 30);
 		dev->caps.cqe_size   = 64;
-		dev->caps.userspace_caps |= MLX4_USER_DEV_CAP_64B_CQE;
+		dev->caps.userspace_caps |= MLX4_USER_DEV_CAP_LARGE_CQE;
 	} else {
 		dev->caps.cqe_size   = 32;
 	}
 
+	/* CX3 is capable of extending CQEs\EQEs to strides larger than 64B */
+	if ((dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_EQE_STRIDE) &&
+	    (dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_CQE_STRIDE)) {
+		dev->caps.eqe_size = cache_line_size();
+		dev->caps.cqe_size = cache_line_size();
+		dev->caps.eqe_factor = 0;
+		MLX4_PUT(inbox, (u8)((ilog2(dev->caps.eqe_size) - 5) << 4 |
+				      (ilog2(dev->caps.eqe_size) - 5)),
+			 INIT_HCA_EQE_CQE_STRIDE_OFFSET);
+
+		/* User still need to know to support CQE > 32B */
+		dev->caps.userspace_caps |= MLX4_USER_DEV_CAP_LARGE_CQE;
+	}
+
 	/* QPC/EEC/CQC/EQC/RDMARC attributes */
 
 	MLX4_PUT(inbox, param->qpc_base,      INIT_HCA_QPC_BASE_OFFSET);
@@ -1616,6 +1644,17 @@ int mlx4_QUERY_HCA(struct mlx4_dev *dev,
 	if (byte_field & 0x40) /* 64-bytes cqe enabled */
 		param->dev_cap_enabled |= MLX4_DEV_CAP_64B_CQE_ENABLED;
 
+	/* CX3 is capable of extending CQEs\EQEs to strides larger than 64B */
+	MLX4_GET(byte_field, outbox, INIT_HCA_EQE_CQE_STRIDE_OFFSET);
+	if (byte_field) {
+		param->dev_cap_enabled |= MLX4_DEV_CAP_64B_EQE_ENABLED;
+		param->dev_cap_enabled |= MLX4_DEV_CAP_64B_CQE_ENABLED;
+		param->cqe_size = 1 << ((byte_field &
+					 MLX4_CQE_SIZE_MASK_STRIDE) + 5);
+		param->eqe_size = 1 << (((byte_field &
+					  MLX4_EQE_SIZE_MASK_STRIDE) >> 4) + 5);
+	}
+
 	/* TPT attributes */
 
 	MLX4_GET(param->dmpt_base,  outbox, INIT_HCA_DMPT_BASE_OFFSET);
diff --git a/drivers/net/ethernet/mellanox/mlx4/fw.h b/drivers/net/ethernet/mellanox/mlx4/fw.h
index 1fce03ebe5c4..9b835aecac96 100644
--- a/drivers/net/ethernet/mellanox/mlx4/fw.h
+++ b/drivers/net/ethernet/mellanox/mlx4/fw.h
@@ -178,6 +178,8 @@ struct mlx4_init_hca_param {
 	u8  uar_page_sz; /* log pg sz in 4k chunks */
 	u8  steering_mode; /* for QUERY_HCA */
 	u64 dev_cap_enabled;
+	u16 cqe_size; /* For use only when CQE stride feature enabled */
+	u16 eqe_size; /* For use only when EQE stride feature enabled */
 };
 
 struct mlx4_init_ib_param {
diff --git a/drivers/net/ethernet/mellanox/mlx4/main.c b/drivers/net/ethernet/mellanox/mlx4/main.c
index 7e2d5d57c598..90de6e1ad06e 100644
--- a/drivers/net/ethernet/mellanox/mlx4/main.c
+++ b/drivers/net/ethernet/mellanox/mlx4/main.c
@@ -78,13 +78,13 @@ MODULE_PARM_DESC(msi_x, "attempt to use MSI-X if nonzero");
 #endif /* CONFIG_PCI_MSI */
 
 static uint8_t num_vfs[3] = {0, 0, 0};
-static int num_vfs_argc = 3;
+static int num_vfs_argc;
 module_param_array(num_vfs, byte , &num_vfs_argc, 0444);
 MODULE_PARM_DESC(num_vfs, "enable #num_vfs functions if num_vfs > 0\n"
 			  "num_vfs=port1,port2,port1+2");
 
 static uint8_t probe_vf[3] = {0, 0, 0};
-static int probe_vfs_argc = 3;
+static int probe_vfs_argc;
 module_param_array(probe_vf, byte, &probe_vfs_argc, 0444);
 MODULE_PARM_DESC(probe_vf, "number of vfs to probe by pf driver (num_vfs > 0)\n"
 			   "probe_vf=port1,port2,port1+2");
@@ -104,7 +104,8 @@ module_param(enable_64b_cqe_eqe, bool, 0444);
 MODULE_PARM_DESC(enable_64b_cqe_eqe,
 		 "Enable 64 byte CQEs/EQEs when the FW supports this (default: True)");
 
-#define PF_CONTEXT_BEHAVIOUR_MASK	MLX4_FUNC_CAP_64B_EQE_CQE
+#define PF_CONTEXT_BEHAVIOUR_MASK	(MLX4_FUNC_CAP_64B_EQE_CQE | \
+					 MLX4_FUNC_CAP_EQE_CQE_STRIDE)
 
 static char mlx4_version[] =
 	DRV_NAME ": Mellanox ConnectX core driver v"
@@ -196,6 +197,40 @@ static void mlx4_set_port_mask(struct mlx4_dev *dev)
 		dev->caps.port_mask[i] = dev->caps.port_type[i];
 }
 
+static void mlx4_enable_cqe_eqe_stride(struct mlx4_dev *dev)
+{
+	struct mlx4_caps *dev_cap = &dev->caps;
+
+	/* FW not supporting or cancelled by user */
+	if (!(dev_cap->flags2 & MLX4_DEV_CAP_FLAG2_EQE_STRIDE) ||
+	    !(dev_cap->flags2 & MLX4_DEV_CAP_FLAG2_CQE_STRIDE))
+		return;
+
+	/* Must have 64B CQE_EQE enabled by FW to use bigger stride
+	 * When FW has NCSI it may decide not to report 64B CQE/EQEs
+	 */
+	if (!(dev_cap->flags & MLX4_DEV_CAP_FLAG_64B_EQE) ||
+	    !(dev_cap->flags & MLX4_DEV_CAP_FLAG_64B_CQE)) {
+		dev_cap->flags2 &= ~MLX4_DEV_CAP_FLAG2_CQE_STRIDE;
+		dev_cap->flags2 &= ~MLX4_DEV_CAP_FLAG2_EQE_STRIDE;
+		return;
+	}
+
+	if (cache_line_size() == 128 || cache_line_size() == 256) {
+		mlx4_dbg(dev, "Enabling CQE stride cacheLine supported\n");
+		/* Changing the real data inside CQE size to 32B */
+		dev_cap->flags &= ~MLX4_DEV_CAP_FLAG_64B_CQE;
+		dev_cap->flags &= ~MLX4_DEV_CAP_FLAG_64B_EQE;
+
+		if (mlx4_is_master(dev))
+			dev_cap->function_caps |= MLX4_FUNC_CAP_EQE_CQE_STRIDE;
+	} else {
+		mlx4_dbg(dev, "Disabling CQE stride cacheLine unsupported\n");
+		dev_cap->flags2 &= ~MLX4_DEV_CAP_FLAG2_CQE_STRIDE;
+		dev_cap->flags2 &= ~MLX4_DEV_CAP_FLAG2_EQE_STRIDE;
+	}
+}
+
 static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 {
 	int err;
@@ -390,6 +425,14 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 			dev->caps.flags &= ~MLX4_DEV_CAP_FLAG_64B_CQE;
 			dev->caps.flags &= ~MLX4_DEV_CAP_FLAG_64B_EQE;
 		}
+
+		if (dev_cap->flags2 &
+		    (MLX4_DEV_CAP_FLAG2_CQE_STRIDE |
+		     MLX4_DEV_CAP_FLAG2_EQE_STRIDE)) {
+			mlx4_warn(dev, "Disabling EQE/CQE stride per user request\n");
+			dev_cap->flags2 &= ~MLX4_DEV_CAP_FLAG2_CQE_STRIDE;
+			dev_cap->flags2 &= ~MLX4_DEV_CAP_FLAG2_EQE_STRIDE;
+		}
 	}
 
 	if ((dev->caps.flags &
@@ -397,6 +440,9 @@ static int mlx4_dev_cap(struct mlx4_dev *dev, struct mlx4_dev_cap *dev_cap)
 	    mlx4_is_master(dev))
 		dev->caps.function_caps |= MLX4_FUNC_CAP_64B_EQE_CQE;
 
+	if (!mlx4_is_slave(dev))
+		mlx4_enable_cqe_eqe_stride(dev);
+
 	return 0;
 }
 
@@ -724,11 +770,22 @@ static int mlx4_slave_cap(struct mlx4_dev *dev)
 
 	if (hca_param.dev_cap_enabled & MLX4_DEV_CAP_64B_CQE_ENABLED) {
 		dev->caps.cqe_size   = 64;
-		dev->caps.userspace_caps |= MLX4_USER_DEV_CAP_64B_CQE;
+		dev->caps.userspace_caps |= MLX4_USER_DEV_CAP_LARGE_CQE;
 	} else {
 		dev->caps.cqe_size   = 32;
 	}
 
+	if (hca_param.dev_cap_enabled & MLX4_DEV_CAP_EQE_STRIDE_ENABLED) {
+		dev->caps.eqe_size = hca_param.eqe_size;
+		dev->caps.eqe_factor = 0;
+	}
+
+	if (hca_param.dev_cap_enabled & MLX4_DEV_CAP_CQE_STRIDE_ENABLED) {
+		dev->caps.cqe_size = hca_param.cqe_size;
+		/* User still need to know when CQE > 32B */
+		dev->caps.userspace_caps |= MLX4_USER_DEV_CAP_LARGE_CQE;
+	}
+
 	dev->caps.flags2 &= ~MLX4_DEV_CAP_FLAG2_TS;
 	mlx4_warn(dev, "Timestamping is not supported in slave mode\n");
 
@@ -2202,115 +2259,18 @@ static void mlx4_free_ownership(struct mlx4_dev *dev)
 	iounmap(owner);
 }
 
-static int __mlx4_init_one(struct pci_dev *pdev, int pci_dev_data)
+static int mlx4_load_one(struct pci_dev *pdev, int pci_dev_data,
+			 int total_vfs, int *nvfs, struct mlx4_priv *priv)
 {
-	struct mlx4_priv *priv;
 	struct mlx4_dev *dev;
+	unsigned sum = 0;
 	int err;
 	int port;
-	int nvfs[MLX4_MAX_PORTS + 1] = {0, 0, 0};
-	int prb_vf[MLX4_MAX_PORTS + 1] = {0, 0, 0};
-	const int param_map[MLX4_MAX_PORTS + 1][MLX4_MAX_PORTS + 1] = {
-		{2, 0, 0}, {0, 1, 2}, {0, 1, 2} };
-	unsigned total_vfs = 0;
-	int sriov_initialized = 0;
-	unsigned int i;
-
-	pr_info(DRV_NAME ": Initializing %s\n", pci_name(pdev));
-
-	err = pci_enable_device(pdev);
-	if (err) {
-		dev_err(&pdev->dev, "Cannot enable PCI device, aborting\n");
-		return err;
-	}
-
-	/* Due to requirement that all VFs and the PF are *guaranteed* 2 MACS
-	 * per port, we must limit the number of VFs to 63 (since their are
-	 * 128 MACs)
-	 */
-	for (i = 0; i < sizeof(nvfs)/sizeof(nvfs[0]) && i < num_vfs_argc;
-	     total_vfs += nvfs[param_map[num_vfs_argc - 1][i]], i++) {
-		nvfs[param_map[num_vfs_argc - 1][i]] = num_vfs[i];
-		if (nvfs[i] < 0) {
-			dev_err(&pdev->dev, "num_vfs module parameter cannot be negative\n");
-			return -EINVAL;
-		}
-	}
-	for (i = 0; i < sizeof(prb_vf)/sizeof(prb_vf[0]) && i < probe_vfs_argc;
-	     i++) {
-		prb_vf[param_map[probe_vfs_argc - 1][i]] = probe_vf[i];
-		if (prb_vf[i] < 0 || prb_vf[i] > nvfs[i]) {
-			dev_err(&pdev->dev, "probe_vf module parameter cannot be negative or greater than num_vfs\n");
-			return -EINVAL;
-		}
-	}
-	if (total_vfs >= MLX4_MAX_NUM_VF) {
-		dev_err(&pdev->dev,
-			"Requested more VF's (%d) than allowed (%d)\n",
-			total_vfs, MLX4_MAX_NUM_VF - 1);
-		return -EINVAL;
-	}
-
-	for (i = 0; i < MLX4_MAX_PORTS; i++) {
-		if (nvfs[i] + nvfs[2] >= MLX4_MAX_NUM_VF_P_PORT) {
-			dev_err(&pdev->dev,
-				"Requested more VF's (%d) for port (%d) than allowed (%d)\n",
-				nvfs[i] + nvfs[2], i + 1,
-				MLX4_MAX_NUM_VF_P_PORT - 1);
-			return -EINVAL;
-		}
-	}
-
-
-	/*
-	 * Check for BARs.
-	 */
-	if (!(pci_dev_data & MLX4_PCI_DEV_IS_VF) &&
-	    !(pci_resource_flags(pdev, 0) & IORESOURCE_MEM)) {
-		dev_err(&pdev->dev, "Missing DCS, aborting (driver_data: 0x%x, pci_resource_flags(pdev, 0):0x%lx)\n",
-			pci_dev_data, pci_resource_flags(pdev, 0));
-		err = -ENODEV;
-		goto err_disable_pdev;
-	}
-	if (!(pci_resource_flags(pdev, 2) & IORESOURCE_MEM)) {
-		dev_err(&pdev->dev, "Missing UAR, aborting\n");
-		err = -ENODEV;
-		goto err_disable_pdev;
-	}
-
-	err = pci_request_regions(pdev, DRV_NAME);
-	if (err) {
-		dev_err(&pdev->dev, "Couldn't get PCI resources, aborting\n");
-		goto err_disable_pdev;
-	}
-
-	pci_set_master(pdev);
-
-	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
-	if (err) {
-		dev_warn(&pdev->dev, "Warning: couldn't set 64-bit PCI DMA mask\n");
-		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
-		if (err) {
-			dev_err(&pdev->dev, "Can't set PCI DMA mask, aborting\n");
-			goto err_release_regions;
-		}
-	}
-	err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
-	if (err) {
-		dev_warn(&pdev->dev, "Warning: couldn't set 64-bit consistent PCI DMA mask\n");
-		err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
-		if (err) {
-			dev_err(&pdev->dev, "Can't set consistent PCI DMA mask, aborting\n");
-			goto err_release_regions;
-		}
-	}
+	int i;
+	int existing_vfs = 0;
 
-	/* Allow large DMA segments, up to the firmware limit of 1 GB */
-	dma_set_max_seg_size(&pdev->dev, 1024 * 1024 * 1024);
+	dev = &priv->dev;
 
-	dev       = pci_get_drvdata(pdev);
-	priv      = mlx4_priv(dev);
-	dev->pdev = pdev;
 	INIT_LIST_HEAD(&priv->ctx_list);
 	spin_lock_init(&priv->ctx_lock);
 
@@ -2324,28 +2284,9 @@ static int __mlx4_init_one(struct pci_dev *pdev, int pci_dev_data)
 
 	dev->rev_id = pdev->revision;
 	dev->numa_node = dev_to_node(&pdev->dev);
+
 	/* Detect if this device is a virtual function */
 	if (pci_dev_data & MLX4_PCI_DEV_IS_VF) {
-		/* When acting as pf, we normally skip vfs unless explicitly
-		 * requested to probe them. */
-		if (total_vfs) {
-			unsigned vfs_offset = 0;
-			for (i = 0; i < sizeof(nvfs)/sizeof(nvfs[0]) &&
-				     vfs_offset + nvfs[i] < extended_func_num(pdev);
-			     vfs_offset += nvfs[i], i++)
-				;
-			if (i == sizeof(nvfs)/sizeof(nvfs[0])) {
-				err = -ENODEV;
-				goto err_free_dev;
-			}
-			if ((extended_func_num(pdev) - vfs_offset)
-			    > prb_vf[i]) {
-				mlx4_warn(dev, "Skipping virtual function:%d\n",
-					  extended_func_num(pdev));
-				err = -ENODEV;
-				goto err_free_dev;
-			}
-		}
 		mlx4_warn(dev, "Detected virtual function - running in slave mode\n");
 		dev->flags |= MLX4_FLAG_SLAVE;
 	} else {
@@ -2355,11 +2296,10 @@ static int __mlx4_init_one(struct pci_dev *pdev, int pci_dev_data)
 		err = mlx4_get_ownership(dev);
 		if (err) {
 			if (err < 0)
-				goto err_free_dev;
+				return err;
 			else {
 				mlx4_warn(dev, "Multiple PFs not yet supported - Skipping PF\n");
-				err = -EINVAL;
-				goto err_free_dev;
+				return -EINVAL;
 			}
 		}
 
@@ -2371,21 +2311,28 @@ static int __mlx4_init_one(struct pci_dev *pdev, int pci_dev_data)
 				GFP_KERNEL);
 			if (NULL == dev->dev_vfs) {
 				mlx4_err(dev, "Failed to allocate memory for VFs\n");
-				err = 0;
+				err = -ENOMEM;
+				goto err_free_own;
 			} else {
 				atomic_inc(&pf_loading);
-				err = pci_enable_sriov(pdev, total_vfs);
+				existing_vfs = pci_num_vf(pdev);
+				if (existing_vfs) {
+					err = 0;
+					if (existing_vfs != total_vfs)
+						mlx4_err(dev, "SR-IOV was already enabled, but with num_vfs (%d) different than requested (%d)\n",
+							 existing_vfs, total_vfs);
+				} else {
+					err = pci_enable_sriov(pdev, total_vfs);
+				}
 				if (err) {
 					mlx4_err(dev, "Failed to enable SR-IOV, continuing without SR-IOV (err = %d)\n",
 						 err);
 					atomic_dec(&pf_loading);
-					err = 0;
 				} else {
 					mlx4_warn(dev, "Running in master mode\n");
 					dev->flags |= MLX4_FLAG_SRIOV |
 						MLX4_FLAG_MASTER;
 					dev->num_vfs = total_vfs;
-					sriov_initialized = 1;
 				}
 			}
 		}
@@ -2401,7 +2348,7 @@ static int __mlx4_init_one(struct pci_dev *pdev, int pci_dev_data)
 		err = mlx4_reset(dev);
 		if (err) {
 			mlx4_err(dev, "Failed to reset HCA, aborting\n");
-			goto err_rel_own;
+			goto err_sriov;
 		}
 	}
 
@@ -2451,34 +2398,46 @@ slave_start:
 	/* In master functions, the communication channel must be initialized
 	 * after obtaining its address from fw */
 	if (mlx4_is_master(dev)) {
-		unsigned sum = 0;
-		err = mlx4_multi_func_init(dev);
-		if (err) {
-			mlx4_err(dev, "Failed to init master mfunc interface, aborting\n");
+		int ib_ports = 0;
+
+		mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB)
+			ib_ports++;
+
+		if (ib_ports &&
+		    (num_vfs_argc > 1 || probe_vfs_argc > 1)) {
+			mlx4_err(dev,
+				 "Invalid syntax of num_vfs/probe_vfs with IB port - single port VFs syntax is only supported when all ports are configured as ethernet\n");
+			err = -EINVAL;
+			goto err_close;
+		}
+		if (dev->caps.num_ports < 2 &&
+		    num_vfs_argc > 1) {
+			err = -EINVAL;
+			mlx4_err(dev,
+				 "Error: Trying to configure VFs on port 2, but HCA has only %d physical ports\n",
+				 dev->caps.num_ports);
 			goto err_close;
 		}
-		if (sriov_initialized) {
-			int ib_ports = 0;
-			mlx4_foreach_port(i, dev, MLX4_PORT_TYPE_IB)
-				ib_ports++;
+		memcpy(dev->nvfs, nvfs, sizeof(dev->nvfs));
 
-			if (ib_ports &&
-			    (num_vfs_argc > 1 || probe_vfs_argc > 1)) {
-				mlx4_err(dev,
-					 "Invalid syntax of num_vfs/probe_vfs with IB port - single port VFs syntax is only supported when all ports are configured as ethernet\n");
-				err = -EINVAL;
-				goto err_master_mfunc;
-			}
-			for (i = 0; i < sizeof(nvfs)/sizeof(nvfs[0]); i++) {
-				unsigned j;
-				for (j = 0; j < nvfs[i]; ++sum, ++j) {
-					dev->dev_vfs[sum].min_port =
-						i < 2 ? i + 1 : 1;
-					dev->dev_vfs[sum].n_ports = i < 2 ? 1 :
-						dev->caps.num_ports;
-				}
+		for (i = 0; i < sizeof(dev->nvfs)/sizeof(dev->nvfs[0]); i++) {
+			unsigned j;
+
+			for (j = 0; j < dev->nvfs[i]; ++sum, ++j) {
+				dev->dev_vfs[sum].min_port = i < 2 ? i + 1 : 1;
+				dev->dev_vfs[sum].n_ports = i < 2 ? 1 :
+					dev->caps.num_ports;
 			}
 		}
+
+		/* In master functions, the communication channel
+		 * must be initialized after obtaining its address from fw
+		 */
+		err = mlx4_multi_func_init(dev);
+		if (err) {
+			mlx4_err(dev, "Failed to init master mfunc interface, aborting.\n");
+			goto err_close;
+		}
 	}
 
 	err = mlx4_alloc_eq_table(dev);
@@ -2499,7 +2458,7 @@ slave_start:
 	if (!mlx4_is_slave(dev)) {
 		err = mlx4_init_steering(dev);
 		if (err)
-			goto err_free_eq;
+			goto err_disable_msix;
 	}
 
 	err = mlx4_setup_hca(dev);
@@ -2559,6 +2518,10 @@ err_steer:
 	if (!mlx4_is_slave(dev))
 		mlx4_clear_steering(dev);
 
+err_disable_msix:
+	if (dev->flags & MLX4_FLAG_MSI_X)
+		pci_disable_msix(pdev);
+
 err_free_eq:
 	mlx4_free_eq_table(dev);
 
@@ -2575,9 +2538,6 @@ err_master_mfunc:
 	}
 
 err_close:
-	if (dev->flags & MLX4_FLAG_MSI_X)
-		pci_disable_msix(pdev);
-
 	mlx4_close_hca(dev);
 
 err_mfunc:
@@ -2588,20 +2548,154 @@ err_cmd:
 	mlx4_cmd_cleanup(dev);
 
 err_sriov:
-	if (dev->flags & MLX4_FLAG_SRIOV)
+	if (dev->flags & MLX4_FLAG_SRIOV && !existing_vfs)
 		pci_disable_sriov(pdev);
 
-err_rel_own:
-	if (!mlx4_is_slave(dev))
-		mlx4_free_ownership(dev);
-
 	if (mlx4_is_master(dev) && dev->num_vfs)
 		atomic_dec(&pf_loading);
 
 	kfree(priv->dev.dev_vfs);
 
-err_free_dev:
-	kfree(priv);
+err_free_own:
+	if (!mlx4_is_slave(dev))
+		mlx4_free_ownership(dev);
+
+	return err;
+}
+
+static int __mlx4_init_one(struct pci_dev *pdev, int pci_dev_data,
+			   struct mlx4_priv *priv)
+{
+	int err;
+	int nvfs[MLX4_MAX_PORTS + 1] = {0, 0, 0};
+	int prb_vf[MLX4_MAX_PORTS + 1] = {0, 0, 0};
+	const int param_map[MLX4_MAX_PORTS + 1][MLX4_MAX_PORTS + 1] = {
+		{2, 0, 0}, {0, 1, 2}, {0, 1, 2} };
+	unsigned total_vfs = 0;
+	unsigned int i;
+
+	pr_info(DRV_NAME ": Initializing %s\n", pci_name(pdev));
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		dev_err(&pdev->dev, "Cannot enable PCI device, aborting\n");
+		return err;
+	}
+
+	/* Due to requirement that all VFs and the PF are *guaranteed* 2 MACS
+	 * per port, we must limit the number of VFs to 63 (since their are
+	 * 128 MACs)
+	 */
+	for (i = 0; i < sizeof(nvfs)/sizeof(nvfs[0]) && i < num_vfs_argc;
+	     total_vfs += nvfs[param_map[num_vfs_argc - 1][i]], i++) {
+		nvfs[param_map[num_vfs_argc - 1][i]] = num_vfs[i];
+		if (nvfs[i] < 0) {
+			dev_err(&pdev->dev, "num_vfs module parameter cannot be negative\n");
+			err = -EINVAL;
+			goto err_disable_pdev;
+		}
+	}
+	for (i = 0; i < sizeof(prb_vf)/sizeof(prb_vf[0]) && i < probe_vfs_argc;
+	     i++) {
+		prb_vf[param_map[probe_vfs_argc - 1][i]] = probe_vf[i];
+		if (prb_vf[i] < 0 || prb_vf[i] > nvfs[i]) {
+			dev_err(&pdev->dev, "probe_vf module parameter cannot be negative or greater than num_vfs\n");
+			err = -EINVAL;
+			goto err_disable_pdev;
+		}
+	}
+	if (total_vfs >= MLX4_MAX_NUM_VF) {
+		dev_err(&pdev->dev,
+			"Requested more VF's (%d) than allowed (%d)\n",
+			total_vfs, MLX4_MAX_NUM_VF - 1);
+		err = -EINVAL;
+		goto err_disable_pdev;
+	}
+
+	for (i = 0; i < MLX4_MAX_PORTS; i++) {
+		if (nvfs[i] + nvfs[2] >= MLX4_MAX_NUM_VF_P_PORT) {
+			dev_err(&pdev->dev,
+				"Requested more VF's (%d) for port (%d) than allowed (%d)\n",
+				nvfs[i] + nvfs[2], i + 1,
+				MLX4_MAX_NUM_VF_P_PORT - 1);
+			err = -EINVAL;
+			goto err_disable_pdev;
+		}
+	}
+
+	/* Check for BARs. */
+	if (!(pci_dev_data & MLX4_PCI_DEV_IS_VF) &&
+	    !(pci_resource_flags(pdev, 0) & IORESOURCE_MEM)) {
+		dev_err(&pdev->dev, "Missing DCS, aborting (driver_data: 0x%x, pci_resource_flags(pdev, 0):0x%lx)\n",
+			pci_dev_data, pci_resource_flags(pdev, 0));
+		err = -ENODEV;
+		goto err_disable_pdev;
+	}
+	if (!(pci_resource_flags(pdev, 2) & IORESOURCE_MEM)) {
+		dev_err(&pdev->dev, "Missing UAR, aborting\n");
+		err = -ENODEV;
+		goto err_disable_pdev;
+	}
+
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err) {
+		dev_err(&pdev->dev, "Couldn't get PCI resources, aborting\n");
+		goto err_disable_pdev;
+	}
+
+	pci_set_master(pdev);
+
+	err = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (err) {
+		dev_warn(&pdev->dev, "Warning: couldn't set 64-bit PCI DMA mask\n");
+		err = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (err) {
+			dev_err(&pdev->dev, "Can't set PCI DMA mask, aborting\n");
+			goto err_release_regions;
+		}
+	}
+	err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (err) {
+		dev_warn(&pdev->dev, "Warning: couldn't set 64-bit consistent PCI DMA mask\n");
+		err = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (err) {
+			dev_err(&pdev->dev, "Can't set consistent PCI DMA mask, aborting\n");
+			goto err_release_regions;
+		}
+	}
+
+	/* Allow large DMA segments, up to the firmware limit of 1 GB */
+	dma_set_max_seg_size(&pdev->dev, 1024 * 1024 * 1024);
+	/* Detect if this device is a virtual function */
+	if (pci_dev_data & MLX4_PCI_DEV_IS_VF) {
+		/* When acting as pf, we normally skip vfs unless explicitly
+		 * requested to probe them.
+		 */
+		if (total_vfs) {
+			unsigned vfs_offset = 0;
+
+			for (i = 0; i < sizeof(nvfs)/sizeof(nvfs[0]) &&
+			     vfs_offset + nvfs[i] < extended_func_num(pdev);
+			     vfs_offset += nvfs[i], i++)
+				;
+			if (i == sizeof(nvfs)/sizeof(nvfs[0])) {
+				err = -ENODEV;
+				goto err_release_regions;
+			}
+			if ((extended_func_num(pdev) - vfs_offset)
+			    > prb_vf[i]) {
+				dev_warn(&pdev->dev, "Skipping virtual function:%d\n",
+					 extended_func_num(pdev));
+				err = -ENODEV;
+				goto err_release_regions;
+			}
+		}
+	}
+
+	err = mlx4_load_one(pdev, pci_dev_data, total_vfs, nvfs, priv);
+	if (err)
+		goto err_release_regions;
+	return 0;
 
 err_release_regions:
 	pci_release_regions(pdev);
@@ -2616,6 +2710,7 @@ static int mlx4_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct mlx4_priv *priv;
 	struct mlx4_dev *dev;
+	int ret;
 
 	printk_once(KERN_INFO "%s", mlx4_version);
 
@@ -2624,28 +2719,38 @@ static int mlx4_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		return -ENOMEM;
 
 	dev       = &priv->dev;
+	dev->pdev = pdev;
 	pci_set_drvdata(pdev, dev);
 	priv->pci_dev_data = id->driver_data;
 
-	return __mlx4_init_one(pdev, id->driver_data);
+	ret =  __mlx4_init_one(pdev, id->driver_data, priv);
+	if (ret)
+		kfree(priv);
+
+	return ret;
 }
 
-static void __mlx4_remove_one(struct pci_dev *pdev)
+static void mlx4_unload_one(struct pci_dev *pdev)
 {
 	struct mlx4_dev  *dev  = pci_get_drvdata(pdev);
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	int               pci_dev_data;
 	int p;
+	int active_vfs = 0;
 
 	if (priv->removed)
 		return;
 
 	pci_dev_data = priv->pci_dev_data;
 
-	/* in SRIOV it is not allowed to unload the pf's
-	 * driver while there are alive vf's */
-	if (mlx4_is_master(dev) && mlx4_how_many_lives_vf(dev))
-		pr_warn("Removing PF when there are assigned VF's !!!\n");
+	/* Disabling SR-IOV is not allowed while there are active vf's */
+	if (mlx4_is_master(dev)) {
+		active_vfs = mlx4_how_many_lives_vf(dev);
+		if (active_vfs) {
+			pr_warn("Removing PF when there are active VF's !!\n");
+			pr_warn("Will not disable SR-IOV.\n");
+		}
+	}
 	mlx4_stop_sense(dev);
 	mlx4_unregister_device(dev);
 
@@ -2688,7 +2793,7 @@ static void __mlx4_remove_one(struct pci_dev *pdev)
 
 	if (dev->flags & MLX4_FLAG_MSI_X)
 		pci_disable_msix(pdev);
-	if (dev->flags & MLX4_FLAG_SRIOV) {
+	if (dev->flags & MLX4_FLAG_SRIOV && !active_vfs) {
 		mlx4_warn(dev, "Disabling SR-IOV\n");
 		pci_disable_sriov(pdev);
 		dev->num_vfs = 0;
@@ -2704,8 +2809,6 @@ static void __mlx4_remove_one(struct pci_dev *pdev)
 	kfree(dev->caps.qp1_proxy);
 	kfree(dev->dev_vfs);
 
-	pci_release_regions(pdev);
-	pci_disable_device(pdev);
 	memset(priv, 0, sizeof(*priv));
 	priv->pci_dev_data = pci_dev_data;
 	priv->removed = 1;
@@ -2716,7 +2819,9 @@ static void mlx4_remove_one(struct pci_dev *pdev)
 	struct mlx4_dev  *dev  = pci_get_drvdata(pdev);
 	struct mlx4_priv *priv = mlx4_priv(dev);
 
-	__mlx4_remove_one(pdev);
+	mlx4_unload_one(pdev);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
 	kfree(priv);
 	pci_set_drvdata(pdev, NULL);
 }
@@ -2725,11 +2830,22 @@ int mlx4_restart_one(struct pci_dev *pdev)
 {
 	struct mlx4_dev	 *dev  = pci_get_drvdata(pdev);
 	struct mlx4_priv *priv = mlx4_priv(dev);
-	int		  pci_dev_data;
+	int nvfs[MLX4_MAX_PORTS + 1] = {0, 0, 0};
+	int pci_dev_data, err, total_vfs;
 
 	pci_dev_data = priv->pci_dev_data;
-	__mlx4_remove_one(pdev);
-	return __mlx4_init_one(pdev, pci_dev_data);
+	total_vfs = dev->num_vfs;
+	memcpy(nvfs, dev->nvfs, sizeof(dev->nvfs));
+
+	mlx4_unload_one(pdev);
+	err = mlx4_load_one(pdev, pci_dev_data, total_vfs, nvfs, priv);
+	if (err) {
+		mlx4_err(dev, "%s: ERROR: mlx4_load_one failed, pci_name=%s, err=%d\n",
+			 __func__, pci_name(pdev), err);
+		return err;
+	}
+
+	return err;
 }
 
 static const struct pci_device_id mlx4_pci_table[] = {
@@ -2783,7 +2899,7 @@ MODULE_DEVICE_TABLE(pci, mlx4_pci_table);
 static pci_ers_result_t mlx4_pci_err_detected(struct pci_dev *pdev,
 					      pci_channel_state_t state)
 {
-	__mlx4_remove_one(pdev);
+	mlx4_unload_one(pdev);
 
 	return state == pci_channel_io_perm_failure ?
 		PCI_ERS_RESULT_DISCONNECT : PCI_ERS_RESULT_NEED_RESET;
@@ -2795,7 +2911,7 @@ static pci_ers_result_t mlx4_pci_slot_reset(struct pci_dev *pdev)
 	struct mlx4_priv *priv = mlx4_priv(dev);
 	int               ret;
 
-	ret = __mlx4_init_one(pdev, priv->pci_dev_data);
+	ret = __mlx4_init_one(pdev, priv->pci_dev_data, priv);
 
 	return ret ? PCI_ERS_RESULT_DISCONNECT : PCI_ERS_RESULT_RECOVERED;
 }
@@ -2809,7 +2925,7 @@ static struct pci_driver mlx4_driver = {
 	.name		= DRV_NAME,
 	.id_table	= mlx4_pci_table,
 	.probe		= mlx4_init_one,
-	.shutdown	= __mlx4_remove_one,
+	.shutdown	= mlx4_unload_one,
 	.remove		= mlx4_remove_one,
 	.err_handler    = &mlx4_err_handler,
 };
diff --git a/drivers/net/ethernet/mellanox/mlx4/mcg.c b/drivers/net/ethernet/mellanox/mlx4/mcg.c
index d80e7a6fac74..ca0f98c95105 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mcg.c
+++ b/drivers/net/ethernet/mellanox/mlx4/mcg.c
@@ -1020,6 +1020,44 @@ int mlx4_flow_detach(struct mlx4_dev *dev, u64 reg_id)
 }
 EXPORT_SYMBOL_GPL(mlx4_flow_detach);
 
+int mlx4_tunnel_steer_add(struct mlx4_dev *dev, unsigned char *addr,
+			  int port, int qpn, u16 prio, u64 *reg_id)
+{
+	int err;
+	struct mlx4_spec_list spec_eth_outer = { {NULL} };
+	struct mlx4_spec_list spec_vxlan     = { {NULL} };
+	struct mlx4_spec_list spec_eth_inner = { {NULL} };
+
+	struct mlx4_net_trans_rule rule = {
+		.queue_mode = MLX4_NET_TRANS_Q_FIFO,
+		.exclusive = 0,
+		.allow_loopback = 1,
+		.promisc_mode = MLX4_FS_REGULAR,
+	};
+
+	__be64 mac_mask = cpu_to_be64(MLX4_MAC_MASK << 16);
+
+	rule.port = port;
+	rule.qpn = qpn;
+	rule.priority = prio;
+	INIT_LIST_HEAD(&rule.list);
+
+	spec_eth_outer.id = MLX4_NET_TRANS_RULE_ID_ETH;
+	memcpy(spec_eth_outer.eth.dst_mac, addr, ETH_ALEN);
+	memcpy(spec_eth_outer.eth.dst_mac_msk, &mac_mask, ETH_ALEN);
+
+	spec_vxlan.id = MLX4_NET_TRANS_RULE_ID_VXLAN;    /* any vxlan header */
+	spec_eth_inner.id = MLX4_NET_TRANS_RULE_ID_ETH;	 /* any inner eth header */
+
+	list_add_tail(&spec_eth_outer.list, &rule.list);
+	list_add_tail(&spec_vxlan.list,     &rule.list);
+	list_add_tail(&spec_eth_inner.list, &rule.list);
+
+	err = mlx4_flow_attach(dev, &rule, reg_id);
+	return err;
+}
+EXPORT_SYMBOL(mlx4_tunnel_steer_add);
+
 int mlx4_FLOW_STEERING_IB_UC_QP_RANGE(struct mlx4_dev *dev, u32 min_range_qpn,
 				      u32 max_range_qpn)
 {
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4.h b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
index b508c7887ef8..de10dbb2e6ed 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4.h
@@ -285,6 +285,9 @@ struct mlx4_icm_table {
 #define MLX4_MPT_STATUS_SW		0xF0
 #define MLX4_MPT_STATUS_HW		0x00
 
+#define MLX4_CQE_SIZE_MASK_STRIDE	0x3
+#define MLX4_EQE_SIZE_MASK_STRIDE	0x30
+
 /*
  * Must be packed because mtt_seg is 64 bits but only aligned to 32 bits.
  */
diff --git a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
index 3de41be49425..8fef65840b3b 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
+++ b/drivers/net/ethernet/mellanox/mlx4/mlx4_en.h
@@ -216,13 +216,16 @@ enum cq_type {
 
 struct mlx4_en_tx_info {
 	struct sk_buff *skb;
-	u32 nr_txbb;
-	u32 nr_bytes;
-	u8 linear;
-	u8 data_offset;
-	u8 inl;
-	u8 ts_requested;
-};
+	dma_addr_t	map0_dma;
+	u32		map0_byte_count;
+	u32		nr_txbb;
+	u32		nr_bytes;
+	u8		linear;
+	u8		data_offset;
+	u8		inl;
+	u8		ts_requested;
+	u8		nr_maps;
+} ____cacheline_aligned_in_smp;
 
 
 #define MLX4_EN_BIT_DESC_OWN	0x80000000
@@ -253,39 +256,46 @@ struct mlx4_en_rx_alloc {
 };
 
 struct mlx4_en_tx_ring {
+	/* cache line used and dirtied in tx completion
+	 * (mlx4_en_free_tx_buf())
+	 */
+	u32			last_nr_txbb;
+	u32			cons;
+	unsigned long		wake_queue;
+
+	/* cache line used and dirtied in mlx4_en_xmit() */
+	u32			prod ____cacheline_aligned_in_smp;
+	unsigned long		bytes;
+	unsigned long		packets;
+	unsigned long		tx_csum;
+	unsigned long		tso_packets;
+	unsigned long		xmit_more;
+	struct mlx4_bf		bf;
+	unsigned long		queue_stopped;
+
+	/* Following part should be mostly read */
+	cpumask_t		affinity_mask;
+	struct mlx4_qp		qp;
 	struct mlx4_hwq_resources wqres;
-	u32 size ; /* number of TXBBs */
-	u32 size_mask;
-	u16 stride;
-	u16 cqn;	/* index of port CQ associated with this ring */
-	u32 prod;
-	u32 cons;
-	u32 buf_size;
-	u32 doorbell_qpn;
-	void *buf;
-	u16 poll_cnt;
-	struct mlx4_en_tx_info *tx_info;
-	u8 *bounce_buf;
-	u8 queue_index;
-	cpumask_t affinity_mask;
-	u32 last_nr_txbb;
-	struct mlx4_qp qp;
-	struct mlx4_qp_context context;
-	int qpn;
-	enum mlx4_qp_state qp_state;
-	struct mlx4_srq dummy;
-	unsigned long bytes;
-	unsigned long packets;
-	unsigned long tx_csum;
-	unsigned long queue_stopped;
-	unsigned long wake_queue;
-	struct mlx4_bf bf;
-	bool bf_enabled;
-	bool bf_alloced;
-	struct netdev_queue *tx_queue;
-	int hwtstamp_tx_type;
-	int inline_thold;
-};
+	u32			size; /* number of TXBBs */
+	u32			size_mask;
+	u16			stride;
+	u16			cqn;	/* index of port CQ associated with this ring */
+	u32			buf_size;
+	__be32			doorbell_qpn;
+	__be32			mr_key;
+	void			*buf;
+	struct mlx4_en_tx_info	*tx_info;
+	u8			*bounce_buf;
+	struct mlx4_qp_context	context;
+	int			qpn;
+	enum mlx4_qp_state	qp_state;
+	u8			queue_index;
+	bool			bf_enabled;
+	bool			bf_alloced;
+	struct netdev_queue	*tx_queue;
+	int			hwtstamp_tx_type;
+} ____cacheline_aligned_in_smp;
 
 struct mlx4_en_rx_desc {
 	/* actual number of entries depends on rx ring stride */
@@ -426,6 +436,7 @@ struct mlx4_en_pkt_stats {
 
 struct mlx4_en_port_stats {
 	unsigned long tso_packets;
+	unsigned long xmit_more;
 	unsigned long queue_stopped;
 	unsigned long wake_queue;
 	unsigned long tx_timeout;
@@ -433,7 +444,7 @@ struct mlx4_en_port_stats {
 	unsigned long rx_chksum_good;
 	unsigned long rx_chksum_none;
 	unsigned long tx_chksum_offload;
-#define NUM_PORT_STATS		8
+#define NUM_PORT_STATS		9
 };
 
 struct mlx4_en_perf_stats {
@@ -542,6 +553,7 @@ struct mlx4_en_priv {
 	unsigned max_mtu;
 	int base_qpn;
 	int cqe_factor;
+	int cqe_size;
 
 	struct mlx4_en_rss_map rss_map;
 	__be32 ctrl_flags;
@@ -612,6 +624,11 @@ struct mlx4_mac_entry {
 	struct rcu_head rcu;
 };
 
+static inline struct mlx4_cqe *mlx4_en_get_cqe(void *buf, int idx, int cqe_sz)
+{
+	return buf + idx * cqe_sz;
+}
+
 #ifdef CONFIG_NET_RX_BUSY_POLL
 static inline void mlx4_en_cq_init_lock(struct mlx4_en_cq *cq)
 {
@@ -836,8 +853,8 @@ extern const struct ethtool_ops mlx4_en_ethtool_ops;
  */
 
 __printf(3, 4)
-int en_print(const char *level, const struct mlx4_en_priv *priv,
-	     const char *format, ...);
+void en_print(const char *level, const struct mlx4_en_priv *priv,
+	      const char *format, ...);
 
 #define en_dbg(mlevel, priv, format, ...)				\
 do {									\
diff --git a/drivers/net/ethernet/mellanox/mlx4/mr.c b/drivers/net/ethernet/mellanox/mlx4/mr.c
index 7d717eccb7b0..193a6adb5d04 100644
--- a/drivers/net/ethernet/mellanox/mlx4/mr.c
+++ b/drivers/net/ethernet/mellanox/mlx4/mr.c
@@ -298,6 +298,7 @@ static int mlx4_HW2SW_MPT(struct mlx4_dev *dev, struct mlx4_cmd_mailbox *mailbox
 			    MLX4_CMD_TIME_CLASS_B, MLX4_CMD_WRAPPED);
 }
 
+/* Must protect against concurrent access */
 int mlx4_mr_hw_get_mpt(struct mlx4_dev *dev, struct mlx4_mr *mmr,
 		       struct mlx4_mpt_entry ***mpt_entry)
 {
@@ -305,13 +306,10 @@ int mlx4_mr_hw_get_mpt(struct mlx4_dev *dev, struct mlx4_mr *mmr,
 	int key = key_to_hw_index(mmr->key) & (dev->caps.num_mpts - 1);
 	struct mlx4_cmd_mailbox *mailbox = NULL;
 
-	/* Make sure that at this point we have single-threaded access only */
-
 	if (mmr->enabled != MLX4_MPT_EN_HW)
 		return -EINVAL;
 
 	err = mlx4_HW2SW_MPT(dev, NULL, key);
-
 	if (err) {
 		mlx4_warn(dev, "HW2SW_MPT failed (%d).", err);
 		mlx4_warn(dev, "Most likely the MR has MWs bound to it.\n");
@@ -333,7 +331,6 @@ int mlx4_mr_hw_get_mpt(struct mlx4_dev *dev, struct mlx4_mr *mmr,
 				   0, MLX4_CMD_QUERY_MPT,
 				   MLX4_CMD_TIME_CLASS_B,
 				   MLX4_CMD_WRAPPED);
-
 		if (err)
 			goto free_mailbox;
 
@@ -378,9 +375,10 @@ int mlx4_mr_hw_write_mpt(struct mlx4_dev *dev, struct mlx4_mr *mmr,
 		err = mlx4_SW2HW_MPT(dev, mailbox, key);
 	}
 
-	mmr->pd = be32_to_cpu((*mpt_entry)->pd_flags) & MLX4_MPT_PD_MASK;
-	if (!err)
+	if (!err) {
+		mmr->pd = be32_to_cpu((*mpt_entry)->pd_flags) & MLX4_MPT_PD_MASK;
 		mmr->enabled = MLX4_MPT_EN_HW;
+	}
 	return err;
 }
 EXPORT_SYMBOL_GPL(mlx4_mr_hw_write_mpt);
@@ -400,11 +398,12 @@ EXPORT_SYMBOL_GPL(mlx4_mr_hw_put_mpt);
 int mlx4_mr_hw_change_pd(struct mlx4_dev *dev, struct mlx4_mpt_entry *mpt_entry,
 			 u32 pdn)
 {
-	u32 pd_flags = be32_to_cpu(mpt_entry->pd_flags);
+	u32 pd_flags = be32_to_cpu(mpt_entry->pd_flags) & ~MLX4_MPT_PD_MASK;
 	/* The wrapper function will put the slave's id here */
 	if (mlx4_is_mfunc(dev))
 		pd_flags &= ~MLX4_MPT_PD_VF_MASK;
-	mpt_entry->pd_flags = cpu_to_be32((pd_flags &  ~MLX4_MPT_PD_MASK) |
+
+	mpt_entry->pd_flags = cpu_to_be32(pd_flags |
 					  (pdn & MLX4_MPT_PD_MASK)
 					  | MLX4_MPT_PD_FLAG_EN_INV);
 	return 0;
@@ -600,14 +599,18 @@ int mlx4_mr_rereg_mem_write(struct mlx4_dev *dev, struct mlx4_mr *mr,
 {
 	int err;
 
-	mpt_entry->start       = cpu_to_be64(mr->iova);
-	mpt_entry->length      = cpu_to_be64(mr->size);
-	mpt_entry->entity_size = cpu_to_be32(mr->mtt.page_shift);
+	mpt_entry->start       = cpu_to_be64(iova);
+	mpt_entry->length      = cpu_to_be64(size);
+	mpt_entry->entity_size = cpu_to_be32(page_shift);
 
 	err = mlx4_mtt_init(dev, npages, page_shift, &mr->mtt);
 	if (err)
 		return err;
 
+	mpt_entry->pd_flags &= cpu_to_be32(MLX4_MPT_PD_MASK |
+					   MLX4_MPT_PD_FLAG_EN_INV);
+	mpt_entry->flags    &= cpu_to_be32(MLX4_MPT_FLAG_FREE |
+					   MLX4_MPT_FLAG_SW_OWNS);
 	if (mr->mtt.order < 0) {
 		mpt_entry->flags |= cpu_to_be32(MLX4_MPT_FLAG_PHYSICAL);
 		mpt_entry->mtt_addr = 0;
@@ -617,6 +620,14 @@ int mlx4_mr_rereg_mem_write(struct mlx4_dev *dev, struct mlx4_mr *mr,
 		if (mr->mtt.page_shift == 0)
 			mpt_entry->mtt_sz    = cpu_to_be32(1 << mr->mtt.order);
 	}
+	if (mr->mtt.order >= 0 && mr->mtt.page_shift == 0) {
+		/* fast register MR in free state */
+		mpt_entry->flags    |= cpu_to_be32(MLX4_MPT_FLAG_FREE);
+		mpt_entry->pd_flags |= cpu_to_be32(MLX4_MPT_PD_FLAG_FAST_REG |
+						   MLX4_MPT_PD_FLAG_RAE);
+	} else {
+		mpt_entry->flags    |= cpu_to_be32(MLX4_MPT_FLAG_SW_OWNS);
+	}
 	mr->enabled = MLX4_MPT_EN_SW;
 
 	return 0;
diff --git a/drivers/net/ethernet/mellanox/mlx4/port.c b/drivers/net/ethernet/mellanox/mlx4/port.c
index 9ba0c1ca10d5..94eeb2c7d7e4 100644
--- a/drivers/net/ethernet/mellanox/mlx4/port.c
+++ b/drivers/net/ethernet/mellanox/mlx4/port.c
@@ -103,7 +103,8 @@ static int find_index(struct mlx4_dev *dev,
 	int i;
 
 	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
-		if ((mac & MLX4_MAC_MASK) ==
+		if (table->refs[i] &&
+		    (MLX4_MAC_MASK & mac) ==
 		    (MLX4_MAC_MASK & be64_to_cpu(table->entries[i])))
 			return i;
 	}
@@ -165,12 +166,14 @@ int __mlx4_register_mac(struct mlx4_dev *dev, u8 port, u64 mac)
 
 	mutex_lock(&table->mutex);
 	for (i = 0; i < MLX4_MAX_MAC_NUM; i++) {
-		if (free < 0 && !table->entries[i]) {
-			free = i;
+		if (!table->refs[i]) {
+			if (free < 0)
+				free = i;
 			continue;
 		}
 
-		if (mac == (MLX4_MAC_MASK & be64_to_cpu(table->entries[i]))) {
+		if ((MLX4_MAC_MASK & mac) ==
+		     (MLX4_MAC_MASK & be64_to_cpu(table->entries[i]))) {
 			/* MAC already registered, increment ref count */
 			err = i;
 			++table->refs[i];
diff --git a/drivers/net/ethernet/mellanox/mlx4/qp.c b/drivers/net/ethernet/mellanox/mlx4/qp.c
index 0dc31d85fc3b..2301365c79c7 100644
--- a/drivers/net/ethernet/mellanox/mlx4/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx4/qp.c
@@ -390,13 +390,14 @@ err_icm:
 EXPORT_SYMBOL_GPL(mlx4_qp_alloc);
 
 #define MLX4_UPDATE_QP_SUPPORTED_ATTRS MLX4_UPDATE_QP_SMAC
-int mlx4_update_qp(struct mlx4_dev *dev, struct mlx4_qp *qp,
+int mlx4_update_qp(struct mlx4_dev *dev, u32 qpn,
 		   enum mlx4_update_qp_attr attr,
 		   struct mlx4_update_qp_params *params)
 {
 	struct mlx4_cmd_mailbox *mailbox;
 	struct mlx4_update_qp_context *cmd;
 	u64 pri_addr_path_mask = 0;
+	u64 qp_mask = 0;
 	int err = 0;
 
 	mailbox = mlx4_alloc_cmd_mailbox(dev);
@@ -413,9 +414,16 @@ int mlx4_update_qp(struct mlx4_dev *dev, struct mlx4_qp *qp,
 		cmd->qp_context.pri_path.grh_mylmc = params->smac_index;
 	}
 
+	if (attr & MLX4_UPDATE_QP_VSD) {
+		qp_mask |= 1ULL << MLX4_UPD_QP_MASK_VSD;
+		if (params->flags & MLX4_UPDATE_QP_PARAMS_FLAGS_VSD_ENABLE)
+			cmd->qp_context.param3 |= cpu_to_be32(MLX4_STRIP_VLAN);
+	}
+
 	cmd->primary_addr_path_mask = cpu_to_be64(pri_addr_path_mask);
+	cmd->qp_mask = cpu_to_be64(qp_mask);
 
-	err = mlx4_cmd(dev, mailbox->dma, qp->qpn & 0xffffff, 0,
+	err = mlx4_cmd(dev, mailbox->dma, qpn & 0xffffff, 0,
 		       MLX4_CMD_UPDATE_QP, MLX4_CMD_TIME_CLASS_A,
 		       MLX4_CMD_NATIVE);
 
diff --git a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
index 1089367fed22..5d2498dcf536 100644
--- a/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
+++ b/drivers/net/ethernet/mellanox/mlx4/resource_tracker.c
@@ -702,11 +702,13 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 	struct mlx4_qp_context	*qpc = inbox->buf + 8;
 	struct mlx4_vport_oper_state *vp_oper;
 	struct mlx4_priv *priv;
+	u32 qp_type;
 	int port;
 
 	port = (qpc->pri_path.sched_queue & 0x40) ? 2 : 1;
 	priv = mlx4_priv(dev);
 	vp_oper = &priv->mfunc.master.vf_oper[slave].vport[port];
+	qp_type	= (be32_to_cpu(qpc->flags) >> 16) & 0xff;
 
 	if (MLX4_VGT != vp_oper->state.default_vlan) {
 		/* the reserved QPs (special, proxy, tunnel)
@@ -715,8 +717,20 @@ static int update_vport_qp_param(struct mlx4_dev *dev,
 		if (mlx4_is_qp_reserved(dev, qpn))
 			return 0;
 
-		/* force strip vlan by clear vsd */
-		qpc->param3 &= ~cpu_to_be32(MLX4_STRIP_VLAN);
+		/* force strip vlan by clear vsd, MLX QP refers to Raw Ethernet */
+		if (qp_type == MLX4_QP_ST_UD ||
+		    (qp_type == MLX4_QP_ST_MLX && mlx4_is_eth(dev, port))) {
+			if (dev->caps.bmme_flags & MLX4_BMME_FLAG_VSD_INIT2RTR) {
+				*(__be32 *)inbox->buf =
+					cpu_to_be32(be32_to_cpu(*(__be32 *)inbox->buf) |
+					MLX4_QP_OPTPAR_VLAN_STRIPPING);
+				qpc->param3 &= ~cpu_to_be32(MLX4_STRIP_VLAN);
+			} else {
+				struct mlx4_update_qp_params params = {.flags = 0};
+
+				mlx4_update_qp(dev, qpn, MLX4_UPDATE_QP_VSD, &params);
+			}
+		}
 
 		if (vp_oper->state.link_state == IFLA_VF_LINK_STATE_DISABLE &&
 		    dev->caps.flags2 & MLX4_DEV_CAP_FLAG2_UPDATE_QP) {
@@ -3998,13 +4012,17 @@ int mlx4_UPDATE_QP_wrapper(struct mlx4_dev *dev, int slave,
 	}
 
 	port = (rqp->sched_queue >> 6 & 1) + 1;
-	smac_index = cmd->qp_context.pri_path.grh_mylmc;
-	err = mac_find_smac_ix_in_slave(dev, slave, port,
-					smac_index, &mac);
-	if (err) {
-		mlx4_err(dev, "Failed to update qpn 0x%x, MAC is invalid. smac_ix: %d\n",
-			 qpn, smac_index);
-		goto err_mac;
+
+	if (pri_addr_path_mask & (1ULL << MLX4_UPD_QP_PATH_MASK_MAC_INDEX)) {
+		smac_index = cmd->qp_context.pri_path.grh_mylmc;
+		err = mac_find_smac_ix_in_slave(dev, slave, port,
+						smac_index, &mac);
+
+		if (err) {
+			mlx4_err(dev, "Failed to update qpn 0x%x, MAC is invalid. smac_ix: %d\n",
+				 qpn, smac_index);
+			goto err_mac;
+		}
 	}
 
 	err = mlx4_cmd(dev, inbox->dma,
@@ -4818,7 +4836,7 @@ void mlx4_vf_immed_vlan_work_handler(struct work_struct *_work)
 			MLX4_VLAN_CTRL_ETH_RX_BLOCK_UNTAGGED;
 
 	upd_context = mailbox->buf;
-	upd_context->qp_mask = cpu_to_be64(MLX4_UPD_QP_MASK_VSD);
+	upd_context->qp_mask = cpu_to_be64(1ULL << MLX4_UPD_QP_MASK_VSD);
 
 	spin_lock_irq(mlx4_tlock(dev));
 	list_for_each_entry_safe(qp, tmp, qp_list, com.list) {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
index 65a7da69e2ac..368c6c5ea014 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/cmd.c
@@ -357,60 +357,24 @@ const char *mlx5_command_str(int command)
 	case MLX5_CMD_OP_2ERR_QP:
 		return "2ERR_QP";
 
-	case MLX5_CMD_OP_RTS2SQD_QP:
-		return "RTS2SQD_QP";
-
-	case MLX5_CMD_OP_SQD2RTS_QP:
-		return "SQD2RTS_QP";
-
 	case MLX5_CMD_OP_2RST_QP:
 		return "2RST_QP";
 
 	case MLX5_CMD_OP_QUERY_QP:
 		return "QUERY_QP";
 
-	case MLX5_CMD_OP_CONF_SQP:
-		return "CONF_SQP";
-
 	case MLX5_CMD_OP_MAD_IFC:
 		return "MAD_IFC";
 
 	case MLX5_CMD_OP_INIT2INIT_QP:
 		return "INIT2INIT_QP";
 
-	case MLX5_CMD_OP_SUSPEND_QP:
-		return "SUSPEND_QP";
-
-	case MLX5_CMD_OP_UNSUSPEND_QP:
-		return "UNSUSPEND_QP";
-
-	case MLX5_CMD_OP_SQD2SQD_QP:
-		return "SQD2SQD_QP";
-
-	case MLX5_CMD_OP_ALLOC_QP_COUNTER_SET:
-		return "ALLOC_QP_COUNTER_SET";
-
-	case MLX5_CMD_OP_DEALLOC_QP_COUNTER_SET:
-		return "DEALLOC_QP_COUNTER_SET";
-
-	case MLX5_CMD_OP_QUERY_QP_COUNTER_SET:
-		return "QUERY_QP_COUNTER_SET";
-
 	case MLX5_CMD_OP_CREATE_PSV:
 		return "CREATE_PSV";
 
 	case MLX5_CMD_OP_DESTROY_PSV:
 		return "DESTROY_PSV";
 
-	case MLX5_CMD_OP_QUERY_PSV:
-		return "QUERY_PSV";
-
-	case MLX5_CMD_OP_QUERY_SIG_RULE_TABLE:
-		return "QUERY_SIG_RULE_TABLE";
-
-	case MLX5_CMD_OP_QUERY_BLOCK_SIZE_TABLE:
-		return "QUERY_BLOCK_SIZE_TABLE";
-
 	case MLX5_CMD_OP_CREATE_SRQ:
 		return "CREATE_SRQ";
 
@@ -1538,16 +1502,9 @@ static const char *cmd_status_str(u8 status)
 	}
 }
 
-int mlx5_cmd_status_to_err(struct mlx5_outbox_hdr *hdr)
+static int cmd_status_to_err(u8 status)
 {
-	if (!hdr->status)
-		return 0;
-
-	pr_warn("command failed, status %s(0x%x), syndrome 0x%x\n",
-		cmd_status_str(hdr->status), hdr->status,
-		be32_to_cpu(hdr->syndrome));
-
-	switch (hdr->status) {
+	switch (status) {
 	case MLX5_CMD_STAT_OK:				return 0;
 	case MLX5_CMD_STAT_INT_ERR:			return -EIO;
 	case MLX5_CMD_STAT_BAD_OP_ERR:			return -EINVAL;
@@ -1567,3 +1524,33 @@ int mlx5_cmd_status_to_err(struct mlx5_outbox_hdr *hdr)
 	default:					return -EIO;
 	}
 }
+
+/* this will be available till all the commands use set/get macros */
+int mlx5_cmd_status_to_err(struct mlx5_outbox_hdr *hdr)
+{
+	if (!hdr->status)
+		return 0;
+
+	pr_warn("command failed, status %s(0x%x), syndrome 0x%x\n",
+		cmd_status_str(hdr->status), hdr->status,
+		be32_to_cpu(hdr->syndrome));
+
+	return cmd_status_to_err(hdr->status);
+}
+
+int mlx5_cmd_status_to_err_v2(void *ptr)
+{
+	u32	syndrome;
+	u8	status;
+
+	status = be32_to_cpu(*(__be32 *)ptr) >> 24;
+	if (!status)
+		return 0;
+
+	syndrome = be32_to_cpu(*(__be32 *)(ptr + 4));
+
+	pr_warn("command failed, status %s(0x%x), syndrome 0x%x\n",
+		cmd_status_str(status), status, syndrome);
+
+	return cmd_status_to_err(status);
+}
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/eq.c b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
index 4e8bd0b34bb0..ed53291468f3 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/eq.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/eq.c
@@ -198,7 +198,7 @@ static int mlx5_eq_int(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 	int eqes_found = 0;
 	int set_ci = 0;
 	u32 cqn;
-	u32 srqn;
+	u32 rsn;
 	u8 port;
 
 	while ((eqe = next_eqe_sw(eq))) {
@@ -224,18 +224,18 @@ static int mlx5_eq_int(struct mlx5_core_dev *dev, struct mlx5_eq *eq)
 		case MLX5_EVENT_TYPE_PATH_MIG_FAILED:
 		case MLX5_EVENT_TYPE_WQ_INVAL_REQ_ERROR:
 		case MLX5_EVENT_TYPE_WQ_ACCESS_ERROR:
+			rsn = be32_to_cpu(eqe->data.qp_srq.qp_srq_n) & 0xffffff;
 			mlx5_core_dbg(dev, "event %s(%d) arrived\n",
 				      eqe_type_str(eqe->type), eqe->type);
-			mlx5_qp_event(dev, be32_to_cpu(eqe->data.qp_srq.qp_srq_n) & 0xffffff,
-				      eqe->type);
+			mlx5_rsc_event(dev, rsn, eqe->type);
 			break;
 
 		case MLX5_EVENT_TYPE_SRQ_RQ_LIMIT:
 		case MLX5_EVENT_TYPE_SRQ_CATAS_ERROR:
-			srqn = be32_to_cpu(eqe->data.qp_srq.qp_srq_n) & 0xffffff;
+			rsn = be32_to_cpu(eqe->data.qp_srq.qp_srq_n) & 0xffffff;
 			mlx5_core_dbg(dev, "SRQ event %s(%d): srqn 0x%x\n",
-				      eqe_type_str(eqe->type), eqe->type, srqn);
-			mlx5_srq_event(dev, srqn, eqe->type);
+				      eqe_type_str(eqe->type), eqe->type, rsn);
+			mlx5_srq_event(dev, rsn, eqe->type);
 			break;
 
 		case MLX5_EVENT_TYPE_CMD:
@@ -468,7 +468,7 @@ int mlx5_start_eqs(struct mlx5_core_dev *dev)
 
 	err = mlx5_create_map_eq(dev, &table->pages_eq,
 				 MLX5_EQ_VEC_PAGES,
-				 dev->caps.max_vf + 1,
+				 dev->caps.gen.max_vf + 1,
 				 1 << MLX5_EVENT_TYPE_PAGE_REQUEST, "mlx5_pages_eq",
 				 &dev->priv.uuari.uars[0]);
 	if (err) {
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/fw.c b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
index f012658b6a92..087c4c797deb 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/fw.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/fw.c
@@ -64,86 +64,9 @@ out_out:
 	return err;
 }
 
-int mlx5_cmd_query_hca_cap(struct mlx5_core_dev *dev,
-			   struct mlx5_caps *caps)
+int mlx5_cmd_query_hca_cap(struct mlx5_core_dev *dev, struct mlx5_caps *caps)
 {
-	struct mlx5_cmd_query_hca_cap_mbox_out *out;
-	struct mlx5_cmd_query_hca_cap_mbox_in in;
-	struct mlx5_query_special_ctxs_mbox_out ctx_out;
-	struct mlx5_query_special_ctxs_mbox_in ctx_in;
-	int err;
-	u16 t16;
-
-	out = kzalloc(sizeof(*out), GFP_KERNEL);
-	if (!out)
-		return -ENOMEM;
-
-	memset(&in, 0, sizeof(in));
-	in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_HCA_CAP);
-	in.hdr.opmod  = cpu_to_be16(0x1);
-	err = mlx5_cmd_exec(dev, &in, sizeof(in), out, sizeof(*out));
-	if (err)
-		goto out_out;
-
-	if (out->hdr.status) {
-		err = mlx5_cmd_status_to_err(&out->hdr);
-		goto out_out;
-	}
-
-
-	caps->log_max_eq = out->hca_cap.log_max_eq & 0xf;
-	caps->max_cqes = 1 << out->hca_cap.log_max_cq_sz;
-	caps->max_wqes = 1 << out->hca_cap.log_max_qp_sz;
-	caps->max_sq_desc_sz = be16_to_cpu(out->hca_cap.max_desc_sz_sq);
-	caps->max_rq_desc_sz = be16_to_cpu(out->hca_cap.max_desc_sz_rq);
-	caps->flags = be64_to_cpu(out->hca_cap.flags);
-	caps->stat_rate_support = be16_to_cpu(out->hca_cap.stat_rate_support);
-	caps->log_max_msg = out->hca_cap.log_max_msg & 0x1f;
-	caps->num_ports = out->hca_cap.num_ports & 0xf;
-	caps->log_max_cq = out->hca_cap.log_max_cq & 0x1f;
-	if (caps->num_ports > MLX5_MAX_PORTS) {
-		mlx5_core_err(dev, "device has %d ports while the driver supports max %d ports\n",
-			      caps->num_ports, MLX5_MAX_PORTS);
-		err = -EINVAL;
-		goto out_out;
-	}
-	caps->log_max_qp = out->hca_cap.log_max_qp & 0x1f;
-	caps->log_max_mkey = out->hca_cap.log_max_mkey & 0x3f;
-	caps->log_max_pd = out->hca_cap.log_max_pd & 0x1f;
-	caps->log_max_srq = out->hca_cap.log_max_srqs & 0x1f;
-	caps->local_ca_ack_delay = out->hca_cap.local_ca_ack_delay & 0x1f;
-	caps->log_max_mcg = out->hca_cap.log_max_mcg;
-	caps->max_qp_mcg = be32_to_cpu(out->hca_cap.max_qp_mcg) & 0xffffff;
-	caps->max_ra_res_qp = 1 << (out->hca_cap.log_max_ra_res_qp & 0x3f);
-	caps->max_ra_req_qp = 1 << (out->hca_cap.log_max_ra_req_qp & 0x3f);
-	caps->max_srq_wqes = 1 << out->hca_cap.log_max_srq_sz;
-	t16 = be16_to_cpu(out->hca_cap.bf_log_bf_reg_size);
-	if (t16 & 0x8000) {
-		caps->bf_reg_size = 1 << (t16 & 0x1f);
-		caps->bf_regs_per_page = MLX5_BF_REGS_PER_PAGE;
-	} else {
-		caps->bf_reg_size = 0;
-		caps->bf_regs_per_page = 0;
-	}
-	caps->min_page_sz = ~(u32)((1 << out->hca_cap.log_pg_sz) - 1);
-
-	memset(&ctx_in, 0, sizeof(ctx_in));
-	memset(&ctx_out, 0, sizeof(ctx_out));
-	ctx_in.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_SPECIAL_CONTEXTS);
-	err = mlx5_cmd_exec(dev, &ctx_in, sizeof(ctx_in),
-				 &ctx_out, sizeof(ctx_out));
-	if (err)
-		goto out_out;
-
-	if (ctx_out.hdr.status)
-		err = mlx5_cmd_status_to_err(&ctx_out.hdr);
-
-	caps->reserved_lkey = be32_to_cpu(ctx_out.reserved_lkey);
-
-out_out:
-	kfree(out);
-
-	return err;
+	return mlx5_core_get_caps(dev, caps, HCA_CAP_OPMOD_GET_CUR);
 }
 
 int mlx5_cmd_init_hca(struct mlx5_core_dev *dev)
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/main.c b/drivers/net/ethernet/mellanox/mlx5/core/main.c
index f2716cc1f51d..3d8e8e489b2d 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/main.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/main.c
@@ -43,6 +43,7 @@
 #include <linux/mlx5/qp.h>
 #include <linux/mlx5/srq.h>
 #include <linux/debugfs.h>
+#include <linux/mlx5/mlx5_ifc.h>
 #include "mlx5_core.h"
 
 #define DRIVER_NAME "mlx5_core"
@@ -207,11 +208,11 @@ static void release_bar(struct pci_dev *pdev)
 static int mlx5_enable_msix(struct mlx5_core_dev *dev)
 {
 	struct mlx5_eq_table *table = &dev->priv.eq_table;
-	int num_eqs = 1 << dev->caps.log_max_eq;
+	int num_eqs = 1 << dev->caps.gen.log_max_eq;
 	int nvec;
 	int i;
 
-	nvec = dev->caps.num_ports * num_online_cpus() + MLX5_EQ_VEC_COMP_BASE;
+	nvec = dev->caps.gen.num_ports * num_online_cpus() + MLX5_EQ_VEC_COMP_BASE;
 	nvec = min_t(int, nvec, num_eqs);
 	if (nvec <= MLX5_EQ_VEC_COMP_BASE)
 		return -ENOMEM;
@@ -250,91 +251,205 @@ struct mlx5_reg_host_endianess {
 #define CAP_MASK(pos, size) ((u64)((1 << (size)) - 1) << (pos))
 
 enum {
-	MLX5_CAP_BITS_RW_MASK	= CAP_MASK(MLX5_CAP_OFF_CMDIF_CSUM, 2) |
-				  CAP_MASK(MLX5_CAP_OFF_DCT, 1),
+	MLX5_CAP_BITS_RW_MASK = CAP_MASK(MLX5_CAP_OFF_CMDIF_CSUM, 2) |
+				MLX5_DEV_CAP_FLAG_DCT,
 };
 
+static u16 to_fw_pkey_sz(u32 size)
+{
+	switch (size) {
+	case 128:
+		return 0;
+	case 256:
+		return 1;
+	case 512:
+		return 2;
+	case 1024:
+		return 3;
+	case 2048:
+		return 4;
+	case 4096:
+		return 5;
+	default:
+		pr_warn("invalid pkey table size %d\n", size);
+		return 0;
+	}
+}
+
 /* selectively copy writable fields clearing any reserved area
  */
-static void copy_rw_fields(struct mlx5_hca_cap *to, struct mlx5_hca_cap *from)
+static void copy_rw_fields(void *to, struct mlx5_caps *from)
 {
+	__be64 *flags_off = (__be64 *)MLX5_ADDR_OF(cmd_hca_cap, to, reserved_22);
 	u64 v64;
 
-	to->log_max_qp = from->log_max_qp & 0x1f;
-	to->log_max_ra_req_dc = from->log_max_ra_req_dc & 0x3f;
-	to->log_max_ra_res_dc = from->log_max_ra_res_dc & 0x3f;
-	to->log_max_ra_req_qp = from->log_max_ra_req_qp & 0x3f;
-	to->log_max_ra_res_qp = from->log_max_ra_res_qp & 0x3f;
-	to->log_max_atomic_size_qp = from->log_max_atomic_size_qp;
-	to->log_max_atomic_size_dc = from->log_max_atomic_size_dc;
-	v64 = be64_to_cpu(from->flags) & MLX5_CAP_BITS_RW_MASK;
-	to->flags = cpu_to_be64(v64);
+	MLX5_SET(cmd_hca_cap, to, log_max_qp, from->gen.log_max_qp);
+	MLX5_SET(cmd_hca_cap, to, log_max_ra_req_qp, from->gen.log_max_ra_req_qp);
+	MLX5_SET(cmd_hca_cap, to, log_max_ra_res_qp, from->gen.log_max_ra_res_qp);
+	MLX5_SET(cmd_hca_cap, to, pkey_table_size, from->gen.pkey_table_size);
+	MLX5_SET(cmd_hca_cap, to, log_max_ra_req_dc, from->gen.log_max_ra_req_dc);
+	MLX5_SET(cmd_hca_cap, to, log_max_ra_res_dc, from->gen.log_max_ra_res_dc);
+	MLX5_SET(cmd_hca_cap, to, pkey_table_size, to_fw_pkey_sz(from->gen.pkey_table_size));
+	v64 = from->gen.flags & MLX5_CAP_BITS_RW_MASK;
+	*flags_off = cpu_to_be64(v64);
 }
 
-enum {
-	HCA_CAP_OPMOD_GET_MAX	= 0,
-	HCA_CAP_OPMOD_GET_CUR	= 1,
-};
+static u16 get_pkey_table_size(int pkey)
+{
+	if (pkey > MLX5_MAX_LOG_PKEY_TABLE)
+		return 0;
 
-static int handle_hca_cap(struct mlx5_core_dev *dev)
+	return MLX5_MIN_PKEY_TABLE_SIZE << pkey;
+}
+
+static void fw2drv_caps(struct mlx5_caps *caps, void *out)
+{
+	struct mlx5_general_caps *gen = &caps->gen;
+
+	gen->max_srq_wqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_srq_sz);
+	gen->max_wqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_qp_sz);
+	gen->log_max_qp = MLX5_GET_PR(cmd_hca_cap, out, log_max_qp);
+	gen->log_max_strq = MLX5_GET_PR(cmd_hca_cap, out, log_max_strq_sz);
+	gen->log_max_srq = MLX5_GET_PR(cmd_hca_cap, out, log_max_srqs);
+	gen->max_cqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_cq_sz);
+	gen->log_max_cq = MLX5_GET_PR(cmd_hca_cap, out, log_max_cq);
+	gen->max_eqes = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_max_eq_sz);
+	gen->log_max_mkey = MLX5_GET_PR(cmd_hca_cap, out, log_max_mkey);
+	gen->log_max_eq = MLX5_GET_PR(cmd_hca_cap, out, log_max_eq);
+	gen->max_indirection = MLX5_GET_PR(cmd_hca_cap, out, max_indirection);
+	gen->log_max_mrw_sz = MLX5_GET_PR(cmd_hca_cap, out, log_max_mrw_sz);
+	gen->log_max_bsf_list_size = MLX5_GET_PR(cmd_hca_cap, out, log_max_bsf_list_size);
+	gen->log_max_klm_list_size = MLX5_GET_PR(cmd_hca_cap, out, log_max_klm_list_size);
+	gen->log_max_ra_req_dc = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_req_dc);
+	gen->log_max_ra_res_dc = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_res_dc);
+	gen->log_max_ra_req_qp = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_req_qp);
+	gen->log_max_ra_res_qp = MLX5_GET_PR(cmd_hca_cap, out, log_max_ra_res_qp);
+	gen->max_qp_counters = MLX5_GET_PR(cmd_hca_cap, out, max_qp_cnt);
+	gen->pkey_table_size = get_pkey_table_size(MLX5_GET_PR(cmd_hca_cap, out, pkey_table_size));
+	gen->local_ca_ack_delay = MLX5_GET_PR(cmd_hca_cap, out, local_ca_ack_delay);
+	gen->num_ports = MLX5_GET_PR(cmd_hca_cap, out, num_ports);
+	gen->log_max_msg = MLX5_GET_PR(cmd_hca_cap, out, log_max_msg);
+	gen->stat_rate_support = MLX5_GET_PR(cmd_hca_cap, out, stat_rate_support);
+	gen->flags = be64_to_cpu(*(__be64 *)MLX5_ADDR_OF(cmd_hca_cap, out, reserved_22));
+	pr_debug("flags = 0x%llx\n", gen->flags);
+	gen->uar_sz = MLX5_GET_PR(cmd_hca_cap, out, uar_sz);
+	gen->min_log_pg_sz = MLX5_GET_PR(cmd_hca_cap, out, log_pg_sz);
+	gen->bf_reg_size = MLX5_GET_PR(cmd_hca_cap, out, bf);
+	gen->bf_reg_size = 1 << MLX5_GET_PR(cmd_hca_cap, out, log_bf_reg_size);
+	gen->max_sq_desc_sz = MLX5_GET_PR(cmd_hca_cap, out, max_wqe_sz_sq);
+	gen->max_rq_desc_sz = MLX5_GET_PR(cmd_hca_cap, out, max_wqe_sz_rq);
+	gen->max_dc_sq_desc_sz = MLX5_GET_PR(cmd_hca_cap, out, max_wqe_sz_sq_dc);
+	gen->max_qp_mcg = MLX5_GET_PR(cmd_hca_cap, out, max_qp_mcg);
+	gen->log_max_pd = MLX5_GET_PR(cmd_hca_cap, out, log_max_pd);
+	gen->log_max_xrcd = MLX5_GET_PR(cmd_hca_cap, out, log_max_xrcd);
+	gen->log_uar_page_sz = MLX5_GET_PR(cmd_hca_cap, out, log_uar_page_sz);
+}
+
+static const char *caps_opmod_str(u16 opmod)
 {
-	struct mlx5_cmd_query_hca_cap_mbox_out *query_out = NULL;
-	struct mlx5_cmd_set_hca_cap_mbox_in *set_ctx = NULL;
-	struct mlx5_cmd_query_hca_cap_mbox_in query_ctx;
-	struct mlx5_cmd_set_hca_cap_mbox_out set_out;
-	u64 flags;
+	switch (opmod) {
+	case HCA_CAP_OPMOD_GET_MAX:
+		return "GET_MAX";
+	case HCA_CAP_OPMOD_GET_CUR:
+		return "GET_CUR";
+	default:
+		return "Invalid";
+	}
+}
+
+int mlx5_core_get_caps(struct mlx5_core_dev *dev, struct mlx5_caps *caps,
+		       u16 opmod)
+{
+	u8 in[MLX5_ST_SZ_BYTES(query_hca_cap_in)];
+	int out_sz = MLX5_ST_SZ_BYTES(query_hca_cap_out);
+	void *out;
 	int err;
 
-	memset(&query_ctx, 0, sizeof(query_ctx));
-	query_out = kzalloc(sizeof(*query_out), GFP_KERNEL);
-	if (!query_out)
+	memset(in, 0, sizeof(in));
+	out = kzalloc(out_sz, GFP_KERNEL);
+	if (!out)
 		return -ENOMEM;
+	MLX5_SET(query_hca_cap_in, in, opcode, MLX5_CMD_OP_QUERY_HCA_CAP);
+	MLX5_SET(query_hca_cap_in, in, op_mod, opmod);
+	err = mlx5_cmd_exec(dev, in, sizeof(in), out, out_sz);
+	if (err)
+		goto query_ex;
 
-	set_ctx = kzalloc(sizeof(*set_ctx), GFP_KERNEL);
-	if (!set_ctx) {
-		err = -ENOMEM;
+	err = mlx5_cmd_status_to_err_v2(out);
+	if (err) {
+		mlx5_core_warn(dev, "query max hca cap failed, %d\n", err);
 		goto query_ex;
 	}
+	mlx5_core_dbg(dev, "%s\n", caps_opmod_str(opmod));
+	fw2drv_caps(caps, MLX5_ADDR_OF(query_hca_cap_out, out, capability_struct));
+
+query_ex:
+	kfree(out);
+	return err;
+}
 
-	query_ctx.hdr.opcode = cpu_to_be16(MLX5_CMD_OP_QUERY_HCA_CAP);
-	query_ctx.hdr.opmod  = cpu_to_be16(HCA_CAP_OPMOD_GET_CUR);
-	err = mlx5_cmd_exec(dev, &query_ctx, sizeof(query_ctx),
-				 query_out, sizeof(*query_out));
+static int set_caps(struct mlx5_core_dev *dev, void *in, int in_sz)
+{
+	u32 out[MLX5_ST_SZ_DW(set_hca_cap_out)];
+	int err;
+
+	memset(out, 0, sizeof(out));
+
+	MLX5_SET(set_hca_cap_in, in, opcode, MLX5_CMD_OP_SET_HCA_CAP);
+	err = mlx5_cmd_exec(dev, in, in_sz, out, sizeof(out));
 	if (err)
-		goto query_ex;
+		return err;
 
-	err = mlx5_cmd_status_to_err(&query_out->hdr);
-	if (err) {
-		mlx5_core_warn(dev, "query hca cap failed, %d\n", err);
+	err = mlx5_cmd_status_to_err_v2(out);
+
+	return err;
+}
+
+static int handle_hca_cap(struct mlx5_core_dev *dev)
+{
+	void *set_ctx = NULL;
+	struct mlx5_profile *prof = dev->profile;
+	struct mlx5_caps *cur_caps = NULL;
+	struct mlx5_caps *max_caps = NULL;
+	int err = -ENOMEM;
+	int set_sz = MLX5_ST_SZ_BYTES(set_hca_cap_in);
+
+	set_ctx = kzalloc(set_sz, GFP_KERNEL);
+	if (!set_ctx)
 		goto query_ex;
-	}
 
-	copy_rw_fields(&set_ctx->hca_cap, &query_out->hca_cap);
+	max_caps = kzalloc(sizeof(*max_caps), GFP_KERNEL);
+	if (!max_caps)
+		goto query_ex;
 
-	if (dev->profile && dev->profile->mask & MLX5_PROF_MASK_QP_SIZE)
-		set_ctx->hca_cap.log_max_qp = dev->profile->log_max_qp;
+	cur_caps = kzalloc(sizeof(*cur_caps), GFP_KERNEL);
+	if (!cur_caps)
+		goto query_ex;
 
-	flags = be64_to_cpu(query_out->hca_cap.flags);
-	/* disable checksum */
-	flags &= ~MLX5_DEV_CAP_FLAG_CMDIF_CSUM;
-
-	set_ctx->hca_cap.flags = cpu_to_be64(flags);
-	memset(&set_out, 0, sizeof(set_out));
-	set_ctx->hca_cap.log_uar_page_sz = cpu_to_be16(PAGE_SHIFT - 12);
-	set_ctx->hdr.opcode = cpu_to_be16(MLX5_CMD_OP_SET_HCA_CAP);
-	err = mlx5_cmd_exec(dev, set_ctx, sizeof(*set_ctx),
-				 &set_out, sizeof(set_out));
-	if (err) {
-		mlx5_core_warn(dev, "set hca cap failed, %d\n", err);
+	err = mlx5_core_get_caps(dev, max_caps, HCA_CAP_OPMOD_GET_MAX);
+	if (err)
 		goto query_ex;
-	}
 
-	err = mlx5_cmd_status_to_err(&set_out.hdr);
+	err = mlx5_core_get_caps(dev, cur_caps, HCA_CAP_OPMOD_GET_CUR);
 	if (err)
 		goto query_ex;
 
+	/* we limit the size of the pkey table to 128 entries for now */
+	cur_caps->gen.pkey_table_size = 128;
+
+	if (prof->mask & MLX5_PROF_MASK_QP_SIZE)
+		cur_caps->gen.log_max_qp = prof->log_max_qp;
+
+	/* disable checksum */
+	cur_caps->gen.flags &= ~MLX5_DEV_CAP_FLAG_CMDIF_CSUM;
+
+	copy_rw_fields(MLX5_ADDR_OF(set_hca_cap_in, set_ctx, hca_capability_struct),
+		       cur_caps);
+	err = set_caps(dev, set_ctx, set_sz);
+
 query_ex:
-	kfree(query_out);
+	kfree(cur_caps);
+	kfree(max_caps);
 	kfree(set_ctx);
 
 	return err;
@@ -782,6 +897,7 @@ static void remove_one(struct pci_dev *pdev)
 
 static const struct pci_device_id mlx5_core_pci_table[] = {
 	{ PCI_VDEVICE(MELLANOX, 4113) }, /* MT4113 Connect-IB */
+	{ PCI_VDEVICE(MELLANOX, 4115) }, /* ConnectX-4 */
 	{ 0, }
 };
 
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/qp.c b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
index 8145b4668229..5261a2b0da43 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/qp.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/qp.c
@@ -39,28 +39,53 @@
 
 #include "mlx5_core.h"
 
-void mlx5_qp_event(struct mlx5_core_dev *dev, u32 qpn, int event_type)
+static struct mlx5_core_rsc_common *mlx5_get_rsc(struct mlx5_core_dev *dev,
+						 u32 rsn)
 {
 	struct mlx5_qp_table *table = &dev->priv.qp_table;
-	struct mlx5_core_qp *qp;
+	struct mlx5_core_rsc_common *common;
 
 	spin_lock(&table->lock);
 
-	qp = radix_tree_lookup(&table->tree, qpn);
-	if (qp)
-		atomic_inc(&qp->refcount);
+	common = radix_tree_lookup(&table->tree, rsn);
+	if (common)
+		atomic_inc(&common->refcount);
 
 	spin_unlock(&table->lock);
 
-	if (!qp) {
-		mlx5_core_warn(dev, "Async event for bogus QP 0x%x\n", qpn);
-		return;
+	if (!common) {
+		mlx5_core_warn(dev, "Async event for bogus resource 0x%x\n",
+			       rsn);
+		return NULL;
 	}
+	return common;
+}
 
-	qp->event(qp, event_type);
+void mlx5_core_put_rsc(struct mlx5_core_rsc_common *common)
+{
+	if (atomic_dec_and_test(&common->refcount))
+		complete(&common->free);
+}
+
+void mlx5_rsc_event(struct mlx5_core_dev *dev, u32 rsn, int event_type)
+{
+	struct mlx5_core_rsc_common *common = mlx5_get_rsc(dev, rsn);
+	struct mlx5_core_qp *qp;
+
+	if (!common)
+		return;
+
+	switch (common->res) {
+	case MLX5_RES_QP:
+		qp = (struct mlx5_core_qp *)common;
+		qp->event(qp, event_type);
+		break;
+
+	default:
+		mlx5_core_warn(dev, "invalid resource type for 0x%x\n", rsn);
+	}
 
-	if (atomic_dec_and_test(&qp->refcount))
-		complete(&qp->free);
+	mlx5_core_put_rsc(common);
 }
 
 int mlx5_core_create_qp(struct mlx5_core_dev *dev,
@@ -92,6 +117,7 @@ int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 	qp->qpn = be32_to_cpu(out.qpn) & 0xffffff;
 	mlx5_core_dbg(dev, "qpn = 0x%x\n", qp->qpn);
 
+	qp->common.res = MLX5_RES_QP;
 	spin_lock_irq(&table->lock);
 	err = radix_tree_insert(&table->tree, qp->qpn, qp);
 	spin_unlock_irq(&table->lock);
@@ -106,9 +132,9 @@ int mlx5_core_create_qp(struct mlx5_core_dev *dev,
 			      qp->qpn);
 
 	qp->pid = current->pid;
-	atomic_set(&qp->refcount, 1);
+	atomic_set(&qp->common.refcount, 1);
 	atomic_inc(&dev->num_qps);
-	init_completion(&qp->free);
+	init_completion(&qp->common.free);
 
 	return 0;
 
@@ -138,9 +164,8 @@ int mlx5_core_destroy_qp(struct mlx5_core_dev *dev,
 	radix_tree_delete(&table->tree, qp->qpn);
 	spin_unlock_irqrestore(&table->lock, flags);
 
-	if (atomic_dec_and_test(&qp->refcount))
-		complete(&qp->free);
-	wait_for_completion(&qp->free);
+	mlx5_core_put_rsc((struct mlx5_core_rsc_common *)qp);
+	wait_for_completion(&qp->common.free);
 
 	memset(&in, 0, sizeof(in));
 	memset(&out, 0, sizeof(out));
@@ -184,13 +209,10 @@ int mlx5_core_qp_modify(struct mlx5_core_dev *dev, enum mlx5_qp_state cur_state,
 			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
 			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
 			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_RTS2RTS_QP,
-			[MLX5_QP_STATE_SQD]	= MLX5_CMD_OP_RTS2SQD_QP,
 		},
 		[MLX5_QP_STATE_SQD] = {
 			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
 			[MLX5_QP_STATE_ERR]	= MLX5_CMD_OP_2ERR_QP,
-			[MLX5_QP_STATE_RTS]	= MLX5_CMD_OP_SQD2RTS_QP,
-			[MLX5_QP_STATE_SQD]	= MLX5_CMD_OP_SQD2SQD_QP,
 		},
 		[MLX5_QP_STATE_SQER] = {
 			[MLX5_QP_STATE_RST]	= MLX5_CMD_OP_2RST_QP,
diff --git a/drivers/net/ethernet/mellanox/mlx5/core/uar.c b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
index 68f5d9c77c7b..0a6348cefc01 100644
--- a/drivers/net/ethernet/mellanox/mlx5/core/uar.c
+++ b/drivers/net/ethernet/mellanox/mlx5/core/uar.c
@@ -174,11 +174,11 @@ int mlx5_alloc_uuars(struct mlx5_core_dev *dev, struct mlx5_uuar_info *uuari)
 	for (i = 0; i < tot_uuars; i++) {
 		bf = &uuari->bfs[i];
 
-		bf->buf_size = dev->caps.bf_reg_size / 2;
+		bf->buf_size = dev->caps.gen.bf_reg_size / 2;
 		bf->uar = &uuari->uars[i / MLX5_BF_REGS_PER_PAGE];
 		bf->regreg = uuari->uars[i / MLX5_BF_REGS_PER_PAGE].map;
 		bf->reg = NULL; /* Add WC support */
-		bf->offset = (i % MLX5_BF_REGS_PER_PAGE) * dev->caps.bf_reg_size +
+		bf->offset = (i % MLX5_BF_REGS_PER_PAGE) * dev->caps.gen.bf_reg_size +
 			MLX5_BF_OFFSET;
 		bf->need_lock = need_uuar_lock(i);
 		spin_lock_init(&bf->lock);
diff --git a/drivers/net/ethernet/micrel/ks8842.c b/drivers/net/ethernet/micrel/ks8842.c
index 822616e3c375..0c33b92a5a81 100644
--- a/drivers/net/ethernet/micrel/ks8842.c
+++ b/drivers/net/ethernet/micrel/ks8842.c
@@ -875,13 +875,11 @@ static void ks8842_stop_dma(struct ks8842_adapter *adapter)
 
 	tx_ctl->adesc = NULL;
 	if (tx_ctl->chan)
-		tx_ctl->chan->device->device_control(tx_ctl->chan,
-			DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(tx_ctl->chan);
 
 	rx_ctl->adesc = NULL;
 	if (rx_ctl->chan)
-		rx_ctl->chan->device->device_control(rx_ctl->chan,
-			DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(rx_ctl->chan);
 
 	if (sg_dma_address(&rx_ctl->sg))
 		dma_unmap_single(adapter->dev, sg_dma_address(&rx_ctl->sg),
diff --git a/drivers/net/ethernet/moxa/moxart_ether.c b/drivers/net/ethernet/moxa/moxart_ether.c
index 5020fd47825d..bde1b70f473b 100644
--- a/drivers/net/ethernet/moxa/moxart_ether.c
+++ b/drivers/net/ethernet/moxa/moxart_ether.c
@@ -206,7 +206,7 @@ static int moxart_rx_poll(struct napi_struct *napi, int budget)
 	int rx_head = priv->rx_head;
 	int rx = 0;
 
-	while (1) {
+	while (rx < budget) {
 		desc = priv->rx_desc_base + (RX_REG_DESC_SIZE * rx_head);
 		desc0 = readl(desc + RX_REG_OFFSET_DESC0);
 
@@ -218,7 +218,7 @@ static int moxart_rx_poll(struct napi_struct *napi, int budget)
 			net_dbg_ratelimited("packet error\n");
 			priv->stats.rx_dropped++;
 			priv->stats.rx_errors++;
-			continue;
+			goto rx_next;
 		}
 
 		len = desc0 & RX_DESC0_FRAME_LEN_MASK;
@@ -226,13 +226,19 @@ static int moxart_rx_poll(struct napi_struct *napi, int budget)
 		if (len > RX_BUF_SIZE)
 			len = RX_BUF_SIZE;
 
-		skb = build_skb(priv->rx_buf[rx_head], priv->rx_buf_size);
+		dma_sync_single_for_cpu(&ndev->dev,
+					priv->rx_mapping[rx_head],
+					priv->rx_buf_size, DMA_FROM_DEVICE);
+		skb = netdev_alloc_skb_ip_align(ndev, len);
+
 		if (unlikely(!skb)) {
-			net_dbg_ratelimited("build_skb failed\n");
+			net_dbg_ratelimited("netdev_alloc_skb_ip_align failed\n");
 			priv->stats.rx_dropped++;
 			priv->stats.rx_errors++;
+			goto rx_next;
 		}
 
+		memcpy(skb->data, priv->rx_buf[rx_head], len);
 		skb_put(skb, len);
 		skb->protocol = eth_type_trans(skb, ndev);
 		napi_gro_receive(&priv->napi, skb);
@@ -244,18 +250,15 @@ static int moxart_rx_poll(struct napi_struct *napi, int budget)
 		if (desc0 & RX_DESC0_MULTICAST)
 			priv->stats.multicast++;
 
+rx_next:
 		writel(RX_DESC0_DMA_OWN, desc + RX_REG_OFFSET_DESC0);
 
 		rx_head = RX_NEXT(rx_head);
 		priv->rx_head = rx_head;
-
-		if (rx >= budget)
-			break;
 	}
 
 	if (rx < budget) {
-		napi_gro_flush(napi, false);
-		__napi_complete(napi);
+		napi_complete(napi);
 	}
 
 	priv->reg_imr |= RPKT_FINISH_M;
@@ -346,10 +349,12 @@ static int moxart_mac_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 		len = ETH_ZLEN;
 	}
 
-	txdes1 = readl(desc + TX_REG_OFFSET_DESC1);
-	txdes1 |= TX_DESC1_LTS | TX_DESC1_FTS;
-	txdes1 &= ~(TX_DESC1_FIFO_COMPLETE | TX_DESC1_INTR_COMPLETE);
-	txdes1 |= (len & TX_DESC1_BUF_SIZE_MASK);
+	dma_sync_single_for_device(&ndev->dev, priv->tx_mapping[tx_head],
+				   priv->tx_buf_size, DMA_TO_DEVICE);
+
+	txdes1 = TX_DESC1_LTS | TX_DESC1_FTS | (len & TX_DESC1_BUF_SIZE_MASK);
+	if (tx_head == TX_DESC_NUM_MASK)
+		txdes1 |= TX_DESC1_END;
 	writel(txdes1, desc + TX_REG_OFFSET_DESC1);
 	writel(TX_DESC0_DMA_OWN, desc + TX_REG_OFFSET_DESC0);
 
@@ -465,8 +470,7 @@ static int moxart_mac_probe(struct platform_device *pdev)
 	spin_lock_init(&priv->txlock);
 
 	priv->tx_buf_size = TX_BUF_SIZE;
-	priv->rx_buf_size = RX_BUF_SIZE +
-			    SKB_DATA_ALIGN(sizeof(struct skb_shared_info));
+	priv->rx_buf_size = RX_BUF_SIZE;
 
 	priv->tx_desc_base = dma_alloc_coherent(NULL, TX_REG_DESC_SIZE *
 						TX_DESC_NUM, &priv->tx_base,
@@ -507,7 +511,6 @@ static int moxart_mac_probe(struct platform_device *pdev)
 		goto init_fail;
 	}
 
-	ether_setup(ndev);
 	ndev->netdev_ops = &moxart_netdev_ops;
 	netif_napi_add(ndev, &priv->napi, moxart_rx_poll, RX_DESC_NUM);
 	ndev->priv_flags |= IFF_UNICAST_FLT;
diff --git a/drivers/net/ethernet/neterion/vxge/vxge-main.c b/drivers/net/ethernet/neterion/vxge/vxge-main.c
index 4f40d7b8629e..cc0485e3c621 100644
--- a/drivers/net/ethernet/neterion/vxge/vxge-main.c
+++ b/drivers/net/ethernet/neterion/vxge/vxge-main.c
@@ -3537,7 +3537,7 @@ static void vxge_device_unregister(struct __vxge_hw_device *hldev)
 	vxge_debug_entryexit(vdev->level_trace,	"%s: %s:%d", vdev->ndev->name,
 			     __func__, __LINE__);
 
-	strncpy(buf, dev->name, IFNAMSIZ);
+	strlcpy(buf, dev->name, IFNAMSIZ);
 
 	flush_work(&vdev->reset_task);
 
diff --git a/drivers/net/ethernet/netx-eth.c b/drivers/net/ethernet/netx-eth.c
index 31eb911e4763..8176c8a1cc6a 100644
--- a/drivers/net/ethernet/netx-eth.c
+++ b/drivers/net/ethernet/netx-eth.c
@@ -315,8 +315,6 @@ static int netx_eth_enable(struct net_device *ndev)
 	unsigned int mac4321, mac65;
 	int running, i;
 
-	ether_setup(ndev);
-
 	ndev->netdev_ops = &netx_eth_netdev_ops;
 	ndev->watchdog_timeo = msecs_to_jiffies(5000);
 
diff --git a/drivers/net/ethernet/nuvoton/w90p910_ether.c b/drivers/net/ethernet/nuvoton/w90p910_ether.c
index 79645f74b3a8..379b7fbded78 100644
--- a/drivers/net/ethernet/nuvoton/w90p910_ether.c
+++ b/drivers/net/ethernet/nuvoton/w90p910_ether.c
@@ -943,7 +943,6 @@ static int w90p910_ether_setup(struct net_device *dev)
 {
 	struct w90p910_ether *ether = netdev_priv(dev);
 
-	ether_setup(dev);
 	dev->netdev_ops = &w90p910_ether_netdev_ops;
 	dev->ethtool_ops = &w90p910_ether_ethtool_ops;
 
diff --git a/drivers/net/ethernet/nvidia/forcedeth.c b/drivers/net/ethernet/nvidia/forcedeth.c
index 925b296d8ab8..f39cae620f61 100644
--- a/drivers/net/ethernet/nvidia/forcedeth.c
+++ b/drivers/net/ethernet/nvidia/forcedeth.c
@@ -1481,7 +1481,7 @@ static int phy_init(struct net_device *dev)
 	}
 
 	/* phy vendor specific configuration */
-	if ((np->phy_oui == PHY_OUI_CICADA)) {
+	if (np->phy_oui == PHY_OUI_CICADA) {
 		if (init_cicada(dev, np, phyinterface)) {
 			netdev_info(dev, "%s: phy init failed\n",
 				    pci_name(np->pci_dev));
diff --git a/drivers/net/ethernet/nxp/lpc_eth.c b/drivers/net/ethernet/nxp/lpc_eth.c
index 8706c0dbd0c3..66fd868152e5 100644
--- a/drivers/net/ethernet/nxp/lpc_eth.c
+++ b/drivers/net/ethernet/nxp/lpc_eth.c
@@ -1220,6 +1220,9 @@ static int lpc_eth_open(struct net_device *ndev)
 
 	__lpc_eth_clock_enable(pldat, true);
 
+	/* Suspended PHY makes LPC ethernet core block, so resume now */
+	phy_resume(pldat->phy_dev);
+
 	/* Reset and initialize */
 	__lpc_eth_reset(pldat);
 	__lpc_eth_init(pldat);
@@ -1374,9 +1377,6 @@ static int lpc_eth_drv_probe(struct platform_device *pdev)
 		goto err_out_iounmap;
 	}
 
-	/* Fill in the fields of the device structure with ethernet values. */
-	ether_setup(ndev);
-
 	/* Setup driver functions */
 	ndev->netdev_ops = &lpc_netdev_ops;
 	ndev->ethtool_ops = &lpc_eth_ethtool_ops;
diff --git a/drivers/net/ethernet/octeon/octeon_mgmt.c b/drivers/net/ethernet/octeon/octeon_mgmt.c
index 979c6980639f..a42293092ea4 100644
--- a/drivers/net/ethernet/octeon/octeon_mgmt.c
+++ b/drivers/net/ethernet/octeon/octeon_mgmt.c
@@ -290,9 +290,11 @@ static void octeon_mgmt_clean_tx_buffers(struct octeon_mgmt *p)
 		/* Read the hardware TX timestamp if one was recorded */
 		if (unlikely(re.s.tstamp)) {
 			struct skb_shared_hwtstamps ts;
+			u64 ns;
+
 			memset(&ts, 0, sizeof(ts));
 			/* Read the timestamp */
-			u64 ns = cvmx_read_csr(CVMX_MIXX_TSTAMP(p->port));
+			ns = cvmx_read_csr(CVMX_MIXX_TSTAMP(p->port));
 			/* Remove the timestamp from the FIFO */
 			cvmx_write_csr(CVMX_MIXX_TSCTL(p->port), 0);
 			/* Tell the kernel about the timestamp */
diff --git a/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig b/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
index 44c8be1c6805..5f7a35212796 100644
--- a/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
+++ b/drivers/net/ethernet/oki-semi/pch_gbe/Kconfig
@@ -7,6 +7,7 @@ config PCH_GBE
 	depends on PCI && (X86_32 || COMPILE_TEST)
 	select MII
 	select PTP_1588_CLOCK_PCH
+	select NET_PTP_CLASSIFY
 	---help---
 	  This is a gigabit ethernet driver for EG20T PCH.
 	  EG20T PCH is the platform controller hub that is used in Intel's
diff --git a/drivers/net/ethernet/packetengines/yellowfin.c b/drivers/net/ethernet/packetengines/yellowfin.c
index 2d6b148528dd..fa2db41e02f8 100644
--- a/drivers/net/ethernet/packetengines/yellowfin.c
+++ b/drivers/net/ethernet/packetengines/yellowfin.c
@@ -693,11 +693,11 @@ static void yellowfin_tx_timeout(struct net_device *dev)
 	/* Note: these should be KERN_DEBUG. */
 	if (yellowfin_debug) {
 		int i;
-		pr_warning("  Rx ring %p: ", yp->rx_ring);
+		pr_warn("  Rx ring %p: ", yp->rx_ring);
 		for (i = 0; i < RX_RING_SIZE; i++)
 			pr_cont(" %08x", yp->rx_ring[i].result_status);
 		pr_cont("\n");
-		pr_warning("  Tx ring %p: ", yp->tx_ring);
+		pr_warn("  Tx ring %p: ", yp->tx_ring);
 		for (i = 0; i < TX_RING_SIZE; i++)
 			pr_cont(" %04x /%08x",
 			       yp->tx_status[i].tx_errs,
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
index db4280ce9c09..716fc37ada5a 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_hw.c
@@ -922,7 +922,7 @@ int netxen_config_ipaddr(struct netxen_adapter *adapter, __be32 ip, int cmd)
 
 	rv = netxen_send_cmd_descs(adapter, (struct cmd_desc_type0 *)&req, 1);
 	if (rv != 0) {
-		printk(KERN_ERR "%s: could not notify %s IP 0x%x reuqest\n",
+		printk(KERN_ERR "%s: could not notify %s IP 0x%x request\n",
 				adapter->netdev->name,
 				(cmd == NX_IP_UP) ? "Add" : "Remove", ip);
 	}
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
index 32058614151a..5c4068353f66 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_init.c
@@ -135,6 +135,7 @@ void netxen_release_tx_buffers(struct netxen_adapter *adapter)
 	int i, j;
 	struct nx_host_tx_ring *tx_ring = adapter->tx_ring;
 
+	spin_lock(&adapter->tx_clean_lock);
 	cmd_buf = tx_ring->cmd_buf_arr;
 	for (i = 0; i < tx_ring->num_desc; i++) {
 		buffrag = cmd_buf->frag_array;
@@ -158,6 +159,7 @@ void netxen_release_tx_buffers(struct netxen_adapter *adapter)
 		}
 		cmd_buf++;
 	}
+	spin_unlock(&adapter->tx_clean_lock);
 }
 
 void netxen_free_sw_resources(struct netxen_adapter *adapter)
@@ -1792,9 +1794,9 @@ int netxen_process_cmd_ring(struct netxen_adapter *adapter)
 			break;
 	}
 
-	if (count && netif_running(netdev)) {
-		tx_ring->sw_consumer = sw_consumer;
+	tx_ring->sw_consumer = sw_consumer;
 
+	if (count && netif_running(netdev)) {
 		smp_mb();
 
 		if (netif_queue_stopped(netdev) && netif_carrier_ok(netdev))
diff --git a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
index 1159031f885b..0b2a1ccd276d 100644
--- a/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
+++ b/drivers/net/ethernet/qlogic/netxen/netxen_nic_main.c
@@ -1186,7 +1186,6 @@ __netxen_nic_down(struct netxen_adapter *adapter, struct net_device *netdev)
 		return;
 
 	smp_mb();
-	spin_lock(&adapter->tx_clean_lock);
 	netif_carrier_off(netdev);
 	netif_tx_disable(netdev);
 
@@ -1204,7 +1203,6 @@ __netxen_nic_down(struct netxen_adapter *adapter, struct net_device *netdev)
 	netxen_napi_disable(adapter);
 
 	netxen_release_tx_buffers(adapter);
-	spin_unlock(&adapter->tx_clean_lock);
 }
 
 /* Usage: During suspend and firmware recovery module */
@@ -1477,9 +1475,8 @@ netxen_nic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 	u32 val;
 
 	if (pdev->revision >= NX_P3_A0 && pdev->revision <= NX_P3_B1) {
-		pr_warning("%s: chip revisions between 0x%x-0x%x "
-				"will not be enabled.\n",
-				module_name(THIS_MODULE), NX_P3_A0, NX_P3_B1);
+		pr_warn("%s: chip revisions between 0x%x-0x%x will not be enabled\n",
+			module_name(THIS_MODULE), NX_P3_A0, NX_P3_B1);
 		return -ENODEV;
 	}
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
index 16039d1497b8..e56c1bb36141 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic.h
@@ -39,8 +39,8 @@
 
 #define _QLCNIC_LINUX_MAJOR 5
 #define _QLCNIC_LINUX_MINOR 3
-#define _QLCNIC_LINUX_SUBVERSION 61
-#define QLCNIC_LINUX_VERSIONID  "5.3.61"
+#define _QLCNIC_LINUX_SUBVERSION 62
+#define QLCNIC_LINUX_VERSIONID  "5.3.62"
 #define QLCNIC_DRV_IDC_VER  0x01
 #define QLCNIC_DRIVER_VERSION  ((_QLCNIC_LINUX_MAJOR << 16) |\
 		 (_QLCNIC_LINUX_MINOR << 8) | (_QLCNIC_LINUX_SUBVERSION))
@@ -268,7 +268,7 @@ struct qlcnic_fdt {
 	u16	cksum;
 	u16	unused;
 	u8	model[16];
-	u16	mfg_id;
+	u8	mfg_id;
 	u16	id;
 	u8	flag;
 	u8	erase_cmd;
@@ -540,6 +540,8 @@ struct qlcnic_hardware_context {
 	u8 lb_mode;
 	u16 vxlan_port;
 	struct device *hwmon_dev;
+	u32 post_mode;
+	bool run_post;
 };
 
 struct qlcnic_adapter_stats {
@@ -2283,6 +2285,7 @@ extern const struct ethtool_ops qlcnic_ethtool_failed_ops;
 
 #define PCI_DEVICE_ID_QLOGIC_QLE824X		0x8020
 #define PCI_DEVICE_ID_QLOGIC_QLE834X		0x8030
+#define PCI_DEVICE_ID_QLOGIC_QLE8830		0x8830
 #define PCI_DEVICE_ID_QLOGIC_VF_QLE834X	0x8430
 #define PCI_DEVICE_ID_QLOGIC_QLE844X		0x8040
 #define PCI_DEVICE_ID_QLOGIC_VF_QLE844X	0x8440
@@ -2307,6 +2310,7 @@ static inline bool qlcnic_83xx_check(struct qlcnic_adapter *adapter)
 	bool status;
 
 	status = ((device == PCI_DEVICE_ID_QLOGIC_QLE834X) ||
+		  (device == PCI_DEVICE_ID_QLOGIC_QLE8830) ||
 		  (device == PCI_DEVICE_ID_QLOGIC_QLE844X) ||
 		  (device == PCI_DEVICE_ID_QLOGIC_VF_QLE844X) ||
 		  (device == PCI_DEVICE_ID_QLOGIC_VF_QLE834X)) ? true : false;
@@ -2362,6 +2366,19 @@ static inline u32 qlcnic_get_vnic_func_count(struct qlcnic_adapter *adapter)
 		return QLC_DEFAULT_VNIC_COUNT;
 }
 
+static inline void qlcnic_swap32_buffer(u32 *buffer, int count)
+{
+#if defined(__BIG_ENDIAN)
+	u32 *tmp = buffer;
+	int i;
+
+	for (i = 0; i < count; i++) {
+		*tmp = swab32(*tmp);
+		tmp++;
+	}
+#endif
+}
+
 #ifdef CONFIG_QLCNIC_HWMON
 void qlcnic_register_hwmon_dev(struct qlcnic_adapter *);
 void qlcnic_unregister_hwmon_dev(struct qlcnic_adapter *);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
index a4a4ec0b68f8..840bf36b5e9d 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.c
@@ -35,6 +35,35 @@ static void qlcnic_83xx_get_beacon_state(struct qlcnic_adapter *);
 #define QLC_SKIP_INACTIVE_PCI_REGS	7
 #define QLC_MAX_LEGACY_FUNC_SUPP	8
 
+/* 83xx Module type */
+#define QLC_83XX_MODULE_FIBRE_10GBASE_LRM	0x1 /* 10GBase-LRM */
+#define QLC_83XX_MODULE_FIBRE_10GBASE_LR	0x2 /* 10GBase-LR */
+#define QLC_83XX_MODULE_FIBRE_10GBASE_SR	0x3 /* 10GBase-SR */
+#define QLC_83XX_MODULE_DA_10GE_PASSIVE_CP	0x4 /* 10GE passive
+						     * copper(compliant)
+						     */
+#define QLC_83XX_MODULE_DA_10GE_ACTIVE_CP	0x5 /* 10GE active limiting
+						     * copper(compliant)
+						     */
+#define QLC_83XX_MODULE_DA_10GE_LEGACY_CP	0x6 /* 10GE passive copper
+						     * (legacy, best effort)
+						     */
+#define QLC_83XX_MODULE_FIBRE_1000BASE_SX	0x7 /* 1000Base-SX */
+#define QLC_83XX_MODULE_FIBRE_1000BASE_LX	0x8 /* 1000Base-LX */
+#define QLC_83XX_MODULE_FIBRE_1000BASE_CX	0x9 /* 1000Base-CX */
+#define QLC_83XX_MODULE_TP_1000BASE_T		0xa /* 1000Base-T*/
+#define QLC_83XX_MODULE_DA_1GE_PASSIVE_CP	0xb /* 1GE passive copper
+						     * (legacy, best effort)
+						     */
+#define QLC_83XX_MODULE_UNKNOWN			0xf /* Unknown module type */
+
+/* Port types */
+#define QLC_83XX_10_CAPABLE	 BIT_8
+#define QLC_83XX_100_CAPABLE	 BIT_9
+#define QLC_83XX_1G_CAPABLE	 BIT_10
+#define QLC_83XX_10G_CAPABLE	 BIT_11
+#define QLC_83XX_AUTONEG_ENABLE	 BIT_15
+
 static const struct qlcnic_mailbox_metadata qlcnic_83xx_mbx_tbl[] = {
 	{QLCNIC_CMD_CONFIGURE_IP_ADDR, 6, 1},
 	{QLCNIC_CMD_CONFIG_INTRPT, 18, 34},
@@ -667,6 +696,7 @@ void qlcnic_83xx_write_crb(struct qlcnic_adapter *adapter, char *buf,
 
 int qlcnic_83xx_get_port_info(struct qlcnic_adapter *adapter)
 {
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
 	int status;
 
 	status = qlcnic_83xx_get_port_config(adapter);
@@ -674,13 +704,20 @@ int qlcnic_83xx_get_port_info(struct qlcnic_adapter *adapter)
 		dev_err(&adapter->pdev->dev,
 			"Get Port Info failed\n");
 	} else {
-		if (QLC_83XX_SFP_10G_CAPABLE(adapter->ahw->port_config))
-			adapter->ahw->port_type = QLCNIC_XGBE;
-		else
-			adapter->ahw->port_type = QLCNIC_GBE;
 
-		if (QLC_83XX_AUTONEG(adapter->ahw->port_config))
-			adapter->ahw->link_autoneg = AUTONEG_ENABLE;
+		if (ahw->port_config & QLC_83XX_10G_CAPABLE) {
+			ahw->port_type = QLCNIC_XGBE;
+		} else if (ahw->port_config & QLC_83XX_10_CAPABLE ||
+			   ahw->port_config & QLC_83XX_100_CAPABLE ||
+			   ahw->port_config & QLC_83XX_1G_CAPABLE) {
+			ahw->port_type = QLCNIC_GBE;
+		} else {
+			ahw->port_type = QLCNIC_XGBE;
+		}
+
+		if (QLC_83XX_AUTONEG(ahw->port_config))
+			ahw->link_autoneg = AUTONEG_ENABLE;
+
 	}
 	return status;
 }
@@ -2603,7 +2640,7 @@ int qlcnic_83xx_lockless_flash_read32(struct qlcnic_adapter *adapter,
 	}
 
 	qlcnic_83xx_wrt_reg_indirect(adapter, QLC_83XX_FLASH_DIRECT_WINDOW,
-				     (addr));
+				     (addr & 0xFFFF0000));
 
 	range = flash_offset + (count * sizeof(u32));
 	/* Check if data is spread across multiple sectors */
@@ -2664,7 +2701,7 @@ static int qlcnic_83xx_poll_flash_status_reg(struct qlcnic_adapter *adapter)
 		    QLC_83XX_FLASH_STATUS_READY)
 			break;
 
-		msleep(QLC_83XX_FLASH_STATUS_REG_POLL_DELAY);
+		usleep_range(1000, 1100);
 	} while (--retries);
 
 	if (!retries)
@@ -2753,7 +2790,7 @@ int qlcnic_83xx_read_flash_descriptor_table(struct qlcnic_adapter *adapter)
 	ret = qlcnic_83xx_lockless_flash_read32(adapter, QLCNIC_FDT_LOCATION,
 						(u8 *)&adapter->ahw->fdt,
 						count);
-
+	qlcnic_swap32_buffer((u32 *)&adapter->ahw->fdt, count);
 	qlcnic_83xx_unlock_flash(adapter);
 	return ret;
 }
@@ -2788,7 +2825,7 @@ int qlcnic_83xx_erase_flash_sector(struct qlcnic_adapter *adapter,
 
 	addr1 = (sector_start_addr & 0xFF) << 16;
 	addr2 = (sector_start_addr & 0xFF0000) >> 16;
-	reversed_addr = addr1 | addr2;
+	reversed_addr = addr1 | addr2 | (sector_start_addr & 0xFF00);
 
 	qlcnic_83xx_wrt_reg_indirect(adapter, QLC_83XX_FLASH_WRDATA,
 				     reversed_addr);
@@ -3176,22 +3213,33 @@ int qlcnic_83xx_test_link(struct qlcnic_adapter *adapter)
 			break;
 		}
 		config = cmd.rsp.arg[3];
-		if (QLC_83XX_SFP_PRESENT(config)) {
-			switch (ahw->module_type) {
-			case LINKEVENT_MODULE_OPTICAL_UNKNOWN:
-			case LINKEVENT_MODULE_OPTICAL_SRLR:
-			case LINKEVENT_MODULE_OPTICAL_LRM:
-			case LINKEVENT_MODULE_OPTICAL_SFP_1G:
-				ahw->supported_type = PORT_FIBRE;
-				break;
-			case LINKEVENT_MODULE_TWINAX_UNSUPPORTED_CABLE:
-			case LINKEVENT_MODULE_TWINAX_UNSUPPORTED_CABLELEN:
-			case LINKEVENT_MODULE_TWINAX:
-				ahw->supported_type = PORT_TP;
-				break;
-			default:
-				ahw->supported_type = PORT_OTHER;
-			}
+		switch (QLC_83XX_SFP_MODULE_TYPE(config)) {
+		case QLC_83XX_MODULE_FIBRE_10GBASE_LRM:
+		case QLC_83XX_MODULE_FIBRE_10GBASE_LR:
+		case QLC_83XX_MODULE_FIBRE_10GBASE_SR:
+			ahw->supported_type = PORT_FIBRE;
+			ahw->port_type = QLCNIC_XGBE;
+			break;
+		case QLC_83XX_MODULE_FIBRE_1000BASE_SX:
+		case QLC_83XX_MODULE_FIBRE_1000BASE_LX:
+		case QLC_83XX_MODULE_FIBRE_1000BASE_CX:
+			ahw->supported_type = PORT_FIBRE;
+			ahw->port_type = QLCNIC_GBE;
+			break;
+		case QLC_83XX_MODULE_TP_1000BASE_T:
+			ahw->supported_type = PORT_TP;
+			ahw->port_type = QLCNIC_GBE;
+			break;
+		case QLC_83XX_MODULE_DA_10GE_PASSIVE_CP:
+		case QLC_83XX_MODULE_DA_10GE_ACTIVE_CP:
+		case QLC_83XX_MODULE_DA_10GE_LEGACY_CP:
+		case QLC_83XX_MODULE_DA_1GE_PASSIVE_CP:
+			ahw->supported_type = PORT_DA;
+			ahw->port_type = QLCNIC_XGBE;
+			break;
+		default:
+			ahw->supported_type = PORT_OTHER;
+			ahw->port_type = QLCNIC_XGBE;
 		}
 		if (config & 1)
 			err = 1;
@@ -3204,9 +3252,9 @@ out:
 int qlcnic_83xx_get_settings(struct qlcnic_adapter *adapter,
 			     struct ethtool_cmd *ecmd)
 {
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
 	u32 config = 0;
 	int status = 0;
-	struct qlcnic_hardware_context *ahw = adapter->ahw;
 
 	if (!test_bit(__QLCNIC_MAINTENANCE_MODE, &adapter->state)) {
 		/* Get port configuration info */
@@ -3229,20 +3277,41 @@ int qlcnic_83xx_get_settings(struct qlcnic_adapter *adapter,
 		ecmd->autoneg = AUTONEG_DISABLE;
 	}
 
-	if (ahw->port_type == QLCNIC_XGBE) {
-		ecmd->supported = SUPPORTED_10000baseT_Full;
-		ecmd->advertising = ADVERTISED_10000baseT_Full;
+	ecmd->supported = (SUPPORTED_10baseT_Full |
+			   SUPPORTED_100baseT_Full |
+			   SUPPORTED_1000baseT_Full |
+			   SUPPORTED_10000baseT_Full |
+			   SUPPORTED_Autoneg);
+
+	if (ecmd->autoneg == AUTONEG_ENABLE) {
+		if (ahw->port_config & QLC_83XX_10_CAPABLE)
+			ecmd->advertising |= SUPPORTED_10baseT_Full;
+		if (ahw->port_config & QLC_83XX_100_CAPABLE)
+			ecmd->advertising |= SUPPORTED_100baseT_Full;
+		if (ahw->port_config & QLC_83XX_1G_CAPABLE)
+			ecmd->advertising |= SUPPORTED_1000baseT_Full;
+		if (ahw->port_config & QLC_83XX_10G_CAPABLE)
+			ecmd->advertising |= SUPPORTED_10000baseT_Full;
+		if (ahw->port_config & QLC_83XX_AUTONEG_ENABLE)
+			ecmd->advertising |= ADVERTISED_Autoneg;
 	} else {
-		ecmd->supported = (SUPPORTED_10baseT_Half |
-				   SUPPORTED_10baseT_Full |
-				   SUPPORTED_100baseT_Half |
-				   SUPPORTED_100baseT_Full |
-				   SUPPORTED_1000baseT_Half |
-				   SUPPORTED_1000baseT_Full);
-		ecmd->advertising = (ADVERTISED_100baseT_Half |
-				     ADVERTISED_100baseT_Full |
-				     ADVERTISED_1000baseT_Half |
-				     ADVERTISED_1000baseT_Full);
+		switch (ahw->link_speed) {
+		case SPEED_10:
+			ecmd->advertising = SUPPORTED_10baseT_Full;
+			break;
+		case SPEED_100:
+			ecmd->advertising = SUPPORTED_100baseT_Full;
+			break;
+		case SPEED_1000:
+			ecmd->advertising = SUPPORTED_1000baseT_Full;
+			break;
+		case SPEED_10000:
+			ecmd->advertising = SUPPORTED_10000baseT_Full;
+			break;
+		default:
+			break;
+		}
+
 	}
 
 	switch (ahw->supported_type) {
@@ -3258,6 +3327,12 @@ int qlcnic_83xx_get_settings(struct qlcnic_adapter *adapter,
 		ecmd->port = PORT_TP;
 		ecmd->transceiver = XCVR_INTERNAL;
 		break;
+	case PORT_DA:
+		ecmd->supported |= SUPPORTED_FIBRE;
+		ecmd->advertising |= ADVERTISED_FIBRE;
+		ecmd->port = PORT_DA;
+		ecmd->transceiver = XCVR_EXTERNAL;
+		break;
 	default:
 		ecmd->supported |= SUPPORTED_FIBRE;
 		ecmd->advertising |= ADVERTISED_FIBRE;
@@ -3272,35 +3347,60 @@ int qlcnic_83xx_get_settings(struct qlcnic_adapter *adapter,
 int qlcnic_83xx_set_settings(struct qlcnic_adapter *adapter,
 			     struct ethtool_cmd *ecmd)
 {
-	int status = 0;
+	struct qlcnic_hardware_context *ahw = adapter->ahw;
 	u32 config = adapter->ahw->port_config;
+	int status = 0;
 
-	if (ecmd->autoneg)
-		adapter->ahw->port_config |= BIT_15;
-
-	switch (ethtool_cmd_speed(ecmd)) {
-	case SPEED_10:
-		adapter->ahw->port_config |= BIT_8;
-		break;
-	case SPEED_100:
-		adapter->ahw->port_config |= BIT_9;
-		break;
-	case SPEED_1000:
-		adapter->ahw->port_config |= BIT_10;
-		break;
-	case SPEED_10000:
-		adapter->ahw->port_config |= BIT_11;
-		break;
-	default:
-		return -EINVAL;
+	/* 83xx devices do not support Half duplex */
+	if (ecmd->duplex == DUPLEX_HALF) {
+			netdev_info(adapter->netdev,
+				    "Half duplex mode not supported\n");
+			return -EINVAL;
 	}
 
+	if (ecmd->autoneg) {
+		ahw->port_config |= QLC_83XX_AUTONEG_ENABLE;
+		ahw->port_config |= (QLC_83XX_100_CAPABLE |
+				     QLC_83XX_1G_CAPABLE |
+				     QLC_83XX_10G_CAPABLE);
+	} else { /* force speed */
+		ahw->port_config &= ~QLC_83XX_AUTONEG_ENABLE;
+		switch (ethtool_cmd_speed(ecmd)) {
+		case SPEED_10:
+			ahw->port_config &= ~(QLC_83XX_100_CAPABLE |
+					      QLC_83XX_1G_CAPABLE |
+					      QLC_83XX_10G_CAPABLE);
+			ahw->port_config |= QLC_83XX_10_CAPABLE;
+			break;
+		case SPEED_100:
+			ahw->port_config &= ~(QLC_83XX_10_CAPABLE |
+					      QLC_83XX_1G_CAPABLE |
+					      QLC_83XX_10G_CAPABLE);
+			ahw->port_config |= QLC_83XX_100_CAPABLE;
+			break;
+		case SPEED_1000:
+			ahw->port_config &= ~(QLC_83XX_10_CAPABLE |
+					      QLC_83XX_100_CAPABLE |
+					      QLC_83XX_10G_CAPABLE);
+			ahw->port_config |= QLC_83XX_1G_CAPABLE;
+			break;
+		case SPEED_10000:
+			ahw->port_config &= ~(QLC_83XX_10_CAPABLE |
+					      QLC_83XX_100_CAPABLE |
+					      QLC_83XX_1G_CAPABLE);
+			ahw->port_config |= QLC_83XX_10G_CAPABLE;
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
 	status = qlcnic_83xx_set_port_config(adapter);
 	if (status) {
-		dev_info(&adapter->pdev->dev,
-			 "Failed to Set Link Speed and autoneg.\n");
-		adapter->ahw->port_config = config;
+		netdev_info(adapter->netdev,
+			    "Failed to Set Link Speed and autoneg.\n");
+		ahw->port_config = config;
 	}
+
 	return status;
 }
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
index 2bf101a47d02..f3346a3779d3 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_hw.h
@@ -83,6 +83,7 @@
 /* Firmware image definitions */
 #define QLC_83XX_BOOTLOADER_FLASH_ADDR	0x10000
 #define QLC_83XX_FW_FILE_NAME		"83xx_fw.bin"
+#define QLC_83XX_POST_FW_FILE_NAME	"83xx_post_fw.bin"
 #define QLC_84XX_FW_FILE_NAME		"84xx_fw.bin"
 #define QLC_83XX_BOOT_FROM_FLASH	0
 #define QLC_83XX_BOOT_FROM_FILE		0x12345678
@@ -360,7 +361,6 @@ enum qlcnic_83xx_states {
 #define QLC_83XX_SFP_MODULE_TYPE(data)		(((data) >> 4) & 0x1F)
 #define QLC_83XX_SFP_CU_LENGTH(data)		(LSB((data) >> 16))
 #define QLC_83XX_SFP_TX_FAULT(data)		((data) & BIT_10)
-#define QLC_83XX_SFP_10G_CAPABLE(data)		((data) & BIT_11)
 #define QLC_83XX_LINK_STATS(data)		((data) & BIT_0)
 #define QLC_83XX_CURRENT_LINK_SPEED(data)	(((data) >> 3) & 7)
 #define QLC_83XX_LINK_PAUSE(data)		(((data) >> 6) & 3)
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
index f33559b72528..2bb48d57e7a5 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_83xx_init.c
@@ -1177,9 +1177,8 @@ static void qlcnic_83xx_setup_idc_parameters(struct qlcnic_adapter *adapter)
 {
 	u32 idc_params, val;
 
-	if (qlcnic_83xx_lockless_flash_read32(adapter,
-					      QLC_83XX_IDC_FLASH_PARAM_ADDR,
-					      (u8 *)&idc_params, 1)) {
+	if (qlcnic_83xx_flash_read32(adapter, QLC_83XX_IDC_FLASH_PARAM_ADDR,
+				     (u8 *)&idc_params, 1)) {
 		dev_info(&adapter->pdev->dev,
 			 "%s:failed to get IDC params from flash\n", __func__);
 		adapter->dev_init_timeo = QLC_83XX_IDC_INIT_TIMEOUT_SECS;
@@ -1378,31 +1377,45 @@ static int qlcnic_83xx_copy_fw_file(struct qlcnic_adapter *adapter)
 {
 	struct qlc_83xx_fw_info *fw_info = adapter->ahw->fw_info;
 	const struct firmware *fw = fw_info->fw;
-	u32 dest, *p_cache;
+	u32 dest, *p_cache, *temp;
 	int i, ret = -EIO;
+	__le32 *temp_le;
 	u8 data[16];
 	size_t size;
 	u64 addr;
 
+	temp = kzalloc(fw->size, GFP_KERNEL);
+	if (!temp) {
+		release_firmware(fw);
+		fw_info->fw = NULL;
+		return -ENOMEM;
+	}
+
+	temp_le = (__le32 *)fw->data;
+
+	/* FW image in file is in little endian, swap the data to nullify
+	 * the effect of writel() operation on big endian platform.
+	 */
+	for (i = 0; i < fw->size / sizeof(u32); i++)
+		temp[i] = __le32_to_cpu(temp_le[i]);
+
 	dest = QLCRDX(adapter->ahw, QLCNIC_FW_IMAGE_ADDR);
 	size = (fw->size & ~0xF);
-	p_cache = (u32 *)fw->data;
+	p_cache = temp;
 	addr = (u64)dest;
 
 	ret = qlcnic_ms_mem_write128(adapter, addr,
 				     p_cache, size / 16);
 	if (ret) {
 		dev_err(&adapter->pdev->dev, "MS memory write failed\n");
-		release_firmware(fw);
-		fw_info->fw = NULL;
-		return -EIO;
+		goto exit;
 	}
 
 	/* alignment check */
 	if (fw->size & 0xF) {
 		addr = dest + size;
 		for (i = 0; i < (fw->size & 0xF); i++)
-			data[i] = fw->data[size + i];
+			data[i] = temp[size + i];
 		for (; i < 16; i++)
 			data[i] = 0;
 		ret = qlcnic_ms_mem_write128(adapter, addr,
@@ -1410,15 +1423,16 @@ static int qlcnic_83xx_copy_fw_file(struct qlcnic_adapter *adapter)
 		if (ret) {
 			dev_err(&adapter->pdev->dev,
 				"MS memory write failed\n");
-			release_firmware(fw);
-			fw_info->fw = NULL;
-			return -EIO;
+			goto exit;
 		}
 	}
+
+exit:
 	release_firmware(fw);
 	fw_info->fw = NULL;
+	kfree(temp);
 
-	return 0;
+	return ret;
 }
 
 static void qlcnic_83xx_dump_pause_control_regs(struct qlcnic_adapter *adapter)
@@ -2060,6 +2074,121 @@ static void qlcnic_83xx_init_hw(struct qlcnic_adapter *p_dev)
 		dev_err(&p_dev->pdev->dev, "%s: failed\n", __func__);
 }
 
+/* POST FW related definations*/
+#define QLC_83XX_POST_SIGNATURE_REG	0x41602014
+#define QLC_83XX_POST_MODE_REG		0x41602018
+#define QLC_83XX_POST_FAST_MODE		0
+#define QLC_83XX_POST_MEDIUM_MODE	1
+#define QLC_83XX_POST_SLOW_MODE		2
+
+/* POST Timeout values in milliseconds */
+#define QLC_83XX_POST_FAST_MODE_TIMEOUT	690
+#define QLC_83XX_POST_MED_MODE_TIMEOUT	2930
+#define QLC_83XX_POST_SLOW_MODE_TIMEOUT	7500
+
+/* POST result values */
+#define QLC_83XX_POST_PASS			0xfffffff0
+#define QLC_83XX_POST_ASIC_STRESS_TEST_FAIL	0xffffffff
+#define QLC_83XX_POST_DDR_TEST_FAIL		0xfffffffe
+#define QLC_83XX_POST_ASIC_MEMORY_TEST_FAIL	0xfffffffc
+#define QLC_83XX_POST_FLASH_TEST_FAIL		0xfffffff8
+
+static int qlcnic_83xx_run_post(struct qlcnic_adapter *adapter)
+{
+	struct qlc_83xx_fw_info *fw_info = adapter->ahw->fw_info;
+	struct device *dev = &adapter->pdev->dev;
+	int timeout, count, ret = 0;
+	u32 signature;
+
+	/* Set timeout values with extra 2 seconds of buffer */
+	switch (adapter->ahw->post_mode) {
+	case QLC_83XX_POST_FAST_MODE:
+		timeout = QLC_83XX_POST_FAST_MODE_TIMEOUT + 2000;
+		break;
+	case QLC_83XX_POST_MEDIUM_MODE:
+		timeout = QLC_83XX_POST_MED_MODE_TIMEOUT + 2000;
+		break;
+	case QLC_83XX_POST_SLOW_MODE:
+		timeout = QLC_83XX_POST_SLOW_MODE_TIMEOUT + 2000;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	strncpy(fw_info->fw_file_name, QLC_83XX_POST_FW_FILE_NAME,
+		QLC_FW_FILE_NAME_LEN);
+
+	ret = request_firmware(&fw_info->fw, fw_info->fw_file_name, dev);
+	if (ret) {
+		dev_err(dev, "POST firmware can not be loaded, skipping POST\n");
+		return 0;
+	}
+
+	ret = qlcnic_83xx_copy_fw_file(adapter);
+	if (ret)
+		return ret;
+
+	/* clear QLC_83XX_POST_SIGNATURE_REG register */
+	qlcnic_ind_wr(adapter, QLC_83XX_POST_SIGNATURE_REG, 0);
+
+	/* Set POST mode */
+	qlcnic_ind_wr(adapter, QLC_83XX_POST_MODE_REG,
+		      adapter->ahw->post_mode);
+
+	QLC_SHARED_REG_WR32(adapter, QLCNIC_FW_IMG_VALID,
+			    QLC_83XX_BOOT_FROM_FILE);
+
+	qlcnic_83xx_start_hw(adapter);
+
+	count = 0;
+	do {
+		msleep(100);
+		count += 100;
+
+		signature = qlcnic_ind_rd(adapter, QLC_83XX_POST_SIGNATURE_REG);
+		if (signature == QLC_83XX_POST_PASS)
+			break;
+	} while (timeout > count);
+
+	if (timeout <= count) {
+		dev_err(dev, "POST timed out, signature = 0x%08x\n", signature);
+		return -EIO;
+	}
+
+	switch (signature) {
+	case QLC_83XX_POST_PASS:
+		dev_info(dev, "POST passed, Signature = 0x%08x\n", signature);
+		break;
+	case QLC_83XX_POST_ASIC_STRESS_TEST_FAIL:
+		dev_err(dev, "POST failed, Test case : ASIC STRESS TEST, Signature = 0x%08x\n",
+			signature);
+		ret = -EIO;
+		break;
+	case QLC_83XX_POST_DDR_TEST_FAIL:
+		dev_err(dev, "POST failed, Test case : DDT TEST, Signature = 0x%08x\n",
+			signature);
+		ret = -EIO;
+		break;
+	case QLC_83XX_POST_ASIC_MEMORY_TEST_FAIL:
+		dev_err(dev, "POST failed, Test case : ASIC MEMORY TEST, Signature = 0x%08x\n",
+			signature);
+		ret = -EIO;
+		break;
+	case QLC_83XX_POST_FLASH_TEST_FAIL:
+		dev_err(dev, "POST failed, Test case : FLASH TEST, Signature = 0x%08x\n",
+			signature);
+		ret = -EIO;
+		break;
+	default:
+		dev_err(dev, "POST failed, Test case : INVALID, Signature = 0x%08x\n",
+			signature);
+		ret = -EIO;
+		break;
+	}
+
+	return ret;
+}
+
 static int qlcnic_83xx_load_fw_image_from_host(struct qlcnic_adapter *adapter)
 {
 	struct qlc_83xx_fw_info *fw_info = adapter->ahw->fw_info;
@@ -2104,8 +2233,27 @@ static int qlcnic_83xx_restart_hw(struct qlcnic_adapter *adapter)
 
 	if (qlcnic_83xx_copy_bootloader(adapter))
 		return err;
+
+	/* Check if POST needs to be run */
+	if (adapter->ahw->run_post) {
+		err = qlcnic_83xx_run_post(adapter);
+		if (err)
+			return err;
+
+		/* No need to run POST in next reset sequence */
+		adapter->ahw->run_post = false;
+
+		/* Again reset the adapter to load regular firmware  */
+		qlcnic_83xx_stop_hw(adapter);
+		qlcnic_83xx_init_hw(adapter);
+
+		err = qlcnic_83xx_copy_bootloader(adapter);
+		if (err)
+			return err;
+	}
+
 	/* Boot either flash image or firmware image from host file system */
-	if (qlcnic_load_fw_file) {
+	if (qlcnic_load_fw_file == 1) {
 		if (qlcnic_83xx_load_fw_image_from_host(adapter))
 			return err;
 	} else {
@@ -2269,6 +2417,7 @@ static int qlcnic_83xx_get_fw_info(struct qlcnic_adapter *adapter)
 		fw_info = ahw->fw_info;
 		switch (pdev->device) {
 		case PCI_DEVICE_ID_QLOGIC_QLE834X:
+		case PCI_DEVICE_ID_QLOGIC_QLE8830:
 			strncpy(fw_info->fw_file_name, QLC_83XX_FW_FILE_NAME,
 				QLC_FW_FILE_NAME_LEN);
 			break;
@@ -2313,6 +2462,25 @@ int qlcnic_83xx_init(struct qlcnic_adapter *adapter, int pci_using_dac)
 	adapter->rx_mac_learn = false;
 	ahw->msix_supported = !!qlcnic_use_msi_x;
 
+	/* Check if POST needs to be run */
+	switch (qlcnic_load_fw_file) {
+	case 2:
+		ahw->post_mode = QLC_83XX_POST_FAST_MODE;
+		ahw->run_post = true;
+		break;
+	case 3:
+		ahw->post_mode = QLC_83XX_POST_MEDIUM_MODE;
+		ahw->run_post = true;
+		break;
+	case 4:
+		ahw->post_mode = QLC_83XX_POST_SLOW_MODE;
+		ahw->run_post = true;
+		break;
+	default:
+		ahw->run_post = false;
+		break;
+	}
+
 	qlcnic_83xx_init_rings(adapter);
 
 	err = qlcnic_83xx_init_mailbox_work(adapter);
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
index ffbae293cef5..6e6f18fc5d76 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ctx.c
@@ -11,7 +11,7 @@ static const struct qlcnic_mailbox_metadata qlcnic_mbx_tbl[] = {
 	{QLCNIC_CMD_CREATE_RX_CTX, 4, 1},
 	{QLCNIC_CMD_DESTROY_RX_CTX, 2, 1},
 	{QLCNIC_CMD_CREATE_TX_CTX, 4, 1},
-	{QLCNIC_CMD_DESTROY_TX_CTX, 2, 1},
+	{QLCNIC_CMD_DESTROY_TX_CTX, 3, 1},
 	{QLCNIC_CMD_INTRPT_TEST, 4, 1},
 	{QLCNIC_CMD_SET_MTU, 4, 1},
 	{QLCNIC_CMD_READ_PHY, 4, 2},
@@ -32,7 +32,7 @@ static const struct qlcnic_mailbox_metadata qlcnic_mbx_tbl[] = {
 	{QLCNIC_CMD_CONFIGURE_ESWITCH, 4, 1},
 	{QLCNIC_CMD_GET_MAC_STATS, 4, 1},
 	{QLCNIC_CMD_GET_ESWITCH_PORT_CONFIG, 4, 3},
-	{QLCNIC_CMD_GET_ESWITCH_STATS, 5, 1},
+	{QLCNIC_CMD_GET_ESWITCH_STATS, 4, 1},
 	{QLCNIC_CMD_CONFIG_PORT, 4, 1},
 	{QLCNIC_CMD_TEMP_SIZE, 4, 4},
 	{QLCNIC_CMD_GET_TEMP_HDR, 4, 1},
@@ -129,7 +129,7 @@ int qlcnic_82xx_issue_cmd(struct qlcnic_adapter *adapter,
 	}
 
 	QLCWR32(adapter, QLCNIC_SIGN_CRB_OFFSET, signature);
-	for (i = 1; i < QLCNIC_CDRP_MAX_ARGS; i++)
+	for (i = 1; i < cmd->req.num; i++)
 		QLCWR32(adapter, QLCNIC_CDRP_ARG(i), cmd->req.arg[i]);
 	QLCWR32(adapter, QLCNIC_CDRP_CRB_OFFSET,
 		QLCNIC_CDRP_FORM_CMD(cmd->req.arg[0]));
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
index 141f116eb868..494e8105adee 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_ethtool.c
@@ -1333,21 +1333,21 @@ static void qlcnic_get_ethtool_stats(struct net_device *dev,
 	struct qlcnic_host_tx_ring *tx_ring;
 	struct qlcnic_esw_statistics port_stats;
 	struct qlcnic_mac_statistics mac_stats;
-	int index, ret, length, size, tx_size, ring;
+	int index, ret, length, size, ring;
 	char *p;
 
-	tx_size = adapter->drv_tx_rings * QLCNIC_TX_STATS_LEN;
+	memset(data, 0, stats->n_stats * sizeof(u64));
 
-	memset(data, 0, tx_size * sizeof(u64));
 	for (ring = 0, index = 0; ring < adapter->drv_tx_rings; ring++) {
-		if (test_bit(__QLCNIC_DEV_UP, &adapter->state)) {
+		if (adapter->is_up == QLCNIC_ADAPTER_UP_MAGIC) {
 			tx_ring = &adapter->tx_ring[ring];
 			data = qlcnic_fill_tx_queue_stats(data, tx_ring);
 			qlcnic_update_stats(adapter);
+		} else {
+			data += QLCNIC_TX_STATS_LEN;
 		}
 	}
 
-	memset(data, 0, stats->n_stats * sizeof(u64));
 	length = QLCNIC_STATS_LEN;
 	for (index = 0; index < length; index++) {
 		p = (char *)adapter + qlcnic_gstrings_stats[index].stat_offset;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
index 851cb4a80d50..69b46c051cc0 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_hw.c
@@ -341,7 +341,7 @@ qlcnic_pcie_sem_lock(struct qlcnic_adapter *adapter, int sem, u32 id_reg)
 			}
 			return -EIO;
 		}
-		msleep(1);
+		usleep_range(1000, 1500);
 	}
 
 	if (id_reg)
@@ -941,7 +941,7 @@ void qlcnic_82xx_config_ipaddr(struct qlcnic_adapter *adapter,
 	rv = qlcnic_send_cmd_descs(adapter, (struct cmd_desc_type0 *)&req, 1);
 	if (rv != 0)
 		dev_err(&adapter->netdev->dev,
-				"could not notify %s IP 0x%x reuqest\n",
+				"could not notify %s IP 0x%x request\n",
 				(cmd == QLCNIC_IP_UP) ? "Add" : "Remove", ip);
 }
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
index c4262c23ed7c..be41e4c77b65 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_init.c
@@ -537,7 +537,7 @@ int qlcnic_pinit_from_rom(struct qlcnic_adapter *adapter)
 	QLCWR32(adapter, QLCNIC_CRB_PEG_NET_3 + 0xc, 0);
 	QLCWR32(adapter, QLCNIC_CRB_PEG_NET_4 + 0x8, 0);
 	QLCWR32(adapter, QLCNIC_CRB_PEG_NET_4 + 0xc, 0);
-	msleep(1);
+	usleep_range(1000, 1500);
 
 	QLC_SHARED_REG_WR32(adapter, QLCNIC_PEG_HALT_STATUS1, 0);
 	QLC_SHARED_REG_WR32(adapter, QLCNIC_PEG_HALT_STATUS2, 0);
@@ -1198,7 +1198,7 @@ qlcnic_load_firmware(struct qlcnic_adapter *adapter)
 			flashaddr += 8;
 		}
 	}
-	msleep(1);
+	usleep_range(1000, 1500);
 
 	QLCWR32(adapter, QLCNIC_CRB_PEG_NET_0 + 0x18, 0x1020);
 	QLCWR32(adapter, QLCNIC_ROMUSB_GLB_SW_RESET, 0x80001e);
@@ -1295,7 +1295,7 @@ next:
 		rc = qlcnic_validate_firmware(adapter);
 		if (rc != 0) {
 			release_firmware(adapter->fw);
-			msleep(1);
+			usleep_range(1000, 1500);
 			goto next;
 		}
 	}
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
index e45bf09af0c9..18e5de72e9b4 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_io.c
@@ -1753,7 +1753,7 @@ qlcnic_83xx_process_rcv(struct qlcnic_adapter *adapter,
 
 	if (qlcnic_encap_length(sts_data[1]) &&
 	    skb->ip_summed == CHECKSUM_UNNECESSARY) {
-		skb->encapsulation = 1;
+		skb->csum_level = 1;
 		adapter->stats.encap_rx_csummed++;
 	}
 
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
index cf08b2de071e..f5e29f7bdae3 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_main.c
@@ -52,7 +52,7 @@ MODULE_PARM_DESC(auto_fw_reset, "Auto firmware reset (0=disabled, 1=enabled)");
 module_param_named(auto_fw_reset, qlcnic_auto_fw_reset, int, 0644);
 
 int qlcnic_load_fw_file;
-MODULE_PARM_DESC(load_fw_file, "Load firmware from (0=flash, 1=file)");
+MODULE_PARM_DESC(load_fw_file, "Load firmware from (0=flash, 1=file, 2=POST in fast mode, 3= POST in medium mode, 4=POST in slow mode)");
 module_param_named(load_fw_file, qlcnic_load_fw_file, int, 0444);
 
 static int qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent);
@@ -111,6 +111,7 @@ static u32 qlcnic_vlan_tx_check(struct qlcnic_adapter *adapter)
 static const struct pci_device_id qlcnic_pci_tbl[] = {
 	ENTRY(PCI_DEVICE_ID_QLOGIC_QLE824X),
 	ENTRY(PCI_DEVICE_ID_QLOGIC_QLE834X),
+	ENTRY(PCI_DEVICE_ID_QLOGIC_QLE8830),
 	ENTRY(PCI_DEVICE_ID_QLOGIC_VF_QLE834X),
 	ENTRY(PCI_DEVICE_ID_QLOGIC_QLE844X),
 	ENTRY(PCI_DEVICE_ID_QLOGIC_VF_QLE844X),
@@ -228,6 +229,11 @@ static const struct qlcnic_board_info qlcnic_boards[] = {
 	  PCI_DEVICE_ID_QLOGIC_QLE834X,
 	  0x0, 0x0, "8300 Series 1/10GbE Controller" },
 	{ PCI_VENDOR_ID_QLOGIC,
+	  PCI_DEVICE_ID_QLOGIC_QLE8830,
+	  0x0,
+	  0x0,
+	  "8830 Series 1/10GbE Controller" },
+	{ PCI_VENDOR_ID_QLOGIC,
 	  PCI_DEVICE_ID_QLOGIC_QLE824X,
 	  PCI_VENDOR_ID_QLOGIC,
 	  0x203,
@@ -1131,6 +1137,7 @@ static void qlcnic_get_bar_length(u32 dev_id, ulong *bar)
 		*bar = QLCNIC_82XX_BAR0_LENGTH;
 		break;
 	case PCI_DEVICE_ID_QLOGIC_QLE834X:
+	case PCI_DEVICE_ID_QLOGIC_QLE8830:
 	case PCI_DEVICE_ID_QLOGIC_QLE844X:
 	case PCI_DEVICE_ID_QLOGIC_VF_QLE834X:
 	case PCI_DEVICE_ID_QLOGIC_VF_QLE844X:
@@ -2474,6 +2481,7 @@ qlcnic_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		ahw->reg_tbl = (u32 *) qlcnic_reg_tbl;
 		break;
 	case PCI_DEVICE_ID_QLOGIC_QLE834X:
+	case PCI_DEVICE_ID_QLOGIC_QLE8830:
 	case PCI_DEVICE_ID_QLOGIC_QLE844X:
 		qlcnic_83xx_register_map(ahw);
 		break;
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c
index e46fc39d425d..c9f57fb84b9e 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_minidump.c
@@ -47,15 +47,26 @@ struct qlcnic_common_entry_hdr {
 	u32     type;
 	u32     offset;
 	u32     cap_size;
+#if defined(__LITTLE_ENDIAN)
 	u8      mask;
 	u8      rsvd[2];
 	u8      flags;
+#else
+	u8      flags;
+	u8      rsvd[2];
+	u8      mask;
+#endif
 } __packed;
 
 struct __crb {
 	u32	addr;
+#if defined(__LITTLE_ENDIAN)
 	u8	stride;
 	u8	rsvd1[3];
+#else
+	u8	rsvd1[3];
+	u8	stride;
+#endif
 	u32	data_size;
 	u32	no_ops;
 	u32	rsvd2[4];
@@ -63,15 +74,28 @@ struct __crb {
 
 struct __ctrl {
 	u32	addr;
+#if defined(__LITTLE_ENDIAN)
 	u8	stride;
 	u8	index_a;
 	u16	timeout;
+#else
+	u16	timeout;
+	u8	index_a;
+	u8	stride;
+#endif
 	u32	data_size;
 	u32	no_ops;
+#if defined(__LITTLE_ENDIAN)
 	u8	opcode;
 	u8	index_v;
 	u8	shl_val;
 	u8	shr_val;
+#else
+	u8	shr_val;
+	u8	shl_val;
+	u8	index_v;
+	u8	opcode;
+#endif
 	u32	val1;
 	u32	val2;
 	u32	val3;
@@ -79,16 +103,27 @@ struct __ctrl {
 
 struct __cache {
 	u32	addr;
+#if defined(__LITTLE_ENDIAN)
 	u16	stride;
 	u16	init_tag_val;
+#else
+	u16	init_tag_val;
+	u16	stride;
+#endif
 	u32	size;
 	u32	no_ops;
 	u32	ctrl_addr;
 	u32	ctrl_val;
 	u32	read_addr;
+#if defined(__LITTLE_ENDIAN)
 	u8	read_addr_stride;
 	u8	read_addr_num;
 	u8	rsvd1[2];
+#else
+	u8	rsvd1[2];
+	u8	read_addr_num;
+	u8	read_addr_stride;
+#endif
 } __packed;
 
 struct __ocm {
@@ -122,23 +157,39 @@ struct __mux {
 
 struct __queue {
 	u32	sel_addr;
+#if defined(__LITTLE_ENDIAN)
 	u16	stride;
 	u8	rsvd[2];
+#else
+	u8	rsvd[2];
+	u16	stride;
+#endif
 	u32	size;
 	u32	no_ops;
 	u8	rsvd2[8];
 	u32	read_addr;
+#if defined(__LITTLE_ENDIAN)
 	u8	read_addr_stride;
 	u8	read_addr_cnt;
 	u8	rsvd3[2];
+#else
+	u8	rsvd3[2];
+	u8	read_addr_cnt;
+	u8	read_addr_stride;
+#endif
 } __packed;
 
 struct __pollrd {
 	u32	sel_addr;
 	u32	read_addr;
 	u32	sel_val;
+#if defined(__LITTLE_ENDIAN)
 	u16	sel_val_stride;
 	u16	no_ops;
+#else
+	u16	no_ops;
+	u16	sel_val_stride;
+#endif
 	u32	poll_wait;
 	u32	poll_mask;
 	u32	data_size;
@@ -153,9 +204,15 @@ struct __mux2 {
 	u32	no_ops;
 	u32	sel_val_mask;
 	u32	read_addr;
+#if defined(__LITTLE_ENDIAN)
 	u8	sel_val_stride;
 	u8	data_size;
 	u8	rsvd[2];
+#else
+	u8	rsvd[2];
+	u8	data_size;
+	u8	sel_val_stride;
+#endif
 } __packed;
 
 struct __pollrdmwr {
diff --git a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
index f5786d5792df..59a721fba018 100644
--- a/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
+++ b/drivers/net/ethernet/qlogic/qlcnic/qlcnic_sysfs.c
@@ -280,6 +280,7 @@ static ssize_t qlcnic_sysfs_read_crb(struct file *filp, struct kobject *kobj,
 	if (ret != 0)
 		return ret;
 	qlcnic_read_crb(adapter, buf, offset, size);
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 
 	return size;
 }
@@ -296,6 +297,7 @@ static ssize_t qlcnic_sysfs_write_crb(struct file *filp, struct kobject *kobj,
 	if (ret != 0)
 		return ret;
 
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	qlcnic_write_crb(adapter, buf, offset, size);
 	return size;
 }
@@ -329,6 +331,7 @@ static ssize_t qlcnic_sysfs_read_mem(struct file *filp, struct kobject *kobj,
 		return -EIO;
 
 	memcpy(buf, &data, size);
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 
 	return size;
 }
@@ -346,6 +349,7 @@ static ssize_t qlcnic_sysfs_write_mem(struct file *filp, struct kobject *kobj,
 	if (ret != 0)
 		return ret;
 
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	memcpy(&data, buf, size);
 
 	if (qlcnic_pci_mem_write_2M(adapter, offset, data))
@@ -412,6 +416,7 @@ static ssize_t qlcnic_sysfs_write_pm_config(struct file *filp,
 	if (rem)
 		return QL_STATUS_INVALID_PARAM;
 
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	pm_cfg = (struct qlcnic_pm_func_cfg *)buf;
 	ret = validate_pm_config(adapter, pm_cfg, count);
 
@@ -474,6 +479,7 @@ static ssize_t qlcnic_sysfs_read_pm_config(struct file *filp,
 		pm_cfg[pci_func].dest_npar = 0;
 		pm_cfg[pci_func].pci_func = i;
 	}
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	return size;
 }
 
@@ -555,6 +561,7 @@ static ssize_t qlcnic_sysfs_write_esw_config(struct file *file,
 	if (rem)
 		return QL_STATUS_INVALID_PARAM;
 
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	esw_cfg = (struct qlcnic_esw_func_cfg *)buf;
 	ret = validate_esw_config(adapter, esw_cfg, count);
 	if (ret)
@@ -649,6 +656,7 @@ static ssize_t qlcnic_sysfs_read_esw_config(struct file *file,
 		if (qlcnic_get_eswitch_port_config(adapter, &esw_cfg[pci_func]))
 			return QL_STATUS_INVALID_PARAM;
 	}
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	return size;
 }
 
@@ -688,6 +696,7 @@ static ssize_t qlcnic_sysfs_write_npar_config(struct file *file,
 	if (rem)
 		return QL_STATUS_INVALID_PARAM;
 
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	np_cfg = (struct qlcnic_npar_func_cfg *)buf;
 	ret = validate_npar_config(adapter, np_cfg, count);
 	if (ret)
@@ -759,6 +768,7 @@ static ssize_t qlcnic_sysfs_read_npar_config(struct file *file,
 		np_cfg[pci_func].max_tx_queues = nic_info.max_tx_ques;
 		np_cfg[pci_func].max_rx_queues = nic_info.max_rx_ques;
 	}
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	return size;
 }
 
@@ -916,6 +926,7 @@ static ssize_t qlcnic_sysfs_read_pci_config(struct file *file,
 
 	pci_cfg = (struct qlcnic_pci_func_cfg *)buf;
 	count = size / sizeof(struct qlcnic_pci_func_cfg);
+	qlcnic_swap32_buffer((u32 *)pci_info, size / sizeof(u32));
 	for (i = 0; i < count; i++) {
 		pci_cfg[i].pci_func = pci_info[i].id;
 		pci_cfg[i].func_type = pci_info[i].type;
@@ -969,6 +980,7 @@ static ssize_t qlcnic_83xx_sysfs_flash_read_handler(struct file *filp,
 	}
 
 	qlcnic_83xx_unlock_flash(adapter);
+	qlcnic_swap32_buffer((u32 *)p_read_buf, count);
 	memcpy(buf, p_read_buf, size);
 	kfree(p_read_buf);
 
@@ -986,9 +998,10 @@ static int qlcnic_83xx_sysfs_flash_bulk_write(struct qlcnic_adapter *adapter,
 	if (!p_cache)
 		return -ENOMEM;
 
+	count = size / sizeof(u32);
+	qlcnic_swap32_buffer((u32 *)buf, count);
 	memcpy(p_cache, buf, size);
 	p_src = p_cache;
-	count = size / sizeof(u32);
 
 	if (qlcnic_83xx_lock_flash(adapter) != 0) {
 		kfree(p_cache);
@@ -1053,6 +1066,7 @@ static int qlcnic_83xx_sysfs_flash_write(struct qlcnic_adapter *adapter,
 	if (!p_cache)
 		return -ENOMEM;
 
+	qlcnic_swap32_buffer((u32 *)buf, size / sizeof(u32));
 	memcpy(p_cache, buf, size);
 	p_src = p_cache;
 	count = size / sizeof(u32);
diff --git a/drivers/net/ethernet/qlogic/qlge/qlge_main.c b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
index 188626e2a861..6c904a6cad2a 100644
--- a/drivers/net/ethernet/qlogic/qlge/qlge_main.c
+++ b/drivers/net/ethernet/qlogic/qlge/qlge_main.c
@@ -1922,7 +1922,7 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 			sbq_desc->p.skb = NULL;
 			skb_reserve(skb, NET_IP_ALIGN);
 		}
-		while (length > 0) {
+		do {
 			lbq_desc = ql_get_curr_lchunk(qdev, rx_ring);
 			size = (length < rx_ring->lbq_buf_size) ? length :
 				rx_ring->lbq_buf_size;
@@ -1939,7 +1939,7 @@ static struct sk_buff *ql_build_rx_skb(struct ql_adapter *qdev,
 			skb->truesize += size;
 			length -= size;
 			i++;
-		}
+		} while (length > 0);
 		ql_update_mac_hdr_len(qdev, ib_mac_rsp, lbq_desc->p.pg_chunk.va,
 				      &hlen);
 		__pskb_pull_tail(skb, hlen);
@@ -2556,6 +2556,7 @@ static int ql_tso(struct sk_buff *skb, struct ob_mac_tso_iocb_req *mac_iocb_ptr)
 
 	if (skb_is_gso(skb)) {
 		int err;
+		__be16 l3_proto = vlan_get_protocol(skb);
 
 		err = skb_cow_head(skb, 0);
 		if (err < 0)
@@ -2572,7 +2573,7 @@ static int ql_tso(struct sk_buff *skb, struct ob_mac_tso_iocb_req *mac_iocb_ptr)
 				<< OB_MAC_TRANSPORT_HDR_SHIFT);
 		mac_iocb_ptr->mss = cpu_to_le16(skb_shinfo(skb)->gso_size);
 		mac_iocb_ptr->flags2 |= OB_MAC_TSO_IOCB_LSO;
-		if (likely(skb->protocol == htons(ETH_P_IP))) {
+		if (likely(l3_proto == htons(ETH_P_IP))) {
 			struct iphdr *iph = ip_hdr(skb);
 			iph->check = 0;
 			mac_iocb_ptr->flags1 |= OB_MAC_TSO_IOCB_IP4;
@@ -2580,7 +2581,7 @@ static int ql_tso(struct sk_buff *skb, struct ob_mac_tso_iocb_req *mac_iocb_ptr)
 								 iph->daddr, 0,
 								 IPPROTO_TCP,
 								 0);
-		} else if (skb->protocol == htons(ETH_P_IPV6)) {
+		} else if (l3_proto == htons(ETH_P_IPV6)) {
 			mac_iocb_ptr->flags1 |= OB_MAC_TSO_IOCB_IP6;
 			tcp_hdr(skb)->check =
 			    ~csum_ipv6_magic(&ipv6_hdr(skb)->saddr,
diff --git a/drivers/net/ethernet/qualcomm/Kconfig b/drivers/net/ethernet/qualcomm/Kconfig
new file mode 100644
index 000000000000..f3a47147937d
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/Kconfig
@@ -0,0 +1,30 @@
+#
+# Qualcomm network device configuration
+#
+
+config NET_VENDOR_QUALCOMM
+	bool "Qualcomm devices"
+	default y
+	depends on SPI_MASTER && OF_GPIO
+	---help---
+	  If you have a network (Ethernet) card belonging to this class, say Y
+	  and read the Ethernet-HOWTO, available from
+	  <http://www.tldp.org/docs.html#howto>.
+
+	  Note that the answer to this question doesn't directly affect the
+	  kernel: saying N will just cause the configurator to skip all
+	  the questions about Qualcomm cards. If you say Y, you will be asked
+	  for your specific card in the following questions.
+
+if NET_VENDOR_QUALCOMM
+
+config QCA7000
+	tristate "Qualcomm Atheros QCA7000 support"
+	depends on SPI_MASTER && OF_GPIO
+	---help---
+	  This SPI protocol driver supports the Qualcomm Atheros QCA7000.
+
+	  To compile this driver as a module, choose M here. The module
+	  will be called qcaspi.
+
+endif # NET_VENDOR_QUALCOMM
diff --git a/drivers/net/ethernet/qualcomm/Makefile b/drivers/net/ethernet/qualcomm/Makefile
new file mode 100644
index 000000000000..9da2d75db700
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/Makefile
@@ -0,0 +1,6 @@
+#
+# Makefile for the Qualcomm network device drivers.
+#
+
+obj-$(CONFIG_QCA7000) += qcaspi.o
+qcaspi-objs := qca_spi.o qca_framing.o qca_7k.o qca_debug.o
diff --git a/drivers/net/ethernet/qualcomm/qca_7k.c b/drivers/net/ethernet/qualcomm/qca_7k.c
new file mode 100644
index 000000000000..f0066fbb44a6
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_7k.c
@@ -0,0 +1,149 @@
+/*
+ *
+ *   Copyright (c) 2011, 2012, Qualcomm Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+/*   This module implements the Qualcomm Atheros SPI protocol for
+ *   kernel-based SPI device.
+ */
+
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/spi/spi.h>
+#include <linux/version.h>
+
+#include "qca_7k.h"
+
+void
+qcaspi_spi_error(struct qcaspi *qca)
+{
+	if (qca->sync != QCASPI_SYNC_READY)
+		return;
+
+	netdev_err(qca->net_dev, "spi error\n");
+	qca->sync = QCASPI_SYNC_UNKNOWN;
+	qca->stats.spi_err++;
+}
+
+int
+qcaspi_read_register(struct qcaspi *qca, u16 reg, u16 *result)
+{
+	__be16 rx_data;
+	__be16 tx_data;
+	struct spi_transfer *transfer;
+	struct spi_message *msg;
+	int ret;
+
+	tx_data = cpu_to_be16(QCA7K_SPI_READ | QCA7K_SPI_INTERNAL | reg);
+
+	if (qca->legacy_mode) {
+		msg = &qca->spi_msg1;
+		transfer = &qca->spi_xfer1;
+		transfer->tx_buf = &tx_data;
+		transfer->rx_buf = NULL;
+		transfer->len = QCASPI_CMD_LEN;
+		spi_sync(qca->spi_dev, msg);
+	} else {
+		msg = &qca->spi_msg2;
+		transfer = &qca->spi_xfer2[0];
+		transfer->tx_buf = &tx_data;
+		transfer->rx_buf = NULL;
+		transfer->len = QCASPI_CMD_LEN;
+		transfer = &qca->spi_xfer2[1];
+	}
+	transfer->tx_buf = NULL;
+	transfer->rx_buf = &rx_data;
+	transfer->len = QCASPI_CMD_LEN;
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (!ret)
+		ret = msg->status;
+
+	if (ret)
+		qcaspi_spi_error(qca);
+	else
+		*result = be16_to_cpu(rx_data);
+
+	return ret;
+}
+
+int
+qcaspi_write_register(struct qcaspi *qca, u16 reg, u16 value)
+{
+	__be16 tx_data[2];
+	struct spi_transfer *transfer;
+	struct spi_message *msg;
+	int ret;
+
+	tx_data[0] = cpu_to_be16(QCA7K_SPI_WRITE | QCA7K_SPI_INTERNAL | reg);
+	tx_data[1] = cpu_to_be16(value);
+
+	if (qca->legacy_mode) {
+		msg = &qca->spi_msg1;
+		transfer = &qca->spi_xfer1;
+		transfer->tx_buf = &tx_data[0];
+		transfer->rx_buf = NULL;
+		transfer->len = QCASPI_CMD_LEN;
+		spi_sync(qca->spi_dev, msg);
+	} else {
+		msg = &qca->spi_msg2;
+		transfer = &qca->spi_xfer2[0];
+		transfer->tx_buf = &tx_data[0];
+		transfer->rx_buf = NULL;
+		transfer->len = QCASPI_CMD_LEN;
+		transfer = &qca->spi_xfer2[1];
+	}
+	transfer->tx_buf = &tx_data[1];
+	transfer->rx_buf = NULL;
+	transfer->len = QCASPI_CMD_LEN;
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (!ret)
+		ret = msg->status;
+
+	if (ret)
+		qcaspi_spi_error(qca);
+
+	return ret;
+}
+
+int
+qcaspi_tx_cmd(struct qcaspi *qca, u16 cmd)
+{
+	__be16 tx_data;
+	struct spi_message *msg = &qca->spi_msg1;
+	struct spi_transfer *transfer = &qca->spi_xfer1;
+	int ret;
+
+	tx_data = cpu_to_be16(cmd);
+	transfer->len = sizeof(tx_data);
+	transfer->tx_buf = &tx_data;
+	transfer->rx_buf = NULL;
+
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (!ret)
+		ret = msg->status;
+
+	if (ret)
+		qcaspi_spi_error(qca);
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/qualcomm/qca_7k.h b/drivers/net/ethernet/qualcomm/qca_7k.h
new file mode 100644
index 000000000000..1cad851ee507
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_7k.h
@@ -0,0 +1,72 @@
+/*
+ *   Copyright (c) 2011, 2012, Qualcomm Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ *
+ */
+
+/*   Qualcomm Atheros SPI register definition.
+ *
+ *   This module is designed to define the Qualcomm Atheros SPI
+ *   register placeholders.
+ */
+
+#ifndef _QCA_7K_H
+#define _QCA_7K_H
+
+#include <linux/types.h>
+
+#include "qca_spi.h"
+
+#define QCA7K_SPI_READ     (1 << 15)
+#define QCA7K_SPI_WRITE    (0 << 15)
+#define QCA7K_SPI_INTERNAL (1 << 14)
+#define QCA7K_SPI_EXTERNAL (0 << 14)
+
+#define QCASPI_CMD_LEN    2
+#define QCASPI_HW_PKT_LEN 4
+#define QCASPI_HW_BUF_LEN 0xC5B
+
+/*   SPI registers;                               */
+#define SPI_REG_BFR_SIZE        0x0100
+#define SPI_REG_WRBUF_SPC_AVA   0x0200
+#define SPI_REG_RDBUF_BYTE_AVA  0x0300
+#define SPI_REG_SPI_CONFIG      0x0400
+#define SPI_REG_SPI_STATUS      0x0500
+#define SPI_REG_INTR_CAUSE      0x0C00
+#define SPI_REG_INTR_ENABLE     0x0D00
+#define SPI_REG_RDBUF_WATERMARK 0x1200
+#define SPI_REG_WRBUF_WATERMARK 0x1300
+#define SPI_REG_SIGNATURE       0x1A00
+#define SPI_REG_ACTION_CTRL     0x1B00
+
+/*   SPI_CONFIG register definition;             */
+#define QCASPI_SLAVE_RESET_BIT (1 << 6)
+
+/*   INTR_CAUSE/ENABLE register definition.      */
+#define SPI_INT_WRBUF_BELOW_WM (1 << 10)
+#define SPI_INT_CPU_ON         (1 << 6)
+#define SPI_INT_ADDR_ERR       (1 << 3)
+#define SPI_INT_WRBUF_ERR      (1 << 2)
+#define SPI_INT_RDBUF_ERR      (1 << 1)
+#define SPI_INT_PKT_AVLBL      (1 << 0)
+
+void qcaspi_spi_error(struct qcaspi *qca);
+int qcaspi_read_register(struct qcaspi *qca, u16 reg, u16 *result);
+int qcaspi_write_register(struct qcaspi *qca, u16 reg, u16 value);
+int qcaspi_tx_cmd(struct qcaspi *qca, u16 cmd);
+
+#endif /* _QCA_7K_H */
diff --git a/drivers/net/ethernet/qualcomm/qca_debug.c b/drivers/net/ethernet/qualcomm/qca_debug.c
new file mode 100644
index 000000000000..8e28234dddad
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_debug.c
@@ -0,0 +1,311 @@
+/*
+ *   Copyright (c) 2011, 2012, Qualcomm Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*   This file contains debugging routines for use in the QCA7K driver.
+ */
+
+#include <linux/debugfs.h>
+#include <linux/ethtool.h>
+#include <linux/seq_file.h>
+#include <linux/types.h>
+
+#include "qca_7k.h"
+#include "qca_debug.h"
+
+#define QCASPI_MAX_REGS 0x20
+
+static const u16 qcaspi_spi_regs[] = {
+	SPI_REG_BFR_SIZE,
+	SPI_REG_WRBUF_SPC_AVA,
+	SPI_REG_RDBUF_BYTE_AVA,
+	SPI_REG_SPI_CONFIG,
+	SPI_REG_SPI_STATUS,
+	SPI_REG_INTR_CAUSE,
+	SPI_REG_INTR_ENABLE,
+	SPI_REG_RDBUF_WATERMARK,
+	SPI_REG_WRBUF_WATERMARK,
+	SPI_REG_SIGNATURE,
+	SPI_REG_ACTION_CTRL
+};
+
+/* The order of these strings must match the order of the fields in
+ * struct qcaspi_stats
+ * See qca_spi.h
+ */
+static const char qcaspi_gstrings_stats[][ETH_GSTRING_LEN] = {
+	"Triggered resets",
+	"Device resets",
+	"Reset timeouts",
+	"Read errors",
+	"Write errors",
+	"Read buffer errors",
+	"Write buffer errors",
+	"Out of memory",
+	"Write buffer misses",
+	"Transmit ring full",
+	"SPI errors",
+};
+
+#ifdef CONFIG_DEBUG_FS
+
+static int
+qcaspi_info_show(struct seq_file *s, void *what)
+{
+	struct qcaspi *qca = s->private;
+
+	seq_printf(s, "RX buffer size   : %lu\n",
+		   (unsigned long)qca->buffer_size);
+
+	seq_puts(s, "TX ring state    : ");
+
+	if (qca->txr.skb[qca->txr.head] == NULL)
+		seq_puts(s, "empty");
+	else if (qca->txr.skb[qca->txr.tail])
+		seq_puts(s, "full");
+	else
+		seq_puts(s, "in use");
+
+	seq_puts(s, "\n");
+
+	seq_printf(s, "TX ring size     : %u\n",
+		   qca->txr.size);
+
+	seq_printf(s, "Sync state       : %u (",
+		   (unsigned int)qca->sync);
+	switch (qca->sync) {
+	case QCASPI_SYNC_UNKNOWN:
+		seq_puts(s, "QCASPI_SYNC_UNKNOWN");
+		break;
+	case QCASPI_SYNC_RESET:
+		seq_puts(s, "QCASPI_SYNC_RESET");
+		break;
+	case QCASPI_SYNC_READY:
+		seq_puts(s, "QCASPI_SYNC_READY");
+		break;
+	default:
+		seq_puts(s, "INVALID");
+		break;
+	}
+	seq_puts(s, ")\n");
+
+	seq_printf(s, "IRQ              : %d\n",
+		   qca->spi_dev->irq);
+	seq_printf(s, "INTR REQ         : %u\n",
+		   qca->intr_req);
+	seq_printf(s, "INTR SVC         : %u\n",
+		   qca->intr_svc);
+
+	seq_printf(s, "SPI max speed    : %lu\n",
+		   (unsigned long)qca->spi_dev->max_speed_hz);
+	seq_printf(s, "SPI mode         : %x\n",
+		   qca->spi_dev->mode);
+	seq_printf(s, "SPI chip select  : %u\n",
+		   (unsigned int)qca->spi_dev->chip_select);
+	seq_printf(s, "SPI legacy mode  : %u\n",
+		   (unsigned int)qca->legacy_mode);
+	seq_printf(s, "SPI burst length : %u\n",
+		   (unsigned int)qca->burst_len);
+
+	return 0;
+}
+
+static int
+qcaspi_info_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, qcaspi_info_show, inode->i_private);
+}
+
+static const struct file_operations qcaspi_info_ops = {
+	.open = qcaspi_info_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = single_release,
+};
+
+void
+qcaspi_init_device_debugfs(struct qcaspi *qca)
+{
+	struct dentry *device_root;
+
+	device_root = debugfs_create_dir(dev_name(&qca->net_dev->dev), NULL);
+	qca->device_root = device_root;
+
+	if (IS_ERR(device_root) || !device_root) {
+		pr_warn("failed to create debugfs directory for %s\n",
+			dev_name(&qca->net_dev->dev));
+		return;
+	}
+	debugfs_create_file("info", S_IFREG | S_IRUGO, device_root, qca,
+			    &qcaspi_info_ops);
+}
+
+void
+qcaspi_remove_device_debugfs(struct qcaspi *qca)
+{
+	debugfs_remove_recursive(qca->device_root);
+}
+
+#else /* CONFIG_DEBUG_FS */
+
+void
+qcaspi_init_device_debugfs(struct qcaspi *qca)
+{
+}
+
+void
+qcaspi_remove_device_debugfs(struct qcaspi *qca)
+{
+}
+
+#endif
+
+static void
+qcaspi_get_drvinfo(struct net_device *dev, struct ethtool_drvinfo *p)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+
+	strlcpy(p->driver, QCASPI_DRV_NAME, sizeof(p->driver));
+	strlcpy(p->version, QCASPI_DRV_VERSION, sizeof(p->version));
+	strlcpy(p->fw_version, "QCA7000", sizeof(p->fw_version));
+	strlcpy(p->bus_info, dev_name(&qca->spi_dev->dev),
+		sizeof(p->bus_info));
+}
+
+static int
+qcaspi_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
+{
+	cmd->transceiver = XCVR_INTERNAL;
+	cmd->supported = SUPPORTED_10baseT_Half;
+	ethtool_cmd_speed_set(cmd,  SPEED_10);
+	cmd->duplex = DUPLEX_HALF;
+	cmd->port = PORT_OTHER;
+	cmd->autoneg = AUTONEG_DISABLE;
+
+	return 0;
+}
+
+static void
+qcaspi_get_ethtool_stats(struct net_device *dev, struct ethtool_stats *estats, u64 *data)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+	struct qcaspi_stats *st = &qca->stats;
+
+	memcpy(data, st, ARRAY_SIZE(qcaspi_gstrings_stats) * sizeof(u64));
+}
+
+static void
+qcaspi_get_strings(struct net_device *dev, u32 stringset, u8 *buf)
+{
+	switch (stringset) {
+	case ETH_SS_STATS:
+		memcpy(buf, &qcaspi_gstrings_stats,
+		       sizeof(qcaspi_gstrings_stats));
+		break;
+	default:
+		WARN_ON(1);
+		break;
+	}
+}
+
+static int
+qcaspi_get_sset_count(struct net_device *dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS:
+		return ARRAY_SIZE(qcaspi_gstrings_stats);
+	default:
+		return -EINVAL;
+	}
+}
+
+static int
+qcaspi_get_regs_len(struct net_device *dev)
+{
+	return sizeof(u32) * QCASPI_MAX_REGS;
+}
+
+static void
+qcaspi_get_regs(struct net_device *dev, struct ethtool_regs *regs, void *p)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+	u32 *regs_buff = p;
+	unsigned int i;
+
+	regs->version = 1;
+	memset(regs_buff, 0, sizeof(u32) * QCASPI_MAX_REGS);
+
+	for (i = 0; i < ARRAY_SIZE(qcaspi_spi_regs); i++) {
+		u16 offset, value;
+
+		qcaspi_read_register(qca, qcaspi_spi_regs[i], &value);
+		offset = qcaspi_spi_regs[i] >> 8;
+		regs_buff[offset] = value;
+	}
+}
+
+static void
+qcaspi_get_ringparam(struct net_device *dev, struct ethtool_ringparam *ring)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+
+	ring->rx_max_pending = 4;
+	ring->tx_max_pending = TX_RING_MAX_LEN;
+	ring->rx_pending = 4;
+	ring->tx_pending = qca->txr.count;
+}
+
+static int
+qcaspi_set_ringparam(struct net_device *dev, struct ethtool_ringparam *ring)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+
+	if ((ring->rx_pending) ||
+	    (ring->rx_mini_pending) ||
+	    (ring->rx_jumbo_pending))
+		return -EINVAL;
+
+	if (netif_running(dev))
+		qcaspi_netdev_close(dev);
+
+	qca->txr.count = max_t(u32, ring->tx_pending, TX_RING_MIN_LEN);
+	qca->txr.count = min_t(u16, qca->txr.count, TX_RING_MAX_LEN);
+
+	if (netif_running(dev))
+		qcaspi_netdev_open(dev);
+
+	return 0;
+}
+
+static const struct ethtool_ops qcaspi_ethtool_ops = {
+	.get_drvinfo = qcaspi_get_drvinfo,
+	.get_link = ethtool_op_get_link,
+	.get_settings = qcaspi_get_settings,
+	.get_ethtool_stats = qcaspi_get_ethtool_stats,
+	.get_strings = qcaspi_get_strings,
+	.get_sset_count = qcaspi_get_sset_count,
+	.get_regs_len = qcaspi_get_regs_len,
+	.get_regs = qcaspi_get_regs,
+	.get_ringparam = qcaspi_get_ringparam,
+	.set_ringparam = qcaspi_set_ringparam,
+};
+
+void qcaspi_set_ethtool_ops(struct net_device *dev)
+{
+	dev->ethtool_ops = &qcaspi_ethtool_ops;
+}
diff --git a/drivers/net/ethernet/qualcomm/qca_debug.h b/drivers/net/ethernet/qualcomm/qca_debug.h
new file mode 100644
index 000000000000..46a785844421
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_debug.h
@@ -0,0 +1,34 @@
+/*
+ *   Copyright (c) 2011, 2012, Qualcomm Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*   This file contains debugging routines for use in the QCA7K driver.
+ */
+
+#ifndef _QCA_DEBUG_H
+#define _QCA_DEBUG_H
+
+#include "qca_spi.h"
+
+void qcaspi_init_device_debugfs(struct qcaspi *qca);
+
+void qcaspi_remove_device_debugfs(struct qcaspi *qca);
+
+void qcaspi_set_ethtool_ops(struct net_device *dev);
+
+#endif /* _QCA_DEBUG_H */
diff --git a/drivers/net/ethernet/qualcomm/qca_framing.c b/drivers/net/ethernet/qualcomm/qca_framing.c
new file mode 100644
index 000000000000..faa924c85e29
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_framing.c
@@ -0,0 +1,156 @@
+/*
+ *   Copyright (c) 2011, 2012, Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*   Atheros ethernet framing. Every Ethernet frame is surrounded
+ *   by an atheros frame while transmitted over a serial channel;
+ */
+
+#include <linux/kernel.h>
+
+#include "qca_framing.h"
+
+u16
+qcafrm_create_header(u8 *buf, u16 length)
+{
+	__le16 len;
+
+	if (!buf)
+		return 0;
+
+	len = cpu_to_le16(length);
+
+	buf[0] = 0xAA;
+	buf[1] = 0xAA;
+	buf[2] = 0xAA;
+	buf[3] = 0xAA;
+	buf[4] = len & 0xff;
+	buf[5] = (len >> 8) & 0xff;
+	buf[6] = 0;
+	buf[7] = 0;
+
+	return QCAFRM_HEADER_LEN;
+}
+
+u16
+qcafrm_create_footer(u8 *buf)
+{
+	if (!buf)
+		return 0;
+
+	buf[0] = 0x55;
+	buf[1] = 0x55;
+	return QCAFRM_FOOTER_LEN;
+}
+
+/*   Gather received bytes and try to extract a full ethernet frame by
+ *   following a simple state machine.
+ *
+ * Return:   QCAFRM_GATHER       No ethernet frame fully received yet.
+ *           QCAFRM_NOHEAD       Header expected but not found.
+ *           QCAFRM_INVLEN       Atheros frame length is invalid
+ *           QCAFRM_NOTAIL       Footer expected but not found.
+ *           > 0                 Number of byte in the fully received
+ *                               Ethernet frame
+ */
+
+s32
+qcafrm_fsm_decode(struct qcafrm_handle *handle, u8 *buf, u16 buf_len, u8 recv_byte)
+{
+	s32 ret = QCAFRM_GATHER;
+	u16 len;
+
+	switch (handle->state) {
+	case QCAFRM_HW_LEN0:
+	case QCAFRM_HW_LEN1:
+		/* by default, just go to next state */
+		handle->state--;
+
+		if (recv_byte != 0x00) {
+			/* first two bytes of length must be 0 */
+			handle->state = QCAFRM_HW_LEN0;
+		}
+		break;
+	case QCAFRM_HW_LEN2:
+	case QCAFRM_HW_LEN3:
+		handle->state--;
+		break;
+	/* 4 bytes header pattern */
+	case QCAFRM_WAIT_AA1:
+	case QCAFRM_WAIT_AA2:
+	case QCAFRM_WAIT_AA3:
+	case QCAFRM_WAIT_AA4:
+		if (recv_byte != 0xAA) {
+			ret = QCAFRM_NOHEAD;
+			handle->state = QCAFRM_HW_LEN0;
+		} else {
+			handle->state--;
+		}
+		break;
+		/* 2 bytes length. */
+		/* Borrow offset field to hold length for now. */
+	case QCAFRM_WAIT_LEN_BYTE0:
+		handle->offset = recv_byte;
+		handle->state = QCAFRM_WAIT_LEN_BYTE1;
+		break;
+	case QCAFRM_WAIT_LEN_BYTE1:
+		handle->offset = handle->offset | (recv_byte << 8);
+		handle->state = QCAFRM_WAIT_RSVD_BYTE1;
+		break;
+	case QCAFRM_WAIT_RSVD_BYTE1:
+		handle->state = QCAFRM_WAIT_RSVD_BYTE2;
+		break;
+	case QCAFRM_WAIT_RSVD_BYTE2:
+		len = handle->offset;
+		if (len > buf_len || len < QCAFRM_ETHMINLEN) {
+			ret = QCAFRM_INVLEN;
+			handle->state = QCAFRM_HW_LEN0;
+		} else {
+			handle->state = (enum qcafrm_state)(len + 1);
+			/* Remaining number of bytes. */
+			handle->offset = 0;
+		}
+		break;
+	default:
+		/* Receiving Ethernet frame itself. */
+		buf[handle->offset] = recv_byte;
+		handle->offset++;
+		handle->state--;
+		break;
+	case QCAFRM_WAIT_551:
+		if (recv_byte != 0x55) {
+			ret = QCAFRM_NOTAIL;
+			handle->state = QCAFRM_HW_LEN0;
+		} else {
+			handle->state = QCAFRM_WAIT_552;
+		}
+		break;
+	case QCAFRM_WAIT_552:
+		if (recv_byte != 0x55) {
+			ret = QCAFRM_NOTAIL;
+			handle->state = QCAFRM_HW_LEN0;
+		} else {
+			ret = handle->offset;
+			/* Frame is fully received. */
+			handle->state = QCAFRM_HW_LEN0;
+		}
+		break;
+	}
+
+	return ret;
+}
diff --git a/drivers/net/ethernet/qualcomm/qca_framing.h b/drivers/net/ethernet/qualcomm/qca_framing.h
new file mode 100644
index 000000000000..5d965959c978
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_framing.h
@@ -0,0 +1,134 @@
+/*
+ *   Copyright (c) 2011, 2012, Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*   Atheros Ethernet framing. Every Ethernet frame is surrounded by an atheros
+ *   frame while transmitted over a serial channel.
+ */
+
+#ifndef _QCA_FRAMING_H
+#define _QCA_FRAMING_H
+
+#include <linux/if_ether.h>
+#include <linux/if_vlan.h>
+#include <linux/types.h>
+
+/* Frame is currently being received */
+#define QCAFRM_GATHER 0
+
+/*  No header byte while expecting it */
+#define QCAFRM_NOHEAD (QCAFRM_ERR_BASE - 1)
+
+/* No tailer byte while expecting it */
+#define QCAFRM_NOTAIL (QCAFRM_ERR_BASE - 2)
+
+/* Frame length is invalid */
+#define QCAFRM_INVLEN (QCAFRM_ERR_BASE - 3)
+
+/* Frame length is invalid */
+#define QCAFRM_INVFRAME (QCAFRM_ERR_BASE - 4)
+
+/* Min/Max Ethernet MTU */
+#define QCAFRM_ETHMINMTU 46
+#define QCAFRM_ETHMAXMTU 1500
+
+/* Min/Max frame lengths */
+#define QCAFRM_ETHMINLEN (QCAFRM_ETHMINMTU + ETH_HLEN)
+#define QCAFRM_ETHMAXLEN (QCAFRM_ETHMAXMTU + VLAN_ETH_HLEN)
+
+/* QCA7K header len */
+#define QCAFRM_HEADER_LEN 8
+
+/* QCA7K footer len */
+#define QCAFRM_FOOTER_LEN 2
+
+/* QCA7K Framing. */
+#define QCAFRM_ERR_BASE -1000
+
+enum qcafrm_state {
+	QCAFRM_HW_LEN0 = 0x8000,
+	QCAFRM_HW_LEN1 = QCAFRM_HW_LEN0 - 1,
+	QCAFRM_HW_LEN2 = QCAFRM_HW_LEN1 - 1,
+	QCAFRM_HW_LEN3 = QCAFRM_HW_LEN2 - 1,
+
+	/*  Waiting first 0xAA of header */
+	QCAFRM_WAIT_AA1 = QCAFRM_HW_LEN3 - 1,
+
+	/*  Waiting second 0xAA of header */
+	QCAFRM_WAIT_AA2 = QCAFRM_WAIT_AA1 - 1,
+
+	/*  Waiting third 0xAA of header */
+	QCAFRM_WAIT_AA3 = QCAFRM_WAIT_AA2 - 1,
+
+	/*  Waiting fourth 0xAA of header */
+	QCAFRM_WAIT_AA4 = QCAFRM_WAIT_AA3 - 1,
+
+	/*  Waiting Byte 0-1 of length (litte endian) */
+	QCAFRM_WAIT_LEN_BYTE0 = QCAFRM_WAIT_AA4 - 1,
+	QCAFRM_WAIT_LEN_BYTE1 = QCAFRM_WAIT_AA4 - 2,
+
+	/* Reserved bytes */
+	QCAFRM_WAIT_RSVD_BYTE1 = QCAFRM_WAIT_AA4 - 3,
+	QCAFRM_WAIT_RSVD_BYTE2 = QCAFRM_WAIT_AA4 - 4,
+
+	/*  The frame length is used as the state until
+	 *  the end of the Ethernet frame
+	 *  Waiting for first 0x55 of footer
+	 */
+	QCAFRM_WAIT_551 = 1,
+
+	/*  Waiting for second 0x55 of footer */
+	QCAFRM_WAIT_552 = QCAFRM_WAIT_551 - 1
+};
+
+/*   Structure to maintain the frame decoding during reception. */
+
+struct qcafrm_handle {
+	/*  Current decoding state */
+	enum qcafrm_state state;
+
+	/* Offset in buffer (borrowed for length too) */
+	s16 offset;
+
+	/* Frame length as kept by this module */
+	u16 len;
+};
+
+u16 qcafrm_create_header(u8 *buf, u16 len);
+
+u16 qcafrm_create_footer(u8 *buf);
+
+static inline void qcafrm_fsm_init(struct qcafrm_handle *handle)
+{
+	handle->state = QCAFRM_HW_LEN0;
+}
+
+/*   Gather received bytes and try to extract a full Ethernet frame
+ *   by following a simple state machine.
+ *
+ * Return:   QCAFRM_GATHER       No Ethernet frame fully received yet.
+ *           QCAFRM_NOHEAD       Header expected but not found.
+ *           QCAFRM_INVLEN       QCA7K frame length is invalid
+ *           QCAFRM_NOTAIL       Footer expected but not found.
+ *           > 0                 Number of byte in the fully received
+ *                               Ethernet frame
+ */
+
+s32 qcafrm_fsm_decode(struct qcafrm_handle *handle, u8 *buf, u16 buf_len, u8 recv_byte);
+
+#endif /* _QCA_FRAMING_H */
diff --git a/drivers/net/ethernet/qualcomm/qca_spi.c b/drivers/net/ethernet/qualcomm/qca_spi.c
new file mode 100644
index 000000000000..2c811f66d5ac
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_spi.c
@@ -0,0 +1,991 @@
+/*
+ *   Copyright (c) 2011, 2012, Qualcomm Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*   This module implements the Qualcomm Atheros SPI protocol for
+ *   kernel-based SPI device; it is essentially an Ethernet-to-SPI
+ *   serial converter;
+ */
+
+#include <linux/errno.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+#include <linux/if_ether.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/jiffies.h>
+#include <linux/kernel.h>
+#include <linux/kthread.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/netdevice.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_net.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/spi/spi.h>
+#include <linux/types.h>
+#include <linux/version.h>
+
+#include "qca_7k.h"
+#include "qca_debug.h"
+#include "qca_framing.h"
+#include "qca_spi.h"
+
+#define MAX_DMA_BURST_LEN 5000
+
+/*   Modules parameters     */
+#define QCASPI_CLK_SPEED_MIN 1000000
+#define QCASPI_CLK_SPEED_MAX 16000000
+#define QCASPI_CLK_SPEED     8000000
+static int qcaspi_clkspeed;
+module_param(qcaspi_clkspeed, int, 0);
+MODULE_PARM_DESC(qcaspi_clkspeed, "SPI bus clock speed (Hz). Use 1000000-16000000.");
+
+#define QCASPI_BURST_LEN_MIN 1
+#define QCASPI_BURST_LEN_MAX MAX_DMA_BURST_LEN
+static int qcaspi_burst_len = MAX_DMA_BURST_LEN;
+module_param(qcaspi_burst_len, int, 0);
+MODULE_PARM_DESC(qcaspi_burst_len, "Number of data bytes per burst. Use 1-5000.");
+
+#define QCASPI_PLUGGABLE_MIN 0
+#define QCASPI_PLUGGABLE_MAX 1
+static int qcaspi_pluggable = QCASPI_PLUGGABLE_MIN;
+module_param(qcaspi_pluggable, int, 0);
+MODULE_PARM_DESC(qcaspi_pluggable, "Pluggable SPI connection (yes/no).");
+
+#define QCASPI_MTU QCAFRM_ETHMAXMTU
+#define QCASPI_TX_TIMEOUT (1 * HZ)
+#define QCASPI_QCA7K_REBOOT_TIME_MS 1000
+
+static void
+start_spi_intr_handling(struct qcaspi *qca, u16 *intr_cause)
+{
+	*intr_cause = 0;
+
+	qcaspi_write_register(qca, SPI_REG_INTR_ENABLE, 0);
+	qcaspi_read_register(qca, SPI_REG_INTR_CAUSE, intr_cause);
+	netdev_dbg(qca->net_dev, "interrupts: 0x%04x\n", *intr_cause);
+}
+
+static void
+end_spi_intr_handling(struct qcaspi *qca, u16 intr_cause)
+{
+	u16 intr_enable = (SPI_INT_CPU_ON |
+			   SPI_INT_PKT_AVLBL |
+			   SPI_INT_RDBUF_ERR |
+			   SPI_INT_WRBUF_ERR);
+
+	qcaspi_write_register(qca, SPI_REG_INTR_CAUSE, intr_cause);
+	qcaspi_write_register(qca, SPI_REG_INTR_ENABLE, intr_enable);
+	netdev_dbg(qca->net_dev, "acking int: 0x%04x\n", intr_cause);
+}
+
+static u32
+qcaspi_write_burst(struct qcaspi *qca, u8 *src, u32 len)
+{
+	__be16 cmd;
+	struct spi_message *msg = &qca->spi_msg2;
+	struct spi_transfer *transfer = &qca->spi_xfer2[0];
+	int ret;
+
+	cmd = cpu_to_be16(QCA7K_SPI_WRITE | QCA7K_SPI_EXTERNAL);
+	transfer->tx_buf = &cmd;
+	transfer->rx_buf = NULL;
+	transfer->len = QCASPI_CMD_LEN;
+	transfer = &qca->spi_xfer2[1];
+	transfer->tx_buf = src;
+	transfer->rx_buf = NULL;
+	transfer->len = len;
+
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (ret || (msg->actual_length != QCASPI_CMD_LEN + len)) {
+		qcaspi_spi_error(qca);
+		return 0;
+	}
+
+	return len;
+}
+
+static u32
+qcaspi_write_legacy(struct qcaspi *qca, u8 *src, u32 len)
+{
+	struct spi_message *msg = &qca->spi_msg1;
+	struct spi_transfer *transfer = &qca->spi_xfer1;
+	int ret;
+
+	transfer->tx_buf = src;
+	transfer->rx_buf = NULL;
+	transfer->len = len;
+
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (ret || (msg->actual_length != len)) {
+		qcaspi_spi_error(qca);
+		return 0;
+	}
+
+	return len;
+}
+
+static u32
+qcaspi_read_burst(struct qcaspi *qca, u8 *dst, u32 len)
+{
+	struct spi_message *msg = &qca->spi_msg2;
+	__be16 cmd;
+	struct spi_transfer *transfer = &qca->spi_xfer2[0];
+	int ret;
+
+	cmd = cpu_to_be16(QCA7K_SPI_READ | QCA7K_SPI_EXTERNAL);
+	transfer->tx_buf = &cmd;
+	transfer->rx_buf = NULL;
+	transfer->len = QCASPI_CMD_LEN;
+	transfer = &qca->spi_xfer2[1];
+	transfer->tx_buf = NULL;
+	transfer->rx_buf = dst;
+	transfer->len = len;
+
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (ret || (msg->actual_length != QCASPI_CMD_LEN + len)) {
+		qcaspi_spi_error(qca);
+		return 0;
+	}
+
+	return len;
+}
+
+static u32
+qcaspi_read_legacy(struct qcaspi *qca, u8 *dst, u32 len)
+{
+	struct spi_message *msg = &qca->spi_msg1;
+	struct spi_transfer *transfer = &qca->spi_xfer1;
+	int ret;
+
+	transfer->tx_buf = NULL;
+	transfer->rx_buf = dst;
+	transfer->len = len;
+
+	ret = spi_sync(qca->spi_dev, msg);
+
+	if (ret || (msg->actual_length != len)) {
+		qcaspi_spi_error(qca);
+		return 0;
+	}
+
+	return len;
+}
+
+static int
+qcaspi_tx_frame(struct qcaspi *qca, struct sk_buff *skb)
+{
+	u32 count;
+	u32 written;
+	u32 offset;
+	u32 len;
+
+	len = skb->len;
+
+	qcaspi_write_register(qca, SPI_REG_BFR_SIZE, len);
+	if (qca->legacy_mode)
+		qcaspi_tx_cmd(qca, QCA7K_SPI_WRITE | QCA7K_SPI_EXTERNAL);
+
+	offset = 0;
+	while (len) {
+		count = len;
+		if (count > qca->burst_len)
+			count = qca->burst_len;
+
+		if (qca->legacy_mode) {
+			written = qcaspi_write_legacy(qca,
+						      skb->data + offset,
+						      count);
+		} else {
+			written = qcaspi_write_burst(qca,
+						     skb->data + offset,
+						     count);
+		}
+
+		if (written != count)
+			return -1;
+
+		offset += count;
+		len -= count;
+	}
+
+	return 0;
+}
+
+static int
+qcaspi_transmit(struct qcaspi *qca)
+{
+	struct net_device_stats *n_stats = &qca->net_dev->stats;
+	u16 available = 0;
+	u32 pkt_len;
+	u16 new_head;
+	u16 packets = 0;
+
+	if (qca->txr.skb[qca->txr.head] == NULL)
+		return 0;
+
+	qcaspi_read_register(qca, SPI_REG_WRBUF_SPC_AVA, &available);
+
+	while (qca->txr.skb[qca->txr.head]) {
+		pkt_len = qca->txr.skb[qca->txr.head]->len + QCASPI_HW_PKT_LEN;
+
+		if (available < pkt_len) {
+			if (packets == 0)
+				qca->stats.write_buf_miss++;
+			break;
+		}
+
+		if (qcaspi_tx_frame(qca, qca->txr.skb[qca->txr.head]) == -1) {
+			qca->stats.write_err++;
+			return -1;
+		}
+
+		packets++;
+		n_stats->tx_packets++;
+		n_stats->tx_bytes += qca->txr.skb[qca->txr.head]->len;
+		available -= pkt_len;
+
+		/* remove the skb from the queue */
+		/* XXX After inconsistent lock states netif_tx_lock()
+		 * has been replaced by netif_tx_lock_bh() and so on.
+		 */
+		netif_tx_lock_bh(qca->net_dev);
+		dev_kfree_skb(qca->txr.skb[qca->txr.head]);
+		qca->txr.skb[qca->txr.head] = NULL;
+		qca->txr.size -= pkt_len;
+		new_head = qca->txr.head + 1;
+		if (new_head >= qca->txr.count)
+			new_head = 0;
+		qca->txr.head = new_head;
+		if (netif_queue_stopped(qca->net_dev))
+			netif_wake_queue(qca->net_dev);
+		netif_tx_unlock_bh(qca->net_dev);
+	}
+
+	return 0;
+}
+
+static int
+qcaspi_receive(struct qcaspi *qca)
+{
+	struct net_device *net_dev = qca->net_dev;
+	struct net_device_stats *n_stats = &net_dev->stats;
+	u16 available = 0;
+	u32 bytes_read;
+	u8 *cp;
+
+	/* Allocate rx SKB if we don't have one available. */
+	if (!qca->rx_skb) {
+		qca->rx_skb = netdev_alloc_skb(net_dev,
+					       net_dev->mtu + VLAN_ETH_HLEN);
+		if (!qca->rx_skb) {
+			netdev_dbg(net_dev, "out of RX resources\n");
+			qca->stats.out_of_mem++;
+			return -1;
+		}
+	}
+
+	/* Read the packet size. */
+	qcaspi_read_register(qca, SPI_REG_RDBUF_BYTE_AVA, &available);
+	netdev_dbg(net_dev, "qcaspi_receive: SPI_REG_RDBUF_BYTE_AVA: Value: %08x\n",
+		   available);
+
+	if (available == 0) {
+		netdev_dbg(net_dev, "qcaspi_receive called without any data being available!\n");
+		return -1;
+	}
+
+	qcaspi_write_register(qca, SPI_REG_BFR_SIZE, available);
+
+	if (qca->legacy_mode)
+		qcaspi_tx_cmd(qca, QCA7K_SPI_READ | QCA7K_SPI_EXTERNAL);
+
+	while (available) {
+		u32 count = available;
+
+		if (count > qca->burst_len)
+			count = qca->burst_len;
+
+		if (qca->legacy_mode) {
+			bytes_read = qcaspi_read_legacy(qca, qca->rx_buffer,
+							count);
+		} else {
+			bytes_read = qcaspi_read_burst(qca, qca->rx_buffer,
+						       count);
+		}
+
+		netdev_dbg(net_dev, "available: %d, byte read: %d\n",
+			   available, bytes_read);
+
+		if (bytes_read) {
+			available -= bytes_read;
+		} else {
+			qca->stats.read_err++;
+			return -1;
+		}
+
+		cp = qca->rx_buffer;
+
+		while ((bytes_read--) && (qca->rx_skb)) {
+			s32 retcode;
+
+			retcode = qcafrm_fsm_decode(&qca->frm_handle,
+						    qca->rx_skb->data,
+						    skb_tailroom(qca->rx_skb),
+						    *cp);
+			cp++;
+			switch (retcode) {
+			case QCAFRM_GATHER:
+			case QCAFRM_NOHEAD:
+				break;
+			case QCAFRM_NOTAIL:
+				netdev_dbg(net_dev, "no RX tail\n");
+				n_stats->rx_errors++;
+				n_stats->rx_dropped++;
+				break;
+			case QCAFRM_INVLEN:
+				netdev_dbg(net_dev, "invalid RX length\n");
+				n_stats->rx_errors++;
+				n_stats->rx_dropped++;
+				break;
+			default:
+				qca->rx_skb->dev = qca->net_dev;
+				n_stats->rx_packets++;
+				n_stats->rx_bytes += retcode;
+				skb_put(qca->rx_skb, retcode);
+				qca->rx_skb->protocol = eth_type_trans(
+					qca->rx_skb, qca->rx_skb->dev);
+				qca->rx_skb->ip_summed = CHECKSUM_UNNECESSARY;
+				netif_rx_ni(qca->rx_skb);
+				qca->rx_skb = netdev_alloc_skb(net_dev,
+					net_dev->mtu + VLAN_ETH_HLEN);
+				if (!qca->rx_skb) {
+					netdev_dbg(net_dev, "out of RX resources\n");
+					n_stats->rx_errors++;
+					qca->stats.out_of_mem++;
+					break;
+				}
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*   Check that tx ring stores only so much bytes
+ *   that fit into the internal QCA buffer.
+ */
+
+static int
+qcaspi_tx_ring_has_space(struct tx_ring *txr)
+{
+	if (txr->skb[txr->tail])
+		return 0;
+
+	return (txr->size + QCAFRM_ETHMAXLEN < QCASPI_HW_BUF_LEN) ? 1 : 0;
+}
+
+/*   Flush the tx ring. This function is only safe to
+ *   call from the qcaspi_spi_thread.
+ */
+
+static void
+qcaspi_flush_tx_ring(struct qcaspi *qca)
+{
+	int i;
+
+	/* XXX After inconsistent lock states netif_tx_lock()
+	 * has been replaced by netif_tx_lock_bh() and so on.
+	 */
+	netif_tx_lock_bh(qca->net_dev);
+	for (i = 0; i < TX_RING_MAX_LEN; i++) {
+		if (qca->txr.skb[i]) {
+			dev_kfree_skb(qca->txr.skb[i]);
+			qca->txr.skb[i] = NULL;
+			qca->net_dev->stats.tx_dropped++;
+		}
+	}
+	qca->txr.tail = 0;
+	qca->txr.head = 0;
+	qca->txr.size = 0;
+	netif_tx_unlock_bh(qca->net_dev);
+}
+
+static void
+qcaspi_qca7k_sync(struct qcaspi *qca, int event)
+{
+	u16 signature = 0;
+	u16 spi_config;
+	u16 wrbuf_space = 0;
+	static u16 reset_count;
+
+	if (event == QCASPI_EVENT_CPUON) {
+		/* Read signature twice, if not valid
+		 * go back to unknown state.
+		 */
+		qcaspi_read_register(qca, SPI_REG_SIGNATURE, &signature);
+		qcaspi_read_register(qca, SPI_REG_SIGNATURE, &signature);
+		if (signature != QCASPI_GOOD_SIGNATURE) {
+			qca->sync = QCASPI_SYNC_UNKNOWN;
+			netdev_dbg(qca->net_dev, "sync: got CPU on, but signature was invalid, restart\n");
+		} else {
+			/* ensure that the WRBUF is empty */
+			qcaspi_read_register(qca, SPI_REG_WRBUF_SPC_AVA,
+					     &wrbuf_space);
+			if (wrbuf_space != QCASPI_HW_BUF_LEN) {
+				netdev_dbg(qca->net_dev, "sync: got CPU on, but wrbuf not empty. reset!\n");
+				qca->sync = QCASPI_SYNC_UNKNOWN;
+			} else {
+				netdev_dbg(qca->net_dev, "sync: got CPU on, now in sync\n");
+				qca->sync = QCASPI_SYNC_READY;
+				return;
+			}
+		}
+	}
+
+	switch (qca->sync) {
+	case QCASPI_SYNC_READY:
+		/* Read signature, if not valid go to unknown state. */
+		qcaspi_read_register(qca, SPI_REG_SIGNATURE, &signature);
+		if (signature != QCASPI_GOOD_SIGNATURE) {
+			qca->sync = QCASPI_SYNC_UNKNOWN;
+			netdev_dbg(qca->net_dev, "sync: bad signature, restart\n");
+			/* don't reset right away */
+			return;
+		}
+		break;
+	case QCASPI_SYNC_UNKNOWN:
+		/* Read signature, if not valid stay in unknown state */
+		qcaspi_read_register(qca, SPI_REG_SIGNATURE, &signature);
+		if (signature != QCASPI_GOOD_SIGNATURE) {
+			netdev_dbg(qca->net_dev, "sync: could not read signature to reset device, retry.\n");
+			return;
+		}
+
+		/* TODO: use GPIO to reset QCA7000 in legacy mode*/
+		netdev_dbg(qca->net_dev, "sync: resetting device.\n");
+		qcaspi_read_register(qca, SPI_REG_SPI_CONFIG, &spi_config);
+		spi_config |= QCASPI_SLAVE_RESET_BIT;
+		qcaspi_write_register(qca, SPI_REG_SPI_CONFIG, spi_config);
+
+		qca->sync = QCASPI_SYNC_RESET;
+		qca->stats.trig_reset++;
+		reset_count = 0;
+		break;
+	case QCASPI_SYNC_RESET:
+		reset_count++;
+		netdev_dbg(qca->net_dev, "sync: waiting for CPU on, count %u.\n",
+			   reset_count);
+		if (reset_count >= QCASPI_RESET_TIMEOUT) {
+			/* reset did not seem to take place, try again */
+			qca->sync = QCASPI_SYNC_UNKNOWN;
+			qca->stats.reset_timeout++;
+			netdev_dbg(qca->net_dev, "sync: reset timeout, restarting process.\n");
+		}
+		break;
+	}
+}
+
+static int
+qcaspi_spi_thread(void *data)
+{
+	struct qcaspi *qca = data;
+	u16 intr_cause = 0;
+
+	netdev_info(qca->net_dev, "SPI thread created\n");
+	while (!kthread_should_stop()) {
+		set_current_state(TASK_INTERRUPTIBLE);
+		if ((qca->intr_req == qca->intr_svc) &&
+		    (qca->txr.skb[qca->txr.head] == NULL) &&
+		    (qca->sync == QCASPI_SYNC_READY))
+			schedule();
+
+		set_current_state(TASK_RUNNING);
+
+		netdev_dbg(qca->net_dev, "have work to do. int: %d, tx_skb: %p\n",
+			   qca->intr_req - qca->intr_svc,
+			   qca->txr.skb[qca->txr.head]);
+
+		qcaspi_qca7k_sync(qca, QCASPI_EVENT_UPDATE);
+
+		if (qca->sync != QCASPI_SYNC_READY) {
+			netdev_dbg(qca->net_dev, "sync: not ready %u, turn off carrier and flush\n",
+				   (unsigned int)qca->sync);
+			netif_stop_queue(qca->net_dev);
+			netif_carrier_off(qca->net_dev);
+			qcaspi_flush_tx_ring(qca);
+			msleep(QCASPI_QCA7K_REBOOT_TIME_MS);
+		}
+
+		if (qca->intr_svc != qca->intr_req) {
+			qca->intr_svc = qca->intr_req;
+			start_spi_intr_handling(qca, &intr_cause);
+
+			if (intr_cause & SPI_INT_CPU_ON) {
+				qcaspi_qca7k_sync(qca, QCASPI_EVENT_CPUON);
+
+				/* not synced. */
+				if (qca->sync != QCASPI_SYNC_READY)
+					continue;
+
+				qca->stats.device_reset++;
+				netif_wake_queue(qca->net_dev);
+				netif_carrier_on(qca->net_dev);
+			}
+
+			if (intr_cause & SPI_INT_RDBUF_ERR) {
+				/* restart sync */
+				netdev_dbg(qca->net_dev, "===> rdbuf error!\n");
+				qca->stats.read_buf_err++;
+				qca->sync = QCASPI_SYNC_UNKNOWN;
+				continue;
+			}
+
+			if (intr_cause & SPI_INT_WRBUF_ERR) {
+				/* restart sync */
+				netdev_dbg(qca->net_dev, "===> wrbuf error!\n");
+				qca->stats.write_buf_err++;
+				qca->sync = QCASPI_SYNC_UNKNOWN;
+				continue;
+			}
+
+			/* can only handle other interrupts
+			 * if sync has occured
+			 */
+			if (qca->sync == QCASPI_SYNC_READY) {
+				if (intr_cause & SPI_INT_PKT_AVLBL)
+					qcaspi_receive(qca);
+			}
+
+			end_spi_intr_handling(qca, intr_cause);
+		}
+
+		if (qca->sync == QCASPI_SYNC_READY)
+			qcaspi_transmit(qca);
+	}
+	set_current_state(TASK_RUNNING);
+	netdev_info(qca->net_dev, "SPI thread exit\n");
+
+	return 0;
+}
+
+static irqreturn_t
+qcaspi_intr_handler(int irq, void *data)
+{
+	struct qcaspi *qca = data;
+
+	qca->intr_req++;
+	if (qca->spi_thread &&
+	    qca->spi_thread->state != TASK_RUNNING)
+		wake_up_process(qca->spi_thread);
+
+	return IRQ_HANDLED;
+}
+
+int
+qcaspi_netdev_open(struct net_device *dev)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+	int ret = 0;
+
+	if (!qca)
+		return -EINVAL;
+
+	qca->intr_req = 1;
+	qca->intr_svc = 0;
+	qca->sync = QCASPI_SYNC_UNKNOWN;
+	qcafrm_fsm_init(&qca->frm_handle);
+
+	qca->spi_thread = kthread_run((void *)qcaspi_spi_thread,
+				      qca, "%s", dev->name);
+
+	if (IS_ERR(qca->spi_thread)) {
+		netdev_err(dev, "%s: unable to start kernel thread.\n",
+			   QCASPI_DRV_NAME);
+		return PTR_ERR(qca->spi_thread);
+	}
+
+	ret = request_irq(qca->spi_dev->irq, qcaspi_intr_handler, 0,
+			  dev->name, qca);
+	if (ret) {
+		netdev_err(dev, "%s: unable to get IRQ %d (irqval=%d).\n",
+			   QCASPI_DRV_NAME, qca->spi_dev->irq, ret);
+		kthread_stop(qca->spi_thread);
+		return ret;
+	}
+
+	netif_start_queue(qca->net_dev);
+
+	return 0;
+}
+
+int
+qcaspi_netdev_close(struct net_device *dev)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+
+	netif_stop_queue(dev);
+
+	qcaspi_write_register(qca, SPI_REG_INTR_ENABLE, 0);
+	free_irq(qca->spi_dev->irq, qca);
+
+	kthread_stop(qca->spi_thread);
+	qca->spi_thread = NULL;
+	qcaspi_flush_tx_ring(qca);
+
+	return 0;
+}
+
+static netdev_tx_t
+qcaspi_netdev_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	u32 frame_len;
+	u8 *ptmp;
+	struct qcaspi *qca = netdev_priv(dev);
+	u16 new_tail;
+	struct sk_buff *tskb;
+	u8 pad_len = 0;
+
+	if (skb->len < QCAFRM_ETHMINLEN)
+		pad_len = QCAFRM_ETHMINLEN - skb->len;
+
+	if (qca->txr.skb[qca->txr.tail]) {
+		netdev_warn(qca->net_dev, "queue was unexpectedly full!\n");
+		netif_stop_queue(qca->net_dev);
+		qca->stats.ring_full++;
+		return NETDEV_TX_BUSY;
+	}
+
+	if ((skb_headroom(skb) < QCAFRM_HEADER_LEN) ||
+	    (skb_tailroom(skb) < QCAFRM_FOOTER_LEN + pad_len)) {
+		tskb = skb_copy_expand(skb, QCAFRM_HEADER_LEN,
+				       QCAFRM_FOOTER_LEN + pad_len, GFP_ATOMIC);
+		if (!tskb) {
+			netdev_dbg(qca->net_dev, "could not allocate tx_buff\n");
+			qca->stats.out_of_mem++;
+			return NETDEV_TX_BUSY;
+		}
+		dev_kfree_skb(skb);
+		skb = tskb;
+	}
+
+	frame_len = skb->len + pad_len;
+
+	ptmp = skb_push(skb, QCAFRM_HEADER_LEN);
+	qcafrm_create_header(ptmp, frame_len);
+
+	if (pad_len) {
+		ptmp = skb_put(skb, pad_len);
+		memset(ptmp, 0, pad_len);
+	}
+
+	ptmp = skb_put(skb, QCAFRM_FOOTER_LEN);
+	qcafrm_create_footer(ptmp);
+
+	netdev_dbg(qca->net_dev, "Tx-ing packet: Size: 0x%08x\n",
+		   skb->len);
+
+	qca->txr.size += skb->len + QCASPI_HW_PKT_LEN;
+
+	new_tail = qca->txr.tail + 1;
+	if (new_tail >= qca->txr.count)
+		new_tail = 0;
+
+	qca->txr.skb[qca->txr.tail] = skb;
+	qca->txr.tail = new_tail;
+
+	if (!qcaspi_tx_ring_has_space(&qca->txr)) {
+		netif_stop_queue(qca->net_dev);
+		qca->stats.ring_full++;
+	}
+
+	dev->trans_start = jiffies;
+
+	if (qca->spi_thread &&
+	    qca->spi_thread->state != TASK_RUNNING)
+		wake_up_process(qca->spi_thread);
+
+	return NETDEV_TX_OK;
+}
+
+static void
+qcaspi_netdev_tx_timeout(struct net_device *dev)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+
+	netdev_info(qca->net_dev, "Transmit timeout at %ld, latency %ld\n",
+		    jiffies, jiffies - dev->trans_start);
+	qca->net_dev->stats.tx_errors++;
+	/* wake the queue if there is room */
+	if (qcaspi_tx_ring_has_space(&qca->txr))
+		netif_wake_queue(dev);
+}
+
+static int
+qcaspi_netdev_init(struct net_device *dev)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+
+	dev->mtu = QCASPI_MTU;
+	dev->type = ARPHRD_ETHER;
+	qca->clkspeed = qcaspi_clkspeed;
+	qca->burst_len = qcaspi_burst_len;
+	qca->spi_thread = NULL;
+	qca->buffer_size = (dev->mtu + VLAN_ETH_HLEN + QCAFRM_HEADER_LEN +
+		QCAFRM_FOOTER_LEN + 4) * 4;
+
+	memset(&qca->stats, 0, sizeof(struct qcaspi_stats));
+
+	qca->rx_buffer = kmalloc(qca->buffer_size, GFP_KERNEL);
+	if (!qca->rx_buffer)
+		return -ENOBUFS;
+
+	qca->rx_skb = netdev_alloc_skb(dev, qca->net_dev->mtu + VLAN_ETH_HLEN);
+	if (!qca->rx_skb) {
+		kfree(qca->rx_buffer);
+		netdev_info(qca->net_dev, "Failed to allocate RX sk_buff.\n");
+		return -ENOBUFS;
+	}
+
+	return 0;
+}
+
+static void
+qcaspi_netdev_uninit(struct net_device *dev)
+{
+	struct qcaspi *qca = netdev_priv(dev);
+
+	kfree(qca->rx_buffer);
+	qca->buffer_size = 0;
+	if (qca->rx_skb)
+		dev_kfree_skb(qca->rx_skb);
+}
+
+static int
+qcaspi_netdev_change_mtu(struct net_device *dev, int new_mtu)
+{
+	if ((new_mtu < QCAFRM_ETHMINMTU) || (new_mtu > QCAFRM_ETHMAXMTU))
+		return -EINVAL;
+
+	dev->mtu = new_mtu;
+
+	return 0;
+}
+
+static const struct net_device_ops qcaspi_netdev_ops = {
+	.ndo_init = qcaspi_netdev_init,
+	.ndo_uninit = qcaspi_netdev_uninit,
+	.ndo_open = qcaspi_netdev_open,
+	.ndo_stop = qcaspi_netdev_close,
+	.ndo_start_xmit = qcaspi_netdev_xmit,
+	.ndo_change_mtu = qcaspi_netdev_change_mtu,
+	.ndo_set_mac_address = eth_mac_addr,
+	.ndo_tx_timeout = qcaspi_netdev_tx_timeout,
+	.ndo_validate_addr = eth_validate_addr,
+};
+
+static void
+qcaspi_netdev_setup(struct net_device *dev)
+{
+	struct qcaspi *qca = NULL;
+
+	dev->netdev_ops = &qcaspi_netdev_ops;
+	qcaspi_set_ethtool_ops(dev);
+	dev->watchdog_timeo = QCASPI_TX_TIMEOUT;
+	dev->flags = IFF_MULTICAST;
+	dev->tx_queue_len = 100;
+
+	qca = netdev_priv(dev);
+	memset(qca, 0, sizeof(struct qcaspi));
+
+	memset(&qca->spi_xfer1, 0, sizeof(struct spi_transfer));
+	memset(&qca->spi_xfer2, 0, sizeof(struct spi_transfer) * 2);
+
+	spi_message_init(&qca->spi_msg1);
+	spi_message_add_tail(&qca->spi_xfer1, &qca->spi_msg1);
+
+	spi_message_init(&qca->spi_msg2);
+	spi_message_add_tail(&qca->spi_xfer2[0], &qca->spi_msg2);
+	spi_message_add_tail(&qca->spi_xfer2[1], &qca->spi_msg2);
+
+	memset(&qca->txr, 0, sizeof(qca->txr));
+	qca->txr.count = TX_RING_MAX_LEN;
+}
+
+static const struct of_device_id qca_spi_of_match[] = {
+	{ .compatible = "qca,qca7000" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, qca_spi_of_match);
+
+static int
+qca_spi_probe(struct spi_device *spi_device)
+{
+	struct qcaspi *qca = NULL;
+	struct net_device *qcaspi_devs = NULL;
+	u8 legacy_mode = 0;
+	u16 signature;
+	const char *mac;
+
+	if (!spi_device->dev.of_node) {
+		dev_err(&spi_device->dev, "Missing device tree\n");
+		return -EINVAL;
+	}
+
+	legacy_mode = of_property_read_bool(spi_device->dev.of_node,
+					    "qca,legacy-mode");
+
+	if (qcaspi_clkspeed == 0) {
+		if (spi_device->max_speed_hz)
+			qcaspi_clkspeed = spi_device->max_speed_hz;
+		else
+			qcaspi_clkspeed = QCASPI_CLK_SPEED;
+	}
+
+	if ((qcaspi_clkspeed < QCASPI_CLK_SPEED_MIN) ||
+	    (qcaspi_clkspeed > QCASPI_CLK_SPEED_MAX)) {
+		dev_info(&spi_device->dev, "Invalid clkspeed: %d\n",
+			 qcaspi_clkspeed);
+		return -EINVAL;
+	}
+
+	if ((qcaspi_burst_len < QCASPI_BURST_LEN_MIN) ||
+	    (qcaspi_burst_len > QCASPI_BURST_LEN_MAX)) {
+		dev_info(&spi_device->dev, "Invalid burst len: %d\n",
+			 qcaspi_burst_len);
+		return -EINVAL;
+	}
+
+	if ((qcaspi_pluggable < QCASPI_PLUGGABLE_MIN) ||
+	    (qcaspi_pluggable > QCASPI_PLUGGABLE_MAX)) {
+		dev_info(&spi_device->dev, "Invalid pluggable: %d\n",
+			 qcaspi_pluggable);
+		return -EINVAL;
+	}
+
+	dev_info(&spi_device->dev, "ver=%s, clkspeed=%d, burst_len=%d, pluggable=%d\n",
+		 QCASPI_DRV_VERSION,
+		 qcaspi_clkspeed,
+		 qcaspi_burst_len,
+		 qcaspi_pluggable);
+
+	spi_device->mode = SPI_MODE_3;
+	spi_device->max_speed_hz = qcaspi_clkspeed;
+	if (spi_setup(spi_device) < 0) {
+		dev_err(&spi_device->dev, "Unable to setup SPI device\n");
+		return -EFAULT;
+	}
+
+	qcaspi_devs = alloc_etherdev(sizeof(struct qcaspi));
+	if (!qcaspi_devs)
+		return -ENOMEM;
+
+	qcaspi_netdev_setup(qcaspi_devs);
+
+	qca = netdev_priv(qcaspi_devs);
+	if (!qca) {
+		free_netdev(qcaspi_devs);
+		dev_err(&spi_device->dev, "Fail to retrieve private structure\n");
+		return -ENOMEM;
+	}
+	qca->net_dev = qcaspi_devs;
+	qca->spi_dev = spi_device;
+	qca->legacy_mode = legacy_mode;
+
+	mac = of_get_mac_address(spi_device->dev.of_node);
+
+	if (mac)
+		ether_addr_copy(qca->net_dev->dev_addr, mac);
+
+	if (!is_valid_ether_addr(qca->net_dev->dev_addr)) {
+		eth_hw_addr_random(qca->net_dev);
+		dev_info(&spi_device->dev, "Using random MAC address: %pM\n",
+			 qca->net_dev->dev_addr);
+	}
+
+	netif_carrier_off(qca->net_dev);
+
+	if (!qcaspi_pluggable) {
+		qcaspi_read_register(qca, SPI_REG_SIGNATURE, &signature);
+		qcaspi_read_register(qca, SPI_REG_SIGNATURE, &signature);
+
+		if (signature != QCASPI_GOOD_SIGNATURE) {
+			dev_err(&spi_device->dev, "Invalid signature (0x%04X)\n",
+				signature);
+			free_netdev(qcaspi_devs);
+			return -EFAULT;
+		}
+	}
+
+	if (register_netdev(qcaspi_devs)) {
+		dev_info(&spi_device->dev, "Unable to register net device %s\n",
+			 qcaspi_devs->name);
+		free_netdev(qcaspi_devs);
+		return -EFAULT;
+	}
+
+	spi_set_drvdata(spi_device, qcaspi_devs);
+
+	qcaspi_init_device_debugfs(qca);
+
+	return 0;
+}
+
+static int
+qca_spi_remove(struct spi_device *spi_device)
+{
+	struct net_device *qcaspi_devs = spi_get_drvdata(spi_device);
+	struct qcaspi *qca = netdev_priv(qcaspi_devs);
+
+	qcaspi_remove_device_debugfs(qca);
+
+	unregister_netdev(qcaspi_devs);
+	free_netdev(qcaspi_devs);
+
+	return 0;
+}
+
+static const struct spi_device_id qca_spi_id[] = {
+	{ "qca7000", 0 },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(spi, qca_spi_id);
+
+static struct spi_driver qca_spi_driver = {
+	.driver	= {
+		.name	= QCASPI_DRV_NAME,
+		.owner	= THIS_MODULE,
+		.of_match_table = qca_spi_of_match,
+	},
+	.id_table = qca_spi_id,
+	.probe    = qca_spi_probe,
+	.remove   = qca_spi_remove,
+};
+module_spi_driver(qca_spi_driver);
+
+MODULE_DESCRIPTION("Qualcomm Atheros SPI Driver");
+MODULE_AUTHOR("Qualcomm Atheros Communications");
+MODULE_AUTHOR("Stefan Wahren <stefan.wahren@i2se.com>");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_VERSION(QCASPI_DRV_VERSION);
diff --git a/drivers/net/ethernet/qualcomm/qca_spi.h b/drivers/net/ethernet/qualcomm/qca_spi.h
new file mode 100644
index 000000000000..6e31a0e744a4
--- /dev/null
+++ b/drivers/net/ethernet/qualcomm/qca_spi.h
@@ -0,0 +1,114 @@
+/*
+ *   Copyright (c) 2011, 2012, Qualcomm Atheros Communications Inc.
+ *   Copyright (c) 2014, I2SE GmbH
+ *
+ *   Permission to use, copy, modify, and/or distribute this software
+ *   for any purpose with or without fee is hereby granted, provided
+ *   that the above copyright notice and this permission notice appear
+ *   in all copies.
+ *
+ *   THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL
+ *   WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED
+ *   WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL
+ *   THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR
+ *   CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
+ *   LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
+ *   NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ *   CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/*   Qualcomm Atheros SPI register definition.
+ *
+ *   This module is designed to define the Qualcomm Atheros SPI register
+ *   placeholders;
+ */
+
+#ifndef _QCA_SPI_H
+#define _QCA_SPI_H
+
+#include <linux/netdevice.h>
+#include <linux/sched.h>
+#include <linux/skbuff.h>
+#include <linux/spi/spi.h>
+#include <linux/types.h>
+
+#include "qca_framing.h"
+
+#define QCASPI_DRV_VERSION "0.2.7-i"
+#define QCASPI_DRV_NAME    "qcaspi"
+
+#define QCASPI_GOOD_SIGNATURE 0xAA55
+
+#define TX_RING_MAX_LEN 10
+#define TX_RING_MIN_LEN 2
+
+/* sync related constants */
+#define QCASPI_SYNC_UNKNOWN 0
+#define QCASPI_SYNC_RESET   1
+#define QCASPI_SYNC_READY   2
+
+#define QCASPI_RESET_TIMEOUT 10
+
+/* sync events */
+#define QCASPI_EVENT_UPDATE 0
+#define QCASPI_EVENT_CPUON  1
+
+struct tx_ring {
+	struct sk_buff *skb[TX_RING_MAX_LEN];
+	u16 head;
+	u16 tail;
+	u16 size;
+	u16 count;
+};
+
+struct qcaspi_stats {
+	u64 trig_reset;
+	u64 device_reset;
+	u64 reset_timeout;
+	u64 read_err;
+	u64 write_err;
+	u64 read_buf_err;
+	u64 write_buf_err;
+	u64 out_of_mem;
+	u64 write_buf_miss;
+	u64 ring_full;
+	u64 spi_err;
+};
+
+struct qcaspi {
+	struct net_device *net_dev;
+	struct spi_device *spi_dev;
+	struct task_struct *spi_thread;
+
+	struct tx_ring txr;
+	struct qcaspi_stats stats;
+
+	struct spi_message spi_msg1;
+	struct spi_message spi_msg2;
+	struct spi_transfer spi_xfer1;
+	struct spi_transfer spi_xfer2[2];
+
+	u8 *rx_buffer;
+	u32 buffer_size;
+	u8 sync;
+
+	struct qcafrm_handle frm_handle;
+	struct sk_buff *rx_skb;
+
+	unsigned int intr_req;
+	unsigned int intr_svc;
+
+#ifdef CONFIG_DEBUG_FS
+	struct dentry *device_root;
+#endif
+
+	/* user configurable options */
+	u32 clkspeed;
+	u8 legacy_mode;
+	u16 burst_len;
+};
+
+int qcaspi_netdev_open(struct net_device *dev);
+int qcaspi_netdev_close(struct net_device *dev);
+
+#endif /* _QCA_SPI_H */
diff --git a/drivers/net/ethernet/realtek/r8169.c b/drivers/net/ethernet/realtek/r8169.c
index 91652e7235e4..cf154f74cba1 100644
--- a/drivers/net/ethernet/realtek/r8169.c
+++ b/drivers/net/ethernet/realtek/r8169.c
@@ -52,6 +52,10 @@
 #define FIRMWARE_8106E_2	"rtl_nic/rtl8106e-2.fw"
 #define FIRMWARE_8168G_2	"rtl_nic/rtl8168g-2.fw"
 #define FIRMWARE_8168G_3	"rtl_nic/rtl8168g-3.fw"
+#define FIRMWARE_8168H_1	"rtl_nic/rtl8168h-1.fw"
+#define FIRMWARE_8168H_2	"rtl_nic/rtl8168h-2.fw"
+#define FIRMWARE_8107E_1	"rtl_nic/rtl8107e-1.fw"
+#define FIRMWARE_8107E_2	"rtl_nic/rtl8107e-2.fw"
 
 #ifdef RTL8169_DEBUG
 #define assert(expr) \
@@ -147,6 +151,13 @@ enum mac_version {
 	RTL_GIGA_MAC_VER_42,
 	RTL_GIGA_MAC_VER_43,
 	RTL_GIGA_MAC_VER_44,
+	RTL_GIGA_MAC_VER_45,
+	RTL_GIGA_MAC_VER_46,
+	RTL_GIGA_MAC_VER_47,
+	RTL_GIGA_MAC_VER_48,
+	RTL_GIGA_MAC_VER_49,
+	RTL_GIGA_MAC_VER_50,
+	RTL_GIGA_MAC_VER_51,
 	RTL_GIGA_MAC_NONE   = 0xff,
 };
 
@@ -282,6 +293,27 @@ static const struct {
 	[RTL_GIGA_MAC_VER_44] =
 		_R("RTL8411",		RTL_TD_1, FIRMWARE_8411_2,
 							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_45] =
+		_R("RTL8168h/8111h",	RTL_TD_1, FIRMWARE_8168H_1,
+							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_46] =
+		_R("RTL8168h/8111h",	RTL_TD_1, FIRMWARE_8168H_2,
+							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_47] =
+		_R("RTL8107e",		RTL_TD_1, FIRMWARE_8107E_1,
+							JUMBO_1K, false),
+	[RTL_GIGA_MAC_VER_48] =
+		_R("RTL8107e",		RTL_TD_1, FIRMWARE_8107E_2,
+							JUMBO_1K, false),
+	[RTL_GIGA_MAC_VER_49] =
+		_R("RTL8168ep/8111ep",	RTL_TD_1, NULL,
+							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_50] =
+		_R("RTL8168ep/8111ep",	RTL_TD_1, NULL,
+							JUMBO_9K, false),
+	[RTL_GIGA_MAC_VER_51] =
+		_R("RTL8168ep/8111ep",	RTL_TD_1, NULL,
+							JUMBO_9K, false),
 };
 #undef _R
 
@@ -380,6 +412,10 @@ enum rtl_registers {
 	FuncEvent	= 0xf0,
 	FuncEventMask	= 0xf4,
 	FuncPresetState	= 0xf8,
+	IBCR0           = 0xf8,
+	IBCR2           = 0xf9,
+	IBIMR0          = 0xfa,
+	IBISR0          = 0xfb,
 	FuncForceEvent	= 0xfc,
 };
 
@@ -410,6 +446,7 @@ enum rtl8168_8101_registers {
 #define	EPHYAR_DATA_MASK		0xffff
 	DLLPR			= 0xd0,
 #define	PFM_EN				(1 << 6)
+#define	TX_10M_PS_EN			(1 << 7)
 	DBG_REG			= 0xd1,
 #define	FIX_NAK_1			(1 << 4)
 #define	FIX_NAK_2			(1 << 3)
@@ -429,6 +466,8 @@ enum rtl8168_8101_registers {
 #define	EFUSEAR_REG_MASK		0x03ff
 #define	EFUSEAR_REG_SHIFT		8
 #define	EFUSEAR_DATA_MASK		0xff
+	MISC_1			= 0xf2,
+#define	PFM_D3COLD_EN			(1 << 6)
 };
 
 enum rtl8168_registers {
@@ -444,9 +483,11 @@ enum rtl8168_registers {
 #define ERIAR_EXGMAC			(0x00 << ERIAR_TYPE_SHIFT)
 #define ERIAR_MSIX			(0x01 << ERIAR_TYPE_SHIFT)
 #define ERIAR_ASF			(0x02 << ERIAR_TYPE_SHIFT)
+#define ERIAR_OOB			(0x02 << ERIAR_TYPE_SHIFT)
 #define ERIAR_MASK_SHIFT		12
 #define ERIAR_MASK_0001			(0x1 << ERIAR_MASK_SHIFT)
 #define ERIAR_MASK_0011			(0x3 << ERIAR_MASK_SHIFT)
+#define ERIAR_MASK_0100			(0x4 << ERIAR_MASK_SHIFT)
 #define ERIAR_MASK_0101			(0x5 << ERIAR_MASK_SHIFT)
 #define ERIAR_MASK_1111			(0xf << ERIAR_MASK_SHIFT)
 	EPHY_RXER_NUM		= 0x7c,
@@ -598,6 +639,9 @@ enum rtl_register_content {
 
 	/* DumpCounterCommand */
 	CounterDump	= 0x8,
+
+	/* magic enable v2 */
+	MagicPacket_v2	= (1 << 16),	/* Wake up when receives a Magic Packet */
 };
 
 enum rtl_desc_bit {
@@ -823,6 +867,10 @@ MODULE_FIRMWARE(FIRMWARE_8106E_1);
 MODULE_FIRMWARE(FIRMWARE_8106E_2);
 MODULE_FIRMWARE(FIRMWARE_8168G_2);
 MODULE_FIRMWARE(FIRMWARE_8168G_3);
+MODULE_FIRMWARE(FIRMWARE_8168H_1);
+MODULE_FIRMWARE(FIRMWARE_8168H_2);
+MODULE_FIRMWARE(FIRMWARE_8107E_1);
+MODULE_FIRMWARE(FIRMWARE_8107E_2);
 
 static void rtl_lock_work(struct rtl8169_private *tp)
 {
@@ -904,93 +952,6 @@ static const struct rtl_cond name = {			\
 							\
 static bool name ## _check(struct rtl8169_private *tp)
 
-DECLARE_RTL_COND(rtl_ocpar_cond)
-{
-	void __iomem *ioaddr = tp->mmio_addr;
-
-	return RTL_R32(OCPAR) & OCPAR_FLAG;
-}
-
-static u32 ocp_read(struct rtl8169_private *tp, u8 mask, u16 reg)
-{
-	void __iomem *ioaddr = tp->mmio_addr;
-
-	RTL_W32(OCPAR, ((u32)mask & 0x0f) << 12 | (reg & 0x0fff));
-
-	return rtl_udelay_loop_wait_high(tp, &rtl_ocpar_cond, 100, 20) ?
-		RTL_R32(OCPDR) : ~0;
-}
-
-static void ocp_write(struct rtl8169_private *tp, u8 mask, u16 reg, u32 data)
-{
-	void __iomem *ioaddr = tp->mmio_addr;
-
-	RTL_W32(OCPDR, data);
-	RTL_W32(OCPAR, OCPAR_FLAG | ((u32)mask & 0x0f) << 12 | (reg & 0x0fff));
-
-	rtl_udelay_loop_wait_low(tp, &rtl_ocpar_cond, 100, 20);
-}
-
-DECLARE_RTL_COND(rtl_eriar_cond)
-{
-	void __iomem *ioaddr = tp->mmio_addr;
-
-	return RTL_R32(ERIAR) & ERIAR_FLAG;
-}
-
-static void rtl8168_oob_notify(struct rtl8169_private *tp, u8 cmd)
-{
-	void __iomem *ioaddr = tp->mmio_addr;
-
-	RTL_W8(ERIDR, cmd);
-	RTL_W32(ERIAR, 0x800010e8);
-	msleep(2);
-
-	if (!rtl_udelay_loop_wait_low(tp, &rtl_eriar_cond, 100, 5))
-		return;
-
-	ocp_write(tp, 0x1, 0x30, 0x00000001);
-}
-
-#define OOB_CMD_RESET		0x00
-#define OOB_CMD_DRIVER_START	0x05
-#define OOB_CMD_DRIVER_STOP	0x06
-
-static u16 rtl8168_get_ocp_reg(struct rtl8169_private *tp)
-{
-	return (tp->mac_version == RTL_GIGA_MAC_VER_31) ? 0xb8 : 0x10;
-}
-
-DECLARE_RTL_COND(rtl_ocp_read_cond)
-{
-	u16 reg;
-
-	reg = rtl8168_get_ocp_reg(tp);
-
-	return ocp_read(tp, 0x0f, reg) & 0x00000800;
-}
-
-static void rtl8168_driver_start(struct rtl8169_private *tp)
-{
-	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_START);
-
-	rtl_msleep_loop_wait_high(tp, &rtl_ocp_read_cond, 10, 10);
-}
-
-static void rtl8168_driver_stop(struct rtl8169_private *tp)
-{
-	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_STOP);
-
-	rtl_msleep_loop_wait_low(tp, &rtl_ocp_read_cond, 10, 10);
-}
-
-static int r8168dp_check_dash(struct rtl8169_private *tp)
-{
-	u16 reg = rtl8168_get_ocp_reg(tp);
-
-	return (ocp_read(tp, 0x0f, reg) & 0x00008000) ? 1 : 0;
-}
-
 static bool rtl_ocp_reg_failure(struct rtl8169_private *tp, u32 reg)
 {
 	if (reg & 0xffff0001) {
@@ -1132,6 +1093,13 @@ static int r8169_mdio_read(struct rtl8169_private *tp, int reg)
 	return value;
 }
 
+DECLARE_RTL_COND(rtl_ocpar_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(OCPAR) & OCPAR_FLAG;
+}
+
 static void r8168dp_1_mdio_access(struct rtl8169_private *tp, int reg, u32 data)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
@@ -1215,12 +1183,12 @@ static void rtl_patchphy(struct rtl8169_private *tp, int reg_addr, int value)
 	rtl_writephy(tp, reg_addr, rtl_readphy(tp, reg_addr) | value);
 }
 
-static void rtl_w1w0_phy(struct rtl8169_private *tp, int reg_addr, int p, int m)
+static void rtl_w0w1_phy(struct rtl8169_private *tp, int reg_addr, int p, int m)
 {
 	int val;
 
 	val = rtl_readphy(tp, reg_addr);
-	rtl_writephy(tp, reg_addr, (val | p) & ~m);
+	rtl_writephy(tp, reg_addr, (val & ~m) | p);
 }
 
 static void rtl_mdio_write(struct net_device *dev, int phy_id, int location,
@@ -1267,6 +1235,13 @@ static u16 rtl_ephy_read(struct rtl8169_private *tp, int reg_addr)
 		RTL_R32(EPHYAR) & EPHYAR_DATA_MASK : ~0;
 }
 
+DECLARE_RTL_COND(rtl_eriar_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R32(ERIAR) & ERIAR_FLAG;
+}
+
 static void rtl_eri_write(struct rtl8169_private *tp, int addr, u32 mask,
 			  u32 val, int type)
 {
@@ -1289,7 +1264,7 @@ static u32 rtl_eri_read(struct rtl8169_private *tp, int addr, int type)
 		RTL_R32(ERIDR) : ~0;
 }
 
-static void rtl_w1w0_eri(struct rtl8169_private *tp, int addr, u32 mask, u32 p,
+static void rtl_w0w1_eri(struct rtl8169_private *tp, int addr, u32 mask, u32 p,
 			 u32 m, int type)
 {
 	u32 val;
@@ -1298,6 +1273,208 @@ static void rtl_w1w0_eri(struct rtl8169_private *tp, int addr, u32 mask, u32 p,
 	rtl_eri_write(tp, addr, mask, (val & ~m) | p, type);
 }
 
+static u32 r8168dp_ocp_read(struct rtl8169_private *tp, u8 mask, u16 reg)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	RTL_W32(OCPAR, ((u32)mask & 0x0f) << 12 | (reg & 0x0fff));
+	return rtl_udelay_loop_wait_high(tp, &rtl_ocpar_cond, 100, 20) ?
+		RTL_R32(OCPDR) : ~0;
+}
+
+static u32 r8168ep_ocp_read(struct rtl8169_private *tp, u8 mask, u16 reg)
+{
+	return rtl_eri_read(tp, reg, ERIAR_OOB);
+}
+
+static u32 ocp_read(struct rtl8169_private *tp, u8 mask, u16 reg)
+{
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_27:
+	case RTL_GIGA_MAC_VER_28:
+	case RTL_GIGA_MAC_VER_31:
+		return r8168dp_ocp_read(tp, mask, reg);
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		return r8168ep_ocp_read(tp, mask, reg);
+	default:
+		BUG();
+		return ~0;
+	}
+}
+
+static void r8168dp_ocp_write(struct rtl8169_private *tp, u8 mask, u16 reg,
+			      u32 data)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	RTL_W32(OCPDR, data);
+	RTL_W32(OCPAR, OCPAR_FLAG | ((u32)mask & 0x0f) << 12 | (reg & 0x0fff));
+	rtl_udelay_loop_wait_low(tp, &rtl_ocpar_cond, 100, 20);
+}
+
+static void r8168ep_ocp_write(struct rtl8169_private *tp, u8 mask, u16 reg,
+			      u32 data)
+{
+	rtl_eri_write(tp, reg, ((u32)mask & 0x0f) << ERIAR_MASK_SHIFT,
+		      data, ERIAR_OOB);
+}
+
+static void ocp_write(struct rtl8169_private *tp, u8 mask, u16 reg, u32 data)
+{
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_27:
+	case RTL_GIGA_MAC_VER_28:
+	case RTL_GIGA_MAC_VER_31:
+		r8168dp_ocp_write(tp, mask, reg, data);
+		break;
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		r8168ep_ocp_write(tp, mask, reg, data);
+		break;
+	default:
+		BUG();
+		break;
+	}
+}
+
+static void rtl8168_oob_notify(struct rtl8169_private *tp, u8 cmd)
+{
+	rtl_eri_write(tp, 0xe8, ERIAR_MASK_0001, cmd, ERIAR_EXGMAC);
+
+	ocp_write(tp, 0x1, 0x30, 0x00000001);
+}
+
+#define OOB_CMD_RESET		0x00
+#define OOB_CMD_DRIVER_START	0x05
+#define OOB_CMD_DRIVER_STOP	0x06
+
+static u16 rtl8168_get_ocp_reg(struct rtl8169_private *tp)
+{
+	return (tp->mac_version == RTL_GIGA_MAC_VER_31) ? 0xb8 : 0x10;
+}
+
+DECLARE_RTL_COND(rtl_ocp_read_cond)
+{
+	u16 reg;
+
+	reg = rtl8168_get_ocp_reg(tp);
+
+	return ocp_read(tp, 0x0f, reg) & 0x00000800;
+}
+
+DECLARE_RTL_COND(rtl_ep_ocp_read_cond)
+{
+	return ocp_read(tp, 0x0f, 0x124) & 0x00000001;
+}
+
+DECLARE_RTL_COND(rtl_ocp_tx_cond)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	return RTL_R8(IBISR0) & 0x02;
+}
+
+static void rtl8168dp_driver_start(struct rtl8169_private *tp)
+{
+	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_START);
+	rtl_msleep_loop_wait_high(tp, &rtl_ocp_read_cond, 10, 10);
+}
+
+static void rtl8168ep_driver_start(struct rtl8169_private *tp)
+{
+	ocp_write(tp, 0x01, 0x180, OOB_CMD_DRIVER_START);
+	ocp_write(tp, 0x01, 0x30, ocp_read(tp, 0x01, 0x30) | 0x01);
+	rtl_msleep_loop_wait_high(tp, &rtl_ep_ocp_read_cond, 10, 10);
+}
+
+static void rtl8168_driver_start(struct rtl8169_private *tp)
+{
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_27:
+	case RTL_GIGA_MAC_VER_28:
+	case RTL_GIGA_MAC_VER_31:
+		rtl8168dp_driver_start(tp);
+		break;
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		rtl8168ep_driver_start(tp);
+		break;
+	default:
+		BUG();
+		break;
+	}
+}
+
+static void rtl8168dp_driver_stop(struct rtl8169_private *tp)
+{
+	rtl8168_oob_notify(tp, OOB_CMD_DRIVER_STOP);
+	rtl_msleep_loop_wait_low(tp, &rtl_ocp_read_cond, 10, 10);
+}
+
+static void rtl8168ep_driver_stop(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+
+	RTL_W8(IBCR2, RTL_R8(IBCR2) & ~0x01);
+	rtl_msleep_loop_wait_low(tp, &rtl_ocp_tx_cond, 50, 2000);
+	RTL_W8(IBISR0, RTL_R8(IBISR0) | 0x20);
+	RTL_W8(IBCR0, RTL_R8(IBCR0) & ~0x01);
+	ocp_write(tp, 0x01, 0x180, OOB_CMD_DRIVER_STOP);
+	ocp_write(tp, 0x01, 0x30, ocp_read(tp, 0x01, 0x30) | 0x01);
+	rtl_msleep_loop_wait_low(tp, &rtl_ep_ocp_read_cond, 10, 10);
+}
+
+static void rtl8168_driver_stop(struct rtl8169_private *tp)
+{
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_27:
+	case RTL_GIGA_MAC_VER_28:
+	case RTL_GIGA_MAC_VER_31:
+		rtl8168dp_driver_stop(tp);
+		break;
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		rtl8168ep_driver_stop(tp);
+		break;
+	default:
+		BUG();
+		break;
+	}
+}
+
+static int r8168dp_check_dash(struct rtl8169_private *tp)
+{
+	u16 reg = rtl8168_get_ocp_reg(tp);
+
+	return (ocp_read(tp, 0x0f, reg) & 0x00008000) ? 1 : 0;
+}
+
+static int r8168ep_check_dash(struct rtl8169_private *tp)
+{
+	return (ocp_read(tp, 0x0f, 0x128) & 0x00000001) ? 1 : 0;
+}
+
+static int r8168_check_dash(struct rtl8169_private *tp)
+{
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_27:
+	case RTL_GIGA_MAC_VER_28:
+	case RTL_GIGA_MAC_VER_31:
+		return r8168dp_check_dash(tp);
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		return r8168ep_check_dash(tp);
+	default:
+		return 0;
+	}
+}
+
 struct exgmac_reg {
 	u16 addr;
 	u16 mask;
@@ -1442,9 +1619,9 @@ static void rtl_link_chg_patch(struct rtl8169_private *tp)
 				      ERIAR_EXGMAC);
 		}
 		/* Reset packet filter */
-		rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01,
+		rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01,
 			     ERIAR_EXGMAC);
-		rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00,
+		rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00,
 			     ERIAR_EXGMAC);
 	} else if (tp->mac_version == RTL_GIGA_MAC_VER_35 ||
 		   tp->mac_version == RTL_GIGA_MAC_VER_36) {
@@ -1514,8 +1691,32 @@ static u32 __rtl8169_get_wol(struct rtl8169_private *tp)
 	options = RTL_R8(Config3);
 	if (options & LinkUp)
 		wolopts |= WAKE_PHY;
-	if (options & MagicPacket)
-		wolopts |= WAKE_MAGIC;
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_34:
+	case RTL_GIGA_MAC_VER_35:
+	case RTL_GIGA_MAC_VER_36:
+	case RTL_GIGA_MAC_VER_37:
+	case RTL_GIGA_MAC_VER_38:
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
+	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		if (rtl_eri_read(tp, 0xdc, ERIAR_EXGMAC) & MagicPacket_v2)
+			wolopts |= WAKE_MAGIC;
+		break;
+	default:
+		if (options & MagicPacket)
+			wolopts |= WAKE_MAGIC;
+		break;
+	}
 
 	options = RTL_R8(Config5);
 	if (options & UWF)
@@ -1543,24 +1744,63 @@ static void rtl8169_get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 static void __rtl8169_set_wol(struct rtl8169_private *tp, u32 wolopts)
 {
 	void __iomem *ioaddr = tp->mmio_addr;
-	unsigned int i;
+	unsigned int i, tmp;
 	static const struct {
 		u32 opt;
 		u16 reg;
 		u8  mask;
 	} cfg[] = {
 		{ WAKE_PHY,   Config3, LinkUp },
-		{ WAKE_MAGIC, Config3, MagicPacket },
 		{ WAKE_UCAST, Config5, UWF },
 		{ WAKE_BCAST, Config5, BWF },
 		{ WAKE_MCAST, Config5, MWF },
-		{ WAKE_ANY,   Config5, LanWake }
+		{ WAKE_ANY,   Config5, LanWake },
+		{ WAKE_MAGIC, Config3, MagicPacket }
 	};
 	u8 options;
 
 	RTL_W8(Cfg9346, Cfg9346_Unlock);
 
-	for (i = 0; i < ARRAY_SIZE(cfg); i++) {
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_34:
+	case RTL_GIGA_MAC_VER_35:
+	case RTL_GIGA_MAC_VER_36:
+	case RTL_GIGA_MAC_VER_37:
+	case RTL_GIGA_MAC_VER_38:
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
+	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		tmp = ARRAY_SIZE(cfg) - 1;
+		if (wolopts & WAKE_MAGIC)
+			rtl_w0w1_eri(tp,
+				     0x0dc,
+				     ERIAR_MASK_0100,
+				     MagicPacket_v2,
+				     0x0000,
+				     ERIAR_EXGMAC);
+		else
+			rtl_w0w1_eri(tp,
+				     0x0dc,
+				     ERIAR_MASK_0100,
+				     0x0000,
+				     MagicPacket_v2,
+				     ERIAR_EXGMAC);
+		break;
+	default:
+		tmp = ARRAY_SIZE(cfg);
+		break;
+	}
+
+	for (i = 0; i < tmp; i++) {
 		options = RTL_R8(cfg[i].reg) & ~cfg[i].mask;
 		if (wolopts & cfg[i].opt)
 			options |= cfg[i].mask;
@@ -1783,33 +2023,31 @@ static void __rtl8169_set_features(struct net_device *dev,
 				   netdev_features_t features)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
-	netdev_features_t changed = features ^ dev->features;
 	void __iomem *ioaddr = tp->mmio_addr;
+	u32 rx_config;
 
-	if (!(changed & (NETIF_F_RXALL | NETIF_F_RXCSUM |
-			 NETIF_F_HW_VLAN_CTAG_RX)))
-		return;
+	rx_config = RTL_R32(RxConfig);
+	if (features & NETIF_F_RXALL)
+		rx_config |= (AcceptErr | AcceptRunt);
+	else
+		rx_config &= ~(AcceptErr | AcceptRunt);
 
-	if (changed & (NETIF_F_RXCSUM | NETIF_F_HW_VLAN_CTAG_RX)) {
-		if (features & NETIF_F_RXCSUM)
-			tp->cp_cmd |= RxChkSum;
-		else
-			tp->cp_cmd &= ~RxChkSum;
+	RTL_W32(RxConfig, rx_config);
 
-		if (dev->features & NETIF_F_HW_VLAN_CTAG_RX)
-			tp->cp_cmd |= RxVlan;
-		else
-			tp->cp_cmd &= ~RxVlan;
+	if (features & NETIF_F_RXCSUM)
+		tp->cp_cmd |= RxChkSum;
+	else
+		tp->cp_cmd &= ~RxChkSum;
 
-		RTL_W16(CPlusCmd, tp->cp_cmd);
-		RTL_R16(CPlusCmd);
-	}
-	if (changed & NETIF_F_RXALL) {
-		int tmp = (RTL_R32(RxConfig) & ~(AcceptErr | AcceptRunt));
-		if (features & NETIF_F_RXALL)
-			tmp |= (AcceptErr | AcceptRunt);
-		RTL_W32(RxConfig, tmp);
-	}
+	if (features & NETIF_F_HW_VLAN_CTAG_RX)
+		tp->cp_cmd |= RxVlan;
+	else
+		tp->cp_cmd &= ~RxVlan;
+
+	tp->cp_cmd |= RTL_R16(CPlusCmd) & ~(RxVlan | RxChkSum);
+
+	RTL_W16(CPlusCmd, tp->cp_cmd);
+	RTL_R16(CPlusCmd);
 }
 
 static int rtl8169_set_features(struct net_device *dev,
@@ -1817,8 +2055,11 @@ static int rtl8169_set_features(struct net_device *dev,
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
 
+	features &= NETIF_F_RXALL | NETIF_F_RXCSUM | NETIF_F_HW_VLAN_CTAG_RX;
+
 	rtl_lock_work(tp);
-	__rtl8169_set_features(dev, features);
+	if (features ^ dev->features)
+		__rtl8169_set_features(dev, features);
 	rtl_unlock_work(tp);
 
 	return 0;
@@ -2044,6 +2285,15 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		u32 val;
 		int mac_version;
 	} mac_info[] = {
+		/* 8168EP family. */
+		{ 0x7cf00000, 0x50200000,	RTL_GIGA_MAC_VER_51 },
+		{ 0x7cf00000, 0x50100000,	RTL_GIGA_MAC_VER_50 },
+		{ 0x7cf00000, 0x50000000,	RTL_GIGA_MAC_VER_49 },
+
+		/* 8168H family. */
+		{ 0x7cf00000, 0x54100000,	RTL_GIGA_MAC_VER_46 },
+		{ 0x7cf00000, 0x54000000,	RTL_GIGA_MAC_VER_45 },
+
 		/* 8168G family. */
 		{ 0x7cf00000, 0x5c800000,	RTL_GIGA_MAC_VER_44 },
 		{ 0x7cf00000, 0x50900000,	RTL_GIGA_MAC_VER_42 },
@@ -2139,6 +2389,14 @@ static void rtl8169_get_mac_version(struct rtl8169_private *tp,
 		tp->mac_version = tp->mii.supports_gmii ?
 				  RTL_GIGA_MAC_VER_42 :
 				  RTL_GIGA_MAC_VER_43;
+	} else if (tp->mac_version == RTL_GIGA_MAC_VER_45) {
+		tp->mac_version = tp->mii.supports_gmii ?
+				  RTL_GIGA_MAC_VER_45 :
+				  RTL_GIGA_MAC_VER_47;
+	} else if (tp->mac_version == RTL_GIGA_MAC_VER_46) {
+		tp->mac_version = tp->mii.supports_gmii ?
+				  RTL_GIGA_MAC_VER_46 :
+				  RTL_GIGA_MAC_VER_48;
 	}
 }
 
@@ -2800,8 +3058,8 @@ static void rtl8168d_1_hw_phy_config(struct rtl8169_private *tp)
 	 * Fine Tune Switching regulator parameter
 	 */
 	rtl_writephy(tp, 0x1f, 0x0002);
-	rtl_w1w0_phy(tp, 0x0b, 0x0010, 0x00ef);
-	rtl_w1w0_phy(tp, 0x0c, 0xa200, 0x5d00);
+	rtl_w0w1_phy(tp, 0x0b, 0x0010, 0x00ef);
+	rtl_w0w1_phy(tp, 0x0c, 0xa200, 0x5d00);
 
 	if (rtl8168d_efuse_read(tp, 0x01) == 0xb1) {
 		static const struct phy_reg phy_reg_init[] = {
@@ -2850,8 +3108,8 @@ static void rtl8168d_1_hw_phy_config(struct rtl8169_private *tp)
 
 	/* Fine tune PLL performance */
 	rtl_writephy(tp, 0x1f, 0x0002);
-	rtl_w1w0_phy(tp, 0x02, 0x0100, 0x0600);
-	rtl_w1w0_phy(tp, 0x03, 0x0000, 0xe000);
+	rtl_w0w1_phy(tp, 0x02, 0x0100, 0x0600);
+	rtl_w0w1_phy(tp, 0x03, 0x0000, 0xe000);
 
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x001b);
@@ -2948,8 +3206,8 @@ static void rtl8168d_2_hw_phy_config(struct rtl8169_private *tp)
 
 	/* Fine tune PLL performance */
 	rtl_writephy(tp, 0x1f, 0x0002);
-	rtl_w1w0_phy(tp, 0x02, 0x0100, 0x0600);
-	rtl_w1w0_phy(tp, 0x03, 0x0000, 0xe000);
+	rtl_w0w1_phy(tp, 0x02, 0x0100, 0x0600);
+	rtl_w0w1_phy(tp, 0x03, 0x0000, 0xe000);
 
 	/* Switching regulator Slew rate */
 	rtl_writephy(tp, 0x1f, 0x0002);
@@ -3077,32 +3335,32 @@ static void rtl8168e_1_hw_phy_config(struct rtl8169_private *tp)
 	/* DCO enable for 10M IDLE Power */
 	rtl_writephy(tp, 0x1f, 0x0007);
 	rtl_writephy(tp, 0x1e, 0x0023);
-	rtl_w1w0_phy(tp, 0x17, 0x0006, 0x0000);
+	rtl_w0w1_phy(tp, 0x17, 0x0006, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* For impedance matching */
 	rtl_writephy(tp, 0x1f, 0x0002);
-	rtl_w1w0_phy(tp, 0x08, 0x8000, 0x7f00);
+	rtl_w0w1_phy(tp, 0x08, 0x8000, 0x7f00);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* PHY auto speed down */
 	rtl_writephy(tp, 0x1f, 0x0007);
 	rtl_writephy(tp, 0x1e, 0x002d);
-	rtl_w1w0_phy(tp, 0x18, 0x0050, 0x0000);
+	rtl_w0w1_phy(tp, 0x18, 0x0050, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
-	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0x0000);
 
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b86);
-	rtl_w1w0_phy(tp, 0x06, 0x0001, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x0001, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x2000);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x2000);
 	rtl_writephy(tp, 0x1f, 0x0007);
 	rtl_writephy(tp, 0x1e, 0x0020);
-	rtl_w1w0_phy(tp, 0x15, 0x0000, 0x1100);
+	rtl_w0w1_phy(tp, 0x15, 0x0000, 0x1100);
 	rtl_writephy(tp, 0x1f, 0x0006);
 	rtl_writephy(tp, 0x00, 0x5a00);
 	rtl_writephy(tp, 0x1f, 0x0000);
@@ -3166,39 +3424,39 @@ static void rtl8168e_2_hw_phy_config(struct rtl8169_private *tp)
 	/* For 4-corner performance improve */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b80);
-	rtl_w1w0_phy(tp, 0x17, 0x0006, 0x0000);
+	rtl_w0w1_phy(tp, 0x17, 0x0006, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* PHY auto speed down */
 	rtl_writephy(tp, 0x1f, 0x0004);
 	rtl_writephy(tp, 0x1f, 0x0007);
 	rtl_writephy(tp, 0x1e, 0x002d);
-	rtl_w1w0_phy(tp, 0x18, 0x0010, 0x0000);
+	rtl_w0w1_phy(tp, 0x18, 0x0010, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0002);
 	rtl_writephy(tp, 0x1f, 0x0000);
-	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0x0000);
 
 	/* improve 10M EEE waveform */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b86);
-	rtl_w1w0_phy(tp, 0x06, 0x0001, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x0001, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* Improve 2-pair detection performance */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
-	rtl_w1w0_phy(tp, 0x06, 0x4000, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x4000, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* EEE setting */
-	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_1111, 0x0000, 0x0003, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x1b0, ERIAR_MASK_1111, 0x0000, 0x0003, ERIAR_EXGMAC);
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x2000);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x2000);
 	rtl_writephy(tp, 0x1f, 0x0004);
 	rtl_writephy(tp, 0x1f, 0x0007);
 	rtl_writephy(tp, 0x1e, 0x0020);
-	rtl_w1w0_phy(tp, 0x15, 0x0000, 0x0100);
+	rtl_w0w1_phy(tp, 0x15, 0x0000, 0x0100);
 	rtl_writephy(tp, 0x1f, 0x0002);
 	rtl_writephy(tp, 0x1f, 0x0000);
 	rtl_writephy(tp, 0x0d, 0x0007);
@@ -3209,8 +3467,8 @@ static void rtl8168e_2_hw_phy_config(struct rtl8169_private *tp)
 
 	/* Green feature */
 	rtl_writephy(tp, 0x1f, 0x0003);
-	rtl_w1w0_phy(tp, 0x19, 0x0000, 0x0001);
-	rtl_w1w0_phy(tp, 0x10, 0x0000, 0x0400);
+	rtl_w0w1_phy(tp, 0x19, 0x0000, 0x0001);
+	rtl_w0w1_phy(tp, 0x10, 0x0000, 0x0400);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* Broken BIOS workaround: feed GigaMAC registers with MAC address. */
@@ -3222,20 +3480,20 @@ static void rtl8168f_hw_phy_config(struct rtl8169_private *tp)
 	/* For 4-corner performance improve */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b80);
-	rtl_w1w0_phy(tp, 0x06, 0x0006, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x0006, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* PHY auto speed down */
 	rtl_writephy(tp, 0x1f, 0x0007);
 	rtl_writephy(tp, 0x1e, 0x002d);
-	rtl_w1w0_phy(tp, 0x18, 0x0010, 0x0000);
+	rtl_w0w1_phy(tp, 0x18, 0x0010, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
-	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0x0000);
 
 	/* Improve 10M EEE waveform */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b86);
-	rtl_w1w0_phy(tp, 0x06, 0x0001, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x0001, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
 
@@ -3285,7 +3543,7 @@ static void rtl8168f_1_hw_phy_config(struct rtl8169_private *tp)
 	/* Improve 2-pair detection performance */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
-	rtl_w1w0_phy(tp, 0x06, 0x4000, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x4000, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
 
@@ -3341,7 +3599,7 @@ static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
 	/* Improve 2-pair detection performance */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
-	rtl_w1w0_phy(tp, 0x06, 0x4000, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x4000, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	rtl_writephy_batch(tp, phy_reg_init, ARRAY_SIZE(phy_reg_init));
@@ -3349,36 +3607,36 @@ static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
 	/* Modify green table for giga */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b54);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0800);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x0800);
 	rtl_writephy(tp, 0x05, 0x8b5d);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0800);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x0800);
 	rtl_writephy(tp, 0x05, 0x8a7c);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x0100);
 	rtl_writephy(tp, 0x05, 0x8a7f);
-	rtl_w1w0_phy(tp, 0x06, 0x0100, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x0100, 0x0000);
 	rtl_writephy(tp, 0x05, 0x8a82);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x0100);
 	rtl_writephy(tp, 0x05, 0x8a85);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x0100);
 	rtl_writephy(tp, 0x05, 0x8a88);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x0100);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x0100);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* uc same-seed solution */
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
-	rtl_w1w0_phy(tp, 0x06, 0x8000, 0x0000);
+	rtl_w0w1_phy(tp, 0x06, 0x8000, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0000);
 
 	/* eee setting */
-	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x00, 0x03, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x00, 0x03, ERIAR_EXGMAC);
 	rtl_writephy(tp, 0x1f, 0x0005);
 	rtl_writephy(tp, 0x05, 0x8b85);
-	rtl_w1w0_phy(tp, 0x06, 0x0000, 0x2000);
+	rtl_w0w1_phy(tp, 0x06, 0x0000, 0x2000);
 	rtl_writephy(tp, 0x1f, 0x0004);
 	rtl_writephy(tp, 0x1f, 0x0007);
 	rtl_writephy(tp, 0x1e, 0x0020);
-	rtl_w1w0_phy(tp, 0x15, 0x0000, 0x0100);
+	rtl_w0w1_phy(tp, 0x15, 0x0000, 0x0100);
 	rtl_writephy(tp, 0x1f, 0x0000);
 	rtl_writephy(tp, 0x0d, 0x0007);
 	rtl_writephy(tp, 0x0e, 0x003c);
@@ -3388,8 +3646,8 @@ static void rtl8411_hw_phy_config(struct rtl8169_private *tp)
 
 	/* Green feature */
 	rtl_writephy(tp, 0x1f, 0x0003);
-	rtl_w1w0_phy(tp, 0x19, 0x0000, 0x0001);
-	rtl_w1w0_phy(tp, 0x10, 0x0000, 0x0400);
+	rtl_w0w1_phy(tp, 0x19, 0x0000, 0x0001);
+	rtl_w0w1_phy(tp, 0x10, 0x0000, 0x0400);
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
 
@@ -3400,45 +3658,45 @@ static void rtl8168g_1_hw_phy_config(struct rtl8169_private *tp)
 	rtl_writephy(tp, 0x1f, 0x0a46);
 	if (rtl_readphy(tp, 0x10) & 0x0100) {
 		rtl_writephy(tp, 0x1f, 0x0bcc);
-		rtl_w1w0_phy(tp, 0x12, 0x0000, 0x8000);
+		rtl_w0w1_phy(tp, 0x12, 0x0000, 0x8000);
 	} else {
 		rtl_writephy(tp, 0x1f, 0x0bcc);
-		rtl_w1w0_phy(tp, 0x12, 0x8000, 0x0000);
+		rtl_w0w1_phy(tp, 0x12, 0x8000, 0x0000);
 	}
 
 	rtl_writephy(tp, 0x1f, 0x0a46);
 	if (rtl_readphy(tp, 0x13) & 0x0100) {
 		rtl_writephy(tp, 0x1f, 0x0c41);
-		rtl_w1w0_phy(tp, 0x15, 0x0002, 0x0000);
+		rtl_w0w1_phy(tp, 0x15, 0x0002, 0x0000);
 	} else {
 		rtl_writephy(tp, 0x1f, 0x0c41);
-		rtl_w1w0_phy(tp, 0x15, 0x0000, 0x0002);
+		rtl_w0w1_phy(tp, 0x15, 0x0000, 0x0002);
 	}
 
 	/* Enable PHY auto speed down */
 	rtl_writephy(tp, 0x1f, 0x0a44);
-	rtl_w1w0_phy(tp, 0x11, 0x000c, 0x0000);
+	rtl_w0w1_phy(tp, 0x11, 0x000c, 0x0000);
 
 	rtl_writephy(tp, 0x1f, 0x0bcc);
-	rtl_w1w0_phy(tp, 0x14, 0x0100, 0x0000);
+	rtl_w0w1_phy(tp, 0x14, 0x0100, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0a44);
-	rtl_w1w0_phy(tp, 0x11, 0x00c0, 0x0000);
+	rtl_w0w1_phy(tp, 0x11, 0x00c0, 0x0000);
 	rtl_writephy(tp, 0x1f, 0x0a43);
 	rtl_writephy(tp, 0x13, 0x8084);
-	rtl_w1w0_phy(tp, 0x14, 0x0000, 0x6000);
-	rtl_w1w0_phy(tp, 0x10, 0x1003, 0x0000);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x6000);
+	rtl_w0w1_phy(tp, 0x10, 0x1003, 0x0000);
 
 	/* EEE auto-fallback function */
 	rtl_writephy(tp, 0x1f, 0x0a4b);
-	rtl_w1w0_phy(tp, 0x11, 0x0004, 0x0000);
+	rtl_w0w1_phy(tp, 0x11, 0x0004, 0x0000);
 
 	/* Enable UC LPF tune function */
 	rtl_writephy(tp, 0x1f, 0x0a43);
 	rtl_writephy(tp, 0x13, 0x8012);
-	rtl_w1w0_phy(tp, 0x14, 0x8000, 0x0000);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0x0000);
 
 	rtl_writephy(tp, 0x1f, 0x0c42);
-	rtl_w1w0_phy(tp, 0x11, 0x4000, 0x2000);
+	rtl_w0w1_phy(tp, 0x11, 0x4000, 0x2000);
 
 	/* Improve SWR Efficiency */
 	rtl_writephy(tp, 0x1f, 0x0bcd);
@@ -3454,7 +3712,7 @@ static void rtl8168g_1_hw_phy_config(struct rtl8169_private *tp)
 	/* Check ALDPS bit, disable it if enabled */
 	rtl_writephy(tp, 0x1f, 0x0a43);
 	if (rtl_readphy(tp, 0x10) & 0x0004)
-		rtl_w1w0_phy(tp, 0x10, 0x0000, 0x0004);
+		rtl_w0w1_phy(tp, 0x10, 0x0000, 0x0004);
 
 	rtl_writephy(tp, 0x1f, 0x0000);
 }
@@ -3464,6 +3722,322 @@ static void rtl8168g_2_hw_phy_config(struct rtl8169_private *tp)
 	rtl_apply_firmware(tp);
 }
 
+static void rtl8168h_1_hw_phy_config(struct rtl8169_private *tp)
+{
+	u16 dout_tapbin;
+	u32 data;
+
+	rtl_apply_firmware(tp);
+
+	/* CHN EST parameters adjust - giga master */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x809b);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0xf800);
+	rtl_writephy(tp, 0x13, 0x80a2);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0xff00);
+	rtl_writephy(tp, 0x13, 0x80a4);
+	rtl_w0w1_phy(tp, 0x14, 0x8500, 0xff00);
+	rtl_writephy(tp, 0x13, 0x809c);
+	rtl_w0w1_phy(tp, 0x14, 0xbd00, 0xff00);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* CHN EST parameters adjust - giga slave */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x80ad);
+	rtl_w0w1_phy(tp, 0x14, 0x7000, 0xf800);
+	rtl_writephy(tp, 0x13, 0x80b4);
+	rtl_w0w1_phy(tp, 0x14, 0x5000, 0xff00);
+	rtl_writephy(tp, 0x13, 0x80ac);
+	rtl_w0w1_phy(tp, 0x14, 0x4000, 0xff00);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* CHN EST parameters adjust - fnet */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x808e);
+	rtl_w0w1_phy(tp, 0x14, 0x1200, 0xff00);
+	rtl_writephy(tp, 0x13, 0x8090);
+	rtl_w0w1_phy(tp, 0x14, 0xe500, 0xff00);
+	rtl_writephy(tp, 0x13, 0x8092);
+	rtl_w0w1_phy(tp, 0x14, 0x9f00, 0xff00);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* enable R-tune & PGA-retune function */
+	dout_tapbin = 0;
+	rtl_writephy(tp, 0x1f, 0x0a46);
+	data = rtl_readphy(tp, 0x13);
+	data &= 3;
+	data <<= 2;
+	dout_tapbin |= data;
+	data = rtl_readphy(tp, 0x12);
+	data &= 0xc000;
+	data >>= 14;
+	dout_tapbin |= data;
+	dout_tapbin = ~(dout_tapbin^0x08);
+	dout_tapbin <<= 12;
+	dout_tapbin &= 0xf000;
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x827a);
+	rtl_w0w1_phy(tp, 0x14, dout_tapbin, 0xf000);
+	rtl_writephy(tp, 0x13, 0x827b);
+	rtl_w0w1_phy(tp, 0x14, dout_tapbin, 0xf000);
+	rtl_writephy(tp, 0x13, 0x827c);
+	rtl_w0w1_phy(tp, 0x14, dout_tapbin, 0xf000);
+	rtl_writephy(tp, 0x13, 0x827d);
+	rtl_w0w1_phy(tp, 0x14, dout_tapbin, 0xf000);
+
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x0811);
+	rtl_w0w1_phy(tp, 0x14, 0x0800, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0a42);
+	rtl_w0w1_phy(tp, 0x16, 0x0002, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* enable GPHY 10M */
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w0w1_phy(tp, 0x11, 0x0800, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* SAR ADC performance */
+	rtl_writephy(tp, 0x1f, 0x0bca);
+	rtl_w0w1_phy(tp, 0x17, 0x4000, 0x3000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x803f);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x3000);
+	rtl_writephy(tp, 0x13, 0x8047);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x3000);
+	rtl_writephy(tp, 0x13, 0x804f);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x3000);
+	rtl_writephy(tp, 0x13, 0x8057);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x3000);
+	rtl_writephy(tp, 0x13, 0x805f);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x3000);
+	rtl_writephy(tp, 0x13, 0x8067);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x3000);
+	rtl_writephy(tp, 0x13, 0x806f);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x3000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* disable phy pfm mode */
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x0080);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Check ALDPS bit, disable it if enabled */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	if (rtl_readphy(tp, 0x10) & 0x0004)
+		rtl_w0w1_phy(tp, 0x10, 0x0000, 0x0004);
+
+	rtl_writephy(tp, 0x1f, 0x0000);
+}
+
+static void rtl8168h_2_hw_phy_config(struct rtl8169_private *tp)
+{
+	u16 ioffset_p3, ioffset_p2, ioffset_p1, ioffset_p0;
+	u16 rlen;
+	u32 data;
+
+	rtl_apply_firmware(tp);
+
+	/* CHIN EST parameter update */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x808a);
+	rtl_w0w1_phy(tp, 0x14, 0x000a, 0x003f);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* enable R-tune & PGA-retune function */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x0811);
+	rtl_w0w1_phy(tp, 0x14, 0x0800, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0a42);
+	rtl_w0w1_phy(tp, 0x16, 0x0002, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* enable GPHY 10M */
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w0w1_phy(tp, 0x11, 0x0800, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	r8168_mac_ocp_write(tp, 0xdd02, 0x807d);
+	data = r8168_mac_ocp_read(tp, 0xdd02);
+	ioffset_p3 = ((data & 0x80)>>7);
+	ioffset_p3 <<= 3;
+
+	data = r8168_mac_ocp_read(tp, 0xdd00);
+	ioffset_p3 |= ((data & (0xe000))>>13);
+	ioffset_p2 = ((data & (0x1e00))>>9);
+	ioffset_p1 = ((data & (0x01e0))>>5);
+	ioffset_p0 = ((data & 0x0010)>>4);
+	ioffset_p0 <<= 3;
+	ioffset_p0 |= (data & (0x07));
+	data = (ioffset_p3<<12)|(ioffset_p2<<8)|(ioffset_p1<<4)|(ioffset_p0);
+
+	if ((ioffset_p3 != 0x0f) || (ioffset_p2 != 0x0f) ||
+	    (ioffset_p1 != 0x0f) || (ioffset_p0 == 0x0f)) {
+		rtl_writephy(tp, 0x1f, 0x0bcf);
+		rtl_writephy(tp, 0x16, data);
+		rtl_writephy(tp, 0x1f, 0x0000);
+	}
+
+	/* Modify rlen (TX LPF corner frequency) level */
+	rtl_writephy(tp, 0x1f, 0x0bcd);
+	data = rtl_readphy(tp, 0x16);
+	data &= 0x000f;
+	rlen = 0;
+	if (data > 3)
+		rlen = data - 3;
+	data = rlen | (rlen<<4) | (rlen<<8) | (rlen<<12);
+	rtl_writephy(tp, 0x17, data);
+	rtl_writephy(tp, 0x1f, 0x0bcd);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* disable phy pfm mode */
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x0080);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Check ALDPS bit, disable it if enabled */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	if (rtl_readphy(tp, 0x10) & 0x0004)
+		rtl_w0w1_phy(tp, 0x10, 0x0000, 0x0004);
+
+	rtl_writephy(tp, 0x1f, 0x0000);
+}
+
+static void rtl8168ep_1_hw_phy_config(struct rtl8169_private *tp)
+{
+	/* Enable PHY auto speed down */
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w0w1_phy(tp, 0x11, 0x000c, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* patch 10M & ALDPS */
+	rtl_writephy(tp, 0x1f, 0x0bcc);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x0100);
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w0w1_phy(tp, 0x11, 0x00c0, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x8084);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x6000);
+	rtl_w0w1_phy(tp, 0x10, 0x1003, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Enable EEE auto-fallback function */
+	rtl_writephy(tp, 0x1f, 0x0a4b);
+	rtl_w0w1_phy(tp, 0x11, 0x0004, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Enable UC LPF tune function */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x8012);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* set rg_sel_sdm_rate */
+	rtl_writephy(tp, 0x1f, 0x0c42);
+	rtl_w0w1_phy(tp, 0x11, 0x4000, 0x2000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Check ALDPS bit, disable it if enabled */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	if (rtl_readphy(tp, 0x10) & 0x0004)
+		rtl_w0w1_phy(tp, 0x10, 0x0000, 0x0004);
+
+	rtl_writephy(tp, 0x1f, 0x0000);
+}
+
+static void rtl8168ep_2_hw_phy_config(struct rtl8169_private *tp)
+{
+	/* patch 10M & ALDPS */
+	rtl_writephy(tp, 0x1f, 0x0bcc);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x0100);
+	rtl_writephy(tp, 0x1f, 0x0a44);
+	rtl_w0w1_phy(tp, 0x11, 0x00c0, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x8084);
+	rtl_w0w1_phy(tp, 0x14, 0x0000, 0x6000);
+	rtl_w0w1_phy(tp, 0x10, 0x1003, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Enable UC LPF tune function */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x8012);
+	rtl_w0w1_phy(tp, 0x14, 0x8000, 0x0000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Set rg_sel_sdm_rate */
+	rtl_writephy(tp, 0x1f, 0x0c42);
+	rtl_w0w1_phy(tp, 0x11, 0x4000, 0x2000);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Channel estimation parameters */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x80f3);
+	rtl_w0w1_phy(tp, 0x14, 0x8b00, ~0x8bff);
+	rtl_writephy(tp, 0x13, 0x80f0);
+	rtl_w0w1_phy(tp, 0x14, 0x3a00, ~0x3aff);
+	rtl_writephy(tp, 0x13, 0x80ef);
+	rtl_w0w1_phy(tp, 0x14, 0x0500, ~0x05ff);
+	rtl_writephy(tp, 0x13, 0x80f6);
+	rtl_w0w1_phy(tp, 0x14, 0x6e00, ~0x6eff);
+	rtl_writephy(tp, 0x13, 0x80ec);
+	rtl_w0w1_phy(tp, 0x14, 0x6800, ~0x68ff);
+	rtl_writephy(tp, 0x13, 0x80ed);
+	rtl_w0w1_phy(tp, 0x14, 0x7c00, ~0x7cff);
+	rtl_writephy(tp, 0x13, 0x80f2);
+	rtl_w0w1_phy(tp, 0x14, 0xf400, ~0xf4ff);
+	rtl_writephy(tp, 0x13, 0x80f4);
+	rtl_w0w1_phy(tp, 0x14, 0x8500, ~0x85ff);
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x8110);
+	rtl_w0w1_phy(tp, 0x14, 0xa800, ~0xa8ff);
+	rtl_writephy(tp, 0x13, 0x810f);
+	rtl_w0w1_phy(tp, 0x14, 0x1d00, ~0x1dff);
+	rtl_writephy(tp, 0x13, 0x8111);
+	rtl_w0w1_phy(tp, 0x14, 0xf500, ~0xf5ff);
+	rtl_writephy(tp, 0x13, 0x8113);
+	rtl_w0w1_phy(tp, 0x14, 0x6100, ~0x61ff);
+	rtl_writephy(tp, 0x13, 0x8115);
+	rtl_w0w1_phy(tp, 0x14, 0x9200, ~0x92ff);
+	rtl_writephy(tp, 0x13, 0x810e);
+	rtl_w0w1_phy(tp, 0x14, 0x0400, ~0x04ff);
+	rtl_writephy(tp, 0x13, 0x810c);
+	rtl_w0w1_phy(tp, 0x14, 0x7c00, ~0x7cff);
+	rtl_writephy(tp, 0x13, 0x810b);
+	rtl_w0w1_phy(tp, 0x14, 0x5a00, ~0x5aff);
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	rtl_writephy(tp, 0x13, 0x80d1);
+	rtl_w0w1_phy(tp, 0x14, 0xff00, ~0xffff);
+	rtl_writephy(tp, 0x13, 0x80cd);
+	rtl_w0w1_phy(tp, 0x14, 0x9e00, ~0x9eff);
+	rtl_writephy(tp, 0x13, 0x80d3);
+	rtl_w0w1_phy(tp, 0x14, 0x0e00, ~0x0eff);
+	rtl_writephy(tp, 0x13, 0x80d5);
+	rtl_w0w1_phy(tp, 0x14, 0xca00, ~0xcaff);
+	rtl_writephy(tp, 0x13, 0x80d7);
+	rtl_w0w1_phy(tp, 0x14, 0x8400, ~0x84ff);
+
+	/* Force PWM-mode */
+	rtl_writephy(tp, 0x1f, 0x0bcd);
+	rtl_writephy(tp, 0x14, 0x5065);
+	rtl_writephy(tp, 0x14, 0xd065);
+	rtl_writephy(tp, 0x1f, 0x0bc8);
+	rtl_writephy(tp, 0x12, 0x00ed);
+	rtl_writephy(tp, 0x1f, 0x0bcd);
+	rtl_writephy(tp, 0x14, 0x1065);
+	rtl_writephy(tp, 0x14, 0x9065);
+	rtl_writephy(tp, 0x14, 0x1065);
+	rtl_writephy(tp, 0x1f, 0x0000);
+
+	/* Check ALDPS bit, disable it if enabled */
+	rtl_writephy(tp, 0x1f, 0x0a43);
+	if (rtl_readphy(tp, 0x10) & 0x0004)
+		rtl_w0w1_phy(tp, 0x10, 0x0000, 0x0004);
+
+	rtl_writephy(tp, 0x1f, 0x0000);
+}
+
 static void rtl8102e_hw_phy_config(struct rtl8169_private *tp)
 {
 	static const struct phy_reg phy_reg_init[] = {
@@ -3654,6 +4228,22 @@ static void rtl_hw_phy_config(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_44:
 		rtl8168g_2_hw_phy_config(tp);
 		break;
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_47:
+		rtl8168h_1_hw_phy_config(tp);
+		break;
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_48:
+		rtl8168h_2_hw_phy_config(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_49:
+		rtl8168ep_1_hw_phy_config(tp);
+		break;
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		rtl8168ep_2_hw_phy_config(tp);
+		break;
 
 	case RTL_GIGA_MAC_VER_41:
 	default:
@@ -3865,6 +4455,13 @@ static void rtl_init_mdio_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_42:
 	case RTL_GIGA_MAC_VER_43:
 	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
 		ops->write	= r8168g_mdio_write;
 		ops->read	= r8168g_mdio_read;
 		break;
@@ -3919,6 +4516,13 @@ static void rtl_wol_suspend_quirk(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_42:
 	case RTL_GIGA_MAC_VER_43:
 	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
 		RTL_W32(RxConfig, RTL_R32(RxConfig) |
 			AcceptBroadcast | AcceptMulticast | AcceptMyPhys);
 		break;
@@ -3987,6 +4591,10 @@ static void r810x_pll_power_up(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_13:
 	case RTL_GIGA_MAC_VER_16:
 		break;
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+		RTL_W8(PMCH, RTL_R8(PMCH) | 0xc0);
+		break;
 	default:
 		RTL_W8(PMCH, RTL_R8(PMCH) | 0x80);
 		break;
@@ -4059,8 +4667,11 @@ static void r8168_pll_power_down(struct rtl8169_private *tp)
 
 	if ((tp->mac_version == RTL_GIGA_MAC_VER_27 ||
 	     tp->mac_version == RTL_GIGA_MAC_VER_28 ||
-	     tp->mac_version == RTL_GIGA_MAC_VER_31) &&
-	    r8168dp_check_dash(tp)) {
+	     tp->mac_version == RTL_GIGA_MAC_VER_31 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_49 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_50 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_51) &&
+	    r8168_check_dash(tp)) {
 		return;
 	}
 
@@ -4087,12 +4698,19 @@ static void r8168_pll_power_down(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_31:
 	case RTL_GIGA_MAC_VER_32:
 	case RTL_GIGA_MAC_VER_33:
+	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
 		RTL_W8(PMCH, RTL_R8(PMCH) & ~0x80);
 		break;
 	case RTL_GIGA_MAC_VER_40:
 	case RTL_GIGA_MAC_VER_41:
-		rtl_w1w0_eri(tp, 0x1a8, ERIAR_MASK_1111, 0x00000000,
+	case RTL_GIGA_MAC_VER_49:
+		rtl_w0w1_eri(tp, 0x1a8, ERIAR_MASK_1111, 0x00000000,
 			     0xfc000000, ERIAR_EXGMAC);
+		RTL_W8(PMCH, RTL_R8(PMCH) & ~0x80);
 		break;
 	}
 }
@@ -4111,9 +4729,18 @@ static void r8168_pll_power_up(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_33:
 		RTL_W8(PMCH, RTL_R8(PMCH) | 0x80);
 		break;
+	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		RTL_W8(PMCH, RTL_R8(PMCH) | 0xc0);
+		break;
 	case RTL_GIGA_MAC_VER_40:
 	case RTL_GIGA_MAC_VER_41:
-		rtl_w1w0_eri(tp, 0x1a8, ERIAR_MASK_1111, 0xfc000000,
+	case RTL_GIGA_MAC_VER_49:
+		RTL_W8(PMCH, RTL_R8(PMCH) | 0xc0);
+		rtl_w0w1_eri(tp, 0x1a8, ERIAR_MASK_1111, 0xfc000000,
 			     0x00000000, ERIAR_EXGMAC);
 		break;
 	}
@@ -4153,6 +4780,8 @@ static void rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_37:
 	case RTL_GIGA_MAC_VER_39:
 	case RTL_GIGA_MAC_VER_43:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
 		ops->down	= r810x_pll_power_down;
 		ops->up		= r810x_pll_power_up;
 		break;
@@ -4182,6 +4811,11 @@ static void rtl_init_pll_power_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_41:
 	case RTL_GIGA_MAC_VER_42:
 	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
 		ops->down	= r8168_pll_power_down;
 		ops->up		= r8168_pll_power_up;
 		break;
@@ -4232,6 +4866,13 @@ static void rtl_init_rxcfg(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_42:
 	case RTL_GIGA_MAC_VER_43:
 	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
 		RTL_W32(RxConfig, RX128_INT_EN | RX_DMA_BURST | RX_EARLY_OFF);
 		break;
 	default:
@@ -4393,6 +5034,13 @@ static void rtl_init_jumbo_ops(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_42:
 	case RTL_GIGA_MAC_VER_43:
 	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
 	default:
 		ops->disable	= NULL;
 		ops->enable	= NULL;
@@ -4414,6 +5062,8 @@ static void rtl_hw_reset(struct rtl8169_private *tp)
 	RTL_W8(ChipCmd, CmdReset);
 
 	rtl_udelay_loop_wait_low(tp, &rtl_chipcmd_cond, 100, 100);
+
+	netdev_reset_queue(tp->dev);
 }
 
 static void rtl_request_uncached_firmware(struct rtl8169_private *tp)
@@ -4495,15 +5145,22 @@ static void rtl8169_hw_reset(struct rtl8169_private *tp)
 	    tp->mac_version == RTL_GIGA_MAC_VER_31) {
 		rtl_udelay_loop_wait_low(tp, &rtl_npq_cond, 20, 42*42);
 	} else if (tp->mac_version == RTL_GIGA_MAC_VER_34 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_35 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_36 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_37 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_40 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_41 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_42 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_43 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_44 ||
-	           tp->mac_version == RTL_GIGA_MAC_VER_38) {
+		   tp->mac_version == RTL_GIGA_MAC_VER_35 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_36 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_37 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_38 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_40 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_41 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_42 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_43 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_44 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_45 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_46 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_47 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_48 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_49 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_50 ||
+		   tp->mac_version == RTL_GIGA_MAC_VER_51) {
 		RTL_W8(ChipCmd, RTL_R8(ChipCmd) | StopReq);
 		rtl_udelay_loop_wait_high(tp, &rtl_txcfg_empty_cond, 100, 666);
 	} else {
@@ -4673,7 +5330,7 @@ static void rtl_hw_start_8169(struct net_device *dev)
 
 	if (tp->mac_version == RTL_GIGA_MAC_VER_02 ||
 	    tp->mac_version == RTL_GIGA_MAC_VER_03) {
-		dprintk("Set MAC Reg C+CR Offset 0xE0. "
+		dprintk("Set MAC Reg C+CR Offset 0xe0. "
 			"Bit-3 and bit-14 MUST be 1\n");
 		tp->cp_cmd |= (1 << 14);
 	}
@@ -4708,7 +5365,7 @@ static void rtl_hw_start_8169(struct net_device *dev)
 	rtl_set_rx_mode(dev);
 
 	/* no early-rx interrupts */
-	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xF000);
+	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xf000);
 }
 
 static void rtl_csi_write(struct rtl8169_private *tp, int addr, int value)
@@ -5171,8 +5828,8 @@ static void rtl_hw_start_8168e_2(struct rtl8169_private *tp)
 	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xcc, ERIAR_MASK_1111, 0x00000050, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xd0, ERIAR_MASK_1111, 0x07ff0060, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00, ERIAR_EXGMAC);
 
 	RTL_W8(MaxTxPacketSize, EarlySize);
 
@@ -5202,10 +5859,10 @@ static void rtl_hw_start_8168f(struct rtl8169_private *tp)
 	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xc8, ERIAR_MASK_1111, 0x00100002, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0x1d0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x1b0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x1d0, ERIAR_MASK_0001, 0x10, 0x00, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xcc, ERIAR_MASK_1111, 0x00000050, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xd0, ERIAR_MASK_1111, 0x00000060, ERIAR_EXGMAC);
 
@@ -5234,7 +5891,7 @@ static void rtl_hw_start_8168f_1(struct rtl8169_private *tp)
 
 	rtl_ephy_init(tp, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
 
-	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0xff00, ERIAR_EXGMAC);
 
 	/* Adjust EEE LED frequency */
 	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
@@ -5254,7 +5911,7 @@ static void rtl_hw_start_8411(struct rtl8169_private *tp)
 
 	rtl_ephy_init(tp, e_info_8168f_1, ARRAY_SIZE(e_info_8168f_1));
 
-	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0x0000, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0c00, 0x0000, ERIAR_EXGMAC);
 }
 
 static void rtl_hw_start_8168g_1(struct rtl8169_private *tp)
@@ -5273,8 +5930,8 @@ static void rtl_hw_start_8168g_1(struct rtl8169_private *tp)
 
 	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
 
-	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0x2f8, ERIAR_MASK_0011, 0x1d8f, ERIAR_EXGMAC);
 
 	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
@@ -5287,8 +5944,8 @@ static void rtl_hw_start_8168g_1(struct rtl8169_private *tp)
 	/* Adjust EEE LED frequency */
 	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
 
-	rtl_w1w0_eri(tp, 0x2fc, ERIAR_MASK_0001, 0x01, 0x06, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0x1b0, ERIAR_MASK_0011, 0x0000, 0x1000, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x2fc, ERIAR_MASK_0001, 0x01, 0x06, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x1b0, ERIAR_MASK_0011, 0x0000, 0x1000, ERIAR_EXGMAC);
 
 	rtl_pcie_state_l2l3_enable(tp, false);
 }
@@ -5330,6 +5987,219 @@ static void rtl_hw_start_8411_2(struct rtl8169_private *tp)
 	rtl_ephy_init(tp, e_info_8411_2, ARRAY_SIZE(e_info_8411_2));
 }
 
+static void rtl_hw_start_8168h_1(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+	u16 rg_saw_cnt;
+	u32 data;
+	static const struct ephy_info e_info_8168h_1[] = {
+		{ 0x1e, 0x0800,	0x0001 },
+		{ 0x1d, 0x0000,	0x0800 },
+		{ 0x05, 0xffff,	0x2089 },
+		{ 0x06, 0xffff,	0x5881 },
+		{ 0x04, 0xffff,	0x154a },
+		{ 0x01, 0xffff,	0x068b }
+	};
+
+	/* disable aspm and clock request before access ephy */
+	RTL_W8(Config2, RTL_R8(Config2) & ~ClkReqEn);
+	RTL_W8(Config5, RTL_R8(Config5) & ~ASPM_en);
+	rtl_ephy_init(tp, e_info_8168h_1, ARRAY_SIZE(e_info_8168h_1));
+
+	RTL_W32(TxConfig, RTL_R32(TxConfig) | TXCFG_AUTO_FIFO);
+
+	rtl_eri_write(tp, 0xc8, ERIAR_MASK_0101, 0x00080002, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xcc, ERIAR_MASK_0001, 0x38, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xd0, ERIAR_MASK_0001, 0x48, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
+
+	rtl_csi_access_enable_1(tp);
+
+	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
+
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_1111, 0x0010, 0x00, ERIAR_EXGMAC);
+
+	rtl_w0w1_eri(tp, 0xd4, ERIAR_MASK_1111, 0x1f00, 0x00, ERIAR_EXGMAC);
+
+	rtl_eri_write(tp, 0x5f0, ERIAR_MASK_0011, 0x4f87, ERIAR_EXGMAC);
+
+	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
+	RTL_W32(MISC, RTL_R32(MISC) & ~RXDV_GATED_EN);
+	RTL_W8(MaxTxPacketSize, EarlySize);
+
+	rtl_eri_write(tp, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+
+	/* Adjust EEE LED frequency */
+	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
+
+	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~PFM_EN);
+	RTL_W8(DLLPR, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
+
+	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~TX_10M_PS_EN);
+
+	rtl_w0w1_eri(tp, 0x1b0, ERIAR_MASK_0011, 0x0000, 0x1000, ERIAR_EXGMAC);
+
+	rtl_pcie_state_l2l3_enable(tp, false);
+
+	rtl_writephy(tp, 0x1f, 0x0c42);
+	rg_saw_cnt = rtl_readphy(tp, 0x13);
+	rtl_writephy(tp, 0x1f, 0x0000);
+	if (rg_saw_cnt > 0) {
+		u16 sw_cnt_1ms_ini;
+
+		sw_cnt_1ms_ini = 16000000/rg_saw_cnt;
+		sw_cnt_1ms_ini &= 0x0fff;
+		data = r8168_mac_ocp_read(tp, 0xd412);
+		data &= 0x0fff;
+		data |= sw_cnt_1ms_ini;
+		r8168_mac_ocp_write(tp, 0xd412, data);
+	}
+
+	data = r8168_mac_ocp_read(tp, 0xe056);
+	data &= 0xf0;
+	data |= 0x07;
+	r8168_mac_ocp_write(tp, 0xe056, data);
+
+	data = r8168_mac_ocp_read(tp, 0xe052);
+	data &= 0x8008;
+	data |= 0x6000;
+	r8168_mac_ocp_write(tp, 0xe052, data);
+
+	data = r8168_mac_ocp_read(tp, 0xe0d6);
+	data &= 0x01ff;
+	data |= 0x017f;
+	r8168_mac_ocp_write(tp, 0xe0d6, data);
+
+	data = r8168_mac_ocp_read(tp, 0xd420);
+	data &= 0x0fff;
+	data |= 0x047f;
+	r8168_mac_ocp_write(tp, 0xd420, data);
+
+	r8168_mac_ocp_write(tp, 0xe63e, 0x0001);
+	r8168_mac_ocp_write(tp, 0xe63e, 0x0000);
+	r8168_mac_ocp_write(tp, 0xc094, 0x0000);
+	r8168_mac_ocp_write(tp, 0xc09e, 0x0000);
+}
+
+static void rtl_hw_start_8168ep(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	struct pci_dev *pdev = tp->pci_dev;
+
+	RTL_W32(TxConfig, RTL_R32(TxConfig) | TXCFG_AUTO_FIFO);
+
+	rtl_eri_write(tp, 0xc8, ERIAR_MASK_0101, 0x00080002, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xcc, ERIAR_MASK_0001, 0x2f, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xd0, ERIAR_MASK_0001, 0x5f, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00100006, ERIAR_EXGMAC);
+
+	rtl_csi_access_enable_1(tp);
+
+	rtl_tx_performance_tweak(pdev, 0x5 << MAX_READ_REQUEST_SHIFT);
+
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+
+	rtl_w0w1_eri(tp, 0xd4, ERIAR_MASK_1111, 0x1f80, 0x00, ERIAR_EXGMAC);
+
+	rtl_eri_write(tp, 0x5f0, ERIAR_MASK_0011, 0x4f87, ERIAR_EXGMAC);
+
+	RTL_W8(ChipCmd, CmdTxEnb | CmdRxEnb);
+	RTL_W32(MISC, RTL_R32(MISC) & ~RXDV_GATED_EN);
+	RTL_W8(MaxTxPacketSize, EarlySize);
+
+	rtl_eri_write(tp, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
+
+	/* Adjust EEE LED frequency */
+	RTL_W8(EEE_LED, RTL_R8(EEE_LED) & ~0x07);
+
+	rtl_w0w1_eri(tp, 0x2fc, ERIAR_MASK_0001, 0x01, 0x06, ERIAR_EXGMAC);
+
+	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~TX_10M_PS_EN);
+
+	rtl_pcie_state_l2l3_enable(tp, false);
+}
+
+static void rtl_hw_start_8168ep_1(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	static const struct ephy_info e_info_8168ep_1[] = {
+		{ 0x00, 0xffff,	0x10ab },
+		{ 0x06, 0xffff,	0xf030 },
+		{ 0x08, 0xffff,	0x2006 },
+		{ 0x0d, 0xffff,	0x1666 },
+		{ 0x0c, 0x3ff0,	0x0000 }
+	};
+
+	/* disable aspm and clock request before access ephy */
+	RTL_W8(Config2, RTL_R8(Config2) & ~ClkReqEn);
+	RTL_W8(Config5, RTL_R8(Config5) & ~ASPM_en);
+	rtl_ephy_init(tp, e_info_8168ep_1, ARRAY_SIZE(e_info_8168ep_1));
+
+	rtl_hw_start_8168ep(tp);
+}
+
+static void rtl_hw_start_8168ep_2(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	static const struct ephy_info e_info_8168ep_2[] = {
+		{ 0x00, 0xffff,	0x10a3 },
+		{ 0x19, 0xffff,	0xfc00 },
+		{ 0x1e, 0xffff,	0x20ea }
+	};
+
+	/* disable aspm and clock request before access ephy */
+	RTL_W8(Config2, RTL_R8(Config2) & ~ClkReqEn);
+	RTL_W8(Config5, RTL_R8(Config5) & ~ASPM_en);
+	rtl_ephy_init(tp, e_info_8168ep_2, ARRAY_SIZE(e_info_8168ep_2));
+
+	rtl_hw_start_8168ep(tp);
+
+	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~PFM_EN);
+	RTL_W8(DLLPR, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
+}
+
+static void rtl_hw_start_8168ep_3(struct rtl8169_private *tp)
+{
+	void __iomem *ioaddr = tp->mmio_addr;
+	u32 data;
+	static const struct ephy_info e_info_8168ep_3[] = {
+		{ 0x00, 0xffff,	0x10a3 },
+		{ 0x19, 0xffff,	0x7c00 },
+		{ 0x1e, 0xffff,	0x20eb },
+		{ 0x0d, 0xffff,	0x1666 }
+	};
+
+	/* disable aspm and clock request before access ephy */
+	RTL_W8(Config2, RTL_R8(Config2) & ~ClkReqEn);
+	RTL_W8(Config5, RTL_R8(Config5) & ~ASPM_en);
+	rtl_ephy_init(tp, e_info_8168ep_3, ARRAY_SIZE(e_info_8168ep_3));
+
+	rtl_hw_start_8168ep(tp);
+
+	RTL_W8(DLLPR, RTL_R8(DLLPR) & ~PFM_EN);
+	RTL_W8(DLLPR, RTL_R8(MISC_1) & ~PFM_D3COLD_EN);
+
+	data = r8168_mac_ocp_read(tp, 0xd3e2);
+	data &= 0xf000;
+	data |= 0x0271;
+	r8168_mac_ocp_write(tp, 0xd3e2, data);
+
+	data = r8168_mac_ocp_read(tp, 0xd3e4);
+	data &= 0xff00;
+	r8168_mac_ocp_write(tp, 0xd3e4, data);
+
+	data = r8168_mac_ocp_read(tp, 0xe860);
+	data |= 0x0080;
+	r8168_mac_ocp_write(tp, 0xe860, data);
+}
+
 static void rtl_hw_start_8168(struct net_device *dev)
 {
 	struct rtl8169_private *tp = netdev_priv(dev);
@@ -5440,6 +6310,23 @@ static void rtl_hw_start_8168(struct net_device *dev)
 		rtl_hw_start_8411_2(tp);
 		break;
 
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+		rtl_hw_start_8168h_1(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_49:
+		rtl_hw_start_8168ep_1(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_50:
+		rtl_hw_start_8168ep_2(tp);
+		break;
+
+	case RTL_GIGA_MAC_VER_51:
+		rtl_hw_start_8168ep_3(tp);
+		break;
+
 	default:
 		printk(KERN_ERR PFX "%s: unknown chipset (mac_version = %d).\n",
 			dev->name, tp->mac_version);
@@ -5452,7 +6339,7 @@ static void rtl_hw_start_8168(struct net_device *dev)
 
 	rtl_set_rx_mode(dev);
 
-	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xF000);
+	RTL_W16(MultiIntr, RTL_R16(MultiIntr) & 0xf000);
 }
 
 #define R810X_CPCMD_QUIRK_MASK (\
@@ -5575,11 +6462,11 @@ static void rtl_hw_start_8402(struct rtl8169_private *tp)
 
 	rtl_eri_write(tp, 0xc8, ERIAR_MASK_1111, 0x00000002, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xe8, ERIAR_MASK_1111, 0x00000006, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x00, 0x01, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0xdc, ERIAR_MASK_0001, 0x01, 0x00, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xc0, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
 	rtl_eri_write(tp, 0xb8, ERIAR_MASK_0011, 0x0000, ERIAR_EXGMAC);
-	rtl_w1w0_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0e00, 0xff00, ERIAR_EXGMAC);
+	rtl_w0w1_eri(tp, 0x0d4, ERIAR_MASK_0011, 0x0e00, 0xff00, ERIAR_EXGMAC);
 
 	rtl_pcie_state_l2l3_enable(tp, false);
 }
@@ -5655,6 +6542,10 @@ static void rtl_hw_start_8101(struct net_device *dev)
 	case RTL_GIGA_MAC_VER_43:
 		rtl_hw_start_8168g_2(tp);
 		break;
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+		rtl_hw_start_8168h_1(tp);
+		break;
 	}
 
 	RTL_W8(Cfg9346, Cfg9346_Lock);
@@ -5895,7 +6786,7 @@ static int rtl8169_xmit_frags(struct rtl8169_private *tp, struct sk_buff *skb,
 {
 	struct skb_shared_info *info = skb_shinfo(skb);
 	unsigned int cur_frag, entry;
-	struct TxDesc * uninitialized_var(txd);
+	struct TxDesc *uninitialized_var(txd);
 	struct device *d = &tp->pci_dev->dev;
 
 	entry = tp->cur_tx;
@@ -6182,6 +7073,8 @@ static netdev_tx_t rtl8169_start_xmit(struct sk_buff *skb,
 
 	txd->opts2 = cpu_to_le32(opts[1]);
 
+	netdev_sent_queue(dev, skb->len);
+
 	skb_tx_timestamp(skb);
 
 	wmb();
@@ -6281,6 +7174,7 @@ static void rtl8169_pcierr_interrupt(struct net_device *dev)
 static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 {
 	unsigned int dirty_tx, tx_left;
+	unsigned int bytes_compl = 0, pkts_compl = 0;
 
 	dirty_tx = tp->dirty_tx;
 	smp_rmb();
@@ -6299,10 +7193,8 @@ static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 		rtl8169_unmap_tx_skb(&tp->pci_dev->dev, tx_skb,
 				     tp->TxDescArray + entry);
 		if (status & LastFrag) {
-			u64_stats_update_begin(&tp->tx_stats.syncp);
-			tp->tx_stats.packets++;
-			tp->tx_stats.bytes += tx_skb->skb->len;
-			u64_stats_update_end(&tp->tx_stats.syncp);
+			pkts_compl++;
+			bytes_compl += tx_skb->skb->len;
 			dev_kfree_skb_any(tx_skb->skb);
 			tx_skb->skb = NULL;
 		}
@@ -6311,6 +7203,13 @@ static void rtl_tx(struct net_device *dev, struct rtl8169_private *tp)
 	}
 
 	if (tp->dirty_tx != dirty_tx) {
+		netdev_completed_queue(tp->dev, pkts_compl, bytes_compl);
+
+		u64_stats_update_begin(&tp->tx_stats.syncp);
+		tp->tx_stats.packets += pkts_compl;
+		tp->tx_stats.bytes += bytes_compl;
+		u64_stats_update_end(&tp->tx_stats.syncp);
+
 		tp->dirty_tx = dirty_tx;
 		/* Sync with rtl8169_start_xmit:
 		 * - publish dirty_tx ring index (write barrier)
@@ -6956,9 +7855,13 @@ static void rtl_remove_one(struct pci_dev *pdev)
 	struct net_device *dev = pci_get_drvdata(pdev);
 	struct rtl8169_private *tp = netdev_priv(dev);
 
-	if (tp->mac_version == RTL_GIGA_MAC_VER_27 ||
-	    tp->mac_version == RTL_GIGA_MAC_VER_28 ||
-	    tp->mac_version == RTL_GIGA_MAC_VER_31) {
+	if ((tp->mac_version == RTL_GIGA_MAC_VER_27 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_28 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_31 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_49 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_50 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_51) &&
+	    r8168_check_dash(tp)) {
 		rtl8168_driver_stop(tp);
 	}
 
@@ -7110,6 +8013,13 @@ static void rtl_hw_initialize(struct rtl8169_private *tp)
 	case RTL_GIGA_MAC_VER_42:
 	case RTL_GIGA_MAC_VER_43:
 	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
 		rtl_hw_init_8168g(tp);
 		break;
 
@@ -7118,8 +8028,7 @@ static void rtl_hw_initialize(struct rtl8169_private *tp)
 	}
 }
 
-static int
-rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
+static int rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	const struct rtl_cfg_info *cfg = rtl_cfg_infos + ent->driver_data;
 	const unsigned int region = cfg->region;
@@ -7194,7 +8103,7 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto err_out_mwi_2;
 	}
 
-	tp->cp_cmd = RxChkSum;
+	tp->cp_cmd = 0;
 
 	if ((sizeof(dma_addr_t) > 4) &&
 	    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64)) && use_dac) {
@@ -7235,13 +8144,6 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	pci_set_master(pdev);
 
-	/*
-	 * Pretend we are using VLANs; This bypasses a nasty bug where
-	 * Interrupts stop flowing on high load on 8110SCd controllers.
-	 */
-	if (tp->mac_version == RTL_GIGA_MAC_VER_05)
-		tp->cp_cmd |= RxVlan;
-
 	rtl_init_mdio_ops(tp);
 	rtl_init_pll_power_ops(tp);
 	rtl_init_jumbo_ops(tp);
@@ -7255,8 +8157,34 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	RTL_W8(Cfg9346, Cfg9346_Unlock);
 	RTL_W8(Config1, RTL_R8(Config1) | PMEnable);
 	RTL_W8(Config5, RTL_R8(Config5) & (BWF | MWF | UWF | LanWake | PMEStatus));
-	if ((RTL_R8(Config3) & (LinkUp | MagicPacket)) != 0)
-		tp->features |= RTL_FEATURE_WOL;
+	switch (tp->mac_version) {
+	case RTL_GIGA_MAC_VER_34:
+	case RTL_GIGA_MAC_VER_35:
+	case RTL_GIGA_MAC_VER_36:
+	case RTL_GIGA_MAC_VER_37:
+	case RTL_GIGA_MAC_VER_38:
+	case RTL_GIGA_MAC_VER_40:
+	case RTL_GIGA_MAC_VER_41:
+	case RTL_GIGA_MAC_VER_42:
+	case RTL_GIGA_MAC_VER_43:
+	case RTL_GIGA_MAC_VER_44:
+	case RTL_GIGA_MAC_VER_45:
+	case RTL_GIGA_MAC_VER_46:
+	case RTL_GIGA_MAC_VER_47:
+	case RTL_GIGA_MAC_VER_48:
+	case RTL_GIGA_MAC_VER_49:
+	case RTL_GIGA_MAC_VER_50:
+	case RTL_GIGA_MAC_VER_51:
+		if (rtl_eri_read(tp, 0xdc, ERIAR_EXGMAC) & MagicPacket_v2)
+			tp->features |= RTL_FEATURE_WOL;
+		if ((RTL_R8(Config3) & LinkUp) != 0)
+			tp->features |= RTL_FEATURE_WOL;
+		break;
+	default:
+		if ((RTL_R8(Config3) & (LinkUp | MagicPacket)) != 0)
+			tp->features |= RTL_FEATURE_WOL;
+		break;
+	}
 	if ((RTL_R8(Config5) & (UWF | BWF | MWF)) != 0)
 		tp->features |= RTL_FEATURE_WOL;
 	tp->features |= rtl_try_msi(tp, cfg);
@@ -7283,6 +8211,30 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	u64_stats_init(&tp->tx_stats.syncp);
 
 	/* Get MAC address */
+	if (tp->mac_version == RTL_GIGA_MAC_VER_35 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_36 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_37 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_38 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_40 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_41 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_42 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_43 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_44 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_45 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_46 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_47 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_48 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_49 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_50 ||
+	    tp->mac_version == RTL_GIGA_MAC_VER_51) {
+		u16 mac_addr[3];
+
+		*(u32 *)&mac_addr[0] = rtl_eri_read(tp, 0xe0, ERIAR_EXGMAC);
+		*(u16 *)&mac_addr[2] = rtl_eri_read(tp, 0xe4, ERIAR_EXGMAC);
+
+		if (is_valid_ether_addr((u8 *)mac_addr))
+			rtl_rar_set(tp, (u8 *)mac_addr);
+	}
 	for (i = 0; i < ETH_ALEN; i++)
 		dev->dev_addr[i] = RTL_R8(MAC0 + i);
 
@@ -7302,8 +8254,14 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	dev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
 		NETIF_F_HIGHDMA;
 
+	tp->cp_cmd |= RxChkSum | RxVlan;
+
+	/*
+	 * Pretend we are using VLANs; This bypasses a nasty bug where
+	 * Interrupts stop flowing on high load on 8110SCd controllers.
+	 */
 	if (tp->mac_version == RTL_GIGA_MAC_VER_05)
-		/* 8110SCd requires hardware Rx VLAN - disallow toggling */
+		/* Disallow toggling */
 		dev->hw_features &= ~NETIF_F_HW_VLAN_CTAG_RX;
 
 	if (tp->txd_version == RTL_TD_0)
@@ -7345,9 +8303,13 @@ rtl_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 			   rtl_chip_infos[chipset].jumbo_tx_csum ? "ok" : "ko");
 	}
 
-	if (tp->mac_version == RTL_GIGA_MAC_VER_27 ||
-	    tp->mac_version == RTL_GIGA_MAC_VER_28 ||
-	    tp->mac_version == RTL_GIGA_MAC_VER_31) {
+	if ((tp->mac_version == RTL_GIGA_MAC_VER_27 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_28 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_31 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_49 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_50 ||
+	     tp->mac_version == RTL_GIGA_MAC_VER_51) &&
+	    r8168_check_dash(tp)) {
 		rtl8168_driver_start(tp);
 	}
 
diff --git a/drivers/net/ethernet/renesas/Kconfig b/drivers/net/ethernet/renesas/Kconfig
index 9e757c792d84..196e98a2d93b 100644
--- a/drivers/net/ethernet/renesas/Kconfig
+++ b/drivers/net/ethernet/renesas/Kconfig
@@ -5,6 +5,7 @@
 config SH_ETH
 	tristate "Renesas SuperH Ethernet support"
 	depends on HAS_DMA
+	depends on ARCH_SHMOBILE || SUPERH || COMPILE_TEST
 	select CRC32
 	select MII
 	select MDIO_BITBANG
diff --git a/drivers/net/ethernet/sfc/farch.c b/drivers/net/ethernet/sfc/farch.c
index 0537381cd2f6..6859437b59fb 100644
--- a/drivers/net/ethernet/sfc/farch.c
+++ b/drivers/net/ethernet/sfc/farch.c
@@ -2933,6 +2933,9 @@ void efx_farch_filter_sync_rx_mode(struct efx_nic *efx)
 	u32 crc;
 	int bit;
 
+	if (!efx_dev_registered(efx))
+		return;
+
 	netif_addr_lock_bh(net_dev);
 
 	efx->unicast_filter = !(net_dev->flags & IFF_PROMISC);
diff --git a/drivers/net/ethernet/sfc/nic.h b/drivers/net/ethernet/sfc/nic.h
index 60f85149fc4c..f77cce034ad4 100644
--- a/drivers/net/ethernet/sfc/nic.h
+++ b/drivers/net/ethernet/sfc/nic.h
@@ -71,9 +71,17 @@ efx_tx_desc(struct efx_tx_queue *tx_queue, unsigned int index)
 	return ((efx_qword_t *) (tx_queue->txd.buf.addr)) + index;
 }
 
-/* Report whether the NIC considers this TX queue empty, given the
- * write_count used for the last doorbell push.  May return false
- * negative.
+/* Get partner of a TX queue, seen as part of the same net core queue */
+static struct efx_tx_queue *efx_tx_queue_partner(struct efx_tx_queue *tx_queue)
+{
+	if (tx_queue->queue & EFX_TXQ_TYPE_OFFLOAD)
+		return tx_queue - EFX_TXQ_TYPE_OFFLOAD;
+	else
+		return tx_queue + EFX_TXQ_TYPE_OFFLOAD;
+}
+
+/* Report whether this TX queue would be empty for the given write_count.
+ * May return false negative.
  */
 static inline bool __efx_nic_tx_is_empty(struct efx_tx_queue *tx_queue,
 					 unsigned int write_count)
@@ -86,9 +94,18 @@ static inline bool __efx_nic_tx_is_empty(struct efx_tx_queue *tx_queue,
 	return ((empty_read_count ^ write_count) & ~EFX_EMPTY_COUNT_VALID) == 0;
 }
 
-static inline bool efx_nic_tx_is_empty(struct efx_tx_queue *tx_queue)
+/* Decide whether we can use TX PIO, ie. write packet data directly into
+ * a buffer on the device.  This can reduce latency at the expense of
+ * throughput, so we only do this if both hardware and software TX rings
+ * are empty.  This also ensures that only one packet at a time can be
+ * using the PIO buffer.
+ */
+static inline bool efx_nic_may_tx_pio(struct efx_tx_queue *tx_queue)
 {
-	return __efx_nic_tx_is_empty(tx_queue, tx_queue->write_count);
+	struct efx_tx_queue *partner = efx_tx_queue_partner(tx_queue);
+	return tx_queue->piobuf &&
+	       __efx_nic_tx_is_empty(tx_queue, tx_queue->insert_count) &&
+	       __efx_nic_tx_is_empty(partner, partner->insert_count);
 }
 
 /* Decide whether to push a TX descriptor to the NIC vs merely writing
@@ -96,6 +113,8 @@ static inline bool efx_nic_tx_is_empty(struct efx_tx_queue *tx_queue)
  * descriptor to an empty queue, but is otherwise pointless.  Further,
  * Falcon and Siena have hardware bugs (SF bug 33851) that may be
  * triggered if we don't check this.
+ * We use the write_count used for the last doorbell push, to get the
+ * NIC's view of the tx queue.
  */
 static inline bool efx_nic_may_push_tx_desc(struct efx_tx_queue *tx_queue,
 					    unsigned int write_count)
diff --git a/drivers/net/ethernet/sfc/tx.c b/drivers/net/ethernet/sfc/tx.c
index 65c220f8661d..ee84a90e371c 100644
--- a/drivers/net/ethernet/sfc/tx.c
+++ b/drivers/net/ethernet/sfc/tx.c
@@ -78,7 +78,7 @@ static void efx_dequeue_buffer(struct efx_tx_queue *tx_queue,
 	if (buffer->flags & EFX_TX_BUF_SKB) {
 		(*pkts_compl)++;
 		(*bytes_compl) += buffer->skb->len;
-		dev_kfree_skb_any((struct sk_buff *) buffer->skb);
+		dev_consume_skb_any((struct sk_buff *)buffer->skb);
 		netif_vdbg(tx_queue->efx, tx_done, tx_queue->efx->net_dev,
 			   "TX queue %d transmission id %x complete\n",
 			   tx_queue->queue, tx_queue->read_count);
@@ -132,15 +132,6 @@ unsigned int efx_tx_max_skb_descs(struct efx_nic *efx)
 	return max_descs;
 }
 
-/* Get partner of a TX queue, seen as part of the same net core queue */
-static struct efx_tx_queue *efx_tx_queue_partner(struct efx_tx_queue *tx_queue)
-{
-	if (tx_queue->queue & EFX_TXQ_TYPE_OFFLOAD)
-		return tx_queue - EFX_TXQ_TYPE_OFFLOAD;
-	else
-		return tx_queue + EFX_TXQ_TYPE_OFFLOAD;
-}
-
 static void efx_tx_maybe_stop_queue(struct efx_tx_queue *txq1)
 {
 	/* We need to consider both queues that the net core sees as one */
@@ -344,6 +335,7 @@ netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 	struct efx_nic *efx = tx_queue->efx;
 	struct device *dma_dev = &efx->pci_dev->dev;
 	struct efx_tx_buffer *buffer;
+	unsigned int old_insert_count = tx_queue->insert_count;
 	skb_frag_t *fragment;
 	unsigned int len, unmap_len = 0;
 	dma_addr_t dma_addr, unmap_addr = 0;
@@ -351,7 +343,7 @@ netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 	unsigned short dma_flags;
 	int i = 0;
 
-	EFX_BUG_ON_PARANOID(tx_queue->write_count != tx_queue->insert_count);
+	EFX_BUG_ON_PARANOID(tx_queue->write_count > tx_queue->insert_count);
 
 	if (skb_shinfo(skb)->gso_size)
 		return efx_enqueue_skb_tso(tx_queue, skb);
@@ -369,9 +361,8 @@ netdev_tx_t efx_enqueue_skb(struct efx_tx_queue *tx_queue, struct sk_buff *skb)
 
 	/* Consider using PIO for short packets */
 #ifdef EFX_USE_PIO
-	if (skb->len <= efx_piobuf_size && tx_queue->piobuf &&
-	    efx_nic_tx_is_empty(tx_queue) &&
-	    efx_nic_tx_is_empty(efx_tx_queue_partner(tx_queue))) {
+	if (skb->len <= efx_piobuf_size && !skb->xmit_more &&
+	    efx_nic_may_tx_pio(tx_queue)) {
 		buffer = efx_enqueue_skb_pio(tx_queue, skb);
 		dma_flags = EFX_TX_BUF_OPTION;
 		goto finish_packet;
@@ -439,13 +430,14 @@ finish_packet:
 
 	netdev_tx_sent_queue(tx_queue->core_txq, skb->len);
 
+	efx_tx_maybe_stop_queue(tx_queue);
+
 	/* Pass off to hardware */
-	efx_nic_push_buffers(tx_queue);
+	if (!skb->xmit_more || netif_xmit_stopped(tx_queue->core_txq))
+		efx_nic_push_buffers(tx_queue);
 
 	tx_queue->tx_packets++;
 
-	efx_tx_maybe_stop_queue(tx_queue);
-
 	return NETDEV_TX_OK;
 
  dma_err:
@@ -458,7 +450,7 @@ finish_packet:
 	dev_kfree_skb_any(skb);
 
 	/* Work backwards until we hit the original insert pointer value */
-	while (tx_queue->insert_count != tx_queue->write_count) {
+	while (tx_queue->insert_count != old_insert_count) {
 		unsigned int pkts_compl = 0, bytes_compl = 0;
 		--tx_queue->insert_count;
 		buffer = __efx_tx_queue_get_insert_buffer(tx_queue);
@@ -989,12 +981,13 @@ static int efx_tso_put_header(struct efx_tx_queue *tx_queue,
 /* Remove buffers put into a tx_queue.  None of the buffers must have
  * an skb attached.
  */
-static void efx_enqueue_unwind(struct efx_tx_queue *tx_queue)
+static void efx_enqueue_unwind(struct efx_tx_queue *tx_queue,
+			       unsigned int insert_count)
 {
 	struct efx_tx_buffer *buffer;
 
 	/* Work backwards until we hit the original insert pointer value */
-	while (tx_queue->insert_count != tx_queue->write_count) {
+	while (tx_queue->insert_count != insert_count) {
 		--tx_queue->insert_count;
 		buffer = __efx_tx_queue_get_insert_buffer(tx_queue);
 		efx_dequeue_buffer(tx_queue, buffer, NULL, NULL);
@@ -1258,13 +1251,14 @@ static int efx_enqueue_skb_tso(struct efx_tx_queue *tx_queue,
 			       struct sk_buff *skb)
 {
 	struct efx_nic *efx = tx_queue->efx;
+	unsigned int old_insert_count = tx_queue->insert_count;
 	int frag_i, rc;
 	struct tso_state state;
 
 	/* Find the packet protocol and sanity-check it */
 	state.protocol = efx_tso_check_protocol(skb);
 
-	EFX_BUG_ON_PARANOID(tx_queue->write_count != tx_queue->insert_count);
+	EFX_BUG_ON_PARANOID(tx_queue->write_count > tx_queue->insert_count);
 
 	rc = tso_start(&state, efx, skb);
 	if (rc)
@@ -1308,11 +1302,12 @@ static int efx_enqueue_skb_tso(struct efx_tx_queue *tx_queue,
 
 	netdev_tx_sent_queue(tx_queue->core_txq, skb->len);
 
-	/* Pass off to hardware */
-	efx_nic_push_buffers(tx_queue);
-
 	efx_tx_maybe_stop_queue(tx_queue);
 
+	/* Pass off to hardware */
+	if (!skb->xmit_more || netif_xmit_stopped(tx_queue->core_txq))
+		efx_nic_push_buffers(tx_queue);
+
 	tx_queue->tso_bursts++;
 	return NETDEV_TX_OK;
 
@@ -1336,6 +1331,6 @@ static int efx_enqueue_skb_tso(struct efx_tx_queue *tx_queue,
 		dma_unmap_single(&efx->pci_dev->dev, state.header_dma_addr,
 				 state.header_unmap_len, DMA_TO_DEVICE);
 
-	efx_enqueue_unwind(tx_queue);
+	efx_enqueue_unwind(tx_queue, old_insert_count);
 	return NETDEV_TX_OK;
 }
diff --git a/drivers/net/ethernet/smsc/smc911x.c b/drivers/net/ethernet/smsc/smc911x.c
index 9778cba9fc74..e88df9c7f1c0 100644
--- a/drivers/net/ethernet/smsc/smc911x.c
+++ b/drivers/net/ethernet/smsc/smc911x.c
@@ -1927,9 +1927,6 @@ static int smc911x_probe(struct net_device *dev)
 	}
 	dev->irq = irq_canonicalize(dev->irq);
 
-	/* Fill in the fields of the device structure with ethernet values. */
-	ether_setup(dev);
-
 	dev->netdev_ops = &smc911x_netdev_ops;
 	dev->watchdog_timeo = msecs_to_jiffies(watchdog);
 	dev->ethtool_ops = &smc911x_ethtool_ops;
diff --git a/drivers/net/ethernet/smsc/smc91x.c b/drivers/net/ethernet/smsc/smc91x.c
index bcaa41af1e62..5e94d00b96b3 100644
--- a/drivers/net/ethernet/smsc/smc91x.c
+++ b/drivers/net/ethernet/smsc/smc91x.c
@@ -1967,9 +1967,6 @@ static int smc_probe(struct net_device *dev, void __iomem *ioaddr,
 	}
 	dev->irq = irq_canonicalize(dev->irq);
 
-	/* Fill in the fields of the device structure with ethernet values. */
-	ether_setup(dev);
-
 	dev->watchdog_timeo = msecs_to_jiffies(watchdog);
 	dev->netdev_ops = &smc_netdev_ops;
 	dev->ethtool_ops = &smc_ethtool_ops;
diff --git a/drivers/net/ethernet/smsc/smsc911x.c b/drivers/net/ethernet/smsc/smsc911x.c
index 5e13fa5524ae..affb29da353e 100644
--- a/drivers/net/ethernet/smsc/smsc911x.c
+++ b/drivers/net/ethernet/smsc/smsc911x.c
@@ -2255,7 +2255,6 @@ static int smsc911x_init(struct net_device *dev)
 	if (smsc911x_soft_reset(pdata))
 		return -ENODEV;
 
-	ether_setup(dev);
 	dev->flags |= IFF_MULTICAST;
 	netif_napi_add(dev, &pdata->napi, smsc911x_poll, SMSC_NAPI_WEIGHT);
 	dev->netdev_ops = &smsc911x_netdev_ops;
diff --git a/drivers/net/ethernet/stmicro/stmmac/Kconfig b/drivers/net/ethernet/stmicro/stmmac/Kconfig
index 2d09c116cbc8..b02d4a3ffa37 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Kconfig
+++ b/drivers/net/ethernet/stmicro/stmmac/Kconfig
@@ -26,6 +26,16 @@ config STMMAC_PLATFORM
 
 	  If unsure, say N.
 
+config DWMAC_MESON
+	bool "Amlogic Meson dwmac support"
+	depends on STMMAC_PLATFORM && ARCH_MESON
+	help
+	  Support for Ethernet controller on Amlogic Meson SoCs.
+
+	  This selects the Amlogic Meson SoC glue layer support for
+	  the stmmac device driver. This driver is used for Meson6 and
+	  Meson8 SoCs.
+
 config DWMAC_SOCFPGA
 	bool "SOCFPGA dwmac support"
 	depends on STMMAC_PLATFORM && MFD_SYSCON && (ARCH_SOCFPGA || COMPILE_TEST)
diff --git a/drivers/net/ethernet/stmicro/stmmac/Makefile b/drivers/net/ethernet/stmicro/stmmac/Makefile
index 18695ebef7e4..0533d0ba783d 100644
--- a/drivers/net/ethernet/stmicro/stmmac/Makefile
+++ b/drivers/net/ethernet/stmicro/stmmac/Makefile
@@ -1,6 +1,7 @@
 obj-$(CONFIG_STMMAC_ETH) += stmmac.o
 stmmac-$(CONFIG_STMMAC_PLATFORM) += stmmac_platform.o
 stmmac-$(CONFIG_STMMAC_PCI) += stmmac_pci.o
+stmmac-$(CONFIG_DWMAC_MESON) += dwmac-meson.o
 stmmac-$(CONFIG_DWMAC_SUNXI) += dwmac-sunxi.o
 stmmac-$(CONFIG_DWMAC_STI) += dwmac-sti.o
 stmmac-$(CONFIG_DWMAC_SOCFPGA) += dwmac-socfpga.o
diff --git a/drivers/net/ethernet/stmicro/stmmac/chain_mode.c b/drivers/net/ethernet/stmicro/stmmac/chain_mode.c
index c553f6b5a913..cf28daba4346 100644
--- a/drivers/net/ethernet/stmicro/stmmac/chain_mode.c
+++ b/drivers/net/ethernet/stmicro/stmmac/chain_mode.c
@@ -28,7 +28,7 @@
 
 #include "stmmac.h"
 
-static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
+static int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 {
 	struct stmmac_priv *priv = (struct stmmac_priv *)p;
 	unsigned int txsize = priv->dma_tx_size;
@@ -47,7 +47,9 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 
 	desc->des2 = dma_map_single(priv->device, skb->data,
 				    bmax, DMA_TO_DEVICE);
-	priv->tx_skbuff_dma[entry] = desc->des2;
+	if (dma_mapping_error(priv->device, desc->des2))
+		return -1;
+	priv->tx_skbuff_dma[entry].buf = desc->des2;
 	priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum, STMMAC_CHAIN_MODE);
 
 	while (len != 0) {
@@ -59,7 +61,9 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 			desc->des2 = dma_map_single(priv->device,
 						    (skb->data + bmax * i),
 						    bmax, DMA_TO_DEVICE);
-			priv->tx_skbuff_dma[entry] = desc->des2;
+			if (dma_mapping_error(priv->device, desc->des2))
+				return -1;
+			priv->tx_skbuff_dma[entry].buf = desc->des2;
 			priv->hw->desc->prepare_tx_desc(desc, 0, bmax, csum,
 							STMMAC_CHAIN_MODE);
 			priv->hw->desc->set_tx_owner(desc);
@@ -69,7 +73,9 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 			desc->des2 = dma_map_single(priv->device,
 						    (skb->data + bmax * i), len,
 						    DMA_TO_DEVICE);
-			priv->tx_skbuff_dma[entry] = desc->des2;
+			if (dma_mapping_error(priv->device, desc->des2))
+				return -1;
+			priv->tx_skbuff_dma[entry].buf = desc->des2;
 			priv->hw->desc->prepare_tx_desc(desc, 0, len, csum,
 							STMMAC_CHAIN_MODE);
 			priv->hw->desc->set_tx_owner(desc);
diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h
index de507c32036c..593e6c4144a7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/common.h
+++ b/drivers/net/ethernet/stmicro/stmmac/common.h
@@ -220,10 +220,10 @@ enum dma_irq_status {
 	handle_tx = 0x8,
 };
 
-#define	CORE_IRQ_TX_PATH_IN_LPI_MODE	(1 << 1)
-#define	CORE_IRQ_TX_PATH_EXIT_LPI_MODE	(1 << 2)
-#define	CORE_IRQ_RX_PATH_IN_LPI_MODE	(1 << 3)
-#define	CORE_IRQ_RX_PATH_EXIT_LPI_MODE	(1 << 4)
+#define	CORE_IRQ_TX_PATH_IN_LPI_MODE	(1 << 0)
+#define	CORE_IRQ_TX_PATH_EXIT_LPI_MODE	(1 << 1)
+#define	CORE_IRQ_RX_PATH_IN_LPI_MODE	(1 << 2)
+#define	CORE_IRQ_RX_PATH_EXIT_LPI_MODE	(1 << 3)
 
 #define	CORE_PCS_ANE_COMPLETE		(1 << 5)
 #define	CORE_PCS_LINK_STATUS		(1 << 6)
@@ -287,7 +287,7 @@ struct dma_features {
 
 /* Default LPI timers */
 #define STMMAC_DEFAULT_LIT_LS	0x3E8
-#define STMMAC_DEFAULT_TWT_LS	0x0
+#define STMMAC_DEFAULT_TWT_LS	0x1E
 
 #define STMMAC_CHAIN_MODE	0x1
 #define STMMAC_RING_MODE	0x2
@@ -425,7 +425,7 @@ struct stmmac_mode_ops {
 	void (*init) (void *des, dma_addr_t phy_addr, unsigned int size,
 		      unsigned int extend_desc);
 	unsigned int (*is_jumbo_frm) (int len, int ehn_desc);
-	unsigned int (*jumbo_frm) (void *priv, struct sk_buff *skb, int csum);
+	int (*jumbo_frm)(void *priv, struct sk_buff *skb, int csum);
 	int (*set_16kib_bfsize)(int mtu);
 	void (*init_desc3)(struct dma_desc *p);
 	void (*refill_desc3) (void *priv, struct dma_desc *p);
@@ -445,6 +445,7 @@ struct mac_device_info {
 	int multicast_filter_bins;
 	int unicast_filter_entries;
 	int mcast_bits_log2;
+	unsigned int rx_csum;
 };
 
 struct mac_device_info *dwmac1000_setup(void __iomem *ioaddr, int mcbins,
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
new file mode 100644
index 000000000000..d225a603e604
--- /dev/null
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-meson.c
@@ -0,0 +1,67 @@
+/*
+ * Amlogic Meson DWMAC glue layer
+ *
+ * Copyright (C) 2014 Beniamino Galvani <b.galvani@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/device.h>
+#include <linux/ethtool.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/platform_device.h>
+#include <linux/stmmac.h>
+
+#define ETHMAC_SPEED_100	BIT(1)
+
+struct meson_dwmac {
+	struct device	*dev;
+	void __iomem	*reg;
+};
+
+static void meson6_dwmac_fix_mac_speed(void *priv, unsigned int speed)
+{
+	struct meson_dwmac *dwmac = priv;
+	unsigned int val;
+
+	val = readl(dwmac->reg);
+
+	switch (speed) {
+	case SPEED_10:
+		val &= ~ETHMAC_SPEED_100;
+		break;
+	case SPEED_100:
+		val |= ETHMAC_SPEED_100;
+		break;
+	}
+
+	writel(val, dwmac->reg);
+}
+
+static void *meson6_dwmac_setup(struct platform_device *pdev)
+{
+	struct meson_dwmac *dwmac;
+	struct resource *res;
+
+	dwmac = devm_kzalloc(&pdev->dev, sizeof(*dwmac), GFP_KERNEL);
+	if (!dwmac)
+		return ERR_PTR(-ENOMEM);
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	dwmac->reg = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(dwmac->reg))
+		return dwmac->reg;
+
+	return dwmac;
+}
+
+const struct stmmac_of_data meson6_dwmac_data = {
+	.setup		= meson6_dwmac_setup,
+	.fix_mac_speed	= meson6_dwmac_fix_mac_speed,
+};
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
index ec632e666c56..3aad413e74b4 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-socfpga.c
@@ -17,6 +17,7 @@
 
 #include <linux/mfd/syscon.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/of_net.h>
 #include <linux/phy.h>
 #include <linux/regmap.h>
@@ -30,6 +31,12 @@
 #define SYSMGR_EMACGRP_CTRL_PHYSEL_WIDTH 2
 #define SYSMGR_EMACGRP_CTRL_PHYSEL_MASK 0x00000003
 
+#define EMAC_SPLITTER_CTRL_REG			0x0
+#define EMAC_SPLITTER_CTRL_SPEED_MASK		0x3
+#define EMAC_SPLITTER_CTRL_SPEED_10		0x2
+#define EMAC_SPLITTER_CTRL_SPEED_100		0x3
+#define EMAC_SPLITTER_CTRL_SPEED_1000		0x0
+
 struct socfpga_dwmac {
 	int	interface;
 	u32	reg_offset;
@@ -37,14 +44,46 @@ struct socfpga_dwmac {
 	struct	device *dev;
 	struct regmap *sys_mgr_base_addr;
 	struct reset_control *stmmac_rst;
+	void __iomem *splitter_base;
 };
 
+static void socfpga_dwmac_fix_mac_speed(void *priv, unsigned int speed)
+{
+	struct socfpga_dwmac *dwmac = (struct socfpga_dwmac *)priv;
+	void __iomem *splitter_base = dwmac->splitter_base;
+	u32 val;
+
+	if (!splitter_base)
+		return;
+
+	val = readl(splitter_base + EMAC_SPLITTER_CTRL_REG);
+	val &= ~EMAC_SPLITTER_CTRL_SPEED_MASK;
+
+	switch (speed) {
+	case 1000:
+		val |= EMAC_SPLITTER_CTRL_SPEED_1000;
+		break;
+	case 100:
+		val |= EMAC_SPLITTER_CTRL_SPEED_100;
+		break;
+	case 10:
+		val |= EMAC_SPLITTER_CTRL_SPEED_10;
+		break;
+	default:
+		return;
+	}
+
+	writel(val, splitter_base + EMAC_SPLITTER_CTRL_REG);
+}
+
 static int socfpga_dwmac_parse_data(struct socfpga_dwmac *dwmac, struct device *dev)
 {
 	struct device_node *np = dev->of_node;
 	struct regmap *sys_mgr_base_addr;
 	u32 reg_offset, reg_shift;
 	int ret;
+	struct device_node *np_splitter;
+	struct resource res_splitter;
 
 	dwmac->stmmac_rst = devm_reset_control_get(dev,
 						  STMMAC_RESOURCE_NAME);
@@ -73,6 +112,20 @@ static int socfpga_dwmac_parse_data(struct socfpga_dwmac *dwmac, struct device *
 		return -EINVAL;
 	}
 
+	np_splitter = of_parse_phandle(np, "altr,emac-splitter", 0);
+	if (np_splitter) {
+		if (of_address_to_resource(np_splitter, 0, &res_splitter)) {
+			dev_info(dev, "Missing emac splitter address\n");
+			return -EINVAL;
+		}
+
+		dwmac->splitter_base = devm_ioremap_resource(dev, &res_splitter);
+		if (IS_ERR(dwmac->splitter_base)) {
+			dev_info(dev, "Failed to mapping emac splitter\n");
+			return PTR_ERR(dwmac->splitter_base);
+		}
+	}
+
 	dwmac->reg_offset = reg_offset;
 	dwmac->reg_shift = reg_shift;
 	dwmac->sys_mgr_base_addr = sys_mgr_base_addr;
@@ -91,6 +144,7 @@ static int socfpga_dwmac_setup(struct socfpga_dwmac *dwmac)
 
 	switch (phymode) {
 	case PHY_INTERFACE_MODE_RGMII:
+	case PHY_INTERFACE_MODE_RGMII_ID:
 		val = SYSMGR_EMACGRP_CTRL_PHYSEL_ENUM_RGMII;
 		break;
 	case PHY_INTERFACE_MODE_MII:
@@ -102,6 +156,13 @@ static int socfpga_dwmac_setup(struct socfpga_dwmac *dwmac)
 		return -EINVAL;
 	}
 
+	/* Overwrite val to GMII if splitter core is enabled. The phymode here
+	 * is the actual phy mode on phy hardware, but phy interface from
+	 * EMAC core is GMII.
+	 */
+	if (dwmac->splitter_base)
+		val = SYSMGR_EMACGRP_CTRL_PHYSEL_ENUM_GMII_MII;
+
 	regmap_read(sys_mgr_base_addr, reg_offset, &ctrl);
 	ctrl &= ~(SYSMGR_EMACGRP_CTRL_PHYSEL_MASK << reg_shift);
 	ctrl |= val << reg_shift;
@@ -196,4 +257,5 @@ const struct stmmac_of_data socfpga_gmac_data = {
 	.setup = socfpga_dwmac_probe,
 	.init = socfpga_dwmac_init,
 	.exit = socfpga_dwmac_exit,
+	.fix_mac_speed = socfpga_dwmac_fix_mac_speed,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c b/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
index 552bbc17863c..ccfe7e510418 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac-sti.c
@@ -3,7 +3,7 @@
  *
  * Copyright (C) 2003-2014 STMicroelectronics (R&D) Limited
  * Author: Srinivas Kandagatla <srinivas.kandagatla@st.com>
- *
+ * Contributors: Giuseppe Cavallaro <peppe.cavallaro@st.com>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -22,45 +22,22 @@
 #include <linux/of.h>
 #include <linux/of_net.h>
 
+#define DWMAC_125MHZ	125000000
+#define DWMAC_50MHZ	50000000
+#define DWMAC_25MHZ	25000000
+#define DWMAC_2_5MHZ	2500000
+
+#define IS_PHY_IF_MODE_RGMII(iface)	(iface == PHY_INTERFACE_MODE_RGMII || \
+			iface == PHY_INTERFACE_MODE_RGMII_ID || \
+			iface == PHY_INTERFACE_MODE_RGMII_RXID || \
+			iface == PHY_INTERFACE_MODE_RGMII_TXID)
+
+#define IS_PHY_IF_MODE_GBIT(iface)	(IS_PHY_IF_MODE_RGMII(iface) || \
+					 iface == PHY_INTERFACE_MODE_GMII)
+
+/* STiH4xx register definitions (STiH415/STiH416/STiH407/STiH410 families) */
+
 /**
- *			STi GMAC glue logic.
- *			--------------------
- *
- *		 _
- *		|  \
- *	--------|0  \ ETH_SEL_INTERNAL_NOTEXT_PHYCLK
- * phyclk	|    |___________________________________________
- *		|    |	|			(phyclk-in)
- *	--------|1  /	|
- * int-clk	|_ /	|
- *			|	 _
- *			|	|  \
- *			|_______|1  \ ETH_SEL_TX_RETIME_CLK
- *				|    |___________________________
- *				|    |		(tx-retime-clk)
- *			 _______|0  /
- *			|	|_ /
- *		 _	|
- *		|  \	|
- *	--------|0  \	|
- * clk_125	|    |__|
- *		|    |	ETH_SEL_TXCLK_NOT_CLK125
- *	--------|1  /
- * txclk	|_ /
- *
- *
- * ETH_SEL_INTERNAL_NOTEXT_PHYCLK is valid only for RMII where PHY can
- * generate 50MHz clock or MAC can generate it.
- * This bit is configured by "st,ext-phyclk" property.
- *
- * ETH_SEL_TXCLK_NOT_CLK125 is only valid for gigabit modes, where the 125Mhz
- * clock either comes from clk-125 pin or txclk pin. This configuration is
- * totally driven by the board wiring. This bit is configured by
- * "st,tx-retime-src" property.
- *
- * TXCLK configuration is different for different phy interface modes
- * and changes according to link speed in modes like RGMII.
- *
  * Below table summarizes the clock requirement and clock sources for
  * supported phy interface modes with link speeds.
  * ________________________________________________
@@ -74,44 +51,58 @@
  * ------------------------------------------------
  *|	RGMII	|     125Mhz	 |	25Mhz	   |
  *|		|  clk-125/txclk |	clkgen     |
+ *|		|    clkgen	 |		   |
  * ------------------------------------------------
  *|	RMII	|	n/a	 |	25Mhz	   |
  *|		|		 |clkgen/phyclk-in |
  * ------------------------------------------------
  *
- * TX lines are always retimed with a clk, which can vary depending
- * on the board configuration. Below is the table of these bits
- * in eth configuration register depending on source of retime clk.
- *
- *---------------------------------------------------------------
- * src	 | tx_rt_clk	| int_not_ext_phyclk	| txclk_n_clk125|
- *---------------------------------------------------------------
- * txclk |	0	|	n/a		|	1	|
- *---------------------------------------------------------------
- * ck_125|	0	|	n/a		|	0	|
- *---------------------------------------------------------------
- * phyclk|	1	|	0		|	n/a	|
- *---------------------------------------------------------------
- * clkgen|	1	|	1		|	n/a	|
- *---------------------------------------------------------------
+ *	  Register Configuration
+ *-------------------------------
+ * src	 |BIT(8)| BIT(7)| BIT(6)|
+ *-------------------------------
+ * txclk |   0	|  n/a	|   1	|
+ *-------------------------------
+ * ck_125|   0	|  n/a	|   0	|
+ *-------------------------------
+ * phyclk|   1	|   0	|  n/a	|
+ *-------------------------------
+ * clkgen|   1	|   1	|  n/a	|
+ *-------------------------------
  */
 
- /* Register definition */
+#define STIH4XX_RETIME_SRC_MASK			GENMASK(8, 6)
+#define STIH4XX_ETH_SEL_TX_RETIME_CLK		BIT(8)
+#define STIH4XX_ETH_SEL_INTERNAL_NOTEXT_PHYCLK	BIT(7)
+#define STIH4XX_ETH_SEL_TXCLK_NOT_CLK125	BIT(6)
+
+/* STiD127 register definitions */
 
- /* 3 bits [8:6]
-  *  [6:6]      ETH_SEL_TXCLK_NOT_CLK125
-  *  [7:7]      ETH_SEL_INTERNAL_NOTEXT_PHYCLK
-  *  [8:8]      ETH_SEL_TX_RETIME_CLK
-  *
-  */
+/**
+ *-----------------------
+ * src	 |BIT(6)| BIT(7)|
+ *-----------------------
+ * MII   |  1	|   n/a	|
+ *-----------------------
+ * RMII  |  n/a	|   1	|
+ * clkgen|	|	|
+ *-----------------------
+ * RMII  |  n/a	|   0	|
+ * phyclk|	|	|
+ *-----------------------
+ * RGMII |  1	|  n/a	|
+ * clkgen|	|	|
+ *-----------------------
+ */
 
-#define TX_RETIME_SRC_MASK		GENMASK(8, 6)
-#define ETH_SEL_TX_RETIME_CLK		BIT(8)
-#define ETH_SEL_INTERNAL_NOTEXT_PHYCLK	BIT(7)
-#define ETH_SEL_TXCLK_NOT_CLK125	BIT(6)
+#define STID127_RETIME_SRC_MASK			GENMASK(7, 6)
+#define STID127_ETH_SEL_INTERNAL_NOTEXT_PHYCLK	BIT(7)
+#define STID127_ETH_SEL_INTERNAL_NOTEXT_TXCLK	BIT(6)
 
-#define ENMII_MASK			GENMASK(5, 5)
-#define ENMII				BIT(5)
+#define ENMII_MASK	GENMASK(5, 5)
+#define ENMII		BIT(5)
+#define EN_MASK		GENMASK(1, 1)
+#define EN		BIT(1)
 
 /**
  * 3 bits [4:2]
@@ -120,29 +111,23 @@
  *	010-SGMII
  *	100-RMII
 */
-#define MII_PHY_SEL_MASK		GENMASK(4, 2)
-#define ETH_PHY_SEL_RMII		BIT(4)
-#define ETH_PHY_SEL_SGMII		BIT(3)
-#define ETH_PHY_SEL_RGMII		BIT(2)
-#define ETH_PHY_SEL_GMII		0x0
-#define ETH_PHY_SEL_MII			0x0
-
-#define IS_PHY_IF_MODE_RGMII(iface)	(iface == PHY_INTERFACE_MODE_RGMII || \
-			iface == PHY_INTERFACE_MODE_RGMII_ID || \
-			iface == PHY_INTERFACE_MODE_RGMII_RXID || \
-			iface == PHY_INTERFACE_MODE_RGMII_TXID)
-
-#define IS_PHY_IF_MODE_GBIT(iface)	(IS_PHY_IF_MODE_RGMII(iface) || \
-			iface == PHY_INTERFACE_MODE_GMII)
+#define MII_PHY_SEL_MASK	GENMASK(4, 2)
+#define ETH_PHY_SEL_RMII	BIT(4)
+#define ETH_PHY_SEL_SGMII	BIT(3)
+#define ETH_PHY_SEL_RGMII	BIT(2)
+#define ETH_PHY_SEL_GMII	0x0
+#define ETH_PHY_SEL_MII		0x0
 
 struct sti_dwmac {
-	int interface;
-	bool ext_phyclk;
-	bool is_tx_retime_src_clk_125;
-	struct clk *clk;
-	int reg;
+	int interface;		/* MII interface */
+	bool ext_phyclk;	/* Clock from external PHY */
+	u32 tx_retime_src;	/* TXCLK Retiming*/
+	struct clk *clk;	/* PHY clock */
+	int ctrl_reg;		/* GMAC glue-logic control register */
+	int clk_sel_reg;	/* GMAC ext clk selection register */
 	struct device *dev;
 	struct regmap *regmap;
+	u32 speed;
 };
 
 static u32 phy_intf_sels[] = {
@@ -162,74 +147,133 @@ enum {
 	TX_RETIME_SRC_CLKGEN,
 };
 
-static const char *const tx_retime_srcs[] = {
-	[TX_RETIME_SRC_NA] = "",
-	[TX_RETIME_SRC_TXCLK] = "txclk",
-	[TX_RETIME_SRC_CLK_125] = "clk_125",
-	[TX_RETIME_SRC_PHYCLK] = "phyclk",
-	[TX_RETIME_SRC_CLKGEN] = "clkgen",
-};
-
-static u32 tx_retime_val[] = {
-	[TX_RETIME_SRC_TXCLK] = ETH_SEL_TXCLK_NOT_CLK125,
+static u32 stih4xx_tx_retime_val[] = {
+	[TX_RETIME_SRC_TXCLK] = STIH4XX_ETH_SEL_TXCLK_NOT_CLK125,
 	[TX_RETIME_SRC_CLK_125] = 0x0,
-	[TX_RETIME_SRC_PHYCLK] = ETH_SEL_TX_RETIME_CLK,
-	[TX_RETIME_SRC_CLKGEN] = ETH_SEL_TX_RETIME_CLK |
-	    ETH_SEL_INTERNAL_NOTEXT_PHYCLK,
+	[TX_RETIME_SRC_PHYCLK] = STIH4XX_ETH_SEL_TX_RETIME_CLK,
+	[TX_RETIME_SRC_CLKGEN] = STIH4XX_ETH_SEL_TX_RETIME_CLK
+				 | STIH4XX_ETH_SEL_INTERNAL_NOTEXT_PHYCLK,
 };
 
-static void setup_retime_src(struct sti_dwmac *dwmac, u32 spd)
+static void stih4xx_fix_retime_src(void *priv, u32 spd)
 {
-	u32 src = 0, freq = 0;
-
-	if (spd == SPEED_100) {
-		if (dwmac->interface == PHY_INTERFACE_MODE_MII ||
-		    dwmac->interface == PHY_INTERFACE_MODE_GMII) {
-			src = TX_RETIME_SRC_TXCLK;
-		} else if (dwmac->interface == PHY_INTERFACE_MODE_RMII) {
-			if (dwmac->ext_phyclk) {
-				src = TX_RETIME_SRC_PHYCLK;
-			} else {
-				src = TX_RETIME_SRC_CLKGEN;
-				freq = 50000000;
-			}
-
-		} else if (IS_PHY_IF_MODE_RGMII(dwmac->interface)) {
+	struct sti_dwmac *dwmac = priv;
+	u32 src = dwmac->tx_retime_src;
+	u32 reg = dwmac->ctrl_reg;
+	u32 freq = 0;
+
+	if (dwmac->interface == PHY_INTERFACE_MODE_MII) {
+		src = TX_RETIME_SRC_TXCLK;
+	} else if (dwmac->interface == PHY_INTERFACE_MODE_RMII) {
+		if (dwmac->ext_phyclk) {
+			src = TX_RETIME_SRC_PHYCLK;
+		} else {
 			src = TX_RETIME_SRC_CLKGEN;
-			freq = 25000000;
+			freq = DWMAC_50MHZ;
 		}
+	} else if (IS_PHY_IF_MODE_RGMII(dwmac->interface)) {
+		/* On GiGa clk source can be either ext or from clkgen */
+		if (spd == SPEED_1000) {
+			freq = DWMAC_125MHZ;
+		} else {
+			/* Switch to clkgen for these speeds */
+			src = TX_RETIME_SRC_CLKGEN;
+			if (spd == SPEED_100)
+				freq = DWMAC_25MHZ;
+			else if (spd == SPEED_10)
+				freq = DWMAC_2_5MHZ;
+		}
+	}
 
-		if (src == TX_RETIME_SRC_CLKGEN && dwmac->clk)
-			clk_set_rate(dwmac->clk, freq);
+	if (src == TX_RETIME_SRC_CLKGEN && dwmac->clk && freq)
+		clk_set_rate(dwmac->clk, freq);
 
-	} else if (spd == SPEED_1000) {
-		if (dwmac->is_tx_retime_src_clk_125)
-			src = TX_RETIME_SRC_CLK_125;
-		else
-			src = TX_RETIME_SRC_TXCLK;
+	regmap_update_bits(dwmac->regmap, reg, STIH4XX_RETIME_SRC_MASK,
+			   stih4xx_tx_retime_val[src]);
+}
+
+static void stid127_fix_retime_src(void *priv, u32 spd)
+{
+	struct sti_dwmac *dwmac = priv;
+	u32 reg = dwmac->ctrl_reg;
+	u32 freq = 0;
+	u32 val = 0;
+
+	if (dwmac->interface == PHY_INTERFACE_MODE_MII) {
+		val = STID127_ETH_SEL_INTERNAL_NOTEXT_TXCLK;
+	} else if (dwmac->interface == PHY_INTERFACE_MODE_RMII) {
+		if (!dwmac->ext_phyclk) {
+			val = STID127_ETH_SEL_INTERNAL_NOTEXT_PHYCLK;
+			freq = DWMAC_50MHZ;
+		}
+	} else if (IS_PHY_IF_MODE_RGMII(dwmac->interface)) {
+		val = STID127_ETH_SEL_INTERNAL_NOTEXT_TXCLK;
+		if (spd == SPEED_1000)
+			freq = DWMAC_125MHZ;
+		else if (spd == SPEED_100)
+			freq = DWMAC_25MHZ;
+		else if (spd == SPEED_10)
+			freq = DWMAC_2_5MHZ;
 	}
 
-	regmap_update_bits(dwmac->regmap, dwmac->reg,
-			   TX_RETIME_SRC_MASK, tx_retime_val[src]);
+	if (dwmac->clk && freq)
+		clk_set_rate(dwmac->clk, freq);
+
+	regmap_update_bits(dwmac->regmap, reg, STID127_RETIME_SRC_MASK, val);
 }
 
-static void sti_dwmac_exit(struct platform_device *pdev, void *priv)
+static void sti_dwmac_ctrl_init(struct sti_dwmac *dwmac)
 {
-	struct sti_dwmac *dwmac = priv;
+	struct regmap *regmap = dwmac->regmap;
+	int iface = dwmac->interface;
+	struct device *dev = dwmac->dev;
+	struct device_node *np = dev->of_node;
+	u32 reg = dwmac->ctrl_reg;
+	u32 val;
 
 	if (dwmac->clk)
-		clk_disable_unprepare(dwmac->clk);
+		clk_prepare_enable(dwmac->clk);
+
+	if (of_property_read_bool(np, "st,gmac_en"))
+		regmap_update_bits(regmap, reg, EN_MASK, EN);
+
+	regmap_update_bits(regmap, reg, MII_PHY_SEL_MASK, phy_intf_sels[iface]);
+
+	val = (iface == PHY_INTERFACE_MODE_REVMII) ? 0 : ENMII;
+	regmap_update_bits(regmap, reg, ENMII_MASK, val);
+}
+
+static int stix4xx_init(struct platform_device *pdev, void *priv)
+{
+	struct sti_dwmac *dwmac = priv;
+	u32 spd = dwmac->speed;
+
+	sti_dwmac_ctrl_init(dwmac);
+
+	stih4xx_fix_retime_src(priv, spd);
+
+	return 0;
 }
 
-static void sti_fix_mac_speed(void *priv, unsigned int spd)
+static int stid127_init(struct platform_device *pdev, void *priv)
 {
 	struct sti_dwmac *dwmac = priv;
+	u32 spd = dwmac->speed;
 
-	setup_retime_src(dwmac, spd);
+	sti_dwmac_ctrl_init(dwmac);
 
-	return;
+	stid127_fix_retime_src(priv, spd);
+
+	return 0;
 }
 
+static void sti_dwmac_exit(struct platform_device *pdev, void *priv)
+{
+	struct sti_dwmac *dwmac = priv;
+
+	if (dwmac->clk)
+		clk_disable_unprepare(dwmac->clk);
+}
 static int sti_dwmac_parse_data(struct sti_dwmac *dwmac,
 				struct platform_device *pdev)
 {
@@ -245,6 +289,13 @@ static int sti_dwmac_parse_data(struct sti_dwmac *dwmac,
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sti-ethconf");
 	if (!res)
 		return -ENODATA;
+	dwmac->ctrl_reg = res->start;
+
+	/* clk selection from extra syscfg register */
+	dwmac->clk_sel_reg = -ENXIO;
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sti-clkconf");
+	if (res)
+		dwmac->clk_sel_reg = res->start;
 
 	regmap = syscon_regmap_lookup_by_phandle(np, "st,syscon");
 	if (IS_ERR(regmap))
@@ -253,53 +304,31 @@ static int sti_dwmac_parse_data(struct sti_dwmac *dwmac,
 	dwmac->dev = dev;
 	dwmac->interface = of_get_phy_mode(np);
 	dwmac->regmap = regmap;
-	dwmac->reg = res->start;
 	dwmac->ext_phyclk = of_property_read_bool(np, "st,ext-phyclk");
-	dwmac->is_tx_retime_src_clk_125 = false;
+	dwmac->tx_retime_src = TX_RETIME_SRC_NA;
+	dwmac->speed = SPEED_100;
 
 	if (IS_PHY_IF_MODE_GBIT(dwmac->interface)) {
 		const char *rs;
+		dwmac->tx_retime_src = TX_RETIME_SRC_CLKGEN;
 
 		err = of_property_read_string(np, "st,tx-retime-src", &rs);
-		if (err < 0) {
-			dev_err(dev, "st,tx-retime-src not specified\n");
-			return err;
-		}
+		if (err < 0)
+			dev_warn(dev, "Use internal clock source\n");
 
 		if (!strcasecmp(rs, "clk_125"))
-			dwmac->is_tx_retime_src_clk_125 = true;
+			dwmac->tx_retime_src = TX_RETIME_SRC_CLK_125;
+		else if (!strcasecmp(rs, "txclk"))
+			dwmac->tx_retime_src = TX_RETIME_SRC_TXCLK;
+
+		dwmac->speed = SPEED_1000;
 	}
 
 	dwmac->clk = devm_clk_get(dev, "sti-ethclk");
-
-	if (IS_ERR(dwmac->clk))
+	if (IS_ERR(dwmac->clk)) {
+		dev_warn(dev, "No phy clock provided...\n");
 		dwmac->clk = NULL;
-
-	return 0;
-}
-
-static int sti_dwmac_init(struct platform_device *pdev, void *priv)
-{
-	struct sti_dwmac *dwmac = priv;
-	struct regmap *regmap = dwmac->regmap;
-	int iface = dwmac->interface;
-	u32 reg = dwmac->reg;
-	u32 val, spd;
-
-	if (dwmac->clk)
-		clk_prepare_enable(dwmac->clk);
-
-	regmap_update_bits(regmap, reg, MII_PHY_SEL_MASK, phy_intf_sels[iface]);
-
-	val = (iface == PHY_INTERFACE_MODE_REVMII) ? 0 : ENMII;
-	regmap_update_bits(regmap, reg, ENMII_MASK, val);
-
-	if (IS_PHY_IF_MODE_GBIT(iface))
-		spd = SPEED_1000;
-	else
-		spd = SPEED_100;
-
-	setup_retime_src(dwmac, spd);
+	}
 
 	return 0;
 }
@@ -322,9 +351,16 @@ static void *sti_dwmac_setup(struct platform_device *pdev)
 	return dwmac;
 }
 
-const struct stmmac_of_data sti_gmac_data = {
-	.fix_mac_speed = sti_fix_mac_speed,
+const struct stmmac_of_data stih4xx_dwmac_data = {
+	.fix_mac_speed = stih4xx_fix_retime_src,
+	.setup = sti_dwmac_setup,
+	.init = stix4xx_init,
+	.exit = sti_dwmac_exit,
+};
+
+const struct stmmac_of_data stid127_dwmac_data = {
+	.fix_mac_speed = stid127_fix_retime_src,
 	.setup = sti_dwmac_setup,
-	.init = sti_dwmac_init,
+	.init = stid127_init,
 	.exit = sti_dwmac_exit,
 };
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
index 71b5419256c1..64d8f56a9c17 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000.h
@@ -153,7 +153,7 @@ enum inter_frame_gap {
 #define GMAC_CONTROL_RE		0x00000004	/* Receiver Enable */
 
 #define GMAC_CORE_INIT (GMAC_CONTROL_JD | GMAC_CONTROL_PS | GMAC_CONTROL_ACS | \
-			GMAC_CONTROL_BE)
+			GMAC_CONTROL_BE | GMAC_CONTROL_DCRS)
 
 /* GMAC Frame Filter defines */
 #define GMAC_FRAME_FILTER_PR	0x00000001	/* Promiscuous Mode */
diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
index d8ef18786a1c..0adcf73cf722 100644
--- a/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/dwmac1000_core.c
@@ -58,7 +58,11 @@ static int dwmac1000_rx_ipc_enable(struct mac_device_info *hw)
 	void __iomem *ioaddr = hw->pcsr;
 	u32 value = readl(ioaddr + GMAC_CONTROL);
 
-	value |= GMAC_CONTROL_IPC;
+	if (hw->rx_csum)
+		value |= GMAC_CONTROL_IPC;
+	else
+		value &= ~GMAC_CONTROL_IPC;
+
 	writel(value, ioaddr + GMAC_CONTROL);
 
 	value = readl(ioaddr + GMAC_CONTROL);
@@ -130,7 +134,7 @@ static void dwmac1000_set_filter(struct mac_device_info *hw,
 	void __iomem *ioaddr = (void __iomem *)dev->base_addr;
 	unsigned int value = 0;
 	unsigned int perfect_addr_number = hw->unicast_filter_entries;
-	u32 mc_filter[2];
+	u32 mc_filter[8];
 	int mcbitslog2 = hw->mcast_bits_log2;
 
 	pr_debug("%s: # mcasts %d, # unicast %d\n", __func__,
@@ -178,7 +182,7 @@ static void dwmac1000_set_filter(struct mac_device_info *hw,
 		struct netdev_hw_addr *ha;
 
 		netdev_for_each_uc_addr(ha, dev) {
-			stmmac_get_mac_addr(ioaddr, ha->addr,
+			stmmac_set_mac_addr(ioaddr, ha->addr,
 					    GMAC_ADDR_HIGH(reg),
 					    GMAC_ADDR_LOW(reg));
 			reg++;
diff --git a/drivers/net/ethernet/stmicro/stmmac/mmc.h b/drivers/net/ethernet/stmicro/stmmac/mmc.h
index 8607488cbcfc..192c2491330b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/mmc.h
+++ b/drivers/net/ethernet/stmicro/stmmac/mmc.h
@@ -68,7 +68,7 @@ struct stmmac_counters {
 	unsigned int mmc_rx_octetcount_g;
 	unsigned int mmc_rx_broadcastframe_g;
 	unsigned int mmc_rx_multicastframe_g;
-	unsigned int mmc_rx_crc_errror;
+	unsigned int mmc_rx_crc_error;
 	unsigned int mmc_rx_align_error;
 	unsigned int mmc_rx_run_error;
 	unsigned int mmc_rx_jabber_error;
diff --git a/drivers/net/ethernet/stmicro/stmmac/mmc_core.c b/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
index 50617c5a0bdb..08c483bd2ec7 100644
--- a/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
+++ b/drivers/net/ethernet/stmicro/stmmac/mmc_core.c
@@ -196,7 +196,7 @@ void dwmac_mmc_read(void __iomem *ioaddr, struct stmmac_counters *mmc)
 	mmc->mmc_rx_octetcount_g += readl(ioaddr + MMC_RX_OCTETCOUNT_G);
 	mmc->mmc_rx_broadcastframe_g += readl(ioaddr + MMC_RX_BROADCASTFRAME_G);
 	mmc->mmc_rx_multicastframe_g += readl(ioaddr + MMC_RX_MULTICASTFRAME_G);
-	mmc->mmc_rx_crc_errror += readl(ioaddr + MMC_RX_CRC_ERRROR);
+	mmc->mmc_rx_crc_error += readl(ioaddr + MMC_RX_CRC_ERRROR);
 	mmc->mmc_rx_align_error += readl(ioaddr + MMC_RX_ALIGN_ERROR);
 	mmc->mmc_rx_run_error += readl(ioaddr + MMC_RX_RUN_ERROR);
 	mmc->mmc_rx_jabber_error += readl(ioaddr + MMC_RX_JABBER_ERROR);
diff --git a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
index 650a4be6bce5..5dd50c6cda5b 100644
--- a/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
+++ b/drivers/net/ethernet/stmicro/stmmac/ring_mode.c
@@ -28,7 +28,7 @@
 
 #include "stmmac.h"
 
-static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
+static int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 {
 	struct stmmac_priv *priv = (struct stmmac_priv *)p;
 	unsigned int txsize = priv->dma_tx_size;
@@ -53,7 +53,10 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 
 		desc->des2 = dma_map_single(priv->device, skb->data,
 					    bmax, DMA_TO_DEVICE);
-		priv->tx_skbuff_dma[entry] = desc->des2;
+		if (dma_mapping_error(priv->device, desc->des2))
+			return -1;
+
+		priv->tx_skbuff_dma[entry].buf = desc->des2;
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
 		priv->hw->desc->prepare_tx_desc(desc, 1, bmax, csum,
 						STMMAC_RING_MODE);
@@ -68,7 +71,9 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 
 		desc->des2 = dma_map_single(priv->device, skb->data + bmax,
 					    len, DMA_TO_DEVICE);
-		priv->tx_skbuff_dma[entry] = desc->des2;
+		if (dma_mapping_error(priv->device, desc->des2))
+			return -1;
+		priv->tx_skbuff_dma[entry].buf = desc->des2;
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
 		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum,
 						STMMAC_RING_MODE);
@@ -77,7 +82,9 @@ static unsigned int stmmac_jumbo_frm(void *p, struct sk_buff *skb, int csum)
 	} else {
 		desc->des2 = dma_map_single(priv->device, skb->data,
 					    nopaged_len, DMA_TO_DEVICE);
-		priv->tx_skbuff_dma[entry] = desc->des2;
+		if (dma_mapping_error(priv->device, desc->des2))
+			return -1;
+		priv->tx_skbuff_dma[entry].buf = desc->des2;
 		desc->des3 = desc->des2 + BUF_SIZE_4KiB;
 		priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len, csum,
 						STMMAC_RING_MODE);
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
index ca01035634a7..c3c40650b309 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h
@@ -34,6 +34,11 @@
 #include <linux/ptp_clock_kernel.h>
 #include <linux/reset.h>
 
+struct stmmac_tx_info {
+	dma_addr_t buf;
+	bool map_as_page;
+};
+
 struct stmmac_priv {
 	/* Frequently used values are kept adjacent for cache effect */
 	struct dma_extended_desc *dma_etx ____cacheline_aligned_in_smp;
@@ -45,7 +50,7 @@ struct stmmac_priv {
 	u32 tx_count_frames;
 	u32 tx_coal_frames;
 	u32 tx_coal_timer;
-	dma_addr_t *tx_skbuff_dma;
+	struct stmmac_tx_info *tx_skbuff_dma;
 	dma_addr_t dma_tx_phy;
 	int tx_coalesce;
 	int hwts_tx_en;
@@ -105,6 +110,8 @@ struct stmmac_priv {
 	struct ptp_clock *ptp_clock;
 	struct ptp_clock_info ptp_clock_ops;
 	unsigned int default_addend;
+	struct clk *clk_ptp_ref;
+	unsigned int clk_ptp_rate;
 	u32 adv_ts;
 	int use_riwt;
 	int irq_wake;
@@ -130,11 +137,15 @@ void stmmac_disable_eee_mode(struct stmmac_priv *priv);
 bool stmmac_eee_init(struct stmmac_priv *priv);
 
 #ifdef CONFIG_STMMAC_PLATFORM
+#ifdef CONFIG_DWMAC_MESON
+extern const struct stmmac_of_data meson6_dwmac_data;
+#endif
 #ifdef CONFIG_DWMAC_SUNXI
 extern const struct stmmac_of_data sun7i_gmac_data;
 #endif
 #ifdef CONFIG_DWMAC_STI
-extern const struct stmmac_of_data sti_gmac_data;
+extern const struct stmmac_of_data stih4xx_dwmac_data;
+extern const struct stmmac_of_data stid127_dwmac_data;
 #endif
 #ifdef CONFIG_DWMAC_SOCFPGA
 extern const struct stmmac_of_data socfpga_gmac_data;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
index 9af50bae4dde..3a08a1f78c73 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ethtool.c
@@ -175,7 +175,7 @@ static const struct stmmac_stats stmmac_mmc[] = {
 	STMMAC_MMC_STAT(mmc_rx_octetcount_g),
 	STMMAC_MMC_STAT(mmc_rx_broadcastframe_g),
 	STMMAC_MMC_STAT(mmc_rx_multicastframe_g),
-	STMMAC_MMC_STAT(mmc_rx_crc_errror),
+	STMMAC_MMC_STAT(mmc_rx_crc_error),
 	STMMAC_MMC_STAT(mmc_rx_align_error),
 	STMMAC_MMC_STAT(mmc_rx_run_error),
 	STMMAC_MMC_STAT(mmc_rx_jabber_error),
@@ -261,11 +261,11 @@ static int stmmac_ethtool_getsettings(struct net_device *dev,
 		ethtool_cmd_speed_set(cmd, priv->xstats.pcs_speed);
 
 		/* Get and convert ADV/LP_ADV from the HW AN registers */
-		if (priv->hw->mac->get_adv)
-			priv->hw->mac->get_adv(priv->hw, &adv);
-		else
+		if (!priv->hw->mac->get_adv)
 			return -EOPNOTSUPP;	/* should never happen indeed */
 
+		priv->hw->mac->get_adv(priv->hw, &adv);
+
 		/* Encoding of PSE bits is defined in 802.3z, 37.2.1.4 */
 
 		if (adv.pause & STMMAC_PCS_PAUSE)
@@ -340,19 +340,17 @@ static int stmmac_ethtool_setsettings(struct net_device *dev,
 		if (cmd->autoneg != AUTONEG_ENABLE)
 			return -EINVAL;
 
-		if (cmd->autoneg == AUTONEG_ENABLE) {
-			mask &= (ADVERTISED_1000baseT_Half |
+		mask &= (ADVERTISED_1000baseT_Half |
 			ADVERTISED_1000baseT_Full |
 			ADVERTISED_100baseT_Half |
 			ADVERTISED_100baseT_Full |
 			ADVERTISED_10baseT_Half |
 			ADVERTISED_10baseT_Full);
 
-			spin_lock(&priv->lock);
-			if (priv->hw->mac->ctrl_ane)
-				priv->hw->mac->ctrl_ane(priv->hw, 1);
-			spin_unlock(&priv->lock);
-		}
+		spin_lock(&priv->lock);
+		if (priv->hw->mac->ctrl_ane)
+			priv->hw->mac->ctrl_ane(priv->hw, 1);
+		spin_unlock(&priv->lock);
 
 		return 0;
 	}
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
index 08addd653728..6f77a46c7e2c 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c
@@ -275,6 +275,7 @@ static void stmmac_eee_ctrl_timer(unsigned long arg)
  */
 bool stmmac_eee_init(struct stmmac_priv *priv)
 {
+	char *phy_bus_name = priv->plat->phy_bus_name;
 	bool ret = false;
 
 	/* Using PCS we cannot dial with the phy registers at this stage
@@ -284,6 +285,10 @@ bool stmmac_eee_init(struct stmmac_priv *priv)
 	    (priv->pcs == STMMAC_PCS_RTBI))
 		goto out;
 
+	/* Never init EEE in case of a switch is attached */
+	if (phy_bus_name && (!strcmp(phy_bus_name, "fixed")))
+		goto out;
+
 	/* MAC core supports the EEE feature. */
 	if (priv->dma_cap.eee) {
 		int tx_lpi_timer = priv->tx_lpi_timer;
@@ -316,10 +321,9 @@ bool stmmac_eee_init(struct stmmac_priv *priv)
 			priv->hw->mac->set_eee_timer(priv->hw,
 						     STMMAC_DEFAULT_LIT_LS,
 						     tx_lpi_timer);
-		} else
-			/* Set HW EEE according to the speed */
-			priv->hw->mac->set_eee_pls(priv->hw,
-						   priv->phydev->link);
+		}
+		/* Set HW EEE according to the speed */
+		priv->hw->mac->set_eee_pls(priv->hw, priv->phydev->link);
 
 		pr_debug("stmmac: Energy-Efficient Ethernet initialized\n");
 
@@ -603,16 +607,16 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr)
 		/* calculate default added value:
 		 * formula is :
 		 * addend = (2^32)/freq_div_ratio;
-		 * where, freq_div_ratio = STMMAC_SYSCLOCK/50MHz
-		 * hence, addend = ((2^32) * 50MHz)/STMMAC_SYSCLOCK;
-		 * NOTE: STMMAC_SYSCLOCK should be >= 50MHz to
+		 * where, freq_div_ratio = clk_ptp_ref_i/50MHz
+		 * hence, addend = ((2^32) * 50MHz)/clk_ptp_ref_i;
+		 * NOTE: clk_ptp_ref_i should be >= 50MHz to
 		 *       achive 20ns accuracy.
 		 *
 		 * 2^x * y == (y << x), hence
 		 * 2^32 * 50000000 ==> (50000000 << 32)
 		 */
 		temp = (u64) (50000000ULL << 32);
-		priv->default_addend = div_u64(temp, STMMAC_SYSCLOCK);
+		priv->default_addend = div_u64(temp, priv->clk_ptp_rate);
 		priv->hw->ptp->config_addend(priv->ioaddr,
 					     priv->default_addend);
 
@@ -638,6 +642,16 @@ static int stmmac_init_ptp(struct stmmac_priv *priv)
 	if (!(priv->dma_cap.time_stamp || priv->dma_cap.atime_stamp))
 		return -EOPNOTSUPP;
 
+	/* Fall-back to main clock in case of no PTP ref is passed */
+	priv->clk_ptp_ref = devm_clk_get(priv->device, "clk_ptp_ref");
+	if (IS_ERR(priv->clk_ptp_ref)) {
+		priv->clk_ptp_rate = clk_get_rate(priv->stmmac_clk);
+		priv->clk_ptp_ref = NULL;
+	} else {
+		clk_prepare_enable(priv->clk_ptp_ref);
+		priv->clk_ptp_rate = clk_get_rate(priv->clk_ptp_ref);
+	}
+
 	priv->adv_ts = 0;
 	if (priv->dma_cap.atime_stamp && priv->extend_desc)
 		priv->adv_ts = 1;
@@ -657,6 +671,8 @@ static int stmmac_init_ptp(struct stmmac_priv *priv)
 
 static void stmmac_release_ptp(struct stmmac_priv *priv)
 {
+	if (priv->clk_ptp_ref)
+		clk_disable_unprepare(priv->clk_ptp_ref);
 	stmmac_ptp_unregister(priv);
 }
 
@@ -818,7 +834,7 @@ static int stmmac_init_phy(struct net_device *dev)
 	/* Stop Advertising 1000BASE Capability if interface is not GMII */
 	if ((interface == PHY_INTERFACE_MODE_MII) ||
 	    (interface == PHY_INTERFACE_MODE_RMII) ||
-		(max_speed < 1000 &&  max_speed > 0))
+		(max_speed < 1000 && max_speed > 0))
 		phydev->advertising &= ~(SUPPORTED_1000baseT_Half |
 					 SUPPORTED_1000baseT_Full);
 
@@ -1061,7 +1077,8 @@ static int init_dma_desc_rings(struct net_device *dev)
 		else
 			p = priv->dma_tx + i;
 		p->des2 = 0;
-		priv->tx_skbuff_dma[i] = 0;
+		priv->tx_skbuff_dma[i].buf = 0;
+		priv->tx_skbuff_dma[i].map_as_page = false;
 		priv->tx_skbuff[i] = NULL;
 	}
 
@@ -1100,17 +1117,24 @@ static void dma_free_tx_skbufs(struct stmmac_priv *priv)
 		else
 			p = priv->dma_tx + i;
 
-		if (priv->tx_skbuff_dma[i]) {
-			dma_unmap_single(priv->device,
-					 priv->tx_skbuff_dma[i],
-					 priv->hw->desc->get_tx_len(p),
-					 DMA_TO_DEVICE);
-			priv->tx_skbuff_dma[i] = 0;
+		if (priv->tx_skbuff_dma[i].buf) {
+			if (priv->tx_skbuff_dma[i].map_as_page)
+				dma_unmap_page(priv->device,
+					       priv->tx_skbuff_dma[i].buf,
+					       priv->hw->desc->get_tx_len(p),
+					       DMA_TO_DEVICE);
+			else
+				dma_unmap_single(priv->device,
+						 priv->tx_skbuff_dma[i].buf,
+						 priv->hw->desc->get_tx_len(p),
+						 DMA_TO_DEVICE);
 		}
 
 		if (priv->tx_skbuff[i] != NULL) {
 			dev_kfree_skb_any(priv->tx_skbuff[i]);
 			priv->tx_skbuff[i] = NULL;
+			priv->tx_skbuff_dma[i].buf = 0;
+			priv->tx_skbuff_dma[i].map_as_page = false;
 		}
 	}
 }
@@ -1131,7 +1155,8 @@ static int alloc_dma_desc_resources(struct stmmac_priv *priv)
 	if (!priv->rx_skbuff)
 		goto err_rx_skbuff;
 
-	priv->tx_skbuff_dma = kmalloc_array(txsize, sizeof(dma_addr_t),
+	priv->tx_skbuff_dma = kmalloc_array(txsize,
+					    sizeof(*priv->tx_skbuff_dma),
 					    GFP_KERNEL);
 	if (!priv->tx_skbuff_dma)
 		goto err_tx_skbuff_dma;
@@ -1293,12 +1318,19 @@ static void stmmac_tx_clean(struct stmmac_priv *priv)
 			pr_debug("%s: curr %d, dirty %d\n", __func__,
 				 priv->cur_tx, priv->dirty_tx);
 
-		if (likely(priv->tx_skbuff_dma[entry])) {
-			dma_unmap_single(priv->device,
-					 priv->tx_skbuff_dma[entry],
-					 priv->hw->desc->get_tx_len(p),
-					 DMA_TO_DEVICE);
-			priv->tx_skbuff_dma[entry] = 0;
+		if (likely(priv->tx_skbuff_dma[entry].buf)) {
+			if (priv->tx_skbuff_dma[entry].map_as_page)
+				dma_unmap_page(priv->device,
+					       priv->tx_skbuff_dma[entry].buf,
+					       priv->hw->desc->get_tx_len(p),
+					       DMA_TO_DEVICE);
+			else
+				dma_unmap_single(priv->device,
+						 priv->tx_skbuff_dma[entry].buf,
+						 priv->hw->desc->get_tx_len(p),
+						 DMA_TO_DEVICE);
+			priv->tx_skbuff_dma[entry].buf = 0;
+			priv->tx_skbuff_dma[entry].map_as_page = false;
 		}
 		priv->hw->mode->clean_desc3(priv, p);
 
@@ -1637,6 +1669,13 @@ static int stmmac_hw_setup(struct net_device *dev)
 	/* Initialize the MAC Core */
 	priv->hw->mac->core_init(priv->hw, dev->mtu);
 
+	ret = priv->hw->mac->rx_ipc(priv->hw);
+	if (!ret) {
+		pr_warn(" RX IPC Checksum Offload disabled\n");
+		priv->plat->rx_coe = STMMAC_RX_COE_NONE;
+		priv->hw->rx_csum = 0;
+	}
+
 	/* Enable the MAC Rx/Tx */
 	stmmac_set_mac(priv->ioaddr, true);
 
@@ -1887,12 +1926,16 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 	if (likely(!is_jumbo)) {
 		desc->des2 = dma_map_single(priv->device, skb->data,
 					    nopaged_len, DMA_TO_DEVICE);
-		priv->tx_skbuff_dma[entry] = desc->des2;
+		if (dma_mapping_error(priv->device, desc->des2))
+			goto dma_map_err;
+		priv->tx_skbuff_dma[entry].buf = desc->des2;
 		priv->hw->desc->prepare_tx_desc(desc, 1, nopaged_len,
 						csum_insertion, priv->mode);
 	} else {
 		desc = first;
 		entry = priv->hw->mode->jumbo_frm(priv, skb, csum_insertion);
+		if (unlikely(entry < 0))
+			goto dma_map_err;
 	}
 
 	for (i = 0; i < nfrags; i++) {
@@ -1908,7 +1951,11 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 
 		desc->des2 = skb_frag_dma_map(priv->device, frag, 0, len,
 					      DMA_TO_DEVICE);
-		priv->tx_skbuff_dma[entry] = desc->des2;
+		if (dma_mapping_error(priv->device, desc->des2))
+			goto dma_map_err; /* should reuse desc w/o issues */
+
+		priv->tx_skbuff_dma[entry].buf = desc->des2;
+		priv->tx_skbuff_dma[entry].map_as_page = true;
 		priv->hw->desc->prepare_tx_desc(desc, 0, len, csum_insertion,
 						priv->mode);
 		wmb();
@@ -1975,7 +2022,12 @@ static netdev_tx_t stmmac_xmit(struct sk_buff *skb, struct net_device *dev)
 	priv->hw->dma->enable_dma_transmission(priv->ioaddr);
 
 	spin_unlock(&priv->tx_lock);
+	return NETDEV_TX_OK;
 
+dma_map_err:
+	dev_err(priv->device, "Tx dma map failed\n");
+	dev_kfree_skb(skb);
+	priv->dev->stats.tx_dropped++;
 	return NETDEV_TX_OK;
 }
 
@@ -2028,7 +2080,12 @@ static inline void stmmac_rx_refill(struct stmmac_priv *priv)
 			priv->rx_skbuff_dma[entry] =
 			    dma_map_single(priv->device, skb->data, bfsize,
 					   DMA_FROM_DEVICE);
-
+			if (dma_mapping_error(priv->device,
+					      priv->rx_skbuff_dma[entry])) {
+				dev_err(priv->device, "Rx dma map failed\n");
+				dev_kfree_skb(skb);
+				break;
+			}
 			p->des2 = priv->rx_skbuff_dma[entry];
 
 			priv->hw->mode->refill_desc3(priv, p);
@@ -2055,7 +2112,7 @@ static int stmmac_rx(struct stmmac_priv *priv, int limit)
 	unsigned int entry = priv->cur_rx % rxsize;
 	unsigned int next_entry;
 	unsigned int count = 0;
-	int coe = priv->plat->rx_coe;
+	int coe = priv->hw->rx_csum;
 
 	if (netif_msg_rx_status(priv)) {
 		pr_debug("%s: descriptor ring:\n", __func__);
@@ -2276,8 +2333,7 @@ static netdev_features_t stmmac_fix_features(struct net_device *dev,
 
 	if (priv->plat->rx_coe == STMMAC_RX_COE_NONE)
 		features &= ~NETIF_F_RXCSUM;
-	else if (priv->plat->rx_coe == STMMAC_RX_COE_TYPE1)
-		features &= ~NETIF_F_IPV6_CSUM;
+
 	if (!priv->plat->tx_coe)
 		features &= ~NETIF_F_ALL_CSUM;
 
@@ -2292,6 +2348,24 @@ static netdev_features_t stmmac_fix_features(struct net_device *dev,
 	return features;
 }
 
+static int stmmac_set_features(struct net_device *netdev,
+			       netdev_features_t features)
+{
+	struct stmmac_priv *priv = netdev_priv(netdev);
+
+	/* Keep the COE Type in case of csum is supporting */
+	if (features & NETIF_F_RXCSUM)
+		priv->hw->rx_csum = priv->plat->rx_coe;
+	else
+		priv->hw->rx_csum = 0;
+	/* No check needed because rx_coe has been set before and it will be
+	 * fixed in case of issue.
+	 */
+	priv->hw->mac->rx_ipc(priv->hw);
+
+	return 0;
+}
+
 /**
  *  stmmac_interrupt - main ISR
  *  @irq: interrupt number.
@@ -2572,6 +2646,7 @@ static const struct net_device_ops stmmac_netdev_ops = {
 	.ndo_stop = stmmac_release,
 	.ndo_change_mtu = stmmac_change_mtu,
 	.ndo_fix_features = stmmac_fix_features,
+	.ndo_set_features = stmmac_set_features,
 	.ndo_set_rx_mode = stmmac_set_rx_mode,
 	.ndo_tx_timeout = stmmac_tx_timeout,
 	.ndo_do_ioctl = stmmac_ioctl,
@@ -2592,7 +2667,6 @@ static const struct net_device_ops stmmac_netdev_ops = {
  */
 static int stmmac_hw_init(struct stmmac_priv *priv)
 {
-	int ret;
 	struct mac_device_info *mac;
 
 	/* Identify the MAC HW device */
@@ -2649,15 +2723,11 @@ static int stmmac_hw_init(struct stmmac_priv *priv)
 	/* To use alternate (extended) or normal descriptor structures */
 	stmmac_selec_desc_mode(priv);
 
-	ret = priv->hw->mac->rx_ipc(priv->hw);
-	if (!ret) {
-		pr_warn(" RX IPC Checksum Offload not configured.\n");
-		priv->plat->rx_coe = STMMAC_RX_COE_NONE;
-	}
-
-	if (priv->plat->rx_coe)
+	if (priv->plat->rx_coe) {
+		priv->hw->rx_csum = priv->plat->rx_coe;
 		pr_info(" RX Checksum Offload Engine supported (type %d)\n",
 			priv->plat->rx_coe);
+	}
 	if (priv->plat->tx_coe)
 		pr_info(" TX Checksum insertion supported\n");
 
@@ -2695,8 +2765,6 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	priv->device = device;
 	priv->dev = ndev;
 
-	ether_setup(ndev);
-
 	stmmac_set_ethtool_ops(ndev);
 	priv->pause = pause;
 	priv->plat = plat_dat;
@@ -2716,8 +2784,15 @@ struct stmmac_priv *stmmac_dvr_probe(struct device *device,
 	if (IS_ERR(priv->stmmac_clk)) {
 		dev_warn(priv->device, "%s: warning: cannot get CSR clock\n",
 			 __func__);
-		ret = PTR_ERR(priv->stmmac_clk);
-		goto error_clk_get;
+		/* If failed to obtain stmmac_clk and specific clk_csr value
+		 * is NOT passed from the platform, probe fail.
+		 */
+		if (!priv->plat->clk_csr) {
+			ret = PTR_ERR(priv->stmmac_clk);
+			goto error_clk_get;
+		} else {
+			priv->stmmac_clk = NULL;
+		}
 	}
 	clk_prepare_enable(priv->stmmac_clk);
 
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
index a5b1e1b776fe..b735fa22ac95 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_mdio.c
@@ -253,7 +253,7 @@ int stmmac_mdio_register(struct net_device *ndev)
 			}
 
 			/*
-			 * If we're  going to bind the MAC to this PHY bus,
+			 * If we're going to bind the MAC to this PHY bus,
 			 * and no PHY number was provided to the MAC,
 			 * use the one probed here.
 			 */
@@ -282,7 +282,7 @@ int stmmac_mdio_register(struct net_device *ndev)
 	}
 
 	if (!found) {
-		pr_warning("%s: No PHY found\n", ndev->name);
+		pr_warn("%s: No PHY found\n", ndev->name);
 		mdiobus_unregister(new_bus);
 		mdiobus_free(new_bus);
 		return -ENODEV;
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
index bb524a932be4..db56fa7ce8f9 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_platform.c
@@ -30,13 +30,17 @@
 #include "stmmac.h"
 
 static const struct of_device_id stmmac_dt_ids[] = {
+#ifdef CONFIG_DWMAC_MESON
+	{ .compatible = "amlogic,meson6-dwmac", .data = &meson6_dwmac_data},
+#endif
 #ifdef CONFIG_DWMAC_SUNXI
 	{ .compatible = "allwinner,sun7i-a20-gmac", .data = &sun7i_gmac_data},
 #endif
 #ifdef CONFIG_DWMAC_STI
-	{ .compatible = "st,stih415-dwmac", .data = &sti_gmac_data},
-	{ .compatible = "st,stih416-dwmac", .data = &sti_gmac_data},
-	{ .compatible = "st,stid127-dwmac", .data = &sti_gmac_data},
+	{ .compatible = "st,stih415-dwmac", .data = &stih4xx_dwmac_data},
+	{ .compatible = "st,stih416-dwmac", .data = &stih4xx_dwmac_data},
+	{ .compatible = "st,stid127-dwmac", .data = &stid127_dwmac_data},
+	{ .compatible = "st,stih407-dwmac", .data = &stih4xx_dwmac_data},
 #endif
 #ifdef CONFIG_DWMAC_SOCFPGA
 	{ .compatible = "altr,socfpga-stmmac", .data = &socfpga_gmac_data },
@@ -157,11 +161,16 @@ static int stmmac_probe_config_dt(struct platform_device *pdev,
 	if (of_property_read_u32(np, "snps,phy-addr", &plat->phy_addr) == 0)
 		dev_warn(&pdev->dev, "snps,phy-addr property is deprecated\n");
 
-	plat->mdio_bus_data = devm_kzalloc(&pdev->dev,
-					   sizeof(struct stmmac_mdio_bus_data),
-					   GFP_KERNEL);
+	if (plat->phy_bus_name)
+		plat->mdio_bus_data = NULL;
+	else
+		plat->mdio_bus_data =
+			devm_kzalloc(&pdev->dev,
+				     sizeof(struct stmmac_mdio_bus_data),
+				     GFP_KERNEL);
 
-	plat->force_sf_dma_mode = of_property_read_bool(np, "snps,force_sf_dma_mode");
+	plat->force_sf_dma_mode =
+		of_property_read_bool(np, "snps,force_sf_dma_mode");
 
 	/* Set the maxmtu to a default of JUMBO_LEN in case the
 	 * parameter is not present in the device tree.
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
index b7ad3565566c..c5ee79d8a8c5 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c
@@ -206,6 +206,7 @@ void stmmac_ptp_unregister(struct stmmac_priv *priv)
 {
 	if (priv->ptp_clock) {
 		ptp_clock_unregister(priv->ptp_clock);
+		priv->ptp_clock = NULL;
 		pr_debug("Removed PTP HW clock successfully on %s\n",
 			 priv->dev->name);
 	}
diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
index 3dbc047622fa..4535df37c227 100644
--- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
+++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.h
@@ -25,8 +25,6 @@
 #ifndef __STMMAC_PTP_H__
 #define __STMMAC_PTP_H__
 
-#define STMMAC_SYSCLOCK 62500000
-
 /* IEEE 1588 PTP register offsets */
 #define PTP_TCR		0x0700	/* Timestamp Control Reg */
 #define PTP_SSIR	0x0704	/* Sub-Second Increment Reg */
diff --git a/drivers/net/ethernet/sun/cassini.c b/drivers/net/ethernet/sun/cassini.c
index 37f87ff28f03..02d370e58110 100644
--- a/drivers/net/ethernet/sun/cassini.c
+++ b/drivers/net/ethernet/sun/cassini.c
@@ -4962,7 +4962,7 @@ static int cas_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	pci_cmd |= PCI_COMMAND_PARITY;
 	pci_write_config_word(pdev, PCI_COMMAND, pci_cmd);
 	if (pci_try_set_mwi(pdev))
-		pr_warning("Could not enable MWI for %s\n", pci_name(pdev));
+		pr_warn("Could not enable MWI for %s\n", pci_name(pdev));
 
 	cas_program_bridge(pdev);
 
diff --git a/drivers/net/ethernet/sun/niu.c b/drivers/net/ethernet/sun/niu.c
index 8216be46540f..904fd1ab5f6e 100644
--- a/drivers/net/ethernet/sun/niu.c
+++ b/drivers/net/ethernet/sun/niu.c
@@ -8717,8 +8717,8 @@ static void niu_divide_channels(struct niu_parent *parent,
 			parent->txchan_per_port[i] = 1;
 	}
 	if (tot_rx < NIU_NUM_RXCHAN || tot_tx < NIU_NUM_TXCHAN) {
-		pr_warning("niu%d: Driver bug, wasted channels, RX[%d] TX[%d]\n",
-			   parent->index, tot_rx, tot_tx);
+		pr_warn("niu%d: Driver bug, wasted channels, RX[%d] TX[%d]\n",
+			parent->index, tot_rx, tot_tx);
 	}
 }
 
diff --git a/drivers/net/ethernet/sun/sungem.c b/drivers/net/ethernet/sun/sungem.c
index f7415b6bf141..fef5dec2cffe 100644
--- a/drivers/net/ethernet/sun/sungem.c
+++ b/drivers/net/ethernet/sun/sungem.c
@@ -115,7 +115,7 @@ static const struct pci_device_id gem_pci_tbl[] = {
 
 MODULE_DEVICE_TABLE(pci, gem_pci_tbl);
 
-static u16 __phy_read(struct gem *gp, int phy_addr, int reg)
+static u16 __sungem_phy_read(struct gem *gp, int phy_addr, int reg)
 {
 	u32 cmd;
 	int limit = 10000;
@@ -141,18 +141,18 @@ static u16 __phy_read(struct gem *gp, int phy_addr, int reg)
 	return cmd & MIF_FRAME_DATA;
 }
 
-static inline int _phy_read(struct net_device *dev, int mii_id, int reg)
+static inline int _sungem_phy_read(struct net_device *dev, int mii_id, int reg)
 {
 	struct gem *gp = netdev_priv(dev);
-	return __phy_read(gp, mii_id, reg);
+	return __sungem_phy_read(gp, mii_id, reg);
 }
 
-static inline u16 phy_read(struct gem *gp, int reg)
+static inline u16 sungem_phy_read(struct gem *gp, int reg)
 {
-	return __phy_read(gp, gp->mii_phy_addr, reg);
+	return __sungem_phy_read(gp, gp->mii_phy_addr, reg);
 }
 
-static void __phy_write(struct gem *gp, int phy_addr, int reg, u16 val)
+static void __sungem_phy_write(struct gem *gp, int phy_addr, int reg, u16 val)
 {
 	u32 cmd;
 	int limit = 10000;
@@ -174,15 +174,15 @@ static void __phy_write(struct gem *gp, int phy_addr, int reg, u16 val)
 	}
 }
 
-static inline void _phy_write(struct net_device *dev, int mii_id, int reg, int val)
+static inline void _sungem_phy_write(struct net_device *dev, int mii_id, int reg, int val)
 {
 	struct gem *gp = netdev_priv(dev);
-	__phy_write(gp, mii_id, reg, val & 0xffff);
+	__sungem_phy_write(gp, mii_id, reg, val & 0xffff);
 }
 
-static inline void phy_write(struct gem *gp, int reg, u16 val)
+static inline void sungem_phy_write(struct gem *gp, int reg, u16 val)
 {
-	__phy_write(gp, gp->mii_phy_addr, reg, val);
+	__sungem_phy_write(gp, gp->mii_phy_addr, reg, val);
 }
 
 static inline void gem_enable_ints(struct gem *gp)
@@ -1687,9 +1687,9 @@ static void gem_init_phy(struct gem *gp)
 			/* Some PHYs used by apple have problem getting back to us,
 			 * we do an additional reset here
 			 */
-			phy_write(gp, MII_BMCR, BMCR_RESET);
+			sungem_phy_write(gp, MII_BMCR, BMCR_RESET);
 			msleep(20);
-			if (phy_read(gp, MII_BMCR) != 0xffff)
+			if (sungem_phy_read(gp, MII_BMCR) != 0xffff)
 				break;
 			if (i == 2)
 				netdev_warn(gp->dev, "GMAC PHY not responding !\n");
@@ -2012,7 +2012,7 @@ static int gem_check_invariants(struct gem *gp)
 
 		for (i = 0; i < 32; i++) {
 			gp->mii_phy_addr = i;
-			if (phy_read(gp, MII_BMCR) != 0xffff)
+			if (sungem_phy_read(gp, MII_BMCR) != 0xffff)
 				break;
 		}
 		if (i == 32) {
@@ -2696,13 +2696,13 @@ static int gem_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 		/* Fallthrough... */
 
 	case SIOCGMIIREG:		/* Read MII PHY register. */
-		data->val_out = __phy_read(gp, data->phy_id & 0x1f,
+		data->val_out = __sungem_phy_read(gp, data->phy_id & 0x1f,
 					   data->reg_num & 0x1f);
 		rc = 0;
 		break;
 
 	case SIOCSMIIREG:		/* Write MII PHY register. */
-		__phy_write(gp, data->phy_id & 0x1f, data->reg_num & 0x1f,
+		__sungem_phy_write(gp, data->phy_id & 0x1f, data->reg_num & 0x1f,
 			    data->val_in);
 		rc = 0;
 		break;
@@ -2933,8 +2933,8 @@ static int gem_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	/* Fill up the mii_phy structure (even if we won't use it) */
 	gp->phy_mii.dev = dev;
-	gp->phy_mii.mdio_read = _phy_read;
-	gp->phy_mii.mdio_write = _phy_write;
+	gp->phy_mii.mdio_read = _sungem_phy_read;
+	gp->phy_mii.mdio_write = _sungem_phy_write;
 #ifdef CONFIG_PPC_PMAC
 	gp->phy_mii.platform_data = gp->of_node;
 #endif
diff --git a/drivers/net/ethernet/sun/sunvnet.c b/drivers/net/ethernet/sun/sunvnet.c
index 23c89ab5a6ad..3652afd3ec78 100644
--- a/drivers/net/ethernet/sun/sunvnet.c
+++ b/drivers/net/ethernet/sun/sunvnet.c
@@ -15,6 +15,14 @@
 #include <linux/ethtool.h>
 #include <linux/etherdevice.h>
 #include <linux/mutex.h>
+#include <linux/if_vlan.h>
+
+#if IS_ENABLED(CONFIG_IPV6)
+#include <linux/icmpv6.h>
+#endif
+
+#include <net/icmp.h>
+#include <net/route.h>
 
 #include <asm/vio.h>
 #include <asm/ldc.h>
@@ -37,8 +45,11 @@ MODULE_VERSION(DRV_MODULE_VERSION);
  */
 #define	VNET_MAX_RETRIES	10
 
+static int __vnet_tx_trigger(struct vnet_port *port, u32 start);
+
 /* Ordered from largest major to lowest */
 static struct vio_version vnet_versions[] = {
+	{ .major = 1, .minor = 6 },
 	{ .major = 1, .minor = 0 },
 };
 
@@ -65,6 +76,7 @@ static int vnet_send_attr(struct vio_driver_state *vio)
 	struct vnet_port *port = to_vnet_port(vio);
 	struct net_device *dev = port->vp->dev;
 	struct vio_net_attr_info pkt;
+	int framelen = ETH_FRAME_LEN;
 	int i;
 
 	memset(&pkt, 0, sizeof(pkt));
@@ -72,19 +84,41 @@ static int vnet_send_attr(struct vio_driver_state *vio)
 	pkt.tag.stype = VIO_SUBTYPE_INFO;
 	pkt.tag.stype_env = VIO_ATTR_INFO;
 	pkt.tag.sid = vio_send_sid(vio);
-	pkt.xfer_mode = VIO_DRING_MODE;
+	if (vio_version_before(vio, 1, 2))
+		pkt.xfer_mode = VIO_DRING_MODE;
+	else
+		pkt.xfer_mode = VIO_NEW_DRING_MODE;
 	pkt.addr_type = VNET_ADDR_ETHERMAC;
 	pkt.ack_freq = 0;
 	for (i = 0; i < 6; i++)
 		pkt.addr |= (u64)dev->dev_addr[i] << ((5 - i) * 8);
-	pkt.mtu = ETH_FRAME_LEN;
+	if (vio_version_after(vio, 1, 3)) {
+		if (port->rmtu) {
+			port->rmtu = min(VNET_MAXPACKET, port->rmtu);
+			pkt.mtu = port->rmtu;
+		} else {
+			port->rmtu = VNET_MAXPACKET;
+			pkt.mtu = port->rmtu;
+		}
+		if (vio_version_after_eq(vio, 1, 6))
+			pkt.options = VIO_TX_DRING;
+	} else if (vio_version_before(vio, 1, 3)) {
+		pkt.mtu = framelen;
+	} else { /* v1.3 */
+		pkt.mtu = framelen + VLAN_HLEN;
+	}
+
+	pkt.plnk_updt = PHYSLINK_UPDATE_NONE;
+	pkt.cflags = 0;
 
 	viodbg(HS, "SEND NET ATTR xmode[0x%x] atype[0x%x] addr[%llx] "
-	       "ackfreq[%u] mtu[%llu]\n",
+	       "ackfreq[%u] plnk_updt[0x%02x] opts[0x%02x] mtu[%llu] "
+	       "cflags[0x%04x] lso_max[%u]\n",
 	       pkt.xfer_mode, pkt.addr_type,
-	       (unsigned long long) pkt.addr,
-	       pkt.ack_freq,
-	       (unsigned long long) pkt.mtu);
+	       (unsigned long long)pkt.addr,
+	       pkt.ack_freq, pkt.plnk_updt, pkt.options,
+	       (unsigned long long)pkt.mtu, pkt.cflags, pkt.ipv4_lso_maxlen);
+
 
 	return vio_ldc_send(vio, &pkt, sizeof(pkt));
 }
@@ -92,18 +126,52 @@ static int vnet_send_attr(struct vio_driver_state *vio)
 static int handle_attr_info(struct vio_driver_state *vio,
 			    struct vio_net_attr_info *pkt)
 {
-	viodbg(HS, "GOT NET ATTR INFO xmode[0x%x] atype[0x%x] addr[%llx] "
-	       "ackfreq[%u] mtu[%llu]\n",
+	struct vnet_port *port = to_vnet_port(vio);
+	u64	localmtu;
+	u8	xfer_mode;
+
+	viodbg(HS, "GOT NET ATTR xmode[0x%x] atype[0x%x] addr[%llx] "
+	       "ackfreq[%u] plnk_updt[0x%02x] opts[0x%02x] mtu[%llu] "
+	       " (rmtu[%llu]) cflags[0x%04x] lso_max[%u]\n",
 	       pkt->xfer_mode, pkt->addr_type,
-	       (unsigned long long) pkt->addr,
-	       pkt->ack_freq,
-	       (unsigned long long) pkt->mtu);
+	       (unsigned long long)pkt->addr,
+	       pkt->ack_freq, pkt->plnk_updt, pkt->options,
+	       (unsigned long long)pkt->mtu, port->rmtu, pkt->cflags,
+	       pkt->ipv4_lso_maxlen);
 
 	pkt->tag.sid = vio_send_sid(vio);
 
-	if (pkt->xfer_mode != VIO_DRING_MODE ||
+	xfer_mode = pkt->xfer_mode;
+	/* for version < 1.2, VIO_DRING_MODE = 0x3 and no bitmask */
+	if (vio_version_before(vio, 1, 2) && xfer_mode == VIO_DRING_MODE)
+		xfer_mode = VIO_NEW_DRING_MODE;
+
+	/* MTU negotiation:
+	 *	< v1.3 - ETH_FRAME_LEN exactly
+	 *	> v1.3 - MIN(pkt.mtu, VNET_MAXPACKET, port->rmtu) and change
+	 *			pkt->mtu for ACK
+	 *	= v1.3 - ETH_FRAME_LEN + VLAN_HLEN exactly
+	 */
+	if (vio_version_before(vio, 1, 3)) {
+		localmtu = ETH_FRAME_LEN;
+	} else if (vio_version_after(vio, 1, 3)) {
+		localmtu = port->rmtu ? port->rmtu : VNET_MAXPACKET;
+		localmtu = min(pkt->mtu, localmtu);
+		pkt->mtu = localmtu;
+	} else { /* v1.3 */
+		localmtu = ETH_FRAME_LEN + VLAN_HLEN;
+	}
+	port->rmtu = localmtu;
+
+	/* for version >= 1.6, ACK packet mode we support */
+	if (vio_version_after_eq(vio, 1, 6)) {
+		pkt->xfer_mode = VIO_NEW_DRING_MODE;
+		pkt->options = VIO_TX_DRING;
+	}
+
+	if (!(xfer_mode | VIO_NEW_DRING_MODE) ||
 	    pkt->addr_type != VNET_ADDR_ETHERMAC ||
-	    pkt->mtu != ETH_FRAME_LEN) {
+	    pkt->mtu != localmtu) {
 		viodbg(HS, "SEND NET ATTR NACK\n");
 
 		pkt->tag.stype = VIO_SUBTYPE_NACK;
@@ -112,7 +180,14 @@ static int handle_attr_info(struct vio_driver_state *vio,
 
 		return -ECONNRESET;
 	} else {
-		viodbg(HS, "SEND NET ATTR ACK\n");
+		viodbg(HS, "SEND NET ATTR ACK xmode[0x%x] atype[0x%x] "
+		       "addr[%llx] ackfreq[%u] plnk_updt[0x%02x] opts[0x%02x] "
+		       "mtu[%llu] (rmtu[%llu]) cflags[0x%04x] lso_max[%u]\n",
+		       pkt->xfer_mode, pkt->addr_type,
+		       (unsigned long long)pkt->addr,
+		       pkt->ack_freq, pkt->plnk_updt, pkt->options,
+		       (unsigned long long)pkt->mtu, port->rmtu, pkt->cflags,
+		       pkt->ipv4_lso_maxlen);
 
 		pkt->tag.stype = VIO_SUBTYPE_ACK;
 
@@ -208,7 +283,7 @@ static int vnet_rx_one(struct vnet_port *port, unsigned int len,
 	int err;
 
 	err = -EMSGSIZE;
-	if (unlikely(len < ETH_ZLEN || len > ETH_FRAME_LEN)) {
+	if (unlikely(len < ETH_ZLEN || len > port->rmtu)) {
 		dev->stats.rx_length_errors++;
 		goto out_dropped;
 	}
@@ -283,10 +358,18 @@ static int vnet_send_ack(struct vnet_port *port, struct vio_dring_state *dr,
 				port->raddr[0], port->raddr[1],
 				port->raddr[2], port->raddr[3],
 				port->raddr[4], port->raddr[5]);
-			err = -ECONNRESET;
+			break;
 		}
 	} while (err == -EAGAIN);
 
+	if (err <= 0 && vio_dring_state == VIO_DRING_STOPPED) {
+		port->stop_rx_idx = end;
+		port->stop_rx = true;
+	} else {
+		port->stop_rx_idx = 0;
+		port->stop_rx = false;
+	}
+
 	return err;
 }
 
@@ -350,14 +433,17 @@ static int vnet_walk_rx_one(struct vnet_port *port,
 	if (IS_ERR(desc))
 		return PTR_ERR(desc);
 
+	if (desc->hdr.state != VIO_DESC_READY)
+		return 1;
+
+	rmb();
+
 	viodbg(DATA, "vio_walk_rx_one desc[%02x:%02x:%08x:%08x:%llx:%llx]\n",
 	       desc->hdr.state, desc->hdr.ack,
 	       desc->size, desc->ncookies,
 	       desc->cookies[0].cookie_addr,
 	       desc->cookies[0].cookie_size);
 
-	if (desc->hdr.state != VIO_DESC_READY)
-		return 1;
 	err = vnet_rx_one(port, desc->size, desc->cookies, desc->ncookies);
 	if (err == -ECONNRESET)
 		return err;
@@ -448,7 +534,7 @@ static int vnet_ack(struct vnet_port *port, void *msgbuf)
 	struct net_device *dev;
 	struct vnet *vp;
 	u32 end;
-
+	struct vio_net_desc *desc;
 	if (unlikely(pkt->tag.stype_env != VIO_DRING_DATA))
 		return 0;
 
@@ -456,7 +542,24 @@ static int vnet_ack(struct vnet_port *port, void *msgbuf)
 	if (unlikely(!idx_is_pending(dr, end)))
 		return 0;
 
+	/* sync for race conditions with vnet_start_xmit() and tell xmit it
+	 * is time to send a trigger.
+	 */
 	dr->cons = next_idx(end, dr);
+	desc = vio_dring_entry(dr, dr->cons);
+	if (desc->hdr.state == VIO_DESC_READY && port->start_cons) {
+		/* vnet_start_xmit() just populated this dring but missed
+		 * sending the "start" LDC message to the consumer.
+		 * Send a "start" trigger on its behalf.
+		 */
+		if (__vnet_tx_trigger(port, dr->cons) > 0)
+			port->start_cons = false;
+		else
+			port->start_cons = true;
+	} else {
+		port->start_cons = true;
+	}
+
 
 	vp = port->vp;
 	dev = vp->dev;
@@ -528,13 +631,15 @@ static void vnet_event(void *arg, int event)
 		vio_link_state_change(vio, event);
 		spin_unlock_irqrestore(&vio->lock, flags);
 
-		if (event == LDC_EVENT_RESET)
+		if (event == LDC_EVENT_RESET) {
+			port->rmtu = 0;
 			vio_port_up(vio);
+		}
 		return;
 	}
 
 	if (unlikely(event != LDC_EVENT_DATA_READY)) {
-		pr_warning("Unexpected LDC event %d\n", event);
+		pr_warn("Unexpected LDC event %d\n", event);
 		spin_unlock_irqrestore(&vio->lock, flags);
 		return;
 	}
@@ -597,7 +702,7 @@ static void vnet_event(void *arg, int event)
 	local_irq_restore(flags);
 }
 
-static int __vnet_tx_trigger(struct vnet_port *port)
+static int __vnet_tx_trigger(struct vnet_port *port, u32 start)
 {
 	struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 	struct vio_dring_data hdr = {
@@ -608,12 +713,21 @@ static int __vnet_tx_trigger(struct vnet_port *port)
 			.sid		= vio_send_sid(&port->vio),
 		},
 		.dring_ident		= dr->ident,
-		.start_idx		= dr->prod,
+		.start_idx		= start,
 		.end_idx		= (u32) -1,
 	};
 	int err, delay;
 	int retries = 0;
 
+	if (port->stop_rx) {
+		err = vnet_send_ack(port,
+				    &port->vio.drings[VIO_DRIVER_RX_RING],
+				    port->stop_rx_idx, -1,
+				    VIO_DRING_STOPPED);
+		if (err <= 0)
+			return err;
+	}
+
 	hdr.seq = dr->snd_nxt;
 	delay = 1;
 	do {
@@ -673,6 +787,117 @@ struct vnet_port *tx_port_find(struct vnet *vp, struct sk_buff *skb)
 	return ret;
 }
 
+static struct sk_buff *vnet_clean_tx_ring(struct vnet_port *port,
+					  unsigned *pending)
+{
+	struct vio_dring_state *dr = &port->vio.drings[VIO_DRIVER_TX_RING];
+	struct sk_buff *skb = NULL;
+	int i, txi;
+
+	*pending = 0;
+
+	txi = dr->prod-1;
+	if (txi < 0)
+		txi = VNET_TX_RING_SIZE-1;
+
+	for (i = 0; i < VNET_TX_RING_SIZE; ++i) {
+		struct vio_net_desc *d;
+
+		d = vio_dring_entry(dr, txi);
+
+		if (d->hdr.state == VIO_DESC_DONE) {
+			if (port->tx_bufs[txi].skb) {
+				BUG_ON(port->tx_bufs[txi].skb->next);
+
+				port->tx_bufs[txi].skb->next = skb;
+				skb = port->tx_bufs[txi].skb;
+				port->tx_bufs[txi].skb = NULL;
+
+				ldc_unmap(port->vio.lp,
+					  port->tx_bufs[txi].cookies,
+					  port->tx_bufs[txi].ncookies);
+			}
+			d->hdr.state = VIO_DESC_FREE;
+		} else if (d->hdr.state == VIO_DESC_READY) {
+			(*pending)++;
+		} else if (d->hdr.state == VIO_DESC_FREE) {
+			break;
+		}
+		--txi;
+		if (txi < 0)
+			txi = VNET_TX_RING_SIZE-1;
+	}
+	return skb;
+}
+
+static inline void vnet_free_skbs(struct sk_buff *skb)
+{
+	struct sk_buff *next;
+
+	while (skb) {
+		next = skb->next;
+		skb->next = NULL;
+		dev_kfree_skb(skb);
+		skb = next;
+	}
+}
+
+static void vnet_clean_timer_expire(unsigned long port0)
+{
+	struct vnet_port *port = (struct vnet_port *)port0;
+	struct sk_buff *freeskbs;
+	unsigned pending;
+	unsigned long flags;
+
+	spin_lock_irqsave(&port->vio.lock, flags);
+	freeskbs = vnet_clean_tx_ring(port, &pending);
+	spin_unlock_irqrestore(&port->vio.lock, flags);
+
+	vnet_free_skbs(freeskbs);
+
+	if (pending)
+		(void)mod_timer(&port->clean_timer,
+				jiffies + VNET_CLEAN_TIMEOUT);
+	 else
+		del_timer(&port->clean_timer);
+}
+
+static inline struct sk_buff *vnet_skb_shape(struct sk_buff *skb, void **pstart,
+					     int *plen)
+{
+	struct sk_buff *nskb;
+	int len, pad;
+
+	len = skb->len;
+	pad = 0;
+	if (len < ETH_ZLEN) {
+		pad += ETH_ZLEN - skb->len;
+		len += pad;
+	}
+	len += VNET_PACKET_SKIP;
+	pad += 8 - (len & 7);
+	len += 8 - (len & 7);
+
+	if (((unsigned long)skb->data & 7) != VNET_PACKET_SKIP ||
+	    skb_tailroom(skb) < pad ||
+	    skb_headroom(skb) < VNET_PACKET_SKIP) {
+		nskb = alloc_and_align_skb(skb->dev, skb->len);
+		skb_reserve(nskb, VNET_PACKET_SKIP);
+		if (skb_copy_bits(skb, 0, nskb->data, skb->len)) {
+			dev_kfree_skb(nskb);
+			dev_kfree_skb(skb);
+			return NULL;
+		}
+		(void)skb_put(nskb, skb->len);
+		dev_kfree_skb(skb);
+		skb = nskb;
+	}
+
+	*pstart = skb->data - VNET_PACKET_SKIP;
+	*plen = len;
+	return skb;
+}
+
 static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct vnet *vp = netdev_priv(dev);
@@ -681,16 +906,55 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	struct vio_net_desc *d;
 	unsigned long flags;
 	unsigned int len;
-	void *tx_buf;
-	int i, err;
+	struct sk_buff *freeskbs = NULL;
+	int i, err, txi;
+	void *start = NULL;
+	int nlen = 0;
+	unsigned pending = 0;
 
 	if (unlikely(!port))
 		goto out_dropped;
 
+	skb = vnet_skb_shape(skb, &start, &nlen);
+
+	if (unlikely(!skb))
+		goto out_dropped;
+
+	if (skb->len > port->rmtu) {
+		unsigned long localmtu = port->rmtu - ETH_HLEN;
+
+		if (vio_version_after_eq(&port->vio, 1, 3))
+			localmtu -= VLAN_HLEN;
+
+		if (skb->protocol == htons(ETH_P_IP)) {
+			struct flowi4 fl4;
+			struct rtable *rt = NULL;
+
+			memset(&fl4, 0, sizeof(fl4));
+			fl4.flowi4_oif = dev->ifindex;
+			fl4.flowi4_tos = RT_TOS(ip_hdr(skb)->tos);
+			fl4.daddr = ip_hdr(skb)->daddr;
+			fl4.saddr = ip_hdr(skb)->saddr;
+
+			rt = ip_route_output_key(dev_net(dev), &fl4);
+			if (!IS_ERR(rt)) {
+				skb_dst_set(skb, &rt->dst);
+				icmp_send(skb, ICMP_DEST_UNREACH,
+					  ICMP_FRAG_NEEDED,
+					  htonl(localmtu));
+			}
+		}
+#if IS_ENABLED(CONFIG_IPV6)
+		else if (skb->protocol == htons(ETH_P_IPV6))
+			icmpv6_send(skb, ICMPV6_PKT_TOOBIG, 0, localmtu);
+#endif
+		goto out_dropped;
+	}
+
 	spin_lock_irqsave(&port->vio.lock, flags);
 
 	dr = &port->vio.drings[VIO_DRIVER_TX_RING];
-	if (unlikely(vnet_tx_dring_avail(dr) < 2)) {
+	if (unlikely(vnet_tx_dring_avail(dr) < 1)) {
 		if (!netif_queue_stopped(dev)) {
 			netif_stop_queue(dev);
 
@@ -704,14 +968,27 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	d = vio_dring_cur(dr);
 
-	tx_buf = port->tx_bufs[dr->prod].buf;
-	skb_copy_from_linear_data(skb, tx_buf + VNET_PACKET_SKIP, skb->len);
+	txi = dr->prod;
+
+	freeskbs = vnet_clean_tx_ring(port, &pending);
+
+	BUG_ON(port->tx_bufs[txi].skb);
 
 	len = skb->len;
-	if (len < ETH_ZLEN) {
+	if (len < ETH_ZLEN)
 		len = ETH_ZLEN;
-		memset(tx_buf+VNET_PACKET_SKIP+skb->len, 0, len - skb->len);
+
+	port->tx_bufs[txi].skb = skb;
+	skb = NULL;
+
+	err = ldc_map_single(port->vio.lp, start, nlen,
+			     port->tx_bufs[txi].cookies, VNET_MAXCOOKIES,
+			     (LDC_MAP_SHADOW | LDC_MAP_DIRECT | LDC_MAP_RW));
+	if (err < 0) {
+		netdev_info(dev, "tx buffer map error %d\n", err);
+		goto out_dropped_unlock;
 	}
+	port->tx_bufs[txi].ncookies = err;
 
 	/* We don't rely on the ACKs to free the skb in vnet_start_xmit(),
 	 * thus it is safe to not set VIO_ACK_ENABLE for each transmission:
@@ -723,9 +1000,9 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	 */
 	d->hdr.ack = VIO_ACK_DISABLE;
 	d->size = len;
-	d->ncookies = port->tx_bufs[dr->prod].ncookies;
+	d->ncookies = port->tx_bufs[txi].ncookies;
 	for (i = 0; i < d->ncookies; i++)
-		d->cookies[i] = port->tx_bufs[dr->prod].cookies[i];
+		d->cookies[i] = port->tx_bufs[txi].cookies[i];
 
 	/* This has to be a non-SMP write barrier because we are writing
 	 * to memory which is shared with the peer LDOM.
@@ -734,7 +1011,30 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	d->hdr.state = VIO_DESC_READY;
 
-	err = __vnet_tx_trigger(port);
+	/* Exactly one ldc "start" trigger (for dr->cons) needs to be sent
+	 * to notify the consumer that some descriptors are READY.
+	 * After that "start" trigger, no additional triggers are needed until
+	 * a DRING_STOPPED is received from the consumer. The dr->cons field
+	 * (set up by vnet_ack()) has the value of the next dring index
+	 * that has not yet been ack-ed. We send a "start" trigger here
+	 * if, and only if, start_cons is true (reset it afterward). Conversely,
+	 * vnet_ack() should check if the dring corresponding to cons
+	 * is marked READY, but start_cons was false.
+	 * If so, vnet_ack() should send out the missed "start" trigger.
+	 *
+	 * Note that the wmb() above makes sure the cookies et al. are
+	 * not globally visible before the VIO_DESC_READY, and that the
+	 * stores are ordered correctly by the compiler. The consumer will
+	 * not proceed until the VIO_DESC_READY is visible assuring that
+	 * the consumer does not observe anything related to descriptors
+	 * out of order. The HV trap from the LDC start trigger is the
+	 * producer to consumer announcement that work is available to the
+	 * consumer
+	 */
+	if (!port->start_cons)
+		goto ldc_start_done; /* previous trigger suffices */
+
+	err = __vnet_tx_trigger(port, dr->cons);
 	if (unlikely(err < 0)) {
 		netdev_info(dev, "TX trigger error %d\n", err);
 		d->hdr.state = VIO_DESC_FREE;
@@ -742,11 +1042,14 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		goto out_dropped_unlock;
 	}
 
+ldc_start_done:
+	port->start_cons = false;
+
 	dev->stats.tx_packets++;
-	dev->stats.tx_bytes += skb->len;
+	dev->stats.tx_bytes += port->tx_bufs[txi].skb->len;
 
 	dr->prod = (dr->prod + 1) & (VNET_TX_RING_SIZE - 1);
-	if (unlikely(vnet_tx_dring_avail(dr) < 2)) {
+	if (unlikely(vnet_tx_dring_avail(dr) < 1)) {
 		netif_stop_queue(dev);
 		if (vnet_tx_dring_avail(dr) > VNET_TX_WAKEUP_THRESH(dr))
 			netif_wake_queue(dev);
@@ -754,7 +1057,9 @@ static int vnet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	spin_unlock_irqrestore(&port->vio.lock, flags);
 
-	dev_kfree_skb(skb);
+	vnet_free_skbs(freeskbs);
+
+	(void)mod_timer(&port->clean_timer, jiffies + VNET_CLEAN_TIMEOUT);
 
 	return NETDEV_TX_OK;
 
@@ -762,7 +1067,14 @@ out_dropped_unlock:
 	spin_unlock_irqrestore(&port->vio.lock, flags);
 
 out_dropped:
-	dev_kfree_skb(skb);
+	if (skb)
+		dev_kfree_skb(skb);
+	vnet_free_skbs(freeskbs);
+	if (pending)
+		(void)mod_timer(&port->clean_timer,
+				jiffies + VNET_CLEAN_TIMEOUT);
+	else if (port)
+		del_timer(&port->clean_timer);
 	dev->stats.tx_dropped++;
 	return NETDEV_TX_OK;
 }
@@ -908,7 +1220,7 @@ static void vnet_set_rx_mode(struct net_device *dev)
 
 static int vnet_change_mtu(struct net_device *dev, int new_mtu)
 {
-	if (new_mtu != ETH_DATA_LEN)
+	if (new_mtu < 68 || new_mtu > 65535)
 		return -EINVAL;
 
 	dev->mtu = new_mtu;
@@ -964,17 +1276,22 @@ static void vnet_port_free_tx_bufs(struct vnet_port *port)
 	}
 
 	for (i = 0; i < VNET_TX_RING_SIZE; i++) {
-		void *buf = port->tx_bufs[i].buf;
+		struct vio_net_desc *d;
+		void *skb = port->tx_bufs[i].skb;
 
-		if (!buf)
+		if (!skb)
 			continue;
 
+		d = vio_dring_entry(dr, i);
+		if (d->hdr.state == VIO_DESC_READY)
+			pr_warn("active transmit buffers freed\n");
+
 		ldc_unmap(port->vio.lp,
 			  port->tx_bufs[i].cookies,
 			  port->tx_bufs[i].ncookies);
-
-		kfree(buf);
-		port->tx_bufs[i].buf = NULL;
+		dev_kfree_skb(skb);
+		port->tx_bufs[i].skb = NULL;
+		d->hdr.state = VIO_DESC_FREE;
 	}
 }
 
@@ -985,34 +1302,6 @@ static int vnet_port_alloc_tx_bufs(struct vnet_port *port)
 	int i, err, ncookies;
 	void *dring;
 
-	for (i = 0; i < VNET_TX_RING_SIZE; i++) {
-		void *buf = kzalloc(ETH_FRAME_LEN + 8, GFP_KERNEL);
-		int map_len = (ETH_FRAME_LEN + 7) & ~7;
-
-		err = -ENOMEM;
-		if (!buf)
-			goto err_out;
-
-		err = -EFAULT;
-		if ((unsigned long)buf & (8UL - 1)) {
-			pr_err("TX buffer misaligned\n");
-			kfree(buf);
-			goto err_out;
-		}
-
-		err = ldc_map_single(port->vio.lp, buf, map_len,
-				     port->tx_bufs[i].cookies, 2,
-				     (LDC_MAP_SHADOW |
-				      LDC_MAP_DIRECT |
-				      LDC_MAP_RW));
-		if (err < 0) {
-			kfree(buf);
-			goto err_out;
-		}
-		port->tx_bufs[i].buf = buf;
-		port->tx_bufs[i].ncookies = err;
-	}
-
 	dr = &port->vio.drings[VIO_DRIVER_TX_RING];
 
 	len = (VNET_TX_RING_SIZE *
@@ -1035,9 +1324,16 @@ static int vnet_port_alloc_tx_bufs(struct vnet_port *port)
 			  (sizeof(struct ldc_trans_cookie) * 2));
 	dr->num_entries = VNET_TX_RING_SIZE;
 	dr->prod = dr->cons = 0;
+	port->start_cons  = true; /* need an initial trigger */
 	dr->pending = VNET_TX_RING_SIZE;
 	dr->ncookies = ncookies;
 
+	for (i = 0; i < VNET_TX_RING_SIZE; ++i) {
+		struct vio_net_desc *d;
+
+		d = vio_dring_entry(dr, i);
+		d->hdr.state = VIO_DESC_FREE;
+	}
 	return 0;
 
 err_out:
@@ -1069,6 +1365,8 @@ static struct vnet *vnet_new(const u64 *local_mac)
 	dev = alloc_etherdev(sizeof(*vp));
 	if (!dev)
 		return ERR_PTR(-ENOMEM);
+	dev->needed_headroom = VNET_PACKET_SKIP + 8;
+	dev->needed_tailroom = 8;
 
 	for (i = 0; i < ETH_ALEN; i++)
 		dev->dev_addr[i] = (*local_mac >> (5 - i) * 8) & 0xff;
@@ -1263,6 +1561,9 @@ static int vnet_port_probe(struct vio_dev *vdev, const struct vio_device_id *id)
 	pr_info("%s: PORT ( remote-mac %pM%s )\n",
 		vp->dev->name, port->raddr, switch_port ? " switch-port" : "");
 
+	setup_timer(&port->clean_timer, vnet_clean_timer_expire,
+		    (unsigned long)port);
+
 	vio_port_up(&port->vio);
 
 	mdesc_release(hp);
@@ -1289,6 +1590,7 @@ static int vnet_port_remove(struct vio_dev *vdev)
 		unsigned long flags;
 
 		del_timer_sync(&port->vio.timer);
+		del_timer_sync(&port->clean_timer);
 
 		spin_lock_irqsave(&vp->lock, flags);
 		list_del(&port->list);
diff --git a/drivers/net/ethernet/sun/sunvnet.h b/drivers/net/ethernet/sun/sunvnet.h
index de5c2c64996f..c91104542619 100644
--- a/drivers/net/ethernet/sun/sunvnet.h
+++ b/drivers/net/ethernet/sun/sunvnet.h
@@ -11,6 +11,12 @@
  */
 #define VNET_TX_TIMEOUT			(5 * HZ)
 
+/* length of time (or less) we expect pending descriptors to be marked
+ * as VIO_DESC_DONE and skbs ready to be freed
+ */
+#define	VNET_CLEAN_TIMEOUT		((HZ/100)+1)
+
+#define VNET_MAXPACKET			(65535ULL + ETH_HLEN + VLAN_HLEN)
 #define VNET_TX_RING_SIZE		512
 #define VNET_TX_WAKEUP_THRESH(dr)	((dr)->pending / 4)
 
@@ -20,10 +26,12 @@
  */
 #define VNET_PACKET_SKIP		6
 
+#define VNET_MAXCOOKIES			(VNET_MAXPACKET/PAGE_SIZE + 1)
+
 struct vnet_tx_entry {
-	void			*buf;
+	struct sk_buff		*skb;
 	unsigned int		ncookies;
-	struct ldc_trans_cookie	cookies[2];
+	struct ldc_trans_cookie	cookies[VNET_MAXCOOKIES];
 };
 
 struct vnet;
@@ -40,6 +48,14 @@ struct vnet_port {
 	struct vnet_tx_entry	tx_bufs[VNET_TX_RING_SIZE];
 
 	struct list_head	list;
+
+	u32			stop_rx_idx;
+	bool			stop_rx;
+	bool			start_cons;
+
+	struct timer_list	clean_timer;
+
+	u64			rmtu;
 };
 
 static inline struct vnet_port *to_vnet_port(struct vio_driver_state *vio)
diff --git a/drivers/net/ethernet/ti/Kconfig b/drivers/net/ethernet/ti/Kconfig
index 1769700a6070..5d8cb7956113 100644
--- a/drivers/net/ethernet/ti/Kconfig
+++ b/drivers/net/ethernet/ti/Kconfig
@@ -62,6 +62,8 @@ config TI_CPSW
 	select TI_DAVINCI_CPDMA
 	select TI_DAVINCI_MDIO
 	select TI_CPSW_PHY_SEL
+	select MFD_SYSCON
+	select REGMAP
 	---help---
 	  This driver supports TI's CPSW Ethernet Switch.
 
diff --git a/drivers/net/ethernet/ti/cpmac.c b/drivers/net/ethernet/ti/cpmac.c
index f9bcf7aa88ca..dd9430043536 100644
--- a/drivers/net/ethernet/ti/cpmac.c
+++ b/drivers/net/ethernet/ti/cpmac.c
@@ -1207,7 +1207,6 @@ static int cpmac_remove(struct platform_device *pdev)
 static struct platform_driver cpmac_driver = {
 	.driver = {
 		.name 	= "cpmac",
-		.owner 	= THIS_MODULE,
 	},
 	.probe 	= cpmac_probe,
 	.remove = cpmac_remove,
diff --git a/drivers/net/ethernet/ti/cpsw-phy-sel.c b/drivers/net/ethernet/ti/cpsw-phy-sel.c
index aa8bf45e53dc..0ea78326cc21 100644
--- a/drivers/net/ethernet/ti/cpsw-phy-sel.c
+++ b/drivers/net/ethernet/ti/cpsw-phy-sel.c
@@ -211,7 +211,6 @@ static struct platform_driver cpsw_phy_sel_driver = {
 	.probe		= cpsw_phy_sel_probe,
 	.driver		= {
 		.name	= "cpsw-phy-sel",
-		.owner	= THIS_MODULE,
 		.of_match_table = cpsw_phy_sel_id_table,
 	},
 };
diff --git a/drivers/net/ethernet/ti/cpsw.c b/drivers/net/ethernet/ti/cpsw.c
index 999fb72688d2..952e1e4764b7 100644
--- a/drivers/net/ethernet/ti/cpsw.c
+++ b/drivers/net/ethernet/ti/cpsw.c
@@ -33,6 +33,8 @@
 #include <linux/of_net.h>
 #include <linux/of_device.h>
 #include <linux/if_vlan.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
 
 #include <linux/pinctrl/consumer.h>
 
@@ -397,6 +399,8 @@ struct cpsw_priv {
 	struct cpdma_ctlr		*dma;
 	struct cpdma_chan		*txch, *rxch;
 	struct cpsw_ale			*ale;
+	bool				rx_pause;
+	bool				tx_pause;
 	/* snapshot of IRQ numbers */
 	u32 irqs_table[4];
 	u32 num_irqs;
@@ -699,6 +703,28 @@ static void cpsw_rx_handler(void *token, int len, int status)
 	cpsw_dual_emac_src_port_detect(status, priv, ndev, skb);
 
 	if (unlikely(status < 0) || unlikely(!netif_running(ndev))) {
+		bool ndev_status = false;
+		struct cpsw_slave *slave = priv->slaves;
+		int n;
+
+		if (priv->data.dual_emac) {
+			/* In dual emac mode check for all interfaces */
+			for (n = priv->data.slaves; n; n--, slave++)
+				if (netif_running(slave->ndev))
+					ndev_status = true;
+		}
+
+		if (ndev_status && (status >= 0)) {
+			/* The packet received is for the interface which
+			 * is already down and the other interface is up
+			 * and running, intead of freeing which results
+			 * in reducing of the number of rx descriptor in
+			 * DMA engine, requeue skb back to cpdma.
+			 */
+			new_skb = skb;
+			goto requeue;
+		}
+
 		/* the interface is going down, skbs are purged */
 		dev_kfree_skb_any(skb);
 		return;
@@ -717,6 +743,7 @@ static void cpsw_rx_handler(void *token, int len, int status)
 		new_skb = skb;
 	}
 
+requeue:
 	ret = cpdma_chan_submit(priv->rxch, new_skb, new_skb->data,
 			skb_tailroom(new_skb), 0);
 	if (WARN_ON(ret < 0))
@@ -832,6 +859,12 @@ static void _cpsw_adjust_link(struct cpsw_slave *slave,
 		else if (phy->speed == 10)
 			mac_control |= BIT(18); /* In Band mode */
 
+		if (priv->rx_pause)
+			mac_control |= BIT(3);
+
+		if (priv->tx_pause)
+			mac_control |= BIT(4);
+
 		*link = true;
 	} else {
 		mac_control = 0;
@@ -1223,6 +1256,9 @@ static int cpsw_ndo_open(struct net_device *ndev)
 		/* enable statistics collection only on all ports */
 		__raw_writel(0x7, &priv->regs->stat_port_en);
 
+		/* Enable internal fifo flow control */
+		writel(0x7, &priv->regs->flow_control);
+
 		if (WARN_ON(!priv->data.rx_descs))
 			priv->data.rx_descs = 128;
 
@@ -1784,6 +1820,30 @@ static int cpsw_set_wol(struct net_device *ndev, struct ethtool_wolinfo *wol)
 		return -EOPNOTSUPP;
 }
 
+static void cpsw_get_pauseparam(struct net_device *ndev,
+				struct ethtool_pauseparam *pause)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+
+	pause->autoneg = AUTONEG_DISABLE;
+	pause->rx_pause = priv->rx_pause ? true : false;
+	pause->tx_pause = priv->tx_pause ? true : false;
+}
+
+static int cpsw_set_pauseparam(struct net_device *ndev,
+			       struct ethtool_pauseparam *pause)
+{
+	struct cpsw_priv *priv = netdev_priv(ndev);
+	bool link;
+
+	priv->rx_pause = pause->rx_pause ? true : false;
+	priv->tx_pause = pause->tx_pause ? true : false;
+
+	for_each_slave(priv, _cpsw_adjust_link, priv, &link);
+
+	return 0;
+}
+
 static const struct ethtool_ops cpsw_ethtool_ops = {
 	.get_drvinfo	= cpsw_get_drvinfo,
 	.get_msglevel	= cpsw_get_msglevel,
@@ -1797,6 +1857,8 @@ static const struct ethtool_ops cpsw_ethtool_ops = {
 	.get_sset_count		= cpsw_get_sset_count,
 	.get_strings		= cpsw_get_strings,
 	.get_ethtool_stats	= cpsw_get_ethtool_stats,
+	.get_pauseparam		= cpsw_get_pauseparam,
+	.set_pauseparam		= cpsw_set_pauseparam,
 	.get_wol	= cpsw_get_wol,
 	.set_wol	= cpsw_set_wol,
 	.get_regs_len	= cpsw_get_regs_len,
@@ -1816,6 +1878,36 @@ static void cpsw_slave_init(struct cpsw_slave *slave, struct cpsw_priv *priv,
 	slave->port_vlan = data->dual_emac_res_vlan;
 }
 
+#define AM33XX_CTRL_MAC_LO_REG(id) (0x630 + 0x8 * id)
+#define AM33XX_CTRL_MAC_HI_REG(id) (0x630 + 0x8 * id + 0x4)
+
+static int cpsw_am33xx_cm_get_macid(struct device *dev, int slave,
+		u8 *mac_addr)
+{
+	u32 macid_lo;
+	u32 macid_hi;
+	struct regmap *syscon;
+
+	syscon = syscon_regmap_lookup_by_phandle(dev->of_node, "syscon");
+	if (IS_ERR(syscon)) {
+		if (PTR_ERR(syscon) == -ENODEV)
+			return 0;
+		return PTR_ERR(syscon);
+	}
+
+	regmap_read(syscon, AM33XX_CTRL_MAC_LO_REG(slave), &macid_lo);
+	regmap_read(syscon, AM33XX_CTRL_MAC_HI_REG(slave), &macid_hi);
+
+	mac_addr[5] = (macid_lo >> 8) & 0xff;
+	mac_addr[4] = macid_lo & 0xff;
+	mac_addr[3] = (macid_hi >> 24) & 0xff;
+	mac_addr[2] = (macid_hi >> 16) & 0xff;
+	mac_addr[1] = (macid_hi >> 8) & 0xff;
+	mac_addr[0] = macid_hi & 0xff;
+
+	return 0;
+}
+
 static int cpsw_probe_dt(struct cpsw_platform_data *data,
 			 struct platform_device *pdev)
 {
@@ -1921,15 +2013,23 @@ static int cpsw_probe_dt(struct cpsw_platform_data *data,
 		mdio = of_find_device_by_node(mdio_node);
 		of_node_put(mdio_node);
 		if (!mdio) {
-			pr_err("Missing mdio platform device\n");
+			dev_err(&pdev->dev, "Missing mdio platform device\n");
 			return -EINVAL;
 		}
 		snprintf(slave_data->phy_id, sizeof(slave_data->phy_id),
 			 PHY_ID_FMT, mdio->name, phyid);
 
 		mac_addr = of_get_mac_address(slave_node);
-		if (mac_addr)
+		if (mac_addr) {
 			memcpy(slave_data->mac_addr, mac_addr, ETH_ALEN);
+		} else {
+			if (of_machine_is_compatible("ti,am33xx")) {
+				ret = cpsw_am33xx_cm_get_macid(&pdev->dev, i,
+							slave_data->mac_addr);
+				if (ret)
+					return ret;
+			}
+		}
 
 		slave_data->phy_if = of_get_phy_mode(slave_node);
 		if (slave_data->phy_if < 0) {
@@ -2063,6 +2163,7 @@ static int cpsw_probe(struct platform_device *pdev)
 	priv->irq_enabled = true;
 	if (!priv->cpts) {
 		dev_err(&pdev->dev, "error allocating cpts\n");
+		ret = -ENOMEM;
 		goto clean_ndev_ret;
 	}
 
@@ -2232,18 +2333,24 @@ static int cpsw_probe(struct platform_device *pdev)
 	}
 
 	while ((res = platform_get_resource(priv->pdev, IORESOURCE_IRQ, k))) {
-		for (i = res->start; i <= res->end; i++) {
-			if (devm_request_irq(&pdev->dev, i, cpsw_interrupt, 0,
-					     dev_name(&pdev->dev), priv)) {
-				dev_err(priv->dev, "error attaching irq\n");
-				goto clean_ale_ret;
-			}
-			priv->irqs_table[k] = i;
-			priv->num_irqs = k + 1;
+		if (k >= ARRAY_SIZE(priv->irqs_table)) {
+			ret = -EINVAL;
+			goto clean_ale_ret;
+		}
+
+		ret = devm_request_irq(&pdev->dev, res->start, cpsw_interrupt,
+				       0, dev_name(&pdev->dev), priv);
+		if (ret < 0) {
+			dev_err(priv->dev, "error attaching irq (%d)\n", ret);
+			goto clean_ale_ret;
 		}
+
+		priv->irqs_table[k] = res->start;
 		k++;
 	}
 
+	priv->num_irqs = k;
+
 	ndev->features |= NETIF_F_HW_VLAN_CTAG_FILTER;
 
 	ndev->netdev_ops = &cpsw_netdev_ops;
@@ -2285,6 +2392,15 @@ clean_ndev_ret:
 	return ret;
 }
 
+static int cpsw_remove_child_device(struct device *dev, void *c)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+
+	of_device_unregister(pdev);
+
+	return 0;
+}
+
 static int cpsw_remove(struct platform_device *pdev)
 {
 	struct net_device *ndev = platform_get_drvdata(pdev);
@@ -2299,6 +2415,7 @@ static int cpsw_remove(struct platform_device *pdev)
 	cpdma_chan_destroy(priv->rxch);
 	cpdma_ctlr_destroy(priv->dma);
 	pm_runtime_disable(&pdev->dev);
+	device_for_each_child(&pdev->dev, NULL, cpsw_remove_child_device);
 	if (priv->data.dual_emac)
 		free_netdev(cpsw_get_slave_ndev(priv, 1));
 	free_netdev(ndev);
@@ -2311,10 +2428,19 @@ static int cpsw_suspend(struct device *dev)
 	struct net_device	*ndev = platform_get_drvdata(pdev);
 	struct cpsw_priv	*priv = netdev_priv(ndev);
 
-	if (netif_running(ndev))
-		cpsw_ndo_stop(ndev);
+	if (priv->data.dual_emac) {
+		int i;
 
-	for_each_slave(priv, soft_reset_slave);
+		for (i = 0; i < priv->data.slaves; i++) {
+			if (netif_running(priv->slaves[i].ndev))
+				cpsw_ndo_stop(priv->slaves[i].ndev);
+			soft_reset_slave(priv->slaves + i);
+		}
+	} else {
+		if (netif_running(ndev))
+			cpsw_ndo_stop(ndev);
+		for_each_slave(priv, soft_reset_slave);
+	}
 
 	pm_runtime_put_sync(&pdev->dev);
 
@@ -2328,14 +2454,24 @@ static int cpsw_resume(struct device *dev)
 {
 	struct platform_device	*pdev = to_platform_device(dev);
 	struct net_device	*ndev = platform_get_drvdata(pdev);
+	struct cpsw_priv	*priv = netdev_priv(ndev);
 
 	pm_runtime_get_sync(&pdev->dev);
 
 	/* Select default pin state */
 	pinctrl_pm_select_default_state(&pdev->dev);
 
-	if (netif_running(ndev))
-		cpsw_ndo_open(ndev);
+	if (priv->data.dual_emac) {
+		int i;
+
+		for (i = 0; i < priv->data.slaves; i++) {
+			if (netif_running(priv->slaves[i].ndev))
+				cpsw_ndo_open(priv->slaves[i].ndev);
+		}
+	} else {
+		if (netif_running(ndev))
+			cpsw_ndo_open(ndev);
+	}
 	return 0;
 }
 
@@ -2353,7 +2489,6 @@ MODULE_DEVICE_TABLE(of, cpsw_of_mtable);
 static struct platform_driver cpsw_driver = {
 	.driver = {
 		.name	 = "cpsw",
-		.owner	 = THIS_MODULE,
 		.pm	 = &cpsw_pm_ops,
 		.of_match_table = cpsw_of_mtable,
 	},
diff --git a/drivers/net/ethernet/ti/cpsw.h b/drivers/net/ethernet/ti/cpsw.h
index 574f49da693f..1b710674630c 100644
--- a/drivers/net/ethernet/ti/cpsw.h
+++ b/drivers/net/ethernet/ti/cpsw.h
@@ -15,6 +15,7 @@
 #define __CPSW_H__
 
 #include <linux/if_ether.h>
+#include <linux/phy.h>
 
 struct cpsw_slave_data {
 	char		phy_id[MII_BUS_ID_SIZE];
diff --git a/drivers/net/ethernet/ti/davinci_cpdma.c b/drivers/net/ethernet/ti/davinci_cpdma.c
index 4a000f6dd6fc..657b65bf5cac 100644
--- a/drivers/net/ethernet/ti/davinci_cpdma.c
+++ b/drivers/net/ethernet/ti/davinci_cpdma.c
@@ -193,12 +193,9 @@ fail:
 
 static void cpdma_desc_pool_destroy(struct cpdma_desc_pool *pool)
 {
-	unsigned long flags;
-
 	if (!pool)
 		return;
 
-	spin_lock_irqsave(&pool->lock, flags);
 	WARN_ON(pool->used_desc);
 	if (pool->cpumap) {
 		dma_free_coherent(pool->dev, pool->mem_size, pool->cpumap,
@@ -206,7 +203,6 @@ static void cpdma_desc_pool_destroy(struct cpdma_desc_pool *pool)
 	} else {
 		iounmap(pool->iomap);
 	}
-	spin_unlock_irqrestore(&pool->lock, flags);
 }
 
 static inline dma_addr_t desc_phys(struct cpdma_desc_pool *pool,
@@ -561,7 +557,6 @@ int cpdma_chan_destroy(struct cpdma_chan *chan)
 		cpdma_chan_stop(chan);
 	ctlr->channels[chan->chan_num] = NULL;
 	spin_unlock_irqrestore(&ctlr->lock, flags);
-	kfree(chan);
 	return 0;
 }
 EXPORT_SYMBOL_GPL(cpdma_chan_destroy);
diff --git a/drivers/net/ethernet/ti/davinci_emac.c b/drivers/net/ethernet/ti/davinci_emac.c
index 35a139e9a833..ea712512c7d1 100644
--- a/drivers/net/ethernet/ti/davinci_emac.c
+++ b/drivers/net/ethernet/ti/davinci_emac.c
@@ -2083,7 +2083,6 @@ MODULE_DEVICE_TABLE(of, davinci_emac_of_match);
 static struct platform_driver davinci_emac_driver = {
 	.driver = {
 		.name	 = "davinci_emac",
-		.owner	 = THIS_MODULE,
 		.pm	 = &davinci_emac_pm_ops,
 		.of_match_table = of_match_ptr(davinci_emac_of_match),
 	},
diff --git a/drivers/net/ethernet/ti/davinci_mdio.c b/drivers/net/ethernet/ti/davinci_mdio.c
index 2791f6f2db11..98655b44b97e 100644
--- a/drivers/net/ethernet/ti/davinci_mdio.c
+++ b/drivers/net/ethernet/ti/davinci_mdio.c
@@ -481,7 +481,6 @@ MODULE_DEVICE_TABLE(of, davinci_mdio_of_mtable);
 static struct platform_driver davinci_mdio_driver = {
 	.driver = {
 		.name	 = "davinci_mdio",
-		.owner	 = THIS_MODULE,
 		.pm	 = &davinci_mdio_pm_ops,
 		.of_match_table = of_match_ptr(davinci_mdio_of_mtable),
 	},
diff --git a/drivers/net/ethernet/tile/tilegx.c b/drivers/net/ethernet/tile/tilegx.c
index 69557a26f749..049747f558c9 100644
--- a/drivers/net/ethernet/tile/tilegx.c
+++ b/drivers/net/ethernet/tile/tilegx.c
@@ -423,7 +423,7 @@ static void tile_net_pop_all_buffers(int instance, int stack)
 /* Provide linux buffers to mPIPE. */
 static void tile_net_provide_needed_buffers(void)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	int instance, kind;
 	for (instance = 0; instance < NR_MPIPE_MAX &&
 		     info->mpipe[instance].has_iqueue; instance++)	{
@@ -551,7 +551,7 @@ static inline bool filter_packet(struct net_device *dev, void *buf)
 static void tile_net_receive_skb(struct net_device *dev, struct sk_buff *skb,
 				 gxio_mpipe_idesc_t *idesc, unsigned long len)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	struct tile_net_priv *priv = netdev_priv(dev);
 	int instance = priv->instance;
 
@@ -585,7 +585,7 @@ static void tile_net_receive_skb(struct net_device *dev, struct sk_buff *skb,
 /* Handle a packet.  Return true if "processed", false if "filtered". */
 static bool tile_net_handle_packet(int instance, gxio_mpipe_idesc_t *idesc)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	struct mpipe_data *md = &mpipe_data[instance];
 	struct net_device *dev = md->tile_net_devs_for_channel[idesc->channel];
 	uint8_t l2_offset;
@@ -651,7 +651,7 @@ drop:
  */
 static int tile_net_poll(struct napi_struct *napi, int budget)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	unsigned int work = 0;
 	gxio_mpipe_idesc_t *idesc;
 	int instance, i, n;
@@ -700,7 +700,7 @@ done:
 /* Handle an ingress interrupt from an instance on the current cpu. */
 static irqreturn_t tile_net_handle_ingress_irq(int irq, void *id)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	napi_schedule(&info->mpipe[(uint64_t)id].napi);
 	return IRQ_HANDLED;
 }
@@ -763,7 +763,7 @@ static enum hrtimer_restart tile_net_handle_tx_wake_timer(struct hrtimer *t)
 /* Make sure the egress timer is scheduled. */
 static void tile_net_schedule_egress_timer(void)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 
 	if (!info->egress_timer_scheduled) {
 		hrtimer_start(&info->egress_timer,
@@ -780,7 +780,7 @@ static void tile_net_schedule_egress_timer(void)
  */
 static enum hrtimer_restart tile_net_handle_egress_timer(struct hrtimer *t)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	unsigned long irqflags;
 	bool pending = false;
 	int i, instance;
@@ -1927,7 +1927,7 @@ static void tso_egress(struct net_device *dev, gxio_mpipe_equeue_t *equeue,
  */
 static int tile_net_tx_tso(struct sk_buff *skb, struct net_device *dev)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	struct tile_net_priv *priv = netdev_priv(dev);
 	int channel = priv->echannel;
 	int instance = priv->instance;
@@ -1996,7 +1996,7 @@ static unsigned int tile_net_tx_frags(struct frag *frags,
 /* Help the kernel transmit a packet. */
 static int tile_net_tx(struct sk_buff *skb, struct net_device *dev)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	struct tile_net_priv *priv = netdev_priv(dev);
 	int instance = priv->instance;
 	struct mpipe_data *md = &mpipe_data[instance];
@@ -2138,7 +2138,7 @@ static int tile_net_set_mac_address(struct net_device *dev, void *p)
 static void tile_net_netpoll(struct net_device *dev)
 {
 	int instance = mpipe_instance(dev);
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	struct mpipe_data *md = &mpipe_data[instance];
 
 	disable_percpu_irq(md->ingress_irq);
@@ -2237,7 +2237,7 @@ static void tile_net_dev_init(const char *name, const uint8_t *mac)
 /* Per-cpu module initialization. */
 static void tile_net_init_module_percpu(void *unused)
 {
-	struct tile_net_info *info = &__get_cpu_var(per_cpu_info);
+	struct tile_net_info *info = this_cpu_ptr(&per_cpu_info);
 	int my_cpu = smp_processor_id();
 	int instance;
 
diff --git a/drivers/net/ethernet/tile/tilepro.c b/drivers/net/ethernet/tile/tilepro.c
index 88c712126692..fb12d31cfcf6 100644
--- a/drivers/net/ethernet/tile/tilepro.c
+++ b/drivers/net/ethernet/tile/tilepro.c
@@ -956,7 +956,7 @@ static int tile_net_open_aux(struct net_device *dev)
 	 */
 	if (hv_dev_pwrite(priv->hv_devhdl, 0, (HV_VirtAddr)&dummy,
 			  sizeof(dummy), NETIO_IPP_START_SHIM_OFF) < 0) {
-		pr_warning("Failed to start LIPP/LEPP.\n");
+		pr_warn("Failed to start LIPP/LEPP\n");
 		return -EIO;
 	}
 
@@ -996,13 +996,13 @@ static void tile_net_register(void *dev_ptr)
 	PDEBUG("tile_net_register(queue_id %d)\n", queue_id);
 
 	if (!strcmp(dev->name, "xgbe0"))
-		info = &__get_cpu_var(hv_xgbe0);
+		info = this_cpu_ptr(&hv_xgbe0);
 	else if (!strcmp(dev->name, "xgbe1"))
-		info = &__get_cpu_var(hv_xgbe1);
+		info = this_cpu_ptr(&hv_xgbe1);
 	else if (!strcmp(dev->name, "gbe0"))
-		info = &__get_cpu_var(hv_gbe0);
+		info = this_cpu_ptr(&hv_gbe0);
 	else if (!strcmp(dev->name, "gbe1"))
-		info = &__get_cpu_var(hv_gbe1);
+		info = this_cpu_ptr(&hv_gbe1);
 	else
 		BUG();
 
@@ -2399,8 +2399,7 @@ static int __init network_cpus_setup(char *str)
 {
 	int rc = cpulist_parse_crop(str, &network_cpus_map);
 	if (rc != 0) {
-		pr_warning("network_cpus=%s: malformed cpu list\n",
-		       str);
+		pr_warn("network_cpus=%s: malformed cpu list\n", str);
 	} else {
 
 		/* Remove dedicated cpus. */
@@ -2409,8 +2408,7 @@ static int __init network_cpus_setup(char *str)
 
 
 		if (cpumask_empty(&network_cpus_map)) {
-			pr_warning("Ignoring network_cpus='%s'.\n",
-			       str);
+			pr_warn("Ignoring network_cpus='%s'\n", str);
 		} else {
 			char buf[1024];
 			cpulist_scnprintf(buf, sizeof(buf), &network_cpus_map);
diff --git a/drivers/net/ethernet/toshiba/spider_net.c b/drivers/net/ethernet/toshiba/spider_net.c
index 3e38f67c6011..8e9371a3388a 100644
--- a/drivers/net/ethernet/toshiba/spider_net.c
+++ b/drivers/net/ethernet/toshiba/spider_net.c
@@ -267,34 +267,6 @@ spider_net_set_promisc(struct spider_net_card *card)
 }
 
 /**
- * spider_net_get_mac_address - read mac address from spider card
- * @card: device structure
- *
- * reads MAC address from GMACUNIMACU and GMACUNIMACL registers
- */
-static int
-spider_net_get_mac_address(struct net_device *netdev)
-{
-	struct spider_net_card *card = netdev_priv(netdev);
-	u32 macl, macu;
-
-	macl = spider_net_read_reg(card, SPIDER_NET_GMACUNIMACL);
-	macu = spider_net_read_reg(card, SPIDER_NET_GMACUNIMACU);
-
-	netdev->dev_addr[0] = (macu >> 24) & 0xff;
-	netdev->dev_addr[1] = (macu >> 16) & 0xff;
-	netdev->dev_addr[2] = (macu >> 8) & 0xff;
-	netdev->dev_addr[3] = macu & 0xff;
-	netdev->dev_addr[4] = (macl >> 8) & 0xff;
-	netdev->dev_addr[5] = macl & 0xff;
-
-	if (!is_valid_ether_addr(&netdev->dev_addr[0]))
-		return -EINVAL;
-
-	return 0;
-}
-
-/**
  * spider_net_get_descr_status -- returns the status of a descriptor
  * @descr: descriptor to look at
  *
@@ -1345,15 +1317,17 @@ spider_net_set_mac(struct net_device *netdev, void *p)
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
+	memcpy(netdev->dev_addr, addr->sa_data, ETH_ALEN);
+
 	/* switch off GMACTPE and GMACRPE */
 	regvalue = spider_net_read_reg(card, SPIDER_NET_GMACOPEMD);
 	regvalue &= ~((1 << 5) | (1 << 6));
 	spider_net_write_reg(card, SPIDER_NET_GMACOPEMD, regvalue);
 
 	/* write mac */
-	macu = (addr->sa_data[0]<<24) + (addr->sa_data[1]<<16) +
-		(addr->sa_data[2]<<8) + (addr->sa_data[3]);
-	macl = (addr->sa_data[4]<<8) + (addr->sa_data[5]);
+	macu = (netdev->dev_addr[0]<<24) + (netdev->dev_addr[1]<<16) +
+		(netdev->dev_addr[2]<<8) + (netdev->dev_addr[3]);
+	macl = (netdev->dev_addr[4]<<8) + (netdev->dev_addr[5]);
 	spider_net_write_reg(card, SPIDER_NET_GMACUNIMACU, macu);
 	spider_net_write_reg(card, SPIDER_NET_GMACUNIMACL, macl);
 
@@ -1364,12 +1338,6 @@ spider_net_set_mac(struct net_device *netdev, void *p)
 
 	spider_net_set_promisc(card);
 
-	/* look up, whether we have been successful */
-	if (spider_net_get_mac_address(netdev))
-		return -EADDRNOTAVAIL;
-	if (memcmp(netdev->dev_addr,addr->sa_data,netdev->addr_len))
-		return -EADDRNOTAVAIL;
-
 	return 0;
 }
 
diff --git a/drivers/net/ethernet/via/via-velocity.c b/drivers/net/ethernet/via/via-velocity.c
index f5fbc12d3e10..a43e8492b1ce 100644
--- a/drivers/net/ethernet/via/via-velocity.c
+++ b/drivers/net/ethernet/via/via-velocity.c
@@ -2056,7 +2056,7 @@ static int velocity_receive_frame(struct velocity_info *vptr, int idx)
 	struct sk_buff *skb;
 
 	if (rd->rdesc0.RSR & (RSR_STP | RSR_EDP)) {
-		VELOCITY_PRT(MSG_LEVEL_VERBOSE, KERN_ERR " %s : the received frame span multple RDs.\n", vptr->netdev->name);
+		VELOCITY_PRT(MSG_LEVEL_VERBOSE, KERN_ERR " %s : the received frame spans multiple RDs.\n", vptr->netdev->name);
 		stats->rx_length_errors++;
 		return -EINVAL;
 	}
diff --git a/drivers/net/ethernet/wiznet/w5100.c b/drivers/net/ethernet/wiznet/w5100.c
index 104d46f37969..0f56b1c0e082 100644
--- a/drivers/net/ethernet/wiznet/w5100.c
+++ b/drivers/net/ethernet/wiznet/w5100.c
@@ -708,7 +708,6 @@ static int w5100_probe(struct platform_device *pdev)
 	priv = netdev_priv(ndev);
 	priv->ndev = ndev;
 
-	ether_setup(ndev);
 	ndev->netdev_ops = &w5100_netdev_ops;
 	ndev->ethtool_ops = &w5100_ethtool_ops;
 	ndev->watchdog_timeo = HZ;
diff --git a/drivers/net/ethernet/wiznet/w5300.c b/drivers/net/ethernet/wiznet/w5300.c
index 1f33c4c86c20..f961f14a0473 100644
--- a/drivers/net/ethernet/wiznet/w5300.c
+++ b/drivers/net/ethernet/wiznet/w5300.c
@@ -620,7 +620,6 @@ static int w5300_probe(struct platform_device *pdev)
 	priv = netdev_priv(ndev);
 	priv->ndev = ndev;
 
-	ether_setup(ndev);
 	ndev->netdev_ops = &w5300_netdev_ops;
 	ndev->ethtool_ops = &w5300_ethtool_ops;
 	ndev->watchdog_timeo = HZ;
diff --git a/drivers/net/ethernet/xilinx/ll_temac_main.c b/drivers/net/ethernet/xilinx/ll_temac_main.c
index fda5891835d4..629077050fce 100644
--- a/drivers/net/ethernet/xilinx/ll_temac_main.c
+++ b/drivers/net/ethernet/xilinx/ll_temac_main.c
@@ -1012,7 +1012,6 @@ static int temac_of_probe(struct platform_device *op)
 	if (!ndev)
 		return -ENOMEM;
 
-	ether_setup(ndev);
 	platform_set_drvdata(op, ndev);
 	SET_NETDEV_DEV(ndev, &op->dev);
 	ndev->flags &= ~IFF_MULTICAST;  /* clear multicast */
diff --git a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
index c8fd94133ecd..4ea2d4e6f1d1 100644
--- a/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
+++ b/drivers/net/ethernet/xilinx/xilinx_axienet_main.c
@@ -1485,7 +1485,6 @@ static int axienet_of_probe(struct platform_device *op)
 	if (!ndev)
 		return -ENOMEM;
 
-	ether_setup(ndev);
 	platform_set_drvdata(op, ndev);
 
 	SET_NETDEV_DEV(ndev, &op->dev);
diff --git a/drivers/net/fddi/defxx.c b/drivers/net/fddi/defxx.c
index c44eaf019dea..caed6eee289c 100644
--- a/drivers/net/fddi/defxx.c
+++ b/drivers/net/fddi/defxx.c
@@ -466,7 +466,8 @@ static void dfx_get_bars(struct device *bdev,
 			*bar_len = (bar | PI_MEM_ADD_MASK_M) + 1;
 		} else {
 			*bar_start = base_addr;
-			*bar_len = PI_ESIC_K_CSR_IO_LEN;
+			*bar_len = PI_ESIC_K_CSR_IO_LEN +
+				   PI_ESIC_K_BURST_HOLDOFF_LEN;
 		}
 	}
 	if (dfx_bus_tc) {
@@ -683,6 +684,9 @@ static void dfx_bus_init(struct net_device *dev)
 	if (dfx_bus_eisa) {
 		unsigned long base_addr = to_eisa_device(bdev)->base_addr;
 
+		/* Disable the board before fiddling with the decoders.  */
+		outb(0, base_addr + PI_ESIC_K_SLOT_CNTRL);
+
 		/* Get the interrupt level from the ESIC chip.  */
 		val = inb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 		val &= PI_CONFIG_STAT_0_M_IRQ;
@@ -709,38 +713,46 @@ static void dfx_bus_init(struct net_device *dev)
 		/*
 		 * Enable memory decoding (MEMCS0) and/or port decoding
 		 * (IOCS1/IOCS0) as appropriate in Function Control
-		 * Register.  One of the port chip selects seems to be
-		 * used for the Burst Holdoff register, but this bit of
-		 * documentation is missing and as yet it has not been
-		 * determined which of the two.  This is also the reason
-		 * the size of the decoded port range is twice as large
-		 * as one required by the PDQ.
+		 * Register.  IOCS0 is used for PDQ registers, taking 16
+		 * 32-bit words, while IOCS1 is used for the Burst Holdoff
+		 * register, taking a single 32-bit word only.  We use the
+		 * slot-specific I/O range as per the ESIC spec, that is
+		 * set bits 15:12 in the mask registers to mask them out.
 		 */
 
 		/* Set the decode range of the board.  */
-		val = ((bp->base.port >> 12) << PI_IO_CMP_V_SLOT);
-		outb(base_addr + PI_ESIC_K_IO_ADD_CMP_0_1, val);
-		outb(base_addr + PI_ESIC_K_IO_ADD_CMP_0_0, 0);
-		outb(base_addr + PI_ESIC_K_IO_ADD_CMP_1_1, val);
-		outb(base_addr + PI_ESIC_K_IO_ADD_CMP_1_0, 0);
-		val = PI_ESIC_K_CSR_IO_LEN - 1;
-		outb(base_addr + PI_ESIC_K_IO_ADD_MASK_0_1, (val >> 8) & 0xff);
-		outb(base_addr + PI_ESIC_K_IO_ADD_MASK_0_0, val & 0xff);
-		outb(base_addr + PI_ESIC_K_IO_ADD_MASK_1_1, (val >> 8) & 0xff);
-		outb(base_addr + PI_ESIC_K_IO_ADD_MASK_1_0, val & 0xff);
+		val = 0;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_CMP_0_1);
+		val = PI_DEFEA_K_CSR_IO;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_CMP_0_0);
+
+		val = PI_IO_CMP_M_SLOT;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_MASK_0_1);
+		val = (PI_ESIC_K_CSR_IO_LEN - 1) & ~3;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_MASK_0_0);
+
+		val = 0;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_CMP_1_1);
+		val = PI_DEFEA_K_BURST_HOLDOFF;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_CMP_1_0);
+
+		val = PI_IO_CMP_M_SLOT;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_MASK_1_1);
+		val = (PI_ESIC_K_BURST_HOLDOFF_LEN - 1) & ~3;
+		outb(val, base_addr + PI_ESIC_K_IO_ADD_MASK_1_0);
 
 		/* Enable the decoders.  */
 		val = PI_FUNCTION_CNTRL_M_IOCS1 | PI_FUNCTION_CNTRL_M_IOCS0;
 		if (dfx_use_mmio)
 			val |= PI_FUNCTION_CNTRL_M_MEMCS0;
-		outb(base_addr + PI_ESIC_K_FUNCTION_CNTRL, val);
+		outb(val, base_addr + PI_ESIC_K_FUNCTION_CNTRL);
 
 		/*
 		 * Enable access to the rest of the module
 		 * (including PDQ and packet memory).
 		 */
 		val = PI_SLOT_CNTRL_M_ENB;
-		outb(base_addr + PI_ESIC_K_SLOT_CNTRL, val);
+		outb(val, base_addr + PI_ESIC_K_SLOT_CNTRL);
 
 		/*
 		 * Map PDQ registers into memory or port space.  This is
@@ -748,15 +760,15 @@ static void dfx_bus_init(struct net_device *dev)
 		 */
 		val = inb(base_addr + PI_DEFEA_K_BURST_HOLDOFF);
 		if (dfx_use_mmio)
-			val |= PI_BURST_HOLDOFF_V_MEM_MAP;
+			val |= PI_BURST_HOLDOFF_M_MEM_MAP;
 		else
-			val &= ~PI_BURST_HOLDOFF_V_MEM_MAP;
-		outb(base_addr + PI_DEFEA_K_BURST_HOLDOFF, val);
+			val &= ~PI_BURST_HOLDOFF_M_MEM_MAP;
+		outb(val, base_addr + PI_DEFEA_K_BURST_HOLDOFF);
 
 		/* Enable interrupts at EISA bus interface chip (ESIC) */
 		val = inb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 		val |= PI_CONFIG_STAT_0_M_INT_ENB;
-		outb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0, val);
+		outb(val, base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 	}
 	if (dfx_bus_pci) {
 		struct pci_dev *pdev = to_pci_dev(bdev);
@@ -825,7 +837,7 @@ static void dfx_bus_uninit(struct net_device *dev)
 		/* Disable interrupts at EISA bus interface chip (ESIC) */
 		val = inb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 		val &= ~PI_CONFIG_STAT_0_M_INT_ENB;
-		outb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0, val);
+		outb(val, base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 	}
 	if (dfx_bus_pci) {
 		/* Disable interrupts at PCI bus interface chip (PFI) */
@@ -1917,7 +1929,7 @@ static irqreturn_t dfx_interrupt(int irq, void *dev_id)
 
 		/* Disable interrupts at the ESIC */
 		status &= ~PI_CONFIG_STAT_0_M_INT_ENB;
-		outb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0, status);
+		outb(status, base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 
 		/* Call interrupt service routine for this adapter */
 		dfx_int_common(dev);
@@ -1925,7 +1937,7 @@ static irqreturn_t dfx_interrupt(int irq, void *dev_id)
 		/* Reenable interrupts at the ESIC */
 		status = inb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 		status |= PI_CONFIG_STAT_0_M_INT_ENB;
-		outb(base_addr + PI_ESIC_K_IO_CONFIG_STAT_0, status);
+		outb(status, base_addr + PI_ESIC_K_IO_CONFIG_STAT_0);
 
 		spin_unlock(&bp->lock);
 	}
diff --git a/drivers/net/fddi/defxx.h b/drivers/net/fddi/defxx.h
index adb63f3f7b4a..9527f0182fd4 100644
--- a/drivers/net/fddi/defxx.h
+++ b/drivers/net/fddi/defxx.h
@@ -1479,8 +1479,10 @@ typedef union
 
 /* Define EISA controller register offsets */
 
-#define PI_ESIC_K_CSR_IO_LEN		0x80		/* 128 bytes */
+#define PI_ESIC_K_CSR_IO_LEN		0x40		/* 64 bytes */
+#define PI_ESIC_K_BURST_HOLDOFF_LEN	0x04		/* 4 bytes */
 
+#define PI_DEFEA_K_CSR_IO		0x000
 #define PI_DEFEA_K_BURST_HOLDOFF	0x040
 
 #define PI_ESIC_K_SLOT_ID            	0xC80
@@ -1558,11 +1560,9 @@ typedef union
 
 #define PI_MEM_ADD_MASK_M		0x3ff
 
-/*
- * Define the fields in the IO Compare registers.
- * The driver must initialize the slot field with the slot ID shifted by the
- * amount shown below.
- */
+/* Define the fields in the I/O Address Compare and Mask registers.  */
+
+#define PI_IO_CMP_M_SLOT		0xf0
 
 #define PI_IO_CMP_V_SLOT		4
 
diff --git a/drivers/net/fddi/skfp/h/skfbi.h b/drivers/net/fddi/skfp/h/skfbi.h
index c1ba26c06d73..3de2f0d15fe2 100644
--- a/drivers/net/fddi/skfp/h/skfbi.h
+++ b/drivers/net/fddi/skfp/h/skfbi.h
@@ -147,11 +147,6 @@
 #define	PCI_MEM64BIT	(2<<1)	     /* Base addr anywhere in 64 Bit range */
 #define	PCI_MEMSPACE	0x00000001L  /* Bit 0:	Memory Space Indic. */
 
-/*	PCI_BASE_2ND	32 bit	2nd Base address */
-#define	PCI_IOBASE	0xffffff00L  /* Bit 31..8:  I/O Base address */
-#define	PCI_IOSIZE	0x000000fcL  /* Bit 7..2:   I/O Size Requirements */
-#define	PCI_IOSPACE	0x00000001L  /* Bit 0:	    I/O Space Indicator */
-
 /*	PCI_SUB_VID	16 bit	Subsystem Vendor ID */
 /*	PCI_SUB_ID	16 bit	Subsystem ID */
 
diff --git a/drivers/net/hyperv/hyperv_net.h b/drivers/net/hyperv/hyperv_net.h
index d5e07def6a59..2f48f790c9b4 100644
--- a/drivers/net/hyperv/hyperv_net.h
+++ b/drivers/net/hyperv/hyperv_net.h
@@ -591,7 +591,7 @@ struct nvsp_message {
 
 #define NETVSC_RECEIVE_BUFFER_ID		0xcafe
 
-#define NETVSC_PACKET_SIZE                      2048
+#define NETVSC_PACKET_SIZE                      4096
 
 #define VRSS_SEND_TAB_SIZE 16
 
@@ -642,7 +642,7 @@ struct netvsc_device {
 	int ring_size;
 
 	/* The primary channel callback buffer */
-	unsigned char cb_buffer[NETVSC_PACKET_SIZE];
+	unsigned char *cb_buffer;
 	/* The sub channel callback buffer */
 	unsigned char *sub_cb_buf;
 };
diff --git a/drivers/net/hyperv/netvsc.c b/drivers/net/hyperv/netvsc.c
index 66979cf7fca6..7d76c9523395 100644
--- a/drivers/net/hyperv/netvsc.c
+++ b/drivers/net/hyperv/netvsc.c
@@ -42,6 +42,12 @@ static struct netvsc_device *alloc_net_device(struct hv_device *device)
 	if (!net_device)
 		return NULL;
 
+	net_device->cb_buffer = kzalloc(NETVSC_PACKET_SIZE, GFP_KERNEL);
+	if (!net_device->cb_buffer) {
+		kfree(net_device);
+		return NULL;
+	}
+
 	init_waitqueue_head(&net_device->wait_drain);
 	net_device->start_remove = false;
 	net_device->destroy = false;
@@ -52,6 +58,12 @@ static struct netvsc_device *alloc_net_device(struct hv_device *device)
 	return net_device;
 }
 
+static void free_netvsc_device(struct netvsc_device *nvdev)
+{
+	kfree(nvdev->cb_buffer);
+	kfree(nvdev);
+}
+
 static struct netvsc_device *get_outbound_net_device(struct hv_device *device)
 {
 	struct netvsc_device *net_device;
@@ -551,7 +563,7 @@ int netvsc_device_remove(struct hv_device *device)
 	if (net_device->sub_cb_buf)
 		vfree(net_device->sub_cb_buf);
 
-	kfree(net_device);
+	free_netvsc_device(net_device);
 	return 0;
 }
 
@@ -705,6 +717,7 @@ int netvsc_send(struct hv_device *device,
 	unsigned int section_index = NETVSC_INVALID_INDEX;
 	u32 msg_size = 0;
 	struct sk_buff *skb;
+	u16 q_idx = packet->q_idx;
 
 
 	net_device = get_outbound_net_device(device);
@@ -769,24 +782,24 @@ int netvsc_send(struct hv_device *device,
 
 	if (ret == 0) {
 		atomic_inc(&net_device->num_outstanding_sends);
-		atomic_inc(&net_device->queue_sends[packet->q_idx]);
+		atomic_inc(&net_device->queue_sends[q_idx]);
 
 		if (hv_ringbuf_avail_percent(&out_channel->outbound) <
 			RING_AVAIL_PERCENT_LOWATER) {
 			netif_tx_stop_queue(netdev_get_tx_queue(
-					    ndev, packet->q_idx));
+					    ndev, q_idx));
 
 			if (atomic_read(&net_device->
-				queue_sends[packet->q_idx]) < 1)
+				queue_sends[q_idx]) < 1)
 				netif_tx_wake_queue(netdev_get_tx_queue(
-						    ndev, packet->q_idx));
+						    ndev, q_idx));
 		}
 	} else if (ret == -EAGAIN) {
 		netif_tx_stop_queue(netdev_get_tx_queue(
-				    ndev, packet->q_idx));
-		if (atomic_read(&net_device->queue_sends[packet->q_idx]) < 1) {
+				    ndev, q_idx));
+		if (atomic_read(&net_device->queue_sends[q_idx]) < 1) {
 			netif_tx_wake_queue(netdev_get_tx_queue(
-					    ndev, packet->q_idx));
+					    ndev, q_idx));
 			ret = -ENOSPC;
 		}
 	} else {
@@ -1042,10 +1055,8 @@ int netvsc_device_add(struct hv_device *device, void *additional_info)
 	struct net_device *ndev;
 
 	net_device = alloc_net_device(device);
-	if (!net_device) {
-		ret = -ENOMEM;
-		goto cleanup;
-	}
+	if (!net_device)
+		return -ENOMEM;
 
 	net_device->ring_size = ring_size;
 
@@ -1093,7 +1104,7 @@ close:
 	vmbus_close(device->channel);
 
 cleanup:
-	kfree(net_device);
+	free_netvsc_device(net_device);
 
 	return ret;
 }
diff --git a/drivers/net/hyperv/netvsc_drv.c b/drivers/net/hyperv/netvsc_drv.c
index a9c5eaadc426..9e17d1a91e71 100644
--- a/drivers/net/hyperv/netvsc_drv.c
+++ b/drivers/net/hyperv/netvsc_drv.c
@@ -162,7 +162,7 @@ union sub_key {
  * data: network byte order
  * return: host byte order
  */
-static u32 comp_hash(u8 *key, int klen, u8 *data, int dlen)
+static u32 comp_hash(u8 *key, int klen, void *data, int dlen)
 {
 	union sub_key subk;
 	int k_next = 4;
@@ -176,7 +176,7 @@ static u32 comp_hash(u8 *key, int klen, u8 *data, int dlen)
 	for (i = 0; i < dlen; i++) {
 		subk.kb = key[k_next];
 		k_next = (k_next + 1) % klen;
-		dt = data[i];
+		dt = ((u8 *)data)[i];
 		for (j = 0; j < 8; j++) {
 			if (dt & 0x80)
 				ret ^= subk.ka;
@@ -190,26 +190,20 @@ static u32 comp_hash(u8 *key, int klen, u8 *data, int dlen)
 
 static bool netvsc_set_hash(u32 *hash, struct sk_buff *skb)
 {
-	struct iphdr *iphdr;
+	struct flow_keys flow;
 	int data_len;
-	bool ret = false;
 
-	if (eth_hdr(skb)->h_proto != htons(ETH_P_IP))
+	if (!skb_flow_dissect(skb, &flow) || flow.n_proto != htons(ETH_P_IP))
 		return false;
 
-	iphdr = ip_hdr(skb);
+	if (flow.ip_proto == IPPROTO_TCP)
+		data_len = 12;
+	else
+		data_len = 8;
 
-	if (iphdr->version == 4) {
-		if (iphdr->protocol == IPPROTO_TCP)
-			data_len = 12;
-		else
-			data_len = 8;
-		*hash = comp_hash(netvsc_hash_key, HASH_KEYLEN,
-				  (u8 *)&iphdr->saddr, data_len);
-		ret = true;
-	}
+	*hash = comp_hash(netvsc_hash_key, HASH_KEYLEN, &flow, data_len);
 
-	return ret;
+	return true;
 }
 
 static u16 netvsc_select_queue(struct net_device *ndev, struct sk_buff *skb,
@@ -387,6 +381,7 @@ static int netvsc_start_xmit(struct sk_buff *skb, struct net_device *net)
 	int  hdr_offset;
 	u32 net_trans_info;
 	u32 hash;
+	u32 skb_length = skb->len;
 
 
 	/* We will atmost need two pages to describe the rndis
@@ -562,7 +557,7 @@ do_send:
 
 drop:
 	if (ret == 0) {
-		net->stats.tx_bytes += skb->len;
+		net->stats.tx_bytes += skb_length;
 		net->stats.tx_packets++;
 	} else {
 		kfree(packet);
diff --git a/drivers/net/ieee802154/mrf24j40.c b/drivers/net/ieee802154/mrf24j40.c
index 9e6a124b13f2..07e0b887c350 100644
--- a/drivers/net/ieee802154/mrf24j40.c
+++ b/drivers/net/ieee802154/mrf24j40.c
@@ -323,8 +323,8 @@ static int mrf24j40_read_rx_buf(struct mrf24j40 *devrec,
 #ifdef DEBUG
 	print_hex_dump(KERN_DEBUG, "mrf24j40 rx: ",
 		DUMP_PREFIX_OFFSET, 16, 1, data, *len, 0);
-	printk(KERN_DEBUG "mrf24j40 rx: lqi: %02hhx rssi: %02hhx\n",
-		lqi_rssi[0], lqi_rssi[1]);
+	pr_debug("mrf24j40 rx: lqi: %02hhx rssi: %02hhx\n",
+		 lqi_rssi[0], lqi_rssi[1]);
 #endif
 
 out:
@@ -385,7 +385,7 @@ err:
 static int mrf24j40_ed(struct ieee802154_dev *dev, u8 *level)
 {
 	/* TODO: */
-	printk(KERN_WARNING "mrf24j40: ed not implemented\n");
+	pr_warn("mrf24j40: ed not implemented\n");
 	*level = 0;
 	return 0;
 }
@@ -412,6 +412,7 @@ static void mrf24j40_stop(struct ieee802154_dev *dev)
 	struct mrf24j40 *devrec = dev->priv;
 	u8 val;
 	int ret;
+
 	dev_dbg(printdev(devrec), "stop\n");
 
 	ret = read_short_reg(devrec, REG_INTCON, &val);
@@ -419,8 +420,6 @@ static void mrf24j40_stop(struct ieee802154_dev *dev)
 		return;
 	val |= 0x1|0x8; /* Set TXNIE and RXIE. Disable Interrupts */
 	write_short_reg(devrec, REG_INTCON, val);
-
-	return;
 }
 
 static int mrf24j40_set_channel(struct ieee802154_dev *dev,
@@ -465,6 +464,7 @@ static int mrf24j40_filter(struct ieee802154_dev *dev,
 	if (changed & IEEE802515_AFILT_SADDR_CHANGED) {
 		/* Short Addr */
 		u8 addrh, addrl;
+
 		addrh = le16_to_cpu(filt->short_addr) >> 8 & 0xff;
 		addrl = le16_to_cpu(filt->short_addr) & 0xff;
 
@@ -483,16 +483,17 @@ static int mrf24j40_filter(struct ieee802154_dev *dev,
 			write_short_reg(devrec, REG_EADR0 + i, addr[i]);
 
 #ifdef DEBUG
-		printk(KERN_DEBUG "Set long addr to: ");
+		pr_debug("Set long addr to: ");
 		for (i = 0; i < 8; i++)
-			printk("%02hhx ", addr[7 - i]);
-		printk(KERN_DEBUG "\n");
+			pr_debug("%02hhx ", addr[7 - i]);
+		pr_debug("\n");
 #endif
 	}
 
 	if (changed & IEEE802515_AFILT_PANID_CHANGED) {
 		/* PAN ID */
 		u8 panidl, panidh;
+
 		panidh = le16_to_cpu(filt->pan_id) >> 8 & 0xff;
 		panidl = le16_to_cpu(filt->pan_id) & 0xff;
 		write_short_reg(devrec, REG_PANIDH, panidh);
@@ -701,7 +702,7 @@ static int mrf24j40_probe(struct spi_device *spi)
 	int ret = -ENOMEM;
 	struct mrf24j40 *devrec;
 
-	printk(KERN_INFO "mrf24j40: probe(). IRQ: %d\n", spi->irq);
+	dev_info(&spi->dev, "probe(). IRQ: %d\n", spi->irq);
 
 	devrec = devm_kzalloc(&spi->dev, sizeof(struct mrf24j40), GFP_KERNEL);
 	if (!devrec)
diff --git a/drivers/net/ifb.c b/drivers/net/ifb.c
index d2d4a3d2237f..34f846b4bd05 100644
--- a/drivers/net/ifb.c
+++ b/drivers/net/ifb.c
@@ -185,7 +185,8 @@ static void ifb_setup(struct net_device *dev)
 
 	dev->flags |= IFF_NOARP;
 	dev->flags &= ~IFF_MULTICAST;
-	dev->priv_flags &= ~(IFF_XMIT_DST_RELEASE | IFF_TX_SKB_SHARING);
+	dev->priv_flags &= ~IFF_TX_SKB_SHARING;
+	netif_keep_dst(dev);
 	eth_hw_addr_random(dev);
 }
 
diff --git a/drivers/net/irda/Kconfig b/drivers/net/irda/Kconfig
index 8d101d63abca..a2c227bfb687 100644
--- a/drivers/net/irda/Kconfig
+++ b/drivers/net/irda/Kconfig
@@ -397,7 +397,7 @@ config MCS_FIR
 config SH_IRDA
 	tristate "SuperH IrDA driver"
 	depends on IRDA
-	depends on ARCH_SHMOBILE || COMPILE_TEST
+	depends on (ARCH_SHMOBILE || COMPILE_TEST) && HAS_IOMEM
 	help
 	  Say Y here if your want to enable SuperH IrDA devices.
 
diff --git a/drivers/net/irda/vlsi_ir.c b/drivers/net/irda/vlsi_ir.c
index a04af9d0f8f9..a2e556168286 100644
--- a/drivers/net/irda/vlsi_ir.c
+++ b/drivers/net/irda/vlsi_ir.c
@@ -324,12 +324,8 @@ static void vlsi_proc_ring(struct seq_file *seq, struct vlsi_ring *r)
 		seq_printf(seq, "current: rd = %d / status = %02x / len = %u\n",
 				h, (unsigned)rd_get_status(rd), j);
 		if (j > 0) {
-			seq_printf(seq, "   data:");
-			if (j > 20)
-				j = 20;
-			for (i = 0; i < j; i++)
-				seq_printf(seq, " %02x", (unsigned)((unsigned char *)rd->buf)[i]);
-			seq_printf(seq, "\n");
+			seq_printf(seq, "   data: %*ph\n",
+				   min_t(unsigned, j, 20), rd->buf);
 		}
 	}
 	for (i = 0; i < r->size; i++) {
diff --git a/drivers/net/loopback.c b/drivers/net/loopback.c
index 8f2262540561..c76283c2f84a 100644
--- a/drivers/net/loopback.c
+++ b/drivers/net/loopback.c
@@ -169,7 +169,7 @@ static void loopback_setup(struct net_device *dev)
 	dev->type		= ARPHRD_LOOPBACK;	/* 0x0001*/
 	dev->flags		= IFF_LOOPBACK;
 	dev->priv_flags		|= IFF_LIVE_ADDR_CHANGE;
-	dev->priv_flags	       &= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(dev);
 	dev->hw_features	= NETIF_F_ALL_TSO | NETIF_F_UFO;
 	dev->features 		= NETIF_F_SG | NETIF_F_FRAGLIST
 		| NETIF_F_ALL_TSO
diff --git a/drivers/net/macvlan.c b/drivers/net/macvlan.c
index 60e4ca01ccbb..29b3bb410781 100644
--- a/drivers/net/macvlan.c
+++ b/drivers/net/macvlan.c
@@ -35,7 +35,9 @@
 #include <net/xfrm.h>
 #include <linux/netpoll.h>
 
-#define MACVLAN_HASH_SIZE	(1 << BITS_PER_BYTE)
+#define MACVLAN_HASH_BITS	8
+#define MACVLAN_HASH_SIZE	(1<<MACVLAN_HASH_BITS)
+#define MACVLAN_BC_QUEUE_LEN	1000
 
 struct macvlan_port {
 	struct net_device	*dev;
@@ -46,6 +48,14 @@ struct macvlan_port {
 	struct work_struct	bc_work;
 	bool 			passthru;
 	int			count;
+	struct hlist_head	vlan_source_hash[MACVLAN_HASH_SIZE];
+};
+
+struct macvlan_source_entry {
+	struct hlist_node	hlist;
+	struct macvlan_dev	*vlan;
+	unsigned char		addr[6+2] __aligned(sizeof(u16));
+	struct rcu_head		rcu;
 };
 
 struct macvlan_skb_cb {
@@ -56,6 +66,20 @@ struct macvlan_skb_cb {
 
 static void macvlan_port_destroy(struct net_device *dev);
 
+/* Hash Ethernet address */
+static u32 macvlan_eth_hash(const unsigned char *addr)
+{
+	u64 value = get_unaligned((u64 *)addr);
+
+	/* only want 6 bytes */
+#ifdef __BIG_ENDIAN
+	value >>= 16;
+#else
+	value <<= 16;
+#endif
+	return hash_64(value, MACVLAN_HASH_BITS);
+}
+
 static struct macvlan_port *macvlan_port_get_rcu(const struct net_device *dev)
 {
 	return rcu_dereference(dev->rx_handler_data);
@@ -72,20 +96,68 @@ static struct macvlan_dev *macvlan_hash_lookup(const struct macvlan_port *port,
 					       const unsigned char *addr)
 {
 	struct macvlan_dev *vlan;
+	u32 idx = macvlan_eth_hash(addr);
 
-	hlist_for_each_entry_rcu(vlan, &port->vlan_hash[addr[5]], hlist) {
+	hlist_for_each_entry_rcu(vlan, &port->vlan_hash[idx], hlist) {
 		if (ether_addr_equal_64bits(vlan->dev->dev_addr, addr))
 			return vlan;
 	}
 	return NULL;
 }
 
+static struct macvlan_source_entry *macvlan_hash_lookup_source(
+	const struct macvlan_dev *vlan,
+	const unsigned char *addr)
+{
+	struct macvlan_source_entry *entry;
+	u32 idx = macvlan_eth_hash(addr);
+	struct hlist_head *h = &vlan->port->vlan_source_hash[idx];
+
+	hlist_for_each_entry_rcu(entry, h, hlist) {
+		if (ether_addr_equal_64bits(entry->addr, addr) &&
+		    entry->vlan == vlan)
+			return entry;
+	}
+	return NULL;
+}
+
+static int macvlan_hash_add_source(struct macvlan_dev *vlan,
+				   const unsigned char *addr)
+{
+	struct macvlan_port *port = vlan->port;
+	struct macvlan_source_entry *entry;
+	struct hlist_head *h;
+
+	entry = macvlan_hash_lookup_source(vlan, addr);
+	if (entry)
+		return 0;
+
+	entry = kmalloc(sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	ether_addr_copy(entry->addr, addr);
+	entry->vlan = vlan;
+	h = &port->vlan_source_hash[macvlan_eth_hash(addr)];
+	hlist_add_head_rcu(&entry->hlist, h);
+	vlan->macaddr_count++;
+
+	return 0;
+}
+
 static void macvlan_hash_add(struct macvlan_dev *vlan)
 {
 	struct macvlan_port *port = vlan->port;
 	const unsigned char *addr = vlan->dev->dev_addr;
+	u32 idx = macvlan_eth_hash(addr);
+
+	hlist_add_head_rcu(&vlan->hlist, &port->vlan_hash[idx]);
+}
 
-	hlist_add_head_rcu(&vlan->hlist, &port->vlan_hash[addr[5]]);
+static void macvlan_hash_del_source(struct macvlan_source_entry *entry)
+{
+	hlist_del_rcu(&entry->hlist);
+	kfree_rcu(entry, rcu);
 }
 
 static void macvlan_hash_del(struct macvlan_dev *vlan, bool sync)
@@ -188,7 +260,7 @@ static void macvlan_broadcast(struct sk_buff *skb,
 					mode == MACVLAN_MODE_BRIDGE) ?:
 				      netif_rx_ni(nskb);
 			macvlan_count_rx(vlan, skb->len + ETH_HLEN,
-					 err == NET_RX_SUCCESS, 1);
+					 err == NET_RX_SUCCESS, true);
 		}
 	}
 }
@@ -248,7 +320,7 @@ static void macvlan_broadcast_enqueue(struct macvlan_port *port,
 		goto err;
 
 	spin_lock(&port->bc_queue.lock);
-	if (skb_queue_len(&port->bc_queue) < skb->dev->tx_queue_len) {
+	if (skb_queue_len(&port->bc_queue) < MACVLAN_BC_QUEUE_LEN) {
 		__skb_queue_tail(&port->bc_queue, nskb);
 		err = 0;
 	}
@@ -266,6 +338,65 @@ err:
 	atomic_long_inc(&skb->dev->rx_dropped);
 }
 
+static void macvlan_flush_sources(struct macvlan_port *port,
+				  struct macvlan_dev *vlan)
+{
+	int i;
+
+	for (i = 0; i < MACVLAN_HASH_SIZE; i++) {
+		struct hlist_node *h, *n;
+
+		hlist_for_each_safe(h, n, &port->vlan_source_hash[i]) {
+			struct macvlan_source_entry *entry;
+
+			entry = hlist_entry(h, struct macvlan_source_entry,
+					    hlist);
+			if (entry->vlan == vlan)
+				macvlan_hash_del_source(entry);
+		}
+	}
+	vlan->macaddr_count = 0;
+}
+
+static void macvlan_forward_source_one(struct sk_buff *skb,
+				       struct macvlan_dev *vlan)
+{
+	struct sk_buff *nskb;
+	struct net_device *dev;
+	int len;
+	int ret;
+
+	dev = vlan->dev;
+	if (unlikely(!(dev->flags & IFF_UP)))
+		return;
+
+	nskb = skb_clone(skb, GFP_ATOMIC);
+	if (!nskb)
+		return;
+
+	len = nskb->len + ETH_HLEN;
+	nskb->dev = dev;
+	nskb->pkt_type = PACKET_HOST;
+
+	ret = netif_rx(nskb);
+	macvlan_count_rx(vlan, len, ret == NET_RX_SUCCESS, false);
+}
+
+static void macvlan_forward_source(struct sk_buff *skb,
+				   struct macvlan_port *port,
+				   const unsigned char *addr)
+{
+	struct macvlan_source_entry *entry;
+	u32 idx = macvlan_eth_hash(addr);
+	struct hlist_head *h = &port->vlan_source_hash[idx];
+
+	hlist_for_each_entry_rcu(entry, h, hlist) {
+		if (ether_addr_equal_64bits(entry->addr, addr))
+			if (entry->vlan->dev->flags & IFF_UP)
+				macvlan_forward_source_one(skb, entry->vlan);
+	}
+}
+
 /* called under rcu_read_lock() from netif_receive_skb */
 static rx_handler_result_t macvlan_handle_frame(struct sk_buff **pskb)
 {
@@ -276,7 +407,8 @@ static rx_handler_result_t macvlan_handle_frame(struct sk_buff **pskb)
 	const struct macvlan_dev *src;
 	struct net_device *dev;
 	unsigned int len = 0;
-	int ret = NET_RX_DROP;
+	int ret;
+	rx_handler_result_t handle_res;
 
 	port = macvlan_port_get_rcu(skb->dev);
 	if (is_multicast_ether_addr(eth->h_dest)) {
@@ -284,6 +416,7 @@ static rx_handler_result_t macvlan_handle_frame(struct sk_buff **pskb)
 		if (!skb)
 			return RX_HANDLER_CONSUMED;
 		eth = eth_hdr(skb);
+		macvlan_forward_source(skb, port, eth->h_source);
 		src = macvlan_hash_lookup(port, eth->h_source);
 		if (src && src->mode != MACVLAN_MODE_VEPA &&
 		    src->mode != MACVLAN_MODE_BRIDGE) {
@@ -291,6 +424,7 @@ static rx_handler_result_t macvlan_handle_frame(struct sk_buff **pskb)
 			vlan = src;
 			ret = macvlan_broadcast_one(skb, vlan, eth, 0) ?:
 			      netif_rx(skb);
+			handle_res = RX_HANDLER_CONSUMED;
 			goto out;
 		}
 
@@ -300,6 +434,7 @@ static rx_handler_result_t macvlan_handle_frame(struct sk_buff **pskb)
 		return RX_HANDLER_PASS;
 	}
 
+	macvlan_forward_source(skb, port, eth->h_source);
 	if (port->passthru)
 		vlan = list_first_or_null_rcu(&port->vlans,
 					      struct macvlan_dev, list);
@@ -315,17 +450,20 @@ static rx_handler_result_t macvlan_handle_frame(struct sk_buff **pskb)
 	}
 	len = skb->len + ETH_HLEN;
 	skb = skb_share_check(skb, GFP_ATOMIC);
-	if (!skb)
+	if (!skb) {
+		ret = NET_RX_DROP;
+		handle_res = RX_HANDLER_CONSUMED;
 		goto out;
+	}
 
 	skb->dev = dev;
 	skb->pkt_type = PACKET_HOST;
 
-	ret = netif_rx(skb);
-
+	ret = NET_RX_SUCCESS;
+	handle_res = RX_HANDLER_ANOTHER;
 out:
-	macvlan_count_rx(vlan, len, ret == NET_RX_SUCCESS, 0);
-	return RX_HANDLER_CONSUMED;
+	macvlan_count_rx(vlan, len, ret == NET_RX_SUCCESS, false);
+	return handle_res;
 }
 
 static int macvlan_queue_xmit(struct sk_buff *skb, struct net_device *dev)
@@ -666,6 +804,7 @@ static void macvlan_uninit(struct net_device *dev)
 
 	free_percpu(vlan->pcpu_stats);
 
+	macvlan_flush_sources(port, vlan);
 	port->count -= 1;
 	if (!port->count)
 		macvlan_port_destroy(port->dev);
@@ -739,7 +878,10 @@ static int macvlan_fdb_add(struct ndmsg *ndm, struct nlattr *tb[],
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	int err = -EINVAL;
 
-	if (!vlan->port->passthru)
+	/* Support unicast filter only on passthru devices.
+	 * Multicast filter should be allowed on all devices.
+	 */
+	if (!vlan->port->passthru && is_unicast_ether_addr(addr))
 		return -EOPNOTSUPP;
 
 	if (flags & NLM_F_REPLACE)
@@ -760,7 +902,10 @@ static int macvlan_fdb_del(struct ndmsg *ndm, struct nlattr *tb[],
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	int err = -EINVAL;
 
-	if (!vlan->port->passthru)
+	/* Support unicast filter only on passthru devices.
+	 * Multicast filter should be allowed on all devices.
+	 */
+	if (!vlan->port->passthru && is_unicast_ether_addr(addr))
 		return -EOPNOTSUPP;
 
 	if (is_unicast_ether_addr(addr))
@@ -800,6 +945,7 @@ static netdev_features_t macvlan_fix_features(struct net_device *dev,
 					     features,
 					     mask);
 	features |= ALWAYS_ON_FEATURES;
+	features &= ~NETIF_F_NETNS_LOCAL;
 
 	return features;
 }
@@ -884,7 +1030,8 @@ void macvlan_common_setup(struct net_device *dev)
 {
 	ether_setup(dev);
 
-	dev->priv_flags	       &= ~(IFF_XMIT_DST_RELEASE | IFF_TX_SKB_SHARING);
+	dev->priv_flags	       &= ~IFF_TX_SKB_SHARING;
+	netif_keep_dst(dev);
 	dev->priv_flags	       |= IFF_UNICAST_FLT;
 	dev->netdev_ops		= &macvlan_netdev_ops;
 	dev->destructor		= free_netdev;
@@ -917,6 +1064,8 @@ static int macvlan_port_create(struct net_device *dev)
 	INIT_LIST_HEAD(&port->vlans);
 	for (i = 0; i < MACVLAN_HASH_SIZE; i++)
 		INIT_HLIST_HEAD(&port->vlan_hash[i]);
+	for (i = 0; i < MACVLAN_HASH_SIZE; i++)
+		INIT_HLIST_HEAD(&port->vlan_source_hash[i]);
 
 	skb_queue_head_init(&port->bc_queue);
 	INIT_WORK(&port->bc_work, macvlan_process_broadcast);
@@ -958,11 +1107,102 @@ static int macvlan_validate(struct nlattr *tb[], struct nlattr *data[])
 		case MACVLAN_MODE_VEPA:
 		case MACVLAN_MODE_BRIDGE:
 		case MACVLAN_MODE_PASSTHRU:
+		case MACVLAN_MODE_SOURCE:
+			break;
+		default:
+			return -EINVAL;
+		}
+	}
+
+	if (data && data[IFLA_MACVLAN_MACADDR_MODE]) {
+		switch (nla_get_u32(data[IFLA_MACVLAN_MACADDR_MODE])) {
+		case MACVLAN_MACADDR_ADD:
+		case MACVLAN_MACADDR_DEL:
+		case MACVLAN_MACADDR_FLUSH:
+		case MACVLAN_MACADDR_SET:
 			break;
 		default:
 			return -EINVAL;
 		}
 	}
+
+	if (data && data[IFLA_MACVLAN_MACADDR]) {
+		if (nla_len(data[IFLA_MACVLAN_MACADDR]) != ETH_ALEN)
+			return -EINVAL;
+
+		if (!is_valid_ether_addr(nla_data(data[IFLA_MACVLAN_MACADDR])))
+			return -EADDRNOTAVAIL;
+	}
+
+	if (data && data[IFLA_MACVLAN_MACADDR_COUNT])
+		return -EINVAL;
+
+	return 0;
+}
+
+/**
+ * reconfigure list of remote source mac address
+ * (only for macvlan devices in source mode)
+ * Note regarding alignment: all netlink data is aligned to 4 Byte, which
+ * suffices for both ether_addr_copy and ether_addr_equal_64bits usage.
+ */
+static int macvlan_changelink_sources(struct macvlan_dev *vlan, u32 mode,
+				      struct nlattr *data[])
+{
+	char *addr = NULL;
+	int ret, rem, len;
+	struct nlattr *nla, *head;
+	struct macvlan_source_entry *entry;
+
+	if (data[IFLA_MACVLAN_MACADDR])
+		addr = nla_data(data[IFLA_MACVLAN_MACADDR]);
+
+	if (mode == MACVLAN_MACADDR_ADD) {
+		if (!addr)
+			return -EINVAL;
+
+		return macvlan_hash_add_source(vlan, addr);
+
+	} else if (mode == MACVLAN_MACADDR_DEL) {
+		if (!addr)
+			return -EINVAL;
+
+		entry = macvlan_hash_lookup_source(vlan, addr);
+		if (entry) {
+			macvlan_hash_del_source(entry);
+			vlan->macaddr_count--;
+		}
+	} else if (mode == MACVLAN_MACADDR_FLUSH) {
+		macvlan_flush_sources(vlan->port, vlan);
+	} else if (mode == MACVLAN_MACADDR_SET) {
+		macvlan_flush_sources(vlan->port, vlan);
+
+		if (addr) {
+			ret = macvlan_hash_add_source(vlan, addr);
+			if (ret)
+				return ret;
+		}
+
+		if (!data || !data[IFLA_MACVLAN_MACADDR_DATA])
+			return 0;
+
+		head = nla_data(data[IFLA_MACVLAN_MACADDR_DATA]);
+		len = nla_len(data[IFLA_MACVLAN_MACADDR_DATA]);
+
+		nla_for_each_attr(nla, head, len, rem) {
+			if (nla_type(nla) != IFLA_MACVLAN_MACADDR ||
+			    nla_len(nla) != ETH_ALEN)
+				continue;
+
+			addr = nla_data(nla);
+			ret = macvlan_hash_add_source(vlan, addr);
+			if (ret)
+				return ret;
+		}
+	} else {
+		return -EINVAL;
+	}
+
 	return 0;
 }
 
@@ -973,6 +1213,7 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 	struct macvlan_port *port;
 	struct net_device *lowerdev;
 	int err;
+	int macmode;
 
 	if (!tb[IFLA_LINK])
 		return -EINVAL;
@@ -1026,6 +1267,15 @@ int macvlan_common_newlink(struct net *src_net, struct net_device *dev,
 		eth_hw_addr_inherit(dev, lowerdev);
 	}
 
+	if (data && data[IFLA_MACVLAN_MACADDR_MODE]) {
+		if (vlan->mode != MACVLAN_MODE_SOURCE)
+			return -EINVAL;
+		macmode = nla_get_u32(data[IFLA_MACVLAN_MACADDR_MODE]);
+		err = macvlan_changelink_sources(vlan, macmode, data);
+		if (err)
+			return err;
+	}
+
 	port->count += 1;
 	err = register_netdevice(dev);
 	if (err < 0)
@@ -1062,6 +1312,8 @@ void macvlan_dellink(struct net_device *dev, struct list_head *head)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
 
+	if (vlan->mode == MACVLAN_MODE_SOURCE)
+		macvlan_flush_sources(vlan->port, vlan);
 	list_del_rcu(&vlan->list);
 	unregister_netdevice_queue(dev, head);
 	netdev_upper_dev_unlink(vlan->lowerdev, dev);
@@ -1074,6 +1326,8 @@ static int macvlan_changelink(struct net_device *dev,
 	struct macvlan_dev *vlan = netdev_priv(dev);
 	enum macvlan_mode mode;
 	bool set_mode = false;
+	enum macvlan_macaddr_mode macmode;
+	int ret;
 
 	/* Validate mode, but don't set yet: setting flags may fail. */
 	if (data && data[IFLA_MACVLAN_MODE]) {
@@ -1083,6 +1337,9 @@ static int macvlan_changelink(struct net_device *dev,
 		if ((mode == MACVLAN_MODE_PASSTHRU) !=
 		    (vlan->mode == MACVLAN_MODE_PASSTHRU))
 			return -EINVAL;
+		if (vlan->mode == MACVLAN_MODE_SOURCE &&
+		    vlan->mode != mode)
+			macvlan_flush_sources(vlan->port, vlan);
 	}
 
 	if (data && data[IFLA_MACVLAN_FLAGS]) {
@@ -1102,26 +1359,77 @@ static int macvlan_changelink(struct net_device *dev,
 	}
 	if (set_mode)
 		vlan->mode = mode;
+	if (data && data[IFLA_MACVLAN_MACADDR_MODE]) {
+		if (vlan->mode != MACVLAN_MODE_SOURCE)
+			return -EINVAL;
+		macmode = nla_get_u32(data[IFLA_MACVLAN_MACADDR_MODE]);
+		ret = macvlan_changelink_sources(vlan, macmode, data);
+		if (ret)
+			return ret;
+	}
 	return 0;
 }
 
+static size_t macvlan_get_size_mac(const struct macvlan_dev *vlan)
+{
+	if (vlan->macaddr_count == 0)
+		return 0;
+	return nla_total_size(0) /* IFLA_MACVLAN_MACADDR_DATA */
+		+ vlan->macaddr_count * nla_total_size(sizeof(u8) * ETH_ALEN);
+}
+
 static size_t macvlan_get_size(const struct net_device *dev)
 {
+	struct macvlan_dev *vlan = netdev_priv(dev);
+
 	return (0
 		+ nla_total_size(4) /* IFLA_MACVLAN_MODE */
 		+ nla_total_size(2) /* IFLA_MACVLAN_FLAGS */
+		+ nla_total_size(4) /* IFLA_MACVLAN_MACADDR_COUNT */
+		+ macvlan_get_size_mac(vlan) /* IFLA_MACVLAN_MACADDR */
 		);
 }
 
+static int macvlan_fill_info_macaddr(struct sk_buff *skb,
+				     const struct macvlan_dev *vlan,
+				     const int i)
+{
+	struct hlist_head *h = &vlan->port->vlan_source_hash[i];
+	struct macvlan_source_entry *entry;
+
+	hlist_for_each_entry_rcu(entry, h, hlist) {
+		if (entry->vlan != vlan)
+			continue;
+		if (nla_put(skb, IFLA_MACVLAN_MACADDR, ETH_ALEN, entry->addr))
+			return 1;
+	}
+	return 0;
+}
+
 static int macvlan_fill_info(struct sk_buff *skb,
 				const struct net_device *dev)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
+	int i;
+	struct nlattr *nest;
 
 	if (nla_put_u32(skb, IFLA_MACVLAN_MODE, vlan->mode))
 		goto nla_put_failure;
 	if (nla_put_u16(skb, IFLA_MACVLAN_FLAGS, vlan->flags))
 		goto nla_put_failure;
+	if (nla_put_u32(skb, IFLA_MACVLAN_MACADDR_COUNT, vlan->macaddr_count))
+		goto nla_put_failure;
+	if (vlan->macaddr_count > 0) {
+		nest = nla_nest_start(skb, IFLA_MACVLAN_MACADDR_DATA);
+		if (nest == NULL)
+			goto nla_put_failure;
+
+		for (i = 0; i < MACVLAN_HASH_SIZE; i++) {
+			if (macvlan_fill_info_macaddr(skb, vlan, i))
+				goto nla_put_failure;
+		}
+		nla_nest_end(skb, nest);
+	}
 	return 0;
 
 nla_put_failure:
@@ -1131,6 +1439,10 @@ nla_put_failure:
 static const struct nla_policy macvlan_policy[IFLA_MACVLAN_MAX + 1] = {
 	[IFLA_MACVLAN_MODE]  = { .type = NLA_U32 },
 	[IFLA_MACVLAN_FLAGS] = { .type = NLA_U16 },
+	[IFLA_MACVLAN_MACADDR_MODE] = { .type = NLA_U32 },
+	[IFLA_MACVLAN_MACADDR] = { .type = NLA_BINARY, .len = MAX_ADDR_LEN },
+	[IFLA_MACVLAN_MACADDR_DATA] = { .type = NLA_NESTED },
+	[IFLA_MACVLAN_MACADDR_COUNT] = { .type = NLA_U32 },
 };
 
 int macvlan_link_register(struct rtnl_link_ops *ops)
diff --git a/drivers/net/macvtap.c b/drivers/net/macvtap.c
index 3381c4f91a8c..65e2892342bd 100644
--- a/drivers/net/macvtap.c
+++ b/drivers/net/macvtap.c
@@ -112,17 +112,15 @@ out:
 	return err;
 }
 
+/* Requires RTNL */
 static int macvtap_set_queue(struct net_device *dev, struct file *file,
 			     struct macvtap_queue *q)
 {
 	struct macvlan_dev *vlan = netdev_priv(dev);
-	int err = -EBUSY;
 
-	rtnl_lock();
 	if (vlan->numqueues == MAX_MACVTAP_QUEUES)
-		goto out;
+		return -EBUSY;
 
-	err = 0;
 	rcu_assign_pointer(q->vlan, vlan);
 	rcu_assign_pointer(vlan->taps[vlan->numvtaps], q);
 	sock_hold(&q->sk);
@@ -136,9 +134,7 @@ static int macvtap_set_queue(struct net_device *dev, struct file *file,
 	vlan->numvtaps++;
 	vlan->numqueues++;
 
-out:
-	rtnl_unlock();
-	return err;
+	return 0;
 }
 
 static int macvtap_disable_queue(struct macvtap_queue *q)
@@ -302,7 +298,7 @@ static rx_handler_result_t macvtap_handle_frame(struct sk_buff **pskb)
 	 */
 	if (q->flags & IFF_VNET_HDR)
 		features |= vlan->tap_features;
-	if (netif_needs_gso(skb, features)) {
+	if (netif_needs_gso(dev, skb, features)) {
 		struct sk_buff *segs = __skb_gso_segment(skb, features, false);
 
 		if (IS_ERR(segs))
@@ -454,11 +450,12 @@ static void macvtap_sock_destruct(struct sock *sk)
 static int macvtap_open(struct inode *inode, struct file *file)
 {
 	struct net *net = current->nsproxy->net_ns;
-	struct net_device *dev = dev_get_by_macvtap_minor(iminor(inode));
+	struct net_device *dev;
 	struct macvtap_queue *q;
-	int err;
+	int err = -ENODEV;
 
-	err = -ENODEV;
+	rtnl_lock();
+	dev = dev_get_by_macvtap_minor(iminor(inode));
 	if (!dev)
 		goto out;
 
@@ -498,6 +495,7 @@ out:
 	if (dev)
 		dev_put(dev);
 
+	rtnl_unlock();
 	return err;
 }
 
diff --git a/drivers/net/phy/Kconfig b/drivers/net/phy/Kconfig
index 65de0cab8d07..75472cf734de 100644
--- a/drivers/net/phy/Kconfig
+++ b/drivers/net/phy/Kconfig
@@ -159,8 +159,6 @@ config MDIO_OCTEON
 config MDIO_SUN4I
 	tristate "Allwinner sun4i MDIO interface support"
 	depends on ARCH_SUNXI
-	select REGULATOR
-	select REGULATOR_FIXED_VOLTAGE
 	help
 	  This driver supports the MDIO interface found in the network
 	  interface units of the Allwinner SoC that have an EMAC (A10,
@@ -205,6 +203,15 @@ config MDIO_BUS_MUX_MMIOREG
 
 	  Currently, only 8-bit registers are supported.
 
+config MDIO_BCM_UNIMAC
+	tristate "Broadcom UniMAC MDIO bus controller"
+	depends on HAS_IOMEM
+	help
+	  This module provides a driver for the Broadcom UniMAC MDIO busses.
+	  This hardware can be found in the Broadcom GENET Ethernet MAC
+	  controllers as well as some Broadcom Ethernet switches such as the
+	  Starfighter 2 switches.
+
 endif # PHYLIB
 
 config MICREL_KS8995MA
diff --git a/drivers/net/phy/Makefile b/drivers/net/phy/Makefile
index 7dc3d5b304cf..eb3b18b5978b 100644
--- a/drivers/net/phy/Makefile
+++ b/drivers/net/phy/Makefile
@@ -34,3 +34,4 @@ obj-$(CONFIG_MDIO_BUS_MUX_MMIOREG) += mdio-mux-mmioreg.o
 obj-$(CONFIG_MDIO_SUN4I)	+= mdio-sun4i.o
 obj-$(CONFIG_MDIO_MOXART)	+= mdio-moxart.o
 obj-$(CONFIG_AMD_XGBE_PHY)	+= amd-xgbe-phy.o
+obj-$(CONFIG_MDIO_BCM_UNIMAC)	+= mdio-bcm-unimac.o
diff --git a/drivers/net/phy/amd-xgbe-phy.c b/drivers/net/phy/amd-xgbe-phy.c
index f3230eef41fd..c456559f6e7f 100644
--- a/drivers/net/phy/amd-xgbe-phy.c
+++ b/drivers/net/phy/amd-xgbe-phy.c
@@ -75,7 +75,6 @@
 #include <linux/of_device.h>
 #include <linux/uaccess.h>
 
-
 MODULE_AUTHOR("Tom Lendacky <thomas.lendacky@amd.com>");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION("1.0.0-a");
@@ -100,9 +99,11 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #ifndef MDIO_PMA_10GBR_PMD_CTRL
 #define MDIO_PMA_10GBR_PMD_CTRL		0x0096
 #endif
+
 #ifndef MDIO_PMA_10GBR_FEC_CTRL
 #define MDIO_PMA_10GBR_FEC_CTRL		0x00ab
 #endif
+
 #ifndef MDIO_AN_XNP
 #define MDIO_AN_XNP			0x0016
 #endif
@@ -110,14 +111,23 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #ifndef MDIO_AN_INTMASK
 #define MDIO_AN_INTMASK			0x8001
 #endif
+
 #ifndef MDIO_AN_INT
 #define MDIO_AN_INT			0x8002
 #endif
 
+#ifndef MDIO_AN_KR_CTRL
+#define MDIO_AN_KR_CTRL			0x8003
+#endif
+
 #ifndef MDIO_CTRL1_SPEED1G
 #define MDIO_CTRL1_SPEED1G		(MDIO_CTRL1_SPEED10G & ~BMCR_SPEED100)
 #endif
 
+#ifndef MDIO_KR_CTRL_PDETECT
+#define MDIO_KR_CTRL_PDETECT		0x01
+#endif
+
 /* SerDes integration register offsets */
 #define SIR0_KR_RT_1			0x002c
 #define SIR0_STATUS			0x0040
@@ -161,7 +171,6 @@ MODULE_DESCRIPTION("AMD 10GbE (amd-xgbe) PHY driver");
 #define SPEED_1000_TXAMP		0xf
 #define SPEED_1000_WORD			0x1
 
-
 /* SerDes RxTx register offsets */
 #define RXTX_REG20			0x0050
 #define RXTX_REG114			0x01c8
@@ -255,7 +264,6 @@ do {									\
 	XSIR1_IOWRITE((_priv), _reg, reg_val);				\
 } while (0)
 
-
 /* Macros for reading or writing SerDes RxTx registers
  *  The ioread macros will get bit fields or full values using the
  *  register definitions formed using the input names
@@ -283,7 +291,6 @@ do {									\
 	XRXTX_IOWRITE((_priv), _reg, reg_val);				\
 } while (0)
 
-
 enum amd_xgbe_phy_an {
 	AMD_XGBE_AN_READY = 0,
 	AMD_XGBE_AN_START,
@@ -331,7 +338,6 @@ struct amd_xgbe_phy_priv {
 
 	/* Maintain link status for re-starting auto-negotiation */
 	unsigned int link;
-	enum amd_xgbe_phy_mode mode;
 	unsigned int speed_set;
 
 	/* Auto-negotiation state machine support */
@@ -342,6 +348,7 @@ struct amd_xgbe_phy_priv {
 	enum amd_xgbe_phy_rx kx_state;
 	struct work_struct an_work;
 	struct workqueue_struct *an_workqueue;
+	unsigned int parallel_detect;
 };
 
 static int amd_xgbe_an_enable_kr_training(struct phy_device *phydev)
@@ -468,8 +475,6 @@ static int amd_xgbe_phy_xgmii_mode(struct phy_device *phydev)
 
 	amd_xgbe_phy_serdes_complete_ratechange(phydev);
 
-	priv->mode = AMD_XGBE_MODE_KR;
-
 	return 0;
 }
 
@@ -518,8 +523,6 @@ static int amd_xgbe_phy_gmii_2500_mode(struct phy_device *phydev)
 
 	amd_xgbe_phy_serdes_complete_ratechange(phydev);
 
-	priv->mode = AMD_XGBE_MODE_KX;
-
 	return 0;
 }
 
@@ -568,18 +571,43 @@ static int amd_xgbe_phy_gmii_mode(struct phy_device *phydev)
 
 	amd_xgbe_phy_serdes_complete_ratechange(phydev);
 
-	priv->mode = AMD_XGBE_MODE_KX;
+	return 0;
+}
+
+static int amd_xgbe_phy_cur_mode(struct phy_device *phydev,
+				 enum amd_xgbe_phy_mode *mode)
+{
+	int ret;
+
+	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
+	if (ret < 0)
+		return ret;
+
+	if ((ret & MDIO_PCS_CTRL2_TYPE) == MDIO_PCS_CTRL2_10GBR)
+		*mode = AMD_XGBE_MODE_KR;
+	else
+		*mode = AMD_XGBE_MODE_KX;
 
 	return 0;
 }
 
+static bool amd_xgbe_phy_in_kr_mode(struct phy_device *phydev)
+{
+	enum amd_xgbe_phy_mode mode;
+
+	if (amd_xgbe_phy_cur_mode(phydev, &mode))
+		return false;
+
+	return (mode == AMD_XGBE_MODE_KR);
+}
+
 static int amd_xgbe_phy_switch_mode(struct phy_device *phydev)
 {
 	struct amd_xgbe_phy_priv *priv = phydev->priv;
 	int ret;
 
 	/* If we are in KR switch to KX, and vice-versa */
-	if (priv->mode == AMD_XGBE_MODE_KR) {
+	if (amd_xgbe_phy_in_kr_mode(phydev)) {
 		if (priv->speed_set == AMD_XGBE_PHY_SPEEDSET_1000_10000)
 			ret = amd_xgbe_phy_gmii_mode(phydev);
 		else
@@ -591,15 +619,20 @@ static int amd_xgbe_phy_switch_mode(struct phy_device *phydev)
 	return ret;
 }
 
-static enum amd_xgbe_phy_an amd_xgbe_an_switch_mode(struct phy_device *phydev)
+static int amd_xgbe_phy_set_mode(struct phy_device *phydev,
+				 enum amd_xgbe_phy_mode mode)
 {
+	enum amd_xgbe_phy_mode cur_mode;
 	int ret;
 
-	ret = amd_xgbe_phy_switch_mode(phydev);
-	if (ret < 0)
-		return AMD_XGBE_AN_ERROR;
+	ret = amd_xgbe_phy_cur_mode(phydev, &cur_mode);
+	if (ret)
+		return ret;
 
-	return AMD_XGBE_AN_START;
+	if (mode != cur_mode)
+		ret = amd_xgbe_phy_switch_mode(phydev);
+
+	return ret;
 }
 
 static enum amd_xgbe_phy_an amd_xgbe_an_tx_training(struct phy_device *phydev,
@@ -610,8 +643,8 @@ static enum amd_xgbe_phy_an amd_xgbe_an_tx_training(struct phy_device *phydev,
 
 	*state = AMD_XGBE_RX_COMPLETE;
 
-	/* If we're in KX mode then we're done */
-	if (priv->mode == AMD_XGBE_MODE_KX)
+	/* If we're not in KR mode then we're done */
+	if (!amd_xgbe_phy_in_kr_mode(phydev))
 		return AMD_XGBE_AN_EVENT;
 
 	/* Enable/Disable FEC */
@@ -669,7 +702,6 @@ static enum amd_xgbe_phy_an amd_xgbe_an_tx_xnp(struct phy_device *phydev,
 static enum amd_xgbe_phy_an amd_xgbe_an_rx_bpa(struct phy_device *phydev,
 					       enum amd_xgbe_phy_rx *state)
 {
-	struct amd_xgbe_phy_priv *priv = phydev->priv;
 	unsigned int link_support;
 	int ret, ad_reg, lp_reg;
 
@@ -679,9 +711,9 @@ static enum amd_xgbe_phy_an amd_xgbe_an_rx_bpa(struct phy_device *phydev,
 		return AMD_XGBE_AN_ERROR;
 
 	/* Check for a supported mode, otherwise restart in a different one */
-	link_support = (priv->mode == AMD_XGBE_MODE_KR) ? 0x80 : 0x20;
+	link_support = amd_xgbe_phy_in_kr_mode(phydev) ? 0x80 : 0x20;
 	if (!(ret & link_support))
-		return amd_xgbe_an_switch_mode(phydev);
+		return AMD_XGBE_AN_INCOMPAT_LINK;
 
 	/* Check Extended Next Page support */
 	ad_reg = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_ADVERTISE);
@@ -722,7 +754,7 @@ static enum amd_xgbe_phy_an amd_xgbe_an_start(struct phy_device *phydev)
 	int ret;
 
 	/* Be sure we aren't looping trying to negotiate */
-	if (priv->mode == AMD_XGBE_MODE_KR) {
+	if (amd_xgbe_phy_in_kr_mode(phydev)) {
 		if (priv->kr_state != AMD_XGBE_RX_READY)
 			return AMD_XGBE_AN_NO_LINK;
 		priv->kr_state = AMD_XGBE_RX_BPA;
@@ -785,6 +817,13 @@ static enum amd_xgbe_phy_an amd_xgbe_an_start(struct phy_device *phydev)
 	/* Enable and start auto-negotiation */
 	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_INT, 0);
 
+	ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_AN_KR_CTRL);
+	if (ret < 0)
+		return AMD_XGBE_AN_ERROR;
+
+	ret |= MDIO_KR_CTRL_PDETECT;
+	phy_write_mmd(phydev, MDIO_MMD_AN, MDIO_AN_KR_CTRL, ret);
+
 	ret = phy_read_mmd(phydev, MDIO_MMD_AN, MDIO_CTRL1);
 	if (ret < 0)
 		return AMD_XGBE_AN_ERROR;
@@ -825,8 +864,8 @@ static enum amd_xgbe_phy_an amd_xgbe_an_page_received(struct phy_device *phydev)
 	enum amd_xgbe_phy_rx *state;
 	int ret;
 
-	state = (priv->mode == AMD_XGBE_MODE_KR) ? &priv->kr_state
-						 : &priv->kx_state;
+	state = amd_xgbe_phy_in_kr_mode(phydev) ? &priv->kr_state
+						: &priv->kx_state;
 
 	switch (*state) {
 	case AMD_XGBE_RX_BPA:
@@ -846,7 +885,13 @@ static enum amd_xgbe_phy_an amd_xgbe_an_page_received(struct phy_device *phydev)
 
 static enum amd_xgbe_phy_an amd_xgbe_an_incompat_link(struct phy_device *phydev)
 {
-	return amd_xgbe_an_switch_mode(phydev);
+	int ret;
+
+	ret = amd_xgbe_phy_switch_mode(phydev);
+	if (ret)
+		return AMD_XGBE_AN_ERROR;
+
+	return AMD_XGBE_AN_START;
 }
 
 static void amd_xgbe_an_state_machine(struct work_struct *work)
@@ -859,6 +904,10 @@ static void amd_xgbe_an_state_machine(struct work_struct *work)
 	int sleep;
 	unsigned int an_supported = 0;
 
+	/* Start in KX mode */
+	if (amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KX))
+		priv->an_state = AMD_XGBE_AN_ERROR;
+
 	while (1) {
 		mutex_lock(&priv->an_mutex);
 
@@ -866,8 +915,9 @@ static void amd_xgbe_an_state_machine(struct work_struct *work)
 
 		switch (priv->an_state) {
 		case AMD_XGBE_AN_START:
-			priv->an_state = amd_xgbe_an_start(phydev);
 			an_supported = 0;
+			priv->parallel_detect = 0;
+			priv->an_state = amd_xgbe_an_start(phydev);
 			break;
 
 		case AMD_XGBE_AN_EVENT:
@@ -884,6 +934,7 @@ static void amd_xgbe_an_state_machine(struct work_struct *work)
 			break;
 
 		case AMD_XGBE_AN_COMPLETE:
+			priv->parallel_detect = an_supported ? 0 : 1;
 			netdev_info(phydev->attached_dev, "%s successful\n",
 				    an_supported ? "Auto negotiation"
 						 : "Parallel detection");
@@ -1018,7 +1069,6 @@ static int amd_xgbe_phy_config_aneg(struct phy_device *phydev)
 {
 	struct amd_xgbe_phy_priv *priv = phydev->priv;
 	u32 mmd_mask = phydev->c45_ids.devices_in_package;
-	int ret;
 
 	if (phydev->autoneg != AUTONEG_ENABLE)
 		return amd_xgbe_phy_setup_forced(phydev);
@@ -1027,11 +1077,6 @@ static int amd_xgbe_phy_config_aneg(struct phy_device *phydev)
 	if (!(mmd_mask & MDIO_DEVS_AN))
 		return -EINVAL;
 
-	/* Get the current speed mode */
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
-	if (ret < 0)
-		return ret;
-
 	/* Start/Restart the auto-negotiation state machine */
 	mutex_lock(&priv->an_mutex);
 	priv->an_result = AMD_XGBE_AN_READY;
@@ -1121,18 +1166,14 @@ static int amd_xgbe_phy_read_status(struct phy_device *phydev)
 {
 	struct amd_xgbe_phy_priv *priv = phydev->priv;
 	u32 mmd_mask = phydev->c45_ids.devices_in_package;
-	int ret, mode, ad_ret, lp_ret;
+	int ret, ad_ret, lp_ret;
 
 	ret = amd_xgbe_phy_update_link(phydev);
 	if (ret)
 		return ret;
 
-	mode = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
-	if (mode < 0)
-		return mode;
-	mode &= MDIO_PCS_CTRL2_TYPE;
-
-	if (phydev->autoneg == AUTONEG_ENABLE) {
+	if ((phydev->autoneg == AUTONEG_ENABLE) &&
+	    !priv->parallel_detect) {
 		if (!(mmd_mask & MDIO_DEVS_AN))
 			return -EINVAL;
 
@@ -1163,40 +1204,39 @@ static int amd_xgbe_phy_read_status(struct phy_device *phydev)
 		ad_ret &= lp_ret;
 		if (ad_ret & 0x80) {
 			phydev->speed = SPEED_10000;
-			if (mode != MDIO_PCS_CTRL2_10GBR) {
-				ret = amd_xgbe_phy_xgmii_mode(phydev);
-				if (ret < 0)
-					return ret;
-			}
+			ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KR);
+			if (ret)
+				return ret;
 		} else {
-			int (*mode_fcn)(struct phy_device *);
-
-			if (priv->speed_set ==
-			    AMD_XGBE_PHY_SPEEDSET_1000_10000) {
+			switch (priv->speed_set) {
+			case AMD_XGBE_PHY_SPEEDSET_1000_10000:
 				phydev->speed = SPEED_1000;
-				mode_fcn = amd_xgbe_phy_gmii_mode;
-			} else {
+				break;
+
+			case AMD_XGBE_PHY_SPEEDSET_2500_10000:
 				phydev->speed = SPEED_2500;
-				mode_fcn = amd_xgbe_phy_gmii_2500_mode;
+				break;
 			}
 
-			if (mode == MDIO_PCS_CTRL2_10GBR) {
-				ret = mode_fcn(phydev);
-				if (ret < 0)
-					return ret;
-			}
+			ret = amd_xgbe_phy_set_mode(phydev, AMD_XGBE_MODE_KX);
+			if (ret)
+				return ret;
 		}
 
 		phydev->duplex = DUPLEX_FULL;
 	} else {
-		if (mode == MDIO_PCS_CTRL2_10GBR) {
+		if (amd_xgbe_phy_in_kr_mode(phydev)) {
 			phydev->speed = SPEED_10000;
 		} else {
-			if (priv->speed_set ==
-			    AMD_XGBE_PHY_SPEEDSET_1000_10000)
+			switch (priv->speed_set) {
+			case AMD_XGBE_PHY_SPEEDSET_1000_10000:
 				phydev->speed = SPEED_1000;
-			else
+				break;
+
+			case AMD_XGBE_PHY_SPEEDSET_2500_10000:
 				phydev->speed = SPEED_2500;
+				break;
+			}
 		}
 		phydev->duplex = DUPLEX_FULL;
 		phydev->pause = 0;
@@ -1329,14 +1369,6 @@ static int amd_xgbe_phy_probe(struct phy_device *phydev)
 
 	priv->link = 1;
 
-	ret = phy_read_mmd(phydev, MDIO_MMD_PCS, MDIO_CTRL2);
-	if (ret < 0)
-		goto err_sir1;
-	if ((ret & MDIO_PCS_CTRL2_TYPE) == MDIO_PCS_CTRL2_10GBR)
-		priv->mode = AMD_XGBE_MODE_KR;
-	else
-		priv->mode = AMD_XGBE_MODE_KX;
-
 	mutex_init(&priv->an_mutex);
 	INIT_WORK(&priv->an_work, amd_xgbe_an_state_machine);
 	priv->an_workqueue = create_singlethread_workqueue(wq_name);
diff --git a/drivers/net/phy/bcm7xxx.c b/drivers/net/phy/bcm7xxx.c
index 526b94cea569..1d211d369039 100644
--- a/drivers/net/phy/bcm7xxx.c
+++ b/drivers/net/phy/bcm7xxx.c
@@ -14,6 +14,7 @@
 #include <linux/delay.h>
 #include <linux/bitops.h>
 #include <linux/brcmphy.h>
+#include <linux/mdio.h>
 
 /* Broadcom BCM7xxx internal PHY registers */
 #define MII_BCM7XXX_CHANNEL_WIDTH	0x2000
@@ -146,15 +147,96 @@ static int bcm7xxx_28nm_afe_config_init(struct phy_device *phydev)
 	return 0;
 }
 
+static int bcm7xxx_apd_enable(struct phy_device *phydev)
+{
+	int val;
+
+	/* Enable powering down of the DLL during auto-power down */
+	val = bcm54xx_shadow_read(phydev, BCM54XX_SHD_SCR3);
+	if (val < 0)
+		return val;
+
+	val |= BCM54XX_SHD_SCR3_DLLAPD_DIS;
+	bcm54xx_shadow_write(phydev, BCM54XX_SHD_SCR3, val);
+
+	/* Enable auto-power down */
+	val = bcm54xx_shadow_read(phydev, BCM54XX_SHD_APD);
+	if (val < 0)
+		return val;
+
+	val |= BCM54XX_SHD_APD_EN;
+	return bcm54xx_shadow_write(phydev, BCM54XX_SHD_APD, val);
+}
+
+static int bcm7xxx_eee_enable(struct phy_device *phydev)
+{
+	int val;
+
+	val = phy_read_mmd_indirect(phydev, BRCM_CL45VEN_EEE_CONTROL,
+				    MDIO_MMD_AN, phydev->addr);
+	if (val < 0)
+		return val;
+
+	/* Enable general EEE feature at the PHY level */
+	val |= LPI_FEATURE_EN | LPI_FEATURE_EN_DIG1000X;
+
+	phy_write_mmd_indirect(phydev, BRCM_CL45VEN_EEE_CONTROL,
+			       MDIO_MMD_AN, phydev->addr, val);
+
+	/* Advertise supported modes */
+	val = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_ADV,
+				    MDIO_MMD_AN, phydev->addr);
+
+	val |= (MDIO_AN_EEE_ADV_100TX | MDIO_AN_EEE_ADV_1000T);
+	phy_write_mmd_indirect(phydev, MDIO_AN_EEE_ADV,
+			       MDIO_MMD_AN, phydev->addr, val);
+
+	return 0;
+}
+
 static int bcm7xxx_28nm_config_init(struct phy_device *phydev)
 {
+	u8 rev = PHY_BRCM_7XXX_REV(phydev->dev_flags);
+	u8 patch = PHY_BRCM_7XXX_PATCH(phydev->dev_flags);
+	int ret = 0;
+
+	dev_info(&phydev->dev, "PHY revision: 0x%02x, patch: %d\n", rev, patch);
+
+	switch (rev) {
+	case 0xa0:
+	case 0xb0:
+		ret = bcm7445_config_init(phydev);
+		break;
+	default:
+		ret = bcm7xxx_28nm_afe_config_init(phydev);
+		break;
+	}
+
+	if (ret)
+		return ret;
+
+	ret = bcm7xxx_eee_enable(phydev);
+	if (ret)
+		return ret;
+
+	return bcm7xxx_apd_enable(phydev);
+}
+
+static int bcm7xxx_28nm_resume(struct phy_device *phydev)
+{
 	int ret;
 
-	ret = bcm7445_config_init(phydev);
+	/* Re-apply workarounds coming out suspend/resume */
+	ret = bcm7xxx_28nm_config_init(phydev);
 	if (ret)
 		return ret;
 
-	return bcm7xxx_28nm_afe_config_init(phydev);
+	/* 28nm Gigabit PHYs come out of reset without any half-duplex
+	 * or "hub" compliant advertised mode, fix that. This does not
+	 * cause any problems with the PHY library since genphy_config_aneg()
+	 * gracefully handles auto-negotiated and forced modes.
+	 */
+	return genphy_config_aneg(phydev);
 }
 
 static int phy_set_clr_bits(struct phy_device *dev, int location,
@@ -184,8 +266,8 @@ static int bcm7xxx_config_init(struct phy_device *phydev)
 	phy_write(phydev, MII_BCM7XXX_AUX_MODE, MII_BCM7XX_64CLK_MDIO);
 	phy_read(phydev, MII_BCM7XXX_AUX_MODE);
 
-	/* Workaround only required for 100Mbits/sec */
-	if (!(phydev->dev_flags & PHY_BRCM_100MBPS_WAR))
+	/* Workaround only required for 100Mbits/sec capable PHYs */
+	if (phydev->supported & PHY_GBIT_FEATURES)
 		return 0;
 
 	/* set shadow mode 2 */
@@ -212,7 +294,7 @@ static int bcm7xxx_config_init(struct phy_device *phydev)
 }
 
 /* Workaround for putting the PHY in IDDQ mode, required
- * for all BCM7XXX PHYs
+ * for all BCM7XXX 40nm and 65nm PHYs
  */
 static int bcm7xxx_suspend(struct phy_device *phydev)
 {
@@ -246,59 +328,53 @@ static int bcm7xxx_dummy_config_init(struct phy_device *phydev)
 	return 0;
 }
 
+#define BCM7XXX_28NM_GPHY(_oui, _name)					\
+{									\
+	.phy_id		= (_oui),					\
+	.phy_id_mask	= 0xfffffff0,					\
+	.name		= _name,					\
+	.features	= PHY_GBIT_FEATURES |				\
+			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,	\
+	.flags		= PHY_IS_INTERNAL,				\
+	.config_init	= bcm7xxx_28nm_afe_config_init,			\
+	.config_aneg	= genphy_config_aneg,				\
+	.read_status	= genphy_read_status,				\
+	.resume		= bcm7xxx_28nm_resume,				\
+	.driver		= { .owner = THIS_MODULE },			\
+}
+
 static struct phy_driver bcm7xxx_driver[] = {
+	BCM7XXX_28NM_GPHY(PHY_ID_BCM7250, "Broadcom BCM7250"),
+	BCM7XXX_28NM_GPHY(PHY_ID_BCM7364, "Broadcom BCM7364"),
+	BCM7XXX_28NM_GPHY(PHY_ID_BCM7366, "Broadcom BCM7366"),
+	BCM7XXX_28NM_GPHY(PHY_ID_BCM7439, "Broadcom BCM7439"),
+	BCM7XXX_28NM_GPHY(PHY_ID_BCM7445, "Broadcom BCM7445"),
 {
-	.phy_id		= PHY_ID_BCM7366,
-	.phy_id_mask	= 0xfffffff0,
-	.name		= "Broadcom BCM7366",
-	.features	= PHY_GBIT_FEATURES |
+	.phy_id         = PHY_ID_BCM7425,
+	.phy_id_mask    = 0xfffffff0,
+	.name           = "Broadcom BCM7425",
+	.features       = PHY_GBIT_FEATURES |
 			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
-	.flags		= PHY_IS_INTERNAL,
-	.config_init	= bcm7xxx_28nm_afe_config_init,
-	.config_aneg	= genphy_config_aneg,
-	.read_status	= genphy_read_status,
-	.suspend	= bcm7xxx_suspend,
-	.resume		= bcm7xxx_28nm_afe_config_init,
-	.driver		= { .owner = THIS_MODULE },
+	.flags          = 0,
+	.config_init    = bcm7xxx_config_init,
+	.config_aneg    = genphy_config_aneg,
+	.read_status    = genphy_read_status,
+	.suspend        = bcm7xxx_suspend,
+	.resume         = bcm7xxx_config_init,
+	.driver         = { .owner = THIS_MODULE },
 }, {
-	.phy_id		= PHY_ID_BCM7439,
-	.phy_id_mask	= 0xfffffff0,
-	.name		= "Broadcom BCM7439",
-	.features	= PHY_GBIT_FEATURES |
+	.phy_id         = PHY_ID_BCM7429,
+	.phy_id_mask    = 0xfffffff0,
+	.name           = "Broadcom BCM7429",
+	.features       = PHY_GBIT_FEATURES |
 			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
-	.flags		= PHY_IS_INTERNAL,
-	.config_init	= bcm7xxx_28nm_afe_config_init,
-	.config_aneg	= genphy_config_aneg,
-	.read_status	= genphy_read_status,
-	.suspend	= bcm7xxx_suspend,
-	.resume		= bcm7xxx_28nm_afe_config_init,
-	.driver		= { .owner = THIS_MODULE },
-}, {
-	.phy_id		= PHY_ID_BCM7445,
-	.phy_id_mask	= 0xfffffff0,
-	.name		= "Broadcom BCM7445",
-	.features	= PHY_GBIT_FEATURES |
-			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
-	.flags		= PHY_IS_INTERNAL,
-	.config_init	= bcm7xxx_28nm_config_init,
-	.config_aneg	= genphy_config_aneg,
-	.read_status	= genphy_read_status,
-	.suspend	= bcm7xxx_suspend,
-	.resume		= bcm7xxx_28nm_config_init,
-	.driver		= { .owner = THIS_MODULE },
-}, {
-	.name		= "Broadcom BCM7XXX 28nm",
-	.phy_id		= PHY_ID_BCM7XXX_28,
-	.phy_id_mask	= PHY_BCM_OUI_MASK,
-	.features	= PHY_GBIT_FEATURES |
-			  SUPPORTED_Pause | SUPPORTED_Asym_Pause,
-	.flags		= PHY_IS_INTERNAL,
-	.config_init	= bcm7xxx_28nm_config_init,
-	.config_aneg	= genphy_config_aneg,
-	.read_status	= genphy_read_status,
-	.suspend	= bcm7xxx_suspend,
-	.resume		= bcm7xxx_28nm_config_init,
-	.driver		= { .owner = THIS_MODULE },
+	.flags          = PHY_IS_INTERNAL,
+	.config_init    = bcm7xxx_config_init,
+	.config_aneg    = genphy_config_aneg,
+	.read_status    = genphy_read_status,
+	.suspend        = bcm7xxx_suspend,
+	.resume         = bcm7xxx_config_init,
+	.driver         = { .owner = THIS_MODULE },
 }, {
 	.phy_id		= PHY_BCM_OUI_4,
 	.phy_id_mask	= 0xffff0000,
@@ -328,10 +404,13 @@ static struct phy_driver bcm7xxx_driver[] = {
 } };
 
 static struct mdio_device_id __maybe_unused bcm7xxx_tbl[] = {
+	{ PHY_ID_BCM7250, 0xfffffff0, },
+	{ PHY_ID_BCM7364, 0xfffffff0, },
 	{ PHY_ID_BCM7366, 0xfffffff0, },
+	{ PHY_ID_BCM7425, 0xfffffff0, },
+	{ PHY_ID_BCM7429, 0xfffffff0, },
 	{ PHY_ID_BCM7439, 0xfffffff0, },
 	{ PHY_ID_BCM7445, 0xfffffff0, },
-	{ PHY_ID_BCM7XXX_28, 0xfffffc00 },
 	{ PHY_BCM_OUI_4, 0xffff0000 },
 	{ PHY_BCM_OUI_5, 0xffffff00 },
 	{ }
diff --git a/drivers/net/phy/broadcom.c b/drivers/net/phy/broadcom.c
index 34088d60da74..854f2c9a7b2b 100644
--- a/drivers/net/phy/broadcom.c
+++ b/drivers/net/phy/broadcom.c
@@ -25,132 +25,10 @@
 #define BRCM_PHY_REV(phydev) \
 	((phydev)->drv->phy_id & ~((phydev)->drv->phy_id_mask))
 
-/*
- * Broadcom LED source encodings.  These are used in BCM5461, BCM5481,
- * BCM5482, and possibly some others.
- */
-#define BCM_LED_SRC_LINKSPD1	0x0
-#define BCM_LED_SRC_LINKSPD2	0x1
-#define BCM_LED_SRC_XMITLED	0x2
-#define BCM_LED_SRC_ACTIVITYLED	0x3
-#define BCM_LED_SRC_FDXLED	0x4
-#define BCM_LED_SRC_SLAVE	0x5
-#define BCM_LED_SRC_INTR	0x6
-#define BCM_LED_SRC_QUALITY	0x7
-#define BCM_LED_SRC_RCVLED	0x8
-#define BCM_LED_SRC_MULTICOLOR1	0xa
-#define BCM_LED_SRC_OPENSHORT	0xb
-#define BCM_LED_SRC_OFF		0xe	/* Tied high */
-#define BCM_LED_SRC_ON		0xf	/* Tied low */
-
-
-/*
- * BCM5482: Shadow registers
- * Shadow values go into bits [14:10] of register 0x1c to select a shadow
- * register to access.
- */
-/* 00101: Spare Control Register 3 */
-#define BCM54XX_SHD_SCR3		0x05
-#define  BCM54XX_SHD_SCR3_DEF_CLK125	0x0001
-#define  BCM54XX_SHD_SCR3_DLLAPD_DIS	0x0002
-#define  BCM54XX_SHD_SCR3_TRDDAPD	0x0004
-
-/* 01010: Auto Power-Down */
-#define BCM54XX_SHD_APD			0x0a
-#define  BCM54XX_SHD_APD_EN		0x0020
-
-#define BCM5482_SHD_LEDS1	0x0d	/* 01101: LED Selector 1 */
-					/* LED3 / ~LINKSPD[2] selector */
-#define BCM5482_SHD_LEDS1_LED3(src)	((src & 0xf) << 4)
-					/* LED1 / ~LINKSPD[1] selector */
-#define BCM5482_SHD_LEDS1_LED1(src)	((src & 0xf) << 0)
-#define BCM54XX_SHD_RGMII_MODE	0x0b	/* 01011: RGMII Mode Selector */
-#define BCM5482_SHD_SSD		0x14	/* 10100: Secondary SerDes control */
-#define BCM5482_SHD_SSD_LEDM	0x0008	/* SSD LED Mode enable */
-#define BCM5482_SHD_SSD_EN	0x0001	/* SSD enable */
-#define BCM5482_SHD_MODE	0x1f	/* 11111: Mode Control Register */
-#define BCM5482_SHD_MODE_1000BX	0x0001	/* Enable 1000BASE-X registers */
-
-
-/*
- * EXPANSION SHADOW ACCESS REGISTERS.  (PHY REG 0x15, 0x16, and 0x17)
- */
-#define MII_BCM54XX_EXP_AADJ1CH0		0x001f
-#define  MII_BCM54XX_EXP_AADJ1CH0_SWP_ABCD_OEN	0x0200
-#define  MII_BCM54XX_EXP_AADJ1CH0_SWSEL_THPF	0x0100
-#define MII_BCM54XX_EXP_AADJ1CH3		0x601f
-#define  MII_BCM54XX_EXP_AADJ1CH3_ADCCKADJ	0x0002
-#define MII_BCM54XX_EXP_EXP08			0x0F08
-#define  MII_BCM54XX_EXP_EXP08_RJCT_2MHZ	0x0001
-#define  MII_BCM54XX_EXP_EXP08_EARLY_DAC_WAKE	0x0200
-#define MII_BCM54XX_EXP_EXP75			0x0f75
-#define  MII_BCM54XX_EXP_EXP75_VDACCTRL		0x003c
-#define  MII_BCM54XX_EXP_EXP75_CM_OSC		0x0001
-#define MII_BCM54XX_EXP_EXP96			0x0f96
-#define  MII_BCM54XX_EXP_EXP96_MYST		0x0010
-#define MII_BCM54XX_EXP_EXP97			0x0f97
-#define  MII_BCM54XX_EXP_EXP97_MYST		0x0c0c
-
-/*
- * BCM5482: Secondary SerDes registers
- */
-#define BCM5482_SSD_1000BX_CTL		0x00	/* 1000BASE-X Control */
-#define BCM5482_SSD_1000BX_CTL_PWRDOWN	0x0800	/* Power-down SSD */
-#define BCM5482_SSD_SGMII_SLAVE		0x15	/* SGMII Slave Register */
-#define BCM5482_SSD_SGMII_SLAVE_EN	0x0002	/* Slave mode enable */
-#define BCM5482_SSD_SGMII_SLAVE_AD	0x0001	/* Slave auto-detection */
-
-
-/*****************************************************************************/
-/* Fast Ethernet Transceiver definitions. */
-/*****************************************************************************/
-
-#define MII_BRCM_FET_INTREG		0x1a	/* Interrupt register */
-#define MII_BRCM_FET_IR_MASK		0x0100	/* Mask all interrupts */
-#define MII_BRCM_FET_IR_LINK_EN		0x0200	/* Link status change enable */
-#define MII_BRCM_FET_IR_SPEED_EN	0x0400	/* Link speed change enable */
-#define MII_BRCM_FET_IR_DUPLEX_EN	0x0800	/* Duplex mode change enable */
-#define MII_BRCM_FET_IR_ENABLE		0x4000	/* Interrupt enable */
-
-#define MII_BRCM_FET_BRCMTEST		0x1f	/* Brcm test register */
-#define MII_BRCM_FET_BT_SRE		0x0080	/* Shadow register enable */
-
-
-/*** Shadow register definitions ***/
-
-#define MII_BRCM_FET_SHDW_MISCCTRL	0x10	/* Shadow misc ctrl */
-#define MII_BRCM_FET_SHDW_MC_FAME	0x4000	/* Force Auto MDIX enable */
-
-#define MII_BRCM_FET_SHDW_AUXMODE4	0x1a	/* Auxiliary mode 4 */
-#define MII_BRCM_FET_SHDW_AM4_LED_MASK	0x0003
-#define MII_BRCM_FET_SHDW_AM4_LED_MODE1 0x0001
-
-#define MII_BRCM_FET_SHDW_AUXSTAT2	0x1b	/* Auxiliary status 2 */
-#define MII_BRCM_FET_SHDW_AS2_APDE	0x0020	/* Auto power down enable */
-
-
 MODULE_DESCRIPTION("Broadcom PHY driver");
 MODULE_AUTHOR("Maciej W. Rozycki");
 MODULE_LICENSE("GPL");
 
-/*
- * Indirect register access functions for the 1000BASE-T/100BASE-TX/10BASE-T
- * 0x1c shadow registers.
- */
-static int bcm54xx_shadow_read(struct phy_device *phydev, u16 shadow)
-{
-	phy_write(phydev, MII_BCM54XX_SHD, MII_BCM54XX_SHD_VAL(shadow));
-	return MII_BCM54XX_SHD_DATA(phy_read(phydev, MII_BCM54XX_SHD));
-}
-
-static int bcm54xx_shadow_write(struct phy_device *phydev, u16 shadow, u16 val)
-{
-	return phy_write(phydev, MII_BCM54XX_SHD,
-			 MII_BCM54XX_SHD_WRITE |
-			 MII_BCM54XX_SHD_VAL(shadow) |
-			 MII_BCM54XX_SHD_DATA(val));
-}
-
 /* Indirect register access functions for the Expansion Registers */
 static int bcm54xx_exp_read(struct phy_device *phydev, u16 regnum)
 {
diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index c301e4cb37ca..2954052706e8 100644
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -721,7 +721,7 @@ static inline u16 exts_chan_to_edata(int ch)
 }
 
 static int decode_evnt(struct dp83640_private *dp83640,
-		       void *data, u16 ests)
+		       void *data, int len, u16 ests)
 {
 	struct phy_txts *phy_txts;
 	struct ptp_clock_event event;
@@ -729,6 +729,16 @@ static int decode_evnt(struct dp83640_private *dp83640,
 	int words = (ests >> EVNT_TS_LEN_SHIFT) & EVNT_TS_LEN_MASK;
 	u16 ext_status = 0;
 
+	/* calculate length of the event timestamp status message */
+	if (ests & MULT_EVNT)
+		parsed = (words + 2) * sizeof(u16);
+	else
+		parsed = (words + 1) * sizeof(u16);
+
+	/* check if enough data is available */
+	if (len < parsed)
+		return len;
+
 	if (ests & MULT_EVNT) {
 		ext_status = *(u16 *) data;
 		data += sizeof(ext_status);
@@ -747,10 +757,7 @@ static int decode_evnt(struct dp83640_private *dp83640,
 		dp83640->edata.ns_lo = phy_txts->ns_lo;
 	}
 
-	if (ext_status) {
-		parsed = words + 2;
-	} else {
-		parsed = words + 1;
+	if (!ext_status) {
 		i = ((ests >> EVNT_NUM_SHIFT) & EVNT_NUM_MASK) - EXT_EVENT;
 		ext_status = exts_chan_to_edata(i);
 	}
@@ -768,7 +775,7 @@ static int decode_evnt(struct dp83640_private *dp83640,
 		}
 	}
 
-	return parsed * sizeof(u16);
+	return parsed;
 }
 
 static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
@@ -905,9 +912,9 @@ static void decode_status_frame(struct dp83640_private *dp83640,
 			decode_txts(dp83640, phy_txts);
 			size = sizeof(*phy_txts);
 
-		} else if (PSF_EVNT == type && len >= sizeof(*phy_txts)) {
+		} else if (PSF_EVNT == type) {
 
-			size = decode_evnt(dp83640, ptr, ests);
+			size = decode_evnt(dp83640, ptr, len, ests);
 
 		} else {
 			size = 0;
@@ -1129,7 +1136,6 @@ static void dp83640_remove(struct phy_device *phydev)
 	struct dp83640_clock *clock;
 	struct list_head *this, *next;
 	struct dp83640_private *tmp, *dp83640 = phydev->priv;
-	struct sk_buff *skb;
 
 	if (phydev->addr == BROADCAST_ADDR)
 		return;
@@ -1137,11 +1143,8 @@ static void dp83640_remove(struct phy_device *phydev)
 	enable_status_frames(phydev, false);
 	cancel_work_sync(&dp83640->ts_work);
 
-	while ((skb = skb_dequeue(&dp83640->rx_queue)) != NULL)
-		kfree_skb(skb);
-
-	while ((skb = skb_dequeue(&dp83640->tx_queue)) != NULL)
-		skb_complete_tx_timestamp(skb, NULL);
+	skb_queue_purge(&dp83640->rx_queue);
+	skb_queue_purge(&dp83640->tx_queue);
 
 	clock = dp83640_clock_get(dp83640->clock);
 
@@ -1398,7 +1401,7 @@ static void dp83640_txtstamp(struct phy_device *phydev,
 
 	case HWTSTAMP_TX_ONESTEP_SYNC:
 		if (is_sync(skb, type)) {
-			skb_complete_tx_timestamp(skb, NULL);
+			kfree_skb(skb);
 			return;
 		}
 		/* fall through */
@@ -1409,7 +1412,7 @@ static void dp83640_txtstamp(struct phy_device *phydev,
 
 	case HWTSTAMP_TX_OFF:
 	default:
-		skb_complete_tx_timestamp(skb, NULL);
+		kfree_skb(skb);
 		break;
 	}
 }
diff --git a/drivers/net/phy/fixed.c b/drivers/net/phy/fixed.c
index d60d875cb445..47872caa0081 100644
--- a/drivers/net/phy/fixed.c
+++ b/drivers/net/phy/fixed.c
@@ -124,6 +124,17 @@ static int fixed_mdio_read(struct mii_bus *bus, int phy_addr, int reg_num)
 	if (reg_num >= MII_REGS_NUM)
 		return -1;
 
+	/* We do not support emulating Clause 45 over Clause 22 register reads
+	 * return an error instead of bogus data.
+	 */
+	switch (reg_num) {
+	case MII_MMD_CTRL:
+	case MII_MMD_DATA:
+		return -1;
+	default:
+		break;
+	}
+
 	list_for_each_entry(fp, &fmb->phys, node) {
 		if (fp->addr == phy_addr) {
 			/* Issue callback if user registered it. */
@@ -222,9 +233,9 @@ EXPORT_SYMBOL_GPL(fixed_phy_del);
 static int phy_fixed_addr;
 static DEFINE_SPINLOCK(phy_fixed_addr_lock);
 
-int fixed_phy_register(unsigned int irq,
-		       struct fixed_phy_status *status,
-		       struct device_node *np)
+struct phy_device *fixed_phy_register(unsigned int irq,
+				      struct fixed_phy_status *status,
+				      struct device_node *np)
 {
 	struct fixed_mdio_bus *fmb = &platform_fmb;
 	struct phy_device *phy;
@@ -235,19 +246,19 @@ int fixed_phy_register(unsigned int irq,
 	spin_lock(&phy_fixed_addr_lock);
 	if (phy_fixed_addr == PHY_MAX_ADDR) {
 		spin_unlock(&phy_fixed_addr_lock);
-		return -ENOSPC;
+		return ERR_PTR(-ENOSPC);
 	}
 	phy_addr = phy_fixed_addr++;
 	spin_unlock(&phy_fixed_addr_lock);
 
 	ret = fixed_phy_add(PHY_POLL, phy_addr, status);
 	if (ret < 0)
-		return ret;
+		return ERR_PTR(ret);
 
 	phy = get_phy_device(fmb->mii_bus, phy_addr, false);
 	if (!phy || IS_ERR(phy)) {
 		fixed_phy_del(phy_addr);
-		return -EINVAL;
+		return ERR_PTR(-EINVAL);
 	}
 
 	of_node_get(np);
@@ -258,10 +269,10 @@ int fixed_phy_register(unsigned int irq,
 		phy_device_free(phy);
 		of_node_put(np);
 		fixed_phy_del(phy_addr);
-		return ret;
+		return ERR_PTR(ret);
 	}
 
-	return 0;
+	return phy;
 }
 
 static int __init fixed_mdio_bus_init(void)
diff --git a/drivers/net/phy/mdio-bcm-unimac.c b/drivers/net/phy/mdio-bcm-unimac.c
new file mode 100644
index 000000000000..5b643e588e8f
--- /dev/null
+++ b/drivers/net/phy/mdio-bcm-unimac.c
@@ -0,0 +1,213 @@
+/*
+ * Broadcom UniMAC MDIO bus controller driver
+ *
+ * Copyright (C) 2014, Broadcom Corporation
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/phy.h>
+#include <linux/platform_device.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/of_mdio.h>
+
+#define MDIO_CMD		0x00
+#define  MDIO_START_BUSY	(1 << 29)
+#define  MDIO_READ_FAIL		(1 << 28)
+#define  MDIO_RD		(2 << 26)
+#define  MDIO_WR		(1 << 26)
+#define  MDIO_PMD_SHIFT		21
+#define  MDIO_PMD_MASK		0x1F
+#define  MDIO_REG_SHIFT		16
+#define  MDIO_REG_MASK		0x1F
+
+#define MDIO_CFG		0x04
+#define  MDIO_C22		(1 << 0)
+#define  MDIO_C45		0
+#define  MDIO_CLK_DIV_SHIFT	4
+#define  MDIO_CLK_DIV_MASK	0x3F
+#define  MDIO_SUPP_PREAMBLE	(1 << 12)
+
+struct unimac_mdio_priv {
+	struct mii_bus		*mii_bus;
+	void __iomem		*base;
+};
+
+static inline void unimac_mdio_start(struct unimac_mdio_priv *priv)
+{
+	u32 reg;
+
+	reg = __raw_readl(priv->base + MDIO_CMD);
+	reg |= MDIO_START_BUSY;
+	__raw_writel(reg, priv->base + MDIO_CMD);
+}
+
+static inline unsigned int unimac_mdio_busy(struct unimac_mdio_priv *priv)
+{
+	return __raw_readl(priv->base + MDIO_CMD) & MDIO_START_BUSY;
+}
+
+static int unimac_mdio_read(struct mii_bus *bus, int phy_id, int reg)
+{
+	struct unimac_mdio_priv *priv = bus->priv;
+	unsigned int timeout = 1000;
+	u32 cmd;
+
+	/* Prepare the read operation */
+	cmd = MDIO_RD | (phy_id << MDIO_PMD_SHIFT) | (reg << MDIO_REG_SHIFT);
+	__raw_writel(cmd, priv->base + MDIO_CMD);
+
+	/* Start MDIO transaction */
+	unimac_mdio_start(priv);
+
+	do {
+		if (!unimac_mdio_busy(priv))
+			break;
+
+		usleep_range(1000, 2000);
+	} while (timeout--);
+
+	if (!timeout)
+		return -ETIMEDOUT;
+
+	cmd = __raw_readl(priv->base + MDIO_CMD);
+	if (cmd & MDIO_READ_FAIL)
+		return -EIO;
+
+	return cmd & 0xffff;
+}
+
+static int unimac_mdio_write(struct mii_bus *bus, int phy_id,
+			     int reg, u16 val)
+{
+	struct unimac_mdio_priv *priv = bus->priv;
+	unsigned int timeout = 1000;
+	u32 cmd;
+
+	/* Prepare the write operation */
+	cmd = MDIO_WR | (phy_id << MDIO_PMD_SHIFT) |
+		(reg << MDIO_REG_SHIFT) | (0xffff & val);
+	__raw_writel(cmd, priv->base + MDIO_CMD);
+
+	unimac_mdio_start(priv);
+
+	do {
+		if (!unimac_mdio_busy(priv))
+			break;
+
+		usleep_range(1000, 2000);
+	} while (timeout--);
+
+	if (!timeout)
+		return -ETIMEDOUT;
+
+	return 0;
+}
+
+static int unimac_mdio_probe(struct platform_device *pdev)
+{
+	struct unimac_mdio_priv *priv;
+	struct device_node *np;
+	struct mii_bus *bus;
+	struct resource *r;
+	int ret;
+
+	np = pdev->dev.of_node;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	/* Just ioremap, as this MDIO block is usually integrated into an
+	 * Ethernet MAC controller register range
+	 */
+	priv->base = devm_ioremap(&pdev->dev, r->start, resource_size(r));
+	if (!priv->base) {
+		dev_err(&pdev->dev, "failed to remap register\n");
+		return -ENOMEM;
+	}
+
+	priv->mii_bus = mdiobus_alloc();
+	if (!priv->mii_bus)
+		return -ENOMEM;
+
+	bus = priv->mii_bus;
+	bus->priv = priv;
+	bus->name = "unimac MII bus";
+	bus->parent = &pdev->dev;
+	bus->read = unimac_mdio_read;
+	bus->write = unimac_mdio_write;
+	snprintf(bus->id, MII_BUS_ID_SIZE, "%s", pdev->name);
+
+	bus->irq = kcalloc(PHY_MAX_ADDR, sizeof(int), GFP_KERNEL);
+	if (!bus->irq) {
+		ret = -ENOMEM;
+		goto out_mdio_free;
+	}
+
+	ret = of_mdiobus_register(bus, np);
+	if (ret) {
+		dev_err(&pdev->dev, "MDIO bus registration failed\n");
+		goto out_mdio_irq;
+	}
+
+	platform_set_drvdata(pdev, priv);
+
+	dev_info(&pdev->dev, "Broadcom UniMAC MDIO bus at 0x%p\n", priv->base);
+
+	return 0;
+
+out_mdio_irq:
+	kfree(bus->irq);
+out_mdio_free:
+	mdiobus_free(bus);
+	return ret;
+}
+
+static int unimac_mdio_remove(struct platform_device *pdev)
+{
+	struct unimac_mdio_priv *priv = platform_get_drvdata(pdev);
+
+	mdiobus_unregister(priv->mii_bus);
+	kfree(priv->mii_bus->irq);
+	mdiobus_free(priv->mii_bus);
+
+	return 0;
+}
+
+static struct of_device_id unimac_mdio_ids[] = {
+	{ .compatible = "brcm,genet-mdio-v4", },
+	{ .compatible = "brcm,genet-mdio-v3", },
+	{ .compatible = "brcm,genet-mdio-v2", },
+	{ .compatible = "brcm,genet-mdio-v1", },
+	{ .compatible = "brcm,unimac-mdio", },
+	{ /* sentinel */ },
+};
+
+static struct platform_driver unimac_mdio_driver = {
+	.driver = {
+		.name = "unimac-mdio",
+		.owner = THIS_MODULE,
+		.of_match_table = unimac_mdio_ids,
+	},
+	.probe	= unimac_mdio_probe,
+	.remove	= unimac_mdio_remove,
+};
+module_platform_driver(unimac_mdio_driver);
+
+MODULE_AUTHOR("Broadcom Corporation");
+MODULE_DESCRIPTION("Broadcom UniMAC MDIO bus controller");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:unimac-mdio");
diff --git a/drivers/net/phy/mdio_bus.c b/drivers/net/phy/mdio_bus.c
index 4eaadcfcb0fe..50051f271b10 100644
--- a/drivers/net/phy/mdio_bus.c
+++ b/drivers/net/phy/mdio_bus.c
@@ -553,8 +553,14 @@ static ssize_t
 phy_interface_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
 	struct phy_device *phydev = to_phy_device(dev);
+	const char *mode = NULL;
 
-	return sprintf(buf, "%s\n", phy_modes(phydev->interface));
+	if (phy_is_internal(phydev))
+		mode = "internal";
+	else
+		mode = phy_modes(phydev->interface);
+
+	return sprintf(buf, "%s\n", mode);
 }
 static DEVICE_ATTR_RO(phy_interface);
 
diff --git a/drivers/net/phy/micrel.c b/drivers/net/phy/micrel.c
index fd0ea7c50ee6..8c2a29a9bd7f 100644
--- a/drivers/net/phy/micrel.c
+++ b/drivers/net/phy/micrel.c
@@ -26,6 +26,7 @@
 #include <linux/phy.h>
 #include <linux/micrel_phy.h>
 #include <linux/of.h>
+#include <linux/clk.h>
 
 /* Operation Mode Strap Override */
 #define MII_KSZPHY_OMSO				0x16
@@ -72,9 +73,12 @@ static int ksz_config_flags(struct phy_device *phydev)
 {
 	int regval;
 
-	if (phydev->dev_flags & MICREL_PHY_50MHZ_CLK) {
+	if (phydev->dev_flags & (MICREL_PHY_50MHZ_CLK | MICREL_PHY_25MHZ_CLK)) {
 		regval = phy_read(phydev, MII_KSZPHY_CTRL);
-		regval |= KSZ8051_RMII_50MHZ_CLK;
+		if (phydev->dev_flags & MICREL_PHY_50MHZ_CLK)
+			regval |= KSZ8051_RMII_50MHZ_CLK;
+		else
+			regval &= ~KSZ8051_RMII_50MHZ_CLK;
 		return phy_write(phydev, MII_KSZPHY_CTRL, regval);
 	}
 	return 0;
@@ -194,8 +198,10 @@ static int ksz8021_config_init(struct phy_device *phydev)
 	if (rc)
 		dev_err(&phydev->dev, "failed to set led mode\n");
 
-	phy_write(phydev, MII_KSZPHY_OMSO, val);
 	rc = ksz_config_flags(phydev);
+	if (rc < 0)
+		return rc;
+	rc = phy_write(phydev, MII_KSZPHY_OMSO, val);
 	return rc < 0 ? rc : 0;
 }
 
@@ -440,6 +446,27 @@ ksz9021_wr_mmd_phyreg(struct phy_device *phydev, int ptrad, int devnum,
 {
 }
 
+static int ksz8021_probe(struct phy_device *phydev)
+{
+	struct clk *clk;
+
+	clk = devm_clk_get(&phydev->dev, "rmii-ref");
+	if (!IS_ERR(clk)) {
+		unsigned long rate = clk_get_rate(clk);
+
+		if (rate > 24500000 && rate < 25500000) {
+			phydev->dev_flags |= MICREL_PHY_25MHZ_CLK;
+		} else if (rate > 49500000 && rate < 50500000) {
+			phydev->dev_flags |= MICREL_PHY_50MHZ_CLK;
+		} else {
+			dev_err(&phydev->dev, "Clock rate out of range: %ld\n", rate);
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
 static struct phy_driver ksphy_driver[] = {
 {
 	.phy_id		= PHY_ID_KS8737,
@@ -462,6 +489,7 @@ static struct phy_driver ksphy_driver[] = {
 	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause |
 			   SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.probe		= ksz8021_probe,
 	.config_init	= ksz8021_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -477,6 +505,7 @@ static struct phy_driver ksphy_driver[] = {
 	.features	= (PHY_BASIC_FEATURES | SUPPORTED_Pause |
 			   SUPPORTED_Asym_Pause),
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
+	.probe		= ksz8021_probe,
 	.config_init	= ksz8021_config_init,
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
@@ -592,8 +621,7 @@ static struct phy_driver ksphy_driver[] = {
 	.phy_id		= PHY_ID_KSZ9031,
 	.phy_id_mask	= 0x00fffff0,
 	.name		= "Micrel KSZ9031 Gigabit PHY",
-	.features	= (PHY_GBIT_FEATURES | SUPPORTED_Pause
-				| SUPPORTED_Asym_Pause),
+	.features	= (PHY_GBIT_FEATURES | SUPPORTED_Pause),
 	.flags		= PHY_HAS_MAGICANEG | PHY_HAS_INTERRUPT,
 	.config_init	= ksz9031_config_init,
 	.config_aneg	= genphy_config_aneg,
diff --git a/drivers/net/phy/phy.c b/drivers/net/phy/phy.c
index c94e2a27446a..1dfffdc9dfc3 100644
--- a/drivers/net/phy/phy.c
+++ b/drivers/net/phy/phy.c
@@ -955,7 +955,7 @@ static inline void mmd_phy_indirect(struct mii_bus *bus, int prtad, int devad,
  * 3) Write reg 13 // MMD Data Command for MMD DEVAD
  * 3) Read  reg 14 // Read MMD data
  */
-static int phy_read_mmd_indirect(struct phy_device *phydev, int prtad,
+int phy_read_mmd_indirect(struct phy_device *phydev, int prtad,
 				 int devad, int addr)
 {
 	struct phy_driver *phydrv = phydev->drv;
@@ -971,6 +971,7 @@ static int phy_read_mmd_indirect(struct phy_device *phydev, int prtad,
 	}
 	return value;
 }
+EXPORT_SYMBOL(phy_read_mmd_indirect);
 
 /**
  * phy_write_mmd_indirect - writes data to the MMD registers
@@ -988,7 +989,7 @@ static int phy_read_mmd_indirect(struct phy_device *phydev, int prtad,
  * 3) Write reg 13 // MMD Data Command for MMD DEVAD
  * 3) Write reg 14 // Write MMD data
  */
-static void phy_write_mmd_indirect(struct phy_device *phydev, int prtad,
+void phy_write_mmd_indirect(struct phy_device *phydev, int prtad,
 				   int devad, int addr, u32 data)
 {
 	struct phy_driver *phydrv = phydev->drv;
@@ -1002,6 +1003,7 @@ static void phy_write_mmd_indirect(struct phy_device *phydev, int prtad,
 		phydrv->write_mmd_indirect(phydev, prtad, devad, addr, data);
 	}
 }
+EXPORT_SYMBOL(phy_write_mmd_indirect);
 
 /**
  * phy_init_eee - init and check the EEE feature
@@ -1017,12 +1019,14 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 {
 	/* According to 802.3az,the EEE is supported only in full duplex-mode.
 	 * Also EEE feature is active when core is operating with MII, GMII
-	 * or RGMII.
+	 * or RGMII. Internal PHYs are also allowed to proceed and should
+	 * return an error if they do not support EEE.
 	 */
 	if ((phydev->duplex == DUPLEX_FULL) &&
 	    ((phydev->interface == PHY_INTERFACE_MODE_MII) ||
 	    (phydev->interface == PHY_INTERFACE_MODE_GMII) ||
-	    (phydev->interface == PHY_INTERFACE_MODE_RGMII))) {
+	    (phydev->interface == PHY_INTERFACE_MODE_RGMII) ||
+	     phy_is_internal(phydev))) {
 		int eee_lp, eee_cap, eee_adv;
 		u32 lp, cap, adv;
 		int status;
@@ -1036,31 +1040,31 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 		/* First check if the EEE ability is supported */
 		eee_cap = phy_read_mmd_indirect(phydev, MDIO_PCS_EEE_ABLE,
 						MDIO_MMD_PCS, phydev->addr);
-		if (eee_cap < 0)
-			return eee_cap;
+		if (eee_cap <= 0)
+			goto eee_exit_err;
 
 		cap = mmd_eee_cap_to_ethtool_sup_t(eee_cap);
 		if (!cap)
-			return -EPROTONOSUPPORT;
+			goto eee_exit_err;
 
 		/* Check which link settings negotiated and verify it in
 		 * the EEE advertising registers.
 		 */
 		eee_lp = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_LPABLE,
 					       MDIO_MMD_AN, phydev->addr);
-		if (eee_lp < 0)
-			return eee_lp;
+		if (eee_lp <= 0)
+			goto eee_exit_err;
 
 		eee_adv = phy_read_mmd_indirect(phydev, MDIO_AN_EEE_ADV,
 						MDIO_MMD_AN, phydev->addr);
-		if (eee_adv < 0)
-			return eee_adv;
+		if (eee_adv <= 0)
+			goto eee_exit_err;
 
 		adv = mmd_eee_adv_to_ethtool_adv_t(eee_adv);
 		lp = mmd_eee_adv_to_ethtool_adv_t(eee_lp);
 		idx = phy_find_setting(phydev->speed, phydev->duplex);
 		if (!(lp & adv & settings[idx].setting))
-			return -EPROTONOSUPPORT;
+			goto eee_exit_err;
 
 		if (clk_stop_enable) {
 			/* Configure the PHY to stop receiving xMII
@@ -1080,7 +1084,7 @@ int phy_init_eee(struct phy_device *phydev, bool clk_stop_enable)
 
 		return 0; /* EEE supported */
 	}
-
+eee_exit_err:
 	return -EPROTONOSUPPORT;
 }
 EXPORT_SYMBOL(phy_init_eee);
diff --git a/drivers/net/phy/phy_device.c b/drivers/net/phy/phy_device.c
index ca5ec3e18d36..3fc91e89f5a5 100644
--- a/drivers/net/phy/phy_device.c
+++ b/drivers/net/phy/phy_device.c
@@ -230,13 +230,13 @@ static int get_phy_c45_ids(struct mii_bus *bus, int addr, u32 *phy_id,
 	for (i = 1;
 	     i < num_ids && c45_ids->devices_in_package == 0;
 	     i++) {
-		reg_addr = MII_ADDR_C45 | i << 16 | 6;
+		reg_addr = MII_ADDR_C45 | i << 16 | MDIO_DEVS2;
 		phy_reg = mdiobus_read(bus, addr, reg_addr);
 		if (phy_reg < 0)
 			return -EIO;
 		c45_ids->devices_in_package = (phy_reg & 0xffff) << 16;
 
-		reg_addr = MII_ADDR_C45 | i << 16 | 5;
+		reg_addr = MII_ADDR_C45 | i << 16 | MDIO_DEVS1;
 		phy_reg = mdiobus_read(bus, addr, reg_addr);
 		if (phy_reg < 0)
 			return -EIO;
diff --git a/drivers/net/phy/smsc.c b/drivers/net/phy/smsc.c
index 180c49479c42..a4b08198fb9f 100644
--- a/drivers/net/phy/smsc.c
+++ b/drivers/net/phy/smsc.c
@@ -43,6 +43,22 @@ static int smsc_phy_ack_interrupt(struct phy_device *phydev)
 
 static int smsc_phy_config_init(struct phy_device *phydev)
 {
+	int rc = phy_read(phydev, MII_LAN83C185_CTRL_STATUS);
+
+	if (rc < 0)
+		return rc;
+
+	/* Enable energy detect mode for this SMSC Transceivers */
+	rc = phy_write(phydev, MII_LAN83C185_CTRL_STATUS,
+		       rc | MII_LAN83C185_EDPWRDOWN);
+	if (rc < 0)
+		return rc;
+
+	return smsc_phy_ack_interrupt(phydev);
+}
+
+static int smsc_phy_reset(struct phy_device *phydev)
+{
 	int rc = phy_read(phydev, MII_LAN83C185_SPECIAL_MODES);
 	if (rc < 0)
 		return rc;
@@ -66,18 +82,7 @@ static int smsc_phy_config_init(struct phy_device *phydev)
 			rc = phy_read(phydev, MII_BMCR);
 		} while (rc & BMCR_RESET);
 	}
-
-	rc = phy_read(phydev, MII_LAN83C185_CTRL_STATUS);
-	if (rc < 0)
-		return rc;
-
-	/* Enable energy detect mode for this SMSC Transceivers */
-	rc = phy_write(phydev, MII_LAN83C185_CTRL_STATUS,
-		       rc | MII_LAN83C185_EDPWRDOWN);
-	if (rc < 0)
-		return rc;
-
-	return smsc_phy_ack_interrupt (phydev);
+	return 0;
 }
 
 static int lan911x_config_init(struct phy_device *phydev)
@@ -142,6 +147,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
 	.config_init	= smsc_phy_config_init,
+	.soft_reset	= smsc_phy_reset,
 
 	/* IRQ related */
 	.ack_interrupt	= smsc_phy_ack_interrupt,
@@ -164,6 +170,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
 	.config_init	= smsc_phy_config_init,
+	.soft_reset	= smsc_phy_reset,
 
 	/* IRQ related */
 	.ack_interrupt	= smsc_phy_ack_interrupt,
@@ -186,6 +193,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= genphy_read_status,
 	.config_init	= smsc_phy_config_init,
+	.soft_reset	= smsc_phy_reset,
 
 	/* IRQ related */
 	.ack_interrupt	= smsc_phy_ack_interrupt,
@@ -230,6 +238,7 @@ static struct phy_driver smsc_phy_driver[] = {
 	.config_aneg	= genphy_config_aneg,
 	.read_status	= lan87xx_read_status,
 	.config_init	= smsc_phy_config_init,
+	.soft_reset	= smsc_phy_reset,
 
 	/* IRQ related */
 	.ack_interrupt	= smsc_phy_ack_interrupt,
diff --git a/drivers/net/ppp/ppp_generic.c b/drivers/net/ppp/ppp_generic.c
index fa0d71727894..68c3a3f4e0ab 100644
--- a/drivers/net/ppp/ppp_generic.c
+++ b/drivers/net/ppp/ppp_generic.c
@@ -594,7 +594,7 @@ static long ppp_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 			if (file == ppp->owner)
 				ppp_shutdown_interface(ppp);
 		}
-		if (atomic_long_read(&file->f_count) <= 2) {
+		if (atomic_long_read(&file->f_count) < 2) {
 			ppp_release(NULL, file);
 			err = 0;
 		} else
@@ -1103,7 +1103,7 @@ static void ppp_setup(struct net_device *dev)
 	dev->type = ARPHRD_PPP;
 	dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
 	dev->features |= NETIF_F_NETNS_LOCAL;
-	dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(dev);
 }
 
 /*
diff --git a/drivers/net/sungem_phy.c b/drivers/net/sungem_phy.c
index ae7cd7f3656d..92578d72e4ee 100644
--- a/drivers/net/sungem_phy.c
+++ b/drivers/net/sungem_phy.c
@@ -47,22 +47,22 @@ static const int phy_BCM5400_link_table[8][3] = {
 	{ 1, 0, 1 },	/* 1000BT */
 };
 
-static inline int __phy_read(struct mii_phy* phy, int id, int reg)
+static inline int __sungem_phy_read(struct mii_phy* phy, int id, int reg)
 {
 	return phy->mdio_read(phy->dev, id, reg);
 }
 
-static inline void __phy_write(struct mii_phy* phy, int id, int reg, int val)
+static inline void __sungem_phy_write(struct mii_phy* phy, int id, int reg, int val)
 {
 	phy->mdio_write(phy->dev, id, reg, val);
 }
 
-static inline int phy_read(struct mii_phy* phy, int reg)
+static inline int sungem_phy_read(struct mii_phy* phy, int reg)
 {
 	return phy->mdio_read(phy->dev, phy->mii_id, reg);
 }
 
-static inline void phy_write(struct mii_phy* phy, int reg, int val)
+static inline void sungem_phy_write(struct mii_phy* phy, int reg, int val)
 {
 	phy->mdio_write(phy->dev, phy->mii_id, reg, val);
 }
@@ -72,21 +72,21 @@ static int reset_one_mii_phy(struct mii_phy* phy, int phy_id)
 	u16 val;
 	int limit = 10000;
 
-	val = __phy_read(phy, phy_id, MII_BMCR);
+	val = __sungem_phy_read(phy, phy_id, MII_BMCR);
 	val &= ~(BMCR_ISOLATE | BMCR_PDOWN);
 	val |= BMCR_RESET;
-	__phy_write(phy, phy_id, MII_BMCR, val);
+	__sungem_phy_write(phy, phy_id, MII_BMCR, val);
 
 	udelay(100);
 
 	while (--limit) {
-		val = __phy_read(phy, phy_id, MII_BMCR);
+		val = __sungem_phy_read(phy, phy_id, MII_BMCR);
 		if ((val & BMCR_RESET) == 0)
 			break;
 		udelay(10);
 	}
 	if ((val & BMCR_ISOLATE) && limit > 0)
-		__phy_write(phy, phy_id, MII_BMCR, val & ~BMCR_ISOLATE);
+		__sungem_phy_write(phy, phy_id, MII_BMCR, val & ~BMCR_ISOLATE);
 
 	return limit <= 0;
 }
@@ -95,19 +95,19 @@ static int bcm5201_init(struct mii_phy* phy)
 {
 	u16 data;
 
-	data = phy_read(phy, MII_BCM5201_MULTIPHY);
+	data = sungem_phy_read(phy, MII_BCM5201_MULTIPHY);
 	data &= ~MII_BCM5201_MULTIPHY_SUPERISOLATE;
-	phy_write(phy, MII_BCM5201_MULTIPHY, data);
+	sungem_phy_write(phy, MII_BCM5201_MULTIPHY, data);
 
-	phy_write(phy, MII_BCM5201_INTERRUPT, 0);
+	sungem_phy_write(phy, MII_BCM5201_INTERRUPT, 0);
 
 	return 0;
 }
 
 static int bcm5201_suspend(struct mii_phy* phy)
 {
-	phy_write(phy, MII_BCM5201_INTERRUPT, 0);
-	phy_write(phy, MII_BCM5201_MULTIPHY, MII_BCM5201_MULTIPHY_SUPERISOLATE);
+	sungem_phy_write(phy, MII_BCM5201_INTERRUPT, 0);
+	sungem_phy_write(phy, MII_BCM5201_MULTIPHY, MII_BCM5201_MULTIPHY_SUPERISOLATE);
 
 	return 0;
 }
@@ -116,20 +116,20 @@ static int bcm5221_init(struct mii_phy* phy)
 {
 	u16 data;
 
-	data = phy_read(phy, MII_BCM5221_TEST);
-	phy_write(phy, MII_BCM5221_TEST,
+	data = sungem_phy_read(phy, MII_BCM5221_TEST);
+	sungem_phy_write(phy, MII_BCM5221_TEST,
 		data | MII_BCM5221_TEST_ENABLE_SHADOWS);
 
-	data = phy_read(phy, MII_BCM5221_SHDOW_AUX_STAT2);
-	phy_write(phy, MII_BCM5221_SHDOW_AUX_STAT2,
+	data = sungem_phy_read(phy, MII_BCM5221_SHDOW_AUX_STAT2);
+	sungem_phy_write(phy, MII_BCM5221_SHDOW_AUX_STAT2,
 		data | MII_BCM5221_SHDOW_AUX_STAT2_APD);
 
-	data = phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
-	phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
+	data = sungem_phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
+	sungem_phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
 		data | MII_BCM5221_SHDOW_AUX_MODE4_CLKLOPWR);
 
-	data = phy_read(phy, MII_BCM5221_TEST);
-	phy_write(phy, MII_BCM5221_TEST,
+	data = sungem_phy_read(phy, MII_BCM5221_TEST);
+	sungem_phy_write(phy, MII_BCM5221_TEST,
 		data & ~MII_BCM5221_TEST_ENABLE_SHADOWS);
 
 	return 0;
@@ -139,12 +139,12 @@ static int bcm5221_suspend(struct mii_phy* phy)
 {
 	u16 data;
 
-	data = phy_read(phy, MII_BCM5221_TEST);
-	phy_write(phy, MII_BCM5221_TEST,
+	data = sungem_phy_read(phy, MII_BCM5221_TEST);
+	sungem_phy_write(phy, MII_BCM5221_TEST,
 		data | MII_BCM5221_TEST_ENABLE_SHADOWS);
 
-	data = phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
-	phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
+	data = sungem_phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
+	sungem_phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
 		  data | MII_BCM5221_SHDOW_AUX_MODE4_IDDQMODE);
 
 	return 0;
@@ -154,20 +154,20 @@ static int bcm5241_init(struct mii_phy* phy)
 {
 	u16 data;
 
-	data = phy_read(phy, MII_BCM5221_TEST);
-	phy_write(phy, MII_BCM5221_TEST,
+	data = sungem_phy_read(phy, MII_BCM5221_TEST);
+	sungem_phy_write(phy, MII_BCM5221_TEST,
 		data | MII_BCM5221_TEST_ENABLE_SHADOWS);
 
-	data = phy_read(phy, MII_BCM5221_SHDOW_AUX_STAT2);
-	phy_write(phy, MII_BCM5221_SHDOW_AUX_STAT2,
+	data = sungem_phy_read(phy, MII_BCM5221_SHDOW_AUX_STAT2);
+	sungem_phy_write(phy, MII_BCM5221_SHDOW_AUX_STAT2,
 		data | MII_BCM5221_SHDOW_AUX_STAT2_APD);
 
-	data = phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
-	phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
+	data = sungem_phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
+	sungem_phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
 		data & ~MII_BCM5241_SHDOW_AUX_MODE4_STANDBYPWR);
 
-	data = phy_read(phy, MII_BCM5221_TEST);
-	phy_write(phy, MII_BCM5221_TEST,
+	data = sungem_phy_read(phy, MII_BCM5221_TEST);
+	sungem_phy_write(phy, MII_BCM5221_TEST,
 		data & ~MII_BCM5221_TEST_ENABLE_SHADOWS);
 
 	return 0;
@@ -177,12 +177,12 @@ static int bcm5241_suspend(struct mii_phy* phy)
 {
 	u16 data;
 
-	data = phy_read(phy, MII_BCM5221_TEST);
-	phy_write(phy, MII_BCM5221_TEST,
+	data = sungem_phy_read(phy, MII_BCM5221_TEST);
+	sungem_phy_write(phy, MII_BCM5221_TEST,
 		data | MII_BCM5221_TEST_ENABLE_SHADOWS);
 
-	data = phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
-	phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
+	data = sungem_phy_read(phy, MII_BCM5221_SHDOW_AUX_MODE4);
+	sungem_phy_write(phy, MII_BCM5221_SHDOW_AUX_MODE4,
 		  data | MII_BCM5241_SHDOW_AUX_MODE4_STANDBYPWR);
 
 	return 0;
@@ -193,26 +193,26 @@ static int bcm5400_init(struct mii_phy* phy)
 	u16 data;
 
 	/* Configure for gigabit full duplex */
-	data = phy_read(phy, MII_BCM5400_AUXCONTROL);
+	data = sungem_phy_read(phy, MII_BCM5400_AUXCONTROL);
 	data |= MII_BCM5400_AUXCONTROL_PWR10BASET;
-	phy_write(phy, MII_BCM5400_AUXCONTROL, data);
+	sungem_phy_write(phy, MII_BCM5400_AUXCONTROL, data);
 
-	data = phy_read(phy, MII_BCM5400_GB_CONTROL);
+	data = sungem_phy_read(phy, MII_BCM5400_GB_CONTROL);
 	data |= MII_BCM5400_GB_CONTROL_FULLDUPLEXCAP;
-	phy_write(phy, MII_BCM5400_GB_CONTROL, data);
+	sungem_phy_write(phy, MII_BCM5400_GB_CONTROL, data);
 
 	udelay(100);
 
 	/* Reset and configure cascaded 10/100 PHY */
 	(void)reset_one_mii_phy(phy, 0x1f);
 
-	data = __phy_read(phy, 0x1f, MII_BCM5201_MULTIPHY);
+	data = __sungem_phy_read(phy, 0x1f, MII_BCM5201_MULTIPHY);
 	data |= MII_BCM5201_MULTIPHY_SERIALMODE;
-	__phy_write(phy, 0x1f, MII_BCM5201_MULTIPHY, data);
+	__sungem_phy_write(phy, 0x1f, MII_BCM5201_MULTIPHY, data);
 
-	data = phy_read(phy, MII_BCM5400_AUXCONTROL);
+	data = sungem_phy_read(phy, MII_BCM5400_AUXCONTROL);
 	data &= ~MII_BCM5400_AUXCONTROL_PWR10BASET;
-	phy_write(phy, MII_BCM5400_AUXCONTROL, data);
+	sungem_phy_write(phy, MII_BCM5400_AUXCONTROL, data);
 
 	return 0;
 }
@@ -220,7 +220,7 @@ static int bcm5400_init(struct mii_phy* phy)
 static int bcm5400_suspend(struct mii_phy* phy)
 {
 #if 0 /* Commented out in Darwin... someone has those dawn docs ? */
-	phy_write(phy, MII_BMCR, BMCR_PDOWN);
+	sungem_phy_write(phy, MII_BMCR, BMCR_PDOWN);
 #endif
 	return 0;
 }
@@ -230,7 +230,7 @@ static int bcm5401_init(struct mii_phy* phy)
 	u16 data;
 	int rev;
 
-	rev = phy_read(phy, MII_PHYSID2) & 0x000f;
+	rev = sungem_phy_read(phy, MII_PHYSID2) & 0x000f;
 	if (rev == 0 || rev == 3) {
 		/* Some revisions of 5401 appear to need this
 		 * initialisation sequence to disable, according
@@ -243,32 +243,32 @@ static int bcm5401_init(struct mii_phy* phy)
 		 * Note: This should (and does) match tg3_init_5401phy_dsp
 		 *       in the tg3.c driver. -DaveM
 		 */
-		phy_write(phy, 0x18, 0x0c20);
-		phy_write(phy, 0x17, 0x0012);
-		phy_write(phy, 0x15, 0x1804);
-		phy_write(phy, 0x17, 0x0013);
-		phy_write(phy, 0x15, 0x1204);
-		phy_write(phy, 0x17, 0x8006);
-		phy_write(phy, 0x15, 0x0132);
-		phy_write(phy, 0x17, 0x8006);
-		phy_write(phy, 0x15, 0x0232);
-		phy_write(phy, 0x17, 0x201f);
-		phy_write(phy, 0x15, 0x0a20);
+		sungem_phy_write(phy, 0x18, 0x0c20);
+		sungem_phy_write(phy, 0x17, 0x0012);
+		sungem_phy_write(phy, 0x15, 0x1804);
+		sungem_phy_write(phy, 0x17, 0x0013);
+		sungem_phy_write(phy, 0x15, 0x1204);
+		sungem_phy_write(phy, 0x17, 0x8006);
+		sungem_phy_write(phy, 0x15, 0x0132);
+		sungem_phy_write(phy, 0x17, 0x8006);
+		sungem_phy_write(phy, 0x15, 0x0232);
+		sungem_phy_write(phy, 0x17, 0x201f);
+		sungem_phy_write(phy, 0x15, 0x0a20);
 	}
 
 	/* Configure for gigabit full duplex */
-	data = phy_read(phy, MII_BCM5400_GB_CONTROL);
+	data = sungem_phy_read(phy, MII_BCM5400_GB_CONTROL);
 	data |= MII_BCM5400_GB_CONTROL_FULLDUPLEXCAP;
-	phy_write(phy, MII_BCM5400_GB_CONTROL, data);
+	sungem_phy_write(phy, MII_BCM5400_GB_CONTROL, data);
 
 	udelay(10);
 
 	/* Reset and configure cascaded 10/100 PHY */
 	(void)reset_one_mii_phy(phy, 0x1f);
 
-	data = __phy_read(phy, 0x1f, MII_BCM5201_MULTIPHY);
+	data = __sungem_phy_read(phy, 0x1f, MII_BCM5201_MULTIPHY);
 	data |= MII_BCM5201_MULTIPHY_SERIALMODE;
-	__phy_write(phy, 0x1f, MII_BCM5201_MULTIPHY, data);
+	__sungem_phy_write(phy, 0x1f, MII_BCM5201_MULTIPHY, data);
 
 	return 0;
 }
@@ -276,7 +276,7 @@ static int bcm5401_init(struct mii_phy* phy)
 static int bcm5401_suspend(struct mii_phy* phy)
 {
 #if 0 /* Commented out in Darwin... someone has those dawn docs ? */
-	phy_write(phy, MII_BMCR, BMCR_PDOWN);
+	sungem_phy_write(phy, MII_BMCR, BMCR_PDOWN);
 #endif
 	return 0;
 }
@@ -288,19 +288,19 @@ static int bcm5411_init(struct mii_phy* phy)
 	/* Here's some more Apple black magic to setup
 	 * some voltage stuffs.
 	 */
-	phy_write(phy, 0x1c, 0x8c23);
-	phy_write(phy, 0x1c, 0x8ca3);
-	phy_write(phy, 0x1c, 0x8c23);
+	sungem_phy_write(phy, 0x1c, 0x8c23);
+	sungem_phy_write(phy, 0x1c, 0x8ca3);
+	sungem_phy_write(phy, 0x1c, 0x8c23);
 
 	/* Here, Apple seems to want to reset it, do
 	 * it as well
 	 */
-	phy_write(phy, MII_BMCR, BMCR_RESET);
-	phy_write(phy, MII_BMCR, 0x1340);
+	sungem_phy_write(phy, MII_BMCR, BMCR_RESET);
+	sungem_phy_write(phy, MII_BMCR, 0x1340);
 
-	data = phy_read(phy, MII_BCM5400_GB_CONTROL);
+	data = sungem_phy_read(phy, MII_BCM5400_GB_CONTROL);
 	data |= MII_BCM5400_GB_CONTROL_FULLDUPLEXCAP;
-	phy_write(phy, MII_BCM5400_GB_CONTROL, data);
+	sungem_phy_write(phy, MII_BCM5400_GB_CONTROL, data);
 
 	udelay(10);
 
@@ -321,7 +321,7 @@ static int genmii_setup_aneg(struct mii_phy *phy, u32 advertise)
 	phy->advertising = advertise;
 
 	/* Setup standard advertise */
-	adv = phy_read(phy, MII_ADVERTISE);
+	adv = sungem_phy_read(phy, MII_ADVERTISE);
 	adv &= ~(ADVERTISE_ALL | ADVERTISE_100BASE4);
 	if (advertise & ADVERTISED_10baseT_Half)
 		adv |= ADVERTISE_10HALF;
@@ -331,12 +331,12 @@ static int genmii_setup_aneg(struct mii_phy *phy, u32 advertise)
 		adv |= ADVERTISE_100HALF;
 	if (advertise & ADVERTISED_100baseT_Full)
 		adv |= ADVERTISE_100FULL;
-	phy_write(phy, MII_ADVERTISE, adv);
+	sungem_phy_write(phy, MII_ADVERTISE, adv);
 
 	/* Start/Restart aneg */
-	ctl = phy_read(phy, MII_BMCR);
+	ctl = sungem_phy_read(phy, MII_BMCR);
 	ctl |= (BMCR_ANENABLE | BMCR_ANRESTART);
-	phy_write(phy, MII_BMCR, ctl);
+	sungem_phy_write(phy, MII_BMCR, ctl);
 
 	return 0;
 }
@@ -350,11 +350,11 @@ static int genmii_setup_forced(struct mii_phy *phy, int speed, int fd)
 	phy->duplex = fd;
 	phy->pause = 0;
 
-	ctl = phy_read(phy, MII_BMCR);
+	ctl = sungem_phy_read(phy, MII_BMCR);
 	ctl &= ~(BMCR_FULLDPLX|BMCR_SPEED100|BMCR_ANENABLE);
 
 	/* First reset the PHY */
-	phy_write(phy, MII_BMCR, ctl | BMCR_RESET);
+	sungem_phy_write(phy, MII_BMCR, ctl | BMCR_RESET);
 
 	/* Select speed & duplex */
 	switch(speed) {
@@ -369,7 +369,7 @@ static int genmii_setup_forced(struct mii_phy *phy, int speed, int fd)
 	}
 	if (fd == DUPLEX_FULL)
 		ctl |= BMCR_FULLDPLX;
-	phy_write(phy, MII_BMCR, ctl);
+	sungem_phy_write(phy, MII_BMCR, ctl);
 
 	return 0;
 }
@@ -378,8 +378,8 @@ static int genmii_poll_link(struct mii_phy *phy)
 {
 	u16 status;
 
-	(void)phy_read(phy, MII_BMSR);
-	status = phy_read(phy, MII_BMSR);
+	(void)sungem_phy_read(phy, MII_BMSR);
+	status = sungem_phy_read(phy, MII_BMSR);
 	if ((status & BMSR_LSTATUS) == 0)
 		return 0;
 	if (phy->autoneg && !(status & BMSR_ANEGCOMPLETE))
@@ -392,7 +392,7 @@ static int genmii_read_link(struct mii_phy *phy)
 	u16 lpa;
 
 	if (phy->autoneg) {
-		lpa = phy_read(phy, MII_LPA);
+		lpa = sungem_phy_read(phy, MII_LPA);
 
 		if (lpa & (LPA_10FULL | LPA_100FULL))
 			phy->duplex = DUPLEX_FULL;
@@ -413,7 +413,7 @@ static int genmii_read_link(struct mii_phy *phy)
 
 static int generic_suspend(struct mii_phy* phy)
 {
-	phy_write(phy, MII_BMCR, BMCR_PDOWN);
+	sungem_phy_write(phy, MII_BMCR, BMCR_PDOWN);
 
 	return 0;
 }
@@ -423,27 +423,27 @@ static int bcm5421_init(struct mii_phy* phy)
 	u16 data;
 	unsigned int id;
 
-	id = (phy_read(phy, MII_PHYSID1) << 16 | phy_read(phy, MII_PHYSID2));
+	id = (sungem_phy_read(phy, MII_PHYSID1) << 16 | sungem_phy_read(phy, MII_PHYSID2));
 
 	/* Revision 0 of 5421 needs some fixups */
 	if (id == 0x002060e0) {
 		/* This is borrowed from MacOS
 		 */
-		phy_write(phy, 0x18, 0x1007);
-		data = phy_read(phy, 0x18);
-		phy_write(phy, 0x18, data | 0x0400);
-		phy_write(phy, 0x18, 0x0007);
-		data = phy_read(phy, 0x18);
-		phy_write(phy, 0x18, data | 0x0800);
-		phy_write(phy, 0x17, 0x000a);
-		data = phy_read(phy, 0x15);
-		phy_write(phy, 0x15, data | 0x0200);
+		sungem_phy_write(phy, 0x18, 0x1007);
+		data = sungem_phy_read(phy, 0x18);
+		sungem_phy_write(phy, 0x18, data | 0x0400);
+		sungem_phy_write(phy, 0x18, 0x0007);
+		data = sungem_phy_read(phy, 0x18);
+		sungem_phy_write(phy, 0x18, data | 0x0800);
+		sungem_phy_write(phy, 0x17, 0x000a);
+		data = sungem_phy_read(phy, 0x15);
+		sungem_phy_write(phy, 0x15, data | 0x0200);
 	}
 
 	/* Pick up some init code from OF for K2 version */
 	if ((id & 0xfffffff0) == 0x002062e0) {
-		phy_write(phy, 4, 0x01e1);
-		phy_write(phy, 9, 0x0300);
+		sungem_phy_write(phy, 4, 0x01e1);
+		sungem_phy_write(phy, 9, 0x0300);
 	}
 
 	/* Check if we can enable automatic low power */
@@ -455,9 +455,9 @@ static int bcm5421_init(struct mii_phy* phy)
 			can_low_power = 0;
 		if (can_low_power) {
 			/* Enable automatic low-power */
-			phy_write(phy, 0x1c, 0x9002);
-			phy_write(phy, 0x1c, 0xa821);
-			phy_write(phy, 0x1c, 0x941d);
+			sungem_phy_write(phy, 0x1c, 0x9002);
+			sungem_phy_write(phy, 0x1c, 0xa821);
+			sungem_phy_write(phy, 0x1c, 0x941d);
 		}
 	}
 #endif /* CONFIG_PPC_PMAC */
@@ -476,7 +476,7 @@ static int bcm54xx_setup_aneg(struct mii_phy *phy, u32 advertise)
 	phy->advertising = advertise;
 
 	/* Setup standard advertise */
-	adv = phy_read(phy, MII_ADVERTISE);
+	adv = sungem_phy_read(phy, MII_ADVERTISE);
 	adv &= ~(ADVERTISE_ALL | ADVERTISE_100BASE4);
 	if (advertise & ADVERTISED_10baseT_Half)
 		adv |= ADVERTISE_10HALF;
@@ -490,21 +490,21 @@ static int bcm54xx_setup_aneg(struct mii_phy *phy, u32 advertise)
 		adv |= ADVERTISE_PAUSE_CAP;
 	if (advertise & ADVERTISED_Asym_Pause)
 		adv |= ADVERTISE_PAUSE_ASYM;
-	phy_write(phy, MII_ADVERTISE, adv);
+	sungem_phy_write(phy, MII_ADVERTISE, adv);
 
 	/* Setup 1000BT advertise */
-	adv = phy_read(phy, MII_1000BASETCONTROL);
+	adv = sungem_phy_read(phy, MII_1000BASETCONTROL);
 	adv &= ~(MII_1000BASETCONTROL_FULLDUPLEXCAP|MII_1000BASETCONTROL_HALFDUPLEXCAP);
 	if (advertise & SUPPORTED_1000baseT_Half)
 		adv |= MII_1000BASETCONTROL_HALFDUPLEXCAP;
 	if (advertise & SUPPORTED_1000baseT_Full)
 		adv |= MII_1000BASETCONTROL_FULLDUPLEXCAP;
-	phy_write(phy, MII_1000BASETCONTROL, adv);
+	sungem_phy_write(phy, MII_1000BASETCONTROL, adv);
 
 	/* Start/Restart aneg */
-	ctl = phy_read(phy, MII_BMCR);
+	ctl = sungem_phy_read(phy, MII_BMCR);
 	ctl |= (BMCR_ANENABLE | BMCR_ANRESTART);
-	phy_write(phy, MII_BMCR, ctl);
+	sungem_phy_write(phy, MII_BMCR, ctl);
 
 	return 0;
 }
@@ -518,11 +518,11 @@ static int bcm54xx_setup_forced(struct mii_phy *phy, int speed, int fd)
 	phy->duplex = fd;
 	phy->pause = 0;
 
-	ctl = phy_read(phy, MII_BMCR);
+	ctl = sungem_phy_read(phy, MII_BMCR);
 	ctl &= ~(BMCR_FULLDPLX|BMCR_SPEED100|BMCR_SPD2|BMCR_ANENABLE);
 
 	/* First reset the PHY */
-	phy_write(phy, MII_BMCR, ctl | BMCR_RESET);
+	sungem_phy_write(phy, MII_BMCR, ctl | BMCR_RESET);
 
 	/* Select speed & duplex */
 	switch(speed) {
@@ -539,7 +539,7 @@ static int bcm54xx_setup_forced(struct mii_phy *phy, int speed, int fd)
 
 	// XXX Should we set the sungem to GII now on 1000BT ?
 
-	phy_write(phy, MII_BMCR, ctl);
+	sungem_phy_write(phy, MII_BMCR, ctl);
 
 	return 0;
 }
@@ -550,7 +550,7 @@ static int bcm54xx_read_link(struct mii_phy *phy)
 	u16 val;
 
 	if (phy->autoneg) {
-	    	val = phy_read(phy, MII_BCM5400_AUXSTATUS);
+	    	val = sungem_phy_read(phy, MII_BCM5400_AUXSTATUS);
 		link_mode = ((val & MII_BCM5400_AUXSTATUS_LINKMODE_MASK) >>
 			     MII_BCM5400_AUXSTATUS_LINKMODE_SHIFT);
 		phy->duplex = phy_BCM5400_link_table[link_mode][0] ?
@@ -559,7 +559,7 @@ static int bcm54xx_read_link(struct mii_phy *phy)
 				SPEED_1000 :
 				(phy_BCM5400_link_table[link_mode][1] ?
 				 SPEED_100 : SPEED_10);
-		val = phy_read(phy, MII_LPA);
+		val = sungem_phy_read(phy, MII_LPA);
 		phy->pause = (phy->duplex == DUPLEX_FULL) &&
 			((val & LPA_PAUSE) != 0);
 	}
@@ -575,19 +575,19 @@ static int marvell88e1111_init(struct mii_phy* phy)
 	u16 rev;
 
 	/* magic init sequence for rev 0 */
-	rev = phy_read(phy, MII_PHYSID2) & 0x000f;
+	rev = sungem_phy_read(phy, MII_PHYSID2) & 0x000f;
 	if (rev == 0) {
-		phy_write(phy, 0x1d, 0x000a);
-		phy_write(phy, 0x1e, 0x0821);
+		sungem_phy_write(phy, 0x1d, 0x000a);
+		sungem_phy_write(phy, 0x1e, 0x0821);
 
-		phy_write(phy, 0x1d, 0x0006);
-		phy_write(phy, 0x1e, 0x8600);
+		sungem_phy_write(phy, 0x1d, 0x0006);
+		sungem_phy_write(phy, 0x1e, 0x8600);
 
-		phy_write(phy, 0x1d, 0x000b);
-		phy_write(phy, 0x1e, 0x0100);
+		sungem_phy_write(phy, 0x1d, 0x000b);
+		sungem_phy_write(phy, 0x1e, 0x0100);
 
-		phy_write(phy, 0x1d, 0x0004);
-		phy_write(phy, 0x1e, 0x4850);
+		sungem_phy_write(phy, 0x1d, 0x0004);
+		sungem_phy_write(phy, 0x1e, 0x4850);
 	}
 	return 0;
 }
@@ -600,8 +600,8 @@ static int bcm5421_poll_link(struct mii_phy* phy)
 	int mode;
 
 	/* find out in what mode we are */
-	phy_write(phy, MII_NCONFIG, 0x1000);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x1000);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	mode = (phy_reg & BCM5421_MODE_MASK) >> 5;
 
@@ -609,8 +609,8 @@ static int bcm5421_poll_link(struct mii_phy* phy)
 		return genmii_poll_link(phy);
 
 	/* try to find out whether we have a link */
-	phy_write(phy, MII_NCONFIG, 0x2000);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x2000);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	if (phy_reg & 0x0020)
 		return 0;
@@ -624,8 +624,8 @@ static int bcm5421_read_link(struct mii_phy* phy)
 	int mode;
 
 	/* find out in what mode we are */
-	phy_write(phy, MII_NCONFIG, 0x1000);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x1000);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	mode = (phy_reg & BCM5421_MODE_MASK ) >> 5;
 
@@ -635,8 +635,8 @@ static int bcm5421_read_link(struct mii_phy* phy)
 	phy->speed = SPEED_1000;
 
 	/* find out whether we are running half- or full duplex */
-	phy_write(phy, MII_NCONFIG, 0x2000);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x2000);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	if ( (phy_reg & 0x0080) >> 7)
 		phy->duplex |=  DUPLEX_HALF;
@@ -649,14 +649,14 @@ static int bcm5421_read_link(struct mii_phy* phy)
 static int bcm5421_enable_fiber(struct mii_phy* phy, int autoneg)
 {
 	/* enable fiber mode */
-	phy_write(phy, MII_NCONFIG, 0x9020);
+	sungem_phy_write(phy, MII_NCONFIG, 0x9020);
 	/* LEDs active in both modes, autosense prio = fiber */
-	phy_write(phy, MII_NCONFIG, 0x945f);
+	sungem_phy_write(phy, MII_NCONFIG, 0x945f);
 
 	if (!autoneg) {
 		/* switch off fibre autoneg */
-		phy_write(phy, MII_NCONFIG, 0xfc01);
-		phy_write(phy, 0x0b, 0x0004);
+		sungem_phy_write(phy, MII_NCONFIG, 0xfc01);
+		sungem_phy_write(phy, 0x0b, 0x0004);
 	}
 
 	phy->autoneg = autoneg;
@@ -673,8 +673,8 @@ static int bcm5461_poll_link(struct mii_phy* phy)
 	int mode;
 
 	/* find out in what mode we are */
-	phy_write(phy, MII_NCONFIG, 0x7c00);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x7c00);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	mode = (phy_reg & BCM5461_MODE_MASK ) >> 1;
 
@@ -682,8 +682,8 @@ static int bcm5461_poll_link(struct mii_phy* phy)
 		return genmii_poll_link(phy);
 
 	/* find out whether we have a link */
-	phy_write(phy, MII_NCONFIG, 0x7000);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x7000);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	if (phy_reg & BCM5461_FIBER_LINK)
 		return 1;
@@ -699,8 +699,8 @@ static int bcm5461_read_link(struct mii_phy* phy)
 	int mode;
 
 	/* find out in what mode we are */
-	phy_write(phy, MII_NCONFIG, 0x7c00);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x7c00);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	mode = (phy_reg & BCM5461_MODE_MASK ) >> 1;
 
@@ -711,8 +711,8 @@ static int bcm5461_read_link(struct mii_phy* phy)
 	phy->speed = SPEED_1000;
 
 	/* find out whether we are running half- or full duplex */
-	phy_write(phy, MII_NCONFIG, 0x7000);
-	phy_reg = phy_read(phy, MII_NCONFIG);
+	sungem_phy_write(phy, MII_NCONFIG, 0x7000);
+	phy_reg = sungem_phy_read(phy, MII_NCONFIG);
 
 	if (phy_reg & BCM5461_FIBER_DUPLEX)
 		phy->duplex |=  DUPLEX_FULL;
@@ -725,15 +725,15 @@ static int bcm5461_read_link(struct mii_phy* phy)
 static int bcm5461_enable_fiber(struct mii_phy* phy, int autoneg)
 {
 	/* select fiber mode, enable 1000 base-X registers */
-	phy_write(phy, MII_NCONFIG, 0xfc0b);
+	sungem_phy_write(phy, MII_NCONFIG, 0xfc0b);
 
 	if (autoneg) {
 		/* enable fiber with no autonegotiation */
-		phy_write(phy, MII_ADVERTISE, 0x01e0);
-		phy_write(phy, MII_BMCR, 0x1140);
+		sungem_phy_write(phy, MII_ADVERTISE, 0x01e0);
+		sungem_phy_write(phy, MII_BMCR, 0x1140);
 	} else {
 		/* enable fiber with autonegotiation */
-		phy_write(phy, MII_BMCR, 0x0140);
+		sungem_phy_write(phy, MII_BMCR, 0x0140);
 	}
 
 	phy->autoneg = autoneg;
@@ -752,7 +752,7 @@ static int marvell_setup_aneg(struct mii_phy *phy, u32 advertise)
 	phy->advertising = advertise;
 
 	/* Setup standard advertise */
-	adv = phy_read(phy, MII_ADVERTISE);
+	adv = sungem_phy_read(phy, MII_ADVERTISE);
 	adv &= ~(ADVERTISE_ALL | ADVERTISE_100BASE4);
 	if (advertise & ADVERTISED_10baseT_Half)
 		adv |= ADVERTISE_10HALF;
@@ -766,7 +766,7 @@ static int marvell_setup_aneg(struct mii_phy *phy, u32 advertise)
 		adv |= ADVERTISE_PAUSE_CAP;
 	if (advertise & ADVERTISED_Asym_Pause)
 		adv |= ADVERTISE_PAUSE_ASYM;
-	phy_write(phy, MII_ADVERTISE, adv);
+	sungem_phy_write(phy, MII_ADVERTISE, adv);
 
 	/* Setup 1000BT advertise & enable crossover detect
 	 * XXX How do we advertise 1000BT ? Darwin source is
@@ -774,7 +774,7 @@ static int marvell_setup_aneg(struct mii_phy *phy, u32 advertise)
 	 * write to control... Someone has specs for those
 	 * beasts ?
 	 */
-	adv = phy_read(phy, MII_M1011_PHY_SPEC_CONTROL);
+	adv = sungem_phy_read(phy, MII_M1011_PHY_SPEC_CONTROL);
 	adv |= MII_M1011_PHY_SPEC_CONTROL_AUTO_MDIX;
 	adv &= ~(MII_1000BASETCONTROL_FULLDUPLEXCAP |
 			MII_1000BASETCONTROL_HALFDUPLEXCAP);
@@ -782,12 +782,12 @@ static int marvell_setup_aneg(struct mii_phy *phy, u32 advertise)
 		adv |= MII_1000BASETCONTROL_HALFDUPLEXCAP;
 	if (advertise & SUPPORTED_1000baseT_Full)
 		adv |= MII_1000BASETCONTROL_FULLDUPLEXCAP;
-	phy_write(phy, MII_1000BASETCONTROL, adv);
+	sungem_phy_write(phy, MII_1000BASETCONTROL, adv);
 
 	/* Start/Restart aneg */
-	ctl = phy_read(phy, MII_BMCR);
+	ctl = sungem_phy_read(phy, MII_BMCR);
 	ctl |= (BMCR_ANENABLE | BMCR_ANRESTART);
-	phy_write(phy, MII_BMCR, ctl);
+	sungem_phy_write(phy, MII_BMCR, ctl);
 
 	return 0;
 }
@@ -801,7 +801,7 @@ static int marvell_setup_forced(struct mii_phy *phy, int speed, int fd)
 	phy->duplex = fd;
 	phy->pause = 0;
 
-	ctl = phy_read(phy, MII_BMCR);
+	ctl = sungem_phy_read(phy, MII_BMCR);
 	ctl &= ~(BMCR_FULLDPLX|BMCR_SPEED100|BMCR_SPD2|BMCR_ANENABLE);
 	ctl |= BMCR_RESET;
 
@@ -824,7 +824,7 @@ static int marvell_setup_forced(struct mii_phy *phy, int speed, int fd)
 	/* Disable crossover. Again, the way Apple does it is strange,
 	 * though I don't assume they are wrong ;)
 	 */
-	ctl2 = phy_read(phy, MII_M1011_PHY_SPEC_CONTROL);
+	ctl2 = sungem_phy_read(phy, MII_M1011_PHY_SPEC_CONTROL);
 	ctl2 &= ~(MII_M1011_PHY_SPEC_CONTROL_MANUAL_MDIX |
 		MII_M1011_PHY_SPEC_CONTROL_AUTO_MDIX |
 		MII_1000BASETCONTROL_FULLDUPLEXCAP |
@@ -833,11 +833,11 @@ static int marvell_setup_forced(struct mii_phy *phy, int speed, int fd)
 		ctl2 |= (fd == DUPLEX_FULL) ?
 			MII_1000BASETCONTROL_FULLDUPLEXCAP :
 			MII_1000BASETCONTROL_HALFDUPLEXCAP;
-	phy_write(phy, MII_1000BASETCONTROL, ctl2);
+	sungem_phy_write(phy, MII_1000BASETCONTROL, ctl2);
 
 	// XXX Should we set the sungem to GII now on 1000BT ?
 
-	phy_write(phy, MII_BMCR, ctl);
+	sungem_phy_write(phy, MII_BMCR, ctl);
 
 	return 0;
 }
@@ -847,7 +847,7 @@ static int marvell_read_link(struct mii_phy *phy)
 	u16 status, pmask;
 
 	if (phy->autoneg) {
-		status = phy_read(phy, MII_M1011_PHY_SPEC_STATUS);
+		status = sungem_phy_read(phy, MII_M1011_PHY_SPEC_STATUS);
 		if ((status & MII_M1011_PHY_SPEC_STATUS_RESOLVED) == 0)
 			return -EAGAIN;
 		if (status & MII_M1011_PHY_SPEC_STATUS_1000)
@@ -1174,7 +1174,7 @@ int sungem_phy_probe(struct mii_phy *phy, int mii_id)
 		goto fail;
 
 	/* Read ID and find matching entry */
-	id = (phy_read(phy, MII_PHYSID1) << 16 | phy_read(phy, MII_PHYSID2));
+	id = (sungem_phy_read(phy, MII_PHYSID1) << 16 | sungem_phy_read(phy, MII_PHYSID2));
 	printk(KERN_DEBUG KBUILD_MODNAME ": " "PHY ID: %x, addr: %x\n",
 	       id, mii_id);
 	for (i=0; (def = mii_phy_table[i]) != NULL; i++)
diff --git a/drivers/net/team/team.c b/drivers/net/team/team.c
index ef10302ec936..2368395d8ae5 100644
--- a/drivers/net/team/team.c
+++ b/drivers/net/team/team.c
@@ -647,7 +647,7 @@ static void team_notify_peers(struct team *team)
 {
 	if (!team->notify_peers.count || !netif_running(team->dev))
 		return;
-	atomic_set(&team->notify_peers.count_pending, team->notify_peers.count);
+	atomic_add(team->notify_peers.count, &team->notify_peers.count_pending);
 	schedule_delayed_work(&team->notify_peers.dw, 0);
 }
 
@@ -687,7 +687,7 @@ static void team_mcast_rejoin(struct team *team)
 {
 	if (!team->mcast_rejoin.count || !netif_running(team->dev))
 		return;
-	atomic_set(&team->mcast_rejoin.count_pending, team->mcast_rejoin.count);
+	atomic_add(team->mcast_rejoin.count, &team->mcast_rejoin.count_pending);
 	schedule_delayed_work(&team->mcast_rejoin.dw, 0);
 }
 
@@ -970,7 +970,8 @@ static void __team_compute_features(struct team *team)
 	struct team_port *port;
 	u32 vlan_features = TEAM_VLAN_FEATURES & NETIF_F_ALL_FOR_ALL;
 	unsigned short max_hard_header_len = ETH_HLEN;
-	unsigned int flags, dst_release_flag = IFF_XMIT_DST_RELEASE;
+	unsigned int dst_release_flag = IFF_XMIT_DST_RELEASE |
+					IFF_XMIT_DST_RELEASE_PERM;
 
 	list_for_each_entry(port, &team->port_list, list) {
 		vlan_features = netdev_increment_features(vlan_features,
@@ -985,8 +986,9 @@ static void __team_compute_features(struct team *team)
 	team->dev->vlan_features = vlan_features;
 	team->dev->hard_header_len = max_hard_header_len;
 
-	flags = team->dev->priv_flags & ~IFF_XMIT_DST_RELEASE;
-	team->dev->priv_flags = flags | dst_release_flag;
+	team->dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
+	if (dst_release_flag == (IFF_XMIT_DST_RELEASE | IFF_XMIT_DST_RELEASE_PERM))
+		team->dev->priv_flags |= IFF_XMIT_DST_RELEASE;
 
 	netdev_change_features(team->dev);
 }
@@ -1003,7 +1005,6 @@ static int team_port_enter(struct team *team, struct team_port *port)
 	int err = 0;
 
 	dev_hold(team->dev);
-	port->dev->priv_flags |= IFF_TEAM_PORT;
 	if (team->ops.port_enter) {
 		err = team->ops.port_enter(team, port);
 		if (err) {
@@ -1016,7 +1017,6 @@ static int team_port_enter(struct team *team, struct team_port *port)
 	return 0;
 
 err_port_enter:
-	port->dev->priv_flags &= ~IFF_TEAM_PORT;
 	dev_put(team->dev);
 
 	return err;
@@ -1026,7 +1026,6 @@ static void team_port_leave(struct team *team, struct team_port *port)
 {
 	if (team->ops.port_leave)
 		team->ops.port_leave(team, port);
-	port->dev->priv_flags &= ~IFF_TEAM_PORT;
 	dev_put(team->dev);
 }
 
@@ -1075,6 +1074,25 @@ static void team_port_disable_netpoll(struct team_port *port)
 }
 #endif
 
+static int team_upper_dev_link(struct net_device *dev,
+			       struct net_device *port_dev)
+{
+	int err;
+
+	err = netdev_master_upper_dev_link(port_dev, dev);
+	if (err)
+		return err;
+	port_dev->priv_flags |= IFF_TEAM_PORT;
+	return 0;
+}
+
+static void team_upper_dev_unlink(struct net_device *dev,
+				  struct net_device *port_dev)
+{
+	netdev_upper_dev_unlink(port_dev, dev);
+	port_dev->priv_flags &= ~IFF_TEAM_PORT;
+}
+
 static void __team_port_change_port_added(struct team_port *port, bool linkup);
 static int team_dev_type_check_change(struct net_device *dev,
 				      struct net_device *port_dev);
@@ -1161,13 +1179,6 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 		goto err_enable_netpoll;
 	}
 
-	err = netdev_master_upper_dev_link(port_dev, dev);
-	if (err) {
-		netdev_err(dev, "Device %s failed to set upper link\n",
-			   portname);
-		goto err_set_upper_link;
-	}
-
 	err = netdev_rx_handler_register(port_dev, team_handle_frame,
 					 port);
 	if (err) {
@@ -1176,6 +1187,13 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 		goto err_handler_register;
 	}
 
+	err = team_upper_dev_link(dev, port_dev);
+	if (err) {
+		netdev_err(dev, "Device %s failed to set upper link\n",
+			   portname);
+		goto err_set_upper_link;
+	}
+
 	err = __team_option_inst_add_port(team, port);
 	if (err) {
 		netdev_err(dev, "Device %s failed to add per-port options\n",
@@ -1195,12 +1213,12 @@ static int team_port_add(struct team *team, struct net_device *port_dev)
 	return 0;
 
 err_option_port_add:
+	team_upper_dev_unlink(dev, port_dev);
+
+err_set_upper_link:
 	netdev_rx_handler_unregister(port_dev);
 
 err_handler_register:
-	netdev_upper_dev_unlink(port_dev, dev);
-
-err_set_upper_link:
 	team_port_disable_netpoll(port);
 
 err_enable_netpoll:
@@ -1239,8 +1257,8 @@ static int team_port_del(struct team *team, struct net_device *port_dev)
 
 	team_port_disable(team, port);
 	list_del_rcu(&port->list);
+	team_upper_dev_unlink(dev, port_dev);
 	netdev_rx_handler_unregister(port_dev);
-	netdev_upper_dev_unlink(port_dev, dev);
 	team_port_disable_netpoll(port);
 	vlan_vids_del_by_dev(port_dev, dev);
 	dev_uc_unsync(port_dev, dev);
diff --git a/drivers/net/tun.c b/drivers/net/tun.c
index acaaf6784179..186ce541c657 100644
--- a/drivers/net/tun.c
+++ b/drivers/net/tun.c
@@ -2152,9 +2152,7 @@ static int tun_chr_fasync(int fd, struct file *file, int on)
 		goto out;
 
 	if (on) {
-		ret = __f_setown(file, task_pid(current), PIDTYPE_PID, 0);
-		if (ret)
-			goto out;
+		__f_setown(file, task_pid(current), PIDTYPE_PID, 0);
 		tfile->flags |= TUN_FASYNC;
 	} else
 		tfile->flags &= ~TUN_FASYNC;
diff --git a/drivers/net/usb/asix_devices.c b/drivers/net/usb/asix_devices.c
index 5d194093f3e1..2c05f6cdb12f 100644
--- a/drivers/net/usb/asix_devices.c
+++ b/drivers/net/usb/asix_devices.c
@@ -890,7 +890,7 @@ static const struct driver_info ax88772_info = {
 	.unbind = ax88772_unbind,
 	.status = asix_status,
 	.link_reset = ax88772_link_reset,
-	.reset = ax88772_reset,
+	.reset = ax88772_link_reset,
 	.flags = FLAG_ETHER | FLAG_FRAMING_AX | FLAG_LINK_INTR | FLAG_MULTI_PACKET,
 	.rx_fixup = asix_rx_fixup_common,
 	.tx_fixup = asix_tx_fixup,
diff --git a/drivers/net/usb/r8152.c b/drivers/net/usb/r8152.c
index 87f710476217..e3d84c322e4e 100644
--- a/drivers/net/usb/r8152.c
+++ b/drivers/net/usb/r8152.c
@@ -22,9 +22,11 @@
 #include <linux/ip.h>
 #include <linux/ipv6.h>
 #include <net/ip6_checksum.h>
+#include <uapi/linux/mdio.h>
+#include <linux/mdio.h>
 
 /* Version Information */
-#define DRIVER_VERSION "v1.06.0 (2014/03/03)"
+#define DRIVER_VERSION "v1.07.0 (2014/10/09)"
 #define DRIVER_AUTHOR "Realtek linux nic maintainers <nic_swsd@realtek.com>"
 #define DRIVER_DESC "Realtek RTL8152/RTL8153 Based USB Ethernet Adapters"
 #define MODULENAME "r8152"
@@ -129,7 +131,9 @@
 #define OCP_SRAM_ADDR		0xa436
 #define OCP_SRAM_DATA		0xa438
 #define OCP_DOWN_SPEED		0xa442
-#define OCP_EEE_CFG2		0xa5d0
+#define OCP_EEE_ABLE		0xa5c4
+#define OCP_EEE_ADV		0xa5d0
+#define OCP_EEE_LPABLE		0xa5d2
 #define OCP_ADC_CFG		0xbc06
 
 /* SRAM Register */
@@ -361,7 +365,8 @@
 #define EEE_NWAY_EN		0x1000
 #define TX_QUIET_EN		0x0200
 #define RX_QUIET_EN		0x0100
-#define SDRISETIME		0x0010	/* bit 4 ~ 6 */
+#define sd_rise_time_mask	0x0070
+#define sd_rise_time(x)		(min(x, 7) << 4)	/* bit 4 ~ 6 */
 #define RG_RXLPI_MSK_HFDUP	0x0008
 #define SDFALLTIME		0x0007	/* bit 0 ~ 2 */
 
@@ -373,7 +378,8 @@
 #define RG_EEEPRG_EN		0x0010
 
 /* OCP_EEE_CONFIG3 */
-#define FST_SNR_EYE_R		0x1500	/* bit 7 ~ 15 */
+#define fast_snr_mask		0xff80
+#define fast_snr(x)		(min(x, 0x1ff) << 7)	/* bit 7 ~ 15 */
 #define RG_LFS_SEL		0x0060	/* bit 6 ~ 5 */
 #define MSK_PH			0x0006	/* bit 0 ~ 3 */
 
@@ -382,11 +388,6 @@
 #define FUN_ADDR		0x0000
 #define FUN_DATA		0x4000
 /* bit[4:0] device addr */
-#define DEVICE_ADDR		0x0007
-
-/* OCP_EEE_DATA */
-#define EEE_ADDR		0x003C
-#define EEE_DATA		0x0002
 
 /* OCP_EEE_CFG */
 #define CTAP_SHORT_EN		0x0040
@@ -395,10 +396,6 @@
 /* OCP_DOWN_SPEED */
 #define EN_10M_BGOFF		0x0080
 
-/* OCP_EEE_CFG2 */
-#define MY1000_EEE		0x0004
-#define MY100_EEE		0x0002
-
 /* OCP_ADC_CFG */
 #define CKADSEL_L		0x0100
 #define ADC_EN			0x0080
@@ -424,7 +421,7 @@ enum rtl_register_content {
 	FULL_DUP	= 0x01,
 };
 
-#define RTL8152_MAX_TX		10
+#define RTL8152_MAX_TX		4
 #define RTL8152_MAX_RX		10
 #define INTBUFSIZE		2
 #define CRC_SIZE		4
@@ -506,6 +503,7 @@ struct rx_desc {
 #define IPF				(1 << 23) /* IP checksum fail */
 #define UDPF				(1 << 22) /* UDP checksum fail */
 #define TCPF				(1 << 21) /* TCP checksum fail */
+#define RX_VLAN_TAG			(1 << 16)
 
 	__le32 opts4;
 	__le32 opts5;
@@ -531,6 +529,7 @@ struct tx_desc {
 #define MSS_MAX			0x7ffU
 #define TCPHO_SHIFT		17
 #define TCPHO_MAX		0x7ffU
+#define TX_VLAN_TAG			(1 << 16)
 };
 
 struct r8152;
@@ -567,6 +566,7 @@ struct r8152 {
 	spinlock_t rx_lock, tx_lock;
 	struct delayed_work schedule;
 	struct mii_if_info mii;
+	struct mutex control;	/* use for hw setting */
 
 	struct rtl_ops {
 		void (*init)(struct r8152 *);
@@ -575,6 +575,8 @@ struct r8152 {
 		void (*up)(struct r8152 *);
 		void (*down)(struct r8152 *);
 		void (*unload)(struct r8152 *);
+		int (*eee_get)(struct r8152 *, struct ethtool_eee *);
+		int (*eee_set)(struct r8152 *, struct ethtool_eee *);
 	} rtl_ops;
 
 	int intr_interval;
@@ -607,9 +609,9 @@ enum tx_csum_stat {
  * The RTL chips use a 64 element hash table based on the Ethernet CRC.
  */
 static const int multicast_filter_limit = 32;
-static unsigned int rx_buf_sz = 16384;
+static unsigned int agg_buf_sz = 16384;
 
-#define RTL_LIMITED_TSO_SIZE	(rx_buf_sz - sizeof(struct tx_desc) - \
+#define RTL_LIMITED_TSO_SIZE	(agg_buf_sz - sizeof(struct tx_desc) - \
 				 VLAN_ETH_HLEN - VLAN_HLEN)
 
 static
@@ -623,8 +625,8 @@ int get_registers(struct r8152 *tp, u16 value, u16 index, u16 size, void *data)
 		return -ENOMEM;
 
 	ret = usb_control_msg(tp->udev, usb_rcvctrlpipe(tp->udev, 0),
-			       RTL8152_REQ_GET_REGS, RTL8152_REQT_READ,
-			       value, index, tmp, size, 500);
+			      RTL8152_REQ_GET_REGS, RTL8152_REQT_READ,
+			      value, index, tmp, size, 500);
 
 	memcpy(data, tmp, size);
 	kfree(tmp);
@@ -643,8 +645,8 @@ int set_registers(struct r8152 *tp, u16 value, u16 index, u16 size, void *data)
 		return -ENOMEM;
 
 	ret = usb_control_msg(tp->udev, usb_sndctrlpipe(tp->udev, 0),
-			       RTL8152_REQ_SET_REGS, RTL8152_REQT_WRITE,
-			       value, index, tmp, size, 500);
+			      RTL8152_REQ_SET_REGS, RTL8152_REQT_WRITE,
+			      value, index, tmp, size, 500);
 
 	kfree(tmp);
 
@@ -652,7 +654,7 @@ int set_registers(struct r8152 *tp, u16 value, u16 index, u16 size, void *data)
 }
 
 static int generic_ocp_read(struct r8152 *tp, u16 index, u16 size,
-				void *data, u16 type)
+			    void *data, u16 type)
 {
 	u16 limit = 64;
 	int ret = 0;
@@ -692,7 +694,7 @@ static int generic_ocp_read(struct r8152 *tp, u16 index, u16 size,
 }
 
 static int generic_ocp_write(struct r8152 *tp, u16 index, u16 byteen,
-				u16 size, void *data, u16 type)
+			     u16 size, void *data, u16 type)
 {
 	int ret;
 	u16 byteen_start, byteen_end, byen;
@@ -726,8 +728,8 @@ static int generic_ocp_write(struct r8152 *tp, u16 index, u16 byteen,
 		while (size) {
 			if (size > limit) {
 				ret = set_registers(tp, index,
-					type | BYTE_EN_DWORD,
-					limit, data);
+						    type | BYTE_EN_DWORD,
+						    limit, data);
 				if (ret < 0)
 					goto error1;
 
@@ -736,8 +738,8 @@ static int generic_ocp_write(struct r8152 *tp, u16 index, u16 byteen,
 				size -= limit;
 			} else {
 				ret = set_registers(tp, index,
-					type | BYTE_EN_DWORD,
-					size, data);
+						    type | BYTE_EN_DWORD,
+						    size, data);
 				if (ret < 0)
 					goto error1;
 
@@ -941,15 +943,8 @@ static int read_mii_word(struct net_device *netdev, int phy_id, int reg)
 	if (phy_id != R8152_PHY_ID)
 		return -EINVAL;
 
-	ret = usb_autopm_get_interface(tp->intf);
-	if (ret < 0)
-		goto out;
-
 	ret = r8152_mdio_read(tp, reg);
 
-	usb_autopm_put_interface(tp->intf);
-
-out:
 	return ret;
 }
 
@@ -964,52 +959,26 @@ void write_mii_word(struct net_device *netdev, int phy_id, int reg, int val)
 	if (phy_id != R8152_PHY_ID)
 		return;
 
-	if (usb_autopm_get_interface(tp->intf) < 0)
-		return;
-
 	r8152_mdio_write(tp, reg, val);
-
-	usb_autopm_put_interface(tp->intf);
 }
 
-static
-int r8152_submit_rx(struct r8152 *tp, struct rx_agg *agg, gfp_t mem_flags);
-
-static inline void set_ethernet_addr(struct r8152 *tp)
-{
-	struct net_device *dev = tp->netdev;
-	int ret;
-	u8 node_id[8] = {0};
-
-	if (tp->version == RTL_VER_01)
-		ret = pla_ocp_read(tp, PLA_IDR, sizeof(node_id), node_id);
-	else
-		ret = pla_ocp_read(tp, PLA_BACKUP, sizeof(node_id), node_id);
-
-	if (ret < 0) {
-		netif_notice(tp, probe, dev, "inet addr fail\n");
-	} else {
-		if (tp->version != RTL_VER_01) {
-			ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CRWECR,
-				       CRWECR_CONFIG);
-			pla_ocp_write(tp, PLA_IDR, BYTE_EN_SIX_BYTES,
-				      sizeof(node_id), node_id);
-			ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CRWECR,
-				       CRWECR_NORAML);
-		}
-
-		memcpy(dev->dev_addr, node_id, dev->addr_len);
-		memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
-	}
-}
+static int
+r8152_submit_rx(struct r8152 *tp, struct rx_agg *agg, gfp_t mem_flags);
 
 static int rtl8152_set_mac_address(struct net_device *netdev, void *p)
 {
 	struct r8152 *tp = netdev_priv(netdev);
 	struct sockaddr *addr = p;
+	int ret = -EADDRNOTAVAIL;
 
 	if (!is_valid_ether_addr(addr->sa_data))
-		return -EADDRNOTAVAIL;
+		goto out1;
+
+	ret = usb_autopm_get_interface(tp->intf);
+	if (ret < 0)
+		goto out1;
+
+	mutex_lock(&tp->control);
 
 	memcpy(netdev->dev_addr, addr->sa_data, netdev->addr_len);
 
@@ -1017,7 +986,42 @@ static int rtl8152_set_mac_address(struct net_device *netdev, void *p)
 	pla_ocp_write(tp, PLA_IDR, BYTE_EN_SIX_BYTES, 8, addr->sa_data);
 	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_CRWECR, CRWECR_NORAML);
 
-	return 0;
+	mutex_unlock(&tp->control);
+
+	usb_autopm_put_interface(tp->intf);
+out1:
+	return ret;
+}
+
+static int set_ethernet_addr(struct r8152 *tp)
+{
+	struct net_device *dev = tp->netdev;
+	struct sockaddr sa;
+	int ret;
+
+	if (tp->version == RTL_VER_01)
+		ret = pla_ocp_read(tp, PLA_IDR, 8, sa.sa_data);
+	else
+		ret = pla_ocp_read(tp, PLA_BACKUP, 8, sa.sa_data);
+
+	if (ret < 0) {
+		netif_err(tp, probe, dev, "Get ether addr fail\n");
+	} else if (!is_valid_ether_addr(sa.sa_data)) {
+		netif_err(tp, probe, dev, "Invalid ether addr %pM\n",
+			  sa.sa_data);
+		eth_hw_addr_random(dev);
+		ether_addr_copy(sa.sa_data, dev->dev_addr);
+		ret = rtl8152_set_mac_address(dev, &sa);
+		netif_info(tp, probe, dev, "Random ether addr %pM\n",
+			   sa.sa_data);
+	} else {
+		if (tp->version == RTL_VER_01)
+			ether_addr_copy(dev->dev_addr, sa.sa_data);
+		else
+			ret = rtl8152_set_mac_address(dev, &sa);
+	}
+
+	return ret;
 }
 
 static void read_bulk_callback(struct urb *urb)
@@ -1248,13 +1252,13 @@ static int alloc_all_mem(struct r8152 *tp)
 	skb_queue_head_init(&tp->tx_queue);
 
 	for (i = 0; i < RTL8152_MAX_RX; i++) {
-		buf = kmalloc_node(rx_buf_sz, GFP_KERNEL, node);
+		buf = kmalloc_node(agg_buf_sz, GFP_KERNEL, node);
 		if (!buf)
 			goto err1;
 
 		if (buf != rx_agg_align(buf)) {
 			kfree(buf);
-			buf = kmalloc_node(rx_buf_sz + RX_ALIGN, GFP_KERNEL,
+			buf = kmalloc_node(agg_buf_sz + RX_ALIGN, GFP_KERNEL,
 					   node);
 			if (!buf)
 				goto err1;
@@ -1274,13 +1278,13 @@ static int alloc_all_mem(struct r8152 *tp)
 	}
 
 	for (i = 0; i < RTL8152_MAX_TX; i++) {
-		buf = kmalloc_node(rx_buf_sz, GFP_KERNEL, node);
+		buf = kmalloc_node(agg_buf_sz, GFP_KERNEL, node);
 		if (!buf)
 			goto err1;
 
 		if (buf != tx_agg_align(buf)) {
 			kfree(buf);
-			buf = kmalloc_node(rx_buf_sz + TX_ALIGN, GFP_KERNEL,
+			buf = kmalloc_node(agg_buf_sz + TX_ALIGN, GFP_KERNEL,
 					   node);
 			if (!buf)
 				goto err1;
@@ -1311,8 +1315,8 @@ static int alloc_all_mem(struct r8152 *tp)
 
 	tp->intr_interval = (int)ep_intr->desc.bInterval;
 	usb_fill_int_urb(tp->intr_urb, tp->udev, usb_rcvintpipe(tp->udev, 3),
-		     tp->intr_buff, INTBUFSIZE, intr_callback,
-		     tp, tp->intr_interval);
+			 tp->intr_buff, INTBUFSIZE, intr_callback,
+			 tp, tp->intr_interval);
 
 	return 0;
 
@@ -1354,8 +1358,7 @@ static inline __be16 get_protocol(struct sk_buff *skb)
 	return protocol;
 }
 
-/*
- * r8152_csum_workaround()
+/* r8152_csum_workaround()
  * The hw limites the value the transport offset. When the offset is out of the
  * range, calculate the checksum by sw.
  */
@@ -1398,8 +1401,7 @@ drop:
 	}
 }
 
-/*
- * msdn_giant_send_check()
+/* msdn_giant_send_check()
  * According to the document of microsoft, the TCP Pseudo Header excludes the
  * packet length for IPv6 TCP large packets.
  */
@@ -1422,6 +1424,25 @@ static int msdn_giant_send_check(struct sk_buff *skb)
 	return ret;
 }
 
+static inline void rtl_tx_vlan_tag(struct tx_desc *desc, struct sk_buff *skb)
+{
+	if (vlan_tx_tag_present(skb)) {
+		u32 opts2;
+
+		opts2 = TX_VLAN_TAG | swab16(vlan_tx_tag_get(skb));
+		desc->opts2 |= cpu_to_le32(opts2);
+	}
+}
+
+static inline void rtl_rx_vlan_tag(struct rx_desc *desc, struct sk_buff *skb)
+{
+	u32 opts2 = le32_to_cpu(desc->opts2);
+
+	if (opts2 & RX_VLAN_TAG)
+		__vlan_hwaccel_put_tag(skb, htons(ETH_P_8021Q),
+				       swab16(opts2 & 0xffff));
+}
+
 static int r8152_tx_csum(struct r8152 *tp, struct tx_desc *desc,
 			 struct sk_buff *skb, u32 len, u32 transport_offset)
 {
@@ -1518,8 +1539,9 @@ static int r8152_tx_agg_fill(struct r8152 *tp, struct tx_agg *agg)
 	spin_unlock(&tx_queue->lock);
 
 	tx_data = agg->head;
-	agg->skb_num = agg->skb_len = 0;
-	remain = rx_buf_sz;
+	agg->skb_num = 0;
+	agg->skb_len = 0;
+	remain = agg_buf_sz;
 
 	while (remain >= ETH_ZLEN + sizeof(struct tx_desc)) {
 		struct tx_desc *tx_desc;
@@ -1548,6 +1570,8 @@ static int r8152_tx_agg_fill(struct r8152 *tp, struct tx_agg *agg)
 			continue;
 		}
 
+		rtl_tx_vlan_tag(tx_desc, skb);
+
 		tx_data += sizeof(*tx_desc);
 
 		len = skb->len;
@@ -1566,7 +1590,7 @@ static int r8152_tx_agg_fill(struct r8152 *tp, struct tx_agg *agg)
 
 		dev_kfree_skb_any(skb);
 
-		remain = rx_buf_sz - (int)(tx_agg_align(tx_data) - agg->head);
+		remain = agg_buf_sz - (int)(tx_agg_align(tx_data) - agg->head);
 	}
 
 	if (!skb_queue_empty(&skb_head)) {
@@ -1689,6 +1713,7 @@ static void rx_bottom(struct r8152 *tp)
 			memcpy(skb->data, rx_data, pkt_len);
 			skb_put(skb, pkt_len);
 			skb->protocol = eth_type_trans(skb, netdev);
+			rtl_rx_vlan_tag(rx_desc, skb);
 			netif_receive_skb(skb);
 			stats->rx_packets++;
 			stats->rx_bytes += pkt_len;
@@ -1772,8 +1797,8 @@ static
 int r8152_submit_rx(struct r8152 *tp, struct rx_agg *agg, gfp_t mem_flags)
 {
 	usb_fill_bulk_urb(agg->urb, tp->udev, usb_rcvbulkpipe(tp->udev, 1),
-		      agg->head, rx_buf_sz,
-		      (usb_complete_t)read_bulk_callback, agg);
+			  agg->head, agg_buf_sz,
+			  (usb_complete_t)read_bulk_callback, agg);
 
 	return usb_submit_urb(agg->urb, mem_flags);
 }
@@ -1835,18 +1860,22 @@ static void _rtl8152_set_rx_mode(struct net_device *netdev)
 		/* Unconditionally log net taps. */
 		netif_notice(tp, link, netdev, "Promiscuous mode enabled\n");
 		ocp_data |= RCR_AM | RCR_AAP;
-		mc_filter[1] = mc_filter[0] = 0xffffffff;
+		mc_filter[1] = 0xffffffff;
+		mc_filter[0] = 0xffffffff;
 	} else if ((netdev_mc_count(netdev) > multicast_filter_limit) ||
 		   (netdev->flags & IFF_ALLMULTI)) {
 		/* Too many to filter perfectly -- accept all multicasts. */
 		ocp_data |= RCR_AM;
-		mc_filter[1] = mc_filter[0] = 0xffffffff;
+		mc_filter[1] = 0xffffffff;
+		mc_filter[0] = 0xffffffff;
 	} else {
 		struct netdev_hw_addr *ha;
 
-		mc_filter[1] = mc_filter[0] = 0;
+		mc_filter[1] = 0;
+		mc_filter[0] = 0;
 		netdev_for_each_mc_addr(ha, netdev) {
 			int bit_nr = ether_crc(ETH_ALEN, ha->addr) >> 26;
+
 			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
 			ocp_data |= RCR_AM;
 		}
@@ -1861,7 +1890,7 @@ static void _rtl8152_set_rx_mode(struct net_device *netdev)
 }
 
 static netdev_tx_t rtl8152_start_xmit(struct sk_buff *skb,
-					struct net_device *netdev)
+				      struct net_device *netdev)
 {
 	struct r8152 *tp = netdev_priv(netdev);
 
@@ -1877,8 +1906,9 @@ static netdev_tx_t rtl8152_start_xmit(struct sk_buff *skb,
 			usb_mark_last_busy(tp->udev);
 			tasklet_schedule(&tp->tl);
 		}
-	} else if (skb_queue_len(&tp->tx_queue) > tp->tx_qlen)
+	} else if (skb_queue_len(&tp->tx_queue) > tp->tx_qlen) {
 		netif_stop_queue(netdev);
+	}
 
 	return NETDEV_TX_OK;
 }
@@ -1903,7 +1933,7 @@ static void rtl8152_nic_reset(struct r8152 *tp)
 	for (i = 0; i < 1000; i++) {
 		if (!(ocp_read_byte(tp, MCU_TYPE_PLA, PLA_CR) & CR_RST))
 			break;
-		udelay(100);
+		usleep_range(100, 400);
 	}
 }
 
@@ -1911,8 +1941,8 @@ static void set_tx_qlen(struct r8152 *tp)
 {
 	struct net_device *netdev = tp->netdev;
 
-	tp->tx_qlen = rx_buf_sz / (netdev->mtu + VLAN_ETH_HLEN + VLAN_HLEN +
-				   sizeof(struct tx_desc));
+	tp->tx_qlen = agg_buf_sz / (netdev->mtu + VLAN_ETH_HLEN + VLAN_HLEN +
+				    sizeof(struct tx_desc));
 }
 
 static inline u8 rtl8152_get_speed(struct r8152 *tp)
@@ -1949,10 +1979,34 @@ static void rxdy_gated_en(struct r8152 *tp, bool enable)
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_MISC_1, ocp_data);
 }
 
+static int rtl_start_rx(struct r8152 *tp)
+{
+	int i, ret = 0;
+
+	INIT_LIST_HEAD(&tp->rx_done);
+	for (i = 0; i < RTL8152_MAX_RX; i++) {
+		INIT_LIST_HEAD(&tp->rx_info[i].list);
+		ret = r8152_submit_rx(tp, &tp->rx_info[i], GFP_KERNEL);
+		if (ret)
+			break;
+	}
+
+	return ret;
+}
+
+static int rtl_stop_rx(struct r8152 *tp)
+{
+	int i;
+
+	for (i = 0; i < RTL8152_MAX_RX; i++)
+		usb_kill_urb(tp->rx_info[i].urb);
+
+	return 0;
+}
+
 static int rtl_enable(struct r8152 *tp)
 {
 	u32 ocp_data;
-	int i, ret;
 
 	r8152b_reset_packet_filter(tp);
 
@@ -1962,14 +2016,7 @@ static int rtl_enable(struct r8152 *tp)
 
 	rxdy_gated_en(tp, false);
 
-	INIT_LIST_HEAD(&tp->rx_done);
-	ret = 0;
-	for (i = 0; i < RTL8152_MAX_RX; i++) {
-		INIT_LIST_HEAD(&tp->rx_info[i].list);
-		ret |= r8152_submit_rx(tp, &tp->rx_info[i], GFP_KERNEL);
-	}
-
-	return ret;
+	return rtl_start_rx(tp);
 }
 
 static int rtl8152_enable(struct r8152 *tp)
@@ -2019,7 +2066,7 @@ static int rtl8153_enable(struct r8152 *tp)
 	return rtl_enable(tp);
 }
 
-static void rtl8152_disable(struct r8152 *tp)
+static void rtl_disable(struct r8152 *tp)
 {
 	u32 ocp_data;
 	int i;
@@ -2044,17 +2091,16 @@ static void rtl8152_disable(struct r8152 *tp)
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if ((ocp_data & FIFO_EMPTY) == FIFO_EMPTY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	for (i = 0; i < 1000; i++) {
 		if (ocp_read_word(tp, MCU_TYPE_PLA, PLA_TCR0) & TCR0_TX_EMPTY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
-	for (i = 0; i < RTL8152_MAX_RX; i++)
-		usb_kill_urb(tp->rx_info[i].urb);
+	rtl_stop_rx(tp);
 
 	rtl8152_nic_reset(tp);
 }
@@ -2075,6 +2121,46 @@ static void r8152_power_cut_en(struct r8152 *tp, bool enable)
 	ocp_write_word(tp, MCU_TYPE_USB, USB_PM_CTRL_STATUS, ocp_data);
 }
 
+static void rtl_rx_vlan_en(struct r8152 *tp, bool enable)
+{
+	u32 ocp_data;
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CPCR);
+	if (enable)
+		ocp_data |= CPCR_RX_VLAN;
+	else
+		ocp_data &= ~CPCR_RX_VLAN;
+	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CPCR, ocp_data);
+}
+
+static int rtl8152_set_features(struct net_device *dev,
+				netdev_features_t features)
+{
+	netdev_features_t changed = features ^ dev->features;
+	struct r8152 *tp = netdev_priv(dev);
+	int ret;
+
+	ret = usb_autopm_get_interface(tp->intf);
+	if (ret < 0)
+		goto out;
+
+	mutex_lock(&tp->control);
+
+	if (changed & NETIF_F_HW_VLAN_CTAG_RX) {
+		if (features & NETIF_F_HW_VLAN_CTAG_RX)
+			rtl_rx_vlan_en(tp, true);
+		else
+			rtl_rx_vlan_en(tp, false);
+	}
+
+	mutex_unlock(&tp->control);
+
+	usb_autopm_put_interface(tp->intf);
+
+out:
+	return ret;
+}
+
 #define WAKE_ANY (WAKE_PHY | WAKE_MAGIC | WAKE_UCAST | WAKE_BCAST | WAKE_MCAST)
 
 static u32 __rtl_get_wol(struct r8152 *tp)
@@ -2185,28 +2271,6 @@ static void rtl_phy_reset(struct r8152 *tp)
 	}
 }
 
-static void rtl_clear_bp(struct r8152 *tp)
-{
-	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_0, 0);
-	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_2, 0);
-	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_4, 0);
-	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_BP_6, 0);
-	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_0, 0);
-	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_2, 0);
-	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_4, 0);
-	ocp_write_dword(tp, MCU_TYPE_USB, USB_BP_6, 0);
-	mdelay(3);
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_BP_BA, 0);
-	ocp_write_word(tp, MCU_TYPE_USB, USB_BP_BA, 0);
-}
-
-static void r8153_clear_bp(struct r8152 *tp)
-{
-	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_BP_EN, 0);
-	ocp_write_byte(tp, MCU_TYPE_USB, USB_BP_EN, 0);
-	rtl_clear_bp(tp);
-}
-
 static void r8153_teredo_off(struct r8152 *tp)
 {
 	u32 ocp_data;
@@ -2232,6 +2296,13 @@ static inline void r8152b_enable_aldps(struct r8152 *tp)
 					    LINKENA | DIS_SDSAVE);
 }
 
+static void rtl8152_disable(struct r8152 *tp)
+{
+	r8152b_disable_aldps(tp);
+	rtl_disable(tp);
+	r8152b_enable_aldps(tp);
+}
+
 static void r8152b_hw_phy_cfg(struct r8152 *tp)
 {
 	u16 data;
@@ -2242,11 +2313,6 @@ static void r8152b_hw_phy_cfg(struct r8152 *tp)
 		r8152_mdio_write(tp, MII_BMCR, data);
 	}
 
-	r8152b_disable_aldps(tp);
-
-	rtl_clear_bp(tp);
-
-	r8152b_enable_aldps(tp);
 	set_bit(PHY_RESET, &tp->flags);
 }
 
@@ -2255,9 +2321,6 @@ static void r8152b_exit_oob(struct r8152 *tp)
 	u32 ocp_data;
 	int i;
 
-	if (test_bit(RTL8152_UNPLUG, &tp->flags))
-		return;
-
 	ocp_data = ocp_read_dword(tp, MCU_TYPE_PLA, PLA_RCR);
 	ocp_data &= ~RCR_ACPT_ALL;
 	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RCR, ocp_data);
@@ -2281,7 +2344,7 @@ static void r8152b_exit_oob(struct r8152 *tp)
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7);
@@ -2292,7 +2355,7 @@ static void r8152b_exit_oob(struct r8152 *tp)
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	rtl8152_nic_reset(tp);
@@ -2323,9 +2386,7 @@ static void r8152b_exit_oob(struct r8152 *tp)
 	ocp_write_dword(tp, MCU_TYPE_USB, USB_TX_DMA,
 			TEST_MODE_DISABLE | TX_SIZE_ADJUST1);
 
-	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CPCR);
-	ocp_data &= ~CPCR_RX_VLAN;
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CPCR, ocp_data);
+	rtl_rx_vlan_en(tp, tp->netdev->features & NETIF_F_HW_VLAN_CTAG_RX);
 
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, RTL8152_RMS);
 
@@ -2347,13 +2408,13 @@ static void r8152b_enter_oob(struct r8152 *tp)
 	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL1, RXFIFO_THR2_OOB);
 	ocp_write_dword(tp, MCU_TYPE_PLA, PLA_RXFIFO_CTRL2, RXFIFO_THR3_OOB);
 
-	rtl8152_disable(tp);
+	rtl_disable(tp);
 
 	for (i = 0; i < 1000; i++) {
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7);
@@ -2364,14 +2425,12 @@ static void r8152b_enter_oob(struct r8152 *tp)
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, RTL8152_RMS);
 
-	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CPCR);
-	ocp_data |= CPCR_RX_VLAN;
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CPCR, ocp_data);
+	rtl_rx_vlan_en(tp, true);
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PAL_BDC_CR);
 	ocp_data |= ALDPS_PROXY_MODE;
@@ -2400,8 +2459,6 @@ static void r8153_hw_phy_cfg(struct r8152 *tp)
 		r8152_mdio_write(tp, MII_BMCR, data);
 	}
 
-	r8153_clear_bp(tp);
-
 	if (tp->version == RTL_VER_03) {
 		data = ocp_reg_read(tp, OCP_EEE_CFG);
 		data &= ~CTAP_SHORT_EN;
@@ -2485,9 +2542,6 @@ static void r8153_first_init(struct r8152 *tp)
 	u32 ocp_data;
 	int i;
 
-	if (test_bit(RTL8152_UNPLUG, &tp->flags))
-		return;
-
 	rxdy_gated_en(tp, true);
 	r8153_teredo_off(tp);
 
@@ -2511,7 +2565,7 @@ static void r8153_first_init(struct r8152 *tp)
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7);
@@ -2522,12 +2576,10 @@ static void r8153_first_init(struct r8152 *tp)
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
-	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CPCR);
-	ocp_data &= ~CPCR_RX_VLAN;
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CPCR, ocp_data);
+	rtl_rx_vlan_en(tp, tp->netdev->features & NETIF_F_HW_VLAN_CTAG_RX);
 
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, RTL8153_RMS);
 	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_MTPS, MTPS_JUMBO);
@@ -2560,13 +2612,13 @@ static void r8153_enter_oob(struct r8152 *tp)
 	ocp_data &= ~NOW_IS_OOB;
 	ocp_write_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL, ocp_data);
 
-	rtl8152_disable(tp);
+	rtl_disable(tp);
 
 	for (i = 0; i < 1000; i++) {
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_SFF_STS_7);
@@ -2577,7 +2629,7 @@ static void r8153_enter_oob(struct r8152 *tp)
 		ocp_data = ocp_read_byte(tp, MCU_TYPE_PLA, PLA_OOB_CTRL);
 		if (ocp_data & LINK_LIST_READY)
 			break;
-		mdelay(1);
+		usleep_range(1000, 2000);
 	}
 
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_RMS, RTL8153_RMS);
@@ -2586,9 +2638,7 @@ static void r8153_enter_oob(struct r8152 *tp)
 	ocp_data &= ~TEREDO_WAKE_MASK;
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_TEREDO_CFG, ocp_data);
 
-	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_CPCR);
-	ocp_data |= CPCR_RX_VLAN;
-	ocp_write_word(tp, MCU_TYPE_PLA, PLA_CPCR, ocp_data);
+	rtl_rx_vlan_en(tp, true);
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PAL_BDC_CR);
 	ocp_data |= ALDPS_PROXY_MODE;
@@ -2624,6 +2674,13 @@ static void r8153_enable_aldps(struct r8152 *tp)
 	ocp_reg_write(tp, OCP_POWER_CFG, data);
 }
 
+static void rtl8153_disable(struct r8152 *tp)
+{
+	r8153_disable_aldps(tp);
+	rtl_disable(tp);
+	r8153_enable_aldps(tp);
+}
+
 static int rtl8152_set_speed(struct r8152 *tp, u8 autoneg, u16 speed, u8 duplex)
 {
 	u16 bmcr, anar, gbcr;
@@ -2714,6 +2771,16 @@ out:
 	return ret;
 }
 
+static void rtl8152_up(struct r8152 *tp)
+{
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return;
+
+	r8152b_disable_aldps(tp);
+	r8152b_exit_oob(tp);
+	r8152b_enable_aldps(tp);
+}
+
 static void rtl8152_down(struct r8152 *tp)
 {
 	if (test_bit(RTL8152_UNPLUG, &tp->flags)) {
@@ -2727,6 +2794,16 @@ static void rtl8152_down(struct r8152 *tp)
 	r8152b_enable_aldps(tp);
 }
 
+static void rtl8153_up(struct r8152 *tp)
+{
+	if (test_bit(RTL8152_UNPLUG, &tp->flags))
+		return;
+
+	r8153_disable_aldps(tp);
+	r8153_first_init(tp);
+	r8153_enable_aldps(tp);
+}
+
 static void rtl8153_down(struct r8152 *tp)
 {
 	if (test_bit(RTL8152_UNPLUG, &tp->flags)) {
@@ -2779,6 +2856,11 @@ static void rtl_work_func_t(struct work_struct *work)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		goto out1;
 
+	if (!mutex_trylock(&tp->control)) {
+		schedule_delayed_work(&tp->schedule, 0);
+		goto out1;
+	}
+
 	if (test_bit(RTL8152_LINK_CHG, &tp->flags))
 		set_carrier(tp);
 
@@ -2794,6 +2876,8 @@ static void rtl_work_func_t(struct work_struct *work)
 	if (test_bit(PHY_RESET, &tp->flags))
 		rtl_phy_reset(tp);
 
+	mutex_unlock(&tp->control);
+
 out1:
 	usb_autopm_put_interface(tp->intf);
 }
@@ -2813,6 +2897,8 @@ static int rtl8152_open(struct net_device *netdev)
 		goto out;
 	}
 
+	mutex_lock(&tp->control);
+
 	/* The WORK_ENABLE may be set when autoresume occurs */
 	if (test_bit(WORK_ENABLE, &tp->flags)) {
 		clear_bit(WORK_ENABLE, &tp->flags);
@@ -2841,6 +2927,8 @@ static int rtl8152_open(struct net_device *netdev)
 		free_all_mem(tp);
 	}
 
+	mutex_unlock(&tp->control);
+
 	usb_autopm_put_interface(tp->intf);
 
 out:
@@ -2861,8 +2949,9 @@ static int rtl8152_close(struct net_device *netdev)
 	if (res < 0) {
 		rtl_drop_queued_tx(tp);
 	} else {
-		/*
-		 * The autosuspend may have been enabled and wouldn't
+		mutex_lock(&tp->control);
+
+		/* The autosuspend may have been enabled and wouldn't
 		 * be disable when autoresume occurs, because the
 		 * netif_running() would be false.
 		 */
@@ -2874,6 +2963,9 @@ static int rtl8152_close(struct net_device *netdev)
 		tasklet_disable(&tp->tl);
 		tp->rtl_ops.down(tp);
 		tasklet_enable(&tp->tl);
+
+		mutex_unlock(&tp->control);
+
 		usb_autopm_put_interface(tp->intf);
 	}
 
@@ -2882,43 +2974,92 @@ static int rtl8152_close(struct net_device *netdev)
 	return res;
 }
 
-static void r8152b_enable_eee(struct r8152 *tp)
+static inline void r8152_mmd_indirect(struct r8152 *tp, u16 dev, u16 reg)
 {
+	ocp_reg_write(tp, OCP_EEE_AR, FUN_ADDR | dev);
+	ocp_reg_write(tp, OCP_EEE_DATA, reg);
+	ocp_reg_write(tp, OCP_EEE_AR, FUN_DATA | dev);
+}
+
+static u16 r8152_mmd_read(struct r8152 *tp, u16 dev, u16 reg)
+{
+	u16 data;
+
+	r8152_mmd_indirect(tp, dev, reg);
+	data = ocp_reg_read(tp, OCP_EEE_DATA);
+	ocp_reg_write(tp, OCP_EEE_AR, 0x0000);
+
+	return data;
+}
+
+static void r8152_mmd_write(struct r8152 *tp, u16 dev, u16 reg, u16 data)
+{
+	r8152_mmd_indirect(tp, dev, reg);
+	ocp_reg_write(tp, OCP_EEE_DATA, data);
+	ocp_reg_write(tp, OCP_EEE_AR, 0x0000);
+}
+
+static void r8152_eee_en(struct r8152 *tp, bool enable)
+{
+	u16 config1, config2, config3;
 	u32 ocp_data;
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EEE_CR);
-	ocp_data |= EEE_RX_EN | EEE_TX_EN;
+	config1 = ocp_reg_read(tp, OCP_EEE_CONFIG1) & ~sd_rise_time_mask;
+	config2 = ocp_reg_read(tp, OCP_EEE_CONFIG2);
+	config3 = ocp_reg_read(tp, OCP_EEE_CONFIG3) & ~fast_snr_mask;
+
+	if (enable) {
+		ocp_data |= EEE_RX_EN | EEE_TX_EN;
+		config1 |= EEE_10_CAP | EEE_NWAY_EN | TX_QUIET_EN | RX_QUIET_EN;
+		config1 |= sd_rise_time(1);
+		config2 |= RG_DACQUIET_EN | RG_LDVQUIET_EN;
+		config3 |= fast_snr(42);
+	} else {
+		ocp_data &= ~(EEE_RX_EN | EEE_TX_EN);
+		config1 &= ~(EEE_10_CAP | EEE_NWAY_EN | TX_QUIET_EN |
+			     RX_QUIET_EN);
+		config1 |= sd_rise_time(7);
+		config2 &= ~(RG_DACQUIET_EN | RG_LDVQUIET_EN);
+		config3 |= fast_snr(511);
+	}
+
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_EEE_CR, ocp_data);
-	ocp_reg_write(tp, OCP_EEE_CONFIG1, RG_TXLPI_MSK_HFDUP | RG_MATCLR_EN |
-					   EEE_10_CAP | EEE_NWAY_EN |
-					   TX_QUIET_EN | RX_QUIET_EN |
-					   SDRISETIME | RG_RXLPI_MSK_HFDUP |
-					   SDFALLTIME);
-	ocp_reg_write(tp, OCP_EEE_CONFIG2, RG_LPIHYS_NUM | RG_DACQUIET_EN |
-					   RG_LDVQUIET_EN | RG_CKRSEL |
-					   RG_EEEPRG_EN);
-	ocp_reg_write(tp, OCP_EEE_CONFIG3, FST_SNR_EYE_R | RG_LFS_SEL | MSK_PH);
-	ocp_reg_write(tp, OCP_EEE_AR, FUN_ADDR | DEVICE_ADDR);
-	ocp_reg_write(tp, OCP_EEE_DATA, EEE_ADDR);
-	ocp_reg_write(tp, OCP_EEE_AR, FUN_DATA | DEVICE_ADDR);
-	ocp_reg_write(tp, OCP_EEE_DATA, EEE_DATA);
-	ocp_reg_write(tp, OCP_EEE_AR, 0x0000);
+	ocp_reg_write(tp, OCP_EEE_CONFIG1, config1);
+	ocp_reg_write(tp, OCP_EEE_CONFIG2, config2);
+	ocp_reg_write(tp, OCP_EEE_CONFIG3, config3);
 }
 
-static void r8153_enable_eee(struct r8152 *tp)
+static void r8152b_enable_eee(struct r8152 *tp)
+{
+	r8152_eee_en(tp, true);
+	r8152_mmd_write(tp, MDIO_MMD_AN, MDIO_AN_EEE_ADV, MDIO_EEE_100TX);
+}
+
+static void r8153_eee_en(struct r8152 *tp, bool enable)
 {
 	u32 ocp_data;
-	u16 data;
+	u16 config;
 
 	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EEE_CR);
-	ocp_data |= EEE_RX_EN | EEE_TX_EN;
+	config = ocp_reg_read(tp, OCP_EEE_CFG);
+
+	if (enable) {
+		ocp_data |= EEE_RX_EN | EEE_TX_EN;
+		config |= EEE10_EN;
+	} else {
+		ocp_data &= ~(EEE_RX_EN | EEE_TX_EN);
+		config &= ~EEE10_EN;
+	}
+
 	ocp_write_word(tp, MCU_TYPE_PLA, PLA_EEE_CR, ocp_data);
-	data = ocp_reg_read(tp, OCP_EEE_CFG);
-	data |= EEE10_EN;
-	ocp_reg_write(tp, OCP_EEE_CFG, data);
-	data = ocp_reg_read(tp, OCP_EEE_CFG2);
-	data |= MY1000_EEE | MY100_EEE;
-	ocp_reg_write(tp, OCP_EEE_CFG2, data);
+	ocp_reg_write(tp, OCP_EEE_CFG, config);
+}
+
+static void r8153_enable_eee(struct r8152 *tp)
+{
+	r8153_eee_en(tp, true);
+	ocp_reg_write(tp, OCP_EEE_ADV, MDIO_EEE_1000T | MDIO_EEE_100TX);
 }
 
 static void r8152b_enable_fc(struct r8152 *tp)
@@ -2946,6 +3087,8 @@ static void r8152b_init(struct r8152 *tp)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		return;
 
+	r8152b_disable_aldps(tp);
+
 	if (tp->version == RTL_VER_01) {
 		ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_LED_FEATURE);
 		ocp_data &= ~LED_MODE_MASK;
@@ -2984,6 +3127,7 @@ static void r8153_init(struct r8152 *tp)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		return;
 
+	r8153_disable_aldps(tp);
 	r8153_u1u2en(tp, false);
 
 	for (i = 0; i < 500; i++) {
@@ -3045,32 +3189,47 @@ static void r8153_init(struct r8152 *tp)
 static int rtl8152_suspend(struct usb_interface *intf, pm_message_t message)
 {
 	struct r8152 *tp = usb_get_intfdata(intf);
+	struct net_device *netdev = tp->netdev;
+	int ret = 0;
+
+	mutex_lock(&tp->control);
+
+	if (PMSG_IS_AUTO(message)) {
+		if (netif_running(netdev) && work_busy(&tp->schedule.work)) {
+			ret = -EBUSY;
+			goto out1;
+		}
 
-	if (PMSG_IS_AUTO(message))
 		set_bit(SELECTIVE_SUSPEND, &tp->flags);
-	else
-		netif_device_detach(tp->netdev);
+	} else {
+		netif_device_detach(netdev);
+	}
 
-	if (netif_running(tp->netdev)) {
+	if (netif_running(netdev)) {
 		clear_bit(WORK_ENABLE, &tp->flags);
 		usb_kill_urb(tp->intr_urb);
-		cancel_delayed_work_sync(&tp->schedule);
+		tasklet_disable(&tp->tl);
 		if (test_bit(SELECTIVE_SUSPEND, &tp->flags)) {
+			rtl_stop_rx(tp);
 			rtl_runtime_suspend_enable(tp, true);
 		} else {
-			tasklet_disable(&tp->tl);
+			cancel_delayed_work_sync(&tp->schedule);
 			tp->rtl_ops.down(tp);
-			tasklet_enable(&tp->tl);
 		}
+		tasklet_enable(&tp->tl);
 	}
+out1:
+	mutex_unlock(&tp->control);
 
-	return 0;
+	return ret;
 }
 
 static int rtl8152_resume(struct usb_interface *intf)
 {
 	struct r8152 *tp = usb_get_intfdata(intf);
 
+	mutex_lock(&tp->control);
+
 	if (!test_bit(SELECTIVE_SUSPEND, &tp->flags)) {
 		tp->rtl_ops.init(tp);
 		netif_device_attach(tp->netdev);
@@ -3080,20 +3239,24 @@ static int rtl8152_resume(struct usb_interface *intf)
 		if (test_bit(SELECTIVE_SUSPEND, &tp->flags)) {
 			rtl_runtime_suspend_enable(tp, false);
 			clear_bit(SELECTIVE_SUSPEND, &tp->flags);
+			set_bit(WORK_ENABLE, &tp->flags);
 			if (tp->speed & LINK_STATUS)
-				tp->rtl_ops.disable(tp);
+				rtl_start_rx(tp);
 		} else {
 			tp->rtl_ops.up(tp);
 			rtl8152_set_speed(tp, AUTONEG_ENABLE,
-				tp->mii.supports_gmii ? SPEED_1000 : SPEED_100,
-				DUPLEX_FULL);
+					  tp->mii.supports_gmii ?
+					  SPEED_1000 : SPEED_100,
+					  DUPLEX_FULL);
+			tp->speed = 0;
+			netif_carrier_off(tp->netdev);
+			set_bit(WORK_ENABLE, &tp->flags);
 		}
-		tp->speed = 0;
-		netif_carrier_off(tp->netdev);
-		set_bit(WORK_ENABLE, &tp->flags);
 		usb_submit_urb(tp->intr_urb, GFP_KERNEL);
 	}
 
+	mutex_unlock(&tp->control);
+
 	return 0;
 }
 
@@ -3104,9 +3267,13 @@ static void rtl8152_get_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 	if (usb_autopm_get_interface(tp->intf) < 0)
 		return;
 
+	mutex_lock(&tp->control);
+
 	wol->supported = WAKE_ANY;
 	wol->wolopts = __rtl_get_wol(tp);
 
+	mutex_unlock(&tp->control);
+
 	usb_autopm_put_interface(tp->intf);
 }
 
@@ -3119,9 +3286,13 @@ static int rtl8152_set_wol(struct net_device *dev, struct ethtool_wolinfo *wol)
 	if (ret < 0)
 		goto out_set_wol;
 
+	mutex_lock(&tp->control);
+
 	__rtl_set_wol(tp, wol->wolopts);
 	tp->saved_wolopts = wol->wolopts & WAKE_ANY;
 
+	mutex_unlock(&tp->control);
+
 	usb_autopm_put_interface(tp->intf);
 
 out_set_wol:
@@ -3147,8 +3318,8 @@ static void rtl8152_get_drvinfo(struct net_device *netdev,
 {
 	struct r8152 *tp = netdev_priv(netdev);
 
-	strncpy(info->driver, MODULENAME, ETHTOOL_BUSINFO_LEN);
-	strncpy(info->version, DRIVER_VERSION, ETHTOOL_BUSINFO_LEN);
+	strlcpy(info->driver, MODULENAME, sizeof(info->driver));
+	strlcpy(info->version, DRIVER_VERSION, sizeof(info->version));
 	usb_make_path(tp->udev, info->bus_info, sizeof(info->bus_info));
 }
 
@@ -3156,11 +3327,25 @@ static
 int rtl8152_get_settings(struct net_device *netdev, struct ethtool_cmd *cmd)
 {
 	struct r8152 *tp = netdev_priv(netdev);
+	int ret;
 
 	if (!tp->mii.mdio_read)
 		return -EOPNOTSUPP;
 
-	return mii_ethtool_gset(&tp->mii, cmd);
+	ret = usb_autopm_get_interface(tp->intf);
+	if (ret < 0)
+		goto out;
+
+	mutex_lock(&tp->control);
+
+	ret = mii_ethtool_gset(&tp->mii, cmd);
+
+	mutex_unlock(&tp->control);
+
+	usb_autopm_put_interface(tp->intf);
+
+out:
+	return ret;
 }
 
 static int rtl8152_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
@@ -3172,8 +3357,12 @@ static int rtl8152_set_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 	if (ret < 0)
 		goto out;
 
+	mutex_lock(&tp->control);
+
 	ret = rtl8152_set_speed(tp, cmd->autoneg, cmd->speed, cmd->duplex);
 
+	mutex_unlock(&tp->control);
+
 	usb_autopm_put_interface(tp->intf);
 
 out:
@@ -3243,6 +3432,132 @@ static void rtl8152_get_strings(struct net_device *dev, u32 stringset, u8 *data)
 	}
 }
 
+static int r8152_get_eee(struct r8152 *tp, struct ethtool_eee *eee)
+{
+	u32 ocp_data, lp, adv, supported = 0;
+	u16 val;
+
+	val = r8152_mmd_read(tp, MDIO_MMD_PCS, MDIO_PCS_EEE_ABLE);
+	supported = mmd_eee_cap_to_ethtool_sup_t(val);
+
+	val = r8152_mmd_read(tp, MDIO_MMD_AN, MDIO_AN_EEE_ADV);
+	adv = mmd_eee_adv_to_ethtool_adv_t(val);
+
+	val = r8152_mmd_read(tp, MDIO_MMD_AN, MDIO_AN_EEE_LPABLE);
+	lp = mmd_eee_adv_to_ethtool_adv_t(val);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EEE_CR);
+	ocp_data &= EEE_RX_EN | EEE_TX_EN;
+
+	eee->eee_enabled = !!ocp_data;
+	eee->eee_active = !!(supported & adv & lp);
+	eee->supported = supported;
+	eee->advertised = adv;
+	eee->lp_advertised = lp;
+
+	return 0;
+}
+
+static int r8152_set_eee(struct r8152 *tp, struct ethtool_eee *eee)
+{
+	u16 val = ethtool_adv_to_mmd_eee_adv_t(eee->advertised);
+
+	r8152_eee_en(tp, eee->eee_enabled);
+
+	if (!eee->eee_enabled)
+		val = 0;
+
+	r8152_mmd_write(tp, MDIO_MMD_AN, MDIO_AN_EEE_ADV, val);
+
+	return 0;
+}
+
+static int r8153_get_eee(struct r8152 *tp, struct ethtool_eee *eee)
+{
+	u32 ocp_data, lp, adv, supported = 0;
+	u16 val;
+
+	val = ocp_reg_read(tp, OCP_EEE_ABLE);
+	supported = mmd_eee_cap_to_ethtool_sup_t(val);
+
+	val = ocp_reg_read(tp, OCP_EEE_ADV);
+	adv = mmd_eee_adv_to_ethtool_adv_t(val);
+
+	val = ocp_reg_read(tp, OCP_EEE_LPABLE);
+	lp = mmd_eee_adv_to_ethtool_adv_t(val);
+
+	ocp_data = ocp_read_word(tp, MCU_TYPE_PLA, PLA_EEE_CR);
+	ocp_data &= EEE_RX_EN | EEE_TX_EN;
+
+	eee->eee_enabled = !!ocp_data;
+	eee->eee_active = !!(supported & adv & lp);
+	eee->supported = supported;
+	eee->advertised = adv;
+	eee->lp_advertised = lp;
+
+	return 0;
+}
+
+static int r8153_set_eee(struct r8152 *tp, struct ethtool_eee *eee)
+{
+	u16 val = ethtool_adv_to_mmd_eee_adv_t(eee->advertised);
+
+	r8153_eee_en(tp, eee->eee_enabled);
+
+	if (!eee->eee_enabled)
+		val = 0;
+
+	ocp_reg_write(tp, OCP_EEE_ADV, val);
+
+	return 0;
+}
+
+static int
+rtl_ethtool_get_eee(struct net_device *net, struct ethtool_eee *edata)
+{
+	struct r8152 *tp = netdev_priv(net);
+	int ret;
+
+	ret = usb_autopm_get_interface(tp->intf);
+	if (ret < 0)
+		goto out;
+
+	mutex_lock(&tp->control);
+
+	ret = tp->rtl_ops.eee_get(tp, edata);
+
+	mutex_unlock(&tp->control);
+
+	usb_autopm_put_interface(tp->intf);
+
+out:
+	return ret;
+}
+
+static int
+rtl_ethtool_set_eee(struct net_device *net, struct ethtool_eee *edata)
+{
+	struct r8152 *tp = netdev_priv(net);
+	int ret;
+
+	ret = usb_autopm_get_interface(tp->intf);
+	if (ret < 0)
+		goto out;
+
+	mutex_lock(&tp->control);
+
+	ret = tp->rtl_ops.eee_set(tp, edata);
+	if (!ret)
+		ret = mii_nway_restart(&tp->mii);
+
+	mutex_unlock(&tp->control);
+
+	usb_autopm_put_interface(tp->intf);
+
+out:
+	return ret;
+}
+
 static struct ethtool_ops ops = {
 	.get_drvinfo = rtl8152_get_drvinfo,
 	.get_settings = rtl8152_get_settings,
@@ -3255,6 +3570,8 @@ static struct ethtool_ops ops = {
 	.get_strings = rtl8152_get_strings,
 	.get_sset_count = rtl8152_get_sset_count,
 	.get_ethtool_stats = rtl8152_get_ethtool_stats,
+	.get_eee = rtl_ethtool_get_eee,
+	.set_eee = rtl_ethtool_set_eee,
 };
 
 static int rtl8152_ioctl(struct net_device *netdev, struct ifreq *rq, int cmd)
@@ -3276,7 +3593,9 @@ static int rtl8152_ioctl(struct net_device *netdev, struct ifreq *rq, int cmd)
 		break;
 
 	case SIOCGMIIREG:
+		mutex_lock(&tp->control);
 		data->val_out = r8152_mdio_read(tp, data->reg_num);
+		mutex_unlock(&tp->control);
 		break;
 
 	case SIOCSMIIREG:
@@ -3284,7 +3603,9 @@ static int rtl8152_ioctl(struct net_device *netdev, struct ifreq *rq, int cmd)
 			res = -EPERM;
 			break;
 		}
+		mutex_lock(&tp->control);
 		r8152_mdio_write(tp, data->reg_num, data->val_in);
+		mutex_unlock(&tp->control);
 		break;
 
 	default:
@@ -3323,6 +3644,7 @@ static const struct net_device_ops rtl8152_netdev_ops = {
 	.ndo_do_ioctl		= rtl8152_ioctl,
 	.ndo_start_xmit		= rtl8152_start_xmit,
 	.ndo_tx_timeout		= rtl8152_tx_timeout,
+	.ndo_set_features	= rtl8152_set_features,
 	.ndo_set_rx_mode	= rtl8152_set_rx_mode,
 	.ndo_set_mac_address	= rtl8152_set_mac_address,
 	.ndo_change_mtu		= rtl8152_change_mtu,
@@ -3377,7 +3699,7 @@ static void rtl8153_unload(struct r8152 *tp)
 	if (test_bit(RTL8152_UNPLUG, &tp->flags))
 		return;
 
-	r8153_power_cut_en(tp, true);
+	r8153_power_cut_en(tp, false);
 }
 
 static int rtl_ops_init(struct r8152 *tp, const struct usb_device_id *id)
@@ -3392,18 +3714,22 @@ static int rtl_ops_init(struct r8152 *tp, const struct usb_device_id *id)
 			ops->init		= r8152b_init;
 			ops->enable		= rtl8152_enable;
 			ops->disable		= rtl8152_disable;
-			ops->up			= r8152b_exit_oob;
+			ops->up			= rtl8152_up;
 			ops->down		= rtl8152_down;
 			ops->unload		= rtl8152_unload;
+			ops->eee_get		= r8152_get_eee;
+			ops->eee_set		= r8152_set_eee;
 			ret = 0;
 			break;
 		case PRODUCT_ID_RTL8153:
 			ops->init		= r8153_init;
 			ops->enable		= rtl8153_enable;
-			ops->disable		= rtl8152_disable;
-			ops->up			= r8153_first_init;
+			ops->disable		= rtl8153_disable;
+			ops->up			= rtl8153_up;
 			ops->down		= rtl8153_down;
 			ops->unload		= rtl8153_unload;
+			ops->eee_get		= r8153_get_eee;
+			ops->eee_set		= r8153_set_eee;
 			ret = 0;
 			break;
 		default:
@@ -3416,10 +3742,12 @@ static int rtl_ops_init(struct r8152 *tp, const struct usb_device_id *id)
 		case PRODUCT_ID_SAMSUNG:
 			ops->init		= r8153_init;
 			ops->enable		= rtl8153_enable;
-			ops->disable		= rtl8152_disable;
-			ops->up			= r8153_first_init;
+			ops->disable		= rtl8153_disable;
+			ops->up			= rtl8153_up;
 			ops->down		= rtl8153_down;
 			ops->unload		= rtl8153_unload;
+			ops->eee_get		= r8153_get_eee;
+			ops->eee_set		= r8153_set_eee;
 			ret = 0;
 			break;
 		default:
@@ -3470,6 +3798,7 @@ static int rtl8152_probe(struct usb_interface *intf,
 		goto out;
 
 	tasklet_init(&tp->tl, bottom_half, (unsigned long)tp);
+	mutex_init(&tp->control);
 	INIT_DELAYED_WORK(&tp->schedule, rtl_work_func_t);
 
 	netdev->netdev_ops = &rtl8152_netdev_ops;
@@ -3477,10 +3806,16 @@ static int rtl8152_probe(struct usb_interface *intf,
 
 	netdev->features |= NETIF_F_RXCSUM | NETIF_F_IP_CSUM | NETIF_F_SG |
 			    NETIF_F_TSO | NETIF_F_FRAGLIST | NETIF_F_IPV6_CSUM |
-			    NETIF_F_TSO6;
+			    NETIF_F_TSO6 | NETIF_F_HW_VLAN_CTAG_RX |
+			    NETIF_F_HW_VLAN_CTAG_TX;
 	netdev->hw_features = NETIF_F_RXCSUM | NETIF_F_IP_CSUM | NETIF_F_SG |
 			      NETIF_F_TSO | NETIF_F_FRAGLIST |
-			      NETIF_F_IPV6_CSUM | NETIF_F_TSO6;
+			      NETIF_F_IPV6_CSUM | NETIF_F_TSO6 |
+			      NETIF_F_HW_VLAN_CTAG_RX |
+			      NETIF_F_HW_VLAN_CTAG_TX;
+	netdev->vlan_features = NETIF_F_SG | NETIF_F_IP_CSUM | NETIF_F_TSO |
+				NETIF_F_HIGHDMA | NETIF_F_FRAGLIST |
+				NETIF_F_IPV6_CSUM | NETIF_F_TSO6;
 
 	netdev->ethtool_ops = &ops;
 	netif_set_gso_max_size(netdev, RTL_LIMITED_TSO_SIZE);
@@ -3530,7 +3865,11 @@ static void rtl8152_disconnect(struct usb_interface *intf)
 
 	usb_set_intfdata(intf, NULL);
 	if (tp) {
-		set_bit(RTL8152_UNPLUG, &tp->flags);
+		struct usb_device *udev = tp->udev;
+
+		if (udev->state == USB_STATE_NOTATTACHED)
+			set_bit(RTL8152_UNPLUG, &tp->flags);
+
 		tasklet_kill(&tp->tl);
 		unregister_netdev(tp->netdev);
 		tp->rtl_ops.unload(tp);
diff --git a/drivers/net/usb/usbnet.c b/drivers/net/usb/usbnet.c
index 5173821a9575..20615bbd693b 100644
--- a/drivers/net/usb/usbnet.c
+++ b/drivers/net/usb/usbnet.c
@@ -69,8 +69,9 @@
 // reawaken network queue this soon after stopping; else watchdog barks
 #define TX_TIMEOUT_JIFFIES	(5*HZ)
 
-// throttle rx/tx briefly after some faults, so khubd might disconnect()
-// us (it polls at HZ/4 usually) before we report too many false errors.
+/* throttle rx/tx briefly after some faults, so hub_wq might disconnect()
+ * us (it polls at HZ/4 usually) before we report too many false errors.
+ */
 #define THROTTLE_JIFFIES	(HZ/8)
 
 // between wakeups
@@ -595,9 +596,9 @@ static void rx_complete (struct urb *urb)
 			  "rx shutdown, code %d\n", urb_status);
 		goto block;
 
-	/* we get controller i/o faults during khubd disconnect() delays.
+	/* we get controller i/o faults during hub_wq disconnect() delays.
 	 * throttle down resubmits, to avoid log floods; just temporarily,
-	 * so we still recover when the fault isn't a khubd delay.
+	 * so we still recover when the fault isn't a hub_wq delay.
 	 */
 	case -EPROTO:
 	case -ETIME:
@@ -1185,8 +1186,9 @@ static void tx_complete (struct urb *urb)
 		case -ESHUTDOWN:		// hardware gone
 			break;
 
-		// like rx, tx gets controller i/o faults during khubd delays
-		// and so it uses the same throttling mechanism.
+		/* like rx, tx gets controller i/o faults during hub_wq
+		 * delays and so it uses the same throttling mechanism.
+		 */
 		case -EPROTO:
 		case -ETIME:
 		case -EILSEQ:
diff --git a/drivers/net/virtio_net.c b/drivers/net/virtio_net.c
index 59caa06f34a6..d75256bd1a6a 100644
--- a/drivers/net/virtio_net.c
+++ b/drivers/net/virtio_net.c
@@ -123,9 +123,6 @@ struct virtnet_info {
 	/* Host can handle any s/g split between our header and packet data */
 	bool any_header_sg;
 
-	/* enable config space updates */
-	bool config_enable;
-
 	/* Active statistics */
 	struct virtnet_stats __percpu *stats;
 
@@ -135,9 +132,6 @@ struct virtnet_info {
 	/* Work struct for config space updates */
 	struct work_struct config_work;
 
-	/* Lock for config space updates */
-	struct mutex config_lock;
-
 	/* Does the affinity hint is set for virtqueues? */
 	bool affinity_hint_set;
 
@@ -546,8 +540,8 @@ static int add_recvbuf_small(struct receive_queue *rq, gfp_t gfp)
 	skb_put(skb, GOOD_PACKET_LEN);
 
 	hdr = skb_vnet_hdr(skb);
+	sg_init_table(rq->sg, MAX_SKB_FRAGS + 2);
 	sg_set_buf(rq->sg, &hdr->hdr, sizeof hdr->hdr);
-
 	skb_to_sgvec(skb, rq->sg + 1, 0, skb->len);
 
 	err = virtqueue_add_inbuf(rq->vq, rq->sg, 2, skb, gfp);
@@ -563,6 +557,8 @@ static int add_recvbuf_big(struct receive_queue *rq, gfp_t gfp)
 	char *p;
 	int i, err, offset;
 
+	sg_init_table(rq->sg, MAX_SKB_FRAGS + 2);
+
 	/* page in rq->sg[MAX_SKB_FRAGS + 1] is list tail */
 	for (i = MAX_SKB_FRAGS + 1; i > 1; --i) {
 		first = get_a_page(rq, gfp);
@@ -899,6 +895,7 @@ static int xmit_skb(struct send_queue *sq, struct sk_buff *skb)
 	if (vi->mergeable_rx_bufs)
 		hdr->mhdr.num_buffers = 0;
 
+	sg_init_table(sq->sg, MAX_SKB_FRAGS + 2);
 	if (can_push) {
 		__skb_push(skb, hdr_len);
 		num_sg = skb_to_sgvec(skb, sq->sg, 0, skb->len);
@@ -917,6 +914,8 @@ static netdev_tx_t start_xmit(struct sk_buff *skb, struct net_device *dev)
 	int qnum = skb_get_queue_mapping(skb);
 	struct send_queue *sq = &vi->sq[qnum];
 	int err;
+	struct netdev_queue *txq = netdev_get_tx_queue(dev, qnum);
+	bool kick = !skb->xmit_more;
 
 	/* Free up any pending old buffers before queueing new ones. */
 	free_old_xmit_skbs(sq);
@@ -934,7 +933,6 @@ static netdev_tx_t start_xmit(struct sk_buff *skb, struct net_device *dev)
 		dev_kfree_skb_any(skb);
 		return NETDEV_TX_OK;
 	}
-	virtqueue_kick(sq->vq);
 
 	/* Don't wait up for transmitted skbs to be freed. */
 	skb_orphan(skb);
@@ -954,6 +952,9 @@ static netdev_tx_t start_xmit(struct sk_buff *skb, struct net_device *dev)
 		}
 	}
 
+	if (kick || netif_xmit_stopped(txq))
+		virtqueue_kick(sq->vq);
+
 	return NETDEV_TX_OK;
 }
 
@@ -1407,13 +1408,9 @@ static void virtnet_config_changed_work(struct work_struct *work)
 		container_of(work, struct virtnet_info, config_work);
 	u16 v;
 
-	mutex_lock(&vi->config_lock);
-	if (!vi->config_enable)
-		goto done;
-
 	if (virtio_cread_feature(vi->vdev, VIRTIO_NET_F_STATUS,
 				 struct virtio_net_config, status, &v) < 0)
-		goto done;
+		return;
 
 	if (v & VIRTIO_NET_S_ANNOUNCE) {
 		netdev_notify_peers(vi->dev);
@@ -1424,7 +1421,7 @@ static void virtnet_config_changed_work(struct work_struct *work)
 	v &= VIRTIO_NET_S_LINK_UP;
 
 	if (vi->status == v)
-		goto done;
+		return;
 
 	vi->status = v;
 
@@ -1435,8 +1432,6 @@ static void virtnet_config_changed_work(struct work_struct *work)
 		netif_carrier_off(vi->dev);
 		netif_tx_stop_all_queues(vi->dev);
 	}
-done:
-	mutex_unlock(&vi->config_lock);
 }
 
 static void virtnet_config_changed(struct virtio_device *vdev)
@@ -1757,8 +1752,6 @@ static int virtnet_probe(struct virtio_device *vdev)
 		u64_stats_init(&virtnet_stats->rx_syncp);
 	}
 
-	mutex_init(&vi->config_lock);
-	vi->config_enable = true;
 	INIT_WORK(&vi->config_work, virtnet_config_changed_work);
 
 	/* If we can receive ANY GSO packets, we must allocate large ones. */
@@ -1806,6 +1799,8 @@ static int virtnet_probe(struct virtio_device *vdev)
 		goto free_vqs;
 	}
 
+	virtio_device_ready(vdev);
+
 	/* Last of all, set up some receive buffers. */
 	for (i = 0; i < vi->curr_queue_pairs; i++) {
 		try_fill_recv(&vi->rq[i], GFP_KERNEL);
@@ -1842,6 +1837,8 @@ static int virtnet_probe(struct virtio_device *vdev)
 	return 0;
 
 free_recv_bufs:
+	vi->vdev->config->reset(vdev);
+
 	free_receive_bufs(vi);
 	unregister_netdev(dev);
 free_vqs:
@@ -1875,17 +1872,13 @@ static void virtnet_remove(struct virtio_device *vdev)
 
 	unregister_hotcpu_notifier(&vi->nb);
 
-	/* Prevent config work handler from accessing the device. */
-	mutex_lock(&vi->config_lock);
-	vi->config_enable = false;
-	mutex_unlock(&vi->config_lock);
+	/* Make sure no work handler is accessing the device. */
+	flush_work(&vi->config_work);
 
 	unregister_netdev(vi->dev);
 
 	remove_vq_common(vi);
 
-	flush_work(&vi->config_work);
-
 	free_percpu(vi->stats);
 	free_netdev(vi->dev);
 }
@@ -1898,10 +1891,8 @@ static int virtnet_freeze(struct virtio_device *vdev)
 
 	unregister_hotcpu_notifier(&vi->nb);
 
-	/* Prevent config work handler from accessing the device */
-	mutex_lock(&vi->config_lock);
-	vi->config_enable = false;
-	mutex_unlock(&vi->config_lock);
+	/* Make sure no work handler is accessing the device */
+	flush_work(&vi->config_work);
 
 	netif_device_detach(vi->dev);
 	cancel_delayed_work_sync(&vi->refill);
@@ -1916,8 +1907,6 @@ static int virtnet_freeze(struct virtio_device *vdev)
 
 	remove_vq_common(vi);
 
-	flush_work(&vi->config_work);
-
 	return 0;
 }
 
@@ -1930,6 +1919,8 @@ static int virtnet_restore(struct virtio_device *vdev)
 	if (err)
 		return err;
 
+	virtio_device_ready(vdev);
+
 	if (netif_running(vi->dev)) {
 		for (i = 0; i < vi->curr_queue_pairs; i++)
 			if (!try_fill_recv(&vi->rq[i], GFP_KERNEL))
@@ -1941,10 +1932,6 @@ static int virtnet_restore(struct virtio_device *vdev)
 
 	netif_device_attach(vi->dev);
 
-	mutex_lock(&vi->config_lock);
-	vi->config_enable = true;
-	mutex_unlock(&vi->config_lock);
-
 	rtnl_lock();
 	virtnet_set_queues(vi, vi->curr_queue_pairs);
 	rtnl_unlock();
diff --git a/drivers/net/vmxnet3/vmxnet3_drv.c b/drivers/net/vmxnet3/vmxnet3_drv.c
index d6e90c72c257..6dfcbf523936 100644
--- a/drivers/net/vmxnet3/vmxnet3_drv.c
+++ b/drivers/net/vmxnet3/vmxnet3_drv.c
@@ -2056,7 +2056,6 @@ vmxnet3_set_mc(struct net_device *netdev)
 		if (!netdev_mc_empty(netdev)) {
 			new_table = vmxnet3_copy_mc(netdev);
 			if (new_table) {
-				new_mode |= VMXNET3_RXM_MCAST;
 				rxConf->mfTableLen = cpu_to_le16(
 					netdev_mc_count(netdev) * ETH_ALEN);
 				new_table_pa = dma_map_single(
@@ -2064,15 +2063,18 @@ vmxnet3_set_mc(struct net_device *netdev)
 							new_table,
 							rxConf->mfTableLen,
 							PCI_DMA_TODEVICE);
+			}
+
+			if (new_table_pa) {
+				new_mode |= VMXNET3_RXM_MCAST;
 				rxConf->mfTablePA = cpu_to_le64(new_table_pa);
 			} else {
-				netdev_info(netdev, "failed to copy mcast list"
-					    ", setting ALL_MULTI\n");
+				netdev_info(netdev,
+					    "failed to copy mcast list, setting ALL_MULTI\n");
 				new_mode |= VMXNET3_RXM_ALL_MULTI;
 			}
 		}
 
-
 	if (!(new_mode & VMXNET3_RXM_MCAST)) {
 		rxConf->mfTableLen = 0;
 		rxConf->mfTablePA = 0;
@@ -2091,11 +2093,10 @@ vmxnet3_set_mc(struct net_device *netdev)
 			       VMXNET3_CMD_UPDATE_MAC_FILTERS);
 	spin_unlock_irqrestore(&adapter->cmd_lock, flags);
 
-	if (new_table) {
+	if (new_table_pa)
 		dma_unmap_single(&adapter->pdev->dev, new_table_pa,
 				 rxConf->mfTableLen, PCI_DMA_TODEVICE);
-		kfree(new_table);
-	}
+	kfree(new_table);
 }
 
 void
diff --git a/drivers/net/vmxnet3/vmxnet3_int.h b/drivers/net/vmxnet3/vmxnet3_int.h
index 29ee77f2c97f..5f0199f6c31e 100644
--- a/drivers/net/vmxnet3/vmxnet3_int.h
+++ b/drivers/net/vmxnet3/vmxnet3_int.h
@@ -69,10 +69,10 @@
 /*
  * Version numbers
  */
-#define VMXNET3_DRIVER_VERSION_STRING   "1.2.0.0-k"
+#define VMXNET3_DRIVER_VERSION_STRING   "1.2.1.0-k"
 
 /* a 32-bit int, each byte encode a verion number in VMXNET3_DRIVER_VERSION */
-#define VMXNET3_DRIVER_VERSION_NUM      0x01020000
+#define VMXNET3_DRIVER_VERSION_NUM      0x01020100
 
 #if defined(CONFIG_PCI_MSI)
 	/* RSS only makes sense if MSI-X is supported. */
@@ -117,7 +117,6 @@ enum {
 /*
  * PCI vendor and device IDs.
  */
-#define PCI_VENDOR_ID_VMWARE            0x15AD
 #define PCI_DEVICE_ID_VMWARE_VMXNET3    0x07B0
 #define MAX_ETHERNET_CARDS		10
 #define MAX_PCI_PASSTHRU_DEVICE		6
diff --git a/drivers/net/vxlan.c b/drivers/net/vxlan.c
index 1fb7b37d1402..ca309820d39e 100644
--- a/drivers/net/vxlan.c
+++ b/drivers/net/vxlan.c
@@ -42,6 +42,7 @@
 #include <net/netns/generic.h>
 #include <net/vxlan.h>
 #include <net/protocol.h>
+#include <net/udp_tunnel.h>
 #if IS_ENABLED(CONFIG_IPV6)
 #include <net/ipv6.h>
 #include <net/addrconf.h>
@@ -1062,7 +1063,6 @@ void vxlan_sock_release(struct vxlan_sock *vs)
 
 	spin_lock(&vn->sock_lock);
 	hlist_del_rcu(&vs->hlist);
-	rcu_assign_sk_user_data(vs->sock->sk, NULL);
 	vxlan_notify_del_rx_port(vs);
 	spin_unlock(&vn->sock_lock);
 
@@ -1158,8 +1158,6 @@ static int vxlan_udp_encap_recv(struct sock *sk, struct sk_buff *skb)
 	if (!vs)
 		goto drop;
 
-	skb_pop_rcv_encapsulation(skb);
-
 	vs->rcv(vs, skb, vxh->vx_vni);
 	return 0;
 
@@ -1327,7 +1325,7 @@ static int arp_reduce(struct net_device *dev, struct sk_buff *skb)
 	} else if (vxlan->flags & VXLAN_F_L3MISS) {
 		union vxlan_addr ipa = {
 			.sin.sin_addr.s_addr = tip,
-			.sa.sa_family = AF_INET,
+			.sin.sin_family = AF_INET,
 		};
 
 		vxlan_ip_miss(dev, &ipa);
@@ -1338,7 +1336,6 @@ out:
 }
 
 #if IS_ENABLED(CONFIG_IPV6)
-
 static struct sk_buff *vxlan_na_create(struct sk_buff *request,
 	struct neighbour *n, bool isrouter)
 {
@@ -1440,9 +1437,6 @@ static int neigh_reduce(struct net_device *dev, struct sk_buff *skb)
 	if (!in6_dev)
 		goto out;
 
-	if (!pskb_may_pull(skb, skb->len))
-		goto out;
-
 	iphdr = ipv6_hdr(skb);
 	saddr = &iphdr->saddr;
 	daddr = &iphdr->daddr;
@@ -1488,7 +1482,7 @@ static int neigh_reduce(struct net_device *dev, struct sk_buff *skb)
 	} else if (vxlan->flags & VXLAN_F_L3MISS) {
 		union vxlan_addr ipa = {
 			.sin6.sin6_addr = msg->target,
-			.sa.sa_family = AF_INET6,
+			.sin6.sin6_family = AF_INET6,
 		};
 
 		vxlan_ip_miss(dev, &ipa);
@@ -1521,7 +1515,7 @@ static bool route_shortcircuit(struct net_device *dev, struct sk_buff *skb)
 		if (!n && (vxlan->flags & VXLAN_F_L3MISS)) {
 			union vxlan_addr ipa = {
 				.sin.sin_addr.s_addr = pip->daddr,
-				.sa.sa_family = AF_INET,
+				.sin.sin_family = AF_INET,
 			};
 
 			vxlan_ip_miss(dev, &ipa);
@@ -1542,7 +1536,7 @@ static bool route_shortcircuit(struct net_device *dev, struct sk_buff *skb)
 		if (!n && (vxlan->flags & VXLAN_F_L3MISS)) {
 			union vxlan_addr ipa = {
 				.sin6.sin6_addr = pip6->daddr,
-				.sa.sa_family = AF_INET6,
+				.sin6.sin6_family = AF_INET6,
 			};
 
 			vxlan_ip_miss(dev, &ipa);
@@ -1572,13 +1566,6 @@ static bool route_shortcircuit(struct net_device *dev, struct sk_buff *skb)
 	return false;
 }
 
-static inline struct sk_buff *vxlan_handle_offloads(struct sk_buff *skb,
-						    bool udp_csum)
-{
-	int type = udp_csum ? SKB_GSO_UDP_TUNNEL_CSUM : SKB_GSO_UDP_TUNNEL;
-	return iptunnel_handle_offloads(skb, udp_csum, type);
-}
-
 #if IS_ENABLED(CONFIG_IPV6)
 static int vxlan6_xmit_skb(struct vxlan_sock *vs,
 			   struct dst_entry *dst, struct sk_buff *skb,
@@ -1587,13 +1574,12 @@ static int vxlan6_xmit_skb(struct vxlan_sock *vs,
 			   __be16 src_port, __be16 dst_port, __be32 vni,
 			   bool xnet)
 {
-	struct ipv6hdr *ip6h;
 	struct vxlanhdr *vxh;
-	struct udphdr *uh;
 	int min_headroom;
 	int err;
+	bool udp_sum = !udp_get_no_check6_tx(vs->sock->sk);
 
-	skb = vxlan_handle_offloads(skb, !udp_get_no_check6_tx(vs->sock->sk));
+	skb = udp_tunnel_handle_offloads(skb, udp_sum);
 	if (IS_ERR(skb))
 		return -EINVAL;
 
@@ -1621,38 +1607,10 @@ static int vxlan6_xmit_skb(struct vxlan_sock *vs,
 	vxh->vx_flags = htonl(VXLAN_FLAGS);
 	vxh->vx_vni = vni;
 
-	__skb_push(skb, sizeof(*uh));
-	skb_reset_transport_header(skb);
-	uh = udp_hdr(skb);
-
-	uh->dest = dst_port;
-	uh->source = src_port;
-
-	uh->len = htons(skb->len);
-
-	memset(&(IPCB(skb)->opt), 0, sizeof(IPCB(skb)->opt));
-	IPCB(skb)->flags &= ~(IPSKB_XFRM_TUNNEL_SIZE | IPSKB_XFRM_TRANSFORMED |
-			      IPSKB_REROUTED);
-	skb_dst_set(skb, dst);
+	skb_set_inner_protocol(skb, htons(ETH_P_TEB));
 
-	udp6_set_csum(udp_get_no_check6_tx(vs->sock->sk), skb,
-		      saddr, daddr, skb->len);
-
-	__skb_push(skb, sizeof(*ip6h));
-	skb_reset_network_header(skb);
-	ip6h		  = ipv6_hdr(skb);
-	ip6h->version	  = 6;
-	ip6h->priority	  = prio;
-	ip6h->flow_lbl[0] = 0;
-	ip6h->flow_lbl[1] = 0;
-	ip6h->flow_lbl[2] = 0;
-	ip6h->payload_len = htons(skb->len);
-	ip6h->nexthdr     = IPPROTO_UDP;
-	ip6h->hop_limit   = ttl;
-	ip6h->daddr	  = *daddr;
-	ip6h->saddr	  = *saddr;
-
-	ip6tunnel_xmit(skb, dev);
+	udp_tunnel6_xmit_skb(vs->sock, dst, skb, dev, saddr, daddr, prio,
+			     ttl, src_port, dst_port);
 	return 0;
 }
 #endif
@@ -1663,11 +1621,11 @@ int vxlan_xmit_skb(struct vxlan_sock *vs,
 		   __be16 src_port, __be16 dst_port, __be32 vni, bool xnet)
 {
 	struct vxlanhdr *vxh;
-	struct udphdr *uh;
 	int min_headroom;
 	int err;
+	bool udp_sum = !vs->sock->sk->sk_no_check_tx;
 
-	skb = vxlan_handle_offloads(skb, !vs->sock->sk->sk_no_check_tx);
+	skb = udp_tunnel_handle_offloads(skb, udp_sum);
 	if (IS_ERR(skb))
 		return -EINVAL;
 
@@ -1693,20 +1651,10 @@ int vxlan_xmit_skb(struct vxlan_sock *vs,
 	vxh->vx_flags = htonl(VXLAN_FLAGS);
 	vxh->vx_vni = vni;
 
-	__skb_push(skb, sizeof(*uh));
-	skb_reset_transport_header(skb);
-	uh = udp_hdr(skb);
-
-	uh->dest = dst_port;
-	uh->source = src_port;
+	skb_set_inner_protocol(skb, htons(ETH_P_TEB));
 
-	uh->len = htons(skb->len);
-
-	udp_set_csum(vs->sock->sk->sk_no_check_tx, skb,
-		     src, dst, skb->len);
-
-	return iptunnel_xmit(vs->sock->sk, rt, skb, src, dst, IPPROTO_UDP,
-			     tos, ttl, df, xnet);
+	return udp_tunnel_xmit_skb(vs->sock, rt, skb, src, dst, tos,
+				   ttl, df, src_port, dst_port, xnet);
 }
 EXPORT_SYMBOL_GPL(vxlan_xmit_skb);
 
@@ -1717,6 +1665,8 @@ static void vxlan_encap_bypass(struct sk_buff *skb, struct vxlan_dev *src_vxlan,
 	struct pcpu_sw_netstats *tx_stats, *rx_stats;
 	union vxlan_addr loopback;
 	union vxlan_addr *remote_ip = &dst_vxlan->default_dst.remote_ip;
+	struct net_device *dev = skb->dev;
+	int len = skb->len;
 
 	tx_stats = this_cpu_ptr(src_vxlan->dev->tstats);
 	rx_stats = this_cpu_ptr(dst_vxlan->dev->tstats);
@@ -1740,16 +1690,16 @@ static void vxlan_encap_bypass(struct sk_buff *skb, struct vxlan_dev *src_vxlan,
 
 	u64_stats_update_begin(&tx_stats->syncp);
 	tx_stats->tx_packets++;
-	tx_stats->tx_bytes += skb->len;
+	tx_stats->tx_bytes += len;
 	u64_stats_update_end(&tx_stats->syncp);
 
 	if (netif_rx(skb) == NET_RX_SUCCESS) {
 		u64_stats_update_begin(&rx_stats->syncp);
 		rx_stats->rx_packets++;
-		rx_stats->rx_bytes += skb->len;
+		rx_stats->rx_bytes += len;
 		u64_stats_update_end(&rx_stats->syncp);
 	} else {
-		skb->dev->stats.rx_dropped++;
+		dev->stats.rx_dropped++;
 	}
 }
 
@@ -1927,7 +1877,8 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 			return arp_reduce(dev, skb);
 #if IS_ENABLED(CONFIG_IPV6)
 		else if (ntohs(eth->h_proto) == ETH_P_IPV6 &&
-			 skb->len >= sizeof(struct ipv6hdr) + sizeof(struct nd_msg) &&
+			 pskb_may_pull(skb, sizeof(struct ipv6hdr)
+				       + sizeof(struct nd_msg)) &&
 			 ipv6_hdr(skb)->nexthdr == IPPROTO_ICMPV6) {
 				struct nd_msg *msg;
 
@@ -1936,6 +1887,7 @@ static netdev_tx_t vxlan_xmit(struct sk_buff *skb, struct net_device *dev)
 				    msg->icmph.icmp6_type == NDISC_NEIGHBOUR_SOLICITATION)
 					return neigh_reduce(dev, skb);
 		}
+		eth = eth_hdr(skb);
 #endif
 	}
 
@@ -2242,7 +2194,7 @@ static void vxlan_setup(struct net_device *dev)
 	dev->hw_features |= NETIF_F_SG | NETIF_F_HW_CSUM | NETIF_F_RXCSUM;
 	dev->hw_features |= NETIF_F_GSO_SOFTWARE;
 	dev->hw_features |= NETIF_F_HW_VLAN_CTAG_TX | NETIF_F_HW_VLAN_STAG_TX;
-	dev->priv_flags	&= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(dev);
 	dev->priv_flags |= IFF_LIVE_ADDR_CHANGE;
 
 	INIT_LIST_HEAD(&vxlan->next);
@@ -2335,8 +2287,7 @@ static const struct ethtool_ops vxlan_ethtool_ops = {
 static void vxlan_del_work(struct work_struct *work)
 {
 	struct vxlan_sock *vs = container_of(work, struct vxlan_sock, del_work);
-
-	sk_release_kernel(vs->sock->sk);
+	udp_tunnel_sock_release(vs->sock);
 	kfree_rcu(vs, rcu);
 }
 
@@ -2369,9 +2320,6 @@ static struct socket *vxlan_create_sock(struct net *net, bool ipv6,
 	if (err < 0)
 		return ERR_PTR(err);
 
-	/* Disable multicast loopback */
-	inet_sk(sock->sk)->mc_loop = 0;
-
 	return sock;
 }
 
@@ -2383,9 +2331,9 @@ static struct vxlan_sock *vxlan_socket_create(struct net *net, __be16 port,
 	struct vxlan_net *vn = net_generic(net, vxlan_net_id);
 	struct vxlan_sock *vs;
 	struct socket *sock;
-	struct sock *sk;
 	unsigned int h;
 	bool ipv6 = !!(flags & VXLAN_F_IPV6);
+	struct udp_tunnel_sock_cfg tunnel_cfg;
 
 	vs = kzalloc(sizeof(*vs), GFP_KERNEL);
 	if (!vs)
@@ -2403,11 +2351,9 @@ static struct vxlan_sock *vxlan_socket_create(struct net *net, __be16 port,
 	}
 
 	vs->sock = sock;
-	sk = sock->sk;
 	atomic_set(&vs->refcnt, 1);
 	vs->rcv = rcv;
 	vs->data = data;
-	rcu_assign_sk_user_data(vs->sock->sk, vs);
 
 	/* Initialize the vxlan udp offloads structure */
 	vs->udp_offloads.port = port;
@@ -2420,14 +2366,12 @@ static struct vxlan_sock *vxlan_socket_create(struct net *net, __be16 port,
 	spin_unlock(&vn->sock_lock);
 
 	/* Mark socket as an encapsulation socket. */
-	udp_sk(sk)->encap_type = 1;
-	udp_sk(sk)->encap_rcv = vxlan_udp_encap_recv;
-#if IS_ENABLED(CONFIG_IPV6)
-	if (ipv6)
-		ipv6_stub->udpv6_encap_enable();
-	else
-#endif
-		udp_encap_enable();
+	tunnel_cfg.sk_user_data = vs;
+	tunnel_cfg.encap_type = 1;
+	tunnel_cfg.encap_rcv = vxlan_udp_encap_recv;
+	tunnel_cfg.encap_destroy = NULL;
+
+	setup_udp_tunnel_sock(net, sock, &tunnel_cfg);
 
 	return vs;
 }
diff --git a/drivers/net/wan/dlci.c b/drivers/net/wan/dlci.c
index 43c9960dce1c..ae6ecf401189 100644
--- a/drivers/net/wan/dlci.c
+++ b/drivers/net/wan/dlci.c
@@ -192,8 +192,10 @@ static netdev_tx_t dlci_transmit(struct sk_buff *skb, struct net_device *dev)
 {
 	struct dlci_local *dlp = netdev_priv(dev);
 
-	if (skb)
-		dlp->slave->netdev_ops->ndo_start_xmit(skb, dlp->slave);
+	if (skb) {
+		struct netdev_queue *txq = skb_get_tx_queue(dev, skb);
+		netdev_start_xmit(skb, dlp->slave, txq, false);
+	}
 	return NETDEV_TX_OK;
 }
 
diff --git a/drivers/net/wan/hdlc_fr.c b/drivers/net/wan/hdlc_fr.c
index e5c7e6165a4b..3ebed1c40abb 100644
--- a/drivers/net/wan/hdlc_fr.c
+++ b/drivers/net/wan/hdlc_fr.c
@@ -1047,7 +1047,7 @@ static void pvc_setup(struct net_device *dev)
 	dev->flags = IFF_POINTOPOINT;
 	dev->hard_header_len = 10;
 	dev->addr_len = 2;
-	dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(dev);
 }
 
 static const struct net_device_ops pvc_ops = {
diff --git a/drivers/net/wimax/Makefile b/drivers/net/wimax/Makefile
index 992bc02bc016..692184dd674a 100644
--- a/drivers/net/wimax/Makefile
+++ b/drivers/net/wimax/Makefile
@@ -1,5 +1 @@
-
 obj-$(CONFIG_WIMAX_I2400M)	+= i2400m/
-
-# (from Sam Ravnborg) force kbuild to create built-in.o
-obj- := dummy.o
diff --git a/drivers/net/wireless/at76c50x-usb.c b/drivers/net/wireless/at76c50x-usb.c
index 334c2ece855a..da92bfa76b7c 100644
--- a/drivers/net/wireless/at76c50x-usb.c
+++ b/drivers/net/wireless/at76c50x-usb.c
@@ -2423,8 +2423,6 @@ static void at76_delete_device(struct at76_priv *priv)
 
 	kfree_skb(priv->rx_skb);
 
-	usb_put_dev(priv->udev);
-
 	at76_dbg(DBG_PROC_ENTRY, "%s: before freeing priv/ieee80211_hw",
 		 __func__);
 	ieee80211_free_hw(priv->hw);
@@ -2558,6 +2556,7 @@ static void at76_disconnect(struct usb_interface *interface)
 
 	wiphy_info(priv->hw->wiphy, "disconnecting\n");
 	at76_delete_device(priv);
+	usb_put_dev(priv->udev);
 	dev_info(&interface->dev, "disconnected\n");
 }
 
diff --git a/drivers/net/wireless/ath/Kconfig b/drivers/net/wireless/ath/Kconfig
index c63d1159db5c..ce7826009eeb 100644
--- a/drivers/net/wireless/ath/Kconfig
+++ b/drivers/net/wireless/ath/Kconfig
@@ -25,6 +25,14 @@ config ATH_DEBUG
 	  Say Y, if you want to debug atheros wireless drivers.
 	  Right now only ath9k makes use of this.
 
+config ATH_TRACEPOINTS
+       bool "Atheros wireless tracing"
+       depends on ATH_DEBUG
+       depends on EVENT_TRACING
+       ---help---
+         This option enables tracepoints for atheros wireless drivers.
+	 Currently, ath9k makes use of this facility.
+
 config ATH_REG_DYNAMIC_USER_REG_HINTS
 	bool "Atheros dynamic user regulatory hints"
 	depends on CFG80211_CERTIFICATION_ONUS
diff --git a/drivers/net/wireless/ath/Makefile b/drivers/net/wireless/ath/Makefile
index 7d023b0f13b4..89f8d5979402 100644
--- a/drivers/net/wireless/ath/Makefile
+++ b/drivers/net/wireless/ath/Makefile
@@ -17,4 +17,8 @@ ath-objs :=	main.o \
 		dfs_pri_detector.o
 
 ath-$(CONFIG_ATH_DEBUG) += debug.o
+ath-$(CONFIG_ATH_TRACEPOINTS) += trace.o
+
 ccflags-y += -D__CHECK_ENDIAN__
+
+CFLAGS_trace.o := -I$(src)
diff --git a/drivers/net/wireless/ath/ath.h b/drivers/net/wireless/ath/ath.h
index fd9e5305e77f..e5ba6faf3281 100644
--- a/drivers/net/wireless/ath/ath.h
+++ b/drivers/net/wireless/ath/ath.h
@@ -234,6 +234,7 @@ void ath_printk(const char *level, const struct ath_common *common,
  *	AR9462.
  * @ATH_DBG_DFS: radar datection
  * @ATH_DBG_WOW: Wake on Wireless
+ * @ATH_DBG_DYNACK: dynack handling
  * @ATH_DBG_ANY: enable all debugging
  *
  * The debug level is used to control the amount and type of debugging output
@@ -261,10 +262,13 @@ enum ATH_DEBUG {
 	ATH_DBG_MCI		= 0x00008000,
 	ATH_DBG_DFS		= 0x00010000,
 	ATH_DBG_WOW		= 0x00020000,
+	ATH_DBG_CHAN_CTX	= 0x00040000,
+	ATH_DBG_DYNACK		= 0x00080000,
 	ATH_DBG_ANY		= 0xffffffff
 };
 
 #define ATH_DBG_DEFAULT (ATH_DBG_FATAL)
+#define ATH_DBG_MAX_LEN 512
 
 #ifdef CONFIG_ATH_DEBUG
 
diff --git a/drivers/net/wireless/ath/ath10k/Kconfig b/drivers/net/wireless/ath/ath10k/Kconfig
index a6f5285235af..72acb822bb11 100644
--- a/drivers/net/wireless/ath/ath10k/Kconfig
+++ b/drivers/net/wireless/ath/ath10k/Kconfig
@@ -24,7 +24,8 @@ config ATH10K_DEBUG
 
 config ATH10K_DEBUGFS
 	bool "Atheros ath10k debugfs support"
-	depends on ATH10K
+	depends on ATH10K && DEBUG_FS
+	select RELAY
 	---help---
 	  Enabled debugfs support
 
diff --git a/drivers/net/wireless/ath/ath10k/Makefile b/drivers/net/wireless/ath/ath10k/Makefile
index a4179f49ee1f..8b1b1adb477a 100644
--- a/drivers/net/wireless/ath/ath10k/Makefile
+++ b/drivers/net/wireless/ath/ath10k/Makefile
@@ -10,6 +10,8 @@ ath10k_core-y += mac.o \
 		 wmi.o \
 		 bmi.o
 
+ath10k_core-$(CONFIG_ATH10K_DEBUGFS) += spectral.o
+ath10k_core-$(CONFIG_NL80211_TESTMODE) += testmode.o
 ath10k_core-$(CONFIG_ATH10K_TRACING) += trace.o
 
 obj-$(CONFIG_ATH10K_PCI) += ath10k_pci.o
diff --git a/drivers/net/wireless/ath/ath10k/bmi.c b/drivers/net/wireless/ath/ath10k/bmi.c
index 17d221abd58c..3d29b0875b3e 100644
--- a/drivers/net/wireless/ath/ath10k/bmi.c
+++ b/drivers/net/wireless/ath/ath10k/bmi.c
@@ -22,7 +22,7 @@
 
 void ath10k_bmi_start(struct ath10k *ar)
 {
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi start\n");
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi start\n");
 
 	ar->bmi.done_sent = false;
 }
@@ -33,10 +33,10 @@ int ath10k_bmi_done(struct ath10k *ar)
 	u32 cmdlen = sizeof(cmd.id) + sizeof(cmd.done);
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi done\n");
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi done\n");
 
 	if (ar->bmi.done_sent) {
-		ath10k_dbg(ATH10K_DBG_BMI, "bmi skipped\n");
+		ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi skipped\n");
 		return 0;
 	}
 
@@ -45,7 +45,7 @@ int ath10k_bmi_done(struct ath10k *ar)
 
 	ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, cmdlen, NULL, NULL);
 	if (ret) {
-		ath10k_warn("unable to write to the device: %d\n", ret);
+		ath10k_warn(ar, "unable to write to the device: %d\n", ret);
 		return ret;
 	}
 
@@ -61,10 +61,10 @@ int ath10k_bmi_get_target_info(struct ath10k *ar,
 	u32 resplen = sizeof(resp.get_target_info);
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi get target info\n");
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi get target info\n");
 
 	if (ar->bmi.done_sent) {
-		ath10k_warn("BMI Get Target Info Command disallowed\n");
+		ath10k_warn(ar, "BMI Get Target Info Command disallowed\n");
 		return -EBUSY;
 	}
 
@@ -72,12 +72,12 @@ int ath10k_bmi_get_target_info(struct ath10k *ar,
 
 	ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, cmdlen, &resp, &resplen);
 	if (ret) {
-		ath10k_warn("unable to get target info from device\n");
+		ath10k_warn(ar, "unable to get target info from device\n");
 		return ret;
 	}
 
 	if (resplen < sizeof(resp.get_target_info)) {
-		ath10k_warn("invalid get_target_info response length (%d)\n",
+		ath10k_warn(ar, "invalid get_target_info response length (%d)\n",
 			    resplen);
 		return -EIO;
 	}
@@ -97,11 +97,11 @@ int ath10k_bmi_read_memory(struct ath10k *ar,
 	u32 rxlen;
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi read address 0x%x length %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi read address 0x%x length %d\n",
 		   address, length);
 
 	if (ar->bmi.done_sent) {
-		ath10k_warn("command disallowed\n");
+		ath10k_warn(ar, "command disallowed\n");
 		return -EBUSY;
 	}
 
@@ -115,7 +115,7 @@ int ath10k_bmi_read_memory(struct ath10k *ar,
 		ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, cmdlen,
 						  &resp, &rxlen);
 		if (ret) {
-			ath10k_warn("unable to read from the device (%d)\n",
+			ath10k_warn(ar, "unable to read from the device (%d)\n",
 				    ret);
 			return ret;
 		}
@@ -137,11 +137,11 @@ int ath10k_bmi_write_memory(struct ath10k *ar,
 	u32 txlen;
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi write address 0x%x length %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi write address 0x%x length %d\n",
 		   address, length);
 
 	if (ar->bmi.done_sent) {
-		ath10k_warn("command disallowed\n");
+		ath10k_warn(ar, "command disallowed\n");
 		return -EBUSY;
 	}
 
@@ -159,7 +159,7 @@ int ath10k_bmi_write_memory(struct ath10k *ar,
 		ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, hdrlen + txlen,
 						  NULL, NULL);
 		if (ret) {
-			ath10k_warn("unable to write to the device (%d)\n",
+			ath10k_warn(ar, "unable to write to the device (%d)\n",
 				    ret);
 			return ret;
 		}
@@ -183,11 +183,11 @@ int ath10k_bmi_execute(struct ath10k *ar, u32 address, u32 param, u32 *result)
 	u32 resplen = sizeof(resp.execute);
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi execute address 0x%x param 0x%x\n",
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi execute address 0x%x param 0x%x\n",
 		   address, param);
 
 	if (ar->bmi.done_sent) {
-		ath10k_warn("command disallowed\n");
+		ath10k_warn(ar, "command disallowed\n");
 		return -EBUSY;
 	}
 
@@ -197,19 +197,19 @@ int ath10k_bmi_execute(struct ath10k *ar, u32 address, u32 param, u32 *result)
 
 	ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, cmdlen, &resp, &resplen);
 	if (ret) {
-		ath10k_warn("unable to read from the device\n");
+		ath10k_warn(ar, "unable to read from the device\n");
 		return ret;
 	}
 
 	if (resplen < sizeof(resp.execute)) {
-		ath10k_warn("invalid execute response length (%d)\n",
+		ath10k_warn(ar, "invalid execute response length (%d)\n",
 			    resplen);
 		return -EIO;
 	}
 
 	*result = __le32_to_cpu(resp.execute.result);
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi execute result 0x%x\n", *result);
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi execute result 0x%x\n", *result);
 
 	return 0;
 }
@@ -221,11 +221,11 @@ int ath10k_bmi_lz_data(struct ath10k *ar, const void *buffer, u32 length)
 	u32 txlen;
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi lz data buffer 0x%p length %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi lz data buffer 0x%p length %d\n",
 		   buffer, length);
 
 	if (ar->bmi.done_sent) {
-		ath10k_warn("command disallowed\n");
+		ath10k_warn(ar, "command disallowed\n");
 		return -EBUSY;
 	}
 
@@ -241,7 +241,7 @@ int ath10k_bmi_lz_data(struct ath10k *ar, const void *buffer, u32 length)
 		ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, hdrlen + txlen,
 						  NULL, NULL);
 		if (ret) {
-			ath10k_warn("unable to write to the device\n");
+			ath10k_warn(ar, "unable to write to the device\n");
 			return ret;
 		}
 
@@ -258,11 +258,11 @@ int ath10k_bmi_lz_stream_start(struct ath10k *ar, u32 address)
 	u32 cmdlen = sizeof(cmd.id) + sizeof(cmd.lz_start);
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI, "bmi lz stream start address 0x%x\n",
+	ath10k_dbg(ar, ATH10K_DBG_BMI, "bmi lz stream start address 0x%x\n",
 		   address);
 
 	if (ar->bmi.done_sent) {
-		ath10k_warn("command disallowed\n");
+		ath10k_warn(ar, "command disallowed\n");
 		return -EBUSY;
 	}
 
@@ -271,7 +271,7 @@ int ath10k_bmi_lz_stream_start(struct ath10k *ar, u32 address)
 
 	ret = ath10k_hif_exchange_bmi_msg(ar, &cmd, cmdlen, NULL, NULL);
 	if (ret) {
-		ath10k_warn("unable to Start LZ Stream to the device\n");
+		ath10k_warn(ar, "unable to Start LZ Stream to the device\n");
 		return ret;
 	}
 
@@ -286,7 +286,7 @@ int ath10k_bmi_fast_download(struct ath10k *ar,
 	u32 trailer_len = length - head_len;
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BMI,
+	ath10k_dbg(ar, ATH10K_DBG_BMI,
 		   "bmi fast download address 0x%x buffer 0x%p length %d\n",
 		   address, buffer, length);
 
diff --git a/drivers/net/wireless/ath/ath10k/bmi.h b/drivers/net/wireless/ath/ath10k/bmi.h
index 111ab701465c..31a990635490 100644
--- a/drivers/net/wireless/ath/ath10k/bmi.h
+++ b/drivers/net/wireless/ath/ath10k/bmi.h
@@ -177,7 +177,6 @@ struct bmi_target_info {
 	u32 type;
 };
 
-
 /* in msec */
 #define BMI_COMMUNICATION_TIMEOUT_HZ (1*HZ)
 
diff --git a/drivers/net/wireless/ath/ath10k/ce.c b/drivers/net/wireless/ath/ath10k/ce.c
index 4333107ecf37..101cadb6e4ba 100644
--- a/drivers/net/wireless/ath/ath10k/ce.c
+++ b/drivers/net/wireless/ath/ath10k/ce.c
@@ -260,7 +260,6 @@ static inline void ath10k_ce_engine_int_status_clear(struct ath10k *ar,
 	ath10k_pci_write32(ar, ce_ctrl_addr + HOST_IS_ADDRESS, mask);
 }
 
-
 /*
  * Guts of ath10k_ce_send, used by both ath10k_ce_send and
  * ath10k_ce_sendlist_send.
@@ -284,13 +283,9 @@ int ath10k_ce_send_nolock(struct ath10k_ce_pipe *ce_state,
 	int ret = 0;
 
 	if (nbytes > ce_state->src_sz_max)
-		ath10k_warn("%s: send more we can (nbytes: %d, max: %d)\n",
+		ath10k_warn(ar, "%s: send more we can (nbytes: %d, max: %d)\n",
 			    __func__, nbytes, ce_state->src_sz_max);
 
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		return ret;
-
 	if (unlikely(CE_RING_DELTA(nentries_mask,
 				   write_index, sw_index - 1) <= 0)) {
 		ret = -ENOSR;
@@ -325,7 +320,6 @@ int ath10k_ce_send_nolock(struct ath10k_ce_pipe *ce_state,
 
 	src_ring->write_index = write_index;
 exit:
-	ath10k_pci_sleep(ar);
 	return ret;
 }
 
@@ -390,49 +384,56 @@ int ath10k_ce_num_free_src_entries(struct ath10k_ce_pipe *pipe)
 	return delta;
 }
 
-int ath10k_ce_recv_buf_enqueue(struct ath10k_ce_pipe *ce_state,
-			       void *per_recv_context,
-			       u32 buffer)
+int __ath10k_ce_rx_num_free_bufs(struct ath10k_ce_pipe *pipe)
 {
-	struct ath10k_ce_ring *dest_ring = ce_state->dest_ring;
-	u32 ctrl_addr = ce_state->ctrl_addr;
-	struct ath10k *ar = ce_state->ar;
+	struct ath10k *ar = pipe->ar;
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	struct ath10k_ce_ring *dest_ring = pipe->dest_ring;
 	unsigned int nentries_mask = dest_ring->nentries_mask;
-	unsigned int write_index;
-	unsigned int sw_index;
-	int ret;
+	unsigned int write_index = dest_ring->write_index;
+	unsigned int sw_index = dest_ring->sw_index;
 
-	spin_lock_bh(&ar_pci->ce_lock);
-	write_index = dest_ring->write_index;
-	sw_index = dest_ring->sw_index;
+	lockdep_assert_held(&ar_pci->ce_lock);
 
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		goto out;
+	return CE_RING_DELTA(nentries_mask, write_index, sw_index - 1);
+}
 
-	if (CE_RING_DELTA(nentries_mask, write_index, sw_index - 1) > 0) {
-		struct ce_desc *base = dest_ring->base_addr_owner_space;
-		struct ce_desc *desc = CE_DEST_RING_TO_DESC(base, write_index);
+int __ath10k_ce_rx_post_buf(struct ath10k_ce_pipe *pipe, void *ctx, u32 paddr)
+{
+	struct ath10k *ar = pipe->ar;
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	struct ath10k_ce_ring *dest_ring = pipe->dest_ring;
+	unsigned int nentries_mask = dest_ring->nentries_mask;
+	unsigned int write_index = dest_ring->write_index;
+	unsigned int sw_index = dest_ring->sw_index;
+	struct ce_desc *base = dest_ring->base_addr_owner_space;
+	struct ce_desc *desc = CE_DEST_RING_TO_DESC(base, write_index);
+	u32 ctrl_addr = pipe->ctrl_addr;
 
-		/* Update destination descriptor */
-		desc->addr    = __cpu_to_le32(buffer);
-		desc->nbytes = 0;
+	lockdep_assert_held(&ar_pci->ce_lock);
 
-		dest_ring->per_transfer_context[write_index] =
-							per_recv_context;
+	if (CE_RING_DELTA(nentries_mask, write_index, sw_index - 1) == 0)
+		return -EIO;
 
-		/* Update Destination Ring Write Index */
-		write_index = CE_RING_IDX_INCR(nentries_mask, write_index);
-		ath10k_ce_dest_ring_write_index_set(ar, ctrl_addr, write_index);
-		dest_ring->write_index = write_index;
-		ret = 0;
-	} else {
-		ret = -EIO;
-	}
-	ath10k_pci_sleep(ar);
+	desc->addr = __cpu_to_le32(paddr);
+	desc->nbytes = 0;
+
+	dest_ring->per_transfer_context[write_index] = ctx;
+	write_index = CE_RING_IDX_INCR(nentries_mask, write_index);
+	ath10k_ce_dest_ring_write_index_set(ar, ctrl_addr, write_index);
+	dest_ring->write_index = write_index;
+
+	return 0;
+}
+
+int ath10k_ce_rx_post_buf(struct ath10k_ce_pipe *pipe, void *ctx, u32 paddr)
+{
+	struct ath10k *ar = pipe->ar;
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	int ret;
 
-out:
+	spin_lock_bh(&ar_pci->ce_lock);
+	ret = __ath10k_ce_rx_post_buf(pipe, ctx, paddr);
 	spin_unlock_bh(&ar_pci->ce_lock);
 
 	return ret;
@@ -588,7 +589,6 @@ static int ath10k_ce_completed_send_next_nolock(struct ath10k_ce_pipe *ce_state,
 	unsigned int sw_index = src_ring->sw_index;
 	struct ce_desc *sdesc, *sbase;
 	unsigned int read_index;
-	int ret;
 
 	if (src_ring->hw_index == sw_index) {
 		/*
@@ -599,18 +599,12 @@ static int ath10k_ce_completed_send_next_nolock(struct ath10k_ce_pipe *ce_state,
 		 * value of the HW index has become stale.
 		 */
 
-		ret = ath10k_pci_wake(ar);
-		if (ret)
-			return ret;
-
 		read_index = ath10k_ce_src_ring_read_index_get(ar, ctrl_addr);
 		if (read_index == 0xffffffff)
 			return -ENODEV;
 
 		read_index &= nentries_mask;
 		src_ring->hw_index = read_index;
-
-		ath10k_pci_sleep(ar);
 	}
 
 	read_index = src_ring->hw_index;
@@ -731,11 +725,6 @@ void ath10k_ce_per_engine_service(struct ath10k *ar, unsigned int ce_id)
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	struct ath10k_ce_pipe *ce_state = &ar_pci->ce_states[ce_id];
 	u32 ctrl_addr = ce_state->ctrl_addr;
-	int ret;
-
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		return;
 
 	spin_lock_bh(&ar_pci->ce_lock);
 
@@ -760,7 +749,6 @@ void ath10k_ce_per_engine_service(struct ath10k *ar, unsigned int ce_id)
 	ath10k_ce_engine_int_status_clear(ar, ctrl_addr, CE_WATERMARK_MASK);
 
 	spin_unlock_bh(&ar_pci->ce_lock);
-	ath10k_pci_sleep(ar);
 }
 
 /*
@@ -771,13 +759,9 @@ void ath10k_ce_per_engine_service(struct ath10k *ar, unsigned int ce_id)
 
 void ath10k_ce_per_engine_service_any(struct ath10k *ar)
 {
-	int ce_id, ret;
+	int ce_id;
 	u32 intr_summary;
 
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		return;
-
 	intr_summary = CE_INTERRUPT_SUMMARY(ar);
 
 	for (ce_id = 0; intr_summary && (ce_id < CE_COUNT); ce_id++) {
@@ -789,8 +773,6 @@ void ath10k_ce_per_engine_service_any(struct ath10k *ar)
 
 		ath10k_ce_per_engine_service(ar, ce_id);
 	}
-
-	ath10k_pci_sleep(ar);
 }
 
 /*
@@ -800,16 +782,11 @@ void ath10k_ce_per_engine_service_any(struct ath10k *ar)
  *
  * Called with ce_lock held.
  */
-static void ath10k_ce_per_engine_handler_adjust(struct ath10k_ce_pipe *ce_state,
-						int disable_copy_compl_intr)
+static void ath10k_ce_per_engine_handler_adjust(struct ath10k_ce_pipe *ce_state)
 {
 	u32 ctrl_addr = ce_state->ctrl_addr;
 	struct ath10k *ar = ce_state->ar;
-	int ret;
-
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		return;
+	bool disable_copy_compl_intr = ce_state->attr_flags & CE_ATTR_DIS_INTR;
 
 	if ((!disable_copy_compl_intr) &&
 	    (ce_state->send_cb || ce_state->recv_cb))
@@ -818,17 +795,11 @@ static void ath10k_ce_per_engine_handler_adjust(struct ath10k_ce_pipe *ce_state,
 		ath10k_ce_copy_complete_intr_disable(ar, ctrl_addr);
 
 	ath10k_ce_watermark_intr_disable(ar, ctrl_addr);
-
-	ath10k_pci_sleep(ar);
 }
 
 int ath10k_ce_disable_interrupts(struct ath10k *ar)
 {
-	int ce_id, ret;
-
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		return ret;
+	int ce_id;
 
 	for (ce_id = 0; ce_id < CE_COUNT; ce_id++) {
 		u32 ctrl_addr = ath10k_ce_base_address(ce_id);
@@ -838,34 +809,16 @@ int ath10k_ce_disable_interrupts(struct ath10k *ar)
 		ath10k_ce_watermark_intr_disable(ar, ctrl_addr);
 	}
 
-	ath10k_pci_sleep(ar);
-
 	return 0;
 }
 
-void ath10k_ce_send_cb_register(struct ath10k_ce_pipe *ce_state,
-				void (*send_cb)(struct ath10k_ce_pipe *),
-				int disable_interrupts)
+void ath10k_ce_enable_interrupts(struct ath10k *ar)
 {
-	struct ath10k *ar = ce_state->ar;
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	int ce_id;
 
-	spin_lock_bh(&ar_pci->ce_lock);
-	ce_state->send_cb = send_cb;
-	ath10k_ce_per_engine_handler_adjust(ce_state, disable_interrupts);
-	spin_unlock_bh(&ar_pci->ce_lock);
-}
-
-void ath10k_ce_recv_cb_register(struct ath10k_ce_pipe *ce_state,
-				void (*recv_cb)(struct ath10k_ce_pipe *))
-{
-	struct ath10k *ar = ce_state->ar;
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	spin_lock_bh(&ar_pci->ce_lock);
-	ce_state->recv_cb = recv_cb;
-	ath10k_ce_per_engine_handler_adjust(ce_state, 0);
-	spin_unlock_bh(&ar_pci->ce_lock);
+	for (ce_id = 0; ce_id < CE_COUNT; ce_id++)
+		ath10k_ce_per_engine_handler_adjust(&ar_pci->ce_states[ce_id]);
 }
 
 static int ath10k_ce_init_src_ring(struct ath10k *ar,
@@ -898,7 +851,7 @@ static int ath10k_ce_init_src_ring(struct ath10k *ar,
 	ath10k_ce_src_ring_lowmark_set(ar, ctrl_addr, 0);
 	ath10k_ce_src_ring_highmark_set(ar, ctrl_addr, nentries);
 
-	ath10k_dbg(ATH10K_DBG_BOOT,
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
 		   "boot init ce src ring id %d entries %d base_addr %p\n",
 		   ce_id, nentries, src_ring->base_addr_owner_space);
 
@@ -932,7 +885,7 @@ static int ath10k_ce_init_dest_ring(struct ath10k *ar,
 	ath10k_ce_dest_ring_lowmark_set(ar, ctrl_addr, 0);
 	ath10k_ce_dest_ring_highmark_set(ar, ctrl_addr, nentries);
 
-	ath10k_dbg(ATH10K_DBG_BOOT,
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
 		   "boot ce dest ring id %d entries %d base_addr %p\n",
 		   ce_id, nentries, dest_ring->base_addr_owner_space);
 
@@ -1067,7 +1020,9 @@ ath10k_ce_alloc_dest_ring(struct ath10k *ar, unsigned int ce_id,
  * initialized by software/firmware.
  */
 int ath10k_ce_init_pipe(struct ath10k *ar, unsigned int ce_id,
-			const struct ce_attr *attr)
+			const struct ce_attr *attr,
+			void (*send_cb)(struct ath10k_ce_pipe *),
+			void (*recv_cb)(struct ath10k_ce_pipe *))
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	struct ath10k_ce_pipe *ce_state = &ar_pci->ce_states[ce_id];
@@ -1084,39 +1039,37 @@ int ath10k_ce_init_pipe(struct ath10k *ar, unsigned int ce_id,
 	BUILD_BUG_ON(2*TARGET_10X_NUM_MSDU_DESC >
 		     (CE_HTT_H2T_MSG_SRC_NENTRIES - 1));
 
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		return ret;
-
 	spin_lock_bh(&ar_pci->ce_lock);
 	ce_state->ar = ar;
 	ce_state->id = ce_id;
 	ce_state->ctrl_addr = ath10k_ce_base_address(ce_id);
 	ce_state->attr_flags = attr->flags;
 	ce_state->src_sz_max = attr->src_sz_max;
+	if (attr->src_nentries)
+		ce_state->send_cb = send_cb;
+	if (attr->dest_nentries)
+		ce_state->recv_cb = recv_cb;
 	spin_unlock_bh(&ar_pci->ce_lock);
 
 	if (attr->src_nentries) {
 		ret = ath10k_ce_init_src_ring(ar, ce_id, attr);
 		if (ret) {
-			ath10k_err("Failed to initialize CE src ring for ID: %d (%d)\n",
+			ath10k_err(ar, "Failed to initialize CE src ring for ID: %d (%d)\n",
 				   ce_id, ret);
-			goto out;
+			return ret;
 		}
 	}
 
 	if (attr->dest_nentries) {
 		ret = ath10k_ce_init_dest_ring(ar, ce_id, attr);
 		if (ret) {
-			ath10k_err("Failed to initialize CE dest ring for ID: %d (%d)\n",
+			ath10k_err(ar, "Failed to initialize CE dest ring for ID: %d (%d)\n",
 				   ce_id, ret);
-			goto out;
+			return ret;
 		}
 	}
 
-out:
-	ath10k_pci_sleep(ar);
-	return ret;
+	return 0;
 }
 
 static void ath10k_ce_deinit_src_ring(struct ath10k *ar, unsigned int ce_id)
@@ -1140,16 +1093,8 @@ static void ath10k_ce_deinit_dest_ring(struct ath10k *ar, unsigned int ce_id)
 
 void ath10k_ce_deinit_pipe(struct ath10k *ar, unsigned int ce_id)
 {
-	int ret;
-
-	ret = ath10k_pci_wake(ar);
-	if (ret)
-		return;
-
 	ath10k_ce_deinit_src_ring(ar, ce_id);
 	ath10k_ce_deinit_dest_ring(ar, ce_id);
-
-	ath10k_pci_sleep(ar);
 }
 
 int ath10k_ce_alloc_pipe(struct ath10k *ar, int ce_id,
@@ -1163,7 +1108,7 @@ int ath10k_ce_alloc_pipe(struct ath10k *ar, int ce_id,
 		ce_state->src_ring = ath10k_ce_alloc_src_ring(ar, ce_id, attr);
 		if (IS_ERR(ce_state->src_ring)) {
 			ret = PTR_ERR(ce_state->src_ring);
-			ath10k_err("failed to allocate copy engine source ring %d: %d\n",
+			ath10k_err(ar, "failed to allocate copy engine source ring %d: %d\n",
 				   ce_id, ret);
 			ce_state->src_ring = NULL;
 			return ret;
@@ -1175,7 +1120,7 @@ int ath10k_ce_alloc_pipe(struct ath10k *ar, int ce_id,
 								attr);
 		if (IS_ERR(ce_state->dest_ring)) {
 			ret = PTR_ERR(ce_state->dest_ring);
-			ath10k_err("failed to allocate copy engine destination ring %d: %d\n",
+			ath10k_err(ar, "failed to allocate copy engine destination ring %d: %d\n",
 				   ce_id, ret);
 			ce_state->dest_ring = NULL;
 			return ret;
diff --git a/drivers/net/wireless/ath/ath10k/ce.h b/drivers/net/wireless/ath/ath10k/ce.h
index 7a5a36fc59c1..329b7340fa72 100644
--- a/drivers/net/wireless/ath/ath10k/ce.h
+++ b/drivers/net/wireless/ath/ath10k/ce.h
@@ -20,7 +20,6 @@
 
 #include "hif.h"
 
-
 /* Maximum number of Copy Engine's supported */
 #define CE_COUNT_MAX 8
 #define CE_HTT_H2T_MSG_SRC_NENTRIES 4096
@@ -37,7 +36,6 @@
 
 struct ath10k_ce_pipe;
 
-
 #define CE_DESC_FLAGS_GATHER         (1 << 0)
 #define CE_DESC_FLAGS_BYTE_SWAP      (1 << 1)
 #define CE_DESC_FLAGS_META_DATA_MASK 0xFFFC
@@ -162,30 +160,13 @@ int ath10k_ce_send_nolock(struct ath10k_ce_pipe *ce_state,
 
 void __ath10k_ce_send_revert(struct ath10k_ce_pipe *pipe);
 
-void ath10k_ce_send_cb_register(struct ath10k_ce_pipe *ce_state,
-				void (*send_cb)(struct ath10k_ce_pipe *),
-				int disable_interrupts);
-
 int ath10k_ce_num_free_src_entries(struct ath10k_ce_pipe *pipe);
 
 /*==================Recv=======================*/
 
-/*
- * Make a buffer available to receive. The buffer must be at least of a
- * minimal size appropriate for this copy engine (src_sz_max attribute).
- *   ce                    - which copy engine to use
- *   per_transfer_recv_context  - context passed back to caller's recv_cb
- *   buffer                     - address of buffer in CE space
- * Returns 0 on success; otherwise an error status.
- *
- * Implemenation note: Pushes a buffer to Dest ring.
- */
-int ath10k_ce_recv_buf_enqueue(struct ath10k_ce_pipe *ce_state,
-			       void *per_transfer_recv_context,
-			       u32 buffer);
-
-void ath10k_ce_recv_cb_register(struct ath10k_ce_pipe *ce_state,
-				void (*recv_cb)(struct ath10k_ce_pipe *));
+int __ath10k_ce_rx_num_free_bufs(struct ath10k_ce_pipe *pipe);
+int __ath10k_ce_rx_post_buf(struct ath10k_ce_pipe *pipe, void *ctx, u32 paddr);
+int ath10k_ce_rx_post_buf(struct ath10k_ce_pipe *pipe, void *ctx, u32 paddr);
 
 /* recv flags */
 /* Data is byte-swapped */
@@ -206,18 +187,20 @@ int ath10k_ce_completed_recv_next(struct ath10k_ce_pipe *ce_state,
  * Pops 1 completed send buffer from Source ring.
  */
 int ath10k_ce_completed_send_next(struct ath10k_ce_pipe *ce_state,
-			   void **per_transfer_contextp,
-			   u32 *bufferp,
-			   unsigned int *nbytesp,
-			   unsigned int *transfer_idp);
+				  void **per_transfer_contextp,
+				  u32 *bufferp,
+				  unsigned int *nbytesp,
+				  unsigned int *transfer_idp);
 
 /*==================CE Engine Initialization=======================*/
 
 int ath10k_ce_init_pipe(struct ath10k *ar, unsigned int ce_id,
-			const struct ce_attr *attr);
+			const struct ce_attr *attr,
+			void (*send_cb)(struct ath10k_ce_pipe *),
+			void (*recv_cb)(struct ath10k_ce_pipe *));
 void ath10k_ce_deinit_pipe(struct ath10k *ar, unsigned int ce_id);
 int ath10k_ce_alloc_pipe(struct ath10k *ar, int ce_id,
-			  const struct ce_attr *attr);
+			 const struct ce_attr *attr);
 void ath10k_ce_free_pipe(struct ath10k *ar, int ce_id);
 
 /*==================CE Engine Shutdown=======================*/
@@ -245,6 +228,7 @@ int ath10k_ce_cancel_send_next(struct ath10k_ce_pipe *ce_state,
 void ath10k_ce_per_engine_service_any(struct ath10k *ar);
 void ath10k_ce_per_engine_service(struct ath10k *ar, unsigned int ce_id);
 int ath10k_ce_disable_interrupts(struct ath10k *ar);
+void ath10k_ce_enable_interrupts(struct ath10k *ar);
 
 /* ce_attr.flags values */
 /* Use NonSnooping PCIe accesses? */
@@ -397,7 +381,6 @@ struct ce_attr {
 #define DST_WATERMARK_HIGH_RESET		0
 #define DST_WATERMARK_ADDRESS			0x0050
 
-
 static inline u32 ath10k_ce_base_address(unsigned int ce_id)
 {
 	return CE0_BASE_ADDRESS + (CE1_BASE_ADDRESS - CE0_BASE_ADDRESS) * ce_id;
diff --git a/drivers/net/wireless/ath/ath10k/core.c b/drivers/net/wireless/ath/ath10k/core.c
index 93adb8c58969..cee18c89d7f2 100644
--- a/drivers/net/wireless/ath/ath10k/core.c
+++ b/drivers/net/wireless/ath/ath10k/core.c
@@ -26,6 +26,7 @@
 #include "bmi.h"
 #include "debug.h"
 #include "htt.h"
+#include "testmode.h"
 
 unsigned int ath10k_debug_mask;
 static bool uart_print;
@@ -53,7 +54,7 @@ static const struct ath10k_hw_params ath10k_hw_params_list[] = {
 
 static void ath10k_send_suspend_complete(struct ath10k *ar)
 {
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot suspend complete\n");
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot suspend complete\n");
 
 	complete(&ar->target_suspend);
 }
@@ -67,14 +68,14 @@ static int ath10k_init_configure_target(struct ath10k *ar)
 	ret = ath10k_bmi_write32(ar, hi_app_host_interest,
 				 HTC_PROTOCOL_VERSION);
 	if (ret) {
-		ath10k_err("settings HTC version failed\n");
+		ath10k_err(ar, "settings HTC version failed\n");
 		return ret;
 	}
 
 	/* set the firmware mode to STA/IBSS/AP */
 	ret = ath10k_bmi_read32(ar, hi_option_flag, &param_host);
 	if (ret) {
-		ath10k_err("setting firmware mode (1/2) failed\n");
+		ath10k_err(ar, "setting firmware mode (1/2) failed\n");
 		return ret;
 	}
 
@@ -93,14 +94,14 @@ static int ath10k_init_configure_target(struct ath10k *ar)
 
 	ret = ath10k_bmi_write32(ar, hi_option_flag, param_host);
 	if (ret) {
-		ath10k_err("setting firmware mode (2/2) failed\n");
+		ath10k_err(ar, "setting firmware mode (2/2) failed\n");
 		return ret;
 	}
 
 	/* We do all byte-swapping on the host */
 	ret = ath10k_bmi_write32(ar, hi_be, 0);
 	if (ret) {
-		ath10k_err("setting host CPU BE mode failed\n");
+		ath10k_err(ar, "setting host CPU BE mode failed\n");
 		return ret;
 	}
 
@@ -108,7 +109,7 @@ static int ath10k_init_configure_target(struct ath10k *ar)
 	ret = ath10k_bmi_write32(ar, hi_fw_swap, 0);
 
 	if (ret) {
-		ath10k_err("setting FW data/desc swap flags failed\n");
+		ath10k_err(ar, "setting FW data/desc swap flags failed\n");
 		return ret;
 	}
 
@@ -146,11 +147,12 @@ static int ath10k_push_board_ext_data(struct ath10k *ar)
 
 	ret = ath10k_bmi_read32(ar, hi_board_ext_data, &board_ext_data_addr);
 	if (ret) {
-		ath10k_err("could not read board ext data addr (%d)\n", ret);
+		ath10k_err(ar, "could not read board ext data addr (%d)\n",
+			   ret);
 		return ret;
 	}
 
-	ath10k_dbg(ATH10K_DBG_BOOT,
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
 		   "boot push board extended data addr 0x%x\n",
 		   board_ext_data_addr);
 
@@ -158,7 +160,7 @@ static int ath10k_push_board_ext_data(struct ath10k *ar)
 		return 0;
 
 	if (ar->board_len != (board_data_size + board_ext_data_size)) {
-		ath10k_err("invalid board (ext) data sizes %zu != %d+%d\n",
+		ath10k_err(ar, "invalid board (ext) data sizes %zu != %d+%d\n",
 			   ar->board_len, board_data_size, board_ext_data_size);
 		return -EINVAL;
 	}
@@ -167,14 +169,15 @@ static int ath10k_push_board_ext_data(struct ath10k *ar)
 				      ar->board_data + board_data_size,
 				      board_ext_data_size);
 	if (ret) {
-		ath10k_err("could not write board ext data (%d)\n", ret);
+		ath10k_err(ar, "could not write board ext data (%d)\n", ret);
 		return ret;
 	}
 
 	ret = ath10k_bmi_write32(ar, hi_board_ext_data_config,
 				 (board_ext_data_size << 16) | 1);
 	if (ret) {
-		ath10k_err("could not write board ext data bit (%d)\n", ret);
+		ath10k_err(ar, "could not write board ext data bit (%d)\n",
+			   ret);
 		return ret;
 	}
 
@@ -189,13 +192,13 @@ static int ath10k_download_board_data(struct ath10k *ar)
 
 	ret = ath10k_push_board_ext_data(ar);
 	if (ret) {
-		ath10k_err("could not push board ext data (%d)\n", ret);
+		ath10k_err(ar, "could not push board ext data (%d)\n", ret);
 		goto exit;
 	}
 
 	ret = ath10k_bmi_read32(ar, hi_board_data, &address);
 	if (ret) {
-		ath10k_err("could not read board data addr (%d)\n", ret);
+		ath10k_err(ar, "could not read board data addr (%d)\n", ret);
 		goto exit;
 	}
 
@@ -203,13 +206,13 @@ static int ath10k_download_board_data(struct ath10k *ar)
 				      min_t(u32, board_data_size,
 					    ar->board_len));
 	if (ret) {
-		ath10k_err("could not write board data (%d)\n", ret);
+		ath10k_err(ar, "could not write board data (%d)\n", ret);
 		goto exit;
 	}
 
 	ret = ath10k_bmi_write32(ar, hi_board_data_initialized, 1);
 	if (ret) {
-		ath10k_err("could not write board data bit (%d)\n", ret);
+		ath10k_err(ar, "could not write board data bit (%d)\n", ret);
 		goto exit;
 	}
 
@@ -225,51 +228,72 @@ static int ath10k_download_and_run_otp(struct ath10k *ar)
 	/* OTP is optional */
 
 	if (!ar->otp_data || !ar->otp_len) {
-		ath10k_warn("Not running otp, calibration will be incorrect (otp-data %p otp_len %zd)!\n",
+		ath10k_warn(ar, "Not running otp, calibration will be incorrect (otp-data %p otp_len %zd)!\n",
 			    ar->otp_data, ar->otp_len);
 		return 0;
 	}
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot upload otp to 0x%x len %zd\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot upload otp to 0x%x len %zd\n",
 		   address, ar->otp_len);
 
 	ret = ath10k_bmi_fast_download(ar, address, ar->otp_data, ar->otp_len);
 	if (ret) {
-		ath10k_err("could not write otp (%d)\n", ret);
+		ath10k_err(ar, "could not write otp (%d)\n", ret);
 		return ret;
 	}
 
 	ret = ath10k_bmi_execute(ar, address, 0, &result);
 	if (ret) {
-		ath10k_err("could not execute otp (%d)\n", ret);
+		ath10k_err(ar, "could not execute otp (%d)\n", ret);
 		return ret;
 	}
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot otp execute result %d\n", result);
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot otp execute result %d\n", result);
 
 	if (result != 0) {
-		ath10k_err("otp calibration failed: %d", result);
+		ath10k_err(ar, "otp calibration failed: %d", result);
 		return -EINVAL;
 	}
 
 	return 0;
 }
 
-static int ath10k_download_fw(struct ath10k *ar)
+static int ath10k_download_fw(struct ath10k *ar, enum ath10k_firmware_mode mode)
 {
-	u32 address;
+	u32 address, data_len;
+	const char *mode_name;
+	const void *data;
 	int ret;
 
 	address = ar->hw_params.patch_load_addr;
 
-	ret = ath10k_bmi_fast_download(ar, address, ar->firmware_data,
-				       ar->firmware_len);
+	switch (mode) {
+	case ATH10K_FIRMWARE_MODE_NORMAL:
+		data = ar->firmware_data;
+		data_len = ar->firmware_len;
+		mode_name = "normal";
+		break;
+	case ATH10K_FIRMWARE_MODE_UTF:
+		data = ar->testmode.utf->data;
+		data_len = ar->testmode.utf->size;
+		mode_name = "utf";
+		break;
+	default:
+		ath10k_err(ar, "unknown firmware mode: %d\n", mode);
+		return -EINVAL;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
+		   "boot uploading firmware image %p len %d mode %s\n",
+		   data, data_len, mode_name);
+
+	ret = ath10k_bmi_fast_download(ar, address, data, data_len);
 	if (ret) {
-		ath10k_err("could not write fw (%d)\n", ret);
-		goto exit;
+		ath10k_err(ar, "failed to download %s firmware: %d\n",
+			   mode_name, ret);
+		return ret;
 	}
 
-exit:
 	return ret;
 }
 
@@ -302,12 +326,12 @@ static int ath10k_core_fetch_firmware_api_1(struct ath10k *ar)
 	int ret = 0;
 
 	if (ar->hw_params.fw.fw == NULL) {
-		ath10k_err("firmware file not defined\n");
+		ath10k_err(ar, "firmware file not defined\n");
 		return -EINVAL;
 	}
 
 	if (ar->hw_params.fw.board == NULL) {
-		ath10k_err("board data file not defined");
+		ath10k_err(ar, "board data file not defined");
 		return -EINVAL;
 	}
 
@@ -316,7 +340,7 @@ static int ath10k_core_fetch_firmware_api_1(struct ath10k *ar)
 					 ar->hw_params.fw.board);
 	if (IS_ERR(ar->board)) {
 		ret = PTR_ERR(ar->board);
-		ath10k_err("could not fetch board data (%d)\n", ret);
+		ath10k_err(ar, "could not fetch board data (%d)\n", ret);
 		goto err;
 	}
 
@@ -328,7 +352,7 @@ static int ath10k_core_fetch_firmware_api_1(struct ath10k *ar)
 					    ar->hw_params.fw.fw);
 	if (IS_ERR(ar->firmware)) {
 		ret = PTR_ERR(ar->firmware);
-		ath10k_err("could not fetch firmware (%d)\n", ret);
+		ath10k_err(ar, "could not fetch firmware (%d)\n", ret);
 		goto err;
 	}
 
@@ -344,7 +368,7 @@ static int ath10k_core_fetch_firmware_api_1(struct ath10k *ar)
 				       ar->hw_params.fw.otp);
 	if (IS_ERR(ar->otp)) {
 		ret = PTR_ERR(ar->otp);
-		ath10k_err("could not fetch otp (%d)\n", ret);
+		ath10k_err(ar, "could not fetch otp (%d)\n", ret);
 		goto err;
 	}
 
@@ -369,7 +393,7 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 	/* first fetch the firmware file (firmware-*.bin) */
 	ar->firmware = ath10k_fetch_fw_file(ar, ar->hw_params.fw.dir, name);
 	if (IS_ERR(ar->firmware)) {
-		ath10k_err("could not fetch firmware file '%s/%s': %ld\n",
+		ath10k_err(ar, "could not fetch firmware file '%s/%s': %ld\n",
 			   ar->hw_params.fw.dir, name, PTR_ERR(ar->firmware));
 		return PTR_ERR(ar->firmware);
 	}
@@ -381,14 +405,14 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 	magic_len = strlen(ATH10K_FIRMWARE_MAGIC) + 1;
 
 	if (len < magic_len) {
-		ath10k_err("firmware file '%s/%s' too small to contain magic: %zu\n",
+		ath10k_err(ar, "firmware file '%s/%s' too small to contain magic: %zu\n",
 			   ar->hw_params.fw.dir, name, len);
 		ret = -EINVAL;
 		goto err;
 	}
 
 	if (memcmp(data, ATH10K_FIRMWARE_MAGIC, magic_len) != 0) {
-		ath10k_err("invalid firmware magic\n");
+		ath10k_err(ar, "invalid firmware magic\n");
 		ret = -EINVAL;
 		goto err;
 	}
@@ -410,7 +434,7 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 		data += sizeof(*hdr);
 
 		if (len < ie_len) {
-			ath10k_err("invalid length for FW IE %d (%zu < %zu)\n",
+			ath10k_err(ar, "invalid length for FW IE %d (%zu < %zu)\n",
 				   ie_id, len, ie_len);
 			ret = -EINVAL;
 			goto err;
@@ -424,7 +448,7 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 			memcpy(ar->hw->wiphy->fw_version, data, ie_len);
 			ar->hw->wiphy->fw_version[ie_len] = '\0';
 
-			ath10k_dbg(ATH10K_DBG_BOOT,
+			ath10k_dbg(ar, ATH10K_DBG_BOOT,
 				   "found fw version %s\n",
 				    ar->hw->wiphy->fw_version);
 			break;
@@ -434,11 +458,11 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 
 			timestamp = (__le32 *)data;
 
-			ath10k_dbg(ATH10K_DBG_BOOT, "found fw timestamp %d\n",
+			ath10k_dbg(ar, ATH10K_DBG_BOOT, "found fw timestamp %d\n",
 				   le32_to_cpup(timestamp));
 			break;
 		case ATH10K_FW_IE_FEATURES:
-			ath10k_dbg(ATH10K_DBG_BOOT,
+			ath10k_dbg(ar, ATH10K_DBG_BOOT,
 				   "found firmware features ie (%zd B)\n",
 				   ie_len);
 
@@ -450,19 +474,19 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 					break;
 
 				if (data[index] & (1 << bit)) {
-					ath10k_dbg(ATH10K_DBG_BOOT,
+					ath10k_dbg(ar, ATH10K_DBG_BOOT,
 						   "Enabling feature bit: %i\n",
 						   i);
 					__set_bit(i, ar->fw_features);
 				}
 			}
 
-			ath10k_dbg_dump(ATH10K_DBG_BOOT, "features", "",
+			ath10k_dbg_dump(ar, ATH10K_DBG_BOOT, "features", "",
 					ar->fw_features,
 					sizeof(ar->fw_features));
 			break;
 		case ATH10K_FW_IE_FW_IMAGE:
-			ath10k_dbg(ATH10K_DBG_BOOT,
+			ath10k_dbg(ar, ATH10K_DBG_BOOT,
 				   "found fw image ie (%zd B)\n",
 				   ie_len);
 
@@ -471,7 +495,7 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 
 			break;
 		case ATH10K_FW_IE_OTP_IMAGE:
-			ath10k_dbg(ATH10K_DBG_BOOT,
+			ath10k_dbg(ar, ATH10K_DBG_BOOT,
 				   "found otp image ie (%zd B)\n",
 				   ie_len);
 
@@ -480,7 +504,7 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 
 			break;
 		default:
-			ath10k_warn("Unknown FW IE: %u\n",
+			ath10k_warn(ar, "Unknown FW IE: %u\n",
 				    le32_to_cpu(hdr->id));
 			break;
 		}
@@ -493,15 +517,22 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 	}
 
 	if (!ar->firmware_data || !ar->firmware_len) {
-		ath10k_warn("No ATH10K_FW_IE_FW_IMAGE found from '%s/%s', skipping\n",
+		ath10k_warn(ar, "No ATH10K_FW_IE_FW_IMAGE found from '%s/%s', skipping\n",
 			    ar->hw_params.fw.dir, name);
 		ret = -ENOMEDIUM;
 		goto err;
 	}
 
+	if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features) &&
+	    !test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+		ath10k_err(ar, "feature bits corrupted: 10.2 feature requires 10.x feature to be set as well");
+		ret = -EINVAL;
+		goto err;
+	}
+
 	/* now fetch the board file */
 	if (ar->hw_params.fw.board == NULL) {
-		ath10k_err("board data file not defined");
+		ath10k_err(ar, "board data file not defined");
 		ret = -EINVAL;
 		goto err;
 	}
@@ -511,7 +542,7 @@ static int ath10k_core_fetch_firmware_api_n(struct ath10k *ar, const char *name)
 					 ar->hw_params.fw.board);
 	if (IS_ERR(ar->board)) {
 		ret = PTR_ERR(ar->board);
-		ath10k_err("could not fetch board data '%s/%s' (%d)\n",
+		ath10k_err(ar, "could not fetch board data '%s/%s' (%d)\n",
 			   ar->hw_params.fw.dir, ar->hw_params.fw.board,
 			   ret);
 		goto err;
@@ -531,45 +562,53 @@ static int ath10k_core_fetch_firmware_files(struct ath10k *ar)
 {
 	int ret;
 
+	ar->fw_api = 3;
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
+
+	ret = ath10k_core_fetch_firmware_api_n(ar, ATH10K_FW_API3_FILE);
+	if (ret == 0)
+		goto success;
+
 	ar->fw_api = 2;
-	ath10k_dbg(ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
 
 	ret = ath10k_core_fetch_firmware_api_n(ar, ATH10K_FW_API2_FILE);
 	if (ret == 0)
 		goto success;
 
 	ar->fw_api = 1;
-	ath10k_dbg(ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "trying fw api %d\n", ar->fw_api);
 
 	ret = ath10k_core_fetch_firmware_api_1(ar);
 	if (ret)
 		return ret;
 
 success:
-	ath10k_dbg(ATH10K_DBG_BOOT, "using fw api %d\n", ar->fw_api);
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "using fw api %d\n", ar->fw_api);
 
 	return 0;
 }
 
-static int ath10k_init_download_firmware(struct ath10k *ar)
+static int ath10k_init_download_firmware(struct ath10k *ar,
+					 enum ath10k_firmware_mode mode)
 {
 	int ret;
 
 	ret = ath10k_download_board_data(ar);
 	if (ret) {
-		ath10k_err("failed to download board data: %d\n", ret);
+		ath10k_err(ar, "failed to download board data: %d\n", ret);
 		return ret;
 	}
 
 	ret = ath10k_download_and_run_otp(ar);
 	if (ret) {
-		ath10k_err("failed to run otp: %d\n", ret);
+		ath10k_err(ar, "failed to run otp: %d\n", ret);
 		return ret;
 	}
 
-	ret = ath10k_download_fw(ar);
+	ret = ath10k_download_fw(ar, mode);
 	if (ret) {
-		ath10k_err("failed to download firmware: %d\n", ret);
+		ath10k_err(ar, "failed to download firmware: %d\n", ret);
 		return ret;
 	}
 
@@ -586,7 +625,7 @@ static int ath10k_init_uart(struct ath10k *ar)
 	 */
 	ret = ath10k_bmi_write32(ar, hi_serial_enable, 0);
 	if (ret) {
-		ath10k_warn("could not disable UART prints (%d)\n", ret);
+		ath10k_warn(ar, "could not disable UART prints (%d)\n", ret);
 		return ret;
 	}
 
@@ -595,24 +634,24 @@ static int ath10k_init_uart(struct ath10k *ar)
 
 	ret = ath10k_bmi_write32(ar, hi_dbg_uart_txpin, 7);
 	if (ret) {
-		ath10k_warn("could not enable UART prints (%d)\n", ret);
+		ath10k_warn(ar, "could not enable UART prints (%d)\n", ret);
 		return ret;
 	}
 
 	ret = ath10k_bmi_write32(ar, hi_serial_enable, 1);
 	if (ret) {
-		ath10k_warn("could not enable UART prints (%d)\n", ret);
+		ath10k_warn(ar, "could not enable UART prints (%d)\n", ret);
 		return ret;
 	}
 
 	/* Set the UART baud rate to 19200. */
 	ret = ath10k_bmi_write32(ar, hi_desired_baud_rate, 19200);
 	if (ret) {
-		ath10k_warn("could not set the baud rate (%d)\n", ret);
+		ath10k_warn(ar, "could not set the baud rate (%d)\n", ret);
 		return ret;
 	}
 
-	ath10k_info("UART prints enabled\n");
+	ath10k_info(ar, "UART prints enabled\n");
 	return 0;
 }
 
@@ -629,14 +668,14 @@ static int ath10k_init_hw_params(struct ath10k *ar)
 	}
 
 	if (i == ARRAY_SIZE(ath10k_hw_params_list)) {
-		ath10k_err("Unsupported hardware version: 0x%x\n",
+		ath10k_err(ar, "Unsupported hardware version: 0x%x\n",
 			   ar->target_version);
 		return -EINVAL;
 	}
 
 	ar->hw_params = *hw_params;
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "Hardware name %s version 0x%x\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "Hardware name %s version 0x%x\n",
 		   ar->hw_params.name, ar->target_version);
 
 	return 0;
@@ -651,14 +690,14 @@ static void ath10k_core_restart(struct work_struct *work)
 	switch (ar->state) {
 	case ATH10K_STATE_ON:
 		ar->state = ATH10K_STATE_RESTARTING;
-		del_timer_sync(&ar->scan.timeout);
-		ath10k_reset_scan((unsigned long)ar);
+		ath10k_hif_stop(ar);
+		ath10k_scan_finish(ar);
 		ieee80211_restart_hw(ar->hw);
 		break;
 	case ATH10K_STATE_OFF:
 		/* this can happen if driver is being unloaded
 		 * or if the crash happens during FW probing */
-		ath10k_warn("cannot restart a device that hasn't been started\n");
+		ath10k_warn(ar, "cannot restart a device that hasn't been started\n");
 		break;
 	case ATH10K_STATE_RESTARTING:
 		/* hw restart might be requested from multiple places */
@@ -667,14 +706,17 @@ static void ath10k_core_restart(struct work_struct *work)
 		ar->state = ATH10K_STATE_WEDGED;
 		/* fall through */
 	case ATH10K_STATE_WEDGED:
-		ath10k_warn("device is wedged, will not restart\n");
+		ath10k_warn(ar, "device is wedged, will not restart\n");
+		break;
+	case ATH10K_STATE_UTF:
+		ath10k_warn(ar, "firmware restart in UTF mode not supported\n");
 		break;
 	}
 
 	mutex_unlock(&ar->conf_mutex);
 }
 
-int ath10k_core_start(struct ath10k *ar)
+int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode)
 {
 	int status;
 
@@ -687,7 +729,7 @@ int ath10k_core_start(struct ath10k *ar)
 		goto err;
 	}
 
-	status = ath10k_init_download_firmware(ar);
+	status = ath10k_init_download_firmware(ar, mode);
 	if (status)
 		goto err;
 
@@ -700,7 +742,7 @@ int ath10k_core_start(struct ath10k *ar)
 
 	status = ath10k_htc_init(ar);
 	if (status) {
-		ath10k_err("could not init HTC (%d)\n", status);
+		ath10k_err(ar, "could not init HTC (%d)\n", status);
 		goto err;
 	}
 
@@ -710,90 +752,98 @@ int ath10k_core_start(struct ath10k *ar)
 
 	status = ath10k_wmi_attach(ar);
 	if (status) {
-		ath10k_err("WMI attach failed: %d\n", status);
+		ath10k_err(ar, "WMI attach failed: %d\n", status);
 		goto err;
 	}
 
 	status = ath10k_htt_init(ar);
 	if (status) {
-		ath10k_err("failed to init htt: %d\n", status);
+		ath10k_err(ar, "failed to init htt: %d\n", status);
 		goto err_wmi_detach;
 	}
 
 	status = ath10k_htt_tx_alloc(&ar->htt);
 	if (status) {
-		ath10k_err("failed to alloc htt tx: %d\n", status);
+		ath10k_err(ar, "failed to alloc htt tx: %d\n", status);
 		goto err_wmi_detach;
 	}
 
 	status = ath10k_htt_rx_alloc(&ar->htt);
 	if (status) {
-		ath10k_err("failed to alloc htt rx: %d\n", status);
+		ath10k_err(ar, "failed to alloc htt rx: %d\n", status);
 		goto err_htt_tx_detach;
 	}
 
 	status = ath10k_hif_start(ar);
 	if (status) {
-		ath10k_err("could not start HIF: %d\n", status);
+		ath10k_err(ar, "could not start HIF: %d\n", status);
 		goto err_htt_rx_detach;
 	}
 
 	status = ath10k_htc_wait_target(&ar->htc);
 	if (status) {
-		ath10k_err("failed to connect to HTC: %d\n", status);
+		ath10k_err(ar, "failed to connect to HTC: %d\n", status);
 		goto err_hif_stop;
 	}
 
-	status = ath10k_htt_connect(&ar->htt);
-	if (status) {
-		ath10k_err("failed to connect htt (%d)\n", status);
-		goto err_hif_stop;
+	if (mode == ATH10K_FIRMWARE_MODE_NORMAL) {
+		status = ath10k_htt_connect(&ar->htt);
+		if (status) {
+			ath10k_err(ar, "failed to connect htt (%d)\n", status);
+			goto err_hif_stop;
+		}
 	}
 
 	status = ath10k_wmi_connect(ar);
 	if (status) {
-		ath10k_err("could not connect wmi: %d\n", status);
+		ath10k_err(ar, "could not connect wmi: %d\n", status);
 		goto err_hif_stop;
 	}
 
 	status = ath10k_htc_start(&ar->htc);
 	if (status) {
-		ath10k_err("failed to start htc: %d\n", status);
+		ath10k_err(ar, "failed to start htc: %d\n", status);
 		goto err_hif_stop;
 	}
 
-	status = ath10k_wmi_wait_for_service_ready(ar);
-	if (status <= 0) {
-		ath10k_warn("wmi service ready event not received");
-		status = -ETIMEDOUT;
-		goto err_htc_stop;
+	if (mode == ATH10K_FIRMWARE_MODE_NORMAL) {
+		status = ath10k_wmi_wait_for_service_ready(ar);
+		if (status <= 0) {
+			ath10k_warn(ar, "wmi service ready event not received");
+			status = -ETIMEDOUT;
+			goto err_hif_stop;
+		}
 	}
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "firmware %s booted\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "firmware %s booted\n",
 		   ar->hw->wiphy->fw_version);
 
 	status = ath10k_wmi_cmd_init(ar);
 	if (status) {
-		ath10k_err("could not send WMI init command (%d)\n", status);
-		goto err_htc_stop;
+		ath10k_err(ar, "could not send WMI init command (%d)\n",
+			   status);
+		goto err_hif_stop;
 	}
 
 	status = ath10k_wmi_wait_for_unified_ready(ar);
 	if (status <= 0) {
-		ath10k_err("wmi unified ready event not received\n");
+		ath10k_err(ar, "wmi unified ready event not received\n");
 		status = -ETIMEDOUT;
-		goto err_htc_stop;
+		goto err_hif_stop;
 	}
 
-	status = ath10k_htt_setup(&ar->htt);
-	if (status) {
-		ath10k_err("failed to setup htt: %d\n", status);
-		goto err_htc_stop;
+	/* we don't care about HTT in UTF mode */
+	if (mode == ATH10K_FIRMWARE_MODE_NORMAL) {
+		status = ath10k_htt_setup(&ar->htt);
+		if (status) {
+			ath10k_err(ar, "failed to setup htt: %d\n", status);
+			goto err_hif_stop;
+		}
 	}
 
 	status = ath10k_debug_start(ar);
 	if (status)
-		goto err_htc_stop;
+		goto err_hif_stop;
 
 	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
 		ar->free_vdev_map = (1 << TARGET_10X_NUM_VDEVS) - 1;
@@ -802,28 +852,8 @@ int ath10k_core_start(struct ath10k *ar)
 
 	INIT_LIST_HEAD(&ar->arvifs);
 
-	if (!test_bit(ATH10K_FLAG_FIRST_BOOT_DONE, &ar->dev_flags)) {
-		ath10k_info("%s (0x%08x, 0x%08x) fw %s api %d htt %d.%d\n",
-			    ar->hw_params.name,
-			    ar->target_version,
-			    ar->chip_id,
-			    ar->hw->wiphy->fw_version,
-			    ar->fw_api,
-			    ar->htt.target_version_major,
-			    ar->htt.target_version_minor);
-		ath10k_info("debug %d debugfs %d tracing %d dfs %d\n",
-			    config_enabled(CONFIG_ATH10K_DEBUG),
-			    config_enabled(CONFIG_ATH10K_DEBUGFS),
-			    config_enabled(CONFIG_ATH10K_TRACING),
-			    config_enabled(CONFIG_ATH10K_DFS_CERTIFIED));
-	}
-
-	__set_bit(ATH10K_FLAG_FIRST_BOOT_DONE, &ar->dev_flags);
-
 	return 0;
 
-err_htc_stop:
-	ath10k_htc_stop(&ar->htc);
 err_hif_stop:
 	ath10k_hif_stop(ar);
 err_htt_rx_detach:
@@ -845,14 +875,14 @@ int ath10k_wait_for_suspend(struct ath10k *ar, u32 suspend_opt)
 
 	ret = ath10k_wmi_pdev_suspend_target(ar, suspend_opt);
 	if (ret) {
-		ath10k_warn("could not suspend target (%d)\n", ret);
+		ath10k_warn(ar, "could not suspend target (%d)\n", ret);
 		return ret;
 	}
 
 	ret = wait_for_completion_timeout(&ar->target_suspend, 1 * HZ);
 
 	if (ret == 0) {
-		ath10k_warn("suspend timed out - target pause event never came\n");
+		ath10k_warn(ar, "suspend timed out - target pause event never came\n");
 		return -ETIMEDOUT;
 	}
 
@@ -864,11 +894,11 @@ void ath10k_core_stop(struct ath10k *ar)
 	lockdep_assert_held(&ar->conf_mutex);
 
 	/* try to suspend target */
-	if (ar->state != ATH10K_STATE_RESTARTING)
+	if (ar->state != ATH10K_STATE_RESTARTING &&
+	    ar->state != ATH10K_STATE_UTF)
 		ath10k_wait_for_suspend(ar, WMI_PDEV_SUSPEND_AND_DISABLE_INTR);
 
 	ath10k_debug_stop(ar);
-	ath10k_htc_stop(&ar->htc);
 	ath10k_hif_stop(ar);
 	ath10k_htt_tx_free(&ar->htt);
 	ath10k_htt_rx_free(&ar->htt);
@@ -887,14 +917,14 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 
 	ret = ath10k_hif_power_up(ar);
 	if (ret) {
-		ath10k_err("could not start pci hif (%d)\n", ret);
+		ath10k_err(ar, "could not start pci hif (%d)\n", ret);
 		return ret;
 	}
 
 	memset(&target_info, 0, sizeof(target_info));
 	ret = ath10k_bmi_get_target_info(ar, &target_info);
 	if (ret) {
-		ath10k_err("could not get target info (%d)\n", ret);
+		ath10k_err(ar, "could not get target info (%d)\n", ret);
 		ath10k_hif_power_down(ar);
 		return ret;
 	}
@@ -904,29 +934,30 @@ static int ath10k_core_probe_fw(struct ath10k *ar)
 
 	ret = ath10k_init_hw_params(ar);
 	if (ret) {
-		ath10k_err("could not get hw params (%d)\n", ret);
+		ath10k_err(ar, "could not get hw params (%d)\n", ret);
 		ath10k_hif_power_down(ar);
 		return ret;
 	}
 
 	ret = ath10k_core_fetch_firmware_files(ar);
 	if (ret) {
-		ath10k_err("could not fetch firmware files (%d)\n", ret);
+		ath10k_err(ar, "could not fetch firmware files (%d)\n", ret);
 		ath10k_hif_power_down(ar);
 		return ret;
 	}
 
 	mutex_lock(&ar->conf_mutex);
 
-	ret = ath10k_core_start(ar);
+	ret = ath10k_core_start(ar, ATH10K_FIRMWARE_MODE_NORMAL);
 	if (ret) {
-		ath10k_err("could not init core (%d)\n", ret);
+		ath10k_err(ar, "could not init core (%d)\n", ret);
 		ath10k_core_free_firmware_files(ar);
 		ath10k_hif_power_down(ar);
 		mutex_unlock(&ar->conf_mutex);
 		return ret;
 	}
 
+	ath10k_print_driver_info(ar);
 	ath10k_core_stop(ar);
 
 	mutex_unlock(&ar->conf_mutex);
@@ -939,7 +970,7 @@ static int ath10k_core_check_chip_id(struct ath10k *ar)
 {
 	u32 hw_revision = MS(ar->chip_id, SOC_CHIP_ID_REV);
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot chip_id 0x%08x hw_revision 0x%x\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot chip_id 0x%08x hw_revision 0x%x\n",
 		   ar->chip_id, hw_revision);
 
 	/* Check that we are not using hw1.0 (some of them have same pci id
@@ -947,7 +978,7 @@ static int ath10k_core_check_chip_id(struct ath10k *ar)
 	 * due to missing hw1.0 workarounds. */
 	switch (hw_revision) {
 	case QCA988X_HW_1_0_CHIP_ID_REV:
-		ath10k_err("ERROR: qca988x hw1.0 is not supported\n");
+		ath10k_err(ar, "ERROR: qca988x hw1.0 is not supported\n");
 		return -EOPNOTSUPP;
 
 	case QCA988X_HW_2_0_CHIP_ID_REV:
@@ -955,7 +986,7 @@ static int ath10k_core_check_chip_id(struct ath10k *ar)
 		return 0;
 
 	default:
-		ath10k_warn("Warning: hardware revision unknown (0x%x), expect problems\n",
+		ath10k_warn(ar, "Warning: hardware revision unknown (0x%x), expect problems\n",
 			    ar->chip_id);
 		return 0;
 	}
@@ -970,25 +1001,33 @@ static void ath10k_core_register_work(struct work_struct *work)
 
 	status = ath10k_core_probe_fw(ar);
 	if (status) {
-		ath10k_err("could not probe fw (%d)\n", status);
+		ath10k_err(ar, "could not probe fw (%d)\n", status);
 		goto err;
 	}
 
 	status = ath10k_mac_register(ar);
 	if (status) {
-		ath10k_err("could not register to mac80211 (%d)\n", status);
+		ath10k_err(ar, "could not register to mac80211 (%d)\n", status);
 		goto err_release_fw;
 	}
 
-	status = ath10k_debug_create(ar);
+	status = ath10k_debug_register(ar);
 	if (status) {
-		ath10k_err("unable to initialize debugfs\n");
+		ath10k_err(ar, "unable to initialize debugfs\n");
 		goto err_unregister_mac;
 	}
 
+	status = ath10k_spectral_create(ar);
+	if (status) {
+		ath10k_err(ar, "failed to initialize spectral\n");
+		goto err_debug_destroy;
+	}
+
 	set_bit(ATH10K_FLAG_CORE_REGISTERED, &ar->dev_flags);
 	return;
 
+err_debug_destroy:
+	ath10k_debug_destroy(ar);
 err_unregister_mac:
 	ath10k_mac_unregister(ar);
 err_release_fw:
@@ -1008,7 +1047,7 @@ int ath10k_core_register(struct ath10k *ar, u32 chip_id)
 
 	status = ath10k_core_check_chip_id(ar);
 	if (status) {
-		ath10k_err("Unsupported chip id 0x%08x\n", ar->chip_id);
+		ath10k_err(ar, "Unsupported chip id 0x%08x\n", ar->chip_id);
 		return status;
 	}
 
@@ -1025,23 +1064,32 @@ void ath10k_core_unregister(struct ath10k *ar)
 	if (!test_bit(ATH10K_FLAG_CORE_REGISTERED, &ar->dev_flags))
 		return;
 
+	/* Stop spectral before unregistering from mac80211 to remove the
+	 * relayfs debugfs file cleanly. Otherwise the parent debugfs tree
+	 * would be already be free'd recursively, leading to a double free.
+	 */
+	ath10k_spectral_destroy(ar);
+
 	/* We must unregister from mac80211 before we stop HTC and HIF.
 	 * Otherwise we will fail to submit commands to FW and mac80211 will be
 	 * unhappy about callback failures. */
 	ath10k_mac_unregister(ar);
 
+	ath10k_testmode_destroy(ar);
+
 	ath10k_core_free_firmware_files(ar);
 
-	ath10k_debug_destroy(ar);
+	ath10k_debug_unregister(ar);
 }
 EXPORT_SYMBOL(ath10k_core_unregister);
 
-struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
+struct ath10k *ath10k_core_create(size_t priv_size, struct device *dev,
 				  const struct ath10k_hif_ops *hif_ops)
 {
 	struct ath10k *ar;
+	int ret;
 
-	ar = ath10k_mac_create();
+	ar = ath10k_mac_create(priv_size);
 	if (!ar)
 		return NULL;
 
@@ -1051,7 +1099,6 @@ struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
 	ar->p2p = !!ath10k_p2p;
 	ar->dev = dev;
 
-	ar->hif.priv = hif_priv;
 	ar->hif.ops = hif_ops;
 
 	init_completion(&ar->scan.started);
@@ -1062,11 +1109,11 @@ struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
 	init_completion(&ar->install_key_done);
 	init_completion(&ar->vdev_setup_done);
 
-	setup_timer(&ar->scan.timeout, ath10k_reset_scan, (unsigned long)ar);
+	INIT_DELAYED_WORK(&ar->scan.timeout, ath10k_scan_timeout_work);
 
 	ar->workqueue = create_singlethread_workqueue("ath10k_wq");
 	if (!ar->workqueue)
-		goto err_wq;
+		goto err_free_mac;
 
 	mutex_init(&ar->conf_mutex);
 	spin_lock_init(&ar->data_lock);
@@ -1084,10 +1131,18 @@ struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
 	INIT_WORK(&ar->register_work, ath10k_core_register_work);
 	INIT_WORK(&ar->restart_work, ath10k_core_restart);
 
+	ret = ath10k_debug_create(ar);
+	if (ret)
+		goto err_free_wq;
+
 	return ar;
 
-err_wq:
+err_free_wq:
+	destroy_workqueue(ar->workqueue);
+
+err_free_mac:
 	ath10k_mac_destroy(ar);
+
 	return NULL;
 }
 EXPORT_SYMBOL(ath10k_core_create);
@@ -1097,6 +1152,7 @@ void ath10k_core_destroy(struct ath10k *ar)
 	flush_workqueue(ar->workqueue);
 	destroy_workqueue(ar->workqueue);
 
+	ath10k_debug_destroy(ar);
 	ath10k_mac_destroy(ar);
 }
 EXPORT_SYMBOL(ath10k_core_destroy);
diff --git a/drivers/net/wireless/ath/ath10k/core.h b/drivers/net/wireless/ath/ath10k/core.h
index 83a5fa91531d..fe531ea6926c 100644
--- a/drivers/net/wireless/ath/ath10k/core.h
+++ b/drivers/net/wireless/ath/ath10k/core.h
@@ -22,6 +22,8 @@
 #include <linux/if_ether.h>
 #include <linux/types.h>
 #include <linux/pci.h>
+#include <linux/uuid.h>
+#include <linux/time.h>
 
 #include "htt.h"
 #include "htc.h"
@@ -31,6 +33,7 @@
 #include "../ath.h"
 #include "../regd.h"
 #include "../dfs_pattern_detector.h"
+#include "spectral.h"
 
 #define MS(_v, _f) (((_v) & _f##_MASK) >> _f##_LSB)
 #define SM(_v, _f) (((_v) << _f##_LSB) & _f##_MASK)
@@ -237,6 +240,7 @@ struct ath10k_vif {
 
 	bool is_started;
 	bool is_up;
+	bool spectral_enabled;
 	u32 aid;
 	u8 bssid[ETH_ALEN];
 
@@ -276,11 +280,20 @@ struct ath10k_vif_iter {
 	struct ath10k_vif *arvif;
 };
 
+/* used for crash-dump storage, protected by data-lock */
+struct ath10k_fw_crash_data {
+	bool crashed_since_read;
+
+	uuid_le uuid;
+	struct timespec timestamp;
+	__le32 registers[REG_DUMP_COUNT_QCA988X];
+};
+
 struct ath10k_debug {
 	struct dentry *debugfs_phy;
 
 	struct ath10k_target_stats target_stats;
-	u32 wmi_service_bitmap[WMI_SERVICE_BM_SIZE];
+	DECLARE_BITMAP(wmi_service_bitmap, WMI_SERVICE_MAX);
 
 	struct completion event_stats_compl;
 
@@ -293,6 +306,8 @@ struct ath10k_debug {
 
 	u8 htt_max_amsdu;
 	u8 htt_max_ampdu;
+
+	struct ath10k_fw_crash_data *fw_crash_data;
 };
 
 enum ath10k_state {
@@ -315,6 +330,17 @@ enum ath10k_state {
 	 * prevents completion timeouts and makes the driver more responsive to
 	 * userspace commands. This is also prevents recursive recovery. */
 	ATH10K_STATE_WEDGED,
+
+	/* factory tests */
+	ATH10K_STATE_UTF,
+};
+
+enum ath10k_firmware_mode {
+	/* the default mode, standard 802.11 functionality */
+	ATH10K_FIRMWARE_MODE_NORMAL,
+
+	/* factory tests etc */
+	ATH10K_FIRMWARE_MODE_UTF,
 };
 
 enum ath10k_fw_features {
@@ -330,6 +356,11 @@ enum ath10k_fw_features {
 	/* Firmware does not support P2P */
 	ATH10K_FW_FEATURE_NO_P2P = 3,
 
+	/* Firmware 10.2 feature bit. The ATH10K_FW_FEATURE_WMI_10X feature bit
+	 * is required to be set as well.
+	 */
+	ATH10K_FW_FEATURE_WMI_10_2 = 4,
+
 	/* keep last */
 	ATH10K_FW_FEATURE_COUNT,
 };
@@ -337,10 +368,32 @@ enum ath10k_fw_features {
 enum ath10k_dev_flags {
 	/* Indicates that ath10k device is during CAC phase of DFS */
 	ATH10K_CAC_RUNNING,
-	ATH10K_FLAG_FIRST_BOOT_DONE,
 	ATH10K_FLAG_CORE_REGISTERED,
 };
 
+enum ath10k_scan_state {
+	ATH10K_SCAN_IDLE,
+	ATH10K_SCAN_STARTING,
+	ATH10K_SCAN_RUNNING,
+	ATH10K_SCAN_ABORTING,
+};
+
+static inline const char *ath10k_scan_state_str(enum ath10k_scan_state state)
+{
+	switch (state) {
+	case ATH10K_SCAN_IDLE:
+		return "idle";
+	case ATH10K_SCAN_STARTING:
+		return "starting";
+	case ATH10K_SCAN_RUNNING:
+		return "running";
+	case ATH10K_SCAN_ABORTING:
+		return "aborting";
+	}
+
+	return "unknown";
+}
+
 struct ath10k {
 	struct ath_common ath_common;
 	struct ieee80211_hw *hw;
@@ -368,7 +421,6 @@ struct ath10k {
 	bool p2p;
 
 	struct {
-		void *priv;
 		const struct ath10k_hif_ops *ops;
 	} hif;
 
@@ -410,10 +462,9 @@ struct ath10k {
 		struct completion started;
 		struct completion completed;
 		struct completion on_channel;
-		struct timer_list timeout;
+		struct delayed_work timeout;
+		enum ath10k_scan_state state;
 		bool is_roc;
-		bool in_progress;
-		bool aborting;
 		int vdev_id;
 		int roc_freq;
 	} scan;
@@ -432,7 +483,6 @@ struct ath10k {
 	struct cfg80211_chan_def chandef;
 
 	int free_vdev_map;
-	bool promisc;
 	bool monitor;
 	int monitor_vdev_id;
 	bool monitor_started;
@@ -494,13 +544,34 @@ struct ath10k {
 #ifdef CONFIG_ATH10K_DEBUGFS
 	struct ath10k_debug debug;
 #endif
+
+	struct {
+		/* relay(fs) channel for spectral scan */
+		struct rchan *rfs_chan_spec_scan;
+
+		/* spectral_mode and spec_config are protected by conf_mutex */
+		enum ath10k_spectral_mode mode;
+		struct ath10k_spec_scan config;
+	} spectral;
+
+	struct {
+		/* protected by conf_mutex */
+		const struct firmware *utf;
+		DECLARE_BITMAP(orig_fw_features, ATH10K_FW_FEATURE_COUNT);
+
+		/* protected by data_lock */
+		bool utf_monitor;
+	} testmode;
+
+	/* must be last */
+	u8 drv_priv[0] __aligned(sizeof(void *));
 };
 
-struct ath10k *ath10k_core_create(void *hif_priv, struct device *dev,
+struct ath10k *ath10k_core_create(size_t priv_size, struct device *dev,
 				  const struct ath10k_hif_ops *hif_ops);
 void ath10k_core_destroy(struct ath10k *ar);
 
-int ath10k_core_start(struct ath10k *ar);
+int ath10k_core_start(struct ath10k *ar, enum ath10k_firmware_mode mode);
 int ath10k_wait_for_suspend(struct ath10k *ar, u32 suspend_opt);
 void ath10k_core_stop(struct ath10k *ar);
 int ath10k_core_register(struct ath10k *ar, u32 chip_id);
diff --git a/drivers/net/wireless/ath/ath10k/debug.c b/drivers/net/wireless/ath/ath10k/debug.c
index 3030158c478e..3756feba3223 100644
--- a/drivers/net/wireless/ath/ath10k/debug.c
+++ b/drivers/net/wireless/ath/ath10k/debug.c
@@ -17,6 +17,9 @@
 
 #include <linux/module.h>
 #include <linux/debugfs.h>
+#include <linux/version.h>
+#include <linux/vermagic.h>
+#include <linux/vmalloc.h>
 
 #include "core.h"
 #include "debug.h"
@@ -24,25 +27,86 @@
 /* ms */
 #define ATH10K_DEBUG_HTT_STATS_INTERVAL 1000
 
-static int ath10k_printk(const char *level, const char *fmt, ...)
-{
-	struct va_format vaf;
-	va_list args;
-	int rtn;
+#define ATH10K_FW_CRASH_DUMP_VERSION 1
 
-	va_start(args, fmt);
+/**
+ * enum ath10k_fw_crash_dump_type - types of data in the dump file
+ * @ATH10K_FW_CRASH_DUMP_REGDUMP: Register crash dump in binary format
+ */
+enum ath10k_fw_crash_dump_type {
+	ATH10K_FW_CRASH_DUMP_REGISTERS = 0,
 
-	vaf.fmt = fmt;
-	vaf.va = &args;
+	ATH10K_FW_CRASH_DUMP_MAX,
+};
 
-	rtn = printk("%sath10k: %pV", level, &vaf);
+struct ath10k_tlv_dump_data {
+	/* see ath10k_fw_crash_dump_type above */
+	__le32 type;
 
-	va_end(args);
+	/* in bytes */
+	__le32 tlv_len;
 
-	return rtn;
-}
+	/* pad to 32-bit boundaries as needed */
+	u8 tlv_data[];
+} __packed;
+
+struct ath10k_dump_file_data {
+	/* dump file information */
+
+	/* "ATH10K-FW-DUMP" */
+	char df_magic[16];
+
+	__le32 len;
+
+	/* file dump version */
+	__le32 version;
+
+	/* some info we can get from ath10k struct that might help */
+
+	u8 uuid[16];
+
+	__le32 chip_id;
+
+	/* 0 for now, in place for later hardware */
+	__le32 bus_type;
+
+	__le32 target_version;
+	__le32 fw_version_major;
+	__le32 fw_version_minor;
+	__le32 fw_version_release;
+	__le32 fw_version_build;
+	__le32 phy_capability;
+	__le32 hw_min_tx_power;
+	__le32 hw_max_tx_power;
+	__le32 ht_cap_info;
+	__le32 vht_cap_info;
+	__le32 num_rf_chains;
+
+	/* firmware version string */
+	char fw_ver[ETHTOOL_FWVERS_LEN];
+
+	/* Kernel related information */
+
+	/* time-of-day stamp */
+	__le64 tv_sec;
+
+	/* time-of-day stamp, nano-seconds */
+	__le64 tv_nsec;
+
+	/* LINUX_VERSION_CODE */
+	__le32 kernel_ver_code;
+
+	/* VERMAGIC_STRING */
+	char kernel_ver[64];
 
-int ath10k_info(const char *fmt, ...)
+	/* room for growth w/out changing binary format */
+	u8 unused[128];
+
+	/* struct ath10k_tlv_dump_data + more */
+	u8 data[0];
+} __packed;
+
+int ath10k_info(struct ath10k *ar, const char *fmt, ...)
 {
 	struct va_format vaf = {
 		.fmt = fmt,
@@ -52,15 +116,34 @@ int ath10k_info(const char *fmt, ...)
 
 	va_start(args, fmt);
 	vaf.va = &args;
-	ret = ath10k_printk(KERN_INFO, "%pV", &vaf);
-	trace_ath10k_log_info(&vaf);
+	ret = dev_info(ar->dev, "%pV", &vaf);
+	trace_ath10k_log_info(ar, &vaf);
 	va_end(args);
 
 	return ret;
 }
 EXPORT_SYMBOL(ath10k_info);
 
-int ath10k_err(const char *fmt, ...)
+void ath10k_print_driver_info(struct ath10k *ar)
+{
+	ath10k_info(ar, "%s (0x%08x, 0x%08x) fw %s api %d htt %d.%d\n",
+		    ar->hw_params.name,
+		    ar->target_version,
+		    ar->chip_id,
+		    ar->hw->wiphy->fw_version,
+		    ar->fw_api,
+		    ar->htt.target_version_major,
+		    ar->htt.target_version_minor);
+	ath10k_info(ar, "debug %d debugfs %d tracing %d dfs %d testmode %d\n",
+		    config_enabled(CONFIG_ATH10K_DEBUG),
+		    config_enabled(CONFIG_ATH10K_DEBUGFS),
+		    config_enabled(CONFIG_ATH10K_TRACING),
+		    config_enabled(CONFIG_ATH10K_DFS_CERTIFIED),
+		    config_enabled(CONFIG_NL80211_TESTMODE));
+}
+EXPORT_SYMBOL(ath10k_print_driver_info);
+
+int ath10k_err(struct ath10k *ar, const char *fmt, ...)
 {
 	struct va_format vaf = {
 		.fmt = fmt,
@@ -70,33 +153,29 @@ int ath10k_err(const char *fmt, ...)
 
 	va_start(args, fmt);
 	vaf.va = &args;
-	ret = ath10k_printk(KERN_ERR, "%pV", &vaf);
-	trace_ath10k_log_err(&vaf);
+	ret = dev_err(ar->dev, "%pV", &vaf);
+	trace_ath10k_log_err(ar, &vaf);
 	va_end(args);
 
 	return ret;
 }
 EXPORT_SYMBOL(ath10k_err);
 
-int ath10k_warn(const char *fmt, ...)
+int ath10k_warn(struct ath10k *ar, const char *fmt, ...)
 {
 	struct va_format vaf = {
 		.fmt = fmt,
 	};
 	va_list args;
-	int ret = 0;
 
 	va_start(args, fmt);
 	vaf.va = &args;
-
-	if (net_ratelimit())
-		ret = ath10k_printk(KERN_WARNING, "%pV", &vaf);
-
-	trace_ath10k_log_warn(&vaf);
+	dev_warn_ratelimited(ar->dev, "%pV", &vaf);
+	trace_ath10k_log_warn(ar, &vaf);
 
 	va_end(args);
 
-	return ret;
+	return 0;
 }
 EXPORT_SYMBOL(ath10k_warn);
 
@@ -115,9 +194,10 @@ static ssize_t ath10k_read_wmi_services(struct file *file,
 {
 	struct ath10k *ar = file->private_data;
 	char *buf;
-	unsigned int len = 0, buf_len = 1500;
-	const char *status;
+	unsigned int len = 0, buf_len = 4096;
+	const char *name;
 	ssize_t ret_cnt;
+	bool enabled;
 	int i;
 
 	buf = kzalloc(buf_len, GFP_KERNEL);
@@ -129,15 +209,22 @@ static ssize_t ath10k_read_wmi_services(struct file *file,
 	if (len > buf_len)
 		len = buf_len;
 
-	for (i = 0; i < WMI_SERVICE_LAST; i++) {
-		if (WMI_SERVICE_IS_ENABLED(ar->debug.wmi_service_bitmap, i))
-			status = "enabled";
-		else
-			status = "disabled";
+	for (i = 0; i < WMI_SERVICE_MAX; i++) {
+		enabled = test_bit(i, ar->debug.wmi_service_bitmap);
+		name = wmi_service_name(i);
+
+		if (!name) {
+			if (enabled)
+				len += scnprintf(buf + len, buf_len - len,
+						 "%-40s %s (bit %d)\n",
+						 "unknown", "enabled", i);
+
+			continue;
+		}
 
 		len += scnprintf(buf + len, buf_len - len,
-				 "0x%02x - %20s - %s\n",
-				 i, wmi_service_name(i), status);
+				 "%-40s %s\n",
+				 name, enabled ? "enabled" : "-");
 	}
 
 	ret_cnt = simple_read_from_buffer(user_buf, count, ppos, buf, len);
@@ -309,7 +396,7 @@ static ssize_t ath10k_read_fw_stats(struct file *file, char __user *user_buf,
 
 	ret = ath10k_wmi_request_stats(ar, WMI_REQUEST_PEER_STAT);
 	if (ret) {
-		ath10k_warn("could not request stats (%d)\n", ret);
+		ath10k_warn(ar, "could not request stats (%d)\n", ret);
 		goto exit;
 	}
 
@@ -478,16 +565,35 @@ static const struct file_operations fops_fw_stats = {
 	.llseek = default_llseek,
 };
 
+/* This is a clean assert crash in firmware. */
+static int ath10k_debug_fw_assert(struct ath10k *ar)
+{
+	struct wmi_vdev_install_key_cmd *cmd;
+	struct sk_buff *skb;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd) + 16);
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_vdev_install_key_cmd *)skb->data;
+	memset(cmd, 0, sizeof(*cmd));
+
+	/* big enough number so that firmware asserts */
+	cmd->vdev_id = __cpu_to_le32(0x7ffe);
+
+	return ath10k_wmi_cmd_send(ar, skb,
+				   ar->wmi.cmd->vdev_install_key_cmdid);
+}
+
 static ssize_t ath10k_read_simulate_fw_crash(struct file *file,
 					     char __user *user_buf,
 					     size_t count, loff_t *ppos)
 {
-	const char buf[] = "To simulate firmware crash write one of the"
-			   " keywords to this file:\n `soft` - this will send"
-			   " WMI_FORCE_FW_HANG_ASSERT to firmware if FW"
-			   " supports that command.\n `hard` - this will send"
-			   " to firmware command with illegal parameters"
-			   " causing firmware crash.\n";
+	const char buf[] =
+		"To simulate firmware crash write one of the keywords to this file:\n"
+		"`soft` - this will send WMI_FORCE_FW_HANG_ASSERT to firmware if FW supports that command.\n"
+		"`hard` - this will send to firmware command with illegal parameters causing firmware crash.\n"
+		"`assert` - this will send special illegal parameter to firmware to cause assert failure and crash.\n";
 
 	return simple_read_from_buffer(user_buf, count, ppos, buf, strlen(buf));
 }
@@ -527,19 +633,26 @@ static ssize_t ath10k_write_simulate_fw_crash(struct file *file,
 	}
 
 	if (!strcmp(buf, "soft")) {
-		ath10k_info("simulating soft firmware crash\n");
+		ath10k_info(ar, "simulating soft firmware crash\n");
 		ret = ath10k_wmi_force_fw_hang(ar, WMI_FORCE_FW_HANG_ASSERT, 0);
 	} else if (!strcmp(buf, "hard")) {
-		ath10k_info("simulating hard firmware crash\n");
-		ret = ath10k_wmi_vdev_set_param(ar, TARGET_NUM_VDEVS + 1,
-					ar->wmi.vdev_param->rts_threshold, 0);
+		ath10k_info(ar, "simulating hard firmware crash\n");
+		/* 0x7fff is vdev id, and it is always out of range for all
+		 * firmware variants in order to force a firmware crash.
+		 */
+		ret = ath10k_wmi_vdev_set_param(ar, 0x7fff,
+						ar->wmi.vdev_param->rts_threshold,
+						0);
+	} else if (!strcmp(buf, "assert")) {
+		ath10k_info(ar, "simulating firmware assert crash\n");
+		ret = ath10k_debug_fw_assert(ar);
 	} else {
 		ret = -EINVAL;
 		goto exit;
 	}
 
 	if (ret) {
-		ath10k_warn("failed to simulate firmware crash: %d\n", ret);
+		ath10k_warn(ar, "failed to simulate firmware crash: %d\n", ret);
 		goto exit;
 	}
 
@@ -577,6 +690,138 @@ static const struct file_operations fops_chip_id = {
 	.llseek = default_llseek,
 };
 
+struct ath10k_fw_crash_data *
+ath10k_debug_get_new_fw_crash_data(struct ath10k *ar)
+{
+	struct ath10k_fw_crash_data *crash_data = ar->debug.fw_crash_data;
+
+	lockdep_assert_held(&ar->data_lock);
+
+	crash_data->crashed_since_read = true;
+	uuid_le_gen(&crash_data->uuid);
+	getnstimeofday(&crash_data->timestamp);
+
+	return crash_data;
+}
+EXPORT_SYMBOL(ath10k_debug_get_new_fw_crash_data);
+
+static struct ath10k_dump_file_data *ath10k_build_dump_file(struct ath10k *ar)
+{
+	struct ath10k_fw_crash_data *crash_data = ar->debug.fw_crash_data;
+	struct ath10k_dump_file_data *dump_data;
+	struct ath10k_tlv_dump_data *dump_tlv;
+	int hdr_len = sizeof(*dump_data);
+	unsigned int len, sofar = 0;
+	unsigned char *buf;
+
+	len = hdr_len;
+	len += sizeof(*dump_tlv) + sizeof(crash_data->registers);
+
+	sofar += hdr_len;
+
+	/* This is going to get big when we start dumping FW RAM and such,
+	 * so go ahead and use vmalloc.
+	 */
+	buf = vzalloc(len);
+	if (!buf)
+		return NULL;
+
+	spin_lock_bh(&ar->data_lock);
+
+	if (!crash_data->crashed_since_read) {
+		spin_unlock_bh(&ar->data_lock);
+		vfree(buf);
+		return NULL;
+	}
+
+	dump_data = (struct ath10k_dump_file_data *)(buf);
+	strlcpy(dump_data->df_magic, "ATH10K-FW-DUMP",
+		sizeof(dump_data->df_magic));
+	dump_data->len = cpu_to_le32(len);
+
+	dump_data->version = cpu_to_le32(ATH10K_FW_CRASH_DUMP_VERSION);
+
+	memcpy(dump_data->uuid, &crash_data->uuid, sizeof(dump_data->uuid));
+	dump_data->chip_id = cpu_to_le32(ar->chip_id);
+	dump_data->bus_type = cpu_to_le32(0);
+	dump_data->target_version = cpu_to_le32(ar->target_version);
+	dump_data->fw_version_major = cpu_to_le32(ar->fw_version_major);
+	dump_data->fw_version_minor = cpu_to_le32(ar->fw_version_minor);
+	dump_data->fw_version_release = cpu_to_le32(ar->fw_version_release);
+	dump_data->fw_version_build = cpu_to_le32(ar->fw_version_build);
+	dump_data->phy_capability = cpu_to_le32(ar->phy_capability);
+	dump_data->hw_min_tx_power = cpu_to_le32(ar->hw_min_tx_power);
+	dump_data->hw_max_tx_power = cpu_to_le32(ar->hw_max_tx_power);
+	dump_data->ht_cap_info = cpu_to_le32(ar->ht_cap_info);
+	dump_data->vht_cap_info = cpu_to_le32(ar->vht_cap_info);
+	dump_data->num_rf_chains = cpu_to_le32(ar->num_rf_chains);
+
+	strlcpy(dump_data->fw_ver, ar->hw->wiphy->fw_version,
+		sizeof(dump_data->fw_ver));
+
+	dump_data->kernel_ver_code = cpu_to_le32(LINUX_VERSION_CODE);
+	strlcpy(dump_data->kernel_ver, VERMAGIC_STRING,
+		sizeof(dump_data->kernel_ver));
+
+	dump_data->tv_sec = cpu_to_le64(crash_data->timestamp.tv_sec);
+	dump_data->tv_nsec = cpu_to_le64(crash_data->timestamp.tv_nsec);
+
+	/* Gather crash-dump */
+	dump_tlv = (struct ath10k_tlv_dump_data *)(buf + sofar);
+	dump_tlv->type = cpu_to_le32(ATH10K_FW_CRASH_DUMP_REGISTERS);
+	dump_tlv->tlv_len = cpu_to_le32(sizeof(crash_data->registers));
+	memcpy(dump_tlv->tlv_data, &crash_data->registers,
+	       sizeof(crash_data->registers));
+	sofar += sizeof(*dump_tlv) + sizeof(crash_data->registers);
+
+	ar->debug.fw_crash_data->crashed_since_read = false;
+
+	spin_unlock_bh(&ar->data_lock);
+
+	return dump_data;
+}
+
+static int ath10k_fw_crash_dump_open(struct inode *inode, struct file *file)
+{
+	struct ath10k *ar = inode->i_private;
+	struct ath10k_dump_file_data *dump;
+
+	dump = ath10k_build_dump_file(ar);
+	if (!dump)
+		return -ENODATA;
+
+	file->private_data = dump;
+
+	return 0;
+}
+
+static ssize_t ath10k_fw_crash_dump_read(struct file *file,
+					 char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct ath10k_dump_file_data *dump_file = file->private_data;
+
+	return simple_read_from_buffer(user_buf, count, ppos,
+				       dump_file,
+				       le32_to_cpu(dump_file->len));
+}
+
+static int ath10k_fw_crash_dump_release(struct inode *inode,
+					struct file *file)
+{
+	vfree(file->private_data);
+
+	return 0;
+}
+
+static const struct file_operations fops_fw_crash_dump = {
+	.open = ath10k_fw_crash_dump_open,
+	.read = ath10k_fw_crash_dump_read,
+	.release = ath10k_fw_crash_dump_release,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
 static int ath10k_debug_htt_stats_req(struct ath10k *ar)
 {
 	u64 cookie;
@@ -596,7 +841,7 @@ static int ath10k_debug_htt_stats_req(struct ath10k *ar)
 	ret = ath10k_htt_h2t_stats_req(&ar->htt, ar->debug.htt_stats_mask,
 				       cookie);
 	if (ret) {
-		ath10k_warn("failed to send htt stats request: %d\n", ret);
+		ath10k_warn(ar, "failed to send htt stats request: %d\n", ret);
 		return ret;
 	}
 
@@ -619,8 +864,8 @@ static void ath10k_debug_htt_stats_dwork(struct work_struct *work)
 }
 
 static ssize_t ath10k_read_htt_stats_mask(struct file *file,
-					    char __user *user_buf,
-					    size_t count, loff_t *ppos)
+					  char __user *user_buf,
+					  size_t count, loff_t *ppos)
 {
 	struct ath10k *ar = file->private_data;
 	char buf[32];
@@ -632,8 +877,8 @@ static ssize_t ath10k_read_htt_stats_mask(struct file *file,
 }
 
 static ssize_t ath10k_write_htt_stats_mask(struct file *file,
-					     const char __user *user_buf,
-					     size_t count, loff_t *ppos)
+					   const char __user *user_buf,
+					   size_t count, loff_t *ppos)
 {
 	struct ath10k *ar = file->private_data;
 	unsigned long mask;
@@ -738,8 +983,8 @@ static const struct file_operations fops_htt_max_amsdu_ampdu = {
 };
 
 static ssize_t ath10k_read_fw_dbglog(struct file *file,
-					    char __user *user_buf,
-					    size_t count, loff_t *ppos)
+				     char __user *user_buf,
+				     size_t count, loff_t *ppos)
 {
 	struct ath10k *ar = file->private_data;
 	unsigned int len;
@@ -770,7 +1015,7 @@ static ssize_t ath10k_write_fw_dbglog(struct file *file,
 	if (ar->state == ATH10K_STATE_ON) {
 		ret = ath10k_wmi_dbglog_cfg(ar, ar->debug.fw_dbglog_mask);
 		if (ret) {
-			ath10k_warn("dbglog cfg failed from debugfs: %d\n",
+			ath10k_warn(ar, "dbglog cfg failed from debugfs: %d\n",
 				    ret);
 			goto exit;
 		}
@@ -801,13 +1046,14 @@ int ath10k_debug_start(struct ath10k *ar)
 	ret = ath10k_debug_htt_stats_req(ar);
 	if (ret)
 		/* continue normally anyway, this isn't serious */
-		ath10k_warn("failed to start htt stats workqueue: %d\n", ret);
+		ath10k_warn(ar, "failed to start htt stats workqueue: %d\n",
+			    ret);
 
 	if (ar->debug.fw_dbglog_mask) {
 		ret = ath10k_wmi_dbglog_cfg(ar, ar->debug.fw_dbglog_mask);
 		if (ret)
 			/* not serious */
-			ath10k_warn("failed to enable dbglog during start: %d",
+			ath10k_warn(ar, "failed to enable dbglog during start: %d",
 				    ret);
 	}
 
@@ -910,11 +1156,29 @@ static const struct file_operations fops_dfs_stats = {
 
 int ath10k_debug_create(struct ath10k *ar)
 {
+	ar->debug.fw_crash_data = vzalloc(sizeof(*ar->debug.fw_crash_data));
+	if (!ar->debug.fw_crash_data)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void ath10k_debug_destroy(struct ath10k *ar)
+{
+	vfree(ar->debug.fw_crash_data);
+	ar->debug.fw_crash_data = NULL;
+}
+
+int ath10k_debug_register(struct ath10k *ar)
+{
 	ar->debug.debugfs_phy = debugfs_create_dir("ath10k",
 						   ar->hw->wiphy->debugfsdir);
+	if (IS_ERR_OR_NULL(ar->debug.debugfs_phy)) {
+		if (IS_ERR(ar->debug.debugfs_phy))
+			return PTR_ERR(ar->debug.debugfs_phy);
 
-	if (!ar->debug.debugfs_phy)
 		return -ENOMEM;
+	}
 
 	INIT_DELAYED_WORK(&ar->debug.htt_stats_dwork,
 			  ath10k_debug_htt_stats_dwork);
@@ -930,6 +1194,9 @@ int ath10k_debug_create(struct ath10k *ar)
 	debugfs_create_file("simulate_fw_crash", S_IRUSR, ar->debug.debugfs_phy,
 			    ar, &fops_simulate_fw_crash);
 
+	debugfs_create_file("fw_crash_dump", S_IRUSR, ar->debug.debugfs_phy,
+			    ar, &fops_fw_crash_dump);
+
 	debugfs_create_file("chip_id", S_IRUSR, ar->debug.debugfs_phy,
 			    ar, &fops_chip_id);
 
@@ -960,7 +1227,7 @@ int ath10k_debug_create(struct ath10k *ar)
 	return 0;
 }
 
-void ath10k_debug_destroy(struct ath10k *ar)
+void ath10k_debug_unregister(struct ath10k *ar)
 {
 	cancel_delayed_work_sync(&ar->debug.htt_stats_dwork);
 }
@@ -968,7 +1235,8 @@ void ath10k_debug_destroy(struct ath10k *ar)
 #endif /* CONFIG_ATH10K_DEBUGFS */
 
 #ifdef CONFIG_ATH10K_DEBUG
-void ath10k_dbg(enum ath10k_debug_mask mask, const char *fmt, ...)
+void ath10k_dbg(struct ath10k *ar, enum ath10k_debug_mask mask,
+		const char *fmt, ...)
 {
 	struct va_format vaf;
 	va_list args;
@@ -979,27 +1247,28 @@ void ath10k_dbg(enum ath10k_debug_mask mask, const char *fmt, ...)
 	vaf.va = &args;
 
 	if (ath10k_debug_mask & mask)
-		ath10k_printk(KERN_DEBUG, "%pV", &vaf);
+		dev_printk(KERN_DEBUG, ar->dev, "%pV", &vaf);
 
-	trace_ath10k_log_dbg(mask, &vaf);
+	trace_ath10k_log_dbg(ar, mask, &vaf);
 
 	va_end(args);
 }
 EXPORT_SYMBOL(ath10k_dbg);
 
-void ath10k_dbg_dump(enum ath10k_debug_mask mask,
+void ath10k_dbg_dump(struct ath10k *ar,
+		     enum ath10k_debug_mask mask,
 		     const char *msg, const char *prefix,
 		     const void *buf, size_t len)
 {
 	if (ath10k_debug_mask & mask) {
 		if (msg)
-			ath10k_dbg(mask, "%s\n", msg);
+			ath10k_dbg(ar, mask, "%s\n", msg);
 
 		print_hex_dump_bytes(prefix, DUMP_PREFIX_OFFSET, buf, len);
 	}
 
 	/* tracing code doesn't like null strings :/ */
-	trace_ath10k_log_dbg_dump(msg ? msg : "", prefix ? prefix : "",
+	trace_ath10k_log_dbg_dump(ar, msg ? msg : "", prefix ? prefix : "",
 				  buf, len);
 }
 EXPORT_SYMBOL(ath10k_dbg_dump);
diff --git a/drivers/net/wireless/ath/ath10k/debug.h b/drivers/net/wireless/ath/ath10k/debug.h
index a5824990bd2a..b3774f7f492c 100644
--- a/drivers/net/wireless/ath/ath10k/debug.h
+++ b/drivers/net/wireless/ath/ath10k/debug.h
@@ -34,25 +34,33 @@ enum ath10k_debug_mask {
 	ATH10K_DBG_DATA		= 0x00000200,
 	ATH10K_DBG_BMI		= 0x00000400,
 	ATH10K_DBG_REGULATORY	= 0x00000800,
+	ATH10K_DBG_TESTMODE	= 0x00001000,
 	ATH10K_DBG_ANY		= 0xffffffff,
 };
 
 extern unsigned int ath10k_debug_mask;
 
-__printf(1, 2) int ath10k_info(const char *fmt, ...);
-__printf(1, 2) int ath10k_err(const char *fmt, ...);
-__printf(1, 2) int ath10k_warn(const char *fmt, ...);
+__printf(2, 3) int ath10k_info(struct ath10k *ar, const char *fmt, ...);
+__printf(2, 3) int ath10k_err(struct ath10k *ar, const char *fmt, ...);
+__printf(2, 3) int ath10k_warn(struct ath10k *ar, const char *fmt, ...);
+void ath10k_print_driver_info(struct ath10k *ar);
 
 #ifdef CONFIG_ATH10K_DEBUGFS
 int ath10k_debug_start(struct ath10k *ar);
 void ath10k_debug_stop(struct ath10k *ar);
 int ath10k_debug_create(struct ath10k *ar);
 void ath10k_debug_destroy(struct ath10k *ar);
+int ath10k_debug_register(struct ath10k *ar);
+void ath10k_debug_unregister(struct ath10k *ar);
 void ath10k_debug_read_service_map(struct ath10k *ar,
 				   void *service_map,
 				   size_t map_size);
 void ath10k_debug_read_target_stats(struct ath10k *ar,
 				    struct wmi_stats_event *ev);
+struct ath10k_fw_crash_data *
+ath10k_debug_get_new_fw_crash_data(struct ath10k *ar);
+
+void ath10k_debug_dbglog_add(struct ath10k *ar, u8 *buffer, int len);
 
 #define ATH10K_DFS_STAT_INC(ar, c) (ar->debug.dfs_stats.c++)
 
@@ -75,6 +83,15 @@ static inline void ath10k_debug_destroy(struct ath10k *ar)
 {
 }
 
+static inline int ath10k_debug_register(struct ath10k *ar)
+{
+	return 0;
+}
+
+static inline void ath10k_debug_unregister(struct ath10k *ar)
+{
+}
+
 static inline void ath10k_debug_read_service_map(struct ath10k *ar,
 						 void *service_map,
 						 size_t map_size)
@@ -86,25 +103,40 @@ static inline void ath10k_debug_read_target_stats(struct ath10k *ar,
 {
 }
 
+static inline void ath10k_debug_dbglog_add(struct ath10k *ar, u8 *buffer,
+					   int len)
+{
+}
+
+static inline struct ath10k_fw_crash_data *
+ath10k_debug_get_new_fw_crash_data(struct ath10k *ar)
+{
+	return NULL;
+}
+
 #define ATH10K_DFS_STAT_INC(ar, c) do { } while (0)
 
 #endif /* CONFIG_ATH10K_DEBUGFS */
 
 #ifdef CONFIG_ATH10K_DEBUG
-__printf(2, 3) void ath10k_dbg(enum ath10k_debug_mask mask,
+__printf(3, 4) void ath10k_dbg(struct ath10k *ar,
+			       enum ath10k_debug_mask mask,
 			       const char *fmt, ...);
-void ath10k_dbg_dump(enum ath10k_debug_mask mask,
+void ath10k_dbg_dump(struct ath10k *ar,
+		     enum ath10k_debug_mask mask,
 		     const char *msg, const char *prefix,
 		     const void *buf, size_t len);
 #else /* CONFIG_ATH10K_DEBUG */
 
-static inline int ath10k_dbg(enum ath10k_debug_mask dbg_mask,
+static inline int ath10k_dbg(struct ath10k *ar,
+			     enum ath10k_debug_mask dbg_mask,
 			     const char *fmt, ...)
 {
 	return 0;
 }
 
-static inline void ath10k_dbg_dump(enum ath10k_debug_mask mask,
+static inline void ath10k_dbg_dump(struct ath10k *ar,
+				   enum ath10k_debug_mask mask,
 				   const char *msg, const char *prefix,
 				   const void *buf, size_t len)
 {
diff --git a/drivers/net/wireless/ath/ath10k/hif.h b/drivers/net/wireless/ath/ath10k/hif.h
index 2ac7beacddca..62323fea27e1 100644
--- a/drivers/net/wireless/ath/ath10k/hif.h
+++ b/drivers/net/wireless/ath/ath10k/hif.h
@@ -91,7 +91,6 @@ struct ath10k_hif_ops {
 	int (*resume)(struct ath10k *ar);
 };
 
-
 static inline int ath10k_hif_tx_sg(struct ath10k *ar, u8 pipe_id,
 				   struct ath10k_hif_sg_item *items,
 				   int n_items)
diff --git a/drivers/net/wireless/ath/ath10k/htc.c b/drivers/net/wireless/ath/ath10k/htc.c
index 5fdc40d3b378..676bd4ed969b 100644
--- a/drivers/net/wireless/ath/ath10k/htc.c
+++ b/drivers/net/wireless/ath/ath10k/htc.c
@@ -45,10 +45,8 @@ static struct sk_buff *ath10k_htc_build_tx_ctrl_skb(void *ar)
 	struct ath10k_skb_cb *skb_cb;
 
 	skb = dev_alloc_skb(ATH10K_HTC_CONTROL_BUFFER_SIZE);
-	if (!skb) {
-		ath10k_warn("Unable to allocate ctrl skb\n");
+	if (!skb)
 		return NULL;
-	}
 
 	skb_reserve(skb, 20); /* FIXME: why 20 bytes? */
 	WARN_ONCE((unsigned long)skb->data & 3, "unaligned skb");
@@ -56,7 +54,7 @@ static struct sk_buff *ath10k_htc_build_tx_ctrl_skb(void *ar)
 	skb_cb = ATH10K_SKB_CB(skb);
 	memset(skb_cb, 0, sizeof(*skb_cb));
 
-	ath10k_dbg(ATH10K_DBG_HTC, "%s: skb %p\n", __func__, skb);
+	ath10k_dbg(ar, ATH10K_DBG_HTC, "%s: skb %p\n", __func__, skb);
 	return skb;
 }
 
@@ -72,13 +70,15 @@ static inline void ath10k_htc_restore_tx_skb(struct ath10k_htc *htc,
 static void ath10k_htc_notify_tx_completion(struct ath10k_htc_ep *ep,
 					    struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_HTC, "%s: ep %d skb %p\n", __func__,
+	struct ath10k *ar = ep->htc->ar;
+
+	ath10k_dbg(ar, ATH10K_DBG_HTC, "%s: ep %d skb %p\n", __func__,
 		   ep->eid, skb);
 
 	ath10k_htc_restore_tx_skb(ep->htc, skb);
 
 	if (!ep->ep_ops.ep_tx_complete) {
-		ath10k_warn("no tx handler for eid %d\n", ep->eid);
+		ath10k_warn(ar, "no tx handler for eid %d\n", ep->eid);
 		dev_kfree_skb_any(skb);
 		return;
 	}
@@ -89,12 +89,14 @@ static void ath10k_htc_notify_tx_completion(struct ath10k_htc_ep *ep,
 /* assumes tx_lock is held */
 static bool ath10k_htc_ep_need_credit_update(struct ath10k_htc_ep *ep)
 {
+	struct ath10k *ar = ep->htc->ar;
+
 	if (!ep->tx_credit_flow_enabled)
 		return false;
 	if (ep->tx_credits >= ep->tx_credits_per_max_message)
 		return false;
 
-	ath10k_dbg(ATH10K_DBG_HTC, "HTC: endpoint %d needs credit update\n",
+	ath10k_dbg(ar, ATH10K_DBG_HTC, "HTC: endpoint %d needs credit update\n",
 		   ep->eid);
 	return true;
 }
@@ -123,6 +125,7 @@ int ath10k_htc_send(struct ath10k_htc *htc,
 		    enum ath10k_htc_ep_id eid,
 		    struct sk_buff *skb)
 {
+	struct ath10k *ar = htc->ar;
 	struct ath10k_htc_ep *ep = &htc->endpoint[eid];
 	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(skb);
 	struct ath10k_hif_sg_item sg_item;
@@ -134,18 +137,10 @@ int ath10k_htc_send(struct ath10k_htc *htc,
 		return -ECOMM;
 
 	if (eid >= ATH10K_HTC_EP_COUNT) {
-		ath10k_warn("Invalid endpoint id: %d\n", eid);
+		ath10k_warn(ar, "Invalid endpoint id: %d\n", eid);
 		return -ENOENT;
 	}
 
-	/* FIXME: This looks ugly, can we fix it? */
-	spin_lock_bh(&htc->tx_lock);
-	if (htc->stopped) {
-		spin_unlock_bh(&htc->tx_lock);
-		return -ESHUTDOWN;
-	}
-	spin_unlock_bh(&htc->tx_lock);
-
 	skb_push(skb, sizeof(struct ath10k_htc_hdr));
 
 	if (ep->tx_credit_flow_enabled) {
@@ -157,7 +152,7 @@ int ath10k_htc_send(struct ath10k_htc *htc,
 			goto err_pull;
 		}
 		ep->tx_credits -= credits;
-		ath10k_dbg(ATH10K_DBG_HTC,
+		ath10k_dbg(ar, ATH10K_DBG_HTC,
 			   "htc ep %d consumed %d credits (total %d)\n",
 			   eid, credits, ep->tx_credits);
 		spin_unlock_bh(&htc->tx_lock);
@@ -188,7 +183,7 @@ err_credits:
 	if (ep->tx_credit_flow_enabled) {
 		spin_lock_bh(&htc->tx_lock);
 		ep->tx_credits += credits;
-		ath10k_dbg(ATH10K_DBG_HTC,
+		ath10k_dbg(ar, ATH10K_DBG_HTC,
 			   "htc ep %d reverted %d credits back (total %d)\n",
 			   eid, credits, ep->tx_credits);
 		spin_unlock_bh(&htc->tx_lock);
@@ -227,11 +222,12 @@ ath10k_htc_process_credit_report(struct ath10k_htc *htc,
 				 int len,
 				 enum ath10k_htc_ep_id eid)
 {
+	struct ath10k *ar = htc->ar;
 	struct ath10k_htc_ep *ep;
 	int i, n_reports;
 
 	if (len % sizeof(*report))
-		ath10k_warn("Uneven credit report len %d", len);
+		ath10k_warn(ar, "Uneven credit report len %d", len);
 
 	n_reports = len / sizeof(*report);
 
@@ -243,7 +239,7 @@ ath10k_htc_process_credit_report(struct ath10k_htc *htc,
 		ep = &htc->endpoint[report->eid];
 		ep->tx_credits += report->credits;
 
-		ath10k_dbg(ATH10K_DBG_HTC, "htc ep %d got %d credits (total %d)\n",
+		ath10k_dbg(ar, ATH10K_DBG_HTC, "htc ep %d got %d credits (total %d)\n",
 			   report->eid, report->credits, ep->tx_credits);
 
 		if (ep->ep_ops.ep_tx_credits) {
@@ -260,6 +256,7 @@ static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
 				      int length,
 				      enum ath10k_htc_ep_id src_eid)
 {
+	struct ath10k *ar = htc->ar;
 	int status = 0;
 	struct ath10k_htc_record *record;
 	u8 *orig_buffer;
@@ -279,7 +276,7 @@ static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
 
 		if (record->hdr.len > length) {
 			/* no room left in buffer for record */
-			ath10k_warn("Invalid record length: %d\n",
+			ath10k_warn(ar, "Invalid record length: %d\n",
 				    record->hdr.len);
 			status = -EINVAL;
 			break;
@@ -289,7 +286,7 @@ static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
 		case ATH10K_HTC_RECORD_CREDITS:
 			len = sizeof(struct ath10k_htc_credit_report);
 			if (record->hdr.len < len) {
-				ath10k_warn("Credit report too long\n");
+				ath10k_warn(ar, "Credit report too long\n");
 				status = -EINVAL;
 				break;
 			}
@@ -299,7 +296,7 @@ static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
 							 src_eid);
 			break;
 		default:
-			ath10k_warn("Unhandled record: id:%d length:%d\n",
+			ath10k_warn(ar, "Unhandled record: id:%d length:%d\n",
 				    record->hdr.id, record->hdr.len);
 			break;
 		}
@@ -313,7 +310,7 @@ static int ath10k_htc_process_trailer(struct ath10k_htc *htc,
 	}
 
 	if (status)
-		ath10k_dbg_dump(ATH10K_DBG_HTC, "htc rx bad trailer", "",
+		ath10k_dbg_dump(ar, ATH10K_DBG_HTC, "htc rx bad trailer", "",
 				orig_buffer, orig_length);
 
 	return status;
@@ -339,8 +336,8 @@ static int ath10k_htc_rx_completion_handler(struct ath10k *ar,
 	eid = hdr->eid;
 
 	if (eid >= ATH10K_HTC_EP_COUNT) {
-		ath10k_warn("HTC Rx: invalid eid %d\n", eid);
-		ath10k_dbg_dump(ATH10K_DBG_HTC, "htc bad header", "",
+		ath10k_warn(ar, "HTC Rx: invalid eid %d\n", eid);
+		ath10k_dbg_dump(ar, ATH10K_DBG_HTC, "htc bad header", "",
 				hdr, sizeof(*hdr));
 		status = -EINVAL;
 		goto out;
@@ -360,19 +357,19 @@ static int ath10k_htc_rx_completion_handler(struct ath10k *ar,
 	payload_len = __le16_to_cpu(hdr->len);
 
 	if (payload_len + sizeof(*hdr) > ATH10K_HTC_MAX_LEN) {
-		ath10k_warn("HTC rx frame too long, len: %zu\n",
+		ath10k_warn(ar, "HTC rx frame too long, len: %zu\n",
 			    payload_len + sizeof(*hdr));
-		ath10k_dbg_dump(ATH10K_DBG_HTC, "htc bad rx pkt len", "",
+		ath10k_dbg_dump(ar, ATH10K_DBG_HTC, "htc bad rx pkt len", "",
 				hdr, sizeof(*hdr));
 		status = -EINVAL;
 		goto out;
 	}
 
 	if (skb->len < payload_len) {
-		ath10k_dbg(ATH10K_DBG_HTC,
+		ath10k_dbg(ar, ATH10K_DBG_HTC,
 			   "HTC Rx: insufficient length, got %d, expected %d\n",
 			   skb->len, payload_len);
-		ath10k_dbg_dump(ATH10K_DBG_HTC, "htc bad rx pkt len",
+		ath10k_dbg_dump(ar, ATH10K_DBG_HTC, "htc bad rx pkt len",
 				"", hdr, sizeof(*hdr));
 		status = -EINVAL;
 		goto out;
@@ -388,7 +385,7 @@ static int ath10k_htc_rx_completion_handler(struct ath10k *ar,
 
 		if ((trailer_len < min_len) ||
 		    (trailer_len > payload_len)) {
-			ath10k_warn("Invalid trailer length: %d\n",
+			ath10k_warn(ar, "Invalid trailer length: %d\n",
 				    trailer_len);
 			status = -EPROTO;
 			goto out;
@@ -421,7 +418,7 @@ static int ath10k_htc_rx_completion_handler(struct ath10k *ar,
 				 * this is a fatal error, target should not be
 				 * sending unsolicited messages on the ep 0
 				 */
-				ath10k_warn("HTC rx ctrl still processing\n");
+				ath10k_warn(ar, "HTC rx ctrl still processing\n");
 				status = -EINVAL;
 				complete(&htc->ctl_resp);
 				goto out;
@@ -442,7 +439,7 @@ static int ath10k_htc_rx_completion_handler(struct ath10k *ar,
 		goto out;
 	}
 
-	ath10k_dbg(ATH10K_DBG_HTC, "htc rx completion ep %d skb %p\n",
+	ath10k_dbg(ar, ATH10K_DBG_HTC, "htc rx completion ep %d skb %p\n",
 		   eid, skb);
 	ep->ep_ops.ep_rx_complete(ar, skb);
 
@@ -459,7 +456,7 @@ static void ath10k_htc_control_rx_complete(struct ath10k *ar,
 {
 	/* This is unexpected. FW is not supposed to send regular rx on this
 	 * endpoint. */
-	ath10k_warn("unexpected htc rx\n");
+	ath10k_warn(ar, "unexpected htc rx\n");
 	kfree_skb(skb);
 }
 
@@ -546,6 +543,7 @@ static u8 ath10k_htc_get_credit_allocation(struct ath10k_htc *htc,
 
 int ath10k_htc_wait_target(struct ath10k_htc *htc)
 {
+	struct ath10k *ar = htc->ar;
 	int i, status = 0;
 	struct ath10k_htc_svc_conn_req conn_req;
 	struct ath10k_htc_svc_conn_resp conn_resp;
@@ -563,25 +561,25 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 		 * iomap writes unmasking PCI CE irqs aren't propagated
 		 * properly in KVM PCI-passthrough sometimes.
 		 */
-		ath10k_warn("failed to receive control response completion, polling..\n");
+		ath10k_warn(ar, "failed to receive control response completion, polling..\n");
 
 		for (i = 0; i < CE_COUNT; i++)
 			ath10k_hif_send_complete_check(htc->ar, i, 1);
 
 		status = wait_for_completion_timeout(&htc->ctl_resp,
-					ATH10K_HTC_WAIT_TIMEOUT_HZ);
+						     ATH10K_HTC_WAIT_TIMEOUT_HZ);
 
 		if (status == 0)
 			status = -ETIMEDOUT;
 	}
 
 	if (status < 0) {
-		ath10k_err("ctl_resp never came in (%d)\n", status);
+		ath10k_err(ar, "ctl_resp never came in (%d)\n", status);
 		return status;
 	}
 
 	if (htc->control_resp_len < sizeof(msg->hdr) + sizeof(msg->ready)) {
-		ath10k_err("Invalid HTC ready msg len:%d\n",
+		ath10k_err(ar, "Invalid HTC ready msg len:%d\n",
 			   htc->control_resp_len);
 		return -ECOMM;
 	}
@@ -592,21 +590,21 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 	credit_size  = __le16_to_cpu(msg->ready.credit_size);
 
 	if (message_id != ATH10K_HTC_MSG_READY_ID) {
-		ath10k_err("Invalid HTC ready msg: 0x%x\n", message_id);
+		ath10k_err(ar, "Invalid HTC ready msg: 0x%x\n", message_id);
 		return -ECOMM;
 	}
 
 	htc->total_transmit_credits = credit_count;
 	htc->target_credit_size = credit_size;
 
-	ath10k_dbg(ATH10K_DBG_HTC,
+	ath10k_dbg(ar, ATH10K_DBG_HTC,
 		   "Target ready! transmit resources: %d size:%d\n",
 		   htc->total_transmit_credits,
 		   htc->target_credit_size);
 
 	if ((htc->total_transmit_credits == 0) ||
 	    (htc->target_credit_size == 0)) {
-		ath10k_err("Invalid credit size received\n");
+		ath10k_err(ar, "Invalid credit size received\n");
 		return -ECOMM;
 	}
 
@@ -623,7 +621,8 @@ int ath10k_htc_wait_target(struct ath10k_htc *htc)
 	/* connect fake service */
 	status = ath10k_htc_connect_service(htc, &conn_req, &conn_resp);
 	if (status) {
-		ath10k_err("could not connect to htc service (%d)\n", status);
+		ath10k_err(ar, "could not connect to htc service (%d)\n",
+			   status);
 		return status;
 	}
 
@@ -634,6 +633,7 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 			       struct ath10k_htc_svc_conn_req *conn_req,
 			       struct ath10k_htc_svc_conn_resp *conn_resp)
 {
+	struct ath10k *ar = htc->ar;
 	struct ath10k_htc_msg *msg;
 	struct ath10k_htc_conn_svc *req_msg;
 	struct ath10k_htc_conn_svc_response resp_msg_dummy;
@@ -659,13 +659,13 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 	tx_alloc = ath10k_htc_get_credit_allocation(htc,
 						    conn_req->service_id);
 	if (!tx_alloc)
-		ath10k_dbg(ATH10K_DBG_BOOT,
+		ath10k_dbg(ar, ATH10K_DBG_BOOT,
 			   "boot htc service %s does not allocate target credits\n",
 			   htc_service_name(conn_req->service_id));
 
 	skb = ath10k_htc_build_tx_ctrl_skb(htc->ar);
 	if (!skb) {
-		ath10k_err("Failed to allocate HTC packet\n");
+		ath10k_err(ar, "Failed to allocate HTC packet\n");
 		return -ENOMEM;
 	}
 
@@ -703,7 +703,7 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 	if (status <= 0) {
 		if (status == 0)
 			status = -ETIMEDOUT;
-		ath10k_err("Service connect timeout: %d\n", status);
+		ath10k_err(ar, "Service connect timeout: %d\n", status);
 		return status;
 	}
 
@@ -716,11 +716,11 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 	if ((message_id != ATH10K_HTC_MSG_CONNECT_SERVICE_RESP_ID) ||
 	    (htc->control_resp_len < sizeof(msg->hdr) +
 	     sizeof(msg->connect_service_response))) {
-		ath10k_err("Invalid resp message ID 0x%x", message_id);
+		ath10k_err(ar, "Invalid resp message ID 0x%x", message_id);
 		return -EPROTO;
 	}
 
-	ath10k_dbg(ATH10K_DBG_HTC,
+	ath10k_dbg(ar, ATH10K_DBG_HTC,
 		   "HTC Service %s connect response: status: 0x%x, assigned ep: 0x%x\n",
 		   htc_service_name(service_id),
 		   resp_msg->status, resp_msg->eid);
@@ -729,7 +729,7 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 
 	/* check response status */
 	if (resp_msg->status != ATH10K_HTC_CONN_SVC_STATUS_SUCCESS) {
-		ath10k_err("HTC Service %s connect request failed: 0x%x)\n",
+		ath10k_err(ar, "HTC Service %s connect request failed: 0x%x)\n",
 			   htc_service_name(service_id),
 			   resp_msg->status);
 		return -EPROTO;
@@ -780,18 +780,18 @@ setup:
 	if (status)
 		return status;
 
-	ath10k_dbg(ATH10K_DBG_BOOT,
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
 		   "boot htc service '%s' ul pipe %d dl pipe %d eid %d ready\n",
 		   htc_service_name(ep->service_id), ep->ul_pipe_id,
 		   ep->dl_pipe_id, ep->eid);
 
-	ath10k_dbg(ATH10K_DBG_BOOT,
+	ath10k_dbg(ar, ATH10K_DBG_BOOT,
 		   "boot htc ep %d ul polled %d dl polled %d\n",
 		   ep->eid, ep->ul_is_polled, ep->dl_is_polled);
 
 	if (disable_credit_flow_ctrl && ep->tx_credit_flow_enabled) {
 		ep->tx_credit_flow_enabled = false;
-		ath10k_dbg(ATH10K_DBG_BOOT,
+		ath10k_dbg(ar, ATH10K_DBG_BOOT,
 			   "boot htc service '%s' eid %d TX flow control disabled\n",
 			   htc_service_name(ep->service_id), assigned_eid);
 	}
@@ -799,27 +799,26 @@ setup:
 	return status;
 }
 
-struct sk_buff *ath10k_htc_alloc_skb(int size)
+struct sk_buff *ath10k_htc_alloc_skb(struct ath10k *ar, int size)
 {
 	struct sk_buff *skb;
 
 	skb = dev_alloc_skb(size + sizeof(struct ath10k_htc_hdr));
-	if (!skb) {
-		ath10k_warn("could not allocate HTC tx skb\n");
+	if (!skb)
 		return NULL;
-	}
 
 	skb_reserve(skb, sizeof(struct ath10k_htc_hdr));
 
 	/* FW/HTC requires 4-byte aligned streams */
 	if (!IS_ALIGNED((unsigned long)skb->data, 4))
-		ath10k_warn("Unaligned HTC tx skb\n");
+		ath10k_warn(ar, "Unaligned HTC tx skb\n");
 
 	return skb;
 }
 
 int ath10k_htc_start(struct ath10k_htc *htc)
 {
+	struct ath10k *ar = htc->ar;
 	struct sk_buff *skb;
 	int status = 0;
 	struct ath10k_htc_msg *msg;
@@ -835,7 +834,7 @@ int ath10k_htc_start(struct ath10k_htc *htc)
 	msg->hdr.message_id =
 		__cpu_to_le16(ATH10K_HTC_MSG_SETUP_COMPLETE_EX_ID);
 
-	ath10k_dbg(ATH10K_DBG_HTC, "HTC is using TX credit flow control\n");
+	ath10k_dbg(ar, ATH10K_DBG_HTC, "HTC is using TX credit flow control\n");
 
 	status = ath10k_htc_send(htc, ATH10K_HTC_EP_0, skb);
 	if (status) {
@@ -846,13 +845,6 @@ int ath10k_htc_start(struct ath10k_htc *htc)
 	return 0;
 }
 
-void ath10k_htc_stop(struct ath10k_htc *htc)
-{
-	spin_lock_bh(&htc->tx_lock);
-	htc->stopped = true;
-	spin_unlock_bh(&htc->tx_lock);
-}
-
 /* registered target arrival callback from the HIF layer */
 int ath10k_htc_init(struct ath10k *ar)
 {
@@ -862,7 +854,6 @@ int ath10k_htc_init(struct ath10k *ar)
 
 	spin_lock_init(&htc->tx_lock);
 
-	htc->stopped = false;
 	ath10k_htc_reset_endpoint_states(htc);
 
 	/* setup HIF layer callbacks */
diff --git a/drivers/net/wireless/ath/ath10k/htc.h b/drivers/net/wireless/ath/ath10k/htc.h
index 4716d331e6b6..527179c0edce 100644
--- a/drivers/net/wireless/ath/ath10k/htc.h
+++ b/drivers/net/wireless/ath/ath10k/htc.h
@@ -214,7 +214,6 @@ struct ath10k_htc_frame {
 	struct ath10k_htc_record trailer[0];
 } __packed __aligned(4);
 
-
 /*******************/
 /* Host-side stuff */
 /*******************/
@@ -332,7 +331,7 @@ struct ath10k_htc {
 	struct ath10k *ar;
 	struct ath10k_htc_ep endpoint[ATH10K_HTC_EP_COUNT];
 
-	/* protects endpoint and stopped fields */
+	/* protects endpoints */
 	spinlock_t tx_lock;
 
 	struct ath10k_htc_ops htc_ops;
@@ -345,8 +344,6 @@ struct ath10k_htc {
 	int total_transmit_credits;
 	struct ath10k_htc_svc_tx_credits service_tx_alloc[ATH10K_HTC_EP_COUNT];
 	int target_credit_size;
-
-	bool stopped;
 };
 
 int ath10k_htc_init(struct ath10k *ar);
@@ -357,7 +354,6 @@ int ath10k_htc_connect_service(struct ath10k_htc *htc,
 			       struct ath10k_htc_svc_conn_resp *conn_resp);
 int ath10k_htc_send(struct ath10k_htc *htc, enum ath10k_htc_ep_id eid,
 		    struct sk_buff *packet);
-void ath10k_htc_stop(struct ath10k_htc *htc);
-struct sk_buff *ath10k_htc_alloc_skb(int size);
+struct sk_buff *ath10k_htc_alloc_skb(struct ath10k *ar, int size);
 
 #endif
diff --git a/drivers/net/wireless/ath/ath10k/htt.c b/drivers/net/wireless/ath/ath10k/htt.c
index 19c12cc8d663..56cb4aceb383 100644
--- a/drivers/net/wireless/ath/ath10k/htt.c
+++ b/drivers/net/wireless/ath/ath10k/htt.c
@@ -74,12 +74,14 @@ int ath10k_htt_init(struct ath10k *ar)
 
 static int ath10k_htt_verify_version(struct ath10k_htt *htt)
 {
-	ath10k_dbg(ATH10K_DBG_BOOT, "htt target version %d.%d\n",
+	struct ath10k *ar = htt->ar;
+
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "htt target version %d.%d\n",
 		   htt->target_version_major, htt->target_version_minor);
 
 	if (htt->target_version_major != 2 &&
 	    htt->target_version_major != 3) {
-		ath10k_err("unsupported htt major version %d. supported versions are 2 and 3\n",
+		ath10k_err(ar, "unsupported htt major version %d. supported versions are 2 and 3\n",
 			   htt->target_version_major);
 		return -ENOTSUPP;
 	}
@@ -89,6 +91,7 @@ static int ath10k_htt_verify_version(struct ath10k_htt *htt)
 
 int ath10k_htt_setup(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
 	int status;
 
 	init_completion(&htt->target_version_received);
@@ -98,9 +101,9 @@ int ath10k_htt_setup(struct ath10k_htt *htt)
 		return status;
 
 	status = wait_for_completion_timeout(&htt->target_version_received,
-						HTT_TARGET_VERSION_TIMEOUT_HZ);
+					     HTT_TARGET_VERSION_TIMEOUT_HZ);
 	if (status <= 0) {
-		ath10k_warn("htt version request timed out\n");
+		ath10k_warn(ar, "htt version request timed out\n");
 		return -ETIMEDOUT;
 	}
 
diff --git a/drivers/net/wireless/ath/ath10k/htt.h b/drivers/net/wireless/ath/ath10k/htt.h
index 6c93f3885ee5..3b44217a6c19 100644
--- a/drivers/net/wireless/ath/ath10k/htt.h
+++ b/drivers/net/wireless/ath/ath10k/htt.h
@@ -265,7 +265,6 @@ enum htt_mgmt_tx_status {
 
 /*=== target -> host messages ===============================================*/
 
-
 enum htt_t2h_msg_type {
 	HTT_T2H_MSG_TYPE_VERSION_CONF		= 0x0,
 	HTT_T2H_MSG_TYPE_RX_IND			= 0x1,
@@ -1032,6 +1031,7 @@ static inline struct htt_stats_conf_item *htt_stats_conf_next_item(
 {
 	return (void *)item + sizeof(*item) + roundup(item->length, 4);
 }
+
 /*
  * host -> target FRAG DESCRIPTOR/MSDU_EXT DESC bank
  *
@@ -1148,7 +1148,6 @@ struct htt_resp {
 	};
 } __packed;
 
-
 /*** host side structures follow ***/
 
 struct htt_tx_done {
diff --git a/drivers/net/wireless/ath/ath10k/htt_rx.c b/drivers/net/wireless/ath/ath10k/htt_rx.c
index 80cdac15588a..60d40a04508b 100644
--- a/drivers/net/wireless/ath/ath10k/htt_rx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_rx.c
@@ -42,7 +42,6 @@
 /* when under memory pressure rx ring refill may fail and needs a retry */
 #define HTT_RX_RING_REFILL_RETRY_MS 50
 
-
 static int ath10k_htt_rx_get_csum_state(struct sk_buff *skb);
 static void ath10k_htt_txrx_compl_task(unsigned long ptr);
 
@@ -133,7 +132,7 @@ static int __ath10k_htt_rx_ring_fill_n(struct ath10k_htt *htt, int num)
 	dma_addr_t paddr;
 	int ret = 0, idx;
 
-	idx = __le32_to_cpu(*(htt->rx_ring.alloc_idx.vaddr));
+	idx = __le32_to_cpu(*htt->rx_ring.alloc_idx.vaddr);
 	while (num > 0) {
 		skb = dev_alloc_skb(HTT_RX_BUF_SIZE + HTT_RX_DESC_ALIGN);
 		if (!skb) {
@@ -171,7 +170,7 @@ static int __ath10k_htt_rx_ring_fill_n(struct ath10k_htt *htt, int num)
 	}
 
 fail:
-	*(htt->rx_ring.alloc_idx.vaddr) = __cpu_to_le32(idx);
+	*htt->rx_ring.alloc_idx.vaddr = __cpu_to_le32(idx);
 	return ret;
 }
 
@@ -223,6 +222,7 @@ static void ath10k_htt_rx_msdu_buff_replenish(struct ath10k_htt *htt)
 static void ath10k_htt_rx_ring_refill_retry(unsigned long arg)
 {
 	struct ath10k_htt *htt = (struct ath10k_htt *)arg;
+
 	ath10k_htt_rx_msdu_buff_replenish(htt);
 }
 
@@ -271,13 +271,14 @@ void ath10k_htt_rx_free(struct ath10k_htt *htt)
 
 static inline struct sk_buff *ath10k_htt_rx_netbuf_pop(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
 	int idx;
 	struct sk_buff *msdu;
 
 	lockdep_assert_held(&htt->rx_ring.lock);
 
 	if (htt->rx_ring.fill_cnt == 0) {
-		ath10k_warn("tried to pop sk_buff from an empty rx ring\n");
+		ath10k_warn(ar, "tried to pop sk_buff from an empty rx ring\n");
 		return NULL;
 	}
 
@@ -311,14 +312,15 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 				   struct sk_buff **tail_msdu,
 				   u32 *attention)
 {
+	struct ath10k *ar = htt->ar;
 	int msdu_len, msdu_chaining = 0;
-	struct sk_buff *msdu;
+	struct sk_buff *msdu, *next;
 	struct htt_rx_desc *rx_desc;
 
 	lockdep_assert_held(&htt->rx_ring.lock);
 
 	if (htt->rx_confused) {
-		ath10k_warn("htt is confused. refusing rx\n");
+		ath10k_warn(ar, "htt is confused. refusing rx\n");
 		return -1;
 	}
 
@@ -331,7 +333,7 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 				 msdu->len + skb_tailroom(msdu),
 				 DMA_FROM_DEVICE);
 
-		ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt rx pop: ",
+		ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "htt rx pop: ",
 				msdu->data, msdu->len + skb_tailroom(msdu));
 
 		rx_desc = (struct htt_rx_desc *)msdu->data;
@@ -354,7 +356,7 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 			ath10k_htt_rx_free_msdu_chain(*head_msdu);
 			*head_msdu = NULL;
 			msdu = NULL;
-			ath10k_err("htt rx stopped. cannot recover\n");
+			ath10k_err(ar, "htt rx stopped. cannot recover\n");
 			htt->rx_confused = true;
 			break;
 		}
@@ -429,7 +431,7 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 					 next->len + skb_tailroom(next),
 					 DMA_FROM_DEVICE);
 
-			ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL,
+			ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL,
 					"htt rx chained: ", next->data,
 					next->len + skb_tailroom(next));
 
@@ -448,11 +450,11 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 		if (last_msdu) {
 			msdu->next = NULL;
 			break;
-		} else {
-			struct sk_buff *next = ath10k_htt_rx_netbuf_pop(htt);
-			msdu->next = next;
-			msdu = next;
 		}
+
+		next = ath10k_htt_rx_netbuf_pop(htt);
+		msdu->next = next;
+		msdu = next;
 	}
 	*tail_msdu = msdu;
 
@@ -478,18 +480,21 @@ static int ath10k_htt_rx_amsdu_pop(struct ath10k_htt *htt,
 static void ath10k_htt_rx_replenish_task(unsigned long ptr)
 {
 	struct ath10k_htt *htt = (struct ath10k_htt *)ptr;
+
 	ath10k_htt_rx_msdu_buff_replenish(htt);
 }
 
 int ath10k_htt_rx_alloc(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
 	dma_addr_t paddr;
 	void *vaddr;
+	size_t size;
 	struct timer_list *timer = &htt->rx_ring.refill_retry_timer;
 
 	htt->rx_ring.size = ath10k_htt_rx_ring_size(htt);
 	if (!is_power_of_2(htt->rx_ring.size)) {
-		ath10k_warn("htt rx ring size is not power of 2\n");
+		ath10k_warn(ar, "htt rx ring size is not power of 2\n");
 		return -EINVAL;
 	}
 
@@ -512,9 +517,9 @@ int ath10k_htt_rx_alloc(struct ath10k_htt *htt)
 	if (!htt->rx_ring.netbufs_ring)
 		goto err_netbuf;
 
-	vaddr = dma_alloc_coherent(htt->ar->dev,
-		   (htt->rx_ring.size * sizeof(htt->rx_ring.paddrs_ring)),
-		   &paddr, GFP_DMA);
+	size = htt->rx_ring.size * sizeof(htt->rx_ring.paddrs_ring);
+
+	vaddr = dma_alloc_coherent(htt->ar->dev, size, &paddr, GFP_DMA);
 	if (!vaddr)
 		goto err_dma_ring;
 
@@ -550,7 +555,7 @@ int ath10k_htt_rx_alloc(struct ath10k_htt *htt)
 	tasklet_init(&htt->txrx_compl_task, ath10k_htt_txrx_compl_task,
 		     (unsigned long)htt);
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "htt rx ring size %d fill_level %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "htt rx ring size %d fill_level %d\n",
 		   htt->rx_ring.size, htt->rx_ring.fill_level);
 	return 0;
 
@@ -572,7 +577,8 @@ err_netbuf:
 	return -ENOMEM;
 }
 
-static int ath10k_htt_rx_crypto_param_len(enum htt_rx_mpdu_encrypt_type type)
+static int ath10k_htt_rx_crypto_param_len(struct ath10k *ar,
+					  enum htt_rx_mpdu_encrypt_type type)
 {
 	switch (type) {
 	case HTT_RX_MPDU_ENCRYPT_WEP40:
@@ -588,11 +594,12 @@ static int ath10k_htt_rx_crypto_param_len(enum htt_rx_mpdu_encrypt_type type)
 		return 0;
 	}
 
-	ath10k_warn("unknown encryption type %d\n", type);
+	ath10k_warn(ar, "unknown encryption type %d\n", type);
 	return 0;
 }
 
-static int ath10k_htt_rx_crypto_tail_len(enum htt_rx_mpdu_encrypt_type type)
+static int ath10k_htt_rx_crypto_tail_len(struct ath10k *ar,
+					 enum htt_rx_mpdu_encrypt_type type)
 {
 	switch (type) {
 	case HTT_RX_MPDU_ENCRYPT_NONE:
@@ -608,7 +615,7 @@ static int ath10k_htt_rx_crypto_tail_len(enum htt_rx_mpdu_encrypt_type type)
 		return 8;
 	}
 
-	ath10k_warn("unknown encryption type %d\n", type);
+	ath10k_warn(ar, "unknown encryption type %d\n", type);
 	return 0;
 }
 
@@ -620,19 +627,21 @@ static struct ieee80211_hdr *ath10k_htt_rx_skb_get_hdr(struct sk_buff *skb)
 
 	rxd = (void *)skb->data - sizeof(*rxd);
 	fmt = MS(__le32_to_cpu(rxd->msdu_start.info1),
-			RX_MSDU_START_INFO1_DECAP_FORMAT);
+		 RX_MSDU_START_INFO1_DECAP_FORMAT);
 
 	if (fmt == RX_MSDU_DECAP_RAW)
 		return (void *)skb->data;
-	else
-		return (void *)skb->data - RX_HTT_HDR_STATUS_LEN;
+
+	return (void *)skb->data - RX_HTT_HDR_STATUS_LEN;
 }
 
 /* This function only applies for first msdu in an msdu chain */
 static bool ath10k_htt_rx_hdr_is_amsdu(struct ieee80211_hdr *hdr)
 {
+	u8 *qc;
+
 	if (ieee80211_is_data_qos(hdr->frame_control)) {
-		u8 *qc = ieee80211_get_qos_ctl(hdr);
+		qc = ieee80211_get_qos_ctl(hdr);
 		if (qc[0] & 0x80)
 			return true;
 	}
@@ -819,19 +828,55 @@ static bool ath10k_htt_rx_h_channel(struct ath10k *ar,
 	return true;
 }
 
+static const char * const tid_to_ac[] = {
+	"BE",
+	"BK",
+	"BK",
+	"BE",
+	"VI",
+	"VI",
+	"VO",
+	"VO",
+};
+
+static char *ath10k_get_tid(struct ieee80211_hdr *hdr, char *out, size_t size)
+{
+	u8 *qc;
+	int tid;
+
+	if (!ieee80211_is_data_qos(hdr->frame_control))
+		return "";
+
+	qc = ieee80211_get_qos_ctl(hdr);
+	tid = *qc & IEEE80211_QOS_CTL_TID_MASK;
+	if (tid < 8)
+		snprintf(out, size, "tid %d (%s)", tid, tid_to_ac[tid]);
+	else
+		snprintf(out, size, "tid %d", tid);
+
+	return out;
+}
+
 static void ath10k_process_rx(struct ath10k *ar,
 			      struct ieee80211_rx_status *rx_status,
 			      struct sk_buff *skb)
 {
 	struct ieee80211_rx_status *status;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	char tid[32];
 
 	status = IEEE80211_SKB_RXCB(skb);
 	*status = *rx_status;
 
-	ath10k_dbg(ATH10K_DBG_DATA,
-		   "rx skb %p len %u %s%s%s%s%s %srate_idx %u vht_nss %u freq %u band %u flag 0x%x fcs-err %imic-err %i\n",
+	ath10k_dbg(ar, ATH10K_DBG_DATA,
+		   "rx skb %p len %u peer %pM %s %s sn %u %s%s%s%s%s %srate_idx %u vht_nss %u freq %u band %u flag 0x%x fcs-err %i mic-err %i amsdu-more %i\n",
 		   skb,
 		   skb->len,
+		   ieee80211_get_SA(hdr),
+		   ath10k_get_tid(hdr, tid, sizeof(tid)),
+		   is_multicast_ether_addr(ieee80211_get_DA(hdr)) ?
+							"mcast" : "ucast",
+		   (__le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4,
 		   status->flag == 0 ? "legacy" : "",
 		   status->flag & RX_FLAG_HT ? "ht" : "",
 		   status->flag & RX_FLAG_VHT ? "vht" : "",
@@ -843,8 +888,9 @@ static void ath10k_process_rx(struct ath10k *ar,
 		   status->freq,
 		   status->band, status->flag,
 		   !!(status->flag & RX_FLAG_FAILED_FCS_CRC),
-		   !!(status->flag & RX_FLAG_MMIC_ERROR));
-	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "rx skb: ",
+		   !!(status->flag & RX_FLAG_MMIC_ERROR),
+		   !!(status->flag & RX_FLAG_AMSDU_MORE));
+	ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "rx skb: ",
 			skb->data, skb->len);
 
 	ieee80211_rx(ar->hw, skb);
@@ -860,18 +906,19 @@ static void ath10k_htt_rx_amsdu(struct ath10k_htt *htt,
 				struct ieee80211_rx_status *rx_status,
 				struct sk_buff *skb_in)
 {
+	struct ath10k *ar = htt->ar;
 	struct htt_rx_desc *rxd;
 	struct sk_buff *skb = skb_in;
 	struct sk_buff *first;
 	enum rx_msdu_decap_format fmt;
 	enum htt_rx_mpdu_encrypt_type enctype;
 	struct ieee80211_hdr *hdr;
-	u8 hdr_buf[64], addr[ETH_ALEN], *qos;
+	u8 hdr_buf[64], da[ETH_ALEN], sa[ETH_ALEN], *qos;
 	unsigned int hdr_len;
 
 	rxd = (void *)skb->data - sizeof(*rxd);
 	enctype = MS(__le32_to_cpu(rxd->mpdu_start.info0),
-			RX_MPDU_START_INFO0_ENCRYPT_TYPE);
+		     RX_MPDU_START_INFO0_ENCRYPT_TYPE);
 
 	hdr = (struct ieee80211_hdr *)rxd->rx_hdr_status;
 	hdr_len = ieee80211_hdrlen(hdr->frame_control);
@@ -893,8 +940,8 @@ static void ath10k_htt_rx_amsdu(struct ath10k_htt *htt,
 		/* First frame in an A-MSDU chain has more decapped data. */
 		if (skb == first) {
 			len = round_up(ieee80211_hdrlen(hdr->frame_control), 4);
-			len += round_up(ath10k_htt_rx_crypto_param_len(enctype),
-					4);
+			len += round_up(ath10k_htt_rx_crypto_param_len(ar,
+						enctype), 4);
 			decap_hdr += len;
 		}
 
@@ -904,10 +951,11 @@ static void ath10k_htt_rx_amsdu(struct ath10k_htt *htt,
 			skb_trim(skb, skb->len - FCS_LEN);
 			break;
 		case RX_MSDU_DECAP_NATIVE_WIFI:
-			/* pull decapped header and copy DA */
+			/* pull decapped header and copy SA & DA */
 			hdr = (struct ieee80211_hdr *)skb->data;
 			hdr_len = ath10k_htt_rx_nwifi_hdrlen(hdr);
-			memcpy(addr, ieee80211_get_DA(hdr), ETH_ALEN);
+			ether_addr_copy(da, ieee80211_get_DA(hdr));
+			ether_addr_copy(sa, ieee80211_get_SA(hdr));
 			skb_pull(skb, hdr_len);
 
 			/* push original 802.11 header */
@@ -921,8 +969,11 @@ static void ath10k_htt_rx_amsdu(struct ath10k_htt *htt,
 			qos = ieee80211_get_qos_ctl(hdr);
 			qos[0] &= ~IEEE80211_QOS_CTL_A_MSDU_PRESENT;
 
-			/* original 802.11 header has a different DA */
-			memcpy(ieee80211_get_DA(hdr), addr, ETH_ALEN);
+			/* original 802.11 header has a different DA and in
+			 * case of 4addr it may also have different SA
+			 */
+			ether_addr_copy(ieee80211_get_DA(hdr), da);
+			ether_addr_copy(ieee80211_get_SA(hdr), sa);
 			break;
 		case RX_MSDU_DECAP_ETHERNET2_DIX:
 			/* strip ethernet header and insert decapped 802.11
@@ -965,6 +1016,7 @@ static void ath10k_htt_rx_msdu(struct ath10k_htt *htt,
 			       struct ieee80211_rx_status *rx_status,
 			       struct sk_buff *skb)
 {
+	struct ath10k *ar = htt->ar;
 	struct htt_rx_desc *rxd;
 	struct ieee80211_hdr *hdr;
 	enum rx_msdu_decap_format fmt;
@@ -974,16 +1026,16 @@ static void ath10k_htt_rx_msdu(struct ath10k_htt *htt,
 
 	/* This shouldn't happen. If it does than it may be a FW bug. */
 	if (skb->next) {
-		ath10k_warn("htt rx received chained non A-MSDU frame\n");
+		ath10k_warn(ar, "htt rx received chained non A-MSDU frame\n");
 		ath10k_htt_rx_free_msdu_chain(skb->next);
 		skb->next = NULL;
 	}
 
 	rxd = (void *)skb->data - sizeof(*rxd);
 	fmt = MS(__le32_to_cpu(rxd->msdu_start.info1),
-			RX_MSDU_START_INFO1_DECAP_FORMAT);
+		 RX_MSDU_START_INFO1_DECAP_FORMAT);
 	enctype = MS(__le32_to_cpu(rxd->mpdu_start.info0),
-			RX_MPDU_START_INFO0_ENCRYPT_TYPE);
+		     RX_MPDU_START_INFO0_ENCRYPT_TYPE);
 	hdr = (struct ieee80211_hdr *)rxd->rx_hdr_status;
 	hdr_len = ieee80211_hdrlen(hdr->frame_control);
 
@@ -1011,7 +1063,8 @@ static void ath10k_htt_rx_msdu(struct ath10k_htt *htt,
 
 		rfc1042 = hdr;
 		rfc1042 += roundup(hdr_len, 4);
-		rfc1042 += roundup(ath10k_htt_rx_crypto_param_len(enctype), 4);
+		rfc1042 += roundup(ath10k_htt_rx_crypto_param_len(ar,
+					enctype), 4);
 
 		skb_pull(skb, sizeof(struct ethhdr));
 		memcpy(skb_push(skb, sizeof(struct rfc1042_hdr)),
@@ -1120,27 +1173,29 @@ static bool ath10k_htt_rx_amsdu_allowed(struct ath10k_htt *htt,
 					bool channel_set,
 					u32 attention)
 {
+	struct ath10k *ar = htt->ar;
+
 	if (head->len == 0) {
-		ath10k_dbg(ATH10K_DBG_HTT,
+		ath10k_dbg(ar, ATH10K_DBG_HTT,
 			   "htt rx dropping due to zero-len\n");
 		return false;
 	}
 
 	if (attention & RX_ATTENTION_FLAGS_DECRYPT_ERR) {
-		ath10k_dbg(ATH10K_DBG_HTT,
+		ath10k_dbg(ar, ATH10K_DBG_HTT,
 			   "htt rx dropping due to decrypt-err\n");
 		return false;
 	}
 
 	if (!channel_set) {
-		ath10k_warn("no channel configured; ignoring frame!\n");
+		ath10k_warn(ar, "no channel configured; ignoring frame!\n");
 		return false;
 	}
 
 	/* Skip mgmt frames while we handle this in WMI */
 	if (status == HTT_RX_IND_MPDU_STATUS_MGMT_CTRL ||
 	    attention & RX_ATTENTION_FLAGS_MGMT_TYPE) {
-		ath10k_dbg(ATH10K_DBG_HTT, "htt rx mgmt ctrl\n");
+		ath10k_dbg(ar, ATH10K_DBG_HTT, "htt rx mgmt ctrl\n");
 		return false;
 	}
 
@@ -1148,14 +1203,14 @@ static bool ath10k_htt_rx_amsdu_allowed(struct ath10k_htt *htt,
 	    status != HTT_RX_IND_MPDU_STATUS_TKIP_MIC_ERR &&
 	    status != HTT_RX_IND_MPDU_STATUS_ERR_INV_PEER &&
 	    !htt->ar->monitor_started) {
-		ath10k_dbg(ATH10K_DBG_HTT,
+		ath10k_dbg(ar, ATH10K_DBG_HTT,
 			   "htt rx ignoring frame w/ status %d\n",
 			   status);
 		return false;
 	}
 
 	if (test_bit(ATH10K_CAC_RUNNING, &htt->ar->dev_flags)) {
-		ath10k_dbg(ATH10K_DBG_HTT,
+		ath10k_dbg(ar, ATH10K_DBG_HTT,
 			   "htt rx CAC running\n");
 		return false;
 	}
@@ -1166,6 +1221,7 @@ static bool ath10k_htt_rx_amsdu_allowed(struct ath10k_htt *htt,
 static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 				  struct htt_rx_indication *rx)
 {
+	struct ath10k *ar = htt->ar;
 	struct ieee80211_rx_status *rx_status = &htt->rx_status;
 	struct htt_rx_indication_mpdu_range *mpdu_ranges;
 	struct htt_rx_desc *rxd;
@@ -1211,7 +1267,7 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 				      rx_status);
 	}
 
-	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt rx ind: ",
+	ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "htt rx ind: ",
 			rx, sizeof(*rx) +
 			(sizeof(struct htt_rx_indication_mpdu_range) *
 				num_mpdu_ranges));
@@ -1233,7 +1289,7 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 						      &attention);
 
 			if (ret < 0) {
-				ath10k_warn("failed to pop amsdu from htt rx ring %d\n",
+				ath10k_warn(ar, "failed to pop amsdu from htt rx ring %d\n",
 					    ret);
 				ath10k_htt_rx_free_msdu_chain(msdu_head);
 				continue;
@@ -1280,8 +1336,9 @@ static void ath10k_htt_rx_handler(struct ath10k_htt *htt,
 }
 
 static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
-				struct htt_rx_fragment_indication *frag)
+				       struct htt_rx_fragment_indication *frag)
 {
+	struct ath10k *ar = htt->ar;
 	struct sk_buff *msdu_head, *msdu_tail;
 	enum htt_rx_mpdu_encrypt_type enctype;
 	struct htt_rx_desc *rxd;
@@ -1308,10 +1365,10 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 				      &attention);
 	spin_unlock_bh(&htt->rx_ring.lock);
 
-	ath10k_dbg(ATH10K_DBG_HTT_DUMP, "htt rx frag ahead\n");
+	ath10k_dbg(ar, ATH10K_DBG_HTT_DUMP, "htt rx frag ahead\n");
 
 	if (ret) {
-		ath10k_warn("failed to pop amsdu from httr rx ring for fragmented rx %d\n",
+		ath10k_warn(ar, "failed to pop amsdu from httr rx ring for fragmented rx %d\n",
 			    ret);
 		ath10k_htt_rx_free_msdu_chain(msdu_head);
 		return;
@@ -1325,10 +1382,10 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 	tkip_mic_err = !!(attention & RX_ATTENTION_FLAGS_TKIP_MIC_ERR);
 	decrypt_err = !!(attention & RX_ATTENTION_FLAGS_DECRYPT_ERR);
 	fmt = MS(__le32_to_cpu(rxd->msdu_start.info1),
-			RX_MSDU_START_INFO1_DECAP_FORMAT);
+		 RX_MSDU_START_INFO1_DECAP_FORMAT);
 
 	if (fmt != RX_MSDU_DECAP_RAW) {
-		ath10k_warn("we dont support non-raw fragmented rx yet\n");
+		ath10k_warn(ar, "we dont support non-raw fragmented rx yet\n");
 		dev_kfree_skb_any(msdu_head);
 		goto end;
 	}
@@ -1340,17 +1397,17 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 	msdu_head->ip_summed = ath10k_htt_rx_get_csum_state(msdu_head);
 
 	if (tkip_mic_err)
-		ath10k_warn("tkip mic error\n");
+		ath10k_warn(ar, "tkip mic error\n");
 
 	if (decrypt_err) {
-		ath10k_warn("decryption err in fragmented rx\n");
+		ath10k_warn(ar, "decryption err in fragmented rx\n");
 		dev_kfree_skb_any(msdu_head);
 		goto end;
 	}
 
 	if (enctype != HTT_RX_MPDU_ENCRYPT_NONE) {
 		hdrlen = ieee80211_hdrlen(hdr->frame_control);
-		paramlen = ath10k_htt_rx_crypto_param_len(enctype);
+		paramlen = ath10k_htt_rx_crypto_param_len(ar, enctype);
 
 		/* It is more efficient to move the header than the payload */
 		memmove((void *)msdu_head->data + paramlen,
@@ -1364,7 +1421,7 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 	trim  = 4;
 
 	/* remove crypto trailer */
-	trim += ath10k_htt_rx_crypto_tail_len(enctype);
+	trim += ath10k_htt_rx_crypto_tail_len(ar, enctype);
 
 	/* last fragment of TKIP frags has MIC */
 	if (!ieee80211_has_morefrags(hdr->frame_control) &&
@@ -1372,20 +1429,20 @@ static void ath10k_htt_rx_frag_handler(struct ath10k_htt *htt,
 		trim += 8;
 
 	if (trim > msdu_head->len) {
-		ath10k_warn("htt rx fragment: trailer longer than the frame itself? drop\n");
+		ath10k_warn(ar, "htt rx fragment: trailer longer than the frame itself? drop\n");
 		dev_kfree_skb_any(msdu_head);
 		goto end;
 	}
 
 	skb_trim(msdu_head, msdu_head->len - trim);
 
-	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt rx frag mpdu: ",
+	ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "htt rx frag mpdu: ",
 			msdu_head->data, msdu_head->len);
 	ath10k_process_rx(htt->ar, rx_status, msdu_head);
 
 end:
 	if (fw_desc_len > 0) {
-		ath10k_dbg(ATH10K_DBG_HTT,
+		ath10k_dbg(ar, ATH10K_DBG_HTT,
 			   "expecting more fragmented rx in one indication %d\n",
 			   fw_desc_len);
 	}
@@ -1415,12 +1472,12 @@ static void ath10k_htt_rx_frm_tx_compl(struct ath10k *ar,
 		tx_done.discard = true;
 		break;
 	default:
-		ath10k_warn("unhandled tx completion status %d\n", status);
+		ath10k_warn(ar, "unhandled tx completion status %d\n", status);
 		tx_done.discard = true;
 		break;
 	}
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt tx completion num_msdus %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt tx completion num_msdus %d\n",
 		   resp->data_tx_completion.num_msdus);
 
 	for (i = 0; i < resp->data_tx_completion.num_msdus; i++) {
@@ -1441,14 +1498,14 @@ static void ath10k_htt_rx_addba(struct ath10k *ar, struct htt_resp *resp)
 	tid = MS(info0, HTT_RX_BA_INFO0_TID);
 	peer_id = MS(info0, HTT_RX_BA_INFO0_PEER_ID);
 
-	ath10k_dbg(ATH10K_DBG_HTT,
+	ath10k_dbg(ar, ATH10K_DBG_HTT,
 		   "htt rx addba tid %hu peer_id %hu size %hhu\n",
 		   tid, peer_id, ev->window_size);
 
 	spin_lock_bh(&ar->data_lock);
 	peer = ath10k_peer_find_by_id(ar, peer_id);
 	if (!peer) {
-		ath10k_warn("received addba event for invalid peer_id: %hu\n",
+		ath10k_warn(ar, "received addba event for invalid peer_id: %hu\n",
 			    peer_id);
 		spin_unlock_bh(&ar->data_lock);
 		return;
@@ -1456,13 +1513,13 @@ static void ath10k_htt_rx_addba(struct ath10k *ar, struct htt_resp *resp)
 
 	arvif = ath10k_get_arvif(ar, peer->vdev_id);
 	if (!arvif) {
-		ath10k_warn("received addba event for invalid vdev_id: %u\n",
+		ath10k_warn(ar, "received addba event for invalid vdev_id: %u\n",
 			    peer->vdev_id);
 		spin_unlock_bh(&ar->data_lock);
 		return;
 	}
 
-	ath10k_dbg(ATH10K_DBG_HTT,
+	ath10k_dbg(ar, ATH10K_DBG_HTT,
 		   "htt rx start rx ba session sta %pM tid %hu size %hhu\n",
 		   peer->addr, tid, ev->window_size);
 
@@ -1481,14 +1538,14 @@ static void ath10k_htt_rx_delba(struct ath10k *ar, struct htt_resp *resp)
 	tid = MS(info0, HTT_RX_BA_INFO0_TID);
 	peer_id = MS(info0, HTT_RX_BA_INFO0_PEER_ID);
 
-	ath10k_dbg(ATH10K_DBG_HTT,
+	ath10k_dbg(ar, ATH10K_DBG_HTT,
 		   "htt rx delba tid %hu peer_id %hu\n",
 		   tid, peer_id);
 
 	spin_lock_bh(&ar->data_lock);
 	peer = ath10k_peer_find_by_id(ar, peer_id);
 	if (!peer) {
-		ath10k_warn("received addba event for invalid peer_id: %hu\n",
+		ath10k_warn(ar, "received addba event for invalid peer_id: %hu\n",
 			    peer_id);
 		spin_unlock_bh(&ar->data_lock);
 		return;
@@ -1496,13 +1553,13 @@ static void ath10k_htt_rx_delba(struct ath10k *ar, struct htt_resp *resp)
 
 	arvif = ath10k_get_arvif(ar, peer->vdev_id);
 	if (!arvif) {
-		ath10k_warn("received addba event for invalid vdev_id: %u\n",
+		ath10k_warn(ar, "received addba event for invalid vdev_id: %u\n",
 			    peer->vdev_id);
 		spin_unlock_bh(&ar->data_lock);
 		return;
 	}
 
-	ath10k_dbg(ATH10K_DBG_HTT,
+	ath10k_dbg(ar, ATH10K_DBG_HTT,
 		   "htt rx stop rx ba session sta %pM tid %hu\n",
 		   peer->addr, tid);
 
@@ -1517,9 +1574,9 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 
 	/* confirm alignment */
 	if (!IS_ALIGNED((unsigned long)skb->data, 4))
-		ath10k_warn("unaligned htt message, expect trouble\n");
+		ath10k_warn(ar, "unaligned htt message, expect trouble\n");
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt rx, msg_type: 0x%0X\n",
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt rx, msg_type: 0x%0X\n",
 		   resp->hdr.msg_type);
 	switch (resp->hdr.msg_type) {
 	case HTT_T2H_MSG_TYPE_VERSION_CONF: {
@@ -1583,7 +1640,7 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		struct ath10k *ar = htt->ar;
 		struct htt_security_indication *ev = &resp->security_indication;
 
-		ath10k_dbg(ATH10K_DBG_HTT,
+		ath10k_dbg(ar, ATH10K_DBG_HTT,
 			   "sec ind peer_id %d unicast %d type %d\n",
 			  __le16_to_cpu(ev->peer_id),
 			  !!(ev->flags & HTT_SECURITY_IS_UNICAST),
@@ -1592,7 +1649,7 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		break;
 	}
 	case HTT_T2H_MSG_TYPE_RX_FRAG_IND: {
-		ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt event: ",
+		ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "htt event: ",
 				skb->data, skb->len);
 		ath10k_htt_rx_frag_handler(htt, &resp->rx_frag_ind);
 		break;
@@ -1601,7 +1658,7 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		/* FIX THIS */
 		break;
 	case HTT_T2H_MSG_TYPE_STATS_CONF:
-		trace_ath10k_htt_stats(skb->data, skb->len);
+		trace_ath10k_htt_stats(ar, skb->data, skb->len);
 		break;
 	case HTT_T2H_MSG_TYPE_TX_INSPECT_IND:
 		/* Firmware can return tx frames if it's unable to fully
@@ -1609,7 +1666,7 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		 * sends all tx frames as already inspected so this shouldn't
 		 * happen unless fw has a bug.
 		 */
-		ath10k_warn("received an unexpected htt tx inspect event\n");
+		ath10k_warn(ar, "received an unexpected htt tx inspect event\n");
 		break;
 	case HTT_T2H_MSG_TYPE_RX_ADDBA:
 		ath10k_htt_rx_addba(ar, resp);
@@ -1624,9 +1681,9 @@ void ath10k_htt_t2h_msg_handler(struct ath10k *ar, struct sk_buff *skb)
 		break;
 	}
 	default:
-		ath10k_dbg(ATH10K_DBG_HTT, "htt event (%d) not handled\n",
+		ath10k_dbg(ar, ATH10K_DBG_HTT, "htt event (%d) not handled\n",
 			   resp->hdr.msg_type);
-		ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt event: ",
+		ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "htt event: ",
 				skb->data, skb->len);
 		break;
 	};
diff --git a/drivers/net/wireless/ath/ath10k/htt_tx.c b/drivers/net/wireless/ath/ath10k/htt_tx.c
index 8b27bfcc1de3..bd87a35201d8 100644
--- a/drivers/net/wireless/ath/ath10k/htt_tx.c
+++ b/drivers/net/wireless/ath/ath10k/htt_tx.c
@@ -58,6 +58,7 @@ exit:
 
 int ath10k_htt_tx_alloc_msdu_id(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
 	int msdu_id;
 
 	lockdep_assert_held(&htt->tx_lock);
@@ -67,24 +68,29 @@ int ath10k_htt_tx_alloc_msdu_id(struct ath10k_htt *htt)
 	if (msdu_id == htt->max_num_pending_tx)
 		return -ENOBUFS;
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt tx alloc msdu_id %d\n", msdu_id);
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt tx alloc msdu_id %d\n", msdu_id);
 	__set_bit(msdu_id, htt->used_msdu_ids);
 	return msdu_id;
 }
 
 void ath10k_htt_tx_free_msdu_id(struct ath10k_htt *htt, u16 msdu_id)
 {
+	struct ath10k *ar = htt->ar;
+
 	lockdep_assert_held(&htt->tx_lock);
 
 	if (!test_bit(msdu_id, htt->used_msdu_ids))
-		ath10k_warn("trying to free unallocated msdu_id %d\n", msdu_id);
+		ath10k_warn(ar, "trying to free unallocated msdu_id %d\n",
+			    msdu_id);
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt tx free msdu_id %hu\n", msdu_id);
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt tx free msdu_id %hu\n", msdu_id);
 	__clear_bit(msdu_id, htt->used_msdu_ids);
 }
 
 int ath10k_htt_tx_alloc(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
+
 	spin_lock_init(&htt->tx_lock);
 	init_waitqueue_head(&htt->empty_tx_wq);
 
@@ -93,7 +99,7 @@ int ath10k_htt_tx_alloc(struct ath10k_htt *htt)
 	else
 		htt->max_num_pending_tx = TARGET_NUM_MSDU_DESC;
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "htt tx max num pending tx %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "htt tx max num pending tx %d\n",
 		   htt->max_num_pending_tx);
 
 	htt->pending_tx = kzalloc(sizeof(*htt->pending_tx) *
@@ -122,6 +128,7 @@ int ath10k_htt_tx_alloc(struct ath10k_htt *htt)
 
 static void ath10k_htt_tx_free_pending(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
 	struct htt_tx_done tx_done = {0};
 	int msdu_id;
 
@@ -130,7 +137,7 @@ static void ath10k_htt_tx_free_pending(struct ath10k_htt *htt)
 		if (!test_bit(msdu_id, htt->used_msdu_ids))
 			continue;
 
-		ath10k_dbg(ATH10K_DBG_HTT, "force cleanup msdu_id %hu\n",
+		ath10k_dbg(ar, ATH10K_DBG_HTT, "force cleanup msdu_id %hu\n",
 			   msdu_id);
 
 		tx_done.discard = 1;
@@ -147,7 +154,6 @@ void ath10k_htt_tx_free(struct ath10k_htt *htt)
 	kfree(htt->pending_tx);
 	kfree(htt->used_msdu_ids);
 	dma_pool_destroy(htt->tx_pool);
-	return;
 }
 
 void ath10k_htt_htc_tx_complete(struct ath10k *ar, struct sk_buff *skb)
@@ -157,6 +163,7 @@ void ath10k_htt_htc_tx_complete(struct ath10k *ar, struct sk_buff *skb)
 
 int ath10k_htt_h2t_ver_req_msg(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
 	struct sk_buff *skb;
 	struct htt_cmd *cmd;
 	int len = 0;
@@ -165,7 +172,7 @@ int ath10k_htt_h2t_ver_req_msg(struct ath10k_htt *htt)
 	len += sizeof(cmd->hdr);
 	len += sizeof(cmd->ver_req);
 
-	skb = ath10k_htc_alloc_skb(len);
+	skb = ath10k_htc_alloc_skb(ar, len);
 	if (!skb)
 		return -ENOMEM;
 
@@ -184,6 +191,7 @@ int ath10k_htt_h2t_ver_req_msg(struct ath10k_htt *htt)
 
 int ath10k_htt_h2t_stats_req(struct ath10k_htt *htt, u8 mask, u64 cookie)
 {
+	struct ath10k *ar = htt->ar;
 	struct htt_stats_req *req;
 	struct sk_buff *skb;
 	struct htt_cmd *cmd;
@@ -192,7 +200,7 @@ int ath10k_htt_h2t_stats_req(struct ath10k_htt *htt, u8 mask, u64 cookie)
 	len += sizeof(cmd->hdr);
 	len += sizeof(cmd->stats_req);
 
-	skb = ath10k_htc_alloc_skb(len);
+	skb = ath10k_htc_alloc_skb(ar, len);
 	if (!skb)
 		return -ENOMEM;
 
@@ -214,7 +222,8 @@ int ath10k_htt_h2t_stats_req(struct ath10k_htt *htt, u8 mask, u64 cookie)
 
 	ret = ath10k_htc_send(&htt->ar->htc, htt->eid, skb);
 	if (ret) {
-		ath10k_warn("failed to send htt type stats request: %d", ret);
+		ath10k_warn(ar, "failed to send htt type stats request: %d",
+			    ret);
 		dev_kfree_skb_any(skb);
 		return ret;
 	}
@@ -224,6 +233,7 @@ int ath10k_htt_h2t_stats_req(struct ath10k_htt *htt, u8 mask, u64 cookie)
 
 int ath10k_htt_send_rx_ring_cfg_ll(struct ath10k_htt *htt)
 {
+	struct ath10k *ar = htt->ar;
 	struct sk_buff *skb;
 	struct htt_cmd *cmd;
 	struct htt_rx_ring_setup_ring *ring;
@@ -242,7 +252,7 @@ int ath10k_htt_send_rx_ring_cfg_ll(struct ath10k_htt *htt)
 
 	len = sizeof(cmd->hdr) + sizeof(cmd->rx_setup.hdr)
 	    + (sizeof(*ring) * num_rx_ring);
-	skb = ath10k_htc_alloc_skb(len);
+	skb = ath10k_htc_alloc_skb(ar, len);
 	if (!skb)
 		return -ENOMEM;
 
@@ -311,6 +321,7 @@ int ath10k_htt_h2t_aggr_cfg_msg(struct ath10k_htt *htt,
 				u8 max_subfrms_ampdu,
 				u8 max_subfrms_amsdu)
 {
+	struct ath10k *ar = htt->ar;
 	struct htt_aggr_conf *aggr_conf;
 	struct sk_buff *skb;
 	struct htt_cmd *cmd;
@@ -328,7 +339,7 @@ int ath10k_htt_h2t_aggr_cfg_msg(struct ath10k_htt *htt,
 	len = sizeof(cmd->hdr);
 	len += sizeof(cmd->aggr_conf);
 
-	skb = ath10k_htc_alloc_skb(len);
+	skb = ath10k_htc_alloc_skb(ar, len);
 	if (!skb)
 		return -ENOMEM;
 
@@ -340,7 +351,7 @@ int ath10k_htt_h2t_aggr_cfg_msg(struct ath10k_htt *htt,
 	aggr_conf->max_num_ampdu_subframes = max_subfrms_ampdu;
 	aggr_conf->max_num_amsdu_subframes = max_subfrms_amsdu;
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt h2t aggr cfg msg amsdu %d ampdu %d",
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt h2t aggr cfg msg amsdu %d ampdu %d",
 		   aggr_conf->max_num_amsdu_subframes,
 		   aggr_conf->max_num_ampdu_subframes);
 
@@ -355,7 +366,8 @@ int ath10k_htt_h2t_aggr_cfg_msg(struct ath10k_htt *htt,
 
 int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 {
-	struct device *dev = htt->ar->dev;
+	struct ath10k *ar = htt->ar;
+	struct device *dev = ar->dev;
 	struct sk_buff *txdesc = NULL;
 	struct htt_cmd *cmd;
 	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(msdu);
@@ -364,7 +376,6 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	int msdu_id = -1;
 	int res;
 
-
 	res = ath10k_htt_tx_inc_pending(htt);
 	if (res)
 		goto err;
@@ -382,7 +393,7 @@ int ath10k_htt_mgmt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	htt->pending_tx[msdu_id] = msdu;
 	spin_unlock_bh(&htt->tx_lock);
 
-	txdesc = ath10k_htc_alloc_skb(len);
+	txdesc = ath10k_htc_alloc_skb(ar, len);
 	if (!txdesc) {
 		res = -ENOMEM;
 		goto err_free_msdu_id;
@@ -429,7 +440,8 @@ err:
 
 int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 {
-	struct device *dev = htt->ar->dev;
+	struct ath10k *ar = htt->ar;
+	struct device *dev = ar->dev;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)msdu->data;
 	struct ath10k_skb_cb *skb_cb = ATH10K_SKB_CB(msdu);
 	struct ath10k_hif_sg_item sg_items[2];
@@ -545,11 +557,11 @@ int ath10k_htt_tx(struct ath10k_htt *htt, struct sk_buff *msdu)
 	skb_cb->htt.txbuf->cmd_tx.frags_paddr = __cpu_to_le32(frags_paddr);
 	skb_cb->htt.txbuf->cmd_tx.peerid = __cpu_to_le32(HTT_INVALID_PEERID);
 
-	ath10k_dbg(ATH10K_DBG_HTT,
+	ath10k_dbg(ar, ATH10K_DBG_HTT,
 		   "htt tx flags0 %hhu flags1 %hu len %d id %hu frags_paddr %08x, msdu_paddr %08x vdev %hhu tid %hhu\n",
 		   flags0, flags1, msdu->len, msdu_id, frags_paddr,
 		   (u32)skb_cb->paddr, vdev_id, tid);
-	ath10k_dbg_dump(ATH10K_DBG_HTT_DUMP, NULL, "htt tx msdu: ",
+	ath10k_dbg_dump(ar, ATH10K_DBG_HTT_DUMP, NULL, "htt tx msdu: ",
 			msdu->data, msdu->len);
 
 	sg_items[0].transfer_id = 0;
diff --git a/drivers/net/wireless/ath/ath10k/hw.h b/drivers/net/wireless/ath/ath10k/hw.h
index 007e855f4ba9..3cf5702c1e7e 100644
--- a/drivers/net/wireless/ath/ath10k/hw.h
+++ b/drivers/net/wireless/ath/ath10k/hw.h
@@ -28,16 +28,21 @@
 #define QCA988X_HW_2_0_CHIP_ID_REV	0x2
 #define QCA988X_HW_2_0_FW_DIR		"ath10k/QCA988X/hw2.0"
 #define QCA988X_HW_2_0_FW_FILE		"firmware.bin"
-#define QCA988X_HW_2_0_FW_2_FILE	"firmware-2.bin"
+#define QCA988X_HW_2_0_FW_3_FILE	"firmware-3.bin"
 #define QCA988X_HW_2_0_OTP_FILE		"otp.bin"
 #define QCA988X_HW_2_0_BOARD_DATA_FILE	"board.bin"
 #define QCA988X_HW_2_0_PATCH_LOAD_ADDR	0x1234
 
 #define ATH10K_FW_API2_FILE		"firmware-2.bin"
+#define ATH10K_FW_API3_FILE		"firmware-3.bin"
+
+#define ATH10K_FW_UTF_FILE		"utf.bin"
 
 /* includes also the null byte */
 #define ATH10K_FIRMWARE_MAGIC               "QCA-ATH10K"
 
+#define REG_DUMP_COUNT_QCA988X 60
+
 struct ath10k_fw_ie {
 	__le32 id;
 	__le32 len;
diff --git a/drivers/net/wireless/ath/ath10k/mac.c b/drivers/net/wireless/ath/ath10k/mac.c
index 9d61bb157189..46709301a51e 100644
--- a/drivers/net/wireless/ath/ath10k/mac.c
+++ b/drivers/net/wireless/ath/ath10k/mac.c
@@ -26,6 +26,7 @@
 #include "wmi.h"
 #include "htt.h"
 #include "txrx.h"
+#include "testmode.h"
 
 /**********/
 /* Crypto */
@@ -36,6 +37,7 @@ static int ath10k_send_key(struct ath10k_vif *arvif,
 			   enum set_key_cmd cmd,
 			   const u8 *macaddr)
 {
+	struct ath10k *ar = arvif->ar;
 	struct wmi_vdev_install_key_arg arg = {
 		.vdev_id = arvif->vdev_id,
 		.key_idx = key->keyidx,
@@ -73,7 +75,7 @@ static int ath10k_send_key(struct ath10k_vif *arvif,
 			arg.key_flags = WMI_KEY_PAIRWISE;
 		break;
 	default:
-		ath10k_warn("cipher %d is not supported\n", key->cipher);
+		ath10k_warn(ar, "cipher %d is not supported\n", key->cipher);
 		return -EOPNOTSUPP;
 	}
 
@@ -168,7 +170,7 @@ static int ath10k_clear_peer_keys(struct ath10k_vif *arvif,
 			first_errno = ret;
 
 		if (ret)
-			ath10k_warn("failed to remove peer wep key %d: %d\n",
+			ath10k_warn(ar, "failed to remove peer wep key %d: %d\n",
 				    i, ret);
 
 		peer->keys[i] = NULL;
@@ -197,7 +199,7 @@ static int ath10k_clear_vdev_key(struct ath10k_vif *arvif,
 		list_for_each_entry(peer, &ar->peers, list) {
 			for (i = 0; i < ARRAY_SIZE(peer->keys); i++) {
 				if (peer->keys[i] == key) {
-					memcpy(addr, peer->addr, ETH_ALEN);
+					ether_addr_copy(addr, peer->addr);
 					peer->keys[i] = NULL;
 					break;
 				}
@@ -216,14 +218,13 @@ static int ath10k_clear_vdev_key(struct ath10k_vif *arvif,
 			first_errno = ret;
 
 		if (ret)
-			ath10k_warn("failed to remove key for %pM: %d\n",
+			ath10k_warn(ar, "failed to remove key for %pM: %d\n",
 				    addr, ret);
 	}
 
 	return first_errno;
 }
 
-
 /*********************/
 /* General utilities */
 /*********************/
@@ -327,14 +328,14 @@ static int ath10k_peer_create(struct ath10k *ar, u32 vdev_id, const u8 *addr)
 
 	ret = ath10k_wmi_peer_create(ar, vdev_id, addr);
 	if (ret) {
-		ath10k_warn("failed to create wmi peer %pM on vdev %i: %i\n",
+		ath10k_warn(ar, "failed to create wmi peer %pM on vdev %i: %i\n",
 			    addr, vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_wait_for_peer_created(ar, vdev_id, addr);
 	if (ret) {
-		ath10k_warn("failed to wait for created wmi peer %pM on vdev %i: %i\n",
+		ath10k_warn(ar, "failed to wait for created wmi peer %pM on vdev %i: %i\n",
 			    addr, vdev_id, ret);
 		return ret;
 	}
@@ -355,7 +356,7 @@ static int ath10k_mac_set_kickout(struct ath10k_vif *arvif)
 	ret = ath10k_wmi_pdev_set_param(ar, param,
 					ATH10K_KICKOUT_THRESHOLD);
 	if (ret) {
-		ath10k_warn("failed to set kickout threshold on vdev %i: %d\n",
+		ath10k_warn(ar, "failed to set kickout threshold on vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -364,7 +365,7 @@ static int ath10k_mac_set_kickout(struct ath10k_vif *arvif)
 	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, param,
 					ATH10K_KEEPALIVE_MIN_IDLE);
 	if (ret) {
-		ath10k_warn("failed to set keepalive minimum idle time on vdev %i: %d\n",
+		ath10k_warn(ar, "failed to set keepalive minimum idle time on vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -373,7 +374,7 @@ static int ath10k_mac_set_kickout(struct ath10k_vif *arvif)
 	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, param,
 					ATH10K_KEEPALIVE_MAX_IDLE);
 	if (ret) {
-		ath10k_warn("failed to set keepalive maximum idle time on vdev %i: %d\n",
+		ath10k_warn(ar, "failed to set keepalive maximum idle time on vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -382,7 +383,7 @@ static int ath10k_mac_set_kickout(struct ath10k_vif *arvif)
 	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, param,
 					ATH10K_KEEPALIVE_MAX_UNRESPONSIVE);
 	if (ret) {
-		ath10k_warn("failed to set keepalive maximum unresponsive time on vdev %i: %d\n",
+		ath10k_warn(ar, "failed to set keepalive maximum unresponsive time on vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -449,7 +450,7 @@ static void ath10k_peer_cleanup(struct ath10k *ar, u32 vdev_id)
 		if (peer->vdev_id != vdev_id)
 			continue;
 
-		ath10k_warn("removing stale peer %pM from vdev_id %d\n",
+		ath10k_warn(ar, "removing stale peer %pM from vdev_id %d\n",
 			    peer->addr, vdev_id);
 
 		list_del(&peer->list);
@@ -492,19 +493,6 @@ static inline int ath10k_vdev_setup_sync(struct ath10k *ar)
 	return 0;
 }
 
-static bool ath10k_monitor_is_enabled(struct ath10k *ar)
-{
-	lockdep_assert_held(&ar->conf_mutex);
-
-	ath10k_dbg(ATH10K_DBG_MAC,
-		   "mac monitor refs: promisc %d monitor %d cac %d\n",
-		   ar->promisc, ar->monitor,
-		   test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags));
-
-	return ar->promisc || ar->monitor ||
-	       test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
-}
-
 static int ath10k_monitor_vdev_start(struct ath10k *ar, int vdev_id)
 {
 	struct cfg80211_chan_def *chandef = &ar->chandef;
@@ -531,35 +519,35 @@ static int ath10k_monitor_vdev_start(struct ath10k *ar, int vdev_id)
 
 	ret = ath10k_wmi_vdev_start(ar, &arg);
 	if (ret) {
-		ath10k_warn("failed to request monitor vdev %i start: %d\n",
+		ath10k_warn(ar, "failed to request monitor vdev %i start: %d\n",
 			    vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn("failed to synchronize setup for monitor vdev %i: %d\n",
+		ath10k_warn(ar, "failed to synchronize setup for monitor vdev %i: %d\n",
 			    vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_wmi_vdev_up(ar, vdev_id, 0, ar->mac_addr);
 	if (ret) {
-		ath10k_warn("failed to put up monitor vdev %i: %d\n",
+		ath10k_warn(ar, "failed to put up monitor vdev %i: %d\n",
 			    vdev_id, ret);
 		goto vdev_stop;
 	}
 
 	ar->monitor_vdev_id = vdev_id;
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor vdev %i started\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor vdev %i started\n",
 		   ar->monitor_vdev_id);
 	return 0;
 
 vdev_stop:
 	ret = ath10k_wmi_vdev_stop(ar, ar->monitor_vdev_id);
 	if (ret)
-		ath10k_warn("failed to stop monitor vdev %i after start failure: %d\n",
+		ath10k_warn(ar, "failed to stop monitor vdev %i after start failure: %d\n",
 			    ar->monitor_vdev_id, ret);
 
 	return ret;
@@ -573,20 +561,20 @@ static int ath10k_monitor_vdev_stop(struct ath10k *ar)
 
 	ret = ath10k_wmi_vdev_down(ar, ar->monitor_vdev_id);
 	if (ret)
-		ath10k_warn("failed to put down monitor vdev %i: %d\n",
+		ath10k_warn(ar, "failed to put down monitor vdev %i: %d\n",
 			    ar->monitor_vdev_id, ret);
 
 	ret = ath10k_wmi_vdev_stop(ar, ar->monitor_vdev_id);
 	if (ret)
-		ath10k_warn("failed to to request monitor vdev %i stop: %d\n",
+		ath10k_warn(ar, "failed to to request monitor vdev %i stop: %d\n",
 			    ar->monitor_vdev_id, ret);
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret)
-		ath10k_warn("failed to synchronise monitor vdev %i: %d\n",
+		ath10k_warn(ar, "failed to synchronise monitor vdev %i: %d\n",
 			    ar->monitor_vdev_id, ret);
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor vdev %i stopped\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor vdev %i stopped\n",
 		   ar->monitor_vdev_id);
 	return ret;
 }
@@ -597,35 +585,29 @@ static int ath10k_monitor_vdev_create(struct ath10k *ar)
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	bit = ffs(ar->free_vdev_map);
-	if (bit == 0) {
-		ath10k_warn("failed to find free vdev id for monitor vdev\n");
+	if (ar->free_vdev_map == 0) {
+		ath10k_warn(ar, "failed to find free vdev id for monitor vdev\n");
 		return -ENOMEM;
 	}
 
+	bit = ffs(ar->free_vdev_map);
+
 	ar->monitor_vdev_id = bit - 1;
-	ar->free_vdev_map &= ~(1 << ar->monitor_vdev_id);
 
 	ret = ath10k_wmi_vdev_create(ar, ar->monitor_vdev_id,
 				     WMI_VDEV_TYPE_MONITOR,
 				     0, ar->mac_addr);
 	if (ret) {
-		ath10k_warn("failed to request monitor vdev %i creation: %d\n",
+		ath10k_warn(ar, "failed to request monitor vdev %i creation: %d\n",
 			    ar->monitor_vdev_id, ret);
-		goto vdev_fail;
+		return ret;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor vdev %d created\n",
+	ar->free_vdev_map &= ~(1 << ar->monitor_vdev_id);
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor vdev %d created\n",
 		   ar->monitor_vdev_id);
 
 	return 0;
-
-vdev_fail:
-	/*
-	 * Restore the ID to the global map.
-	 */
-	ar->free_vdev_map |= 1 << (ar->monitor_vdev_id);
-	return ret;
 }
 
 static int ath10k_monitor_vdev_delete(struct ath10k *ar)
@@ -636,14 +618,14 @@ static int ath10k_monitor_vdev_delete(struct ath10k *ar)
 
 	ret = ath10k_wmi_vdev_delete(ar, ar->monitor_vdev_id);
 	if (ret) {
-		ath10k_warn("failed to request wmi monitor vdev %i removal: %d\n",
+		ath10k_warn(ar, "failed to request wmi monitor vdev %i removal: %d\n",
 			    ar->monitor_vdev_id, ret);
 		return ret;
 	}
 
-	ar->free_vdev_map |= 1 << (ar->monitor_vdev_id);
+	ar->free_vdev_map |= 1 << ar->monitor_vdev_id;
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor vdev %d deleted\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor vdev %d deleted\n",
 		   ar->monitor_vdev_id);
 	return ret;
 }
@@ -654,63 +636,70 @@ static int ath10k_monitor_start(struct ath10k *ar)
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	if (!ath10k_monitor_is_enabled(ar)) {
-		ath10k_warn("trying to start monitor with no references\n");
-		return 0;
-	}
-
-	if (ar->monitor_started) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac monitor already started\n");
-		return 0;
-	}
-
 	ret = ath10k_monitor_vdev_create(ar);
 	if (ret) {
-		ath10k_warn("failed to create monitor vdev: %d\n", ret);
+		ath10k_warn(ar, "failed to create monitor vdev: %d\n", ret);
 		return ret;
 	}
 
 	ret = ath10k_monitor_vdev_start(ar, ar->monitor_vdev_id);
 	if (ret) {
-		ath10k_warn("failed to start monitor vdev: %d\n", ret);
+		ath10k_warn(ar, "failed to start monitor vdev: %d\n", ret);
 		ath10k_monitor_vdev_delete(ar);
 		return ret;
 	}
 
 	ar->monitor_started = true;
-	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor started\n");
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor started\n");
 
 	return 0;
 }
 
-static void ath10k_monitor_stop(struct ath10k *ar)
+static int ath10k_monitor_stop(struct ath10k *ar)
 {
 	int ret;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	if (ath10k_monitor_is_enabled(ar)) {
-		ath10k_dbg(ATH10K_DBG_MAC,
-			   "mac monitor will be stopped later\n");
-		return;
+	ret = ath10k_monitor_vdev_stop(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to stop monitor vdev: %d\n", ret);
+		return ret;
 	}
 
-	if (!ar->monitor_started) {
-		ath10k_dbg(ATH10K_DBG_MAC,
-			   "mac monitor probably failed to start earlier\n");
-		return;
+	ret = ath10k_monitor_vdev_delete(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to delete monitor vdev: %d\n", ret);
+		return ret;
 	}
 
-	ret = ath10k_monitor_vdev_stop(ar);
-	if (ret)
-		ath10k_warn("failed to stop monitor vdev: %d\n", ret);
+	ar->monitor_started = false;
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac monitor stopped\n");
 
-	ret = ath10k_monitor_vdev_delete(ar);
-	if (ret)
-		ath10k_warn("failed to delete monitor vdev: %d\n", ret);
+	return 0;
+}
 
-	ar->monitor_started = false;
-	ath10k_dbg(ATH10K_DBG_MAC, "mac monitor stopped\n");
+static int ath10k_monitor_recalc(struct ath10k *ar)
+{
+	bool should_start;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	should_start = ar->monitor ||
+		       ar->filter_flags & FIF_PROMISC_IN_BSS ||
+		       test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
+
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
+		   "mac monitor recalc started? %d should? %d\n",
+		   ar->monitor_started, should_start);
+
+	if (should_start == ar->monitor_started)
+		return 0;
+
+	if (should_start)
+		return ath10k_monitor_start(ar);
+
+	return ath10k_monitor_stop(ar);
 }
 
 static int ath10k_recalc_rtscts_prot(struct ath10k_vif *arvif)
@@ -741,14 +730,14 @@ static int ath10k_start_cac(struct ath10k *ar)
 
 	set_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
 
-	ret = ath10k_monitor_start(ar);
+	ret = ath10k_monitor_recalc(ar);
 	if (ret) {
-		ath10k_warn("failed to start monitor (cac): %d\n", ret);
+		ath10k_warn(ar, "failed to start monitor (cac): %d\n", ret);
 		clear_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
 		return ret;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac cac start monitor vdev %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac cac start monitor vdev %d\n",
 		   ar->monitor_vdev_id);
 
 	return 0;
@@ -765,7 +754,7 @@ static int ath10k_stop_cac(struct ath10k *ar)
 	clear_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
 	ath10k_monitor_stop(ar);
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac cac finished\n");
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac cac finished\n");
 
 	return 0;
 }
@@ -791,12 +780,12 @@ static void ath10k_recalc_radar_detection(struct ath10k *ar)
 		 * radiation is not allowed, make this channel DFS_UNAVAILABLE
 		 * by indicating that radar was detected.
 		 */
-		ath10k_warn("failed to start CAC: %d\n", ret);
+		ath10k_warn(ar, "failed to start CAC: %d\n", ret);
 		ieee80211_radar_detected(ar->hw);
 	}
 }
 
-static int ath10k_vdev_start(struct ath10k_vif *arvif)
+static int ath10k_vdev_start_restart(struct ath10k_vif *arvif, bool restart)
 {
 	struct ath10k *ar = arvif->ar;
 	struct cfg80211_chan_def *chandef = &ar->chandef;
@@ -833,21 +822,25 @@ static int ath10k_vdev_start(struct ath10k_vif *arvif)
 		arg.ssid_len = arvif->vif->bss_conf.ssid_len;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC,
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
 		   "mac vdev %d start center_freq %d phymode %s\n",
 		   arg.vdev_id, arg.channel.freq,
 		   ath10k_wmi_phymode_str(arg.channel.mode));
 
-	ret = ath10k_wmi_vdev_start(ar, &arg);
+	if (restart)
+		ret = ath10k_wmi_vdev_restart(ar, &arg);
+	else
+		ret = ath10k_wmi_vdev_start(ar, &arg);
+
 	if (ret) {
-		ath10k_warn("failed to start WMI vdev %i: %d\n",
+		ath10k_warn(ar, "failed to start WMI vdev %i: %d\n",
 			    arg.vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn("failed to synchronise setup for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to synchronise setup for vdev %i: %d\n",
 			    arg.vdev_id, ret);
 		return ret;
 	}
@@ -858,6 +851,16 @@ static int ath10k_vdev_start(struct ath10k_vif *arvif)
 	return ret;
 }
 
+static int ath10k_vdev_start(struct ath10k_vif *arvif)
+{
+	return ath10k_vdev_start_restart(arvif, false);
+}
+
+static int ath10k_vdev_restart(struct ath10k_vif *arvif)
+{
+	return ath10k_vdev_start_restart(arvif, true);
+}
+
 static int ath10k_vdev_stop(struct ath10k_vif *arvif)
 {
 	struct ath10k *ar = arvif->ar;
@@ -869,14 +872,14 @@ static int ath10k_vdev_stop(struct ath10k_vif *arvif)
 
 	ret = ath10k_wmi_vdev_stop(ar, arvif->vdev_id);
 	if (ret) {
-		ath10k_warn("failed to stop WMI vdev %i: %d\n",
+		ath10k_warn(ar, "failed to stop WMI vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_vdev_setup_sync(ar);
 	if (ret) {
-		ath10k_warn("failed to syncronise setup for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to syncronise setup for vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -892,8 +895,9 @@ static int ath10k_vdev_stop(struct ath10k_vif *arvif)
 }
 
 static void ath10k_control_beaconing(struct ath10k_vif *arvif,
-				struct ieee80211_bss_conf *info)
+				     struct ieee80211_bss_conf *info)
 {
+	struct ath10k *ar = arvif->ar;
 	int ret = 0;
 
 	lockdep_assert_held(&arvif->ar->conf_mutex);
@@ -926,12 +930,12 @@ static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 		return;
 
 	arvif->aid = 0;
-	memcpy(arvif->bssid, info->bssid, ETH_ALEN);
+	ether_addr_copy(arvif->bssid, info->bssid);
 
 	ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, arvif->aid,
 				 arvif->bssid);
 	if (ret) {
-		ath10k_warn("failed to bring up vdev %d: %i\n",
+		ath10k_warn(ar, "failed to bring up vdev %d: %i\n",
 			    arvif->vdev_id, ret);
 		ath10k_vdev_stop(arvif);
 		return;
@@ -940,13 +944,14 @@ static void ath10k_control_beaconing(struct ath10k_vif *arvif,
 	arvif->is_started = true;
 	arvif->is_up = true;
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d up\n", arvif->vdev_id);
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d up\n", arvif->vdev_id);
 }
 
 static void ath10k_control_ibss(struct ath10k_vif *arvif,
 				struct ieee80211_bss_conf *info,
 				const u8 self_peer[ETH_ALEN])
 {
+	struct ath10k *ar = arvif->ar;
 	u32 vdev_param;
 	int ret = 0;
 
@@ -955,7 +960,7 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 	if (!info->ibss_joined) {
 		ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id, self_peer);
 		if (ret)
-			ath10k_warn("failed to delete IBSS self peer %pM for vdev %d: %d\n",
+			ath10k_warn(ar, "failed to delete IBSS self peer %pM for vdev %d: %d\n",
 				    self_peer, arvif->vdev_id, ret);
 
 		if (is_zero_ether_addr(arvif->bssid))
@@ -964,7 +969,7 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 		ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id,
 					 arvif->bssid);
 		if (ret) {
-			ath10k_warn("failed to delete IBSS BSSID peer %pM for vdev %d: %d\n",
+			ath10k_warn(ar, "failed to delete IBSS BSSID peer %pM for vdev %d: %d\n",
 				    arvif->bssid, arvif->vdev_id, ret);
 			return;
 		}
@@ -976,7 +981,7 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 
 	ret = ath10k_peer_create(arvif->ar, arvif->vdev_id, self_peer);
 	if (ret) {
-		ath10k_warn("failed to create IBSS self peer %pM for vdev %d: %d\n",
+		ath10k_warn(ar, "failed to create IBSS self peer %pM for vdev %d: %d\n",
 			    self_peer, arvif->vdev_id, ret);
 		return;
 	}
@@ -985,7 +990,7 @@ static void ath10k_control_ibss(struct ath10k_vif *arvif,
 	ret = ath10k_wmi_vdev_set_param(arvif->ar, arvif->vdev_id, vdev_param,
 					ATH10K_DEFAULT_ATIM);
 	if (ret)
-		ath10k_warn("failed to set IBSS ATIM for vdev %d: %d\n",
+		ath10k_warn(ar, "failed to set IBSS ATIM for vdev %d: %d\n",
 			    arvif->vdev_id, ret);
 }
 
@@ -1012,7 +1017,7 @@ static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id, param,
 						  conf->dynamic_ps_timeout);
 		if (ret) {
-			ath10k_warn("failed to set inactivity time for vdev %d: %i\n",
+			ath10k_warn(ar, "failed to set inactivity time for vdev %d: %i\n",
 				    arvif->vdev_id, ret);
 			return ret;
 		}
@@ -1020,12 +1025,12 @@ static int ath10k_mac_vif_setup_ps(struct ath10k_vif *arvif)
 		psmode = WMI_STA_PS_MODE_DISABLED;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d psmode %s\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d psmode %s\n",
 		   arvif->vdev_id, psmode ? "enable" : "disable");
 
 	ret = ath10k_wmi_set_psmode(ar, arvif->vdev_id, psmode);
 	if (ret) {
-		ath10k_warn("failed to set PS Mode %d for vdev %d: %d\n",
+		ath10k_warn(ar, "failed to set PS Mode %d for vdev %d: %d\n",
 			    psmode, arvif->vdev_id, ret);
 		return ret;
 	}
@@ -1045,7 +1050,7 @@ static void ath10k_peer_assoc_h_basic(struct ath10k *ar,
 {
 	lockdep_assert_held(&ar->conf_mutex);
 
-	memcpy(arg->addr, sta->addr, ETH_ALEN);
+	ether_addr_copy(arg->addr, sta->addr);
 	arg->vdev_id = arvif->vdev_id;
 	arg->peer_aid = sta->aid;
 	arg->peer_flags |= WMI_PEER_AUTH;
@@ -1100,21 +1105,21 @@ static void ath10k_peer_assoc_h_crypto(struct ath10k *ar,
 		ies = rcu_dereference(bss->ies);
 
 		wpaie = cfg80211_find_vendor_ie(WLAN_OUI_MICROSOFT,
-				WLAN_OUI_TYPE_MICROSOFT_WPA,
-				ies->data,
-				ies->len);
+						WLAN_OUI_TYPE_MICROSOFT_WPA,
+						ies->data,
+						ies->len);
 		rcu_read_unlock();
 		cfg80211_put_bss(ar->hw->wiphy, bss);
 	}
 
 	/* FIXME: base on RSN IE/WPA IE is a correct idea? */
 	if (rsnie || wpaie) {
-		ath10k_dbg(ATH10K_DBG_WMI, "%s: rsn ie found\n", __func__);
+		ath10k_dbg(ar, ATH10K_DBG_WMI, "%s: rsn ie found\n", __func__);
 		arg->peer_flags |= WMI_PEER_NEED_PTK_4_WAY;
 	}
 
 	if (wpaie) {
-		ath10k_dbg(ATH10K_DBG_WMI, "%s: wpa ie found\n", __func__);
+		ath10k_dbg(ar, ATH10K_DBG_WMI, "%s: wpa ie found\n", __func__);
 		arg->peer_flags |= WMI_PEER_NEED_GTK_2_WAY;
 	}
 }
@@ -1152,6 +1157,7 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 {
 	const struct ieee80211_sta_ht_cap *ht_cap = &sta->ht_cap;
 	int i, n;
+	u32 stbc;
 
 	lockdep_assert_held(&ar->conf_mutex);
 
@@ -1188,7 +1194,6 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 	}
 
 	if (ht_cap->cap & IEEE80211_HT_CAP_RX_STBC) {
-		u32 stbc;
 		stbc = ht_cap->cap & IEEE80211_HT_CAP_RX_STBC;
 		stbc = stbc >> IEEE80211_HT_CAP_RX_STBC_SHIFT;
 		stbc = stbc << WMI_RC_RX_STBC_FLAG_S;
@@ -1223,7 +1228,7 @@ static void ath10k_peer_assoc_h_ht(struct ath10k *ar,
 		arg->peer_num_spatial_streams = sta->rx_nss;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac ht peer %pM mcs cnt %d nss %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac ht peer %pM mcs cnt %d nss %d\n",
 		   arg->addr,
 		   arg->peer_ht_rates.num_rates,
 		   arg->peer_num_spatial_streams);
@@ -1240,7 +1245,7 @@ static int ath10k_peer_assoc_qos_ap(struct ath10k *ar,
 	lockdep_assert_held(&ar->conf_mutex);
 
 	if (sta->wme && sta->uapsd_queues) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac uapsd_queues 0x%x max_sp %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac uapsd_queues 0x%x max_sp %d\n",
 			   sta->uapsd_queues, sta->max_sp);
 
 		if (sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_VO)
@@ -1256,7 +1261,6 @@ static int ath10k_peer_assoc_qos_ap(struct ath10k *ar,
 			uapsd |= WMI_AP_PS_UAPSD_AC0_DELIVERY_EN |
 				 WMI_AP_PS_UAPSD_AC0_TRIGGER_EN;
 
-
 		if (sta->max_sp < MAX_WMI_AP_PS_PEER_PARAM_MAX_SP)
 			max_sp = sta->max_sp;
 
@@ -1265,7 +1269,7 @@ static int ath10k_peer_assoc_qos_ap(struct ath10k *ar,
 						 WMI_AP_PS_PEER_PARAM_UAPSD,
 						 uapsd);
 		if (ret) {
-			ath10k_warn("failed to set ap ps peer param uapsd for vdev %i: %d\n",
+			ath10k_warn(ar, "failed to set ap ps peer param uapsd for vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 			return ret;
 		}
@@ -1275,7 +1279,7 @@ static int ath10k_peer_assoc_qos_ap(struct ath10k *ar,
 						 WMI_AP_PS_PEER_PARAM_MAX_SP,
 						 max_sp);
 		if (ret) {
-			ath10k_warn("failed to set ap ps peer param max sp for vdev %i: %d\n",
+			ath10k_warn(ar, "failed to set ap ps peer param max sp for vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 			return ret;
 		}
@@ -1285,9 +1289,10 @@ static int ath10k_peer_assoc_qos_ap(struct ath10k *ar,
 		   sta->listen_interval - mac80211 patch required.
 		   Currently use 10 seconds */
 		ret = ath10k_wmi_set_ap_ps_param(ar, arvif->vdev_id, sta->addr,
-					WMI_AP_PS_PEER_PARAM_AGEOUT_TIME, 10);
+						 WMI_AP_PS_PEER_PARAM_AGEOUT_TIME,
+						 10);
 		if (ret) {
-			ath10k_warn("failed to set ap ps peer param ageout time for vdev %i: %d\n",
+			ath10k_warn(ar, "failed to set ap ps peer param ageout time for vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 			return ret;
 		}
@@ -1309,7 +1314,6 @@ static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
 	arg->peer_flags |= WMI_PEER_VHT;
 	arg->peer_vht_caps = vht_cap->cap;
 
-
 	ampdu_factor = (vht_cap->cap &
 			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK) >>
 		       IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_SHIFT;
@@ -1334,7 +1338,7 @@ static void ath10k_peer_assoc_h_vht(struct ath10k *ar,
 	arg->peer_vht_rates.tx_mcs_set =
 		__le16_to_cpu(vht_cap->vht_mcs.tx_mcs_map);
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vht peer %pM max_mpdu %d flags 0x%x\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vht peer %pM max_mpdu %d flags 0x%x\n",
 		   sta->addr, arg->peer_max_mpdu, arg->peer_flags);
 }
 
@@ -1407,7 +1411,7 @@ static void ath10k_peer_assoc_h_phymode(struct ath10k *ar,
 		break;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac peer %pM phymode %s\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac peer %pM phymode %s\n",
 		   sta->addr, ath10k_wmi_phymode_str(phymode));
 
 	arg->peer_phymode = phymode;
@@ -1480,7 +1484,7 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 
 	ap_sta = ieee80211_find_sta(vif, bss_conf->bssid);
 	if (!ap_sta) {
-		ath10k_warn("failed to find station entry for bss %pM vdev %i\n",
+		ath10k_warn(ar, "failed to find station entry for bss %pM vdev %i\n",
 			    bss_conf->bssid, arvif->vdev_id);
 		rcu_read_unlock();
 		return;
@@ -1493,7 +1497,7 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 	ret = ath10k_peer_assoc_prepare(ar, arvif, ap_sta,
 					bss_conf, &peer_arg);
 	if (ret) {
-		ath10k_warn("failed to prepare peer assoc for %pM vdev %i: %d\n",
+		ath10k_warn(ar, "failed to prepare peer assoc for %pM vdev %i: %d\n",
 			    bss_conf->bssid, arvif->vdev_id, ret);
 		rcu_read_unlock();
 		return;
@@ -1503,28 +1507,28 @@ static void ath10k_bss_assoc(struct ieee80211_hw *hw,
 
 	ret = ath10k_wmi_peer_assoc(ar, &peer_arg);
 	if (ret) {
-		ath10k_warn("failed to run peer assoc for %pM vdev %i: %d\n",
+		ath10k_warn(ar, "failed to run peer assoc for %pM vdev %i: %d\n",
 			    bss_conf->bssid, arvif->vdev_id, ret);
 		return;
 	}
 
 	ret = ath10k_setup_peer_smps(ar, arvif, bss_conf->bssid, &ht_cap);
 	if (ret) {
-		ath10k_warn("failed to setup peer SMPS for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to setup peer SMPS for vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC,
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
 		   "mac vdev %d up (associated) bssid %pM aid %d\n",
 		   arvif->vdev_id, bss_conf->bssid, bss_conf->aid);
 
 	arvif->aid = bss_conf->aid;
-	memcpy(arvif->bssid, bss_conf->bssid, ETH_ALEN);
+	ether_addr_copy(arvif->bssid, bss_conf->bssid);
 
 	ret = ath10k_wmi_vdev_up(ar, arvif->vdev_id, arvif->aid, arvif->bssid);
 	if (ret) {
-		ath10k_warn("failed to set vdev %d up: %d\n",
+		ath10k_warn(ar, "failed to set vdev %d up: %d\n",
 			    arvif->vdev_id, ret);
 		return;
 	}
@@ -1550,7 +1554,7 @@ static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
 	 * No idea why this happens, even though VDEV-DOWN is supposed
 	 * to be analogous to link down, so just stop the VDEV.
 	 */
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d stop (disassociated\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d stop (disassociated\n",
 		   arvif->vdev_id);
 
 	/* FIXME: check return value */
@@ -1563,7 +1567,7 @@ static void ath10k_bss_disassoc(struct ieee80211_hw *hw,
 	 * interfaces as it expects there is no rx when no interface is
 	 * running.
 	 */
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d down\n", arvif->vdev_id);
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d down\n", arvif->vdev_id);
 
 	/* FIXME: why don't we print error if wmi call fails? */
 	ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
@@ -1584,7 +1588,7 @@ static int ath10k_station_assoc(struct ath10k *ar, struct ath10k_vif *arvif,
 
 	ret = ath10k_peer_assoc_prepare(ar, arvif, sta, NULL, &peer_arg);
 	if (ret) {
-		ath10k_warn("failed to prepare WMI peer assoc for %pM vdev %i: %i\n",
+		ath10k_warn(ar, "failed to prepare WMI peer assoc for %pM vdev %i: %i\n",
 			    sta->addr, arvif->vdev_id, ret);
 		return ret;
 	}
@@ -1592,23 +1596,23 @@ static int ath10k_station_assoc(struct ath10k *ar, struct ath10k_vif *arvif,
 	peer_arg.peer_reassoc = reassoc;
 	ret = ath10k_wmi_peer_assoc(ar, &peer_arg);
 	if (ret) {
-		ath10k_warn("failed to run peer assoc for STA %pM vdev %i: %d\n",
+		ath10k_warn(ar, "failed to run peer assoc for STA %pM vdev %i: %d\n",
 			    sta->addr, arvif->vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_setup_peer_smps(ar, arvif, sta->addr, &sta->ht_cap);
 	if (ret) {
-		ath10k_warn("failed to setup peer SMPS for vdev %d: %d\n",
+		ath10k_warn(ar, "failed to setup peer SMPS for vdev %d: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
 
-	if (!sta->wme) {
+	if (!sta->wme && !reassoc) {
 		arvif->num_legacy_stations++;
 		ret  = ath10k_recalc_rtscts_prot(arvif);
 		if (ret) {
-			ath10k_warn("failed to recalculate rts/cts prot for vdev %d: %d\n",
+			ath10k_warn(ar, "failed to recalculate rts/cts prot for vdev %d: %d\n",
 				    arvif->vdev_id, ret);
 			return ret;
 		}
@@ -1616,14 +1620,14 @@ static int ath10k_station_assoc(struct ath10k *ar, struct ath10k_vif *arvif,
 
 	ret = ath10k_install_peer_wep_keys(arvif, sta->addr);
 	if (ret) {
-		ath10k_warn("failed to install peer wep keys for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to install peer wep keys for vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
 
 	ret = ath10k_peer_assoc_qos_ap(ar, arvif, sta);
 	if (ret) {
-		ath10k_warn("failed to set qos params for STA %pM for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to set qos params for STA %pM for vdev %i: %d\n",
 			    sta->addr, arvif->vdev_id, ret);
 		return ret;
 	}
@@ -1642,7 +1646,7 @@ static int ath10k_station_disassoc(struct ath10k *ar, struct ath10k_vif *arvif,
 		arvif->num_legacy_stations--;
 		ret = ath10k_recalc_rtscts_prot(arvif);
 		if (ret) {
-			ath10k_warn("failed to recalculate rts/cts prot for vdev %d: %d\n",
+			ath10k_warn(ar, "failed to recalculate rts/cts prot for vdev %d: %d\n",
 				    arvif->vdev_id, ret);
 			return ret;
 		}
@@ -1650,7 +1654,7 @@ static int ath10k_station_disassoc(struct ath10k *ar, struct ath10k_vif *arvif,
 
 	ret = ath10k_clear_peer_keys(arvif, sta->addr);
 	if (ret) {
-		ath10k_warn("failed to clear all peer wep keys for vdev %i: %d\n",
+		ath10k_warn(ar, "failed to clear all peer wep keys for vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		return ret;
 	}
@@ -1742,7 +1746,7 @@ static int ath10k_update_channel_list(struct ath10k *ar)
 			if (WARN_ON_ONCE(ch->mode == MODE_UNKNOWN))
 				continue;
 
-			ath10k_dbg(ATH10K_DBG_WMI,
+			ath10k_dbg(ar, ATH10K_DBG_WMI,
 				   "mac channel [%zd/%d] freq %d maxpower %d regpower %d antenna %d mode %d\n",
 				    ch - arg.channels, arg.n_channels,
 				   ch->freq, ch->max_power, ch->max_reg_power,
@@ -1785,7 +1789,7 @@ static void ath10k_regd_update(struct ath10k *ar)
 
 	ret = ath10k_update_channel_list(ar);
 	if (ret)
-		ath10k_warn("failed to update channel list: %d\n", ret);
+		ath10k_warn(ar, "failed to update channel list: %d\n", ret);
 
 	regpair = ar->ath_common.regulatory.regpair;
 
@@ -1806,7 +1810,7 @@ static void ath10k_regd_update(struct ath10k *ar)
 					    regpair->reg_5ghz_ctl,
 					    wmi_dfs_reg);
 	if (ret)
-		ath10k_warn("failed to set pdev regdomain: %d\n", ret);
+		ath10k_warn(ar, "failed to set pdev regdomain: %d\n", ret);
 }
 
 static void ath10k_reg_notifier(struct wiphy *wiphy,
@@ -1819,12 +1823,12 @@ static void ath10k_reg_notifier(struct wiphy *wiphy,
 	ath_reg_notifier_apply(wiphy, request, &ar->ath_common.regulatory);
 
 	if (config_enabled(CONFIG_ATH10K_DFS_CERTIFIED) && ar->dfs_detector) {
-		ath10k_dbg(ATH10K_DBG_REGULATORY, "dfs region 0x%x\n",
+		ath10k_dbg(ar, ATH10K_DBG_REGULATORY, "dfs region 0x%x\n",
 			   request->dfs_region);
 		result = ar->dfs_detector->set_dfs_domain(ar->dfs_detector,
 							  request->dfs_region);
 		if (!result)
-			ath10k_warn("DFS region 0x%X not supported, will trigger radar for every pulse\n",
+			ath10k_warn(ar, "DFS region 0x%X not supported, will trigger radar for every pulse\n",
 				    request->dfs_region);
 	}
 
@@ -1852,16 +1856,15 @@ static u8 ath10k_tx_h_get_tid(struct ieee80211_hdr *hdr)
 	return ieee80211_get_qos_ctl(hdr)[0] & IEEE80211_QOS_CTL_TID_MASK;
 }
 
-static u8 ath10k_tx_h_get_vdev_id(struct ath10k *ar,
-				  struct ieee80211_tx_info *info)
+static u8 ath10k_tx_h_get_vdev_id(struct ath10k *ar, struct ieee80211_vif *vif)
 {
-	if (info->control.vif)
-		return ath10k_vif_to_arvif(info->control.vif)->vdev_id;
+	if (vif)
+		return ath10k_vif_to_arvif(vif)->vdev_id;
 
 	if (ar->monitor_started)
 		return ar->monitor_vdev_id;
 
-	ath10k_warn("failed to resolve vdev id\n");
+	ath10k_warn(ar, "failed to resolve vdev id\n");
 	return 0;
 }
 
@@ -1897,6 +1900,7 @@ static void ath10k_tx_wep_key_work(struct work_struct *work)
 {
 	struct ath10k_vif *arvif = container_of(work, struct ath10k_vif,
 						wep_key_work);
+	struct ath10k *ar = arvif->ar;
 	int ret, keyidx = arvif->def_wep_key_newidx;
 
 	mutex_lock(&arvif->ar->conf_mutex);
@@ -1907,7 +1911,7 @@ static void ath10k_tx_wep_key_work(struct work_struct *work)
 	if (arvif->def_wep_key_idx == keyidx)
 		goto unlock;
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d set keyidx %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d set keyidx %d\n",
 		   arvif->vdev_id, keyidx);
 
 	ret = ath10k_wmi_vdev_set_param(arvif->ar,
@@ -1915,7 +1919,7 @@ static void ath10k_tx_wep_key_work(struct work_struct *work)
 					arvif->ar->wmi.vdev_param->def_keyid,
 					keyidx);
 	if (ret) {
-		ath10k_warn("failed to update wep key index for vdev %d: %d\n",
+		ath10k_warn(ar, "failed to update wep key index for vdev %d: %d\n",
 			    arvif->vdev_id,
 			    ret);
 		goto unlock;
@@ -1995,7 +1999,7 @@ static void ath10k_tx_htt(struct ath10k *ar, struct sk_buff *skb)
 			     ar->fw_features)) {
 			if (skb_queue_len(&ar->wmi_mgmt_tx_queue) >=
 			    ATH10K_MAX_NUM_MGMT_PENDING) {
-				ath10k_warn("reached WMI management tranmist queue limit\n");
+				ath10k_warn(ar, "reached WMI management transmit queue limit\n");
 				ret = -EBUSY;
 				goto exit;
 			}
@@ -2019,7 +2023,8 @@ static void ath10k_tx_htt(struct ath10k *ar, struct sk_buff *skb)
 
 exit:
 	if (ret) {
-		ath10k_warn("failed to transmit packet, dropping: %d\n", ret);
+		ath10k_warn(ar, "failed to transmit packet, dropping: %d\n",
+			    ret);
 		ieee80211_free_txskb(ar->hw, skb);
 	}
 }
@@ -2061,7 +2066,7 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 
 		mutex_lock(&ar->conf_mutex);
 
-		ath10k_dbg(ATH10K_DBG_MAC, "mac offchannel skb %p\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac offchannel skb %p\n",
 			   skb);
 
 		hdr = (struct ieee80211_hdr *)skb->data;
@@ -2074,13 +2079,13 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 
 		if (peer)
 			/* FIXME: should this use ath10k_warn()? */
-			ath10k_dbg(ATH10K_DBG_MAC, "peer %pM on vdev %d already present\n",
+			ath10k_dbg(ar, ATH10K_DBG_MAC, "peer %pM on vdev %d already present\n",
 				   peer_addr, vdev_id);
 
 		if (!peer) {
 			ret = ath10k_peer_create(ar, vdev_id, peer_addr);
 			if (ret)
-				ath10k_warn("failed to create peer %pM on vdev %d: %d\n",
+				ath10k_warn(ar, "failed to create peer %pM on vdev %d: %d\n",
 					    peer_addr, vdev_id, ret);
 		}
 
@@ -2094,13 +2099,13 @@ void ath10k_offchan_tx_work(struct work_struct *work)
 		ret = wait_for_completion_timeout(&ar->offchan_tx_completed,
 						  3 * HZ);
 		if (ret <= 0)
-			ath10k_warn("timed out waiting for offchannel skb %p\n",
+			ath10k_warn(ar, "timed out waiting for offchannel skb %p\n",
 				    skb);
 
 		if (!peer) {
 			ret = ath10k_peer_delete(ar, vdev_id, peer_addr);
 			if (ret)
-				ath10k_warn("failed to delete peer %pM on vdev %d: %d\n",
+				ath10k_warn(ar, "failed to delete peer %pM on vdev %d: %d\n",
 					    peer_addr, vdev_id, ret);
 		}
 
@@ -2134,7 +2139,7 @@ void ath10k_mgmt_over_wmi_tx_work(struct work_struct *work)
 
 		ret = ath10k_wmi_mgmt_tx(ar, skb);
 		if (ret) {
-			ath10k_warn("failed to transmit management frame via WMI: %d\n",
+			ath10k_warn(ar, "failed to transmit management frame via WMI: %d\n",
 				    ret);
 			ieee80211_free_txskb(ar->hw, skb);
 		}
@@ -2145,34 +2150,40 @@ void ath10k_mgmt_over_wmi_tx_work(struct work_struct *work)
 /* Scanning */
 /************/
 
-/*
- * This gets called if we dont get a heart-beat during scan.
- * This may indicate the FW has hung and we need to abort the
- * scan manually to prevent cancel_hw_scan() from deadlocking
- */
-void ath10k_reset_scan(unsigned long ptr)
+void __ath10k_scan_finish(struct ath10k *ar)
 {
-	struct ath10k *ar = (struct ath10k *)ptr;
+	lockdep_assert_held(&ar->data_lock);
 
-	spin_lock_bh(&ar->data_lock);
-	if (!ar->scan.in_progress) {
-		spin_unlock_bh(&ar->data_lock);
-		return;
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+		break;
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
+		if (ar->scan.is_roc)
+			ieee80211_remain_on_channel_expired(ar->hw);
+		else
+			ieee80211_scan_completed(ar->hw,
+						 (ar->scan.state ==
+						  ATH10K_SCAN_ABORTING));
+		/* fall through */
+	case ATH10K_SCAN_STARTING:
+		ar->scan.state = ATH10K_SCAN_IDLE;
+		ar->scan_channel = NULL;
+		ath10k_offchan_tx_purge(ar);
+		cancel_delayed_work(&ar->scan.timeout);
+		complete_all(&ar->scan.completed);
+		break;
 	}
+}
 
-	ath10k_warn("scan timed out, firmware problem?\n");
-
-	if (ar->scan.is_roc)
-		ieee80211_remain_on_channel_expired(ar->hw);
-	else
-		ieee80211_scan_completed(ar->hw, 1 /* aborted */);
-
-	ar->scan.in_progress = false;
-	complete_all(&ar->scan.completed);
+void ath10k_scan_finish(struct ath10k *ar)
+{
+	spin_lock_bh(&ar->data_lock);
+	__ath10k_scan_finish(ar);
 	spin_unlock_bh(&ar->data_lock);
 }
 
-static int ath10k_abort_scan(struct ath10k *ar)
+static int ath10k_scan_stop(struct ath10k *ar)
 {
 	struct wmi_stop_scan_arg arg = {
 		.req_id = 1, /* FIXME */
@@ -2183,47 +2194,79 @@ static int ath10k_abort_scan(struct ath10k *ar)
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	del_timer_sync(&ar->scan.timeout);
+	ret = ath10k_wmi_stop_scan(ar, &arg);
+	if (ret) {
+		ath10k_warn(ar, "failed to stop wmi scan: %d\n", ret);
+		goto out;
+	}
 
-	spin_lock_bh(&ar->data_lock);
-	if (!ar->scan.in_progress) {
-		spin_unlock_bh(&ar->data_lock);
-		return 0;
+	ret = wait_for_completion_timeout(&ar->scan.completed, 3*HZ);
+	if (ret == 0) {
+		ath10k_warn(ar, "failed to receive scan abortion completion: timed out\n");
+		ret = -ETIMEDOUT;
+	} else if (ret > 0) {
+		ret = 0;
 	}
 
-	ar->scan.aborting = true;
+out:
+	/* Scan state should be updated upon scan completion but in case
+	 * firmware fails to deliver the event (for whatever reason) it is
+	 * desired to clean up scan state anyway. Firmware may have just
+	 * dropped the scan completion event delivery due to transport pipe
+	 * being overflown with data and/or it can recover on its own before
+	 * next scan request is submitted.
+	 */
+	spin_lock_bh(&ar->data_lock);
+	if (ar->scan.state != ATH10K_SCAN_IDLE)
+		__ath10k_scan_finish(ar);
 	spin_unlock_bh(&ar->data_lock);
 
-	ret = ath10k_wmi_stop_scan(ar, &arg);
-	if (ret) {
-		ath10k_warn("failed to stop wmi scan: %d\n", ret);
-		spin_lock_bh(&ar->data_lock);
-		ar->scan.in_progress = false;
-		ath10k_offchan_tx_purge(ar);
-		spin_unlock_bh(&ar->data_lock);
-		return -EIO;
-	}
+	return ret;
+}
 
-	ret = wait_for_completion_timeout(&ar->scan.completed, 3*HZ);
-	if (ret == 0)
-		ath10k_warn("timed out while waiting for scan to stop\n");
+static void ath10k_scan_abort(struct ath10k *ar)
+{
+	int ret;
 
-	/* scan completion may be done right after we timeout here, so let's
-	 * check the in_progress and tell mac80211 scan is completed. if we
-	 * don't do that and FW fails to send us scan completion indication
-	 * then userspace won't be able to scan anymore */
-	ret = 0;
+	lockdep_assert_held(&ar->conf_mutex);
 
 	spin_lock_bh(&ar->data_lock);
-	if (ar->scan.in_progress) {
-		ath10k_warn("failed to stop scan, it's still in progress\n");
-		ar->scan.in_progress = false;
-		ath10k_offchan_tx_purge(ar);
-		ret = -ETIMEDOUT;
+
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+		/* This can happen if timeout worker kicked in and called
+		 * abortion while scan completion was being processed.
+		 */
+		break;
+	case ATH10K_SCAN_STARTING:
+	case ATH10K_SCAN_ABORTING:
+		ath10k_warn(ar, "refusing scan abortion due to invalid scan state: %s (%d)\n",
+			    ath10k_scan_state_str(ar->scan.state),
+			    ar->scan.state);
+		break;
+	case ATH10K_SCAN_RUNNING:
+		ar->scan.state = ATH10K_SCAN_ABORTING;
+		spin_unlock_bh(&ar->data_lock);
+
+		ret = ath10k_scan_stop(ar);
+		if (ret)
+			ath10k_warn(ar, "failed to abort scan: %d\n", ret);
+
+		spin_lock_bh(&ar->data_lock);
+		break;
 	}
+
 	spin_unlock_bh(&ar->data_lock);
+}
 
-	return ret;
+void ath10k_scan_timeout_work(struct work_struct *work)
+{
+	struct ath10k *ar = container_of(work, struct ath10k,
+					 scan.timeout.work);
+
+	mutex_lock(&ar->conf_mutex);
+	ath10k_scan_abort(ar);
+	mutex_unlock(&ar->conf_mutex);
 }
 
 static int ath10k_start_scan(struct ath10k *ar,
@@ -2239,17 +2282,16 @@ static int ath10k_start_scan(struct ath10k *ar,
 
 	ret = wait_for_completion_timeout(&ar->scan.started, 1*HZ);
 	if (ret == 0) {
-		ath10k_abort_scan(ar);
-		return ret;
+		ret = ath10k_scan_stop(ar);
+		if (ret)
+			ath10k_warn(ar, "failed to stop scan: %d\n", ret);
+
+		return -ETIMEDOUT;
 	}
 
-	/* the scan can complete earlier, before we even
-	 * start the timer. in that case the timer handler
-	 * checks ar->scan.in_progress and bails out if its
-	 * false. Add a 200ms margin to account event/command
-	 * processing. */
-	mod_timer(&ar->scan.timeout, jiffies +
-		  msecs_to_jiffies(arg->max_scan_time+200));
+	/* Add a 200ms margin to account for event/command processing */
+	ieee80211_queue_delayed_work(ar->hw, &ar->scan.timeout,
+				     msecs_to_jiffies(arg->max_scan_time+200));
 	return 0;
 }
 
@@ -2269,11 +2311,11 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 
 	/* We should disable CCK RATE due to P2P */
 	if (info->flags & IEEE80211_TX_CTL_NO_CCK_RATE)
-		ath10k_dbg(ATH10K_DBG_MAC, "IEEE80211_TX_CTL_NO_CCK_RATE\n");
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "IEEE80211_TX_CTL_NO_CCK_RATE\n");
 
 	ATH10K_SKB_CB(skb)->htt.is_offchan = false;
 	ATH10K_SKB_CB(skb)->htt.tid = ath10k_tx_h_get_tid(hdr);
-	ATH10K_SKB_CB(skb)->vdev_id = ath10k_tx_h_get_vdev_id(ar, info);
+	ATH10K_SKB_CB(skb)->vdev_id = ath10k_tx_h_get_vdev_id(ar, vif);
 
 	/* it makes no sense to process injected frames like that */
 	if (vif && vif->type != NL80211_IFTYPE_MONITOR) {
@@ -2289,7 +2331,8 @@ static void ath10k_tx(struct ieee80211_hw *hw,
 		ATH10K_SKB_CB(skb)->vdev_id = ar->scan.vdev_id;
 		spin_unlock_bh(&ar->data_lock);
 
-		ath10k_dbg(ATH10K_DBG_MAC, "queued offchannel skb %p\n", skb);
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "queued offchannel skb %p\n",
+			   skb);
 
 		skb_queue_tail(&ar->offchan_tx_queue, skb);
 		ieee80211_queue_work(hw, &ar->offchan_tx_work);
@@ -2318,15 +2361,16 @@ void ath10k_halt(struct ath10k *ar)
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	if (ath10k_monitor_is_enabled(ar)) {
-		clear_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
-		ar->promisc = false;
-		ar->monitor = false;
+	clear_bit(ATH10K_CAC_RUNNING, &ar->dev_flags);
+	ar->filter_flags = 0;
+	ar->monitor = false;
+
+	if (ar->monitor_started)
 		ath10k_monitor_stop(ar);
-	}
 
-	del_timer_sync(&ar->scan.timeout);
-	ath10k_reset_scan((unsigned long)ar);
+	ar->monitor_started = false;
+
+	ath10k_scan_finish(ar);
 	ath10k_peer_cleanup_all(ar);
 	ath10k_core_stop(ar);
 	ath10k_hif_power_down(ar);
@@ -2380,7 +2424,7 @@ static int __ath10k_set_antenna(struct ath10k *ar, u32 tx_ant, u32 rx_ant)
 	ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->tx_chain_mask,
 					tx_ant);
 	if (ret) {
-		ath10k_warn("failed to set tx-chainmask: %d, req 0x%x\n",
+		ath10k_warn(ar, "failed to set tx-chainmask: %d, req 0x%x\n",
 			    ret, tx_ant);
 		return ret;
 	}
@@ -2388,7 +2432,7 @@ static int __ath10k_set_antenna(struct ath10k *ar, u32 tx_ant, u32 rx_ant)
 	ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->rx_chain_mask,
 					rx_ant);
 	if (ret) {
-		ath10k_warn("failed to set rx-chainmask: %d, req 0x%x\n",
+		ath10k_warn(ar, "failed to set rx-chainmask: %d, req 0x%x\n",
 			    ret, rx_ant);
 		return ret;
 	}
@@ -2435,29 +2479,32 @@ static int ath10k_start(struct ieee80211_hw *hw)
 		WARN_ON(1);
 		ret = -EINVAL;
 		goto err;
+	case ATH10K_STATE_UTF:
+		ret = -EBUSY;
+		goto err;
 	}
 
 	ret = ath10k_hif_power_up(ar);
 	if (ret) {
-		ath10k_err("Could not init hif: %d\n", ret);
+		ath10k_err(ar, "Could not init hif: %d\n", ret);
 		goto err_off;
 	}
 
-	ret = ath10k_core_start(ar);
+	ret = ath10k_core_start(ar, ATH10K_FIRMWARE_MODE_NORMAL);
 	if (ret) {
-		ath10k_err("Could not init core: %d\n", ret);
+		ath10k_err(ar, "Could not init core: %d\n", ret);
 		goto err_power_down;
 	}
 
 	ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->pmf_qos, 1);
 	if (ret) {
-		ath10k_warn("failed to enable PMF QOS: %d\n", ret);
+		ath10k_warn(ar, "failed to enable PMF QOS: %d\n", ret);
 		goto err_core_stop;
 	}
 
 	ret = ath10k_wmi_pdev_set_param(ar, ar->wmi.pdev_param->dynamic_bw, 1);
 	if (ret) {
-		ath10k_warn("failed to enable dynamic BW: %d\n", ret);
+		ath10k_warn(ar, "failed to enable dynamic BW: %d\n", ret);
 		goto err_core_stop;
 	}
 
@@ -2477,7 +2524,7 @@ static int ath10k_start(struct ieee80211_hw *hw)
 	ret = ath10k_wmi_pdev_set_param(ar,
 					ar->wmi.pdev_param->arp_ac_override, 0);
 	if (ret) {
-		ath10k_warn("failed to set arp ac override parameter: %d\n",
+		ath10k_warn(ar, "failed to set arp ac override parameter: %d\n",
 			    ret);
 		goto err_core_stop;
 	}
@@ -2485,6 +2532,8 @@ static int ath10k_start(struct ieee80211_hw *hw)
 	ar->num_started_vdevs = 0;
 	ath10k_regd_update(ar);
 
+	ath10k_spectral_start(ar);
+
 	mutex_unlock(&ar->conf_mutex);
 	return 0;
 
@@ -2515,6 +2564,7 @@ static void ath10k_stop(struct ieee80211_hw *hw)
 	}
 	mutex_unlock(&ar->conf_mutex);
 
+	cancel_delayed_work_sync(&ar->scan.timeout);
 	cancel_work_sync(&ar->restart_work);
 }
 
@@ -2528,7 +2578,7 @@ static int ath10k_config_ps(struct ath10k *ar)
 	list_for_each_entry(arvif, &ar->arvifs, list) {
 		ret = ath10k_mac_vif_setup_ps(arvif);
 		if (ret) {
-			ath10k_warn("failed to setup powersave: %d\n", ret);
+			ath10k_warn(ar, "failed to setup powersave: %d\n", ret);
 			break;
 		}
 	}
@@ -2566,7 +2616,7 @@ static void ath10k_config_chan(struct ath10k *ar)
 
 	lockdep_assert_held(&ar->conf_mutex);
 
-	ath10k_dbg(ATH10K_DBG_MAC,
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
 		   "mac config channel to %dMHz (cf1 %dMHz cf2 %dMHz width %s)\n",
 		   ar->chandef.chan->center_freq,
 		   ar->chandef.center_freq1,
@@ -2576,24 +2626,27 @@ static void ath10k_config_chan(struct ath10k *ar)
 	/* First stop monitor interface. Some FW versions crash if there's a
 	 * lone monitor interface. */
 	if (ar->monitor_started)
-		ath10k_monitor_vdev_stop(ar);
+		ath10k_monitor_stop(ar);
 
 	list_for_each_entry(arvif, &ar->arvifs, list) {
 		if (!arvif->is_started)
 			continue;
 
+		if (!arvif->is_up)
+			continue;
+
 		if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
 			continue;
 
-		ret = ath10k_vdev_stop(arvif);
+		ret = ath10k_wmi_vdev_down(ar, arvif->vdev_id);
 		if (ret) {
-			ath10k_warn("failed to stop vdev %d: %d\n",
+			ath10k_warn(ar, "failed to down vdev %d: %d\n",
 				    arvif->vdev_id, ret);
 			continue;
 		}
 	}
 
-	/* all vdevs are now stopped - now attempt to restart them */
+	/* all vdevs are downed now - attempt to restart and re-up them */
 
 	list_for_each_entry(arvif, &ar->arvifs, list) {
 		if (!arvif->is_started)
@@ -2602,9 +2655,9 @@ static void ath10k_config_chan(struct ath10k *ar)
 		if (arvif->vdev_type == WMI_VDEV_TYPE_MONITOR)
 			continue;
 
-		ret = ath10k_vdev_start(arvif);
+		ret = ath10k_vdev_restart(arvif);
 		if (ret) {
-			ath10k_warn("failed to start vdev %d: %d\n",
+			ath10k_warn(ar, "failed to restart vdev %d: %d\n",
 				    arvif->vdev_id, ret);
 			continue;
 		}
@@ -2615,14 +2668,13 @@ static void ath10k_config_chan(struct ath10k *ar)
 		ret = ath10k_wmi_vdev_up(arvif->ar, arvif->vdev_id, arvif->aid,
 					 arvif->bssid);
 		if (ret) {
-			ath10k_warn("failed to bring vdev up %d: %d\n",
+			ath10k_warn(ar, "failed to bring vdev up %d: %d\n",
 				    arvif->vdev_id, ret);
 			continue;
 		}
 	}
 
-	if (ath10k_monitor_is_enabled(ar))
-		ath10k_monitor_vdev_start(ar, ar->monitor_vdev_id);
+	ath10k_monitor_recalc(ar);
 }
 
 static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
@@ -2635,7 +2687,7 @@ static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
 	mutex_lock(&ar->conf_mutex);
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		ath10k_dbg(ATH10K_DBG_MAC,
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "mac config channel %dMHz flags 0x%x radar %d\n",
 			   conf->chandef.chan->center_freq,
 			   conf->chandef.chan->flags,
@@ -2655,21 +2707,21 @@ static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_POWER) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac config power %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac config power %d\n",
 			   hw->conf.power_level);
 
 		param = ar->wmi.pdev_param->txpower_limit2g;
 		ret = ath10k_wmi_pdev_set_param(ar, param,
 						hw->conf.power_level * 2);
 		if (ret)
-			ath10k_warn("failed to set 2g txpower %d: %d\n",
+			ath10k_warn(ar, "failed to set 2g txpower %d: %d\n",
 				    hw->conf.power_level, ret);
 
 		param = ar->wmi.pdev_param->txpower_limit5g;
 		ret = ath10k_wmi_pdev_set_param(ar, param,
 						hw->conf.power_level * 2);
 		if (ret)
-			ath10k_warn("failed to set 5g txpower %d: %d\n",
+			ath10k_warn(ar, "failed to set 5g txpower %d: %d\n",
 				    hw->conf.power_level, ret);
 	}
 
@@ -2677,19 +2729,10 @@ static int ath10k_config(struct ieee80211_hw *hw, u32 changed)
 		ath10k_config_ps(ar);
 
 	if (changed & IEEE80211_CONF_CHANGE_MONITOR) {
-		if (conf->flags & IEEE80211_CONF_MONITOR && !ar->monitor) {
-			ar->monitor = true;
-			ret = ath10k_monitor_start(ar);
-			if (ret) {
-				ath10k_warn("failed to start monitor (config): %d\n",
-					    ret);
-				ar->monitor = false;
-			}
-		} else if (!(conf->flags & IEEE80211_CONF_MONITOR) &&
-			   ar->monitor) {
-			ar->monitor = false;
-			ath10k_monitor_stop(ar);
-		}
+		ar->monitor = conf->flags & IEEE80211_CONF_MONITOR;
+		ret = ath10k_monitor_recalc(ar);
+		if (ret)
+			ath10k_warn(ar, "failed to recalc monitor: %d\n", ret);
 	}
 
 	mutex_unlock(&ar->conf_mutex);
@@ -2724,11 +2767,12 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	INIT_WORK(&arvif->wep_key_work, ath10k_tx_wep_key_work);
 	INIT_LIST_HEAD(&arvif->list);
 
-	bit = ffs(ar->free_vdev_map);
-	if (bit == 0) {
+	if (ar->free_vdev_map == 0) {
+		ath10k_warn(ar, "Free vdev map is empty, no more interfaces allowed.\n");
 		ret = -EBUSY;
 		goto err;
 	}
+	bit = ffs(ar->free_vdev_map);
 
 	arvif->vdev_id = bit - 1;
 	arvif->vdev_subtype = WMI_VDEV_SUBTYPE_NONE;
@@ -2760,25 +2804,25 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		break;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev create %d (add interface) type %d subtype %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev create %d (add interface) type %d subtype %d\n",
 		   arvif->vdev_id, arvif->vdev_type, arvif->vdev_subtype);
 
 	ret = ath10k_wmi_vdev_create(ar, arvif->vdev_id, arvif->vdev_type,
 				     arvif->vdev_subtype, vif->addr);
 	if (ret) {
-		ath10k_warn("failed to create WMI vdev %i: %d\n",
+		ath10k_warn(ar, "failed to create WMI vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 		goto err;
 	}
 
-	ar->free_vdev_map &= ~BIT(arvif->vdev_id);
+	ar->free_vdev_map &= ~(1 << arvif->vdev_id);
 	list_add(&arvif->list, &ar->arvifs);
 
 	vdev_param = ar->wmi.vdev_param->def_keyid;
 	ret = ath10k_wmi_vdev_set_param(ar, 0, vdev_param,
 					arvif->def_wep_key_idx);
 	if (ret) {
-		ath10k_warn("failed to set vdev %i default key id: %d\n",
+		ath10k_warn(ar, "failed to set vdev %i default key id: %d\n",
 			    arvif->vdev_id, ret);
 		goto err_vdev_delete;
 	}
@@ -2788,7 +2832,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 					ATH10K_HW_TXRX_NATIVE_WIFI);
 	/* 10.X firmware does not support this VDEV parameter. Do not warn */
 	if (ret && ret != -EOPNOTSUPP) {
-		ath10k_warn("failed to set vdev %i TX encapsulation: %d\n",
+		ath10k_warn(ar, "failed to set vdev %i TX encapsulation: %d\n",
 			    arvif->vdev_id, ret);
 		goto err_vdev_delete;
 	}
@@ -2796,14 +2840,14 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
 		ret = ath10k_peer_create(ar, arvif->vdev_id, vif->addr);
 		if (ret) {
-			ath10k_warn("failed to create vdev %i peer for AP: %d\n",
+			ath10k_warn(ar, "failed to create vdev %i peer for AP: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_vdev_delete;
 		}
 
 		ret = ath10k_mac_set_kickout(arvif);
 		if (ret) {
-			ath10k_warn("failed to set vdev %i kickout parameters: %d\n",
+			ath10k_warn(ar, "failed to set vdev %i kickout parameters: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
@@ -2815,7 +2859,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
 						  param, value);
 		if (ret) {
-			ath10k_warn("failed to set vdev %i RX wake policy: %d\n",
+			ath10k_warn(ar, "failed to set vdev %i RX wake policy: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
@@ -2825,7 +2869,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
 						  param, value);
 		if (ret) {
-			ath10k_warn("failed to set vdev %i TX wake thresh: %d\n",
+			ath10k_warn(ar, "failed to set vdev %i TX wake thresh: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
@@ -2835,7 +2879,7 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_set_sta_ps_param(ar, arvif->vdev_id,
 						  param, value);
 		if (ret) {
-			ath10k_warn("failed to set vdev %i PSPOLL count: %d\n",
+			ath10k_warn(ar, "failed to set vdev %i PSPOLL count: %d\n",
 				    arvif->vdev_id, ret);
 			goto err_peer_delete;
 		}
@@ -2843,14 +2887,14 @@ static int ath10k_add_interface(struct ieee80211_hw *hw,
 
 	ret = ath10k_mac_set_rts(arvif, ar->hw->wiphy->rts_threshold);
 	if (ret) {
-		ath10k_warn("failed to set rts threshold for vdev %d: %d\n",
+		ath10k_warn(ar, "failed to set rts threshold for vdev %d: %d\n",
 			    arvif->vdev_id, ret);
 		goto err_peer_delete;
 	}
 
 	ret = ath10k_mac_set_frag(arvif, ar->hw->wiphy->frag_threshold);
 	if (ret) {
-		ath10k_warn("failed to set frag threshold for vdev %d: %d\n",
+		ath10k_warn(ar, "failed to set frag threshold for vdev %d: %d\n",
 			    arvif->vdev_id, ret);
 		goto err_peer_delete;
 	}
@@ -2864,7 +2908,7 @@ err_peer_delete:
 
 err_vdev_delete:
 	ath10k_wmi_vdev_delete(ar, arvif->vdev_id);
-	ar->free_vdev_map &= ~BIT(arvif->vdev_id);
+	ar->free_vdev_map |= 1 << arvif->vdev_id;
 	list_del(&arvif->list);
 
 err:
@@ -2892,26 +2936,32 @@ static void ath10k_remove_interface(struct ieee80211_hw *hw,
 		dev_kfree_skb_any(arvif->beacon);
 		arvif->beacon = NULL;
 	}
+
 	spin_unlock_bh(&ar->data_lock);
 
-	ar->free_vdev_map |= 1 << (arvif->vdev_id);
+	ret = ath10k_spectral_vif_stop(arvif);
+	if (ret)
+		ath10k_warn(ar, "failed to stop spectral for vdev %i: %d\n",
+			    arvif->vdev_id, ret);
+
+	ar->free_vdev_map |= 1 << arvif->vdev_id;
 	list_del(&arvif->list);
 
 	if (arvif->vdev_type == WMI_VDEV_TYPE_AP) {
 		ret = ath10k_peer_delete(arvif->ar, arvif->vdev_id, vif->addr);
 		if (ret)
-			ath10k_warn("failed to remove peer for AP vdev %i: %d\n",
+			ath10k_warn(ar, "failed to remove peer for AP vdev %i: %d\n",
 				    arvif->vdev_id, ret);
 
 		kfree(arvif->u.ap.noa_data);
 	}
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %i delete (remove interface)\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %i delete (remove interface)\n",
 		   arvif->vdev_id);
 
 	ret = ath10k_wmi_vdev_delete(ar, arvif->vdev_id);
 	if (ret)
-		ath10k_warn("failed to delete WMI vdev %i: %d\n",
+		ath10k_warn(ar, "failed to delete WMI vdev %i: %d\n",
 			    arvif->vdev_id, ret);
 
 	ath10k_peer_cleanup(ar, arvif->vdev_id);
@@ -2946,18 +2996,9 @@ static void ath10k_configure_filter(struct ieee80211_hw *hw,
 	*total_flags &= SUPPORTED_FILTERS;
 	ar->filter_flags = *total_flags;
 
-	if (ar->filter_flags & FIF_PROMISC_IN_BSS && !ar->promisc) {
-		ar->promisc = true;
-		ret = ath10k_monitor_start(ar);
-		if (ret) {
-			ath10k_warn("failed to start monitor (promisc): %d\n",
-				    ret);
-			ar->promisc = false;
-		}
-	} else if (!(ar->filter_flags & FIF_PROMISC_IN_BSS) && ar->promisc) {
-		ar->promisc = false;
-		ath10k_monitor_stop(ar);
-	}
+	ret = ath10k_monitor_recalc(ar);
+	if (ret)
+		ath10k_warn(ar, "failed to recalc montior: %d\n", ret);
 
 	mutex_unlock(&ar->conf_mutex);
 }
@@ -2970,7 +3011,7 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 	struct ath10k *ar = hw->priv;
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	int ret = 0;
-	u32 vdev_param, pdev_param;
+	u32 vdev_param, pdev_param, slottime, preamble;
 
 	mutex_lock(&ar->conf_mutex);
 
@@ -2982,17 +3023,17 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		vdev_param = ar->wmi.vdev_param->beacon_interval;
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						arvif->beacon_interval);
-		ath10k_dbg(ATH10K_DBG_MAC,
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "mac vdev %d beacon_interval %d\n",
 			   arvif->vdev_id, arvif->beacon_interval);
 
 		if (ret)
-			ath10k_warn("failed to set beacon interval for vdev %d: %i\n",
+			ath10k_warn(ar, "failed to set beacon interval for vdev %d: %i\n",
 				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_BEACON) {
-		ath10k_dbg(ATH10K_DBG_MAC,
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "vdev %d set beacon tx mode to staggered\n",
 			   arvif->vdev_id);
 
@@ -3000,14 +3041,14 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_pdev_set_param(ar, pdev_param,
 						WMI_BEACON_STAGGERED_MODE);
 		if (ret)
-			ath10k_warn("failed to set beacon mode for vdev %d: %i\n",
+			ath10k_warn(ar, "failed to set beacon mode for vdev %d: %i\n",
 				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_BEACON_INFO) {
 		arvif->dtim_period = info->dtim_period;
 
-		ath10k_dbg(ATH10K_DBG_MAC,
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "mac vdev %d dtim_period %d\n",
 			   arvif->vdev_id, arvif->dtim_period);
 
@@ -3015,7 +3056,7 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						arvif->dtim_period);
 		if (ret)
-			ath10k_warn("failed to set dtim period for vdev %d: %i\n",
+			ath10k_warn(ar, "failed to set dtim period for vdev %d: %i\n",
 				    arvif->vdev_id, ret);
 	}
 
@@ -3034,14 +3075,14 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 	if (changed & BSS_CHANGED_BSSID &&
 	    vif->type != NL80211_IFTYPE_AP) {
 		if (!is_zero_ether_addr(info->bssid)) {
-			ath10k_dbg(ATH10K_DBG_MAC,
+			ath10k_dbg(ar, ATH10K_DBG_MAC,
 				   "mac vdev %d create peer %pM\n",
 				   arvif->vdev_id, info->bssid);
 
 			ret = ath10k_peer_create(ar, arvif->vdev_id,
 						 info->bssid);
 			if (ret)
-				ath10k_warn("failed to add peer %pM for vdev %d when changing bssid: %i\n",
+				ath10k_warn(ar, "failed to add peer %pM for vdev %d when changing bssid: %i\n",
 					    info->bssid, arvif->vdev_id, ret);
 
 			if (vif->type == NL80211_IFTYPE_STATION) {
@@ -3049,15 +3090,15 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 				 * this is never erased as we it for crypto key
 				 * clearing; this is FW requirement
 				 */
-				memcpy(arvif->bssid, info->bssid, ETH_ALEN);
+				ether_addr_copy(arvif->bssid, info->bssid);
 
-				ath10k_dbg(ATH10K_DBG_MAC,
+				ath10k_dbg(ar, ATH10K_DBG_MAC,
 					   "mac vdev %d start %pM\n",
 					   arvif->vdev_id, info->bssid);
 
 				ret = ath10k_vdev_start(arvif);
 				if (ret) {
-					ath10k_warn("failed to start vdev %i: %d\n",
+					ath10k_warn(ar, "failed to start vdev %i: %d\n",
 						    arvif->vdev_id, ret);
 					goto exit;
 				}
@@ -3081,42 +3122,40 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 
 	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
 		arvif->use_cts_prot = info->use_cts_prot;
-		ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d cts_prot %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d cts_prot %d\n",
 			   arvif->vdev_id, info->use_cts_prot);
 
 		ret = ath10k_recalc_rtscts_prot(arvif);
 		if (ret)
-			ath10k_warn("failed to recalculate rts/cts prot for vdev %d: %d\n",
+			ath10k_warn(ar, "failed to recalculate rts/cts prot for vdev %d: %d\n",
 				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
-		u32 slottime;
 		if (info->use_short_slot)
 			slottime = WMI_VDEV_SLOT_TIME_SHORT; /* 9us */
 
 		else
 			slottime = WMI_VDEV_SLOT_TIME_LONG; /* 20us */
 
-		ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d slot_time %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d slot_time %d\n",
 			   arvif->vdev_id, slottime);
 
 		vdev_param = ar->wmi.vdev_param->slot_time;
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						slottime);
 		if (ret)
-			ath10k_warn("failed to set erp slot for vdev %d: %i\n",
+			ath10k_warn(ar, "failed to set erp slot for vdev %d: %i\n",
 				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		u32 preamble;
 		if (info->use_short_preamble)
 			preamble = WMI_VDEV_PREAMBLE_SHORT;
 		else
 			preamble = WMI_VDEV_PREAMBLE_LONG;
 
-		ath10k_dbg(ATH10K_DBG_MAC,
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "mac vdev %d preamble %dn",
 			   arvif->vdev_id, preamble);
 
@@ -3124,13 +3163,21 @@ static void ath10k_bss_info_changed(struct ieee80211_hw *hw,
 		ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 						preamble);
 		if (ret)
-			ath10k_warn("failed to set preamble for vdev %d: %i\n",
+			ath10k_warn(ar, "failed to set preamble for vdev %d: %i\n",
 				    arvif->vdev_id, ret);
 	}
 
 	if (changed & BSS_CHANGED_ASSOC) {
-		if (info->assoc)
+		if (info->assoc) {
+			/* Workaround: Make sure monitor vdev is not running
+			 * when associating to prevent some firmware revisions
+			 * (e.g. 10.1 and 10.2) from crashing.
+			 */
+			if (ar->monitor_started)
+				ath10k_monitor_stop(ar);
 			ath10k_bss_assoc(hw, vif, info);
+			ath10k_monitor_recalc(ar);
+		}
 	}
 
 exit:
@@ -3151,20 +3198,26 @@ static int ath10k_hw_scan(struct ieee80211_hw *hw,
 	mutex_lock(&ar->conf_mutex);
 
 	spin_lock_bh(&ar->data_lock);
-	if (ar->scan.in_progress) {
-		spin_unlock_bh(&ar->data_lock);
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+		reinit_completion(&ar->scan.started);
+		reinit_completion(&ar->scan.completed);
+		ar->scan.state = ATH10K_SCAN_STARTING;
+		ar->scan.is_roc = false;
+		ar->scan.vdev_id = arvif->vdev_id;
+		ret = 0;
+		break;
+	case ATH10K_SCAN_STARTING:
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
 		ret = -EBUSY;
-		goto exit;
+		break;
 	}
-
-	reinit_completion(&ar->scan.started);
-	reinit_completion(&ar->scan.completed);
-	ar->scan.in_progress = true;
-	ar->scan.aborting = false;
-	ar->scan.is_roc = false;
-	ar->scan.vdev_id = arvif->vdev_id;
 	spin_unlock_bh(&ar->data_lock);
 
+	if (ret)
+		goto exit;
+
 	memset(&arg, 0, sizeof(arg));
 	ath10k_wmi_start_scan_init(ar, &arg);
 	arg.vdev_id = arvif->vdev_id;
@@ -3196,9 +3249,9 @@ static int ath10k_hw_scan(struct ieee80211_hw *hw,
 
 	ret = ath10k_start_scan(ar, &arg);
 	if (ret) {
-		ath10k_warn("failed to start hw scan: %d\n", ret);
+		ath10k_warn(ar, "failed to start hw scan: %d\n", ret);
 		spin_lock_bh(&ar->data_lock);
-		ar->scan.in_progress = false;
+		ar->scan.state = ATH10K_SCAN_IDLE;
 		spin_unlock_bh(&ar->data_lock);
 	}
 
@@ -3211,14 +3264,10 @@ static void ath10k_cancel_hw_scan(struct ieee80211_hw *hw,
 				  struct ieee80211_vif *vif)
 {
 	struct ath10k *ar = hw->priv;
-	int ret;
 
 	mutex_lock(&ar->conf_mutex);
-	ret = ath10k_abort_scan(ar);
-	if (ret) {
-		ath10k_warn("failed to abort scan: %d\n", ret);
-		ieee80211_scan_completed(hw, 1 /* aborted */);
-	}
+	cancel_delayed_work_sync(&ar->scan.timeout);
+	ath10k_scan_abort(ar);
 	mutex_unlock(&ar->conf_mutex);
 }
 
@@ -3256,7 +3305,7 @@ static void ath10k_set_key_h_def_keyidx(struct ath10k *ar,
 	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id, vdev_param,
 					key->keyidx);
 	if (ret)
-		ath10k_warn("failed to set vdev %i group key as default key: %d\n",
+		ath10k_warn(ar, "failed to set vdev %i group key as default key: %d\n",
 			    arvif->vdev_id, ret);
 }
 
@@ -3294,7 +3343,7 @@ static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	if (!peer) {
 		if (cmd == SET_KEY) {
-			ath10k_warn("failed to install key for non-existent peer %pM\n",
+			ath10k_warn(ar, "failed to install key for non-existent peer %pM\n",
 				    peer_addr);
 			ret = -EOPNOTSUPP;
 			goto exit;
@@ -3317,7 +3366,7 @@ static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	ret = ath10k_install_key(arvif, key, cmd, peer_addr);
 	if (ret) {
-		ath10k_warn("failed to install key for vdev %i peer %pM: %d\n",
+		ath10k_warn(ar, "failed to install key for vdev %i peer %pM: %d\n",
 			    arvif->vdev_id, peer_addr, ret);
 		goto exit;
 	}
@@ -3332,7 +3381,7 @@ static int ath10k_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		peer->keys[key->keyidx] = NULL;
 	else if (peer == NULL)
 		/* impossible unless FW goes crazy */
-		ath10k_warn("Peer %pM disappeared!\n", peer_addr);
+		ath10k_warn(ar, "Peer %pM disappeared!\n", peer_addr);
 	spin_unlock_bh(&ar->data_lock);
 
 exit:
@@ -3368,45 +3417,45 @@ static void ath10k_sta_rc_update_wk(struct work_struct *wk)
 	mutex_lock(&ar->conf_mutex);
 
 	if (changed & IEEE80211_RC_BW_CHANGED) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac update sta %pM peer bw %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac update sta %pM peer bw %d\n",
 			   sta->addr, bw);
 
 		err = ath10k_wmi_peer_set_param(ar, arvif->vdev_id, sta->addr,
 						WMI_PEER_CHAN_WIDTH, bw);
 		if (err)
-			ath10k_warn("failed to update STA %pM peer bw %d: %d\n",
+			ath10k_warn(ar, "failed to update STA %pM peer bw %d: %d\n",
 				    sta->addr, bw, err);
 	}
 
 	if (changed & IEEE80211_RC_NSS_CHANGED) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac update sta %pM nss %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac update sta %pM nss %d\n",
 			   sta->addr, nss);
 
 		err = ath10k_wmi_peer_set_param(ar, arvif->vdev_id, sta->addr,
 						WMI_PEER_NSS, nss);
 		if (err)
-			ath10k_warn("failed to update STA %pM nss %d: %d\n",
+			ath10k_warn(ar, "failed to update STA %pM nss %d: %d\n",
 				    sta->addr, nss, err);
 	}
 
 	if (changed & IEEE80211_RC_SMPS_CHANGED) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac update sta %pM smps %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac update sta %pM smps %d\n",
 			   sta->addr, smps);
 
 		err = ath10k_wmi_peer_set_param(ar, arvif->vdev_id, sta->addr,
 						WMI_PEER_SMPS_STATE, smps);
 		if (err)
-			ath10k_warn("failed to update STA %pM smps %d: %d\n",
+			ath10k_warn(ar, "failed to update STA %pM smps %d: %d\n",
 				    sta->addr, smps, err);
 	}
 
 	if (changed & IEEE80211_RC_SUPP_RATES_CHANGED) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac update sta %pM supp rates\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac update sta %pM supp rates\n",
 			   sta->addr);
 
 		err = ath10k_station_assoc(ar, arvif, sta, true);
 		if (err)
-			ath10k_warn("failed to reassociate station: %pM\n",
+			ath10k_warn(ar, "failed to reassociate station: %pM\n",
 				    sta->addr);
 	}
 
@@ -3451,31 +3500,31 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 			max_num_peers = TARGET_NUM_PEERS;
 
 		if (ar->num_peers >= max_num_peers) {
-			ath10k_warn("number of peers exceeded: peers number %d (max peers %d)\n",
+			ath10k_warn(ar, "number of peers exceeded: peers number %d (max peers %d)\n",
 				    ar->num_peers, max_num_peers);
 			ret = -ENOBUFS;
 			goto exit;
 		}
 
-		ath10k_dbg(ATH10K_DBG_MAC,
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "mac vdev %d peer create %pM (new sta) num_peers %d\n",
 			   arvif->vdev_id, sta->addr, ar->num_peers);
 
 		ret = ath10k_peer_create(ar, arvif->vdev_id, sta->addr);
 		if (ret)
-			ath10k_warn("failed to add peer %pM for vdev %d when adding a new sta: %i\n",
+			ath10k_warn(ar, "failed to add peer %pM for vdev %d when adding a new sta: %i\n",
 				    sta->addr, arvif->vdev_id, ret);
 	} else if ((old_state == IEEE80211_STA_NONE &&
 		    new_state == IEEE80211_STA_NOTEXIST)) {
 		/*
 		 * Existing station deletion.
 		 */
-		ath10k_dbg(ATH10K_DBG_MAC,
+		ath10k_dbg(ar, ATH10K_DBG_MAC,
 			   "mac vdev %d peer delete %pM (sta gone)\n",
 			   arvif->vdev_id, sta->addr);
 		ret = ath10k_peer_delete(ar, arvif->vdev_id, sta->addr);
 		if (ret)
-			ath10k_warn("failed to delete peer %pM for vdev %d: %i\n",
+			ath10k_warn(ar, "failed to delete peer %pM for vdev %d: %i\n",
 				    sta->addr, arvif->vdev_id, ret);
 
 		if (vif->type == NL80211_IFTYPE_STATION)
@@ -3487,12 +3536,12 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 		/*
 		 * New association.
 		 */
-		ath10k_dbg(ATH10K_DBG_MAC, "mac sta %pM associated\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac sta %pM associated\n",
 			   sta->addr);
 
 		ret = ath10k_station_assoc(ar, arvif, sta, false);
 		if (ret)
-			ath10k_warn("failed to associate station %pM for vdev %i: %i\n",
+			ath10k_warn(ar, "failed to associate station %pM for vdev %i: %i\n",
 				    sta->addr, arvif->vdev_id, ret);
 	} else if (old_state == IEEE80211_STA_ASSOC &&
 		   new_state == IEEE80211_STA_AUTH &&
@@ -3501,12 +3550,12 @@ static int ath10k_sta_state(struct ieee80211_hw *hw,
 		/*
 		 * Disassociation.
 		 */
-		ath10k_dbg(ATH10K_DBG_MAC, "mac sta %pM disassociated\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac sta %pM disassociated\n",
 			   sta->addr);
 
 		ret = ath10k_station_disassoc(ar, arvif, sta);
 		if (ret)
-			ath10k_warn("failed to disassociate station: %pM vdev %i: %i\n",
+			ath10k_warn(ar, "failed to disassociate station: %pM vdev %i: %i\n",
 				    sta->addr, arvif->vdev_id, ret);
 	}
 exit:
@@ -3515,7 +3564,7 @@ exit:
 }
 
 static int ath10k_conf_tx_uapsd(struct ath10k *ar, struct ieee80211_vif *vif,
-				 u16 ac, bool enable)
+				u16 ac, bool enable)
 {
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	u32 value = 0;
@@ -3554,7 +3603,7 @@ static int ath10k_conf_tx_uapsd(struct ath10k *ar, struct ieee80211_vif *vif,
 					  WMI_STA_PS_PARAM_UAPSD,
 					  arvif->u.sta.uapsd);
 	if (ret) {
-		ath10k_warn("failed to set uapsd params: %d\n", ret);
+		ath10k_warn(ar, "failed to set uapsd params: %d\n", ret);
 		goto exit;
 	}
 
@@ -3567,7 +3616,7 @@ static int ath10k_conf_tx_uapsd(struct ath10k *ar, struct ieee80211_vif *vif,
 					  WMI_STA_PS_PARAM_RX_WAKE_POLICY,
 					  value);
 	if (ret)
-		ath10k_warn("failed to set rx wake param: %d\n", ret);
+		ath10k_warn(ar, "failed to set rx wake param: %d\n", ret);
 
 exit:
 	return ret;
@@ -3617,13 +3666,13 @@ static int ath10k_conf_tx(struct ieee80211_hw *hw,
 	/* FIXME: FW accepts wmm params per hw, not per vif */
 	ret = ath10k_wmi_pdev_set_wmm_params(ar, &ar->wmm_params);
 	if (ret) {
-		ath10k_warn("failed to set wmm params: %d\n", ret);
+		ath10k_warn(ar, "failed to set wmm params: %d\n", ret);
 		goto exit;
 	}
 
 	ret = ath10k_conf_tx_uapsd(ar, vif, ac, params->uapsd);
 	if (ret)
-		ath10k_warn("failed to set sta uapsd: %d\n", ret);
+		ath10k_warn(ar, "failed to set sta uapsd: %d\n", ret);
 
 exit:
 	mutex_unlock(&ar->conf_mutex);
@@ -3641,27 +3690,33 @@ static int ath10k_remain_on_channel(struct ieee80211_hw *hw,
 	struct ath10k *ar = hw->priv;
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 	struct wmi_start_scan_arg arg;
-	int ret;
+	int ret = 0;
 
 	mutex_lock(&ar->conf_mutex);
 
 	spin_lock_bh(&ar->data_lock);
-	if (ar->scan.in_progress) {
-		spin_unlock_bh(&ar->data_lock);
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+		reinit_completion(&ar->scan.started);
+		reinit_completion(&ar->scan.completed);
+		reinit_completion(&ar->scan.on_channel);
+		ar->scan.state = ATH10K_SCAN_STARTING;
+		ar->scan.is_roc = true;
+		ar->scan.vdev_id = arvif->vdev_id;
+		ar->scan.roc_freq = chan->center_freq;
+		ret = 0;
+		break;
+	case ATH10K_SCAN_STARTING:
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
 		ret = -EBUSY;
-		goto exit;
+		break;
 	}
-
-	reinit_completion(&ar->scan.started);
-	reinit_completion(&ar->scan.completed);
-	reinit_completion(&ar->scan.on_channel);
-	ar->scan.in_progress = true;
-	ar->scan.aborting = false;
-	ar->scan.is_roc = true;
-	ar->scan.vdev_id = arvif->vdev_id;
-	ar->scan.roc_freq = chan->center_freq;
 	spin_unlock_bh(&ar->data_lock);
 
+	if (ret)
+		goto exit;
+
 	memset(&arg, 0, sizeof(arg));
 	ath10k_wmi_start_scan_init(ar, &arg);
 	arg.vdev_id = arvif->vdev_id;
@@ -3676,17 +3731,21 @@ static int ath10k_remain_on_channel(struct ieee80211_hw *hw,
 
 	ret = ath10k_start_scan(ar, &arg);
 	if (ret) {
-		ath10k_warn("failed to start roc scan: %d\n", ret);
+		ath10k_warn(ar, "failed to start roc scan: %d\n", ret);
 		spin_lock_bh(&ar->data_lock);
-		ar->scan.in_progress = false;
+		ar->scan.state = ATH10K_SCAN_IDLE;
 		spin_unlock_bh(&ar->data_lock);
 		goto exit;
 	}
 
 	ret = wait_for_completion_timeout(&ar->scan.on_channel, 3*HZ);
 	if (ret == 0) {
-		ath10k_warn("failed to switch to channel for roc scan\n");
-		ath10k_abort_scan(ar);
+		ath10k_warn(ar, "failed to switch to channel for roc scan\n");
+
+		ret = ath10k_scan_stop(ar);
+		if (ret)
+			ath10k_warn(ar, "failed to stop scan: %d\n", ret);
+
 		ret = -ETIMEDOUT;
 		goto exit;
 	}
@@ -3702,7 +3761,8 @@ static int ath10k_cancel_remain_on_channel(struct ieee80211_hw *hw)
 	struct ath10k *ar = hw->priv;
 
 	mutex_lock(&ar->conf_mutex);
-	ath10k_abort_scan(ar);
+	cancel_delayed_work_sync(&ar->scan.timeout);
+	ath10k_scan_abort(ar);
 	mutex_unlock(&ar->conf_mutex);
 
 	return 0;
@@ -3721,12 +3781,12 @@ static int ath10k_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 
 	mutex_lock(&ar->conf_mutex);
 	list_for_each_entry(arvif, &ar->arvifs, list) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d rts threshold %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d rts threshold %d\n",
 			   arvif->vdev_id, value);
 
 		ret = ath10k_mac_set_rts(arvif, value);
 		if (ret) {
-			ath10k_warn("failed to set rts threshold for vdev %d: %d\n",
+			ath10k_warn(ar, "failed to set rts threshold for vdev %d: %d\n",
 				    arvif->vdev_id, ret);
 			break;
 		}
@@ -3744,12 +3804,12 @@ static int ath10k_set_frag_threshold(struct ieee80211_hw *hw, u32 value)
 
 	mutex_lock(&ar->conf_mutex);
 	list_for_each_entry(arvif, &ar->arvifs, list) {
-		ath10k_dbg(ATH10K_DBG_MAC, "mac vdev %d fragmentation threshold %d\n",
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac vdev %d fragmentation threshold %d\n",
 			   arvif->vdev_id, value);
 
 		ret = ath10k_mac_set_rts(arvif, value);
 		if (ret) {
-			ath10k_warn("failed to set fragmentation threshold for vdev %d: %d\n",
+			ath10k_warn(ar, "failed to set fragmentation threshold for vdev %d: %d\n",
 				    arvif->vdev_id, ret);
 			break;
 		}
@@ -3789,7 +3849,7 @@ static void ath10k_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		}), ATH10K_FLUSH_TIMEOUT_HZ);
 
 	if (ret <= 0 || skip)
-		ath10k_warn("failed to flush transmit queue (skip %i ar-state %i): %i\n",
+		ath10k_warn(ar, "failed to flush transmit queue (skip %i ar-state %i): %i\n",
 			    skip, ar->state, ret);
 
 skip:
@@ -3824,7 +3884,7 @@ static int ath10k_suspend(struct ieee80211_hw *hw,
 
 	ret = ath10k_hif_suspend(ar);
 	if (ret) {
-		ath10k_warn("failed to suspend hif: %d\n", ret);
+		ath10k_warn(ar, "failed to suspend hif: %d\n", ret);
 		goto resume;
 	}
 
@@ -3833,7 +3893,7 @@ static int ath10k_suspend(struct ieee80211_hw *hw,
 resume:
 	ret = ath10k_wmi_pdev_resume_target(ar);
 	if (ret)
-		ath10k_warn("failed to resume target: %d\n", ret);
+		ath10k_warn(ar, "failed to resume target: %d\n", ret);
 
 	ret = 1;
 exit:
@@ -3850,14 +3910,14 @@ static int ath10k_resume(struct ieee80211_hw *hw)
 
 	ret = ath10k_hif_resume(ar);
 	if (ret) {
-		ath10k_warn("failed to resume hif: %d\n", ret);
+		ath10k_warn(ar, "failed to resume hif: %d\n", ret);
 		ret = 1;
 		goto exit;
 	}
 
 	ret = ath10k_wmi_pdev_resume_target(ar);
 	if (ret) {
-		ath10k_warn("failed to resume target: %d\n", ret);
+		ath10k_warn(ar, "failed to resume target: %d\n", ret);
 		ret = 1;
 		goto exit;
 	}
@@ -3878,7 +3938,7 @@ static void ath10k_restart_complete(struct ieee80211_hw *hw)
 	/* If device failed to restart it will be in a different state, e.g.
 	 * ATH10K_STATE_WEDGED */
 	if (ar->state == ATH10K_STATE_RESTARTED) {
-		ath10k_info("device successfully recovered\n");
+		ath10k_info(ar, "device successfully recovered\n");
 		ar->state = ATH10K_STATE_ON;
 	}
 
@@ -4005,8 +4065,8 @@ ath10k_bitrate_mask_nss(const struct cfg80211_bitrate_mask *mask,
 			continue;
 		else if (mask->control[band].ht_mcs[i] == 0x00)
 			break;
-		else
-			return false;
+
+		return false;
 	}
 
 	ht_nss = i;
@@ -4017,8 +4077,8 @@ ath10k_bitrate_mask_nss(const struct cfg80211_bitrate_mask *mask,
 			continue;
 		else if (mask->control[band].vht_mcs[i] == 0x0000)
 			break;
-		else
-			return false;
+
+		return false;
 	}
 
 	vht_nss = i;
@@ -4075,7 +4135,8 @@ ath10k_bitrate_mask_correct(const struct cfg80211_bitrate_mask *mask,
 }
 
 static bool
-ath10k_bitrate_mask_rate(const struct cfg80211_bitrate_mask *mask,
+ath10k_bitrate_mask_rate(struct ath10k *ar,
+			 const struct cfg80211_bitrate_mask *mask,
 			 enum ieee80211_band band,
 			 u8 *fixed_rate,
 			 u8 *fixed_nss)
@@ -4133,7 +4194,7 @@ ath10k_bitrate_mask_rate(const struct cfg80211_bitrate_mask *mask,
 	nss <<= 4;
 	pream <<= 6;
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac fixed rate pream 0x%02x nss 0x%02x rate 0x%02x\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac fixed rate pream 0x%02x nss 0x%02x rate 0x%02x\n",
 		   pream, nss, rate);
 
 	*fixed_rate = pream | nss | rate;
@@ -4141,7 +4202,8 @@ ath10k_bitrate_mask_rate(const struct cfg80211_bitrate_mask *mask,
 	return true;
 }
 
-static bool ath10k_get_fixed_rate_nss(const struct cfg80211_bitrate_mask *mask,
+static bool ath10k_get_fixed_rate_nss(struct ath10k *ar,
+				      const struct cfg80211_bitrate_mask *mask,
 				      enum ieee80211_band band,
 				      u8 *fixed_rate,
 				      u8 *fixed_nss)
@@ -4151,7 +4213,7 @@ static bool ath10k_get_fixed_rate_nss(const struct cfg80211_bitrate_mask *mask,
 		return true;
 
 	/* Next Check single rate is set */
-	return ath10k_bitrate_mask_rate(mask, band, fixed_rate, fixed_nss);
+	return ath10k_bitrate_mask_rate(ar, mask, band, fixed_rate, fixed_nss);
 }
 
 static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
@@ -4171,16 +4233,16 @@ static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
 		goto exit;
 
 	if (fixed_rate == WMI_FIXED_RATE_NONE)
-		ath10k_dbg(ATH10K_DBG_MAC, "mac disable fixed bitrate mask\n");
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac disable fixed bitrate mask\n");
 
 	if (force_sgi)
-		ath10k_dbg(ATH10K_DBG_MAC, "mac force sgi\n");
+		ath10k_dbg(ar, ATH10K_DBG_MAC, "mac force sgi\n");
 
 	vdev_param = ar->wmi.vdev_param->fixed_rate;
 	ret = ath10k_wmi_vdev_set_param(ar, arvif->vdev_id,
 					vdev_param, fixed_rate);
 	if (ret) {
-		ath10k_warn("failed to set fixed rate param 0x%02x: %d\n",
+		ath10k_warn(ar, "failed to set fixed rate param 0x%02x: %d\n",
 			    fixed_rate, ret);
 		ret = -EINVAL;
 		goto exit;
@@ -4193,7 +4255,7 @@ static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
 					vdev_param, fixed_nss);
 
 	if (ret) {
-		ath10k_warn("failed to set fixed nss param %d: %d\n",
+		ath10k_warn(ar, "failed to set fixed nss param %d: %d\n",
 			    fixed_nss, ret);
 		ret = -EINVAL;
 		goto exit;
@@ -4206,7 +4268,7 @@ static int ath10k_set_fixed_rate_param(struct ath10k_vif *arvif,
 					force_sgi);
 
 	if (ret) {
-		ath10k_warn("failed to set sgi param %d: %d\n",
+		ath10k_warn(ar, "failed to set sgi param %d: %d\n",
 			    force_sgi, ret);
 		ret = -EINVAL;
 		goto exit;
@@ -4235,14 +4297,14 @@ static int ath10k_set_bitrate_mask(struct ieee80211_hw *hw,
 		return -EINVAL;
 
 	if (!ath10k_default_bitrate_mask(ar, band, mask)) {
-		if (!ath10k_get_fixed_rate_nss(mask, band,
+		if (!ath10k_get_fixed_rate_nss(ar, mask, band,
 					       &fixed_rate,
 					       &fixed_nss))
 			return -EINVAL;
 	}
 
 	if (fixed_rate == WMI_FIXED_RATE_NONE && force_sgi) {
-		ath10k_warn("failed to force SGI usage for default rate settings\n");
+		ath10k_warn(ar, "failed to force SGI usage for default rate settings\n");
 		return -EINVAL;
 	}
 
@@ -4261,7 +4323,7 @@ static void ath10k_sta_rc_update(struct ieee80211_hw *hw,
 
 	spin_lock_bh(&ar->data_lock);
 
-	ath10k_dbg(ATH10K_DBG_MAC,
+	ath10k_dbg(ar, ATH10K_DBG_MAC,
 		   "mac sta rc update for %pM changed %08x bw %d nss %d smps %d\n",
 		   sta->addr, changed, sta->bandwidth, sta->rx_nss,
 		   sta->smps_mode);
@@ -4280,7 +4342,7 @@ static void ath10k_sta_rc_update(struct ieee80211_hw *hw,
 			bw = WMI_PEER_CHWIDTH_80MHZ;
 			break;
 		case IEEE80211_STA_RX_BW_160:
-			ath10k_warn("Invalid bandwith %d in rc update for %pM\n",
+			ath10k_warn(ar, "Invalid bandwith %d in rc update for %pM\n",
 				    sta->bandwidth, sta->addr);
 			bw = WMI_PEER_CHWIDTH_20MHZ;
 			break;
@@ -4307,7 +4369,7 @@ static void ath10k_sta_rc_update(struct ieee80211_hw *hw,
 			smps = WMI_PEER_SMPS_DYNAMIC;
 			break;
 		case IEEE80211_SMPS_NUM_MODES:
-			ath10k_warn("Invalid smps %d in sta rc update for %pM\n",
+			ath10k_warn(ar, "Invalid smps %d in sta rc update for %pM\n",
 				    sta->smps_mode, sta->addr);
 			smps = WMI_PEER_SMPS_PS_NONE;
 			break;
@@ -4339,9 +4401,10 @@ static int ath10k_ampdu_action(struct ieee80211_hw *hw,
 			       struct ieee80211_sta *sta, u16 tid, u16 *ssn,
 			       u8 buf_size)
 {
+	struct ath10k *ar = hw->priv;
 	struct ath10k_vif *arvif = ath10k_vif_to_arvif(vif);
 
-	ath10k_dbg(ATH10K_DBG_MAC, "mac ampdu vdev_id %i sta %pM tid %hu action %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_MAC, "mac ampdu vdev_id %i sta %pM tid %hu action %d\n",
 		   arvif->vdev_id, sta->addr, tid, action);
 
 	switch (action) {
@@ -4393,6 +4456,9 @@ static const struct ieee80211_ops ath10k_ops = {
 	.sta_rc_update			= ath10k_sta_rc_update,
 	.get_tsf			= ath10k_get_tsf,
 	.ampdu_action			= ath10k_ampdu_action,
+
+	CFG80211_TESTMODE_CMD(ath10k_tm_cmd)
+
 #ifdef CONFIG_PM
 	.suspend			= ath10k_suspend,
 	.resume				= ath10k_resume,
@@ -4489,12 +4555,12 @@ static struct ieee80211_rate ath10k_rates[] = {
 #define ath10k_g_rates (ath10k_rates + 0)
 #define ath10k_g_rates_size (ARRAY_SIZE(ath10k_rates))
 
-struct ath10k *ath10k_mac_create(void)
+struct ath10k *ath10k_mac_create(size_t priv_size)
 {
 	struct ieee80211_hw *hw;
 	struct ath10k *ar;
 
-	hw = ieee80211_alloc_hw(sizeof(struct ath10k), &ath10k_ops);
+	hw = ieee80211_alloc_hw(sizeof(struct ath10k) + priv_size, &ath10k_ops);
 	if (!hw)
 		return NULL;
 
@@ -4644,7 +4710,6 @@ static struct ieee80211_sta_ht_cap ath10k_get_ht_cap(struct ath10k *ar)
 	return ht_cap;
 }
 
-
 static void ath10k_get_arvif_iter(void *data, u8 *mac,
 				  struct ieee80211_vif *vif)
 {
@@ -4669,7 +4734,7 @@ struct ath10k_vif *ath10k_get_arvif(struct ath10k *ar, u32 vdev_id)
 						   ath10k_get_arvif_iter,
 						   &arvif_iter);
 	if (!arvif_iter.arvif) {
-		ath10k_warn("No VIF found for vdev %d\n", vdev_id);
+		ath10k_warn(ar, "No VIF found for vdev %d\n", vdev_id);
 		return NULL;
 	}
 
@@ -4759,7 +4824,6 @@ int ath10k_mac_register(struct ath10k *ar)
 			IEEE80211_HW_MFP_CAPABLE |
 			IEEE80211_HW_REPORTS_TX_ACK_STATUS |
 			IEEE80211_HW_HAS_RATE_CONTROL |
-			IEEE80211_HW_SUPPORTS_STATIC_SMPS |
 			IEEE80211_HW_AP_LINK_PS |
 			IEEE80211_HW_SPECTRUM_MGMT;
 
@@ -4767,8 +4831,10 @@ int ath10k_mac_register(struct ath10k *ar)
 	 * bytes is used for padding/alignment if necessary. */
 	ar->hw->extra_tx_headroom += sizeof(struct htt_data_tx_desc_frag)*2 + 4;
 
+	ar->hw->wiphy->features |= NL80211_FEATURE_STATIC_SMPS;
+
 	if (ar->ht_cap_info & WMI_HT_CAP_DYNAMIC_SMPS)
-		ar->hw->flags |= IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS;
+		ar->hw->wiphy->features |= NL80211_FEATURE_DYNAMIC_SMPS;
 
 	if (ar->ht_cap_info & WMI_HT_CAP_ENABLED) {
 		ar->hw->flags |= IEEE80211_HW_AMPDU_AGGREGATION;
@@ -4815,19 +4881,19 @@ int ath10k_mac_register(struct ath10k *ar)
 							     NL80211_DFS_UNSET);
 
 		if (!ar->dfs_detector)
-			ath10k_warn("failed to initialise DFS pattern detector\n");
+			ath10k_warn(ar, "failed to initialise DFS pattern detector\n");
 	}
 
 	ret = ath_regd_init(&ar->ath_common.regulatory, ar->hw->wiphy,
 			    ath10k_reg_notifier);
 	if (ret) {
-		ath10k_err("failed to initialise regulatory: %i\n", ret);
+		ath10k_err(ar, "failed to initialise regulatory: %i\n", ret);
 		goto err_free;
 	}
 
 	ret = ieee80211_register_hw(ar->hw);
 	if (ret) {
-		ath10k_err("failed to register ieee80211: %d\n", ret);
+		ath10k_err(ar, "failed to register ieee80211: %d\n", ret);
 		goto err_free;
 	}
 
diff --git a/drivers/net/wireless/ath/ath10k/mac.h b/drivers/net/wireless/ath/ath10k/mac.h
index ef4f84376d7c..6c80eeada3e2 100644
--- a/drivers/net/wireless/ath/ath10k/mac.h
+++ b/drivers/net/wireless/ath/ath10k/mac.h
@@ -26,12 +26,14 @@ struct ath10k_generic_iter {
 	int ret;
 };
 
-struct ath10k *ath10k_mac_create(void);
+struct ath10k *ath10k_mac_create(size_t priv_size);
 void ath10k_mac_destroy(struct ath10k *ar);
 int ath10k_mac_register(struct ath10k *ar);
 void ath10k_mac_unregister(struct ath10k *ar);
 struct ath10k_vif *ath10k_get_arvif(struct ath10k *ar, u32 vdev_id);
-void ath10k_reset_scan(unsigned long ptr);
+void __ath10k_scan_finish(struct ath10k *ar);
+void ath10k_scan_finish(struct ath10k *ar);
+void ath10k_scan_timeout_work(struct work_struct *work);
 void ath10k_offchan_tx_purge(struct ath10k *ar);
 void ath10k_offchan_tx_work(struct work_struct *work);
 void ath10k_mgmt_over_wmi_tx_purge(struct ath10k *ar);
diff --git a/drivers/net/wireless/ath/ath10k/pci.c b/drivers/net/wireless/ath/ath10k/pci.c
index 3376963a4862..59e0ea83be50 100644
--- a/drivers/net/wireless/ath/ath10k/pci.c
+++ b/drivers/net/wireless/ath/ath10k/pci.c
@@ -44,13 +44,9 @@ enum ath10k_pci_reset_mode {
 	ATH10K_PCI_RESET_WARM_ONLY = 1,
 };
 
-static unsigned int ath10k_pci_target_ps;
 static unsigned int ath10k_pci_irq_mode = ATH10K_PCI_IRQ_AUTO;
 static unsigned int ath10k_pci_reset_mode = ATH10K_PCI_RESET_AUTO;
 
-module_param_named(target_ps, ath10k_pci_target_ps, uint, 0644);
-MODULE_PARM_DESC(target_ps, "Enable ath10k Target (SoC) PS option");
-
 module_param_named(irq_mode, ath10k_pci_irq_mode, uint, 0644);
 MODULE_PARM_DESC(irq_mode, "0: auto, 1: legacy, 2: msi (default: 0)");
 
@@ -68,13 +64,7 @@ static const struct pci_device_id ath10k_pci_id_table[] = {
 	{0}
 };
 
-static int ath10k_pci_diag_read_access(struct ath10k *ar, u32 address,
-				       u32 *data);
-
-static int ath10k_pci_post_rx(struct ath10k *ar);
-static int ath10k_pci_post_rx_pipe(struct ath10k_pci_pipe *pipe_info,
-					     int num);
-static void ath10k_pci_rx_pipe_cleanup(struct ath10k_pci_pipe *pipe_info);
+static void ath10k_pci_buffer_cleanup(struct ath10k *ar);
 static int ath10k_pci_cold_reset(struct ath10k *ar);
 static int ath10k_pci_warm_reset(struct ath10k *ar);
 static int ath10k_pci_wait_for_target_init(struct ath10k *ar);
@@ -156,79 +146,175 @@ static const struct ce_attr host_ce_config_wlan[] = {
 static const struct ce_pipe_config target_ce_config_wlan[] = {
 	/* CE0: host->target HTC control and raw streams */
 	{
-		.pipenum = 0,
-		.pipedir = PIPEDIR_OUT,
-		.nentries = 32,
-		.nbytes_max = 256,
-		.flags = CE_ATTR_FLAGS,
-		.reserved = 0,
+		.pipenum = __cpu_to_le32(0),
+		.pipedir = __cpu_to_le32(PIPEDIR_OUT),
+		.nentries = __cpu_to_le32(32),
+		.nbytes_max = __cpu_to_le32(256),
+		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
+		.reserved = __cpu_to_le32(0),
 	},
 
 	/* CE1: target->host HTT + HTC control */
 	{
-		.pipenum = 1,
-		.pipedir = PIPEDIR_IN,
-		.nentries = 32,
-		.nbytes_max = 512,
-		.flags = CE_ATTR_FLAGS,
-		.reserved = 0,
+		.pipenum = __cpu_to_le32(1),
+		.pipedir = __cpu_to_le32(PIPEDIR_IN),
+		.nentries = __cpu_to_le32(32),
+		.nbytes_max = __cpu_to_le32(512),
+		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
+		.reserved = __cpu_to_le32(0),
 	},
 
 	/* CE2: target->host WMI */
 	{
-		.pipenum = 2,
-		.pipedir = PIPEDIR_IN,
-		.nentries = 32,
-		.nbytes_max = 2048,
-		.flags = CE_ATTR_FLAGS,
-		.reserved = 0,
+		.pipenum = __cpu_to_le32(2),
+		.pipedir = __cpu_to_le32(PIPEDIR_IN),
+		.nentries = __cpu_to_le32(32),
+		.nbytes_max = __cpu_to_le32(2048),
+		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
+		.reserved = __cpu_to_le32(0),
 	},
 
 	/* CE3: host->target WMI */
 	{
-		.pipenum = 3,
-		.pipedir = PIPEDIR_OUT,
-		.nentries = 32,
-		.nbytes_max = 2048,
-		.flags = CE_ATTR_FLAGS,
-		.reserved = 0,
+		.pipenum = __cpu_to_le32(3),
+		.pipedir = __cpu_to_le32(PIPEDIR_OUT),
+		.nentries = __cpu_to_le32(32),
+		.nbytes_max = __cpu_to_le32(2048),
+		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
+		.reserved = __cpu_to_le32(0),
 	},
 
 	/* CE4: host->target HTT */
 	{
-		.pipenum = 4,
-		.pipedir = PIPEDIR_OUT,
-		.nentries = 256,
-		.nbytes_max = 256,
-		.flags = CE_ATTR_FLAGS,
-		.reserved = 0,
+		.pipenum = __cpu_to_le32(4),
+		.pipedir = __cpu_to_le32(PIPEDIR_OUT),
+		.nentries = __cpu_to_le32(256),
+		.nbytes_max = __cpu_to_le32(256),
+		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
+		.reserved = __cpu_to_le32(0),
 	},
 
 	/* NB: 50% of src nentries, since tx has 2 frags */
 
 	/* CE5: unused */
 	{
-		.pipenum = 5,
-		.pipedir = PIPEDIR_OUT,
-		.nentries = 32,
-		.nbytes_max = 2048,
-		.flags = CE_ATTR_FLAGS,
-		.reserved = 0,
+		.pipenum = __cpu_to_le32(5),
+		.pipedir = __cpu_to_le32(PIPEDIR_OUT),
+		.nentries = __cpu_to_le32(32),
+		.nbytes_max = __cpu_to_le32(2048),
+		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
+		.reserved = __cpu_to_le32(0),
 	},
 
 	/* CE6: Reserved for target autonomous hif_memcpy */
 	{
-		.pipenum = 6,
-		.pipedir = PIPEDIR_INOUT,
-		.nentries = 32,
-		.nbytes_max = 4096,
-		.flags = CE_ATTR_FLAGS,
-		.reserved = 0,
+		.pipenum = __cpu_to_le32(6),
+		.pipedir = __cpu_to_le32(PIPEDIR_INOUT),
+		.nentries = __cpu_to_le32(32),
+		.nbytes_max = __cpu_to_le32(4096),
+		.flags = __cpu_to_le32(CE_ATTR_FLAGS),
+		.reserved = __cpu_to_le32(0),
 	},
 
 	/* CE7 used only by Host */
 };
 
+/*
+ * Map from service/endpoint to Copy Engine.
+ * This table is derived from the CE_PCI TABLE, above.
+ * It is passed to the Target at startup for use by firmware.
+ */
+static const struct service_to_pipe target_service_to_ce_map_wlan[] = {
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_VO),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(3),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_VO),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(2),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_BK),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(3),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_BK),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(2),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_BE),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(3),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_BE),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(2),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_VI),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(3),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_DATA_VI),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(2),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_CONTROL),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(3),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_WMI_CONTROL),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(2),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_RSVD_CTRL),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(0),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_RSVD_CTRL),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(1),
+	},
+	{ /* not used */
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_TEST_RAW_STREAMS),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(0),
+	},
+	{ /* not used */
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_TEST_RAW_STREAMS),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(1),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_HTT_DATA_MSG),
+		__cpu_to_le32(PIPEDIR_OUT),	/* out = UL = host -> target */
+		__cpu_to_le32(4),
+	},
+	{
+		__cpu_to_le32(ATH10K_HTC_SVC_ID_HTT_DATA_MSG),
+		__cpu_to_le32(PIPEDIR_IN),	/* in = DL = target -> host */
+		__cpu_to_le32(1),
+	},
+
+	/* (Additions here) */
+
+	{ /* must be last */
+		__cpu_to_le32(0),
+		__cpu_to_le32(0),
+		__cpu_to_le32(0),
+	},
+};
+
 static bool ath10k_pci_irq_pending(struct ath10k *ar)
 {
 	u32 cause;
@@ -254,8 +340,8 @@ static void ath10k_pci_disable_and_clear_legacy_irq(struct ath10k *ar)
 
 	/* IMPORTANT: this extra read transaction is required to
 	 * flush the posted write buffer. */
-	(void) ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
-				 PCIE_INTR_ENABLE_ADDRESS);
+	(void)ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
+				PCIE_INTR_ENABLE_ADDRESS);
 }
 
 static void ath10k_pci_enable_legacy_irq(struct ath10k *ar)
@@ -266,48 +352,116 @@ static void ath10k_pci_enable_legacy_irq(struct ath10k *ar)
 
 	/* IMPORTANT: this extra read transaction is required to
 	 * flush the posted write buffer. */
-	(void) ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
-				 PCIE_INTR_ENABLE_ADDRESS);
+	(void)ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
+				PCIE_INTR_ENABLE_ADDRESS);
 }
 
-static irqreturn_t ath10k_pci_early_irq_handler(int irq, void *arg)
+static inline const char *ath10k_pci_get_irq_method(struct ath10k *ar)
 {
-	struct ath10k *ar = arg;
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 
-	if (ar_pci->num_msi_intrs == 0) {
-		if (!ath10k_pci_irq_pending(ar))
-			return IRQ_NONE;
-
-		ath10k_pci_disable_and_clear_legacy_irq(ar);
-	}
+	if (ar_pci->num_msi_intrs > 1)
+		return "msi-x";
 
-	tasklet_schedule(&ar_pci->early_irq_tasklet);
+	if (ar_pci->num_msi_intrs == 1)
+		return "msi";
 
-	return IRQ_HANDLED;
+	return "legacy";
 }
 
-static int ath10k_pci_request_early_irq(struct ath10k *ar)
+static int __ath10k_pci_rx_post_buf(struct ath10k_pci_pipe *pipe)
 {
+	struct ath10k *ar = pipe->hif_ce_state;
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	struct ath10k_ce_pipe *ce_pipe = pipe->ce_hdl;
+	struct sk_buff *skb;
+	dma_addr_t paddr;
 	int ret;
 
-	/* Regardless whether MSI-X/MSI/legacy irqs have been set up the first
-	 * interrupt from irq vector is triggered in all cases for FW
-	 * indication/errors */
-	ret = request_irq(ar_pci->pdev->irq, ath10k_pci_early_irq_handler,
-			  IRQF_SHARED, "ath10k_pci (early)", ar);
+	lockdep_assert_held(&ar_pci->ce_lock);
+
+	skb = dev_alloc_skb(pipe->buf_sz);
+	if (!skb)
+		return -ENOMEM;
+
+	WARN_ONCE((unsigned long)skb->data & 3, "unaligned skb");
+
+	paddr = dma_map_single(ar->dev, skb->data,
+			       skb->len + skb_tailroom(skb),
+			       DMA_FROM_DEVICE);
+	if (unlikely(dma_mapping_error(ar->dev, paddr))) {
+		ath10k_warn(ar, "failed to dma map pci rx buf\n");
+		dev_kfree_skb_any(skb);
+		return -EIO;
+	}
+
+	ATH10K_SKB_CB(skb)->paddr = paddr;
+
+	ret = __ath10k_ce_rx_post_buf(ce_pipe, skb, paddr);
 	if (ret) {
-		ath10k_warn("failed to request early irq: %d\n", ret);
+		ath10k_warn(ar, "failed to post pci rx buf: %d\n", ret);
+		dma_unmap_single(ar->dev, paddr, skb->len + skb_tailroom(skb),
+				 DMA_FROM_DEVICE);
+		dev_kfree_skb_any(skb);
 		return ret;
 	}
 
 	return 0;
 }
 
-static void ath10k_pci_free_early_irq(struct ath10k *ar)
+static void __ath10k_pci_rx_post_pipe(struct ath10k_pci_pipe *pipe)
+{
+	struct ath10k *ar = pipe->hif_ce_state;
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	struct ath10k_ce_pipe *ce_pipe = pipe->ce_hdl;
+	int ret, num;
+
+	lockdep_assert_held(&ar_pci->ce_lock);
+
+	if (pipe->buf_sz == 0)
+		return;
+
+	if (!ce_pipe->dest_ring)
+		return;
+
+	num = __ath10k_ce_rx_num_free_bufs(ce_pipe);
+	while (num--) {
+		ret = __ath10k_pci_rx_post_buf(pipe);
+		if (ret) {
+			ath10k_warn(ar, "failed to post pci rx buf: %d\n", ret);
+			mod_timer(&ar_pci->rx_post_retry, jiffies +
+				  ATH10K_PCI_RX_POST_RETRY_MS);
+			break;
+		}
+	}
+}
+
+static void ath10k_pci_rx_post_pipe(struct ath10k_pci_pipe *pipe)
+{
+	struct ath10k *ar = pipe->hif_ce_state;
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+
+	spin_lock_bh(&ar_pci->ce_lock);
+	__ath10k_pci_rx_post_pipe(pipe);
+	spin_unlock_bh(&ar_pci->ce_lock);
+}
+
+static void ath10k_pci_rx_post(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	int i;
+
+	spin_lock_bh(&ar_pci->ce_lock);
+	for (i = 0; i < CE_COUNT; i++)
+		__ath10k_pci_rx_post_pipe(&ar_pci->pipe_info[i]);
+	spin_unlock_bh(&ar_pci->ce_lock);
+}
+
+static void ath10k_pci_rx_replenish_retry(unsigned long ptr)
 {
-	free_irq(ath10k_pci_priv(ar)->pdev->irq, ar);
+	struct ath10k *ar = (void *)ptr;
+
+	ath10k_pci_rx_post(ar);
 }
 
 /*
@@ -331,25 +485,6 @@ static int ath10k_pci_diag_read_mem(struct ath10k *ar, u32 address, void *data,
 	void *data_buf = NULL;
 	int i;
 
-	/*
-	 * This code cannot handle reads to non-memory space. Redirect to the
-	 * register read fn but preserve the multi word read capability of
-	 * this fn
-	 */
-	if (address < DRAM_BASE_ADDRESS) {
-		if (!IS_ALIGNED(address, 4) ||
-		    !IS_ALIGNED((unsigned long)data, 4))
-			return -EIO;
-
-		while ((nbytes >= 4) &&  ((ret = ath10k_pci_diag_read_access(
-					   ar, address, (u32 *)data)) == 0)) {
-			nbytes -= sizeof(u32);
-			address += sizeof(u32);
-			data += sizeof(u32);
-		}
-		return ret;
-	}
-
 	ce_diag = ar_pci->ce_diag;
 
 	/*
@@ -376,7 +511,7 @@ static int ath10k_pci_diag_read_mem(struct ath10k *ar, u32 address, void *data,
 		nbytes = min_t(unsigned int, remaining_bytes,
 			       DIAG_TRANSFER_LIMIT);
 
-		ret = ath10k_ce_recv_buf_enqueue(ce_diag, NULL, ce_data);
+		ret = ath10k_ce_rx_post_buf(ce_diag, NULL, ce_data);
 		if (ret != 0)
 			goto done;
 
@@ -389,13 +524,11 @@ static int ath10k_pci_diag_read_mem(struct ath10k *ar, u32 address, void *data,
 		 * convert it from Target CPU virtual address space
 		 * to CE address space
 		 */
-		ath10k_pci_wake(ar);
 		address = TARG_CPU_SPACE_TO_CE_SPACE(ar, ar_pci->mem,
 						     address);
-		ath10k_pci_sleep(ar);
 
 		ret = ath10k_ce_send(ce_diag, NULL, (u32)address, nbytes, 0,
-				 0);
+				     0);
 		if (ret)
 			goto done;
 
@@ -415,7 +548,7 @@ static int ath10k_pci_diag_read_mem(struct ath10k *ar, u32 address, void *data,
 			goto done;
 		}
 
-		if (buf != (u32) address) {
+		if (buf != (u32)address) {
 			ret = -EIO;
 			goto done;
 		}
@@ -448,15 +581,10 @@ static int ath10k_pci_diag_read_mem(struct ath10k *ar, u32 address, void *data,
 	}
 
 done:
-	if (ret == 0) {
-		/* Copy data from allocated DMA buf to caller's buf */
-		WARN_ON_ONCE(orig_nbytes & 3);
-		for (i = 0; i < orig_nbytes / sizeof(__le32); i++) {
-			((u32 *)data)[i] =
-				__le32_to_cpu(((__le32 *)data_buf)[i]);
-		}
-	} else
-		ath10k_warn("failed to read diag value at 0x%x: %d\n",
+	if (ret == 0)
+		memcpy(data, data_buf, orig_nbytes);
+	else
+		ath10k_warn(ar, "failed to read diag value at 0x%x: %d\n",
 			    address, ret);
 
 	if (data_buf)
@@ -466,20 +594,45 @@ done:
 	return ret;
 }
 
-/* Read 4-byte aligned data from Target memory or register */
-static int ath10k_pci_diag_read_access(struct ath10k *ar, u32 address,
-				       u32 *data)
+static int ath10k_pci_diag_read32(struct ath10k *ar, u32 address, u32 *value)
 {
-	/* Assume range doesn't cross this boundary */
-	if (address >= DRAM_BASE_ADDRESS)
-		return ath10k_pci_diag_read_mem(ar, address, data, sizeof(u32));
+	__le32 val = 0;
+	int ret;
+
+	ret = ath10k_pci_diag_read_mem(ar, address, &val, sizeof(val));
+	*value = __le32_to_cpu(val);
+
+	return ret;
+}
+
+static int __ath10k_pci_diag_read_hi(struct ath10k *ar, void *dest,
+				     u32 src, u32 len)
+{
+	u32 host_addr, addr;
+	int ret;
+
+	host_addr = host_interest_item_address(src);
+
+	ret = ath10k_pci_diag_read32(ar, host_addr, &addr);
+	if (ret != 0) {
+		ath10k_warn(ar, "failed to get memcpy hi address for firmware address %d: %d\n",
+			    src, ret);
+		return ret;
+	}
+
+	ret = ath10k_pci_diag_read_mem(ar, addr, dest, len);
+	if (ret != 0) {
+		ath10k_warn(ar, "failed to memcpy firmware memory from %d (%d B): %d\n",
+			    addr, len, ret);
+		return ret;
+	}
 
-	ath10k_pci_wake(ar);
-	*data = ath10k_pci_read32(ar, address);
-	ath10k_pci_sleep(ar);
 	return 0;
 }
 
+#define ath10k_pci_diag_read_hi(ar, dest, src, len)		\
+	__ath10k_pci_diag_read_hi(ar, dest, HI_ITEM(src), len)
+
 static int ath10k_pci_diag_write_mem(struct ath10k *ar, u32 address,
 				     const void *data, int nbytes)
 {
@@ -514,9 +667,7 @@ static int ath10k_pci_diag_write_mem(struct ath10k *ar, u32 address,
 	}
 
 	/* Copy caller's data to allocated DMA buf */
-	WARN_ON_ONCE(orig_nbytes & 3);
-	for (i = 0; i < orig_nbytes / sizeof(__le32); i++)
-		((__le32 *)data_buf)[i] = __cpu_to_le32(((u32 *)data)[i]);
+	memcpy(data_buf, data, orig_nbytes);
 
 	/*
 	 * The address supplied by the caller is in the
@@ -528,9 +679,7 @@ static int ath10k_pci_diag_write_mem(struct ath10k *ar, u32 address,
 	 * to
 	 *    CE address space
 	 */
-	ath10k_pci_wake(ar);
 	address = TARG_CPU_SPACE_TO_CE_SPACE(ar, ar_pci->mem, address);
-	ath10k_pci_sleep(ar);
 
 	remaining_bytes = orig_nbytes;
 	ce_data = ce_data_base;
@@ -539,7 +688,7 @@ static int ath10k_pci_diag_write_mem(struct ath10k *ar, u32 address,
 		nbytes = min_t(int, remaining_bytes, DIAG_TRANSFER_LIMIT);
 
 		/* Set up to receive directly into Target(!) address */
-		ret = ath10k_ce_recv_buf_enqueue(ce_diag, NULL, address);
+		ret = ath10k_ce_rx_post_buf(ce_diag, NULL, address);
 		if (ret != 0)
 			goto done;
 
@@ -547,7 +696,7 @@ static int ath10k_pci_diag_write_mem(struct ath10k *ar, u32 address,
 		 * Request CE to send caller-supplied data that
 		 * was copied to bounce buffer to Target(!) address.
 		 */
-		ret = ath10k_ce_send(ce_diag, NULL, (u32) ce_data,
+		ret = ath10k_ce_send(ce_diag, NULL, (u32)ce_data,
 				     nbytes, 0, 0);
 		if (ret != 0)
 			goto done;
@@ -608,66 +757,34 @@ done:
 	}
 
 	if (ret != 0)
-		ath10k_warn("failed to write diag value at 0x%x: %d\n",
+		ath10k_warn(ar, "failed to write diag value at 0x%x: %d\n",
 			    address, ret);
 
 	return ret;
 }
 
-/* Write 4B data to Target memory or register */
-static int ath10k_pci_diag_write_access(struct ath10k *ar, u32 address,
-					u32 data)
+static int ath10k_pci_diag_write32(struct ath10k *ar, u32 address, u32 value)
 {
-	/* Assume range doesn't cross this boundary */
-	if (address >= DRAM_BASE_ADDRESS)
-		return ath10k_pci_diag_write_mem(ar, address, &data,
-						 sizeof(u32));
+	__le32 val = __cpu_to_le32(value);
 
-	ath10k_pci_wake(ar);
-	ath10k_pci_write32(ar, address, data);
-	ath10k_pci_sleep(ar);
-	return 0;
+	return ath10k_pci_diag_write_mem(ar, address, &val, sizeof(val));
 }
 
-static bool ath10k_pci_target_is_awake(struct ath10k *ar)
+static bool ath10k_pci_is_awake(struct ath10k *ar)
 {
-	void __iomem *mem = ath10k_pci_priv(ar)->mem;
-	u32 val;
-	val = ioread32(mem + PCIE_LOCAL_BASE_ADDRESS +
-		       RTC_STATE_ADDRESS);
-	return (RTC_STATE_V_GET(val) == RTC_STATE_V_ON);
+	u32 val = ath10k_pci_reg_read32(ar, RTC_STATE_ADDRESS);
+
+	return RTC_STATE_V_GET(val) == RTC_STATE_V_ON;
 }
 
-int ath10k_do_pci_wake(struct ath10k *ar)
+static int ath10k_pci_wake_wait(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	void __iomem *pci_addr = ar_pci->mem;
 	int tot_delay = 0;
 	int curr_delay = 5;
 
-	if (atomic_read(&ar_pci->keep_awake_count) == 0) {
-		/* Force AWAKE */
-		iowrite32(PCIE_SOC_WAKE_V_MASK,
-			  pci_addr + PCIE_LOCAL_BASE_ADDRESS +
-			  PCIE_SOC_WAKE_ADDRESS);
-	}
-	atomic_inc(&ar_pci->keep_awake_count);
-
-	if (ar_pci->verified_awake)
-		return 0;
-
-	for (;;) {
-		if (ath10k_pci_target_is_awake(ar)) {
-			ar_pci->verified_awake = true;
+	while (tot_delay < PCIE_WAKE_TIMEOUT) {
+		if (ath10k_pci_is_awake(ar))
 			return 0;
-		}
-
-		if (tot_delay > PCIE_WAKE_TIMEOUT) {
-			ath10k_warn("target took longer %d us to wake up (awake count %d)\n",
-				    PCIE_WAKE_TIMEOUT,
-				    atomic_read(&ar_pci->keep_awake_count));
-			return -ETIMEDOUT;
-		}
 
 		udelay(curr_delay);
 		tot_delay += curr_delay;
@@ -675,20 +792,21 @@ int ath10k_do_pci_wake(struct ath10k *ar)
 		if (curr_delay < 50)
 			curr_delay += 5;
 	}
+
+	return -ETIMEDOUT;
 }
 
-void ath10k_do_pci_sleep(struct ath10k *ar)
+static int ath10k_pci_wake(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	void __iomem *pci_addr = ar_pci->mem;
+	ath10k_pci_reg_write32(ar, PCIE_SOC_WAKE_ADDRESS,
+			       PCIE_SOC_WAKE_V_MASK);
+	return ath10k_pci_wake_wait(ar);
+}
 
-	if (atomic_dec_and_test(&ar_pci->keep_awake_count)) {
-		/* Allow sleep */
-		ar_pci->verified_awake = false;
-		iowrite32(PCIE_SOC_WAKE_RESET,
-			  pci_addr + PCIE_LOCAL_BASE_ADDRESS +
-			  PCIE_SOC_WAKE_ADDRESS);
-	}
+static void ath10k_pci_sleep(struct ath10k *ar)
+{
+	ath10k_pci_reg_write32(ar, PCIE_SOC_WAKE_ADDRESS,
+			       PCIE_SOC_WAKE_RESET);
 }
 
 /* Called by lower (CE) layer when a send to Target completes. */
@@ -726,19 +844,17 @@ static void ath10k_pci_ce_recv_data(struct ath10k_ce_pipe *ce_state)
 	unsigned int nbytes, max_nbytes;
 	unsigned int transfer_id;
 	unsigned int flags;
-	int err, num_replenish = 0;
 
 	while (ath10k_ce_completed_recv_next(ce_state, &transfer_context,
 					     &ce_data, &nbytes, &transfer_id,
 					     &flags) == 0) {
-		num_replenish++;
 		skb = transfer_context;
 		max_nbytes = skb->len + skb_tailroom(skb);
 		dma_unmap_single(ar->dev, ATH10K_SKB_CB(skb)->paddr,
 				 max_nbytes, DMA_FROM_DEVICE);
 
 		if (unlikely(max_nbytes < nbytes)) {
-			ath10k_warn("rxed more than expected (nbytes %d, max %d)",
+			ath10k_warn(ar, "rxed more than expected (nbytes %d, max %d)",
 				    nbytes, max_nbytes);
 			dev_kfree_skb_any(skb);
 			continue;
@@ -748,12 +864,7 @@ static void ath10k_pci_ce_recv_data(struct ath10k_ce_pipe *ce_state)
 		cb->rx_completion(ar, skb, pipe_info->pipe_num);
 	}
 
-	err = ath10k_pci_post_rx_pipe(pipe_info, num_replenish);
-	if (unlikely(err)) {
-		/* FIXME: retry */
-		ath10k_warn("failed to replenish CE rx ring %d (%d bufs): %d\n",
-			    pipe_info->pipe_num, num_replenish, err);
-	}
+	ath10k_pci_rx_post_pipe(pipe_info);
 }
 
 static int ath10k_pci_hif_tx_sg(struct ath10k *ar, u8 pipe_id,
@@ -781,10 +892,10 @@ static int ath10k_pci_hif_tx_sg(struct ath10k *ar, u8 pipe_id,
 	}
 
 	for (i = 0; i < n_items - 1; i++) {
-		ath10k_dbg(ATH10K_DBG_PCI,
+		ath10k_dbg(ar, ATH10K_DBG_PCI,
 			   "pci tx item %d paddr 0x%08x len %d n_items %d\n",
 			   i, items[i].paddr, items[i].len, n_items);
-		ath10k_dbg_dump(ATH10K_DBG_PCI_DUMP, NULL, "item data: ",
+		ath10k_dbg_dump(ar, ATH10K_DBG_PCI_DUMP, NULL, "pci tx data: ",
 				items[i].vaddr, items[i].len);
 
 		err = ath10k_ce_send_nolock(ce_pipe,
@@ -799,10 +910,10 @@ static int ath10k_pci_hif_tx_sg(struct ath10k *ar, u8 pipe_id,
 
 	/* `i` is equal to `n_items -1` after for() */
 
-	ath10k_dbg(ATH10K_DBG_PCI,
+	ath10k_dbg(ar, ATH10K_DBG_PCI,
 		   "pci tx item %d paddr 0x%08x len %d n_items %d\n",
 		   i, items[i].paddr, items[i].len, n_items);
-	ath10k_dbg_dump(ATH10K_DBG_PCI_DUMP, NULL, "item data: ",
+	ath10k_dbg_dump(ar, ATH10K_DBG_PCI_DUMP, NULL, "pci tx data: ",
 			items[i].vaddr, items[i].len);
 
 	err = ath10k_ce_send_nolock(ce_pipe,
@@ -829,52 +940,64 @@ static u16 ath10k_pci_hif_get_free_queue_number(struct ath10k *ar, u8 pipe)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 
-	ath10k_dbg(ATH10K_DBG_PCI, "pci hif get free queue number\n");
+	ath10k_dbg(ar, ATH10K_DBG_PCI, "pci hif get free queue number\n");
 
 	return ath10k_ce_num_free_src_entries(ar_pci->pipe_info[pipe].ce_hdl);
 }
 
-static void ath10k_pci_hif_dump_area(struct ath10k *ar)
+static void ath10k_pci_dump_registers(struct ath10k *ar,
+				      struct ath10k_fw_crash_data *crash_data)
 {
-	u32 reg_dump_area = 0;
-	u32 reg_dump_values[REG_DUMP_COUNT_QCA988X] = {};
-	u32 host_addr;
-	int ret;
-	u32 i;
+	__le32 reg_dump_values[REG_DUMP_COUNT_QCA988X] = {};
+	int i, ret;
 
-	ath10k_err("firmware crashed!\n");
-	ath10k_err("hardware name %s version 0x%x\n",
-		   ar->hw_params.name, ar->target_version);
-	ath10k_err("firmware version: %s\n", ar->hw->wiphy->fw_version);
+	lockdep_assert_held(&ar->data_lock);
 
-	host_addr = host_interest_item_address(HI_ITEM(hi_failure_state));
-	ret = ath10k_pci_diag_read_mem(ar, host_addr,
-				       &reg_dump_area, sizeof(u32));
+	ret = ath10k_pci_diag_read_hi(ar, &reg_dump_values[0],
+				      hi_failure_state,
+				      REG_DUMP_COUNT_QCA988X * sizeof(__le32));
 	if (ret) {
-		ath10k_err("failed to read FW dump area address: %d\n", ret);
-		return;
-	}
-
-	ath10k_err("target register Dump Location: 0x%08X\n", reg_dump_area);
-
-	ret = ath10k_pci_diag_read_mem(ar, reg_dump_area,
-				       &reg_dump_values[0],
-				       REG_DUMP_COUNT_QCA988X * sizeof(u32));
-	if (ret != 0) {
-		ath10k_err("failed to read FW dump area: %d\n", ret);
+		ath10k_err(ar, "failed to read firmware dump area: %d\n", ret);
 		return;
 	}
 
 	BUILD_BUG_ON(REG_DUMP_COUNT_QCA988X % 4);
 
-	ath10k_err("target Register Dump\n");
+	ath10k_err(ar, "firmware register dump:\n");
 	for (i = 0; i < REG_DUMP_COUNT_QCA988X; i += 4)
-		ath10k_err("[%02d]: 0x%08X 0x%08X 0x%08X 0x%08X\n",
+		ath10k_err(ar, "[%02d]: 0x%08X 0x%08X 0x%08X 0x%08X\n",
 			   i,
-			   reg_dump_values[i],
-			   reg_dump_values[i + 1],
-			   reg_dump_values[i + 2],
-			   reg_dump_values[i + 3]);
+			   __le32_to_cpu(reg_dump_values[i]),
+			   __le32_to_cpu(reg_dump_values[i + 1]),
+			   __le32_to_cpu(reg_dump_values[i + 2]),
+			   __le32_to_cpu(reg_dump_values[i + 3]));
+
+	if (!crash_data)
+		return;
+
+	for (i = 0; i < REG_DUMP_COUNT_QCA988X; i++)
+		crash_data->registers[i] = reg_dump_values[i];
+}
+
+static void ath10k_pci_fw_crashed_dump(struct ath10k *ar)
+{
+	struct ath10k_fw_crash_data *crash_data;
+	char uuid[50];
+
+	spin_lock_bh(&ar->data_lock);
+
+	crash_data = ath10k_debug_get_new_fw_crash_data(ar);
+
+	if (crash_data)
+		scnprintf(uuid, sizeof(uuid), "%pUl", &crash_data->uuid);
+	else
+		scnprintf(uuid, sizeof(uuid), "n/a");
+
+	ath10k_err(ar, "firmware crashed! (uuid %s)\n", uuid);
+	ath10k_print_driver_info(ar);
+	ath10k_pci_dump_registers(ar, crash_data);
+
+	spin_unlock_bh(&ar->data_lock);
 
 	queue_work(ar->workqueue, &ar->restart_work);
 }
@@ -882,7 +1005,7 @@ static void ath10k_pci_hif_dump_area(struct ath10k *ar)
 static void ath10k_pci_hif_send_complete_check(struct ath10k *ar, u8 pipe,
 					       int force)
 {
-	ath10k_dbg(ATH10K_DBG_PCI, "pci hif send complete check\n");
+	ath10k_dbg(ar, ATH10K_DBG_PCI, "pci hif send complete check\n");
 
 	if (!force) {
 		int resources;
@@ -910,43 +1033,12 @@ static void ath10k_pci_hif_set_callbacks(struct ath10k *ar,
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 
-	ath10k_dbg(ATH10K_DBG_PCI, "pci hif set callbacks\n");
+	ath10k_dbg(ar, ATH10K_DBG_PCI, "pci hif set callbacks\n");
 
 	memcpy(&ar_pci->msg_callbacks_current, callbacks,
 	       sizeof(ar_pci->msg_callbacks_current));
 }
 
-static int ath10k_pci_setup_ce_irq(struct ath10k *ar)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	const struct ce_attr *attr;
-	struct ath10k_pci_pipe *pipe_info;
-	int pipe_num, disable_interrupts;
-
-	for (pipe_num = 0; pipe_num < CE_COUNT; pipe_num++) {
-		pipe_info = &ar_pci->pipe_info[pipe_num];
-
-		/* Handle Diagnostic CE specially */
-		if (pipe_info->ce_hdl == ar_pci->ce_diag)
-			continue;
-
-		attr = &host_ce_config_wlan[pipe_num];
-
-		if (attr->src_nentries) {
-			disable_interrupts = attr->flags & CE_ATTR_DIS_INTR;
-			ath10k_ce_send_cb_register(pipe_info->ce_hdl,
-						   ath10k_pci_ce_send_done,
-						   disable_interrupts);
-		}
-
-		if (attr->dest_nentries)
-			ath10k_ce_recv_cb_register(pipe_info->ce_hdl,
-						   ath10k_pci_ce_recv_data);
-	}
-
-	return 0;
-}
-
 static void ath10k_pci_kill_tasklet(struct ath10k *ar)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
@@ -954,82 +1046,72 @@ static void ath10k_pci_kill_tasklet(struct ath10k *ar)
 
 	tasklet_kill(&ar_pci->intr_tq);
 	tasklet_kill(&ar_pci->msi_fw_err);
-	tasklet_kill(&ar_pci->early_irq_tasklet);
 
 	for (i = 0; i < CE_COUNT; i++)
 		tasklet_kill(&ar_pci->pipe_info[i].intr);
+
+	del_timer_sync(&ar_pci->rx_post_retry);
 }
 
-/* TODO - temporary mapping while we have too few CE's */
 static int ath10k_pci_hif_map_service_to_pipe(struct ath10k *ar,
 					      u16 service_id, u8 *ul_pipe,
 					      u8 *dl_pipe, int *ul_is_polled,
 					      int *dl_is_polled)
 {
-	int ret = 0;
+	const struct service_to_pipe *entry;
+	bool ul_set = false, dl_set = false;
+	int i;
 
-	ath10k_dbg(ATH10K_DBG_PCI, "pci hif map service\n");
+	ath10k_dbg(ar, ATH10K_DBG_PCI, "pci hif map service\n");
 
 	/* polling for received messages not supported */
 	*dl_is_polled = 0;
 
-	switch (service_id) {
-	case ATH10K_HTC_SVC_ID_HTT_DATA_MSG:
-		/*
-		 * Host->target HTT gets its own pipe, so it can be polled
-		 * while other pipes are interrupt driven.
-		 */
-		*ul_pipe = 4;
-		/*
-		 * Use the same target->host pipe for HTC ctrl, HTC raw
-		 * streams, and HTT.
-		 */
-		*dl_pipe = 1;
-		break;
-
-	case ATH10K_HTC_SVC_ID_RSVD_CTRL:
-	case ATH10K_HTC_SVC_ID_TEST_RAW_STREAMS:
-		/*
-		 * Note: HTC_RAW_STREAMS_SVC is currently unused, and
-		 * HTC_CTRL_RSVD_SVC could share the same pipe as the
-		 * WMI services.  So, if another CE is needed, change
-		 * this to *ul_pipe = 3, which frees up CE 0.
-		 */
-		/* *ul_pipe = 3; */
-		*ul_pipe = 0;
-		*dl_pipe = 1;
-		break;
+	for (i = 0; i < ARRAY_SIZE(target_service_to_ce_map_wlan); i++) {
+		entry = &target_service_to_ce_map_wlan[i];
 
-	case ATH10K_HTC_SVC_ID_WMI_DATA_BK:
-	case ATH10K_HTC_SVC_ID_WMI_DATA_BE:
-	case ATH10K_HTC_SVC_ID_WMI_DATA_VI:
-	case ATH10K_HTC_SVC_ID_WMI_DATA_VO:
+		if (__le32_to_cpu(entry->service_id) != service_id)
+			continue;
 
-	case ATH10K_HTC_SVC_ID_WMI_CONTROL:
-		*ul_pipe = 3;
-		*dl_pipe = 2;
-		break;
+		switch (__le32_to_cpu(entry->pipedir)) {
+		case PIPEDIR_NONE:
+			break;
+		case PIPEDIR_IN:
+			WARN_ON(dl_set);
+			*dl_pipe = __le32_to_cpu(entry->pipenum);
+			dl_set = true;
+			break;
+		case PIPEDIR_OUT:
+			WARN_ON(ul_set);
+			*ul_pipe = __le32_to_cpu(entry->pipenum);
+			ul_set = true;
+			break;
+		case PIPEDIR_INOUT:
+			WARN_ON(dl_set);
+			WARN_ON(ul_set);
+			*dl_pipe = __le32_to_cpu(entry->pipenum);
+			*ul_pipe = __le32_to_cpu(entry->pipenum);
+			dl_set = true;
+			ul_set = true;
+			break;
+		}
+	}
 
-		/* pipe 5 unused   */
-		/* pipe 6 reserved */
-		/* pipe 7 reserved */
+	if (WARN_ON(!ul_set || !dl_set))
+		return -ENOENT;
 
-	default:
-		ret = -1;
-		break;
-	}
 	*ul_is_polled =
 		(host_ce_config_wlan[*ul_pipe].flags & CE_ATTR_DIS_INTR) != 0;
 
-	return ret;
+	return 0;
 }
 
 static void ath10k_pci_hif_get_default_pipe(struct ath10k *ar,
-						u8 *ul_pipe, u8 *dl_pipe)
+					    u8 *ul_pipe, u8 *dl_pipe)
 {
 	int ul_is_polled, dl_is_polled;
 
-	ath10k_dbg(ATH10K_DBG_PCI, "pci hif get default pipe\n");
+	ath10k_dbg(ar, ATH10K_DBG_PCI, "pci hif get default pipe\n");
 
 	(void)ath10k_pci_hif_map_service_to_pipe(ar,
 						 ATH10K_HTC_SVC_ID_RSVD_CTRL,
@@ -1039,141 +1121,34 @@ static void ath10k_pci_hif_get_default_pipe(struct ath10k *ar,
 						 &dl_is_polled);
 }
 
-static int ath10k_pci_post_rx_pipe(struct ath10k_pci_pipe *pipe_info,
-				   int num)
+static void ath10k_pci_irq_disable(struct ath10k *ar)
 {
-	struct ath10k *ar = pipe_info->hif_ce_state;
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	struct ath10k_ce_pipe *ce_state = pipe_info->ce_hdl;
-	struct sk_buff *skb;
-	dma_addr_t ce_data;
-	int i, ret = 0;
-
-	if (pipe_info->buf_sz == 0)
-		return 0;
-
-	for (i = 0; i < num; i++) {
-		skb = dev_alloc_skb(pipe_info->buf_sz);
-		if (!skb) {
-			ath10k_warn("failed to allocate skbuff for pipe %d\n",
-				    num);
-			ret = -ENOMEM;
-			goto err;
-		}
-
-		WARN_ONCE((unsigned long)skb->data & 3, "unaligned skb");
-
-		ce_data = dma_map_single(ar->dev, skb->data,
-					 skb->len + skb_tailroom(skb),
-					 DMA_FROM_DEVICE);
-
-		if (unlikely(dma_mapping_error(ar->dev, ce_data))) {
-			ath10k_warn("failed to DMA map sk_buff\n");
-			dev_kfree_skb_any(skb);
-			ret = -EIO;
-			goto err;
-		}
-
-		ATH10K_SKB_CB(skb)->paddr = ce_data;
-
-		pci_dma_sync_single_for_device(ar_pci->pdev, ce_data,
-					       pipe_info->buf_sz,
-					       PCI_DMA_FROMDEVICE);
-
-		ret = ath10k_ce_recv_buf_enqueue(ce_state, (void *)skb,
-						 ce_data);
-		if (ret) {
-			ath10k_warn("failed to enqueue to pipe %d: %d\n",
-				    num, ret);
-			goto err;
-		}
-	}
+	int i;
 
-	return ret;
+	ath10k_ce_disable_interrupts(ar);
+	ath10k_pci_disable_and_clear_legacy_irq(ar);
+	/* FIXME: How to mask all MSI interrupts? */
 
-err:
-	ath10k_pci_rx_pipe_cleanup(pipe_info);
-	return ret;
+	for (i = 0; i < max(1, ar_pci->num_msi_intrs); i++)
+		synchronize_irq(ar_pci->pdev->irq + i);
 }
 
-static int ath10k_pci_post_rx(struct ath10k *ar)
+static void ath10k_pci_irq_enable(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	struct ath10k_pci_pipe *pipe_info;
-	const struct ce_attr *attr;
-	int pipe_num, ret = 0;
-
-	for (pipe_num = 0; pipe_num < CE_COUNT; pipe_num++) {
-		pipe_info = &ar_pci->pipe_info[pipe_num];
-		attr = &host_ce_config_wlan[pipe_num];
-
-		if (attr->dest_nentries == 0)
-			continue;
-
-		ret = ath10k_pci_post_rx_pipe(pipe_info,
-					      attr->dest_nentries - 1);
-		if (ret) {
-			ath10k_warn("failed to post RX buffer for pipe %d: %d\n",
-				    pipe_num, ret);
-
-			for (; pipe_num >= 0; pipe_num--) {
-				pipe_info = &ar_pci->pipe_info[pipe_num];
-				ath10k_pci_rx_pipe_cleanup(pipe_info);
-			}
-			return ret;
-		}
-	}
-
-	return 0;
+	ath10k_ce_enable_interrupts(ar);
+	ath10k_pci_enable_legacy_irq(ar);
+	/* FIXME: How to unmask all MSI interrupts? */
 }
 
 static int ath10k_pci_hif_start(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	int ret, ret_early;
-
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot hif start\n");
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot hif start\n");
 
-	ath10k_pci_free_early_irq(ar);
-	ath10k_pci_kill_tasklet(ar);
+	ath10k_pci_irq_enable(ar);
+	ath10k_pci_rx_post(ar);
 
-	ret = ath10k_pci_request_irq(ar);
-	if (ret) {
-		ath10k_warn("failed to post RX buffers for all pipes: %d\n",
-			    ret);
-		goto err_early_irq;
-	}
-
-	ret = ath10k_pci_setup_ce_irq(ar);
-	if (ret) {
-		ath10k_warn("failed to setup CE interrupts: %d\n", ret);
-		goto err_stop;
-	}
-
-	/* Post buffers once to start things off. */
-	ret = ath10k_pci_post_rx(ar);
-	if (ret) {
-		ath10k_warn("failed to post RX buffers for all pipes: %d\n",
-			    ret);
-		goto err_stop;
-	}
-
-	ar_pci->started = 1;
 	return 0;
-
-err_stop:
-	ath10k_ce_disable_interrupts(ar);
-	ath10k_pci_free_irq(ar);
-	ath10k_pci_kill_tasklet(ar);
-err_early_irq:
-	/* Though there should be no interrupts (device was reset)
-	 * power_down() expects the early IRQ to be installed as per the
-	 * driver lifecycle. */
-	ret_early = ath10k_pci_request_early_irq(ar);
-	if (ret_early)
-		ath10k_warn("failed to re-enable early irq: %d\n", ret_early);
-
-	return ret;
 }
 
 static void ath10k_pci_rx_pipe_cleanup(struct ath10k_pci_pipe *pipe_info)
@@ -1193,10 +1168,6 @@ static void ath10k_pci_rx_pipe_cleanup(struct ath10k_pci_pipe *pipe_info)
 
 	ar = pipe_info->hif_ce_state;
 	ar_pci = ath10k_pci_priv(ar);
-
-	if (!ar_pci->started)
-		return;
-
 	ce_hdl = pipe_info->ce_hdl;
 
 	while (ath10k_ce_revoke_recv_next(ce_hdl, (void **)&netbuf,
@@ -1227,10 +1198,6 @@ static void ath10k_pci_tx_pipe_cleanup(struct ath10k_pci_pipe *pipe_info)
 
 	ar = pipe_info->hif_ce_state;
 	ar_pci = ath10k_pci_priv(ar);
-
-	if (!ar_pci->started)
-		return;
-
 	ce_hdl = pipe_info->ce_hdl;
 
 	while (ath10k_ce_cancel_send_next(ce_hdl, (void **)&netbuf,
@@ -1275,41 +1242,31 @@ static void ath10k_pci_ce_deinit(struct ath10k *ar)
 		ath10k_ce_deinit_pipe(ar, i);
 }
 
-static void ath10k_pci_hif_stop(struct ath10k *ar)
+static void ath10k_pci_flush(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	int ret;
-
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot hif stop\n");
-
-	if (WARN_ON(!ar_pci->started))
-		return;
-
-	ret = ath10k_ce_disable_interrupts(ar);
-	if (ret)
-		ath10k_warn("failed to disable CE interrupts: %d\n", ret);
-
-	ath10k_pci_free_irq(ar);
 	ath10k_pci_kill_tasklet(ar);
-
-	ret = ath10k_pci_request_early_irq(ar);
-	if (ret)
-		ath10k_warn("failed to re-enable early irq: %d\n", ret);
-
-	/* At this point, asynchronous threads are stopped, the target should
-	 * not DMA nor interrupt. We process the leftovers and then free
-	 * everything else up. */
-
 	ath10k_pci_buffer_cleanup(ar);
+}
 
-	/* Make the sure the device won't access any structures on the host by
-	 * resetting it. The device was fed with PCI CE ringbuffer
-	 * configuration during init. If ringbuffers are freed and the device
-	 * were to access them this could lead to memory corruption on the
-	 * host. */
+static void ath10k_pci_hif_stop(struct ath10k *ar)
+{
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot hif stop\n");
+
+	/* Most likely the device has HTT Rx ring configured. The only way to
+	 * prevent the device from accessing (and possible corrupting) host
+	 * memory is to reset the chip now.
+	 *
+	 * There's also no known way of masking MSI interrupts on the device.
+	 * For ranged MSI the CE-related interrupts can be masked. However
+	 * regardless how many MSI interrupts are assigned the first one
+	 * is always used for firmware indications (crashes) and cannot be
+	 * masked. To prevent the device from asserting the interrupt reset it
+	 * before proceeding with cleanup.
+	 */
 	ath10k_pci_warm_reset(ar);
 
-	ar_pci->started = 0;
+	ath10k_pci_irq_disable(ar);
+	ath10k_pci_flush(ar);
 }
 
 static int ath10k_pci_hif_exchange_bmi_msg(struct ath10k *ar,
@@ -1360,7 +1317,7 @@ static int ath10k_pci_hif_exchange_bmi_msg(struct ath10k *ar,
 		xfer.wait_for_resp = true;
 		xfer.resp_len = 0;
 
-		ath10k_ce_recv_buf_enqueue(ce_rx, &xfer, resp_paddr);
+		ath10k_ce_rx_post_buf(ce_rx, &xfer, resp_paddr);
 	}
 
 	ret = ath10k_ce_send(ce_tx, &xfer, req_paddr, req_len, -1, 0);
@@ -1418,6 +1375,7 @@ static void ath10k_pci_bmi_send_done(struct ath10k_ce_pipe *ce_state)
 
 static void ath10k_pci_bmi_recv_data(struct ath10k_ce_pipe *ce_state)
 {
+	struct ath10k *ar = ce_state->ar;
 	struct bmi_xfer *xfer;
 	u32 ce_data;
 	unsigned int nbytes;
@@ -1429,7 +1387,7 @@ static void ath10k_pci_bmi_recv_data(struct ath10k_ce_pipe *ce_state)
 		return;
 
 	if (!xfer->wait_for_resp) {
-		ath10k_warn("unexpected: BMI data received; ignoring\n");
+		ath10k_warn(ar, "unexpected: BMI data received; ignoring\n");
 		return;
 	}
 
@@ -1457,129 +1415,17 @@ static int ath10k_pci_bmi_wait(struct ath10k_ce_pipe *tx_pipe,
 }
 
 /*
- * Map from service/endpoint to Copy Engine.
- * This table is derived from the CE_PCI TABLE, above.
- * It is passed to the Target at startup for use by firmware.
- */
-static const struct service_to_pipe target_service_to_ce_map_wlan[] = {
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_VO,
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 3,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_VO,
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 2,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_BK,
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 3,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_BK,
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 2,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_BE,
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 3,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_BE,
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 2,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_VI,
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 3,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_DATA_VI,
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 2,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_CONTROL,
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 3,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_WMI_CONTROL,
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 2,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_RSVD_CTRL,
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 0,		/* could be moved to 3 (share with WMI) */
-	},
-	{
-		 ATH10K_HTC_SVC_ID_RSVD_CTRL,
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 1,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_TEST_RAW_STREAMS,	/* not currently used */
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 0,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_TEST_RAW_STREAMS,	/* not currently used */
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 1,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_HTT_DATA_MSG,
-		 PIPEDIR_OUT,		/* out = UL = host -> target */
-		 4,
-	},
-	{
-		 ATH10K_HTC_SVC_ID_HTT_DATA_MSG,
-		 PIPEDIR_IN,		/* in = DL = target -> host */
-		 1,
-	},
-
-	/* (Additions here) */
-
-	{				/* Must be last */
-		 0,
-		 0,
-		 0,
-	},
-};
-
-/*
  * Send an interrupt to the device to wake up the Target CPU
  * so it has an opportunity to notice any changed state.
  */
 static int ath10k_pci_wake_target_cpu(struct ath10k *ar)
 {
-	int ret;
-	u32 core_ctrl;
+	u32 addr, val;
 
-	ret = ath10k_pci_diag_read_access(ar, SOC_CORE_BASE_ADDRESS |
-					      CORE_CTRL_ADDRESS,
-					  &core_ctrl);
-	if (ret) {
-		ath10k_warn("failed to read core_ctrl: %d\n", ret);
-		return ret;
-	}
-
-	/* A_INUM_FIRMWARE interrupt to Target CPU */
-	core_ctrl |= CORE_CTRL_CPU_INTR_MASK;
-
-	ret = ath10k_pci_diag_write_access(ar, SOC_CORE_BASE_ADDRESS |
-					       CORE_CTRL_ADDRESS,
-					   core_ctrl);
-	if (ret) {
-		ath10k_warn("failed to set target CPU interrupt mask: %d\n",
-			    ret);
-		return ret;
-	}
+	addr = SOC_CORE_BASE_ADDRESS | CORE_CTRL_ADDRESS;
+	val = ath10k_pci_read32(ar, addr);
+	val |= CORE_CTRL_CPU_INTR_MASK;
+	ath10k_pci_write32(ar, addr, val);
 
 	return 0;
 }
@@ -1602,92 +1448,92 @@ static int ath10k_pci_init_config(struct ath10k *ar)
 		host_interest_item_address(HI_ITEM(hi_interconnect_state));
 
 	/* Supply Target-side CE configuration */
-	ret = ath10k_pci_diag_read_access(ar, interconnect_targ_addr,
-					  &pcie_state_targ_addr);
+	ret = ath10k_pci_diag_read32(ar, interconnect_targ_addr,
+				     &pcie_state_targ_addr);
 	if (ret != 0) {
-		ath10k_err("Failed to get pcie state addr: %d\n", ret);
+		ath10k_err(ar, "Failed to get pcie state addr: %d\n", ret);
 		return ret;
 	}
 
 	if (pcie_state_targ_addr == 0) {
 		ret = -EIO;
-		ath10k_err("Invalid pcie state addr\n");
+		ath10k_err(ar, "Invalid pcie state addr\n");
 		return ret;
 	}
 
-	ret = ath10k_pci_diag_read_access(ar, pcie_state_targ_addr +
+	ret = ath10k_pci_diag_read32(ar, (pcie_state_targ_addr +
 					  offsetof(struct pcie_state,
-						   pipe_cfg_addr),
-					  &pipe_cfg_targ_addr);
+						   pipe_cfg_addr)),
+				     &pipe_cfg_targ_addr);
 	if (ret != 0) {
-		ath10k_err("Failed to get pipe cfg addr: %d\n", ret);
+		ath10k_err(ar, "Failed to get pipe cfg addr: %d\n", ret);
 		return ret;
 	}
 
 	if (pipe_cfg_targ_addr == 0) {
 		ret = -EIO;
-		ath10k_err("Invalid pipe cfg addr\n");
+		ath10k_err(ar, "Invalid pipe cfg addr\n");
 		return ret;
 	}
 
 	ret = ath10k_pci_diag_write_mem(ar, pipe_cfg_targ_addr,
-				 target_ce_config_wlan,
-				 sizeof(target_ce_config_wlan));
+					target_ce_config_wlan,
+					sizeof(target_ce_config_wlan));
 
 	if (ret != 0) {
-		ath10k_err("Failed to write pipe cfg: %d\n", ret);
+		ath10k_err(ar, "Failed to write pipe cfg: %d\n", ret);
 		return ret;
 	}
 
-	ret = ath10k_pci_diag_read_access(ar, pcie_state_targ_addr +
+	ret = ath10k_pci_diag_read32(ar, (pcie_state_targ_addr +
 					  offsetof(struct pcie_state,
-						   svc_to_pipe_map),
-					  &svc_to_pipe_map);
+						   svc_to_pipe_map)),
+				     &svc_to_pipe_map);
 	if (ret != 0) {
-		ath10k_err("Failed to get svc/pipe map: %d\n", ret);
+		ath10k_err(ar, "Failed to get svc/pipe map: %d\n", ret);
 		return ret;
 	}
 
 	if (svc_to_pipe_map == 0) {
 		ret = -EIO;
-		ath10k_err("Invalid svc_to_pipe map\n");
+		ath10k_err(ar, "Invalid svc_to_pipe map\n");
 		return ret;
 	}
 
 	ret = ath10k_pci_diag_write_mem(ar, svc_to_pipe_map,
-				 target_service_to_ce_map_wlan,
-				 sizeof(target_service_to_ce_map_wlan));
+					target_service_to_ce_map_wlan,
+					sizeof(target_service_to_ce_map_wlan));
 	if (ret != 0) {
-		ath10k_err("Failed to write svc/pipe map: %d\n", ret);
+		ath10k_err(ar, "Failed to write svc/pipe map: %d\n", ret);
 		return ret;
 	}
 
-	ret = ath10k_pci_diag_read_access(ar, pcie_state_targ_addr +
+	ret = ath10k_pci_diag_read32(ar, (pcie_state_targ_addr +
 					  offsetof(struct pcie_state,
-						   config_flags),
-					  &pcie_config_flags);
+						   config_flags)),
+				     &pcie_config_flags);
 	if (ret != 0) {
-		ath10k_err("Failed to get pcie config_flags: %d\n", ret);
+		ath10k_err(ar, "Failed to get pcie config_flags: %d\n", ret);
 		return ret;
 	}
 
 	pcie_config_flags &= ~PCIE_CONFIG_FLAG_ENABLE_L1;
 
-	ret = ath10k_pci_diag_write_mem(ar, pcie_state_targ_addr +
-				 offsetof(struct pcie_state, config_flags),
-				 &pcie_config_flags,
-				 sizeof(pcie_config_flags));
+	ret = ath10k_pci_diag_write32(ar, (pcie_state_targ_addr +
+					   offsetof(struct pcie_state,
+						    config_flags)),
+				      pcie_config_flags);
 	if (ret != 0) {
-		ath10k_err("Failed to write pcie config_flags: %d\n", ret);
+		ath10k_err(ar, "Failed to write pcie config_flags: %d\n", ret);
 		return ret;
 	}
 
 	/* configure early allocation */
 	ealloc_targ_addr = host_interest_item_address(HI_ITEM(hi_early_alloc));
 
-	ret = ath10k_pci_diag_read_access(ar, ealloc_targ_addr, &ealloc_value);
+	ret = ath10k_pci_diag_read32(ar, ealloc_targ_addr, &ealloc_value);
 	if (ret != 0) {
-		ath10k_err("Faile to get early alloc val: %d\n", ret);
+		ath10k_err(ar, "Faile to get early alloc val: %d\n", ret);
 		return ret;
 	}
 
@@ -1697,26 +1543,26 @@ static int ath10k_pci_init_config(struct ath10k *ar)
 	ealloc_value |= ((1 << HI_EARLY_ALLOC_IRAM_BANKS_SHIFT) &
 			 HI_EARLY_ALLOC_IRAM_BANKS_MASK);
 
-	ret = ath10k_pci_diag_write_access(ar, ealloc_targ_addr, ealloc_value);
+	ret = ath10k_pci_diag_write32(ar, ealloc_targ_addr, ealloc_value);
 	if (ret != 0) {
-		ath10k_err("Failed to set early alloc val: %d\n", ret);
+		ath10k_err(ar, "Failed to set early alloc val: %d\n", ret);
 		return ret;
 	}
 
 	/* Tell Target to proceed with initialization */
 	flag2_targ_addr = host_interest_item_address(HI_ITEM(hi_option_flag2));
 
-	ret = ath10k_pci_diag_read_access(ar, flag2_targ_addr, &flag2_value);
+	ret = ath10k_pci_diag_read32(ar, flag2_targ_addr, &flag2_value);
 	if (ret != 0) {
-		ath10k_err("Failed to get option val: %d\n", ret);
+		ath10k_err(ar, "Failed to get option val: %d\n", ret);
 		return ret;
 	}
 
 	flag2_value |= HI_OPTION_EARLY_CFG_DONE;
 
-	ret = ath10k_pci_diag_write_access(ar, flag2_targ_addr, flag2_value);
+	ret = ath10k_pci_diag_write32(ar, flag2_targ_addr, flag2_value);
 	if (ret != 0) {
-		ath10k_err("Failed to set option val: %d\n", ret);
+		ath10k_err(ar, "Failed to set option val: %d\n", ret);
 		return ret;
 	}
 
@@ -1730,7 +1576,7 @@ static int ath10k_pci_alloc_ce(struct ath10k *ar)
 	for (i = 0; i < CE_COUNT; i++) {
 		ret = ath10k_ce_alloc_pipe(ar, i, &host_ce_config_wlan[i]);
 		if (ret) {
-			ath10k_err("failed to allocate copy engine pipe %d: %d\n",
+			ath10k_err(ar, "failed to allocate copy engine pipe %d: %d\n",
 				   i, ret);
 			return ret;
 		}
@@ -1761,9 +1607,11 @@ static int ath10k_pci_ce_init(struct ath10k *ar)
 		pipe_info->hif_ce_state = ar;
 		attr = &host_ce_config_wlan[pipe_num];
 
-		ret = ath10k_ce_init_pipe(ar, pipe_num, attr);
+		ret = ath10k_ce_init_pipe(ar, pipe_num, attr,
+					  ath10k_pci_ce_send_done,
+					  ath10k_pci_ce_recv_data);
 		if (ret) {
-			ath10k_err("failed to initialize copy engine pipe %d: %d\n",
+			ath10k_err(ar, "failed to initialize copy engine pipe %d: %d\n",
 				   pipe_num, ret);
 			return ret;
 		}
@@ -1777,38 +1625,25 @@ static int ath10k_pci_ce_init(struct ath10k *ar)
 			continue;
 		}
 
-		pipe_info->buf_sz = (size_t) (attr->src_sz_max);
+		pipe_info->buf_sz = (size_t)(attr->src_sz_max);
 	}
 
 	return 0;
 }
 
-static void ath10k_pci_fw_interrupt_handler(struct ath10k *ar)
+static bool ath10k_pci_has_fw_crashed(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	u32 fw_indicator;
-
-	ath10k_pci_wake(ar);
-
-	fw_indicator = ath10k_pci_read32(ar, FW_INDICATOR_ADDRESS);
-
-	if (fw_indicator & FW_IND_EVENT_PENDING) {
-		/* ACK: clear Target-side pending event */
-		ath10k_pci_write32(ar, FW_INDICATOR_ADDRESS,
-				   fw_indicator & ~FW_IND_EVENT_PENDING);
+	return ath10k_pci_read32(ar, FW_INDICATOR_ADDRESS) &
+	       FW_IND_EVENT_PENDING;
+}
 
-		if (ar_pci->started) {
-			ath10k_pci_hif_dump_area(ar);
-		} else {
-			/*
-			 * Probable Target failure before we're prepared
-			 * to handle it.  Generally unexpected.
-			 */
-			ath10k_warn("early firmware event indicated\n");
-		}
-	}
+static void ath10k_pci_fw_crashed_clear(struct ath10k *ar)
+{
+	u32 val;
 
-	ath10k_pci_sleep(ar);
+	val = ath10k_pci_read32(ar, FW_INDICATOR_ADDRESS);
+	val &= ~FW_IND_EVENT_PENDING;
+	ath10k_pci_write32(ar, FW_INDICATOR_ADDRESS, val);
 }
 
 /* this function effectively clears target memory controller assert line */
@@ -1833,25 +1668,19 @@ static void ath10k_pci_warm_reset_si0(struct ath10k *ar)
 
 static int ath10k_pci_warm_reset(struct ath10k *ar)
 {
-	int ret = 0;
 	u32 val;
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot warm reset\n");
-
-	ret = ath10k_do_pci_wake(ar);
-	if (ret) {
-		ath10k_err("failed to wake up target: %d\n", ret);
-		return ret;
-	}
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot warm reset\n");
 
 	/* debug */
 	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
 				PCIE_INTR_CAUSE_ADDRESS);
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot host cpu intr cause: 0x%08x\n", val);
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot host cpu intr cause: 0x%08x\n",
+		   val);
 
 	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
 				CPU_INTR_ADDRESS);
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot target cpu intr cause: 0x%08x\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot target cpu intr cause: 0x%08x\n",
 		   val);
 
 	/* disable pending irqs */
@@ -1894,11 +1723,12 @@ static int ath10k_pci_warm_reset(struct ath10k *ar)
 	/* debug */
 	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
 				PCIE_INTR_CAUSE_ADDRESS);
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot host cpu intr cause: 0x%08x\n", val);
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot host cpu intr cause: 0x%08x\n",
+		   val);
 
 	val = ath10k_pci_read32(ar, SOC_CORE_BASE_ADDRESS +
 				CPU_INTR_ADDRESS);
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot target cpu intr cause: 0x%08x\n",
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot target cpu intr cause: 0x%08x\n",
 		   val);
 
 	/* CPU warm reset */
@@ -1909,20 +1739,18 @@ static int ath10k_pci_warm_reset(struct ath10k *ar)
 
 	val = ath10k_pci_read32(ar, RTC_SOC_BASE_ADDRESS +
 				SOC_RESET_CONTROL_ADDRESS);
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot target reset state: 0x%08x\n", val);
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot target reset state: 0x%08x\n",
+		   val);
 
 	msleep(100);
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot warm reset complete\n");
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot warm reset complete\n");
 
-	ath10k_do_pci_sleep(ar);
-	return ret;
+	return 0;
 }
 
 static int __ath10k_pci_hif_power_up(struct ath10k *ar, bool cold_reset)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	const char *irq_mode;
 	int ret;
 
 	/*
@@ -1941,80 +1769,39 @@ static int __ath10k_pci_hif_power_up(struct ath10k *ar, bool cold_reset)
 		ret = ath10k_pci_warm_reset(ar);
 
 	if (ret) {
-		ath10k_err("failed to reset target: %d\n", ret);
+		ath10k_err(ar, "failed to reset target: %d\n", ret);
 		goto err;
 	}
 
-	if (!test_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features))
-		/* Force AWAKE forever */
-		ath10k_do_pci_wake(ar);
-
 	ret = ath10k_pci_ce_init(ar);
 	if (ret) {
-		ath10k_err("failed to initialize CE: %d\n", ret);
-		goto err_ps;
-	}
-
-	ret = ath10k_ce_disable_interrupts(ar);
-	if (ret) {
-		ath10k_err("failed to disable CE interrupts: %d\n", ret);
-		goto err_ce;
-	}
-
-	ret = ath10k_pci_init_irq(ar);
-	if (ret) {
-		ath10k_err("failed to init irqs: %d\n", ret);
-		goto err_ce;
-	}
-
-	ret = ath10k_pci_request_early_irq(ar);
-	if (ret) {
-		ath10k_err("failed to request early irq: %d\n", ret);
-		goto err_deinit_irq;
+		ath10k_err(ar, "failed to initialize CE: %d\n", ret);
+		goto err;
 	}
 
 	ret = ath10k_pci_wait_for_target_init(ar);
 	if (ret) {
-		ath10k_err("failed to wait for target to init: %d\n", ret);
-		goto err_free_early_irq;
+		ath10k_err(ar, "failed to wait for target to init: %d\n", ret);
+		goto err_ce;
 	}
 
 	ret = ath10k_pci_init_config(ar);
 	if (ret) {
-		ath10k_err("failed to setup init config: %d\n", ret);
-		goto err_free_early_irq;
+		ath10k_err(ar, "failed to setup init config: %d\n", ret);
+		goto err_ce;
 	}
 
 	ret = ath10k_pci_wake_target_cpu(ar);
 	if (ret) {
-		ath10k_err("could not wake up target CPU: %d\n", ret);
-		goto err_free_early_irq;
+		ath10k_err(ar, "could not wake up target CPU: %d\n", ret);
+		goto err_ce;
 	}
 
-	if (ar_pci->num_msi_intrs > 1)
-		irq_mode = "MSI-X";
-	else if (ar_pci->num_msi_intrs == 1)
-		irq_mode = "MSI";
-	else
-		irq_mode = "legacy";
-
-	if (!test_bit(ATH10K_FLAG_FIRST_BOOT_DONE, &ar->dev_flags))
-		ath10k_info("pci irq %s irq_mode %d reset_mode %d\n",
-			    irq_mode, ath10k_pci_irq_mode,
-			    ath10k_pci_reset_mode);
-
 	return 0;
 
-err_free_early_irq:
-	ath10k_pci_free_early_irq(ar);
-err_deinit_irq:
-	ath10k_pci_deinit_irq(ar);
 err_ce:
 	ath10k_pci_ce_deinit(ar);
 	ath10k_pci_warm_reset(ar);
-err_ps:
-	if (!test_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features))
-		ath10k_do_pci_sleep(ar);
 err:
 	return ret;
 }
@@ -2034,7 +1821,7 @@ static int ath10k_pci_hif_power_up_warm(struct ath10k *ar)
 		if (ret == 0)
 			break;
 
-		ath10k_warn("failed to warm reset (attempt %d out of %d): %d\n",
+		ath10k_warn(ar, "failed to warm reset (attempt %d out of %d): %d\n",
 			    i + 1, ATH10K_PCI_NUM_WARM_RESET_ATTEMPTS, ret);
 	}
 
@@ -2045,7 +1832,7 @@ static int ath10k_pci_hif_power_up(struct ath10k *ar)
 {
 	int ret;
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot hif power up\n");
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot hif power up\n");
 
 	/*
 	 * Hardware CUS232 version 2 has some issues with cold reset and the
@@ -2057,17 +1844,17 @@ static int ath10k_pci_hif_power_up(struct ath10k *ar)
 	 */
 	ret = ath10k_pci_hif_power_up_warm(ar);
 	if (ret) {
-		ath10k_warn("failed to power up target using warm reset: %d\n",
+		ath10k_warn(ar, "failed to power up target using warm reset: %d\n",
 			    ret);
 
 		if (ath10k_pci_reset_mode == ATH10K_PCI_RESET_WARM_ONLY)
 			return ret;
 
-		ath10k_warn("trying cold reset\n");
+		ath10k_warn(ar, "trying cold reset\n");
 
 		ret = __ath10k_pci_hif_power_up(ar, true);
 		if (ret) {
-			ath10k_err("failed to power up target using cold reset too (%d)\n",
+			ath10k_err(ar, "failed to power up target using cold reset too (%d)\n",
 				   ret);
 			return ret;
 		}
@@ -2078,18 +1865,9 @@ static int ath10k_pci_hif_power_up(struct ath10k *ar)
 
 static void ath10k_pci_hif_power_down(struct ath10k *ar)
 {
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot hif power down\n");
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot hif power down\n");
 
-	ath10k_pci_free_early_irq(ar);
-	ath10k_pci_kill_tasklet(ar);
-	ath10k_pci_deinit_irq(ar);
-	ath10k_pci_ce_deinit(ar);
 	ath10k_pci_warm_reset(ar);
-
-	if (!test_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features))
-		ath10k_do_pci_sleep(ar);
 }
 
 #ifdef CONFIG_PM
@@ -2171,7 +1949,13 @@ static void ath10k_msi_err_tasklet(unsigned long data)
 {
 	struct ath10k *ar = (struct ath10k *)data;
 
-	ath10k_pci_fw_interrupt_handler(ar);
+	if (!ath10k_pci_has_fw_crashed(ar)) {
+		ath10k_warn(ar, "received unsolicited fw crash interrupt\n");
+		return;
+	}
+
+	ath10k_pci_fw_crashed_clear(ar);
+	ath10k_pci_fw_crashed_dump(ar);
 }
 
 /*
@@ -2185,7 +1969,8 @@ static irqreturn_t ath10k_pci_per_engine_handler(int irq, void *arg)
 	int ce_id = irq - ar_pci->pdev->irq - MSI_ASSIGN_CE_INITIAL;
 
 	if (ce_id < 0 || ce_id >= ARRAY_SIZE(ar_pci->pipe_info)) {
-		ath10k_warn("unexpected/invalid irq %d ce_id %d\n", irq, ce_id);
+		ath10k_warn(ar, "unexpected/invalid irq %d ce_id %d\n", irq,
+			    ce_id);
 		return IRQ_HANDLED;
 	}
 
@@ -2232,36 +2017,17 @@ static irqreturn_t ath10k_pci_interrupt_handler(int irq, void *arg)
 	return IRQ_HANDLED;
 }
 
-static void ath10k_pci_early_irq_tasklet(unsigned long data)
+static void ath10k_pci_tasklet(unsigned long data)
 {
 	struct ath10k *ar = (struct ath10k *)data;
-	u32 fw_ind;
-	int ret;
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 
-	ret = ath10k_pci_wake(ar);
-	if (ret) {
-		ath10k_warn("failed to wake target in early irq tasklet: %d\n",
-			    ret);
+	if (ath10k_pci_has_fw_crashed(ar)) {
+		ath10k_pci_fw_crashed_clear(ar);
+		ath10k_pci_fw_crashed_dump(ar);
 		return;
 	}
 
-	fw_ind = ath10k_pci_read32(ar, FW_INDICATOR_ADDRESS);
-	if (fw_ind & FW_IND_EVENT_PENDING) {
-		ath10k_pci_write32(ar, FW_INDICATOR_ADDRESS,
-				   fw_ind & ~FW_IND_EVENT_PENDING);
-		ath10k_pci_hif_dump_area(ar);
-	}
-
-	ath10k_pci_sleep(ar);
-	ath10k_pci_enable_legacy_irq(ar);
-}
-
-static void ath10k_pci_tasklet(unsigned long data)
-{
-	struct ath10k *ar = (struct ath10k *)data;
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	ath10k_pci_fw_interrupt_handler(ar); /* FIXME: Handle FW error */
 	ath10k_ce_per_engine_service_any(ar);
 
 	/* Re-enable legacy irq that was disabled in the irq handler */
@@ -2278,7 +2044,7 @@ static int ath10k_pci_request_irq_msix(struct ath10k *ar)
 			  ath10k_pci_msi_fw_handler,
 			  IRQF_SHARED, "ath10k_pci", ar);
 	if (ret) {
-		ath10k_warn("failed to request MSI-X fw irq %d: %d\n",
+		ath10k_warn(ar, "failed to request MSI-X fw irq %d: %d\n",
 			    ar_pci->pdev->irq + MSI_ASSIGN_FW, ret);
 		return ret;
 	}
@@ -2288,7 +2054,7 @@ static int ath10k_pci_request_irq_msix(struct ath10k *ar)
 				  ath10k_pci_per_engine_handler,
 				  IRQF_SHARED, "ath10k_pci", ar);
 		if (ret) {
-			ath10k_warn("failed to request MSI-X ce irq %d: %d\n",
+			ath10k_warn(ar, "failed to request MSI-X ce irq %d: %d\n",
 				    ar_pci->pdev->irq + i, ret);
 
 			for (i--; i >= MSI_ASSIGN_CE_INITIAL; i--)
@@ -2311,7 +2077,7 @@ static int ath10k_pci_request_irq_msi(struct ath10k *ar)
 			  ath10k_pci_interrupt_handler,
 			  IRQF_SHARED, "ath10k_pci", ar);
 	if (ret) {
-		ath10k_warn("failed to request MSI irq %d: %d\n",
+		ath10k_warn(ar, "failed to request MSI irq %d: %d\n",
 			    ar_pci->pdev->irq, ret);
 		return ret;
 	}
@@ -2328,7 +2094,7 @@ static int ath10k_pci_request_irq_legacy(struct ath10k *ar)
 			  ath10k_pci_interrupt_handler,
 			  IRQF_SHARED, "ath10k_pci", ar);
 	if (ret) {
-		ath10k_warn("failed to request legacy irq %d: %d\n",
+		ath10k_warn(ar, "failed to request legacy irq %d: %d\n",
 			    ar_pci->pdev->irq, ret);
 		return ret;
 	}
@@ -2349,7 +2115,7 @@ static int ath10k_pci_request_irq(struct ath10k *ar)
 		return ath10k_pci_request_irq_msix(ar);
 	}
 
-	ath10k_warn("unknown irq configuration upon request\n");
+	ath10k_warn(ar, "unknown irq configuration upon request\n");
 	return -EINVAL;
 }
 
@@ -2372,8 +2138,6 @@ static void ath10k_pci_init_irq_tasklets(struct ath10k *ar)
 	tasklet_init(&ar_pci->intr_tq, ath10k_pci_tasklet, (unsigned long)ar);
 	tasklet_init(&ar_pci->msi_fw_err, ath10k_msi_err_tasklet,
 		     (unsigned long)ar);
-	tasklet_init(&ar_pci->early_irq_tasklet, ath10k_pci_early_irq_tasklet,
-		     (unsigned long)ar);
 
 	for (i = 0; i < CE_COUNT; i++) {
 		ar_pci->pipe_info[i].ar_pci = ar_pci;
@@ -2385,21 +2149,19 @@ static void ath10k_pci_init_irq_tasklets(struct ath10k *ar)
 static int ath10k_pci_init_irq(struct ath10k *ar)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-	bool msix_supported = test_bit(ATH10K_PCI_FEATURE_MSI_X,
-				       ar_pci->features);
 	int ret;
 
 	ath10k_pci_init_irq_tasklets(ar);
 
-	if (ath10k_pci_irq_mode != ATH10K_PCI_IRQ_AUTO &&
-	    !test_bit(ATH10K_FLAG_FIRST_BOOT_DONE, &ar->dev_flags))
-		ath10k_info("limiting irq mode to: %d\n", ath10k_pci_irq_mode);
+	if (ath10k_pci_irq_mode != ATH10K_PCI_IRQ_AUTO)
+		ath10k_info(ar, "limiting irq mode to: %d\n",
+			    ath10k_pci_irq_mode);
 
 	/* Try MSI-X */
-	if (ath10k_pci_irq_mode == ATH10K_PCI_IRQ_AUTO && msix_supported) {
+	if (ath10k_pci_irq_mode == ATH10K_PCI_IRQ_AUTO) {
 		ar_pci->num_msi_intrs = MSI_NUM_REQUEST;
 		ret = pci_enable_msi_range(ar_pci->pdev, ar_pci->num_msi_intrs,
-							 ar_pci->num_msi_intrs);
+					   ar_pci->num_msi_intrs);
 		if (ret > 0)
 			return 0;
 
@@ -2426,34 +2188,16 @@ static int ath10k_pci_init_irq(struct ath10k *ar)
 	 * synchronization checking. */
 	ar_pci->num_msi_intrs = 0;
 
-	ret = ath10k_pci_wake(ar);
-	if (ret) {
-		ath10k_warn("failed to wake target: %d\n", ret);
-		return ret;
-	}
-
 	ath10k_pci_write32(ar, SOC_CORE_BASE_ADDRESS + PCIE_INTR_ENABLE_ADDRESS,
 			   PCIE_INTR_FIRMWARE_MASK | PCIE_INTR_CE_MASK_ALL);
-	ath10k_pci_sleep(ar);
 
 	return 0;
 }
 
-static int ath10k_pci_deinit_irq_legacy(struct ath10k *ar)
+static void ath10k_pci_deinit_irq_legacy(struct ath10k *ar)
 {
-	int ret;
-
-	ret = ath10k_pci_wake(ar);
-	if (ret) {
-		ath10k_warn("failed to wake target: %d\n", ret);
-		return ret;
-	}
-
 	ath10k_pci_write32(ar, SOC_CORE_BASE_ADDRESS + PCIE_INTR_ENABLE_ADDRESS,
 			   0);
-	ath10k_pci_sleep(ar);
-
-	return 0;
 }
 
 static int ath10k_pci_deinit_irq(struct ath10k *ar)
@@ -2462,7 +2206,8 @@ static int ath10k_pci_deinit_irq(struct ath10k *ar)
 
 	switch (ar_pci->num_msi_intrs) {
 	case 0:
-		return ath10k_pci_deinit_irq_legacy(ar);
+		ath10k_pci_deinit_irq_legacy(ar);
+		return 0;
 	case 1:
 		/* fall-through */
 	case MSI_NUM_REQUEST:
@@ -2472,7 +2217,7 @@ static int ath10k_pci_deinit_irq(struct ath10k *ar)
 		pci_disable_msi(ar_pci->pdev);
 	}
 
-	ath10k_warn("unknown irq configuration upon deinit\n");
+	ath10k_warn(ar, "unknown irq configuration upon deinit\n");
 	return -EINVAL;
 }
 
@@ -2480,23 +2225,17 @@ static int ath10k_pci_wait_for_target_init(struct ath10k *ar)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 	unsigned long timeout;
-	int ret;
 	u32 val;
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot waiting target to initialise\n");
-
-	ret = ath10k_pci_wake(ar);
-	if (ret) {
-		ath10k_err("failed to wake up target for init: %d\n", ret);
-		return ret;
-	}
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot waiting target to initialise\n");
 
 	timeout = jiffies + msecs_to_jiffies(ATH10K_PCI_TARGET_WAIT);
 
 	do {
 		val = ath10k_pci_read32(ar, FW_INDICATOR_ADDRESS);
 
-		ath10k_dbg(ATH10K_DBG_BOOT, "boot target indicator %x\n", val);
+		ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot target indicator %x\n",
+			   val);
 
 		/* target should never return this */
 		if (val == 0xffffffff)
@@ -2511,55 +2250,42 @@ static int ath10k_pci_wait_for_target_init(struct ath10k *ar)
 
 		if (ar_pci->num_msi_intrs == 0)
 			/* Fix potential race by repeating CORE_BASE writes */
-			ath10k_pci_soc_write32(ar, PCIE_INTR_ENABLE_ADDRESS,
-					       PCIE_INTR_FIRMWARE_MASK |
-					       PCIE_INTR_CE_MASK_ALL);
+			ath10k_pci_write32(ar, SOC_CORE_BASE_ADDRESS +
+					   PCIE_INTR_ENABLE_ADDRESS,
+					   PCIE_INTR_FIRMWARE_MASK |
+					   PCIE_INTR_CE_MASK_ALL);
 
 		mdelay(10);
 	} while (time_before(jiffies, timeout));
 
 	if (val == 0xffffffff) {
-		ath10k_err("failed to read device register, device is gone\n");
-		ret = -EIO;
-		goto out;
+		ath10k_err(ar, "failed to read device register, device is gone\n");
+		return -EIO;
 	}
 
 	if (val & FW_IND_EVENT_PENDING) {
-		ath10k_warn("device has crashed during init\n");
-		ath10k_pci_write32(ar, FW_INDICATOR_ADDRESS,
-				   val & ~FW_IND_EVENT_PENDING);
-		ath10k_pci_hif_dump_area(ar);
-		ret = -ECOMM;
-		goto out;
+		ath10k_warn(ar, "device has crashed during init\n");
+		ath10k_pci_fw_crashed_clear(ar);
+		ath10k_pci_fw_crashed_dump(ar);
+		return -ECOMM;
 	}
 
 	if (!(val & FW_IND_INITIALIZED)) {
-		ath10k_err("failed to receive initialized event from target: %08x\n",
+		ath10k_err(ar, "failed to receive initialized event from target: %08x\n",
 			   val);
-		ret = -ETIMEDOUT;
-		goto out;
+		return -ETIMEDOUT;
 	}
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot target initialised\n");
-
-out:
-	ath10k_pci_sleep(ar);
-	return ret;
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot target initialised\n");
+	return 0;
 }
 
 static int ath10k_pci_cold_reset(struct ath10k *ar)
 {
-	int i, ret;
+	int i;
 	u32 val;
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot cold reset\n");
-
-	ret = ath10k_do_pci_wake(ar);
-	if (ret) {
-		ath10k_err("failed to wake up target: %d\n",
-			   ret);
-		return ret;
-	}
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot cold reset\n");
 
 	/* Put Target, including PCIe, into RESET. */
 	val = ath10k_pci_reg_read32(ar, SOC_GLOBAL_RESET_ADDRESS);
@@ -2584,169 +2310,199 @@ static int ath10k_pci_cold_reset(struct ath10k *ar)
 		msleep(1);
 	}
 
-	ath10k_do_pci_sleep(ar);
-
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot cold reset complete\n");
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot cold reset complete\n");
 
 	return 0;
 }
 
-static void ath10k_pci_dump_features(struct ath10k_pci *ar_pci)
-{
-	int i;
-
-	for (i = 0; i < ATH10K_PCI_FEATURE_COUNT; i++) {
-		if (!test_bit(i, ar_pci->features))
-			continue;
-
-		switch (i) {
-		case ATH10K_PCI_FEATURE_MSI_X:
-			ath10k_dbg(ATH10K_DBG_BOOT, "device supports MSI-X\n");
-			break;
-		case ATH10K_PCI_FEATURE_SOC_POWER_SAVE:
-			ath10k_dbg(ATH10K_DBG_BOOT, "QCA98XX SoC power save enabled\n");
-			break;
-		}
-	}
-}
-
-static int ath10k_pci_probe(struct pci_dev *pdev,
-			    const struct pci_device_id *pci_dev)
+static int ath10k_pci_claim(struct ath10k *ar)
 {
-	void __iomem *mem;
-	int ret = 0;
-	struct ath10k *ar;
-	struct ath10k_pci *ar_pci;
-	u32 lcr_val, chip_id;
-
-	ath10k_dbg(ATH10K_DBG_PCI, "pci probe\n");
-
-	ar_pci = kzalloc(sizeof(*ar_pci), GFP_KERNEL);
-	if (ar_pci == NULL)
-		return -ENOMEM;
-
-	ar_pci->pdev = pdev;
-	ar_pci->dev = &pdev->dev;
-
-	switch (pci_dev->device) {
-	case QCA988X_2_0_DEVICE_ID:
-		set_bit(ATH10K_PCI_FEATURE_MSI_X, ar_pci->features);
-		break;
-	default:
-		ret = -ENODEV;
-		ath10k_err("Unknown device ID: %d\n", pci_dev->device);
-		goto err_ar_pci;
-	}
-
-	if (ath10k_pci_target_ps)
-		set_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features);
-
-	ath10k_pci_dump_features(ar_pci);
-
-	ar = ath10k_core_create(ar_pci, ar_pci->dev, &ath10k_pci_hif_ops);
-	if (!ar) {
-		ath10k_err("failed to create driver core\n");
-		ret = -EINVAL;
-		goto err_ar_pci;
-	}
-
-	ar_pci->ar = ar;
-	atomic_set(&ar_pci->keep_awake_count, 0);
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	struct pci_dev *pdev = ar_pci->pdev;
+	u32 lcr_val;
+	int ret;
 
 	pci_set_drvdata(pdev, ar);
 
 	ret = pci_enable_device(pdev);
 	if (ret) {
-		ath10k_err("failed to enable PCI device: %d\n", ret);
-		goto err_ar;
+		ath10k_err(ar, "failed to enable pci device: %d\n", ret);
+		return ret;
 	}
 
-	/* Request MMIO resources */
 	ret = pci_request_region(pdev, BAR_NUM, "ath");
 	if (ret) {
-		ath10k_err("failed to request MMIO region: %d\n", ret);
+		ath10k_err(ar, "failed to request region BAR%d: %d\n", BAR_NUM,
+			   ret);
 		goto err_device;
 	}
 
-	/*
-	 * Target structures have a limit of 32 bit DMA pointers.
-	 * DMA pointers can be wider than 32 bits by default on some systems.
-	 */
+	/* Target expects 32 bit DMA. Enforce it. */
 	ret = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		ath10k_err("failed to set DMA mask to 32-bit: %d\n", ret);
+		ath10k_err(ar, "failed to set dma mask to 32-bit: %d\n", ret);
 		goto err_region;
 	}
 
 	ret = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
 	if (ret) {
-		ath10k_err("failed to set consistent DMA mask to 32-bit\n");
+		ath10k_err(ar, "failed to set consistent dma mask to 32-bit: %d\n",
+			   ret);
 		goto err_region;
 	}
 
-	/* Set bus master bit in PCI_COMMAND to enable DMA */
 	pci_set_master(pdev);
 
-	/*
-	 * Temporary FIX: disable ASPM
-	 * Will be removed after the OTP is programmed
-	 */
+	/* Workaround: Disable ASPM */
 	pci_read_config_dword(pdev, 0x80, &lcr_val);
 	pci_write_config_dword(pdev, 0x80, (lcr_val & 0xffffff00));
 
 	/* Arrange for access to Target SoC registers. */
-	mem = pci_iomap(pdev, BAR_NUM, 0);
-	if (!mem) {
-		ath10k_err("failed to perform IOMAP for BAR%d\n", BAR_NUM);
+	ar_pci->mem = pci_iomap(pdev, BAR_NUM, 0);
+	if (!ar_pci->mem) {
+		ath10k_err(ar, "failed to iomap BAR%d\n", BAR_NUM);
 		ret = -EIO;
 		goto err_master;
 	}
 
-	ar_pci->mem = mem;
+	ath10k_dbg(ar, ATH10K_DBG_BOOT, "boot pci_mem 0x%p\n", ar_pci->mem);
+	return 0;
+
+err_master:
+	pci_clear_master(pdev);
+
+err_region:
+	pci_release_region(pdev, BAR_NUM);
+
+err_device:
+	pci_disable_device(pdev);
+
+	return ret;
+}
+
+static void ath10k_pci_release(struct ath10k *ar)
+{
+	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
+	struct pci_dev *pdev = ar_pci->pdev;
+
+	pci_iounmap(pdev, ar_pci->mem);
+	pci_release_region(pdev, BAR_NUM);
+	pci_clear_master(pdev);
+	pci_disable_device(pdev);
+}
+
+static int ath10k_pci_probe(struct pci_dev *pdev,
+			    const struct pci_device_id *pci_dev)
+{
+	int ret = 0;
+	struct ath10k *ar;
+	struct ath10k_pci *ar_pci;
+	u32 chip_id;
+
+	ar = ath10k_core_create(sizeof(*ar_pci), &pdev->dev,
+				&ath10k_pci_hif_ops);
+	if (!ar) {
+		dev_err(&pdev->dev, "failed to allocate core\n");
+		return -ENOMEM;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_PCI, "pci probe\n");
+
+	ar_pci = ath10k_pci_priv(ar);
+	ar_pci->pdev = pdev;
+	ar_pci->dev = &pdev->dev;
+	ar_pci->ar = ar;
 
 	spin_lock_init(&ar_pci->ce_lock);
+	setup_timer(&ar_pci->rx_post_retry, ath10k_pci_rx_replenish_retry,
+		    (unsigned long)ar);
 
-	ret = ath10k_do_pci_wake(ar);
+	ret = ath10k_pci_claim(ar);
 	if (ret) {
-		ath10k_err("Failed to get chip id: %d\n", ret);
-		goto err_iomap;
+		ath10k_err(ar, "failed to claim device: %d\n", ret);
+		goto err_core_destroy;
 	}
 
-	chip_id = ath10k_pci_soc_read32(ar, SOC_CHIP_ID_ADDRESS);
+	ret = ath10k_pci_wake(ar);
+	if (ret) {
+		ath10k_err(ar, "failed to wake up: %d\n", ret);
+		goto err_release;
+	}
 
-	ath10k_do_pci_sleep(ar);
+	chip_id = ath10k_pci_soc_read32(ar, SOC_CHIP_ID_ADDRESS);
+	if (chip_id == 0xffffffff) {
+		ath10k_err(ar, "failed to get chip id\n");
+		goto err_sleep;
+	}
 
 	ret = ath10k_pci_alloc_ce(ar);
 	if (ret) {
-		ath10k_err("failed to allocate copy engine pipes: %d\n", ret);
-		goto err_iomap;
+		ath10k_err(ar, "failed to allocate copy engine pipes: %d\n",
+			   ret);
+		goto err_sleep;
 	}
 
-	ath10k_dbg(ATH10K_DBG_BOOT, "boot pci_mem 0x%p\n", ar_pci->mem);
+	ath10k_pci_ce_deinit(ar);
 
-	ret = ath10k_core_register(ar, chip_id);
+	ret = ath10k_ce_disable_interrupts(ar);
 	if (ret) {
-		ath10k_err("failed to register driver core: %d\n", ret);
+		ath10k_err(ar, "failed to disable copy engine interrupts: %d\n",
+			   ret);
 		goto err_free_ce;
 	}
 
+	/* Workaround: There's no known way to mask all possible interrupts via
+	 * device CSR. The only way to make sure device doesn't assert
+	 * interrupts is to reset it. Interrupts are then disabled on host
+	 * after handlers are registered.
+	 */
+	ath10k_pci_warm_reset(ar);
+
+	ret = ath10k_pci_init_irq(ar);
+	if (ret) {
+		ath10k_err(ar, "failed to init irqs: %d\n", ret);
+		goto err_free_ce;
+	}
+
+	ath10k_info(ar, "pci irq %s interrupts %d irq_mode %d reset_mode %d\n",
+		    ath10k_pci_get_irq_method(ar), ar_pci->num_msi_intrs,
+		    ath10k_pci_irq_mode, ath10k_pci_reset_mode);
+
+	ret = ath10k_pci_request_irq(ar);
+	if (ret) {
+		ath10k_warn(ar, "failed to request irqs: %d\n", ret);
+		goto err_deinit_irq;
+	}
+
+	/* This shouldn't race as the device has been reset above. */
+	ath10k_pci_irq_disable(ar);
+
+	ret = ath10k_core_register(ar, chip_id);
+	if (ret) {
+		ath10k_err(ar, "failed to register driver core: %d\n", ret);
+		goto err_free_irq;
+	}
+
 	return 0;
 
+err_free_irq:
+	ath10k_pci_free_irq(ar);
+	ath10k_pci_kill_tasklet(ar);
+
+err_deinit_irq:
+	ath10k_pci_deinit_irq(ar);
+
 err_free_ce:
 	ath10k_pci_free_ce(ar);
-err_iomap:
-	pci_iounmap(pdev, mem);
-err_master:
-	pci_clear_master(pdev);
-err_region:
-	pci_release_region(pdev, BAR_NUM);
-err_device:
-	pci_disable_device(pdev);
-err_ar:
+
+err_sleep:
+	ath10k_pci_sleep(ar);
+
+err_release:
+	ath10k_pci_release(ar);
+
+err_core_destroy:
 	ath10k_core_destroy(ar);
-err_ar_pci:
-	/* call HIF PCI free here */
-	kfree(ar_pci);
 
 	return ret;
 }
@@ -2756,7 +2512,7 @@ static void ath10k_pci_remove(struct pci_dev *pdev)
 	struct ath10k *ar = pci_get_drvdata(pdev);
 	struct ath10k_pci *ar_pci;
 
-	ath10k_dbg(ATH10K_DBG_PCI, "pci remove\n");
+	ath10k_dbg(ar, ATH10K_DBG_PCI, "pci remove\n");
 
 	if (!ar)
 		return;
@@ -2767,15 +2523,14 @@ static void ath10k_pci_remove(struct pci_dev *pdev)
 		return;
 
 	ath10k_core_unregister(ar);
+	ath10k_pci_free_irq(ar);
+	ath10k_pci_kill_tasklet(ar);
+	ath10k_pci_deinit_irq(ar);
+	ath10k_pci_ce_deinit(ar);
 	ath10k_pci_free_ce(ar);
-
-	pci_iounmap(pdev, ar_pci->mem);
-	pci_release_region(pdev, BAR_NUM);
-	pci_clear_master(pdev);
-	pci_disable_device(pdev);
-
+	ath10k_pci_sleep(ar);
+	ath10k_pci_release(ar);
 	ath10k_core_destroy(ar);
-	kfree(ar_pci);
 }
 
 MODULE_DEVICE_TABLE(pci, ath10k_pci_id_table);
@@ -2793,7 +2548,8 @@ static int __init ath10k_pci_init(void)
 
 	ret = pci_register_driver(&ath10k_pci_driver);
 	if (ret)
-		ath10k_err("failed to register PCI driver: %d\n", ret);
+		printk(KERN_ERR "failed to register ath10k pci driver: %d\n",
+		       ret);
 
 	return ret;
 }
@@ -2809,5 +2565,5 @@ module_exit(ath10k_pci_exit);
 MODULE_AUTHOR("Qualcomm Atheros");
 MODULE_DESCRIPTION("Driver support for Atheros QCA988X PCIe devices");
 MODULE_LICENSE("Dual BSD/GPL");
-MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" QCA988X_HW_2_0_FW_2_FILE);
+MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" QCA988X_HW_2_0_FW_3_FILE);
 MODULE_FIRMWARE(QCA988X_HW_2_0_FW_DIR "/" QCA988X_HW_2_0_BOARD_DATA_FILE);
diff --git a/drivers/net/wireless/ath/ath10k/pci.h b/drivers/net/wireless/ath/ath10k/pci.h
index 940129209990..cf36511c7f4d 100644
--- a/drivers/net/wireless/ath/ath10k/pci.h
+++ b/drivers/net/wireless/ath/ath10k/pci.h
@@ -23,9 +23,6 @@
 #include "hw.h"
 #include "ce.h"
 
-/* FW dump area */
-#define REG_DUMP_COUNT_QCA988X 60
-
 /*
  * maximum number of bytes that can be handled atomically by DiagRead/DiagWrite
  */
@@ -103,12 +100,12 @@ struct pcie_state {
  * NOTE: Structure is shared between Host software and Target firmware!
  */
 struct ce_pipe_config {
-	u32 pipenum;
-	u32 pipedir;
-	u32 nentries;
-	u32 nbytes_max;
-	u32 flags;
-	u32 reserved;
+	__le32 pipenum;
+	__le32 pipedir;
+	__le32 nentries;
+	__le32 nbytes_max;
+	__le32 flags;
+	__le32 reserved;
 };
 
 /*
@@ -130,17 +127,9 @@ struct ce_pipe_config {
 
 /* Establish a mapping between a service/direction and a pipe. */
 struct service_to_pipe {
-	u32 service_id;
-	u32 pipedir;
-	u32 pipenum;
-};
-
-enum ath10k_pci_features {
-	ATH10K_PCI_FEATURE_MSI_X		= 0,
-	ATH10K_PCI_FEATURE_SOC_POWER_SAVE	= 1,
-
-	/* keep last */
-	ATH10K_PCI_FEATURE_COUNT
+	__le32 service_id;
+	__le32 pipedir;
+	__le32 pipenum;
 };
 
 /* Per-pipe state. */
@@ -169,8 +158,6 @@ struct ath10k_pci {
 	struct ath10k *ar;
 	void __iomem *mem;
 
-	DECLARE_BITMAP(features, ATH10K_PCI_FEATURE_COUNT);
-
 	/*
 	 * Number of MSI interrupts granted, 0 --> using legacy PCI line
 	 * interrupts.
@@ -179,12 +166,6 @@ struct ath10k_pci {
 
 	struct tasklet_struct intr_tq;
 	struct tasklet_struct msi_fw_err;
-	struct tasklet_struct early_irq_tasklet;
-
-	int started;
-
-	atomic_t keep_awake_count;
-	bool verified_awake;
 
 	struct ath10k_pci_pipe pipe_info[CE_COUNT_MAX];
 
@@ -198,27 +179,15 @@ struct ath10k_pci {
 
 	/* Map CE id to ce_state */
 	struct ath10k_ce_pipe ce_states[CE_COUNT_MAX];
+	struct timer_list rx_post_retry;
 };
 
 static inline struct ath10k_pci *ath10k_pci_priv(struct ath10k *ar)
 {
-	return ar->hif.priv;
-}
-
-static inline u32 ath10k_pci_reg_read32(struct ath10k *ar, u32 addr)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	return ioread32(ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS + addr);
-}
-
-static inline void ath10k_pci_reg_write32(struct ath10k *ar, u32 addr, u32 val)
-{
-	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
-
-	iowrite32(val, ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS + addr);
+	return (struct ath10k_pci *)ar->drv_priv;
 }
 
+#define ATH10K_PCI_RX_POST_RETRY_MS 50
 #define ATH_PCI_RESET_WAIT_MAX 10 /* ms */
 #define PCIE_WAKE_TIMEOUT 5000	/* 5ms */
 
@@ -242,35 +211,17 @@ static inline void ath10k_pci_reg_write32(struct ath10k *ar, u32 addr, u32 val)
 /* Wait up to this many Ms for a Diagnostic Access CE operation to complete */
 #define DIAG_ACCESS_CE_TIMEOUT_MS 10
 
-/*
- * This API allows the Host to access Target registers directly
- * and relatively efficiently over PCIe.
- * This allows the Host to avoid extra overhead associated with
- * sending a message to firmware and waiting for a response message
- * from firmware, as is done on other interconnects.
- *
- * Yet there is some complexity with direct accesses because the
- * Target's power state is not known a priori. The Host must issue
- * special PCIe reads/writes in order to explicitly wake the Target
- * and to verify that it is awake and will remain awake.
- *
- * Usage:
+/* Target exposes its registers for direct access. However before host can
+ * access them it needs to make sure the target is awake (ath10k_pci_wake,
+ * ath10k_pci_wake_wait, ath10k_pci_is_awake). Once target is awake it won't go
+ * to sleep unless host tells it to (ath10k_pci_sleep).
  *
- *   Use ath10k_pci_read32 and ath10k_pci_write32 to access Target space.
- *   These calls must be bracketed by ath10k_pci_wake and
- *   ath10k_pci_sleep.  A single BEGIN/END pair is adequate for
- *   multiple READ/WRITE operations.
+ * If host tries to access target registers without waking it up it can
+ * scribble over host memory.
  *
- *   Use ath10k_pci_wake to put the Target in a state in
- *   which it is legal for the Host to directly access it. This
- *   may involve waking the Target from a low power state, which
- *   may take up to 2Ms!
- *
- *   Use ath10k_pci_sleep to tell the Target that as far as
- *   this code path is concerned, it no longer needs to remain
- *   directly accessible.  BEGIN/END is under a reference counter;
- *   multiple code paths may issue BEGIN/END on a single targid.
+ * If target is asleep waking it up may take up to even 2ms.
  */
+
 static inline void ath10k_pci_write32(struct ath10k *ar, u32 offset,
 				      u32 value)
 {
@@ -296,25 +247,18 @@ static inline void ath10k_pci_soc_write32(struct ath10k *ar, u32 addr, u32 val)
 	ath10k_pci_write32(ar, RTC_SOC_BASE_ADDRESS + addr, val);
 }
 
-int ath10k_do_pci_wake(struct ath10k *ar);
-void ath10k_do_pci_sleep(struct ath10k *ar);
-
-static inline int ath10k_pci_wake(struct ath10k *ar)
+static inline u32 ath10k_pci_reg_read32(struct ath10k *ar, u32 addr)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 
-	if (test_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features))
-		return ath10k_do_pci_wake(ar);
-
-	return 0;
+	return ioread32(ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS + addr);
 }
 
-static inline void ath10k_pci_sleep(struct ath10k *ar)
+static inline void ath10k_pci_reg_write32(struct ath10k *ar, u32 addr, u32 val)
 {
 	struct ath10k_pci *ar_pci = ath10k_pci_priv(ar);
 
-	if (test_bit(ATH10K_PCI_FEATURE_SOC_POWER_SAVE, ar_pci->features))
-		ath10k_do_pci_sleep(ar);
+	iowrite32(val, ar_pci->mem + PCIE_LOCAL_BASE_ADDRESS + addr);
 }
 
 #endif /* _PCI_H_ */
diff --git a/drivers/net/wireless/ath/ath10k/rx_desc.h b/drivers/net/wireless/ath/ath10k/rx_desc.h
index 1c584c4b019c..e1ffdd57a18c 100644
--- a/drivers/net/wireless/ath/ath10k/rx_desc.h
+++ b/drivers/net/wireless/ath/ath10k/rx_desc.h
@@ -839,7 +839,6 @@ struct rx_ppdu_start {
  *		Reserved: HW should fill with 0, FW should ignore.
 */
 
-
 #define RX_PPDU_END_FLAGS_PHY_ERR             (1 << 0)
 #define RX_PPDU_END_FLAGS_RX_LOCATION         (1 << 1)
 #define RX_PPDU_END_FLAGS_TXBF_H_INFO         (1 << 2)
diff --git a/drivers/net/wireless/ath/ath10k/spectral.c b/drivers/net/wireless/ath/ath10k/spectral.c
new file mode 100644
index 000000000000..3e1454b74e00
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/spectral.c
@@ -0,0 +1,561 @@
+/*
+ * Copyright (c) 2013 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/relay.h>
+#include "core.h"
+#include "debug.h"
+
+static void send_fft_sample(struct ath10k *ar,
+			    const struct fft_sample_tlv *fft_sample_tlv)
+{
+	int length;
+
+	if (!ar->spectral.rfs_chan_spec_scan)
+		return;
+
+	length = __be16_to_cpu(fft_sample_tlv->length) +
+		 sizeof(*fft_sample_tlv);
+	relay_write(ar->spectral.rfs_chan_spec_scan, fft_sample_tlv, length);
+}
+
+static uint8_t get_max_exp(s8 max_index, u16 max_magnitude, size_t bin_len,
+			   u8 *data)
+{
+	int dc_pos;
+	u8 max_exp;
+
+	dc_pos = bin_len / 2;
+
+	/* peak index outside of bins */
+	if (dc_pos < max_index || -dc_pos >= max_index)
+		return 0;
+
+	for (max_exp = 0; max_exp < 8; max_exp++) {
+		if (data[dc_pos + max_index] == (max_magnitude >> max_exp))
+			break;
+	}
+
+	/* max_exp not found */
+	if (data[dc_pos + max_index] != (max_magnitude >> max_exp))
+		return 0;
+
+	return max_exp;
+}
+
+int ath10k_spectral_process_fft(struct ath10k *ar,
+				struct wmi_single_phyerr_rx_event *event,
+				struct phyerr_fft_report *fftr,
+				size_t bin_len, u64 tsf)
+{
+	struct fft_sample_ath10k *fft_sample;
+	u8 buf[sizeof(*fft_sample) + SPECTRAL_ATH10K_MAX_NUM_BINS];
+	u16 freq1, freq2, total_gain_db, base_pwr_db, length, peak_mag;
+	u32 reg0, reg1, nf_list1, nf_list2;
+	u8 chain_idx, *bins;
+	int dc_pos;
+
+	fft_sample = (struct fft_sample_ath10k *)&buf;
+
+	if (bin_len < 64 || bin_len > SPECTRAL_ATH10K_MAX_NUM_BINS)
+		return -EINVAL;
+
+	reg0 = __le32_to_cpu(fftr->reg0);
+	reg1 = __le32_to_cpu(fftr->reg1);
+
+	length = sizeof(*fft_sample) - sizeof(struct fft_sample_tlv) + bin_len;
+	fft_sample->tlv.type = ATH_FFT_SAMPLE_ATH10K;
+	fft_sample->tlv.length = __cpu_to_be16(length);
+
+	/* TODO: there might be a reason why the hardware reports 20/40/80 MHz,
+	 * but the results/plots suggest that its actually 22/44/88 MHz.
+	 */
+	switch (event->hdr.chan_width_mhz) {
+	case 20:
+		fft_sample->chan_width_mhz = 22;
+		break;
+	case 40:
+		fft_sample->chan_width_mhz = 44;
+		break;
+	case 80:
+		/* TODO: As experiments with an analogue sender and various
+		 * configuaritions (fft-sizes of 64/128/256 and 20/40/80 Mhz)
+		 * show, the particular configuration of 80 MHz/64 bins does
+		 * not match with the other smaples at all. Until the reason
+		 * for that is found, don't report these samples.
+		 */
+		if (bin_len == 64)
+			return -EINVAL;
+		fft_sample->chan_width_mhz = 88;
+		break;
+	default:
+		fft_sample->chan_width_mhz = event->hdr.chan_width_mhz;
+	}
+
+	fft_sample->relpwr_db = MS(reg1, SEARCH_FFT_REPORT_REG1_RELPWR_DB);
+	fft_sample->avgpwr_db = MS(reg1, SEARCH_FFT_REPORT_REG1_AVGPWR_DB);
+
+	peak_mag = MS(reg1, SEARCH_FFT_REPORT_REG1_PEAK_MAG);
+	fft_sample->max_magnitude = __cpu_to_be16(peak_mag);
+	fft_sample->max_index = MS(reg0, SEARCH_FFT_REPORT_REG0_PEAK_SIDX);
+	fft_sample->rssi = event->hdr.rssi_combined;
+
+	total_gain_db = MS(reg0, SEARCH_FFT_REPORT_REG0_TOTAL_GAIN_DB);
+	base_pwr_db = MS(reg0, SEARCH_FFT_REPORT_REG0_BASE_PWR_DB);
+	fft_sample->total_gain_db = __cpu_to_be16(total_gain_db);
+	fft_sample->base_pwr_db = __cpu_to_be16(base_pwr_db);
+
+	freq1 = __le16_to_cpu(event->hdr.freq1);
+	freq2 = __le16_to_cpu(event->hdr.freq2);
+	fft_sample->freq1 = __cpu_to_be16(freq1);
+	fft_sample->freq2 = __cpu_to_be16(freq2);
+
+	nf_list1 = __le32_to_cpu(event->hdr.nf_list_1);
+	nf_list2 = __le32_to_cpu(event->hdr.nf_list_2);
+	chain_idx = MS(reg0, SEARCH_FFT_REPORT_REG0_FFT_CHN_IDX);
+
+	switch (chain_idx) {
+	case 0:
+		fft_sample->noise = __cpu_to_be16(nf_list1 & 0xffffu);
+		break;
+	case 1:
+		fft_sample->noise = __cpu_to_be16((nf_list1 >> 16) & 0xffffu);
+		break;
+	case 2:
+		fft_sample->noise = __cpu_to_be16(nf_list2 & 0xffffu);
+		break;
+	case 3:
+		fft_sample->noise = __cpu_to_be16((nf_list2 >> 16) & 0xffffu);
+		break;
+	}
+
+	bins = (u8 *)fftr;
+	bins += sizeof(*fftr);
+
+	fft_sample->tsf = __cpu_to_be64(tsf);
+
+	/* max_exp has been directly reported by previous hardware (ath9k),
+	 * maybe its possible to get it by other means?
+	 */
+	fft_sample->max_exp = get_max_exp(fft_sample->max_index, peak_mag,
+					  bin_len, bins);
+
+	memcpy(fft_sample->data, bins, bin_len);
+
+	/* DC value (value in the middle) is the blind spot of the spectral
+	 * sample and invalid, interpolate it.
+	 */
+	dc_pos = bin_len / 2;
+	fft_sample->data[dc_pos] = (fft_sample->data[dc_pos + 1] +
+				    fft_sample->data[dc_pos - 1]) / 2;
+
+	send_fft_sample(ar, &fft_sample->tlv);
+
+	return 0;
+}
+
+static struct ath10k_vif *ath10k_get_spectral_vdev(struct ath10k *ar)
+{
+	struct ath10k_vif *arvif;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	if (list_empty(&ar->arvifs))
+		return NULL;
+
+	/* if there already is a vif doing spectral, return that. */
+	list_for_each_entry(arvif, &ar->arvifs, list)
+		if (arvif->spectral_enabled)
+			return arvif;
+
+	/* otherwise, return the first vif. */
+	return list_first_entry(&ar->arvifs, typeof(*arvif), list);
+}
+
+static int ath10k_spectral_scan_trigger(struct ath10k *ar)
+{
+	struct ath10k_vif *arvif;
+	int res;
+	int vdev_id;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	arvif = ath10k_get_spectral_vdev(ar);
+	if (!arvif)
+		return -ENODEV;
+	vdev_id = arvif->vdev_id;
+
+	if (ar->spectral.mode == SPECTRAL_DISABLED)
+		return 0;
+
+	res = ath10k_wmi_vdev_spectral_enable(ar, vdev_id,
+					      WMI_SPECTRAL_TRIGGER_CMD_CLEAR,
+					      WMI_SPECTRAL_ENABLE_CMD_ENABLE);
+	if (res < 0)
+		return res;
+
+	res = ath10k_wmi_vdev_spectral_enable(ar, vdev_id,
+					      WMI_SPECTRAL_TRIGGER_CMD_TRIGGER,
+					      WMI_SPECTRAL_ENABLE_CMD_ENABLE);
+	if (res < 0)
+		return res;
+
+	return 0;
+}
+
+static int ath10k_spectral_scan_config(struct ath10k *ar,
+				       enum ath10k_spectral_mode mode)
+{
+	struct wmi_vdev_spectral_conf_arg arg;
+	struct ath10k_vif *arvif;
+	int vdev_id, count, res = 0;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	arvif = ath10k_get_spectral_vdev(ar);
+	if (!arvif)
+		return -ENODEV;
+
+	vdev_id = arvif->vdev_id;
+
+	arvif->spectral_enabled = (mode != SPECTRAL_DISABLED);
+	ar->spectral.mode = mode;
+
+	res = ath10k_wmi_vdev_spectral_enable(ar, vdev_id,
+					      WMI_SPECTRAL_TRIGGER_CMD_CLEAR,
+					      WMI_SPECTRAL_ENABLE_CMD_DISABLE);
+	if (res < 0) {
+		ath10k_warn(ar, "failed to enable spectral scan: %d\n", res);
+		return res;
+	}
+
+	if (mode == SPECTRAL_DISABLED)
+		return 0;
+
+	if (mode == SPECTRAL_BACKGROUND)
+		count = WMI_SPECTRAL_COUNT_DEFAULT;
+	else
+		count = max_t(u8, 1, ar->spectral.config.count);
+
+	arg.vdev_id = vdev_id;
+	arg.scan_count = count;
+	arg.scan_period = WMI_SPECTRAL_PERIOD_DEFAULT;
+	arg.scan_priority = WMI_SPECTRAL_PRIORITY_DEFAULT;
+	arg.scan_fft_size = ar->spectral.config.fft_size;
+	arg.scan_gc_ena = WMI_SPECTRAL_GC_ENA_DEFAULT;
+	arg.scan_restart_ena = WMI_SPECTRAL_RESTART_ENA_DEFAULT;
+	arg.scan_noise_floor_ref = WMI_SPECTRAL_NOISE_FLOOR_REF_DEFAULT;
+	arg.scan_init_delay = WMI_SPECTRAL_INIT_DELAY_DEFAULT;
+	arg.scan_nb_tone_thr = WMI_SPECTRAL_NB_TONE_THR_DEFAULT;
+	arg.scan_str_bin_thr = WMI_SPECTRAL_STR_BIN_THR_DEFAULT;
+	arg.scan_wb_rpt_mode = WMI_SPECTRAL_WB_RPT_MODE_DEFAULT;
+	arg.scan_rssi_rpt_mode = WMI_SPECTRAL_RSSI_RPT_MODE_DEFAULT;
+	arg.scan_rssi_thr = WMI_SPECTRAL_RSSI_THR_DEFAULT;
+	arg.scan_pwr_format = WMI_SPECTRAL_PWR_FORMAT_DEFAULT;
+	arg.scan_rpt_mode = WMI_SPECTRAL_RPT_MODE_DEFAULT;
+	arg.scan_bin_scale = WMI_SPECTRAL_BIN_SCALE_DEFAULT;
+	arg.scan_dbm_adj = WMI_SPECTRAL_DBM_ADJ_DEFAULT;
+	arg.scan_chn_mask = WMI_SPECTRAL_CHN_MASK_DEFAULT;
+
+	res = ath10k_wmi_vdev_spectral_conf(ar, &arg);
+	if (res < 0) {
+		ath10k_warn(ar, "failed to configure spectral scan: %d\n", res);
+		return res;
+	}
+
+	return 0;
+}
+
+static ssize_t read_file_spec_scan_ctl(struct file *file, char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	char *mode = "";
+	unsigned int len;
+	enum ath10k_spectral_mode spectral_mode;
+
+	mutex_lock(&ar->conf_mutex);
+	spectral_mode = ar->spectral.mode;
+	mutex_unlock(&ar->conf_mutex);
+
+	switch (spectral_mode) {
+	case SPECTRAL_DISABLED:
+		mode = "disable";
+		break;
+	case SPECTRAL_BACKGROUND:
+		mode = "background";
+		break;
+	case SPECTRAL_MANUAL:
+		mode = "manual";
+		break;
+	}
+
+	len = strlen(mode);
+	return simple_read_from_buffer(user_buf, count, ppos, mode, len);
+}
+
+static ssize_t write_file_spec_scan_ctl(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	char buf[32];
+	ssize_t len;
+	int res;
+
+	len = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, user_buf, len))
+		return -EFAULT;
+
+	buf[len] = '\0';
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (strncmp("trigger", buf, 7) == 0) {
+		if (ar->spectral.mode == SPECTRAL_MANUAL ||
+		    ar->spectral.mode == SPECTRAL_BACKGROUND) {
+			/* reset the configuration to adopt possibly changed
+			 * debugfs parameters
+			 */
+			res = ath10k_spectral_scan_config(ar,
+							  ar->spectral.mode);
+			if (res < 0) {
+				ath10k_warn(ar, "failed to reconfigure spectral scan: %d\n",
+					    res);
+			}
+			res = ath10k_spectral_scan_trigger(ar);
+			if (res < 0) {
+				ath10k_warn(ar, "failed to trigger spectral scan: %d\n",
+					    res);
+			}
+		} else {
+			res = -EINVAL;
+		}
+	} else if (strncmp("background", buf, 9) == 0) {
+		res = ath10k_spectral_scan_config(ar, SPECTRAL_BACKGROUND);
+	} else if (strncmp("manual", buf, 6) == 0) {
+		res = ath10k_spectral_scan_config(ar, SPECTRAL_MANUAL);
+	} else if (strncmp("disable", buf, 7) == 0) {
+		res = ath10k_spectral_scan_config(ar, SPECTRAL_DISABLED);
+	} else {
+		res = -EINVAL;
+	}
+
+	mutex_unlock(&ar->conf_mutex);
+
+	if (res < 0)
+		return res;
+
+	return count;
+}
+
+static const struct file_operations fops_spec_scan_ctl = {
+	.read = read_file_spec_scan_ctl,
+	.write = write_file_spec_scan_ctl,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
+static ssize_t read_file_spectral_count(struct file *file,
+					char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	char buf[32];
+	unsigned int len;
+	u8 spectral_count;
+
+	mutex_lock(&ar->conf_mutex);
+	spectral_count = ar->spectral.config.count;
+	mutex_unlock(&ar->conf_mutex);
+
+	len = sprintf(buf, "%d\n", spectral_count);
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static ssize_t write_file_spectral_count(struct file *file,
+					 const char __user *user_buf,
+					 size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	unsigned long val;
+	char buf[32];
+	ssize_t len;
+
+	len = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, user_buf, len))
+		return -EFAULT;
+
+	buf[len] = '\0';
+	if (kstrtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (val < 0 || val > 255)
+		return -EINVAL;
+
+	mutex_lock(&ar->conf_mutex);
+	ar->spectral.config.count = val;
+	mutex_unlock(&ar->conf_mutex);
+
+	return count;
+}
+
+static const struct file_operations fops_spectral_count = {
+	.read = read_file_spectral_count,
+	.write = write_file_spectral_count,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
+static ssize_t read_file_spectral_bins(struct file *file,
+				       char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	char buf[32];
+	unsigned int len, bins, fft_size, bin_scale;
+
+	mutex_lock(&ar->conf_mutex);
+
+	fft_size = ar->spectral.config.fft_size;
+	bin_scale = WMI_SPECTRAL_BIN_SCALE_DEFAULT;
+	bins = 1 << (fft_size - bin_scale);
+
+	mutex_unlock(&ar->conf_mutex);
+
+	len = sprintf(buf, "%d\n", bins);
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static ssize_t write_file_spectral_bins(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct ath10k *ar = file->private_data;
+	unsigned long val;
+	char buf[32];
+	ssize_t len;
+
+	len = min(count, sizeof(buf) - 1);
+	if (copy_from_user(buf, user_buf, len))
+		return -EFAULT;
+
+	buf[len] = '\0';
+	if (kstrtoul(buf, 0, &val))
+		return -EINVAL;
+
+	if (val < 64 || val > SPECTRAL_ATH10K_MAX_NUM_BINS)
+		return -EINVAL;
+
+	if (!is_power_of_2(val))
+		return -EINVAL;
+
+	mutex_lock(&ar->conf_mutex);
+	ar->spectral.config.fft_size = ilog2(val);
+	ar->spectral.config.fft_size += WMI_SPECTRAL_BIN_SCALE_DEFAULT;
+	mutex_unlock(&ar->conf_mutex);
+
+	return count;
+}
+
+static const struct file_operations fops_spectral_bins = {
+	.read = read_file_spectral_bins,
+	.write = write_file_spectral_bins,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+
+static struct dentry *create_buf_file_handler(const char *filename,
+					      struct dentry *parent,
+					      umode_t mode,
+					      struct rchan_buf *buf,
+					      int *is_global)
+{
+	struct dentry *buf_file;
+
+	buf_file = debugfs_create_file(filename, mode, parent, buf,
+				       &relay_file_operations);
+	*is_global = 1;
+	return buf_file;
+}
+
+static int remove_buf_file_handler(struct dentry *dentry)
+{
+	debugfs_remove(dentry);
+
+	return 0;
+}
+
+static struct rchan_callbacks rfs_spec_scan_cb = {
+	.create_buf_file = create_buf_file_handler,
+	.remove_buf_file = remove_buf_file_handler,
+};
+
+int ath10k_spectral_start(struct ath10k *ar)
+{
+	struct ath10k_vif *arvif;
+
+	lockdep_assert_held(&ar->conf_mutex);
+
+	list_for_each_entry(arvif, &ar->arvifs, list)
+		arvif->spectral_enabled = 0;
+
+	ar->spectral.mode = SPECTRAL_DISABLED;
+	ar->spectral.config.count = WMI_SPECTRAL_COUNT_DEFAULT;
+	ar->spectral.config.fft_size = WMI_SPECTRAL_FFT_SIZE_DEFAULT;
+
+	return 0;
+}
+
+int ath10k_spectral_vif_stop(struct ath10k_vif *arvif)
+{
+	if (!arvif->spectral_enabled)
+		return 0;
+
+	return ath10k_spectral_scan_config(arvif->ar, SPECTRAL_DISABLED);
+}
+
+int ath10k_spectral_create(struct ath10k *ar)
+{
+	ar->spectral.rfs_chan_spec_scan = relay_open("spectral_scan",
+						     ar->debug.debugfs_phy,
+						     1024, 256,
+						     &rfs_spec_scan_cb, NULL);
+	debugfs_create_file("spectral_scan_ctl",
+			    S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar,
+			    &fops_spec_scan_ctl);
+	debugfs_create_file("spectral_count",
+			    S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar,
+			    &fops_spectral_count);
+	debugfs_create_file("spectral_bins",
+			    S_IRUSR | S_IWUSR,
+			    ar->debug.debugfs_phy, ar,
+			    &fops_spectral_bins);
+
+	return 0;
+}
+
+void ath10k_spectral_destroy(struct ath10k *ar)
+{
+	if (ar->spectral.rfs_chan_spec_scan) {
+		relay_close(ar->spectral.rfs_chan_spec_scan);
+		ar->spectral.rfs_chan_spec_scan = NULL;
+	}
+}
diff --git a/drivers/net/wireless/ath/ath10k/spectral.h b/drivers/net/wireless/ath/ath10k/spectral.h
new file mode 100644
index 000000000000..ddc57c557272
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/spectral.h
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2013 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef SPECTRAL_H
+#define SPECTRAL_H
+
+#include "../spectral_common.h"
+
+/**
+ * struct ath10k_spec_scan - parameters for Atheros spectral scan
+ *
+ * @count: number of scan results requested for manual mode
+ * @fft_size: number of bins to be requested = 2^(fft_size - bin_scale)
+ */
+struct ath10k_spec_scan {
+	u8 count;
+	u8 fft_size;
+};
+
+/* enum ath10k_spectral_mode:
+ *
+ * @SPECTRAL_DISABLED: spectral mode is disabled
+ * @SPECTRAL_BACKGROUND: hardware sends samples when it is not busy with
+ *	something else.
+ * @SPECTRAL_MANUAL: spectral scan is enabled, triggering for samples
+ *	is performed manually.
+ */
+enum ath10k_spectral_mode {
+	SPECTRAL_DISABLED = 0,
+	SPECTRAL_BACKGROUND,
+	SPECTRAL_MANUAL,
+};
+
+#ifdef CONFIG_ATH10K_DEBUGFS
+
+int ath10k_spectral_process_fft(struct ath10k *ar,
+				struct wmi_single_phyerr_rx_event *event,
+				struct phyerr_fft_report *fftr,
+				size_t bin_len, u64 tsf);
+int ath10k_spectral_start(struct ath10k *ar);
+int ath10k_spectral_vif_stop(struct ath10k_vif *arvif);
+int ath10k_spectral_create(struct ath10k *ar);
+void ath10k_spectral_destroy(struct ath10k *ar);
+
+#else
+
+static inline int
+ath10k_spectral_process_fft(struct ath10k *ar,
+			    struct wmi_single_phyerr_rx_event *event,
+			    struct phyerr_fft_report *fftr,
+			    size_t bin_len, u64 tsf)
+{
+	return 0;
+}
+
+static inline int ath10k_spectral_start(struct ath10k *ar)
+{
+	return 0;
+}
+
+static inline int ath10k_spectral_vif_stop(struct ath10k_vif *arvif)
+{
+	return 0;
+}
+
+static inline int ath10k_spectral_create(struct ath10k *ar)
+{
+	return 0;
+}
+
+static inline void ath10k_spectral_destroy(struct ath10k *ar)
+{
+}
+
+#endif /* CONFIG_ATH10K_DEBUGFS */
+
+#endif /* SPECTRAL_H */
diff --git a/drivers/net/wireless/ath/ath10k/targaddrs.h b/drivers/net/wireless/ath/ath10k/targaddrs.h
index be7ba1e78afe..9d0ae30f9ff1 100644
--- a/drivers/net/wireless/ath/ath10k/targaddrs.h
+++ b/drivers/net/wireless/ath/ath10k/targaddrs.h
@@ -284,7 +284,6 @@ Fw Mode/SubMode Mask
 #define HI_OPTION_ALL_FW_SUBMODE_MASK  0xFF00
 #define HI_OPTION_ALL_FW_SUBMODE_SHIFT 0x8
 
-
 /* hi_option_flag2 options */
 #define HI_OPTION_OFFLOAD_AMSDU     0x01
 #define HI_OPTION_DFS_SUPPORT       0x02 /* Enable DFS support */
diff --git a/drivers/net/wireless/ath/ath10k/testmode.c b/drivers/net/wireless/ath/ath10k/testmode.c
new file mode 100644
index 000000000000..483db9cb8c96
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/testmode.c
@@ -0,0 +1,382 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "testmode.h"
+
+#include <net/netlink.h>
+#include <linux/firmware.h>
+
+#include "debug.h"
+#include "wmi.h"
+#include "hif.h"
+#include "hw.h"
+
+#include "testmode_i.h"
+
+static const struct nla_policy ath10k_tm_policy[ATH10K_TM_ATTR_MAX + 1] = {
+	[ATH10K_TM_ATTR_CMD]		= { .type = NLA_U32 },
+	[ATH10K_TM_ATTR_DATA]		= { .type = NLA_BINARY,
+					    .len = ATH10K_TM_DATA_MAX_LEN },
+	[ATH10K_TM_ATTR_WMI_CMDID]	= { .type = NLA_U32 },
+	[ATH10K_TM_ATTR_VERSION_MAJOR]	= { .type = NLA_U32 },
+	[ATH10K_TM_ATTR_VERSION_MINOR]	= { .type = NLA_U32 },
+};
+
+/* Returns true if callee consumes the skb and the skb should be discarded.
+ * Returns false if skb is not used. Does not sleep.
+ */
+bool ath10k_tm_event_wmi(struct ath10k *ar, u32 cmd_id, struct sk_buff *skb)
+{
+	struct sk_buff *nl_skb;
+	bool consumed;
+	int ret;
+
+	ath10k_dbg(ar, ATH10K_DBG_TESTMODE,
+		   "testmode event wmi cmd_id %d skb %p skb->len %d\n",
+		   cmd_id, skb, skb->len);
+
+	ath10k_dbg_dump(ar, ATH10K_DBG_TESTMODE, NULL, "", skb->data, skb->len);
+
+	spin_lock_bh(&ar->data_lock);
+
+	if (!ar->testmode.utf_monitor) {
+		consumed = false;
+		goto out;
+	}
+
+	/* Only testmode.c should be handling events from utf firmware,
+	 * otherwise all sort of problems will arise as mac80211 operations
+	 * are not initialised.
+	 */
+	consumed = true;
+
+	nl_skb = cfg80211_testmode_alloc_event_skb(ar->hw->wiphy,
+						   2 * sizeof(u32) + skb->len,
+						   GFP_ATOMIC);
+	if (!nl_skb) {
+		ath10k_warn(ar,
+			    "failed to allocate skb for testmode wmi event\n");
+		goto out;
+	}
+
+	ret = nla_put_u32(nl_skb, ATH10K_TM_ATTR_CMD, ATH10K_TM_CMD_WMI);
+	if (ret) {
+		ath10k_warn(ar,
+			    "failed to to put testmode wmi event cmd attribute: %d\n",
+			    ret);
+		kfree_skb(nl_skb);
+		goto out;
+	}
+
+	ret = nla_put_u32(nl_skb, ATH10K_TM_ATTR_WMI_CMDID, cmd_id);
+	if (ret) {
+		ath10k_warn(ar,
+			    "failed to to put testmode wmi even cmd_id: %d\n",
+			    ret);
+		kfree_skb(nl_skb);
+		goto out;
+	}
+
+	ret = nla_put(nl_skb, ATH10K_TM_ATTR_DATA, skb->len, skb->data);
+	if (ret) {
+		ath10k_warn(ar,
+			    "failed to copy skb to testmode wmi event: %d\n",
+			    ret);
+		kfree_skb(nl_skb);
+		goto out;
+	}
+
+	cfg80211_testmode_event(nl_skb, GFP_ATOMIC);
+
+out:
+	spin_unlock_bh(&ar->data_lock);
+
+	return consumed;
+}
+
+static int ath10k_tm_cmd_get_version(struct ath10k *ar, struct nlattr *tb[])
+{
+	struct sk_buff *skb;
+	int ret;
+
+	ath10k_dbg(ar, ATH10K_DBG_TESTMODE,
+		   "testmode cmd get version_major %d version_minor %d\n",
+		   ATH10K_TESTMODE_VERSION_MAJOR,
+		   ATH10K_TESTMODE_VERSION_MINOR);
+
+	skb = cfg80211_testmode_alloc_reply_skb(ar->hw->wiphy,
+						nla_total_size(sizeof(u32)));
+	if (!skb)
+		return -ENOMEM;
+
+	ret = nla_put_u32(skb, ATH10K_TM_ATTR_VERSION_MAJOR,
+			  ATH10K_TESTMODE_VERSION_MAJOR);
+	if (ret) {
+		kfree_skb(skb);
+		return ret;
+	}
+
+	ret = nla_put_u32(skb, ATH10K_TM_ATTR_VERSION_MINOR,
+			  ATH10K_TESTMODE_VERSION_MINOR);
+	if (ret) {
+		kfree_skb(skb);
+		return ret;
+	}
+
+	return cfg80211_testmode_reply(skb);
+}
+
+static int ath10k_tm_cmd_utf_start(struct ath10k *ar, struct nlattr *tb[])
+{
+	char filename[100];
+	int ret;
+
+	ath10k_dbg(ar, ATH10K_DBG_TESTMODE, "testmode cmd utf start\n");
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (ar->state == ATH10K_STATE_UTF) {
+		ret = -EALREADY;
+		goto err;
+	}
+
+	/* start utf only when the driver is not in use  */
+	if (ar->state != ATH10K_STATE_OFF) {
+		ret = -EBUSY;
+		goto err;
+	}
+
+	if (WARN_ON(ar->testmode.utf != NULL)) {
+		/* utf image is already downloaded, it shouldn't be */
+		ret = -EEXIST;
+		goto err;
+	}
+
+	snprintf(filename, sizeof(filename), "%s/%s",
+		 ar->hw_params.fw.dir, ATH10K_FW_UTF_FILE);
+
+	/* load utf firmware image */
+	ret = request_firmware(&ar->testmode.utf, filename, ar->dev);
+	if (ret) {
+		ath10k_warn(ar, "failed to retrieve utf firmware '%s': %d\n",
+			    filename, ret);
+		goto err;
+	}
+
+	spin_lock_bh(&ar->data_lock);
+
+	ar->testmode.utf_monitor = true;
+
+	spin_unlock_bh(&ar->data_lock);
+
+	BUILD_BUG_ON(sizeof(ar->fw_features) !=
+		     sizeof(ar->testmode.orig_fw_features));
+
+	memcpy(ar->testmode.orig_fw_features, ar->fw_features,
+	       sizeof(ar->fw_features));
+
+	/* utf.bin firmware image does not advertise firmware features. Do
+	 * an ugly hack where we force the firmware features so that wmi.c
+	 * will use the correct WMI interface.
+	 */
+	memset(ar->fw_features, 0, sizeof(ar->fw_features));
+	__set_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features);
+
+	ret = ath10k_hif_power_up(ar);
+	if (ret) {
+		ath10k_err(ar, "failed to power up hif (testmode): %d\n", ret);
+		ar->state = ATH10K_STATE_OFF;
+		goto err_fw_features;
+	}
+
+	ret = ath10k_core_start(ar, ATH10K_FIRMWARE_MODE_UTF);
+	if (ret) {
+		ath10k_err(ar, "failed to start core (testmode): %d\n", ret);
+		ar->state = ATH10K_STATE_OFF;
+		goto err_power_down;
+	}
+
+	ar->state = ATH10K_STATE_UTF;
+
+	ath10k_info(ar, "UTF firmware started\n");
+
+	mutex_unlock(&ar->conf_mutex);
+
+	return 0;
+
+err_power_down:
+	ath10k_hif_power_down(ar);
+
+err_fw_features:
+	/* return the original firmware features */
+	memcpy(ar->fw_features, ar->testmode.orig_fw_features,
+	       sizeof(ar->fw_features));
+
+	release_firmware(ar->testmode.utf);
+	ar->testmode.utf = NULL;
+
+err:
+	mutex_unlock(&ar->conf_mutex);
+
+	return ret;
+}
+
+static void __ath10k_tm_cmd_utf_stop(struct ath10k *ar)
+{
+	lockdep_assert_held(&ar->conf_mutex);
+
+	ath10k_core_stop(ar);
+	ath10k_hif_power_down(ar);
+
+	spin_lock_bh(&ar->data_lock);
+
+	ar->testmode.utf_monitor = false;
+
+	spin_unlock_bh(&ar->data_lock);
+
+	/* return the original firmware features */
+	memcpy(ar->fw_features, ar->testmode.orig_fw_features,
+	       sizeof(ar->fw_features));
+
+	release_firmware(ar->testmode.utf);
+	ar->testmode.utf = NULL;
+
+	ar->state = ATH10K_STATE_OFF;
+}
+
+static int ath10k_tm_cmd_utf_stop(struct ath10k *ar, struct nlattr *tb[])
+{
+	int ret;
+
+	ath10k_dbg(ar, ATH10K_DBG_TESTMODE, "testmode cmd utf stop\n");
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (ar->state != ATH10K_STATE_UTF) {
+		ret = -ENETDOWN;
+		goto out;
+	}
+
+	__ath10k_tm_cmd_utf_stop(ar);
+
+	ret = 0;
+
+	ath10k_info(ar, "UTF firmware stopped\n");
+
+out:
+	mutex_unlock(&ar->conf_mutex);
+	return ret;
+}
+
+static int ath10k_tm_cmd_wmi(struct ath10k *ar, struct nlattr *tb[])
+{
+	struct sk_buff *skb;
+	int ret, buf_len;
+	u32 cmd_id;
+	void *buf;
+
+	mutex_lock(&ar->conf_mutex);
+
+	if (ar->state != ATH10K_STATE_UTF) {
+		ret = -ENETDOWN;
+		goto out;
+	}
+
+	if (!tb[ATH10K_TM_ATTR_DATA]) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	if (!tb[ATH10K_TM_ATTR_WMI_CMDID]) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	buf = nla_data(tb[ATH10K_TM_ATTR_DATA]);
+	buf_len = nla_len(tb[ATH10K_TM_ATTR_DATA]);
+	cmd_id = nla_get_u32(tb[ATH10K_TM_ATTR_WMI_CMDID]);
+
+	ath10k_dbg(ar, ATH10K_DBG_TESTMODE,
+		   "testmode cmd wmi cmd_id %d buf %p buf_len %d\n",
+		   cmd_id, buf, buf_len);
+
+	ath10k_dbg_dump(ar, ATH10K_DBG_TESTMODE, NULL, "", buf, buf_len);
+
+	skb = ath10k_wmi_alloc_skb(ar, buf_len);
+	if (!skb) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	memcpy(skb->data, buf, buf_len);
+
+	ret = ath10k_wmi_cmd_send(ar, skb, cmd_id);
+	if (ret) {
+		ath10k_warn(ar, "failed to transmit wmi command (testmode): %d\n",
+			    ret);
+		goto out;
+	}
+
+	ret = 0;
+
+out:
+	mutex_unlock(&ar->conf_mutex);
+	return ret;
+}
+
+int ath10k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		  void *data, int len)
+{
+	struct ath10k *ar = hw->priv;
+	struct nlattr *tb[ATH10K_TM_ATTR_MAX + 1];
+	int ret;
+
+	ret = nla_parse(tb, ATH10K_TM_ATTR_MAX, data, len,
+			ath10k_tm_policy);
+	if (ret)
+		return ret;
+
+	if (!tb[ATH10K_TM_ATTR_CMD])
+		return -EINVAL;
+
+	switch (nla_get_u32(tb[ATH10K_TM_ATTR_CMD])) {
+	case ATH10K_TM_CMD_GET_VERSION:
+		return ath10k_tm_cmd_get_version(ar, tb);
+	case ATH10K_TM_CMD_UTF_START:
+		return ath10k_tm_cmd_utf_start(ar, tb);
+	case ATH10K_TM_CMD_UTF_STOP:
+		return ath10k_tm_cmd_utf_stop(ar, tb);
+	case ATH10K_TM_CMD_WMI:
+		return ath10k_tm_cmd_wmi(ar, tb);
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
+void ath10k_testmode_destroy(struct ath10k *ar)
+{
+	mutex_lock(&ar->conf_mutex);
+
+	if (ar->state != ATH10K_STATE_UTF) {
+		/* utf firmware is not running, nothing to do */
+		goto out;
+	}
+
+	__ath10k_tm_cmd_utf_stop(ar);
+
+out:
+	mutex_unlock(&ar->conf_mutex);
+}
diff --git a/drivers/net/wireless/ath/ath10k/testmode.h b/drivers/net/wireless/ath/ath10k/testmode.h
new file mode 100644
index 000000000000..9cdd150815db
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/testmode.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "core.h"
+
+#ifdef CONFIG_NL80211_TESTMODE
+
+void ath10k_testmode_destroy(struct ath10k *ar);
+
+bool ath10k_tm_event_wmi(struct ath10k *ar, u32 cmd_id, struct sk_buff *skb);
+int ath10k_tm_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		  void *data, int len);
+
+#else
+
+static inline void ath10k_testmode_destroy(struct ath10k *ar)
+{
+}
+
+static inline bool ath10k_tm_event_wmi(struct ath10k *ar, u32 cmd_id,
+				       struct sk_buff *skb)
+{
+	return false;
+}
+
+static inline int ath10k_tm_cmd(struct ieee80211_hw *hw,
+				struct ieee80211_vif *vif,
+				void *data, int len)
+{
+	return 0;
+}
+
+#endif
diff --git a/drivers/net/wireless/ath/ath10k/testmode_i.h b/drivers/net/wireless/ath/ath10k/testmode_i.h
new file mode 100644
index 000000000000..ba81bf66ce85
--- /dev/null
+++ b/drivers/net/wireless/ath/ath10k/testmode_i.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/* "API" level of the ath10k testmode interface. Bump it after every
+ * incompatible interface change.
+ */
+#define ATH10K_TESTMODE_VERSION_MAJOR 1
+
+/* Bump this after every _compatible_ interface change, for example
+ * addition of a new command or an attribute.
+ */
+#define ATH10K_TESTMODE_VERSION_MINOR 0
+
+#define ATH10K_TM_DATA_MAX_LEN		5000
+
+enum ath10k_tm_attr {
+	__ATH10K_TM_ATTR_INVALID	= 0,
+	ATH10K_TM_ATTR_CMD		= 1,
+	ATH10K_TM_ATTR_DATA		= 2,
+	ATH10K_TM_ATTR_WMI_CMDID	= 3,
+	ATH10K_TM_ATTR_VERSION_MAJOR	= 4,
+	ATH10K_TM_ATTR_VERSION_MINOR	= 5,
+
+	/* keep last */
+	__ATH10K_TM_ATTR_AFTER_LAST,
+	ATH10K_TM_ATTR_MAX		= __ATH10K_TM_ATTR_AFTER_LAST - 1,
+};
+
+/* All ath10k testmode interface commands specified in
+ * ATH10K_TM_ATTR_CMD
+ */
+enum ath10k_tm_cmd {
+	/* Returns the supported ath10k testmode interface version in
+	 * ATH10K_TM_ATTR_VERSION. Always guaranteed to work. User space
+	 * uses this to verify it's using the correct version of the
+	 * testmode interface
+	 */
+	ATH10K_TM_CMD_GET_VERSION = 0,
+
+	/* Boots the UTF firmware, the netdev interface must be down at the
+	 * time.
+	 */
+	ATH10K_TM_CMD_UTF_START = 1,
+
+	/* Shuts down the UTF firmware and puts the driver back into OFF
+	 * state.
+	 */
+	ATH10K_TM_CMD_UTF_STOP = 2,
+
+	/* The command used to transmit a WMI command to the firmware and
+	 * the event to receive WMI events from the firmware. Without
+	 * struct wmi_cmd_hdr header, only the WMI payload. Command id is
+	 * provided with ATH10K_TM_ATTR_WMI_CMDID and payload in
+	 * ATH10K_TM_ATTR_DATA.
+	 */
+	ATH10K_TM_CMD_WMI = 3,
+};
diff --git a/drivers/net/wireless/ath/ath10k/trace.h b/drivers/net/wireless/ath/ath10k/trace.h
index 4eb2ecbc06ef..574b75ab2609 100644
--- a/drivers/net/wireless/ath/ath10k/trace.h
+++ b/drivers/net/wireless/ath/ath10k/trace.h
@@ -18,6 +18,7 @@
 #if !defined(_TRACE_H_) || defined(TRACE_HEADER_MULTI_READ)
 
 #include <linux/tracepoint.h>
+#include "core.h"
 
 #define _TRACE_H_
 
@@ -39,59 +40,79 @@ static inline void trace_ ## name(proto) {}
 #define ATH10K_MSG_MAX 200
 
 DECLARE_EVENT_CLASS(ath10k_log_event,
-	TP_PROTO(struct va_format *vaf),
-	TP_ARGS(vaf),
+	TP_PROTO(struct ath10k *ar, struct va_format *vaf),
+	TP_ARGS(ar, vaf),
 	TP_STRUCT__entry(
+		__string(device, dev_name(ar->dev))
+		__string(driver, dev_driver_string(ar->dev))
 		__dynamic_array(char, msg, ATH10K_MSG_MAX)
 	),
 	TP_fast_assign(
+		__assign_str(device, dev_name(ar->dev));
+		__assign_str(driver, dev_driver_string(ar->dev));
 		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
 				       ATH10K_MSG_MAX,
 				       vaf->fmt,
 				       *vaf->va) >= ATH10K_MSG_MAX);
 	),
-	TP_printk("%s", __get_str(msg))
+	TP_printk(
+		"%s %s %s",
+		__get_str(driver),
+		__get_str(device),
+		__get_str(msg)
+	)
 );
 
 DEFINE_EVENT(ath10k_log_event, ath10k_log_err,
-	     TP_PROTO(struct va_format *vaf),
-	     TP_ARGS(vaf)
+	     TP_PROTO(struct ath10k *ar, struct va_format *vaf),
+	     TP_ARGS(ar, vaf)
 );
 
 DEFINE_EVENT(ath10k_log_event, ath10k_log_warn,
-	     TP_PROTO(struct va_format *vaf),
-	     TP_ARGS(vaf)
+	     TP_PROTO(struct ath10k *ar, struct va_format *vaf),
+	     TP_ARGS(ar, vaf)
 );
 
 DEFINE_EVENT(ath10k_log_event, ath10k_log_info,
-	     TP_PROTO(struct va_format *vaf),
-	     TP_ARGS(vaf)
+	     TP_PROTO(struct ath10k *ar, struct va_format *vaf),
+	     TP_ARGS(ar, vaf)
 );
 
 TRACE_EVENT(ath10k_log_dbg,
-	TP_PROTO(unsigned int level, struct va_format *vaf),
-	TP_ARGS(level, vaf),
+	TP_PROTO(struct ath10k *ar, unsigned int level, struct va_format *vaf),
+	TP_ARGS(ar, level, vaf),
 	TP_STRUCT__entry(
+		__string(device, dev_name(ar->dev))
+		__string(driver, dev_driver_string(ar->dev))
 		__field(unsigned int, level)
 		__dynamic_array(char, msg, ATH10K_MSG_MAX)
 	),
 	TP_fast_assign(
+		__assign_str(device, dev_name(ar->dev));
+		__assign_str(driver, dev_driver_string(ar->dev));
 		__entry->level = level;
 		WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
 				       ATH10K_MSG_MAX,
 				       vaf->fmt,
 				       *vaf->va) >= ATH10K_MSG_MAX);
 	),
-	TP_printk("%s", __get_str(msg))
+	TP_printk(
+		"%s %s %s",
+		__get_str(driver),
+		__get_str(device),
+		__get_str(msg)
+	)
 );
 
 TRACE_EVENT(ath10k_log_dbg_dump,
-	TP_PROTO(const char *msg, const char *prefix,
+	TP_PROTO(struct ath10k *ar, const char *msg, const char *prefix,
 		 const void *buf, size_t buf_len),
 
-	TP_ARGS(msg, prefix, buf, buf_len),
+	TP_ARGS(ar, msg, prefix, buf, buf_len),
 
 	TP_STRUCT__entry(
+		__string(device, dev_name(ar->dev))
+		__string(driver, dev_driver_string(ar->dev))
 		__string(msg, msg)
 		__string(prefix, prefix)
 		__field(size_t, buf_len)
@@ -99,6 +120,8 @@ TRACE_EVENT(ath10k_log_dbg_dump,
 	),
 
 	TP_fast_assign(
+		__assign_str(device, dev_name(ar->dev));
+		__assign_str(driver, dev_driver_string(ar->dev));
 		__assign_str(msg, msg);
 		__assign_str(prefix, prefix);
 		__entry->buf_len = buf_len;
@@ -106,16 +129,22 @@ TRACE_EVENT(ath10k_log_dbg_dump,
 	),
 
 	TP_printk(
-		"%s/%s\n", __get_str(prefix), __get_str(msg)
+		"%s %s %s/%s\n",
+		__get_str(driver),
+		__get_str(device),
+		__get_str(prefix),
+		__get_str(msg)
 	)
 );
 
 TRACE_EVENT(ath10k_wmi_cmd,
-	TP_PROTO(int id, void *buf, size_t buf_len, int ret),
+	TP_PROTO(struct ath10k *ar, int id, void *buf, size_t buf_len, int ret),
 
-	TP_ARGS(id, buf, buf_len, ret),
+	TP_ARGS(ar, id, buf, buf_len, ret),
 
 	TP_STRUCT__entry(
+		__string(device, dev_name(ar->dev))
+		__string(driver, dev_driver_string(ar->dev))
 		__field(unsigned int, id)
 		__field(size_t, buf_len)
 		__dynamic_array(u8, buf, buf_len)
@@ -123,6 +152,8 @@ TRACE_EVENT(ath10k_wmi_cmd,
 	),
 
 	TP_fast_assign(
+		__assign_str(device, dev_name(ar->dev));
+		__assign_str(driver, dev_driver_string(ar->dev));
 		__entry->id = id;
 		__entry->buf_len = buf_len;
 		__entry->ret = ret;
@@ -130,7 +161,9 @@ TRACE_EVENT(ath10k_wmi_cmd,
 	),
 
 	TP_printk(
-		"id %d len %zu ret %d",
+		"%s %s id %d len %zu ret %d",
+		__get_str(driver),
+		__get_str(device),
 		__entry->id,
 		__entry->buf_len,
 		__entry->ret
@@ -138,67 +171,85 @@ TRACE_EVENT(ath10k_wmi_cmd,
 );
 
 TRACE_EVENT(ath10k_wmi_event,
-	TP_PROTO(int id, void *buf, size_t buf_len),
+	TP_PROTO(struct ath10k *ar, int id, void *buf, size_t buf_len),
 
-	TP_ARGS(id, buf, buf_len),
+	TP_ARGS(ar, id, buf, buf_len),
 
 	TP_STRUCT__entry(
+		__string(device, dev_name(ar->dev))
+		__string(driver, dev_driver_string(ar->dev))
 		__field(unsigned int, id)
 		__field(size_t, buf_len)
 		__dynamic_array(u8, buf, buf_len)
 	),
 
 	TP_fast_assign(
+		__assign_str(device, dev_name(ar->dev));
+		__assign_str(driver, dev_driver_string(ar->dev));
 		__entry->id = id;
 		__entry->buf_len = buf_len;
 		memcpy(__get_dynamic_array(buf), buf, buf_len);
 	),
 
 	TP_printk(
-		"id %d len %zu",
+		"%s %s id %d len %zu",
+		__get_str(driver),
+		__get_str(device),
 		__entry->id,
 		__entry->buf_len
 	)
 );
 
 TRACE_EVENT(ath10k_htt_stats,
-	TP_PROTO(void *buf, size_t buf_len),
+	TP_PROTO(struct ath10k *ar, void *buf, size_t buf_len),
 
-	TP_ARGS(buf, buf_len),
+	TP_ARGS(ar, buf, buf_len),
 
 	TP_STRUCT__entry(
+		__string(device, dev_name(ar->dev))
+		__string(driver, dev_driver_string(ar->dev))
 		__field(size_t, buf_len)
 		__dynamic_array(u8, buf, buf_len)
 	),
 
 	TP_fast_assign(
+		__assign_str(device, dev_name(ar->dev));
+		__assign_str(driver, dev_driver_string(ar->dev));
 		__entry->buf_len = buf_len;
 		memcpy(__get_dynamic_array(buf), buf, buf_len);
 	),
 
 	TP_printk(
-		"len %zu",
+		"%s %s len %zu",
+		__get_str(driver),
+		__get_str(device),
 		__entry->buf_len
 	)
 );
 
 TRACE_EVENT(ath10k_wmi_dbglog,
-	TP_PROTO(void *buf, size_t buf_len),
+	TP_PROTO(struct ath10k *ar, void *buf, size_t buf_len),
 
-	TP_ARGS(buf, buf_len),
+	TP_ARGS(ar, buf, buf_len),
 
 	TP_STRUCT__entry(
+		__string(device, dev_name(ar->dev))
+		__string(driver, dev_driver_string(ar->dev))
 		__field(size_t, buf_len)
 		__dynamic_array(u8, buf, buf_len)
 	),
 
 	TP_fast_assign(
+		__assign_str(device, dev_name(ar->dev));
+		__assign_str(driver, dev_driver_string(ar->dev));
 		__entry->buf_len = buf_len;
 		memcpy(__get_dynamic_array(buf), buf, buf_len);
 	),
 
 	TP_printk(
-		"len %zu",
+		"%s %s len %zu",
+		__get_str(driver),
+		__get_str(device),
 		__entry->buf_len
 	)
 );
diff --git a/drivers/net/wireless/ath/ath10k/txrx.c b/drivers/net/wireless/ath/ath10k/txrx.c
index f4fa22d1d591..a0cbc21d0d4b 100644
--- a/drivers/net/wireless/ath/ath10k/txrx.c
+++ b/drivers/net/wireless/ath/ath10k/txrx.c
@@ -32,14 +32,14 @@ static void ath10k_report_offchan_tx(struct ath10k *ar, struct sk_buff *skb)
 	 * offchan_tx_skb. */
 	spin_lock_bh(&ar->data_lock);
 	if (ar->offchan_tx_skb != skb) {
-		ath10k_warn("completed old offchannel frame\n");
+		ath10k_warn(ar, "completed old offchannel frame\n");
 		goto out;
 	}
 
 	complete(&ar->offchan_tx_completed);
 	ar->offchan_tx_skb = NULL; /* just for sanity */
 
-	ath10k_dbg(ATH10K_DBG_HTT, "completed offchannel skb %p\n", skb);
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "completed offchannel skb %p\n", skb);
 out:
 	spin_unlock_bh(&ar->data_lock);
 }
@@ -47,18 +47,19 @@ out:
 void ath10k_txrx_tx_unref(struct ath10k_htt *htt,
 			  const struct htt_tx_done *tx_done)
 {
-	struct device *dev = htt->ar->dev;
+	struct ath10k *ar = htt->ar;
+	struct device *dev = ar->dev;
 	struct ieee80211_tx_info *info;
 	struct ath10k_skb_cb *skb_cb;
 	struct sk_buff *msdu;
 
 	lockdep_assert_held(&htt->tx_lock);
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt tx completion msdu_id %u discard %d no_ack %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt tx completion msdu_id %u discard %d no_ack %d\n",
 		   tx_done->msdu_id, !!tx_done->discard, !!tx_done->no_ack);
 
 	if (tx_done->msdu_id >= htt->max_num_pending_tx) {
-		ath10k_warn("warning: msdu_id %d too big, ignoring\n",
+		ath10k_warn(ar, "warning: msdu_id %d too big, ignoring\n",
 			    tx_done->msdu_id);
 		return;
 	}
@@ -177,12 +178,12 @@ void ath10k_peer_map_event(struct ath10k_htt *htt,
 			goto exit;
 
 		peer->vdev_id = ev->vdev_id;
-		memcpy(peer->addr, ev->addr, ETH_ALEN);
+		ether_addr_copy(peer->addr, ev->addr);
 		list_add(&peer->list, &ar->peers);
 		wake_up(&ar->peer_mapping_wq);
 	}
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt peer map vdev %d peer %pM id %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt peer map vdev %d peer %pM id %d\n",
 		   ev->vdev_id, ev->addr, ev->peer_id);
 
 	set_bit(ev->peer_id, peer->peer_ids);
@@ -199,12 +200,12 @@ void ath10k_peer_unmap_event(struct ath10k_htt *htt,
 	spin_lock_bh(&ar->data_lock);
 	peer = ath10k_peer_find_by_id(ar, ev->peer_id);
 	if (!peer) {
-		ath10k_warn("peer-unmap-event: unknown peer id %d\n",
+		ath10k_warn(ar, "peer-unmap-event: unknown peer id %d\n",
 			    ev->peer_id);
 		goto exit;
 	}
 
-	ath10k_dbg(ATH10K_DBG_HTT, "htt peer unmap vdev %d peer %pM id %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_HTT, "htt peer unmap vdev %d peer %pM id %d\n",
 		   peer->vdev_id, peer->addr, ev->peer_id);
 
 	clear_bit(ev->peer_id, peer->peer_ids);
diff --git a/drivers/net/wireless/ath/ath10k/wmi.c b/drivers/net/wireless/ath/ath10k/wmi.c
index c2c87c916b5a..2c42bd504b79 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.c
+++ b/drivers/net/wireless/ath/ath10k/wmi.c
@@ -23,6 +23,7 @@
 #include "debug.h"
 #include "wmi.h"
 #include "mac.h"
+#include "testmode.h"
 
 /* MAIN WMI cmd track */
 static struct wmi_cmd_map wmi_cmd_map = {
@@ -487,9 +488,131 @@ static struct wmi_pdev_param_map wmi_10x_pdev_param_map = {
 	.burst_enable = WMI_10X_PDEV_PARAM_BURST_ENABLE,
 };
 
+/* firmware 10.2 specific mappings */
+static struct wmi_cmd_map wmi_10_2_cmd_map = {
+	.init_cmdid = WMI_10_2_INIT_CMDID,
+	.start_scan_cmdid = WMI_10_2_START_SCAN_CMDID,
+	.stop_scan_cmdid = WMI_10_2_STOP_SCAN_CMDID,
+	.scan_chan_list_cmdid = WMI_10_2_SCAN_CHAN_LIST_CMDID,
+	.scan_sch_prio_tbl_cmdid = WMI_CMD_UNSUPPORTED,
+	.pdev_set_regdomain_cmdid = WMI_10_2_PDEV_SET_REGDOMAIN_CMDID,
+	.pdev_set_channel_cmdid = WMI_10_2_PDEV_SET_CHANNEL_CMDID,
+	.pdev_set_param_cmdid = WMI_10_2_PDEV_SET_PARAM_CMDID,
+	.pdev_pktlog_enable_cmdid = WMI_10_2_PDEV_PKTLOG_ENABLE_CMDID,
+	.pdev_pktlog_disable_cmdid = WMI_10_2_PDEV_PKTLOG_DISABLE_CMDID,
+	.pdev_set_wmm_params_cmdid = WMI_10_2_PDEV_SET_WMM_PARAMS_CMDID,
+	.pdev_set_ht_cap_ie_cmdid = WMI_10_2_PDEV_SET_HT_CAP_IE_CMDID,
+	.pdev_set_vht_cap_ie_cmdid = WMI_10_2_PDEV_SET_VHT_CAP_IE_CMDID,
+	.pdev_set_quiet_mode_cmdid = WMI_10_2_PDEV_SET_QUIET_MODE_CMDID,
+	.pdev_green_ap_ps_enable_cmdid = WMI_10_2_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+	.pdev_get_tpc_config_cmdid = WMI_10_2_PDEV_GET_TPC_CONFIG_CMDID,
+	.pdev_set_base_macaddr_cmdid = WMI_10_2_PDEV_SET_BASE_MACADDR_CMDID,
+	.vdev_create_cmdid = WMI_10_2_VDEV_CREATE_CMDID,
+	.vdev_delete_cmdid = WMI_10_2_VDEV_DELETE_CMDID,
+	.vdev_start_request_cmdid = WMI_10_2_VDEV_START_REQUEST_CMDID,
+	.vdev_restart_request_cmdid = WMI_10_2_VDEV_RESTART_REQUEST_CMDID,
+	.vdev_up_cmdid = WMI_10_2_VDEV_UP_CMDID,
+	.vdev_stop_cmdid = WMI_10_2_VDEV_STOP_CMDID,
+	.vdev_down_cmdid = WMI_10_2_VDEV_DOWN_CMDID,
+	.vdev_set_param_cmdid = WMI_10_2_VDEV_SET_PARAM_CMDID,
+	.vdev_install_key_cmdid = WMI_10_2_VDEV_INSTALL_KEY_CMDID,
+	.peer_create_cmdid = WMI_10_2_PEER_CREATE_CMDID,
+	.peer_delete_cmdid = WMI_10_2_PEER_DELETE_CMDID,
+	.peer_flush_tids_cmdid = WMI_10_2_PEER_FLUSH_TIDS_CMDID,
+	.peer_set_param_cmdid = WMI_10_2_PEER_SET_PARAM_CMDID,
+	.peer_assoc_cmdid = WMI_10_2_PEER_ASSOC_CMDID,
+	.peer_add_wds_entry_cmdid = WMI_10_2_PEER_ADD_WDS_ENTRY_CMDID,
+	.peer_remove_wds_entry_cmdid = WMI_10_2_PEER_REMOVE_WDS_ENTRY_CMDID,
+	.peer_mcast_group_cmdid = WMI_10_2_PEER_MCAST_GROUP_CMDID,
+	.bcn_tx_cmdid = WMI_10_2_BCN_TX_CMDID,
+	.pdev_send_bcn_cmdid = WMI_10_2_PDEV_SEND_BCN_CMDID,
+	.bcn_tmpl_cmdid = WMI_CMD_UNSUPPORTED,
+	.bcn_filter_rx_cmdid = WMI_10_2_BCN_FILTER_RX_CMDID,
+	.prb_req_filter_rx_cmdid = WMI_10_2_PRB_REQ_FILTER_RX_CMDID,
+	.mgmt_tx_cmdid = WMI_10_2_MGMT_TX_CMDID,
+	.prb_tmpl_cmdid = WMI_CMD_UNSUPPORTED,
+	.addba_clear_resp_cmdid = WMI_10_2_ADDBA_CLEAR_RESP_CMDID,
+	.addba_send_cmdid = WMI_10_2_ADDBA_SEND_CMDID,
+	.addba_status_cmdid = WMI_10_2_ADDBA_STATUS_CMDID,
+	.delba_send_cmdid = WMI_10_2_DELBA_SEND_CMDID,
+	.addba_set_resp_cmdid = WMI_10_2_ADDBA_SET_RESP_CMDID,
+	.send_singleamsdu_cmdid = WMI_10_2_SEND_SINGLEAMSDU_CMDID,
+	.sta_powersave_mode_cmdid = WMI_10_2_STA_POWERSAVE_MODE_CMDID,
+	.sta_powersave_param_cmdid = WMI_10_2_STA_POWERSAVE_PARAM_CMDID,
+	.sta_mimo_ps_mode_cmdid = WMI_10_2_STA_MIMO_PS_MODE_CMDID,
+	.pdev_dfs_enable_cmdid = WMI_10_2_PDEV_DFS_ENABLE_CMDID,
+	.pdev_dfs_disable_cmdid = WMI_10_2_PDEV_DFS_DISABLE_CMDID,
+	.roam_scan_mode = WMI_10_2_ROAM_SCAN_MODE,
+	.roam_scan_rssi_threshold = WMI_10_2_ROAM_SCAN_RSSI_THRESHOLD,
+	.roam_scan_period = WMI_10_2_ROAM_SCAN_PERIOD,
+	.roam_scan_rssi_change_threshold =
+				WMI_10_2_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+	.roam_ap_profile = WMI_10_2_ROAM_AP_PROFILE,
+	.ofl_scan_add_ap_profile = WMI_10_2_OFL_SCAN_ADD_AP_PROFILE,
+	.ofl_scan_remove_ap_profile = WMI_10_2_OFL_SCAN_REMOVE_AP_PROFILE,
+	.ofl_scan_period = WMI_10_2_OFL_SCAN_PERIOD,
+	.p2p_dev_set_device_info = WMI_10_2_P2P_DEV_SET_DEVICE_INFO,
+	.p2p_dev_set_discoverability = WMI_10_2_P2P_DEV_SET_DISCOVERABILITY,
+	.p2p_go_set_beacon_ie = WMI_10_2_P2P_GO_SET_BEACON_IE,
+	.p2p_go_set_probe_resp_ie = WMI_10_2_P2P_GO_SET_PROBE_RESP_IE,
+	.p2p_set_vendor_ie_data_cmdid = WMI_CMD_UNSUPPORTED,
+	.ap_ps_peer_param_cmdid = WMI_10_2_AP_PS_PEER_PARAM_CMDID,
+	.ap_ps_peer_uapsd_coex_cmdid = WMI_CMD_UNSUPPORTED,
+	.peer_rate_retry_sched_cmdid = WMI_10_2_PEER_RATE_RETRY_SCHED_CMDID,
+	.wlan_profile_trigger_cmdid = WMI_10_2_WLAN_PROFILE_TRIGGER_CMDID,
+	.wlan_profile_set_hist_intvl_cmdid =
+				WMI_10_2_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+	.wlan_profile_get_profile_data_cmdid =
+				WMI_10_2_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+	.wlan_profile_enable_profile_id_cmdid =
+				WMI_10_2_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+	.wlan_profile_list_profile_id_cmdid =
+				WMI_10_2_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+	.pdev_suspend_cmdid = WMI_10_2_PDEV_SUSPEND_CMDID,
+	.pdev_resume_cmdid = WMI_10_2_PDEV_RESUME_CMDID,
+	.add_bcn_filter_cmdid = WMI_10_2_ADD_BCN_FILTER_CMDID,
+	.rmv_bcn_filter_cmdid = WMI_10_2_RMV_BCN_FILTER_CMDID,
+	.wow_add_wake_pattern_cmdid = WMI_10_2_WOW_ADD_WAKE_PATTERN_CMDID,
+	.wow_del_wake_pattern_cmdid = WMI_10_2_WOW_DEL_WAKE_PATTERN_CMDID,
+	.wow_enable_disable_wake_event_cmdid =
+				WMI_10_2_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+	.wow_enable_cmdid = WMI_10_2_WOW_ENABLE_CMDID,
+	.wow_hostwakeup_from_sleep_cmdid =
+				WMI_10_2_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+	.rtt_measreq_cmdid = WMI_10_2_RTT_MEASREQ_CMDID,
+	.rtt_tsf_cmdid = WMI_10_2_RTT_TSF_CMDID,
+	.vdev_spectral_scan_configure_cmdid =
+				WMI_10_2_VDEV_SPECTRAL_SCAN_CONFIGURE_CMDID,
+	.vdev_spectral_scan_enable_cmdid =
+				WMI_10_2_VDEV_SPECTRAL_SCAN_ENABLE_CMDID,
+	.request_stats_cmdid = WMI_10_2_REQUEST_STATS_CMDID,
+	.set_arp_ns_offload_cmdid = WMI_CMD_UNSUPPORTED,
+	.network_list_offload_config_cmdid = WMI_CMD_UNSUPPORTED,
+	.gtk_offload_cmdid = WMI_CMD_UNSUPPORTED,
+	.csa_offload_enable_cmdid = WMI_CMD_UNSUPPORTED,
+	.csa_offload_chanswitch_cmdid = WMI_CMD_UNSUPPORTED,
+	.chatter_set_mode_cmdid = WMI_CMD_UNSUPPORTED,
+	.peer_tid_addba_cmdid = WMI_CMD_UNSUPPORTED,
+	.peer_tid_delba_cmdid = WMI_CMD_UNSUPPORTED,
+	.sta_dtim_ps_method_cmdid = WMI_CMD_UNSUPPORTED,
+	.sta_uapsd_auto_trig_cmdid = WMI_CMD_UNSUPPORTED,
+	.sta_keepalive_cmd = WMI_CMD_UNSUPPORTED,
+	.echo_cmdid = WMI_10_2_ECHO_CMDID,
+	.pdev_utf_cmdid = WMI_10_2_PDEV_UTF_CMDID,
+	.dbglog_cfg_cmdid = WMI_10_2_DBGLOG_CFG_CMDID,
+	.pdev_qvit_cmdid = WMI_10_2_PDEV_QVIT_CMDID,
+	.pdev_ftm_intg_cmdid = WMI_CMD_UNSUPPORTED,
+	.vdev_set_keepalive_cmdid = WMI_CMD_UNSUPPORTED,
+	.vdev_get_keepalive_cmdid = WMI_CMD_UNSUPPORTED,
+	.force_fw_hang_cmdid = WMI_CMD_UNSUPPORTED,
+	.gpio_config_cmdid = WMI_10_2_GPIO_CONFIG_CMDID,
+	.gpio_output_cmdid = WMI_10_2_GPIO_OUTPUT_CMDID,
+};
+
 int ath10k_wmi_wait_for_service_ready(struct ath10k *ar)
 {
 	int ret;
+
 	ret = wait_for_completion_timeout(&ar->wmi.service_ready,
 					  WMI_SERVICE_READY_TIMEOUT_HZ);
 	return ret;
@@ -498,23 +621,24 @@ int ath10k_wmi_wait_for_service_ready(struct ath10k *ar)
 int ath10k_wmi_wait_for_unified_ready(struct ath10k *ar)
 {
 	int ret;
+
 	ret = wait_for_completion_timeout(&ar->wmi.unified_ready,
 					  WMI_UNIFIED_READY_TIMEOUT_HZ);
 	return ret;
 }
 
-static struct sk_buff *ath10k_wmi_alloc_skb(u32 len)
+struct sk_buff *ath10k_wmi_alloc_skb(struct ath10k *ar, u32 len)
 {
 	struct sk_buff *skb;
 	u32 round_len = roundup(len, 4);
 
-	skb = ath10k_htc_alloc_skb(WMI_SKB_HEADROOM + round_len);
+	skb = ath10k_htc_alloc_skb(ar, WMI_SKB_HEADROOM + round_len);
 	if (!skb)
 		return NULL;
 
 	skb_reserve(skb, WMI_SKB_HEADROOM);
 	if (!IS_ALIGNED((unsigned long)skb->data, 4))
-		ath10k_warn("Unaligned WMI skb\n");
+		ath10k_warn(ar, "Unaligned WMI skb\n");
 
 	skb_put(skb, round_len);
 	memset(skb->data, 0, round_len);
@@ -545,7 +669,7 @@ static int ath10k_wmi_cmd_send_nowait(struct ath10k *ar, struct sk_buff *skb,
 
 	memset(skb_cb, 0, sizeof(*skb_cb));
 	ret = ath10k_htc_send(&ar->htc, ar->wmi.eid, skb);
-	trace_ath10k_wmi_cmd(cmd_id, skb->data, skb->len, ret);
+	trace_ath10k_wmi_cmd(ar, cmd_id, skb->data, skb->len, ret);
 
 	if (ret)
 		goto err_pull;
@@ -604,15 +728,14 @@ static void ath10k_wmi_op_ep_tx_credits(struct ath10k *ar)
 	wake_up(&ar->wmi.tx_credits_wq);
 }
 
-static int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb,
-			       u32 cmd_id)
+int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id)
 {
 	int ret = -EOPNOTSUPP;
 
 	might_sleep();
 
 	if (cmd_id == WMI_CMD_UNSUPPORTED) {
-		ath10k_warn("wmi command %d is not supported by firmware\n",
+		ath10k_warn(ar, "wmi command %d is not supported by firmware\n",
 			    cmd_id);
 		return ret;
 	}
@@ -660,7 +783,7 @@ int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb)
 
 	len = round_up(len, 4);
 
-	wmi_skb = ath10k_wmi_alloc_skb(len);
+	wmi_skb = ath10k_wmi_alloc_skb(ar, len);
 	if (!wmi_skb)
 		return -ENOMEM;
 
@@ -671,10 +794,10 @@ int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb)
 	cmd->hdr.tx_power = 0;
 	cmd->hdr.buf_len = __cpu_to_le32(buf_len);
 
-	memcpy(cmd->hdr.peer_macaddr.addr, ieee80211_get_DA(hdr), ETH_ALEN);
+	ether_addr_copy(cmd->hdr.peer_macaddr.addr, ieee80211_get_DA(hdr));
 	memcpy(cmd->buf, skb->data, skb->len);
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi mgmt tx skb %p len %d ftype %02x stype %02x\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi mgmt tx skb %p len %d ftype %02x stype %02x\n",
 		   wmi_skb, wmi_skb->len, fc & IEEE80211_FCTL_FTYPE,
 		   fc & IEEE80211_FCTL_STYPE);
 
@@ -690,6 +813,130 @@ int ath10k_wmi_mgmt_tx(struct ath10k *ar, struct sk_buff *skb)
 	return ret;
 }
 
+static void ath10k_wmi_event_scan_started(struct ath10k *ar)
+{
+	lockdep_assert_held(&ar->data_lock);
+
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
+		ath10k_warn(ar, "received scan started event in an invalid scan state: %s (%d)\n",
+			    ath10k_scan_state_str(ar->scan.state),
+			    ar->scan.state);
+		break;
+	case ATH10K_SCAN_STARTING:
+		ar->scan.state = ATH10K_SCAN_RUNNING;
+
+		if (ar->scan.is_roc)
+			ieee80211_ready_on_channel(ar->hw);
+
+		complete(&ar->scan.started);
+		break;
+	}
+}
+
+static void ath10k_wmi_event_scan_completed(struct ath10k *ar)
+{
+	lockdep_assert_held(&ar->data_lock);
+
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+	case ATH10K_SCAN_STARTING:
+		/* One suspected reason scan can be completed while starting is
+		 * if firmware fails to deliver all scan events to the host,
+		 * e.g. when transport pipe is full. This has been observed
+		 * with spectral scan phyerr events starving wmi transport
+		 * pipe. In such case the "scan completed" event should be (and
+		 * is) ignored by the host as it may be just firmware's scan
+		 * state machine recovering.
+		 */
+		ath10k_warn(ar, "received scan completed event in an invalid scan state: %s (%d)\n",
+			    ath10k_scan_state_str(ar->scan.state),
+			    ar->scan.state);
+		break;
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
+		__ath10k_scan_finish(ar);
+		break;
+	}
+}
+
+static void ath10k_wmi_event_scan_bss_chan(struct ath10k *ar)
+{
+	lockdep_assert_held(&ar->data_lock);
+
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+	case ATH10K_SCAN_STARTING:
+		ath10k_warn(ar, "received scan bss chan event in an invalid scan state: %s (%d)\n",
+			    ath10k_scan_state_str(ar->scan.state),
+			    ar->scan.state);
+		break;
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
+		ar->scan_channel = NULL;
+		break;
+	}
+}
+
+static void ath10k_wmi_event_scan_foreign_chan(struct ath10k *ar, u32 freq)
+{
+	lockdep_assert_held(&ar->data_lock);
+
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+	case ATH10K_SCAN_STARTING:
+		ath10k_warn(ar, "received scan foreign chan event in an invalid scan state: %s (%d)\n",
+			    ath10k_scan_state_str(ar->scan.state),
+			    ar->scan.state);
+		break;
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
+		ar->scan_channel = ieee80211_get_channel(ar->hw->wiphy, freq);
+
+		if (ar->scan.is_roc && ar->scan.roc_freq == freq)
+			complete(&ar->scan.on_channel);
+		break;
+	}
+}
+
+static const char *
+ath10k_wmi_event_scan_type_str(enum wmi_scan_event_type type,
+			       enum wmi_scan_completion_reason reason)
+{
+	switch (type) {
+	case WMI_SCAN_EVENT_STARTED:
+		return "started";
+	case WMI_SCAN_EVENT_COMPLETED:
+		switch (reason) {
+		case WMI_SCAN_REASON_COMPLETED:
+			return "completed";
+		case WMI_SCAN_REASON_CANCELLED:
+			return "completed [cancelled]";
+		case WMI_SCAN_REASON_PREEMPTED:
+			return "completed [preempted]";
+		case WMI_SCAN_REASON_TIMEDOUT:
+			return "completed [timedout]";
+		case WMI_SCAN_REASON_MAX:
+			break;
+		}
+		return "completed [unknown]";
+	case WMI_SCAN_EVENT_BSS_CHANNEL:
+		return "bss channel";
+	case WMI_SCAN_EVENT_FOREIGN_CHANNEL:
+		return "foreign channel";
+	case WMI_SCAN_EVENT_DEQUEUED:
+		return "dequeued";
+	case WMI_SCAN_EVENT_PREEMPTED:
+		return "preempted";
+	case WMI_SCAN_EVENT_START_FAILED:
+		return "start failed";
+	default:
+		return "unknown";
+	}
+}
+
 static int ath10k_wmi_event_scan(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_scan_event *event = (struct wmi_scan_event *)skb->data;
@@ -707,81 +954,32 @@ static int ath10k_wmi_event_scan(struct ath10k *ar, struct sk_buff *skb)
 	scan_id    = __le32_to_cpu(event->scan_id);
 	vdev_id    = __le32_to_cpu(event->vdev_id);
 
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_SCAN_EVENTID\n");
-	ath10k_dbg(ATH10K_DBG_WMI,
-		   "scan event type %d reason %d freq %d req_id %d "
-		   "scan_id %d vdev_id %d\n",
-		   event_type, reason, freq, req_id, scan_id, vdev_id);
-
 	spin_lock_bh(&ar->data_lock);
 
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "scan event %s type %d reason %d freq %d req_id %d scan_id %d vdev_id %d state %s (%d)\n",
+		   ath10k_wmi_event_scan_type_str(event_type, reason),
+		   event_type, reason, freq, req_id, scan_id, vdev_id,
+		   ath10k_scan_state_str(ar->scan.state), ar->scan.state);
+
 	switch (event_type) {
 	case WMI_SCAN_EVENT_STARTED:
-		ath10k_dbg(ATH10K_DBG_WMI, "SCAN_EVENT_STARTED\n");
-		if (ar->scan.in_progress && ar->scan.is_roc)
-			ieee80211_ready_on_channel(ar->hw);
-
-		complete(&ar->scan.started);
+		ath10k_wmi_event_scan_started(ar);
 		break;
 	case WMI_SCAN_EVENT_COMPLETED:
-		ath10k_dbg(ATH10K_DBG_WMI, "SCAN_EVENT_COMPLETED\n");
-		switch (reason) {
-		case WMI_SCAN_REASON_COMPLETED:
-			ath10k_dbg(ATH10K_DBG_WMI, "SCAN_REASON_COMPLETED\n");
-			break;
-		case WMI_SCAN_REASON_CANCELLED:
-			ath10k_dbg(ATH10K_DBG_WMI, "SCAN_REASON_CANCELED\n");
-			break;
-		case WMI_SCAN_REASON_PREEMPTED:
-			ath10k_dbg(ATH10K_DBG_WMI, "SCAN_REASON_PREEMPTED\n");
-			break;
-		case WMI_SCAN_REASON_TIMEDOUT:
-			ath10k_dbg(ATH10K_DBG_WMI, "SCAN_REASON_TIMEDOUT\n");
-			break;
-		default:
-			break;
-		}
-
-		ar->scan_channel = NULL;
-		if (!ar->scan.in_progress) {
-			ath10k_warn("no scan requested, ignoring\n");
-			break;
-		}
-
-		if (ar->scan.is_roc) {
-			ath10k_offchan_tx_purge(ar);
-
-			if (!ar->scan.aborting)
-				ieee80211_remain_on_channel_expired(ar->hw);
-		} else {
-			ieee80211_scan_completed(ar->hw, ar->scan.aborting);
-		}
-
-		del_timer(&ar->scan.timeout);
-		complete_all(&ar->scan.completed);
-		ar->scan.in_progress = false;
+		ath10k_wmi_event_scan_completed(ar);
 		break;
 	case WMI_SCAN_EVENT_BSS_CHANNEL:
-		ath10k_dbg(ATH10K_DBG_WMI, "SCAN_EVENT_BSS_CHANNEL\n");
-		ar->scan_channel = NULL;
+		ath10k_wmi_event_scan_bss_chan(ar);
 		break;
 	case WMI_SCAN_EVENT_FOREIGN_CHANNEL:
-		ath10k_dbg(ATH10K_DBG_WMI, "SCAN_EVENT_FOREIGN_CHANNEL\n");
-		ar->scan_channel = ieee80211_get_channel(ar->hw->wiphy, freq);
-		if (ar->scan.in_progress && ar->scan.is_roc &&
-		    ar->scan.roc_freq == freq) {
-			complete(&ar->scan.on_channel);
-		}
-		break;
-	case WMI_SCAN_EVENT_DEQUEUED:
-		ath10k_dbg(ATH10K_DBG_WMI, "SCAN_EVENT_DEQUEUED\n");
-		break;
-	case WMI_SCAN_EVENT_PREEMPTED:
-		ath10k_dbg(ATH10K_DBG_WMI, "WMI_SCAN_EVENT_PREEMPTED\n");
+		ath10k_wmi_event_scan_foreign_chan(ar, freq);
 		break;
 	case WMI_SCAN_EVENT_START_FAILED:
-		ath10k_dbg(ATH10K_DBG_WMI, "WMI_SCAN_EVENT_START_FAILED\n");
+		ath10k_warn(ar, "received scan start failure event\n");
 		break;
+	case WMI_SCAN_EVENT_DEQUEUED:
+	case WMI_SCAN_EVENT_PREEMPTED:
 	default:
 		break;
 	}
@@ -911,7 +1109,7 @@ static int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 
 	memset(status, 0, sizeof(*status));
 
-	ath10k_dbg(ATH10K_DBG_MGMT,
+	ath10k_dbg(ar, ATH10K_DBG_MGMT,
 		   "event mgmt rx status %08x\n", rx_status);
 
 	if (test_bit(ATH10K_CAC_RUNNING, &ar->dev_flags)) {
@@ -947,9 +1145,9 @@ static int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 
 		if (phy_mode == MODE_11B &&
 		    status->band == IEEE80211_BAND_5GHZ)
-			ath10k_dbg(ATH10K_DBG_MGMT, "wmi mgmt rx 11b (CCK) on 5GHz\n");
+			ath10k_dbg(ar, ATH10K_DBG_MGMT, "wmi mgmt rx 11b (CCK) on 5GHz\n");
 	} else {
-		ath10k_warn("using (unreliable) phy_mode to extract band for mgmt rx\n");
+		ath10k_warn(ar, "using (unreliable) phy_mode to extract band for mgmt rx\n");
 		status->band = phy_mode_to_band(phy_mode);
 	}
 
@@ -979,12 +1177,12 @@ static int ath10k_wmi_event_mgmt_rx(struct ath10k *ar, struct sk_buff *skb)
 		}
 	}
 
-	ath10k_dbg(ATH10K_DBG_MGMT,
+	ath10k_dbg(ar, ATH10K_DBG_MGMT,
 		   "event mgmt rx skb %p len %d ftype %02x stype %02x\n",
 		   skb, skb->len,
 		   fc & IEEE80211_FCTL_FTYPE, fc & IEEE80211_FCTL_STYPE);
 
-	ath10k_dbg(ATH10K_DBG_MGMT,
+	ath10k_dbg(ar, ATH10K_DBG_MGMT,
 		   "event mgmt rx freq %d band %d snr %d, rate_idx %d\n",
 		   status->freq, status->band, status->signal,
 		   status->rate_idx);
@@ -1034,21 +1232,26 @@ static void ath10k_wmi_event_chan_info(struct ath10k *ar, struct sk_buff *skb)
 	rx_clear_count = __le32_to_cpu(ev->rx_clear_count);
 	cycle_count = __le32_to_cpu(ev->cycle_count);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "chan info err_code %d freq %d cmd_flags %d noise_floor %d rx_clear_count %d cycle_count %d\n",
 		   err_code, freq, cmd_flags, noise_floor, rx_clear_count,
 		   cycle_count);
 
 	spin_lock_bh(&ar->data_lock);
 
-	if (!ar->scan.in_progress) {
-		ath10k_warn("chan info event without a scan request?\n");
+	switch (ar->scan.state) {
+	case ATH10K_SCAN_IDLE:
+	case ATH10K_SCAN_STARTING:
+		ath10k_warn(ar, "received chan info event without a scan request, ignoring\n");
 		goto exit;
+	case ATH10K_SCAN_RUNNING:
+	case ATH10K_SCAN_ABORTING:
+		break;
 	}
 
 	idx = freq_to_idx(ar, freq);
 	if (idx >= ARRAY_SIZE(ar->survey)) {
-		ath10k_warn("chan info: invalid frequency %d (idx %d out of bounds)\n",
+		ath10k_warn(ar, "chan info: invalid frequency %d (idx %d out of bounds)\n",
 			    freq, idx);
 		goto exit;
 	}
@@ -1079,15 +1282,15 @@ exit:
 
 static void ath10k_wmi_event_echo(struct ath10k *ar, struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_ECHO_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_ECHO_EVENTID\n");
 }
 
 static int ath10k_wmi_event_debug_mesg(struct ath10k *ar, struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi event debug mesg len %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi event debug mesg len %d\n",
 		   skb->len);
 
-	trace_ath10k_wmi_dbglog(skb->data, skb->len);
+	trace_ath10k_wmi_dbglog(ar, skb->data, skb->len);
 
 	return 0;
 }
@@ -1097,7 +1300,7 @@ static void ath10k_wmi_event_update_stats(struct ath10k *ar,
 {
 	struct wmi_stats_event *ev = (struct wmi_stats_event *)skb->data;
 
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_UPDATE_STATS_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_UPDATE_STATS_EVENTID\n");
 
 	ath10k_debug_read_target_stats(ar, ev);
 }
@@ -1107,7 +1310,7 @@ static void ath10k_wmi_event_vdev_start_resp(struct ath10k *ar,
 {
 	struct wmi_vdev_start_response_event *ev;
 
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_START_RESP_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_START_RESP_EVENTID\n");
 
 	ev = (struct wmi_vdev_start_response_event *)skb->data;
 
@@ -1120,7 +1323,7 @@ static void ath10k_wmi_event_vdev_start_resp(struct ath10k *ar,
 static void ath10k_wmi_event_vdev_stopped(struct ath10k *ar,
 					  struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_STOPPED_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_STOPPED_EVENTID\n");
 	complete(&ar->vdev_setup_done);
 }
 
@@ -1132,14 +1335,14 @@ static void ath10k_wmi_event_peer_sta_kickout(struct ath10k *ar,
 
 	ev = (struct wmi_peer_sta_kickout_event *)skb->data;
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi event peer sta kickout %pM\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi event peer sta kickout %pM\n",
 		   ev->peer_macaddr.addr);
 
 	rcu_read_lock();
 
 	sta = ieee80211_find_sta_by_ifaddr(ar->hw, ev->peer_macaddr.addr, NULL);
 	if (!sta) {
-		ath10k_warn("Spurious quick kickout for STA %pM\n",
+		ath10k_warn(ar, "Spurious quick kickout for STA %pM\n",
 			    ev->peer_macaddr.addr);
 		goto exit;
 	}
@@ -1183,6 +1386,8 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 	struct ieee80211_tim_ie *tim;
 	u8 *ies, *ie;
 	u8 ie_len, pvm_len;
+	__le32 t;
+	u32 v;
 
 	/* if next SWBA has no tim_changed the tim_bitmap is garbage.
 	 * we must copy the bitmap upon change and reuse it later */
@@ -1193,8 +1398,8 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 			     sizeof(bcn_info->tim_info.tim_bitmap));
 
 		for (i = 0; i < sizeof(arvif->u.ap.tim_bitmap); i++) {
-			__le32 t = bcn_info->tim_info.tim_bitmap[i / 4];
-			u32 v = __le32_to_cpu(t);
+			t = bcn_info->tim_info.tim_bitmap[i / 4];
+			v = __le32_to_cpu(t);
 			arvif->u.ap.tim_bitmap[i] = (v >> ((i % 4) * 8)) & 0xFF;
 		}
 
@@ -1216,7 +1421,7 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 				    (u8 *)skb_tail_pointer(bcn) - ies);
 	if (!ie) {
 		if (arvif->vdev_type != WMI_VDEV_TYPE_IBSS)
-			ath10k_warn("no tim ie found;\n");
+			ath10k_warn(ar, "no tim ie found;\n");
 		return;
 	}
 
@@ -1236,12 +1441,12 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 			ie_len += expand_size;
 			pvm_len += expand_size;
 		} else {
-			ath10k_warn("tim expansion failed\n");
+			ath10k_warn(ar, "tim expansion failed\n");
 		}
 	}
 
 	if (pvm_len > sizeof(arvif->u.ap.tim_bitmap)) {
-		ath10k_warn("tim pvm length is too great (%d)\n", pvm_len);
+		ath10k_warn(ar, "tim pvm length is too great (%d)\n", pvm_len);
 		return;
 	}
 
@@ -1255,7 +1460,7 @@ static void ath10k_wmi_update_tim(struct ath10k *ar,
 			ATH10K_SKB_CB(bcn)->bcn.deliver_cab = true;
 	}
 
-	ath10k_dbg(ATH10K_DBG_MGMT, "dtim %d/%d mcast %d pvmlen %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_MGMT, "dtim %d/%d mcast %d pvmlen %d\n",
 		   tim->dtim_count, tim->dtim_period,
 		   tim->bitmap_ctrl, pvm_len);
 }
@@ -1310,7 +1515,6 @@ static u32 ath10k_p2p_calc_noa_ie_len(struct wmi_p2p_noa_info *noa)
 	u8 opp_ps_info = noa->ctwindow_oppps;
 	bool opps_enabled = !!(opp_ps_info & WMI_P2P_OPPPS_ENABLE_BIT);
 
-
 	if (!noa_descriptors && !opps_enabled)
 		return len;
 
@@ -1333,7 +1537,7 @@ static void ath10k_wmi_update_noa(struct ath10k *ar, struct ath10k_vif *arvif,
 	if (arvif->vdev_subtype != WMI_VDEV_SUBTYPE_P2P_GO)
 		return;
 
-	ath10k_dbg(ATH10K_DBG_MGMT, "noa changed: %d\n", noa->changed);
+	ath10k_dbg(ar, ATH10K_DBG_MGMT, "noa changed: %d\n", noa->changed);
 	if (noa->changed & WMI_P2P_NOA_CHANGED_BIT) {
 		new_len = ath10k_p2p_calc_noa_ie_len(noa);
 		if (!new_len)
@@ -1367,7 +1571,6 @@ cleanup:
 	kfree(old_data);
 }
 
-
 static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_host_swba_event *ev;
@@ -1381,7 +1584,7 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 	ev = (struct wmi_host_swba_event *)skb->data;
 	map = __le32_to_cpu(ev->vdev_map);
 
-	ath10k_dbg(ATH10K_DBG_MGMT, "mgmt swba vdev_map 0x%x\n",
+	ath10k_dbg(ar, ATH10K_DBG_MGMT, "mgmt swba vdev_map 0x%x\n",
 		   ev->vdev_map);
 
 	for (; map; map >>= 1, vdev_id++) {
@@ -1391,13 +1594,13 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 		i++;
 
 		if (i >= WMI_MAX_AP_VDEV) {
-			ath10k_warn("swba has corrupted vdev map\n");
+			ath10k_warn(ar, "swba has corrupted vdev map\n");
 			break;
 		}
 
 		bcn_info = &ev->bcn_info[i];
 
-		ath10k_dbg(ATH10K_DBG_MGMT,
+		ath10k_dbg(ar, ATH10K_DBG_MGMT,
 			   "mgmt event bcn_info %d tim_len %d mcast %d changed %d num_ps_pending %d bitmap 0x%08x%08x%08x%08x\n",
 			   i,
 			   __le32_to_cpu(bcn_info->tim_info.tim_len),
@@ -1411,7 +1614,8 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 
 		arvif = ath10k_get_arvif(ar, vdev_id);
 		if (arvif == NULL) {
-			ath10k_warn("no vif for vdev_id %d found\n", vdev_id);
+			ath10k_warn(ar, "no vif for vdev_id %d found\n",
+				    vdev_id);
 			continue;
 		}
 
@@ -1428,7 +1632,7 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 
 		bcn = ieee80211_beacon_get(ar->hw, arvif->vif);
 		if (!bcn) {
-			ath10k_warn("could not get mac80211 beacon\n");
+			ath10k_warn(ar, "could not get mac80211 beacon\n");
 			continue;
 		}
 
@@ -1440,7 +1644,7 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 
 		if (arvif->beacon) {
 			if (!arvif->beacon_sent)
-				ath10k_warn("SWBA overrun on vdev %d\n",
+				ath10k_warn(ar, "SWBA overrun on vdev %d\n",
 					    arvif->vdev_id);
 
 			dma_unmap_single(arvif->ar->dev,
@@ -1456,7 +1660,7 @@ static void ath10k_wmi_event_host_swba(struct ath10k *ar, struct sk_buff *skb)
 		ret = dma_mapping_error(arvif->ar->dev,
 					ATH10K_SKB_CB(bcn)->paddr);
 		if (ret) {
-			ath10k_warn("failed to map beacon: %d\n", ret);
+			ath10k_warn(ar, "failed to map beacon: %d\n", ret);
 			dev_kfree_skb_any(bcn);
 			goto skip;
 		}
@@ -1473,7 +1677,7 @@ skip:
 static void ath10k_wmi_event_tbttoffset_update(struct ath10k *ar,
 					       struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_TBTTOFFSET_UPDATE_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TBTTOFFSET_UPDATE_EVENTID\n");
 }
 
 static void ath10k_dfs_radar_report(struct ath10k *ar,
@@ -1489,20 +1693,20 @@ static void ath10k_dfs_radar_report(struct ath10k *ar,
 	reg0 = __le32_to_cpu(rr->reg0);
 	reg1 = __le32_to_cpu(rr->reg1);
 
-	ath10k_dbg(ATH10K_DBG_REGULATORY,
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "wmi phyerr radar report chirp %d max_width %d agc_total_gain %d pulse_delta_diff %d\n",
 		   MS(reg0, RADAR_REPORT_REG0_PULSE_IS_CHIRP),
 		   MS(reg0, RADAR_REPORT_REG0_PULSE_IS_MAX_WIDTH),
 		   MS(reg0, RADAR_REPORT_REG0_AGC_TOTAL_GAIN),
 		   MS(reg0, RADAR_REPORT_REG0_PULSE_DELTA_DIFF));
-	ath10k_dbg(ATH10K_DBG_REGULATORY,
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "wmi phyerr radar report pulse_delta_pean %d pulse_sidx %d fft_valid %d agc_mb_gain %d subchan_mask %d\n",
 		   MS(reg0, RADAR_REPORT_REG0_PULSE_DELTA_PEAK),
 		   MS(reg0, RADAR_REPORT_REG0_PULSE_SIDX),
 		   MS(reg1, RADAR_REPORT_REG1_PULSE_SRCH_FFT_VALID),
 		   MS(reg1, RADAR_REPORT_REG1_PULSE_AGC_MB_GAIN),
 		   MS(reg1, RADAR_REPORT_REG1_PULSE_SUBCHAN_MASK));
-	ath10k_dbg(ATH10K_DBG_REGULATORY,
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "wmi phyerr radar report pulse_tsf_offset 0x%X pulse_dur: %d\n",
 		   MS(reg1, RADAR_REPORT_REG1_PULSE_TSF_OFFSET),
 		   MS(reg1, RADAR_REPORT_REG1_PULSE_DUR));
@@ -1529,25 +1733,25 @@ static void ath10k_dfs_radar_report(struct ath10k *ar,
 	pe.width = width;
 	pe.rssi = rssi;
 
-	ath10k_dbg(ATH10K_DBG_REGULATORY,
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "dfs add pulse freq: %d, width: %d, rssi %d, tsf: %llX\n",
 		   pe.freq, pe.width, pe.rssi, pe.ts);
 
 	ATH10K_DFS_STAT_INC(ar, pulses_detected);
 
 	if (!ar->dfs_detector->add_pulse(ar->dfs_detector, &pe)) {
-		ath10k_dbg(ATH10K_DBG_REGULATORY,
+		ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 			   "dfs no pulse pattern detected, yet\n");
 		return;
 	}
 
-	ath10k_dbg(ATH10K_DBG_REGULATORY, "dfs radar detected\n");
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY, "dfs radar detected\n");
 	ATH10K_DFS_STAT_INC(ar, radar_detected);
 
 	/* Control radar events reporting in debugfs file
 	   dfs_block_radar_events */
 	if (ar->dfs_block_radar_events) {
-		ath10k_info("DFS Radar detected, but ignored as requested\n");
+		ath10k_info(ar, "DFS Radar detected, but ignored as requested\n");
 		return;
 	}
 
@@ -1566,13 +1770,13 @@ static int ath10k_dfs_fft_report(struct ath10k *ar,
 	reg1 = __le32_to_cpu(fftr->reg1);
 	rssi = event->hdr.rssi_combined;
 
-	ath10k_dbg(ATH10K_DBG_REGULATORY,
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "wmi phyerr fft report total_gain_db %d base_pwr_db %d fft_chn_idx %d peak_sidx %d\n",
 		   MS(reg0, SEARCH_FFT_REPORT_REG0_TOTAL_GAIN_DB),
 		   MS(reg0, SEARCH_FFT_REPORT_REG0_BASE_PWR_DB),
 		   MS(reg0, SEARCH_FFT_REPORT_REG0_FFT_CHN_IDX),
 		   MS(reg0, SEARCH_FFT_REPORT_REG0_PEAK_SIDX));
-	ath10k_dbg(ATH10K_DBG_REGULATORY,
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "wmi phyerr fft report rel_pwr_db %d avgpwr_db %d peak_mag %d num_store_bin %d\n",
 		   MS(reg1, SEARCH_FFT_REPORT_REG1_RELPWR_DB),
 		   MS(reg1, SEARCH_FFT_REPORT_REG1_AVGPWR_DB),
@@ -1584,7 +1788,7 @@ static int ath10k_dfs_fft_report(struct ath10k *ar,
 	/* false event detection */
 	if (rssi == DFS_RSSI_POSSIBLY_FALSE &&
 	    peak_mag < 2 * DFS_PEAK_MAG_THOLD_POSSIBLY_FALSE) {
-		ath10k_dbg(ATH10K_DBG_REGULATORY, "dfs false pulse detected\n");
+		ath10k_dbg(ar, ATH10K_DBG_REGULATORY, "dfs false pulse detected\n");
 		ATH10K_DFS_STAT_INC(ar, pulses_discarded);
 		return -EINVAL;
 	}
@@ -1603,7 +1807,7 @@ static void ath10k_wmi_event_dfs(struct ath10k *ar,
 	u8 *tlv_buf;
 
 	buf_len = __le32_to_cpu(event->hdr.buf_len);
-	ath10k_dbg(ATH10K_DBG_REGULATORY,
+	ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 		   "wmi event dfs err_code %d rssi %d tsfl 0x%X tsf64 0x%llX len %d\n",
 		   event->hdr.phy_err_code, event->hdr.rssi_combined,
 		   __le32_to_cpu(event->hdr.tsf_timestamp), tsf, buf_len);
@@ -1616,21 +1820,22 @@ static void ath10k_wmi_event_dfs(struct ath10k *ar,
 
 	while (i < buf_len) {
 		if (i + sizeof(*tlv) > buf_len) {
-			ath10k_warn("too short buf for tlv header (%d)\n", i);
+			ath10k_warn(ar, "too short buf for tlv header (%d)\n",
+				    i);
 			return;
 		}
 
 		tlv = (struct phyerr_tlv *)&event->bufp[i];
 		tlv_len = __le16_to_cpu(tlv->len);
 		tlv_buf = &event->bufp[i + sizeof(*tlv)];
-		ath10k_dbg(ATH10K_DBG_REGULATORY,
+		ath10k_dbg(ar, ATH10K_DBG_REGULATORY,
 			   "wmi event dfs tlv_len %d tlv_tag 0x%02X tlv_sig 0x%02X\n",
 			   tlv_len, tlv->tag, tlv->sig);
 
 		switch (tlv->tag) {
 		case PHYERR_TLV_TAG_RADAR_PULSE_SUMMARY:
 			if (i + sizeof(*tlv) + sizeof(*rr) > buf_len) {
-				ath10k_warn("too short radar pulse summary (%d)\n",
+				ath10k_warn(ar, "too short radar pulse summary (%d)\n",
 					    i);
 				return;
 			}
@@ -1640,7 +1845,8 @@ static void ath10k_wmi_event_dfs(struct ath10k *ar,
 			break;
 		case PHYERR_TLV_TAG_SEARCH_FFT_REPORT:
 			if (i + sizeof(*tlv) + sizeof(*fftr) > buf_len) {
-				ath10k_warn("too short fft report (%d)\n", i);
+				ath10k_warn(ar, "too short fft report (%d)\n",
+					    i);
 				return;
 			}
 
@@ -1655,11 +1861,59 @@ static void ath10k_wmi_event_dfs(struct ath10k *ar,
 	}
 }
 
-static void ath10k_wmi_event_spectral_scan(struct ath10k *ar,
-				struct wmi_single_phyerr_rx_event *event,
-				u64 tsf)
+static void
+ath10k_wmi_event_spectral_scan(struct ath10k *ar,
+			       struct wmi_single_phyerr_rx_event *event,
+			       u64 tsf)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi event spectral scan\n");
+	int buf_len, tlv_len, res, i = 0;
+	struct phyerr_tlv *tlv;
+	u8 *tlv_buf;
+	struct phyerr_fft_report *fftr;
+	size_t fftr_len;
+
+	buf_len = __le32_to_cpu(event->hdr.buf_len);
+
+	while (i < buf_len) {
+		if (i + sizeof(*tlv) > buf_len) {
+			ath10k_warn(ar, "failed to parse phyerr tlv header at byte %d\n",
+				    i);
+			return;
+		}
+
+		tlv = (struct phyerr_tlv *)&event->bufp[i];
+		tlv_len = __le16_to_cpu(tlv->len);
+		tlv_buf = &event->bufp[i + sizeof(*tlv)];
+
+		if (i + sizeof(*tlv) + tlv_len > buf_len) {
+			ath10k_warn(ar, "failed to parse phyerr tlv payload at byte %d\n",
+				    i);
+			return;
+		}
+
+		switch (tlv->tag) {
+		case PHYERR_TLV_TAG_SEARCH_FFT_REPORT:
+			if (sizeof(*fftr) > tlv_len) {
+				ath10k_warn(ar, "failed to parse fft report at byte %d\n",
+					    i);
+				return;
+			}
+
+			fftr_len = tlv_len - sizeof(*fftr);
+			fftr = (struct phyerr_fft_report *)tlv_buf;
+			res = ath10k_spectral_process_fft(ar, event,
+							  fftr, fftr_len,
+							  tsf);
+			if (res < 0) {
+				ath10k_warn(ar, "failed to process fft report: %d\n",
+					    res);
+				return;
+			}
+			break;
+		}
+
+		i += sizeof(*tlv) + tlv_len;
+	}
 }
 
 static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
@@ -1674,7 +1928,7 @@ static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
 
 	/* Check if combined event available */
 	if (left_len < sizeof(*comb_event)) {
-		ath10k_warn("wmi phyerr combined event wrong len\n");
+		ath10k_warn(ar, "wmi phyerr combined event wrong len\n");
 		return;
 	}
 
@@ -1688,7 +1942,7 @@ static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
 	tsf <<= 32;
 	tsf |= __le32_to_cpu(comb_event->hdr.tsf_l32);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi event phyerr count %d tsf64 0x%llX\n",
 		   count, tsf);
 
@@ -1696,7 +1950,8 @@ static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
 	for (i = 0; i < count; i++) {
 		/* Check if we can read event header */
 		if (left_len < sizeof(*event)) {
-			ath10k_warn("single event (%d) wrong head len\n", i);
+			ath10k_warn(ar, "single event (%d) wrong head len\n",
+				    i);
 			return;
 		}
 
@@ -1706,7 +1961,7 @@ static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
 		phy_err_code = event->hdr.phy_err_code;
 
 		if (left_len < buf_len) {
-			ath10k_warn("single event (%d) wrong buf len\n", i);
+			ath10k_warn(ar, "single event (%d) wrong buf len\n", i);
 			return;
 		}
 
@@ -1733,13 +1988,13 @@ static void ath10k_wmi_event_phyerr(struct ath10k *ar, struct sk_buff *skb)
 
 static void ath10k_wmi_event_roam(struct ath10k *ar, struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_ROAM_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_ROAM_EVENTID\n");
 }
 
 static void ath10k_wmi_event_profile_match(struct ath10k *ar,
-				    struct sk_buff *skb)
+					   struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_PROFILE_MATCH\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PROFILE_MATCH\n");
 }
 
 static void ath10k_wmi_event_debug_print(struct ath10k *ar,
@@ -1764,7 +2019,7 @@ static void ath10k_wmi_event_debug_print(struct ath10k *ar,
 	}
 
 	if (i == sizeof(buf) - 1)
-		ath10k_warn("wmi debug print truncated: %d\n", skb->len);
+		ath10k_warn(ar, "wmi debug print truncated: %d\n", skb->len);
 
 	/* for some reason the debug prints end with \n, remove that */
 	if (skb->data[i - 1] == '\n')
@@ -1773,112 +2028,112 @@ static void ath10k_wmi_event_debug_print(struct ath10k *ar,
 	/* the last byte is always reserved for the null character */
 	buf[i] = '\0';
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi event debug print '%s'\n", buf);
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi event debug print '%s'\n", buf);
 }
 
 static void ath10k_wmi_event_pdev_qvit(struct ath10k *ar, struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_PDEV_QVIT_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PDEV_QVIT_EVENTID\n");
 }
 
 static void ath10k_wmi_event_wlan_profile_data(struct ath10k *ar,
 					       struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_WLAN_PROFILE_DATA_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_WLAN_PROFILE_DATA_EVENTID\n");
 }
 
 static void ath10k_wmi_event_rtt_measurement_report(struct ath10k *ar,
-					     struct sk_buff *skb)
+						    struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_RTT_MEASUREMENT_REPORT_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_RTT_MEASUREMENT_REPORT_EVENTID\n");
 }
 
 static void ath10k_wmi_event_tsf_measurement_report(struct ath10k *ar,
-					     struct sk_buff *skb)
+						    struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_TSF_MEASUREMENT_REPORT_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TSF_MEASUREMENT_REPORT_EVENTID\n");
 }
 
 static void ath10k_wmi_event_rtt_error_report(struct ath10k *ar,
 					      struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_RTT_ERROR_REPORT_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_RTT_ERROR_REPORT_EVENTID\n");
 }
 
 static void ath10k_wmi_event_wow_wakeup_host(struct ath10k *ar,
 					     struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_WOW_WAKEUP_HOST_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_WOW_WAKEUP_HOST_EVENTID\n");
 }
 
 static void ath10k_wmi_event_dcs_interference(struct ath10k *ar,
 					      struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_DCS_INTERFERENCE_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_DCS_INTERFERENCE_EVENTID\n");
 }
 
 static void ath10k_wmi_event_pdev_tpc_config(struct ath10k *ar,
 					     struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_PDEV_TPC_CONFIG_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PDEV_TPC_CONFIG_EVENTID\n");
 }
 
 static void ath10k_wmi_event_pdev_ftm_intg(struct ath10k *ar,
 					   struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_PDEV_FTM_INTG_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_PDEV_FTM_INTG_EVENTID\n");
 }
 
 static void ath10k_wmi_event_gtk_offload_status(struct ath10k *ar,
-					 struct sk_buff *skb)
+						struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_GTK_OFFLOAD_STATUS_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_GTK_OFFLOAD_STATUS_EVENTID\n");
 }
 
 static void ath10k_wmi_event_gtk_rekey_fail(struct ath10k *ar,
 					    struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_GTK_REKEY_FAIL_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_GTK_REKEY_FAIL_EVENTID\n");
 }
 
 static void ath10k_wmi_event_delba_complete(struct ath10k *ar,
 					    struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_TX_DELBA_COMPLETE_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TX_DELBA_COMPLETE_EVENTID\n");
 }
 
 static void ath10k_wmi_event_addba_complete(struct ath10k *ar,
 					    struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_TX_ADDBA_COMPLETE_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_TX_ADDBA_COMPLETE_EVENTID\n");
 }
 
 static void ath10k_wmi_event_vdev_install_key_complete(struct ath10k *ar,
-						struct sk_buff *skb)
+						       struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_INSTALL_KEY_COMPLETE_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_INSTALL_KEY_COMPLETE_EVENTID\n");
 }
 
 static void ath10k_wmi_event_inst_rssi_stats(struct ath10k *ar,
 					     struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_INST_RSSI_STATS_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_INST_RSSI_STATS_EVENTID\n");
 }
 
 static void ath10k_wmi_event_vdev_standby_req(struct ath10k *ar,
 					      struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_STANDBY_REQ_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_STANDBY_REQ_EVENTID\n");
 }
 
 static void ath10k_wmi_event_vdev_resume_req(struct ath10k *ar,
 					     struct sk_buff *skb)
 {
-	ath10k_dbg(ATH10K_DBG_WMI, "WMI_VDEV_RESUME_REQ_EVENTID\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "WMI_VDEV_RESUME_REQ_EVENTID\n");
 }
 
 static int ath10k_wmi_alloc_host_mem(struct ath10k *ar, u32 req_id,
-				      u32 num_units, u32 unit_len)
+				     u32 num_units, u32 unit_len)
 {
 	dma_addr_t paddr;
 	u32 pool_size;
@@ -1894,7 +2149,7 @@ static int ath10k_wmi_alloc_host_mem(struct ath10k *ar, u32 req_id,
 							   &paddr,
 							   GFP_ATOMIC);
 	if (!ar->wmi.mem_chunks[idx].vaddr) {
-		ath10k_warn("failed to allocate memory chunk\n");
+		ath10k_warn(ar, "failed to allocate memory chunk\n");
 		return -ENOMEM;
 	}
 
@@ -1912,9 +2167,10 @@ static void ath10k_wmi_service_ready_event_rx(struct ath10k *ar,
 					      struct sk_buff *skb)
 {
 	struct wmi_service_ready_event *ev = (void *)skb->data;
+	DECLARE_BITMAP(svc_bmap, WMI_SERVICE_MAX) = {};
 
 	if (skb->len < sizeof(*ev)) {
-		ath10k_warn("Service ready event was %d B but expected %zu B. Wrong firmware version?\n",
+		ath10k_warn(ar, "Service ready event was %d B but expected %zu B. Wrong firmware version?\n",
 			    skb->len, sizeof(*ev));
 		return;
 	}
@@ -1937,7 +2193,7 @@ static void ath10k_wmi_service_ready_event_rx(struct ath10k *ar,
 		set_bit(ATH10K_FW_FEATURE_EXT_WMI_MGMT_RX, ar->fw_features);
 
 	if (ar->num_rf_chains > WMI_MAX_SPATIAL_STREAM) {
-		ath10k_warn("hardware advertises support for more spatial streams than it should (%d > %d)\n",
+		ath10k_warn(ar, "hardware advertises support for more spatial streams than it should (%d > %d)\n",
 			    ar->num_rf_chains, WMI_MAX_SPATIAL_STREAM);
 		ar->num_rf_chains = WMI_MAX_SPATIAL_STREAM;
 	}
@@ -1945,8 +2201,10 @@ static void ath10k_wmi_service_ready_event_rx(struct ath10k *ar,
 	ar->ath_common.regulatory.current_rd =
 		__le32_to_cpu(ev->hal_reg_capabilities.eeprom_rd);
 
-	ath10k_debug_read_service_map(ar, ev->wmi_service_bitmap,
-				      sizeof(ev->wmi_service_bitmap));
+	wmi_main_svc_map(ev->wmi_service_bitmap, svc_bmap);
+	ath10k_debug_read_service_map(ar, svc_bmap, sizeof(svc_bmap));
+	ath10k_dbg_dump(ar, ATH10K_DBG_WMI, NULL, "wmi svc: ",
+			ev->wmi_service_bitmap, sizeof(ev->wmi_service_bitmap));
 
 	if (strlen(ar->hw->wiphy->fw_version) == 0) {
 		snprintf(ar->hw->wiphy->fw_version,
@@ -1960,11 +2218,11 @@ static void ath10k_wmi_service_ready_event_rx(struct ath10k *ar,
 
 	/* FIXME: it probably should be better to support this */
 	if (__le32_to_cpu(ev->num_mem_reqs) > 0) {
-		ath10k_warn("target requested %d memory chunks; ignoring\n",
+		ath10k_warn(ar, "target requested %d memory chunks; ignoring\n",
 			    __le32_to_cpu(ev->num_mem_reqs));
 	}
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi event service ready sw_ver 0x%08x sw_ver1 0x%08x abi_ver %u phy_cap 0x%08x ht_cap 0x%08x vht_cap 0x%08x vht_supp_msc 0x%08x sys_cap_info 0x%08x mem_reqs %u num_rf_chains %u\n",
 		   __le32_to_cpu(ev->sw_version),
 		   __le32_to_cpu(ev->sw_version_1),
@@ -1986,9 +2244,10 @@ static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
 	u32 num_units, req_id, unit_size, num_mem_reqs, num_unit_info, i;
 	int ret;
 	struct wmi_service_ready_event_10x *ev = (void *)skb->data;
+	DECLARE_BITMAP(svc_bmap, WMI_SERVICE_MAX) = {};
 
 	if (skb->len < sizeof(*ev)) {
-		ath10k_warn("Service ready event was %d B but expected %zu B. Wrong firmware version?\n",
+		ath10k_warn(ar, "Service ready event was %d B but expected %zu B. Wrong firmware version?\n",
 			    skb->len, sizeof(*ev));
 		return;
 	}
@@ -2004,7 +2263,7 @@ static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
 	ar->num_rf_chains = __le32_to_cpu(ev->num_rf_chains);
 
 	if (ar->num_rf_chains > WMI_MAX_SPATIAL_STREAM) {
-		ath10k_warn("hardware advertises support for more spatial streams than it should (%d > %d)\n",
+		ath10k_warn(ar, "hardware advertises support for more spatial streams than it should (%d > %d)\n",
 			    ar->num_rf_chains, WMI_MAX_SPATIAL_STREAM);
 		ar->num_rf_chains = WMI_MAX_SPATIAL_STREAM;
 	}
@@ -2012,8 +2271,10 @@ static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
 	ar->ath_common.regulatory.current_rd =
 		__le32_to_cpu(ev->hal_reg_capabilities.eeprom_rd);
 
-	ath10k_debug_read_service_map(ar, ev->wmi_service_bitmap,
-				      sizeof(ev->wmi_service_bitmap));
+	wmi_10x_svc_map(ev->wmi_service_bitmap, svc_bmap);
+	ath10k_debug_read_service_map(ar, svc_bmap, sizeof(svc_bmap));
+	ath10k_dbg_dump(ar, ATH10K_DBG_WMI, NULL, "wmi svc: ",
+			ev->wmi_service_bitmap, sizeof(ev->wmi_service_bitmap));
 
 	if (strlen(ar->hw->wiphy->fw_version) == 0) {
 		snprintf(ar->hw->wiphy->fw_version,
@@ -2026,7 +2287,7 @@ static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
 	num_mem_reqs = __le32_to_cpu(ev->num_mem_reqs);
 
 	if (num_mem_reqs > ATH10K_MAX_MEM_REQS) {
-		ath10k_warn("requested memory chunks number (%d) exceeds the limit\n",
+		ath10k_warn(ar, "requested memory chunks number (%d) exceeds the limit\n",
 			    num_mem_reqs);
 		return;
 	}
@@ -2034,7 +2295,7 @@ static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
 	if (!num_mem_reqs)
 		goto exit;
 
-	ath10k_dbg(ATH10K_DBG_WMI, "firmware has requested %d memory chunks\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "firmware has requested %d memory chunks\n",
 		   num_mem_reqs);
 
 	for (i = 0; i < num_mem_reqs; ++i) {
@@ -2052,7 +2313,7 @@ static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
 		else if (num_unit_info & NUM_UNITS_IS_NUM_VDEVS)
 			num_units = TARGET_10X_NUM_VDEVS + 1;
 
-		ath10k_dbg(ATH10K_DBG_WMI,
+		ath10k_dbg(ar, ATH10K_DBG_WMI,
 			   "wmi mem_req_id %d num_units %d num_unit_info %d unit size %d actual units %d\n",
 			   req_id,
 			   __le32_to_cpu(ev->mem_reqs[i].num_units),
@@ -2067,7 +2328,7 @@ static void ath10k_wmi_10x_service_ready_event_rx(struct ath10k *ar,
 	}
 
 exit:
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi event service ready sw_ver 0x%08x abi_ver %u phy_cap 0x%08x ht_cap 0x%08x vht_cap 0x%08x vht_supp_msc 0x%08x sys_cap_info 0x%08x mem_reqs %u num_rf_chains %u\n",
 		   __le32_to_cpu(ev->sw_version),
 		   __le32_to_cpu(ev->abi_version),
@@ -2089,9 +2350,9 @@ static int ath10k_wmi_ready_event_rx(struct ath10k *ar, struct sk_buff *skb)
 	if (WARN_ON(skb->len < sizeof(*ev)))
 		return -EINVAL;
 
-	memcpy(ar->mac_addr, ev->mac_addr.addr, ETH_ALEN);
+	ether_addr_copy(ar->mac_addr, ev->mac_addr.addr);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi event ready sw_version %u abi_version %u mac_addr %pM status %d skb->len %i ev-sz %zu\n",
 		   __le32_to_cpu(ev->sw_version),
 		   __le32_to_cpu(ev->abi_version),
@@ -2113,7 +2374,7 @@ static void ath10k_wmi_main_process_rx(struct ath10k *ar, struct sk_buff *skb)
 	if (skb_pull(skb, sizeof(struct wmi_cmd_hdr)) == NULL)
 		return;
 
-	trace_ath10k_wmi_event(id, skb->data, skb->len);
+	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
 
 	switch (id) {
 	case WMI_MGMT_RX_EVENTID:
@@ -2211,7 +2472,7 @@ static void ath10k_wmi_main_process_rx(struct ath10k *ar, struct sk_buff *skb)
 		ath10k_wmi_ready_event_rx(ar, skb);
 		break;
 	default:
-		ath10k_warn("Unknown eventid: %d\n", id);
+		ath10k_warn(ar, "Unknown eventid: %d\n", id);
 		break;
 	}
 
@@ -2222,6 +2483,7 @@ static void ath10k_wmi_10x_process_rx(struct ath10k *ar, struct sk_buff *skb)
 {
 	struct wmi_cmd_hdr *cmd_hdr;
 	enum wmi_10x_event_id id;
+	bool consumed;
 
 	cmd_hdr = (struct wmi_cmd_hdr *)skb->data;
 	id = MS(__le32_to_cpu(cmd_hdr->cmd_id), WMI_CMD_HDR_CMD_ID);
@@ -2229,7 +2491,19 @@ static void ath10k_wmi_10x_process_rx(struct ath10k *ar, struct sk_buff *skb)
 	if (skb_pull(skb, sizeof(struct wmi_cmd_hdr)) == NULL)
 		return;
 
-	trace_ath10k_wmi_event(id, skb->data, skb->len);
+	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
+
+	consumed = ath10k_tm_event_wmi(ar, id, skb);
+
+	/* Ready event must be handled normally also in UTF mode so that we
+	 * know the UTF firmware has booted, others we are just bypass WMI
+	 * events to testmode.
+	 */
+	if (consumed && id != WMI_10X_READY_EVENTID) {
+		ath10k_dbg(ar, ATH10K_DBG_WMI,
+			   "wmi testmode consumed 0x%x\n", id);
+		goto out;
+	}
 
 	switch (id) {
 	case WMI_10X_MGMT_RX_EVENTID:
@@ -2317,28 +2591,156 @@ static void ath10k_wmi_10x_process_rx(struct ath10k *ar, struct sk_buff *skb)
 	case WMI_10X_READY_EVENTID:
 		ath10k_wmi_ready_event_rx(ar, skb);
 		break;
+	case WMI_10X_PDEV_UTF_EVENTID:
+		/* ignore utf events */
+		break;
 	default:
-		ath10k_warn("Unknown eventid: %d\n", id);
+		ath10k_warn(ar, "Unknown eventid: %d\n", id);
 		break;
 	}
 
+out:
 	dev_kfree_skb(skb);
 }
 
+static void ath10k_wmi_10_2_process_rx(struct ath10k *ar, struct sk_buff *skb)
+{
+	struct wmi_cmd_hdr *cmd_hdr;
+	enum wmi_10_2_event_id id;
+
+	cmd_hdr = (struct wmi_cmd_hdr *)skb->data;
+	id = MS(__le32_to_cpu(cmd_hdr->cmd_id), WMI_CMD_HDR_CMD_ID);
+
+	if (skb_pull(skb, sizeof(struct wmi_cmd_hdr)) == NULL)
+		return;
+
+	trace_ath10k_wmi_event(ar, id, skb->data, skb->len);
+
+	switch (id) {
+	case WMI_10_2_MGMT_RX_EVENTID:
+		ath10k_wmi_event_mgmt_rx(ar, skb);
+		/* mgmt_rx() owns the skb now! */
+		return;
+	case WMI_10_2_SCAN_EVENTID:
+		ath10k_wmi_event_scan(ar, skb);
+		break;
+	case WMI_10_2_CHAN_INFO_EVENTID:
+		ath10k_wmi_event_chan_info(ar, skb);
+		break;
+	case WMI_10_2_ECHO_EVENTID:
+		ath10k_wmi_event_echo(ar, skb);
+		break;
+	case WMI_10_2_DEBUG_MESG_EVENTID:
+		ath10k_wmi_event_debug_mesg(ar, skb);
+		break;
+	case WMI_10_2_UPDATE_STATS_EVENTID:
+		ath10k_wmi_event_update_stats(ar, skb);
+		break;
+	case WMI_10_2_VDEV_START_RESP_EVENTID:
+		ath10k_wmi_event_vdev_start_resp(ar, skb);
+		break;
+	case WMI_10_2_VDEV_STOPPED_EVENTID:
+		ath10k_wmi_event_vdev_stopped(ar, skb);
+		break;
+	case WMI_10_2_PEER_STA_KICKOUT_EVENTID:
+		ath10k_wmi_event_peer_sta_kickout(ar, skb);
+		break;
+	case WMI_10_2_HOST_SWBA_EVENTID:
+		ath10k_wmi_event_host_swba(ar, skb);
+		break;
+	case WMI_10_2_TBTTOFFSET_UPDATE_EVENTID:
+		ath10k_wmi_event_tbttoffset_update(ar, skb);
+		break;
+	case WMI_10_2_PHYERR_EVENTID:
+		ath10k_wmi_event_phyerr(ar, skb);
+		break;
+	case WMI_10_2_ROAM_EVENTID:
+		ath10k_wmi_event_roam(ar, skb);
+		break;
+	case WMI_10_2_PROFILE_MATCH:
+		ath10k_wmi_event_profile_match(ar, skb);
+		break;
+	case WMI_10_2_DEBUG_PRINT_EVENTID:
+		ath10k_wmi_event_debug_print(ar, skb);
+		break;
+	case WMI_10_2_PDEV_QVIT_EVENTID:
+		ath10k_wmi_event_pdev_qvit(ar, skb);
+		break;
+	case WMI_10_2_WLAN_PROFILE_DATA_EVENTID:
+		ath10k_wmi_event_wlan_profile_data(ar, skb);
+		break;
+	case WMI_10_2_RTT_MEASUREMENT_REPORT_EVENTID:
+		ath10k_wmi_event_rtt_measurement_report(ar, skb);
+		break;
+	case WMI_10_2_TSF_MEASUREMENT_REPORT_EVENTID:
+		ath10k_wmi_event_tsf_measurement_report(ar, skb);
+		break;
+	case WMI_10_2_RTT_ERROR_REPORT_EVENTID:
+		ath10k_wmi_event_rtt_error_report(ar, skb);
+		break;
+	case WMI_10_2_WOW_WAKEUP_HOST_EVENTID:
+		ath10k_wmi_event_wow_wakeup_host(ar, skb);
+		break;
+	case WMI_10_2_DCS_INTERFERENCE_EVENTID:
+		ath10k_wmi_event_dcs_interference(ar, skb);
+		break;
+	case WMI_10_2_PDEV_TPC_CONFIG_EVENTID:
+		ath10k_wmi_event_pdev_tpc_config(ar, skb);
+		break;
+	case WMI_10_2_INST_RSSI_STATS_EVENTID:
+		ath10k_wmi_event_inst_rssi_stats(ar, skb);
+		break;
+	case WMI_10_2_VDEV_STANDBY_REQ_EVENTID:
+		ath10k_wmi_event_vdev_standby_req(ar, skb);
+		break;
+	case WMI_10_2_VDEV_RESUME_REQ_EVENTID:
+		ath10k_wmi_event_vdev_resume_req(ar, skb);
+		break;
+	case WMI_10_2_SERVICE_READY_EVENTID:
+		ath10k_wmi_10x_service_ready_event_rx(ar, skb);
+		break;
+	case WMI_10_2_READY_EVENTID:
+		ath10k_wmi_ready_event_rx(ar, skb);
+		break;
+	case WMI_10_2_RTT_KEEPALIVE_EVENTID:
+	case WMI_10_2_GPIO_INPUT_EVENTID:
+	case WMI_10_2_PEER_RATECODE_LIST_EVENTID:
+	case WMI_10_2_GENERIC_BUFFER_EVENTID:
+	case WMI_10_2_MCAST_BUF_RELEASE_EVENTID:
+	case WMI_10_2_MCAST_LIST_AGEOUT_EVENTID:
+	case WMI_10_2_WDS_PEER_EVENTID:
+		ath10k_dbg(ar, ATH10K_DBG_WMI,
+			   "received event id %d not implemented\n", id);
+		break;
+	default:
+		ath10k_warn(ar, "Unknown eventid: %d\n", id);
+		break;
+	}
+
+	dev_kfree_skb(skb);
+}
 
 static void ath10k_wmi_process_rx(struct ath10k *ar, struct sk_buff *skb)
 {
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
-		ath10k_wmi_10x_process_rx(ar, skb);
-	else
+	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
+			ath10k_wmi_10_2_process_rx(ar, skb);
+		else
+			ath10k_wmi_10x_process_rx(ar, skb);
+	} else {
 		ath10k_wmi_main_process_rx(ar, skb);
+	}
 }
 
 /* WMI Initialization functions */
 int ath10k_wmi_attach(struct ath10k *ar)
 {
 	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
-		ar->wmi.cmd = &wmi_10x_cmd_map;
+		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
+			ar->wmi.cmd = &wmi_10_2_cmd_map;
+		else
+			ar->wmi.cmd = &wmi_10x_cmd_map;
+
 		ar->wmi.vdev_param = &wmi_10x_vdev_param_map;
 		ar->wmi.pdev_param = &wmi_10x_pdev_param_map;
 	} else {
@@ -2388,7 +2790,7 @@ int ath10k_wmi_connect(struct ath10k *ar)
 
 	status = ath10k_htc_connect_service(&ar->htc, &conn_req, &conn_resp);
 	if (status) {
-		ath10k_warn("failed to connect to WMI CONTROL service status: %d\n",
+		ath10k_warn(ar, "failed to connect to WMI CONTROL service status: %d\n",
 			    status);
 		return status;
 	}
@@ -2404,7 +2806,7 @@ static int ath10k_wmi_main_pdev_set_regdomain(struct ath10k *ar, u16 rd,
 	struct wmi_pdev_set_regdomain_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -2415,7 +2817,7 @@ static int ath10k_wmi_main_pdev_set_regdomain(struct ath10k *ar, u16 rd,
 	cmd->conformance_test_limit_2G = __cpu_to_le32(ctl2g);
 	cmd->conformance_test_limit_5G = __cpu_to_le32(ctl5g);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi pdev regdomain rd %x rd2g %x rd5g %x ctl2g %x ctl5g %x\n",
 		   rd, rd2g, rd5g, ctl2g, ctl5g);
 
@@ -2431,7 +2833,7 @@ static int ath10k_wmi_10x_pdev_set_regdomain(struct ath10k *ar, u16 rd,
 	struct wmi_pdev_set_regdomain_cmd_10x *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -2443,7 +2845,7 @@ static int ath10k_wmi_10x_pdev_set_regdomain(struct ath10k *ar, u16 rd,
 	cmd->conformance_test_limit_5G = __cpu_to_le32(ctl5g);
 	cmd->dfs_domain = __cpu_to_le32(dfs_reg);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi pdev regdomain rd %x rd2g %x rd5g %x ctl2g %x ctl5g %x dfs_region %x\n",
 		   rd, rd2g, rd5g, ctl2g, ctl5g, dfs_reg);
 
@@ -2473,7 +2875,7 @@ int ath10k_wmi_pdev_set_channel(struct ath10k *ar,
 	if (arg->passive)
 		return -EINVAL;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -2491,7 +2893,7 @@ int ath10k_wmi_pdev_set_channel(struct ath10k *ar,
 	cmd->chan.reg_classid       = arg->reg_class_id;
 	cmd->chan.antenna_max       = arg->max_antenna_gain;
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi set channel mode %d freq %d\n",
 		   arg->mode, arg->freq);
 
@@ -2504,7 +2906,7 @@ int ath10k_wmi_pdev_suspend_target(struct ath10k *ar, u32 suspend_opt)
 	struct wmi_pdev_suspend_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -2518,7 +2920,7 @@ int ath10k_wmi_pdev_resume_target(struct ath10k *ar)
 {
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(0);
+	skb = ath10k_wmi_alloc_skb(ar, 0);
 	if (skb == NULL)
 		return -ENOMEM;
 
@@ -2531,11 +2933,12 @@ int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
 	struct sk_buff *skb;
 
 	if (id == WMI_PDEV_PARAM_UNSUPPORTED) {
-		ath10k_warn("pdev param %d not supported by firmware\n", id);
+		ath10k_warn(ar, "pdev param %d not supported by firmware\n",
+			    id);
 		return -EOPNOTSUPP;
 	}
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -2543,7 +2946,7 @@ int ath10k_wmi_pdev_set_param(struct ath10k *ar, u32 id, u32 value)
 	cmd->param_id    = __cpu_to_le32(id);
 	cmd->param_value = __cpu_to_le32(value);
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi pdev set param %d value %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi pdev set param %d value %d\n",
 		   id, value);
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->pdev_set_param_cmdid);
 }
@@ -2610,7 +3013,7 @@ static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
 	len = sizeof(*cmd) +
 	      (sizeof(struct host_memory_chunk) * ar->wmi.num_mem_chunks);
 
-	buf = ath10k_wmi_alloc_skb(len);
+	buf = ath10k_wmi_alloc_skb(ar, len);
 	if (!buf)
 		return -ENOMEM;
 
@@ -2621,7 +3024,7 @@ static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
 		goto out;
 	}
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi sending %d memory chunks info.\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi sending %d memory chunks info.\n",
 		   ar->wmi.num_mem_chunks);
 
 	cmd->num_host_mem_chunks = __cpu_to_le32(ar->wmi.num_mem_chunks);
@@ -2634,7 +3037,7 @@ static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
 		cmd->host_mem_chunks[i].req_id =
 			__cpu_to_le32(ar->wmi.mem_chunks[i].req_id);
 
-		ath10k_dbg(ATH10K_DBG_WMI,
+		ath10k_dbg(ar, ATH10K_DBG_WMI,
 			   "wmi chunk %d len %d requested, addr 0x%llx\n",
 			   i,
 			   ar->wmi.mem_chunks[i].len,
@@ -2643,7 +3046,7 @@ static int ath10k_wmi_main_cmd_init(struct ath10k *ar)
 out:
 	memcpy(&cmd->resource_config, &config, sizeof(config));
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi init\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi init\n");
 	return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
 }
 
@@ -2701,7 +3104,7 @@ static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
 	len = sizeof(*cmd) +
 	      (sizeof(struct host_memory_chunk) * ar->wmi.num_mem_chunks);
 
-	buf = ath10k_wmi_alloc_skb(len);
+	buf = ath10k_wmi_alloc_skb(ar, len);
 	if (!buf)
 		return -ENOMEM;
 
@@ -2712,7 +3115,7 @@ static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
 		goto out;
 	}
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi sending %d memory chunks info.\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi sending %d memory chunks info.\n",
 		   ar->wmi.num_mem_chunks);
 
 	cmd->num_host_mem_chunks = __cpu_to_le32(ar->wmi.num_mem_chunks);
@@ -2725,7 +3128,7 @@ static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
 		cmd->host_mem_chunks[i].req_id =
 			__cpu_to_le32(ar->wmi.mem_chunks[i].req_id);
 
-		ath10k_dbg(ATH10K_DBG_WMI,
+		ath10k_dbg(ar, ATH10K_DBG_WMI,
 			   "wmi chunk %d len %d requested, addr 0x%llx\n",
 			   i,
 			   ar->wmi.mem_chunks[i].len,
@@ -2734,7 +3137,98 @@ static int ath10k_wmi_10x_cmd_init(struct ath10k *ar)
 out:
 	memcpy(&cmd->resource_config, &config, sizeof(config));
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi init 10x\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi init 10x\n");
+	return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
+}
+
+static int ath10k_wmi_10_2_cmd_init(struct ath10k *ar)
+{
+	struct wmi_init_cmd_10_2 *cmd;
+	struct sk_buff *buf;
+	struct wmi_resource_config_10x config = {};
+	u32 len, val;
+	int i;
+
+	config.num_vdevs = __cpu_to_le32(TARGET_10X_NUM_VDEVS);
+	config.num_peers = __cpu_to_le32(TARGET_10X_NUM_PEERS);
+	config.num_peer_keys = __cpu_to_le32(TARGET_10X_NUM_PEER_KEYS);
+	config.num_tids = __cpu_to_le32(TARGET_10X_NUM_TIDS);
+	config.ast_skid_limit = __cpu_to_le32(TARGET_10X_AST_SKID_LIMIT);
+	config.tx_chain_mask = __cpu_to_le32(TARGET_10X_TX_CHAIN_MASK);
+	config.rx_chain_mask = __cpu_to_le32(TARGET_10X_RX_CHAIN_MASK);
+	config.rx_timeout_pri_vo = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_LO_PRI);
+	config.rx_timeout_pri_vi = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_LO_PRI);
+	config.rx_timeout_pri_be = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_LO_PRI);
+	config.rx_timeout_pri_bk = __cpu_to_le32(TARGET_10X_RX_TIMEOUT_HI_PRI);
+	config.rx_decap_mode = __cpu_to_le32(TARGET_10X_RX_DECAP_MODE);
+
+	config.scan_max_pending_reqs =
+		__cpu_to_le32(TARGET_10X_SCAN_MAX_PENDING_REQS);
+
+	config.bmiss_offload_max_vdev =
+		__cpu_to_le32(TARGET_10X_BMISS_OFFLOAD_MAX_VDEV);
+
+	config.roam_offload_max_vdev =
+		__cpu_to_le32(TARGET_10X_ROAM_OFFLOAD_MAX_VDEV);
+
+	config.roam_offload_max_ap_profiles =
+		__cpu_to_le32(TARGET_10X_ROAM_OFFLOAD_MAX_AP_PROFILES);
+
+	config.num_mcast_groups = __cpu_to_le32(TARGET_10X_NUM_MCAST_GROUPS);
+	config.num_mcast_table_elems =
+		__cpu_to_le32(TARGET_10X_NUM_MCAST_TABLE_ELEMS);
+
+	config.mcast2ucast_mode = __cpu_to_le32(TARGET_10X_MCAST2UCAST_MODE);
+	config.tx_dbg_log_size = __cpu_to_le32(TARGET_10X_TX_DBG_LOG_SIZE);
+	config.num_wds_entries = __cpu_to_le32(TARGET_10X_NUM_WDS_ENTRIES);
+	config.dma_burst_size = __cpu_to_le32(TARGET_10X_DMA_BURST_SIZE);
+	config.mac_aggr_delim = __cpu_to_le32(TARGET_10X_MAC_AGGR_DELIM);
+
+	val = TARGET_10X_RX_SKIP_DEFRAG_TIMEOUT_DUP_DETECTION_CHECK;
+	config.rx_skip_defrag_timeout_dup_detection_check = __cpu_to_le32(val);
+
+	config.vow_config = __cpu_to_le32(TARGET_10X_VOW_CONFIG);
+
+	config.num_msdu_desc = __cpu_to_le32(TARGET_10X_NUM_MSDU_DESC);
+	config.max_frag_entries = __cpu_to_le32(TARGET_10X_MAX_FRAG_ENTRIES);
+
+	len = sizeof(*cmd) +
+	      (sizeof(struct host_memory_chunk) * ar->wmi.num_mem_chunks);
+
+	buf = ath10k_wmi_alloc_skb(ar, len);
+	if (!buf)
+		return -ENOMEM;
+
+	cmd = (struct wmi_init_cmd_10_2 *)buf->data;
+
+	if (ar->wmi.num_mem_chunks == 0) {
+		cmd->num_host_mem_chunks = 0;
+		goto out;
+	}
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi sending %d memory chunks info.\n",
+		   ar->wmi.num_mem_chunks);
+
+	cmd->num_host_mem_chunks = __cpu_to_le32(ar->wmi.num_mem_chunks);
+
+	for (i = 0; i < ar->wmi.num_mem_chunks; i++) {
+		cmd->host_mem_chunks[i].ptr =
+			__cpu_to_le32(ar->wmi.mem_chunks[i].paddr);
+		cmd->host_mem_chunks[i].size =
+			__cpu_to_le32(ar->wmi.mem_chunks[i].len);
+		cmd->host_mem_chunks[i].req_id =
+			__cpu_to_le32(ar->wmi.mem_chunks[i].req_id);
+
+		ath10k_dbg(ar, ATH10K_DBG_WMI,
+			   "wmi chunk %d len %d requested, addr 0x%llx\n",
+			   i,
+			   ar->wmi.mem_chunks[i].len,
+			   (unsigned long long)ar->wmi.mem_chunks[i].paddr);
+	}
+out:
+	memcpy(&cmd->resource_config.common, &config, sizeof(config));
+
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi init 10.2\n");
 	return ath10k_wmi_cmd_send(ar, buf, ar->wmi.cmd->init_cmdid);
 }
 
@@ -2742,10 +3236,14 @@ int ath10k_wmi_cmd_init(struct ath10k *ar)
 {
 	int ret;
 
-	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features))
-		ret = ath10k_wmi_10x_cmd_init(ar);
-	else
+	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
+			ret = ath10k_wmi_10_2_cmd_init(ar);
+		else
+			ret = ath10k_wmi_10x_cmd_init(ar);
+	} else {
 		ret = ath10k_wmi_main_cmd_init(ar);
+	}
 
 	return ret;
 }
@@ -2822,7 +3320,7 @@ int ath10k_wmi_start_scan(struct ath10k *ar,
 	if (len < 0)
 		return len; /* len contains error code here */
 
-	skb = ath10k_wmi_alloc_skb(len);
+	skb = ath10k_wmi_alloc_skb(ar, len);
 	if (!skb)
 		return -ENOMEM;
 
@@ -2865,8 +3363,8 @@ int ath10k_wmi_start_scan(struct ath10k *ar,
 		channels->num_chan = __cpu_to_le32(arg->n_channels);
 
 		for (i = 0; i < arg->n_channels; i++)
-			channels->channel_list[i] =
-				__cpu_to_le32(arg->channels[i]);
+			channels->channel_list[i].freq =
+				__cpu_to_le16(arg->channels[i]);
 
 		off += sizeof(*channels);
 		off += sizeof(__le32) * arg->n_channels;
@@ -2918,7 +3416,7 @@ int ath10k_wmi_start_scan(struct ath10k *ar,
 		return -EINVAL;
 	}
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi start scan\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi start scan\n");
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->start_scan_cmdid);
 }
 
@@ -2960,7 +3458,7 @@ int ath10k_wmi_stop_scan(struct ath10k *ar, const struct wmi_stop_scan_arg *arg)
 	if (arg->req_type == WMI_SCAN_STOP_ONE && arg->u.scan_id > 0xFFF)
 		return -EINVAL;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -2976,7 +3474,7 @@ int ath10k_wmi_stop_scan(struct ath10k *ar, const struct wmi_stop_scan_arg *arg)
 	cmd->scan_id     = __cpu_to_le32(scan_id);
 	cmd->scan_req_id = __cpu_to_le32(req_id);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi stop scan reqid %d req_type %d vdev/scan_id %d\n",
 		   arg->req_id, arg->req_type, arg->u.scan_id);
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->stop_scan_cmdid);
@@ -2990,7 +3488,7 @@ int ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
 	struct wmi_vdev_create_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -2998,9 +3496,9 @@ int ath10k_wmi_vdev_create(struct ath10k *ar, u32 vdev_id,
 	cmd->vdev_id      = __cpu_to_le32(vdev_id);
 	cmd->vdev_type    = __cpu_to_le32(type);
 	cmd->vdev_subtype = __cpu_to_le32(subtype);
-	memcpy(cmd->vdev_macaddr.addr, macaddr, ETH_ALEN);
+	ether_addr_copy(cmd->vdev_macaddr.addr, macaddr);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "WMI vdev create: id %d type %d subtype %d macaddr %pM\n",
 		   vdev_id, type, subtype, macaddr);
 
@@ -3012,22 +3510,23 @@ int ath10k_wmi_vdev_delete(struct ath10k *ar, u32 vdev_id)
 	struct wmi_vdev_delete_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_vdev_delete_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "WMI vdev delete id %d\n", vdev_id);
 
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_delete_cmdid);
 }
 
-static int ath10k_wmi_vdev_start_restart(struct ath10k *ar,
-				const struct wmi_vdev_start_request_arg *arg,
-				u32 cmd_id)
+static int
+ath10k_wmi_vdev_start_restart(struct ath10k *ar,
+			      const struct wmi_vdev_start_request_arg *arg,
+			      u32 cmd_id)
 {
 	struct wmi_vdev_start_request_cmd *cmd;
 	struct sk_buff *skb;
@@ -3052,7 +3551,7 @@ static int ath10k_wmi_vdev_start_restart(struct ath10k *ar,
 	else
 		return -EINVAL; /* should not happen, we already check cmd_id */
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3090,9 +3589,9 @@ static int ath10k_wmi_vdev_start_restart(struct ath10k *ar,
 	cmd->chan.reg_classid = arg->channel.reg_class_id;
 	cmd->chan.antenna_max = arg->channel.max_antenna_gain;
 
-	ath10k_dbg(ATH10K_DBG_WMI,
-		   "wmi vdev %s id 0x%x flags: 0x%0X, freq %d, mode %d, "
-		   "ch_flags: 0x%0X, max_power: %d\n", cmdname, arg->vdev_id,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
+		   "wmi vdev %s id 0x%x flags: 0x%0X, freq %d, mode %d, ch_flags: 0x%0X, max_power: %d\n",
+		   cmdname, arg->vdev_id,
 		   flags, arg->channel.freq, arg->channel.mode,
 		   cmd->chan.flags, arg->channel.max_power);
 
@@ -3108,7 +3607,7 @@ int ath10k_wmi_vdev_start(struct ath10k *ar,
 }
 
 int ath10k_wmi_vdev_restart(struct ath10k *ar,
-		     const struct wmi_vdev_start_request_arg *arg)
+			    const struct wmi_vdev_start_request_arg *arg)
 {
 	u32 cmd_id = ar->wmi.cmd->vdev_restart_request_cmdid;
 
@@ -3120,14 +3619,14 @@ int ath10k_wmi_vdev_stop(struct ath10k *ar, u32 vdev_id)
 	struct wmi_vdev_stop_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_vdev_stop_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi vdev stop id 0x%x\n", vdev_id);
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi vdev stop id 0x%x\n", vdev_id);
 
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_stop_cmdid);
 }
@@ -3137,16 +3636,16 @@ int ath10k_wmi_vdev_up(struct ath10k *ar, u32 vdev_id, u32 aid, const u8 *bssid)
 	struct wmi_vdev_up_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_vdev_up_cmd *)skb->data;
 	cmd->vdev_id       = __cpu_to_le32(vdev_id);
 	cmd->vdev_assoc_id = __cpu_to_le32(aid);
-	memcpy(&cmd->vdev_bssid.addr, bssid, ETH_ALEN);
+	ether_addr_copy(cmd->vdev_bssid.addr, bssid);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi mgmt vdev up id 0x%x assoc id %d bssid %pM\n",
 		   vdev_id, aid, bssid);
 
@@ -3158,14 +3657,14 @@ int ath10k_wmi_vdev_down(struct ath10k *ar, u32 vdev_id)
 	struct wmi_vdev_down_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_vdev_down_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi mgmt vdev down id 0x%x\n", vdev_id);
 
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->vdev_down_cmdid);
@@ -3178,13 +3677,13 @@ int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
 	struct sk_buff *skb;
 
 	if (param_id == WMI_VDEV_PARAM_UNSUPPORTED) {
-		ath10k_dbg(ATH10K_DBG_WMI,
+		ath10k_dbg(ar, ATH10K_DBG_WMI,
 			   "vdev param %d not supported by firmware\n",
 			    param_id);
 		return -EOPNOTSUPP;
 	}
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3193,7 +3692,7 @@ int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
 	cmd->param_id    = __cpu_to_le32(param_id);
 	cmd->param_value = __cpu_to_le32(param_value);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi vdev id 0x%x set param %d value %d\n",
 		   vdev_id, param_id, param_value);
 
@@ -3211,7 +3710,7 @@ int ath10k_wmi_vdev_install_key(struct ath10k *ar,
 	if (arg->key_cipher != WMI_CIPHER_NONE && arg->key_data == NULL)
 		return -EINVAL;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd) + arg->key_len);
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd) + arg->key_len);
 	if (!skb)
 		return -ENOMEM;
 
@@ -3225,32 +3724,88 @@ int ath10k_wmi_vdev_install_key(struct ath10k *ar,
 	cmd->key_rxmic_len = __cpu_to_le32(arg->key_rxmic_len);
 
 	if (arg->macaddr)
-		memcpy(cmd->peer_macaddr.addr, arg->macaddr, ETH_ALEN);
+		ether_addr_copy(cmd->peer_macaddr.addr, arg->macaddr);
 	if (arg->key_data)
 		memcpy(cmd->key_data, arg->key_data, arg->key_len);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi vdev install key idx %d cipher %d len %d\n",
 		   arg->key_idx, arg->key_cipher, arg->key_len);
 	return ath10k_wmi_cmd_send(ar, skb,
 				   ar->wmi.cmd->vdev_install_key_cmdid);
 }
 
+int ath10k_wmi_vdev_spectral_conf(struct ath10k *ar,
+				  const struct wmi_vdev_spectral_conf_arg *arg)
+{
+	struct wmi_vdev_spectral_conf_cmd *cmd;
+	struct sk_buff *skb;
+	u32 cmdid;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_vdev_spectral_conf_cmd *)skb->data;
+	cmd->vdev_id = __cpu_to_le32(arg->vdev_id);
+	cmd->scan_count = __cpu_to_le32(arg->scan_count);
+	cmd->scan_period = __cpu_to_le32(arg->scan_period);
+	cmd->scan_priority = __cpu_to_le32(arg->scan_priority);
+	cmd->scan_fft_size = __cpu_to_le32(arg->scan_fft_size);
+	cmd->scan_gc_ena = __cpu_to_le32(arg->scan_gc_ena);
+	cmd->scan_restart_ena = __cpu_to_le32(arg->scan_restart_ena);
+	cmd->scan_noise_floor_ref = __cpu_to_le32(arg->scan_noise_floor_ref);
+	cmd->scan_init_delay = __cpu_to_le32(arg->scan_init_delay);
+	cmd->scan_nb_tone_thr = __cpu_to_le32(arg->scan_nb_tone_thr);
+	cmd->scan_str_bin_thr = __cpu_to_le32(arg->scan_str_bin_thr);
+	cmd->scan_wb_rpt_mode = __cpu_to_le32(arg->scan_wb_rpt_mode);
+	cmd->scan_rssi_rpt_mode = __cpu_to_le32(arg->scan_rssi_rpt_mode);
+	cmd->scan_rssi_thr = __cpu_to_le32(arg->scan_rssi_thr);
+	cmd->scan_pwr_format = __cpu_to_le32(arg->scan_pwr_format);
+	cmd->scan_rpt_mode = __cpu_to_le32(arg->scan_rpt_mode);
+	cmd->scan_bin_scale = __cpu_to_le32(arg->scan_bin_scale);
+	cmd->scan_dbm_adj = __cpu_to_le32(arg->scan_dbm_adj);
+	cmd->scan_chn_mask = __cpu_to_le32(arg->scan_chn_mask);
+
+	cmdid = ar->wmi.cmd->vdev_spectral_scan_configure_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmdid);
+}
+
+int ath10k_wmi_vdev_spectral_enable(struct ath10k *ar, u32 vdev_id, u32 trigger,
+				    u32 enable)
+{
+	struct wmi_vdev_spectral_enable_cmd *cmd;
+	struct sk_buff *skb;
+	u32 cmdid;
+
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
+	if (!skb)
+		return -ENOMEM;
+
+	cmd = (struct wmi_vdev_spectral_enable_cmd *)skb->data;
+	cmd->vdev_id = __cpu_to_le32(vdev_id);
+	cmd->trigger_cmd = __cpu_to_le32(trigger);
+	cmd->enable_cmd = __cpu_to_le32(enable);
+
+	cmdid = ar->wmi.cmd->vdev_spectral_scan_enable_cmdid;
+	return ath10k_wmi_cmd_send(ar, skb, cmdid);
+}
+
 int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
 			   const u8 peer_addr[ETH_ALEN])
 {
 	struct wmi_peer_create_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_peer_create_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
-	memcpy(cmd->peer_macaddr.addr, peer_addr, ETH_ALEN);
+	ether_addr_copy(cmd->peer_macaddr.addr, peer_addr);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer create vdev_id %d peer_addr %pM\n",
 		   vdev_id, peer_addr);
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_create_cmdid);
@@ -3262,15 +3817,15 @@ int ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
 	struct wmi_peer_delete_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_peer_delete_cmd *)skb->data;
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
-	memcpy(cmd->peer_macaddr.addr, peer_addr, ETH_ALEN);
+	ether_addr_copy(cmd->peer_macaddr.addr, peer_addr);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer delete vdev_id %d peer_addr %pM\n",
 		   vdev_id, peer_addr);
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_delete_cmdid);
@@ -3282,16 +3837,16 @@ int ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
 	struct wmi_peer_flush_tids_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_peer_flush_tids_cmd *)skb->data;
 	cmd->vdev_id         = __cpu_to_le32(vdev_id);
 	cmd->peer_tid_bitmap = __cpu_to_le32(tid_bitmap);
-	memcpy(cmd->peer_macaddr.addr, peer_addr, ETH_ALEN);
+	ether_addr_copy(cmd->peer_macaddr.addr, peer_addr);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer flush vdev_id %d peer_addr %pM tids %08x\n",
 		   vdev_id, peer_addr, tid_bitmap);
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->peer_flush_tids_cmdid);
@@ -3304,7 +3859,7 @@ int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
 	struct wmi_peer_set_param_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3312,9 +3867,9 @@ int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
 	cmd->vdev_id     = __cpu_to_le32(vdev_id);
 	cmd->param_id    = __cpu_to_le32(param_id);
 	cmd->param_value = __cpu_to_le32(param_value);
-	memcpy(&cmd->peer_macaddr.addr, peer_addr, ETH_ALEN);
+	ether_addr_copy(cmd->peer_macaddr.addr, peer_addr);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi vdev %d peer 0x%pM set param %d value %d\n",
 		   vdev_id, peer_addr, param_id, param_value);
 
@@ -3327,7 +3882,7 @@ int ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
 	struct wmi_sta_powersave_mode_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3335,7 +3890,7 @@ int ath10k_wmi_set_psmode(struct ath10k *ar, u32 vdev_id,
 	cmd->vdev_id     = __cpu_to_le32(vdev_id);
 	cmd->sta_ps_mode = __cpu_to_le32(psmode);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi set powersave id 0x%x mode %d\n",
 		   vdev_id, psmode);
 
@@ -3350,7 +3905,7 @@ int ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
 	struct wmi_sta_powersave_param_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3359,7 +3914,7 @@ int ath10k_wmi_set_sta_ps_param(struct ath10k *ar, u32 vdev_id,
 	cmd->param_id    = __cpu_to_le32(param_id);
 	cmd->param_value = __cpu_to_le32(value);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi sta ps param vdev_id 0x%x param %d value %d\n",
 		   vdev_id, param_id, value);
 	return ath10k_wmi_cmd_send(ar, skb,
@@ -3375,7 +3930,7 @@ int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
 	if (!mac)
 		return -EINVAL;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3383,9 +3938,9 @@ int ath10k_wmi_set_ap_ps_param(struct ath10k *ar, u32 vdev_id, const u8 *mac,
 	cmd->vdev_id = __cpu_to_le32(vdev_id);
 	cmd->param_id = __cpu_to_le32(param_id);
 	cmd->param_value = __cpu_to_le32(value);
-	memcpy(&cmd->peer_macaddr, mac, ETH_ALEN);
+	ether_addr_copy(cmd->peer_macaddr.addr, mac);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi ap ps param vdev_id 0x%X param %d value %d mac_addr %pM\n",
 		   vdev_id, param_id, value, mac);
 
@@ -3405,7 +3960,7 @@ int ath10k_wmi_scan_chan_list(struct ath10k *ar,
 
 	len = sizeof(*cmd) + arg->n_channels * sizeof(struct wmi_channel);
 
-	skb = ath10k_wmi_alloc_skb(len);
+	skb = ath10k_wmi_alloc_skb(ar, len);
 	if (!skb)
 		return -EINVAL;
 
@@ -3447,24 +4002,12 @@ int ath10k_wmi_scan_chan_list(struct ath10k *ar,
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->scan_chan_list_cmdid);
 }
 
-int ath10k_wmi_peer_assoc(struct ath10k *ar,
-			  const struct wmi_peer_assoc_complete_arg *arg)
+static void
+ath10k_wmi_peer_assoc_fill(struct ath10k *ar, void *buf,
+			   const struct wmi_peer_assoc_complete_arg *arg)
 {
-	struct wmi_peer_assoc_complete_cmd *cmd;
-	struct sk_buff *skb;
+	struct wmi_common_peer_assoc_complete_cmd *cmd = buf;
 
-	if (arg->peer_mpdu_density > 16)
-		return -EINVAL;
-	if (arg->peer_legacy_rates.num_rates > MAX_SUPPORTED_RATES)
-		return -EINVAL;
-	if (arg->peer_ht_rates.num_rates > MAX_SUPPORTED_RATES)
-		return -EINVAL;
-
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
-	if (!skb)
-		return -ENOMEM;
-
-	cmd = (struct wmi_peer_assoc_complete_cmd *)skb->data;
 	cmd->vdev_id            = __cpu_to_le32(arg->vdev_id);
 	cmd->peer_new_assoc     = __cpu_to_le32(arg->peer_reassoc ? 0 : 1);
 	cmd->peer_associd       = __cpu_to_le32(arg->peer_aid);
@@ -3479,7 +4022,7 @@ int ath10k_wmi_peer_assoc(struct ath10k *ar,
 	cmd->peer_vht_caps      = __cpu_to_le32(arg->peer_vht_caps);
 	cmd->peer_phymode       = __cpu_to_le32(arg->peer_phymode);
 
-	memcpy(cmd->peer_macaddr.addr, arg->addr, ETH_ALEN);
+	ether_addr_copy(cmd->peer_macaddr.addr, arg->addr);
 
 	cmd->peer_legacy_rates.num_rates =
 		__cpu_to_le32(arg->peer_legacy_rates.num_rates);
@@ -3499,8 +4042,80 @@ int ath10k_wmi_peer_assoc(struct ath10k *ar,
 		__cpu_to_le32(arg->peer_vht_rates.tx_max_rate);
 	cmd->peer_vht_rates.tx_mcs_set =
 		__cpu_to_le32(arg->peer_vht_rates.tx_mcs_set);
+}
+
+static void
+ath10k_wmi_peer_assoc_fill_main(struct ath10k *ar, void *buf,
+				const struct wmi_peer_assoc_complete_arg *arg)
+{
+	struct wmi_main_peer_assoc_complete_cmd *cmd = buf;
+
+	ath10k_wmi_peer_assoc_fill(ar, buf, arg);
+	memset(cmd->peer_ht_info, 0, sizeof(cmd->peer_ht_info));
+}
+
+static void
+ath10k_wmi_peer_assoc_fill_10_1(struct ath10k *ar, void *buf,
+				const struct wmi_peer_assoc_complete_arg *arg)
+{
+	ath10k_wmi_peer_assoc_fill(ar, buf, arg);
+}
+
+static void
+ath10k_wmi_peer_assoc_fill_10_2(struct ath10k *ar, void *buf,
+				const struct wmi_peer_assoc_complete_arg *arg)
+{
+	struct wmi_10_2_peer_assoc_complete_cmd *cmd = buf;
+	int max_mcs, max_nss;
+	u32 info0;
+
+	/* TODO: Is using max values okay with firmware? */
+	max_mcs = 0xf;
+	max_nss = 0xf;
+
+	info0 = SM(max_mcs, WMI_PEER_ASSOC_INFO0_MAX_MCS_IDX) |
+		SM(max_nss, WMI_PEER_ASSOC_INFO0_MAX_NSS);
+
+	ath10k_wmi_peer_assoc_fill(ar, buf, arg);
+	cmd->info0 = __cpu_to_le32(info0);
+}
+
+int ath10k_wmi_peer_assoc(struct ath10k *ar,
+			  const struct wmi_peer_assoc_complete_arg *arg)
+{
+	struct sk_buff *skb;
+	int len;
+
+	if (arg->peer_mpdu_density > 16)
+		return -EINVAL;
+	if (arg->peer_legacy_rates.num_rates > MAX_SUPPORTED_RATES)
+		return -EINVAL;
+	if (arg->peer_ht_rates.num_rates > MAX_SUPPORTED_RATES)
+		return -EINVAL;
+
+	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
+			len = sizeof(struct wmi_10_2_peer_assoc_complete_cmd);
+		else
+			len = sizeof(struct wmi_10_1_peer_assoc_complete_cmd);
+	} else {
+		len = sizeof(struct wmi_main_peer_assoc_complete_cmd);
+	}
+
+	skb = ath10k_wmi_alloc_skb(ar, len);
+	if (!skb)
+		return -ENOMEM;
+
+	if (test_bit(ATH10K_FW_FEATURE_WMI_10X, ar->fw_features)) {
+		if (test_bit(ATH10K_FW_FEATURE_WMI_10_2, ar->fw_features))
+			ath10k_wmi_peer_assoc_fill_10_1(ar, skb->data, arg);
+		else
+			ath10k_wmi_peer_assoc_fill_10_2(ar, skb->data, arg);
+	} else {
+		ath10k_wmi_peer_assoc_fill_main(ar, skb->data, arg);
+	}
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi peer assoc vdev %d addr %pM (%s)\n",
 		   arg->vdev_id, arg->addr,
 		   arg->peer_reassoc ? "reassociate" : "new");
@@ -3518,7 +4133,7 @@ int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif)
 	int ret;
 	u16 fc;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3532,6 +4147,7 @@ int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif)
 	cmd->msdu_id = 0;
 	cmd->frame_control = __cpu_to_le32(fc);
 	cmd->flags = 0;
+	cmd->antenna_mask = __cpu_to_le32(WMI_BCN_TX_REF_DEF_ANTENNA);
 
 	if (ATH10K_SKB_CB(beacon)->bcn.dtim_zero)
 		cmd->flags |= __cpu_to_le32(WMI_BCN_TX_REF_FLAG_DTIM_ZERO);
@@ -3560,12 +4176,12 @@ static void ath10k_wmi_pdev_set_wmm_param(struct wmi_wmm_params *params,
 }
 
 int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
-			const struct wmi_pdev_set_wmm_params_arg *arg)
+				   const struct wmi_pdev_set_wmm_params_arg *arg)
 {
 	struct wmi_pdev_set_wmm_params *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3575,7 +4191,7 @@ int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
 	ath10k_wmi_pdev_set_wmm_param(&cmd->ac_vi, &arg->ac_vi);
 	ath10k_wmi_pdev_set_wmm_param(&cmd->ac_vo, &arg->ac_vo);
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi pdev set wmm params\n");
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi pdev set wmm params\n");
 	return ath10k_wmi_cmd_send(ar, skb,
 				   ar->wmi.cmd->pdev_set_wmm_params_cmdid);
 }
@@ -3585,14 +4201,14 @@ int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id)
 	struct wmi_request_stats_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
 	cmd = (struct wmi_request_stats_cmd *)skb->data;
 	cmd->stats_id = __cpu_to_le32(stats_id);
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi request stats %d\n", (int)stats_id);
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi request stats %d\n", (int)stats_id);
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->request_stats_cmdid);
 }
 
@@ -3602,7 +4218,7 @@ int ath10k_wmi_force_fw_hang(struct ath10k *ar,
 	struct wmi_force_fw_hang_cmd *cmd;
 	struct sk_buff *skb;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3610,7 +4226,7 @@ int ath10k_wmi_force_fw_hang(struct ath10k *ar,
 	cmd->type = __cpu_to_le32(type);
 	cmd->delay_ms = __cpu_to_le32(delay_ms);
 
-	ath10k_dbg(ATH10K_DBG_WMI, "wmi force fw hang %d delay %d\n",
+	ath10k_dbg(ar, ATH10K_DBG_WMI, "wmi force fw hang %d delay %d\n",
 		   type, delay_ms);
 	return ath10k_wmi_cmd_send(ar, skb, ar->wmi.cmd->force_fw_hang_cmdid);
 }
@@ -3621,7 +4237,7 @@ int ath10k_wmi_dbglog_cfg(struct ath10k *ar, u32 module_enable)
 	struct sk_buff *skb;
 	u32 cfg;
 
-	skb = ath10k_wmi_alloc_skb(sizeof(*cmd));
+	skb = ath10k_wmi_alloc_skb(ar, sizeof(*cmd));
 	if (!skb)
 		return -ENOMEM;
 
@@ -3642,7 +4258,7 @@ int ath10k_wmi_dbglog_cfg(struct ath10k *ar, u32 module_enable)
 	cmd->config_enable = __cpu_to_le32(cfg);
 	cmd->config_valid = __cpu_to_le32(ATH10K_DBGLOG_CFG_LOG_LVL_MASK);
 
-	ath10k_dbg(ATH10K_DBG_WMI,
+	ath10k_dbg(ar, ATH10K_DBG_WMI,
 		   "wmi dbglog cfg modules %08x %08x config %08x %08x\n",
 		   __le32_to_cpu(cmd->module_enable),
 		   __le32_to_cpu(cmd->module_valid),
diff --git a/drivers/net/wireless/ath/ath10k/wmi.h b/drivers/net/wireless/ath/ath10k/wmi.h
index e93df2c10413..86f5ebccfe79 100644
--- a/drivers/net/wireless/ath/ath10k/wmi.h
+++ b/drivers/net/wireless/ath/ath10k/wmi.h
@@ -73,119 +73,280 @@ struct wmi_cmd_hdr {
 #define HTC_PROTOCOL_VERSION    0x0002
 #define WMI_PROTOCOL_VERSION    0x0002
 
-enum wmi_service_id {
-	WMI_SERVICE_BEACON_OFFLOAD = 0,   /* beacon offload */
-	WMI_SERVICE_SCAN_OFFLOAD,	  /* scan offload */
-	WMI_SERVICE_ROAM_OFFLOAD,	  /* roam offload */
-	WMI_SERVICE_BCN_MISS_OFFLOAD,     /* beacon miss offload */
-	WMI_SERVICE_STA_PWRSAVE,	  /* fake sleep + basic power save */
-	WMI_SERVICE_STA_ADVANCED_PWRSAVE, /* uapsd, pspoll, force sleep */
-	WMI_SERVICE_AP_UAPSD,		  /* uapsd on AP */
-	WMI_SERVICE_AP_DFS,		  /* DFS on AP */
-	WMI_SERVICE_11AC,		  /* supports 11ac */
-	WMI_SERVICE_BLOCKACK,	/* Supports triggering ADDBA/DELBA from host*/
-	WMI_SERVICE_PHYERR,		  /* PHY error */
-	WMI_SERVICE_BCN_FILTER,		  /* Beacon filter support */
-	WMI_SERVICE_RTT,		  /* RTT (round trip time) support */
-	WMI_SERVICE_RATECTRL,		  /* Rate-control */
-	WMI_SERVICE_WOW,		  /* WOW Support */
-	WMI_SERVICE_RATECTRL_CACHE,       /* Rate-control caching */
-	WMI_SERVICE_IRAM_TIDS,            /* TIDs in IRAM */
-	WMI_SERVICE_ARPNS_OFFLOAD,	  /* ARP NS Offload support */
-	WMI_SERVICE_NLO,		  /* Network list offload service */
-	WMI_SERVICE_GTK_OFFLOAD,	  /* GTK offload */
-	WMI_SERVICE_SCAN_SCH,		  /* Scan Scheduler Service */
-	WMI_SERVICE_CSA_OFFLOAD,	  /* CSA offload service */
-	WMI_SERVICE_CHATTER,		  /* Chatter service */
-	WMI_SERVICE_COEX_FREQAVOID,	  /* FW report freq range to avoid */
-	WMI_SERVICE_PACKET_POWER_SAVE,	  /* packet power save service */
-	WMI_SERVICE_FORCE_FW_HANG,        /* To test fw recovery mechanism */
-	WMI_SERVICE_GPIO,                 /* GPIO service */
-	WMI_SERVICE_STA_DTIM_PS_MODULATED_DTIM, /* Modulated DTIM support */
-	WMI_STA_UAPSD_BASIC_AUTO_TRIG,    /* UAPSD AC Trigger Generation  */
-	WMI_STA_UAPSD_VAR_AUTO_TRIG,      /* -do- */
-	WMI_SERVICE_STA_KEEP_ALIVE,       /* STA keep alive mechanism support */
-	WMI_SERVICE_TX_ENCAP,             /* Packet type for TX encapsulation */
-
-	WMI_SERVICE_LAST,
-	WMI_MAX_SERVICE = 64		  /* max service */
+enum wmi_service {
+	WMI_SERVICE_BEACON_OFFLOAD = 0,
+	WMI_SERVICE_SCAN_OFFLOAD,
+	WMI_SERVICE_ROAM_OFFLOAD,
+	WMI_SERVICE_BCN_MISS_OFFLOAD,
+	WMI_SERVICE_STA_PWRSAVE,
+	WMI_SERVICE_STA_ADVANCED_PWRSAVE,
+	WMI_SERVICE_AP_UAPSD,
+	WMI_SERVICE_AP_DFS,
+	WMI_SERVICE_11AC,
+	WMI_SERVICE_BLOCKACK,
+	WMI_SERVICE_PHYERR,
+	WMI_SERVICE_BCN_FILTER,
+	WMI_SERVICE_RTT,
+	WMI_SERVICE_RATECTRL,
+	WMI_SERVICE_WOW,
+	WMI_SERVICE_RATECTRL_CACHE,
+	WMI_SERVICE_IRAM_TIDS,
+	WMI_SERVICE_ARPNS_OFFLOAD,
+	WMI_SERVICE_NLO,
+	WMI_SERVICE_GTK_OFFLOAD,
+	WMI_SERVICE_SCAN_SCH,
+	WMI_SERVICE_CSA_OFFLOAD,
+	WMI_SERVICE_CHATTER,
+	WMI_SERVICE_COEX_FREQAVOID,
+	WMI_SERVICE_PACKET_POWER_SAVE,
+	WMI_SERVICE_FORCE_FW_HANG,
+	WMI_SERVICE_GPIO,
+	WMI_SERVICE_STA_DTIM_PS_MODULATED_DTIM,
+	WMI_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG,
+	WMI_SERVICE_STA_UAPSD_VAR_AUTO_TRIG,
+	WMI_SERVICE_STA_KEEP_ALIVE,
+	WMI_SERVICE_TX_ENCAP,
+	WMI_SERVICE_BURST,
+	WMI_SERVICE_SMART_ANTENNA_SW_SUPPORT,
+	WMI_SERVICE_SMART_ANTENNA_HW_SUPPORT,
+
+	/* keep last */
+	WMI_SERVICE_MAX,
+};
+
+enum wmi_10x_service {
+	WMI_10X_SERVICE_BEACON_OFFLOAD = 0,
+	WMI_10X_SERVICE_SCAN_OFFLOAD,
+	WMI_10X_SERVICE_ROAM_OFFLOAD,
+	WMI_10X_SERVICE_BCN_MISS_OFFLOAD,
+	WMI_10X_SERVICE_STA_PWRSAVE,
+	WMI_10X_SERVICE_STA_ADVANCED_PWRSAVE,
+	WMI_10X_SERVICE_AP_UAPSD,
+	WMI_10X_SERVICE_AP_DFS,
+	WMI_10X_SERVICE_11AC,
+	WMI_10X_SERVICE_BLOCKACK,
+	WMI_10X_SERVICE_PHYERR,
+	WMI_10X_SERVICE_BCN_FILTER,
+	WMI_10X_SERVICE_RTT,
+	WMI_10X_SERVICE_RATECTRL,
+	WMI_10X_SERVICE_WOW,
+	WMI_10X_SERVICE_RATECTRL_CACHE,
+	WMI_10X_SERVICE_IRAM_TIDS,
+	WMI_10X_SERVICE_BURST,
+
+	/* introduced in 10.2 */
+	WMI_10X_SERVICE_SMART_ANTENNA_SW_SUPPORT,
+	WMI_10X_SERVICE_FORCE_FW_HANG,
+	WMI_10X_SERVICE_SMART_ANTENNA_HW_SUPPORT,
+};
+
+enum wmi_main_service {
+	WMI_MAIN_SERVICE_BEACON_OFFLOAD = 0,
+	WMI_MAIN_SERVICE_SCAN_OFFLOAD,
+	WMI_MAIN_SERVICE_ROAM_OFFLOAD,
+	WMI_MAIN_SERVICE_BCN_MISS_OFFLOAD,
+	WMI_MAIN_SERVICE_STA_PWRSAVE,
+	WMI_MAIN_SERVICE_STA_ADVANCED_PWRSAVE,
+	WMI_MAIN_SERVICE_AP_UAPSD,
+	WMI_MAIN_SERVICE_AP_DFS,
+	WMI_MAIN_SERVICE_11AC,
+	WMI_MAIN_SERVICE_BLOCKACK,
+	WMI_MAIN_SERVICE_PHYERR,
+	WMI_MAIN_SERVICE_BCN_FILTER,
+	WMI_MAIN_SERVICE_RTT,
+	WMI_MAIN_SERVICE_RATECTRL,
+	WMI_MAIN_SERVICE_WOW,
+	WMI_MAIN_SERVICE_RATECTRL_CACHE,
+	WMI_MAIN_SERVICE_IRAM_TIDS,
+	WMI_MAIN_SERVICE_ARPNS_OFFLOAD,
+	WMI_MAIN_SERVICE_NLO,
+	WMI_MAIN_SERVICE_GTK_OFFLOAD,
+	WMI_MAIN_SERVICE_SCAN_SCH,
+	WMI_MAIN_SERVICE_CSA_OFFLOAD,
+	WMI_MAIN_SERVICE_CHATTER,
+	WMI_MAIN_SERVICE_COEX_FREQAVOID,
+	WMI_MAIN_SERVICE_PACKET_POWER_SAVE,
+	WMI_MAIN_SERVICE_FORCE_FW_HANG,
+	WMI_MAIN_SERVICE_GPIO,
+	WMI_MAIN_SERVICE_STA_DTIM_PS_MODULATED_DTIM,
+	WMI_MAIN_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG,
+	WMI_MAIN_SERVICE_STA_UAPSD_VAR_AUTO_TRIG,
+	WMI_MAIN_SERVICE_STA_KEEP_ALIVE,
+	WMI_MAIN_SERVICE_TX_ENCAP,
 };
 
 static inline char *wmi_service_name(int service_id)
 {
+#define SVCSTR(x) case x: return #x
+
 	switch (service_id) {
-	case WMI_SERVICE_BEACON_OFFLOAD:
-		return "BEACON_OFFLOAD";
-	case WMI_SERVICE_SCAN_OFFLOAD:
-		return "SCAN_OFFLOAD";
-	case WMI_SERVICE_ROAM_OFFLOAD:
-		return "ROAM_OFFLOAD";
-	case WMI_SERVICE_BCN_MISS_OFFLOAD:
-		return "BCN_MISS_OFFLOAD";
-	case WMI_SERVICE_STA_PWRSAVE:
-		return "STA_PWRSAVE";
-	case WMI_SERVICE_STA_ADVANCED_PWRSAVE:
-		return "STA_ADVANCED_PWRSAVE";
-	case WMI_SERVICE_AP_UAPSD:
-		return "AP_UAPSD";
-	case WMI_SERVICE_AP_DFS:
-		return "AP_DFS";
-	case WMI_SERVICE_11AC:
-		return "11AC";
-	case WMI_SERVICE_BLOCKACK:
-		return "BLOCKACK";
-	case WMI_SERVICE_PHYERR:
-		return "PHYERR";
-	case WMI_SERVICE_BCN_FILTER:
-		return "BCN_FILTER";
-	case WMI_SERVICE_RTT:
-		return "RTT";
-	case WMI_SERVICE_RATECTRL:
-		return "RATECTRL";
-	case WMI_SERVICE_WOW:
-		return "WOW";
-	case WMI_SERVICE_RATECTRL_CACHE:
-		return "RATECTRL CACHE";
-	case WMI_SERVICE_IRAM_TIDS:
-		return "IRAM TIDS";
-	case WMI_SERVICE_ARPNS_OFFLOAD:
-		return "ARPNS_OFFLOAD";
-	case WMI_SERVICE_NLO:
-		return "NLO";
-	case WMI_SERVICE_GTK_OFFLOAD:
-		return "GTK_OFFLOAD";
-	case WMI_SERVICE_SCAN_SCH:
-		return "SCAN_SCH";
-	case WMI_SERVICE_CSA_OFFLOAD:
-		return "CSA_OFFLOAD";
-	case WMI_SERVICE_CHATTER:
-		return "CHATTER";
-	case WMI_SERVICE_COEX_FREQAVOID:
-		return "COEX_FREQAVOID";
-	case WMI_SERVICE_PACKET_POWER_SAVE:
-		return "PACKET_POWER_SAVE";
-	case WMI_SERVICE_FORCE_FW_HANG:
-		return "FORCE FW HANG";
-	case WMI_SERVICE_GPIO:
-		return "GPIO";
-	case WMI_SERVICE_STA_DTIM_PS_MODULATED_DTIM:
-		return "MODULATED DTIM";
-	case WMI_STA_UAPSD_BASIC_AUTO_TRIG:
-		return "BASIC UAPSD";
-	case WMI_STA_UAPSD_VAR_AUTO_TRIG:
-		return "VAR UAPSD";
-	case WMI_SERVICE_STA_KEEP_ALIVE:
-		return "STA KEEP ALIVE";
-	case WMI_SERVICE_TX_ENCAP:
-		return "TX ENCAP";
+	SVCSTR(WMI_SERVICE_BEACON_OFFLOAD);
+	SVCSTR(WMI_SERVICE_SCAN_OFFLOAD);
+	SVCSTR(WMI_SERVICE_ROAM_OFFLOAD);
+	SVCSTR(WMI_SERVICE_BCN_MISS_OFFLOAD);
+	SVCSTR(WMI_SERVICE_STA_PWRSAVE);
+	SVCSTR(WMI_SERVICE_STA_ADVANCED_PWRSAVE);
+	SVCSTR(WMI_SERVICE_AP_UAPSD);
+	SVCSTR(WMI_SERVICE_AP_DFS);
+	SVCSTR(WMI_SERVICE_11AC);
+	SVCSTR(WMI_SERVICE_BLOCKACK);
+	SVCSTR(WMI_SERVICE_PHYERR);
+	SVCSTR(WMI_SERVICE_BCN_FILTER);
+	SVCSTR(WMI_SERVICE_RTT);
+	SVCSTR(WMI_SERVICE_RATECTRL);
+	SVCSTR(WMI_SERVICE_WOW);
+	SVCSTR(WMI_SERVICE_RATECTRL_CACHE);
+	SVCSTR(WMI_SERVICE_IRAM_TIDS);
+	SVCSTR(WMI_SERVICE_ARPNS_OFFLOAD);
+	SVCSTR(WMI_SERVICE_NLO);
+	SVCSTR(WMI_SERVICE_GTK_OFFLOAD);
+	SVCSTR(WMI_SERVICE_SCAN_SCH);
+	SVCSTR(WMI_SERVICE_CSA_OFFLOAD);
+	SVCSTR(WMI_SERVICE_CHATTER);
+	SVCSTR(WMI_SERVICE_COEX_FREQAVOID);
+	SVCSTR(WMI_SERVICE_PACKET_POWER_SAVE);
+	SVCSTR(WMI_SERVICE_FORCE_FW_HANG);
+	SVCSTR(WMI_SERVICE_GPIO);
+	SVCSTR(WMI_SERVICE_STA_DTIM_PS_MODULATED_DTIM);
+	SVCSTR(WMI_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG);
+	SVCSTR(WMI_SERVICE_STA_UAPSD_VAR_AUTO_TRIG);
+	SVCSTR(WMI_SERVICE_STA_KEEP_ALIVE);
+	SVCSTR(WMI_SERVICE_TX_ENCAP);
+	SVCSTR(WMI_SERVICE_BURST);
+	SVCSTR(WMI_SERVICE_SMART_ANTENNA_SW_SUPPORT);
+	SVCSTR(WMI_SERVICE_SMART_ANTENNA_HW_SUPPORT);
 	default:
-		return "UNKNOWN SERVICE\n";
+		return NULL;
 	}
+
+#undef SVCSTR
+}
+
+#define WMI_SERVICE_IS_ENABLED(wmi_svc_bmap, svc_id) \
+	(__le32_to_cpu((wmi_svc_bmap)[(svc_id)/(sizeof(u32))]) & \
+	 BIT((svc_id)%(sizeof(u32))))
+
+#define SVCMAP(x, y) \
+	do { \
+		if (WMI_SERVICE_IS_ENABLED((in), (x))) \
+			__set_bit(y, out); \
+	} while (0)
+
+static inline void wmi_10x_svc_map(const __le32 *in, unsigned long *out)
+{
+	SVCMAP(WMI_10X_SERVICE_BEACON_OFFLOAD,
+	       WMI_SERVICE_BEACON_OFFLOAD);
+	SVCMAP(WMI_10X_SERVICE_SCAN_OFFLOAD,
+	       WMI_SERVICE_SCAN_OFFLOAD);
+	SVCMAP(WMI_10X_SERVICE_ROAM_OFFLOAD,
+	       WMI_SERVICE_ROAM_OFFLOAD);
+	SVCMAP(WMI_10X_SERVICE_BCN_MISS_OFFLOAD,
+	       WMI_SERVICE_BCN_MISS_OFFLOAD);
+	SVCMAP(WMI_10X_SERVICE_STA_PWRSAVE,
+	       WMI_SERVICE_STA_PWRSAVE);
+	SVCMAP(WMI_10X_SERVICE_STA_ADVANCED_PWRSAVE,
+	       WMI_SERVICE_STA_ADVANCED_PWRSAVE);
+	SVCMAP(WMI_10X_SERVICE_AP_UAPSD,
+	       WMI_SERVICE_AP_UAPSD);
+	SVCMAP(WMI_10X_SERVICE_AP_DFS,
+	       WMI_SERVICE_AP_DFS);
+	SVCMAP(WMI_10X_SERVICE_11AC,
+	       WMI_SERVICE_11AC);
+	SVCMAP(WMI_10X_SERVICE_BLOCKACK,
+	       WMI_SERVICE_BLOCKACK);
+	SVCMAP(WMI_10X_SERVICE_PHYERR,
+	       WMI_SERVICE_PHYERR);
+	SVCMAP(WMI_10X_SERVICE_BCN_FILTER,
+	       WMI_SERVICE_BCN_FILTER);
+	SVCMAP(WMI_10X_SERVICE_RTT,
+	       WMI_SERVICE_RTT);
+	SVCMAP(WMI_10X_SERVICE_RATECTRL,
+	       WMI_SERVICE_RATECTRL);
+	SVCMAP(WMI_10X_SERVICE_WOW,
+	       WMI_SERVICE_WOW);
+	SVCMAP(WMI_10X_SERVICE_RATECTRL_CACHE,
+	       WMI_SERVICE_RATECTRL_CACHE);
+	SVCMAP(WMI_10X_SERVICE_IRAM_TIDS,
+	       WMI_SERVICE_IRAM_TIDS);
+	SVCMAP(WMI_10X_SERVICE_BURST,
+	       WMI_SERVICE_BURST);
+	SVCMAP(WMI_10X_SERVICE_SMART_ANTENNA_SW_SUPPORT,
+	       WMI_SERVICE_SMART_ANTENNA_SW_SUPPORT);
+	SVCMAP(WMI_10X_SERVICE_FORCE_FW_HANG,
+	       WMI_SERVICE_FORCE_FW_HANG);
+	SVCMAP(WMI_10X_SERVICE_SMART_ANTENNA_HW_SUPPORT,
+	       WMI_SERVICE_SMART_ANTENNA_HW_SUPPORT);
 }
 
+static inline void wmi_main_svc_map(const __le32 *in, unsigned long *out)
+{
+	SVCMAP(WMI_MAIN_SERVICE_BEACON_OFFLOAD,
+	       WMI_SERVICE_BEACON_OFFLOAD);
+	SVCMAP(WMI_MAIN_SERVICE_SCAN_OFFLOAD,
+	       WMI_SERVICE_SCAN_OFFLOAD);
+	SVCMAP(WMI_MAIN_SERVICE_ROAM_OFFLOAD,
+	       WMI_SERVICE_ROAM_OFFLOAD);
+	SVCMAP(WMI_MAIN_SERVICE_BCN_MISS_OFFLOAD,
+	       WMI_SERVICE_BCN_MISS_OFFLOAD);
+	SVCMAP(WMI_MAIN_SERVICE_STA_PWRSAVE,
+	       WMI_SERVICE_STA_PWRSAVE);
+	SVCMAP(WMI_MAIN_SERVICE_STA_ADVANCED_PWRSAVE,
+	       WMI_SERVICE_STA_ADVANCED_PWRSAVE);
+	SVCMAP(WMI_MAIN_SERVICE_AP_UAPSD,
+	       WMI_SERVICE_AP_UAPSD);
+	SVCMAP(WMI_MAIN_SERVICE_AP_DFS,
+	       WMI_SERVICE_AP_DFS);
+	SVCMAP(WMI_MAIN_SERVICE_11AC,
+	       WMI_SERVICE_11AC);
+	SVCMAP(WMI_MAIN_SERVICE_BLOCKACK,
+	       WMI_SERVICE_BLOCKACK);
+	SVCMAP(WMI_MAIN_SERVICE_PHYERR,
+	       WMI_SERVICE_PHYERR);
+	SVCMAP(WMI_MAIN_SERVICE_BCN_FILTER,
+	       WMI_SERVICE_BCN_FILTER);
+	SVCMAP(WMI_MAIN_SERVICE_RTT,
+	       WMI_SERVICE_RTT);
+	SVCMAP(WMI_MAIN_SERVICE_RATECTRL,
+	       WMI_SERVICE_RATECTRL);
+	SVCMAP(WMI_MAIN_SERVICE_WOW,
+	       WMI_SERVICE_WOW);
+	SVCMAP(WMI_MAIN_SERVICE_RATECTRL_CACHE,
+	       WMI_SERVICE_RATECTRL_CACHE);
+	SVCMAP(WMI_MAIN_SERVICE_IRAM_TIDS,
+	       WMI_SERVICE_IRAM_TIDS);
+	SVCMAP(WMI_MAIN_SERVICE_ARPNS_OFFLOAD,
+	       WMI_SERVICE_ARPNS_OFFLOAD);
+	SVCMAP(WMI_MAIN_SERVICE_NLO,
+	       WMI_SERVICE_NLO);
+	SVCMAP(WMI_MAIN_SERVICE_GTK_OFFLOAD,
+	       WMI_SERVICE_GTK_OFFLOAD);
+	SVCMAP(WMI_MAIN_SERVICE_SCAN_SCH,
+	       WMI_SERVICE_SCAN_SCH);
+	SVCMAP(WMI_MAIN_SERVICE_CSA_OFFLOAD,
+	       WMI_SERVICE_CSA_OFFLOAD);
+	SVCMAP(WMI_MAIN_SERVICE_CHATTER,
+	       WMI_SERVICE_CHATTER);
+	SVCMAP(WMI_MAIN_SERVICE_COEX_FREQAVOID,
+	       WMI_SERVICE_COEX_FREQAVOID);
+	SVCMAP(WMI_MAIN_SERVICE_PACKET_POWER_SAVE,
+	       WMI_SERVICE_PACKET_POWER_SAVE);
+	SVCMAP(WMI_MAIN_SERVICE_FORCE_FW_HANG,
+	       WMI_SERVICE_FORCE_FW_HANG);
+	SVCMAP(WMI_MAIN_SERVICE_GPIO,
+	       WMI_SERVICE_GPIO);
+	SVCMAP(WMI_MAIN_SERVICE_STA_DTIM_PS_MODULATED_DTIM,
+	       WMI_SERVICE_STA_DTIM_PS_MODULATED_DTIM);
+	SVCMAP(WMI_MAIN_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG,
+	       WMI_SERVICE_STA_UAPSD_BASIC_AUTO_TRIG);
+	SVCMAP(WMI_MAIN_SERVICE_STA_UAPSD_VAR_AUTO_TRIG,
+	       WMI_SERVICE_STA_UAPSD_VAR_AUTO_TRIG);
+	SVCMAP(WMI_MAIN_SERVICE_STA_KEEP_ALIVE,
+	       WMI_SERVICE_STA_KEEP_ALIVE);
+	SVCMAP(WMI_MAIN_SERVICE_TX_ENCAP,
+	       WMI_SERVICE_TX_ENCAP);
+}
 
-#define WMI_SERVICE_BM_SIZE \
-	((WMI_MAX_SERVICE + sizeof(u32) - 1)/sizeof(u32))
+#undef SVCMAP
 
 /* 2 word representation of MAC addr */
 struct wmi_mac_addr {
@@ -803,6 +964,159 @@ enum wmi_10x_event_id {
 	WMI_10X_PDEV_UTF_EVENTID = WMI_10X_END_EVENTID-1,
 };
 
+enum wmi_10_2_cmd_id {
+	WMI_10_2_START_CMDID = 0x9000,
+	WMI_10_2_END_CMDID = 0x9FFF,
+	WMI_10_2_INIT_CMDID,
+	WMI_10_2_START_SCAN_CMDID = WMI_10_2_START_CMDID,
+	WMI_10_2_STOP_SCAN_CMDID,
+	WMI_10_2_SCAN_CHAN_LIST_CMDID,
+	WMI_10_2_ECHO_CMDID,
+	WMI_10_2_PDEV_SET_REGDOMAIN_CMDID,
+	WMI_10_2_PDEV_SET_CHANNEL_CMDID,
+	WMI_10_2_PDEV_SET_PARAM_CMDID,
+	WMI_10_2_PDEV_PKTLOG_ENABLE_CMDID,
+	WMI_10_2_PDEV_PKTLOG_DISABLE_CMDID,
+	WMI_10_2_PDEV_SET_WMM_PARAMS_CMDID,
+	WMI_10_2_PDEV_SET_HT_CAP_IE_CMDID,
+	WMI_10_2_PDEV_SET_VHT_CAP_IE_CMDID,
+	WMI_10_2_PDEV_SET_BASE_MACADDR_CMDID,
+	WMI_10_2_PDEV_SET_QUIET_MODE_CMDID,
+	WMI_10_2_PDEV_GREEN_AP_PS_ENABLE_CMDID,
+	WMI_10_2_PDEV_GET_TPC_CONFIG_CMDID,
+	WMI_10_2_VDEV_CREATE_CMDID,
+	WMI_10_2_VDEV_DELETE_CMDID,
+	WMI_10_2_VDEV_START_REQUEST_CMDID,
+	WMI_10_2_VDEV_RESTART_REQUEST_CMDID,
+	WMI_10_2_VDEV_UP_CMDID,
+	WMI_10_2_VDEV_STOP_CMDID,
+	WMI_10_2_VDEV_DOWN_CMDID,
+	WMI_10_2_VDEV_STANDBY_RESPONSE_CMDID,
+	WMI_10_2_VDEV_RESUME_RESPONSE_CMDID,
+	WMI_10_2_VDEV_SET_PARAM_CMDID,
+	WMI_10_2_VDEV_INSTALL_KEY_CMDID,
+	WMI_10_2_VDEV_SET_DSCP_TID_MAP_CMDID,
+	WMI_10_2_PEER_CREATE_CMDID,
+	WMI_10_2_PEER_DELETE_CMDID,
+	WMI_10_2_PEER_FLUSH_TIDS_CMDID,
+	WMI_10_2_PEER_SET_PARAM_CMDID,
+	WMI_10_2_PEER_ASSOC_CMDID,
+	WMI_10_2_PEER_ADD_WDS_ENTRY_CMDID,
+	WMI_10_2_PEER_UPDATE_WDS_ENTRY_CMDID,
+	WMI_10_2_PEER_REMOVE_WDS_ENTRY_CMDID,
+	WMI_10_2_PEER_MCAST_GROUP_CMDID,
+	WMI_10_2_BCN_TX_CMDID,
+	WMI_10_2_BCN_PRB_TMPL_CMDID,
+	WMI_10_2_BCN_FILTER_RX_CMDID,
+	WMI_10_2_PRB_REQ_FILTER_RX_CMDID,
+	WMI_10_2_MGMT_TX_CMDID,
+	WMI_10_2_ADDBA_CLEAR_RESP_CMDID,
+	WMI_10_2_ADDBA_SEND_CMDID,
+	WMI_10_2_ADDBA_STATUS_CMDID,
+	WMI_10_2_DELBA_SEND_CMDID,
+	WMI_10_2_ADDBA_SET_RESP_CMDID,
+	WMI_10_2_SEND_SINGLEAMSDU_CMDID,
+	WMI_10_2_STA_POWERSAVE_MODE_CMDID,
+	WMI_10_2_STA_POWERSAVE_PARAM_CMDID,
+	WMI_10_2_STA_MIMO_PS_MODE_CMDID,
+	WMI_10_2_DBGLOG_CFG_CMDID,
+	WMI_10_2_PDEV_DFS_ENABLE_CMDID,
+	WMI_10_2_PDEV_DFS_DISABLE_CMDID,
+	WMI_10_2_PDEV_QVIT_CMDID,
+	WMI_10_2_ROAM_SCAN_MODE,
+	WMI_10_2_ROAM_SCAN_RSSI_THRESHOLD,
+	WMI_10_2_ROAM_SCAN_PERIOD,
+	WMI_10_2_ROAM_SCAN_RSSI_CHANGE_THRESHOLD,
+	WMI_10_2_ROAM_AP_PROFILE,
+	WMI_10_2_OFL_SCAN_ADD_AP_PROFILE,
+	WMI_10_2_OFL_SCAN_REMOVE_AP_PROFILE,
+	WMI_10_2_OFL_SCAN_PERIOD,
+	WMI_10_2_P2P_DEV_SET_DEVICE_INFO,
+	WMI_10_2_P2P_DEV_SET_DISCOVERABILITY,
+	WMI_10_2_P2P_GO_SET_BEACON_IE,
+	WMI_10_2_P2P_GO_SET_PROBE_RESP_IE,
+	WMI_10_2_AP_PS_PEER_PARAM_CMDID,
+	WMI_10_2_AP_PS_PEER_UAPSD_COEX_CMDID,
+	WMI_10_2_PEER_RATE_RETRY_SCHED_CMDID,
+	WMI_10_2_WLAN_PROFILE_TRIGGER_CMDID,
+	WMI_10_2_WLAN_PROFILE_SET_HIST_INTVL_CMDID,
+	WMI_10_2_WLAN_PROFILE_GET_PROFILE_DATA_CMDID,
+	WMI_10_2_WLAN_PROFILE_ENABLE_PROFILE_ID_CMDID,
+	WMI_10_2_WLAN_PROFILE_LIST_PROFILE_ID_CMDID,
+	WMI_10_2_PDEV_SUSPEND_CMDID,
+	WMI_10_2_PDEV_RESUME_CMDID,
+	WMI_10_2_ADD_BCN_FILTER_CMDID,
+	WMI_10_2_RMV_BCN_FILTER_CMDID,
+	WMI_10_2_WOW_ADD_WAKE_PATTERN_CMDID,
+	WMI_10_2_WOW_DEL_WAKE_PATTERN_CMDID,
+	WMI_10_2_WOW_ENABLE_DISABLE_WAKE_EVENT_CMDID,
+	WMI_10_2_WOW_ENABLE_CMDID,
+	WMI_10_2_WOW_HOSTWAKEUP_FROM_SLEEP_CMDID,
+	WMI_10_2_RTT_MEASREQ_CMDID,
+	WMI_10_2_RTT_TSF_CMDID,
+	WMI_10_2_RTT_KEEPALIVE_CMDID,
+	WMI_10_2_PDEV_SEND_BCN_CMDID,
+	WMI_10_2_VDEV_SPECTRAL_SCAN_CONFIGURE_CMDID,
+	WMI_10_2_VDEV_SPECTRAL_SCAN_ENABLE_CMDID,
+	WMI_10_2_REQUEST_STATS_CMDID,
+	WMI_10_2_GPIO_CONFIG_CMDID,
+	WMI_10_2_GPIO_OUTPUT_CMDID,
+	WMI_10_2_VDEV_RATEMASK_CMDID,
+	WMI_10_2_PDEV_SMART_ANT_ENABLE_CMDID,
+	WMI_10_2_PDEV_SMART_ANT_SET_RX_ANTENNA_CMDID,
+	WMI_10_2_PEER_SMART_ANT_SET_TX_ANTENNA_CMDID,
+	WMI_10_2_PEER_SMART_ANT_SET_TRAIN_INFO_CMDID,
+	WMI_10_2_PEER_SMART_ANT_SET_NODE_CONFIG_OPS_CMDID,
+	WMI_10_2_FORCE_FW_HANG_CMDID,
+	WMI_10_2_PDEV_SET_ANTENNA_SWITCH_TABLE_CMDID,
+	WMI_10_2_PDEV_SET_CTL_TABLE_CMDID,
+	WMI_10_2_PDEV_SET_MIMOGAIN_TABLE_CMDID,
+	WMI_10_2_PDEV_RATEPWR_TABLE_CMDID,
+	WMI_10_2_PDEV_RATEPWR_CHAINMSK_TABLE_CMDID,
+	WMI_10_2_PDEV_UTF_CMDID = WMI_10_2_END_CMDID - 1,
+};
+
+enum wmi_10_2_event_id {
+	WMI_10_2_SERVICE_READY_EVENTID = 0x8000,
+	WMI_10_2_READY_EVENTID,
+	WMI_10_2_DEBUG_MESG_EVENTID,
+	WMI_10_2_START_EVENTID = 0x9000,
+	WMI_10_2_END_EVENTID = 0x9FFF,
+	WMI_10_2_SCAN_EVENTID = WMI_10_2_START_EVENTID,
+	WMI_10_2_ECHO_EVENTID,
+	WMI_10_2_UPDATE_STATS_EVENTID,
+	WMI_10_2_INST_RSSI_STATS_EVENTID,
+	WMI_10_2_VDEV_START_RESP_EVENTID,
+	WMI_10_2_VDEV_STANDBY_REQ_EVENTID,
+	WMI_10_2_VDEV_RESUME_REQ_EVENTID,
+	WMI_10_2_VDEV_STOPPED_EVENTID,
+	WMI_10_2_PEER_STA_KICKOUT_EVENTID,
+	WMI_10_2_HOST_SWBA_EVENTID,
+	WMI_10_2_TBTTOFFSET_UPDATE_EVENTID,
+	WMI_10_2_MGMT_RX_EVENTID,
+	WMI_10_2_CHAN_INFO_EVENTID,
+	WMI_10_2_PHYERR_EVENTID,
+	WMI_10_2_ROAM_EVENTID,
+	WMI_10_2_PROFILE_MATCH,
+	WMI_10_2_DEBUG_PRINT_EVENTID,
+	WMI_10_2_PDEV_QVIT_EVENTID,
+	WMI_10_2_WLAN_PROFILE_DATA_EVENTID,
+	WMI_10_2_RTT_MEASUREMENT_REPORT_EVENTID,
+	WMI_10_2_TSF_MEASUREMENT_REPORT_EVENTID,
+	WMI_10_2_RTT_ERROR_REPORT_EVENTID,
+	WMI_10_2_RTT_KEEPALIVE_EVENTID,
+	WMI_10_2_WOW_WAKEUP_HOST_EVENTID,
+	WMI_10_2_DCS_INTERFERENCE_EVENTID,
+	WMI_10_2_PDEV_TPC_CONFIG_EVENTID,
+	WMI_10_2_GPIO_INPUT_EVENTID,
+	WMI_10_2_PEER_RATECODE_LIST_EVENTID,
+	WMI_10_2_GENERIC_BUFFER_EVENTID,
+	WMI_10_2_MCAST_BUF_RELEASE_EVENTID,
+	WMI_10_2_MCAST_LIST_AGEOUT_EVENTID,
+	WMI_10_2_WDS_PEER_EVENTID,
+	WMI_10_2_PDEV_UTF_EVENTID = WMI_10_2_END_EVENTID - 1,
+};
+
 enum wmi_phy_mode {
 	MODE_11A        = 0,   /* 11a Mode */
 	MODE_11G        = 1,   /* 11b/g Mode */
@@ -955,7 +1269,6 @@ enum wmi_channel_change_cause {
 				WMI_HT_CAP_RX_STBC       | \
 				WMI_HT_CAP_LDPC)
 
-
 /*
  * WMI_VHT_CAP_* these maps to ieee 802.11ac vht capability information
  * field. The fields not defined here are not supported, or reserved.
@@ -1076,10 +1389,6 @@ struct wlan_host_mem_req {
 	__le32 num_units;
 } __packed;
 
-#define WMI_SERVICE_IS_ENABLED(wmi_svc_bmap, svc_id) \
-	((((wmi_svc_bmap)[(svc_id)/(sizeof(u32))]) & \
-	(1 << ((svc_id)%(sizeof(u32))))) != 0)
-
 /*
  * The following struct holds optional payload for
  * wmi_service_ready_event,e.g., 11ac pass some of the
@@ -1093,7 +1402,7 @@ struct wmi_service_ready_event {
 	__le32 phy_capability;
 	/* Maximum number of frag table entries that SW will populate less 1 */
 	__le32 max_frag_entry;
-	__le32 wmi_service_bitmap[WMI_SERVICE_BM_SIZE];
+	__le32 wmi_service_bitmap[16];
 	__le32 num_rf_chains;
 	/*
 	 * The following field is only valid for service type
@@ -1132,7 +1441,7 @@ struct wmi_service_ready_event_10x {
 
 	/* Maximum number of frag table entries that SW will populate less 1 */
 	__le32 max_frag_entry;
-	__le32 wmi_service_bitmap[WMI_SERVICE_BM_SIZE];
+	__le32 wmi_service_bitmap[16];
 	__le32 num_rf_chains;
 
 	/*
@@ -1161,7 +1470,6 @@ struct wmi_service_ready_event_10x {
 	struct wlan_host_mem_req mem_reqs[1];
 } __packed;
 
-
 #define WMI_SERVICE_READY_TIMEOUT_HZ (5*HZ)
 #define WMI_UNIFIED_READY_TIMEOUT_HZ (5*HZ)
 
@@ -1551,6 +1859,16 @@ struct wmi_resource_config_10x {
 	__le32 max_frag_entries;
 } __packed;
 
+struct wmi_resource_config_10_2 {
+	struct wmi_resource_config_10x common;
+	__le32 max_peer_ext_stats;
+	__le32 smart_ant_cap; /* 0-disable, 1-enable */
+	__le32 bk_min_free;
+	__le32 be_min_free;
+	__le32 vi_min_free;
+	__le32 vo_min_free;
+	__le32 rx_batchmode; /* 0-disable, 1-enable */
+} __packed;
 
 #define NUM_UNITS_IS_NUM_VDEVS   0x1
 #define NUM_UNITS_IS_NUM_PEERS   0x2
@@ -1588,11 +1906,28 @@ struct wmi_init_cmd_10x {
 	struct host_memory_chunk host_mem_chunks[1];
 } __packed;
 
+struct wmi_init_cmd_10_2 {
+	struct wmi_resource_config_10_2 resource_config;
+	__le32 num_host_mem_chunks;
+
+	/*
+	 * variable number of host memory chunks.
+	 * This should be the last element in the structure
+	 */
+	struct host_memory_chunk host_mem_chunks[1];
+} __packed;
+
+struct wmi_chan_list_entry {
+	__le16 freq;
+	u8 phy_mode; /* valid for 10.2 only */
+	u8 reserved;
+} __packed;
+
 /* TLV for channel list */
 struct wmi_chan_list {
 	__le32 tag; /* WMI_CHAN_LIST_TAG */
 	__le32 num_chan;
-	__le32 channel_list[0];
+	struct wmi_chan_list_entry channel_list[0];
 } __packed;
 
 struct wmi_bssid_list {
@@ -1788,7 +2123,6 @@ struct wmi_start_scan_cmd_10x {
 	 */
 } __packed;
 
-
 struct wmi_ssid_arg {
 	int len;
 	const u8 *ssid;
@@ -1821,7 +2155,7 @@ struct wmi_start_scan_arg {
 	u32 n_bssids;
 
 	u8 ie[WLAN_SCAN_PARAMS_MAX_IE_LEN];
-	u32 channels[64];
+	u16 channels[64];
 	struct wmi_ssid_arg ssids[WLAN_SCAN_PARAMS_MAX_SSID];
 	struct wmi_bssid_arg bssids[WLAN_SCAN_PARAMS_MAX_BSSID];
 };
@@ -1849,7 +2183,6 @@ struct wmi_start_scan_arg {
 /* WMI_SCAN_CLASS_MASK must be the same value as IEEE80211_SCAN_CLASS_MASK */
 #define WMI_SCAN_CLASS_MASK 0xFF000000
 
-
 enum wmi_stop_scan_type {
 	WMI_SCAN_STOP_ONE	= 0x00000000, /* stop by scan_id */
 	WMI_SCAN_STOP_VDEV_ALL	= 0x01000000, /* stop by vdev_id */
@@ -2034,7 +2367,6 @@ struct wmi_single_phyerr_rx_hdr {
 	__le32 nf_list_1;
 	__le32 nf_list_2;
 
-
 	/* Length of the frame */
 	__le32 buf_len;
 } __packed;
@@ -2067,6 +2399,7 @@ struct wmi_comb_phyerr_rx_event {
 #define PHYERR_TLV_SIG				0xBB
 #define PHYERR_TLV_TAG_SEARCH_FFT_REPORT	0xFB
 #define PHYERR_TLV_TAG_RADAR_PULSE_SUMMARY	0xF8
+#define PHYERR_TLV_TAG_SPECTRAL_SUMMARY_REPORT	0xF9
 
 struct phyerr_radar_report {
 	__le32 reg0; /* RADAR_REPORT_REG0_* */
@@ -2135,7 +2468,6 @@ struct phyerr_fft_report {
 #define SEARCH_FFT_REPORT_REG1_NUM_STR_BINS_IB_MASK	0x000000FF
 #define SEARCH_FFT_REPORT_REG1_NUM_STR_BINS_IB_LSB	0
 
-
 struct phyerr_tlv {
 	__le16 len;
 	u8 tag;
@@ -2166,7 +2498,6 @@ struct wmi_echo_cmd {
 	__le32 value;
 } __packed;
 
-
 struct wmi_pdev_set_regdomain_cmd {
 	__le32 reg_domain;
 	__le32 reg_domain_2G;
@@ -2215,7 +2546,6 @@ struct wmi_pdev_set_quiet_cmd {
 	__le32 enabled;
 } __packed;
 
-
 /*
  * 802.11g protection mode.
  */
@@ -2515,6 +2845,19 @@ enum wmi_10x_pdev_param {
 	WMI_10X_PDEV_PARAM_BURST_DUR,
 	/* Set Bursting Enable*/
 	WMI_10X_PDEV_PARAM_BURST_ENABLE,
+
+	/* following are available as of firmware 10.2 */
+	WMI_10X_PDEV_PARAM_SMART_ANTENNA_DEFAULT_ANTENNA,
+	WMI_10X_PDEV_PARAM_IGMPMLD_OVERRIDE,
+	WMI_10X_PDEV_PARAM_IGMPMLD_TID,
+	WMI_10X_PDEV_PARAM_ANTENNA_GAIN,
+	WMI_10X_PDEV_PARAM_RX_DECAP_MODE,
+	WMI_10X_PDEV_PARAM_RX_FILTER,
+	WMI_10X_PDEV_PARAM_SET_MCAST_TO_UCAST_TID,
+	WMI_10X_PDEV_PARAM_PROXY_STA_MODE,
+	WMI_10X_PDEV_PARAM_SET_MCAST2UCAST_MODE,
+	WMI_10X_PDEV_PARAM_SET_MCAST2UCAST_BUFFER,
+	WMI_10X_PDEV_PARAM_REMOVE_MCAST2UCAST_BUFFER,
 };
 
 struct wmi_pdev_set_param_cmd {
@@ -3387,6 +3730,14 @@ enum wmi_10x_vdev_param {
 	WMI_10X_VDEV_PARAM_ENABLE_RTSCTS,
 
 	WMI_10X_VDEV_PARAM_AP_DETECT_OUT_OF_SYNC_SLEEPING_STA_TIME_SECS,
+
+	/* following are available as of firmware 10.2 */
+	WMI_10X_VDEV_PARAM_TX_ENCAP_TYPE,
+	WMI_10X_VDEV_PARAM_CABQ_MAXDUR,
+	WMI_10X_VDEV_PARAM_MFPTEST_SET,
+	WMI_10X_VDEV_PARAM_RTS_FIXED_RATE,
+	WMI_10X_VDEV_PARAM_VHT_SGIMASK,
+	WMI_10X_VDEV_PARAM_VHT80_RATEMASK,
 };
 
 /* slot time long */
@@ -3444,6 +3795,98 @@ struct wmi_vdev_simple_event {
 /* unsupported VDEV combination */
 #define WMI_INIFIED_VDEV_START_RESPONSE_NOT_SUPPORTED	0x2
 
+/* TODO: please add more comments if you have in-depth information */
+struct wmi_vdev_spectral_conf_cmd {
+	__le32 vdev_id;
+
+	/* number of fft samples to send (0 for infinite) */
+	__le32 scan_count;
+	__le32 scan_period;
+	__le32 scan_priority;
+
+	/* number of bins in the FFT: 2^(fft_size - bin_scale) */
+	__le32 scan_fft_size;
+	__le32 scan_gc_ena;
+	__le32 scan_restart_ena;
+	__le32 scan_noise_floor_ref;
+	__le32 scan_init_delay;
+	__le32 scan_nb_tone_thr;
+	__le32 scan_str_bin_thr;
+	__le32 scan_wb_rpt_mode;
+	__le32 scan_rssi_rpt_mode;
+	__le32 scan_rssi_thr;
+	__le32 scan_pwr_format;
+
+	/* rpt_mode: Format of FFT report to software for spectral scan
+	 * triggered FFTs:
+	 *	0: No FFT report (only spectral scan summary report)
+	 *	1: 2-dword summary of metrics for each completed FFT + spectral
+	 *	   scan	summary report
+	 *	2: 2-dword summary of metrics for each completed FFT +
+	 *	   1x- oversampled bins(in-band) per FFT + spectral scan summary
+	 *	   report
+	 *	3: 2-dword summary of metrics for each completed FFT +
+	 *	   2x- oversampled bins	(all) per FFT + spectral scan summary
+	 */
+	__le32 scan_rpt_mode;
+	__le32 scan_bin_scale;
+	__le32 scan_dbm_adj;
+	__le32 scan_chn_mask;
+} __packed;
+
+struct wmi_vdev_spectral_conf_arg {
+	u32 vdev_id;
+	u32 scan_count;
+	u32 scan_period;
+	u32 scan_priority;
+	u32 scan_fft_size;
+	u32 scan_gc_ena;
+	u32 scan_restart_ena;
+	u32 scan_noise_floor_ref;
+	u32 scan_init_delay;
+	u32 scan_nb_tone_thr;
+	u32 scan_str_bin_thr;
+	u32 scan_wb_rpt_mode;
+	u32 scan_rssi_rpt_mode;
+	u32 scan_rssi_thr;
+	u32 scan_pwr_format;
+	u32 scan_rpt_mode;
+	u32 scan_bin_scale;
+	u32 scan_dbm_adj;
+	u32 scan_chn_mask;
+};
+
+#define WMI_SPECTRAL_ENABLE_DEFAULT              0
+#define WMI_SPECTRAL_COUNT_DEFAULT               0
+#define WMI_SPECTRAL_PERIOD_DEFAULT             35
+#define WMI_SPECTRAL_PRIORITY_DEFAULT            1
+#define WMI_SPECTRAL_FFT_SIZE_DEFAULT            7
+#define WMI_SPECTRAL_GC_ENA_DEFAULT              1
+#define WMI_SPECTRAL_RESTART_ENA_DEFAULT         0
+#define WMI_SPECTRAL_NOISE_FLOOR_REF_DEFAULT   -96
+#define WMI_SPECTRAL_INIT_DELAY_DEFAULT         80
+#define WMI_SPECTRAL_NB_TONE_THR_DEFAULT        12
+#define WMI_SPECTRAL_STR_BIN_THR_DEFAULT         8
+#define WMI_SPECTRAL_WB_RPT_MODE_DEFAULT         0
+#define WMI_SPECTRAL_RSSI_RPT_MODE_DEFAULT       0
+#define WMI_SPECTRAL_RSSI_THR_DEFAULT         0xf0
+#define WMI_SPECTRAL_PWR_FORMAT_DEFAULT          0
+#define WMI_SPECTRAL_RPT_MODE_DEFAULT            2
+#define WMI_SPECTRAL_BIN_SCALE_DEFAULT           1
+#define WMI_SPECTRAL_DBM_ADJ_DEFAULT             1
+#define WMI_SPECTRAL_CHN_MASK_DEFAULT            1
+
+struct wmi_vdev_spectral_enable_cmd {
+	__le32 vdev_id;
+	__le32 trigger_cmd;
+	__le32 enable_cmd;
+} __packed;
+
+#define WMI_SPECTRAL_TRIGGER_CMD_TRIGGER  1
+#define WMI_SPECTRAL_TRIGGER_CMD_CLEAR    2
+#define WMI_SPECTRAL_ENABLE_CMD_ENABLE    1
+#define WMI_SPECTRAL_ENABLE_CMD_DISABLE   2
+
 /* Beacon processing related command and event structures */
 struct wmi_bcn_tx_hdr {
 	__le32 vdev_id;
@@ -3470,6 +3913,11 @@ enum wmi_bcn_tx_ref_flags {
 	WMI_BCN_TX_REF_FLAG_DELIVER_CAB = 0x2,
 };
 
+/* TODO: It is unclear why "no antenna" works while any other seemingly valid
+ * chainmask yields no beacons on the air at all.
+ */
+#define WMI_BCN_TX_REF_DEF_ANTENNA 0
+
 struct wmi_bcn_tx_ref_cmd {
 	__le32 vdev_id;
 	__le32 data_len;
@@ -3481,6 +3929,8 @@ struct wmi_bcn_tx_ref_cmd {
 	__le32 frame_control;
 	/* to control CABQ traffic: WMI_BCN_TX_REF_FLAG_ */
 	__le32 flags;
+	/* introduced in 10.2 */
+	__le32 antenna_mask;
 } __packed;
 
 /* Beacon filter */
@@ -3833,7 +4283,6 @@ struct wmi_tbtt_offset_event {
 	__le32 tbttoffset_list[WMI_MAX_AP_VDEV];
 } __packed;
 
-
 struct wmi_peer_create_cmd {
 	__le32 vdev_id;
 	struct wmi_mac_addr peer_macaddr;
@@ -4053,7 +4502,7 @@ struct wmi_peer_set_q_empty_callback_cmd {
 /* Maximum listen interval supported by hw in units of beacon interval */
 #define ATH10K_MAX_HW_LISTEN_INTERVAL 5
 
-struct wmi_peer_assoc_complete_cmd {
+struct wmi_common_peer_assoc_complete_cmd {
 	struct wmi_mac_addr peer_macaddr;
 	__le32 vdev_id;
 	__le32 peer_new_assoc; /* 1=assoc, 0=reassoc */
@@ -4071,11 +4520,30 @@ struct wmi_peer_assoc_complete_cmd {
 	__le32 peer_vht_caps;
 	__le32 peer_phymode;
 	struct wmi_vht_rate_set peer_vht_rates;
+};
+
+struct wmi_main_peer_assoc_complete_cmd {
+	struct wmi_common_peer_assoc_complete_cmd cmd;
+
 	/* HT Operation Element of the peer. Five bytes packed in 2
 	 *  INT32 array and filled from lsb to msb. */
 	__le32 peer_ht_info[2];
 } __packed;
 
+struct wmi_10_1_peer_assoc_complete_cmd {
+	struct wmi_common_peer_assoc_complete_cmd cmd;
+} __packed;
+
+#define WMI_PEER_ASSOC_INFO0_MAX_MCS_IDX_LSB 0
+#define WMI_PEER_ASSOC_INFO0_MAX_MCS_IDX_MASK 0x0f
+#define WMI_PEER_ASSOC_INFO0_MAX_NSS_LSB 4
+#define WMI_PEER_ASSOC_INFO0_MAX_NSS_MASK 0xf0
+
+struct wmi_10_2_peer_assoc_complete_cmd {
+	struct wmi_common_peer_assoc_complete_cmd cmd;
+	__le32 info0; /* WMI_PEER_ASSOC_INFO0_ */
+} __packed;
+
 struct wmi_peer_assoc_complete_arg {
 	u8 addr[ETH_ALEN];
 	u32 vdev_id;
@@ -4260,6 +4728,10 @@ int ath10k_wmi_wait_for_service_ready(struct ath10k *ar);
 int ath10k_wmi_wait_for_unified_ready(struct ath10k *ar);
 
 int ath10k_wmi_connect(struct ath10k *ar);
+
+struct sk_buff *ath10k_wmi_alloc_skb(struct ath10k *ar, u32 len);
+int ath10k_wmi_cmd_send(struct ath10k *ar, struct sk_buff *skb, u32 cmd_id);
+
 int ath10k_wmi_pdev_set_channel(struct ath10k *ar,
 				const struct wmi_channel_arg *);
 int ath10k_wmi_pdev_suspend_target(struct ath10k *ar, u32 suspend_opt);
@@ -4290,12 +4762,16 @@ int ath10k_wmi_vdev_set_param(struct ath10k *ar, u32 vdev_id,
 			      u32 param_id, u32 param_value);
 int ath10k_wmi_vdev_install_key(struct ath10k *ar,
 				const struct wmi_vdev_install_key_arg *arg);
+int ath10k_wmi_vdev_spectral_conf(struct ath10k *ar,
+				  const struct wmi_vdev_spectral_conf_arg *arg);
+int ath10k_wmi_vdev_spectral_enable(struct ath10k *ar, u32 vdev_id, u32 trigger,
+				    u32 enable);
 int ath10k_wmi_peer_create(struct ath10k *ar, u32 vdev_id,
-		    const u8 peer_addr[ETH_ALEN]);
+			   const u8 peer_addr[ETH_ALEN]);
 int ath10k_wmi_peer_delete(struct ath10k *ar, u32 vdev_id,
-		    const u8 peer_addr[ETH_ALEN]);
+			   const u8 peer_addr[ETH_ALEN]);
 int ath10k_wmi_peer_flush(struct ath10k *ar, u32 vdev_id,
-		   const u8 peer_addr[ETH_ALEN], u32 tid_bitmap);
+			  const u8 peer_addr[ETH_ALEN], u32 tid_bitmap);
 int ath10k_wmi_peer_set_param(struct ath10k *ar, u32 vdev_id,
 			      const u8 *peer_addr,
 			      enum wmi_peer_param param_id, u32 param_value);
@@ -4312,7 +4788,7 @@ int ath10k_wmi_scan_chan_list(struct ath10k *ar,
 			      const struct wmi_scan_chan_list_arg *arg);
 int ath10k_wmi_beacon_send_ref_nowait(struct ath10k_vif *arvif);
 int ath10k_wmi_pdev_set_wmm_params(struct ath10k *ar,
-			const struct wmi_pdev_set_wmm_params_arg *arg);
+				   const struct wmi_pdev_set_wmm_params_arg *arg);
 int ath10k_wmi_request_stats(struct ath10k *ar, enum wmi_stats_id stats_id);
 int ath10k_wmi_force_fw_hang(struct ath10k *ar,
 			     enum wmi_force_fw_hang_type type, u32 delay_ms);
diff --git a/drivers/net/wireless/ath/ath5k/Kconfig b/drivers/net/wireless/ath/ath5k/Kconfig
index c9f81a388f15..93caf8e68901 100644
--- a/drivers/net/wireless/ath/ath5k/Kconfig
+++ b/drivers/net/wireless/ath/ath5k/Kconfig
@@ -1,13 +1,12 @@
 config ATH5K
 	tristate "Atheros 5xxx wireless cards support"
-	depends on (PCI || ATHEROS_AR231X) && MAC80211
+	depends on PCI && MAC80211
 	select ATH_COMMON
 	select MAC80211_LEDS
 	select LEDS_CLASS
 	select NEW_LEDS
 	select AVERAGE
-	select ATH5K_AHB if (ATHEROS_AR231X && !PCI)
-	select ATH5K_PCI if (!ATHEROS_AR231X && PCI)
+	select ATH5K_PCI
 	---help---
 	  This module adds support for wireless adapters based on
 	  Atheros 5xxx chipset.
@@ -52,16 +51,9 @@ config ATH5K_TRACER
 
 	  If unsure, say N.
 
-config ATH5K_AHB
-	bool "Atheros 5xxx AHB bus support"
-	depends on (ATHEROS_AR231X && !PCI)
-	---help---
-	  This adds support for WiSoC type chipsets of the 5xxx Atheros
-	  family.
-
 config ATH5K_PCI
 	bool "Atheros 5xxx PCI bus support"
-	depends on (!ATHEROS_AR231X && PCI)
+	depends on PCI
 	---help---
 	  This adds support for PCI type chipsets of the 5xxx Atheros
 	  family.
diff --git a/drivers/net/wireless/ath/ath5k/Makefile b/drivers/net/wireless/ath/ath5k/Makefile
index 1b3a34f7f224..51e2d8668041 100644
--- a/drivers/net/wireless/ath/ath5k/Makefile
+++ b/drivers/net/wireless/ath/ath5k/Makefile
@@ -17,6 +17,5 @@ ath5k-y				+= ani.o
 ath5k-y				+= sysfs.o
 ath5k-y				+= mac80211-ops.o
 ath5k-$(CONFIG_ATH5K_DEBUG)	+= debug.o
-ath5k-$(CONFIG_ATH5K_AHB)	+= ahb.o
 ath5k-$(CONFIG_ATH5K_PCI)	+= pci.o
 obj-$(CONFIG_ATH5K)		+= ath5k.o
diff --git a/drivers/net/wireless/ath/ath5k/ahb.c b/drivers/net/wireless/ath/ath5k/ahb.c
deleted file mode 100644
index 79bffe165cab..000000000000
--- a/drivers/net/wireless/ath/ath5k/ahb.c
+++ /dev/null
@@ -1,234 +0,0 @@
-/*
- * Copyright (c) 2008-2009 Atheros Communications Inc.
- * Copyright (c) 2009 Gabor Juhos <juhosg@openwrt.org>
- * Copyright (c) 2009 Imre Kaloz <kaloz@openwrt.org>
- *
- * Permission to use, copy, modify, and/or distribute this software for any
- * purpose with or without fee is hereby granted, provided that the above
- * copyright notice and this permission notice appear in all copies.
- *
- * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
- * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
- * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
- * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
- * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
- * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
- * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
- */
-
-#include <linux/nl80211.h>
-#include <linux/platform_device.h>
-#include <linux/etherdevice.h>
-#include <linux/export.h>
-#include <ar231x_platform.h>
-#include "ath5k.h"
-#include "debug.h"
-#include "base.h"
-#include "reg.h"
-
-/* return bus cachesize in 4B word units */
-static void ath5k_ahb_read_cachesize(struct ath_common *common, int *csz)
-{
-	*csz = L1_CACHE_BYTES >> 2;
-}
-
-static bool
-ath5k_ahb_eeprom_read(struct ath_common *common, u32 off, u16 *data)
-{
-	struct ath5k_hw *ah = common->priv;
-	struct platform_device *pdev = to_platform_device(ah->dev);
-	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
-	u16 *eeprom, *eeprom_end;
-
-	eeprom = (u16 *) bcfg->radio;
-	eeprom_end = ((void *) bcfg->config) + BOARD_CONFIG_BUFSZ;
-
-	eeprom += off;
-	if (eeprom > eeprom_end)
-		return false;
-
-	*data = *eeprom;
-	return true;
-}
-
-int ath5k_hw_read_srev(struct ath5k_hw *ah)
-{
-	struct platform_device *pdev = to_platform_device(ah->dev);
-	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
-	ah->ah_mac_srev = bcfg->devid;
-	return 0;
-}
-
-static int ath5k_ahb_eeprom_read_mac(struct ath5k_hw *ah, u8 *mac)
-{
-	struct platform_device *pdev = to_platform_device(ah->dev);
-	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
-	u8 *cfg_mac;
-
-	if (to_platform_device(ah->dev)->id == 0)
-		cfg_mac = bcfg->config->wlan0_mac;
-	else
-		cfg_mac = bcfg->config->wlan1_mac;
-
-	memcpy(mac, cfg_mac, ETH_ALEN);
-	return 0;
-}
-
-static const struct ath_bus_ops ath_ahb_bus_ops = {
-	.ath_bus_type = ATH_AHB,
-	.read_cachesize = ath5k_ahb_read_cachesize,
-	.eeprom_read = ath5k_ahb_eeprom_read,
-	.eeprom_read_mac = ath5k_ahb_eeprom_read_mac,
-};
-
-/*Initialization*/
-static int ath_ahb_probe(struct platform_device *pdev)
-{
-	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
-	struct ath5k_hw *ah;
-	struct ieee80211_hw *hw;
-	struct resource *res;
-	void __iomem *mem;
-	int irq;
-	int ret = 0;
-	u32 reg;
-
-	if (!dev_get_platdata(&pdev->dev)) {
-		dev_err(&pdev->dev, "no platform data specified\n");
-		ret = -EINVAL;
-		goto err_out;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "no memory resource found\n");
-		ret = -ENXIO;
-		goto err_out;
-	}
-
-	mem = ioremap_nocache(res->start, resource_size(res));
-	if (mem == NULL) {
-		dev_err(&pdev->dev, "ioremap failed\n");
-		ret = -ENOMEM;
-		goto err_out;
-	}
-
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
-		dev_err(&pdev->dev, "no IRQ resource found\n");
-		ret = -ENXIO;
-		goto err_iounmap;
-	}
-
-	irq = res->start;
-
-	hw = ieee80211_alloc_hw(sizeof(struct ath5k_hw), &ath5k_hw_ops);
-	if (hw == NULL) {
-		dev_err(&pdev->dev, "no memory for ieee80211_hw\n");
-		ret = -ENOMEM;
-		goto err_iounmap;
-	}
-
-	ah = hw->priv;
-	ah->hw = hw;
-	ah->dev = &pdev->dev;
-	ah->iobase = mem;
-	ah->irq = irq;
-	ah->devid = bcfg->devid;
-
-	if (bcfg->devid >= AR5K_SREV_AR2315_R6) {
-		/* Enable WMAC AHB arbitration */
-		reg = ioread32((void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
-		reg |= AR5K_AR2315_AHB_ARB_CTL_WLAN;
-		iowrite32(reg, (void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
-
-		/* Enable global WMAC swapping */
-		reg = ioread32((void __iomem *) AR5K_AR2315_BYTESWAP);
-		reg |= AR5K_AR2315_BYTESWAP_WMAC;
-		iowrite32(reg, (void __iomem *) AR5K_AR2315_BYTESWAP);
-	} else {
-		/* Enable WMAC DMA access (assuming 5312 or 231x*/
-		/* TODO: check other platforms */
-		reg = ioread32((void __iomem *) AR5K_AR5312_ENABLE);
-		if (to_platform_device(ah->dev)->id == 0)
-			reg |= AR5K_AR5312_ENABLE_WLAN0;
-		else
-			reg |= AR5K_AR5312_ENABLE_WLAN1;
-		iowrite32(reg, (void __iomem *) AR5K_AR5312_ENABLE);
-
-		/*
-		 * On a dual-band AR5312, the multiband radio is only
-		 * used as pass-through. Disable 2 GHz support in the
-		 * driver for it
-		 */
-		if (to_platform_device(ah->dev)->id == 0 &&
-		    (bcfg->config->flags & (BD_WLAN0 | BD_WLAN1)) ==
-		     (BD_WLAN1 | BD_WLAN0))
-			ah->ah_capabilities.cap_needs_2GHz_ovr = true;
-		else
-			ah->ah_capabilities.cap_needs_2GHz_ovr = false;
-	}
-
-	ret = ath5k_init_ah(ah, &ath_ahb_bus_ops);
-	if (ret != 0) {
-		dev_err(&pdev->dev, "failed to attach device, err=%d\n", ret);
-		ret = -ENODEV;
-		goto err_free_hw;
-	}
-
-	platform_set_drvdata(pdev, hw);
-
-	return 0;
-
- err_free_hw:
-	ieee80211_free_hw(hw);
- err_iounmap:
-        iounmap(mem);
- err_out:
-	return ret;
-}
-
-static int ath_ahb_remove(struct platform_device *pdev)
-{
-	struct ar231x_board_config *bcfg = dev_get_platdata(&pdev->dev);
-	struct ieee80211_hw *hw = platform_get_drvdata(pdev);
-	struct ath5k_hw *ah;
-	u32 reg;
-
-	if (!hw)
-		return 0;
-
-	ah = hw->priv;
-
-	if (bcfg->devid >= AR5K_SREV_AR2315_R6) {
-		/* Disable WMAC AHB arbitration */
-		reg = ioread32((void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
-		reg &= ~AR5K_AR2315_AHB_ARB_CTL_WLAN;
-		iowrite32(reg, (void __iomem *) AR5K_AR2315_AHB_ARB_CTL);
-	} else {
-		/*Stop DMA access */
-		reg = ioread32((void __iomem *) AR5K_AR5312_ENABLE);
-		if (to_platform_device(ah->dev)->id == 0)
-			reg &= ~AR5K_AR5312_ENABLE_WLAN0;
-		else
-			reg &= ~AR5K_AR5312_ENABLE_WLAN1;
-		iowrite32(reg, (void __iomem *) AR5K_AR5312_ENABLE);
-	}
-
-	ath5k_deinit_ah(ah);
-	iounmap(ah->iobase);
-	ieee80211_free_hw(hw);
-
-	return 0;
-}
-
-static struct platform_driver ath_ahb_driver = {
-	.probe      = ath_ahb_probe,
-	.remove     = ath_ahb_remove,
-	.driver		= {
-		.name	= "ar231x-wmac",
-		.owner	= THIS_MODULE,
-	},
-};
-
-module_platform_driver(ath_ahb_driver);
diff --git a/drivers/net/wireless/ath/ath5k/ath5k.h b/drivers/net/wireless/ath/ath5k/ath5k.h
index 85316bb3f8c6..ed2468220216 100644
--- a/drivers/net/wireless/ath/ath5k/ath5k.h
+++ b/drivers/net/wireless/ath/ath5k/ath5k.h
@@ -1647,32 +1647,6 @@ static inline struct ath_regulatory *ath5k_hw_regulatory(struct ath5k_hw *ah)
 	return &(ath5k_hw_common(ah)->regulatory);
 }
 
-#ifdef CONFIG_ATHEROS_AR231X
-#define AR5K_AR2315_PCI_BASE	((void __iomem *)0xb0100000)
-
-static inline void __iomem *ath5k_ahb_reg(struct ath5k_hw *ah, u16 reg)
-{
-	/* On AR2315 and AR2317 the PCI clock domain registers
-	 * are outside of the WMAC register space */
-	if (unlikely((reg >= 0x4000) && (reg < 0x5000) &&
-	    (ah->ah_mac_srev >= AR5K_SREV_AR2315_R6)))
-		return AR5K_AR2315_PCI_BASE + reg;
-
-	return ah->iobase + reg;
-}
-
-static inline u32 ath5k_hw_reg_read(struct ath5k_hw *ah, u16 reg)
-{
-	return ioread32(ath5k_ahb_reg(ah, reg));
-}
-
-static inline void ath5k_hw_reg_write(struct ath5k_hw *ah, u32 val, u16 reg)
-{
-	iowrite32(val, ath5k_ahb_reg(ah, reg));
-}
-
-#else
-
 static inline u32 ath5k_hw_reg_read(struct ath5k_hw *ah, u16 reg)
 {
 	return ioread32(ah->iobase + reg);
@@ -1683,8 +1657,6 @@ static inline void ath5k_hw_reg_write(struct ath5k_hw *ah, u32 val, u16 reg)
 	iowrite32(val, ah->iobase + reg);
 }
 
-#endif
-
 static inline enum ath_bus_type ath5k_get_bus_type(struct ath5k_hw *ah)
 {
 	return ath5k_hw_common(ah)->bus_ops->ath_bus_type;
diff --git a/drivers/net/wireless/ath/ath5k/attach.c b/drivers/net/wireless/ath/ath5k/attach.c
index 7106547a14dd..66b6366158b9 100644
--- a/drivers/net/wireless/ath/ath5k/attach.c
+++ b/drivers/net/wireless/ath/ath5k/attach.c
@@ -351,8 +351,7 @@ void ath5k_hw_deinit(struct ath5k_hw *ah)
 {
 	__set_bit(ATH_STAT_INVALID, ah->status);
 
-	if (ah->ah_rf_banks != NULL)
-		kfree(ah->ah_rf_banks);
+	kfree(ah->ah_rf_banks);
 
 	ath5k_eeprom_detach(ah);
 
diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 8ad2550bce7f..a4a09bb8f2f3 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -99,15 +99,6 @@ static int ath5k_reset(struct ath5k_hw *ah, struct ieee80211_channel *chan,
 
 /* Known SREVs */
 static const struct ath5k_srev_name srev_names[] = {
-#ifdef CONFIG_ATHEROS_AR231X
-	{ "5312",	AR5K_VERSION_MAC,	AR5K_SREV_AR5312_R2 },
-	{ "5312",	AR5K_VERSION_MAC,	AR5K_SREV_AR5312_R7 },
-	{ "2313",	AR5K_VERSION_MAC,	AR5K_SREV_AR2313_R8 },
-	{ "2315",	AR5K_VERSION_MAC,	AR5K_SREV_AR2315_R6 },
-	{ "2315",	AR5K_VERSION_MAC,	AR5K_SREV_AR2315_R7 },
-	{ "2317",	AR5K_VERSION_MAC,	AR5K_SREV_AR2317_R1 },
-	{ "2317",	AR5K_VERSION_MAC,	AR5K_SREV_AR2317_R2 },
-#else
 	{ "5210",	AR5K_VERSION_MAC,	AR5K_SREV_AR5210 },
 	{ "5311",	AR5K_VERSION_MAC,	AR5K_SREV_AR5311 },
 	{ "5311A",	AR5K_VERSION_MAC,	AR5K_SREV_AR5311A },
@@ -126,7 +117,6 @@ static const struct ath5k_srev_name srev_names[] = {
 	{ "5418",	AR5K_VERSION_MAC,	AR5K_SREV_AR5418 },
 	{ "2425",	AR5K_VERSION_MAC,	AR5K_SREV_AR2425 },
 	{ "2417",	AR5K_VERSION_MAC,	AR5K_SREV_AR2417 },
-#endif
 	{ "xxxxx",	AR5K_VERSION_MAC,	AR5K_SREV_UNKNOWN },
 	{ "5110",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5110 },
 	{ "5111",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5111 },
@@ -142,10 +132,6 @@ static const struct ath5k_srev_name srev_names[] = {
 	{ "5413",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5413 },
 	{ "5424",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5424 },
 	{ "5133",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_5133 },
-#ifdef CONFIG_ATHEROS_AR231X
-	{ "2316",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_2316 },
-	{ "2317",	AR5K_VERSION_RAD,	AR5K_SREV_RAD_2317 },
-#endif
 	{ "xxxxx",	AR5K_VERSION_RAD,	AR5K_SREV_UNKNOWN },
 };
 
@@ -1423,7 +1409,7 @@ ath5k_receive_frame(struct ath5k_hw *ah, struct sk_buff *skb,
 		break;
 	}
 
-	if (rxs->rate_idx >= 0 && rs->rs_rate ==
+	if (rs->rs_rate ==
 	    ah->sbands[ah->curchan->band].bitrates[rxs->rate_idx].hw_value_short)
 		rxs->flag |= RX_FLAG_SHORTPRE;
 
diff --git a/drivers/net/wireless/ath/ath5k/debug.c b/drivers/net/wireless/ath/ath5k/debug.c
index b8d031ae63c2..c70782e8f07b 100644
--- a/drivers/net/wireless/ath/ath5k/debug.c
+++ b/drivers/net/wireless/ath/ath5k/debug.c
@@ -62,9 +62,11 @@
 
 #include <linux/export.h>
 #include <linux/moduleparam.h>
+#include <linux/vmalloc.h>
 
 #include <linux/seq_file.h>
 #include <linux/list.h>
+#include <linux/vmalloc.h>
 #include "debug.h"
 #include "ath5k.h"
 #include "reg.h"
@@ -894,6 +896,100 @@ static const struct file_operations fops_queue = {
 	.llseek = default_llseek,
 };
 
+/* debugfs: eeprom */
+
+struct eeprom_private {
+	u16 *buf;
+	int len;
+};
+
+static int open_file_eeprom(struct inode *inode, struct file *file)
+{
+	struct eeprom_private *ep;
+	struct ath5k_hw *ah = inode->i_private;
+	bool res;
+	int i, ret;
+	u32 eesize;
+	u16 val, *buf;
+
+	/* Get eeprom size */
+
+	res = ath5k_hw_nvram_read(ah, AR5K_EEPROM_SIZE_UPPER, &val);
+	if (!res)
+		return -EACCES;
+
+	if (val == 0) {
+		eesize = AR5K_EEPROM_INFO_MAX + AR5K_EEPROM_INFO_BASE;
+	} else {
+		eesize = (val & AR5K_EEPROM_SIZE_UPPER_MASK) <<
+			AR5K_EEPROM_SIZE_ENDLOC_SHIFT;
+		ath5k_hw_nvram_read(ah, AR5K_EEPROM_SIZE_LOWER, &val);
+		eesize = eesize | val;
+	}
+
+	if (eesize > 4096)
+		return -EINVAL;
+
+	/* Create buffer and read in eeprom */
+
+	buf = vmalloc(eesize);
+	if (!buf) {
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	for (i = 0; i < eesize; ++i) {
+		AR5K_EEPROM_READ(i, val);
+		buf[i] = val;
+	}
+
+	/* Create private struct and assign to file */
+
+	ep = kmalloc(sizeof(*ep), GFP_KERNEL);
+	if (!ep) {
+		ret = -ENOMEM;
+		goto freebuf;
+	}
+
+	ep->buf = buf;
+	ep->len = i;
+
+	file->private_data = (void *)ep;
+
+	return 0;
+
+freebuf:
+	vfree(buf);
+err:
+	return ret;
+
+}
+
+static ssize_t read_file_eeprom(struct file *file, char __user *user_buf,
+				   size_t count, loff_t *ppos)
+{
+	struct eeprom_private *ep = file->private_data;
+
+	return simple_read_from_buffer(user_buf, count, ppos, ep->buf, ep->len);
+}
+
+static int release_file_eeprom(struct inode *inode, struct file *file)
+{
+	struct eeprom_private *ep = file->private_data;
+
+	vfree(ep->buf);
+	kfree(ep);
+
+	return 0;
+}
+
+static const struct file_operations fops_eeprom = {
+	.open = open_file_eeprom,
+	.read = read_file_eeprom,
+	.release = release_file_eeprom,
+	.owner = THIS_MODULE,
+};
+
 
 void
 ath5k_debug_init_device(struct ath5k_hw *ah)
@@ -921,6 +1017,8 @@ ath5k_debug_init_device(struct ath5k_hw *ah)
 
 	debugfs_create_file("misc", S_IRUSR, phydir, ah, &fops_misc);
 
+	debugfs_create_file("eeprom", S_IRUSR, phydir, ah, &fops_eeprom);
+
 	debugfs_create_file("frameerrors", S_IWUSR | S_IRUSR, phydir, ah,
 			    &fops_frameerrors);
 
diff --git a/drivers/net/wireless/ath/ath5k/led.c b/drivers/net/wireless/ath/ath5k/led.c
index 48a6a69b57bc..0beb7e7d6075 100644
--- a/drivers/net/wireless/ath/ath5k/led.c
+++ b/drivers/net/wireless/ath/ath5k/led.c
@@ -130,6 +130,7 @@ ath5k_register_led(struct ath5k_hw *ah, struct ath5k_led *led,
 
 	led->ah = ah;
 	strncpy(led->name, name, sizeof(led->name));
+	led->name[sizeof(led->name)-1] = 0;
 	led->led_dev.name = led->name;
 	led->led_dev.default_trigger = trigger;
 	led->led_dev.brightness_set = ath5k_led_brightness_set;
@@ -162,20 +163,14 @@ int ath5k_init_leds(struct ath5k_hw *ah)
 {
 	int ret = 0;
 	struct ieee80211_hw *hw = ah->hw;
-#ifndef CONFIG_ATHEROS_AR231X
 	struct pci_dev *pdev = ah->pdev;
-#endif
 	char name[ATH5K_LED_MAX_NAME_LEN + 1];
 	const struct pci_device_id *match;
 
 	if (!ah->pdev)
 		return 0;
 
-#ifdef CONFIG_ATHEROS_AR231X
-	match = NULL;
-#else
 	match = pci_match_id(&ath5k_led_devices[0], pdev);
-#endif
 	if (match) {
 		__set_bit(ATH_STAT_LEDSOFT, ah->status);
 		ah->led_pin = ATH_PIN(match->driver_data);
diff --git a/drivers/net/wireless/ath/ath5k/mac80211-ops.c b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
index b65c38fdaa4b..ab2709a43768 100644
--- a/drivers/net/wireless/ath/ath5k/mac80211-ops.c
+++ b/drivers/net/wireless/ath/ath5k/mac80211-ops.c
@@ -704,7 +704,7 @@ ath5k_get_survey(struct ieee80211_hw *hw, int idx, struct survey_info *survey)
  * reset.
  */
 static void
-ath5k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
+ath5k_set_coverage_class(struct ieee80211_hw *hw, s16 coverage_class)
 {
 	struct ath5k_hw *ah = hw->priv;
 
diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index e535807c3d89..ba60e37213eb 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/drivers/net/wireless/ath/ath6kl/cfg80211.c
@@ -717,6 +717,7 @@ ath6kl_add_bss_if_needed(struct ath6kl_vif *vif,
 		memcpy(ie + 2, vif->ssid, vif->ssid_len);
 		memcpy(ie + 2 + vif->ssid_len, beacon_ie, beacon_ie_len);
 		bss = cfg80211_inform_bss(ar->wiphy, chan,
+					  CFG80211_BSS_FTYPE_UNKNOWN,
 					  bssid, 0, cap_val, 100,
 					  ie, 2 + vif->ssid_len + beacon_ie_len,
 					  0, GFP_KERNEL);
diff --git a/drivers/net/wireless/ath/ath6kl/init.c b/drivers/net/wireless/ath/ath6kl/init.c
index fffd52355123..6e473fa4b13c 100644
--- a/drivers/net/wireless/ath/ath6kl/init.c
+++ b/drivers/net/wireless/ath/ath6kl/init.c
@@ -1049,7 +1049,7 @@ static int ath6kl_fetch_fw_apin(struct ath6kl *ar, const char *name)
 			ar->hw.reserved_ram_size = le32_to_cpup(val);
 
 			ath6kl_dbg(ATH6KL_DBG_BOOT,
-				   "found reserved ram size ie 0x%d\n",
+				   "found reserved ram size ie %d\n",
 				   ar->hw.reserved_ram_size);
 			break;
 		case ATH6KL_FW_IE_CAPABILITIES:
diff --git a/drivers/net/wireless/ath/ath6kl/main.c b/drivers/net/wireless/ath/ath6kl/main.c
index 21516bc65785..933aef025698 100644
--- a/drivers/net/wireless/ath/ath6kl/main.c
+++ b/drivers/net/wireless/ath/ath6kl/main.c
@@ -225,7 +225,7 @@ int ath6kl_diag_write32(struct ath6kl *ar, u32 address, __le32 value)
 	ret = ath6kl_hif_diag_write32(ar, address, value);
 
 	if (ret) {
-		ath6kl_err("failed to write 0x%x during diagnose window to 0x%d\n",
+		ath6kl_err("failed to write 0x%x during diagnose window to 0x%x\n",
 			   address, value);
 		return ret;
 	}
diff --git a/drivers/net/wireless/ath/ath6kl/sdio.c b/drivers/net/wireless/ath/ath6kl/sdio.c
index 339d89f14d32..eab0ab976af2 100644
--- a/drivers/net/wireless/ath/ath6kl/sdio.c
+++ b/drivers/net/wireless/ath/ath6kl/sdio.c
@@ -1400,6 +1400,7 @@ static const struct sdio_device_id ath6kl_sdio_devices[] = {
 	{SDIO_DEVICE(MANUFACTURER_CODE, (MANUFACTURER_ID_AR6003_BASE | 0x1))},
 	{SDIO_DEVICE(MANUFACTURER_CODE, (MANUFACTURER_ID_AR6004_BASE | 0x0))},
 	{SDIO_DEVICE(MANUFACTURER_CODE, (MANUFACTURER_ID_AR6004_BASE | 0x1))},
+	{SDIO_DEVICE(MANUFACTURER_CODE, (MANUFACTURER_ID_AR6004_BASE | 0x2))},
 	{},
 };
 
diff --git a/drivers/net/wireless/ath/ath6kl/usb.c b/drivers/net/wireless/ath/ath6kl/usb.c
index c44325856b81..a6a5e40b3e98 100644
--- a/drivers/net/wireless/ath/ath6kl/usb.c
+++ b/drivers/net/wireless/ath/ath6kl/usb.c
@@ -1229,26 +1229,7 @@ static struct usb_driver ath6kl_usb_driver = {
 	.disable_hub_initiated_lpm = 1,
 };
 
-static int ath6kl_usb_init(void)
-{
-	int ret;
-
-	ret = usb_register(&ath6kl_usb_driver);
-	if (ret) {
-		ath6kl_err("usb registration failed: %d\n", ret);
-		return ret;
-	}
-
-	return 0;
-}
-
-static void ath6kl_usb_exit(void)
-{
-	usb_deregister(&ath6kl_usb_driver);
-}
-
-module_init(ath6kl_usb_init);
-module_exit(ath6kl_usb_exit);
+module_usb_driver(ath6kl_usb_driver);
 
 MODULE_AUTHOR("Atheros Communications, Inc.");
 MODULE_DESCRIPTION("Driver support for Atheros AR600x USB devices");
diff --git a/drivers/net/wireless/ath/ath6kl/wmi.c b/drivers/net/wireless/ath/ath6kl/wmi.c
index 94df345d08c2..b921005ad7ee 100644
--- a/drivers/net/wireless/ath/ath6kl/wmi.c
+++ b/drivers/net/wireless/ath/ath6kl/wmi.c
@@ -619,8 +619,7 @@ static int ath6kl_wmi_rx_probe_req_event_rx(struct wmi *wmi, u8 *datap, int len,
 		   dlen, freq, vif->probe_req_report);
 
 	if (vif->probe_req_report || vif->nw_type == AP_NETWORK)
-		cfg80211_rx_mgmt(&vif->wdev, freq, 0, ev->data, dlen, 0,
-				 GFP_ATOMIC);
+		cfg80211_rx_mgmt(&vif->wdev, freq, 0, ev->data, dlen, 0);
 
 	return 0;
 }
@@ -659,7 +658,7 @@ static int ath6kl_wmi_rx_action_event_rx(struct wmi *wmi, u8 *datap, int len,
 		return -EINVAL;
 	}
 	ath6kl_dbg(ATH6KL_DBG_WMI, "rx_action: len=%u freq=%u\n", dlen, freq);
-	cfg80211_rx_mgmt(&vif->wdev, freq, 0, ev->data, dlen, 0, GFP_ATOMIC);
+	cfg80211_rx_mgmt(&vif->wdev, freq, 0, ev->data, dlen, 0);
 
 	return 0;
 }
@@ -1093,7 +1092,6 @@ static int ath6kl_wmi_bssinfo_event_rx(struct wmi *wmi, u8 *datap, int len,
 	u8 *buf;
 	struct ieee80211_channel *channel;
 	struct ath6kl *ar = wmi->parent_dev;
-	struct ieee80211_mgmt *mgmt;
 	struct cfg80211_bss *bss;
 
 	if (len <= sizeof(struct wmi_bss_info_hdr2))
@@ -1139,39 +1137,15 @@ static int ath6kl_wmi_bssinfo_event_rx(struct wmi *wmi, u8 *datap, int len,
 		}
 	}
 
-	/*
-	 * In theory, use of cfg80211_inform_bss() would be more natural here
-	 * since we do not have the full frame. However, at least for now,
-	 * cfg80211 can only distinguish Beacon and Probe Response frames from
-	 * each other when using cfg80211_inform_bss_frame(), so let's build a
-	 * fake IEEE 802.11 header to be able to take benefit of this.
-	 */
-	mgmt = kmalloc(24 + len, GFP_ATOMIC);
-	if (mgmt == NULL)
-		return -EINVAL;
-
-	if (bih->frame_type == BEACON_FTYPE) {
-		mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-						  IEEE80211_STYPE_BEACON);
-		memset(mgmt->da, 0xff, ETH_ALEN);
-	} else {
-		struct net_device *dev = vif->ndev;
-
-		mgmt->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-						  IEEE80211_STYPE_PROBE_RESP);
-		memcpy(mgmt->da, dev->dev_addr, ETH_ALEN);
-	}
-	mgmt->duration = cpu_to_le16(0);
-	memcpy(mgmt->sa, bih->bssid, ETH_ALEN);
-	memcpy(mgmt->bssid, bih->bssid, ETH_ALEN);
-	mgmt->seq_ctrl = cpu_to_le16(0);
-
-	memcpy(&mgmt->u.beacon, buf, len);
-
-	bss = cfg80211_inform_bss_frame(ar->wiphy, channel, mgmt,
-					24 + len, (bih->snr - 95) * 100,
-					GFP_ATOMIC);
-	kfree(mgmt);
+	bss = cfg80211_inform_bss(ar->wiphy, channel,
+				  bih->frame_type == BEACON_FTYPE ?
+					CFG80211_BSS_FTYPE_BEACON :
+					CFG80211_BSS_FTYPE_PRESP,
+				  bih->bssid, get_unaligned_le64((__le64 *)buf),
+				  get_unaligned_le16(((__le16 *)buf) + 5),
+				  get_unaligned_le16(((__le16 *)buf) + 4),
+				  buf + 8 + 2 + 2, len - 8 - 2 - 2,
+				  (bih->snr - 95) * 100, GFP_ATOMIC);
 	if (bss == NULL)
 		return -ENOMEM;
 	cfg80211_put_bss(ar->wiphy, bss);
diff --git a/drivers/net/wireless/ath/ath9k/Kconfig b/drivers/net/wireless/ath/ath9k/Kconfig
index 8fcc029a76a6..896e63281b3b 100644
--- a/drivers/net/wireless/ath/ath9k/Kconfig
+++ b/drivers/net/wireless/ath/ath9k/Kconfig
@@ -92,6 +92,15 @@ config ATH9K_DFS_CERTIFIED
 	  developed. At this point enabling this option won't do anything
 	  except increase code size.
 
+config ATH9K_DYNACK
+	bool "Atheros ath9k ACK timeout estimation algorithm (EXPERIMENTAL)"
+	depends on ATH9K
+	default n
+	---help---
+	  This option enables ath9k dynamic ACK timeout estimation algorithm
+	  based on ACK frame RX timestamp, TX frame timestamp and frame
+	  duration
+
 config ATH9K_TX99
 	bool "Atheros ath9k TX99 testing support"
 	depends on ATH9K_DEBUGFS && CFG80211_CERTIFICATION_ONUS
@@ -130,6 +139,15 @@ config ATH9K_RFKILL
 	  seconds. Turn off to save power, but enable it if you have
 	  a platform that can toggle the RF-Kill GPIO.
 
+config ATH9K_CHANNEL_CONTEXT
+       bool "Channel Context support"
+       depends on ATH9K
+       default n
+       ---help---
+         This option enables channel context support in ath9k, which is needed
+	 for multi-channel concurrency. Enable this if P2P PowerSave support
+	 is required.
+
 config ATH9K_HTC
        tristate "Atheros HTC based wireless cards support"
        depends on USB && MAC80211
diff --git a/drivers/net/wireless/ath/ath9k/Makefile b/drivers/net/wireless/ath/ath9k/Makefile
index 6b4020a57984..73704c1be736 100644
--- a/drivers/net/wireless/ath/ath9k/Makefile
+++ b/drivers/net/wireless/ath/ath9k/Makefile
@@ -49,6 +49,9 @@ ath9k_hw-$(CONFIG_ATH9K_WOW) += ar9003_wow.o
 
 ath9k_hw-$(CONFIG_ATH9K_BTCOEX_SUPPORT) += btcoex.o \
 					   ar9003_mci.o
+
+ath9k_hw-$(CONFIG_ATH9K_DYNACK) += dynack.o
+
 obj-$(CONFIG_ATH9K_HW) += ath9k_hw.o
 
 obj-$(CONFIG_ATH9K_COMMON) += ath9k_common.o
diff --git a/drivers/net/wireless/ath/ath9k/ar5008_phy.c b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
index 00fb8badbacc..b72d0be716db 100644
--- a/drivers/net/wireless/ath/ath9k/ar5008_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar5008_phy.c
@@ -1004,9 +1004,11 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 	case ATH9K_ANI_FIRSTEP_LEVEL:{
 		u32 level = param;
 
-		value = level;
+		value = level * 2;
 		REG_RMW_FIELD(ah, AR_PHY_FIND_SIG,
 			      AR_PHY_FIND_SIG_FIRSTEP, value);
+		REG_RMW_FIELD(ah, AR_PHY_FIND_SIG_LOW,
+			      AR_PHY_FIND_SIG_FIRSTEP_LOW, value);
 
 		if (level != aniState->firstepLevel) {
 			ath_dbg(common, ANI,
@@ -1040,9 +1042,8 @@ static bool ar5008_hw_ani_control_new(struct ath_hw *ah,
 		REG_RMW_FIELD(ah, AR_PHY_TIMING5,
 			      AR_PHY_TIMING5_CYCPWR_THR1, value);
 
-		if (IS_CHAN_HT40(ah->curchan))
-			REG_RMW_FIELD(ah, AR_PHY_EXT_CCA,
-				      AR_PHY_EXT_TIMING5_CYCPWR_THR1, value);
+		REG_RMW_FIELD(ah, AR_PHY_EXT_CCA,
+				  AR_PHY_EXT_TIMING5_CYCPWR_THR1, value - 1);
 
 		if (level != aniState->spurImmunityLevel) {
 			ath_dbg(common, ANI,
diff --git a/drivers/net/wireless/ath/ath9k/ar9002_mac.c b/drivers/net/wireless/ath/ath9k/ar9002_mac.c
index 59af9f9712da..2a93519f4bdf 100644
--- a/drivers/net/wireless/ath/ath9k/ar9002_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9002_mac.c
@@ -384,6 +384,24 @@ static int ar9002_hw_proc_txdesc(struct ath_hw *ah, void *ds,
 	return 0;
 }
 
+static int ar9002_hw_get_duration(struct ath_hw *ah, const void *ds, int index)
+{
+	struct ar5416_desc *ads = AR5416DESC(ds);
+
+	switch (index) {
+	case 0:
+		return MS(ACCESS_ONCE(ads->ds_ctl4), AR_PacketDur0);
+	case 1:
+		return MS(ACCESS_ONCE(ads->ds_ctl4), AR_PacketDur1);
+	case 2:
+		return MS(ACCESS_ONCE(ads->ds_ctl5), AR_PacketDur2);
+	case 3:
+		return MS(ACCESS_ONCE(ads->ds_ctl5), AR_PacketDur3);
+	default:
+		return -1;
+	}
+}
+
 void ath9k_hw_setuprxdesc(struct ath_hw *ah, struct ath_desc *ds,
 			  u32 size, u32 flags)
 {
@@ -406,4 +424,5 @@ void ar9002_hw_attach_mac_ops(struct ath_hw *ah)
 	ops->get_isr = ar9002_hw_get_isr;
 	ops->set_txdesc = ar9002_set_txdesc;
 	ops->proc_txdesc = ar9002_hw_proc_txdesc;
+	ops->get_duration = ar9002_hw_get_duration;
 }
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_mac.c b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
index 71e38e85aa99..057b1657c428 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_mac.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_mac.c
@@ -431,6 +431,24 @@ static int ar9003_hw_proc_txdesc(struct ath_hw *ah, void *ds,
 	return 0;
 }
 
+static int ar9003_hw_get_duration(struct ath_hw *ah, const void *ds, int index)
+{
+	const struct ar9003_txc *adc = ds;
+
+	switch (index) {
+	case 0:
+		return MS(ACCESS_ONCE(adc->ctl15), AR_PacketDur0);
+	case 1:
+		return MS(ACCESS_ONCE(adc->ctl15), AR_PacketDur1);
+	case 2:
+		return MS(ACCESS_ONCE(adc->ctl16), AR_PacketDur2);
+	case 3:
+		return MS(ACCESS_ONCE(adc->ctl16), AR_PacketDur3);
+	default:
+		return 0;
+	}
+}
+
 void ar9003_hw_attach_mac_ops(struct ath_hw *hw)
 {
 	struct ath_hw_ops *ops = ath9k_hw_ops(hw);
@@ -440,6 +458,7 @@ void ar9003_hw_attach_mac_ops(struct ath_hw *hw)
 	ops->get_isr = ar9003_hw_get_isr;
 	ops->set_txdesc = ar9003_set_txdesc;
 	ops->proc_txdesc = ar9003_hw_proc_txdesc;
+	ops->get_duration = ar9003_hw_get_duration;
 }
 
 void ath9k_hw_set_rx_bufsize(struct ath_hw *ah, u16 buf_size)
diff --git a/drivers/net/wireless/ath/ath9k/ar9003_phy.c b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
index 542a8d51d3b0..697c4ae90af0 100644
--- a/drivers/net/wireless/ath/ath9k/ar9003_phy.c
+++ b/drivers/net/wireless/ath/ath9k/ar9003_phy.c
@@ -517,6 +517,23 @@ static void ar9003_hw_spur_mitigate(struct ath_hw *ah,
 	ar9003_hw_spur_mitigate_ofdm(ah, chan);
 }
 
+static u32 ar9003_hw_compute_pll_control_soc(struct ath_hw *ah,
+					     struct ath9k_channel *chan)
+{
+	u32 pll;
+
+	pll = SM(0x5, AR_RTC_9300_SOC_PLL_REFDIV);
+
+	if (chan && IS_CHAN_HALF_RATE(chan))
+		pll |= SM(0x1, AR_RTC_9300_SOC_PLL_CLKSEL);
+	else if (chan && IS_CHAN_QUARTER_RATE(chan))
+		pll |= SM(0x2, AR_RTC_9300_SOC_PLL_CLKSEL);
+
+	pll |= SM(0x2c, AR_RTC_9300_SOC_PLL_DIV_INT);
+
+	return pll;
+}
+
 static u32 ar9003_hw_compute_pll_control(struct ath_hw *ah,
 					 struct ath9k_channel *chan)
 {
@@ -1781,7 +1798,12 @@ void ar9003_hw_attach_phy_ops(struct ath_hw *ah)
 
 	priv_ops->rf_set_freq = ar9003_hw_set_channel;
 	priv_ops->spur_mitigate_freq = ar9003_hw_spur_mitigate;
-	priv_ops->compute_pll_control = ar9003_hw_compute_pll_control;
+
+	if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah))
+		priv_ops->compute_pll_control = ar9003_hw_compute_pll_control_soc;
+	else
+		priv_ops->compute_pll_control = ar9003_hw_compute_pll_control;
+
 	priv_ops->set_channel_regs = ar9003_hw_set_channel_regs;
 	priv_ops->init_bb = ar9003_hw_init_bb;
 	priv_ops->process_ini = ar9003_hw_process_ini;
diff --git a/drivers/net/wireless/ath/ath9k/ath9k.h b/drivers/net/wireless/ath/ath9k/ath9k.h
index 7fc13a8da675..01a7db061c6a 100644
--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -31,6 +31,7 @@
 #include "spectral.h"
 
 struct ath_node;
+struct ath_vif;
 
 extern struct ieee80211_ops ath9k_ops;
 extern int ath9k_modparam_nohwcrypt;
@@ -273,6 +274,9 @@ struct ath_node {
 	struct ath_rx_rate_stats rx_rate_stats;
 #endif
 	u8 key_idx[4];
+
+	u32 ackto;
+	struct list_head list;
 };
 
 struct ath_tx_control {
@@ -290,7 +294,6 @@ struct ath_tx_control {
  *  (axq_qnum).
  */
 struct ath_tx {
-	u16 seq_no;
 	u32 txqsetup;
 	spinlock_t txbuflock;
 	struct list_head txbuf;
@@ -313,7 +316,6 @@ struct ath_rx {
 	bool discard_next;
 	u32 *rxlink;
 	u32 num_pkts;
-	unsigned int rxfilter;
 	struct list_head rxbuf;
 	struct ath_descdma rxdma;
 	struct ath_rx_edma rx_edma[ATH9K_RX_QUEUE_MAX];
@@ -324,6 +326,10 @@ struct ath_rx {
 	u32 ampdu_ref;
 };
 
+/*******************/
+/* Channel Context */
+/*******************/
+
 struct ath_chanctx {
 	struct cfg80211_chan_def chandef;
 	struct list_head vifs;
@@ -345,6 +351,10 @@ struct ath_chanctx {
 	bool active;
 	bool assigned;
 	bool switch_after_beacon;
+
+	short nvifs;
+	short nvifs_assigned;
+	unsigned int rxfilter;
 };
 
 enum ath_chanctx_event {
@@ -354,7 +364,9 @@ enum ath_chanctx_event {
 	ATH_CHANCTX_EVENT_BEACON_RECEIVED,
 	ATH_CHANCTX_EVENT_ASSOC,
 	ATH_CHANCTX_EVENT_SWITCH,
+	ATH_CHANCTX_EVENT_ASSIGN,
 	ATH_CHANCTX_EVENT_UNASSIGN,
+	ATH_CHANCTX_EVENT_CHANGE,
 	ATH_CHANCTX_EVENT_ENABLE_MULTICHANNEL,
 };
 
@@ -369,6 +381,9 @@ enum ath_chanctx_state {
 struct ath_chanctx_sched {
 	bool beacon_pending;
 	bool offchannel_pending;
+	bool wait_switch;
+	bool force_noa_update;
+	bool extend_absence;
 	enum ath_chanctx_state state;
 	u8 beacon_miss;
 
@@ -403,38 +418,130 @@ struct ath_offchannel {
 	int roc_duration;
 	int duration;
 };
+
+#define case_rtn_string(val) case val: return #val
+
 #define ath_for_each_chanctx(_sc, _ctx)                             \
 	for (ctx = &sc->chanctx[0];                                 \
 	     ctx <= &sc->chanctx[ARRAY_SIZE(sc->chanctx) - 1];      \
 	     ctx++)
 
-void ath9k_fill_chanctx_ops(void);
-void ath9k_chanctx_force_active(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif);
+void ath_chanctx_init(struct ath_softc *sc);
+void ath_chanctx_set_channel(struct ath_softc *sc, struct ath_chanctx *ctx,
+			     struct cfg80211_chan_def *chandef);
+
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+
 static inline struct ath_chanctx *
 ath_chanctx_get(struct ieee80211_chanctx_conf *ctx)
 {
 	struct ath_chanctx **ptr = (void *) ctx->drv_priv;
 	return *ptr;
 }
-void ath_chanctx_init(struct ath_softc *sc);
-void ath_chanctx_set_channel(struct ath_softc *sc, struct ath_chanctx *ctx,
-			     struct cfg80211_chan_def *chandef);
-void ath_chanctx_switch(struct ath_softc *sc, struct ath_chanctx *ctx,
-			struct cfg80211_chan_def *chandef);
+
+bool ath9k_is_chanctx_enabled(void);
+void ath9k_fill_chanctx_ops(void);
+void ath9k_init_channel_context(struct ath_softc *sc);
+void ath9k_offchannel_init(struct ath_softc *sc);
+void ath9k_deinit_channel_context(struct ath_softc *sc);
+int ath9k_init_p2p(struct ath_softc *sc);
+void ath9k_deinit_p2p(struct ath_softc *sc);
+void ath9k_p2p_remove_vif(struct ath_softc *sc,
+			  struct ieee80211_vif *vif);
+void ath9k_p2p_beacon_sync(struct ath_softc *sc);
+void ath9k_p2p_bss_info_changed(struct ath_softc *sc,
+				struct ieee80211_vif *vif);
+void ath9k_beacon_add_noa(struct ath_softc *sc, struct ath_vif *avp,
+			  struct sk_buff *skb);
+void ath9k_p2p_ps_timer(void *priv);
+void ath9k_chanctx_wake_queues(struct ath_softc *sc, struct ath_chanctx *ctx);
+void ath9k_chanctx_stop_queues(struct ath_softc *sc, struct ath_chanctx *ctx);
 void ath_chanctx_check_active(struct ath_softc *sc, struct ath_chanctx *ctx);
-void ath_offchannel_timer(unsigned long data);
-void ath_offchannel_channel_change(struct ath_softc *sc);
-void ath_chanctx_offchan_switch(struct ath_softc *sc,
-				struct ieee80211_channel *chan);
-struct ath_chanctx *ath_chanctx_get_oper_chan(struct ath_softc *sc,
-					      bool active);
+
+void ath_chanctx_beacon_recv_ev(struct ath_softc *sc,
+				enum ath_chanctx_event ev);
+void ath_chanctx_beacon_sent_ev(struct ath_softc *sc,
+				enum ath_chanctx_event ev);
 void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 		       enum ath_chanctx_event ev);
-void ath_chanctx_timer(unsigned long data);
+void ath_chanctx_set_next(struct ath_softc *sc, bool force);
+void ath_offchannel_next(struct ath_softc *sc);
+void ath_scan_complete(struct ath_softc *sc, bool abort);
+void ath_roc_complete(struct ath_softc *sc, bool abort);
+
+#else
+
+static inline bool ath9k_is_chanctx_enabled(void)
+{
+	return false;
+}
+static inline void ath9k_fill_chanctx_ops(void)
+{
+}
+static inline void ath9k_init_channel_context(struct ath_softc *sc)
+{
+}
+static inline void ath9k_offchannel_init(struct ath_softc *sc)
+{
+}
+static inline void ath9k_deinit_channel_context(struct ath_softc *sc)
+{
+}
+static inline void ath_chanctx_beacon_recv_ev(struct ath_softc *sc,
+					      enum ath_chanctx_event ev)
+{
+}
+static inline void ath_chanctx_beacon_sent_ev(struct ath_softc *sc,
+					      enum ath_chanctx_event ev)
+{
+}
+static inline void ath_chanctx_event(struct ath_softc *sc,
+				     struct ieee80211_vif *vif,
+				     enum ath_chanctx_event ev)
+{
+}
+static inline int ath9k_init_p2p(struct ath_softc *sc)
+{
+	return 0;
+}
+static inline void ath9k_deinit_p2p(struct ath_softc *sc)
+{
+}
+static inline void ath9k_p2p_remove_vif(struct ath_softc *sc,
+					struct ieee80211_vif *vif)
+{
+}
+static inline void ath9k_p2p_beacon_sync(struct ath_softc *sc)
+{
+}
+static inline void ath9k_p2p_bss_info_changed(struct ath_softc *sc,
+					      struct ieee80211_vif *vif)
+{
+}
+static inline void ath9k_beacon_add_noa(struct ath_softc *sc, struct ath_vif *avp,
+					struct sk_buff *skb)
+{
+}
+static inline void ath9k_p2p_ps_timer(struct ath_softc *sc)
+{
+}
+static inline void ath9k_chanctx_wake_queues(struct ath_softc *sc,
+					     struct ath_chanctx *ctx)
+{
+}
+static inline void ath9k_chanctx_stop_queues(struct ath_softc *sc,
+					     struct ath_chanctx *ctx)
+{
+}
+static inline void ath_chanctx_check_active(struct ath_softc *sc,
+					    struct ath_chanctx *ctx)
+{
+}
+
+#endif /* CONFIG_ATH9K_CHANNEL_CONTEXT */
 
 int ath_reset_internal(struct ath_softc *sc, struct ath9k_channel *hchan);
-int ath_startrecv(struct ath_softc *sc);
+void ath_startrecv(struct ath_softc *sc);
 bool ath_stoprecv(struct ath_softc *sc);
 u32 ath_calcrxfilter(struct ath_softc *sc);
 int ath_rx_init(struct ath_softc *sc, int nbufs);
@@ -455,6 +562,7 @@ int ath_tx_init(struct ath_softc *sc, int nbufs);
 int ath_txq_update(struct ath_softc *sc, int qnum,
 		   struct ath9k_tx_queue_info *q);
 void ath_update_max_aggr_framelen(struct ath_softc *sc, int queue, int txop);
+void ath_assign_seq(struct ath_common *common, struct sk_buff *skb);
 int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 		 struct ath_tx_control *txctl);
 void ath_tx_cabq(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
@@ -479,9 +587,18 @@ void ath9k_release_buffered_frames(struct ieee80211_hw *hw,
 /* VIFs */
 /********/
 
+#define P2P_DEFAULT_CTWIN 10
+
 struct ath_vif {
 	struct list_head list;
 
+	u16 seq_no;
+
+	/* BSS info */
+	u8 bssid[ETH_ALEN];
+	u16 aid;
+	bool assoc;
+
 	struct ieee80211_vif *vif;
 	struct ath_node mcast_node;
 	int av_bslot;
@@ -497,8 +614,10 @@ struct ath_vif {
 	u32 offchannel_start;
 	u32 offchannel_duration;
 
-	u32 periodic_noa_start;
-	u32 periodic_noa_duration;
+	/* These are used for both periodic and one-shot */
+	u32 noa_start;
+	u32 noa_duration;
+	bool periodic_noa;
 };
 
 struct ath9k_vif_iter_data {
@@ -583,7 +702,6 @@ void ath9k_csa_update(struct ath_softc *sc);
 #define ATH_PAPRD_TIMEOUT         100 /* msecs */
 #define ATH_PLL_WORK_INTERVAL     100
 
-void ath_chanctx_work(struct work_struct *work);
 void ath_tx_complete_poll_work(struct work_struct *work);
 void ath_reset_work(struct work_struct *work);
 bool ath_hw_check(struct ath_softc *sc);
@@ -597,8 +715,6 @@ int ath_update_survey_stats(struct ath_softc *sc);
 void ath_update_survey_nf(struct ath_softc *sc, int channel);
 void ath9k_queue_reset(struct ath_softc *sc, enum ath_reset_type type);
 void ath_ps_full_sleep(unsigned long data);
-void ath9k_p2p_ps_timer(void *priv);
-void ath9k_update_p2p_ps(struct ath_softc *sc, struct ieee80211_vif *vif);
 void __ath9k_flush(struct ieee80211_hw *hw, u32 queues, bool drop);
 
 /**********/
@@ -849,12 +965,17 @@ struct ath_softc {
 	struct mutex mutex;
 	struct work_struct paprd_work;
 	struct work_struct hw_reset_work;
-	struct work_struct chanctx_work;
 	struct completion paprd_complete;
 	wait_queue_head_t tx_wait;
 
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+	struct work_struct chanctx_work;
 	struct ath_gen_timer *p2p_ps_timer;
 	struct ath_vif *p2p_ps_vif;
+	struct ath_chanctx_sched sched;
+	struct ath_offchannel offchannel;
+	struct ath_chanctx *next_chan;
+#endif
 
 	unsigned long driver_data;
 
@@ -865,7 +986,6 @@ struct ath_softc {
 	bool ps_enabled;
 	bool ps_idle;
 	short nbcnvifs;
-	short nvifs;
 	unsigned long ps_usecount;
 
 	struct ath_rx rx;
@@ -875,10 +995,7 @@ struct ath_softc {
 	struct cfg80211_chan_def cur_chandef;
 	struct ath_chanctx chanctx[ATH9K_NUM_CHANCTX];
 	struct ath_chanctx *cur_chan;
-	struct ath_chanctx *next_chan;
 	spinlock_t chan_lock;
-	struct ath_offchannel offchannel;
-	struct ath_chanctx_sched sched;
 
 #ifdef CONFIG_MAC80211_LEDS
 	bool led_registered;
diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index eaf8f058c151..ecb783beeec2 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -108,55 +108,6 @@ static void ath9k_beacon_setup(struct ath_softc *sc, struct ieee80211_vif *vif,
 	ath9k_hw_set_txdesc(ah, bf->bf_desc, &info);
 }
 
-static void ath9k_beacon_add_noa(struct ath_softc *sc, struct ath_vif *avp,
-				 struct sk_buff *skb)
-{
-	static const u8 noa_ie_hdr[] = {
-		WLAN_EID_VENDOR_SPECIFIC,	/* type */
-		0,				/* length */
-		0x50, 0x6f, 0x9a,		/* WFA OUI */
-		0x09,				/* P2P subtype */
-		0x0c,				/* Notice of Absence */
-		0x00,				/* LSB of little-endian len */
-		0x00,				/* MSB of little-endian len */
-	};
-
-	struct ieee80211_p2p_noa_attr *noa;
-	int noa_len, noa_desc, i = 0;
-	u8 *hdr;
-
-	if (!avp->offchannel_duration && !avp->periodic_noa_duration)
-		return;
-
-	noa_desc = !!avp->offchannel_duration + !!avp->periodic_noa_duration;
-	noa_len = 2 + sizeof(struct ieee80211_p2p_noa_desc) * noa_desc;
-
-	hdr = skb_put(skb, sizeof(noa_ie_hdr));
-	memcpy(hdr, noa_ie_hdr, sizeof(noa_ie_hdr));
-	hdr[1] = sizeof(noa_ie_hdr) + noa_len - 2;
-	hdr[7] = noa_len;
-
-	noa = (void *) skb_put(skb, noa_len);
-	memset(noa, 0, noa_len);
-
-	noa->index = avp->noa_index;
-	if (avp->periodic_noa_duration) {
-		u32 interval = TU_TO_USEC(sc->cur_chan->beacon.beacon_interval);
-
-		noa->desc[i].count = 255;
-		noa->desc[i].start_time = cpu_to_le32(avp->periodic_noa_start);
-		noa->desc[i].duration = cpu_to_le32(avp->periodic_noa_duration);
-		noa->desc[i].interval = cpu_to_le32(interval);
-		i++;
-	}
-
-	if (avp->offchannel_duration) {
-		noa->desc[i].count = 1;
-		noa->desc[i].start_time = cpu_to_le32(avp->offchannel_start);
-		noa->desc[i].duration = cpu_to_le32(avp->offchannel_duration);
-	}
-}
-
 static struct ath_buf *ath9k_beacon_generate(struct ieee80211_hw *hw,
 					     struct ieee80211_vif *vif)
 {
@@ -193,16 +144,8 @@ static struct ath_buf *ath9k_beacon_generate(struct ieee80211_hw *hw,
 	mgmt_hdr->u.beacon.timestamp = avp->tsf_adjust;
 
 	info = IEEE80211_SKB_CB(skb);
-	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
-		/*
-		 * TODO: make sure the seq# gets assigned properly (vs. other
-		 * TX frames)
-		 */
-		struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-		sc->tx.seq_no += 0x10;
-		hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
-		hdr->seq_ctrl |= cpu_to_le16(sc->tx.seq_no);
-	}
+
+	ath_assign_seq(common, skb);
 
 	if (vif->p2p)
 		ath9k_beacon_add_noa(sc, avp, skb);
@@ -232,7 +175,7 @@ static struct ath_buf *ath9k_beacon_generate(struct ieee80211_hw *hw,
 	spin_unlock_bh(&cabq->axq_lock);
 
 	if (skb && cabq_depth) {
-		if (sc->nvifs > 1) {
+		if (sc->cur_chan->nvifs > 1) {
 			ath_dbg(common, BEACON,
 				"Flushing previous cabq traffic\n");
 			ath_draintxq(sc, cabq);
@@ -427,9 +370,10 @@ void ath9k_beacon_tasklet(unsigned long data)
 
 	/* EDMA devices check that in the tx completion function. */
 	if (!edma) {
-		if (sc->sched.beacon_pending)
-			ath_chanctx_event(sc, NULL,
+		if (ath9k_is_chanctx_enabled()) {
+			ath_chanctx_beacon_sent_ev(sc,
 					  ATH_CHANCTX_EVENT_BEACON_SENT);
+		}
 
 		if (ath9k_csa_is_finished(sc, vif))
 			return;
@@ -438,7 +382,10 @@ void ath9k_beacon_tasklet(unsigned long data)
 	if (!vif || !vif->bss_conf.enable_beacon)
 		return;
 
-	ath_chanctx_event(sc, vif, ATH_CHANCTX_EVENT_BEACON_PREPARE);
+	if (ath9k_is_chanctx_enabled()) {
+		ath_chanctx_event(sc, vif, ATH_CHANCTX_EVENT_BEACON_PREPARE);
+	}
+
 	bf = ath9k_beacon_generate(sc->hw, vif);
 
 	if (sc->beacon.bmisscnt != 0) {
@@ -559,6 +506,18 @@ static bool ath9k_allow_beacon_config(struct ath_softc *sc,
 				      struct ieee80211_vif *vif)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_vif *avp = (void *)vif->drv_priv;
+
+	if (ath9k_is_chanctx_enabled()) {
+		/*
+		 * If the VIF is not present in the current channel context,
+		 * then we can't do the usual opmode checks. Allow the
+		 * beacon config for the VIF to be updated in this case and
+		 * return immediately.
+		 */
+		if (sc->cur_chan != avp->chanctx)
+			return true;
+	}
 
 	if (sc->sc_ah->opmode == NL80211_IFTYPE_AP) {
 		if ((vif->type != NL80211_IFTYPE_AP) ||
diff --git a/drivers/net/wireless/ath/ath9k/channel.c b/drivers/net/wireless/ath/ath9k/channel.c
index ba214ebdcd16..945c89826b14 100644
--- a/drivers/net/wireless/ath/ath9k/channel.c
+++ b/drivers/net/wireless/ath/ath9k/channel.c
@@ -83,8 +83,6 @@ static int ath_set_channel(struct ath_softc *sc)
 	if (hw->conf.radar_enabled) {
 		u32 rxfilter;
 
-		/* set HW specific DFS configuration */
-		ath9k_hw_set_radar_params(ah);
 		rxfilter = ath9k_hw_getrxfilter(ah);
 		rxfilter |= ATH9K_RX_FILTER_PHYRADAR |
 				ATH9K_RX_FILTER_PHYERR;
@@ -101,202 +99,6 @@ static int ath_set_channel(struct ath_softc *sc)
 	return 0;
 }
 
-static bool
-ath_chanctx_send_vif_ps_frame(struct ath_softc *sc, struct ath_vif *avp,
-			      bool powersave)
-{
-	struct ieee80211_vif *vif = avp->vif;
-	struct ieee80211_sta *sta = NULL;
-	struct ieee80211_hdr_3addr *nullfunc;
-	struct ath_tx_control txctl;
-	struct sk_buff *skb;
-	int band = sc->cur_chan->chandef.chan->band;
-
-	switch (vif->type) {
-	case NL80211_IFTYPE_STATION:
-		if (!vif->bss_conf.assoc)
-			return false;
-
-		skb = ieee80211_nullfunc_get(sc->hw, vif);
-		if (!skb)
-			return false;
-
-		nullfunc = (struct ieee80211_hdr_3addr *) skb->data;
-		if (powersave)
-			nullfunc->frame_control |=
-				cpu_to_le16(IEEE80211_FCTL_PM);
-
-		skb_set_queue_mapping(skb, IEEE80211_AC_VO);
-		if (!ieee80211_tx_prepare_skb(sc->hw, vif, skb, band, &sta)) {
-			dev_kfree_skb_any(skb);
-			return false;
-		}
-		break;
-	default:
-		return false;
-	}
-
-	memset(&txctl, 0, sizeof(txctl));
-	txctl.txq = sc->tx.txq_map[IEEE80211_AC_VO];
-	txctl.sta = sta;
-	txctl.force_channel = true;
-	if (ath_tx_start(sc->hw, skb, &txctl)) {
-		ieee80211_free_txskb(sc->hw, skb);
-		return false;
-	}
-
-	return true;
-}
-
-void ath_chanctx_check_active(struct ath_softc *sc, struct ath_chanctx *ctx)
-{
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ath_vif *avp;
-	bool active = false;
-	u8 n_active = 0;
-
-	if (!ctx)
-		return;
-
-	list_for_each_entry(avp, &ctx->vifs, list) {
-		struct ieee80211_vif *vif = avp->vif;
-
-		switch (vif->type) {
-		case NL80211_IFTYPE_P2P_CLIENT:
-		case NL80211_IFTYPE_STATION:
-			if (vif->bss_conf.assoc)
-				active = true;
-			break;
-		default:
-			active = true;
-			break;
-		}
-	}
-	ctx->active = active;
-
-	ath_for_each_chanctx(sc, ctx) {
-		if (!ctx->assigned || list_empty(&ctx->vifs))
-			continue;
-		n_active++;
-	}
-
-	if (n_active <= 1) {
-		clear_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags);
-		return;
-	}
-	if (test_and_set_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags))
-		return;
-	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_ENABLE_MULTICHANNEL);
-}
-
-static bool
-ath_chanctx_send_ps_frame(struct ath_softc *sc, bool powersave)
-{
-	struct ath_vif *avp;
-	bool sent = false;
-
-	rcu_read_lock();
-	list_for_each_entry(avp, &sc->cur_chan->vifs, list) {
-		if (ath_chanctx_send_vif_ps_frame(sc, avp, powersave))
-			sent = true;
-	}
-	rcu_read_unlock();
-
-	return sent;
-}
-
-static bool ath_chanctx_defer_switch(struct ath_softc *sc)
-{
-	if (sc->cur_chan == &sc->offchannel.chan)
-		return false;
-
-	switch (sc->sched.state) {
-	case ATH_CHANCTX_STATE_SWITCH:
-		return false;
-	case ATH_CHANCTX_STATE_IDLE:
-		if (!sc->cur_chan->switch_after_beacon)
-			return false;
-
-		sc->sched.state = ATH_CHANCTX_STATE_WAIT_FOR_BEACON;
-		break;
-	default:
-		break;
-	}
-
-	return true;
-}
-
-static void ath_chanctx_set_next(struct ath_softc *sc, bool force)
-{
-	struct timespec ts;
-	bool measure_time = false;
-	bool send_ps = false;
-
-	spin_lock_bh(&sc->chan_lock);
-	if (!sc->next_chan) {
-		spin_unlock_bh(&sc->chan_lock);
-		return;
-	}
-
-	if (!force && ath_chanctx_defer_switch(sc)) {
-		spin_unlock_bh(&sc->chan_lock);
-		return;
-	}
-
-	if (sc->cur_chan != sc->next_chan) {
-		sc->cur_chan->stopped = true;
-		spin_unlock_bh(&sc->chan_lock);
-
-		if (sc->next_chan == &sc->offchannel.chan) {
-			getrawmonotonic(&ts);
-			measure_time = true;
-		}
-		__ath9k_flush(sc->hw, ~0, true);
-
-		if (ath_chanctx_send_ps_frame(sc, true))
-			__ath9k_flush(sc->hw, BIT(IEEE80211_AC_VO), false);
-
-		send_ps = true;
-		spin_lock_bh(&sc->chan_lock);
-
-		if (sc->cur_chan != &sc->offchannel.chan) {
-			getrawmonotonic(&sc->cur_chan->tsf_ts);
-			sc->cur_chan->tsf_val = ath9k_hw_gettsf64(sc->sc_ah);
-		}
-	}
-	sc->cur_chan = sc->next_chan;
-	sc->cur_chan->stopped = false;
-	sc->next_chan = NULL;
-	sc->sched.offchannel_duration = 0;
-	if (sc->sched.state != ATH_CHANCTX_STATE_FORCE_ACTIVE)
-		sc->sched.state = ATH_CHANCTX_STATE_IDLE;
-
-	spin_unlock_bh(&sc->chan_lock);
-
-	if (sc->sc_ah->chip_fullsleep ||
-	    memcmp(&sc->cur_chandef, &sc->cur_chan->chandef,
-		   sizeof(sc->cur_chandef))) {
-		ath_set_channel(sc);
-		if (measure_time)
-			sc->sched.channel_switch_time =
-				ath9k_hw_get_tsf_offset(&ts, NULL);
-	}
-	if (send_ps)
-		ath_chanctx_send_ps_frame(sc, false);
-
-	ath_offchannel_channel_change(sc);
-	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_SWITCH);
-}
-
-void ath_chanctx_work(struct work_struct *work)
-{
-	struct ath_softc *sc = container_of(work, struct ath_softc,
-					    chanctx_work);
-	mutex_lock(&sc->mutex);
-	ath_chanctx_set_next(sc, false);
-	mutex_unlock(&sc->mutex);
-}
-
 void ath_chanctx_init(struct ath_softc *sc)
 {
 	struct ath_chanctx *ctx;
@@ -318,115 +120,124 @@ void ath_chanctx_init(struct ath_softc *sc)
 		for (j = 0; j < ARRAY_SIZE(ctx->acq); j++)
 			INIT_LIST_HEAD(&ctx->acq[j]);
 	}
-	ctx = &sc->offchannel.chan;
-	cfg80211_chandef_create(&ctx->chandef, chan, NL80211_CHAN_HT20);
-	INIT_LIST_HEAD(&ctx->vifs);
-	ctx->txpower = ATH_TXPOWER_MAX;
-	for (j = 0; j < ARRAY_SIZE(ctx->acq); j++)
-		INIT_LIST_HEAD(&ctx->acq[j]);
-	sc->offchannel.chan.offchannel = true;
-
 }
 
-void ath9k_chanctx_force_active(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif)
+void ath_chanctx_set_channel(struct ath_softc *sc, struct ath_chanctx *ctx,
+			     struct cfg80211_chan_def *chandef)
 {
-	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ath_vif *avp = (struct ath_vif *) vif->drv_priv;
-	bool changed = false;
-
-	if (!test_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags))
-		return;
-
-	if (!avp->chanctx)
-		return;
-
-	mutex_lock(&sc->mutex);
+	bool cur_chan;
 
 	spin_lock_bh(&sc->chan_lock);
-	if (sc->next_chan || (sc->cur_chan != avp->chanctx)) {
-		sc->next_chan = avp->chanctx;
-		changed = true;
-	}
-	sc->sched.state = ATH_CHANCTX_STATE_FORCE_ACTIVE;
+	if (chandef)
+		memcpy(&ctx->chandef, chandef, sizeof(*chandef));
+	cur_chan = sc->cur_chan == ctx;
 	spin_unlock_bh(&sc->chan_lock);
 
-	if (changed)
-		ath_chanctx_set_next(sc, true);
+	if (!cur_chan) {
+		ath_dbg(common, CHAN_CTX,
+			"Current context differs from the new context\n");
+		return;
+	}
 
-	mutex_unlock(&sc->mutex);
+	ath_set_channel(sc);
 }
 
-void ath_chanctx_switch(struct ath_softc *sc, struct ath_chanctx *ctx,
-			struct cfg80211_chan_def *chandef)
-{
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
 
-	spin_lock_bh(&sc->chan_lock);
+/**********************************************************/
+/* Functions to handle the channel context state machine. */
+/**********************************************************/
 
-	if (test_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags) &&
-	    (sc->cur_chan != ctx) && (ctx == &sc->offchannel.chan)) {
-		sc->sched.offchannel_pending = true;
-		spin_unlock_bh(&sc->chan_lock);
-		return;
+static const char *offchannel_state_string(enum ath_offchannel_state state)
+{
+	switch (state) {
+		case_rtn_string(ATH_OFFCHANNEL_IDLE);
+		case_rtn_string(ATH_OFFCHANNEL_PROBE_SEND);
+		case_rtn_string(ATH_OFFCHANNEL_PROBE_WAIT);
+		case_rtn_string(ATH_OFFCHANNEL_SUSPEND);
+		case_rtn_string(ATH_OFFCHANNEL_ROC_START);
+		case_rtn_string(ATH_OFFCHANNEL_ROC_WAIT);
+		case_rtn_string(ATH_OFFCHANNEL_ROC_DONE);
+	default:
+		return "unknown";
 	}
+}
 
-	sc->next_chan = ctx;
-	if (chandef)
-		ctx->chandef = *chandef;
-
-	if (sc->next_chan == &sc->offchannel.chan) {
-		sc->sched.offchannel_duration =
-			TU_TO_USEC(sc->offchannel.duration) +
-			sc->sched.channel_switch_time;
+static const char *chanctx_event_string(enum ath_chanctx_event ev)
+{
+	switch (ev) {
+		case_rtn_string(ATH_CHANCTX_EVENT_BEACON_PREPARE);
+		case_rtn_string(ATH_CHANCTX_EVENT_BEACON_SENT);
+		case_rtn_string(ATH_CHANCTX_EVENT_TSF_TIMER);
+		case_rtn_string(ATH_CHANCTX_EVENT_BEACON_RECEIVED);
+		case_rtn_string(ATH_CHANCTX_EVENT_ASSOC);
+		case_rtn_string(ATH_CHANCTX_EVENT_SWITCH);
+		case_rtn_string(ATH_CHANCTX_EVENT_ASSIGN);
+		case_rtn_string(ATH_CHANCTX_EVENT_UNASSIGN);
+		case_rtn_string(ATH_CHANCTX_EVENT_CHANGE);
+		case_rtn_string(ATH_CHANCTX_EVENT_ENABLE_MULTICHANNEL);
+	default:
+		return "unknown";
 	}
-	spin_unlock_bh(&sc->chan_lock);
-	ieee80211_queue_work(sc->hw, &sc->chanctx_work);
 }
 
-void ath_chanctx_set_channel(struct ath_softc *sc, struct ath_chanctx *ctx,
-			     struct cfg80211_chan_def *chandef)
+static const char *chanctx_state_string(enum ath_chanctx_state state)
 {
-	bool cur_chan;
+	switch (state) {
+		case_rtn_string(ATH_CHANCTX_STATE_IDLE);
+		case_rtn_string(ATH_CHANCTX_STATE_WAIT_FOR_BEACON);
+		case_rtn_string(ATH_CHANCTX_STATE_WAIT_FOR_TIMER);
+		case_rtn_string(ATH_CHANCTX_STATE_SWITCH);
+		case_rtn_string(ATH_CHANCTX_STATE_FORCE_ACTIVE);
+	default:
+		return "unknown";
+	}
+}
 
-	spin_lock_bh(&sc->chan_lock);
-	if (chandef)
-		memcpy(&ctx->chandef, chandef, sizeof(*chandef));
-	cur_chan = sc->cur_chan == ctx;
-	spin_unlock_bh(&sc->chan_lock);
+void ath_chanctx_check_active(struct ath_softc *sc, struct ath_chanctx *ctx)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_vif *avp;
+	bool active = false;
+	u8 n_active = 0;
 
-	if (!cur_chan)
+	if (!ctx)
 		return;
 
-	ath_set_channel(sc);
-}
+	list_for_each_entry(avp, &ctx->vifs, list) {
+		struct ieee80211_vif *vif = avp->vif;
 
-struct ath_chanctx *ath_chanctx_get_oper_chan(struct ath_softc *sc, bool active)
-{
-	struct ath_chanctx *ctx;
+		switch (vif->type) {
+		case NL80211_IFTYPE_P2P_CLIENT:
+		case NL80211_IFTYPE_STATION:
+			if (avp->assoc)
+				active = true;
+			break;
+		default:
+			active = true;
+			break;
+		}
+	}
+	ctx->active = active;
 
 	ath_for_each_chanctx(sc, ctx) {
 		if (!ctx->assigned || list_empty(&ctx->vifs))
 			continue;
-		if (active && !ctx->active)
-			continue;
-
-		if (ctx->switch_after_beacon)
-			return ctx;
+		n_active++;
 	}
 
-	return &sc->chanctx[0];
-}
-
-void ath_chanctx_offchan_switch(struct ath_softc *sc,
-				struct ieee80211_channel *chan)
-{
-	struct cfg80211_chan_def chandef;
-
-	cfg80211_chandef_create(&chandef, chan, NL80211_CHAN_NO_HT);
+	if (n_active <= 1) {
+		clear_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags);
+		return;
+	}
+	if (test_and_set_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags))
+		return;
 
-	ath_chanctx_switch(sc, &sc->offchannel.chan, &chandef);
+	if (ath9k_is_chanctx_enabled()) {
+		ath_chanctx_event(sc, NULL,
+				  ATH_CHANCTX_EVENT_ENABLE_MULTICHANNEL);
+	}
 }
 
 static struct ath_chanctx *
@@ -449,6 +260,9 @@ static void ath_chanctx_adjust_tbtt_delta(struct ath_softc *sc)
 	cur = sc->cur_chan;
 	prev = ath_chanctx_get_next(sc, cur);
 
+	if (!prev->switch_after_beacon)
+		return;
+
 	getrawmonotonic(&ts);
 	cur_tsf = (u32) cur->tsf_val +
 		  ath9k_hw_get_tsf_offset(&cur->tsf_ts, &ts);
@@ -469,25 +283,22 @@ static void ath_chanctx_adjust_tbtt_delta(struct ath_softc *sc)
 	prev->tsf_val += offset;
 }
 
-void ath_chanctx_timer(unsigned long data)
-{
-	struct ath_softc *sc = (struct ath_softc *) data;
-
-	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_TSF_TIMER);
-}
-
 /* Configure the TSF based hardware timer for a channel switch.
  * Also set up backup software timer, in case the gen timer fails.
  * This could be caused by a hardware reset.
  */
 static void ath_chanctx_setup_timer(struct ath_softc *sc, u32 tsf_time)
 {
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_hw *ah = sc->sc_ah;
 
 	ath9k_hw_gen_timer_start(ah, sc->p2p_ps_timer, tsf_time, 1000000);
 	tsf_time -= ath9k_hw_gettsf32(ah);
 	tsf_time = msecs_to_jiffies(tsf_time / 1000) + 1;
-	mod_timer(&sc->sched.timer, tsf_time);
+	mod_timer(&sc->sched.timer, jiffies + tsf_time);
+
+	ath_dbg(common, CHAN_CTX,
+		"Setup chanctx timer with timeout: %d ms\n", jiffies_to_msecs(tsf_time));
 }
 
 void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
@@ -500,40 +311,56 @@ void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 	struct ath_chanctx *ctx;
 	u32 tsf_time;
 	u32 beacon_int;
-	bool noa_changed = false;
 
 	if (vif)
 		avp = (struct ath_vif *) vif->drv_priv;
 
 	spin_lock_bh(&sc->chan_lock);
 
+	ath_dbg(common, CHAN_CTX, "cur_chan: %d MHz, event: %s, state: %s\n",
+		sc->cur_chan->chandef.center_freq1,
+		chanctx_event_string(ev),
+		chanctx_state_string(sc->sched.state));
+
 	switch (ev) {
 	case ATH_CHANCTX_EVENT_BEACON_PREPARE:
 		if (avp->offchannel_duration)
 			avp->offchannel_duration = 0;
 
-		if (avp->chanctx != sc->cur_chan)
+		if (avp->chanctx != sc->cur_chan) {
+			ath_dbg(common, CHAN_CTX,
+				"Contexts differ, not preparing beacon\n");
 			break;
+		}
 
-		if (sc->sched.offchannel_pending) {
+		if (sc->sched.offchannel_pending && !sc->sched.wait_switch) {
 			sc->sched.offchannel_pending = false;
 			sc->next_chan = &sc->offchannel.chan;
 			sc->sched.state = ATH_CHANCTX_STATE_WAIT_FOR_BEACON;
+			ath_dbg(common, CHAN_CTX,
+				"Setting offchannel_pending to false\n");
 		}
 
 		ctx = ath_chanctx_get_next(sc, sc->cur_chan);
 		if (ctx->active && sc->sched.state == ATH_CHANCTX_STATE_IDLE) {
 			sc->next_chan = ctx;
 			sc->sched.state = ATH_CHANCTX_STATE_WAIT_FOR_BEACON;
+			ath_dbg(common, CHAN_CTX,
+				"Set next context, move chanctx state to WAIT_FOR_BEACON\n");
 		}
 
 		/* if the timer missed its window, use the next interval */
-		if (sc->sched.state == ATH_CHANCTX_STATE_WAIT_FOR_TIMER)
+		if (sc->sched.state == ATH_CHANCTX_STATE_WAIT_FOR_TIMER) {
 			sc->sched.state = ATH_CHANCTX_STATE_WAIT_FOR_BEACON;
+			ath_dbg(common, CHAN_CTX,
+				"Move chanctx state from WAIT_FOR_TIMER to WAIT_FOR_BEACON\n");
+		}
 
 		if (sc->sched.state != ATH_CHANCTX_STATE_WAIT_FOR_BEACON)
 			break;
 
+		ath_dbg(common, CHAN_CTX, "Preparing beacon for vif: %pM\n", vif->addr);
+
 		sc->sched.beacon_pending = true;
 		sc->sched.next_tbtt = REG_READ(ah, AR_NEXT_TBTT_TIMER);
 
@@ -545,47 +372,107 @@ void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 		sc->sched.switch_start_time = tsf_time;
 		sc->cur_chan->last_beacon = sc->sched.next_tbtt;
 
-		/* Prevent wrap-around issues */
-		if (avp->periodic_noa_duration &&
-		    tsf_time - avp->periodic_noa_start > BIT(30))
-			avp->periodic_noa_duration = 0;
-
-		if (ctx->active && !avp->periodic_noa_duration) {
-			avp->periodic_noa_start = tsf_time;
-			avp->periodic_noa_duration =
-				TU_TO_USEC(cur_conf->beacon_interval) / 2 -
-				sc->sched.channel_switch_time;
-			noa_changed = true;
-		} else if (!ctx->active && avp->periodic_noa_duration) {
-			avp->periodic_noa_duration = 0;
-			noa_changed = true;
+		/*
+		 * If an offchannel switch is scheduled to happen after
+		 * a beacon transmission, update the NoA with one-shot
+		 * values and increment the index.
+		 */
+		if (sc->next_chan == &sc->offchannel.chan) {
+			avp->noa_index++;
+			avp->offchannel_start = tsf_time;
+			avp->offchannel_duration = sc->sched.offchannel_duration;
+
+			ath_dbg(common, CHAN_CTX,
+				"offchannel noa_duration: %d, noa_start: %d, noa_index: %d\n",
+				avp->offchannel_duration,
+				avp->offchannel_start,
+				avp->noa_index);
+
+			/*
+			 * When multiple contexts are active, the NoA
+			 * has to be recalculated and advertised after
+			 * an offchannel operation.
+			 */
+			if (ctx->active && avp->noa_duration)
+				avp->noa_duration = 0;
+
+			break;
+		}
+
+		/*
+		 * Clear the extend_absence flag if it had been
+		 * set during the previous beacon transmission,
+		 * since we need to revert to the normal NoA
+		 * schedule.
+		 */
+		if (ctx->active && sc->sched.extend_absence) {
+			avp->noa_duration = 0;
+			sc->sched.extend_absence = false;
 		}
 
 		/* If at least two consecutive beacons were missed on the STA
 		 * chanctx, stay on the STA channel for one extra beacon period,
 		 * to resync the timer properly.
 		 */
-		if (ctx->active && sc->sched.beacon_miss >= 2)
-			sc->sched.offchannel_duration = 3 * beacon_int / 2;
-
-		if (sc->sched.offchannel_duration) {
-			noa_changed = true;
-			avp->offchannel_start = tsf_time;
-			avp->offchannel_duration =
-				sc->sched.offchannel_duration;
+		if (ctx->active && sc->sched.beacon_miss >= 2) {
+			avp->noa_duration = 0;
+			sc->sched.extend_absence = true;
 		}
 
-		if (noa_changed)
+		/* Prevent wrap-around issues */
+		if (avp->noa_duration && tsf_time - avp->noa_start > BIT(30))
+			avp->noa_duration = 0;
+
+		/*
+		 * If multiple contexts are active, start periodic
+		 * NoA and increment the index for the first
+		 * announcement.
+		 */
+		if (ctx->active &&
+		    (!avp->noa_duration || sc->sched.force_noa_update)) {
 			avp->noa_index++;
+			avp->noa_start = tsf_time;
+
+			if (sc->sched.extend_absence)
+				avp->noa_duration = (3 * beacon_int / 2) +
+					sc->sched.channel_switch_time;
+			else
+				avp->noa_duration =
+					TU_TO_USEC(cur_conf->beacon_interval) / 2 +
+					sc->sched.channel_switch_time;
+
+			if (test_bit(ATH_OP_SCANNING, &common->op_flags) ||
+			    sc->sched.extend_absence)
+				avp->periodic_noa = false;
+			else
+				avp->periodic_noa = true;
+
+			ath_dbg(common, CHAN_CTX,
+				"noa_duration: %d, noa_start: %d, noa_index: %d, periodic: %d\n",
+				avp->noa_duration,
+				avp->noa_start,
+				avp->noa_index,
+				avp->periodic_noa);
+		}
+
+		if (ctx->active && sc->sched.force_noa_update)
+			sc->sched.force_noa_update = false;
+
 		break;
 	case ATH_CHANCTX_EVENT_BEACON_SENT:
-		if (!sc->sched.beacon_pending)
+		if (!sc->sched.beacon_pending) {
+			ath_dbg(common, CHAN_CTX,
+				"No pending beacon\n");
 			break;
+		}
 
 		sc->sched.beacon_pending = false;
 		if (sc->sched.state != ATH_CHANCTX_STATE_WAIT_FOR_BEACON)
 			break;
 
+		ath_dbg(common, CHAN_CTX,
+			"Move chanctx state to WAIT_FOR_TIMER\n");
+
 		sc->sched.state = ATH_CHANCTX_STATE_WAIT_FOR_TIMER;
 		ath_chanctx_setup_timer(sc, sc->sched.switch_start_time);
 		break;
@@ -597,6 +484,9 @@ void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 		    sc->sched.beacon_pending)
 			sc->sched.beacon_miss++;
 
+		ath_dbg(common, CHAN_CTX,
+			"Move chanctx state to SWITCH\n");
+
 		sc->sched.state = ATH_CHANCTX_STATE_SWITCH;
 		ieee80211_queue_work(sc->hw, &sc->chanctx_work);
 		break;
@@ -625,6 +515,9 @@ void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 		    avp->chanctx != sc->cur_chan)
 			break;
 
+		ath_dbg(common, CHAN_CTX,
+			"Move chanctx state from FORCE_ACTIVE to IDLE\n");
+
 		sc->sched.state = ATH_CHANCTX_STATE_IDLE;
 		/* fall through */
 	case ATH_CHANCTX_EVENT_SWITCH:
@@ -640,10 +533,15 @@ void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 		sc->next_chan = ath_chanctx_get_next(sc, sc->cur_chan);
 		cur_conf = &sc->cur_chan->beacon;
 
+		ath_dbg(common, CHAN_CTX,
+			"Move chanctx state to WAIT_FOR_TIMER (event SWITCH)\n");
+
 		sc->sched.state = ATH_CHANCTX_STATE_WAIT_FOR_TIMER;
+		sc->sched.wait_switch = false;
 
 		tsf_time = TU_TO_USEC(cur_conf->beacon_interval) / 2;
-		if (sc->sched.beacon_miss >= 2) {
+
+		if (sc->sched.extend_absence) {
 			sc->sched.beacon_miss = 0;
 			tsf_time *= 3;
 		}
@@ -679,7 +577,874 @@ void ath_chanctx_event(struct ath_softc *sc, struct ieee80211_vif *vif,
 		sc->next_chan = ctx;
 		ieee80211_queue_work(sc->hw, &sc->chanctx_work);
 		break;
+	case ATH_CHANCTX_EVENT_ASSIGN:
+		/*
+		 * When adding a new channel context, check if a scan
+		 * is in progress and abort it since the addition of
+		 * a new channel context is usually followed by VIF
+		 * assignment, in which case we have to start multi-channel
+		 * operation.
+		 */
+		if (test_bit(ATH_OP_SCANNING, &common->op_flags)) {
+			ath_dbg(common, CHAN_CTX,
+				"Aborting HW scan to add new context\n");
+
+			spin_unlock_bh(&sc->chan_lock);
+			del_timer_sync(&sc->offchannel.timer);
+			ath_scan_complete(sc, true);
+			spin_lock_bh(&sc->chan_lock);
+		}
+		break;
+	case ATH_CHANCTX_EVENT_CHANGE:
+		break;
+	}
+
+	spin_unlock_bh(&sc->chan_lock);
+}
+
+void ath_chanctx_beacon_sent_ev(struct ath_softc *sc,
+				enum ath_chanctx_event ev)
+{
+	if (sc->sched.beacon_pending)
+		ath_chanctx_event(sc, NULL, ev);
+}
+
+void ath_chanctx_beacon_recv_ev(struct ath_softc *sc,
+				enum ath_chanctx_event ev)
+{
+	ath_chanctx_event(sc, NULL, ev);
+}
+
+static int ath_scan_channel_duration(struct ath_softc *sc,
+				     struct ieee80211_channel *chan)
+{
+	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
+
+	if (!req->n_ssids || (chan->flags & IEEE80211_CHAN_NO_IR))
+		return (HZ / 9); /* ~110 ms */
+
+	return (HZ / 16); /* ~60 ms */
+}
+
+static void ath_chanctx_switch(struct ath_softc *sc, struct ath_chanctx *ctx,
+			       struct cfg80211_chan_def *chandef)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	spin_lock_bh(&sc->chan_lock);
+
+	if (test_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags) &&
+	    (sc->cur_chan != ctx) && (ctx == &sc->offchannel.chan)) {
+		if (chandef)
+			ctx->chandef = *chandef;
+
+		sc->sched.offchannel_pending = true;
+		sc->sched.wait_switch = true;
+		sc->sched.offchannel_duration =
+			jiffies_to_usecs(sc->offchannel.duration) +
+			sc->sched.channel_switch_time;
+
+		spin_unlock_bh(&sc->chan_lock);
+		ath_dbg(common, CHAN_CTX,
+			"Set offchannel_pending to true\n");
+		return;
+	}
+
+	sc->next_chan = ctx;
+	if (chandef) {
+		ctx->chandef = *chandef;
+		ath_dbg(common, CHAN_CTX,
+			"Assigned next_chan to %d MHz\n", chandef->center_freq1);
+	}
+
+	if (sc->next_chan == &sc->offchannel.chan) {
+		sc->sched.offchannel_duration =
+			jiffies_to_usecs(sc->offchannel.duration) +
+			sc->sched.channel_switch_time;
+
+		if (chandef) {
+			ath_dbg(common, CHAN_CTX,
+				"Offchannel duration for chan %d MHz : %u\n",
+				chandef->center_freq1,
+				sc->sched.offchannel_duration);
+		}
+	}
+	spin_unlock_bh(&sc->chan_lock);
+	ieee80211_queue_work(sc->hw, &sc->chanctx_work);
+}
+
+static void ath_chanctx_offchan_switch(struct ath_softc *sc,
+				       struct ieee80211_channel *chan)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct cfg80211_chan_def chandef;
+
+	cfg80211_chandef_create(&chandef, chan, NL80211_CHAN_NO_HT);
+	ath_dbg(common, CHAN_CTX,
+		"Channel definition created: %d MHz\n", chandef.center_freq1);
+
+	ath_chanctx_switch(sc, &sc->offchannel.chan, &chandef);
+}
+
+static struct ath_chanctx *ath_chanctx_get_oper_chan(struct ath_softc *sc,
+						     bool active)
+{
+	struct ath_chanctx *ctx;
+
+	ath_for_each_chanctx(sc, ctx) {
+		if (!ctx->assigned || list_empty(&ctx->vifs))
+			continue;
+		if (active && !ctx->active)
+			continue;
+
+		if (ctx->switch_after_beacon)
+			return ctx;
+	}
+
+	return &sc->chanctx[0];
+}
+
+static void
+ath_scan_next_channel(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
+	struct ieee80211_channel *chan;
+
+	if (sc->offchannel.scan_idx >= req->n_channels) {
+		ath_dbg(common, CHAN_CTX,
+			"Moving offchannel state to ATH_OFFCHANNEL_IDLE, "
+			"scan_idx: %d, n_channels: %d\n",
+			sc->offchannel.scan_idx,
+			req->n_channels);
+
+		sc->offchannel.state = ATH_OFFCHANNEL_IDLE;
+		ath_chanctx_switch(sc, ath_chanctx_get_oper_chan(sc, false),
+				   NULL);
+		return;
+	}
+
+	ath_dbg(common, CHAN_CTX,
+		"Moving offchannel state to ATH_OFFCHANNEL_PROBE_SEND, scan_idx: %d\n",
+		sc->offchannel.scan_idx);
+
+	chan = req->channels[sc->offchannel.scan_idx++];
+	sc->offchannel.duration = ath_scan_channel_duration(sc, chan);
+	sc->offchannel.state = ATH_OFFCHANNEL_PROBE_SEND;
+
+	ath_chanctx_offchan_switch(sc, chan);
+}
+
+void ath_offchannel_next(struct ath_softc *sc)
+{
+	struct ieee80211_vif *vif;
+
+	if (sc->offchannel.scan_req) {
+		vif = sc->offchannel.scan_vif;
+		sc->offchannel.chan.txpower = vif->bss_conf.txpower;
+		ath_scan_next_channel(sc);
+	} else if (sc->offchannel.roc_vif) {
+		vif = sc->offchannel.roc_vif;
+		sc->offchannel.chan.txpower = vif->bss_conf.txpower;
+		sc->offchannel.duration =
+			msecs_to_jiffies(sc->offchannel.roc_duration);
+		sc->offchannel.state = ATH_OFFCHANNEL_ROC_START;
+		ath_chanctx_offchan_switch(sc, sc->offchannel.roc_chan);
+	} else {
+		ath_chanctx_switch(sc, ath_chanctx_get_oper_chan(sc, false),
+				   NULL);
+		sc->offchannel.state = ATH_OFFCHANNEL_IDLE;
+		if (sc->ps_idle)
+			ath_cancel_work(sc);
+	}
+}
+
+void ath_roc_complete(struct ath_softc *sc, bool abort)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	if (abort)
+		ath_dbg(common, CHAN_CTX, "RoC aborted\n");
+	else
+		ath_dbg(common, CHAN_CTX, "RoC expired\n");
+
+	sc->offchannel.roc_vif = NULL;
+	sc->offchannel.roc_chan = NULL;
+	if (!abort)
+		ieee80211_remain_on_channel_expired(sc->hw);
+	ath_offchannel_next(sc);
+	ath9k_ps_restore(sc);
+}
+
+void ath_scan_complete(struct ath_softc *sc, bool abort)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	if (abort)
+		ath_dbg(common, CHAN_CTX, "HW scan aborted\n");
+	else
+		ath_dbg(common, CHAN_CTX, "HW scan complete\n");
+
+	sc->offchannel.scan_req = NULL;
+	sc->offchannel.scan_vif = NULL;
+	sc->offchannel.state = ATH_OFFCHANNEL_IDLE;
+	ieee80211_scan_completed(sc->hw, abort);
+	clear_bit(ATH_OP_SCANNING, &common->op_flags);
+	spin_lock_bh(&sc->chan_lock);
+	if (test_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags))
+		sc->sched.force_noa_update = true;
+	spin_unlock_bh(&sc->chan_lock);
+	ath_offchannel_next(sc);
+	ath9k_ps_restore(sc);
+}
+
+static void ath_scan_send_probe(struct ath_softc *sc,
+				struct cfg80211_ssid *ssid)
+{
+	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
+	struct ieee80211_vif *vif = sc->offchannel.scan_vif;
+	struct ath_tx_control txctl = {};
+	struct sk_buff *skb;
+	struct ieee80211_tx_info *info;
+	int band = sc->offchannel.chan.chandef.chan->band;
+
+	skb = ieee80211_probereq_get(sc->hw, vif,
+			ssid->ssid, ssid->ssid_len, req->ie_len);
+	if (!skb)
+		return;
+
+	info = IEEE80211_SKB_CB(skb);
+	if (req->no_cck)
+		info->flags |= IEEE80211_TX_CTL_NO_CCK_RATE;
+
+	if (req->ie_len)
+		memcpy(skb_put(skb, req->ie_len), req->ie, req->ie_len);
+
+	skb_set_queue_mapping(skb, IEEE80211_AC_VO);
+
+	if (!ieee80211_tx_prepare_skb(sc->hw, vif, skb, band, NULL))
+		goto error;
+
+	txctl.txq = sc->tx.txq_map[IEEE80211_AC_VO];
+	txctl.force_channel = true;
+	if (ath_tx_start(sc->hw, skb, &txctl))
+		goto error;
+
+	return;
+
+error:
+	ieee80211_free_txskb(sc->hw, skb);
+}
+
+static void ath_scan_channel_start(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
+	int i;
+
+	if (!(sc->cur_chan->chandef.chan->flags & IEEE80211_CHAN_NO_IR) &&
+	    req->n_ssids) {
+		for (i = 0; i < req->n_ssids; i++)
+			ath_scan_send_probe(sc, &req->ssids[i]);
+
+	}
+
+	ath_dbg(common, CHAN_CTX,
+		"Moving offchannel state to ATH_OFFCHANNEL_PROBE_WAIT\n");
+
+	sc->offchannel.state = ATH_OFFCHANNEL_PROBE_WAIT;
+	mod_timer(&sc->offchannel.timer, jiffies + sc->offchannel.duration);
+}
+
+static void ath_chanctx_timer(unsigned long data)
+{
+	struct ath_softc *sc = (struct ath_softc *) data;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	ath_dbg(common, CHAN_CTX,
+		"Channel context timer invoked\n");
+
+	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_TSF_TIMER);
+}
+
+static void ath_offchannel_timer(unsigned long data)
+{
+	struct ath_softc *sc = (struct ath_softc *)data;
+	struct ath_chanctx *ctx;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	ath_dbg(common, CHAN_CTX, "%s: offchannel state: %s\n",
+		__func__, offchannel_state_string(sc->offchannel.state));
+
+	switch (sc->offchannel.state) {
+	case ATH_OFFCHANNEL_PROBE_WAIT:
+		if (!sc->offchannel.scan_req)
+			return;
+
+		/* get first active channel context */
+		ctx = ath_chanctx_get_oper_chan(sc, true);
+		if (ctx->active) {
+			ath_dbg(common, CHAN_CTX,
+				"Switch to oper/active context, "
+				"move offchannel state to ATH_OFFCHANNEL_SUSPEND\n");
+
+			sc->offchannel.state = ATH_OFFCHANNEL_SUSPEND;
+			ath_chanctx_switch(sc, ctx, NULL);
+			mod_timer(&sc->offchannel.timer, jiffies + HZ / 10);
+			break;
+		}
+		/* fall through */
+	case ATH_OFFCHANNEL_SUSPEND:
+		if (!sc->offchannel.scan_req)
+			return;
+
+		ath_scan_next_channel(sc);
+		break;
+	case ATH_OFFCHANNEL_ROC_START:
+	case ATH_OFFCHANNEL_ROC_WAIT:
+		ctx = ath_chanctx_get_oper_chan(sc, false);
+		sc->offchannel.state = ATH_OFFCHANNEL_ROC_DONE;
+		ath_chanctx_switch(sc, ctx, NULL);
+		break;
+	default:
+		break;
+	}
+}
+
+static bool
+ath_chanctx_send_vif_ps_frame(struct ath_softc *sc, struct ath_vif *avp,
+			      bool powersave)
+{
+	struct ieee80211_vif *vif = avp->vif;
+	struct ieee80211_sta *sta = NULL;
+	struct ieee80211_hdr_3addr *nullfunc;
+	struct ath_tx_control txctl;
+	struct sk_buff *skb;
+	int band = sc->cur_chan->chandef.chan->band;
+
+	switch (vif->type) {
+	case NL80211_IFTYPE_STATION:
+		if (!avp->assoc)
+			return false;
+
+		skb = ieee80211_nullfunc_get(sc->hw, vif);
+		if (!skb)
+			return false;
+
+		nullfunc = (struct ieee80211_hdr_3addr *) skb->data;
+		if (powersave)
+			nullfunc->frame_control |=
+				cpu_to_le16(IEEE80211_FCTL_PM);
+
+		skb_set_queue_mapping(skb, IEEE80211_AC_VO);
+		if (!ieee80211_tx_prepare_skb(sc->hw, vif, skb, band, &sta)) {
+			dev_kfree_skb_any(skb);
+			return false;
+		}
+		break;
+	default:
+		return false;
+	}
+
+	memset(&txctl, 0, sizeof(txctl));
+	txctl.txq = sc->tx.txq_map[IEEE80211_AC_VO];
+	txctl.sta = sta;
+	txctl.force_channel = true;
+	if (ath_tx_start(sc->hw, skb, &txctl)) {
+		ieee80211_free_txskb(sc->hw, skb);
+		return false;
+	}
+
+	return true;
+}
+
+static bool
+ath_chanctx_send_ps_frame(struct ath_softc *sc, bool powersave)
+{
+	struct ath_vif *avp;
+	bool sent = false;
+
+	rcu_read_lock();
+	list_for_each_entry(avp, &sc->cur_chan->vifs, list) {
+		if (ath_chanctx_send_vif_ps_frame(sc, avp, powersave))
+			sent = true;
+	}
+	rcu_read_unlock();
+
+	return sent;
+}
+
+static bool ath_chanctx_defer_switch(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	if (sc->cur_chan == &sc->offchannel.chan)
+		return false;
+
+	switch (sc->sched.state) {
+	case ATH_CHANCTX_STATE_SWITCH:
+		return false;
+	case ATH_CHANCTX_STATE_IDLE:
+		if (!sc->cur_chan->switch_after_beacon)
+			return false;
+
+		ath_dbg(common, CHAN_CTX,
+			"Defer switch, set chanctx state to WAIT_FOR_BEACON\n");
+
+		sc->sched.state = ATH_CHANCTX_STATE_WAIT_FOR_BEACON;
+		break;
+	default:
+		break;
+	}
+
+	return true;
+}
+
+static void ath_offchannel_channel_change(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	ath_dbg(common, CHAN_CTX, "%s: offchannel state: %s\n",
+		__func__, offchannel_state_string(sc->offchannel.state));
+
+	switch (sc->offchannel.state) {
+	case ATH_OFFCHANNEL_PROBE_SEND:
+		if (!sc->offchannel.scan_req)
+			return;
+
+		if (sc->cur_chan->chandef.chan !=
+		    sc->offchannel.chan.chandef.chan)
+			return;
+
+		ath_scan_channel_start(sc);
+		break;
+	case ATH_OFFCHANNEL_IDLE:
+		if (!sc->offchannel.scan_req)
+			return;
+
+		ath_scan_complete(sc, false);
+		break;
+	case ATH_OFFCHANNEL_ROC_START:
+		if (sc->cur_chan != &sc->offchannel.chan)
+			break;
+
+		sc->offchannel.state = ATH_OFFCHANNEL_ROC_WAIT;
+		mod_timer(&sc->offchannel.timer,
+			  jiffies + sc->offchannel.duration);
+		ieee80211_ready_on_channel(sc->hw);
+		break;
+	case ATH_OFFCHANNEL_ROC_DONE:
+		ath_roc_complete(sc, false);
+		break;
+	default:
+		break;
+	}
+}
+
+void ath_chanctx_set_next(struct ath_softc *sc, bool force)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_chanctx *old_ctx;
+	struct timespec ts;
+	bool measure_time = false;
+	bool send_ps = false;
+	bool queues_stopped = false;
+
+	spin_lock_bh(&sc->chan_lock);
+	if (!sc->next_chan) {
+		spin_unlock_bh(&sc->chan_lock);
+		return;
+	}
+
+	if (!force && ath_chanctx_defer_switch(sc)) {
+		spin_unlock_bh(&sc->chan_lock);
+		return;
+	}
+
+	ath_dbg(common, CHAN_CTX,
+		"%s: current: %d MHz, next: %d MHz\n",
+		__func__,
+		sc->cur_chan->chandef.center_freq1,
+		sc->next_chan->chandef.center_freq1);
+
+	if (sc->cur_chan != sc->next_chan) {
+		ath_dbg(common, CHAN_CTX,
+			"Stopping current chanctx: %d\n",
+			sc->cur_chan->chandef.center_freq1);
+		sc->cur_chan->stopped = true;
+		spin_unlock_bh(&sc->chan_lock);
+
+		if (sc->next_chan == &sc->offchannel.chan) {
+			getrawmonotonic(&ts);
+			measure_time = true;
+		}
+
+		ath9k_chanctx_stop_queues(sc, sc->cur_chan);
+		queues_stopped = true;
+
+		__ath9k_flush(sc->hw, ~0, true);
+
+		if (ath_chanctx_send_ps_frame(sc, true))
+			__ath9k_flush(sc->hw, BIT(IEEE80211_AC_VO), false);
+
+		send_ps = true;
+		spin_lock_bh(&sc->chan_lock);
+
+		if (sc->cur_chan != &sc->offchannel.chan) {
+			getrawmonotonic(&sc->cur_chan->tsf_ts);
+			sc->cur_chan->tsf_val = ath9k_hw_gettsf64(sc->sc_ah);
+		}
 	}
+	old_ctx = sc->cur_chan;
+	sc->cur_chan = sc->next_chan;
+	sc->cur_chan->stopped = false;
+	sc->next_chan = NULL;
+
+	if (!sc->sched.offchannel_pending)
+		sc->sched.offchannel_duration = 0;
+
+	if (sc->sched.state != ATH_CHANCTX_STATE_FORCE_ACTIVE)
+		sc->sched.state = ATH_CHANCTX_STATE_IDLE;
 
 	spin_unlock_bh(&sc->chan_lock);
+
+	if (sc->sc_ah->chip_fullsleep ||
+	    memcmp(&sc->cur_chandef, &sc->cur_chan->chandef,
+		   sizeof(sc->cur_chandef))) {
+		ath_dbg(common, CHAN_CTX,
+			"%s: Set channel %d MHz\n",
+			__func__, sc->cur_chan->chandef.center_freq1);
+		ath_set_channel(sc);
+		if (measure_time)
+			sc->sched.channel_switch_time =
+				ath9k_hw_get_tsf_offset(&ts, NULL);
+		/*
+		 * A reset will ensure that all queues are woken up,
+		 * so there is no need to awaken them again.
+		 */
+		goto out;
+	}
+
+	if (queues_stopped)
+		ath9k_chanctx_wake_queues(sc, old_ctx);
+out:
+	if (send_ps)
+		ath_chanctx_send_ps_frame(sc, false);
+
+	ath_offchannel_channel_change(sc);
+	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_SWITCH);
 }
+
+static void ath_chanctx_work(struct work_struct *work)
+{
+	struct ath_softc *sc = container_of(work, struct ath_softc,
+					    chanctx_work);
+	mutex_lock(&sc->mutex);
+	ath_chanctx_set_next(sc, false);
+	mutex_unlock(&sc->mutex);
+}
+
+void ath9k_offchannel_init(struct ath_softc *sc)
+{
+	struct ath_chanctx *ctx;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *chan;
+	int i;
+
+	sband = &common->sbands[IEEE80211_BAND_2GHZ];
+	if (!sband->n_channels)
+		sband = &common->sbands[IEEE80211_BAND_5GHZ];
+
+	chan = &sband->channels[0];
+
+	ctx = &sc->offchannel.chan;
+	INIT_LIST_HEAD(&ctx->vifs);
+	ctx->txpower = ATH_TXPOWER_MAX;
+	cfg80211_chandef_create(&ctx->chandef, chan, NL80211_CHAN_HT20);
+
+	for (i = 0; i < ARRAY_SIZE(ctx->acq); i++)
+		INIT_LIST_HEAD(&ctx->acq[i]);
+
+	sc->offchannel.chan.offchannel = true;
+}
+
+void ath9k_init_channel_context(struct ath_softc *sc)
+{
+	INIT_WORK(&sc->chanctx_work, ath_chanctx_work);
+
+	setup_timer(&sc->offchannel.timer, ath_offchannel_timer,
+		    (unsigned long)sc);
+	setup_timer(&sc->sched.timer, ath_chanctx_timer,
+		    (unsigned long)sc);
+}
+
+void ath9k_deinit_channel_context(struct ath_softc *sc)
+{
+	cancel_work_sync(&sc->chanctx_work);
+}
+
+bool ath9k_is_chanctx_enabled(void)
+{
+	return (ath9k_use_chanctx == 1);
+}
+
+/********************/
+/* Queue management */
+/********************/
+
+void ath9k_chanctx_stop_queues(struct ath_softc *sc, struct ath_chanctx *ctx)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	int i;
+
+	if (ctx == &sc->offchannel.chan) {
+		ieee80211_stop_queue(sc->hw,
+				     sc->hw->offchannel_tx_hw_queue);
+	} else {
+		for (i = 0; i < IEEE80211_NUM_ACS; i++)
+			ieee80211_stop_queue(sc->hw,
+					     ctx->hw_queue_base + i);
+	}
+
+	if (ah->opmode == NL80211_IFTYPE_AP)
+		ieee80211_stop_queue(sc->hw, sc->hw->queues - 2);
+}
+
+
+void ath9k_chanctx_wake_queues(struct ath_softc *sc, struct ath_chanctx *ctx)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	int i;
+
+	if (ctx == &sc->offchannel.chan) {
+		ieee80211_wake_queue(sc->hw,
+				     sc->hw->offchannel_tx_hw_queue);
+	} else {
+		for (i = 0; i < IEEE80211_NUM_ACS; i++)
+			ieee80211_wake_queue(sc->hw,
+					     ctx->hw_queue_base + i);
+	}
+
+	if (ah->opmode == NL80211_IFTYPE_AP)
+		ieee80211_wake_queue(sc->hw, sc->hw->queues - 2);
+}
+
+/*****************/
+/* P2P Powersave */
+/*****************/
+
+static void ath9k_update_p2p_ps_timer(struct ath_softc *sc, struct ath_vif *avp)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	s32 tsf, target_tsf;
+
+	if (!avp || !avp->noa.has_next_tsf)
+		return;
+
+	ath9k_hw_gen_timer_stop(ah, sc->p2p_ps_timer);
+
+	tsf = ath9k_hw_gettsf32(sc->sc_ah);
+
+	target_tsf = avp->noa.next_tsf;
+	if (!avp->noa.absent)
+		target_tsf -= ATH_P2P_PS_STOP_TIME;
+
+	if (target_tsf - tsf < ATH_P2P_PS_STOP_TIME)
+		target_tsf = tsf + ATH_P2P_PS_STOP_TIME;
+
+	ath9k_hw_gen_timer_start(ah, sc->p2p_ps_timer, (u32) target_tsf, 1000000);
+}
+
+static void ath9k_update_p2p_ps(struct ath_softc *sc, struct ieee80211_vif *vif)
+{
+	struct ath_vif *avp = (void *)vif->drv_priv;
+	u32 tsf;
+
+	if (!sc->p2p_ps_timer)
+		return;
+
+	if (vif->type != NL80211_IFTYPE_STATION || !vif->p2p)
+		return;
+
+	sc->p2p_ps_vif = avp;
+	tsf = ath9k_hw_gettsf32(sc->sc_ah);
+	ieee80211_parse_p2p_noa(&vif->bss_conf.p2p_noa_attr, &avp->noa, tsf);
+	ath9k_update_p2p_ps_timer(sc, avp);
+}
+
+static u8 ath9k_get_ctwin(struct ath_softc *sc, struct ath_vif *avp)
+{
+	struct ath_beacon_config *cur_conf = &sc->cur_chan->beacon;
+	u8 switch_time, ctwin;
+
+	/*
+	 * Channel switch in multi-channel mode is deferred
+	 * by a quarter beacon interval when handling
+	 * ATH_CHANCTX_EVENT_BEACON_PREPARE, so the P2P-GO
+	 * interface is guaranteed to be discoverable
+	 * for that duration after a TBTT.
+	 */
+	switch_time = cur_conf->beacon_interval / 4;
+
+	ctwin = avp->vif->bss_conf.p2p_noa_attr.oppps_ctwindow;
+	if (ctwin && (ctwin < switch_time))
+		return ctwin;
+
+	if (switch_time < P2P_DEFAULT_CTWIN)
+		return 0;
+
+	return P2P_DEFAULT_CTWIN;
+}
+
+void ath9k_beacon_add_noa(struct ath_softc *sc, struct ath_vif *avp,
+			  struct sk_buff *skb)
+{
+	static const u8 noa_ie_hdr[] = {
+		WLAN_EID_VENDOR_SPECIFIC,	/* type */
+		0,				/* length */
+		0x50, 0x6f, 0x9a,		/* WFA OUI */
+		0x09,				/* P2P subtype */
+		0x0c,				/* Notice of Absence */
+		0x00,				/* LSB of little-endian len */
+		0x00,				/* MSB of little-endian len */
+	};
+
+	struct ieee80211_p2p_noa_attr *noa;
+	int noa_len, noa_desc, i = 0;
+	u8 *hdr;
+
+	if (!avp->offchannel_duration && !avp->noa_duration)
+		return;
+
+	noa_desc = !!avp->offchannel_duration + !!avp->noa_duration;
+	noa_len = 2 + sizeof(struct ieee80211_p2p_noa_desc) * noa_desc;
+
+	hdr = skb_put(skb, sizeof(noa_ie_hdr));
+	memcpy(hdr, noa_ie_hdr, sizeof(noa_ie_hdr));
+	hdr[1] = sizeof(noa_ie_hdr) + noa_len - 2;
+	hdr[7] = noa_len;
+
+	noa = (void *) skb_put(skb, noa_len);
+	memset(noa, 0, noa_len);
+
+	noa->index = avp->noa_index;
+	noa->oppps_ctwindow = ath9k_get_ctwin(sc, avp);
+
+	if (avp->noa_duration) {
+		if (avp->periodic_noa) {
+			u32 interval = TU_TO_USEC(sc->cur_chan->beacon.beacon_interval);
+			noa->desc[i].count = 255;
+			noa->desc[i].interval = cpu_to_le32(interval);
+		} else {
+			noa->desc[i].count = 1;
+		}
+
+		noa->desc[i].start_time = cpu_to_le32(avp->noa_start);
+		noa->desc[i].duration = cpu_to_le32(avp->noa_duration);
+		i++;
+	}
+
+	if (avp->offchannel_duration) {
+		noa->desc[i].count = 1;
+		noa->desc[i].start_time = cpu_to_le32(avp->offchannel_start);
+		noa->desc[i].duration = cpu_to_le32(avp->offchannel_duration);
+	}
+}
+
+void ath9k_p2p_ps_timer(void *priv)
+{
+	struct ath_softc *sc = priv;
+	struct ath_vif *avp = sc->p2p_ps_vif;
+	struct ieee80211_vif *vif;
+	struct ieee80211_sta *sta;
+	struct ath_node *an;
+	u32 tsf;
+
+	del_timer_sync(&sc->sched.timer);
+	ath9k_hw_gen_timer_stop(sc->sc_ah, sc->p2p_ps_timer);
+	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_TSF_TIMER);
+
+	if (!avp || avp->chanctx != sc->cur_chan)
+		return;
+
+	tsf = ath9k_hw_gettsf32(sc->sc_ah);
+	if (!avp->noa.absent)
+		tsf += ATH_P2P_PS_STOP_TIME;
+
+	if (!avp->noa.has_next_tsf ||
+	    avp->noa.next_tsf - tsf > BIT(31))
+		ieee80211_update_p2p_noa(&avp->noa, tsf);
+
+	ath9k_update_p2p_ps_timer(sc, avp);
+
+	rcu_read_lock();
+
+	vif = avp->vif;
+	sta = ieee80211_find_sta(vif, avp->bssid);
+	if (!sta)
+		goto out;
+
+	an = (void *) sta->drv_priv;
+	if (an->sleeping == !!avp->noa.absent)
+		goto out;
+
+	an->sleeping = avp->noa.absent;
+	if (an->sleeping)
+		ath_tx_aggr_sleep(sta, sc, an);
+	else
+		ath_tx_aggr_wakeup(sc, an);
+
+out:
+	rcu_read_unlock();
+}
+
+void ath9k_p2p_bss_info_changed(struct ath_softc *sc,
+				struct ieee80211_vif *vif)
+{
+	unsigned long flags;
+
+	spin_lock_bh(&sc->sc_pcu_lock);
+	spin_lock_irqsave(&sc->sc_pm_lock, flags);
+	if (!(sc->ps_flags & PS_BEACON_SYNC))
+		ath9k_update_p2p_ps(sc, vif);
+	spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
+	spin_unlock_bh(&sc->sc_pcu_lock);
+}
+
+void ath9k_p2p_beacon_sync(struct ath_softc *sc)
+{
+	if (sc->p2p_ps_vif)
+		ath9k_update_p2p_ps(sc, sc->p2p_ps_vif->vif);
+}
+
+void ath9k_p2p_remove_vif(struct ath_softc *sc,
+			  struct ieee80211_vif *vif)
+{
+	struct ath_vif *avp = (void *)vif->drv_priv;
+
+	spin_lock_bh(&sc->sc_pcu_lock);
+	if (avp == sc->p2p_ps_vif) {
+		sc->p2p_ps_vif = NULL;
+		ath9k_update_p2p_ps_timer(sc, NULL);
+	}
+	spin_unlock_bh(&sc->sc_pcu_lock);
+}
+
+int ath9k_init_p2p(struct ath_softc *sc)
+{
+	sc->p2p_ps_timer = ath_gen_timer_alloc(sc->sc_ah, ath9k_p2p_ps_timer,
+					       NULL, sc, AR_FIRST_NDP_TIMER);
+	if (!sc->p2p_ps_timer)
+		return -ENOMEM;
+
+	return 0;
+}
+
+void ath9k_deinit_p2p(struct ath_softc *sc)
+{
+	if (sc->p2p_ps_timer)
+		ath_gen_timer_free(sc->sc_ah, sc->p2p_ps_timer);
+}
+
+#endif /* CONFIG_ATH9K_CHANNEL_CONTEXT */
diff --git a/drivers/net/wireless/ath/ath9k/common-beacon.c b/drivers/net/wireless/ath/ath9k/common-beacon.c
index 733be5178481..6ad44470d0f2 100644
--- a/drivers/net/wireless/ath/ath9k/common-beacon.c
+++ b/drivers/net/wireless/ath/ath9k/common-beacon.c
@@ -57,7 +57,7 @@ int ath9k_cmn_beacon_config_sta(struct ath_hw *ah,
 				 struct ath9k_beacon_state *bs)
 {
 	struct ath_common *common = ath9k_hw_common(ah);
-	int dtim_intval, sleepduration;
+	int dtim_intval;
 	u64 tsf;
 
 	/* No need to configure beacon if we are not associated */
@@ -75,7 +75,6 @@ int ath9k_cmn_beacon_config_sta(struct ath_hw *ah,
 	 * last beacon we received (which may be none).
 	 */
 	dtim_intval = conf->intval * conf->dtim_period;
-	sleepduration = ah->hw->conf.listen_interval * conf->intval;
 
 	/*
 	 * Pull nexttbtt forward to reflect the current
@@ -113,7 +112,7 @@ int ath9k_cmn_beacon_config_sta(struct ath_hw *ah,
 	 */
 
 	bs->bs_sleepduration = TU_TO_USEC(roundup(IEEE80211_MS_TO_TU(100),
-						  sleepduration));
+						  conf->intval));
 	if (bs->bs_sleepduration > bs->bs_dtimperiod)
 		bs->bs_sleepduration = bs->bs_dtimperiod;
 
diff --git a/drivers/net/wireless/ath/ath9k/debug.c b/drivers/net/wireless/ath/ath9k/debug.c
index d2279365be6f..46f20a309b5f 100644
--- a/drivers/net/wireless/ath/ath9k/debug.c
+++ b/drivers/net/wireless/ath/ath9k/debug.c
@@ -838,7 +838,7 @@ static ssize_t read_file_misc(struct file *file, char __user *user_buf,
 			iter_data.nmeshes, iter_data.nwds);
 		len += scnprintf(buf + len, sizeof(buf) - len,
 			" ADHOC: %i TOTAL: %hi BEACON-VIF: %hi\n",
-			iter_data.nadhocs, sc->nvifs, sc->nbcnvifs);
+			iter_data.nadhocs, sc->cur_chan->nvifs, sc->nbcnvifs);
 	}
 
 	if (len > sizeof(buf))
@@ -1169,6 +1169,29 @@ static const struct file_operations fops_btcoex = {
 };
 #endif
 
+#ifdef CONFIG_ATH9K_DYNACK
+static ssize_t read_file_ackto(struct file *file, char __user *user_buf,
+			       size_t count, loff_t *ppos)
+{
+	struct ath_softc *sc = file->private_data;
+	struct ath_hw *ah = sc->sc_ah;
+	char buf[32];
+	unsigned int len;
+
+	len = sprintf(buf, "%u %c\n", ah->dynack.ackto,
+		      (ah->dynack.enabled) ? 'A' : 'S');
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, len);
+}
+
+static const struct file_operations fops_ackto = {
+	.read = read_file_ackto,
+	.open = simple_open,
+	.owner = THIS_MODULE,
+	.llseek = default_llseek,
+};
+#endif
+
 /* Ethtool support for get-stats */
 
 #define AMKSTR(nm) #nm "_BE", #nm "_BK", #nm "_VI", #nm "_VO"
@@ -1374,5 +1397,10 @@ int ath9k_init_debug(struct ath_hw *ah)
 			    &fops_btcoex);
 #endif
 
+#ifdef CONFIG_ATH9K_DYNACK
+	debugfs_create_file("ack_to", S_IRUSR | S_IWUSR, sc->debug.debugfs_phy,
+			    sc, &fops_ackto);
+#endif
+
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/ath9k/dynack.c b/drivers/net/wireless/ath/ath9k/dynack.c
new file mode 100644
index 000000000000..22b3cc4c27cd
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/dynack.c
@@ -0,0 +1,351 @@
+/*
+ * Copyright (c) 2014, Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "ath9k.h"
+#include "hw.h"
+#include "dynack.h"
+
+#define COMPUTE_TO		(5 * HZ)
+#define LATEACK_DELAY		(10 * HZ)
+#define LATEACK_TO		256
+#define MAX_DELAY		300
+#define EWMA_LEVEL		96
+#define EWMA_DIV		128
+
+/**
+ * ath_dynack_ewma - EWMA (Exponentially Weighted Moving Average) calculation
+ *
+ */
+static inline u32 ath_dynack_ewma(u32 old, u32 new)
+{
+	return (new * (EWMA_DIV - EWMA_LEVEL) + old * EWMA_LEVEL) / EWMA_DIV;
+}
+
+/**
+ * ath_dynack_get_sifs - get sifs time based on phy used
+ * @ah: ath hw
+ * @phy: phy used
+ *
+ */
+static inline u32 ath_dynack_get_sifs(struct ath_hw *ah, int phy)
+{
+	u32 sifs = CCK_SIFS_TIME;
+
+	if (phy == WLAN_RC_PHY_OFDM) {
+		if (IS_CHAN_QUARTER_RATE(ah->curchan))
+			sifs = OFDM_SIFS_TIME_QUARTER;
+		else if (IS_CHAN_HALF_RATE(ah->curchan))
+			sifs = OFDM_SIFS_TIME_HALF;
+		else
+			sifs = OFDM_SIFS_TIME;
+	}
+	return sifs;
+}
+
+/**
+ * ath_dynack_bssidmask - filter out ACK frames based on BSSID mask
+ * @ah: ath hw
+ * @mac: receiver address
+ */
+static inline bool ath_dynack_bssidmask(struct ath_hw *ah, const u8 *mac)
+{
+	int i;
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	for (i = 0; i < ETH_ALEN; i++) {
+		if ((common->macaddr[i] & common->bssidmask[i]) !=
+		    (mac[i] & common->bssidmask[i]))
+			return false;
+	}
+
+	return true;
+}
+
+/**
+ * ath_dynack_compute_ackto - compute ACK timeout as the maximum STA timeout
+ * @ah: ath hw
+ *
+ * should be called while holding qlock
+ */
+static void ath_dynack_compute_ackto(struct ath_hw *ah)
+{
+	struct ath_node *an;
+	u32 to = 0;
+	struct ath_dynack *da = &ah->dynack;
+	struct ath_common *common = ath9k_hw_common(ah);
+
+	list_for_each_entry(an, &da->nodes, list)
+		if (an->ackto > to)
+			to = an->ackto;
+
+	if (to && da->ackto != to) {
+		u32 slottime;
+
+		slottime = (to - 3) / 2;
+		da->ackto = to;
+		ath_dbg(common, DYNACK, "ACK timeout %u slottime %u\n",
+			da->ackto, slottime);
+		ath9k_hw_setslottime(ah, slottime);
+		ath9k_hw_set_ack_timeout(ah, da->ackto);
+		ath9k_hw_set_cts_timeout(ah, da->ackto);
+	}
+}
+
+/**
+ * ath_dynack_compute_to - compute STA ACK timeout
+ * @ah: ath hw
+ *
+ * should be called while holding qlock
+ */
+static void ath_dynack_compute_to(struct ath_hw *ah)
+{
+	u32 ackto, ack_ts;
+	u8 *dst, *src;
+	struct ieee80211_sta *sta;
+	struct ath_node *an;
+	struct ts_info *st_ts;
+	struct ath_dynack *da = &ah->dynack;
+
+	rcu_read_lock();
+
+	while (da->st_rbf.h_rb != da->st_rbf.t_rb &&
+	       da->ack_rbf.h_rb != da->ack_rbf.t_rb) {
+		ack_ts = da->ack_rbf.tstamp[da->ack_rbf.h_rb];
+		st_ts = &da->st_rbf.ts[da->st_rbf.h_rb];
+		dst = da->st_rbf.addr[da->st_rbf.h_rb].h_dest;
+		src = da->st_rbf.addr[da->st_rbf.h_rb].h_src;
+
+		ath_dbg(ath9k_hw_common(ah), DYNACK,
+			"ack_ts %u st_ts %u st_dur %u [%u-%u]\n",
+			ack_ts, st_ts->tstamp, st_ts->dur,
+			da->ack_rbf.h_rb, da->st_rbf.h_rb);
+
+		if (ack_ts > st_ts->tstamp + st_ts->dur) {
+			ackto = ack_ts - st_ts->tstamp - st_ts->dur;
+
+			if (ackto < MAX_DELAY) {
+				sta = ieee80211_find_sta_by_ifaddr(ah->hw, dst,
+								   src);
+				if (sta) {
+					an = (struct ath_node *)sta->drv_priv;
+					an->ackto = ath_dynack_ewma(an->ackto,
+								    ackto);
+					ath_dbg(ath9k_hw_common(ah), DYNACK,
+						"%pM to %u\n", dst, an->ackto);
+					if (time_is_before_jiffies(da->lto)) {
+						ath_dynack_compute_ackto(ah);
+						da->lto = jiffies + COMPUTE_TO;
+					}
+				}
+				INCR(da->ack_rbf.h_rb, ATH_DYN_BUF);
+			}
+			INCR(da->st_rbf.h_rb, ATH_DYN_BUF);
+		} else {
+			INCR(da->ack_rbf.h_rb, ATH_DYN_BUF);
+		}
+	}
+
+	rcu_read_unlock();
+}
+
+/**
+ * ath_dynack_sample_tx_ts - status timestamp sampling method
+ * @ah: ath hw
+ * @skb: socket buffer
+ * @ts: tx status info
+ *
+ */
+void ath_dynack_sample_tx_ts(struct ath_hw *ah, struct sk_buff *skb,
+			     struct ath_tx_status *ts)
+{
+	u8 ridx;
+	struct ieee80211_hdr *hdr;
+	struct ath_dynack *da = &ah->dynack;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+	if ((info->flags & IEEE80211_TX_CTL_NO_ACK) || !da->enabled)
+		return;
+
+	spin_lock_bh(&da->qlock);
+
+	hdr = (struct ieee80211_hdr *)skb->data;
+
+	/* late ACK */
+	if (ts->ts_status & ATH9K_TXERR_XRETRY) {
+		if (ieee80211_is_assoc_req(hdr->frame_control) ||
+		    ieee80211_is_assoc_resp(hdr->frame_control)) {
+			ath_dbg(common, DYNACK, "late ack\n");
+			ath9k_hw_setslottime(ah, (LATEACK_TO - 3) / 2);
+			ath9k_hw_set_ack_timeout(ah, LATEACK_TO);
+			ath9k_hw_set_cts_timeout(ah, LATEACK_TO);
+			da->lto = jiffies + LATEACK_DELAY;
+		}
+
+		spin_unlock_bh(&da->qlock);
+		return;
+	}
+
+	ridx = ts->ts_rateindex;
+
+	da->st_rbf.ts[da->st_rbf.t_rb].tstamp = ts->ts_tstamp;
+	da->st_rbf.ts[da->st_rbf.t_rb].dur = ts->duration;
+	ether_addr_copy(da->st_rbf.addr[da->st_rbf.t_rb].h_dest, hdr->addr1);
+	ether_addr_copy(da->st_rbf.addr[da->st_rbf.t_rb].h_src, hdr->addr2);
+
+	if (!(info->status.rates[ridx].flags & IEEE80211_TX_RC_MCS)) {
+		u32 phy, sifs;
+		const struct ieee80211_rate *rate;
+		struct ieee80211_tx_rate *rates = info->status.rates;
+
+		rate = &common->sbands[info->band].bitrates[rates[ridx].idx];
+		if (info->band == IEEE80211_BAND_2GHZ &&
+		    !(rate->flags & IEEE80211_RATE_ERP_G))
+			phy = WLAN_RC_PHY_CCK;
+		else
+			phy = WLAN_RC_PHY_OFDM;
+
+		sifs = ath_dynack_get_sifs(ah, phy);
+		da->st_rbf.ts[da->st_rbf.t_rb].dur -= sifs;
+	}
+
+	ath_dbg(common, DYNACK, "{%pM} tx sample %u [dur %u][h %u-t %u]\n",
+		hdr->addr1, da->st_rbf.ts[da->st_rbf.t_rb].tstamp,
+		da->st_rbf.ts[da->st_rbf.t_rb].dur, da->st_rbf.h_rb,
+		(da->st_rbf.t_rb + 1) % ATH_DYN_BUF);
+
+	INCR(da->st_rbf.t_rb, ATH_DYN_BUF);
+	if (da->st_rbf.t_rb == da->st_rbf.h_rb)
+		INCR(da->st_rbf.h_rb, ATH_DYN_BUF);
+
+	ath_dynack_compute_to(ah);
+
+	spin_unlock_bh(&da->qlock);
+}
+EXPORT_SYMBOL(ath_dynack_sample_tx_ts);
+
+/**
+ * ath_dynack_sample_ack_ts - ACK timestamp sampling method
+ * @ah: ath hw
+ * @skb: socket buffer
+ * @ts: rx timestamp
+ *
+ */
+void ath_dynack_sample_ack_ts(struct ath_hw *ah, struct sk_buff *skb,
+			      u32 ts)
+{
+	struct ath_dynack *da = &ah->dynack;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+
+	if (!ath_dynack_bssidmask(ah, hdr->addr1) || !da->enabled)
+		return;
+
+	spin_lock_bh(&da->qlock);
+	da->ack_rbf.tstamp[da->ack_rbf.t_rb] = ts;
+
+	ath_dbg(common, DYNACK, "rx sample %u [h %u-t %u]\n",
+		da->ack_rbf.tstamp[da->ack_rbf.t_rb],
+		da->ack_rbf.h_rb, (da->ack_rbf.t_rb + 1) % ATH_DYN_BUF);
+
+	INCR(da->ack_rbf.t_rb, ATH_DYN_BUF);
+	if (da->ack_rbf.t_rb == da->ack_rbf.h_rb)
+		INCR(da->ack_rbf.h_rb, ATH_DYN_BUF);
+
+	ath_dynack_compute_to(ah);
+
+	spin_unlock_bh(&da->qlock);
+}
+EXPORT_SYMBOL(ath_dynack_sample_ack_ts);
+
+/**
+ * ath_dynack_node_init - init ath_node related info
+ * @ah: ath hw
+ * @an: ath node
+ *
+ */
+void ath_dynack_node_init(struct ath_hw *ah, struct ath_node *an)
+{
+	/* ackto = slottime + sifs + air delay */
+	u32 ackto = ATH9K_SLOT_TIME_9 + 16 + 64;
+	struct ath_dynack *da = &ah->dynack;
+
+	an->ackto = ackto;
+
+	spin_lock(&da->qlock);
+	list_add_tail(&an->list, &da->nodes);
+	spin_unlock(&da->qlock);
+}
+EXPORT_SYMBOL(ath_dynack_node_init);
+
+/**
+ * ath_dynack_node_deinit - deinit ath_node related info
+ * @ah: ath hw
+ * @an: ath node
+ *
+ */
+void ath_dynack_node_deinit(struct ath_hw *ah, struct ath_node *an)
+{
+	struct ath_dynack *da = &ah->dynack;
+
+	spin_lock(&da->qlock);
+	list_del(&an->list);
+	spin_unlock(&da->qlock);
+}
+EXPORT_SYMBOL(ath_dynack_node_deinit);
+
+/**
+ * ath_dynack_reset - reset dynack processing
+ * @ah: ath hw
+ *
+ */
+void ath_dynack_reset(struct ath_hw *ah)
+{
+	/* ackto = slottime + sifs + air delay */
+	u32 ackto = ATH9K_SLOT_TIME_9 + 16 + 64;
+	struct ath_dynack *da = &ah->dynack;
+
+	da->lto = jiffies;
+	da->ackto = ackto;
+
+	da->st_rbf.t_rb = 0;
+	da->st_rbf.h_rb = 0;
+	da->ack_rbf.t_rb = 0;
+	da->ack_rbf.h_rb = 0;
+
+	/* init acktimeout */
+	ath9k_hw_setslottime(ah, (ackto - 3) / 2);
+	ath9k_hw_set_ack_timeout(ah, ackto);
+	ath9k_hw_set_cts_timeout(ah, ackto);
+}
+EXPORT_SYMBOL(ath_dynack_reset);
+
+/**
+ * ath_dynack_init - init dynack data structure
+ * @ah: ath hw
+ *
+ */
+void ath_dynack_init(struct ath_hw *ah)
+{
+	struct ath_dynack *da = &ah->dynack;
+
+	memset(da, 0, sizeof(struct ath_dynack));
+
+	spin_lock_init(&da->qlock);
+	INIT_LIST_HEAD(&da->nodes);
+
+	ah->hw->wiphy->features |= NL80211_FEATURE_ACKTO_ESTIMATION;
+}
diff --git a/drivers/net/wireless/ath/ath9k/dynack.h b/drivers/net/wireless/ath/ath9k/dynack.h
new file mode 100644
index 000000000000..6d7bef976742
--- /dev/null
+++ b/drivers/net/wireless/ath/ath9k/dynack.h
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2014, Lorenzo Bianconi <lorenzo.bianconi83@gmail.com>
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef DYNACK_H
+#define DYNACK_H
+
+#define ATH_DYN_BUF	64
+
+struct ath_hw;
+struct ath_node;
+
+/**
+ * struct ath_dyn_rxbuf - ACK frame ring buffer
+ * @h_rb: ring buffer head
+ * @t_rb: ring buffer tail
+ * @tstamp: ACK RX timestamp buffer
+ */
+struct ath_dyn_rxbuf {
+	u16 h_rb, t_rb;
+	u32 tstamp[ATH_DYN_BUF];
+};
+
+struct ts_info {
+	u32 tstamp;
+	u32 dur;
+};
+
+struct haddr_pair {
+	u8 h_dest[ETH_ALEN];
+	u8 h_src[ETH_ALEN];
+};
+
+/**
+ * struct ath_dyn_txbuf - tx frame ring buffer
+ * @h_rb: ring buffer head
+ * @t_rb: ring buffer tail
+ * @addr: dest/src address pair for a given TX frame
+ * @ts: TX frame timestamp buffer
+ */
+struct ath_dyn_txbuf {
+	u16 h_rb, t_rb;
+	struct haddr_pair addr[ATH_DYN_BUF];
+	struct ts_info ts[ATH_DYN_BUF];
+};
+
+/**
+ * struct ath_dynack - dynack processing info
+ * @enabled: enable dyn ack processing
+ * @ackto: current ACK timeout
+ * @lto: last ACK timeout computation
+ * @nodes: ath_node linked list
+ * @qlock: ts queue spinlock
+ * @ack_rbf: ACK ts ring buffer
+ * @st_rbf: status ts ring buffer
+ */
+struct ath_dynack {
+	bool enabled;
+	int ackto;
+	unsigned long lto;
+
+	struct list_head nodes;
+
+	/* protect timestamp queue access */
+	spinlock_t qlock;
+	struct ath_dyn_rxbuf ack_rbf;
+	struct ath_dyn_txbuf st_rbf;
+};
+
+#if defined(CONFIG_ATH9K_DYNACK)
+void ath_dynack_reset(struct ath_hw *ah);
+void ath_dynack_node_init(struct ath_hw *ah, struct ath_node *an);
+void ath_dynack_node_deinit(struct ath_hw *ah, struct ath_node *an);
+void ath_dynack_init(struct ath_hw *ah);
+void ath_dynack_sample_ack_ts(struct ath_hw *ah, struct sk_buff *skb, u32 ts);
+void ath_dynack_sample_tx_ts(struct ath_hw *ah, struct sk_buff *skb,
+			     struct ath_tx_status *ts);
+#else
+static inline void ath_dynack_init(struct ath_hw *ah) {}
+static inline void ath_dynack_node_init(struct ath_hw *ah,
+					struct ath_node *an) {}
+static inline void ath_dynack_node_deinit(struct ath_hw *ah,
+					  struct ath_node *an) {}
+static inline void ath_dynack_sample_ack_ts(struct ath_hw *ah,
+					    struct sk_buff *skb, u32 ts) {}
+static inline void ath_dynack_sample_tx_ts(struct ath_hw *ah,
+					   struct sk_buff *skb,
+					   struct ath_tx_status *ts) {}
+#endif
+
+#endif /* DYNACK_H */
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_init.c b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
index 8a3bd5fe3a54..4014c4be6e79 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_init.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_init.c
@@ -464,6 +464,7 @@ static int ath9k_init_priv(struct ath9k_htc_priv *priv,
 		return -ENOMEM;
 
 	ah->dev = priv->dev;
+	ah->hw = priv->hw;
 	ah->hw_version.devid = devid;
 	ah->hw_version.usbdev = drv_info;
 	ah->ah_flags |= AH_USE_EEPROM;
@@ -592,6 +593,8 @@ static void ath9k_set_hw_capab(struct ath9k_htc_priv *priv,
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN |
 			    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
 
+	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_TDLS;
+
 	hw->queues = 4;
 	hw->max_listen_interval = 1;
 
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_main.c b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
index 5627917c5ff7..994fff1ff519 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_main.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_main.c
@@ -1722,7 +1722,7 @@ static int ath9k_htc_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 }
 
 static void ath9k_htc_set_coverage_class(struct ieee80211_hw *hw,
-					 u8 coverage_class)
+					 s16 coverage_class)
 {
 	struct ath9k_htc_priv *priv = hw->priv;
 
diff --git a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
index bb86eb2ffc95..f0484b1b617e 100644
--- a/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
+++ b/drivers/net/wireless/ath/ath9k/htc_drv_txrx.c
@@ -978,7 +978,7 @@ static bool ath9k_rx_prepare(struct ath9k_htc_priv *priv,
 	struct ath_hw *ah = common->ah;
 	struct ath_htc_rx_status *rxstatus;
 	struct ath_rx_status rx_stats;
-	bool decrypt_error;
+	bool decrypt_error = false;
 
 	if (skb->len < HTC_RX_FRAME_HEADER_SIZE) {
 		ath_err(common, "Corrupted RX frame, dropping (len: %d)\n",
diff --git a/drivers/net/wireless/ath/ath9k/hw-ops.h b/drivers/net/wireless/ath/ath9k/hw-ops.h
index a47ea8423f1e..8e85efeaeffc 100644
--- a/drivers/net/wireless/ath/ath9k/hw-ops.h
+++ b/drivers/net/wireless/ath/ath9k/hw-ops.h
@@ -67,6 +67,12 @@ static inline int ath9k_hw_txprocdesc(struct ath_hw *ah, void *ds,
 	return ath9k_hw_ops(ah)->proc_txdesc(ah, ds, ts);
 }
 
+static inline int ath9k_hw_get_duration(struct ath_hw *ah, const void *ds,
+					int index)
+{
+	return ath9k_hw_ops(ah)->get_duration(ah, ds, index);
+}
+
 static inline void ath9k_hw_antdiv_comb_conf_get(struct ath_hw *ah,
 		struct ath_hw_antcomb_conf *antconf)
 {
diff --git a/drivers/net/wireless/ath/ath9k/hw.c b/drivers/net/wireless/ath/ath9k/hw.c
index 69bbea1184d2..8be4b1453394 100644
--- a/drivers/net/wireless/ath/ath9k/hw.c
+++ b/drivers/net/wireless/ath/ath9k/hw.c
@@ -222,31 +222,28 @@ static void ath9k_hw_read_revisions(struct ath_hw *ah)
 {
 	u32 val;
 
+	if (ah->get_mac_revision)
+		ah->hw_version.macRev = ah->get_mac_revision();
+
 	switch (ah->hw_version.devid) {
 	case AR5416_AR9100_DEVID:
 		ah->hw_version.macVersion = AR_SREV_VERSION_9100;
 		break;
 	case AR9300_DEVID_AR9330:
 		ah->hw_version.macVersion = AR_SREV_VERSION_9330;
-		if (ah->get_mac_revision) {
-			ah->hw_version.macRev = ah->get_mac_revision();
-		} else {
+		if (!ah->get_mac_revision) {
 			val = REG_READ(ah, AR_SREV);
 			ah->hw_version.macRev = MS(val, AR_SREV_REVISION2);
 		}
 		return;
 	case AR9300_DEVID_AR9340:
 		ah->hw_version.macVersion = AR_SREV_VERSION_9340;
-		val = REG_READ(ah, AR_SREV);
-		ah->hw_version.macRev = MS(val, AR_SREV_REVISION2);
 		return;
 	case AR9300_DEVID_QCA955X:
 		ah->hw_version.macVersion = AR_SREV_VERSION_9550;
 		return;
 	case AR9300_DEVID_AR953X:
 		ah->hw_version.macVersion = AR_SREV_VERSION_9531;
-		if (ah->get_mac_revision)
-			ah->hw_version.macRev = ah->get_mac_revision();
 		return;
 	}
 
@@ -647,6 +644,8 @@ int ath9k_hw_init(struct ath_hw *ah)
 		return ret;
 	}
 
+	ath_dynack_init(ah);
+
 	return 0;
 }
 EXPORT_SYMBOL(ath9k_hw_init);
@@ -702,6 +701,8 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 {
 	u32 pll;
 
+	pll = ath9k_hw_compute_pll_control(ah, chan);
+
 	if (AR_SREV_9485(ah) || AR_SREV_9565(ah)) {
 		/* program BB PLL ki and kd value, ki=0x4, kd=0x40 */
 		REG_RMW_FIELD(ah, AR_CH0_BB_DPLL2,
@@ -752,7 +753,8 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 		REG_RMW_FIELD(ah, AR_CH0_DDR_DPLL3,
 			      AR_CH0_DPLL3_PHASE_SHIFT, 0x1);
 
-		REG_WRITE(ah, AR_RTC_PLL_CONTROL, 0x1142c);
+		REG_WRITE(ah, AR_RTC_PLL_CONTROL,
+			  pll | AR_RTC_9300_PLL_BYPASS);
 		udelay(1000);
 
 		/* program refdiv, nint, frac to RTC register */
@@ -768,7 +770,8 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 	} else if (AR_SREV_9340(ah) || AR_SREV_9550(ah) || AR_SREV_9531(ah)) {
 		u32 regval, pll2_divint, pll2_divfrac, refdiv;
 
-		REG_WRITE(ah, AR_RTC_PLL_CONTROL, 0x1142c);
+		REG_WRITE(ah, AR_RTC_PLL_CONTROL,
+			  pll | AR_RTC_9300_SOC_PLL_BYPASS);
 		udelay(1000);
 
 		REG_SET_BIT(ah, AR_PHY_PLL_MODE, 0x1 << 16);
@@ -841,7 +844,6 @@ static void ath9k_hw_init_pll(struct ath_hw *ah,
 		udelay(1000);
 	}
 
-	pll = ath9k_hw_compute_pll_control(ah, chan);
 	if (AR_SREV_9565(ah))
 		pll |= 0x40000;
 	REG_WRITE(ah, AR_RTC_PLL_CONTROL, pll);
@@ -935,21 +937,21 @@ static void ath9k_hw_set_sifs_time(struct ath_hw *ah, u32 us)
 	REG_WRITE(ah, AR_D_GBL_IFS_SIFS, val);
 }
 
-static void ath9k_hw_setslottime(struct ath_hw *ah, u32 us)
+void ath9k_hw_setslottime(struct ath_hw *ah, u32 us)
 {
 	u32 val = ath9k_hw_mac_to_clks(ah, us);
 	val = min(val, (u32) 0xFFFF);
 	REG_WRITE(ah, AR_D_GBL_IFS_SLOT, val);
 }
 
-static void ath9k_hw_set_ack_timeout(struct ath_hw *ah, u32 us)
+void ath9k_hw_set_ack_timeout(struct ath_hw *ah, u32 us)
 {
 	u32 val = ath9k_hw_mac_to_clks(ah, us);
 	val = min(val, (u32) MS(0xFFFFFFFF, AR_TIME_OUT_ACK));
 	REG_RMW_FIELD(ah, AR_TIME_OUT, AR_TIME_OUT_ACK, val);
 }
 
-static void ath9k_hw_set_cts_timeout(struct ath_hw *ah, u32 us)
+void ath9k_hw_set_cts_timeout(struct ath_hw *ah, u32 us)
 {
 	u32 val = ath9k_hw_mac_to_clks(ah, us);
 	val = min(val, (u32) MS(0xFFFFFFFF, AR_TIME_OUT_CTS));
@@ -1053,6 +1055,14 @@ void ath9k_hw_init_global_settings(struct ath_hw *ah)
 		ctstimeout += 48 - sifstime - ah->slottime;
 	}
 
+	if (ah->dynack.enabled) {
+		acktimeout = ah->dynack.ackto;
+		ctstimeout = acktimeout;
+		slottime = (acktimeout - 3) / 2;
+	} else {
+		ah->dynack.ackto = acktimeout;
+	}
+
 	ath9k_hw_set_sifs_time(ah, sifstime);
 	ath9k_hw_setslottime(ah, slottime);
 	ath9k_hw_set_ack_timeout(ah, acktimeout);
@@ -1182,9 +1192,12 @@ static void ath9k_hw_set_operating_mode(struct ath_hw *ah, int opmode)
 
 	switch (opmode) {
 	case NL80211_IFTYPE_ADHOC:
-		set |= AR_STA_ID1_ADHOC;
-		REG_SET_BIT(ah, AR_CFG, AR_CFG_AP_ADHOC_INDICATION);
-		break;
+		if (!AR_SREV_9340_13(ah)) {
+			set |= AR_STA_ID1_ADHOC;
+			REG_SET_BIT(ah, AR_CFG, AR_CFG_AP_ADHOC_INDICATION);
+			break;
+		}
+		/* fall through */
 	case NL80211_IFTYPE_MESH_POINT:
 	case NL80211_IFTYPE_AP:
 		set |= AR_STA_ID1_STA_AP;
@@ -1954,6 +1967,12 @@ int ath9k_hw_reset(struct ath_hw *ah, struct ath9k_channel *chan,
 	if (AR_SREV_9565(ah) && common->bt_ant_diversity)
 		REG_SET_BIT(ah, AR_BTCOEX_WL_LNADIV, AR_BTCOEX_WL_LNADIV_FORCE_ON);
 
+	if (ah->hw->conf.radar_enabled) {
+		/* set HW specific DFS configuration */
+		ah->radar_conf.ext_channel = IS_CHAN_HT40(chan);
+		ath9k_hw_set_radar_params(ah);
+	}
+
 	return 0;
 }
 EXPORT_SYMBOL(ath9k_hw_reset);
diff --git a/drivers/net/wireless/ath/ath9k/hw.h b/drivers/net/wireless/ath/ath9k/hw.h
index 51b4ebe04c04..975074fc11bc 100644
--- a/drivers/net/wireless/ath/ath9k/hw.h
+++ b/drivers/net/wireless/ath/ath9k/hw.h
@@ -29,6 +29,7 @@
 #include "reg.h"
 #include "phy.h"
 #include "btcoex.h"
+#include "dynack.h"
 
 #include "../regd.h"
 
@@ -690,6 +691,7 @@ struct ath_hw_ops {
 			   struct ath_tx_info *i);
 	int (*proc_txdesc)(struct ath_hw *ah, void *ds,
 			   struct ath_tx_status *ts);
+	int (*get_duration)(struct ath_hw *ah, const void *ds, int index);
 	void (*antdiv_comb_conf_get)(struct ath_hw *ah,
 			struct ath_hw_antcomb_conf *antconf);
 	void (*antdiv_comb_conf_set)(struct ath_hw *ah,
@@ -924,6 +926,8 @@ struct ath_hw {
 	int (*external_reset)(void);
 
 	const struct firmware *eeprom_blob;
+
+	struct ath_dynack dynack;
 };
 
 struct ath_bus_ops {
@@ -1080,6 +1084,10 @@ void ar9002_hw_load_ani_reg(struct ath_hw *ah, struct ath9k_channel *chan);
 void ath9k_ani_reset(struct ath_hw *ah, bool is_scanning);
 void ath9k_hw_ani_monitor(struct ath_hw *ah, struct ath9k_channel *chan);
 
+void ath9k_hw_set_ack_timeout(struct ath_hw *ah, u32 us);
+void ath9k_hw_set_cts_timeout(struct ath_hw *ah, u32 us);
+void ath9k_hw_setslottime(struct ath_hw *ah, u32 us);
+
 #ifdef CONFIG_ATH9K_BTCOEX_SUPPORT
 static inline bool ath9k_hw_btcoex_is_enabled(struct ath_hw *ah)
 {
diff --git a/drivers/net/wireless/ath/ath9k/init.c b/drivers/net/wireless/ath/ath9k/init.c
index 39419ea845cc..156a944134dc 100644
--- a/drivers/net/wireless/ath/ath9k/init.c
+++ b/drivers/net/wireless/ath/ath9k/init.c
@@ -61,10 +61,14 @@ static int ath9k_ps_enable;
 module_param_named(ps_enable, ath9k_ps_enable, int, 0444);
 MODULE_PARM_DESC(ps_enable, "Enable WLAN PowerSave");
 
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+
 int ath9k_use_chanctx;
 module_param_named(use_chanctx, ath9k_use_chanctx, int, 0444);
 MODULE_PARM_DESC(use_chanctx, "Enable channel context for concurrency");
 
+#endif /* CONFIG_ATH9K_CHANNEL_CONTEXT */
+
 bool is_ath9k_unloaded;
 
 #ifdef CONFIG_MAC80211_LEDS
@@ -511,7 +515,7 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	sc->tx99_power = MAX_RATE_POWER + 1;
 	init_waitqueue_head(&sc->tx_wait);
 	sc->cur_chan = &sc->chanctx[0];
-	if (!ath9k_use_chanctx)
+	if (!ath9k_is_chanctx_enabled())
 		sc->cur_chan->hw_queue_base = 0;
 
 	if (!pdata || pdata->use_eeprom) {
@@ -567,11 +571,9 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	setup_timer(&sc->sleep_timer, ath_ps_full_sleep, (unsigned long)sc);
 	INIT_WORK(&sc->hw_reset_work, ath_reset_work);
 	INIT_WORK(&sc->paprd_work, ath_paprd_calibrate);
-	INIT_WORK(&sc->chanctx_work, ath_chanctx_work);
 	INIT_DELAYED_WORK(&sc->hw_pll_work, ath_hw_pll_work);
-	setup_timer(&sc->offchannel.timer, ath_offchannel_timer,
-		    (unsigned long)sc);
-	setup_timer(&sc->sched.timer, ath_chanctx_timer, (unsigned long)sc);
+
+	ath9k_init_channel_context(sc);
 
 	/*
 	 * Cache line size is used to size and align various
@@ -600,13 +602,15 @@ static int ath9k_init_softc(u16 devid, struct ath_softc *sc,
 	if (ret)
 		goto err_btcoex;
 
-	sc->p2p_ps_timer = ath_gen_timer_alloc(sc->sc_ah, ath9k_p2p_ps_timer,
-		NULL, sc, AR_FIRST_NDP_TIMER);
+	ret = ath9k_init_p2p(sc);
+	if (ret)
+		goto err_btcoex;
 
 	ath9k_cmn_init_crypto(sc->sc_ah);
 	ath9k_init_misc(sc);
 	ath_fill_led_pin(sc);
 	ath_chanctx_init(sc);
+	ath9k_offchannel_init(sc);
 
 	if (common->bus_ops->aspm_init)
 		common->bus_ops->aspm_init(common);
@@ -672,18 +676,14 @@ static const struct ieee80211_iface_limit wds_limits[] = {
 	{ .max = 2048,	.types = BIT(NL80211_IFTYPE_WDS) },
 };
 
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+
 static const struct ieee80211_iface_limit if_limits_multi[] = {
-	{ .max = 1,	.types = BIT(NL80211_IFTYPE_STATION) },
-	{ .max = 1,	.types = BIT(NL80211_IFTYPE_P2P_CLIENT) |
+	{ .max = 2,	.types = BIT(NL80211_IFTYPE_STATION) |
+				 BIT(NL80211_IFTYPE_AP) |
+				 BIT(NL80211_IFTYPE_P2P_CLIENT) |
 				 BIT(NL80211_IFTYPE_P2P_GO) },
-};
-
-static const struct ieee80211_iface_limit if_dfs_limits[] = {
-	{ .max = 1,	.types = BIT(NL80211_IFTYPE_AP) |
-#ifdef CONFIG_MAC80211_MESH
-				 BIT(NL80211_IFTYPE_MESH_POINT) |
-#endif
-				 BIT(NL80211_IFTYPE_ADHOC) },
+	{ .max = 1,	.types = BIT(NL80211_IFTYPE_ADHOC) },
 };
 
 static const struct ieee80211_iface_combination if_comb_multi[] = {
@@ -696,6 +696,16 @@ static const struct ieee80211_iface_combination if_comb_multi[] = {
 	},
 };
 
+#endif /* CONFIG_ATH9K_CHANNEL_CONTEXT */
+
+static const struct ieee80211_iface_limit if_dfs_limits[] = {
+	{ .max = 1,	.types = BIT(NL80211_IFTYPE_AP) |
+#ifdef CONFIG_MAC80211_MESH
+				 BIT(NL80211_IFTYPE_MESH_POINT) |
+#endif
+				 BIT(NL80211_IFTYPE_ADHOC) },
+};
+
 static const struct ieee80211_iface_combination if_comb[] = {
 	{
 		.limits = if_limits,
@@ -753,8 +763,9 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	if (AR_SREV_9160_10_OR_LATER(sc->sc_ah) || ath9k_modparam_nohwcrypt)
 		hw->flags |= IEEE80211_HW_MFP_CAPABLE;
 
-	hw->wiphy->features |= (NL80211_FEATURE_ACTIVE_MONITOR |
-				NL80211_FEATURE_AP_MODE_CHAN_WIDTH_CHANGE);
+	hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR |
+			       NL80211_FEATURE_AP_MODE_CHAN_WIDTH_CHANGE |
+			       NL80211_FEATURE_P2P_GO_CTWIN;
 
 	if (!config_enabled(CONFIG_ATH9K_TX99)) {
 		hw->wiphy->interface_modes =
@@ -763,24 +774,31 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 			BIT(NL80211_IFTYPE_AP) |
 			BIT(NL80211_IFTYPE_STATION) |
 			BIT(NL80211_IFTYPE_ADHOC) |
-			BIT(NL80211_IFTYPE_MESH_POINT);
-		if (!ath9k_use_chanctx) {
+			BIT(NL80211_IFTYPE_MESH_POINT) |
+			BIT(NL80211_IFTYPE_WDS);
+
 			hw->wiphy->iface_combinations = if_comb;
 			hw->wiphy->n_iface_combinations = ARRAY_SIZE(if_comb);
-			hw->wiphy->interface_modes |= BIT(NL80211_IFTYPE_WDS);
-		} else {
-			hw->wiphy->iface_combinations = if_comb_multi;
-			hw->wiphy->n_iface_combinations =
-				ARRAY_SIZE(if_comb_multi);
-			hw->wiphy->max_scan_ssids = 255;
-			hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
-			hw->wiphy->max_remain_on_channel_duration = 10000;
-			hw->chanctx_data_size = sizeof(void *);
-			hw->extra_beacon_tailroom =
-				sizeof(struct ieee80211_p2p_noa_attr) + 9;
-		}
 	}
 
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+
+	if (ath9k_is_chanctx_enabled()) {
+		hw->wiphy->interface_modes &= ~ BIT(NL80211_IFTYPE_WDS);
+		hw->wiphy->iface_combinations = if_comb_multi;
+		hw->wiphy->n_iface_combinations = ARRAY_SIZE(if_comb_multi);
+		hw->wiphy->max_scan_ssids = 255;
+		hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
+		hw->wiphy->max_remain_on_channel_duration = 10000;
+		hw->chanctx_data_size = sizeof(void *);
+		hw->extra_beacon_tailroom =
+			sizeof(struct ieee80211_p2p_noa_attr) + 9;
+
+		ath_dbg(common, CHAN_CTX, "Use channel contexts\n");
+	}
+
+#endif /* CONFIG_ATH9K_CHANNEL_CONTEXT */
+
 	hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
@@ -793,7 +811,7 @@ static void ath9k_set_hw_capab(struct ath_softc *sc, struct ieee80211_hw *hw)
 	/* allow 4 queues per channel context +
 	 * 1 cab queue + 1 offchannel tx queue
 	 */
-	hw->queues = 10;
+	hw->queues = ATH9K_NUM_TX_QUEUES;
 	/* last queue for offchannel */
 	hw->offchannel_tx_hw_queue = hw->queues - 1;
 	hw->max_rates = 4;
@@ -915,9 +933,7 @@ static void ath9k_deinit_softc(struct ath_softc *sc)
 {
 	int i = 0;
 
-	if (sc->p2p_ps_timer)
-		ath_gen_timer_free(sc->sc_ah, sc->p2p_ps_timer);
-
+	ath9k_deinit_p2p(sc);
 	ath9k_deinit_btcoex(sc);
 
 	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++)
diff --git a/drivers/net/wireless/ath/ath9k/mac.h b/drivers/net/wireless/ath/ath9k/mac.h
index 6c56cafa5ca4..aa69ceaad0be 100644
--- a/drivers/net/wireless/ath/ath9k/mac.h
+++ b/drivers/net/wireless/ath/ath9k/mac.h
@@ -121,6 +121,7 @@ struct ath_tx_status {
 	u32 evm0;
 	u32 evm1;
 	u32 evm2;
+	u32 duration;
 };
 
 struct ath_rx_status {
diff --git a/drivers/net/wireless/ath/ath9k/main.c b/drivers/net/wireless/ath/ath9k/main.c
index e6ac8d2e610c..6f6a974f7fdb 100644
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -60,8 +60,10 @@ static bool ath9k_has_pending_frames(struct ath_softc *sc, struct ath_txq *txq)
 
 	spin_lock_bh(&txq->axq_lock);
 
-	if (txq->axq_depth)
+	if (txq->axq_depth) {
 		pending = true;
+		goto out;
+	}
 
 	if (txq->mac80211_qnum >= 0) {
 		struct list_head *list;
@@ -70,6 +72,7 @@ static bool ath9k_has_pending_frames(struct ath_softc *sc, struct ath_txq *txq)
 		if (!list_empty(list))
 			pending = true;
 	}
+out:
 	spin_unlock_bh(&txq->axq_lock);
 	return pending;
 }
@@ -223,18 +226,12 @@ static bool ath_complete_reset(struct ath_softc *sc, bool start)
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	unsigned long flags;
-	int i;
-
-	if (ath_startrecv(sc) != 0) {
-		ath_err(common, "Unable to restart recv logic\n");
-		return false;
-	}
 
+	ath9k_calculate_summary_state(sc, sc->cur_chan);
+	ath_startrecv(sc);
 	ath9k_cmn_update_txpow(ah, sc->curtxpow,
 			       sc->cur_chan->txpower, &sc->curtxpow);
-
 	clear_bit(ATH_OP_HW_RESET, &common->op_flags);
-	ath9k_calculate_summary_state(sc, sc->cur_chan);
 
 	if (!sc->cur_chan->offchannel && start) {
 		/* restore per chanctx TSF timer */
@@ -267,22 +264,7 @@ static bool ath_complete_reset(struct ath_softc *sc, bool start)
 
 	ath9k_hw_set_interrupts(ah);
 	ath9k_hw_enable_interrupts(ah);
-
-	if (!ath9k_use_chanctx)
-		ieee80211_wake_queues(sc->hw);
-	else {
-		if (sc->cur_chan == &sc->offchannel.chan)
-			ieee80211_wake_queue(sc->hw,
-					sc->hw->offchannel_tx_hw_queue);
-		else {
-			for (i = 0; i < IEEE80211_NUM_ACS; i++)
-				ieee80211_wake_queue(sc->hw,
-					sc->cur_chan->hw_queue_base + i);
-		}
-		if (ah->opmode == NL80211_IFTYPE_AP)
-			ieee80211_wake_queue(sc->hw, sc->hw->queues - 2);
-	}
-
+	ieee80211_wake_queues(sc->hw);
 	ath9k_p2p_ps_timer(sc);
 
 	return true;
@@ -314,6 +296,9 @@ int ath_reset_internal(struct ath_softc *sc, struct ath9k_channel *hchan)
 	if (!ath_prepare_reset(sc))
 		fastcc = false;
 
+	if (ath9k_is_chanctx_enabled())
+		fastcc = false;
+
 	spin_lock_bh(&sc->chan_lock);
 	sc->cur_chandef = sc->cur_chan->chandef;
 	spin_unlock_bh(&sc->chan_lock);
@@ -358,12 +343,16 @@ static void ath_node_attach(struct ath_softc *sc, struct ieee80211_sta *sta,
 	memset(&an->key_idx, 0, sizeof(an->key_idx));
 
 	ath_tx_node_init(sc, an);
+
+	ath_dynack_node_init(sc->sc_ah, an);
 }
 
 static void ath_node_detach(struct ath_softc *sc, struct ieee80211_sta *sta)
 {
 	struct ath_node *an = (struct ath_node *)sta->drv_priv;
 	ath_tx_node_cleanup(sc, an);
+
+	ath_dynack_node_deinit(sc->sc_ah, an);
 }
 
 void ath9k_tasklet(unsigned long data)
@@ -513,7 +502,7 @@ irqreturn_t ath_isr(int irq, void *dev)
 	 * touch anything. Note this can happen early
 	 * on if the IRQ is shared.
 	 */
-	if (test_bit(ATH_OP_INVALID, &common->op_flags))
+	if (!ah || test_bit(ATH_OP_INVALID, &common->op_flags))
 		return IRQ_NONE;
 
 	/* shared irq, not for us */
@@ -822,7 +811,8 @@ static void ath9k_stop(struct ieee80211_hw *hw)
 	struct ath_common *common = ath9k_hw_common(ah);
 	bool prev_idle;
 
-	cancel_work_sync(&sc->chanctx_work);
+	ath9k_deinit_channel_context(sc);
+
 	mutex_lock(&sc->mutex);
 
 	ath_cancel_work(sc);
@@ -903,9 +893,10 @@ static bool ath9k_uses_beacons(int type)
 	}
 }
 
-static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
+static void ath9k_vif_iter(struct ath9k_vif_iter_data *iter_data,
+			   u8 *mac, struct ieee80211_vif *vif)
 {
-	struct ath9k_vif_iter_data *iter_data = data;
+	struct ath_vif *avp = (struct ath_vif *)vif->drv_priv;
 	int i;
 
 	if (iter_data->has_hw_macaddr) {
@@ -923,12 +914,10 @@ static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 	switch (vif->type) {
 	case NL80211_IFTYPE_AP:
 		iter_data->naps++;
-		if (vif->bss_conf.enable_beacon)
-			iter_data->beacons = true;
 		break;
 	case NL80211_IFTYPE_STATION:
 		iter_data->nstations++;
-		if (vif->bss_conf.assoc && !iter_data->primary_sta)
+		if (avp->assoc && !iter_data->primary_sta)
 			iter_data->primary_sta = vif;
 		break;
 	case NL80211_IFTYPE_ADHOC:
@@ -949,6 +938,34 @@ static void ath9k_vif_iter(void *data, u8 *mac, struct ieee80211_vif *vif)
 	}
 }
 
+static void ath9k_update_bssid_mask(struct ath_softc *sc,
+				    struct ath_chanctx *ctx,
+				    struct ath9k_vif_iter_data *iter_data)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_vif *avp;
+	int i;
+
+	if (!ath9k_is_chanctx_enabled())
+		return;
+
+	list_for_each_entry(avp, &ctx->vifs, list) {
+		if (ctx->nvifs_assigned != 1)
+			continue;
+
+		if (!avp->vif->p2p || !iter_data->has_hw_macaddr)
+			continue;
+
+		ether_addr_copy(common->curbssid, avp->bssid);
+
+		/* perm_addr will be used as the p2p device address. */
+		for (i = 0; i < ETH_ALEN; i++)
+			iter_data->mask[i] &=
+				~(iter_data->hw_macaddr[i] ^
+				  sc->hw->wiphy->perm_addr[i]);
+	}
+}
+
 /* Called with sc->mutex held. */
 void ath9k_calculate_iter_data(struct ath_softc *sc,
 			       struct ath_chanctx *ctx,
@@ -968,38 +985,20 @@ void ath9k_calculate_iter_data(struct ath_softc *sc,
 	list_for_each_entry(avp, &ctx->vifs, list)
 		ath9k_vif_iter(iter_data, avp->vif->addr, avp->vif);
 
-	if (ctx == &sc->offchannel.chan) {
-		struct ieee80211_vif *vif;
-
-		if (sc->offchannel.state < ATH_OFFCHANNEL_ROC_START)
-			vif = sc->offchannel.scan_vif;
-		else
-			vif = sc->offchannel.roc_vif;
-
-		if (vif)
-			ath9k_vif_iter(iter_data, vif->addr, vif);
-		iter_data->beacons = false;
-	}
+	ath9k_update_bssid_mask(sc, ctx, iter_data);
 }
 
 static void ath9k_set_assoc_state(struct ath_softc *sc,
 				  struct ieee80211_vif *vif, bool changed)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	struct ath_vif *avp = (struct ath_vif *)vif->drv_priv;
 	unsigned long flags;
 
 	set_bit(ATH_OP_PRIM_STA_VIF, &common->op_flags);
-	/* Set the AID, BSSID and do beacon-sync only when
-	 * the HW opmode is STATION.
-	 *
-	 * But the primary bit is set above in any case.
-	 */
-	if (sc->sc_ah->opmode != NL80211_IFTYPE_STATION)
-		return;
 
-	ether_addr_copy(common->curbssid, bss_conf->bssid);
-	common->curaid = bss_conf->aid;
+	ether_addr_copy(common->curbssid, avp->bssid);
+	common->curaid = avp->aid;
 	ath9k_hw_write_associd(sc->sc_ah);
 
 	if (changed) {
@@ -1019,6 +1018,43 @@ static void ath9k_set_assoc_state(struct ath_softc *sc,
 		vif->addr, common->curbssid);
 }
 
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+static void ath9k_set_offchannel_state(struct ath_softc *sc)
+{
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ieee80211_vif *vif = NULL;
+
+	ath9k_ps_wakeup(sc);
+
+	if (sc->offchannel.state < ATH_OFFCHANNEL_ROC_START)
+		vif = sc->offchannel.scan_vif;
+	else
+		vif = sc->offchannel.roc_vif;
+
+	if (WARN_ON(!vif))
+		goto exit;
+
+	eth_zero_addr(common->curbssid);
+	eth_broadcast_addr(common->bssidmask);
+	ether_addr_copy(common->macaddr, vif->addr);
+	common->curaid = 0;
+	ah->opmode = vif->type;
+	ah->imask &= ~ATH9K_INT_SWBA;
+	ah->imask &= ~ATH9K_INT_TSFOOR;
+	ah->slottime = ATH9K_SLOT_TIME_9;
+
+	ath_hw_setbssidmask(common);
+	ath9k_hw_setopmode(ah);
+	ath9k_hw_write_associd(sc->sc_ah);
+	ath9k_hw_set_interrupts(ah);
+	ath9k_hw_init_global_settings(ah);
+
+exit:
+	ath9k_ps_restore(sc);
+}
+#endif
+
 /* Called with sc->mutex held. */
 void ath9k_calculate_summary_state(struct ath_softc *sc,
 				   struct ath_chanctx *ctx)
@@ -1026,12 +1062,18 @@ void ath9k_calculate_summary_state(struct ath_softc *sc,
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_vif_iter_data iter_data;
+	struct ath_beacon_config *cur_conf;
 
 	ath_chanctx_check_active(sc, ctx);
 
 	if (ctx != sc->cur_chan)
 		return;
 
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+	if (ctx == &sc->offchannel.chan)
+		return ath9k_set_offchannel_state(sc);
+#endif
+
 	ath9k_ps_wakeup(sc);
 	ath9k_calculate_iter_data(sc, ctx, &iter_data);
 
@@ -1042,8 +1084,11 @@ void ath9k_calculate_summary_state(struct ath_softc *sc,
 	ath_hw_setbssidmask(common);
 
 	if (iter_data.naps > 0) {
+		cur_conf = &ctx->beacon;
 		ath9k_hw_set_tsfadjust(ah, true);
 		ah->opmode = NL80211_IFTYPE_AP;
+		if (cur_conf->enable_beacon)
+			iter_data.beacons = true;
 	} else {
 		ath9k_hw_set_tsfadjust(ah, false);
 
@@ -1072,13 +1117,11 @@ void ath9k_calculate_summary_state(struct ath_softc *sc,
 	if (ah->opmode == NL80211_IFTYPE_STATION) {
 		bool changed = (iter_data.primary_sta != ctx->primary_sta);
 
-		iter_data.beacons = true;
 		if (iter_data.primary_sta) {
+			iter_data.beacons = true;
 			ath9k_set_assoc_state(sc, iter_data.primary_sta,
 					      changed);
-			if (!ctx->primary_sta ||
-			    !ctx->primary_sta->bss_conf.assoc)
-				ctx->primary_sta = iter_data.primary_sta;
+			ctx->primary_sta = iter_data.primary_sta;
 		} else {
 			ctx->primary_sta = NULL;
 			memset(common->curbssid, 0, ETH_ALEN);
@@ -1107,11 +1150,27 @@ void ath9k_calculate_summary_state(struct ath_softc *sc,
 	else
 		clear_bit(ATH_OP_PRIM_STA_VIF, &common->op_flags);
 
-	ctx->primary_sta = iter_data.primary_sta;
+	ath_dbg(common, CONFIG,
+		"macaddr: %pM, bssid: %pM, bssidmask: %pM\n",
+		common->macaddr, common->curbssid, common->bssidmask);
 
 	ath9k_ps_restore(sc);
 }
 
+static void ath9k_assign_hw_queues(struct ieee80211_hw *hw,
+				   struct ieee80211_vif *vif)
+{
+	int i;
+
+	for (i = 0; i < IEEE80211_NUM_ACS; i++)
+		vif->hw_queue[i] = i;
+
+	if (vif->type == NL80211_IFTYPE_AP)
+		vif->cab_queue = hw->queues - 2;
+	else
+		vif->cab_queue = IEEE80211_INVAL_HW_QUEUE;
+}
+
 static int ath9k_add_interface(struct ieee80211_hw *hw,
 			       struct ieee80211_vif *vif)
 {
@@ -1120,12 +1179,11 @@ static int ath9k_add_interface(struct ieee80211_hw *hw,
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
 	struct ath_node *an = &avp->mcast_node;
-	int i;
 
 	mutex_lock(&sc->mutex);
 
 	if (config_enabled(CONFIG_ATH9K_TX99)) {
-		if (sc->nvifs >= 1) {
+		if (sc->cur_chan->nvifs >= 1) {
 			mutex_unlock(&sc->mutex);
 			return -EOPNOTSUPP;
 		}
@@ -1133,22 +1191,18 @@ static int ath9k_add_interface(struct ieee80211_hw *hw,
 	}
 
 	ath_dbg(common, CONFIG, "Attach a VIF of type: %d\n", vif->type);
-	sc->nvifs++;
+	sc->cur_chan->nvifs++;
 
 	if (ath9k_uses_beacons(vif->type))
 		ath9k_beacon_assign_slot(sc, vif);
 
 	avp->vif = vif;
-	if (!ath9k_use_chanctx) {
+	if (!ath9k_is_chanctx_enabled()) {
 		avp->chanctx = sc->cur_chan;
 		list_add_tail(&avp->list, &avp->chanctx->vifs);
 	}
-	for (i = 0; i < IEEE80211_NUM_ACS; i++)
-		vif->hw_queue[i] = i;
-	if (vif->type == NL80211_IFTYPE_AP)
-		vif->cab_queue = hw->queues - 2;
-	else
-		vif->cab_queue = IEEE80211_INVAL_HW_QUEUE;
+
+	ath9k_assign_hw_queues(hw, vif);
 
 	an->sc = sc;
 	an->sta = NULL;
@@ -1168,7 +1222,6 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
 	struct ath_softc *sc = hw->priv;
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
-	int i;
 
 	mutex_lock(&sc->mutex);
 
@@ -1188,43 +1241,13 @@ static int ath9k_change_interface(struct ieee80211_hw *hw,
 	if (ath9k_uses_beacons(vif->type))
 		ath9k_beacon_assign_slot(sc, vif);
 
-	for (i = 0; i < IEEE80211_NUM_ACS; i++)
-		vif->hw_queue[i] = i;
-
-	if (vif->type == NL80211_IFTYPE_AP)
-		vif->cab_queue = hw->queues - 2;
-	else
-		vif->cab_queue = IEEE80211_INVAL_HW_QUEUE;
-
+	ath9k_assign_hw_queues(hw, vif);
 	ath9k_calculate_summary_state(sc, avp->chanctx);
 
 	mutex_unlock(&sc->mutex);
 	return 0;
 }
 
-static void
-ath9k_update_p2p_ps_timer(struct ath_softc *sc, struct ath_vif *avp)
-{
-	struct ath_hw *ah = sc->sc_ah;
-	s32 tsf, target_tsf;
-
-	if (!avp || !avp->noa.has_next_tsf)
-		return;
-
-	ath9k_hw_gen_timer_stop(ah, sc->p2p_ps_timer);
-
-	tsf = ath9k_hw_gettsf32(sc->sc_ah);
-
-	target_tsf = avp->noa.next_tsf;
-	if (!avp->noa.absent)
-		target_tsf -= ATH_P2P_PS_STOP_TIME;
-
-	if (target_tsf - tsf < ATH_P2P_PS_STOP_TIME)
-		target_tsf = tsf + ATH_P2P_PS_STOP_TIME;
-
-	ath9k_hw_gen_timer_start(ah, sc->p2p_ps_timer, (u32) target_tsf, 1000000);
-}
-
 static void ath9k_remove_interface(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif)
 {
@@ -1236,16 +1259,11 @@ static void ath9k_remove_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&sc->mutex);
 
-	spin_lock_bh(&sc->sc_pcu_lock);
-	if (avp == sc->p2p_ps_vif) {
-		sc->p2p_ps_vif = NULL;
-		ath9k_update_p2p_ps_timer(sc, NULL);
-	}
-	spin_unlock_bh(&sc->sc_pcu_lock);
+	ath9k_p2p_remove_vif(sc, vif);
 
-	sc->nvifs--;
+	sc->cur_chan->nvifs--;
 	sc->tx99_vif = NULL;
-	if (!ath9k_use_chanctx)
+	if (!ath9k_is_chanctx_enabled())
 		list_del(&avp->list);
 
 	if (ath9k_uses_beacons(vif->type))
@@ -1423,7 +1441,7 @@ static int ath9k_config(struct ieee80211_hw *hw, u32 changed)
 		}
 	}
 
-	if (!ath9k_use_chanctx && (changed & IEEE80211_CONF_CHANGE_CHANNEL)) {
+	if (!ath9k_is_chanctx_enabled() && (changed & IEEE80211_CONF_CHANGE_CHANNEL)) {
 		ctx->offchannel = !!(conf->flags & IEEE80211_CONF_OFFCHANNEL);
 		ath_chanctx_set_channel(sc, ctx, &hw->conf.chandef);
 	}
@@ -1463,7 +1481,10 @@ static void ath9k_configure_filter(struct ieee80211_hw *hw,
 	changed_flags &= SUPPORTED_FILTERS;
 	*total_flags &= SUPPORTED_FILTERS;
 
-	sc->rx.rxfilter = *total_flags;
+	spin_lock_bh(&sc->chan_lock);
+	sc->cur_chan->rxfilter = *total_flags;
+	spin_unlock_bh(&sc->chan_lock);
+
 	ath9k_ps_wakeup(sc);
 	rfilt = ath_calcrxfilter(sc);
 	ath9k_hw_setrxfilter(sc->sc_ah, rfilt);
@@ -1687,70 +1708,6 @@ static int ath9k_set_key(struct ieee80211_hw *hw,
 	return ret;
 }
 
-void ath9k_p2p_ps_timer(void *priv)
-{
-	struct ath_softc *sc = priv;
-	struct ath_vif *avp = sc->p2p_ps_vif;
-	struct ieee80211_vif *vif;
-	struct ieee80211_sta *sta;
-	struct ath_node *an;
-	u32 tsf;
-
-	del_timer_sync(&sc->sched.timer);
-	ath9k_hw_gen_timer_stop(sc->sc_ah, sc->p2p_ps_timer);
-	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_TSF_TIMER);
-
-	if (!avp || avp->chanctx != sc->cur_chan)
-		return;
-
-	tsf = ath9k_hw_gettsf32(sc->sc_ah);
-	if (!avp->noa.absent)
-		tsf += ATH_P2P_PS_STOP_TIME;
-
-	if (!avp->noa.has_next_tsf ||
-	    avp->noa.next_tsf - tsf > BIT(31))
-		ieee80211_update_p2p_noa(&avp->noa, tsf);
-
-	ath9k_update_p2p_ps_timer(sc, avp);
-
-	rcu_read_lock();
-
-	vif = avp->vif;
-	sta = ieee80211_find_sta(vif, vif->bss_conf.bssid);
-	if (!sta)
-		goto out;
-
-	an = (void *) sta->drv_priv;
-	if (an->sleeping == !!avp->noa.absent)
-		goto out;
-
-	an->sleeping = avp->noa.absent;
-	if (an->sleeping)
-		ath_tx_aggr_sleep(sta, sc, an);
-	else
-		ath_tx_aggr_wakeup(sc, an);
-
-out:
-	rcu_read_unlock();
-}
-
-void ath9k_update_p2p_ps(struct ath_softc *sc, struct ieee80211_vif *vif)
-{
-	struct ath_vif *avp = (void *)vif->drv_priv;
-	u32 tsf;
-
-	if (!sc->p2p_ps_timer)
-		return;
-
-	if (vif->type != NL80211_IFTYPE_STATION || !vif->p2p)
-		return;
-
-	sc->p2p_ps_vif = avp;
-	tsf = ath9k_hw_gettsf32(sc->sc_ah);
-	ieee80211_parse_p2p_noa(&vif->bss_conf.p2p_noa_attr, &avp->noa, tsf);
-	ath9k_update_p2p_ps_timer(sc, avp);
-}
-
 static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 				   struct ieee80211_vif *vif,
 				   struct ieee80211_bss_conf *bss_conf,
@@ -1765,7 +1722,6 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
-	unsigned long flags;
 	int slottime;
 
 	ath9k_ps_wakeup(sc);
@@ -1775,9 +1731,17 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 		ath_dbg(common, CONFIG, "BSSID %pM Changed ASSOC %d\n",
 			bss_conf->bssid, bss_conf->assoc);
 
+		ether_addr_copy(avp->bssid, bss_conf->bssid);
+		avp->aid = bss_conf->aid;
+		avp->assoc = bss_conf->assoc;
+
 		ath9k_calculate_summary_state(sc, avp->chanctx);
-		if (bss_conf->assoc)
-			ath_chanctx_event(sc, vif, ATH_CHANCTX_EVENT_ASSOC);
+
+		if (ath9k_is_chanctx_enabled()) {
+			if (bss_conf->assoc)
+				ath_chanctx_event(sc, vif,
+						  ATH_CHANCTX_EVENT_ASSOC);
+		}
 	}
 
 	if (changed & BSS_CHANGED_IBSS) {
@@ -1789,9 +1753,9 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 	if ((changed & BSS_CHANGED_BEACON_ENABLED) ||
 	    (changed & BSS_CHANGED_BEACON_INT) ||
 	    (changed & BSS_CHANGED_BEACON_INFO)) {
+		ath9k_beacon_config(sc, vif, changed);
 		if (changed & BSS_CHANGED_BEACON_ENABLED)
 			ath9k_calculate_summary_state(sc, avp->chanctx);
-		ath9k_beacon_config(sc, vif, changed);
 	}
 
 	if ((avp->chanctx == sc->cur_chan) &&
@@ -1814,14 +1778,8 @@ static void ath9k_bss_info_changed(struct ieee80211_hw *hw,
 		}
 	}
 
-	if (changed & BSS_CHANGED_P2P_PS) {
-		spin_lock_bh(&sc->sc_pcu_lock);
-		spin_lock_irqsave(&sc->sc_pm_lock, flags);
-		if (!(sc->ps_flags & PS_BEACON_SYNC))
-			ath9k_update_p2p_ps(sc, vif);
-		spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
-		spin_unlock_bh(&sc->sc_pcu_lock);
-	}
+	if (changed & BSS_CHANGED_P2P_PS)
+		ath9k_p2p_bss_info_changed(sc, vif);
 
 	if (changed & CHECK_ANI)
 		ath_check_ani(sc);
@@ -1959,7 +1917,22 @@ static int ath9k_get_survey(struct ieee80211_hw *hw, int idx,
 	return 0;
 }
 
-static void ath9k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
+static void ath9k_enable_dynack(struct ath_softc *sc)
+{
+#ifdef CONFIG_ATH9K_DYNACK
+	u32 rfilt;
+	struct ath_hw *ah = sc->sc_ah;
+
+	ath_dynack_reset(ah);
+
+	ah->dynack.enabled = true;
+	rfilt = ath_calcrxfilter(sc);
+	ath9k_hw_setrxfilter(ah, rfilt);
+#endif
+}
+
+static void ath9k_set_coverage_class(struct ieee80211_hw *hw,
+				     s16 coverage_class)
 {
 	struct ath_softc *sc = hw->priv;
 	struct ath_hw *ah = sc->sc_ah;
@@ -1968,11 +1941,22 @@ static void ath9k_set_coverage_class(struct ieee80211_hw *hw, u8 coverage_class)
 		return;
 
 	mutex_lock(&sc->mutex);
-	ah->coverage_class = coverage_class;
 
-	ath9k_ps_wakeup(sc);
-	ath9k_hw_init_global_settings(ah);
-	ath9k_ps_restore(sc);
+	if (coverage_class >= 0) {
+		ah->coverage_class = coverage_class;
+		if (ah->dynack.enabled) {
+			u32 rfilt;
+
+			ah->dynack.enabled = false;
+			rfilt = ath_calcrxfilter(sc);
+			ath9k_hw_setrxfilter(ah, rfilt);
+		}
+		ath9k_ps_wakeup(sc);
+		ath9k_hw_init_global_settings(ah);
+		ath9k_ps_restore(sc);
+	} else if (!ah->dynack.enabled) {
+		ath9k_enable_dynack(sc);
+	}
 
 	mutex_unlock(&sc->mutex);
 }
@@ -1985,9 +1969,6 @@ static bool ath9k_has_tx_pending(struct ath_softc *sc)
 		if (!ATH_TXQ_SETUP(sc, i))
 			continue;
 
-		if (!sc->tx.txq[i].axq_depth)
-			continue;
-
 		npend = ath9k_has_pending_frames(sc, &sc->tx.txq[i]);
 		if (npend)
 			break;
@@ -2013,7 +1994,6 @@ void __ath9k_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 	struct ath_common *common = ath9k_hw_common(ah);
 	int timeout = HZ / 5; /* 200 ms */
 	bool drain_txq;
-	int i;
 
 	cancel_delayed_work_sync(&sc->tx_complete_work);
 
@@ -2041,10 +2021,6 @@ void __ath9k_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 			ath_reset(sc);
 
 		ath9k_ps_restore(sc);
-		for (i = 0; i < IEEE80211_NUM_ACS; i++) {
-			ieee80211_wake_queue(sc->hw,
-					     sc->cur_chan->hw_queue_base + i);
-		}
 	}
 
 	ieee80211_queue_delayed_work(hw, &sc->tx_complete_work, 0);
@@ -2053,16 +2029,8 @@ void __ath9k_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 static bool ath9k_tx_frames_pending(struct ieee80211_hw *hw)
 {
 	struct ath_softc *sc = hw->priv;
-	int i;
-
-	for (i = 0; i < ATH9K_NUM_TX_QUEUES; i++) {
-		if (!ATH_TXQ_SETUP(sc, i))
-			continue;
 
-		if (ath9k_has_pending_frames(sc, &sc->tx.txq[i]))
-			return true;
-	}
-	return false;
+	return ath9k_has_tx_pending(sc);
 }
 
 static int ath9k_tx_last_beacon(struct ieee80211_hw *hw)
@@ -2207,207 +2175,7 @@ static void ath9k_sw_scan_complete(struct ieee80211_hw *hw)
 	clear_bit(ATH_OP_SCANNING, &common->op_flags);
 }
 
-static int ath_scan_channel_duration(struct ath_softc *sc,
-				     struct ieee80211_channel *chan)
-{
-	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
-
-	if (!req->n_ssids || (chan->flags & IEEE80211_CHAN_NO_IR))
-		return (HZ / 9); /* ~110 ms */
-
-	return (HZ / 16); /* ~60 ms */
-}
-
-static void
-ath_scan_next_channel(struct ath_softc *sc)
-{
-	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
-	struct ieee80211_channel *chan;
-
-	if (sc->offchannel.scan_idx >= req->n_channels) {
-		sc->offchannel.state = ATH_OFFCHANNEL_IDLE;
-		ath_chanctx_switch(sc, ath_chanctx_get_oper_chan(sc, false),
-				   NULL);
-		return;
-	}
-
-	chan = req->channels[sc->offchannel.scan_idx++];
-	sc->offchannel.duration = ath_scan_channel_duration(sc, chan);
-	sc->offchannel.state = ATH_OFFCHANNEL_PROBE_SEND;
-	ath_chanctx_offchan_switch(sc, chan);
-}
-
-static void ath_offchannel_next(struct ath_softc *sc)
-{
-	struct ieee80211_vif *vif;
-
-	if (sc->offchannel.scan_req) {
-		vif = sc->offchannel.scan_vif;
-		sc->offchannel.chan.txpower = vif->bss_conf.txpower;
-		ath_scan_next_channel(sc);
-	} else if (sc->offchannel.roc_vif) {
-		vif = sc->offchannel.roc_vif;
-		sc->offchannel.chan.txpower = vif->bss_conf.txpower;
-		sc->offchannel.duration = sc->offchannel.roc_duration;
-		sc->offchannel.state = ATH_OFFCHANNEL_ROC_START;
-		ath_chanctx_offchan_switch(sc, sc->offchannel.roc_chan);
-	} else {
-		ath_chanctx_switch(sc, ath_chanctx_get_oper_chan(sc, false),
-				   NULL);
-		sc->offchannel.state = ATH_OFFCHANNEL_IDLE;
-		if (sc->ps_idle)
-			ath_cancel_work(sc);
-	}
-}
-
-static void ath_roc_complete(struct ath_softc *sc, bool abort)
-{
-	sc->offchannel.roc_vif = NULL;
-	sc->offchannel.roc_chan = NULL;
-	if (!abort)
-		ieee80211_remain_on_channel_expired(sc->hw);
-	ath_offchannel_next(sc);
-	ath9k_ps_restore(sc);
-}
-
-static void ath_scan_complete(struct ath_softc *sc, bool abort)
-{
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-
-	sc->offchannel.scan_req = NULL;
-	sc->offchannel.scan_vif = NULL;
-	sc->offchannel.state = ATH_OFFCHANNEL_IDLE;
-	ieee80211_scan_completed(sc->hw, abort);
-	clear_bit(ATH_OP_SCANNING, &common->op_flags);
-	ath_offchannel_next(sc);
-	ath9k_ps_restore(sc);
-}
-
-static void ath_scan_send_probe(struct ath_softc *sc,
-				struct cfg80211_ssid *ssid)
-{
-	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
-	struct ieee80211_vif *vif = sc->offchannel.scan_vif;
-	struct ath_tx_control txctl = {};
-	struct sk_buff *skb;
-	struct ieee80211_tx_info *info;
-	int band = sc->offchannel.chan.chandef.chan->band;
-
-	skb = ieee80211_probereq_get(sc->hw, vif,
-			ssid->ssid, ssid->ssid_len, req->ie_len);
-	if (!skb)
-		return;
-
-	info = IEEE80211_SKB_CB(skb);
-	if (req->no_cck)
-		info->flags |= IEEE80211_TX_CTL_NO_CCK_RATE;
-
-	if (req->ie_len)
-		memcpy(skb_put(skb, req->ie_len), req->ie, req->ie_len);
-
-	skb_set_queue_mapping(skb, IEEE80211_AC_VO);
-
-	if (!ieee80211_tx_prepare_skb(sc->hw, vif, skb, band, NULL))
-		goto error;
-
-	txctl.txq = sc->tx.txq_map[IEEE80211_AC_VO];
-	txctl.force_channel = true;
-	if (ath_tx_start(sc->hw, skb, &txctl))
-		goto error;
-
-	return;
-
-error:
-	ieee80211_free_txskb(sc->hw, skb);
-}
-
-static void ath_scan_channel_start(struct ath_softc *sc)
-{
-	struct cfg80211_scan_request *req = sc->offchannel.scan_req;
-	int i;
-
-	if (!(sc->cur_chan->chandef.chan->flags & IEEE80211_CHAN_NO_IR) &&
-	    req->n_ssids) {
-		for (i = 0; i < req->n_ssids; i++)
-			ath_scan_send_probe(sc, &req->ssids[i]);
-
-	}
-
-	sc->offchannel.state = ATH_OFFCHANNEL_PROBE_WAIT;
-	mod_timer(&sc->offchannel.timer, jiffies + sc->offchannel.duration);
-}
-
-void ath_offchannel_channel_change(struct ath_softc *sc)
-{
-	switch (sc->offchannel.state) {
-	case ATH_OFFCHANNEL_PROBE_SEND:
-		if (!sc->offchannel.scan_req)
-			return;
-
-		if (sc->cur_chan->chandef.chan !=
-		    sc->offchannel.chan.chandef.chan)
-			return;
-
-		ath_scan_channel_start(sc);
-		break;
-	case ATH_OFFCHANNEL_IDLE:
-		if (!sc->offchannel.scan_req)
-			return;
-
-		ath_scan_complete(sc, false);
-		break;
-	case ATH_OFFCHANNEL_ROC_START:
-		if (sc->cur_chan != &sc->offchannel.chan)
-			break;
-
-		sc->offchannel.state = ATH_OFFCHANNEL_ROC_WAIT;
-		mod_timer(&sc->offchannel.timer, jiffies +
-			  msecs_to_jiffies(sc->offchannel.duration));
-		ieee80211_ready_on_channel(sc->hw);
-		break;
-	case ATH_OFFCHANNEL_ROC_DONE:
-		ath_roc_complete(sc, false);
-		break;
-	default:
-		break;
-	}
-}
-
-void ath_offchannel_timer(unsigned long data)
-{
-	struct ath_softc *sc = (struct ath_softc *)data;
-	struct ath_chanctx *ctx;
-
-	switch (sc->offchannel.state) {
-	case ATH_OFFCHANNEL_PROBE_WAIT:
-		if (!sc->offchannel.scan_req)
-			return;
-
-		/* get first active channel context */
-		ctx = ath_chanctx_get_oper_chan(sc, true);
-		if (ctx->active) {
-			sc->offchannel.state = ATH_OFFCHANNEL_SUSPEND;
-			ath_chanctx_switch(sc, ctx, NULL);
-			mod_timer(&sc->offchannel.timer, jiffies + HZ / 10);
-			break;
-		}
-		/* fall through */
-	case ATH_OFFCHANNEL_SUSPEND:
-		if (!sc->offchannel.scan_req)
-			return;
-
-		ath_scan_next_channel(sc);
-		break;
-	case ATH_OFFCHANNEL_ROC_START:
-	case ATH_OFFCHANNEL_ROC_WAIT:
-		ctx = ath_chanctx_get_oper_chan(sc, false);
-		sc->offchannel.state = ATH_OFFCHANNEL_ROC_DONE;
-		ath_chanctx_switch(sc, ctx, NULL);
-		break;
-	default:
-		break;
-	}
-}
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
 
 static int ath9k_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			 struct ieee80211_scan_request *hw_req)
@@ -2430,8 +2198,13 @@ static int ath9k_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	sc->offchannel.scan_req = req;
 	sc->offchannel.scan_idx = 0;
 
-	if (sc->offchannel.state == ATH_OFFCHANNEL_IDLE)
+	ath_dbg(common, CHAN_CTX, "HW scan request received on vif: %pM\n",
+		vif->addr);
+
+	if (sc->offchannel.state == ATH_OFFCHANNEL_IDLE) {
+		ath_dbg(common, CHAN_CTX, "Starting HW scan\n");
 		ath_offchannel_next(sc);
+	}
 
 out:
 	mutex_unlock(&sc->mutex);
@@ -2443,6 +2216,9 @@ static void ath9k_cancel_hw_scan(struct ieee80211_hw *hw,
 				 struct ieee80211_vif *vif)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+
+	ath_dbg(common, CHAN_CTX, "Cancel HW scan on vif: %pM\n", vif->addr);
 
 	mutex_lock(&sc->mutex);
 	del_timer_sync(&sc->offchannel.timer);
@@ -2456,6 +2232,7 @@ static int ath9k_remain_on_channel(struct ieee80211_hw *hw,
 				   enum ieee80211_roc_type type)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	int ret = 0;
 
 	mutex_lock(&sc->mutex);
@@ -2470,8 +2247,14 @@ static int ath9k_remain_on_channel(struct ieee80211_hw *hw,
 	sc->offchannel.roc_chan = chan;
 	sc->offchannel.roc_duration = duration;
 
-	if (sc->offchannel.state == ATH_OFFCHANNEL_IDLE)
+	ath_dbg(common, CHAN_CTX,
+		"RoC request on vif: %pM, type: %d duration: %d\n",
+		vif->addr, type, duration);
+
+	if (sc->offchannel.state == ATH_OFFCHANNEL_IDLE) {
+		ath_dbg(common, CHAN_CTX, "Starting RoC period\n");
 		ath_offchannel_next(sc);
+	}
 
 out:
 	mutex_unlock(&sc->mutex);
@@ -2482,9 +2265,11 @@ out:
 static int ath9k_cancel_remain_on_channel(struct ieee80211_hw *hw)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 
 	mutex_lock(&sc->mutex);
 
+	ath_dbg(common, CHAN_CTX, "Cancel RoC\n");
 	del_timer_sync(&sc->offchannel.timer);
 
 	if (sc->offchannel.roc_vif) {
@@ -2501,6 +2286,7 @@ static int ath9k_add_chanctx(struct ieee80211_hw *hw,
 			     struct ieee80211_chanctx_conf *conf)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_chanctx *ctx, **ptr;
 	int pos;
 
@@ -2515,10 +2301,18 @@ static int ath9k_add_chanctx(struct ieee80211_hw *hw,
 		ctx->assigned = true;
 		pos = ctx - &sc->chanctx[0];
 		ctx->hw_queue_base = pos * IEEE80211_NUM_ACS;
+
+		ath_dbg(common, CHAN_CTX,
+			"Add channel context: %d MHz\n",
+			conf->def.chan->center_freq);
+
 		ath_chanctx_set_channel(sc, ctx, &conf->def);
+		ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_ASSIGN);
+
 		mutex_unlock(&sc->mutex);
 		return 0;
 	}
+
 	mutex_unlock(&sc->mutex);
 	return -ENOSPC;
 }
@@ -2528,12 +2322,19 @@ static void ath9k_remove_chanctx(struct ieee80211_hw *hw,
 				 struct ieee80211_chanctx_conf *conf)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_chanctx *ctx = ath_chanctx_get(conf);
 
 	mutex_lock(&sc->mutex);
+
+	ath_dbg(common, CHAN_CTX,
+		"Remove channel context: %d MHz\n",
+		conf->def.chan->center_freq);
+
 	ctx->assigned = false;
-	ctx->hw_queue_base = -1;
+	ctx->hw_queue_base = 0;
 	ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_UNASSIGN);
+
 	mutex_unlock(&sc->mutex);
 }
 
@@ -2542,9 +2343,13 @@ static void ath9k_change_chanctx(struct ieee80211_hw *hw,
 				 u32 changed)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_chanctx *ctx = ath_chanctx_get(conf);
 
 	mutex_lock(&sc->mutex);
+	ath_dbg(common, CHAN_CTX,
+		"Change channel context: %d MHz\n",
+		conf->def.chan->center_freq);
 	ath_chanctx_set_channel(sc, ctx, &conf->def);
 	mutex_unlock(&sc->mutex);
 }
@@ -2554,16 +2359,25 @@ static int ath9k_assign_vif_chanctx(struct ieee80211_hw *hw,
 				    struct ieee80211_chanctx_conf *conf)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
 	struct ath_chanctx *ctx = ath_chanctx_get(conf);
 	int i;
 
 	mutex_lock(&sc->mutex);
+
+	ath_dbg(common, CHAN_CTX,
+		"Assign VIF (addr: %pM, type: %d, p2p: %d) to channel context: %d MHz\n",
+		vif->addr, vif->type, vif->p2p,
+		conf->def.chan->center_freq);
+
 	avp->chanctx = ctx;
+	ctx->nvifs_assigned++;
 	list_add_tail(&avp->list, &ctx->vifs);
 	ath9k_calculate_summary_state(sc, ctx);
 	for (i = 0; i < IEEE80211_NUM_ACS; i++)
 		vif->hw_queue[i] = ctx->hw_queue_base + i;
+
 	mutex_unlock(&sc->mutex);
 
 	return 0;
@@ -2574,36 +2388,80 @@ static void ath9k_unassign_vif_chanctx(struct ieee80211_hw *hw,
 				       struct ieee80211_chanctx_conf *conf)
 {
 	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
 	struct ath_chanctx *ctx = ath_chanctx_get(conf);
 	int ac;
 
 	mutex_lock(&sc->mutex);
+
+	ath_dbg(common, CHAN_CTX,
+		"Remove VIF (addr: %pM, type: %d, p2p: %d) from channel context: %d MHz\n",
+		vif->addr, vif->type, vif->p2p,
+		conf->def.chan->center_freq);
+
 	avp->chanctx = NULL;
+	ctx->nvifs_assigned--;
 	list_del(&avp->list);
 	ath9k_calculate_summary_state(sc, ctx);
 	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
 		vif->hw_queue[ac] = IEEE80211_INVAL_HW_QUEUE;
+
+	mutex_unlock(&sc->mutex);
+}
+
+static void ath9k_mgd_prepare_tx(struct ieee80211_hw *hw,
+				 struct ieee80211_vif *vif)
+{
+	struct ath_softc *sc = hw->priv;
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ath_vif *avp = (struct ath_vif *) vif->drv_priv;
+	bool changed = false;
+
+	if (!test_bit(ATH_OP_MULTI_CHANNEL, &common->op_flags))
+		return;
+
+	if (!avp->chanctx)
+		return;
+
+	mutex_lock(&sc->mutex);
+
+	spin_lock_bh(&sc->chan_lock);
+	if (sc->next_chan || (sc->cur_chan != avp->chanctx)) {
+		sc->next_chan = avp->chanctx;
+		changed = true;
+	}
+	ath_dbg(common, CHAN_CTX,
+		"%s: Set chanctx state to FORCE_ACTIVE, changed: %d\n",
+		__func__, changed);
+	sc->sched.state = ATH_CHANCTX_STATE_FORCE_ACTIVE;
+	spin_unlock_bh(&sc->chan_lock);
+
+	if (changed)
+		ath_chanctx_set_next(sc, true);
+
 	mutex_unlock(&sc->mutex);
 }
 
 void ath9k_fill_chanctx_ops(void)
 {
-	if (!ath9k_use_chanctx)
+	if (!ath9k_is_chanctx_enabled())
 		return;
 
-	ath9k_ops.hw_scan = ath9k_hw_scan;
-	ath9k_ops.cancel_hw_scan = ath9k_cancel_hw_scan;
-	ath9k_ops.remain_on_channel  = ath9k_remain_on_channel;
+	ath9k_ops.hw_scan                  = ath9k_hw_scan;
+	ath9k_ops.cancel_hw_scan           = ath9k_cancel_hw_scan;
+	ath9k_ops.remain_on_channel        = ath9k_remain_on_channel;
 	ath9k_ops.cancel_remain_on_channel = ath9k_cancel_remain_on_channel;
-	ath9k_ops.add_chanctx        = ath9k_add_chanctx;
-	ath9k_ops.remove_chanctx     = ath9k_remove_chanctx;
-	ath9k_ops.change_chanctx     = ath9k_change_chanctx;
-	ath9k_ops.assign_vif_chanctx = ath9k_assign_vif_chanctx;
-	ath9k_ops.unassign_vif_chanctx = ath9k_unassign_vif_chanctx;
-	ath9k_ops.mgd_prepare_tx = ath9k_chanctx_force_active;
+	ath9k_ops.add_chanctx              = ath9k_add_chanctx;
+	ath9k_ops.remove_chanctx           = ath9k_remove_chanctx;
+	ath9k_ops.change_chanctx           = ath9k_change_chanctx;
+	ath9k_ops.assign_vif_chanctx       = ath9k_assign_vif_chanctx;
+	ath9k_ops.unassign_vif_chanctx     = ath9k_unassign_vif_chanctx;
+	ath9k_ops.mgd_prepare_tx           = ath9k_mgd_prepare_tx;
 }
 
+#endif
+
 struct ieee80211_ops ath9k_ops = {
 	.tx 		    = ath9k_tx,
 	.start 		    = ath9k_start,
diff --git a/drivers/net/wireless/ath/ath9k/recv.c b/drivers/net/wireless/ath/ath9k/recv.c
index 74ab1d02013b..6914e21816e4 100644
--- a/drivers/net/wireless/ath/ath9k/recv.c
+++ b/drivers/net/wireless/ath/ath9k/recv.c
@@ -387,7 +387,9 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 	if (sc->hw->conf.radar_enabled)
 		rfilt |= ATH9K_RX_FILTER_PHYRADAR | ATH9K_RX_FILTER_PHYERR;
 
-	if (sc->rx.rxfilter & FIF_PROBE_REQ)
+	spin_lock_bh(&sc->chan_lock);
+
+	if (sc->cur_chan->rxfilter & FIF_PROBE_REQ)
 		rfilt |= ATH9K_RX_FILTER_PROBEREQ;
 
 	/*
@@ -398,24 +400,25 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 	if (sc->sc_ah->is_monitoring)
 		rfilt |= ATH9K_RX_FILTER_PROM;
 
-	if (sc->rx.rxfilter & FIF_CONTROL)
+	if ((sc->cur_chan->rxfilter & FIF_CONTROL) ||
+	    sc->sc_ah->dynack.enabled)
 		rfilt |= ATH9K_RX_FILTER_CONTROL;
 
 	if ((sc->sc_ah->opmode == NL80211_IFTYPE_STATION) &&
-	    (sc->nvifs <= 1) &&
-	    !(sc->rx.rxfilter & FIF_BCN_PRBRESP_PROMISC))
+	    (sc->cur_chan->nvifs <= 1) &&
+	    !(sc->cur_chan->rxfilter & FIF_BCN_PRBRESP_PROMISC))
 		rfilt |= ATH9K_RX_FILTER_MYBEACON;
 	else
 		rfilt |= ATH9K_RX_FILTER_BEACON;
 
 	if ((sc->sc_ah->opmode == NL80211_IFTYPE_AP) ||
-	    (sc->rx.rxfilter & FIF_PSPOLL))
+	    (sc->cur_chan->rxfilter & FIF_PSPOLL))
 		rfilt |= ATH9K_RX_FILTER_PSPOLL;
 
-	if (conf_is_ht(&sc->hw->conf))
+	if (sc->cur_chandef.width != NL80211_CHAN_WIDTH_20_NOHT)
 		rfilt |= ATH9K_RX_FILTER_COMP_BAR;
 
-	if (sc->nvifs > 1 || (sc->rx.rxfilter & FIF_OTHER_BSS)) {
+	if (sc->cur_chan->nvifs > 1 || (sc->cur_chan->rxfilter & FIF_OTHER_BSS)) {
 		/* This is needed for older chips */
 		if (sc->sc_ah->hw_version.macVersion <= AR_SREV_VERSION_9160)
 			rfilt |= ATH9K_RX_FILTER_PROM;
@@ -425,22 +428,24 @@ u32 ath_calcrxfilter(struct ath_softc *sc)
 	if (AR_SREV_9550(sc->sc_ah) || AR_SREV_9531(sc->sc_ah))
 		rfilt |= ATH9K_RX_FILTER_4ADDRESS;
 
-	if (ath9k_use_chanctx &&
+	if (ath9k_is_chanctx_enabled() &&
 	    test_bit(ATH_OP_SCANNING, &common->op_flags))
 		rfilt |= ATH9K_RX_FILTER_BEACON;
 
+	spin_unlock_bh(&sc->chan_lock);
+
 	return rfilt;
 
 }
 
-int ath_startrecv(struct ath_softc *sc)
+void ath_startrecv(struct ath_softc *sc)
 {
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_rxbuf *bf, *tbf;
 
 	if (ah->caps.hw_caps & ATH9K_HW_CAP_EDMA) {
 		ath_edma_start_recv(sc);
-		return 0;
+		return;
 	}
 
 	if (list_empty(&sc->rx.rxbuf))
@@ -463,8 +468,6 @@ int ath_startrecv(struct ath_softc *sc)
 start_recv:
 	ath_opmode_init(sc);
 	ath9k_hw_startpcureceive(ah, sc->cur_chan->offchannel);
-
-	return 0;
 }
 
 static void ath_flushrecv(struct ath_softc *sc)
@@ -535,6 +538,7 @@ static bool ath_beacon_dtim_pending_cab(struct sk_buff *skb)
 static void ath_rx_ps_beacon(struct ath_softc *sc, struct sk_buff *skb)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	bool skip_beacon = false;
 
 	if (skb->len < 24 + 8 + 2 + 2)
 		return;
@@ -545,10 +549,19 @@ static void ath_rx_ps_beacon(struct ath_softc *sc, struct sk_buff *skb)
 		sc->ps_flags &= ~PS_BEACON_SYNC;
 		ath_dbg(common, PS,
 			"Reconfigure beacon timers based on synchronized timestamp\n");
-		if (!(WARN_ON_ONCE(sc->cur_chan->beacon.beacon_interval == 0)))
+
+#ifdef CONFIG_ATH9K_CHANNEL_CONTEXT
+		if (ath9k_is_chanctx_enabled()) {
+			if (sc->cur_chan == &sc->offchannel.chan)
+				skip_beacon = true;
+		}
+#endif
+
+		if (!skip_beacon &&
+		    !(WARN_ON_ONCE(sc->cur_chan->beacon.beacon_interval == 0)))
 			ath9k_set_beacon(sc);
-		if (sc->p2p_ps_vif)
-			ath9k_update_p2p_ps(sc, sc->p2p_ps_vif->vif);
+
+		ath9k_p2p_beacon_sync(sc);
 	}
 
 	if (ath_beacon_dtim_pending_cab(skb)) {
@@ -867,8 +880,13 @@ static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 	 * everything but the rate is checked here, the rate check is done
 	 * separately to avoid doing two lookups for a rate for each frame.
 	 */
-	if (!ath9k_cmn_rx_accept(common, hdr, rx_status, rx_stats, decrypt_error, sc->rx.rxfilter))
+	spin_lock_bh(&sc->chan_lock);
+	if (!ath9k_cmn_rx_accept(common, hdr, rx_status, rx_stats, decrypt_error,
+				 sc->cur_chan->rxfilter)) {
+		spin_unlock_bh(&sc->chan_lock);
 		return -EINVAL;
+	}
+	spin_unlock_bh(&sc->chan_lock);
 
 	if (ath_is_mybeacon(common, hdr)) {
 		RX_STAT_INC(rx_beacons);
@@ -892,9 +910,10 @@ static int ath9k_rx_skb_preprocess(struct ath_softc *sc,
 		return -EINVAL;
 	}
 
-	if (rx_stats->is_mybeacon) {
-		sc->sched.next_tbtt = rx_stats->rs_tstamp;
-		ath_chanctx_event(sc, NULL, ATH_CHANCTX_EVENT_BEACON_RECEIVED);
+	if (ath9k_is_chanctx_enabled()) {
+		if (rx_stats->is_mybeacon)
+			ath_chanctx_beacon_recv_ev(sc,
+					   ATH_CHANCTX_EVENT_BEACON_RECEIVED);
 	}
 
 	ath9k_cmn_process_rssi(common, hw, rx_stats, rx_status);
@@ -991,6 +1010,7 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 	unsigned long flags;
 	dma_addr_t new_buf_addr;
 	unsigned int budget = 512;
+	struct ieee80211_hdr *hdr;
 
 	if (edma)
 		dma_type = DMA_BIDIRECTIONAL;
@@ -1120,6 +1140,10 @@ int ath_rx_tasklet(struct ath_softc *sc, int flush, bool hp)
 		ath9k_apply_ampdu_details(sc, &rs, rxs);
 		ath_debug_rate_stats(sc, &rs, skb);
 
+		hdr = (struct ieee80211_hdr *)skb->data;
+		if (ieee80211_is_ack(hdr->frame_control))
+			ath_dynack_sample_ack_ts(sc->sc_ah, skb, rs.rs_tstamp);
+
 		ieee80211_rx(hw, skb);
 
 requeue_drop_frag:
diff --git a/drivers/net/wireless/ath/ath9k/reg.h b/drivers/net/wireless/ath/ath9k/reg.h
index a1499700bcf2..2a938f4feac5 100644
--- a/drivers/net/wireless/ath/ath9k/reg.h
+++ b/drivers/net/wireless/ath/ath9k/reg.h
@@ -903,6 +903,10 @@
 #define AR_SREV_9340(_ah) \
 	(((_ah)->hw_version.macVersion == AR_SREV_VERSION_9340))
 
+#define AR_SREV_9340_13(_ah) \
+	(AR_SREV_9340((_ah)) && \
+	 ((_ah)->hw_version.macRev == AR_SREV_REVISION_9340_13))
+
 #define AR_SREV_9340_13_OR_LATER(_ah) \
 	(AR_SREV_9340((_ah)) && \
 	 ((_ah)->hw_version.macRev >= AR_SREV_REVISION_9340_13))
@@ -1240,12 +1244,23 @@ enum {
 #define AR_CH0_DPLL3_PHASE_SHIFT_S   23
 #define AR_PHY_CCA_NOM_VAL_2GHZ      -118
 
+#define AR_RTC_9300_SOC_PLL_DIV_INT          0x0000003f
+#define AR_RTC_9300_SOC_PLL_DIV_INT_S        0
+#define AR_RTC_9300_SOC_PLL_DIV_FRAC         0x000fffc0
+#define AR_RTC_9300_SOC_PLL_DIV_FRAC_S       6
+#define AR_RTC_9300_SOC_PLL_REFDIV           0x01f00000
+#define AR_RTC_9300_SOC_PLL_REFDIV_S         20
+#define AR_RTC_9300_SOC_PLL_CLKSEL           0x06000000
+#define AR_RTC_9300_SOC_PLL_CLKSEL_S         25
+#define AR_RTC_9300_SOC_PLL_BYPASS           0x08000000
+
 #define AR_RTC_9300_PLL_DIV          0x000003ff
 #define AR_RTC_9300_PLL_DIV_S        0
 #define AR_RTC_9300_PLL_REFDIV       0x00003C00
 #define AR_RTC_9300_PLL_REFDIV_S     10
 #define AR_RTC_9300_PLL_CLKSEL       0x0000C000
 #define AR_RTC_9300_PLL_CLKSEL_S     14
+#define AR_RTC_9300_PLL_BYPASS       0x00010000
 
 #define AR_RTC_9160_PLL_DIV	0x000003ff
 #define AR_RTC_9160_PLL_DIV_S   0
diff --git a/drivers/net/wireless/ath/ath9k/spectral.c b/drivers/net/wireless/ath/ath9k/spectral.c
index 5fe29b9f8fa2..8f68426ca653 100644
--- a/drivers/net/wireless/ath/ath9k/spectral.c
+++ b/drivers/net/wireless/ath/ath9k/spectral.c
@@ -253,7 +253,7 @@ static ssize_t write_file_spec_scan_ctl(struct file *file,
 
 	if (strncmp("trigger", buf, 7) == 0) {
 		ath9k_spectral_scan_trigger(sc->hw);
-	} else if (strncmp("background", buf, 9) == 0) {
+	} else if (strncmp("background", buf, 10) == 0) {
 		ath9k_spectral_scan_config(sc->hw, SPECTRAL_BACKGROUND);
 		ath_dbg(common, CONFIG, "spectral scan: background mode enabled\n");
 	} else if (strncmp("chanscan", buf, 8) == 0) {
diff --git a/drivers/net/wireless/ath/ath9k/spectral.h b/drivers/net/wireless/ath/ath9k/spectral.h
index ead63412ee1a..7b410c6858b0 100644
--- a/drivers/net/wireless/ath/ath9k/spectral.h
+++ b/drivers/net/wireless/ath/ath9k/spectral.h
@@ -17,6 +17,8 @@
 #ifndef SPECTRAL_H
 #define SPECTRAL_H
 
+#include "../spectral_common.h"
+
 /* enum spectral_mode:
  *
  * @SPECTRAL_DISABLED: spectral mode is disabled
@@ -54,8 +56,6 @@ struct ath_ht20_mag_info {
 	u8 max_exp;
 } __packed;
 
-#define SPECTRAL_HT20_NUM_BINS		56
-
 /* WARNING: don't actually use this struct! MAC may vary the amount of
  * data by -1/+2. This struct is for reference only.
  */
@@ -83,8 +83,6 @@ struct ath_ht20_40_mag_info {
 	u8 max_exp;
 } __packed;
 
-#define SPECTRAL_HT20_40_NUM_BINS		128
-
 /* WARNING: don't actually use this struct! MAC may vary the amount of
  * data. This struct is for reference only.
  */
@@ -125,71 +123,6 @@ static inline u8 spectral_bitmap_weight(u8 *bins)
 	return bins[0] & 0x3f;
 }
 
-/* FFT sample format given to userspace via debugfs.
- *
- * Please keep the type/length at the front position and change
- * other fields after adding another sample type
- *
- * TODO: this might need rework when switching to nl80211-based
- * interface.
- */
-enum ath_fft_sample_type {
-	ATH_FFT_SAMPLE_HT20 = 1,
-	ATH_FFT_SAMPLE_HT20_40,
-};
-
-struct fft_sample_tlv {
-	u8 type;	/* see ath_fft_sample */
-	__be16 length;
-	/* type dependent data follows */
-} __packed;
-
-struct fft_sample_ht20 {
-	struct fft_sample_tlv tlv;
-
-	u8 max_exp;
-
-	__be16 freq;
-	s8 rssi;
-	s8 noise;
-
-	__be16 max_magnitude;
-	u8 max_index;
-	u8 bitmap_weight;
-
-	__be64 tsf;
-
-	u8 data[SPECTRAL_HT20_NUM_BINS];
-} __packed;
-
-struct fft_sample_ht20_40 {
-	struct fft_sample_tlv tlv;
-
-	u8 channel_type;
-	__be16 freq;
-
-	s8 lower_rssi;
-	s8 upper_rssi;
-
-	__be64 tsf;
-
-	s8 lower_noise;
-	s8 upper_noise;
-
-	__be16 lower_max_magnitude;
-	__be16 upper_max_magnitude;
-
-	u8 lower_max_index;
-	u8 upper_max_index;
-
-	u8 lower_bitmap_weight;
-	u8 upper_bitmap_weight;
-
-	u8 max_exp;
-
-	u8 data[SPECTRAL_HT20_40_NUM_BINS];
-} __packed;
-
 void ath9k_spectral_init_debug(struct ath_softc *sc);
 void ath9k_spectral_deinit_debug(struct ath_softc *sc);
 
diff --git a/drivers/net/wireless/ath/ath9k/tx99.c b/drivers/net/wireless/ath/ath9k/tx99.c
index 23972924c774..40ab65e6882f 100644
--- a/drivers/net/wireless/ath/ath9k/tx99.c
+++ b/drivers/net/wireless/ath/ath9k/tx99.c
@@ -54,6 +54,12 @@ static struct sk_buff *ath9k_build_tx99_skb(struct ath_softc *sc)
 	struct ieee80211_hdr *hdr;
 	struct ieee80211_tx_info *tx_info;
 	struct sk_buff *skb;
+	struct ath_vif *avp;
+
+	if (!sc->tx99_vif)
+		return NULL;
+
+	avp = (struct ath_vif *)sc->tx99_vif->drv_priv;
 
 	skb = alloc_skb(len, GFP_KERNEL);
 	if (!skb)
@@ -71,7 +77,7 @@ static struct sk_buff *ath9k_build_tx99_skb(struct ath_softc *sc)
 	memcpy(hdr->addr2, hw->wiphy->perm_addr, ETH_ALEN);
 	memcpy(hdr->addr3, hw->wiphy->perm_addr, ETH_ALEN);
 
-	hdr->seq_ctrl |= cpu_to_le16(sc->tx.seq_no);
+	hdr->seq_ctrl |= cpu_to_le16(avp->seq_no);
 
 	tx_info = IEEE80211_SKB_CB(skb);
 	memset(tx_info, 0, sizeof(*tx_info));
@@ -174,7 +180,7 @@ static ssize_t write_file_tx99(struct file *file, const char __user *user_buf,
 	ssize_t len;
 	int r;
 
-	if (sc->nvifs > 1)
+	if (sc->cur_chan->nvifs > 1)
 		return -EOPNOTSUPP;
 
 	len = min(count, sizeof(buf) - 1);
diff --git a/drivers/net/wireless/ath/ath9k/wow.c b/drivers/net/wireless/ath/ath9k/wow.c
index a4f4f0da81f6..5f30e580d942 100644
--- a/drivers/net/wireless/ath/ath9k/wow.c
+++ b/drivers/net/wireless/ath/ath9k/wow.c
@@ -193,7 +193,8 @@ int ath9k_suspend(struct ieee80211_hw *hw,
 	u32 wow_triggers_enabled = 0;
 	int ret = 0;
 
-	cancel_work_sync(&sc->chanctx_work);
+	ath9k_deinit_channel_context(sc);
+
 	mutex_lock(&sc->mutex);
 
 	ath_cancel_work(sc);
@@ -231,7 +232,7 @@ int ath9k_suspend(struct ieee80211_hw *hw,
 		goto fail_wow;
 	}
 
-	if (sc->nvifs > 1) {
+	if (sc->cur_chan->nvifs > 1) {
 		ath_dbg(common, WOW, "WoW for multivif is not yet supported\n");
 		ret = 1;
 		goto fail_wow;
diff --git a/drivers/net/wireless/ath/ath9k/xmit.c b/drivers/net/wireless/ath/ath9k/xmit.c
index 704fcbcbe20b..493a183d0aaf 100644
--- a/drivers/net/wireless/ath/ath9k/xmit.c
+++ b/drivers/net/wireless/ath/ath9k/xmit.c
@@ -158,7 +158,6 @@ static void ath_txq_skb_done(struct ath_softc *sc, struct ath_txq *txq,
 {
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ath_frame_info *fi = get_frame_info(skb);
-	int hw_queue;
 	int q = fi->txq;
 
 	if (q < 0)
@@ -168,10 +167,9 @@ static void ath_txq_skb_done(struct ath_softc *sc, struct ath_txq *txq,
 	if (WARN_ON(--txq->pending_frames < 0))
 		txq->pending_frames = 0;
 
-	hw_queue = (info->hw_queue >= sc->hw->queues - 2) ? q : info->hw_queue;
 	if (txq->stopped &&
 	    txq->pending_frames < sc->tx.txq_max_pending[q]) {
-		ieee80211_wake_queue(sc->hw, hw_queue);
+		ieee80211_wake_queue(sc->hw, info->hw_queue);
 		txq->stopped = false;
 	}
 }
@@ -587,6 +585,10 @@ static void ath_tx_complete_aggr(struct ath_softc *sc, struct ath_txq *txq,
 				memcpy(tx_info->control.rates, rates, sizeof(rates));
 				ath_tx_rc_status(sc, bf, ts, nframes, nbad, txok);
 				rc_update = false;
+				if (bf == bf->bf_lastbf)
+					ath_dynack_sample_tx_ts(sc->sc_ah,
+								bf->bf_mpdu,
+								ts);
 			}
 
 			ath_tx_complete_buf(sc, bf, txq, &bf_head, ts,
@@ -681,12 +683,15 @@ static void ath_tx_process_buffer(struct ath_softc *sc, struct ath_txq *txq,
 	if (bf_is_ampdu_not_probing(bf))
 		txq->axq_ampdu_depth--;
 
+	ts->duration = ath9k_hw_get_duration(sc->sc_ah, bf->bf_desc,
+					     ts->ts_rateindex);
 	if (!bf_isampdu(bf)) {
 		if (!flush) {
 			info = IEEE80211_SKB_CB(bf->bf_mpdu);
 			memcpy(info->control.rates, bf->rates,
 			       sizeof(info->control.rates));
 			ath_tx_rc_status(sc, bf, ts, 1, txok ? 0 : 1, txok);
+			ath_dynack_sample_tx_ts(sc->sc_ah, bf->bf_mpdu, ts);
 		}
 		ath_tx_complete_buf(sc, bf, txq, bf_head, ts, txok);
 	} else
@@ -1836,15 +1841,17 @@ void ath_txq_schedule(struct ath_softc *sc, struct ath_txq *txq)
 	if (txq->mac80211_qnum < 0)
 		return;
 
+	if (test_bit(ATH_OP_HW_RESET, &common->op_flags))
+		return;
+
 	spin_lock_bh(&sc->chan_lock);
 	ac_list = &sc->cur_chan->acq[txq->mac80211_qnum];
-	spin_unlock_bh(&sc->chan_lock);
 
-	if (test_bit(ATH_OP_HW_RESET, &common->op_flags) ||
-	    list_empty(ac_list))
+	if (list_empty(ac_list)) {
+		spin_unlock_bh(&sc->chan_lock);
 		return;
+	}
 
-	spin_lock_bh(&sc->chan_lock);
 	rcu_read_lock();
 
 	last_ac = list_entry(ac_list->prev, struct ath_atx_ac, list);
@@ -2132,6 +2139,28 @@ static struct ath_buf *ath_tx_setup_buffer(struct ath_softc *sc,
 	return bf;
 }
 
+void ath_assign_seq(struct ath_common *common, struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_vif *vif = info->control.vif;
+	struct ath_vif *avp;
+
+	if (!(info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ))
+		return;
+
+	if (!vif)
+		return;
+
+	avp = (struct ath_vif *)vif->drv_priv;
+
+	if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
+		avp->seq_no += 0x10;
+
+	hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
+	hdr->seq_ctrl |= cpu_to_le16(avp->seq_no);
+}
+
 static int ath_tx_prepare(struct ieee80211_hw *hw, struct sk_buff *skb,
 			  struct ath_tx_control *txctl)
 {
@@ -2155,17 +2184,7 @@ static int ath_tx_prepare(struct ieee80211_hw *hw, struct sk_buff *skb,
 	if (info->control.hw_key)
 		frmlen += info->control.hw_key->icv_len;
 
-	/*
-	 * As a temporary workaround, assign seq# here; this will likely need
-	 * to be cleaned up to work better with Beacon transmission and virtual
-	 * BSSes.
-	 */
-	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
-		if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
-			sc->tx.seq_no += 0x10;
-		hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
-		hdr->seq_ctrl |= cpu_to_le16(sc->tx.seq_no);
-	}
+	ath_assign_seq(ath9k_hw_common(sc->sc_ah), skb);
 
 	if ((vif && vif->type != NL80211_IFTYPE_AP &&
 	            vif->type != NL80211_IFTYPE_AP_VLAN) ||
@@ -2202,9 +2221,8 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 	struct ath_txq *txq = txctl->txq;
 	struct ath_atx_tid *tid = NULL;
 	struct ath_buf *bf;
-	bool queue;
-	int q, hw_queue;
-	int ret;
+	bool queue, skip_uapsd = false;
+	int q, ret;
 
 	if (vif)
 		avp = (void *)vif->drv_priv;
@@ -2223,14 +2241,13 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 	 */
 
 	q = skb_get_queue_mapping(skb);
-	hw_queue = (info->hw_queue >= sc->hw->queues - 2) ? q : info->hw_queue;
 
 	ath_txq_lock(sc, txq);
 	if (txq == sc->tx.txq_map[q]) {
 		fi->txq = q;
 		if (++txq->pending_frames > sc->tx.txq_max_pending[q] &&
 		    !txq->stopped) {
-			ieee80211_stop_queue(sc->hw, hw_queue);
+			ieee80211_stop_queue(sc->hw, info->hw_queue);
 			txq->stopped = true;
 		}
 	}
@@ -2245,15 +2262,14 @@ int ath_tx_start(struct ieee80211_hw *hw, struct sk_buff *skb,
 	     sc->cur_chan->stopped) && !txctl->force_channel) {
 		if (!txctl->an)
 			txctl->an = &avp->mcast_node;
-		info->flags &= ~IEEE80211_TX_CTL_PS_RESPONSE;
 		queue = true;
+		skip_uapsd = true;
 	}
 
 	if (txctl->an && queue)
 		tid = ath_get_skb_tid(sc, txctl->an, skb);
 
-	if (info->flags & (IEEE80211_TX_CTL_PS_RESPONSE |
-			   IEEE80211_TX_CTL_TX_OFFCHAN)) {
+	if (!skip_uapsd && (info->flags & IEEE80211_TX_CTL_PS_RESPONSE)) {
 		ath_txq_unlock(sc, txq);
 		txq = sc->tx.uapsdq;
 		ath_txq_lock(sc, txq);
@@ -2632,8 +2648,11 @@ void ath_tx_edma_tasklet(struct ath_softc *sc)
 			sc->beacon.tx_processed = true;
 			sc->beacon.tx_last = !(ts.ts_status & ATH9K_TXERR_MASK);
 
-			ath_chanctx_event(sc, NULL,
-					  ATH_CHANCTX_EVENT_BEACON_SENT);
+			if (ath9k_is_chanctx_enabled()) {
+				ath_chanctx_event(sc, NULL,
+						  ATH_CHANCTX_EVENT_BEACON_SENT);
+			}
+
 			ath9k_csa_update(sc);
 			continue;
 		}
diff --git a/drivers/net/wireless/ath/carl9170/main.c b/drivers/net/wireless/ath/carl9170/main.c
index f8ded84b7be8..ef5b6dc7b7f1 100644
--- a/drivers/net/wireless/ath/carl9170/main.c
+++ b/drivers/net/wireless/ath/carl9170/main.c
@@ -1430,18 +1430,10 @@ static int carl9170_op_ampdu_action(struct ieee80211_hw *hw,
 		if (!sta_info->ht_sta)
 			return -EOPNOTSUPP;
 
-		rcu_read_lock();
-		if (rcu_dereference(sta_info->agg[tid])) {
-			rcu_read_unlock();
-			return -EBUSY;
-		}
-
 		tid_info = kzalloc(sizeof(struct carl9170_sta_tid),
 				   GFP_ATOMIC);
-		if (!tid_info) {
-			rcu_read_unlock();
+		if (!tid_info)
 			return -ENOMEM;
-		}
 
 		tid_info->hsn = tid_info->bsn = tid_info->snx = (*ssn);
 		tid_info->state = CARL9170_TID_STATE_PROGRESS;
@@ -1460,7 +1452,6 @@ static int carl9170_op_ampdu_action(struct ieee80211_hw *hw,
 		list_add_tail_rcu(&tid_info->list, &ar->tx_ampdu_list);
 		rcu_assign_pointer(sta_info->agg[tid], tid_info);
 		spin_unlock_bh(&ar->tx_ampdu_list_lock);
-		rcu_read_unlock();
 
 		ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 		break;
diff --git a/drivers/net/wireless/ath/carl9170/tx.c b/drivers/net/wireless/ath/carl9170/tx.c
index 4cadfd48ffdf..ae86a600d920 100644
--- a/drivers/net/wireless/ath/carl9170/tx.c
+++ b/drivers/net/wireless/ath/carl9170/tx.c
@@ -1557,7 +1557,7 @@ static struct carl9170_vif_info *carl9170_pick_beaconing_vif(struct ar9170 *ar)
 	}
 
 out:
-	rcu_assign_pointer(ar->beacon_iter, cvif);
+	RCU_INIT_POINTER(ar->beacon_iter, cvif);
 	return cvif;
 }
 
diff --git a/drivers/net/wireless/ath/main.c b/drivers/net/wireless/ath/main.c
index 8b0ac14d5c32..338d72337604 100644
--- a/drivers/net/wireless/ath/main.c
+++ b/drivers/net/wireless/ath/main.c
@@ -20,6 +20,7 @@
 #include <linux/module.h>
 
 #include "ath.h"
+#include "trace.h"
 
 MODULE_AUTHOR("Atheros Communications");
 MODULE_DESCRIPTION("Shared library for Atheros wireless LAN cards.");
@@ -78,11 +79,13 @@ void ath_printk(const char *level, const struct ath_common* common,
 	vaf.fmt = fmt;
 	vaf.va = &args;
 
-	if (common && common->hw && common->hw->wiphy)
+	if (common && common->hw && common->hw->wiphy) {
 		printk("%sath: %s: %pV",
 		       level, wiphy_name(common->hw->wiphy), &vaf);
-	else
+		trace_ath_log(common->hw->wiphy, &vaf);
+	} else {
 		printk("%sath: %pV", level, &vaf);
+	}
 
 	va_end(args);
 }
diff --git a/drivers/net/wireless/ath/spectral_common.h b/drivers/net/wireless/ath/spectral_common.h
new file mode 100644
index 000000000000..0d742acb1599
--- /dev/null
+++ b/drivers/net/wireless/ath/spectral_common.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2013 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef SPECTRAL_COMMON_H
+#define SPECTRAL_COMMON_H
+
+#define SPECTRAL_HT20_NUM_BINS		56
+#define SPECTRAL_HT20_40_NUM_BINS		128
+
+/* TODO: could possibly be 512, but no samples this large
+ * could be acquired so far.
+ */
+#define SPECTRAL_ATH10K_MAX_NUM_BINS		256
+
+/* FFT sample format given to userspace via debugfs.
+ *
+ * Please keep the type/length at the front position and change
+ * other fields after adding another sample type
+ *
+ * TODO: this might need rework when switching to nl80211-based
+ * interface.
+ */
+enum ath_fft_sample_type {
+	ATH_FFT_SAMPLE_HT20 = 1,
+	ATH_FFT_SAMPLE_HT20_40,
+	ATH_FFT_SAMPLE_ATH10K,
+};
+
+struct fft_sample_tlv {
+	u8 type;	/* see ath_fft_sample */
+	__be16 length;
+	/* type dependent data follows */
+} __packed;
+
+struct fft_sample_ht20 {
+	struct fft_sample_tlv tlv;
+
+	u8 max_exp;
+
+	__be16 freq;
+	s8 rssi;
+	s8 noise;
+
+	__be16 max_magnitude;
+	u8 max_index;
+	u8 bitmap_weight;
+
+	__be64 tsf;
+
+	u8 data[SPECTRAL_HT20_NUM_BINS];
+} __packed;
+
+struct fft_sample_ht20_40 {
+	struct fft_sample_tlv tlv;
+
+	u8 channel_type;
+	__be16 freq;
+
+	s8 lower_rssi;
+	s8 upper_rssi;
+
+	__be64 tsf;
+
+	s8 lower_noise;
+	s8 upper_noise;
+
+	__be16 lower_max_magnitude;
+	__be16 upper_max_magnitude;
+
+	u8 lower_max_index;
+	u8 upper_max_index;
+
+	u8 lower_bitmap_weight;
+	u8 upper_bitmap_weight;
+
+	u8 max_exp;
+
+	u8 data[SPECTRAL_HT20_40_NUM_BINS];
+} __packed;
+
+struct fft_sample_ath10k {
+	struct fft_sample_tlv tlv;
+	u8 chan_width_mhz;
+	__be16 freq1;
+	__be16 freq2;
+	__be16 noise;
+	__be16 max_magnitude;
+	__be16 total_gain_db;
+	__be16 base_pwr_db;
+	__be64 tsf;
+	s8 max_index;
+	u8 rssi;
+	u8 relpwr_db;
+	u8 avgpwr_db;
+	u8 max_exp;
+
+	u8 data[0];
+} __packed;
+
+#endif /* SPECTRAL_COMMON_H */
diff --git a/drivers/net/wireless/ath/trace.c b/drivers/net/wireless/ath/trace.c
new file mode 100644
index 000000000000..18fb3a071931
--- /dev/null
+++ b/drivers/net/wireless/ath/trace.c
@@ -0,0 +1,20 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/module.h>
+
+#define CREATE_TRACE_POINTS
+#include "trace.h"
diff --git a/drivers/net/wireless/ath/trace.h b/drivers/net/wireless/ath/trace.h
new file mode 100644
index 000000000000..ba711644d27e
--- /dev/null
+++ b/drivers/net/wireless/ath/trace.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#if !defined(_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+#define _TRACE_H
+
+#include <linux/tracepoint.h>
+#include "ath.h"
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM ath
+
+#if !defined(CONFIG_ATH_TRACEPOINTS)
+
+#undef TRACE_EVENT
+#define TRACE_EVENT(name, proto, ...) static inline void trace_ ## name(proto) {}
+
+#endif /* CONFIG_ATH_TRACEPOINTS */
+
+TRACE_EVENT(ath_log,
+
+	    TP_PROTO(struct wiphy *wiphy,
+		     struct va_format *vaf),
+
+	    TP_ARGS(wiphy, vaf),
+
+	    TP_STRUCT__entry(
+		    __string(device, wiphy_name(wiphy))
+		    __string(driver, KBUILD_MODNAME)
+		    __dynamic_array(char, msg, ATH_DBG_MAX_LEN)
+	    ),
+
+	    TP_fast_assign(
+		    __assign_str(device, wiphy_name(wiphy));
+		    __assign_str(driver, KBUILD_MODNAME);
+		    WARN_ON_ONCE(vsnprintf(__get_dynamic_array(msg),
+					   ATH_DBG_MAX_LEN,
+					   vaf->fmt,
+					   *vaf->va) >= ATH_DBG_MAX_LEN);
+	    ),
+
+	    TP_printk(
+		    "%s %s %s",
+		    __get_str(driver),
+		    __get_str(device),
+		    __get_str(msg)
+	    )
+);
+
+#endif /* _TRACE_H || TRACE_HEADER_MULTI_READ */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+/* This part must be outside protection */
+#include <trace/define_trace.h>
diff --git a/drivers/net/wireless/ath/wil6210/Kconfig b/drivers/net/wireless/ath/wil6210/Kconfig
index ce8c0381825e..481680a3aa55 100644
--- a/drivers/net/wireless/ath/wil6210/Kconfig
+++ b/drivers/net/wireless/ath/wil6210/Kconfig
@@ -39,3 +39,12 @@ config WIL6210_TRACING
 	  option if you are interested in debugging the driver.
 
 	  If unsure, say Y to make it easier to debug problems.
+
+config WIL6210_PLATFORM_MSM
+	bool "wil6210 MSM platform specific support"
+	depends on WIL6210
+	depends on ARCH_MSM
+	default y
+	---help---
+	  Say Y here to enable wil6210 driver support for MSM
+	  platform specific features
diff --git a/drivers/net/wireless/ath/wil6210/Makefile b/drivers/net/wireless/ath/wil6210/Makefile
index c7a3465fd02a..8ad4b5f97e04 100644
--- a/drivers/net/wireless/ath/wil6210/Makefile
+++ b/drivers/net/wireless/ath/wil6210/Makefile
@@ -10,7 +10,12 @@ wil6210-y += interrupt.o
 wil6210-y += txrx.o
 wil6210-y += debug.o
 wil6210-y += rx_reorder.o
+wil6210-y += ioctl.o
+wil6210-y += fw.o
 wil6210-$(CONFIG_WIL6210_TRACING) += trace.o
+wil6210-y += wil_platform.o
+wil6210-$(CONFIG_WIL6210_PLATFORM_MSM) += wil_platform_msm.o
+wil6210-y += ethtool.o
 
 # for tracing framework to find trace.h
 CFLAGS_trace.o := -I$(src)
diff --git a/drivers/net/wireless/ath/wil6210/cfg80211.c b/drivers/net/wireless/ath/wil6210/cfg80211.c
index 4ac2c208c9ba..d9f4b30dd343 100644
--- a/drivers/net/wireless/ath/wil6210/cfg80211.c
+++ b/drivers/net/wireless/ath/wil6210/cfg80211.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -296,6 +296,7 @@ static int wil_cfg80211_scan(struct wiphy *wiphy,
 	n = min(request->n_channels, 4U);
 	for (i = 0; i < n; i++) {
 		int ch = request->channels[i]->hw_value;
+
 		if (ch == 0) {
 			wil_err(wil,
 				"Scan requested for unknown frequency %dMhz\n",
@@ -308,15 +309,47 @@ static int wil_cfg80211_scan(struct wiphy *wiphy,
 			     request->channels[i]->center_freq);
 	}
 
+	if (request->ie_len)
+		print_hex_dump_bytes("Scan IE ", DUMP_PREFIX_OFFSET,
+				     request->ie, request->ie_len);
+	else
+		wil_dbg_misc(wil, "Scan has no IE's\n");
+
+	rc = wmi_set_ie(wil, WMI_FRAME_PROBE_REQ, request->ie_len,
+			request->ie);
+	if (rc) {
+		wil_err(wil, "Aborting scan, set_ie failed: %d\n", rc);
+		goto out;
+	}
+
 	rc = wmi_send(wil, WMI_START_SCAN_CMDID, &cmd, sizeof(cmd.cmd) +
 			cmd.cmd.num_channels * sizeof(cmd.cmd.channel_list[0]));
 
-	if (rc)
+out:
+	if (rc) {
+		del_timer_sync(&wil->scan_timer);
 		wil->scan_request = NULL;
+	}
 
 	return rc;
 }
 
+static void wil_print_connect_params(struct wil6210_priv *wil,
+				     struct cfg80211_connect_params *sme)
+{
+	wil_info(wil, "Connecting to:\n");
+	if (sme->channel) {
+		wil_info(wil, "  Channel: %d freq %d\n",
+			 sme->channel->hw_value, sme->channel->center_freq);
+	}
+	if (sme->bssid)
+		wil_info(wil, "  BSSID: %pM\n", sme->bssid);
+	if (sme->ssid)
+		print_hex_dump(KERN_INFO, "  SSID: ", DUMP_PREFIX_OFFSET,
+			       16, 1, sme->ssid, sme->ssid_len, true);
+	wil_info(wil, "  Privacy: %s\n", sme->privacy ? "secure" : "open");
+}
+
 static int wil_cfg80211_connect(struct wiphy *wiphy,
 				struct net_device *ndev,
 				struct cfg80211_connect_params *sme)
@@ -333,6 +366,8 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 	    test_bit(wil_status_fwconnected, &wil->status))
 		return -EALREADY;
 
+	wil_print_connect_params(wil, sme);
+
 	bss = cfg80211_get_bss(wiphy, sme->channel, sme->bssid,
 			       sme->ssid, sme->ssid_len,
 			       WLAN_CAPABILITY_ESS, WLAN_CAPABILITY_ESS);
@@ -358,22 +393,22 @@ static int wil_cfg80211_connect(struct wiphy *wiphy,
 				sme->ie_len);
 			goto out;
 		}
-		/*
-		 * For secure assoc, send:
-		 * (1) WMI_DELETE_CIPHER_KEY_CMD
-		 * (2) WMI_SET_APPIE_CMD
-		 */
+		/* For secure assoc, send WMI_DELETE_CIPHER_KEY_CMD */
 		rc = wmi_del_cipher_key(wil, 0, bss->bssid);
 		if (rc) {
 			wil_err(wil, "WMI_DELETE_CIPHER_KEY_CMD failed\n");
 			goto out;
 		}
-		/* WMI_SET_APPIE_CMD */
-		rc = wmi_set_ie(wil, WMI_FRAME_ASSOC_REQ, sme->ie_len, sme->ie);
-		if (rc) {
-			wil_err(wil, "WMI_SET_APPIE_CMD failed\n");
-			goto out;
-		}
+	}
+
+	/* WMI_SET_APPIE_CMD. ie may contain rsn info as well as other info
+	 * elements. Send it also in case it's empty, to erase previously set
+	 * ies in FW.
+	 */
+	rc = wmi_set_ie(wil, WMI_FRAME_ASSOC_REQ, sme->ie_len, sme->ie);
+	if (rc) {
+		wil_err(wil, "WMI_SET_APPIE_CMD failed\n");
+		goto out;
 	}
 
 	/* WMI_CONNECT_CMD */
@@ -619,6 +654,45 @@ static int wil_fix_bcon(struct wil6210_priv *wil,
 	return rc;
 }
 
+static int wil_cfg80211_change_beacon(struct wiphy *wiphy,
+				      struct net_device *ndev,
+				      struct cfg80211_beacon_data *bcon)
+{
+	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
+	int rc;
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	if (wil_fix_bcon(wil, bcon)) {
+		wil_dbg_misc(wil, "Fixed bcon\n");
+		wil_print_bcon_data(bcon);
+	}
+
+	/* FW do not form regular beacon, so bcon IE's are not set
+	 * For the DMG bcon, when it will be supported, bcon IE's will
+	 * be reused; add something like:
+	 * wmi_set_ie(wil, WMI_FRAME_BEACON, bcon->beacon_ies_len,
+	 * bcon->beacon_ies);
+	 */
+	rc = wmi_set_ie(wil, WMI_FRAME_PROBE_RESP,
+			bcon->proberesp_ies_len,
+			bcon->proberesp_ies);
+	if (rc) {
+		wil_err(wil, "set_ie(PROBE_RESP) failed\n");
+		return rc;
+	}
+
+	rc = wmi_set_ie(wil, WMI_FRAME_ASSOC_RESP,
+			bcon->assocresp_ies_len,
+			bcon->assocresp_ies);
+	if (rc) {
+		wil_err(wil, "set_ie(ASSOC_RESP) failed\n");
+		return rc;
+	}
+
+	return 0;
+}
+
 static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 				 struct net_device *ndev,
 				 struct cfg80211_ap_settings *info)
@@ -654,14 +728,12 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 		wil_print_bcon_data(bcon);
 	}
 
-	mutex_lock(&wil->mutex);
+	wil_set_recovery_state(wil, fw_recovery_idle);
 
-	rc = wil_reset(wil);
-	if (rc)
-		goto out;
+	mutex_lock(&wil->mutex);
 
-	/* Rx VRING. */
-	rc = wil_rx_init(wil);
+	__wil_down(wil);
+	rc = __wil_up(wil);
 	if (rc)
 		goto out;
 
@@ -669,9 +741,6 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 	if (rc)
 		goto out;
 
-	/* MAC address - pre-requisite for other commands */
-	wmi_set_mac_address(wil, ndev->dev_addr);
-
 	/* IE's */
 	/* bcon 'head IE's are not relevant for 60g band */
 	/*
@@ -693,7 +762,6 @@ static int wil_cfg80211_start_ap(struct wiphy *wiphy,
 	if (rc)
 		goto out;
 
-
 	netif_carrier_on(ndev);
 
 out:
@@ -704,17 +772,23 @@ out:
 static int wil_cfg80211_stop_ap(struct wiphy *wiphy,
 				struct net_device *ndev)
 {
-	int rc = 0;
+	int rc, rc1;
 	struct wil6210_priv *wil = wiphy_to_wil(wiphy);
 
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
+	wil_set_recovery_state(wil, fw_recovery_idle);
+
 	mutex_lock(&wil->mutex);
 
 	rc = wmi_pcp_stop(wil);
 
+	__wil_down(wil);
+	rc1 = __wil_up(wil);
+
 	mutex_unlock(&wil->mutex);
-	return rc;
+
+	return min(rc, rc1);
 }
 
 static int wil_cfg80211_del_station(struct wiphy *wiphy,
@@ -744,6 +818,7 @@ static struct cfg80211_ops wil_cfg80211_ops = {
 	.del_key = wil_cfg80211_del_key,
 	.set_default_key = wil_cfg80211_set_default_key,
 	/* AP mode */
+	.change_beacon = wil_cfg80211_change_beacon,
 	.start_ap = wil_cfg80211_start_ap,
 	.stop_ap = wil_cfg80211_stop_ap,
 	.del_station = wil_cfg80211_del_station,
@@ -753,6 +828,7 @@ static void wil_wiphy_init(struct wiphy *wiphy)
 {
 	/* TODO: set real value */
 	wiphy->max_scan_ssids = 10;
+	wiphy->max_scan_ie_len = WMI_MAX_IE_LEN;
 	wiphy->max_num_pmkids = 0 /* TODO: */;
 	wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
 				 BIT(NL80211_IFTYPE_AP) |
@@ -762,8 +838,8 @@ static void wil_wiphy_init(struct wiphy *wiphy)
 	 */
 	wiphy->flags |= WIPHY_FLAG_HAVE_AP_SME |
 			WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD;
-	dev_warn(wiphy_dev(wiphy), "%s : flags = 0x%08x\n",
-		 __func__, wiphy->flags);
+	dev_dbg(wiphy_dev(wiphy), "%s : flags = 0x%08x\n",
+		__func__, wiphy->flags);
 	wiphy->probe_resp_offload =
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS |
 		NL80211_PROBE_RESP_OFFLOAD_SUPPORT_WPS2 |
@@ -784,7 +860,9 @@ struct wireless_dev *wil_cfg80211_init(struct device *dev)
 	int rc = 0;
 	struct wireless_dev *wdev;
 
-	wdev = kzalloc(sizeof(struct wireless_dev), GFP_KERNEL);
+	dev_dbg(dev, "%s()\n", __func__);
+
+	wdev = kzalloc(sizeof(*wdev), GFP_KERNEL);
 	if (!wdev)
 		return ERR_PTR(-ENOMEM);
 
@@ -816,6 +894,8 @@ void wil_wdev_free(struct wil6210_priv *wil)
 {
 	struct wireless_dev *wdev = wil_to_wdev(wil);
 
+	dev_dbg(wil_to_dev(wil), "%s()\n", __func__);
+
 	if (!wdev)
 		return;
 
diff --git a/drivers/net/wireless/ath/wil6210/debug.c b/drivers/net/wireless/ath/wil6210/debug.c
index 9eeabf4a5879..8d99021d27a8 100644
--- a/drivers/net/wireless/ath/wil6210/debug.c
+++ b/drivers/net/wireless/ath/wil6210/debug.c
@@ -17,43 +17,37 @@
 #include "wil6210.h"
 #include "trace.h"
 
-int wil_err(struct wil6210_priv *wil, const char *fmt, ...)
+void wil_err(struct wil6210_priv *wil, const char *fmt, ...)
 {
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct va_format vaf = {
 		.fmt = fmt,
 	};
 	va_list args;
-	int ret;
 
 	va_start(args, fmt);
 	vaf.va = &args;
-	ret = netdev_err(ndev, "%pV", &vaf);
+	netdev_err(ndev, "%pV", &vaf);
 	trace_wil6210_log_err(&vaf);
 	va_end(args);
-
-	return ret;
 }
 
-int wil_info(struct wil6210_priv *wil, const char *fmt, ...)
+void wil_info(struct wil6210_priv *wil, const char *fmt, ...)
 {
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct va_format vaf = {
 		.fmt = fmt,
 	};
 	va_list args;
-	int ret;
 
 	va_start(args, fmt);
 	vaf.va = &args;
-	ret = netdev_info(ndev, "%pV", &vaf);
+	netdev_info(ndev, "%pV", &vaf);
 	trace_wil6210_log_info(&vaf);
 	va_end(args);
-
-	return ret;
 }
 
-int wil_dbg_trace(struct wil6210_priv *wil, const char *fmt, ...)
+void wil_dbg_trace(struct wil6210_priv *wil, const char *fmt, ...)
 {
 	struct va_format vaf = {
 		.fmt = fmt,
@@ -64,6 +58,4 @@ int wil_dbg_trace(struct wil6210_priv *wil, const char *fmt, ...)
 	vaf.va = &args;
 	trace_wil6210_log_dbg(&vaf);
 	va_end(args);
-
-	return 0;
 }
diff --git a/drivers/net/wireless/ath/wil6210/debugfs.c b/drivers/net/wireless/ath/wil6210/debugfs.c
index 8f66186adb8c..54a6ddc6301b 100644
--- a/drivers/net/wireless/ath/wil6210/debugfs.c
+++ b/drivers/net/wireless/ath/wil6210/debugfs.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -22,6 +22,7 @@
 #include <linux/power_supply.h>
 
 #include "wil6210.h"
+#include "wmi.h"
 #include "txrx.h"
 
 /* Nasty hack. Better have per device instances */
@@ -29,6 +30,21 @@ static u32 mem_addr;
 static u32 dbg_txdesc_index;
 static u32 dbg_vring_index; /* 24+ for Rx, 0..23 for Tx */
 
+enum dbg_off_type {
+	doff_u32 = 0,
+	doff_x32 = 1,
+	doff_ulong = 2,
+	doff_io32 = 3,
+};
+
+/* offset to "wil" */
+struct dbg_off {
+	const char *name;
+	umode_t mode;
+	ulong off;
+	enum dbg_off_type type;
+};
+
 static void wil_print_vring(struct seq_file *s, struct wil6210_priv *wil,
 			    const char *name, struct vring *vring,
 			    char _s, char _h)
@@ -45,20 +61,22 @@ static void wil_print_vring(struct seq_file *s, struct wil6210_priv *wil,
 	if (x)
 		seq_printf(s, "0x%08x\n", ioread32(x));
 	else
-		seq_printf(s, "???\n");
+		seq_puts(s, "???\n");
 
 	if (vring->va && (vring->size < 1025)) {
 		uint i;
+
 		for (i = 0; i < vring->size; i++) {
 			volatile struct vring_tx_desc *d = &vring->va[i].tx;
+
 			if ((i % 64) == 0 && (i != 0))
-				seq_printf(s, "\n");
+				seq_puts(s, "\n");
 			seq_printf(s, "%c", (d->dma.status & BIT(0)) ?
 					_s : (vring->ctx[i].skb ? _h : 'h'));
 		}
-		seq_printf(s, "\n");
+		seq_puts(s, "\n");
 	}
-	seq_printf(s, "}\n");
+	seq_puts(s, "}\n");
 }
 
 static int wil_vring_debugfs_show(struct seq_file *s, void *data)
@@ -69,7 +87,7 @@ static int wil_vring_debugfs_show(struct seq_file *s, void *data)
 	wil_print_vring(s, wil, "rx", &wil->vring_rx, 'S', '_');
 
 	for (i = 0; i < ARRAY_SIZE(wil->vring_tx); i++) {
-		struct vring *vring = &(wil->vring_tx[i]);
+		struct vring *vring = &wil->vring_tx[i];
 		struct vring_tx_data *txdata = &wil->vring_tx_data[i];
 
 		if (vring->va) {
@@ -147,7 +165,7 @@ static void wil_print_ring(struct seq_file *s, const char *prefix,
 	if (!wmi_addr(wil, r.base) ||
 	    !wmi_addr(wil, r.tail) ||
 	    !wmi_addr(wil, r.head)) {
-		seq_printf(s, "  ??? pointers are garbage?\n");
+		seq_puts(s, "  ??? pointers are garbage?\n");
 		goto out;
 	}
 
@@ -166,6 +184,7 @@ static void wil_print_ring(struct seq_file *s, const char *prefix,
 			   le32_to_cpu(d.addr));
 		if (0 == wmi_read_hdr(wil, d.addr, &hdr)) {
 			u16 len = le16_to_cpu(hdr.len);
+
 			seq_printf(s, " -> %04x %04x %04x %02x\n",
 				   le16_to_cpu(hdr.seq), len,
 				   le16_to_cpu(hdr.type), hdr.flags);
@@ -183,6 +202,7 @@ static void wil_print_ring(struct seq_file *s, const char *prefix,
 				wil_memcpy_fromio_32(databuf, src, len);
 				while (n < len) {
 					int l = min(len - n, 16);
+
 					hex_dump_to_buffer(databuf + n, l,
 							   16, 1, printbuf,
 							   sizeof(printbuf),
@@ -192,11 +212,11 @@ static void wil_print_ring(struct seq_file *s, const char *prefix,
 				}
 			}
 		} else {
-			seq_printf(s, "\n");
+			seq_puts(s, "\n");
 		}
 	}
  out:
-	seq_printf(s, "}\n");
+	seq_puts(s, "}\n");
 }
 
 static int wil_mbox_debugfs_show(struct seq_file *s, void *data)
@@ -244,9 +264,9 @@ DEFINE_SIMPLE_ATTRIBUTE(fops_iomem_x32, wil_debugfs_iomem_x32_get,
 static struct dentry *wil_debugfs_create_iomem_x32(const char *name,
 						   umode_t mode,
 						   struct dentry *parent,
-						   void __iomem *value)
+						   void *value)
 {
-	return debugfs_create_file(name, mode, parent, (void * __force)value,
+	return debugfs_create_file(name, mode, parent, value,
 				   &fops_iomem_x32);
 }
 
@@ -255,11 +275,13 @@ static int wil_debugfs_ulong_set(void *data, u64 val)
 	*(ulong *)data = val;
 	return 0;
 }
+
 static int wil_debugfs_ulong_get(void *data, u64 *val)
 {
 	*val = *(ulong *)data;
 	return 0;
 }
+
 DEFINE_SIMPLE_ATTRIBUTE(wil_fops_ulong, wil_debugfs_ulong_get,
 			wil_debugfs_ulong_set, "%llu\n");
 
@@ -270,6 +292,62 @@ static struct dentry *wil_debugfs_create_ulong(const char *name, umode_t mode,
 	return debugfs_create_file(name, mode, parent, value, &wil_fops_ulong);
 }
 
+/**
+ * wil6210_debugfs_init_offset - create set of debugfs files
+ * @wil - driver's context, used for printing
+ * @dbg - directory on the debugfs, where files will be created
+ * @base - base address used in address calculation
+ * @tbl - table with file descriptions. Should be terminated with empty element.
+ *
+ * Creates files accordingly to the @tbl.
+ */
+static void wil6210_debugfs_init_offset(struct wil6210_priv *wil,
+					struct dentry *dbg, void *base,
+					const struct dbg_off * const tbl)
+{
+	int i;
+
+	for (i = 0; tbl[i].name; i++) {
+		struct dentry *f;
+
+		switch (tbl[i].type) {
+		case doff_u32:
+			f = debugfs_create_u32(tbl[i].name, tbl[i].mode, dbg,
+					       base + tbl[i].off);
+			break;
+		case doff_x32:
+			f = debugfs_create_x32(tbl[i].name, tbl[i].mode, dbg,
+					       base + tbl[i].off);
+			break;
+		case doff_ulong:
+			f = wil_debugfs_create_ulong(tbl[i].name, tbl[i].mode,
+						     dbg, base + tbl[i].off);
+			break;
+		case doff_io32:
+			f = wil_debugfs_create_iomem_x32(tbl[i].name,
+							 tbl[i].mode, dbg,
+							 base + tbl[i].off);
+			break;
+		default:
+			f = ERR_PTR(-EINVAL);
+		}
+		if (IS_ERR_OR_NULL(f))
+			wil_err(wil, "Create file \"%s\": err %ld\n",
+				tbl[i].name, PTR_ERR(f));
+	}
+}
+
+static const struct dbg_off isr_off[] = {
+	{"ICC", S_IRUGO | S_IWUSR, offsetof(struct RGF_ICR, ICC), doff_io32},
+	{"ICR", S_IRUGO | S_IWUSR, offsetof(struct RGF_ICR, ICR), doff_io32},
+	{"ICM", S_IRUGO | S_IWUSR, offsetof(struct RGF_ICR, ICM), doff_io32},
+	{"ICS",		  S_IWUSR, offsetof(struct RGF_ICR, ICS), doff_io32},
+	{"IMV", S_IRUGO | S_IWUSR, offsetof(struct RGF_ICR, IMV), doff_io32},
+	{"IMS",		  S_IWUSR, offsetof(struct RGF_ICR, IMS), doff_io32},
+	{"IMC",		  S_IWUSR, offsetof(struct RGF_ICR, IMC), doff_io32},
+	{},
+};
+
 static int wil6210_debugfs_create_ISR(struct wil6210_priv *wil,
 				      const char *name,
 				      struct dentry *parent, u32 off)
@@ -279,24 +357,19 @@ static int wil6210_debugfs_create_ISR(struct wil6210_priv *wil,
 	if (IS_ERR_OR_NULL(d))
 		return -ENODEV;
 
-	wil_debugfs_create_iomem_x32("ICC", S_IRUGO | S_IWUSR, d,
-				     wil->csr + off);
-	wil_debugfs_create_iomem_x32("ICR", S_IRUGO | S_IWUSR, d,
-				     wil->csr + off + 4);
-	wil_debugfs_create_iomem_x32("ICM", S_IRUGO | S_IWUSR, d,
-				     wil->csr + off + 8);
-	wil_debugfs_create_iomem_x32("ICS", S_IWUSR, d,
-				     wil->csr + off + 12);
-	wil_debugfs_create_iomem_x32("IMV", S_IRUGO | S_IWUSR, d,
-				     wil->csr + off + 16);
-	wil_debugfs_create_iomem_x32("IMS", S_IWUSR, d,
-				     wil->csr + off + 20);
-	wil_debugfs_create_iomem_x32("IMC", S_IWUSR, d,
-				     wil->csr + off + 24);
+	wil6210_debugfs_init_offset(wil, d, (void * __force)wil->csr + off,
+				    isr_off);
 
 	return 0;
 }
 
+static const struct dbg_off pseudo_isr_off[] = {
+	{"CAUSE",   S_IRUGO, HOSTADDR(RGF_DMA_PSEUDO_CAUSE), doff_io32},
+	{"MASK_SW", S_IRUGO, HOSTADDR(RGF_DMA_PSEUDO_CAUSE_MASK_SW), doff_io32},
+	{"MASK_FW", S_IRUGO, HOSTADDR(RGF_DMA_PSEUDO_CAUSE_MASK_FW), doff_io32},
+	{},
+};
+
 static int wil6210_debugfs_create_pseudo_ISR(struct wil6210_priv *wil,
 					     struct dentry *parent)
 {
@@ -305,16 +378,19 @@ static int wil6210_debugfs_create_pseudo_ISR(struct wil6210_priv *wil,
 	if (IS_ERR_OR_NULL(d))
 		return -ENODEV;
 
-	wil_debugfs_create_iomem_x32("CAUSE", S_IRUGO, d, wil->csr +
-				     HOSTADDR(RGF_DMA_PSEUDO_CAUSE));
-	wil_debugfs_create_iomem_x32("MASK_SW", S_IRUGO, d, wil->csr +
-				     HOSTADDR(RGF_DMA_PSEUDO_CAUSE_MASK_SW));
-	wil_debugfs_create_iomem_x32("MASK_FW", S_IRUGO, d, wil->csr +
-				     HOSTADDR(RGF_DMA_PSEUDO_CAUSE_MASK_FW));
+	wil6210_debugfs_init_offset(wil, d, (void * __force)wil->csr,
+				    pseudo_isr_off);
 
 	return 0;
 }
 
+static const struct dbg_off itr_cnt_off[] = {
+	{"TRSH", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_CNT_TRSH), doff_io32},
+	{"DATA", S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_CNT_DATA), doff_io32},
+	{"CTL",  S_IRUGO | S_IWUSR, HOSTADDR(RGF_DMA_ITR_CNT_CRL), doff_io32},
+	{},
+};
+
 static int wil6210_debugfs_create_ITR_CNT(struct wil6210_priv *wil,
 					  struct dentry *parent)
 {
@@ -323,12 +399,8 @@ static int wil6210_debugfs_create_ITR_CNT(struct wil6210_priv *wil,
 	if (IS_ERR_OR_NULL(d))
 		return -ENODEV;
 
-	wil_debugfs_create_iomem_x32("TRSH", S_IRUGO | S_IWUSR, d, wil->csr +
-				     HOSTADDR(RGF_DMA_ITR_CNT_TRSH));
-	wil_debugfs_create_iomem_x32("DATA", S_IRUGO | S_IWUSR, d, wil->csr +
-				     HOSTADDR(RGF_DMA_ITR_CNT_DATA));
-	wil_debugfs_create_iomem_x32("CTL", S_IRUGO | S_IWUSR, d, wil->csr +
-				     HOSTADDR(RGF_DMA_ITR_CNT_CRL));
+	wil6210_debugfs_init_offset(wil, d, (void * __force)wil->csr,
+				    itr_cnt_off);
 
 	return 0;
 }
@@ -359,7 +431,7 @@ static const struct file_operations fops_memread = {
 };
 
 static ssize_t wil_read_file_ioblob(struct file *file, char __user *user_buf,
-				size_t count, loff_t *ppos)
+				    size_t count, loff_t *ppos)
 {
 	enum { max_count = 4096 };
 	struct debugfs_blob_wrapper *blob = file->private_data;
@@ -411,6 +483,7 @@ struct dentry *wil_debugfs_create_ioblob(const char *name,
 {
 	return debugfs_create_file(name, mode, parent, blob, &fops_ioblob);
 }
+
 /*---reset---*/
 static ssize_t wil_write_file_reset(struct file *file, const char __user *buf,
 				    size_t len, loff_t *ppos)
@@ -436,6 +509,7 @@ static const struct file_operations fops_reset = {
 	.write = wil_write_file_reset,
 	.open  = simple_open,
 };
+
 /*---write channel 1..4 to rxon for it, 0 to rxoff---*/
 static ssize_t wil_write_file_rxon(struct file *file, const char __user *buf,
 				   size_t len, loff_t *ppos)
@@ -446,6 +520,7 @@ static ssize_t wil_write_file_rxon(struct file *file, const char __user *buf,
 	bool on;
 
 	char *kbuf = kmalloc(len + 1, GFP_KERNEL);
+
 	if (!kbuf)
 		return -ENOMEM;
 	if (copy_from_user(kbuf, buf, len)) {
@@ -482,6 +557,7 @@ static const struct file_operations fops_rxon = {
 	.write = wil_write_file_rxon,
 	.open  = simple_open,
 };
+
 /*---tx_mgmt---*/
 /* Write mgmt frame to this file to send it */
 static ssize_t wil_write_file_txmgmt(struct file *file, const char __user *buf,
@@ -492,8 +568,8 @@ static ssize_t wil_write_file_txmgmt(struct file *file, const char __user *buf,
 	struct wireless_dev *wdev = wil_to_wdev(wil);
 	struct cfg80211_mgmt_tx_params params;
 	int rc;
-
 	void *frame = kmalloc(len, GFP_KERNEL);
+
 	if (!frame)
 		return -ENOMEM;
 
@@ -562,8 +638,10 @@ static void wil_seq_hexdump(struct seq_file *s, void *p, int len,
 {
 	char printbuf[16 * 3 + 2];
 	int i = 0;
+
 	while (i < len) {
 		int l = min(len - i, 16);
+
 		hex_dump_to_buffer(p + i, l, 16, 1, printbuf,
 				   sizeof(printbuf), false);
 		seq_printf(s, "%s%s\n", prefix, printbuf);
@@ -601,10 +679,8 @@ static int wil_txdesc_debugfs_show(struct seq_file *s, void *data)
 	struct wil6210_priv *wil = s->private;
 	struct vring *vring;
 	bool tx = (dbg_vring_index < WIL6210_MAX_TX_RINGS);
-	if (tx)
-		vring = &(wil->vring_tx[dbg_vring_index]);
-	else
-		vring = &wil->vring_rx;
+
+	vring = tx ? &wil->vring_tx[dbg_vring_index] : &wil->vring_rx;
 
 	if (!vring->va) {
 		if (tx)
@@ -619,7 +695,7 @@ static int wil_txdesc_debugfs_show(struct seq_file *s, void *data)
 		 * only field used, .dma.length, is the same
 		 */
 		volatile struct vring_tx_desc *d =
-				&(vring->va[dbg_txdesc_index].tx);
+				&vring->va[dbg_txdesc_index].tx;
 		volatile u32 *u = (volatile u32 *)d;
 		struct sk_buff *skb = vring->ctx[dbg_txdesc_index].skb;
 
@@ -639,7 +715,7 @@ static int wil_txdesc_debugfs_show(struct seq_file *s, void *data)
 			wil_seq_print_skb(s, skb);
 			kfree_skb(skb);
 		}
-		seq_printf(s, "}\n");
+		seq_puts(s, "}\n");
 	} else {
 		if (tx)
 			seq_printf(s, "[%2d] TxDesc index (%d) >= size (%d)\n",
@@ -666,16 +742,79 @@ static const struct file_operations fops_txdesc = {
 };
 
 /*---------beamforming------------*/
+static char *wil_bfstatus_str(u32 status)
+{
+	switch (status) {
+	case 0:
+		return "Failed";
+	case 1:
+		return "OK";
+	case 2:
+		return "Retrying";
+	default:
+		return "??";
+	}
+}
+
+static bool is_all_zeros(void * const x_, size_t sz)
+{
+	/* if reply is all-0, ignore this CID */
+	u32 *x = x_;
+	int n;
+
+	for (n = 0; n < sz / sizeof(*x); n++)
+		if (x[n])
+			return false;
+
+	return true;
+}
+
 static int wil_bf_debugfs_show(struct seq_file *s, void *data)
 {
+	int rc;
+	int i;
 	struct wil6210_priv *wil = s->private;
-	seq_printf(s,
-		   "TSF : 0x%016llx\n"
-		   "TxMCS : %d\n"
-		   "Sectors(rx:tx) my %2d:%2d peer %2d:%2d\n",
-		   wil->stats.tsf, wil->stats.bf_mcs,
-		   wil->stats.my_rx_sector, wil->stats.my_tx_sector,
-		   wil->stats.peer_rx_sector, wil->stats.peer_tx_sector);
+	struct wmi_notify_req_cmd cmd = {
+		.interval_usec = 0,
+	};
+	struct {
+		struct wil6210_mbox_hdr_wmi wmi;
+		struct wmi_notify_req_done_event evt;
+	} __packed reply;
+
+	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
+		u32 status;
+
+		cmd.cid = i;
+		rc = wmi_call(wil, WMI_NOTIFY_REQ_CMDID, &cmd, sizeof(cmd),
+			      WMI_NOTIFY_REQ_DONE_EVENTID, &reply,
+			      sizeof(reply), 20);
+		/* if reply is all-0, ignore this CID */
+		if (rc || is_all_zeros(&reply.evt, sizeof(reply.evt)))
+			continue;
+
+		status = le32_to_cpu(reply.evt.status);
+		seq_printf(s, "CID %d {\n"
+			   "  TSF = 0x%016llx\n"
+			   "  TxMCS = %2d TxTpt = %4d\n"
+			   "  SQI = %4d\n"
+			   "  Status = 0x%08x %s\n"
+			   "  Sectors(rx:tx) my %2d:%2d peer %2d:%2d\n"
+			   "  Goodput(rx:tx) %4d:%4d\n"
+			   "}\n",
+			   i,
+			   le64_to_cpu(reply.evt.tsf),
+			   le16_to_cpu(reply.evt.bf_mcs),
+			   le32_to_cpu(reply.evt.tx_tpt),
+			   reply.evt.sqi,
+			   status, wil_bfstatus_str(status),
+			   le16_to_cpu(reply.evt.my_rx_sector),
+			   le16_to_cpu(reply.evt.my_tx_sector),
+			   le16_to_cpu(reply.evt.other_rx_sector),
+			   le16_to_cpu(reply.evt.other_tx_sector),
+			   le32_to_cpu(reply.evt.rx_goodput),
+			   le32_to_cpu(reply.evt.tx_goodput));
+	}
 	return 0;
 }
 
@@ -690,6 +829,7 @@ static const struct file_operations fops_bf = {
 	.read		= seq_read,
 	.llseek		= seq_lseek,
 };
+
 /*---------SSID------------*/
 static ssize_t wil_read_file_ssid(struct file *file, char __user *user_buf,
 				  size_t count, loff_t *ppos)
@@ -752,10 +892,10 @@ static int wil_temp_debugfs_show(struct seq_file *s, void *data)
 {
 	struct wil6210_priv *wil = s->private;
 	u32 t_m, t_r;
-
 	int rc = wmi_get_temperature(wil, &t_m, &t_r);
+
 	if (rc) {
-		seq_printf(s, "Failed\n");
+		seq_puts(s, "Failed\n");
 		return 0;
 	}
 
@@ -811,6 +951,7 @@ static int wil_link_debugfs_show(struct seq_file *s, void *data)
 	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
 		struct wil_sta_info *p = &wil->sta[i];
 		char *status = "unknown";
+
 		switch (p->status) {
 		case wil_sta_unused:
 			status = "unused   ";
@@ -871,7 +1012,6 @@ static int wil_info_debugfs_show(struct seq_file *s, void *data)
 	rxf_old = rxf;
 	txf_old = txf;
 
-
 #define CHECK_QSTATE(x) (state & BIT(__QUEUE_STATE_ ## x)) ? \
 	" " __stringify(x) : ""
 
@@ -901,11 +1041,77 @@ static const struct file_operations fops_info = {
 	.llseek		= seq_lseek,
 };
 
+/*---------recovery------------*/
+/* mode = [manual|auto]
+ * state = [idle|pending|running]
+ */
+static ssize_t wil_read_file_recovery(struct file *file, char __user *user_buf,
+				      size_t count, loff_t *ppos)
+{
+	struct wil6210_priv *wil = file->private_data;
+	char buf[80];
+	int n;
+	static const char * const sstate[] = {"idle", "pending", "running"};
+
+	n = snprintf(buf, sizeof(buf), "mode = %s\nstate = %s\n",
+		     no_fw_recovery ? "manual" : "auto",
+		     sstate[wil->recovery_state]);
+
+	n = min_t(int, n, sizeof(buf));
+
+	return simple_read_from_buffer(user_buf, count, ppos,
+				       buf, n);
+}
+
+static ssize_t wil_write_file_recovery(struct file *file,
+				       const char __user *buf_,
+				       size_t count, loff_t *ppos)
+{
+	struct wil6210_priv *wil = file->private_data;
+	static const char run_command[] = "run";
+	char buf[sizeof(run_command) + 1]; /* to detect "runx" */
+	ssize_t rc;
+
+	if (wil->recovery_state != fw_recovery_pending) {
+		wil_err(wil, "No recovery pending\n");
+		return -EINVAL;
+	}
+
+	if (*ppos != 0) {
+		wil_err(wil, "Offset [%d]\n", (int)*ppos);
+		return -EINVAL;
+	}
+
+	if (count > sizeof(buf)) {
+		wil_err(wil, "Input too long, len = %d\n", (int)count);
+		return -EINVAL;
+	}
+
+	rc = simple_write_to_buffer(buf, sizeof(buf) - 1, ppos, buf_, count);
+	if (rc < 0)
+		return rc;
+
+	buf[rc] = '\0';
+	if (0 == strcmp(buf, run_command))
+		wil_set_recovery_state(wil, fw_recovery_running);
+	else
+		wil_err(wil, "Bad recovery command \"%s\"\n", buf);
+
+	return rc;
+}
+
+static const struct file_operations fops_recovery = {
+	.read = wil_read_file_recovery,
+	.write = wil_write_file_recovery,
+	.open  = simple_open,
+};
+
 /*---------Station matrix------------*/
 static void wil_print_rxtid(struct seq_file *s, struct wil_tid_ampdu_rx *r)
 {
 	int i;
 	u16 index = ((r->head_seq_num - r->ssn) & 0xfff) % r->buf_size;
+
 	seq_printf(s, "0x%03x [", r->head_seq_num);
 	for (i = 0; i < r->buf_size; i++) {
 		if (i == index)
@@ -920,10 +1126,12 @@ static int wil_sta_debugfs_show(struct seq_file *s, void *data)
 {
 	struct wil6210_priv *wil = s->private;
 	int i, tid;
+	unsigned long flags;
 
 	for (i = 0; i < ARRAY_SIZE(wil->sta); i++) {
 		struct wil_sta_info *p = &wil->sta[i];
 		char *status = "unknown";
+
 		switch (p->status) {
 		case wil_sta_unused:
 			status = "unused   ";
@@ -939,13 +1147,16 @@ static int wil_sta_debugfs_show(struct seq_file *s, void *data)
 			   (p->data_port_open ? " data_port_open" : ""));
 
 		if (p->status == wil_sta_connected) {
+			spin_lock_irqsave(&p->tid_rx_lock, flags);
 			for (tid = 0; tid < WIL_STA_TID_NUM; tid++) {
 				struct wil_tid_ampdu_rx *r = p->tid_rx[tid];
+
 				if (r) {
 					seq_printf(s, "[%2d] ", tid);
 					wil_print_rxtid(s, r);
 				}
 			}
+			spin_unlock_irqrestore(&p->tid_rx_lock, flags);
 		}
 	}
 
@@ -985,6 +1196,89 @@ static void wil6210_debugfs_init_blobs(struct wil6210_priv *wil,
 	}
 }
 
+/* misc files */
+static const struct {
+	const char *name;
+	umode_t mode;
+	const struct file_operations *fops;
+} dbg_files[] = {
+	{"mbox",	S_IRUGO,		&fops_mbox},
+	{"vrings",	S_IRUGO,		&fops_vring},
+	{"stations",	S_IRUGO,		&fops_sta},
+	{"desc",	S_IRUGO,		&fops_txdesc},
+	{"bf",		S_IRUGO,		&fops_bf},
+	{"ssid",	S_IRUGO | S_IWUSR,	&fops_ssid},
+	{"mem_val",	S_IRUGO,		&fops_memread},
+	{"reset",		  S_IWUSR,	&fops_reset},
+	{"rxon",		  S_IWUSR,	&fops_rxon},
+	{"tx_mgmt",		  S_IWUSR,	&fops_txmgmt},
+	{"wmi_send",		  S_IWUSR,	&fops_wmi},
+	{"temp",	S_IRUGO,		&fops_temp},
+	{"freq",	S_IRUGO,		&fops_freq},
+	{"link",	S_IRUGO,		&fops_link},
+	{"info",	S_IRUGO,		&fops_info},
+	{"recovery",	S_IRUGO | S_IWUSR,	&fops_recovery},
+};
+
+static void wil6210_debugfs_init_files(struct wil6210_priv *wil,
+				       struct dentry *dbg)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dbg_files); i++)
+		debugfs_create_file(dbg_files[i].name, dbg_files[i].mode, dbg,
+				    wil, dbg_files[i].fops);
+}
+
+/* interrupt control blocks */
+static const struct {
+	const char *name;
+	u32 icr_off;
+} dbg_icr[] = {
+	{"USER_ICR",		HOSTADDR(RGF_USER_USER_ICR)},
+	{"DMA_EP_TX_ICR",	HOSTADDR(RGF_DMA_EP_TX_ICR)},
+	{"DMA_EP_RX_ICR",	HOSTADDR(RGF_DMA_EP_RX_ICR)},
+	{"DMA_EP_MISC_ICR",	HOSTADDR(RGF_DMA_EP_MISC_ICR)},
+};
+
+static void wil6210_debugfs_init_isr(struct wil6210_priv *wil,
+				     struct dentry *dbg)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(dbg_icr); i++)
+		wil6210_debugfs_create_ISR(wil, dbg_icr[i].name, dbg,
+					   dbg_icr[i].icr_off);
+}
+
+#define WIL_FIELD(name, mode, type) { __stringify(name), mode, \
+	offsetof(struct wil6210_priv, name), type}
+
+/* fields in struct wil6210_priv */
+static const struct dbg_off dbg_wil_off[] = {
+	WIL_FIELD(secure_pcp,	S_IRUGO | S_IWUSR,	doff_u32),
+	WIL_FIELD(status,	S_IRUGO | S_IWUSR,	doff_ulong),
+	WIL_FIELD(fw_version,	S_IRUGO,		doff_u32),
+	WIL_FIELD(hw_version,	S_IRUGO,		doff_x32),
+	WIL_FIELD(recovery_count, S_IRUGO,		doff_u32),
+	{},
+};
+
+static const struct dbg_off dbg_wil_regs[] = {
+	{"RGF_MAC_MTRL_COUNTER_0", S_IRUGO, HOSTADDR(RGF_MAC_MTRL_COUNTER_0),
+		doff_io32},
+	{"RGF_USER_USAGE_1", S_IRUGO, HOSTADDR(RGF_USER_USAGE_1), doff_io32},
+	{},
+};
+
+/* static parameters */
+static const struct dbg_off dbg_statics[] = {
+	{"desc_index",	S_IRUGO | S_IWUSR, (ulong)&dbg_txdesc_index, doff_u32},
+	{"vring_index",	S_IRUGO | S_IWUSR, (ulong)&dbg_vring_index, doff_u32},
+	{"mem_addr",	S_IRUGO | S_IWUSR, (ulong)&mem_addr, doff_u32},
+	{},
+};
+
 int wil6210_debugfs_init(struct wil6210_priv *wil)
 {
 	struct dentry *dbg = wil->debug = debugfs_create_dir(WIL_NAME,
@@ -993,51 +1287,17 @@ int wil6210_debugfs_init(struct wil6210_priv *wil)
 	if (IS_ERR_OR_NULL(dbg))
 		return -ENODEV;
 
-	debugfs_create_file("mbox", S_IRUGO, dbg, wil, &fops_mbox);
-	debugfs_create_file("vrings", S_IRUGO, dbg, wil, &fops_vring);
-	debugfs_create_file("stations", S_IRUGO, dbg, wil, &fops_sta);
-	debugfs_create_file("desc", S_IRUGO, dbg, wil, &fops_txdesc);
-	debugfs_create_u32("desc_index", S_IRUGO | S_IWUSR, dbg,
-			   &dbg_txdesc_index);
-	debugfs_create_u32("vring_index", S_IRUGO | S_IWUSR, dbg,
-			   &dbg_vring_index);
-
-	debugfs_create_file("bf", S_IRUGO, dbg, wil, &fops_bf);
-	debugfs_create_file("ssid", S_IRUGO | S_IWUSR, dbg, wil, &fops_ssid);
-	debugfs_create_u32("secure_pcp", S_IRUGO | S_IWUSR, dbg,
-			   &wil->secure_pcp);
-	wil_debugfs_create_ulong("status", S_IRUGO | S_IWUSR, dbg,
-				 &wil->status);
-	debugfs_create_u32("fw_version", S_IRUGO, dbg, &wil->fw_version);
-	debugfs_create_x32("hw_version", S_IRUGO, dbg, &wil->hw_version);
-
-	wil6210_debugfs_create_ISR(wil, "USER_ICR", dbg,
-				   HOSTADDR(RGF_USER_USER_ICR));
-	wil6210_debugfs_create_ISR(wil, "DMA_EP_TX_ICR", dbg,
-				   HOSTADDR(RGF_DMA_EP_TX_ICR));
-	wil6210_debugfs_create_ISR(wil, "DMA_EP_RX_ICR", dbg,
-				   HOSTADDR(RGF_DMA_EP_RX_ICR));
-	wil6210_debugfs_create_ISR(wil, "DMA_EP_MISC_ICR", dbg,
-				   HOSTADDR(RGF_DMA_EP_MISC_ICR));
-	wil6210_debugfs_create_pseudo_ISR(wil, dbg);
-	wil6210_debugfs_create_ITR_CNT(wil, dbg);
+	wil6210_debugfs_init_files(wil, dbg);
+	wil6210_debugfs_init_isr(wil, dbg);
+	wil6210_debugfs_init_blobs(wil, dbg);
+	wil6210_debugfs_init_offset(wil, dbg, wil, dbg_wil_off);
+	wil6210_debugfs_init_offset(wil, dbg, (void * __force)wil->csr,
+				    dbg_wil_regs);
+	wil6210_debugfs_init_offset(wil, dbg, NULL, dbg_statics);
 
-	wil_debugfs_create_iomem_x32("RGF_USER_USAGE_1", S_IRUGO, dbg,
-				     wil->csr +
-				     HOSTADDR(RGF_USER_USAGE_1));
-	debugfs_create_u32("mem_addr", S_IRUGO | S_IWUSR, dbg, &mem_addr);
-	debugfs_create_file("mem_val", S_IRUGO, dbg, wil, &fops_memread);
-
-	debugfs_create_file("reset", S_IWUSR, dbg, wil, &fops_reset);
-	debugfs_create_file("rxon", S_IWUSR, dbg, wil, &fops_rxon);
-	debugfs_create_file("tx_mgmt", S_IWUSR, dbg, wil, &fops_txmgmt);
-	debugfs_create_file("wmi_send", S_IWUSR, dbg, wil, &fops_wmi);
-	debugfs_create_file("temp", S_IRUGO, dbg, wil, &fops_temp);
-	debugfs_create_file("freq", S_IRUGO, dbg, wil, &fops_freq);
-	debugfs_create_file("link", S_IRUGO, dbg, wil, &fops_link);
-	debugfs_create_file("info", S_IRUGO, dbg, wil, &fops_info);
+	wil6210_debugfs_create_pseudo_ISR(wil, dbg);
 
-	wil6210_debugfs_init_blobs(wil, dbg);
+	wil6210_debugfs_create_ITR_CNT(wil, dbg);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/ath/wil6210/ethtool.c b/drivers/net/wireless/ath/wil6210/ethtool.c
new file mode 100644
index 000000000000..d686638972be
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/ethtool.c
@@ -0,0 +1,103 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/etherdevice.h>
+#include <linux/pci.h>
+#include <linux/rtnetlink.h>
+#include <net/cfg80211.h>
+
+#include "wil6210.h"
+
+static int wil_ethtoolops_begin(struct net_device *ndev)
+{
+	struct wil6210_priv *wil = ndev_to_wil(ndev);
+
+	mutex_lock(&wil->mutex);
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	return 0;
+}
+
+static void wil_ethtoolops_complete(struct net_device *ndev)
+{
+	struct wil6210_priv *wil = ndev_to_wil(ndev);
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	mutex_unlock(&wil->mutex);
+}
+
+static int wil_ethtoolops_get_coalesce(struct net_device *ndev,
+				       struct ethtool_coalesce *cp)
+{
+	struct wil6210_priv *wil = ndev_to_wil(ndev);
+	u32 itr_en, itr_val = 0;
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	itr_en = ioread32(wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_CRL));
+	if (itr_en & BIT_DMA_ITR_CNT_CRL_EN)
+		itr_val = ioread32(wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_TRSH));
+
+	cp->rx_coalesce_usecs = itr_val;
+
+	return 0;
+}
+
+static int wil_ethtoolops_set_coalesce(struct net_device *ndev,
+				       struct ethtool_coalesce *cp)
+{
+	struct wil6210_priv *wil = ndev_to_wil(ndev);
+
+	wil_dbg_misc(wil, "%s(%d usec)\n", __func__, cp->rx_coalesce_usecs);
+
+	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR) {
+		wil_dbg_misc(wil, "No IRQ coalescing in monitor mode\n");
+		return -EINVAL;
+	}
+
+	/* only @rx_coalesce_usecs supported, ignore
+	 * other parameters
+	 */
+
+	if (cp->rx_coalesce_usecs > WIL6210_ITR_TRSH_MAX)
+		goto out_bad;
+
+	wil->itr_trsh = cp->rx_coalesce_usecs;
+	wil_set_itr_trsh(wil);
+
+	return 0;
+
+out_bad:
+	wil_dbg_misc(wil, "Unsupported coalescing params. Raw command:\n");
+	print_hex_dump_debug("DBG[MISC] coal ", DUMP_PREFIX_OFFSET, 16, 4,
+			     cp, sizeof(*cp), false);
+	return -EINVAL;
+}
+
+static const struct ethtool_ops wil_ethtool_ops = {
+	.begin		= wil_ethtoolops_begin,
+	.complete	= wil_ethtoolops_complete,
+	.get_drvinfo	= cfg80211_get_drvinfo,
+	.get_coalesce	= wil_ethtoolops_get_coalesce,
+	.set_coalesce	= wil_ethtoolops_set_coalesce,
+};
+
+void wil_set_ethtoolops(struct net_device *ndev)
+{
+	ndev->ethtool_ops = &wil_ethtool_ops;
+}
diff --git a/drivers/net/wireless/ath/wil6210/fw.c b/drivers/net/wireless/ath/wil6210/fw.c
new file mode 100644
index 000000000000..8c6f3b041f77
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/fw.c
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+#include <linux/firmware.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/crc32.h>
+#include "wil6210.h"
+#include "fw.h"
+
+MODULE_FIRMWARE(WIL_FW_NAME);
+
+/* target operations */
+/* register read */
+#define R(a) ioread32(wil->csr + HOSTADDR(a))
+/* register write. wmb() to make sure it is completed */
+#define W(a, v) do { iowrite32(v, wil->csr + HOSTADDR(a)); wmb(); } while (0)
+/* register set = read, OR, write */
+#define S(a, v) W(a, R(a) | v)
+/* register clear = read, AND with inverted, write */
+#define C(a, v) W(a, R(a) & ~v)
+
+static
+void wil_memset_toio_32(volatile void __iomem *dst, u32 val,
+			size_t count)
+{
+	volatile u32 __iomem *d = dst;
+
+	for (count += 4; count > 4; count -= 4)
+		__raw_writel(val, d++);
+}
+
+#include "fw_inc.c"
diff --git a/drivers/net/wireless/ath/wil6210/fw.h b/drivers/net/wireless/ath/wil6210/fw.h
new file mode 100644
index 000000000000..7a2c6c129ad5
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/fw.h
@@ -0,0 +1,149 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#define WIL_FW_SIGNATURE (0x36323130) /* '0126' */
+#define WIL_FW_FMT_VERSION (1) /* format version driver supports */
+
+enum wil_fw_record_type {
+	wil_fw_type_comment = 1,
+	wil_fw_type_data = 2,
+	wil_fw_type_fill = 3,
+	wil_fw_type_action = 4,
+	wil_fw_type_verify = 5,
+	wil_fw_type_file_header = 6,
+	wil_fw_type_direct_write = 7,
+	wil_fw_type_gateway_data = 8,
+	wil_fw_type_gateway_data4 = 9,
+};
+
+struct wil_fw_record_head {
+	__le16 type; /* enum wil_fw_record_type */
+	__le16 flags; /* to be defined */
+	__le32 size; /* whole record, bytes after head */
+} __packed;
+
+/* data block. write starting from @addr
+ * data_size inferred from the @head.size. For this case,
+ * data_size = @head.size - offsetof(struct wil_fw_record_data, data)
+ */
+struct wil_fw_record_data { /* type == wil_fw_type_data */
+	__le32 addr;
+	__le32 data[0]; /* [data_size], see above */
+} __packed;
+
+/* fill with constant @value, @size bytes starting from @addr */
+struct wil_fw_record_fill { /* type == wil_fw_type_fill */
+	__le32 addr;
+	__le32 value;
+	__le32 size;
+} __packed;
+
+/* free-form comment
+ * for informational purpose, data_size is @head.size from record header
+ */
+struct wil_fw_record_comment { /* type == wil_fw_type_comment */
+	u8 data[0]; /* free-form data [data_size], see above */
+} __packed;
+
+/* perform action
+ * data_size = @head.size - offsetof(struct wil_fw_record_action, data)
+ */
+struct wil_fw_record_action { /* type == wil_fw_type_action */
+	__le32 action; /* action to perform: reset, wait for fw ready etc. */
+	__le32 data[0]; /* action specific, [data_size], see above */
+} __packed;
+
+/* data block for struct wil_fw_record_direct_write */
+struct wil_fw_data_dwrite {
+	__le32 addr;
+	__le32 value;
+	__le32 mask;
+} __packed;
+
+/* write @value to the @addr,
+ * preserve original bits accordingly to the @mask
+ * data_size is @head.size where @head is record header
+ */
+struct wil_fw_record_direct_write { /* type == wil_fw_type_direct_write */
+	struct wil_fw_data_dwrite data[0];
+} __packed;
+
+/* verify condition: [@addr] & @mask == @value
+ * if condition not met, firmware download fails
+ */
+struct wil_fw_record_verify { /* type == wil_fw_verify */
+	__le32 addr; /* read from this address */
+	__le32 value; /* reference value */
+	__le32 mask; /* mask for verification */
+} __packed;
+
+/* file header
+ * First record of every file
+ */
+struct wil_fw_record_file_header {
+	__le32 signature ; /* Wilocity signature */
+	__le32 reserved;
+	__le32 crc; /* crc32 of the following data  */
+	__le32 version; /* format version */
+	__le32 data_len; /* total data in file, including this record */
+	u8 comment[32]; /* short description */
+} __packed;
+
+/* 1-dword gateway */
+/* data block for the struct wil_fw_record_gateway_data */
+struct wil_fw_data_gw {
+	__le32 addr;
+	__le32 value;
+} __packed;
+
+/* gateway write block.
+ * write starting address and values from the data buffer
+ * through the gateway
+ * data_size inferred from the @head.size. For this case,
+ * data_size = @head.size - offsetof(struct wil_fw_record_gateway_data, data)
+ */
+struct wil_fw_record_gateway_data { /* type == wil_fw_type_gateway_data */
+	__le32 gateway_addr_addr;
+	__le32 gateway_value_addr;
+	__le32 gateway_cmd_addr;
+	__le32 gateway_ctrl_address;
+#define WIL_FW_GW_CTL_BUSY	BIT(29) /* gateway busy performing operation */
+#define WIL_FW_GW_CTL_RUN	BIT(30) /* start gateway operation */
+	__le32 command;
+	struct wil_fw_data_gw data[0]; /* total size [data_size], see above */
+} __packed;
+
+/* 4-dword gateway */
+/* data block for the struct wil_fw_record_gateway_data4 */
+struct wil_fw_data_gw4 {
+	__le32 addr;
+	__le32 value[4];
+} __packed;
+
+/* gateway write block.
+ * write starting address and values from the data buffer
+ * through the gateway
+ * data_size inferred from the @head.size. For this case,
+ * data_size = @head.size - offsetof(struct wil_fw_record_gateway_data4, data)
+ */
+struct wil_fw_record_gateway_data4 { /* type == wil_fw_type_gateway_data4 */
+	__le32 gateway_addr_addr;
+	__le32 gateway_value_addr[4];
+	__le32 gateway_cmd_addr;
+	__le32 gateway_ctrl_address; /* same logic as for 1-dword gw */
+	__le32 command;
+	struct wil_fw_data_gw4 data[0]; /* total size [data_size], see above */
+} __packed;
diff --git a/drivers/net/wireless/ath/wil6210/fw_inc.c b/drivers/net/wireless/ath/wil6210/fw_inc.c
new file mode 100644
index 000000000000..44cb71f5ea5b
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/fw_inc.c
@@ -0,0 +1,495 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+/* Algorithmic part of the firmware download.
+ * To be included in the container file providing framework
+ */
+
+#define wil_err_fw(wil, fmt, arg...) wil_err(wil, "ERR[ FW ]" fmt, ##arg)
+#define wil_dbg_fw(wil, fmt, arg...) wil_dbg(wil, "DBG[ FW ]" fmt, ##arg)
+#define wil_hex_dump_fw(prefix_str, prefix_type, rowsize,		\
+			groupsize, buf, len, ascii)			\
+			print_hex_dump_debug("DBG[ FW ]" prefix_str,	\
+					     prefix_type, rowsize,	\
+					     groupsize, buf, len, ascii)
+
+#define FW_ADDR_CHECK(ioaddr, val, msg) do { \
+		ioaddr = wmi_buffer(wil, val); \
+		if (!ioaddr) { \
+			wil_err_fw(wil, "bad " msg ": 0x%08x\n", \
+				   le32_to_cpu(val)); \
+			return -EINVAL; \
+		} \
+	} while (0)
+
+/**
+ * wil_fw_verify - verify firmware file validity
+ *
+ * perform various checks for the firmware file header.
+ * records are not validated.
+ *
+ * Return file size or negative error
+ */
+static int wil_fw_verify(struct wil6210_priv *wil, const u8 *data, size_t size)
+{
+	const struct wil_fw_record_head *hdr = (const void *)data;
+	struct wil_fw_record_file_header fh;
+	const struct wil_fw_record_file_header *fh_;
+	u32 crc;
+	u32 dlen;
+
+	if (size % 4) {
+		wil_err_fw(wil, "image size not aligned: %zu\n", size);
+		return -EINVAL;
+	}
+	/* have enough data for the file header? */
+	if (size < sizeof(*hdr) + sizeof(fh)) {
+		wil_err_fw(wil, "file too short: %zu bytes\n", size);
+		return -EINVAL;
+	}
+
+	/* start with the file header? */
+	if (le16_to_cpu(hdr->type) != wil_fw_type_file_header) {
+		wil_err_fw(wil, "no file header\n");
+		return -EINVAL;
+	}
+
+	/* data_len */
+	fh_ = (struct wil_fw_record_file_header *)&hdr[1];
+	dlen = le32_to_cpu(fh_->data_len);
+	if (dlen % 4) {
+		wil_err_fw(wil, "data length not aligned: %lu\n", (ulong)dlen);
+		return -EINVAL;
+	}
+	if (size < dlen) {
+		wil_err_fw(wil, "file truncated at %zu/%lu\n",
+			   size, (ulong)dlen);
+		return -EINVAL;
+	}
+	if (dlen < sizeof(*hdr) + sizeof(fh)) {
+		wil_err_fw(wil, "data length too short: %lu\n", (ulong)dlen);
+		return -EINVAL;
+	}
+
+	/* signature */
+	if (le32_to_cpu(fh_->signature) != WIL_FW_SIGNATURE) {
+		wil_err_fw(wil, "bad header signature: 0x%08x\n",
+			   le32_to_cpu(fh_->signature));
+		return -EINVAL;
+	}
+
+	/* version */
+	if (le32_to_cpu(fh_->version) > WIL_FW_FMT_VERSION) {
+		wil_err_fw(wil, "unsupported header version: %d\n",
+			   le32_to_cpu(fh_->version));
+		return -EINVAL;
+	}
+
+	/* checksum. ~crc32(~0, data, size) when fh.crc set to 0*/
+	fh = *fh_;
+	fh.crc = 0;
+
+	crc = crc32_le(~0, (unsigned char const *)hdr, sizeof(*hdr));
+	crc = crc32_le(crc, (unsigned char const *)&fh, sizeof(fh));
+	crc = crc32_le(crc, (unsigned char const *)&fh_[1],
+		       dlen - sizeof(*hdr) - sizeof(fh));
+	crc = ~crc;
+
+	if (crc != le32_to_cpu(fh_->crc)) {
+		wil_err_fw(wil, "checksum mismatch:"
+			   " calculated for %lu bytes 0x%08x != 0x%08x\n",
+			   (ulong)dlen, crc, le32_to_cpu(fh_->crc));
+		return -EINVAL;
+	}
+
+	return (int)dlen;
+}
+
+static int fw_handle_comment(struct wil6210_priv *wil, const void *data,
+			     size_t size)
+{
+	wil_hex_dump_fw("", DUMP_PREFIX_OFFSET, 16, 1, data, size, true);
+
+	return 0;
+}
+
+static int fw_handle_data(struct wil6210_priv *wil, const void *data,
+			  size_t size)
+{
+	const struct wil_fw_record_data *d = data;
+	void __iomem *dst;
+	size_t s = size - sizeof(*d);
+
+	if (size < sizeof(*d) + sizeof(u32)) {
+		wil_err_fw(wil, "data record too short: %zu\n", size);
+		return -EINVAL;
+	}
+
+	FW_ADDR_CHECK(dst, d->addr, "address");
+	wil_dbg_fw(wil, "write [0x%08x] <== %zu bytes\n", le32_to_cpu(d->addr),
+		   s);
+	wil_memcpy_toio_32(dst, d->data, s);
+	wmb(); /* finish before processing next record */
+
+	return 0;
+}
+
+static int fw_handle_fill(struct wil6210_priv *wil, const void *data,
+			  size_t size)
+{
+	const struct wil_fw_record_fill *d = data;
+	void __iomem *dst;
+	u32 v;
+	size_t s = (size_t)le32_to_cpu(d->size);
+
+	if (size != sizeof(*d)) {
+		wil_err_fw(wil, "bad size for fill record: %zu\n", size);
+		return -EINVAL;
+	}
+
+	if (s < sizeof(u32)) {
+		wil_err_fw(wil, "fill size too short: %zu\n", s);
+		return -EINVAL;
+	}
+
+	if (s % sizeof(u32)) {
+		wil_err_fw(wil, "fill size not aligned: %zu\n", s);
+		return -EINVAL;
+	}
+
+	FW_ADDR_CHECK(dst, d->addr, "address");
+
+	v = le32_to_cpu(d->value);
+	wil_dbg_fw(wil, "fill [0x%08x] <== 0x%08x, %zu bytes\n",
+		   le32_to_cpu(d->addr), v, s);
+	wil_memset_toio_32(dst, v, s);
+	wmb(); /* finish before processing next record */
+
+	return 0;
+}
+
+static int fw_handle_file_header(struct wil6210_priv *wil, const void *data,
+				 size_t size)
+{
+	const struct wil_fw_record_file_header *d = data;
+
+	if (size != sizeof(*d)) {
+		wil_err_fw(wil, "file header length incorrect: %zu\n", size);
+		return -EINVAL;
+	}
+
+	wil_dbg_fw(wil, "new file, ver. %d, %i bytes\n",
+		   d->version, d->data_len);
+	wil_hex_dump_fw("", DUMP_PREFIX_OFFSET, 16, 1, d->comment,
+			sizeof(d->comment), true);
+
+	return 0;
+}
+
+static int fw_handle_direct_write(struct wil6210_priv *wil, const void *data,
+				  size_t size)
+{
+	const struct wil_fw_record_direct_write *d = data;
+	const struct wil_fw_data_dwrite *block = d->data;
+	int n, i;
+
+	if (size % sizeof(*block)) {
+		wil_err_fw(wil, "record size not aligned on %zu: %zu\n",
+			   sizeof(*block), size);
+		return -EINVAL;
+	}
+	n = size / sizeof(*block);
+
+	for (i = 0; i < n; i++) {
+		void __iomem *dst;
+		u32 m = le32_to_cpu(block[i].mask);
+		u32 v = le32_to_cpu(block[i].value);
+		u32 x, y;
+
+		FW_ADDR_CHECK(dst, block[i].addr, "address");
+
+		x = ioread32(dst);
+		y = (x & m) | (v & ~m);
+		wil_dbg_fw(wil, "write [0x%08x] <== 0x%08x "
+			   "(old 0x%08x val 0x%08x mask 0x%08x)\n",
+			   le32_to_cpu(block[i].addr), y, x, v, m);
+		iowrite32(y, dst);
+		wmb(); /* finish before processing next record */
+	}
+
+	return 0;
+}
+
+static int gw_write(struct wil6210_priv *wil, void __iomem *gwa_addr,
+		    void __iomem *gwa_cmd, void __iomem *gwa_ctl, u32 gw_cmd,
+		    u32 a)
+{
+	unsigned delay = 0;
+
+	iowrite32(a, gwa_addr);
+	iowrite32(gw_cmd, gwa_cmd);
+	wmb(); /* finish before activate gw */
+
+	iowrite32(WIL_FW_GW_CTL_RUN, gwa_ctl); /* activate gw */
+	do {
+		udelay(1); /* typical time is few usec */
+		if (delay++ > 100) {
+			wil_err_fw(wil, "gw timeout\n");
+			return -EINVAL;
+		}
+	} while (ioread32(gwa_ctl) & WIL_FW_GW_CTL_BUSY); /* gw done? */
+
+	return 0;
+}
+
+static int fw_handle_gateway_data(struct wil6210_priv *wil, const void *data,
+				  size_t size)
+{
+	const struct wil_fw_record_gateway_data *d = data;
+	const struct wil_fw_data_gw *block = d->data;
+	void __iomem *gwa_addr;
+	void __iomem *gwa_val;
+	void __iomem *gwa_cmd;
+	void __iomem *gwa_ctl;
+	u32 gw_cmd;
+	int n, i;
+
+	if (size < sizeof(*d) + sizeof(*block)) {
+		wil_err_fw(wil, "gateway record too short: %zu\n", size);
+		return -EINVAL;
+	}
+
+	if ((size - sizeof(*d)) % sizeof(*block)) {
+		wil_err_fw(wil, "gateway record data size"
+			   " not aligned on %zu: %zu\n",
+			   sizeof(*block), size - sizeof(*d));
+		return -EINVAL;
+	}
+	n = (size - sizeof(*d)) / sizeof(*block);
+
+	gw_cmd = le32_to_cpu(d->command);
+
+	wil_dbg_fw(wil, "gw write record [%3d] blocks, cmd 0x%08x\n",
+		   n, gw_cmd);
+
+	FW_ADDR_CHECK(gwa_addr, d->gateway_addr_addr, "gateway_addr_addr");
+	FW_ADDR_CHECK(gwa_val, d->gateway_value_addr, "gateway_value_addr");
+	FW_ADDR_CHECK(gwa_cmd, d->gateway_cmd_addr, "gateway_cmd_addr");
+	FW_ADDR_CHECK(gwa_ctl, d->gateway_ctrl_address, "gateway_ctrl_address");
+
+	wil_dbg_fw(wil, "gw addresses: addr 0x%08x val 0x%08x"
+		   " cmd 0x%08x ctl 0x%08x\n",
+		   le32_to_cpu(d->gateway_addr_addr),
+		   le32_to_cpu(d->gateway_value_addr),
+		   le32_to_cpu(d->gateway_cmd_addr),
+		   le32_to_cpu(d->gateway_ctrl_address));
+
+	for (i = 0; i < n; i++) {
+		int rc;
+		u32 a = le32_to_cpu(block[i].addr);
+		u32 v = le32_to_cpu(block[i].value);
+
+		wil_dbg_fw(wil, "  gw write[%3d] [0x%08x] <== 0x%08x\n",
+			   i, a, v);
+
+		iowrite32(v, gwa_val);
+		rc = gw_write(wil, gwa_addr, gwa_cmd, gwa_ctl, gw_cmd, a);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static int fw_handle_gateway_data4(struct wil6210_priv *wil, const void *data,
+				   size_t size)
+{
+	const struct wil_fw_record_gateway_data4 *d = data;
+	const struct wil_fw_data_gw4 *block = d->data;
+	void __iomem *gwa_addr;
+	void __iomem *gwa_val[ARRAY_SIZE(block->value)];
+	void __iomem *gwa_cmd;
+	void __iomem *gwa_ctl;
+	u32 gw_cmd;
+	int n, i, k;
+
+	if (size < sizeof(*d) + sizeof(*block)) {
+		wil_err_fw(wil, "gateway4 record too short: %zu\n", size);
+		return -EINVAL;
+	}
+
+	if ((size - sizeof(*d)) % sizeof(*block)) {
+		wil_err_fw(wil, "gateway4 record data size"
+			   " not aligned on %zu: %zu\n",
+			   sizeof(*block), size - sizeof(*d));
+		return -EINVAL;
+	}
+	n = (size - sizeof(*d)) / sizeof(*block);
+
+	gw_cmd = le32_to_cpu(d->command);
+
+	wil_dbg_fw(wil, "gw4 write record [%3d] blocks, cmd 0x%08x\n",
+		   n, gw_cmd);
+
+	FW_ADDR_CHECK(gwa_addr, d->gateway_addr_addr, "gateway_addr_addr");
+	for (k = 0; k < ARRAY_SIZE(block->value); k++)
+		FW_ADDR_CHECK(gwa_val[k], d->gateway_value_addr[k],
+			      "gateway_value_addr");
+	FW_ADDR_CHECK(gwa_cmd, d->gateway_cmd_addr, "gateway_cmd_addr");
+	FW_ADDR_CHECK(gwa_ctl, d->gateway_ctrl_address, "gateway_ctrl_address");
+
+	wil_dbg_fw(wil, "gw4 addresses: addr 0x%08x cmd 0x%08x ctl 0x%08x\n",
+		   le32_to_cpu(d->gateway_addr_addr),
+		   le32_to_cpu(d->gateway_cmd_addr),
+		   le32_to_cpu(d->gateway_ctrl_address));
+	wil_hex_dump_fw("val addresses: ", DUMP_PREFIX_NONE, 16, 4,
+			d->gateway_value_addr, sizeof(d->gateway_value_addr),
+			false);
+
+	for (i = 0; i < n; i++) {
+		int rc;
+		u32 a = le32_to_cpu(block[i].addr);
+		u32 v[ARRAY_SIZE(block->value)];
+
+		for (k = 0; k < ARRAY_SIZE(block->value); k++)
+			v[k] = le32_to_cpu(block[i].value[k]);
+
+		wil_dbg_fw(wil, "  gw4 write[%3d] [0x%08x] <==\n", i, a);
+		wil_hex_dump_fw("    val ", DUMP_PREFIX_NONE, 16, 4, v,
+				sizeof(v), false);
+
+		for (k = 0; k < ARRAY_SIZE(block->value); k++)
+			iowrite32(v[k], gwa_val[k]);
+		rc = gw_write(wil, gwa_addr, gwa_cmd, gwa_ctl, gw_cmd, a);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+static const struct {
+	int type;
+	int (*handler)(struct wil6210_priv *wil, const void *data, size_t size);
+} wil_fw_handlers[] = {
+	{wil_fw_type_comment, fw_handle_comment},
+	{wil_fw_type_data, fw_handle_data},
+	{wil_fw_type_fill, fw_handle_fill},
+	/* wil_fw_type_action */
+	/* wil_fw_type_verify */
+	{wil_fw_type_file_header, fw_handle_file_header},
+	{wil_fw_type_direct_write, fw_handle_direct_write},
+	{wil_fw_type_gateway_data, fw_handle_gateway_data},
+	{wil_fw_type_gateway_data4, fw_handle_gateway_data4},
+};
+
+static int wil_fw_handle_record(struct wil6210_priv *wil, int type,
+				const void *data, size_t size)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(wil_fw_handlers); i++) {
+		if (wil_fw_handlers[i].type == type)
+			return wil_fw_handlers[i].handler(wil, data, size);
+	}
+
+	wil_err_fw(wil, "unknown record type: %d\n", type);
+	return -EINVAL;
+}
+
+/**
+ * wil_fw_load - load FW into device
+ *
+ * Load the FW and uCode code and data to the corresponding device
+ * memory regions
+ *
+ * Return error code
+ */
+static int wil_fw_load(struct wil6210_priv *wil, const void *data, size_t size)
+{
+	int rc = 0;
+	const struct wil_fw_record_head *hdr;
+	size_t s, hdr_sz;
+
+	for (hdr = data;; hdr = (const void *)hdr + s, size -= s) {
+		if (size < sizeof(*hdr))
+			break;
+		hdr_sz = le32_to_cpu(hdr->size);
+		s = sizeof(*hdr) + hdr_sz;
+		if (s > size)
+			break;
+		if (hdr_sz % 4) {
+			wil_err_fw(wil, "unaligned record size: %zu\n",
+				   hdr_sz);
+			return -EINVAL;
+		}
+		rc = wil_fw_handle_record(wil, le16_to_cpu(hdr->type),
+					  &hdr[1], hdr_sz);
+		if (rc)
+			return rc;
+	}
+	if (size) {
+		wil_err_fw(wil, "unprocessed bytes: %zu\n", size);
+		if (size >= sizeof(*hdr)) {
+			wil_err_fw(wil, "Stop at offset %ld"
+				   " record type %d [%zd bytes]\n",
+				   (const void *)hdr - data,
+				   le16_to_cpu(hdr->type), hdr_sz);
+		}
+		return -EINVAL;
+	}
+	/* Mark FW as loaded from host */
+	S(RGF_USER_USAGE_6, 1);
+
+	return rc;
+}
+
+/**
+ * wil_request_firmware - Request firmware and load to device
+ *
+ * Request firmware image from the file and load it to device
+ *
+ * Return error code
+ */
+int wil_request_firmware(struct wil6210_priv *wil, const char *name)
+{
+	int rc, rc1;
+	const struct firmware *fw;
+	size_t sz;
+	const void *d;
+
+	rc = request_firmware(&fw, name, wil_to_pcie_dev(wil));
+	if (rc) {
+		wil_err_fw(wil, "Failed to load firmware %s\n", name);
+		return rc;
+	}
+	wil_dbg_fw(wil, "Loading <%s>, %zu bytes\n", name, fw->size);
+
+	for (sz = fw->size, d = fw->data; sz; sz -= rc1, d += rc1) {
+		rc1 = wil_fw_verify(wil, d, sz);
+		if (rc1 < 0) {
+			rc = rc1;
+			goto out;
+		}
+		rc = wil_fw_load(wil, d, rc1);
+		if (rc < 0)
+			goto out;
+	}
+
+out:
+	release_firmware(fw);
+	return rc;
+}
diff --git a/drivers/net/wireless/ath/wil6210/interrupt.c b/drivers/net/wireless/ath/wil6210/interrupt.c
index 67f1002a03a1..90f416f239bd 100644
--- a/drivers/net/wireless/ath/wil6210/interrupt.c
+++ b/drivers/net/wireless/ath/wil6210/interrupt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -135,7 +135,7 @@ static void wil6210_unmask_irq_pseudo(struct wil6210_priv *wil)
 		  HOSTADDR(RGF_DMA_PSEUDO_CAUSE_MASK_SW));
 }
 
-void wil6210_disable_irq(struct wil6210_priv *wil)
+void wil_mask_irq(struct wil6210_priv *wil)
 {
 	wil_dbg_irq(wil, "%s()\n", __func__);
 
@@ -145,7 +145,7 @@ void wil6210_disable_irq(struct wil6210_priv *wil)
 	wil6210_mask_irq_pseudo(wil);
 }
 
-void wil6210_enable_irq(struct wil6210_priv *wil)
+void wil_unmask_irq(struct wil6210_priv *wil)
 {
 	wil_dbg_irq(wil, "%s()\n", __func__);
 
@@ -157,17 +157,7 @@ void wil6210_enable_irq(struct wil6210_priv *wil)
 		  offsetof(struct RGF_ICR, ICC));
 
 	/* interrupt moderation parameters */
-	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR) {
-		/* disable interrupt moderation for monitor
-		 * to get better timestamp precision
-		 */
-		iowrite32(0, wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_CRL));
-	} else {
-		iowrite32(WIL6210_ITR_TRSH,
-			  wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_TRSH));
-		iowrite32(BIT_DMA_ITR_CNT_CRL_EN,
-			  wil->csr + HOSTADDR(RGF_DMA_ITR_CNT_CRL));
-	}
+	wil_set_itr_trsh(wil);
 
 	wil6210_unmask_irq_pseudo(wil);
 	wil6210_unmask_irq_tx(wil);
@@ -196,8 +186,13 @@ static irqreturn_t wil6210_irq_rx(int irq, void *cookie)
 		wil_dbg_irq(wil, "RX done\n");
 		isr &= ~BIT_DMA_EP_RX_ICR_RX_DONE;
 		if (test_bit(wil_status_reset_done, &wil->status)) {
-			wil_dbg_txrx(wil, "NAPI(Rx) schedule\n");
-			napi_schedule(&wil->napi_rx);
+			if (test_bit(wil_status_napi_en, &wil->status)) {
+				wil_dbg_txrx(wil, "NAPI(Rx) schedule\n");
+				napi_schedule(&wil->napi_rx);
+			} else {
+				wil_err(wil, "Got Rx interrupt while "
+					"stopping interface\n");
+			}
 		} else {
 			wil_err(wil, "Got Rx interrupt while in reset\n");
 		}
@@ -506,7 +501,8 @@ free0:
 
 	return rc;
 }
-/* can't use wil_ioread32_and_clear because ICC value is not ser yet */
+
+/* can't use wil_ioread32_and_clear because ICC value is not set yet */
 static inline void wil_clear32(void __iomem *addr)
 {
 	u32 x = ioread32(addr);
@@ -522,11 +518,15 @@ void wil6210_clear_irq(struct wil6210_priv *wil)
 		    offsetof(struct RGF_ICR, ICR));
 	wil_clear32(wil->csr + HOSTADDR(RGF_DMA_EP_MISC_ICR) +
 		    offsetof(struct RGF_ICR, ICR));
+	wmb(); /* make sure write completed */
 }
 
 int wil6210_init_irq(struct wil6210_priv *wil, int irq)
 {
 	int rc;
+
+	wil_dbg_misc(wil, "%s() n_msi=%d\n", __func__, wil->n_msi);
+
 	if (wil->n_msi == 3)
 		rc = wil6210_request_3msi(wil, irq);
 	else
@@ -534,17 +534,14 @@ int wil6210_init_irq(struct wil6210_priv *wil, int irq)
 					  wil6210_thread_irq,
 					  wil->n_msi ? 0 : IRQF_SHARED,
 					  WIL_NAME, wil);
-	if (rc)
-		return rc;
-
-	wil6210_enable_irq(wil);
-
-	return 0;
+	return rc;
 }
 
 void wil6210_fini_irq(struct wil6210_priv *wil, int irq)
 {
-	wil6210_disable_irq(wil);
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	wil_mask_irq(wil);
 	free_irq(irq, wil);
 	if (wil->n_msi == 3) {
 		free_irq(irq + 1, wil);
diff --git a/drivers/net/wireless/ath/wil6210/ioctl.c b/drivers/net/wireless/ath/wil6210/ioctl.c
new file mode 100644
index 000000000000..e9c0673819c6
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/ioctl.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/uaccess.h>
+
+#include "wil6210.h"
+#include <uapi/linux/wil6210_uapi.h>
+
+#define wil_hex_dump_ioctl(prefix_str, buf, len) \
+	print_hex_dump_debug("DBG[IOC ]" prefix_str, \
+			     DUMP_PREFIX_OFFSET, 16, 1, buf, len, true)
+#define wil_dbg_ioctl(wil, fmt, arg...) wil_dbg(wil, "DBG[IOC ]" fmt, ##arg)
+
+static void __iomem *wil_ioc_addr(struct wil6210_priv *wil, uint32_t addr,
+				  uint32_t size, enum wil_memio_op op)
+{
+	void __iomem *a;
+	u32 off;
+
+	switch (op & wil_mmio_addr_mask) {
+	case wil_mmio_addr_linker:
+		a = wmi_buffer(wil, cpu_to_le32(addr));
+		break;
+	case wil_mmio_addr_ahb:
+		a = wmi_addr(wil, addr);
+		break;
+	case wil_mmio_addr_bar:
+		a = wmi_addr(wil, addr + WIL6210_FW_HOST_OFF);
+		break;
+	default:
+		wil_err(wil, "Unsupported address mode, op = 0x%08x\n", op);
+		return NULL;
+	}
+
+	off = a - wil->csr;
+	if (size >= WIL6210_MEM_SIZE - off) {
+		wil_err(wil, "Requested block does not fit into memory: "
+			"off = 0x%08x size = 0x%08x\n", off, size);
+		return NULL;
+	}
+
+	return a;
+}
+
+static int wil_ioc_memio_dword(struct wil6210_priv *wil, void __user *data)
+{
+	struct wil_memio io;
+	void __iomem *a;
+	bool need_copy = false;
+
+	if (copy_from_user(&io, data, sizeof(io)))
+		return -EFAULT;
+
+	wil_dbg_ioctl(wil, "IO: addr = 0x%08x val = 0x%08x op = 0x%08x\n",
+		      io.addr, io.val, io.op);
+
+	a = wil_ioc_addr(wil, io.addr, sizeof(u32), io.op);
+	if (!a) {
+		wil_err(wil, "invalid address 0x%08x, op = 0x%08x\n", io.addr,
+			io.op);
+		return -EINVAL;
+	}
+	/* operation */
+	switch (io.op & wil_mmio_op_mask) {
+	case wil_mmio_read:
+		io.val = ioread32(a);
+		need_copy = true;
+		break;
+	case wil_mmio_write:
+		iowrite32(io.val, a);
+		wmb(); /* make sure write propagated to HW */
+		break;
+	default:
+		wil_err(wil, "Unsupported operation, op = 0x%08x\n", io.op);
+		return -EINVAL;
+	}
+
+	if (need_copy) {
+		wil_dbg_ioctl(wil, "IO done: addr = 0x%08x"
+			      " val = 0x%08x op = 0x%08x\n",
+			      io.addr, io.val, io.op);
+		if (copy_to_user(data, &io, sizeof(io)))
+			return -EFAULT;
+	}
+
+	return 0;
+}
+
+static int wil_ioc_memio_block(struct wil6210_priv *wil, void __user *data)
+{
+	struct wil_memio_block io;
+	void *block;
+	void __iomem *a;
+	int rc = 0;
+
+	if (copy_from_user(&io, data, sizeof(io)))
+		return -EFAULT;
+
+	wil_dbg_ioctl(wil, "IO: addr = 0x%08x size = 0x%08x op = 0x%08x\n",
+		      io.addr, io.size, io.op);
+
+	/* size */
+	if (io.size % 4) {
+		wil_err(wil, "size is not multiple of 4:  0x%08x\n", io.size);
+		return -EINVAL;
+	}
+
+	a = wil_ioc_addr(wil, io.addr, io.size, io.op);
+	if (!a) {
+		wil_err(wil, "invalid address 0x%08x, op = 0x%08x\n", io.addr,
+			io.op);
+		return -EINVAL;
+	}
+
+	block = kmalloc(io.size, GFP_USER);
+	if (!block)
+		return -ENOMEM;
+
+	/* operation */
+	switch (io.op & wil_mmio_op_mask) {
+	case wil_mmio_read:
+		wil_memcpy_fromio_32(block, a, io.size);
+		wil_hex_dump_ioctl("Read  ", block, io.size);
+		if (copy_to_user(io.block, block, io.size)) {
+			rc = -EFAULT;
+			goto out_free;
+		}
+		break;
+	case wil_mmio_write:
+		if (copy_from_user(block, io.block, io.size)) {
+			rc = -EFAULT;
+			goto out_free;
+		}
+		wil_memcpy_toio_32(a, block, io.size);
+		wmb(); /* make sure write propagated to HW */
+		wil_hex_dump_ioctl("Write ", block, io.size);
+		break;
+	default:
+		wil_err(wil, "Unsupported operation, op = 0x%08x\n", io.op);
+		rc = -EINVAL;
+		break;
+	}
+
+out_free:
+	kfree(block);
+	return rc;
+}
+
+int wil_ioctl(struct wil6210_priv *wil, void __user *data, int cmd)
+{
+	switch (cmd) {
+	case WIL_IOCTL_MEMIO:
+		return wil_ioc_memio_dword(wil, data);
+	case WIL_IOCTL_MEMIO_BLOCK:
+		return wil_ioc_memio_block(wil, data);
+	default:
+		wil_dbg_ioctl(wil, "Unsupported IOCTL 0x%04x\n", cmd);
+		return -ENOIOCTLCMD;
+	}
+}
diff --git a/drivers/net/wireless/ath/wil6210/main.c b/drivers/net/wireless/ath/wil6210/main.c
index 3704d2a434f3..6500caf8d609 100644
--- a/drivers/net/wireless/ath/wil6210/main.c
+++ b/drivers/net/wireless/ath/wil6210/main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -20,10 +20,26 @@
 
 #include "wil6210.h"
 #include "txrx.h"
+#include "wmi.h"
 
-static bool no_fw_recovery;
+#define WAIT_FOR_DISCONNECT_TIMEOUT_MS 2000
+#define WAIT_FOR_DISCONNECT_INTERVAL_MS 10
+
+bool no_fw_recovery;
 module_param(no_fw_recovery, bool, S_IRUGO | S_IWUSR);
-MODULE_PARM_DESC(no_fw_recovery, " disable FW error recovery");
+MODULE_PARM_DESC(no_fw_recovery, " disable automatic FW error recovery");
+
+static bool no_fw_load = true;
+module_param(no_fw_load, bool, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(no_fw_load, " do not download FW, use one in on-card flash.");
+
+static unsigned int itr_trsh = WIL6210_ITR_TRSH_DEFAULT;
+
+module_param(itr_trsh, uint, S_IRUGO);
+MODULE_PARM_DESC(itr_trsh, " Interrupt moderation threshold, usecs.");
+
+#define RST_DELAY (20) /* msec, for loop in @wil_target_reset */
+#define RST_COUNT (1 + 1000/RST_DELAY) /* round up to be above 1 sec total */
 
 /*
  * Due to a hardware issue,
@@ -64,6 +80,7 @@ static void wil_disconnect_cid(struct wil6210_priv *wil, int cid)
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct wireless_dev *wdev = wil->wdev;
 	struct wil_sta_info *sta = &wil->sta[cid];
+
 	wil_dbg_misc(wil, "%s(CID %d, status %d)\n", __func__, cid,
 		     sta->status);
 
@@ -83,9 +100,16 @@ static void wil_disconnect_cid(struct wil6210_priv *wil, int cid)
 	}
 
 	for (i = 0; i < WIL_STA_TID_NUM; i++) {
-		struct wil_tid_ampdu_rx *r = sta->tid_rx[i];
+		struct wil_tid_ampdu_rx *r;
+		unsigned long flags;
+
+		spin_lock_irqsave(&sta->tid_rx_lock, flags);
+
+		r = sta->tid_rx[i];
 		sta->tid_rx[i] = NULL;
 		wil_tid_ampdu_rx_free(wil, r);
+
+		spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
 	}
 	for (i = 0; i < ARRAY_SIZE(wil->vring_tx); i++) {
 		if (wil->vring2cid_tid[i][0] == cid)
@@ -167,17 +191,38 @@ static void wil_scan_timer_fn(ulong x)
 	schedule_work(&wil->fw_error_worker);
 }
 
+static int wil_wait_for_recovery(struct wil6210_priv *wil)
+{
+	if (wait_event_interruptible(wil->wq, wil->recovery_state !=
+				     fw_recovery_pending)) {
+		wil_err(wil, "Interrupt, canceling recovery\n");
+		return -ERESTARTSYS;
+	}
+	if (wil->recovery_state != fw_recovery_running) {
+		wil_info(wil, "Recovery cancelled\n");
+		return -EINTR;
+	}
+	wil_info(wil, "Proceed with recovery\n");
+	return 0;
+}
+
+void wil_set_recovery_state(struct wil6210_priv *wil, int state)
+{
+	wil_dbg_misc(wil, "%s(%d -> %d)\n", __func__,
+		     wil->recovery_state, state);
+
+	wil->recovery_state = state;
+	wake_up_interruptible(&wil->wq);
+}
+
 static void wil_fw_error_worker(struct work_struct *work)
 {
-	struct wil6210_priv *wil = container_of(work,
-			struct wil6210_priv, fw_error_worker);
+	struct wil6210_priv *wil = container_of(work, struct wil6210_priv,
+						fw_error_worker);
 	struct wireless_dev *wdev = wil->wdev;
 
 	wil_dbg_misc(wil, "fw error worker\n");
 
-	if (no_fw_recovery)
-		return;
-
 	/* increment @recovery_count if less then WIL6210_FW_RECOVERY_TO
 	 * passed since last recovery attempt
 	 */
@@ -200,12 +245,15 @@ static void wil_fw_error_worker(struct work_struct *work)
 	case NL80211_IFTYPE_STATION:
 	case NL80211_IFTYPE_P2P_CLIENT:
 	case NL80211_IFTYPE_MONITOR:
-		wil_info(wil, "fw error recovery started (try %d)...\n",
+		wil_info(wil, "fw error recovery requested (try %d)...\n",
 			 wil->recovery_count);
-		wil_reset(wil);
+		if (!no_fw_recovery)
+			wil->recovery_state = fw_recovery_running;
+		if (0 != wil_wait_for_recovery(wil))
+			break;
 
-		/* need to re-allocate Rx ring after reset */
-		wil_rx_init(wil);
+		__wil_down(wil);
+		__wil_up(wil);
 		break;
 	case NL80211_IFTYPE_AP:
 	case NL80211_IFTYPE_P2P_GO:
@@ -220,6 +268,7 @@ static void wil_fw_error_worker(struct work_struct *work)
 static int wil_find_free_vring(struct wil6210_priv *wil)
 {
 	int i;
+
 	for (i = 0; i < WIL6210_MAX_TX_RINGS; i++) {
 		if (!wil->vring_tx[i].va)
 			return i;
@@ -254,14 +303,19 @@ static void wil_connect_worker(struct work_struct *work)
 
 int wil_priv_init(struct wil6210_priv *wil)
 {
+	uint i;
+
 	wil_dbg_misc(wil, "%s()\n", __func__);
 
 	memset(wil->sta, 0, sizeof(wil->sta));
+	for (i = 0; i < WIL6210_MAX_CID; i++)
+		spin_lock_init(&wil->sta[i].tid_rx_lock);
 
 	mutex_init(&wil->mutex);
 	mutex_init(&wil->wmi_mutex);
 
 	init_completion(&wil->wmi_ready);
+	init_completion(&wil->wmi_call);
 
 	wil->pending_connect_cid = -1;
 	setup_timer(&wil->connect_timer, wil_connect_timer_fn, (ulong)wil);
@@ -274,6 +328,7 @@ int wil_priv_init(struct wil6210_priv *wil)
 
 	INIT_LIST_HEAD(&wil->pending_wmi_ev);
 	spin_lock_init(&wil->wmi_ev_lock);
+	init_waitqueue_head(&wil->wq);
 
 	wil->wmi_wq = create_singlethread_workqueue(WIL_NAME"_wmi");
 	if (!wil->wmi_wq)
@@ -286,18 +341,24 @@ int wil_priv_init(struct wil6210_priv *wil)
 	}
 
 	wil->last_fw_recovery = jiffies;
+	wil->itr_trsh = itr_trsh;
 
 	return 0;
 }
 
 void wil6210_disconnect(struct wil6210_priv *wil, const u8 *bssid)
 {
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	del_timer_sync(&wil->connect_timer);
 	_wil6210_disconnect(wil, bssid);
 }
 
 void wil_priv_deinit(struct wil6210_priv *wil)
 {
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	wil_set_recovery_state(wil, fw_recovery_idle);
 	del_timer_sync(&wil->scan_timer);
 	cancel_work_sync(&wil->disconnect_worker);
 	cancel_work_sync(&wil->fw_error_worker);
@@ -309,7 +370,29 @@ void wil_priv_deinit(struct wil6210_priv *wil)
 	destroy_workqueue(wil->wmi_wq);
 }
 
-static void wil_target_reset(struct wil6210_priv *wil)
+/* target operations */
+/* register read */
+#define R(a) ioread32(wil->csr + HOSTADDR(a))
+/* register write. wmb() to make sure it is completed */
+#define W(a, v) do { iowrite32(v, wil->csr + HOSTADDR(a)); wmb(); } while (0)
+/* register set = read, OR, write */
+#define S(a, v) W(a, R(a) | v)
+/* register clear = read, AND with inverted, write */
+#define C(a, v) W(a, R(a) & ~v)
+
+static inline void wil_halt_cpu(struct wil6210_priv *wil)
+{
+	W(RGF_USER_USER_CPU_0, BIT_USER_USER_CPU_MAN_RST);
+	W(RGF_USER_MAC_CPU_0,  BIT_USER_MAC_CPU_MAN_RST);
+}
+
+static inline void wil_release_cpu(struct wil6210_priv *wil)
+{
+	/* Start CPU */
+	W(RGF_USER_USER_CPU_0, 1);
+}
+
+static int wil_target_reset(struct wil6210_priv *wil)
 {
 	int delay = 0;
 	u32 hw_state;
@@ -318,57 +401,41 @@ static void wil_target_reset(struct wil6210_priv *wil)
 
 	wil_dbg_misc(wil, "Resetting \"%s\"...\n", wil->board->name);
 
-	/* register read */
-#define R(a) ioread32(wil->csr + HOSTADDR(a))
-	/* register write */
-#define W(a, v) iowrite32(v, wil->csr + HOSTADDR(a))
-	/* register set = read, OR, write */
-#define S(a, v) W(a, R(a) | v)
-	/* register clear = read, AND with inverted, write */
-#define C(a, v) W(a, R(a) & ~v)
-
 	wil->hw_version = R(RGF_USER_FW_REV_ID);
 	rev_id = wil->hw_version & 0xff;
 
 	/* Clear MAC link up */
 	S(RGF_HP_CTRL, BIT(15));
-	/* hpal_perst_from_pad_src_n_mask */
-	S(RGF_USER_CLKS_CTL_SW_RST_MASK_0, BIT(6));
-	/* car_perst_rst_src_n_mask */
-	S(RGF_USER_CLKS_CTL_SW_RST_MASK_0, BIT(7));
-	wmb(); /* order is important here */
+	S(RGF_USER_CLKS_CTL_SW_RST_MASK_0, BIT_HPAL_PERST_FROM_PAD);
+	S(RGF_USER_CLKS_CTL_SW_RST_MASK_0, BIT_CAR_PERST_RST);
+
+	wil_halt_cpu(wil);
+	C(RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_CAR_AHB_SW_SEL); /* 40 MHz */
 
 	if (is_sparrow) {
 		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0, 0x3ff81f);
-		wmb(); /* order is important here */
+		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1, 0xf);
 	}
 
-	W(RGF_USER_MAC_CPU_0,  BIT(1)); /* mac_cpu_man_rst */
-	W(RGF_USER_USER_CPU_0, BIT(1)); /* user_cpu_man_rst */
-	wmb(); /* order is important here */
-
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0xFE000000);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0x0000003F);
-	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, is_sparrow ? 0x000000B0 : 0x00000170);
-	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0xFFE7FC00);
-	wmb(); /* order is important here */
+	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, is_sparrow ? 0x000000f0 : 0x00000170);
+	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0xFFE7FE00);
 
 	if (is_sparrow) {
 		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0, 0x0);
-		wmb(); /* order is important here */
+		W(RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1, 0x0);
 	}
 
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_1, 0);
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0);
-	wmb(); /* order is important here */
 
 	if (is_sparrow) {
 		W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000003);
 		/* reset A2 PCIE AHB */
 		W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000);
-
 	} else {
 		W(RGF_USER_CLKS_CTL_SW_RST_VEC_3, 0x00000001);
 		if (rev_id == 1) {
@@ -378,21 +445,19 @@ static void wil_target_reset(struct wil6210_priv *wil)
 			W(RGF_PCIE_LOS_COUNTER_CTL, BIT(6) | BIT(8));
 			W(RGF_USER_CLKS_CTL_SW_RST_VEC_2, 0x00008000);
 		}
-
 	}
 
 	/* TODO: check order here!!! Erez code is different */
 	W(RGF_USER_CLKS_CTL_SW_RST_VEC_0, 0);
-	wmb(); /* order is important here */
 
-	/* wait until device ready */
+	/* wait until device ready. typical time is 200..250 msec */
 	do {
-		msleep(1);
+		msleep(RST_DELAY);
 		hw_state = R(RGF_USER_HW_MACHINE_STATE);
-		if (delay++ > 100) {
+		if (delay++ > RST_COUNT) {
 			wil_err(wil, "Reset not completed, hw_state 0x%08x\n",
 				hw_state);
-			return;
+			return -ETIME;
 		}
 	} while (hw_state != HW_MACHINE_BOOT_DONE);
 
@@ -401,15 +466,35 @@ static void wil_target_reset(struct wil6210_priv *wil)
 		W(RGF_PCIE_LOS_COUNTER_CTL, BIT(8));
 
 	C(RGF_USER_CLKS_CTL_0, BIT_USER_CLKS_RST_PWGD);
-	wmb(); /* order is important here */
 
-	wil_dbg_misc(wil, "Reset completed in %d ms\n", delay);
+	wil_dbg_misc(wil, "Reset completed in %d ms\n", delay * RST_DELAY);
+	return 0;
+}
+
+/**
+ * wil_set_itr_trsh: - apply interrupt coalescing params
+ */
+void wil_set_itr_trsh(struct wil6210_priv *wil)
+{
+	/* disable, use usec resolution */
+	W(RGF_DMA_ITR_CNT_CRL, BIT_DMA_ITR_CNT_CRL_EXT_TICK);
+
+	/* disable interrupt moderation for monitor
+	 * to get better timestamp precision
+	 */
+	if (wil->wdev->iftype == NL80211_IFTYPE_MONITOR)
+		return;
+
+	wil_info(wil, "set ITR_TRSH = %d usec\n", wil->itr_trsh);
+	W(RGF_DMA_ITR_CNT_TRSH, wil->itr_trsh);
+	W(RGF_DMA_ITR_CNT_CRL, BIT_DMA_ITR_CNT_CRL_EN |
+	  BIT_DMA_ITR_CNT_CRL_EXT_TICK); /* start it */
+}
 
 #undef R
 #undef W
 #undef S
 #undef C
-}
 
 void wil_mbox_ring_le2cpus(struct wil6210_mbox_ring *r)
 {
@@ -424,6 +509,7 @@ static int wil_wait_for_fw_ready(struct wil6210_priv *wil)
 {
 	ulong to = msecs_to_jiffies(1000);
 	ulong left = wait_for_completion_timeout(&wil->wmi_ready, to);
+
 	if (0 == left) {
 		wil_err(wil, "Firmware not ready\n");
 		return -ETIME;
@@ -443,15 +529,15 @@ int wil_reset(struct wil6210_priv *wil)
 {
 	int rc;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	WARN_ON(!mutex_is_locked(&wil->mutex));
+	WARN_ON(test_bit(wil_status_napi_en, &wil->status));
 
 	cancel_work_sync(&wil->disconnect_worker);
 	wil6210_disconnect(wil, NULL);
 
 	wil->status = 0; /* prevent NAPI from being scheduled */
-	if (test_bit(wil_status_napi_en, &wil->status)) {
-		napi_synchronize(&wil->napi_rx);
-	}
 
 	if (wil->scan_request) {
 		wil_dbg_misc(wil, "Abort scan_request 0x%p\n",
@@ -461,24 +547,50 @@ int wil_reset(struct wil6210_priv *wil)
 		wil->scan_request = NULL;
 	}
 
-	wil6210_disable_irq(wil);
+	wil_mask_irq(wil);
 
 	wmi_event_flush(wil);
 
 	flush_workqueue(wil->wmi_wq_conn);
 	flush_workqueue(wil->wmi_wq);
 
-	/* TODO: put MAC in reset */
-	wil_target_reset(wil);
-
+	rc = wil_target_reset(wil);
 	wil_rx_fini(wil);
+	if (rc)
+		return rc;
+
+	if (!no_fw_load) {
+		wil_info(wil, "Use firmware <%s>\n", WIL_FW_NAME);
+		wil_halt_cpu(wil);
+		/* Loading f/w from the file */
+		rc = wil_request_firmware(wil, WIL_FW_NAME);
+		if (rc)
+			return rc;
+
+		/* clear any interrupts which on-card-firmware may have set */
+		wil6210_clear_irq(wil);
+		{ /* CAF_ICR - clear and mask */
+			u32 a = HOSTADDR(RGF_CAF_ICR) +
+				offsetof(struct RGF_ICR, ICR);
+			u32 m = HOSTADDR(RGF_CAF_ICR) +
+				offsetof(struct RGF_ICR, IMV);
+			u32 icr = ioread32(wil->csr + a);
+
+			iowrite32(icr, wil->csr + a); /* W1C */
+			iowrite32(~0, wil->csr + m);
+			wmb(); /* wait for completion */
+		}
+		wil_release_cpu(wil);
+	} else {
+		wil_info(wil, "Use firmware from on-card flash\n");
+	}
 
 	/* init after reset */
 	wil->pending_connect_cid = -1;
 	reinit_completion(&wil->wmi_ready);
+	reinit_completion(&wil->wmi_call);
 
-	/* TODO: release MAC reset */
-	wil6210_enable_irq(wil);
+	wil_unmask_irq(wil);
 
 	/* we just started MAC, wait for FW ready */
 	rc = wil_wait_for_fw_ready(wil);
@@ -489,6 +601,7 @@ int wil_reset(struct wil6210_priv *wil)
 void wil_fw_error_recovery(struct wil6210_priv *wil)
 {
 	wil_dbg_misc(wil, "starting fw error recovery\n");
+	wil->recovery_state = fw_recovery_pending;
 	schedule_work(&wil->fw_error_worker);
 }
 
@@ -514,7 +627,7 @@ void wil_link_off(struct wil6210_priv *wil)
 	netif_carrier_off(ndev);
 }
 
-static int __wil_up(struct wil6210_priv *wil)
+int __wil_up(struct wil6210_priv *wil)
 {
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct wireless_dev *wdev = wil->wdev;
@@ -560,11 +673,15 @@ static int __wil_up(struct wil6210_priv *wil)
 	/* MAC address - pre-requisite for other commands */
 	wmi_set_mac_address(wil, ndev->dev_addr);
 
-
+	wil_dbg_misc(wil, "NAPI enable\n");
 	napi_enable(&wil->napi_rx);
 	napi_enable(&wil->napi_tx);
 	set_bit(wil_status_napi_en, &wil->status);
 
+	if (wil->platform_ops.bus_request)
+		wil->platform_ops.bus_request(wil->platform_handle,
+					      WIL_MAX_BUS_REQUEST_KBPS);
+
 	return 0;
 }
 
@@ -572,6 +689,8 @@ int wil_up(struct wil6210_priv *wil)
 {
 	int rc;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	mutex_lock(&wil->mutex);
 	rc = __wil_up(wil);
 	mutex_unlock(&wil->mutex);
@@ -579,13 +698,23 @@ int wil_up(struct wil6210_priv *wil)
 	return rc;
 }
 
-static int __wil_down(struct wil6210_priv *wil)
+int __wil_down(struct wil6210_priv *wil)
 {
+	int iter = WAIT_FOR_DISCONNECT_TIMEOUT_MS /
+			WAIT_FOR_DISCONNECT_INTERVAL_MS;
+
 	WARN_ON(!mutex_is_locked(&wil->mutex));
 
-	clear_bit(wil_status_napi_en, &wil->status);
-	napi_disable(&wil->napi_rx);
-	napi_disable(&wil->napi_tx);
+	if (wil->platform_ops.bus_request)
+		wil->platform_ops.bus_request(wil->platform_handle, 0);
+
+	wil_disable_irq(wil);
+	if (test_and_clear_bit(wil_status_napi_en, &wil->status)) {
+		napi_disable(&wil->napi_rx);
+		napi_disable(&wil->napi_tx);
+		wil_dbg_misc(wil, "NAPI disable\n");
+	}
+	wil_enable_irq(wil);
 
 	if (wil->scan_request) {
 		wil_dbg_misc(wil, "Abort scan_request 0x%p\n",
@@ -595,7 +724,24 @@ static int __wil_down(struct wil6210_priv *wil)
 		wil->scan_request = NULL;
 	}
 
-	wil6210_disconnect(wil, NULL);
+	if (test_bit(wil_status_fwconnected, &wil->status) ||
+	    test_bit(wil_status_fwconnecting, &wil->status))
+		wmi_send(wil, WMI_DISCONNECT_CMDID, NULL, 0);
+
+	/* make sure wil is idle (not connected) */
+	mutex_unlock(&wil->mutex);
+	while (iter--) {
+		int idle = !test_bit(wil_status_fwconnected, &wil->status) &&
+			   !test_bit(wil_status_fwconnecting, &wil->status);
+		if (idle)
+			break;
+		msleep(WAIT_FOR_DISCONNECT_INTERVAL_MS);
+	}
+	mutex_lock(&wil->mutex);
+
+	if (!iter)
+		wil_err(wil, "timeout waiting for idle FW/HW\n");
+
 	wil_rx_fini(wil);
 
 	return 0;
@@ -605,6 +751,9 @@ int wil_down(struct wil6210_priv *wil)
 {
 	int rc;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
+	wil_set_recovery_state(wil, fw_recovery_idle);
 	mutex_lock(&wil->mutex);
 	rc = __wil_down(wil);
 	mutex_unlock(&wil->mutex);
diff --git a/drivers/net/wireless/ath/wil6210/netdev.c b/drivers/net/wireless/ath/wil6210/netdev.c
index 7afce6e8c507..239965106c05 100644
--- a/drivers/net/wireless/ath/wil6210/netdev.c
+++ b/drivers/net/wireless/ath/wil6210/netdev.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -17,11 +17,14 @@
 #include <linux/etherdevice.h>
 
 #include "wil6210.h"
+#include "txrx.h"
 
 static int wil_open(struct net_device *ndev)
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	return wil_up(wil);
 }
 
@@ -29,6 +32,8 @@ static int wil_stop(struct net_device *ndev)
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	return wil_down(wil);
 }
 
@@ -36,8 +41,10 @@ static int wil_change_mtu(struct net_device *ndev, int new_mtu)
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
 
-	if (new_mtu < 68 || new_mtu > IEEE80211_MAX_DATA_LEN_DMG)
+	if (new_mtu < 68 || new_mtu > (TX_BUF_LEN - ETH_HLEN)) {
+		wil_err(wil, "invalid MTU %d\n", new_mtu);
 		return -EINVAL;
+	}
 
 	wil_dbg_misc(wil, "change MTU %d -> %d\n", ndev->mtu, new_mtu);
 	ndev->mtu = new_mtu;
@@ -45,6 +52,17 @@ static int wil_change_mtu(struct net_device *ndev, int new_mtu)
 	return 0;
 }
 
+static int wil_do_ioctl(struct net_device *ndev, struct ifreq *ifr, int cmd)
+{
+	struct wil6210_priv *wil = ndev_to_wil(ndev);
+
+	int ret = wil_ioctl(wil, ifr->ifr_data, cmd);
+
+	wil_dbg_misc(wil, "ioctl(0x%04x) -> %d\n", cmd, ret);
+
+	return ret;
+}
+
 static const struct net_device_ops wil_netdev_ops = {
 	.ndo_open		= wil_open,
 	.ndo_stop		= wil_stop,
@@ -52,6 +70,7 @@ static const struct net_device_ops wil_netdev_ops = {
 	.ndo_set_mac_address	= eth_mac_addr,
 	.ndo_validate_addr	= eth_validate_addr,
 	.ndo_change_mtu		= wil_change_mtu,
+	.ndo_do_ioctl		= wil_do_ioctl,
 };
 
 static int wil6210_netdev_poll_rx(struct napi_struct *napi, int budget)
@@ -121,6 +140,8 @@ void *wil_if_alloc(struct device *dev, void __iomem *csr)
 	wil->csr = csr;
 	wil->wdev = wdev;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	rc = wil_priv_init(wil);
 	if (rc) {
 		dev_err(dev, "wil_priv_init failed\n");
@@ -140,6 +161,7 @@ void *wil_if_alloc(struct device *dev, void __iomem *csr)
 	}
 
 	ndev->netdev_ops = &wil_netdev_ops;
+	wil_set_ethtoolops(ndev);
 	ndev->ieee80211_ptr = wdev;
 	ndev->hw_features = NETIF_F_HW_CSUM | NETIF_F_RXCSUM |
 			    NETIF_F_SG | NETIF_F_GRO;
@@ -168,11 +190,17 @@ void *wil_if_alloc(struct device *dev, void __iomem *csr)
 void wil_if_free(struct wil6210_priv *wil)
 {
 	struct net_device *ndev = wil_to_ndev(wil);
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	if (!ndev)
 		return;
 
-	free_netdev(ndev);
 	wil_priv_deinit(wil);
+
+	wil_to_ndev(wil) = NULL;
+	free_netdev(ndev);
+
 	wil_wdev_free(wil);
 }
 
@@ -181,6 +209,8 @@ int wil_if_add(struct wil6210_priv *wil)
 	struct net_device *ndev = wil_to_ndev(wil);
 	int rc;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	rc = register_netdev(ndev);
 	if (rc < 0) {
 		dev_err(&ndev->dev, "Failed to register netdev: %d\n", rc);
@@ -196,5 +226,7 @@ void wil_if_remove(struct wil6210_priv *wil)
 {
 	struct net_device *ndev = wil_to_ndev(wil);
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	unregister_netdev(ndev);
 }
diff --git a/drivers/net/wireless/ath/wil6210/pcie_bus.c b/drivers/net/wireless/ath/wil6210/pcie_bus.c
index d3fbfa28db62..66626a8ee728 100644
--- a/drivers/net/wireless/ath/wil6210/pcie_bus.c
+++ b/drivers/net/wireless/ath/wil6210/pcie_bus.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -17,6 +17,7 @@
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/moduleparam.h>
+#include <linux/interrupt.h>
 
 #include "wil6210.h"
 
@@ -30,6 +31,28 @@ static bool debug_fw; /* = false; */
 module_param(debug_fw, bool, S_IRUGO);
 MODULE_PARM_DESC(debug_fw, " load driver if FW not ready. For FW debug");
 
+void wil_disable_irq(struct wil6210_priv *wil)
+{
+	int irq = wil->pdev->irq;
+
+	disable_irq(irq);
+	if (wil->n_msi == 3) {
+		disable_irq(irq + 1);
+		disable_irq(irq + 2);
+	}
+}
+
+void wil_enable_irq(struct wil6210_priv *wil)
+{
+	int irq = wil->pdev->irq;
+
+	enable_irq(irq);
+	if (wil->n_msi == 3) {
+		enable_irq(irq + 1);
+		enable_irq(irq + 2);
+	}
+}
+
 /* Bus ops */
 static int wil_if_pcie_enable(struct wil6210_priv *wil)
 {
@@ -41,6 +64,8 @@ static int wil_if_pcie_enable(struct wil6210_priv *wil)
 	 */
 	int msi_only = pdev->msi_enabled;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	pdev->msi_enabled = 0;
 
 	pci_set_master(pdev);
@@ -107,6 +132,8 @@ static int wil_if_pcie_disable(struct wil6210_priv *wil)
 {
 	struct pci_dev *pdev = wil->pdev;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	pci_clear_master(pdev);
 	/* disable and release IRQ */
 	wil6210_fini_irq(wil, pdev->irq);
@@ -180,6 +207,10 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	wil->board = board;
 
 	wil6210_clear_irq(wil);
+
+	wil->platform_handle =
+			wil_platform_init(&pdev->dev, &wil->platform_ops);
+
 	/* FW should raise IRQ when ready */
 	rc = wil_if_pcie_enable(wil);
 	if (rc) {
@@ -204,6 +235,8 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
  bus_disable:
 	wil_if_pcie_disable(wil);
  if_free:
+	if (wil->platform_ops.uninit)
+		wil->platform_ops.uninit(wil->platform_handle);
 	wil_if_free(wil);
  err_iounmap:
 	pci_iounmap(pdev, csr);
@@ -218,12 +251,17 @@ static int wil_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 static void wil_pcie_remove(struct pci_dev *pdev)
 {
 	struct wil6210_priv *wil = pci_get_drvdata(pdev);
+	void __iomem *csr = wil->csr;
+
+	wil_dbg_misc(wil, "%s()\n", __func__);
 
 	wil6210_debugfs_remove(wil);
-	wil_if_pcie_disable(wil);
 	wil_if_remove(wil);
+	wil_if_pcie_disable(wil);
+	if (wil->platform_ops.uninit)
+		wil->platform_ops.uninit(wil->platform_handle);
 	wil_if_free(wil);
-	pci_iounmap(pdev, wil->csr);
+	pci_iounmap(pdev, csr);
 	pci_release_region(pdev, 0);
 	pci_disable_device(pdev);
 }
@@ -243,6 +281,8 @@ static const struct pci_device_id wil6210_pcie_ids[] = {
 	  .driver_data = (kernel_ulong_t)&wil_board_marlon },
 	{ PCI_DEVICE(0x1ae9, 0x0310),
 	  .driver_data = (kernel_ulong_t)&wil_board_sparrow },
+	{ PCI_DEVICE(0x1ae9, 0x0302), /* same as above, firmware broken */
+	  .driver_data = (kernel_ulong_t)&wil_board_sparrow },
 	{ /* end: all zeroes */	},
 };
 MODULE_DEVICE_TABLE(pci, wil6210_pcie_ids);
diff --git a/drivers/net/wireless/ath/wil6210/rx_reorder.c b/drivers/net/wireless/ath/wil6210/rx_reorder.c
index 180ca4793904..489cb73d139b 100644
--- a/drivers/net/wireless/ath/wil6210/rx_reorder.c
+++ b/drivers/net/wireless/ath/wil6210/rx_reorder.c
@@ -1,3 +1,19 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
 #include "wil6210.h"
 #include "txrx.h"
 
@@ -82,22 +98,25 @@ void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
 	int mid = wil_rxdesc_mid(d);
 	u16 seq = wil_rxdesc_seq(d);
 	struct wil_sta_info *sta = &wil->sta[cid];
-	struct wil_tid_ampdu_rx *r = sta->tid_rx[tid];
+	struct wil_tid_ampdu_rx *r;
 	u16 hseq;
 	int index;
+	unsigned long flags;
 
 	wil_dbg_txrx(wil, "MID %d CID %d TID %d Seq 0x%03x\n",
 		     mid, cid, tid, seq);
 
+	spin_lock_irqsave(&sta->tid_rx_lock, flags);
+
+	r = sta->tid_rx[tid];
 	if (!r) {
+		spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
 		wil_netif_rx_any(skb, ndev);
 		return;
 	}
 
 	hseq = r->head_seq_num;
 
-	spin_lock(&r->reorder_lock);
-
 	/** Due to the race between WMI events, where BACK establishment
 	 * reported, and data Rx, few packets may be pass up before reorder
 	 * buffer get allocated. Catch up by pretending SSN is what we
@@ -160,13 +179,14 @@ void wil_rx_reorder(struct wil6210_priv *wil, struct sk_buff *skb)
 	wil_reorder_release(wil, r);
 
 out:
-	spin_unlock(&r->reorder_lock);
+	spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
 }
 
 struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
 						int size, u16 ssn)
 {
 	struct wil_tid_ampdu_rx *r = kzalloc(sizeof(*r), GFP_KERNEL);
+
 	if (!r)
 		return NULL;
 
@@ -181,7 +201,6 @@ struct wil_tid_ampdu_rx *wil_tid_ampdu_rx_alloc(struct wil6210_priv *wil,
 		return NULL;
 	}
 
-	spin_lock_init(&r->reorder_lock);
 	r->ssn = ssn;
 	r->head_seq_num = ssn;
 	r->buf_size = size;
diff --git a/drivers/net/wireless/ath/wil6210/txrx.c b/drivers/net/wireless/ath/wil6210/txrx.c
index d3467943d39d..2936ef0c18cb 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.c
+++ b/drivers/net/wireless/ath/wil6210/txrx.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -52,6 +52,7 @@ static inline int wil_vring_is_full(struct vring *vring)
 {
 	return wil_vring_next_tail(vring) == vring->swhead;
 }
+
 /*
  * Available space in Tx Vring
  */
@@ -86,6 +87,8 @@ static int wil_vring_alloc(struct wil6210_priv *wil, struct vring *vring)
 	size_t sz = vring->size * sizeof(vring->va[0]);
 	uint i;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	BUILD_BUG_ON(sizeof(vring->va[0]) != 32);
 
 	vring->swhead = 0;
@@ -110,7 +113,8 @@ static int wil_vring_alloc(struct wil6210_priv *wil, struct vring *vring)
 	 * we can use any
 	 */
 	for (i = 0; i < vring->size; i++) {
-		volatile struct vring_tx_desc *_d = &(vring->va[i].tx);
+		volatile struct vring_tx_desc *_d = &vring->va[i].tx;
+
 		_d->dma.status = TX_DMA_STATUS_DU;
 	}
 
@@ -125,6 +129,7 @@ static void wil_txdesc_unmap(struct device *dev, struct vring_tx_desc *d,
 {
 	dma_addr_t pa = wil_desc_addr(&d->dma.addr);
 	u16 dmalen = le16_to_cpu(d->dma.length);
+
 	switch (ctx->mapped_as) {
 	case wil_mapped_as_single:
 		dma_unmap_single(dev, pa, dmalen, DMA_TO_DEVICE);
@@ -143,6 +148,18 @@ static void wil_vring_free(struct wil6210_priv *wil, struct vring *vring,
 	struct device *dev = wil_to_dev(wil);
 	size_t sz = vring->size * sizeof(vring->va[0]);
 
+	if (tx) {
+		int vring_index = vring - wil->vring_tx;
+
+		wil_dbg_misc(wil, "free Tx vring %d [%d] 0x%p:%pad 0x%p\n",
+			     vring_index, vring->size, vring->va,
+			     &vring->pa, vring->ctx);
+	} else {
+		wil_dbg_misc(wil, "free Rx vring [%d] 0x%p:%pad 0x%p\n",
+			     vring->size, vring->va,
+			     &vring->pa, vring->ctx);
+	}
+
 	while (!wil_vring_is_empty(vring)) {
 		dma_addr_t pa;
 		u16 dmalen;
@@ -191,11 +208,12 @@ static int wil_vring_alloc_skb(struct wil6210_priv *wil, struct vring *vring,
 	struct device *dev = wil_to_dev(wil);
 	unsigned int sz = RX_BUF_LEN;
 	struct vring_rx_desc dd, *d = &dd;
-	volatile struct vring_rx_desc *_d = &(vring->va[i].rx);
+	volatile struct vring_rx_desc *_d = &vring->va[i].rx;
 	dma_addr_t pa;
 
 	/* TODO align */
 	struct sk_buff *skb = dev_alloc_skb(sz + headroom);
+
 	if (unlikely(!skb))
 		return -ENOMEM;
 
@@ -274,9 +292,11 @@ static void wil_rx_add_radiotap_header(struct wil6210_priv *wil,
 			 */
 			int len = min_t(int, 8 + sizeof(phy_data),
 					wil_rxdesc_phy_length(d));
+
 			if (len > 8) {
 				void *p = skb_tail_pointer(skb);
 				void *pa = PTR_ALIGN(p, 8);
+
 				if (skb_tailroom(skb) >= len + (pa - p)) {
 					phy_length = len - 8;
 					memcpy(phy_data, pa, phy_length);
@@ -372,13 +392,12 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	int cid;
 	struct wil_net_stats *stats;
 
-
 	BUILD_BUG_ON(sizeof(struct vring_rx_desc) > sizeof(skb->cb));
 
 	if (wil_vring_is_empty(vring))
 		return NULL;
 
-	_d = &(vring->va[vring->swhead].rx);
+	_d = &vring->va[vring->swhead].rx;
 	if (!(_d->dma.status & RX_DMA_STATUS_DU)) {
 		/* it is not error, we just reached end of Rx done area */
 		return NULL;
@@ -414,7 +433,6 @@ static struct sk_buff *wil_vring_reap_rx(struct wil6210_priv *wil,
 	cid = wil_rxdesc_cid(d);
 	stats = &wil->sta[cid].stats;
 	stats->last_mcs_rx = wil_rxdesc_mcs(d);
-	wil->stats.last_mcs_rx = stats->last_mcs_rx;
 
 	/* use radiotap header only if required */
 	if (ndev->type == ARPHRD_IEEE80211_RADIOTAP)
@@ -533,7 +551,7 @@ void wil_netif_rx_any(struct sk_buff *skb, struct net_device *ndev)
 			[GRO_NORMAL]		= "GRO_NORMAL",
 			[GRO_DROP]		= "GRO_DROP",
 		};
-		wil_dbg_txrx(wil, "Rx complete %d bytes => %s,\n",
+		wil_dbg_txrx(wil, "Rx complete %d bytes => %s\n",
 			     len, gro_res_str[rc]);
 	}
 }
@@ -574,7 +592,6 @@ void wil_rx_handle(struct wil6210_priv *wil, int *quota)
 			else
 				wil_netif_rx_any(skb, ndev);
 		}
-
 	}
 	wil_rx_refill(wil, v->size);
 }
@@ -584,6 +601,8 @@ int wil_rx_init(struct wil6210_priv *wil)
 	struct vring *vring = &wil->vring_rx;
 	int rc;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	if (vring->va) {
 		wil_err(wil, "Rx ring already allocated\n");
 		return -EINVAL;
@@ -613,6 +632,8 @@ void wil_rx_fini(struct wil6210_priv *wil)
 {
 	struct vring *vring = &wil->vring_rx;
 
+	wil_dbg_misc(wil, "%s()\n", __func__);
+
 	if (vring->va)
 		wil_vring_free(wil, vring, 0);
 }
@@ -647,6 +668,9 @@ int wil_vring_init_tx(struct wil6210_priv *wil, int id, int size,
 	struct vring *vring = &wil->vring_tx[id];
 	struct vring_tx_data *txdata = &wil->vring_tx_data[id];
 
+	wil_dbg_misc(wil, "%s() max_mpdu_size %d\n", __func__,
+		     cmd.vring_cfg.tx_sw_ring.max_mpdu_size);
+
 	if (vring->va) {
 		wil_err(wil, "Tx ring [%d] already allocated\n", id);
 		rc = -EINVAL;
@@ -696,6 +720,8 @@ void wil_vring_fini_tx(struct wil6210_priv *wil, int id)
 	if (!vring->va)
 		return;
 
+	wil_dbg_misc(wil, "%s() id=%d\n", __func__, id);
+
 	/* make sure NAPI won't touch this vring */
 	wil->vring_tx_data[id].enabled = 0;
 	if (test_bit(wil_status_napi_en, &wil->status))
@@ -722,6 +748,7 @@ static struct vring *wil_find_tx_vring(struct wil6210_priv *wil,
 	for (i = 0; i < ARRAY_SIZE(wil->vring2cid_tid); i++) {
 		if (wil->vring2cid_tid[i][0] == cid) {
 			struct vring *v = &wil->vring_tx[i];
+
 			wil_dbg_txrx(wil, "%s(%pM) -> [%d]\n",
 				     __func__, eth->h_dest, i);
 			if (v->va) {
@@ -741,6 +768,7 @@ static void wil_set_da_for_vring(struct wil6210_priv *wil,
 {
 	struct ethhdr *eth = (void *)skb->data;
 	int cid = wil->vring2cid_tid[vring_index][0];
+
 	memcpy(eth->h_dest, wil->sta[cid].addr, ETH_ALEN);
 }
 
@@ -751,7 +779,7 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
  * duplicate skb and send it to other active vrings
  */
 static struct vring *wil_tx_bcast(struct wil6210_priv *wil,
-				       struct sk_buff *skb)
+				  struct sk_buff *skb)
 {
 	struct vring *v, *v2;
 	struct sk_buff *skb2;
@@ -834,8 +862,8 @@ void wil_tx_desc_set_nr_frags(struct vring_tx_desc *d, int nr_frags)
 }
 
 static int wil_tx_desc_offload_cksum_set(struct wil6210_priv *wil,
-				struct vring_tx_desc *d,
-				struct sk_buff *skb)
+					 struct vring_tx_desc *d,
+					 struct sk_buff *skb)
 {
 	int protocol;
 
@@ -903,10 +931,9 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 			1 + nr_frags);
 		return -ENOMEM;
 	}
-	_d = &(vring->va[i].tx);
+	_d = &vring->va[i].tx;
 
-	pa = dma_map_single(dev, skb->data,
-			skb_headlen(skb), DMA_TO_DEVICE);
+	pa = dma_map_single(dev, skb->data, skb_headlen(skb), DMA_TO_DEVICE);
 
 	wil_dbg_txrx(wil, "Tx skb %d bytes 0x%p -> %pad\n", skb_headlen(skb),
 		     skb->data, &pa);
@@ -935,10 +962,11 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 		const struct skb_frag_struct *frag =
 				&skb_shinfo(skb)->frags[f];
 		int len = skb_frag_size(frag);
+
 		i = (swhead + f + 1) % vring->size;
-		_d = &(vring->va[i].tx);
+		_d = &vring->va[i].tx;
 		pa = skb_frag_dma_map(dev, frag, 0, skb_frag_size(frag),
-				DMA_TO_DEVICE);
+				      DMA_TO_DEVICE);
 		if (unlikely(dma_mapping_error(dev, pa)))
 			goto dma_error;
 		vring->ctx[i].mapped_as = wil_mapped_as_page;
@@ -983,7 +1011,7 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 
 		i = (swhead + f) % vring->size;
 		ctx = &vring->ctx[i];
-		_d = &(vring->va[i].tx);
+		_d = &vring->va[i].tx;
 		*d = *_d;
 		_d->dma.status = TX_DMA_STATUS_DU;
 		wil_txdesc_unmap(dev, d, ctx);
@@ -997,7 +1025,6 @@ static int wil_tx_vring(struct wil6210_priv *wil, struct vring *vring,
 	return -EINVAL;
 }
 
-
 netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 {
 	struct wil6210_priv *wil = ndev_to_wil(ndev);
@@ -1025,15 +1052,15 @@ netdev_tx_t wil_start_xmit(struct sk_buff *skb, struct net_device *ndev)
 	pr_once_fw = false;
 
 	/* find vring */
-	if (is_unicast_ether_addr(eth->h_dest)) {
+	if (is_unicast_ether_addr(eth->h_dest))
 		vring = wil_find_tx_vring(wil, skb);
-	} else {
+	else
 		vring = wil_tx_bcast(wil, skb);
-	}
 	if (!vring) {
 		wil_dbg_txrx(wil, "No Tx VRING found for %pM\n", eth->h_dest);
 		goto drop;
 	}
+
 	/* set up vring entry */
 	rc = wil_tx_vring(wil, vring, skb);
 
diff --git a/drivers/net/wireless/ath/wil6210/txrx.h b/drivers/net/wireless/ath/wil6210/txrx.h
index bc5706a4f007..de046716d2b7 100644
--- a/drivers/net/wireless/ath/wil6210/txrx.h
+++ b/drivers/net/wireless/ath/wil6210/txrx.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -20,9 +20,9 @@
 #define BUF_SW_OWNED    (1)
 #define BUF_HW_OWNED    (0)
 
-/* size of max. Rx packet */
-#define RX_BUF_LEN      (2048)
-#define TX_BUF_LEN      (2048)
+/* size of max. Tx/Rx buffers, as supported by FW */
+#define RX_BUF_LEN      (2242)
+#define TX_BUF_LEN      (2242)
 /* how many bytes to reserve for rtap header? */
 #define WIL6210_RTAP_SIZE (128)
 
@@ -237,7 +237,6 @@ struct vring_tx_mac {
 #define DMA_CFG_DESC_TX_0_L4_TYPE_LEN 2
 #define DMA_CFG_DESC_TX_0_L4_TYPE_MSK 0xC0000000 /* L4 type: 0-UDP, 2-TCP */
 
-
 #define DMA_CFG_DESC_TX_OFFLOAD_CFG_MAC_LEN_POS 0
 #define DMA_CFG_DESC_TX_OFFLOAD_CFG_MAC_LEN_LEN 7
 #define DMA_CFG_DESC_TX_OFFLOAD_CFG_MAC_LEN_MSK 0x7F /* MAC hdr len */
@@ -246,7 +245,6 @@ struct vring_tx_mac {
 #define DMA_CFG_DESC_TX_OFFLOAD_CFG_L3T_IPV4_LEN 1
 #define DMA_CFG_DESC_TX_OFFLOAD_CFG_L3T_IPV4_MSK 0x80 /* 1-IPv4, 0-IPv6 */
 
-
 #define TX_DMA_STATUS_DU         BIT(0)
 
 struct vring_tx_dma {
@@ -347,7 +345,6 @@ struct vring_rx_mac {
 #define RX_DMA_ERROR_L3_ERR   BIT(4)
 #define RX_DMA_ERROR_L4_ERR   BIT(5)
 
-
 /* Status field */
 #define RX_DMA_STATUS_DU         BIT(0)
 #define RX_DMA_STATUS_ERROR      BIT(2)
diff --git a/drivers/net/wireless/ath/wil6210/wil6210.h b/drivers/net/wireless/ath/wil6210/wil6210.h
index 67e9624f7111..ce6488e42091 100644
--- a/drivers/net/wireless/ath/wil6210/wil6210.h
+++ b/drivers/net/wireless/ath/wil6210/wil6210.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -21,8 +21,14 @@
 #include <linux/wireless.h>
 #include <net/cfg80211.h>
 #include <linux/timex.h>
+#include "wil_platform.h"
+
+extern bool no_fw_recovery;
 
 #define WIL_NAME "wil6210"
+#define WIL_FW_NAME "wil6210.fw"
+
+#define WIL_MAX_BUS_REQUEST_KBPS 800000 /* ~6.1Gbps */
 
 struct wil_board {
 	int board;
@@ -47,7 +53,9 @@ static inline u32 WIL_GET_BITS(u32 x, int b0, int b1)
 #define WIL6210_MAX_TX_RINGS	(24) /* HW limit */
 #define WIL6210_MAX_CID		(8) /* HW limit */
 #define WIL6210_NAPI_BUDGET	(16) /* arbitrary */
-#define WIL6210_ITR_TRSH	(10000) /* arbitrary - about 15 IRQs/msec */
+/* Max supported by wil6210 value for interrupt threshold is 5sec. */
+#define WIL6210_ITR_TRSH_MAX (5000000)
+#define WIL6210_ITR_TRSH_DEFAULT	(300) /* usec */
 #define WIL6210_FW_RECOVERY_RETRIES	(5) /* try to recover this many times */
 #define WIL6210_FW_RECOVERY_TO	msecs_to_jiffies(5000)
 #define WIL6210_SCAN_TO		msecs_to_jiffies(10000)
@@ -86,22 +94,29 @@ struct RGF_ICR {
 
 /* registers - FW addresses */
 #define RGF_USER_USAGE_1		(0x880004)
+#define RGF_USER_USAGE_6		(0x880018)
 #define RGF_USER_HW_MACHINE_STATE	(0x8801dc)
 	#define HW_MACHINE_BOOT_DONE	(0x3fffffd)
 #define RGF_USER_USER_CPU_0		(0x8801e0)
+	#define BIT_USER_USER_CPU_MAN_RST	BIT(1) /* user_cpu_man_rst */
 #define RGF_USER_MAC_CPU_0		(0x8801fc)
+	#define BIT_USER_MAC_CPU_MAN_RST	BIT(1) /* mac_cpu_man_rst */
 #define RGF_USER_USER_SCRATCH_PAD	(0x8802bc)
 #define RGF_USER_FW_REV_ID		(0x880a8c) /* chip revision */
 #define RGF_USER_CLKS_CTL_0		(0x880abc)
+	#define BIT_USER_CLKS_CAR_AHB_SW_SEL	BIT(1) /* ref clk/PLL */
 	#define BIT_USER_CLKS_RST_PWGD	BIT(11) /* reset on "power good" */
 #define RGF_USER_CLKS_CTL_SW_RST_VEC_0	(0x880b04)
 #define RGF_USER_CLKS_CTL_SW_RST_VEC_1	(0x880b08)
 #define RGF_USER_CLKS_CTL_SW_RST_VEC_2	(0x880b0c)
 #define RGF_USER_CLKS_CTL_SW_RST_VEC_3	(0x880b10)
 #define RGF_USER_CLKS_CTL_SW_RST_MASK_0	(0x880b14)
+	#define BIT_HPAL_PERST_FROM_PAD	BIT(6)
+	#define BIT_CAR_PERST_RST	BIT(7)
 #define RGF_USER_USER_ICR		(0x880b4c) /* struct RGF_ICR */
 	#define BIT_USER_USER_ICR_SW_INT_2	BIT(18)
 #define RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_0	(0x880c18)
+#define RGF_USER_CLKS_CTL_EXT_SW_RST_VEC_1	(0x880c2c)
 
 #define RGF_DMA_EP_TX_ICR		(0x881bb4) /* struct RGF_ICR */
 	#define BIT_DMA_EP_TX_ICR_TX_DONE	BIT(0)
@@ -133,6 +148,11 @@ struct RGF_ICR {
 #define RGF_HP_CTRL			(0x88265c)
 #define RGF_PCIE_LOS_COUNTER_CTL	(0x882dc4)
 
+/* MAC timer, usec, for packet lifetime */
+#define RGF_MAC_MTRL_COUNTER_0		(0x886aa8)
+
+#define RGF_CAF_ICR			(0x88946c) /* struct RGF_ICR */
+
 /* popular locations */
 #define HOST_MBOX   HOSTADDR(RGF_USER_USER_SCRATCH_PAD)
 #define HOST_SW_INT (HOSTADDR(RGF_USER_USER_ICR) + \
@@ -151,6 +171,7 @@ struct fw_map {
 	u32 host; /* PCI/Host address - BAR0 + 0x880000 */
 	const char *name; /* for debugfs */
 };
+
 /* array size should be in sync with actual definition in the wmi.c */
 extern const struct fw_map fw_mapping[7];
 
@@ -300,18 +321,12 @@ struct pci_dev;
  * @timeout: reset timer value (in TUs).
  * @dialog_token: dialog token for aggregation session
  * @rcu_head: RCU head used for freeing this struct
- * @reorder_lock: serializes access to reorder buffer, see below.
  *
  * This structure's lifetime is managed by RCU, assignments to
  * the array holding it must hold the aggregation mutex.
  *
- * The @reorder_lock is used to protect the members of this
- * struct, except for @timeout, @buf_size and @dialog_token,
- * which are constant across the lifetime of the struct (the
- * dialog token being used only for debugging).
  */
 struct wil_tid_ampdu_rx {
-	spinlock_t reorder_lock; /* see above */
 	struct sk_buff **reorder_buf;
 	unsigned long *reorder_time;
 	struct timer_list session_timer;
@@ -327,17 +342,6 @@ struct wil_tid_ampdu_rx {
 	bool first_time; /* is it 1-st time this buffer used? */
 };
 
-struct wil6210_stats {
-	u64 tsf;
-	u32 snr;
-	u16 last_mcs_rx;
-	u16 bf_mcs; /* last BF, used for Tx */
-	u16 my_rx_sector;
-	u16 my_tx_sector;
-	u16 peer_rx_sector;
-	u16 peer_tx_sector;
-};
-
 enum wil_sta_status {
 	wil_sta_unused = 0,
 	wil_sta_conn_pending = 1,
@@ -371,10 +375,17 @@ struct wil_sta_info {
 	bool data_port_open; /* can send any data, not only EAPOL */
 	/* Rx BACK */
 	struct wil_tid_ampdu_rx *tid_rx[WIL_STA_TID_NUM];
+	spinlock_t tid_rx_lock; /* guarding tid_rx array */
 	unsigned long tid_rx_timer_expired[BITS_TO_LONGS(WIL_STA_TID_NUM)];
 	unsigned long tid_rx_stop_requested[BITS_TO_LONGS(WIL_STA_TID_NUM)];
 };
 
+enum {
+	fw_recovery_idle = 0,
+	fw_recovery_pending = 1,
+	fw_recovery_running = 2,
+};
+
 struct wil6210_priv {
 	struct pci_dev *pdev;
 	int n_msi;
@@ -385,18 +396,22 @@ struct wil6210_priv {
 	u32 hw_version;
 	struct wil_board *board;
 	u8 n_mids; /* number of additional MIDs as reported by FW */
-	int recovery_count; /* num of FW recovery attempts in a short time */
+	u32 recovery_count; /* num of FW recovery attempts in a short time */
+	u32 recovery_state; /* FW recovery state machine */
 	unsigned long last_fw_recovery; /* jiffies of last fw recovery */
+	wait_queue_head_t wq; /* for all wait_event() use */
 	/* profile */
 	u32 monitor_flags;
 	u32 secure_pcp; /* create secure PCP? */
 	int sinfo_gen;
+	u32 itr_trsh;
 	/* cached ISR registers */
 	u32 isr_misc;
 	/* mailbox related */
 	struct mutex wmi_mutex;
 	struct wil6210_mbox_ctl mbox_ctl;
 	struct completion wmi_ready;
+	struct completion wmi_call;
 	u16 wmi_seq;
 	u16 reply_id; /**< wait for this WMI event */
 	void *reply_buf;
@@ -430,11 +445,13 @@ struct wil6210_priv {
 
 	struct mutex mutex; /* for wil6210_priv access in wil_{up|down} */
 	/* statistics */
-	struct wil6210_stats stats;
 	atomic_t isr_count_rx, isr_count_tx;
 	/* debugfs */
 	struct dentry *debug;
 	struct debugfs_blob_wrapper blobs[ARRAY_SIZE(fw_mapping)];
+
+	void *platform_handle;
+	struct wil_platform_ops platform_ops;
 };
 
 #define wil_to_wiphy(i) (i->wdev->wiphy)
@@ -444,10 +461,11 @@ struct wil6210_priv {
 #define wdev_to_wil(w) (struct wil6210_priv *)(wdev_priv(w))
 #define wil_to_ndev(i) (wil_to_wdev(i)->netdev)
 #define ndev_to_wil(n) (wdev_to_wil(n->ieee80211_ptr))
+#define wil_to_pcie_dev(i) (&i->pdev->dev)
 
-int wil_dbg_trace(struct wil6210_priv *wil, const char *fmt, ...);
-int wil_err(struct wil6210_priv *wil, const char *fmt, ...);
-int wil_info(struct wil6210_priv *wil, const char *fmt, ...);
+void wil_dbg_trace(struct wil6210_priv *wil, const char *fmt, ...);
+void wil_err(struct wil6210_priv *wil, const char *fmt, ...);
+void wil_info(struct wil6210_priv *wil, const char *fmt, ...);
 #define wil_dbg(wil, fmt, arg...) do { \
 	netdev_dbg(wil_to_ndev(wil), fmt, ##arg); \
 	wil_dbg_trace(wil, fmt, ##arg); \
@@ -458,6 +476,7 @@ int wil_info(struct wil6210_priv *wil, const char *fmt, ...);
 #define wil_dbg_wmi(wil, fmt, arg...) wil_dbg(wil, "DBG[ WMI]" fmt, ##arg)
 #define wil_dbg_misc(wil, fmt, arg...) wil_dbg(wil, "DBG[MISC]" fmt, ##arg)
 
+#if defined(CONFIG_DYNAMIC_DEBUG)
 #define wil_hex_dump_txrx(prefix_str, prefix_type, rowsize,	\
 			  groupsize, buf, len, ascii)		\
 			  print_hex_dump_debug("DBG[TXRX]" prefix_str,\
@@ -469,6 +488,19 @@ int wil_info(struct wil6210_priv *wil, const char *fmt, ...);
 			 print_hex_dump_debug("DBG[ WMI]" prefix_str,\
 					prefix_type, rowsize,	\
 					groupsize, buf, len, ascii)
+#else /* defined(CONFIG_DYNAMIC_DEBUG) */
+static inline
+void wil_hex_dump_txrx(const char *prefix_str, int prefix_type, int rowsize,
+		       int groupsize, const void *buf, size_t len, bool ascii)
+{
+}
+
+static inline
+void wil_hex_dump_wmi(const char *prefix_str, int prefix_type, int rowsize,
+		      int groupsize, const void *buf, size_t len, bool ascii)
+{
+}
+#endif /* defined(CONFIG_DYNAMIC_DEBUG) */
 
 void wil_memcpy_fromio_32(void *dst, const volatile void __iomem *src,
 			  size_t count);
@@ -482,13 +514,18 @@ void wil_if_remove(struct wil6210_priv *wil);
 int wil_priv_init(struct wil6210_priv *wil);
 void wil_priv_deinit(struct wil6210_priv *wil);
 int wil_reset(struct wil6210_priv *wil);
+void wil_set_itr_trsh(struct wil6210_priv *wil);
 void wil_fw_error_recovery(struct wil6210_priv *wil);
+void wil_set_recovery_state(struct wil6210_priv *wil, int state);
 void wil_link_on(struct wil6210_priv *wil);
 void wil_link_off(struct wil6210_priv *wil);
 int wil_up(struct wil6210_priv *wil);
+int __wil_up(struct wil6210_priv *wil);
 int wil_down(struct wil6210_priv *wil);
+int __wil_down(struct wil6210_priv *wil);
 void wil_mbox_ring_le2cpus(struct wil6210_mbox_ring *r);
 int wil_find_cid(struct wil6210_priv *wil, const u8 *mac);
+void wil_set_ethtoolops(struct net_device *ndev);
 
 void __iomem *wmi_buffer(struct wil6210_priv *wil, __le32 ptr);
 void __iomem *wmi_addr(struct wil6210_priv *wil, u32 ptr);
@@ -519,8 +556,10 @@ int wmi_disconnect_sta(struct wil6210_priv *wil, const u8 *mac, u16 reason);
 void wil6210_clear_irq(struct wil6210_priv *wil);
 int wil6210_init_irq(struct wil6210_priv *wil, int irq);
 void wil6210_fini_irq(struct wil6210_priv *wil, int irq);
-void wil6210_disable_irq(struct wil6210_priv *wil);
-void wil6210_enable_irq(struct wil6210_priv *wil);
+void wil_mask_irq(struct wil6210_priv *wil);
+void wil_unmask_irq(struct wil6210_priv *wil);
+void wil_disable_irq(struct wil6210_priv *wil);
+void wil_enable_irq(struct wil6210_priv *wil);
 int wil_cfg80211_mgmt_tx(struct wiphy *wiphy, struct wireless_dev *wdev,
 			 struct cfg80211_mgmt_tx_params *params,
 			 u64 *cookie);
@@ -556,4 +595,7 @@ void wil6210_unmask_irq_rx(struct wil6210_priv *wil);
 
 int wil_iftype_nl2wmi(enum nl80211_iftype type);
 
+int wil_ioctl(struct wil6210_priv *wil, void __user *data, int cmd);
+int wil_request_firmware(struct wil6210_priv *wil, const char *name);
+
 #endif /* __WIL6210_H__ */
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform.c b/drivers/net/wireless/ath/wil6210/wil_platform.c
new file mode 100644
index 000000000000..8f1d78f8a74d
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/wil_platform.c
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "linux/device.h"
+#include "wil_platform.h"
+
+#ifdef CONFIG_WIL6210_PLATFORM_MSM
+#include "wil_platform_msm.h"
+#endif
+
+/**
+ * wil_platform_init() - wil6210 platform module init
+ *
+ * The function must be called before all other functions in this module.
+ * It returns a handle which is used with the rest of the API
+ *
+ */
+void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops)
+{
+	void *handle = NULL;
+
+	if (!ops) {
+		dev_err(dev, "Invalid parameter. Cannot init platform module\n");
+		return NULL;
+	}
+
+#ifdef CONFIG_WIL6210_PLATFORM_MSM
+	handle = wil_platform_msm_init(dev, ops);
+	if (handle)
+		return handle;
+#endif
+
+	/* other platform specific init functions should be called here */
+
+	return handle;
+}
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform.h b/drivers/net/wireless/ath/wil6210/wil_platform.h
new file mode 100644
index 000000000000..158c73b049a9
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/wil_platform.h
@@ -0,0 +1,34 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef __WIL_PLATFORM_H__
+#define __WIL_PLATFORM_H__
+
+struct device;
+
+/**
+ * struct wil_platform_ops - wil platform module callbacks
+ */
+struct wil_platform_ops {
+	int (*bus_request)(void *handle, uint32_t kbps /* KBytes/Sec */);
+	int (*suspend)(void *handle);
+	int (*resume)(void *handle);
+	void (*uninit)(void *handle);
+};
+
+void *wil_platform_init(struct device *dev, struct wil_platform_ops *ops);
+
+#endif /* __WIL_PLATFORM_H__ */
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform_msm.c b/drivers/net/wireless/ath/wil6210/wil_platform_msm.c
new file mode 100644
index 000000000000..b354a743240d
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/wil_platform_msm.c
@@ -0,0 +1,257 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <linux/of.h>
+#include <linux/slab.h>
+#include <linux/msm-bus.h>
+
+#include "wil_platform.h"
+#include "wil_platform_msm.h"
+
+/**
+ * struct wil_platform_msm - wil6210 msm platform module info
+ *
+ * @dev: device object
+ * @msm_bus_handle: handle for using msm_bus API
+ * @pdata: bus scale info retrieved from DT
+ */
+struct wil_platform_msm {
+	struct device *dev;
+	uint32_t msm_bus_handle;
+	struct msm_bus_scale_pdata *pdata;
+};
+
+#define KBTOB(a) (a * 1000ULL)
+
+/**
+ * wil_platform_get_pdata() - Generate bus client data from device tree
+ * provided by clients.
+ *
+ * dev: device object
+ * of_node: Device tree node to extract information from
+ *
+ * The function returns a valid pointer to the allocated bus-scale-pdata
+ * if the vectors were correctly read from the client's device node.
+ * Any error in reading or parsing the device node will return NULL
+ * to the caller.
+ */
+static struct msm_bus_scale_pdata *wil_platform_get_pdata(
+		struct device *dev,
+		struct device_node *of_node)
+{
+	struct msm_bus_scale_pdata *pdata;
+	struct msm_bus_paths *usecase;
+	int i, j, ret, len;
+	unsigned int num_usecases, num_paths, mem_size;
+	const uint32_t *vec_arr;
+	struct msm_bus_vectors *vectors;
+
+	/* first read num_usecases and num_paths so we can calculate
+	 * amount of memory to allocate
+	 */
+	ret = of_property_read_u32(of_node, "qcom,msm-bus,num-cases",
+				   &num_usecases);
+	if (ret) {
+		dev_err(dev, "Error: num-usecases not found\n");
+		return NULL;
+	}
+
+	ret = of_property_read_u32(of_node, "qcom,msm-bus,num-paths",
+				   &num_paths);
+	if (ret) {
+		dev_err(dev, "Error: num_paths not found\n");
+		return NULL;
+	}
+
+	/* pdata memory layout:
+	 *   msm_bus_scale_pdata
+	 *   msm_bus_paths[num_usecases]
+	 *   msm_bus_vectors[num_usecases][num_paths]
+	 */
+	mem_size = sizeof(struct msm_bus_scale_pdata) +
+		   sizeof(struct msm_bus_paths) * num_usecases +
+		   sizeof(struct msm_bus_vectors) * num_usecases * num_paths;
+
+	pdata = kzalloc(mem_size, GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	ret = of_property_read_string(of_node, "qcom,msm-bus,name",
+				      &pdata->name);
+	if (ret) {
+		dev_err(dev, "Error: Client name not found\n");
+		goto err;
+	}
+
+	if (of_property_read_bool(of_node, "qcom,msm-bus,active-only")) {
+		pdata->active_only = 1;
+	} else {
+		dev_info(dev, "active_only flag absent.\n");
+		dev_info(dev, "Using dual context by default\n");
+	}
+
+	pdata->num_usecases = num_usecases;
+	pdata->usecase = (struct msm_bus_paths *)(pdata + 1);
+
+	vec_arr = of_get_property(of_node, "qcom,msm-bus,vectors-KBps", &len);
+	if (vec_arr == NULL) {
+		dev_err(dev, "Error: Vector array not found\n");
+		goto err;
+	}
+
+	if (len != num_usecases * num_paths * sizeof(uint32_t) * 4) {
+		dev_err(dev, "Error: Length-error on getting vectors\n");
+		goto err;
+	}
+
+	vectors = (struct msm_bus_vectors *)(pdata->usecase + num_usecases);
+	for (i = 0; i < num_usecases; i++) {
+		usecase = &pdata->usecase[i];
+		usecase->num_paths = num_paths;
+		usecase->vectors = &vectors[i];
+
+		for (j = 0; j < num_paths; j++) {
+			int index = ((i * num_paths) + j) * 4;
+
+			usecase->vectors[j].src = be32_to_cpu(vec_arr[index]);
+			usecase->vectors[j].dst =
+				be32_to_cpu(vec_arr[index + 1]);
+			usecase->vectors[j].ab = (uint64_t)
+				KBTOB(be32_to_cpu(vec_arr[index + 2]));
+			usecase->vectors[j].ib = (uint64_t)
+				KBTOB(be32_to_cpu(vec_arr[index + 3]));
+		}
+	}
+
+	return pdata;
+
+err:
+	kfree(pdata);
+
+	return NULL;
+}
+
+/* wil_platform API (callbacks) */
+
+static int wil_platform_bus_request(void *handle,
+				    uint32_t kbps /* KBytes/Sec */)
+{
+	int rc, i;
+	struct wil_platform_msm *msm = (struct wil_platform_msm *)handle;
+	int vote = 0; /* vote 0 in case requested kbps cannot be satisfied */
+	struct msm_bus_paths *usecase;
+	uint32_t usecase_kbps;
+	uint32_t min_kbps = ~0;
+
+	/* find the lowest usecase that is bigger than requested kbps */
+	for (i = 0; i < msm->pdata->num_usecases; i++) {
+		usecase = &msm->pdata->usecase[i];
+		/* assume we have single path (vectors[0]). If we ever
+		 * have multiple paths, need to define the behavior */
+		usecase_kbps = div64_u64(usecase->vectors[0].ib, 1000);
+		if (usecase_kbps >= kbps && usecase_kbps < min_kbps) {
+			min_kbps = usecase_kbps;
+			vote = i;
+		}
+	}
+
+	rc = msm_bus_scale_client_update_request(msm->msm_bus_handle, vote);
+	if (rc)
+		dev_err(msm->dev, "Failed msm_bus voting. kbps=%d vote=%d, rc=%d\n",
+			kbps, vote, rc);
+	else
+		/* TOOD: remove */
+		dev_info(msm->dev, "msm_bus_scale_client_update_request succeeded. kbps=%d vote=%d\n",
+			 kbps, vote);
+
+	return rc;
+}
+
+static void wil_platform_uninit(void *handle)
+{
+	struct wil_platform_msm *msm = (struct wil_platform_msm *)handle;
+
+	dev_info(msm->dev, "wil_platform_uninit\n");
+
+	if (msm->msm_bus_handle)
+		msm_bus_scale_unregister_client(msm->msm_bus_handle);
+
+	kfree(msm->pdata);
+	kfree(msm);
+}
+
+static int wil_platform_msm_bus_register(struct wil_platform_msm *msm,
+					 struct device_node *node)
+{
+	msm->pdata = wil_platform_get_pdata(msm->dev, node);
+	if (!msm->pdata) {
+		dev_err(msm->dev, "Failed getting DT info\n");
+		return -EINVAL;
+	}
+
+	msm->msm_bus_handle = msm_bus_scale_register_client(msm->pdata);
+	if (!msm->msm_bus_handle) {
+		dev_err(msm->dev, "Failed msm_bus registration\n");
+		return -EINVAL;
+	}
+
+	dev_info(msm->dev, "msm_bus registration succeeded! handle 0x%x\n",
+		 msm->msm_bus_handle);
+
+	return 0;
+}
+
+/**
+ * wil_platform_msm_init() - wil6210 msm platform module init
+ *
+ * The function must be called before all other functions in this module.
+ * It returns a handle which is used with the rest of the API
+ *
+ */
+void *wil_platform_msm_init(struct device *dev, struct wil_platform_ops *ops)
+{
+	struct device_node *of_node;
+	struct wil_platform_msm *msm;
+	int rc;
+
+	of_node = of_find_compatible_node(NULL, NULL, "qcom,wil6210");
+	if (!of_node) {
+		/* this could mean non-msm platform */
+		dev_err(dev, "DT node not found\n");
+		return NULL;
+	}
+
+	msm = kzalloc(sizeof(*msm), GFP_KERNEL);
+	if (!msm)
+		return NULL;
+
+	msm->dev = dev;
+
+	/* register with msm_bus module for scaling requests */
+	rc = wil_platform_msm_bus_register(msm, of_node);
+	if (rc)
+		goto cleanup;
+
+	memset(ops, 0, sizeof(*ops));
+	ops->bus_request = wil_platform_bus_request;
+	ops->uninit = wil_platform_uninit;
+
+	return (void *)msm;
+
+cleanup:
+	kfree(msm);
+	return NULL;
+}
diff --git a/drivers/net/wireless/ath/wil6210/wil_platform_msm.h b/drivers/net/wireless/ath/wil6210/wil_platform_msm.h
new file mode 100644
index 000000000000..2f2229edb498
--- /dev/null
+++ b/drivers/net/wireless/ath/wil6210/wil_platform_msm.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2014 Qualcomm Atheros, Inc.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#ifndef __WIL_PLATFORM__MSM_H__
+#define __WIL_PLATFORM_MSM_H__
+
+#include "wil_platform.h"
+
+void *wil_platform_msm_init(struct device *dev, struct wil_platform_ops *ops);
+
+#endif /* __WIL_PLATFORM__MSM_H__ */
diff --git a/drivers/net/wireless/ath/wil6210/wmi.c b/drivers/net/wireless/ath/wil6210/wmi.c
index 1d1d0afdd2e1..4311df982c60 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.c
+++ b/drivers/net/wireless/ath/wil6210/wmi.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -14,6 +14,7 @@
  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
  */
 
+#include <linux/moduleparam.h>
 #include <linux/etherdevice.h>
 #include <linux/if_arp.h>
 
@@ -22,6 +23,10 @@
 #include "wmi.h"
 #include "trace.h"
 
+static uint max_assoc_sta = 1;
+module_param(max_assoc_sta, uint, S_IRUGO | S_IWUSR);
+MODULE_PARM_DESC(max_assoc_sta, " Max number of stations associated to the AP");
+
 /**
  * WMI event receiving - theory of operations
  *
@@ -152,6 +157,7 @@ int wmi_read_hdr(struct wil6210_priv *wil, __le32 ptr,
 		 struct wil6210_mbox_hdr *hdr)
 {
 	void __iomem *src = wmi_buffer(wil, ptr);
+
 	if (!src)
 		return -EINVAL;
 
@@ -273,6 +279,7 @@ static void wmi_evt_ready(struct wil6210_priv *wil, int id, void *d, int len)
 	struct net_device *ndev = wil_to_ndev(wil);
 	struct wireless_dev *wdev = wil->wdev;
 	struct wmi_ready_event *evt = d;
+
 	wil->fw_version = le32_to_cpu(evt->sw_version);
 	wil->n_mids = evt->numof_additional_mids;
 
@@ -292,8 +299,9 @@ static void wmi_evt_fw_ready(struct wil6210_priv *wil, int id, void *d,
 {
 	wil_dbg_wmi(wil, "WMI: got FW ready event\n");
 
+	wil_set_recovery_state(wil, fw_recovery_idle);
 	set_bit(wil_status_fwready, &wil->status);
-	/* reuse wmi_ready for the firmware ready indication */
+	/* let the reset sequence continue */
 	complete(&wil->wmi_ready);
 }
 
@@ -346,11 +354,11 @@ static void wmi_evt_rx_mgmt(struct wil6210_priv *wil, int id, void *d, int len)
 				    rx_mgmt_frame->bssid);
 			cfg80211_put_bss(wiphy, bss);
 		} else {
-			wil_err(wil, "cfg80211_inform_bss() failed\n");
+			wil_err(wil, "cfg80211_inform_bss_frame() failed\n");
 		}
 	} else {
 		cfg80211_rx_mgmt(wil->wdev, freq, signal,
-				 (void *)rx_mgmt_frame, d_len, 0, GFP_KERNEL);
+				 (void *)rx_mgmt_frame, d_len, 0);
 	}
 }
 
@@ -482,33 +490,6 @@ static void wmi_evt_disconnect(struct wil6210_priv *wil, int id,
 	mutex_unlock(&wil->mutex);
 }
 
-static void wmi_evt_notify(struct wil6210_priv *wil, int id, void *d, int len)
-{
-	struct wmi_notify_req_done_event *evt = d;
-
-	if (len < sizeof(*evt)) {
-		wil_err(wil, "Short NOTIFY event\n");
-		return;
-	}
-
-	wil->stats.tsf = le64_to_cpu(evt->tsf);
-	wil->stats.snr = le32_to_cpu(evt->snr_val);
-	wil->stats.bf_mcs = le16_to_cpu(evt->bf_mcs);
-	wil->stats.my_rx_sector = le16_to_cpu(evt->my_rx_sector);
-	wil->stats.my_tx_sector = le16_to_cpu(evt->my_tx_sector);
-	wil->stats.peer_rx_sector = le16_to_cpu(evt->other_rx_sector);
-	wil->stats.peer_tx_sector = le16_to_cpu(evt->other_tx_sector);
-	wil_dbg_wmi(wil, "Link status, MCS %d TSF 0x%016llx\n"
-		    "BF status 0x%08x SNR 0x%08x SQI %d%%\n"
-		    "Tx Tpt %d goodput %d Rx goodput %d\n"
-		    "Sectors(rx:tx) my %d:%d peer %d:%d\n",
-		    wil->stats.bf_mcs, wil->stats.tsf, evt->status,
-		    wil->stats.snr, evt->sqi, le32_to_cpu(evt->tx_tpt),
-		    le32_to_cpu(evt->tx_goodput), le32_to_cpu(evt->rx_goodput),
-		    wil->stats.my_rx_sector, wil->stats.my_tx_sector,
-		    wil->stats.peer_rx_sector, wil->stats.peer_tx_sector);
-}
-
 /*
  * Firmware reports EAPOL frame using WME event.
  * Reconstruct Ethernet frame and deliver it via normal Rx
@@ -617,27 +598,40 @@ static void wmi_evt_ba_status(struct wil6210_priv *wil, int id, void *d,
 		return;
 	}
 
+	mutex_lock(&wil->mutex);
+
 	cid = wil->vring2cid_tid[evt->ringid][0];
 	if (cid >= WIL6210_MAX_CID) {
 		wil_err(wil, "invalid CID %d for vring %d\n", cid, evt->ringid);
-		return;
+		goto out;
 	}
 
 	sta = &wil->sta[cid];
 	if (sta->status == wil_sta_unused) {
 		wil_err(wil, "CID %d unused\n", cid);
-		return;
+		goto out;
 	}
 
 	wil_dbg_wmi(wil, "BACK for CID %d %pM\n", cid, sta->addr);
 	for (i = 0; i < WIL_STA_TID_NUM; i++) {
-		struct wil_tid_ampdu_rx *r = sta->tid_rx[i];
+		struct wil_tid_ampdu_rx *r;
+		unsigned long flags;
+
+		spin_lock_irqsave(&sta->tid_rx_lock, flags);
+
+		r = sta->tid_rx[i];
 		sta->tid_rx[i] = NULL;
 		wil_tid_ampdu_rx_free(wil, r);
+
+		spin_unlock_irqrestore(&sta->tid_rx_lock, flags);
+
 		if ((evt->status == WMI_BA_AGREED) && evt->agg_wsize)
 			sta->tid_rx[i] = wil_tid_ampdu_rx_alloc(wil,
 						evt->agg_wsize, 0);
 	}
+
+out:
+	mutex_unlock(&wil->mutex);
 }
 
 static const struct {
@@ -651,7 +645,6 @@ static const struct {
 	{WMI_SCAN_COMPLETE_EVENTID,	wmi_evt_scan_complete},
 	{WMI_CONNECT_EVENTID,		wmi_evt_connect},
 	{WMI_DISCONNECT_EVENTID,	wmi_evt_disconnect},
-	{WMI_NOTIFY_REQ_DONE_EVENTID,	wmi_evt_notify},
 	{WMI_EAPOL_RX_EVENTID,		wmi_evt_eapol_rx},
 	{WMI_DATA_PORT_OPEN_EVENTID,	wmi_evt_linkup},
 	{WMI_WBE_LINKDOWN_EVENTID,	wmi_evt_linkdown},
@@ -676,7 +669,7 @@ void wmi_recv_cmd(struct wil6210_priv *wil)
 	unsigned n;
 
 	if (!test_bit(wil_status_reset_done, &wil->status)) {
-		wil_err(wil, "Reset not completed\n");
+		wil_err(wil, "Reset in progress. Cannot handle WMI event\n");
 		return;
 	}
 
@@ -731,6 +724,7 @@ void wmi_recv_cmd(struct wil6210_priv *wil)
 			struct wil6210_mbox_hdr_wmi *wmi = &evt->event.wmi;
 			u16 id = le16_to_cpu(wmi->id);
 			u32 tstamp = le32_to_cpu(wmi->timestamp);
+
 			wil_dbg_wmi(wil, "WMI event 0x%04x MID %d @%d msec\n",
 				    id, wmi->mid, tstamp);
 			trace_wil6210_wmi_event(wmi, &wmi[1],
@@ -771,8 +765,8 @@ int wmi_call(struct wil6210_priv *wil, u16 cmdid, void *buf, u16 len,
 	wil->reply_id = reply_id;
 	wil->reply_buf = reply;
 	wil->reply_size = reply_size;
-	remain = wait_for_completion_timeout(&wil->wmi_ready,
-			msecs_to_jiffies(to_msec));
+	remain = wait_for_completion_timeout(&wil->wmi_call,
+					     msecs_to_jiffies(to_msec));
 	if (0 == remain) {
 		wil_err(wil, "wmi_call(0x%04x->0x%04x) timeout %d msec\n",
 			cmdid, reply_id, to_msec);
@@ -822,7 +816,7 @@ int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan)
 		.network_type = wmi_nettype,
 		.disable_sec_offload = 1,
 		.channel = chan - 1,
-		.pcp_max_assoc_sta = WIL6210_MAX_CID,
+		.pcp_max_assoc_sta = max_assoc_sta,
 	};
 	struct {
 		struct wil6210_mbox_hdr_wmi wmi;
@@ -832,6 +826,14 @@ int wmi_pcp_start(struct wil6210_priv *wil, int bi, u8 wmi_nettype, u8 chan)
 	if (!wil->secure_pcp)
 		cmd.disable_sec = 1;
 
+	if ((cmd.pcp_max_assoc_sta > WIL6210_MAX_CID) ||
+	    (cmd.pcp_max_assoc_sta <= 0)) {
+		wil_info(wil,
+			 "Requested connection limit %u, valid values are 1 - %d. Setting to %d\n",
+			 max_assoc_sta, WIL6210_MAX_CID, WIL6210_MAX_CID);
+		cmd.pcp_max_assoc_sta = WIL6210_MAX_CID;
+	}
+
 	/*
 	 * Processing time may be huge, in case of secure AP it takes about
 	 * 3500ms for FW to start AP
@@ -968,8 +970,11 @@ int wmi_set_ie(struct wil6210_priv *wil, u8 type, u16 ie_len, const void *ie)
 	int rc;
 	u16 len = sizeof(struct wmi_set_appie_cmd) + ie_len;
 	struct wmi_set_appie_cmd *cmd = kzalloc(len, GFP_KERNEL);
+
 	if (!cmd)
 		return -ENOMEM;
+	if (!ie)
+		ie_len = 0;
 
 	cmd->mgmt_frm_type = type;
 	/* BUG: FW API define ieLen as u8. Will fix FW */
@@ -1143,6 +1148,9 @@ static void wmi_event_handle(struct wil6210_priv *wil,
 		struct wil6210_mbox_hdr_wmi *wmi = (void *)(&hdr[1]);
 		void *evt_data = (void *)(&wmi[1]);
 		u16 id = le16_to_cpu(wmi->id);
+
+		wil_dbg_wmi(wil, "Handle WMI 0x%04x (reply_id 0x%04x)\n",
+			    id, wil->reply_id);
 		/* check if someone waits for this event */
 		if (wil->reply_id && wil->reply_id == id) {
 			if (wil->reply_buf) {
@@ -1153,7 +1161,7 @@ static void wmi_event_handle(struct wil6210_priv *wil,
 						     len - sizeof(*wmi));
 			}
 			wil_dbg_wmi(wil, "Complete WMI 0x%04x\n", id);
-			complete(&wil->wmi_ready);
+			complete(&wil->wmi_call);
 			return;
 		}
 		/* unsolicited event */
@@ -1199,9 +1207,11 @@ void wmi_event_worker(struct work_struct *work)
 	struct pending_wmi_event *evt;
 	struct list_head *lh;
 
+	wil_dbg_wmi(wil, "Start %s\n", __func__);
 	while ((lh = next_wmi_ev(wil)) != NULL) {
 		evt = list_entry(lh, struct pending_wmi_event, list);
 		wmi_event_handle(wil, &evt->event.hdr);
 		kfree(evt);
 	}
+	wil_dbg_wmi(wil, "Finished %s\n", __func__);
 }
diff --git a/drivers/net/wireless/ath/wil6210/wmi.h b/drivers/net/wireless/ath/wil6210/wmi.h
index 17334c852866..27b97432d1c2 100644
--- a/drivers/net/wireless/ath/wil6210/wmi.h
+++ b/drivers/net/wireless/ath/wil6210/wmi.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012 Qualcomm Atheros, Inc.
+ * Copyright (c) 2012-2014 Qualcomm Atheros, Inc.
  * Copyright (c) 2006-2012 Wilocity .
  *
  * Permission to use, copy, modify, and/or distribute this software for any
@@ -179,7 +179,6 @@ enum wmi_crypto_type {
 	WMI_CRYPT_AES_GCMP		= 0x20,
 };
 
-
 enum wmi_connect_ctrl_flag_bits {
 	WMI_CONNECT_ASSOC_POLICY_USER		= 0x0001,
 	WMI_CONNECT_SEND_REASSOC		= 0x0002,
@@ -219,7 +218,6 @@ struct wmi_disconnect_sta_cmd {
 	__le16 disconnect_reason;
 } __packed;
 
-
 /*
  * WMI_SET_PMK_CMDID
  */
@@ -234,7 +232,6 @@ struct  wmi_set_pmk_cmd {
 	u8 pmk[WMI_PMK_LEN];
 } __packed;
 
-
 /*
  * WMI_SET_PASSPHRASE_CMDID
  */
@@ -273,7 +270,6 @@ struct wmi_delete_cipher_key_cmd {
 	u8 mac[WMI_MAC_LEN];
 } __packed;
 
-
 /*
  * WMI_START_SCAN_CMDID
  *
@@ -325,7 +321,6 @@ struct wmi_probed_ssid_cmd {
 	u8 ssid[WMI_MAX_SSID_LEN];
 } __packed;
 
-
 /*
  * WMI_SET_APPIE_CMDID
  * Add Application specified IE to a management frame
@@ -351,7 +346,6 @@ struct wmi_set_appie_cmd {
 	u8 ie_info[0];
 } __packed;
 
-
 /*
  * WMI_PXMT_RANGE_CFG_CMDID
  */
@@ -380,7 +374,6 @@ struct wmi_rf_mgmt_cmd {
 	__le32 rf_mgmt_type;
 } __packed;
 
-
 /*
  * WMI_RF_RX_TEST_CMDID
  */
@@ -426,7 +419,6 @@ struct wmi_bcon_ctrl_cmd {
 	u8 disable_sec;
 } __packed;
 
-
 /******* P2P ***********/
 
 /*
@@ -797,7 +789,6 @@ struct wmi_temp_sense_cmd {
 	__le32 measure_marlon_r_en;
 } __packed;
 
-
 /*
  * WMI Events
  */
@@ -887,7 +878,6 @@ enum wmi_event_id {
  * Events data structures
  */
 
-
 enum wmi_fw_status {
 	WMI_FW_STATUS_SUCCESS,
 	WMI_FW_STATUS_FAILURE,
@@ -980,7 +970,7 @@ struct wmi_ready_event {
  * WMI_NOTIFY_REQ_DONE_EVENTID
  */
 struct wmi_notify_req_done_event {
-	__le32 status;
+	__le32 status; /* beamforming status, 0: fail; 1: OK; 2: retrying */
 	__le64 tsf;
 	__le32 snr_val;
 	__le32 tx_tpt;
@@ -1038,8 +1028,8 @@ struct wmi_disconnect_event {
 	__le16 protocol_reason_status;	/* reason code, see 802.11 spec. */
 	u8 bssid[WMI_MAC_LEN];		/* set if known */
 	u8 disconnect_reason;		/* see wmi_disconnect_reason */
-	u8 assoc_resp_len;		/* not in use */
-	u8 assoc_info[0];		/* not in use */
+	u8 assoc_resp_len;	/* not used */
+	u8 assoc_info[0];	/* not used */
 } __packed;
 
 /*
@@ -1081,7 +1071,6 @@ struct wmi_delba_event {
 	__le16 reason;
 } __packed;
 
-
 /*
  * WMI_VRING_CFG_DONE_EVENTID
  */
@@ -1147,7 +1136,6 @@ struct wmi_data_port_open_event {
 	u8 reserved[3];
 } __packed;
 
-
 /*
  * WMI_GET_PCP_CHANNEL_EVENTID
  */
@@ -1156,7 +1144,6 @@ struct wmi_get_pcp_channel_event {
 	u8 reserved[3];
 } __packed;
 
-
 /*
 * WMI_PORT_ALLOCATED_EVENTID
 */
@@ -1260,7 +1247,6 @@ struct wmi_rx_mgmt_info {
 	u8 channel;	/* From Radio MNGR */
 } __packed;
 
-
 /*
  * WMI_TX_MGMT_PACKET_EVENTID
  */
diff --git a/drivers/net/wireless/atmel_cs.c b/drivers/net/wireless/atmel_cs.c
index 4cfb4d99ced0..7afc9c5329fb 100644
--- a/drivers/net/wireless/atmel_cs.c
+++ b/drivers/net/wireless/atmel_cs.c
@@ -66,18 +66,18 @@ static void atmel_release(struct pcmcia_device *link);
 
 static void atmel_detach(struct pcmcia_device *p_dev);
 
-typedef struct local_info_t {
+struct local_info {
 	struct net_device *eth_dev;
-} local_info_t;
+};
 
 static int atmel_probe(struct pcmcia_device *p_dev)
 {
-	local_info_t *local;
+	struct local_info *local;
 
 	dev_dbg(&p_dev->dev, "atmel_attach()\n");
 
 	/* Allocate space for private device-specific data */
-	local = kzalloc(sizeof(local_info_t), GFP_KERNEL);
+	local = kzalloc(sizeof(*local), GFP_KERNEL);
 	if (!local)
 		return -ENOMEM;
 
@@ -117,7 +117,7 @@ static int atmel_config_check(struct pcmcia_device *p_dev, void *priv_data)
 
 static int atmel_config(struct pcmcia_device *link)
 {
-	local_info_t *dev;
+	struct local_info *dev;
 	int ret;
 	const struct pcmcia_device_id *did;
 
@@ -141,14 +141,14 @@ static int atmel_config(struct pcmcia_device *link)
 	if (ret)
 		goto failed;
 
-	((local_info_t*)link->priv)->eth_dev =
+	((struct local_info *)link->priv)->eth_dev =
 		init_atmel_card(link->irq,
 				link->resource[0]->start,
 				did ? did->driver_info : ATMEL_FW_TYPE_NONE,
 				&link->dev,
 				card_present,
 				link);
-	if (!((local_info_t*)link->priv)->eth_dev)
+	if (!((struct local_info *)link->priv)->eth_dev)
 			goto failed;
 
 
@@ -161,20 +161,20 @@ static int atmel_config(struct pcmcia_device *link)
 
 static void atmel_release(struct pcmcia_device *link)
 {
-	struct net_device *dev = ((local_info_t*)link->priv)->eth_dev;
+	struct net_device *dev = ((struct local_info *)link->priv)->eth_dev;
 
 	dev_dbg(&link->dev, "atmel_release\n");
 
 	if (dev)
 		stop_atmel_card(dev);
-	((local_info_t*)link->priv)->eth_dev = NULL;
+	((struct local_info *)link->priv)->eth_dev = NULL;
 
 	pcmcia_disable_device(link);
 }
 
 static int atmel_suspend(struct pcmcia_device *link)
 {
-	local_info_t *local = link->priv;
+	struct local_info *local = link->priv;
 
 	netif_device_detach(local->eth_dev);
 
@@ -183,7 +183,7 @@ static int atmel_suspend(struct pcmcia_device *link)
 
 static int atmel_resume(struct pcmcia_device *link)
 {
-	local_info_t *local = link->priv;
+	struct local_info *local = link->priv;
 
 	atmel_open(local->eth_dev);
 	netif_device_attach(local->eth_dev);
diff --git a/drivers/net/wireless/b43/Makefile b/drivers/net/wireless/b43/Makefile
index 6e00b8804ada..9f7965aae93d 100644
--- a/drivers/net/wireless/b43/Makefile
+++ b/drivers/net/wireless/b43/Makefile
@@ -18,6 +18,7 @@ b43-y				+= xmit.o
 b43-y				+= dma.o
 b43-y				+= pio.o
 b43-y				+= rfkill.o
+b43-y				+= ppr.o
 b43-$(CONFIG_B43_LEDS)		+= leds.o
 b43-$(CONFIG_B43_PCMCIA)	+= pcmcia.o
 b43-$(CONFIG_B43_SDIO)		+= sdio.o
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 4113b6934764..bb12586cd7cd 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -45,6 +45,7 @@
 #define B43_MMIO_RAM_DATA		0x134
 #define B43_MMIO_PS_STATUS		0x140
 #define B43_MMIO_RADIO_HWENABLED_HI	0x158
+#define B43_MMIO_MAC_HW_CAP		0x15C	/* MAC capabilities (corerev >= 13) */
 #define B43_MMIO_SHM_CONTROL		0x160
 #define B43_MMIO_SHM_DATA		0x164
 #define B43_MMIO_SHM_DATA_UNALIGNED	0x166
@@ -253,6 +254,8 @@ enum {
 #define B43_SHM_SH_CHAN			0x00A0	/* Current channel (low 8bit only) */
 #define  B43_SHM_SH_CHAN_5GHZ		0x0100	/* Bit set, if 5 Ghz channel */
 #define  B43_SHM_SH_CHAN_40MHZ		0x0200	/* Bit set, if 40 Mhz channel width */
+#define B43_SHM_SH_MACHW_L		0x00C0	/* Location where the ucode expects the MAC capabilities */
+#define B43_SHM_SH_MACHW_H		0x00C2	/* Location where the ucode expects the MAC capabilities */
 #define B43_SHM_SH_HOSTF5		0x00D4	/* Hostflags 5 for ucode options */
 #define B43_SHM_SH_BCMCFIFOID		0x0108	/* Last posted cookie to the bcast/mcast FIFO */
 /* TSSI information */
@@ -297,6 +300,7 @@ enum {
 #define B43_SHM_SH_LFFBLIM		0x0046	/* Long frame fallback retry limit */
 #define B43_SHM_SH_BEACPHYCTL		0x0054	/* Beacon PHY TX control word (see PHY TX control) */
 #define B43_SHM_SH_EXTNPHYCTL		0x00B0	/* Extended bytes for beacon PHY control (N) */
+#define B43_SHM_SH_BCN_LI		0x00B6	/* beacon listen interval */
 /* SHM_SHARED ACK/CTS control */
 #define B43_SHM_SH_ACKCTSPHYCTL		0x0022	/* ACK/CTS PHY control word (see PHY TX control) */
 /* SHM_SHARED probe response variables */
@@ -457,6 +461,7 @@ enum {
 #define B43_MACCTL_RADIOLOCK		0x00080000	/* Radio lock */
 #define B43_MACCTL_BEACPROMISC		0x00100000	/* Beacon Promiscuous */
 #define B43_MACCTL_KEEP_BADPLCP		0x00200000	/* Keep frames with bad PLCP */
+#define B43_MACCTL_PHY_LOCK		0x00200000
 #define B43_MACCTL_KEEP_CTL		0x00400000	/* Keep control frames */
 #define B43_MACCTL_KEEP_BAD		0x00800000	/* Keep bad frames (FCS) */
 #define B43_MACCTL_PROMISC		0x01000000	/* Promiscuous mode */
@@ -475,6 +480,11 @@ enum {
 #define B43_MACCMD_CCA			0x00000008	/* Clear channel assessment */
 #define B43_MACCMD_BGNOISE		0x00000010	/* Background noise */
 
+/* B43_MMIO_PSM_PHY_HDR bits */
+#define B43_PSM_HDR_MAC_PHY_RESET	0x00000001
+#define B43_PSM_HDR_MAC_PHY_CLOCK_EN	0x00000002
+#define B43_PSM_HDR_MAC_PHY_FORCE_CLK	0x00000004
+
 /* See BCMA_CLKCTLST_EXTRESREQ and BCMA_CLKCTLST_EXTRESST */
 #define B43_BCMA_CLKCTLST_80211_PLL_REQ	0x00000100
 #define B43_BCMA_CLKCTLST_PHY_PLL_REQ	0x00000200
@@ -791,6 +801,13 @@ struct b43_firmware {
 	bool pcm_request_failed;
 };
 
+enum b43_band {
+	B43_BAND_2G = 0,
+	B43_BAND_5G_LO = 1,
+	B43_BAND_5G_MI = 2,
+	B43_BAND_5G_HI = 3,
+};
+
 /* Device (802.11 core) initialization status. */
 enum {
 	B43_STAT_UNINIT = 0,	/* Uninitialized. */
@@ -1012,6 +1029,16 @@ static inline void b43_write16(struct b43_wldev *dev, u16 offset, u16 value)
 	dev->dev->write16(dev->dev, offset, value);
 }
 
+/* To optimize this check for flush_writes on BCM47XX_BCMA only. */
+static inline void b43_write16f(struct b43_wldev *dev, u16 offset, u16 value)
+{
+	b43_write16(dev, offset, value);
+#if defined(CONFIG_BCM47XX_BCMA)
+	if (dev->dev->flush_writes)
+		b43_read16(dev, offset);
+#endif
+}
+
 static inline void b43_maskset16(struct b43_wldev *dev, u16 offset, u16 mask,
 				 u16 set)
 {
diff --git a/drivers/net/wireless/b43/bus.c b/drivers/net/wireless/b43/bus.c
index 565fdbdd6915..17d16a391fe6 100644
--- a/drivers/net/wireless/b43/bus.c
+++ b/drivers/net/wireless/b43/bus.c
@@ -22,6 +22,10 @@
 
 */
 
+#ifdef CONFIG_BCM47XX_BCMA
+#include <asm/mach-bcm47xx/bcm47xx.h>
+#endif
+
 #include "b43.h"
 #include "bus.h"
 
@@ -102,6 +106,12 @@ struct b43_bus_dev *b43_bus_dev_bcma_init(struct bcma_device *core)
 	dev->write32 = b43_bus_bcma_write32;
 	dev->block_read = b43_bus_bcma_block_read;
 	dev->block_write = b43_bus_bcma_block_write;
+#ifdef CONFIG_BCM47XX_BCMA
+	if (b43_bus_host_is_pci(dev) &&
+	    bcm47xx_bus_type == BCM47XX_BUS_TYPE_BCMA &&
+	    bcm47xx_bus.bcma.bus.chipinfo.id == BCMA_CHIP_ID_BCM4716)
+		dev->flush_writes = true;
+#endif
 
 	dev->dev = &core->dev;
 	dev->dma_dev = core->dma_dev;
diff --git a/drivers/net/wireless/b43/bus.h b/drivers/net/wireless/b43/bus.h
index f3205c6988bc..256c2c17939a 100644
--- a/drivers/net/wireless/b43/bus.h
+++ b/drivers/net/wireless/b43/bus.h
@@ -33,6 +33,7 @@ struct b43_bus_dev {
 			   size_t count, u16 offset, u8 reg_width);
 	void (*block_write)(struct b43_bus_dev *dev, const void *buffer,
 			    size_t count, u16 offset, u8 reg_width);
+	bool flush_writes;
 
 	struct device *dev;
 	struct device *dma_dev;
@@ -60,7 +61,21 @@ static inline bool b43_bus_host_is_pcmcia(struct b43_bus_dev *dev)
 #else
 	return false;
 #endif
+};
+
+static inline bool b43_bus_host_is_pci(struct b43_bus_dev *dev)
+{
+#ifdef CONFIG_B43_BCMA
+	if (dev->bus_type == B43_BUS_BCMA)
+		return (dev->bdev->bus->hosttype == BCMA_HOSTTYPE_PCI);
+#endif
+#ifdef CONFIG_B43_SSB
+	if (dev->bus_type == B43_BUS_SSB)
+		return (dev->sdev->bus->bustype == SSB_BUSTYPE_PCI);
+#endif
+	return false;
 }
+
 static inline bool b43_bus_host_is_sdio(struct b43_bus_dev *dev)
 {
 #ifdef CONFIG_B43_SSB
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 2af1ac396eb4..5d4173ee55bc 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -1204,6 +1204,36 @@ void b43_power_saving_ctl_bits(struct b43_wldev *dev, unsigned int ps_flags)
 	}
 }
 
+/* http://bcm-v4.sipsolutions.net/802.11/PHY/BmacCorePllReset */
+void b43_wireless_core_phy_pll_reset(struct b43_wldev *dev)
+{
+	struct bcma_drv_cc *bcma_cc __maybe_unused;
+	struct ssb_chipcommon *ssb_cc __maybe_unused;
+
+	switch (dev->dev->bus_type) {
+#ifdef CONFIG_B43_BCMA
+	case B43_BUS_BCMA:
+		bcma_cc = &dev->dev->bdev->bus->drv_cc;
+
+		bcma_cc_write32(bcma_cc, BCMA_CC_CHIPCTL_ADDR, 0);
+		bcma_cc_mask32(bcma_cc, BCMA_CC_CHIPCTL_DATA, ~0x4);
+		bcma_cc_set32(bcma_cc, BCMA_CC_CHIPCTL_DATA, 0x4);
+		bcma_cc_mask32(bcma_cc, BCMA_CC_CHIPCTL_DATA, ~0x4);
+		break;
+#endif
+#ifdef CONFIG_B43_SSB
+	case B43_BUS_SSB:
+		ssb_cc = &dev->dev->sdev->bus->chipco;
+
+		chipco_write32(ssb_cc, SSB_CHIPCO_CHIPCTL_ADDR, 0);
+		chipco_mask32(ssb_cc, SSB_CHIPCO_CHIPCTL_DATA, ~0x4);
+		chipco_set32(ssb_cc, SSB_CHIPCO_CHIPCTL_DATA, 0x4);
+		chipco_mask32(ssb_cc, SSB_CHIPCO_CHIPCTL_DATA, ~0x4);
+		break;
+#endif
+	}
+}
+
 #ifdef CONFIG_B43_BCMA
 static void b43_bcma_phy_reset(struct b43_wldev *dev)
 {
@@ -2985,7 +3015,22 @@ void b43_mac_switch_freq(struct b43_wldev *dev, u8 spurmode)
 {
 	u16 chip_id = dev->dev->chip_id;
 
-	if (chip_id == BCMA_CHIP_ID_BCM43131 ||
+	if (chip_id == BCMA_CHIP_ID_BCM4331) {
+		switch (spurmode) {
+		case 2: /* 168 Mhz: 2^26/168 = 0x61862 */
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, 0x1862);
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x6);
+			break;
+		case 1: /* 164 Mhz: 2^26/164 = 0x63e70 */
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, 0x3e70);
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x6);
+			break;
+		default: /* 160 Mhz: 2^26/160 = 0x66666 */
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, 0x6666);
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x6);
+			break;
+		}
+	} else if (chip_id == BCMA_CHIP_ID_BCM43131 ||
 	    chip_id == BCMA_CHIP_ID_BCM43217 ||
 	    chip_id == BCMA_CHIP_ID_BCM43222 ||
 	    chip_id == BCMA_CHIP_ID_BCM43224 ||
@@ -3106,6 +3151,7 @@ static void b43_rate_memory_init(struct b43_wldev *dev)
 	case B43_PHYTYPE_HT:
 	case B43_PHYTYPE_LCN:
 		b43_rate_memory_write(dev, B43_OFDM_RATE_6MB, 1);
+		b43_rate_memory_write(dev, B43_OFDM_RATE_9MB, 1);
 		b43_rate_memory_write(dev, B43_OFDM_RATE_12MB, 1);
 		b43_rate_memory_write(dev, B43_OFDM_RATE_18MB, 1);
 		b43_rate_memory_write(dev, B43_OFDM_RATE_24MB, 1);
@@ -3884,6 +3930,12 @@ static int b43_switch_band(struct b43_wldev *dev,
 	return 0;
 }
 
+static void b43_set_beacon_listen_interval(struct b43_wldev *dev, u16 interval)
+{
+	interval = min_t(u16, interval, (u16)0xFF);
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BCN_LI, interval);
+}
+
 /* Write the short and long frame retry limit values. */
 static void b43_set_retry_limits(struct b43_wldev *dev,
 				 unsigned int short_retry,
@@ -3912,6 +3964,9 @@ static int b43_op_config(struct ieee80211_hw *hw, u32 changed)
 	mutex_lock(&wl->mutex);
 	b43_mac_suspend(dev);
 
+	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL)
+		b43_set_beacon_listen_interval(dev, conf->listen_interval);
+
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
 		phy->chandef = &conf->chandef;
 		phy->channel = conf->chandef.chan->hw_value;
@@ -4466,10 +4521,10 @@ static int b43_phy_versioning(struct b43_wldev *dev)
 	if (core_rev == 40 || core_rev == 42) {
 		radio_manuf = 0x17F;
 
-		b43_write16(dev, B43_MMIO_RADIO24_CONTROL, 0);
+		b43_write16f(dev, B43_MMIO_RADIO24_CONTROL, 0);
 		radio_rev = b43_read16(dev, B43_MMIO_RADIO24_DATA);
 
-		b43_write16(dev, B43_MMIO_RADIO24_CONTROL, 1);
+		b43_write16f(dev, B43_MMIO_RADIO24_CONTROL, 1);
 		radio_id = b43_read16(dev, B43_MMIO_RADIO24_DATA);
 
 		radio_ver = 0; /* Is there version somewhere? */
@@ -4477,7 +4532,7 @@ static int b43_phy_versioning(struct b43_wldev *dev)
 		u16 radio24[3];
 
 		for (tmp = 0; tmp < 3; tmp++) {
-			b43_write16(dev, B43_MMIO_RADIO24_CONTROL, tmp);
+			b43_write16f(dev, B43_MMIO_RADIO24_CONTROL, tmp);
 			radio24[tmp] = b43_read16(dev, B43_MMIO_RADIO24_DATA);
 		}
 
@@ -4494,13 +4549,12 @@ static int b43_phy_versioning(struct b43_wldev *dev)
 			else
 				tmp = 0x5205017F;
 		} else {
-			b43_write16(dev, B43_MMIO_RADIO_CONTROL,
-				    B43_RADIOCTL_ID);
+			b43_write16f(dev, B43_MMIO_RADIO_CONTROL,
+				     B43_RADIOCTL_ID);
 			tmp = b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
-			b43_write16(dev, B43_MMIO_RADIO_CONTROL,
-				    B43_RADIOCTL_ID);
-			tmp |= (u32)b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH)
-				<< 16;
+			b43_write16f(dev, B43_MMIO_RADIO_CONTROL,
+				     B43_RADIOCTL_ID);
+			tmp |= b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH) << 16;
 		}
 		radio_manuf = (tmp & 0x00000FFF);
 		radio_id = (tmp & 0x0FFFF000) >> 12;
@@ -4813,6 +4867,16 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
 	hf &= ~B43_HF_SKCFPUP;
 	b43_hf_write(dev, hf);
 
+	/* tell the ucode MAC capabilities */
+	if (dev->dev->core_rev >= 13) {
+		u32 mac_hw_cap = b43_read32(dev, B43_MMIO_MAC_HW_CAP);
+
+		b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_MACHW_L,
+				mac_hw_cap & 0xffff);
+		b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_MACHW_H,
+				(mac_hw_cap >> 16) & 0xffff);
+	}
+
 	b43_set_retry_limits(dev, B43_DEFAULT_SHORT_RETRY_LIMIT,
 			     B43_DEFAULT_LONG_RETRY_LIMIT);
 	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SFFBLIM, 3);
@@ -4835,6 +4899,10 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
 	/* Maximum Contention Window */
 	b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
 
+	/* write phytype and phyvers */
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PHYTYPE, phy->type);
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PHYVER, phy->rev);
+
 	if (b43_bus_host_is_pcmcia(dev->dev) ||
 	    b43_bus_host_is_sdio(dev->dev)) {
 		dev->__using_pio_transfers = true;
diff --git a/drivers/net/wireless/b43/main.h b/drivers/net/wireless/b43/main.h
index 9f22e4b4c132..c46430cc725c 100644
--- a/drivers/net/wireless/b43/main.h
+++ b/drivers/net/wireless/b43/main.h
@@ -96,6 +96,8 @@ void b43_controller_restart(struct b43_wldev *dev, const char *reason);
 #define B43_PS_ASLEEP	(1 << 3)	/* Force device asleep */
 void b43_power_saving_ctl_bits(struct b43_wldev *dev, unsigned int ps_flags);
 
+void b43_wireless_core_phy_pll_reset(struct b43_wldev *dev);
+
 void b43_mac_suspend(struct b43_wldev *dev);
 void b43_mac_enable(struct b43_wldev *dev);
 void b43_mac_phy_clock_set(struct b43_wldev *dev, bool on);
diff --git a/drivers/net/wireless/b43/phy_a.c b/drivers/net/wireless/b43/phy_a.c
index 25e40432d68b..99c036f5ecb7 100644
--- a/drivers/net/wireless/b43/phy_a.c
+++ b/drivers/net/wireless/b43/phy_a.c
@@ -444,14 +444,14 @@ static inline u16 adjust_phyreg(struct b43_wldev *dev, u16 offset)
 static u16 b43_aphy_op_read(struct b43_wldev *dev, u16 reg)
 {
 	reg = adjust_phyreg(dev, reg);
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	return b43_read16(dev, B43_MMIO_PHY_DATA);
 }
 
 static void b43_aphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
 {
 	reg = adjust_phyreg(dev, reg);
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_PHY_DATA, value);
 }
 
diff --git a/drivers/net/wireless/b43/phy_common.c b/drivers/net/wireless/b43/phy_common.c
index 3cbef21b4726..1dfc682a8055 100644
--- a/drivers/net/wireless/b43/phy_common.c
+++ b/drivers/net/wireless/b43/phy_common.c
@@ -222,12 +222,18 @@ static inline void assert_mac_suspended(struct b43_wldev *dev)
 u16 b43_radio_read(struct b43_wldev *dev, u16 reg)
 {
 	assert_mac_suspended(dev);
+	dev->phy.writes_counter = 0;
 	return dev->phy.ops->radio_read(dev, reg);
 }
 
 void b43_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
 {
 	assert_mac_suspended(dev);
+	if (b43_bus_host_is_pci(dev->dev) &&
+	    ++dev->phy.writes_counter > B43_MAX_WRITES_IN_ROW) {
+		b43_read32(dev, B43_MMIO_MACCTL);
+		dev->phy.writes_counter = 1;
+	}
 	dev->phy.ops->radio_write(dev, reg, value);
 }
 
@@ -268,17 +274,28 @@ u16 b43_phy_read(struct b43_wldev *dev, u16 reg)
 {
 	assert_mac_suspended(dev);
 	dev->phy.writes_counter = 0;
-	return dev->phy.ops->phy_read(dev, reg);
+
+	if (dev->phy.ops->phy_read)
+		return dev->phy.ops->phy_read(dev, reg);
+
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
+	return b43_read16(dev, B43_MMIO_PHY_DATA);
 }
 
 void b43_phy_write(struct b43_wldev *dev, u16 reg, u16 value)
 {
 	assert_mac_suspended(dev);
-	dev->phy.ops->phy_write(dev, reg, value);
-	if (++dev->phy.writes_counter == B43_MAX_WRITES_IN_ROW) {
+	if (b43_bus_host_is_pci(dev->dev) &&
+	    ++dev->phy.writes_counter > B43_MAX_WRITES_IN_ROW) {
 		b43_read16(dev, B43_MMIO_PHY_VER);
-		dev->phy.writes_counter = 0;
+		dev->phy.writes_counter = 1;
 	}
+
+	if (dev->phy.ops->phy_write)
+		return dev->phy.ops->phy_write(dev, reg, value);
+
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16(dev, B43_MMIO_PHY_DATA, value);
 }
 
 void b43_phy_copy(struct b43_wldev *dev, u16 destreg, u16 srcreg)
diff --git a/drivers/net/wireless/b43/phy_g.c b/drivers/net/wireless/b43/phy_g.c
index 8f5c14bc10e6..727ce6edb4b3 100644
--- a/drivers/net/wireless/b43/phy_g.c
+++ b/drivers/net/wireless/b43/phy_g.c
@@ -2555,13 +2555,13 @@ static void b43_gphy_op_exit(struct b43_wldev *dev)
 
 static u16 b43_gphy_op_read(struct b43_wldev *dev, u16 reg)
 {
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	return b43_read16(dev, B43_MMIO_PHY_DATA);
 }
 
 static void b43_gphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
 {
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_PHY_DATA, value);
 }
 
@@ -2572,7 +2572,7 @@ static u16 b43_gphy_op_radio_read(struct b43_wldev *dev, u16 reg)
 	/* G-PHY needs 0x80 for read access. */
 	reg |= 0x80;
 
-	b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
 	return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
 }
 
@@ -2581,7 +2581,7 @@ static void b43_gphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
 	/* Register 1 is a 32-bit register. */
 	B43_WARN_ON(reg == 1);
 
-	b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
 }
 
diff --git a/drivers/net/wireless/b43/phy_ht.c b/drivers/net/wireless/b43/phy_ht.c
index f2974c6b1c01..bd68945965d6 100644
--- a/drivers/net/wireless/b43/phy_ht.c
+++ b/drivers/net/wireless/b43/phy_ht.c
@@ -81,80 +81,104 @@ static void b43_radio_2059_channel_setup(struct b43_wldev *dev,
 	udelay(50);
 
 	/* Calibration */
-	b43_radio_mask(dev, 0x2b, ~0x1);
-	b43_radio_mask(dev, 0x2e, ~0x4);
-	b43_radio_set(dev, 0x2e, 0x4);
-	b43_radio_set(dev, 0x2b, 0x1);
+	b43_radio_mask(dev, R2059_RFPLL_MISC_EN, ~0x1);
+	b43_radio_mask(dev, R2059_RFPLL_MISC_CAL_RESETN, ~0x4);
+	b43_radio_set(dev, R2059_RFPLL_MISC_CAL_RESETN, 0x4);
+	b43_radio_set(dev, R2059_RFPLL_MISC_EN, 0x1);
 
 	udelay(300);
 }
 
-static void b43_radio_2059_init(struct b43_wldev *dev)
+/* Calibrate resistors in LPF of PLL? */
+static void b43_radio_2059_rcal(struct b43_wldev *dev)
+{
+	/* Enable */
+	b43_radio_set(dev, R2059_C3 | R2059_RCAL_CONFIG, 0x1);
+	usleep_range(10, 20);
+
+	b43_radio_set(dev, R2059_C3 | 0x0BF, 0x1);
+	b43_radio_maskset(dev, R2059_C3 | 0x19B, 0x3, 0x2);
+
+	/* Start */
+	b43_radio_set(dev, R2059_C3 | R2059_RCAL_CONFIG, 0x2);
+	usleep_range(100, 200);
+
+	/* Stop */
+	b43_radio_mask(dev, R2059_C3 | R2059_RCAL_CONFIG, ~0x2);
+
+	if (!b43_radio_wait_value(dev, R2059_C3 | R2059_RCAL_STATUS, 1, 1, 100,
+				  1000000))
+		b43err(dev->wl, "Radio 0x2059 rcal timeout\n");
+
+	/* Disable */
+	b43_radio_mask(dev, R2059_C3 | R2059_RCAL_CONFIG, ~0x1);
+
+	b43_radio_set(dev, 0xa, 0x60);
+}
+
+/* Calibrate the internal RC oscillator? */
+static void b43_radio_2057_rccal(struct b43_wldev *dev)
 {
-	const u16 routing[] = { R2059_C1, R2059_C2, R2059_C3 };
 	const u16 radio_values[3][2] = {
 		{ 0x61, 0xE9 }, { 0x69, 0xD5 }, { 0x73, 0x99 },
 	};
-	u16 i, j;
+	int i;
 
-	b43_radio_write(dev, R2059_ALL | 0x51, 0x0070);
-	b43_radio_write(dev, R2059_ALL | 0x5a, 0x0003);
+	for (i = 0; i < 3; i++) {
+		b43_radio_write(dev, R2059_RCCAL_MASTER, radio_values[i][0]);
+		b43_radio_write(dev, R2059_RCCAL_X1, 0x6E);
+		b43_radio_write(dev, R2059_RCCAL_TRC0, radio_values[i][1]);
 
-	for (i = 0; i < ARRAY_SIZE(routing); i++)
-		b43_radio_set(dev, routing[i] | 0x146, 0x3);
+		/* Start */
+		b43_radio_write(dev, R2059_RCCAL_START_R1_Q1_P1, 0x55);
 
-	b43_radio_set(dev, 0x2e, 0x0078);
-	b43_radio_set(dev, 0xc0, 0x0080);
-	msleep(2);
-	b43_radio_mask(dev, 0x2e, ~0x0078);
-	b43_radio_mask(dev, 0xc0, ~0x0080);
+		/* Wait */
+		if (!b43_radio_wait_value(dev, R2059_RCCAL_DONE_OSCCAP, 2, 2,
+					  500, 5000000))
+			b43err(dev->wl, "Radio 0x2059 rccal timeout\n");
 
-	if (1) { /* FIXME */
-		b43_radio_set(dev, R2059_C3 | 0x4, 0x1);
-		udelay(10);
-		b43_radio_set(dev, R2059_C3 | 0x0BF, 0x1);
-		b43_radio_maskset(dev, R2059_C3 | 0x19B, 0x3, 0x2);
+		/* Stop */
+		b43_radio_write(dev, R2059_RCCAL_START_R1_Q1_P1, 0x15);
+	}
 
-		b43_radio_set(dev, R2059_C3 | 0x4, 0x2);
-		udelay(100);
-		b43_radio_mask(dev, R2059_C3 | 0x4, ~0x2);
+	b43_radio_mask(dev, R2059_RCCAL_MASTER, ~0x1);
+}
 
-		for (i = 0; i < 10000; i++) {
-			if (b43_radio_read(dev, R2059_C3 | 0x145) & 1) {
-				i = 0;
-				break;
-			}
-			udelay(100);
-		}
-		if (i)
-			b43err(dev->wl, "radio 0x945 timeout\n");
-
-		b43_radio_mask(dev, R2059_C3 | 0x4, ~0x1);
-		b43_radio_set(dev, 0xa, 0x60);
-
-		for (i = 0; i < 3; i++) {
-			b43_radio_write(dev, 0x17F, radio_values[i][0]);
-			b43_radio_write(dev, 0x13D, 0x6E);
-			b43_radio_write(dev, 0x13E, radio_values[i][1]);
-			b43_radio_write(dev, 0x13C, 0x55);
-
-			for (j = 0; j < 10000; j++) {
-				if (b43_radio_read(dev, 0x140) & 2) {
-					j = 0;
-					break;
-				}
-				udelay(500);
-			}
-			if (j)
-				b43err(dev->wl, "radio 0x140 timeout\n");
+static void b43_radio_2059_init_pre(struct b43_wldev *dev)
+{
+	b43_phy_mask(dev, B43_PHY_HT_RF_CTL_CMD, ~B43_PHY_HT_RF_CTL_CMD_CHIP0_PU);
+	b43_phy_set(dev, B43_PHY_HT_RF_CTL_CMD, B43_PHY_HT_RF_CTL_CMD_FORCE);
+	b43_phy_mask(dev, B43_PHY_HT_RF_CTL_CMD, ~B43_PHY_HT_RF_CTL_CMD_FORCE);
+	b43_phy_set(dev, B43_PHY_HT_RF_CTL_CMD, B43_PHY_HT_RF_CTL_CMD_CHIP0_PU);
+}
 
-			b43_radio_write(dev, 0x13C, 0x15);
-		}
+static void b43_radio_2059_init(struct b43_wldev *dev)
+{
+	const u16 routing[] = { R2059_C1, R2059_C2, R2059_C3 };
+	int i;
 
-		b43_radio_mask(dev, 0x17F, ~0x1);
+	/* Prepare (reset?) radio */
+	b43_radio_2059_init_pre(dev);
+
+	r2059_upload_inittabs(dev);
+
+	for (i = 0; i < ARRAY_SIZE(routing); i++)
+		b43_radio_set(dev, routing[i] | 0x146, 0x3);
+
+	/* Post init starts below */
+
+	b43_radio_set(dev, R2059_RFPLL_MISC_CAL_RESETN, 0x0078);
+	b43_radio_set(dev, R2059_XTAL_CONFIG2, 0x0080);
+	msleep(2);
+	b43_radio_mask(dev, R2059_RFPLL_MISC_CAL_RESETN, ~0x0078);
+	b43_radio_mask(dev, R2059_XTAL_CONFIG2, ~0x0080);
+
+	if (1) { /* FIXME */
+		b43_radio_2059_rcal(dev);
+		b43_radio_2057_rccal(dev);
 	}
 
-	b43_radio_mask(dev, 0x11, ~0x0008);
+	b43_radio_mask(dev, R2059_RFPLL_MASTER, ~0x0008);
 }
 
 /**************************************************
@@ -297,6 +321,26 @@ static void b43_phy_ht_bphy_init(struct b43_wldev *dev)
 	b43_phy_write(dev, B43_PHY_N_BMODE(0x38), 0x668);
 }
 
+static void b43_phy_ht_bphy_reset(struct b43_wldev *dev, bool reset)
+{
+	u16 tmp;
+
+	tmp = b43_read16(dev, B43_MMIO_PSM_PHY_HDR);
+	b43_write16(dev, B43_MMIO_PSM_PHY_HDR,
+		    tmp | B43_PSM_HDR_MAC_PHY_FORCE_CLK);
+
+	/* Put BPHY in or take it out of the reset */
+	if (reset)
+		b43_phy_set(dev, B43_PHY_B_BBCFG,
+			    B43_PHY_B_BBCFG_RSTCCA | B43_PHY_B_BBCFG_RSTRX);
+	else
+		b43_phy_mask(dev, B43_PHY_B_BBCFG,
+			     (u16)~(B43_PHY_B_BBCFG_RSTCCA |
+				    B43_PHY_B_BBCFG_RSTRX));
+
+	b43_write16(dev, B43_MMIO_PSM_PHY_HDR, tmp);
+}
+
 /**************************************************
  * Samples
  **************************************************/
@@ -704,7 +748,6 @@ static void b43_phy_ht_spur_avoid(struct b43_wldev *dev,
 {
 	struct bcma_device *core = dev->dev->bdev;
 	int spuravoid = 0;
-	u16 tmp;
 
 	/* Check for 13 and 14 is just a guess, we don't have enough logs. */
 	if (new_channel->hw_value == 13 || new_channel->hw_value == 14)
@@ -717,22 +760,9 @@ static void b43_phy_ht_spur_avoid(struct b43_wldev *dev,
 			  B43_BCMA_CLKCTLST_80211_PLL_ST |
 			  B43_BCMA_CLKCTLST_PHY_PLL_ST, false);
 
-	/* Values has been taken from wlc_bmac_switch_macfreq comments */
-	switch (spuravoid) {
-	case 2: /* 126MHz */
-		tmp = 0x2082;
-		break;
-	case 1: /* 123MHz */
-		tmp = 0x5341;
-		break;
-	default: /* 120MHz */
-		tmp = 0x8889;
-	}
-
-	b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, tmp);
-	b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x8);
+	b43_mac_switch_freq(dev, spuravoid);
 
-	/* TODO: reset PLL */
+	b43_wireless_core_phy_pll_reset(dev);
 
 	if (spuravoid)
 		b43_phy_set(dev, B43_PHY_HT_BBCFG, B43_PHY_HT_BBCFG_RSTRX);
@@ -747,13 +777,19 @@ static void b43_phy_ht_channel_setup(struct b43_wldev *dev,
 				const struct b43_phy_ht_channeltab_e_phy *e,
 				struct ieee80211_channel *new_channel)
 {
-	bool old_band_5ghz;
+	if (new_channel->band == IEEE80211_BAND_5GHZ) {
+		/* Switch to 2 GHz for a moment to access B-PHY regs */
+		b43_phy_mask(dev, B43_PHY_HT_BANDCTL, ~B43_PHY_HT_BANDCTL_5GHZ);
+
+		b43_phy_ht_bphy_reset(dev, true);
+
+		/* Switch to 5 GHz */
+		b43_phy_set(dev, B43_PHY_HT_BANDCTL, B43_PHY_HT_BANDCTL_5GHZ);
+	} else {
+		/* Switch to 2 GHz */
+		b43_phy_mask(dev, B43_PHY_HT_BANDCTL, ~B43_PHY_HT_BANDCTL_5GHZ);
 
-	old_band_5ghz = b43_phy_read(dev, B43_PHY_HT_BANDCTL) & 0; /* FIXME */
-	if (new_channel->band == IEEE80211_BAND_5GHZ && !old_band_5ghz) {
-		/* TODO */
-	} else if (new_channel->band == IEEE80211_BAND_2GHZ && old_band_5ghz) {
-		/* TODO */
+		b43_phy_ht_bphy_reset(dev, false);
 	}
 
 	b43_phy_write(dev, B43_PHY_HT_BW1, e->bw1);
@@ -1002,19 +1038,10 @@ static void b43_phy_ht_op_software_rfkill(struct b43_wldev *dev,
 	if (b43_read32(dev, B43_MMIO_MACCTL) & B43_MACCTL_ENABLED)
 		b43err(dev->wl, "MAC not suspended\n");
 
-	/* In the following PHY ops we copy wl's dummy behaviour.
-	 * TODO: Find out if reads (currently hidden in masks/masksets) are
-	 * needed and replace following ops with just writes or w&r.
-	 * Note: B43_PHY_HT_RF_CTL1 register is tricky, wrong operation can
-	 * cause delayed (!) machine lock up. */
 	if (blocked) {
-		b43_phy_mask(dev, B43_PHY_HT_RF_CTL1, 0);
+		b43_phy_mask(dev, B43_PHY_HT_RF_CTL_CMD,
+			     ~B43_PHY_HT_RF_CTL_CMD_CHIP0_PU);
 	} else {
-		b43_phy_mask(dev, B43_PHY_HT_RF_CTL1, 0);
-		b43_phy_maskset(dev, B43_PHY_HT_RF_CTL1, 0, 0x1);
-		b43_phy_mask(dev, B43_PHY_HT_RF_CTL1, 0);
-		b43_phy_maskset(dev, B43_PHY_HT_RF_CTL1, 0, 0x2);
-
 		if (dev->phy.radio_ver == 0x2059)
 			b43_radio_2059_init(dev);
 		else
@@ -1071,22 +1098,10 @@ static unsigned int b43_phy_ht_op_get_default_chan(struct b43_wldev *dev)
  * R/W ops.
  **************************************************/
 
-static u16 b43_phy_ht_op_read(struct b43_wldev *dev, u16 reg)
-{
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	return b43_read16(dev, B43_MMIO_PHY_DATA);
-}
-
-static void b43_phy_ht_op_write(struct b43_wldev *dev, u16 reg, u16 value)
-{
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	b43_write16(dev, B43_MMIO_PHY_DATA, value);
-}
-
 static void b43_phy_ht_op_maskset(struct b43_wldev *dev, u16 reg, u16 mask,
 				 u16 set)
 {
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_PHY_DATA,
 		    (b43_read16(dev, B43_MMIO_PHY_DATA) & mask) | set);
 }
@@ -1096,14 +1111,14 @@ static u16 b43_phy_ht_op_radio_read(struct b43_wldev *dev, u16 reg)
 	/* HT-PHY needs 0x200 for read access */
 	reg |= 0x200;
 
-	b43_write16(dev, B43_MMIO_RADIO24_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO24_CONTROL, reg);
 	return b43_read16(dev, B43_MMIO_RADIO24_DATA);
 }
 
 static void b43_phy_ht_op_radio_write(struct b43_wldev *dev, u16 reg,
 				      u16 value)
 {
-	b43_write16(dev, B43_MMIO_RADIO24_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO24_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_RADIO24_DATA, value);
 }
 
@@ -1126,8 +1141,6 @@ const struct b43_phy_operations b43_phyops_ht = {
 	.free			= b43_phy_ht_op_free,
 	.prepare_structs	= b43_phy_ht_op_prepare_structs,
 	.init			= b43_phy_ht_op_init,
-	.phy_read		= b43_phy_ht_op_read,
-	.phy_write		= b43_phy_ht_op_write,
 	.phy_maskset		= b43_phy_ht_op_maskset,
 	.radio_read		= b43_phy_ht_op_radio_read,
 	.radio_write		= b43_phy_ht_op_radio_write,
diff --git a/drivers/net/wireless/b43/phy_ht.h b/drivers/net/wireless/b43/phy_ht.h
index 6cae370d1018..c086f56ce478 100644
--- a/drivers/net/wireless/b43/phy_ht.h
+++ b/drivers/net/wireless/b43/phy_ht.h
@@ -81,7 +81,9 @@
 #define B43_PHY_HT_RF_SEQ_STATUS		B43_PHY_EXTG(0x004)
 /* Values for the status are the same as for the trigger */
 
-#define B43_PHY_HT_RF_CTL1			B43_PHY_EXTG(0x010)
+#define B43_PHY_HT_RF_CTL_CMD			0x810
+#define  B43_PHY_HT_RF_CTL_CMD_FORCE		0x0001
+#define  B43_PHY_HT_RF_CTL_CMD_CHIP0_PU		0x0002
 
 #define B43_PHY_HT_RF_CTL_INT_C1		B43_PHY_EXTG(0x04c)
 #define B43_PHY_HT_RF_CTL_INT_C2		B43_PHY_EXTG(0x06c)
@@ -104,6 +106,9 @@
 #define  B43_PHY_HT_TXPCTL_TARG_PWR2_C3_SHIFT	0
 #define B43_PHY_HT_TX_PCTL_STATUS_C3		B43_PHY_EXTG(0x169)
 
+#define B43_PHY_B_BBCFG				B43_PHY_N_BMODE(0x001)
+#define  B43_PHY_B_BBCFG_RSTCCA			0x4000 /* Reset CCA */
+#define  B43_PHY_B_BBCFG_RSTRX			0x8000 /* Reset RX */
 #define B43_PHY_HT_TEST				B43_PHY_N_BMODE(0x00A)
 
 
diff --git a/drivers/net/wireless/b43/phy_lcn.c b/drivers/net/wireless/b43/phy_lcn.c
index e76bbdf3247e..97461ccf3e1e 100644
--- a/drivers/net/wireless/b43/phy_lcn.c
+++ b/drivers/net/wireless/b43/phy_lcn.c
@@ -810,22 +810,10 @@ static void b43_phy_lcn_op_adjust_txpower(struct b43_wldev *dev)
  * R/W ops.
  **************************************************/
 
-static u16 b43_phy_lcn_op_read(struct b43_wldev *dev, u16 reg)
-{
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	return b43_read16(dev, B43_MMIO_PHY_DATA);
-}
-
-static void b43_phy_lcn_op_write(struct b43_wldev *dev, u16 reg, u16 value)
-{
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	b43_write16(dev, B43_MMIO_PHY_DATA, value);
-}
-
 static void b43_phy_lcn_op_maskset(struct b43_wldev *dev, u16 reg, u16 mask,
 				   u16 set)
 {
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_PHY_DATA,
 		    (b43_read16(dev, B43_MMIO_PHY_DATA) & mask) | set);
 }
@@ -835,14 +823,14 @@ static u16 b43_phy_lcn_op_radio_read(struct b43_wldev *dev, u16 reg)
 	/* LCN-PHY needs 0x200 for read access */
 	reg |= 0x200;
 
-	b43_write16(dev, B43_MMIO_RADIO24_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO24_CONTROL, reg);
 	return b43_read16(dev, B43_MMIO_RADIO24_DATA);
 }
 
 static void b43_phy_lcn_op_radio_write(struct b43_wldev *dev, u16 reg,
 				       u16 value)
 {
-	b43_write16(dev, B43_MMIO_RADIO24_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO24_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_RADIO24_DATA, value);
 }
 
@@ -855,8 +843,6 @@ const struct b43_phy_operations b43_phyops_lcn = {
 	.free			= b43_phy_lcn_op_free,
 	.prepare_structs	= b43_phy_lcn_op_prepare_structs,
 	.init			= b43_phy_lcn_op_init,
-	.phy_read		= b43_phy_lcn_op_read,
-	.phy_write		= b43_phy_lcn_op_write,
 	.phy_maskset		= b43_phy_lcn_op_maskset,
 	.radio_read		= b43_phy_lcn_op_radio_read,
 	.radio_write		= b43_phy_lcn_op_radio_write,
diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/b43/phy_lp.c
index 92190dacf689..058a9f232050 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/b43/phy_lp.c
@@ -1985,22 +1985,10 @@ static void lpphy_calibration(struct b43_wldev *dev)
 	b43_mac_enable(dev);
 }
 
-static u16 b43_lpphy_op_read(struct b43_wldev *dev, u16 reg)
-{
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	return b43_read16(dev, B43_MMIO_PHY_DATA);
-}
-
-static void b43_lpphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
-{
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	b43_write16(dev, B43_MMIO_PHY_DATA, value);
-}
-
 static void b43_lpphy_op_maskset(struct b43_wldev *dev, u16 reg, u16 mask,
 				 u16 set)
 {
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_PHY_DATA,
 		    (b43_read16(dev, B43_MMIO_PHY_DATA) & mask) | set);
 }
@@ -2016,7 +2004,7 @@ static u16 b43_lpphy_op_radio_read(struct b43_wldev *dev, u16 reg)
 	} else
 		reg |= 0x200;
 
-	b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
 	return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
 }
 
@@ -2025,7 +2013,7 @@ static void b43_lpphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
 	/* Register 1 is a 32-bit register. */
 	B43_WARN_ON(reg == 1);
 
-	b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
 }
 
@@ -2713,8 +2701,6 @@ const struct b43_phy_operations b43_phyops_lp = {
 	.free			= b43_lpphy_op_free,
 	.prepare_structs	= b43_lpphy_op_prepare_structs,
 	.init			= b43_lpphy_op_init,
-	.phy_read		= b43_lpphy_op_read,
-	.phy_write		= b43_lpphy_op_write,
 	.phy_maskset		= b43_lpphy_op_maskset,
 	.radio_read		= b43_lpphy_op_radio_read,
 	.radio_write		= b43_lpphy_op_radio_write,
diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index e2a3f0d5bcc2..9f0bcf3b8414 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -34,6 +34,7 @@
 #include "radio_2056.h"
 #include "radio_2057.h"
 #include "main.h"
+#include "ppr.h"
 
 struct nphy_txgains {
 	u16 tx_lpf[2];
@@ -3606,16 +3607,6 @@ static void b43_nphy_iq_cal_gain_params(struct b43_wldev *dev, u16 core,
  * Tx and Rx
  **************************************************/
 
-static void b43_nphy_op_adjust_txpower(struct b43_wldev *dev)
-{//TODO
-}
-
-static enum b43_txpwr_result b43_nphy_op_recalc_txpower(struct b43_wldev *dev,
-							bool ignore_tssi)
-{//TODO
-	return B43_TXPWR_RES_DONE;
-}
-
 /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/TxPwrCtrlEnable */
 static void b43_nphy_tx_power_ctrl(struct b43_wldev *dev, bool enable)
 {
@@ -4069,6 +4060,7 @@ static void b43_nphy_tx_power_ctl_setup(struct b43_wldev *dev)
 
 	s16 a1[2], b0[2], b1[2];
 	u8 idle[2];
+	u8 ppr_max;
 	s8 target[2];
 	s32 num, den, pwr;
 	u32 regval[64];
@@ -4147,7 +4139,12 @@ static void b43_nphy_tx_power_ctl_setup(struct b43_wldev *dev)
 			b1[0] = b1[1] = -1393;
 		}
 	}
-	/* target[0] = target[1] = nphy->tx_power_max; */
+
+	ppr_max = b43_ppr_get_max(dev, &nphy->tx_pwr_max_ppr);
+	if (ppr_max) {
+		target[0] = ppr_max;
+		target[1] = ppr_max;
+	}
 
 	if (dev->phy.rev >= 3) {
 		if (sprom->fem.ghz2.tssipos)
@@ -4235,8 +4232,9 @@ static void b43_nphy_tx_gain_table_upload(struct b43_wldev *dev)
 
 	const u32 *table = NULL;
 	u32 rfpwr_offset;
-	u8 pga_gain;
+	u8 pga_gain, pad_gain;
 	int i;
+	const s16 *uninitialized_var(rf_pwr_offset_table);
 
 	table = b43_nphy_get_tx_gain_table(dev);
 	if (!table)
@@ -4252,13 +4250,27 @@ static void b43_nphy_tx_gain_table_upload(struct b43_wldev *dev)
 	nphy->gmval = (table[0] >> 16) & 0x7000;
 #endif
 
+	if (phy->rev >= 19) {
+		return;
+	} else if (phy->rev >= 7) {
+		rf_pwr_offset_table = b43_ntab_get_rf_pwr_offset_table(dev);
+		if (!rf_pwr_offset_table)
+			return;
+		/* TODO: Enable this once we have gains configured */
+		return;
+	}
+
 	for (i = 0; i < 128; i++) {
 		if (phy->rev >= 19) {
 			/* TODO */
 			return;
 		} else if (phy->rev >= 7) {
-			/* TODO */
-			return;
+			pga_gain = (table[i] >> 24) & 0xf;
+			pad_gain = (table[i] >> 19) & 0x1f;
+			if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
+				rfpwr_offset = rf_pwr_offset_table[pad_gain];
+			else
+				rfpwr_offset = rf_pwr_offset_table[pga_gain];
 		} else {
 			pga_gain = (table[i] >> 24) & 0xF;
 			if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
@@ -5874,6 +5886,69 @@ static void b43_nphy_set_rx_core_state(struct b43_wldev *dev, u8 mask)
 	b43_mac_enable(dev);
 }
 
+static enum b43_txpwr_result b43_nphy_op_recalc_txpower(struct b43_wldev *dev,
+							bool ignore_tssi)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_phy_n *nphy = dev->phy.n;
+	struct ieee80211_channel *channel = dev->wl->hw->conf.chandef.chan;
+	struct b43_ppr *ppr = &nphy->tx_pwr_max_ppr;
+	u8 max; /* qdBm */
+	bool tx_pwr_state;
+
+	if (nphy->tx_pwr_last_recalc_freq == channel->center_freq &&
+	    nphy->tx_pwr_last_recalc_limit == phy->desired_txpower)
+		return B43_TXPWR_RES_DONE;
+
+	/* Make sure we have a clean PPR */
+	b43_ppr_clear(dev, ppr);
+
+	/* HW limitations */
+	b43_ppr_load_max_from_sprom(dev, ppr, B43_BAND_2G);
+
+	/* Regulatory & user settings */
+	max = INT_TO_Q52(phy->chandef->chan->max_power);
+	if (phy->desired_txpower)
+		max = min_t(u8, max, INT_TO_Q52(phy->desired_txpower));
+	b43_ppr_apply_max(dev, ppr, max);
+	if (b43_debug(dev, B43_DBG_XMITPOWER))
+		b43dbg(dev->wl, "Calculated TX power: " Q52_FMT "\n",
+		       Q52_ARG(b43_ppr_get_max(dev, ppr)));
+
+	/* TODO: Enable this once we get gains working */
+#if 0
+	/* Some extra gains */
+	hw_gain = 6; /* N-PHY specific */
+	if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ)
+		hw_gain += sprom->antenna_gain.a0;
+	else
+		hw_gain += sprom->antenna_gain.a1;
+	b43_ppr_add(dev, ppr, -hw_gain);
+#endif
+
+	/* Make sure we didn't go too low */
+	b43_ppr_apply_min(dev, ppr, INT_TO_Q52(8));
+
+	/* Apply */
+	tx_pwr_state = nphy->txpwrctrl;
+	b43_mac_suspend(dev);
+	b43_nphy_tx_power_ctl_setup(dev);
+	if (dev->dev->core_rev == 11 || dev->dev->core_rev == 12) {
+		b43_maskset32(dev, B43_MMIO_MACCTL, ~0, B43_MACCTL_PHY_LOCK);
+		b43_read32(dev, B43_MMIO_MACCTL);
+		udelay(1);
+	}
+	b43_nphy_tx_power_ctrl(dev, nphy->txpwrctrl);
+	if (dev->dev->core_rev == 11 || dev->dev->core_rev == 12)
+		b43_maskset32(dev, B43_MMIO_MACCTL, ~B43_MACCTL_PHY_LOCK, 0);
+	b43_mac_enable(dev);
+
+	nphy->tx_pwr_last_recalc_freq = channel->center_freq;
+	nphy->tx_pwr_last_recalc_limit = phy->desired_txpower;
+
+	return B43_TXPWR_RES_DONE;
+}
+
 /**************************************************
  * N-PHY init
  **************************************************/
@@ -6294,7 +6369,7 @@ static void b43_nphy_channel_setup(struct b43_wldev *dev,
 		b43_mac_switch_freq(dev, spuravoid);
 
 		if (dev->phy.rev == 3 || dev->phy.rev == 4)
-			; /* TODO: reset PLL */
+			b43_wireless_core_phy_pll_reset(dev);
 
 		if (spuravoid)
 			b43_phy_set(dev, B43_NPHY_BBCFG, B43_NPHY_BBCFG_RSTRX);
@@ -6407,6 +6482,7 @@ static int b43_nphy_op_allocate(struct b43_wldev *dev)
 	nphy = kzalloc(sizeof(*nphy), GFP_KERNEL);
 	if (!nphy)
 		return -ENOMEM;
+
 	dev->phy.n = nphy;
 
 	return 0;
@@ -6497,26 +6573,13 @@ static inline void check_phyreg(struct b43_wldev *dev, u16 offset)
 #endif /* B43_DEBUG */
 }
 
-static u16 b43_nphy_op_read(struct b43_wldev *dev, u16 reg)
-{
-	check_phyreg(dev, reg);
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	return b43_read16(dev, B43_MMIO_PHY_DATA);
-}
-
-static void b43_nphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
-{
-	check_phyreg(dev, reg);
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
-	b43_write16(dev, B43_MMIO_PHY_DATA, value);
-}
-
 static void b43_nphy_op_maskset(struct b43_wldev *dev, u16 reg, u16 mask,
 				 u16 set)
 {
 	check_phyreg(dev, reg);
-	b43_write16(dev, B43_MMIO_PHY_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_PHY_CONTROL, reg);
 	b43_maskset16(dev, B43_MMIO_PHY_DATA, mask, set);
+	dev->phy.writes_counter = 1;
 }
 
 static u16 b43_nphy_op_radio_read(struct b43_wldev *dev, u16 reg)
@@ -6529,7 +6592,7 @@ static u16 b43_nphy_op_radio_read(struct b43_wldev *dev, u16 reg)
 	else
 		reg |= 0x100;
 
-	b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
 	return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
 }
 
@@ -6538,7 +6601,7 @@ static void b43_nphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
 	/* Register 1 is a 32-bit register. */
 	B43_WARN_ON(dev->phy.rev < 7 && reg == 1);
 
-	b43_write16(dev, B43_MMIO_RADIO_CONTROL, reg);
+	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, reg);
 	b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
 }
 
@@ -6652,8 +6715,6 @@ const struct b43_phy_operations b43_phyops_n = {
 	.free			= b43_nphy_op_free,
 	.prepare_structs	= b43_nphy_op_prepare_structs,
 	.init			= b43_nphy_op_init,
-	.phy_read		= b43_nphy_op_read,
-	.phy_write		= b43_nphy_op_write,
 	.phy_maskset		= b43_nphy_op_maskset,
 	.radio_read		= b43_nphy_op_radio_read,
 	.radio_write		= b43_nphy_op_radio_write,
@@ -6662,5 +6723,4 @@ const struct b43_phy_operations b43_phyops_n = {
 	.switch_channel		= b43_nphy_op_switch_channel,
 	.get_default_chan	= b43_nphy_op_get_default_chan,
 	.recalc_txpower		= b43_nphy_op_recalc_txpower,
-	.adjust_txpower		= b43_nphy_op_adjust_txpower,
 };
diff --git a/drivers/net/wireless/b43/phy_n.h b/drivers/net/wireless/b43/phy_n.h
index 30bec815b969..a6da2c31a99c 100644
--- a/drivers/net/wireless/b43/phy_n.h
+++ b/drivers/net/wireless/b43/phy_n.h
@@ -2,6 +2,7 @@
 #define B43_NPHY_H_
 
 #include "phy_common.h"
+#include "ppr.h"
 
 
 /* N-PHY registers. */
@@ -967,6 +968,9 @@ struct b43_phy_n {
 	struct b43_phy_n_txpwrindex txpwrindex[2];
 	struct b43_phy_n_pwr_ctl_info pwr_ctl_info[2];
 	struct b43_chanspec txiqlocal_chanspec;
+	struct b43_ppr tx_pwr_max_ppr;
+	u16 tx_pwr_last_recalc_freq;
+	int tx_pwr_last_recalc_limit;
 
 	u8 txrx_chain;
 	u16 tx_rx_cal_phy_saveregs[11];
diff --git a/drivers/net/wireless/b43/ppr.c b/drivers/net/wireless/b43/ppr.c
new file mode 100644
index 000000000000..9a770279c415
--- /dev/null
+++ b/drivers/net/wireless/b43/ppr.c
@@ -0,0 +1,199 @@
+/*
+ * Broadcom B43 wireless driver
+ * PPR (Power Per Rate) management
+ *
+ * Copyright (c) 2014 Rafał Miłecki <zajec5@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "ppr.h"
+#include "b43.h"
+
+#define ppr_for_each_entry(ppr, i, entry)				\
+	for (i = 0, entry = &(ppr)->__all_rates[i];			\
+	     i < B43_PPR_RATES_NUM;					\
+	     i++, entry++)
+
+void b43_ppr_clear(struct b43_wldev *dev, struct b43_ppr *ppr)
+{
+	memset(ppr, 0, sizeof(*ppr));
+
+	/* Compile-time PPR check */
+	BUILD_BUG_ON(sizeof(struct b43_ppr) != B43_PPR_RATES_NUM * sizeof(u8));
+}
+
+void b43_ppr_add(struct b43_wldev *dev, struct b43_ppr *ppr, int diff)
+{
+	int i;
+	u8 *rate;
+
+	ppr_for_each_entry(ppr, i, rate) {
+		*rate = clamp_val(*rate + diff, 0, 127);
+	}
+}
+
+void b43_ppr_apply_max(struct b43_wldev *dev, struct b43_ppr *ppr, u8 max)
+{
+	int i;
+	u8 *rate;
+
+	ppr_for_each_entry(ppr, i, rate) {
+		*rate = min(*rate, max);
+	}
+}
+
+void b43_ppr_apply_min(struct b43_wldev *dev, struct b43_ppr *ppr, u8 min)
+{
+	int i;
+	u8 *rate;
+
+	ppr_for_each_entry(ppr, i, rate) {
+		*rate = max(*rate, min);
+	}
+}
+
+u8 b43_ppr_get_max(struct b43_wldev *dev, struct b43_ppr *ppr)
+{
+	u8 res = 0;
+	int i;
+	u8 *rate;
+
+	ppr_for_each_entry(ppr, i, rate) {
+		res = max(*rate, res);
+	}
+
+	return res;
+}
+
+bool b43_ppr_load_max_from_sprom(struct b43_wldev *dev, struct b43_ppr *ppr,
+				 enum b43_band band)
+{
+	struct b43_ppr_rates *rates = &ppr->rates;
+	struct ssb_sprom *sprom = dev->dev->bus_sprom;
+	struct b43_phy *phy = &dev->phy;
+	u8 maxpwr, off;
+	u32 sprom_ofdm_po;
+	u16 *sprom_mcs_po;
+	u8 extra_cdd_po, extra_stbc_po;
+	int i;
+
+	switch (band) {
+	case B43_BAND_2G:
+		maxpwr = min(sprom->core_pwr_info[0].maxpwr_2g,
+			     sprom->core_pwr_info[1].maxpwr_2g);
+		sprom_ofdm_po = sprom->ofdm2gpo;
+		sprom_mcs_po = sprom->mcs2gpo;
+		extra_cdd_po = (sprom->cddpo >> 0) & 0xf;
+		extra_stbc_po = (sprom->stbcpo >> 0) & 0xf;
+		break;
+	case B43_BAND_5G_LO:
+		maxpwr = min(sprom->core_pwr_info[0].maxpwr_5gl,
+			     sprom->core_pwr_info[1].maxpwr_5gl);
+		sprom_ofdm_po = sprom->ofdm5glpo;
+		sprom_mcs_po = sprom->mcs5glpo;
+		extra_cdd_po = (sprom->cddpo >> 8) & 0xf;
+		extra_stbc_po = (sprom->stbcpo >> 8) & 0xf;
+		break;
+	case B43_BAND_5G_MI:
+		maxpwr = min(sprom->core_pwr_info[0].maxpwr_5g,
+			     sprom->core_pwr_info[1].maxpwr_5g);
+		sprom_ofdm_po = sprom->ofdm5gpo;
+		sprom_mcs_po = sprom->mcs5gpo;
+		extra_cdd_po = (sprom->cddpo >> 4) & 0xf;
+		extra_stbc_po = (sprom->stbcpo >> 4) & 0xf;
+		break;
+	case B43_BAND_5G_HI:
+		maxpwr = min(sprom->core_pwr_info[0].maxpwr_5gh,
+			     sprom->core_pwr_info[1].maxpwr_5gh);
+		sprom_ofdm_po = sprom->ofdm5ghpo;
+		sprom_mcs_po = sprom->mcs5ghpo;
+		extra_cdd_po = (sprom->cddpo >> 12) & 0xf;
+		extra_stbc_po = (sprom->stbcpo >> 12) & 0xf;
+		break;
+	default:
+		WARN_ON_ONCE(1);
+		return false;
+	}
+
+	if (band == B43_BAND_2G) {
+		for (i = 0; i < 4; i++) {
+			off = ((sprom->cck2gpo >> (i * 4)) & 0xf) * 2;
+			rates->cck[i] = maxpwr - off;
+		}
+	}
+
+	/* OFDM */
+	for (i = 0; i < 8; i++) {
+		off = ((sprom_ofdm_po >> (i * 4)) & 0xf) * 2;
+		rates->ofdm[i] = maxpwr - off;
+	}
+
+	/* MCS 20 SISO */
+	rates->mcs_20[0] = rates->ofdm[0];
+	rates->mcs_20[1] = rates->ofdm[2];
+	rates->mcs_20[2] = rates->ofdm[3];
+	rates->mcs_20[3] = rates->ofdm[4];
+	rates->mcs_20[4] = rates->ofdm[5];
+	rates->mcs_20[5] = rates->ofdm[6];
+	rates->mcs_20[6] = rates->ofdm[7];
+	rates->mcs_20[7] = rates->ofdm[7];
+
+	/* MCS 20 CDD */
+	for (i = 0; i < 4; i++) {
+		off = ((sprom_mcs_po[0] >> (i * 4)) & 0xf) * 2;
+		rates->mcs_20_cdd[i] = maxpwr - off;
+		if (phy->type == B43_PHYTYPE_N && phy->rev >= 3)
+			rates->mcs_20_cdd[i] -= extra_cdd_po;
+	}
+	for (i = 0; i < 4; i++) {
+		off = ((sprom_mcs_po[1] >> (i * 4)) & 0xf) * 2;
+		rates->mcs_20_cdd[4 + i] = maxpwr - off;
+		if (phy->type == B43_PHYTYPE_N && phy->rev >= 3)
+			rates->mcs_20_cdd[4 + i] -= extra_cdd_po;
+	}
+
+	/* OFDM 20 CDD */
+	rates->ofdm_20_cdd[0] = rates->mcs_20_cdd[0];
+	rates->ofdm_20_cdd[1] = rates->mcs_20_cdd[0];
+	rates->ofdm_20_cdd[2] = rates->mcs_20_cdd[1];
+	rates->ofdm_20_cdd[3] = rates->mcs_20_cdd[2];
+	rates->ofdm_20_cdd[4] = rates->mcs_20_cdd[3];
+	rates->ofdm_20_cdd[5] = rates->mcs_20_cdd[4];
+	rates->ofdm_20_cdd[6] = rates->mcs_20_cdd[5];
+	rates->ofdm_20_cdd[7] = rates->mcs_20_cdd[6];
+
+	/* MCS 20 STBC */
+	for (i = 0; i < 4; i++) {
+		off = ((sprom_mcs_po[0] >> (i * 4)) & 0xf) * 2;
+		rates->mcs_20_stbc[i] = maxpwr - off;
+		if (phy->type == B43_PHYTYPE_N && phy->rev >= 3)
+			rates->mcs_20_stbc[i] -= extra_stbc_po;
+	}
+	for (i = 0; i < 4; i++) {
+		off = ((sprom_mcs_po[1] >> (i * 4)) & 0xf) * 2;
+		rates->mcs_20_stbc[4 + i] = maxpwr - off;
+		if (phy->type == B43_PHYTYPE_N && phy->rev >= 3)
+			rates->mcs_20_stbc[4 + i] -= extra_stbc_po;
+	}
+
+	/* MCS 20 SDM */
+	for (i = 0; i < 4; i++) {
+		off = ((sprom_mcs_po[2] >> (i * 4)) & 0xf) * 2;
+		rates->mcs_20_sdm[i] = maxpwr - off;
+	}
+	for (i = 0; i < 4; i++) {
+		off = ((sprom_mcs_po[3] >> (i * 4)) & 0xf) * 2;
+		rates->mcs_20_sdm[4 + i] = maxpwr - off;
+	}
+
+	return true;
+}
diff --git a/drivers/net/wireless/b43/ppr.h b/drivers/net/wireless/b43/ppr.h
new file mode 100644
index 000000000000..24d7447e9f01
--- /dev/null
+++ b/drivers/net/wireless/b43/ppr.h
@@ -0,0 +1,45 @@
+#ifndef LINUX_B43_PPR_H_
+#define LINUX_B43_PPR_H_
+
+#include <linux/types.h>
+
+#define B43_PPR_CCK_RATES_NUM		4
+#define B43_PPR_OFDM_RATES_NUM		8
+#define B43_PPR_MCS_RATES_NUM		8
+
+#define B43_PPR_RATES_NUM	(B43_PPR_CCK_RATES_NUM +	\
+				 B43_PPR_OFDM_RATES_NUM * 2 +	\
+				 B43_PPR_MCS_RATES_NUM * 4)
+
+struct b43_ppr_rates {
+	u8 cck[B43_PPR_CCK_RATES_NUM];
+	u8 ofdm[B43_PPR_OFDM_RATES_NUM];
+	u8 ofdm_20_cdd[B43_PPR_OFDM_RATES_NUM];
+	u8 mcs_20[B43_PPR_MCS_RATES_NUM]; /* SISO */
+	u8 mcs_20_cdd[B43_PPR_MCS_RATES_NUM];
+	u8 mcs_20_stbc[B43_PPR_MCS_RATES_NUM];
+	u8 mcs_20_sdm[B43_PPR_MCS_RATES_NUM];
+};
+
+struct b43_ppr {
+	/* All powers are in qdbm (Q5.2) */
+	union {
+		u8 __all_rates[B43_PPR_RATES_NUM];
+		struct b43_ppr_rates rates;
+	};
+};
+
+struct b43_wldev;
+enum b43_band;
+
+void b43_ppr_clear(struct b43_wldev *dev, struct b43_ppr *ppr);
+
+void b43_ppr_add(struct b43_wldev *dev, struct b43_ppr *ppr, int diff);
+void b43_ppr_apply_max(struct b43_wldev *dev, struct b43_ppr *ppr, u8 max);
+void b43_ppr_apply_min(struct b43_wldev *dev, struct b43_ppr *ppr, u8 min);
+u8 b43_ppr_get_max(struct b43_wldev *dev, struct b43_ppr *ppr);
+
+bool b43_ppr_load_max_from_sprom(struct b43_wldev *dev, struct b43_ppr *ppr,
+				 enum b43_band band);
+
+#endif /* LINUX_B43_PPR_H_ */
diff --git a/drivers/net/wireless/b43/radio_2059.c b/drivers/net/wireless/b43/radio_2059.c
index 38e31d857e3e..a3cf9efd7e21 100644
--- a/drivers/net/wireless/b43/radio_2059.c
+++ b/drivers/net/wireless/b43/radio_2059.c
@@ -25,6 +25,13 @@
 #include "b43.h"
 #include "radio_2059.h"
 
+/* Extracted from MMIO dump of 6.30.223.141 */
+static u16 r2059_phy_rev1_init[][2] = {
+	{ 0x051, 0x70 }, { 0x05a, 0x03 }, { 0x079, 0x01 }, { 0x082, 0x70 },
+	{ 0x083, 0x00 }, { 0x084, 0x70 }, { 0x09a, 0x7f }, { 0x0b6, 0x10 },
+	{ 0x188, 0x05 },
+};
+
 #define RADIOREGS(r00, r01, r02, r03, r04, r05, r06, r07, r08, r09, \
 		  r10, r11, r12, r13, r14, r15, r16, r17, r18, r19, \
 		  r20) \
@@ -58,73 +65,87 @@
 	.phy_regs.bw5	= r4,	\
 	.phy_regs.bw6	= r5
 
+/* Extracted from MMIO dump of 6.30.223.141
+ * TODO: Values for channels 12 & 13 are outdated (from some old 5.x driver)!
+ */
 static const struct b43_phy_ht_channeltab_e_radio2059 b43_phy_ht_channeltab_radio2059[] = {
-  {	.freq			= 2412,
-	RADIOREGS(0x48, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x6c,
-		  0x09, 0x0f, 0x0a, 0x00, 0x0a, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03c9, 0x03c5, 0x03c1, 0x043a, 0x043f, 0x0443),
-  },
-  {	.freq			= 2417,
-	RADIOREGS(0x4b, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x71,
-		  0x09, 0x0f, 0x0a, 0x00, 0x0a, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03cb, 0x03c7, 0x03c3, 0x0438, 0x043d, 0x0441),
-  },
-  {	.freq			= 2422,
-	RADIOREGS(0x4e, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x76,
-		  0x09, 0x0f, 0x09, 0x00, 0x09, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03cd, 0x03c9, 0x03c5, 0x0436, 0x043a, 0x043f),
-  },
-  {	.freq			= 2427,
-	RADIOREGS(0x52, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x7b,
-		  0x09, 0x0f, 0x09, 0x00, 0x09, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03cf, 0x03cb, 0x03c7, 0x0434, 0x0438, 0x043d),
-  },
-  {	.freq			= 2432,
-	RADIOREGS(0x55, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x80,
-		  0x09, 0x0f, 0x08, 0x00, 0x08, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03d1, 0x03cd, 0x03c9, 0x0431, 0x0436, 0x043a),
-  },
-  {	.freq			= 2437,
-	RADIOREGS(0x58, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x85,
-		  0x09, 0x0f, 0x08, 0x00, 0x08, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03d3, 0x03cf, 0x03cb, 0x042f, 0x0434, 0x0438),
-  },
-  {	.freq			= 2442,
-	RADIOREGS(0x5c, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x8a,
-		  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03d5, 0x03d1, 0x03cd, 0x042d, 0x0431, 0x0436),
-  },
-  {	.freq			= 2447,
-	RADIOREGS(0x5f, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x8f,
-		  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03d7, 0x03d3, 0x03cf, 0x042b, 0x042f, 0x0434),
-  },
-  {	.freq			= 2452,
-	RADIOREGS(0x62, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x94,
-		  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03d9, 0x03d5, 0x03d1, 0x0429, 0x042d, 0x0431),
-  },
-  {	.freq			= 2457,
-	RADIOREGS(0x66, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x99,
-		  0x09, 0x0f, 0x06, 0x00, 0x06, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03db, 0x03d7, 0x03d3, 0x0427, 0x042b, 0x042f),
-  },
-  {	.freq			= 2462,
-	RADIOREGS(0x69, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x9e,
-		  0x09, 0x0f, 0x06, 0x00, 0x06, 0x00, 0x61, 0x03,
-		  0x00, 0x00, 0x00, 0xf0, 0x00),
-	PHYREGS(0x03dd, 0x03d9, 0x03d5, 0x0424, 0x0429, 0x042d),
-  },
+	{
+		.freq			= 2412,
+		RADIOREGS(0x48, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x6c,
+			  0x09, 0x0f, 0x0a, 0x00, 0x0a, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0xd0, 0x00),
+		PHYREGS(0x03c9, 0x03c5, 0x03c1, 0x043a, 0x043f, 0x0443),
+	},
+	{
+		.freq			= 2417,
+		RADIOREGS(0x4b, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x71,
+			  0x09, 0x0f, 0x0a, 0x00, 0x0a, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0xd0, 0x00),
+		PHYREGS(0x03cb, 0x03c7, 0x03c3, 0x0438, 0x043d, 0x0441),
+	},
+	{
+		.freq			= 2422,
+		RADIOREGS(0x4e, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x76,
+			  0x09, 0x0f, 0x09, 0x00, 0x09, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0xd0, 0x00),
+		PHYREGS(0x03cd, 0x03c9, 0x03c5, 0x0436, 0x043a, 0x043f),
+	},
+	{
+		.freq			= 2427,
+		RADIOREGS(0x52, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x7b,
+			  0x09, 0x0f, 0x09, 0x00, 0x09, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0xa0, 0x00),
+		PHYREGS(0x03cf, 0x03cb, 0x03c7, 0x0434, 0x0438, 0x043d),
+	},
+	{
+		.freq			= 2432,
+		RADIOREGS(0x55, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x80,
+			  0x09, 0x0f, 0x08, 0x00, 0x08, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0xa0, 0x00),
+		PHYREGS(0x03d1, 0x03cd, 0x03c9, 0x0431, 0x0436, 0x043a),
+	},
+	{
+		.freq			= 2437,
+		RADIOREGS(0x58, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x85,
+			  0x09, 0x0f, 0x08, 0x00, 0x08, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0xa0, 0x00),
+		PHYREGS(0x03d3, 0x03cf, 0x03cb, 0x042f, 0x0434, 0x0438),
+	},
+	{
+		.freq			= 2442,
+		RADIOREGS(0x5c, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x8a,
+			  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0x80, 0x00),
+		PHYREGS(0x03d5, 0x03d1, 0x03cd, 0x042d, 0x0431, 0x0436),
+	},
+	{
+		.freq			= 2447,
+		RADIOREGS(0x5f, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x8f,
+			  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0x80, 0x00),
+		PHYREGS(0x03d7, 0x03d3, 0x03cf, 0x042b, 0x042f, 0x0434),
+	},
+	{
+		.freq			= 2452,
+		RADIOREGS(0x62, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x94,
+			  0x09, 0x0f, 0x07, 0x00, 0x07, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0x80, 0x00),
+		PHYREGS(0x03d9, 0x03d5, 0x03d1, 0x0429, 0x042d, 0x0431),
+	},
+	{
+		.freq			= 2457,
+		RADIOREGS(0x66, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x99,
+			  0x09, 0x0f, 0x06, 0x00, 0x06, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0x60, 0x00),
+		PHYREGS(0x03db, 0x03d7, 0x03d3, 0x0427, 0x042b, 0x042f),
+	},
+	{
+		.freq			= 2462,
+		RADIOREGS(0x69, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0x9e,
+			  0x09, 0x0f, 0x06, 0x00, 0x06, 0x00, 0x61, 0x73,
+			  0x00, 0x00, 0x00, 0x60, 0x00),
+		PHYREGS(0x03dd, 0x03d9, 0x03d5, 0x0424, 0x0429, 0x042d),
+	},
   {	.freq			= 2467,
 	RADIOREGS(0x6c, 0x16, 0x30, 0x1b, 0x0a, 0x0a, 0x30, 0xa3,
 		  0x09, 0x0f, 0x05, 0x00, 0x05, 0x00, 0x61, 0x03,
@@ -137,8 +158,196 @@ static const struct b43_phy_ht_channeltab_e_radio2059 b43_phy_ht_channeltab_radi
 		  0x00, 0x00, 0x00, 0xf0, 0x00),
 	PHYREGS(0x03e1, 0x03dd, 0x03d9, 0x0420, 0x0424, 0x0429),
   },
+	{
+		.freq			= 5180,
+		RADIOREGS(0xbe, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x06,
+			  0x02, 0x0c, 0x00, 0x0c, 0x00, 0x0c, 0x00, 0x00,
+			  0x0f, 0x4f, 0xa3, 0x00, 0xfc),
+		PHYREGS(0x081c, 0x0818, 0x0814, 0x01f9, 0x01fa, 0x01fb),
+	},
+	{
+		.freq			= 5200,
+		RADIOREGS(0xc5, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x08,
+			  0x02, 0x0c, 0x00, 0x0c, 0x00, 0x0c, 0x00, 0x00,
+			  0x0f, 0x4f, 0x93, 0x00, 0xfb),
+		PHYREGS(0x0824, 0x0820, 0x081c, 0x01f7, 0x01f8, 0x01f9),
+	},
+	{
+		.freq			= 5220,
+		RADIOREGS(0xcc, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x0a,
+			  0x02, 0x0c, 0x00, 0x0c, 0x00, 0x0c, 0x00, 0x00,
+			  0x0f, 0x4f, 0x93, 0x00, 0xea),
+		PHYREGS(0x082c, 0x0828, 0x0824, 0x01f5, 0x01f6, 0x01f7),
+	},
+	{
+		.freq			= 5240,
+		RADIOREGS(0xd2, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x0c,
+			  0x02, 0x0c, 0x00, 0x0c, 0x00, 0x0c, 0x00, 0x00,
+			  0x0f, 0x4f, 0x93, 0x00, 0xda),
+		PHYREGS(0x0834, 0x0830, 0x082c, 0x01f3, 0x01f4, 0x01f5),
+	},
+	{
+		.freq			= 5260,
+		RADIOREGS(0xd9, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x0e,
+			  0x02, 0x0b, 0x00, 0x0b, 0x00, 0x0b, 0x00, 0x00,
+			  0x0f, 0x4f, 0x93, 0x00, 0xca),
+		PHYREGS(0x083c, 0x0838, 0x0834, 0x01f1, 0x01f2, 0x01f3),
+	},
+	{
+		.freq			= 5280,
+		RADIOREGS(0xe0, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x10,
+			  0x02, 0x0b, 0x00, 0x0b, 0x00, 0x0b, 0x00, 0x00,
+			  0x0f, 0x4f, 0x93, 0x00, 0xb9),
+		PHYREGS(0x0844, 0x0840, 0x083c, 0x01f0, 0x01f0, 0x01f1),
+	},
+	{
+		.freq			= 5300,
+		RADIOREGS(0xe6, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x12,
+			  0x02, 0x0b, 0x00, 0x0b, 0x00, 0x0b, 0x00, 0x00,
+			  0x0f, 0x4c, 0x83, 0x00, 0xb8),
+		PHYREGS(0x084c, 0x0848, 0x0844, 0x01ee, 0x01ef, 0x01f0),
+	},
+	{
+		.freq			= 5320,
+		RADIOREGS(0xed, 0x16, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x14,
+			  0x02, 0x0b, 0x00, 0x0b, 0x00, 0x0b, 0x00, 0x00,
+			  0x0f, 0x4c, 0x83, 0x00, 0xa8),
+		PHYREGS(0x0854, 0x0850, 0x084c, 0x01ec, 0x01ed, 0x01ee),
+	},
+	{
+		.freq			= 5500,
+		RADIOREGS(0x29, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x26,
+			  0x02, 0x09, 0x00, 0x09, 0x00, 0x09, 0x00, 0x00,
+			  0x0a, 0x46, 0x43, 0x00, 0x75),
+		PHYREGS(0x089c, 0x0898, 0x0894, 0x01dc, 0x01dd, 0x01dd),
+	},
+	{
+		.freq			= 5520,
+		RADIOREGS(0x30, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x28,
+			  0x02, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x00,
+			  0x0a, 0x46, 0x43, 0x00, 0x75),
+		PHYREGS(0x08a4, 0x08a0, 0x089c, 0x01da, 0x01db, 0x01dc),
+	},
+	{
+		.freq			= 5540,
+		RADIOREGS(0x36, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x2a,
+			  0x02, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x00,
+			  0x0a, 0x46, 0x43, 0x00, 0x75),
+		PHYREGS(0x08ac, 0x08a8, 0x08a4, 0x01d8, 0x01d9, 0x01da),
+	},
+	{
+		.freq			= 5560,
+		RADIOREGS(0x3d, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x2c,
+			  0x02, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x00,
+			  0x0a, 0x46, 0x43, 0x00, 0x75),
+		PHYREGS(0x08b4, 0x08b0, 0x08ac, 0x01d7, 0x01d7, 0x01d8),
+	},
+	{
+		.freq			= 5580,
+		RADIOREGS(0x44, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x2e,
+			  0x02, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x00,
+			  0x0a, 0x46, 0x43, 0x00, 0x74),
+		PHYREGS(0x08bc, 0x08b8, 0x08b4, 0x01d5, 0x01d6, 0x01d7),
+	},
+	{
+		.freq			= 5600,
+		RADIOREGS(0x4a, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x30,
+			  0x02, 0x08, 0x00, 0x08, 0x00, 0x08, 0x00, 0x00,
+			  0x09, 0x44, 0x23, 0x00, 0x54),
+		PHYREGS(0x08c4, 0x08c0, 0x08bc, 0x01d3, 0x01d4, 0x01d5),
+	},
+	{
+		.freq			= 5620,
+		RADIOREGS(0x51, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x32,
+			  0x02, 0x07, 0x00, 0x07, 0x00, 0x07, 0x00, 0x00,
+			  0x09, 0x44, 0x23, 0x00, 0x54),
+		PHYREGS(0x08cc, 0x08c8, 0x08c4, 0x01d2, 0x01d2, 0x01d3),
+	},
+	{
+		.freq			= 5640,
+		RADIOREGS(0x58, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x34,
+			  0x02, 0x07, 0x00, 0x07, 0x00, 0x07, 0x00, 0x00,
+			  0x09, 0x44, 0x23, 0x00, 0x43),
+		PHYREGS(0x08d4, 0x08d0, 0x08cc, 0x01d0, 0x01d1, 0x01d2),
+	},
+	{
+		.freq			= 5660,
+		RADIOREGS(0x5e, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x36,
+			  0x02, 0x07, 0x00, 0x07, 0x00, 0x07, 0x00, 0x00,
+			  0x09, 0x43, 0x23, 0x00, 0x43),
+		PHYREGS(0x08dc, 0x08d8, 0x08d4, 0x01ce, 0x01cf, 0x01d0),
+	},
+	{
+		.freq			= 5680,
+		RADIOREGS(0x65, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x38,
+			  0x02, 0x07, 0x00, 0x07, 0x00, 0x07, 0x00, 0x00,
+			  0x09, 0x42, 0x23, 0x00, 0x43),
+		PHYREGS(0x08e4, 0x08e0, 0x08dc, 0x01cd, 0x01ce, 0x01ce),
+	},
+	{
+		.freq			= 5700,
+		RADIOREGS(0x6c, 0x17, 0x10, 0x1f, 0x08, 0x08, 0x3f, 0x3a,
+			  0x02, 0x07, 0x00, 0x07, 0x00, 0x07, 0x00, 0x00,
+			  0x08, 0x42, 0x13, 0x00, 0x32),
+		PHYREGS(0x08ec, 0x08e8, 0x08e4, 0x01cb, 0x01cc, 0x01cd),
+	},
+	{
+		.freq			= 5745,
+		RADIOREGS(0x7b, 0x17, 0x20, 0x1f, 0x08, 0x08, 0x3f, 0x7d,
+			  0x04, 0x06, 0x00, 0x06, 0x00, 0x06, 0x00, 0x00,
+			  0x08, 0x42, 0x13, 0x00, 0x21),
+		PHYREGS(0x08fe, 0x08fa, 0x08f6, 0x01c8, 0x01c8, 0x01c9),
+	},
+	{
+		.freq			= 5765,
+		RADIOREGS(0x81, 0x17, 0x20, 0x1f, 0x08, 0x08, 0x3f, 0x81,
+			  0x04, 0x06, 0x00, 0x06, 0x00, 0x06, 0x00, 0x00,
+			  0x08, 0x42, 0x13, 0x00, 0x11),
+		PHYREGS(0x0906, 0x0902, 0x08fe, 0x01c6, 0x01c7, 0x01c8),
+	},
+	{
+		.freq			= 5785,
+		RADIOREGS(0x88, 0x17, 0x20, 0x1f, 0x08, 0x08, 0x3f, 0x85,
+			  0x04, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00,
+			  0x08, 0x42, 0x13, 0x00, 0x00),
+		PHYREGS(0x090e, 0x090a, 0x0906, 0x01c4, 0x01c5, 0x01c6),
+	},
+	{
+		.freq			= 5805,
+		RADIOREGS(0x8f, 0x17, 0x20, 0x1f, 0x08, 0x08, 0x3f, 0x89,
+			  0x04, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00,
+			  0x06, 0x41, 0x03, 0x00, 0x00),
+		PHYREGS(0x0916, 0x0912, 0x090e, 0x01c3, 0x01c4, 0x01c4),
+	},
+	{
+		.freq			= 5825,
+		RADIOREGS(0x95, 0x17, 0x20, 0x1f, 0x08, 0x08, 0x3f, 0x8d,
+			  0x04, 0x05, 0x00, 0x05, 0x00, 0x05, 0x00, 0x00,
+			  0x06, 0x41, 0x03, 0x00, 0x00),
+		PHYREGS(0x091e, 0x091a, 0x0916, 0x01c1, 0x01c2, 0x01c3),
+	},
 };
 
+void r2059_upload_inittabs(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 *table = NULL;
+	u16 size, i;
+
+	switch (phy->rev) {
+	case 1:
+		table = r2059_phy_rev1_init[0];
+		size = ARRAY_SIZE(r2059_phy_rev1_init);
+		break;
+	default:
+		B43_WARN_ON(1);
+		return;
+	}
+
+	for (i = 0; i < size; i++, table += 2)
+		b43_radio_write(dev, R2059_ALL | table[0], table[1]);
+}
+
 const struct b43_phy_ht_channeltab_e_radio2059
 *b43_phy_ht_get_channeltab_e_r2059(struct b43_wldev *dev, u16 freq)
 {
diff --git a/drivers/net/wireless/b43/radio_2059.h b/drivers/net/wireless/b43/radio_2059.h
index 40a82d7f510c..9e22fb60588b 100644
--- a/drivers/net/wireless/b43/radio_2059.h
+++ b/drivers/net/wireless/b43/radio_2059.h
@@ -10,6 +10,18 @@
 #define R2059_C3			0x800
 #define R2059_ALL			0xC00
 
+#define R2059_RCAL_CONFIG			0x004
+#define R2059_RFPLL_MASTER			0x011
+#define R2059_RFPLL_MISC_EN			0x02b
+#define R2059_RFPLL_MISC_CAL_RESETN		0x02e
+#define R2059_XTAL_CONFIG2			0x0c0
+#define R2059_RCCAL_START_R1_Q1_P1		0x13c
+#define R2059_RCCAL_X1				0x13d
+#define R2059_RCCAL_TRC0			0x13e
+#define R2059_RCCAL_DONE_OSCCAP			0x140
+#define R2059_RCAL_STATUS			0x145
+#define R2059_RCCAL_MASTER			0x17f
+
 /* Values for various registers uploaded on channel switching */
 struct b43_phy_ht_channeltab_e_radio2059 {
 	/* The channel frequency in MHz */
@@ -40,6 +52,8 @@ struct b43_phy_ht_channeltab_e_radio2059 {
 	struct b43_phy_ht_channeltab_e_phy phy_regs;
 };
 
+void r2059_upload_inittabs(struct b43_wldev *dev);
+
 const struct b43_phy_ht_channeltab_e_radio2059
 *b43_phy_ht_get_channeltab_e_r2059(struct b43_wldev *dev, u16 freq);
 
diff --git a/drivers/net/wireless/b43/tables_nphy.c b/drivers/net/wireless/b43/tables_nphy.c
index 4b5885077b01..25d1cbd34306 100644
--- a/drivers/net/wireless/b43/tables_nphy.c
+++ b/drivers/net/wireless/b43/tables_nphy.c
@@ -2878,6 +2878,40 @@ const s8 b43_ntab_papd_pga_gain_delta_ipa_2g[] = {
 	-54, -46, -39, -31, -23, -15, -8, 0
 };
 
+/* Extracted from MMIO dump of 6.30.223.248
+ * Entries: 0, 15, 17, 21, 24, 26, 27, 29, 30 were guessed
+ */
+static const s16 b43_ntab_rf_pwr_offset_2057_rev9_2g[] = {
+	-133, -133, -107, -92, -81,
+	-73, -66, -61, -56, -52,
+	-48, -44, -41, -37, -34,
+	-31, -28, -25, -22, -19,
+	-17, -14, -12, -10, -9,
+	-7, -5, -4, -3, -2,
+	-1, 0,
+};
+
+/* Extracted from MMIO dump of 6.30.223.248 */
+static const s16 b43_ntab_rf_pwr_offset_2057_rev9_5g[] = {
+	-101, -94, -86, -79, -72,
+	-65, -57, -50, -42, -35,
+	-28, -21, -16, -9, -4,
+	0,
+};
+
+/* Extracted from MMIO dump of 6.30.223.248
+ * Entries: 0, 26, 28, 29, 30, 31 were guessed
+ */
+static const s16 b43_ntab_rf_pwr_offset_2057_rev14_2g[] = {
+	-111, -111, -111, -84, -70,
+	-59, -52, -45, -40, -36,
+	-32, -29, -26, -23, -21,
+	-18, -16, -15, -13, -11,
+	-10, -8, -7, -6, -5,
+	-4, -4, -3, -3, -2,
+	-2, -1,
+};
+
 const u16 tbl_iqcal_gainparams[2][9][8] = {
 	{
 		{ 0x000, 0, 0, 2, 0x69, 0x69, 0x69, 0x69 },
@@ -3197,7 +3231,7 @@ static struct nphy_gain_ctl_workaround_entry nphy_gain_ctl_workaround[2][4] = {
 			{ 3, 3, 3, 3, 3, 3, 3, 3, 3, 3 },
 			0x527E, /* invalid for external LNA! */
 			{ 0x513F, 0x513F, 0x513F, 0x513F }, /* invalid for external LNA! */
-			0x1076, 0x0066, 0x0000, /* low is invalid (the last one) */
+			0x007E, 0x0066, 0x0000, /* low is invalid (the last one) */
 			0x18, 0x18, 0x18,
 			0x01D0, 0x5,
 		},
@@ -3708,9 +3742,43 @@ const u32 *b43_nphy_get_tx_gain_table(struct b43_wldev *dev)
 	}
 }
 
+const s16 *b43_ntab_get_rf_pwr_offset_table(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
+		switch (phy->rev) {
+		case 17:
+			if (phy->radio_rev == 14)
+				return b43_ntab_rf_pwr_offset_2057_rev14_2g;
+			break;
+		case 16:
+			if (phy->radio_rev == 9)
+				return b43_ntab_rf_pwr_offset_2057_rev9_2g;
+			break;
+		}
+
+		b43err(dev->wl,
+		       "No 2GHz RF power table available for this device\n");
+		return NULL;
+	} else {
+		switch (phy->rev) {
+		case 16:
+			if (phy->radio_rev == 9)
+				return b43_ntab_rf_pwr_offset_2057_rev9_5g;
+			break;
+		}
+
+		b43err(dev->wl,
+		       "No 5GHz RF power table available for this device\n");
+		return NULL;
+	}
+}
+
 struct nphy_gain_ctl_workaround_entry *b43_nphy_get_gain_ctl_workaround_ent(
 	struct b43_wldev *dev, bool ghz5, bool ext_lna)
 {
+	struct b43_phy *phy = &dev->phy;
 	struct nphy_gain_ctl_workaround_entry *e;
 	u8 phy_idx;
 
@@ -3729,37 +3797,49 @@ struct nphy_gain_ctl_workaround_entry *b43_nphy_get_gain_ctl_workaround_ent(
 	e = &nphy_gain_ctl_workaround[ghz5][phy_idx];
 
 	/* Some workarounds to the workarounds... */
-	if (ghz5 && dev->phy.rev >= 6) {
-		if (dev->phy.radio_rev == 11 &&
-		    !b43_is_40mhz(dev))
-			e->cliplo_gain = 0x2d;
-	} else if (!ghz5 && dev->phy.rev >= 5) {
-		static const int gain_data[] = {0x0062, 0x0064, 0x006a, 0x106a,
-						0x106c, 0x1074, 0x107c, 0x207c};
+	if (!ghz5) {
 		u8 tr_iso = dev->dev->bus_sprom->fem.ghz2.tr_iso;
 
-		if (ext_lna) {
+		if (tr_iso > 7)
+			tr_iso = 3;
+
+		if (phy->rev >= 6) {
+			static const int gain_data[] = { 0x106a, 0x106c, 0x1074,
+							 0x107c, 0x007e, 0x107e,
+							 0x207e, 0x307e, };
+
+			e->cliplo_gain = gain_data[tr_iso];
+		} else if (phy->rev == 5) {
+			static const int gain_data[] = { 0x0062, 0x0064, 0x006a,
+							 0x106a, 0x106c, 0x1074,
+							 0x107c, 0x207c, };
+
+			e->cliplo_gain = gain_data[tr_iso];
+		}
+
+		if (phy->rev >= 5 && ext_lna) {
 			e->rfseq_init[0] &= ~0x4000;
 			e->rfseq_init[1] &= ~0x4000;
 			e->rfseq_init[2] &= ~0x4000;
 			e->rfseq_init[3] &= ~0x4000;
 			e->init_gain &= ~0x4000;
 		}
-		if (tr_iso > 7)
-			tr_iso = 3;
-		e->cliplo_gain = gain_data[tr_iso];
-
-	} else if (ghz5 && dev->phy.rev == 4 && ext_lna) {
-		e->rfseq_init[0] &= ~0x4000;
-		e->rfseq_init[1] &= ~0x4000;
-		e->rfseq_init[2] &= ~0x4000;
-		e->rfseq_init[3] &= ~0x4000;
-		e->init_gain &= ~0x4000;
-		e->rfseq_init[0] |= 0x1000;
-		e->rfseq_init[1] |= 0x1000;
-		e->rfseq_init[2] |= 0x1000;
-		e->rfseq_init[3] |= 0x1000;
-		e->init_gain |= 0x1000;
+	} else {
+		if (phy->rev >= 6) {
+			if (phy->radio_rev == 11 && !b43_is_40mhz(dev))
+				e->crsminu = 0x2d;
+		} else if (phy->rev == 4 && ext_lna) {
+			e->rfseq_init[0] &= ~0x4000;
+			e->rfseq_init[1] &= ~0x4000;
+			e->rfseq_init[2] &= ~0x4000;
+			e->rfseq_init[3] &= ~0x4000;
+			e->init_gain &= ~0x4000;
+			e->rfseq_init[0] |= 0x1000;
+			e->rfseq_init[1] |= 0x1000;
+			e->rfseq_init[2] |= 0x1000;
+			e->rfseq_init[3] |= 0x1000;
+			e->init_gain |= 0x1000;
+		}
 	}
 
 	return e;
diff --git a/drivers/net/wireless/b43/tables_nphy.h b/drivers/net/wireless/b43/tables_nphy.h
index 3ce2e6f3a278..b51f386db02f 100644
--- a/drivers/net/wireless/b43/tables_nphy.h
+++ b/drivers/net/wireless/b43/tables_nphy.h
@@ -191,6 +191,8 @@ void b43_nphy_tables_init(struct b43_wldev *dev);
 
 const u32 *b43_nphy_get_tx_gain_table(struct b43_wldev *dev);
 
+const s16 *b43_ntab_get_rf_pwr_offset_table(struct b43_wldev *dev);
+
 extern const s8 b43_ntab_papd_pga_gain_delta_ipa_2g[];
 
 extern const u16 tbl_iqcal_gainparams[2][9][8];
diff --git a/drivers/net/wireless/b43/xmit.h b/drivers/net/wireless/b43/xmit.h
index 98d90747836a..ba6115308068 100644
--- a/drivers/net/wireless/b43/xmit.h
+++ b/drivers/net/wireless/b43/xmit.h
@@ -97,9 +97,13 @@ struct b43_tx_legacy_rate_phy_ctl_entry {
 };
 
 /* MAC TX control */
+#define B43_TXH_MAC_RTS_FB_SHORTPRMBL	0x80000000 /* RTS fallback preamble */
+#define B43_TXH_MAC_RTS_SHORTPRMBL	0x40000000 /* RTS main rate preamble */
+#define B43_TXH_MAC_FB_SHORTPRMBL	0x20000000 /* Main fallback preamble */
 #define B43_TXH_MAC_USEFBR		0x10000000 /* Use fallback rate for this AMPDU */
 #define B43_TXH_MAC_KEYIDX		0x0FF00000 /* Security key index */
 #define B43_TXH_MAC_KEYIDX_SHIFT	20
+#define B43_TXH_MAC_ALT_TXPWR		0x00080000 /* Use alternate txpwr defined at loc. M_ALT_TXPWR_IDX */
 #define B43_TXH_MAC_KEYALG		0x00070000 /* Security key algorithm */
 #define B43_TXH_MAC_KEYALG_SHIFT	16
 #define B43_TXH_MAC_AMIC		0x00008000 /* AMIC */
@@ -126,25 +130,25 @@ struct b43_tx_legacy_rate_phy_ctl_entry {
 #define B43_TXH_EFT_FB			0x03 /* Data frame fallback encoding */
 #define  B43_TXH_EFT_FB_CCK		0x00 /* CCK */
 #define  B43_TXH_EFT_FB_OFDM		0x01 /* OFDM */
-#define  B43_TXH_EFT_FB_EWC		0x02 /* EWC */
-#define  B43_TXH_EFT_FB_N		0x03 /* N */
+#define  B43_TXH_EFT_FB_HT		0x02 /* HT */
+#define  B43_TXH_EFT_FB_VHT		0x03 /* VHT */
 #define B43_TXH_EFT_RTS			0x0C /* RTS/CTS encoding */
 #define  B43_TXH_EFT_RTS_CCK		0x00 /* CCK */
 #define  B43_TXH_EFT_RTS_OFDM		0x04 /* OFDM */
-#define  B43_TXH_EFT_RTS_EWC		0x08 /* EWC */
-#define  B43_TXH_EFT_RTS_N		0x0C /* N */
+#define  B43_TXH_EFT_RTS_HT		0x08 /* HT */
+#define  B43_TXH_EFT_RTS_VHT		0x0C /* VHT */
 #define B43_TXH_EFT_RTSFB		0x30 /* RTS/CTS fallback encoding */
 #define  B43_TXH_EFT_RTSFB_CCK		0x00 /* CCK */
 #define  B43_TXH_EFT_RTSFB_OFDM		0x10 /* OFDM */
-#define  B43_TXH_EFT_RTSFB_EWC		0x20 /* EWC */
-#define  B43_TXH_EFT_RTSFB_N		0x30 /* N */
+#define  B43_TXH_EFT_RTSFB_HT		0x20 /* HT */
+#define  B43_TXH_EFT_RTSFB_VHT		0x30 /* VHT */
 
 /* PHY TX control word */
 #define B43_TXH_PHY_ENC			0x0003 /* Data frame encoding */
 #define  B43_TXH_PHY_ENC_CCK		0x0000 /* CCK */
 #define  B43_TXH_PHY_ENC_OFDM		0x0001 /* OFDM */
-#define  B43_TXH_PHY_ENC_EWC		0x0002 /* EWC */
-#define  B43_TXH_PHY_ENC_N		0x0003 /* N */
+#define  B43_TXH_PHY_ENC_HT		0x0002 /* HT */
+#define  B43_TXH_PHY_ENC_VHT		0x0003 /* VHT */
 #define B43_TXH_PHY_SHORTPRMBL		0x0010 /* Use short preamble */
 #define B43_TXH_PHY_ANT			0x03C0 /* Antenna selection */
 #define  B43_TXH_PHY_ANT0		0x0000 /* Use antenna 0 */
@@ -162,7 +166,7 @@ struct b43_tx_legacy_rate_phy_ctl_entry {
 #define  B43_TXH_PHY1_BW_20		0x0002 /* 20 MHz */
 #define  B43_TXH_PHY1_BW_20U		0x0003 /* 20 MHz upper */
 #define  B43_TXH_PHY1_BW_40		0x0004 /* 40 MHz */
-#define  B43_TXH_PHY1_BW_40DUP		0x0005 /* 50 MHz duplicate */
+#define  B43_TXH_PHY1_BW_40DUP		0x0005 /* 40 MHz duplicate */
 #define B43_TXH_PHY1_MODE		0x0038 /* Mode */
 #define  B43_TXH_PHY1_MODE_SISO		0x0000 /* SISO */
 #define  B43_TXH_PHY1_MODE_CDD		0x0008 /* CDD */
diff --git a/drivers/net/wireless/brcm80211/Kconfig b/drivers/net/wireless/brcm80211/Kconfig
index b8e2561ea645..fe3dc126b149 100644
--- a/drivers/net/wireless/brcm80211/Kconfig
+++ b/drivers/net/wireless/brcm80211/Kconfig
@@ -27,10 +27,17 @@ config BRCMFMAC
 	  one of the bus interface support. If you choose to build a module,
 	  it'll be called brcmfmac.ko.
 
+config BRCMFMAC_PROTO_BCDC
+	bool
+
+config BRCMFMAC_PROTO_MSGBUF
+	bool
+
 config BRCMFMAC_SDIO
 	bool "SDIO bus interface support for FullMAC driver"
 	depends on (MMC = y || MMC = BRCMFMAC)
 	depends on BRCMFMAC
+	select BRCMFMAC_PROTO_BCDC
 	select FW_LOADER
 	default y
 	---help---
@@ -42,6 +49,7 @@ config BRCMFMAC_USB
 	bool "USB bus interface support for FullMAC driver"
 	depends on (USB = y || USB = BRCMFMAC)
 	depends on BRCMFMAC
+	select BRCMFMAC_PROTO_BCDC
 	select FW_LOADER
 	---help---
 	  This option enables the USB bus interface support for Broadcom
@@ -52,6 +60,8 @@ config BRCMFMAC_PCIE
 	bool "PCIE bus interface support for FullMAC driver"
 	depends on BRCMFMAC
 	depends on PCI
+	depends on HAS_DMA
+	select BRCMFMAC_PROTO_MSGBUF
 	select FW_LOADER
 	---help---
 	  This option enables the PCIE bus interface support for Broadcom
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/Makefile b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
index c35adf4bc70b..90a977fe9a64 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/Makefile
+++ b/drivers/net/wireless/brcm80211/brcmfmac/Makefile
@@ -30,16 +30,18 @@ brcmfmac-objs += \
 		fwsignal.o \
 		p2p.o \
 		proto.o \
-		bcdc.o \
-		commonring.o \
-		flowring.o \
-		msgbuf.o \
 		dhd_common.o \
 		dhd_linux.o \
 		firmware.o \
 		feature.o \
 		btcoex.o \
 		vendor.o
+brcmfmac-$(CONFIG_BRCMFMAC_PROTO_BCDC) += \
+		bcdc.o
+brcmfmac-$(CONFIG_BRCMFMAC_PROTO_MSGBUF) += \
+		commonring.o \
+		flowring.o \
+		msgbuf.o
 brcmfmac-$(CONFIG_BRCMFMAC_SDIO) += \
 		dhd_sdio.o \
 		bcmsdh.o
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/bcdc.h b/drivers/net/wireless/brcm80211/brcmfmac/bcdc.h
index 17e8c039ff32..6003179c0ceb 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/bcdc.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/bcdc.h
@@ -16,9 +16,12 @@
 #ifndef BRCMFMAC_BCDC_H
 #define BRCMFMAC_BCDC_H
 
-
+#ifdef CONFIG_BRCMFMAC_PROTO_BCDC
 int brcmf_proto_bcdc_attach(struct brcmf_pub *drvr);
 void brcmf_proto_bcdc_detach(struct brcmf_pub *drvr);
-
+#else
+static inline int brcmf_proto_bcdc_attach(struct brcmf_pub *drvr) { return 0; }
+static inline void brcmf_proto_bcdc_detach(struct brcmf_pub *drvr) {}
+#endif
 
 #endif /* BRCMFMAC_BCDC_H */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
index 3122b86050a1..80e73a1262be 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/dhd_bus.h
@@ -67,6 +67,7 @@ struct brcmf_bus_dcmd {
  * @txctl: transmit a control request message to dongle.
  * @rxctl: receive a control response message from dongle.
  * @gettxq: obtain a reference of bus transmit queue (optional).
+ * @wowl_config: specify if dongle is configured for wowl when going to suspend
  *
  * This structure provides an abstract interface towards the
  * bus specific driver. For control messages to common driver
@@ -80,6 +81,7 @@ struct brcmf_bus_ops {
 	int (*txctl)(struct device *dev, unsigned char *msg, uint len);
 	int (*rxctl)(struct device *dev, unsigned char *msg, uint len);
 	struct pktq * (*gettxq)(struct device *dev);
+	void (*wowl_config)(struct device *dev, bool enabled);
 };
 
 
@@ -114,6 +116,7 @@ struct brcmf_bus_msgbuf {
  * @dstats: dongle-based statistical data.
  * @dcmd_list: bus/device specific dongle initialization commands.
  * @chip: device identifier of the dongle chip.
+ * @wowl_supported: is wowl supported by bus driver.
  * @chiprev: revision of the dongle chip.
  */
 struct brcmf_bus {
@@ -131,6 +134,7 @@ struct brcmf_bus {
 	u32 chip;
 	u32 chiprev;
 	bool always_use_fws_queue;
+	bool wowl_supported;
 
 	struct brcmf_bus_ops *ops;
 	struct brcmf_bus_msgbuf *msgbuf;
@@ -177,6 +181,13 @@ struct pktq *brcmf_bus_gettxq(struct brcmf_bus *bus)
 	return bus->ops->gettxq(bus->dev);
 }
 
+static inline
+void brcmf_bus_wowl_config(struct brcmf_bus *bus, bool enabled)
+{
+	if (bus->ops->wowl_config)
+		bus->ops->wowl_config(bus->dev, enabled);
+}
+
 static inline bool brcmf_bus_ready(struct brcmf_bus *bus)
 {
 	return bus->state == BRCMF_BUS_LOAD || bus->state == BRCMF_BUS_DATA;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/feature.c b/drivers/net/wireless/brcm80211/brcmfmac/feature.c
index 50877e3c5d2f..aed53acef456 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/feature.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/feature.c
@@ -107,6 +107,8 @@ void brcmf_feat_attach(struct brcmf_pub *drvr)
 	struct brcmf_if *ifp = drvr->iflist[0];
 
 	brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_MCHAN, "mchan");
+	if (drvr->bus_if->wowl_supported)
+		brcmf_feat_iovar_int_get(ifp, BRCMF_FEAT_WOWL, "wowl");
 
 	/* set chip related quirks */
 	switch (drvr->bus_if->chip) {
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/feature.h b/drivers/net/wireless/brcm80211/brcmfmac/feature.h
index 961d175f8afb..b9a796d0a44d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/feature.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/feature.h
@@ -22,7 +22,8 @@
  * MCHAN: multi-channel for concurrent P2P.
  */
 #define BRCMF_FEAT_LIST \
-	BRCMF_FEAT_DEF(MCHAN)
+	BRCMF_FEAT_DEF(MCHAN) \
+	BRCMF_FEAT_DEF(WOWL)
 /*
  * Quirks:
  *
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
index a1016b811284..1faa929f5fff 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/flowring.c
@@ -354,7 +354,7 @@ struct brcmf_flowring *brcmf_flowring_attach(struct device *dev, u16 nrofrings)
 	struct brcmf_flowring *flow;
 	u32 i;
 
-	flow = kzalloc(sizeof(*flow), GFP_ATOMIC);
+	flow = kzalloc(sizeof(*flow), GFP_KERNEL);
 	if (flow) {
 		flow->dev = dev;
 		flow->nrofrings = nrofrings;
@@ -364,7 +364,7 @@ struct brcmf_flowring *brcmf_flowring_attach(struct device *dev, u16 nrofrings)
 		for (i = 0; i < ARRAY_SIZE(flow->hash); i++)
 			flow->hash[i].ifidx = BRCMF_FLOWRING_INVALID_IFIDX;
 		flow->rings = kcalloc(nrofrings, sizeof(*flow->rings),
-				      GFP_ATOMIC);
+				      GFP_KERNEL);
 		if (!flow->rings) {
 			kfree(flow);
 			flow = NULL;
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fweh.c b/drivers/net/wireless/brcm80211/brcmfmac/fweh.c
index 4f1daabc551b..44fc85f68f7a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fweh.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fweh.c
@@ -185,7 +185,13 @@ static void brcmf_fweh_handle_if_event(struct brcmf_pub *drvr,
 		  ifevent->action, ifevent->ifidx, ifevent->bssidx,
 		  ifevent->flags, ifevent->role);
 
-	if (ifevent->flags & BRCMF_E_IF_FLAG_NOIF) {
+	/* The P2P Device interface event must not be ignored
+	 * contrary to what firmware tells us. The only way to
+	 * distinguish the P2P Device is by looking at the ifidx
+	 * and bssidx received.
+	 */
+	if (!(ifevent->ifidx == 0 && ifevent->bssidx == 1) &&
+	    (ifevent->flags & BRCMF_E_IF_FLAG_NOIF)) {
 		brcmf_dbg(EVENT, "event can be ignored\n");
 		return;
 	}
@@ -210,12 +216,12 @@ static void brcmf_fweh_handle_if_event(struct brcmf_pub *drvr,
 				return;
 	}
 
-	if (ifevent->action == BRCMF_E_IF_CHANGE)
+	if (ifp && ifevent->action == BRCMF_E_IF_CHANGE)
 		brcmf_fws_reset_interface(ifp);
 
 	err = brcmf_fweh_call_event_handler(ifp, emsg->event_code, emsg, data);
 
-	if (ifevent->action == BRCMF_E_IF_DEL) {
+	if (ifp && ifevent->action == BRCMF_E_IF_DEL) {
 		brcmf_fws_del_interface(ifp);
 		brcmf_del_if(drvr, ifevent->bssidx);
 	}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fweh.h b/drivers/net/wireless/brcm80211/brcmfmac/fweh.h
index dd20b1862d44..cbf033f59109 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fweh.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fweh.h
@@ -172,6 +172,8 @@ enum brcmf_fweh_event_code {
 #define BRCMF_E_IF_ROLE_STA			0
 #define BRCMF_E_IF_ROLE_AP			1
 #define BRCMF_E_IF_ROLE_WDS			2
+#define BRCMF_E_IF_ROLE_P2P_GO			3
+#define BRCMF_E_IF_ROLE_P2P_CLIENT		4
 
 /**
  * definitions for event packet validation.
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h b/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
index 2bc68a2137fc..5ff5cd0bb032 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwil_types.h
@@ -53,6 +53,62 @@
 #define BRCMF_OBSS_COEX_OFF		0
 #define BRCMF_OBSS_COEX_ON		1
 
+/* WOWL bits */
+/* Wakeup on Magic packet: */
+#define WL_WOWL_MAGIC			(1 << 0)
+/* Wakeup on Netpattern */
+#define WL_WOWL_NET			(1 << 1)
+/* Wakeup on loss-of-link due to Disassoc/Deauth: */
+#define WL_WOWL_DIS			(1 << 2)
+/* Wakeup on retrograde TSF: */
+#define WL_WOWL_RETR			(1 << 3)
+/* Wakeup on loss of beacon: */
+#define WL_WOWL_BCN			(1 << 4)
+/* Wakeup after test: */
+#define WL_WOWL_TST			(1 << 5)
+/* Wakeup after PTK refresh: */
+#define WL_WOWL_M1			(1 << 6)
+/* Wakeup after receipt of EAP-Identity Req: */
+#define WL_WOWL_EAPID			(1 << 7)
+/* Wakeind via PME(0) or GPIO(1): */
+#define WL_WOWL_PME_GPIO		(1 << 8)
+/* need tkip phase 1 key to be updated by the driver: */
+#define WL_WOWL_NEEDTKIP1		(1 << 9)
+/* enable wakeup if GTK fails: */
+#define WL_WOWL_GTK_FAILURE		(1 << 10)
+/* support extended magic packets: */
+#define WL_WOWL_EXTMAGPAT		(1 << 11)
+/* support ARP/NS/keepalive offloading: */
+#define WL_WOWL_ARPOFFLOAD		(1 << 12)
+/* read protocol version for EAPOL frames: */
+#define WL_WOWL_WPA2			(1 << 13)
+/* If the bit is set, use key rotaton: */
+#define WL_WOWL_KEYROT			(1 << 14)
+/* If the bit is set, frm received was bcast frame: */
+#define WL_WOWL_BCAST			(1 << 15)
+/* If the bit is set, scan offload is enabled: */
+#define WL_WOWL_SCANOL			(1 << 16)
+/* Wakeup on tcpkeep alive timeout: */
+#define WL_WOWL_TCPKEEP_TIME		(1 << 17)
+/* Wakeup on mDNS Conflict Resolution: */
+#define WL_WOWL_MDNS_CONFLICT		(1 << 18)
+/* Wakeup on mDNS Service Connect: */
+#define WL_WOWL_MDNS_SERVICE		(1 << 19)
+/* tcp keepalive got data: */
+#define WL_WOWL_TCPKEEP_DATA		(1 << 20)
+/* Firmware died in wowl mode: */
+#define WL_WOWL_FW_HALT			(1 << 21)
+/* Enable detection of radio button changes: */
+#define WL_WOWL_ENAB_HWRADIO		(1 << 22)
+/* Offloads detected MIC failure(s): */
+#define WL_WOWL_MIC_FAIL		(1 << 23)
+/* Wakeup in Unassociated state (Net/Magic Pattern): */
+#define WL_WOWL_UNASSOC			(1 << 24)
+/* Wakeup if received matched secured pattern: */
+#define WL_WOWL_SECURE			(1 << 25)
+/* Link Down indication in WoWL mode: */
+#define WL_WOWL_LINKDOWN		(1 << 31)
+
 /* join preference types for join_pref iovar */
 enum brcmf_join_pref_types {
 	BRCMF_JOIN_PREF_RSSI = 1,
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
index d42f7d04b65f..183f08d7fc8c 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/fwsignal.c
@@ -1636,7 +1636,7 @@ int brcmf_fws_hdrpull(struct brcmf_pub *drvr, int ifidx, s16 signal_len,
 	if (!signal_len)
 		return 0;
 	/* if flow control disabled, skip to packet data and leave */
-	if (!fws->fw_signals) {
+	if ((!fws) || (!fws->fw_signals)) {
 		skb_pull(skb, signal_len);
 		return 0;
 	}
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
index 8f8b9373de95..11cc051f97cd 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.c
@@ -208,6 +208,14 @@ struct msgbuf_flowring_flush_resp {
 	__le32				rsvd0[3];
 };
 
+struct brcmf_msgbuf_work_item {
+	struct list_head queue;
+	u32 flowid;
+	int ifidx;
+	u8 sa[ETH_ALEN];
+	u8 da[ETH_ALEN];
+};
+
 struct brcmf_msgbuf {
 	struct brcmf_pub *drvr;
 
@@ -230,7 +238,7 @@ struct brcmf_msgbuf {
 	dma_addr_t ioctbuf_handle;
 	u32 ioctbuf_phys_hi;
 	u32 ioctbuf_phys_lo;
-	u32 ioctl_resp_status;
+	int ioctl_resp_status;
 	u32 ioctl_resp_ret_len;
 	u32 ioctl_resp_pktid;
 
@@ -248,6 +256,10 @@ struct brcmf_msgbuf {
 	struct work_struct txflow_work;
 	unsigned long *flow_map;
 	unsigned long *txstatus_done_map;
+
+	struct work_struct flowring_work;
+	spinlock_t flowring_work_lock;
+	struct list_head work_queue;
 };
 
 struct brcmf_msgbuf_pktid {
@@ -284,11 +296,11 @@ brcmf_msgbuf_init_pktids(u32 nr_array_entries,
 	struct brcmf_msgbuf_pktid *array;
 	struct brcmf_msgbuf_pktids *pktids;
 
-	array = kcalloc(nr_array_entries, sizeof(*array), GFP_ATOMIC);
+	array = kcalloc(nr_array_entries, sizeof(*array), GFP_KERNEL);
 	if (!array)
 		return NULL;
 
-	pktids = kzalloc(sizeof(*pktids), GFP_ATOMIC);
+	pktids = kzalloc(sizeof(*pktids), GFP_KERNEL);
 	if (!pktids) {
 		kfree(array);
 		return NULL;
@@ -544,11 +556,29 @@ brcmf_msgbuf_remove_flowring(struct brcmf_msgbuf *msgbuf, u16 flowid)
 }
 
 
-static u32 brcmf_msgbuf_flowring_create(struct brcmf_msgbuf *msgbuf, int ifidx,
-					struct sk_buff *skb)
+static struct brcmf_msgbuf_work_item *
+brcmf_msgbuf_dequeue_work(struct brcmf_msgbuf *msgbuf)
+{
+	struct brcmf_msgbuf_work_item *work = NULL;
+	ulong flags;
+
+	spin_lock_irqsave(&msgbuf->flowring_work_lock, flags);
+	if (!list_empty(&msgbuf->work_queue)) {
+		work = list_first_entry(&msgbuf->work_queue,
+					struct brcmf_msgbuf_work_item, queue);
+		list_del(&work->queue);
+	}
+	spin_unlock_irqrestore(&msgbuf->flowring_work_lock, flags);
+
+	return work;
+}
+
+
+static u32
+brcmf_msgbuf_flowring_create_worker(struct brcmf_msgbuf *msgbuf,
+				    struct brcmf_msgbuf_work_item *work)
 {
 	struct msgbuf_tx_flowring_create_req *create;
-	struct ethhdr *eh = (struct ethhdr *)(skb->data);
 	struct brcmf_commonring *commonring;
 	void *ret_ptr;
 	u32 flowid;
@@ -557,16 +587,11 @@ static u32 brcmf_msgbuf_flowring_create(struct brcmf_msgbuf *msgbuf, int ifidx,
 	long long address;
 	int err;
 
-	flowid = brcmf_flowring_create(msgbuf->flow, eh->h_dest,
-				       skb->priority, ifidx);
-	if (flowid == BRCMF_FLOWRING_INVALID_ID)
-		return flowid;
-
+	flowid = work->flowid;
 	dma_sz = BRCMF_H2D_TXFLOWRING_MAX_ITEM * BRCMF_H2D_TXFLOWRING_ITEMSIZE;
-
 	dma_buf = dma_alloc_coherent(msgbuf->drvr->bus_if->dev, dma_sz,
 				     &msgbuf->flowring_dma_handle[flowid],
-				     GFP_ATOMIC);
+				     GFP_KERNEL);
 	if (!dma_buf) {
 		brcmf_err("dma_alloc_coherent failed\n");
 		brcmf_flowring_delete(msgbuf->flow, flowid);
@@ -589,13 +614,13 @@ static u32 brcmf_msgbuf_flowring_create(struct brcmf_msgbuf *msgbuf, int ifidx,
 
 	create = (struct msgbuf_tx_flowring_create_req *)ret_ptr;
 	create->msg.msgtype = MSGBUF_TYPE_FLOW_RING_CREATE;
-	create->msg.ifidx = ifidx;
+	create->msg.ifidx = work->ifidx;
 	create->msg.request_id = 0;
 	create->tid = brcmf_flowring_tid(msgbuf->flow, flowid);
 	create->flow_ring_id = cpu_to_le16(flowid +
 					   BRCMF_NROF_H2D_COMMON_MSGRINGS);
-	memcpy(create->sa, eh->h_source, ETH_ALEN);
-	memcpy(create->da, eh->h_dest, ETH_ALEN);
+	memcpy(create->sa, work->sa, ETH_ALEN);
+	memcpy(create->da, work->da, ETH_ALEN);
 	address = (long long)(long)msgbuf->flowring_dma_handle[flowid];
 	create->flow_ring_addr.high_addr = cpu_to_le32(address >> 32);
 	create->flow_ring_addr.low_addr = cpu_to_le32(address & 0xffffffff);
@@ -603,7 +628,7 @@ static u32 brcmf_msgbuf_flowring_create(struct brcmf_msgbuf *msgbuf, int ifidx,
 	create->len_item = cpu_to_le16(BRCMF_H2D_TXFLOWRING_ITEMSIZE);
 
 	brcmf_dbg(MSGBUF, "Send Flow Create Req flow ID %d for peer %pM prio %d ifindex %d\n",
-		  flowid, eh->h_dest, create->tid, ifidx);
+		  flowid, work->da, create->tid, work->ifidx);
 
 	err = brcmf_commonring_write_complete(commonring);
 	brcmf_commonring_unlock(commonring);
@@ -617,6 +642,53 @@ static u32 brcmf_msgbuf_flowring_create(struct brcmf_msgbuf *msgbuf, int ifidx,
 }
 
 
+static void brcmf_msgbuf_flowring_worker(struct work_struct *work)
+{
+	struct brcmf_msgbuf *msgbuf;
+	struct brcmf_msgbuf_work_item *create;
+
+	msgbuf = container_of(work, struct brcmf_msgbuf, flowring_work);
+
+	while ((create = brcmf_msgbuf_dequeue_work(msgbuf))) {
+		brcmf_msgbuf_flowring_create_worker(msgbuf, create);
+		kfree(create);
+	}
+}
+
+
+static u32 brcmf_msgbuf_flowring_create(struct brcmf_msgbuf *msgbuf, int ifidx,
+					struct sk_buff *skb)
+{
+	struct brcmf_msgbuf_work_item *create;
+	struct ethhdr *eh = (struct ethhdr *)(skb->data);
+	u32 flowid;
+	ulong flags;
+
+	create = kzalloc(sizeof(*create), GFP_ATOMIC);
+	if (create == NULL)
+		return BRCMF_FLOWRING_INVALID_ID;
+
+	flowid = brcmf_flowring_create(msgbuf->flow, eh->h_dest,
+				       skb->priority, ifidx);
+	if (flowid == BRCMF_FLOWRING_INVALID_ID) {
+		kfree(create);
+		return flowid;
+	}
+
+	create->flowid = flowid;
+	create->ifidx = ifidx;
+	memcpy(create->sa, eh->h_source, ETH_ALEN);
+	memcpy(create->da, eh->h_dest, ETH_ALEN);
+
+	spin_lock_irqsave(&msgbuf->flowring_work_lock, flags);
+	list_add_tail(&create->queue, &msgbuf->work_queue);
+	spin_unlock_irqrestore(&msgbuf->flowring_work_lock, flags);
+	schedule_work(&msgbuf->flowring_work);
+
+	return flowid;
+}
+
+
 static void brcmf_msgbuf_txflow(struct brcmf_msgbuf *msgbuf, u8 flowid)
 {
 	struct brcmf_flowring *flow = msgbuf->flow;
@@ -767,7 +839,8 @@ brcmf_msgbuf_process_ioctl_complete(struct brcmf_msgbuf *msgbuf, void *buf)
 
 	ioctl_resp = (struct msgbuf_ioctl_resp_hdr *)buf;
 
-	msgbuf->ioctl_resp_status = le16_to_cpu(ioctl_resp->compl_hdr.status);
+	msgbuf->ioctl_resp_status =
+			(s16)le16_to_cpu(ioctl_resp->compl_hdr.status);
 	msgbuf->ioctl_resp_ret_len = le16_to_cpu(ioctl_resp->resp_len);
 	msgbuf->ioctl_resp_pktid = le32_to_cpu(ioctl_resp->msg.request_id);
 
@@ -1271,7 +1344,7 @@ int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
 	u32 count;
 
 	if_msgbuf = drvr->bus_if->msgbuf;
-	msgbuf = kzalloc(sizeof(*msgbuf), GFP_ATOMIC);
+	msgbuf = kzalloc(sizeof(*msgbuf), GFP_KERNEL);
 	if (!msgbuf)
 		goto fail;
 
@@ -1282,11 +1355,11 @@ int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
 	}
 	INIT_WORK(&msgbuf->txflow_work, brcmf_msgbuf_txflow_worker);
 	count = BITS_TO_LONGS(if_msgbuf->nrof_flowrings);
-	msgbuf->flow_map = kzalloc(count, GFP_ATOMIC);
+	msgbuf->flow_map = kzalloc(count, GFP_KERNEL);
 	if (!msgbuf->flow_map)
 		goto fail;
 
-	msgbuf->txstatus_done_map = kzalloc(count, GFP_ATOMIC);
+	msgbuf->txstatus_done_map = kzalloc(count, GFP_KERNEL);
 	if (!msgbuf->txstatus_done_map)
 		goto fail;
 
@@ -1294,7 +1367,7 @@ int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
 	msgbuf->ioctbuf = dma_alloc_coherent(drvr->bus_if->dev,
 					     BRCMF_TX_IOCTL_MAX_MSG_SIZE,
 					     &msgbuf->ioctbuf_handle,
-					     GFP_ATOMIC);
+					     GFP_KERNEL);
 	if (!msgbuf->ioctbuf)
 		goto fail;
 	address = (long long)(long)msgbuf->ioctbuf_handle;
@@ -1317,7 +1390,7 @@ int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
 	msgbuf->flowrings = (struct brcmf_commonring **)if_msgbuf->flowrings;
 	msgbuf->nrof_flowrings = if_msgbuf->nrof_flowrings;
 	msgbuf->flowring_dma_handle = kzalloc(msgbuf->nrof_flowrings *
-		sizeof(*msgbuf->flowring_dma_handle), GFP_ATOMIC);
+		sizeof(*msgbuf->flowring_dma_handle), GFP_KERNEL);
 	if (!msgbuf->flowring_dma_handle)
 		goto fail;
 
@@ -1357,6 +1430,10 @@ int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
 	brcmf_msgbuf_rxbuf_event_post(msgbuf);
 	brcmf_msgbuf_rxbuf_ioctlresp_post(msgbuf);
 
+	INIT_WORK(&msgbuf->flowring_work, brcmf_msgbuf_flowring_worker);
+	spin_lock_init(&msgbuf->flowring_work_lock);
+	INIT_LIST_HEAD(&msgbuf->work_queue);
+
 	return 0;
 
 fail:
@@ -1379,11 +1456,19 @@ fail:
 void brcmf_proto_msgbuf_detach(struct brcmf_pub *drvr)
 {
 	struct brcmf_msgbuf *msgbuf;
+	struct brcmf_msgbuf_work_item *work;
 
 	brcmf_dbg(TRACE, "Enter\n");
 	if (drvr->proto->pd) {
 		msgbuf = (struct brcmf_msgbuf *)drvr->proto->pd;
-
+		cancel_work_sync(&msgbuf->flowring_work);
+		while (!list_empty(&msgbuf->work_queue)) {
+			work = list_first_entry(&msgbuf->work_queue,
+						struct brcmf_msgbuf_work_item,
+						queue);
+			list_del(&work->queue);
+			kfree(work);
+		}
 		kfree(msgbuf->flow_map);
 		kfree(msgbuf->txstatus_done_map);
 		if (msgbuf->txflow_wq)
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h
index f901ae52bf2b..77a51b8c1e12 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/msgbuf.h
@@ -15,6 +15,7 @@
 #ifndef BRCMFMAC_MSGBUF_H
 #define BRCMFMAC_MSGBUF_H
 
+#ifdef CONFIG_BRCMFMAC_PROTO_MSGBUF
 
 #define BRCMF_H2D_MSGRING_CONTROL_SUBMIT_MAX_ITEM	20
 #define BRCMF_H2D_MSGRING_RXPOST_SUBMIT_MAX_ITEM	256
@@ -32,9 +33,15 @@
 
 
 int brcmf_proto_msgbuf_rx_trigger(struct device *dev);
+void brcmf_msgbuf_delete_flowring(struct brcmf_pub *drvr, u8 flowid);
 int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr);
 void brcmf_proto_msgbuf_detach(struct brcmf_pub *drvr);
-void brcmf_msgbuf_delete_flowring(struct brcmf_pub *drvr, u8 flowid);
-
+#else
+static inline int brcmf_proto_msgbuf_attach(struct brcmf_pub *drvr)
+{
+	return 0;
+}
+static inline void brcmf_proto_msgbuf_detach(struct brcmf_pub *drvr) {}
+#endif
 
 #endif /* BRCMFMAC_MSGBUF_H */
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
index 057b982ea8b3..d54c58a32faa 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/p2p.c
@@ -440,8 +440,11 @@ static int brcmf_p2p_set_firmware(struct brcmf_if *ifp, u8 *p2p_mac)
 
 	/* In case of COB type, firmware has default mac address
 	 * After Initializing firmware, we have to set current mac address to
-	 * firmware for P2P device address
+	 * firmware for P2P device address. This must be done with discovery
+	 * disabled.
 	 */
+	brcmf_fil_iovar_int_set(ifp, "p2p_disc", 0);
+
 	ret = brcmf_fil_iovar_data_set(ifp, "p2p_da_override", p2p_mac,
 				       ETH_ALEN);
 	if (ret)
@@ -1431,8 +1434,7 @@ int brcmf_p2p_notify_action_frame_rx(struct brcmf_if *ifp,
 					      IEEE80211_BAND_5GHZ);
 
 	wdev = &ifp->vif->wdev;
-	cfg80211_rx_mgmt(wdev, freq, 0, (u8 *)mgmt_frame, mgmt_frame_len, 0,
-			 GFP_ATOMIC);
+	cfg80211_rx_mgmt(wdev, freq, 0, (u8 *)mgmt_frame, mgmt_frame_len, 0);
 
 	kfree(mgmt_frame);
 	return 0;
@@ -1896,8 +1898,7 @@ s32 brcmf_p2p_notify_rx_mgmt_p2p_probereq(struct brcmf_if *ifp,
 					      IEEE80211_BAND_2GHZ :
 					      IEEE80211_BAND_5GHZ);
 
-	cfg80211_rx_mgmt(&vif->wdev, freq, 0, mgmt_frame, mgmt_frame_len, 0,
-			 GFP_ATOMIC);
+	cfg80211_rx_mgmt(&vif->wdev, freq, 0, mgmt_frame, mgmt_frame_len, 0);
 
 	brcmf_dbg(INFO, "mgmt_frame_len (%d) , e->datalen (%d), chanspec (%04x), freq (%d)\n",
 		  mgmt_frame_len, e->datalen, chanspec, freq);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
index e5101b287e4e..8c0632ec9f7a 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/pcie.c
@@ -165,6 +165,8 @@ enum brcmf_pcie_state {
 
 #define BRCMF_H2D_HOST_D3_INFORM		0x00000001
 #define BRCMF_H2D_HOST_DS_ACK			0x00000002
+#define BRCMF_H2D_HOST_D0_INFORM_IN_USE		0x00000008
+#define BRCMF_H2D_HOST_D0_INFORM		0x00000010
 
 #define BRCMF_PCIE_MBDATA_TIMEOUT		2000
 
@@ -243,6 +245,7 @@ struct brcmf_pciedev_info {
 	wait_queue_head_t mbdata_resp_wait;
 	bool mbdata_completed;
 	bool irq_allocated;
+	bool wowl_enabled;
 };
 
 struct brcmf_pcie_ringbuf {
@@ -537,7 +540,7 @@ static int brcmf_pcie_exit_download_state(struct brcmf_pciedev_info *devinfo,
 }
 
 
-static void
+static int
 brcmf_pcie_send_mb_data(struct brcmf_pciedev_info *devinfo, u32 htod_mb_data)
 {
 	struct brcmf_pcie_shared_info *shared;
@@ -558,13 +561,15 @@ brcmf_pcie_send_mb_data(struct brcmf_pciedev_info *devinfo, u32 htod_mb_data)
 		msleep(10);
 		i++;
 		if (i > 100)
-			break;
+			return -EIO;
 		cur_htod_mb_data = brcmf_pcie_read_tcm32(devinfo, addr);
 	}
 
 	brcmf_pcie_write_tcm32(devinfo, addr, htod_mb_data);
 	pci_write_config_dword(devinfo->pdev, BRCMF_PCIE_REG_SBMBX, 1);
 	pci_write_config_dword(devinfo->pdev, BRCMF_PCIE_REG_SBMBX, 1);
+
+	return 0;
 }
 
 
@@ -1229,11 +1234,27 @@ static int brcmf_pcie_rx_ctlpkt(struct device *dev, unsigned char *msg,
 }
 
 
+static void brcmf_pcie_wowl_config(struct device *dev, bool enabled)
+{
+	struct brcmf_bus *bus_if = dev_get_drvdata(dev);
+	struct brcmf_pciedev *buspub = bus_if->bus_priv.pcie;
+	struct brcmf_pciedev_info *devinfo = buspub->devinfo;
+
+	brcmf_dbg(PCIE, "Configuring WOWL, enabled=%d\n", enabled);
+	devinfo->wowl_enabled = enabled;
+	if (enabled)
+		device_set_wakeup_enable(&devinfo->pdev->dev, true);
+	else
+		device_set_wakeup_enable(&devinfo->pdev->dev, false);
+}
+
+
 static struct brcmf_bus_ops brcmf_pcie_bus_ops = {
 	.txdata = brcmf_pcie_tx,
 	.stop = brcmf_pcie_down,
 	.txctl = brcmf_pcie_tx_ctlpkt,
 	.rxctl = brcmf_pcie_rx_ctlpkt,
+	.wowl_config = brcmf_pcie_wowl_config,
 };
 
 
@@ -1668,6 +1689,7 @@ brcmf_pcie_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	bus->ops = &brcmf_pcie_bus_ops;
 	bus->proto_type = BRCMF_PROTO_MSGBUF;
 	bus->chip = devinfo->coreid;
+	bus->wowl_supported = pci_pme_capable(pdev, PCI_D3hot);
 	dev_set_drvdata(&pdev->dev, bus);
 
 	ret = brcmf_pcie_get_fwnames(devinfo);
@@ -1759,36 +1781,62 @@ static int brcmf_pcie_suspend(struct pci_dev *pdev, pm_message_t state)
 		brcmf_err("Timeout on response for entering D3 substate\n");
 		return -EIO;
 	}
-	brcmf_pcie_release_irq(devinfo);
+	brcmf_pcie_send_mb_data(devinfo, BRCMF_H2D_HOST_D0_INFORM_IN_USE);
 
 	err = pci_save_state(pdev);
-	if (err) {
+	if (err)
 		brcmf_err("pci_save_state failed, err=%d\n", err);
-		return err;
+	if ((err) || (!devinfo->wowl_enabled)) {
+		brcmf_chip_detach(devinfo->ci);
+		devinfo->ci = NULL;
+		brcmf_pcie_remove(pdev);
+		return 0;
 	}
 
-	brcmf_chip_detach(devinfo->ci);
-	devinfo->ci = NULL;
-
-	brcmf_pcie_remove(pdev);
-
 	return pci_prepare_to_sleep(pdev);
 }
 
-
 static int brcmf_pcie_resume(struct pci_dev *pdev)
 {
+	struct brcmf_pciedev_info *devinfo;
+	struct brcmf_bus *bus;
 	int err;
 
-	brcmf_dbg(PCIE, "Enter, pdev=%p\n", pdev);
+	bus = dev_get_drvdata(&pdev->dev);
+	brcmf_dbg(PCIE, "Enter, pdev=%p, bus=%p\n", pdev, bus);
 
 	err = pci_set_power_state(pdev, PCI_D0);
 	if (err) {
 		brcmf_err("pci_set_power_state failed, err=%d\n", err);
-		return err;
+		goto cleanup;
 	}
 	pci_restore_state(pdev);
+	pci_enable_wake(pdev, PCI_D3hot, false);
+	pci_enable_wake(pdev, PCI_D3cold, false);
+
+	/* Check if device is still up and running, if so we are ready */
+	if (bus) {
+		devinfo = bus->bus_priv.pcie->devinfo;
+		if (brcmf_pcie_read_reg32(devinfo,
+					  BRCMF_PCIE_PCIE2REG_INTMASK) != 0) {
+			if (brcmf_pcie_send_mb_data(devinfo,
+						    BRCMF_H2D_HOST_D0_INFORM))
+				goto cleanup;
+			brcmf_dbg(PCIE, "Hot resume, continue....\n");
+			brcmf_pcie_select_core(devinfo, BCMA_CORE_PCIE2);
+			brcmf_bus_change_state(bus, BRCMF_BUS_DATA);
+			brcmf_pcie_intr_enable(devinfo);
+			return 0;
+		}
+	}
 
+cleanup:
+	if (bus) {
+		devinfo = bus->bus_priv.pcie->devinfo;
+		brcmf_chip_detach(devinfo->ci);
+		devinfo->ci = NULL;
+		brcmf_pcie_remove(pdev);
+	}
 	err = brcmf_pcie_probe(pdev, NULL);
 	if (err)
 		brcmf_err("probe after resume failed, err=%d\n", err);
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
index 02fe706fc9ec..28fa25b509db 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.c
@@ -37,6 +37,7 @@
 #include "fwil.h"
 #include "proto.h"
 #include "vendor.h"
+#include "dhd_bus.h"
 
 #define BRCMF_SCAN_IE_LEN_MAX		2048
 #define BRCMF_PNO_VERSION		2
@@ -497,8 +498,11 @@ brcmf_configure_arp_offload(struct brcmf_if *ifp, bool enable)
 static void
 brcmf_cfg80211_update_proto_addr_mode(struct wireless_dev *wdev)
 {
-	struct net_device *ndev = wdev->netdev;
-	struct brcmf_if *ifp = netdev_priv(ndev);
+	struct brcmf_cfg80211_vif *vif;
+	struct brcmf_if *ifp;
+
+	vif = container_of(wdev, struct brcmf_cfg80211_vif, wdev);
+	ifp = vif->ifp;
 
 	if ((wdev->iftype == NL80211_IFTYPE_ADHOC) ||
 	    (wdev->iftype == NL80211_IFTYPE_AP) ||
@@ -2394,9 +2398,13 @@ static s32 brcmf_inform_single_bss(struct brcmf_cfg80211_info *cfg,
 	brcmf_dbg(CONN, "Beacon interval: %d\n", notify_interval);
 	brcmf_dbg(CONN, "Signal: %d\n", notify_signal);
 
-	bss = cfg80211_inform_bss(wiphy, notify_channel, (const u8 *)bi->BSSID,
-		0, notify_capability, notify_interval, notify_ie,
-		notify_ielen, notify_signal, GFP_KERNEL);
+	bss = cfg80211_inform_bss(wiphy, notify_channel,
+				  CFG80211_BSS_FTYPE_UNKNOWN,
+				  (const u8 *)bi->BSSID,
+				  0, notify_capability,
+				  notify_interval, notify_ie,
+				  notify_ielen, notify_signal,
+				  GFP_KERNEL);
 
 	if (!bss)
 		return -ENOMEM;
@@ -2422,7 +2430,7 @@ static s32 brcmf_inform_bss(struct brcmf_cfg80211_info *cfg)
 	s32 err = 0;
 	int i;
 
-	bss_list = cfg->bss_list;
+	bss_list = (struct brcmf_scan_results *)cfg->escan_info.escan_buf;
 	if (bss_list->count != 0 &&
 	    bss_list->version != BRCMF_BSS_INFO_VERSION) {
 		brcmf_err("Version %d != WL_BSS_INFO_VERSION\n",
@@ -2498,9 +2506,11 @@ static s32 wl_inform_ibss(struct brcmf_cfg80211_info *cfg,
 	brcmf_dbg(CONN, "beacon interval: %d\n", notify_interval);
 	brcmf_dbg(CONN, "signal: %d\n", notify_signal);
 
-	bss = cfg80211_inform_bss(wiphy, notify_channel, bssid,
-		0, notify_capability, notify_interval,
-		notify_ie, notify_ielen, notify_signal, GFP_KERNEL);
+	bss = cfg80211_inform_bss(wiphy, notify_channel,
+				  CFG80211_BSS_FTYPE_UNKNOWN, bssid, 0,
+				  notify_capability, notify_interval,
+				  notify_ie, notify_ielen, notify_signal,
+				  GFP_KERNEL);
 
 	if (!bss) {
 		err = -ENOMEM;
@@ -2596,6 +2606,7 @@ static void brcmf_cfg80211_escan_timeout_worker(struct work_struct *work)
 			container_of(work, struct brcmf_cfg80211_info,
 				     escan_timeout_work);
 
+	brcmf_inform_bss(cfg);
 	brcmf_notify_escan_complete(cfg, cfg->escan_info.ifp, true, true);
 }
 
@@ -2734,12 +2745,9 @@ brcmf_cfg80211_escan_handler(struct brcmf_if *ifp,
 		if (brcmf_p2p_scan_finding_common_channel(cfg, NULL))
 			goto exit;
 		if (cfg->scan_request) {
-			cfg->bss_list = (struct brcmf_scan_results *)
-				cfg->escan_info.escan_buf;
 			brcmf_inform_bss(cfg);
 			aborted = status != BRCMF_E_STATUS_SUCCESS;
-			brcmf_notify_escan_complete(cfg, ifp, aborted,
-						    false);
+			brcmf_notify_escan_complete(cfg, ifp, aborted, false);
 		} else
 			brcmf_dbg(SCAN, "Ignored scan complete result 0x%x\n",
 				  status);
@@ -2773,50 +2781,91 @@ static __always_inline void brcmf_delay(u32 ms)
 
 static s32 brcmf_cfg80211_resume(struct wiphy *wiphy)
 {
+	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
+	struct net_device *ndev = cfg_to_ndev(cfg);
+	struct brcmf_if *ifp = netdev_priv(ndev);
+
 	brcmf_dbg(TRACE, "Enter\n");
 
+	if (cfg->wowl_enabled) {
+		brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM,
+				      cfg->pre_wowl_pmmode);
+		brcmf_fil_iovar_data_set(ifp, "wowl_pattern", "clr", 4);
+		brcmf_fil_iovar_int_set(ifp, "wowl_clear", 0);
+		cfg->wowl_enabled = false;
+	}
 	return 0;
 }
 
+static void brcmf_configure_wowl(struct brcmf_cfg80211_info *cfg,
+				 struct brcmf_if *ifp,
+				 struct cfg80211_wowlan *wowl)
+{
+	u32 wowl_config;
+
+	brcmf_dbg(TRACE, "Suspend, wowl config.\n");
+
+	brcmf_fil_cmd_int_get(ifp, BRCMF_C_GET_PM, &cfg->pre_wowl_pmmode);
+	brcmf_fil_cmd_int_set(ifp, BRCMF_C_SET_PM, PM_MAX);
+
+	wowl_config = 0;
+	if (wowl->disconnect)
+		wowl_config |= WL_WOWL_DIS | WL_WOWL_BCN | WL_WOWL_RETR;
+		/* Note: if "wowl" target and not "wowlpf" then wowl_bcn_loss
+		 * should be configured. This paramater is not supported by
+		 * wowlpf.
+		 */
+	if (wowl->magic_pkt)
+		wowl_config |= WL_WOWL_MAGIC;
+	brcmf_fil_iovar_int_set(ifp, "wowl", wowl_config);
+	brcmf_fil_iovar_int_set(ifp, "wowl_activate", 1);
+	brcmf_bus_wowl_config(cfg->pub->bus_if, true);
+	cfg->wowl_enabled = true;
+}
+
 static s32 brcmf_cfg80211_suspend(struct wiphy *wiphy,
-				  struct cfg80211_wowlan *wow)
+				  struct cfg80211_wowlan *wowl)
 {
 	struct brcmf_cfg80211_info *cfg = wiphy_to_cfg(wiphy);
 	struct net_device *ndev = cfg_to_ndev(cfg);
+	struct brcmf_if *ifp = netdev_priv(ndev);
 	struct brcmf_cfg80211_vif *vif;
 
 	brcmf_dbg(TRACE, "Enter\n");
 
-	/*
-	 * if the primary net_device is not READY there is nothing
+	/* if the primary net_device is not READY there is nothing
 	 * we can do but pray resume goes smoothly.
 	 */
-	vif = ((struct brcmf_if *)netdev_priv(ndev))->vif;
-	if (!check_vif_up(vif))
+	if (!check_vif_up(ifp->vif))
 		goto exit;
 
-	list_for_each_entry(vif, &cfg->vif_list, list) {
-		if (!test_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state))
-			continue;
-		/*
-		 * While going to suspend if associated with AP disassociate
-		 * from AP to save power while system is in suspended state
-		 */
-		brcmf_link_down(vif);
-
-		/* Make sure WPA_Supplicant receives all the event
-		 * generated due to DISASSOC call to the fw to keep
-		 * the state fw and WPA_Supplicant state consistent
-		 */
-		brcmf_delay(500);
-	}
-
 	/* end any scanning */
 	if (test_bit(BRCMF_SCAN_STATUS_BUSY, &cfg->scan_status))
 		brcmf_abort_scanning(cfg);
 
-	/* Turn off watchdog timer */
-	brcmf_set_mpc(netdev_priv(ndev), 1);
+	if (wowl == NULL) {
+		brcmf_bus_wowl_config(cfg->pub->bus_if, false);
+		list_for_each_entry(vif, &cfg->vif_list, list) {
+			if (!test_bit(BRCMF_VIF_STATUS_READY, &vif->sme_state))
+				continue;
+			/* While going to suspend if associated with AP
+			 * disassociate from AP to save power while system is
+			 * in suspended state
+			 */
+			brcmf_link_down(vif);
+			/* Make sure WPA_Supplicant receives all the event
+			 * generated due to DISASSOC call to the fw to keep
+			 * the state fw and WPA_Supplicant state consistent
+			 */
+			brcmf_delay(500);
+		}
+		/* Configure MPC */
+		brcmf_set_mpc(ifp, 1);
+
+	} else {
+		/* Configure WOWL paramaters */
+		brcmf_configure_wowl(cfg, ifp, wowl);
+	}
 
 exit:
 	brcmf_dbg(TRACE, "Exit\n");
@@ -4918,7 +4967,7 @@ static void brcmf_count_20mhz_channels(struct brcmf_cfg80211_info *cfg,
 	struct brcmu_chan ch;
 	int i;
 
-	for (i = 0; i <= total; i++) {
+	for (i = 0; i < total; i++) {
 		ch.chspec = (u16)le32_to_cpu(chlist->element[i]);
 		cfg->d11inf.decchspec(&ch);
 
@@ -5143,6 +5192,7 @@ static int brcmf_enable_bw40_2g(struct brcmf_cfg80211_info *cfg)
 
 		ch.band = BRCMU_CHAN_BAND_2G;
 		ch.bw = BRCMU_CHAN_BW_40;
+		ch.sb = BRCMU_CHAN_SB_NONE;
 		ch.chnum = 0;
 		cfg->d11inf.encchspec(&ch);
 
@@ -5176,6 +5226,7 @@ static int brcmf_enable_bw40_2g(struct brcmf_cfg80211_info *cfg)
 
 			brcmf_update_bw40_channel_flag(&band->channels[j], &ch);
 		}
+		kfree(pbuf);
 	}
 	return err;
 }
@@ -5389,6 +5440,21 @@ static void brcmf_wiphy_pno_params(struct wiphy *wiphy)
 	wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
 }
 
+
+#ifdef CONFIG_PM
+static const struct wiphy_wowlan_support brcmf_wowlan_support = {
+	.flags = WIPHY_WOWLAN_MAGIC_PKT | WIPHY_WOWLAN_DISCONNECT,
+};
+#endif
+
+static void brcmf_wiphy_wowl_params(struct wiphy *wiphy)
+{
+#ifdef CONFIG_PM
+	/* wowl settings */
+	wiphy->wowlan = &brcmf_wowlan_support;
+#endif
+}
+
 static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 {
 	struct ieee80211_iface_combination ifc_combo;
@@ -5426,6 +5492,9 @@ static int brcmf_setup_wiphy(struct wiphy *wiphy, struct brcmf_if *ifp)
 	wiphy->vendor_commands = brcmf_vendor_cmds;
 	wiphy->n_vendor_commands = BRCMF_VNDR_CMDS_LAST - 1;
 
+	if (brcmf_feat_is_enabled(ifp, BRCMF_FEAT_WOWL))
+		brcmf_wiphy_wowl_params(wiphy);
+
 	return brcmf_setup_wiphybands(wiphy);
 }
 
diff --git a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
index f9fb10998e79..6abf94e41d3d 100644
--- a/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
+++ b/drivers/net/wireless/brcm80211/brcmfmac/wl_cfg80211.h
@@ -35,7 +35,7 @@
 #define WL_SCAN_PASSIVE_TIME		120
 
 #define WL_ESCAN_BUF_SIZE		(1024 * 64)
-#define WL_ESCAN_TIMER_INTERVAL_MS	8000 /* E-Scan timeout */
+#define WL_ESCAN_TIMER_INTERVAL_MS	10000 /* E-Scan timeout */
 
 #define WL_ESCAN_ACTION_START		1
 #define WL_ESCAN_ACTION_CONTINUE	2
@@ -363,6 +363,8 @@ struct brcmf_cfg80211_vif_event {
  * @vif_list: linked list of vif instances.
  * @vif_cnt: number of vif instances.
  * @vif_event: vif event signalling.
+ * @wowl_enabled; set during suspend, is wowl used.
+ * @pre_wowl_pmmode: intermediate storage of pm mode during wowl.
  */
 struct brcmf_cfg80211_info {
 	struct wiphy *wiphy;
@@ -371,7 +373,6 @@ struct brcmf_cfg80211_info {
 	struct brcmf_btcoex_info *btcoex;
 	struct cfg80211_scan_request *scan_request;
 	struct mutex usr_sync;
-	struct brcmf_scan_results *bss_list;
 	struct brcmf_cfg80211_scan_req scan_req_int;
 	struct wl_cfg80211_bss_info *bss_info;
 	struct brcmf_cfg80211_ie ie;
@@ -397,6 +398,8 @@ struct brcmf_cfg80211_info {
 	struct brcmf_cfg80211_vif_event vif_event;
 	struct completion vif_disabled;
 	struct brcmu_d11inf d11inf;
+	bool wowl_enabled;
+	u32 pre_wowl_pmmode;
 };
 
 /**
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/dma.c b/drivers/net/wireless/brcm80211/brcmsmac/dma.c
index 4fb9635d3919..796f5f9d5d5a 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/dma.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/dma.c
@@ -746,7 +746,7 @@ dma64_dd_upd(struct dma_info *di, struct dma64desc *ddring,
 /* !! may be called with core in reset */
 void dma_detach(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 
 	brcms_dbg_dma(di->core, "%s:\n", di->name);
 
@@ -842,7 +842,7 @@ static void _dma_rxenable(struct dma_info *di)
 
 void dma_rxinit(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 
 	brcms_dbg_dma(di->core, "%s:\n", di->name);
 
@@ -924,7 +924,7 @@ static struct sk_buff *_dma_getnextrxp(struct dma_info *di, bool forceall)
  */
 int dma_rx(struct dma_pub *pub, struct sk_buff_head *skb_list)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	struct sk_buff_head dma_frames;
 	struct sk_buff *p, *next;
 	uint len;
@@ -1022,7 +1022,7 @@ static bool dma64_txidle(struct dma_info *di)
  */
 bool dma_rxfill(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	struct sk_buff *p;
 	u16 rxin, rxout;
 	u32 flags = 0;
@@ -1106,7 +1106,7 @@ bool dma_rxfill(struct dma_pub *pub)
 
 void dma_rxreclaim(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	struct sk_buff *p;
 
 	brcms_dbg_dma(di->core, "%s:\n", di->name);
@@ -1126,7 +1126,7 @@ void dma_counterreset(struct dma_pub *pub)
 /* get the address of the var in order to change later */
 unsigned long dma_getvar(struct dma_pub *pub, const char *name)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 
 	if (!strcmp(name, "&txavail"))
 		return (unsigned long)&(di->dma.txavail);
@@ -1137,7 +1137,7 @@ unsigned long dma_getvar(struct dma_pub *pub, const char *name)
 
 void dma_txinit(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	u32 control = D64_XC_XE;
 
 	brcms_dbg_dma(di->core, "%s:\n", di->name);
@@ -1170,7 +1170,7 @@ void dma_txinit(struct dma_pub *pub)
 
 void dma_txsuspend(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 
 	brcms_dbg_dma(di->core, "%s:\n", di->name);
 
@@ -1182,7 +1182,7 @@ void dma_txsuspend(struct dma_pub *pub)
 
 void dma_txresume(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 
 	brcms_dbg_dma(di->core, "%s:\n", di->name);
 
@@ -1194,7 +1194,7 @@ void dma_txresume(struct dma_pub *pub)
 
 bool dma_txsuspended(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 
 	return (di->ntxd == 0) ||
 	       ((bcma_read32(di->core,
@@ -1204,7 +1204,7 @@ bool dma_txsuspended(struct dma_pub *pub)
 
 void dma_txreclaim(struct dma_pub *pub, enum txd_range range)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	struct sk_buff *p;
 
 	brcms_dbg_dma(di->core, "%s: %s\n",
@@ -1225,7 +1225,7 @@ void dma_txreclaim(struct dma_pub *pub, enum txd_range range)
 
 bool dma_txreset(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	u32 status;
 
 	if (di->ntxd == 0)
@@ -1252,7 +1252,7 @@ bool dma_txreset(struct dma_pub *pub)
 
 bool dma_rxreset(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	u32 status;
 
 	if (di->nrxd == 0)
@@ -1377,7 +1377,7 @@ static void dma_update_txavail(struct dma_info *di)
 int dma_txfast(struct brcms_c_info *wlc, struct dma_pub *pub,
 	       struct sk_buff *p)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	struct brcms_ampdu_session *session = &di->ampdu_session;
 	struct ieee80211_tx_info *tx_info;
 	bool is_ampdu;
@@ -1427,7 +1427,7 @@ int dma_txfast(struct brcms_c_info *wlc, struct dma_pub *pub,
 
 void dma_txflush(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	struct brcms_ampdu_session *session = &di->ampdu_session;
 
 	if (!skb_queue_empty(&session->skb_list))
@@ -1436,7 +1436,7 @@ void dma_txflush(struct dma_pub *pub)
 
 int dma_txpending(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	return ntxdactive(di, di->txin, di->txout);
 }
 
@@ -1446,7 +1446,7 @@ int dma_txpending(struct dma_pub *pub)
  */
 void dma_kick_tx(struct dma_pub *pub)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	struct brcms_ampdu_session *session = &di->ampdu_session;
 
 	if (!skb_queue_empty(&session->skb_list) && dma64_txidle(di))
@@ -1465,7 +1465,7 @@ void dma_kick_tx(struct dma_pub *pub)
  */
 struct sk_buff *dma_getnexttxp(struct dma_pub *pub, enum txd_range range)
 {
-	struct dma_info *di = (struct dma_info *)pub;
+	struct dma_info *di = container_of(pub, struct dma_info, dma);
 	u16 start, end, i;
 	u16 active_desc;
 	struct sk_buff *txp;
@@ -1547,7 +1547,7 @@ struct sk_buff *dma_getnexttxp(struct dma_pub *pub, enum txd_range range)
 void dma_walk_packets(struct dma_pub *dmah, void (*callback_fnc)
 		      (void *pkt, void *arg_a), void *arg_a)
 {
-	struct dma_info *di = (struct dma_info *) dmah;
+	struct dma_info *di = container_of(dmah, struct dma_info, dma);
 	uint i =   di->txin;
 	uint end = di->txout;
 	struct sk_buff *skb;
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
index 57ecc05802e9..941b1e41f366 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_cmn.c
@@ -128,19 +128,19 @@ static const u8 ofdm_rate_lookup[] = {
 
 void wlc_phyreg_enter(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	wlapi_bmac_ucode_wake_override_phyreg_set(pi->sh->physhim);
 }
 
 void wlc_phyreg_exit(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	wlapi_bmac_ucode_wake_override_phyreg_clear(pi->sh->physhim);
 }
 
 void wlc_radioreg_enter(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	wlapi_bmac_mctrl(pi->sh->physhim, MCTL_LOCK_RADIO, MCTL_LOCK_RADIO);
 
 	udelay(10);
@@ -148,7 +148,7 @@ void wlc_radioreg_enter(struct brcms_phy_pub *pih)
 
 void wlc_radioreg_exit(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	(void)bcma_read16(pi->d11core, D11REGOFFS(phyversion));
 	pi->phy_wreg = 0;
@@ -586,7 +586,7 @@ err:
 
 void wlc_phy_detach(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (pih) {
 		if (--pi->refcnt)
@@ -613,7 +613,7 @@ bool
 wlc_phy_get_phyversion(struct brcms_phy_pub *pih, u16 *phytype, u16 *phyrev,
 		       u16 *radioid, u16 *radiover)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	*phytype = (u16) pi->pubpi.phy_type;
 	*phyrev = (u16) pi->pubpi.phy_rev;
 	*radioid = pi->pubpi.radioid;
@@ -624,19 +624,19 @@ wlc_phy_get_phyversion(struct brcms_phy_pub *pih, u16 *phytype, u16 *phyrev,
 
 bool wlc_phy_get_encore(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	return pi->pubpi.abgphy_encore;
 }
 
 u32 wlc_phy_get_coreflags(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	return pi->pubpi.coreflags;
 }
 
 void wlc_phy_anacore(struct brcms_phy_pub *pih, bool on)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (ISNPHY(pi)) {
 		if (on) {
@@ -673,7 +673,7 @@ void wlc_phy_anacore(struct brcms_phy_pub *pih, bool on)
 
 u32 wlc_phy_clk_bwbits(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	u32 phy_bw_clkbits = 0;
 
@@ -698,14 +698,14 @@ u32 wlc_phy_clk_bwbits(struct brcms_phy_pub *pih)
 
 void wlc_phy_por_inform(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	pi->phy_init_por = true;
 }
 
 void wlc_phy_edcrs_lock(struct brcms_phy_pub *pih, bool lock)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	pi->edcrs_threshold_lock = lock;
 
@@ -717,14 +717,14 @@ void wlc_phy_edcrs_lock(struct brcms_phy_pub *pih, bool lock)
 
 void wlc_phy_initcal_enable(struct brcms_phy_pub *pih, bool initcal)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	pi->do_initcal = initcal;
 }
 
 void wlc_phy_hw_clk_state_upd(struct brcms_phy_pub *pih, bool newstate)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (!pi || !pi->sh)
 		return;
@@ -734,7 +734,7 @@ void wlc_phy_hw_clk_state_upd(struct brcms_phy_pub *pih, bool newstate)
 
 void wlc_phy_hw_state_upd(struct brcms_phy_pub *pih, bool newstate)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (!pi || !pi->sh)
 		return;
@@ -746,7 +746,7 @@ void wlc_phy_init(struct brcms_phy_pub *pih, u16 chanspec)
 {
 	u32 mc;
 	void (*phy_init)(struct brcms_phy *) = NULL;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (pi->init_in_progress)
 		return;
@@ -798,7 +798,7 @@ void wlc_phy_init(struct brcms_phy_pub *pih, u16 chanspec)
 
 void wlc_phy_cal_init(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	void (*cal_init)(struct brcms_phy *) = NULL;
 
 	if (WARN((bcma_read32(pi->d11core, D11REGOFFS(maccontrol)) &
@@ -816,7 +816,7 @@ void wlc_phy_cal_init(struct brcms_phy_pub *pih)
 
 int wlc_phy_down(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	int callbacks = 0;
 
 	if (pi->phycal_timer
@@ -1070,7 +1070,7 @@ void wlc_phy_do_dummy_tx(struct brcms_phy *pi, bool ofdm, bool pa_on)
 
 void wlc_phy_hold_upd(struct brcms_phy_pub *pih, u32 id, bool set)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (set)
 		mboolset(pi->measure_hold, id);
@@ -1082,7 +1082,7 @@ void wlc_phy_hold_upd(struct brcms_phy_pub *pih, u32 id, bool set)
 
 void wlc_phy_mute_upd(struct brcms_phy_pub *pih, bool mute, u32 flags)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (mute)
 		mboolset(pi->measure_hold, PHY_HOLD_FOR_MUTE);
@@ -1096,7 +1096,7 @@ void wlc_phy_mute_upd(struct brcms_phy_pub *pih, bool mute, u32 flags)
 
 void wlc_phy_clear_tssi(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (ISNPHY(pi)) {
 		return;
@@ -1115,7 +1115,7 @@ static bool wlc_phy_cal_txpower_recalc_sw(struct brcms_phy *pi)
 
 void wlc_phy_switch_radio(struct brcms_phy_pub *pih, bool on)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	(void)bcma_read32(pi->d11core, D11REGOFFS(maccontrol));
 
 	if (ISNPHY(pi)) {
@@ -1149,35 +1149,35 @@ void wlc_phy_switch_radio(struct brcms_phy_pub *pih, bool on)
 
 u16 wlc_phy_bw_state_get(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	return pi->bw;
 }
 
 void wlc_phy_bw_state_set(struct brcms_phy_pub *ppi, u16 bw)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	pi->bw = bw;
 }
 
 void wlc_phy_chanspec_radio_set(struct brcms_phy_pub *ppi, u16 newch)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	pi->radio_chanspec = newch;
 
 }
 
 u16 wlc_phy_chanspec_get(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	return pi->radio_chanspec;
 }
 
 void wlc_phy_chanspec_set(struct brcms_phy_pub *ppi, u16 chanspec)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	u16 m_cur_channel;
 	void (*chanspec_set)(struct brcms_phy *, u16) = NULL;
 	m_cur_channel = CHSPEC_CHANNEL(chanspec);
@@ -1226,7 +1226,7 @@ int wlc_phy_chanspec_bandrange_get(struct brcms_phy *pi, u16 chanspec)
 void wlc_phy_chanspec_ch14_widefilter_set(struct brcms_phy_pub *ppi,
 					  bool wide_filter)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	pi->channel_14_wide_filter = wide_filter;
 
@@ -1246,7 +1246,7 @@ void
 wlc_phy_chanspec_band_validch(struct brcms_phy_pub *ppi, uint band,
 			      struct brcms_chanvec *channels)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	uint i;
 	uint channel;
 
@@ -1267,7 +1267,7 @@ wlc_phy_chanspec_band_validch(struct brcms_phy_pub *ppi, uint band,
 
 u16 wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi, uint band)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	uint i;
 	uint channel;
 	u16 chspec;
@@ -1311,7 +1311,7 @@ u16 wlc_phy_chanspec_band_firstch(struct brcms_phy_pub *ppi, uint band)
 
 int wlc_phy_txpower_get(struct brcms_phy_pub *ppi, uint *qdbm, bool *override)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	*qdbm = pi->tx_user_target[0];
 	if (override != NULL)
@@ -1323,7 +1323,7 @@ void wlc_phy_txpower_target_set(struct brcms_phy_pub *ppi,
 				struct txpwr_limits *txpwr)
 {
 	bool mac_enabled = false;
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	memcpy(&pi->tx_user_target[TXP_FIRST_CCK],
 	       &txpwr->cck[0], BRCMS_NUM_RATES_CCK);
@@ -1371,7 +1371,7 @@ void wlc_phy_txpower_target_set(struct brcms_phy_pub *ppi,
 
 int wlc_phy_txpower_set(struct brcms_phy_pub *ppi, uint qdbm, bool override)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	int i;
 
 	if (qdbm > 127)
@@ -1407,7 +1407,7 @@ void
 wlc_phy_txpower_sromlimit(struct brcms_phy_pub *ppi, uint channel, u8 *min_pwr,
 			  u8 *max_pwr, int txp_rate_idx)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	uint i;
 
 	*min_pwr = pi->min_txpower * BRCMS_TXPWR_DB_FACTOR;
@@ -1456,7 +1456,7 @@ void
 wlc_phy_txpower_sromlimit_max_get(struct brcms_phy_pub *ppi, uint chan,
 				  u8 *max_txpwr, u8 *min_txpwr)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	u8 tx_pwr_max = 0;
 	u8 tx_pwr_min = 255;
 	u8 max_num_rate;
@@ -1493,14 +1493,14 @@ wlc_phy_txpower_boardlimit_band(struct brcms_phy_pub *ppi, uint bandunit,
 
 u8 wlc_phy_txpower_get_target_min(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	return pi->tx_power_min;
 }
 
 u8 wlc_phy_txpower_get_target_max(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	return pi->tx_power_max;
 }
@@ -1812,21 +1812,21 @@ wlc_phy_txpower_reg_limit_calc(struct brcms_phy *pi, struct txpwr_limits *txpwr,
 
 void wlc_phy_txpwr_percent_set(struct brcms_phy_pub *ppi, u8 txpwr_percent)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	pi->txpwr_percent = txpwr_percent;
 }
 
 void wlc_phy_machwcap_set(struct brcms_phy_pub *ppi, u32 machwcap)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	pi->sh->machwcap = machwcap;
 }
 
 void wlc_phy_runbist_config(struct brcms_phy_pub *ppi, bool start_end)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	u16 rxc;
 	rxc = 0;
 
@@ -1857,7 +1857,7 @@ void
 wlc_phy_txpower_limit_set(struct brcms_phy_pub *ppi, struct txpwr_limits *txpwr,
 			  u16 chanspec)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	wlc_phy_txpower_reg_limit_calc(pi, txpwr, chanspec);
 
@@ -1881,14 +1881,14 @@ wlc_phy_txpower_limit_set(struct brcms_phy_pub *ppi, struct txpwr_limits *txpwr,
 
 void wlc_phy_ofdm_rateset_war(struct brcms_phy_pub *pih, bool war)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	pi->ofdm_rateset_war = war;
 }
 
 void wlc_phy_bf_preempt_enable(struct brcms_phy_pub *pih, bool bf_preempt)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	pi->bf_preempt_4306 = bf_preempt;
 }
@@ -1945,7 +1945,7 @@ void wlc_phy_txpower_update_shm(struct brcms_phy *pi)
 
 bool wlc_phy_txpower_hw_ctrl_get(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	if (ISNPHY(pi))
 		return pi->nphy_txpwrctrl;
@@ -1955,7 +1955,7 @@ bool wlc_phy_txpower_hw_ctrl_get(struct brcms_phy_pub *ppi)
 
 void wlc_phy_txpower_hw_ctrl_set(struct brcms_phy_pub *ppi, bool hwpwrctrl)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	bool suspend;
 
 	if (!pi->hwpwrctrl_capable)
@@ -2038,7 +2038,7 @@ void
 wlc_phy_txpower_get_current(struct brcms_phy_pub *ppi, struct tx_power *power,
 			    uint channel)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	uint rate, num_rates;
 	u8 min_pwr, max_pwr;
 
@@ -2136,21 +2136,21 @@ wlc_phy_txpower_get_current(struct brcms_phy_pub *ppi, struct tx_power *power,
 
 void wlc_phy_antsel_type_set(struct brcms_phy_pub *ppi, u8 antsel_type)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	pi->antsel_type = antsel_type;
 }
 
 bool wlc_phy_test_ison(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	return pi->phytest_on;
 }
 
 void wlc_phy_ant_rxdiv_set(struct brcms_phy_pub *ppi, u8 val)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	bool suspend;
 
 	pi->sh->rx_antdiv = val;
@@ -2283,7 +2283,7 @@ static s8 wlc_phy_noise_read_shmem(struct brcms_phy *pi)
 
 void wlc_phy_noise_sample_intr(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	u16 jssi_aux;
 	u8 channel = 0;
 	s8 noise_dbm = PHY_NOISE_FIXED_VAL_NPHY;
@@ -2339,7 +2339,7 @@ void wlc_phy_noise_sample_intr(struct brcms_phy_pub *pih)
 static void
 wlc_phy_noise_sample_request(struct brcms_phy_pub *pih, u8 reason, u8 ch)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	s8 noise_dbm = PHY_NOISE_FIXED_VAL_NPHY;
 	bool sampling_in_progress = (pi->phynoise_state != 0);
 	bool wait_for_intr = true;
@@ -2531,7 +2531,7 @@ int wlc_phy_rssi_compute(struct brcms_phy_pub *pih,
 {
 	int rssi = rxh->PhyRxStatus_1 & PRXS1_JSSI_MASK;
 	uint radioid = pih->radioid;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if ((pi->sh->corerev >= 11)
 	    && !(rxh->RxStatus2 & RXS_PHYRXST_VALID)) {
@@ -2591,7 +2591,7 @@ void wlc_phy_set_deaf(struct brcms_phy_pub *ppi, bool user_flag)
 
 void wlc_phy_watchdog(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	bool delay_phy_cal = false;
 	pi->sh->now++;
 
@@ -2651,7 +2651,7 @@ void wlc_phy_watchdog(struct brcms_phy_pub *pih)
 
 void wlc_phy_BSSinit(struct brcms_phy_pub *pih, bool bonlyap, int rssi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	uint i;
 	uint k;
 
@@ -2711,7 +2711,7 @@ void wlc_phy_cal_perical(struct brcms_phy_pub *pih, u8 reason)
 	s16 nphy_currtemp = 0;
 	s16 delta_temp = 0;
 	bool do_periodic_cal = true;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	if (!ISNPHY(pi))
 		return;
@@ -2804,7 +2804,7 @@ u8 wlc_phy_nbits(s32 value)
 
 void wlc_phy_stf_chain_init(struct brcms_phy_pub *pih, u8 txchain, u8 rxchain)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	pi->sh->hw_phytxchain = txchain;
 	pi->sh->hw_phyrxchain = rxchain;
@@ -2815,7 +2815,7 @@ void wlc_phy_stf_chain_init(struct brcms_phy_pub *pih, u8 txchain, u8 rxchain)
 
 void wlc_phy_stf_chain_set(struct brcms_phy_pub *pih, u8 txchain, u8 rxchain)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	pi->sh->phytxchain = txchain;
 
@@ -2827,7 +2827,7 @@ void wlc_phy_stf_chain_set(struct brcms_phy_pub *pih, u8 txchain, u8 rxchain)
 
 void wlc_phy_stf_chain_get(struct brcms_phy_pub *pih, u8 *txchain, u8 *rxchain)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	*txchain = pi->sh->phytxchain;
 	*rxchain = pi->sh->phyrxchain;
@@ -2837,7 +2837,7 @@ u8 wlc_phy_stf_chain_active_get(struct brcms_phy_pub *pih)
 {
 	s16 nphy_currtemp;
 	u8 active_bitmap;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	active_bitmap = (pi->phy_txcore_heatedup) ? 0x31 : 0x33;
 
@@ -2867,7 +2867,7 @@ u8 wlc_phy_stf_chain_active_get(struct brcms_phy_pub *pih)
 
 s8 wlc_phy_stf_ssmode_get(struct brcms_phy_pub *pih, u16 chanspec)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	u8 siso_mcs_id, cdd_mcs_id;
 
 	siso_mcs_id =
@@ -2944,7 +2944,7 @@ s8 wlc_phy_upd_rssi_offset(struct brcms_phy *pi, s8 rssi, u16 chanspec)
 
 bool wlc_phy_txpower_ipa_ison(struct brcms_phy_pub *ppi)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	if (ISNPHY(pi))
 		return wlc_phy_n_txpower_ipa_ison(pi);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
index b2d6d6da3daf..5f1366234a0d 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_lcn.c
@@ -2865,7 +2865,7 @@ static void wlc_lcnphy_idle_tssi_est(struct brcms_phy_pub *ppi)
 {
 	bool suspend, tx_gain_override_old;
 	struct lcnphy_txgains old_gains;
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	u16 idleTssi, idleTssi0_2C, idleTssi0_OB, idleTssi0_regvalue_OB,
 	    idleTssi0_regvalue_2C;
 	u16 SAVE_txpwrctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
@@ -3084,7 +3084,7 @@ static void wlc_lcnphy_tx_pwr_ctrl_init(struct brcms_phy_pub *ppi)
 	s32 a1, b0, b1;
 	s32 tssi, pwr, maxtargetpwr, mintargetpwr;
 	bool suspend;
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 
 	suspend = (0 == (bcma_read32(pi->d11core, D11REGOFFS(maccontrol)) &
 			 MCTL_EN_MAC));
@@ -4348,7 +4348,7 @@ void wlc_lcnphy_tx_power_adjustment(struct brcms_phy_pub *ppi)
 {
 	s8 index;
 	u16 index2;
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	struct brcms_phy_lcnphy *pi_lcn = pi->u.pi_lcnphy;
 	u16 SAVE_txpwrctrl = wlc_lcnphy_get_tx_pwr_ctrl(pi);
 	if (wlc_lcnphy_tempsense_based_pwr_ctrl_enabled(pi) &&
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
index 93869e89aa3d..084f18f4f950 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/phy/phy_n.c
@@ -14121,7 +14121,7 @@ static u8 ant_sw_ctrl_tbl_rev8_2057v7_core1[] = {
 
 bool wlc_phy_bist_check_phy(struct brcms_phy_pub *pih)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	u32 phybist0, phybist1, phybist2, phybist3, phybist4;
 
 	if (NREV_GE(pi->pubpi.phy_rev, 16))
@@ -19734,7 +19734,7 @@ void wlc_phy_rxcore_setstate_nphy(struct brcms_phy_pub *pih, u8 rxcore_bitmask)
 	u16 regval;
 	u16 tbl_buf[16];
 	uint i;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 	u16 tbl_opcode;
 	bool suspend;
 
@@ -19812,7 +19812,7 @@ void wlc_phy_rxcore_setstate_nphy(struct brcms_phy_pub *pih, u8 rxcore_bitmask)
 u8 wlc_phy_rxcore_getstate_nphy(struct brcms_phy_pub *pih)
 {
 	u16 regval, rxen_bits;
-	struct brcms_phy *pi = (struct brcms_phy *) pih;
+	struct brcms_phy *pi = container_of(pih, struct brcms_phy, pubpi_ro);
 
 	regval = read_phy_reg(pi, 0xa2);
 	rxen_bits = (regval >> 4) & 0xf;
@@ -21342,7 +21342,7 @@ void wlc_phy_chanspec_set_nphy(struct brcms_phy *pi, u16 chanspec)
 
 void wlc_phy_antsel_init(struct brcms_phy_pub *ppi, bool lut_init)
 {
-	struct brcms_phy *pi = (struct brcms_phy *) ppi;
+	struct brcms_phy *pi = container_of(ppi, struct brcms_phy, pubpi_ro);
 	u16 mask = 0xfc00;
 	u32 mc = 0;
 
diff --git a/drivers/net/wireless/brcm80211/include/defs.h b/drivers/net/wireless/brcm80211/include/defs.h
index fb7cbcf81179..8d1e85e0ed51 100644
--- a/drivers/net/wireless/brcm80211/include/defs.h
+++ b/drivers/net/wireless/brcm80211/include/defs.h
@@ -74,10 +74,6 @@
 #define	BRCM_BAND_2G		2	/* 2.4 Ghz */
 #define	BRCM_BAND_ALL		3	/* all bands */
 
-/* Values for PM */
-#define PM_OFF	0
-#define PM_MAX	1
-
 /* Debug levels */
 #define BRCM_DL_INFO		0x00000001
 #define BRCM_DL_MAC80211	0x00000002
@@ -87,6 +83,7 @@
 #define BRCM_DL_DMA		0x00000020
 #define BRCM_DL_HT		0x00000040
 
+/* Values for PM */
 #define PM_OFF	0
 #define PM_MAX	1
 #define PM_FAST 2
diff --git a/drivers/net/wireless/cw1200/cw1200_spi.c b/drivers/net/wireless/cw1200/cw1200_spi.c
index 40078f5f932e..964b64ab7fe3 100644
--- a/drivers/net/wireless/cw1200/cw1200_spi.c
+++ b/drivers/net/wireless/cw1200/cw1200_spi.c
@@ -398,7 +398,7 @@ static int cw1200_spi_probe(struct spi_device *func)
 		return -1;
 	}
 
-	self = kzalloc(sizeof(*self), GFP_KERNEL);
+	self = devm_kzalloc(&func->dev, sizeof(*self), GFP_KERNEL);
 	if (!self) {
 		pr_err("Can't allocate SPI hwbus_priv.");
 		return -ENOMEM;
@@ -424,7 +424,6 @@ static int cw1200_spi_probe(struct spi_device *func)
 	if (status) {
 		cw1200_spi_irq_unsubscribe(self);
 		cw1200_spi_off(plat_data);
-		kfree(self);
 	}
 
 	return status;
@@ -441,7 +440,6 @@ static int cw1200_spi_disconnect(struct spi_device *func)
 			cw1200_core_release(self->core);
 			self->core = NULL;
 		}
-		kfree(self);
 	}
 	cw1200_spi_off(dev_get_platdata(&func->dev));
 
diff --git a/drivers/net/wireless/hostap/hostap_proc.c b/drivers/net/wireless/hostap/hostap_proc.c
index 4e5c0f8c9496..dd84557cf957 100644
--- a/drivers/net/wireless/hostap/hostap_proc.c
+++ b/drivers/net/wireless/hostap/hostap_proc.c
@@ -168,7 +168,6 @@ static int prism2_bss_list_proc_show(struct seq_file *m, void *v)
 	local_info_t *local = m->private;
 	struct list_head *ptr = v;
 	struct hostap_bss_info *bss;
-	int i;
 
 	if (ptr == &local->bss_list) {
 		seq_printf(m, "#BSSID\tlast_update\tcount\tcapab_info\tSSID(txt)\t"
@@ -181,16 +180,12 @@ static int prism2_bss_list_proc_show(struct seq_file *m, void *v)
 		   bss->bssid, bss->last_update,
 		   bss->count, bss->capab_info);
 
-	for (i = 0; i < bss->ssid_len; i++)
-		seq_putc(m,bss->ssid[i] >= 32 && bss->ssid[i] < 127 ?
-			   bss->ssid[i] : '_');
+	seq_printf(m, "%*pE", (int)bss->ssid_len, bss->ssid);
 
 	seq_putc(m, '\t');
-	for (i = 0; i < bss->ssid_len; i++)
-		seq_printf(m, "%02x", bss->ssid[i]);
+	seq_printf(m, "%*phN", (int)bss->ssid_len, bss->ssid);
 	seq_putc(m, '\t');
-	for (i = 0; i < bss->wpa_ie_len; i++)
-		seq_printf(m, "%02x", bss->wpa_ie[i]);
+	seq_printf(m, "%*phN", (int)bss->wpa_ie_len, bss->wpa_ie);
 	seq_putc(m, '\n');
 	return 0;
 }
diff --git a/drivers/net/wireless/ipw2x00/ipw2100.c b/drivers/net/wireless/ipw2x00/ipw2100.c
index c3d726f334e3..6fabea0309dd 100644
--- a/drivers/net/wireless/ipw2x00/ipw2100.c
+++ b/drivers/net/wireless/ipw2x00/ipw2100.c
@@ -2005,7 +2005,6 @@ static void isr_indicate_associated(struct ipw2100_priv *priv, u32 status)
 	u32 chan;
 	char *txratename;
 	u8 bssid[ETH_ALEN];
-	DECLARE_SSID_BUF(ssid);
 
 	/*
 	 * TBD: BSSID is usually 00:00:00:00:00:00 here and not
@@ -2067,8 +2066,8 @@ static void isr_indicate_associated(struct ipw2100_priv *priv, u32 status)
 		break;
 	}
 
-	IPW_DEBUG_INFO("%s: Associated with '%s' at %s, channel %d (BSSID=%pM)\n",
-		       priv->net_dev->name, print_ssid(ssid, essid, essid_len),
+	IPW_DEBUG_INFO("%s: Associated with '%*pE' at %s, channel %d (BSSID=%pM)\n",
+		       priv->net_dev->name, essid_len, essid,
 		       txratename, chan, bssid);
 
 	/* now we copy read ssid into dev */
@@ -2095,9 +2094,8 @@ static int ipw2100_set_essid(struct ipw2100_priv *priv, char *essid,
 		.host_command_length = ssid_len
 	};
 	int err;
-	DECLARE_SSID_BUF(ssid);
 
-	IPW_DEBUG_HC("SSID: '%s'\n", print_ssid(ssid, essid, ssid_len));
+	IPW_DEBUG_HC("SSID: '%*pE'\n", ssid_len, essid);
 
 	if (ssid_len)
 		memcpy(cmd.host_command_parameters, essid, ssid_len);
@@ -2138,11 +2136,8 @@ static int ipw2100_set_essid(struct ipw2100_priv *priv, char *essid,
 
 static void isr_indicate_association_lost(struct ipw2100_priv *priv, u32 status)
 {
-	DECLARE_SSID_BUF(ssid);
-
 	IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE | IPW_DL_ASSOC,
-		  "disassociated: '%s' %pM\n",
-		  print_ssid(ssid, priv->essid, priv->essid_len),
+		  "disassociated: '%*pE' %pM\n", priv->essid_len, priv->essid,
 		  priv->bssid);
 
 	priv->status &= ~(STATUS_ASSOCIATED | STATUS_ASSOCIATING);
@@ -6975,7 +6970,6 @@ static int ipw2100_wx_set_essid(struct net_device *dev,
 	char *essid = "";	/* ANY */
 	int length = 0;
 	int err = 0;
-	DECLARE_SSID_BUF(ssid);
 
 	mutex_lock(&priv->action_mutex);
 	if (!(priv->status & STATUS_INITIALIZED)) {
@@ -7005,8 +6999,7 @@ static int ipw2100_wx_set_essid(struct net_device *dev,
 		goto done;
 	}
 
-	IPW_DEBUG_WX("Setting ESSID: '%s' (%d)\n",
-		     print_ssid(ssid, essid, length), length);
+	IPW_DEBUG_WX("Setting ESSID: '%*pE' (%d)\n", length, essid, length);
 
 	priv->essid_len = length;
 	memcpy(priv->essid, essid, priv->essid_len);
@@ -7027,13 +7020,12 @@ static int ipw2100_wx_get_essid(struct net_device *dev,
 	 */
 
 	struct ipw2100_priv *priv = libipw_priv(dev);
-	DECLARE_SSID_BUF(ssid);
 
 	/* If we are associated, trying to associate, or have a statically
 	 * configured ESSID then return that; otherwise return ANY */
 	if (priv->config & CFG_STATIC_ESSID || priv->status & STATUS_ASSOCIATED) {
-		IPW_DEBUG_WX("Getting essid: '%s'\n",
-			     print_ssid(ssid, priv->essid, priv->essid_len));
+		IPW_DEBUG_WX("Getting essid: '%*pE'\n",
+			     priv->essid_len, priv->essid);
 		memcpy(extra, priv->essid, priv->essid_len);
 		wrqu->essid.length = priv->essid_len;
 		wrqu->essid.flags = 1;	/* active */
diff --git a/drivers/net/wireless/ipw2x00/ipw2200.c b/drivers/net/wireless/ipw2x00/ipw2200.c
index a42f9c335090..edc344334a75 100644
--- a/drivers/net/wireless/ipw2x00/ipw2200.c
+++ b/drivers/net/wireless/ipw2x00/ipw2200.c
@@ -4496,7 +4496,6 @@ static void handle_scan_event(struct ipw_priv *priv)
 static void ipw_rx_notification(struct ipw_priv *priv,
 				       struct ipw_rx_notification *notif)
 {
-	DECLARE_SSID_BUF(ssid);
 	u16 size = le16_to_cpu(notif->size);
 
 	IPW_DEBUG_NOTIF("type = %i (%d bytes)\n", notif->subtype, size);
@@ -4509,9 +4508,8 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 			case CMAS_ASSOCIATED:{
 					IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 						  IPW_DL_ASSOC,
-						  "associated: '%s' %pM\n",
-						  print_ssid(ssid, priv->essid,
-							     priv->essid_len),
+						  "associated: '%*pE' %pM\n",
+						  priv->essid_len, priv->essid,
 						  priv->bssid);
 
 					switch (priv->ieee->iw_mode) {
@@ -4585,14 +4583,9 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 						IPW_DEBUG(IPW_DL_NOTIF |
 							  IPW_DL_STATE |
 							  IPW_DL_ASSOC,
-							  "deauthenticated: '%s' "
-							  "%pM"
-							  ": (0x%04X) - %s\n",
-							  print_ssid(ssid,
-								     priv->
-								     essid,
-								     priv->
-								     essid_len),
+							  "deauthenticated: '%*pE' %pM: (0x%04X) - %s\n",
+							  priv->essid_len,
+							  priv->essid,
 							  priv->bssid,
 							  le16_to_cpu(auth->status),
 							  ipw_get_status_code
@@ -4610,9 +4603,8 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 
 					IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 						  IPW_DL_ASSOC,
-						  "authenticated: '%s' %pM\n",
-						  print_ssid(ssid, priv->essid,
-							     priv->essid_len),
+						  "authenticated: '%*pE' %pM\n",
+						  priv->essid_len, priv->essid,
 						  priv->bssid);
 					break;
 				}
@@ -4638,9 +4630,8 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 
 					IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 						  IPW_DL_ASSOC,
-						  "disassociated: '%s' %pM\n",
-						  print_ssid(ssid, priv->essid,
-							     priv->essid_len),
+						  "disassociated: '%*pE' %pM\n",
+						  priv->essid_len, priv->essid,
 						  priv->bssid);
 
 					priv->status &=
@@ -4676,9 +4667,8 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 			switch (auth->state) {
 			case CMAS_AUTHENTICATED:
 				IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE,
-					  "authenticated: '%s' %pM\n",
-					  print_ssid(ssid, priv->essid,
-						     priv->essid_len),
+					  "authenticated: '%*pE' %pM\n",
+					  priv->essid_len, priv->essid,
 					  priv->bssid);
 				priv->status |= STATUS_AUTH;
 				break;
@@ -4695,9 +4685,8 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 				}
 				IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 					  IPW_DL_ASSOC,
-					  "deauthenticated: '%s' %pM\n",
-					  print_ssid(ssid, priv->essid,
-						     priv->essid_len),
+					  "deauthenticated: '%*pE' %pM\n",
+					  priv->essid_len, priv->essid,
 					  priv->bssid);
 
 				priv->status &= ~(STATUS_ASSOCIATING |
@@ -5516,16 +5505,13 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 				  int roaming)
 {
 	struct ipw_supported_rates rates;
-	DECLARE_SSID_BUF(ssid);
 
 	/* Verify that this network's capability is compatible with the
 	 * current mode (AdHoc or Infrastructure) */
 	if ((priv->ieee->iw_mode == IW_MODE_ADHOC &&
 	     !(network->capability & WLAN_CAPABILITY_IBSS))) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded due to "
-				"capability mismatch.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded due to capability mismatch.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
@@ -5536,10 +5522,8 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 		if ((network->ssid_len != match->network->ssid_len) ||
 		    memcmp(network->ssid, match->network->ssid,
 			   network->ssid_len)) {
-			IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-					"because of non-network ESSID.\n",
-					print_ssid(ssid, network->ssid,
-						   network->ssid_len),
+			IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of non-network ESSID.\n",
+					network->ssid_len, network->ssid,
 					network->bssid);
 			return 0;
 		}
@@ -5550,17 +5534,10 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 		    ((network->ssid_len != priv->essid_len) ||
 		     memcmp(network->ssid, priv->essid,
 			    min(network->ssid_len, priv->essid_len)))) {
-			char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
-
-			strncpy(escaped,
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
-				sizeof(escaped));
-			IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-					"because of ESSID mismatch: '%s'.\n",
-					escaped, network->bssid,
-					print_ssid(ssid, priv->essid,
-						   priv->essid_len));
+			IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of ESSID mismatch: '%*pE'.\n",
+					network->ssid_len, network->ssid,
+					network->bssid, priv->essid_len,
+					priv->essid);
 			return 0;
 		}
 	}
@@ -5569,26 +5546,20 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	 * testing everything else. */
 
 	if (network->time_stamp[0] < match->network->time_stamp[0]) {
-		IPW_DEBUG_MERGE("Network '%s excluded because newer than "
-				"current network.\n",
-				print_ssid(ssid, match->network->ssid,
-					   match->network->ssid_len));
+		IPW_DEBUG_MERGE("Network '%*pE excluded because newer than current network.\n",
+				match->network->ssid_len, match->network->ssid);
 		return 0;
 	} else if (network->time_stamp[1] < match->network->time_stamp[1]) {
-		IPW_DEBUG_MERGE("Network '%s excluded because newer than "
-				"current network.\n",
-				print_ssid(ssid, match->network->ssid,
-					   match->network->ssid_len));
+		IPW_DEBUG_MERGE("Network '%*pE excluded because newer than current network.\n",
+				match->network->ssid_len, match->network->ssid);
 		return 0;
 	}
 
 	/* Now go through and see if the requested network is valid... */
 	if (priv->ieee->scan_age != 0 &&
 	    time_after(jiffies, network->last_scanned + priv->ieee->scan_age)) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-				"because of age: %ums.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of age: %ums.\n",
+				network->ssid_len, network->ssid,
 				network->bssid,
 				jiffies_to_msecs(jiffies -
 						 network->last_scanned));
@@ -5597,10 +5568,8 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 
 	if ((priv->config & CFG_STATIC_CHANNEL) &&
 	    (network->channel != priv->channel)) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-				"because of channel mismatch: %d != %d.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of channel mismatch: %d != %d.\n",
+				network->ssid_len, network->ssid,
 				network->bssid,
 				network->channel, priv->channel);
 		return 0;
@@ -5609,10 +5578,8 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	/* Verify privacy compatibility */
 	if (((priv->capability & CAP_PRIVACY_ON) ? 1 : 0) !=
 	    ((network->capability & WLAN_CAPABILITY_PRIVACY) ? 1 : 0)) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-				"because of privacy mismatch: %s != %s.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of privacy mismatch: %s != %s.\n",
+				network->ssid_len, network->ssid,
 				network->bssid,
 				priv->
 				capability & CAP_PRIVACY_ON ? "on" : "off",
@@ -5623,22 +5590,16 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	}
 
 	if (ether_addr_equal(network->bssid, priv->bssid)) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-				"because of the same BSSID match: %pM"
-				".\n", print_ssid(ssid, network->ssid,
-						  network->ssid_len),
-				network->bssid,
-				priv->bssid);
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of the same BSSID match: %pM.\n",
+				network->ssid_len, network->ssid,
+				network->bssid, priv->bssid);
 		return 0;
 	}
 
 	/* Filter out any incompatible freq / mode combinations */
 	if (!libipw_is_valid_mode(priv->ieee, network->mode)) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-				"because of invalid frequency/mode "
-				"combination.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of invalid frequency/mode combination.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
@@ -5646,20 +5607,15 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	/* Ensure that the rates supported by the driver are compatible with
 	 * this AP, including verification of basic rates (mandatory) */
 	if (!ipw_compatible_rates(priv, network, &rates)) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-				"because configured rate mask excludes "
-				"AP mandatory rate.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because configured rate mask excludes AP mandatory rate.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
 
 	if (rates.num_rates == 0) {
-		IPW_DEBUG_MERGE("Network '%s (%pM)' excluded "
-				"because of no compatible rates.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_MERGE("Network '%*pE (%pM)' excluded because of no compatible rates.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
@@ -5671,16 +5627,14 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	/* Set up 'new' AP to this network */
 	ipw_copy_rates(&match->rates, &rates);
 	match->network = network;
-	IPW_DEBUG_MERGE("Network '%s (%pM)' is a viable match.\n",
-			print_ssid(ssid, network->ssid, network->ssid_len),
-			network->bssid);
+	IPW_DEBUG_MERGE("Network '%*pE (%pM)' is a viable match.\n",
+			network->ssid_len, network->ssid, network->bssid);
 
 	return 1;
 }
 
 static void ipw_merge_adhoc_network(struct work_struct *work)
 {
-	DECLARE_SSID_BUF(ssid);
 	struct ipw_priv *priv =
 		container_of(work, struct ipw_priv, merge_networks);
 	struct libipw_network *network = NULL;
@@ -5710,9 +5664,8 @@ static void ipw_merge_adhoc_network(struct work_struct *work)
 
 		mutex_lock(&priv->mutex);
 		if ((priv->ieee->iw_mode == IW_MODE_ADHOC)) {
-			IPW_DEBUG_MERGE("remove network %s\n",
-					print_ssid(ssid, priv->essid,
-						   priv->essid_len));
+			IPW_DEBUG_MERGE("remove network %*pE\n",
+					priv->essid_len, priv->essid);
 			ipw_remove_current_network(priv);
 		}
 
@@ -5728,7 +5681,6 @@ static int ipw_best_network(struct ipw_priv *priv,
 			    struct libipw_network *network, int roaming)
 {
 	struct ipw_supported_rates rates;
-	DECLARE_SSID_BUF(ssid);
 
 	/* Verify that this network's capability is compatible with the
 	 * current mode (AdHoc or Infrastructure) */
@@ -5736,10 +5688,8 @@ static int ipw_best_network(struct ipw_priv *priv,
 	     !(network->capability & WLAN_CAPABILITY_ESS)) ||
 	    (priv->ieee->iw_mode == IW_MODE_ADHOC &&
 	     !(network->capability & WLAN_CAPABILITY_IBSS))) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded due to "
-				"capability mismatch.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded due to capability mismatch.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
@@ -5750,10 +5700,8 @@ static int ipw_best_network(struct ipw_priv *priv,
 		if ((network->ssid_len != match->network->ssid_len) ||
 		    memcmp(network->ssid, match->network->ssid,
 			   network->ssid_len)) {
-			IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-					"because of non-network ESSID.\n",
-					print_ssid(ssid, network->ssid,
-						   network->ssid_len),
+			IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of non-network ESSID.\n",
+					network->ssid_len, network->ssid,
 					network->bssid);
 			return 0;
 		}
@@ -5764,16 +5712,10 @@ static int ipw_best_network(struct ipw_priv *priv,
 		    ((network->ssid_len != priv->essid_len) ||
 		     memcmp(network->ssid, priv->essid,
 			    min(network->ssid_len, priv->essid_len)))) {
-			char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
-			strncpy(escaped,
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
-				sizeof(escaped));
-			IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-					"because of ESSID mismatch: '%s'.\n",
-					escaped, network->bssid,
-					print_ssid(ssid, priv->essid,
-						   priv->essid_len));
+			IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of ESSID mismatch: '%*pE'.\n",
+					network->ssid_len, network->ssid,
+					network->bssid, priv->essid_len,
+					priv->essid);
 			return 0;
 		}
 	}
@@ -5781,16 +5723,10 @@ static int ipw_best_network(struct ipw_priv *priv,
 	/* If the old network rate is better than this one, don't bother
 	 * testing everything else. */
 	if (match->network && match->network->stats.rssi > network->stats.rssi) {
-		char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
-		strncpy(escaped,
-			print_ssid(ssid, network->ssid, network->ssid_len),
-			sizeof(escaped));
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded because "
-				"'%s (%pM)' has a stronger signal.\n",
-				escaped, network->bssid,
-				print_ssid(ssid, match->network->ssid,
-					   match->network->ssid_len),
-				match->network->bssid);
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because '%*pE (%pM)' has a stronger signal.\n",
+				network->ssid_len, network->ssid,
+				network->bssid, match->network->ssid_len,
+				match->network->ssid, match->network->bssid);
 		return 0;
 	}
 
@@ -5798,11 +5734,8 @@ static int ipw_best_network(struct ipw_priv *priv,
 	 * last 3 seconds, do not try and associate again... */
 	if (network->last_associate &&
 	    time_after(network->last_associate + (HZ * 3UL), jiffies)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of storming (%ums since last "
-				"assoc attempt).\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of storming (%ums since last assoc attempt).\n",
+				network->ssid_len, network->ssid,
 				network->bssid,
 				jiffies_to_msecs(jiffies -
 						 network->last_associate));
@@ -5812,10 +5745,8 @@ static int ipw_best_network(struct ipw_priv *priv,
 	/* Now go through and see if the requested network is valid... */
 	if (priv->ieee->scan_age != 0 &&
 	    time_after(jiffies, network->last_scanned + priv->ieee->scan_age)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of age: %ums.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of age: %ums.\n",
+				network->ssid_len, network->ssid,
 				network->bssid,
 				jiffies_to_msecs(jiffies -
 						 network->last_scanned));
@@ -5824,10 +5755,8 @@ static int ipw_best_network(struct ipw_priv *priv,
 
 	if ((priv->config & CFG_STATIC_CHANNEL) &&
 	    (network->channel != priv->channel)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of channel mismatch: %d != %d.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of channel mismatch: %d != %d.\n",
+				network->ssid_len, network->ssid,
 				network->bssid,
 				network->channel, priv->channel);
 		return 0;
@@ -5836,10 +5765,8 @@ static int ipw_best_network(struct ipw_priv *priv,
 	/* Verify privacy compatibility */
 	if (((priv->capability & CAP_PRIVACY_ON) ? 1 : 0) !=
 	    ((network->capability & WLAN_CAPABILITY_PRIVACY) ? 1 : 0)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of privacy mismatch: %s != %s.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of privacy mismatch: %s != %s.\n",
+				network->ssid_len, network->ssid,
 				network->bssid,
 				priv->capability & CAP_PRIVACY_ON ? "on" :
 				"off",
@@ -5850,31 +5777,24 @@ static int ipw_best_network(struct ipw_priv *priv,
 
 	if ((priv->config & CFG_STATIC_BSSID) &&
 	    !ether_addr_equal(network->bssid, priv->bssid)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of BSSID mismatch: %pM.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of BSSID mismatch: %pM.\n",
+				network->ssid_len, network->ssid,
 				network->bssid, priv->bssid);
 		return 0;
 	}
 
 	/* Filter out any incompatible freq / mode combinations */
 	if (!libipw_is_valid_mode(priv->ieee, network->mode)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of invalid frequency/mode "
-				"combination.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of invalid frequency/mode combination.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
 
 	/* Filter out invalid channel in current GEO */
 	if (!libipw_is_valid_channel(priv->ieee, network->channel)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of invalid channel in current GEO\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of invalid channel in current GEO\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
@@ -5882,20 +5802,15 @@ static int ipw_best_network(struct ipw_priv *priv,
 	/* Ensure that the rates supported by the driver are compatible with
 	 * this AP, including verification of basic rates (mandatory) */
 	if (!ipw_compatible_rates(priv, network, &rates)) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because configured rate mask excludes "
-				"AP mandatory rate.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because configured rate mask excludes AP mandatory rate.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
 
 	if (rates.num_rates == 0) {
-		IPW_DEBUG_ASSOC("Network '%s (%pM)' excluded "
-				"because of no compatible rates.\n",
-				print_ssid(ssid, network->ssid,
-					   network->ssid_len),
+		IPW_DEBUG_ASSOC("Network '%*pE (%pM)' excluded because of no compatible rates.\n",
+				network->ssid_len, network->ssid,
 				network->bssid);
 		return 0;
 	}
@@ -5908,9 +5823,8 @@ static int ipw_best_network(struct ipw_priv *priv,
 	ipw_copy_rates(&match->rates, &rates);
 	match->network = network;
 
-	IPW_DEBUG_ASSOC("Network '%s (%pM)' is a viable match.\n",
-			print_ssid(ssid, network->ssid, network->ssid_len),
-			network->bssid);
+	IPW_DEBUG_ASSOC("Network '%*pE (%pM)' is a viable match.\n",
+			network->ssid_len, network->ssid, network->bssid);
 
 	return 1;
 }
@@ -6152,7 +6066,6 @@ static void ipw_bg_adhoc_check(struct work_struct *work)
 
 static void ipw_debug_config(struct ipw_priv *priv)
 {
-	DECLARE_SSID_BUF(ssid);
 	IPW_DEBUG_INFO("Scan completed, no valid APs matched "
 		       "[CFG 0x%08X]\n", priv->config);
 	if (priv->config & CFG_STATIC_CHANNEL)
@@ -6160,8 +6073,8 @@ static void ipw_debug_config(struct ipw_priv *priv)
 	else
 		IPW_DEBUG_INFO("Channel unlocked.\n");
 	if (priv->config & CFG_STATIC_ESSID)
-		IPW_DEBUG_INFO("ESSID locked to '%s'\n",
-			       print_ssid(ssid, priv->essid, priv->essid_len));
+		IPW_DEBUG_INFO("ESSID locked to '%*pE'\n",
+			       priv->essid_len, priv->essid);
 	else
 		IPW_DEBUG_INFO("ESSID unlocked.\n");
 	if (priv->config & CFG_STATIC_BSSID)
@@ -7385,7 +7298,6 @@ static int ipw_associate_network(struct ipw_priv *priv,
 				 struct ipw_supported_rates *rates, int roaming)
 {
 	int err;
-	DECLARE_SSID_BUF(ssid);
 
 	if (priv->config & CFG_FIXED_RATE)
 		ipw_set_fixed_rate(priv, network->mode);
@@ -7451,10 +7363,9 @@ static int ipw_associate_network(struct ipw_priv *priv,
 		priv->assoc_request.capability &=
 		    ~cpu_to_le16(WLAN_CAPABILITY_SHORT_SLOT_TIME);
 
-	IPW_DEBUG_ASSOC("%ssociation attempt: '%s', channel %d, "
-			"802.11%c [%d], %s[:%s], enc=%s%s%s%c%c\n",
+	IPW_DEBUG_ASSOC("%ssociation attempt: '%*pE', channel %d, 802.11%c [%d], %s[:%s], enc=%s%s%s%c%c\n",
 			roaming ? "Rea" : "A",
-			print_ssid(ssid, priv->essid, priv->essid_len),
+			priv->essid_len, priv->essid,
 			network->channel,
 			ipw_modes[priv->assoc_request.ieee_mode],
 			rates->num_rates,
@@ -7553,9 +7464,8 @@ static int ipw_associate_network(struct ipw_priv *priv,
 		return err;
 	}
 
-	IPW_DEBUG(IPW_DL_STATE, "associating: '%s' %pM\n",
-		  print_ssid(ssid, priv->essid, priv->essid_len),
-		  priv->bssid);
+	IPW_DEBUG(IPW_DL_STATE, "associating: '%*pE' %pM\n",
+		  priv->essid_len, priv->essid, priv->bssid);
 
 	return 0;
 }
@@ -7645,7 +7555,6 @@ static int ipw_associate(void *data)
 	struct ipw_supported_rates *rates;
 	struct list_head *element;
 	unsigned long flags;
-	DECLARE_SSID_BUF(ssid);
 
 	if (priv->ieee->iw_mode == IW_MODE_MONITOR) {
 		IPW_DEBUG_ASSOC("Not attempting association (monitor mode)\n");
@@ -7704,10 +7613,8 @@ static int ipw_associate(void *data)
 			/* If there are no more slots, expire the oldest */
 			list_del(&oldest->list);
 			target = oldest;
-			IPW_DEBUG_ASSOC("Expired '%s' (%pM) from "
-					"network list.\n",
-					print_ssid(ssid, target->ssid,
-						   target->ssid_len),
+			IPW_DEBUG_ASSOC("Expired '%*pE' (%pM) from network list.\n",
+					target->ssid_len, target->ssid,
 					target->bssid);
 			list_add_tail(&target->list,
 				      &priv->ieee->network_free_list);
@@ -9093,7 +9000,6 @@ static int ipw_wx_set_essid(struct net_device *dev,
 {
 	struct ipw_priv *priv = libipw_priv(dev);
         int length;
-	DECLARE_SSID_BUF(ssid);
 
         mutex_lock(&priv->mutex);
 
@@ -9118,8 +9024,7 @@ static int ipw_wx_set_essid(struct net_device *dev,
 		return 0;
 	}
 
-	IPW_DEBUG_WX("Setting ESSID: '%s' (%d)\n",
-		     print_ssid(ssid, extra, length), length);
+	IPW_DEBUG_WX("Setting ESSID: '%*pE' (%d)\n", length, extra, length);
 
 	priv->essid_len = length;
 	memcpy(priv->essid, extra, priv->essid_len);
@@ -9138,15 +9043,14 @@ static int ipw_wx_get_essid(struct net_device *dev,
 			    union iwreq_data *wrqu, char *extra)
 {
 	struct ipw_priv *priv = libipw_priv(dev);
-	DECLARE_SSID_BUF(ssid);
 
 	/* If we are associated, trying to associate, or have a statically
 	 * configured ESSID then return that; otherwise return ANY */
 	mutex_lock(&priv->mutex);
 	if (priv->config & CFG_STATIC_ESSID ||
 	    priv->status & (STATUS_ASSOCIATED | STATUS_ASSOCIATING)) {
-		IPW_DEBUG_WX("Getting essid: '%s'\n",
-			     print_ssid(ssid, priv->essid, priv->essid_len));
+		IPW_DEBUG_WX("Getting essid: '%*pE'\n",
+			     priv->essid_len, priv->essid);
 		memcpy(extra, priv->essid, priv->essid_len);
 		wrqu->essid.length = priv->essid_len;
 		wrqu->essid.flags = 1;	/* active */
diff --git a/drivers/net/wireless/ipw2x00/libipw_rx.c b/drivers/net/wireless/ipw2x00/libipw_rx.c
index a586a85bfcfe..2d66984079bb 100644
--- a/drivers/net/wireless/ipw2x00/libipw_rx.c
+++ b/drivers/net/wireless/ipw2x00/libipw_rx.c
@@ -1120,7 +1120,6 @@ static int libipw_parse_info_param(struct libipw_info_element
 				      *info_element, u16 length,
 				      struct libipw_network *network)
 {
-	DECLARE_SSID_BUF(ssid);
 	u8 i;
 #ifdef CONFIG_LIBIPW_DEBUG
 	char rates_str[64];
@@ -1151,10 +1150,9 @@ static int libipw_parse_info_param(struct libipw_info_element
 				memset(network->ssid + network->ssid_len, 0,
 				       IW_ESSID_MAX_SIZE - network->ssid_len);
 
-			LIBIPW_DEBUG_MGMT("WLAN_EID_SSID: '%s' len=%d.\n",
-					     print_ssid(ssid, network->ssid,
-							network->ssid_len),
-					     network->ssid_len);
+			LIBIPW_DEBUG_MGMT("WLAN_EID_SSID: '%*pE' len=%d.\n",
+					  network->ssid_len, network->ssid,
+					  network->ssid_len);
 			break;
 
 		case WLAN_EID_SUPP_RATES:
@@ -1399,8 +1397,6 @@ static int libipw_network_init(struct libipw_device *ieee, struct libipw_probe_r
 					 struct libipw_network *network,
 					 struct libipw_rx_stats *stats)
 {
-	DECLARE_SSID_BUF(ssid);
-
 	network->qos_data.active = 0;
 	network->qos_data.supported = 0;
 	network->qos_data.param_count = 0;
@@ -1447,11 +1443,9 @@ static int libipw_network_init(struct libipw_device *ieee, struct libipw_probe_r
 	}
 
 	if (network->mode == 0) {
-		LIBIPW_DEBUG_SCAN("Filtered out '%s (%pM)' "
-				     "network.\n",
-				     print_ssid(ssid, network->ssid,
-						 network->ssid_len),
-				     network->bssid);
+		LIBIPW_DEBUG_SCAN("Filtered out '%*pE (%pM)' network.\n",
+				  network->ssid_len, network->ssid,
+				  network->bssid);
 		return 1;
 	}
 
@@ -1563,11 +1557,9 @@ static void libipw_process_probe_response(struct libipw_device
 	struct libipw_info_element *info_element = beacon->info_element;
 #endif
 	unsigned long flags;
-	DECLARE_SSID_BUF(ssid);
 
-	LIBIPW_DEBUG_SCAN("'%s' (%pM"
-		     "): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
-		     print_ssid(ssid, info_element->data, info_element->len),
+	LIBIPW_DEBUG_SCAN("'%*pE' (%pM): %c%c%c%c %c%c%c%c-%c%c%c%c %c%c%c%c\n",
+		     info_element->len, info_element->data,
 		     beacon->header.addr3,
 		     (beacon->capability & cpu_to_le16(1 << 0xf)) ? '1' : '0',
 		     (beacon->capability & cpu_to_le16(1 << 0xe)) ? '1' : '0',
@@ -1587,12 +1579,11 @@ static void libipw_process_probe_response(struct libipw_device
 		     (beacon->capability & cpu_to_le16(1 << 0x0)) ? '1' : '0');
 
 	if (libipw_network_init(ieee, beacon, &network, stats)) {
-		LIBIPW_DEBUG_SCAN("Dropped '%s' (%pM) via %s.\n",
-				     print_ssid(ssid, info_element->data,
-						 info_element->len),
-				     beacon->header.addr3,
-				     is_beacon(beacon->header.frame_ctl) ?
-				     "BEACON" : "PROBE RESPONSE");
+		LIBIPW_DEBUG_SCAN("Dropped '%*pE' (%pM) via %s.\n",
+				  info_element->len, info_element->data,
+				  beacon->header.addr3,
+				  is_beacon(beacon->header.frame_ctl) ?
+				  "BEACON" : "PROBE RESPONSE");
 		return;
 	}
 
@@ -1624,11 +1615,9 @@ static void libipw_process_probe_response(struct libipw_device
 			/* If there are no more slots, expire the oldest */
 			list_del(&oldest->list);
 			target = oldest;
-			LIBIPW_DEBUG_SCAN("Expired '%s' (%pM) from "
-					     "network list.\n",
-					     print_ssid(ssid, target->ssid,
-							 target->ssid_len),
-					     target->bssid);
+			LIBIPW_DEBUG_SCAN("Expired '%*pE' (%pM) from network list.\n",
+					  target->ssid_len, target->ssid,
+					  target->bssid);
 			libipw_network_reset(target);
 		} else {
 			/* Otherwise just pull from the free list */
@@ -1638,23 +1627,21 @@ static void libipw_process_probe_response(struct libipw_device
 		}
 
 #ifdef CONFIG_LIBIPW_DEBUG
-		LIBIPW_DEBUG_SCAN("Adding '%s' (%pM) via %s.\n",
-				     print_ssid(ssid, network.ssid,
-						 network.ssid_len),
-				     network.bssid,
-				     is_beacon(beacon->header.frame_ctl) ?
-				     "BEACON" : "PROBE RESPONSE");
+		LIBIPW_DEBUG_SCAN("Adding '%*pE' (%pM) via %s.\n",
+				  network.ssid_len, network.ssid,
+				  network.bssid,
+				  is_beacon(beacon->header.frame_ctl) ?
+				  "BEACON" : "PROBE RESPONSE");
 #endif
 		memcpy(target, &network, sizeof(*target));
 		network.ibss_dfs = NULL;
 		list_add_tail(&target->list, &ieee->network_list);
 	} else {
-		LIBIPW_DEBUG_SCAN("Updating '%s' (%pM) via %s.\n",
-				     print_ssid(ssid, target->ssid,
-						 target->ssid_len),
-				     target->bssid,
-				     is_beacon(beacon->header.frame_ctl) ?
-				     "BEACON" : "PROBE RESPONSE");
+		LIBIPW_DEBUG_SCAN("Updating '%*pE' (%pM) via %s.\n",
+				  target->ssid_len, target->ssid,
+				  target->bssid,
+				  is_beacon(beacon->header.frame_ctl) ?
+				  "BEACON" : "PROBE RESPONSE");
 		update_network(target, &network);
 		network.ibss_dfs = NULL;
 	}
diff --git a/drivers/net/wireless/ipw2x00/libipw_wx.c b/drivers/net/wireless/ipw2x00/libipw_wx.c
index 54aba4744438..dd29f46d086b 100644
--- a/drivers/net/wireless/ipw2x00/libipw_wx.c
+++ b/drivers/net/wireless/ipw2x00/libipw_wx.c
@@ -272,7 +272,6 @@ int libipw_wx_get_scan(struct libipw_device *ieee,
 	char *ev = extra;
 	char *stop = ev + wrqu->data.length;
 	int i = 0;
-	DECLARE_SSID_BUF(ssid);
 
 	LIBIPW_DEBUG_WX("Getting scan\n");
 
@@ -290,12 +289,10 @@ int libipw_wx_get_scan(struct libipw_device *ieee,
 			ev = libipw_translate_scan(ieee, ev, stop, network,
 						      info);
 		else {
-			LIBIPW_DEBUG_SCAN("Not showing network '%s ("
-					     "%pM)' due to age (%ums).\n",
-					     print_ssid(ssid, network->ssid,
-							 network->ssid_len),
-					     network->bssid,
-					     elapsed_jiffies_msecs(
+			LIBIPW_DEBUG_SCAN("Not showing network '%*pE (%pM)' due to age (%ums).\n",
+					  network->ssid_len, network->ssid,
+					  network->bssid,
+					  elapsed_jiffies_msecs(
 					               network->last_scanned));
 		}
 	}
@@ -322,7 +319,6 @@ int libipw_wx_set_encode(struct libipw_device *ieee,
 	int i, key, key_provided, len;
 	struct lib80211_crypt_data **crypt;
 	int host_crypto = ieee->host_encrypt || ieee->host_decrypt;
-	DECLARE_SSID_BUF(ssid);
 
 	LIBIPW_DEBUG_WX("SET_ENCODE\n");
 
@@ -417,8 +413,8 @@ int libipw_wx_set_encode(struct libipw_device *ieee,
 		if (len > erq->length)
 			memset(sec.keys[key] + erq->length, 0,
 			       len - erq->length);
-		LIBIPW_DEBUG_WX("Setting key %d to '%s' (%d:%d bytes)\n",
-				   key, print_ssid(ssid, sec.keys[key], len),
+		LIBIPW_DEBUG_WX("Setting key %d to '%*pE' (%d:%d bytes)\n",
+				   key, len, sec.keys[key],
 				   erq->length, len);
 		sec.key_sizes[key] = len;
 		if (*crypt)
diff --git a/drivers/net/wireless/iwlegacy/4965-mac.c b/drivers/net/wireless/iwlegacy/4965-mac.c
index 3dcbe2cd2b28..26fec54dcd03 100644
--- a/drivers/net/wireless/iwlegacy/4965-mac.c
+++ b/drivers/net/wireless/iwlegacy/4965-mac.c
@@ -4633,7 +4633,7 @@ il4965_store_tx_power(struct device *d, struct device_attribute *attr,
 	else {
 		ret = il_set_tx_power(il, val, false);
 		if (ret)
-			IL_ERR("failed setting tx power (0x%d).\n", ret);
+			IL_ERR("failed setting tx power (0x%08x).\n", ret);
 		else
 			ret = count;
 	}
@@ -5757,9 +5757,8 @@ il4965_mac_setup_register(struct il_priv *il, u32 max_probe_length)
 	    IEEE80211_HW_REPORTS_TX_ACK_STATUS | IEEE80211_HW_SUPPORTS_PS |
 	    IEEE80211_HW_SUPPORTS_DYNAMIC_PS;
 	if (il->cfg->sku & IL_SKU_N)
-		hw->flags |=
-		    IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
-		    IEEE80211_HW_SUPPORTS_STATIC_SMPS;
+		hw->wiphy->features |= NL80211_FEATURE_DYNAMIC_SMPS |
+				       NL80211_FEATURE_STATIC_SMPS;
 
 	hw->sta_data_size = sizeof(struct il_station_priv);
 	hw->vif_data_size = sizeof(struct il_vif_priv);
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index 6451d2b6abcf..267e48a2915e 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -51,7 +51,6 @@ config IWLWIFI_LEDS
 
 config IWLDVM
 	tristate "Intel Wireless WiFi DVM Firmware support"
-	depends on m
 	default IWLWIFI
 	help
 	  This is the driver that supports the DVM firmware which is
@@ -60,7 +59,6 @@ config IWLDVM
 
 config IWLMVM
 	tristate "Intel Wireless WiFi MVM Firmware support"
-	depends on m
 	help
 	  This is the driver that supports the MVM firmware which is
 	  currently only available for 7260 and 3160 devices.
@@ -87,6 +85,16 @@ config IWLWIFI_BCAST_FILTERING
 	  If unsure, don't enable this option, as some programs might
 	  expect incoming broadcasts for their normal operations.
 
+config IWLWIFI_UAPSD
+	bool "enable U-APSD by default"
+	depends on IWLMVM
+	help
+	  Say Y here to enable U-APSD by default. This may cause
+	  interoperability problems with some APs, manifesting in lower than
+	  expected throughput due to those APs not enabling aggregation
+
+	  If unsure, say N.
+
 menu "Debugging Options"
 
 config IWLWIFI_DEBUG
diff --git a/drivers/net/wireless/iwlwifi/dvm/mac80211.c b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
index afb98f4fdaf3..2364a3c09b9e 100644
--- a/drivers/net/wireless/iwlwifi/dvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/dvm/mac80211.c
@@ -125,8 +125,8 @@ int iwlagn_mac_setup_register(struct iwl_priv *priv,
 	 */
 
 	if (priv->nvm_data->sku_cap_11n_enable)
-		hw->flags |= IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
-			     IEEE80211_HW_SUPPORTS_STATIC_SMPS;
+		hw->wiphy->features |= NL80211_FEATURE_DYNAMIC_SMPS |
+				       NL80211_FEATURE_STATIC_SMPS;
 
 	/*
 	 * Enable 11w if advertised by firmware and software crypto
diff --git a/drivers/net/wireless/iwlwifi/dvm/power.c b/drivers/net/wireless/iwlwifi/dvm/power.c
index 760c45c34ef3..1513dbc79c14 100644
--- a/drivers/net/wireless/iwlwifi/dvm/power.c
+++ b/drivers/net/wireless/iwlwifi/dvm/power.c
@@ -40,7 +40,7 @@
 #include "commands.h"
 #include "power.h"
 
-static bool force_cam;
+static bool force_cam = true;
 module_param(force_cam, bool, 0644);
 MODULE_PARM_DESC(force_cam, "force continuously aware mode (no power saving at all)");
 
diff --git a/drivers/net/wireless/iwlwifi/dvm/rxon.c b/drivers/net/wireless/iwlwifi/dvm/rxon.c
index 6dc5dd3ced44..ed50de6362ed 100644
--- a/drivers/net/wireless/iwlwifi/dvm/rxon.c
+++ b/drivers/net/wireless/iwlwifi/dvm/rxon.c
@@ -1068,6 +1068,13 @@ int iwlagn_commit_rxon(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 	/* recalculate basic rates */
 	iwl_calc_basic_rates(priv, ctx);
 
+	/*
+	 * force CTS-to-self frames protection if RTS-CTS is not preferred
+	 * one aggregation protection method
+	 */
+	if (!priv->hw_params.use_rts_for_aggregation)
+		ctx->staging.flags |= RXON_FLG_SELF_CTS_EN;
+
 	if ((ctx->vif && ctx->vif->bss_conf.use_short_slot) ||
 	    !(ctx->staging.flags & RXON_FLG_BAND_24G_MSK))
 		ctx->staging.flags |= RXON_FLG_SHORT_SLOT_MSK;
@@ -1473,6 +1480,11 @@ void iwlagn_bss_info_changed(struct ieee80211_hw *hw,
 	else
 		ctx->staging.flags &= ~RXON_FLG_TGG_PROTECT_MSK;
 
+	if (bss_conf->use_cts_prot)
+		ctx->staging.flags |= RXON_FLG_SELF_CTS_EN;
+	else
+		ctx->staging.flags &= ~RXON_FLG_SELF_CTS_EN;
+
 	memcpy(ctx->staging.bssid_addr, bss_conf->bssid, ETH_ALEN);
 
 	if (vif->type == NL80211_IFTYPE_AP ||
diff --git a/drivers/net/wireless/iwlwifi/dvm/tx.c b/drivers/net/wireless/iwlwifi/dvm/tx.c
index 3255a1723d17..d1ce3ce13591 100644
--- a/drivers/net/wireless/iwlwifi/dvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/dvm/tx.c
@@ -580,7 +580,7 @@ turn_off:
 		 * time, or we hadn't time to drain the AC queues.
 		 */
 		if (agg_state == IWL_AGG_ON)
-			iwl_trans_txq_disable(priv->trans, txq_id);
+			iwl_trans_txq_disable(priv->trans, txq_id, true);
 		else
 			IWL_DEBUG_TX_QUEUES(priv, "Don't disable tx agg: %d\n",
 					    agg_state);
@@ -686,7 +686,7 @@ int iwlagn_tx_agg_flush(struct iwl_priv *priv, struct ieee80211_vif *vif,
 		 * time, or we hadn't time to drain the AC queues.
 		 */
 		if (agg_state == IWL_AGG_ON)
-			iwl_trans_txq_disable(priv->trans, txq_id);
+			iwl_trans_txq_disable(priv->trans, txq_id, true);
 		else
 			IWL_DEBUG_TX_QUEUES(priv, "Don't disable tx agg: %d\n",
 					    agg_state);
@@ -781,7 +781,7 @@ static void iwlagn_check_ratid_empty(struct iwl_priv *priv, int sta_id, u8 tid)
 				"Can continue DELBA flow ssn = next_recl = %d\n",
 				tid_data->next_reclaimed);
 			iwl_trans_txq_disable(priv->trans,
-					      tid_data->agg.txq_id);
+					      tid_data->agg.txq_id, true);
 			iwlagn_dealloc_agg_txq(priv, tid_data->agg.txq_id);
 			tid_data->agg.state = IWL_AGG_OFF;
 			ieee80211_stop_tx_ba_cb_irqsafe(vif, addr, tid);
diff --git a/drivers/net/wireless/iwlwifi/iwl-7000.c b/drivers/net/wireless/iwlwifi/iwl-7000.c
index 48730064da73..b04b8858c690 100644
--- a/drivers/net/wireless/iwlwifi/iwl-7000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-7000.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -67,8 +69,8 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL7260_UCODE_API_MAX	9
-#define IWL3160_UCODE_API_MAX	9
+#define IWL7260_UCODE_API_MAX	10
+#define IWL3160_UCODE_API_MAX	10
 
 /* Oldest version we won't warn about */
 #define IWL7260_UCODE_API_OK	9
@@ -83,6 +85,8 @@
 #define IWL7260_TX_POWER_VERSION	0xffff /* meaningless */
 #define IWL3160_NVM_VERSION		0x709
 #define IWL3160_TX_POWER_VERSION	0xffff /* meaningless */
+#define IWL3165_NVM_VERSION		0x709
+#define IWL3165_TX_POWER_VERSION	0xffff /* meaningless */
 #define IWL7265_NVM_VERSION		0x0a1d
 #define IWL7265_TX_POWER_VERSION	0xffff /* meaningless */
 
@@ -92,6 +96,9 @@
 #define IWL3160_FW_PRE "iwlwifi-3160-"
 #define IWL3160_MODULE_FIRMWARE(api) IWL3160_FW_PRE __stringify(api) ".ucode"
 
+#define IWL3165_FW_PRE "iwlwifi-3165-"
+#define IWL3165_MODULE_FIRMWARE(api) IWL3165_FW_PRE __stringify(api) ".ucode"
+
 #define IWL7265_FW_PRE "iwlwifi-7265-"
 #define IWL7265_MODULE_FIRMWARE(api) IWL7265_FW_PRE __stringify(api) ".ucode"
 
@@ -124,7 +131,8 @@ static const struct iwl_ht_params iwl7000_ht_params = {
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.base_params = &iwl7000_base_params,			\
 	.led_mode = IWL_LED_RF_STATE,				\
-	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_7000
+	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_7000,	\
+	.non_shared_ant = ANT_A
 
 
 const struct iwl_cfg iwl7260_2ac_cfg = {
@@ -213,11 +221,27 @@ static const struct iwl_pwr_tx_backoff iwl7265_pwr_tx_backoffs[] = {
 	{0},
 };
 
+static const struct iwl_ht_params iwl7265_ht_params = {
+	.stbc = true,
+	.ldpc = true,
+	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
+};
+
+const struct iwl_cfg iwl3165_2ac_cfg = {
+	.name = "Intel(R) Dual Band Wireless AC 3165",
+	.fw_name_pre = IWL3165_FW_PRE,
+	IWL_DEVICE_7000,
+	.ht_params = &iwl7000_ht_params,
+	.nvm_ver = IWL3165_NVM_VERSION,
+	.nvm_calib_ver = IWL3165_TX_POWER_VERSION,
+	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
+};
+
 const struct iwl_cfg iwl7265_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 7265",
 	.fw_name_pre = IWL7265_FW_PRE,
 	IWL_DEVICE_7000,
-	.ht_params = &iwl7000_ht_params,
+	.ht_params = &iwl7265_ht_params,
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
@@ -227,7 +251,7 @@ const struct iwl_cfg iwl7265_2n_cfg = {
 	.name = "Intel(R) Dual Band Wireless N 7265",
 	.fw_name_pre = IWL7265_FW_PRE,
 	IWL_DEVICE_7000,
-	.ht_params = &iwl7000_ht_params,
+	.ht_params = &iwl7265_ht_params,
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
@@ -237,7 +261,7 @@ const struct iwl_cfg iwl7265_n_cfg = {
 	.name = "Intel(R) Wireless N 7265",
 	.fw_name_pre = IWL7265_FW_PRE,
 	IWL_DEVICE_7000,
-	.ht_params = &iwl7000_ht_params,
+	.ht_params = &iwl7265_ht_params,
 	.nvm_ver = IWL7265_NVM_VERSION,
 	.nvm_calib_ver = IWL7265_TX_POWER_VERSION,
 	.pwr_tx_backoffs = iwl7265_pwr_tx_backoffs,
@@ -245,4 +269,5 @@ const struct iwl_cfg iwl7265_n_cfg = {
 
 MODULE_FIRMWARE(IWL7260_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
 MODULE_FIRMWARE(IWL3160_MODULE_FIRMWARE(IWL3160_UCODE_API_OK));
+MODULE_FIRMWARE(IWL3165_MODULE_FIRMWARE(IWL3160_UCODE_API_OK));
 MODULE_FIRMWARE(IWL7265_MODULE_FIRMWARE(IWL7260_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-8000.c b/drivers/net/wireless/iwlwifi/iwl-8000.c
index 44b19e015102..e4351487ca72 100644
--- a/drivers/net/wireless/iwlwifi/iwl-8000.c
+++ b/drivers/net/wireless/iwlwifi/iwl-8000.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -67,7 +69,7 @@
 #include "iwl-agn-hw.h"
 
 /* Highest firmware API version supported */
-#define IWL8000_UCODE_API_MAX	9
+#define IWL8000_UCODE_API_MAX	10
 
 /* Oldest version we won't warn about */
 #define IWL8000_UCODE_API_OK	8
@@ -79,7 +81,7 @@
 #define IWL8000_NVM_VERSION		0x0a1d
 #define IWL8000_TX_POWER_VERSION	0xffff /* meaningless */
 
-#define IWL8000_FW_PRE "iwlwifi-8000-"
+#define IWL8000_FW_PRE "iwlwifi-8000"
 #define IWL8000_MODULE_FIRMWARE(api) IWL8000_FW_PRE __stringify(api) ".ucode"
 
 #define NVM_HW_SECTION_NUM_FAMILY_8000		10
@@ -101,6 +103,7 @@ static const struct iwl_base_params iwl8000_base_params = {
 };
 
 static const struct iwl_ht_params iwl8000_ht_params = {
+	.ldpc = true,
 	.ht40_bands = BIT(IEEE80211_BAND_2GHZ) | BIT(IEEE80211_BAND_5GHZ),
 };
 
@@ -113,7 +116,17 @@ static const struct iwl_ht_params iwl8000_ht_params = {
 	.max_data_size = IWL60_RTC_DATA_SIZE,			\
 	.base_params = &iwl8000_base_params,			\
 	.led_mode = IWL_LED_RF_STATE,				\
-	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_8000
+	.nvm_hw_section_num = NVM_HW_SECTION_NUM_FAMILY_8000,	\
+	.non_shared_ant = ANT_A
+
+const struct iwl_cfg iwl8260_2n_cfg = {
+	.name = "Intel(R) Dual Band Wireless N 8260",
+	.fw_name_pre = IWL8000_FW_PRE,
+	IWL_DEVICE_8000,
+	.ht_params = &iwl8000_ht_params,
+	.nvm_ver = IWL8000_NVM_VERSION,
+	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
+};
 
 const struct iwl_cfg iwl8260_2ac_cfg = {
 	.name = "Intel(R) Dual Band Wireless AC 8260",
@@ -133,6 +146,7 @@ const struct iwl_cfg iwl8260_2ac_sdio_cfg = {
 	.nvm_calib_ver = IWL8000_TX_POWER_VERSION,
 	.default_nvm_file = DEFAULT_NVM_FILE_FAMILY_8000,
 	.max_rx_agg_size = MAX_RX_AGG_SIZE_8260_SDIO,
+	.disable_dummy_notification = true,
 };
 
 MODULE_FIRMWARE(IWL8000_MODULE_FIRMWARE(IWL8000_UCODE_API_OK));
diff --git a/drivers/net/wireless/iwlwifi/iwl-config.h b/drivers/net/wireless/iwlwifi/iwl-config.h
index 8da596db9abe..2ef83a39ff10 100644
--- a/drivers/net/wireless/iwlwifi/iwl-config.h
+++ b/drivers/net/wireless/iwlwifi/iwl-config.h
@@ -120,6 +120,8 @@ enum iwl_led_mode {
 #define IWL_LONG_WD_TIMEOUT	10000
 #define IWL_MAX_WD_TIMEOUT	120000
 
+#define IWL_DEFAULT_MAX_TX_POWER 22
+
 /* Antenna presence definitions */
 #define	ANT_NONE	0x0
 #define	ANT_A		BIT(0)
@@ -169,6 +171,7 @@ struct iwl_base_params {
 
 /*
  * @stbc: support Tx STBC and 1*SS Rx STBC
+ * @ldpc: support Tx/Rx with LDPC
  * @use_rts_for_aggregation: use rts/cts protection for HT traffic
  * @ht40_bands: bitmap of bands (using %IEEE80211_BAND_*) that support HT40
  */
@@ -176,6 +179,7 @@ struct iwl_ht_params {
 	enum ieee80211_smps_mode smps_mode;
 	const bool ht_greenfield_support; /* if used set to true */
 	const bool stbc;
+	const bool ldpc;
 	bool use_rts_for_aggregation;
 	u8 ht40_bands;
 };
@@ -226,6 +230,7 @@ struct iwl_pwr_tx_backoff {
  * @max_data_size: The maximal length of the fw data section
  * @valid_tx_ant: valid transmit antenna
  * @valid_rx_ant: valid receive antenna
+ * @non_shared_ant: the antenna that is for WiFi only
  * @nvm_ver: NVM version
  * @nvm_calib_ver: NVM calibration version
  * @lib: pointer to the lib ops
@@ -258,6 +263,7 @@ struct iwl_cfg {
 	const u32 max_inst_size;
 	u8   valid_tx_ant;
 	u8   valid_rx_ant;
+	u8   non_shared_ant;
 	bool bt_shared_single_ant;
 	u16  nvm_ver;
 	u16  nvm_calib_ver;
@@ -278,6 +284,7 @@ struct iwl_cfg {
 	bool no_power_up_nic_in_init;
 	const char *default_nvm_file;
 	unsigned int max_rx_agg_size;
+	bool disable_dummy_notification;
 };
 
 /*
@@ -335,9 +342,11 @@ extern const struct iwl_cfg iwl7260_n_cfg;
 extern const struct iwl_cfg iwl3160_2ac_cfg;
 extern const struct iwl_cfg iwl3160_2n_cfg;
 extern const struct iwl_cfg iwl3160_n_cfg;
+extern const struct iwl_cfg iwl3165_2ac_cfg;
 extern const struct iwl_cfg iwl7265_2ac_cfg;
 extern const struct iwl_cfg iwl7265_2n_cfg;
 extern const struct iwl_cfg iwl7265_n_cfg;
+extern const struct iwl_cfg iwl8260_2n_cfg;
 extern const struct iwl_cfg iwl8260_2ac_cfg;
 extern const struct iwl_cfg iwl8260_2ac_sdio_cfg;
 #endif /* CONFIG_IWLMVM */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index fe129c94ae3e..3f6f015285e5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -293,6 +295,16 @@
 #define CSR_HW_REV_DASH(_val)          (((_val) & 0x0000003) >> 0)
 #define CSR_HW_REV_STEP(_val)          (((_val) & 0x000000C) >> 2)
 
+
+/**
+ *  hw_rev values
+ */
+enum {
+	SILICON_A_STEP = 0,
+	SILICON_B_STEP,
+};
+
+
 #define CSR_HW_REV_TYPE_MSK            (0x000FFF0)
 #define CSR_HW_REV_TYPE_5300           (0x0000020)
 #define CSR_HW_REV_TYPE_5350           (0x0000030)
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index 295083510e72..0a70bcd241f5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -145,6 +145,7 @@ do {                                            			\
 #define IWL_DL_HCMD		0x00000004
 #define IWL_DL_STATE		0x00000008
 /* 0x000000F0 - 0x00000010 */
+#define IWL_DL_QUOTA		0x00000010
 #define IWL_DL_TE		0x00000020
 #define IWL_DL_EEPROM		0x00000040
 #define IWL_DL_RADIO		0x00000080
@@ -189,6 +190,7 @@ do {                                            			\
 #define IWL_DEBUG_LED(p, f, a...)	IWL_DEBUG(p, IWL_DL_LED, f, ## a)
 #define IWL_DEBUG_WEP(p, f, a...)	IWL_DEBUG(p, IWL_DL_WEP, f, ## a)
 #define IWL_DEBUG_HC(p, f, a...)	IWL_DEBUG(p, IWL_DL_HCMD, f, ## a)
+#define IWL_DEBUG_QUOTA(p, f, a...)	IWL_DEBUG(p, IWL_DL_QUOTA, f, ## a)
 #define IWL_DEBUG_TE(p, f, a...)	IWL_DEBUG(p, IWL_DL_TE, f, ## a)
 #define IWL_DEBUG_EEPROM(d, f, a...)	IWL_DEBUG_DEV(d, IWL_DL_EEPROM, f, ## a)
 #define IWL_DEBUG_CALIB(p, f, a...)	IWL_DEBUG(p, IWL_DL_CALIB, f, ## a)
diff --git a/drivers/net/wireless/iwlwifi/iwl-devtrace.c b/drivers/net/wireless/iwlwifi/iwl-devtrace.c
index 23e7351e02de..90987d6f348e 100644
--- a/drivers/net/wireless/iwlwifi/iwl-devtrace.c
+++ b/drivers/net/wireless/iwlwifi/iwl-devtrace.c
@@ -36,15 +36,8 @@
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_iowrite8);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ioread32);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_iowrite32);
-EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_rx);
-EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_tx);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_event);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_error);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_cont_event);
 EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dev_ucode_wrap_event);
-EXPORT_TRACEPOINT_SYMBOL(iwlwifi_info);
-EXPORT_TRACEPOINT_SYMBOL(iwlwifi_warn);
-EXPORT_TRACEPOINT_SYMBOL(iwlwifi_crit);
-EXPORT_TRACEPOINT_SYMBOL(iwlwifi_err);
-EXPORT_TRACEPOINT_SYMBOL(iwlwifi_dbg);
 #endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.c b/drivers/net/wireless/iwlwifi/iwl-drv.c
index 77e3178040b2..0f1084f09caa 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.c
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -67,6 +69,7 @@
 #include <linux/vmalloc.h>
 
 #include "iwl-drv.h"
+#include "iwl-csr.h"
 #include "iwl-debug.h"
 #include "iwl-trans.h"
 #include "iwl-op-mode.h"
@@ -242,6 +245,23 @@ static int iwl_request_firmware(struct iwl_drv *drv, bool first)
 	snprintf(drv->firmware_name, sizeof(drv->firmware_name), "%s%s.ucode",
 		 name_pre, tag);
 
+	/*
+	 * Starting 8000B - FW name format has changed. This overwrites the
+	 * previous name and uses the new format.
+	 */
+	if (drv->trans->cfg->device_family == IWL_DEVICE_FAMILY_8000) {
+		char rev_step[2] = {
+			'A' + CSR_HW_REV_STEP(drv->trans->hw_rev), 0
+		};
+
+		/* A-step doesn't have an indication */
+		if (CSR_HW_REV_STEP(drv->trans->hw_rev) == SILICON_A_STEP)
+			rev_step[0] = 0;
+
+		snprintf(drv->firmware_name, sizeof(drv->firmware_name),
+			 "%s%s-%s.ucode", name_pre, rev_step, tag);
+	}
+
 	IWL_DEBUG_INFO(drv, "attempting to load firmware %s'%s'\n",
 		       (drv->fw_index == UCODE_EXPERIMENTAL_INDEX)
 				? "EXPERIMENTAL " : "",
@@ -1254,7 +1274,9 @@ struct iwl_mod_params iwlwifi_mod_params = {
 	.bt_coex_active = true,
 	.power_level = IWL_POWER_INDEX_1,
 	.wd_disable = true,
-	.uapsd_disable = false,
+#ifndef CONFIG_IWLWIFI_UAPSD
+	.uapsd_disable = true,
+#endif /* CONFIG_IWLWIFI_UAPSD */
 	/* the rest are 0 by default */
 };
 IWL_EXPORT_SYMBOL(iwlwifi_mod_params);
@@ -1359,7 +1381,7 @@ MODULE_PARM_DESC(fw_restart, "restart firmware in case of error (default true)")
 module_param_named(antenna_coupling, iwlwifi_mod_params.ant_coupling,
 		   int, S_IRUGO);
 MODULE_PARM_DESC(antenna_coupling,
-		 "specify antenna coupling in dB (defualt: 0 dB)");
+		 "specify antenna coupling in dB (default: 0 dB)");
 
 module_param_named(wd_disable, iwlwifi_mod_params.wd_disable, int, S_IRUGO);
 MODULE_PARM_DESC(wd_disable,
@@ -1370,7 +1392,11 @@ MODULE_PARM_DESC(nvm_file, "NVM file name");
 
 module_param_named(uapsd_disable, iwlwifi_mod_params.uapsd_disable,
 		   bool, S_IRUGO);
+#ifdef CONFIG_IWLWIFI_UAPSD
 MODULE_PARM_DESC(uapsd_disable, "disable U-APSD functionality (default: N)");
+#else
+MODULE_PARM_DESC(uapsd_disable, "disable U-APSD functionality (default: Y)");
+#endif
 
 /*
  * set bt_coex_active to true, uCode will do kill/defer
diff --git a/drivers/net/wireless/iwlwifi/iwl-drv.h b/drivers/net/wireless/iwlwifi/iwl-drv.h
index 3c72cb710b0c..be4f8972241a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-drv.h
+++ b/drivers/net/wireless/iwlwifi/iwl-drv.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
index 07ff7e0028ee..74b796dc4242 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom-parse.c
@@ -758,6 +758,9 @@ void iwl_init_ht_hw_capab(const struct iwl_cfg *cfg,
 			ht_info->cap |= IEEE80211_HT_CAP_TX_STBC;
 	}
 
+	if (cfg->ht_params->ldpc)
+		ht_info->cap |= IEEE80211_HT_CAP_LDPC_CODING;
+
 	if (iwlwifi_mod_params.amsdu_size_8K)
 		ht_info->cap |= IEEE80211_HT_CAP_MAX_AMSDU;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
index de5994a776c7..e30a41d04c8b 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-error-dump.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw-file.h b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
index 929a8063354c..401f7be36b93 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw-file.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw-file.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/iwl-fw.h b/drivers/net/wireless/iwlwifi/iwl-fw.h
index 1bb5193c5b1b..4f6e66892acc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-fw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-fw.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -125,6 +127,9 @@ enum iwl_ucode_tlv_flag {
  * @IWL_UCODE_TLV_API_CSA_FLOW: ucode can do unbind-bind flow for CSA.
  * @IWL_UCODE_TLV_API_DISABLE_STA_TX: ucode supports tx_disable bit.
  * @IWL_UCODE_TLV_API_LMAC_SCAN: This ucode uses LMAC unified scan API.
+ * @IWL_UCODE_TLV_API_SF_NO_DUMMY_NOTIF: ucode supports disabling dummy notif.
+ * @IWL_UCODE_TLV_API_FRAGMENTED_SCAN: This ucode supports active dwell time
+ *	longer than the passive one, which is essential for fragmented scan.
  */
 enum iwl_ucode_tlv_api {
 	IWL_UCODE_TLV_API_WOWLAN_CONFIG_TID	= BIT(0),
@@ -133,14 +138,31 @@ enum iwl_ucode_tlv_api {
 	IWL_UCODE_TLV_API_CSA_FLOW		= BIT(4),
 	IWL_UCODE_TLV_API_DISABLE_STA_TX	= BIT(5),
 	IWL_UCODE_TLV_API_LMAC_SCAN		= BIT(6),
+	IWL_UCODE_TLV_API_SF_NO_DUMMY_NOTIF	= BIT(7),
+	IWL_UCODE_TLV_API_FRAGMENTED_SCAN	= BIT(8),
 };
 
 /**
  * enum iwl_ucode_tlv_capa - ucode capabilities
  * @IWL_UCODE_TLV_CAPA_D0I3_SUPPORT: supports D0i3
+ * @IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT: supports insertion of current
+ *	tx power value into TPC Report action frame and Link Measurement Report
+ *	action frame
+ * @IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT: supports adding DS params
+ *	element in probe requests.
+ * @IWL_UCODE_TLV_CAPA_WFA_TPC_REP_IE_SUPPORT: supports adding TPC Report IE in
+ *	probe requests.
+ * @IWL_UCODE_TLV_CAPA_QUIET_PERIOD_SUPPORT: supports Quiet Period requests
+ * @IWL_UCODE_TLV_CAPA_DQA_SUPPORT: supports dynamic queue allocation (DQA),
+ *	which also implies support for the scheduler configuration command
  */
 enum iwl_ucode_tlv_capa {
-	IWL_UCODE_TLV_CAPA_D0I3_SUPPORT		= BIT(0),
+	IWL_UCODE_TLV_CAPA_D0I3_SUPPORT			= BIT(0),
+	IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT	= BIT(8),
+	IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT	= BIT(9),
+	IWL_UCODE_TLV_CAPA_WFA_TPC_REP_IE_SUPPORT	= BIT(10),
+	IWL_UCODE_TLV_CAPA_QUIET_PERIOD_SUPPORT		= BIT(11),
+	IWL_UCODE_TLV_CAPA_DQA_SUPPORT			= BIT(12),
 };
 
 /* The default calibrate table size if not specified by firmware file */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.c b/drivers/net/wireless/iwlwifi/iwl-io.c
index 5eef4ae7333b..7a2cbf6f90db 100644
--- a/drivers/net/wireless/iwlwifi/iwl-io.c
+++ b/drivers/net/wireless/iwlwifi/iwl-io.c
@@ -193,7 +193,7 @@ void iwl_force_nmi(struct iwl_trans *trans)
 	 * DEVICE_SET_NMI_8000B_REG - is used.
 	 */
 	if ((trans->cfg->device_family != IWL_DEVICE_FAMILY_8000) ||
-	    ((trans->hw_rev & 0xc) == 0x0))
+	    (CSR_HW_REV_STEP(trans->hw_rev) == SILICON_A_STEP))
 		iwl_write_prph(trans, DEVICE_SET_NMI_REG, DEVICE_SET_NMI_VAL);
 	else
 		iwl_write_prph(trans, DEVICE_SET_NMI_8000B_REG,
diff --git a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
index 018af2957d3b..c302e7468559 100644
--- a/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
+++ b/drivers/net/wireless/iwlwifi/iwl-nvm-parse.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2008 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -146,8 +148,6 @@ static const u8 iwl_nvm_channels_family_8000[] = {
 #define LAST_2GHZ_HT_PLUS		9
 #define LAST_5GHZ_HT			161
 
-#define DEFAULT_MAX_TX_POWER 16
-
 /* rate data (static) */
 static struct ieee80211_rate iwl_cfg80211_rates[] = {
 	{ .bitrate = 1 * 10, .hw_value = 0, .hw_value_short = 0, },
@@ -295,7 +295,7 @@ static int iwl_init_channel_map(struct device *dev, const struct iwl_cfg *cfg,
 		 * Default value - highest tx power value.  max_power
 		 * is not used in mvm, and is used for backwards compatibility
 		 */
-		channel->max_power = DEFAULT_MAX_TX_POWER;
+		channel->max_power = IWL_DEFAULT_MAX_TX_POWER;
 		is_5ghz = channel->band == IEEE80211_BAND_5GHZ;
 		IWL_DEBUG_EEPROM(dev,
 				 "Ch. %d [%sGHz] %s%s%s%s%s%s%s(0x%02x %ddBm): Ad-Hoc %ssupported\n",
@@ -334,6 +334,9 @@ static void iwl_init_vht_hw_capab(const struct iwl_cfg *cfg,
 		       3 << IEEE80211_VHT_CAP_BEAMFORMEE_STS_SHIFT |
 		       7 << IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_SHIFT;
 
+	if (cfg->ht_params->ldpc)
+		vht_cap->cap |= IEEE80211_VHT_CAP_RXLDPC;
+
 	if (num_tx_ants > 1)
 		vht_cap->cap |= IEEE80211_VHT_CAP_TXSTBC;
 	else
diff --git a/drivers/net/wireless/iwlwifi/iwl-op-mode.h b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
index 99785c892f96..b6d666ee8359 100644
--- a/drivers/net/wireless/iwlwifi/iwl-op-mode.h
+++ b/drivers/net/wireless/iwlwifi/iwl-op-mode.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index 47033a35a402..1560f4576c7d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -281,6 +283,7 @@
 #define SCD_CHAINEXT_EN		(SCD_BASE + 0x244)
 #define SCD_AGGR_SEL		(SCD_BASE + 0x248)
 #define SCD_INTERRUPT_MASK	(SCD_BASE + 0x108)
+#define SCD_EN_CTRL		(SCD_BASE + 0x254)
 
 static inline unsigned int SCD_QUEUE_WRPTR(unsigned int chnl)
 {
diff --git a/drivers/net/wireless/iwlwifi/iwl-scd.h b/drivers/net/wireless/iwlwifi/iwl-scd.h
new file mode 100644
index 000000000000..6c622b21bba7
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-scd.h
@@ -0,0 +1,118 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_scd_h__
+#define __iwl_scd_h__
+
+#include "iwl-trans.h"
+#include "iwl-io.h"
+#include "iwl-prph.h"
+
+
+static inline void iwl_scd_txq_set_inactive(struct iwl_trans *trans,
+					    u16 txq_id)
+{
+	iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
+		       (0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
+		       (1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+}
+
+static inline void iwl_scd_txq_set_chain(struct iwl_trans *trans,
+					 u16 txq_id)
+{
+	iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
+}
+
+static inline void iwl_scd_txq_enable_agg(struct iwl_trans *trans,
+					  u16 txq_id)
+{
+	iwl_set_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+}
+
+static inline void iwl_scd_txq_disable_agg(struct iwl_trans *trans,
+					   u16 txq_id)
+{
+	iwl_clear_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+}
+
+static inline void iwl_scd_disable_agg(struct iwl_trans *trans)
+{
+	iwl_set_bits_prph(trans, SCD_AGGR_SEL, 0);
+}
+
+static inline void iwl_scd_activate_fifos(struct iwl_trans *trans)
+{
+	iwl_write_prph(trans, SCD_TXFACT, IWL_MASK(0, 7));
+}
+
+static inline void iwl_scd_deactivate_fifos(struct iwl_trans *trans)
+{
+	iwl_write_prph(trans, SCD_TXFACT, 0);
+}
+
+static inline void iwl_scd_enable_set_active(struct iwl_trans *trans,
+					     u32 value)
+{
+	iwl_write_prph(trans, SCD_EN_CTRL, value);
+}
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-trans.h b/drivers/net/wireless/iwlwifi/iwl-trans.h
index 656371a668da..9eb85249e89c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-trans.h
+++ b/drivers/net/wireless/iwlwifi/iwl-trans.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -375,6 +377,7 @@ enum iwl_trans_status {
  *	if unset 4k will be the RX buffer size
  * @bc_table_dword: set to true if the BC table expects the byte count to be
  *	in DWORD (as opposed to bytes)
+ * @scd_set_active: should the transport configure the SCD for HCMD queue
  * @queue_watchdog_timeout: time (in ms) after which queues
  *	are considered stuck and will trigger device restart
  * @command_names: array of command names, must be 256 entries
@@ -390,6 +393,7 @@ struct iwl_trans_config {
 
 	bool rx_buf_size_8k;
 	bool bc_table_dword;
+	bool scd_set_active;
 	unsigned int queue_watchdog_timeout;
 	const char *const *command_names;
 };
@@ -401,6 +405,14 @@ struct iwl_trans_dump_data {
 
 struct iwl_trans;
 
+struct iwl_trans_txq_scd_cfg {
+	u8 fifo;
+	s8 sta_id;
+	u8 tid;
+	bool aggregate;
+	int frame_limit;
+};
+
 /**
  * struct iwl_trans_ops - transport specific operations
  *
@@ -437,7 +449,9 @@ struct iwl_trans;
  *	Must be atomic
  * @txq_enable: setup a queue. To setup an AC queue, use the
  *	iwl_trans_ac_txq_enable wrapper. fw_alive must have been called before
- *	this one. The op_mode must not configure the HCMD queue. May sleep.
+ *	this one. The op_mode must not configure the HCMD queue. The scheduler
+ *	configuration may be %NULL, in which case the hardware will not be
+ *	configured. May sleep.
  * @txq_disable: de-configure a Tx queue to send AMPDUs
  *	Must be atomic
  * @wait_tx_queue_empty: wait until tx queues are empty. May sleep.
@@ -492,9 +506,10 @@ struct iwl_trans_ops {
 	void (*reclaim)(struct iwl_trans *trans, int queue, int ssn,
 			struct sk_buff_head *skbs);
 
-	void (*txq_enable)(struct iwl_trans *trans, int queue, int fifo,
-			   int sta_id, int tid, int frame_limit, u16 ssn);
-	void (*txq_disable)(struct iwl_trans *trans, int queue);
+	void (*txq_enable)(struct iwl_trans *trans, int queue, u16 ssn,
+			   const struct iwl_trans_txq_scd_cfg *cfg);
+	void (*txq_disable)(struct iwl_trans *trans, int queue,
+			    bool configure_scd);
 
 	int (*dbgfs_register)(struct iwl_trans *trans, struct dentry* dir);
 	int (*wait_tx_queue_empty)(struct iwl_trans *trans, u32 txq_bm);
@@ -766,29 +781,51 @@ static inline void iwl_trans_reclaim(struct iwl_trans *trans, int queue,
 	trans->ops->reclaim(trans, queue, ssn, skbs);
 }
 
-static inline void iwl_trans_txq_disable(struct iwl_trans *trans, int queue)
+static inline void iwl_trans_txq_disable(struct iwl_trans *trans, int queue,
+					 bool configure_scd)
 {
-	trans->ops->txq_disable(trans, queue);
+	trans->ops->txq_disable(trans, queue, configure_scd);
 }
 
-static inline void iwl_trans_txq_enable(struct iwl_trans *trans, int queue,
-					int fifo, int sta_id, int tid,
-					int frame_limit, u16 ssn)
+static inline void
+iwl_trans_txq_enable_cfg(struct iwl_trans *trans, int queue, u16 ssn,
+			 const struct iwl_trans_txq_scd_cfg *cfg)
 {
 	might_sleep();
 
 	if (unlikely((trans->state != IWL_TRANS_FW_ALIVE)))
 		IWL_ERR(trans, "%s bad state = %d\n", __func__, trans->state);
 
-	trans->ops->txq_enable(trans, queue, fifo, sta_id, tid,
-				 frame_limit, ssn);
+	trans->ops->txq_enable(trans, queue, ssn, cfg);
+}
+
+static inline void iwl_trans_txq_enable(struct iwl_trans *trans, int queue,
+					int fifo, int sta_id, int tid,
+					int frame_limit, u16 ssn)
+{
+	struct iwl_trans_txq_scd_cfg cfg = {
+		.fifo = fifo,
+		.sta_id = sta_id,
+		.tid = tid,
+		.frame_limit = frame_limit,
+		.aggregate = sta_id >= 0,
+	};
+
+	iwl_trans_txq_enable_cfg(trans, queue, ssn, &cfg);
 }
 
 static inline void iwl_trans_ac_txq_enable(struct iwl_trans *trans, int queue,
 					   int fifo)
 {
-	iwl_trans_txq_enable(trans, queue, fifo, -1,
-			     IWL_MAX_TID_COUNT, IWL_FRAME_LIMIT, 0);
+	struct iwl_trans_txq_scd_cfg cfg = {
+		.fifo = fifo,
+		.sta_id = -1,
+		.tid = IWL_MAX_TID_COUNT,
+		.frame_limit = IWL_FRAME_LIMIT,
+		.aggregate = false,
+	};
+
+	iwl_trans_txq_enable_cfg(trans, queue, 0, &cfg);
 }
 
 static inline int iwl_trans_wait_tx_queue_empty(struct iwl_trans *trans,
diff --git a/drivers/net/wireless/iwlwifi/mvm/Makefile b/drivers/net/wireless/iwlwifi/mvm/Makefile
index a28235913c2c..2d7c3ea3c4f8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/Makefile
+++ b/drivers/net/wireless/iwlwifi/mvm/Makefile
@@ -3,7 +3,7 @@ iwlmvm-y += fw.o mac80211.o nvm.o ops.o phy-ctxt.o mac-ctxt.o
 iwlmvm-y += utils.o rx.o tx.o binding.o quota.o sta.o sf.o
 iwlmvm-y += scan.o time-event.o rs.o
 iwlmvm-y += power.o coex.o coex_legacy.o
-iwlmvm-y += tt.o offloading.o
+iwlmvm-y += tt.o offloading.o tdls.o
 iwlmvm-$(CONFIG_IWLWIFI_DEBUGFS) += debugfs.o debugfs-vif.o
 iwlmvm-$(CONFIG_IWLWIFI_LEDS) += led.o
 iwlmvm-$(CONFIG_PM_SLEEP) += d3.o
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex.c b/drivers/net/wireless/iwlwifi/mvm/coex.c
index 2291bbcaaeab..8df2021f9856 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -585,8 +587,6 @@ int iwl_send_bt_init_conf(struct iwl_mvm *mvm)
 	lockdep_assert_held(&mvm->mutex);
 
 	if (unlikely(mvm->bt_force_ant_mode != BT_FORCE_ANT_DIS)) {
-		u32 mode;
-
 		switch (mvm->bt_force_ant_mode) {
 		case BT_FORCE_ANT_BT:
 			mode = BT_COEX_BT;
@@ -756,7 +756,8 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
 	struct iwl_bt_iterator_data *data = _data;
 	struct iwl_mvm *mvm = data->mvm;
 	struct ieee80211_chanctx_conf *chanctx_conf;
-	enum ieee80211_smps_mode smps_mode;
+	/* default smps_mode is AUTOMATIC - only used for client modes */
+	enum ieee80211_smps_mode smps_mode = IEEE80211_SMPS_AUTOMATIC;
 	u32 bt_activity_grading;
 	int ave_rssi;
 
@@ -764,8 +765,6 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
 
 	switch (vif->type) {
 	case NL80211_IFTYPE_STATION:
-		/* default smps_mode for BSS / P2P client is AUTOMATIC */
-		smps_mode = IEEE80211_SMPS_AUTOMATIC;
 		break;
 	case NL80211_IFTYPE_AP:
 		if (!mvmvif->ap_ibss_active)
@@ -797,7 +796,7 @@ static void iwl_mvm_bt_notif_iterator(void *_data, u8 *mac,
 	else if (bt_activity_grading >= BT_LOW_TRAFFIC)
 		smps_mode = IEEE80211_SMPS_DYNAMIC;
 
-	/* relax SMPS contraints for next association */
+	/* relax SMPS constraints for next association */
 	if (!vif->bss_conf.assoc)
 		smps_mode = IEEE80211_SMPS_AUTOMATIC;
 
@@ -1147,6 +1146,10 @@ bool iwl_mvm_bt_coex_is_mimo_allowed(struct iwl_mvm *mvm,
 
 bool iwl_mvm_bt_coex_is_shared_ant_avail(struct iwl_mvm *mvm)
 {
+	/* there is no other antenna, shared antenna is always available */
+	if (mvm->cfg->bt_shared_single_ant)
+		return true;
+
 	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_BT_COEX_SPLIT))
 		return iwl_mvm_bt_coex_is_shared_ant_avail_old(mvm);
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
index a3be33359927..585c0ab4a3ec 100644
--- a/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
+++ b/drivers/net/wireless/iwlwifi/mvm/coex_legacy.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/constants.h b/drivers/net/wireless/iwlwifi/mvm/constants.h
index ca79f7160573..d4dfbe4cb66d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/constants.h
+++ b/drivers/net/wireless/iwlwifi/mvm/constants.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -63,12 +65,18 @@
 #ifndef __MVM_CONSTANTS_H
 #define __MVM_CONSTANTS_H
 
+#include <linux/ieee80211.h>
+
 #define IWL_MVM_DEFAULT_PS_TX_DATA_TIMEOUT	(100 * USEC_PER_MSEC)
 #define IWL_MVM_DEFAULT_PS_RX_DATA_TIMEOUT	(100 * USEC_PER_MSEC)
 #define IWL_MVM_WOWLAN_PS_TX_DATA_TIMEOUT	(10 * USEC_PER_MSEC)
 #define IWL_MVM_WOWLAN_PS_RX_DATA_TIMEOUT	(10 * USEC_PER_MSEC)
 #define IWL_MVM_UAPSD_RX_DATA_TIMEOUT		(50 * USEC_PER_MSEC)
 #define IWL_MVM_UAPSD_TX_DATA_TIMEOUT		(50 * USEC_PER_MSEC)
+#define IWL_MVM_UAPSD_QUEUES		(IEEE80211_WMM_IE_STA_QOSINFO_AC_VO |\
+					 IEEE80211_WMM_IE_STA_QOSINFO_AC_VI |\
+					 IEEE80211_WMM_IE_STA_QOSINFO_AC_BK |\
+					 IEEE80211_WMM_IE_STA_QOSINFO_AC_BE)
 #define IWL_MVM_PS_HEAVY_TX_THLD_PACKETS	20
 #define IWL_MVM_PS_HEAVY_RX_THLD_PACKETS	8
 #define IWL_MVM_PS_SNOOZE_HEAVY_TX_THLD_PACKETS	30
@@ -82,7 +90,10 @@
 #define IWL_MVM_BT_COEX_EN_RED_TXP_THRESH	62
 #define IWL_MVM_BT_COEX_DIS_RED_TXP_THRESH	65
 #define IWL_MVM_BT_COEX_SYNC2SCO		1
-#define IWL_MVM_BT_COEX_CORUNNING		1
+#define IWL_MVM_BT_COEX_CORUNNING		0
 #define IWL_MVM_BT_COEX_MPLUT			1
+#define IWL_MVM_FW_MCAST_FILTER_PASS_ALL	0
+#define IWL_MVM_QUOTA_THRESHOLD			8
+#define IWL_MVM_RS_RSSI_BASED_INIT_RATE         0
 
 #endif /* __MVM_CONSTANTS_H */
diff --git a/drivers/net/wireless/iwlwifi/mvm/d3.c b/drivers/net/wireless/iwlwifi/mvm/d3.c
index 645b3cfc29a5..c17be0fb7283 100644
--- a/drivers/net/wireless/iwlwifi/mvm/d3.c
+++ b/drivers/net/wireless/iwlwifi/mvm/d3.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -700,7 +702,7 @@ static int iwl_mvm_d3_reprogram(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 		return ret;
 	rcu_assign_pointer(mvm->fw_id_to_mac_id[mvmvif->ap_sta_id], ap_sta);
 
-	ret = iwl_mvm_mac_ctxt_changed(mvm, vif, false);
+	ret = iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
 	if (ret)
 		return ret;
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
index 2e90ff795c13..9aa2311a776c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs-vif.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -74,8 +76,7 @@ static void iwl_dbgfs_update_pm(struct iwl_mvm *mvm,
 
 	switch (param) {
 	case MVM_DEBUGFS_PM_KEEP_ALIVE: {
-		struct ieee80211_hw *hw = mvm->hw;
-		int dtimper = hw->conf.ps_dtim_period ?: 1;
+		int dtimper = vif->bss_conf.dtim_period ?: 1;
 		int dtimper_msec = dtimper * vif->bss_conf.beacon_int;
 
 		IWL_DEBUG_POWER(mvm, "debugfs: set keep_alive= %d sec\n", val);
@@ -119,6 +120,10 @@ static void iwl_dbgfs_update_pm(struct iwl_mvm *mvm,
 		IWL_DEBUG_POWER(mvm, "uapsd_misbehaving_enable=%d\n", val);
 		dbgfs_pm->uapsd_misbehaving = val;
 		break;
+	case MVM_DEBUGFS_PM_USE_PS_POLL:
+		IWL_DEBUG_POWER(mvm, "use_ps_poll=%d\n", val);
+		dbgfs_pm->use_ps_poll = val;
+		break;
 	}
 }
 
@@ -169,6 +174,10 @@ static ssize_t iwl_dbgfs_pm_params_write(struct ieee80211_vif *vif, char *buf,
 		if (sscanf(buf + 18, "%d", &val) != 1)
 			return -EINVAL;
 		param = MVM_DEBUGFS_PM_UAPSD_MISBEHAVING;
+	} else if (!strncmp("use_ps_poll=", buf, 12)) {
+		if (sscanf(buf + 12, "%d", &val) != 1)
+			return -EINVAL;
+		param = MVM_DEBUGFS_PM_USE_PS_POLL;
 	} else {
 		return -EINVAL;
 	}
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.c b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
index 7d18f466fbb3..50527a9bb267 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -257,6 +259,96 @@ static ssize_t iwl_dbgfs_sram_write(struct iwl_mvm *mvm, char *buf,
 	return count;
 }
 
+static ssize_t iwl_dbgfs_set_nic_temperature_read(struct file *file,
+						  char __user *user_buf,
+						  size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	char buf[16];
+	int pos;
+
+	if (!mvm->temperature_test)
+		pos = scnprintf(buf , sizeof(buf), "disabled\n");
+	else
+		pos = scnprintf(buf , sizeof(buf), "%d\n", mvm->temperature);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
+/*
+ * Set NIC Temperature
+ * Cause the driver to ignore the actual NIC temperature reported by the FW
+ * Enable: any value between IWL_MVM_DEBUG_SET_TEMPERATURE_MIN -
+ * IWL_MVM_DEBUG_SET_TEMPERATURE_MAX
+ * Disable: IWL_MVM_DEBUG_SET_TEMPERATURE_DISABLE
+ */
+static ssize_t iwl_dbgfs_set_nic_temperature_write(struct iwl_mvm *mvm,
+						   char *buf, size_t count,
+						   loff_t *ppos)
+{
+	int temperature;
+
+	if (!mvm->ucode_loaded && !mvm->temperature_test)
+		return -EIO;
+
+	if (kstrtoint(buf, 10, &temperature))
+		return -EINVAL;
+	/* not a legal temperature */
+	if ((temperature > IWL_MVM_DEBUG_SET_TEMPERATURE_MAX &&
+	     temperature != IWL_MVM_DEBUG_SET_TEMPERATURE_DISABLE) ||
+	    temperature < IWL_MVM_DEBUG_SET_TEMPERATURE_MIN)
+		return -EINVAL;
+
+	mutex_lock(&mvm->mutex);
+	if (temperature == IWL_MVM_DEBUG_SET_TEMPERATURE_DISABLE) {
+		if (!mvm->temperature_test)
+			goto out;
+
+		mvm->temperature_test = false;
+		/* Since we can't read the temp while awake, just set
+		 * it to zero until we get the next RX stats from the
+		 * firmware.
+		 */
+		mvm->temperature = 0;
+	} else {
+		mvm->temperature_test = true;
+		mvm->temperature = temperature;
+	}
+	IWL_DEBUG_TEMP(mvm, "%sabling debug set temperature (temp = %d)\n",
+		       mvm->temperature_test ? "En" : "Dis" ,
+		       mvm->temperature);
+	/* handle the temperature change */
+	iwl_mvm_tt_handler(mvm);
+
+out:
+	mutex_unlock(&mvm->mutex);
+
+	return count;
+}
+
+static ssize_t iwl_dbgfs_nic_temp_read(struct file *file,
+				       char __user *user_buf,
+				       size_t count, loff_t *ppos)
+{
+	struct iwl_mvm *mvm = file->private_data;
+	char buf[16];
+	int pos, temp;
+
+	if (!mvm->ucode_loaded)
+		return -EIO;
+
+	mutex_lock(&mvm->mutex);
+	temp = iwl_mvm_get_temp(mvm);
+	mutex_unlock(&mvm->mutex);
+
+	if (temp < 0)
+		return temp;
+
+	pos = scnprintf(buf , sizeof(buf), "%d\n", temp);
+
+	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+}
+
 static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
 				       size_t count, loff_t *ppos)
 {
@@ -1190,6 +1282,18 @@ static ssize_t iwl_dbgfs_d0i3_refs_read(struct file *file,
 	PRINT_MVM_REF(IWL_MVM_REF_P2P_CLIENT);
 	PRINT_MVM_REF(IWL_MVM_REF_AP_IBSS);
 	PRINT_MVM_REF(IWL_MVM_REF_USER);
+	PRINT_MVM_REF(IWL_MVM_REF_TX);
+	PRINT_MVM_REF(IWL_MVM_REF_TX_AGG);
+	PRINT_MVM_REF(IWL_MVM_REF_ADD_IF);
+	PRINT_MVM_REF(IWL_MVM_REF_START_AP);
+	PRINT_MVM_REF(IWL_MVM_REF_BSS_CHANGED);
+	PRINT_MVM_REF(IWL_MVM_REF_PREPARE_TX);
+	PRINT_MVM_REF(IWL_MVM_REF_PROTECT_TDLS);
+	PRINT_MVM_REF(IWL_MVM_REF_CHECK_CTKILL);
+	PRINT_MVM_REF(IWL_MVM_REF_PRPH_READ);
+	PRINT_MVM_REF(IWL_MVM_REF_PRPH_WRITE);
+	PRINT_MVM_REF(IWL_MVM_REF_NMI);
+	PRINT_MVM_REF(IWL_MVM_REF_TM_CMD);
 	PRINT_MVM_REF(IWL_MVM_REF_EXIT_WORK);
 
 	return simple_read_from_buffer(user_buf, count, ppos, buf, pos);
@@ -1296,6 +1400,8 @@ MVM_DEBUGFS_READ_WRITE_FILE_OPS(prph_reg, 64);
 MVM_DEBUGFS_WRITE_FILE_OPS(tx_flush, 16);
 MVM_DEBUGFS_WRITE_FILE_OPS(sta_drain, 8);
 MVM_DEBUGFS_READ_WRITE_FILE_OPS(sram, 64);
+MVM_DEBUGFS_READ_WRITE_FILE_OPS(set_nic_temperature, 64);
+MVM_DEBUGFS_READ_FILE_OPS(nic_temp);
 MVM_DEBUGFS_READ_FILE_OPS(stations);
 MVM_DEBUGFS_READ_FILE_OPS(bt_notif);
 MVM_DEBUGFS_READ_FILE_OPS(bt_cmd);
@@ -1336,6 +1442,9 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 	MVM_DEBUGFS_ADD_FILE(tx_flush, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(sta_drain, mvm->debugfs_dir, S_IWUSR);
 	MVM_DEBUGFS_ADD_FILE(sram, mvm->debugfs_dir, S_IWUSR | S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE(set_nic_temperature, mvm->debugfs_dir,
+			     S_IWUSR | S_IRUSR);
+	MVM_DEBUGFS_ADD_FILE(nic_temp, dbgfs_dir, S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(stations, dbgfs_dir, S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(fw_error_dump, dbgfs_dir, S_IRUSR);
 	MVM_DEBUGFS_ADD_FILE(bt_notif, dbgfs_dir, S_IRUSR);
@@ -1380,6 +1489,13 @@ int iwl_mvm_dbgfs_register(struct iwl_mvm *mvm, struct dentry *dbgfs_dir)
 		goto err;
 #endif
 
+	if (!debugfs_create_u8("low_latency_agg_frame_limit", S_IRUSR | S_IWUSR,
+			       mvm->debugfs_dir,
+			       &mvm->low_latency_agg_frame_limit))
+		goto err;
+	if (!debugfs_create_u8("ps_disabled", S_IRUSR,
+			       mvm->debugfs_dir, &mvm->ps_disabled))
+		goto err;
 	if (!debugfs_create_blob("nvm_hw", S_IRUSR,
 				  mvm->debugfs_dir, &mvm->nvm_hw_blob))
 		goto err;
diff --git a/drivers/net/wireless/iwlwifi/mvm/debugfs.h b/drivers/net/wireless/iwlwifi/mvm/debugfs.h
index e3a9774af495..8c4190e7e027 100644
--- a/drivers/net/wireless/iwlwifi/mvm/debugfs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/debugfs.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h
index 69875716dcdb..816883f9ff94 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-coex.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
index 13696fe419b7..e74cdf2132f8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-d3.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
index c3a8c86b550d..27dd86395b39 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-power.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
index c02a9e45ec5e..1354c68f6468 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-scan.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -668,6 +670,8 @@ struct iwl_scan_channel_opt {
  * @IWL_MVM_LMAC_SCAN_FLAG_ITER_COMPLETE: send iteration complete notification
  * @IWL_MVM_LMAC_SCAN_FLAG_MULTIPLE_SSIDS multiple SSID matching
  * @IWL_MVM_LMAC_SCAN_FLAG_FRAGMENTED: all passive scans will be fragmented
+ * @IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED: insert WFA vendor-specific TPC report
+ *	and DS parameter set IEs into probe requests.
  */
 enum iwl_mvm_lmac_scan_flags {
 	IWL_MVM_LMAC_SCAN_FLAG_PASS_ALL		= BIT(0),
@@ -676,6 +680,7 @@ enum iwl_mvm_lmac_scan_flags {
 	IWL_MVM_LMAC_SCAN_FLAG_ITER_COMPLETE	= BIT(3),
 	IWL_MVM_LMAC_SCAN_FLAG_MULTIPLE_SSIDS	= BIT(4),
 	IWL_MVM_LMAC_SCAN_FLAG_FRAGMENTED	= BIT(5),
+	IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED	= BIT(6),
 };
 
 enum iwl_scan_priority {
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
index 47bd0406355d..21dd5b771660 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-sta.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h b/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h
index d6073f67b212..5bca1f8bfebf 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api-tx.h
@@ -66,6 +66,7 @@
 /**
  * enum iwl_tx_flags - bitmasks for tx_flags in TX command
  * @TX_CMD_FLG_PROT_REQUIRE: use RTS or CTS-to-self to protect the frame
+ * @TX_CMD_FLG_WRITE_TX_POWER: update current tx power value in the mgmt frame
  * @TX_CMD_FLG_ACK: expect ACK from receiving station
  * @TX_CMD_FLG_STA_RATE: use RS table with initial index from the TX command.
  *	Otherwise, use rate_n_flags from the TX command
@@ -97,6 +98,7 @@
  */
 enum iwl_tx_flags {
 	TX_CMD_FLG_PROT_REQUIRE		= BIT(0),
+	TX_CMD_FLG_WRITE_TX_POWER	= BIT(1),
 	TX_CMD_FLG_ACK			= BIT(3),
 	TX_CMD_FLG_STA_RATE		= BIT(4),
 	TX_CMD_FLG_BAR			= BIT(6),
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw-api.h b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
index 95f5b3274efb..667a92274c87 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw-api.h
+++ b/drivers/net/wireless/iwlwifi/mvm/fw-api.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -73,16 +75,20 @@
 #include "fw-api-coex.h"
 #include "fw-api-scan.h"
 
-/* maximal number of Tx queues in any platform */
-#define IWL_MVM_MAX_QUEUES	20
-
 /* Tx queue numbers */
 enum {
 	IWL_MVM_OFFCHANNEL_QUEUE = 8,
 	IWL_MVM_CMD_QUEUE = 9,
 };
 
-#define IWL_MVM_CMD_FIFO	7
+enum iwl_mvm_tx_fifo {
+	IWL_MVM_TX_FIFO_BK = 0,
+	IWL_MVM_TX_FIFO_BE,
+	IWL_MVM_TX_FIFO_VI,
+	IWL_MVM_TX_FIFO_VO,
+	IWL_MVM_TX_FIFO_MCAST = 5,
+	IWL_MVM_TX_FIFO_CMD = 7,
+};
 
 #define IWL_MVM_STATION_COUNT	16
 
@@ -110,6 +116,9 @@ enum {
 	TXPATH_FLUSH = 0x1e,
 	MGMT_MCAST_KEY = 0x1f,
 
+	/* scheduler config */
+	SCD_QUEUE_CFG = 0x1d,
+
 	/* global key */
 	WEP_KEY = 0x20,
 
@@ -184,6 +193,8 @@ enum {
 	REPLY_RX_MPDU_CMD = 0xc1,
 	BA_NOTIF = 0xc5,
 
+	MARKER_CMD = 0xcb,
+
 	/* BT Coex */
 	BT_COEX_PRIO_TABLE = 0xcc,
 	BT_COEX_PROT_ENV = 0xcd,
@@ -197,6 +208,10 @@ enum {
 	REPLY_SF_CFG_CMD = 0xd1,
 	REPLY_BEACON_FILTERING_CMD = 0xd2,
 
+	/* DTS measurements */
+	CMD_DTS_MEASUREMENT_TRIGGER = 0xdc,
+	DTS_MEASUREMENT_NOTIFICATION = 0xdd,
+
 	REPLY_DEBUG_CMD = 0xf0,
 	DEBUG_LOG_MSG = 0xf7,
 
@@ -542,7 +557,7 @@ enum iwl_time_event_type {
 	TE_WIDI_TX_SYNC,
 
 	/* Channel Switch NoA */
-	TE_P2P_GO_CSA_NOA,
+	TE_CHANNEL_SWITCH_PERIOD,
 
 	TE_MAX
 }; /* MAC_EVENT_TYPE_API_E_VER_1 */
@@ -1307,6 +1322,38 @@ struct iwl_bcast_filter_cmd {
 	struct iwl_fw_bcast_mac macs[NUM_MAC_INDEX_DRIVER];
 } __packed; /* BCAST_FILTERING_HCMD_API_S_VER_1 */
 
+/*
+ * enum iwl_mvm_marker_id - maker ids
+ *
+ * The ids for different type of markers to insert into the usniffer logs
+ */
+enum iwl_mvm_marker_id {
+	MARKER_ID_TX_FRAME_LATENCY = 1,
+}; /* MARKER_ID_API_E_VER_1 */
+
+/**
+ * struct iwl_mvm_marker - mark info into the usniffer logs
+ *
+ * (MARKER_CMD = 0xcb)
+ *
+ * Mark the UTC time stamp into the usniffer logs together with additional
+ * metadata, so the usniffer output can be parsed.
+ * In the command response the ucode will return the GP2 time.
+ *
+ * @dw_len: The amount of dwords following this byte including this byte.
+ * @marker_id: A unique marker id (iwl_mvm_marker_id).
+ * @reserved: reserved.
+ * @timestamp: in milliseconds since 1970-01-01 00:00:00 UTC
+ * @metadata: additional meta data that will be written to the unsiffer log
+ */
+struct iwl_mvm_marker {
+	u8 dwLen;
+	u8 markerId;
+	__le16 reserved;
+	__le64 timestamp;
+	__le32 metadata[0];
+} __packed; /* MARKER_API_S_VER_1 */
+
 struct mvm_statistics_dbg {
 	__le32 burst_check;
 	__le32 burst_count;
@@ -1561,19 +1608,106 @@ enum iwl_sf_scenario {
 
 #define SF_LONG_DELAY_AGING_TIMER 1000000	/* 1 Sec */
 
+#define SF_CFG_DUMMY_NOTIF_OFF	BIT(16)
+
 /**
  * Smart Fifo configuration command.
- * @state: smart fifo state, types listed in iwl_sf_sate.
+ * @state: smart fifo state, types listed in enum %iwl_sf_sate.
  * @watermark: Minimum allowed availabe free space in RXF for transient state.
  * @long_delay_timeouts: aging and idle timer values for each scenario
  * in long delay state.
  * @full_on_timeouts: timer values for each scenario in full on state.
  */
 struct iwl_sf_cfg_cmd {
-	enum iwl_sf_state state;
+	__le32 state;
 	__le32 watermark[SF_TRANSIENT_STATES_NUMBER];
 	__le32 long_delay_timeouts[SF_NUM_SCENARIO][SF_NUM_TIMEOUT_TYPES];
 	__le32 full_on_timeouts[SF_NUM_SCENARIO][SF_NUM_TIMEOUT_TYPES];
 } __packed; /* SF_CFG_API_S_VER_2 */
 
+/* DTS measurements */
+
+enum iwl_dts_measurement_flags {
+	DTS_TRIGGER_CMD_FLAGS_TEMP	= BIT(0),
+	DTS_TRIGGER_CMD_FLAGS_VOLT	= BIT(1),
+};
+
+/**
+ * iwl_dts_measurement_cmd - request DTS temperature and/or voltage measurements
+ *
+ * @flags: indicates which measurements we want as specified in &enum
+ *	   iwl_dts_measurement_flags
+ */
+struct iwl_dts_measurement_cmd {
+	__le32 flags;
+} __packed; /* TEMPERATURE_MEASUREMENT_TRIGGER_CMD_S */
+
+/**
+ * iwl_dts_measurement_notif - notification received with the measurements
+ *
+ * @temp: the measured temperature
+ * @voltage: the measured voltage
+ */
+struct iwl_dts_measurement_notif {
+	__le32 temp;
+	__le32 voltage;
+} __packed; /* TEMPERATURE_MEASUREMENT_TRIGGER_NTFY_S */
+
+/**
+ * enum iwl_scd_control - scheduler config command control flags
+ * @IWL_SCD_CONTROL_RM_TID: remove TID from this queue
+ * @IWL_SCD_CONTROL_SET_SSN: use the SSN and program it into HW
+ */
+enum iwl_scd_control {
+	IWL_SCD_CONTROL_RM_TID	= BIT(4),
+	IWL_SCD_CONTROL_SET_SSN	= BIT(5),
+};
+
+/**
+ * enum iwl_scd_flags - scheduler config command flags
+ * @IWL_SCD_FLAGS_SHARE_TID: multiple TIDs map to this queue
+ * @IWL_SCD_FLAGS_SHARE_RA: multiple RAs map to this queue
+ * @IWL_SCD_FLAGS_DQA_ENABLED: DQA is enabled
+ */
+enum iwl_scd_flags {
+	IWL_SCD_FLAGS_SHARE_TID		= BIT(0),
+	IWL_SCD_FLAGS_SHARE_RA		= BIT(1),
+	IWL_SCD_FLAGS_DQA_ENABLED	= BIT(2),
+};
+
+#define IWL_SCDQ_INVALID_STA	0xff
+
+/**
+ * struct iwl_scd_txq_cfg_cmd - New txq hw scheduler config command
+ * @token:	dialog token addba - unused legacy
+ * @sta_id:	station id 4-bit
+ * @tid:	TID 0..7
+ * @scd_queue:	TFD queue num 0 .. 31
+ * @enable:	1 queue enable, 0 queue disable
+ * @aggregate:	1 aggregated queue, 0 otherwise
+ * @tx_fifo:	tx fifo num 0..7
+ * @window:	up to 64
+ * @ssn:	starting seq num 12-bit
+ * @control:	command control flags
+ * @flags:	flags - see &enum iwl_scd_flags
+ *
+ * Note that every time the command is sent, all parameters must
+ * be filled with the exception of
+ *  - the SSN, which is only used with @IWL_SCD_CONTROL_SET_SSN
+ *  - the window, which is only relevant when starting aggregation
+ */
+struct iwl_scd_txq_cfg_cmd {
+	u8 token;
+	u8 sta_id;
+	u8 tid;
+	u8 scd_queue;
+	u8 enable;
+	u8 aggregate;
+	u8 tx_fifo;
+	u8 window;
+	__le16 ssn;
+	u8 control;
+	u8 flags;
+} __packed;
+
 #endif /* __fw_api_h__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/fw.c b/drivers/net/wireless/iwlwifi/mvm/fw.c
index 883e702152d5..23fd711a67e4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/fw.c
+++ b/drivers/net/wireless/iwlwifi/mvm/fw.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -242,10 +244,10 @@ static int iwl_mvm_load_ucode_wait_alive(struct iwl_mvm *mvm,
 			mvm->queue_to_mac80211[i] = i;
 		else
 			mvm->queue_to_mac80211[i] = IWL_INVALID_MAC80211_QUEUE;
-		atomic_set(&mvm->queue_stop_count[i], 0);
 	}
 
-	mvm->transport_queue_stop = 0;
+	for (i = 0; i < IEEE80211_MAX_QUEUES; i++)
+		atomic_set(&mvm->mac80211_queue_stop_count[i], 0);
 
 	mvm->ucode_loaded = true;
 
@@ -452,6 +454,9 @@ int iwl_mvm_up(struct iwl_mvm *mvm)
 	for (i = 0; i < IWL_MVM_STATION_COUNT; i++)
 		RCU_INIT_POINTER(mvm->fw_id_to_mac_id[i], NULL);
 
+	/* reset quota debouncing buffer - 0xff will yield invalid data */
+	memset(&mvm->last_quota_cmd, 0xff, sizeof(mvm->last_quota_cmd));
+
 	/* Add auxiliary station for scanning */
 	ret = iwl_mvm_add_aux_sta(mvm);
 	if (ret)
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
index 0e523e28cabf..0c5c0b0e23f5 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac-ctxt.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -81,7 +83,7 @@ struct iwl_mvm_mac_iface_iterator_data {
 	struct ieee80211_vif *vif;
 	unsigned long available_mac_ids[BITS_TO_LONGS(NUM_MAC_INDEX_DRIVER)];
 	unsigned long available_tsf_ids[BITS_TO_LONGS(NUM_TSF_IDS)];
-	unsigned long used_hw_queues[BITS_TO_LONGS(IWL_MVM_MAX_QUEUES)];
+	u32 used_hw_queues;
 	enum iwl_tsf_id preferred_tsf;
 	bool found_vif;
 };
@@ -192,12 +194,31 @@ static void iwl_mvm_mac_tsf_id_iter(void *_data, u8 *mac,
 		data->preferred_tsf = NUM_TSF_IDS;
 }
 
+/*
+ * Get the mask of the queues used by the vif
+ */
+u32 iwl_mvm_mac_get_queues_mask(struct iwl_mvm *mvm,
+				struct ieee80211_vif *vif)
+{
+	u32 qmask = 0, ac;
+
+	if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
+		return BIT(IWL_MVM_OFFCHANNEL_QUEUE);
+
+	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
+		qmask |= BIT(vif->hw_queue[ac]);
+
+	if (vif->type == NL80211_IFTYPE_AP)
+		qmask |= BIT(vif->cab_queue);
+
+	return qmask;
+}
+
 static void iwl_mvm_mac_iface_iterator(void *_data, u8 *mac,
 				       struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_mac_iface_iterator_data *data = _data;
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	u32 ac;
 
 	/* Iterator may already find the interface being added -- skip it */
 	if (vif == data->vif) {
@@ -206,12 +227,7 @@ static void iwl_mvm_mac_iface_iterator(void *_data, u8 *mac,
 	}
 
 	/* Mark the queues used by the vif */
-	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
-		if (vif->hw_queue[ac] != IEEE80211_INVAL_HW_QUEUE)
-			__set_bit(vif->hw_queue[ac], data->used_hw_queues);
-
-	if (vif->cab_queue != IEEE80211_INVAL_HW_QUEUE)
-		__set_bit(vif->cab_queue, data->used_hw_queues);
+	data->used_hw_queues |= iwl_mvm_mac_get_queues_mask(data->mvm, vif);
 
 	/* Mark MAC IDs as used by clearing the available bit, and
 	 * (below) mark TSFs as used if their existing use is not
@@ -225,24 +241,6 @@ static void iwl_mvm_mac_iface_iterator(void *_data, u8 *mac,
 	iwl_mvm_mac_tsf_id_iter(_data, mac, vif);
 }
 
-/*
- * Get the mask of the queus used by the vif
- */
-u32 iwl_mvm_mac_get_queues_mask(struct iwl_mvm *mvm,
-				struct ieee80211_vif *vif)
-{
-	u32 qmask = 0, ac;
-
-	if (vif->type == NL80211_IFTYPE_P2P_DEVICE)
-		return BIT(IWL_MVM_OFFCHANNEL_QUEUE);
-
-	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
-		if (vif->hw_queue[ac] != IEEE80211_INVAL_HW_QUEUE)
-			qmask |= BIT(vif->hw_queue[ac]);
-
-	return qmask;
-}
-
 void iwl_mvm_mac_ctxt_recalc_tsf_id(struct iwl_mvm *mvm,
 				    struct ieee80211_vif *vif)
 {
@@ -277,15 +275,15 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
 		.available_tsf_ids = { (1 << NUM_TSF_IDS) - 1 },
 		/* no preference yet */
 		.preferred_tsf = NUM_TSF_IDS,
-		.used_hw_queues = {
+		.used_hw_queues =
 			BIT(IWL_MVM_OFFCHANNEL_QUEUE) |
 			BIT(mvm->aux_queue) |
-			BIT(IWL_MVM_CMD_QUEUE)
-		},
+			BIT(IWL_MVM_CMD_QUEUE),
 		.found_vif = false,
 	};
 	u32 ac;
 	int ret, i;
+	unsigned long used_hw_queues;
 
 	/*
 	 * Allocate a MAC ID and a TSF for this MAC, along with the queues
@@ -368,9 +366,11 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
 		return 0;
 	}
 
+	used_hw_queues = data.used_hw_queues;
+
 	/* Find available queues, and allocate them to the ACs */
 	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
-		u8 queue = find_first_zero_bit(data.used_hw_queues,
+		u8 queue = find_first_zero_bit(&used_hw_queues,
 					       mvm->first_agg_queue);
 
 		if (queue >= mvm->first_agg_queue) {
@@ -379,13 +379,13 @@ static int iwl_mvm_mac_ctxt_allocate_resources(struct iwl_mvm *mvm,
 			goto exit_fail;
 		}
 
-		__set_bit(queue, data.used_hw_queues);
+		__set_bit(queue, &used_hw_queues);
 		vif->hw_queue[ac] = queue;
 	}
 
 	/* Allocate the CAB queue for softAP and GO interfaces */
 	if (vif->type == NL80211_IFTYPE_AP) {
-		u8 queue = find_first_zero_bit(data.used_hw_queues,
+		u8 queue = find_first_zero_bit(&used_hw_queues,
 					       mvm->first_agg_queue);
 
 		if (queue >= mvm->first_agg_queue) {
@@ -427,17 +427,17 @@ int iwl_mvm_mac_ctxt_init(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 	switch (vif->type) {
 	case NL80211_IFTYPE_P2P_DEVICE:
-		iwl_trans_ac_txq_enable(mvm->trans, IWL_MVM_OFFCHANNEL_QUEUE,
-					IWL_MVM_TX_FIFO_VO);
+		iwl_mvm_enable_ac_txq(mvm, IWL_MVM_OFFCHANNEL_QUEUE,
+				      IWL_MVM_TX_FIFO_VO);
 		break;
 	case NL80211_IFTYPE_AP:
-		iwl_trans_ac_txq_enable(mvm->trans, vif->cab_queue,
-					IWL_MVM_TX_FIFO_MCAST);
+		iwl_mvm_enable_ac_txq(mvm, vif->cab_queue,
+				      IWL_MVM_TX_FIFO_MCAST);
 		/* fall through */
 	default:
 		for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
-			iwl_trans_ac_txq_enable(mvm->trans, vif->hw_queue[ac],
-						iwl_mvm_ac_to_tx_fifo[ac]);
+			iwl_mvm_enable_ac_txq(mvm, vif->hw_queue[ac],
+					      iwl_mvm_ac_to_tx_fifo[ac]);
 		break;
 	}
 
@@ -452,14 +452,14 @@ void iwl_mvm_mac_ctxt_release(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 
 	switch (vif->type) {
 	case NL80211_IFTYPE_P2P_DEVICE:
-		iwl_trans_txq_disable(mvm->trans, IWL_MVM_OFFCHANNEL_QUEUE);
+		iwl_mvm_disable_txq(mvm, IWL_MVM_OFFCHANNEL_QUEUE);
 		break;
 	case NL80211_IFTYPE_AP:
-		iwl_trans_txq_disable(mvm->trans, vif->cab_queue);
+		iwl_mvm_disable_txq(mvm, vif->cab_queue);
 		/* fall through */
 	default:
 		for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
-			iwl_trans_txq_disable(mvm->trans, vif->hw_queue[ac]);
+			iwl_mvm_disable_txq(mvm, vif->hw_queue[ac]);
 	}
 }
 
@@ -586,6 +586,7 @@ static void iwl_mvm_mac_ctxt_set_ht_flags(struct iwl_mvm *mvm,
 static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
 					struct ieee80211_vif *vif,
 					struct iwl_mac_ctx_cmd *cmd,
+					const u8 *bssid_override,
 					u32 action)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
@@ -593,6 +594,7 @@ static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
 	bool ht_enabled = !!(vif->bss_conf.ht_operation_mode &
 			     IEEE80211_HT_OP_MODE_PROTECTION);
 	u8 cck_ack_rates, ofdm_ack_rates;
+	const u8 *bssid = bssid_override ?: vif->bss_conf.bssid;
 	int i;
 
 	cmd->id_and_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id,
@@ -625,8 +627,9 @@ static void iwl_mvm_mac_ctxt_cmd_common(struct iwl_mvm *mvm,
 	cmd->tsf_id = cpu_to_le32(mvmvif->tsf_id);
 
 	memcpy(cmd->node_addr, vif->addr, ETH_ALEN);
-	if (vif->bss_conf.bssid)
-		memcpy(cmd->bssid_addr, vif->bss_conf.bssid, ETH_ALEN);
+
+	if (bssid)
+		memcpy(cmd->bssid_addr, bssid, ETH_ALEN);
 	else
 		eth_broadcast_addr(cmd->bssid_addr);
 
@@ -695,7 +698,8 @@ static int iwl_mvm_mac_ctxt_send_cmd(struct iwl_mvm *mvm,
 
 static int iwl_mvm_mac_ctxt_cmd_sta(struct iwl_mvm *mvm,
 				    struct ieee80211_vif *vif,
-				    u32 action, bool force_assoc_off)
+				    u32 action, bool force_assoc_off,
+				    const u8 *bssid_override)
 {
 	struct iwl_mac_ctx_cmd cmd = {};
 	struct iwl_mac_data_sta *ctxt_sta;
@@ -703,7 +707,7 @@ static int iwl_mvm_mac_ctxt_cmd_sta(struct iwl_mvm *mvm,
 	WARN_ON(vif->type != NL80211_IFTYPE_STATION);
 
 	/* Fill the common data for all mac context types */
-	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
+	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, bssid_override, action);
 
 	if (vif->p2p) {
 		struct ieee80211_p2p_noa_attr *noa =
@@ -721,11 +725,6 @@ static int iwl_mvm_mac_ctxt_cmd_sta(struct iwl_mvm *mvm,
 	    !force_assoc_off) {
 		u32 dtim_offs;
 
-		/* Allow beacons to pass through as long as we are not
-		 * associated, or we do not have dtim period information.
-		 */
-		cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_BEACON);
-
 		/*
 		 * The DTIM count counts down, so when it is N that means N
 		 * more beacon intervals happen until the DTIM TBTT. Therefore
@@ -759,6 +758,11 @@ static int iwl_mvm_mac_ctxt_cmd_sta(struct iwl_mvm *mvm,
 		ctxt_sta->is_assoc = cpu_to_le32(1);
 	} else {
 		ctxt_sta->is_assoc = cpu_to_le32(0);
+
+		/* Allow beacons to pass through as long as we are not
+		 * associated, or we do not have dtim period information.
+		 */
+		cmd.filter_flags |= cpu_to_le32(MAC_FILTER_IN_BEACON);
 	}
 
 	ctxt_sta->bi = cpu_to_le32(vif->bss_conf.beacon_int);
@@ -784,7 +788,7 @@ static int iwl_mvm_mac_ctxt_cmd_listener(struct iwl_mvm *mvm,
 
 	WARN_ON(vif->type != NL80211_IFTYPE_MONITOR);
 
-	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
+	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
 	cmd.filter_flags = cpu_to_le32(MAC_FILTER_IN_PROMISC |
 				       MAC_FILTER_IN_CONTROL_AND_MGMT |
@@ -805,7 +809,7 @@ static int iwl_mvm_mac_ctxt_cmd_ibss(struct iwl_mvm *mvm,
 
 	WARN_ON(vif->type != NL80211_IFTYPE_ADHOC);
 
-	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
+	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
 	cmd.filter_flags = cpu_to_le32(MAC_FILTER_IN_BEACON |
 				       MAC_FILTER_IN_PROBE_REQUEST);
@@ -844,7 +848,7 @@ static int iwl_mvm_mac_ctxt_cmd_p2p_device(struct iwl_mvm *mvm,
 
 	WARN_ON(vif->type != NL80211_IFTYPE_P2P_DEVICE);
 
-	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
+	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
 	cmd.protection_flags |= cpu_to_le32(MAC_PROT_FLG_TGG_PROTECT);
 
@@ -1072,7 +1076,7 @@ static int iwl_mvm_mac_ctxt_cmd_ap(struct iwl_mvm *mvm,
 	WARN_ON(vif->type != NL80211_IFTYPE_AP || vif->p2p);
 
 	/* Fill the common data for all mac context types */
-	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
+	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
 	/*
 	 * pass probe requests and beacons from other APs (needed
@@ -1098,7 +1102,7 @@ static int iwl_mvm_mac_ctxt_cmd_go(struct iwl_mvm *mvm,
 	WARN_ON(vif->type != NL80211_IFTYPE_AP || !vif->p2p);
 
 	/* Fill the common data for all mac context types */
-	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, action);
+	iwl_mvm_mac_ctxt_cmd_common(mvm, vif, &cmd, NULL, action);
 
 	/*
 	 * pass probe requests and beacons from other APs (needed
@@ -1121,12 +1125,14 @@ static int iwl_mvm_mac_ctxt_cmd_go(struct iwl_mvm *mvm,
 }
 
 static int iwl_mvm_mac_ctx_send(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-				u32 action, bool force_assoc_off)
+				u32 action, bool force_assoc_off,
+				const u8 *bssid_override)
 {
 	switch (vif->type) {
 	case NL80211_IFTYPE_STATION:
 		return iwl_mvm_mac_ctxt_cmd_sta(mvm, vif, action,
-						force_assoc_off);
+						force_assoc_off,
+						bssid_override);
 		break;
 	case NL80211_IFTYPE_AP:
 		if (!vif->p2p)
@@ -1157,7 +1163,7 @@ int iwl_mvm_mac_ctxt_add(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 		return -EIO;
 
 	ret = iwl_mvm_mac_ctx_send(mvm, vif, FW_CTXT_ACTION_ADD,
-				   true);
+				   true, NULL);
 	if (ret)
 		return ret;
 
@@ -1169,7 +1175,7 @@ int iwl_mvm_mac_ctxt_add(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 }
 
 int iwl_mvm_mac_ctxt_changed(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			     bool force_assoc_off)
+			     bool force_assoc_off, const u8 *bssid_override)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 
@@ -1178,7 +1184,7 @@ int iwl_mvm_mac_ctxt_changed(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 		return -EIO;
 
 	return iwl_mvm_mac_ctx_send(mvm, vif, FW_CTXT_ACTION_MODIFY,
-				    force_assoc_off);
+				    force_assoc_off, bssid_override);
 }
 
 int iwl_mvm_mac_ctxt_remove(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
@@ -1226,13 +1232,13 @@ static void iwl_mvm_csa_count_down(struct iwl_mvm *mvm,
 		    !iwl_mvm_te_scheduled(&mvmvif->time_event_data) && gp2) {
 			u32 rel_time = (c + 1) *
 				       csa_vif->bss_conf.beacon_int -
-				       IWL_MVM_CHANNEL_SWITCH_TIME;
+				       IWL_MVM_CHANNEL_SWITCH_TIME_GO;
 			u32 apply_time = gp2 + rel_time * 1024;
 
-			iwl_mvm_schedule_csa_noa(mvm, csa_vif,
-						 IWL_MVM_CHANNEL_SWITCH_TIME -
-						 IWL_MVM_CHANNEL_SWITCH_MARGIN,
-						 apply_time);
+			iwl_mvm_schedule_csa_period(mvm, csa_vif,
+					 IWL_MVM_CHANNEL_SWITCH_TIME_GO -
+					 IWL_MVM_CHANNEL_SWITCH_MARGIN,
+					 apply_time);
 		}
 	} else if (!iwl_mvm_te_scheduled(&mvmvif->time_event_data)) {
 		/* we don't have CSA NoA scheduled yet, switch now */
diff --git a/drivers/net/wireless/iwlwifi/mvm/mac80211.c b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
index 7c8796584c25..c7a73c68bdab 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mac80211.c
+++ b/drivers/net/wireless/iwlwifi/mvm/mac80211.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -277,14 +279,6 @@ static void iwl_mvm_reset_phy_ctxts(struct iwl_mvm *mvm)
 	}
 }
 
-static int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm)
-{
-	/* we create the 802.11 header and SSID element */
-	if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID)
-		return mvm->fw->ucode_capa.max_probe_length - 24 - 2;
-	return mvm->fw->ucode_capa.max_probe_length - 24 - 34;
-}
-
 int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 {
 	struct ieee80211_hw *hw = mvm->hw;
@@ -302,8 +296,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 		    IEEE80211_HW_TIMING_BEACON_ONLY |
 		    IEEE80211_HW_CONNECTION_MONITOR |
 		    IEEE80211_HW_CHANCTX_STA_CSA |
-		    IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
-		    IEEE80211_HW_SUPPORTS_STATIC_SMPS;
+		    IEEE80211_HW_SUPPORTS_CLONED_SKBS;
 
 	hw->queues = mvm->first_agg_queue;
 	hw->offchannel_tx_hw_queue = IWL_MVM_OFFCHANNEL_QUEUE;
@@ -325,7 +318,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	    IWL_UCODE_API(mvm->fw->ucode_ver) >= 9 &&
 	    !iwlwifi_mod_params.uapsd_disable) {
 		hw->flags |= IEEE80211_HW_SUPPORTS_UAPSD;
-		hw->uapsd_queues = IWL_UAPSD_AC_INFO;
+		hw->uapsd_queues = IWL_MVM_UAPSD_QUEUES;
 		hw->uapsd_max_sp_len = IWL_UAPSD_MAX_SP;
 	}
 
@@ -378,7 +371,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 
 	iwl_mvm_reset_phy_ctxts(mvm);
 
-	hw->wiphy->max_scan_ie_len = iwl_mvm_max_scan_ie_len(mvm);
+	hw->wiphy->max_scan_ie_len = iwl_mvm_max_scan_ie_len(mvm, false);
 
 	hw->wiphy->max_scan_ssids = PROBE_OPTION_MAX;
 
@@ -396,16 +389,36 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 	else
 		hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
 
-	/* TODO: enable that only for firmwares that don't crash */
-	/* hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN; */
-	hw->wiphy->max_sched_scan_ssids = PROBE_OPTION_MAX;
-	hw->wiphy->max_match_sets = IWL_SCAN_MAX_PROFILES;
-	/* we create the 802.11 header and zero length SSID IE. */
-	hw->wiphy->max_sched_scan_ie_len = SCAN_OFFLOAD_PROBE_REQ_SIZE - 24 - 2;
+	if (IWL_UCODE_API(mvm->fw->ucode_ver) >= 10) {
+		hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_SCHED_SCAN;
+		hw->wiphy->max_sched_scan_ssids = PROBE_OPTION_MAX;
+		hw->wiphy->max_match_sets = IWL_SCAN_MAX_PROFILES;
+		/* we create the 802.11 header and zero length SSID IE. */
+		hw->wiphy->max_sched_scan_ie_len =
+			SCAN_OFFLOAD_PROBE_REQ_SIZE - 24 - 2;
+	}
 
 	hw->wiphy->features |= NL80211_FEATURE_P2P_GO_CTWIN |
 			       NL80211_FEATURE_LOW_PRIORITY_SCAN |
-			       NL80211_FEATURE_P2P_GO_OPPPS;
+			       NL80211_FEATURE_P2P_GO_OPPPS |
+			       NL80211_FEATURE_DYNAMIC_SMPS |
+			       NL80211_FEATURE_STATIC_SMPS;
+
+	if (mvm->fw->ucode_capa.capa[0] &
+	    IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT)
+		hw->wiphy->features |= NL80211_FEATURE_TX_POWER_INSERTION;
+	if (mvm->fw->ucode_capa.capa[0] &
+	    IWL_UCODE_TLV_CAPA_QUIET_PERIOD_SUPPORT)
+		hw->wiphy->features |= NL80211_FEATURE_QUIET;
+
+	if (mvm->fw->ucode_capa.capa[0] &
+	    IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT)
+		hw->wiphy->features |=
+			NL80211_FEATURE_DS_PARAM_SET_IE_IN_PROBES;
+
+	if (mvm->fw->ucode_capa.capa[0] &
+	    IWL_UCODE_TLV_CAPA_WFA_TPC_REP_IE_SUPPORT)
+		hw->wiphy->features |= NL80211_FEATURE_WFA_TPC_IE_IN_PROBES;
 
 	mvm->rts_threshold = IEEE80211_MAX_RTS_THRESHOLD;
 
@@ -666,8 +679,9 @@ static void iwl_mvm_cleanup_iterator(void *data, u8 *mac,
 }
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
-static void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
+void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 {
+	static char *env[] = { "DRIVER=iwlwifi", "EVENT=error_dump", NULL };
 	struct iwl_fw_error_dump_file *dump_file;
 	struct iwl_fw_error_dump_data *dump_data;
 	struct iwl_fw_error_dump_info *dump_info;
@@ -759,23 +773,20 @@ static void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm)
 		file_len += fw_error_dump->trans_ptr->len;
 	dump_file->file_len = cpu_to_le32(file_len);
 	mvm->fw_error_dump = fw_error_dump;
+
+	/* notify the userspace about the error we had */
+	kobject_uevent_env(&mvm->hw->wiphy->dev.kobj, KOBJ_CHANGE, env);
 }
 #endif
 
 static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 {
-#ifdef CONFIG_IWLWIFI_DEBUGFS
-	static char *env[] = { "DRIVER=iwlwifi", "EVENT=error_dump", NULL };
-
 	iwl_mvm_fw_error_dump(mvm);
 
-	/* notify the userspace about the error we had */
-	kobject_uevent_env(&mvm->hw->wiphy->dev.kobj, KOBJ_CHANGE, env);
-#endif
-
 	iwl_trans_stop_device(mvm->trans);
 
 	mvm->scan_status = IWL_MVM_SCAN_NONE;
+	mvm->ps_disabled = false;
 
 	/* just in case one was running */
 	ieee80211_remain_on_channel_expired(mvm->hw);
@@ -803,16 +814,18 @@ static void iwl_mvm_restart_cleanup(struct iwl_mvm *mvm)
 	 * ucode_down ref until reconfig is complete */
 	iwl_mvm_unref_all_except(mvm, IWL_MVM_REF_UCODE_DOWN);
 
+	/* clear any stale d0i3 state */
+	clear_bit(IWL_MVM_STATUS_IN_D0I3, &mvm->status);
+
 	mvm->vif_count = 0;
 	mvm->rx_ba_sessions = 0;
 }
 
-static int iwl_mvm_mac_start(struct ieee80211_hw *hw)
+int __iwl_mvm_mac_start(struct iwl_mvm *mvm)
 {
-	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	int ret;
 
-	mutex_lock(&mvm->mutex);
+	lockdep_assert_held(&mvm->mutex);
 
 	/* Clean up some internal and mac80211 state on restart */
 	if (test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status))
@@ -829,6 +842,16 @@ static int iwl_mvm_mac_start(struct ieee80211_hw *hw)
 		iwl_mvm_d0i3_enable_tx(mvm, NULL);
 	}
 
+	return ret;
+}
+
+static int iwl_mvm_mac_start(struct ieee80211_hw *hw)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	int ret;
+
+	mutex_lock(&mvm->mutex);
+	ret = __iwl_mvm_mac_start(mvm);
 	mutex_unlock(&mvm->mutex);
 
 	return ret;
@@ -854,14 +877,9 @@ static void iwl_mvm_mac_restart_complete(struct ieee80211_hw *hw)
 	mutex_unlock(&mvm->mutex);
 }
 
-static void iwl_mvm_mac_stop(struct ieee80211_hw *hw)
+void __iwl_mvm_mac_stop(struct iwl_mvm *mvm)
 {
-	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
-
-	flush_work(&mvm->d0i3_exit_work);
-	flush_work(&mvm->async_handlers_wk);
-
-	mutex_lock(&mvm->mutex);
+	lockdep_assert_held(&mvm->mutex);
 
 	/* disallow low power states when the FW is down */
 	iwl_mvm_ref(mvm, IWL_MVM_REF_UCODE_DOWN);
@@ -880,8 +898,21 @@ static void iwl_mvm_mac_stop(struct ieee80211_hw *hw)
 	/* async_handlers_list is empty and will stay empty: HW is stopped */
 
 	/* the fw is stopped, the aux sta is dead: clean up driver state */
-	iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
+	iwl_mvm_del_aux_sta(mvm);
+
+	mvm->ucode_loaded = false;
+}
+
+static void iwl_mvm_mac_stop(struct ieee80211_hw *hw)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+
+	flush_work(&mvm->d0i3_exit_work);
+	flush_work(&mvm->async_handlers_wk);
+	flush_work(&mvm->fw_error_dump_wk);
 
+	mutex_lock(&mvm->mutex);
+	__iwl_mvm_mac_stop(mvm);
 	mutex_unlock(&mvm->mutex);
 
 	/*
@@ -965,10 +996,7 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 	 */
 	if (vif->type == NL80211_IFTYPE_AP ||
 	    vif->type == NL80211_IFTYPE_ADHOC) {
-		u32 qmask = iwl_mvm_mac_get_queues_mask(mvm, vif);
-		ret = iwl_mvm_allocate_int_sta(mvm, &mvmvif->bcast_sta,
-					       qmask,
-					       ieee80211_vif_type_p2p(vif));
+		ret = iwl_mvm_alloc_bcast_sta(mvm, vif);
 		if (ret) {
 			IWL_ERR(mvm, "Failed to allocate bcast sta\n");
 			goto out_release;
@@ -1016,7 +1044,7 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 		if (ret)
 			goto out_unref_phy;
 
-		ret = iwl_mvm_add_bcast_sta(mvm, vif, &mvmvif->bcast_sta);
+		ret = iwl_mvm_add_bcast_sta(mvm, vif);
 		if (ret)
 			goto out_unbind;
 
@@ -1057,14 +1085,7 @@ static int iwl_mvm_mac_add_interface(struct ieee80211_hw *hw,
 static void iwl_mvm_prepare_mac_removal(struct iwl_mvm *mvm,
 					struct ieee80211_vif *vif)
 {
-	u32 tfd_msk = 0, ac;
-
-	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++)
-		if (vif->hw_queue[ac] != IEEE80211_INVAL_HW_QUEUE)
-			tfd_msk |= BIT(vif->hw_queue[ac]);
-
-	if (vif->cab_queue != IEEE80211_INVAL_HW_QUEUE)
-		tfd_msk |= BIT(vif->cab_queue);
+	u32 tfd_msk = iwl_mvm_mac_get_queues_mask(mvm, vif);
 
 	if (tfd_msk) {
 		mutex_lock(&mvm->mutex);
@@ -1120,13 +1141,13 @@ static void iwl_mvm_mac_remove_interface(struct ieee80211_hw *hw,
 			mvm->noa_duration = 0;
 		}
 #endif
-		iwl_mvm_dealloc_int_sta(mvm, &mvmvif->bcast_sta);
+		iwl_mvm_dealloc_bcast_sta(mvm, vif);
 		goto out_release;
 	}
 
 	if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
 		mvm->p2p_device_vif = NULL;
-		iwl_mvm_rm_bcast_sta(mvm, &mvmvif->bcast_sta);
+		iwl_mvm_rm_bcast_sta(mvm, vif);
 		iwl_mvm_binding_remove_vif(mvm, vif);
 		iwl_mvm_phy_ctxt_unref(mvm, mvmvif->phy_ctxt);
 		mvmvif->phy_ctxt = NULL;
@@ -1200,14 +1221,15 @@ static u64 iwl_mvm_prepare_multicast(struct ieee80211_hw *hw,
 	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
 	struct iwl_mcast_filter_cmd *cmd;
 	struct netdev_hw_addr *addr;
-	int addr_count = netdev_hw_addr_list_count(mc_list);
-	bool pass_all = false;
+	int addr_count;
+	bool pass_all;
 	int len;
 
-	if (addr_count > MAX_MCAST_FILTERING_ADDRESSES) {
-		pass_all = true;
+	addr_count = netdev_hw_addr_list_count(mc_list);
+	pass_all = addr_count > MAX_MCAST_FILTERING_ADDRESSES ||
+		   IWL_MVM_FW_MCAST_FILTER_PASS_ALL;
+	if (pass_all)
 		addr_count = 0;
-	}
 
 	len = roundup(sizeof(*cmd) + addr_count * ETH_ALEN, 4);
 	cmd = kzalloc(len, GFP_ATOMIC);
@@ -1407,28 +1429,6 @@ static inline int iwl_mvm_configure_bcast_filter(struct iwl_mvm *mvm,
 }
 #endif
 
-static void iwl_mvm_teardown_tdls_peers(struct iwl_mvm *mvm)
-{
-	struct ieee80211_sta *sta;
-	struct iwl_mvm_sta *mvmsta;
-	int i;
-
-	lockdep_assert_held(&mvm->mutex);
-
-	for (i = 0; i < IWL_MVM_STATION_COUNT; i++) {
-		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[i],
-						lockdep_is_held(&mvm->mutex));
-		if (!sta || IS_ERR(sta) || !sta->tdls)
-			continue;
-
-		mvmsta = iwl_mvm_sta_from_mac80211(sta);
-		ieee80211_tdls_oper_request(mvmsta->vif, sta->addr,
-				NL80211_TDLS_TEARDOWN,
-				WLAN_REASON_TDLS_TEARDOWN_UNSPECIFIED,
-				GFP_KERNEL);
-	}
-}
-
 static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 					     struct ieee80211_vif *vif,
 					     struct ieee80211_bss_conf *bss_conf,
@@ -1445,10 +1445,23 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 	if (changes & BSS_CHANGED_ASSOC && bss_conf->assoc)
 		iwl_mvm_mac_ctxt_recalc_tsf_id(mvm, vif);
 
-	ret = iwl_mvm_mac_ctxt_changed(mvm, vif, false);
+	/*
+	 * If we're not associated yet, take the (new) BSSID before associating
+	 * so the firmware knows. If we're already associated, then use the old
+	 * BSSID here, and we'll send a cleared one later in the CHANGED_ASSOC
+	 * branch for disassociation below.
+	 */
+	if (changes & BSS_CHANGED_BSSID && !mvmvif->associated)
+		memcpy(mvmvif->bssid, bss_conf->bssid, ETH_ALEN);
+
+	ret = iwl_mvm_mac_ctxt_changed(mvm, vif, false, mvmvif->bssid);
 	if (ret)
 		IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 
+	/* after sending it once, adopt mac80211 data */
+	memcpy(mvmvif->bssid, bss_conf->bssid, ETH_ALEN);
+	mvmvif->associated = bss_conf->assoc;
+
 	if (changes & BSS_CHANGED_ASSOC) {
 		if (bss_conf->assoc) {
 			/* add quota for this interface */
@@ -1476,13 +1489,17 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 				 */
 				u32 dur = (11 * vif->bss_conf.beacon_int) / 10;
 				iwl_mvm_protect_session(mvm, vif, dur, dur,
-							5 * dur);
+							5 * dur, false);
 			}
 
 			iwl_mvm_sf_update(mvm, vif, false);
 			iwl_mvm_power_vif_assoc(mvm, vif);
-			if (vif->p2p)
+			if (vif->p2p) {
 				iwl_mvm_ref(mvm, IWL_MVM_REF_P2P_CLIENT);
+				iwl_mvm_update_smps(mvm, vif,
+						    IWL_MVM_SMPS_REQ_PROT,
+						    IEEE80211_SMPS_DYNAMIC);
+			}
 		} else if (mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT) {
 			/*
 			 * If update fails - SF might be running in associated
@@ -1506,6 +1523,13 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 
 			if (vif->p2p)
 				iwl_mvm_unref(mvm, IWL_MVM_REF_P2P_CLIENT);
+
+			/* this will take the cleared BSSID from bss_conf */
+			ret = iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
+			if (ret)
+				IWL_ERR(mvm,
+					"failed to update MAC %pM (clear after unassoc)\n",
+					vif->addr);
 		}
 
 		iwl_mvm_recalc_multicast(mvm);
@@ -1524,11 +1548,6 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 		 */
 		iwl_mvm_remove_time_event(mvm, mvmvif,
 					  &mvmvif->time_event_data);
-	} else if (changes & (BSS_CHANGED_PS | BSS_CHANGED_P2P_PS |
-			      BSS_CHANGED_QOS)) {
-		ret = iwl_mvm_power_update_mac(mvm);
-		if (ret)
-			IWL_ERR(mvm, "failed to update power mode\n");
 	}
 
 	if (changes & BSS_CHANGED_BEACON_INFO) {
@@ -1536,6 +1555,12 @@ static void iwl_mvm_bss_info_changed_station(struct iwl_mvm *mvm,
 		WARN_ON(iwl_mvm_enable_beacon_filter(mvm, vif, 0));
 	}
 
+	if (changes & (BSS_CHANGED_PS | BSS_CHANGED_P2P_PS | BSS_CHANGED_QOS)) {
+		ret = iwl_mvm_power_update_mac(mvm);
+		if (ret)
+			IWL_ERR(mvm, "failed to update power mode\n");
+	}
+
 	if (changes & BSS_CHANGED_TXPOWER) {
 		IWL_DEBUG_CALIB(mvm, "Changing TX Power to %d\n",
 				bss_conf->txpower);
@@ -1601,7 +1626,7 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 
 	/* Send the bcast station. At this stage the TBTT and DTIM time events
 	 * are added and applied to the scheduler */
-	ret = iwl_mvm_send_bcast_sta(mvm, vif, &mvmvif->bcast_sta);
+	ret = iwl_mvm_send_add_bcast_sta(mvm, vif);
 	if (ret)
 		goto out_unbind;
 
@@ -1617,7 +1642,7 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 
 	/* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
 	if (vif->p2p && mvm->p2p_device_vif)
-		iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false);
+		iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false, NULL);
 
 	iwl_mvm_ref(mvm, IWL_MVM_REF_AP_IBSS);
 
@@ -1633,7 +1658,7 @@ static int iwl_mvm_start_ap_ibss(struct ieee80211_hw *hw,
 out_quota_failed:
 	iwl_mvm_power_update_mac(mvm);
 	mvmvif->ap_ibss_active = false;
-	iwl_mvm_send_rm_bcast_sta(mvm, &mvmvif->bcast_sta);
+	iwl_mvm_send_rm_bcast_sta(mvm, vif);
 out_unbind:
 	iwl_mvm_binding_remove_vif(mvm, vif);
 out_remove:
@@ -1675,10 +1700,10 @@ static void iwl_mvm_stop_ap_ibss(struct ieee80211_hw *hw,
 
 	/* Need to update the P2P Device MAC (only GO, IBSS is single vif) */
 	if (vif->p2p && mvm->p2p_device_vif)
-		iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false);
+		iwl_mvm_mac_ctxt_changed(mvm, mvm->p2p_device_vif, false, NULL);
 
 	iwl_mvm_update_quotas(mvm, NULL);
-	iwl_mvm_send_rm_bcast_sta(mvm, &mvmvif->bcast_sta);
+	iwl_mvm_send_rm_bcast_sta(mvm, vif);
 	iwl_mvm_binding_remove_vif(mvm, vif);
 
 	iwl_mvm_power_update_mac(mvm);
@@ -1701,8 +1726,8 @@ iwl_mvm_bss_info_changed_ap_ibss(struct iwl_mvm *mvm,
 		return;
 
 	if (changes & (BSS_CHANGED_ERP_CTS_PROT | BSS_CHANGED_HT |
-		       BSS_CHANGED_BANDWIDTH) &&
-	    iwl_mvm_mac_ctxt_changed(mvm, vif, false))
+		       BSS_CHANGED_BANDWIDTH | BSS_CHANGED_QOS) &&
+	    iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL))
 		IWL_ERR(mvm, "failed to update MAC %pM\n", vif->addr);
 
 	/* Need to send a new beacon template to the FW */
@@ -1932,48 +1957,6 @@ static void iwl_mvm_sta_pre_rcu_remove(struct ieee80211_hw *hw,
 	mutex_unlock(&mvm->mutex);
 }
 
-int iwl_mvm_tdls_sta_count(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
-{
-	struct ieee80211_sta *sta;
-	struct iwl_mvm_sta *mvmsta;
-	int count = 0;
-	int i;
-
-	lockdep_assert_held(&mvm->mutex);
-
-	for (i = 0; i < IWL_MVM_STATION_COUNT; i++) {
-		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[i],
-						lockdep_is_held(&mvm->mutex));
-		if (!sta || IS_ERR(sta) || !sta->tdls)
-			continue;
-
-		if (vif) {
-			mvmsta = iwl_mvm_sta_from_mac80211(sta);
-			if (mvmsta->vif != vif)
-				continue;
-		}
-
-		count++;
-	}
-
-	return count;
-}
-
-static void iwl_mvm_recalc_tdls_state(struct iwl_mvm *mvm,
-				      struct ieee80211_vif *vif,
-				      bool sta_added)
-{
-	int tdls_sta_cnt = iwl_mvm_tdls_sta_count(mvm, vif);
-
-	/*
-	 * Disable ps when the first TDLS sta is added and re-enable it
-	 * when the last TDLS sta is removed
-	 */
-	if ((tdls_sta_cnt == 1 && sta_added) ||
-	    (tdls_sta_cnt == 0 && !sta_added))
-		iwl_mvm_power_update_mac(mvm);
-}
-
 static int iwl_mvm_mac_sta_state(struct ieee80211_hw *hw,
 				 struct ieee80211_vif *vif,
 				 struct ieee80211_sta *sta,
@@ -2113,7 +2096,7 @@ static int iwl_mvm_mac_conf_tx(struct ieee80211_hw *hw,
 		int ret;
 
 		mutex_lock(&mvm->mutex);
-		ret = iwl_mvm_mac_ctxt_changed(mvm, vif, false);
+		ret = iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
 		mutex_unlock(&mvm->mutex);
 		return ret;
 	}
@@ -2141,33 +2124,12 @@ static void iwl_mvm_mac_mgd_prepare_tx(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 	/* Try really hard to protect the session and hear a beacon */
-	iwl_mvm_protect_session(mvm, vif, duration, min_duration, 500);
+	iwl_mvm_protect_session(mvm, vif, duration, min_duration, 500, false);
 	mutex_unlock(&mvm->mutex);
 
 	iwl_mvm_unref(mvm, IWL_MVM_REF_PREPARE_TX);
 }
 
-static void iwl_mvm_mac_mgd_protect_tdls_discover(struct ieee80211_hw *hw,
-						  struct ieee80211_vif *vif)
-{
-	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
-	u32 duration = 2 * vif->bss_conf.dtim_period * vif->bss_conf.beacon_int;
-
-	/*
-	 * iwl_mvm_protect_session() reads directly from the device
-	 * (the system time), so make sure it is available.
-	 */
-	if (iwl_mvm_ref_sync(mvm, IWL_MVM_REF_PROTECT_TDLS))
-		return;
-
-	mutex_lock(&mvm->mutex);
-	/* Protect the session to hear the TDLS setup response on the channel */
-	iwl_mvm_protect_session(mvm, vif, duration, duration, 100);
-	mutex_unlock(&mvm->mutex);
-
-	iwl_mvm_unref(mvm, IWL_MVM_REF_PROTECT_TDLS);
-}
-
 static int iwl_mvm_mac_sched_scan_start(struct ieee80211_hw *hw,
 					struct ieee80211_vif *vif,
 					struct cfg80211_sched_scan_request *req,
@@ -2182,7 +2144,13 @@ static int iwl_mvm_mac_sched_scan_start(struct ieee80211_hw *hw,
 
 	mutex_lock(&mvm->mutex);
 
-	if (!iwl_mvm_is_idle(mvm)) {
+	/* Newest FW fixes sched scan while connected on another interface */
+	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN) {
+		if (!vif->bss_conf.idle) {
+			ret = -EBUSY;
+			goto out;
+		}
+	} else if (!iwl_mvm_is_idle(mvm)) {
 		ret = -EBUSY;
 		goto out;
 	}
@@ -2700,7 +2668,10 @@ static int __iwl_mvm_assign_vif_chanctx(struct iwl_mvm *mvm,
 		ret = 0;
 		goto out;
 	case NL80211_IFTYPE_STATION:
+		break;
 	case NL80211_IFTYPE_MONITOR:
+		/* always disable PS when a monitor interface is active */
+		mvmvif->ps_disabled = true;
 		break;
 	default:
 		ret = -EINVAL;
@@ -2732,7 +2703,20 @@ static int __iwl_mvm_assign_vif_chanctx(struct iwl_mvm *mvm,
 	if ((vif->type == NL80211_IFTYPE_AP) ||
 	    (switching_chanctx && (vif->type == NL80211_IFTYPE_STATION))) {
 		iwl_mvm_update_quotas(mvm, NULL);
-		iwl_mvm_mac_ctxt_changed(mvm, vif, false);
+		iwl_mvm_mac_ctxt_changed(mvm, vif, false, NULL);
+	}
+
+	if (vif->csa_active && vif->type == NL80211_IFTYPE_STATION) {
+		struct iwl_mvm_sta *mvmsta;
+
+		mvmsta = iwl_mvm_sta_from_staid_protected(mvm,
+							  mvmvif->ap_sta_id);
+
+		if (WARN_ON(!mvmsta))
+			goto out;
+
+		/* TODO: only re-enable after the first beacon */
+		iwl_mvm_sta_modify_disable_tx(mvm, mvmsta, false);
 	}
 
 	goto out;
@@ -2766,6 +2750,7 @@ static void __iwl_mvm_unassign_vif_chanctx(struct iwl_mvm *mvm,
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct ieee80211_vif *disabled_vif = NULL;
+	struct iwl_mvm_sta *mvmsta;
 
 	lockdep_assert_held(&mvm->mutex);
 
@@ -2776,6 +2761,7 @@ static void __iwl_mvm_unassign_vif_chanctx(struct iwl_mvm *mvm,
 		goto out;
 	case NL80211_IFTYPE_MONITOR:
 		mvmvif->monitor_active = false;
+		mvmvif->ps_disabled = false;
 		break;
 	case NL80211_IFTYPE_AP:
 		/* This part is triggered only during CSA */
@@ -2796,7 +2782,13 @@ static void __iwl_mvm_unassign_vif_chanctx(struct iwl_mvm *mvm,
 
 		disabled_vif = vif;
 
-		iwl_mvm_mac_ctxt_changed(mvm, vif, true);
+		mvmsta = iwl_mvm_sta_from_staid_protected(mvm,
+							  mvmvif->ap_sta_id);
+
+		if (!WARN_ON(!mvmsta))
+			iwl_mvm_sta_modify_disable_tx(mvm, mvmsta, true);
+
+		iwl_mvm_mac_ctxt_changed(mvm, vif, true, NULL);
 		break;
 	default:
 		break;
diff --git a/drivers/net/wireless/iwlwifi/mvm/mvm.h b/drivers/net/wireless/iwlwifi/mvm/mvm.h
index 2e73d3bd7757..b153ced7015b 100644
--- a/drivers/net/wireless/iwlwifi/mvm/mvm.h
+++ b/drivers/net/wireless/iwlwifi/mvm/mvm.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -85,11 +87,11 @@
 /* A TimeUnit is 1024 microsecond */
 #define MSEC_TO_TU(_msec)	(_msec*1000/1024)
 
-/*
- * The CSA NoA is scheduled IWL_MVM_CHANNEL_SWITCH_TIME TUs before "beacon 0"
- * TBTT. This value should be big enough to ensure that we switch in time.
+/* This value represents the number of TUs before CSA "beacon 0" TBTT
+ * when the CSA time-event needs to be scheduled to start.  It must be
+ * big enough to ensure that we switch in time.
  */
-#define IWL_MVM_CHANNEL_SWITCH_TIME 40
+#define IWL_MVM_CHANNEL_SWITCH_TIME_GO		40
 
 /*
  * This value (in TUs) is used to fine tune the CSA NoA end time which should
@@ -103,14 +105,6 @@
  */
 #define IWL_MVM_CS_UNBLOCK_TX_TIMEOUT 3
 
-enum iwl_mvm_tx_fifo {
-	IWL_MVM_TX_FIFO_BK = 0,
-	IWL_MVM_TX_FIFO_BE,
-	IWL_MVM_TX_FIFO_VI,
-	IWL_MVM_TX_FIFO_VO,
-	IWL_MVM_TX_FIFO_MCAST = 5,
-};
-
 extern const struct ieee80211_ops iwl_mvm_hw_ops;
 
 /**
@@ -186,10 +180,6 @@ enum iwl_power_scheme {
 };
 
 #define IWL_CONN_MAX_LISTEN_INTERVAL	10
-#define IWL_UAPSD_AC_INFO		(IEEE80211_WMM_IE_STA_QOSINFO_AC_VO |\
-					 IEEE80211_WMM_IE_STA_QOSINFO_AC_VI |\
-					 IEEE80211_WMM_IE_STA_QOSINFO_AC_BK |\
-					 IEEE80211_WMM_IE_STA_QOSINFO_AC_BE)
 #define IWL_UAPSD_MAX_SP		IEEE80211_WMM_IE_STA_QOSINFO_SP_2
 
 #ifdef CONFIG_IWLWIFI_DEBUGFS
@@ -203,6 +193,7 @@ enum iwl_dbgfs_pm_mask {
 	MVM_DEBUGFS_PM_LPRX_RSSI_THRESHOLD = BIT(7),
 	MVM_DEBUGFS_PM_SNOOZE_ENABLE = BIT(8),
 	MVM_DEBUGFS_PM_UAPSD_MISBEHAVING = BIT(9),
+	MVM_DEBUGFS_PM_USE_PS_POLL = BIT(10),
 };
 
 struct iwl_dbgfs_pm {
@@ -215,6 +206,7 @@ struct iwl_dbgfs_pm {
 	u32 lprx_rssi_threshold;
 	bool snooze_ena;
 	bool uapsd_misbehaving;
+	bool use_ps_poll;
 	int mask;
 };
 
@@ -253,6 +245,7 @@ struct iwl_dbgfs_bf {
 enum iwl_mvm_smps_type_request {
 	IWL_MVM_SMPS_REQ_BT_COEX,
 	IWL_MVM_SMPS_REQ_TT,
+	IWL_MVM_SMPS_REQ_PROT,
 	NUM_IWL_MVM_SMPS_REQ,
 };
 
@@ -277,6 +270,8 @@ enum iwl_mvm_ref_type {
 	IWL_MVM_REF_TM_CMD,
 	IWL_MVM_REF_EXIT_WORK,
 
+	/* update debugfs.c when changing this */
+
 	IWL_MVM_REF_COUNT,
 };
 
@@ -315,6 +310,9 @@ struct iwl_mvm_vif_bf_data {
  * @id: between 0 and 3
  * @color: to solve races upon MAC addition and removal
  * @ap_sta_id: the sta_id of the AP - valid only if VIF type is STA
+ * @bssid: BSSID for this (client) interface
+ * @associated: indicates that we're currently associated, used only for
+ *	managing the firmware state in iwl_mvm_bss_info_changed_station()
  * @uploaded: indicates the MAC context has been added to the device
  * @ap_ibss_active: indicates that AP/IBSS is configured and that the interface
  *	should get quota etc.
@@ -323,6 +321,7 @@ struct iwl_mvm_vif_bf_data {
  *	interface should get quota etc.
  * @low_latency: indicates that this interface is in low-latency mode
  *	(VMACLowLatencyMode)
+ * @ps_disabled: indicates that this interface requires PS to be disabled
  * @queue_params: QoS params for this MAC
  * @bcast_sta: station used for broadcast packets. Used by the following
  *  vifs: P2P_DEVICE, GO and AP.
@@ -335,11 +334,15 @@ struct iwl_mvm_vif {
 	u16 color;
 	u8 ap_sta_id;
 
+	u8 bssid[ETH_ALEN];
+	bool associated;
+
 	bool uploaded;
 	bool ap_ibss_active;
 	bool pm_enabled;
 	bool monitor_active;
 	bool low_latency;
+	bool ps_disabled;
 	struct iwl_mvm_vif_bf_data bf_data;
 
 	u32 ap_beacon_time;
@@ -512,6 +515,10 @@ enum {
 	D0I3_PENDING_WAKEUP,
 };
 
+#define IWL_MVM_DEBUG_SET_TEMPERATURE_DISABLE 0xff
+#define IWL_MVM_DEBUG_SET_TEMPERATURE_MIN -100
+#define IWL_MVM_DEBUG_SET_TEMPERATURE_MAX 200
+
 struct iwl_mvm {
 	/* for logger access */
 	struct device *dev;
@@ -553,9 +560,8 @@ struct iwl_mvm {
 
 	struct mvm_statistics_rx rx_stats;
 
-	unsigned long transport_queue_stop;
 	u8 queue_to_mac80211[IWL_MAX_HW_QUEUES];
-	atomic_t queue_stop_count[IWL_MAX_HW_QUEUES];
+	atomic_t mac80211_queue_stop_count[IEEE80211_MAX_QUEUES];
 
 	const char *nvm_file_name;
 	struct iwl_nvm_data *nvm_data;
@@ -641,6 +647,7 @@ struct iwl_mvm {
 
 	/* -1 for always, 0 for never, >0 for that many times */
 	s8 restart_fw;
+	struct work_struct fw_error_dump_wk;
 	struct iwl_mvm_dump_ptrs *fw_error_dump;
 
 #ifdef CONFIG_IWLWIFI_LEDS
@@ -694,6 +701,14 @@ struct iwl_mvm {
 	/* Thermal Throttling and CTkill */
 	struct iwl_mvm_tt_mgmt thermal_throttle;
 	s32 temperature;	/* Celsius */
+	/*
+	 * Debug option to set the NIC temperature. This option makes the
+	 * driver think this is the actual NIC temperature, and ignore the
+	 * real temperature that is received from the fw
+	 */
+	bool temperature_test;  /* Debug test temperature is enabled */
+
+	struct iwl_time_quota_cmd last_quota_cmd;
 
 #ifdef CONFIG_NL80211_TESTMODE
 	u32 noa_duration;
@@ -706,7 +721,7 @@ struct iwl_mvm {
 	u8 last_agg_queue;
 
 	/* Indicate if device power save is allowed */
-	bool ps_disabled;
+	u8 ps_disabled; /* u8 instead of bool to ease debugfs_create_* usage */
 
 	struct ieee80211_vif __rcu *csa_vif;
 	struct ieee80211_vif __rcu *csa_tx_blocked_vif;
@@ -714,6 +729,8 @@ struct iwl_mvm {
 
 	/* system time of last beacon (for AP/GO interface) */
 	u32 ap_last_beacon_gp2;
+
+	u8 low_latency_agg_frame_limit;
 };
 
 /* Extract MVM priv from op_mode and _hw */
@@ -762,6 +779,11 @@ static inline bool iwl_mvm_is_d0i3_supported(struct iwl_mvm *mvm)
 	       (mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_D0I3_SUPPORT);
 }
 
+static inline bool iwl_mvm_is_dqa_supported(struct iwl_mvm *mvm)
+{
+	return mvm->fw->ucode_capa.capa[0] & IWL_UCODE_TLV_CAPA_DQA_SUPPORT;
+}
+
 extern const u8 iwl_mvm_ac_to_tx_fifo[];
 
 struct iwl_rate_info {
@@ -772,6 +794,9 @@ struct iwl_rate_info {
 	u8 ieee;	/* MAC header:  IWL_RATE_6M_IEEE, etc. */
 };
 
+void __iwl_mvm_mac_stop(struct iwl_mvm *mvm);
+int __iwl_mvm_mac_start(struct iwl_mvm *mvm);
+
 /******************
  * MVM Methods
  ******************/
@@ -878,7 +903,7 @@ int iwl_mvm_mac_ctxt_init(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 void iwl_mvm_mac_ctxt_release(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_mac_ctxt_add(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 int iwl_mvm_mac_ctxt_changed(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			     bool force_assoc_off);
+			     bool force_assoc_off, const u8 *bssid_override);
 int iwl_mvm_mac_ctxt_remove(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
 u32 iwl_mvm_mac_get_queues_mask(struct iwl_mvm *mvm,
 				struct ieee80211_vif *vif);
@@ -910,6 +935,7 @@ int iwl_mvm_rx_scan_response(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 int iwl_mvm_rx_scan_complete(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 			     struct iwl_device_cmd *cmd);
 int iwl_mvm_cancel_scan(struct iwl_mvm *mvm);
+int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan);
 
 /* Scheduled scan */
 int iwl_mvm_rx_scan_offload_complete_notif(struct iwl_mvm *mvm,
@@ -964,10 +990,14 @@ void iwl_mvm_update_frame_stats(struct iwl_mvm *mvm,
 				struct iwl_mvm_frame_stats *stats,
 				u32 rate, bool agg);
 int rs_pretty_print_rate(char *buf, const u32 rate);
+void rs_update_last_rssi(struct iwl_mvm *mvm,
+			 struct iwl_lq_sta *lq_sta,
+			 struct ieee80211_rx_status *rx_status);
 
 /* power management */
 int iwl_mvm_power_update_device(struct iwl_mvm *mvm);
 int iwl_mvm_power_update_mac(struct iwl_mvm *mvm);
+int iwl_mvm_power_update_ps(struct iwl_mvm *mvm);
 int iwl_mvm_power_mac_dbgfs_read(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 				 char *buf, int bufsz);
 
@@ -1120,6 +1150,39 @@ static inline bool iwl_mvm_vif_low_latency(struct iwl_mvm_vif *mvmvif)
 	return mvmvif->low_latency;
 }
 
+/* hw scheduler queue config */
+void iwl_mvm_enable_txq(struct iwl_mvm *mvm, int queue, u16 ssn,
+			const struct iwl_trans_txq_scd_cfg *cfg);
+void iwl_mvm_disable_txq(struct iwl_mvm *mvm, int queue);
+
+static inline void iwl_mvm_enable_ac_txq(struct iwl_mvm *mvm, int queue,
+					 u8 fifo)
+{
+	struct iwl_trans_txq_scd_cfg cfg = {
+		.fifo = fifo,
+		.tid = IWL_MAX_TID_COUNT,
+		.aggregate = false,
+		.frame_limit = IWL_FRAME_LIMIT,
+	};
+
+	iwl_mvm_enable_txq(mvm, queue, 0, &cfg);
+}
+
+static inline void iwl_mvm_enable_agg_txq(struct iwl_mvm *mvm, int queue,
+					  int fifo, int sta_id, int tid,
+					  int frame_limit, u16 ssn)
+{
+	struct iwl_trans_txq_scd_cfg cfg = {
+		.fifo = fifo,
+		.sta_id = sta_id,
+		.tid = tid,
+		.frame_limit = frame_limit,
+		.aggregate = true,
+	};
+
+	iwl_mvm_enable_txq(mvm, queue, ssn, &cfg);
+}
+
 /* Assoc status */
 bool iwl_mvm_is_idle(struct iwl_mvm *mvm);
 
@@ -1129,6 +1192,7 @@ void iwl_mvm_tt_handler(struct iwl_mvm *mvm);
 void iwl_mvm_tt_initialize(struct iwl_mvm *mvm, u32 min_backoff);
 void iwl_mvm_tt_exit(struct iwl_mvm *mvm);
 void iwl_mvm_set_hw_ctkill_state(struct iwl_mvm *mvm, bool state);
+int iwl_mvm_get_temp(struct iwl_mvm *mvm);
 
 /* smart fifo */
 int iwl_mvm_sf_update(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
@@ -1136,7 +1200,17 @@ int iwl_mvm_sf_update(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 /* TDLS */
 int iwl_mvm_tdls_sta_count(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+void iwl_mvm_teardown_tdls_peers(struct iwl_mvm *mvm);
+void iwl_mvm_recalc_tdls_state(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			       bool sta_added);
+void iwl_mvm_mac_mgd_protect_tdls_discover(struct ieee80211_hw *hw,
+					   struct ieee80211_vif *vif);
 
 void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error);
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm);
+#else
+static inline void iwl_mvm_fw_error_dump(struct iwl_mvm *mvm) {}
+#endif
 
 #endif /* __IWL_MVM_H__ */
diff --git a/drivers/net/wireless/iwlwifi/mvm/nvm.c b/drivers/net/wireless/iwlwifi/mvm/nvm.c
index cfdd314fdd5d..af074563e770 100644
--- a/drivers/net/wireless/iwlwifi/mvm/nvm.c
+++ b/drivers/net/wireless/iwlwifi/mvm/nvm.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -62,6 +64,7 @@
  *****************************************************************************/
 #include <linux/firmware.h>
 #include "iwl-trans.h"
+#include "iwl-csr.h"
 #include "mvm.h"
 #include "iwl-eeprom-parse.h"
 #include "iwl-eeprom-read.h"
@@ -347,7 +350,7 @@ static int iwl_mvm_read_external_nvm(struct iwl_mvm *mvm)
 	/* Maximal size depends on HW family and step */
 	if (mvm->trans->cfg->device_family != IWL_DEVICE_FAMILY_8000)
 		max_section_size = IWL_MAX_NVM_SECTION_SIZE;
-	else if ((mvm->trans->hw_rev & 0xc) == 0) /* Family 8000 A-step */
+	else if (CSR_HW_REV_STEP(mvm->trans->hw_rev) == SILICON_A_STEP)
 		max_section_size = IWL_MAX_NVM_8000A_SECTION_SIZE;
 	else /* Family 8000 B-step */
 		max_section_size = IWL_MAX_NVM_8000B_SECTION_SIZE;
diff --git a/drivers/net/wireless/iwlwifi/mvm/offloading.c b/drivers/net/wireless/iwlwifi/mvm/offloading.c
index 9bfb95e89cfb..adcbf4c8edd8 100644
--- a/drivers/net/wireless/iwlwifi/mvm/offloading.c
+++ b/drivers/net/wireless/iwlwifi/mvm/offloading.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/ops.c b/drivers/net/wireless/iwlwifi/mvm/ops.c
index 610dbcb0dc27..15aa298ee79c 100644
--- a/drivers/net/wireless/iwlwifi/mvm/ops.c
+++ b/drivers/net/wireless/iwlwifi/mvm/ops.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -330,6 +332,8 @@ static const char *const iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(BCAST_FILTER_CMD),
 	CMD(REPLY_SF_CFG_CMD),
 	CMD(REPLY_BEACON_FILTERING_CMD),
+	CMD(CMD_DTS_MEASUREMENT_TRIGGER),
+	CMD(DTS_MEASUREMENT_NOTIFICATION),
 	CMD(REPLY_THERMAL_MNG_BACKOFF),
 	CMD(MAC_PM_POWER_TABLE),
 	CMD(BT_COEX_CI),
@@ -338,6 +342,7 @@ static const char *const iwl_mvm_cmd_strings[REPLY_MAX] = {
 	CMD(BT_COEX_UPDATE_REDUCED_TXP),
 	CMD(PSM_UAPSD_AP_MISBEHAVING_NOTIFICATION),
 	CMD(ANTENNA_COUPLING_NOTIFICATION),
+	CMD(SCD_QUEUE_CFG),
 };
 #undef CMD
 
@@ -362,6 +367,8 @@ static u32 calc_min_backoff(struct iwl_trans *trans, const struct iwl_cfg *cfg)
 	return 0;
 }
 
+static void iwl_mvm_fw_error_dump_wk(struct work_struct *work);
+
 static struct iwl_op_mode *
 iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 		      const struct iwl_fw *fw, struct dentry *dbgfs_dir)
@@ -415,6 +422,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 		mvm->first_agg_queue = 12;
 	}
 	mvm->sf_state = SF_UNINIT;
+	mvm->low_latency_agg_frame_limit = 6;
 
 	mutex_init(&mvm->mutex);
 	mutex_init(&mvm->d0i3_suspend_mutex);
@@ -428,6 +436,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	INIT_WORK(&mvm->roc_done_wk, iwl_mvm_roc_done_wk);
 	INIT_WORK(&mvm->sta_drained_wk, iwl_mvm_sta_drained_wk);
 	INIT_WORK(&mvm->d0i3_exit_work, iwl_mvm_d0i3_exit_work);
+	INIT_WORK(&mvm->fw_error_dump_wk, iwl_mvm_fw_error_dump_wk);
 
 	spin_lock_init(&mvm->d0i3_tx_lock);
 	spin_lock_init(&mvm->refs_lock);
@@ -456,7 +465,8 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 	trans_cfg.command_names = iwl_mvm_cmd_strings;
 
 	trans_cfg.cmd_queue = IWL_MVM_CMD_QUEUE;
-	trans_cfg.cmd_fifo = IWL_MVM_CMD_FIFO;
+	trans_cfg.cmd_fifo = IWL_MVM_TX_FIFO_CMD;
+	trans_cfg.scd_set_active = true;
 
 	snprintf(mvm->hw->wiphy->fw_version,
 		 sizeof(mvm->hw->wiphy->fw_version),
@@ -494,7 +504,7 @@ iwl_op_mode_mvm_start(struct iwl_trans *trans, const struct iwl_cfg *cfg,
 		goto out_free;
 
 	/*
-	 * Even if nvm exists in the nvm_file driver should read agin the nvm
+	 * Even if nvm exists in the nvm_file driver should read again the nvm
 	 * from the nic because there might be entries that exist in the OTP
 	 * and not in the file.
 	 * for nics with no_power_up_nic_in_init: rely completley on nvm_file
@@ -700,14 +710,13 @@ static void iwl_mvm_stop_sw_queue(struct iwl_op_mode *op_mode, int queue)
 	if (WARN_ON_ONCE(mq == IWL_INVALID_MAC80211_QUEUE))
 		return;
 
-	if (atomic_inc_return(&mvm->queue_stop_count[mq]) > 1) {
+	if (atomic_inc_return(&mvm->mac80211_queue_stop_count[mq]) > 1) {
 		IWL_DEBUG_TX_QUEUES(mvm,
 				    "queue %d (mac80211 %d) already stopped\n",
 				    queue, mq);
 		return;
 	}
 
-	set_bit(mq, &mvm->transport_queue_stop);
 	ieee80211_stop_queue(mvm->hw, mq);
 }
 
@@ -719,15 +728,13 @@ static void iwl_mvm_wake_sw_queue(struct iwl_op_mode *op_mode, int queue)
 	if (WARN_ON_ONCE(mq == IWL_INVALID_MAC80211_QUEUE))
 		return;
 
-	if (atomic_dec_return(&mvm->queue_stop_count[mq]) > 0) {
+	if (atomic_dec_return(&mvm->mac80211_queue_stop_count[mq]) > 0) {
 		IWL_DEBUG_TX_QUEUES(mvm,
-				    "queue %d (mac80211 %d) already awake\n",
+				    "queue %d (mac80211 %d) still stopped\n",
 				    queue, mq);
 		return;
 	}
 
-	clear_bit(mq, &mvm->transport_queue_stop);
-
 	ieee80211_wake_queue(mvm->hw, mq);
 }
 
@@ -781,6 +788,16 @@ static void iwl_mvm_reprobe_wk(struct work_struct *wk)
 	module_put(THIS_MODULE);
 }
 
+static void iwl_mvm_fw_error_dump_wk(struct work_struct *work)
+{
+	struct iwl_mvm *mvm =
+		container_of(work, struct iwl_mvm, fw_error_dump_wk);
+
+	mutex_lock(&mvm->mutex);
+	iwl_mvm_fw_error_dump(mvm);
+	mutex_unlock(&mvm->mutex);
+}
+
 void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error)
 {
 	iwl_abort_notification_waits(&mvm->notif_wait);
@@ -846,6 +863,8 @@ void iwl_mvm_nic_restart(struct iwl_mvm *mvm, bool fw_error)
 		if (fw_error && mvm->restart_fw > 0)
 			mvm->restart_fw--;
 		ieee80211_restart_hw(mvm->hw);
+	} else if (fw_error) {
+		schedule_work(&mvm->fw_error_dump_wk);
 	}
 }
 
diff --git a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
index 6cc243f7cf60..12283b55ee84 100644
--- a/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/power.c b/drivers/net/wireless/iwlwifi/mvm/power.c
index 2b2d10800a55..5b85b0cc7a2a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/power.c
+++ b/drivers/net/wireless/iwlwifi/mvm/power.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -198,8 +200,15 @@ static void iwl_mvm_power_configure_uapsd(struct iwl_mvm *mvm,
 		}
 	}
 
-	if (!(cmd->flags & cpu_to_le16(POWER_FLAGS_ADVANCE_PM_ENA_MSK)))
+	if (!(cmd->flags & cpu_to_le16(POWER_FLAGS_ADVANCE_PM_ENA_MSK))) {
+#ifdef CONFIG_IWLWIFI_DEBUGFS
+		/* set advanced pm flag with no uapsd ACs to enable ps-poll */
+		if (mvmvif->dbgfs_pm.use_ps_poll)
+			cmd->flags |=
+				cpu_to_le16(POWER_FLAGS_ADVANCE_PM_ENA_MSK);
+#endif
 		return;
+	}
 
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_UAPSD_MISBEHAVING_ENA_MSK);
 
@@ -277,13 +286,28 @@ static bool iwl_mvm_power_allow_uapsd(struct iwl_mvm *mvm,
 	return true;
 }
 
+static bool iwl_mvm_power_is_radar(struct ieee80211_vif *vif)
+{
+	struct ieee80211_chanctx_conf *chanctx_conf;
+	struct ieee80211_channel *chan;
+	bool radar_detect = false;
+
+	rcu_read_lock();
+	chanctx_conf = rcu_dereference(vif->chanctx_conf);
+	WARN_ON(!chanctx_conf);
+	if (chanctx_conf) {
+		chan = chanctx_conf->def.chan;
+		radar_detect = chan->flags & IEEE80211_CHAN_RADAR;
+	}
+	rcu_read_unlock();
+
+	return radar_detect;
+}
+
 static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 				    struct ieee80211_vif *vif,
 				    struct iwl_mac_power_cmd *cmd)
 {
-	struct ieee80211_hw *hw = mvm->hw;
-	struct ieee80211_chanctx_conf *chanctx_conf;
-	struct ieee80211_channel *chan;
 	int dtimper, dtimper_msec;
 	int keep_alive;
 	bool radar_detect = false;
@@ -292,7 +316,7 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 
 	cmd->id_and_color = cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id,
 							    mvmvif->color));
-	dtimper = hw->conf.ps_dtim_period ?: 1;
+	dtimper = vif->bss_conf.dtim_period;
 
 	/*
 	 * Regardless of power management state the driver must set
@@ -312,7 +336,7 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
 
 	if (!vif->bss_conf.ps || iwl_mvm_vif_low_latency(mvmvif) ||
-	    !mvmvif->pm_enabled)
+	    !mvmvif->pm_enabled || iwl_mvm_tdls_sta_count(mvm, vif))
 		return;
 
 	cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_MANAGEMENT_ENA_MSK);
@@ -325,14 +349,7 @@ static void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm,
 	}
 
 	/* Check if radar detection is required on current channel */
-	rcu_read_lock();
-	chanctx_conf = rcu_dereference(vif->chanctx_conf);
-	WARN_ON(!chanctx_conf);
-	if (chanctx_conf) {
-		chan = chanctx_conf->def.chan;
-		radar_detect = chan->flags & IEEE80211_CHAN_RADAR;
-	}
-	rcu_read_unlock();
+	radar_detect = iwl_mvm_power_is_radar(vif);
 
 	/* Check skip over DTIM conditions */
 	if (!radar_detect && (dtimper <= 10) &&
@@ -493,17 +510,33 @@ struct iwl_power_vifs {
 	bool bss_active;
 	bool ap_active;
 	bool monitor_active;
-	bool bss_tdls;
-	bool p2p_tdls;
 };
 
-static void iwl_mvm_power_iterator(void *_data, u8 *mac,
-				   struct ieee80211_vif *vif)
+static void iwl_mvm_power_disable_pm_iterator(void *_data, u8* mac,
+					      struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	struct iwl_power_vifs *power_iterator = _data;
 
 	mvmvif->pm_enabled = false;
+}
+
+static void iwl_mvm_power_ps_disabled_iterator(void *_data, u8* mac,
+					       struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	bool *disable_ps = _data;
+
+	if (mvmvif->phy_ctxt)
+		if (mvmvif->phy_ctxt->id < MAX_PHYS)
+			*disable_ps |= mvmvif->ps_disabled;
+}
+
+static void iwl_mvm_power_get_vifs_iterator(void *_data, u8 *mac,
+					    struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_power_vifs *power_iterator = _data;
+
 	switch (ieee80211_vif_type_p2p(vif)) {
 	case NL80211_IFTYPE_P2P_DEVICE:
 		break;
@@ -531,8 +564,6 @@ static void iwl_mvm_power_iterator(void *_data, u8 *mac,
 		/* only a single MAC of the same type */
 		WARN_ON(power_iterator->p2p_vif);
 		power_iterator->p2p_vif = vif;
-		power_iterator->p2p_tdls =
-			!!iwl_mvm_tdls_sta_count(power_iterator->mvm, vif);
 		if (mvmvif->phy_ctxt)
 			if (mvmvif->phy_ctxt->id < MAX_PHYS)
 				power_iterator->p2p_active = true;
@@ -542,8 +573,6 @@ static void iwl_mvm_power_iterator(void *_data, u8 *mac,
 		/* only a single MAC of the same type */
 		WARN_ON(power_iterator->bss_vif);
 		power_iterator->bss_vif = vif;
-		power_iterator->bss_tdls =
-			!!iwl_mvm_tdls_sta_count(power_iterator->mvm, vif);
 		if (mvmvif->phy_ctxt)
 			if (mvmvif->phy_ctxt->id < MAX_PHYS)
 				power_iterator->bss_active = true;
@@ -559,9 +588,8 @@ static void iwl_mvm_power_iterator(void *_data, u8 *mac,
 	}
 }
 
-static void
-iwl_mvm_power_set_pm(struct iwl_mvm *mvm,
-				    struct iwl_power_vifs *vifs)
+static void iwl_mvm_power_set_pm(struct iwl_mvm *mvm,
+				 struct iwl_power_vifs *vifs)
 {
 	struct iwl_mvm_vif *bss_mvmvif = NULL;
 	struct iwl_mvm_vif *p2p_mvmvif = NULL;
@@ -571,10 +599,11 @@ iwl_mvm_power_set_pm(struct iwl_mvm *mvm,
 
 	lockdep_assert_held(&mvm->mutex);
 
-	/* get vifs info + set pm_enable to false */
+	/* set pm_enable to false */
 	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
-					    IEEE80211_IFACE_ITER_NORMAL,
-					    iwl_mvm_power_iterator, vifs);
+					IEEE80211_IFACE_ITER_NORMAL,
+					iwl_mvm_power_disable_pm_iterator,
+					NULL);
 
 	if (vifs->bss_vif)
 		bss_mvmvif = iwl_mvm_vif_from_mac80211(vifs->bss_vif);
@@ -586,15 +615,13 @@ iwl_mvm_power_set_pm(struct iwl_mvm *mvm,
 		ap_mvmvif = iwl_mvm_vif_from_mac80211(vifs->ap_vif);
 
 	/* enable PM on bss if bss stand alone */
-	if (vifs->bss_active && !vifs->p2p_active && !vifs->ap_active &&
-	    !vifs->bss_tdls) {
+	if (vifs->bss_active && !vifs->p2p_active && !vifs->ap_active) {
 		bss_mvmvif->pm_enabled = true;
 		return;
 	}
 
 	/* enable PM on p2p if p2p stand alone */
-	if (vifs->p2p_active && !vifs->bss_active && !vifs->ap_active &&
-	    !vifs->p2p_tdls) {
+	if (vifs->p2p_active && !vifs->bss_active && !vifs->ap_active) {
 		if (mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_P2P_PM)
 			p2p_mvmvif->pm_enabled = true;
 		return;
@@ -817,32 +844,92 @@ int iwl_mvm_disable_beacon_filter(struct iwl_mvm *mvm,
 	return ret;
 }
 
-int iwl_mvm_power_update_mac(struct iwl_mvm *mvm)
+static int iwl_mvm_power_set_ps(struct iwl_mvm *mvm)
+{
+	bool disable_ps;
+	int ret;
+
+	/* disable PS if CAM */
+	disable_ps = (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM);
+	/* ...or if any of the vifs require PS to be off */
+	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
+					IEEE80211_IFACE_ITER_NORMAL,
+					iwl_mvm_power_ps_disabled_iterator,
+					&disable_ps);
+
+	/* update device power state if it has changed */
+	if (mvm->ps_disabled != disable_ps) {
+		bool old_ps_disabled = mvm->ps_disabled;
+
+		mvm->ps_disabled = disable_ps;
+		ret = iwl_mvm_power_update_device(mvm);
+		if (ret) {
+			mvm->ps_disabled = old_ps_disabled;
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+static int iwl_mvm_power_set_ba(struct iwl_mvm *mvm,
+				struct iwl_power_vifs *vifs)
 {
 	struct iwl_mvm_vif *mvmvif;
+	bool ba_enable;
+
+	if (!vifs->bf_vif)
+		return 0;
+
+	mvmvif = iwl_mvm_vif_from_mac80211(vifs->bf_vif);
+
+	ba_enable = !(!mvmvif->pm_enabled || mvm->ps_disabled ||
+		      !vifs->bf_vif->bss_conf.ps ||
+		      iwl_mvm_vif_low_latency(mvmvif));
+
+	return iwl_mvm_update_beacon_abort(mvm, vifs->bf_vif, ba_enable);
+}
+
+int iwl_mvm_power_update_ps(struct iwl_mvm *mvm)
+{
+	struct iwl_power_vifs vifs = {
+		.mvm = mvm,
+	};
+	int ret;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	/* get vifs info */
+	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
+					IEEE80211_IFACE_ITER_NORMAL,
+					iwl_mvm_power_get_vifs_iterator, &vifs);
+
+	ret = iwl_mvm_power_set_ps(mvm);
+	if (ret)
+		return ret;
+
+	return iwl_mvm_power_set_ba(mvm, &vifs);
+}
+
+int iwl_mvm_power_update_mac(struct iwl_mvm *mvm)
+{
 	struct iwl_power_vifs vifs = {
 		.mvm = mvm,
 	};
-	bool ba_enable;
 	int ret;
 
 	lockdep_assert_held(&mvm->mutex);
 
+	/* get vifs info */
+	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
+					IEEE80211_IFACE_ITER_NORMAL,
+					iwl_mvm_power_get_vifs_iterator, &vifs);
+
 	iwl_mvm_power_set_pm(mvm, &vifs);
 
-	/* disable PS if CAM */
-	if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM) {
-		mvm->ps_disabled = true;
-	} else {
-	/* don't update device power state unless we add / remove monitor */
-		if (vifs.monitor_vif) {
-			if (vifs.monitor_active)
-				mvm->ps_disabled = true;
-			ret = iwl_mvm_power_update_device(mvm);
-			if (ret)
-				return ret;
-		}
-	}
+	ret = iwl_mvm_power_set_ps(mvm);
+	if (ret)
+		return ret;
 
 	if (vifs.bss_vif) {
 		ret = iwl_mvm_power_send_cmd(mvm, vifs.bss_vif);
@@ -856,16 +943,7 @@ int iwl_mvm_power_update_mac(struct iwl_mvm *mvm)
 			return ret;
 	}
 
-	if (!vifs.bf_vif)
-		return 0;
-
-	mvmvif = iwl_mvm_vif_from_mac80211(vifs.bf_vif);
-
-	ba_enable = !(!mvmvif->pm_enabled || mvm->ps_disabled ||
-		      !vifs.bf_vif->bss_conf.ps ||
-		      iwl_mvm_vif_low_latency(mvmvif));
-
-	return iwl_mvm_update_beacon_abort(mvm, vifs.bf_vif, ba_enable);
+	return iwl_mvm_power_set_ba(mvm, &vifs);
 }
 
 int iwl_mvm_update_d0i3_power_mode(struct iwl_mvm *mvm,
@@ -884,17 +962,22 @@ int iwl_mvm_update_d0i3_power_mode(struct iwl_mvm *mvm,
 
 	iwl_mvm_power_build_cmd(mvm, vif, &cmd);
 	if (enable) {
-		/* configure skip over dtim up to 300 msec */
-		int dtimper = mvm->hw->conf.ps_dtim_period ?: 1;
-		int dtimper_msec = dtimper * vif->bss_conf.beacon_int;
+		/* configure skip over dtim up to 306TU - 314 msec */
+		int dtimper = vif->bss_conf.dtim_period ?: 1;
+		int dtimper_tu = dtimper * vif->bss_conf.beacon_int;
+		bool radar_detect = iwl_mvm_power_is_radar(vif);
 
-		if (WARN_ON(!dtimper_msec))
+		if (WARN_ON(!dtimper_tu))
 			return 0;
 
-		cmd.skip_dtim_periods = 300 / dtimper_msec;
-		if (cmd.skip_dtim_periods)
-			cmd.flags |=
-				cpu_to_le16(POWER_FLAGS_SKIP_OVER_DTIM_MSK);
+		/* Check skip over DTIM conditions */
+		/* TODO: check that multicast wake lock is off */
+		if (!radar_detect && (dtimper < 10)) {
+			cmd.skip_dtim_periods = 306 / dtimper_tu;
+			if (cmd.skip_dtim_periods)
+				cmd.flags |= cpu_to_le16(
+					POWER_FLAGS_SKIP_OVER_DTIM_MSK);
+		}
 	}
 	iwl_mvm_power_log(mvm, &cmd);
 #ifdef CONFIG_IWLWIFI_DEBUGFS
diff --git a/drivers/net/wireless/iwlwifi/mvm/quota.c b/drivers/net/wireless/iwlwifi/mvm/quota.c
index 4e20b3ce2b6a..dbb2594390e9 100644
--- a/drivers/net/wireless/iwlwifi/mvm/quota.c
+++ b/drivers/net/wireless/iwlwifi/mvm/quota.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -161,6 +163,9 @@ static void iwl_mvm_adjust_quota_for_noa(struct iwl_mvm *mvm,
 		quota *= (beacon_int - mvm->noa_duration);
 		quota /= beacon_int;
 
+		IWL_DEBUG_QUOTA(mvm, "quota: adjust for NoA from %d to %d\n",
+				le32_to_cpu(cmd->quotas[i].quota), quota);
+
 		cmd->quotas[i].quota = cpu_to_le32(quota);
 	}
 #endif
@@ -170,12 +175,14 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
 			  struct ieee80211_vif *disabled_vif)
 {
 	struct iwl_time_quota_cmd cmd = {};
-	int i, idx, ret, num_active_macs, quota, quota_rem, n_non_lowlat;
+	int i, idx, err, num_active_macs, quota, quota_rem, n_non_lowlat;
 	struct iwl_mvm_quota_iterator_data data = {
 		.n_interfaces = {},
 		.colors = { -1, -1, -1, -1 },
 		.disabled_vif = disabled_vif,
 	};
+	struct iwl_time_quota_cmd *last = &mvm->last_quota_cmd;
+	bool send = false;
 
 	lockdep_assert_held(&mvm->mutex);
 
@@ -222,6 +229,9 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
 		quota = (QUOTA_100 - QUOTA_LOWLAT_MIN) / n_non_lowlat;
 		quota_rem = QUOTA_100 - n_non_lowlat * quota -
 			    QUOTA_LOWLAT_MIN;
+		IWL_DEBUG_QUOTA(mvm,
+				"quota: low-latency binding active, remaining quota per other binding: %d\n",
+				quota);
 	} else if (num_active_macs) {
 		/*
 		 * There are 0 or more than 1 low latency bindings, or all the
@@ -230,6 +240,9 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
 		 */
 		quota = QUOTA_100 / num_active_macs;
 		quota_rem = QUOTA_100 % num_active_macs;
+		IWL_DEBUG_QUOTA(mvm,
+				"quota: splitting evenly per binding: %d\n",
+				quota);
 	} else {
 		/* values don't really matter - won't be used */
 		quota = 0;
@@ -271,6 +284,9 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
 	for (i = 0; i < MAX_BINDINGS; i++) {
 		if (le32_to_cpu(cmd.quotas[i].quota) != 0) {
 			le32_add_cpu(&cmd.quotas[i].quota, quota_rem);
+			IWL_DEBUG_QUOTA(mvm,
+					"quota: giving remainder of %d to binding %d\n",
+					quota_rem, i);
 			break;
 		}
 	}
@@ -279,15 +295,33 @@ int iwl_mvm_update_quotas(struct iwl_mvm *mvm,
 
 	/* check that we have non-zero quota for all valid bindings */
 	for (i = 0; i < MAX_BINDINGS; i++) {
+		if (cmd.quotas[i].id_and_color != last->quotas[i].id_and_color)
+			send = true;
+		if (cmd.quotas[i].max_duration != last->quotas[i].max_duration)
+			send = true;
+		if (abs((int)le32_to_cpu(cmd.quotas[i].quota) -
+			(int)le32_to_cpu(last->quotas[i].quota))
+						> IWL_MVM_QUOTA_THRESHOLD)
+			send = true;
 		if (cmd.quotas[i].id_and_color == cpu_to_le32(FW_CTXT_INVALID))
 			continue;
 		WARN_ONCE(cmd.quotas[i].quota == 0,
 			  "zero quota on binding %d\n", i);
 	}
 
-	ret = iwl_mvm_send_cmd_pdu(mvm, TIME_QUOTA_CMD, 0,
-				   sizeof(cmd), &cmd);
-	if (ret)
-		IWL_ERR(mvm, "Failed to send quota: %d\n", ret);
-	return ret;
+	if (!send) {
+		/* don't send a practically unchanged command, the firmware has
+		 * to re-initialize a lot of state and that can have an adverse
+		 * impact on it
+		 */
+		return 0;
+	}
+
+	err = iwl_mvm_send_cmd_pdu(mvm, TIME_QUOTA_CMD, 0, sizeof(cmd), &cmd);
+
+	if (err)
+		IWL_ERR(mvm, "Failed to send quota: %d\n", err);
+	else
+		mvm->last_quota_cmd = cmd;
+	return err;
 }
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.c b/drivers/net/wireless/iwlwifi/mvm/rs.c
index c70e959bf0e3..18a539999580 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.c
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -376,9 +377,9 @@ static int iwl_hwrate_to_plcp_idx(u32 rate_n_flags)
 }
 
 static void rs_rate_scale_perform(struct iwl_mvm *mvm,
-				   struct sk_buff *skb,
-				   struct ieee80211_sta *sta,
-				   struct iwl_lq_sta *lq_sta);
+				  struct ieee80211_sta *sta,
+				  struct iwl_lq_sta *lq_sta,
+				  int tid);
 static void rs_fill_lq_cmd(struct iwl_mvm *mvm,
 			   struct ieee80211_sta *sta,
 			   struct iwl_lq_sta *lq_sta,
@@ -504,10 +505,10 @@ static const char *rs_pretty_lq_type(enum iwl_table_type type)
 static inline void rs_dump_rate(struct iwl_mvm *mvm, const struct rs_rate *rate,
 				const char *prefix)
 {
-	IWL_DEBUG_RATE(mvm, "%s: (%s: %d) ANT: %s BW: %d SGI: %d\n",
+	IWL_DEBUG_RATE(mvm, "%s: (%s: %d) ANT: %s BW: %d SGI: %d LDPC: %d\n",
 		       prefix, rs_pretty_lq_type(rate->type),
 		       rate->index, rs_pretty_ant(rate->ant),
-		       rate->bw, rate->sgi);
+		       rate->bw, rate->sgi, rate->ldpc);
 }
 
 static void rs_rate_scale_clear_window(struct iwl_rate_scale_data *window)
@@ -671,8 +672,10 @@ static int rs_collect_tx_data(struct iwl_lq_sta *lq_sta,
 		return -EINVAL;
 
 	if (tbl->column != RS_COLUMN_INVALID) {
-		lq_sta->tx_stats[tbl->column][scale_index].total += attempts;
-		lq_sta->tx_stats[tbl->column][scale_index].success += successes;
+		struct lq_sta_pers *pers = &lq_sta->pers;
+
+		pers->tx_stats[tbl->column][scale_index].total += attempts;
+		pers->tx_stats[tbl->column][scale_index].success += successes;
 	}
 
 	/* Select window for current tx bit rate */
@@ -741,6 +744,8 @@ static u32 ucode_rate_from_rs_rate(struct iwl_mvm *mvm,
 	ucode_rate |= rate->bw;
 	if (rate->sgi)
 		ucode_rate |= RATE_MCS_SGI_MSK;
+	if (rate->ldpc)
+		ucode_rate |= RATE_MCS_LDPC_MSK;
 
 	return ucode_rate;
 }
@@ -778,6 +783,8 @@ static int rs_rate_from_ucode_rate(const u32 ucode_rate,
 	/* HT or VHT */
 	if (ucode_rate & RATE_MCS_SGI_MSK)
 		rate->sgi = true;
+	if (ucode_rate & RATE_MCS_LDPC_MSK)
+		rate->ldpc = true;
 
 	rate->bw = ucode_rate & RATE_MCS_CHAN_WIDTH_MSK;
 
@@ -964,13 +971,13 @@ static void rs_get_lower_rate_down_column(struct iwl_lq_sta *lq_sta,
 			     rate->index > IWL_RATE_MCS_9_INDEX);
 
 		rate->index = rs_ht_to_legacy[rate->index];
+		rate->ldpc = false;
 	} else {
 		/* Downgrade to SISO with same MCS if in MIMO  */
 		rate->type = is_vht_mimo2(rate) ?
 			LQ_VHT_SISO : LQ_HT_SISO;
 	}
 
-
 	if (num_of_ant(rate->ant) > 1)
 		rate->ant = first_antenna(mvm->fw->valid_tx_ant);
 
@@ -1000,27 +1007,35 @@ static u32 rs_ch_width_from_mac_flags(enum mac80211_rate_control_flags flags)
 	return RATE_MCS_CHAN_WIDTH_20;
 }
 
-/*
- * mac80211 sends us Tx status
- */
-static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
-			 struct ieee80211_sta *sta, void *priv_sta,
-			 struct sk_buff *skb)
+static u8 rs_get_tid(struct ieee80211_hdr *hdr)
+{
+	u8 tid = IWL_MAX_TID_COUNT;
+
+	if (ieee80211_is_data_qos(hdr->frame_control)) {
+		u8 *qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & 0xf;
+	}
+
+	if (unlikely(tid > IWL_MAX_TID_COUNT))
+		tid = IWL_MAX_TID_COUNT;
+
+	return tid;
+}
+
+void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
+			  int tid, struct ieee80211_tx_info *info)
 {
 	int legacy_success;
 	int retries;
 	int mac_index, i;
-	struct iwl_lq_sta *lq_sta = priv_sta;
 	struct iwl_lq_cmd *table;
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-	struct iwl_op_mode *op_mode = (struct iwl_op_mode *)mvm_r;
-	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	enum mac80211_rate_control_flags mac_flags;
 	u32 ucode_rate;
 	struct rs_rate rate;
 	struct iwl_scale_tbl_info *curr_tbl, *other_tbl, *tmp_tbl;
 	u8 reduced_txp = (uintptr_t)info->status.status_driver_data[0];
+	struct iwl_mvm_sta *mvmsta = iwl_mvm_sta_from_mac80211(sta);
+	struct iwl_lq_sta *lq_sta = &mvmsta->lq_sta;
 
 	/* Treat uninitialized rate scaling data same as non-existing. */
 	if (!lq_sta) {
@@ -1038,10 +1053,6 @@ static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
 		return;
 	}
 #endif
-	if (!ieee80211_is_data(hdr->frame_control) ||
-	    info->flags & IEEE80211_TX_CTL_NO_ACK)
-		return;
-
 	/* This packet was aggregated but doesn't carry status info */
 	if ((info->flags & IEEE80211_TX_CTL_AMPDU) &&
 	    !(info->flags & IEEE80211_TX_STAT_AMPDU))
@@ -1087,7 +1098,7 @@ static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
 		for (tid = 0; tid < IWL_MAX_TID_COUNT; tid++)
 			ieee80211_stop_tx_ba_session(sta, tid);
 
-		iwl_mvm_rs_rate_init(mvm, sta, sband->band, false);
+		iwl_mvm_rs_rate_init(mvm, sta, info->band, false);
 		return;
 	}
 	lq_sta->last_tx = jiffies;
@@ -1214,8 +1225,28 @@ static void rs_tx_status(void *mvm_r, struct ieee80211_supported_band *sband,
 	IWL_DEBUG_RATE(mvm, "reduced txpower: %d\n", reduced_txp);
 done:
 	/* See if there's a better rate or modulation mode to try. */
-	if (sta && sta->supp_rates[sband->band])
-		rs_rate_scale_perform(mvm, skb, sta, lq_sta);
+	if (sta && sta->supp_rates[info->band])
+		rs_rate_scale_perform(mvm, sta, lq_sta, tid);
+}
+
+/*
+ * mac80211 sends us Tx status
+ */
+static void rs_mac80211_tx_status(void *mvm_r,
+				  struct ieee80211_supported_band *sband,
+				  struct ieee80211_sta *sta, void *priv_sta,
+				  struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct iwl_op_mode *op_mode = (struct iwl_op_mode *)mvm_r;
+	struct iwl_mvm *mvm = IWL_OP_MODE_GET_MVM(op_mode);
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+	if (!ieee80211_is_data(hdr->frame_control) ||
+	    info->flags & IEEE80211_TX_CTL_NO_ACK)
+		return;
+
+	iwl_mvm_rs_tx_status(mvm, sta, rs_get_tid(hdr), info);
 }
 
 /*
@@ -1486,22 +1517,6 @@ static void rs_update_rate_tbl(struct iwl_mvm *mvm,
 	iwl_mvm_send_lq_cmd(mvm, &lq_sta->lq, false);
 }
 
-static u8 rs_get_tid(struct iwl_lq_sta *lq_data,
-		     struct ieee80211_hdr *hdr)
-{
-	u8 tid = IWL_MAX_TID_COUNT;
-
-	if (ieee80211_is_data_qos(hdr->frame_control)) {
-		u8 *qc = ieee80211_get_qos_ctl(hdr);
-		tid = qc[0] & 0xf;
-	}
-
-	if (unlikely(tid > IWL_MAX_TID_COUNT))
-		tid = IWL_MAX_TID_COUNT;
-
-	return tid;
-}
-
 static enum rs_column rs_get_next_column(struct iwl_mvm *mvm,
 					 struct iwl_lq_sta *lq_sta,
 					 struct ieee80211_sta *sta,
@@ -1620,6 +1635,7 @@ static int rs_switch_to_column(struct iwl_mvm *mvm,
 	}
 
 	rate->bw = rs_bw_from_sta_bw(sta);
+	rate->ldpc = lq_sta->ldpc;
 	search_tbl->column = col_id;
 	rs_set_expected_tpt_table(lq_sta, search_tbl);
 
@@ -1939,12 +1955,10 @@ static bool rs_tpc_perform(struct iwl_mvm *mvm,
  * Do rate scaling and search for new modulation mode.
  */
 static void rs_rate_scale_perform(struct iwl_mvm *mvm,
-				  struct sk_buff *skb,
 				  struct ieee80211_sta *sta,
-				  struct iwl_lq_sta *lq_sta)
+				  struct iwl_lq_sta *lq_sta,
+				  int tid)
 {
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	int low = IWL_RATE_INVALID;
 	int high = IWL_RATE_INVALID;
 	int index;
@@ -1961,29 +1975,12 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 	u8 done_search = 0;
 	u16 high_low;
 	s32 sr;
-	u8 tid = IWL_MAX_TID_COUNT;
 	u8 prev_agg = lq_sta->is_agg;
 	struct iwl_mvm_sta *sta_priv = (void *)sta->drv_priv;
 	struct iwl_mvm_tid_data *tid_data;
 	struct rs_rate *rate;
 
-	/* Send management frames and NO_ACK data using lowest rate. */
-	/* TODO: this could probably be improved.. */
-	if (!ieee80211_is_data(hdr->frame_control) ||
-	    info->flags & IEEE80211_TX_CTL_NO_ACK)
-		return;
-
-	tid = rs_get_tid(lq_sta, hdr);
-	if ((tid != IWL_MAX_TID_COUNT) &&
-	    (lq_sta->tx_agg_tid_en & (1 << tid))) {
-		tid_data = &sta_priv->tid_data[tid];
-		if (tid_data->state == IWL_AGG_OFF)
-			lq_sta->is_agg = 0;
-		else
-			lq_sta->is_agg = 1;
-	} else {
-		lq_sta->is_agg = 0;
-	}
+	lq_sta->is_agg = !!sta_priv->agg_tids;
 
 	/*
 	 * Select rate-scale / modulation-mode table to work with in
@@ -2030,18 +2027,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 		return;
 	}
 
-	/* force user max rate if set by user */
-	if ((lq_sta->max_rate_idx != -1) &&
-	    (lq_sta->max_rate_idx < index)) {
-		index = lq_sta->max_rate_idx;
-		update_lq = 1;
-		window = &(tbl->win[index]);
-		IWL_DEBUG_RATE(mvm,
-			       "Forcing user max rate %d\n",
-			       index);
-		goto lq_update;
-	}
-
+	/* TODO: handle rate_idx_mask and rate_idx_mcs_mask */
 	window = &(tbl->win[index]);
 
 	/*
@@ -2129,10 +2115,7 @@ static void rs_rate_scale_perform(struct iwl_mvm *mvm,
 	low = high_low & 0xff;
 	high = (high_low >> 8) & 0xff;
 
-	/* If user set max rate, dont allow higher than user constrain */
-	if ((lq_sta->max_rate_idx != -1) &&
-	    (lq_sta->max_rate_idx < high))
-		high = IWL_RATE_INVALID;
+	/* TODO: handle rate_idx_mask and rate_idx_mcs_mask */
 
 	sr = window->success_ratio;
 
@@ -2294,6 +2277,110 @@ out:
 	lq_sta->last_txrate_idx = index;
 }
 
+struct rs_init_rate_info {
+	s8 rssi;
+	u8 rate_idx;
+};
+
+static const struct rs_init_rate_info rs_init_rates_24ghz[] = {
+	{ -60, IWL_RATE_54M_INDEX },
+	{ -64, IWL_RATE_48M_INDEX },
+	{ -68, IWL_RATE_36M_INDEX },
+	{ -80, IWL_RATE_24M_INDEX },
+	{ -84, IWL_RATE_18M_INDEX },
+	{ -85, IWL_RATE_12M_INDEX },
+	{ -86, IWL_RATE_11M_INDEX },
+	{ -88, IWL_RATE_5M_INDEX  },
+	{ -90, IWL_RATE_2M_INDEX  },
+	{ S8_MIN, IWL_RATE_1M_INDEX },
+};
+
+static const struct rs_init_rate_info rs_init_rates_5ghz[] = {
+	{ -60, IWL_RATE_54M_INDEX },
+	{ -64, IWL_RATE_48M_INDEX },
+	{ -72, IWL_RATE_36M_INDEX },
+	{ -80, IWL_RATE_24M_INDEX },
+	{ -84, IWL_RATE_18M_INDEX },
+	{ -85, IWL_RATE_12M_INDEX },
+	{ -87, IWL_RATE_9M_INDEX  },
+	{ S8_MIN, IWL_RATE_6M_INDEX },
+};
+
+/* Choose an initial legacy rate and antenna to use based on the RSSI
+ * of last Rx
+ */
+static void rs_get_initial_rate(struct iwl_mvm *mvm,
+				struct iwl_lq_sta *lq_sta,
+				enum ieee80211_band band,
+				struct rs_rate *rate)
+{
+	int i, nentries;
+	s8 best_rssi = S8_MIN;
+	u8 best_ant = ANT_NONE;
+	u8 valid_tx_ant = mvm->fw->valid_tx_ant;
+	const struct rs_init_rate_info *initial_rates;
+
+	for (i = 0; i < ARRAY_SIZE(lq_sta->pers.chain_signal); i++) {
+		if (!(lq_sta->pers.chains & BIT(i)))
+			continue;
+
+		if (lq_sta->pers.chain_signal[i] > best_rssi) {
+			best_rssi = lq_sta->pers.chain_signal[i];
+			best_ant = BIT(i);
+		}
+	}
+
+	IWL_DEBUG_RATE(mvm, "Best ANT: %s Best RSSI: %d\n",
+		       rs_pretty_ant(best_ant), best_rssi);
+
+	if (best_ant != ANT_A && best_ant != ANT_B)
+		rate->ant = first_antenna(valid_tx_ant);
+	else
+		rate->ant = best_ant;
+
+	rate->sgi = false;
+	rate->ldpc = false;
+	rate->bw = RATE_MCS_CHAN_WIDTH_20;
+
+	rate->index = find_first_bit(&lq_sta->active_legacy_rate,
+				     BITS_PER_LONG);
+
+	if (band == IEEE80211_BAND_5GHZ) {
+		rate->type = LQ_LEGACY_A;
+		initial_rates = rs_init_rates_5ghz;
+		nentries = ARRAY_SIZE(rs_init_rates_5ghz);
+	} else {
+		rate->type = LQ_LEGACY_G;
+		initial_rates = rs_init_rates_24ghz;
+		nentries = ARRAY_SIZE(rs_init_rates_24ghz);
+	}
+
+	if (IWL_MVM_RS_RSSI_BASED_INIT_RATE) {
+		for (i = 0; i < nentries; i++) {
+			int rate_idx = initial_rates[i].rate_idx;
+			if ((best_rssi >= initial_rates[i].rssi) &&
+			    (BIT(rate_idx) & lq_sta->active_legacy_rate)) {
+				rate->index = rate_idx;
+				break;
+			}
+		}
+	}
+
+	IWL_DEBUG_RATE(mvm, "rate_idx %d ANT %s\n", rate->index,
+		       rs_pretty_ant(rate->ant));
+}
+
+/* Save info about RSSI of last Rx */
+void rs_update_last_rssi(struct iwl_mvm *mvm,
+			 struct iwl_lq_sta *lq_sta,
+			 struct ieee80211_rx_status *rx_status)
+{
+	lq_sta->pers.chains = rx_status->chains;
+	lq_sta->pers.chain_signal[0] = rx_status->chain_signal[0];
+	lq_sta->pers.chain_signal[1] = rx_status->chain_signal[1];
+	lq_sta->pers.chain_signal[2] = rx_status->chain_signal[2];
+}
+
 /**
  * rs_initialize_lq - Initialize a station's hardware rate table
  *
@@ -2316,17 +2403,11 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
 {
 	struct iwl_scale_tbl_info *tbl;
 	struct rs_rate *rate;
-	int i;
 	u8 active_tbl = 0;
-	u8 valid_tx_ant;
 
 	if (!sta || !lq_sta)
 		return;
 
-	i = lq_sta->last_txrate_idx;
-
-	valid_tx_ant = mvm->fw->valid_tx_ant;
-
 	if (!lq_sta->search_better_tbl)
 		active_tbl = lq_sta->active_tbl;
 	else
@@ -2335,17 +2416,8 @@ static void rs_initialize_lq(struct iwl_mvm *mvm,
 	tbl = &(lq_sta->lq_info[active_tbl]);
 	rate = &tbl->rate;
 
-	if ((i < 0) || (i >= IWL_RATE_COUNT))
-		i = 0;
-
-	rate->index = i;
-	rate->ant = first_antenna(valid_tx_ant);
-	rate->sgi = false;
-	rate->bw = RATE_MCS_CHAN_WIDTH_20;
-	if (band == IEEE80211_BAND_5GHZ)
-		rate->type = LQ_LEGACY_A;
-	else
-		rate->type = LQ_LEGACY_G;
+	rs_get_initial_rate(mvm, lq_sta, band, rate);
+	lq_sta->last_txrate_idx = rate->index;
 
 	WARN_ON_ONCE(rate->ant != ANT_A && rate->ant != ANT_B);
 	if (rate->ant == ANT_A)
@@ -2363,23 +2435,13 @@ static void rs_get_rate(void *mvm_r, struct ieee80211_sta *sta, void *mvm_sta,
 			struct ieee80211_tx_rate_control *txrc)
 {
 	struct sk_buff *skb = txrc->skb;
-	struct ieee80211_supported_band *sband = txrc->sband;
 	struct iwl_op_mode *op_mode __maybe_unused =
 			(struct iwl_op_mode *)mvm_r;
 	struct iwl_mvm *mvm __maybe_unused = IWL_OP_MODE_GET_MVM(op_mode);
 	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct iwl_lq_sta *lq_sta = mvm_sta;
 
-	/* Get max rate if user set max rate */
-	if (lq_sta) {
-		lq_sta->max_rate_idx = txrc->max_rate_idx;
-		if ((sband->band == IEEE80211_BAND_5GHZ) &&
-		    (lq_sta->max_rate_idx != -1))
-			lq_sta->max_rate_idx += IWL_FIRST_OFDM_RATE;
-		if ((lq_sta->max_rate_idx < 0) ||
-		    (lq_sta->max_rate_idx >= IWL_RATE_COUNT))
-			lq_sta->max_rate_idx = -1;
-	}
+	/* TODO: handle rate_idx_mask and rate_idx_mcs_mask */
 
 	/* Treat uninitialized rate scaling data same as non-existing. */
 	if (lq_sta && !lq_sta->pers.drv) {
@@ -2412,6 +2474,8 @@ static void *rs_alloc_sta(void *mvm_rate, struct ieee80211_sta *sta,
 	lq_sta->pers.dbg_fixed_rate = 0;
 	lq_sta->pers.dbg_fixed_txp_reduction = TPC_INVALID;
 #endif
+	lq_sta->pers.chains = 0;
+	memset(lq_sta->pers.chain_signal, 0, sizeof(lq_sta->pers.chain_signal));
 
 	return &sta_priv->lq_sta;
 }
@@ -2580,7 +2644,6 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	 * previous packets? Need to have IEEE 802.1X auth succeed immediately
 	 * after assoc.. */
 
-	lq_sta->max_rate_idx = -1;
 	lq_sta->missed_rate_counter = IWL_MISSED_RATE_MAX;
 	lq_sta->band = sband->band;
 	/*
@@ -2609,9 +2672,16 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 		lq_sta->active_mimo2_rate <<= IWL_FIRST_OFDM_RATE;
 
 		lq_sta->is_vht = false;
+		if (mvm->cfg->ht_params->ldpc &&
+		    (ht_cap->cap & IEEE80211_HT_CAP_LDPC_CODING))
+			lq_sta->ldpc = true;
 	} else {
 		rs_vht_set_enabled_rates(sta, vht_cap, lq_sta);
 		lq_sta->is_vht = true;
+
+		if (mvm->cfg->ht_params->ldpc &&
+		    (vht_cap->cap & IEEE80211_VHT_CAP_RXLDPC))
+			lq_sta->ldpc = true;
 	}
 
 	lq_sta->max_legacy_rate_idx = find_last_bit(&lq_sta->active_legacy_rate,
@@ -2621,11 +2691,12 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 	lq_sta->max_mimo2_rate_idx = find_last_bit(&lq_sta->active_mimo2_rate,
 						   BITS_PER_LONG);
 
-	IWL_DEBUG_RATE(mvm, "RATE MASK: LEGACY=%lX SISO=%lX MIMO2=%lX VHT=%d\n",
+	IWL_DEBUG_RATE(mvm,
+		       "RATE MASK: LEGACY=%lX SISO=%lX MIMO2=%lX VHT=%d LDPC=%d\n",
 		       lq_sta->active_legacy_rate,
 		       lq_sta->active_siso_rate,
 		       lq_sta->active_mimo2_rate,
-		       lq_sta->is_vht);
+		       lq_sta->is_vht, lq_sta->ldpc);
 	IWL_DEBUG_RATE(mvm, "MAX RATE: LEGACY=%d SISO=%d MIMO2=%d\n",
 		       lq_sta->max_legacy_rate_idx,
 		       lq_sta->max_siso_rate_idx,
@@ -2638,11 +2709,6 @@ void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 
 	/* as default allow aggregation for all tids */
 	lq_sta->tx_agg_tid_en = IWL_AGG_ALL_TID;
-
-	/* Set last_txrate_idx to lowest rate */
-	lq_sta->last_txrate_idx = rate_lowest_index(sband, sta);
-	if (sband->band == IEEE80211_BAND_5GHZ)
-		lq_sta->last_txrate_idx += IWL_FIRST_OFDM_RATE;
 	lq_sta->is_agg = 0;
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 	iwl_mvm_reset_frame_stats(mvm, &mvm->drv_rx_stats);
@@ -2678,6 +2744,7 @@ static void rs_build_rates_table_from_fixed(struct iwl_mvm *mvm,
 	int i;
 	int num_rates = ARRAY_SIZE(lq_cmd->rs_table);
 	__le32 ucode_rate_le32 = cpu_to_le32(ucode_rate);
+	u8 ant = (ucode_rate & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS;
 
 	for (i = 0; i < num_rates; i++)
 		lq_cmd->rs_table[i] = ucode_rate_le32;
@@ -2688,6 +2755,13 @@ static void rs_build_rates_table_from_fixed(struct iwl_mvm *mvm,
 		lq_cmd->mimo_delim = num_rates - 1;
 	else
 		lq_cmd->mimo_delim = 0;
+
+	lq_cmd->reduced_tpc = 0;
+
+	if (num_of_ant(ant) == 1)
+		lq_cmd->single_stream_ant_msk = ant;
+
+	lq_cmd->agg_frame_cnt_limit = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
 }
 #endif /* CONFIG_MAC80211_DEBUGFS */
 
@@ -2811,31 +2885,55 @@ static void rs_fill_lq_cmd(struct iwl_mvm *mvm,
 			   const struct rs_rate *initial_rate)
 {
 	struct iwl_lq_cmd *lq_cmd = &lq_sta->lq;
-	u8 ant = initial_rate->ant;
+	struct iwl_mvm_sta *mvmsta;
+	struct iwl_mvm_vif *mvmvif;
+
+	lq_cmd->agg_disable_start_th = LINK_QUAL_AGG_DISABLE_START_DEF;
+	lq_cmd->agg_time_limit =
+		cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
 
 #ifdef CONFIG_MAC80211_DEBUGFS
 	if (lq_sta->pers.dbg_fixed_rate) {
 		rs_build_rates_table_from_fixed(mvm, lq_cmd,
 						lq_sta->band,
 						lq_sta->pers.dbg_fixed_rate);
-		lq_cmd->reduced_tpc = 0;
-		ant = (lq_sta->pers.dbg_fixed_rate & RATE_MCS_ANT_ABC_MSK) >>
-			RATE_MCS_ANT_POS;
-	} else
+		return;
+	}
 #endif
-		rs_build_rates_table(mvm, lq_sta, initial_rate);
+	if (WARN_ON_ONCE(!sta || !initial_rate))
+		return;
 
-	if (num_of_ant(ant) == 1)
-		lq_cmd->single_stream_ant_msk = ant;
+	rs_build_rates_table(mvm, lq_sta, initial_rate);
 
-	lq_cmd->agg_frame_cnt_limit = LINK_QUAL_AGG_FRAME_LIMIT_DEF;
-	lq_cmd->agg_disable_start_th = LINK_QUAL_AGG_DISABLE_START_DEF;
+	if (num_of_ant(initial_rate->ant) == 1)
+		lq_cmd->single_stream_ant_msk = initial_rate->ant;
 
-	lq_cmd->agg_time_limit =
-		cpu_to_le16(LINK_QUAL_AGG_TIME_LIMIT_DEF);
+	mvmsta = iwl_mvm_sta_from_mac80211(sta);
+	mvmvif = iwl_mvm_vif_from_mac80211(mvmsta->vif);
 
-	if (sta)
-		lq_cmd->agg_time_limit =
+	if (num_of_ant(initial_rate->ant) == 1)
+		lq_cmd->single_stream_ant_msk = initial_rate->ant;
+
+	lq_cmd->agg_frame_cnt_limit = mvmsta->max_agg_bufsize;
+
+	/*
+	 * In case of low latency, tell the firwmare to leave a frame in the
+	 * Tx Fifo so that it can start a transaction in the same TxOP. This
+	 * basically allows the firmware to send bursts.
+	 */
+	if (iwl_mvm_vif_low_latency(mvmvif)) {
+		lq_cmd->agg_frame_cnt_limit--;
+
+		if (mvm->low_latency_agg_frame_limit)
+			lq_cmd->agg_frame_cnt_limit =
+				min(lq_cmd->agg_frame_cnt_limit,
+				    mvm->low_latency_agg_frame_limit);
+	}
+
+	if (mvmsta->vif->p2p)
+		lq_cmd->flags |= LQ_FLAG_USE_RTS_MSK;
+
+	lq_cmd->agg_time_limit =
 			cpu_to_le16(iwl_mvm_coex_agg_time_limit(mvm, sta));
 }
 
@@ -2932,10 +3030,7 @@ static void rs_program_fix_rate(struct iwl_mvm *mvm,
 		       lq_sta->lq.sta_id, lq_sta->pers.dbg_fixed_rate);
 
 	if (lq_sta->pers.dbg_fixed_rate) {
-		struct rs_rate rate;
-		rs_rate_from_ucode_rate(lq_sta->pers.dbg_fixed_rate,
-					lq_sta->band, &rate);
-		rs_fill_lq_cmd(mvm, NULL, lq_sta, &rate);
+		rs_fill_lq_cmd(mvm, NULL, lq_sta, NULL);
 		iwl_mvm_send_lq_cmd(lq_sta->pers.drv, &lq_sta->lq, false);
 	}
 }
@@ -3002,8 +3097,9 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 				   (is_ht20(rate)) ? "20MHz" :
 				   (is_ht40(rate)) ? "40MHz" :
 				   (is_ht80(rate)) ? "80Mhz" : "BAD BW");
-		   desc += sprintf(buff+desc, " %s %s\n",
+		   desc += sprintf(buff+desc, " %s %s %s\n",
 				   (rate->sgi) ? "SGI" : "NGI",
+				   (rate->ldpc) ? "LDPC" : "BCC",
 				   (lq_sta->is_agg) ? "AGG on" : "");
 	}
 	desc += sprintf(buff+desc, "last tx rate=0x%X\n",
@@ -3151,7 +3247,7 @@ static ssize_t rs_sta_dbgfs_drv_tx_stats_read(struct file *file,
 				 "%s,", column_name[col]);
 
 		for (rate = 0; rate < IWL_RATE_COUNT; rate++) {
-			stats = &(lq_sta->tx_stats[col][rate]);
+			stats = &(lq_sta->pers.tx_stats[col][rate]);
 			pos += scnprintf(pos, endpos - pos,
 					 "%llu/%llu,",
 					 stats->success,
@@ -3170,7 +3266,7 @@ static ssize_t rs_sta_dbgfs_drv_tx_stats_write(struct file *file,
 					       size_t count, loff_t *ppos)
 {
 	struct iwl_lq_sta *lq_sta = file->private_data;
-	memset(lq_sta->tx_stats, 0, sizeof(lq_sta->tx_stats));
+	memset(lq_sta->pers.tx_stats, 0, sizeof(lq_sta->pers.tx_stats));
 
 	return count;
 }
@@ -3216,7 +3312,7 @@ static void rs_rate_init_stub(void *mvm_r,
 
 static const struct rate_control_ops rs_mvm_ops = {
 	.name = RS_NAME,
-	.tx_status = rs_tx_status,
+	.tx_status = rs_mac80211_tx_status,
 	.get_rate = rs_get_rate,
 	.rate_init = rs_rate_init_stub,
 	.alloc = rs_alloc,
diff --git a/drivers/net/wireless/iwlwifi/mvm/rs.h b/drivers/net/wireless/iwlwifi/mvm/rs.h
index f27b9d687a25..eb34c1209acc 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rs.h
+++ b/drivers/net/wireless/iwlwifi/mvm/rs.h
@@ -207,6 +207,7 @@ struct rs_rate {
 	u8 ant;
 	u32 bw;
 	bool sgi;
+	bool ldpc;
 };
 
 
@@ -329,10 +330,9 @@ struct iwl_lq_sta {
 				 */
 	u64 last_tx;
 	bool is_vht;
+	bool ldpc;              /* LDPC Rx is supported by the STA */
 	enum ieee80211_band band;
 
-	struct rs_rate_stats tx_stats[RS_COLUMN_COUNT][IWL_RATE_COUNT];
-
 	/* The following are bitmaps of rates; IWL_RATE_6M_MASK, etc. */
 	unsigned long active_legacy_rate;
 	unsigned long active_siso_rate;
@@ -343,7 +343,6 @@ struct iwl_lq_sta {
 	u8 max_siso_rate_idx;
 	u8 max_mimo2_rate_idx;
 
-	s8 max_rate_idx;     /* Max rate set by user */
 	u8 missed_rate_counter;
 
 	struct iwl_lq_cmd lq;
@@ -361,11 +360,14 @@ struct iwl_lq_sta {
 	int tpc_reduce;
 
 	/* persistent fields - initialized only once - keep last! */
-	struct {
+	struct lq_sta_pers {
 #ifdef CONFIG_MAC80211_DEBUGFS
 		u32 dbg_fixed_rate;
 		u8 dbg_fixed_txp_reduction;
 #endif
+		u8 chains;
+		s8 chain_signal[IEEE80211_MAX_CHAINS];
+		struct rs_rate_stats tx_stats[RS_COLUMN_COUNT][IWL_RATE_COUNT];
 		struct iwl_mvm *drv;
 	} pers;
 };
@@ -374,6 +376,10 @@ struct iwl_lq_sta {
 void iwl_mvm_rs_rate_init(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
 			  enum ieee80211_band band, bool init);
 
+/* Notify RS about Tx status */
+void iwl_mvm_rs_tx_status(struct iwl_mvm *mvm, struct ieee80211_sta *sta,
+			  int tid, struct ieee80211_tx_info *info);
+
 /**
  * iwl_rate_control_register - Register the rate control algorithm callbacks
  *
diff --git a/drivers/net/wireless/iwlwifi/mvm/rx.c b/drivers/net/wireless/iwlwifi/mvm/rx.c
index 4b98987fc413..3cf40f3f58ec 100644
--- a/drivers/net/wireless/iwlwifi/mvm/rx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/rx.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -149,13 +151,13 @@ static void iwl_mvm_get_signal_strength(struct iwl_mvm *mvm,
 	    le32_to_cpu(phy_info->non_cfg_phy[IWL_RX_INFO_ENERGY_ANT_ABC_IDX]);
 	energy_a = (val & IWL_RX_INFO_ENERGY_ANT_A_MSK) >>
 						IWL_RX_INFO_ENERGY_ANT_A_POS;
-	energy_a = energy_a ? -energy_a : -256;
+	energy_a = energy_a ? -energy_a : S8_MIN;
 	energy_b = (val & IWL_RX_INFO_ENERGY_ANT_B_MSK) >>
 						IWL_RX_INFO_ENERGY_ANT_B_POS;
-	energy_b = energy_b ? -energy_b : -256;
+	energy_b = energy_b ? -energy_b : S8_MIN;
 	energy_c = (val & IWL_RX_INFO_ENERGY_ANT_C_MSK) >>
 						IWL_RX_INFO_ENERGY_ANT_C_POS;
-	energy_c = energy_c ? -energy_c : -256;
+	energy_c = energy_c ? -energy_c : S8_MIN;
 	max_energy = max(energy_a, energy_b);
 	max_energy = max(max_energy, energy_c);
 
@@ -244,6 +246,7 @@ int iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 	struct iwl_rx_packet *pkt = rxb_addr(rxb);
 	struct iwl_rx_phy_info *phy_info;
 	struct iwl_rx_mpdu_res_start *rx_res;
+	struct ieee80211_sta *sta;
 	u32 len;
 	u32 ampdu_status;
 	u32 rate_n_flags;
@@ -259,23 +262,6 @@ int iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 	memset(&rx_status, 0, sizeof(rx_status));
 
 	/*
-	 * We have tx blocked stations (with CS bit). If we heard frames from
-	 * a blocked station on a new channel we can TX to it again.
-	 */
-	if (unlikely(mvm->csa_tx_block_bcn_timeout)) {
-		struct ieee80211_sta *sta;
-
-		rcu_read_lock();
-
-		sta = ieee80211_find_sta(
-			rcu_dereference(mvm->csa_tx_blocked_vif), hdr->addr2);
-		if (sta)
-			iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, false);
-
-		rcu_read_unlock();
-	}
-
-	/*
 	 * drop the packet if it has failed being decrypted by HW
 	 */
 	if (iwl_mvm_set_mac80211_rx_flag(mvm, hdr, &rx_status, rx_pkt_status)) {
@@ -323,6 +309,29 @@ int iwl_mvm_rx_rx_mpdu(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 	IWL_DEBUG_STATS_LIMIT(mvm, "Rssi %d, TSF %llu\n", rx_status.signal,
 			      (unsigned long long)rx_status.mactime);
 
+	rcu_read_lock();
+	/*
+	 * We have tx blocked stations (with CS bit). If we heard frames from
+	 * a blocked station on a new channel we can TX to it again.
+	 */
+	if (unlikely(mvm->csa_tx_block_bcn_timeout)) {
+		sta = ieee80211_find_sta(
+			rcu_dereference(mvm->csa_tx_blocked_vif), hdr->addr2);
+		if (sta)
+			iwl_mvm_sta_modify_disable_tx_ap(mvm, sta, false);
+	}
+
+	/* This is fine since we don't support multiple AP interfaces */
+	sta = ieee80211_find_sta_by_ifaddr(mvm->hw, hdr->addr2, NULL);
+	if (sta) {
+		struct iwl_mvm_sta *mvmsta;
+		mvmsta = iwl_mvm_sta_from_mac80211(sta);
+		rs_update_last_rssi(mvm, &mvmsta->lq_sta,
+				    &rx_status);
+	}
+
+	rcu_read_unlock();
+
 	/* set the preamble flag if appropriate */
 	if (phy_info->phy_flags & cpu_to_le16(RX_RES_PHY_FLAGS_SHORT_PREAMBLE))
 		rx_status.flag |= RX_FLAG_SHORTPRE;
@@ -491,10 +500,29 @@ int iwl_mvm_rx_statistics(struct iwl_mvm *mvm,
 		.mvm = mvm,
 	};
 
+	/*
+	 * set temperature debug enabled - ignore FW temperature updates
+	 * and use the user set temperature.
+	 */
+	if (mvm->temperature_test) {
+		if (mvm->temperature < le32_to_cpu(common->temperature))
+			IWL_DEBUG_TEMP(mvm,
+				       "Ignoring FW temperature update that is greater than the debug set temperature (debug temp = %d, fw temp = %d)\n",
+				       mvm->temperature,
+				       le32_to_cpu(common->temperature));
+		/*
+		 * skip iwl_mvm_tt_handler since we are in
+		 * temperature debug mode and we are ignoring
+		 * the new temperature value
+		 */
+		goto update;
+	}
+
 	if (mvm->temperature != le32_to_cpu(common->temperature)) {
 		mvm->temperature = le32_to_cpu(common->temperature);
 		iwl_mvm_tt_handler(mvm);
 	}
+update:
 	iwl_mvm_update_rx_statistics(mvm, stats);
 
 	ieee80211_iterate_active_interfaces(mvm->hw,
diff --git a/drivers/net/wireless/iwlwifi/mvm/scan.c b/drivers/net/wireless/iwlwifi/mvm/scan.c
index 004b1f5d0314..cb85e63c20aa 100644
--- a/drivers/net/wireless/iwlwifi/mvm/scan.c
+++ b/drivers/net/wireless/iwlwifi/mvm/scan.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -158,8 +160,8 @@ static void iwl_mvm_scan_fill_ssids(struct iwl_ssid_ie *cmd_ssid,
 static u16 iwl_mvm_get_active_dwell(enum ieee80211_band band, int n_ssids)
 {
 	if (band == IEEE80211_BAND_2GHZ)
-		return 30  + 3 * (n_ssids + 1);
-	return 20  + 2 * (n_ssids + 1);
+		return 20  + 3 * (n_ssids + 1);
+	return 10  + 2 * (n_ssids + 1);
 }
 
 static u16 iwl_mvm_get_passive_dwell(enum ieee80211_band band)
@@ -279,6 +281,7 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 {
 	bool global_bound = false;
 	enum ieee80211_band band;
+	u8 frag_passive_dwell = 0;
 
 	ieee80211_iterate_active_interfaces_atomic(mvm->hw,
 					    IEEE80211_IFACE_ITER_NORMAL,
@@ -288,12 +291,36 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 	if (!global_bound)
 		goto not_bound;
 
-	params->suspend_time = 100;
-	params->max_out_time = 600;
+	params->suspend_time = 30;
+	params->max_out_time = 170;
 
 	if (iwl_mvm_low_latency(mvm)) {
-		params->suspend_time = 250;
-		params->max_out_time = 250;
+		if (mvm->fw->ucode_capa.api[0] &
+		    IWL_UCODE_TLV_API_FRAGMENTED_SCAN) {
+			params->suspend_time = 105;
+			params->max_out_time = 70;
+			frag_passive_dwell = 20;
+		} else {
+			params->suspend_time = 120;
+			params->max_out_time = 120;
+		}
+	}
+
+	if (frag_passive_dwell && (mvm->fw->ucode_capa.api[0] &
+				   IWL_UCODE_TLV_API_FRAGMENTED_SCAN)) {
+		/*
+		 * P2P device scan should not be fragmented to avoid negative
+		 * impact on P2P device discovery. Configure max_out_time to be
+		 * equal to dwell time on passive channel. Take a longest
+		 * possible value, one that corresponds to 2GHz band
+		 */
+		if (vif->type == NL80211_IFTYPE_P2P_DEVICE) {
+			u32 passive_dwell =
+				iwl_mvm_get_passive_dwell(IEEE80211_BAND_2GHZ);
+			params->max_out_time = passive_dwell;
+		} else {
+			params->passive_fragmented = true;
+		}
 	}
 
 	if (flags & NL80211_SCAN_FLAG_LOW_PRIORITY)
@@ -302,12 +329,65 @@ static void iwl_mvm_scan_calc_params(struct iwl_mvm *mvm,
 not_bound:
 
 	for (band = IEEE80211_BAND_2GHZ; band < IEEE80211_NUM_BANDS; band++) {
-		params->dwell[band].passive = iwl_mvm_get_passive_dwell(band);
+		if (params->passive_fragmented)
+			params->dwell[band].passive = frag_passive_dwell;
+		else
+			params->dwell[band].passive =
+				iwl_mvm_get_passive_dwell(band);
 		params->dwell[band].active = iwl_mvm_get_active_dwell(band,
 								      n_ssids);
 	}
 }
 
+static inline bool iwl_mvm_rrm_scan_needed(struct iwl_mvm *mvm)
+{
+	/* require rrm scan whenever the fw supports it */
+	return mvm->fw->ucode_capa.capa[0] &
+	       IWL_UCODE_TLV_CAPA_DS_PARAM_SET_IE_SUPPORT;
+}
+
+static int iwl_mvm_max_scan_ie_fw_cmd_room(struct iwl_mvm *mvm,
+					   bool is_sched_scan)
+{
+	int max_probe_len;
+
+	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN)
+		max_probe_len = SCAN_OFFLOAD_PROBE_REQ_SIZE;
+	else
+		max_probe_len = mvm->fw->ucode_capa.max_probe_length;
+
+	/* we create the 802.11 header and SSID element */
+	max_probe_len -= 24 + 2;
+
+	/* basic ssid is added only for hw_scan with and old api */
+	if (!(mvm->fw->ucode_capa.flags & IWL_UCODE_TLV_FLAGS_NO_BASIC_SSID) &&
+	    !(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN) &&
+	    !is_sched_scan)
+		max_probe_len -= 32;
+
+	return max_probe_len;
+}
+
+int iwl_mvm_max_scan_ie_len(struct iwl_mvm *mvm, bool is_sched_scan)
+{
+	int max_ie_len = iwl_mvm_max_scan_ie_fw_cmd_room(mvm, is_sched_scan);
+
+	if (!(mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_LMAC_SCAN))
+		return max_ie_len;
+
+	/* TODO: [BUG] This function should return the maximum allowed size of
+	 * scan IEs, however the LMAC scan api contains both 2GHZ and 5GHZ IEs
+	 * in the same command. So the correct implementation of this function
+	 * is just iwl_mvm_max_scan_ie_fw_cmd_room() / 2. Currently the scan
+	 * command has only 512 bytes and it would leave us with about 240
+	 * bytes for scan IEs, which is clearly not enough. So meanwhile
+	 * we will report an incorrect value. This may result in a failure to
+	 * issue a scan in unified_scan_lmac and unified_sched_scan_lmac
+	 * functions with -ENOBUFS, if a large enough probe will be provided.
+	 */
+	return max_ie_len;
+}
+
 int iwl_mvm_scan_request(struct iwl_mvm *mvm,
 			 struct ieee80211_vif *vif,
 			 struct cfg80211_scan_request *req)
@@ -1100,10 +1180,11 @@ iwl_mvm_build_generic_unified_scan_cmd(struct iwl_mvm *mvm,
 				       struct iwl_mvm_scan_params *params)
 {
 	memset(cmd, 0, ksize(cmd));
-	cmd->active_dwell = (u8)params->dwell[IEEE80211_BAND_2GHZ].active;
-	cmd->passive_dwell = (u8)params->dwell[IEEE80211_BAND_2GHZ].passive;
-	/* TODO: Use params; now fragmented isn't used. */
-	cmd->fragmented_dwell = 0;
+	cmd->active_dwell = params->dwell[IEEE80211_BAND_2GHZ].active;
+	cmd->passive_dwell = params->dwell[IEEE80211_BAND_2GHZ].passive;
+	if (params->passive_fragmented)
+		cmd->fragmented_dwell =
+				params->dwell[IEEE80211_BAND_2GHZ].passive;
 	cmd->rx_chain_select = iwl_mvm_scan_rx_chain(mvm);
 	cmd->max_out_time = cpu_to_le32(params->max_out_time);
 	cmd->suspend_time = cpu_to_le32(params->suspend_time);
@@ -1121,6 +1202,10 @@ iwl_mvm_build_generic_unified_scan_cmd(struct iwl_mvm *mvm,
 				    IWL_SCAN_CHANNEL_FLAG_EBS_ACCURATE |
 				    IWL_SCAN_CHANNEL_FLAG_CACHE_ADD);
 	}
+
+	if (iwl_mvm_rrm_scan_needed(mvm))
+		cmd->scan_flags |=
+			cpu_to_le32(IWL_MVM_LMAC_SCAN_FLAGS_RRM_ENABLED);
 }
 
 int iwl_mvm_unified_scan_lmac(struct iwl_mvm *mvm,
@@ -1148,13 +1233,12 @@ int iwl_mvm_unified_scan_lmac(struct iwl_mvm *mvm,
 	if (WARN_ON(mvm->scan_cmd == NULL))
 		return -ENOMEM;
 
-	if (WARN_ON_ONCE(req->req.n_ssids > PROBE_OPTION_MAX ||
-			 req->ies.common_ie_len + req->ies.len[0] +
-				req->ies.len[1] + 24 + 2 >
-					SCAN_OFFLOAD_PROBE_REQ_SIZE ||
-			 req->req.n_channels >
-				mvm->fw->ucode_capa.n_scan_channels))
-		return -1;
+	if (req->req.n_ssids > PROBE_OPTION_MAX ||
+	    req->ies.common_ie_len + req->ies.len[NL80211_BAND_2GHZ] +
+	    req->ies.len[NL80211_BAND_5GHZ] >
+		iwl_mvm_max_scan_ie_fw_cmd_room(mvm, false) ||
+	    req->req.n_channels > mvm->fw->ucode_capa.n_scan_channels)
+		return -ENOBUFS;
 
 	mvm->scan_status = IWL_MVM_SCAN_OS;
 
@@ -1176,7 +1260,7 @@ int iwl_mvm_unified_scan_lmac(struct iwl_mvm *mvm,
 	if (req->req.n_ssids == 0)
 		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASSIVE;
 
-	cmd->scan_flags = cpu_to_le32(flags);
+	cmd->scan_flags |= cpu_to_le32(flags);
 
 	cmd->flags = iwl_mvm_scan_rxon_flags(req->req.channels[0]->band);
 	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
@@ -1242,10 +1326,11 @@ int iwl_mvm_unified_sched_scan_lmac(struct iwl_mvm *mvm,
 	if (WARN_ON(mvm->scan_cmd == NULL))
 		return -ENOMEM;
 
-	if (WARN_ON_ONCE(req->n_ssids > PROBE_OPTION_MAX ||
-			 ies->common_ie_len + ies->len[0] + ies->len[1] + 24 + 2
-				> SCAN_OFFLOAD_PROBE_REQ_SIZE ||
-			 req->n_channels > mvm->fw->ucode_capa.n_scan_channels))
+	if (req->n_ssids > PROBE_OPTION_MAX ||
+	    ies->common_ie_len + ies->len[NL80211_BAND_2GHZ] +
+	    ies->len[NL80211_BAND_5GHZ] >
+		iwl_mvm_max_scan_ie_fw_cmd_room(mvm, true) ||
+	    req->n_channels > mvm->fw->ucode_capa.n_scan_channels)
 		return -ENOBUFS;
 
 	iwl_mvm_scan_calc_params(mvm, vif, req->n_ssids, 0, &params);
@@ -1273,7 +1358,7 @@ int iwl_mvm_unified_sched_scan_lmac(struct iwl_mvm *mvm,
 	if (req->n_ssids == 0)
 		flags |= IWL_MVM_LMAC_SCAN_FLAG_PASSIVE;
 
-	cmd->scan_flags = cpu_to_le32(flags);
+	cmd->scan_flags |= cpu_to_le32(flags);
 
 	cmd->flags = iwl_mvm_scan_rxon_flags(req->channels[0]->band);
 	cmd->filter_flags = cpu_to_le32(MAC_FILTER_ACCEPT_GRP |
diff --git a/drivers/net/wireless/iwlwifi/mvm/sf.c b/drivers/net/wireless/iwlwifi/mvm/sf.c
index 7edfd15efc9d..7eb78e2c240a 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sf.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sf.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -172,11 +174,15 @@ static int iwl_mvm_sf_config(struct iwl_mvm *mvm, u8 sta_id,
 			     enum iwl_sf_state new_state)
 {
 	struct iwl_sf_cfg_cmd sf_cmd = {
-		.state = new_state,
+		.state = cpu_to_le32(new_state),
 	};
 	struct ieee80211_sta *sta;
 	int ret = 0;
 
+	if (mvm->fw->ucode_capa.api[0] & IWL_UCODE_TLV_API_SF_NO_DUMMY_NOTIF &&
+	    mvm->cfg->disable_dummy_notification)
+		sf_cmd.state |= cpu_to_le32(SF_CFG_DUMMY_NOTIF_OFF);
+
 	/*
 	 * If an associated AP sta changed its antenna configuration, the state
 	 * will remain FULL_ON but SF parameters need to be reconsidered.
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.c b/drivers/net/wireless/iwlwifi/mvm/sta.c
index 763548880399..1731c205c81d 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.c
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -245,15 +247,20 @@ int iwl_mvm_add_sta(struct iwl_mvm *mvm,
 		memset(&mvm_sta->tid_data[i], 0, sizeof(mvm_sta->tid_data[i]));
 		mvm_sta->tid_data[i].seq_number = seq;
 	}
+	mvm_sta->agg_tids = 0;
 
 	ret = iwl_mvm_sta_send_to_fw(mvm, sta, false);
 	if (ret)
 		return ret;
 
-	/* The first station added is the AP, the others are TDLS STAs */
-	if (vif->type == NL80211_IFTYPE_STATION &&
-	    mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT)
-		mvmvif->ap_sta_id = sta_id;
+	if (vif->type == NL80211_IFTYPE_STATION) {
+		if (!sta->tdls) {
+			WARN_ON(mvmvif->ap_sta_id != IWL_MVM_STATION_COUNT);
+			mvmvif->ap_sta_id = sta_id;
+		} else {
+			WARN_ON(mvmvif->ap_sta_id == IWL_MVM_STATION_COUNT);
+		}
+	}
 
 	rcu_assign_pointer(mvm->fw_id_to_mac_id[sta_id], sta);
 
@@ -458,8 +465,9 @@ int iwl_mvm_rm_sta_id(struct iwl_mvm *mvm,
 	return ret;
 }
 
-int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta,
-			     u32 qmask, enum nl80211_iftype iftype)
+static int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm,
+				    struct iwl_mvm_int_sta *sta,
+				    u32 qmask, enum nl80211_iftype iftype)
 {
 	if (!test_bit(IWL_MVM_STATUS_IN_HW_RESTART, &mvm->status)) {
 		sta->sta_id = iwl_mvm_find_free_sta_id(mvm, iftype);
@@ -474,7 +482,8 @@ int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta,
 	return 0;
 }
 
-void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta)
+static void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm,
+				    struct iwl_mvm_int_sta *sta)
 {
 	RCU_INIT_POINTER(mvm->fw_id_to_mac_id[sta->sta_id], NULL);
 	memset(sta, 0, sizeof(struct iwl_mvm_int_sta));
@@ -527,8 +536,8 @@ int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm)
 	lockdep_assert_held(&mvm->mutex);
 
 	/* Map Aux queue to fifo - needs to happen before adding Aux station */
-	iwl_trans_ac_txq_enable(mvm->trans, mvm->aux_queue,
-				IWL_MVM_TX_FIFO_MCAST);
+	iwl_mvm_enable_ac_txq(mvm, mvm->aux_queue,
+			      IWL_MVM_TX_FIFO_MCAST);
 
 	/* Allocate aux station and assign to it the aux queue */
 	ret = iwl_mvm_allocate_int_sta(mvm, &mvm->aux_sta, BIT(mvm->aux_queue),
@@ -544,6 +553,13 @@ int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm)
 	return ret;
 }
 
+void iwl_mvm_del_aux_sta(struct iwl_mvm *mvm)
+{
+	lockdep_assert_held(&mvm->mutex);
+
+	iwl_mvm_dealloc_int_sta(mvm, &mvm->aux_sta);
+}
+
 /*
  * Send the add station command for the vif's broadcast station.
  * Assumes that the station was already allocated.
@@ -552,10 +568,10 @@ int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm)
  * @vif: the interface to which the broadcast station is added
  * @bsta: the broadcast station to add.
  */
-int iwl_mvm_send_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			   struct iwl_mvm_int_sta *bsta)
+int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	struct iwl_mvm_int_sta *bsta = &mvmvif->bcast_sta;
 	static const u8 _baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
 	const u8 *baddr = _baddr;
 
@@ -573,19 +589,40 @@ int iwl_mvm_send_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 
 /* Send the FW a request to remove the station from it's internal data
  * structures, but DO NOT remove the entry from the local data structures. */
-int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm,
-			      struct iwl_mvm_int_sta *bsta)
+int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	int ret;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	ret = iwl_mvm_rm_sta_common(mvm, bsta->sta_id);
+	ret = iwl_mvm_rm_sta_common(mvm, mvmvif->bcast_sta.sta_id);
 	if (ret)
 		IWL_WARN(mvm, "Failed sending remove station\n");
 	return ret;
 }
 
+int iwl_mvm_alloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+	u32 qmask;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	qmask = iwl_mvm_mac_get_queues_mask(mvm, vif);
+
+	/*
+	 * The firmware defines the TFD queue mask to only be relevant
+	 * for *unicast* queues, so the multicast (CAB) queue shouldn't
+	 * be included.
+	 */
+	if (vif->type == NL80211_IFTYPE_AP)
+		qmask &= ~BIT(vif->cab_queue);
+
+	return iwl_mvm_allocate_int_sta(mvm, &mvmvif->bcast_sta, qmask,
+					ieee80211_vif_type_p2p(vif));
+}
+
 /* Allocate a new station entry for the broadcast station to the given vif,
  * and send it to the FW.
  * Note that each P2P mac should have its own broadcast station.
@@ -593,45 +630,47 @@ int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm,
  * @mvm: the mvm component
  * @vif: the interface to which the broadcast station is added
  * @bsta: the broadcast station to add. */
-int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			  struct iwl_mvm_int_sta *bsta)
+int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
-	static const u8 baddr[] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
-	u32 qmask;
+	struct iwl_mvm_int_sta *bsta = &mvmvif->bcast_sta;
 	int ret;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	qmask = iwl_mvm_mac_get_queues_mask(mvm, vif);
-	ret = iwl_mvm_allocate_int_sta(mvm, bsta, qmask,
-				       ieee80211_vif_type_p2p(vif));
+	ret = iwl_mvm_alloc_bcast_sta(mvm, vif);
 	if (ret)
 		return ret;
 
-	ret = iwl_mvm_add_int_sta_common(mvm, bsta, baddr,
-					 mvmvif->id, mvmvif->color);
+	ret = iwl_mvm_send_add_bcast_sta(mvm, vif);
 
 	if (ret)
 		iwl_mvm_dealloc_int_sta(mvm, bsta);
+
 	return ret;
 }
 
+void iwl_mvm_dealloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
+
+	iwl_mvm_dealloc_int_sta(mvm, &mvmvif->bcast_sta);
+}
+
 /*
  * Send the FW a request to remove the station from it's internal data
  * structures, and in addition remove it from the local data structure.
  */
-int iwl_mvm_rm_bcast_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *bsta)
+int iwl_mvm_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
 {
 	int ret;
 
 	lockdep_assert_held(&mvm->mutex);
 
-	ret = iwl_mvm_rm_sta_common(mvm, bsta->sta_id);
-	if (ret)
-		return ret;
+	ret = iwl_mvm_send_rm_bcast_sta(mvm, vif);
+
+	iwl_mvm_dealloc_bcast_sta(mvm, vif);
 
-	iwl_mvm_dealloc_int_sta(mvm, bsta);
 	return ret;
 }
 
@@ -834,12 +873,16 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	int queue, fifo, ret;
 	u16 ssn;
 
+	BUILD_BUG_ON((sizeof(mvmsta->agg_tids) * BITS_PER_BYTE)
+		     != IWL_MAX_TID_COUNT);
+
 	buf_size = min_t(int, buf_size, LINK_QUAL_AGG_FRAME_LIMIT_DEF);
 
 	spin_lock_bh(&mvmsta->lock);
 	ssn = tid_data->ssn;
 	queue = tid_data->txq_id;
 	tid_data->state = IWL_AGG_ON;
+	mvmsta->agg_tids |= BIT(tid);
 	tid_data->ssn = 0xffff;
 	spin_unlock_bh(&mvmsta->lock);
 
@@ -849,8 +892,8 @@ int iwl_mvm_sta_tx_agg_oper(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	if (ret)
 		return -EIO;
 
-	iwl_trans_txq_enable(mvm->trans, queue, fifo, mvmsta->sta_id, tid,
-			     buf_size, ssn);
+	iwl_mvm_enable_agg_txq(mvm, queue, fifo, mvmsta->sta_id, tid,
+			       buf_size, ssn);
 
 	/*
 	 * Even though in theory the peer could have different
@@ -894,6 +937,8 @@ int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 	IWL_DEBUG_TX_QUEUES(mvm, "Stop AGG: sta %d tid %d q %d state %d\n",
 			    mvmsta->sta_id, tid, txq_id, tid_data->state);
 
+	mvmsta->agg_tids &= ~BIT(tid);
+
 	switch (tid_data->state) {
 	case IWL_AGG_ON:
 		tid_data->ssn = IEEE80211_SEQ_TO_SN(tid_data->seq_number);
@@ -910,8 +955,16 @@ int iwl_mvm_sta_tx_agg_stop(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 		}
 
 		tid_data->ssn = 0xffff;
-		iwl_trans_txq_disable(mvm->trans, txq_id);
-		/* fall through */
+		tid_data->state = IWL_AGG_OFF;
+		mvm->queue_to_mac80211[txq_id] = IWL_INVALID_MAC80211_QUEUE;
+		spin_unlock_bh(&mvmsta->lock);
+
+		ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
+
+		iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
+
+		iwl_mvm_disable_txq(mvm, txq_id);
+		return 0;
 	case IWL_AGG_STARTING:
 	case IWL_EMPTYING_HW_QUEUE_ADDBA:
 		/*
@@ -959,13 +1012,16 @@ int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			    mvmsta->sta_id, tid, txq_id, tid_data->state);
 	old_state = tid_data->state;
 	tid_data->state = IWL_AGG_OFF;
+	mvmsta->agg_tids &= ~BIT(tid);
 	spin_unlock_bh(&mvmsta->lock);
 
 	if (old_state >= IWL_AGG_ON) {
 		if (iwl_mvm_flush_tx_path(mvm, BIT(txq_id), true))
 			IWL_ERR(mvm, "Couldn't flush the AGG queue\n");
 
-		iwl_trans_txq_disable(mvm->trans, tid_data->txq_id);
+		iwl_mvm_sta_tx_agg(mvm, sta, tid, txq_id, false);
+
+		iwl_mvm_disable_txq(mvm, tid_data->txq_id);
 	}
 
 	mvm->queue_to_mac80211[tid_data->txq_id] =
diff --git a/drivers/net/wireless/iwlwifi/mvm/sta.h b/drivers/net/wireless/iwlwifi/mvm/sta.h
index 3b1c8bd6cb54..d9c0d7b0e9d4 100644
--- a/drivers/net/wireless/iwlwifi/mvm/sta.h
+++ b/drivers/net/wireless/iwlwifi/mvm/sta.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -297,6 +299,7 @@ static inline u16 iwl_mvm_tid_queued(struct iwl_mvm_tid_data *tid_data)
  * @tx_protection: reference counter for controlling the Tx protection.
  * @tt_tx_protection: is thermal throttling enable Tx protection?
  * @disable_tx: is tx to this STA disabled?
+ * @agg_tids: bitmap of tids whose status is operational aggregated (IWL_AGG_ON)
  *
  * When mac80211 creates a station it reserves some space (hw->sta_data_size)
  * in the structure for use by driver. This structure is placed in that
@@ -321,6 +324,7 @@ struct iwl_mvm_sta {
 	bool tt_tx_protection;
 
 	bool disable_tx;
+	u8 agg_tids;
 };
 
 static inline struct iwl_mvm_sta *
@@ -387,17 +391,15 @@ int iwl_mvm_sta_tx_agg_flush(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
 			    struct ieee80211_sta *sta, u16 tid);
 
 int iwl_mvm_add_aux_sta(struct iwl_mvm *mvm);
-int iwl_mvm_allocate_int_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *sta,
-			     u32 qmask, enum nl80211_iftype iftype);
-void iwl_mvm_dealloc_int_sta(struct iwl_mvm *mvm,
-			     struct iwl_mvm_int_sta *sta);
-int iwl_mvm_send_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			   struct iwl_mvm_int_sta *bsta);
-int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm,
-			      struct iwl_mvm_int_sta *bsta);
-int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
-			  struct iwl_mvm_int_sta *bsta);
-int iwl_mvm_rm_bcast_sta(struct iwl_mvm *mvm, struct iwl_mvm_int_sta *bsta);
+void iwl_mvm_del_aux_sta(struct iwl_mvm *mvm);
+
+int iwl_mvm_alloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_send_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_add_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_send_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+int iwl_mvm_rm_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+void iwl_mvm_dealloc_bcast_sta(struct iwl_mvm *mvm, struct ieee80211_vif *vif);
+
 void iwl_mvm_sta_drained_wk(struct work_struct *wk);
 void iwl_mvm_sta_modify_ps_wake(struct iwl_mvm *mvm,
 				struct ieee80211_sta *sta);
diff --git a/drivers/net/wireless/iwlwifi/mvm/tdls.c b/drivers/net/wireless/iwlwifi/mvm/tdls.c
new file mode 100644
index 000000000000..66c82df2d0a1
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/mvm/tdls.c
@@ -0,0 +1,149 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called COPYING.
+ *
+ * Contact Information:
+ *  Intel Linux Wireless <ilw@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2014 Intel Mobile Communications GmbH
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#include "mvm.h"
+#include "time-event.h"
+
+void iwl_mvm_teardown_tdls_peers(struct iwl_mvm *mvm)
+{
+	struct ieee80211_sta *sta;
+	struct iwl_mvm_sta *mvmsta;
+	int i;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	for (i = 0; i < IWL_MVM_STATION_COUNT; i++) {
+		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[i],
+						lockdep_is_held(&mvm->mutex));
+		if (!sta || IS_ERR(sta) || !sta->tdls)
+			continue;
+
+		mvmsta = iwl_mvm_sta_from_mac80211(sta);
+		ieee80211_tdls_oper_request(mvmsta->vif, sta->addr,
+				NL80211_TDLS_TEARDOWN,
+				WLAN_REASON_TDLS_TEARDOWN_UNSPECIFIED,
+				GFP_KERNEL);
+	}
+}
+
+int iwl_mvm_tdls_sta_count(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
+{
+	struct ieee80211_sta *sta;
+	struct iwl_mvm_sta *mvmsta;
+	int count = 0;
+	int i;
+
+	lockdep_assert_held(&mvm->mutex);
+
+	for (i = 0; i < IWL_MVM_STATION_COUNT; i++) {
+		sta = rcu_dereference_protected(mvm->fw_id_to_mac_id[i],
+						lockdep_is_held(&mvm->mutex));
+		if (!sta || IS_ERR(sta) || !sta->tdls)
+			continue;
+
+		if (vif) {
+			mvmsta = iwl_mvm_sta_from_mac80211(sta);
+			if (mvmsta->vif != vif)
+				continue;
+		}
+
+		count++;
+	}
+
+	return count;
+}
+
+void iwl_mvm_recalc_tdls_state(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
+			       bool sta_added)
+{
+	int tdls_sta_cnt = iwl_mvm_tdls_sta_count(mvm, vif);
+
+	/*
+	 * Disable ps when the first TDLS sta is added and re-enable it
+	 * when the last TDLS sta is removed
+	 */
+	if ((tdls_sta_cnt == 1 && sta_added) ||
+	    (tdls_sta_cnt == 0 && !sta_added))
+		iwl_mvm_power_update_mac(mvm);
+}
+
+void iwl_mvm_mac_mgd_protect_tdls_discover(struct ieee80211_hw *hw,
+					   struct ieee80211_vif *vif)
+{
+	struct iwl_mvm *mvm = IWL_MAC80211_GET_MVM(hw);
+	u32 duration = 2 * vif->bss_conf.dtim_period * vif->bss_conf.beacon_int;
+
+	/*
+	 * iwl_mvm_protect_session() reads directly from the device
+	 * (the system time), so make sure it is available.
+	 */
+	if (iwl_mvm_ref_sync(mvm, IWL_MVM_REF_PROTECT_TDLS))
+		return;
+
+	mutex_lock(&mvm->mutex);
+	/* Protect the session to hear the TDLS setup response on the channel */
+	iwl_mvm_protect_session(mvm, vif, duration, duration, 100, true);
+	mutex_unlock(&mvm->mutex);
+
+	iwl_mvm_unref(mvm, IWL_MVM_REF_PROTECT_TDLS);
+}
diff --git a/drivers/net/wireless/iwlwifi/mvm/testmode.h b/drivers/net/wireless/iwlwifi/mvm/testmode.h
index 0241665925f7..79ab6beb6b26 100644
--- a/drivers/net/wireless/iwlwifi/mvm/testmode.h
+++ b/drivers/net/wireless/iwlwifi/mvm/testmode.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.c b/drivers/net/wireless/iwlwifi/mvm/time-event.c
index 33e5041f1efc..b7f9e61d14e2 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.c
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -348,6 +350,38 @@ unlock:
 	return 0;
 }
 
+static bool iwl_mvm_te_notif(struct iwl_notif_wait_data *notif_wait,
+			     struct iwl_rx_packet *pkt, void *data)
+{
+	struct iwl_mvm *mvm =
+		container_of(notif_wait, struct iwl_mvm, notif_wait);
+	struct iwl_mvm_time_event_data *te_data = data;
+	struct iwl_time_event_notif *resp;
+	int resp_len = iwl_rx_packet_payload_len(pkt);
+
+	if (WARN_ON(pkt->hdr.cmd != TIME_EVENT_NOTIFICATION))
+		return true;
+
+	if (WARN_ON_ONCE(resp_len != sizeof(*resp))) {
+		IWL_ERR(mvm, "Invalid TIME_EVENT_NOTIFICATION response\n");
+		return true;
+	}
+
+	resp = (void *)pkt->data;
+
+	/* te_data->uid is already set in the TIME_EVENT_CMD response */
+	if (le32_to_cpu(resp->unique_id) != te_data->uid)
+		return false;
+
+	IWL_DEBUG_TE(mvm, "TIME_EVENT_NOTIFICATION response - UID = 0x%x\n",
+		     te_data->uid);
+	if (!resp->status)
+		IWL_ERR(mvm,
+			"TIME_EVENT_NOTIFICATION received but not executed\n");
+
+	return true;
+}
+
 static bool iwl_mvm_time_event_response(struct iwl_notif_wait_data *notif_wait,
 					struct iwl_rx_packet *pkt, void *data)
 {
@@ -441,10 +475,12 @@ static int iwl_mvm_time_event_send_add(struct iwl_mvm *mvm,
 void iwl_mvm_protect_session(struct iwl_mvm *mvm,
 			     struct ieee80211_vif *vif,
 			     u32 duration, u32 min_duration,
-			     u32 max_delay)
+			     u32 max_delay, bool wait_for_notif)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mvm_time_event_data *te_data = &mvmvif->time_event_data;
+	const u8 te_notif_response[] = { TIME_EVENT_NOTIFICATION };
+	struct iwl_notification_wait wait_te_notif;
 	struct iwl_time_event_cmd time_cmd = {};
 
 	lockdep_assert_held(&mvm->mutex);
@@ -489,7 +525,28 @@ void iwl_mvm_protect_session(struct iwl_mvm *mvm,
 				      TE_V2_NOTIF_HOST_EVENT_END |
 				      T2_V2_START_IMMEDIATELY);
 
-	iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd);
+	if (!wait_for_notif) {
+		iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd);
+		return;
+	}
+
+	/*
+	 * Create notification_wait for the TIME_EVENT_NOTIFICATION to use
+	 * right after we send the time event
+	 */
+	iwl_init_notification_wait(&mvm->notif_wait, &wait_te_notif,
+				   te_notif_response,
+				   ARRAY_SIZE(te_notif_response),
+				   iwl_mvm_te_notif, te_data);
+
+	/* If TE was sent OK - wait for the notification that started */
+	if (iwl_mvm_time_event_send_add(mvm, vif, te_data, &time_cmd)) {
+		IWL_ERR(mvm, "Failed to add TE to protect session\n");
+		iwl_remove_notification(&mvm->notif_wait, &wait_te_notif);
+	} else if (iwl_wait_notification(&mvm->notif_wait, &wait_te_notif,
+					 TU_TO_JIFFIES(max_delay))) {
+		IWL_ERR(mvm, "Failed to protect session until TE\n");
+	}
 }
 
 /*
@@ -643,9 +700,9 @@ void iwl_mvm_stop_p2p_roc(struct iwl_mvm *mvm)
 	iwl_mvm_roc_finished(mvm);
 }
 
-int iwl_mvm_schedule_csa_noa(struct iwl_mvm *mvm,
-			      struct ieee80211_vif *vif,
-			      u32 duration, u32 apply_time)
+int iwl_mvm_schedule_csa_period(struct iwl_mvm *mvm,
+				struct ieee80211_vif *vif,
+				u32 duration, u32 apply_time)
 {
 	struct iwl_mvm_vif *mvmvif = iwl_mvm_vif_from_mac80211(vif);
 	struct iwl_mvm_time_event_data *te_data = &mvmvif->time_event_data;
@@ -654,14 +711,14 @@ int iwl_mvm_schedule_csa_noa(struct iwl_mvm *mvm,
 	lockdep_assert_held(&mvm->mutex);
 
 	if (te_data->running) {
-		IWL_DEBUG_TE(mvm, "CS NOA is already scheduled\n");
+		IWL_DEBUG_TE(mvm, "CS period is already scheduled\n");
 		return -EBUSY;
 	}
 
 	time_cmd.action = cpu_to_le32(FW_CTXT_ACTION_ADD);
 	time_cmd.id_and_color =
 		cpu_to_le32(FW_CMD_ID_AND_COLOR(mvmvif->id, mvmvif->color));
-	time_cmd.id = cpu_to_le32(TE_P2P_GO_CSA_NOA);
+	time_cmd.id = cpu_to_le32(TE_CHANNEL_SWITCH_PERIOD);
 	time_cmd.apply_time = cpu_to_le32(apply_time);
 	time_cmd.max_frags = TE_V2_FRAG_NONE;
 	time_cmd.duration = cpu_to_le32(duration);
diff --git a/drivers/net/wireless/iwlwifi/mvm/time-event.h b/drivers/net/wireless/iwlwifi/mvm/time-event.h
index 2f48a90d4ad3..b350e47e19da 100644
--- a/drivers/net/wireless/iwlwifi/mvm/time-event.h
+++ b/drivers/net/wireless/iwlwifi/mvm/time-event.h
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -124,10 +126,12 @@
  * @min_duration: will start a new session if the current session will end
  *	in less than min_duration.
  * @max_delay: maximum delay before starting the time event (in TU)
+ * @wait_for_notif: true if it is required that a time event notification be
+ *	waited for (that the time event has been scheduled before returning)
  *
  * This function can be used to start a session protection which means that the
  * fw will stay on the channel for %duration_ms milliseconds. This function
- * will block (sleep) until the session starts. This function can also be used
+ * can block (sleep) until the session starts. This function can also be used
  * to extend a currently running session.
  * This function is meant to be used for BSS association for example, where we
  * want to make sure that the fw stays on the channel during the association.
@@ -135,7 +139,7 @@
 void iwl_mvm_protect_session(struct iwl_mvm *mvm,
 			     struct ieee80211_vif *vif,
 			     u32 duration, u32 min_duration,
-			     u32 max_delay);
+			     u32 max_delay, bool wait_for_notif);
 
 /**
  * iwl_mvm_stop_session_protection - cancel the session protection.
@@ -215,7 +219,7 @@ void iwl_mvm_te_clear_data(struct iwl_mvm *mvm,
 void iwl_mvm_roc_done_wk(struct work_struct *wk);
 
 /**
- * iwl_mvm_schedule_csa_noa - request NoA for channel switch
+ * iwl_mvm_schedule_csa_period - request channel switch absence period
  * @mvm: the mvm component
  * @vif: the virtual interface for which the channel switch is issued
  * @duration: the duration of the NoA in TU.
@@ -224,9 +228,9 @@ void iwl_mvm_roc_done_wk(struct work_struct *wk);
  * This function is used to schedule NoA time event and is used to perform
  * the channel switch flow.
  */
-int iwl_mvm_schedule_csa_noa(struct iwl_mvm *mvm,
-			     struct ieee80211_vif *vif,
-			     u32 duration, u32 apply_time);
+int iwl_mvm_schedule_csa_period(struct iwl_mvm *mvm,
+				struct ieee80211_vif *vif,
+				u32 duration, u32 apply_time);
 
 /**
  * iwl_mvm_te_scheduled - check if the fw received the TE cmd
diff --git a/drivers/net/wireless/iwlwifi/mvm/tt.c b/drivers/net/wireless/iwlwifi/mvm/tt.c
index 0464599c111e..acca44a45086 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tt.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tt.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2013 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -67,263 +69,99 @@
 #include "iwl-csr.h"
 #include "iwl-prph.h"
 
-#define OTP_DTS_DIODE_DEVIATION 96 /*in words*/
-/* VBG - Voltage Band Gap error data (temperature offset) */
-#define OTP_WP_DTS_VBG			(OTP_DTS_DIODE_DEVIATION + 2)
-#define MEAS_VBG_MIN_VAL		2300
-#define MEAS_VBG_MAX_VAL		3000
-#define MEAS_VBG_DEFAULT_VAL		2700
-#define DTS_DIODE_VALID(flags)		(flags & DTS_DIODE_REG_FLAGS_PASS_ONCE)
-#define MIN_TEMPERATURE			0
-#define MAX_TEMPERATURE			125
-#define TEMPERATURE_ERROR		(MAX_TEMPERATURE + 1)
-#define PTAT_DIGITAL_VALUE_MIN_VALUE	0
-#define PTAT_DIGITAL_VALUE_MAX_VALUE	0xFF
-#define DTS_VREFS_NUM			5
-static inline u32 DTS_DIODE_GET_VREFS_ID(u32 flags)
-{
-	return (flags & DTS_DIODE_REG_FLAGS_VREFS_ID) >>
-					DTS_DIODE_REG_FLAGS_VREFS_ID_POS;
-}
+#define IWL_MVM_TEMP_NOTIF_WAIT_TIMEOUT	HZ
 
-#define CALC_VREFS_MIN_DIFF	43
-#define CALC_VREFS_MAX_DIFF	51
-#define CALC_LUT_SIZE		(1 + CALC_VREFS_MAX_DIFF - CALC_VREFS_MIN_DIFF)
-#define CALC_LUT_INDEX_OFFSET	CALC_VREFS_MIN_DIFF
-#define CALC_TEMPERATURE_RESULT_SHIFT_OFFSET	23
-
-/*
- * @digital_value: The diode's digital-value sampled (temperature/voltage)
- * @vref_low: The lower voltage-reference (the vref just below the diode's
- *	sampled digital-value)
- * @vref_high: The higher voltage-reference (the vref just above the diode's
- *	sampled digital-value)
- * @flags: bits[1:0]: The ID of the Vrefs pair (lowVref,highVref)
- *	bits[6:2]: Reserved.
- *	bits[7:7]: Indicates completion of at least 1 successful sample
- *	since last DTS reset.
- */
-struct iwl_mvm_dts_diode_bits {
-	u8 digital_value;
-	u8 vref_low;
-	u8 vref_high;
-	u8 flags;
-} __packed;
-
-union dts_diode_results {
-	u32 reg_value;
-	struct iwl_mvm_dts_diode_bits bits;
-} __packed;
-
-static s16 iwl_mvm_dts_get_volt_band_gap(struct iwl_mvm *mvm)
+static void iwl_mvm_enter_ctkill(struct iwl_mvm *mvm)
 {
-	struct iwl_nvm_section calib_sec;
-	const __le16 *calib;
-	u16 vbg;
-
-	/* TODO: move parsing to NVM code */
-	calib_sec = mvm->nvm_sections[NVM_SECTION_TYPE_CALIBRATION];
-	calib = (__le16 *)calib_sec.data;
+	u32 duration = mvm->thermal_throttle.params->ct_kill_duration;
 
-	vbg = le16_to_cpu(calib[OTP_WP_DTS_VBG]);
+	if (test_bit(IWL_MVM_STATUS_HW_CTKILL, &mvm->status))
+		return;
 
-	if (vbg < MEAS_VBG_MIN_VAL || vbg > MEAS_VBG_MAX_VAL)
-		vbg = MEAS_VBG_DEFAULT_VAL;
+	IWL_ERR(mvm, "Enter CT Kill\n");
+	iwl_mvm_set_hw_ctkill_state(mvm, true);
 
-	return vbg;
+	/* Don't schedule an exit work if we're in test mode, since
+	 * the temperature will not change unless we manually set it
+	 * again (or disable testing).
+	 */
+	if (!mvm->temperature_test)
+		schedule_delayed_work(&mvm->thermal_throttle.ct_kill_exit,
+				      round_jiffies_relative(duration * HZ));
 }
 
-static u16 iwl_mvm_dts_get_ptat_deviation_offset(struct iwl_mvm *mvm)
+static void iwl_mvm_exit_ctkill(struct iwl_mvm *mvm)
 {
-	const u8 *calib;
-	u8 ptat, pa1, pa2, median;
-
-	/* TODO: move parsing to NVM code */
-	calib = mvm->nvm_sections[NVM_SECTION_TYPE_CALIBRATION].data;
-	ptat = calib[OTP_DTS_DIODE_DEVIATION * 2];
-	pa1 = calib[OTP_DTS_DIODE_DEVIATION * 2 + 1];
-	pa2 = calib[OTP_DTS_DIODE_DEVIATION * 2 + 2];
-
-	/* get the median: */
-	if (ptat > pa1) {
-		if (ptat > pa2)
-			median = (pa1 > pa2) ? pa1 : pa2;
-		else
-			median = ptat;
-	} else {
-		if (pa1 > pa2)
-			median = (ptat > pa2) ? ptat : pa2;
-		else
-			median = pa1;
-	}
+	if (!test_bit(IWL_MVM_STATUS_HW_CTKILL, &mvm->status))
+		return;
 
-	return ptat - median;
+	IWL_ERR(mvm, "Exit CT Kill\n");
+	iwl_mvm_set_hw_ctkill_state(mvm, false);
 }
 
-static u8 iwl_mvm_dts_calibrate_ptat_deviation(struct iwl_mvm *mvm, u8 value)
+static bool iwl_mvm_temp_notif(struct iwl_notif_wait_data *notif_wait,
+			       struct iwl_rx_packet *pkt, void *data)
 {
-	/* Calibrate the PTAT digital value, based on PTAT deviation data: */
-	s16 new_val = value - iwl_mvm_dts_get_ptat_deviation_offset(mvm);
+	struct iwl_mvm *mvm =
+		container_of(notif_wait, struct iwl_mvm, notif_wait);
+	int *temp = data;
+	struct iwl_dts_measurement_notif *notif;
+	int len = iwl_rx_packet_payload_len(pkt);
+
+	if (WARN_ON_ONCE(len != sizeof(*notif))) {
+		IWL_ERR(mvm, "Invalid DTS_MEASUREMENT_NOTIFICATION\n");
+		return true;
+	}
 
-	if (new_val > PTAT_DIGITAL_VALUE_MAX_VALUE)
-		new_val = PTAT_DIGITAL_VALUE_MAX_VALUE;
-	else if (new_val < PTAT_DIGITAL_VALUE_MIN_VALUE)
-		new_val = PTAT_DIGITAL_VALUE_MIN_VALUE;
+	notif = (void *)pkt->data;
 
-	return new_val;
-}
+	*temp = le32_to_cpu(notif->temp);
 
-static bool dts_get_adjacent_vrefs(struct iwl_mvm *mvm,
-				   union dts_diode_results *avg_ptat)
-{
-	u8 vrefs_results[DTS_VREFS_NUM];
-	u8 low_vref_index = 0, flags;
-	u32 reg;
-
-	reg = iwl_read_prph(mvm->trans, DTSC_VREF_AVG);
-	memcpy(vrefs_results, &reg, sizeof(reg));
-	reg = iwl_read_prph(mvm->trans, DTSC_VREF5_AVG);
-	vrefs_results[4] = reg & 0xff;
-
-	if (avg_ptat->bits.digital_value < vrefs_results[0] ||
-	    avg_ptat->bits.digital_value > vrefs_results[4])
-		return false;
-
-	if (avg_ptat->bits.digital_value > vrefs_results[3])
-		low_vref_index = 3;
-	else if (avg_ptat->bits.digital_value > vrefs_results[2])
-		low_vref_index = 2;
-	else if (avg_ptat->bits.digital_value > vrefs_results[1])
-		low_vref_index = 1;
-
-	avg_ptat->bits.vref_low  = vrefs_results[low_vref_index];
-	avg_ptat->bits.vref_high = vrefs_results[low_vref_index + 1];
-	flags = avg_ptat->bits.flags;
-	avg_ptat->bits.flags =
-		(flags & ~DTS_DIODE_REG_FLAGS_VREFS_ID) |
-		(low_vref_index & DTS_DIODE_REG_FLAGS_VREFS_ID);
-	return true;
-}
+	/* shouldn't be negative, but since it's s32, make sure it isn't */
+	if (WARN_ON_ONCE(*temp < 0))
+		*temp = 0;
 
-/*
- * return true it the results are valid, and false otherwise.
- */
-static bool dts_read_ptat_avg_results(struct iwl_mvm *mvm,
-				      union dts_diode_results *avg_ptat)
-{
-	u32 reg;
-	u8 tmp;
-
-	/* fill the diode value and pass_once with avg-reg results */
-	reg = iwl_read_prph(mvm->trans, DTSC_PTAT_AVG);
-	reg &= DTS_DIODE_REG_DIG_VAL | DTS_DIODE_REG_PASS_ONCE;
-	avg_ptat->reg_value = reg;
-
-	/* calibrate the PTAT digital value */
-	tmp = avg_ptat->bits.digital_value;
-	tmp = iwl_mvm_dts_calibrate_ptat_deviation(mvm, tmp);
-	avg_ptat->bits.digital_value = tmp;
-
-	/*
-	 * fill vrefs fields, based on the avgVrefs results
-	 * and the diode value
-	 */
-	return dts_get_adjacent_vrefs(mvm, avg_ptat) &&
-		DTS_DIODE_VALID(avg_ptat->bits.flags);
+	IWL_DEBUG_TEMP(mvm, "DTS_MEASUREMENT_NOTIFICATION - %d\n", *temp);
+	return true;
 }
 
-static s32 calculate_nic_temperature(union dts_diode_results avg_ptat,
-				     u16 volt_band_gap)
+static int iwl_mvm_get_temp_cmd(struct iwl_mvm *mvm)
 {
-	u32 tmp_result;
-	u8 vrefs_diff;
-	/*
-	 * For temperature calculation (at the end, shift right by 23)
-	 * LUT[(D2-D1)] = ROUND{ 2^23 / ((D2-D1)*9*10) }
-	 * (D2-D1) ==   43    44    45    46    47    48    49    50    51
-	 */
-	static const u16 calc_lut[CALC_LUT_SIZE] = {
-		2168, 2118, 2071, 2026, 1983, 1942, 1902, 1864, 1828,
+	struct iwl_dts_measurement_cmd cmd = {
+		.flags = cpu_to_le32(DTS_TRIGGER_CMD_FLAGS_TEMP),
 	};
 
-	/*
-	 * The diff between the high and low voltage-references is assumed
-	 * to be strictly be in range of [60,68]
-	 */
-	vrefs_diff = avg_ptat.bits.vref_high - avg_ptat.bits.vref_low;
-
-	if (vrefs_diff < CALC_VREFS_MIN_DIFF ||
-	    vrefs_diff > CALC_VREFS_MAX_DIFF)
-		return TEMPERATURE_ERROR;
-
-	/* calculate the result: */
-	tmp_result =
-		vrefs_diff * (DTS_DIODE_GET_VREFS_ID(avg_ptat.bits.flags) + 9);
-	tmp_result += avg_ptat.bits.digital_value;
-	tmp_result -= avg_ptat.bits.vref_high;
-
-	/* multiply by the LUT value (based on the diff) */
-	tmp_result *= calc_lut[vrefs_diff - CALC_LUT_INDEX_OFFSET];
-
-	/*
-	 * Get the BandGap (the voltage refereces source) error data
-	 * (temperature offset)
-	 */
-	tmp_result *= volt_band_gap;
-
-	/*
-	 * here, tmp_result value can be up to 32-bits. We want to right-shift
-	 * it *without* sign-extend.
-	 */
-	tmp_result = tmp_result >> CALC_TEMPERATURE_RESULT_SHIFT_OFFSET;
-
-	/*
-	 * at this point, tmp_result should be in the range:
-	 * 200 <= tmp_result <= 365
-	 */
-	return (s16)tmp_result - 240;
+	return iwl_mvm_send_cmd_pdu(mvm, CMD_DTS_MEASUREMENT_TRIGGER, 0,
+				    sizeof(cmd), &cmd);
 }
 
-static s32 check_nic_temperature(struct iwl_mvm *mvm)
+int iwl_mvm_get_temp(struct iwl_mvm *mvm)
 {
-	u16 volt_band_gap;
-	union dts_diode_results avg_ptat;
-
-	volt_band_gap = iwl_mvm_dts_get_volt_band_gap(mvm);
-
-	/* disable DTS */
-	iwl_write_prph(mvm->trans, SHR_MISC_WFM_DTS_EN, 0);
+	struct iwl_notification_wait wait_temp_notif;
+	static const u8 temp_notif[] = { DTS_MEASUREMENT_NOTIFICATION };
+	int ret, temp;
 
-	/* SV initialization */
-	iwl_write_prph(mvm->trans, SHR_MISC_WFM_DTS_EN, 1);
-	iwl_write_prph(mvm->trans, DTSC_CFG_MODE,
-		       DTSC_CFG_MODE_PERIODIC);
-
-	/* wait for results */
-	msleep(100);
-	if (!dts_read_ptat_avg_results(mvm, &avg_ptat))
-		return TEMPERATURE_ERROR;
-
-	/* disable DTS */
-	iwl_write_prph(mvm->trans, SHR_MISC_WFM_DTS_EN, 0);
+	lockdep_assert_held(&mvm->mutex);
 
-	return calculate_nic_temperature(avg_ptat, volt_band_gap);
-}
+	iwl_init_notification_wait(&mvm->notif_wait, &wait_temp_notif,
+				   temp_notif, ARRAY_SIZE(temp_notif),
+				   iwl_mvm_temp_notif, &temp);
 
-static void iwl_mvm_enter_ctkill(struct iwl_mvm *mvm)
-{
-	u32 duration = mvm->thermal_throttle.params->ct_kill_duration;
+	ret = iwl_mvm_get_temp_cmd(mvm);
+	if (ret) {
+		IWL_ERR(mvm, "Failed to get the temperature (err=%d)\n", ret);
+		iwl_remove_notification(&mvm->notif_wait, &wait_temp_notif);
+		return ret;
+	}
 
-	IWL_ERR(mvm, "Enter CT Kill\n");
-	iwl_mvm_set_hw_ctkill_state(mvm, true);
-	schedule_delayed_work(&mvm->thermal_throttle.ct_kill_exit,
-			      round_jiffies_relative(duration * HZ));
-}
+	ret = iwl_wait_notification(&mvm->notif_wait, &wait_temp_notif,
+				    IWL_MVM_TEMP_NOTIF_WAIT_TIMEOUT);
+	if (ret) {
+		IWL_ERR(mvm, "Getting the temperature timed out\n");
+		return ret;
+	}
 
-static void iwl_mvm_exit_ctkill(struct iwl_mvm *mvm)
-{
-	IWL_ERR(mvm, "Exit CT Kill\n");
-	iwl_mvm_set_hw_ctkill_state(mvm, false);
+	return temp;
 }
 
 static void check_exit_ctkill(struct work_struct *work)
@@ -338,28 +176,36 @@ static void check_exit_ctkill(struct work_struct *work)
 
 	duration = tt->params->ct_kill_duration;
 
+	mutex_lock(&mvm->mutex);
+
+	if (__iwl_mvm_mac_start(mvm))
+		goto reschedule;
+
 	/* make sure the device is available for direct read/writes */
-	if (iwl_mvm_ref_sync(mvm, IWL_MVM_REF_CHECK_CTKILL))
+	if (iwl_mvm_ref_sync(mvm, IWL_MVM_REF_CHECK_CTKILL)) {
+		__iwl_mvm_mac_stop(mvm);
 		goto reschedule;
+	}
 
-	iwl_trans_start_hw(mvm->trans);
-	temp = check_nic_temperature(mvm);
-	iwl_trans_stop_device(mvm->trans);
+	temp = iwl_mvm_get_temp(mvm);
 
 	iwl_mvm_unref(mvm, IWL_MVM_REF_CHECK_CTKILL);
 
-	if (temp < MIN_TEMPERATURE || temp > MAX_TEMPERATURE) {
-		IWL_DEBUG_TEMP(mvm, "Failed to measure NIC temperature\n");
+	__iwl_mvm_mac_stop(mvm);
+
+	if (temp < 0)
 		goto reschedule;
-	}
+
 	IWL_DEBUG_TEMP(mvm, "NIC temperature: %d\n", temp);
 
 	if (temp <= tt->params->ct_kill_exit) {
+		mutex_unlock(&mvm->mutex);
 		iwl_mvm_exit_ctkill(mvm);
 		return;
 	}
 
 reschedule:
+	mutex_unlock(&mvm->mutex);
 	schedule_delayed_work(&mvm->thermal_throttle.ct_kill_exit,
 			      round_jiffies(duration * HZ));
 }
@@ -444,6 +290,12 @@ void iwl_mvm_tt_handler(struct iwl_mvm *mvm)
 		return;
 	}
 
+	if (params->support_ct_kill &&
+	    temperature <= tt->params->ct_kill_exit) {
+		iwl_mvm_exit_ctkill(mvm);
+		return;
+	}
+
 	if (params->support_dynamic_smps) {
 		if (!tt->dynamic_smps &&
 		    temperature >= params->dynamic_smps_entry) {
diff --git a/drivers/net/wireless/iwlwifi/mvm/tx.c b/drivers/net/wireless/iwlwifi/mvm/tx.c
index dbc870713882..1cb793a498ac 100644
--- a/drivers/net/wireless/iwlwifi/mvm/tx.c
+++ b/drivers/net/wireless/iwlwifi/mvm/tx.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -131,6 +133,11 @@ static void iwl_mvm_set_tx_cmd(struct iwl_mvm *mvm, struct sk_buff *skb,
 	    !is_multicast_ether_addr(ieee80211_get_DA(hdr)))
 		tx_flags |= TX_CMD_FLG_PROT_REQUIRE;
 
+	if ((mvm->fw->ucode_capa.capa[0] &
+	     IWL_UCODE_TLV_CAPA_TXPOWER_INSERTION_SUPPORT) &&
+	    ieee80211_action_contains_tpc(skb))
+		tx_flags |= TX_CMD_FLG_WRITE_TX_POWER;
+
 	tx_cmd->tx_flags = cpu_to_le32(tx_flags);
 	/* Total # bytes to be transmitted */
 	tx_cmd->len = cpu_to_le16((u16)skb->len);
@@ -168,10 +175,14 @@ static void iwl_mvm_set_tx_cmd_rate(struct iwl_mvm *mvm,
 
 	/*
 	 * for data packets, rate info comes from the table inside the fw. This
-	 * table is controlled by LINK_QUALITY commands
+	 * table is controlled by LINK_QUALITY commands. Exclude ctrl port
+	 * frames like EAPOLs which should be treated as mgmt frames. This
+	 * avoids them being sent initially in high rates which increases the
+	 * chances for completion of the 4-Way handshake.
 	 */
 
-	if (ieee80211_is_data(fc) && sta) {
+	if (ieee80211_is_data(fc) && sta &&
+	    !(info->control.flags & IEEE80211_TX_CTRL_PORT_CTRL_PROTO)) {
 		tx_cmd->initial_rate_index = 0;
 		tx_cmd->tx_flags |= cpu_to_le32(TX_CMD_FLG_STA_RATE);
 		return;
@@ -207,7 +218,7 @@ static void iwl_mvm_set_tx_cmd_rate(struct iwl_mvm *mvm,
 
 	if (info->band == IEEE80211_BAND_2GHZ &&
 	    !iwl_mvm_bt_coex_is_shared_ant_avail(mvm))
-		rate_flags = BIT(ANT_A) << RATE_MCS_ANT_POS;
+		rate_flags = BIT(mvm->cfg->non_shared_ant) << RATE_MCS_ANT_POS;
 	else
 		rate_flags =
 			BIT(mvm->mgmt_last_antenna_idx) << RATE_MCS_ANT_POS;
@@ -482,11 +493,11 @@ static void iwl_mvm_check_ratid_empty(struct iwl_mvm *mvm,
 		IWL_DEBUG_TX_QUEUES(mvm,
 				    "Can continue DELBA flow ssn = next_recl = %d\n",
 				    tid_data->next_reclaimed);
-		iwl_trans_txq_disable(mvm->trans, tid_data->txq_id);
+		iwl_mvm_disable_txq(mvm, tid_data->txq_id);
 		tid_data->state = IWL_AGG_OFF;
 		/*
 		 * we can't hold the mutex - but since we are after a sequence
-		 * point (call to iwl_trans_txq_disable), so we don't even need
+		 * point (call to iwl_mvm_disable_txq(), so we don't even need
 		 * a memory barrier.
 		 */
 		mvm->queue_to_mac80211[tid_data->txq_id] =
@@ -862,6 +873,19 @@ int iwl_mvm_rx_tx_cmd(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 	return 0;
 }
 
+static void iwl_mvm_tx_info_from_ba_notif(struct ieee80211_tx_info *info,
+					  struct iwl_mvm_ba_notif *ba_notif,
+					  struct iwl_mvm_tid_data *tid_data)
+{
+	info->flags |= IEEE80211_TX_STAT_AMPDU;
+	info->status.ampdu_ack_len = ba_notif->txed_2_done;
+	info->status.ampdu_len = ba_notif->txed;
+	iwl_mvm_hwrate_to_tx_status(tid_data->rate_n_flags,
+				    info);
+	info->status.status_driver_data[0] =
+		(void *)(uintptr_t)tid_data->reduced_tpc;
+}
+
 int iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 			struct iwl_device_cmd *cmd)
 {
@@ -948,21 +972,37 @@ int iwl_mvm_rx_ba_notif(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 		 */
 		info->flags |= IEEE80211_TX_STAT_ACK;
 
-		if (freed == 1) {
-			/* this is the first skb we deliver in this batch */
-			/* put the rate scaling data there */
-			info->flags |= IEEE80211_TX_STAT_AMPDU;
-			info->status.ampdu_ack_len = ba_notif->txed_2_done;
-			info->status.ampdu_len = ba_notif->txed;
-			iwl_mvm_hwrate_to_tx_status(tid_data->rate_n_flags,
-						    info);
-			info->status.status_driver_data[0] =
-				(void *)(uintptr_t)tid_data->reduced_tpc;
-		}
+		/* this is the first skb we deliver in this batch */
+		/* put the rate scaling data there */
+		if (freed == 1)
+			iwl_mvm_tx_info_from_ba_notif(info, ba_notif, tid_data);
 	}
 
 	spin_unlock_bh(&mvmsta->lock);
 
+	/* We got a BA notif with 0 acked or scd_ssn didn't progress which is
+	 * possible (i.e. first MPDU in the aggregation wasn't acked)
+	 * Still it's important to update RS about sent vs. acked.
+	 */
+	if (skb_queue_empty(&reclaimed_skbs)) {
+		struct ieee80211_tx_info ba_info = {};
+		struct ieee80211_chanctx_conf *chanctx_conf = NULL;
+
+		if (mvmsta->vif)
+			chanctx_conf =
+				rcu_dereference(mvmsta->vif->chanctx_conf);
+
+		if (WARN_ON_ONCE(!chanctx_conf))
+			goto out;
+
+		ba_info.band = chanctx_conf->def.chan->band;
+		iwl_mvm_tx_info_from_ba_notif(&ba_info, ba_notif, tid_data);
+
+		IWL_DEBUG_TX_REPLY(mvm, "No reclaim. Update rs directly\n");
+		iwl_mvm_rs_tx_status(mvm, sta, tid, &ba_info);
+	}
+
+out:
 	rcu_read_unlock();
 
 	while (!skb_queue_empty(&reclaimed_skbs)) {
diff --git a/drivers/net/wireless/iwlwifi/mvm/utils.c b/drivers/net/wireless/iwlwifi/mvm/utils.c
index ac249da8a22b..8021f6eec27f 100644
--- a/drivers/net/wireless/iwlwifi/mvm/utils.c
+++ b/drivers/net/wireless/iwlwifi/mvm/utils.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2012 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -387,15 +389,19 @@ struct iwl_error_event_table {
 struct iwl_umac_error_event_table {
 	u32 valid;		/* (nonzero) valid, (0) log is empty */
 	u32 error_id;		/* type of error */
-	u32 pc;			/* program counter */
 	u32 blink1;		/* branch link */
 	u32 blink2;		/* branch link */
 	u32 ilink1;		/* interrupt link */
 	u32 ilink2;		/* interrupt link */
 	u32 data1;		/* error-specific data */
 	u32 data2;		/* error-specific data */
-	u32 line;		/* source code line of error */
-	u32 umac_ver;		/* umac version */
+	u32 data3;		/* error-specific data */
+	u32 umac_fw_ver;	/* UMAC version */
+	u32 umac_fw_api_ver;	/* UMAC FW API ver */
+	u32 frame_pointer;	/* core register 27*/
+	u32 stack_pointer;	/* core register 28 */
+	u32 cmd_header;	/* latest host cmd sent to UMAC */
+	u32 nic_isr_pref;	/* ISR status register */
 } __packed;
 
 #define ERROR_START_OFFSET  (1 * sizeof(u32))
@@ -409,7 +415,7 @@ static void iwl_mvm_dump_umac_error_log(struct iwl_mvm *mvm)
 
 	base = mvm->umac_error_event_table;
 
-	if (base < 0x800000 || base >= 0x80C000) {
+	if (base < 0x800000) {
 		IWL_ERR(mvm,
 			"Not valid error log pointer 0x%08X for %s uCode\n",
 			base,
@@ -428,14 +434,19 @@ static void iwl_mvm_dump_umac_error_log(struct iwl_mvm *mvm)
 
 	IWL_ERR(mvm, "0x%08X | %-28s\n", table.error_id,
 		desc_lookup(table.error_id));
-	IWL_ERR(mvm, "0x%08X | umac uPc\n", table.pc);
 	IWL_ERR(mvm, "0x%08X | umac branchlink1\n", table.blink1);
 	IWL_ERR(mvm, "0x%08X | umac branchlink2\n", table.blink2);
 	IWL_ERR(mvm, "0x%08X | umac interruptlink1\n", table.ilink1);
 	IWL_ERR(mvm, "0x%08X | umac interruptlink2\n", table.ilink2);
 	IWL_ERR(mvm, "0x%08X | umac data1\n", table.data1);
 	IWL_ERR(mvm, "0x%08X | umac data2\n", table.data2);
-	IWL_ERR(mvm, "0x%08X | umac version\n", table.umac_ver);
+	IWL_ERR(mvm, "0x%08X | umac data3\n", table.data3);
+	IWL_ERR(mvm, "0x%08X | umac version\n", table.umac_fw_ver);
+	IWL_ERR(mvm, "0x%08X | umac api version\n", table.umac_fw_api_ver);
+	IWL_ERR(mvm, "0x%08X | frame pointer\n", table.frame_pointer);
+	IWL_ERR(mvm, "0x%08X | stack pointer\n", table.stack_pointer);
+	IWL_ERR(mvm, "0x%08X | last host cmd\n", table.cmd_header);
+	IWL_ERR(mvm, "0x%08X | isr status reg\n", table.nic_isr_pref);
 }
 
 void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
@@ -519,6 +530,52 @@ void iwl_mvm_dump_nic_error_log(struct iwl_mvm *mvm)
 		iwl_mvm_dump_umac_error_log(mvm);
 }
 
+void iwl_mvm_enable_txq(struct iwl_mvm *mvm, int queue, u16 ssn,
+			const struct iwl_trans_txq_scd_cfg *cfg)
+{
+	if (iwl_mvm_is_dqa_supported(mvm)) {
+		struct iwl_scd_txq_cfg_cmd cmd = {
+			.scd_queue = queue,
+			.enable = 1,
+			.window = cfg->frame_limit,
+			.sta_id = cfg->sta_id,
+			.ssn = cpu_to_le16(ssn),
+			.tx_fifo = cfg->fifo,
+			.aggregate = cfg->aggregate,
+			.flags = IWL_SCD_FLAGS_DQA_ENABLED,
+			.tid = cfg->tid,
+			.control = IWL_SCD_CONTROL_SET_SSN,
+		};
+		int ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, 0,
+					       sizeof(cmd), &cmd);
+		if (ret)
+			IWL_ERR(mvm,
+				"Failed to configure queue %d on FIFO %d\n",
+				queue, cfg->fifo);
+	}
+
+	iwl_trans_txq_enable_cfg(mvm->trans, queue, ssn,
+				 iwl_mvm_is_dqa_supported(mvm) ? NULL : cfg);
+}
+
+void iwl_mvm_disable_txq(struct iwl_mvm *mvm, int queue)
+{
+	iwl_trans_txq_disable(mvm->trans, queue,
+			      !iwl_mvm_is_dqa_supported(mvm));
+
+	if (iwl_mvm_is_dqa_supported(mvm)) {
+		struct iwl_scd_txq_cfg_cmd cmd = {
+			.scd_queue = queue,
+			.enable = 0,
+		};
+		int ret = iwl_mvm_send_cmd_pdu(mvm, SCD_QUEUE_CFG, CMD_ASYNC,
+					       sizeof(cmd), &cmd);
+		if (ret)
+			IWL_ERR(mvm, "Failed to disable queue %d (ret=%d)\n",
+				queue, ret);
+	}
+}
+
 /**
  * iwl_mvm_send_lq_cmd() - Send link quality command
  * @init: This command is sent as part of station initialization right
diff --git a/drivers/net/wireless/iwlwifi/pcie/drv.c b/drivers/net/wireless/iwlwifi/pcie/drv.c
index f0e722ced080..6ced8549eb3a 100644
--- a/drivers/net/wireless/iwlwifi/pcie/drv.c
+++ b/drivers/net/wireless/iwlwifi/pcie/drv.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -273,6 +275,8 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x08B1, 0x4070, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B1, 0x4072, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B1, 0x4170, iwl7260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x08B1, 0x4C60, iwl7260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x08B1, 0x4C70, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B1, 0x4060, iwl7260_2n_cfg)},
 	{IWL_PCI_DEVICE(0x08B1, 0x406A, iwl7260_2n_cfg)},
 	{IWL_PCI_DEVICE(0x08B1, 0x4160, iwl7260_2n_cfg)},
@@ -316,6 +320,8 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x08B1, 0xC770, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B1, 0xC760, iwl7260_2n_cfg)},
 	{IWL_PCI_DEVICE(0x08B2, 0xC270, iwl7260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x08B1, 0xCC70, iwl7260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x08B1, 0xCC60, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B2, 0xC272, iwl7260_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B2, 0xC260, iwl7260_2n_cfg)},
 	{IWL_PCI_DEVICE(0x08B2, 0xC26A, iwl7260_n_cfg)},
@@ -352,11 +358,17 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x08B3, 0x8060, iwl3160_2n_cfg)},
 	{IWL_PCI_DEVICE(0x08B3, 0x8062, iwl3160_n_cfg)},
 	{IWL_PCI_DEVICE(0x08B4, 0x8270, iwl3160_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x08B4, 0x8370, iwl3160_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x08B4, 0x8272, iwl3160_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B3, 0x8470, iwl3160_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B3, 0x8570, iwl3160_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B3, 0x1070, iwl3160_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x08B3, 0x1170, iwl3160_2ac_cfg)},
 
+/* 3165 Series */
+	{IWL_PCI_DEVICE(0x3165, 0x4010, iwl3165_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x3165, 0x4210, iwl3165_2ac_cfg)},
+
 /* 7265 Series */
 	{IWL_PCI_DEVICE(0x095A, 0x5010, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x5110, iwl7265_2ac_cfg)},
@@ -378,6 +390,7 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x095B, 0x5202, iwl7265_n_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9010, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9012, iwl7265_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x095A, 0x900A, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9110, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9112, iwl7265_2ac_cfg)},
 	{IWL_PCI_DEVICE(0x095A, 0x9210, iwl7265_2ac_cfg)},
@@ -396,6 +409,7 @@ static const struct pci_device_id iwl_hw_card_ids[] = {
 
 /* 8000 Series */
 	{IWL_PCI_DEVICE(0x24F3, 0x0010, iwl8260_2ac_cfg)},
+	{IWL_PCI_DEVICE(0x24F3, 0x0004, iwl8260_2n_cfg)},
 	{IWL_PCI_DEVICE(0x24F4, 0x0030, iwl8260_2ac_cfg)},
 #endif /* CONFIG_IWLMVM */
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/internal.h b/drivers/net/wireless/iwlwifi/pcie/internal.h
index 78f72c34438a..1aea6b66c594 100644
--- a/drivers/net/wireless/iwlwifi/pcie/internal.h
+++ b/drivers/net/wireless/iwlwifi/pcie/internal.h
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portions of the ieee80211 subsystem header files.
@@ -256,6 +257,7 @@ iwl_pcie_get_scratchbuf_dma(struct iwl_txq *txq, int idx)
  * @cmd_queue - command queue number
  * @rx_buf_size_8k: 8 kB RX buffer size
  * @bc_table_dword: true if the BC table expects DWORD (as opposed to bytes)
+ * @scd_set_active: should the transport configure the SCD for HCMD queue
  * @rx_page_order: page order for receive buffer size
  * @wd_timeout: queue watchdog timeout (jiffies)
  * @reg_lock: protect hw register access
@@ -305,6 +307,7 @@ struct iwl_trans_pcie {
 
 	bool rx_buf_size_8k;
 	bool bc_table_dword;
+	bool scd_set_active;
 	u32 rx_page_order;
 
 	const char *const *command_names;
@@ -364,9 +367,10 @@ int iwl_pcie_tx_init(struct iwl_trans *trans);
 void iwl_pcie_tx_start(struct iwl_trans *trans, u32 scd_base_addr);
 int iwl_pcie_tx_stop(struct iwl_trans *trans);
 void iwl_pcie_tx_free(struct iwl_trans *trans);
-void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
-			       int sta_id, int tid, int frame_limit, u16 ssn);
-void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int queue);
+void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int queue, u16 ssn,
+			       const struct iwl_trans_txq_scd_cfg *cfg);
+void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int queue,
+				bool configure_scd);
 int iwl_trans_pcie_tx(struct iwl_trans *trans, struct sk_buff *skb,
 		      struct iwl_device_cmd *dev_cmd, int txq_id);
 void iwl_pcie_txq_check_wrptrs(struct iwl_trans *trans);
diff --git a/drivers/net/wireless/iwlwifi/pcie/rx.c b/drivers/net/wireless/iwlwifi/pcie/rx.c
index a2698e5e062c..7b7e2f223fb2 100644
--- a/drivers/net/wireless/iwlwifi/pcie/rx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/rx.c
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portions of the ieee80211 subsystem header files.
@@ -639,7 +640,7 @@ static void iwl_pcie_rx_handle_rb(struct iwl_trans *trans,
 		err = iwl_op_mode_rx(trans->op_mode, &rxcb, cmd);
 
 		if (reclaim) {
-			kfree(txq->entries[cmd_index].free_buf);
+			kzfree(txq->entries[cmd_index].free_buf);
 			txq->entries[cmd_index].free_buf = NULL;
 		}
 
diff --git a/drivers/net/wireless/iwlwifi/pcie/trans.c b/drivers/net/wireless/iwlwifi/pcie/trans.c
index 06e04aaf61ee..1393bac0025c 100644
--- a/drivers/net/wireless/iwlwifi/pcie/trans.c
+++ b/drivers/net/wireless/iwlwifi/pcie/trans.c
@@ -6,6 +6,7 @@
  * GPL LICENSE SUMMARY
  *
  * Copyright(c) 2007 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of version 2 of the GNU General Public License as
@@ -31,6 +32,7 @@
  * BSD LICENSE
  *
  * Copyright(c) 2005 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -1169,6 +1171,7 @@ static void iwl_trans_pcie_configure(struct iwl_trans *trans,
 
 	trans_pcie->command_names = trans_cfg->command_names;
 	trans_pcie->bc_table_dword = trans_cfg->bc_table_dword;
+	trans_pcie->scd_set_active = trans_cfg->scd_set_active;
 
 	/* Initialize NAPI here - it should be before registering to mac80211
 	 * in the opmode but after the HW struct is allocated.
@@ -2187,7 +2190,7 @@ struct iwl_trans *iwl_trans_pcie_alloc(struct pci_dev *pdev,
 	 */
 	if (trans->cfg->device_family == IWL_DEVICE_FAMILY_8000)
 		trans->hw_rev = (trans->hw_rev & 0xfff0) |
-				((trans->hw_rev << 2) & 0xc);
+				(CSR_HW_REV_STEP(trans->hw_rev << 2) << 2);
 
 	trans->hw_id = (pdev->device << 16) + pdev->subsystem_device;
 	snprintf(trans->hw_id_str, sizeof(trans->hw_id_str),
diff --git a/drivers/net/wireless/iwlwifi/pcie/tx.c b/drivers/net/wireless/iwlwifi/pcie/tx.c
index 6acccb19c4f3..eb8e2984c5e9 100644
--- a/drivers/net/wireless/iwlwifi/pcie/tx.c
+++ b/drivers/net/wireless/iwlwifi/pcie/tx.c
@@ -1,6 +1,7 @@
 /******************************************************************************
  *
  * Copyright(c) 2003 - 2014 Intel Corporation. All rights reserved.
+ * Copyright(c) 2013 - 2014 Intel Mobile Communications GmbH
  *
  * Portions of this file are derived from the ipw3945 project, as well
  * as portions of the ieee80211 subsystem header files.
@@ -34,6 +35,7 @@
 #include "iwl-csr.h"
 #include "iwl-prph.h"
 #include "iwl-io.h"
+#include "iwl-scd.h"
 #include "iwl-op-mode.h"
 #include "internal.h"
 /* FIXME: need to abstract out TX command (once we know what it looks like) */
@@ -618,8 +620,8 @@ static void iwl_pcie_txq_free(struct iwl_trans *trans, int txq_id)
 	/* De-alloc array of command/tx buffers */
 	if (txq_id == trans_pcie->cmd_queue)
 		for (i = 0; i < txq->q.n_window; i++) {
-			kfree(txq->entries[i].cmd);
-			kfree(txq->entries[i].free_buf);
+			kzfree(txq->entries[i].cmd);
+			kzfree(txq->entries[i].free_buf);
 		}
 
 	/* De-alloc circular buffer of TFDs */
@@ -644,17 +646,6 @@ static void iwl_pcie_txq_free(struct iwl_trans *trans, int txq_id)
 	memset(txq, 0, sizeof(*txq));
 }
 
-/*
- * Activate/Deactivate Tx DMA/FIFO channels according tx fifos mask
- */
-static void iwl_pcie_txq_set_sched(struct iwl_trans *trans, u32 mask)
-{
-	struct iwl_trans_pcie __maybe_unused *trans_pcie =
-		IWL_TRANS_GET_PCIE_TRANS(trans);
-
-	iwl_write_prph(trans, SCD_TXFACT, mask);
-}
-
 void iwl_pcie_tx_start(struct iwl_trans *trans, u32 scd_base_addr)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
@@ -692,7 +683,7 @@ void iwl_pcie_tx_start(struct iwl_trans *trans, u32 scd_base_addr)
 				trans_pcie->cmd_fifo);
 
 	/* Activate all Tx DMA/FIFO channels */
-	iwl_pcie_txq_set_sched(trans, IWL_MASK(0, 7));
+	iwl_scd_activate_fifos(trans);
 
 	/* Enable DMA channel */
 	for (chan = 0; chan < FH_TCSR_CHNL_NUM; chan++)
@@ -745,7 +736,7 @@ int iwl_pcie_tx_stop(struct iwl_trans *trans)
 	/* Turn off all Tx DMA fifos */
 	spin_lock(&trans_pcie->irq_lock);
 
-	iwl_pcie_txq_set_sched(trans, 0);
+	iwl_scd_deactivate_fifos(trans);
 
 	/* Stop each Tx DMA channel, and wait for it to be idle */
 	for (ch = 0; ch < FH_TCSR_CHNL_NUM; ch++) {
@@ -886,7 +877,7 @@ int iwl_pcie_tx_init(struct iwl_trans *trans)
 	spin_lock(&trans_pcie->irq_lock);
 
 	/* Turn off all Tx DMA fifos */
-	iwl_write_prph(trans, SCD_TXFACT, 0);
+	iwl_scd_deactivate_fifos(trans);
 
 	/* Tell NIC where to find the "keep warm" buffer */
 	iwl_write_direct32(trans, FH_KW_MEM_ADDR_REG,
@@ -1072,55 +1063,53 @@ static int iwl_pcie_txq_set_ratid_map(struct iwl_trans *trans, u16 ra_tid,
 	return 0;
 }
 
-static inline void iwl_pcie_txq_set_inactive(struct iwl_trans *trans,
-					     u16 txq_id)
-{
-	/* Simply stop the queue, but don't change any configuration;
-	 * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
-	iwl_write_prph(trans,
-		SCD_QUEUE_STATUS_BITS(txq_id),
-		(0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
-		(1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
-}
-
 /* Receiver address (actually, Rx station's index into station table),
  * combined with Traffic ID (QOS priority), in format used by Tx Scheduler */
 #define BUILD_RAxTID(sta_id, tid)	(((sta_id) << 4) + (tid))
 
-void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
-			       int sta_id, int tid, int frame_limit, u16 ssn)
+void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, u16 ssn,
+			       const struct iwl_trans_txq_scd_cfg *cfg)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
+	int fifo = -1;
 
 	if (test_and_set_bit(txq_id, trans_pcie->queue_used))
 		WARN_ONCE(1, "queue %d already used - expect issues", txq_id);
 
-	/* Stop this Tx queue before configuring it */
-	iwl_pcie_txq_set_inactive(trans, txq_id);
+	if (cfg) {
+		fifo = cfg->fifo;
 
-	/* Set this queue as a chain-building queue unless it is CMD queue */
-	if (txq_id != trans_pcie->cmd_queue)
-		iwl_set_bits_prph(trans, SCD_QUEUECHAIN_SEL, BIT(txq_id));
+		/* Disable the scheduler prior configuring the cmd queue */
+		if (txq_id == trans_pcie->cmd_queue &&
+		    trans_pcie->scd_set_active)
+			iwl_scd_enable_set_active(trans, 0);
 
-	/* If this queue is mapped to a certain station: it is an AGG queue */
-	if (sta_id >= 0) {
-		u16 ra_tid = BUILD_RAxTID(sta_id, tid);
+		/* Stop this Tx queue before configuring it */
+		iwl_scd_txq_set_inactive(trans, txq_id);
 
-		/* Map receiver-address / traffic-ID to this queue */
-		iwl_pcie_txq_set_ratid_map(trans, ra_tid, txq_id);
+		/* Set this queue as a chain-building queue unless it is CMD */
+		if (txq_id != trans_pcie->cmd_queue)
+			iwl_scd_txq_set_chain(trans, txq_id);
 
-		/* enable aggregations for the queue */
-		iwl_set_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
-		trans_pcie->txq[txq_id].ampdu = true;
-	} else {
-		/*
-		 * disable aggregations for the queue, this will also make the
-		 * ra_tid mapping configuration irrelevant since it is now a
-		 * non-AGG queue.
-		 */
-		iwl_clear_bits_prph(trans, SCD_AGGR_SEL, BIT(txq_id));
+		if (cfg->aggregate) {
+			u16 ra_tid = BUILD_RAxTID(cfg->sta_id, cfg->tid);
+
+			/* Map receiver-address / traffic-ID to this queue */
+			iwl_pcie_txq_set_ratid_map(trans, ra_tid, txq_id);
+
+			/* enable aggregations for the queue */
+			iwl_scd_txq_enable_agg(trans, txq_id);
+			trans_pcie->txq[txq_id].ampdu = true;
+		} else {
+			/*
+			 * disable aggregations for the queue, this will also
+			 * make the ra_tid mapping configuration irrelevant
+			 * since it is now a non-AGG queue.
+			 */
+			iwl_scd_txq_disable_agg(trans, txq_id);
 
-		ssn = trans_pcie->txq[txq_id].q.read_ptr;
+			ssn = trans_pcie->txq[txq_id].q.read_ptr;
+		}
 	}
 
 	/* Place first TFD at index corresponding to start sequence number.
@@ -1128,32 +1117,44 @@ void iwl_trans_pcie_txq_enable(struct iwl_trans *trans, int txq_id, int fifo,
 	trans_pcie->txq[txq_id].q.read_ptr = (ssn & 0xff);
 	trans_pcie->txq[txq_id].q.write_ptr = (ssn & 0xff);
 
-	iwl_write_direct32(trans, HBUS_TARG_WRPTR,
-			   (ssn & 0xff) | (txq_id << 8));
-	iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), ssn);
+	if (cfg) {
+		u8 frame_limit = cfg->frame_limit;
+
+		iwl_write_direct32(trans, HBUS_TARG_WRPTR,
+				   (ssn & 0xff) | (txq_id << 8));
+		iwl_write_prph(trans, SCD_QUEUE_RDPTR(txq_id), ssn);
 
-	/* Set up Tx window size and frame limit for this queue */
-	iwl_trans_write_mem32(trans, trans_pcie->scd_base_addr +
-			SCD_CONTEXT_QUEUE_OFFSET(txq_id), 0);
-	iwl_trans_write_mem32(trans, trans_pcie->scd_base_addr +
+		/* Set up Tx window size and frame limit for this queue */
+		iwl_trans_write_mem32(trans, trans_pcie->scd_base_addr +
+				SCD_CONTEXT_QUEUE_OFFSET(txq_id), 0);
+		iwl_trans_write_mem32(trans,
+			trans_pcie->scd_base_addr +
 			SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
 			((frame_limit << SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
-				SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
+					SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
 			((frame_limit << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
-				SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
-
-	/* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
-	iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
-		       (1 << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
-		       (fifo << SCD_QUEUE_STTS_REG_POS_TXF) |
-		       (1 << SCD_QUEUE_STTS_REG_POS_WSL) |
-		       SCD_QUEUE_STTS_REG_MSK);
+					SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
+
+		/* Set up status area in SRAM, map to Tx DMA/FIFO, activate */
+		iwl_write_prph(trans, SCD_QUEUE_STATUS_BITS(txq_id),
+			       (1 << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+			       (cfg->fifo << SCD_QUEUE_STTS_REG_POS_TXF) |
+			       (1 << SCD_QUEUE_STTS_REG_POS_WSL) |
+			       SCD_QUEUE_STTS_REG_MSK);
+
+		/* enable the scheduler for this queue (only) */
+		if (txq_id == trans_pcie->cmd_queue &&
+		    trans_pcie->scd_set_active)
+			iwl_scd_enable_set_active(trans, BIT(txq_id));
+	}
+
 	trans_pcie->txq[txq_id].active = true;
 	IWL_DEBUG_TX_QUEUES(trans, "Activate queue %d on FIFO %d WrPtr: %d\n",
 			    txq_id, fifo, ssn & 0xff);
 }
 
-void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int txq_id)
+void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int txq_id,
+				bool configure_scd)
 {
 	struct iwl_trans_pcie *trans_pcie = IWL_TRANS_GET_PCIE_TRANS(trans);
 	u32 stts_addr = trans_pcie->scd_base_addr +
@@ -1172,10 +1173,12 @@ void iwl_trans_pcie_txq_disable(struct iwl_trans *trans, int txq_id)
 		return;
 	}
 
-	iwl_pcie_txq_set_inactive(trans, txq_id);
+	if (configure_scd) {
+		iwl_scd_txq_set_inactive(trans, txq_id);
 
-	iwl_trans_write_mem(trans, stts_addr, (void *)zero_val,
-			    ARRAY_SIZE(zero_val));
+		iwl_trans_write_mem(trans, stts_addr, (void *)zero_val,
+				    ARRAY_SIZE(zero_val));
+	}
 
 	iwl_pcie_txq_unmap(trans, txq_id);
 	trans_pcie->txq[txq_id].ampdu = false;
@@ -1406,7 +1409,7 @@ static int iwl_pcie_enqueue_hcmd(struct iwl_trans *trans,
 
 	out_meta->flags = cmd->flags;
 	if (WARN_ON_ONCE(txq->entries[idx].free_buf))
-		kfree(txq->entries[idx].free_buf);
+		kzfree(txq->entries[idx].free_buf);
 	txq->entries[idx].free_buf = dup_buf;
 
 	trace_iwlwifi_dev_hcmd(trans->dev, cmd, cmd_size, &out_cmd->hdr);
diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/libertas/cfg.c
index 47a998d8f99e..34f09ef90bb3 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/libertas/cfg.c
@@ -590,7 +590,6 @@ static int lbs_ret_scan(struct lbs_private *priv, unsigned long dummy,
 		int chan_no = -1;
 		const u8 *ssid = NULL;
 		u8 ssid_len = 0;
-		DECLARE_SSID_BUF(ssid_buf);
 
 		int len = get_unaligned_le16(pos);
 		pos += 2;
@@ -644,15 +643,14 @@ static int lbs_ret_scan(struct lbs_private *priv, unsigned long dummy,
 			struct ieee80211_channel *channel =
 				ieee80211_get_channel(wiphy, freq);
 
-			lbs_deb_scan("scan: %pM, capa %04x, chan %2d, %s, "
-				     "%d dBm\n",
-				     bssid, capa, chan_no,
-				     print_ssid(ssid_buf, ssid, ssid_len),
+			lbs_deb_scan("scan: %pM, capa %04x, chan %2d, %*pE, %d dBm\n",
+				     bssid, capa, chan_no, ssid_len, ssid,
 				     LBS_SCAN_RSSI_TO_MBM(rssi)/100);
 
 			if (channel &&
 			    !(channel->flags & IEEE80211_CHAN_DISABLED)) {
 				bss = cfg80211_inform_bss(wiphy, channel,
+					CFG80211_BSS_FTYPE_UNKNOWN,
 					bssid, get_unaligned_le64(tsfdesc),
 					capa, intvl, ie, ielen,
 					LBS_SCAN_RSSI_TO_MBM(rssi),
@@ -1353,7 +1351,7 @@ static int lbs_cfg_connect(struct wiphy *wiphy, struct net_device *dev,
 		wait_event_interruptible_timeout(priv->scan_q,
 						 (priv->scan_req == NULL),
 						 (15 * HZ));
-		lbs_deb_assoc("assoc: scanning competed\n");
+		lbs_deb_assoc("assoc: scanning completed\n");
 	}
 
 	/* Find the BSS we want using available scan results */
@@ -1754,6 +1752,7 @@ static void lbs_join_post(struct lbs_private *priv,
 
 	bss = cfg80211_inform_bss(priv->wdev->wiphy,
 				  params->chandef.chan,
+				  CFG80211_BSS_FTYPE_UNKNOWN,
 				  bssid,
 				  0,
 				  capability,
@@ -1982,7 +1981,6 @@ static int lbs_join_ibss(struct wiphy *wiphy, struct net_device *dev,
 	struct lbs_private *priv = wiphy_priv(wiphy);
 	int ret = 0;
 	struct cfg80211_bss *bss;
-	DECLARE_SSID_BUF(ssid_buf);
 
 	if (dev == priv->mesh_dev)
 		return -EOPNOTSUPP;
diff --git a/drivers/net/wireless/libertas/mesh.c b/drivers/net/wireless/libertas/mesh.c
index 01a67f62696f..d0c881dd5846 100644
--- a/drivers/net/wireless/libertas/mesh.c
+++ b/drivers/net/wireless/libertas/mesh.c
@@ -93,7 +93,6 @@ static int lbs_mesh_config(struct lbs_private *priv, uint16_t action,
 {
 	struct cmd_ds_mesh_config cmd;
 	struct mrvl_meshie *ie;
-	DECLARE_SSID_BUF(ssid);
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.channel = cpu_to_le16(chan);
@@ -122,9 +121,9 @@ static int lbs_mesh_config(struct lbs_private *priv, uint16_t action,
 	default:
 		return -1;
 	}
-	lbs_deb_cmd("mesh config action %d type %x channel %d SSID %s\n",
-		    action, priv->mesh_tlv, chan,
-		    print_ssid(ssid, priv->mesh_ssid, priv->mesh_ssid_len));
+	lbs_deb_cmd("mesh config action %d type %x channel %d SSID %*pE\n",
+		    action, priv->mesh_tlv, chan, priv->mesh_ssid_len,
+		    priv->mesh_ssid);
 
 	return __lbs_mesh_config_send(priv, &cmd, action, priv->mesh_tlv);
 }
diff --git a/drivers/net/wireless/libertas_tf/main.c b/drivers/net/wireless/libertas_tf/main.c
index 088de9d25c39..25c5acc78bd1 100644
--- a/drivers/net/wireless/libertas_tf/main.c
+++ b/drivers/net/wireless/libertas_tf/main.c
@@ -332,7 +332,7 @@ static int lbtf_op_start(struct ieee80211_hw *hw)
 
 err_prog_firmware:
 	priv->hw_reset_device(card);
-	lbtf_deb_leave_args(LBTF_DEB_MACOPS, "error programing fw; ret=%d", ret);
+	lbtf_deb_leave_args(LBTF_DEB_MACOPS, "error programming fw; ret=%d", ret);
 	return ret;
 }
 
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
index 1326f6121835..babbdc1ce741 100644
--- a/drivers/net/wireless/mac80211_hwsim.c
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -2045,8 +2045,6 @@ static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 
 	hw->flags = IEEE80211_HW_MFP_CAPABLE |
 		    IEEE80211_HW_SIGNAL_DBM |
-		    IEEE80211_HW_SUPPORTS_STATIC_SMPS |
-		    IEEE80211_HW_SUPPORTS_DYNAMIC_SMPS |
 		    IEEE80211_HW_AMPDU_AGGREGATION |
 		    IEEE80211_HW_WANT_MONITOR_VIF |
 		    IEEE80211_HW_QUEUE_CONTROL |
@@ -2059,8 +2057,10 @@ static int mac80211_hwsim_create_radio(int channels, const char *reg_alpha2,
 			    WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL |
 			    WIPHY_FLAG_AP_UAPSD |
 			    WIPHY_FLAG_HAS_CHANNEL_SWITCH;
-	hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR;
-	hw->wiphy->features |= NL80211_FEATURE_AP_MODE_CHAN_WIDTH_CHANGE;
+	hw->wiphy->features |= NL80211_FEATURE_ACTIVE_MONITOR |
+			       NL80211_FEATURE_AP_MODE_CHAN_WIDTH_CHANGE |
+			       NL80211_FEATURE_STATIC_SMPS |
+			       NL80211_FEATURE_DYNAMIC_SMPS;
 
 	/* ask mac80211 to reserve space for magic */
 	hw->vif_data_size = sizeof(struct hwsim_vif_priv);
diff --git a/drivers/net/wireless/mwifiex/11n_rxreorder.c b/drivers/net/wireless/mwifiex/11n_rxreorder.c
index 06a2c215ef5e..40057079ffb9 100644
--- a/drivers/net/wireless/mwifiex/11n_rxreorder.c
+++ b/drivers/net/wireless/mwifiex/11n_rxreorder.c
@@ -183,6 +183,15 @@ mwifiex_del_rx_reorder_entry(struct mwifiex_private *priv,
 	if (!tbl)
 		return;
 
+	spin_lock_irqsave(&priv->adapter->rx_proc_lock, flags);
+	priv->adapter->rx_locked = true;
+	if (priv->adapter->rx_processing) {
+		spin_unlock_irqrestore(&priv->adapter->rx_proc_lock, flags);
+		flush_workqueue(priv->adapter->rx_workqueue);
+	} else {
+		spin_unlock_irqrestore(&priv->adapter->rx_proc_lock, flags);
+	}
+
 	start_win = (tbl->start_win + tbl->win_size) & (MAX_TID_VALUE - 1);
 	mwifiex_11n_dispatch_pkt_until_start_win(priv, tbl, start_win);
 
@@ -194,6 +203,11 @@ mwifiex_del_rx_reorder_entry(struct mwifiex_private *priv,
 
 	kfree(tbl->rx_reorder_ptr);
 	kfree(tbl);
+
+	spin_lock_irqsave(&priv->adapter->rx_proc_lock, flags);
+	priv->adapter->rx_locked = false;
+	spin_unlock_irqrestore(&priv->adapter->rx_proc_lock, flags);
+
 }
 
 /*
diff --git a/drivers/net/wireless/mwifiex/Kconfig b/drivers/net/wireless/mwifiex/Kconfig
index ecdf34505b54..e70d0df9b0da 100644
--- a/drivers/net/wireless/mwifiex/Kconfig
+++ b/drivers/net/wireless/mwifiex/Kconfig
@@ -9,12 +9,12 @@ config MWIFIEX
 	  mwifiex.
 
 config MWIFIEX_SDIO
-	tristate "Marvell WiFi-Ex Driver for SD8786/SD8787/SD8797/SD8897"
+	tristate "Marvell WiFi-Ex Driver for SD8786/SD8787/SD8797/SD8887/SD8897"
 	depends on MWIFIEX && MMC
 	select FW_LOADER
 	---help---
 	  This adds support for wireless adapters based on Marvell
-	  8786/8787/8797 chipsets with SDIO interface.
+	  8786/8787/8797/8887/8897 chipsets with SDIO interface.
 
 	  If you choose to build it as a module, it will be called
 	  mwifiex_sdio.
diff --git a/drivers/net/wireless/mwifiex/cfg80211.c b/drivers/net/wireless/mwifiex/cfg80211.c
index e2e6bf13c2d8..0dd672954ad1 100644
--- a/drivers/net/wireless/mwifiex/cfg80211.c
+++ b/drivers/net/wireless/mwifiex/cfg80211.c
@@ -246,7 +246,7 @@ mwifiex_cfg80211_remain_on_channel(struct wiphy *wiphy,
 	}
 
 	if (priv->roc_cfg.cookie) {
-		wiphy_dbg(wiphy, "info: ongoing ROC, cookie = 0x%llu\n",
+		wiphy_dbg(wiphy, "info: ongoing ROC, cookie = 0x%llx\n",
 			  priv->roc_cfg.cookie);
 		return -EBUSY;
 	}
@@ -1557,6 +1557,7 @@ static int mwifiex_cfg80211_inform_ibss_bss(struct mwifiex_private *priv)
 						       band));
 
 	bss = cfg80211_inform_bss(priv->wdev->wiphy, chan,
+				  CFG80211_BSS_FTYPE_UNKNOWN,
 				  bss_info.bssid, 0, WLAN_CAPABILITY_IBSS,
 				  0, ie_buf, ie_len, 0, GFP_KERNEL);
 	cfg80211_put_bss(priv->wdev->wiphy, bss);
@@ -1935,13 +1936,6 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 
 	wiphy_dbg(wiphy, "info: received scan request on %s\n", dev->name);
 
-	if ((request->flags & NL80211_SCAN_FLAG_LOW_PRIORITY) &&
-	    atomic_read(&priv->wmm.tx_pkts_queued) >=
-	    MWIFIEX_MIN_TX_PENDING_TO_CANCEL_SCAN) {
-		dev_dbg(priv->adapter->dev, "scan rejected due to traffic\n");
-		return -EBUSY;
-	}
-
 	/* Block scan request if scan operation or scan cleanup when interface
 	 * is disabled is in process
 	 */
@@ -1980,7 +1974,7 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 		user_scan_cfg->chan_list[i].chan_number = chan->hw_value;
 		user_scan_cfg->chan_list[i].radio_type = chan->band;
 
-		if (chan->flags & IEEE80211_CHAN_NO_IR)
+		if ((chan->flags & IEEE80211_CHAN_NO_IR) || !request->n_ssids)
 			user_scan_cfg->chan_list[i].scan_type =
 						MWIFIEX_SCAN_TYPE_PASSIVE;
 		else
@@ -1990,6 +1984,11 @@ mwifiex_cfg80211_scan(struct wiphy *wiphy,
 		user_scan_cfg->chan_list[i].scan_time = 0;
 	}
 
+	if (priv->adapter->scan_chan_gap_enabled &&
+	    mwifiex_is_any_intf_active(priv))
+		user_scan_cfg->scan_chan_gap =
+					      priv->adapter->scan_chan_gap_time;
+
 	ret = mwifiex_scan_networks(priv, user_scan_cfg);
 	kfree(user_scan_cfg);
 	if (ret) {
@@ -2914,7 +2913,6 @@ int mwifiex_register_cfg80211(struct mwifiex_adapter *adapter)
 
 	wiphy->features |= NL80211_FEATURE_HT_IBSS |
 			   NL80211_FEATURE_INACTIVITY_TIMER |
-			   NL80211_FEATURE_LOW_PRIORITY_SCAN |
 			   NL80211_FEATURE_NEED_OBSS_SCAN;
 
 	/* Reserve space for mwifiex specific private data for BSS */
diff --git a/drivers/net/wireless/mwifiex/cmdevt.c b/drivers/net/wireless/mwifiex/cmdevt.c
index baf0aab63c04..85597200badc 100644
--- a/drivers/net/wireless/mwifiex/cmdevt.c
+++ b/drivers/net/wireless/mwifiex/cmdevt.c
@@ -1470,7 +1470,7 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 	struct host_cmd_ds_get_hw_spec *hw_spec = &resp->params.hw_spec;
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct mwifiex_ie_types_header *tlv;
-	struct hw_spec_fw_api_rev *api_rev;
+	struct hw_spec_api_rev *api_rev;
 	u16 resp_size, api_id;
 	int i, left_len, parsed_len = 0;
 
@@ -1538,23 +1538,30 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 		while (left_len > sizeof(struct mwifiex_ie_types_header)) {
 			tlv = (void *)&hw_spec->tlvs + parsed_len;
 			switch (le16_to_cpu(tlv->type)) {
-			case TLV_TYPE_FW_API_REV:
-				api_rev = (struct hw_spec_fw_api_rev *)tlv;
+			case TLV_TYPE_API_REV:
+				api_rev = (struct hw_spec_api_rev *)tlv;
 				api_id = le16_to_cpu(api_rev->api_id);
 				switch (api_id) {
 				case KEY_API_VER_ID:
-					adapter->fw_key_api_major_ver =
+					adapter->key_api_major_ver =
 							api_rev->major_ver;
-					adapter->fw_key_api_minor_ver =
+					adapter->key_api_minor_ver =
 							api_rev->minor_ver;
 					dev_dbg(adapter->dev,
-						"fw_key_api v%d.%d\n",
-						adapter->fw_key_api_major_ver,
-						adapter->fw_key_api_minor_ver);
+						"key_api v%d.%d\n",
+						adapter->key_api_major_ver,
+						adapter->key_api_minor_ver);
+					break;
+				case FW_API_VER_ID:
+					adapter->fw_api_ver =
+							api_rev->major_ver;
+					dev_dbg(adapter->dev,
+						"Firmware api version %d\n",
+						adapter->fw_api_ver);
 					break;
 				default:
 					dev_warn(adapter->dev,
-						 "Unknown FW api_id: %d\n",
+						 "Unknown api_id: %d\n",
 						 api_id);
 					break;
 				}
@@ -1567,7 +1574,8 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 			}
 			parsed_len += le16_to_cpu(tlv->len) +
 				      sizeof(struct mwifiex_ie_types_header);
-			left_len -= parsed_len;
+			left_len -= le16_to_cpu(tlv->len) +
+				      sizeof(struct mwifiex_ie_types_header);
 		}
 	}
 
@@ -1605,5 +1613,8 @@ int mwifiex_ret_get_hw_spec(struct mwifiex_private *priv,
 		adapter->if_ops.update_mp_end_port(adapter,
 					le16_to_cpu(hw_spec->mp_end_port));
 
+	if (adapter->fw_api_ver == MWIFIEX_FW_V15)
+		adapter->scan_chan_gap_enabled = true;
+
 	return 0;
 }
diff --git a/drivers/net/wireless/mwifiex/decl.h b/drivers/net/wireless/mwifiex/decl.h
index 0e03fe39fc35..e0d00a7f0ec3 100644
--- a/drivers/net/wireless/mwifiex/decl.h
+++ b/drivers/net/wireless/mwifiex/decl.h
@@ -48,8 +48,8 @@
 #define MWIFIEX_UAP_AMPDU_DEF_RXWINSIZE        16
 #define MWIFIEX_11AC_STA_AMPDU_DEF_TXWINSIZE   64
 #define MWIFIEX_11AC_STA_AMPDU_DEF_RXWINSIZE   64
-#define MWIFIEX_11AC_UAP_AMPDU_DEF_TXWINSIZE   48
-#define MWIFIEX_11AC_UAP_AMPDU_DEF_RXWINSIZE   32
+#define MWIFIEX_11AC_UAP_AMPDU_DEF_TXWINSIZE   64
+#define MWIFIEX_11AC_UAP_AMPDU_DEF_RXWINSIZE   64
 
 #define MWIFIEX_DEFAULT_BLOCK_ACK_TIMEOUT  0xffff
 
diff --git a/drivers/net/wireless/mwifiex/fw.h b/drivers/net/wireless/mwifiex/fw.h
index 49da2d53d294..1eb61739071f 100644
--- a/drivers/net/wireless/mwifiex/fw.h
+++ b/drivers/net/wireless/mwifiex/fw.h
@@ -83,7 +83,7 @@ enum KEY_TYPE_ID {
 #define WPA_PN_SIZE		8
 #define KEY_PARAMS_FIXED_LEN	10
 #define KEY_INDEX_MASK		0xf
-#define FW_KEY_API_VER_MAJOR_V2	2
+#define KEY_API_VER_MAJOR_V2	2
 
 #define KEY_MCAST	BIT(0)
 #define KEY_UNICAST	BIT(1)
@@ -170,7 +170,8 @@ enum MWIFIEX_802_11_PRIVACY_FILTER {
 #define TLV_TYPE_COALESCE_RULE      (PROPRIETARY_TLV_BASE_ID + 154)
 #define TLV_TYPE_KEY_PARAM_V2       (PROPRIETARY_TLV_BASE_ID + 156)
 #define TLV_TYPE_TDLS_IDLE_TIMEOUT  (PROPRIETARY_TLV_BASE_ID + 194)
-#define TLV_TYPE_FW_API_REV         (PROPRIETARY_TLV_BASE_ID + 199)
+#define TLV_TYPE_SCAN_CHANNEL_GAP   (PROPRIETARY_TLV_BASE_ID + 197)
+#define TLV_TYPE_API_REV            (PROPRIETARY_TLV_BASE_ID + 199)
 
 #define MWIFIEX_TX_DATA_BUF_SIZE_2K        2048
 
@@ -653,6 +654,12 @@ struct mwifiex_ie_types_num_probes {
 	__le16 num_probes;
 } __packed;
 
+struct mwifiex_ie_types_scan_chan_gap {
+	struct mwifiex_ie_types_header header;
+	/* time gap in TUs to be used between two consecutive channels scan */
+	__le16 chan_gap;
+} __packed;
+
 struct mwifiex_ie_types_wildcard_ssid_params {
 	struct mwifiex_ie_types_header header;
 	u8 max_ssid_length;
@@ -844,11 +851,12 @@ struct host_cmd_ds_802_11_ps_mode_enh {
 	} params;
 } __packed;
 
-enum FW_API_VER_ID {
+enum API_VER_ID {
 	KEY_API_VER_ID = 1,
+	FW_API_VER_ID = 2,
 };
 
-struct hw_spec_fw_api_rev {
+struct hw_spec_api_rev {
 	struct mwifiex_ie_types_header header;
 	__le16 api_id;
 	u8 major_ver;
@@ -1248,6 +1256,7 @@ struct mwifiex_user_scan_cfg {
 	u8 num_ssids;
 	/* Variable number (fixed maximum) of channels to scan up */
 	struct mwifiex_user_scan_chan chan_list[MWIFIEX_USER_SCAN_CHAN_MAX];
+	u16 scan_chan_gap;
 } __packed;
 
 struct ie_body {
diff --git a/drivers/net/wireless/mwifiex/init.c b/drivers/net/wireless/mwifiex/init.c
index 269a277d0a2e..580aa45ec4bc 100644
--- a/drivers/net/wireless/mwifiex/init.c
+++ b/drivers/net/wireless/mwifiex/init.c
@@ -212,6 +212,7 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	adapter->specific_scan_time = MWIFIEX_SPECIFIC_SCAN_CHAN_TIME;
 	adapter->active_scan_time = MWIFIEX_ACTIVE_SCAN_CHAN_TIME;
 	adapter->passive_scan_time = MWIFIEX_PASSIVE_SCAN_CHAN_TIME;
+	adapter->scan_chan_gap_time = MWIFIEX_DEF_SCAN_CHAN_GAP_TIME;
 
 	adapter->scan_probes = 1;
 
@@ -280,10 +281,9 @@ static void mwifiex_init_adapter(struct mwifiex_adapter *adapter)
 	memset(&adapter->arp_filter, 0, sizeof(adapter->arp_filter));
 	adapter->arp_filter_size = 0;
 	adapter->max_mgmt_ie_index = MAX_MGMT_IE_INDEX;
-	adapter->empty_tx_q_cnt = 0;
 	adapter->ext_scan = true;
-	adapter->fw_key_api_major_ver = 0;
-	adapter->fw_key_api_minor_ver = 0;
+	adapter->key_api_major_ver = 0;
+	adapter->key_api_minor_ver = 0;
 }
 
 /*
@@ -447,8 +447,10 @@ int mwifiex_init_lock_list(struct mwifiex_adapter *adapter)
 	spin_lock_init(&adapter->cmd_free_q_lock);
 	spin_lock_init(&adapter->cmd_pending_q_lock);
 	spin_lock_init(&adapter->scan_pending_q_lock);
+	spin_lock_init(&adapter->rx_proc_lock);
 
 	skb_queue_head_init(&adapter->usb_rx_data_q);
+	skb_queue_head_init(&adapter->rx_data_q);
 
 	for (i = 0; i < adapter->priv_num; ++i) {
 		INIT_LIST_HEAD(&adapter->bss_prio_tbl[i].bss_prio_head);
@@ -614,6 +616,7 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 	int ret = -EINPROGRESS;
 	struct mwifiex_private *priv;
 	s32 i;
+	unsigned long flags;
 	struct sk_buff *skb;
 
 	/* mwifiex already shutdown */
@@ -648,6 +651,21 @@ mwifiex_shutdown_drv(struct mwifiex_adapter *adapter)
 		}
 	}
 
+	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
+
+	while ((skb = skb_dequeue(&adapter->rx_data_q))) {
+		struct mwifiex_rxinfo *rx_info = MWIFIEX_SKB_RXCB(skb);
+
+		atomic_dec(&adapter->rx_pending);
+		priv = adapter->priv[rx_info->bss_num];
+		if (priv)
+			priv->stats.rx_dropped++;
+
+		dev_kfree_skb_any(skb);
+	}
+
+	spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+
 	spin_lock(&adapter->mwifiex_lock);
 
 	if (adapter->if_ops.data_complete) {
diff --git a/drivers/net/wireless/mwifiex/main.c b/drivers/net/wireless/mwifiex/main.c
index dfa37eadc4db..d5070c444fe1 100644
--- a/drivers/net/wireless/mwifiex/main.c
+++ b/drivers/net/wireless/mwifiex/main.c
@@ -28,91 +28,6 @@ const char driver_version[] = "mwifiex " VERSION " (%s) ";
 static char *cal_data_cfg;
 module_param(cal_data_cfg, charp, 0);
 
-static void scan_delay_timer_fn(unsigned long data)
-{
-	struct mwifiex_private *priv = (struct mwifiex_private *)data;
-	struct mwifiex_adapter *adapter = priv->adapter;
-	struct cmd_ctrl_node *cmd_node, *tmp_node;
-	spinlock_t *scan_q_lock = &adapter->scan_pending_q_lock;
-	unsigned long flags;
-
-	if (adapter->surprise_removed)
-		return;
-
-	if (adapter->scan_delay_cnt == MWIFIEX_MAX_SCAN_DELAY_CNT ||
-	    !adapter->scan_processing) {
-		/*
-		 * Abort scan operation by cancelling all pending scan
-		 * commands
-		 */
-		spin_lock_irqsave(scan_q_lock, flags);
-		list_for_each_entry_safe(cmd_node, tmp_node,
-					 &adapter->scan_pending_q, list) {
-			list_del(&cmd_node->list);
-			mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
-		}
-		spin_unlock_irqrestore(scan_q_lock, flags);
-
-		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
-		adapter->scan_processing = false;
-		adapter->scan_delay_cnt = 0;
-		adapter->empty_tx_q_cnt = 0;
-		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
-
-		if (priv->scan_request) {
-			dev_dbg(adapter->dev, "info: aborting scan\n");
-			cfg80211_scan_done(priv->scan_request, 1);
-			priv->scan_request = NULL;
-		} else {
-			priv->scan_aborting = false;
-			dev_dbg(adapter->dev, "info: scan already aborted\n");
-		}
-		goto done;
-	}
-
-	if (!atomic_read(&priv->adapter->is_tx_received)) {
-		adapter->empty_tx_q_cnt++;
-		if (adapter->empty_tx_q_cnt == MWIFIEX_MAX_EMPTY_TX_Q_CNT) {
-			/*
-			 * No Tx traffic for 200msec. Get scan command from
-			 * scan pending queue and put to cmd pending queue to
-			 * resume scan operation
-			 */
-			adapter->scan_delay_cnt = 0;
-			adapter->empty_tx_q_cnt = 0;
-			spin_lock_irqsave(scan_q_lock, flags);
-
-			if (list_empty(&adapter->scan_pending_q)) {
-				spin_unlock_irqrestore(scan_q_lock, flags);
-				goto done;
-			}
-
-			cmd_node = list_first_entry(&adapter->scan_pending_q,
-						    struct cmd_ctrl_node, list);
-			list_del(&cmd_node->list);
-			spin_unlock_irqrestore(scan_q_lock, flags);
-
-			mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
-							true);
-			queue_work(adapter->workqueue, &adapter->main_work);
-			goto done;
-		}
-	} else {
-		adapter->empty_tx_q_cnt = 0;
-	}
-
-	/* Delay scan operation further by 20msec */
-	mod_timer(&priv->scan_delay_timer, jiffies +
-		  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
-	adapter->scan_delay_cnt++;
-
-done:
-	if (atomic_read(&priv->adapter->is_tx_received))
-		atomic_set(&priv->adapter->is_tx_received, false);
-
-	return;
-}
-
 /*
  * This function registers the device and performs all the necessary
  * initializations.
@@ -160,10 +75,6 @@ static int mwifiex_register(void *card, struct mwifiex_if_ops *if_ops,
 
 		adapter->priv[i]->adapter = adapter;
 		adapter->priv_num++;
-
-		setup_timer(&adapter->priv[i]->scan_delay_timer,
-			    scan_delay_timer_fn,
-			    (unsigned long)adapter->priv[i]);
 	}
 	mwifiex_init_lock_list(adapter);
 
@@ -207,7 +118,6 @@ static int mwifiex_unregister(struct mwifiex_adapter *adapter)
 	for (i = 0; i < adapter->priv_num; i++) {
 		if (adapter->priv[i]) {
 			mwifiex_free_curr_bcn(adapter->priv[i]);
-			del_timer_sync(&adapter->priv[i]->scan_delay_timer);
 			kfree(adapter->priv[i]);
 		}
 	}
@@ -216,6 +126,38 @@ static int mwifiex_unregister(struct mwifiex_adapter *adapter)
 	return 0;
 }
 
+static int mwifiex_process_rx(struct mwifiex_adapter *adapter)
+{
+	unsigned long flags;
+	struct sk_buff *skb;
+
+	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
+	if (adapter->rx_processing || adapter->rx_locked) {
+		spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+		goto exit_rx_proc;
+	} else {
+		adapter->rx_processing = true;
+		spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+	}
+
+	/* Check for Rx data */
+	while ((skb = skb_dequeue(&adapter->rx_data_q))) {
+		atomic_dec(&adapter->rx_pending);
+		if (adapter->delay_main_work &&
+		    (atomic_read(&adapter->rx_pending) < LOW_RX_PENDING)) {
+			adapter->delay_main_work = false;
+			queue_work(adapter->workqueue, &adapter->main_work);
+		}
+		mwifiex_handle_rx_packet(adapter, skb);
+	}
+	spin_lock_irqsave(&adapter->rx_proc_lock, flags);
+	adapter->rx_processing = false;
+	spin_unlock_irqrestore(&adapter->rx_proc_lock, flags);
+
+exit_rx_proc:
+	return 0;
+}
+
 /*
  * The main process.
  *
@@ -253,6 +195,19 @@ process_start:
 		    (adapter->hw_status == MWIFIEX_HW_STATUS_NOT_READY))
 			break;
 
+		/* If we process interrupts first, it would increase RX pending
+		 * even further. Avoid this by checking if rx_pending has
+		 * crossed high threshold and schedule rx work queue
+		 * and then process interrupts
+		 */
+		if (atomic_read(&adapter->rx_pending) >= HIGH_RX_PENDING) {
+			adapter->delay_main_work = true;
+			if (!adapter->rx_processing)
+				queue_work(adapter->rx_workqueue,
+					   &adapter->rx_work);
+			break;
+		}
+
 		/* Handle pending interrupt if any */
 		if (adapter->int_status) {
 			if (adapter->hs_activated)
@@ -261,6 +216,9 @@ process_start:
 				adapter->if_ops.process_int_status(adapter);
 		}
 
+		if (adapter->rx_work_enabled && adapter->data_received)
+			queue_work(adapter->rx_workqueue, &adapter->rx_work);
+
 		/* Need to wake up the card ? */
 		if ((adapter->ps_state == PS_STATE_SLEEP) &&
 		    (adapter->pm_wakeup_card_req &&
@@ -273,6 +231,7 @@ process_start:
 		}
 
 		if (IS_CARD_RX_RCVD(adapter)) {
+			adapter->data_received = false;
 			adapter->pm_wakeup_fw_try = false;
 			if (adapter->ps_state == PS_STATE_SLEEP)
 				adapter->ps_state = PS_STATE_AWAKE;
@@ -284,8 +243,8 @@ process_start:
 			    adapter->tx_lock_flag)
 				break;
 
-			if ((adapter->scan_processing &&
-			     !adapter->scan_delay_cnt) || adapter->data_sent ||
+			if ((!adapter->scan_chan_gap_enabled &&
+			     adapter->scan_processing) || adapter->data_sent ||
 			    mwifiex_wmm_lists_empty(adapter)) {
 				if (adapter->cmd_sent || adapter->curr_cmd ||
 				    (!is_command_pending(adapter)))
@@ -339,7 +298,8 @@ process_start:
 			}
 		}
 
-		if ((!adapter->scan_processing || adapter->scan_delay_cnt) &&
+		if ((adapter->scan_chan_gap_enabled ||
+		     !adapter->scan_processing) &&
 		    !adapter->data_sent && !mwifiex_wmm_lists_empty(adapter)) {
 			mwifiex_wmm_process_tx(adapter);
 			if (adapter->hs_activated) {
@@ -366,7 +326,8 @@ process_start:
 	} while (true);
 
 	spin_lock_irqsave(&adapter->main_proc_lock, flags);
-	if ((adapter->int_status) || IS_CARD_RX_RCVD(adapter)) {
+	if (!adapter->delay_main_work &&
+	    (adapter->int_status || IS_CARD_RX_RCVD(adapter))) {
 		spin_unlock_irqrestore(&adapter->main_proc_lock, flags);
 		goto process_start;
 	}
@@ -407,6 +368,12 @@ static void mwifiex_terminate_workqueue(struct mwifiex_adapter *adapter)
 	flush_workqueue(adapter->workqueue);
 	destroy_workqueue(adapter->workqueue);
 	adapter->workqueue = NULL;
+
+	if (adapter->rx_workqueue) {
+		flush_workqueue(adapter->rx_workqueue);
+		destroy_workqueue(adapter->rx_workqueue);
+		adapter->rx_workqueue = NULL;
+	}
 }
 
 /*
@@ -598,9 +565,6 @@ int mwifiex_queue_tx_pkt(struct mwifiex_private *priv, struct sk_buff *skb)
 	atomic_inc(&priv->adapter->tx_pending);
 	mwifiex_wmm_add_buf_txqueue(priv, skb);
 
-	if (priv->adapter->scan_delay_cnt)
-		atomic_set(&priv->adapter->is_tx_received, true);
-
 	queue_work(priv->adapter->workqueue, &priv->adapter->main_work);
 
 	return 0;
@@ -824,6 +788,21 @@ int is_command_pending(struct mwifiex_adapter *adapter)
 }
 
 /*
+ * This is the RX work queue function.
+ *
+ * It handles the RX operations.
+ */
+static void mwifiex_rx_work_queue(struct work_struct *work)
+{
+	struct mwifiex_adapter *adapter =
+		container_of(work, struct mwifiex_adapter, rx_work);
+
+	if (adapter->surprise_removed)
+		return;
+	mwifiex_process_rx(adapter);
+}
+
+/*
  * This is the main work queue function.
  *
  * It handles the main process, which in turn handles the complete
@@ -879,6 +858,11 @@ mwifiex_add_card(void *card, struct semaphore *sem,
 	adapter->cmd_wait_q.status = 0;
 	adapter->scan_wait_q_woken = false;
 
+	if (num_possible_cpus() > 1) {
+		adapter->rx_work_enabled = true;
+		pr_notice("rx work enabled, cpus %d\n", num_possible_cpus());
+	}
+
 	adapter->workqueue =
 		alloc_workqueue("MWIFIEX_WORK_QUEUE",
 				WQ_HIGHPRI | WQ_MEM_RECLAIM | WQ_UNBOUND, 1);
@@ -886,6 +870,18 @@ mwifiex_add_card(void *card, struct semaphore *sem,
 		goto err_kmalloc;
 
 	INIT_WORK(&adapter->main_work, mwifiex_main_work_queue);
+
+	if (adapter->rx_work_enabled) {
+		adapter->rx_workqueue = alloc_workqueue("MWIFIEX_RX_WORK_QUEUE",
+							WQ_HIGHPRI |
+							WQ_MEM_RECLAIM |
+							WQ_UNBOUND, 1);
+		if (!adapter->rx_workqueue)
+			goto err_kmalloc;
+
+		INIT_WORK(&adapter->rx_work, mwifiex_rx_work_queue);
+	}
+
 	if (adapter->if_ops.iface_work)
 		INIT_WORK(&adapter->iface_work, adapter->if_ops.iface_work);
 
diff --git a/drivers/net/wireless/mwifiex/main.h b/drivers/net/wireless/mwifiex/main.h
index a2733b1e63f9..e2635747d966 100644
--- a/drivers/net/wireless/mwifiex/main.h
+++ b/drivers/net/wireless/mwifiex/main.h
@@ -58,6 +58,9 @@ enum {
 #define MAX_TX_PENDING      100
 #define LOW_TX_PENDING      80
 
+#define HIGH_RX_PENDING     50
+#define LOW_RX_PENDING      20
+
 #define MWIFIEX_UPLD_SIZE               (2312)
 
 #define MAX_EVENT_SIZE                  2048
@@ -84,17 +87,12 @@ enum {
 #define MWIFIEX_PASSIVE_SCAN_CHAN_TIME	110
 #define MWIFIEX_ACTIVE_SCAN_CHAN_TIME	30
 #define MWIFIEX_SPECIFIC_SCAN_CHAN_TIME	30
+#define MWIFIEX_DEF_SCAN_CHAN_GAP_TIME  50
 
 #define SCAN_RSSI(RSSI)					(0x100 - ((u8)(RSSI)))
 
 #define MWIFIEX_MAX_TOTAL_SCAN_TIME	(MWIFIEX_TIMER_10S - MWIFIEX_TIMER_1S)
 
-#define MWIFIEX_MAX_SCAN_DELAY_CNT			50
-#define MWIFIEX_MAX_EMPTY_TX_Q_CNT			10
-#define MWIFIEX_SCAN_DELAY_MSEC				20
-
-#define MWIFIEX_MIN_TX_PENDING_TO_CANCEL_SCAN		2
-
 #define RSN_GTK_OUI_OFFSET				2
 
 #define MWIFIEX_OUI_NOT_PRESENT			0
@@ -415,6 +413,7 @@ struct mwifiex_roc_cfg {
 #define FW_DUMP_MAX_NAME_LEN		8
 #define FW_DUMP_HOST_READY		0xEE
 #define FW_DUMP_DONE			0xFF
+#define FW_DUMP_READ_DONE		0xFE
 
 struct memory_type_mapping {
 	u8 mem_name[FW_DUMP_MAX_NAME_LEN];
@@ -547,7 +546,6 @@ struct mwifiex_private {
 	u8 nick_name[16];
 	u16 current_key_index;
 	struct semaphore async_sem;
-	u8 report_scan_result;
 	struct cfg80211_scan_request *scan_request;
 	u8 cfg_bssid[6];
 	struct wps wps;
@@ -561,7 +559,6 @@ struct mwifiex_private {
 	u16 proberesp_idx;
 	u16 assocresp_idx;
 	u16 rsn_idx;
-	struct timer_list scan_delay_timer;
 	u8 ap_11n_enabled;
 	u8 ap_11ac_enabled;
 	u32 mgmt_frame_mask;
@@ -721,6 +718,12 @@ struct mwifiex_adapter {
 	atomic_t cmd_pending;
 	struct workqueue_struct *workqueue;
 	struct work_struct main_work;
+	struct workqueue_struct *rx_workqueue;
+	struct work_struct rx_work;
+	bool rx_work_enabled;
+	bool rx_processing;
+	bool delay_main_work;
+	bool rx_locked;
 	struct mwifiex_bss_prio_tbl bss_prio_tbl[MWIFIEX_MAX_BSS_NUM];
 	/* spin lock for init/shutdown */
 	spinlock_t mwifiex_lock;
@@ -761,6 +764,8 @@ struct mwifiex_adapter {
 	struct list_head scan_pending_q;
 	/* spin lock for scan_pending_q */
 	spinlock_t scan_pending_q_lock;
+	/* spin lock for RX processing routine */
+	spinlock_t rx_proc_lock;
 	struct sk_buff_head usb_rx_data_q;
 	u32 scan_processing;
 	u16 region_code;
@@ -770,6 +775,7 @@ struct mwifiex_adapter {
 	u16 specific_scan_time;
 	u16 active_scan_time;
 	u16 passive_scan_time;
+	u16 scan_chan_gap_time;
 	u8 fw_bands;
 	u8 adhoc_start_band;
 	u8 config_bands;
@@ -815,8 +821,6 @@ struct mwifiex_adapter {
 	spinlock_t queue_lock;		/* lock for tx queues */
 	u8 country_code[IEEE80211_COUNTRY_STRING_LEN];
 	u16 max_mgmt_ie_index;
-	u8 scan_delay_cnt;
-	u8 empty_tx_q_cnt;
 	const struct firmware *cal_data;
 	struct device_node *dt_node;
 
@@ -828,17 +832,18 @@ struct mwifiex_adapter {
 	u32 usr_dot_11ac_dev_cap_a;
 	u32 usr_dot_11ac_mcs_support;
 
-	atomic_t is_tx_received;
 	atomic_t pending_bridged_pkts;
 	struct semaphore *card_sem;
 	bool ext_scan;
 	u8 fw_api_ver;
-	u8 fw_key_api_major_ver, fw_key_api_minor_ver;
+	u8 key_api_major_ver, key_api_minor_ver;
 	struct work_struct iface_work;
 	unsigned long iface_work_flags;
 	struct memory_type_mapping *mem_type_mapping_tbl;
 	u8 num_mem_types;
 	u8 curr_mem_idx;
+	bool scan_chan_gap_enabled;
+	struct sk_buff_head rx_data_q;
 };
 
 int mwifiex_init_lock_list(struct mwifiex_adapter *adapter);
@@ -1139,6 +1144,25 @@ mwifiex_11h_get_csa_closed_channel(struct mwifiex_private *priv)
 	return priv->csa_chan;
 }
 
+static inline u8 mwifiex_is_any_intf_active(struct mwifiex_private *priv)
+{
+	struct mwifiex_private *priv_num;
+	int i;
+
+	for (i = 0; i < priv->adapter->priv_num; i++) {
+		priv_num = priv->adapter->priv[i];
+		if (priv_num) {
+			if ((GET_BSS_ROLE(priv_num) == MWIFIEX_BSS_ROLE_UAP &&
+			     priv_num->bss_started) ||
+			    (GET_BSS_ROLE(priv_num) == MWIFIEX_BSS_ROLE_STA &&
+			     priv_num->media_connected))
+				return 1;
+		}
+	}
+
+	return 0;
+}
+
 int mwifiex_init_shutdown_fw(struct mwifiex_private *priv,
 			     u32 func_init_shutdown);
 int mwifiex_add_card(void *, struct semaphore *, struct mwifiex_if_ops *, u8);
@@ -1274,6 +1298,7 @@ void mwifiex_disable_all_tdls_links(struct mwifiex_private *priv);
 bool mwifiex_is_bss_in_11ac_mode(struct mwifiex_private *priv);
 u8 mwifiex_get_center_freq_index(struct mwifiex_private *priv, u8 band,
 				 u32 pri_chan, u8 chan_bw);
+int mwifiex_init_channel_scan_gap(struct mwifiex_adapter *adapter);
 
 #ifdef CONFIG_DEBUG_FS
 void mwifiex_debugfs_init(void);
diff --git a/drivers/net/wireless/mwifiex/pcie.c b/drivers/net/wireless/mwifiex/pcie.c
index ff0545888dd0..c3a20f94f3c9 100644
--- a/drivers/net/wireless/mwifiex/pcie.c
+++ b/drivers/net/wireless/mwifiex/pcie.c
@@ -42,6 +42,10 @@ static struct memory_type_mapping mem_type_mapping_tbl[] = {
 	{"DTCM", NULL, 0, 0xF1},
 	{"SQRAM", NULL, 0, 0xF2},
 	{"IRAM", NULL, 0, 0xF3},
+	{"APU", NULL, 0, 0xF4},
+	{"CIU", NULL, 0, 0xF5},
+	{"ICU", NULL, 0, 0xF6},
+	{"MAC", NULL, 0, 0xF7},
 };
 
 static int
@@ -1271,12 +1275,26 @@ static int mwifiex_pcie_process_recv_data(struct mwifiex_adapter *adapter)
 		 */
 		pkt_len = *((__le16 *)skb_data->data);
 		rx_len = le16_to_cpu(pkt_len);
-		skb_put(skb_data, rx_len);
-		dev_dbg(adapter->dev,
-			"info: RECV DATA: Rd=%#x, Wr=%#x, Len=%d\n",
-			card->rxbd_rdptr, wrptr, rx_len);
-		skb_pull(skb_data, INTF_HEADER_LEN);
-		mwifiex_handle_rx_packet(adapter, skb_data);
+		if (WARN_ON(rx_len <= INTF_HEADER_LEN ||
+			    rx_len > MWIFIEX_RX_DATA_BUF_SIZE)) {
+			dev_err(adapter->dev,
+				"Invalid RX len %d, Rd=%#x, Wr=%#x\n",
+				rx_len, card->rxbd_rdptr, wrptr);
+			dev_kfree_skb_any(skb_data);
+		} else {
+			skb_put(skb_data, rx_len);
+			dev_dbg(adapter->dev,
+				"info: RECV DATA: Rd=%#x, Wr=%#x, Len=%d\n",
+				card->rxbd_rdptr, wrptr, rx_len);
+			skb_pull(skb_data, INTF_HEADER_LEN);
+			if (adapter->rx_work_enabled) {
+				skb_queue_tail(&adapter->rx_data_q, skb_data);
+				adapter->data_received = true;
+				atomic_inc(&adapter->rx_pending);
+			} else {
+				mwifiex_handle_rx_packet(adapter, skb_data);
+			}
+		}
 
 		skb_tmp = dev_alloc_skb(MWIFIEX_RX_DATA_BUF_SIZE);
 		if (!skb_tmp) {
@@ -1718,6 +1736,13 @@ static int mwifiex_pcie_process_event_ready(struct mwifiex_adapter *adapter)
 		   buffer is released. This is just to make things simpler,
 		   we need to find a better method of managing these buffers.
 		*/
+	} else {
+		if (mwifiex_write_reg(adapter, PCIE_CPU_INT_EVENT,
+				      CPU_INTR_EVENT_DONE)) {
+			dev_warn(adapter->dev,
+				 "Write register failed\n");
+			return -1;
+		}
 	}
 
 	return 0;
@@ -2218,8 +2243,8 @@ mwifiex_pcie_rdwr_firmware(struct mwifiex_adapter *adapter, u8 doneflag)
 		if (ctrl_data != FW_DUMP_HOST_READY) {
 			dev_info(adapter->dev,
 				 "The ctrl reg was changed, re-try again!\n");
-			mwifiex_write_reg(adapter, reg->fw_dump_ctrl,
-					  FW_DUMP_HOST_READY);
+			ret = mwifiex_write_reg(adapter, reg->fw_dump_ctrl,
+						FW_DUMP_HOST_READY);
 			if (ret) {
 				dev_err(adapter->dev, "PCIE write err\n");
 				return RDWR_STATUS_FAILURE;
@@ -2241,6 +2266,7 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 	u8 *dbg_ptr, *end_ptr, dump_num, idx, i, read_reg, doneflag = 0;
 	enum rdwr_status stat;
 	u32 memory_size;
+	int ret;
 	static char *env[] = { "DRIVER=mwifiex_pcie", "EVENT=fw_dump", NULL };
 
 	if (!card->pcie.supports_fw_dump)
@@ -2284,6 +2310,12 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 
 		if (memory_size == 0) {
 			dev_info(adapter->dev, "Firmware dump Finished!\n");
+			ret = mwifiex_write_reg(adapter, creg->fw_dump_ctrl,
+						FW_DUMP_READ_DONE);
+			if (ret) {
+				dev_err(adapter->dev, "PCIE write err\n");
+				goto done;
+			}
 			break;
 		}
 
@@ -2312,11 +2344,13 @@ static void mwifiex_pcie_fw_dump_work(struct mwifiex_adapter *adapter)
 			reg_end = creg->fw_dump_end;
 			for (reg = reg_start; reg <= reg_end; reg++) {
 				mwifiex_read_reg_byte(adapter, reg, dbg_ptr);
-				if (dbg_ptr < end_ptr)
+				if (dbg_ptr < end_ptr) {
 					dbg_ptr++;
-				else
+				} else {
 					dev_err(adapter->dev,
 						"Allocated buf not enough\n");
+					goto done;
+				}
 			}
 
 			if (stat != RDWR_STATUS_DONE)
diff --git a/drivers/net/wireless/mwifiex/pcie.h b/drivers/net/wireless/mwifiex/pcie.h
index a1a8fd3bc1be..200e8b0cb582 100644
--- a/drivers/net/wireless/mwifiex/pcie.h
+++ b/drivers/net/wireless/mwifiex/pcie.h
@@ -40,8 +40,8 @@
 #define MWIFIEX_TXBD_MASK			0x3F
 #define MWIFIEX_RXBD_MASK			0x3F
 
-#define MWIFIEX_MAX_EVT_BD			0x04
-#define MWIFIEX_EVTBD_MASK			0x07
+#define MWIFIEX_MAX_EVT_BD			0x08
+#define MWIFIEX_EVTBD_MASK			0x0f
 
 /* PCIE INTERNAL REGISTERS */
 #define PCIE_SCRATCH_0_REG				0xC10
@@ -69,6 +69,7 @@
 #define CPU_INTR_DOOR_BELL				BIT(1)
 #define CPU_INTR_SLEEP_CFM_DONE			BIT(2)
 #define CPU_INTR_RESET					BIT(3)
+#define CPU_INTR_EVENT_DONE				BIT(5)
 
 #define HOST_INTR_DNLD_DONE				BIT(0)
 #define HOST_INTR_UPLD_RDY				BIT(1)
diff --git a/drivers/net/wireless/mwifiex/scan.c b/drivers/net/wireless/mwifiex/scan.c
index dee717a19ddb..ca64d4c94112 100644
--- a/drivers/net/wireless/mwifiex/scan.c
+++ b/drivers/net/wireless/mwifiex/scan.c
@@ -799,6 +799,7 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
 	struct mwifiex_ie_types_num_probes *num_probes_tlv;
+	struct mwifiex_ie_types_scan_chan_gap *chan_gap_tlv;
 	struct mwifiex_ie_types_wildcard_ssid_params *wildcard_ssid_tlv;
 	struct mwifiex_ie_types_bssid_list *bssid_tlv;
 	u8 *tlv_pos;
@@ -925,6 +926,23 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 		if ((i && ssid_filter) ||
 		    !is_zero_ether_addr(scan_cfg_out->specific_bssid))
 			*filtered_scan = true;
+
+		if (user_scan_in->scan_chan_gap) {
+			dev_dbg(adapter->dev, "info: scan: channel gap = %d\n",
+				user_scan_in->scan_chan_gap);
+			*max_chan_per_scan =
+					MWIFIEX_MAX_CHANNELS_PER_SPECIFIC_SCAN;
+
+			chan_gap_tlv = (void *)tlv_pos;
+			chan_gap_tlv->header.type =
+					 cpu_to_le16(TLV_TYPE_SCAN_CHANNEL_GAP);
+			chan_gap_tlv->header.len =
+				    cpu_to_le16(sizeof(chan_gap_tlv->chan_gap));
+			chan_gap_tlv->chan_gap =
+				     cpu_to_le16((user_scan_in->scan_chan_gap));
+			tlv_pos +=
+				  sizeof(struct mwifiex_ie_types_scan_chan_gap);
+		}
 	} else {
 		scan_cfg_out->bss_mode = (u8) adapter->scan_mode;
 		num_probes = adapter->scan_probes;
@@ -1050,12 +1068,6 @@ mwifiex_config_scan(struct mwifiex_private *priv,
 							    *filtered_scan);
 	}
 
-	/*
-	 * In associated state we will reduce the number of channels scanned per
-	 * scan command to 1 to avoid any traffic delay/loss.
-	 */
-	if (priv->media_connected)
-			*max_chan_per_scan = 1;
 }
 
 /*
@@ -1719,7 +1731,8 @@ mwifiex_parse_single_response_buf(struct mwifiex_private *priv, u8 **bss_info,
 
 		if (chan && !(chan->flags & IEEE80211_CHAN_DISABLED)) {
 			bss = cfg80211_inform_bss(priv->wdev->wiphy,
-					    chan, bssid, timestamp,
+					    chan, CFG80211_BSS_FTYPE_UNKNOWN,
+					    bssid, timestamp,
 					    cap_info_bitmap, beacon_period,
 					    ie_buf, ie_len, rssi, GFP_KERNEL);
 			bss_priv = (struct mwifiex_bss_priv *)bss->priv;
@@ -1754,7 +1767,7 @@ static void mwifiex_complete_scan(struct mwifiex_private *priv)
 static void mwifiex_check_next_scan_command(struct mwifiex_private *priv)
 {
 	struct mwifiex_adapter *adapter = priv->adapter;
-	struct cmd_ctrl_node *cmd_node;
+	struct cmd_ctrl_node *cmd_node, *tmp_node;
 	unsigned long flags;
 
 	spin_lock_irqsave(&adapter->scan_pending_q_lock, flags);
@@ -1767,9 +1780,6 @@ static void mwifiex_check_next_scan_command(struct mwifiex_private *priv)
 		if (!adapter->ext_scan)
 			mwifiex_complete_scan(priv);
 
-		if (priv->report_scan_result)
-			priv->report_scan_result = false;
-
 		if (priv->scan_request) {
 			dev_dbg(adapter->dev, "info: notifying scan done\n");
 			cfg80211_scan_done(priv->scan_request, 0);
@@ -1778,37 +1788,36 @@ static void mwifiex_check_next_scan_command(struct mwifiex_private *priv)
 			priv->scan_aborting = false;
 			dev_dbg(adapter->dev, "info: scan already aborted\n");
 		}
-	} else {
-		if ((priv->scan_aborting && !priv->scan_request) ||
-		    priv->scan_block) {
-			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
-					       flags);
-			adapter->scan_delay_cnt = MWIFIEX_MAX_SCAN_DELAY_CNT;
-			mod_timer(&priv->scan_delay_timer, jiffies);
-			dev_dbg(priv->adapter->dev,
-				"info: %s: triggerring scan abort\n", __func__);
-		} else if (!mwifiex_wmm_lists_empty(adapter) &&
-			   (priv->scan_request && (priv->scan_request->flags &
-					    NL80211_SCAN_FLAG_LOW_PRIORITY))) {
-			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
-					       flags);
-			adapter->scan_delay_cnt = 1;
-			mod_timer(&priv->scan_delay_timer, jiffies +
-				  msecs_to_jiffies(MWIFIEX_SCAN_DELAY_MSEC));
-			dev_dbg(priv->adapter->dev,
-				"info: %s: deferring scan\n", __func__);
-		} else {
-			/* Get scan command from scan_pending_q and put to
-			 * cmd_pending_q
-			 */
-			cmd_node = list_first_entry(&adapter->scan_pending_q,
-						    struct cmd_ctrl_node, list);
+	} else if ((priv->scan_aborting && !priv->scan_request) ||
+		   priv->scan_block) {
+		list_for_each_entry_safe(cmd_node, tmp_node,
+					 &adapter->scan_pending_q, list) {
 			list_del(&cmd_node->list);
-			spin_unlock_irqrestore(&adapter->scan_pending_q_lock,
-					       flags);
-			mwifiex_insert_cmd_to_pending_q(adapter, cmd_node,
-							true);
+			mwifiex_insert_cmd_to_free_q(adapter, cmd_node);
+		}
+		spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
+
+		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
+		adapter->scan_processing = false;
+		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
+
+		if (priv->scan_request) {
+			dev_dbg(adapter->dev, "info: aborting scan\n");
+			cfg80211_scan_done(priv->scan_request, 1);
+			priv->scan_request = NULL;
+		} else {
+			priv->scan_aborting = false;
+			dev_dbg(adapter->dev, "info: scan already aborted\n");
 		}
+	} else {
+		/* Get scan command from scan_pending_q and put to
+		 * cmd_pending_q
+		 */
+		cmd_node = list_first_entry(&adapter->scan_pending_q,
+					    struct cmd_ctrl_node, list);
+		list_del(&cmd_node->list);
+		spin_unlock_irqrestore(&adapter->scan_pending_q_lock, flags);
+		mwifiex_insert_cmd_to_pending_q(adapter, cmd_node, true);
 	}
 
 	return;
@@ -1970,9 +1979,34 @@ int mwifiex_cmd_802_11_scan_ext(struct mwifiex_private *priv,
 /* This function handles the command response of extended scan */
 int mwifiex_ret_802_11_scan_ext(struct mwifiex_private *priv)
 {
+	struct mwifiex_adapter *adapter = priv->adapter;
+	struct host_cmd_ds_command *cmd_ptr;
+	struct cmd_ctrl_node *cmd_node;
+	unsigned long cmd_flags, scan_flags;
+	bool complete_scan = false;
+
 	dev_dbg(priv->adapter->dev, "info: EXT scan returns successfully\n");
 
-	mwifiex_complete_scan(priv);
+	spin_lock_irqsave(&adapter->cmd_pending_q_lock, cmd_flags);
+	spin_lock_irqsave(&adapter->scan_pending_q_lock, scan_flags);
+	if (list_empty(&adapter->scan_pending_q)) {
+		complete_scan = true;
+		list_for_each_entry(cmd_node, &adapter->cmd_pending_q, list) {
+			cmd_ptr = (void *)cmd_node->cmd_skb->data;
+			if (le16_to_cpu(cmd_ptr->command) ==
+			    HostCmd_CMD_802_11_SCAN_EXT) {
+				dev_dbg(priv->adapter->dev,
+					"Scan pending in command pending list");
+				complete_scan = false;
+				break;
+			}
+		}
+	}
+	spin_unlock_irqrestore(&adapter->scan_pending_q_lock, scan_flags);
+	spin_unlock_irqrestore(&adapter->cmd_pending_q_lock, cmd_flags);
+
+	if (complete_scan)
+		mwifiex_complete_scan(priv);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/mwifiex/sdio.c b/drivers/net/wireless/mwifiex/sdio.c
index 1770fa3fc1e6..b25766b43b9f 100644
--- a/drivers/net/wireless/mwifiex/sdio.c
+++ b/drivers/net/wireless/mwifiex/sdio.c
@@ -279,6 +279,8 @@ static int mwifiex_sdio_suspend(struct device *dev)
 #define SDIO_DEVICE_ID_MARVELL_8797   (0x9129)
 /* Device ID for SD8897 */
 #define SDIO_DEVICE_ID_MARVELL_8897   (0x912d)
+/* Device ID for SD8887 */
+#define SDIO_DEVICE_ID_MARVELL_8887   (0x9135)
 
 /* WLAN IDs */
 static const struct sdio_device_id mwifiex_ids[] = {
@@ -290,6 +292,8 @@ static const struct sdio_device_id mwifiex_ids[] = {
 		.driver_data = (unsigned long) &mwifiex_sdio_sd8797},
 	{SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, SDIO_DEVICE_ID_MARVELL_8897),
 		.driver_data = (unsigned long) &mwifiex_sdio_sd8897},
+	{SDIO_DEVICE(SDIO_VENDOR_ID_MARVELL, SDIO_DEVICE_ID_MARVELL_8887),
+		.driver_data = (unsigned long)&mwifiex_sdio_sd8887},
 	{},
 };
 
@@ -448,28 +452,31 @@ static int mwifiex_pm_wakeup_card_complete(struct mwifiex_adapter *adapter)
 static int mwifiex_init_sdio_new_mode(struct mwifiex_adapter *adapter)
 {
 	u8 reg;
+	struct sdio_mmc_card *card = adapter->card;
 
 	adapter->ioport = MEM_PORT;
 
 	/* enable sdio new mode */
-	if (mwifiex_read_reg(adapter, CARD_CONFIG_2_1_REG, &reg))
+	if (mwifiex_read_reg(adapter, card->reg->card_cfg_2_1_reg, &reg))
 		return -1;
-	if (mwifiex_write_reg(adapter, CARD_CONFIG_2_1_REG,
+	if (mwifiex_write_reg(adapter, card->reg->card_cfg_2_1_reg,
 			      reg | CMD53_NEW_MODE))
 		return -1;
 
 	/* Configure cmd port and enable reading rx length from the register */
-	if (mwifiex_read_reg(adapter, CMD_CONFIG_0, &reg))
+	if (mwifiex_read_reg(adapter, card->reg->cmd_cfg_0, &reg))
 		return -1;
-	if (mwifiex_write_reg(adapter, CMD_CONFIG_0, reg | CMD_PORT_RD_LEN_EN))
+	if (mwifiex_write_reg(adapter, card->reg->cmd_cfg_0,
+			      reg | CMD_PORT_RD_LEN_EN))
 		return -1;
 
 	/* Enable Dnld/Upld ready auto reset for cmd port after cmd53 is
 	 * completed
 	 */
-	if (mwifiex_read_reg(adapter, CMD_CONFIG_1, &reg))
+	if (mwifiex_read_reg(adapter, card->reg->cmd_cfg_1, &reg))
 		return -1;
-	if (mwifiex_write_reg(adapter, CMD_CONFIG_1, reg | CMD_PORT_AUTO_EN))
+	if (mwifiex_write_reg(adapter, card->reg->cmd_cfg_1,
+			      reg | CMD_PORT_AUTO_EN))
 		return -1;
 
 	return 0;
@@ -496,17 +503,17 @@ static int mwifiex_init_sdio_ioport(struct mwifiex_adapter *adapter)
 	}
 
 	/* Read the IO port */
-	if (!mwifiex_read_reg(adapter, IO_PORT_0_REG, &reg))
+	if (!mwifiex_read_reg(adapter, card->reg->io_port_0_reg, &reg))
 		adapter->ioport |= (reg & 0xff);
 	else
 		return -1;
 
-	if (!mwifiex_read_reg(adapter, IO_PORT_1_REG, &reg))
+	if (!mwifiex_read_reg(adapter, card->reg->io_port_1_reg, &reg))
 		adapter->ioport |= ((reg & 0xff) << 8);
 	else
 		return -1;
 
-	if (!mwifiex_read_reg(adapter, IO_PORT_2_REG, &reg))
+	if (!mwifiex_read_reg(adapter, card->reg->io_port_2_reg, &reg))
 		adapter->ioport |= ((reg & 0xff) << 16);
 	else
 		return -1;
@@ -514,8 +521,8 @@ cont:
 	pr_debug("info: SDIO FUNC1 IO port: %#x\n", adapter->ioport);
 
 	/* Set Host interrupt reset to read to clear */
-	if (!mwifiex_read_reg(adapter, HOST_INT_RSR_REG, &reg))
-		mwifiex_write_reg(adapter, HOST_INT_RSR_REG,
+	if (!mwifiex_read_reg(adapter, card->reg->host_int_rsr_reg, &reg))
+		mwifiex_write_reg(adapter, card->reg->host_int_rsr_reg,
 				  reg | card->reg->sdio_int_mask);
 	else
 		return -1;
@@ -622,22 +629,15 @@ static int mwifiex_get_wr_port_data(struct mwifiex_adapter *adapter, u32 *port)
 
 	dev_dbg(adapter->dev, "data: mp_wr_bitmap=0x%08x\n", wr_bitmap);
 
-	if (card->supports_sdio_new_mode &&
-	    !(wr_bitmap & reg->data_port_mask)) {
+	if (!(wr_bitmap & card->mp_data_port_mask)) {
 		adapter->data_sent = true;
 		return -EBUSY;
-	} else if (!card->supports_sdio_new_mode &&
-		   !(wr_bitmap & card->mp_data_port_mask)) {
-		return -1;
 	}
 
 	if (card->mp_wr_bitmap & (1 << card->curr_wr_port)) {
 		card->mp_wr_bitmap &= (u32) (~(1 << card->curr_wr_port));
 		*port = card->curr_wr_port;
-		if (((card->supports_sdio_new_mode) &&
-		     (++card->curr_wr_port == card->max_ports)) ||
-		    ((!card->supports_sdio_new_mode) &&
-		     (++card->curr_wr_port == card->mp_end_port)))
+		if (++card->curr_wr_port == card->mp_end_port)
 			card->curr_wr_port = reg->start_wr_port;
 	} else {
 		adapter->data_sent = true;
@@ -715,7 +715,7 @@ static void mwifiex_sdio_disable_host_int(struct mwifiex_adapter *adapter)
 	struct sdio_func *func = card->func;
 
 	sdio_claim_host(func);
-	mwifiex_write_reg_locked(func, HOST_INT_MASK_REG, 0);
+	mwifiex_write_reg_locked(func, card->reg->host_int_mask_reg, 0);
 	sdio_release_irq(func);
 	sdio_release_host(func);
 }
@@ -736,7 +736,7 @@ static void mwifiex_interrupt_status(struct mwifiex_adapter *adapter)
 		return;
 	}
 
-	sdio_ireg = card->mp_regs[HOST_INTSTATUS_REG];
+	sdio_ireg = card->mp_regs[card->reg->host_int_status_reg];
 	if (sdio_ireg) {
 		/*
 		 * DN_LD_HOST_INT_STATUS and/or UP_LD_HOST_INT_STATUS
@@ -801,7 +801,7 @@ static int mwifiex_sdio_enable_host_int(struct mwifiex_adapter *adapter)
 	}
 
 	/* Simply write the mask to the register */
-	ret = mwifiex_write_reg_locked(func, HOST_INT_MASK_REG,
+	ret = mwifiex_write_reg_locked(func, card->reg->host_int_mask_reg,
 				       card->reg->host_int_enable);
 	if (ret) {
 		dev_err(adapter->dev, "enable host interrupt failed\n");
@@ -1055,7 +1055,13 @@ static int mwifiex_decode_rx_packet(struct mwifiex_adapter *adapter,
 	switch (upld_typ) {
 	case MWIFIEX_TYPE_DATA:
 		dev_dbg(adapter->dev, "info: --- Rx: Data packet ---\n");
-		mwifiex_handle_rx_packet(adapter, skb);
+		if (adapter->rx_work_enabled) {
+			skb_queue_tail(&adapter->rx_data_q, skb);
+			adapter->data_received = true;
+			atomic_inc(&adapter->rx_pending);
+		} else {
+			mwifiex_handle_rx_packet(adapter, skb);
+		}
 		break;
 
 	case MWIFIEX_TYPE_CMD:
@@ -1335,8 +1341,8 @@ static int mwifiex_process_int_status(struct mwifiex_adapter *adapter)
 		u32 pkt_type;
 
 		/* read the len of control packet */
-		rx_len = card->mp_regs[CMD_RD_LEN_1] << 8;
-		rx_len |= (u16) card->mp_regs[CMD_RD_LEN_0];
+		rx_len = card->mp_regs[reg->cmd_rd_len_1] << 8;
+		rx_len |= (u16)card->mp_regs[reg->cmd_rd_len_0];
 		rx_blocks = DIV_ROUND_UP(rx_len, MWIFIEX_SDIO_BLOCK_SIZE);
 		if (rx_len <= INTF_HEADER_LEN ||
 		    (rx_blocks * MWIFIEX_SDIO_BLOCK_SIZE) >
@@ -1527,8 +1533,7 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 			__func__);
 
 		if (MP_TX_AGGR_IN_PROGRESS(card)) {
-			if (!mp_tx_aggr_port_limit_reached(card) &&
-			    MP_TX_AGGR_BUF_HAS_ROOM(card, pkt_len)) {
+			if (MP_TX_AGGR_BUF_HAS_ROOM(card, pkt_len)) {
 				f_precopy_cur_buf = 1;
 
 				if (!(card->mp_wr_bitmap &
@@ -1540,8 +1545,7 @@ static int mwifiex_host_to_card_mp_aggr(struct mwifiex_adapter *adapter,
 				/* No room in Aggr buf, send it */
 				f_send_aggr_buf = 1;
 
-				if (mp_tx_aggr_port_limit_reached(card) ||
-				    !(card->mp_wr_bitmap &
+				if (!(card->mp_wr_bitmap &
 				      (1 << card->curr_wr_port)))
 					f_send_cur_buf = 1;
 				else
@@ -1826,11 +1830,11 @@ static int mwifiex_init_sdio(struct mwifiex_adapter *adapter)
 	sdio_set_drvdata(card->func, card);
 
 	/*
-	 * Read the HOST_INT_STATUS_REG for ACK the first interrupt got
+	 * Read the host_int_status_reg for ACK the first interrupt got
 	 * from the bootloader. If we don't do this we get a interrupt
 	 * as soon as we register the irq.
 	 */
-	mwifiex_read_reg(adapter, HOST_INTSTATUS_REG, &sdio_ireg);
+	mwifiex_read_reg(adapter, card->reg->host_int_status_reg, &sdio_ireg);
 
 	/* Get SDIO ioport */
 	mwifiex_init_sdio_ioport(adapter);
@@ -2233,3 +2237,4 @@ MODULE_FIRMWARE(SD8786_DEFAULT_FW_NAME);
 MODULE_FIRMWARE(SD8787_DEFAULT_FW_NAME);
 MODULE_FIRMWARE(SD8797_DEFAULT_FW_NAME);
 MODULE_FIRMWARE(SD8897_DEFAULT_FW_NAME);
+MODULE_FIRMWARE(SD8887_DEFAULT_FW_NAME);
diff --git a/drivers/net/wireless/mwifiex/sdio.h b/drivers/net/wireless/mwifiex/sdio.h
index 6b8835ec88f1..20cd9adc98d3 100644
--- a/drivers/net/wireless/mwifiex/sdio.h
+++ b/drivers/net/wireless/mwifiex/sdio.h
@@ -33,6 +33,7 @@
 #define SD8787_DEFAULT_FW_NAME "mrvl/sd8787_uapsta.bin"
 #define SD8797_DEFAULT_FW_NAME "mrvl/sd8797_uapsta.bin"
 #define SD8897_DEFAULT_FW_NAME "mrvl/sd8897_uapsta.bin"
+#define SD8887_DEFAULT_FW_NAME "mrvl/sd8887_uapsta.bin"
 
 #define BLOCK_MODE	1
 #define BYTE_MODE	0
@@ -52,13 +53,9 @@
 #define HOST_TERM_CMD53			(0x1U << 2)
 #define REG_PORT			0
 #define MEM_PORT			0x10000
-#define CMD_RD_LEN_0			0xB4
-#define CMD_RD_LEN_1			0xB5
-#define CARD_CONFIG_2_1_REG             0xCD
+
 #define CMD53_NEW_MODE			(0x1U << 0)
-#define CMD_CONFIG_0			0xB8
 #define CMD_PORT_RD_LEN_EN		(0x1U << 2)
-#define CMD_CONFIG_1			0xB9
 #define CMD_PORT_AUTO_EN		(0x1U << 0)
 #define CMD_PORT_SLCT			0x8000
 #define UP_LD_CMD_PORT_HOST_INT_STATUS	(0x40U)
@@ -70,38 +67,23 @@
 /* Misc. Config Register : Auto Re-enable interrupts */
 #define AUTO_RE_ENABLE_INT              BIT(4)
 
-/* Host Control Registers */
-/* Host Control Registers : I/O port 0 */
-#define IO_PORT_0_REG			0x78
-/* Host Control Registers : I/O port 1 */
-#define IO_PORT_1_REG			0x79
-/* Host Control Registers : I/O port 2 */
-#define IO_PORT_2_REG			0x7A
-
 /* Host Control Registers : Configuration */
 #define CONFIGURATION_REG		0x00
 /* Host Control Registers : Host power up */
 #define HOST_POWER_UP			(0x1U << 1)
 
-/* Host Control Registers : Host interrupt mask */
-#define HOST_INT_MASK_REG		0x02
 /* Host Control Registers : Upload host interrupt mask */
 #define UP_LD_HOST_INT_MASK		(0x1U)
 /* Host Control Registers : Download host interrupt mask */
 #define DN_LD_HOST_INT_MASK		(0x2U)
 
-/* Host Control Registers : Host interrupt status */
-#define HOST_INTSTATUS_REG		0x03
 /* Host Control Registers : Upload host interrupt status */
 #define UP_LD_HOST_INT_STATUS		(0x1U)
 /* Host Control Registers : Download host interrupt status */
 #define DN_LD_HOST_INT_STATUS		(0x2U)
 
-/* Host Control Registers : Host interrupt RSR */
-#define HOST_INT_RSR_REG		0x01
-
 /* Host Control Registers : Host interrupt status */
-#define HOST_INT_STATUS_REG		0x28
+#define CARD_INT_STATUS_REG		0x28
 
 /* Card Control Registers : Card I/O ready */
 #define CARD_IO_READY                   (0x1U << 3)
@@ -203,10 +185,16 @@ struct mwifiex_sdio_card_reg {
 	u8 base_1_reg;
 	u8 poll_reg;
 	u8 host_int_enable;
+	u8 host_int_rsr_reg;
+	u8 host_int_status_reg;
+	u8 host_int_mask_reg;
 	u8 status_reg_0;
 	u8 status_reg_1;
 	u8 sdio_int_mask;
 	u32 data_port_mask;
+	u8 io_port_0_reg;
+	u8 io_port_1_reg;
+	u8 io_port_2_reg;
 	u8 max_mp_regs;
 	u8 rd_bitmap_l;
 	u8 rd_bitmap_u;
@@ -219,6 +207,15 @@ struct mwifiex_sdio_card_reg {
 	u8 rd_len_p0_l;
 	u8 rd_len_p0_u;
 	u8 card_misc_cfg_reg;
+	u8 card_cfg_2_1_reg;
+	u8 cmd_rd_len_0;
+	u8 cmd_rd_len_1;
+	u8 cmd_rd_len_2;
+	u8 cmd_rd_len_3;
+	u8 cmd_cfg_0;
+	u8 cmd_cfg_1;
+	u8 cmd_cfg_2;
+	u8 cmd_cfg_3;
 	u8 fw_dump_ctrl;
 	u8 fw_dump_start;
 	u8 fw_dump_end;
@@ -274,10 +271,16 @@ static const struct mwifiex_sdio_card_reg mwifiex_reg_sd87xx = {
 	.base_1_reg = 0x0041,
 	.poll_reg = 0x30,
 	.host_int_enable = UP_LD_HOST_INT_MASK | DN_LD_HOST_INT_MASK,
+	.host_int_rsr_reg = 0x1,
+	.host_int_mask_reg = 0x02,
+	.host_int_status_reg = 0x03,
 	.status_reg_0 = 0x60,
 	.status_reg_1 = 0x61,
 	.sdio_int_mask = 0x3f,
 	.data_port_mask = 0x0000fffe,
+	.io_port_0_reg = 0x78,
+	.io_port_1_reg = 0x79,
+	.io_port_2_reg = 0x7A,
 	.max_mp_regs = 64,
 	.rd_bitmap_l = 0x04,
 	.rd_bitmap_u = 0x05,
@@ -296,10 +299,16 @@ static const struct mwifiex_sdio_card_reg mwifiex_reg_sd8897 = {
 	.poll_reg = 0x50,
 	.host_int_enable = UP_LD_HOST_INT_MASK | DN_LD_HOST_INT_MASK |
 			CMD_PORT_UPLD_INT_MASK | CMD_PORT_DNLD_INT_MASK,
+	.host_int_rsr_reg = 0x1,
+	.host_int_status_reg = 0x03,
+	.host_int_mask_reg = 0x02,
 	.status_reg_0 = 0xc0,
 	.status_reg_1 = 0xc1,
 	.sdio_int_mask = 0xff,
 	.data_port_mask = 0xffffffff,
+	.io_port_0_reg = 0xD8,
+	.io_port_1_reg = 0xD9,
+	.io_port_2_reg = 0xDA,
 	.max_mp_regs = 184,
 	.rd_bitmap_l = 0x04,
 	.rd_bitmap_u = 0x05,
@@ -312,11 +321,61 @@ static const struct mwifiex_sdio_card_reg mwifiex_reg_sd8897 = {
 	.rd_len_p0_l = 0x0c,
 	.rd_len_p0_u = 0x0d,
 	.card_misc_cfg_reg = 0xcc,
+	.card_cfg_2_1_reg = 0xcd,
+	.cmd_rd_len_0 = 0xb4,
+	.cmd_rd_len_1 = 0xb5,
+	.cmd_rd_len_2 = 0xb6,
+	.cmd_rd_len_3 = 0xb7,
+	.cmd_cfg_0 = 0xb8,
+	.cmd_cfg_1 = 0xb9,
+	.cmd_cfg_2 = 0xba,
+	.cmd_cfg_3 = 0xbb,
 	.fw_dump_ctrl = 0xe2,
 	.fw_dump_start = 0xe3,
 	.fw_dump_end = 0xea,
 };
 
+static const struct mwifiex_sdio_card_reg mwifiex_reg_sd8887 = {
+	.start_rd_port = 0,
+	.start_wr_port = 0,
+	.base_0_reg = 0x6C,
+	.base_1_reg = 0x6D,
+	.poll_reg = 0x5C,
+	.host_int_enable = UP_LD_HOST_INT_MASK | DN_LD_HOST_INT_MASK |
+			CMD_PORT_UPLD_INT_MASK | CMD_PORT_DNLD_INT_MASK,
+	.host_int_rsr_reg = 0x4,
+	.host_int_status_reg = 0x0C,
+	.host_int_mask_reg = 0x08,
+	.status_reg_0 = 0x90,
+	.status_reg_1 = 0x91,
+	.sdio_int_mask = 0xff,
+	.data_port_mask = 0xffffffff,
+	.io_port_0_reg = 0xE4,
+	.io_port_1_reg = 0xE5,
+	.io_port_2_reg = 0xE6,
+	.max_mp_regs = 196,
+	.rd_bitmap_l = 0x10,
+	.rd_bitmap_u = 0x11,
+	.rd_bitmap_1l = 0x12,
+	.rd_bitmap_1u = 0x13,
+	.wr_bitmap_l = 0x14,
+	.wr_bitmap_u = 0x15,
+	.wr_bitmap_1l = 0x16,
+	.wr_bitmap_1u = 0x17,
+	.rd_len_p0_l = 0x18,
+	.rd_len_p0_u = 0x19,
+	.card_misc_cfg_reg = 0xd8,
+	.card_cfg_2_1_reg = 0xd9,
+	.cmd_rd_len_0 = 0xc0,
+	.cmd_rd_len_1 = 0xc1,
+	.cmd_rd_len_2 = 0xc2,
+	.cmd_rd_len_3 = 0xc3,
+	.cmd_cfg_0 = 0xc4,
+	.cmd_cfg_1 = 0xc5,
+	.cmd_cfg_2 = 0xc6,
+	.cmd_cfg_3 = 0xc7,
+};
+
 static const struct mwifiex_sdio_device mwifiex_sdio_sd8786 = {
 	.firmware = SD8786_DEFAULT_FW_NAME,
 	.reg = &mwifiex_reg_sd87xx,
@@ -369,6 +428,19 @@ static const struct mwifiex_sdio_device mwifiex_sdio_sd8897 = {
 	.supports_fw_dump = true,
 };
 
+static const struct mwifiex_sdio_device mwifiex_sdio_sd8887 = {
+	.firmware = SD8887_DEFAULT_FW_NAME,
+	.reg = &mwifiex_reg_sd8887,
+	.max_ports = 32,
+	.mp_agg_pkt_limit = 16,
+	.supports_sdio_new_mode = true,
+	.has_control_mask = false,
+	.tx_buf_size = MWIFIEX_TX_DATA_BUF_SIZE_4K,
+	.mp_tx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_32K,
+	.mp_rx_agg_buf_size = MWIFIEX_MP_AGGR_BUF_SIZE_32K,
+	.supports_fw_dump = false,
+};
+
 /*
  * .cmdrsp_complete handler
  */
diff --git a/drivers/net/wireless/mwifiex/sta_cmd.c b/drivers/net/wireless/mwifiex/sta_cmd.c
index 733de92a4c61..1c2ca291d1f5 100644
--- a/drivers/net/wireless/mwifiex/sta_cmd.c
+++ b/drivers/net/wireless/mwifiex/sta_cmd.c
@@ -938,7 +938,7 @@ mwifiex_cmd_802_11_key_material_v1(struct mwifiex_private *priv,
 		cmd->size = cpu_to_le16(sizeof(key_material->action) + S_DS_GEN
 					+ key_param_len);
 
-		if (priv->bss_type == MWIFIEX_BSS_TYPE_UAP) {
+		if (GET_BSS_ROLE(priv) == MWIFIEX_BSS_ROLE_UAP) {
 			tlv_mac = (void *)((u8 *)&key_material->key_param_set +
 					   key_param_len);
 			tlv_mac->header.type =
@@ -965,7 +965,7 @@ mwifiex_cmd_802_11_key_material(struct mwifiex_private *priv,
 				u16 cmd_action, u32 cmd_oid,
 				struct mwifiex_ds_encrypt_key *enc_key)
 {
-	if (priv->adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2)
+	if (priv->adapter->key_api_major_ver == KEY_API_VER_MAJOR_V2)
 		return mwifiex_cmd_802_11_key_material_v2(priv, cmd,
 							  cmd_action, cmd_oid,
 							  enc_key);
diff --git a/drivers/net/wireless/mwifiex/sta_cmdresp.c b/drivers/net/wireless/mwifiex/sta_cmdresp.c
index 08b78baeb846..4aad44685f8d 100644
--- a/drivers/net/wireless/mwifiex/sta_cmdresp.c
+++ b/drivers/net/wireless/mwifiex/sta_cmdresp.c
@@ -85,8 +85,6 @@ mwifiex_process_cmdresp_error(struct mwifiex_private *priv,
 		spin_lock_irqsave(&adapter->mwifiex_cmd_lock, flags);
 		adapter->scan_processing = false;
 		spin_unlock_irqrestore(&adapter->mwifiex_cmd_lock, flags);
-		if (priv->report_scan_result)
-			priv->report_scan_result = false;
 		break;
 
 	case HostCmd_CMD_MAC_CONTROL:
@@ -637,7 +635,7 @@ static int mwifiex_ret_802_11_key_material_v2(struct mwifiex_private *priv,
 static int mwifiex_ret_802_11_key_material(struct mwifiex_private *priv,
 					   struct host_cmd_ds_command *resp)
 {
-	if (priv->adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2)
+	if (priv->adapter->key_api_major_ver == KEY_API_VER_MAJOR_V2)
 		return mwifiex_ret_802_11_key_material_v2(priv, resp);
 	else
 		return mwifiex_ret_802_11_key_material_v1(priv, resp);
diff --git a/drivers/net/wireless/mwifiex/sta_ioctl.c b/drivers/net/wireless/mwifiex/sta_ioctl.c
index caae9738100a..92f3eb839866 100644
--- a/drivers/net/wireless/mwifiex/sta_ioctl.c
+++ b/drivers/net/wireless/mwifiex/sta_ioctl.c
@@ -287,10 +287,13 @@ int mwifiex_bss_start(struct mwifiex_private *priv, struct cfg80211_bss *bss,
 			return -1;
 
 		if (mwifiex_band_to_radio_type(bss_desc->bss_band) ==
-						HostCmd_SCAN_RADIO_TYPE_BG)
+						HostCmd_SCAN_RADIO_TYPE_BG) {
 			config_bands = BAND_B | BAND_G | BAND_GN;
-		else
-			config_bands = BAND_A | BAND_AN | BAND_AAC;
+		} else {
+			config_bands = BAND_A | BAND_AN;
+			if (adapter->fw_bands & BAND_AAC)
+				config_bands |= BAND_AAC;
+		}
 
 		if (!((config_bands | adapter->fw_bands) & ~adapter->fw_bands))
 			adapter->config_bands = config_bands;
@@ -877,7 +880,7 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 			return -1;
 		}
 
-		if (adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2) {
+		if (adapter->key_api_major_ver == KEY_API_VER_MAJOR_V2) {
 			memcpy(encrypt_key->key_material,
 			       wep_key->key_material, wep_key->key_length);
 			encrypt_key->key_len = wep_key->key_length;
@@ -903,7 +906,7 @@ static int mwifiex_sec_ioctl_set_wep_key(struct mwifiex_private *priv,
 			memset(&priv->wep_key[index], 0,
 			       sizeof(struct mwifiex_wep_key));
 
-		if (adapter->fw_key_api_major_ver == FW_KEY_API_VER_MAJOR_V2)
+		if (adapter->key_api_major_ver == KEY_API_VER_MAJOR_V2)
 			enc_key = encrypt_key;
 		else
 			enc_key = NULL;
diff --git a/drivers/net/wireless/mwifiex/tdls.c b/drivers/net/wireless/mwifiex/tdls.c
index 4c5fd953893d..e2949077f5b5 100644
--- a/drivers/net/wireless/mwifiex/tdls.c
+++ b/drivers/net/wireless/mwifiex/tdls.c
@@ -871,7 +871,9 @@ void mwifiex_process_tdls_action_frame(struct mwifiex_private *priv,
 			break;
 		case WLAN_EID_RSN:
 			memcpy((u8 *)&sta_ptr->tdls_cap.rsn_ie, pos,
-			       sizeof(struct ieee_types_header) + pos[1]);
+			       sizeof(struct ieee_types_header) +
+			       min_t(u8, pos[1], IEEE_MAX_IE_SIZE -
+				     sizeof(struct ieee_types_header)));
 			break;
 		case WLAN_EID_QOS_CAPA:
 			sta_ptr->tdls_cap.qos_info = pos[2];
diff --git a/drivers/net/wireless/mwifiex/usb.c b/drivers/net/wireless/mwifiex/usb.c
index 7118a18b91ba..4371e12b36f3 100644
--- a/drivers/net/wireless/mwifiex/usb.c
+++ b/drivers/net/wireless/mwifiex/usb.c
@@ -357,7 +357,7 @@ static int mwifiex_usb_probe(struct usb_interface *intf,
 		card->usb_boot_state = USB8XXX_FW_READY;
 		break;
 	default:
-		pr_warning("unknown id_product %#x\n", id_product);
+		pr_warn("unknown id_product %#x\n", id_product);
 		card->usb_boot_state = USB8XXX_FW_DNLD;
 		break;
 	}
diff --git a/drivers/net/wireless/mwifiex/util.c b/drivers/net/wireless/mwifiex/util.c
index cee028321a9a..ec79c49de097 100644
--- a/drivers/net/wireless/mwifiex/util.c
+++ b/drivers/net/wireless/mwifiex/util.c
@@ -172,7 +172,7 @@ mwifiex_process_mgmt_packet(struct mwifiex_private *priv,
 
 	cfg80211_rx_mgmt(priv->wdev, priv->roc_cfg.chan.center_freq,
 			 CAL_RSSI(rx_pd->snr, rx_pd->nf), skb->data, pkt_len,
-			 0, GFP_ATOMIC);
+			 0);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/orinoco/orinoco_usb.c b/drivers/net/wireless/orinoco/orinoco_usb.c
index d3cf7c3ebfd6..995846422dc0 100644
--- a/drivers/net/wireless/orinoco/orinoco_usb.c
+++ b/drivers/net/wireless/orinoco/orinoco_usb.c
@@ -534,7 +534,7 @@ static void ezusb_request_out_callback(struct urb *urb)
 
 	if (ctx->killed) {
 		spin_unlock_irqrestore(&upriv->req_lock, flags);
-		pr_warning("interrupt called with dead ctx");
+		pr_warn("interrupt called with dead ctx\n");
 		goto out;
 	}
 
@@ -671,8 +671,8 @@ static void ezusb_request_in_callback(struct ezusb_priv *upriv,
 	default:
 		spin_unlock_irqrestore(&upriv->req_lock, flags);
 
-		pr_warning("Matched IN URB, unexpected context state(0x%x)",
-		     state);
+		pr_warn("Matched IN URB, unexpected context state(0x%x)\n",
+			state);
 		/* Throw this CTX away and try submitting another */
 		del_timer(&ctx->timer);
 		ctx->outurb->transfer_flags |= URB_ASYNC_UNLINK;
@@ -1394,12 +1394,12 @@ static void ezusb_bulk_in_callback(struct urb *urb)
 		/* When a device gets unplugged we get this every time
 		 * we resubmit, flooding the logs.  Since we don't use
 		 * USB timeouts, it shouldn't happen any other time*/
-		pr_warning("%s: urb timed out, not resubmiting", __func__);
+		pr_warn("%s: urb timed out, not resubmitting\n", __func__);
 		return;
 	}
 	if (urb->status == -ECONNABORTED) {
-		pr_warning("%s: connection abort, resubmiting urb",
-		     __func__);
+		pr_warn("%s: connection abort, resubmitting urb\n",
+			__func__);
 		goto resubmit;
 	}
 	if ((urb->status == -EILSEQ)
@@ -1605,13 +1605,10 @@ static int ezusb_probe(struct usb_interface *interface,
 	for (i = 0; i < iface_desc->bNumEndpoints; ++i) {
 		ep = &interface->altsetting[0].endpoint[i].desc;
 
-		if (((ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK)
-		     == USB_DIR_IN) &&
-		    ((ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-		     == USB_ENDPOINT_XFER_BULK)) {
+		if (usb_endpoint_is_bulk_in(ep)) {
 			/* we found a bulk in endpoint */
 			if (upriv->read_urb != NULL) {
-				pr_warning("Found a second bulk in ep, ignored");
+				pr_warn("Found a second bulk in ep, ignored\n");
 				continue;
 			}
 
@@ -1621,10 +1618,10 @@ static int ezusb_probe(struct usb_interface *interface,
 				goto error;
 			}
 			if (le16_to_cpu(ep->wMaxPacketSize) != 64)
-				pr_warning("bulk in: wMaxPacketSize!= 64");
+				pr_warn("bulk in: wMaxPacketSize!= 64\n");
 			if (ep->bEndpointAddress != (2 | USB_DIR_IN))
-				pr_warning("bulk in: bEndpointAddress: %d",
-				     ep->bEndpointAddress);
+				pr_warn("bulk in: bEndpointAddress: %d\n",
+					ep->bEndpointAddress);
 			upriv->read_pipe = usb_rcvbulkpipe(udev,
 							 ep->
 							 bEndpointAddress);
@@ -1636,21 +1633,18 @@ static int ezusb_probe(struct usb_interface *interface,
 			}
 		}
 
-		if (((ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK)
-		     == USB_DIR_OUT) &&
-		    ((ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK)
-		     == USB_ENDPOINT_XFER_BULK)) {
+		if (usb_endpoint_is_bulk_out(ep)) {
 			/* we found a bulk out endpoint */
 			if (upriv->bap_buf != NULL) {
-				pr_warning("Found a second bulk out ep, ignored");
+				pr_warn("Found a second bulk out ep, ignored\n");
 				continue;
 			}
 
 			if (le16_to_cpu(ep->wMaxPacketSize) != 64)
-				pr_warning("bulk out: wMaxPacketSize != 64");
+				pr_warn("bulk out: wMaxPacketSize != 64\n");
 			if (ep->bEndpointAddress != 2)
-				pr_warning("bulk out: bEndpointAddress: %d",
-				     ep->bEndpointAddress);
+				pr_warn("bulk out: bEndpointAddress: %d\n",
+					ep->bEndpointAddress);
 			upriv->write_pipe = usb_sndbulkpipe(udev,
 							  ep->
 							  bEndpointAddress);
diff --git a/drivers/net/wireless/orinoco/scan.c b/drivers/net/wireless/orinoco/scan.c
index e175b9b8561b..2c66166add70 100644
--- a/drivers/net/wireless/orinoco/scan.c
+++ b/drivers/net/wireless/orinoco/scan.c
@@ -123,9 +123,10 @@ static void orinoco_add_hostscan_result(struct orinoco_private *priv,
 	beacon_interval = le16_to_cpu(bss->a.beacon_interv);
 	signal = SIGNAL_TO_MBM(le16_to_cpu(bss->a.level));
 
-	cbss = cfg80211_inform_bss(wiphy, channel, bss->a.bssid, timestamp,
-				   capability, beacon_interval, ie_buf, ie_len,
-				   signal, GFP_KERNEL);
+	cbss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN,
+				   bss->a.bssid, timestamp, capability,
+				   beacon_interval, ie_buf, ie_len, signal,
+				   GFP_KERNEL);
 	cfg80211_put_bss(wiphy, cbss);
 }
 
@@ -156,9 +157,10 @@ void orinoco_add_extscan_result(struct orinoco_private *priv,
 	ie = bss->data;
 	signal = SIGNAL_TO_MBM(bss->level);
 
-	cbss = cfg80211_inform_bss(wiphy, channel, bss->bssid, timestamp,
-				   capability, beacon_interval, ie, ie_len,
-				   signal, GFP_KERNEL);
+	cbss = cfg80211_inform_bss(wiphy, channel, CFG80211_BSS_FTYPE_UNKNOWN,
+				   bss->bssid, timestamp, capability,
+				   beacon_interval, ie, ie_len, signal,
+				   GFP_KERNEL);
 	cfg80211_put_bss(wiphy, cbss);
 }
 
diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/p54/main.c
index 7be3a4839640..97aeff0edb84 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/p54/main.c
@@ -696,7 +696,8 @@ static void p54_flush(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
 	WARN(total, "tx flush timeout, unresponsive firmware");
 }
 
-static void p54_set_coverage_class(struct ieee80211_hw *dev, u8 coverage_class)
+static void p54_set_coverage_class(struct ieee80211_hw *dev,
+				   s16 coverage_class)
 {
 	struct p54_common *priv = dev->priv;
 
diff --git a/drivers/net/wireless/ray_cs.h b/drivers/net/wireless/ray_cs.h
index e79848fbcca1..524c2f02dd82 100644
--- a/drivers/net/wireless/ray_cs.h
+++ b/drivers/net/wireless/ray_cs.h
@@ -3,7 +3,8 @@
    Written by Corey Thomas
 */
 
-#ifndef RAYLINK_H
+#ifndef _RAY_CS_H_
+#define _RAY_CS_H_
 
 struct beacon_rx {
     struct mac_header mac;
@@ -69,4 +70,4 @@ typedef struct ray_dev_t {
 } ray_dev_t;
 /*****************************************************************************/
 
-#endif /* RAYLINK_H */
+#endif /* _RAY_CS_H_ */
diff --git a/drivers/net/wireless/rayctl.h b/drivers/net/wireless/rayctl.h
index 3c3b98b152c3..b21ed64e15df 100644
--- a/drivers/net/wireless/rayctl.h
+++ b/drivers/net/wireless/rayctl.h
@@ -1,4 +1,5 @@
-#ifndef RAYLINK_H
+#ifndef _RAYCTL_H_
+#define _RAYCTL_H_
 
 typedef unsigned char UCHAR;
 
@@ -729,4 +730,4 @@ typedef struct snaphdr_t
 #define RAY_IPX_TYPE  0x8137
 #define APPLEARP_TYPE 0x80f3
 /*****************************************************************************/
-#endif /* #ifndef RAYLINK_H */
+#endif /* _RAYCTL_H_ */
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index d2a9a08210be..1a4facd1fbf3 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -2022,9 +2022,10 @@ static bool rndis_bss_info_update(struct usbnet *usbdev,
 	capability = le16_to_cpu(fixed->capabilities);
 	beacon_interval = le16_to_cpu(fixed->beacon_interval);
 
-	bss = cfg80211_inform_bss(priv->wdev.wiphy, channel, bssid->mac,
-		timestamp, capability, beacon_interval, ie, ie_len, signal,
-		GFP_KERNEL);
+	bss = cfg80211_inform_bss(priv->wdev.wiphy, channel,
+				  CFG80211_BSS_FTYPE_UNKNOWN, bssid->mac,
+				  timestamp, capability, beacon_interval,
+				  ie, ie_len, signal, GFP_KERNEL);
 	cfg80211_put_bss(priv->wdev.wiphy, bss);
 
 	return (bss != NULL);
@@ -2711,9 +2712,10 @@ static void rndis_wlan_craft_connected_bss(struct usbnet *usbdev, u8 *bssid,
 		bssid, (u32)timestamp, capability, beacon_period, ie_len,
 		ssid.essid, signal);
 
-	bss = cfg80211_inform_bss(priv->wdev.wiphy, channel, bssid,
-		timestamp, capability, beacon_period, ie_buf, ie_len,
-		signal, GFP_KERNEL);
+	bss = cfg80211_inform_bss(priv->wdev.wiphy, channel,
+				  CFG80211_BSS_FTYPE_UNKNOWN, bssid,
+				  timestamp, capability, beacon_period,
+				  ie_buf, ie_len, signal, GFP_KERNEL);
 	cfg80211_put_bss(priv->wdev.wiphy, bss);
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index a394a9a95919..ebd5625d13f1 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -52,6 +52,7 @@
  * RF5592 2.4G/5G 2T2R
  * RF3070 2.4G 1T1R
  * RF5360 2.4G 1T1R
+ * RF5362 2.4G 1T1R
  * RF5370 2.4G 1T1R
  * RF5390 2.4G 1T1R
  */
@@ -72,6 +73,7 @@
 #define RF3070				0x3070
 #define RF3290				0x3290
 #define RF5360				0x5360
+#define RF5362				0x5362
 #define RF5370				0x5370
 #define RF5372				0x5372
 #define RF5390				0x5390
@@ -2039,7 +2041,7 @@ struct mac_iveiv_entry {
  * 2 - drop tx power by 12dBm,
  * 3 - increase tx power by 6dBm
  */
-#define BBP1_TX_POWER_CTRL		FIELD8(0x07)
+#define BBP1_TX_POWER_CTRL		FIELD8(0x03)
 #define BBP1_TX_ANTENNA			FIELD8(0x18)
 
 /*
@@ -2145,7 +2147,7 @@ struct mac_iveiv_entry {
 /* Bits [7-4] for RF3320 (RT3370/RT3390), on other chipsets reserved */
 #define RFCSR3_PA1_BIAS_CCK		FIELD8(0x70)
 #define RFCSR3_PA2_CASCODE_BIAS_CCKK	FIELD8(0x80)
-/* Bits for RF3290/RF5360/RF5370/RF5372/RF5390/RF5392 */
+/* Bits for RF3290/RF5360/RF5362/RF5370/RF5372/RF5390/RF5392 */
 #define RFCSR3_VCOCAL_EN		FIELD8(0x80)
 /* Bits for RF3050 */
 #define RFCSR3_BIT1			FIELD8(0x02)
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 893c9d5f3d6f..9f57a2db791c 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -3186,6 +3186,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 		break;
 	case RF3070:
 	case RF5360:
+	case RF5362:
 	case RF5370:
 	case RF5372:
 	case RF5390:
@@ -3203,6 +3204,7 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	    rt2x00_rf(rt2x00dev, RF3290) ||
 	    rt2x00_rf(rt2x00dev, RF3322) ||
 	    rt2x00_rf(rt2x00dev, RF5360) ||
+	    rt2x00_rf(rt2x00dev, RF5362) ||
 	    rt2x00_rf(rt2x00dev, RF5370) ||
 	    rt2x00_rf(rt2x00dev, RF5372) ||
 	    rt2x00_rf(rt2x00dev, RF5390) ||
@@ -4317,6 +4319,7 @@ void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
 	case RF3070:
 	case RF3290:
 	case RF5360:
+	case RF5362:
 	case RF5370:
 	case RF5372:
 	case RF5390:
@@ -7095,6 +7098,7 @@ static int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	case RF3320:
 	case RF3322:
 	case RF5360:
+	case RF5362:
 	case RF5370:
 	case RF5372:
 	case RF5390:
@@ -7551,6 +7555,7 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	case RF3320:
 	case RF3322:
 	case RF5360:
+	case RF5362:
 	case RF5370:
 	case RF5372:
 	case RF5390:
@@ -7680,6 +7685,7 @@ static int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	case RF3070:
 	case RF3290:
 	case RF5360:
+	case RF5362:
 	case RF5370:
 	case RF5372:
 	case RF5390:
diff --git a/drivers/net/wireless/rtl818x/rtl8180/dev.c b/drivers/net/wireless/rtl818x/rtl8180/dev.c
index 026d912f516b..706b844bce00 100644
--- a/drivers/net/wireless/rtl818x/rtl8180/dev.c
+++ b/drivers/net/wireless/rtl818x/rtl8180/dev.c
@@ -189,6 +189,9 @@ static const int rtl8187se_queues_map[RTL8187SE_NR_TX_QUEUES] = {5, 4, 3, 2, 7};
 
 static const int rtl8180_queues_map[RTL8180_NR_TX_QUEUES] = {4, 7};
 
+/* LNA gain table for rtl8187se */
+static const u8 rtl8187se_lna_gain[4] = {02, 17, 29, 39};
+
 void rtl8180_write_phy(struct ieee80211_hw *dev, u8 addr, u32 data)
 {
 	struct rtl8180_priv *priv = dev->priv;
@@ -210,13 +213,14 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 	struct rtl8180_priv *priv = dev->priv;
 	struct rtl818x_rx_cmd_desc *cmd_desc;
 	unsigned int count = 32;
-	u8 agc, sq, signal = 1;
+	u8 agc, sq;
+	s8 signal = 1;
 	dma_addr_t mapping;
 
 	while (count--) {
 		void *entry = priv->rx_ring + priv->rx_idx * priv->rx_ring_sz;
 		struct sk_buff *skb = priv->rx_buf[priv->rx_idx];
-		u32 flags, flags2;
+		u32 flags, flags2, flags3 = 0;
 		u64 tsft;
 
 		if (priv->chip_family == RTL818X_CHIP_FAMILY_RTL8187SE) {
@@ -229,6 +233,7 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 			 * the ownership flag
 			 */
 			rmb();
+			flags3 = le32_to_cpu(desc->flags3);
 			flags2 = le32_to_cpu(desc->flags2);
 			tsft = le64_to_cpu(desc->tsft);
 		} else {
@@ -287,8 +292,21 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 				signal = priv->rf->calc_rssi(agc, sq);
 				break;
 			case RTL818X_CHIP_FAMILY_RTL8187SE:
-				/* TODO: rtl8187se rssi */
-				signal = 10;
+				/* OFDM measure reported by HW is signed,
+				 * in 0.5dBm unit, with zero centered @ -41dBm
+				 * input signal.
+				 */
+				if (rx_status.rate_idx > 3) {
+					signal = (s8)((flags3 >> 16) & 0xff);
+					signal = signal / 2 - 41;
+				} else {
+					int idx, bb;
+
+					idx = (agc & 0x60) >> 5;
+					bb = (agc & 0x1F) * 2;
+					/* bias + BB gain + LNA gain */
+					signal = 4 - bb - rtl8187se_lna_gain[idx];
+				}
 				break;
 			}
 			rx_status.signal = signal;
@@ -724,35 +742,49 @@ static void rtl8180_int_disable(struct ieee80211_hw *dev)
 }
 
 static void rtl8180_conf_basic_rates(struct ieee80211_hw *dev,
-			    u32 rates_mask)
+			    u32 basic_mask)
 {
 	struct rtl8180_priv *priv = dev->priv;
-
-	u8 max, min;
 	u16 reg;
-
-	max = fls(rates_mask) - 1;
-	min = ffs(rates_mask) - 1;
+	u32 resp_mask;
+	u8 basic_max;
+	u8 resp_max, resp_min;
+
+	resp_mask = basic_mask;
+	/* IEEE80211 says the response rate should be equal to the highest basic
+	 * rate that is not faster than received frame. But it says also that if
+	 * the basic rate set does not contains any rate for the current
+	 * modulation class then mandatory rate set must be used for that
+	 * modulation class. Eventually add OFDM mandatory rates..
+	 */
+	if ((resp_mask & 0xf) == resp_mask)
+		resp_mask |= 0x150; /* 6, 12, 24Mbps */
 
 	switch (priv->chip_family) {
 
 	case RTL818X_CHIP_FAMILY_RTL8180:
 		/* in 8180 this is NOT a BITMAP */
+		basic_max = fls(basic_mask) - 1;
 		reg = rtl818x_ioread16(priv, &priv->map->BRSR);
 		reg &= ~3;
-		reg |= max;
+		reg |= basic_max;
 		rtl818x_iowrite16(priv, &priv->map->BRSR, reg);
 		break;
 
 	case RTL818X_CHIP_FAMILY_RTL8185:
+		resp_max = fls(resp_mask) - 1;
+		resp_min = ffs(resp_mask) - 1;
 		/* in 8185 this is a BITMAP */
-		rtl818x_iowrite16(priv, &priv->map->BRSR, rates_mask);
-		rtl818x_iowrite8(priv, &priv->map->RESP_RATE, (max << 4) | min);
+		rtl818x_iowrite16(priv, &priv->map->BRSR, basic_mask);
+		rtl818x_iowrite8(priv, &priv->map->RESP_RATE, (resp_max << 4) |
+				resp_min);
 		break;
 
 	case RTL818X_CHIP_FAMILY_RTL8187SE:
-		/* in 8187se this is a BITMAP */
-		rtl818x_iowrite16(priv, &priv->map->BRSR_8187SE, rates_mask);
+		/* in 8187se this is a BITMAP. BRSR reg actually sets
+		 * response rates.
+		 */
+		rtl818x_iowrite16(priv, &priv->map->BRSR_8187SE, resp_mask);
 		break;
 	}
 }
@@ -1835,7 +1867,7 @@ static int rtl8180_probe(struct pci_dev *pdev,
 		pci_try_set_mwi(pdev);
 	}
 
-	if (priv->chip_family == RTL818X_CHIP_FAMILY_RTL8185)
+	if (priv->chip_family != RTL818X_CHIP_FAMILY_RTL8180)
 		dev->flags |= IEEE80211_HW_SIGNAL_DBM;
 	else
 		dev->flags |= IEEE80211_HW_SIGNAL_UNSPEC;
diff --git a/drivers/net/wireless/rtlwifi/Kconfig b/drivers/net/wireless/rtlwifi/Kconfig
index bf3cf124e4ea..5cf509d346e8 100644
--- a/drivers/net/wireless/rtlwifi/Kconfig
+++ b/drivers/net/wireless/rtlwifi/Kconfig
@@ -5,7 +5,8 @@ menuconfig RTL_CARDS
 	---help---
 	  This option will enable support for the Realtek mac80211-based
 	  wireless drivers. Drivers rtl8192ce, rtl8192cu, rtl8192se, rtl8192de,
-	  rtl8723ae, rtl8723be, and rtl8188ae share some common code.
+	  rtl8723ae, rtl8723be, rtl8188ee, rtl8192ee, and rtl8821ae share
+	  some common code.
 
 if RTL_CARDS
 
@@ -80,6 +81,30 @@ config RTL8188EE
 
 	If you choose to build it as a module, it will be called rtl8188ee
 
+config RTL8192EE
+	tristate "Realtek RTL8192EE Wireless Network Adapter"
+	depends on PCI
+	select RTLWIFI
+	select RTLWIFI_PCI
+	select RTLBTCOEXIST
+	---help---
+	This is the driver for Realtek RTL8192EE 802.11n PCIe
+	wireless network adapters.
+
+	If you choose to build it as a module, it will be called rtl8192ee
+
+config RTL8821AE
+	tristate "Realtek RTL8821AE/RTL8812AE Wireless Network Adapter"
+	depends on PCI
+	select RTLWIFI
+	select RTLWIFI_PCI
+	select RTLBTCOEXIST
+	---help---
+	This is the driver for Realtek RTL8i821AE/RTL8812AE 802.11av PCIe
+	wireless network adapters.
+
+	If you choose to build it as a module, it will be called rtl8821ae
+
 config RTL8192CU
 	tristate "Realtek RTL8192CU/RTL8188CU USB Wireless Network Adapter"
 	depends on USB
@@ -123,7 +148,7 @@ config RTL8723_COMMON
 
 config RTLBTCOEXIST
 	tristate
-	depends on RTL8723AE || RTL8723BE
+	depends on RTL8723AE || RTL8723BE || RTL8821AE || RTL8192EE
 	default y
 
 endif
diff --git a/drivers/net/wireless/rtlwifi/Makefile b/drivers/net/wireless/rtlwifi/Makefile
index bba36a06abcc..ad6d3c52ec57 100644
--- a/drivers/net/wireless/rtlwifi/Makefile
+++ b/drivers/net/wireless/rtlwifi/Makefile
@@ -28,5 +28,7 @@ obj-$(CONFIG_RTL8723BE)		+= rtl8723be/
 obj-$(CONFIG_RTL8188EE)		+= rtl8188ee/
 obj-$(CONFIG_RTLBTCOEXIST)	+= btcoexist/
 obj-$(CONFIG_RTL8723_COMMON)	+= rtl8723com/
+obj-$(CONFIG_RTL8821AE)		+= rtl8821ae/
+obj-$(CONFIG_RTL8192EE)		+= rtl8192ee/
 
 ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/net/wireless/rtlwifi/base.c b/drivers/net/wireless/rtlwifi/base.c
index 93bb384eb001..58ba71830886 100644
--- a/drivers/net/wireless/rtlwifi/base.c
+++ b/drivers/net/wireless/rtlwifi/base.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -34,7 +30,7 @@
 #include "cam.h"
 #include "ps.h"
 #include "regd.h"
-
+#include "pci.h"
 #include <linux/ip.h>
 #include <linux/module.h>
 #include <linux/udp.h>
@@ -211,7 +207,6 @@ static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
 	 *highest supported RX rate
 	 */
 	if (rtlpriv->dm.supp_phymode_switch) {
-
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
 			 "Support phy mode switch\n");
 
@@ -244,6 +239,83 @@ static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
 	}
 }
 
+static void _rtl_init_hw_vht_capab(struct ieee80211_hw *hw,
+				   struct ieee80211_sta_vht_cap *vht_cap)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		u16 mcs_map;
+
+		vht_cap->vht_supported = true;
+		vht_cap->cap =
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
+			IEEE80211_VHT_CAP_SHORT_GI_80 |
+			IEEE80211_VHT_CAP_TXSTBC |
+			IEEE80211_VHT_CAP_RXSTBC_1 |
+			IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
+			IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+			IEEE80211_VHT_CAP_HTC_VHT |
+			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK |
+			IEEE80211_VHT_CAP_RX_ANTENNA_PATTERN |
+			IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN |
+			0;
+
+		mcs_map = IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
+			IEEE80211_VHT_MCS_SUPPORT_0_9 << 2 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 4 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 6 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 8 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 10 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 12 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 14;
+
+		vht_cap->vht_mcs.rx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.rx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9);
+		vht_cap->vht_mcs.tx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.tx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9);
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		u16 mcs_map;
+
+		vht_cap->vht_supported = true;
+		vht_cap->cap =
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991 |
+			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
+			IEEE80211_VHT_CAP_SHORT_GI_80 |
+			IEEE80211_VHT_CAP_TXSTBC |
+			IEEE80211_VHT_CAP_RXSTBC_1 |
+			IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
+			IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
+			IEEE80211_VHT_CAP_HTC_VHT |
+			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK |
+			IEEE80211_VHT_CAP_RX_ANTENNA_PATTERN |
+			IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN |
+			0;
+
+		mcs_map = IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 2 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 4 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 6 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 8 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 10 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 12 |
+			IEEE80211_VHT_MCS_NOT_SUPPORTED << 14;
+
+		vht_cap->vht_mcs.rx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.rx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9);
+		vht_cap->vht_mcs.tx_mcs_map = cpu_to_le16(mcs_map);
+		vht_cap->vht_mcs.tx_highest =
+			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9);
+	}
+}
+
 static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -252,9 +324,8 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct ieee80211_supported_band *sband;
 
-
-	if (rtlhal->macphymode == SINGLEMAC_SINGLEPHY && rtlhal->bandset ==
-	    BAND_ON_BOTH) {
+	if (rtlhal->macphymode == SINGLEMAC_SINGLEPHY &&
+	    rtlhal->bandset == BAND_ON_BOTH) {
 		/* 1: 2.4 G bands */
 		/* <1> use  mac->bands as mem for hw->wiphy->bands */
 		sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
@@ -282,6 +353,7 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 		/* <3> init ht cap base on ant_num */
 		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
 
+		_rtl_init_hw_vht_capab(hw, &sband->vht_cap);
 		/* <4> set mac->sband to wiphy->sband */
 		hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
 	} else {
@@ -292,8 +364,8 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 			/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
 			 * to default value(1T1R) */
 			memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]),
-				 &rtl_band_2ghz,
-				 sizeof(struct ieee80211_supported_band));
+			       &rtl_band_2ghz,
+			       sizeof(struct ieee80211_supported_band));
 
 			/* <3> init ht cap base on ant_num */
 			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
@@ -307,12 +379,13 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 			/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
 			 * to default value(1T1R) */
 			memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]),
-				 &rtl_band_5ghz,
-				 sizeof(struct ieee80211_supported_band));
+			       &rtl_band_5ghz,
+			       sizeof(struct ieee80211_supported_band));
 
 			/* <3> init ht cap base on ant_num */
 			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
 
+			_rtl_init_hw_vht_capab(hw, &sband->vht_cap);
 			/* <4> set mac->sband to wiphy->sband */
 			hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
 		} else {
@@ -326,7 +399,6 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	    IEEE80211_HW_AMPDU_AGGREGATION |
 	    IEEE80211_HW_CONNECTION_MONITOR |
 	    /* IEEE80211_HW_SUPPORTS_CQM_RSSI | */
-	    IEEE80211_HW_CONNECTION_MONITOR |
 	    IEEE80211_HW_MFP_CAPABLE |
 	    IEEE80211_HW_REPORTS_TX_ACK_STATUS | 0;
 
@@ -336,7 +408,6 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 			IEEE80211_HW_PS_NULLFUNC_STACK |
 			/* IEEE80211_HW_SUPPORTS_DYNAMIC_PS | */
 			0;
-
 	hw->wiphy->interface_modes =
 	    BIT(NL80211_IFTYPE_AP) |
 	    BIT(NL80211_IFTYPE_STATION) |
@@ -344,8 +415,10 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	    BIT(NL80211_IFTYPE_MESH_POINT) |
 	    BIT(NL80211_IFTYPE_P2P_CLIENT) |
 	    BIT(NL80211_IFTYPE_P2P_GO);
-
 	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
+
+	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
+
 	hw->wiphy->rts_threshold = 2347;
 
 	hw->queues = AC_MAX;
@@ -358,6 +431,21 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 	/* hw->max_rates = 1; */
 	hw->sta_data_size = sizeof(struct rtl_sta_info);
 
+/* wowlan is not supported by kernel if CONFIG_PM is not defined */
+#ifdef CONFIG_PM
+	if (rtlpriv->psc.wo_wlan_mode) {
+		if (rtlpriv->psc.wo_wlan_mode & WAKE_ON_MAGIC_PACKET)
+			rtlpriv->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT;
+		if (rtlpriv->psc.wo_wlan_mode & WAKE_ON_PATTERN_MATCH) {
+			rtlpriv->wowlan.n_patterns =
+				MAX_SUPPORT_WOL_PATTERN_NUM;
+			rtlpriv->wowlan.pattern_min_len = MIN_WOL_PATTERN_SIZE;
+			rtlpriv->wowlan.pattern_max_len = MAX_WOL_PATTERN_SIZE;
+		}
+		hw->wiphy->wowlan = &rtlpriv->wowlan;
+	}
+#endif
+
 	/* <6> mac address */
 	if (is_valid_ether_addr(rtlefuse->dev_addr)) {
 		SET_IEEE80211_PERM_ADDR(hw, rtlefuse->dev_addr);
@@ -366,7 +454,6 @@ static void _rtl_init_mac80211(struct ieee80211_hw *hw)
 		get_random_bytes((rtlmac1 + (ETH_ALEN - 1)), 1);
 		SET_IEEE80211_PERM_ADDR(hw, rtlmac1);
 	}
-
 }
 
 static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
@@ -378,10 +465,9 @@ static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
 		    rtl_watch_dog_timer_callback, (unsigned long)hw);
 	setup_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer,
 		    rtl_easy_concurrent_retrytimer_callback, (unsigned long)hw);
-
 	/* <2> work queue */
 	rtlpriv->works.hw = hw;
-	rtlpriv->works.rtl_wq = alloc_workqueue("%s", 0, 0, rtlpriv->cfg->name);
+	rtlpriv->works.rtl_wq = alloc_workqueue(rtlpriv->cfg->name, 0, 0);
 	INIT_DELAYED_WORK(&rtlpriv->works.watchdog_wq,
 			  (void *)rtl_watchdog_wq_callback);
 	INIT_DELAYED_WORK(&rtlpriv->works.ips_nic_off_wq,
@@ -424,7 +510,7 @@ void rtl_init_rfkill(struct ieee80211_hw *hw)
 	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
 
 	if (valid) {
-		pr_info("wireless switch is %s\n",
+		pr_info("rtlwifi: wireless switch is %s\n",
 			rtlpriv->rfkill.rfkill_state ? "on" : "off");
 
 		rtlpriv->rfkill.rfkill_state = radio_state;
@@ -466,22 +552,18 @@ int rtl_init_core(struct ieee80211_hw *hw)
 
 	/* <4> locks */
 	mutex_init(&rtlpriv->locks.conf_mutex);
-	mutex_init(&rtlpriv->locks.ps_mutex);
 	spin_lock_init(&rtlpriv->locks.ips_lock);
 	spin_lock_init(&rtlpriv->locks.irq_th_lock);
-	spin_lock_init(&rtlpriv->locks.irq_pci_lock);
-	spin_lock_init(&rtlpriv->locks.tx_lock);
 	spin_lock_init(&rtlpriv->locks.h2c_lock);
 	spin_lock_init(&rtlpriv->locks.rf_ps_lock);
 	spin_lock_init(&rtlpriv->locks.rf_lock);
 	spin_lock_init(&rtlpriv->locks.waitq_lock);
 	spin_lock_init(&rtlpriv->locks.entry_list_lock);
-	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
 	spin_lock_init(&rtlpriv->locks.cck_and_rw_pagea_lock);
 	spin_lock_init(&rtlpriv->locks.check_sendpkt_lock);
 	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
 	spin_lock_init(&rtlpriv->locks.lps_lock);
-
+	spin_lock_init(&rtlpriv->locks.iqk_lock);
 	/* <5> init list */
 	INIT_LIST_HEAD(&rtlpriv->entry_list);
 
@@ -539,6 +621,7 @@ static void _rtl_query_shortgi(struct ieee80211_hw *hw,
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u8 rate_flag = info->control.rates[0].flags;
 	u8 sgi_40 = 0, sgi_20 = 0, bw_40 = 0;
+	u8 sgi_80 = 0, bw_80 = 0;
 	tcb_desc->use_shortgi = false;
 
 	if (sta == NULL)
@@ -546,24 +629,35 @@ static void _rtl_query_shortgi(struct ieee80211_hw *hw,
 
 	sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
 	sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
+	sgi_80 = sta->vht_cap.cap & IEEE80211_VHT_CAP_SHORT_GI_80;
 
-	if (!(sta->ht_cap.ht_supported))
+	if ((!sta->ht_cap.ht_supported) && (!sta->vht_cap.vht_supported))
 		return;
 
 	if (!sgi_40 && !sgi_20)
 		return;
 
-	if (mac->opmode == NL80211_IFTYPE_STATION)
+	if (mac->opmode == NL80211_IFTYPE_STATION) {
 		bw_40 = mac->bw_40;
-	else if (mac->opmode == NL80211_IFTYPE_AP ||
+		bw_80 = mac->bw_80;
+	} else if (mac->opmode == NL80211_IFTYPE_AP ||
 		 mac->opmode == NL80211_IFTYPE_ADHOC ||
-		 mac->opmode == NL80211_IFTYPE_MESH_POINT)
-		bw_40 = sta->bandwidth >= IEEE80211_STA_RX_BW_40;
+		 mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		bw_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40;
+		bw_80 = sta->vht_cap.vht_supported;
+	}
 
-	if (bw_40 && sgi_40)
-		tcb_desc->use_shortgi = true;
-	else if ((bw_40 == false) && sgi_20)
-		tcb_desc->use_shortgi = true;
+	if (bw_80) {
+		if (sgi_80)
+			tcb_desc->use_shortgi = true;
+		else
+			tcb_desc->use_shortgi = false;
+	} else {
+		if (bw_40 && sgi_40)
+			tcb_desc->use_shortgi = true;
+		else if (!bw_40 && sgi_20)
+			tcb_desc->use_shortgi = true;
+	}
 
 	if (!(rate_flag & IEEE80211_TX_RC_SHORT_GI))
 		tcb_desc->use_shortgi = false;
@@ -613,7 +707,7 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 		if (mac->opmode == NL80211_IFTYPE_STATION) {
 			tcb_desc->ratr_index = 0;
 		} else if (mac->opmode == NL80211_IFTYPE_ADHOC ||
-			   mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+				mac->opmode == NL80211_IFTYPE_MESH_POINT) {
 			if (tcb_desc->multicast || tcb_desc->broadcast) {
 				tcb_desc->hw_rate =
 				    rtlpriv->cfg->maps[RTL_RC_CCK_RATE2M];
@@ -634,7 +728,13 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 		    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
 			tcb_desc->mac_id = 0;
 
-			if (mac->mode == WIRELESS_MODE_N_24G)
+			if (mac->mode == WIRELESS_MODE_AC_5G)
+				tcb_desc->ratr_index =
+					RATR_INX_WIRELESS_AC_5N;
+			else if (mac->mode == WIRELESS_MODE_AC_24G)
+				tcb_desc->ratr_index =
+					RATR_INX_WIRELESS_AC_24N;
+			else if (mac->mode == WIRELESS_MODE_N_24G)
 				tcb_desc->ratr_index = RATR_INX_WIRELESS_NGB;
 			else if (mac->mode == WIRELESS_MODE_N_5G)
 				tcb_desc->ratr_index = RATR_INX_WIRELESS_NG;
@@ -644,8 +744,9 @@ static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
 				tcb_desc->ratr_index = RATR_INX_WIRELESS_B;
 			else if (mac->mode & WIRELESS_MODE_A)
 				tcb_desc->ratr_index = RATR_INX_WIRELESS_G;
+
 		} else if (mac->opmode == NL80211_IFTYPE_AP ||
-			   mac->opmode == NL80211_IFTYPE_ADHOC) {
+			mac->opmode == NL80211_IFTYPE_ADHOC) {
 			if (NULL != sta) {
 				if (sta->aid > 0)
 					tcb_desc->mac_id = sta->aid + 1;
@@ -671,7 +772,8 @@ static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
 	if (mac->opmode == NL80211_IFTYPE_AP ||
 	    mac->opmode == NL80211_IFTYPE_ADHOC ||
 	    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-		if (sta->bandwidth == IEEE80211_STA_RX_BW_20)
+		if (!(sta->ht_cap.ht_supported) ||
+		    !(sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
 			return;
 	} else if (mac->opmode == NL80211_IFTYPE_STATION) {
 		if (!mac->bw_40 || !(sta->ht_cap.ht_supported))
@@ -684,16 +786,74 @@ static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
 	if (tcb_desc->hw_rate <= rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M])
 		return;
 
-	tcb_desc->packet_bw = true;
+	tcb_desc->packet_bw = HT_CHANNEL_WIDTH_20_40;
+
+	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE ||
+	    rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+		    mac->opmode == NL80211_IFTYPE_ADHOC ||
+		    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+			if (!(sta->vht_cap.vht_supported))
+				return;
+		} else if (mac->opmode == NL80211_IFTYPE_STATION) {
+			if (!mac->bw_80 ||
+			    !(sta->vht_cap.vht_supported))
+				return;
+		}
+		if (tcb_desc->hw_rate <=
+			rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15])
+			return;
+		tcb_desc->packet_bw = HT_CHANNEL_WIDTH_80;
+	}
 }
 
-static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw)
+static u8 _rtl_get_vht_highest_n_rate(struct ieee80211_hw *hw,
+				      struct ieee80211_sta *sta)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	u8 hw_rate;
+	u16 tx_mcs_map = le16_to_cpu(sta->vht_cap.vht_mcs.tx_mcs_map);
+
+	if ((get_rf_type(rtlphy) == RF_2T2R) &&
+	    (tx_mcs_map & 0x000c) != 0x000c) {
+		if ((tx_mcs_map & 0x000c) >> 2 ==
+			IEEE80211_VHT_MCS_SUPPORT_0_7)
+			hw_rate =
+			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS7];
+		else if ((tx_mcs_map  & 0x000c) >> 2 ==
+			IEEE80211_VHT_MCS_SUPPORT_0_8)
+			hw_rate =
+			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS9];
+		else
+			hw_rate =
+			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS9];
+	} else {
+		if ((tx_mcs_map  & 0x0003) ==
+			IEEE80211_VHT_MCS_SUPPORT_0_7)
+			hw_rate =
+			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS7];
+		else if ((tx_mcs_map  & 0x0003) ==
+			IEEE80211_VHT_MCS_SUPPORT_0_8)
+			hw_rate =
+			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS9];
+		else
+			hw_rate =
+			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS9];
+	}
 
-	if (get_rf_type(rtlphy) == RF_2T2R)
+	return hw_rate;
+}
+
+static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw,
+				  struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 hw_rate;
+
+	if ((get_rf_type(rtlphy) == RF_2T2R) &&
+	    (sta->ht_cap.mcs.rx_mask[1] != 0))
 		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15];
 	else
 		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS7];
@@ -801,9 +961,7 @@ int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
 				break;
 			}
 		}
-
 	} else {
-
 		switch (desc_rate) {
 		case DESC92_RATEMCS0:
 			rate_idx = 0;
@@ -862,31 +1020,6 @@ int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
 }
 EXPORT_SYMBOL(rtlwifi_rate_mapping);
 
-bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	__le16 fc = rtl_get_fc(skb);
-
-	if (rtlpriv->dm.supp_phymode_switch &&
-	    mac->link_state < MAC80211_LINKED &&
-	    (ieee80211_is_auth(fc) || ieee80211_is_probe_req(fc))) {
-		if (rtlpriv->cfg->ops->chk_switch_dmdp)
-			rtlpriv->cfg->ops->chk_switch_dmdp(hw);
-	}
-	if (ieee80211_is_auth(fc)) {
-		RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG, "MAC80211_LINKING\n");
-		rtl_ips_nic_on(hw);
-
-		mac->link_state = MAC80211_LINKING;
-		/* Dual mac */
-		rtlpriv->phy.need_iqk = true;
-	}
-
-	return true;
-}
-EXPORT_SYMBOL_GPL(rtl_tx_mgmt_proc);
-
 void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 		      struct ieee80211_tx_info *info,
 		      struct ieee80211_sta *sta,
@@ -896,13 +1029,11 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
 	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
 	struct ieee80211_rate *txrate;
-	__le16 fc = hdr->frame_control;
+	__le16 fc = rtl_get_fc(skb);
 
 	txrate = ieee80211_get_tx_rate(hw, info);
 	if (txrate)
 		tcb_desc->hw_rate = txrate->hw_value;
-	else
-		tcb_desc->hw_rate = 0;
 
 	if (ieee80211_is_data(fc)) {
 		/*
@@ -929,15 +1060,21 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 			 *and N rate will all be controlled by FW
 			 *when tcb_desc->use_driver_rate = false
 			 */
-			if (sta && (sta->ht_cap.ht_supported)) {
-				tcb_desc->hw_rate = _rtl_get_highest_n_rate(hw);
+			if (sta && sta->vht_cap.vht_supported) {
+				tcb_desc->hw_rate =
+				_rtl_get_vht_highest_n_rate(hw, sta);
 			} else {
-				if (rtlmac->mode == WIRELESS_MODE_B) {
+				if (sta && (sta->ht_cap.ht_supported)) {
 					tcb_desc->hw_rate =
-					   rtlpriv->cfg->maps[RTL_RC_CCK_RATE11M];
+						_rtl_get_highest_n_rate(hw, sta);
 				} else {
-					tcb_desc->hw_rate =
-					   rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M];
+					if (rtlmac->mode == WIRELESS_MODE_B) {
+						tcb_desc->hw_rate =
+						    rtlpriv->cfg->maps[RTL_RC_CCK_RATE11M];
+					} else {
+						tcb_desc->hw_rate =
+						    rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M];
+					}
 				}
 			}
 		}
@@ -962,54 +1099,58 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 }
 EXPORT_SYMBOL(rtl_get_tcb_desc);
 
-static bool addbareq_rx(struct ieee80211_hw *hw, struct sk_buff *skb)
+bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_sta *sta = NULL;
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	struct rtl_sta_info *sta_entry = NULL;
-	struct ieee80211_mgmt *mgmt = (void *)skb->data;
-	u16 capab = 0, tid = 0;
-	struct rtl_tid_data *tid_data;
-	struct sk_buff *skb_delba = NULL;
-	struct ieee80211_rx_status rx_status = { 0 };
+	__le16 fc = rtl_get_fc(skb);
 
-	rcu_read_lock();
-	sta = rtl_find_sta(hw, hdr->addr3);
-	if (sta == NULL) {
-		RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_EMERG,
-			 "sta is NULL\n");
-		rcu_read_unlock();
-		return true;
+	if (rtlpriv->dm.supp_phymode_switch &&
+	    mac->link_state < MAC80211_LINKED &&
+	    (ieee80211_is_auth(fc) || ieee80211_is_probe_req(fc))) {
+		if (rtlpriv->cfg->ops->chk_switch_dmdp)
+			rtlpriv->cfg->ops->chk_switch_dmdp(hw);
 	}
+	if (ieee80211_is_auth(fc)) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG, "MAC80211_LINKING\n");
+		rtl_ips_nic_on(hw);
+
+		mac->link_state = MAC80211_LINKING;
+		/* Dul mac */
+		rtlpriv->phy.need_iqk = true;
 
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	if (!sta_entry) {
-		rcu_read_unlock();
-		return true;
 	}
-	capab = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
-	tid = (capab & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
-	tid_data = &sta_entry->tids[tid];
-	if (tid_data->agg.rx_agg_state == RTL_RX_AGG_START) {
-		skb_delba = rtl_make_del_ba(hw, hdr->addr2, hdr->addr3, tid);
-		if (skb_delba) {
-			rx_status.freq = hw->conf.chandef.chan->center_freq;
-			rx_status.band = hw->conf.chandef.chan->band;
-			rx_status.flag |= RX_FLAG_DECRYPTED;
-			rx_status.flag |= RX_FLAG_MACTIME_END;
-			rx_status.rate_idx = 0;
-			rx_status.signal = 50 + 10;
-			memcpy(IEEE80211_SKB_RXCB(skb_delba), &rx_status,
-			       sizeof(rx_status));
-			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG,
-				      "fake del\n", skb_delba->data,
-				      skb_delba->len);
-			ieee80211_rx_irqsafe(hw, skb_delba);
-		}
+
+	return true;
+}
+EXPORT_SYMBOL_GPL(rtl_tx_mgmt_proc);
+
+struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw, u8 *sa,
+				u8 *bssid, u16 tid);
+
+static void process_agg_start(struct ieee80211_hw *hw,
+			      struct ieee80211_hdr *hdr, u16 tid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ieee80211_rx_status rx_status = { 0 };
+	struct sk_buff *skb_delba = NULL;
+
+	skb_delba = rtl_make_del_ba(hw, hdr->addr2, hdr->addr3, tid);
+	if (skb_delba) {
+		rx_status.freq = hw->conf.chandef.chan->center_freq;
+		rx_status.band = hw->conf.chandef.chan->band;
+		rx_status.flag |= RX_FLAG_DECRYPTED;
+		rx_status.flag |= RX_FLAG_MACTIME_START;
+		rx_status.rate_idx = 0;
+		rx_status.signal = 50 + 10;
+		memcpy(IEEE80211_SKB_RXCB(skb_delba),
+		       &rx_status, sizeof(rx_status));
+		RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG,
+			      "fake del\n",
+			      skb_delba->data,
+			      skb_delba->len);
+		ieee80211_rx_irqsafe(hw, skb_delba);
 	}
-	rcu_read_unlock();
-	return false;
 }
 
 bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
@@ -1017,8 +1158,8 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	__le16 fc = hdr->frame_control;
-	u8 *act = (u8 *)skb->data + MAC80211_3ADDR_LEN;
+	__le16 fc = rtl_get_fc(skb);
+	u8 *act = (u8 *)(((u8 *)skb->data + MAC80211_3ADDR_LEN));
 	u8 category;
 
 	if (!ieee80211_is_action(fc))
@@ -1034,18 +1175,47 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 				return false;
 
 			RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
-				 "%s ACT_ADDBAREQ From :%pM\n",
-				 is_tx ? "Tx" : "Rx", hdr->addr2);
+				"%s ACT_ADDBAREQ From :%pM\n",
+				is_tx ? "Tx" : "Rx", hdr->addr2);
 			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "req\n",
-				      skb->data, skb->len);
-			if (!is_tx)
-				if (addbareq_rx(hw, skb))
+				skb->data, skb->len);
+			if (!is_tx) {
+				struct ieee80211_sta *sta = NULL;
+				struct rtl_sta_info *sta_entry = NULL;
+				struct rtl_tid_data *tid_data;
+				struct ieee80211_mgmt *mgmt = (void *)skb->data;
+				u16 capab = 0, tid = 0;
+
+				rcu_read_lock();
+				sta = rtl_find_sta(hw, hdr->addr3);
+				if (sta == NULL) {
+					RT_TRACE(rtlpriv, COMP_SEND | COMP_RECV,
+						 DBG_DMESG, "sta is NULL\n");
+					rcu_read_unlock();
+					return true;
+				}
+
+				sta_entry =
+					(struct rtl_sta_info *)sta->drv_priv;
+				if (!sta_entry) {
+					rcu_read_unlock();
 					return true;
+				}
+				capab =
+				  le16_to_cpu(mgmt->u.action.u.addba_req.capab);
+				tid = (capab &
+				       IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
+				tid_data = &sta_entry->tids[tid];
+				if (tid_data->agg.rx_agg_state ==
+				    RTL_RX_AGG_START)
+					process_agg_start(hw, hdr, tid);
+				rcu_read_unlock();
+			}
 			break;
 		case ACT_ADDBARSP:
 			RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
 				 "%s ACT_ADDBARSP From :%pM\n",
-				 is_tx ? "Tx" : "Rx", hdr->addr2);
+				  is_tx ? "Tx" : "Rx", hdr->addr2);
 			break;
 		case ACT_DELBA:
 			RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
@@ -1061,6 +1231,17 @@ bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 }
 EXPORT_SYMBOL_GPL(rtl_action_proc);
 
+static void setup_arp_tx(struct rtl_priv *rtlpriv, struct rtl_ps_ctl *ppsc)
+{
+	rtlpriv->ra.is_special_data = true;
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_special_packet_notify(
+					rtlpriv, 1);
+	rtlpriv->enter_ps = false;
+	schedule_work(&rtlpriv->works.lps_change_work);
+	ppsc->last_delaylps_stamp_jiffies = jiffies;
+}
+
 /*should call before software enc*/
 u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 {
@@ -1069,57 +1250,77 @@ u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
 	__le16 fc = rtl_get_fc(skb);
 	u16 ether_type;
 	u8 mac_hdr_len = ieee80211_get_hdrlen_from_skb(skb);
+	u8 encrypt_header_len = 0;
+	u8 offset;
 	const struct iphdr *ip;
 
 	if (!ieee80211_is_data(fc))
-		return false;
-
-	ip = (const struct iphdr *)(skb->data + mac_hdr_len +
-				    SNAP_SIZE + PROTOC_TYPE_SIZE);
-	ether_type = be16_to_cpup((__be16 *)
-				  (skb->data + mac_hdr_len + SNAP_SIZE));
-
-	switch (ether_type) {
-	case ETH_P_IP: {
-		struct udphdr *udp;
-		u16 src;
-		u16 dst;
+		goto end;
 
-		if (ip->protocol != IPPROTO_UDP)
-			return false;
-		udp = (struct udphdr *)((u8 *)ip + (ip->ihl << 2));
-		src = be16_to_cpu(udp->source);
-		dst = be16_to_cpu(udp->dest);
-
-		/* If this case involves port 68 (UDP BOOTP client) connecting
-		 * with port 67 (UDP BOOTP server), then return true so that
-		 * the lowest speed is used.
-		 */
-		if (!((src == 68 && dst == 67) || (src == 67 && dst == 68)))
-			return false;
-
-		RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
-			 "dhcp %s !!\n", is_tx ? "Tx" : "Rx");
+	switch (rtlpriv->sec.pairwise_enc_algorithm) {
+	case WEP40_ENCRYPTION:
+	case WEP104_ENCRYPTION:
+		encrypt_header_len = 4;/*WEP_IV_LEN*/
 		break;
-	}
-	case ETH_P_ARP:
+	case TKIP_ENCRYPTION:
+		encrypt_header_len = 8;/*TKIP_IV_LEN*/
 		break;
-	case ETH_P_PAE:
-		RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
-			 "802.1X %s EAPOL pkt!!\n", is_tx ? "Tx" : "Rx");
+	case AESCCMP_ENCRYPTION:
+		encrypt_header_len = 8;/*CCMP_HDR_LEN;*/
 		break;
-	case ETH_P_IPV6:
-		/* TODO: Is this right? */
-		return false;
 	default:
-		return false;
+		break;
 	}
-	if (is_tx) {
-		rtlpriv->enter_ps = false;
-		schedule_work(&rtlpriv->works.lps_change_work);
-		ppsc->last_delaylps_stamp_jiffies = jiffies;
+
+	offset = mac_hdr_len + SNAP_SIZE + encrypt_header_len;
+	ether_type = be16_to_cpup((__be16 *)(skb->data + offset));
+
+	if (ETH_P_IP == ether_type) {
+		ip = (struct iphdr *)((u8 *)skb->data + offset +
+		     PROTOC_TYPE_SIZE);
+		if (IPPROTO_UDP == ip->protocol) {
+			struct udphdr *udp = (struct udphdr *)((u8 *)ip +
+							       (ip->ihl << 2));
+			if (((((u8 *)udp)[1] == 68) &&
+			     (((u8 *)udp)[3] == 67)) ||
+			    ((((u8 *)udp)[1] == 67) &&
+			     (((u8 *)udp)[3] == 68))) {
+				/* 68 : UDP BOOTP client
+				 * 67 : UDP BOOTP server
+				 */
+				RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV),
+					 DBG_DMESG, "dhcp %s !!\n",
+					 (is_tx) ? "Tx" : "Rx");
+
+				if (is_tx)
+					setup_arp_tx(rtlpriv, ppsc);
+				return true;
+			}
+		}
+	} else if (ETH_P_ARP == ether_type) {
+		if (is_tx)
+			setup_arp_tx(rtlpriv, ppsc);
+
+		return true;
+	} else if (ETH_P_PAE == ether_type) {
+		RT_TRACE(rtlpriv, (COMP_SEND | COMP_RECV), DBG_DMESG,
+			 "802.1X %s EAPOL pkt!!\n", (is_tx) ? "Tx" : "Rx");
+
+		if (is_tx) {
+			rtlpriv->ra.is_special_data = true;
+			rtlpriv->enter_ps = false;
+			schedule_work(&rtlpriv->works.lps_change_work);
+			ppsc->last_delaylps_stamp_jiffies = jiffies;
+		}
+
+		return true;
+	} else if (0x86DD == ether_type) {
+		return true;
 	}
-	return true;
+
+end:
+	rtlpriv->ra.is_special_data = false;
+	return false;
 }
 EXPORT_SYMBOL_GPL(rtl_is_special_data);
 
@@ -1128,12 +1329,11 @@ EXPORT_SYMBOL_GPL(rtl_is_special_data);
  * functions called by core.c
  *
  *********************************************************/
-int rtl_tx_agg_start(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u16 tid, u16 *ssn)
+int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		     struct ieee80211_sta *sta, u16 tid, u16 *ssn)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_tid_data *tid_data;
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_sta_info *sta_entry = NULL;
 
 	if (sta == NULL)
@@ -1147,43 +1347,38 @@ int rtl_tx_agg_start(struct ieee80211_hw *hw,
 		return -ENXIO;
 	tid_data = &sta_entry->tids[tid];
 
-	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG, "on ra = %pM tid = %d seq:%d\n",
-		 sta->addr, tid, tid_data->seq_number);
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG,
+		 "on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
+		 tid_data->seq_number);
 
 	*ssn = tid_data->seq_number;
 	tid_data->agg.agg_state = RTL_AGG_START;
 
-	ieee80211_start_tx_ba_cb_irqsafe(mac->vif, sta->addr, tid);
-
+	ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 	return 0;
 }
 
-int rtl_tx_agg_stop(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u16 tid)
+int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		    struct ieee80211_sta *sta, u16 tid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_tid_data *tid_data;
 	struct rtl_sta_info *sta_entry = NULL;
 
 	if (sta == NULL)
 		return -EINVAL;
 
-	if (!sta->addr) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "ra = NULL\n");
-		return -EINVAL;
-	}
-
-	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG, "on ra = %pM tid = %d\n",
-		 sta->addr, tid);
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG,
+		 "on ra = %pM tid = %d\n", sta->addr, tid);
 
 	if (unlikely(tid >= MAX_TID_COUNT))
 		return -EINVAL;
 
 	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	tid_data = &sta_entry->tids[tid];
 	sta_entry->tids[tid].agg.agg_state = RTL_AGG_STOP;
 
-	ieee80211_stop_tx_ba_cb_irqsafe(mac->vif, sta->addr, tid);
-
+	ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
 	return 0;
 }
 
@@ -1222,11 +1417,6 @@ int rtl_rx_agg_stop(struct ieee80211_hw *hw,
 	if (sta == NULL)
 		return -EINVAL;
 
-	if (!sta->addr) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "ra = NULL\n");
-		return -EINVAL;
-	}
-
 	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG,
 		 "on ra = %pM tid = %d\n", sta->addr, tid);
 
@@ -1238,7 +1428,6 @@ int rtl_rx_agg_stop(struct ieee80211_hw *hw,
 
 	return 0;
 }
-
 int rtl_tx_agg_oper(struct ieee80211_hw *hw,
 		struct ieee80211_sta *sta, u16 tid)
 {
@@ -1248,13 +1437,8 @@ int rtl_tx_agg_oper(struct ieee80211_hw *hw,
 	if (sta == NULL)
 		return -EINVAL;
 
-	if (!sta->addr) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "ra = NULL\n");
-		return -EINVAL;
-	}
-
-	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG, "on ra = %pM tid = %d\n",
-		 sta->addr, tid);
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_DMESG,
+		 "on ra = %pM tid = %d\n", sta->addr, tid);
 
 	if (unlikely(tid >= MAX_TID_COUNT))
 		return -EINVAL;
@@ -1292,7 +1476,7 @@ void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb)
 		return;
 
 	/* and only beacons from the associated BSSID, please */
-	if (!ether_addr_equal_64bits(hdr->addr3, rtlpriv->mac80211.bssid))
+	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
 		return;
 
 	rtlpriv->link_info.bcn_rx_inperiod++;
@@ -1332,8 +1516,7 @@ void rtl_watchdog_wq_callback(void *data)
 		mac->cnt_after_linked = 0;
 	}
 
-	/*
-	 *<2> to check if traffic busy, if
+	/* <2> to check if traffic busy, if
 	 * busytraffic we don't change channel
 	 */
 	if (mac->link_state >= MAC80211_LINKED) {
@@ -1381,32 +1564,29 @@ void rtl_watchdog_wq_callback(void *data)
 		for (tid = 0; tid <= 7; tid++) {
 			for (idx = 0; idx <= 2; idx++)
 				rtlpriv->link_info.tidtx_in4period[tid][idx] =
-				  rtlpriv->link_info.tidtx_in4period[tid]
-				  [idx + 1];
+					rtlpriv->link_info.tidtx_in4period[tid]
+					[idx + 1];
 			rtlpriv->link_info.tidtx_in4period[tid][3] =
 				rtlpriv->link_info.tidtx_inperiod[tid];
 
 			for (idx = 0; idx <= 3; idx++)
 				tidtx_inp4eriod[tid] +=
-				  rtlpriv->link_info.tidtx_in4period[tid][idx];
+				   rtlpriv->link_info.tidtx_in4period[tid][idx];
 			aver_tidtx_inperiod[tid] = tidtx_inp4eriod[tid] / 4;
 			if (aver_tidtx_inperiod[tid] > 5000)
 				rtlpriv->link_info.higher_busytxtraffic[tid] =
-						   true;
+									true;
 			else
 				rtlpriv->link_info.higher_busytxtraffic[tid] =
-						   false;
+									false;
 		}
 
 		if (((rtlpriv->link_info.num_rx_inperiod +
 		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
 		    (rtlpriv->link_info.num_rx_inperiod > 2))
-			rtlpriv->enter_ps = true;
+			rtl_lps_enter(hw);
 		else
-			rtlpriv->enter_ps = false;
-
-		/* LeisurePS only work in infra mode. */
-		schedule_work(&rtlpriv->works.lps_change_work);
+			rtl_lps_leave(hw);
 	}
 
 	rtlpriv->link_info.num_rx_inperiod = 0;
@@ -1421,32 +1601,37 @@ void rtl_watchdog_wq_callback(void *data)
 	rtlpriv->link_info.higher_busyrxtraffic = higher_busyrxtraffic;
 
 	/* <3> DM */
-	rtlpriv->cfg->ops->dm_watchdog(hw);
+	if (!rtlpriv->cfg->mod_params->disable_watchdog)
+		rtlpriv->cfg->ops->dm_watchdog(hw);
 
 	/* <4> roaming */
 	if (mac->link_state == MAC80211_LINKED &&
 	    mac->opmode == NL80211_IFTYPE_STATION) {
 		if ((rtlpriv->link_info.bcn_rx_inperiod +
-		     rtlpriv->link_info.num_rx_inperiod) == 0) {
+		    rtlpriv->link_info.num_rx_inperiod) == 0) {
 			rtlpriv->link_info.roam_times++;
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_DMESG,
 				 "AP off for %d s\n",
-				 (rtlpriv->link_info.roam_times * 2));
+				(rtlpriv->link_info.roam_times * 2));
 
-			/* if we can't recv beacon for 6s, we should
-			 * reconnect this AP
+			/* if we can't recv beacon for 10s,
+			 * we should reconnect this AP
 			 */
-			if ((rtlpriv->link_info.roam_times >= 3) &&
-			    !is_zero_ether_addr(rtlpriv->mac80211.bssid)) {
+			if (rtlpriv->link_info.roam_times >= 5) {
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 					 "AP off, try to reconnect now\n");
 				rtlpriv->link_info.roam_times = 0;
-				ieee80211_connection_loss(rtlpriv->mac80211.vif);
+				ieee80211_connection_loss(
+					rtlpriv->mac80211.vif);
 			}
 		} else {
 			rtlpriv->link_info.roam_times = 0;
 		}
 	}
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
+
 	rtlpriv->link_info.bcn_rx_inperiod = 0;
 }
 
@@ -1461,7 +1646,6 @@ void rtl_watch_dog_timer_callback(unsigned long data)
 	mod_timer(&rtlpriv->works.watchdog_timer,
 		  jiffies + MSECS(RTL_WATCH_DOG_TIME));
 }
-
 void rtl_fwevt_wq_callback(void *data)
 {
 	struct rtl_works *rtlworks =
@@ -1471,7 +1655,6 @@ void rtl_fwevt_wq_callback(void *data)
 
 	rtlpriv->cfg->ops->c2h_command_handle(hw);
 }
-
 void rtl_easy_concurrent_retrytimer_callback(unsigned long data)
 {
 	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
@@ -1483,7 +1666,6 @@ void rtl_easy_concurrent_retrytimer_callback(unsigned long data)
 
 	rtlpriv->cfg->ops->dualmac_easy_concurrent(hw);
 }
-
 /*********************************************************
  *
  * frame process functions
@@ -1511,7 +1693,8 @@ u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie)
 /* when we use 2 rx ants we send IEEE80211_SMPS_OFF */
 /* when we use 1 rx ant we send IEEE80211_SMPS_STATIC */
 static struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
-		enum ieee80211_smps_mode smps, u8 *da, u8 *bssid)
+				     enum ieee80211_smps_mode smps,
+				     u8 *da, u8 *bssid)
 {
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct sk_buff *skb;
@@ -1536,6 +1719,9 @@ static struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
 	case IEEE80211_SMPS_AUTOMATIC:/* 0 */
 	case IEEE80211_SMPS_NUM_MODES:/* 4 */
 		WARN_ON(1);
+	/* Here will get a 'MISSING_BREAK' in Coverity Test, just ignore it.
+	 * According to Kernel Code, here is right.
+	 */
 	case IEEE80211_SMPS_OFF:/* 1 */ /*MIMO_PS_NOLIMIT*/
 		action_frame->u.action.u.ht_smps.smps_control =
 				WLAN_HT_SMPS_CONTROL_DISABLED;/* 0 */
@@ -1554,8 +1740,8 @@ static struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
 }
 
 int rtl_send_smps_action(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta,
-		enum ieee80211_smps_mode smps)
+			 struct ieee80211_sta *sta,
+			 enum ieee80211_smps_mode smps)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -1590,6 +1776,7 @@ int rtl_send_smps_action(struct ieee80211_hw *hw,
 		struct rtl_sta_info *sta_entry =
 			(struct rtl_sta_info *) sta->drv_priv;
 		sta_entry->mimo_ps = smps;
+		/* rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0); */
 
 		info->control.rates[0].idx = 0;
 		info->band = hw->conf.chandef.chan->band;
@@ -1631,10 +1818,10 @@ void rtl_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
 }
 EXPORT_SYMBOL(rtl_phy_scan_operation_backup);
 
-/* There seem to be issues in mac80211 regarding when del ba frames can be
- * received. As a work around, we make a fake del_ba if we receive a ba_req;
- * however, rx_agg was opened to let mac80211 release some ba related
- * resources. This del_ba is for tx only.
+/* because mac80211 have issues when can receive del ba
+ * so here we just make a fake del_ba if we receive a ba_req
+ * but rx_agg was opened to let mac80211 release some ba
+ * related resources, so please this del_ba for tx
  */
 struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
 				u8 *sa, u8 *bssid, u16 tid)
@@ -1660,7 +1847,7 @@ struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
 	action_frame->u.action.category = WLAN_CATEGORY_BACK;
 	action_frame->u.action.u.delba.action_code = WLAN_ACTION_DELBA;
 	params = (u16)(1 << 11);	/* bit 11 initiator */
-	params |= (u16)(tid << 12);		/* bit 15:12 TID number */
+	params |= (u16)(tid << 12);	/* bit 15:12 TID number */
 
 	action_frame->u.action.u.delba.params = cpu_to_le16(params);
 	action_frame->u.action.u.delba.reason_code =
@@ -1675,7 +1862,7 @@ struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
  *
  *********************************************************/
 static bool rtl_chk_vendor_ouisub(struct ieee80211_hw *hw,
-		struct octet_string vendor_ie)
+				  struct octet_string vendor_ie)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	bool matched = false;
@@ -1848,11 +2035,13 @@ static ssize_t rtl_store_debug_level(struct device *d,
 
 	ret = kstrtoul(buf, 0, &val);
 	if (ret) {
-		printk(KERN_DEBUG "%s is not in hex or decimal form.\n", buf);
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_DMESG,
+			 "%s is not in hex or decimal form.\n", buf);
 	} else {
 		rtlpriv->dbg.global_debuglevel = val;
-		printk(KERN_DEBUG "debuglevel:%x\n",
-		       rtlpriv->dbg.global_debuglevel);
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_DMESG,
+			 "debuglevel:%x\n",
+			 rtlpriv->dbg.global_debuglevel);
 	}
 
 	return strnlen(buf, count);
@@ -1892,7 +2081,7 @@ EXPORT_SYMBOL_GPL(rtl_global_var);
 static int __init rtl_core_module_init(void)
 {
 	if (rtl_rate_control_register())
-		pr_err("Unable to register rtl_rc, use default RC !!\n");
+		pr_err("rtl: Unable to register rtl_rc, use default RC !!\n");
 
 	/* init some global vars */
 	INIT_LIST_HEAD(&rtl_global_var.glb_priv_list);
diff --git a/drivers/net/wireless/rtlwifi/base.h b/drivers/net/wireless/rtlwifi/base.h
index 0cd07420777a..982f2450feea 100644
--- a/drivers/net/wireless/rtlwifi/base.h
+++ b/drivers/net/wireless/rtlwifi/base.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -41,7 +37,7 @@ enum ap_peer {
 	PEER_MARV = 7,
 	PEER_AIRGO = 9,
 	PEER_MAX = 10,
-} ;
+};
 
 #define RTL_DUMMY_OFFSET	0
 #define RTL_DUMMY_UNIT		8
@@ -55,6 +51,16 @@ enum ap_peer {
 #define MAX_BIT_RATE_40MHZ_MCS15	300	/* Mbps */
 #define MAX_BIT_RATE_40MHZ_MCS7		150	/* Mbps */
 
+#define MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9	867	/* Mbps */
+#define MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS7	650	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_2NSS_80MHZ_MCS9	780	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_2NSS_80MHZ_MCS7	585	/* Mbps */
+
+#define MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9	434	/* Mbps */
+#define MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS7	325	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS9	390	/* Mbps */
+#define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS7	293	/* Mbps */
+
 #define RTL_RATE_COUNT_LEGACY		12
 #define RTL_CHANNEL_COUNT		14
 
@@ -78,9 +84,9 @@ enum ap_peer {
 #define SET_80211_PS_POLL_AID(_hdr, _val)		\
 	(*(u16 *)((u8 *)(_hdr) + 2) = _val)
 #define SET_80211_PS_POLL_BSSID(_hdr, _val)		\
-	memcpy(((u8 *)(_hdr)) + 4, (u8 *)(_val), ETH_ALEN)
+	ether_addr_copy(((u8 *)(_hdr)) + 4, (u8 *)(_val))
 #define SET_80211_PS_POLL_TA(_hdr, _val)		\
-	memcpy(((u8 *)(_hdr)) + 10, (u8 *)(_val), ETH_ALEN)
+	ether_addr_copy(((u8 *)(_hdr))+10, (u8 *)(_val))
 
 #define SET_80211_HDR_DURATION(_hdr, _val)	\
 	(*(u16 *)((u8 *)(_hdr) + FRAME_OFFSET_DURATION) = le16_to_cpu(_val))
@@ -113,23 +119,27 @@ void rtl_init_rx_config(struct ieee80211_hw *hw);
 void rtl_init_rfkill(struct ieee80211_hw *hw);
 void rtl_deinit_rfkill(struct ieee80211_hw *hw);
 
-void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
+void rtl_watch_dog_timer_callback(unsigned long data);
 void rtl_deinit_deferred_work(struct ieee80211_hw *hw);
 
 bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
+int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
+			 bool isht, u8 desc_rate, bool first_ampdu);
+bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
 u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
 
+void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
 void rtl_watch_dog_timer_callback(unsigned long data);
-int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
-		     u16 tid, u16 *ssn);
-int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
-		    u16 tid);
-int rtl_tx_agg_oper(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
-		    u16 tid);
-int rtl_rx_agg_start(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
-		     u16 tid);
-int rtl_rx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
-		    u16 tid);
+int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+	struct ieee80211_sta *sta, u16 tid, u16 *ssn);
+int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+	struct ieee80211_sta *sta, u16 tid);
+int rtl_tx_agg_oper(struct ieee80211_hw *hw,
+		    struct ieee80211_sta *sta, u16 tid);
+int rtl_rx_agg_start(struct ieee80211_hw *hw,
+		     struct ieee80211_sta *sta, u16 tid);
+int rtl_rx_agg_stop(struct ieee80211_hw *hw,
+		    struct ieee80211_sta *sta, u16 tid);
 void rtl_watchdog_wq_callback(void *data);
 void rtl_fwevt_wq_callback(void *data);
 
@@ -139,19 +149,14 @@ void rtl_get_tcb_desc(struct ieee80211_hw *hw,
 		      struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc);
 
 int rtl_send_smps_action(struct ieee80211_hw *hw,
-			 struct ieee80211_sta *sta,
-			 enum ieee80211_smps_mode smps);
+		struct ieee80211_sta *sta,
+		enum ieee80211_smps_mode smps);
 u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie);
 void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len);
 u8 rtl_tid_to_ac(u8 tid);
 extern struct attribute_group rtl_attribute_group;
 void rtl_easy_concurrent_retrytimer_callback(unsigned long data);
 extern struct rtl_global_var rtl_global_var;
-int rtlwifi_rate_mapping(struct ieee80211_hw *hw,
-			 bool isht, u8 desc_rate, bool first_ampdu);
-bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
-struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
-				u8 *sa, u8 *bssid, u16 tid);
 void rtl_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbt_precomp.h b/drivers/net/wireless/rtlwifi/btcoexist/halbt_precomp.h
index d76684eb24d0..39b9a3309cfd 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/halbt_precomp.h
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbt_precomp.h
@@ -37,7 +37,13 @@
 
 #include "halbtcoutsrc.h"
 
+#include "halbtc8192e2ant.h"
+#include "halbtc8723b1ant.h"
 #include "halbtc8723b2ant.h"
+#include "halbtc8821a2ant.h"
+#include "halbtc8821a1ant.h"
+
+#define GetDefaultAdapter(padapter)	padapter
 
 #define BIT0	0x00000001
 #define BIT1	0x00000002
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8192e2ant.c
index ab2cc2fadd80..53261d6f8578 100644
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8192e2ant.c
@@ -1,3 +1,27 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
 /**************************************************************
  * Description:
  *
@@ -20,7 +44,7 @@ static struct coex_dm_8192e_2ant *coex_dm = &glcoex_dm_8192e_2ant;
 static struct coex_sta_8192e_2ant glcoex_sta_8192e_2ant;
 static struct coex_sta_8192e_2ant *coex_sta = &glcoex_sta_8192e_2ant;
 
-static const char *const glbt_infosrc8192e2ant[] = {
+static const char *const GLBtInfoSrc8192e2Ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
@@ -235,7 +259,7 @@ static u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist *btcoexist,
 	return wifirssi_state;
 }
 
-static void halbtc_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+static void btc8192e2ant_monitor_bt_enable_dis(struct btc_coexist *btcoexist)
 {
 	static bool pre_bt_disabled;
 	static u32 bt_disable_cnt;
@@ -315,7 +339,7 @@ static u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
 	return disra_mask;
 }
 
-static void halbtc8192e2ant_updatera_mask(struct btc_coexist *btcoexist,
+static void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
 					  bool force_exec, u32 dis_ratemask)
 {
 	coex_dm->curra_mask = dis_ratemask;
@@ -326,8 +350,8 @@ static void halbtc8192e2ant_updatera_mask(struct btc_coexist *btcoexist,
 	coex_dm->prera_mask = coex_dm->curra_mask;
 }
 
-static void autorate_fallback_retry(struct btc_coexist *btcoexist,
-				    bool force_exec, u8 type)
+static void btc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
+						 bool force_exec, u8 type)
 {
 	bool wifi_under_bmode = false;
 
@@ -374,15 +398,15 @@ static void halbtc8192e2ant_retrylimit(struct btc_coexist *btcoexist,
 			   coex_dm->cur_retrylimit_type)) {
 		switch (coex_dm->cur_retrylimit_type) {
 		case 0:	/* normal mode */
-			btcoexist->btc_write_2byte(btcoexist, 0x42a,
-					    coex_dm->backup_retrylimit);
-			break;
+				btcoexist->btc_write_2byte(btcoexist, 0x42a,
+						    coex_dm->backup_retrylimit);
+				break;
 		case 1:	/* retry limit = 8 */
-			btcoexist->btc_write_2byte(btcoexist, 0x42a,
-						   0x0808);
-			break;
+				btcoexist->btc_write_2byte(btcoexist, 0x42a,
+							   0x0808);
+				break;
 		default:
-			break;
+				break;
 		}
 	}
 
@@ -423,20 +447,19 @@ static void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
 	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist,
 						   coex_dm->cur_sstype,
 						   ra_masktype);
-	halbtc8192e2ant_updatera_mask(btcoexist, force_exec, disra_mask);
-
-	autorate_fallback_retry(btcoexist, force_exec, arfr_type);
+	halbtc8192e2ant_Updatera_mask(btcoexist, force_exec, disra_mask);
+btc8192e2ant_autorate_fallback_retry(btcoexist, force_exec, arfr_type);
 	halbtc8192e2ant_retrylimit(btcoexist, force_exec, retrylimit_type);
 	halbtc8192e2ant_ampdu_maxtime(btcoexist, force_exec, ampdutime_type);
 }
 
 static void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
 				       bool force_exec, bool rej_ap_agg_pkt,
-				       bool b_bt_ctrl_agg_buf_size,
+				       bool bt_ctrl_agg_buf_size,
 				       u8 agg_buf_size)
 {
 	bool reject_rx_agg = rej_ap_agg_pkt;
-	bool bt_ctrl_rx_agg_size = b_bt_ctrl_agg_buf_size;
+	bool bt_ctrl_rx_agg_size = bt_ctrl_agg_buf_size;
 	u8 rx_agg_size = agg_buf_size;
 
 	/*********************************************
@@ -448,7 +471,8 @@ static void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE,
 			   &bt_ctrl_rx_agg_size);
 	/* aggregation buf size, only work
-	 * when BT control Rx aggregation size. */
+	 * when BT control Rx aggregation size.
+	 */
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rx_agg_size);
 	/* real update aggregation setting */
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
@@ -492,7 +516,7 @@ static void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
 
 	coex_sta->c2h_bt_info_req_sent = true;
 
-	h2c_parameter[0] |= BIT(0);	/* trigger */
+	h2c_parameter[0] |= BIT0;	/* trigger */
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
@@ -563,7 +587,7 @@ static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 	bool bt_hson = false;
 	u8 algorithm = BT_8192E_2ANT_COEX_ALGO_UNDEFINED;
-	u8 num_diffprofile = 0;
+	u8 numdiffprofile = 0;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
 
@@ -574,15 +598,15 @@ static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 	}
 
 	if (bt_link_info->sco_exist)
-		num_diffprofile++;
+		numdiffprofile++;
 	if (bt_link_info->hid_exist)
-		num_diffprofile++;
+		numdiffprofile++;
 	if (bt_link_info->pan_exist)
-		num_diffprofile++;
+		numdiffprofile++;
 	if (bt_link_info->a2dp_exist)
-		num_diffprofile++;
+		numdiffprofile++;
 
-	if (num_diffprofile == 1) {
+	if (numdiffprofile == 1) {
 		if (bt_link_info->sco_exist) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "SCO only\n");
@@ -610,7 +634,7 @@ static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 				}
 			}
 		}
-	} else if (num_diffprofile == 2) {
+	} else if (numdiffprofile == 2) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist) {
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -673,7 +697,7 @@ static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 				}
 			}
 		}
-	} else if (num_diffprofile == 3) {
+	} else if (numdiffprofile == 3) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist &&
 			    bt_link_info->a2dp_exist) {
@@ -722,7 +746,7 @@ static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
 				}
 			}
 		}
-	} else if (num_diffprofile >= 3) {
+	} else if (numdiffprofile >= 3) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist &&
 			    bt_link_info->pan_exist &&
@@ -750,7 +774,8 @@ static void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
 	u8 h2c_parameter[1] = {0};
 
 	/* There are several type of dacswing
-	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
+	 */
 	h2c_parameter[0] = dac_swinglvl;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
@@ -785,7 +810,7 @@ static void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], preBtDecPwrLvl =%d, curBtDecPwrLvl =%d\n",
+			  "[BTCoex], preBtDecPwrLvl=%d, curBtDecPwrLvl=%d\n",
 			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
 	}
 	halbtc8192e2ant_set_fwdec_btpwr(btcoexist, coex_dm->cur_dec_bt_pwr);
@@ -801,7 +826,7 @@ static void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
 	h2c_parameter[0] = 0;
 
 	if (enable_autoreport)
-		h2c_parameter[0] |= BIT(0);
+		h2c_parameter[0] |= BIT0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], BT FW auto report : %s, FW write 0x68 = 0x%x\n",
@@ -823,7 +848,7 @@ static void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex] bPreBtAutoReport =%d, bCurBtAutoReport =%d\n",
+			  "[BTCoex] bPreBtAutoReport=%d, bCurBtAutoReport=%d\n",
 			  coex_dm->pre_bt_auto_report,
 			  coex_dm->cur_bt_auto_report);
 
@@ -846,7 +871,7 @@ static void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex] preFwDacSwingLvl =%d, curFwDacSwingLvl =%d\n",
+			  "[BTCoex] preFwDacSwingLvl=%d, curFwDacSwingLvl=%d\n",
 			  coex_dm->pre_fw_dac_swing_lvl,
 			  coex_dm->cur_fw_dac_swing_lvl);
 
@@ -861,8 +886,8 @@ static void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-				    bool rx_rf_shrink_on)
+static void btc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+						 bool rx_rf_shrink_on)
 {
 	if (rx_rf_shrink_on) {
 		/* Shrink RF Rx LPF corner */
@@ -872,7 +897,8 @@ static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
 					  0xfffff, 0xffffc);
 	} else {
 		/* Resume RF Rx LPF corner
-		 * After initialized, we can use coex_dm->btRf0x1eBackup */
+		 * After initialized, we can use coex_dm->btRf0x1eBackup
+		 */
 		if (btcoexist->initilized) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
 				  "[BTCoex], Resume RF Rx LPF corner!!\n");
@@ -894,7 +920,7 @@ static void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex]bPreRfRxLpfShrink =%d, bCurRfRxLpfShrink =%d\n",
+			  "[BTCoex]bPreRfRxLpfShrink=%d,bCurRfRxLpfShrink=%d\n",
 			  coex_dm->pre_rf_rx_lpf_shrink,
 			  coex_dm->cur_rf_rx_lpf_shrink);
 
@@ -902,7 +928,8 @@ static void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
 		    coex_dm->cur_rf_rx_lpf_shrink)
 			return;
 	}
-	set_sw_rf_rx_lpf_corner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
+	btc8192e2ant_set_sw_rf_rx_lpf_corner(btcoexist,
+					     coex_dm->cur_rf_rx_lpf_shrink);
 
 	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
 }
@@ -917,9 +944,9 @@ static void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
 }
 
-static void setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
-				    bool sw_dac_swingon,
-				    u32 sw_dac_swinglvl)
+static void btc8192e2ant_setsw_full_swing(struct btc_coexist *btcoexist,
+					  bool sw_dac_swingon,
+					  u32 sw_dac_swinglvl)
 {
 	if (sw_dac_swingon)
 		halbtc8192e2ant_set_dac_swingreg(btcoexist, sw_dac_swinglvl);
@@ -927,12 +954,12 @@ static void setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
 		halbtc8192e2ant_set_dac_swingreg(btcoexist, 0x18);
 }
 
-static void halbtc8192e2ant_dacswing(struct btc_coexist *btcoexist,
+static void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
 				     bool force_exec, bool dac_swingon,
 				     u32 dac_swinglvl)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn DacSwing =%s, dac_swinglvl = 0x%x\n",
+		  "[BTCoex], %s turn DacSwing=%s, dac_swinglvl = 0x%x\n",
 		  (force_exec ? "force to" : ""),
 		  ((dac_swingon) ? "ON" : "OFF"), dac_swinglvl);
 	coex_dm->cur_dac_swing_on = dac_swingon;
@@ -940,11 +967,11 @@ static void halbtc8192e2ant_dacswing(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreDacSwingOn =%d, preDacSwingLvl = 0x%x, ",
+			  "[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl = 0x%x, ",
 			  coex_dm->pre_dac_swing_on,
 			  coex_dm->pre_dac_swing_lvl);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "bCurDacSwingOn =%d, curDacSwingLvl = 0x%x\n",
+			  "bCurDacSwingOn=%d, curDacSwingLvl = 0x%x\n",
 			  coex_dm->cur_dac_swing_on,
 			  coex_dm->cur_dac_swing_lvl);
 
@@ -953,7 +980,7 @@ static void halbtc8192e2ant_dacswing(struct btc_coexist *btcoexist,
 			return;
 	}
 	mdelay(30);
-	setsw_fulltime_dacswing(btcoexist, dac_swingon, dac_swinglvl);
+	btc8192e2ant_setsw_full_swing(btcoexist, dac_swingon, dac_swinglvl);
 
 	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
 	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
@@ -984,7 +1011,7 @@ static void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
 	}
 }
 
-static void halbtc8192e2ant_agctable(struct btc_coexist *btcoexist,
+static void halbtc8192e2ant_AgcTable(struct btc_coexist *btcoexist,
 				     bool force_exec, bool agc_table_en)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
@@ -995,7 +1022,7 @@ static void halbtc8192e2ant_agctable(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAgcTableEn =%d, bCurAgcTableEn =%d\n",
+			  "[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n",
 			  coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en);
 
 		if (coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en)
@@ -1027,9 +1054,10 @@ static void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
 }
 
-static void halbtc_coex_table(struct btc_coexist *btcoexist, bool force_exec,
-			      u32 val0x6c0, u32 val0x6c4,
-			      u32 val0x6c8, u8 val0x6cc)
+static void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist,
+				       bool force_exec,
+				       u32 val0x6c0, u32 val0x6c4,
+				       u32 val0x6c8, u8 val0x6cc)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
 		  "[BTCoex], %s write Coex Table 0x6c0 = 0x%x, ",
@@ -1047,10 +1075,10 @@ static void halbtc_coex_table(struct btc_coexist *btcoexist, bool force_exec,
 			  "[BTCoex], preVal0x6c0 = 0x%x, preVal0x6c4 = 0x%x, ",
 			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "preVal0x6c8 = 0x%x, preVal0x6cc = 0x%x!!\n",
+			  "preVal0x6c8 = 0x%x, preVal0x6cc = 0x%x !!\n",
 			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], curVal0x6c0 = 0x%x, curVal0x6c4 = 0x%x\n",
+			  "[BTCoex], curVal0x6c0 = 0x%x, curVal0x6c4 = 0x%x,\n",
 			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
 			  "curVal0x6c8 = 0x%x, curVal0x6cc = 0x%x !!\n",
@@ -1071,29 +1099,29 @@ static void halbtc_coex_table(struct btc_coexist *btcoexist, bool force_exec,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-static void halbtc_coex_table_with_type(struct btc_coexist *btcoexist,
-					bool force_exec, u8 type)
+static void btc8192e2ant_coex_tbl_w_type(struct btc_coexist *btcoexist,
+					 bool force_exec, u8 type)
 {
 	switch (type) {
 	case 0:
-		halbtc_coex_table(btcoexist, force_exec, 0x55555555,
-				  0x5a5a5a5a, 0xffffff, 0x3);
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5a5a5a5a, 0xffffff, 0x3);
 		break;
 	case 1:
-		halbtc_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
-				  0x5a5a5a5a, 0xffffff, 0x3);
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffffff, 0x3);
 		break;
 	case 2:
-		halbtc_coex_table(btcoexist, force_exec, 0x55555555,
-				  0x5ffb5ffb, 0xffffff, 0x3);
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x5ffb5ffb, 0xffffff, 0x3);
 		break;
 	case 3:
-		halbtc_coex_table(btcoexist, force_exec, 0xdfffdfff,
-				  0x5fdb5fdb, 0xffffff, 0x3);
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0xdfffdfff,
+					   0x5fdb5fdb, 0xffffff, 0x3);
 		break;
 	case 4:
-		halbtc_coex_table(btcoexist, force_exec, 0xdfffdfff,
-				  0x5ffb5ffb, 0xffffff, 0x3);
+		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0xdfffdfff,
+					   0x5ffb5ffb, 0xffffff, 0x3);
 		break;
 	default:
 		break;
@@ -1106,7 +1134,7 @@ static void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
 	u8 h2c_parameter[1] = {0};
 
 	if (enable)
-		h2c_parameter[0] |= BIT(0); /* function enable */
+		h2c_parameter[0] |= BIT0; /* function enable */
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex]set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
@@ -1115,7 +1143,7 @@ static void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
 }
 
-static void halbtc8192e2ant_ignorewlanact(struct btc_coexist *btcoexist,
+static void halbtc8192e2ant_IgnoreWlanAct(struct btc_coexist *btcoexist,
 					  bool force_exec, bool enable)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
@@ -1140,10 +1168,10 @@ static void halbtc8192e2ant_ignorewlanact(struct btc_coexist *btcoexist,
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-static void halbtc8192e2ant_setfwpstdma(struct btc_coexist *btcoexist, u8 byte1,
+static void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
 					u8 byte2, u8 byte3, u8 byte4, u8 byte5)
 {
-	u8 h2c_parameter[5];
+	u8 h2c_parameter[5] = {0};
 
 	h2c_parameter[0] = byte1;
 	h2c_parameter[1] = byte2;
@@ -1166,19 +1194,19 @@ static void halbtc8192e2ant_setfwpstdma(struct btc_coexist *btcoexist, u8 byte1,
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-static void halbtc_sw_mechanism1(struct btc_coexist *btcoexist,
+static void btc8192e2ant_sw_mec1(struct btc_coexist *btcoexist,
 				 bool shrink_rx_lpf, bool low_penalty_ra,
 				 bool limited_dig, bool btlan_constrain)
 {
 	halbtc8192e2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
 }
 
-static void halbtc_sw_mechanism2(struct btc_coexist *btcoexist,
+static void btc8192e2ant_sw_mec2(struct btc_coexist *btcoexist,
 				 bool agc_table_shift, bool adc_backoff,
 				 bool sw_dac_swing, u32 dac_swinglvl)
 {
-	halbtc8192e2ant_agctable(btcoexist, NORMAL_EXEC, agc_table_shift);
-	halbtc8192e2ant_dacswing(btcoexist, NORMAL_EXEC, sw_dac_swing,
+	halbtc8192e2ant_AgcTable(btcoexist, NORMAL_EXEC, agc_table_shift);
+	halbtc8192e2ant_DacSwing(btcoexist, NORMAL_EXEC, sw_dac_swing,
 				 dac_swinglvl);
 }
 
@@ -1186,7 +1214,7 @@ static void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
 				    bool force_exec, bool turn_on, u8 type)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn %s PS TDMA, type =%d\n",
+		  "[BTCoex], %s turn %s PS TDMA, type=%d\n",
 		  (force_exec ? "force to" : ""),
 		  (turn_on ? "ON" : "OFF"), type);
 	coex_dm->cur_ps_tdma_on = turn_on;
@@ -1208,91 +1236,91 @@ static void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
 		switch (type) {
 		case 1:
 		default:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
 						    0x1a, 0xe1, 0x90);
 			break;
 		case 2:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
 						    0x12, 0xe1, 0x90);
 			break;
 		case 3:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
 						    0x3, 0xf1, 0x90);
 			break;
 		case 4:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x10,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x10,
 						    0x3, 0xf1, 0x90);
 			break;
 		case 5:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
 						    0x1a, 0x60, 0x90);
 			break;
 		case 6:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
 						    0x12, 0x60, 0x90);
 			break;
 		case 7:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
 						    0x3, 0x70, 0x90);
 			break;
 		case 8:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xa3, 0x10,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xa3, 0x10,
 						    0x3, 0x70, 0x90);
 			break;
 		case 9:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
 						    0x1a, 0xe1, 0x10);
 			break;
 		case 10:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
 						    0x12, 0xe1, 0x10);
 			break;
 		case 11:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
 						    0x3, 0xf1, 0x10);
 			break;
 		case 12:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x10,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x10,
 						    0x3, 0xf1, 0x10);
 			break;
 		case 13:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
 						    0x1a, 0xe0, 0x10);
 			break;
 		case 14:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
 						    0x12, 0xe0, 0x10);
 			break;
 		case 15:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1c,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
 						    0x3, 0xf0, 0x10);
 			break;
 		case 16:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x12,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
 						    0x3, 0xf0, 0x10);
 			break;
 		case 17:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0x61, 0x20,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0x61, 0x20,
 						    0x03, 0x10, 0x10);
 			break;
 		case 18:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x5,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x5,
 						    0x5, 0xe1, 0x90);
 			break;
 		case 19:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x25,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x25,
 						    0x25, 0xe1, 0x90);
 			break;
 		case 20:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x25,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x25,
 						    0x25, 0x60, 0x90);
 			break;
 		case 21:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x15,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x15,
 						    0x03, 0x70, 0x90);
 			break;
 		case 71:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0xe3, 0x1a,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
 						    0x1a, 0xe1, 0x90);
 			break;
 		}
@@ -1301,12 +1329,12 @@ static void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
 		switch (type) {
 		default:
 		case 0:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0x8, 0x0, 0x0,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0x8, 0x0, 0x0,
 						    0x0, 0x0);
 			btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x4);
 			break;
 		case 1:
-			halbtc8192e2ant_setfwpstdma(btcoexist, 0x0, 0x0, 0x0,
+			halbtc8192e2ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0,
 						    0x8, 0x0);
 			mdelay(5);
 			btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
@@ -1319,7 +1347,8 @@ static void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-static void set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
+static void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist,
+					      u8 sstype)
 {
 	u8 mimops = BTC_MIMO_PS_DYNAMIC;
 	u32 disra_mask = 0x0;
@@ -1329,7 +1358,7 @@ static void set_switch_sstype(struct btc_coexist *btcoexist, u8 sstype)
 
 	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist, sstype,
 						   coex_dm->curra_masktype);
-	halbtc8192e2ant_updatera_mask(btcoexist, FORCE_EXEC, disra_mask);
+	halbtc8192e2ant_Updatera_mask(btcoexist, FORCE_EXEC, disra_mask);
 
 	if (sstype == 1) {
 		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
@@ -1366,7 +1395,7 @@ static void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
 		if (coex_dm->pre_sstype == coex_dm->cur_sstype)
 			return;
 	}
-	set_switch_sstype(btcoexist, coex_dm->cur_sstype);
+	halbtc8192e2ant_set_switch_sstype(btcoexist, coex_dm->cur_sstype);
 
 	coex_dm->pre_sstype = coex_dm->cur_sstype;
 }
@@ -1379,11 +1408,11 @@ static void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 
 	/* sw all off */
-	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc_sw_mechanism2(btcoexist, false, false, false, 0x18);
+	btc8192e2ant_sw_mec1(btcoexist, false, false, false, false);
+	btc8192e2ant_sw_mec2(btcoexist, false, false, false, 0x18);
 
 	/* hw all off */
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 0);
 }
 
 static void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
@@ -1394,11 +1423,11 @@ static void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, FORCE_EXEC, 6);
 	halbtc8192e2ant_dec_btpwr(btcoexist, FORCE_EXEC, 0);
 
-	halbtc_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, FORCE_EXEC, 0);
 	halbtc8192e2ant_switch_sstype(btcoexist, FORCE_EXEC, 2);
 
-	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc_sw_mechanism2(btcoexist, false, false, false, 0x18);
+	btc8192e2ant_sw_mec1(btcoexist, false, false, false, false);
+	btc8192e2ant_sw_mec2(btcoexist, false, false, false, 0x18);
 }
 
 static void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
@@ -1410,13 +1439,13 @@ static void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 
 	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 2);
 	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 
-	halbtc_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc_sw_mechanism2(btcoexist, false, false, false, 0x18);
+	btc8192e2ant_sw_mec1(btcoexist, false, false, false, false);
+	btc8192e2ant_sw_mec2(btcoexist, false, false, false, 0x18);
 }
 
 static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
@@ -1447,17 +1476,17 @@ static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 		     coex_dm->bt_status) ||
 		    (BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
 		     coex_dm->bt_status)) {
-			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
-						      2);
-			halbtc_coex_table_with_type(btcoexist,
-						    NORMAL_EXEC, 1);
-			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						0);
+			halbtc8192e2ant_switch_sstype(btcoexist,
+						      NORMAL_EXEC, 2);
+			btc8192e2ant_coex_tbl_w_type(btcoexist,
+						     NORMAL_EXEC, 1);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0);
 		} else {
 			halbtc8192e2ant_switch_sstype(btcoexist,
 						      NORMAL_EXEC, 1);
-			halbtc_coex_table_with_type(btcoexist,
-						    NORMAL_EXEC, 0);
+			btc8192e2ant_coex_tbl_w_type(btcoexist,
+						     NORMAL_EXEC, 0);
 			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						false, 1);
 		}
@@ -1465,10 +1494,8 @@ static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 		halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 
-		halbtc_sw_mechanism1(btcoexist, false, false, false,
-				     false);
-		halbtc_sw_mechanism2(btcoexist, false, false, false,
-				     0x18);
+		btc8192e2ant_sw_mec1(btcoexist, false, false, false, false);
+		btc8192e2ant_sw_mec2(btcoexist, false, false, false, 0x18);
 
 		common = true;
 	} else {
@@ -1484,17 +1511,17 @@ static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 
 			halbtc8192e2ant_switch_sstype(btcoexist,
 						      NORMAL_EXEC, 2);
-			halbtc_coex_table_with_type(btcoexist,
-						    NORMAL_EXEC, 1);
+			btc8192e2ant_coex_tbl_w_type(btcoexist,
+						     NORMAL_EXEC, 1);
 			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						false, 0);
-			halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC,
-							6);
+			halbtc8192e2ant_fw_dac_swinglvl(btcoexist,
+							NORMAL_EXEC, 6);
 			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 
 			common = true;
@@ -1510,19 +1537,19 @@ static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "Wifi connected + BT connected-idle!!\n");
 
-			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
-						      2);
-			halbtc_coex_table_with_type(btcoexist,
-						    NORMAL_EXEC, 1);
+			halbtc8192e2ant_switch_sstype(btcoexist,
+						      NORMAL_EXEC, 2);
+			btc8192e2ant_coex_tbl_w_type(btcoexist,
+						     NORMAL_EXEC, 1);
 			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						false, 0);
 			halbtc8192e2ant_fw_dac_swinglvl(btcoexist,
 							NORMAL_EXEC, 6);
 			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 
 			common = true;
@@ -1542,20 +1569,18 @@ static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 
 				halbtc8192e2ant_switch_sstype(btcoexist,
 							      NORMAL_EXEC, 1);
-				halbtc_coex_table_with_type(btcoexist,
-							    NORMAL_EXEC, 2);
+				btc8192e2ant_coex_tbl_w_type(btcoexist,
+							     NORMAL_EXEC, 2);
 				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 21);
 				halbtc8192e2ant_fw_dac_swinglvl(btcoexist,
 								NORMAL_EXEC, 6);
 				halbtc8192e2ant_dec_btpwr(btcoexist,
 							  NORMAL_EXEC, 0);
-				halbtc_sw_mechanism1(btcoexist, false,
-						     false, false,
-						     false);
-				halbtc_sw_mechanism2(btcoexist, false,
-						     false, false,
-						     0x18);
+				btc8192e2ant_sw_mec1(btcoexist, false,
+						     false, false, false);
+				btc8192e2ant_sw_mec2(btcoexist, false,
+						     false, false, 0x18);
 				common = true;
 			}
 		}
@@ -1563,13 +1588,593 @@ static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
 	return common;
 }
 
+static void btc8192e_int1(struct btc_coexist *btcoexist, bool tx_pause,
+			  int result)
+{
+	if (tx_pause) {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 1\n");
+
+		if (coex_dm->cur_ps_tdma == 71) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+			coex_dm->tdma_adj_type = 5;
+		} else if (coex_dm->cur_ps_tdma == 1) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+			coex_dm->tdma_adj_type = 5;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 8);
+			coex_dm->tdma_adj_type = 8;
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 13);
+			coex_dm->tdma_adj_type = 13;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 5);
+				coex_dm->tdma_adj_type = 5;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 13);
+				coex_dm->tdma_adj_type = 13;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 71);
+			coex_dm->tdma_adj_type = 71;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 4);
+			coex_dm->tdma_adj_type = 4;
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+			coex_dm->tdma_adj_type = 9;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 71) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 1);
+				coex_dm->tdma_adj_type = 1;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 4) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 1);
+				coex_dm->tdma_adj_type = 1;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 71);
+				coex_dm->tdma_adj_type = 71;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->tdma_adj_type = 9;
+			}
+		}
+	}
+}
+
+static void btc8192e_int2(struct btc_coexist *btcoexist, bool tx_pause,
+			  int result)
+{
+	if (tx_pause) {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 1\n");
+		if (coex_dm->cur_ps_tdma == 1) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 8);
+			coex_dm->tdma_adj_type = 8;
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 4);
+			coex_dm->tdma_adj_type = 4;
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 4) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			}
+		}
+	}
+}
+
+static void btc8192e_int3(struct btc_coexist *btcoexist, bool tx_pause,
+			  int result)
+{
+	if (tx_pause) {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 1\n");
+		if (coex_dm->cur_ps_tdma == 1) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 8);
+			coex_dm->tdma_adj_type = 8;
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM,
+			  ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 4);
+			coex_dm->tdma_adj_type = 4;
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 4) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			}
+		}
+	}
+}
+
 static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 						 bool sco_hid, bool tx_pause,
 						 u8 max_interval)
 {
 	static int up, dn, m, n, wait_cnt;
 	/* 0: no change, +1: increase WiFi duration,
-	 * -1: decrease WiFi duration */
+	 * -1: decrease WiFi duration
+	 */
 	int result;
 	u8 retry_cnt = 0;
 
@@ -1586,44 +2191,44 @@ static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
+					coex_dm->tdma_adj_type = 13;
 				} else if (max_interval == 2) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
+					coex_dm->tdma_adj_type = 14;
 				} else if (max_interval == 3) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
+					coex_dm->tdma_adj_type = 15;
 				} else {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
+					coex_dm->tdma_adj_type = 15;
 				}
 			} else {
 				if (max_interval == 1) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
+					coex_dm->tdma_adj_type = 9;
 				} else if (max_interval == 2) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
+					coex_dm->tdma_adj_type = 10;
 				} else if (max_interval == 3) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
+					coex_dm->tdma_adj_type = 11;
 				} else {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
+					coex_dm->tdma_adj_type = 11;
 				}
 			}
 		} else {
@@ -1632,44 +2237,44 @@ static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
+					coex_dm->tdma_adj_type = 5;
 				} else if (max_interval == 2) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
+					coex_dm->tdma_adj_type = 6;
 				} else if (max_interval == 3) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
+					coex_dm->tdma_adj_type = 7;
 				} else {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
+					coex_dm->tdma_adj_type = 7;
 				}
 			} else {
 				if (max_interval == 1) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 1);
-					coex_dm->ps_tdma_du_adj_type = 1;
+					coex_dm->tdma_adj_type = 1;
 				} else if (max_interval == 2) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
+					coex_dm->tdma_adj_type = 2;
 				} else if (max_interval == 3) {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
+					coex_dm->tdma_adj_type = 3;
 				} else {
 					halbtc8192e2ant_ps_tdma(btcoexist,
 								NORMAL_EXEC,
 								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
+					coex_dm->tdma_adj_type = 3;
 				}
 			}
 		}
@@ -1686,7 +2291,7 @@ static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 			  "[BTCoex], retry_cnt = %d\n", retry_cnt);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_cnt =%d\n",
+			  "[BTCoex], up=%d, dn=%d, m=%d, n=%d, wait_cnt=%d\n",
 			  up, dn, m, n, wait_cnt);
 		result = 0;
 		wait_cnt++;
@@ -1753,857 +2358,26 @@ static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 			  "[BTCoex], max Interval = %d\n", max_interval);
-		if (max_interval == 1) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-
-				if (coex_dm->cur_ps_tdma == 71) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									     8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 5);
-						coex_dm->ps_tdma_du_adj_type =
-									     5;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 13);
-						coex_dm->ps_tdma_du_adj_type =
-									     13;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 71);
-					coex_dm->ps_tdma_du_adj_type = 71;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 71) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									     1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									     4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									     1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 71);
-						coex_dm->ps_tdma_du_adj_type =
-									     71;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 9);
-						coex_dm->ps_tdma_du_adj_type =
-									     9;
-					}
-				}
-			}
-		} else if (max_interval == 2) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									     8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									     4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					}
-				}
-			}
-		} else if (max_interval == 3) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									     8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									     4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					}
-				}
-			}
-		}
+		if (max_interval == 1)
+			btc8192e_int1(btcoexist, tx_pause, result);
+		else if (max_interval == 2)
+			btc8192e_int2(btcoexist, tx_pause, result);
+		else if (max_interval == 3)
+			btc8192e_int3(btcoexist, tx_pause, result);
 	}
 
 	/* if current PsTdma not match with
 	 * the recorded one (when scan, dhcp...),
-	 * then we have to adjust it back to the previous record one. */
-	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
+	 * then we have to adjust it back to the previous record one.
+	 */
+	if (coex_dm->cur_ps_tdma != coex_dm->tdma_adj_type) {
 		bool scan = false, link = false, roam = false;
+
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 			  "[BTCoex], PsTdma type dismatch!!!, ");
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "curPsTdma =%d, recordPsTdma =%d\n",
-			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
+			  "curPsTdma=%d, recordPsTdma=%d\n",
+			  coex_dm->cur_ps_tdma, coex_dm->tdma_adj_type);
 
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
@@ -2612,7 +2386,7 @@ static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
 		if (!scan && !link && !roam)
 			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true,
-						coex_dm->ps_tdma_du_adj_type);
+						coex_dm->tdma_adj_type);
 		else
 			BTC_PRINT(BTC_MSG_ALGORITHM,
 				  ALGO_TRACE_FW_DETAIL,
@@ -2633,7 +2407,7 @@ static void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
 
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 4);
 
 	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
 
@@ -2657,27 +2431,27 @@ static void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x6);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x6);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x6);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x6);
 		}
 	}
@@ -2695,7 +2469,7 @@ static void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
 
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 4);
 
 	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
 
@@ -2719,27 +2493,27 @@ static void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x6);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x6);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x6);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x6);
 		}
 	}
@@ -2760,7 +2534,7 @@ static void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 3);
 
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -2780,27 +2554,27 @@ static void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
@@ -2837,9 +2611,9 @@ static void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 
 	if (long_dist)
-		halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 0);
 	else
-		halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 2);
 
 	if (long_dist) {
 		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 17);
@@ -2869,27 +2643,27 @@ static void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
@@ -2907,7 +2681,7 @@ static void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
 
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 2);
 
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -2915,13 +2689,13 @@ static void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
 	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+						     false, 2);
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
 	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     2);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+						     false, 2);
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
 	}
 
@@ -2930,27 +2704,27 @@ static void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     true, 0x6);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     true, 0x6);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     true, 0x6);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     true, 0x6);
 		}
 	}
@@ -2969,7 +2743,7 @@ static void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
 
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 2);
 
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -2990,27 +2764,27 @@ static void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
@@ -3030,7 +2804,7 @@ static void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
 
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 2);
 
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -3048,27 +2822,27 @@ static void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
@@ -3088,7 +2862,7 @@ static void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 2);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
@@ -3099,40 +2873,40 @@ static void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     3);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+						     false, 3);
 	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     3);
+		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
+						     false, 3);
 	}
 
 	/* sw mechanism	*/
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, false,
+			btc8192e2ant_sw_mec1(btcoexist, true, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, false,
+			btc8192e2ant_sw_mec1(btcoexist, false, false,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
@@ -3153,7 +2927,7 @@ static void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 
 	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 3);
 
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -3161,46 +2935,48 @@ static void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
 	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
 	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
 		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
+			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
+			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
 	}
 
 	/* sw mechanism */
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
 }
 
 /* HID+A2DP+PAN(EDR) */
-static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+static void btc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 {
 	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_bw;
@@ -3215,7 +2991,7 @@ static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 3);
 
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -3235,27 +3011,27 @@ static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
@@ -3274,7 +3050,7 @@ static void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	halbtc_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, NORMAL_EXEC, 3);
 
 	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
 	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
@@ -3294,27 +3070,27 @@ static void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, true, true,
+			btc8192e2ant_sw_mec1(btcoexist, true, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	} else {
 		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, true, false,
+			btc8192e2ant_sw_mec2(btcoexist, true, false,
 					     false, 0x18);
 		} else {
-			halbtc_sw_mechanism1(btcoexist, false, true,
+			btc8192e2ant_sw_mec1(btcoexist, false, true,
 					     false, false);
-			halbtc_sw_mechanism2(btcoexist, false, false,
+			btc8192e2ant_sw_mec2(btcoexist, false, false,
 					     false, 0x18);
 		}
 	}
@@ -3325,7 +3101,7 @@ static void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	u8 algorithm = 0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], RunCoexistMechanism() ===>\n");
+		  "[BTCoex], RunCoexistMechanism()===>\n");
 
 	if (btcoexist->manual_control) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -3359,7 +3135,7 @@ static void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	} else {
 		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex] preAlgorithm =%d, curAlgorithm =%d\n",
+				  "[BTCoex] preAlgorithm=%d, curAlgorithm=%d\n",
 				  coex_dm->pre_algorithm,
 				  coex_dm->cur_algorithm);
 			coex_dm->auto_tdma_adjust = false;
@@ -3413,7 +3189,7 @@ static void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
-			action_hid_a2dp_pan_edr(btcoexist);
+			btc8192e2ant_action_hid_a2dp_pan_edr(btcoexist);
 			break;
 		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -3467,7 +3243,7 @@ static void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist,
 	else
 		btcoexist->btc_write_4byte(btcoexist, 0x64, 0x30030004);
 
-	halbtc_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	btc8192e2ant_coex_tbl_w_type(btcoexist, FORCE_EXEC, 0);
 
 	/* antenna switch control parameter */
 	btcoexist->btc_write_4byte(btcoexist, 0x858, 0x55555555);
@@ -3487,21 +3263,21 @@ static void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte(btcoexist, 0x40, 0x20);
 	/* enable mailbox interface */
 	u16tmp = btcoexist->btc_read_2byte(btcoexist, 0x40);
-	u16tmp |= BIT(9);
+	u16tmp |= BIT9;
 	btcoexist->btc_write_2byte(btcoexist, 0x40, u16tmp);
 
 	/* enable PTA I2C mailbox  */
 	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x101);
-	u8tmp |= BIT(4);
+	u8tmp |= BIT4;
 	btcoexist->btc_write_1byte(btcoexist, 0x101, u8tmp);
 
 	/* enable bt clock when wifi is disabled. */
 	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x93);
-	u8tmp |= BIT(0);
+	u8tmp |= BIT0;
 	btcoexist->btc_write_1byte(btcoexist, 0x93, u8tmp);
 	/* enable bt clock when suspend. */
 	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x7);
-	u8tmp |= BIT(0);
+	u8tmp |= BIT0;
 	btcoexist->btc_write_1byte(btcoexist, 0x7, u8tmp);
 }
 
@@ -3529,7 +3305,7 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 {
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	u8 *cli_buf = btcoexist->cli_buf;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
 	u16 u16tmp[4];
 	u32 u32tmp[4];
@@ -3540,87 +3316,75 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 	u8 wifi_dot11_chnl, wifi_hs_chnl;
 	u32 fw_ver = 0, bt_patch_ver = 0;
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n ============[BT Coexist info] ============");
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n ============[BT Coexist info]============");
 
 	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ===========[Under Manual Control] ===========");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n ===========[Under Manual Control]===========");
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
 	}
 
 	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n BT not exists !!!");
 		return;
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism: ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:",
 		   board_info->pg_ant_num, board_info->btdm_ant_num);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s / %d",
 		   "BT stack/ hci ext ver",
 		   ((stack_info->profile_notified) ? "Yes" : "No"),
 		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)",
 		   "CoexVer/ FwVer/ PatchVer",
 		   glcoex_ver_date_8192e_2ant, glcoex_ver_8192e_2ant,
 		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
 			   &wifi_dot11_chnl);
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d / %d(%d)",
 		   "Dot11 channel / HsMode(HsChnl)",
 		   wifi_dot11_chnl, bt_hson, wifi_hs_chnl);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %02x %02x %02x ",
 		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
 		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifirssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "Wifi rssi/ HS rssi", wifirssi, bt_hs_rssi);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d ",
 		   "Wifi link/ roam/ scan", link, roam, scan);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
 			   &wifi_traffic_dir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s / %s/ %s ",
 		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
 		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
 			(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
 		   ((!wifi_busy) ? "idle" :
 			((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
 				"uplink" : "downlink")));
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = [%s/ %d/ %d] ",
 		   "BT [status/ rssi/ retryCnt]",
 		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
 		    ((coex_sta->c2h_bt_inquiry_page) ?
@@ -3630,166 +3394,139 @@ void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
 			 ((BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
 			   coex_dm->bt_status) ? "connected-idle" : "busy")))),
 		   coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d / %d / %d / %d",
 		   "SCO/HID/PAN/A2DP", stack_info->sco_exist,
 		   stack_info->hid_exist, stack_info->pan_exist,
 		   stack_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s",
 		   "BT Info A2DP rate",
-		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
+		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
 
 	for (i = 0; i < BT_INFO_SRC_8192E_2ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 				   "\r\n %-35s = %02x %02x %02x %02x ",
-				   glbt_infosrc8192e2ant[i],
+				   GLBtInfoSrc8192e2Ant[i],
 				   coex_sta->bt_info_c2h[i][0],
 				   coex_sta->bt_info_c2h[i][1],
 				   coex_sta->bt_info_c2h[i][2],
 				   coex_sta->bt_info_c2h[i][3]);
-			CL_PRINTF(cli_buf);
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 				   "%02x %02x %02x(%d)",
 				   coex_sta->bt_info_c2h[i][4],
 				   coex_sta->bt_info_c2h[i][5],
 				   coex_sta->bt_info_c2h[i][6],
 				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
 		}
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s/%s",
 		   "PS state, IPS/LPS",
 		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
 		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
-	CL_PRINTF(cli_buf);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "SS Type",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x ", "SS Type",
 		   coex_dm->cur_sstype);
-	CL_PRINTF(cli_buf);
 
 	/* Sw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Sw mechanism] ============");
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+		   "============[Sw mechanism]============");
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d ",
 		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
 		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
 		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
 		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
 		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Rate Mask",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x ", "Rate Mask",
 		   btcoexist->bt_info.ra_mask);
-	CL_PRINTF(cli_buf);
 
 	/* Fw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Fw mechanism] ============");
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+		   "============[Fw mechanism]============");
 
 	ps_tdma_case = coex_dm->cur_ps_tdma;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
 		   "PS TDMA", coex_dm->ps_tdma_para[0],
 		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
 		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
 		   ps_tdma_case, coex_dm->auto_tdma_adjust);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d ",
 		   "DecBtPwr/ IgnWlanAct",
 		   coex_dm->cur_dec_bt_pwr, coex_dm->cur_ignore_wlan_act);
-	CL_PRINTF(cli_buf);
 
 	/* Hw setting */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Hw setting] ============");
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+		   "============[Hw setting]============");
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x",
 		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
 		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
 		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retrylimit,
 		   coex_dm->backup_ampdu_maxtime);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
 	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
 		   "0x430/0x434/0x42a/0x456",
 		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc04);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xd04);
 	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x90c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0xc04/ 0xd04/ 0x90c", u32tmp[0], u32tmp[1], u32tmp[2]);
-	CL_PRINTF(cli_buf);
 
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x", "0x778",
 		   u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x92c);
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x930);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x92c/ 0x930", (u8tmp[0]), u32tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
 	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x4f);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x40/ 0x4f", u8tmp[0], u8tmp[1]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x", "0xc50(dig)",
 		   u32tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
 	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
 		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
 		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x770(hp rx[31:16]/tx[15:0])",
 		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x774(lp rx[31:16]/tx[15:0])",
 		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
 #if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 1)
 	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
 #endif
@@ -3900,22 +3637,22 @@ void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 				       u8 *tmp_buf, u8 length)
 {
 	u8 bt_info = 0;
-	u8 i, rspsource = 0;
+	u8 i, rsp_source = 0;
 	bool bt_busy = false, limited_dig = false;
 	bool wifi_connected = false;
 
 	coex_sta->c2h_bt_info_req_sent = false;
 
-	rspsource = tmp_buf[0] & 0xf;
-	if (rspsource >= BT_INFO_SRC_8192E_2ANT_MAX)
-		rspsource = BT_INFO_SRC_8192E_2ANT_WIFI_FW;
-	coex_sta->bt_info_c2h_cnt[rspsource]++;
+	rsp_source = tmp_buf[0] & 0xf;
+	if (rsp_source >= BT_INFO_SRC_8192E_2ANT_MAX)
+		rsp_source = BT_INFO_SRC_8192E_2ANT_WIFI_FW;
+	coex_sta->bt_info_c2h_cnt[rsp_source]++;
 
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
-		  rspsource, length);
+		  "[BTCoex], Bt info[%d], length=%d, hex data = [",
+		  rsp_source, length);
 	for (i = 0; i < length; i++) {
-		coex_sta->bt_info_c2h[rspsource][i] = tmp_buf[i];
+		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
 		if (i == 1)
 			bt_info = tmp_buf[i];
 		if (i == length-1)
@@ -3926,19 +3663,20 @@ void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 				  "0x%02x, ", tmp_buf[i]);
 	}
 
-	if (BT_INFO_SRC_8192E_2ANT_WIFI_FW != rspsource) {
+	if (BT_INFO_SRC_8192E_2ANT_WIFI_FW != rsp_source) {
 		coex_sta->bt_retry_cnt =	/* [3:0] */
-			coex_sta->bt_info_c2h[rspsource][2] & 0xf;
+			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
 
 		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rspsource][3] * 2 + 10;
+			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
 
 		coex_sta->bt_info_ext =
-			coex_sta->bt_info_c2h[rspsource][4];
+			coex_sta->bt_info_c2h[rsp_source][4];
 
 		/* Here we need to resend some wifi info to BT
-		 * because bt is reset and loss of the info. */
-		if ((coex_sta->bt_info_ext & BIT(1))) {
+		 * because bt is reset and loss of the info.
+		 */
+		if ((coex_sta->bt_info_ext & BIT1)) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "bit1, send wifi BW&Chnl to BT!!\n");
 			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
@@ -3953,22 +3691,23 @@ void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 							BTC_MEDIA_DISCONNECT);
 		}
 
-		if ((coex_sta->bt_info_ext & BIT(3))) {
+		if ((coex_sta->bt_info_ext & BIT3)) {
 			if (!btcoexist->manual_control &&
 			    !btcoexist->stop_coex_dm) {
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 					  "bit3, BT NOT ignore Wlan active!\n");
-				halbtc8192e2ant_ignorewlanact(btcoexist,
+				halbtc8192e2ant_IgnoreWlanAct(btcoexist,
 							      FORCE_EXEC,
 							      false);
 			}
 		} else {
 			/* BT already NOT ignore Wlan active,
-			 * do nothing here. */
+			 * do nothing here.
+			 */
 		}
 
 #if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
-		if ((coex_sta->bt_info_ext & BIT(4))) {
+		if ((coex_sta->bt_info_ext & BIT4)) {
 			/* BT auto report already enabled, do nothing */
 		} else {
 			halbtc8192e2ant_bt_autoreport(btcoexist, FORCE_EXEC,
@@ -3977,7 +3716,7 @@ void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
 #endif
 	}
 
-	/* check BIT(2) first ==> check if bt is under inquiry or page scan */
+	/* check BIT2 first ==> check if bt is under inquiry or page scan */
 	if (bt_info & BT_INFO_8192E_2ANT_B_INQ_PAGE)
 		coex_sta->c2h_bt_inquiry_page = true;
 	else
@@ -4062,7 +3801,7 @@ void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
 {
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
 
-	halbtc8192e2ant_ignorewlanact(btcoexist, FORCE_EXEC, true);
+	halbtc8192e2ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, true);
 	ex_halbtc8192e2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
 }
 
@@ -4074,7 +3813,7 @@ void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "======================= Periodical =======================\n");
+		  "=======================Periodical=======================\n");
 	if (dis_ver_info_cnt <= 5) {
 		dis_ver_info_cnt += 1;
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
@@ -4101,7 +3840,7 @@ void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
 #if (BT_AUTO_REPORT_ONLY_8192E_2ANT == 0)
 	halbtc8192e2ant_querybt_info(btcoexist);
 	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
-	halbtc_monitor_bt_enable_disable(btcoexist);
+	btc8192e2ant_monitor_bt_enable_dis(btcoexist);
 #else
 	if (halbtc8192e2ant_iswifi_status_changed(btcoexist) ||
 	    coex_dm->auto_tdma_adjust)
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8192e2ant.h
index f10bf1be69fe..75e1f7d0db06 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.h
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8192e2ant.h
@@ -1,6 +1,32 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
 /*****************************************************************
  *   The following is for 8192E 2Ant BT Co-exist definition
  *****************************************************************/
+#define	BT_AUTO_REPORT_ONLY_8192E_2ANT			0
+
 #define	BT_INFO_8192E_2ANT_B_FTP			BIT7
 #define	BT_INFO_8192E_2ANT_B_A2DP			BIT6
 #define	BT_INFO_8192E_2ANT_B_HID			BIT5
@@ -12,14 +38,14 @@
 
 #define BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT		2
 
-enum bt_info_src_8192e_2ant{
+enum bt_info_src_8192e_2ant {
 	BT_INFO_SRC_8192E_2ANT_WIFI_FW			= 0x0,
 	BT_INFO_SRC_8192E_2ANT_BT_RSP			= 0x1,
 	BT_INFO_SRC_8192E_2ANT_BT_ACTIVE_SEND		= 0x2,
 	BT_INFO_SRC_8192E_2ANT_MAX
 };
 
-enum bt_8192e_2ant_bt_status{
+enum bt_8192e_2ant_bt_status {
 	BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
 	BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
 	BT_8192E_2ANT_BT_STATUS_INQ_PAGE		= 0x2,
@@ -29,7 +55,7 @@ enum bt_8192e_2ant_bt_status{
 	BT_8192E_2ANT_BT_STATUS_MAX
 };
 
-enum bt_8192e_2ant_coex_algo{
+enum bt_8192e_2ant_coex_algo {
 	BT_8192E_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
 	BT_8192E_2ANT_COEX_ALGO_SCO			= 0x1,
 	BT_8192E_2ANT_COEX_ALGO_SCO_PAN			= 0x2,
@@ -45,7 +71,7 @@ enum bt_8192e_2ant_coex_algo{
 	BT_8192E_2ANT_COEX_ALGO_MAX			= 0xc
 };
 
-struct coex_dm_8192e_2ant{
+struct coex_dm_8192e_2ant {
 	/* fw mechanism */
 	u8 pre_dec_bt_pwr;
 	u8 cur_dec_bt_pwr;
@@ -56,7 +82,7 @@ struct coex_dm_8192e_2ant{
 	u8 pre_ps_tdma;
 	u8 cur_ps_tdma;
 	u8 ps_tdma_para[5];
-	u8 ps_tdma_du_adj_type;
+	u8 tdma_adj_type;
 	bool reset_tdma_adjust;
 	bool auto_tdma_adjust;
 	bool pre_ps_tdma_on;
@@ -113,7 +139,7 @@ struct coex_dm_8192e_2ant{
 	u8 cur_ampdutime_type;
 };
 
-struct coex_sta_8192e_2ant{
+struct coex_sta_8192e_2ant {
 	bool bt_link_exist;
 	bool sco_exist;
 	bool a2dp_exist;
@@ -151,10 +177,9 @@ void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
 void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
 					      u8 type);
 void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpBuf, u8 length);
+				       u8 *tmpbuf, u8 length);
 void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
 					       u8 type);
 void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist);
 void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist);
 void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist);
-
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b1ant.c
index 153048f6b4de..c4acd403e5f6 100644
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b1ant.c
@@ -1,3 +1,28 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
 /***************************************************************
  * Description:
  *
@@ -20,7 +45,7 @@ static struct coex_dm_8723b_1ant *coex_dm = &glcoex_dm_8723b_1ant;
 static struct coex_sta_8723b_1ant glcoex_sta_8723b_1ant;
 static struct coex_sta_8723b_1ant *coex_sta = &glcoex_sta_8723b_1ant;
 
-static const char *const glb_infosrc8723b1ant[] = {
+static const char *const GLBtInfoSrc8723b1Ant[] = {
 	"BT Info[wifi fw]",
 	"BT Info[bt rsp]",
 	"BT Info[bt auto report]",
@@ -39,7 +64,7 @@ static u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 					u8 rssi_thresh1)
 {
 	s32 bt_rssi = 0;
-	u8 rssi_state = coex_sta->pre_bt_rssi_state;
+	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
 
 	bt_rssi = coex_sta->bt_rssi;
 
@@ -48,21 +73,21 @@ static u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
 			if (bt_rssi >= rssi_thresh +
 					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				rssi_state = BTC_RSSI_STATE_HIGH;
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to High\n");
 			} else {
-				rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at Low\n");
 			}
 		} else {
 			if (bt_rssi < rssi_thresh) {
-				rssi_state = BTC_RSSI_STATE_LOW;
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Low\n");
 			} else {
-				rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at High\n");
 			}
@@ -78,11 +103,11 @@ static u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
 			if (bt_rssi >= rssi_thresh +
 					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				rssi_state = BTC_RSSI_STATE_MEDIUM;
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Medium\n");
 			} else {
-				rssi_state = BTC_RSSI_STATE_STAY_LOW;
+				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at Low\n");
 			}
@@ -92,34 +117,34 @@ static u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 					BTC_RSSI_STATE_STAY_MEDIUM)) {
 			if (bt_rssi >= rssi_thresh1 +
 					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				rssi_state = BTC_RSSI_STATE_HIGH;
+				bt_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to High\n");
 			} else if (bt_rssi < rssi_thresh) {
-				rssi_state = BTC_RSSI_STATE_LOW;
+				bt_rssi_state = BTC_RSSI_STATE_LOW;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Low\n");
 			} else {
-				rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
+				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at Medium\n");
 			}
 		} else {
 			if (bt_rssi < rssi_thresh1) {
-				rssi_state = BTC_RSSI_STATE_MEDIUM;
+				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Medium\n");
 			} else {
-				rssi_state = BTC_RSSI_STATE_STAY_HIGH;
+				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at High\n");
 			}
 		}
 	}
 
-	coex_sta->pre_bt_rssi_state = rssi_state;
+	coex_sta->pre_bt_rssi_state = bt_rssi_state;
 
-	return rssi_state;
+	return bt_rssi_state;
 }
 
 static u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
@@ -238,8 +263,8 @@ static void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
 	coex_dm->prera_mask = coex_dm->curra_mask;
 }
 
-static void auto_rate_fallback_retry(struct btc_coexist *btcoexist,
-				     bool force_exec, u8 type)
+static void btc8723b1ant_auto_rate_fb_retry(struct btc_coexist *btcoexist,
+					    bool force_exec, u8 type)
 {
 	bool wifi_under_bmode = false;
 
@@ -309,15 +334,15 @@ static void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
 		coex_dm->cur_ampdu_time_type)) {
 		switch (coex_dm->cur_ampdu_time_type) {
 		case 0:	/* normal mode */
-			btcoexist->btc_write_1byte(btcoexist, 0x456,
-					coex_dm->backup_ampdu_max_time);
-			break;
+				btcoexist->btc_write_1byte(btcoexist, 0x456,
+						coex_dm->backup_ampdu_max_time);
+				break;
 		case 1:	/* AMPDU timw = 0x38 * 32us */
-			btcoexist->btc_write_1byte(btcoexist,
-						   0x456, 0x38);
-			break;
+				btcoexist->btc_write_1byte(btcoexist,
+							   0x456, 0x38);
+				break;
 		default:
-			break;
+				break;
 		}
 	}
 
@@ -346,17 +371,18 @@ static void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
 		break;
 	}
 
-	auto_rate_fallback_retry(btcoexist, force_exec, arfr_type);
+	btc8723b1ant_auto_rate_fb_retry(btcoexist, force_exec, arfr_type);
 	halbtc8723b1ant_retry_limit(btcoexist, force_exec, retry_limit_type);
 	halbtc8723b1ant_ampdu_maxtime(btcoexist, force_exec, ampdu_time_type);
 }
 
 static void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
 				       bool force_exec, bool rej_ap_agg_pkt,
-				bool b_bt_ctrl_agg_buf_size, u8 agg_buf_size)
+				       bool bt_ctrl_agg_buf_size,
+				       u8 agg_buf_size)
 {
 	bool reject_rx_agg = rej_ap_agg_pkt;
-	bool bt_ctrl_rx_agg_size = b_bt_ctrl_agg_buf_size;
+	bool bt_ctrl_rx_agg_size = bt_ctrl_agg_buf_size;
 	u8 rxaggsize = agg_buf_size;
 
 	/**********************************************
@@ -368,7 +394,8 @@ static void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE,
 			   &bt_ctrl_rx_agg_size);
 	/* aggregation buf size, only work
-	 *when BT control Rx aggregation size.  */
+	 * when BT control Rx aggregation size.
+	 */
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rxaggsize);
 	/* real update aggregation setting  */
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
@@ -406,7 +433,7 @@ static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
 
 	coex_sta->c2h_bt_info_req_sent = true;
 
-	h2c_parameter[0] |= BIT(0);	/* trigger*/
+	h2c_parameter[0] |= BIT0;	/* trigger*/
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
@@ -415,7 +442,7 @@ static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
 
-static bool is_wifi_status_changed(struct btc_coexist *btcoexist)
+static bool btc8723b1ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
 {
 	static bool pre_wifi_busy;
 	static bool pre_under_4way, pre_bt_hs_on;
@@ -500,7 +527,7 @@ static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool bt_hs_on = false;
 	u8 algorithm = BT_8723B_1ANT_COEX_ALGO_UNDEFINED;
-	u8 numofdiffprofile = 0;
+	u8 numdiffprofile = 0;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 
@@ -511,15 +538,15 @@ static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 	}
 
 	if (bt_link_info->sco_exist)
-		numofdiffprofile++;
+		numdiffprofile++;
 	if (bt_link_info->hid_exist)
-		numofdiffprofile++;
+		numdiffprofile++;
 	if (bt_link_info->pan_exist)
-		numofdiffprofile++;
+		numdiffprofile++;
 	if (bt_link_info->a2dp_exist)
-		numofdiffprofile++;
+		numdiffprofile++;
 
-	if (numofdiffprofile == 1) {
+	if (numdiffprofile == 1) {
 		if (bt_link_info->sco_exist) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], BT Profile = SCO only\n");
@@ -547,7 +574,7 @@ static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 				}
 			}
 		}
-	} else if (numofdiffprofile == 2) {
+	} else if (numdiffprofile == 2) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist) {
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -603,7 +630,7 @@ static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 				}
 			}
 		}
-	} else if (numofdiffprofile == 3) {
+	} else if (numdiffprofile == 3) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist &&
 			    bt_link_info->a2dp_exist) {
@@ -653,7 +680,7 @@ static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 				}
 			}
 		}
-	} else if (numofdiffprofile >= 3) {
+	} else if (numdiffprofile >= 3) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist &&
 			    bt_link_info->pan_exist &&
@@ -663,7 +690,7 @@ static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 						  "[BTCoex], Error!!! BT Profile = SCO + HID + A2DP + PAN(HS)\n");
 				} else {
 					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
+						  "[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n");
 					algorithm =
 					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
 				}
@@ -674,15 +701,15 @@ static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
 	return algorithm;
 }
 
-static void set_sw_penalty_tx_rate_adapt(struct btc_coexist *btcoexist,
-					 bool low_penalty_ra)
+static void btc8723b1ant_set_sw_pen_tx_rate_adapt(struct btc_coexist *btcoexist,
+						  bool low_penalty_ra)
 {
 	u8 h2c_parameter[6] = {0};
 
-	h2c_parameter[0] = 0x6;	/* opCode, 0x6 = Retry_Penalty */
+	h2c_parameter[0] = 0x6;	/* opCode, 0x6= Retry_Penalty */
 
 	if (low_penalty_ra) {
-		h2c_parameter[1] |= BIT(0);
+		h2c_parameter[1] |= BIT0;
 		/*normal rate except MCS7/6/5, OFDM54/48/36 */
 		h2c_parameter[2] = 0x00;
 		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54 */
@@ -706,7 +733,8 @@ static void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
 		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
 			return;
 	}
-	set_sw_penalty_tx_rate_adapt(btcoexist, coex_dm->cur_low_penalty_ra);
+	btc8723b1ant_set_sw_pen_tx_rate_adapt(btcoexist,
+					      coex_dm->cur_low_penalty_ra);
 
 	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
 }
@@ -762,8 +790,8 @@ static void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-static void coex_table_with_type(struct btc_coexist *btcoexist,
-				 bool force_exec, u8 type)
+static void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
+						 bool force_exec, u8 type)
 {
 	switch (type) {
 	case 0:
@@ -803,13 +831,13 @@ static void coex_table_with_type(struct btc_coexist *btcoexist,
 	}
 }
 
-static void set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
-				   bool enable)
+static void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
+					       bool enable)
 {
 	u8 h2c_parameter[1] = {0};
 
 	if (enable)
-		h2c_parameter[0] |= BIT(0);	/* function enable */
+		h2c_parameter[0] |= BIT0;	/* function enable */
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
@@ -836,7 +864,7 @@ static void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 		    coex_dm->cur_ignore_wlan_act)
 			return;
 	}
-	set_fw_ignore_wlan_act(btcoexist, enable);
+	halbtc8723b1ant_SetFwIgnoreWlanAct(btcoexist, enable);
 
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
@@ -853,14 +881,14 @@ static void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
 			   &ap_enable);
 
 	if (ap_enable) {
-		if ((byte1 & BIT(4)) && !(byte1 & BIT(5))) {
+		if ((byte1 & BIT4) && !(byte1 & BIT5)) {
 			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 				  "[BTCoex], FW for 1Ant AP mode\n");
-			real_byte1 &= ~BIT(4);
-			real_byte1 |= BIT(5);
+			real_byte1 &= ~BIT4;
+			real_byte1 |= BIT5;
 
-			real_byte5 |= BIT(5);
-			real_byte5 &= ~BIT(6);
+			real_byte5 |= BIT5;
+			real_byte5 &= ~BIT6;
 		}
 	}
 
@@ -877,7 +905,7 @@ static void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
 	coex_dm->ps_tdma_para[4] = real_byte5;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], PS-TDMA H2C cmd = 0x%x%08x\n",
+		  "[BTCoex], PS-TDMA H2C cmd =0x%x%08x\n",
 		  h2c_parameter[0],
 		  h2c_parameter[1] << 24 |
 		  h2c_parameter[2] << 16 |
@@ -897,7 +925,7 @@ static void halbtc8723b1ant_set_lps_rpwm(struct btc_coexist *btcoexist,
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_RPWM_VAL, &rpwm);
 }
 
-static void halbtc8723b1ant_lpsrpwm(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist,
 				    bool force_exec,
 				    u8 lps_val, u8 rpwm_val)
 {
@@ -909,13 +937,13 @@ static void halbtc8723b1ant_lpsrpwm(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], LPS-RxBeaconMode = 0x%x, LPS-RPWM = 0x%x!!\n",
+			  "[BTCoex], LPS-RxBeaconMode = 0x%x , LPS-RPWM = 0x%x!!\n",
 			  coex_dm->cur_lps, coex_dm->cur_rpwm);
 
 		if ((coex_dm->pre_lps == coex_dm->cur_lps) &&
 		    (coex_dm->pre_rpwm == coex_dm->cur_rpwm)) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], LPS-RPWM_Last = 0x%x, LPS-RPWM_Now = 0x%x!!\n",
+				  "[BTCoex], LPS-RPWM_Last = 0x%x , LPS-RPWM_Now = 0x%x!!\n",
 				  coex_dm->pre_rpwm, coex_dm->cur_rpwm);
 
 			return;
@@ -936,11 +964,11 @@ static void halbtc8723b1ant_sw_mechanism(struct btc_coexist *btcoexist,
 	halbtc8723b1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
 }
 
-static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
+static void halbtc8723b1ant_SetAntPath(struct btc_coexist *btcoexist,
 				       u8 ant_pos_type, bool init_hw_cfg,
 				bool wifi_off)
 {
-	struct btc_board_info *brd_info = &btcoexist->board_info;
+	struct btc_board_info *board_info = &btcoexist->board_info;
 	u32 fw_ver = 0, u32tmp = 0;
 	bool pg_ext_switch = false;
 	bool use_ext_switch = false;
@@ -966,22 +994,25 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
 
 		/* 0x4c[24:23] = 00, Set Antenna control by BT_RFE_CTRL
-		 * BT Vendor 0xac = 0xf002 */
+		 * BT Vendor 0xac = 0xf002
+		 */
 		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u32tmp &= ~BIT(23);
-		u32tmp &= ~BIT(24);
+		u32tmp &= ~BIT23;
+		u32tmp &= ~BIT24;
 		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
 	}
 
 	if (use_ext_switch) {
 		if (init_hw_cfg) {
-			/* 0x4c[23] = 0, 0x4c[24] = 1  Antenna ctrl by WL/BT */
+			/* 0x4c[23] = 0, 0x4c[24] = 1
+			 *	Antenna control by WL/BT
+			 */
 			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-			u32tmp &= ~BIT(23);
-			u32tmp |= BIT(24);
+			u32tmp &= ~BIT23;
+			u32tmp |= BIT24;
 			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
 
-			if (brd_info->btdm_ant_pos ==
+			if (board_info->btdm_ant_pos ==
 			    BTC_ANTENNA_AT_MAIN_PORT) {
 				/* Main Ant to  BT for IPS case 0x4c[23] = 1 */
 				btcoexist->btc_write_1byte_bitmask(btcoexist,
@@ -1009,7 +1040,7 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 
 		/* fixed internal switch first*/
 		/* fixed internal switch S1->WiFi, S0->BT*/
-		if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
 			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
 		else/* fixed internal switch S0->WiFi, S1->BT*/
 			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
@@ -1017,7 +1048,8 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 		/* ext switch setting */
 		switch (ant_pos_type) {
 		case BTC_ANT_PATH_WIFI:
-			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT)
 				btcoexist->btc_write_1byte_bitmask(btcoexist,
 								   0x92c, 0x3,
 								   0x1);
@@ -1027,7 +1059,8 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 								   0x2);
 			break;
 		case BTC_ANT_PATH_BT:
-			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT)
 				btcoexist->btc_write_1byte_bitmask(btcoexist,
 								   0x92c, 0x3,
 								   0x2);
@@ -1038,7 +1071,8 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 			break;
 		default:
 		case BTC_ANT_PATH_PTA:
-			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			if (board_info->btdm_ant_pos ==
+			    BTC_ANTENNA_AT_MAIN_PORT)
 				btcoexist->btc_write_1byte_bitmask(btcoexist,
 								   0x92c, 0x3,
 								   0x1);
@@ -1053,11 +1087,11 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 		if (init_hw_cfg) {
 			/* 0x4c[23] = 1, 0x4c[24] = 0  Antenna control by 0x64*/
 			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-			u32tmp |= BIT(23);
-			u32tmp &= ~BIT(24);
+			u32tmp |= BIT23;
+			u32tmp &= ~BIT24;
 			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
 
-			if (brd_info->btdm_ant_pos ==
+			if (board_info->btdm_ant_pos ==
 			    BTC_ANTENNA_AT_MAIN_PORT) {
 				/*Main Ant to  WiFi for IPS case 0x4c[23] = 1*/
 				btcoexist->btc_write_1byte_bitmask(btcoexist,
@@ -1085,7 +1119,8 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 
 		/* fixed external switch first*/
 		/*Main->WiFi, Aux->BT*/
-		if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+		if (board_info->btdm_ant_pos ==
+			BTC_ANTENNA_AT_MAIN_PORT)
 			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
 							   0x3, 0x1);
 		else/*Main->BT, Aux->WiFi */
@@ -1095,7 +1130,8 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 		/* internal switch setting*/
 		switch (ant_pos_type) {
 		case BTC_ANT_PATH_WIFI:
-			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			if (board_info->btdm_ant_pos ==
+				BTC_ANTENNA_AT_MAIN_PORT)
 				btcoexist->btc_write_2byte(btcoexist, 0x948,
 							   0x0);
 			else
@@ -1103,7 +1139,8 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 							   0x280);
 			break;
 		case BTC_ANT_PATH_BT:
-			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			if (board_info->btdm_ant_pos ==
+				BTC_ANTENNA_AT_MAIN_PORT)
 				btcoexist->btc_write_2byte(btcoexist, 0x948,
 							   0x280);
 			else
@@ -1112,7 +1149,8 @@ static void halbtc8723b1ant_setantpath(struct btc_coexist *btcoexist,
 			break;
 		default:
 		case BTC_ANT_PATH_PTA:
-			if (brd_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
+			if (board_info->btdm_ant_pos ==
+				BTC_ANTENNA_AT_MAIN_PORT)
 				btcoexist->btc_write_2byte(btcoexist, 0x948,
 							   0x200);
 			else
@@ -1303,9 +1341,10 @@ static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x12,
 						       0x3, 0x14, 0x50);
 			break;
-		/* SoftAP only with no sta associated, BT disable ,
+		/* SoftAP only with no sta associated,BT disable ,
 		 * TDMA mode for power saving
-		 * here softap mode screen off will cost 70-80mA for phone */
+		 * here softap mode screen off will cost 70-80mA for phone
+		 */
 		case 40:
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x23, 0x18,
 						       0x00, 0x10, 0x24);
@@ -1316,20 +1355,20 @@ static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
 		case 8: /*PTA Control */
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x8, 0x0,
 						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_PTA,
+			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_PTA,
 						   false, false);
 			break;
 		case 0:
 		default:  /*Software control, Antenna at BT side */
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
 						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT,
+			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT,
 						   false, false);
 			break;
 		case 9:   /*Software control, Antenna at WiFi side */
 			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
 						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_WIFI,
+			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_WIFI,
 						   false, false);
 			break;
 		}
@@ -1384,7 +1423,7 @@ static bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
 		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE !=
 		    coex_dm->bt_status)) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi non connected-idle + BT Busy!!\n");
+			  ("[BTCoex], Wifi non connected-idle + BT Busy!!\n"));
 		halbtc8723b1ant_sw_mechanism(btcoexist, false);
 		commom = true;
 	} else {
@@ -1401,12 +1440,13 @@ static bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
 	return commom;
 }
 
-static void tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
-					 u8 wifi_status)
+static void btc8723b1ant_tdma_dur_adj_for_acl(struct btc_coexist *btcoexist,
+					      u8 wifi_status)
 {
 	static s32 up, dn, m, n, wait_count;
 	/* 0: no change, +1: increase WiFi duration,
-	 * -1: decrease WiFi duration */
+	 * -1: decrease WiFi duration
+	 */
 	s32 result;
 	u8 retry_count = 0, bt_info_ext;
 	bool wifi_busy = false;
@@ -1427,7 +1467,7 @@ static void tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
 		    coex_dm->cur_ps_tdma != 3 && coex_dm->cur_ps_tdma != 9) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 9);
-			coex_dm->ps_tdma_du_adj_type = 9;
+			coex_dm->tdma_adj_type = 9;
 
 			up = 0;
 			dn = 0;
@@ -1445,7 +1485,7 @@ static void tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
 			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
 
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
-		coex_dm->ps_tdma_du_adj_type = 2;
+		coex_dm->tdma_adj_type = 2;
 
 		up = 0;
 		dn = 0;
@@ -1526,19 +1566,19 @@ static void tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
 			     (coex_dm->cur_ps_tdma == 2))) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 1) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
+				coex_dm->tdma_adj_type = 2;
 			} else if (coex_dm->cur_ps_tdma == 2) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 9) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 11);
-				coex_dm->ps_tdma_du_adj_type = 11;
+				coex_dm->tdma_adj_type = 11;
 			}
 		} else if (result == 1) {
 			if ((BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
@@ -1546,21 +1586,22 @@ static void tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
 			     (coex_dm->cur_ps_tdma == 2))) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 11) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 9) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
+				coex_dm->tdma_adj_type = 2;
 			} else if (coex_dm->cur_ps_tdma == 2) {
 				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 							true, 1);
-				coex_dm->ps_tdma_du_adj_type = 1;
+				coex_dm->tdma_adj_type = 1;
 			}
 		} else {	  /*no change */
+			/*if busy / idle change */
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 				  "[BTCoex],********* TDMA(on, %d) ********\n",
 				  coex_dm->cur_ps_tdma);
@@ -1570,13 +1611,13 @@ static void tdma_duration_adjust_for_acl(struct btc_coexist *btcoexist,
 		    coex_dm->cur_ps_tdma != 9 && coex_dm->cur_ps_tdma != 11) {
 			/* recover to previous adjust type */
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
-						coex_dm->ps_tdma_du_adj_type);
+						coex_dm->tdma_adj_type);
 		}
 	}
 }
 
-static void pstdmacheckforpowersavestate(struct btc_coexist *btcoexist,
-					 bool new_ps_state)
+static void btc8723b1ant_pstdmachkpwrsave(struct btc_coexist *btcoexist,
+					  bool new_ps_state)
 {
 	u8 lps_mode = 0x0;
 
@@ -1616,8 +1657,8 @@ static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
 		break;
 	case BTC_PS_LPS_ON:
-		pstdmacheckforpowersavestate(btcoexist, true);
-		halbtc8723b1ant_lpsrpwm(btcoexist, NORMAL_EXEC, lps_val,
+		btc8723b1ant_pstdmachkpwrsave(btcoexist, true);
+		halbtc8723b1ant_LpsRpwm(btcoexist, NORMAL_EXEC, lps_val,
 					rpwm_val);
 		/* when coex force to enter LPS, do not enter 32k low power. */
 		low_pwr_disable = true;
@@ -1627,7 +1668,7 @@ static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
 		break;
 	case BTC_PS_LPS_OFF:
-		pstdmacheckforpowersavestate(btcoexist, false);
+		btc8723b1ant_pstdmachkpwrsave(btcoexist, false);
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
 		break;
 	default:
@@ -1685,7 +1726,7 @@ static void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 }
 
 /* HID+A2DP+PAN(EDR) */
-static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+static void btc8723b1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 {
 	halbtc8723b1ant_sw_mechanism(btcoexist, true);
 }
@@ -1706,13 +1747,13 @@ static void halbtc8723b1ant_action_wifi_multiport(struct btc_coexist *btcoexist)
 					 0x0, 0x0);
 
 	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 }
 
 static void halbtc8723b1ant_action_hs(struct btc_coexist *btcoexist)
 {
 	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 }
 
 static void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
@@ -1729,13 +1770,13 @@ static void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 		halbtc8723b1ant_power_save_state(btcoexist,
 						 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	} else if (bt_link_info->sco_exist || bt_link_info->hid_only) {
 		/* SCO/HID-only busy */
 		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 						 0x0, 0x0);
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	} else {
 		if (ap_enable)
 			halbtc8723b1ant_power_save_state(btcoexist,
@@ -1747,12 +1788,12 @@ static void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 							 0x50, 0x4);
 
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 30);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	}
 }
 
-static void action_bt_sco_hid_only_busy(struct btc_coexist *btcoexist,
-					u8 wifi_status)
+static void btc8723b1ant_act_bt_sco_hid_only_busy(struct btc_coexist *btcoexist,
+						  u8 wifi_status)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_connected = false;
@@ -1764,10 +1805,10 @@ static void action_bt_sco_hid_only_busy(struct btc_coexist *btcoexist,
 
 	if (bt_link_info->sco_exist) {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	} else { /* HID */
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 6);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 5);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 5);
 	}
 }
 
@@ -1775,35 +1816,40 @@ static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 					struct btc_coexist *btcoexist,
 					u8 wifi_status)
 {
+	u8 bt_rssi_state;
+
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	u8 rssi_state;
 
-	rssi_state = halbtc8723b1ant_bt_rssi_state(2, 28, 0);
+	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 28, 0);
 
 	if (bt_link_info->hid_only) {  /*HID */
-		action_bt_sco_hid_only_busy(btcoexist, wifi_status);
+		btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist, wifi_status);
 		coex_dm->auto_tdma_adjust = false;
 		return;
 	} else if (bt_link_info->a2dp_only) { /*A2DP */
 		if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE == wifi_status) {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						8);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 8);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
 			coex_dm->auto_tdma_adjust = false;
-		} else if ((rssi_state == BTC_RSSI_STATE_HIGH) ||
-			   (rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			tdma_duration_adjust_for_acl(btcoexist, wifi_status);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		} else if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+			   (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8723b1ant_tdma_dur_adj_for_acl(btcoexist,
+							  wifi_status);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
 		} else { /*for low BT RSSI */
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 11);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
 			coex_dm->auto_tdma_adjust = false;
 		}
 	} else if (bt_link_info->hid_exist &&
-		   bt_link_info->a2dp_exist) { /*HID+A2DP */
-		if ((rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			bt_link_info->a2dp_exist) { /*HID+A2DP */
+		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 14);
 			coex_dm->auto_tdma_adjust = false;
@@ -1813,28 +1859,28 @@ static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
 			coex_dm->auto_tdma_adjust = false;
 		}
 
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
-	 /*PAN(OPP, FTP), HID+PAN(OPP, FTP) */
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
+	 /*PAN(OPP,FTP), HID+PAN(OPP,FTP) */
 	} else if (bt_link_info->pan_only ||
 		   (bt_link_info->hid_exist && bt_link_info->pan_exist)) {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
 		coex_dm->auto_tdma_adjust = false;
-	 /*A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP)*/
+	 /*A2DP+PAN(OPP,FTP), HID+A2DP+PAN(OPP,FTP)*/
 	} else if ((bt_link_info->a2dp_exist && bt_link_info->pan_exist) ||
 		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
 		    bt_link_info->pan_exist)) {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 		coex_dm->auto_tdma_adjust = false;
 	} else {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 		coex_dm->auto_tdma_adjust = false;
 	}
 }
 
-static void action_wifi_not_connected(struct btc_coexist *btcoexist)
+static void btc8723b1ant_action_wifi_not_conn(struct btc_coexist *btcoexist)
 {
 	/* power save state */
 	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
@@ -1842,60 +1888,64 @@ static void action_wifi_not_connected(struct btc_coexist *btcoexist)
 
 	/* tdma and coex table */
 	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-static void action_wifi_not_connected_scan(struct btc_coexist *btcoexist)
+static void btc8723b1ant_action_wifi_not_conn_scan(struct btc_coexist *btcoex)
 {
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+	struct btc_bt_link_info *bt_link_info = &btcoex->bt_link_info;
+
+	halbtc8723b1ant_power_save_state(btcoex, BTC_PS_WIFI_NATIVE,
 					 0x0, 0x0);
 
 	/* tdma and coex table */
 	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
 		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+			halbtc8723b1ant_ps_tdma(btcoex, NORMAL_EXEC,
 						true, 22);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8723b1ant_coex_table_with_type(btcoex,
+							     NORMAL_EXEC, 1);
 		} else if (bt_link_info->pan_only) {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+			halbtc8723b1ant_ps_tdma(btcoex, NORMAL_EXEC,
 						true, 20);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+			halbtc8723b1ant_coex_table_with_type(btcoex,
+							     NORMAL_EXEC, 2);
 		} else {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+			halbtc8723b1ant_ps_tdma(btcoex, NORMAL_EXEC,
 						true, 20);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8723b1ant_coex_table_with_type(btcoex,
+							     NORMAL_EXEC, 1);
 		}
 	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
 		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-		    coex_dm->bt_status)) {
-		action_bt_sco_hid_only_busy(btcoexist,
+		    coex_dm->bt_status)){
+		btc8723b1ant_act_bt_sco_hid_only_busy(btcoex,
 				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
 	} else {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_ps_tdma(btcoex, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_coex_table_with_type(btcoex, NORMAL_EXEC, 2);
 	}
 }
 
-static void action_wifi_not_connected_asso_auth(struct btc_coexist *btcoexist)
+static void btc8723b1ant_act_wifi_not_conn_asso_auth(struct btc_coexist *btcoex)
 {
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	struct btc_bt_link_info *bt_link_info = &btcoex->bt_link_info;
 
-	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+	halbtc8723b1ant_power_save_state(btcoex, BTC_PS_WIFI_NATIVE,
 					 0x0, 0x0);
 
 	if ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) ||
 	    (bt_link_info->sco_exist) || (bt_link_info->hid_only) ||
 	    (bt_link_info->a2dp_only) || (bt_link_info->pan_only)) {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+		halbtc8723b1ant_ps_tdma(btcoex, NORMAL_EXEC, false, 8);
+		halbtc8723b1ant_coex_table_with_type(btcoex, NORMAL_EXEC, 7);
 	} else {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_ps_tdma(btcoex, NORMAL_EXEC, true, 20);
+		halbtc8723b1ant_coex_table_with_type(btcoex, NORMAL_EXEC, 1);
 	}
 }
 
-static void action_wifi_connected_scan(struct btc_coexist *btcoexist)
+static void btc8723b1ant_action_wifi_conn_scan(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 
@@ -1907,28 +1957,32 @@ static void action_wifi_connected_scan(struct btc_coexist *btcoexist)
 		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 22);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
 		} else if (bt_link_info->pan_only) {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 20);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
 		} else {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						true, 20);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
 		}
 	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
 		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
 		    coex_dm->bt_status)) {
-		action_bt_sco_hid_only_busy(btcoexist,
+		btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist,
 				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
 	} else {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 	}
 }
 
-static void action_wifi_connected_special_packet(struct btc_coexist *btcoexist)
+static void halbtc8723b1ant_action_wifi_connected_special_packet(
+						struct btc_coexist *btcoexist)
 {
 	bool hs_connecting = false;
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
@@ -1943,10 +1997,10 @@ static void action_wifi_connected_special_packet(struct btc_coexist *btcoexist)
 	    (bt_link_info->sco_exist) || (bt_link_info->hid_only) ||
 	    (bt_link_info->a2dp_only) || (bt_link_info->pan_only)) {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
 	} else {
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	}
 }
 
@@ -1957,12 +2011,12 @@ static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	bool under_4way = false, ap_enable = false;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], CoexForWifiConnect() ===>\n");
+		  "[BTCoex], CoexForWifiConnect()===>\n");
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
 			   &under_4way);
 	if (under_4way) {
-		action_wifi_connected_special_packet(btcoexist);
+		halbtc8723b1ant_action_wifi_connected_special_packet(btcoexist);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n");
 		return;
@@ -1974,9 +2028,10 @@ static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 
 	if (scan || link || roam) {
 		if (scan)
-			action_wifi_connected_scan(btcoexist);
+			btc8723b1ant_action_wifi_conn_scan(btcoexist);
 		else
-			action_wifi_connected_special_packet(btcoexist);
+			halbtc8723b1ant_action_wifi_connected_special_packet(
+								     btcoexist);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n");
 		return;
@@ -2010,31 +2065,34 @@ static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 						coex_dm->bt_status) ||
 			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
 						coex_dm->bt_status)) {
-			action_bt_sco_hid_only_busy(btcoexist,
+			btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist,
 				     BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
 		} else {
 			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						false, 8);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
 		}
 	} else {
 		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
 			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
 				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
 		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
-			    coex_dm->bt_status) ||
+			   coex_dm->bt_status) ||
 			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
 			    coex_dm->bt_status)) {
-			action_bt_sco_hid_only_busy(btcoexist,
+			btc8723b1ant_act_bt_sco_hid_only_busy(btcoexist,
 				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
 		} else {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-			coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 8);
+			halbtc8723b1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
 		}
 	}
 }
 
-static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+static void btc8723b1ant_run_sw_coex_mech(struct btc_coexist *btcoexist)
 {
 	u8 algorithm = 0;
 
@@ -2086,7 +2144,7 @@ static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
 		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Action algorithm = HID+A2DP+PAN.\n");
-			action_hid_a2dp_pan_edr(btcoexist);
+			btc8723b1ant_action_hid_a2dp_pan_edr(btcoexist);
 			break;
 		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -2107,14 +2165,14 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_connected = false, bt_hs_on = false;
 	bool increase_scan_dev_num = false;
-	bool b_bt_ctrl_agg_buf_size = false;
+	bool bt_ctrl_agg_buf_size = false;
 	u8 agg_buf_size = 5;
 	u8 wifi_rssi_state = BTC_RSSI_STATE_HIGH;
 	u32 wifi_link_status = 0;
 	u32 num_of_wifi_link = 0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], RunCoexistMechanism() ===>\n");
+		  "[BTCoex], RunCoexistMechanism()===>\n");
 
 	if (btcoexist->manual_control) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -2136,8 +2194,9 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 
 	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
 	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status))
+	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
 		increase_scan_dev_num = true;
+	}
 
 	btcoexist->btc_set(btcoexist, BTC_SET_BL_INC_SCAN_DEV_NUM,
 			   &increase_scan_dev_num);
@@ -2151,7 +2210,7 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	if (num_of_wifi_link >= 2) {
 		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
 		halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
-					   b_bt_ctrl_agg_buf_size,
+					   bt_ctrl_agg_buf_size,
 					   agg_buf_size);
 		halbtc8723b1ant_action_wifi_multiport(btcoexist);
 		return;
@@ -2181,19 +2240,19 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 
 	if (bt_link_info->sco_exist) {
-		b_bt_ctrl_agg_buf_size = true;
+		bt_ctrl_agg_buf_size = true;
 		agg_buf_size = 0x3;
 	} else if (bt_link_info->hid_exist) {
-		b_bt_ctrl_agg_buf_size = true;
+		bt_ctrl_agg_buf_size = true;
 		agg_buf_size = 0x5;
 	} else if (bt_link_info->a2dp_exist || bt_link_info->pan_exist) {
-		b_bt_ctrl_agg_buf_size = true;
+		bt_ctrl_agg_buf_size = true;
 		agg_buf_size = 0x8;
 	}
 	halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
-				   b_bt_ctrl_agg_buf_size, agg_buf_size);
+				   bt_ctrl_agg_buf_size, agg_buf_size);
 
-	run_sw_coexist_mechanism(btcoexist);
+	btc8723b1ant_run_sw_coex_mech(btcoexist);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 
@@ -2217,11 +2276,13 @@ static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 
 		if (scan || link || roam) {
 			if (scan)
-				action_wifi_not_connected_scan(btcoexist);
+				btc8723b1ant_action_wifi_not_conn_scan(
+								     btcoexist);
 			else
-				action_wifi_not_connected_asso_auth(btcoexist);
+				btc8723b1ant_act_wifi_not_conn_asso_auth(
+								     btcoexist);
 		} else {
-			action_wifi_not_connected(btcoexist);
+			btc8723b1ant_action_wifi_not_conn(btcoexist);
 		}
 	} else { /* wifi LPS/Busy */
 		halbtc8723b1ant_action_wifi_connected(btcoexist);
@@ -2234,10 +2295,11 @@ static void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_sw_mechanism(btcoexist, false);
 
 	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
-	coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
 }
 
-static void init_hw_config(struct btc_coexist *btcoexist, bool backup)
+static void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist,
+					   bool backup)
 {
 	u32 u32tmp = 0;
 	u8 u8tmp = 0;
@@ -2270,14 +2332,14 @@ static void init_hw_config(struct btc_coexist *btcoexist, bool backup)
 	while (cnt_bt_cal_chk <= 20) {
 		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x49d);
 		cnt_bt_cal_chk++;
-		if (u32tmp & BIT(0)) {
+		if (u32tmp & BIT0) {
 			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-				  "[BTCoex], ########### BT calibration(cnt =%d) ###########\n",
+				  "[BTCoex], ########### BT calibration(cnt=%d) ###########\n",
 				  cnt_bt_cal_chk);
 			mdelay(50);
 		} else {
 			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-				  "[BTCoex], ********** BT NOT calibration (cnt =%d)**********\n",
+				  "[BTCoex], ********** BT NOT calibration (cnt=%d)**********\n",
 				  cnt_bt_cal_chk);
 			break;
 		}
@@ -2290,15 +2352,15 @@ static void init_hw_config(struct btc_coexist *btcoexist, bool backup)
 	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
 
 	/* Enable counter statistics */
-	/*0x76e[3] = 1, WLAN_Act control by PTA */
+	/*0x76e[3] =1, WLAN_Act control by PTA */
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x1);
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
 
 	/*Antenna config */
-	halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_PTA, true, false);
+	halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_PTA, true, false);
 	/* PTA parameter */
-	coex_table_with_type(btcoexist, FORCE_EXEC, 0);
+	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
 }
 
 static void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
@@ -2316,7 +2378,7 @@ static void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
 
 void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist)
 {
-	init_hw_config(btcoexist, true);
+	halbtc8723b1ant_init_hw_config(btcoexist, true);
 }
 
 void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
@@ -2333,11 +2395,11 @@ void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
 
 void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 {
-	struct btc_board_info *brd_info = &btcoexist->board_info;
+	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	u8 *cli_buf = btcoexist->cli_buf;
-	u8 u8tmp[4], i, bt_info_ext, stdmacase = 0;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
+	u8 u8tmp[4], i, bt_info_ext, pstdmacase = 0;
 	u16 u16tmp[4];
 	u32 u32tmp[4];
 	bool roam = false, scan = false;
@@ -2348,138 +2410,123 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 	u8 wifi_dot11_chnl, wifi_hs_chnl;
 	u32 fw_ver = 0, bt_patch_ver = 0;
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n ============[BT Coexist info] ============");
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n ============[BT Coexist info]============");
 
 	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ============[Under Manual Control] ==========");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n ============[Under Manual Control]==========");
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
 	}
 	if (btcoexist->stop_coex_dm) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ============[Coex is STOPPED] ============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n ============[Coex is STOPPED]============");
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
 	}
 
-	if (!brd_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
+	if (!board_info->bt_exist) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n BT not exists !!!");
 		return;
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
-		   "Ant PG Num/ Ant Mech/ Ant Pos: ",
-		   brd_info->pg_ant_num, brd_info->btdm_ant_num,
-		   brd_info->btdm_ant_pos);
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d",
+		   "Ant PG Num/ Ant Mech/ Ant Pos:",
+		   board_info->pg_ant_num, board_info->btdm_ant_num,
+		   board_info->btdm_ant_pos);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s / %d",
 		   "BT stack/ hci ext ver",
 		   ((stack_info->profile_notified) ? "Yes" : "No"),
 		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
 		   "CoexVer/ FwVer/ PatchVer",
 		   glcoex_ver_date_8723b_1ant, glcoex_ver_8723b_1ant,
 		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
 			   &wifi_dot11_chnl);
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d / %d(%d)",
 		   "Dot11 channel / HsChnl(HsMode)",
 		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %02x %02x %02x ",
 		   "H2C Wifi inform bt chnl Info",
 		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
 		   coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "Wifi rssi/ HS rssi", wifi_rssi, bt_hs_rssi);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d ",
 		   "Wifi link/ roam/ scan", link, roam, scan);
-	CL_PRINTF(cli_buf);
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G,
+	btcoexist->btc_get(btcoexist , BTC_GET_BL_WIFI_UNDER_5G,
 			   &wifi_under_5g);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
 			   &wifi_traffic_dir);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s / %s/ %s ",
 		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
 		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
 			(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
 		   ((!wifi_busy) ? "idle" :
 			((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
 				"uplink" : "downlink")));
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
 			   &wifi_link_status);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d/ %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d/ %d/ %d",
 		   "sta/vwifi/hs/p2pGo/p2pGc",
 		   ((wifi_link_status & WIFI_STA_CONNECTED) ? 1 : 0),
 		   ((wifi_link_status & WIFI_AP_CONNECTED) ? 1 : 0),
 		   ((wifi_link_status & WIFI_HS_CONNECTED) ? 1 : 0),
 		   ((wifi_link_status & WIFI_P2P_GO_CONNECTED) ? 1 : 0),
 		   ((wifi_link_status & WIFI_P2P_GC_CONNECTED) ? 1 : 0));
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = [%s/ %d/ %d] ",
 		   "BT [status/ rssi/ retryCnt]",
 		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
 		    ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
-		     ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ?
+		     ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		       coex_dm->bt_status) ?
 		      "non-connected idle" :
-		      ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) ?
+		      ((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE ==
+			coex_dm->bt_status) ?
 		       "connected-idle" : "busy")))),
-		       coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
-		"SCO/HID/PAN/A2DP", bt_link_info->sco_exist,
-		bt_link_info->hid_exist, bt_link_info->pan_exist,
-		bt_link_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
+		     coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d / %d / %d / %d",
+		   "SCO/HID/PAN/A2DP", bt_link_info->sco_exist,
+		   bt_link_info->hid_exist, bt_link_info->pan_exist,
+		   bt_link_info->a2dp_exist);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s",
 		   "BT Info A2DP rate",
-		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
+		   (bt_info_ext & BIT0) ? "Basic rate" : "EDR rate");
 
 	for (i = 0; i < BT_INFO_SRC_8723B_1ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
-				   glb_infosrc8723b1ant[i],
+				   GLBtInfoSrc8723b1Ant[i],
 				   coex_sta->bt_info_c2h[i][0],
 				   coex_sta->bt_info_c2h[i][1],
 				   coex_sta->bt_info_c2h[i][2],
@@ -2488,130 +2535,111 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 				   coex_sta->bt_info_c2h[i][5],
 				   coex_sta->bt_info_c2h[i][6],
 				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
 		}
 	}
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = %s/%s, (0x%x/0x%x)",
 		   "PS state, IPS/LPS, (lps/rpwm)",
 		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
 		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")),
 		   btcoexist->bt_info.lps_val,
 		   btcoexist->bt_info.rpwm_val);
-	CL_PRINTF(cli_buf);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
 
 	if (!btcoexist->manual_control) {
 		/* Sw mechanism	*/
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-			   "============[Sw mechanism] ============");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+			   "============[Sw mechanism]============");
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/",
 			   "SM[LowPenaltyRA]", coex_dm->cur_low_penalty_ra);
-		CL_PRINTF(cli_buf);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/ %s/ %d ",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s/ %s/ %d ",
 			   "DelBA/ BtCtrlAgg/ AggSize",
 			   (btcoexist->bt_info.reject_agg_pkt ? "Yes" : "No"),
-			   (btcoexist->bt_info.b_bt_ctrl_buf_size ? "Yes" : "No"),
+			   (btcoexist->bt_info.bt_ctrl_buf_size ? "Yes" : "No"),
 			   btcoexist->bt_info.agg_buf_size);
-		CL_PRINTF(cli_buf);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x ",
 			   "Rate Mask", btcoexist->bt_info.ra_mask);
-		CL_PRINTF(cli_buf);
 
 		/* Fw mechanism	*/
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-			   "============[Fw mechanism] ============");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+			   "============[Fw mechanism]============");
 
-		stdmacase = coex_dm->cur_ps_tdma;
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		pstdmacase = coex_dm->cur_ps_tdma;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
 			   "PS TDMA", coex_dm->ps_tdma_para[0],
 			   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
 			   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
-			   stdmacase, coex_dm->auto_tdma_adjust);
-		CL_PRINTF(cli_buf);
+			   pstdmacase, coex_dm->auto_tdma_adjust);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d ",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d ",
 			   "IgnWlanAct", coex_dm->cur_ignore_wlan_act);
-		CL_PRINTF(cli_buf);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x ",
 			   "Latest error condition(should be 0)",
 			   coex_dm->error_condition);
-		CL_PRINTF(cli_buf);
 	}
 
 	/* Hw setting */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Hw setting] ============");
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+		   "============[Hw setting]============");
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
 		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
 		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retry_limit,
 		   coex_dm->backup_ampdu_max_time);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
 	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
 		   "0x430/0x434/0x42a/0x456",
 		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6cc);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x880);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x778/0x6cc/0x880[29:25]", u8tmp[0], u32tmp[0],
 		   (u32tmp[1] & 0x3e000000) >> 25);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
 	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x948/ 0x67[5] / 0x765",
 		   u32tmp[0], ((u8tmp[0] & 0x20) >> 5), u8tmp[1]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
 	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
 		   u32tmp[0] & 0x3, u32tmp[1] & 0xff, u32tmp[2] & 0x3);
-	CL_PRINTF(cli_buf);
 
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
 	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
 	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
 		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
 		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
 		   ((u32tmp[0] & 0x01800000) >> 23), u8tmp[2] & 0x1);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0] & 0xff, u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
@@ -2629,27 +2657,23 @@ void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
 		   (u32tmp[3] & 0xffff);
 	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "OFDM-CCA/OFDM-FA/CCK-FA",
 		   u32tmp[0] & 0xffff, fa_ofdm, fa_cck);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
 	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x6c0/0x6c4/0x6c8(coexTable)",
 		   u32tmp[0], u32tmp[1], u32tmp[2]);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x770(high-pri rx/tx)", coex_sta->high_priority_rx,
 		   coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
 		   coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
 #if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 1)
 	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
 #endif
@@ -2666,11 +2690,11 @@ void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 			  "[BTCoex], IPS ENTER notify\n");
 		coex_sta->under_ips = true;
 
-		halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT,
+		halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT,
 					   false, true);
 		/* set PTA control */
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
-		coex_table_with_type(btcoexist,
+		halbtc8723b1ant_coex_table_with_type(btcoexist,
 						     NORMAL_EXEC, 0);
 		halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
 	} else if (BTC_IPS_LEAVE == type) {
@@ -2678,7 +2702,7 @@ void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 			  "[BTCoex], IPS LEAVE notify\n");
 		coex_sta->under_ips = false;
 
-		init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_hw_config(btcoexist, false);
 		halbtc8723b1ant_init_coex_dm(btcoexist);
 		halbtc8723b1ant_query_bt_info(btcoexist);
 	}
@@ -2741,14 +2765,14 @@ void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN START notify\n");
 		if (!wifi_connected)	/* non-connected scan */
-			action_wifi_not_connected_scan(btcoexist);
+			btc8723b1ant_action_wifi_not_conn_scan(btcoexist);
 		else	/* wifi is connected */
-			action_wifi_connected_scan(btcoexist);
+			btc8723b1ant_action_wifi_conn_scan(btcoexist);
 	} else if (BTC_SCAN_FINISH == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN FINISH notify\n");
 		if (!wifi_connected)	/* non-connected scan */
-			action_wifi_not_connected(btcoexist);
+			btc8723b1ant_action_wifi_not_conn(btcoexist);
 		else
 			halbtc8723b1ant_action_wifi_connected(btcoexist);
 	}
@@ -2789,7 +2813,7 @@ void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 	if (BTC_ASSOCIATE_START == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT START notify\n");
-		action_wifi_not_connected_asso_auth(btcoexist);
+		btc8723b1ant_act_wifi_not_conn_asso_auth(btcoexist);
 	} else if (BTC_ASSOCIATE_FINISH == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT FINISH notify\n");
@@ -2797,7 +2821,7 @@ void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
 				   &wifi_connected);
 		if (!wifi_connected) /* non-connected scan */
-			action_wifi_not_connected(btcoexist);
+			btc8723b1ant_action_wifi_not_conn(btcoexist);
 		else
 			halbtc8723b1ant_action_wifi_connected(btcoexist);
 	}
@@ -2808,7 +2832,7 @@ void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
 {
 	u8 h2c_parameter[3] = {0};
 	u32 wifi_bw;
-	u8 wificentralchnl;
+	u8 wifiCentralChnl;
 
 	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
 	    btcoexist->bt_info.bt_disabled)
@@ -2823,12 +2847,12 @@ void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
 
 	/* only 2.4G we need to inform bt the chnl mask */
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
-			   &wificentralchnl);
+			   &wifiCentralChnl);
 
 	if ((BTC_MEDIA_CONNECT == type) &&
-	    (wificentralchnl <= 14)) {
+	    (wifiCentralChnl <= 14)) {
 		h2c_parameter[0] = 0x0;
-		h2c_parameter[1] = wificentralchnl;
+		h2c_parameter[1] = wifiCentralChnl;
 		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 		if (BTC_WIFI_BW_HT40 == wifi_bw)
 			h2c_parameter[2] = 0x30;
@@ -2862,7 +2886,7 @@ void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
 		return;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_LINK_STATUS,
-			   &wifi_link_status);
+		&wifi_link_status);
 	num_of_wifi_link = wifi_link_status >> 16;
 	if (num_of_wifi_link >= 2) {
 		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
@@ -2887,7 +2911,7 @@ void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
 	    BTC_PACKET_EAPOL == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], special Packet(%d) notify\n", type);
-		action_wifi_connected_special_packet(btcoexist);
+		halbtc8723b1ant_action_wifi_connected_special_packet(btcoexist);
 	}
 }
 
@@ -2907,7 +2931,7 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 	coex_sta->bt_info_c2h_cnt[rsp_source]++;
 
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  "[BTCoex], Bt info[%d], length=%d, hex data = [",
 		  rsp_source, length);
 	for (i = 0; i < length; i++) {
 		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
@@ -2932,8 +2956,9 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 			coex_sta->bt_info_c2h[rsp_source][4];
 
 		/* Here we need to resend some wifi info to BT
-		 * because bt is reset and loss of the info.*/
-		if (coex_sta->bt_info_ext & BIT(1)) {
+		 * because bt is reset and loss of the info.
+		 */
+		if (coex_sta->bt_info_ext & BIT1) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n");
 			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
@@ -2946,7 +2971,7 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 							  BTC_MEDIA_DISCONNECT);
 		}
 
-		if (coex_sta->bt_info_ext & BIT(3)) {
+		if (coex_sta->bt_info_ext & BIT3) {
 			if (!btcoexist->manual_control &&
 			    !btcoexist->stop_coex_dm) {
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -2959,7 +2984,7 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 			/* BT already NOT ignore Wlan active, do nothing here.*/
 		}
 #if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
-		if (coex_sta->bt_info_ext & BIT(4)) {
+		if (coex_sta->bt_info_ext & BIT4) {
 			/* BT auto report already enabled, do nothing */
 		} else {
 			halbtc8723b1ant_bt_auto_report(btcoexist, FORCE_EXEC,
@@ -2968,7 +2993,7 @@ void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 #endif
 	}
 
-	/* check BIT(2) first ==> check if bt is under inquiry or page scan */
+	/* check BIT2 first ==> check if bt is under inquiry or page scan */
 	if (bt_info & BT_INFO_8723B_1ANT_B_INQ_PAGE)
 		coex_sta->c2h_bt_inquiry_page = true;
 	else
@@ -3048,7 +3073,7 @@ void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
 
 	btcoexist->stop_coex_dm = true;
 
-	halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT, false, true);
+	halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT, false, true);
 
 	halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
 	halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
@@ -3068,18 +3093,18 @@ void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], Pnp notify to SLEEP\n");
 		btcoexist->stop_coex_dm = true;
-		halbtc8723b1ant_setantpath(btcoexist, BTC_ANT_PATH_BT, false,
+		halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT, false,
 					   true);
 		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
 						 0x0, 0x0);
 		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
 		halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
 	} else if (BTC_WIFI_PNP_WAKE_UP == pnp_state) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], Pnp notify to WAKE UP\n");
 		btcoexist->stop_coex_dm = false;
-		init_hw_config(btcoexist, false);
+		halbtc8723b1ant_init_hw_config(btcoexist, false);
 		halbtc8723b1ant_init_coex_dm(btcoexist);
 		halbtc8723b1ant_query_bt_info(btcoexist);
 	}
@@ -3090,7 +3115,7 @@ void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist)
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 		  "[BTCoex], *****************Coex DM Reset****************\n");
 
-	init_hw_config(btcoexist, false);
+	halbtc8723b1ant_init_hw_config(btcoexist, false);
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x2, 0xfffff, 0x0);
 	halbtc8723b1ant_init_coex_dm(btcoexist);
@@ -3098,13 +3123,13 @@ void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist)
 
 void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
 {
-	struct btc_board_info *brd_info = &btcoexist->board_info;
+	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 	static u8 dis_ver_info_cnt;
 	u32 fw_ver = 0, bt_patch_ver = 0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], ========================== Periodical ===========================\n");
+		  "[BTCoex], ==========================Periodical===========================\n");
 
 	if (dis_ver_info_cnt <= 5) {
 		dis_ver_info_cnt += 1;
@@ -3112,8 +3137,8 @@ void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
 			  "[BTCoex], ****************************************************************\n");
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
-			  brd_info->pg_ant_num, brd_info->btdm_ant_num,
-			  brd_info->btdm_ant_pos);
+			  board_info->pg_ant_num, board_info->btdm_ant_num,
+			  board_info->btdm_ant_pos);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
 			  ((stack_info->profile_notified) ? "Yes" : "No"),
@@ -3135,7 +3160,7 @@ void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
 	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
 	halbtc8723b1ant_monitor_bt_enable_disable(btcoexist);
 #else
-	if (is_wifi_status_changed(btcoexist) ||
+	if (btc8723b1ant_is_wifi_status_changed(btcoexist) ||
 	    coex_dm->auto_tdma_adjust) {
 		halbtc8723b1ant_run_coexist_mechanism(btcoexist);
 	}
@@ -3143,4 +3168,3 @@ void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
 	coex_sta->special_pkt_period_cnt++;
 #endif
 }
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b1ant.h
index 5ce292f2e7c6..75f8094b7a34 100644
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.h
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b1ant.h
@@ -1,3 +1,27 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
 /**********************************************************************
  * The following is for 8723B 1ANT BT Co-exist definition
  **********************************************************************/
@@ -13,18 +37,18 @@
 #define	BT_INFO_8723B_1ANT_B_CONNECTION			BIT0
 
 #define	BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
-		(((_BT_INFO_EXT_&BIT0))? true:false)
+		(((_BT_INFO_EXT_&BIT0)) ? true : false)
 
 #define	BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT		2
 
-typedef enum _BT_INFO_SRC_8723B_1ANT{
+enum _BT_INFO_SRC_8723B_1ANT {
 	BT_INFO_SRC_8723B_1ANT_WIFI_FW			= 0x0,
 	BT_INFO_SRC_8723B_1ANT_BT_RSP			= 0x1,
 	BT_INFO_SRC_8723B_1ANT_BT_ACTIVE_SEND		= 0x2,
 	BT_INFO_SRC_8723B_1ANT_MAX
-}BT_INFO_SRC_8723B_1ANT,*PBT_INFO_SRC_8723B_1ANT;
+};
 
-typedef enum _BT_8723B_1ANT_BT_STATUS{
+enum _BT_8723B_1ANT_BT_STATUS {
 	BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
 	BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
 	BT_8723B_1ANT_BT_STATUS_INQ_PAGE		= 0x2,
@@ -32,9 +56,9 @@ typedef enum _BT_8723B_1ANT_BT_STATUS{
 	BT_8723B_1ANT_BT_STATUS_SCO_BUSY		= 0x4,
 	BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
 	BT_8723B_1ANT_BT_STATUS_MAX
-}BT_8723B_1ANT_BT_STATUS,*PBT_8723B_1ANT_BT_STATUS;
+};
 
-typedef enum _BT_8723B_1ANT_WIFI_STATUS{
+enum _BT_8723B_1ANT_WIFI_STATUS {
 	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE			= 0x0,
 	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN		= 0x1,
 	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN			= 0x2,
@@ -42,9 +66,9 @@ typedef enum _BT_8723B_1ANT_WIFI_STATUS{
 	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE			= 0x4,
 	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY			= 0x5,
 	BT_8723B_1ANT_WIFI_STATUS_MAX
-}BT_8723B_1ANT_WIFI_STATUS,*PBT_8723B_1ANT_WIFI_STATUS;
+};
 
-typedef enum _BT_8723B_1ANT_COEX_ALGO{
+enum _BT_8723B_1ANT_COEX_ALGO {
 	BT_8723B_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
 	BT_8723B_1ANT_COEX_ALGO_SCO			= 0x1,
 	BT_8723B_1ANT_COEX_ALGO_HID			= 0x2,
@@ -57,20 +81,16 @@ typedef enum _BT_8723B_1ANT_COEX_ALGO{
 	BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
 	BT_8723B_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
 	BT_8723B_1ANT_COEX_ALGO_MAX			= 0xb,
-}BT_8723B_1ANT_COEX_ALGO,*PBT_8723B_1ANT_COEX_ALGO;
+};
 
-struct coex_dm_8723b_1ant{
+struct coex_dm_8723b_1ant {
 	/* fw mechanism */
-	bool pre_dec_bt_pwr;
-	bool cur_dec_bt_pwr;
-	u8 pre_fw_dac_swing_lvl;
-	u8 cur_fw_dac_swing_lvl;
 	bool cur_ignore_wlan_act;
 	bool pre_ignore_wlan_act;
 	u8 pre_ps_tdma;
 	u8 cur_ps_tdma;
 	u8 ps_tdma_para[5];
-	u8 ps_tdma_du_adj_type;
+	u8 tdma_adj_type;
 	bool auto_tdma_adjust;
 	bool pre_ps_tdma_on;
 	bool cur_ps_tdma_on;
@@ -82,19 +102,8 @@ struct coex_dm_8723b_1ant{
 	u8 cur_rpwm;
 
 	/* sw mechanism */
-	bool pre_rf_rx_lpf_shrink;
-	bool cur_rf_rx_lpf_shrink;
-	u32 bt_rf0x1e_backup;
 	bool pre_low_penalty_ra;
 	bool cur_low_penalty_ra;
-	bool pre_dac_swing_on;
-	u32 pre_dac_swing_lvl;
-	bool cur_dac_swing_on;
-	u32 cur_dac_swing_lvl;
-	bool pre_adc_backoff;
-	bool cur_adc_backoff;
-	bool pre_agc_table_en;
-	bool cur_agc_table_en;
 	u32 pre_val0x6c0;
 	u32 cur_val0x6c0;
 	u32 pre_val0x6c4;
@@ -128,7 +137,7 @@ struct coex_dm_8723b_1ant{
 	u8 error_condition;
 };
 
-struct coex_sta_8723b_1ant{
+struct coex_sta_8723b_1ant {
 	bool bt_link_exist;
 	bool sco_exist;
 	bool a2dp_exist;
@@ -169,7 +178,7 @@ void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
 void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
 				       u8 *tmpbuf, u8 length);
 void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpState);
+void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
+void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist);
 void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist);
 void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist);
-
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
index d916ab9f3c38..cefe26991421 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.c
@@ -49,8 +49,8 @@ static const char *const glbt_info_src_8723b_2ant[] = {
 	"BT Info[bt auto report]",
 };
 
-static u32 glcoex_ver_date_8723b_2ant = 20130731;
-static u32 glcoex_ver_8723b_2ant = 0x3b;
+static u32 glcoex_ver_date_8723b_2ant = 20131113;
+static u32 glcoex_ver_8723b_2ant = 0x3f;
 
 /**************************************************************
  * local function proto type if needed
@@ -303,6 +303,21 @@ static void btc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
+static void btc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
+{
+	u8 h2c_parameter[1] = {0};
+
+	coex_sta->c2h_bt_info_req_sent = true;
+
+	h2c_parameter[0] |= BIT0;	/* trigger */
+
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
+		  h2c_parameter[0]);
+
+	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
+}
+
 static bool btc8723b2ant_is_wifi_status_changed(struct btc_coexist *btcoexist)
 {
 	static bool pre_wifi_busy;
@@ -604,7 +619,7 @@ static bool btc8723b_need_dec_pwr(struct btc_coexist *btcoexist)
 	if (!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
 		return false;
 
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
 
 	if (wifi_connected) {
 		if (bt_hs_on) {
@@ -824,7 +839,6 @@ static void btc8723b2ant_set_sw_fulltime_dac_swing(struct btc_coexist *btcoex,
 		btc8723b2ant_set_dac_swing_reg(btcoex, 0x18);
 }
 
-
 static void btc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
 				   bool force_exec, bool dac_swing_on,
 				   u32 dac_swing_lvl)
@@ -884,7 +898,6 @@ static void btc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
 		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa4200001);
 	}
 
-
 	/* RF Gain */
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
 	if (agc_table_en) {
@@ -1160,8 +1173,87 @@ static void btc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
 			       dac_swing_lvl);
 }
 
+static void btc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
+				      u8 antpos_type, bool init_hwcfg,
+				      bool wifi_off)
+{
+	struct btc_board_info *board_info = &btcoexist->board_info;
+	u32 fw_ver = 0, u32tmp = 0;
+	bool pg_ext_switch = false;
+	bool use_ext_switch = false;
+	u8 h2c_parameter[2] = {0};
+
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
+
+	if ((fw_ver < 0xc0000) || pg_ext_switch)
+		use_ext_switch = true;
+
+	if (init_hwcfg) {
+		/* 0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT */
+		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
+		u32tmp &= ~BIT23;
+		u32tmp |= BIT24;
+		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
+
+		btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
+		btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
+
+		/* Force GNT_BT to low */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
+		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+
+		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
+			/* tell firmware "no antenna inverse" */
+			h2c_parameter[0] = 0;
+			h2c_parameter[1] = 1;  /* ext switch type */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		} else {
+			/* tell firmware "antenna inverse" */
+			h2c_parameter[0] = 1;
+			h2c_parameter[1] = 1;  /* ext switch type */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+		}
+	}
+
+	/* ext switch setting */
+	if (use_ext_switch) {
+		/* fixed internal switch S1->WiFi, S0->BT */
+		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+		switch (antpos_type) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			/* ext switch main at wifi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
+							   0x3, 0x1);
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			/* ext switch aux at wifi */
+			btcoexist->btc_write_1byte_bitmask(btcoexist,
+							   0x92c, 0x3, 0x2);
+			break;
+		}
+	} else {	/* internal switch */
+		/* fixed ext switch */
+		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c, 0x3, 0x1);
+		switch (antpos_type) {
+		case BTC_ANT_WIFI_AT_MAIN:
+			/* fixed internal switch S1->WiFi, S0->BT */
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
+			break;
+		case BTC_ANT_WIFI_AT_AUX:
+			/* fixed internal switch S0->WiFi, S1->BT */
+			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
+			break;
+		}
+	}
+}
+
 static void btc8723b2ant_ps_tdma(struct btc_coexist *btcoexist, bool force_exec,
-			     bool turn_on, u8 type)
+				 bool turn_on, u8 type)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s turn %s PS TDMA, type=%d\n",
@@ -1351,7 +1443,8 @@ static void btc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 	coex_dm->need_recover_0x948 = true;
 	coex_dm->backup_0x948 = btcoexist->btc_read_2byte(btcoexist, 0x948);
 
-	btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
+	btc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_AUX,
+				  false, false);
 }
 
 static bool btc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
@@ -1520,7 +1613,9 @@ static void set_tdma_int1(struct btc_coexist *btcoexist, bool tx_pause,
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 					     true, 8);
 			coex_dm->tdma_adj_type = 8;
-		} else if (coex_dm->cur_ps_tdma == 9) {
+		}
+
+		if (coex_dm->cur_ps_tdma == 9) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 					     true, 13);
 			coex_dm->tdma_adj_type = 13;
@@ -1607,7 +1702,9 @@ static void set_tdma_int1(struct btc_coexist *btcoexist, bool tx_pause,
 		} else if (coex_dm->cur_ps_tdma == 8) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 4);
 			coex_dm->tdma_adj_type = 4;
-		} else if (coex_dm->cur_ps_tdma == 13) {
+		}
+
+		if (coex_dm->cur_ps_tdma == 13) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
 			coex_dm->tdma_adj_type = 9;
 		} else if (coex_dm->cur_ps_tdma == 14) {
@@ -1652,23 +1749,34 @@ static void set_tdma_int1(struct btc_coexist *btcoexist, bool tx_pause,
 				coex_dm->tdma_adj_type = 12;
 			}
 		}  else if (result == 1) {
-			int tmp = coex_dm->cur_ps_tdma;
-			switch (tmp) {
-			case 4:
-			case 3:
-			case 2:
-			case 12:
-			case 11:
-			case 10:
-				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						     true, tmp - 1);
-				coex_dm->tdma_adj_type = tmp - 1;
-				break;
-			case 1:
+			if (coex_dm->cur_ps_tdma == 4) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 1);
+				coex_dm->tdma_adj_type = 1;
+			} else if (coex_dm->cur_ps_tdma == 1) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 71);
 				coex_dm->tdma_adj_type = 71;
-				break;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						     true, 9);
+				coex_dm->tdma_adj_type = 9;
 			}
 		}
 	}
@@ -1694,7 +1802,8 @@ static void set_tdma_int2(struct btc_coexist *btcoexist, bool tx_pause,
 		} else if (coex_dm->cur_ps_tdma == 4) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 8);
 			coex_dm->tdma_adj_type = 8;
-		} else if (coex_dm->cur_ps_tdma == 9) {
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
 			coex_dm->tdma_adj_type = 14;
 		} else if (coex_dm->cur_ps_tdma == 10) {
@@ -1776,7 +1885,8 @@ static void set_tdma_int2(struct btc_coexist *btcoexist, bool tx_pause,
 		} else if (coex_dm->cur_ps_tdma == 8) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 4);
 			coex_dm->tdma_adj_type = 4;
-		} else if (coex_dm->cur_ps_tdma == 13) {
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
 			coex_dm->tdma_adj_type = 10;
 		} else if (coex_dm->cur_ps_tdma == 14) {
@@ -1865,7 +1975,8 @@ static void set_tdma_int3(struct btc_coexist *btcoexist, bool tx_pause,
 		} else if (coex_dm->cur_ps_tdma == 4) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 8);
 			coex_dm->tdma_adj_type = 8;
-		} else if (coex_dm->cur_ps_tdma == 9) {
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 15);
 			coex_dm->tdma_adj_type = 15;
 		} else if (coex_dm->cur_ps_tdma == 10) {
@@ -1935,101 +2046,80 @@ static void set_tdma_int3(struct btc_coexist *btcoexist, bool tx_pause,
 		BTC_PRINT(BTC_MSG_ALGORITHM,
 			  ALGO_TRACE_FW_DETAIL,
 			  "[BTCoex], TxPause = 0\n");
-		switch (coex_dm->cur_ps_tdma) {
-		case 5:
+		if (coex_dm->cur_ps_tdma == 5) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
 			coex_dm->tdma_adj_type = 3;
-			break;
-		case 6:
+		} else if (coex_dm->cur_ps_tdma == 6) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
 			coex_dm->tdma_adj_type = 3;
-			break;
-		case 7:
+		} else if (coex_dm->cur_ps_tdma == 7) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
 			coex_dm->tdma_adj_type = 3;
-			break;
-		case 8:
+		} else if (coex_dm->cur_ps_tdma == 8) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 4);
 			coex_dm->tdma_adj_type = 4;
-			break;
-		case 13:
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
 			coex_dm->tdma_adj_type = 11;
-			break;
-		case 14:
+		} else if (coex_dm->cur_ps_tdma == 14) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
 			coex_dm->tdma_adj_type = 11;
-			break;
-		case 15:
+		} else if (coex_dm->cur_ps_tdma == 15) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
 			coex_dm->tdma_adj_type = 11;
-			break;
-		case 16:
+		} else if (coex_dm->cur_ps_tdma == 16) {
 			btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 12);
 			coex_dm->tdma_adj_type = 12;
-			break;
 		}
 		if (result == -1) {
-			switch (coex_dm->cur_ps_tdma) {
-			case 1:
+			if (coex_dm->cur_ps_tdma == 1) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 3);
 				coex_dm->tdma_adj_type = 3;
-				break;
-			case 2:
+			} else if (coex_dm->cur_ps_tdma == 2) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 3);
 				coex_dm->tdma_adj_type = 3;
-				break;
-			case 3:
+			} else if (coex_dm->cur_ps_tdma == 3) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 4);
 				coex_dm->tdma_adj_type = 4;
-				break;
-			case 9:
+			} else if (coex_dm->cur_ps_tdma == 9) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 11);
 				coex_dm->tdma_adj_type = 11;
-				break;
-			case 10:
+			} else if (coex_dm->cur_ps_tdma == 10) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 11);
 				coex_dm->tdma_adj_type = 11;
-				break;
-			case 11:
+			} else if (coex_dm->cur_ps_tdma == 11) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 12);
 				coex_dm->tdma_adj_type = 12;
-				break;
 			}
 		} else if (result == 1) {
-			switch (coex_dm->cur_ps_tdma) {
-			case 4:
+			if (coex_dm->cur_ps_tdma == 4) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 3);
 				coex_dm->tdma_adj_type = 3;
-				break;
-			case 3:
+			} else if (coex_dm->cur_ps_tdma == 3) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 3);
 				coex_dm->tdma_adj_type = 3;
-				break;
-			case 2:
+			} else if (coex_dm->cur_ps_tdma == 2) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 3);
 				coex_dm->tdma_adj_type = 3;
-				break;
-			case 12:
+			} else if (coex_dm->cur_ps_tdma == 12) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 11);
 				coex_dm->tdma_adj_type = 11;
-				break;
-			case 11:
+			} else if (coex_dm->cur_ps_tdma == 11) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 11);
 				coex_dm->tdma_adj_type = 11;
-				break;
-			case 10:
+			} else if (coex_dm->cur_ps_tdma == 10) {
 				btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
 						     true, 11);
 				coex_dm->tdma_adj_type = 11;
@@ -2328,7 +2418,7 @@ static void btc8723b2ant_action_hid(struct btc_coexist *btcoexist)
 
 	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
 						       0, 2, 15, 0);
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
 
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 
@@ -2385,12 +2475,43 @@ static void btc8723b2ant_action_hid(struct btc_coexist *btcoexist)
 /*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
 static void btc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state;
+	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
 	u32 wifi_bw;
+	u8 ap_num = 0;
 
 	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
 						       0, 2, 15, 0);
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	wifi_rssi_state1 = btc8723b2ant_wifi_rssi_state(btcoexist,
+							1, 2, 40, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
+
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
+
+	/* define the office environment */
+	/* driver don't know AP num in Linux, so we will never enter this if */
+	if (ap_num >= 10 && BTC_RSSI_HIGH(wifi_rssi_state1)) {
+		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
+					  0x0);
+		btc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+		btc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
+		btc8723b_coex_tbl_type(btcoexist, NORMAL_EXEC, 0);
+		btc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
+
+		/* sw mechanism */
+		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
+		if (BTC_WIFI_BW_HT40 == wifi_bw) {
+			btc8723b2ant_sw_mechanism1(btcoexist, true, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   true, 0x18);
+		} else {
+			btc8723b2ant_sw_mechanism1(btcoexist, false, false,
+						   false, false);
+			btc8723b2ant_sw_mechanism2(btcoexist, true, false,
+						   true, 0x18);
+		}
+		return;
+	}
 
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 
@@ -2501,7 +2622,7 @@ static void btc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
 
 	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
 						       0, 2, 15, 0);
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
 
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 
@@ -2612,7 +2733,7 @@ static void btc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 
 	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
 						       0, 2, 15, 0);
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
 
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 
@@ -2676,7 +2797,7 @@ static void btc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 
 	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
 						       0, 2, 15, 0);
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
 	if (btc8723b_need_dec_pwr(btcoexist))
@@ -2746,7 +2867,7 @@ static void btc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 
 	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
 						       0, 2, 15, 0);
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
 
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 
@@ -2809,8 +2930,8 @@ static void btc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 	u32 wifi_bw;
 
 	wifi_rssi_state = btc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 35, 0);
+						       0, 2, 15, 0);
+	bt_rssi_state = btc8723b2ant_bt_rssi_state(2, 29, 0);
 
 	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
 
@@ -2982,7 +3103,15 @@ static void btc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-
+static void btc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
+{
+	/* set wlan_act to low */
+	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
+	/* Force GNT_BT to High */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
+	/* BT select s0/s1 is controlled by BT */
+	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
+}
 
 /*********************************************************************
  *  work around function start with wa_btc8723b2ant_
@@ -2990,98 +3119,24 @@ static void btc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 /*********************************************************************
  *  extern function start with EXbtc8723b2ant_
  *********************************************************************/
-void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
+void ex_btc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
 {
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	u32 u32tmp = 0, fw_ver;
 	u8 u8tmp = 0;
-	u8 h2c_parameter[2] = {0};
-
 
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 		  "[BTCoex], 2Ant Init HW Config!!\n");
+	coex_dm->bt_rf0x1e_backup =
+		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
 
-	/* backup rf 0x1e value */
-	coex_dm->bt_rf0x1e_backup = btcoexist->btc_get_rf_reg(btcoexist,
-							      BTC_RF_A, 0x1e,
-							      0xfffff);
-
-	/* 0x4c[23]=0, 0x4c[24]=1  Antenna control by WL/BT */
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-	u32tmp &= ~BIT23;
-	u32tmp |= BIT24;
-	btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
-
-	btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
-	btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
-
-	/* Antenna switch control parameter */
-	/* btcoexist->btc_write_4byte(btcoexist, 0x858, 0x55555555);*/
-
-	/*Force GNT_BT to low*/
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
-	btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-
-	/* 0x790[5:0]=0x5 */
+	/* 0x790[5:0] = 0x5 */
 	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
 	u8tmp &= 0xc0;
 	u8tmp |= 0x5;
 	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
 
-
-	/*Antenna config	*/
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-
- /*ext switch for fw ver < 0xc */
-	if (fw_ver < 0xc00) {
-		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x1);
-			/*Main Ant to  BT for IPS case 0x4c[23]=1*/
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1,
-							   0x1);
-
-			/*tell firmware "no antenna inverse"*/
-			h2c_parameter[0] = 0;
-			h2c_parameter[1] = 1;  /* ext switch type */
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		} else {
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x2);
-			/*Aux Ant to  BT for IPS case 0x4c[23]=1*/
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1,
-							   0x0);
-
-			/*tell firmware "antenna inverse"*/
-			h2c_parameter[0] = 1;
-			h2c_parameter[1] = 1;  /*ext switch type*/
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		}
-	} else {
-		/*ext switch always at s1 (if exist) */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c, 0x3, 0x1);
-		/*Main Ant to  BT for IPS case 0x4c[23]=1*/
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1, 0x1);
-
-		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
-			/*tell firmware "no antenna inverse"*/
-			h2c_parameter[0] = 0;
-			h2c_parameter[1] = 0;  /*ext switch type*/
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		} else {
-			/*tell firmware "antenna inverse"*/
-			h2c_parameter[0] = 1;
-			h2c_parameter[1] = 0;  /*ext switch type*/
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		}
-	}
-
+	/*Antenna config */
+	btc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN,
+				  true, false);
 	/* PTA parameter */
 	btc8723b_coex_tbl_type(btcoexist, FORCE_EXEC, 0);
 
@@ -3092,19 +3147,19 @@ void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
 }
 
-void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
+void ex_btc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 		  "[BTCoex], Coex Mechanism Init!!\n");
 	btc8723b2ant_init_coex_dm(btcoexist);
 }
 
-void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
+void ex_btc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 {
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	u8 *cli_buf = btcoexist->cli_buf;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
 	u32 u32tmp[4];
 	bool roam = false, scan = false;
@@ -3114,106 +3169,93 @@ void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
 	u8 wifi_dot11_chnl, wifi_hs_chnl;
 	u32 fw_ver = 0, bt_patch_ver = 0;
+	u8 ap_num = 0;
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cli_buf);
 
 	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			   "\r\n ==========[Under Manual Control]============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
 	}
 
 	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n BT not exists !!!");
 		return;
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d ",
 		   "Ant PG number/ Ant mechanism:",
 		   board_info->pg_ant_num, board_info->btdm_ant_num);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s / %d",
 		   "BT stack/ hci ext ver",
 		   ((stack_info->profile_notified) ? "Yes" : "No"),
 		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
 		   "CoexVer/ FwVer/ PatchVer",
 		   glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
 		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
 			   &wifi_dot11_chnl);
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d / %d(%d)",
 		   "Dot11 channel / HsChnl(HsMode)",
 		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %02x %02x %02x ",
 		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
 		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "Wifi rssi/ HS rssi", wifi_rssi, bt_hs_rssi);
-	CL_PRINTF(cli_buf);
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d",
+		   "Wifi rssi/ HS rssi/ AP#", wifi_rssi, bt_hs_rssi, ap_num);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d ",
 		   "Wifi link/ roam/ scan", link, roam, scan);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
 			   &wifi_traffic_dir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s / %s/ %s ",
 		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
 		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
 		   (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
 		   ((!wifi_busy) ? "idle" :
 		   ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
 		   "uplink" : "downlink")));
-	CL_PRINTF(cli_buf);
 
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d / %d / %d / %d",
 		   "SCO/HID/PAN/A2DP",
 		   bt_link_info->sco_exist, bt_link_info->hid_exist,
 		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s",
 		   "BT Info A2DP rate",
 		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
 
 	for (i = 0; i < BT_INFO_SRC_8723B_2ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 				   "\r\n %-35s = %02x %02x %02x "
 				   "%02x %02x %02x %02x(%d)",
 				   glbt_info_src_8723b_2ant[i],
@@ -3225,105 +3267,88 @@ void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 				   coex_sta->bt_info_c2h[i][5],
 				   coex_sta->bt_info_c2h[i][6],
 				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
 		}
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s/%s",
 		   "PS state, IPS/LPS",
 		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
 		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
-	CL_PRINTF(cli_buf);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
 
 	/* Sw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s", "============[Sw mechanism]============");
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d ",
 		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
 		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
 		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
 		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
 		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
-	CL_PRINTF(cli_buf);
 
 	/* Fw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
 		   "============[Fw mechanism]============");
-	CL_PRINTF(cli_buf);
 
 	ps_tdma_case = coex_dm->cur_ps_tdma;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
 		   "PS TDMA", coex_dm->ps_tdma_para[0],
 		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
 		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
 		   ps_tdma_case, coex_dm->auto_tdma_adjust);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d ",
 		   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
 		   coex_dm->cur_ignore_wlan_act);
-	CL_PRINTF(cli_buf);
 
 	/* Hw setting */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
 		   "============[Hw setting]============");
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x",
 		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
 
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x880);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x778/0x880[29:25]", u8tmp[0],
 		   (u32tmp[0]&0x3e000000) >> 25);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
 	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x948/ 0x67[5] / 0x765",
 		   u32tmp[0], ((u8tmp[0]&0x20) >> 5), u8tmp[1]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
 	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
 		   u32tmp[0]&0x3, u32tmp[1]&0xff, u32tmp[2]&0x3);
-	CL_PRINTF(cli_buf);
-
 
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
 	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
 	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
 		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
 		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
 		   ((u32tmp[0]&0x01800000)>>23), u8tmp[2]&0x1);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0]&0xff, u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
@@ -3341,29 +3366,25 @@ void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 		   (u32tmp[3] & 0xffff);
 	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "OFDM-CCA/OFDM-FA/CCK-FA",
 		   u32tmp[0]&0xffff, fa_ofdm, fa_cck);
-	CL_PRINTF(cli_buf);
 
 	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
 	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
 	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
 	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
 		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
 		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x770(high-pri rx/tx)",
 		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
 		   coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
 #if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 1)
 	btc8723b2ant_monitor_bt_ctr(btcoexist);
 #endif
@@ -3371,22 +3392,26 @@ void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
 	BTC_DBG_DISP_COEX_STATISTICS);
 }
 
-
-void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	if (BTC_IPS_ENTER == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], IPS ENTER notify\n");
 		coex_sta->under_ips = true;
+		btc8723b2ant_wifioff_hwcfg(btcoexist);
+		btc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
 		btc8723b2ant_coex_alloff(btcoexist);
 	} else if (BTC_IPS_LEAVE == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], IPS LEAVE notify\n");
 		coex_sta->under_ips = false;
+		ex_btc8723b2ant_init_hwconfig(btcoexist);
+		btc8723b2ant_init_coex_dm(btcoexist);
+		btc8723b2ant_query_bt_info(btcoexist);
 	}
 }
 
-void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	if (BTC_LPS_ENABLE == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
@@ -3399,7 +3424,7 @@ void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
 	}
 }
 
-void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	if (BTC_SCAN_START == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
@@ -3409,7 +3434,7 @@ void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 			  "[BTCoex], SCAN FINISH notify\n");
 }
 
-void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
+void ex_btc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 {
 	if (BTC_ASSOCIATE_START == type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
@@ -3419,8 +3444,8 @@ void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 			  "[BTCoex], CONNECT FINISH notify\n");
 }
 
-void btc8723b_med_stat_notify(struct btc_coexist *btcoexist,
-					    u8 type)
+void ex_btc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
+					 u8 type)
 {
 	u8 h2c_parameter[3] = {0};
 	u32 wifi_bw;
@@ -3460,16 +3485,16 @@ void btc8723b_med_stat_notify(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
 }
 
-void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type)
+void ex_btc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					   u8 type)
 {
 	if (type == BTC_PACKET_DHCP)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], DHCP Packet notify\n");
 }
 
-void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length)
+void ex_btc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				    u8 *tmpbuf, u8 length)
 {
 	u8 bt_info = 0;
 	u8 i, rsp_source = 0;
@@ -3516,7 +3541,7 @@ void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
 			coex_sta->bt_info_c2h[rsp_source][4];
 
 		/* Here we need to resend some wifi info to BT
-		 * because bt is reset and loss of the info.
+		     because bt is reset and loss of the info.
 		 */
 		if ((coex_sta->bt_info_ext & BIT1)) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -3525,11 +3550,13 @@ void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
 			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
 					   &wifi_connected);
 			if (wifi_connected)
-				btc8723b_med_stat_notify(btcoexist,
-							 BTC_MEDIA_CONNECT);
+				ex_btc8723b2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_CONNECT);
 			else
-				btc8723b_med_stat_notify(btcoexist,
-							 BTC_MEDIA_DISCONNECT);
+				ex_btc8723b2ant_media_status_notify(
+							btcoexist,
+							BTC_MEDIA_DISCONNECT);
 		}
 
 		if ((coex_sta->bt_info_ext & BIT3)) {
@@ -3564,7 +3591,7 @@ void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
 		coex_sta->a2dp_exist = false;
 		coex_sta->hid_exist = false;
 		coex_sta->sco_exist = false;
-	} else { /*  connection exists */
+	} else { /* connection exists */
 		coex_sta->bt_link_exist = true;
 		if (bt_info & BT_INFO_8723B_2ANT_B_FTP)
 			coex_sta->pan_exist = true;
@@ -3601,7 +3628,7 @@ void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
 		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_SCO_BUSY;
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
-	} else if (bt_info & BT_INFO_8723B_2ANT_B_ACL_BUSY) {
+	} else if (bt_info&BT_INFO_8723B_2ANT_B_ACL_BUSY) {
 		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
@@ -3630,26 +3657,16 @@ void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
 	btc8723b2ant_run_coexist_mechanism(btcoexist);
 }
 
-void ex_halbtc8723b2ant_stack_operation_notify(struct btc_coexist *btcoexist,
-					       u8 type)
-{
-	if (BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex],StackOP Inquiry/page/pair start notify\n");
-	else if (BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex],StackOP Inquiry/page/pair finish notify\n");
-}
-
-void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist)
+void ex_btc8723b2ant_halt_notify(struct btc_coexist *btcoexist)
 {
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
 
+	btc8723b2ant_wifioff_hwcfg(btcoexist);
 	btc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
-	btc8723b_med_stat_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+	ex_btc8723b2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
 }
 
-void ex_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist)
+void ex_btc8723b2ant_periodical(struct btc_coexist *btcoexist)
 {
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
@@ -3677,8 +3694,7 @@ void ex_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist)
 				   &bt_patch_ver);
 		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], CoexVer/ FwVer/ PatchVer = "
-			  "%d_%x/ 0x%x/ 0x%x(%d)\n",
+			  "[BTCoex], CoexVer/ fw_ver/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
 			  glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
 			  fw_ver, bt_patch_ver, bt_patch_ver);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.h
index e0ad8e545f82..567f354caf95 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.h
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8723b2ant.h
@@ -153,21 +153,20 @@ struct coex_sta_8723b_2ant {
 /*********************************************************************
  * The following is interface which will notify coex module.
  *********************************************************************/
-void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void btc8723b_med_stat_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8723b2ant_stack_operation_notify(struct btc_coexist *btcoexist,
-					       u8 type);
-void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_btc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_btc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_btc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_btc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
+					 u8 type);
+void ex_btc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
+					   u8 type);
+void ex_btc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
+				    u8 *tmpbuf, u8 length);
+void ex_btc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_btc8723b2ant_periodical(struct btc_coexist *btcoexist);
+void ex_btc8723b2ant_display_coex_info(struct btc_coexist *btcoexist);
 
 #endif
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a1ant.c
index fb5286387d80..b72e5377bdbc 100644
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a1ant.c
@@ -1,48 +1,80 @@
-/*  */
-/*  Description: */
-/*  */
-/*  This file is for RTL8821A Co-exist mechanism */
-/*  */
-/*  History */
-/*  2012/11/15 Cosa first check in. */
-/*  */
-/*  */
-
-/*  */
-/*  include files */
-/*  */
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+/*============================================================
+ * Description:
+ *
+ * This file is for RTL8821A Co-exist mechanism
+ *
+ * History
+ * 2012/11/15 Cosa first check in.
+ *
+ *============================================================
+*/
+/*============================================================
+ * include files
+ *============================================================
+ */
 #include "halbt_precomp.h"
-/*  */
-/*  Global variables, these are static variables */
-/*  */
+/*============================================================
+ * Global variables, these are static variables
+ *============================================================
+ */
 static struct coex_dm_8821a_1ant glcoex_dm_8821a_1ant;
 static struct coex_dm_8821a_1ant *coex_dm = &glcoex_dm_8821a_1ant;
 static struct coex_sta_8821a_1ant glcoex_sta_8821a_1ant;
 static struct coex_sta_8821a_1ant *coex_sta = &glcoex_sta_8821a_1ant;
 
 static const char *const glbt_info_src_8821a_1ant[] = {
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
+	  "BT Info[wifi fw]",
+	  "BT Info[bt rsp]",
+	  "BT Info[bt auto report]",
 };
 
-static u32 glcoex_ver_date_8821a_1ant = 20130816;
-static u32 glcoex_ver_8821a_1ant = 0x41;
+static u32	glcoex_ver_date_8821a_1ant = 20130816;
+static u32	glcoex_ver_8821a_1ant = 0x41;
 
-/*  local function proto type if needed */
-/*  local function start with halbtc8821a1ant_ */
+/*============================================================
+ * local function proto type if needed
+ *
+ * local function start with halbtc8821a1ant_
+ *============================================================
+ */
 static u8 halbtc8821a1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 					u8 rssi_thresh1)
 {
-	long			bt_rssi = 0;
-	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
+	long	bt_rssi = 0;
+	u8	bt_rssi_state = coex_sta->pre_bt_rssi_state;
 
 	bt_rssi = coex_sta->bt_rssi;
 
 	if (level_num == 2) {
 		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+			if (bt_rssi >= (rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
 				bt_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to High\n");
@@ -71,7 +103,8 @@ static u8 halbtc8821a1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 
 		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+			if (bt_rssi >= (rssi_thresh +
+					BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
 				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Medium\n");
@@ -80,9 +113,12 @@ static u8 halbtc8821a1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at Low\n");
 			}
-		} else if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (bt_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+		} else if ((coex_sta->pre_bt_rssi_state ==
+			   BTC_RSSI_STATE_MEDIUM) ||
+			   (coex_sta->pre_bt_rssi_state ==
+			    BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (bt_rssi >= (rssi_thresh1 +
+					BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
 				bt_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to High\n");
@@ -107,25 +143,27 @@ static u8 halbtc8821a1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 			}
 		}
 	}
-
 	coex_sta->pre_bt_rssi_state = bt_rssi_state;
 
 	return bt_rssi_state;
 }
 
-static u8 Wifi_rssi_state(struct btc_coexist *btcoexist, u8 index,
-			  u8 level_num, u8 rssi_thresh,
-			  u8 rssi_thresh1)
+static u8 halbtc8821a1ant_WifiRssiState(struct btc_coexist *btcoexist,
+					u8 index, u8 level_num, u8 rssi_thresh,
+					u8 rssi_thresh1)
 {
 	long	wifi_rssi = 0;
-	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
+	u8	wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 
 	if (level_num == 2) {
-		if ((coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+		if ((coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_LOW) ||
+		    (coex_sta->pre_wifi_rssi_state[index] ==
+		     BTC_RSSI_STATE_STAY_LOW)) {
+			if (wifi_rssi >=
+			    (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
 				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM,
 					  ALGO_WIFI_RSSI_STATE,
@@ -160,8 +198,8 @@ static u8 Wifi_rssi_state(struct btc_coexist *btcoexist, u8 index,
 		     BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_wifi_rssi_state[index] ==
 		     BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= (rssi_thresh +
-			    BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+			if (wifi_rssi >=
+			    (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
 				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM,
 					  ALGO_WIFI_RSSI_STATE,
@@ -173,11 +211,12 @@ static u8 Wifi_rssi_state(struct btc_coexist *btcoexist, u8 index,
 					  "[BTCoex], wifi RSSI state stay at Low\n");
 			}
 		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
-			    BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_wifi_rssi_state[index] ==
-			    BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (wifi_rssi >= (rssi_thresh1 +
-			    BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
+			BTC_RSSI_STATE_MEDIUM) ||
+			(coex_sta->pre_wifi_rssi_state[index] ==
+			BTC_RSSI_STATE_STAY_MEDIUM)) {
+			if (wifi_rssi >=
+			    (rssi_thresh1 +
+			     BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT)) {
 				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM,
 					  ALGO_WIFI_RSSI_STATE,
@@ -207,34 +246,35 @@ static u8 Wifi_rssi_state(struct btc_coexist *btcoexist, u8 index,
 			}
 		}
 	}
-
 	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
 
 	return wifi_rssi_state;
 }
 
-static void update_ra_mask(struct btc_coexist *btcoexist,
-			   bool force_exec, u32 dis_rate_mask)
+static void halbtc8821a1ant_update_ra_mask(struct btc_coexist *btcoexist,
+					   bool force_exec, u32 dis_rate_mask)
 {
 	coex_dm->cur_ra_mask = dis_rate_mask;
 
-	if (force_exec || (coex_dm->pre_ra_mask != coex_dm->cur_ra_mask)) {
+	if (force_exec ||
+	    (coex_dm->pre_ra_mask != coex_dm->cur_ra_mask)) {
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_UPDATE_ra_mask,
 				   &coex_dm->cur_ra_mask);
 	}
 	coex_dm->pre_ra_mask = coex_dm->cur_ra_mask;
 }
 
-static void auto_rate_fallback_retry(struct btc_coexist *btcoexist,
-				     bool force_exec, u8 type)
+static void btc8821a1ant_auto_rate_fb_retry(struct btc_coexist *btcoexist,
+					    bool force_exec, u8 type)
 {
-	bool wifi_under_b_mode = false;
+	bool	wifi_under_b_mode = false;
 
 	coex_dm->cur_arfr_type = type;
 
-	if (force_exec || (coex_dm->pre_arfr_type != coex_dm->cur_arfr_type)) {
+	if (force_exec ||
+	    (coex_dm->pre_arfr_type != coex_dm->cur_arfr_type)) {
 		switch (coex_dm->cur_arfr_type) {
-		case 0:	/*  normal mode */
+		case 0:	/* normal mode*/
 			btcoexist->btc_write_4byte(btcoexist, 0x430,
 						   coex_dm->backup_arfr_cnt1);
 			btcoexist->btc_write_4byte(btcoexist, 0x434,
@@ -245,16 +285,14 @@ static void auto_rate_fallback_retry(struct btc_coexist *btcoexist,
 					   BTC_GET_BL_WIFI_UNDER_B_MODE,
 					   &wifi_under_b_mode);
 			if (wifi_under_b_mode) {
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x430, 0x0);
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x434,
+				btcoexist->btc_write_4byte(btcoexist, 0x430,
+							   0x0);
+				btcoexist->btc_write_4byte(btcoexist, 0x434,
 							   0x01010101);
 			} else {
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x430, 0x0);
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x434,
+				btcoexist->btc_write_4byte(btcoexist, 0x430,
+							   0x0);
+				btcoexist->btc_write_4byte(btcoexist, 0x434,
 							   0x04030201);
 			}
 			break;
@@ -271,20 +309,20 @@ static void halbtc8821a1ant_retry_limit(struct btc_coexist *btcoexist,
 {
 	coex_dm->cur_retry_limit_type = type;
 
-	if (force_exec || (coex_dm->pre_retry_limit_type !=
-	    coex_dm->cur_retry_limit_type)) {
+	if (force_exec ||
+	    (coex_dm->pre_retry_limit_type != coex_dm->cur_retry_limit_type)) {
 		switch (coex_dm->cur_retry_limit_type) {
-		case 0:	/*  normal mode */
-			btcoexist->btc_write_2byte(btcoexist, 0x42a, coex_dm->backup_retry_limit);
+		case 0:	/* normal mode*/
+			btcoexist->btc_write_2byte(btcoexist, 0x42a,
+						   coex_dm->backup_retry_limit);
 			break;
-		case 1:	/*  retry limit = 8 */
+		case 1:	/* retry limit = 8*/
 			btcoexist->btc_write_2byte(btcoexist, 0x42a, 0x0808);
 			break;
 		default:
 			break;
 		}
 	}
-
 	coex_dm->pre_retry_limit_type = coex_dm->cur_retry_limit_type;
 }
 
@@ -296,10 +334,11 @@ static void halbtc8821a1ant_ampdu_max_time(struct btc_coexist *btcoexist,
 	if (force_exec ||
 	    (coex_dm->pre_ampdu_time_type != coex_dm->cur_ampdu_time_type)) {
 		switch (coex_dm->cur_ampdu_time_type) {
-		case 0:	/*  normal mode */
-			btcoexist->btc_write_1byte(btcoexist, 0x456, coex_dm->backup_ampdu_max_time);
+		case 0:	/* normal mode*/
+			btcoexist->btc_write_1byte(btcoexist, 0x456,
+						   coex_dm->backup_ampdu_max_time);
 			break;
-		case 1:	/*  AMPDU timw = 0x38 * 32us */
+		case 1:	/* AMPDU timw = 0x38 * 32us*/
 			btcoexist->btc_write_1byte(btcoexist, 0x456, 0x38);
 			break;
 		default:
@@ -316,20 +355,22 @@ static void halbtc8821a1ant_limited_tx(struct btc_coexist *btcoexist,
 				       u8 ampdu_time_type)
 {
 	switch (ra_mask_type) {
-	case 0:	/*  normal mode */
-		update_ra_mask(btcoexist, force_exec, 0x0);
+	case 0:	/* normal mode*/
+		halbtc8821a1ant_update_ra_mask(btcoexist, force_exec, 0x0);
 		break;
-	case 1:	/*  disable cck 1/2 */
-		update_ra_mask(btcoexist, force_exec, 0x00000003);
+	case 1:	/* disable cck 1/2*/
+		halbtc8821a1ant_update_ra_mask(btcoexist, force_exec,
+					       0x00000003);
 		break;
-	case 2:	/*  disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4 */
-		update_ra_mask(btcoexist, force_exec, 0x0001f1f7);
+	case 2:	/* disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4*/
+		halbtc8821a1ant_update_ra_mask(btcoexist, force_exec,
+					       0x0001f1f7);
 		break;
 	default:
 		break;
 	}
 
-	auto_rate_fallback_retry(btcoexist, force_exec, arfr_type);
+	btc8821a1ant_auto_rate_fb_retry(btcoexist, force_exec, arfr_type);
 	halbtc8821a1ant_retry_limit(btcoexist, force_exec, retry_limit_type);
 	halbtc8821a1ant_ampdu_max_time(btcoexist, force_exec, ampdu_time_type);
 }
@@ -343,23 +384,24 @@ static void halbtc8821a1ant_limited_rx(struct btc_coexist *btcoexist,
 	bool bt_ctrl_rx_agg_size = bt_ctrl_agg_buf_size;
 	u8 rx_agg_size = agg_buf_size;
 
-	/*  */
-	/*	Rx Aggregation related setting */
-	/*  */
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &reject_rx_agg);
-	/*  decide BT control aggregation buf size or not */
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &bt_ctrl_rx_agg_size);
-	/*  aggregation buf size, only work when BT control Rx aggregation size. */
+	/*============================================*/
+	/*	Rx Aggregation related setting*/
+	/*============================================*/
+	btcoexist->btc_set(btcoexist,
+		 BTC_SET_BL_TO_REJ_AP_AGG_PKT, &reject_rx_agg);
+	/* decide BT control aggregation buf size or not*/
+	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE,
+			   &bt_ctrl_rx_agg_size);
+	/* aggregation buf size, only work when BT control Rx agg size.*/
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rx_agg_size);
-	/*  real update aggregation setting */
+	/* real update aggregation setting*/
 	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
-
 }
 
 static void halbtc8821a1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 {
-	u32 reg_hp_tx_rx, reg_lp_tx_rx, u4_tmp;
-	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
+	u32	reg_hp_tx_rx, reg_lp_tx_rx, u4_tmp;
+	u32	reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
 
 	reg_hp_tx_rx = 0x770;
 	reg_lp_tx_rx = 0x774;
@@ -377,7 +419,7 @@ static void halbtc8821a1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 	coex_sta->low_priority_tx = reg_lp_tx;
 	coex_sta->low_priority_rx = reg_lp_rx;
 
-	/*  reset counter */
+	/* reset counter*/
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
@@ -387,11 +429,11 @@ static void halbtc8821a1ant_query_bt_info(struct btc_coexist *btcoexist)
 
 	coex_sta->c2h_bt_info_req_sent = true;
 
-	h2c_parameter[0] |= BIT(0);	/*  trigger */
+	h2c_parameter[0] |= BIT0;	/* trigger*/
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
-		h2c_parameter[0]);
+		  h2c_parameter[0]);
 
 	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
 }
@@ -399,7 +441,7 @@ static void halbtc8821a1ant_query_bt_info(struct btc_coexist *btcoexist)
 static void halbtc8821a1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info	*bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hs_on = false;
+	bool	bt_hs_on = false;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 
@@ -409,44 +451,44 @@ static void halbtc8821a1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 	bt_link_info->pan_exist = coex_sta->pan_exist;
 	bt_link_info->hid_exist = coex_sta->hid_exist;
 
-	/*  work around for HS mode. */
+	/* work around for HS mode.*/
 	if (bt_hs_on) {
 		bt_link_info->pan_exist = true;
 		bt_link_info->bt_link_exist = true;
 	}
 
-	/*  check if Sco only */
+	/* check if Sco only*/
 	if (bt_link_info->sco_exist &&
-		!bt_link_info->a2dp_exist &&
-		!bt_link_info->pan_exist &&
-		!bt_link_info->hid_exist)
+	    !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist &&
+	    !bt_link_info->hid_exist)
 		bt_link_info->sco_only = true;
 	else
 		bt_link_info->sco_only = false;
 
-	/*  check if A2dp only */
+	/* check if A2dp only*/
 	if (!bt_link_info->sco_exist &&
-		bt_link_info->a2dp_exist &&
-		!bt_link_info->pan_exist &&
-		!bt_link_info->hid_exist)
+	    bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist &&
+	    !bt_link_info->hid_exist)
 		bt_link_info->a2dp_only = true;
 	else
 		bt_link_info->a2dp_only = false;
 
-	/*  check if Pan only */
+	/* check if Pan only*/
 	if (!bt_link_info->sco_exist &&
-		!bt_link_info->a2dp_exist &&
-		bt_link_info->pan_exist &&
-		!bt_link_info->hid_exist)
+	    !bt_link_info->a2dp_exist &&
+	    bt_link_info->pan_exist &&
+	    !bt_link_info->hid_exist)
 		bt_link_info->pan_only = true;
 	else
 		bt_link_info->pan_only = false;
 
-	/*  check if Hid only */
+	/* check if Hid only*/
 	if (!bt_link_info->sco_exist &&
-		!bt_link_info->a2dp_exist &&
-		!bt_link_info->pan_exist &&
-		bt_link_info->hid_exist)
+	    !bt_link_info->a2dp_exist &&
+	    !bt_link_info->pan_exist &&
+	    bt_link_info->hid_exist)
 		bt_link_info->hid_only = true;
 	else
 		bt_link_info->hid_only = false;
@@ -455,14 +497,15 @@ static void halbtc8821a1ant_update_bt_link_info(struct btc_coexist *btcoexist)
 static u8 halbtc8821a1ant_action_algorithm(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hs_on = false;
-	u8 algorithm = BT_8821A_1ANT_COEX_ALGO_UNDEFINED;
-	u8 num_of_diff_profile = 0;
+	bool	bt_hs_on = false;
+	u8	algorithm = BT_8821A_1ANT_COEX_ALGO_UNDEFINED;
+	u8	num_of_diff_profile = 0;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 
 	if (!bt_link_info->bt_link_exist) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], No BT link exists!!!\n");
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], No BT link exists!!!\n");
 		return algorithm;
 	}
 
@@ -477,21 +520,26 @@ static u8 halbtc8821a1ant_action_algorithm(struct btc_coexist *btcoexist)
 
 	if (num_of_diff_profile == 1) {
 		if (bt_link_info->sco_exist) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO only\n");
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], BT Profile = SCO only\n");
 			algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
 		} else {
 			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID only\n");
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = HID only\n");
 				algorithm = BT_8821A_1ANT_COEX_ALGO_HID;
 			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = A2DP only\n");
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = A2DP only\n");
 				algorithm = BT_8821A_1ANT_COEX_ALGO_A2DP;
 			} else if (bt_link_info->pan_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = PAN(HS) only\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = PAN(HS) only\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANHS;
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = PAN(EDR) only\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = PAN(EDR) only\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR;
 				}
 			}
@@ -499,41 +547,50 @@ static u8 halbtc8821a1ant_action_algorithm(struct btc_coexist *btcoexist)
 	} else if (num_of_diff_profile == 2) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID\n");
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = SCO + HID\n");
 				algorithm = BT_8821A_1ANT_COEX_ALGO_HID;
 			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + A2DP ==> SCO\n");
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = SCO + A2DP ==> SCO\n");
 				algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
 			} else if (bt_link_info->pan_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + PAN(HS)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + PAN(HS)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + PAN(EDR)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + PAN(EDR)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
 				}
 			}
 		} else {
 			if (bt_link_info->hid_exist &&
-				bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + A2DP\n");
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = HID + A2DP\n");
 				algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
 			} else if (bt_link_info->hid_exist &&
-				bt_link_info->pan_exist) {
+				   bt_link_info->pan_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + PAN(HS)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + PAN(HS)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + PAN(EDR)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + PAN(EDR)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
 				}
 			} else if (bt_link_info->pan_exist &&
-				bt_link_info->a2dp_exist) {
+				   bt_link_info->a2dp_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = A2DP + PAN(HS)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = A2DP + PAN(HS)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_A2DP_PANHS;
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = A2DP + PAN(EDR)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = A2DP + PAN(EDR)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_A2DP;
 				}
 			}
@@ -541,37 +598,44 @@ static u8 halbtc8821a1ant_action_algorithm(struct btc_coexist *btcoexist)
 	} else if (num_of_diff_profile == 3) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist &&
-				bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + A2DP ==> HID\n");
+			    bt_link_info->a2dp_exist) {
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], BT Profile = SCO + HID + A2DP ==> HID\n");
 				algorithm = BT_8821A_1ANT_COEX_ALGO_HID;
 			} else if (bt_link_info->hid_exist &&
 				bt_link_info->pan_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + PAN(HS)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + HID + PAN(HS)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + PAN(EDR)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + HID + PAN(EDR)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
 				}
 			} else if (bt_link_info->pan_exist &&
 				bt_link_info->a2dp_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + A2DP + PAN(HS)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + A2DP + PAN(HS)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_SCO;
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + A2DP + PAN(EDR) ==> HID\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + A2DP + PAN(EDR) ==> HID\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
 				}
 			}
 		} else {
 			if (bt_link_info->hid_exist &&
-				bt_link_info->pan_exist &&
-				bt_link_info->a2dp_exist) {
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + A2DP + PAN(HS)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + A2DP + PAN(HS)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP;
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = HID + A2DP + PAN(EDR)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = HID + A2DP + PAN(EDR)\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
 				}
 			}
@@ -579,18 +643,20 @@ static u8 halbtc8821a1ant_action_algorithm(struct btc_coexist *btcoexist)
 	} else if (num_of_diff_profile >= 3) {
 		if (bt_link_info->sco_exist) {
 			if (bt_link_info->hid_exist &&
-				bt_link_info->pan_exist &&
-				bt_link_info->a2dp_exist) {
+			    bt_link_info->pan_exist &&
+			    bt_link_info->a2dp_exist) {
 				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Error!!! BT Profile = SCO + HID + A2DP + PAN(HS)\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], Error!!! BT Profile = SCO + HID + A2DP + PAN(HS)\n");
+
 				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
+					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+						  "[BTCoex], BT Profile = SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n");
 					algorithm = BT_8821A_1ANT_COEX_ALGO_PANEDR_HID;
 				}
 			}
 		}
 	}
-
 	return algorithm;
 }
 
@@ -602,12 +668,12 @@ static void halbtc8821a1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 	h2c_parameter[0] = 0;
 
 	if (enable_auto_report)
-		h2c_parameter[0] |= BIT(0);
+		h2c_parameter[0] |= BIT0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], BT FW auto report : %s, FW write 0x68 = 0x%x\n",
 		  (enable_auto_report ? "Enabled!!" : "Disabled!!"),
-		  h2c_parameter[0]);
+		   h2c_parameter[0]);
 
 	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
 }
@@ -616,13 +682,17 @@ static void halbtc8821a1ant_bt_auto_report(struct btc_coexist *btcoexist,
 					   bool force_exec,
 					   bool enable_auto_report)
 {
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, "[BTCoex], %s BT Auto report = %s\n",
-		(force_exec ? "force to" : ""), ((enable_auto_report) ? "Enabled" : "Disabled"));
+	BTC_PRINT(BTC_MSG_ALGORITHM,
+		  ALGO_TRACE_FW, "[BTCoex], %s BT Auto report = %s\n",
+		  (force_exec ? "force to" : ""), ((enable_auto_report) ?
+		  "Enabled" : "Disabled"));
 	coex_dm->cur_bt_auto_report = enable_auto_report;
 
 	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], pre_bt_auto_report =%d, cur_bt_auto_report =%d\n",
-			coex_dm->pre_bt_auto_report, coex_dm->cur_bt_auto_report);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], pre_bt_auto_report = %d, cur_bt_auto_report = %d\n",
+			  coex_dm->pre_bt_auto_report,
+			  coex_dm->cur_bt_auto_report);
 
 		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
 			return;
@@ -632,20 +702,20 @@ static void halbtc8821a1ant_bt_auto_report(struct btc_coexist *btcoexist,
 	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
 }
 
-static void set_sw_penalty_tx_rate_adaptive(struct btc_coexist *btcoexist,
+static void btc8821a1ant_set_sw_pen_tx_rate(struct btc_coexist *btcoexist,
 					    bool low_penalty_ra)
 {
 	u8 h2c_parameter[6] = {0};
 
-	h2c_parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+	h2c_parameter[0] = 0x6;	/* opCode, 0x6= Retry_Penalty*/
 
 	if (low_penalty_ra) {
-		h2c_parameter[1] |= BIT(0);
-		h2c_parameter[2] = 0x00;  /* normal rate except MCS7/6/5,
-					   * OFDM54/48/36 */
-		h2c_parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
-		h2c_parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
-		h2c_parameter[5] = 0xf9;  /* MCS5 or OFDM36  */
+		h2c_parameter[1] |= BIT0;
+		/*normal rate except MCS7/6/5, OFDM54/48/36*/
+		h2c_parameter[2] = 0x00;
+		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54*/
+		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48*/
+		h2c_parameter[5] = 0xf9;	/*MCS5 or OFDM36*/
 	}
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
@@ -664,7 +734,7 @@ static void halbtc8821a1ant_low_penalty_ra(struct btc_coexist *btcoexist,
 		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
 			return;
 	}
-	set_sw_penalty_tx_rate_adaptive(btcoexist, coex_dm->cur_low_penalty_ra);
+	btc8821a1ant_set_sw_pen_tx_rate(btcoexist, coex_dm->cur_low_penalty_ra);
 
 	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
 }
@@ -724,57 +794,50 @@ static void halbtc8821a1ant_coex_table_with_type(struct btc_coexist *btcoexist,
 {
 	switch (type) {
 	case 0:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0x55555555, 0x55555555,
-					   0xffffff, 0x3);
+		halbtc8821a1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0x55555555, 0xffffff, 0x3);
 		break;
 	case 1:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0x55555555, 0x5a5a5a5a,
-					   0xffffff, 0x3);
-		break;
+			halbtc8821a1ant_coex_table(btcoexist, force_exec,
+						   0x55555555, 0x5a5a5a5a,
+						   0xffffff, 0x3);
+			break;
 	case 2:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0x5a5a5a5a, 0x5a5a5a5a,
-					   0xffffff, 0x3);
+		halbtc8821a1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
+					   0x5a5a5a5a, 0xffffff, 0x3);
 		break;
 	case 3:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0x55555555, 0xaaaaaaaa,
-					   0xffffff, 0x3);
+		halbtc8821a1ant_coex_table(btcoexist, force_exec, 0x55555555,
+					   0xaaaaaaaa, 0xffffff, 0x3);
 		break;
 	case 4:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0xffffffff, 0xffffffff,
-					   0xffffff, 0x3);
+		halbtc8821a1ant_coex_table(btcoexist, force_exec, 0xffffffff,
+					   0xffffffff, 0xffffff, 0x3);
 		break;
 	case 5:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0x5fff5fff, 0x5fff5fff,
-					   0xffffff, 0x3);
+		halbtc8821a1ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
+					   0x5fff5fff, 0xffffff, 0x3);
 		break;
 	case 6:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0x55ff55ff, 0x5a5a5a5a,
-					   0xffffff, 0x3);
+		halbtc8821a1ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
+					   0x5a5a5a5a, 0xffffff, 0x3);
 		break;
 	case 7:
-		halbtc8821a1ant_coex_table(btcoexist, force_exec,
-					   0x5afa5afa, 0x5afa5afa,
-					   0xffffff, 0x3);
+		halbtc8821a1ant_coex_table(btcoexist, force_exec, 0x5afa5afa,
+					   0x5afa5afa, 0xffffff, 0x3);
 		break;
 	default:
 		break;
 	}
 }
 
-static void halbtc8821a1ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
-						   bool enable)
+static void btc8821a1ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
+						bool enable)
 {
-	u8 h2c_parameter[1] = {0};
+	u8	h2c_parameter[1] = {0};
 
 	if (enable)
-		h2c_parameter[0] |= BIT(0);	/*  function enable */
+		h2c_parameter[0] |= BIT0;	/* function enable*/
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
@@ -801,20 +864,14 @@ static void halbtc8821a1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 		    coex_dm->cur_ignore_wlan_act)
 			return;
 	}
-	halbtc8821a1ant_set_fw_ignore_wlan_act(btcoexist, enable);
+	btc8821a1ant_set_fw_ignore_wlan_act(btcoexist, enable);
 
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
 
-static void
-halbtc8821a1ant_set_fw_pstdma(
-	struct btc_coexist *btcoexist,
-	u8 byte1,
-	u8 byte2,
-	u8 byte3,
-	u8 byte4,
-	u8 byte5
-	)
+static void halbtc8821a1ant_set_fw_pstdma(struct btc_coexist *btcoexist,
+					  u8 byte1, u8 byte2, u8 byte3,
+					  u8 byte4, u8 byte5)
 {
 	u8 h2c_parameter[5] = {0};
 
@@ -830,48 +887,45 @@ halbtc8821a1ant_set_fw_pstdma(
 	coex_dm->ps_tdma_para[3] = byte4;
 	coex_dm->ps_tdma_para[4] = byte5;
 
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, "[BTCoex], PS-TDMA H2C cmd = 0x%x%08x\n",
-		h2c_parameter[0],
-		h2c_parameter[1]<<24|h2c_parameter[2]<<16|h2c_parameter[3]<<8|h2c_parameter[4]);
-
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
+		  "[BTCoex], PS-TDMA H2C cmd =0x%x%08x\n",
+		  h2c_parameter[0],
+		  h2c_parameter[1]<<24 |
+		  h2c_parameter[2]<<16 |
+		  h2c_parameter[3]<<8 |
+		  h2c_parameter[4]);
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-static void
-halbtc8821a1ant_set_lps_rpwm(
-	struct btc_coexist *btcoexist,
-	u8 lps_val,
-	u8 rpwm_val
-	)
+static void halbtc8821a1ant_set_lps_rpwm(struct btc_coexist *btcoexist,
+					 u8 lps_val, u8 rpwm_val)
 {
-	u8 lps = lps_val;
-	u8 rpwm = rpwm_val;
+	u8	lps = lps_val;
+	u8	rpwm = rpwm_val;
 
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_LPS_VAL, &lps);
 	btcoexist->btc_set(btcoexist, BTC_SET_U1_RPWM_VAL, &rpwm);
 }
 
-static void
-halbtc8821a1ant_lps_rpwm(
-	struct btc_coexist *btcoexist,
-	bool force_exec,
-	u8 lps_val,
-	u8 rpwm_val
-	)
+static void halbtc8821a1ant_lps_rpwm(struct btc_coexist *btcoexist,
+				     bool force_exec, u8 lps_val, u8 rpwm_val)
 {
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, "[BTCoex], %s set lps/rpwm = 0x%x/0x%x\n",
-		(force_exec ? "force to" : ""), lps_val, rpwm_val);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], %s set lps/rpwm = 0x%x/0x%x\n",
+		  (force_exec ? "force to" : ""), lps_val, rpwm_val);
 	coex_dm->cur_lps = lps_val;
 	coex_dm->cur_rpwm = rpwm_val;
 
 	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], LPS-RxBeaconMode = 0x%x , LPS-RPWM = 0x%x!!\n",
-			 coex_dm->cur_lps, coex_dm->cur_rpwm);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], LPS-RxBeaconMode = 0x%x, LPS-RPWM = 0x%x!!\n",
+			  coex_dm->cur_lps, coex_dm->cur_rpwm);
 
 		if ((coex_dm->pre_lps == coex_dm->cur_lps) &&
-			(coex_dm->pre_rpwm == coex_dm->cur_rpwm)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], LPS-RPWM_Last = 0x%x , LPS-RPWM_Now = 0x%x!!\n",
-				 coex_dm->pre_rpwm, coex_dm->cur_rpwm);
+		    (coex_dm->pre_rpwm == coex_dm->cur_rpwm)) {
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], LPS-RPWM_Last = 0x%x, LPS-RPWM_Now = 0x%x!!\n",
+				  coex_dm->pre_rpwm, coex_dm->cur_rpwm);
 
 			return;
 		}
@@ -882,93 +936,98 @@ halbtc8821a1ant_lps_rpwm(
 	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
 }
 
-static void
-halbtc8821a1ant_sw_mechanism(
-	struct btc_coexist *btcoexist,
-	bool low_penalty_ra
-	)
+static void halbtc8821a1ant_sw_mechanism(struct btc_coexist *btcoexist,
+					 bool low_penalty_ra)
 {
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, "[BTCoex], SM[LpRA] = %d\n", low_penalty_ra);
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
+		  "[BTCoex], SM[LpRA] = %d\n", low_penalty_ra);
 
 	halbtc8821a1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
 }
 
-static void
-halbtc8821a1ant_set_ant_path(
-	struct btc_coexist *btcoexist,
-	u8 ant_pos_type,
-	bool init_hw_cfg,
-	bool wifi_off
-	)
+static void halbtc8821a1ant_set_ant_path(struct btc_coexist *btcoexist,
+					 u8 ant_pos_type, bool init_hw_cfg,
+					 bool wifi_off)
 {
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	u32 u4_tmp = 0;
 	u8 h2c_parameter[2] = {0};
 
 	if (init_hw_cfg) {
-		/*  0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT */
+		/* 0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT*/
 		u4_tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u4_tmp &= ~BIT(23);
-		u4_tmp |= BIT(24);
+		u4_tmp &= ~BIT23;
+		u4_tmp |= BIT24;
 		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4_tmp);
 
 		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x975, 0x3, 0x3);
 		btcoexist->btc_write_1byte(btcoexist, 0xcb4, 0x77);
 
 		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
-			/* tell firmware "antenna inverse"  ==> WRONG firmware antenna control code.==>need fw to fix */
+			/*tell firmware "antenna inverse"  ==>
+			 * WRONG firmware antenna control code.==>need fw to fix
+			 */
 			h2c_parameter[0] = 1;
 			h2c_parameter[1] = 1;
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2, h2c_parameter);
-
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1, 0x1); /* Main Ant to  BT for IPS case 0x4c[23] = 1 */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+			/*Main Ant to  BT for IPS case 0x4c[23] = 1*/
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64,
+							   0x1, 0x1);
 		} else {
-			/* tell firmware "no antenna inverse" ==> WRONG firmware antenna control code.==>need fw to fix */
+			/*tell firmware "no antenna inverse" ==>
+			 * WRONG firmware antenna control code.==>need fw to fix
+			 */
 			h2c_parameter[0] = 0;
 			h2c_parameter[1] = 1;
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2, h2c_parameter);
-
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64, 0x1, 0x0); /* Aux Ant to  BT for IPS case 0x4c[23] = 1 */
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
+			/*Aux Ant to  BT for IPS case 0x4c[23] = 1*/
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x64,
+							   0x1, 0x0);
 		}
 	} else if (wifi_off) {
-		/*  0x4c[24:23] = 00, Set Antenna control by BT_RFE_CTRL	BT Vendor 0xac = 0xf002 */
+		/* 0x4c[24:23] = 00, Set Antenna control
+		 *	by BT_RFE_CTRL	BT Vendor 0xac = 0xf002
+		 */
 		u4_tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u4_tmp &= ~BIT(23);
-		u4_tmp &= ~BIT(24);
+		u4_tmp &= ~BIT23;
+		u4_tmp &= ~BIT24;
 		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4_tmp);
 	}
 
-	/*  ext switch setting */
+	/* ext switch setting*/
 	switch (ant_pos_type) {
 	case BTC_ANT_PATH_WIFI:
 		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7,
+							   0x30, 0x1);
 		else
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7,
+							   0x30, 0x2);
 		break;
 	case BTC_ANT_PATH_BT:
 		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7,
+							   0x30, 0x2);
 		else
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7,
+							   0x30, 0x1);
 		break;
-	case BTC_ANT_PATH_PTA:
 	default:
+	case BTC_ANT_PATH_PTA:
 		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7,
+							   0x30, 0x1);
 		else
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x2);
+			btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7,
+							   0x30, 0x2);
 		break;
 	}
 }
 
-static void
-halbtc8821a1ant_ps_tdma(
-	struct btc_coexist *btcoexist,
-	bool force_exec,
-	bool turn_on,
-	u8 type
-	)
+static void halbtc8821a1ant_ps_tdma(struct btc_coexist *btcoexist,
+				    bool force_exec, bool turn_on, u8 type)
 {
 	u8 rssi_adjust_val = 0;
 
@@ -977,241 +1036,299 @@ halbtc8821a1ant_ps_tdma(
 
 	if (!force_exec) {
 		if (coex_dm->cur_ps_tdma_on) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], ********** TDMA(on, %d) **********\n",
-				coex_dm->cur_ps_tdma);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], ********** TDMA(on, %d) **********\n",
+				  coex_dm->cur_ps_tdma);
 		} else {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], ********** TDMA(off, %d) **********\n",
-				coex_dm->cur_ps_tdma);
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], ********** TDMA(off, %d) **********\n",
+				  coex_dm->cur_ps_tdma);
 		}
-
 		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
-			(coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
+		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
 			return;
 	}
 	if (turn_on) {
 		switch (type) {
 		default:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1a, 0x1a, 0x0, 0x50);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1a,
+						      0x1a, 0x0, 0x50);
 			break;
 		case 1:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x3a, 0x03, 0x10, 0x50);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x3a,
+						      0x03, 0x10, 0x50);
 			rssi_adjust_val = 11;
 			break;
 		case 2:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x2b, 0x03, 0x10, 0x50);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x2b,
+						      0x03, 0x10, 0x50);
 			rssi_adjust_val = 14;
 			break;
 		case 3:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1d, 0x1d, 0x0, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x1d,
+						      0x1d, 0x0, 0x10);
 			break;
 		case 4:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x15,
+						      0x3, 0x14, 0x0);
 			rssi_adjust_val = 17;
 			break;
 		case 5:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x15, 0x3, 0x11, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x15,
+						      0x3, 0x11, 0x10);
 			break;
 		case 6:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0x3, 0x0, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa,
+						      0x3, 0x0, 0x0);
 			break;
 		case 7:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xc,
+						      0x5, 0x0, 0x0);
 			break;
 		case 8:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x25,
+						      0x3, 0x10, 0x0);
 			break;
 		case 9:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x21, 0x3, 0x10, 0x50);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x21,
+						      0x3, 0x10, 0x50);
 			rssi_adjust_val = 18;
 			break;
 		case 10:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa,
+						      0xa, 0x0, 0x40);
 			break;
 		case 11:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x14, 0x03, 0x10, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x14,
+						      0x03, 0x10, 0x10);
 			rssi_adjust_val = 20;
 			break;
 		case 12:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x0a, 0x0a, 0x0, 0x50);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x0a,
+						      0x0a, 0x0, 0x50);
 			break;
 		case 13:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x18, 0x18, 0x0, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x18,
+						      0x18, 0x0, 0x10);
 			break;
 		case 14:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x21, 0x3, 0x10, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x21,
+						      0x3, 0x10, 0x10);
 			break;
 		case 15:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa, 0x3, 0x8, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x13, 0xa,
+						      0x3, 0x8, 0x0);
 			break;
 		case 16:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x15, 0x3, 0x10, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x15,
+						      0x3, 0x10, 0x0);
 			rssi_adjust_val = 18;
 			break;
 		case 18:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x93, 0x25,
+						      0x3, 0x10, 0x0);
 			rssi_adjust_val = 14;
 			break;
 		case 20:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x35, 0x03, 0x11, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x35,
+						      0x03, 0x11, 0x10);
 			break;
 		case 21:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x15, 0x03, 0x11, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x15,
+						      0x03, 0x11, 0x10);
 			break;
 		case 22:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x25, 0x03, 0x11, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0x25,
+						      0x03, 0x11, 0x10);
 			break;
 		case 23:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x18);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x25,
+						      0x3, 0x31, 0x18);
 			rssi_adjust_val = 22;
 			break;
 		case 24:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x15, 0x3, 0x31, 0x18);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x15,
+						      0x3, 0x31, 0x18);
 			rssi_adjust_val = 22;
 			break;
 		case 25:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0xa,
+						      0x3, 0x31, 0x18);
 			rssi_adjust_val = 22;
 			break;
 		case 26:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0xa,
+						      0x3, 0x31, 0x18);
 			rssi_adjust_val = 22;
 			break;
 		case 27:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x98);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xe3, 0x25,
+						      0x3, 0x31, 0x98);
 			rssi_adjust_val = 22;
 			break;
 		case 28:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x69, 0x25, 0x3, 0x31, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x69, 0x25,
+						      0x3, 0x31, 0x0);
 			break;
 		case 29:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xab, 0x1a, 0x1a, 0x1, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xab, 0x1a,
+						      0x1a, 0x1, 0x10);
 			break;
 		case 30:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x14, 0x3, 0x10, 0x50);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x51, 0x14,
+						      0x3, 0x10, 0x50);
 			break;
 		case 31:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0, 0x58);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x1a,
+						      0x1a, 0, 0x58);
 			break;
 		case 32:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0xa, 0x3, 0x10, 0x0);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x61, 0xa,
+						      0x3, 0x10, 0x0);
 			break;
 		case 33:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xa3, 0x25, 0x3, 0x30, 0x90);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xa3, 0x25,
+						      0x3, 0x30, 0x90);
 			break;
 		case 34:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x53, 0x1a, 0x1a, 0x0, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x53, 0x1a,
+						      0x1a, 0x0, 0x10);
 			break;
 		case 35:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x63, 0x1a, 0x1a, 0x0, 0x10);
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x63, 0x1a,
+						      0x1a, 0x0, 0x10);
 			break;
 		case 36:
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x12, 0x3, 0x14, 0x50);
-				break;
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0xd3, 0x12,
+						      0x3, 0x14, 0x50);
+			break;
 		}
 	} else {
-		/*  disable PS tdma */
+		/* disable PS tdma*/
 		switch (type) {
-		case 8: /* PTA Control */
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
-			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA, false, false);
+		case 8: /*PTA Control*/
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x8, 0x0, 0x0,
+						      0x0, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
+						     false, false);
 			break;
 		case 0:
-		default:  /* Software control, Antenna at BT side */
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
-			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false, false);
+		default:  /*Software control, Antenna at BT side*/
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x0, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
+						     false, false);
 			break;
-		case 9:   /* Software control, Antenna at WiFi side */
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
-			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_WIFI, false, false);
+		case 9:   /*Software control, Antenna at WiFi side*/
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x0, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_WIFI,
+						     false, false);
 			break;
-		case 10:	/*  under 5G */
-			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
-			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false, false);
+		case 10:	/* under 5G*/
+			halbtc8821a1ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
+						      0x8, 0x0);
+			halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
+						     false, false);
 			break;
 		}
 	}
 	rssi_adjust_val = 0;
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssi_adjust_val);
+	btcoexist->btc_set(btcoexist,
+		 BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssi_adjust_val);
 
-	/*  update pre state */
+	/* update pre state*/
 	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
-static bool
-halbtc8821a1ant_is_common_action(
-	struct btc_coexist *btcoexist
-	)
+static bool halbtc8821a1ant_is_common_action(struct btc_coexist *btcoexist)
 {
-	bool bCommon = false, wifi_connected = false, wifi_busy = false;
+	bool	common = false, wifi_connected = false, wifi_busy = false;
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			   &wifi_connected);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 
 	if (!wifi_connected &&
-		BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n");
+	    BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+	    coex_dm->bt_status) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n");
 		halbtc8821a1ant_sw_mechanism(btcoexist, false);
 
-		bCommon = true;
+		common = true;
 	} else if (wifi_connected &&
-			(BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi connected + BT non connected-idle!!\n");
+		   (BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi connected + BT non connected-idle!!\n");
 		halbtc8821a1ant_sw_mechanism(btcoexist, false);
 
-		bCommon = true;
+		common = true;
 	} else if (!wifi_connected &&
-		(BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi non connected-idle + BT connected-idle!!\n");
+		   (BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + BT connected-idle!!\n");
 		halbtc8821a1ant_sw_mechanism(btcoexist, false);
 
-		bCommon = true;
+		common = true;
 	} else if (wifi_connected &&
-		(BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi connected + BT connected-idle!!\n");
+		   (BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		   coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi connected + BT connected-idle!!\n");
 		halbtc8821a1ant_sw_mechanism(btcoexist, false);
 
-		bCommon = true;
+		common = true;
 	} else if (!wifi_connected &&
-		(BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE != coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi non connected-idle + BT Busy!!\n");
+		   (BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE !=
+		    coex_dm->bt_status)) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], Wifi non connected-idle + BT Busy!!\n");
 		halbtc8821a1ant_sw_mechanism(btcoexist, false);
 
-		bCommon = true;
+		common = true;
 	} else {
 		if (wifi_busy) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi Connected-Busy + BT Busy!!\n");
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Connected-Busy + BT Busy!!\n");
 		} else {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Wifi Connected-Idle + BT Busy!!\n");
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+				  "[BTCoex], Wifi Connected-Idle + BT Busy!!\n");
 		}
 
-		bCommon = false;
+		common = false;
 	}
 
-	return bCommon;
+	return common;
 }
 
-static void
-halbtc8821a1ant_tdma_duration_adjust_for_acl(
-	struct btc_coexist *btcoexist,
-	u8 wifi_status
-	)
+static void btc8821a1ant_tdma_dur_adj(struct btc_coexist *btcoexist,
+				      u8 wifi_status)
 {
 	static long		up, dn, m, n, wait_count;
-	long			result;   /* 0: no change, +1: increase WiFi duration, -1: decrease WiFi duration */
-	u8 retry_count = 0, bt_info_ext;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, "[BTCoex], TdmaDurationAdjustForAcl()\n");
+	/*0: no change, +1: increase WiFi duration, -1: decrease WiFi duration*/
+	long			result;
+	u8			retry_count = 0, bt_info_ext;
 
-	if ((BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifi_status) ||
-		(BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifi_status) ||
-		(BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifi_status)) {
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
+		  "[BTCoex], TdmaDurationAdjustForAcl()\n");
+
+	if ((BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN ==
+	     wifi_status) ||
+	    (BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN ==
+	     wifi_status) ||
+	    (BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT ==
+	     wifi_status)) {
 		if (coex_dm->cur_ps_tdma != 1 &&
-			coex_dm->cur_ps_tdma != 2 &&
-			coex_dm->cur_ps_tdma != 3 &&
-			coex_dm->cur_ps_tdma != 9) {
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-			coex_dm->ps_tdma_du_adj_type = 9;
+		    coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 3 &&
+		    coex_dm->cur_ps_tdma != 9) {
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+			coex_dm->tdma_adj_type = 9;
 
 			up = 0;
 			dn = 0;
@@ -1225,11 +1342,12 @@ halbtc8821a1ant_tdma_duration_adjust_for_acl(
 
 	if (!coex_dm->auto_tdma_adjust) {
 		coex_dm->auto_tdma_adjust = true;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], first run TdmaDurationAdjust()!!\n");
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
 
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
-		coex_dm->ps_tdma_du_adj_type = 2;
-		/*  */
+		coex_dm->tdma_adj_type = 2;
+		/*============*/
 		up = 0;
 		dn = 0;
 		m = 1;
@@ -1237,17 +1355,14 @@ halbtc8821a1ant_tdma_duration_adjust_for_acl(
 		result = 0;
 		wait_count = 0;
 	} else {
-		/* accquire the BT TRx retry count from BT_Info byte2 */
+		/*accquire the BT TRx retry count from BT_Info byte2*/
 		retry_count = coex_sta->bt_retry_cnt;
 		bt_info_ext = coex_sta->bt_info_ext;
-		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], retry_count = %d\n", retry_count)); */
-		/* BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_count =%d\n",  */
-		/*	up, dn, m, n, wait_count)); */
 		result = 0;
 		wait_count++;
 
 		if (retry_count == 0) {
-			/*  no retry in the last 2-second duration */
+			/* no retry in the last 2-second duration*/
 			up++;
 			dn--;
 
@@ -1255,14 +1370,20 @@ halbtc8821a1ant_tdma_duration_adjust_for_acl(
 				dn = 0;
 
 			if (up >= n) {
+				/* if (retry count == 0) for 2*n seconds ,
+				 * make WiFi duration wider
+				 */
 				wait_count = 0;
 				n = 3;
 				up = 0;
 				dn = 0;
 				result = 1;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], Increase wifi duration!!\n");
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Increase wifi duration!!\n");
 			}
 		} else if (retry_count <= 3) {
+			/* <=3 retry in the last 2-second duration*/
 			up--;
 			dn++;
 
@@ -1270,25 +1391,40 @@ halbtc8821a1ant_tdma_duration_adjust_for_acl(
 				up = 0;
 
 			if (dn == 2) {
+				/* if retry count< 3 for 2*2 seconds,
+				 * shrink wifi duration
+				 */
 				if (wait_count <= 2)
-					m++;
+					m++; /* avoid bounce in two levels */
 				else
 					m = 1;
-				if (m >= 20)
-					m = 20;
 
+				if (m >= 20) {
+					/* m max value is 20, max time is 120 s,
+					 *	recheck if adjust WiFi duration.
+					 */
+					m = 20;
+				}
 				n = 3*m;
 				up = 0;
 				dn = 0;
 				wait_count = 0;
 				result = -1;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], Decrease wifi duration for retryCounter<3!!\n");
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_TRACE_FW_DETAIL,
+					  "[BTCoex], Decrease wifi duration for retryCounter<3!!\n");
 			}
 		} else {
+			/* retry count > 3, if retry count > 3 happens once,
+			 *	shrink WiFi duration
+			 */
 			if (wait_count == 1)
-				m++;
+				m++; /* avoid bounce in two levels */
 			else
 				m = 1;
+		/* m max value is 20, max time is 120 second,
+		 *	recheck if adjust WiFi duration.
+		*/
 			if (m >= 20)
 				m = 20;
 
@@ -1297,110 +1433,123 @@ halbtc8821a1ant_tdma_duration_adjust_for_acl(
 			dn = 0;
 			wait_count = 0;
 			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], Decrease wifi duration for retryCounter>3!!\n");
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], Decrease wifi duration for retryCounter>3!!\n");
 		}
 
 		if (result == -1) {
 			if ((BT_INFO_8821A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
-				((coex_dm->cur_ps_tdma == 1) || (coex_dm->cur_ps_tdma == 2))) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+			    ((coex_dm->cur_ps_tdma == 1) ||
+			     (coex_dm->cur_ps_tdma == 2))) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 1) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
 			} else if (coex_dm->cur_ps_tdma == 2) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 9) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
-				coex_dm->ps_tdma_du_adj_type = 11;
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
 			}
 		} else if (result == 1) {
 			if ((BT_INFO_8821A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
-				((coex_dm->cur_ps_tdma == 1) || (coex_dm->cur_ps_tdma == 2))) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+			    ((coex_dm->cur_ps_tdma == 1) ||
+			     (coex_dm->cur_ps_tdma == 2))) {
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 11) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->tdma_adj_type = 9;
 			} else if (coex_dm->cur_ps_tdma == 9) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
 			} else if (coex_dm->cur_ps_tdma == 2) {
-				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
-				coex_dm->ps_tdma_du_adj_type = 1;
+				halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 1);
+				coex_dm->tdma_adj_type = 1;
 			}
 		} else {
-			/* no change */
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], ********** TDMA(on, %d) **********\n",
+			/*no change*/
+			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+				  "[BTCoex], ********** TDMA(on, %d) **********\n",
 				coex_dm->cur_ps_tdma);
 		}
 
 		if (coex_dm->cur_ps_tdma != 1 &&
-			coex_dm->cur_ps_tdma != 2 &&
-			coex_dm->cur_ps_tdma != 9 &&
-			coex_dm->cur_ps_tdma != 11) {
-			/*  recover to previous adjust type */
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, coex_dm->ps_tdma_du_adj_type);
+		    coex_dm->cur_ps_tdma != 2 &&
+		    coex_dm->cur_ps_tdma != 9 &&
+		    coex_dm->cur_ps_tdma != 11) {
+			/* recover to previous adjust type*/
+			halbtc8821a1ant_ps_tdma(btcoexist,
+						NORMAL_EXEC, true,
+						coex_dm->tdma_adj_type);
 		}
 	}
 }
 
-static void
-halbtc8821a1ant_ps_tdma_check_for_power_save_state(
-	struct btc_coexist *btcoexist,
-	bool new_ps_state
-	)
+static void btc8821a1ant_ps_tdma_check_for_pwr_save(struct btc_coexist *btcoex,
+						    bool new_ps_state)
 {
-	u8 lps_mode = 0x0;
+	u8	lps_mode = 0x0;
 
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_LPS_MODE, &lps_mode);
+	btcoex->btc_get(btcoex, BTC_GET_U1_LPS_MODE, &lps_mode);
 
 	if (lps_mode) {
-		/*  already under LPS state */
-		if (!new_ps_state) {
-			/*  will leave LPS state, turn off psTdma first */
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+		/* already under LPS state*/
+		if (new_ps_state) {
+			/* keep state under LPS, do nothing.*/
+		} else {
+			/* will leave LPS state, turn off psTdma first*/
+			halbtc8821a1ant_ps_tdma(btcoex, NORMAL_EXEC, false, 0);
 		}
 	} else {
-		/*  NO PS state */
+		/* NO PS state*/
 		if (new_ps_state) {
-			/*  will enter LPS state, turn off psTdma first */
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
+			/* will enter LPS state, turn off psTdma first*/
+			halbtc8821a1ant_ps_tdma(btcoex, NORMAL_EXEC, false, 0);
 		} else {
-			/*  keep state under NO PS state, do nothing. */
+			/* keep state under NO PS state, do nothing.*/
 		}
 	}
 }
 
-static void
-halbtc8821a1ant_power_save_state(
-	struct btc_coexist *btcoexist,
-	u8 ps_type,
-	u8 lps_val,
-	u8 rpwm_val
-	)
+static void halbtc8821a1ant_power_save_state(struct btc_coexist *btcoexist,
+					     u8 ps_type, u8 lps_val,
+					     u8 rpwm_val)
 {
 	bool low_pwr_disable = false;
 
 	switch (ps_type) {
 	case BTC_PS_WIFI_NATIVE:
-		/*  recover to original 32k low power setting */
+		/* recover to original 32k low power setting*/
 		low_pwr_disable = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &low_pwr_disable);
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
 		break;
 	case BTC_PS_LPS_ON:
-		halbtc8821a1ant_ps_tdma_check_for_power_save_state(btcoexist, true);
-		halbtc8821a1ant_lps_rpwm(btcoexist, NORMAL_EXEC, lps_val, rpwm_val);
-		/*  when coex force to enter LPS, do not enter 32k low power. */
+		btc8821a1ant_ps_tdma_check_for_pwr_save(btcoexist,
+							true);
+		halbtc8821a1ant_lps_rpwm(btcoexist,
+					 NORMAL_EXEC, lps_val, rpwm_val);
+		/* when coex force to enter LPS, do not enter 32k low power.*/
 		low_pwr_disable = true;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &low_pwr_disable);
-		/*  power save must executed before psTdma. */
+		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+				   &low_pwr_disable);
+		/* power save must executed before psTdma.*/
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
 		break;
 	case BTC_PS_LPS_OFF:
-		halbtc8821a1ant_ps_tdma_check_for_power_save_state(btcoexist, false);
+		btc8821a1ant_ps_tdma_check_for_pwr_save(btcoexist, false);
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
 		break;
 	default:
@@ -1408,13 +1557,10 @@ halbtc8821a1ant_power_save_state(
 	}
 }
 
-static void
-halbtc8821a1ant_coex_under_5g(
-	struct btc_coexist *btcoexist
-	)
+static void halbtc8821a1ant_coex_under_5g(struct btc_coexist *btcoexist)
 {
-	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
 	halbtc8821a1ant_ignore_wlan_act(btcoexist, NORMAL_EXEC, true);
 
 	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 10);
@@ -1426,30 +1572,24 @@ halbtc8821a1ant_coex_under_5g(
 	halbtc8821a1ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 5);
 }
 
-static void
-halbtc8821a1ant_action_wifi_only(
-	struct btc_coexist *btcoexist
-	)
+static void halbtc8821a1ant_action_wifi_only(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 9);
 }
 
-static void
-halbtc8821a1ant_monitor_bt_enable_disable(
-	struct btc_coexist *btcoexist
-	)
+static void btc8821a1ant_mon_bt_en_dis(struct btc_coexist *btcoexist)
 {
-	static bool pre_bt_disabled;
-	static u32 bt_disable_cnt;
-	bool bt_active = true, bt_disabled = false;
+	static bool	pre_bt_disabled;
+	static u32	bt_disable_cnt;
+	bool		bt_active = true, bt_disabled = false;
 
-	/*  This function check if bt is disabled */
+	/* This function check if bt is disabled*/
 
 	if (coex_sta->high_priority_tx == 0 &&
-		coex_sta->high_priority_rx == 0 &&
-		coex_sta->low_priority_tx == 0 &&
-		coex_sta->low_priority_rx == 0) {
+	    coex_sta->high_priority_rx == 0 &&
+	    coex_sta->low_priority_tx == 0 &&
+	    coex_sta->low_priority_rx == 0) {
 		bt_active = false;
 	}
 	if (coex_sta->high_priority_tx == 0xffff &&
@@ -1482,19 +1622,25 @@ halbtc8821a1ant_monitor_bt_enable_disable(
 	if (pre_bt_disabled != bt_disabled) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
 			  "[BTCoex], BT is from %s to %s!!\n",
-			  (pre_bt_disabled ? "disabled" : "enabled"),
-			  (bt_disabled ? "disabled" : "enabled"));
+			(pre_bt_disabled ? "disabled" : "enabled"),
+			(bt_disabled ? "disabled" : "enabled"));
 		pre_bt_disabled = bt_disabled;
 		if (bt_disabled) {
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_LEAVE_LPS, NULL);
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_NORMAL_LPS, NULL);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS,
+					   NULL);
+			btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS,
+					   NULL);
 		}
 	}
 }
 
-/*	Software Coex Mechanism start */
+/*=============================================*/
+/**/
+/*	Software Coex Mechanism start*/
+/**/
+/*=============================================*/
+
+/* SCO only or SCO+PAN(HS)*/
 static void halbtc8821a1ant_action_sco(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_sw_mechanism(btcoexist, true);
@@ -1505,7 +1651,7 @@ static void halbtc8821a1ant_action_hid(struct btc_coexist *btcoexist)
 	halbtc8821a1ant_sw_mechanism(btcoexist, true);
 }
 
-/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
+/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
 static void halbtc8821a1ant_action_a2dp(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_sw_mechanism(btcoexist, false);
@@ -1521,13 +1667,13 @@ static void halbtc8821a1ant_action_pan_edr(struct btc_coexist *btcoexist)
 	halbtc8821a1ant_sw_mechanism(btcoexist, false);
 }
 
-/* PAN(HS) only */
+/*PAN(HS) only*/
 static void halbtc8821a1ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_sw_mechanism(btcoexist, false);
 }
 
-/* PAN(EDR)+A2DP */
+/*PAN(EDR)+A2DP*/
 static void halbtc8821a1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_sw_mechanism(btcoexist, false);
@@ -1538,8 +1684,8 @@ static void halbtc8821a1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 	halbtc8821a1ant_sw_mechanism(btcoexist, true);
 }
 
-/*  HID+A2DP+PAN(EDR) */
-static void halbtc8821a1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+/* HID+A2DP+PAN(EDR)*/
+static void btc8821a1ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_sw_mechanism(btcoexist, true);
 }
@@ -1549,7 +1695,12 @@ static void halbtc8821a1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 	halbtc8821a1ant_sw_mechanism(btcoexist, true);
 }
 
-/*	Non-Software Coex Mechanism start */
+/*=============================================*/
+/**/
+/*	Non-Software Coex Mechanism start*/
+/**/
+/*=============================================*/
+
 static void halbtc8821a1ant_action_hs(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
@@ -1561,83 +1712,94 @@ static void halbtc8821a1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 	bool wifi_connected = false;
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+	btcoexist->btc_get(btcoexist,
+		 BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
 
 	if (!wifi_connected) {
-		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8821a1ant_power_save_state(btcoexist,
+						 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	} else if ((bt_link_info->sco_exist) ||
-			(bt_link_info->hid_only)) {
-		/*  SCO/HID-only busy */
-		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		   (bt_link_info->hid_only)) {
+		/* SCO/HID-only busy*/
+		halbtc8821a1ant_power_save_state(btcoexist,
+						 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	} else {
-		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_LPS_ON, 0x50, 0x4);
+		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_LPS_ON,
+						 0x50, 0x4);
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 30);
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	}
 }
 
-static void
-halbtc8821a1ant_action_bt_sco_hid_only_busy(
-	struct btc_coexist *btcoexist,
-	u8 wifi_status
-	)
-{
-	/*  tdma and coex table */
+static void btc8821a1ant_act_bt_sco_hid_only_busy(struct btc_coexist *btcoexist,
+						  u8 wifi_status) {
+	/* tdma and coex table*/
 	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
 
-	if (BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifi_status)
+	if (BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN ==
+	    wifi_status)
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	else
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 }
 
-static void action_wifi_connected_bt_acl_busy(struct btc_coexist *btcoexist, u8 wifi_status)
+static void btc8821a1ant_act_wifi_con_bt_acl_busy(struct btc_coexist *btcoexist,
+						  u8 wifi_status)
 {
-	u8 bt_rssi_state;
+	u8		bt_rssi_state;
 
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+
 	bt_rssi_state = halbtc8821a1ant_bt_rssi_state(2, 28, 0);
 
-	if (bt_link_info->hid_only)  {
-		/* HID */
-		halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist, wifi_status);
+	if (bt_link_info->hid_only) {
+		/*HID*/
+		btc8821a1ant_act_bt_sco_hid_only_busy(btcoexist,
+						      wifi_status);
 		coex_dm->auto_tdma_adjust = false;
 		return;
-	} else if (bt_link_info->a2dp_only) { /* A2DP */
+	} else if (bt_link_info->a2dp_only) {
+		/*A2DP*/
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			 halbtc8821a1ant_tdma_duration_adjust_for_acl(btcoexist, wifi_status);
-		} else { /* for low BT RSSI */
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8821a1ant_tdma_dur_adj(btcoexist, wifi_status);
+		} else {
+			/*for low BT RSSI*/
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
 			coex_dm->auto_tdma_adjust = false;
 		}
 
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	} else if (bt_link_info->hid_exist && bt_link_info->a2dp_exist) {
-		/* HID+A2DP */
+		/*HID+A2DP*/
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
 			coex_dm->auto_tdma_adjust = false;
-		} else /* for low BT RSSI */ {
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
+		} else {
+			/*for low BT RSSI*/
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
 			coex_dm->auto_tdma_adjust = false;
 		}
 
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	} else if ((bt_link_info->pan_only) || (bt_link_info->hid_exist && bt_link_info->pan_exist)) {
-		/* PAN(OPP, FTP), HID+PAN(OPP, FTP) */
+	} else if ((bt_link_info->pan_only) ||
+		(bt_link_info->hid_exist && bt_link_info->pan_exist)) {
+		/*PAN(OPP, FTP), HID+PAN(OPP, FTP)*/
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 		coex_dm->auto_tdma_adjust = false;
 	} else if (((bt_link_info->a2dp_exist) && (bt_link_info->pan_exist)) ||
 		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
 		    bt_link_info->pan_exist)) {
-		/* A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP) */
+		/*A2DP+PAN(OPP, FTP), HID+A2DP+PAN(OPP, FTP)*/
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 		coex_dm->auto_tdma_adjust = false;
@@ -1648,49 +1810,51 @@ static void action_wifi_connected_bt_acl_busy(struct btc_coexist *btcoexist, u8
 	}
 }
 
-static void
-halbtc8821a1ant_action_wifi_not_connected(
-	struct btc_coexist *btcoexist
-	)
+static void halbtc8821a1ant_action_wifi_not_connected(
+	struct btc_coexist *btcoexist)
 {
-	/*  power save state */
-	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	/* power save state*/
+	halbtc8821a1ant_power_save_state(btcoexist,
+					 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 
-	/*  tdma and coex table	 */
+	/* tdma and coex table*/
 	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
 	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
 }
 
-static void wifi_not_connected_asso_auth_scan(struct btc_coexist *btcoexist)
+static void btc8821a1ant_act_wifi_not_conn_scan(struct btc_coexist *btcoexist)
 {
-	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	halbtc8821a1ant_power_save_state(btcoexist,
+					 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 
 	halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
 	halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 }
 
-static void
-halbtc8821a1ant_action_wifi_connected_scan(
-	struct btc_coexist *btcoexist
-	)
-{
+static void halbtc8821a1ant_action_wifi_connected_scan(
+	struct btc_coexist *btcoexist) {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
 
-	/*  power save state */
-	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	/* power save state*/
+	halbtc8821a1ant_power_save_state(btcoexist,
+					 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 
-	/*  tdma and coex table */
+	/* tdma and coex table*/
 	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
 		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
-			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 22);
+			halbtc8821a1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
 		} else {
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
 		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
 	}
-	} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-			(BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
-		halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist,
+	} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY ==
+		    coex_dm->bt_status) ||
+		   (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+		    coex_dm->bt_status)) {
+		btc8821a1ant_act_bt_sco_hid_only_busy(btcoexist,
 			BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN);
 	} else {
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
@@ -1698,23 +1862,28 @@ halbtc8821a1ant_action_wifi_connected_scan(
 	}
 }
 
-static void action_wifi_connected_special_packet(struct btc_coexist *btcoexist)
+static void btc8821a1ant_act_wifi_conn_sp_pkt(struct btc_coexist *btcoexist)
 {
-	bool hs_connecting = false;
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
+	bool	hs_connecting = false;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
 
-	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
+					 0x0, 0x0);
 
-	/*  tdma and coex table */
+	/* tdma and coex table*/
 	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
 		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
-			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 22);
+			halbtc8821a1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
 		} else {
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 20);
+			halbtc8821a1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 1);
 		}
 	} else {
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
@@ -1724,16 +1893,19 @@ static void action_wifi_connected_special_packet(struct btc_coexist *btcoexist)
 
 static void halbtc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 {
-	bool wifi_busy = false;
-	bool scan = false, link = false, roam = false;
-	bool under_4way = false;
+	bool	wifi_busy = false;
+	bool	scan = false, link = false, roam = false;
+	bool	under_4way = false;
 
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], CoexForWifiConnect() ===>\n");
+	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+		  "[BTCoex], CoexForWifiConnect()===>\n");
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &under_4way);
+	btcoexist->btc_get(btcoexist,
+		 BTC_GET_BL_WIFI_4_WAY_PROGRESS, &under_4way);
 	if (under_4way) {
-		action_wifi_connected_special_packet(btcoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n");
+		btc8821a1ant_act_wifi_conn_sp_pkt(btcoexist);
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n");
 		return;
 	}
 
@@ -1742,39 +1914,48 @@ static void halbtc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
 	if (scan || link || roam) {
 		halbtc8821a1ant_action_wifi_connected_scan(btcoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n");
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n");
 		return;
 	}
 
-	/*  power save state */
-	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status && !btcoexist->bt_link_info.hid_only)
-		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_LPS_ON, 0x50, 0x4);
+	/* power save state*/
+	if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY ==
+			coex_dm->bt_status && !btcoexist->bt_link_info.hid_only)
+		halbtc8821a1ant_power_save_state(btcoexist,
+						 BTC_PS_LPS_ON, 0x50, 0x4);
 	else
-		halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
+		halbtc8821a1ant_power_save_state(btcoexist,
+						 BTC_PS_WIFI_NATIVE,
+						 0x0, 0x0);
 
-	/*  tdma and coex table */
+	/* tdma and coex table*/
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
 	if (!wifi_busy) {
 		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-			action_wifi_connected_bt_acl_busy(btcoexist,
+			btc8821a1ant_act_wifi_con_bt_acl_busy(btcoexist,
 				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE);
-		} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-			(BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
-			halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist,
+		} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY ==
+			    coex_dm->bt_status) ||
+			   (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY ==
+			    coex_dm->bt_status)) {
+			btc8821a1ant_act_bt_sco_hid_only_busy(btcoexist,
 				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE);
 		} else {
-			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-			halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
+			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+			halbtc8821a1ant_coex_table_with_type(btcoexist,
+							     NORMAL_EXEC, 2);
 		}
 	} else {
 		if (BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-			action_wifi_connected_bt_acl_busy(btcoexist,
+			btc8821a1ant_act_wifi_con_bt_acl_busy(btcoexist,
 				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY);
 		} else if ((BT_8821A_1ANT_BT_STATUS_SCO_BUSY ==
 			    coex_dm->bt_status) ||
 			   (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY ==
 			    coex_dm->bt_status)) {
-			halbtc8821a1ant_action_bt_sco_hid_only_busy(btcoexist,
+			btc8821a1ant_act_bt_sco_hid_only_busy(btcoexist,
 				BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY);
 		} else {
 			halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC,
@@ -1785,9 +1966,9 @@ static void halbtc8821a1ant_action_wifi_connected(struct btc_coexist *btcoexist)
 	}
 }
 
-static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
+static void btc8821a1ant_run_sw_coex_mech(struct btc_coexist *btcoexist)
 {
-	u8 algorithm = 0;
+	u8	algorithm = 0;
 
 	algorithm = halbtc8821a1ant_action_algorithm(btcoexist);
 	coex_dm->cur_algorithm = algorithm;
@@ -1837,7 +2018,7 @@ static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
 		case BT_8821A_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Action algorithm = HID+A2DP+PAN.\n");
-			halbtc8821a1ant_action_hid_a2dp_pan_edr(btcoexist);
+			btc8821a1ant_action_hid_a2dp_pan_edr(btcoexist);
 			break;
 		case BT_8821A_1ANT_COEX_ALGO_HID_A2DP:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -1847,6 +2028,7 @@ static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
 		default:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Action algorithm = coexist All Off!!\n");
+			/*halbtc8821a1ant_coex_all_off(btcoexist);*/
 			break;
 		}
 		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
@@ -1856,15 +2038,15 @@ static void run_sw_coexist_mechanism(struct btc_coexist *btcoexist)
 static void halbtc8821a1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 {
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool wifi_connected = false, bt_hs_on = false;
-	bool increase_scan_dev_num = false;
-	bool bt_ctrl_agg_buf_size = false;
-	u8 agg_buf_size = 5;
-	u8 wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-	bool wifi_under_5g = false;
+	bool	wifi_connected = false, bt_hs_on = false;
+	bool	increase_scan_dev_num = false;
+	bool	bt_ctrl_agg_buf_size = false;
+	u8	agg_buf_size = 5;
+	u8	wifi_rssi_state = BTC_RSSI_STATE_HIGH;
+	bool	wifi_under_5g = false;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], RunCoexistMechanism() ===>\n");
+		  "[BTCoex], RunCoexistMechanism()===>\n");
 
 	if (btcoexist->manual_control) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -1884,8 +2066,7 @@ static void halbtc8821a1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 		return;
 	}
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G,
-			   &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
 	if (wifi_under_5g) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], RunCoexistMechanism(), return for 5G <===\n");
@@ -1895,31 +2076,32 @@ static void halbtc8821a1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 
 	if ((BT_8821A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
 	    (BT_8821A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
+	    (BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status))
 		increase_scan_dev_num = true;
-	}
 
 	btcoexist->btc_set(btcoexist, BTC_SET_BL_INC_SCAN_DEV_NUM,
 			   &increase_scan_dev_num);
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
+	btcoexist->btc_get(btcoexist,
+		 BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
 
 	if (!bt_link_info->sco_exist && !bt_link_info->hid_exist) {
 		halbtc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
 	} else {
 		if (wifi_connected) {
-			wifi_rssi_state = Wifi_rssi_state(btcoexist, 1, 2,
-							  30, 0);
+			wifi_rssi_state =
+				 halbtc8821a1ant_WifiRssiState(btcoexist, 1, 2,
+							       30, 0);
 			if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 				halbtc8821a1ant_limited_tx(btcoexist,
 							   NORMAL_EXEC, 1, 1,
 							   1, 1);
-			else
+			} else {
 				halbtc8821a1ant_limited_tx(btcoexist,
 							   NORMAL_EXEC, 1, 1,
 							   1, 1);
+			}
 		} else {
 			halbtc8821a1ant_limited_tx(btcoexist, NORMAL_EXEC,
 						   0, 0, 0, 0);
@@ -1939,7 +2121,7 @@ static void halbtc8821a1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	halbtc8821a1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
 				   bt_ctrl_agg_buf_size, agg_buf_size);
 
-	run_sw_coexist_mechanism(btcoexist);
+	btc8821a1ant_run_sw_coex_mech(btcoexist);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 	if (coex_sta->c2h_bt_inquiry_page) {
@@ -1951,28 +2133,29 @@ static void halbtc8821a1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 
 	if (!wifi_connected) {
-		bool scan = false, link = false, roam = false;
+		bool	scan = false, link = false, roam = false;
 
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], wifi is non connected-idle !!!\n");
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+			  "[BTCoex], wifi is non connected-idle !!!\n");
 
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
 
 		if (scan || link || roam)
-			wifi_not_connected_asso_auth_scan(btcoexist);
+			btc8821a1ant_act_wifi_not_conn_scan(btcoexist);
 		else
 			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
 	} else {
-		/*  wifi LPS/Busy */
+		/* wifi LPS/Busy*/
 		halbtc8821a1ant_action_wifi_connected(btcoexist);
 	}
 }
 
 static void halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
-	/*  force to reset coex mechanism */
-	/*  sw all off */
+	/* force to reset coex mechanism*/
+	/* sw all off*/
 	halbtc8821a1ant_sw_mechanism(btcoexist, false);
 
 	halbtc8821a1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
@@ -1982,24 +2165,24 @@ static void halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist)
 static void halbtc8821a1ant_init_hw_config(struct btc_coexist *btcoexist,
 					   bool back_up)
 {
-	u8 u1_tmp = 0;
-	bool wifi_under_5g = false;
+	u8	u1_tmp = 0;
+	bool	wifi_under_5g = false;
 
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 		  "[BTCoex], 1Ant Init HW Config!!\n");
 
 	if (back_up) {
-		coex_dm->backup_arfr_cnt1 =
-			btcoexist->btc_read_4byte(btcoexist, 0x430);
-		coex_dm->backup_arfr_cnt2 =
-			btcoexist->btc_read_4byte(btcoexist, 0x434);
+		coex_dm->backup_arfr_cnt1 = btcoexist->btc_read_4byte(btcoexist,
+								      0x430);
+		coex_dm->backup_arfr_cnt2 = btcoexist->btc_read_4byte(btcoexist,
+								      0x434);
 		coex_dm->backup_retry_limit =
-			btcoexist->btc_read_2byte(btcoexist, 0x42a);
+			 btcoexist->btc_read_2byte(btcoexist, 0x42a);
 		coex_dm->backup_ampdu_max_time =
-			btcoexist->btc_read_1byte(btcoexist, 0x456);
+			 btcoexist->btc_read_1byte(btcoexist, 0x456);
 	}
 
-	/*  0x790[5:0] = 0x5 */
+	/* 0x790[5:0] = 0x5*/
 	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
 	u1_tmp &= 0xc0;
 	u1_tmp |= 0x5;
@@ -2007,43 +2190,38 @@ static void halbtc8821a1ant_init_hw_config(struct btc_coexist *btcoexist,
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
 
-	/* Antenna config */
+	/*Antenna config*/
 	if (wifi_under_5g)
 		halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
 					     true, false);
 	else
 		halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_PTA,
 					     true, false);
-	/*  PTA parameter */
+	/* PTA parameter*/
 	halbtc8821a1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
 
-	/*  Enable counter statistics */
-	/* 0x76e[3] = 1, WLAN_Act control by PTA */
+	/* Enable counter statistics*/
+	/*0x76e[3] =1, WLAN_Act control by PTA*/
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
 }
 
-/*  */
-/*  work around function start with wa_halbtc8821a1ant_ */
-/*  */
-/*  */
-/*  extern function start with EXhalbtc8821a1ant_ */
-/*  */
-void
-ex_halbtc8821a1ant_init_hwconfig(
-	struct btc_coexist *btcoexist
-	)
+/*============================================================*/
+/* work around function start with wa_halbtc8821a1ant_*/
+/*============================================================*/
+/*============================================================*/
+/* extern function start with EXhalbtc8821a1ant_*/
+/*============================================================*/
+void ex_halbtc8821a1ant_init_hwconfig(struct btc_coexist *btcoexist)
 {
 	halbtc8821a1ant_init_hw_config(btcoexist, true);
 }
 
-void
-ex_halbtc8821a1ant_init_coex_dm(
-	struct btc_coexist *btcoexist
-	)
+void ex_halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, "[BTCoex], Coex Mechanism Init!!\n");
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
+		  "[BTCoex], Coex Mechanism Init!!\n");
 
 	btcoexist->stop_coex_dm = false;
 
@@ -2057,7 +2235,7 @@ void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	u8 *cli_buf = btcoexist->cli_buf;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	u8 u1_tmp[4], i, bt_info_ext, ps_tdma_case = 0;
 	u16 u2_tmp[4];
 	u32 u4_tmp[4];
@@ -2068,125 +2246,127 @@ void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
 	u8 wifi_dot11_chnl, wifi_hs_chnl;
 	u32 fw_ver = 0, bt_patch_ver = 0;
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n ============[BT Coexist info] ============");
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n ============[BT Coexist info]============");
 
 	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n ============[Under Manual Control] ============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n ==========================================");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n ============[Under Manual Control]============");
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n ==========================================");
 	}
 	if (btcoexist->stop_coex_dm) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n ============[Coex is STOPPED] ============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n ==========================================");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n ============[Coex is STOPPED]============");
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n ==========================================");
 	}
 
 	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n BT not exists !!!");
 		return;
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d",
-		   "Ant PG Num/ Ant Mech/ Ant Pos: ",
-		   board_info->pg_ant_num, board_info->btdm_ant_num,
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d/ %d",
+		   "Ant PG Num/ Ant Mech/ Ant Pos:",
+		   board_info->pg_ant_num,
+		   board_info->btdm_ant_num,
 		   board_info->btdm_ant_pos);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %d",
-		   "BT stack/ hci ext ver",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %s / %d", "BT stack/ hci ext ver",
 		   ((stack_info->profile_notified) ? "Yes" : "No"),
-		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
+		stack_info->hci_version);
 
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
+			   &bt_patch_ver);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
 		   "CoexVer/ FwVer/ PatchVer",
-		   glcoex_ver_date_8821a_1ant, glcoex_ver_8821a_1ant, fw_ver,
-		   bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
+		   glcoex_ver_date_8821a_1ant,
+		   glcoex_ver_8821a_1ant,
+		   fw_ver, bt_patch_ver,
+		   bt_patch_ver);
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION,
+			   &bt_hs_on);
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
 			   &wifi_dot11_chnl);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d / %d(%d)",
+	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL,
+			   &wifi_hs_chnl);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d / %d(%d)",
 		   "Dot11 channel / HsChnl(HsMode)",
 		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %02x %02x %02x ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %02x %02x %02x ",
 		   "H2C Wifi inform bt chnl Info",
 		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
 		   coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
-		   "Wifi rssi/ HS rssi",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi",
 		   (int)wifi_rssi, (int)bt_hs_rssi);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d ",
-		   "Wifi link/ roam/ scan",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d/ %d ", "Wifi link/ roam/ scan",
 		   link, roam, scan);
-	CL_PRINTF(cli_buf);
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G,
+			   &wifi_under_5g);
+	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW,
+			   &wifi_bw);
+	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY,
+			   &wifi_busy);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
 			   &wifi_traffic_dir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %s/ %s ",
-		   "Wifi status",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %s / %s/ %s ", "Wifi status",
 		   (wifi_under_5g ? "5G" : "2.4G"),
 		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
-		    (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
+		   (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
 		   ((!wifi_busy) ? "idle" :
-		    ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
-		     "uplink" : "downlink")));
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]",
+		   ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+		   "uplink" : "downlink")));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]",
 		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
-		    ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
-		     ((BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
-		       coex_dm->bt_status) ? "non-connected idle" :
-		      ((BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE ==
-		       coex_dm->bt_status) ? "connected-idle" : "busy")))),
+		   ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
+		   ((BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
+		     coex_dm->bt_status) ?
+		   "non-connected idle" :
+		   ((BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE ==
+		     coex_dm->bt_status) ?
+		   "connected-idle" : "busy")))),
 		   coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d / %d / %d / %d",
-		   "SCO/HID/PAN/A2DP",
-		   bt_link_info->sco_exist, bt_link_info->hid_exist,
-		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP",
+		   bt_link_info->sco_exist,
+		   bt_link_info->hid_exist,
+		   bt_link_info->pan_exist,
+		   bt_link_info->a2dp_exist);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %s",
 		   "BT Info A2DP rate",
-		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
+		   (bt_info_ext&BIT0) ?
+		   "Basic rate" : "EDR rate");
 
 	for (i = 0; i < BT_INFO_SRC_8821A_1ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-				   "\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
 				   glbt_info_src_8821a_1ant[i],
 				   coex_sta->bt_info_c2h[i][0],
 				   coex_sta->bt_info_c2h[i][1],
@@ -2196,152 +2376,144 @@ void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist)
 				   coex_sta->bt_info_c2h[i][5],
 				   coex_sta->bt_info_c2h[i][6],
 				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
 		}
 	}
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s/%s, (0x%x/0x%x)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %s/%s, (0x%x/0x%x)",
 		   "PS state, IPS/LPS, (lps/rpwm)",
 		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
-		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")),
+		   ((coex_sta->under_Lps ? "LPS ON" : "LPS OFF")),
 		   btcoexist->bt_info.lps_val,
 		   btcoexist->bt_info.rpwm_val);
-	CL_PRINTF(cli_buf);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
 
 	if (!btcoexist->manual_control) {
-		/*  Sw mechanism	 */
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n %-35s", "============[Sw mechanism] ============");
-		CL_PRINTF(cli_buf);
+		/* Sw mechanism*/
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s", "============[Sw mechanism]============");
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n %-35s = %d", "SM[LowPenaltyRA]",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = %d", "SM[LowPenaltyRA]",
 			   coex_dm->cur_low_penalty_ra);
-		CL_PRINTF(cli_buf);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n %-35s = %s/ %s/ %d ", "DelBA/ BtCtrlAgg/ AggSize",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = %s/ %s/ %d ",
+			   "DelBA/ BtCtrlAgg/ AggSize",
 			   (btcoexist->bt_info.reject_agg_pkt ? "Yes" : "No"),
-			   (btcoexist->bt_info.b_bt_ctrl_buf_size ?
-			    "Yes" : "No"),
+			   (btcoexist->bt_info.bt_ctrl_buf_size ? "Yes" : "No"),
 			   btcoexist->bt_info.agg_buf_size);
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n %-35s = 0x%x ", "Rate Mask",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = 0x%x ", "Rate Mask",
 			   btcoexist->bt_info.ra_mask);
-		CL_PRINTF(cli_buf);
 
-		/*  Fw mechanism */
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
-			   "============[Fw mechanism] ============");
-		CL_PRINTF(cli_buf);
+		/* Fw mechanism*/
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+			   "============[Fw mechanism]============");
 
 		ps_tdma_case = coex_dm->cur_ps_tdma;
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
 			   "PS TDMA",
-			   coex_dm->ps_tdma_para[0], coex_dm->ps_tdma_para[1],
-			   coex_dm->ps_tdma_para[2], coex_dm->ps_tdma_para[3],
-			   coex_dm->ps_tdma_para[4], ps_tdma_case,
+			   coex_dm->ps_tdma_para[0],
+			   coex_dm->ps_tdma_para[1],
+			   coex_dm->ps_tdma_para[2],
+			   coex_dm->ps_tdma_para[3],
+			   coex_dm->ps_tdma_para[4],
+			   ps_tdma_case,
 			   coex_dm->auto_tdma_adjust);
-		CL_PRINTF(cli_buf);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x ",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = 0x%x ",
 			   "Latest error condition(should be 0)",
 			   coex_dm->error_condition);
-		CL_PRINTF(cli_buf);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d ",
-			   "IgnWlanAct",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = %d ", "IgnWlanAct",
 			   coex_dm->cur_ignore_wlan_act);
-		CL_PRINTF(cli_buf);
 	}
 
-	/*  Hw setting		 */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
-		   "============[Hw setting] ============");
-	CL_PRINTF(cli_buf);
+	/* Hw setting*/
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s", "============[Hw setting]============");
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
 		   "backup ARFR1/ARFR2/RL/AMaxTime",
-		   coex_dm->backup_arfr_cnt1, coex_dm->backup_arfr_cnt2,
-		   coex_dm->backup_retry_limit, coex_dm->backup_ampdu_max_time);
-	CL_PRINTF(cli_buf);
+		   coex_dm->backup_arfr_cnt1,
+		   coex_dm->backup_arfr_cnt2,
+		   coex_dm->backup_retry_limit,
+		   coex_dm->backup_ampdu_max_time);
 
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
 	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
 	u2_tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/0x%x/0x%x/0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
 		   "0x430/0x434/0x42a/0x456",
 		   u4_tmp[0], u4_tmp[1], u2_tmp[0], u1_tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc58);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n %-35s = 0x%x/ 0x%x", "0x778/ 0xc58[29:25]",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/ 0x%x", "0x778/ 0xc58[29:25]",
 		   u1_tmp[0], (u4_tmp[0]&0x3e000000) >> 25);
-	CL_PRINTF(cli_buf);
 
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x8db);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x", "0x8db[6:5]",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x", "0x8db[6:5]",
 		   ((u1_tmp[0]&0x60)>>5));
-	CL_PRINTF(cli_buf);
 
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x975);
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xcb4);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0xcb4[29:28]/0xcb4[7:0]/0x974[9:8]",
-		   (u4_tmp[0]&0x30000000)>>28, u4_tmp[0]&0xff, u1_tmp[0] & 0x3);
-	CL_PRINTF(cli_buf);
+		   (u4_tmp[0] & 0x30000000)>>28,
+		    u4_tmp[0] & 0xff,
+		    u1_tmp[0] & 0x3);
 
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
 	u1_tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x64);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x40/0x4c[24:23]/0x64[0]",
 		   u1_tmp[0], ((u4_tmp[0]&0x01800000)>>23), u1_tmp[1]&0x1);
-	CL_PRINTF(cli_buf);
 
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
-		   "0x550(bcn ctrl)/0x522",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522",
 		   u4_tmp[0], u1_tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x", "0xc50(dig)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x", "0xc50(dig)",
 		   u4_tmp[0]&0xff);
-	CL_PRINTF(cli_buf);
 
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xf48);
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5d);
 	u1_tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
-		   "OFDM-FA/ CCK-FA",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/ 0x%x", "OFDM-FA/ CCK-FA",
 		   u4_tmp[0], (u1_tmp[0]<<8) + u1_tmp[1]);
-	CL_PRINTF(cli_buf);
 
 	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
 	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
 	u4_tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
 	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
 		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
 		   u4_tmp[0], u4_tmp[1], u4_tmp[2], u1_tmp[0]);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
-		   "0x770(high-pri rx/tx)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d", "0x770(high-pri rx/tx)",
 		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
-		   "0x774(low-pri rx/tx)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d", "0x774(low-pri rx/tx)",
 		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
 #if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 1)
 	halbtc8821a1ant_monitor_bt_ctr(btcoexist);
 #endif
@@ -2357,11 +2529,12 @@ void ex_halbtc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], IPS ENTER notify\n");
 		coex_sta->under_ips = true;
-		halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT,
-					     false, true);
-		/* set PTA control */
+		halbtc8821a1ant_set_ant_path(btcoexist,
+					     BTC_ANT_PATH_BT, false, true);
+		/*set PTA control*/
 		halbtc8821a1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-		halbtc8821a1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
+		halbtc8821a1ant_coex_table_with_type(btcoexist,
+						     NORMAL_EXEC, 0);
 	} else if (BTC_IPS_LEAVE == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], IPS LEAVE notify\n");
@@ -2379,11 +2552,11 @@ void ex_halbtc8821a1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
 	if (BTC_LPS_ENABLE == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], LPS ENABLE notify\n");
-		coex_sta->under_lps = true;
+		coex_sta->under_Lps = true;
 	} else if (BTC_LPS_DISABLE == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], LPS DISABLE notify\n");
-		coex_sta->under_lps = false;
+		coex_sta->under_Lps = false;
 	}
 }
 
@@ -2396,9 +2569,10 @@ void ex_halbtc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 	    btcoexist->bt_info.bt_disabled)
 		return;
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
+	btcoexist->btc_get(btcoexist,
+		 BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist,
+		 BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
 
 	halbtc8821a1ant_query_bt_info(btcoexist);
 
@@ -2414,25 +2588,27 @@ void ex_halbtc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN START notify\n");
 		if (!wifi_connected) {
-			/*  non-connected scan */
-			wifi_not_connected_asso_auth_scan(btcoexist);
+			/* non-connected scan*/
+			btc8821a1ant_act_wifi_not_conn_scan(btcoexist);
 		} else {
-			/*  wifi is connected */
+			/* wifi is connected*/
 			halbtc8821a1ant_action_wifi_connected_scan(btcoexist);
 		}
 	} else if (BTC_SCAN_FINISH == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN FINISH notify\n");
-		if (!wifi_connected)	/*  non-connected scan */
+		if (!wifi_connected) {
+			/* non-connected scan*/
 			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
-		else
+		} else {
 			halbtc8821a1ant_action_wifi_connected(btcoexist);
+		}
 	}
 }
 
 void ex_halbtc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 {
-	bool wifi_connected = false, bt_hs_on = false;
+	bool	wifi_connected = false, bt_hs_on = false;
 
 	if (btcoexist->manual_control ||
 	    btcoexist->stop_coex_dm ||
@@ -2451,17 +2627,19 @@ void ex_halbtc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 	if (BTC_ASSOCIATE_START == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT START notify\n");
-		wifi_not_connected_asso_auth_scan(btcoexist);
+		btc8821a1ant_act_wifi_not_conn_scan(btcoexist);
 	} else if (BTC_ASSOCIATE_FINISH == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT FINISH notify\n");
 
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-				   &wifi_connected);
-		if (!wifi_connected) /*  non-connected scan */
+		btcoexist->btc_get(btcoexist,
+			 BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+		if (!wifi_connected) {
+			/* non-connected scan*/
 			halbtc8821a1ant_action_wifi_not_connected(btcoexist);
-		else
+		} else {
 			halbtc8821a1ant_action_wifi_connected(btcoexist);
+		}
 	}
 }
 
@@ -2477,19 +2655,21 @@ void ex_halbtc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
 	    btcoexist->bt_info.bt_disabled)
 		return;
 
-	if (BTC_MEDIA_CONNECT == type)
+	if (BTC_MEDIA_CONNECT == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], MEDIA connect notify\n");
-	else
+	} else {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], MEDIA disconnect notify\n");
+	}
 
-	/*  only 2.4G we need to inform bt the chnl mask */
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
+	/* only 2.4G we need to inform bt the chnl mask*/
+	btcoexist->btc_get(btcoexist,
+			   BTC_GET_U1_WIFI_CENTRAL_CHNL,
 			   &wifi_central_chnl);
 	if ((BTC_MEDIA_CONNECT == type) &&
 	    (wifi_central_chnl <= 14)) {
-		/* h2c_parameter[0] = 0x1; */
+		/*h2c_parameter[0] = 0x1;*/
 		h2c_parameter[0] = 0x0;
 		h2c_parameter[1] = wifi_central_chnl;
 		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
@@ -2535,7 +2715,7 @@ void ex_halbtc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
 	    BTC_PACKET_EAPOL == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], special Packet(%d) notify\n", type);
-		action_wifi_connected_special_packet(btcoexist);
+		btc8821a1ant_act_wifi_conn_sp_pkt(btcoexist);
 	}
 }
 
@@ -2550,7 +2730,8 @@ void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 
 	coex_sta->c2h_bt_info_req_sent = false;
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist,
+		 BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
 
 	rsp_source = tmp_buf[0]&0xf;
 	if (rsp_source >= BT_INFO_SRC_8821A_1ANT_MAX)
@@ -2558,22 +2739,23 @@ void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 	coex_sta->bt_info_c2h_cnt[rsp_source]++;
 
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  "[BTCoex], Bt info[%d], length = %d, hex data = [",
 		  rsp_source, length);
 	for (i = 0; i < length; i++) {
 		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
 		if (i == 1)
 			bt_info = tmp_buf[i];
-		if (i == length-1)
+		if (i == length-1) {
 			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 				  "0x%02x]\n", tmp_buf[i]);
-		else
+		} else {
 			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 				  "0x%02x, ", tmp_buf[i]);
+		}
 	}
 
 	if (BT_INFO_SRC_8821A_1ANT_WIFI_FW != rsp_source) {
-		coex_sta->bt_retry_cnt =	/*  [3:0] */
+		coex_sta->bt_retry_cnt =	/* [3:0]*/
 			coex_sta->bt_info_c2h[rsp_source][2]&0xf;
 
 		coex_sta->bt_rssi =
@@ -2582,36 +2764,35 @@ void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		coex_sta->bt_info_ext =
 			coex_sta->bt_info_c2h[rsp_source][4];
 
-		/*  Here we need to resend some wifi info to BT */
-		/*  because bt is reset and loss of the info. */
-		if (coex_sta->bt_info_ext & BIT(1)) {
+		/* Here we need to resend some wifi info to BT*/
+		/* because bt is reset and loss of the info.*/
+		if (coex_sta->bt_info_ext & BIT1) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n");
-			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
+			btcoexist->btc_get(btcoexist,
+					   BTC_GET_BL_WIFI_CONNECTED,
 					   &wifi_connected);
-			if (wifi_connected)
+			if (wifi_connected) {
 				ex_halbtc8821a1ant_media_status_notify(btcoexist,
-								       BTC_MEDIA_CONNECT);
-			else
+							       BTC_MEDIA_CONNECT);
+			} else {
 				ex_halbtc8821a1ant_media_status_notify(btcoexist,
-								       BTC_MEDIA_DISCONNECT);
+							       BTC_MEDIA_DISCONNECT);
+			}
 		}
 
-		if ((coex_sta->bt_info_ext & BIT(3)) && !wifi_under_5g) {
+		if ((coex_sta->bt_info_ext & BIT3) && !wifi_under_5g) {
 			if (!btcoexist->manual_control &&
 			    !btcoexist->stop_coex_dm) {
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 					  "[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n");
 				halbtc8821a1ant_ignore_wlan_act(btcoexist,
-								FORCE_EXEC, false);
+								FORCE_EXEC,
+								false);
 			}
-		} else {
-			/*  BT already NOT ignore Wlan active, do nothing here. */
 		}
 #if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 0)
-		if ((coex_sta->bt_info_ext & BIT(4))) {
-			/*  BT auto report already enabled, do nothing */
-		} else {
+		if (!(coex_sta->bt_info_ext & BIT4)) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], BT ext info bit4 check, set BT to enable Auto Report!!\n");
 			halbtc8821a1ant_bt_auto_report(btcoexist,
@@ -2620,13 +2801,13 @@ void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 #endif
 	}
 
-	/*  check BIT(2) first ==> check if bt is under inquiry or page scan */
+	/* check BIT2 first ==> check if bt is under inquiry or page scan*/
 	if (bt_info & BT_INFO_8821A_1ANT_B_INQ_PAGE)
 		coex_sta->c2h_bt_inquiry_page = true;
 	else
 		coex_sta->c2h_bt_inquiry_page = false;
 
-	/*  set link exist status */
+	/* set link exist status*/
 	if (!(bt_info&BT_INFO_8821A_1ANT_B_CONNECTION)) {
 		coex_sta->bt_link_exist = false;
 		coex_sta->pan_exist = false;
@@ -2634,7 +2815,7 @@ void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		coex_sta->hid_exist = false;
 		coex_sta->sco_exist = false;
 	} else {
-		/*  connection exists */
+		/* connection exists*/
 		coex_sta->bt_link_exist = true;
 		if (bt_info & BT_INFO_8821A_1ANT_B_FTP)
 			coex_sta->pan_exist = true;
@@ -2661,12 +2842,12 @@ void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n");
 	} else if (bt_info == BT_INFO_8821A_1ANT_B_CONNECTION) {
-		/*  connection exists but not busy */
+		/* connection exists but no busy*/
 		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE;
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
 	} else if ((bt_info&BT_INFO_8821A_1ANT_B_SCO_ESCO) ||
-		   (bt_info&BT_INFO_8821A_1ANT_B_SCO_BUSY)) {
+		(bt_info&BT_INFO_8821A_1ANT_B_SCO_BUSY)) {
 		coex_dm->bt_status = BT_8821A_1ANT_BT_STATUS_SCO_BUSY;
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
@@ -2688,30 +2869,35 @@ void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
 		bt_busy = true;
 	else
 		bt_busy = false;
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
+	btcoexist->btc_set(btcoexist,
+			   BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
 
 	halbtc8821a1ant_run_coexist_mechanism(btcoexist);
 }
 
 void ex_halbtc8821a1ant_halt_notify(struct btc_coexist *btcoexist)
 {
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Halt notify\n");
 
 	btcoexist->stop_coex_dm = true;
 
-	halbtc8821a1ant_set_ant_path(btcoexist, BTC_ANT_PATH_BT, false, true);
+	halbtc8821a1ant_set_ant_path(btcoexist,
+				     BTC_ANT_PATH_BT, false, true);
 	halbtc8821a1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
 
-	halbtc8821a1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-					 0x0, 0x0);
+	halbtc8821a1ant_power_save_state(btcoexist,
+					 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
 	halbtc8821a1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
 
-	ex_halbtc8821a1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
+	ex_halbtc8821a1ant_media_status_notify(btcoexist,
+					       BTC_MEDIA_DISCONNECT);
 }
 
 void ex_halbtc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
 {
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Pnp notify\n");
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Pnp notify\n");
 
 	if (BTC_WIFI_PNP_SLEEP == pnp_state) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
@@ -2731,15 +2917,16 @@ void ex_halbtc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
 	}
 }
 
-void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist)
-{
-	static u8 dis_ver_info_cnt;
-	u32 fw_ver = 0, bt_patch_ver = 0;
+void
+ex_halbtc8821a1ant_periodical(
+	struct btc_coexist *btcoexist) {
+	static u8	dis_ver_info_cnt;
+	u32		fw_ver = 0, bt_patch_ver = 0;
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], ========================== Periodical ===========================\n");
+		  "[BTCoex], ==========================Periodical===========================\n");
 
 	if (dis_ver_info_cnt <= 5) {
 		dis_ver_info_cnt += 1;
@@ -2747,7 +2934,8 @@ void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist)
 			  "[BTCoex], ****************************************************************\n");
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
-			  board_info->pg_ant_num, board_info->btdm_ant_num,
+			  board_info->pg_ant_num,
+			  board_info->btdm_ant_num,
 			  board_info->btdm_ant_pos);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
@@ -2758,8 +2946,10 @@ void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist)
 		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
-			  glcoex_ver_date_8821a_1ant, glcoex_ver_8821a_1ant,
-			  fw_ver, bt_patch_ver, bt_patch_ver);
+			glcoex_ver_date_8821a_1ant,
+			glcoex_ver_8821a_1ant,
+			fw_ver, bt_patch_ver,
+			bt_patch_ver);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], ****************************************************************\n");
 	}
@@ -2767,7 +2957,7 @@ void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist)
 #if (BT_AUTO_REPORT_ONLY_8821A_1ANT == 0)
 	halbtc8821a1ant_query_bt_info(btcoexist);
 	halbtc8821a1ant_monitor_bt_ctr(btcoexist);
-	halbtc8821a1ant_monitor_bt_enable_disable(btcoexist);
+	btc8821a1ant_mon_bt_en_dis(btcoexist);
 #else
 	if (halbtc8821a1ant_Is_wifi_status_changed(btcoexist) ||
 	    coex_dm->auto_tdma_adjust) {
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a1ant.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a1ant.h
new file mode 100644
index 000000000000..20e904890fc2
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a1ant.h
@@ -0,0 +1,188 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+/*===========================================
+ * The following is for 8821A 1ANT BT Co-exist definition
+ *===========================================
+ */
+#define	BT_AUTO_REPORT_ONLY_8821A_1ANT				0
+
+#define	BT_INFO_8821A_1ANT_B_FTP	BIT7
+#define	BT_INFO_8821A_1ANT_B_A2DP	BIT6
+#define	BT_INFO_8821A_1ANT_B_HID	BIT5
+#define	BT_INFO_8821A_1ANT_B_SCO_BUSY	BIT4
+#define	BT_INFO_8821A_1ANT_B_ACL_BUSY	BIT3
+#define	BT_INFO_8821A_1ANT_B_INQ_PAGE	BIT2
+#define	BT_INFO_8821A_1ANT_B_SCO_ESCO	BIT1
+#define	BT_INFO_8821A_1ANT_B_CONNECTION	BIT0
+
+#define	BT_INFO_8821A_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
+		(((_BT_INFO_EXT_&BIT0)) ? true : false)
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT		2
+
+enum _BT_INFO_SRC_8821A_1ANT {
+	BT_INFO_SRC_8821A_1ANT_WIFI_FW			= 0x0,
+	BT_INFO_SRC_8821A_1ANT_BT_RSP			= 0x1,
+	BT_INFO_SRC_8821A_1ANT_BT_ACTIVE_SEND		= 0x2,
+	BT_INFO_SRC_8821A_1ANT_MAX
+};
+
+enum _BT_8821A_1ANT_BT_STATUS {
+	BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE		= 0x0,
+	BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE			= 0x1,
+	BT_8821A_1ANT_BT_STATUS_INQ_PAGE			= 0x2,
+	BT_8821A_1ANT_BT_STATUS_ACL_BUSY			= 0x3,
+	BT_8821A_1ANT_BT_STATUS_SCO_BUSY			= 0x4,
+	BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY			= 0x5,
+	BT_8821A_1ANT_BT_STATUS_MAX
+};
+
+enum _BT_8821A_1ANT_WIFI_STATUS {
+	BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE		= 0x0,
+	BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN	= 0x1,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN		= 0x2,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT		= 0x3,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE		= 0x4,
+	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY		= 0x5,
+	BT_8821A_1ANT_WIFI_STATUS_MAX
+};
+
+enum BT_8821A_1ANT_COEX_ALGO {
+	BT_8821A_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8821A_1ANT_COEX_ALGO_SCO			= 0x1,
+	BT_8821A_1ANT_COEX_ALGO_HID			= 0x2,
+	BT_8821A_1ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8821A_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8821A_1ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8821A_1ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8821A_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8821A_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8821A_1ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
+	BT_8821A_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8821A_1ANT_COEX_ALGO_MAX			= 0xb,
+};
+
+struct coex_dm_8821a_1ant {
+	/* fw mechanism */
+	bool	cur_ignore_wlan_act;
+	bool	pre_ignore_wlan_act;
+	u8	pre_ps_tdma;
+	u8	cur_ps_tdma;
+	u8	ps_tdma_para[5];
+	u8	tdma_adj_type;
+	bool	auto_tdma_adjust;
+	bool	pre_ps_tdma_on;
+	bool	cur_ps_tdma_on;
+	bool	pre_bt_auto_report;
+	bool	cur_bt_auto_report;
+	u8	pre_lps;
+	u8	cur_lps;
+	u8	pre_rpwm;
+	u8	cur_rpwm;
+
+	/* sw mechanism */
+	bool	pre_low_penalty_ra;
+	bool	cur_low_penalty_ra;
+	u32	pre_val_0x6c0;
+	u32	cur_val_0x6c0;
+	u32	pre_val_0x6c4;
+	u32	cur_val_0x6c4;
+	u32	pre_val_0x6c8;
+	u32	cur_val_0x6c8;
+	u8	pre_val_0x6cc;
+	u8	cur_val_0x6cc;
+	/* Auto Rate Fallback Retry cnt */
+	u32	backup_arfr_cnt1;
+	/* Auto Rate Fallback Retry cnt */
+	u32	backup_arfr_cnt2;
+	u16	backup_retry_limit;
+	u8	backup_ampdu_max_time;
+
+	/* algorithm related */
+	u8	pre_algorithm;
+	u8	cur_algorithm;
+	u8	bt_status;
+	u8	wifi_chnl_info[3];
+
+	u32	pre_ra_mask;
+	u32	cur_ra_mask;
+	u8	pre_arfr_type;
+	u8	cur_arfr_type;
+	u8	pre_retry_limit_type;
+	u8	cur_retry_limit_type;
+	u8	pre_ampdu_time_type;
+	u8	cur_ampdu_time_type;
+
+	u8	error_condition;
+};
+
+struct coex_sta_8821a_1ant {
+	bool	bt_link_exist;
+	bool	sco_exist;
+	bool	a2dp_exist;
+	bool	hid_exist;
+	bool	pan_exist;
+
+	bool	under_Lps;
+	bool	under_ips;
+	u32	special_pkt_period_cnt;
+	u32	high_priority_tx;
+	u32	high_priority_rx;
+	u32	low_priority_tx;
+	u32	low_priority_rx;
+	u8	bt_rssi;
+	u8	pre_bt_rssi_state;
+	u8	pre_wifi_rssi_state[4];
+	bool	c2h_bt_info_req_sent;
+	u8	bt_info_c2h[BT_INFO_SRC_8821A_1ANT_MAX][10];
+	u32	bt_info_c2h_cnt[BT_INFO_SRC_8821A_1ANT_MAX];
+	bool	c2h_bt_inquiry_page;
+	u8	bt_retry_cnt;
+	u8	bt_info_ext;
+};
+
+/*===========================================
+ * The following is interface which will notify coex module.
+ *===========================================
+ */
+void ex_halbtc8821a1ant_init_hwconfig(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
+void ex_halbtc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
+					    u8 type);
+void ex_halbtc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
+					      u8 type);
+void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
+				       u8 *tmpbuf, u8 length);
+void ex_halbtc8821a1ant_halt_notify(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
+void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist);
+void ex_halbtc8821a1ant_dbg_control(struct btc_coexist *btcoexist, u8 op_code,
+				    u8 op_len, u8 *data);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a2ant.c
index 244d5599e0dc..cf819f02ed23 100644
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a2ant.c
@@ -1,12 +1,49 @@
-/*  Description: */
-/*  This file is for RTL8821A Co-exist mechanism */
-/*  History */
-/*  2012/08/22 Cosa first check in. */
-/*  2012/11/14 Cosa Revise for 8821A 2Ant out sourcing. */
-
-/*  include files */
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+/*============================================================
+ * Description:
+ *
+ * This file is for RTL8821A Co-exist mechanism
+ *
+ * History
+ * 2012/08/22 Cosa first check in.
+ * 2012/11/14 Cosa Revise for 8821A 2Ant out sourcing.
+ *
+ *============================================================
+ */
+
+/*============================================================
+ * include files
+ *============================================================
+*/
 #include "halbt_precomp.h"
-/*  Global variables, these are static variables */
+/*============================================================
+ * Global variables, these are static variables
+ *============================================================
+ */
 static struct coex_dm_8821a_2ant	glcoex_dm_8821a_2ant;
 static struct coex_dm_8821a_2ant	*coex_dm = &glcoex_dm_8821a_2ant;
 static struct coex_sta_8821a_2ant	glcoex_sta_8821a_2ant;
@@ -18,24 +55,30 @@ static const char *const glbt_info_src_8821a_2ant[] = {
 	"BT Info[bt auto report]",
 };
 
-static u32 glcoex_ver_date_8821a_2ant = 20130618;
-static u32 glcoex_ver_8821a_2ant = 0x5050;
+static u32	glcoex_ver_date_8821a_2ant = 20130618;
+static u32	glcoex_ver_8821a_2ant = 0x5050;
 
-/*  local function proto type if needed */
-/*  local function start with halbtc8821a2ant_ */
+/*============================================================
+ * local function proto type if needed
+ *============================================================
+ *============================================================
+ * local function start with halbtc8821a2ant_
+ *============================================================
+ */
 static u8 halbtc8821a2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 					u8 rssi_thresh1)
 {
-	long bt_rssi = 0;
-	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
+	long	bt_rssi = 0;
+	u8	bt_rssi_state = coex_sta->pre_bt_rssi_state;
 
 	bt_rssi = coex_sta->bt_rssi;
 
 	if (level_num == 2) {
 		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= (rssi_thresh +
-			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+			long tmp = rssi_thresh +
+				   BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT;
+			if (bt_rssi >= tmp) {
 				bt_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to High\n");
@@ -64,8 +107,8 @@ static u8 halbtc8821a2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 
 		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= (rssi_thresh +
-			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+			if (bt_rssi >=
+			    (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
 				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Medium\n");
@@ -75,36 +118,32 @@ static u8 halbtc8821a2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 					  "[BTCoex], BT Rssi state stay at Low\n");
 			}
 		} else if ((coex_sta->pre_bt_rssi_state ==
-			    BTC_RSSI_STATE_MEDIUM) ||
+			   BTC_RSSI_STATE_MEDIUM) ||
 			   (coex_sta->pre_bt_rssi_state ==
 			    BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (bt_rssi >= (rssi_thresh1 +
-			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+			if (bt_rssi >=
+			    (rssi_thresh1 +
+			     BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
 				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_BT_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to High\n");
 			} else if (bt_rssi < rssi_thresh) {
 				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_BT_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Low\n");
 			} else {
 				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_BT_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at Medium\n");
 			}
 		} else {
 			if (bt_rssi < rssi_thresh1) {
 				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_BT_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state switch to Medium\n");
 			} else {
 				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_BT_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
 					  "[BTCoex], BT Rssi state stay at High\n");
 			}
 		}
@@ -115,12 +154,12 @@ static u8 halbtc8821a2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
 	return bt_rssi_state;
 }
 
-static u8 wifi21a_rssi_state(struct btc_coexist *btcoexist,
-			     u8 index, u8 level_num,
-			     u8 rssi_thresh, u8 rssi_thresh1)
+static u8 halbtc8821a2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
+					  u8 index, u8 level_num,
+					  u8 rssi_thresh, u8 rssi_thresh1)
 {
 	long	wifi_rssi = 0;
-	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
+	u8	wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 
@@ -129,8 +168,8 @@ static u8 wifi21a_rssi_state(struct btc_coexist *btcoexist,
 		     BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_wifi_rssi_state[index] ==
 		     BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= (rssi_thresh +
-			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+			if (wifi_rssi >=
+			    (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
 				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
 				BTC_PRINT(BTC_MSG_ALGORITHM,
 					  ALGO_WIFI_RSSI_STATE,
@@ -165,8 +204,8 @@ static u8 wifi21a_rssi_state(struct btc_coexist *btcoexist,
 		    BTC_RSSI_STATE_LOW) ||
 		    (coex_sta->pre_wifi_rssi_state[index] ==
 		     BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= (rssi_thresh +
-			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+			if (wifi_rssi >=
+			    (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
 				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
 				BTC_PRINT(BTC_MSG_ALGORITHM,
 					  ALGO_WIFI_RSSI_STATE,
@@ -178,47 +217,52 @@ static u8 wifi21a_rssi_state(struct btc_coexist *btcoexist,
 					  "[BTCoex], wifi RSSI state stay at Low\n");
 			}
 		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
-			    BTC_RSSI_STATE_MEDIUM) ||
+			   BTC_RSSI_STATE_MEDIUM) ||
 			   (coex_sta->pre_wifi_rssi_state[index] ==
 			    BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (wifi_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
+			if (wifi_rssi >= (rssi_thresh1 +
+			    BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT)) {
 				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
 					  "[BTCoex], wifi RSSI state switch to High\n");
 			} else if (wifi_rssi < rssi_thresh) {
 				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
 					  "[BTCoex], wifi RSSI state switch to Low\n");
 			} else {
 				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
 					  "[BTCoex], wifi RSSI state stay at Medium\n");
 			}
 		} else {
 			if (wifi_rssi < rssi_thresh1) {
 				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
 					  "[BTCoex], wifi RSSI state switch to Medium\n");
 			} else {
 				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
+				BTC_PRINT(BTC_MSG_ALGORITHM,
+					  ALGO_WIFI_RSSI_STATE,
 					  "[BTCoex], wifi RSSI state stay at High\n");
 			}
 		}
 	}
-
 	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
 
 	return wifi_rssi_state;
 }
 
-static void monitor_bt_enable_disable(struct btc_coexist *btcoexist)
+static void btc8821a2ant_mon_bt_en_dis(struct btc_coexist *btcoexist)
 {
-	static bool pre_bt_disabled;
-	static u32 bt_disable_cnt;
-	bool bt_active = true, bt_disabled = false;
+	static bool	pre_bt_disabled;
+	static u32	bt_disable_cnt;
+	bool		bt_active = true, bt_disabled = false;
 
-	/*  This function check if bt is disabled */
+	/* This function check if bt is disabled*/
 
 	if (coex_sta->high_priority_tx == 0 &&
 	    coex_sta->high_priority_rx == 0 &&
@@ -261,8 +305,8 @@ static void monitor_bt_enable_disable(struct btc_coexist *btcoexist)
 
 static void halbtc8821a2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 {
-	u32 reg_hp_txrx, reg_lp_txrx, u4tmp;
-	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
+	u32	reg_hp_txrx, reg_lp_txrx, u4tmp;
+	u32	reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
 
 	reg_hp_txrx = 0x770;
 	reg_lp_txrx = 0x774;
@@ -287,17 +331,17 @@ static void halbtc8821a2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
 		  "[BTCoex], Low Priority Tx/Rx (reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
 		  reg_lp_txrx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
 
-	/*  reset counter */
+	/* reset counter */
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 }
 
 static void halbtc8821a2ant_query_bt_info(struct btc_coexist *btcoexist)
 {
-	u8 h2c_parameter[1] = {0};
+	u8	h2c_parameter[1] = {0};
 
 	coex_sta->c2h_bt_info_req_sent = true;
 
-	h2c_parameter[0] |= BIT(0);	/*  trigger */
+	h2c_parameter[0] |= BIT0;	/* trigger */
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
@@ -315,12 +359,11 @@ static u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
 
-	/* for win-8 stack HID report error */
-	if (!stack_info->hid_exist) {
-		/* sync  BTInfo with BT firmware and stack */
+	/*for win-8 stack HID report error*/
+	/* sync  BTInfo with BT firmware and stack */
+	if (!stack_info->hid_exist)
 		stack_info->hid_exist = coex_sta->hid_exist;
-	}
-	/*  when stack HID report error, here we use the info from bt fw. */
+	/* when stack HID report error, here we use the info from bt fw. */
 	if (!stack_info->bt_link_exist)
 		stack_info->bt_link_exist = coex_sta->bt_link_exist;
 
@@ -368,7 +411,8 @@ static u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
 	} else if (num_of_diff_profile == 2) {
 		if (coex_sta->sco_exist) {
 			if (coex_sta->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], SCO + HID\n");
+				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
+					  "[BTCoex], SCO + HID\n");
 				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
 			} else if (coex_sta->a2dp_exist) {
 				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -392,7 +436,7 @@ static u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
 					  "[BTCoex], HID + A2DP\n");
 				algorithm = BT_8821A_2ANT_COEX_ALGO_HID_A2DP;
 			} else if (coex_sta->hid_exist &&
-				   coex_sta->pan_exist) {
+				coex_sta->pan_exist) {
 				if (bt_hs_on) {
 					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 						  "[BTCoex], HID + PAN(HS)\n");
@@ -403,7 +447,7 @@ static u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
 					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
 				}
 			} else if (coex_sta->pan_exist &&
-				   coex_sta->a2dp_exist) {
+				coex_sta->a2dp_exist) {
 				if (bt_hs_on) {
 					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 						  "[BTCoex], A2DP + PAN(HS)\n");
@@ -423,7 +467,7 @@ static u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
 					  "[BTCoex], SCO + HID + A2DP ==> HID\n");
 				algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
 			} else if (coex_sta->hid_exist &&
-				   coex_sta->pan_exist) {
+				coex_sta->pan_exist) {
 				if (bt_hs_on) {
 					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 						  "[BTCoex], SCO + HID + PAN(HS)\n");
@@ -471,7 +515,7 @@ static u8 halbtc8821a2ant_action_algorithm(struct btc_coexist *btcoexist)
 
 				} else {
 					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
+						  "[BTCoex], SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n");
 					algorithm = BT_8821A_2ANT_COEX_ALGO_PANEDR_HID;
 				}
 			}
@@ -516,13 +560,14 @@ static bool halbtc8821a2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
 	return ret;
 }
 
-static void set_fw_dac_swing_level(struct btc_coexist *btcoexist,
-				   u8 dac_swing_lvl)
+static void btc8821a2ant_set_fw_dac_swing_lev(struct btc_coexist *btcoexist,
+					      u8 dac_swing_lvl)
 {
-	u8 h2c_parameter[1] = {0};
+	u8	h2c_parameter[1] = {0};
 
-	/*  There are several type of dacswing */
-	/*  0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
+	/* There are several type of dacswing
+	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
+	 */
 	h2c_parameter[0] = dac_swing_lvl;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
@@ -536,12 +581,12 @@ static void set_fw_dac_swing_level(struct btc_coexist *btcoexist,
 static void halbtc8821a2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
 					      bool dec_bt_pwr)
 {
-	u8 h2c_parameter[1] = {0};
+	u8			h2c_parameter[1] = {0};
 
 	h2c_parameter[0] = 0;
 
 	if (dec_bt_pwr)
-		h2c_parameter[0] |= BIT(1);
+		h2c_parameter[0] |= BIT1;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], decrease Bt Power : %s, FW write 0x62 = 0x%x\n",
@@ -561,7 +606,7 @@ static void halbtc8821a2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], pre_dec_bt_pwr =%d, cur_dec_bt_pwr =%d\n",
+			  "[BTCoex], pre_dec_bt_pwr = %d, cur_dec_bt_pwr = %d\n",
 			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
 
 		if (coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr)
@@ -572,15 +617,15 @@ static void halbtc8821a2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
 	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
 }
 
-static void set_fw_bt_lna_constrain(struct btc_coexist *btcoexist,
-				    bool bt_lna_cons_on)
+static void btc8821a2ant_set_fw_bt_lna_constr(struct btc_coexist *btcoexist,
+					      bool bt_lna_cons_on)
 {
 	u8 h2c_parameter[2] = {0};
 
-	h2c_parameter[0] = 0x3;	/*  opCode, 0x3 = BT_SET_LNA_CONSTRAIN */
+	h2c_parameter[0] = 0x3;	/* opCode, 0x3 = BT_SET_LNA_CONSTRAIN */
 
 	if (bt_lna_cons_on)
-		h2c_parameter[1] |= BIT(0);
+		h2c_parameter[1] |= BIT0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], set BT LNA Constrain: %s, FW write 0x69 = 0x%x\n",
@@ -590,8 +635,8 @@ static void set_fw_bt_lna_constrain(struct btc_coexist *btcoexist,
 	btcoexist->btc_fill_h2c(btcoexist, 0x69, 2, h2c_parameter);
 }
 
-static void set_bt_lna_constrain(struct btc_coexist *btcoexist, bool force_exec,
-				 bool bt_lna_cons_on)
+static void btc8821a2_set_bt_lna_const(struct btc_coexist *btcoexist,
+				       bool force_exec, bool bt_lna_cons_on)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], %s BT Constrain = %s\n",
@@ -601,7 +646,7 @@ static void set_bt_lna_constrain(struct btc_coexist *btcoexist, bool force_exec,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], pre_bt_lna_constrain =%d, cur_bt_lna_constrain =%d\n",
+			  "[BTCoex], pre_bt_lna_constrain = %d,cur_bt_lna_constrain = %d\n",
 			  coex_dm->pre_bt_lna_constrain,
 			  coex_dm->cur_bt_lna_constrain);
 
@@ -609,7 +654,8 @@ static void set_bt_lna_constrain(struct btc_coexist *btcoexist, bool force_exec,
 		    coex_dm->cur_bt_lna_constrain)
 			return;
 	}
-	set_fw_bt_lna_constrain(btcoexist, coex_dm->cur_bt_lna_constrain);
+	btc8821a2ant_set_fw_bt_lna_constr(btcoexist,
+					  coex_dm->cur_bt_lna_constrain);
 
 	coex_dm->pre_bt_lna_constrain = coex_dm->cur_bt_lna_constrain;
 }
@@ -619,14 +665,14 @@ static void halbtc8821a2ant_set_fw_bt_psd_mode(struct btc_coexist *btcoexist,
 {
 	u8 h2c_parameter[2] = {0};
 
-	h2c_parameter[0] = 0x2;	/*  opCode, 0x2 = BT_SET_PSD_MODE */
+	h2c_parameter[0] = 0x2;	/* opCode, 0x2 = BT_SET_PSD_MODE */
 
 	h2c_parameter[1] = bt_psd_mode;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], set BT PSD mode = 0x%x, FW write 0x69 = 0x%x\n",
 		  h2c_parameter[1],
-		  h2c_parameter[0] << 8 | h2c_parameter[1]);
+		  h2c_parameter[0]<<8|h2c_parameter[1]);
 
 	btcoexist->btc_fill_h2c(btcoexist, 0x69, 2, h2c_parameter);
 }
@@ -647,7 +693,8 @@ static void halbtc8821a2ant_set_bt_psd_mode(struct btc_coexist *btcoexist,
 		if (coex_dm->pre_bt_psd_mode == coex_dm->cur_bt_psd_mode)
 			return;
 	}
-	halbtc8821a2ant_set_fw_bt_psd_mode(btcoexist, coex_dm->cur_bt_psd_mode);
+	halbtc8821a2ant_set_fw_bt_psd_mode(btcoexist,
+					   coex_dm->cur_bt_psd_mode);
 
 	coex_dm->pre_bt_psd_mode = coex_dm->cur_bt_psd_mode;
 }
@@ -660,7 +707,7 @@ static void halbtc8821a2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
 	h2c_parameter[0] = 0;
 
 	if (enable_auto_report)
-		h2c_parameter[0] |= BIT(0);
+		h2c_parameter[0] |= BIT0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], BT FW auto report : %s, FW write 0x68 = 0x%x\n",
@@ -682,7 +729,7 @@ static void halbtc8821a2ant_bt_auto_report(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], pre_bt_auto_report =%d, cur_bt_auto_report =%d\n",
+			  "[BTCoex], pre_bt_auto_report = %d, cur_bt_auto_report = %d\n",
 			  coex_dm->pre_bt_auto_report,
 			  coex_dm->cur_bt_auto_report);
 
@@ -706,7 +753,7 @@ static void halbtc8821a2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], pre_fw_dac_swing_lvl =%d, cur_fw_dac_swing_lvl =%d\n",
+			  "[BTCoex], pre_fw_dac_swing_lvl = %d, cur_fw_dac_swing_lvl = %d\n",
 			  coex_dm->pre_fw_dac_swing_lvl,
 			  coex_dm->cur_fw_dac_swing_lvl);
 
@@ -715,13 +762,14 @@ static void halbtc8821a2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
 			return;
 	}
 
-	set_fw_dac_swing_level(btcoexist, coex_dm->cur_fw_dac_swing_lvl);
+	btc8821a2ant_set_fw_dac_swing_lev(btcoexist,
+					  coex_dm->cur_fw_dac_swing_lvl);
 
 	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
 }
 
-static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-				    bool rx_rf_shrink_on)
+static void btc8821a2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
+						 bool rx_rf_shrink_on)
 {
 	if (rx_rf_shrink_on) {
 		/* Shrink RF Rx LPF corner */
@@ -730,14 +778,15 @@ static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
 		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
 					  0xfffff, 0xffffc);
 	} else {
-		/* Resume RF Rx LPF corner */
-		/*  After initialized, we can use coex_dm->bt_rf0x1e_backup */
+		/* Resume RF Rx LPF corner
+		 * After initialized, we can use coex_dm->bt_rf0x1e_backup
+		 */
 		if (btcoexist->initilized) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
 				  "[BTCoex], Resume RF Rx LPF corner!!\n");
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-						  0xfffff,
-						  coex_dm->bt_rf0x1e_backup);
+			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A,
+						  0x1e, 0xfffff,
+						   coex_dm->bt_rf0x1e_backup);
 		}
 	}
 }
@@ -753,7 +802,7 @@ static void halbtc8821a2ant_RfShrink(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], pre_rf_rx_lpf_shrink =%d, cur_rf_rx_lpf_shrink =%d\n",
+			  "[BTCoex], pre_rf_rx_lpf_shrink = %d, cur_rf_rx_lpf_shrink = %d\n",
 			  coex_dm->pre_rf_rx_lpf_shrink,
 			  coex_dm->cur_rf_rx_lpf_shrink);
 
@@ -761,25 +810,29 @@ static void halbtc8821a2ant_RfShrink(struct btc_coexist *btcoexist,
 		    coex_dm->cur_rf_rx_lpf_shrink)
 			return;
 	}
-	set_sw_rf_rx_lpf_corner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
+	btc8821a2ant_set_sw_rf_rx_lpf_corner(btcoexist,
+					     coex_dm->cur_rf_rx_lpf_shrink);
 
 	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
 }
 
-static void set_sw_penalty_tx_rate_adap(struct btc_coexist *btcoexist,
-					bool low_penalty_ra)
+static void btc8821a2ant_SetSwPenTxRateAdapt(struct btc_coexist *btcoexist,
+					     bool low_penalty_ra)
 {
 	u8 h2c_parameter[6] = {0};
 
-	h2c_parameter[0] = 0x6;	/*  opCode, 0x6 = Retry_Penalty */
+	h2c_parameter[0] = 0x6;	/* opCode, 0x6 = Retry_Penalty */
 
 	if (low_penalty_ra) {
-		h2c_parameter[1] |= BIT(0);
-		/* normal rate except MCS7/6/5, OFDM54/48/36 */
+		h2c_parameter[1] |= BIT0;
+		/*normal rate except MCS7/6/5, OFDM54/48/36 */
 		h2c_parameter[2] = 0x00;
-		h2c_parameter[3] = 0xf7;  /* MCS7 or OFDM54 */
-		h2c_parameter[4] = 0xf8;  /* MCS6 or OFDM48 */
-		h2c_parameter[5] = 0xf9;  /* MCS5 or OFDM36  */
+		/*MCS7 or OFDM54 */
+		h2c_parameter[3] = 0xf7;
+		/*MCS6 or OFDM48 */
+		h2c_parameter[4] = 0xf8;
+		/*MCS5 or OFDM36 */
+		h2c_parameter[5] = 0xf9;
 	}
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
@@ -792,7 +845,7 @@ static void set_sw_penalty_tx_rate_adap(struct btc_coexist *btcoexist,
 static void halbtc8821a2ant_low_penalty_ra(struct btc_coexist *btcoexist,
 					   bool force_exec, bool low_penalty_ra)
 {
-	/* return; */
+	/*return;*/
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
 		  "[BTCoex], %s turn LowPenaltyRA = %s\n",
 		  (force_exec ? "force to" : ""),
@@ -801,14 +854,15 @@ static void halbtc8821a2ant_low_penalty_ra(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], pre_low_penalty_ra =%d, cur_low_penalty_ra =%d\n",
+			  "[BTCoex], pre_low_penalty_ra = %d, cur_low_penalty_ra = %d\n",
 			  coex_dm->pre_low_penalty_ra,
 			  coex_dm->cur_low_penalty_ra);
 
 		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
 			return;
 	}
-	set_sw_penalty_tx_rate_adap(btcoexist, coex_dm->cur_low_penalty_ra);
+	btc8821a2ant_SetSwPenTxRateAdapt(btcoexist,
+					 coex_dm->cur_low_penalty_ra);
 
 	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
 }
@@ -823,9 +877,9 @@ static void halbtc8821a2ant_set_dac_swing_reg(struct btc_coexist *btcoexist,
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc5b, 0x3e, val);
 }
 
-static void set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
-				      bool sw_dac_swing_on,
-				      u32 sw_dac_swing_lvl)
+static void btc8821a2ant_set_sw_full_dac_swing(struct btc_coexist *btcoexist,
+					       bool sw_dac_swing_on,
+					       u32 sw_dac_swing_lvl)
 {
 	if (sw_dac_swing_on)
 		halbtc8821a2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
@@ -838,26 +892,29 @@ static void halbtc8821a2ant_dac_swing(struct btc_coexist *btcoexist,
 				      u32 dac_swing_lvl)
 {
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn DacSwing =%s, dac_swing_lvl = 0x%x\n",
+		  "[BTCoex], %s turn DacSwing = %s, dac_swing_lvl = 0x%x\n",
 		  (force_exec ? "force to" : ""),
-		  ((dac_swing_on) ? "ON" : "OFF"), dac_swing_lvl);
+		  ((dac_swing_on) ? "ON" : "OFF"),
+		  dac_swing_lvl);
 	coex_dm->cur_dac_swing_on = dac_swing_on;
 	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], pre_dac_swing_on =%d, pre_dac_swing_lvl = 0x%x, cur_dac_swing_on =%d, cur_dac_swing_lvl = 0x%x\n",
+			  "[BTCoex], pre_dac_swing_on = %d, pre_dac_swing_lvl = 0x%x, cur_dac_swing_on = %d, cur_dac_swing_lvl = 0x%x\n",
 			  coex_dm->pre_dac_swing_on,
 			  coex_dm->pre_dac_swing_lvl,
 			  coex_dm->cur_dac_swing_on,
 			  coex_dm->cur_dac_swing_lvl);
 
 		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
-		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
+		    (coex_dm->pre_dac_swing_lvl ==
+		     coex_dm->cur_dac_swing_lvl))
 			return;
 	}
 	mdelay(30);
-	set_sw_fulltime_dac_swing(btcoexist, dac_swing_on, dac_swing_lvl);
+	btc8821a2ant_set_sw_full_dac_swing(btcoexist, dac_swing_on,
+					   dac_swing_lvl);
 
 	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
 	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
@@ -888,8 +945,8 @@ static void halbtc8821a2ant_adc_back_off(struct btc_coexist *btcoexist,
 
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], pre_adc_back_off =%d, cur_adc_back_off =%d\n",
-			coex_dm->pre_adc_back_off, coex_dm->cur_adc_back_off);
+			  "[BTCoex], pre_adc_back_off = %d, cur_adc_back_off = %d\n",
+			  coex_dm->pre_adc_back_off, coex_dm->cur_adc_back_off);
 
 		if (coex_dm->pre_adc_back_off == coex_dm->cur_adc_back_off)
 			return;
@@ -936,21 +993,25 @@ static void halbtc8821a2ant_coex_table(struct btc_coexist *btcoexist,
 	if (!force_exec) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
 			  "[BTCoex], pre_val0x6c0 = 0x%x, pre_val0x6c4 = 0x%x, pre_val0x6c8 = 0x%x, pre_val0x6cc = 0x%x !!\n",
-			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4,
-			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
+			  coex_dm->pre_val0x6c0,
+			  coex_dm->pre_val0x6c4,
+			  coex_dm->pre_val0x6c8,
+			  coex_dm->pre_val0x6cc);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
 			  "[BTCoex], cur_val0x6c0 = 0x%x, cur_val0x6c4 = 0x%x, cur_val0x6c8 = 0x%x, cur_val0x6cc = 0x%x !!\n",
-			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4,
-			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
+			  coex_dm->cur_val0x6c0,
+			  coex_dm->cur_val0x6c4,
+			  coex_dm->cur_val0x6c8,
+			  coex_dm->cur_val0x6cc);
 
 		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
-			(coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
-			(coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
-			(coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
+		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
+		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
+		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
 			return;
 	}
-	halbtc8821a2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
-				       val0x6c8, val0x6cc);
+	halbtc8821a2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4, val0x6c8,
+				       val0x6cc);
 
 	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
 	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
@@ -958,18 +1019,19 @@ static void halbtc8821a2ant_coex_table(struct btc_coexist *btcoexist,
 	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
 }
 
-static void set_fw_ignore_wlan_act(struct btc_coexist *btcoexist, bool enable)
+static void halbtc8821a2ant_set_fw_ignore_wlan_act(struct btc_coexist *btcoex,
+						   bool enable)
 {
 	u8 h2c_parameter[1] = {0};
 
 	if (enable)
-		h2c_parameter[0] |= BIT(0);		/*  function enable */
+		h2c_parameter[0] |= BIT0;/* function enable */
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
 		  h2c_parameter[0]);
 
-	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
+	btcoex->btc_fill_h2c(btcoex, 0x63, 1, h2c_parameter);
 }
 
 static void halbtc8821a2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
@@ -985,11 +1047,12 @@ static void halbtc8821a2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
 			  "[BTCoex], pre_ignore_wlan_act = %d, cur_ignore_wlan_act = %d!!\n",
 			  coex_dm->pre_ignore_wlan_act,
 			  coex_dm->cur_ignore_wlan_act);
+
 		if (coex_dm->pre_ignore_wlan_act ==
 		    coex_dm->cur_ignore_wlan_act)
 			return;
 	}
-	set_fw_ignore_wlan_act(btcoexist, enable);
+	halbtc8821a2ant_set_fw_ignore_wlan_act(btcoexist, enable);
 
 	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
 }
@@ -998,7 +1061,7 @@ static void halbtc8821a2ant_set_fw_pstdma(struct btc_coexist *btcoexist,
 					  u8 byte1, u8 byte2, u8 byte3,
 					  u8 byte4, u8 byte5)
 {
-	u8 h2c_parameter[5] = {0};
+	u8 h2c_parameter[5];
 
 	h2c_parameter[0] = byte1;
 	h2c_parameter[1] = byte2;
@@ -1015,35 +1078,43 @@ static void halbtc8821a2ant_set_fw_pstdma(struct btc_coexist *btcoexist,
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], FW write 0x60(5bytes) = 0x%x%08x\n",
 		  h2c_parameter[0],
-		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
-		  h2c_parameter[3]<<8|h2c_parameter[4]);
+		  h2c_parameter[1]<<24|
+		  h2c_parameter[2]<<16|
+		  h2c_parameter[3]<<8|
+		  h2c_parameter[4]);
 
 	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
 }
 
-static void sw_mechanism1(struct btc_coexist *btcoexist, bool shrink_rx_lpf,
-			  bool low_penalty_ra, bool limited_dig,
-			  bool bt_lna_constrain)
+static void btc8821a2ant_sw_mech1(struct btc_coexist *btcoexist,
+				  bool shrink_rx_lpf,
+				  bool low_penalty_ra, bool limited_dig,
+				  bool bt_lna_constrain)
 {
 	u32 wifi_bw;
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_HT40 != wifi_bw) {  /* only shrink RF Rx LPF for HT40 */
+	if (BTC_WIFI_BW_HT40 != wifi_bw) {
+		/*only shrink RF Rx LPF for HT40*/
 		if (shrink_rx_lpf)
 			shrink_rx_lpf = false;
 	}
 
-	 halbtc8821a2ant_RfShrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
-	halbtc8821a2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
+	halbtc8821a2ant_RfShrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
+	halbtc8821a2ant_low_penalty_ra(btcoexist,
+				       NORMAL_EXEC, low_penalty_ra);
 
-	/* no limited DIG */
-	/* set_bt_lna_constrain(btcoexist, NORMAL_EXEC, bBTLNAConstrain); */
+	/* no limited DIG
+	 * btc8821a2_set_bt_lna_const(btcoexist,
+		NORMAL_EXEC, bBTLNAConstrain);
+	 */
 }
 
-static void sw_mechanism2(struct btc_coexist *btcoexist, bool agc_table_shift,
-			  bool adc_back_off, bool sw_dac_swing,
-			  u32 dac_swing_lvl)
+static void btc8821a2ant_sw_mech2(struct btc_coexist *btcoexist,
+				  bool agc_table_shift,
+				  bool adc_back_off, bool sw_dac_swing,
+				  u32 dac_swing_lvl)
 {
 	/* halbtc8821a2ant_AgcTable(btcoexist, NORMAL_EXEC, bAGCTableShift); */
 	halbtc8821a2ant_adc_back_off(btcoexist, NORMAL_EXEC, adc_back_off);
@@ -1062,22 +1133,27 @@ static void halbtc8821a2ant_set_ant_path(struct btc_coexist *btcoexist,
 	if (init_hw_cfg) {
 		/*  0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT */
 		u4tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u4tmp &= ~BIT(23);
-		u4tmp |= BIT(24);
+		u4tmp &= ~BIT23;
+		u4tmp |= BIT24;
 		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4tmp);
 
 		btcoexist->btc_write_4byte(btcoexist, 0x974, 0x3ff);
 		btcoexist->btc_write_1byte(btcoexist, 0xcb4, 0x77);
 
 		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
-			/* tell firmware "antenna inverse"  ==> WRONG firmware
-			 * antenna control code.==>need fw to fix */
+			/* tell firmware "antenna inverse"  ==>
+			 *	WRONG firmware antenna control code.
+			 *	==>need fw to fix
+			 */
 			h2c_parameter[0] = 1;
 			h2c_parameter[1] = 1;
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2, h2c_parameter);
+			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
+						h2c_parameter);
 		} else {
-			/* tell firmware "no antenna inverse" ==> WRONG firmware
-			 * antenna control code.==>need fw to fix */
+			/* tell firmware "no antenna inverse"
+			 *	==> WRONG firmware antenna control code.
+			 *	==>need fw to fix
+			 */
 			h2c_parameter[0] = 0;
 			h2c_parameter[1] = 1;
 			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
@@ -1085,7 +1161,7 @@ static void halbtc8821a2ant_set_ant_path(struct btc_coexist *btcoexist,
 		}
 	}
 
-	/*  ext switch setting */
+	/* ext switch setting */
 	switch (ant_pos_type) {
 	case BTC_ANT_WIFI_AT_MAIN:
 		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xcb7, 0x30, 0x1);
@@ -1096,16 +1172,13 @@ static void halbtc8821a2ant_set_ant_path(struct btc_coexist *btcoexist,
 	}
 }
 
-static void ps21a_tdma(struct btc_coexist *btcoexist, bool force_exec,
-		       bool turn_on, u8 type)
+static void halbtc8821a2ant_ps_tdma(struct btc_coexist *btcoexist,
+				    bool force_exec, bool turn_on, u8 type)
 {
-	/* bool turn_on_by_cnt = false; */
-	/* u8 ps_tdma_type_by_cnt = 0; */
-
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn %s PS TDMA, type =%d\n",
-		  (force_exec ? "force to" : ""),
-		  (turn_on ? "ON" : "OFF"), type);
+		  "[BTCoex], %s turn %s PS TDMA, type = %d\n",
+		  (force_exec ? "force to" : ""), (turn_on ? "ON" : "OFF"),
+		  type);
 	coex_dm->cur_ps_tdma_on = turn_on;
 	coex_dm->cur_ps_tdma = type;
 
@@ -1177,8 +1250,8 @@ static void ps21a_tdma(struct btc_coexist *btcoexist, bool force_exec,
 						      0x1a, 0x60, 0x90);
 			break;
 		case 14:
-			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0x12,
-						      0x12, 0x60, 0x90);
+			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3,
+						      0x12, 0x12, 0x60, 0x90);
 			break;
 		case 15:
 			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0xe3, 0xa,
@@ -1214,7 +1287,7 @@ static void ps21a_tdma(struct btc_coexist *btcoexist, bool force_exec,
 			break;
 		}
 	} else {
-		/*  disable PS tdma */
+		/* disable PS tdma */
 		switch (type) {
 		case 0:
 			halbtc8821a2ant_set_fw_pstdma(btcoexist, 0x0, 0x0, 0x0,
@@ -1231,25 +1304,25 @@ static void ps21a_tdma(struct btc_coexist *btcoexist, bool force_exec,
 		}
 	}
 
-	/*  update pre state */
+	/* update pre state */
 	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
 	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
 }
 
 static void halbtc8821a2ant_coex_all_off(struct btc_coexist *btcoexist)
 {
-	/*  fw all off */
-	ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+	/* fw all off */
+	halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
 	halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
 
-	/*  sw all off */
-	sw_mechanism1(btcoexist, false, false, false, false);
-	sw_mechanism2(btcoexist, false, false, false, 0x18);
+	/* sw all off */
+	btc8821a2ant_sw_mech1(btcoexist, false, false, false, false);
+	btc8821a2ant_sw_mech2(btcoexist, false, false, false, 0x18);
 
-	/*  hw all off */
-	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55555555,
-				   0x55555555, 0xffff, 0x3);
+	/* hw all off */
+	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC,
+				   0x55555555, 0x55555555, 0xffff, 0x3);
 }
 
 static void halbtc8821a2ant_coex_under_5g(struct btc_coexist *btcoexist)
@@ -1259,16 +1332,16 @@ static void halbtc8821a2ant_coex_under_5g(struct btc_coexist *btcoexist)
 
 static void halbtc8821a2ant_init_coex_dm(struct btc_coexist *btcoexist)
 {
-	/*  force to reset coex mechanism */
+	/* force to reset coex mechanism */
 	halbtc8821a2ant_coex_table(btcoexist, FORCE_EXEC, 0x55555555,
 				   0x55555555, 0xffff, 0x3);
 
-	ps21a_tdma(btcoexist, FORCE_EXEC, false, 1);
+	halbtc8821a2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
 	halbtc8821a2ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
 
-	sw_mechanism1(btcoexist, false, false, false, false);
-	sw_mechanism2(btcoexist, false, false, false, 0x18);
+	btc8821a2ant_sw_mech1(btcoexist, false, false, false, false);
+	btc8821a2ant_sw_mech2(btcoexist, false, false, false, 0x18);
 }
 
 static void halbtc8821a2ant_bt_inquiry_page(struct btc_coexist *btcoexist)
@@ -1280,7 +1353,7 @@ static void halbtc8821a2ant_bt_inquiry_page(struct btc_coexist *btcoexist)
 
 	halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 				   0x5afa5afa, 0xffff, 0x3);
-	ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
+	halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
 }
 
 static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
@@ -1304,12 +1377,12 @@ static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], Wifi IPS + BT IPS!!\n");
 
-		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
 		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
 
-		sw_mechanism1(btcoexist, false, false, false, false);
-		sw_mechanism2(btcoexist, false, false, false, 0x18);
+		btc8821a2ant_sw_mech1(btcoexist, false, false, false, false);
+		btc8821a2ant_sw_mech2(btcoexist, false, false, false, 0x18);
 
 		common = true;
 	} else if (wifi_connected &&
@@ -1321,23 +1394,24 @@ static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
 		if (wifi_busy) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Wifi Busy + BT IPS!!\n");
-			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
 		} else {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Wifi LPS + BT IPS!!\n");
-			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
 		}
 
 		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
 
-		sw_mechanism1(btcoexist, false, false, false, false);
-		sw_mechanism2(btcoexist, false, false, false, 0x18);
+		btc8821a2ant_sw_mech1(btcoexist, false, false, false, false);
+		btc8821a2ant_sw_mech2(btcoexist, false, false, false, 0x18);
 
 		common = true;
 	} else if (!wifi_connected &&
-		   (BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE ==
-		    coex_dm->bt_status)) {
+		   (BT_8821A_2ANT_BT_STATUS_CON_IDLE == coex_dm->bt_status)) {
 		low_pwr_disable = true;
 		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
 				   &low_pwr_disable);
@@ -1345,58 +1419,62 @@ static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], Wifi IPS + BT LPS!!\n");
 
-		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
 		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
 
-		sw_mechanism1(btcoexist, false, false, false, false);
-		sw_mechanism2(btcoexist, false, false, false, 0x18);
-
+		btc8821a2ant_sw_mech1(btcoexist, false, false, false, false);
+		btc8821a2ant_sw_mech2(btcoexist, false, false, false, 0x18);
 		common = true;
 	} else if (wifi_connected &&
-		   (BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE ==
-		    coex_dm->bt_status)) {
+		   (BT_8821A_2ANT_BT_STATUS_CON_IDLE == coex_dm->bt_status)) {
 		low_pwr_disable = true;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-				   &low_pwr_disable);
+		btcoexist->btc_set(btcoexist,
+			BTC_SET_ACT_DISABLE_LOW_POWER, &low_pwr_disable);
 
 		if (wifi_busy) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Wifi Busy + BT LPS!!\n");
-			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
 		} else {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Wifi LPS + BT LPS!!\n");
-			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
 		}
 
 		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
 
-		sw_mechanism1(btcoexist, true, true, true, true);
-		sw_mechanism2(btcoexist, false, false, false, 0x18);
+		btc8821a2ant_sw_mech1(btcoexist, true, true, true, true);
+		btc8821a2ant_sw_mech2(btcoexist, false, false, false, 0x18);
 
 		common = true;
 	} else if (!wifi_connected &&
-		   (BT_8821A_2ANT_BT_STATUS_NON_IDLE == coex_dm->bt_status)) {
+		   (BT_8821A_2ANT_BT_STATUS_NON_IDLE ==
+		    coex_dm->bt_status)) {
 		low_pwr_disable = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-				   &low_pwr_disable);
+		btcoexist->btc_set(btcoexist,
+			BTC_SET_ACT_DISABLE_LOW_POWER, &low_pwr_disable);
 
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 			  "[BTCoex], Wifi IPS + BT Busy!!\n");
 
-		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
 		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
 		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
 
-		sw_mechanism1(btcoexist, false, false, false, false);
-		sw_mechanism2(btcoexist, false, false, false, 0x18);
+		btc8821a2ant_sw_mech1(btcoexist, false, false,
+				      false, false);
+		btc8821a2ant_sw_mech2(btcoexist, false, false,
+				      false, 0x18);
 
 		common = true;
 	} else {
 		low_pwr_disable = true;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
+		btcoexist->btc_set(btcoexist,
+				   BTC_SET_ACT_DISABLE_LOW_POWER,
 				   &low_pwr_disable);
 
 		if (wifi_busy) {
@@ -1406,7 +1484,8 @@ static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
 		} else {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Wifi LPS + BT Busy!!\n");
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 21);
+			halbtc8821a2ant_ps_tdma(btcoexist,
+						NORMAL_EXEC, true, 21);
 
 			if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
 				halbtc8821a2ant_dec_bt_pwr(btcoexist,
@@ -1417,18 +1496,594 @@ static bool halbtc8821a2ant_is_common_action(struct btc_coexist *btcoexist)
 
 			common = true;
 		}
-		sw_mechanism1(btcoexist, true, true, true, true);
+		btc8821a2ant_sw_mech1(btcoexist, true, true, true, true);
 	}
 	return common;
 }
 
-static void tdma_duration_adjust(struct btc_coexist *btcoexist,
-				 bool sco_hid, bool tx_pause, u8 max_interval)
+static void btc8821a2_int1(struct btc_coexist *btcoexist, bool tx_pause,
+			   int result)
+{
+	if (tx_pause) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 1\n");
+
+		if (coex_dm->cur_ps_tdma == 71) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+			coex_dm->tdma_adj_type = 5;
+		} else if (coex_dm->cur_ps_tdma == 1) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+			coex_dm->tdma_adj_type = 5;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 8);
+			coex_dm->tdma_adj_type = 8;
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 13);
+			coex_dm->tdma_adj_type = 13;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 5);
+				coex_dm->tdma_adj_type = 5;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 13);
+				coex_dm->tdma_adj_type = 13;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 71);
+			coex_dm->tdma_adj_type = 71;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 4);
+			coex_dm->tdma_adj_type = 4;
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+			coex_dm->tdma_adj_type = 9;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 71) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 1);
+				coex_dm->tdma_adj_type = 1;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 4) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 1);
+				coex_dm->tdma_adj_type = 1;
+			} else if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 71);
+				coex_dm->tdma_adj_type = 71;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 9);
+				coex_dm->tdma_adj_type = 9;
+			}
+		}
+	}
+}
+
+static void btc8821a2_int2(struct btc_coexist *btcoexist, bool tx_pause,
+			   int result)
+{
+	if (tx_pause) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 1\n");
+		if (coex_dm->cur_ps_tdma == 1) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 6);
+			coex_dm->tdma_adj_type = 6;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 8);
+			coex_dm->tdma_adj_type = 8;
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+			coex_dm->tdma_adj_type = 14;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 6);
+				coex_dm->tdma_adj_type = 6;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 14);
+				coex_dm->tdma_adj_type = 14;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 2);
+			coex_dm->tdma_adj_type = 2;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 4);
+			coex_dm->tdma_adj_type = 4;
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+			coex_dm->tdma_adj_type = 10;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 4) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 2);
+				coex_dm->tdma_adj_type = 2;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 10);
+				coex_dm->tdma_adj_type = 10;
+			}
+		}
+	}
+}
+
+static void btc8821a2_int3(struct btc_coexist *btcoexist, bool tx_pause,
+			   int result)
+{
+	if (tx_pause) {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 1\n");
+		if (coex_dm->cur_ps_tdma == 1) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 2) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 3) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 7);
+			coex_dm->tdma_adj_type = 7;
+		} else if (coex_dm->cur_ps_tdma == 4) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 8);
+			coex_dm->tdma_adj_type = 8;
+		}
+		if (coex_dm->cur_ps_tdma == 9) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 10) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 11) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 15);
+			coex_dm->tdma_adj_type = 15;
+		} else if (coex_dm->cur_ps_tdma == 12) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 16);
+			coex_dm->tdma_adj_type = 16;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 5) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 8);
+				coex_dm->tdma_adj_type = 8;
+			} else if (coex_dm->cur_ps_tdma == 13) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 16);
+				coex_dm->tdma_adj_type = 16;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 8) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 7) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 6) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 7);
+				coex_dm->tdma_adj_type = 7;
+			} else if (coex_dm->cur_ps_tdma == 16) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 15) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			} else if (coex_dm->cur_ps_tdma == 14) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 15);
+				coex_dm->tdma_adj_type = 15;
+			}
+		}
+	} else {
+		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
+			  "[BTCoex], TxPause = 0\n");
+		if (coex_dm->cur_ps_tdma == 5) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 6) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 7) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 3);
+			coex_dm->tdma_adj_type = 3;
+		} else if (coex_dm->cur_ps_tdma == 8) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 4);
+			coex_dm->tdma_adj_type = 4;
+		}
+		if (coex_dm->cur_ps_tdma == 13) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 14) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 15) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 11);
+			coex_dm->tdma_adj_type = 11;
+		} else if (coex_dm->cur_ps_tdma == 16) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 12);
+			coex_dm->tdma_adj_type = 12;
+		}
+		if (result == -1) {
+			if (coex_dm->cur_ps_tdma == 1) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 4);
+				coex_dm->tdma_adj_type = 4;
+			} else if (coex_dm->cur_ps_tdma == 9) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 12);
+				coex_dm->tdma_adj_type = 12;
+			}
+		} else if (result == 1) {
+			if (coex_dm->cur_ps_tdma == 4) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 3) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 2) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 3);
+				coex_dm->tdma_adj_type = 3;
+			} else if (coex_dm->cur_ps_tdma == 12) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 11) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			} else if (coex_dm->cur_ps_tdma == 10) {
+				halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+							true, 11);
+				coex_dm->tdma_adj_type = 11;
+			}
+		}
+	}
+}
+
+static void btc8821a2ant_tdma_dur_adj(struct btc_coexist *btcoexist,
+				      bool sco_hid, bool tx_pause,
+				      u8 max_interval)
 {
-	static long up, dn, m, n, wait_count;
-	long result;
-	/* 0: no change, +1: incr WiFi duration, -1: decr WiFi duration */
-	u8 retry_count = 0;
+	static long	up, dn, m, n, wait_count;
+	 /* 0: no change, +1: increase WiFi duration,
+	  * -1: decrease WiFi duration
+	  */
+	int		result;
+	u8		retry_count = 0;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
 		  "[BTCoex], TdmaDurationAdjust()\n");
@@ -1440,64 +2095,97 @@ static void tdma_duration_adjust(struct btc_coexist *btcoexist,
 		if (sco_hid) {
 			if (tx_pause) {
 				if (max_interval == 1) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 13);
+					coex_dm->tdma_adj_type = 13;
 				} else if (max_interval == 2) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 14);
+					coex_dm->tdma_adj_type = 14;
 				} else if (max_interval == 3) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->tdma_adj_type = 15;
 				} else {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 15);
+					coex_dm->tdma_adj_type = 15;
 				}
 			} else {
 				if (max_interval == 1) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 9);
+					coex_dm->tdma_adj_type = 9;
 				} else if (max_interval == 2) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 10);
+					coex_dm->tdma_adj_type = 10;
 				} else if (max_interval == 3) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->tdma_adj_type = 11;
 				} else {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 11);
+					coex_dm->tdma_adj_type = 11;
 				}
 			}
 		} else {
 			if (tx_pause) {
 				if (max_interval == 1) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 5);
+					coex_dm->tdma_adj_type = 5;
 				} else if (max_interval == 2) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 6);
+					coex_dm->tdma_adj_type = 6;
 				} else if (max_interval == 3) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->tdma_adj_type = 7;
 				} else {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 7);
+					coex_dm->tdma_adj_type = 7;
 				}
 			} else {
 				if (max_interval == 1) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
-					coex_dm->ps_tdma_du_adj_type = 1;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 1);
+					coex_dm->tdma_adj_type = 1;
 				} else if (max_interval == 2) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 2);
+					coex_dm->tdma_adj_type = 2;
 				} else if (max_interval == 3) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->tdma_adj_type = 3;
 				} else {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
+					halbtc8821a2ant_ps_tdma(btcoexist,
+								NORMAL_EXEC,
+								true, 3);
+					coex_dm->tdma_adj_type = 3;
 				}
 			}
 		}
+
 		up = 0;
 		dn = 0;
 		m = 1;
@@ -1510,13 +2198,13 @@ static void tdma_duration_adjust(struct btc_coexist *btcoexist,
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 			  "[BTCoex], retry_count = %d\n", retry_count);
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_count =%d\n",
+			  "[BTCoex], up = %d, dn = %d, m = %d, n = %d, wait_count = %d\n",
 			  (int)up, (int)dn, (int)m, (int)n, (int)wait_count);
 		result = 0;
 		wait_count++;
 
 		if (retry_count == 0) {
-			/*  no retry in the last 2-second duration */
+			/* no retry in the last 2-second duration */
 			up++;
 			dn--;
 
@@ -1524,6 +2212,9 @@ static void tdma_duration_adjust(struct btc_coexist *btcoexist,
 				dn = 0;
 
 			if (up >= n) {
+				/* if (retry count == 0) for 2*n seconds,
+				 * make WiFi duration wider
+				 */
 				wait_count = 0;
 				n = 3;
 				up = 0;
@@ -1534,7 +2225,7 @@ static void tdma_duration_adjust(struct btc_coexist *btcoexist,
 					  "[BTCoex], Increase wifi duration!!\n");
 			}
 		} else if (retry_count <= 3) {
-			/*  <= 3 retry in the last 2-second duration */
+			/* <=3 retry in the last 2-second duration */
 			up--;
 			dn++;
 
@@ -1542,11 +2233,16 @@ static void tdma_duration_adjust(struct btc_coexist *btcoexist,
 				up = 0;
 
 			if (dn == 2) {
+				/* if retry count< 3 for 2*2 seconds,
+				 * shrink wifi duration
+				 */
 				if (wait_count <= 2)
-					m++;
+					m++; /* avoid bounce in two levels */
 				else
 					m = 1;
-
+				/* m max value is 20, max time is 120 second,
+				 * recheck if adjust WiFi duration.
+				 */
 				if (m >= 20)
 					m = 20;
 
@@ -1560,11 +2256,16 @@ static void tdma_duration_adjust(struct btc_coexist *btcoexist,
 					  "[BTCoex], Decrease wifi duration for retryCounter<3!!\n");
 			}
 		} else {
+			/* retry count > 3, if retry count > 3 happens once,
+			 * shrink WiFi duration
+			 */
 			if (wait_count == 1)
-				m++;
+				m++; /* avoid bounce in two levels */
 			else
 				m = 1;
-
+			/* m max value is 20, max time is 120 second,
+			 * recheck if adjust WiFi duration.
+			 */
 			if (m >= 20)
 				m = 20;
 
@@ -1579,510 +2280,49 @@ static void tdma_duration_adjust(struct btc_coexist *btcoexist,
 
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 			  "[BTCoex], max Interval = %d\n", max_interval);
-		if (max_interval == 1) {
-			if (tx_pause) {
-				/* TODO: refactor here */
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 71) {
-					ps21a_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 1) {
-					ps21a_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					ps21a_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					ps21a_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-						coex_dm->ps_tdma_du_adj_type = 6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
-						coex_dm->ps_tdma_du_adj_type = 8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-						coex_dm->ps_tdma_du_adj_type = 14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
-						coex_dm->ps_tdma_du_adj_type = 16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-						coex_dm->ps_tdma_du_adj_type = 6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
-						coex_dm->ps_tdma_du_adj_type = 5;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-						coex_dm->ps_tdma_du_adj_type = 14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
-						coex_dm->ps_tdma_du_adj_type = 13;
-					}
-				}
-			} else {
-				/* TODO: refactor here */
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 71);
-					coex_dm->ps_tdma_du_adj_type = 71;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 71) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
-						coex_dm->ps_tdma_du_adj_type = 1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-						coex_dm->ps_tdma_du_adj_type = 2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
-						coex_dm->ps_tdma_du_adj_type = 4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-						coex_dm->ps_tdma_du_adj_type = 10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
-						coex_dm->ps_tdma_du_adj_type = 12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-						coex_dm->ps_tdma_du_adj_type = 2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
-						coex_dm->ps_tdma_du_adj_type = 1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 71);
-						coex_dm->ps_tdma_du_adj_type = 71;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-						coex_dm->ps_tdma_du_adj_type = 10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
-						coex_dm->ps_tdma_du_adj_type = 9;
-					}
-				}
-			}
-		} else if (max_interval == 2) {
-			if (tx_pause) {
-				/* TODO: refactor here */
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-						coex_dm->ps_tdma_du_adj_type = 6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
-						coex_dm->ps_tdma_du_adj_type = 8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-						coex_dm->ps_tdma_du_adj_type = 14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
-						coex_dm->ps_tdma_du_adj_type = 16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-						coex_dm->ps_tdma_du_adj_type = 6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 6);
-						coex_dm->ps_tdma_du_adj_type = 6;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-						coex_dm->ps_tdma_du_adj_type = 14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
-						coex_dm->ps_tdma_du_adj_type = 14;
-					}
-				}
-			} else {
-				/* TODO: refactor here */
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-						coex_dm->ps_tdma_du_adj_type = 2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 4);
-						coex_dm->ps_tdma_du_adj_type = 4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-						coex_dm->ps_tdma_du_adj_type = 10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
-						coex_dm->ps_tdma_du_adj_type = 12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-						coex_dm->ps_tdma_du_adj_type = 2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 2);
-						coex_dm->ps_tdma_du_adj_type = 2;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-						coex_dm->ps_tdma_du_adj_type = 10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-						coex_dm->ps_tdma_du_adj_type = 10;
-					}
-				}
-			}
-		} else if (max_interval == 3) {
-			if (tx_pause) {
-				/* TODO: refactor here */
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 8);
-						coex_dm->ps_tdma_du_adj_type = 8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 16);
-						coex_dm->ps_tdma_du_adj_type = 16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 7);
-						coex_dm->ps_tdma_du_adj_type = 7;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 15);
-						coex_dm->ps_tdma_du_adj_type = 15;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					ps21a_tdma(btcoexist, NORMAL_EXEC,
-						   true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						ps21a_tdma(btcoexist,
-							   NORMAL_EXEC,
-							   true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						ps21a_tdma(btcoexist,
-							   NORMAL_EXEC,
-							   true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						ps21a_tdma(btcoexist,
-							   NORMAL_EXEC,
-							   true, 4);
-						coex_dm->ps_tdma_du_adj_type = 4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 12);
-						coex_dm->ps_tdma_du_adj_type = 12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC, true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						ps21a_tdma(btcoexist,
-							   NORMAL_EXEC, true, 3);
-						coex_dm->ps_tdma_du_adj_type = 3;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						ps21a_tdma(btcoexist, NORMAL_EXEC,
-							   true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						ps21a_tdma(btcoexist,
-							   NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						ps21a_tdma(btcoexist,
-							   NORMAL_EXEC, true, 11);
-						coex_dm->ps_tdma_du_adj_type = 11;
-					}
-				}
-			}
-		}
+		if (max_interval == 1)
+			btc8821a2_int1(btcoexist, tx_pause, result);
+		else if (max_interval == 2)
+			btc8821a2_int2(btcoexist, tx_pause, result);
+		else if (max_interval == 3)
+			btc8821a2_int3(btcoexist, tx_pause, result);
 	}
 
-	/*  if current PsTdma not match with the recorded one
-	 * (when scan, dhcp...),
-	 *  then we have to adjust it back to the previous record one. */
-	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
-		bool scan = false, link = false, roam = false;
+	/* if current PsTdma not match with the recorded one
+	 * (when scan, dhcp...), then we have to adjust it back to
+	 * the previous recorded one.
+	 */
+	if (coex_dm->cur_ps_tdma != coex_dm->tdma_adj_type) {
+		bool	scan = false, link = false, roam = false;
+
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], PsTdma type dismatch!!!, cur_ps_tdma =%d, recordPsTdma =%d\n",
-			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
+			  "[BTCoex], PsTdma type dismatch!!!, cur_ps_tdma = %d, recordPsTdma = %d\n",
+			  coex_dm->cur_ps_tdma, coex_dm->tdma_adj_type);
 
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
 
-		if (!scan && !link && !roam)
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true,
-				   coex_dm->ps_tdma_du_adj_type);
-		else
+		if (!scan && !link && !roam) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
+						coex_dm->tdma_adj_type);
+		} else {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
 				  "[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n");
+		}
 	}
 
-	/* when tdma_duration_adjust() is called, fw dac swing is
-	 * included in the function. */
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 0x6);
 }
 
-/*  SCO only or SCO+PAN(HS) */
+/* SCO only or SCO+PAN(HS)*/
 static void halbtc8821a2ant_action_sco(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state;
+	u8	wifi_rssi_state, bt_rssi_state;
 	u32 wifi_bw;
 
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist, 0, 2,
+							  15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
@@ -2094,63 +2334,79 @@ static void halbtc8821a2ant_action_sco(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for SCO quality at 11b/g mode */
-		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x5a5a5a5a,
-					   0x5a5a5a5a, 0xffff, 0x3);
-	else  /* for SCO quality & wifi performance balance at 11n mode */
-		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x5aea5aea,
-					   0x5aea5aea, 0xffff, 0x3);
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) {
+		/* for SCO quality at 11b/g mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC,
+					   0x5a5a5a5a, 0x5a5a5a5a, 0xffff, 0x3);
+	} else {
+		/* for SCO quality & wifi performance balance at 11n mode */
+		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC,
+					   0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
+	}
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
-		/* ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5); */
+		/* fw mechanism
+		 * halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		 */
 
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC,
-				   false, 0); /* for voice qual */
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC,
-				   false, 0); /* for voice qual */
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0); /*for voice quality*/
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0); /*for voice quality*/
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
-
+		/* fw mechanism
+		 * halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		 */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC,
-				   false, 0); /* for voice qual */
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC,
-				   false, 0); /* for voice qual */
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0); /*for voice quality*/
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 0); /*for voice quality*/
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
 
 static void halbtc8821a2ant_action_hid(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
+	u8	wifi_rssi_state, bt_rssi_state;
+	u32	wifi_bw;
 
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
@@ -2162,45 +2418,63 @@ static void halbtc8821a2ant_action_hid(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) {
+		/* for HID at 11b/g mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5a5a5a5a, 0xffff, 0x3);
-	else  /* for HID quality & wifi performance balance at 11n mode */
+	} else {
+		/* for HID quality & wifi performance balance at 11n mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5aea5aea, 0xffff, 0x3);
+	}
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
-		/*  sw mechanism */
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 13);
+		}
+
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 9);
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 13);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 9);
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 13);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
@@ -2208,14 +2482,16 @@ static void halbtc8821a2ant_action_hid(struct btc_coexist *btcoexist)
 /* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
 static void halbtc8821a2ant_action_a2dp(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
+	u8		wifi_rssi_state, bt_rssi_state;
+	u32		wifi_bw;
 
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist, 0, 2,
+							  15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
-	/* fw dac swing is called in tdma_duration_adjust() */
-	/* halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6); */
+	/* fw dac swing is called in btc8821a2ant_tdma_dur_adj()
+	 * halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+	 */
 
 	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
 		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
@@ -2225,53 +2501,65 @@ static void halbtc8821a2ant_action_a2dp(struct btc_coexist *btcoexist)
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			tdma_duration_adjust(btcoexist, false, false, 1);
-		else
-			tdma_duration_adjust(btcoexist, false, true, 1);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, false, 1);
+		} else {
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, true, 1);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			tdma_duration_adjust(btcoexist, false, false, 1);
-		else
-			tdma_duration_adjust(btcoexist, false, true, 1);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, false, 1);
+		} else {
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, true, 1);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
 
 static void halbtc8821a2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
+	u8		wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32		wifi_bw;
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist, 0, 2,
+							  15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
-	/* fw dac swing is called in tdma_duration_adjust() */
-	/* halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6); */
+	/*fw dac swing is called in btc8821a2ant_tdma_dur_adj()
+	 *halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
+	 */
 
 	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
 		halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
@@ -2281,46 +2569,61 @@ static void halbtc8821a2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
-		if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-			tdma_duration_adjust(btcoexist, false, true, 2);
-		else				/* a2dp edr rate */
-			tdma_duration_adjust(btcoexist, false, true, 1);
+		/* fw mechanism */
+		if (bt_info_ext&BIT0) {
+			/*a2dp basic rate*/
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, true, 2);
+		} else {
+			/*a2dp edr rate*/
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, true, 1);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
-		if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-			tdma_duration_adjust(btcoexist, false, true, 2);
-		else				/* a2dp edr rate */
-			tdma_duration_adjust(btcoexist, false, true, 1);
+		/* fw mechanism */
+		if (bt_info_ext&BIT0) {
+			/* a2dp basic rate */
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, true, 2);
+		} else {
+			/* a2dp edr rate */
+			btc8821a2ant_tdma_dur_adj(btcoexist, false, true, 1);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
 
 static void halbtc8821a2ant_action_pan_edr(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
+	u8		wifi_rssi_state, bt_rssi_state;
+	u32		wifi_bw;
 
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist, 0, 2,
+							  15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
@@ -2332,46 +2635,63 @@ static void halbtc8821a2ant_action_pan_edr(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) {
+		/* for HID at 11b/g mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5aff5aff, 0xffff, 0x3);
-	else  /* for HID quality & wifi performance balance at 11n mode */
+	} else {
+		/* for HID quality & wifi performance balance at 11n mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5aff5aff, 0xffff, 0x3);
+	}
 
-		if (BTC_WIFI_BW_HT40 == wifi_bw) {
-			/*  fw mechanism */
+	if (BTC_WIFI_BW_HT40 == wifi_bw) {
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 1);
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 1);
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 5);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 1);
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 5);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
@@ -2379,10 +2699,11 @@ static void halbtc8821a2ant_action_pan_edr(struct btc_coexist *btcoexist)
 /* PAN(HS) only */
 static void halbtc8821a2ant_action_pan_hs(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
+	u8		wifi_rssi_state, bt_rssi_state;
+	u32		wifi_bw;
 
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
@@ -2390,49 +2711,62 @@ static void halbtc8821a2ant_action_pan_hs(struct btc_coexist *btcoexist)
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
 			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
 						   true);
-		else
+		} else {
 			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
 						   false);
-		ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		}
+		halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   true);
-		else
-			halbtc8821a2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
+		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_dec_bt_pwr(btcoexist,
+						   NORMAL_EXEC, true);
+		} else {
+			halbtc8821a2ant_dec_bt_pwr(btcoexist,
+						   NORMAL_EXEC, false);
+		}
 
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC, false, 1);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						false, 1);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
@@ -2440,11 +2774,12 @@ static void halbtc8821a2ant_action_pan_hs(struct btc_coexist *btcoexist)
 /* PAN(EDR)+A2DP */
 static void halbtc8821a2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
+	u8	wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32	wifi_bw;
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist, 0, 2,
+							  15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
@@ -2456,74 +2791,102 @@ static void halbtc8821a2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) {
+		/* for HID at 11b/g mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5afa5afa, 0xffff, 0x3);
-	else  /* for HID quality & wifi performance balance at 11n mode */
+	} else {
+		/* for HID quality & wifi performance balance at 11n mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5afa5afa, 0xffff, 0x3);
+	}
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, false,
-						     false, 3);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, false,
-						     false, 3);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  false, 3);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  false, 3);
+			}
 		} else {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, false, true, 3);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, false, true, 3);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  true, 3);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  true, 3);
+			}
 		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
-		}
+			btc8821a2ant_sw_mech1(btcoexist, true, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
+		};
 	} else {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, false,
-						     false, 3);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, false,
-						     false, 3);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  false, 3);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  false, 3);
+			}
 		} else {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, false, true, 3);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, false, true, 3);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  true, 3);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, false,
+							  true, 3);
+			}
 		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, false, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, false,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
 
 static void halbtc8821a2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
+	u8	wifi_rssi_state, bt_rssi_state;
+	u32	wifi_bw;
 
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist, 0, 2,
+							  15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
@@ -2535,60 +2898,78 @@ static void halbtc8821a2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) {
+		/* for HID at 11b/g mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5a5f5a5f, 0xffff, 0x3);
-	else  /* for HID quality & wifi performance balance at 11n mode */
+	} else {
+		/* for HID quality & wifi performance balance at 11n mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5a5f5a5f, 0xffff, 0x3);
+	}
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
 		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 3);
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
 		halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 10);
-		else
-			ps21a_tdma(btcoexist, NORMAL_EXEC, true, 14);
+		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 10);
+		} else {
+			halbtc8821a2ant_ps_tdma(btcoexist, NORMAL_EXEC,
+						true, 14);
+		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
 
-/*  HID+A2DP+PAN(EDR) */
-static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
+/* HID+A2DP+PAN(EDR) */
+static void btc8821a2ant_act_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
+	u8	wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32	wifi_bw;
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist,
+							  0, 2, 15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	halbtc8821a2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
@@ -2600,75 +2981,103 @@ static void action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) {
+		/* for HID at 11b/g mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5a5a5a5a, 0xffff, 0x3);
-	else  /* for HID quality & wifi performance balance at 11n mode */
+	} else {
+		/* for HID quality & wifi performance balance at 11n mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5a5a5a5a, 0xffff, 0x3);
+	}
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT(0)) {	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, true, 3);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  true, 3);
 			} else {
 				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, true, true, 3);
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  true, 3);
 			}
 		} else {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, true, 3);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, true, true, 3);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  true, 3);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  true, 3);
+			}
 		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, false, 3);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, true, false, 3);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  false, 3);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  false, 3);
+			}
 		} else {
-			if (bt_info_ext & BIT(0)) {
+			if (bt_info_ext&BIT0) {
 				/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, true, 3);
-			} else				/* a2dp edr rate */ {
-				tdma_duration_adjust(btcoexist, true, true, 3);
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  true, 3);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist, true,
+							  true, 3);
 			}
 		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
 
 static void halbtc8821a2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 {
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
+	u8	wifi_rssi_state, bt_rssi_state, bt_info_ext;
+	u32	wifi_bw;
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = wifi21a_rssi_state(btcoexist, 0, 2, 15, 0);
+	wifi_rssi_state = halbtc8821a2ant_wifi_rssi_state(btcoexist, 0, 2,
+							  15, 0);
 	bt_rssi_state = halbtc8821a2ant_bt_rssi_state(2, 35, 0);
 
 	if (halbtc8821a2ant_need_to_dec_bt_pwr(btcoexist))
@@ -2678,68 +3087,100 @@ static void halbtc8821a2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
 
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /* for HID at 11b/g mode */
+	if (BTC_WIFI_BW_LEGACY == wifi_bw) {
+		/* for HID at 11b/g mode */
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5f5b5f5b, 0xffffff, 0x3);
-	else  /* for HID quality & wifi performance balance at 11n mode */
+	} else {
+		/*for HID quality & wifi performance balance at 11n mode*/
 		halbtc8821a2ant_coex_table(btcoexist, NORMAL_EXEC, 0x55ff55ff,
 					   0x5f5b5f5b, 0xffffff, 0x3);
+	}
 
 	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+			}
 		} else {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+			}
 		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, true, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, true, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	} else {
-		/*  fw mechanism */
+		/* fw mechanism */
 		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
+			if (bt_info_ext&BIT0) {
+				/* a2dp basic rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+
+			} else {
+				/* a2dp edr rate */
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+			}
 		} else {
-			if (bt_info_ext & BIT(0))	/* a2dp basic rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
-			else				/* a2dp edr rate */
-				tdma_duration_adjust(btcoexist, true, true, 2);
+			if (bt_info_ext&BIT0) {
+				/*a2dp basic rate*/
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+			} else {
+				/*a2dp edr rate*/
+				btc8821a2ant_tdma_dur_adj(btcoexist,
+							  true, true, 2);
+			}
 		}
 
-		/*  sw mechanism */
+		/* sw mechanism */
 		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
 		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, true, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, true, false,
+					      false, 0x18);
 		} else {
-			sw_mechanism1(btcoexist, false, true, false, false);
-			sw_mechanism2(btcoexist, false, false, false, 0x18);
+			btc8821a2ant_sw_mech1(btcoexist, false, true,
+					      false, false);
+			btc8821a2ant_sw_mech2(btcoexist, false, false,
+					      false, 0x18);
 		}
 	}
 }
 
 static void halbtc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 {
-	bool wifi_under_5g = false;
-	u8 algorithm = 0;
+	bool	wifi_under_5g = false;
+	u8	algorithm = 0;
 
 	if (btcoexist->manual_control) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -2747,7 +3188,8 @@ static void halbtc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 		return;
 	}
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
 
 	if (wifi_under_5g) {
 		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -2776,9 +3218,8 @@ static void halbtc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	} else {
 		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], pre_algorithm =%d, cur_algorithm =%d\n",
-				  coex_dm->pre_algorithm,
-				  coex_dm->cur_algorithm);
+				  "[BTCoex], pre_algorithm = %d, cur_algorithm = %d\n",
+			coex_dm->pre_algorithm, coex_dm->cur_algorithm);
 			coex_dm->reset_tdma_adjust = true;
 		}
 		switch (coex_dm->cur_algorithm) {
@@ -2825,7 +3266,7 @@ static void halbtc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 		case BT_8821A_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
 				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
-			action_hid_a2dp_pan_edr(btcoexist);
+			btc8821a2ant_act_hid_a2dp_pan_edr(btcoexist);
 			break;
 		case BT_8821A_2ANT_COEX_ALGO_HID_A2DP:
 			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
@@ -2842,8 +3283,13 @@ static void halbtc8821a2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
 	}
 }
 
-/*  work around function start with wa_halbtc8821a2ant_ */
-/*  extern function start with EXhalbtc8821a2ant_ */
+/*============================================================
+ *work around function start with wa_halbtc8821a2ant_
+ *============================================================
+ *============================================================
+ * extern function start with EXhalbtc8821a2ant_
+ *============================================================
+ */
 void ex_halbtc8821a2ant_init_hwconfig(struct btc_coexist *btcoexist)
 {
 	u8 u1tmp = 0;
@@ -2851,33 +3297,36 @@ void ex_halbtc8821a2ant_init_hwconfig(struct btc_coexist *btcoexist)
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 		  "[BTCoex], 2Ant Init HW Config!!\n");
 
-	/*  backup rf 0x1e value */
+	/* backup rf 0x1e value */
 	coex_dm->bt_rf0x1e_backup =
 		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
 
-	/*  0x790[5:0] = 0x5 */
+	/* 0x790[5:0] = 0x5 */
 	u1tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
 	u1tmp &= 0xc0;
 	u1tmp |= 0x5;
 	btcoexist->btc_write_1byte(btcoexist, 0x790, u1tmp);
 
-	/* Antenna config */
-	halbtc8821a2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN,
-				     true, false);
+	/*Antenna config */
+	halbtc8821a2ant_set_ant_path(btcoexist,
+				     BTC_ANT_WIFI_AT_MAIN, true, false);
 
-	/*  PTA parameter */
-	halbtc8821a2ant_coex_table(btcoexist, FORCE_EXEC,
-				   0x55555555, 0x55555555,
+	/* PTA parameter */
+	halbtc8821a2ant_coex_table(btcoexist,
+				   FORCE_EXEC, 0x55555555, 0x55555555,
 				   0xffff, 0x3);
 
-	/*  Enable counter statistics */
-	/* 0x76e[3] = 1, WLAN_Act control by PTA */
+	/* Enable counter statistics */
+	/*0x76e[3] = 1, WLAN_Act control by PTA*/
 	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
 	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
 	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
 }
 
-void ex_halbtc8821a2ant_init_coex_dm(struct btc_coexist *btcoexist)
+void
+ex_halbtc8821a2ant_init_coex_dm(
+	struct btc_coexist *btcoexist
+	)
 {
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 		  "[BTCoex], Coex Mechanism Init!!\n");
@@ -2885,11 +3334,14 @@ void ex_halbtc8821a2ant_init_coex_dm(struct btc_coexist *btcoexist)
 	halbtc8821a2ant_init_coex_dm(btcoexist);
 }
 
-void ex_halbtc8821a2ant_display_coex_info(struct btc_coexist *btcoexist)
+void
+ex_halbtc8821a2ant_display_coex_info(
+	struct btc_coexist *btcoexist
+	)
 {
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	u8 *cli_buf = btcoexist->cli_buf;
+	struct rtl_priv *rtlpriv = btcoexist->adapter;
 	u8 u1tmp[4], i, bt_info_ext, ps_tdma_case = 0;
 	u32 u4tmp[4];
 	bool roam = false, scan = false, link = false, wifi_under_5g = false;
@@ -2899,116 +3351,110 @@ void ex_halbtc8821a2ant_display_coex_info(struct btc_coexist *btcoexist)
 	u8 wifi_dot_11_chnl, wifi_hs_chnl;
 	u32 fw_ver = 0, bt_patch_ver = 0;
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n ============[BT Coexist info] ============");
-	CL_PRINTF(cli_buf);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n ============[BT Coexist info]============");
 
 	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n BT not exists !!!");
 		return;
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d ",
-		   "Ant PG number/ Ant mechanism: ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:",
 		   board_info->pg_ant_num, board_info->btdm_ant_num);
-	CL_PRINTF(cli_buf);
 
 	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
-			   "[Action Manual control]!!");
-		CL_PRINTF(cli_buf);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s", "[Action Manual control]!!");
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %d",
-		   "BT stack/ hci ext ver",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %s / %d", "BT stack/ hci ext ver",
 		   ((stack_info->profile_notified) ? "Yes" : "No"),
 		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
 	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)",
 		   "CoexVer/ FwVer/ PatchVer",
 		   glcoex_ver_date_8821a_2ant, glcoex_ver_8821a_2ant,
 		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
-			   &wifi_dot_11_chnl);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d / %d(%d)",
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_BL_HS_OPERATION, &bt_hs_on);
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U1_WIFI_DOT11_CHNL, &wifi_dot_11_chnl);
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d / %d(%d)",
 		   "Dot11 channel / HsMode(HsChnl)",
 		   wifi_dot_11_chnl, bt_hs_on, wifi_hs_chnl);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %02x %02x %02x ",
+		   "H2C Wifi inform bt chnl Info",
 		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
 		   coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
 	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n %-35s = %ld/ %ld", "Wifi rssi/ HS rssi",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %ld/ %ld", "Wifi rssi/ HS rssi",
 		   wifi_rssi, bt_hs_rssi);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d ",
-		   "Wifi link/ roam/ scan",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d/ %d ", "Wifi link/ roam/ scan",
 		   link, roam, scan);
-	CL_PRINTF(cli_buf);
 
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
-			   &wifi_traffic_dir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s / %s/ %s ",
-		   "Wifi status",
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U4_WIFI_BW, &wifi_bw);
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_BL_WIFI_BUSY, &wifi_busy);
+	btcoexist->btc_get(btcoexist,
+		BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifi_traffic_dir);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %s / %s/ %s ", "Wifi status",
 		   (wifi_under_5g ? "5G" : "2.4G"),
 		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
 		    (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
 		   ((!wifi_busy) ? "idle" :
-		    ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ? "uplink" :
-		    "downlink")));
-	CL_PRINTF(cli_buf);
+		    ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
+		     "uplink" : "downlink")));
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = [%s/ %d/ %d] ",
-		   "BT [status/ rssi/ retryCnt]",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]",
 		   ((coex_sta->c2h_bt_inquiry_page) ? ("inquiry/page scan") :
-		   ((BT_8821A_2ANT_BT_STATUS_IDLE == coex_dm->bt_status) ?
-		    "idle" : ((BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE ==
-		    coex_dm->bt_status) ? "connected-idle" : "busy"))),
-		coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-	CL_PRINTF(cli_buf);
+		    ((BT_8821A_2ANT_BT_STATUS_IDLE == coex_dm->bt_status)
+		     ? "idle" : ((BT_8821A_2ANT_BT_STATUS_CON_IDLE ==
+		     coex_dm->bt_status) ? "connected-idle" : "busy"))),
+		    coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
 
 	if (stack_info->profile_notified) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP",
 			   stack_info->sco_exist, stack_info->hid_exist,
 			   stack_info->pan_exist, stack_info->a2dp_exist);
-		CL_PRINTF(cli_buf);
 
 		btcoexist->btc_disp_dbg_msg(btcoexist,
 					    BTC_DBG_DISP_BT_LINK_INFO);
 	}
 
 	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s",
 		   "BT Info A2DP rate",
-		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
+		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
 
 	for (i = 0; i < BT_INFO_SRC_8821A_2ANT_MAX; i++) {
 		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-				   "\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
 				   glbt_info_src_8821a_2ant[i],
 				   coex_sta->bt_info_c2h[i][0],
 				   coex_sta->bt_info_c2h[i][1],
@@ -3018,137 +3464,118 @@ void ex_halbtc8821a2ant_display_coex_info(struct btc_coexist *btcoexist)
 				   coex_sta->bt_info_c2h[i][5],
 				   coex_sta->bt_info_c2h[i][6],
 				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
 		}
 	}
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %s/%s",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %s/%s",
 		   "PS state, IPS/LPS",
 		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
 		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
-	CL_PRINTF(cli_buf);
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
 
-	/*  Sw mechanism	 */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
-		   "============[Sw mechanism] ============");
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d/ %d ",
+	/* Sw mechanism*/
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+		   "============[Sw mechanism]============");
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d/ %d/ %d ",
 		   "SM1[ShRf/ LpRA/ LimDig/ btLna]",
 		   coex_dm->cur_rf_rx_lpf_shrink, coex_dm->cur_low_penalty_ra,
 		   coex_dm->limited_dig, coex_dm->cur_bt_lna_constrain);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d/ %d(0x%x) ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = %d/ %d/ %d(0x%x) ",
 		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
 		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
 		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
-	CL_PRINTF(cli_buf);
 
-	/*  Fw mechanism		 */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s",
-		   "============[Fw mechanism] ============");
-	CL_PRINTF(cli_buf);
+	/* Fw mechanism*/
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s",
+		   "============[Fw mechanism]============");
 
 	if (!btcoexist->manual_control) {
 		ps_tdma_case = coex_dm->cur_ps_tdma;
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\n %-35s = %02x %02x %02x %02x %02x case-%d",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d",
 			   "PS TDMA",
 			   coex_dm->ps_tdma_para[0], coex_dm->ps_tdma_para[1],
 			   coex_dm->ps_tdma_para[2], coex_dm->ps_tdma_para[3],
 			   coex_dm->ps_tdma_para[4], ps_tdma_case);
-		CL_PRINTF(cli_buf);
 
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d ",
-			   "DecBtPwr/ IgnWlanAct",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			   "\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct",
 			   coex_dm->cur_dec_bt_pwr,
 			   coex_dm->cur_ignore_wlan_act);
-		CL_PRINTF(cli_buf);
 	}
 
-	/*  Hw setting		 */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n %-35s", "============[Hw setting] ============");
-	CL_PRINTF(cli_buf);
+	/* Hw setting*/
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s", "============[Hw setting]============");
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x",
-		   "RF-A, 0x1e initVal",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		   "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal",
 		   coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
 
 	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
 	u1tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x ",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x ",
 		   "0x778 (W_Act)/ 0x6cc (CoTab Sel)",
 		   u1tmp[0], u1tmp[1]);
-	CL_PRINTF(cli_buf);
 
 	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x8db);
 	u1tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xc5b);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x8db(ADC)/0xc5b[29:25](DAC)",
 		   ((u1tmp[0]&0x60)>>5), ((u1tmp[1]&0x3e)>>1));
-	CL_PRINTF(cli_buf);
 
 	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xcb4);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0xcb4[7:0](ctrl)/ 0xcb4[29:28](val)",
 		   u4tmp[0]&0xff, ((u4tmp[0]&0x30000000)>>28));
-	CL_PRINTF(cli_buf);
 
 	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
 	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
 	u4tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x974);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x40/ 0x4c[24:23]/ 0x974",
 		   u1tmp[0], ((u4tmp[0]&0x01800000)>>23), u4tmp[1]);
-	CL_PRINTF(cli_buf);
 
 	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
 	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0x550(bcn ctrl)/0x522",
 		   u4tmp[0], u1tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
 	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa0a);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
 		   "0xc50(DIG)/0xa0a(CCK-TH)",
 		   u4tmp[0], u1tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xf48);
 	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
 	u1tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\n %-35s = 0x%x/ 0x%x", "OFDM-FA/ CCK-FA",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x",
+		   "OFDM-FA/ CCK-FA",
 		   u4tmp[0], (u1tmp[0]<<8) + u1tmp[1]);
-	CL_PRINTF(cli_buf);
 
 	u4tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
 	u4tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
 	u4tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x/ 0x%x/ 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
 		   "0x6c0/0x6c4/0x6c8",
 		   u4tmp[0], u4tmp[1], u4tmp[2]);
-	CL_PRINTF(cli_buf);
 
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x770 (hi-pri Rx/Tx)",
 		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = %d/ %d",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = %d/ %d",
 		   "0x774(low-pri Rx/Tx)",
 		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
 
-	/*  Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang */
+	/* Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang*/
 	u1tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x41b);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\n %-35s = 0x%x",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "\r\n %-35s = 0x%x",
 		   "0x41b (mgntQ hang chk == 0xf)",
 		   u1tmp[0]);
-	CL_PRINTF(cli_buf);
 
 	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
 }
@@ -3164,6 +3591,7 @@ void ex_halbtc8821a2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], IPS LEAVE notify\n");
 		coex_sta->under_ips = false;
+		/*halbtc8821a2ant_init_coex_dm(btcoexist);*/
 	}
 }
 
@@ -3182,39 +3610,42 @@ void ex_halbtc8821a2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
 
 void ex_halbtc8821a2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
 {
-	if (BTC_SCAN_START == type)
+	if (BTC_SCAN_START == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN START notify\n");
-	else if (BTC_SCAN_FINISH == type)
+	} else if (BTC_SCAN_FINISH == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], SCAN FINISH notify\n");
+	}
 }
 
 void ex_halbtc8821a2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
 {
-	if (BTC_ASSOCIATE_START == type)
+	if (BTC_ASSOCIATE_START == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT START notify\n");
-	else if (BTC_ASSOCIATE_FINISH == type)
+	} else if (BTC_ASSOCIATE_FINISH == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], CONNECT FINISH notify\n");
+	}
 }
 
 void ex_halbtc8821a2ant_media_status_notify(struct btc_coexist *btcoexist,
 					    u8 type)
 {
-	u8 h2c_parameter[3] = {0};
-	u32 wifi_bw;
-	u8 wifi_central_chnl;
+	u8	h2c_parameter[3] = {0};
+	u32	wifi_bw;
+	u8	wifi_central_chnl;
 
-	if (BTC_MEDIA_CONNECT == type)
+	if (BTC_MEDIA_CONNECT == type) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], MEDIA connect notify\n");
-	else
+	} else {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], MEDIA disconnect notify\n");
+	}
 
-	/*  only 2.4G we need to inform bt the chnl mask */
+	/* only 2.4G we need to inform bt the chnl mask*/
 	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
 			   &wifi_central_chnl);
 	if ((BTC_MEDIA_CONNECT == type) &&
@@ -3234,84 +3665,88 @@ void ex_halbtc8821a2ant_media_status_notify(struct btc_coexist *btcoexist,
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
 		  "[BTCoex], FW write 0x66 = 0x%x\n",
-		  h2c_parameter[0] << 16 |
-		  h2c_parameter[1] << 8 | h2c_parameter[2]);
+		h2c_parameter[0]<<16|h2c_parameter[1]<<8|h2c_parameter[2]);
 
 	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
 }
 
 void ex_halbtc8821a2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type)
-{
-	if (type == BTC_PACKET_DHCP)
+					      u8 type) {
+	if (type == BTC_PACKET_DHCP) {
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
 			  "[BTCoex], DHCP Packet notify\n");
+	}
 }
 
 void ex_halbtc8821a2ant_bt_info_notify(struct btc_coexist *btcoexist,
 				       u8 *tmp_buf, u8 length)
 {
-	u8 bt_info = 0;
-	u8 i, rsp_source = 0;
-	static u32 set_bt_lna_cnt, set_bt_psd_mode;
-	bool bt_busy = false, limited_dig = false;
-	bool wifi_connected = false, bt_hs_on = false;
+	u8		bt_info = 0;
+	u8		i, rsp_source = 0;
+	static u32	set_bt_lna_cnt, set_bt_psd_mode;
+	bool		bt_busy = false, limited_dig = false;
+	bool		wifi_connected = false, bt_hs_on = false;
 
 	coex_sta->c2h_bt_info_req_sent = false;
+
 	rsp_source = tmp_buf[0]&0xf;
 	if (rsp_source >= BT_INFO_SRC_8821A_2ANT_MAX)
 		rsp_source = BT_INFO_SRC_8821A_2ANT_WIFI_FW;
 	coex_sta->bt_info_c2h_cnt[rsp_source]++;
 
 	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
+		  "[BTCoex], Bt info[%d], length = %d, hex data = [",
 		  rsp_source, length);
 	for (i = 0; i < length; i++) {
 		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
 		if (i == 1)
 			bt_info = tmp_buf[i];
-		if (i == length-1)
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "0x%02x]\n",
-				  tmp_buf[i]);
-		else
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "0x%02x, ",
-				  tmp_buf[i]);
+		if (i == length-1) {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x]\n", tmp_buf[i]);
+		} else {
+			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+				  "0x%02x, ", tmp_buf[i]);
+		}
 	}
 
 	if (BT_INFO_SRC_8821A_2ANT_WIFI_FW != rsp_source) {
-		coex_sta->bt_retry_cnt =	/*  [3:0] */
+		coex_sta->bt_retry_cnt =	/* [3:0]*/
 			coex_sta->bt_info_c2h[rsp_source][2]&0xf;
+
 		coex_sta->bt_rssi =
 			coex_sta->bt_info_c2h[rsp_source][3]*2+10;
+
 		coex_sta->bt_info_ext =
 			coex_sta->bt_info_c2h[rsp_source][4];
 
-		/*  Here we need to resend some wifi info to BT */
-		/*  because bt is reset and loss of the info. */
-		if ((coex_sta->bt_info_ext & BIT(1))) {
-			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-					   &wifi_connected);
-			if (wifi_connected)
+		/* Here we need to resend some wifi info to BT*/
+		/* because bt is reset and loss of the info.*/
+		if ((coex_sta->bt_info_ext & BIT1)) {
+			btcoexist->btc_get(btcoexist,
+				BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
+			if (wifi_connected) {
 				ex_halbtc8821a2ant_media_status_notify(btcoexist,
-								       BTC_MEDIA_CONNECT);
-			else
+					BTC_MEDIA_CONNECT);
+			} else {
 				ex_halbtc8821a2ant_media_status_notify(btcoexist,
-								       BTC_MEDIA_DISCONNECT);
+					BTC_MEDIA_DISCONNECT);
+			}
 
 			set_bt_psd_mode = 0;
 		}
 		if (set_bt_psd_mode <= 3) {
-			/* fix CH-BW mode  */
-			halbtc8821a2ant_set_bt_psd_mode(btcoexist,
-							FORCE_EXEC, 0x0);
+			halbtc8821a2ant_set_bt_psd_mode(btcoexist, FORCE_EXEC,
+							0x0); /*fix CH-BW mode*/
 			set_bt_psd_mode++;
 		}
 
 		if (coex_dm->cur_bt_lna_constrain) {
-			if (!(coex_sta->bt_info_ext & BIT(2))) {
+			if (!(coex_sta->bt_info_ext & BIT2)) {
 				if (set_bt_lna_cnt <= 3) {
-					set_bt_lna_constrain(btcoexist,
-							     FORCE_EXEC, true);
+					btc8821a2_set_bt_lna_const(btcoexist,
+								   FORCE_EXEC,
+								   true);
 					set_bt_lna_cnt++;
 				}
 			}
@@ -3319,30 +3754,34 @@ void ex_halbtc8821a2ant_bt_info_notify(struct btc_coexist *btcoexist,
 			set_bt_lna_cnt = 0;
 		}
 
-		if ((coex_sta->bt_info_ext & BIT(3)))
+		if ((coex_sta->bt_info_ext & BIT3)) {
 			halbtc8821a2ant_ignore_wlan_act(btcoexist,
 							FORCE_EXEC, false);
-		else
-			/* BT already NOT ignore Wlan active, do nothing here */
+		} else {
+			/* BT already NOT ignore Wlan active, do nothing here.*/
+		}
 
-		if (!(coex_sta->bt_info_ext & BIT(4)))
+		if ((coex_sta->bt_info_ext & BIT4)) {
+			/* BT auto report already enabled, do nothing*/
+		} else {
 			halbtc8821a2ant_bt_auto_report(btcoexist,
 						       FORCE_EXEC, true);
+		}
 	}
 
 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	/*  check BIT(2) first ==> check if bt is under inquiry or page scan */
+	/* check BIT2 first ==> check if bt is under inquiry or page scan*/
 	if (bt_info & BT_INFO_8821A_2ANT_B_INQ_PAGE) {
 		coex_sta->c2h_bt_inquiry_page = true;
 		coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_NON_IDLE;
 	} else {
 		coex_sta->c2h_bt_inquiry_page = false;
-		if (bt_info == 0x1) {	/*  connection exists but not busy */
+		if (bt_info == 0x1) {
+			/* connection exists but not busy*/
 			coex_sta->bt_link_exist = true;
-			coex_dm->bt_status =
-				BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE;
+			coex_dm->bt_status = BT_8821A_2ANT_BT_STATUS_CON_IDLE;
 		} else if (bt_info & BT_INFO_8821A_2ANT_B_CONNECTION) {
-			/*  connection exists and some link is busy */
+			/* connection exists and some link is busy*/
 			coex_sta->bt_link_exist = true;
 			if (bt_info & BT_INFO_8821A_2ANT_B_FTP)
 				coex_sta->pan_exist = true;
@@ -3385,14 +3824,16 @@ void ex_halbtc8821a2ant_bt_info_notify(struct btc_coexist *btcoexist,
 	else
 		limited_dig = false;
 	coex_dm->limited_dig = limited_dig;
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
+	btcoexist->btc_set(btcoexist,
+		BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
 
 	halbtc8821a2ant_run_coexist_mechanism(btcoexist);
 }
 
 void ex_halbtc8821a2ant_halt_notify(struct btc_coexist *btcoexist)
 {
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
+	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
+		  "[BTCoex], Halt notify\n");
 
 	halbtc8821a2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
 	ex_halbtc8821a2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
@@ -3400,13 +3841,13 @@ void ex_halbtc8821a2ant_halt_notify(struct btc_coexist *btcoexist)
 
 void ex_halbtc8821a2ant_periodical(struct btc_coexist *btcoexist)
 {
-	static u8 dis_ver_info_cnt;
-	u32 fw_ver = 0, bt_patch_ver = 0;
+	static u8	dis_ver_info_cnt;
+	u32		fw_ver = 0, bt_patch_ver = 0;
 	struct btc_board_info *board_info = &btcoexist->board_info;
 	struct btc_stack_info *stack_info = &btcoexist->stack_info;
 
 	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], ========================== Periodical ===========================\n");
+		  "[BTCoex], ==========================Periodical===========================\n");
 
 	if (dis_ver_info_cnt <= 5) {
 		dis_ver_info_cnt += 1;
@@ -3414,7 +3855,8 @@ void ex_halbtc8821a2ant_periodical(struct btc_coexist *btcoexist)
 			  "[BTCoex], ****************************************************************\n");
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
-			  board_info->pg_ant_num, board_info->btdm_ant_num,
+			  board_info->pg_ant_num,
+			  board_info->btdm_ant_num,
 			  board_info->btdm_ant_pos);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
@@ -3425,8 +3867,7 @@ void ex_halbtc8821a2ant_periodical(struct btc_coexist *btcoexist)
 		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
-			  glcoex_ver_date_8821a_2ant,
-			  glcoex_ver_8821a_2ant,
+			  glcoex_ver_date_8821a_2ant, glcoex_ver_8821a_2ant,
 			  fw_ver, bt_patch_ver, bt_patch_ver);
 		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
 			  "[BTCoex], ****************************************************************\n");
@@ -3434,5 +3875,5 @@ void ex_halbtc8821a2ant_periodical(struct btc_coexist *btcoexist)
 
 	halbtc8821a2ant_query_bt_info(btcoexist);
 	halbtc8821a2ant_monitor_bt_ctr(btcoexist);
-	monitor_bt_enable_disable(btcoexist);
+	btc8821a2ant_mon_bt_en_dis(btcoexist);
 }
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a2ant.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a2ant.h
new file mode 100644
index 000000000000..b4cf1f53d510
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtc8821a2ant.h
@@ -0,0 +1,205 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2012  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+/*===========================================
+ * The following is for 8821A 2Ant BT Co-exist definition
+ *===========================================
+*/
+#define	BT_INFO_8821A_2ANT_B_FTP		BIT7
+#define	BT_INFO_8821A_2ANT_B_A2DP		BIT6
+#define	BT_INFO_8821A_2ANT_B_HID		BIT5
+#define	BT_INFO_8821A_2ANT_B_SCO_BUSY		BIT4
+#define	BT_INFO_8821A_2ANT_B_ACL_BUSY		BIT3
+#define	BT_INFO_8821A_2ANT_B_INQ_PAGE		BIT2
+#define	BT_INFO_8821A_2ANT_B_SCO_ESCO		BIT1
+#define	BT_INFO_8821A_2ANT_B_CONNECTION		BIT0
+
+#define	BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT	2
+
+enum _BT_INFO_SRC_8821A_2ANT {
+	BT_INFO_SRC_8821A_2ANT_WIFI_FW		= 0x0,
+	BT_INFO_SRC_8821A_2ANT_BT_RSP		= 0x1,
+	BT_INFO_SRC_8821A_2ANT_BT_ACTIVE_SEND	= 0x2,
+	BT_INFO_SRC_8821A_2ANT_MAX
+};
+
+enum _BT_8821A_2ANT_BT_STATUS {
+	BT_8821A_2ANT_BT_STATUS_IDLE		= 0x0,
+	BT_8821A_2ANT_BT_STATUS_CON_IDLE	= 0x1,
+	BT_8821A_2ANT_BT_STATUS_NON_IDLE	= 0x2,
+	BT_8821A_2ANT_BT_STATUS_MAX
+};
+
+enum _BT_8821A_2ANT_COEX_ALGO {
+	BT_8821A_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
+	BT_8821A_2ANT_COEX_ALGO_SCO			= 0x1,
+	BT_8821A_2ANT_COEX_ALGO_HID			= 0x2,
+	BT_8821A_2ANT_COEX_ALGO_A2DP			= 0x3,
+	BT_8821A_2ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
+	BT_8821A_2ANT_COEX_ALGO_PANEDR			= 0x5,
+	BT_8821A_2ANT_COEX_ALGO_PANHS			= 0x6,
+	BT_8821A_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
+	BT_8821A_2ANT_COEX_ALGO_PANEDR_HID		= 0x8,
+	BT_8821A_2ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
+	BT_8821A_2ANT_COEX_ALGO_HID_A2DP		= 0xa,
+	BT_8821A_2ANT_COEX_ALGO_MAX			= 0xb,
+};
+
+struct coex_dm_8821a_2ant {
+	/* fw mechanism */
+	bool		pre_dec_bt_pwr;
+	bool		cur_dec_bt_pwr;
+	bool		pre_bt_lna_constrain;
+	bool		cur_bt_lna_constrain;
+	u8		pre_bt_psd_mode;
+	u8		cur_bt_psd_mode;
+	u8		pre_fw_dac_swing_lvl;
+	u8		cur_fw_dac_swing_lvl;
+	bool		cur_ignore_wlan_act;
+	bool		pre_ignore_wlan_act;
+	u8		pre_ps_tdma;
+	u8		cur_ps_tdma;
+	u8		ps_tdma_para[5];
+	u8		tdma_adj_type;
+	bool		reset_tdma_adjust;
+	bool		pre_ps_tdma_on;
+	bool		cur_ps_tdma_on;
+	bool		pre_bt_auto_report;
+	bool		cur_bt_auto_report;
+
+	/* sw mechanism */
+	bool		pre_rf_rx_lpf_shrink;
+	bool		cur_rf_rx_lpf_shrink;
+	u32		bt_rf0x1e_backup;
+	bool		pre_low_penalty_ra;
+	bool		cur_low_penalty_ra;
+	bool		pre_dac_swing_on;
+	u32		pre_dac_swing_lvl;
+	bool		cur_dac_swing_on;
+	u32		cur_dac_swing_lvl;
+	bool		pre_adc_back_off;
+	bool		cur_adc_back_off;
+	bool		pre_agc_table_en;
+	bool		cur_agc_table_en;
+	u32		pre_val0x6c0;
+	u32		cur_val0x6c0;
+	u32		pre_val0x6c4;
+	u32		cur_val0x6c4;
+	u32		pre_val0x6c8;
+	u32		cur_val0x6c8;
+	u8		pre_val0x6cc;
+	u8		cur_val0x6cc;
+	bool		limited_dig;
+
+	/* algorithm related */
+	u8		pre_algorithm;
+	u8		cur_algorithm;
+	u8		bt_status;
+	u8		wifi_chnl_info[3];
+};
+
+struct coex_sta_8821a_2ant {
+	bool	bt_link_exist;
+	bool	sco_exist;
+	bool	a2dp_exist;
+	bool	hid_exist;
+	bool	pan_exist;
+	bool	under_lps;
+	bool	under_ips;
+	u32	high_priority_tx;
+	u32	high_priority_rx;
+	u32	low_priority_tx;
+	u32	low_priority_rx;
+	u8	bt_rssi;
+	u8	pre_bt_rssi_state;
+	u8	pre_wifi_rssi_state[4];
+	bool	c2h_bt_info_req_sent;
+	u8	bt_info_c2h[BT_INFO_SRC_8821A_2ANT_MAX][10];
+	u32	bt_info_c2h_cnt[BT_INFO_SRC_8821A_2ANT_MAX];
+	bool	c2h_bt_inquiry_page;
+	u8	bt_retry_cnt;
+	u8	bt_info_ext;
+};
+
+/*===========================================
+ *	The following is interface which will notify coex module.
+ *===========================================
+ */
+void
+ex_halbtc8821a2ant_init_hwconfig(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_init_coex_dm(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_ips_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_lps_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_scan_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_connect_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_media_status_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_special_packet_notify(
+	struct btc_coexist *btcoexist,
+	u8 type
+	);
+void
+ex_halbtc8821a2ant_bt_info_notify(
+	struct btc_coexist *btcoexist,
+	u8 *tmp_buf,
+	u8 length
+	);
+void
+ex_halbtc8821a2ant_halt_notify(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_periodical(
+	struct btc_coexist *btcoexist
+	);
+void
+ex_halbtc8821a2ant_display_coex_info(
+	struct btc_coexist *btcoexist
+	);
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.c b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.c
index 33da3dfcfa4f..b2791c893417 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.c
@@ -32,7 +32,6 @@
 struct btc_coexist gl_bt_coexist;
 
 u32 btc_dbg_type[BTC_MSG_MAX];
-static u8 btc_dbg_buf[100];
 
 /***************************************************
  *		Debug related function
@@ -101,7 +100,7 @@ static bool halbtc_legacy(struct rtl_priv *adapter)
 
 	bool is_legacy = false;
 
-	if ((mac->mode == WIRELESS_MODE_B) || (mac->mode == WIRELESS_MODE_B))
+	if ((mac->mode == WIRELESS_MODE_B) || (mac->mode == WIRELESS_MODE_G))
 		is_legacy = true;
 
 	return is_legacy;
@@ -389,7 +388,7 @@ static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 		btcoexist->bt_info.reject_agg_pkt = *bool_tmp;
 		break;
 	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
-		btcoexist->bt_info.b_bt_ctrl_buf_size = *bool_tmp;
+		btcoexist->bt_info.bt_ctrl_buf_size = *bool_tmp;
 		break;
 	case BTC_SET_BL_INC_SCAN_DEV_NUM:
 		btcoexist->bt_info.increase_scan_dev_num = *bool_tmp;
@@ -417,10 +416,10 @@ static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
 	/*	rtlpriv->mlmepriv.scan_compensation = *u8_tmp;  */
 		break;
 	case BTC_SET_U1_1ANT_LPS:
-		btcoexist->bt_info.lps_1ant = *u8_tmp;
+		btcoexist->bt_info.lps_val = *u8_tmp;
 		break;
 	case BTC_SET_U1_1ANT_RPWM:
-		btcoexist->bt_info.rpwm_1ant = *u8_tmp;
+		btcoexist->bt_info.rpwm_val = *u8_tmp;
 		break;
 	/* the following are some action which will be triggered  */
 	case BTC_SET_ACT_LEAVE_LPS:
@@ -497,7 +496,7 @@ static u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
 	return	rtl_read_dword(rtlpriv, reg_addr);
 }
 
-static void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
+static void halbtc_write_1byte(void *bt_context, u32 reg_addr, u32 data)
 {
 	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
 	struct rtl_priv *rtlpriv = btcoexist->adapter;
@@ -652,9 +651,7 @@ bool exhalbtc_initlize_variables(struct rtl_priv *adapter)
 	btcoexist->btc_get = halbtc_get;
 	btcoexist->btc_set = halbtc_set;
 
-	btcoexist->cli_buf = &btc_dbg_buf[0];
-
-	btcoexist->bt_info.b_bt_ctrl_buf_size = false;
+	btcoexist->bt_info.bt_ctrl_buf_size = false;
 	btcoexist->bt_info.agg_buf_size = 5;
 
 	btcoexist->bt_info.increase_scan_dev_num = false;
@@ -672,7 +669,7 @@ void exhalbtc_init_hw_config(struct btc_coexist *btcoexist)
 	btcoexist->statistics.cnt_init_hw_config++;
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_init_hwconfig(btcoexist);
+		ex_btc8723b2ant_init_hwconfig(btcoexist);
 }
 
 void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist)
@@ -686,7 +683,7 @@ void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist)
 	btcoexist->statistics.cnt_init_coex_dm++;
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_init_coex_dm(btcoexist);
+		ex_btc8723b2ant_init_coex_dm(btcoexist);
 
 	btcoexist->initilized = true;
 }
@@ -711,7 +708,7 @@ void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type)
 	halbtc_leave_low_power();
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_ips_notify(btcoexist, ips_type);
+		ex_btc8723b2ant_ips_notify(btcoexist, ips_type);
 
 	halbtc_nomal_low_power();
 }
@@ -734,7 +731,7 @@ void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type)
 		lps_type = BTC_LPS_ENABLE;
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_lps_notify(btcoexist, lps_type);
+		ex_btc8723b2ant_lps_notify(btcoexist, lps_type);
 }
 
 void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type)
@@ -757,7 +754,7 @@ void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type)
 	halbtc_leave_low_power();
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_scan_notify(btcoexist, scan_type);
+		ex_btc8723b2ant_scan_notify(btcoexist, scan_type);
 
 	halbtc_nomal_low_power();
 }
@@ -782,14 +779,12 @@ void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action)
 	halbtc_leave_low_power();
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_connect_notify(btcoexist, asso_type);
+		ex_btc8723b2ant_connect_notify(btcoexist, asso_type);
 }
 
 void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
-				 enum _RT_MEDIA_STATUS media_status)
+				 enum rt_media_status media_status)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 status;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
@@ -805,9 +800,6 @@ void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
 
 	halbtc_leave_low_power();
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		btc8723b_med_stat_notify(btcoexist, status);
-
 	halbtc_nomal_low_power();
 }
 
@@ -828,8 +820,8 @@ void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type)
 	halbtc_leave_low_power();
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_special_packet_notify(btcoexist,
-							 packet_type);
+		ex_btc8723b2ant_special_packet_notify(btcoexist,
+						      packet_type);
 
 	halbtc_nomal_low_power();
 }
@@ -844,13 +836,11 @@ void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist,
 	btcoexist->statistics.cnt_bt_info_notify++;
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_bt_info_notify(btcoexist, tmp_buf, length);
+		ex_btc8723b2ant_bt_info_notify(btcoexist, tmp_buf, length);
 }
 
 void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
 {
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 stack_op_type;
 
 	if (!halbtc_is_bt_coexist_available(btcoexist))
@@ -863,10 +853,6 @@ void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
 
 	halbtc_leave_low_power();
 
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_stack_operation_notify(btcoexist,
-							  stack_op_type);
-
 	halbtc_nomal_low_power();
 }
 
@@ -878,7 +864,7 @@ void exhalbtc_halt_notify(struct btc_coexist *btcoexist)
 		return;
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_halt_notify(btcoexist);
+		ex_btc8723b2ant_halt_notify(btcoexist);
 }
 
 void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
@@ -898,7 +884,7 @@ void exhalbtc_periodical(struct btc_coexist *btcoexist)
 	halbtc_leave_low_power();
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_periodical(btcoexist);
+		ex_btc8723b2ant_periodical(btcoexist);
 
 	halbtc_nomal_low_power();
 }
@@ -997,5 +983,5 @@ void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist)
 		return;
 
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE)
-		ex_halbtc8723b2ant_display_coex_info(btcoexist);
+		ex_btc8723b2ant_display_coex_info(btcoexist);
 }
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.h b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.h
index 049f4c8d98a8..0a903ea179ef 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.h
+++ b/drivers/net/wireless/rtlwifi/btcoexist/halbtcoutsrc.h
@@ -55,9 +55,16 @@
 #define		BTC_RATE_DISABLE			0
 #define		BTC_RATE_ENABLE				1
 
+/* single Antenna definition */
 #define		BTC_ANT_PATH_WIFI			0
 #define		BTC_ANT_PATH_BT				1
 #define		BTC_ANT_PATH_PTA			2
+/* dual Antenna definition */
+#define		BTC_ANT_WIFI_AT_MAIN			0
+#define		BTC_ANT_WIFI_AT_AUX			1
+/* coupler Antenna definition */
+#define		BTC_ANT_WIFI_AT_CPL_MAIN		0
+#define		BTC_ANT_WIFI_AT_CPL_AUX			1
 
 enum btc_chip_interface {
 	BTC_INTF_UNKNOWN	= 0,
@@ -68,7 +75,7 @@ enum btc_chip_interface {
 	BTC_INTF_MAX
 };
 
-enum BTC_CHIP_TYPE {
+enum btc_chip_type {
 	BTC_CHIP_UNDEF		= 0,
 	BTC_CHIP_CSR_BC4	= 1,
 	BTC_CHIP_CSR_BC8	= 2,
@@ -78,11 +85,12 @@ enum BTC_CHIP_TYPE {
 	BTC_CHIP_MAX
 };
 
-enum BTC_MSG_TYPE {
+enum btc_msg_type {
 	BTC_MSG_INTERFACE	= 0x0,
 	BTC_MSG_ALGORITHM	= 0x1,
 	BTC_MSG_MAX
 };
+
 extern u32 btc_dbg_type[];
 
 /* following is for BTC_MSG_INTERFACE */
@@ -101,20 +109,12 @@ extern u32 btc_dbg_type[];
 #define		ALGO_TRACE_SW_DETAIL			BIT8
 #define		ALGO_TRACE_SW_EXEC			BIT9
 
-#define		BT_COEX_ANT_TYPE_PG			0
-#define		BT_COEX_ANT_TYPE_ANTDIV			1
-#define		BT_COEX_ANT_TYPE_DETECTED		2
-#define		BTC_MIMO_PS_STATIC			0
-#define		BTC_MIMO_PS_DYNAMIC			1
-#define		BTC_RATE_DISABLE			0
-#define		BTC_RATE_ENABLE				1
-#define		BTC_ANT_PATH_WIFI			0
-#define		BTC_ANT_PATH_BT				1
-#define		BTC_ANT_PATH_PTA			2
-
-
-#define	CL_SPRINTF	snprintf
-#define	CL_PRINTF(buf)	printk("%s", buf)
+/* following is for wifi link status */
+#define		WIFI_STA_CONNECTED			BIT0
+#define		WIFI_AP_CONNECTED			BIT1
+#define		WIFI_HS_CONNECTED			BIT2
+#define		WIFI_P2P_GO_CONNECTED			BIT3
+#define		WIFI_P2P_GC_CONNECTED			BIT4
 
 #define	BTC_PRINT(dbgtype, dbgflag, printstr, ...)		\
 	do {							\
@@ -123,46 +123,15 @@ extern u32 btc_dbg_type[];
 		}						\
 	} while (0)
 
-#define	BTC_PRINT_F(dbgtype, dbgflag, printstr, ...)		\
-	do {							\
-		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {\
-			pr_info("%s: ", __func__);	\
-			printk(printstr, ##__VA_ARGS__);	\
-		}						\
-	} while (0)
-
-#define	BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _ptr)	\
-	do {							\
-		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {	\
-			int __i;				\
-			u8 *__ptr = (u8 *)_ptr;			\
-			printk printstr;			\
-			for (__i = 0; __i < 6; __i++)		\
-				printk("%02X%s", __ptr[__i], (__i == 5) ? \
-				       "" : "-");		\
-			pr_info("\n");				\
-		}						\
-	} while (0)
-
-#define BTC_PRINT_DATA(dbgtype, dbgflag, _titlestring, _hexdata, _hexdatalen) \
-	do {								\
-		if (unlikely(btc_dbg_type[dbgtype] & dbgflag))	{	\
-			int __i;					\
-			u8 *__ptr = (u8 *)_hexdata;			\
-			printk(_titlestring);				\
-			for (__i = 0; __i < (int)_hexdatalen; __i++) {	\
-				printk("%02X%s", __ptr[__i], (((__i + 1) % 4) \
-							== 0) ? "  " : " ");\
-				if (((__i + 1) % 16) == 0)		\
-					printk("\n");			\
-			}						\
-			pr_debug("\n");					\
-		}							\
-	} while (0)
-
-#define BTC_ANT_PATH_WIFI	0
-#define BTC_ANT_PATH_BT		1
-#define BTC_ANT_PATH_PTA	2
+#define	BTC_RSSI_HIGH(_rssi_)	\
+	((_rssi_ == BTC_RSSI_STATE_HIGH ||	\
+	  _rssi_ == BTC_RSSI_STATE_STAY_HIGH) ? true : false)
+#define	BTC_RSSI_MEDIUM(_rssi_)	\
+	((_rssi_ == BTC_RSSI_STATE_MEDIUM ||	\
+	  _rssi_ == BTC_RSSI_STATE_STAY_MEDIUM) ? true : false)
+#define	BTC_RSSI_LOW(_rssi_)	\
+	((_rssi_ == BTC_RSSI_STATE_LOW ||	\
+	  _rssi_ == BTC_RSSI_STATE_STAY_LOW) ? true : false)
 
 enum btc_power_save_type {
 	BTC_PS_WIFI_NATIVE = 0,
@@ -224,7 +193,6 @@ enum btc_wifi_pnp {
 	BTC_WIFI_PNP_MAX
 };
 
-
 enum btc_get_type {
 	/* type bool */
 	BTC_GET_BL_HS_OPERATION,
@@ -253,6 +221,7 @@ enum btc_get_type {
 	BTC_GET_U4_WIFI_BW,
 	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
 	BTC_GET_U4_WIFI_FW_VER,
+	BTC_GET_U4_WIFI_LINK_STATUS,
 	BTC_GET_U4_BT_PATCH_VER,
 
 	/* type u1Byte */
@@ -260,6 +229,7 @@ enum btc_get_type {
 	BTC_GET_U1_WIFI_CENTRAL_CHNL,
 	BTC_GET_U1_WIFI_HS_CHNL,
 	BTC_GET_U1_MAC_PHY_MODE,
+	BTC_GET_U1_AP_NUM,
 
 	/* for 1Ant */
 	BTC_GET_U1_LPS_MODE,
@@ -270,7 +240,6 @@ enum btc_get_type {
 	BTC_GET_MAX
 };
 
-
 enum btc_set_type {
 	/* type bool */
 	BTC_SET_BL_BT_DISABLE,
@@ -283,7 +252,6 @@ enum btc_set_type {
 
 	/* type u1Byte */
 	BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
-	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
 	BTC_SET_UI_SCAN_SIG_COMPENSATION,
 	BTC_SET_U1_AGG_BUF_SIZE,
 
@@ -295,6 +263,9 @@ enum btc_set_type {
 	/* type bool */
 	BTC_SET_BL_BT_SCO_BUSY,
 	/* type u1Byte */
+	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
+	BTC_SET_U1_LPS_VAL,
+	BTC_SET_U1_RPWM_VAL,
 	BTC_SET_U1_1ANT_LPS,
 	BTC_SET_U1_1ANT_RPWM,
 	/* type trigger some action */
@@ -358,6 +329,20 @@ enum btc_notify_type_special_packet {
 	BTC_PACKET_MAX
 };
 
+enum hci_ext_bt_operation {
+	HCI_BT_OP_NONE = 0x0,
+	HCI_BT_OP_INQUIRY_START = 0x1,
+	HCI_BT_OP_INQUIRY_FINISH = 0x2,
+	HCI_BT_OP_PAGING_START = 0x3,
+	HCI_BT_OP_PAGING_SUCCESS = 0x4,
+	HCI_BT_OP_PAGING_UNSUCCESS = 0x5,
+	HCI_BT_OP_PAIRING_START = 0x6,
+	HCI_BT_OP_PAIRING_FINISH = 0x7,
+	HCI_BT_OP_BT_DEV_ENABLE = 0x8,
+	HCI_BT_OP_BT_DEV_DISABLE = 0x9,
+	HCI_BT_OP_MAX
+};
+
 enum btc_notify_type_stack_operation {
 	BTC_STACK_OP_NONE = 0x0,
 	BTC_STACK_OP_INQ_PAGE_PAIR_START = 0x1,
@@ -365,14 +350,13 @@ enum btc_notify_type_stack_operation {
 	BTC_STACK_OP_MAX
 };
 
-
 typedef u8 (*bfp_btc_r1)(void *btc_context, u32 reg_addr);
 
 typedef u16 (*bfp_btc_r2)(void *btc_context, u32 reg_addr);
 
 typedef u32 (*bfp_btc_r4)(void *btc_context, u32 reg_addr);
 
-typedef void (*bfp_btc_w1)(void *btc_context, u32 reg_addr, u8 data);
+typedef void (*bfp_btc_w1)(void *btc_context, u32 reg_addr, u32 data);
 
 typedef void (*bfp_btc_w1_bit_mak)(void *btc_context, u32 reg_addr,
 				   u32 bit_mask, u8 data1b);
@@ -413,20 +397,22 @@ struct btc_bt_info {
 	u8 agg_buf_size;
 	bool limited_dig;
 	bool reject_agg_pkt;
-	bool b_bt_ctrl_buf_size;
+	bool bt_ctrl_buf_size;
 	bool increase_scan_dev_num;
 	u16 bt_hci_ver;
 	u16 bt_real_fw_ver;
 	u8 bt_fw_ver;
 
+	bool bt_disable_low_pwr;
+
 	/* the following is for 1Ant solution */
 	bool bt_ctrl_lps;
 	bool bt_pwr_save_mode;
 	bool bt_lps_on;
 	bool force_to_roam;
 	u8 force_exec_pwr_cmd_cnt;
-	u8 lps_1ant;
-	u8 rpwm_1ant;
+	u8 lps_val;
+	u8 rpwm_val;
 	u32 ra_mask;
 };
 
@@ -457,6 +443,7 @@ struct btc_statistics {
 	u32 cnt_special_packet_notify;
 	u32 cnt_bt_info_notify;
 	u32 cnt_periodical;
+	u32 cnt_coex_dm_switch;
 	u32 cnt_stack_operation_notify;
 	u32 cnt_dbg_ctrl;
 };
@@ -493,7 +480,6 @@ struct btc_coexist {
 	bool initilized;
 	bool stop_coex_dm;
 	bool manual_control;
-	u8 *cli_buf;
 	struct btc_statistics statistics;
 	u8 pwr_mode_val[10];
 
@@ -509,7 +495,6 @@ struct btc_coexist {
 	bfp_btc_set_bb_reg btc_set_bb_reg;
 	bfp_btc_get_bb_reg btc_get_bb_reg;
 
-
 	bfp_btc_set_rf_reg btc_set_rf_reg;
 	bfp_btc_get_rf_reg btc_get_rf_reg;
 
@@ -533,13 +518,14 @@ void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type);
 void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type);
 void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action);
 void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
-				 enum _RT_MEDIA_STATUS media_status);
+				 enum rt_media_status media_status);
 void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type);
 void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist, u8 *tmp_buf,
 			     u8 length);
 void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type);
 void exhalbtc_halt_notify(struct btc_coexist *btcoexist);
 void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
+void exhalbtc_coex_dm_switch(struct btc_coexist *btcoexist);
 void exhalbtc_periodical(struct btc_coexist *btcoexist);
 void exhalbtc_dbg_control(struct btc_coexist *btcoexist, u8 code, u8 len,
 			  u8 *data);
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.c b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.c
index 0ab94fe4cbbe..b9b0cb7af8ea 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.c
+++ b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.c
@@ -22,19 +22,19 @@
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
  *****************************************************************************/
-
 #include "../wifi.h"
-#include "rtl_btc.h"
-#include "halbt_precomp.h"
-
 #include <linux/vmalloc.h>
 #include <linux/module.h>
 
+#include "rtl_btc.h"
+#include "halbt_precomp.h"
+
 static struct rtl_btc_ops rtl_btc_operation = {
 	.btc_init_variables = rtl_btc_init_variables,
 	.btc_init_hal_vars = rtl_btc_init_hal_vars,
 	.btc_init_hw_config = rtl_btc_init_hw_config,
 	.btc_ips_notify = rtl_btc_ips_notify,
+	.btc_lps_notify = rtl_btc_lps_notify,
 	.btc_scan_notify = rtl_btc_scan_notify,
 	.btc_connect_notify = rtl_btc_connect_notify,
 	.btc_mediastatus_notify = rtl_btc_mediastatus_notify,
@@ -44,6 +44,7 @@ static struct rtl_btc_ops rtl_btc_operation = {
 	.btc_is_limited_dig = rtl_btc_is_limited_dig,
 	.btc_is_disable_edca_turbo = rtl_btc_is_disable_edca_turbo,
 	.btc_is_bt_disabled = rtl_btc_is_bt_disabled,
+	.btc_special_packet_notify = rtl_btc_special_packet_notify,
 };
 
 void rtl_btc_init_variables(struct rtl_priv *rtlpriv)
@@ -85,6 +86,11 @@ void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type)
 	exhalbtc_ips_notify(&gl_bt_coexist, type);
 }
 
+void rtl_btc_lps_notify(struct rtl_priv *rtlpriv, u8 type)
+{
+	exhalbtc_lps_notify(&gl_bt_coexist, type);
+}
+
 void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype)
 {
 	exhalbtc_scan_notify(&gl_bt_coexist, scantype);
@@ -96,13 +102,14 @@ void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action)
 }
 
 void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
-				enum _RT_MEDIA_STATUS mstatus)
+				enum rt_media_status mstatus)
 {
 	exhalbtc_mediastatus_notify(&gl_bt_coexist, mstatus);
 }
 
 void rtl_btc_periodical(struct rtl_priv *rtlpriv)
 {
+	/*rtl_bt_dm_monitor();*/
 	exhalbtc_periodical(&gl_bt_coexist);
 }
 
@@ -150,12 +157,18 @@ bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv)
 
 bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv)
 {
+	/* It seems 'bt_disabled' is never be initialized or set. */
 	if (gl_bt_coexist.bt_info.bt_disabled)
 		return true;
 	else
 		return false;
 }
 
+void rtl_btc_special_packet_notify(struct rtl_priv *rtlpriv, u8 pkt_type)
+{
+	return exhalbtc_special_packet_notify(&gl_bt_coexist, pkt_type);
+}
+
 struct rtl_btc_ops *rtl_btc_get_ops_pointer(void)
 {
 	return &rtl_btc_operation;
@@ -174,11 +187,11 @@ u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv)
 	return num;
 }
 
-enum _RT_MEDIA_STATUS mgnt_link_status_query(struct ieee80211_hw *hw)
+enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	enum _RT_MEDIA_STATUS    m_status = RT_MEDIA_DISCONNECT;
+	enum rt_media_status    m_status = RT_MEDIA_DISCONNECT;
 
 	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
 
diff --git a/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.h b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.h
index 805b22cc8fc8..ccd5a0f91e3b 100644
--- a/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.h
+++ b/drivers/net/wireless/rtlwifi/btcoexist/rtl_btc.h
@@ -31,22 +31,24 @@ void rtl_btc_init_variables(struct rtl_priv *rtlpriv);
 void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv);
 void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv);
 void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type);
+void rtl_btc_lps_notify(struct rtl_priv *rtlpriv, u8 type);
 void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype);
 void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action);
 void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
-				enum _RT_MEDIA_STATUS mstatus);
+				enum rt_media_status mstatus);
 void rtl_btc_periodical(struct rtl_priv *rtlpriv);
 void rtl_btc_halt_notify(void);
 void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmpbuf, u8 length);
 bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv);
 bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv);
 bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv);
+void rtl_btc_special_packet_notify(struct rtl_priv *rtlpriv, u8 pkt_type);
 
 struct rtl_btc_ops *rtl_btc_get_ops_pointer(void);
 
 u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv);
 u8 rtl_get_hwpg_bt_exist(struct rtl_priv *rtlpriv);
 u8 rtl_get_hwpg_bt_type(struct rtl_priv *rtlpriv);
-enum _RT_MEDIA_STATUS mgnt_link_status_query(struct ieee80211_hw *hw);
+enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/cam.c b/drivers/net/wireless/rtlwifi/cam.c
index 0276153c72cc..8fe8b4cfae6c 100644
--- a/drivers/net/wireless/rtlwifi/cam.c
+++ b/drivers/net/wireless/rtlwifi/cam.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -26,10 +22,9 @@
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
  *****************************************************************************/
-
-#include <linux/export.h>
 #include "wifi.h"
 #include "cam.h"
+#include <linux/export.h>
 
 void rtl_cam_reset_sec_info(struct ieee80211_hw *hw)
 {
@@ -52,8 +47,8 @@ static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
 	u32 target_content = 0;
 	u8 entry_i;
 
-	RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "key_cont_128: %6phC\n",
-		 key_cont_128);
+	RT_PRINT_DATA(rtlpriv, COMP_SEC, DBG_DMESG, "Key content :",
+		      key_cont_128, 16);
 
 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
 		target_command = entry_i + CAM_CONTENT_COUNT * entry_no;
@@ -68,11 +63,13 @@ static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
 			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
 					target_command);
 
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "WRITE %x: %x\n",
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 "WRITE %x: %x\n",
 				 rtlpriv->cfg->maps[WCAMI], target_content);
 			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
 				 "The Key ID is %d\n", entry_no);
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "WRITE %x: %x\n",
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 "WRITE %x: %x\n",
 				 rtlpriv->cfg->maps[RWCAM], target_command);
 
 		} else if (entry_i == 1) {
@@ -87,10 +84,10 @@ static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
 			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
 					target_command);
 
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "WRITE A4: %x\n",
-				 target_content);
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "WRITE A0: %x\n",
-				 target_command);
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 "WRITE A4: %x\n", target_content);
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 "WRITE A0: %x\n", target_command);
 
 		} else {
 
@@ -107,15 +104,15 @@ static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
 					target_command);
 			udelay(100);
 
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "WRITE A4: %x\n",
-				 target_content);
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "WRITE A0: %x\n",
-				 target_command);
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 "WRITE A4: %x\n", target_content);
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+				 "WRITE A0: %x\n", target_command);
 		}
 	}
 
-	RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD, "after set key, usconfig:%x\n",
-		 us_config);
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+		 "after set key, usconfig:%x\n", us_config);
 }
 
 u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
@@ -125,27 +122,26 @@ u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
 	u32 us_config;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 		 "EntryNo:%x, ulKeyId=%x, ulEncAlg=%x, ulUseDK=%x MacAddr %pM\n",
 		 ul_entry_idx, ul_key_id, ul_enc_alg,
 		 ul_default_key, mac_addr);
 
 	if (ul_key_id == TOTAL_CAM_ENTRY) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "<=== ulKeyId exceed!\n");
+			 "ulKeyId exceed!\n");
 		return 0;
 	}
 
-	if (ul_default_key == 1) {
+	if (ul_default_key == 1)
 		us_config = CFG_VALID | ((u16) (ul_enc_alg) << 2);
-	} else {
+	else
 		us_config = CFG_VALID | ((ul_enc_alg) << 2) | ul_key_id;
-	}
 
 	rtl_cam_program_entry(hw, ul_entry_idx, mac_addr,
-			      key_content, us_config);
+			      (u8 *)key_content, us_config);
 
-	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "<===\n");
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "end\n");
 
 	return 1;
 
@@ -289,7 +285,8 @@ u8 rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr)
 	u8 i, *addr;
 
 	if (NULL == sta_addr) {
-		RT_TRACE(rtlpriv, COMP_SEC, DBG_EMERG, "sta_addr is NULL\n");
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_EMERG,
+			 "sta_addr is NULL.\n");
 		return TOTAL_CAM_ENTRY;
 	}
 	/* Does STA already exist? */
@@ -322,7 +319,9 @@ void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
 	u8 i, *addr;
 
 	if (NULL == sta_addr) {
-		RT_TRACE(rtlpriv, COMP_SEC, DBG_EMERG, "sta_addr is NULL\n");
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_EMERG,
+			 "sta_addr is NULL.\n");
+		return;
 	}
 
 	if (is_zero_ether_addr(sta_addr)) {
@@ -339,8 +338,8 @@ void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
 			/* Remove from HW Security CAM */
 			eth_zero_addr(rtlpriv->sec.hwsec_cam_sta_addr[i]);
 			rtlpriv->sec.hwsec_cam_bitmap &= ~(BIT(0) << i);
-			RT_TRACE(rtlpriv, COMP_SEC, DBG_LOUD,
-				 "del CAM entry %d\n", i);
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "&&&&&&&&&del entry %d\n", i);
 		}
 	}
 	return;
diff --git a/drivers/net/wireless/rtlwifi/cam.h b/drivers/net/wireless/rtlwifi/cam.h
index 0105e6c1901e..35508087c0c5 100644
--- a/drivers/net/wireless/rtlwifi/cam.h
+++ b/drivers/net/wireless/rtlwifi/cam.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -36,15 +32,15 @@
 #define CFG_VALID					BIT(15)
 
 #define PAIRWISE_KEYIDX					0
-#define CAM_PAIRWISE_KEY_POSITION		4
+#define CAM_PAIRWISE_KEY_POSITION			4
 
 #define	CAM_CONFIG_USEDK				1
 #define	CAM_CONFIG_NO_USEDK				0
 
 void rtl_cam_reset_all_entry(struct ieee80211_hw *hw);
 u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
-			 u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
-			 u32 ul_default_key, u8 *key_content);
+				u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
+				u32 ul_default_key, u8 *key_content);
 int rtl_cam_delete_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
 			     u32 ul_key_id);
 void rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index);
diff --git a/drivers/net/wireless/rtlwifi/core.c b/drivers/net/wireless/rtlwifi/core.c
index 56e218e0469c..f6179bc06086 100644
--- a/drivers/net/wireless/rtlwifi/core.c
+++ b/drivers/net/wireless/rtlwifi/core.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -31,10 +27,13 @@
 #include "core.h"
 #include "cam.h"
 #include "base.h"
-#include "pci.h"
 #include "ps.h"
+#include "pwrseqcmd.h"
 
+#include "btcoexist/rtl_btc.h"
+#include <linux/firmware.h>
 #include <linux/export.h>
+#include <net/cfg80211.h>
 
 void rtl_addr_delay(u32 addr)
 {
@@ -103,7 +102,7 @@ void rtl_fw_cb(const struct firmware *firmware, void *context)
 	int err;
 
 	RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
-			 "Firmware callback routine entered!\n");
+		 "Firmware callback routine entered!\n");
 	complete(&rtlpriv->firmware_loading_complete);
 	if (!firmware) {
 		if (rtlpriv->cfg->alt_fw_name) {
@@ -129,26 +128,13 @@ found_alt:
 	memcpy(rtlpriv->rtlhal.pfirmware, firmware->data, firmware->size);
 	rtlpriv->rtlhal.fwsize = firmware->size;
 	release_firmware(firmware);
-
-	err = ieee80211_register_hw(hw);
-	if (err) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Can't register mac80211 hw\n");
-		return;
-	} else {
-		rtlpriv->mac80211.mac80211_registered = 1;
-	}
-	set_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
-
-	/*init rfkill */
-	rtl_init_rfkill(hw);
 }
 EXPORT_SYMBOL(rtl_fw_cb);
 
 /*mutex for start & stop is must here. */
 static int rtl_op_start(struct ieee80211_hw *hw)
 {
-	int err;
+	int err = 0;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
@@ -170,28 +156,33 @@ static void rtl_op_stop(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool support_remote_wakeup = false;
 
 	if (is_hal_stop(rtlhal))
 		return;
 
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&support_remote_wakeup));
 	/* here is must, because adhoc do stop and start,
 	 * but stop with RFOFF may cause something wrong,
 	 * like adhoc TP
 	 */
-	if (unlikely(ppsc->rfpwr_state == ERFOFF)) {
+	if (unlikely(ppsc->rfpwr_state == ERFOFF))
 		rtl_ips_nic_on(hw);
-	}
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
+	/* if wowlan supported, DON'T clear connected info */
+	if (!(support_remote_wakeup &&
+	      rtlhal->enter_pnp_sleep)) {
+		mac->link_state = MAC80211_NOLINK;
+		memset(mac->bssid, 0, 6);
+		mac->vendor = PEER_UNKNOWN;
 
-	mac->link_state = MAC80211_NOLINK;
-	memset(mac->bssid, 0, ETH_ALEN);
-	mac->vendor = PEER_UNKNOWN;
-
-	/*reset sec info */
-	rtl_cam_reset_sec_info(hw);
+		/* reset sec info */
+		rtl_cam_reset_sec_info(hw);
 
-	rtl_deinit_deferred_work(hw);
+		rtl_deinit_deferred_work(hw);
+	}
 	rtlpriv->intf_ops->adapter_stop(hw);
 
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
@@ -215,7 +206,6 @@ static void rtl_op_tx(struct ieee80211_hw *hw,
 
 	if (!rtlpriv->intf_ops->waitq_insert(hw, control->sta, skb))
 		rtlpriv->intf_ops->adapter_tx(hw, control->sta, skb, &tcb_desc);
-
 	return;
 
 err_free:
@@ -229,18 +219,17 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	int err = 0;
 
-	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
-
 	if (mac->vif) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "vif has been set!! mac->vif = 0x%p\n", mac->vif);
 		return -EOPNOTSUPP;
 	}
 
+	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
+
 	rtl_ips_nic_on(hw);
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
-
 	switch (ieee80211_vif_type_p2p(vif)) {
 	case NL80211_IFTYPE_P2P_CLIENT:
 		mac->p2p = P2P_ROLE_CLIENT;
@@ -251,10 +240,8 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 				 "NL80211_IFTYPE_STATION\n");
 			mac->beacon_enabled = 0;
 			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-					rtlpriv->cfg->maps
-					[RTL_IBSS_INT_MASKS]);
+					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
 		}
-		mac->link_state = MAC80211_LINKED;
 		break;
 	case NL80211_IFTYPE_ADHOC:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
@@ -267,7 +254,7 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		else
 			mac->basic_rates = 0xff0;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *) (&mac->basic_rates));
+				(u8 *)(&mac->basic_rates));
 
 		break;
 	case NL80211_IFTYPE_P2P_GO:
@@ -284,7 +271,7 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		else
 			mac->basic_rates = 0xff0;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *) (&mac->basic_rates));
+					      (u8 *)(&mac->basic_rates));
 		break;
 	case NL80211_IFTYPE_MESH_POINT:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
@@ -301,7 +288,7 @@ static int rtl_op_add_interface(struct ieee80211_hw *hw,
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "operation mode %d is not supported!\n", vif->type);
+			 "operation mode %d is not support!\n", vif->type);
 		err = -EOPNOTSUPP;
 		goto out;
 	}
@@ -339,8 +326,7 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 		if (mac->beacon_enabled == 1) {
 			mac->beacon_enabled = 0;
 			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-					rtlpriv->cfg->maps
-					[RTL_IBSS_INT_MASKS]);
+					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
 		}
 	}
 
@@ -355,12 +341,12 @@ static void rtl_op_remove_interface(struct ieee80211_hw *hw,
 	mac->vendor = PEER_UNKNOWN;
 	mac->opmode = NL80211_IFTYPE_UNSPECIFIED;
 	rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
+
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
-
 static int rtl_op_change_interface(struct ieee80211_hw *hw,
-				      struct ieee80211_vif *vif,
-				      enum nl80211_iftype new_type, bool p2p)
+				   struct ieee80211_vif *vif,
+				   enum nl80211_iftype new_type, bool p2p)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int ret;
@@ -370,10 +356,221 @@ static int rtl_op_change_interface(struct ieee80211_hw *hw,
 	vif->p2p = p2p;
 	ret = rtl_op_add_interface(hw, vif);
 	RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
-		 "p2p %x\n", p2p);
+		 "p2p  %x\n", p2p);
 	return ret;
 }
 
+#ifdef CONFIG_PM
+static u16 crc16_ccitt(u8 data, u16 crc)
+{
+	u8 shift_in, data_bit, crc_bit11, crc_bit4, crc_bit15;
+	u8 i;
+	u16 result;
+
+	for (i = 0; i < 8; i++) {
+		crc_bit15 = ((crc & BIT(15)) ? 1 : 0);
+		data_bit  = (data & (BIT(0) << i) ? 1 : 0);
+		shift_in = crc_bit15 ^ data_bit;
+
+		result = crc << 1;
+		if (shift_in == 0)
+			result &= (~BIT(0));
+		else
+			result |= BIT(0);
+
+		crc_bit11 = ((crc & BIT(11)) ? 1 : 0) ^ shift_in;
+		if (crc_bit11 == 0)
+			result &= (~BIT(12));
+		else
+			result |= BIT(12);
+
+		crc_bit4 = ((crc & BIT(4)) ? 1 : 0) ^ shift_in;
+		if (crc_bit4 == 0)
+			result &= (~BIT(5));
+		else
+			result |= BIT(5);
+
+		crc = result;
+	}
+
+	return crc;
+}
+
+static u16 _calculate_wol_pattern_crc(u8 *pattern, u16 len)
+{
+	u16 crc = 0xffff;
+	u32 i;
+
+	for (i = 0; i < len; i++)
+		crc = crc16_ccitt(pattern[i], crc);
+
+	crc = ~crc;
+
+	return crc;
+}
+
+static void _rtl_add_wowlan_patterns(struct ieee80211_hw *hw,
+				     struct cfg80211_wowlan *wow)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = &rtlpriv->mac80211;
+	struct cfg80211_pkt_pattern *patterns = wow->patterns;
+	struct rtl_wow_pattern rtl_pattern;
+	const u8 *pattern_os, *mask_os;
+	u8 mask[MAX_WOL_BIT_MASK_SIZE] = {0};
+	u8 content[MAX_WOL_PATTERN_SIZE] = {0};
+	u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+	u8 multicast_addr1[2] = {0x33, 0x33};
+	u8 multicast_addr2[3] = {0x01, 0x00, 0x5e};
+	u8 i, mask_len;
+	u16 j, len;
+
+	for (i = 0; i < wow->n_patterns; i++) {
+		memset(&rtl_pattern, 0, sizeof(struct rtl_wow_pattern));
+		memset(mask, 0, MAX_WOL_BIT_MASK_SIZE);
+		if (patterns[i].pattern_len > MAX_WOL_PATTERN_SIZE) {
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_WARNING,
+				 "Pattern[%d] is too long\n", i);
+			continue;
+		}
+		pattern_os = patterns[i].pattern;
+		mask_len = DIV_ROUND_UP(patterns[i].pattern_len, 8);
+		mask_os = patterns[i].mask;
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "pattern content\n", pattern_os,
+			       patterns[i].pattern_len);
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "mask content\n", mask_os, mask_len);
+		/* 1. unicast? multicast? or broadcast? */
+		if (memcmp(pattern_os, broadcast_addr, 6) == 0)
+			rtl_pattern.type = BROADCAST_PATTERN;
+		else if (memcmp(pattern_os, multicast_addr1, 2) == 0 ||
+			 memcmp(pattern_os, multicast_addr2, 3) == 0)
+			rtl_pattern.type = MULTICAST_PATTERN;
+		else if  (memcmp(pattern_os, mac->mac_addr, 6) == 0)
+			rtl_pattern.type = UNICAST_PATTERN;
+		else
+			rtl_pattern.type = UNKNOWN_TYPE;
+
+		/* 2. translate mask_from_os to mask_for_hw */
+
+/******************************************************************************
+ * pattern from OS uses 'ethenet frame', like this:
+
+		   |    6   |    6   |   2  |     20    |  Variable  |	4  |
+		   |--------+--------+------+-----------+------------+-----|
+		   |    802.3 Mac Header    | IP Header | TCP Packet | FCS |
+		   |   DA   |   SA   | Type |
+
+ * BUT, packet catched by our HW is in '802.11 frame', begin from LLC,
+
+	|     24 or 30      |    6   |   2  |     20    |  Variable  |  4  |
+	|-------------------+--------+------+-----------+------------+-----|
+	| 802.11 MAC Header |       LLC     | IP Header | TCP Packet | FCS |
+			    | Others | Tpye |
+
+ * Therefore, we need translate mask_from_OS to mask_to_hw.
+ * We should left-shift mask by 6 bits, then set the new bit[0~5] = 0,
+ * because new mask[0~5] means 'SA', but our HW packet begins from LLC,
+ * bit[0~5] corresponds to first 6 Bytes in LLC, they just don't match.
+ ******************************************************************************/
+
+		/* Shift 6 bits */
+		for (j = 0; j < mask_len - 1; j++) {
+			mask[j] = mask_os[j] >> 6;
+			mask[j] |= (mask_os[j + 1] & 0x3F) << 2;
+		}
+		mask[j] = (mask_os[j] >> 6) & 0x3F;
+		/* Set bit 0-5 to zero */
+		mask[0] &= 0xC0;
+
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "mask to hw\n", mask, mask_len);
+		for (j = 0; j < (MAX_WOL_BIT_MASK_SIZE + 1) / 4; j++) {
+			rtl_pattern.mask[j] = mask[j * 4];
+			rtl_pattern.mask[j] |= (mask[j * 4 + 1] << 8);
+			rtl_pattern.mask[j] |= (mask[j * 4 + 2] << 16);
+			rtl_pattern.mask[j] |= (mask[j * 4 + 3] << 24);
+		}
+
+		/* To get the wake up pattern from the mask.
+		 * We do not count first 12 bits which means
+		 * DA[6] and SA[6] in the pattern to match HW design.
+		 */
+		len = 0;
+		for (j = 12; j < patterns[i].pattern_len; j++) {
+			if ((mask_os[j / 8] >> (j % 8)) & 0x01) {
+				content[len] = pattern_os[j];
+				len++;
+			}
+		}
+
+		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+			      "pattern to hw\n", content, len);
+		/* 3. calculate crc */
+		rtl_pattern.crc = _calculate_wol_pattern_crc(content, len);
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+			 "CRC_Remainder = 0x%x", rtl_pattern.crc);
+
+		/* 4. write crc & mask_for_hw to hw */
+		rtlpriv->cfg->ops->add_wowlan_pattern(hw, &rtl_pattern, i);
+	}
+	rtl_write_byte(rtlpriv, 0x698, wow->n_patterns);
+}
+
+static int rtl_op_suspend(struct ieee80211_hw *hw,
+			  struct cfg80211_wowlan *wow)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct timeval ts;
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, "\n");
+	if (WARN_ON(!wow))
+		return -EINVAL;
+
+	/* to resolve s4 can not wake up*/
+	do_gettimeofday(&ts);
+	rtlhal->last_suspend_sec = ts.tv_sec;
+
+	if ((ppsc->wo_wlan_mode & WAKE_ON_PATTERN_MATCH) && wow->n_patterns)
+		_rtl_add_wowlan_patterns(hw, wow);
+
+	rtlhal->driver_is_goingto_unload = true;
+	rtlhal->enter_pnp_sleep = true;
+
+	rtl_lps_leave(hw);
+	rtl_op_stop(hw);
+	device_set_wakeup_enable(wiphy_dev(hw->wiphy), true);
+	return 0;
+}
+
+static int rtl_op_resume(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct timeval ts;
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG, "\n");
+	rtlhal->driver_is_goingto_unload = false;
+	rtlhal->enter_pnp_sleep = false;
+	rtlhal->wake_from_pnp_sleep = true;
+
+	/* to resovle s4 can not wake up*/
+	do_gettimeofday(&ts);
+	if (ts.tv_sec - rtlhal->last_suspend_sec < 5)
+		return -1;
+
+	rtl_op_start(hw);
+	device_set_wakeup_enable(wiphy_dev(hw->wiphy), false);
+	ieee80211_resume_disconnect(mac->vif);
+	rtlhal->wake_from_pnp_sleep = false;
+	return 0;
+}
+#endif
+
 static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -386,7 +583,7 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 		return 1;
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
-	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {	/*BIT(2)*/
+	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {	/* BIT(2)*/
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 			 "IEEE80211_CONF_CHANGE_LISTEN_INTERVAL\n");
 	}
@@ -421,8 +618,8 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 			 * is worked very well */
 			if (!rtlpriv->psc.multi_buffered)
 				queue_delayed_work(rtlpriv->works.rtl_wq,
-						&rtlpriv->works.ps_work,
-						MSECS(5));
+						   &rtlpriv->works.ps_work,
+						   MSECS(5));
 		} else {
 			rtl_swlps_rf_awake(hw);
 			rtlpriv->psc.sw_ps_enabled = false;
@@ -436,20 +633,26 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 		mac->retry_long = hw->conf.long_frame_max_tx_count;
 		mac->retry_short = hw->conf.long_frame_max_tx_count;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RETRY_LIMIT,
-					      (u8 *) (&hw->conf.
-						      long_frame_max_tx_count));
+				(u8 *)(&hw->conf.long_frame_max_tx_count));
 	}
 
-	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
+	if (changed & IEEE80211_CONF_CHANGE_CHANNEL &&
+	    !rtlpriv->proximity.proxim_on) {
 		struct ieee80211_channel *channel = hw->conf.chandef.chan;
+		enum nl80211_chan_width width = hw->conf.chandef.width;
+		enum nl80211_channel_type channel_type = NL80211_CHAN_NO_HT;
 		u8 wide_chan = (u8) channel->hw_value;
 
+		/* channel_type is for 20&40M */
+		if (width < NL80211_CHAN_WIDTH_80)
+			channel_type =
+				cfg80211_get_chandef_type(&hw->conf.chandef);
 		if (mac->act_scanning)
 			mac->n_channels++;
 
 		if (rtlpriv->dm.supp_phymode_switch &&
-		    mac->link_state < MAC80211_LINKED &&
-		    !mac->act_scanning) {
+			mac->link_state < MAC80211_LINKED &&
+			!mac->act_scanning) {
 			if (rtlpriv->cfg->ops->chk_switch_dmdp)
 				rtlpriv->cfg->ops->chk_switch_dmdp(hw);
 		}
@@ -463,48 +666,98 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 		 *info for cisco1253 bw20, so we modify
 		 *it here based on UPPER & LOWER
 		 */
-		switch (cfg80211_get_chandef_type(&hw->conf.chandef)) {
-		case NL80211_CHAN_HT20:
-		case NL80211_CHAN_NO_HT:
-			/* SC */
-			mac->cur_40_prime_sc =
-				PRIME_CHNL_OFFSET_DONT_CARE;
-			rtlphy->current_chan_bw = HT_CHANNEL_WIDTH_20;
-			mac->bw_40 = false;
-			break;
-		case NL80211_CHAN_HT40MINUS:
-			/* SC */
-			mac->cur_40_prime_sc = PRIME_CHNL_OFFSET_UPPER;
-			rtlphy->current_chan_bw =
-				HT_CHANNEL_WIDTH_20_40;
-			mac->bw_40 = true;
-
-			/*wide channel */
-			wide_chan -= 2;
-
-			break;
-		case NL80211_CHAN_HT40PLUS:
-			/* SC */
-			mac->cur_40_prime_sc = PRIME_CHNL_OFFSET_LOWER;
-			rtlphy->current_chan_bw =
-				HT_CHANNEL_WIDTH_20_40;
-			mac->bw_40 = true;
-
-			/*wide channel */
-			wide_chan += 2;
-
-			break;
-		default:
-			mac->bw_40 = false;
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
-			break;
+
+		if (width >= NL80211_CHAN_WIDTH_80) {
+			if (width == NL80211_CHAN_WIDTH_80) {
+				u32 center = hw->conf.chandef.center_freq1;
+				u32 primary =
+				(u32)hw->conf.chandef.chan->center_freq;
+
+				rtlphy->current_chan_bw =
+					HT_CHANNEL_WIDTH_80;
+				mac->bw_80 = true;
+				mac->bw_40 = true;
+				if (center > primary) {
+					mac->cur_80_prime_sc =
+					PRIME_CHNL_OFFSET_LOWER;
+					if (center - primary == 10) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_UPPER;
+
+						wide_chan += 2;
+					} else if (center - primary == 30) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_LOWER;
+
+						wide_chan += 6;
+					}
+				} else {
+					mac->cur_80_prime_sc =
+					PRIME_CHNL_OFFSET_UPPER;
+					if (primary - center == 10) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_LOWER;
+
+						wide_chan -= 2;
+					} else if (primary - center == 30) {
+						mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_UPPER;
+
+						wide_chan -= 6;
+					}
+				}
+			}
+		} else {
+			switch (channel_type) {
+			case NL80211_CHAN_HT20:
+			case NL80211_CHAN_NO_HT:
+					/* SC */
+					mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_DONT_CARE;
+					rtlphy->current_chan_bw =
+						HT_CHANNEL_WIDTH_20;
+					mac->bw_40 = false;
+					mac->bw_80 = false;
+					break;
+			case NL80211_CHAN_HT40MINUS:
+					/* SC */
+					mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_UPPER;
+					rtlphy->current_chan_bw =
+						HT_CHANNEL_WIDTH_20_40;
+					mac->bw_40 = true;
+					mac->bw_80 = false;
+
+					/*wide channel */
+					wide_chan -= 2;
+
+					break;
+			case NL80211_CHAN_HT40PLUS:
+					/* SC */
+					mac->cur_40_prime_sc =
+						PRIME_CHNL_OFFSET_LOWER;
+					rtlphy->current_chan_bw =
+						HT_CHANNEL_WIDTH_20_40;
+					mac->bw_40 = true;
+					mac->bw_80 = false;
+
+					/*wide channel */
+					wide_chan += 2;
+
+					break;
+			default:
+					mac->bw_40 = false;
+					mac->bw_80 = false;
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+						 "switch case not processed\n");
+					break;
+			}
 		}
 
 		if (wide_chan <= 0)
 			wide_chan = 1;
 
-		/* In scanning, before we go offchannel we may send a ps = 1
+		/* In scanning, when before we offchannel we may send a ps=1
 		 * null to AP, and then we may send a ps = 0 null to AP quickly,
 		 * but first null may have caused AP to put lots of packet to
 		 * hw tx buffer. These packets must be tx'd before we go off
@@ -516,12 +769,12 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 			rtlpriv->mac80211.offchan_delay = false;
 			mdelay(50);
 		}
+
 		rtlphy->current_channel = wide_chan;
 
 		rtlpriv->cfg->ops->switch_channel(hw);
 		rtlpriv->cfg->ops->set_channel_access(hw);
-		rtlpriv->cfg->ops->set_bw_mode(hw,
-				cfg80211_get_chandef_type(&hw->conf.chandef));
+		rtlpriv->cfg->ops->set_bw_mode(hw, channel_type);
 	}
 
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
@@ -530,45 +783,25 @@ static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
 }
 
 static void rtl_op_configure_filter(struct ieee80211_hw *hw,
-			     unsigned int changed_flags,
-			     unsigned int *new_flags, u64 multicast)
+				    unsigned int changed_flags,
+				    unsigned int *new_flags, u64 multicast)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u32 rx_conf;
 
 	*new_flags &= RTL_SUPPORTED_FILTERS;
-	if (!changed_flags)
+	if (0 == changed_flags)
 		return;
 
-	/* if ssid not set to hw don't check bssid
-	 * here just used for linked scanning, & linked
-	 * and nolink check bssid is set in set network_type */
-	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
-		(mac->link_state >= MAC80211_LINKED)) {
-		if (mac->opmode != NL80211_IFTYPE_AP &&
-		    mac->opmode != NL80211_IFTYPE_MESH_POINT) {
-			if (*new_flags & FIF_BCN_PRBRESP_PROMISC) {
-				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
-			} else {
-				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
-			}
-		}
-	}
-
-	/* must be called after set_chk_bssid since that function modifies the
-	 * RCR register too. */
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&rx_conf));
-
 	/*TODO: we disable broadcase now, so enable here */
 	if (changed_flags & FIF_ALLMULTI) {
 		if (*new_flags & FIF_ALLMULTI) {
-			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
 			    rtlpriv->cfg->maps[MAC_RCR_AB];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Enable receive multicast frame\n");
 		} else {
-			rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
+			mac->rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
 					  rtlpriv->cfg->maps[MAC_RCR_AB]);
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Disable receive multicast frame\n");
@@ -577,43 +810,55 @@ static void rtl_op_configure_filter(struct ieee80211_hw *hw,
 
 	if (changed_flags & FIF_FCSFAIL) {
 		if (*new_flags & FIF_FCSFAIL) {
-			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Enable receive FCS error frame\n");
 		} else {
-			rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 				 "Disable receive FCS error frame\n");
 		}
 	}
 
+	/* if ssid not set to hw don't check bssid
+	 * here just used for linked scanning, & linked
+	 * and nolink check bssid is set in set network_type
+	 */
+	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
+	    (mac->link_state >= MAC80211_LINKED)) {
+		if (mac->opmode != NL80211_IFTYPE_AP &&
+		    mac->opmode != NL80211_IFTYPE_MESH_POINT) {
+			if (*new_flags & FIF_BCN_PRBRESP_PROMISC)
+				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
+			else
+				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
+		}
+	}
 
 	if (changed_flags & FIF_CONTROL) {
 		if (*new_flags & FIF_CONTROL) {
-			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
 
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
-				 "Enable receive control frame\n");
+				 "Enable receive control frame.\n");
 		} else {
-			rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
-				 "Disable receive control frame\n");
+				 "Disable receive control frame.\n");
 		}
 	}
 
 	if (changed_flags & FIF_OTHER_BSS) {
 		if (*new_flags & FIF_OTHER_BSS) {
-			rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
+			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
-				 "Enable receive other BSS's frame\n");
+				 "Enable receive other BSS's frame.\n");
 		} else {
-			rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
+			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
-				 "Disable receive other BSS's frame\n");
+				 "Disable receive other BSS's frame.\n");
 		}
 	}
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR, (u8 *)(&rx_conf));
 }
 static int rtl_op_sta_add(struct ieee80211_hw *hw,
 			 struct ieee80211_vif *vif,
@@ -625,7 +870,7 @@ static int rtl_op_sta_add(struct ieee80211_hw *hw,
 	struct rtl_sta_info *sta_entry;
 
 	if (sta) {
-		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
 		list_add_tail(&sta_entry->list, &rtlpriv->entry_list);
 		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
@@ -633,15 +878,17 @@ static int rtl_op_sta_add(struct ieee80211_hw *hw,
 			sta_entry->wireless_mode = WIRELESS_MODE_G;
 			if (sta->supp_rates[0] <= 0xf)
 				sta_entry->wireless_mode = WIRELESS_MODE_B;
-			if (sta->ht_cap.ht_supported == true)
+			if (sta->ht_cap.ht_supported)
 				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
 
 			if (vif->type == NL80211_IFTYPE_ADHOC)
 				sta_entry->wireless_mode = WIRELESS_MODE_G;
 		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
 			sta_entry->wireless_mode = WIRELESS_MODE_A;
-			if (sta->ht_cap.ht_supported == true)
-				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
+			if (sta->ht_cap.ht_supported)
+				sta_entry->wireless_mode = WIRELESS_MODE_N_5G;
+			if (sta->vht_cap.vht_supported)
+				sta_entry->wireless_mode = WIRELESS_MODE_AC_5G;
 
 			if (vif->type == NL80211_IFTYPE_ADHOC)
 				sta_entry->wireless_mode = WIRELESS_MODE_A;
@@ -652,9 +899,10 @@ static int rtl_op_sta_add(struct ieee80211_hw *hw,
 
 		memcpy(sta_entry->mac_addr, sta->addr, ETH_ALEN);
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
-			 "Add sta addr is %pM\n", sta->addr);
+			"Add sta addr is %pM\n", sta->addr);
 		rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
 	}
+
 	return 0;
 }
 
@@ -667,17 +915,15 @@ static int rtl_op_sta_remove(struct ieee80211_hw *hw,
 	if (sta) {
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
 			 "Remove sta addr is %pM\n", sta->addr);
-		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 		sta_entry->wireless_mode = 0;
 		sta_entry->ratr_index = 0;
-
 		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
 		list_del(&sta_entry->list);
 		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
 	}
 	return 0;
 }
-
 static int _rtl_get_hal_qnum(u16 queue)
 {
 	int qnum;
@@ -707,8 +953,8 @@ static int _rtl_get_hal_qnum(u16 queue)
  *for rtl819x  BE = 0, BK = 1, VI = 2, VO = 3
  */
 static int rtl_op_conf_tx(struct ieee80211_hw *hw,
-		   struct ieee80211_vif *vif, u16 queue,
-		   const struct ieee80211_tx_queue_params *param)
+			  struct ieee80211_vif *vif, u16 queue,
+			  const struct ieee80211_tx_queue_params *param)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -731,14 +977,14 @@ static int rtl_op_conf_tx(struct ieee80211_hw *hw,
 }
 
 static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
-			     struct ieee80211_vif *vif,
-			     struct ieee80211_bss_conf *bss_conf, u32 changed)
+				    struct ieee80211_vif *vif,
+				    struct ieee80211_bss_conf *bss_conf,
+				    u32 changed)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct ieee80211_sta *sta = NULL;
 
 	mutex_lock(&rtlpriv->locks.conf_mutex);
 	if ((vif->type == NL80211_IFTYPE_ADHOC) ||
@@ -756,15 +1002,14 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				mac->beacon_enabled = 1;
 				rtlpriv->cfg->ops->update_interrupt_mask(hw,
 						rtlpriv->cfg->maps
-						[RTL_IBSS_INT_MASKS],
-						0);
+						[RTL_IBSS_INT_MASKS], 0);
 
 				if (rtlpriv->cfg->ops->linked_set_reg)
 					rtlpriv->cfg->ops->linked_set_reg(hw);
 			}
 		}
 		if ((changed & BSS_CHANGED_BEACON_ENABLED &&
-			!bss_conf->enable_beacon)) {
+		    !bss_conf->enable_beacon)) {
 			if (mac->beacon_enabled == 1) {
 				RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
 					 "ADHOC DISABLE BEACON\n");
@@ -785,8 +1030,12 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 
 	/*TODO: reference to enum ieee80211_bss_change */
 	if (changed & BSS_CHANGED_ASSOC) {
+		u8 mstatus;
 		if (bss_conf->assoc) {
 			struct ieee80211_sta *sta = NULL;
+			u8 keep_alive = 10;
+
+			mstatus = RT_MEDIA_CONNECT;
 			/* we should reset all sec info & cam
 			 * before set cam after linked, we should not
 			 * reset in disassoc, that will cause tkip->wep
@@ -804,47 +1053,89 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 
 			if (rtlpriv->cfg->ops->linked_set_reg)
 				rtlpriv->cfg->ops->linked_set_reg(hw);
+
 			rcu_read_lock();
 			sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
 			if (!sta) {
-				pr_err("ieee80211_find_sta returned NULL\n");
 				rcu_read_unlock();
 				goto out;
 			}
-
-			if (vif->type == NL80211_IFTYPE_STATION && sta)
-				rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
 			RT_TRACE(rtlpriv, COMP_EASY_CONCURRENT, DBG_LOUD,
 				 "send PS STATIC frame\n");
 			if (rtlpriv->dm.supp_phymode_switch) {
 				if (sta->ht_cap.ht_supported)
 					rtl_send_smps_action(hw, sta,
-						 IEEE80211_SMPS_STATIC);
+							IEEE80211_SMPS_STATIC);
+			}
+
+			if (rtlhal->current_bandtype == BAND_ON_5G) {
+				mac->mode = WIRELESS_MODE_A;
+			} else {
+				if (sta->supp_rates[0] <= 0xf)
+					mac->mode = WIRELESS_MODE_B;
+				else
+					mac->mode = WIRELESS_MODE_G;
+			}
+
+			if (sta->ht_cap.ht_supported) {
+				if (rtlhal->current_bandtype == BAND_ON_2_4G)
+					mac->mode = WIRELESS_MODE_N_24G;
+				else
+					mac->mode = WIRELESS_MODE_N_5G;
 			}
+
+			if (sta->vht_cap.vht_supported) {
+				if (rtlhal->current_bandtype == BAND_ON_5G)
+					mac->mode = WIRELESS_MODE_AC_5G;
+				else
+					mac->mode = WIRELESS_MODE_AC_24G;
+			}
+
+			if (vif->type == NL80211_IFTYPE_STATION && sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
 			rcu_read_unlock();
 
+			/* to avoid AP Disassociation caused by inactivity */
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_KEEP_ALIVE,
+						      (u8 *)(&keep_alive));
+
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
 				 "BSS_CHANGED_ASSOC\n");
 		} else {
+			mstatus = RT_MEDIA_DISCONNECT;
+
 			if (mac->link_state == MAC80211_LINKED) {
 				rtlpriv->enter_ps = false;
 				schedule_work(&rtlpriv->works.lps_change_work);
 			}
-
 			if (ppsc->p2p_ps_info.p2p_ps_mode > P2P_PS_NONE)
 				rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 			mac->link_state = MAC80211_NOLINK;
 			memset(mac->bssid, 0, ETH_ALEN);
 			mac->vendor = PEER_UNKNOWN;
+			mac->mode = 0;
 
 			if (rtlpriv->dm.supp_phymode_switch) {
 				if (rtlpriv->cfg->ops->chk_switch_dmdp)
 					rtlpriv->cfg->ops->chk_switch_dmdp(hw);
 			}
-
 			RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
 				 "BSS_CHANGED_UN_ASSOC\n");
 		}
+		rtlpriv->cfg->ops->set_network_type(hw, vif->type);
+		/* For FW LPS:
+		 * To tell firmware we have connected or disconnected
+		 */
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+					      HW_VAR_H2C_FW_JOINBSSRPT,
+					      (u8 *)(&mstatus));
+		ppsc->report_linked = (mstatus == RT_MEDIA_CONNECT) ?
+				      true : false;
+
+		if (rtlpriv->cfg->ops->get_btc_status())
+			rtlpriv->btcoexist.btc_ops->btc_mediastatus_notify(
+							rtlpriv, mstatus);
 	}
 
 	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
@@ -856,11 +1147,11 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_LOUD,
 			 "BSS_CHANGED_ERP_PREAMBLE use short preamble:%x\n",
-			 bss_conf->use_short_preamble);
+			  bss_conf->use_short_preamble);
 
 		mac->short_preamble = bss_conf->use_short_preamble;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACK_PREAMBLE,
-					      &mac->short_preamble);
+					      (u8 *)(&mac->short_preamble));
 	}
 
 	if (changed & BSS_CHANGED_ERP_SLOT) {
@@ -873,13 +1164,17 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 			mac->slot_time = RTL_SLOT_TIME_20;
 
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-					      &mac->slot_time);
+					      (u8 *)(&mac->slot_time));
 	}
 
 	if (changed & BSS_CHANGED_HT) {
-		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE, "BSS_CHANGED_HT\n");
+		struct ieee80211_sta *sta = NULL;
+
+		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
+			 "BSS_CHANGED_HT\n");
+
 		rcu_read_lock();
-		sta = get_sta(hw, vif, bss_conf->bssid);
+		sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
 		if (sta) {
 			if (sta->ht_cap.ampdu_density >
 			    mac->current_ampdu_density)
@@ -893,7 +1188,7 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 		rcu_read_unlock();
 
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SHORTGI_DENSITY,
-					      &mac->max_mss_density);
+					      (u8 *)(&mac->max_mss_density));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_FACTOR,
 					      &mac->current_ampdu_factor);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_MIN_SPACE,
@@ -902,19 +1197,19 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 
 	if (changed & BSS_CHANGED_BSSID) {
 		u32 basic_rates;
+		struct ieee80211_sta *sta = NULL;
 
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BSSID,
-					      (u8 *) bss_conf->bssid);
+					      (u8 *)bss_conf->bssid);
 
-		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG, "%pM\n",
-			 bss_conf->bssid);
+		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_DMESG,
+			 "bssid: %pM\n", bss_conf->bssid);
 
 		mac->vendor = PEER_UNKNOWN;
 		memcpy(mac->bssid, bss_conf->bssid, ETH_ALEN);
-		rtlpriv->cfg->ops->set_network_type(hw, vif->type);
 
 		rcu_read_lock();
-		sta = get_sta(hw, vif, bss_conf->bssid);
+		sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
 		if (!sta) {
 			rcu_read_unlock();
 			goto out;
@@ -936,11 +1231,18 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 				mac->mode = WIRELESS_MODE_N_5G;
 		}
 
+		if (sta->vht_cap.vht_supported) {
+			if (rtlhal->current_bandtype == BAND_ON_5G)
+				mac->mode = WIRELESS_MODE_AC_5G;
+			else
+				mac->mode = WIRELESS_MODE_AC_24G;
+		}
+
 		/* just station need it, because ibss & ap mode will
 		 * set in sta_add, and will be NULL here */
-		if (mac->opmode == NL80211_IFTYPE_STATION) {
+		if (vif->type == NL80211_IFTYPE_STATION) {
 			struct rtl_sta_info *sta_entry;
-			sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+			sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 			sta_entry->wireless_mode = mac->mode;
 		}
 
@@ -955,6 +1257,9 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 			 * */
 		}
 
+		if (sta->vht_cap.vht_supported)
+			mac->vht_enable = true;
+
 		if (changed & BSS_CHANGED_BASIC_RATES) {
 			/* for 5G must << RATE_6M_INDEX = 4,
 			 * because 5G have no cck rate*/
@@ -969,40 +1274,6 @@ static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
 		}
 		rcu_read_unlock();
 	}
-
-	/*
-	 * For FW LPS:
-	 * To tell firmware we have connected
-	 * to an AP. For 92SE/CE power save v2.
-	 */
-	if (changed & BSS_CHANGED_ASSOC) {
-		if (bss_conf->assoc) {
-			if (ppsc->fwctrl_lps) {
-				u8 mstatus = RT_MEDIA_CONNECT;
-				u8 keep_alive = 10;
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-						 HW_VAR_KEEP_ALIVE,
-						 &keep_alive);
-
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_H2C_FW_JOINBSSRPT,
-						      &mstatus);
-				ppsc->report_linked = true;
-			}
-		} else {
-			if (ppsc->fwctrl_lps) {
-				u8 mstatus = RT_MEDIA_DISCONNECT;
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_H2C_FW_JOINBSSRPT,
-						      &mstatus);
-				ppsc->report_linked = false;
-			}
-		}
-		if (rtlpriv->cfg->ops->bt_wifi_media_status_notify)
-			rtlpriv->cfg->ops->bt_wifi_media_status_notify(hw,
-							 ppsc->report_linked);
-	}
-
 out:
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
 }
@@ -1012,28 +1283,27 @@ static u64 rtl_op_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u64 tsf;
 
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *) (&tsf));
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *)(&tsf));
 	return tsf;
 }
 
-static void rtl_op_set_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-			   u64 tsf)
+static void rtl_op_set_tsf(struct ieee80211_hw *hw,
+			   struct ieee80211_vif *vif, u64 tsf)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
 
 	mac->tsf = tsf;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, &bibss);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *)(&bibss));
 }
 
-static void rtl_op_reset_tsf(struct ieee80211_hw *hw,
-			     struct ieee80211_vif *vif)
+static void rtl_op_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmp = 0;
 
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_DUAL_TSF_RST, &tmp);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_DUAL_TSF_RST, (u8 *)(&tmp));
 }
 
 static void rtl_op_sta_notify(struct ieee80211_hw *hw,
@@ -1063,13 +1333,13 @@ static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
 	case IEEE80211_AMPDU_TX_START:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 "IEEE80211_AMPDU_TX_START: TID:%d\n", tid);
-		return rtl_tx_agg_start(hw, sta, tid, ssn);
+		return rtl_tx_agg_start(hw, vif, sta, tid, ssn);
 	case IEEE80211_AMPDU_TX_STOP_CONT:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH:
 	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 "IEEE80211_AMPDU_TX_STOP: TID:%d\n", tid);
-		return rtl_tx_agg_stop(hw, sta, tid);
+		return rtl_tx_agg_stop(hw, vif, sta, tid);
 	case IEEE80211_AMPDU_TX_OPERATIONAL:
 		RT_TRACE(rtlpriv, COMP_MAC80211, DBG_TRACE,
 			 "IEEE80211_AMPDU_TX_OPERATIONAL:TID:%d\n", tid);
@@ -1103,10 +1373,14 @@ static void rtl_op_sw_scan_start(struct ieee80211_hw *hw)
 		return;
 	}
 
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_scan_notify(rtlpriv, 1);
+
 	if (rtlpriv->dm.supp_phymode_switch) {
 		if (rtlpriv->cfg->ops->chk_switch_dmdp)
 			rtlpriv->cfg->ops->chk_switch_dmdp(hw);
 	}
+
 	if (mac->link_state == MAC80211_LINKED) {
 		rtlpriv->enter_ps = false;
 		schedule_work(&rtlpriv->works.lps_change_work);
@@ -1115,11 +1389,11 @@ static void rtl_op_sw_scan_start(struct ieee80211_hw *hw)
 		rtl_ips_nic_on(hw);
 	}
 
-	/* Dual mac */
+	/* Dul mac */
 	rtlpriv->rtlhal.load_imrandiqk_setting_for2g = false;
 
 	rtlpriv->cfg->ops->led_control(hw, LED_CTL_SITE_SURVEY);
-	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_BACKUP);
+	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_BACKUP_BAND0);
 }
 
 static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
@@ -1133,13 +1407,13 @@ static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
 	if (rtlpriv->link_info.higher_busytraffic)
 		return;
 
-	/*p2p will use 1/6/11 to scan */
+	/* p2p will use 1/6/11 to scan */
 	if (mac->n_channels == 3)
 		mac->p2p_in_use = true;
 	else
 		mac->p2p_in_use = false;
 	mac->n_channels = 0;
-	/* Dual mac */
+	/* Dul mac */
 	rtlpriv->rtlhal.load_imrandiqk_setting_for2g = false;
 
 	if (mac->link_state == MAC80211_LINKED_SCANNING) {
@@ -1151,6 +1425,8 @@ static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
 	}
 
 	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_RESTORE);
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_scan_notify(rtlpriv, 0);
 }
 
 static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
@@ -1158,7 +1434,6 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			  struct ieee80211_key_conf *key)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u8 key_type = NO_ENCRYPTION;
 	u8 key_idx;
 	bool group_key = false;
@@ -1174,13 +1449,13 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	}
 	/* To support IBSS, use sw-crypto for GTK */
 	if (((vif->type == NL80211_IFTYPE_ADHOC) ||
-	     (vif->type == NL80211_IFTYPE_MESH_POINT)) &&
-	      !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
+	    (vif->type == NL80211_IFTYPE_MESH_POINT)) &&
+	   !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
 		return -ENOSPC;
 	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 		 "%s hardware based encryption for keyidx: %d, mac: %pM\n",
-		 cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
-		 sta ? sta->addr : bcast_addr);
+		  cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
+		  sta ? sta->addr : bcast_addr);
 	rtlpriv->sec.being_setkey = true;
 	rtl_ips_nic_on(hw);
 	mutex_lock(&rtlpriv->locks.conf_mutex);
@@ -1204,21 +1479,23 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "alg:CCMP\n");
 		break;
 	case WLAN_CIPHER_SUITE_AES_CMAC:
-		/*HW doesn't support CMAC encryption, use software CMAC */
+		/* HW don't support CMAC encryption,
+		 * use software CMAC encryption
+		 */
 		key_type = AESCMAC_ENCRYPTION;
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "alg:CMAC\n");
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
-			 "HW don't support CMAC encryption, use software CMAC\n");
+			 "HW don't support CMAC encrypiton, use software CMAC encrypiton\n");
 		err = -EOPNOTSUPP;
 		goto out_unlock;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "alg_err:%x!!!!\n",
-			 key->cipher);
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "alg_err:%x!!!!:\n", key->cipher);
 		goto out_unlock;
 	}
 	if (key_type == WEP40_ENCRYPTION ||
-			key_type == WEP104_ENCRYPTION ||
-			mac->opmode == NL80211_IFTYPE_ADHOC)
+	   key_type == WEP104_ENCRYPTION ||
+	   vif->type == NL80211_IFTYPE_ADHOC)
 		rtlpriv->sec.use_defaultkey = true;
 
 	/* <2> get key_idx */
@@ -1232,14 +1509,14 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	 * 1) wep only: is just for wep enc, in this condition
 	 * rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION
 	 * will be true & enable_hw_sec will be set when wep
-	 * key setting.
+	 * ke setting.
 	 * 2) wep(group) + AES(pairwise): some AP like cisco
 	 * may use it, in this condition enable_hw_sec will not
 	 * be set when wep key setting */
 	/* we must reset sec_info after lingked before set key,
 	 * or some flag will be wrong*/
 	if (vif->type == NL80211_IFTYPE_AP ||
-	    vif->type == NL80211_IFTYPE_MESH_POINT) {
+		vif->type == NL80211_IFTYPE_MESH_POINT) {
 		if (!group_key || key_type == WEP40_ENCRYPTION ||
 			key_type == WEP104_ENCRYPTION) {
 			if (group_key)
@@ -1247,11 +1524,11 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			rtlpriv->cfg->ops->enable_hw_sec(hw);
 		}
 	} else {
-		if ((!group_key) || (mac->opmode == NL80211_IFTYPE_ADHOC) ||
-		     rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION) {
+		if ((!group_key) || (vif->type == NL80211_IFTYPE_ADHOC) ||
+		    rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION) {
 			if (rtlpriv->sec.pairwise_enc_algorithm ==
 			    NO_ENCRYPTION &&
-			    (key_type == WEP40_ENCRYPTION ||
+			   (key_type == WEP40_ENCRYPTION ||
 			    key_type == WEP104_ENCRYPTION))
 				wep_only = true;
 			rtlpriv->sec.pairwise_enc_algorithm = key_type;
@@ -1323,7 +1600,7 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 			 "disable key delete one entry\n");
 		/*set local buf about wep key. */
 		if (vif->type == NL80211_IFTYPE_AP ||
-		    vif->type == NL80211_IFTYPE_MESH_POINT) {
+			vif->type == NL80211_IFTYPE_MESH_POINT) {
 			if (sta)
 				rtl_cam_del_entry(hw, sta->addr);
 		}
@@ -1336,13 +1613,10 @@ static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		 *or clear all entry here.
 		 */
 		rtl_cam_delete_one_entry(hw, mac_addr, key_idx);
-
-		rtl_cam_reset_sec_info(hw);
-
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "cmd_err:%x!!!!\n", cmd);
+			 "cmd_err:%x!!!!:\n", cmd);
 	}
 out_unlock:
 	mutex_unlock(&rtlpriv->locks.conf_mutex);
@@ -1372,7 +1646,7 @@ static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
 
 			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 				 "wireless radio switch turned %s\n",
-				 radio_state ? "on" : "off");
+				  radio_state ? "on" : "off");
 
 			blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
 			wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
@@ -1383,18 +1657,148 @@ static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
 }
 
 /* this function is called by mac80211 to flush tx buffer
- * before switch channel or power save, or tx buffer packet
+ * before switch channle or power save, or tx buffer packet
  * maybe send after offchannel or rf sleep, this may cause
  * dis-association by AP */
-static void rtl_op_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-			 u32 queues, bool drop)
+static void rtl_op_flush(struct ieee80211_hw *hw,
+			 struct ieee80211_vif *vif,
+			 u32 queues,
+			 bool drop)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	if (rtlpriv->intf_ops->flush)
-		rtlpriv->intf_ops->flush(hw, drop);
+		rtlpriv->intf_ops->flush(hw, queues, drop);
 }
 
+/*	Description:
+ *		This routine deals with the Power Configuration CMD
+ *		 parsing for RTL8723/RTL8188E Series IC.
+ *	Assumption:
+ *		We should follow specific format that was released from HW SD.
+ */
+bool rtl_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
+			      u8 faversion, u8 interface_type,
+			      struct wlan_pwr_cfg pwrcfgcmd[])
+{
+	struct wlan_pwr_cfg cfg_cmd = {0};
+	bool polling_bit = false;
+	u32 ary_idx = 0;
+	u8 value = 0;
+	u32 offset = 0;
+	u32 polling_count = 0;
+	u32 max_polling_cnt = 5000;
+
+	do {
+		cfg_cmd = pwrcfgcmd[ary_idx];
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "rtl_hal_pwrseqcmdparsing(): offset(%#x),cut_msk(%#x), famsk(%#x), interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
+			 GET_PWR_CFG_OFFSET(cfg_cmd),
+					    GET_PWR_CFG_CUT_MASK(cfg_cmd),
+			 GET_PWR_CFG_FAB_MASK(cfg_cmd),
+					      GET_PWR_CFG_INTF_MASK(cfg_cmd),
+			 GET_PWR_CFG_BASE(cfg_cmd), GET_PWR_CFG_CMD(cfg_cmd),
+			 GET_PWR_CFG_MASK(cfg_cmd), GET_PWR_CFG_VALUE(cfg_cmd));
+
+		if ((GET_PWR_CFG_FAB_MASK(cfg_cmd)&faversion) &&
+		    (GET_PWR_CFG_CUT_MASK(cfg_cmd)&cut_version) &&
+		    (GET_PWR_CFG_INTF_MASK(cfg_cmd)&interface_type)) {
+			switch (GET_PWR_CFG_CMD(cfg_cmd)) {
+			case PWR_CMD_READ:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_READ\n");
+				break;
+			case PWR_CMD_WRITE:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n");
+				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
+
+				/*Read the value from system register*/
+				value = rtl_read_byte(rtlpriv, offset);
+				value &= (~(GET_PWR_CFG_MASK(cfg_cmd)));
+				value |= (GET_PWR_CFG_VALUE(cfg_cmd) &
+					  GET_PWR_CFG_MASK(cfg_cmd));
+
+				/*Write the value back to sytem register*/
+				rtl_write_byte(rtlpriv, offset, value);
+				break;
+			case PWR_CMD_POLLING:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n");
+				polling_bit = false;
+				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
+
+				do {
+					value = rtl_read_byte(rtlpriv, offset);
+
+					value &= GET_PWR_CFG_MASK(cfg_cmd);
+					if (value ==
+					    (GET_PWR_CFG_VALUE(cfg_cmd) &
+					     GET_PWR_CFG_MASK(cfg_cmd)))
+						polling_bit = true;
+					else
+						udelay(10);
+
+					if (polling_count++ > max_polling_cnt)
+						return false;
+				} while (!polling_bit);
+				break;
+			case PWR_CMD_DELAY:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n");
+				if (GET_PWR_CFG_VALUE(cfg_cmd) ==
+				    PWRSEQ_DELAY_US)
+					udelay(GET_PWR_CFG_OFFSET(cfg_cmd));
+				else
+					mdelay(GET_PWR_CFG_OFFSET(cfg_cmd));
+				break;
+			case PWR_CMD_END:
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_END\n");
+				return true;
+			default:
+				RT_ASSERT(false,
+					  "rtl_hal_pwrseqcmdparsing(): Unknown CMD!!\n");
+				break;
+			}
+		}
+		ary_idx++;
+	} while (1);
+
+	return true;
+}
+EXPORT_SYMBOL(rtl_hal_pwrseqcmdparsing);
+
+bool rtl_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	struct rtl_tx_desc *pdesc;
+	unsigned long flags;
+	struct sk_buff *pskb = NULL;
+
+	ring = &rtlpci->tx_ring[BEACON_QUEUE];
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	pskb = __skb_dequeue(&ring->queue);
+	if (pskb)
+		kfree_skb(pskb);
+
+	/*this is wrong, fill_tx_cmddesc needs update*/
+	pdesc = &ring->desc[0];
+
+	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
+
+	__skb_queue_tail(&ring->queue, skb);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+
+	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
+
+	return true;
+}
+EXPORT_SYMBOL(rtl_cmd_send_packet);
 const struct ieee80211_ops rtl_ops = {
 	.start = rtl_op_start,
 	.stop = rtl_op_stop,
@@ -1402,10 +1806,12 @@ const struct ieee80211_ops rtl_ops = {
 	.add_interface = rtl_op_add_interface,
 	.remove_interface = rtl_op_remove_interface,
 	.change_interface = rtl_op_change_interface,
+#ifdef CONFIG_PM
+	.suspend = rtl_op_suspend,
+	.resume = rtl_op_resume,
+#endif
 	.config = rtl_op_config,
 	.configure_filter = rtl_op_configure_filter,
-	.sta_add = rtl_op_sta_add,
-	.sta_remove = rtl_op_sta_remove,
 	.set_key = rtl_op_set_key,
 	.conf_tx = rtl_op_conf_tx,
 	.bss_info_changed = rtl_op_bss_info_changed,
@@ -1417,6 +1823,8 @@ const struct ieee80211_ops rtl_ops = {
 	.sw_scan_start = rtl_op_sw_scan_start,
 	.sw_scan_complete = rtl_op_sw_scan_complete,
 	.rfkill_poll = rtl_op_rfkill_poll,
+	.sta_add = rtl_op_sta_add,
+	.sta_remove = rtl_op_sta_remove,
 	.flush = rtl_op_flush,
 };
 EXPORT_SYMBOL_GPL(rtl_ops);
diff --git a/drivers/net/wireless/rtlwifi/core.h b/drivers/net/wireless/rtlwifi/core.h
index 027e75374dcc..59cd3b9dca25 100644
--- a/drivers/net/wireless/rtlwifi/core.h
+++ b/drivers/net/wireless/rtlwifi/core.h
@@ -2,20 +2,16 @@
  *
  * Copyright(c) 2009-2012  Realtek Corporation.
  *
- * Tmis program is free software; you can redistribute it and/or modify it
+ * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
  * published by the Free Software Foundation.
  *
- * Tmis program is distributed in the hope that it will be useful, but WITHOUT
+ * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * tmis program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * Tme full GNU General Public License is included in this distribution in the
+ * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
  * Contact Information:
@@ -45,5 +41,6 @@ void rtl_addr_delay(u32 addr);
 void rtl_rfreg_delay(struct ieee80211_hw *hw, enum radio_path rfpath, u32 addr,
 		     u32 mask, u32 data);
 void rtl_bb_delay(struct ieee80211_hw *hw, u32 addr, u32 data);
+bool rtl_cmd_send_packet(struct ieee80211_hw *hw, struct sk_buff *skb);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/debug.c b/drivers/net/wireless/rtlwifi/debug.c
index 76e2086e137e..fd25abad2b9e 100644
--- a/drivers/net/wireless/rtlwifi/debug.c
+++ b/drivers/net/wireless/rtlwifi/debug.c
@@ -2,20 +2,16 @@
  *
  * Copyright(c) 2009-2012  Realtek Corporation.
  *
- * Tmis program is free software; you can redistribute it and/or modify it
+ * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
  * published by the Free Software Foundation.
  *
- * Tmis program is distributed in the hope that it will be useful, but WITHOUT
+ * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * tmis program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * Tme full GNU General Public License is included in this distribution in the
+ * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
  * Contact Information:
diff --git a/drivers/net/wireless/rtlwifi/debug.h b/drivers/net/wireless/rtlwifi/debug.h
index 6d669364e3d9..fc794b3e9f4a 100644
--- a/drivers/net/wireless/rtlwifi/debug.h
+++ b/drivers/net/wireless/rtlwifi/debug.h
@@ -2,20 +2,16 @@
  *
  * Copyright(c) 2009-2012  Realtek Corporation.
  *
- * Tmis program is free software; you can redistribute it and/or modify it
+ * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
  * published by the Free Software Foundation.
  *
- * Tmis program is distributed in the hope that it will be useful, but WITHOUT
+ * This program is distributed in the hope that it will be useful, but WITHOUT
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * tmis program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * Tme full GNU General Public License is included in this distribution in the
+ * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
  * Contact Information:
@@ -108,6 +104,7 @@
 #define COMP_USB			BIT(29)
 #define COMP_EASY_CONCURRENT	COMP_USB /* reuse of this bit is OK */
 #define COMP_BT_COEXIST			BIT(30)
+#define COMP_IQK			BIT(31)
 
 /*--------------------------------------------------------------
 		Define the rt_print components
diff --git a/drivers/net/wireless/rtlwifi/efuse.c b/drivers/net/wireless/rtlwifi/efuse.c
index 2ffc7298f686..0b4082c9272a 100644
--- a/drivers/net/wireless/rtlwifi/efuse.c
+++ b/drivers/net/wireless/rtlwifi/efuse.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * tmis program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * Tme full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -26,10 +22,9 @@
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
  *****************************************************************************/
-
-#include <linux/export.h>
 #include "wifi.h"
 #include "efuse.h"
+#include <linux/export.h>
 
 static const u8 MAX_PGPKT_SIZE = 9;
 static const u8 PGPKT_DATA_SIZE = 8;
@@ -63,21 +58,19 @@ static void efuse_shadow_write_2byte(struct ieee80211_hw *hw, u16 offset,
 				     u16 value);
 static void efuse_shadow_write_4byte(struct ieee80211_hw *hw, u16 offset,
 				     u32 value);
-static int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr,
-					u8 *data);
 static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr,
-					u8 data);
+				u8 data);
 static void efuse_read_all_map(struct ieee80211_hw *hw, u8 *efuse);
 static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset,
-					u8 *data);
+				u8 *data);
 static int efuse_pg_packet_write(struct ieee80211_hw *hw, u8 offset,
 				 u8 word_en, u8 *data);
 static void efuse_word_enable_data_read(u8 word_en, u8 *sourdata,
 					u8 *targetdata);
-static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
-				       u16 efuse_addr, u8 word_en, u8 *data);
+static u8 enable_efuse_data_write(struct ieee80211_hw *hw,
+				  u16 efuse_addr, u8 word_en, u8 *data);
 static void efuse_power_switch(struct ieee80211_hw *hw, u8 write,
-					u8 pwrstate);
+			       u8 pwrstate);
 static u16 efuse_get_current_size(struct ieee80211_hw *hw);
 static u8 efuse_calculate_word_cnts(u8 word_en);
 
@@ -258,7 +251,7 @@ void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf)
 	}
 
 	/* allocate memory for efuse_tbl and efuse_word */
-	efuse_tbl = kmalloc(rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE] *
+	efuse_tbl = kzalloc(rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE] *
 			    sizeof(u8), GFP_ATOMIC);
 	if (!efuse_tbl)
 		return;
@@ -266,7 +259,7 @@ void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf)
 	if (!efuse_word)
 		goto out;
 	for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
-		efuse_word[i] = kmalloc(efuse_max_section * sizeof(u16),
+		efuse_word[i] = kzalloc(efuse_max_section * sizeof(u16),
 					GFP_ATOMIC);
 		if (!efuse_word[i])
 			goto done;
@@ -413,8 +406,7 @@ bool efuse_shadow_update_chk(struct ieee80211_hw *hw)
 	efuse_used = rtlefuse->efuse_usedbytes;
 
 	if ((totalbytes + efuse_used) >=
-	    (EFUSE_MAX_SIZE -
-	     rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))
+	    (EFUSE_MAX_SIZE - rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))
 		result = false;
 
 	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD,
@@ -428,13 +420,14 @@ void efuse_shadow_read(struct ieee80211_hw *hw, u8 type,
 		       u16 offset, u32 *value)
 {
 	if (type == 1)
-		efuse_shadow_read_1byte(hw, offset, (u8 *) value);
+		efuse_shadow_read_1byte(hw, offset, (u8 *)value);
 	else if (type == 2)
-		efuse_shadow_read_2byte(hw, offset, (u16 *) value);
+		efuse_shadow_read_2byte(hw, offset, (u16 *)value);
 	else if (type == 4)
 		efuse_shadow_read_4byte(hw, offset, value);
 
 }
+EXPORT_SYMBOL(efuse_shadow_read);
 
 void efuse_shadow_write(struct ieee80211_hw *hw, u8 type, u16 offset,
 				u32 value)
@@ -456,7 +449,7 @@ bool efuse_shadow_update(struct ieee80211_hw *hw)
 	u8 word_en = 0x0F;
 	u8 first_pg = false;
 
-	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD, "--->\n");
+	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD, "\n");
 
 	if (!efuse_shadow_update_chk(hw)) {
 		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
@@ -465,7 +458,7 @@ bool efuse_shadow_update(struct ieee80211_hw *hw)
 		       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
 
 		RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD,
-			 "<---efuse out of capacity!!\n");
+			 "efuse out of capacity!!\n");
 		return false;
 	}
 	efuse_power_switch(hw, true, true);
@@ -477,7 +470,6 @@ bool efuse_shadow_update(struct ieee80211_hw *hw)
 
 		for (i = 0; i < 8; i++) {
 			if (first_pg) {
-
 				word_en &= ~(BIT(i / 2));
 
 				rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
@@ -500,7 +492,7 @@ bool efuse_shadow_update(struct ieee80211_hw *hw)
 			       &rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base],
 			       8);
 			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD,
-				      "U-efuse", tmpdata, 8);
+				      "U-efuse\n", tmpdata, 8);
 
 			if (!efuse_pg_packet_write(hw, (u8) offset, word_en,
 						   tmpdata)) {
@@ -519,7 +511,7 @@ bool efuse_shadow_update(struct ieee80211_hw *hw)
 	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
 	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
 
-	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD, "<---\n");
+	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD, "\n");
 	return true;
 }
 
@@ -529,14 +521,14 @@ void rtl_efuse_shadow_map_update(struct ieee80211_hw *hw)
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 
 	if (rtlefuse->autoload_failflag)
-		memset(&rtlefuse->efuse_map[EFUSE_INIT_MAP][0], 0xFF,
-			rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+		memset((&rtlefuse->efuse_map[EFUSE_INIT_MAP][0]),
+		       0xFF, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
 	else
 		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
 
 	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
-	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
+			&rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+			rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
 
 }
 EXPORT_SYMBOL(rtl_efuse_shadow_map_update);
@@ -619,7 +611,7 @@ static void efuse_shadow_write_4byte(struct ieee80211_hw *hw,
 
 }
 
-static int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data)
+int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmpidx = 0;
@@ -650,14 +642,15 @@ static int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data)
 	}
 	return result;
 }
+EXPORT_SYMBOL(efuse_one_byte_read);
 
 static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr, u8 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmpidx = 0;
 
-	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD, "Addr = %x Data=%x\n",
-		 addr, data);
+	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD,
+		 "Addr = %x Data=%x\n", addr, data);
 
 	rtl_write_byte(rtlpriv,
 		       rtlpriv->cfg->maps[EFUSE_CTRL] + 1, (u8) (addr & 0xff));
@@ -677,11 +670,10 @@ static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr, u8 data)
 
 	if (tmpidx < 100)
 		return true;
-
 	return false;
 }
 
-static void efuse_read_all_map(struct ieee80211_hw *hw, u8 * efuse)
+static void efuse_read_all_map(struct ieee80211_hw *hw, u8 *efuse)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	efuse_power_switch(hw, false, true);
@@ -706,14 +698,14 @@ static void efuse_read_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
 	if (hoffset == offset) {
 		for (tmpidx = 0; tmpidx < word_cnts * 2; tmpidx++) {
 			if (efuse_one_byte_read(hw, *efuse_addr + 1 + tmpidx,
-			    &efuse_data)) {
+						&efuse_data)) {
 				tmpdata[tmpidx] = efuse_data;
 				if (efuse_data != 0xff)
-					dataempty = true;
+					dataempty = false;
 			}
 		}
 
-		if (dataempty) {
+		if (!dataempty) {
 			*readstate = PG_STATE_DATA;
 		} else {
 			*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
@@ -729,7 +721,9 @@ static void efuse_read_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
 static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset, u8 *data)
 {
 	u8 readstate = PG_STATE_HEADER;
+
 	bool continual = true;
+
 	u8 efuse_data, word_cnts = 0;
 	u16 efuse_addr = 0;
 	u8 tmpdata[8];
@@ -747,9 +741,8 @@ static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset, u8 *data)
 			if (efuse_one_byte_read(hw, efuse_addr, &efuse_data)
 			    && (efuse_data != 0xFF))
 				efuse_read_data_case1(hw, &efuse_addr,
-						      efuse_data,
-						      offset, tmpdata,
-						      &readstate);
+						      efuse_data, offset,
+						      tmpdata, &readstate);
 			else
 				continual = false;
 		} else if (readstate & PG_STATE_DATA) {
@@ -771,13 +764,14 @@ static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset, u8 *data)
 }
 
 static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
-			u8 efuse_data, u8 offset, int *continual,
-			u8 *write_state, struct pgpkt_struct *target_pkt,
-			int *repeat_times, int *result, u8 word_en)
+				   u8 efuse_data, u8 offset,
+				   int *continual, u8 *write_state,
+				   struct pgpkt_struct *target_pkt,
+				   int *repeat_times, int *result, u8 word_en)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct pgpkt_struct tmp_pkt;
-	bool dataempty = true;
+	int dataempty = true;
 	u8 originaldata[8 * sizeof(u8)];
 	u8 badworden = 0x0F;
 	u8 match_word_en, tmp_word_en;
@@ -794,9 +788,10 @@ static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
 		*write_state = PG_STATE_HEADER;
 	} else {
 		for (tmpindex = 0; tmpindex < (tmp_word_cnts * 2); tmpindex++) {
-			u16 address = *efuse_addr + 1 + tmpindex;
-			if (efuse_one_byte_read(hw, address,
-			     &efuse_data) && (efuse_data != 0xFF))
+			if (efuse_one_byte_read(hw,
+						(*efuse_addr + 1 + tmpindex),
+						&efuse_data) &&
+			    (efuse_data != 0xFF))
 				dataempty = false;
 		}
 
@@ -806,33 +801,34 @@ static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
 		} else {
 			match_word_en = 0x0F;
 			if (!((target_pkt->word_en & BIT(0)) |
-			     (tmp_pkt.word_en & BIT(0))))
+			    (tmp_pkt.word_en & BIT(0))))
 				match_word_en &= (~BIT(0));
 
 			if (!((target_pkt->word_en & BIT(1)) |
-			     (tmp_pkt.word_en & BIT(1))))
+			    (tmp_pkt.word_en & BIT(1))))
 				match_word_en &= (~BIT(1));
 
 			if (!((target_pkt->word_en & BIT(2)) |
-			     (tmp_pkt.word_en & BIT(2))))
+			    (tmp_pkt.word_en & BIT(2))))
 				match_word_en &= (~BIT(2));
 
 			if (!((target_pkt->word_en & BIT(3)) |
-			     (tmp_pkt.word_en & BIT(3))))
+			    (tmp_pkt.word_en & BIT(3))))
 				match_word_en &= (~BIT(3));
 
 			if ((match_word_en & 0x0F) != 0x0F) {
-				badworden = efuse_word_enable_data_write(
-							    hw, *efuse_addr + 1,
-							    tmp_pkt.word_en,
-							    target_pkt->data);
+				badworden =
+				  enable_efuse_data_write(hw,
+							  *efuse_addr + 1,
+							  tmp_pkt.word_en,
+							  target_pkt->data);
 
-				if (0x0F != (badworden & 0x0F)) {
+				if (0x0F != (badworden & 0x0F))	{
 					u8 reorg_offset = offset;
 					u8 reorg_worden = badworden;
 					efuse_pg_packet_write(hw, reorg_offset,
-							       reorg_worden,
-							       originaldata);
+							      reorg_worden,
+							      originaldata);
 				}
 
 				tmp_word_en = 0x0F;
@@ -845,11 +841,11 @@ static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
 					tmp_word_en &= (~BIT(1));
 
 				if ((target_pkt->word_en & BIT(2)) ^
-				     (match_word_en & BIT(2)))
+				    (match_word_en & BIT(2)))
 					tmp_word_en &= (~BIT(2));
 
 				if ((target_pkt->word_en & BIT(3)) ^
-				     (match_word_en & BIT(3)))
+				    (match_word_en & BIT(3)))
 					tmp_word_en &= (~BIT(3));
 
 				if ((tmp_word_en & 0x0F) != 0x0F) {
@@ -873,7 +869,7 @@ static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
 			}
 		}
 	}
-	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,  "efuse PG_STATE_HEADER-1\n");
+	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, "efuse PG_STATE_HEADER-1\n");
 }
 
 static void efuse_write_data_case2(struct ieee80211_hw *hw, u16 *efuse_addr,
@@ -908,12 +904,13 @@ static void efuse_write_data_case2(struct ieee80211_hw *hw, u16 *efuse_addr,
 
 		tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
 
-		memset(originaldata, 0xff, 8 * sizeof(u8));
+		memset(originaldata, 0xff,  8 * sizeof(u8));
 
 		if (efuse_pg_packet_read(hw, tmp_pkt.offset, originaldata)) {
-			badworden = efuse_word_enable_data_write(hw,
-				    *efuse_addr + 1, tmp_pkt.word_en,
-				    originaldata);
+			badworden = enable_efuse_data_write(hw,
+							    *efuse_addr + 1,
+							    tmp_pkt.word_en,
+							    originaldata);
 
 			if (0x0F != (badworden & 0x0F)) {
 				u8 reorg_offset = tmp_pkt.offset;
@@ -923,8 +920,8 @@ static void efuse_write_data_case2(struct ieee80211_hw *hw, u16 *efuse_addr,
 						      originaldata);
 				*efuse_addr = efuse_get_current_size(hw);
 			} else {
-				*efuse_addr = *efuse_addr + (tmp_word_cnts * 2)
-					      + 1;
+				*efuse_addr = *efuse_addr +
+					      (tmp_word_cnts * 2) + 1;
 			}
 		} else {
 			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
@@ -948,7 +945,7 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct pgpkt_struct target_pkt;
 	u8 write_state = PG_STATE_HEADER;
-	int continual = true, result = true;
+	int continual = true, dataempty = true, result = true;
 	u16 efuse_addr = 0;
 	u8 efuse_data;
 	u8 target_word_cnts = 0;
@@ -956,7 +953,7 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 	static int repeat_times;
 
 	if (efuse_get_current_size(hw) >= (EFUSE_MAX_SIZE -
-	    rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
 		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
 			"efuse_pg_packet_write error\n");
 		return false;
@@ -965,17 +962,18 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 	target_pkt.offset = offset;
 	target_pkt.word_en = word_en;
 
-	memset(target_pkt.data, 0xFF, 8 * sizeof(u8));
+	memset(target_pkt.data, 0xFF,  8 * sizeof(u8));
 
 	efuse_word_enable_data_read(word_en, data, target_pkt.data);
 	target_word_cnts = efuse_calculate_word_cnts(target_pkt.word_en);
 
-	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,  "efuse Power ON\n");
+	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, "efuse Power ON\n");
 
 	while (continual && (efuse_addr < (EFUSE_MAX_SIZE -
-	       rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))) {
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))) {
 
 		if (write_state == PG_STATE_HEADER) {
+			dataempty = true;
 			badworden = 0x0F;
 			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
 				"efuse PG_STATE_HEADER\n");
@@ -985,7 +983,8 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 				efuse_write_data_case1(hw, &efuse_addr,
 						       efuse_data, offset,
 						       &continual,
-						       &write_state, &target_pkt,
+						       &write_state,
+						       &target_pkt,
 						       &repeat_times, &result,
 						       word_en);
 			else
@@ -999,15 +998,17 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 		} else if (write_state == PG_STATE_DATA) {
 			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
 				"efuse PG_STATE_DATA\n");
+			badworden = 0x0f;
 			badworden =
-			    efuse_word_enable_data_write(hw, efuse_addr + 1,
-							 target_pkt.word_en,
-							 target_pkt.data);
+			    enable_efuse_data_write(hw, efuse_addr + 1,
+						    target_pkt.word_en,
+						    target_pkt.data);
 
 			if ((badworden & 0x0F) == 0x0F) {
 				continual = false;
 			} else {
-				efuse_addr += (2 * target_word_cnts) + 1;
+				efuse_addr =
+				    efuse_addr + (2 * target_word_cnts) + 1;
 
 				target_pkt.offset = offset;
 				target_pkt.word_en = badworden;
@@ -1027,7 +1028,7 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 	}
 
 	if (efuse_addr >= (EFUSE_MAX_SIZE -
-	    rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
+		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
 		RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD,
 			 "efuse_addr(%#x) Out of size!!\n", efuse_addr);
 	}
@@ -1035,8 +1036,8 @@ static int efuse_pg_packet_write(struct ieee80211_hw *hw,
 	return true;
 }
 
-static void efuse_word_enable_data_read(u8 word_en,
-					u8 *sourdata, u8 *targetdata)
+static void efuse_word_enable_data_read(u8 word_en, u8 *sourdata,
+					u8 *targetdata)
 {
 	if (!(word_en & BIT(0))) {
 		targetdata[0] = sourdata[0];
@@ -1059,8 +1060,8 @@ static void efuse_word_enable_data_read(u8 word_en,
 	}
 }
 
-static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
-				       u16 efuse_addr, u8 word_en, u8 *data)
+static u8 enable_efuse_data_write(struct ieee80211_hw *hw,
+				  u16 efuse_addr, u8 word_en, u8 *data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 tmpaddr;
@@ -1069,8 +1070,8 @@ static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
 	u8 tmpdata[8];
 
 	memset(tmpdata, 0xff, PGPKT_DATA_SIZE);
-	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD, "word_en = %x efuse_addr=%x\n",
-		 word_en, efuse_addr);
+	RT_TRACE(rtlpriv, COMP_EFUSE, DBG_LOUD,
+		 "word_en = %x efuse_addr=%x\n", word_en, efuse_addr);
 
 	if (!(word_en & BIT(0))) {
 		tmpaddr = start_addr;
@@ -1127,19 +1128,22 @@ static void efuse_power_switch(struct ieee80211_hw *hw, u8 write, u8 pwrstate)
 	u16 tmpV16;
 
 	if (pwrstate && (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)) {
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
-			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS],
-				       0x69);
 
-		tmpV16 = rtl_read_word(rtlpriv,
-				       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
-		if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_PWC_EV12V])) {
-			tmpV16 |= rtlpriv->cfg->maps[EFUSE_PWC_EV12V];
-			rtl_write_word(rtlpriv,
-				       rtlpriv->cfg->maps[SYS_ISO_CTRL],
-				       tmpV16);
+		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192CE &&
+		    rtlhal->hw_type != HARDWARE_TYPE_RTL8192DE) {
+			rtl_write_byte(rtlpriv,
+				       rtlpriv->cfg->maps[EFUSE_ACCESS], 0x69);
+		} else {
+			tmpV16 =
+			  rtl_read_word(rtlpriv,
+					rtlpriv->cfg->maps[SYS_ISO_CTRL]);
+			if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_PWC_EV12V])) {
+				tmpV16 |= rtlpriv->cfg->maps[EFUSE_PWC_EV12V];
+				rtl_write_word(rtlpriv,
+					       rtlpriv->cfg->maps[SYS_ISO_CTRL],
+					       tmpV16);
+			}
 		}
-
 		tmpV16 = rtl_read_word(rtlpriv,
 				       rtlpriv->cfg->maps[SYS_FUNC_EN]);
 		if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_FEN_ELDR])) {
@@ -1164,7 +1168,10 @@ static void efuse_power_switch(struct ieee80211_hw *hw, u8 write, u8 pwrstate)
 						rtlpriv->cfg->maps[EFUSE_TEST] +
 						3);
 
-			if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE) {
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+				tempval &= ~(BIT(3) | BIT(4) | BIT(5) | BIT(6));
+				tempval |= (VOLTAGE_V25 << 3);
+			} else if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE) {
 				tempval &= 0x0F;
 				tempval |= (VOLTAGE_V25 << 4);
 			}
@@ -1176,11 +1183,11 @@ static void efuse_power_switch(struct ieee80211_hw *hw, u8 write, u8 pwrstate)
 
 		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
 			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
-						0x03);
+				       0x03);
 		}
-
 	} else {
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
+		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192CE &&
+		    rtlhal->hw_type != HARDWARE_TYPE_RTL8192DE)
 			rtl_write_byte(rtlpriv,
 				       rtlpriv->cfg->maps[EFUSE_ACCESS], 0);
 
@@ -1195,27 +1202,28 @@ static void efuse_power_switch(struct ieee80211_hw *hw, u8 write, u8 pwrstate)
 
 		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
 			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
-						0x02);
+				       0x02);
 		}
-
 	}
-
 }
 
 static u16 efuse_get_current_size(struct ieee80211_hw *hw)
 {
+	int continual = true;
 	u16 efuse_addr = 0;
-	u8 hworden;
+	u8 hoffset, hworden;
 	u8 efuse_data, word_cnts;
 
-	while (efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
-	       efuse_addr < EFUSE_MAX_SIZE) {
-		if (efuse_data == 0xFF)
-			break;
-
-		hworden = efuse_data & 0x0F;
-		word_cnts = efuse_calculate_word_cnts(hworden);
-		efuse_addr = efuse_addr + (word_cnts * 2) + 1;
+	while (continual && efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
+	       (efuse_addr < EFUSE_MAX_SIZE)) {
+		if (efuse_data != 0xFF) {
+			hoffset = (efuse_data >> 4) & 0x0F;
+			hworden = efuse_data & 0x0F;
+			word_cnts = efuse_calculate_word_cnts(hworden);
+			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
+		} else {
+			continual = false;
+		}
 	}
 
 	return efuse_addr;
diff --git a/drivers/net/wireless/rtlwifi/efuse.h b/drivers/net/wireless/rtlwifi/efuse.h
index 1663b3afd41e..fdab8240a5d7 100644
--- a/drivers/net/wireless/rtlwifi/efuse.h
+++ b/drivers/net/wireless/rtlwifi/efuse.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -32,7 +28,6 @@
 
 #define EFUSE_IC_ID_OFFSET		506
 
-#define EFUSE_MAP_LEN			128
 #define EFUSE_MAX_WORD_UNIT		4
 
 #define EFUSE_INIT_MAP			0
@@ -107,12 +102,14 @@ struct efuse_priv {
 void read_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
 void efuse_initialize(struct ieee80211_hw *hw);
 u8 efuse_read_1byte(struct ieee80211_hw *hw, u16 address);
+int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data);
 void efuse_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value);
-void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf);
-void efuse_shadow_read(struct ieee80211_hw *hw, u8 type, u16 offset,
-		       u32 *value);
-void efuse_shadow_write(struct ieee80211_hw *hw, u8 type, u16 offset,
-			u32 value);
+void read_efuse(struct ieee80211_hw *hw, u16 _offset,
+		u16 _size_byte, u8 *pbuf);
+void efuse_shadow_read(struct ieee80211_hw *hw, u8 type,
+		       u16 offset, u32 *value);
+void efuse_shadow_write(struct ieee80211_hw *hw, u8 type,
+			u16 offset, u32 value);
 bool efuse_shadow_update(struct ieee80211_hw *hw);
 bool efuse_shadow_update_chk(struct ieee80211_hw *hw);
 void rtl_efuse_shadow_map_update(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/pci.c b/drivers/net/wireless/rtlwifi/pci.c
index 67d1ee6edcad..667aba81246c 100644
--- a/drivers/net/wireless/rtlwifi/pci.c
+++ b/drivers/net/wireless/rtlwifi/pci.c
@@ -33,6 +33,7 @@
 #include "base.h"
 #include "ps.h"
 #include "efuse.h"
+#include <linux/interrupt.h>
 #include <linux/export.h>
 #include <linux/kmemleak.h>
 #include <linux/module.h>
@@ -44,10 +45,10 @@ MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("PCI basic driver for rtlwifi");
 
 static const u16 pcibridge_vendors[PCI_BRIDGE_VENDOR_MAX] = {
-	PCI_VENDOR_ID_INTEL,
-	PCI_VENDOR_ID_ATI,
-	PCI_VENDOR_ID_AMD,
-	PCI_VENDOR_ID_SI
+	INTEL_VENDOR_ID,
+	ATI_VENDOR_ID,
+	AMD_VENDOR_ID,
+	SIS_VENDOR_ID
 };
 
 static const u8 ac_to_hwq[] = {
@@ -566,27 +567,25 @@ static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
 	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
 
 	while (skb_queue_len(&ring->queue)) {
-		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
 		struct sk_buff *skb;
 		struct ieee80211_tx_info *info;
 		__le16 fc;
 		u8 tid;
+		u8 *entry;
 
-		u8 own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) entry, true,
-							  HW_DESC_OWN);
+		if (rtlpriv->use_new_trx_flow)
+			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
+		else
+			entry = (u8 *)(&ring->desc[ring->idx]);
 
-		/*beacon packet will only use the first
-		 *descriptor by defaut, and the own may not
-		 *be cleared by the hardware
-		 */
-		if (own)
+		if (!rtlpriv->cfg->ops->is_tx_desc_closed(hw, prio, ring->idx))
 			return;
 		ring->idx = (ring->idx + 1) % ring->entries;
 
 		skb = __skb_dequeue(&ring->queue);
 		pci_unmap_single(rtlpci->pdev,
 				 rtlpriv->cfg->ops->
-					     get_desc((u8 *) entry, true,
+					     get_desc((u8 *)entry, true,
 						      HW_DESC_TXBUFF_ADDR),
 				 skb->len, PCI_DMA_TODEVICE);
 
@@ -598,7 +597,7 @@ static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
 			 "new ring->idx:%d, free: skb_queue_len:%d, free: seq:%x\n",
 			 ring->idx,
 			 skb_queue_len(&ring->queue),
-			 *(u16 *) (skb->data + 22));
+			 *(u16 *)(skb->data + 22));
 
 		if (prio == TXCMD_QUEUE) {
 			dev_kfree_skb(skb);
@@ -646,7 +645,7 @@ static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
 				== 2) {
 
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
-				 "more desc left, wake skb_queue@%d, ring->idx = %d, skb_queue_len = 0x%d\n",
+				 "more desc left, wake skb_queue@%d, ring->idx = %d, skb_queue_len = 0x%x\n",
 				 prio, ring->idx,
 				 skb_queue_len(&ring->queue));
 
@@ -666,175 +665,276 @@ tx_status_ok:
 	}
 }
 
-static void _rtl_receive_one(struct ieee80211_hw *hw, struct sk_buff *skb,
-			     struct ieee80211_rx_status rx_status)
+static int _rtl_pci_init_one_rxdesc(struct ieee80211_hw *hw,
+				    u8 *entry, int rxring_idx, int desc_idx)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	__le16 fc = rtl_get_fc(skb);
-	bool unicast = false;
-	struct sk_buff *uskb = NULL;
-	u8 *pdata;
-
-
-	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
-
-	if (is_broadcast_ether_addr(hdr->addr1)) {
-		;/*TODO*/
-	} else if (is_multicast_ether_addr(hdr->addr1)) {
-		;/*TODO*/
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 bufferaddress;
+	u8 tmp_one = 1;
+	struct sk_buff *skb;
+
+	skb = dev_alloc_skb(rtlpci->rxbuffersize);
+	if (!skb)
+		return 0;
+	rtlpci->rx_ring[rxring_idx].rx_buf[desc_idx] = skb;
+
+	/* just set skb->cb to mapping addr for pci_unmap_single use */
+	*((dma_addr_t *)skb->cb) =
+		pci_map_single(rtlpci->pdev, skb_tail_pointer(skb),
+			       rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
+	bufferaddress = *((dma_addr_t *)skb->cb);
+	if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress))
+		return 0;
+	if (rtlpriv->use_new_trx_flow) {
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RX_PREPARE,
+					    (u8 *)&bufferaddress);
 	} else {
-		unicast = true;
-		rtlpriv->stats.rxbytesunicast += skb->len;
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXBUFF_ADDR,
+					    (u8 *)&bufferaddress);
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXPKT_LEN,
+					    (u8 *)&rtlpci->rxbuffersize);
+		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXOWN,
+					    (u8 *)&tmp_one);
 	}
+	return 1;
+}
 
-	if (ieee80211_is_data(fc)) {
-		rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
-
-		if (unicast)
-			rtlpriv->link_info.num_rx_inperiod++;
+/* inorder to receive 8K AMSDU we have set skb to
+ * 9100bytes in init rx ring, but if this packet is
+ * not a AMSDU, this large packet will be sent to
+ * TCP/IP directly, this cause big packet ping fail
+ * like: "ping -s 65507", so here we will realloc skb
+ * based on the true size of packet, Mac80211
+ * Probably will do it better, but does not yet.
+ *
+ * Some platform will fail when alloc skb sometimes.
+ * in this condition, we will send the old skb to
+ * mac80211 directly, this will not cause any other
+ * issues, but only this packet will be lost by TCP/IP
+ */
+static void _rtl_pci_rx_to_mac80211(struct ieee80211_hw *hw,
+				    struct sk_buff *skb,
+				    struct ieee80211_rx_status rx_status)
+{
+	if (unlikely(!rtl_action_proc(hw, skb, false))) {
+		dev_kfree_skb_any(skb);
+	} else {
+		struct sk_buff *uskb = NULL;
+		u8 *pdata;
+
+		uskb = dev_alloc_skb(skb->len + 128);
+		if (likely(uskb)) {
+			memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
+			       sizeof(rx_status));
+			pdata = (u8 *)skb_put(uskb, skb->len);
+			memcpy(pdata, skb->data, skb->len);
+			dev_kfree_skb_any(skb);
+			ieee80211_rx_irqsafe(hw, uskb);
+		} else {
+			ieee80211_rx_irqsafe(hw, skb);
+		}
 	}
+}
 
-	/* static bcn for roaming */
-	rtl_beacon_statistic(hw, skb);
-	rtl_p2p_info(hw, (void *)skb->data, skb->len);
-
-	/* for sw lps */
-	rtl_swlps_beacon(hw, (void *)skb->data, skb->len);
-	rtl_recognize_peer(hw, (void *)skb->data, skb->len);
-	if ((rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) &&
-	    (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G) &&
-	     (ieee80211_is_beacon(fc) || ieee80211_is_probe_resp(fc)))
-		return;
-
-	if (unlikely(!rtl_action_proc(hw, skb, false)))
-		return;
-
-	uskb = dev_alloc_skb(skb->len + 128);
-	if (!uskb)
-		return;		/* exit if allocation failed */
-	memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status, sizeof(rx_status));
-	pdata = (u8 *)skb_put(uskb, skb->len);
-	memcpy(pdata, skb->data, skb->len);
+/*hsisr interrupt handler*/
+static void _rtl_pci_hs_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	ieee80211_rx_irqsafe(hw, uskb);
+	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR],
+		       rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR]) |
+		       rtlpci->sys_irq_mask);
 }
 
 static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int rx_queue_idx = RTL_PCI_RX_MPDU_QUEUE;
-
+	int rxring_idx = RTL_PCI_RX_MPDU_QUEUE;
 	struct ieee80211_rx_status rx_status = { 0 };
 	unsigned int count = rtlpci->rxringcount;
 	u8 own;
 	u8 tmp_one;
-	u32 bufferaddress;
-
+	bool unicast = false;
+	u8 hw_queue = 0;
+	unsigned int rx_remained_cnt;
 	struct rtl_stats stats = {
 		.signal = 0,
 		.rate = 0,
 	};
-	int index = rtlpci->rx_ring[rx_queue_idx].idx;
 
-	if (rtlpci->driver_is_goingto_unload)
-		return;
 	/*RX NORMAL PKT */
 	while (count--) {
-		/*rx descriptor */
-		struct rtl_rx_desc *pdesc = &rtlpci->rx_ring[rx_queue_idx].desc[
-				index];
+		struct ieee80211_hdr *hdr;
+		__le16 fc;
+		u16 len;
+		/*rx buffer descriptor */
+		struct rtl_rx_buffer_desc *buffer_desc = NULL;
+		/*if use new trx flow, it means wifi info */
+		struct rtl_rx_desc *pdesc = NULL;
 		/*rx pkt */
-		struct sk_buff *skb = rtlpci->rx_ring[rx_queue_idx].rx_buf[
-				index];
-		struct sk_buff *new_skb = NULL;
-
-		own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc,
-						       false, HW_DESC_OWN);
-
-		/*wait data to be filled by hardware */
-		if (own)
-			break;
+		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[
+				      rtlpci->rx_ring[rxring_idx].idx];
+
+		if (rtlpriv->use_new_trx_flow) {
+			rx_remained_cnt =
+				rtlpriv->cfg->ops->rx_desc_buff_remained_cnt(hw,
+								      hw_queue);
+			if (rx_remained_cnt < 1)
+				return;
+
+		} else {	/* rx descriptor */
+			pdesc = &rtlpci->rx_ring[rxring_idx].desc[
+				rtlpci->rx_ring[rxring_idx].idx];
+
+			own = (u8)rtlpriv->cfg->ops->get_desc((u8 *)pdesc,
+							      false,
+							      HW_DESC_OWN);
+			if (own) /* wait data to be filled by hardware */
+				return;
+		}
 
+		/* Reaching this point means: data is filled already
+		 * AAAAAAttention !!!
+		 * We can NOT access 'skb' before 'pci_unmap_single'
+		 */
+		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *)skb->cb),
+				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
+
+		if (rtlpriv->use_new_trx_flow) {
+			buffer_desc =
+			  &rtlpci->rx_ring[rxring_idx].buffer_desc
+				[rtlpci->rx_ring[rxring_idx].idx];
+			/*means rx wifi info*/
+			pdesc = (struct rtl_rx_desc *)skb->data;
+		}
+		memset(&rx_status , 0 , sizeof(rx_status));
 		rtlpriv->cfg->ops->query_rx_desc(hw, &stats,
-						 &rx_status,
-						 (u8 *) pdesc, skb);
+						 &rx_status, (u8 *)pdesc, skb);
 
-		if (stats.crc || stats.hwerror)
-			goto done;
+		if (rtlpriv->use_new_trx_flow)
+			rtlpriv->cfg->ops->rx_check_dma_ok(hw,
+							   (u8 *)buffer_desc,
+							   hw_queue);
 
-		new_skb = dev_alloc_skb(rtlpci->rxbuffersize);
-		if (unlikely(!new_skb)) {
-			RT_TRACE(rtlpriv, (COMP_INTR | COMP_RECV), DBG_DMESG,
-				 "can't alloc skb for rx\n");
-			goto done;
-		}
-		kmemleak_not_leak(new_skb);
+		len = rtlpriv->cfg->ops->get_desc((u8 *)pdesc, false,
+						  HW_DESC_RXPKT_LEN);
 
-		pci_unmap_single(rtlpci->pdev,
-				 *((dma_addr_t *) skb->cb),
-				 rtlpci->rxbuffersize,
-				 PCI_DMA_FROMDEVICE);
+		if (skb->end - skb->tail > len) {
+			skb_put(skb, len);
+			if (rtlpriv->use_new_trx_flow)
+				skb_reserve(skb, stats.rx_drvinfo_size +
+					    stats.rx_bufshift + 24);
+			else
+				skb_reserve(skb, stats.rx_drvinfo_size +
+					    stats.rx_bufshift);
 
-		skb_put(skb, rtlpriv->cfg->ops->get_desc((u8 *) pdesc, false,
-			HW_DESC_RXPKT_LEN));
-		skb_reserve(skb, stats.rx_drvinfo_size + stats.rx_bufshift);
+		} else {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+				 "skb->end - skb->tail = %d, len is %d\n",
+				 skb->end - skb->tail, len);
+			break;
+		}
+		/* handle command packet here */
+		if (rtlpriv->cfg->ops->rx_command_packet(hw, stats, skb)) {
+				dev_kfree_skb_any(skb);
+				goto end;
+		}
 
 		/*
 		 * NOTICE This can not be use for mac80211,
 		 * this is done in mac80211 code,
-		 * if you done here sec DHCP will fail
+		 * if done here sec DHCP will fail
 		 * skb_trim(skb, skb->len - 4);
 		 */
 
-		_rtl_receive_one(hw, skb, rx_status);
+		hdr = rtl_get_hdr(skb);
+		fc = rtl_get_fc(skb);
+
+		if (!stats.crc && !stats.hwerror) {
+			memcpy(IEEE80211_SKB_RXCB(skb), &rx_status,
+			       sizeof(rx_status));
+
+			if (is_broadcast_ether_addr(hdr->addr1)) {
+				;/*TODO*/
+			} else if (is_multicast_ether_addr(hdr->addr1)) {
+				;/*TODO*/
+			} else {
+				unicast = true;
+				rtlpriv->stats.rxbytesunicast += skb->len;
+			}
+			rtl_is_special_data(hw, skb, false);
 
+			if (ieee80211_is_data(fc)) {
+				rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
+				if (unicast)
+					rtlpriv->link_info.num_rx_inperiod++;
+			}
+			/* static bcn for roaming */
+			rtl_beacon_statistic(hw, skb);
+			rtl_p2p_info(hw, (void *)skb->data, skb->len);
+			/* for sw lps */
+			rtl_swlps_beacon(hw, (void *)skb->data, skb->len);
+			rtl_recognize_peer(hw, (void *)skb->data, skb->len);
+			if ((rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) &&
+			    (rtlpriv->rtlhal.current_bandtype ==
+			     BAND_ON_2_4G) &&
+			    (ieee80211_is_beacon(fc) ||
+			     ieee80211_is_probe_resp(fc))) {
+				dev_kfree_skb_any(skb);
+			} else {
+				_rtl_pci_rx_to_mac80211(hw, skb, rx_status);
+			}
+		} else {
+			dev_kfree_skb_any(skb);
+		}
+		if (rtlpriv->use_new_trx_flow) {
+			rtlpci->rx_ring[hw_queue].next_rx_rp += 1;
+			rtlpci->rx_ring[hw_queue].next_rx_rp %=
+					RTL_PCI_MAX_RX_COUNT;
+
+			rx_remained_cnt--;
+			rtl_write_word(rtlpriv, 0x3B4,
+				       rtlpci->rx_ring[hw_queue].next_rx_rp);
+		}
 		if (((rtlpriv->link_info.num_rx_inperiod +
 		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
 		      (rtlpriv->link_info.num_rx_inperiod > 2)) {
 			rtlpriv->enter_ps = false;
 			schedule_work(&rtlpriv->works.lps_change_work);
 		}
+end:
+		if (rtlpriv->use_new_trx_flow) {
+			_rtl_pci_init_one_rxdesc(hw, (u8 *)buffer_desc,
+						 rxring_idx,
+					       rtlpci->rx_ring[rxring_idx].idx);
+		} else {
+			_rtl_pci_init_one_rxdesc(hw, (u8 *)pdesc, rxring_idx,
+						 rtlpci->rx_ring[rxring_idx].idx);
 
-		dev_kfree_skb_any(skb);
-		skb = new_skb;
-
-		rtlpci->rx_ring[rx_queue_idx].rx_buf[index] = skb;
-		*((dma_addr_t *) skb->cb) =
-			    pci_map_single(rtlpci->pdev, skb_tail_pointer(skb),
-					   rtlpci->rxbuffersize,
-					   PCI_DMA_FROMDEVICE);
-
-done:
-		bufferaddress = (*((dma_addr_t *)skb->cb));
-		if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress))
-			return;
-		tmp_one = 1;
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false,
-					    HW_DESC_RXBUFF_ADDR,
-					    (u8 *)&bufferaddress);
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false,
-					    HW_DESC_RXPKT_LEN,
-					    (u8 *)&rtlpci->rxbuffersize);
-
-		if (index == rtlpci->rxringcount - 1)
-			rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false,
-						    HW_DESC_RXERO,
-						    &tmp_one);
-
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, false, HW_DESC_RXOWN,
-					    &tmp_one);
-
-		index = (index + 1) % rtlpci->rxringcount;
+			if (rtlpci->rx_ring[rxring_idx].idx ==
+			    rtlpci->rxringcount - 1)
+				rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc,
+							    false,
+							    HW_DESC_RXERO,
+							    (u8 *)&tmp_one);
+		}
+		rtlpci->rx_ring[rxring_idx].idx =
+				(rtlpci->rx_ring[rxring_idx].idx + 1) %
+				rtlpci->rxringcount;
 	}
-
-	rtlpci->rx_ring[rx_queue_idx].idx = index;
 }
 
 static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 {
 	struct ieee80211_hw *hw = dev_id;
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	unsigned long flags;
@@ -842,16 +942,18 @@ static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 	u32 intb = 0;
 	irqreturn_t ret = IRQ_HANDLED;
 
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	if (rtlpci->irq_enabled == 0)
+		return ret;
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock , flags);
+	rtlpriv->cfg->ops->disable_interrupt(hw);
 
 	/*read ISR: 4/8bytes */
 	rtlpriv->cfg->ops->interrupt_recognized(hw, &inta, &intb);
 
 	/*Shared IRQ or HW disappared */
-	if (!inta || inta == 0xffff) {
-		ret = IRQ_NONE;
+	if (!inta || inta == 0xffff)
 		goto done;
-	}
 
 	/*<1> beacon related */
 	if (inta & rtlpriv->cfg->maps[RTL_IMR_TBDOK]) {
@@ -874,8 +976,8 @@ static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 		tasklet_schedule(&rtlpriv->works.irq_prepare_bcn_tasklet);
 	}
 
-	/*<3> Tx related */
-	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_TXFOVW]))
+	/*<2> Tx related */
+	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_TXFOVW]))
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING, "IMR_TXFOVW!\n");
 
 	if (inta & rtlpriv->cfg->maps[RTL_IMR_MGNTDOK]) {
@@ -932,7 +1034,7 @@ static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 		}
 	}
 
-	/*<2> Rx related */
+	/*<3> Rx related */
 	if (inta & rtlpriv->cfg->maps[RTL_IMR_ROK]) {
 		RT_TRACE(rtlpriv, COMP_INTR, DBG_TRACE, "Rx ok interrupt!\n");
 		_rtl_pci_rx_interrupt(hw);
@@ -944,12 +1046,12 @@ static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 		_rtl_pci_rx_interrupt(hw);
 	}
 
-	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_RXFOVW])) {
+	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_RXFOVW])) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING, "rx overflow !\n");
 		_rtl_pci_rx_interrupt(hw);
 	}
 
-	/*fw related*/
+	/*<4> fw related*/
 	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723AE) {
 		if (inta & rtlpriv->cfg->maps[RTL_IMR_C2HCMD]) {
 			RT_TRACE(rtlpriv, COMP_INTR, DBG_TRACE,
@@ -959,10 +1061,26 @@ static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
 		}
 	}
 
+	/*<5> hsisr related*/
+	/* Only 8188EE & 8723BE Supported.
+	 * If Other ICs Come in, System will corrupt,
+	 * because maps[RTL_IMR_HSISR_IND] & maps[MAC_HSISR]
+	 * are not initialized
+	 */
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE ||
+	    rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
+		if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_HSISR_IND])) {
+			RT_TRACE(rtlpriv, COMP_INTR, DBG_TRACE,
+				 "hsisr interrupt!\n");
+			_rtl_pci_hs_interrupt(hw);
+		}
+	}
+
 	if (rtlpriv->rtlhal.earlymode_enable)
 		tasklet_schedule(&rtlpriv->works.irq_tasklet);
 
 done:
+	rtlpriv->cfg->ops->enable_interrupt(hw);
 	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
 	return ret;
 }
@@ -990,13 +1108,8 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
 	ring = &rtlpci->tx_ring[BEACON_QUEUE];
 	pskb = __skb_dequeue(&ring->queue);
-	if (pskb) {
-		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
-		pci_unmap_single(rtlpci->pdev, rtlpriv->cfg->ops->get_desc(
-				 (u8 *) entry, true, HW_DESC_TXBUFF_ADDR),
-				 pskb->len, PCI_DMA_TODEVICE);
+	if (pskb)
 		kfree_skb(pskb);
-	}
 
 	/*NB: the beacon data buffer must be 32-bit aligned. */
 	pskb = ieee80211_beacon_get(hw, mac->vif);
@@ -1005,7 +1118,10 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 	hdr = rtl_get_hdr(pskb);
 	info = IEEE80211_SKB_CB(pskb);
 	pdesc = &ring->desc[0];
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
+	if (rtlpriv->use_new_trx_flow)
+		pbuffer_desc = &ring->buffer_desc[0];
+
+	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc,
 					(u8 *)pbuffer_desc, info, NULL, pskb,
 					BEACON_QUEUE, &tcb_desc);
 
@@ -1020,10 +1136,18 @@ static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
 static void _rtl_pci_init_trx_var(struct ieee80211_hw *hw)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 i;
+	u16 desc_num;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
+		desc_num = TX_DESC_NUM_92E;
+	else
+		desc_num = RT_TXDESC_NUM;
 
 	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
-		rtlpci->txringcount[i] = RT_TXDESC_NUM;
+		rtlpci->txringcount[i] = desc_num;
 
 	/*
 	 *we just alloc 2 desc for beacon queue,
@@ -1031,12 +1155,12 @@ static void _rtl_pci_init_trx_var(struct ieee80211_hw *hw)
 	 */
 	rtlpci->txringcount[BEACON_QUEUE] = 2;
 
-	/*
-	 *BE queue need more descriptor for performance
+	/*BE queue need more descriptor for performance
 	 *consideration or, No more tx desc will happen,
 	 *and may cause mac80211 mem leakage.
 	 */
-	rtlpci->txringcount[BE_QUEUE] = RT_TXDESC_NUM_BE_QUEUE;
+	if (!rtl_priv(hw)->use_new_trx_flow)
+		rtlpci->txringcount[BE_QUEUE] = RT_TXDESC_NUM_BE_QUEUE;
 
 	rtlpci->rxbuffersize = 9100;	/*2048/1024; */
 	rtlpci->rxringcount = RTL_PCI_MAX_RX_COUNT;	/*64; */
@@ -1087,113 +1211,124 @@ static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tx_desc *ring;
-	dma_addr_t dma;
+	struct rtl_tx_buffer_desc *buffer_desc;
+	struct rtl_tx_desc *desc;
+	dma_addr_t buffer_desc_dma, desc_dma;
 	u32 nextdescaddress;
 	int i;
 
-	ring = pci_zalloc_consistent(rtlpci->pdev, sizeof(*ring) * entries,
-				     &dma);
-	if (!ring || (unsigned long)ring & 0xFF) {
+	/* alloc tx buffer desc for new trx flow*/
+	if (rtlpriv->use_new_trx_flow) {
+		buffer_desc =
+		   pci_zalloc_consistent(rtlpci->pdev,
+					 sizeof(*buffer_desc) * entries,
+					 &buffer_desc_dma);
+
+		if (!buffer_desc || (unsigned long)buffer_desc & 0xFF) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Cannot allocate TX ring (prio = %d)\n",
+				 prio);
+			return -ENOMEM;
+		}
+
+		rtlpci->tx_ring[prio].buffer_desc = buffer_desc;
+		rtlpci->tx_ring[prio].buffer_desc_dma = buffer_desc_dma;
+
+		rtlpci->tx_ring[prio].cur_tx_rp = 0;
+		rtlpci->tx_ring[prio].cur_tx_wp = 0;
+		rtlpci->tx_ring[prio].avl_desc = entries;
+	}
+
+	/* alloc dma for this ring */
+	desc = pci_zalloc_consistent(rtlpci->pdev,
+				     sizeof(*desc) * entries, &desc_dma);
+
+	if (!desc || (unsigned long)desc & 0xFF) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Cannot allocate TX ring (prio = %d)\n", prio);
 		return -ENOMEM;
 	}
 
-	rtlpci->tx_ring[prio].desc = ring;
-	rtlpci->tx_ring[prio].dma = dma;
+	rtlpci->tx_ring[prio].desc = desc;
+	rtlpci->tx_ring[prio].dma = desc_dma;
+
 	rtlpci->tx_ring[prio].idx = 0;
 	rtlpci->tx_ring[prio].entries = entries;
 	skb_queue_head_init(&rtlpci->tx_ring[prio].queue);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "queue:%d, ring_addr:%p\n",
-		 prio, ring);
-
-	for (i = 0; i < entries; i++) {
-		nextdescaddress = (u32) dma +
-					      ((i + 1) % entries) *
-					      sizeof(*ring);
-
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)&(ring[i]),
-					    true, HW_DESC_TX_NEXTDESC_ADDR,
-					    (u8 *)&nextdescaddress);
+		 prio, desc);
+
+	/* init every desc in this ring */
+	if (!rtlpriv->use_new_trx_flow) {
+		for (i = 0; i < entries; i++) {
+			nextdescaddress = (u32)desc_dma +
+					  ((i +	1) % entries) *
+					  sizeof(*desc);
+
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *)&desc[i],
+						    true,
+						    HW_DESC_TX_NEXTDESC_ADDR,
+						    (u8 *)&nextdescaddress);
+		}
 	}
-
 	return 0;
 }
 
-static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw)
+static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_rx_desc *entry = NULL;
-	int i, rx_queue_idx;
-	u8 tmp_one = 1;
+	int i;
 
-	/*
-	 *rx_queue_idx 0:RX_MPDU_QUEUE
-	 *rx_queue_idx 1:RX_CMD_QUEUE
-	 */
-	for (rx_queue_idx = 0; rx_queue_idx < RTL_PCI_MAX_RX_QUEUE;
-	     rx_queue_idx++) {
-		rtlpci->rx_ring[rx_queue_idx].desc =
-			pci_zalloc_consistent(rtlpci->pdev,
-					      sizeof(*rtlpci->rx_ring[rx_queue_idx].desc) * rtlpci->rxringcount,
-					      &rtlpci->rx_ring[rx_queue_idx].dma);
-
-		if (!rtlpci->rx_ring[rx_queue_idx].desc ||
-		    (unsigned long)rtlpci->rx_ring[rx_queue_idx].desc & 0xFF) {
+	if (rtlpriv->use_new_trx_flow) {
+		struct rtl_rx_buffer_desc *entry = NULL;
+		/* alloc dma for this ring */
+		rtlpci->rx_ring[rxring_idx].buffer_desc =
+		    pci_zalloc_consistent(rtlpci->pdev,
+					  sizeof(*rtlpci->rx_ring[rxring_idx].
+						 buffer_desc) *
+						 rtlpci->rxringcount,
+					  &rtlpci->rx_ring[rxring_idx].dma);
+		if (!rtlpci->rx_ring[rxring_idx].buffer_desc ||
+		    (ulong)rtlpci->rx_ring[rxring_idx].buffer_desc & 0xFF) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 				 "Cannot allocate RX ring\n");
 			return -ENOMEM;
 		}
 
-		rtlpci->rx_ring[rx_queue_idx].idx = 0;
+		/* init every desc in this ring */
+		rtlpci->rx_ring[rxring_idx].idx = 0;
+		for (i = 0; i < rtlpci->rxringcount; i++) {
+			entry = &rtlpci->rx_ring[rxring_idx].buffer_desc[i];
+			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
+						      rxring_idx, i))
+				return -ENOMEM;
+		}
+	} else {
+		struct rtl_rx_desc *entry = NULL;
+		u8 tmp_one = 1;
+		/* alloc dma for this ring */
+		rtlpci->rx_ring[rxring_idx].desc =
+		    pci_zalloc_consistent(rtlpci->pdev,
+					  sizeof(*rtlpci->rx_ring[rxring_idx].
+					  desc) * rtlpci->rxringcount,
+					  &rtlpci->rx_ring[rxring_idx].dma);
+		if (!rtlpci->rx_ring[rxring_idx].desc ||
+		    (unsigned long)rtlpci->rx_ring[rxring_idx].desc & 0xFF) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Cannot allocate RX ring\n");
+			return -ENOMEM;
+		}
 
-		/* If amsdu_8k is disabled, set buffersize to 4096. This
-		 * change will reduce memory fragmentation.
-		 */
-		if (rtlpci->rxbuffersize > 4096 &&
-		    rtlpriv->rtlhal.disable_amsdu_8k)
-			rtlpci->rxbuffersize = 4096;
+		/* init every desc in this ring */
+		rtlpci->rx_ring[rxring_idx].idx = 0;
 
 		for (i = 0; i < rtlpci->rxringcount; i++) {
-			struct sk_buff *skb =
-			    dev_alloc_skb(rtlpci->rxbuffersize);
-			u32 bufferaddress;
-			if (!skb)
-				return 0;
-			kmemleak_not_leak(skb);
-			entry = &rtlpci->rx_ring[rx_queue_idx].desc[i];
-
-			/*skb->dev = dev; */
-
-			rtlpci->rx_ring[rx_queue_idx].rx_buf[i] = skb;
-
-			/*
-			 *just set skb->cb to mapping addr
-			 *for pci_unmap_single use
-			 */
-			*((dma_addr_t *) skb->cb) =
-			    pci_map_single(rtlpci->pdev, skb_tail_pointer(skb),
-					   rtlpci->rxbuffersize,
-					   PCI_DMA_FROMDEVICE);
-
-			bufferaddress = (*((dma_addr_t *)skb->cb));
-			if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress)) {
-				dev_kfree_skb_any(skb);
-				return 1;
-			}
-			rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-						    HW_DESC_RXBUFF_ADDR,
-						    (u8 *)&bufferaddress);
-			rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-						    HW_DESC_RXPKT_LEN,
-						    (u8 *)&rtlpci->
-						    rxbuffersize);
-			rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-						    HW_DESC_RXOWN,
-						    &tmp_one);
+			entry = &rtlpci->rx_ring[rxring_idx].desc[i];
+			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
+						      rxring_idx, i))
+				return -ENOMEM;
 		}
 
 		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
@@ -1209,56 +1344,70 @@ static void _rtl_pci_free_tx_ring(struct ieee80211_hw *hw,
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
 
+	/* free every desc in this ring */
 	while (skb_queue_len(&ring->queue)) {
-		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
+		u8 *entry;
 		struct sk_buff *skb = __skb_dequeue(&ring->queue);
 
+		if (rtlpriv->use_new_trx_flow)
+			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
+		else
+			entry = (u8 *)(&ring->desc[ring->idx]);
+
 		pci_unmap_single(rtlpci->pdev,
 				 rtlpriv->cfg->
-					     ops->get_desc((u8 *) entry, true,
+					     ops->get_desc((u8 *)entry, true,
 						   HW_DESC_TXBUFF_ADDR),
 				 skb->len, PCI_DMA_TODEVICE);
 		kfree_skb(skb);
 		ring->idx = (ring->idx + 1) % ring->entries;
 	}
 
-	if (ring->desc) {
+	/* free dma of this ring */
+	pci_free_consistent(rtlpci->pdev,
+			    sizeof(*ring->desc) * ring->entries,
+			    ring->desc, ring->dma);
+	ring->desc = NULL;
+	if (rtlpriv->use_new_trx_flow) {
 		pci_free_consistent(rtlpci->pdev,
 				    sizeof(*ring->desc) * ring->entries,
-				    ring->desc, ring->dma);
+				    ring->buffer_desc, ring->buffer_desc_dma);
 		ring->desc = NULL;
 	}
 }
 
-static void _rtl_pci_free_rx_ring(struct rtl_pci *rtlpci)
+static void _rtl_pci_free_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
 {
-	int i, rx_queue_idx;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	int i;
 
-	/*rx_queue_idx 0:RX_MPDU_QUEUE */
-	/*rx_queue_idx 1:RX_CMD_QUEUE */
-	for (rx_queue_idx = 0; rx_queue_idx < RTL_PCI_MAX_RX_QUEUE;
-	     rx_queue_idx++) {
-		for (i = 0; i < rtlpci->rxringcount; i++) {
-			struct sk_buff *skb =
-			    rtlpci->rx_ring[rx_queue_idx].rx_buf[i];
-			if (!skb)
-				continue;
-
-			pci_unmap_single(rtlpci->pdev,
-					 *((dma_addr_t *) skb->cb),
-					 rtlpci->rxbuffersize,
-					 PCI_DMA_FROMDEVICE);
-			kfree_skb(skb);
-		}
+	/* free every desc in this ring */
+	for (i = 0; i < rtlpci->rxringcount; i++) {
+		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[i];
 
-		if (rtlpci->rx_ring[rx_queue_idx].desc) {
-			pci_free_consistent(rtlpci->pdev,
-				    sizeof(*rtlpci->rx_ring[rx_queue_idx].
-					   desc) * rtlpci->rxringcount,
-				    rtlpci->rx_ring[rx_queue_idx].desc,
-				    rtlpci->rx_ring[rx_queue_idx].dma);
-			rtlpci->rx_ring[rx_queue_idx].desc = NULL;
-		}
+		if (!skb)
+			continue;
+		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *)skb->cb),
+				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
+		kfree_skb(skb);
+	}
+
+	/* free dma of this ring */
+	if (rtlpriv->use_new_trx_flow) {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*rtlpci->rx_ring[rxring_idx].
+				    buffer_desc) * rtlpci->rxringcount,
+				    rtlpci->rx_ring[rxring_idx].buffer_desc,
+				    rtlpci->rx_ring[rxring_idx].dma);
+		rtlpci->rx_ring[rxring_idx].buffer_desc = NULL;
+	} else {
+		pci_free_consistent(rtlpci->pdev,
+				    sizeof(*rtlpci->rx_ring[rxring_idx].desc) *
+				    rtlpci->rxringcount,
+				    rtlpci->rx_ring[rxring_idx].desc,
+				    rtlpci->rx_ring[rxring_idx].dma);
+		rtlpci->rx_ring[rxring_idx].desc = NULL;
 	}
 }
 
@@ -1266,11 +1415,16 @@ static int _rtl_pci_init_trx_ring(struct ieee80211_hw *hw)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	int ret;
-	int i;
+	int i, rxring_idx;
 
-	ret = _rtl_pci_init_rx_ring(hw);
-	if (ret)
-		return ret;
+	/* rxring_idx 0:RX_MPDU_QUEUE
+	 * rxring_idx 1:RX_CMD_QUEUE
+	 */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
+		ret = _rtl_pci_init_rx_ring(hw, rxring_idx);
+		if (ret)
+			return ret;
+	}
 
 	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
 		ret = _rtl_pci_init_tx_ring(hw, i,
@@ -1282,10 +1436,12 @@ static int _rtl_pci_init_trx_ring(struct ieee80211_hw *hw)
 	return 0;
 
 err_free_rings:
-	_rtl_pci_free_rx_ring(rtlpci);
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
+		_rtl_pci_free_rx_ring(hw, rxring_idx);
 
 	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
-		if (rtlpci->tx_ring[i].desc)
+		if (rtlpci->tx_ring[i].desc ||
+		    rtlpci->tx_ring[i].buffer_desc)
 			_rtl_pci_free_tx_ring(hw, i);
 
 	return 1;
@@ -1293,11 +1449,11 @@ err_free_rings:
 
 static int _rtl_pci_deinit_trx_ring(struct ieee80211_hw *hw)
 {
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u32 i;
+	u32 i, rxring_idx;
 
 	/*free rx rings */
-	_rtl_pci_free_rx_ring(rtlpci);
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
+		_rtl_pci_free_rx_ring(hw, rxring_idx);
 
 	/*free tx rings */
 	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
@@ -1310,48 +1466,76 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int i, rx_queue_idx;
+	int i, rxring_idx;
 	unsigned long flags;
 	u8 tmp_one = 1;
-
-	/*rx_queue_idx 0:RX_MPDU_QUEUE */
-	/*rx_queue_idx 1:RX_CMD_QUEUE */
-	for (rx_queue_idx = 0; rx_queue_idx < RTL_PCI_MAX_RX_QUEUE;
-	     rx_queue_idx++) {
-		/*
-		 *force the rx_ring[RX_MPDU_QUEUE/
-		 *RX_CMD_QUEUE].idx to the first one
-		 */
-		if (rtlpci->rx_ring[rx_queue_idx].desc) {
+	u32 bufferaddress;
+	/* rxring_idx 0:RX_MPDU_QUEUE */
+	/* rxring_idx 1:RX_CMD_QUEUE */
+	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
+		/* force the rx_ring[RX_MPDU_QUEUE/
+		 * RX_CMD_QUEUE].idx to the first one
+		 *new trx flow, do nothing
+		*/
+		if (!rtlpriv->use_new_trx_flow &&
+		    rtlpci->rx_ring[rxring_idx].desc) {
 			struct rtl_rx_desc *entry = NULL;
 
+			rtlpci->rx_ring[rxring_idx].idx = 0;
 			for (i = 0; i < rtlpci->rxringcount; i++) {
-				entry = &rtlpci->rx_ring[rx_queue_idx].desc[i];
-				rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry,
-							    false,
-							    HW_DESC_RXOWN,
-							    &tmp_one);
+				entry = &rtlpci->rx_ring[rxring_idx].desc[i];
+				bufferaddress =
+				  rtlpriv->cfg->ops->get_desc((u8 *)entry,
+				  false , HW_DESC_RXBUFF_ADDR);
+				memset((u8 *)entry , 0 ,
+				       sizeof(*rtlpci->rx_ring
+				       [rxring_idx].desc));/*clear one entry*/
+				if (rtlpriv->use_new_trx_flow) {
+					rtlpriv->cfg->ops->set_desc(hw,
+					    (u8 *)entry, false,
+					    HW_DESC_RX_PREPARE,
+					    (u8 *)&bufferaddress);
+				} else {
+					rtlpriv->cfg->ops->set_desc(hw,
+					    (u8 *)entry, false,
+					    HW_DESC_RXBUFF_ADDR,
+					    (u8 *)&bufferaddress);
+					rtlpriv->cfg->ops->set_desc(hw,
+					    (u8 *)entry, false,
+					    HW_DESC_RXPKT_LEN,
+					    (u8 *)&rtlpci->rxbuffersize);
+					rtlpriv->cfg->ops->set_desc(hw,
+					    (u8 *)entry, false,
+					    HW_DESC_RXOWN,
+					    (u8 *)&tmp_one);
+				}
 			}
-			rtlpci->rx_ring[rx_queue_idx].idx = 0;
+			rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
+					    HW_DESC_RXERO, (u8 *)&tmp_one);
 		}
+		rtlpci->rx_ring[rxring_idx].idx = 0;
 	}
 
 	/*
 	 *after reset, release previous pending packet,
 	 *and force the  tx idx to the first one
 	 */
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
-		if (rtlpci->tx_ring[i].desc) {
+		if (rtlpci->tx_ring[i].desc ||
+		    rtlpci->tx_ring[i].buffer_desc) {
 			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[i];
 
 			while (skb_queue_len(&ring->queue)) {
-				struct rtl_tx_desc *entry;
-				struct sk_buff *skb;
+				u8 *entry;
+				struct sk_buff *skb =
+					__skb_dequeue(&ring->queue);
+				if (rtlpriv->use_new_trx_flow)
+					entry = (u8 *)(&ring->buffer_desc
+								[ring->idx]);
+				else
+					entry = (u8 *)(&ring->desc[ring->idx]);
 
-				spin_lock_irqsave(&rtlpriv->locks.irq_th_lock,
-						  flags);
-				entry = &ring->desc[ring->idx];
-				skb = __skb_dequeue(&ring->queue);
 				pci_unmap_single(rtlpci->pdev,
 						 rtlpriv->cfg->ops->
 							 get_desc((u8 *)
@@ -1360,13 +1544,13 @@ int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
 							 HW_DESC_TXBUFF_ADDR),
 						 skb->len, PCI_DMA_TODEVICE);
 				ring->idx = (ring->idx + 1) % ring->entries;
-				spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock,
-						  flags);
 				kfree_skb(skb);
+				ring->idx = (ring->idx + 1) % ring->entries;
 			}
 			ring->idx = 0;
 		}
 	}
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
 
 	return 0;
 }
@@ -1421,7 +1605,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 	struct rtl8192_tx_ring *ring;
 	struct rtl_tx_desc *pdesc;
 	struct rtl_tx_buffer_desc *ptx_bd_desc = NULL;
-	u8 idx;
+	u16 idx;
 	u8 hw_queue = _rtl_mac_to_hwqueue(hw, skb);
 	unsigned long flags;
 	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
@@ -1454,11 +1638,15 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 
 	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 	ring = &rtlpci->tx_ring[hw_queue];
-	if (hw_queue != BEACON_QUEUE)
-		idx = (ring->idx + skb_queue_len(&ring->queue)) %
-				ring->entries;
-	else
+	if (hw_queue != BEACON_QUEUE) {
+		if (rtlpriv->use_new_trx_flow)
+			idx = ring->cur_tx_wp;
+		else
+			idx = (ring->idx + skb_queue_len(&ring->queue)) %
+			      ring->entries;
+	} else {
 		idx = 0;
+	}
 
 	pdesc = &ring->desc[idx];
 	if (rtlpriv->use_new_trx_flow) {
@@ -1469,7 +1657,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 
 		if ((own == 1) && (hw_queue != BEACON_QUEUE)) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-				 "No more TX desc@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%d\n",
+				 "No more TX desc@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%x\n",
 				 hw_queue, ring->idx, idx,
 				 skb_queue_len(&ring->queue));
 
@@ -1511,7 +1699,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 	if ((ring->entries - skb_queue_len(&ring->queue)) < 2 &&
 	    hw_queue != BEACON_QUEUE) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
-			 "less desc left, stop skb_queue@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%d\n",
+			 "less desc left, stop skb_queue@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%x\n",
 			 hw_queue, ring->idx, idx,
 			 skb_queue_len(&ring->queue));
 
@@ -1525,7 +1713,7 @@ static int rtl_pci_tx(struct ieee80211_hw *hw,
 	return 0;
 }
 
-static void rtl_pci_flush(struct ieee80211_hw *hw, bool drop)
+static void rtl_pci_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
@@ -1540,6 +1728,11 @@ static void rtl_pci_flush(struct ieee80211_hw *hw, bool drop)
 
 	for (queue_id = RTL_PCI_MAX_TX_QUEUE_COUNT - 1; queue_id >= 0;) {
 		u32 queue_len;
+
+		if (((queues >> queue_id) & 0x1) == 0) {
+			queue_id--;
+			continue;
+		}
 		ring = &pcipriv->dev.tx_ring[queue_id];
 		queue_len = skb_queue_len(&ring->queue);
 		if (queue_len == 0 || queue_id == BEACON_QUEUE ||
@@ -1603,6 +1796,10 @@ static int rtl_pci_start(struct ieee80211_hw *hw)
 	rtl_pci_reset_trx_ring(hw);
 
 	rtlpci->driver_is_goingto_unload = false;
+	if (rtlpriv->cfg->ops->get_btc_status()) {
+		rtlpriv->btcoexist.btc_ops->btc_init_variables(rtlpriv);
+		rtlpriv->btcoexist.btc_ops->btc_init_hal_vars(rtlpriv);
+	}
 	err = rtlpriv->cfg->ops->hw_init(hw);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
@@ -1622,7 +1819,7 @@ static int rtl_pci_start(struct ieee80211_hw *hw)
 
 	rtlpci->up_first_time = false;
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "OK\n");
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "rtl_pci_start OK\n");
 	return 0;
 }
 
@@ -1635,6 +1832,9 @@ static void rtl_pci_stop(struct ieee80211_hw *hw)
 	unsigned long flags;
 	u8 RFInProgressTimeOut = 0;
 
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_halt_notify();
+
 	/*
 	 *should be before disable interrupt&adapter
 	 *and will do it immediately.
@@ -1753,6 +1953,22 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 		rtlhal->hw_type = HARDWARE_TYPE_RTL8188EE;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 			 "Find adapter, Hardware type is 8188EE\n");
+	} else if (deviceid == RTL_PCI_8723BE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8723BE;
+			RT_TRACE(rtlpriv, COMP_INIT , DBG_LOUD,
+				 "Find adapter, Hardware type is 8723BE\n");
+	} else if (deviceid == RTL_PCI_8192EE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8192EE;
+			RT_TRACE(rtlpriv, COMP_INIT , DBG_LOUD,
+				 "Find adapter, Hardware type is 8192EE\n");
+	} else if (deviceid == RTL_PCI_8821AE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8821AE;
+			RT_TRACE(rtlpriv, COMP_INIT , DBG_LOUD,
+				 "Find adapter, Hardware type is 8821AE\n");
+	} else if (deviceid == RTL_PCI_8812AE_DID) {
+			rtlhal->hw_type = HARDWARE_TYPE_RTL8812AE;
+			RT_TRACE(rtlpriv, COMP_INIT , DBG_LOUD,
+				 "Find adapter, Hardware type is 8812AE\n");
 	} else {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Err: Unknown device - vid/did=%x/%x\n",
@@ -1779,11 +1995,20 @@ static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
 			rtlhal->interfaceindex = 0;
 		}
 	}
+
+	/* 92ee use new trx flow */
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
+		rtlpriv->use_new_trx_flow = true;
+	else
+		rtlpriv->use_new_trx_flow = false;
+
 	/*find bus info */
 	pcipriv->ndis_adapter.busnumber = pdev->bus->number;
 	pcipriv->ndis_adapter.devnumber = PCI_SLOT(pdev->devfn);
 	pcipriv->ndis_adapter.funcnumber = PCI_FUNC(pdev->devfn);
 
+	/*find bridge info */
+	pcipriv->ndis_adapter.pcibridge_vendor = PCI_BRIDGE_VENDOR_UNKNOWN;
 	/* some ARM have no bridge_pdev and will crash here
 	 * so we should check if bridge_pdev is NULL
 	 */
@@ -1951,6 +2176,11 @@ int rtl_pci_probe(struct pci_dev *pdev,
 	pcipriv = (void *)rtlpriv->priv;
 	pcipriv->dev.pdev = pdev;
 	init_completion(&rtlpriv->firmware_loading_complete);
+	/*proximity init here*/
+	rtlpriv->proximity.proxim_on = false;
+
+	pcipriv = (void *)rtlpriv->priv;
+	pcipriv->dev.pdev = pdev;
 
 	/* init cfg & intf_ops */
 	rtlpriv->rtlhal.interface = INTF_PCI;
@@ -2013,9 +2243,6 @@ int rtl_pci_probe(struct pci_dev *pdev,
 	/*like read eeprom and so on */
 	rtlpriv->cfg->ops->read_eeprom_info(hw);
 
-	/*aspm */
-	rtl_pci_init_aspm(hw);
-
 	/* Init mac80211 sw */
 	err = rtl_init_core(hw);
 	if (err) {
@@ -2036,9 +2263,20 @@ int rtl_pci_probe(struct pci_dev *pdev,
 		err = -ENODEV;
 		goto fail3;
 	}
-
 	rtlpriv->cfg->ops->init_sw_leds(hw);
 
+	/*aspm */
+	rtl_pci_init_aspm(hw);
+
+	err = ieee80211_register_hw(hw);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Can't register mac80211 hw.\n");
+		err = -ENODEV;
+		goto fail3;
+	}
+	rtlpriv->mac80211.mac80211_registered = 1;
+
 	err = sysfs_create_group(&pdev->dev.kobj, &rtl_attribute_group);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -2046,6 +2284,9 @@ int rtl_pci_probe(struct pci_dev *pdev,
 		goto fail3;
 	}
 
+	/*init rfkill */
+	rtl_init_rfkill(hw);	/* Init PCI sw */
+
 	rtlpci = rtl_pcidev(pcipriv);
 	err = rtl_pci_intr_mode_decide(hw);
 	if (err) {
@@ -2056,9 +2297,11 @@ int rtl_pci_probe(struct pci_dev *pdev,
 	}
 	rtlpci->irq_alloc = 1;
 
+	set_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
 	return 0;
 
 fail3:
+	pci_set_drvdata(pdev, NULL);
 	rtl_deinit_core(hw);
 
 	if (rtlpriv->io.pci_mem_start != 0)
@@ -2128,6 +2371,8 @@ void rtl_pci_disconnect(struct pci_dev *pdev)
 
 	rtl_pci_disable_aspm(hw);
 
+	pci_set_drvdata(pdev, NULL);
+
 	ieee80211_free_hw(hw);
 }
 EXPORT_SYMBOL(rtl_pci_disconnect);
diff --git a/drivers/net/wireless/rtlwifi/pci.h b/drivers/net/wireless/rtlwifi/pci.h
index 90174a814a6d..5e832306dba9 100644
--- a/drivers/net/wireless/rtlwifi/pci.h
+++ b/drivers/net/wireless/rtlwifi/pci.h
@@ -39,10 +39,11 @@
 #define RTL_PCI_RX_CMD_QUEUE			1
 #define RTL_PCI_MAX_RX_QUEUE			2
 
-#define RTL_PCI_MAX_RX_COUNT			64
+#define RTL_PCI_MAX_RX_COUNT			512/*64*/
 #define RTL_PCI_MAX_TX_QUEUE_COUNT		9
 
 #define RT_TXDESC_NUM				128
+#define TX_DESC_NUM_92E				512
 #define RT_TXDESC_NUM_BE_QUEUE			256
 
 #define BK_QUEUE				0
@@ -62,6 +63,12 @@
 	.subdevice = PCI_ANY_ID,\
 	.driver_data = (kernel_ulong_t)&(cfg)
 
+#define INTEL_VENDOR_ID				0x8086
+#define SIS_VENDOR_ID				0x1039
+#define ATI_VENDOR_ID				0x1002
+#define ATI_DEVICE_ID				0x7914
+#define AMD_VENDOR_ID				0x1022
+
 #define PCI_MAX_BRIDGE_NUMBER			255
 #define PCI_MAX_DEVICES				32
 #define PCI_MAX_FUNCTION			8
@@ -69,6 +76,11 @@
 #define PCI_CONF_ADDRESS	0x0CF8	/*PCI Configuration Space Address */
 #define PCI_CONF_DATA		0x0CFC	/*PCI Configuration Space Data */
 
+#define PCI_CLASS_BRIDGE_DEV		0x06
+#define PCI_SUBCLASS_BR_PCI_TO_PCI	0x04
+#define PCI_CAPABILITY_ID_PCI_EXPRESS	0x10
+#define PCI_CAP_ID_EXP			0x10
+
 #define U1DONTCARE			0xFF
 #define U2DONTCARE			0xFFFF
 #define U4DONTCARE			0xFFFFFFFF
@@ -87,6 +99,7 @@
 #define RTL_PCI_700F_DID	0x700F
 #define RTL_PCI_701F_DID	0x701F
 #define RTL_PCI_DLINK_DID	0x3304
+#define RTL_PCI_8723AE_DID	0x8723	/*8723e */
 #define RTL_PCI_8192CET_DID	0x8191	/*8192ce */
 #define RTL_PCI_8192CE_DID	0x8178	/*8192ce */
 #define RTL_PCI_8191CE_DID	0x8177	/*8192ce */
@@ -95,6 +108,10 @@
 #define RTL_PCI_8192DE_DID	0x8193	/*8192de */
 #define RTL_PCI_8192DE_DID2	0x002B	/*92DE*/
 #define RTL_PCI_8188EE_DID	0x8179  /*8188ee*/
+#define RTL_PCI_8723BE_DID	0xB723  /*8723be*/
+#define RTL_PCI_8192EE_DID	0x818B	/*8192ee*/
+#define RTL_PCI_8821AE_DID	0x8821	/*8821ae*/
+#define RTL_PCI_8812AE_DID	0x8812	/*8812ae*/
 
 /*8192 support 16 pages of IO registers*/
 #define RTL_MEM_MAPPED_IO_RANGE_8190PCI		0x1000
@@ -125,24 +142,34 @@ struct rtl_pci_capabilities_header {
 	u8 next;
 };
 
-struct rtl_rx_desc {
-	u32 dword[8];
+/* In new TRX flow, Buffer_desc is new concept
+ * But TX wifi info == TX descriptor in old flow
+ * RX wifi info == RX descriptor in old flow
+ */
+struct rtl_tx_buffer_desc {
+#if (RTL8192EE_SEG_NUM == 2)
+	u32 dword[2*(DMA_IS_64BIT + 1)*8]; /*seg = 8*/
+#elif (RTL8192EE_SEG_NUM == 1)
+	u32 dword[2*(DMA_IS_64BIT + 1)*4]; /*seg = 4*/
+#elif (RTL8192EE_SEG_NUM == 0)
+	u32 dword[2*(DMA_IS_64BIT + 1)*2]; /*seg = 2*/
+#endif
 } __packed;
 
 struct rtl_tx_desc {
 	u32 dword[16];
 } __packed;
 
-struct rtl_tx_cmd_desc {
-	u32 dword[16];
+struct rtl_rx_buffer_desc { /*rx buffer desc*/
+	u32 dword[2];
 } __packed;
 
-/* In new TRX flow, Buffer_desc is new concept
- * But TX wifi info == TX descriptor in old flow
- * RX wifi info == RX descriptor in old flow
- */
-struct rtl_tx_buffer_desc {
-	u32 dword[8]; /*seg = 4*/
+struct rtl_rx_desc { /*old: rx desc new: rx wifi info*/
+	u32 dword[8];
+} __packed;
+
+struct rtl_tx_cmd_desc {
+	u32 dword[16];
 } __packed;
 
 struct rtl8192_tx_ring {
@@ -153,6 +180,10 @@ struct rtl8192_tx_ring {
 	struct sk_buff_head queue;
 	/*add for new trx flow*/
 	struct rtl_tx_buffer_desc *buffer_desc; /*tx buffer descriptor*/
+	dma_addr_t buffer_desc_dma; /*tx bufferd desc dma memory*/
+	u16 avl_desc; /* available_desc_to_write */
+	u16 cur_tx_wp; /* current_tx_write_point */
+	u16 cur_tx_rp; /* current_tx_read_point */
 };
 
 struct rtl8192_rx_ring {
@@ -160,6 +191,9 @@ struct rtl8192_rx_ring {
 	dma_addr_t dma;
 	unsigned int idx;
 	struct sk_buff *rx_buf[RTL_PCI_MAX_RX_COUNT];
+	/*add for new trx flow*/
+	struct rtl_rx_buffer_desc *buffer_desc; /*rx buffer descriptor*/
+	u16 next_rx_rp; /* next_rx_read_point */
 };
 
 struct rtl_pci {
diff --git a/drivers/net/wireless/rtlwifi/ps.c b/drivers/net/wireless/rtlwifi/ps.c
index 50504942ded1..b69321d45f04 100644
--- a/drivers/net/wireless/rtlwifi/ps.c
+++ b/drivers/net/wireless/rtlwifi/ps.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -27,110 +23,11 @@
  *
  *****************************************************************************/
 
-#include <linux/export.h>
 #include "wifi.h"
 #include "base.h"
 #include "ps.h"
-
-/*	Description:
- *		This routine deals with the Power Configuration CMD
- *		 parsing for RTL8723/RTL8188E Series IC.
- *	Assumption:
- *		We should follow specific format that was released from HW SD.
- */
-bool rtl_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-			      u8 faversion, u8 interface_type,
-			      struct wlan_pwr_cfg pwrcfgcmd[])
-{
-	struct wlan_pwr_cfg cfg_cmd = {0};
-	bool polling_bit = false;
-	u32 ary_idx = 0;
-	u8 value = 0;
-	u32 offset = 0;
-	u32 polling_count = 0;
-	u32 max_polling_cnt = 5000;
-
-	do {
-		cfg_cmd = pwrcfgcmd[ary_idx];
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			"rtl_hal_pwrseqcmdparsing(): offset(%#x),cut_msk(%#x), famsk(%#x),"
-			"interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
-			GET_PWR_CFG_OFFSET(cfg_cmd),
-					   GET_PWR_CFG_CUT_MASK(cfg_cmd),
-			GET_PWR_CFG_FAB_MASK(cfg_cmd),
-					     GET_PWR_CFG_INTF_MASK(cfg_cmd),
-			GET_PWR_CFG_BASE(cfg_cmd), GET_PWR_CFG_CMD(cfg_cmd),
-			GET_PWR_CFG_MASK(cfg_cmd), GET_PWR_CFG_VALUE(cfg_cmd));
-
-		if ((GET_PWR_CFG_FAB_MASK(cfg_cmd)&faversion) &&
-		    (GET_PWR_CFG_CUT_MASK(cfg_cmd)&cut_version) &&
-		    (GET_PWR_CFG_INTF_MASK(cfg_cmd)&interface_type)) {
-			switch (GET_PWR_CFG_CMD(cfg_cmd)) {
-			case PWR_CMD_READ:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_READ\n");
-				break;
-			case PWR_CMD_WRITE:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n");
-				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
-
-				/*Read the value from system register*/
-				value = rtl_read_byte(rtlpriv, offset);
-				value &= (~(GET_PWR_CFG_MASK(cfg_cmd)));
-				value |= (GET_PWR_CFG_VALUE(cfg_cmd) &
-					  GET_PWR_CFG_MASK(cfg_cmd));
-
-				/*Write the value back to sytem register*/
-				rtl_write_byte(rtlpriv, offset, value);
-				break;
-			case PWR_CMD_POLLING:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n");
-				polling_bit = false;
-				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
-
-				do {
-					value = rtl_read_byte(rtlpriv, offset);
-
-					value &= GET_PWR_CFG_MASK(cfg_cmd);
-					if (value ==
-					    (GET_PWR_CFG_VALUE(cfg_cmd)
-					    & GET_PWR_CFG_MASK(cfg_cmd)))
-						polling_bit = true;
-					else
-						udelay(10);
-
-					if (polling_count++ > max_polling_cnt)
-						return false;
-				} while (!polling_bit);
-				break;
-			case PWR_CMD_DELAY:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n");
-				if (GET_PWR_CFG_VALUE(cfg_cmd) ==
-				    PWRSEQ_DELAY_US)
-					udelay(GET_PWR_CFG_OFFSET(cfg_cmd));
-				else
-					mdelay(GET_PWR_CFG_OFFSET(cfg_cmd));
-				break;
-			case PWR_CMD_END:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_END\n");
-				return true;
-			default:
-				RT_ASSERT(false,
-					 "rtl_hal_pwrseqcmdparsing(): Unknown CMD!!\n");
-				break;
-			}
-
-		}
-		ary_idx++;
-	} while (1);
-
-	return true;
-}
-EXPORT_SYMBOL(rtl_hal_pwrseqcmdparsing);
+#include <linux/export.h>
+#include "btcoexist/rtl_btc.h"
 
 bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
 {
@@ -181,11 +78,49 @@ EXPORT_SYMBOL(rtl_ps_disable_nic);
 
 bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
 			 enum rf_pwrstate state_toset,
-			 u32 changesource)
+			 u32 changesource, bool protect_or_not)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	enum rf_pwrstate rtstate;
 	bool actionallowed = false;
+	u16 rfwait_cnt = 0;
+
+	if (protect_or_not)
+		goto no_protect;
+
+	/*Only one thread can change
+	 *the RF state at one time, and others
+	 *should wait to be executed.
+	 */
+	while (true) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		if (ppsc->rfchange_inprogress) {
+			spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+				 "RF Change in progress! Wait to set..state_toset(%d).\n",
+				  state_toset);
+
+			/* Set RF after the previous action is done.  */
+			while (ppsc->rfchange_inprogress) {
+				rfwait_cnt++;
+				mdelay(1);
+				/*Wait too long, return false to avoid
+				 *to be stuck here.
+				 */
+				if (rfwait_cnt > 100)
+					return false;
+			}
+		} else {
+			ppsc->rfchange_inprogress = true;
+			spin_unlock(&rtlpriv->locks.rf_ps_lock);
+			break;
+		}
+	}
+
+no_protect:
+	rtstate = ppsc->rfpwr_state;
 
 	switch (state_toset) {
 	case ERFON:
@@ -227,6 +162,12 @@ bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
 	if (actionallowed)
 		rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
 
+	if (!protect_or_not) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
 	return actionallowed;
 }
 EXPORT_SYMBOL(rtl_ps_set_rf_state);
@@ -249,12 +190,13 @@ static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
 		}
 	}
 
-	rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate, RF_CHANGE_BY_IPS);
+	rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate,
+			    RF_CHANGE_BY_IPS, false);
 
 	if (ppsc->inactive_pwrstate == ERFOFF &&
 	    rtlhal->interface == INTF_PCI) {
 		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
-			!RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+		    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 			rtlpriv->intf_ops->enable_aspm(hw);
 			RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 		}
@@ -318,6 +260,11 @@ void rtl_ips_nic_off_wq_callback(void *data)
 			ppsc->inactive_pwrstate = ERFOFF;
 			ppsc->in_powersavemode = true;
 
+			/* call before RF off */
+			if (rtlpriv->cfg->ops->get_btc_status())
+				rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
+									ppsc->inactive_pwrstate);
+
 			/*rtl_pci_reset_trx_ring(hw); */
 			_rtl_ps_inactive_ps(hw);
 		}
@@ -328,10 +275,9 @@ void rtl_ips_nic_off(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	/*
-	 *because when link with ap, mac80211 will ask us
-	 *to disable nic quickly after scan before linking,
-	 *this will cause link failed, so we delay 100ms here
+	/* because when link with ap, mac80211 will ask us
+	 * to disable nic quickly after scan before linking,
+	 * this will cause link failed, so we delay 100ms here
 	 */
 	queue_delayed_work(rtlpriv->works.rtl_wq,
 			   &rtlpriv->works.ips_nic_off_wq, MSECS(100));
@@ -343,16 +289,12 @@ void rtl_ips_nic_off(struct ieee80211_hw *hw)
 void rtl_ips_nic_on(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	enum rf_pwrstate rtstate;
-	unsigned long flags;
-
-	if (mac->opmode != NL80211_IFTYPE_STATION)
-		return;
 
-	spin_lock_irqsave(&rtlpriv->locks.ips_lock, flags);
+	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
 
+	spin_lock(&rtlpriv->locks.ips_lock);
 	if (ppsc->inactiveps) {
 		rtstate = ppsc->rfpwr_state;
 
@@ -362,12 +304,14 @@ void rtl_ips_nic_on(struct ieee80211_hw *hw)
 
 			ppsc->inactive_pwrstate = ERFON;
 			ppsc->in_powersavemode = false;
-
 			_rtl_ps_inactive_ps(hw);
+			/* call after RF on */
+			if (rtlpriv->cfg->ops->get_btc_status())
+				rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
+									ppsc->inactive_pwrstate);
 		}
 	}
-
-	spin_unlock_irqrestore(&rtlpriv->locks.ips_lock, flags);
+	spin_unlock(&rtlpriv->locks.ips_lock);
 }
 EXPORT_SYMBOL_GPL(rtl_ips_nic_on);
 
@@ -404,7 +348,7 @@ static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
 }
 
 /* Change current and default preamble mode.*/
-static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
+void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -437,21 +381,24 @@ static void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
 		if (ppsc->dot11_psmode == EACTIVE) {
 			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 				 "FW LPS leave ps_mode:%x\n",
-				 FW_PS_ACTIVE_MODE);
+				  FW_PS_ACTIVE_MODE);
 			enter_fwlps = false;
 			ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
 			ppsc->smart_ps = 0;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_FW_LPS_ACTION,
-						(u8 *)(&enter_fwlps));
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_LPS_ACTION,
+						      (u8 *)(&enter_fwlps));
 			if (ppsc->p2p_ps_info.opp_ps)
-				rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
+				rtl_p2p_ps_cmd(hw , P2P_PS_ENABLE);
 
+			if (rtlpriv->cfg->ops->get_btc_status())
+				rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode);
 		} else {
 			if (rtl_get_fwlps_doze(hw)) {
 				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 					 "FW LPS enter ps_mode:%x\n",
 					 ppsc->fwctrl_psmode);
+				if (rtlpriv->cfg->ops->get_btc_status())
+					rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode);
 				enter_fwlps = true;
 				ppsc->pwr_mode = ppsc->fwctrl_psmode;
 				ppsc->smart_ps = 2;
@@ -473,6 +420,7 @@ void rtl_lps_enter(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned long flag;
 
 	if (!ppsc->fwctrl_lps)
 		return;
@@ -493,7 +441,7 @@ void rtl_lps_enter(struct ieee80211_hw *hw)
 	if (mac->link_state != MAC80211_LINKED)
 		return;
 
-	mutex_lock(&rtlpriv->locks.ps_mutex);
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 
 	/* Idle for a while if we connect to AP a while ago. */
 	if (mac->cnt_after_linked >= 2) {
@@ -505,8 +453,9 @@ void rtl_lps_enter(struct ieee80211_hw *hw)
 		}
 	}
 
-	mutex_unlock(&rtlpriv->locks.ps_mutex);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 }
+EXPORT_SYMBOL(rtl_lps_enter);
 
 /*Leave the leisure power save mode.*/
 void rtl_lps_leave(struct ieee80211_hw *hw)
@@ -514,14 +463,15 @@ void rtl_lps_leave(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	unsigned long flag;
 
-	mutex_lock(&rtlpriv->locks.ps_mutex);
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
 
 	if (ppsc->fwctrl_lps) {
 		if (ppsc->dot11_psmode != EACTIVE) {
 
 			/*FIX ME */
-			rtlpriv->cfg->ops->enable_interrupt(hw);
+			/*rtlpriv->cfg->ops->enable_interrupt(hw); */
 
 			if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
 			    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
@@ -536,8 +486,9 @@ void rtl_lps_leave(struct ieee80211_hw *hw)
 			rtl_lps_set_psmode(hw, EACTIVE);
 		}
 	}
-	mutex_unlock(&rtlpriv->locks.ps_mutex);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 }
+EXPORT_SYMBOL(rtl_lps_leave);
 
 /* For sw LPS*/
 void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
@@ -613,7 +564,7 @@ void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
 		/* back to low-power land. and delay is
 		 * prevent null power save frame tx fail */
 		queue_delayed_work(rtlpriv->works.rtl_wq,
-				&rtlpriv->works.ps_work, MSECS(5));
+				   &rtlpriv->works.ps_work, MSECS(5));
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
 			 "u_bufferd: %x, m_buffered: %x\n", u_buffed, m_buffed);
@@ -626,6 +577,7 @@ void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	unsigned long flag;
 
 	if (!rtlpriv->psc.swctrl_lps)
 		return;
@@ -633,14 +585,14 @@ void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
 		return;
 
 	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
-		RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+	    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 		rtlpriv->intf_ops->disable_aspm(hw);
 		RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 	}
 
-	mutex_lock(&rtlpriv->locks.ps_mutex);
-	rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS);
-	mutex_unlock(&rtlpriv->locks.ps_mutex);
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+	rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS, false);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 }
 
 void rtl_swlps_rfon_wq_callback(void *data)
@@ -657,6 +609,7 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	unsigned long flag;
 	u8 sleep_intv;
 
 	if (!rtlpriv->psc.sw_ps_enabled)
@@ -673,12 +626,19 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	if (rtlpriv->link_info.busytraffic)
 		return;
 
-	mutex_lock(&rtlpriv->locks.ps_mutex);
-	rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS);
-	mutex_unlock(&rtlpriv->locks.ps_mutex);
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	if (rtlpriv->psc.rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		return;
+	}
+	spin_unlock(&rtlpriv->locks.rf_ps_lock);
+
+	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
+	rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS , false);
+	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
 
 	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
-		!RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
+	    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
 		rtlpriv->intf_ops->enable_aspm(hw);
 		RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
 	}
@@ -706,7 +666,7 @@ void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
 	 * awake before every dtim */
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
 		 "dtim_counter:%x will sleep :%d beacon_intv\n",
-		 rtlpriv->psc.dtim_counter, sleep_intv);
+		  rtlpriv->psc.dtim_counter, sleep_intv);
 
 	/* we tested that 40ms is enough for sw & hw sw delay */
 	queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
@@ -744,7 +704,7 @@ void rtl_swlps_wq_callback(void *data)
 
 		if (rtlpriv->psc.state && !ps) {
 			rtlpriv->psc.sleep_ms = jiffies_to_msecs(jiffies -
-					rtlpriv->psc.last_action);
+						 rtlpriv->psc.last_action);
 		}
 
 		if (ps)
@@ -764,7 +724,7 @@ static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
 	u8 *pos, *end, *ie;
 	u16 noa_len;
 	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
-	u8 noa_num, index, i, noa_index = 0;
+	u8 noa_num, index , i, noa_index = 0;
 	bool find_p2p_ie = false , find_p2p_ps_ie = false;
 	pos = (u8 *)mgmt->u.beacon.variable;
 	end = data + len;
@@ -814,7 +774,7 @@ static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
 				index = 5;
 				for (i = 0; i < noa_num; i++) {
 					p2pinfo->noa_count_type[i] =
-						 READEF1BYTE(ie+index);
+							READEF1BYTE(ie+index);
 					index += 1;
 					p2pinfo->noa_duration[i] =
 						 READEF4BYTE((__le32 *)ie+index);
@@ -842,7 +802,7 @@ static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
 					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 				}
 			}
-		break;
+			break;
 		}
 		ie += 3 + noa_len;
 	}
@@ -860,7 +820,7 @@ static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct ieee80211_mgmt *mgmt = data;
 	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
-	u8 noa_num, index, i, noa_index = 0;
+	u8 noa_num, index , i , noa_index = 0;
 	u8 *pos, *end, *ie;
 	u16 noa_len;
 	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
@@ -906,7 +866,7 @@ static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
 				index = 5;
 				for (i = 0; i < noa_num; i++) {
 					p2pinfo->noa_count_type[i] =
-							 READEF1BYTE(ie+index);
+							READEF1BYTE(ie+index);
 					index += 1;
 					p2pinfo->noa_duration[i] =
 							 READEF4BYTE((__le32 *)ie+index);
@@ -934,37 +894,37 @@ static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
 					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
 				}
 			}
-		break;
+			break;
 		}
 		ie += 3 + noa_len;
 	}
 }
 
-void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+void rtl_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 	struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
 
-	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n", p2p_ps_state);
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, " p2p state %x\n" , p2p_ps_state);
 	switch (p2p_ps_state) {
 	case P2P_PS_DISABLE:
 		p2pinfo->p2p_ps_state = p2p_ps_state;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 					      &p2p_ps_state);
-
 		p2pinfo->noa_index = 0;
 		p2pinfo->ctwindow = 0;
 		p2pinfo->opp_ps = 0;
 		p2pinfo->noa_num = 0;
 		p2pinfo->p2p_ps_mode = P2P_PS_NONE;
-		if (rtlps->fw_current_inpsmode == true) {
+		if (rtlps->fw_current_inpsmode) {
 			if (rtlps->smart_ps == 0) {
 				rtlps->smart_ps = 2;
 				rtlpriv->cfg->ops->set_hw_reg(hw,
 					 HW_VAR_H2C_FW_PWRMODE,
 					 &rtlps->pwr_mode);
 			}
+
 		}
 		break;
 	case P2P_PS_ENABLE:
@@ -982,6 +942,7 @@ void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 			rtlpriv->cfg->ops->set_hw_reg(hw,
 				 HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 				 &p2p_ps_state);
+
 		}
 		break;
 	case P2P_PS_SCAN:
@@ -998,12 +959,16 @@ void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 		break;
 	}
 	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
-		 "ctwindow %x oppps %x\n", p2pinfo->ctwindow, p2pinfo->opp_ps);
+		 "ctwindow %x oppps %x\n",
+		 p2pinfo->ctwindow , p2pinfo->opp_ps);
 	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 		 "count %x duration %x index %x interval %x start time %x noa num %x\n",
-		 p2pinfo->noa_count_type[0], p2pinfo->noa_duration[0],
-		 p2pinfo->noa_index, p2pinfo->noa_interval[0],
-		 p2pinfo->noa_start_time[0], p2pinfo->noa_num);
+		 p2pinfo->noa_count_type[0],
+		 p2pinfo->noa_duration[0],
+		 p2pinfo->noa_index,
+		 p2pinfo->noa_interval[0],
+		 p2pinfo->noa_start_time[0],
+		 p2pinfo->noa_num);
 	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "end\n");
 }
 
@@ -1032,8 +997,8 @@ void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
 		return;
 
 	if (ieee80211_is_action(hdr->frame_control))
-		rtl_p2p_action_ie(hw, data, len - FCS_LEN);
+		rtl_p2p_action_ie(hw , data , len - FCS_LEN);
 	else
-		rtl_p2p_noa_ie(hw, data, len - FCS_LEN);
+		rtl_p2p_noa_ie(hw , data , len - FCS_LEN);
 }
 EXPORT_SYMBOL_GPL(rtl_p2p_info);
diff --git a/drivers/net/wireless/rtlwifi/ps.h b/drivers/net/wireless/rtlwifi/ps.h
index 3bd41f958974..29dfc514212d 100644
--- a/drivers/net/wireless/rtlwifi/ps.h
+++ b/drivers/net/wireless/rtlwifi/ps.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -32,68 +28,9 @@
 
 #define MAX_SW_LPS_SLEEP_INTV	5
 
-/*---------------------------------------------
- * 3 The value of cmd: 4 bits
- *---------------------------------------------
- */
-#define    PWR_CMD_READ		0x00
-#define    PWR_CMD_WRITE	0x01
-#define    PWR_CMD_POLLING	0x02
-#define    PWR_CMD_DELAY	0x03
-#define    PWR_CMD_END		0x04
-
-/* define the base address of each block */
-#define	PWR_BASEADDR_MAC	0x00
-#define	PWR_BASEADDR_USB	0x01
-#define	PWR_BASEADDR_PCIE	0x02
-#define	PWR_BASEADDR_SDIO	0x03
-
-#define	PWR_FAB_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
-#define	PWR_CUT_A_MSK		BIT(1)
-#define	PWR_CUT_B_MSK		BIT(2)
-#define	PWR_CUT_C_MSK		BIT(3)
-#define	PWR_CUT_D_MSK		BIT(4)
-#define	PWR_CUT_E_MSK		BIT(5)
-#define	PWR_CUT_F_MSK		BIT(6)
-#define	PWR_CUT_G_MSK		BIT(7)
-#define	PWR_CUT_ALL_MSK		0xFF
-#define PWR_INTF_SDIO_MSK	BIT(0)
-#define PWR_INTF_USB_MSK	BIT(1)
-#define PWR_INTF_PCI_MSK	BIT(2)
-#define PWR_INTF_ALL_MSK	(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-
-enum pwrseq_delay_unit {
-	PWRSEQ_DELAY_US,
-	PWRSEQ_DELAY_MS,
-};
-
-struct wlan_pwr_cfg {
-	u16 offset;
-	u8 cut_msk;
-	u8 fab_msk:4;
-	u8 interface_msk:4;
-	u8 base:4;
-	u8 cmd:4;
-	u8 msk;
-	u8 value;
-};
-
-#define	GET_PWR_CFG_OFFSET(__PWR_CMD)	(__PWR_CMD.offset)
-#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)	(__PWR_CMD.cut_msk)
-#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)	(__PWR_CMD.fab_msk)
-#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	(__PWR_CMD.interface_msk)
-#define	GET_PWR_CFG_BASE(__PWR_CMD)	(__PWR_CMD.base)
-#define	GET_PWR_CFG_CMD(__PWR_CMD)	(__PWR_CMD.cmd)
-#define	GET_PWR_CFG_MASK(__PWR_CMD)	(__PWR_CMD.msk)
-#define	GET_PWR_CFG_VALUE(__PWR_CMD)	(__PWR_CMD.value)
-
-bool rtl_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-			      u8 fab_version, u8 interface_type,
-			      struct wlan_pwr_cfg pwrcfgcmd[]);
-
 bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
-			 enum rf_pwrstate state_toset, u32 changesource);
+			 enum rf_pwrstate state_toset, u32 changesource,
+			 bool protect_or_not);
 bool rtl_ps_enable_nic(struct ieee80211_hw *hw);
 bool rtl_ps_disable_nic(struct ieee80211_hw *hw);
 void rtl_ips_nic_off(struct ieee80211_hw *hw);
@@ -102,12 +39,14 @@ void rtl_ips_nic_off_wq_callback(void *data);
 void rtl_lps_enter(struct ieee80211_hw *hw);
 void rtl_lps_leave(struct ieee80211_hw *hw);
 
+void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode);
+
 void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len);
 void rtl_swlps_wq_callback(void *data);
 void rtl_swlps_rfon_wq_callback(void *data);
 void rtl_swlps_rf_awake(struct ieee80211_hw *hw);
 void rtl_swlps_rf_sleep(struct ieee80211_hw *hw);
-void rtl_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
+void rtl_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state);
 void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len);
 void rtl_lps_change_work_callback(struct work_struct *work);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseqcmd.h b/drivers/net/wireless/rtlwifi/pwrseqcmd.h
index 6e0f3ea37ec0..17ce0cb2c35c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseqcmd.h
+++ b/drivers/net/wireless/rtlwifi/pwrseqcmd.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,7 +26,7 @@
 #ifndef __RTL8723E_PWRSEQCMD_H__
 #define __RTL8723E_PWRSEQCMD_H__
 
-#include "../wifi.h"
+#include "wifi.h"
 /*---------------------------------------------
  * 3 The value of cmd: 4 bits
  *---------------------------------------------
diff --git a/drivers/net/wireless/rtlwifi/rc.c b/drivers/net/wireless/rtlwifi/rc.c
index ee28a1a3d010..7863bd278b22 100644
--- a/drivers/net/wireless/rtlwifi/rc.c
+++ b/drivers/net/wireless/rtlwifi/rc.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -35,13 +31,13 @@
  *Finds the highest rate index we can use
  *if skb is special data like DHCP/EAPOL, we set should
  *it to lowest rate CCK_1M, otherwise we set rate to
- *CCK11M or OFDM_54M based on wireless mode.
+ *highest rate based on wireless mode used for iwconfig
+ *show Tx rate.
  */
 static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
 				  struct ieee80211_sta *sta,
 				  struct sk_buff *skb, bool not_data)
 {
-	struct rtl_mac *rtlmac = rtl_mac(rtlpriv);
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_sta_info *sta_entry = NULL;
@@ -54,21 +50,13 @@ static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
 	 *2.in rtl_get_tcb_desc when we check rate is
 	 *      1M we will not use FW rate but user rate.
 	 */
-	if (rtlmac->opmode == NL80211_IFTYPE_AP ||
-	    rtlmac->opmode == NL80211_IFTYPE_ADHOC ||
-	    rtlmac->opmode == NL80211_IFTYPE_MESH_POINT) {
-		if (sta) {
-			sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-			wireless_mode = sta_entry->wireless_mode;
-		} else {
-			return 0;
-		}
-	} else {
-		wireless_mode = rtlmac->mode;
+
+	if (sta) {
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+		wireless_mode = sta_entry->wireless_mode;
 	}
 
-	if (rtl_is_special_data(rtlpriv->mac80211.hw, skb, true) ||
-			not_data) {
+	if (rtl_is_special_data(rtlpriv->mac80211.hw, skb, true) || not_data) {
 		return 0;
 	} else {
 		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
@@ -76,21 +64,27 @@ static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
 				return B_MODE_MAX_RIX;
 			} else if (wireless_mode == WIRELESS_MODE_G) {
 				return G_MODE_MAX_RIX;
-			} else {
+			} else if (wireless_mode == WIRELESS_MODE_N_24G) {
 				if (get_rf_type(rtlphy) != RF_2T2R)
 					return N_MODE_MCS7_RIX;
 				else
 					return N_MODE_MCS15_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_AC_24G) {
+				return AC_MODE_MCS9_RIX;
 			}
+			return 0;
 		} else {
 			if (wireless_mode == WIRELESS_MODE_A) {
 				return A_MODE_MAX_RIX;
-			} else {
+			} else if (wireless_mode == WIRELESS_MODE_N_5G) {
 				if (get_rf_type(rtlphy) != RF_2T2R)
 					return N_MODE_MCS7_RIX;
 				else
 					return N_MODE_MCS15_RIX;
+			} else if (wireless_mode == WIRELESS_MODE_AC_5G) {
+				return AC_MODE_MCS9_RIX;
 			}
+			return 0;
 		}
 	}
 }
@@ -103,35 +97,52 @@ static void _rtl_rc_rate_set_series(struct rtl_priv *rtlpriv,
 				    bool not_data)
 {
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	u8 sgi_20 = 0, sgi_40 = 0;
+	struct rtl_sta_info *sta_entry = NULL;
+	u8 wireless_mode = 0;
+	u8 sgi_20 = 0, sgi_40 = 0, sgi_80 = 0;
 
 	if (sta) {
 		sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
 		sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
+		sgi_80 = sta->vht_cap.cap & IEEE80211_VHT_CAP_SHORT_GI_80;
+		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+		wireless_mode = sta_entry->wireless_mode;
 	}
 	rate->count = tries;
 	rate->idx = rix >= 0x00 ? rix : 0x00;
+	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE &&
+	    wireless_mode == WIRELESS_MODE_AC_5G)
+		rate->idx += 0x10;/*2NSS for 8812AE*/
 
 	if (!not_data) {
 		if (txrc->short_preamble)
 			rate->flags |= IEEE80211_TX_RC_USE_SHORT_PREAMBLE;
 		if (mac->opmode == NL80211_IFTYPE_AP ||
-		    mac->opmode == NL80211_IFTYPE_ADHOC) {
-			if (sta && (sta->bandwidth >= IEEE80211_STA_RX_BW_40))
+			mac->opmode == NL80211_IFTYPE_ADHOC) {
+			if (sta && (sta->ht_cap.cap &
+				    IEEE80211_HT_CAP_SUP_WIDTH_20_40))
 				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+			if (sta && (sta->vht_cap.vht_supported))
+				rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
 		} else {
 			if (mac->bw_40)
 				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
+			if (mac->bw_80)
+				rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
 		}
-		if (sgi_20 || sgi_40)
+
+		if (sgi_20 || sgi_40 || sgi_80)
 			rate->flags |= IEEE80211_TX_RC_SHORT_GI;
-		if (sta && sta->ht_cap.ht_supported)
+		if (sta && sta->ht_cap.ht_supported &&
+		    ((wireless_mode == WIRELESS_MODE_N_5G) ||
+		     (wireless_mode == WIRELESS_MODE_N_24G)))
 			rate->flags |= IEEE80211_TX_RC_MCS;
 	}
 }
 
 static void rtl_get_rate(void *ppriv, struct ieee80211_sta *sta,
-			 void *priv_sta, struct ieee80211_tx_rate_control *txrc)
+			 void *priv_sta,
+			 struct ieee80211_tx_rate_control *txrc)
 {
 	struct rtl_priv *rtlpriv = ppriv;
 	struct sk_buff *skb = txrc->skb;
@@ -158,7 +169,7 @@ static void rtl_get_rate(void *ppriv, struct ieee80211_sta *sta,
 }
 
 static bool _rtl_tx_aggr_check(struct rtl_priv *rtlpriv,
-		struct rtl_sta_info *sta_entry, u16 tid)
+			       struct rtl_sta_info *sta_entry, u16 tid)
 {
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
 
@@ -166,7 +177,7 @@ static bool _rtl_tx_aggr_check(struct rtl_priv *rtlpriv,
 		return false;
 
 	if (mac->opmode == NL80211_IFTYPE_STATION &&
-		mac->cnt_after_linked < 3)
+	    mac->cnt_after_linked < 3)
 		return false;
 
 	if (sta_entry->tids[tid].agg.agg_state == RTL_AGG_STOP)
@@ -193,23 +204,23 @@ static void rtl_tx_status(void *ppriv,
 	if (rtl_is_special_data(mac->hw, skb, true))
 		return;
 
-	if (is_multicast_ether_addr(ieee80211_get_DA(hdr))
-	    || is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
 		return;
 
 	if (sta) {
 		/* Check if aggregation has to be enabled for this tid */
 		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
 		if ((sta->ht_cap.ht_supported) &&
-				!(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
+		    !(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
 			if (ieee80211_is_data_qos(fc)) {
 				u8 tid = rtl_get_tid(skb);
 				if (_rtl_tx_aggr_check(rtlpriv, sta_entry,
-				    tid)) {
+						       tid)) {
 					sta_entry->tids[tid].agg.agg_state =
-							 RTL_AGG_PROGRESS;
-					ieee80211_start_tx_ba_session(sta,
-								 tid, 5000);
+						RTL_AGG_PROGRESS;
+					ieee80211_start_tx_ba_session(sta, tid,
+								      5000);
 				}
 			}
 		}
@@ -223,8 +234,15 @@ static void rtl_rate_init(void *ppriv,
 {
 }
 
-static void *rtl_rate_alloc(struct ieee80211_hw *hw,
-		struct dentry *debugfsdir)
+static void rtl_rate_update(void *ppriv,
+			    struct ieee80211_supported_band *sband,
+			    struct cfg80211_chan_def *chandef,
+			    struct ieee80211_sta *sta, void *priv_sta,
+			    u32 changed)
+{
+}
+
+static void *rtl_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	return rtlpriv;
@@ -260,13 +278,14 @@ static void rtl_rate_free_sta(void *rtlpriv,
 	kfree(rate_priv);
 }
 
-static const struct rate_control_ops rtl_rate_ops = {
+static struct rate_control_ops rtl_rate_ops = {
 	.name = "rtl_rc",
 	.alloc = rtl_rate_alloc,
 	.free = rtl_rate_free,
 	.alloc_sta = rtl_rate_alloc_sta,
 	.free_sta = rtl_rate_free_sta,
 	.rate_init = rtl_rate_init,
+	.rate_update = rtl_rate_update,
 	.tx_status = rtl_tx_status,
 	.get_rate = rtl_get_rate,
 };
diff --git a/drivers/net/wireless/rtlwifi/rc.h b/drivers/net/wireless/rtlwifi/rc.h
index 4d6176160610..f29643d60d6b 100644
--- a/drivers/net/wireless/rtlwifi/rc.h
+++ b/drivers/net/wireless/rtlwifi/rc.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -38,10 +34,15 @@
 #define N_MODE_MCS7_RIX 7
 #define N_MODE_MCS15_RIX 15
 
+#define AC_MODE_MCS7_RIX 7
+#define AC_MODE_MCS8_RIX 8
+#define AC_MODE_MCS9_RIX 9
+
 struct rtl_rate_priv {
 	u8 ht_cap;
 };
 
 int rtl_rate_control_register(void);
 void rtl_rate_control_unregister(void);
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/regd.c b/drivers/net/wireless/rtlwifi/regd.c
index a4eb9b271438..1893d01b9e78 100644
--- a/drivers/net/wireless/rtlwifi/regd.c
+++ b/drivers/net/wireless/rtlwifi/regd.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -59,26 +55,23 @@ static struct country_code_to_enum_rd allCountries[] = {
  */
 #define RTL819x_2GHZ_CH12_13	\
 	REG_RULE(2467-10, 2472+10, 40, 0, 20,\
-		 NL80211_RRF_NO_IR)
+	NL80211_RRF_PASSIVE_SCAN)
 
 #define RTL819x_2GHZ_CH14	\
 	REG_RULE(2484-10, 2484+10, 40, 0, 20, \
-		 NL80211_RRF_NO_IR | NL80211_RRF_NO_OFDM)
+	NL80211_RRF_PASSIVE_SCAN | \
+	NL80211_RRF_NO_OFDM)
+
 
 /* 5G chan 36 - chan 64*/
 #define RTL819x_5GHZ_5150_5350	\
-	REG_RULE(5150-10, 5350+10, 40, 0, 30, \
-		 NL80211_RRF_NO_IR)
-
+	REG_RULE(5150-10, 5350+10, 80, 0, 30, 0)
 /* 5G chan 100 - chan 165*/
 #define RTL819x_5GHZ_5470_5850	\
-	REG_RULE(5470-10, 5850+10, 40, 0, 30, \
-		 NL80211_RRF_NO_IR)
-
+	REG_RULE(5470-10, 5850+10, 80, 0, 30, 0)
 /* 5G chan 149 - chan 165*/
 #define RTL819x_5GHZ_5725_5850	\
-	REG_RULE(5725-10, 5850+10, 40, 0, 30, \
-		 NL80211_RRF_NO_IR)
+	REG_RULE(5725-10, 5850+10, 80, 0, 30, 0)
 
 #define RTL819x_5GHZ_ALL	\
 	(RTL819x_5GHZ_5150_5350, RTL819x_5GHZ_5470_5850)
@@ -143,7 +136,7 @@ static const struct ieee80211_regdomain rtl_regdom_14 = {
 
 static bool _rtl_is_radar_freq(u16 center_freq)
 {
-	return (center_freq >= 5260 && center_freq <= 5700);
+	return center_freq >= 5260 && center_freq <= 5700;
 }
 
 static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
@@ -169,10 +162,9 @@ static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
 				continue;
 			if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
 				reg_rule = freq_reg_info(wiphy,
-							 MHZ_TO_KHZ(ch->center_freq));
+							 ch->center_freq);
 				if (IS_ERR(reg_rule))
 					continue;
-
 				/*
 				 *If 11d had a rule for this channel ensure
 				 *we enable adhoc/beaconing if it allows us to
@@ -182,11 +174,16 @@ static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
 				 *regulatory_hint().
 				 */
 
-				if (!(reg_rule->flags & NL80211_RRF_NO_IR))
-					ch->flags &= ~IEEE80211_CHAN_NO_IR;
+				if (!(reg_rule->flags & NL80211_RRF_NO_IBSS))
+					ch->flags &= ~IEEE80211_CHAN_NO_IBSS;
+				if (!(reg_rule->flags &
+				      NL80211_RRF_PASSIVE_SCAN))
+					ch->flags &=
+					    ~IEEE80211_CHAN_PASSIVE_SCAN;
 			} else {
 				if (ch->beacon_found)
-					ch->flags &= ~IEEE80211_CHAN_NO_IR;
+					ch->flags &= ~(IEEE80211_CHAN_NO_IBSS |
+						   IEEE80211_CHAN_PASSIVE_SCAN);
 			}
 		}
 	}
@@ -211,35 +208,35 @@ static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
 	 */
 	if (initiator != NL80211_REGDOM_SET_BY_COUNTRY_IE) {
 		ch = &sband->channels[11];	/* CH 12 */
-		if (ch->flags & IEEE80211_CHAN_NO_IR)
-			ch->flags &= ~IEEE80211_CHAN_NO_IR;
+		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
 		ch = &sband->channels[12];	/* CH 13 */
-		if (ch->flags & IEEE80211_CHAN_NO_IR)
-			ch->flags &= ~IEEE80211_CHAN_NO_IR;
+		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
 		return;
 	}
 
 	/*
-	 *If a country IE has been received check its rule for this
+	 *If a country IE has been recieved check its rule for this
 	 *channel first before enabling active scan. The passive scan
 	 *would have been enforced by the initial processing of our
 	 *custom regulatory domain.
 	 */
 
 	ch = &sband->channels[11];	/* CH 12 */
-	reg_rule = freq_reg_info(wiphy, MHZ_TO_KHZ(ch->center_freq));
+	reg_rule = freq_reg_info(wiphy, ch->center_freq);
 	if (!IS_ERR(reg_rule)) {
-		if (!(reg_rule->flags & NL80211_RRF_NO_IR))
-			if (ch->flags & IEEE80211_CHAN_NO_IR)
-				ch->flags &= ~IEEE80211_CHAN_NO_IR;
+		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
+			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
 	}
 
 	ch = &sband->channels[12];	/* CH 13 */
-	reg_rule = freq_reg_info(wiphy, MHZ_TO_KHZ(ch->center_freq));
+	reg_rule = freq_reg_info(wiphy, ch->center_freq);
 	if (!IS_ERR(reg_rule)) {
-		if (!(reg_rule->flags & NL80211_RRF_NO_IR))
-			if (ch->flags & IEEE80211_CHAN_NO_IR)
-				ch->flags &= ~IEEE80211_CHAN_NO_IR;
+		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
+			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
+				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
 	}
 }
 
@@ -276,7 +273,8 @@ static void _rtl_reg_apply_radar_flags(struct wiphy *wiphy)
 		 */
 		if (!(ch->flags & IEEE80211_CHAN_DISABLED))
 			ch->flags |= IEEE80211_CHAN_RADAR |
-				     IEEE80211_CHAN_NO_IR;
+			    IEEE80211_CHAN_NO_IBSS |
+			    IEEE80211_CHAN_PASSIVE_SCAN;
 	}
 }
 
@@ -289,9 +287,25 @@ static void _rtl_reg_apply_world_flags(struct wiphy *wiphy,
 	return;
 }
 
-static void _rtl_reg_notifier_apply(struct wiphy *wiphy,
-				    struct regulatory_request *request,
-				    struct rtl_regulatory *reg)
+static void _rtl_dump_channel_map(struct wiphy *wiphy)
+{
+	enum ieee80211_band band;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *ch;
+	unsigned int i;
+
+	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
+		if (!wiphy->bands[band])
+			continue;
+		sband = wiphy->bands[band];
+		for (i = 0; i < sband->n_channels; i++)
+			ch = &sband->channels[i];
+	}
+}
+
+static int _rtl_reg_notifier_apply(struct wiphy *wiphy,
+				   struct regulatory_request *request,
+				   struct rtl_regulatory *reg)
 {
 	/* We always apply this */
 	_rtl_reg_apply_radar_flags(wiphy);
@@ -305,10 +319,14 @@ static void _rtl_reg_notifier_apply(struct wiphy *wiphy,
 		_rtl_reg_apply_world_flags(wiphy, request->initiator, reg);
 		break;
 	}
+
+	_rtl_dump_channel_map(wiphy);
+
+	return 0;
 }
 
 static const struct ieee80211_regdomain *_rtl_regdomain_select(
-					       struct rtl_regulatory *reg)
+						struct rtl_regulatory *reg)
 {
 	switch (reg->country_code) {
 	case COUNTRY_CODE_FCC:
@@ -337,9 +355,9 @@ static const struct ieee80211_regdomain *_rtl_regdomain_select(
 
 static int _rtl_regd_init_wiphy(struct rtl_regulatory *reg,
 				struct wiphy *wiphy,
-				void (*reg_notifier) (struct wiphy *wiphy,
-						      struct regulatory_request *
-						      request))
+				void (*reg_notifier)(struct wiphy *wiphy,
+						     struct regulatory_request *
+						     request))
 {
 	const struct ieee80211_regdomain *regd;
 
@@ -348,7 +366,6 @@ static int _rtl_regd_init_wiphy(struct rtl_regulatory *reg,
 	wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG;
 	wiphy->regulatory_flags &= ~REGULATORY_STRICT_REG;
 	wiphy->regulatory_flags &= ~REGULATORY_DISABLE_BEACON_HINTS;
-
 	regd = _rtl_regdomain_select(reg);
 	wiphy_apply_custom_regulatory(wiphy, regd);
 	_rtl_reg_apply_radar_flags(wiphy);
@@ -368,7 +385,7 @@ static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
 }
 
 int rtl_regd_init(struct ieee80211_hw *hw,
-		  void (*reg_notifier) (struct wiphy *wiphy,
+		  void (*reg_notifier)(struct wiphy *wiphy,
 				       struct regulatory_request *request))
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -382,7 +399,8 @@ int rtl_regd_init(struct ieee80211_hw *hw,
 	rtlpriv->regd.country_code = rtlpriv->efuse.channel_plan;
 
 	RT_TRACE(rtlpriv, COMP_REGD, DBG_TRACE,
-		 "rtl: EEPROM regdomain: 0x%0x\n", rtlpriv->regd.country_code);
+		 "rtl: EEPROM regdomain: 0x%0x\n",
+		  rtlpriv->regd.country_code);
 
 	if (rtlpriv->regd.country_code >= COUNTRY_CODE_MAX) {
 		RT_TRACE(rtlpriv, COMP_REGD, DBG_DMESG,
@@ -403,7 +421,7 @@ int rtl_regd_init(struct ieee80211_hw *hw,
 
 	RT_TRACE(rtlpriv, COMP_REGD, DBG_TRACE,
 		 "rtl: Country alpha2 being used: %c%c\n",
-		 rtlpriv->regd.alpha2[0], rtlpriv->regd.alpha2[1]);
+		  rtlpriv->regd.alpha2[0], rtlpriv->regd.alpha2[1]);
 
 	_rtl_regd_init_wiphy(&rtlpriv->regd, wiphy, reg_notifier);
 
diff --git a/drivers/net/wireless/rtlwifi/regd.h b/drivers/net/wireless/rtlwifi/regd.h
index 4e1f4f00e6e9..3bbbaaa68530 100644
--- a/drivers/net/wireless/rtlwifi/regd.h
+++ b/drivers/net/wireless/rtlwifi/regd.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,6 +26,10 @@
 #ifndef __RTL_REGD_H__
 #define __RTL_REGD_H__
 
+/* for kernel 3.14 , both value are changed to IEEE80211_CHAN_NO_IR*/
+#define IEEE80211_CHAN_NO_IBSS IEEE80211_CHAN_NO_IR
+#define IEEE80211_CHAN_PASSIVE_SCAN IEEE80211_CHAN_NO_IR
+
 struct country_code_to_enum_rd {
 	u16 countrycode;
 	const char *iso_name;
@@ -56,6 +56,7 @@ enum country_code_type_t {
 
 int rtl_regd_init(struct ieee80211_hw *hw,
 		  void (*reg_notifier) (struct wiphy *wiphy,
-					struct regulatory_request *request));
+		  struct regulatory_request *request));
 void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request);
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/def.h b/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
index c764fff9ebe6..d9ea9d0c79a5 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/def.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -111,7 +107,6 @@
 
 #define CHIP_BONDING_IDENTIFIER(_value)	(((_value)>>22)&0x3)
 
-
 /* [15:12] IC version(CUT): A-cut=0, B-cut=1, C-cut=2, D-cut=3
  * [7] Manufacturer: TSMC=0, UMC=1
  * [6:4] RF type: 1T1R=0, 1T2R=1, 2T2R=2
@@ -130,7 +125,6 @@
 #define D_CUT_VERSION			((BIT(12)|BIT(13)))
 #define E_CUT_VERSION			BIT(14)
 
-
 /* MASK */
 #define IC_TYPE_MASK			(BIT(0)|BIT(1)|BIT(2))
 #define CHIP_TYPE_MASK			BIT(3)
@@ -147,7 +141,6 @@
 #define GET_CVID_ROM_VERSION(version)	((version) & ROM_VERSION_MASK)
 #define GET_CVID_CUT_VERSION(version)	((version) & CUT_VERSION_MASK)
 
-
 #define IS_81XXC(version)						\
 	((GET_CVID_IC_TYPE(version) == 0) ? true : false)
 #define IS_8723_SERIES(version)						\
@@ -174,7 +167,7 @@
 #define IS_81xxC_VENDOR_UMC_A_CUT(version)				\
 	(IS_81XXC(version) ? ((IS_CHIP_VENDOR_UMC(version)) ?		\
 	 ((GET_CVID_CUT_VERSION(version)) ? false : true) : false) : false)
-#define IS_81xxC_VENDOR_UMC_B_CUT(version)				\
+#define IS_81XXC_VENDOR_UMC_B_CUT(version)				\
 	(IS_81XXC(version) ? (IS_CHIP_VENDOR_UMC(version) ?		\
 	((GET_CVID_CUT_VERSION(version) == B_CUT_VERSION) ? true	\
 	: false) : false) : false)
@@ -225,44 +218,37 @@ enum power_polocy_config {
 };
 
 enum interface_select_pci {
-	INTF_SEL1_MINICARD,
-	INTF_SEL0_PCIE,
-	INTF_SEL2_RSV,
-	INTF_SEL3_RSV,
+	INTF_SEL1_MINICARD = 0,
+	INTF_SEL0_PCIE = 1,
+	INTF_SEL2_RSV = 2,
+	INTF_SEL3_RSV = 3,
 };
 
 enum hal_fw_c2h_cmd_id {
-	HAL_FW_C2H_CMD_Read_MACREG,
-	HAL_FW_C2H_CMD_Read_BBREG,
-	HAL_FW_C2H_CMD_Read_RFREG,
-	HAL_FW_C2H_CMD_Read_EEPROM,
-	HAL_FW_C2H_CMD_Read_EFUSE,
-	HAL_FW_C2H_CMD_Read_CAM,
-	HAL_FW_C2H_CMD_Get_BasicRate,
-	HAL_FW_C2H_CMD_Get_DataRate,
-	HAL_FW_C2H_CMD_Survey,
-	HAL_FW_C2H_CMD_SurveyDone,
-	HAL_FW_C2H_CMD_JoinBss,
-	HAL_FW_C2H_CMD_AddSTA,
-	HAL_FW_C2H_CMD_DelSTA,
-	HAL_FW_C2H_CMD_AtimDone,
-	HAL_FW_C2H_CMD_TX_Report,
-	HAL_FW_C2H_CMD_CCX_Report,
-	HAL_FW_C2H_CMD_DTM_Report,
-	HAL_FW_C2H_CMD_TX_Rate_Statistics,
-	HAL_FW_C2H_CMD_C2HLBK,
-	HAL_FW_C2H_CMD_C2HDBG,
-	HAL_FW_C2H_CMD_C2HFEEDBACK,
+	HAL_FW_C2H_CMD_READ_MACREG = 0,
+	HAL_FW_C2H_CMD_READ_BBREG = 1,
+	HAL_FW_C2H_CMD_READ_RFREG = 2,
+	HAL_FW_C2H_CMD_READ_EEPROM = 3,
+	HAL_FW_C2H_CMD_READ_EFUSE = 4,
+	HAL_FW_C2H_CMD_READ_CAM = 5,
+	HAL_FW_C2H_CMD_GET_BASICRATE = 6,
+	HAL_FW_C2H_CMD_GET_DATARATE = 7,
+	HAL_FW_C2H_CMD_SURVEY = 8,
+	HAL_FW_C2H_CMD_SURVEYDONE = 9,
+	HAL_FW_C2H_CMD_JOINBSS = 10,
+	HAL_FW_C2H_CMD_ADDSTA = 11,
+	HAL_FW_C2H_CMD_DELSTA = 12,
+	HAL_FW_C2H_CMD_ATIMDONE = 13,
+	HAL_FW_C2H_CMD_TX_REPORT = 14,
+	HAL_FW_C2H_CMD_CCX_REPORT = 15,
+	HAL_FW_C2H_CMD_DTM_REPORT = 16,
+	HAL_FW_C2H_CMD_TX_RATE_STATISTICS = 17,
+	HAL_FW_C2H_CMD_C2HLBK = 18,
+	HAL_FW_C2H_CMD_C2HDBG = 19,
+	HAL_FW_C2H_CMD_C2HFEEDBACK = 20,
 	HAL_FW_C2H_CMD_MAX
 };
 
-enum wake_on_wlan_mode {
-	ewowlandisable,
-	ewakeonmagicpacketonly,
-	ewakeonpatternmatchonly,
-	ewakeonbothtypepacket
-};
-
 enum rtl_desc_qsel {
 	QSLT_BK = 0x2,
 	QSLT_BE = 0x0,
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
index f8daa61cf1c3..2aa34d9055f0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -188,21 +184,24 @@ static void rtl88e_set_iqk_matrix(struct ieee80211_hw *hw,
 		switch (rfpath) {
 		case RF90_PATH_A:
 			value32 = (ele_d << 22)|((ele_c & 0x3F)<<16) | ele_a;
-			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, MASKDWORD,
-				      value32);
+			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+				      MASKDWORD, value32);
 			value32 = (ele_c & 0x000003C0) >> 6;
-			rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS, value32);
+			rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS,
+				      value32);
 			value32 = ((iqk_result_x * ele_d) >> 7) & 0x01;
-			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(24), value32);
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(24),
+				      value32);
 			break;
 		case RF90_PATH_B:
 			value32 = (ele_d << 22)|((ele_c & 0x3F)<<16) | ele_a;
-			rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBAL,
-				      MASKDWORD, value32);
+			rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, MASKDWORD,
+				      value32);
 			value32 = (ele_c & 0x000003C0) >> 6;
 			rtl_set_bbreg(hw, ROFDM0_XDTXAFE, MASKH4BITS, value32);
 			value32 = ((iqk_result_x * ele_d) >> 7) & 0x01;
-			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(28), value32);
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(28),
+				      value32);
 			break;
 		default:
 			break;
@@ -210,16 +209,20 @@ static void rtl88e_set_iqk_matrix(struct ieee80211_hw *hw,
 	} else {
 		switch (rfpath) {
 		case RF90_PATH_A:
-			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, MASKDWORD,
-				      ofdmswing_table[ofdm_index]);
-			rtl_set_bbreg(hw, ROFDM0_XCTXAFE, MASKH4BITS, 0x00);
-			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(24), 0x00);
+			rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
+				      MASKDWORD, ofdmswing_table[ofdm_index]);
+			rtl_set_bbreg(hw, ROFDM0_XCTXAFE,
+				      MASKH4BITS, 0x00);
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+				      BIT(24), 0x00);
 			break;
 		case RF90_PATH_B:
-			rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBAL, MASKDWORD,
-				      ofdmswing_table[ofdm_index]);
-			rtl_set_bbreg(hw, ROFDM0_XDTXAFE, MASKH4BITS, 0x00);
-			rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(28), 0x00);
+			rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
+				      MASKDWORD, ofdmswing_table[ofdm_index]);
+			rtl_set_bbreg(hw, ROFDM0_XDTXAFE,
+				      MASKH4BITS, 0x00);
+			rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD,
+				      BIT(28), 0x00);
 			break;
 		default:
 			break;
@@ -244,7 +247,7 @@ void rtl88e_dm_txpower_track_adjust(struct ieee80211_hw *hw,
 			pwr_val = ofdm_base - ofdm_val;
 		} else {
 			*pdirection = 2;
-			pwr_val = ofdm_val - ofdm_base;
+			pwr_val = ofdm_base - ofdm_val;
 		}
 	} else if (type == 1) {
 		if (cck_val <= cck_base) {
@@ -263,46 +266,75 @@ void rtl88e_dm_txpower_track_adjust(struct ieee80211_hw *hw,
 			 (pwr_val << 24);
 }
 
-
-static void rtl88e_chk_tx_track(struct ieee80211_hw *hw,
-				enum pwr_track_control_method method,
-				u8 rfpath, u8 index)
+static void dm_tx_pwr_track_set_pwr(struct ieee80211_hw *hw,
+				    enum pwr_track_control_method method,
+				    u8 rfpath, u8 channel_mapped_index)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	int jj = rtldm->swing_idx_cck;
-	int i;
 
 	if (method == TXAGC) {
-		if (rtldm->swing_flag_ofdm == true ||
-		    rtldm->swing_flag_cck == true) {
-			u8 chan = rtlphy->current_channel;
-			rtl88e_phy_set_txpower_level(hw, chan);
+		if (rtldm->swing_flag_ofdm ||
+		    rtldm->swing_flag_cck) {
+			rtl88e_phy_set_txpower_level(hw,
+						     rtlphy->current_channel);
 			rtldm->swing_flag_ofdm = false;
 			rtldm->swing_flag_cck = false;
 		}
 	} else if (method == BBSWING) {
 		if (!rtldm->cck_inch14) {
-			for (i = 0; i < 8; i++)
-				rtl_write_byte(rtlpriv, 0xa22 + i,
-					       cck_tbl_ch1_13[jj][i]);
+			rtl_write_byte(rtlpriv, 0xa22,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][0]);
+			rtl_write_byte(rtlpriv, 0xa23,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][1]);
+			rtl_write_byte(rtlpriv, 0xa24,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][2]);
+			rtl_write_byte(rtlpriv, 0xa25,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][3]);
+			rtl_write_byte(rtlpriv, 0xa26,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][4]);
+			rtl_write_byte(rtlpriv, 0xa27,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][5]);
+			rtl_write_byte(rtlpriv, 0xa28,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][6]);
+			rtl_write_byte(rtlpriv, 0xa29,
+				       cck_tbl_ch1_13[rtldm->swing_idx_cck][7]);
 		} else {
-			for (i = 0; i < 8; i++)
-				rtl_write_byte(rtlpriv, 0xa22 + i,
-					       cck_tbl_ch14[jj][i]);
+			rtl_write_byte(rtlpriv, 0xa22,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][0]);
+			rtl_write_byte(rtlpriv, 0xa23,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][1]);
+			rtl_write_byte(rtlpriv, 0xa24,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][2]);
+			rtl_write_byte(rtlpriv, 0xa25,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][3]);
+			rtl_write_byte(rtlpriv, 0xa26,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][4]);
+			rtl_write_byte(rtlpriv, 0xa27,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][5]);
+			rtl_write_byte(rtlpriv, 0xa28,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][6]);
+			rtl_write_byte(rtlpriv, 0xa29,
+				       cck_tbl_ch14[rtldm->swing_idx_cck][7]);
 		}
 
 		if (rfpath == RF90_PATH_A) {
-			long x = rtlphy->iqk_matrix[index].value[0][0];
-			long y = rtlphy->iqk_matrix[index].value[0][1];
-			u8 indx = rtldm->swing_idx_ofdm[rfpath];
-			rtl88e_set_iqk_matrix(hw, indx, rfpath, x, y);
+			rtl88e_set_iqk_matrix(hw, rtldm->swing_idx_ofdm[rfpath],
+					      rfpath, rtlphy->iqk_matrix
+					      [channel_mapped_index].
+					      value[0][0],
+					      rtlphy->iqk_matrix
+					      [channel_mapped_index].
+					      value[0][1]);
 		} else if (rfpath == RF90_PATH_B) {
-			u8 indx = rtldm->swing_idx_ofdm[rfpath];
-			long x = rtlphy->iqk_matrix[indx].value[0][4];
-			long y = rtlphy->iqk_matrix[indx].value[0][5];
-			rtl88e_set_iqk_matrix(hw, indx, rfpath, x, y);
+			rtl88e_set_iqk_matrix(hw, rtldm->swing_idx_ofdm[rfpath],
+					      rfpath, rtlphy->iqk_matrix
+					      [channel_mapped_index].
+					      value[0][4],
+					      rtlphy->iqk_matrix
+					      [channel_mapped_index].
+					      value[0][5]);
 		}
 	} else {
 		return;
@@ -317,7 +349,7 @@ static void rtl88e_dm_diginit(struct ieee80211_hw *hw)
 	dm_dig->dig_enable_flag = true;
 	dm_dig->cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
 	dm_dig->pre_igvalue = 0;
-	dm_dig->cursta_cstate = DIG_STA_DISCONNECT;
+	dm_dig->cur_sta_cstate = DIG_STA_DISCONNECT;
 	dm_dig->presta_cstate = DIG_STA_DISCONNECT;
 	dm_dig->curmultista_cstate = DIG_MULTISTA_DISCONNECT;
 	dm_dig->rssi_lowthresh = DM_DIG_THRESH_LOW;
@@ -348,22 +380,23 @@ static u8 rtl88e_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
 	long rssi_val_min = 0;
 
 	if ((dm_dig->curmultista_cstate == DIG_MULTISTA_CONNECT) &&
-	    (dm_dig->cursta_cstate == DIG_STA_CONNECT)) {
+	    (dm_dig->cur_sta_cstate == DIG_STA_CONNECT)) {
 		if (rtlpriv->dm.entry_min_undec_sm_pwdb != 0)
 			rssi_val_min =
 			    (rtlpriv->dm.entry_min_undec_sm_pwdb >
-			    rtlpriv->dm.undec_sm_pwdb) ?
+			     rtlpriv->dm.undec_sm_pwdb) ?
 			    rtlpriv->dm.undec_sm_pwdb :
 			    rtlpriv->dm.entry_min_undec_sm_pwdb;
 		else
 			rssi_val_min = rtlpriv->dm.undec_sm_pwdb;
-	} else if (dm_dig->cursta_cstate == DIG_STA_CONNECT ||
-		   dm_dig->cursta_cstate == DIG_STA_BEFORE_CONNECT) {
+	} else if (dm_dig->cur_sta_cstate == DIG_STA_CONNECT ||
+		   dm_dig->cur_sta_cstate == DIG_STA_BEFORE_CONNECT) {
 		rssi_val_min = rtlpriv->dm.undec_sm_pwdb;
 	} else if (dm_dig->curmultista_cstate ==
 		DIG_MULTISTA_CONNECT) {
 		rssi_val_min = rtlpriv->dm.entry_min_undec_sm_pwdb;
 	}
+
 	return (u8)rssi_val_min;
 }
 
@@ -371,57 +404,58 @@ static void rtl88e_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 {
 	u32 ret_value;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct false_alarm_statistics *alm_cnt = &(rtlpriv->falsealm_cnt);
+	struct false_alarm_statistics *falsealm_cnt = &rtlpriv->falsealm_cnt;
 
 	rtl_set_bbreg(hw, ROFDM0_LSTF, BIT(31), 1);
 	rtl_set_bbreg(hw, ROFDM1_LSTF, BIT(31), 1);
 
 	ret_value = rtl_get_bbreg(hw, ROFDM0_FRAMESYNC, MASKDWORD);
-	alm_cnt->cnt_fast_fsync_fail = (ret_value&0xffff);
-	alm_cnt->cnt_sb_search_fail = ((ret_value&0xffff0000)>>16);
+	falsealm_cnt->cnt_fast_fsync_fail = (ret_value&0xffff);
+	falsealm_cnt->cnt_sb_search_fail = ((ret_value&0xffff0000)>>16);
 
 	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER1, MASKDWORD);
-	alm_cnt->cnt_ofdm_cca = (ret_value&0xffff);
-	alm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
+	falsealm_cnt->cnt_ofdm_cca = (ret_value&0xffff);
+	falsealm_cnt->cnt_parity_fail = ((ret_value & 0xffff0000) >> 16);
 
 	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER2, MASKDWORD);
-	alm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
-	alm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
+	falsealm_cnt->cnt_rate_illegal = (ret_value & 0xffff);
+	falsealm_cnt->cnt_crc8_fail = ((ret_value & 0xffff0000) >> 16);
 
 	ret_value = rtl_get_bbreg(hw, ROFDM_PHYCOUNTER3, MASKDWORD);
-	alm_cnt->cnt_mcs_fail = (ret_value & 0xffff);
-	alm_cnt->cnt_ofdm_fail = alm_cnt->cnt_parity_fail +
-				 alm_cnt->cnt_rate_illegal +
-				 alm_cnt->cnt_crc8_fail +
-				 alm_cnt->cnt_mcs_fail +
-				 alm_cnt->cnt_fast_fsync_fail +
-				 alm_cnt->cnt_sb_search_fail;
+	falsealm_cnt->cnt_mcs_fail = (ret_value & 0xffff);
+	falsealm_cnt->cnt_ofdm_fail = falsealm_cnt->cnt_parity_fail +
+		falsealm_cnt->cnt_rate_illegal +
+		falsealm_cnt->cnt_crc8_fail +
+		falsealm_cnt->cnt_mcs_fail +
+		falsealm_cnt->cnt_fast_fsync_fail +
+		falsealm_cnt->cnt_sb_search_fail;
 
 	ret_value = rtl_get_bbreg(hw, REG_SC_CNT, MASKDWORD);
-	alm_cnt->cnt_bw_lsc = (ret_value & 0xffff);
-	alm_cnt->cnt_bw_usc = ((ret_value & 0xffff0000) >> 16);
+	falsealm_cnt->cnt_bw_lsc = (ret_value & 0xffff);
+	falsealm_cnt->cnt_bw_usc = ((ret_value & 0xffff0000) >> 16);
 
 	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(12), 1);
 	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(14), 1);
 
 	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERLOWER, MASKBYTE0);
-	alm_cnt->cnt_cck_fail = ret_value;
+	falsealm_cnt->cnt_cck_fail = ret_value;
 
 	ret_value = rtl_get_bbreg(hw, RCCK0_FACOUNTERUPPER, MASKBYTE3);
-	alm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
+	falsealm_cnt->cnt_cck_fail += (ret_value & 0xff) << 8;
 
 	ret_value = rtl_get_bbreg(hw, RCCK0_CCA_CNT, MASKDWORD);
-	alm_cnt->cnt_cck_cca = ((ret_value & 0xff) << 8) |
-				((ret_value&0xFF00)>>8);
-
-	alm_cnt->cnt_all = alm_cnt->cnt_fast_fsync_fail +
-			   alm_cnt->cnt_sb_search_fail +
-			   alm_cnt->cnt_parity_fail +
-			   alm_cnt->cnt_rate_illegal +
-			   alm_cnt->cnt_crc8_fail +
-			   alm_cnt->cnt_mcs_fail +
-			   alm_cnt->cnt_cck_fail;
-	alm_cnt->cnt_cca_all = alm_cnt->cnt_ofdm_cca + alm_cnt->cnt_cck_cca;
+	falsealm_cnt->cnt_cck_cca = ((ret_value & 0xff) << 8) |
+		((ret_value&0xFF00)>>8);
+
+	falsealm_cnt->cnt_all = (falsealm_cnt->cnt_fast_fsync_fail +
+				falsealm_cnt->cnt_sb_search_fail +
+				falsealm_cnt->cnt_parity_fail +
+				falsealm_cnt->cnt_rate_illegal +
+				falsealm_cnt->cnt_crc8_fail +
+				falsealm_cnt->cnt_mcs_fail +
+				falsealm_cnt->cnt_cck_fail);
+	falsealm_cnt->cnt_cca_all = falsealm_cnt->cnt_ofdm_cca +
+		falsealm_cnt->cnt_cck_cca;
 
 	rtl_set_bbreg(hw, ROFDM0_TRSWISOLATION, BIT(31), 1);
 	rtl_set_bbreg(hw, ROFDM0_TRSWISOLATION, BIT(31), 0);
@@ -435,16 +469,15 @@ static void rtl88e_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, BIT(15)|BIT(14), 2);
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, "
-		 "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
-		 alm_cnt->cnt_parity_fail,
-		 alm_cnt->cnt_rate_illegal,
-		 alm_cnt->cnt_crc8_fail, alm_cnt->cnt_mcs_fail);
+		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+		 falsealm_cnt->cnt_parity_fail,
+		 falsealm_cnt->cnt_rate_illegal,
+		 falsealm_cnt->cnt_crc8_fail, falsealm_cnt->cnt_mcs_fail);
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
 		 "cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
-		 alm_cnt->cnt_ofdm_fail,
-		 alm_cnt->cnt_cck_fail, alm_cnt->cnt_all);
+		 falsealm_cnt->cnt_ofdm_fail,
+		 falsealm_cnt->cnt_cck_fail, falsealm_cnt->cnt_all);
 }
 
 static void rtl88e_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
@@ -453,7 +486,7 @@ static void rtl88e_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 	u8 cur_cck_cca_thresh;
 
-	if (dm_dig->cursta_cstate == DIG_STA_CONNECT) {
+	if (dm_dig->cur_sta_cstate == DIG_STA_CONNECT) {
 		dm_dig->rssi_val_min = rtl88e_dm_initial_gain_min_pwdb(hw);
 		if (dm_dig->rssi_val_min > 25) {
 			cur_cck_cca_thresh = 0xcd;
@@ -486,10 +519,10 @@ static void rtl88e_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 static void rtl88e_dm_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 dig_min, dig_maxofmin;
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+	u8 dig_dynamic_min, dig_maxofmin;
 	bool bfirstconnect;
 	u8 dm_dig_max, dm_dig_min;
 	u8 current_igi = dm_dig->cur_igvalue;
@@ -502,19 +535,19 @@ static void rtl88e_dm_dig(struct ieee80211_hw *hw)
 		return;
 
 	if (mac->link_state >= MAC80211_LINKED)
-		dm_dig->cursta_cstate = DIG_STA_CONNECT;
+		dm_dig->cur_sta_cstate = DIG_STA_CONNECT;
 	else
-		dm_dig->cursta_cstate = DIG_STA_DISCONNECT;
+		dm_dig->cur_sta_cstate = DIG_STA_DISCONNECT;
 	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
 	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC)
-		dm_dig->cursta_cstate = DIG_STA_DISCONNECT;
+		dm_dig->cur_sta_cstate = DIG_STA_DISCONNECT;
 
 	dm_dig_max = DM_DIG_MAX;
 	dm_dig_min = DM_DIG_MIN;
 	dig_maxofmin = DM_DIG_MAX_AP;
-	dig_min = dm_dig->dig_min_0;
+	dig_dynamic_min = dm_dig->dig_min_0;
 	bfirstconnect = ((mac->link_state >= MAC80211_LINKED) ? true : false) &&
-			 (dm_dig->media_connect_0 == false);
+			 !dm_dig->media_connect_0;
 
 	dm_dig->rssi_val_min =
 		rtl88e_dm_initial_gain_min_pwdb(hw);
@@ -528,18 +561,18 @@ static void rtl88e_dm_dig(struct ieee80211_hw *hw)
 			dm_dig->rx_gain_max = dm_dig->rssi_val_min + 20;
 
 		if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) {
-			dig_min  = dm_dig->antdiv_rssi_max;
+			dig_dynamic_min  = dm_dig->antdiv_rssi_max;
 		} else {
 			if (dm_dig->rssi_val_min < dm_dig_min)
-				dig_min = dm_dig_min;
+				dig_dynamic_min = dm_dig_min;
 			else if (dm_dig->rssi_val_min < dig_maxofmin)
-				dig_min = dig_maxofmin;
+				dig_dynamic_min = dig_maxofmin;
 			else
-				dig_min = dm_dig->rssi_val_min;
+				dig_dynamic_min = dm_dig->rssi_val_min;
 		}
 	} else {
 		dm_dig->rx_gain_max = dm_dig_max;
-		dig_min = dm_dig_min;
+		dig_dynamic_min = dm_dig_min;
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "no link\n");
 	}
 
@@ -551,10 +584,13 @@ static void rtl88e_dm_dig(struct ieee80211_hw *hw)
 		}
 
 		if (dm_dig->large_fa_hit >= 3) {
-			if ((dm_dig->forbidden_igi + 1) > dm_dig->rx_gain_max)
-				dm_dig->rx_gain_min = dm_dig->rx_gain_max;
+			if ((dm_dig->forbidden_igi + 1) >
+				dm_dig->rx_gain_max)
+				dm_dig->rx_gain_min =
+					dm_dig->rx_gain_max;
 			else
-				dm_dig->rx_gain_min = dm_dig->forbidden_igi + 1;
+				dm_dig->rx_gain_min =
+					dm_dig->forbidden_igi + 1;
 			dm_dig->recover_cnt = 3600;
 		}
 	} else {
@@ -562,13 +598,14 @@ static void rtl88e_dm_dig(struct ieee80211_hw *hw)
 			dm_dig->recover_cnt--;
 		} else {
 			if (dm_dig->large_fa_hit == 0) {
-				if ((dm_dig->forbidden_igi - 1) < dig_min) {
-					dm_dig->forbidden_igi = dig_min;
-					dm_dig->rx_gain_min = dig_min;
+				if ((dm_dig->forbidden_igi - 1) <
+				    dig_dynamic_min) {
+					dm_dig->forbidden_igi = dig_dynamic_min;
+					dm_dig->rx_gain_min = dig_dynamic_min;
 				} else {
 					dm_dig->forbidden_igi--;
 					dm_dig->rx_gain_min =
-						 dm_dig->forbidden_igi + 1;
+						dm_dig->forbidden_igi + 1;
 				}
 			} else if (dm_dig->large_fa_hit == 3) {
 				dm_dig->large_fa_hit = 0;
@@ -576,7 +613,7 @@ static void rtl88e_dm_dig(struct ieee80211_hw *hw)
 		}
 	}
 
-	if (dm_dig->cursta_cstate == DIG_STA_CONNECT) {
+	if (dm_dig->cur_sta_cstate == DIG_STA_CONNECT) {
 		if (bfirstconnect) {
 			current_igi = dm_dig->rssi_val_min;
 		} else {
@@ -606,9 +643,9 @@ static void rtl88e_dm_dig(struct ieee80211_hw *hw)
 
 	dm_dig->cur_igvalue = current_igi;
 	rtl88e_dm_write_dig(hw);
-	dm_dig->media_connect_0 = ((mac->link_state >= MAC80211_LINKED) ?
-				    true : false);
-	dm_dig->dig_min_0 = dig_min;
+	dm_dig->media_connect_0 =
+		((mac->link_state >= MAC80211_LINKED) ? true : false);
+	dm_dig->dig_min_0 = dig_dynamic_min;
 
 	rtl88e_dm_cck_packet_detection_thresh(hw);
 }
@@ -626,7 +663,7 @@ static void rtl88e_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
 static void rtl92c_dm_dynamic_txpower(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	long undec_sm_pwdb;
 
@@ -641,7 +678,7 @@ static void rtl92c_dm_dynamic_txpower(struct ieee80211_hw *hw)
 	if ((mac->link_state < MAC80211_LINKED) &&
 	    (rtlpriv->dm.entry_min_undec_sm_pwdb == 0)) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
-			 "Not connected\n");
+			 "Not connected to any\n");
 
 		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
 
@@ -664,10 +701,12 @@ static void rtl92c_dm_dynamic_txpower(struct ieee80211_hw *hw)
 				  undec_sm_pwdb);
 		}
 	} else {
-		undec_sm_pwdb = rtlpriv->dm.entry_min_undec_sm_pwdb;
+		undec_sm_pwdb =
+		    rtlpriv->dm.entry_min_undec_sm_pwdb;
 
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
-			 "AP Ext Port PWDB = 0x%lx\n", undec_sm_pwdb);
+			 "AP Ext Port PWDB = 0x%lx\n",
+			  undec_sm_pwdb);
 	}
 
 	if (undec_sm_pwdb >= TX_POWER_NEAR_FIELD_THRESH_LVL2) {
@@ -676,17 +715,20 @@ static void rtl92c_dm_dynamic_txpower(struct ieee80211_hw *hw)
 			 "TXHIGHPWRLEVEL_LEVEL1 (TxPwr = 0x0)\n");
 	} else if ((undec_sm_pwdb <
 		    (TX_POWER_NEAR_FIELD_THRESH_LVL2 - 3)) &&
-		   (undec_sm_pwdb >= TX_POWER_NEAR_FIELD_THRESH_LVL1)) {
+		   (undec_sm_pwdb >=
+		    TX_POWER_NEAR_FIELD_THRESH_LVL1)) {
 		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_LEVEL1;
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "TXHIGHPWRLEVEL_LEVEL1 (TxPwr = 0x10)\n");
-	} else if (undec_sm_pwdb < (TX_POWER_NEAR_FIELD_THRESH_LVL1 - 5)) {
+	} else if (undec_sm_pwdb <
+		   (TX_POWER_NEAR_FIELD_THRESH_LVL1 - 5)) {
 		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "TXHIGHPWRLEVEL_NORMAL\n");
 	}
 
-	if ((rtlpriv->dm.dynamic_txhighpower_lvl != rtlpriv->dm.last_dtp_lvl)) {
+	if ((rtlpriv->dm.dynamic_txhighpower_lvl !=
+		rtlpriv->dm.last_dtp_lvl)) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "PHY_SetTxPowerLevel8192S() Channel = %d\n",
 			  rtlphy->current_channel);
@@ -702,10 +744,9 @@ void rtl88e_dm_write_dig(struct ieee80211_hw *hw)
 	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
-		 "cur_igvalue = 0x%x, "
-		  "pre_igvalue = 0x%x, back_val = %d\n",
-		  dm_dig->cur_igvalue, dm_dig->pre_igvalue,
-		  dm_dig->back_val);
+		 "cur_igvalue = 0x%x, pre_igvalue = 0x%x, backoff_val = %d\n",
+		 dm_dig->cur_igvalue, dm_dig->pre_igvalue,
+		 dm_dig->back_val);
 
 	if (dm_dig->cur_igvalue > 0x3f)
 		dm_dig->cur_igvalue = 0x3f;
@@ -722,17 +763,19 @@ static void rtl88e_dm_pwdb_monitor(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_sta_info *drv_priv;
-	static u64 last_txok;
-	static u64 last_rx;
+	static u64 last_record_txok_cnt;
+	static u64 last_record_rxok_cnt;
 	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
 
 	if (rtlhal->oem_id == RT_CID_819X_HP) {
 		u64 cur_txok_cnt = 0;
 		u64 cur_rxok_cnt = 0;
-		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok;
-		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rx;
-		last_txok = cur_txok_cnt;
-		last_rx = cur_rxok_cnt;
+		cur_txok_cnt = rtlpriv->stats.txbytesunicast -
+			last_record_txok_cnt;
+		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast -
+			last_record_rxok_cnt;
+		last_record_txok_cnt = cur_txok_cnt;
+		last_record_rxok_cnt = cur_rxok_cnt;
 
 		if (cur_rxok_cnt > (cur_txok_cnt * 6))
 			rtl_write_dword(rtlpriv, REG_ARFR0, 0x8f015);
@@ -743,9 +786,11 @@ static void rtl88e_dm_pwdb_monitor(struct ieee80211_hw *hw)
 	/* AP & ADHOC & MESH */
 	spin_lock_bh(&rtlpriv->locks.entry_list_lock);
 	list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
-		if (drv_priv->rssi_stat.undec_sm_pwdb < tmp_entry_min_pwdb)
+		if (drv_priv->rssi_stat.undec_sm_pwdb <
+			tmp_entry_min_pwdb)
 			tmp_entry_min_pwdb = drv_priv->rssi_stat.undec_sm_pwdb;
-		if (drv_priv->rssi_stat.undec_sm_pwdb > tmp_entry_max_pwdb)
+		if (drv_priv->rssi_stat.undec_sm_pwdb >
+			tmp_entry_max_pwdb)
 			tmp_entry_max_pwdb = drv_priv->rssi_stat.undec_sm_pwdb;
 	}
 	spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
@@ -762,13 +807,19 @@ static void rtl88e_dm_pwdb_monitor(struct ieee80211_hw *hw)
 	if (tmp_entry_min_pwdb != 0xff) {
 		rtlpriv->dm.entry_min_undec_sm_pwdb = tmp_entry_min_pwdb;
 		RTPRINT(rtlpriv, FDM, DM_PWDB, "EntryMinPWDB = 0x%lx(%ld)\n",
-			tmp_entry_min_pwdb, tmp_entry_min_pwdb);
+					tmp_entry_min_pwdb, tmp_entry_min_pwdb);
 	} else {
 		rtlpriv->dm.entry_min_undec_sm_pwdb = 0;
 	}
 	/* Indicate Rx signal strength to FW. */
-	if (!rtlpriv->dm.useramask)
+	if (rtlpriv->dm.useramask) {
+		u8 h2c_parameter[3] = { 0 };
+
+		h2c_parameter[2] = (u8)(rtlpriv->dm.undec_sm_pwdb & 0xFF);
+		h2c_parameter[0] = 0x20;
+	} else {
 		rtl_write_byte(rtlpriv, 0x4fe, rtlpriv->dm.undec_sm_pwdb);
+	}
 }
 
 void rtl88e_dm_init_edca_turbo(struct ieee80211_hw *hw)
@@ -783,7 +834,6 @@ void rtl88e_dm_init_edca_turbo(struct ieee80211_hw *hw)
 static void rtl88e_dm_check_edca_turbo(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	static u64 last_txok_cnt;
 	static u64 last_rxok_cnt;
@@ -793,40 +843,33 @@ static void rtl88e_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	u64 cur_rxok_cnt = 0;
 	u32 edca_be_ul = 0x5ea42b;
 	u32 edca_be_dl = 0x5ea42b;
-	bool change_edca = false;
+	bool bt_change_edca = false;
 
-	if ((last_bt_edca_ul != rtlpcipriv->bt_coexist.bt_edca_ul) ||
-	    (last_bt_edca_dl != rtlpcipriv->bt_coexist.bt_edca_dl)) {
+	if ((last_bt_edca_ul != rtlpriv->btcoexist.bt_edca_ul) ||
+	    (last_bt_edca_dl != rtlpriv->btcoexist.bt_edca_dl)) {
 		rtlpriv->dm.current_turbo_edca = false;
-		last_bt_edca_ul = rtlpcipriv->bt_coexist.bt_edca_ul;
-		last_bt_edca_dl = rtlpcipriv->bt_coexist.bt_edca_dl;
+		last_bt_edca_ul = rtlpriv->btcoexist.bt_edca_ul;
+		last_bt_edca_dl = rtlpriv->btcoexist.bt_edca_dl;
 	}
 
-	if (rtlpcipriv->bt_coexist.bt_edca_ul != 0) {
-		edca_be_ul = rtlpcipriv->bt_coexist.bt_edca_ul;
-		change_edca = true;
+	if (rtlpriv->btcoexist.bt_edca_ul != 0) {
+		edca_be_ul = rtlpriv->btcoexist.bt_edca_ul;
+		bt_change_edca = true;
 	}
 
-	if (rtlpcipriv->bt_coexist.bt_edca_dl != 0) {
-		edca_be_ul = rtlpcipriv->bt_coexist.bt_edca_dl;
-		change_edca = true;
+	if (rtlpriv->btcoexist.bt_edca_dl != 0) {
+		edca_be_ul = rtlpriv->btcoexist.bt_edca_dl;
+		bt_change_edca = true;
 	}
 
 	if (mac->link_state != MAC80211_LINKED) {
 		rtlpriv->dm.current_turbo_edca = false;
 		return;
 	}
+	if ((bt_change_edca) ||
+	    ((!rtlpriv->dm.is_any_nonbepkts) &&
+	     (!rtlpriv->dm.disable_framebursting))) {
 
-	if ((!mac->ht_enable) && (!rtlpcipriv->bt_coexist.bt_coexistence)) {
-		if (!(edca_be_ul & 0xffff0000))
-			edca_be_ul |= 0x005e0000;
-
-		if (!(edca_be_dl & 0xffff0000))
-			edca_be_dl |= 0x005e0000;
-	}
-
-	if ((change_edca) || ((!rtlpriv->dm.is_any_nonbepkts) &&
-			      (!rtlpriv->dm.disable_framebursting))) {
 		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
 		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
 
@@ -851,7 +894,9 @@ static void rtl88e_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	} else {
 		if (rtlpriv->dm.current_turbo_edca) {
 			u8 tmp = AC0_BE;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_AC_PARAM,
 						      &tmp);
 			rtlpriv->dm.current_turbo_edca = false;
 		}
@@ -862,29 +907,29 @@ static void rtl88e_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
 }
 
-static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
-							     *hw)
+static void dm_txpower_track_cb_therm(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 thermalvalue = 0, delta, delta_lck, delta_iqk, off;
-	u8 th_avg_cnt = 0;
+	u8 thermalvalue = 0, delta, delta_lck, delta_iqk, offset;
+	u8 thermalvalue_avg_count = 0;
 	u32 thermalvalue_avg = 0;
 	long  ele_d, temp_cck;
-	char ofdm_index[2], cck_index = 0, ofdm_old[2] = {0, 0}, cck_old = 0;
+	char ofdm_index[2], cck_index = 0,
+		ofdm_index_old[2] = {0, 0}, cck_index_old = 0;
 	int i = 0;
-	bool is2t = false;
+	/*bool is2t = false;*/
 
-	u8 ofdm_min_index = 6, rf = (is2t) ? 2 : 1;
-	u8 index_for_channel;
-	enum _dec_inc {dec, power_inc};
+	u8 ofdm_min_index = 6, rf = 1;
+	/*u8 index_for_channel;*/
+	enum _power_dec_inc {power_dec, power_inc};
 
-	/* 0.1 the following TWO tables decide the final index of
-	 * OFDM/CCK swing table
+	/*0.1 the following TWO tables decide the
+	 *final index of OFDM/CCK swing table
 	 */
-	char del_tbl_idx[2][15] = {
+	char delta_swing_table_idx[2][15]  = {
 		{0, 0, 2, 3, 4, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11},
 		{0, 0, -1, -2, -3, -4, -4, -4, -4, -5, -7, -8, -9, -9, -10}
 	};
@@ -896,9 +941,10 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 	/*Initilization (7 steps in total) */
 	rtlpriv->dm.txpower_trackinginit = true;
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "rtl88e_dm_txpower_tracking_callback_thermalmeter\n");
+		 "dm_txpower_track_cb_therm\n");
 
-	thermalvalue = (u8) rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER, 0xfc00);
+	thermalvalue = (u8)rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER,
+					 0xfc00);
 	if (!thermalvalue)
 		return;
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
@@ -907,55 +953,44 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 		 rtlefuse->eeprom_thermalmeter);
 
 	/*1. Query OFDM Default Setting: Path A*/
-	ele_d = rtl_get_bbreg(hw, ROFDM0_XATXIQIMBAL, MASKDWORD) & MASKOFDM_D;
+	ele_d = rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE, MASKDWORD) &
+			      MASKOFDM_D;
 	for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
 		if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
-			ofdm_old[0] = (u8) i;
-			rtldm->swing_idx_ofdm_base[0] = (u8)i;
+			ofdm_index_old[0] = (u8)i;
+			rtldm->swing_idx_ofdm_base[RF90_PATH_A] = (u8)i;
 			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 				 "Initial pathA ele_d reg0x%x = 0x%lx, ofdm_index = 0x%x\n",
-				 ROFDM0_XATXIQIMBAL,
-				 ele_d, ofdm_old[0]);
+				 ROFDM0_XATXIQIMBALANCE,
+				 ele_d, ofdm_index_old[0]);
 			break;
 		}
 	}
 
-	if (is2t) {
-		ele_d = rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBAL,
-				      MASKDWORD) & MASKOFDM_D;
-		for (i = 0; i < OFDM_TABLE_LENGTH; i++) {
-			if (ele_d == (ofdmswing_table[i] & MASKOFDM_D)) {
-				ofdm_old[1] = (u8)i;
-
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
-					 DBG_LOUD,
-					 "Initial pathB ele_d reg0x%x = 0x%lx, ofdm_index = 0x%x\n",
-					 ROFDM0_XBTXIQIMBAL, ele_d,
-					 ofdm_old[1]);
-				break;
-			}
-		}
-	}
 	/*2.Query CCK default setting From 0xa24*/
 	temp_cck = rtl_get_bbreg(hw, RCCK0_TXFILTER2, MASKDWORD) & MASKCCK;
 	for (i = 0; i < CCK_TABLE_LENGTH; i++) {
 		if (rtlpriv->dm.cck_inch14) {
 			if (memcmp(&temp_cck, &cck_tbl_ch14[i][2], 4) == 0) {
-				cck_old = (u8)i;
+				cck_index_old = (u8)i;
 				rtldm->swing_idx_cck_base = (u8)i;
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+					 DBG_LOUD,
 					 "Initial reg0x%x = 0x%lx, cck_index = 0x%x, ch 14 %d\n",
-					 RCCK0_TXFILTER2, temp_cck, cck_old,
+					 RCCK0_TXFILTER2, temp_cck,
+					 cck_index_old,
 					 rtlpriv->dm.cck_inch14);
 				break;
 			}
 		} else {
 			if (memcmp(&temp_cck, &cck_tbl_ch1_13[i][2], 4) == 0) {
-				cck_old = (u8)i;
+				cck_index_old = (u8)i;
 				rtldm->swing_idx_cck_base = (u8)i;
-				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+					 DBG_LOUD,
 					 "Initial reg0x%x = 0x%lx, cck_index = 0x%x, ch14 %d\n",
-					 RCCK0_TXFILTER2, temp_cck, cck_old,
+					 RCCK0_TXFILTER2, temp_cck,
+					 cck_index_old,
 					 rtlpriv->dm.cck_inch14);
 				break;
 			}
@@ -968,8 +1003,8 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 		rtlpriv->dm.thermalvalue_lck = thermalvalue;
 		rtlpriv->dm.thermalvalue_iqk = thermalvalue;
 		for (i = 0; i < rf; i++)
-			rtlpriv->dm.ofdm_index[i] = ofdm_old[i];
-		rtlpriv->dm.cck_index = cck_old;
+			rtlpriv->dm.ofdm_index[i] = ofdm_index_old[i];
+		rtlpriv->dm.cck_index = cck_index_old;
 	}
 
 	/*4 Calculate average thermal meter*/
@@ -981,12 +1016,12 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 	for (i = 0; i < AVG_THERMAL_NUM_88E; i++) {
 		if (rtldm->thermalvalue_avg[i]) {
 			thermalvalue_avg += rtldm->thermalvalue_avg[i];
-			th_avg_cnt++;
+			thermalvalue_avg_count++;
 		}
 	}
 
-	if (th_avg_cnt)
-		thermalvalue = (u8)(thermalvalue_avg / th_avg_cnt);
+	if (thermalvalue_avg_count)
+		thermalvalue = (u8)(thermalvalue_avg / thermalvalue_avg_count);
 
 	/* 5 Calculate delta, delta_LCK, delta_IQK.*/
 	if (rtlhal->reloadtxpowerindex) {
@@ -997,24 +1032,22 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 		rtlpriv->dm.done_txpower = false;
 	} else if (rtlpriv->dm.done_txpower) {
 		delta = (thermalvalue > rtlpriv->dm.thermalvalue) ?
-			(thermalvalue - rtlpriv->dm.thermalvalue) :
-			(rtlpriv->dm.thermalvalue - thermalvalue);
+		    (thermalvalue - rtlpriv->dm.thermalvalue) :
+		    (rtlpriv->dm.thermalvalue - thermalvalue);
 	} else {
 		delta = (thermalvalue > rtlefuse->eeprom_thermalmeter) ?
-			(thermalvalue - rtlefuse->eeprom_thermalmeter) :
-			(rtlefuse->eeprom_thermalmeter - thermalvalue);
+		    (thermalvalue - rtlefuse->eeprom_thermalmeter) :
+		    (rtlefuse->eeprom_thermalmeter - thermalvalue);
 	}
 	delta_lck = (thermalvalue > rtlpriv->dm.thermalvalue_lck) ?
-		    (thermalvalue - rtlpriv->dm.thermalvalue_lck) :
-		    (rtlpriv->dm.thermalvalue_lck - thermalvalue);
+	    (thermalvalue - rtlpriv->dm.thermalvalue_lck) :
+	    (rtlpriv->dm.thermalvalue_lck - thermalvalue);
 	delta_iqk = (thermalvalue > rtlpriv->dm.thermalvalue_iqk) ?
-		    (thermalvalue - rtlpriv->dm.thermalvalue_iqk) :
-		    (rtlpriv->dm.thermalvalue_iqk - thermalvalue);
+	    (thermalvalue - rtlpriv->dm.thermalvalue_iqk) :
+	    (rtlpriv->dm.thermalvalue_iqk - thermalvalue);
 
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
-		 "eeprom_thermalmeter 0x%x delta 0x%x "
-		 "delta_lck 0x%x delta_iqk 0x%x\n",
+		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x eeprom_thermalmeter 0x%x delta 0x%x delta_lck 0x%x delta_iqk 0x%x\n",
 		 thermalvalue, rtlpriv->dm.thermalvalue,
 		 rtlefuse->eeprom_thermalmeter, delta, delta_lck,
 		 delta_iqk);
@@ -1024,28 +1057,35 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 		rtl88e_phy_lc_calibrate(hw);
 	}
 
-	/* 7 If necessary, move the index of swing table to adjust Tx power. */
+	/* 7 If necessary, move the index of
+	 * swing table to adjust Tx power.
+	 */
 	if (delta > 0 && rtlpriv->dm.txpower_track_control) {
 		delta = (thermalvalue > rtlefuse->eeprom_thermalmeter) ?
-			(thermalvalue - rtlefuse->eeprom_thermalmeter) :
-			(rtlefuse->eeprom_thermalmeter - thermalvalue);
+		    (thermalvalue - rtlefuse->eeprom_thermalmeter) :
+		    (rtlefuse->eeprom_thermalmeter - thermalvalue);
 
 		/* 7.1 Get the final CCK_index and OFDM_index for each
 		 * swing table.
 		 */
 		if (thermalvalue > rtlefuse->eeprom_thermalmeter) {
-			CAL_SWING_OFF(off, power_inc, IDX_MAP, delta);
+			CAL_SWING_OFF(offset, power_inc, INDEX_MAPPING_NUM,
+				      delta);
 			for (i = 0; i < rf; i++)
-				ofdm_index[i] = rtldm->ofdm_index[i] +
-						del_tbl_idx[power_inc][off];
+				ofdm_index[i] =
+				  rtldm->ofdm_index[i] +
+				  delta_swing_table_idx[power_inc][offset];
 			cck_index = rtldm->cck_index +
-				    del_tbl_idx[power_inc][off];
+				delta_swing_table_idx[power_inc][offset];
 		} else {
-			CAL_SWING_OFF(off, dec, IDX_MAP, delta);
+			CAL_SWING_OFF(offset, power_dec, INDEX_MAPPING_NUM,
+				      delta);
 			for (i = 0; i < rf; i++)
-				ofdm_index[i] = rtldm->ofdm_index[i] +
-						del_tbl_idx[dec][off];
-			cck_index = rtldm->cck_index + del_tbl_idx[dec][off];
+				ofdm_index[i] =
+				  rtldm->ofdm_index[i] +
+				  delta_swing_table_idx[power_dec][offset];
+			cck_index = rtldm->cck_index +
+				delta_swing_table_idx[power_dec][offset];
 		}
 
 		/* 7.2 Handle boundary conditions of index.*/
@@ -1056,8 +1096,8 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 				ofdm_index[i] = ofdm_min_index;
 		}
 
-		if (cck_index > CCK_TABLE_SIZE - 1)
-			cck_index = CCK_TABLE_SIZE - 1;
+		if (cck_index > CCK_TABLE_SIZE-1)
+			cck_index = CCK_TABLE_SIZE-1;
 		else if (cck_index < 0)
 			cck_index = 0;
 
@@ -1065,10 +1105,7 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 		if (rtlpriv->dm.txpower_track_control) {
 			rtldm->done_txpower = true;
 			rtldm->swing_idx_ofdm[RF90_PATH_A] =
-				 (u8)ofdm_index[RF90_PATH_A];
-			if (is2t)
-				rtldm->swing_idx_ofdm[RF90_PATH_B] =
-					 (u8)ofdm_index[RF90_PATH_B];
+				(u8)ofdm_index[RF90_PATH_A];
 			rtldm->swing_idx_cck = cck_index;
 			if (rtldm->swing_idx_ofdm_cur !=
 			    rtldm->swing_idx_ofdm[0]) {
@@ -1082,12 +1119,7 @@ static void rtl88e_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw
 				rtldm->swing_flag_cck = true;
 			}
 
-			rtl88e_chk_tx_track(hw, TXAGC, 0, 0);
-
-			if (is2t)
-				rtl88e_chk_tx_track(hw, BBSWING,
-						    RF90_PATH_B,
-						    index_for_channel);
+			dm_tx_pwr_track_set_pwr(hw, TXAGC, 0, 0);
 		}
 	}
 
@@ -1115,7 +1147,7 @@ static void rtl88e_dm_init_txpower_tracking(struct ieee80211_hw *hw)
 	rtlpriv->dm.swing_idx_ofdm_cur = 12;
 	rtlpriv->dm.swing_flag_ofdm = false;
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "  rtlpriv->dm.txpower_tracking = %d\n",
+		 "rtlpriv->dm.txpower_tracking = %d\n",
 		 rtlpriv->dm.txpower_tracking);
 }
 
@@ -1137,7 +1169,7 @@ void rtl88e_dm_check_txpower_tracking(struct ieee80211_hw *hw)
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Schedule TxPowerTracking !!\n");
-		rtl88e_dm_txpower_tracking_callback_thermalmeter(hw);
+				dm_txpower_track_cb_therm(hw);
 		tm_trigger = 0;
 	}
 }
@@ -1145,7 +1177,7 @@ void rtl88e_dm_check_txpower_tracking(struct ieee80211_hw *hw)
 void rtl88e_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
 
 	p_ra->ratr_state = DM_RATR_STA_INIT;
 	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
@@ -1161,9 +1193,9 @@ static void rtl88e_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
+	u32 low_rssithresh_for_ra, high_rssithresh_for_ra;
 	struct ieee80211_sta *sta = NULL;
-	u32 low_rssi, hi_rssi;
 
 	if (is_hal_stop(rtlhal)) {
 		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
@@ -1181,26 +1213,28 @@ static void rtl88e_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 	    mac->opmode == NL80211_IFTYPE_STATION) {
 		switch (p_ra->pre_ratr_state) {
 		case DM_RATR_STA_HIGH:
-			hi_rssi = 50;
-			low_rssi = 20;
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 20;
 			break;
 		case DM_RATR_STA_MIDDLE:
-			hi_rssi = 55;
-			low_rssi = 20;
+			high_rssithresh_for_ra = 55;
+			low_rssithresh_for_ra = 20;
 			break;
 		case DM_RATR_STA_LOW:
-			hi_rssi = 50;
-			low_rssi = 25;
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 25;
 			break;
 		default:
-			hi_rssi = 50;
-			low_rssi = 20;
+			high_rssithresh_for_ra = 50;
+			low_rssithresh_for_ra = 20;
 			break;
 		}
 
-		if (rtlpriv->dm.undec_sm_pwdb > (long)hi_rssi)
+		if (rtlpriv->dm.undec_sm_pwdb >
+		    (long)high_rssithresh_for_ra)
 			p_ra->ratr_state = DM_RATR_STA_HIGH;
-		else if (rtlpriv->dm.undec_sm_pwdb > (long)low_rssi)
+		else if (rtlpriv->dm.undec_sm_pwdb >
+			 (long)low_rssithresh_for_ra)
 			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
 		else
 			p_ra->ratr_state = DM_RATR_STA_LOW;
@@ -1208,7 +1242,7 @@ static void rtl88e_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
 			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
 				 "RSSI = %ld\n",
-				 rtlpriv->dm.undec_sm_pwdb);
+				  rtlpriv->dm.undec_sm_pwdb);
 			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
 				 "RSSI_LEVEL = %d\n", p_ra->ratr_state);
 			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
@@ -1219,7 +1253,7 @@ static void rtl88e_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 			sta = rtl_find_sta(hw, mac->bssid);
 			if (sta)
 				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
-						   p_ra->ratr_state);
+								   p_ra->ratr_state);
 			rcu_read_unlock();
 
 			p_ra->pre_ratr_state = p_ra->ratr_state;
@@ -1239,56 +1273,62 @@ static void rtl92c_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
 	dm_pstable->rssi_val_min = 0;
 }
 
-static void rtl88e_dm_update_rx_idle_ant(struct ieee80211_hw *hw, u8 ant)
+static void rtl88e_dm_update_rx_idle_ant(struct ieee80211_hw *hw,
+					 u8 ant)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
-	u32 def_ant, opt_ant;
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
+	u32 default_ant, optional_ant;
 
-	if (fat_tbl->rx_idle_ant != ant) {
+	if (pfat_table->rx_idle_ant != ant) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 			 "need to update rx idle ant\n");
 		if (ant == MAIN_ANT) {
-			def_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
-				   MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
-			opt_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
-				   AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+			default_ant =
+			  (pfat_table->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+			  MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+			optional_ant =
+			  (pfat_table->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+			  AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
 		} else {
-			def_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
-				   AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
-			opt_ant = (fat_tbl->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
-				   MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+			default_ant =
+			   (pfat_table->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+			   AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+			optional_ant =
+			   (pfat_table->rx_idle_ant == CG_TRX_HW_ANTDIV) ?
+			   MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
 		}
 
 		if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) {
-			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(5) |
-				      BIT(4) | BIT(3), def_ant);
-			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) |
-				      BIT(7) | BIT(6), opt_ant);
-			rtl_set_bbreg(hw, DM_REG_ANTSEL_CTRL_11N, BIT(14) |
-				      BIT(13) | BIT(12), def_ant);
-			rtl_set_bbreg(hw, DM_REG_RESP_TX_11N, BIT(6) | BIT(7),
-				      def_ant);
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N,
+				      BIT(5) | BIT(4) | BIT(3), default_ant);
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N,
+				      BIT(8) | BIT(7) | BIT(6), optional_ant);
+			rtl_set_bbreg(hw, DM_REG_ANTSEL_CTRL_11N,
+				      BIT(14) | BIT(13) | BIT(12),
+				      default_ant);
+			rtl_set_bbreg(hw, DM_REG_RESP_TX_11N,
+				      BIT(6) | BIT(7), default_ant);
 		} else if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV) {
-			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(5) |
-				      BIT(4) | BIT(3), def_ant);
-			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) |
-				      BIT(7) | BIT(6), opt_ant);
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N,
+				      BIT(5) | BIT(4) | BIT(3), default_ant);
+			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N,
+				      BIT(8) | BIT(7) | BIT(6), optional_ant);
 		}
 	}
-	fat_tbl->rx_idle_ant = ant;
+	pfat_table->rx_idle_ant = ant;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "RxIdleAnt %s\n",
-		 ((ant == MAIN_ANT) ? ("MAIN_ANT") : ("AUX_ANT")));
+		 (ant == MAIN_ANT) ? ("MAIN_ANT") : ("AUX_ANT"));
 }
 
 static void rtl88e_dm_update_tx_ant(struct ieee80211_hw *hw,
-	u8 ant, u32 mac_id)
+				    u8 ant, u32 mac_id)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
 	u8 target_ant;
 
 	if (ant == MAIN_ANT)
@@ -1296,23 +1336,25 @@ static void rtl88e_dm_update_tx_ant(struct ieee80211_hw *hw,
 	else
 		target_ant = AUX_ANT_CG_TRX;
 
-	fat_tbl->antsel_a[mac_id] = target_ant & BIT(0);
-	fat_tbl->antsel_b[mac_id] = (target_ant & BIT(1)) >> 1;
-	fat_tbl->antsel_c[mac_id] = (target_ant & BIT(2)) >> 2;
+	pfat_table->antsel_a[mac_id] = target_ant & BIT(0);
+	pfat_table->antsel_b[mac_id] = (target_ant & BIT(1)) >> 1;
+	pfat_table->antsel_c[mac_id] = (target_ant & BIT(2)) >> 2;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "txfrominfo target ant %s\n",
-		 ((ant == MAIN_ANT) ? ("MAIN_ANT") : ("AUX_ANT")));
+		(ant == MAIN_ANT) ? ("MAIN_ANT") : ("AUX_ANT"));
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "antsel_tr_mux = 3'b%d%d%d\n",
-		 fat_tbl->antsel_c[mac_id],
-		 fat_tbl->antsel_b[mac_id], fat_tbl->antsel_a[mac_id]);
+		pfat_table->antsel_c[mac_id],
+		pfat_table->antsel_b[mac_id],
+		pfat_table->antsel_a[mac_id]);
 }
 
 static void rtl88e_dm_rx_hw_antena_div_init(struct ieee80211_hw *hw)
 {
 	u32  value32;
+
 	/*MAC Setting*/
 	value32 = rtl_get_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD);
-	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD, value32 |
-		     (BIT(23) | BIT(25)));
+	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N,
+		      MASKDWORD, value32 | (BIT(23) | BIT(25)));
 	/*Pin Setting*/
 	rtl_set_bbreg(hw, DM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);
 	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(10), 0);
@@ -1333,8 +1375,8 @@ static void rtl88e_dm_trx_hw_antenna_div_init(struct ieee80211_hw *hw)
 
 	/*MAC Setting*/
 	value32 = rtl_get_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD);
-	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD, value32 |
-		     (BIT(23) | BIT(25)));
+	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD,
+		      value32 | (BIT(23) | BIT(25)));
 	/*Pin Setting*/
 	rtl_set_bbreg(hw, DM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);
 	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(10), 0);
@@ -1354,28 +1396,30 @@ static void rtl88e_dm_trx_hw_antenna_div_init(struct ieee80211_hw *hw)
 static void rtl88e_dm_fast_training_init(struct ieee80211_hw *hw)
 {
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
-	u32 ant_combo = 2;
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
+	u32 ant_combination = 2;
 	u32 value32, i;
 
 	for (i = 0; i < 6; i++) {
-		fat_tbl->bssid[i] = 0;
-		fat_tbl->ant_sum[i] = 0;
-		fat_tbl->ant_cnt[i] = 0;
-		fat_tbl->ant_ave[i] = 0;
+		pfat_table->bssid[i] = 0;
+		pfat_table->ant_sum[i] = 0;
+		pfat_table->ant_cnt[i] = 0;
+		pfat_table->ant_ave[i] = 0;
 	}
-	fat_tbl->train_idx = 0;
-	fat_tbl->fat_state = FAT_NORMAL_STATE;
+	pfat_table->train_idx = 0;
+	pfat_table->fat_state = FAT_NORMAL_STATE;
 
 	/*MAC Setting*/
 	value32 = rtl_get_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD);
-	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N, MASKDWORD, value32 | (BIT(23) |
-		      BIT(25)));
-	value32 = rtl_get_bbreg(hw, DM_REG_ANT_TRAIN_2, MASKDWORD);
-	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2, MASKDWORD, value32 | (BIT(16) |
-		      BIT(17)));
-	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2, MASKLWORD, 0);
-	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_1, MASKDWORD, 0);
+	rtl_set_bbreg(hw, DM_REG_ANTSEL_PIN_11N,
+		      MASKDWORD, value32 | (BIT(23) | BIT(25)));
+	value32 = rtl_get_bbreg(hw, DM_REG_ANT_TRAIN_PARA2_11N, MASKDWORD);
+	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_PARA2_11N,
+		      MASKDWORD, value32 | (BIT(16) | BIT(17)));
+	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_PARA2_11N,
+		      MASKLWORD, 0);
+	rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_PARA1_11N,
+		      MASKDWORD, 0);
 
 	/*Pin Setting*/
 	rtl_set_bbreg(hw, DM_REG_PIN_CTRL_11N, BIT(9) | BIT(8), 0);
@@ -1386,26 +1430,17 @@ static void rtl88e_dm_fast_training_init(struct ieee80211_hw *hw)
 	/*OFDM Setting*/
 	rtl_set_bbreg(hw, DM_REG_ANTDIV_PARA1_11N, MASKDWORD, 0x000000a0);
 	/*antenna mapping table*/
-	if (ant_combo == 2) {
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE0, 1);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE1, 2);
-	} else if (ant_combo == 7) {
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE0, 1);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE1, 2);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE2, 2);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE3, 3);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE0, 4);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE1, 5);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE2, 6);
-		rtl_set_bbreg(hw, DM_REG_ANT_MAPPING2_11N, MASKBYTE3, 7);
-	}
+	rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE0, 1);
+	rtl_set_bbreg(hw, DM_REG_ANT_MAPPING1_11N, MASKBYTE1, 2);
 
 	/*TX Setting*/
 	rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N, BIT(21), 1);
-	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(5) | BIT(4) | BIT(3), 0);
-	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) | BIT(7) | BIT(6), 1);
-	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(2) | BIT(1) | BIT(0),
-		      (ant_combo - 1));
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N,
+		      BIT(5) | BIT(4) | BIT(3), 0);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N,
+		      BIT(8) | BIT(7) | BIT(6), 1);
+	rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N,
+		      BIT(2) | BIT(1) | BIT(0), (ant_combination - 1));
 
 	rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 1);
 }
@@ -1420,6 +1455,7 @@ static void rtl88e_dm_antenna_div_init(struct ieee80211_hw *hw)
 		rtl88e_dm_trx_hw_antenna_div_init(hw);
 	else if (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV)
 		rtl88e_dm_fast_training_init(hw);
+
 }
 
 void rtl88e_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
@@ -1427,38 +1463,39 @@ void rtl88e_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
 {
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
 
 	if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
-	    (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)) {
-		SET_TX_DESC_ANTSEL_A(pdesc, fat_tbl->antsel_a[mac_id]);
-		SET_TX_DESC_ANTSEL_B(pdesc, fat_tbl->antsel_b[mac_id]);
-		SET_TX_DESC_ANTSEL_C(pdesc, fat_tbl->antsel_c[mac_id]);
+	    (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV)) {
+		SET_TX_DESC_ANTSEL_A(pdesc, pfat_table->antsel_a[mac_id]);
+		SET_TX_DESC_ANTSEL_B(pdesc, pfat_table->antsel_b[mac_id]);
+		SET_TX_DESC_ANTSEL_C(pdesc, pfat_table->antsel_c[mac_id]);
 	}
 }
 
 void rtl88e_dm_ant_sel_statistics(struct ieee80211_hw *hw,
-				  u8 antsel_tr_mux, u32 mac_id, u32 rx_pwdb_all)
+				  u8 antsel_tr_mux, u32 mac_id,
+				  u32 rx_pwdb_all)
 {
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
 
 	if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) {
 		if (antsel_tr_mux == MAIN_ANT_CG_TRX) {
-			fat_tbl->main_ant_sum[mac_id] += rx_pwdb_all;
-			fat_tbl->main_ant_cnt[mac_id]++;
+			pfat_table->main_ant_sum[mac_id] += rx_pwdb_all;
+			pfat_table->main_ant_cnt[mac_id]++;
 		} else {
-			fat_tbl->aux_ant_sum[mac_id] += rx_pwdb_all;
-			fat_tbl->aux_ant_cnt[mac_id]++;
+			pfat_table->aux_ant_sum[mac_id] += rx_pwdb_all;
+			pfat_table->aux_ant_cnt[mac_id]++;
 		}
 	} else if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV) {
 		if (antsel_tr_mux == MAIN_ANT_CGCS_RX) {
-			fat_tbl->main_ant_sum[mac_id] += rx_pwdb_all;
-			fat_tbl->main_ant_cnt[mac_id]++;
+			pfat_table->main_ant_sum[mac_id] += rx_pwdb_all;
+			pfat_table->main_ant_cnt[mac_id]++;
 		} else {
-			fat_tbl->aux_ant_sum[mac_id] += rx_pwdb_all;
-			fat_tbl->aux_ant_cnt[mac_id]++;
+			pfat_table->aux_ant_sum[mac_id] += rx_pwdb_all;
+			pfat_table->aux_ant_cnt[mac_id]++;
 		}
 	}
 }
@@ -1466,43 +1503,43 @@ void rtl88e_dm_ant_sel_statistics(struct ieee80211_hw *hw,
 static void rtl88e_dm_hw_ant_div(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
 	struct rtl_sta_info *drv_priv;
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
-	u32 i, min_rssi = 0xff, ant_div_max_rssi = 0, max_rssi = 0;
-	u32 local_min_rssi, local_max_rssi;
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+	u32 i, min_rssi = 0xff, ant_div_max_rssi = 0;
+	u32 max_rssi = 0, local_min_rssi, local_max_rssi;
 	u32 main_rssi, aux_rssi;
 	u8 rx_idle_ant = 0, target_ant = 7;
 
+	/*for sta its self*/
 	i = 0;
-	main_rssi = (fat_tbl->main_ant_cnt[i] != 0) ?
-		    (fat_tbl->main_ant_sum[i] /
-		     fat_tbl->main_ant_cnt[i]) : 0;
-	aux_rssi = (fat_tbl->aux_ant_cnt[i] != 0) ?
-		(fat_tbl->aux_ant_sum[i] / fat_tbl->aux_ant_cnt[i]) : 0;
+	main_rssi = (pfat_table->main_ant_cnt[i] != 0) ?
+		(pfat_table->main_ant_sum[i] / pfat_table->main_ant_cnt[i]) : 0;
+	aux_rssi = (pfat_table->aux_ant_cnt[i] != 0) ?
+		(pfat_table->aux_ant_sum[i] / pfat_table->aux_ant_cnt[i]) : 0;
 	target_ant = (main_rssi == aux_rssi) ?
-		     fat_tbl->rx_idle_ant : ((main_rssi >= aux_rssi) ?
-		     MAIN_ANT : AUX_ANT);
+		pfat_table->rx_idle_ant : ((main_rssi >= aux_rssi) ?
+		MAIN_ANT : AUX_ANT);
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "main_ant_sum %d main_ant_cnt %d\n",
-		 fat_tbl->main_ant_sum[i], fat_tbl->main_ant_cnt[i]);
+		"main_ant_sum %d main_ant_cnt %d\n",
+		pfat_table->main_ant_sum[i],
+		pfat_table->main_ant_cnt[i]);
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 		 "aux_ant_sum %d aux_ant_cnt %d\n",
-		 fat_tbl->aux_ant_sum[i],
-		 fat_tbl->aux_ant_cnt[i]);
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "main_rssi %d aux_rssi%d\n", main_rssi, aux_rssi);
+		 pfat_table->aux_ant_sum[i], pfat_table->aux_ant_cnt[i]);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "main_rssi %d aux_rssi%d\n",
+		 main_rssi, aux_rssi);
 	local_max_rssi = (main_rssi > aux_rssi) ? main_rssi : aux_rssi;
 	if ((local_max_rssi > ant_div_max_rssi) && (local_max_rssi < 40))
 		ant_div_max_rssi = local_max_rssi;
 	if (local_max_rssi > max_rssi)
 		max_rssi = local_max_rssi;
 
-	if ((fat_tbl->rx_idle_ant == MAIN_ANT) && (main_rssi == 0))
+	if ((pfat_table->rx_idle_ant == MAIN_ANT) && (main_rssi == 0))
 		main_rssi = aux_rssi;
-	else if ((fat_tbl->rx_idle_ant == AUX_ANT) && (aux_rssi == 0))
+	else if ((pfat_table->rx_idle_ant == AUX_ANT) && (aux_rssi == 0))
 		aux_rssi = main_rssi;
 
 	local_min_rssi = (main_rssi > aux_rssi) ? aux_rssi : main_rssi;
@@ -1518,32 +1555,33 @@ static void rtl88e_dm_hw_ant_div(struct ieee80211_hw *hw)
 		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
 		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
 			i++;
-			main_rssi = (fat_tbl->main_ant_cnt[i] != 0) ?
-				(fat_tbl->main_ant_sum[i] /
-				 fat_tbl->main_ant_cnt[i]) : 0;
-			aux_rssi = (fat_tbl->aux_ant_cnt[i] != 0) ?
-				   (fat_tbl->aux_ant_sum[i] /
-				    fat_tbl->aux_ant_cnt[i]) : 0;
+			main_rssi = (pfat_table->main_ant_cnt[i] != 0) ?
+				(pfat_table->main_ant_sum[i] /
+				pfat_table->main_ant_cnt[i]) : 0;
+			aux_rssi = (pfat_table->aux_ant_cnt[i] != 0) ?
+				(pfat_table->aux_ant_sum[i] /
+				pfat_table->aux_ant_cnt[i]) : 0;
 			target_ant = (main_rssi == aux_rssi) ?
-				      fat_tbl->rx_idle_ant : ((main_rssi >=
-				      aux_rssi) ? MAIN_ANT : AUX_ANT);
-
+				pfat_table->rx_idle_ant : ((main_rssi >=
+				aux_rssi) ? MAIN_ANT : AUX_ANT);
 
-			local_max_rssi = max_t(u32, main_rssi, aux_rssi);
+			local_max_rssi = (main_rssi > aux_rssi) ?
+					 main_rssi : aux_rssi;
 			if ((local_max_rssi > ant_div_max_rssi) &&
 			    (local_max_rssi < 40))
 				ant_div_max_rssi = local_max_rssi;
 			if (local_max_rssi > max_rssi)
 				max_rssi = local_max_rssi;
 
-			if ((fat_tbl->rx_idle_ant == MAIN_ANT) && !main_rssi)
+			if ((pfat_table->rx_idle_ant == MAIN_ANT) &&
+			    (main_rssi == 0))
 				main_rssi = aux_rssi;
-			else if ((fat_tbl->rx_idle_ant == AUX_ANT) &&
+			else if ((pfat_table->rx_idle_ant == AUX_ANT) &&
 				 (aux_rssi == 0))
 				aux_rssi = main_rssi;
 
 			local_min_rssi = (main_rssi > aux_rssi) ?
-					  aux_rssi : main_rssi;
+				aux_rssi : main_rssi;
 			if (local_min_rssi < min_rssi) {
 				min_rssi = local_min_rssi;
 				rx_idle_ant = target_ant;
@@ -1555,10 +1593,10 @@ static void rtl88e_dm_hw_ant_div(struct ieee80211_hw *hw)
 	}
 
 	for (i = 0; i < ASSOCIATE_ENTRY_NUM; i++) {
-		fat_tbl->main_ant_sum[i] = 0;
-		fat_tbl->aux_ant_sum[i] = 0;
-		fat_tbl->main_ant_cnt[i] = 0;
-		fat_tbl->aux_ant_cnt[i] = 0;
+		pfat_table->main_ant_sum[i] = 0;
+		pfat_table->aux_ant_sum[i] = 0;
+		pfat_table->main_ant_cnt[i] = 0;
+		pfat_table->aux_ant_cnt[i] = 0;
 	}
 
 	rtl88e_dm_update_rx_idle_ant(hw, rx_idle_ant);
@@ -1573,27 +1611,27 @@ static void rtl88e_set_next_mac_address_target(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
 	struct rtl_sta_info *drv_priv;
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
 	u32 value32, i, j = 0;
 
 	if (mac->link_state >= MAC80211_LINKED) {
 		for (i = 0; i < ASSOCIATE_ENTRY_NUM; i++) {
-			if ((fat_tbl->train_idx + 1) == ASSOCIATE_ENTRY_NUM)
-				fat_tbl->train_idx = 0;
+			if ((pfat_table->train_idx + 1) == ASSOCIATE_ENTRY_NUM)
+				pfat_table->train_idx = 0;
 			else
-				fat_tbl->train_idx++;
+				pfat_table->train_idx++;
 
-			if (fat_tbl->train_idx == 0) {
+			if (pfat_table->train_idx == 0) {
 				value32 = (mac->mac_addr[5] << 8) |
-					   mac->mac_addr[4];
-				rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2,
+					  mac->mac_addr[4];
+				rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_PARA2_11N,
 					      MASKLWORD, value32);
 
 				value32 = (mac->mac_addr[3] << 24) |
 					  (mac->mac_addr[2] << 16) |
 					  (mac->mac_addr[1] << 8) |
-					   mac->mac_addr[0];
-				rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_1,
+					  mac->mac_addr[0];
+				rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_PARA1_11N,
 					      MASKDWORD, value32);
 				break;
 			}
@@ -1602,28 +1640,29 @@ static void rtl88e_set_next_mac_address_target(struct ieee80211_hw *hw)
 			    NL80211_IFTYPE_STATION) {
 				spin_lock_bh(&rtlpriv->locks.entry_list_lock);
 				list_for_each_entry(drv_priv,
-						    &rtlpriv->entry_list,
-						    list) {
+						    &rtlpriv->entry_list, list) {
 					j++;
-					if (j != fat_tbl->train_idx)
+					if (j != pfat_table->train_idx)
 						continue;
 
 					value32 = (drv_priv->mac_addr[5] << 8) |
-						   drv_priv->mac_addr[4];
-					rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_2,
+						  drv_priv->mac_addr[4];
+					rtl_set_bbreg(hw,
+						      DM_REG_ANT_TRAIN_PARA2_11N,
 						      MASKLWORD, value32);
 
-					value32 = (drv_priv->mac_addr[3]<<24) |
-						  (drv_priv->mac_addr[2]<<16) |
-						  (drv_priv->mac_addr[1]<<8) |
-						   drv_priv->mac_addr[0];
-					rtl_set_bbreg(hw, DM_REG_ANT_TRAIN_1,
+					value32 = (drv_priv->mac_addr[3] << 24) |
+						  (drv_priv->mac_addr[2] << 16) |
+						  (drv_priv->mac_addr[1] << 8) |
+						  drv_priv->mac_addr[0];
+					rtl_set_bbreg(hw,
+						      DM_REG_ANT_TRAIN_PARA1_11N,
 						      MASKDWORD, value32);
 					break;
 				}
 				spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
 				/*find entry, break*/
-				if (j == fat_tbl->train_idx)
+				if (j == pfat_table->train_idx)
 					break;
 			}
 		}
@@ -1634,23 +1673,24 @@ static void rtl88e_dm_fast_ant_training(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
 	u32 i, max_rssi = 0;
 	u8 target_ant = 2;
 	bool bpkt_filter_match = false;
 
-	if (fat_tbl->fat_state == FAT_TRAINING_STATE) {
+	if (pfat_table->fat_state == FAT_TRAINING_STATE) {
 		for (i = 0; i < 7; i++) {
-			if (fat_tbl->ant_cnt[i] == 0) {
-				fat_tbl->ant_ave[i] = 0;
+			if (pfat_table->ant_cnt[i] == 0) {
+				pfat_table->ant_ave[i] = 0;
 			} else {
-				fat_tbl->ant_ave[i] = fat_tbl->ant_sum[i] /
-					fat_tbl->ant_cnt[i];
+				pfat_table->ant_ave[i] =
+					pfat_table->ant_sum[i] /
+					pfat_table->ant_cnt[i];
 				bpkt_filter_match = true;
 			}
 
-			if (fat_tbl->ant_ave[i] > max_rssi) {
-				max_rssi = fat_tbl->ant_ave[i];
+			if (pfat_table->ant_ave[i] > max_rssi) {
+				max_rssi = pfat_table->ant_ave[i];
 				target_ant = (u8) i;
 			}
 		}
@@ -1664,32 +1704,33 @@ static void rtl88e_dm_fast_ant_training(struct ieee80211_hw *hw)
 				      BIT(16), 0);
 			rtl_set_bbreg(hw, DM_REG_RX_ANT_CTRL_11N, BIT(8) |
 				      BIT(7) | BIT(6), target_ant);
-			rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N, BIT(21), 1);
+			rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N,
+				      BIT(21), 1);
 
-			fat_tbl->antsel_a[fat_tbl->train_idx] =
-				  target_ant & BIT(0);
-			fat_tbl->antsel_b[fat_tbl->train_idx] =
-				 (target_ant & BIT(1)) >> 1;
-			fat_tbl->antsel_c[fat_tbl->train_idx] =
-				 (target_ant & BIT(2)) >> 2;
+			pfat_table->antsel_a[pfat_table->train_idx] =
+				target_ant & BIT(0);
+			pfat_table->antsel_b[pfat_table->train_idx] =
+				(target_ant & BIT(1)) >> 1;
+			pfat_table->antsel_c[pfat_table->train_idx] =
+				(target_ant & BIT(2)) >> 2;
 
 			if (target_ant == 0)
 				rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 0);
 		}
 
 		for (i = 0; i < 7; i++) {
-			fat_tbl->ant_sum[i] = 0;
-			fat_tbl->ant_cnt[i] = 0;
+			pfat_table->ant_sum[i] = 0;
+			pfat_table->ant_cnt[i] = 0;
 		}
 
-		fat_tbl->fat_state = FAT_NORMAL_STATE;
+		pfat_table->fat_state = FAT_NORMAL_STATE;
 		return;
 	}
 
-	if (fat_tbl->fat_state == FAT_NORMAL_STATE) {
+	if (pfat_table->fat_state == FAT_NORMAL_STATE) {
 		rtl88e_set_next_mac_address_target(hw);
 
-		fat_tbl->fat_state = FAT_TRAINING_STATE;
+		pfat_table->fat_state = FAT_TRAINING_STATE;
 		rtl_set_bbreg(hw, DM_REG_TXAGC_A_1_MCS32_11N, BIT(16), 1);
 		rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 1);
 
@@ -1711,11 +1752,11 @@ static void rtl88e_dm_antenna_diversity(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_training *fat_tbl = &(rtldm->fat_table);
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
 
 	if (mac->link_state < MAC80211_LINKED) {
 		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "No Link\n");
-		if (fat_tbl->becomelinked == true) {
+		if (pfat_table->becomelinked) {
 			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 				 "need to turn off HW AntDiv\n");
 			rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 0);
@@ -1724,12 +1765,13 @@ static void rtl88e_dm_antenna_diversity(struct ieee80211_hw *hw)
 			if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
 				rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N,
 					      BIT(21), 0);
-			fat_tbl->becomelinked =
-			  (mac->link_state == MAC80211_LINKED) ? true : false;
+			pfat_table->becomelinked =
+				(mac->link_state == MAC80211_LINKED) ?
+				true : false;
 		}
 		return;
 	} else {
-		if (fat_tbl->becomelinked == false) {
+		if (!pfat_table->becomelinked) {
 			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
 				 "Need to turn on HW AntDiv\n");
 			rtl_set_bbreg(hw, DM_REG_IGI_A_11N, BIT(7), 1);
@@ -1738,8 +1780,9 @@ static void rtl88e_dm_antenna_diversity(struct ieee80211_hw *hw)
 			if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
 				rtl_set_bbreg(hw, DM_REG_TX_ANT_CTRL_11N,
 					      BIT(21), 1);
-			fat_tbl->becomelinked =
-			   (mac->link_state >= MAC80211_LINKED) ? true : false;
+			pfat_table->becomelinked =
+				(mac->link_state >= MAC80211_LINKED) ?
+				true : false;
 		}
 	}
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
index 0e07f72ea158..64f1f3ea9807 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/dm.h
@@ -156,7 +156,6 @@
 #define	DM_REG_SLEEP_11N				0xEE0
 #define	DM_REG_PMPD_ANAEN_11N				0xEEC
 
-
 /*MAC REG LIST*/
 #define	DM_REG_BB_RST_11N				0x02
 #define	DM_REG_ANTSEL_PIN_11N				0x4C
@@ -168,8 +167,9 @@
 #define	DM_REG_EDCA_BK_11N				0x50C
 #define	DM_REG_TXPAUSE_11N				0x522
 #define	DM_REG_RESP_TX_11N				0x6D8
-#define	DM_REG_ANT_TRAIN_1				0x7b0
-#define	DM_REG_ANT_TRAIN_2				0x7b4
+#define	DM_REG_ANT_TRAIN_PARA1_11N			0x7b0
+#define	DM_REG_ANT_TRAIN_PARA2_11N			0x7b4
+
 
 /*DIG Related*/
 #define	DM_BIT_IGI_11N					0x0000007F
@@ -208,7 +208,7 @@
 #define DM_DIG_BACKOFF_MIN				-4
 #define DM_DIG_BACKOFF_DEFAULT				10
 
-#define RXPATHSELECTION_SS_TH_LOW			30
+#define RXPATHSELECTION_SS_TH_W				30
 #define RXPATHSELECTION_DIFF_TH				18
 
 #define DM_RATR_STA_INIT				0
@@ -232,20 +232,22 @@
 
 #define TX_POWER_NEAR_FIELD_THRESH_LVL2			74
 #define TX_POWER_NEAR_FIELD_THRESH_LVL1			67
-#define TXPWRTRACK_MAX_IDX				6
+#define TXPWRTRACK_MAX_IDX				 6
 
 struct swat_t {
 	u8 failure_cnt;
 	u8 try_flag;
 	u8 stop_trying;
+
 	long pre_rssi;
 	long trying_threshold;
 	u8 cur_antenna;
 	u8 pre_antenna;
+
 };
 
 enum FAT_STATE {
-	FAT_NORMAL_STATE	= 0,
+	FAT_NORMAL_STATE = 0,
 	FAT_TRAINING_STATE = 1,
 };
 
@@ -310,8 +312,9 @@ enum pwr_track_control_method {
 
 void rtl88e_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
 				     u8 *pdesc, u32 mac_id);
-void rtl88e_dm_ant_sel_statistics(struct ieee80211_hw *hw, u8 antsel_tr_mux,
-				  u32 mac_id, u32 rx_pwdb_all);
+void rtl88e_dm_ant_sel_statistics(struct ieee80211_hw *hw,
+				  u8 antsel_tr_mux, u32 mac_id,
+				  u32 rx_pwdb_all);
 void rtl88e_dm_fast_antenna_training_callback(unsigned long data);
 void rtl88e_dm_init(struct ieee80211_hw *hw);
 void rtl88e_dm_watchdog(struct ieee80211_hw *hw);
@@ -320,7 +323,5 @@ void rtl88e_dm_init_edca_turbo(struct ieee80211_hw *hw);
 void rtl88e_dm_check_txpower_tracking(struct ieee80211_hw *hw);
 void rtl88e_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
 void rtl88e_dm_txpower_track_adjust(struct ieee80211_hw *hw,
-				    u8 type, u8 *pdirection,
-				    u32 *poutwrite_val);
-
+	u8 type, u8 *pdirection, u32 *poutwrite_val);
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/fw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.c
index 4f9376ad4739..c8058aa73ecf 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,12 +26,11 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "fw.h"
 
-#include <linux/kmemleak.h>
-
 static void _rtl88e_enable_fw_download(struct ieee80211_hw *hw, bool enable)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -62,26 +57,26 @@ static void _rtl88e_fw_block_write(struct ieee80211_hw *hw,
 				   const u8 *buffer, u32 size)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 blk_sz = sizeof(u32);
-	u8 *buf_ptr = (u8 *)buffer;
+	u32 blocksize = sizeof(u32);
+	u8 *bufferptr = (u8 *)buffer;
 	u32 *pu4BytePtr = (u32 *)buffer;
-	u32 i, offset, blk_cnt, remain;
+	u32 i, offset, blockcount, remainsize;
 
-	blk_cnt = size / blk_sz;
-	remain = size % blk_sz;
+	blockcount = size / blocksize;
+	remainsize = size % blocksize;
 
-	for (i = 0; i < blk_cnt; i++) {
-		offset = i * blk_sz;
+	for (i = 0; i < blockcount; i++) {
+		offset = i * blocksize;
 		rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
 				*(pu4BytePtr + i));
 	}
 
-	if (remain) {
-		offset = blk_cnt * blk_sz;
-		buf_ptr += offset;
-		for (i = 0; i < remain; i++) {
+	if (remainsize) {
+		offset = blockcount * blocksize;
+		bufferptr += offset;
+		for (i = 0; i < remainsize; i++) {
 			rtl_write_byte(rtlpriv, (FW_8192C_START_ADDRESS +
-						 offset + i), *(buf_ptr + i));
+						 offset + i), *(bufferptr + i));
 		}
 	}
 }
@@ -119,32 +114,33 @@ static void _rtl88e_write_fw(struct ieee80211_hw *hw,
 			     enum version_8188e version, u8 *buffer, u32 size)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 *buf_ptr = buffer;
-	u32 page_no, remain;
+	u8 *bufferptr = (u8 *)buffer;
+	u32 pagenums, remainsize;
 	u32 page, offset;
 
 	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "FW size is %d bytes,\n", size);
 
-	_rtl88e_fill_dummy(buf_ptr, &size);
+	_rtl88e_fill_dummy(bufferptr, &size);
 
-	page_no = size / FW_8192C_PAGE_SIZE;
-	remain = size % FW_8192C_PAGE_SIZE;
+	pagenums = size / FW_8192C_PAGE_SIZE;
+	remainsize = size % FW_8192C_PAGE_SIZE;
 
-	if (page_no > 8) {
+	if (pagenums > 8) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Page numbers should not greater then 8\n");
 	}
 
-	for (page = 0; page < page_no; page++) {
+	for (page = 0; page < pagenums; page++) {
 		offset = page * FW_8192C_PAGE_SIZE;
-		_rtl88e_fw_page_write(hw, page, (buf_ptr + offset),
+		_rtl88e_fw_page_write(hw, page, (bufferptr + offset),
 				      FW_8192C_PAGE_SIZE);
 	}
 
-	if (remain) {
-		offset = page_no * FW_8192C_PAGE_SIZE;
-		page = page_no;
-		_rtl88e_fw_page_write(hw, page, (buf_ptr + offset), remain);
+	if (remainsize) {
+		offset = pagenums * FW_8192C_PAGE_SIZE;
+		page = pagenums;
+		_rtl88e_fw_page_write(hw, page, (bufferptr + offset),
+				      remainsize);
 	}
 }
 
@@ -199,7 +195,8 @@ exit:
 	return err;
 }
 
-int rtl88e_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
+int rtl88e_download_fw(struct ieee80211_hw *hw,
+		       bool buse_wake_on_wlan_fw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -221,8 +218,8 @@ int rtl88e_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
 	if (IS_FW_HEADER_EXIST(pfwheader)) {
 		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 			 "Firmware Version(%d), Signature(%#x), Size(%d)\n",
-			 pfwheader->version, pfwheader->signature,
-			 (int)sizeof(struct rtl92c_firmware_header));
+			  pfwheader->version, pfwheader->signature,
+			  (int)sizeof(struct rtl92c_firmware_header));
 
 		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
 		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
@@ -237,9 +234,14 @@ int rtl88e_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
 	_rtl88e_enable_fw_download(hw, false);
 
 	err = _rtl88e_fw_free_to_go(hw);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Firmware is not ready to run!\n");
+	} else {
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+			 "Firmware is ready to run!\n");
+	}
 
-	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
-		 "Firmware is%s ready to run!\n", err ? " not" : "");
 	return 0;
 }
 
@@ -266,9 +268,9 @@ static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
 	bool isfw_read = false;
 	u8 buf_index = 0;
 	bool write_sucess = false;
-	u8 wait_h2c_limit = 100;
+	u8 wait_h2c_limmit = 100;
 	u8 wait_writeh2c_limit = 100;
-	u8 boxc[4], boxext[2];
+	u8 boxcontent[4], boxextcontent[4];
 	u32 h2c_waitcounter = 0;
 	unsigned long flag;
 	u8 idx;
@@ -331,18 +333,17 @@ static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
 			box_extreg = REG_HMEBOX_EXT_3;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
-
 		isfw_read = _rtl88e_check_fw_read_last_h2c(hw, boxnum);
 		while (!isfw_read) {
-			wait_h2c_limit--;
-			if (wait_h2c_limit == 0) {
+			wait_h2c_limmit--;
+			if (wait_h2c_limmit == 0) {
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-					 "Waiting too long for FW read "
-					 "clear HMEBox(%d)!\n", boxnum);
+					 "Waiting too long for FW read clear HMEBox(%d)!\n",
+					 boxnum);
 				break;
 			}
 
@@ -351,20 +352,20 @@ static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
 			isfw_read = _rtl88e_check_fw_read_last_h2c(hw, boxnum);
 			u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Waiting for FW read clear HMEBox(%d)!!! "
-				 "0x130 = %2x\n", boxnum, u1b_tmp);
+				 "Waiting for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
+				 boxnum, u1b_tmp);
 		}
 
 		if (!isfw_read) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Write H2C register BOX[%d] fail!!!!! "
-				 "Fw do not read.\n", boxnum);
+				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
+				 boxnum);
 			break;
 		}
 
-		memset(boxc, 0, sizeof(boxc));
-		memset(boxext, 0, sizeof(boxext));
-		boxc[0] = element_id;
+		memset(boxcontent, 0, sizeof(boxcontent));
+		memset(boxextcontent, 0, sizeof(boxextcontent));
+		boxcontent[0] = element_id;
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "Write element_id box_reg(%4x) = %2x\n",
 			 box_reg, element_id);
@@ -373,33 +374,38 @@ static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
 		case 1:
 		case 2:
 		case 3:
-			/*boxc[0] &= ~(BIT(7));*/
-			memcpy((u8 *)(boxc) + 1, cmd_b + buf_index, cmd_len);
+			/*boxcontent[0] &= ~(BIT(7));*/
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmd_b + buf_index, cmd_len);
 
-			for (idx = 0; idx < 4; idx++)
-				rtl_write_byte(rtlpriv, box_reg+idx, boxc[idx]);
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxcontent[idx]);
+			}
 			break;
 		case 4:
 		case 5:
 		case 6:
 		case 7:
-			/*boxc[0] |= (BIT(7));*/
-			memcpy((u8 *)(boxext), cmd_b + buf_index+3, cmd_len-3);
-			memcpy((u8 *)(boxc) + 1, cmd_b + buf_index, 3);
+			/*boxcontent[0] |= (BIT(7));*/
+			memcpy((u8 *)(boxextcontent),
+			       cmd_b + buf_index+3, cmd_len-3);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmd_b + buf_index, 3);
 
 			for (idx = 0; idx < 2; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
-					       boxext[idx]);
+					       boxextcontent[idx]);
 			}
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
-					       boxc[idx]);
+					       boxcontent[idx]);
 			}
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
@@ -411,7 +417,7 @@ static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
 
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "pHalData->last_hmeboxnum  = %d\n",
-			 rtlhal->last_hmeboxnum);
+			  rtlhal->last_hmeboxnum);
 	}
 
 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
@@ -422,18 +428,19 @@ static void _rtl88e_fill_h2c_command(struct ieee80211_hw *hw,
 }
 
 void rtl88e_fill_h2c_cmd(struct ieee80211_hw *hw,
-			 u8 element_id, u32 cmd_len, u8 *cmd_b)
+			 u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 {
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 tmp_cmdbuf[2];
 
-	if (rtlhal->fw_ready == false) {
-		RT_ASSERT(false, "fail H2C cmd - Fw download fail!!!\n");
+	if (!rtlhal->fw_ready) {
+		RT_ASSERT(false,
+			  "return H2C cmd because of Fw download fail!!!\n");
 		return;
 	}
 
 	memset(tmp_cmdbuf, 0, 8);
-	memcpy(tmp_cmdbuf, cmd_b, cmd_len);
+	memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
 	_rtl88e_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
 
 	return;
@@ -448,7 +455,8 @@ void rtl88e_firmware_selfreset(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp | BIT(2)));
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "8051Reset88E(): 8051 reset success.\n");
+		 "8051Reset88E(): 8051 reset success\n");
+
 }
 
 void rtl88e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
@@ -456,28 +464,29 @@ void rtl88e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 u1_h2c_set_pwrmode[H2C_88E_PWEMODE_LENGTH] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u8 power_state = 0;
-
+	u8 rlbm, power_state = 0;
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
+
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
-	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, 0);
+	rlbm = 0;/*YJ, temp, 120316. FW now not support RLBM=2.*/
+	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
-					 (rtlpriv->mac80211.p2p) ?
-					 ppsc->smart_ps : 1);
+		(rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
 	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
-					       ppsc->reg_max_lps_awakeintvl);
+		ppsc->reg_max_lps_awakeintvl);
 	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
 	if (mode == FW_PS_ACTIVE_MODE)
 		power_state |= FW_PWR_STATE_ACTIVE;
 	else
 		power_state |= FW_PWR_STATE_RF_OFF;
+
 	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
 		      u1_h2c_set_pwrmode, H2C_88E_PWEMODE_LENGTH);
-	rtl88e_fill_h2c_cmd(hw, H2C_88E_SETPWRMODE, H2C_88E_PWEMODE_LENGTH,
-			    u1_h2c_set_pwrmode);
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_SETPWRMODE,
+			    H2C_88E_PWEMODE_LENGTH, u1_h2c_set_pwrmode);
 }
 
 void rtl88e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
@@ -499,39 +508,9 @@ void rtl88e_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
 	SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->hiddenssid);
 	SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
 
-	rtl88e_fill_h2c_cmd(hw, H2C_88E_AP_OFFLOAD, H2C_88E_AP_OFFLOAD_LENGTH,
-			    u1_apoffload_parm);
-}
+	rtl88e_fill_h2c_cmd(hw, H2C_88E_AP_OFFLOAD,
+			    H2C_88E_AP_OFFLOAD_LENGTH, u1_apoffload_parm);
 
-static bool _rtl88e_cmd_send_packet(struct ieee80211_hw *hw,
-				    struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	struct sk_buff *pskb = NULL;
-	unsigned long flags;
-
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-
-	pdesc = &ring->desc[0];
-
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
-
-	__skb_queue_tail(&ring->queue, skb);
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
-
-	return true;
 }
 
 #define BEACON_PG		0 /* ->1 */
@@ -656,14 +635,15 @@ void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct sk_buff *skb = NULL;
-
 	u32 totalpacketlen;
-	u8 u1RsvdPageLoc[5] = { 0 };
-
+	bool rtstatus;
+	u8 u1rsvdpageloc[5] = { 0 };
+	bool b_dlok = false;
 	u8 *beacon;
-	u8 *pspoll;
+	u8 *p_pspoll;
 	u8 *nullfunc;
-	u8 *probersp;
+	u8 *p_probersp;
+
 	/*---------------------------------------------------------
 	 *			(1) beacon
 	 *---------------------------------------------------------
@@ -676,12 +656,12 @@ void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 	 *			(2) ps-poll
 	 *--------------------------------------------------------
 	 */
-	pspoll = &reserved_page_packet[PSPOLL_PG * 128];
-	SET_80211_PS_POLL_AID(pspoll, (mac->assoc_id | 0xc000));
-	SET_80211_PS_POLL_BSSID(pspoll, mac->bssid);
-	SET_80211_PS_POLL_TA(pspoll, mac->mac_addr);
+	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 
 	/*--------------------------------------------------------
 	 *			(3) null data
@@ -692,18 +672,18 @@ void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 
 	/*---------------------------------------------------------
 	 *			(4) probe response
 	 *----------------------------------------------------------
 	 */
-	probersp = &reserved_page_packet[PROBERSP_PG * 128];
-	SET_80211_HDR_ADDRESS1(probersp, mac->bssid);
-	SET_80211_HDR_ADDRESS2(probersp, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(probersp, mac->bssid);
+	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
+	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 
 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 
@@ -712,33 +692,36 @@ void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 		      &reserved_page_packet[0], totalpacketlen);
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 		      "rtl88e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
-		      u1RsvdPageLoc, 3);
+		      u1rsvdpageloc, 3);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	if (!skb)
-		return;
-	kmemleak_not_leak(skb);
 	memcpy(skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
-	if (_rtl88e_cmd_send_packet(hw, skb)) {
+	rtstatus = rtl_cmd_send_packet(hw, skb);
+
+	if (rtstatus)
+		b_dlok = true;
+
+	if (b_dlok) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "Set RSVD page location to Fw.\n");
 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-			      "H2C_RSVDPAGE:\n", u1RsvdPageLoc, 3);
+			      "H2C_RSVDPAGE:\n", u1rsvdpageloc, 3);
 		rtl88e_fill_h2c_cmd(hw, H2C_88E_RSVDPAGE,
-				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+				    sizeof(u1rsvdpageloc), u1rsvdpageloc);
 	} else
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
 }
 
-/*Shoud check FW support p2p or not.*/
+/*Should check FW support p2p or not.*/
 static void rtl88e_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
 {
-	u8 u1_ctwindow_period[1] = {ctwindow};
+	u8 u1_ctwindow_period[1] = { ctwindow};
 
 	rtl88e_fill_h2c_cmd(hw, H2C_88E_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
+
 }
 
 void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
@@ -755,7 +738,7 @@ void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 	switch (p2p_ps_state) {
 	case P2P_PS_DISABLE:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
-		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
+		memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
 		break;
 	case P2P_PS_ENABLE:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
@@ -765,8 +748,9 @@ void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 			ctwindow = p2pinfo->ctwindow;
 			rtl88e_set_p2p_ctw_period_cmd(hw, ctwindow);
 		}
+
 		/* hw only support 2 set of NoA */
-		for (i = 0; i < p2pinfo->noa_num; i++) {
+		for (i = 0 ; i < p2pinfo->noa_num; i++) {
 			/* To control the register setting for which NOA*/
 			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
 			if (i == 0)
@@ -785,7 +769,7 @@ void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 
 			start_time = p2pinfo->noa_start_time[i];
 			if (p2pinfo->noa_count_type[i] != 1) {
-				while (start_time <= (tsf_low + (50 * 1024))) {
+				while (start_time <= (tsf_low+(50*1024))) {
 					start_time += p2pinfo->noa_interval[i];
 					if (p2pinfo->noa_count_type[i] != 255)
 						p2pinfo->noa_count_type[i]--;
@@ -804,7 +788,7 @@ void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 
 			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
 				p2p_ps_offload->role = 1;
-				p2p_ps_offload->allstasleep = 0;
+				p2p_ps_offload->allstasleep = -1;
 			} else {
 				p2p_ps_offload->role = 0;
 			}
@@ -827,4 +811,5 @@ void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 
 	rtl88e_fill_h2c_cmd(hw, H2C_88E_P2P_PS_OFFLOAD, 1,
 			    (u8 *)p2p_ps_offload);
+
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/fw.h b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.h
index 854a9875cd5f..05e944e451f4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/fw.h
@@ -55,10 +55,11 @@
 #define H2C_88E_AOAC_RSVDPAGE_LOC_LEN		7
 
 /* Fw PS state for RPWM.
- * BIT[2:0] = HW state
- * BIT[3] = Protocol PS state, 1: register active state, 0: register sleep state
- * BIT[4] = sub-state
- */
+*BIT[2:0] = HW state
+*BIT[3] = Protocol PS state,
+*1: register active state , 0: register sleep state
+*BIT[4] = sub-state
+*/
 #define	FW_PS_GO_ON			BIT(0)
 #define	FW_PS_TX_NULL			BIT(1)
 #define	FW_PS_RF_ON			BIT(2)
@@ -98,10 +99,13 @@
 #define	FW_PS_STATE_S2			(FW_PS_RF_OFF)
 #define	FW_PS_STATE_S3			(FW_PS_ALL_ON)
 #define	FW_PS_STATE_S4			((FW_PS_ST_ACTIVE) | (FW_PS_ALL_ON))
-
+/* ((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))*/
 #define	FW_PS_STATE_ALL_ON_88E		(FW_PS_CLOCK_ON)
+/* (FW_PS_RF_ON)*/
 #define	FW_PS_STATE_RF_ON_88E		(FW_PS_CLOCK_ON)
-#define	FW_PS_STATE_RF_OFF_88E		(FW_PS_CLOCK_ON)
+/* 0x0*/
+#define	FW_PS_STATE_RF_OFF_88E	(FW_PS_CLOCK_ON)
+/* (FW_PS_STATE_RF_OFF)*/
 #define	FW_PS_STATE_RF_OFF_LOW_PWR_88E	(FW_PS_CLOCK_OFF)
 
 #define	FW_PS_STATE_ALL_ON_92C		(FW_PS_STATE_S4)
@@ -146,7 +150,7 @@ struct rtl92c_firmware_header {
 	u32 rsvd5;
 };
 
-enum rtl8192c_h2c_cmd {
+enum rtl8188e_h2c_cmd {
 	H2C_88E_RSVDPAGE = 0,
 	H2C_88E_JOINBSSRPT = 1,
 	H2C_88E_SCAN = 2,
@@ -175,7 +179,7 @@ enum rtl8192c_h2c_cmd {
 	H2C_88E_AOAC_GLOBAL_INFO = 0x82,
 	H2C_88E_AOAC_RSVDPAGE = 0x83,
 #endif
-	/* Not defined in new 88E H2C CMD Format */
+	/*Not defined in new 88E H2C CMD Format*/
 	H2C_88E_RA_MASK,
 	H2C_88E_SELECTIVE_SUSPEND_ROF_CMD,
 	H2C_88E_P2P_PS_MODE,
@@ -289,13 +293,12 @@ enum rtl8192c_h2c_cmd {
 int rtl88e_download_fw(struct ieee80211_hw *hw,
 		       bool buse_wake_on_wlan_fw);
 void rtl88e_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
-			 u32 cmd_len, u8 *p_cmdbuffer);
+			 u32 cmd_len, u8 *cmdbuffer);
 void rtl88e_firmware_selfreset(struct ieee80211_hw *hw);
 void rtl88e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
-void rtl88e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw,
-				      u8 mstatus);
-void rtl88e_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,  u8 enable);
+void rtl88e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
+void rtl88e_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
+				   u8 ap_offload_enable);
 void rtl88e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
 void rtl88e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
-
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
index d840ad7bdf65..f2b9713c456e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/hw.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -93,7 +89,9 @@ static void _rtl88ee_return_beacon_queue_skb(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+	unsigned long flags;
 
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 	while (skb_queue_len(&ring->queue)) {
 		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
 		struct sk_buff *skb = __skb_dequeue(&ring->queue);
@@ -105,6 +103,7 @@ static void _rtl88ee_return_beacon_queue_skb(struct ieee80211_hw *hw)
 		kfree_skb(skb);
 		ring->idx = (ring->idx + 1) % ring->entries;
 	}
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
 }
 
 static void _rtl88ee_disable_bcn_sub_func(struct ieee80211_hw *hw)
@@ -113,16 +112,16 @@ static void _rtl88ee_disable_bcn_sub_func(struct ieee80211_hw *hw)
 }
 
 static void _rtl88ee_set_fw_clock_on(struct ieee80211_hw *hw,
-				     u8 rpwm_val, bool need_turn_off_ckk)
+				     u8 rpwm_val, bool b_need_turn_off_ckk)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool support_remote_wake_up;
+	bool b_support_remote_wake_up;
 	u32 count = 0, isr_regaddr, content;
-	bool schedule_timer = need_turn_off_ckk;
-
+	bool schedule_timer = b_need_turn_off_ckk;
 	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
-				      (u8 *)(&support_remote_wake_up));
+					(u8 *)(&b_support_remote_wake_up));
+
 	if (!rtlhal->fw_ready)
 		return;
 	if (!rtlpriv->psc.fw_current_inpsmode)
@@ -133,8 +132,9 @@ static void _rtl88ee_set_fw_clock_on(struct ieee80211_hw *hw,
 		if (rtlhal->fw_clk_change_in_progress) {
 			while (rtlhal->fw_clk_change_in_progress) {
 				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+				count++;
 				udelay(100);
-				if (++count > 1000)
+				if (count > 1000)
 					return;
 				spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
 			}
@@ -173,6 +173,7 @@ static void _rtl88ee_set_fw_clock_on(struct ieee80211_hw *hw,
 			mod_timer(&rtlpriv->works.fw_clockoff_timer,
 				  jiffies + MSECS(10));
 		}
+
 	} else  {
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
 		rtlhal->fw_clk_change_in_progress = false;
@@ -247,11 +248,9 @@ static void _rtl88ee_set_fw_ps_rf_on(struct ieee80211_hw *hw)
 static void _rtl88ee_set_fw_ps_rf_off_low_power(struct ieee80211_hw *hw)
 {
 	u8 rpwm_val = 0;
-
 	rpwm_val |= FW_PS_STATE_RF_OFF_LOW_PWR_88E;
 	_rtl88ee_set_fw_clock_off(hw, rpwm_val);
 }
-
 void rtl88ee_fw_clk_off_timer_callback(unsigned long data)
 {
 	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
@@ -325,23 +324,23 @@ void rtl88ee_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
 		break;
 	case HW_VAR_FWLPS_RF_ON:{
-			enum rf_pwrstate rfstate;
-			u32 val_rcr;
-
-			rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE,
-						      (u8 *)(&rfstate));
-			if (rfstate == ERFOFF) {
+		enum rf_pwrstate rfstate;
+		u32 val_rcr;
+
+		rtlpriv->cfg->ops->get_hw_reg(hw,
+					      HW_VAR_RF_STATE,
+					      (u8 *)(&rfstate));
+		if (rfstate == ERFOFF) {
+			*((bool *)(val)) = true;
+		} else {
+			val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+			val_rcr &= 0x00070000;
+			if (val_rcr)
+				*((bool *)(val)) = false;
+			else
 				*((bool *)(val)) = true;
-			} else {
-				val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
-				val_rcr &= 0x00070000;
-				if (val_rcr)
-					*((bool *)(val)) = false;
-				else
-					*((bool *)(val)) = true;
-			}
-			break;
 		}
+		break; }
 	case HW_VAR_FW_PSMODE_STATUS:
 		*((bool *)(val)) = ppsc->fw_current_inpsmode;
 		break;
@@ -373,25 +372,32 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 	switch (variable) {
 	case HW_VAR_ETHER_ADDR:
-		for (idx = 0; idx < ETH_ALEN; idx++)
-			rtl_write_byte(rtlpriv, (REG_MACID + idx), val[idx]);
+		for (idx = 0; idx < ETH_ALEN; idx++) {
+			rtl_write_byte(rtlpriv, (REG_MACID + idx),
+				       val[idx]);
+		}
 		break;
 	case HW_VAR_BASIC_RATE:{
-		u16 rate_cfg = ((u16 *)val)[0];
+		u16 b_rate_cfg = ((u16 *)val)[0];
 		u8 rate_index = 0;
-		rate_cfg = rate_cfg & 0x15f;
-		rate_cfg |= 0x01;
-		rtl_write_byte(rtlpriv, REG_RRSR, rate_cfg & 0xff);
-		rtl_write_byte(rtlpriv, REG_RRSR + 1, (rate_cfg >> 8) & 0xff);
-		while (rate_cfg > 0x1) {
-			rate_cfg = (rate_cfg >> 1);
+		b_rate_cfg = b_rate_cfg & 0x15f;
+		b_rate_cfg |= 0x01;
+		rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
+		rtl_write_byte(rtlpriv, REG_RRSR + 1,
+			       (b_rate_cfg >> 8) & 0xff);
+		while (b_rate_cfg > 0x1) {
+			b_rate_cfg = (b_rate_cfg >> 1);
 			rate_index++;
 		}
-		rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL, rate_index);
-		break; }
+		rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL,
+			       rate_index);
+		break;
+		}
 	case HW_VAR_BSSID:
-		for (idx = 0; idx < ETH_ALEN; idx++)
-			rtl_write_byte(rtlpriv, (REG_BSSID + idx), val[idx]);
+		for (idx = 0; idx < ETH_ALEN; idx++) {
+			rtl_write_byte(rtlpriv, (REG_BSSID + idx),
+				       val[idx]);
+		}
 		break;
 	case HW_VAR_SIFS:
 		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
@@ -401,7 +407,8 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
 
 		if (!mac->ht_enable)
-			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM, 0x0e0e);
+			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+				       0x0e0e);
 		else
 			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
 				       *((u16 *)val));
@@ -418,17 +425,20 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
 						      &e_aci);
 		}
-		break; }
+		break;
+		}
 	case HW_VAR_ACK_PREAMBLE:{
 		u8 reg_tmp;
 		u8 short_preamble = (bool)*val;
 		reg_tmp = rtl_read_byte(rtlpriv, REG_TRXPTCL_CTL+2);
 		if (short_preamble) {
 			reg_tmp |= 0x02;
-			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL +
+				       2, reg_tmp);
 		} else {
 			reg_tmp |= 0xFD;
-			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
+			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL +
+				       2, reg_tmp);
 		}
 		break; }
 	case HW_VAR_WPA_CONFIG:
@@ -446,7 +456,8 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				min_spacing_to_set = sec_min_space;
 
 			mac->min_space_cfg = ((mac->min_space_cfg &
-					       0xf8) | min_spacing_to_set);
+					       0xf8) |
+					      min_spacing_to_set);
 
 			*val = min_spacing_to_set;
 
@@ -470,35 +481,44 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 		rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
 			       mac->min_space_cfg);
-		break; }
+		break;
+		}
 	case HW_VAR_AMPDU_FACTOR:{
 		u8 regtoset_normal[4] = { 0x41, 0xa8, 0x72, 0xb9 };
-		u8 factor;
-		u8 *reg = NULL;
-		u8 id = 0;
-
-		reg = regtoset_normal;
-
-		factor = *val;
-		if (factor <= 3) {
-			factor = (1 << (factor + 2));
-			if (factor > 0xf)
-				factor = 0xf;
-
-			for (id = 0; id < 4; id++) {
-				if ((reg[id] & 0xf0) > (factor << 4))
-					reg[id] = (reg[id] & 0x0f) |
-						  (factor << 4);
+		u8 factor_toset;
+		u8 *p_regtoset = NULL;
+		u8 index = 0;
+
+		p_regtoset = regtoset_normal;
+
+		factor_toset = *val;
+		if (factor_toset <= 3) {
+			factor_toset = (1 << (factor_toset + 2));
+			if (factor_toset > 0xf)
+				factor_toset = 0xf;
+
+			for (index = 0; index < 4; index++) {
+				if ((p_regtoset[index] & 0xf0) >
+				    (factor_toset << 4))
+					p_regtoset[index] =
+					    (p_regtoset[index] & 0x0f) |
+					    (factor_toset << 4);
+
+				if ((p_regtoset[index] & 0x0f) >
+				    factor_toset)
+					p_regtoset[index] =
+					    (p_regtoset[index] & 0xf0) |
+					    (factor_toset);
+
+				rtl_write_byte(rtlpriv,
+					       (REG_AGGLEN_LMT + index),
+					       p_regtoset[index]);
 
-				if ((reg[id] & 0x0f) > factor)
-					reg[id] = (reg[id] & 0xf0) | (factor);
-
-				rtl_write_byte(rtlpriv, (REG_AGGLEN_LMT + id),
-					       reg[id]);
 			}
 
 			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
-				 "Set HW_VAR_AMPDU_FACTOR: %#x\n", factor);
+				 "Set HW_VAR_AMPDU_FACTOR: %#x\n",
+				  factor_toset);
 		}
 		break; }
 	case HW_VAR_AC_PARAM:{
@@ -506,7 +526,8 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtl88e_dm_init_edca_turbo(hw);
 
 		if (rtlpci->acm_method != EACMWAY2_SW)
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_ACM_CTRL,
 						      &e_aci);
 		break; }
 	case HW_VAR_ACM_CTRL:{
@@ -516,7 +537,8 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		u8 acm = p_aci_aifsn->f.acm;
 		u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
 
-		acm_ctrl = acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
+		acm_ctrl = acm_ctrl |
+			   ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
 
 		if (acm) {
 			switch (e_aci) {
@@ -609,66 +631,76 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			_rtl88ee_fwlps_enter(hw);
 		 else
 			_rtl88ee_fwlps_leave(hw);
+
 		 break; }
 	case HW_VAR_H2C_FW_JOINBSSRPT:{
 		u8 mstatus = *val;
-		u8 tmp, tmp_reg422, uval;
+		u8 tmp_regcr, tmp_reg422, bcnvalid_reg;
 		u8 count = 0, dlbcn_count = 0;
-		bool recover = false;
+		bool b_recover = false;
 
 		if (mstatus == RT_MEDIA_CONNECT) {
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID,
+						      NULL);
 
-			tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
-			rtl_write_byte(rtlpriv, REG_CR + 1, (tmp | BIT(0)));
+			tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+			rtl_write_byte(rtlpriv, REG_CR + 1,
+				       (tmp_regcr | BIT(0)));
 
 			_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
 			_rtl88ee_set_bcn_ctrl_reg(hw, BIT(4), 0);
 
-			tmp_reg422 = rtl_read_byte(rtlpriv,
-						   REG_FWHW_TXQ_CTRL + 2);
+			tmp_reg422 =
+			    rtl_read_byte(rtlpriv,
+					  REG_FWHW_TXQ_CTRL + 2);
 			rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
 				       tmp_reg422 & (~BIT(6)));
 			if (tmp_reg422 & BIT(6))
-				recover = true;
+				b_recover = true;
 
 			do {
-				uval = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
+				bcnvalid_reg = rtl_read_byte(rtlpriv,
+							     REG_TDECTRL+2);
 				rtl_write_byte(rtlpriv, REG_TDECTRL+2,
-					       (uval | BIT(0)));
+					       (bcnvalid_reg | BIT(0)));
 				_rtl88ee_return_beacon_queue_skb(hw);
 
 				rtl88e_set_fw_rsvdpagepkt(hw, 0);
-				uval = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
+				bcnvalid_reg = rtl_read_byte(rtlpriv,
+							     REG_TDECTRL+2);
 				count = 0;
-				while (!(uval & BIT(0)) && count < 20) {
+				while (!(bcnvalid_reg & BIT(0)) && count < 20) {
 					count++;
 					udelay(10);
-					uval = rtl_read_byte(rtlpriv,
-							     REG_TDECTRL+2);
+					bcnvalid_reg =
+					  rtl_read_byte(rtlpriv, REG_TDECTRL+2);
 				}
 				dlbcn_count++;
-			} while (!(uval & BIT(0)) && dlbcn_count < 5);
+			} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
 
-			if (uval & BIT(0))
+			if (bcnvalid_reg & BIT(0))
 				rtl_write_byte(rtlpriv, REG_TDECTRL+2, BIT(0));
 
 			_rtl88ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
 			_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(4));
 
-			if (recover) {
-				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+			if (b_recover) {
+				rtl_write_byte(rtlpriv,
+					       REG_FWHW_TXQ_CTRL + 2,
 					       tmp_reg422);
 			}
-			rtl_write_byte(rtlpriv, REG_CR + 1, (tmp & ~(BIT(0))));
+
+			rtl_write_byte(rtlpriv, REG_CR + 1,
+				       (tmp_regcr & ~(BIT(0))));
 		}
-		rtl88e_set_fw_joinbss_report_cmd(hw, *val);
+		rtl88e_set_fw_joinbss_report_cmd(hw, (*(u8 *)val));
 		break; }
 	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
 		rtl88e_set_p2p_ps_offload_cmd(hw, *val);
 		break;
 	case HW_VAR_AID:{
 		u16 u2btmp;
+
 		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
 		u2btmp &= 0xC000;
 		rtl_write_word(rtlpriv, REG_BCN_PSR_RPT, (u2btmp |
@@ -677,21 +709,29 @@ void rtl88ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_CORRECT_TSF:{
 		u8 btype_ibss = *val;
 
-		if (btype_ibss == true)
+		if (btype_ibss)
 			_rtl88ee_stop_tx_beacon(hw);
 
 		_rtl88ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
 
 		rtl_write_dword(rtlpriv, REG_TSFTR,
-				(u32) (mac->tsf & 0xffffffff));
+				(u32)(mac->tsf & 0xffffffff));
 		rtl_write_dword(rtlpriv, REG_TSFTR + 4,
-				(u32) ((mac->tsf >> 32) & 0xffffffff));
+				(u32)((mac->tsf >> 32) & 0xffffffff));
 
 		_rtl88ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
 
-		if (btype_ibss == true)
+		if (btype_ibss)
 			_rtl88ee_resume_tx_beacon(hw);
 		break; }
+	case HW_VAR_KEEP_ALIVE: {
+		u8 array[2];
+
+		array[0] = 0xff;
+		array[1] = *((u8 *)val);
+		rtl88e_fill_h2c_cmd(hw, H2C_88E_KEEP_ALIVE_CTRL,
+				    2, array);
+		break; }
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "switch case not process %x\n", variable);
@@ -740,7 +780,7 @@ static bool _rtl88ee_llt_table_init(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x01);
 	rtl_write_dword(rtlpriv, REG_RQPN, 0x80730d29);
 
-
+	/*0x2600   MaxRxBuff=10k-max(TxReportSize(64*8), WOLPattern(16*24)) */
 	rtl_write_dword(rtlpriv, REG_TRXFF_BNDY, (0x25FF0000 | txpktbuf_bndy));
 	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, txpktbuf_bndy);
 
@@ -797,10 +837,11 @@ static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
 	u8 bytetmp;
 	u16 wordtmp;
 
-	/*Disable XTAL OUTPUT for power saving. YJ, add, 111206. */
+	/*Disable XTAL OUTPUT for power saving. YJ,add,111206. */
 	bytetmp = rtl_read_byte(rtlpriv, REG_XCK_OUT_CTRL) & (~BIT(0));
 	rtl_write_byte(rtlpriv, REG_XCK_OUT_CTRL, bytetmp);
 	/*Auto Power Down to CHIP-off State*/
@@ -811,7 +852,7 @@ static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
 	/* HW Power on sequence */
 	if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK,
 				      PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
-				      Rtl8188E_NIC_ENABLE_FLOW)) {
+				      RTL8188EE_NIC_ENABLE_FLOW)) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 			 "init MAC Fail as rtl_hal_pwrseqcmdparsing\n");
 		return false;
@@ -853,8 +894,6 @@ static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
 			return false;
 		}
 	}
-
-
 	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
 	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
 
@@ -889,9 +928,8 @@ static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
 			DMA_BIT_MASK(32));
 
 	/* if we want to support 64 bit DMA, we should set it here,
-	 * but at the moment we do not support 64 bit DMA
+	 * but now we do not support 64 bit DMA
 	 */
-
 	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
 
 	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
@@ -910,8 +948,12 @@ static bool _rtl88ee_init_mac(struct ieee80211_hw *hw)
 static void _rtl88ee_hw_configure(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 reg_prsr;
+	u8 reg_bw_opmode;
+	u32 reg_ratr, reg_prsr;
 
+	reg_bw_opmode = BW_OPMODE_20MHZ;
+	reg_ratr = RATE_ALL_CCK | RATE_ALL_OFDM_AG |
+	    RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
 	reg_prsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
 
 	rtl_write_dword(rtlpriv, REG_RRSR, reg_prsr);
@@ -923,7 +965,7 @@ static void _rtl88ee_enable_aspm_back_door(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	u8 tmp1byte = 0;
-	u32 tmp4Byte = 0, count;
+	u32 tmp4byte = 0, count = 0;
 
 	rtl_write_word(rtlpriv, 0x354, 0x8104);
 	rtl_write_word(rtlpriv, 0x358, 0x24);
@@ -938,8 +980,8 @@ static void _rtl88ee_enable_aspm_back_door(struct ieee80211_hw *hw)
 		count++;
 	}
 	if (0 == tmp1byte) {
-		tmp4Byte = rtl_read_dword(rtlpriv, 0x34c);
-		rtl_write_dword(rtlpriv, 0x348, tmp4Byte|BIT(31));
+		tmp4byte = rtl_read_dword(rtlpriv, 0x34c);
+		rtl_write_dword(rtlpriv, 0x348, tmp4byte|BIT(31));
 		rtl_write_word(rtlpriv, 0x350, 0xf70c);
 		rtl_write_byte(rtlpriv, 0x352, 0x1);
 	}
@@ -961,12 +1003,14 @@ static void _rtl88ee_enable_aspm_back_door(struct ieee80211_hw *hw)
 		tmp1byte = rtl_read_byte(rtlpriv, 0x352);
 		count++;
 	}
+
 	if (ppsc->support_backdoor || (0 == tmp1byte)) {
-		tmp4Byte = rtl_read_dword(rtlpriv, 0x34c);
-		rtl_write_dword(rtlpriv, 0x348, tmp4Byte|BIT(11)|BIT(12));
+		tmp4byte = rtl_read_dword(rtlpriv, 0x34c);
+		rtl_write_dword(rtlpriv, 0x348, tmp4byte|BIT(11)|BIT(12));
 		rtl_write_word(rtlpriv, 0x350, 0xf718);
 		rtl_write_byte(rtlpriv, 0x352, 0x1);
 	}
+
 	tmp1byte = rtl_read_byte(rtlpriv, 0x352);
 	count = 0;
 	while (tmp1byte && count < 20) {
@@ -983,14 +1027,15 @@ void rtl88ee_enable_hw_security_config(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
-		 rtlpriv->sec.pairwise_enc_algorithm,
-		 rtlpriv->sec.group_enc_algorithm);
+		  rtlpriv->sec.pairwise_enc_algorithm,
+		  rtlpriv->sec.group_enc_algorithm);
 
 	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 			 "not open hw encryption\n");
 		return;
 	}
+
 	sec_reg_value = SCR_TXENCENABLE | SCR_RXDECENABLE;
 
 	if (rtlpriv->sec.use_defaultkey) {
@@ -1004,6 +1049,7 @@ void rtl88ee_enable_hw_security_config(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 		 "The SECR-value %x\n", sec_reg_value);
+
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
 }
 
@@ -1021,7 +1067,6 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	u8 tmp_u1b, u1byte;
 	unsigned long flags;
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Rtl8188EE hw init\n");
 	rtlpriv->rtlhal.being_init_adapter = true;
 	/* As this function can take a very long time (up to 350 ms)
 	 * and can be called with irqs disabled, reenable the irqs
@@ -1032,6 +1077,7 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	 */
 	local_save_flags(flags);
 	local_irq_enable();
+	rtlhal->fw_ready = false;
 
 	rtlpriv->intf_ops->disable_aspm(hw);
 
@@ -1057,9 +1103,8 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
 		goto exit;
-	} else {
-		rtlhal->fw_ready = true;
 	}
+	rtlhal->fw_ready = true;
 	/*fw related variable initialize */
 	rtlhal->last_hmeboxnum = 0;
 	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_88E;
@@ -1068,10 +1113,10 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	ppsc->fw_current_inpsmode = false;
 
 	rtl88e_phy_mac_config(hw);
-	/* because last function modifies RCR, we update
-	 * rcr var here, or TP will be unstable for receive_config
-	 * is wrong, RX RCR_ACRC32 will cause TP unstable & Rx
-	 * RCR_APP_ICV will cause mac80211 disassoc for cisco 1252
+	/* because last function modify RCR, so we update
+	 * rcr var here, or TP will unstable for receive_config
+	 * is wrong, RX RCR_ACRC32 will cause TP unstabel & Rx
+	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
 	 */
 	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
 	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
@@ -1101,15 +1146,14 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 	if (ppsc->rfpwr_state == ERFON) {
 		if ((rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV) ||
 		    ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) &&
-		    (rtlhal->oem_id == RT_CID_819X_HP))) {
+		     (rtlhal->oem_id == RT_CID_819X_HP))) {
 			rtl88e_phy_set_rfpath_switch(hw, true);
 			rtlpriv->dm.fat_table.rx_idle_ant = MAIN_ANT;
 		} else {
 			rtl88e_phy_set_rfpath_switch(hw, false);
 			rtlpriv->dm.fat_table.rx_idle_ant = AUX_ANT;
 		}
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-			 "rx idle ant %s\n",
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "rx idle ant %s\n",
 			 (rtlpriv->dm.fat_table.rx_idle_ant == MAIN_ANT) ?
 			 ("MAIN_ANT") : ("AUX_ANT"));
 
@@ -1119,6 +1163,7 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 			rtl88e_phy_iq_calibrate(hw, false);
 			rtlphy->iqk_initialized = true;
 		}
+
 		rtl88e_dm_check_txpower_tracking(hw);
 		rtl88e_phy_lc_calibrate(hw);
 	}
@@ -1142,8 +1187,6 @@ int rtl88ee_hw_init(struct ieee80211_hw *hw)
 exit:
 	local_irq_restore(flags);
 	rtlpriv->rtlhal.being_init_adapter = false;
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "end of Rtl8188EE hw init %x\n",
-		 err);
 	return err;
 }
 
@@ -1176,62 +1219,67 @@ static int _rtl88ee_set_media_status(struct ieee80211_hw *hw,
 				     enum nl80211_iftype type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR) & 0xfc;
 	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
-	bt_msr &= 0xfc;
-
-	if (type == NL80211_IFTYPE_UNSPECIFIED ||
-	    type == NL80211_IFTYPE_STATION) {
-		_rtl88ee_stop_tx_beacon(hw);
-		_rtl88ee_enable_bcn_sub_func(hw);
-	} else if (type == NL80211_IFTYPE_ADHOC ||
-		type == NL80211_IFTYPE_AP ||
-		type == NL80211_IFTYPE_MESH_POINT) {
-		_rtl88ee_resume_tx_beacon(hw);
-		_rtl88ee_disable_bcn_sub_func(hw);
-	} else {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
-			 type);
-	}
+	u8 mode = MSR_NOLINK;
 
 	switch (type) {
 	case NL80211_IFTYPE_UNSPECIFIED:
-		bt_msr |= MSR_NOLINK;
-		ledaction = LED_CTL_LINK;
+		mode = MSR_NOLINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to NO LINK!\n");
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		bt_msr |= MSR_ADHOC;
+	case NL80211_IFTYPE_MESH_POINT:
+		mode = MSR_ADHOC;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to Ad Hoc!\n");
 		break;
 	case NL80211_IFTYPE_STATION:
-		bt_msr |= MSR_INFRA;
+		mode = MSR_INFRA;
 		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to STA!\n");
 		break;
 	case NL80211_IFTYPE_AP:
-		bt_msr |= MSR_AP;
+		mode = MSR_AP;
+		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to AP!\n");
 		break;
-	case NL80211_IFTYPE_MESH_POINT:
-		bt_msr |= MSR_ADHOC;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Set Network type to Mesh Point!\n");
-		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Network type %d not support!\n", type);
 		return 1;
+		break;
+	}
+
+	/* MSR_INFRA == Link in infrastructure network;
+	 * MSR_ADHOC == Link in ad hoc network;
+	 * Therefore, check link state is necessary.
+	 *
+	 * MSR_AP == AP mode; link state is not cared here.
+	 */
+	if (mode != MSR_AP && rtlpriv->mac80211.link_state < MAC80211_LINKED) {
+		mode = MSR_NOLINK;
+		ledaction = LED_CTL_NO_LINK;
+	}
+
+	if (mode == MSR_NOLINK || mode == MSR_INFRA) {
+		_rtl88ee_stop_tx_beacon(hw);
+		_rtl88ee_enable_bcn_sub_func(hw);
+	} else if (mode == MSR_ADHOC || mode == MSR_AP) {
+		_rtl88ee_resume_tx_beacon(hw);
+		_rtl88ee_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
+			 mode);
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
-	if ((bt_msr & MSR_MASK) == MSR_AP)
+	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
 	else
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
@@ -1241,13 +1289,12 @@ static int _rtl88ee_set_media_status(struct ieee80211_hw *hw,
 void rtl88ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 reg_rcr;
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rcr = rtlpci->receive_config;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
-
 	if (check_bssid == true) {
 		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
@@ -1259,9 +1306,11 @@ void rtl88ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 		rtlpriv->cfg->ops->set_hw_reg(hw,
 			HW_VAR_RCR, (u8 *)(&reg_rcr));
 	}
+
 }
 
-int rtl88ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+int rtl88ee_set_network_type(struct ieee80211_hw *hw,
+			     enum nl80211_iftype type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -1279,7 +1328,9 @@ int rtl88ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
 	return 0;
 }
 
-/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
+/* don't set REG_EDCA_BE_PARAM here
+ * because mac80211 will send pkt when scan
+ */
 void rtl88ee_set_qos(struct ieee80211_hw *hw, int aci)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1302,22 +1353,41 @@ void rtl88ee_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
+static void rtl88ee_clear_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 tmp;
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISR);
+	rtl_write_dword(rtlpriv, REG_HISR, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
+	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
+	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
+}
+
 void rtl88ee_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
-	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
+	rtl88ee_clear_interrupt(hw);/*clear it here first*/
+	rtl_write_dword(rtlpriv, REG_HIMR,
+			rtlpci->irq_mask[0] & 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv, REG_HIMRE,
+			rtlpci->irq_mask[1] & 0xFFFFFFFF);
 	rtlpci->irq_enabled = true;
-	/* there are some C2H CMDs have been sent before system interrupt
-	 * is enabled, e.g., C2H, CPWM.
-	 * So we need to clear all C2H events that FW has notified, otherwise
-	 * FW won't schedule any commands anymore.
+	/* there are some C2H CMDs have been sent
+	 * before system interrupt is enabled, e.g., C2H, CPWM.
+	 * So we need to clear all C2H events that FW has notified,
+	 * otherwise FW won't schedule any commands anymore.
 	 */
 	rtl_write_byte(rtlpriv, REG_C2HEVT_CLEAR, 0);
 	/*enable system interrupt*/
-	rtl_write_dword(rtlpriv, REG_HSIMR, rtlpci->sys_irq_mask & 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv, REG_HSIMR,
+			rtlpci->sys_irq_mask & 0xFFFFFFFF);
 }
 
 void rtl88ee_disable_interrupt(struct ieee80211_hw *hw)
@@ -1328,7 +1398,7 @@ void rtl88ee_disable_interrupt(struct ieee80211_hw *hw)
 	rtl_write_dword(rtlpriv, REG_HIMR, IMR_DISABLED);
 	rtl_write_dword(rtlpriv, REG_HIMRE, IMR_DISABLED);
 	rtlpci->irq_enabled = false;
-	synchronize_irq(rtlpci->pdev->irq);
+	/*synchronize_irq(rtlpci->pdev->irq);*/
 }
 
 static void _rtl88ee_poweroff_adapter(struct ieee80211_hw *hw)
@@ -1354,7 +1424,7 @@ static void _rtl88ee_poweroff_adapter(struct ieee80211_hw *hw)
 
 	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
 				 PWR_INTF_PCI_MSK,
-				 Rtl8188E_NIC_LPS_ENTER_FLOW);
+				 RTL8188EE_NIC_LPS_ENTER_FLOW);
 
 	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
 
@@ -1369,7 +1439,7 @@ static void _rtl88ee_poweroff_adapter(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_32K_CTRL, (u1b_tmp & (~BIT(0))));
 
 	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-				 PWR_INTF_PCI_MSK, Rtl8188E_NIC_DISABLE_FLOW);
+				 PWR_INTF_PCI_MSK, RTL8188EE_NIC_DISABLE_FLOW);
 
 	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
 	rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(3))));
@@ -1426,6 +1496,7 @@ void rtl88ee_interrupt_recognized(struct ieee80211_hw *hw,
 
 	*p_intb = rtl_read_dword(rtlpriv, REG_HISRE) & rtlpci->irq_mask[1];
 	rtl_write_dword(rtlpriv, REG_HISRE, *p_intb);
+
 }
 
 void rtl88ee_set_beacon_related_registers(struct ieee80211_hw *hw)
@@ -1471,233 +1542,241 @@ void rtl88ee_update_interrupt_mask(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_INTR, DBG_LOUD,
 		 "add_msr:%x, rm_msr:%x\n", add_msr, rm_msr);
 
-	rtl88ee_disable_interrupt(hw);
 	if (add_msr)
 		rtlpci->irq_mask[0] |= add_msr;
 	if (rm_msr)
 		rtlpci->irq_mask[0] &= (~rm_msr);
+	rtl88ee_disable_interrupt(hw);
 	rtl88ee_enable_interrupt(hw);
 }
 
-static inline u8 get_chnl_group(u8 chnl)
+static u8 _rtl88e_get_chnl_group(u8 chnl)
 {
-	u8 group;
-
-	group = chnl / 3;
-	if (chnl == 14)
+	u8 group = 0;
+
+	if (chnl < 3)
+		group = 0;
+	else if (chnl < 6)
+		group = 1;
+	else if (chnl < 9)
+		group = 2;
+	else if (chnl < 12)
+		group = 3;
+	else if (chnl < 14)
+		group = 4;
+	else if (chnl == 14)
 		group = 5;
 
 	return group;
 }
 
-static void set_diff0_2g(struct txpower_info_2g *pwr2g, u8 *hwinfo, u32 path,
-			 u32 i, u32 eadr)
-{
-	pwr2g->bw40_diff[path][i] = 0;
-	if (hwinfo[eadr] == 0xFF) {
-		pwr2g->bw20_diff[path][i] = 0x02;
-	} else {
-		pwr2g->bw20_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
-		/*bit sign number to 8 bit sign number*/
-		if (pwr2g->bw20_diff[path][i] & BIT(3))
-			pwr2g->bw20_diff[path][i] |= 0xF0;
-	}
-
-	if (hwinfo[eadr] == 0xFF) {
-		pwr2g->ofdm_diff[path][i] = 0x04;
-	} else {
-		pwr2g->ofdm_diff[path][i] = (hwinfo[eadr] & 0x0f);
-		/*bit sign number to 8 bit sign number*/
-		if (pwr2g->ofdm_diff[path][i] & BIT(3))
-			pwr2g->ofdm_diff[path][i] |= 0xF0;
-	}
-	pwr2g->cck_diff[path][i] = 0;
-}
-
-static void set_diff0_5g(struct txpower_info_5g *pwr5g, u8 *hwinfo, u32 path,
-			 u32 i, u32 eadr)
-{
-	pwr5g->bw40_diff[path][i] = 0;
-	if (hwinfo[eadr] == 0xFF) {
-		pwr5g->bw20_diff[path][i] = 0;
-	} else {
-		pwr5g->bw20_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
-		/*bit sign number to 8 bit sign number*/
-		if (pwr5g->bw20_diff[path][i] & BIT(3))
-			pwr5g->bw20_diff[path][i] |= 0xF0;
-	}
-
-	if (hwinfo[eadr] == 0xFF) {
-		pwr5g->ofdm_diff[path][i] = 0x04;
-	} else {
-		pwr5g->ofdm_diff[path][i] = (hwinfo[eadr] & 0x0f);
-		/*bit sign number to 8 bit sign number*/
-		if (pwr5g->ofdm_diff[path][i] & BIT(3))
-			pwr5g->ofdm_diff[path][i] |= 0xF0;
-	}
-}
-
-static void set_diff1_2g(struct txpower_info_2g *pwr2g, u8 *hwinfo, u32 path,
-			 u32 i, u32 eadr)
-{
-	if (hwinfo[eadr] == 0xFF) {
-		pwr2g->bw40_diff[path][i] = 0xFE;
-	} else {
-		pwr2g->bw40_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
-		if (pwr2g->bw40_diff[path][i] & BIT(3))
-			pwr2g->bw40_diff[path][i] |= 0xF0;
-	}
-
-	if (hwinfo[eadr] == 0xFF) {
-		pwr2g->bw20_diff[path][i] = 0xFE;
-	} else {
-		pwr2g->bw20_diff[path][i] = (hwinfo[eadr]&0x0f);
-		if (pwr2g->bw20_diff[path][i] & BIT(3))
-			pwr2g->bw20_diff[path][i] |= 0xF0;
-	}
-}
-
-static void set_diff1_5g(struct txpower_info_5g *pwr5g, u8 *hwinfo, u32 path,
-			 u32 i, u32 eadr)
+static void set_24g_base(struct txpower_info_2g *pwrinfo24g, u32 rfpath)
 {
-	if (hwinfo[eadr] == 0xFF) {
-		pwr5g->bw40_diff[path][i] = 0xFE;
-	} else {
-		pwr5g->bw40_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
-		if (pwr5g->bw40_diff[path][i] & BIT(3))
-			pwr5g->bw40_diff[path][i] |= 0xF0;
-	}
+	int group, txcnt;
 
-	if (hwinfo[eadr] == 0xFF) {
-		pwr5g->bw20_diff[path][i] = 0xFE;
-	} else {
-		pwr5g->bw20_diff[path][i] = (hwinfo[eadr] & 0x0f);
-		if (pwr5g->bw20_diff[path][i] & BIT(3))
-			pwr5g->bw20_diff[path][i] |= 0xF0;
+	for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+		pwrinfo24g->index_cck_base[rfpath][group] = 0x2D;
+		pwrinfo24g->index_bw40_base[rfpath][group] = 0x2D;
 	}
-}
-
-static void set_diff2_2g(struct txpower_info_2g *pwr2g, u8 *hwinfo, u32 path,
-			 u32 i, u32 eadr)
-{
-	if (hwinfo[eadr] == 0xFF) {
-		pwr2g->ofdm_diff[path][i] = 0xFE;
-	} else {
-		pwr2g->ofdm_diff[path][i] = (hwinfo[eadr]&0xf0)>>4;
-		if (pwr2g->ofdm_diff[path][i] & BIT(3))
-			pwr2g->ofdm_diff[path][i] |= 0xF0;
-	}
-
-	if (hwinfo[eadr] == 0xFF) {
-		pwr2g->cck_diff[path][i] = 0xFE;
-	} else {
-		pwr2g->cck_diff[path][i] = (hwinfo[eadr]&0x0f);
-		if (pwr2g->cck_diff[path][i] & BIT(3))
-			pwr2g->cck_diff[path][i] |= 0xF0;
+	for (txcnt = 0; txcnt < MAX_TX_COUNT; txcnt++) {
+		if (txcnt == 0) {
+			pwrinfo24g->bw20_diff[rfpath][0] = 0x02;
+			pwrinfo24g->ofdm_diff[rfpath][0] = 0x04;
+		} else {
+			pwrinfo24g->bw20_diff[rfpath][txcnt] = 0xFE;
+			pwrinfo24g->bw40_diff[rfpath][txcnt] = 0xFE;
+			pwrinfo24g->cck_diff[rfpath][txcnt] =	0xFE;
+			pwrinfo24g->ofdm_diff[rfpath][txcnt] = 0xFE;
+		}
 	}
 }
 
-static void _rtl8188e_read_power_value_fromprom(struct ieee80211_hw *hw,
-						struct txpower_info_2g *pwr2g,
-						struct txpower_info_5g *pwr5g,
-						bool autoload_fail,
-						u8 *hwinfo)
+static void read_power_value_fromprom(struct ieee80211_hw *hw,
+				      struct txpower_info_2g *pwrinfo24g,
+				      struct txpower_info_5g *pwrinfo5g,
+				      bool autoload_fail, u8 *hwinfo)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 path, eadr = EEPROM_TX_PWR_INX, i;
+	u32 rfpath, eeaddr = EEPROM_TX_PWR_INX, group, txcnt = 0;
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "hal_ReadPowerValueFromPROM88E(): PROMContent[0x%x]= 0x%x\n",
-		 (eadr+1), hwinfo[eadr+1]);
-	if (0xFF == hwinfo[eadr+1])
+		 "hal_ReadPowerValueFromPROM88E():PROMContent[0x%x]=0x%x\n",
+		 (eeaddr+1), hwinfo[eeaddr+1]);
+	if (0xFF == hwinfo[eeaddr+1])  /*YJ,add,120316*/
 		autoload_fail = true;
 
 	if (autoload_fail) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 			 "auto load fail : Use Default value!\n");
-		for (path = 0; path < MAX_RF_PATH; path++) {
+		for (rfpath = 0 ; rfpath < MAX_RF_PATH ; rfpath++) {
 			/* 2.4G default value */
-			for (i = 0; i < MAX_CHNL_GROUP_24G; i++) {
-				pwr2g->index_cck_base[path][i] = 0x2D;
-				pwr2g->index_bw40_base[path][i] = 0x2D;
-			}
-			for (i = 0; i < MAX_TX_COUNT; i++) {
-				if (i == 0) {
-					pwr2g->bw20_diff[path][0] = 0x02;
-					pwr2g->ofdm_diff[path][0] = 0x04;
-				} else {
-					pwr2g->bw20_diff[path][i] = 0xFE;
-					pwr2g->bw40_diff[path][i] = 0xFE;
-					pwr2g->cck_diff[path][i] = 0xFE;
-					pwr2g->ofdm_diff[path][i] = 0xFE;
-				}
-			}
+			set_24g_base(pwrinfo24g, rfpath);
 		}
 		return;
 	}
 
-	for (path = 0; path < MAX_RF_PATH; path++) {
+	for (rfpath = 0 ; rfpath < MAX_RF_PATH ; rfpath++) {
 		/*2.4G default value*/
-		for (i = 0; i < MAX_CHNL_GROUP_24G; i++) {
-			pwr2g->index_cck_base[path][i] = hwinfo[eadr++];
-			if (pwr2g->index_cck_base[path][i] == 0xFF)
-				pwr2g->index_cck_base[path][i] = 0x2D;
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+			pwrinfo24g->index_cck_base[rfpath][group] =
+			  hwinfo[eeaddr++];
+			if (pwrinfo24g->index_cck_base[rfpath][group] == 0xFF)
+				pwrinfo24g->index_cck_base[rfpath][group] =
+				  0x2D;
 		}
-		for (i = 0; i < MAX_CHNL_GROUP_24G; i++) {
-			pwr2g->index_bw40_base[path][i] = hwinfo[eadr++];
-			if (pwr2g->index_bw40_base[path][i] == 0xFF)
-				pwr2g->index_bw40_base[path][i] = 0x2D;
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G-1; group++) {
+			pwrinfo24g->index_bw40_base[rfpath][group] =
+				hwinfo[eeaddr++];
+			if (pwrinfo24g->index_bw40_base[rfpath][group] == 0xFF)
+				pwrinfo24g->index_bw40_base[rfpath][group] =
+					0x2D;
 		}
-		for (i = 0; i < MAX_TX_COUNT; i++) {
-			if (i == 0) {
-				set_diff0_2g(pwr2g, hwinfo, path, i, eadr);
-				eadr++;
+		pwrinfo24g->bw40_diff[rfpath][0] = 0;
+		if (hwinfo[eeaddr] == 0xFF) {
+			pwrinfo24g->bw20_diff[rfpath][0] = 0x02;
+		} else {
+			pwrinfo24g->bw20_diff[rfpath][0] =
+				(hwinfo[eeaddr]&0xf0)>>4;
+			/*bit sign number to 8 bit sign number*/
+			if (pwrinfo24g->bw20_diff[rfpath][0] & BIT(3))
+				pwrinfo24g->bw20_diff[rfpath][0] |= 0xF0;
+		}
+
+		if (hwinfo[eeaddr] == 0xFF) {
+			pwrinfo24g->ofdm_diff[rfpath][0] = 0x04;
+		} else {
+			pwrinfo24g->ofdm_diff[rfpath][0] =
+				(hwinfo[eeaddr]&0x0f);
+				/*bit sign number to 8 bit sign number*/
+			if (pwrinfo24g->ofdm_diff[rfpath][0] & BIT(3))
+				pwrinfo24g->ofdm_diff[rfpath][0] |= 0xF0;
+		}
+		pwrinfo24g->cck_diff[rfpath][0] = 0;
+		eeaddr++;
+		for (txcnt = 1; txcnt < MAX_TX_COUNT; txcnt++) {
+			if (hwinfo[eeaddr] == 0xFF) {
+				pwrinfo24g->bw40_diff[rfpath][txcnt] = 0xFE;
+			} else {
+				pwrinfo24g->bw40_diff[rfpath][txcnt] =
+				  (hwinfo[eeaddr]&0xf0)>>4;
+				if (pwrinfo24g->bw40_diff[rfpath][txcnt] &
+				    BIT(3))
+					pwrinfo24g->bw40_diff[rfpath][txcnt] |=
+					  0xF0;
+			}
+
+			if (hwinfo[eeaddr] == 0xFF) {
+				pwrinfo24g->bw20_diff[rfpath][txcnt] =
+					0xFE;
 			} else {
-				set_diff1_2g(pwr2g, hwinfo, path, i, eadr);
-				eadr++;
+				pwrinfo24g->bw20_diff[rfpath][txcnt] =
+				  (hwinfo[eeaddr]&0x0f);
+				if (pwrinfo24g->bw20_diff[rfpath][txcnt] &
+				    BIT(3))
+					pwrinfo24g->bw20_diff[rfpath][txcnt] |=
+					  0xF0;
+			}
+			eeaddr++;
 
-				set_diff2_2g(pwr2g, hwinfo, path, i, eadr);
-				eadr++;
+			if (hwinfo[eeaddr] == 0xFF) {
+				pwrinfo24g->ofdm_diff[rfpath][txcnt] = 0xFE;
+			} else {
+				pwrinfo24g->ofdm_diff[rfpath][txcnt] =
+				  (hwinfo[eeaddr]&0xf0)>>4;
+				if (pwrinfo24g->ofdm_diff[rfpath][txcnt] &
+				    BIT(3))
+					pwrinfo24g->ofdm_diff[rfpath][txcnt] |=
+					  0xF0;
 			}
+
+			if (hwinfo[eeaddr] == 0xFF) {
+				pwrinfo24g->cck_diff[rfpath][txcnt] =	0xFE;
+			} else {
+				pwrinfo24g->cck_diff[rfpath][txcnt] =
+				  (hwinfo[eeaddr]&0x0f);
+				if (pwrinfo24g->cck_diff[rfpath][txcnt] &
+				    BIT(3))
+					pwrinfo24g->cck_diff[rfpath][txcnt] |=
+					  0xF0;
+			}
+			eeaddr++;
 		}
 
 		/*5G default value*/
-		for (i = 0; i < MAX_CHNL_GROUP_5G; i++) {
-			pwr5g->index_bw40_base[path][i] = hwinfo[eadr++];
-			if (pwr5g->index_bw40_base[path][i] == 0xFF)
-				pwr5g->index_bw40_base[path][i] = 0xFE;
+		for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++) {
+			pwrinfo5g->index_bw40_base[rfpath][group] =
+				hwinfo[eeaddr++];
+			if (pwrinfo5g->index_bw40_base[rfpath][group] == 0xFF)
+				pwrinfo5g->index_bw40_base[rfpath][group] =
+				  0xFE;
 		}
 
-		for (i = 0; i < MAX_TX_COUNT; i++) {
-			if (i == 0) {
-				set_diff0_5g(pwr5g, hwinfo, path, i, eadr);
-				eadr++;
+		pwrinfo5g->bw40_diff[rfpath][0] = 0;
+
+		if (hwinfo[eeaddr] == 0xFF) {
+			pwrinfo5g->bw20_diff[rfpath][0] = 0;
+		} else {
+			pwrinfo5g->bw20_diff[rfpath][0] =
+			  (hwinfo[eeaddr]&0xf0)>>4;
+			if (pwrinfo5g->bw20_diff[rfpath][0] & BIT(3))
+				pwrinfo5g->bw20_diff[rfpath][0] |= 0xF0;
+		}
+
+		if (hwinfo[eeaddr] == 0xFF) {
+			pwrinfo5g->ofdm_diff[rfpath][0] = 0x04;
+		} else {
+			pwrinfo5g->ofdm_diff[rfpath][0] = (hwinfo[eeaddr]&0x0f);
+			if (pwrinfo5g->ofdm_diff[rfpath][0] & BIT(3))
+				pwrinfo5g->ofdm_diff[rfpath][0] |= 0xF0;
+		}
+		eeaddr++;
+		for (txcnt = 1; txcnt < MAX_TX_COUNT; txcnt++) {
+			if (hwinfo[eeaddr] == 0xFF) {
+				pwrinfo5g->bw40_diff[rfpath][txcnt] =	0xFE;
 			} else {
-				set_diff1_5g(pwr5g, hwinfo, path, i, eadr);
-				eadr++;
+				pwrinfo5g->bw40_diff[rfpath][txcnt] =
+				  (hwinfo[eeaddr]&0xf0)>>4;
+				if (pwrinfo5g->bw40_diff[rfpath][txcnt] &
+				    BIT(3))
+					pwrinfo5g->bw40_diff[rfpath][txcnt] |=
+					  0xF0;
 			}
+
+			if (hwinfo[eeaddr] == 0xFF) {
+				pwrinfo5g->bw20_diff[rfpath][txcnt] =	0xFE;
+			} else {
+				pwrinfo5g->bw20_diff[rfpath][txcnt] =
+				  (hwinfo[eeaddr]&0x0f);
+				if (pwrinfo5g->bw20_diff[rfpath][txcnt] &
+				    BIT(3))
+					pwrinfo5g->bw20_diff[rfpath][txcnt] |=
+					  0xF0;
+			}
+			eeaddr++;
 		}
 
-		if (hwinfo[eadr] == 0xFF) {
-			pwr5g->ofdm_diff[path][1] = 0xFE;
-			pwr5g->ofdm_diff[path][2] = 0xFE;
+		if (hwinfo[eeaddr] == 0xFF) {
+			pwrinfo5g->ofdm_diff[rfpath][1] = 0xFE;
+			pwrinfo5g->ofdm_diff[rfpath][2] = 0xFE;
 		} else {
-			pwr5g->ofdm_diff[path][1] = (hwinfo[eadr] & 0xf0) >> 4;
-			pwr5g->ofdm_diff[path][2] = (hwinfo[eadr] & 0x0f);
+			pwrinfo5g->ofdm_diff[rfpath][1] =
+					(hwinfo[eeaddr]&0xf0)>>4;
+			pwrinfo5g->ofdm_diff[rfpath][2] =
+					(hwinfo[eeaddr]&0x0f);
 		}
-		eadr++;
+		eeaddr++;
 
-		if (hwinfo[eadr] == 0xFF)
-			pwr5g->ofdm_diff[path][3] = 0xFE;
+		if (hwinfo[eeaddr] == 0xFF)
+			pwrinfo5g->ofdm_diff[rfpath][3] = 0xFE;
 		else
-			pwr5g->ofdm_diff[path][3] = (hwinfo[eadr]&0x0f);
-		eadr++;
-
-		for (i = 1; i < MAX_TX_COUNT; i++) {
-			if (pwr5g->ofdm_diff[path][i] == 0xFF)
-				pwr5g->ofdm_diff[path][i] = 0xFE;
-			else if (pwr5g->ofdm_diff[path][i] & BIT(3))
-				pwr5g->ofdm_diff[path][i] |= 0xF0;
+			pwrinfo5g->ofdm_diff[rfpath][3] = (hwinfo[eeaddr]&0x0f);
+		eeaddr++;
+
+		for (txcnt = 1; txcnt < MAX_TX_COUNT; txcnt++) {
+			if (pwrinfo5g->ofdm_diff[rfpath][txcnt] == 0xFF)
+				pwrinfo5g->ofdm_diff[rfpath][txcnt] =	0xFE;
+			else if (pwrinfo5g->ofdm_diff[rfpath][txcnt] & BIT(3))
+				pwrinfo5g->ofdm_diff[rfpath][txcnt] |= 0xF0;
 		}
 	}
 }
@@ -1712,41 +1791,36 @@ static void _rtl88ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	struct txpower_info_5g pwrinfo5g;
 	u8 rf_path, index;
 	u8 i;
-	int jj = EEPROM_RF_BOARD_OPTION_88E;
-	int kk = EEPROM_THERMAL_METER_88E;
 
-	_rtl8188e_read_power_value_fromprom(hw, &pwrinfo24g, &pwrinfo5g,
-					    autoload_fail, hwinfo);
+	read_power_value_fromprom(hw, &pwrinfo24g,
+				  &pwrinfo5g, autoload_fail, hwinfo);
 
 	for (rf_path = 0; rf_path < 2; rf_path++) {
 		for (i = 0; i < 14; i++) {
-			index = get_chnl_group(i+1);
+			index = _rtl88e_get_chnl_group(i+1);
 
 			rtlefuse->txpwrlevel_cck[rf_path][i] =
-				 pwrinfo24g.index_cck_base[rf_path][index];
-			if (i == 13)
-				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
-				     pwrinfo24g.index_bw40_base[rf_path][4];
-			else
-				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
-				     pwrinfo24g.index_bw40_base[rf_path][index];
+				pwrinfo24g.index_cck_base[rf_path][index];
+			rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+				pwrinfo24g.index_bw40_base[rf_path][index];
 			rtlefuse->txpwr_ht20diff[rf_path][i] =
-				 pwrinfo24g.bw20_diff[rf_path][0];
+				pwrinfo24g.bw20_diff[rf_path][0];
 			rtlefuse->txpwr_legacyhtdiff[rf_path][i] =
-				 pwrinfo24g.ofdm_diff[rf_path][0];
+				pwrinfo24g.ofdm_diff[rf_path][0];
 		}
 
 		for (i = 0; i < 14; i++) {
 			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
-				"RF(%d)-Ch(%d) [CCK / HT40_1S ] = "
-				"[0x%x / 0x%x ]\n", rf_path, i,
+				"RF(%d)-Ch(%d) [CCK / HT40_1S ] = [0x%x / 0x%x ]\n",
+				rf_path, i,
 				rtlefuse->txpwrlevel_cck[rf_path][i],
 				rtlefuse->txpwrlevel_ht40_1s[rf_path][i]);
 		}
 	}
 
 	if (!autoload_fail)
-		rtlefuse->eeprom_thermalmeter = hwinfo[kk];
+		rtlefuse->eeprom_thermalmeter =
+			hwinfo[EEPROM_THERMAL_METER_88E];
 	else
 		rtlefuse->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
 
@@ -1760,8 +1834,9 @@ static void _rtl88ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
 
 	if (!autoload_fail) {
-		rtlefuse->eeprom_regulatory = hwinfo[jj] & 0x07;/*bit0~2*/
-		if (hwinfo[jj] == 0xFF)
+		rtlefuse->eeprom_regulatory =
+			hwinfo[EEPROM_RF_BOARD_OPTION_88E] & 0x07;/*bit0~2*/
+		if (hwinfo[EEPROM_RF_BOARD_OPTION_88E] == 0xFF)
 			rtlefuse->eeprom_regulatory = 0;
 	} else {
 		rtlefuse->eeprom_regulatory = 0;
@@ -1775,12 +1850,9 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
 	u16 i, usvalue;
 	u8 hwinfo[HWSET_MAX_SIZE];
 	u16 eeprom_id;
-	int jj = EEPROM_RF_BOARD_OPTION_88E;
-	int kk = EEPROM_RF_FEATURE_OPTION_88E;
 
 	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
 		rtl_efuse_shadow_map_update(hw);
@@ -1790,9 +1862,14 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 	} else if (rtlefuse->epromtype == EEPROM_93C46) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "RTL819X Not boot from eeprom, check it !!");
+		return;
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "boot from neither eeprom nor efuse, check it !!");
+		return;
 	}
 
-	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP\n"),
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "MAP\n",
 		      hwinfo, HWSET_MAX_SIZE);
 
 	eeprom_id = *((u16 *)&hwinfo[0]);
@@ -1825,7 +1902,7 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 	/*customer ID*/
 	rtlefuse->eeprom_oemid = hwinfo[EEPROM_CUSTOMER_ID];
 	if (rtlefuse->eeprom_oemid == 0xFF)
-		rtlefuse->eeprom_oemid = 0;
+		rtlefuse->eeprom_oemid =	0;
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 		 "EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid);
@@ -1844,34 +1921,40 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 	/* set channel paln to world wide 13 */
 	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
 	/*tx power*/
-	_rtl88ee_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+	_rtl88ee_read_txpower_info_from_hwpg(hw,
+					     rtlefuse->autoload_failflag,
 					     hwinfo);
 	rtlefuse->txpwr_fromeprom = true;
 
 	rtl8188ee_read_bt_coexist_info_from_hwpg(hw,
 						 rtlefuse->autoload_failflag,
 						 hwinfo);
+
 	/*board type*/
-	rtlefuse->board_type = (hwinfo[jj] & 0xE0) >> 5;
+	rtlefuse->board_type =
+		((hwinfo[EEPROM_RF_BOARD_OPTION_88E] & 0xE0) >> 5);
+	rtlhal->board_type = rtlefuse->board_type;
 	/*Wake on wlan*/
-	rtlefuse->wowlan_enable = ((hwinfo[kk] & 0x40) >> 6);
+	rtlefuse->wowlan_enable =
+		((hwinfo[EEPROM_RF_FEATURE_OPTION_88E] & 0x40) >> 6);
 	/*parse xtal*/
 	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_88E];
 	if (hwinfo[EEPROM_XTAL_88E])
 		rtlefuse->crystalcap = 0x20;
 	/*antenna diversity*/
-	rtlefuse->antenna_div_cfg = (hwinfo[jj] & 0x18) >> 3;
-	if (hwinfo[jj] == 0xFF)
+	rtlefuse->antenna_div_cfg =
+		(hwinfo[EEPROM_RF_BOARD_OPTION_88E] & 0x18) >> 3;
+	if (hwinfo[EEPROM_RF_BOARD_OPTION_88E] == 0xFF)
 		rtlefuse->antenna_div_cfg = 0;
-	if (rppriv->bt_coexist.eeprom_bt_coexist != 0 &&
-	    rppriv->bt_coexist.eeprom_bt_ant_num == ANT_X1)
+	if (rtlpriv->btcoexist.eeprom_bt_coexist != 0 &&
+	    rtlpriv->btcoexist.eeprom_bt_ant_num == ANT_X1)
 		rtlefuse->antenna_div_cfg = 0;
 
 	rtlefuse->antenna_div_type = hwinfo[EEPROM_RF_ANTENNA_OPT_88E];
 	if (rtlefuse->antenna_div_type == 0xFF)
 		rtlefuse->antenna_div_type = 0x01;
 	if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV ||
-	    rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+		rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
 		rtlefuse->antenna_div_cfg = 1;
 
 	if (rtlhal->oem_id == RT_CID_DEFAULT) {
@@ -1881,12 +1964,12 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 				if (rtlefuse->eeprom_svid == 0x1025) {
 					rtlhal->oem_id = RT_CID_819X_ACER;
 				} else if ((rtlefuse->eeprom_svid == 0x10EC &&
-					    rtlefuse->eeprom_smid == 0x0179) ||
-					    (rtlefuse->eeprom_svid == 0x17AA &&
-					    rtlefuse->eeprom_smid == 0x0179)) {
+				     rtlefuse->eeprom_smid == 0x0179) ||
+				     (rtlefuse->eeprom_svid == 0x17AA &&
+				     rtlefuse->eeprom_smid == 0x0179)) {
 					rtlhal->oem_id = RT_CID_819X_LENOVO;
 				} else if (rtlefuse->eeprom_svid == 0x103c &&
-					 rtlefuse->eeprom_smid == 0x197d) {
+					   rtlefuse->eeprom_smid == 0x197d) {
 					rtlhal->oem_id = RT_CID_819X_HP;
 				} else {
 					rtlhal->oem_id = RT_CID_DEFAULT;
@@ -1905,6 +1988,7 @@ static void _rtl88ee_read_adapter_info(struct ieee80211_hw *hw)
 		default:
 			rtlhal->oem_id = RT_CID_DEFAULT;
 			break;
+
 		}
 	}
 }
@@ -1943,14 +2027,13 @@ void rtl88ee_read_eeprom_info(struct ieee80211_hw *hw)
 	u8 tmp_u1b;
 
 	rtlhal->version = _rtl88ee_read_chip_version(hw);
-	if (get_rf_type(rtlphy) == RF_1T1R) {
-		rtlpriv->dm.rfpath_rxenable[0] = true;
-	} else {
+	if (get_rf_type(rtlphy) == RF_1T1R)
 		rtlpriv->dm.rfpath_rxenable[0] = true;
-		rtlpriv->dm.rfpath_rxenable[1] = true;
-	}
+	else
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "VersionID = 0x%4x\n",
-		 rtlhal->version);
+						rtlhal->version);
 	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
 	if (tmp_u1b & BIT(4)) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EEPROM\n");
@@ -1970,24 +2053,25 @@ void rtl88ee_read_eeprom_info(struct ieee80211_hw *hw)
 }
 
 static void rtl88ee_update_hal_rate_table(struct ieee80211_hw *hw,
-					  struct ieee80211_sta *sta)
+		struct ieee80211_sta *sta)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 ratr_value;
 	u8 ratr_index = 0;
-	u8 nmode = mac->ht_enable;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	u8 b_nmode = mac->ht_enable;
+	/*u8 mimo_ps = IEEE80211_SMPS_OFF;*/
 	u16 shortgi_rate;
 	u32 tmp_ratr_value;
-	u8 ctx40 = mac->bw_40;
-	u16 cap = sta->ht_cap.cap;
-	u8 short40 = (cap & IEEE80211_HT_CAP_SGI_40) ?  1 : 0;
-	u8 short20 = (cap & IEEE80211_HT_CAP_SGI_20) ?  1 : 0;
+	u8 curtxbw_40mhz = mac->bw_40;
+	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+				1 : 0;
+	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+				1 : 0;
 	enum wireless_mode wirelessmode = mac->mode;
+	u32 ratr_mask;
 
 	if (rtlhal->current_bandtype == BAND_ON_5G)
 		ratr_value = sta->supp_rates[1] << 4;
@@ -1996,7 +2080,7 @@ static void rtl88ee_update_hal_rate_table(struct ieee80211_hw *hw,
 	if (mac->opmode == NL80211_IFTYPE_ADHOC)
 		ratr_value = 0xfff;
 	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
-			sta->ht_cap.mcs.rx_mask[0] << 12);
+		       sta->ht_cap.mcs.rx_mask[0] << 12);
 	switch (wirelessmode) {
 	case WIRELESS_MODE_B:
 		if (ratr_value & 0x0000000c)
@@ -2009,20 +2093,14 @@ static void rtl88ee_update_hal_rate_table(struct ieee80211_hw *hw,
 		break;
 	case WIRELESS_MODE_N_24G:
 	case WIRELESS_MODE_N_5G:
-		nmode = 1;
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			ratr_value &= 0x0007F005;
-		} else {
-			u32 ratr_mask;
-
-			if (get_rf_type(rtlphy) == RF_1T2R ||
-			    get_rf_type(rtlphy) == RF_1T1R)
-				ratr_mask = 0x000ff005;
-			else
-				ratr_mask = 0x0f0ff005;
+		b_nmode = 1;
+		if (get_rf_type(rtlphy) == RF_1T2R ||
+		    get_rf_type(rtlphy) == RF_1T1R)
+			ratr_mask = 0x000ff005;
+		else
+			ratr_mask = 0x0f0ff005;
 
-			ratr_value &= ratr_mask;
-		}
+		ratr_value &= ratr_mask;
 		break;
 	default:
 		if (rtlphy->rf_type == RF_1T2R)
@@ -2033,18 +2111,19 @@ static void rtl88ee_update_hal_rate_table(struct ieee80211_hw *hw,
 		break;
 	}
 
-	if ((rppriv->bt_coexist.bt_coexistence) &&
-	    (rppriv->bt_coexist.bt_coexist_type == BT_CSR_BC4) &&
-	    (rppriv->bt_coexist.bt_cur_state) &&
-	    (rppriv->bt_coexist.bt_ant_isolation) &&
-	    ((rppriv->bt_coexist.bt_service == BT_SCO) ||
-	    (rppriv->bt_coexist.bt_service == BT_BUSY)))
+	if ((rtlpriv->btcoexist.bt_coexistence) &&
+	    (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4) &&
+	    (rtlpriv->btcoexist.bt_cur_state) &&
+	    (rtlpriv->btcoexist.bt_ant_isolation) &&
+	    ((rtlpriv->btcoexist.bt_service == BT_SCO) ||
+	     (rtlpriv->btcoexist.bt_service == BT_BUSY)))
 		ratr_value &= 0x0fffcfc0;
 	else
 		ratr_value &= 0x0FFFFFFF;
 
-	if (nmode && ((ctx40 && short40) ||
-		      (!ctx40 && short20))) {
+	if (b_nmode &&
+	    ((curtxbw_40mhz && curshortgi_40mhz) ||
+	     (!curtxbw_40mhz && curshortgi_20mhz))) {
 		ratr_value |= 0x10000000;
 		tmp_ratr_value = (ratr_value >> 12);
 
@@ -2064,7 +2143,7 @@ static void rtl88ee_update_hal_rate_table(struct ieee80211_hw *hw,
 }
 
 static void rtl88ee_update_hal_rate_mask(struct ieee80211_hw *hw,
-					 struct ieee80211_sta *sta, u8 rssi)
+		struct ieee80211_sta *sta, u8 rssi_level)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -2073,23 +2152,25 @@ static void rtl88ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 	struct rtl_sta_info *sta_entry = NULL;
 	u32 ratr_bitmap;
 	u8 ratr_index;
-	u16 cap = sta->ht_cap.cap;
-	u8 ctx40 = (cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
-	u8 short40 = (cap & IEEE80211_HT_CAP_SGI_40) ?  1 : 0;
-	u8 short20 = (cap & IEEE80211_HT_CAP_SGI_20) ?  1 : 0;
+	u8 curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
+				? 1 : 0;
+	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+				1 : 0;
+	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+				1 : 0;
 	enum wireless_mode wirelessmode = 0;
-	bool shortgi = false;
+	bool b_shortgi = false;
 	u8 rate_mask[5];
 	u8 macid = 0;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	/*u8 mimo_ps = IEEE80211_SMPS_OFF;*/
 
 	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 	wirelessmode = sta_entry->wireless_mode;
 	if (mac->opmode == NL80211_IFTYPE_STATION ||
-	    mac->opmode == NL80211_IFTYPE_MESH_POINT)
-		ctx40 = mac->bw_40;
+		mac->opmode == NL80211_IFTYPE_MESH_POINT)
+		curtxbw_40mhz = mac->bw_40;
 	else if (mac->opmode == NL80211_IFTYPE_AP ||
-		 mac->opmode == NL80211_IFTYPE_ADHOC)
+		mac->opmode == NL80211_IFTYPE_ADHOC)
 		macid = sta->aid + 1;
 
 	if (rtlhal->current_bandtype == BAND_ON_5G)
@@ -2111,70 +2192,59 @@ static void rtl88ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 	case WIRELESS_MODE_G:
 		ratr_index = RATR_INX_WIRELESS_GB;
 
-		if (rssi == 1)
+		if (rssi_level == 1)
 			ratr_bitmap &= 0x00000f00;
-		else if (rssi == 2)
+		else if (rssi_level == 2)
 			ratr_bitmap &= 0x00000ff0;
 		else
 			ratr_bitmap &= 0x00000ff5;
 		break;
-	case WIRELESS_MODE_A:
-		ratr_index = RATR_INX_WIRELESS_A;
-		ratr_bitmap &= 0x00000ff0;
-		break;
 	case WIRELESS_MODE_N_24G:
 	case WIRELESS_MODE_N_5G:
 		ratr_index = RATR_INX_WIRELESS_NGB;
-
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			if (rssi == 1)
-				ratr_bitmap &= 0x00070000;
-			else if (rssi == 2)
-				ratr_bitmap &= 0x0007f000;
-			else
-				ratr_bitmap &= 0x0007f005;
+		if (rtlphy->rf_type == RF_1T2R ||
+		    rtlphy->rf_type == RF_1T1R) {
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff015;
+			} else {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff005;
+			}
 		} else {
-			if (rtlphy->rf_type == RF_1T2R ||
-			    rtlphy->rf_type == RF_1T1R) {
-				if (ctx40) {
-					if (rssi == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff015;
-				} else {
-					if (rssi == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff005;
-				}
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f8f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f8ff000;
+				else
+					ratr_bitmap &= 0x0f8ff015;
 			} else {
-				if (ctx40) {
-					if (rssi == 1)
-						ratr_bitmap &= 0x0f8f0000;
-					else if (rssi == 2)
-						ratr_bitmap &= 0x0f8ff000;
-					else
-						ratr_bitmap &= 0x0f8ff015;
-				} else {
-					if (rssi == 1)
-						ratr_bitmap &= 0x0f8f0000;
-					else if (rssi == 2)
-						ratr_bitmap &= 0x0f8ff000;
-					else
-						ratr_bitmap &= 0x0f8ff005;
-				}
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f8f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f8ff000;
+				else
+					ratr_bitmap &= 0x0f8ff005;
 			}
 		}
+		/*}*/
+
+		if ((curtxbw_40mhz && curshortgi_40mhz) ||
+		    (!curtxbw_40mhz && curshortgi_20mhz)) {
 
-		if ((ctx40 && short40) || (!ctx40 && short20)) {
 			if (macid == 0)
-				shortgi = true;
+				b_shortgi = true;
 			else if (macid == 1)
-				shortgi = false;
+				b_shortgi = false;
 		}
 		break;
 	default:
@@ -2192,22 +2262,24 @@ static void rtl88ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 		 "ratr_bitmap :%x\n", ratr_bitmap);
 	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) |
 			     (ratr_index << 28);
-	rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
+	rate_mask[4] = macid | (b_shortgi ? 0x20 : 0x00) | 0x80;
 	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
 		 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x\n",
-		 ratr_index, ratr_bitmap, rate_mask[0], rate_mask[1],
-		 rate_mask[2], rate_mask[3], rate_mask[4]);
+		 ratr_index, ratr_bitmap,
+		 rate_mask[0], rate_mask[1],
+		 rate_mask[2], rate_mask[3],
+		 rate_mask[4]);
 	rtl88e_fill_h2c_cmd(hw, H2C_88E_RA_MASK, 5, rate_mask);
 	_rtl88ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
 }
 
 void rtl88ee_update_hal_rate_tbl(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u8 rssi)
+		struct ieee80211_sta *sta, u8 rssi_level)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	if (rtlpriv->dm.useramask)
-		rtl88ee_update_hal_rate_mask(hw, sta, rssi);
+		rtl88ee_update_hal_rate_mask(hw, sta, rssi_level);
 	else
 		rtl88ee_update_hal_rate_table(hw, sta);
 }
@@ -2230,9 +2302,9 @@ bool rtl88ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	enum rf_pwrstate state_toset;
+	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
 	u32 u4tmp;
-	bool actuallyset = false;
+	bool b_actuallyset = false;
 
 	if (rtlpriv->rtlhal.being_init_adapter)
 		return false;
@@ -2249,27 +2321,29 @@ bool rtl88ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 		spin_unlock(&rtlpriv->locks.rf_ps_lock);
 	}
 
-	u4tmp = rtl_read_dword(rtlpriv, REG_GPIO_OUTPUT);
-	state_toset = (u4tmp & BIT(31)) ? ERFON : ERFOFF;
+	cur_rfstate = ppsc->rfpwr_state;
 
+	u4tmp = rtl_read_dword(rtlpriv, REG_GPIO_OUTPUT);
+	e_rfpowerstate_toset = (u4tmp & BIT(31)) ? ERFON : ERFOFF;
 
-	if ((ppsc->hwradiooff == true) && (state_toset == ERFON)) {
+	if (ppsc->hwradiooff && (e_rfpowerstate_toset == ERFON)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 "GPIOChangeRF  - HW Radio ON, RF ON\n");
 
-		state_toset = ERFON;
+		e_rfpowerstate_toset = ERFON;
 		ppsc->hwradiooff = false;
-		actuallyset = true;
-	} else if ((ppsc->hwradiooff == false) && (state_toset == ERFOFF)) {
+		b_actuallyset = true;
+	} else if ((!ppsc->hwradiooff) &&
+		   (e_rfpowerstate_toset == ERFOFF)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 "GPIOChangeRF  - HW Radio OFF, RF OFF\n");
 
-		state_toset = ERFOFF;
+		e_rfpowerstate_toset = ERFOFF;
 		ppsc->hwradiooff = true;
-		actuallyset = true;
+		b_actuallyset = true;
 	}
 
-	if (actuallyset) {
+	if (b_actuallyset) {
 		spin_lock(&rtlpriv->locks.rf_ps_lock);
 		ppsc->rfchange_inprogress = false;
 		spin_unlock(&rtlpriv->locks.rf_ps_lock);
@@ -2284,50 +2358,19 @@ bool rtl88ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 
 	*valid = 1;
 	return !ppsc->hwradiooff;
-}
-
-static void add_one_key(struct ieee80211_hw *hw, u8 *macaddr,
-			struct rtl_mac *mac, u32 key, u32 id,
-			u8 enc_algo, bool is_pairwise)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "add one entry\n");
-	if (is_pairwise) {
-		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "set Pairwise key\n");
 
-		rtl_cam_add_one_entry(hw, macaddr, key, id, enc_algo,
-				      CAM_CONFIG_NO_USEDK,
-				      rtlpriv->sec.key_buf[key]);
-	} else {
-		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "set group key\n");
-
-		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-			rtl_cam_add_one_entry(hw, rtlefuse->dev_addr,
-					      PAIRWISE_KEYIDX,
-					      CAM_PAIRWISE_KEY_POSITION,
-					      enc_algo,
-					      CAM_CONFIG_NO_USEDK,
-					      rtlpriv->sec.key_buf[id]);
-		}
-
-		rtl_cam_add_one_entry(hw, macaddr, key, id, enc_algo,
-				      CAM_CONFIG_NO_USEDK,
-				      rtlpriv->sec.key_buf[id]);
-	}
 }
 
-void rtl88ee_set_key(struct ieee80211_hw *hw, u32 key,
-		     u8 *mac_ad, bool is_group, u8 enc_algo,
+void rtl88ee_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
 		     bool is_wepkey, bool clear_all)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u8 *macaddr = mac_ad;
-	u32 id = 0;
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 *macaddr = p_macaddr;
+	u32 entry_id = 0;
 	bool is_pairwise = false;
-
 	static u8 cam_const_addr[4][6] = {
 		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
 		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
@@ -2372,122 +2415,176 @@ void rtl88ee_set_key(struct ieee80211_hw *hw, u32 key,
 			break;
 		default:
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+				 "switch case not process\n");
 			enc_algo = CAM_TKIP;
 			break;
 		}
 
 		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
-			macaddr = cam_const_addr[key];
-			id = key;
+			macaddr = cam_const_addr[key_index];
+			entry_id = key_index;
 		} else {
 			if (is_group) {
 				macaddr = cam_const_broad;
-				id = key;
+				entry_id = key_index;
 			} else {
 				if (mac->opmode == NL80211_IFTYPE_AP ||
 				    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-					id = rtl_cam_get_free_entry(hw, mac_ad);
-					if (id >=  TOTAL_CAM_ENTRY) {
+					entry_id =
+					  rtl_cam_get_free_entry(hw, p_macaddr);
+					if (entry_id >=  TOTAL_CAM_ENTRY) {
 						RT_TRACE(rtlpriv, COMP_SEC,
 							 DBG_EMERG,
 							 "Can not find free hw security cam entry\n");
 						return;
 					}
 				} else {
-					id = CAM_PAIRWISE_KEY_POSITION;
+					entry_id = CAM_PAIRWISE_KEY_POSITION;
 				}
-
-				key = PAIRWISE_KEYIDX;
+				key_index = PAIRWISE_KEYIDX;
 				is_pairwise = true;
 			}
 		}
 
-		if (rtlpriv->sec.key_len[key] == 0) {
+		if (rtlpriv->sec.key_len[key_index] == 0) {
 			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
-				 "delete one entry, id is %d\n", id);
+				 "delete one entry, entry_id is %d\n",
+				 entry_id);
 			if (mac->opmode == NL80211_IFTYPE_AP ||
-			    mac->opmode == NL80211_IFTYPE_MESH_POINT)
-				rtl_cam_del_entry(hw, mac_ad);
-			rtl_cam_delete_one_entry(hw, mac_ad, id);
+				mac->opmode == NL80211_IFTYPE_MESH_POINT)
+				rtl_cam_del_entry(hw, p_macaddr);
+			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
 		} else {
-			add_one_key(hw, macaddr, mac, key, id, enc_algo,
-				    is_pairwise);
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "add one entry\n");
+			if (is_pairwise) {
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set Pairwise key\n");
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						      entry_id, enc_algo,
+						      CAM_CONFIG_NO_USEDK,
+						      rtlpriv->sec.key_buf[key_index]);
+			} else {
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set group key\n");
+
+				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+					rtl_cam_add_one_entry(hw,
+							rtlefuse->dev_addr,
+							PAIRWISE_KEYIDX,
+							CAM_PAIRWISE_KEY_POSITION,
+							enc_algo,
+							CAM_CONFIG_NO_USEDK,
+							rtlpriv->sec.key_buf
+							[entry_id]);
+				}
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						      entry_id, enc_algo,
+						      CAM_CONFIG_NO_USEDK,
+						      rtlpriv->sec.key_buf[entry_id]);
+			}
+
 		}
 	}
 }
 
 static void rtl8188ee_bt_var_init(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
-	struct bt_coexist_info coexist = rppriv->bt_coexist;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	coexist.bt_coexistence = rppriv->bt_coexist.eeprom_bt_coexist;
-	coexist.bt_ant_num = coexist.eeprom_bt_ant_num;
-	coexist.bt_coexist_type = coexist.eeprom_bt_type;
+	rtlpriv->btcoexist.bt_coexistence =
+		rtlpriv->btcoexist.eeprom_bt_coexist;
+	rtlpriv->btcoexist.bt_ant_num = rtlpriv->btcoexist.eeprom_bt_ant_num;
+	rtlpriv->btcoexist.bt_coexist_type = rtlpriv->btcoexist.eeprom_bt_type;
 
-	if (coexist.reg_bt_iso == 2)
-		coexist.bt_ant_isolation = coexist.eeprom_bt_ant_isol;
+	if (rtlpriv->btcoexist.reg_bt_iso == 2)
+		rtlpriv->btcoexist.bt_ant_isolation =
+				rtlpriv->btcoexist.eeprom_bt_ant_isol;
 	else
-		coexist.bt_ant_isolation = coexist.reg_bt_iso;
-
-	coexist.bt_radio_shared_type = coexist.eeprom_bt_radio_shared;
-
-	if (coexist.bt_coexistence) {
-		if (coexist.reg_bt_sco == 1)
-			coexist.bt_service = BT_OTHER_ACTION;
-		else if (coexist.reg_bt_sco == 2)
-			coexist.bt_service = BT_SCO;
-		else if (coexist.reg_bt_sco == 4)
-			coexist.bt_service = BT_BUSY;
-		else if (coexist.reg_bt_sco == 5)
-			coexist.bt_service = BT_OTHERBUSY;
+		rtlpriv->btcoexist.bt_ant_isolation =
+				rtlpriv->btcoexist.reg_bt_iso;
+
+	rtlpriv->btcoexist.bt_radio_shared_type =
+		rtlpriv->btcoexist.eeprom_bt_radio_shared;
+
+	if (rtlpriv->btcoexist.bt_coexistence) {
+		if (rtlpriv->btcoexist.reg_bt_sco == 1)
+			rtlpriv->btcoexist.bt_service = BT_OTHER_ACTION;
+		else if (rtlpriv->btcoexist.reg_bt_sco == 2)
+			rtlpriv->btcoexist.bt_service = BT_SCO;
+		else if (rtlpriv->btcoexist.reg_bt_sco == 4)
+			rtlpriv->btcoexist.bt_service = BT_BUSY;
+		else if (rtlpriv->btcoexist.reg_bt_sco == 5)
+			rtlpriv->btcoexist.bt_service = BT_OTHERBUSY;
 		else
-			coexist.bt_service = BT_IDLE;
+			rtlpriv->btcoexist.bt_service = BT_IDLE;
 
-		coexist.bt_edca_ul = 0;
-		coexist.bt_edca_dl = 0;
-		coexist.bt_rssi_state = 0xff;
+		rtlpriv->btcoexist.bt_edca_ul = 0;
+		rtlpriv->btcoexist.bt_edca_dl = 0;
+		rtlpriv->btcoexist.bt_rssi_state = 0xff;
 	}
 }
 
 void rtl8188ee_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
 					      bool auto_load_fail, u8 *hwinfo)
 {
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value;
+
+	if (!auto_load_fail) {
+		rtlpriv->btcoexist.eeprom_bt_coexist =
+			((hwinfo[EEPROM_RF_FEATURE_OPTION_88E] & 0xe0) >> 5);
+		if (hwinfo[EEPROM_RF_FEATURE_OPTION_88E] == 0xFF)
+			rtlpriv->btcoexist.eeprom_bt_coexist  = 0;
+		value = hwinfo[EEPROM_RF_BT_SETTING_88E];
+		rtlpriv->btcoexist.eeprom_bt_type = ((value & 0xe) >> 1);
+		rtlpriv->btcoexist.eeprom_bt_ant_num = (value & 0x1);
+		rtlpriv->btcoexist.eeprom_bt_ant_isol = ((value & 0x10) >> 4);
+		rtlpriv->btcoexist.eeprom_bt_radio_shared =
+				 ((value & 0x20) >> 5);
+	} else {
+		rtlpriv->btcoexist.eeprom_bt_coexist = 0;
+		rtlpriv->btcoexist.eeprom_bt_type = BT_2WIRE;
+		rtlpriv->btcoexist.eeprom_bt_ant_num = ANT_X2;
+		rtlpriv->btcoexist.eeprom_bt_ant_isol = 0;
+		rtlpriv->btcoexist.eeprom_bt_radio_shared = BT_RADIO_SHARED;
+	}
+
 	rtl8188ee_bt_var_init(hw);
 }
 
 void rtl8188ee_bt_reg_init(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	/* 0:Low, 1:High, 2:From Efuse. */
-	rppriv->bt_coexist.reg_bt_iso = 2;
+	rtlpriv->btcoexist.reg_bt_iso = 2;
 	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
-	rppriv->bt_coexist.reg_bt_sco = 3;
+	rtlpriv->btcoexist.reg_bt_sco = 3;
 	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
-	rppriv->bt_coexist.reg_bt_sco = 0;
+	rtlpriv->btcoexist.reg_bt_sco = 0;
 }
 
 void rtl8188ee_bt_hw_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_pci_priv *rppriv = rtl_pcipriv(hw);
-	struct bt_coexist_info coexist = rppriv->bt_coexist;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 u1_tmp;
 
-	if (coexist.bt_coexistence &&
-	    ((coexist.bt_coexist_type == BT_CSR_BC4) ||
-	      coexist.bt_coexist_type == BT_CSR_BC8)) {
-		if (coexist.bt_ant_isolation)
+	if (rtlpriv->btcoexist.bt_coexistence &&
+	    ((rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4) ||
+	      rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC8)) {
+		if (rtlpriv->btcoexist.bt_ant_isolation)
 			rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG, 0xa0);
 
 		u1_tmp = rtl_read_byte(rtlpriv, 0x4fd) &
-				       BIT_OFFSET_LEN_MASK_32(0, 1);
-		u1_tmp = u1_tmp | ((coexist.bt_ant_isolation == 1) ?
+			 BIT_OFFSET_LEN_MASK_32(0, 1);
+		u1_tmp = u1_tmp |
+			 ((rtlpriv->btcoexist.bt_ant_isolation == 1) ?
 			 0 : BIT_OFFSET_LEN_MASK_32(1, 1)) |
-			 ((coexist.bt_service == BT_SCO) ?
+			 ((rtlpriv->btcoexist.bt_service == BT_SCO) ?
 			 0 : BIT_OFFSET_LEN_MASK_32(2, 1));
 		rtl_write_byte(rtlpriv, 0x4fd, u1_tmp);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/led.c b/drivers/net/wireless/rtlwifi/rtl8188ee/led.c
index c81a9cb6894c..b504bd092fc4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/led.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/led.c
@@ -32,8 +32,8 @@
 #include "reg.h"
 #include "led.h"
 
-static void rtl88ee_init_led(struct ieee80211_hw *hw,
-			     struct rtl_led *pled, enum rtl_led_pin ledpin)
+static void _rtl88ee_init_led(struct ieee80211_hw *hw,
+			      struct rtl_led *pled, enum rtl_led_pin ledpin)
 {
 	pled->hw = hw;
 	pled->ledpin = ledpin;
@@ -46,23 +46,23 @@ void rtl88ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
-		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	switch (pled->ledpin) {
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
 		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
-		rtl_write_byte(rtlpriv, REG_LEDCFG2,
-			       (ledcfg & 0xf0) | BIT(5) | BIT(6));
+		rtl_write_byte(rtlpriv,
+			       REG_LEDCFG2, (ledcfg & 0xf0) | BIT(5) | BIT(6));
 		break;
 	case LED_PIN_LED1:
 		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
 		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg & 0x10);
 		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
 	pled->ledon = true;
@@ -73,10 +73,9 @@ void rtl88ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	u8 ledcfg;
-	u8 val;
 
 	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
-		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	switch (pled->ledpin) {
 	case LED_PIN_GPIO0:
@@ -84,15 +83,15 @@ void rtl88ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 	case LED_PIN_LED0:
 		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
 		ledcfg &= 0xf0;
-		val = ledcfg | BIT(3) | BIT(5) | BIT(6);
-		if (pcipriv->ledctl.led_opendrain == true) {
-			rtl_write_byte(rtlpriv, REG_LEDCFG2, val);
+		if (pcipriv->ledctl.led_opendrain) {
+			rtl_write_byte(rtlpriv, REG_LEDCFG2,
+				       (ledcfg | BIT(3) | BIT(5) | BIT(6)));
 			ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
-			val = ledcfg & 0xFE;
-			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, val);
-		} else {
-			rtl_write_byte(rtlpriv, REG_LEDCFG2, val);
-		}
+			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG,
+				       (ledcfg & 0xFE));
+		} else
+			rtl_write_byte(rtlpriv, REG_LEDCFG2,
+				       (ledcfg | BIT(3) | BIT(5) | BIT(6)));
 		break;
 	case LED_PIN_LED1:
 		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
@@ -100,8 +99,8 @@ void rtl88ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 		rtl_write_byte(rtlpriv, REG_LEDCFG1, (ledcfg | BIT(3)));
 		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
 	pled->ledon = false;
@@ -110,17 +109,15 @@ void rtl88ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 void rtl88ee_init_sw_leds(struct ieee80211_hw *hw)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-
-	rtl88ee_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
-	rtl88ee_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+	_rtl88ee_init_led(hw, &pcipriv->ledctl.sw_led0, LED_PIN_LED0);
+	_rtl88ee_init_led(hw, &pcipriv->ledctl.sw_led1, LED_PIN_LED1);
 }
 
-static void rtl88ee_sw_led_control(struct ieee80211_hw *hw,
+static void _rtl88ee_sw_led_control(struct ieee80211_hw *hw,
 				    enum led_ctl_mode ledaction)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
-
 	switch (ledaction) {
 	case LED_CTL_POWER_ON:
 	case LED_CTL_LINK:
@@ -152,6 +149,6 @@ void rtl88ee_led_control(struct ieee80211_hw *hw,
 		return;
 	}
 	RT_TRACE(rtlpriv, COMP_LED, DBG_TRACE, "ledaction %d,\n",
-		 ledaction);
-	rtl88ee_sw_led_control(hw, ledaction);
+				ledaction);
+	_rtl88ee_sw_led_control(hw, ledaction);
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/led.h b/drivers/net/wireless/rtlwifi/rtl8188ee/led.h
index 4073f6f847b2..4b325b75faaf 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/led.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/led.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
index 1cd6c16d597e..3f6c59cdeaba 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -29,7 +25,6 @@
 
 #include "../wifi.h"
 #include "../pci.h"
-#include "../core.h"
 #include "../ps.h"
 #include "reg.h"
 #include "def.h"
@@ -38,443 +33,32 @@
 #include "dm.h"
 #include "table.h"
 
-static void set_baseband_phy_config(struct ieee80211_hw *hw);
-static void set_baseband_agc_config(struct ieee80211_hw *hw);
-static void store_pwrindex_offset(struct ieee80211_hw *hw,
-				  u32 regaddr, u32 bitmask,
-				  u32 data);
-static bool check_cond(struct ieee80211_hw *hw, const u32  condition);
-
-static u32 rf_serial_read(struct ieee80211_hw *hw,
-			  enum radio_path rfpath, u32 offset)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *phreg = &rtlphy->phyreg_def[rfpath];
-	u32 newoffset;
-	u32 tmplong, tmplong2;
-	u8 rfpi_enable = 0;
-	u32 ret;
-	int jj = RF90_PATH_A;
-	int kk = RF90_PATH_B;
-
-	offset &= 0xff;
-	newoffset = offset;
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "return all one\n");
-		return 0xFFFFFFFF;
-	}
-	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
-	if (rfpath == jj)
-		tmplong2 = tmplong;
-	else
-		tmplong2 = rtl_get_bbreg(hw, phreg->rfhssi_para2, MASKDWORD);
-	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
-	    (newoffset << 23) | BLSSIREADEDGE;
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
-		      tmplong & (~BLSSIREADEDGE));
-	mdelay(1);
-	rtl_set_bbreg(hw, phreg->rfhssi_para2, MASKDWORD, tmplong2);
-	mdelay(2);
-	if (rfpath == jj)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
-						 BIT(8));
-	else if (rfpath == kk)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
-						 BIT(8));
-	if (rfpi_enable)
-		ret = rtl_get_bbreg(hw, phreg->rf_rbpi, BLSSIREADBACKDATA);
-	else
-		ret = rtl_get_bbreg(hw, phreg->rf_rb, BLSSIREADBACKDATA);
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFR-%d Addr[0x%x]= 0x%x\n",
-		 rfpath, phreg->rf_rb, ret);
-	return ret;
-}
-
-static void rf_serial_write(struct ieee80211_hw *hw,
-			    enum radio_path rfpath, u32 offset,
-			    u32 data)
-{
-	u32 data_and_addr;
-	u32 newoffset;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *phreg = &rtlphy->phyreg_def[rfpath];
-
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "stop\n");
-		return;
-	}
-	offset &= 0xff;
-	newoffset = offset;
-	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
-	rtl_set_bbreg(hw, phreg->rf3wire_offset, MASKDWORD, data_and_addr);
-	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFW-%d Addr[0x%x]= 0x%x\n",
-		 rfpath, phreg->rf3wire_offset, data_and_addr);
-}
-
-static u32 cal_bit_shift(u32 bitmask)
-{
-	u32 i;
-
-	for (i = 0; i <= 31; i++) {
-		if (((bitmask >> i) & 0x1) == 1)
-			break;
-	}
-	return i;
-}
-
-static bool config_bb_with_header(struct ieee80211_hw *hw,
-				  u8 configtype)
-{
-	if (configtype == BASEBAND_CONFIG_PHY_REG)
-		set_baseband_phy_config(hw);
-	else if (configtype == BASEBAND_CONFIG_AGC_TAB)
-		set_baseband_agc_config(hw);
-	return true;
-}
-
-static bool config_bb_with_pgheader(struct ieee80211_hw *hw,
-				    u8 configtype)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i;
-	u32 *table_pg;
-	u16 tbl_page_len;
-	u32 v1 = 0, v2 = 0;
-
-	tbl_page_len = RTL8188EEPHY_REG_ARRAY_PGLEN;
-	table_pg = RTL8188EEPHY_REG_ARRAY_PG;
-
-	if (configtype == BASEBAND_CONFIG_PHY_REG) {
-		for (i = 0; i < tbl_page_len; i = i + 3) {
-			v1 = table_pg[i];
-			v2 = table_pg[i + 1];
-
-			if (v1 < 0xcdcdcdcd) {
-				rtl_addr_delay(table_pg[i]);
-
-				store_pwrindex_offset(hw, table_pg[i],
-						      table_pg[i + 1],
-						      table_pg[i + 2]);
-				continue;
-			} else {
-				if (!check_cond(hw, table_pg[i])) {
-					/*don't need the hw_body*/
-					i += 2; /* skip the pair of expression*/
-					v1 = table_pg[i];
-					v2 = table_pg[i + 1];
-					while (v2 != 0xDEAD) {
-						i += 3;
-						v1 = table_pg[i];
-						v2 = table_pg[i + 1];
-					}
-				}
-			}
-		}
-	} else {
-		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
-			 "configtype != BaseBand_Config_PHY_REG\n");
-	}
-	return true;
-}
-
-static bool config_parafile(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
-	bool rtstatus;
-
-	rtstatus = config_bb_with_header(hw, BASEBAND_CONFIG_PHY_REG);
-	if (rtstatus != true) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
-		return false;
-	}
-
-	if (fuse->autoload_failflag == false) {
-		rtlphy->pwrgroup_cnt = 0;
-		rtstatus = config_bb_with_pgheader(hw, BASEBAND_CONFIG_PHY_REG);
-	}
-	if (rtstatus != true) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
-		return false;
-	}
-	rtstatus = config_bb_with_header(hw, BASEBAND_CONFIG_AGC_TAB);
-	if (rtstatus != true) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
-		return false;
-	}
-	rtlphy->cck_high_power = (bool) (rtl_get_bbreg(hw,
-				 RFPGA0_XA_HSSIPARAMETER2, 0x200));
-
-	return true;
-}
-
-static void rtl88e_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	int jj = RF90_PATH_A;
-	int kk = RF90_PATH_B;
-
-	rtlphy->phyreg_def[jj].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-	rtlphy->phyreg_def[kk].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_C].rfintfs = RFPGA0_XCD_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_D].rfintfs = RFPGA0_XCD_RFINTERFACESW;
-
-	rtlphy->phyreg_def[jj].rfintfi = RFPGA0_XAB_RFINTERFACERB;
-	rtlphy->phyreg_def[kk].rfintfi = RFPGA0_XAB_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_C].rfintfi = RFPGA0_XCD_RFINTERFACERB;
-	rtlphy->phyreg_def[RF90_PATH_D].rfintfi = RFPGA0_XCD_RFINTERFACERB;
-
-	rtlphy->phyreg_def[jj].rfintfo = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[kk].rfintfo = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[jj].rfintfe = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[kk].rfintfe = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[jj].rf3wire_offset = RFPGA0_XA_LSSIPARAMETER;
-	rtlphy->phyreg_def[kk].rf3wire_offset = RFPGA0_XB_LSSIPARAMETER;
-
-	rtlphy->phyreg_def[jj].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[kk].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = rFPGA0_XCD_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = rFPGA0_XCD_RFPARAMETER;
-
-	rtlphy->phyreg_def[jj].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[kk].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_C].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-	rtlphy->phyreg_def[RF90_PATH_D].rftxgain_stage = RFPGA0_TXGAINSTAGE;
-
-	rtlphy->phyreg_def[jj].rfhssi_para1 = RFPGA0_XA_HSSIPARAMETER1;
-	rtlphy->phyreg_def[kk].rfhssi_para1 = RFPGA0_XB_HSSIPARAMETER1;
-
-	rtlphy->phyreg_def[jj].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
-	rtlphy->phyreg_def[kk].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
-
-	rtlphy->phyreg_def[jj].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
-	rtlphy->phyreg_def[kk].rfsw_ctrl = RFPGA0_XAB_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_C].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
-	rtlphy->phyreg_def[RF90_PATH_D].rfsw_ctrl = RFPGA0_XCD_SWITCHCONTROL;
-
-	rtlphy->phyreg_def[jj].rfagc_control1 = ROFDM0_XAAGCCORE1;
-	rtlphy->phyreg_def[kk].rfagc_control1 = ROFDM0_XBAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control1 = ROFDM0_XCAGCCORE1;
-	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control1 = ROFDM0_XDAGCCORE1;
-
-	rtlphy->phyreg_def[jj].rfagc_control2 = ROFDM0_XAAGCCORE2;
-	rtlphy->phyreg_def[kk].rfagc_control2 = ROFDM0_XBAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control2 = ROFDM0_XCAGCCORE2;
-	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control2 = ROFDM0_XDAGCCORE2;
-
-	rtlphy->phyreg_def[jj].rfrxiq_imbal = ROFDM0_XARXIQIMBAL;
-	rtlphy->phyreg_def[kk].rfrxiq_imbal = ROFDM0_XBRXIQIMBAL;
-	rtlphy->phyreg_def[RF90_PATH_C].rfrxiq_imbal = ROFDM0_XCRXIQIMBAL;
-	rtlphy->phyreg_def[RF90_PATH_D].rfrxiq_imbal = ROFDM0_XDRXIQIMBAL;
-
-	rtlphy->phyreg_def[jj].rfrx_afe = ROFDM0_XARXAFE;
-	rtlphy->phyreg_def[kk].rfrx_afe = ROFDM0_XBRXAFE;
-	rtlphy->phyreg_def[RF90_PATH_C].rfrx_afe = ROFDM0_XCRXAFE;
-	rtlphy->phyreg_def[RF90_PATH_D].rfrx_afe = ROFDM0_XDRXAFE;
-
-	rtlphy->phyreg_def[jj].rftxiq_imbal = ROFDM0_XATXIQIMBAL;
-	rtlphy->phyreg_def[kk].rftxiq_imbal = ROFDM0_XBTXIQIMBAL;
-	rtlphy->phyreg_def[RF90_PATH_C].rftxiq_imbal = ROFDM0_XCTXIQIMBAL;
-	rtlphy->phyreg_def[RF90_PATH_D].rftxiq_imbal = ROFDM0_XDTXIQIMBAL;
-
-	rtlphy->phyreg_def[jj].rftx_afe = ROFDM0_XATXAFE;
-	rtlphy->phyreg_def[kk].rftx_afe = ROFDM0_XBTXAFE;
-
-	rtlphy->phyreg_def[jj].rf_rb = RFPGA0_XA_LSSIREADBACK;
-	rtlphy->phyreg_def[kk].rf_rb = RFPGA0_XB_LSSIREADBACK;
-
-	rtlphy->phyreg_def[jj].rf_rbpi = TRANSCEIVEA_HSPI_READBACK;
-	rtlphy->phyreg_def[kk].rf_rbpi = TRANSCEIVEB_HSPI_READBACK;
-}
-
-static bool rtl88e_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
-					    u32 cmdtableidx, u32 cmdtablesz,
-					    enum swchnlcmd_id cmdid,
-					    u32 para1, u32 para2, u32 msdelay)
-{
-	struct swchnlcmd *pcmd;
-
-	if (cmdtable == NULL) {
-		RT_ASSERT(false, "cmdtable cannot be NULL.\n");
-		return false;
-	}
-
-	if (cmdtableidx >= cmdtablesz)
-		return false;
-
-	pcmd = cmdtable + cmdtableidx;
-	pcmd->cmdid = cmdid;
-	pcmd->para1 = para1;
-	pcmd->para2 = para2;
-	pcmd->msdelay = msdelay;
-	return true;
-}
-
-static bool chnl_step_by_step(struct ieee80211_hw *hw,
-			      u8 channel, u8 *stage, u8 *step,
-			      u32 *delay)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
-	u32 precommoncmdcnt;
-	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
-	u32 postcommoncmdcnt;
-	struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
-	u32 rfdependcmdcnt;
-	struct swchnlcmd *currentcmd = NULL;
-	u8 rfpath;
-	u8 num_total_rfpath = rtlphy->num_total_rfpath;
-
-	precommoncmdcnt = 0;
-	rtl88e_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
-					MAX_PRECMD_CNT,
-					CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
-	rtl88e_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
-					MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
-
-	postcommoncmdcnt = 0;
-
-	rtl88e_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
-					MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
-
-	rfdependcmdcnt = 0;
-
-	RT_ASSERT((channel >= 1 && channel <= 14),
-		  "illegal channel for Zebra: %d\n", channel);
-
-	rtl88e_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
-					MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
-					RF_CHNLBW, channel, 10);
-
-	rtl88e_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
-					MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0,
-					 0);
-
-	do {
-		switch (*stage) {
-		case 0:
-			currentcmd = &precommoncmd[*step];
-			break;
-		case 1:
-			currentcmd = &rfdependcmd[*step];
-			break;
-		case 2:
-			currentcmd = &postcommoncmd[*step];
-			break;
-		}
-
-		if (currentcmd->cmdid == CMDID_END) {
-			if ((*stage) == 2) {
-				return true;
-			} else {
-				(*stage)++;
-				(*step) = 0;
-				continue;
-			}
-		}
-
-		switch (currentcmd->cmdid) {
-		case CMDID_SET_TXPOWEROWER_LEVEL:
-			rtl88e_phy_set_txpower_level(hw, channel);
-			break;
-		case CMDID_WRITEPORT_ULONG:
-			rtl_write_dword(rtlpriv, currentcmd->para1,
-					currentcmd->para2);
-			break;
-		case CMDID_WRITEPORT_USHORT:
-			rtl_write_word(rtlpriv, currentcmd->para1,
-				       (u16) currentcmd->para2);
-			break;
-		case CMDID_WRITEPORT_UCHAR:
-			rtl_write_byte(rtlpriv, currentcmd->para1,
-				       (u8) currentcmd->para2);
-			break;
-		case CMDID_RF_WRITEREG:
-			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
-				rtlphy->rfreg_chnlval[rfpath] =
-				    ((rtlphy->rfreg_chnlval[rfpath] &
-				      0xfffffc00) | currentcmd->para2);
-
-				rtl_set_rfreg(hw, (enum radio_path)rfpath,
-					      currentcmd->para1,
-					      RFREG_OFFSET_MASK,
-					      rtlphy->rfreg_chnlval[rfpath]);
-			}
-			break;
-		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
-			break;
-		}
-
-		break;
-	} while (true);
-
-	(*delay) = currentcmd->msdelay;
-	(*step)++;
-	return false;
-}
-
-static long rtl88e_pwr_idx_dbm(struct ieee80211_hw *hw,
-			       enum wireless_mode wirelessmode,
-			       u8 txpwridx)
-{
-	long offset;
-	long pwrout_dbm;
-
-	switch (wirelessmode) {
-	case WIRELESS_MODE_B:
-		offset = -7;
-		break;
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-		offset = -8;
-		break;
-	default:
-		offset = -8;
-		break;
-	}
-	pwrout_dbm = txpwridx / 2 + offset;
-	return pwrout_dbm;
-}
-
-static void rtl88e_phy_set_io(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
-
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
-		 rtlphy->current_io_type, rtlphy->set_io_inprogress);
-	switch (rtlphy->current_io_type) {
-	case IO_CMD_RESUME_DM_BY_SCAN:
-		dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
-		/*rtl92c_dm_write_dig(hw);*/
-		rtl88e_phy_set_txpower_level(hw, rtlphy->current_channel);
-		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
-		break;
-	case IO_CMD_PAUSE_DM_BY_SCAN:
-		rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
-		dm_digtable->cur_igvalue = 0x17;
-		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
-		break;
-	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
-		break;
-	}
-	rtlphy->set_io_inprogress = false;
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 "(%#x)\n", rtlphy->current_io_type);
-}
+static u32 _rtl88e_phy_rf_serial_read(struct ieee80211_hw *hw,
+				      enum radio_path rfpath, u32 offset);
+static void _rtl88e_phy_rf_serial_write(struct ieee80211_hw *hw,
+					enum radio_path rfpath, u32 offset,
+					u32 data);
+static u32 _rtl88e_phy_calculate_bit_shift(u32 bitmask);
+static bool _rtl88e_phy_bb8188e_config_parafile(struct ieee80211_hw *hw);
+static bool _rtl88e_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+static bool phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+					  u8 configtype);
+static bool phy_config_bb_with_pghdr(struct ieee80211_hw *hw,
+				     u8 configtype);
+static void _rtl88e_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
+static bool _rtl88e_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					     u32 cmdtableidx, u32 cmdtablesz,
+					     enum swchnlcmd_id cmdid, u32 para1,
+					     u32 para2, u32 msdelay);
+static bool _rtl88e_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					     u8 channel, u8 *stage, u8 *step,
+					     u32 *delay);
+
+static long _rtl88e_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					 enum wireless_mode wirelessmode,
+					 u8 txpwridx);
+static void rtl88ee_phy_set_rf_on(struct ieee80211_hw *hw);
+static void rtl88e_phy_set_io(struct ieee80211_hw *hw);
 
 u32 rtl88e_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
 {
@@ -484,14 +68,15 @@ u32 rtl88e_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), bitmask(%#x)\n", regaddr, bitmask);
 	originalvalue = rtl_read_dword(rtlpriv, regaddr);
-	bitshift = cal_bit_shift(bitmask);
+	bitshift = _rtl88e_phy_calculate_bit_shift(bitmask);
 	returnvalue = (originalvalue & bitmask) >> bitshift;
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "BBR MASK = 0x%x Addr[0x%x]= 0x%x\n", bitmask,
+		 "BBR MASK=0x%x Addr[0x%x]=0x%x\n", bitmask,
 		 regaddr, originalvalue);
 
 	return returnvalue;
+
 }
 
 void rtl88e_phy_set_bb_reg(struct ieee80211_hw *hw,
@@ -501,12 +86,12 @@ void rtl88e_phy_set_bb_reg(struct ieee80211_hw *hw,
 	u32 originalvalue, bitshift;
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "regaddr(%#x), bitmask(%#x),data(%#x)\n",
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
 		 regaddr, bitmask, data);
 
 	if (bitmask != MASKDWORD) {
 		originalvalue = rtl_read_dword(rtlpriv, regaddr);
-		bitshift = cal_bit_shift(bitmask);
+		bitshift = _rtl88e_phy_calculate_bit_shift(bitmask);
 		data = ((originalvalue & (~bitmask)) | (data << bitshift));
 	}
 
@@ -531,8 +116,8 @@ u32 rtl88e_phy_query_rf_reg(struct ieee80211_hw *hw,
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 
 
-	original_value = rf_serial_read(hw, rfpath, regaddr);
-	bitshift = cal_bit_shift(bitmask);
+	original_value = _rtl88e_phy_rf_serial_read(hw, rfpath, regaddr);
+	bitshift = _rtl88e_phy_calculate_bit_shift(bitmask);
 	readback_value = (original_value & bitmask) >> bitshift;
 
 	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
@@ -540,7 +125,6 @@ u32 rtl88e_phy_query_rf_reg(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
 		  regaddr, rfpath, bitmask, original_value);
-
 	return readback_value;
 }
 
@@ -559,13 +143,16 @@ void rtl88e_phy_set_rf_reg(struct ieee80211_hw *hw,
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 
 	if (bitmask != RFREG_OFFSET_MASK) {
-			original_value = rf_serial_read(hw, rfpath, regaddr);
-			bitshift = cal_bit_shift(bitmask);
-			data = ((original_value & (~bitmask)) |
-				(data << bitshift));
+			original_value = _rtl88e_phy_rf_serial_read(hw,
+								    rfpath,
+								    regaddr);
+			bitshift = _rtl88e_phy_calculate_bit_shift(bitmask);
+			data =
+			    ((original_value & (~bitmask)) |
+			     (data << bitshift));
 		}
 
-	rf_serial_write(hw, rfpath, regaddr, data);
+	_rtl88e_phy_rf_serial_write(hw, rfpath, regaddr, data);
 
 
 	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
@@ -575,27 +162,91 @@ void rtl88e_phy_set_rf_reg(struct ieee80211_hw *hw,
 		 regaddr, bitmask, data, rfpath);
 }
 
-static bool config_mac_with_header(struct ieee80211_hw *hw)
+static u32 _rtl88e_phy_rf_serial_read(struct ieee80211_hw *hw,
+				      enum radio_path rfpath, u32 offset)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+	u32 newoffset;
+	u32 tmplong, tmplong2;
+	u8 rfpi_enable = 0;
+	u32 retvalue;
+
+	offset &= 0xff;
+	newoffset = offset;
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "return all one\n");
+		return 0xFFFFFFFF;
+	}
+	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
+	if (rfpath == RF90_PATH_A)
+		tmplong2 = tmplong;
+	else
+		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
+	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
+	    (newoffset << 23) | BLSSIREADEDGE;
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong & (~BLSSIREADEDGE));
+	mdelay(1);
+	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
+	mdelay(2);
+	if (rfpath == RF90_PATH_A)
+		rfpi_enable = (u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+						BIT(8));
+	else if (rfpath == RF90_PATH_B)
+		rfpi_enable = (u8)rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+						BIT(8));
+	if (rfpi_enable)
+		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rbpi,
+					 BLSSIREADBACKDATA);
+	else
+		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rb,
+					 BLSSIREADBACKDATA);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "RFR-%d Addr[0x%x]=0x%x\n",
+		 rfpath, pphyreg->rf_rb, retvalue);
+	return retvalue;
+}
+
+static void _rtl88e_phy_rf_serial_write(struct ieee80211_hw *hw,
+					enum radio_path rfpath, u32 offset,
+					u32 data)
+{
+	u32 data_and_addr;
+	u32 newoffset;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "stop\n");
+		return;
+	}
+	offset &= 0xff;
+	newoffset = offset;
+	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
+	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "RFW-%d Addr[0x%x]=0x%x\n",
+		 rfpath, pphyreg->rf3wire_offset, data_and_addr);
+}
+
+static u32 _rtl88e_phy_calculate_bit_shift(u32 bitmask)
+{
 	u32 i;
-	u32 arraylength;
-	u32 *ptrarray;
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read Rtl8188EMACPHY_Array\n");
-	arraylength = RTL8188EEMAC_1T_ARRAYLEN;
-	ptrarray = RTL8188EEMAC_1T_ARRAY;
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "Img:RTL8188EEMAC_1T_ARRAY LEN %d\n", arraylength);
-	for (i = 0; i < arraylength; i = i + 2)
-		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
-	return true;
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
 }
 
 bool rtl88e_phy_mac_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool rtstatus = config_mac_with_header(hw);
+	bool rtstatus = _rtl88e_phy_config_mac_with_headerfile(hw);
 
 	rtl_write_byte(rtlpriv, 0x04CA, 0x0B);
 	return rtstatus;
@@ -606,9 +257,9 @@ bool rtl88e_phy_bb_config(struct ieee80211_hw *hw)
 	bool rtstatus = true;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 regval;
-	u8 reg_hwparafile = 1;
+	u8 b_reg_hwparafile = 1;
 	u32 tmp;
-	rtl88e_phy_init_bb_rf_register_definition(hw);
+	_rtl88e_phy_init_bb_rf_register_definition(hw);
 	regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
 	rtl_write_word(rtlpriv, REG_SYS_FUNC_EN,
 		       regval | BIT(13) | BIT(0) | BIT(1));
@@ -619,8 +270,8 @@ bool rtl88e_phy_bb_config(struct ieee80211_hw *hw)
 		       FEN_BB_GLB_RSTN | FEN_BBRSTB);
 	tmp = rtl_read_dword(rtlpriv, 0x4c);
 	rtl_write_dword(rtlpriv, 0x4c, tmp | BIT(23));
-	if (reg_hwparafile == 1)
-		rtstatus = config_parafile(hw);
+	if (b_reg_hwparafile == 1)
+		rtstatus = _rtl88e_phy_bb8188e_config_parafile(hw);
 	return rtstatus;
 }
 
@@ -629,12 +280,12 @@ bool rtl88e_phy_rf_config(struct ieee80211_hw *hw)
 	return rtl88e_phy_rf6052_config(hw);
 }
 
-static bool check_cond(struct ieee80211_hw *hw,
+static bool _rtl88e_check_condition(struct ieee80211_hw *hw,
 				    const u32  condition)
 {
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
-	u32 _board = fuse->board_type; /*need efuse define*/
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 _board = rtlefuse->board_type; /*need efuse define*/
 	u32 _interface = rtlhal->interface;
 	u32 _platform = 0x08;/*SupportPlatform */
 	u32 cond = condition;
@@ -658,314 +309,504 @@ static bool check_cond(struct ieee80211_hw *hw,
 	return true;
 }
 
-static void _rtl8188e_config_rf_reg(struct ieee80211_hw *hw,
-				    u32 addr, u32 data, enum radio_path rfpath,
+static void _rtl8188e_config_rf_reg(struct ieee80211_hw *hw, u32 addr,
+				    u32 data, enum radio_path rfpath,
 				    u32 regaddr)
 {
-	rtl_rfreg_delay(hw, rfpath, regaddr,
-			RFREG_OFFSET_MASK,
-			data);
+	if (addr == 0xffe) {
+		mdelay(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		rtl_set_rfreg(hw, rfpath, regaddr,
+			      RFREG_OFFSET_MASK,
+			      data);
+		udelay(1);
+	}
 }
 
-static void rtl88_config_s(struct ieee80211_hw *hw,
-	u32 addr, u32 data)
+static void _rtl8188e_config_rf_radio_a(struct ieee80211_hw *hw,
+					u32 addr, u32 data)
 {
 	u32 content = 0x1000; /*RF Content: radio_a_txt*/
 	u32 maskforphyset = (u32)(content & 0xE000);
 
 	_rtl8188e_config_rf_reg(hw, addr, data, RF90_PATH_A,
-				addr | maskforphyset);
+		addr | maskforphyset);
+}
+
+static void _rtl8188e_config_bb_reg(struct ieee80211_hw *hw,
+				    u32 addr, u32 data)
+{
+	if (addr == 0xfe) {
+		mdelay(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		rtl_set_bbreg(hw, addr, MASKDWORD, data);
+		udelay(1);
+	}
 }
 
-#define NEXT_PAIR(v1, v2, i)				\
+static bool _rtl88e_phy_bb8188e_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus;
+
+	rtstatus = phy_config_bb_with_headerfile(hw, BASEBAND_CONFIG_PHY_REG);
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
+		return false;
+	}
+
+	if (!rtlefuse->autoload_failflag) {
+		rtlphy->pwrgroup_cnt = 0;
+		rtstatus =
+		  phy_config_bb_with_pghdr(hw, BASEBAND_CONFIG_PHY_REG);
+	}
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
+		return false;
+	}
+	rtstatus =
+	  phy_config_bb_with_headerfile(hw, BASEBAND_CONFIG_AGC_TAB);
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
+		return false;
+	}
+	rtlphy->cck_high_power =
+	  (bool)(rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, 0x200));
+
+	return true;
+}
+
+static bool _rtl88e_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read Rtl8188EMACPHY_Array\n");
+	arraylength = RTL8188EEMAC_1T_ARRAYLEN;
+	ptrarray = RTL8188EEMAC_1T_ARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Img:RTL8188EEMAC_1T_ARRAY LEN %d\n", arraylength);
+	for (i = 0; i < arraylength; i = i + 2)
+		rtl_write_byte(rtlpriv, ptrarray[i], (u8)ptrarray[i + 1]);
+	return true;
+}
+
+#define READ_NEXT_PAIR(v1, v2, i)			\
 	do {						\
 		i += 2; v1 = array_table[i];		\
-		v2 = array_table[i + 1];		\
+		v2 = array_table[i+1];			\
 	} while (0)
 
-static void set_baseband_agc_config(struct ieee80211_hw *hw)
+static void handle_branch1(struct ieee80211_hw *hw, u16 arraylen,
+			   u32 *array_table)
 {
+	u32 v1;
+	u32 v2;
 	int i;
-	u32 *array_table;
-	u16 arraylen;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 v1 = 0, v2 = 0;
 
-	arraylen = RTL8188EEAGCTAB_1TARRAYLEN;
-	array_table = RTL8188EEAGCTAB_1TARRAY;
+	for (i = 0; i < arraylen; i = i + 2) {
+		v1 = array_table[i];
+		v2 = array_table[i+1];
+		if (v1 < 0xcdcdcdcd) {
+			_rtl8188e_config_bb_reg(hw, v1, v2);
+		} else { /*This line is the start line of branch.*/
+			/* to protect READ_NEXT_PAIR not overrun */
+			if (i >= arraylen - 2)
+				break;
+
+			if (!_rtl88e_check_condition(hw, array_table[i])) {
+				/*Discard the following (offset, data) pairs*/
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+				i -= 2; /* prevent from for-loop += 2*/
+			} else { /* Configure matched pairs and skip
+				  * to end of if-else.
+				  */
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2)
+					_rtl8188e_config_bb_reg(hw, v1, v2);
+					READ_NEXT_PAIR(v1, v2, i);
+
+				while (v2 != 0xDEAD && i < arraylen - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+			}
+		}
+	}
+}
+
+static void handle_branch2(struct ieee80211_hw *hw, u16 arraylen,
+			   u32 *array_table)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1;
+	u32 v2;
+	int i;
 
-	for (i = 0; i < arraylen; i += 2) {
+	for (i = 0; i < arraylen; i = i + 2) {
 		v1 = array_table[i];
-		v2 = array_table[i + 1];
+		v2 = array_table[i+1];
 		if (v1 < 0xCDCDCDCD) {
 			rtl_set_bbreg(hw, array_table[i], MASKDWORD,
 				      array_table[i + 1]);
 			udelay(1);
 			continue;
-		} else {/*This line is the start line of branch.*/
-			if (!check_cond(hw, array_table[i])) {
+		} else { /*This line is the start line of branch.*/
+			/* to protect READ_NEXT_PAIR not overrun */
+			if (i >= arraylen - 2)
+				break;
+
+			if (!_rtl88e_check_condition(hw, array_table[i])) {
 				/*Discard the following (offset, data) pairs*/
-				NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD && v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2) {
-					NEXT_PAIR(v1, v2, i);
-				}
-				i -= 2; /* compensate for loop's += 2*/
-			} else {
-				/* Configure matched pairs and skip to end */
-				NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD && v2 != 0xCDEF &&
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylen - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+				i -= 2; /* prevent from for-loop += 2*/
+			} else { /* Configure matched pairs and skip
+				  * to end of if-else.
+				  */
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
 				       v2 != 0xCDCD && i < arraylen - 2) {
 					rtl_set_bbreg(hw, array_table[i],
 						      MASKDWORD,
 						      array_table[i + 1]);
 					udelay(1);
-					NEXT_PAIR(v1, v2, i);
+					READ_NEXT_PAIR(v1, v2, i);
 				}
 
 				while (v2 != 0xDEAD && i < arraylen - 2)
-					NEXT_PAIR(v1, v2, i);
+					READ_NEXT_PAIR(v1, v2, i);
 			}
 		}
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "The agctab_array_table[0] is %x Rtl818EEPHY_REGArray[1] is %x\n",
-			 array_table[i],
-			 array_table[i + 1]);
+			 array_table[i], array_table[i + 1]);
 	}
 }
 
-static void set_baseband_phy_config(struct ieee80211_hw *hw)
+static bool phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+					  u8 configtype)
 {
-	int i;
 	u32 *array_table;
 	u16 arraylen;
-	u32 v1 = 0, v2 = 0;
-
-	arraylen = RTL8188EEPHY_REG_1TARRAYLEN;
-	array_table = RTL8188EEPHY_REG_1TARRAY;
-
-	for (i = 0; i < arraylen; i += 2) {
-		v1 = array_table[i];
-		v2 = array_table[i + 1];
-		if (v1 < 0xcdcdcdcd) {
-			rtl_bb_delay(hw, v1, v2);
-		} else {/*This line is the start line of branch.*/
-			if (!check_cond(hw, array_table[i])) {
-				/*Discard the following (offset, data) pairs*/
-				NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2)
-					NEXT_PAIR(v1, v2, i);
-				i -= 2; /* prevent from for-loop += 2*/
-			} else {
-				/* Configure matched pairs and skip to end */
-				NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2) {
-					rtl_bb_delay(hw, v1, v2);
-					NEXT_PAIR(v1, v2, i);
-				}
 
-				while (v2 != 0xDEAD && i < arraylen - 2)
-					NEXT_PAIR(v1, v2, i);
-			}
-		}
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		arraylen = RTL8188EEPHY_REG_1TARRAYLEN;
+		array_table = RTL8188EEPHY_REG_1TARRAY;
+		handle_branch1(hw, arraylen, array_table);
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+		arraylen = RTL8188EEAGCTAB_1TARRAYLEN;
+		array_table = RTL8188EEAGCTAB_1TARRAY;
+		handle_branch2(hw, arraylen, array_table);
 	}
+	return true;
 }
 
-static void store_pwrindex_offset(struct ieee80211_hw *hw,
-				  u32 regaddr, u32 bitmask,
-				  u32 data)
+static void store_pwrindex_rate_offset(struct ieee80211_hw *hw,
+				       u32 regaddr, u32 bitmask,
+				       u32 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	int count = rtlphy->pwrgroup_cnt;
 
 	if (regaddr == RTXAGC_A_RATE18_06) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][0] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][0] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][0]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][0]);
 	}
 	if (regaddr == RTXAGC_A_RATE54_24) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][1] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][1] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][1] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][1]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][1]);
 	}
 	if (regaddr == RTXAGC_A_CCK1_MCS32) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][6] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][6] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][6] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][6]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][6]);
 	}
 	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][7] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][7] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][7] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][7]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][7]);
 	}
 	if (regaddr == RTXAGC_A_MCS03_MCS00) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][2] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][2] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][2] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][2]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][2]);
 	}
 	if (regaddr == RTXAGC_A_MCS07_MCS04) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][3] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][3] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][3] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][3]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][3]);
 	}
 	if (regaddr == RTXAGC_A_MCS11_MCS08) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][4] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][4] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][4] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][4]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][4]);
 	}
 	if (regaddr == RTXAGC_A_MCS15_MCS12) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][5] = data;
-		if (get_rf_type(rtlphy) == RF_1T1R)
-			rtlphy->pwrgroup_cnt++;
+		rtlphy->mcs_txpwrlevel_origoffset[count][5] = data;
+		if (get_rf_type(rtlphy) == RF_1T1R) {
+			count++;
+			rtlphy->pwrgroup_cnt = count;
+		}
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][5] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][5]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][5]);
 	}
 	if (regaddr == RTXAGC_B_RATE18_06) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][8] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][8] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][8] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][8]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][8]);
 	}
 	if (regaddr == RTXAGC_B_RATE54_24) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][9] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][9] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][9] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][9]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][9]);
 	}
 	if (regaddr == RTXAGC_B_CCK1_55_MCS32) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][14] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][14] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][14] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][14]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][14]);
 	}
 	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][15] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][15] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][15] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][15]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][15]);
 	}
 	if (regaddr == RTXAGC_B_MCS03_MCS00) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][10] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][10] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][10] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][10]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][10]);
 	}
 	if (regaddr == RTXAGC_B_MCS07_MCS04) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][11] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][11] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][11] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][11]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][11]);
 	}
 	if (regaddr == RTXAGC_B_MCS11_MCS08) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][12] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][12] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][12] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][12]);
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][12]);
 	}
 	if (regaddr == RTXAGC_B_MCS15_MCS12) {
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][13] = data;
+		rtlphy->mcs_txpwrlevel_origoffset[count][13] = data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][13] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][13]);
-		if (get_rf_type(rtlphy) != RF_1T1R)
-			rtlphy->pwrgroup_cnt++;
+			  count,
+			  rtlphy->mcs_txpwrlevel_origoffset[count][13]);
+		if (get_rf_type(rtlphy) != RF_1T1R) {
+			count++;
+			rtlphy->pwrgroup_cnt = count;
+		}
 	}
 }
 
-#define READ_NEXT_RF_PAIR(v1, v2, i)		\
-	do {					\
-		i += 2; v1 = a_table[i];	\
-		v2 = a_table[i + 1];		\
-	} while (0)
-
-bool rtl88e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
-					  enum radio_path rfpath)
+static bool phy_config_bb_with_pghdr(struct ieee80211_hw *hw, u8 configtype)
 {
-	int i;
-	u32 *a_table;
-	u16 a_len;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u32 v1 = 0, v2 = 0;
+	int i;
+	u32 *phy_reg_page;
+	u16 phy_reg_page_len;
+	u32 v1 = 0, v2 = 0, v3 = 0;
+
+	phy_reg_page_len = RTL8188EEPHY_REG_ARRAY_PGLEN;
+	phy_reg_page = RTL8188EEPHY_REG_ARRAY_PG;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		for (i = 0; i < phy_reg_page_len; i = i + 3) {
+			v1 = phy_reg_page[i];
+			v2 = phy_reg_page[i+1];
+			v3 = phy_reg_page[i+2];
 
-	a_len = RTL8188EE_RADIOA_1TARRAYLEN;
-	a_table = RTL8188EE_RADIOA_1TARRAY;
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "Radio_A:RTL8188EE_RADIOA_1TARRAY %d\n", a_len);
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
-	switch (rfpath) {
-	case RF90_PATH_A:
-		for (i = 0; i < a_len; i = i + 2) {
-			v1 = a_table[i];
-			v2 = a_table[i + 1];
 			if (v1 < 0xcdcdcdcd) {
-				rtl88_config_s(hw, v1, v2);
-			} else {/*This line is the start line of branch.*/
-				if (!check_cond(hw, a_table[i])) {
-					/* Discard the following (offset, data)
-					 * pairs
-					 */
+				if (phy_reg_page[i] == 0xfe)
+					mdelay(50);
+				else if (phy_reg_page[i] == 0xfd)
+					mdelay(5);
+				else if (phy_reg_page[i] == 0xfc)
+					mdelay(1);
+				else if (phy_reg_page[i] == 0xfb)
+					udelay(50);
+				else if (phy_reg_page[i] == 0xfa)
+					udelay(5);
+				else if (phy_reg_page[i] == 0xf9)
+					udelay(1);
+
+				store_pwrindex_rate_offset(hw, phy_reg_page[i],
+							   phy_reg_page[i + 1],
+							   phy_reg_page[i + 2]);
+				continue;
+			} else {
+				if (!_rtl88e_check_condition(hw,
+							     phy_reg_page[i])) {
+					/*don't need the hw_body*/
+				    i += 2; /* skip the pair of expression*/
+				    /* to protect 'i+1' 'i+2' not overrun */
+				    if (i >= phy_reg_page_len - 2)
+					break;
+
+				    v1 = phy_reg_page[i];
+				    v2 = phy_reg_page[i+1];
+				    v3 = phy_reg_page[i+2];
+				    while (v2 != 0xDEAD &&
+					   i < phy_reg_page_len - 5) {
+					i += 3;
+					v1 = phy_reg_page[i];
+					v2 = phy_reg_page[i+1];
+					v3 = phy_reg_page[i+2];
+				    }
+				}
+			}
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "configtype != BaseBand_Config_PHY_REG\n");
+	}
+	return true;
+}
+
+#define READ_NEXT_RF_PAIR(v1, v2, i) \
+do { \
+	i += 2; \
+	v1 = radioa_array_table[i]; \
+	v2 = radioa_array_table[i+1]; \
+} while (0)
+
+static void process_path_a(struct ieee80211_hw *hw,
+			   u16  radioa_arraylen,
+			   u32 *radioa_array_table)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 v1, v2;
+	int i;
+
+	for (i = 0; i < radioa_arraylen; i = i + 2) {
+		v1 = radioa_array_table[i];
+		v2 = radioa_array_table[i+1];
+		if (v1 < 0xcdcdcdcd) {
+			_rtl8188e_config_rf_radio_a(hw, v1, v2);
+		} else { /*This line is the start line of branch.*/
+			/* to protect READ_NEXT_PAIR not overrun */
+			if (i >= radioa_arraylen - 2)
+				break;
+
+			if (!_rtl88e_check_condition(hw, radioa_array_table[i])) {
+				/*Discard the following (offset, data) pairs*/
+				READ_NEXT_RF_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD &&
+				       i < radioa_arraylen - 2) {
 					READ_NEXT_RF_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD && v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < a_len - 2)
-						READ_NEXT_RF_PAIR(v1, v2, i);
-					i -= 2; /* prevent from for-loop += 2*/
-				} else {
-					/* Configure matched pairs and skip to
-					 * end of if-else.
-					 */
+				}
+				i -= 2; /* prevent from for-loop += 2*/
+			} else { /* Configure matched pairs and
+				  * skip to end of if-else.
+				  */
+				READ_NEXT_RF_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD &&
+				       i < radioa_arraylen - 2) {
+					_rtl8188e_config_rf_radio_a(hw, v1, v2);
 					READ_NEXT_RF_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD && v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < a_len - 2) {
-						rtl88_config_s(hw, v1, v2);
-						READ_NEXT_RF_PAIR(v1, v2, i);
-					}
-
-					while (v2 != 0xDEAD && i < a_len - 2)
-						READ_NEXT_RF_PAIR(v1, v2, i);
 				}
+
+				while (v2 != 0xDEAD &&
+				       i < radioa_arraylen - 2)
+					READ_NEXT_RF_PAIR(v1, v2, i);
 			}
 		}
+	}
 
-		if (rtlhal->oem_id == RT_CID_819X_HP)
-			rtl88_config_s(hw, 0x52, 0x7E4BD);
+	if (rtlhal->oem_id == RT_CID_819X_HP)
+		_rtl8188e_config_rf_radio_a(hw, 0x52, 0x7E4BD);
+}
 
-		break;
+bool rtl88e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					  enum radio_path rfpath)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool rtstatus = true;
+	u32 *radioa_array_table;
+	u16 radioa_arraylen;
 
+	radioa_arraylen = RTL8188EE_RADIOA_1TARRAYLEN;
+	radioa_array_table = RTL8188EE_RADIOA_1TARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Radio_A:RTL8188EE_RADIOA_1TARRAY %d\n", radioa_arraylen);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
+	rtstatus = true;
+	switch (rfpath) {
+	case RF90_PATH_A:
+		process_path_a(hw, radioa_arraylen, radioa_array_table);
+		break;
 	case RF90_PATH_B:
 	case RF90_PATH_C:
 	case RF90_PATH_D:
-	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
 		break;
 	}
 	return true;
@@ -974,26 +815,26 @@ bool rtl88e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 void rtl88e_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
-	rtlphy->default_initialgain[0] = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1,
-						       MASKBYTE0);
-	rtlphy->default_initialgain[1] = rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1,
-						       MASKBYTE0);
-	rtlphy->default_initialgain[2] = rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1,
-						       MASKBYTE0);
-	rtlphy->default_initialgain[3] = rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1,
-						       MASKBYTE0);
+	rtlphy->default_initialgain[0] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[1] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[2] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[3] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-		 "Default initial gain (c50 = 0x%x, c58 = 0x%x, c60 = 0x%x, c68 = 0x%x\n",
-		  rtlphy->default_initialgain[0],
-		  rtlphy->default_initialgain[1],
-		  rtlphy->default_initialgain[2],
-		  rtlphy->default_initialgain[3]);
-
-	rtlphy->framesync = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
-					  MASKBYTE0);
+		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
+		 rtlphy->default_initialgain[0],
+		 rtlphy->default_initialgain[1],
+		 rtlphy->default_initialgain[2],
+		 rtlphy->default_initialgain[3]);
+
+	rtlphy->framesync = (u8)rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
+					      MASKBYTE0);
 	rtlphy->framesync_c34 = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR2,
 					      MASKDWORD);
 
@@ -1002,106 +843,277 @@ void rtl88e_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 		 ROFDM0_RXDETECTOR3, rtlphy->framesync);
 }
 
+static void _rtl88e_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfs = RFPGA0_XCD_RFINTERFACESW;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfi = RFPGA0_XAB_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_C].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+	rtlphy->phyreg_def[RF90_PATH_D].rfintfi = RFPGA0_XCD_RFINTERFACERB;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset =
+	    RFPGA0_XA_LSSIPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
+	    RFPGA0_XB_LSSIPARAMETER;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_select = RFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_select = RFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = RFPGA0_XCD_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = RFPGA0_XCD_RFPARAMETER;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxgain_stage = RFPGA0_TXGAINSTAGE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para1 = RFPGA0_XA_HSSIPARAMETER1;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para1 = RFPGA0_XB_HSSIPARAMETER1;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfsw_ctrl =
+	    RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_B].rfsw_ctrl =
+	    RFPGA0_XAB_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_C].rfsw_ctrl =
+	    RFPGA0_XCD_SWITCHCONTROL;
+	rtlphy->phyreg_def[RF90_PATH_D].rfsw_ctrl =
+	    RFPGA0_XCD_SWITCHCONTROL;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control1 = ROFDM0_XAAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control1 = ROFDM0_XBAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control1 = ROFDM0_XCAGCCORE1;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control1 = ROFDM0_XDAGCCORE1;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfagc_control2 = ROFDM0_XAAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_B].rfagc_control2 = ROFDM0_XBAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_C].rfagc_control2 = ROFDM0_XCAGCCORE2;
+	rtlphy->phyreg_def[RF90_PATH_D].rfagc_control2 = ROFDM0_XDAGCCORE2;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfrxiq_imbal = ROFDM0_XARXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfrxiq_imbal = ROFDM0_XBRXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrxiq_imbal = ROFDM0_XCRXIQIMBANLANCE;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrxiq_imbal = ROFDM0_XDRXIQIMBALANCE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfrx_afe = ROFDM0_XARXAFE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfrx_afe = ROFDM0_XBRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_C].rfrx_afe = ROFDM0_XCRXAFE;
+	rtlphy->phyreg_def[RF90_PATH_D].rfrx_afe = ROFDM0_XDRXAFE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftxiq_imbal = ROFDM0_XATXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftxiq_imbal = ROFDM0_XBTXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_C].rftxiq_imbal = ROFDM0_XCTXIQIMBALANCE;
+	rtlphy->phyreg_def[RF90_PATH_D].rftxiq_imbal = ROFDM0_XDTXIQIMBALANCE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rftx_afe = ROFDM0_XATXAFE;
+	rtlphy->phyreg_def[RF90_PATH_B].rftx_afe = ROFDM0_XBTXAFE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rb = RFPGA0_XA_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rb = RFPGA0_XB_LSSIREADBACK;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rbpi = TRANSCEIVEA_HSPI_READBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rbpi = TRANSCEIVEB_HSPI_READBACK;
+}
+
 void rtl88e_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 level;
-	long dbm;
-
-	level = rtlphy->cur_cck_txpwridx;
-	dbm = rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_B, level);
-	level = rtlphy->cur_ofdm24g_txpwridx;
-	if (rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_G, level) > dbm)
-		dbm = rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_G, level);
-	level = rtlphy->cur_ofdm24g_txpwridx;
-	if (rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_N_24G, level) > dbm)
-		dbm = rtl88e_pwr_idx_dbm(hw, WIRELESS_MODE_N_24G, level);
-	*powerlevel = dbm;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 txpwr_level;
+	long txpwr_dbm;
+
+	txpwr_level = rtlphy->cur_cck_txpwridx;
+	txpwr_dbm = _rtl88e_phy_txpwr_idx_to_dbm(hw,
+						 WIRELESS_MODE_B, txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl88e_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl88e_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+						 txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl88e_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_N_24G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl88e_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+						 txpwr_level);
+	*powerlevel = txpwr_dbm;
+}
+
+static void handle_path_a(struct rtl_efuse *rtlefuse, u8 index,
+			  u8 *cckpowerlevel, u8 *ofdmpowerlevel,
+			  u8 *bw20powerlevel, u8 *bw40powerlevel)
+{
+	cckpowerlevel[RF90_PATH_A] =
+	    rtlefuse->txpwrlevel_cck[RF90_PATH_A][index];
+		/*-8~7 */
+	if (rtlefuse->txpwr_ht20diff[RF90_PATH_A][index] > 0x0f)
+		bw20powerlevel[RF90_PATH_A] =
+		  rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_A][index] -
+		  (~(rtlefuse->txpwr_ht20diff[RF90_PATH_A][index]) + 1);
+	else
+		bw20powerlevel[RF90_PATH_A] =
+		  rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_A][index] +
+		  rtlefuse->txpwr_ht20diff[RF90_PATH_A][index];
+	if (rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][index] > 0xf)
+		ofdmpowerlevel[RF90_PATH_A] =
+		  rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_A][index] -
+		  (~(rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][index])+1);
+	else
+		ofdmpowerlevel[RF90_PATH_A] =
+		rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_A][index] +
+		  rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][index];
+	bw40powerlevel[RF90_PATH_A] =
+	  rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_A][index];
 }
 
 static void _rtl88e_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
-				      u8 *cckpower, u8 *ofdm, u8 *bw20_pwr,
-				      u8 *bw40_pwr)
+				      u8 *cckpowerlevel, u8 *ofdmpowerlevel,
+				      u8 *bw20powerlevel, u8 *bw40powerlevel)
 {
-	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
-	u8 i = (channel - 1);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 index = (channel - 1);
 	u8 rf_path = 0;
-	int jj = RF90_PATH_A;
-	int kk = RF90_PATH_B;
 
 	for (rf_path = 0; rf_path < 2; rf_path++) {
-		if (rf_path == jj) {
-			cckpower[jj] = fuse->txpwrlevel_cck[jj][i];
-			if (fuse->txpwr_ht20diff[jj][i] > 0x0f) /*-8~7 */
-				bw20_pwr[jj] = fuse->txpwrlevel_ht40_1s[jj][i] -
-					(~(fuse->txpwr_ht20diff[jj][i]) + 1);
-			else
-				bw20_pwr[jj] = fuse->txpwrlevel_ht40_1s[jj][i] +
-					 fuse->txpwr_ht20diff[jj][i];
-			if (fuse->txpwr_legacyhtdiff[jj][i] > 0xf)
-				ofdm[jj] = fuse->txpwrlevel_ht40_1s[jj][i] -
-					(~(fuse->txpwr_legacyhtdiff[jj][i])+1);
-			else
-				ofdm[jj] = fuse->txpwrlevel_ht40_1s[jj][i] +
-					   fuse->txpwr_legacyhtdiff[jj][i];
-			bw40_pwr[jj] = fuse->txpwrlevel_ht40_1s[jj][i];
-
-		} else if (rf_path == kk) {
-			cckpower[kk] = fuse->txpwrlevel_cck[kk][i];
-			bw20_pwr[kk] = fuse->txpwrlevel_ht40_1s[kk][i] +
-				       fuse->txpwr_ht20diff[kk][i];
-			ofdm[kk] = fuse->txpwrlevel_ht40_1s[kk][i] +
-					fuse->txpwr_legacyhtdiff[kk][i];
-			bw40_pwr[kk] = fuse->txpwrlevel_ht40_1s[kk][i];
+		if (rf_path == RF90_PATH_A) {
+			handle_path_a(rtlefuse, index, cckpowerlevel,
+				      ofdmpowerlevel, bw20powerlevel,
+				      bw40powerlevel);
+		} else if (rf_path == RF90_PATH_B) {
+			cckpowerlevel[RF90_PATH_B] =
+			  rtlefuse->txpwrlevel_cck[RF90_PATH_B][index];
+			bw20powerlevel[RF90_PATH_B] =
+			  rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_B][index] +
+			  rtlefuse->txpwr_ht20diff[RF90_PATH_B][index];
+			ofdmpowerlevel[RF90_PATH_B] =
+			  rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_B][index] +
+			  rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][index];
+			bw40powerlevel[RF90_PATH_B] =
+			  rtlefuse->txpwrlevel_ht40_1s[RF90_PATH_B][index];
 		}
 	}
+
 }
 
 static void _rtl88e_ccxpower_index_check(struct ieee80211_hw *hw,
-					 u8 channel, u8 *cckpower,
-					 u8 *ofdm, u8 *bw20_pwr,
-					 u8 *bw40_pwr)
+					 u8 channel, u8 *cckpowerlevel,
+					 u8 *ofdmpowerlevel, u8 *bw20powerlevel,
+					 u8 *bw40powerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	rtlphy->cur_cck_txpwridx = cckpowerlevel[0];
+	rtlphy->cur_ofdm24g_txpwridx = ofdmpowerlevel[0];
+	rtlphy->cur_bw20_txpwridx = bw20powerlevel[0];
+	rtlphy->cur_bw40_txpwridx = bw40powerlevel[0];
 
-	rtlphy->cur_cck_txpwridx = cckpower[0];
-	rtlphy->cur_ofdm24g_txpwridx = ofdm[0];
-	rtlphy->cur_bw20_txpwridx = bw20_pwr[0];
-	rtlphy->cur_bw40_txpwridx = bw40_pwr[0];
 }
 
 void rtl88e_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
 {
-	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
-	u8 cckpower[MAX_TX_COUNT]  = {0}, ofdm[MAX_TX_COUNT] = {0};
-	u8 bw20_pwr[MAX_TX_COUNT] = {0}, bw40_pwr[MAX_TX_COUNT] = {0};
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 cckpowerlevel[MAX_TX_COUNT]  = {0};
+	u8 ofdmpowerlevel[MAX_TX_COUNT] = {0};
+	u8 bw20powerlevel[MAX_TX_COUNT] = {0};
+	u8 bw40powerlevel[MAX_TX_COUNT] = {0};
 
-	if (fuse->txpwr_fromeprom == false)
+	if (!rtlefuse->txpwr_fromeprom)
 		return;
-	_rtl88e_get_txpower_index(hw, channel, &cckpower[0], &ofdm[0],
-				  &bw20_pwr[0], &bw40_pwr[0]);
-	_rtl88e_ccxpower_index_check(hw, channel, &cckpower[0], &ofdm[0],
-				     &bw20_pwr[0], &bw40_pwr[0]);
-	rtl88e_phy_rf6052_set_cck_txpower(hw, &cckpower[0]);
-	rtl88e_phy_rf6052_set_ofdm_txpower(hw, &ofdm[0], &bw20_pwr[0],
-					   &bw40_pwr[0], channel);
+	_rtl88e_get_txpower_index(hw, channel,
+				  &cckpowerlevel[0], &ofdmpowerlevel[0],
+				  &bw20powerlevel[0], &bw40powerlevel[0]);
+	_rtl88e_ccxpower_index_check(hw, channel,
+				     &cckpowerlevel[0], &ofdmpowerlevel[0],
+				     &bw20powerlevel[0], &bw40powerlevel[0]);
+	rtl88e_phy_rf6052_set_cck_txpower(hw, &cckpowerlevel[0]);
+	rtl88e_phy_rf6052_set_ofdm_txpower(hw, &ofdmpowerlevel[0],
+					   &bw20powerlevel[0],
+					   &bw40powerlevel[0], channel);
+}
+
+static long _rtl88e_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					 enum wireless_mode wirelessmode,
+					 u8 txpwridx)
+{
+	long offset;
+	long pwrout_dbm;
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		offset = -7;
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
+	default:
+		offset = -8;
+		break;
+	}
+	pwrout_dbm = txpwridx / 2 + offset;
+	return pwrout_dbm;
+}
+
+void rtl88e_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum io_type iotype;
+
+	if (!is_hal_stop(rtlhal)) {
+		switch (operation) {
+		case SCAN_OPT_BACKUP_BAND0:
+			iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Unknown Scan Backup operation.\n");
+			break;
+		}
+	}
 }
 
 void rtl88e_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u8 reg_bw_opmode;
 	u8 reg_prsr_rsc;
 
 	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
 		 "Switch to %s bandwidth\n",
-		 rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
-		 "20MHz" : "40MHz");
+		  rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" : "40MHz");
 
 	if (is_hal_stop(rtlhal)) {
 		rtlphy->set_bwmode_inprogress = false;
@@ -1162,7 +1174,7 @@ void rtl88e_phy_set_bw_mode(struct ieee80211_hw *hw,
 			    enum nl80211_channel_type ch_type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 tmp_bw = rtlphy->current_chan_bw;
 
@@ -1173,7 +1185,7 @@ void rtl88e_phy_set_bw_mode(struct ieee80211_hw *hw,
 		rtl88e_phy_set_bw_mode_callback(hw);
 	} else {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "FALSE driver sleep or unload\n");
+			 "false driver sleep or unload\n");
 		rtlphy->set_bwmode_inprogress = false;
 		rtlphy->current_chan_bw = tmp_bw;
 	}
@@ -1183,7 +1195,7 @@ void rtl88e_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 delay;
 
 	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
@@ -1193,9 +1205,9 @@ void rtl88e_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 	do {
 		if (!rtlphy->sw_chnl_inprogress)
 			break;
-		if (!chnl_step_by_step(hw, rtlphy->current_channel,
-				       &rtlphy->sw_chnl_stage,
-				       &rtlphy->sw_chnl_step, &delay)) {
+		if (!_rtl88e_phy_sw_chnl_step_by_step
+		    (hw, rtlphy->current_channel, &rtlphy->sw_chnl_stage,
+		     &rtlphy->sw_chnl_step, &delay)) {
 			if (delay > 0)
 				mdelay(delay);
 			else
@@ -1211,7 +1223,7 @@ void rtl88e_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 u8 rtl88e_phy_sw_chnl(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	if (rtlphy->sw_chnl_inprogress)
@@ -1237,9 +1249,140 @@ u8 rtl88e_phy_sw_chnl(struct ieee80211_hw *hw)
 	return 1;
 }
 
+static bool _rtl88e_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					     u8 channel, u8 *stage, u8 *step,
+					     u32 *delay)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
+	u32 precommoncmdcnt;
+	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
+	u32 postcommoncmdcnt;
+	struct swchnlcmd rfdependcmd[MAX_RFDEPENDCMD_CNT];
+	u32 rfdependcmdcnt;
+	struct swchnlcmd *currentcmd = NULL;
+	u8 rfpath;
+	u8 num_total_rfpath = rtlphy->num_total_rfpath;
+
+	precommoncmdcnt = 0;
+	_rtl88e_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT,
+					 CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
+	_rtl88e_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
+					 MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+
+	postcommoncmdcnt = 0;
+
+	_rtl88e_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
+					 MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
+
+	rfdependcmdcnt = 0;
+
+	RT_ASSERT((channel >= 1 && channel <= 14),
+		  "illegal channel for Zebra: %d\n", channel);
+
+	_rtl88e_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
+					 RF_CHNLBW, channel, 10);
+
+	_rtl88e_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
+					 MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0,
+					 0);
+
+	do {
+		switch (*stage) {
+		case 0:
+			currentcmd = &precommoncmd[*step];
+			break;
+		case 1:
+			currentcmd = &rfdependcmd[*step];
+			break;
+		case 2:
+			currentcmd = &postcommoncmd[*step];
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Invalid 'stage' = %d, Check it!\n", *stage);
+			return true;
+		}
+
+		if (currentcmd->cmdid == CMDID_END) {
+			if ((*stage) == 2)
+				return true;
+			(*stage)++;
+			(*step) = 0;
+			continue;
+		}
+
+		switch (currentcmd->cmdid) {
+		case CMDID_SET_TXPOWEROWER_LEVEL:
+			rtl88e_phy_set_txpower_level(hw, channel);
+			break;
+		case CMDID_WRITEPORT_ULONG:
+			rtl_write_dword(rtlpriv, currentcmd->para1,
+					currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_USHORT:
+			rtl_write_word(rtlpriv, currentcmd->para1,
+				       (u16)currentcmd->para2);
+			break;
+		case CMDID_WRITEPORT_UCHAR:
+			rtl_write_byte(rtlpriv, currentcmd->para1,
+				       (u8)currentcmd->para2);
+			break;
+		case CMDID_RF_WRITEREG:
+			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
+				rtlphy->rfreg_chnlval[rfpath] =
+				    ((rtlphy->rfreg_chnlval[rfpath] &
+				      0xfffffc00) | currentcmd->para2);
+
+				rtl_set_rfreg(hw, (enum radio_path)rfpath,
+					      currentcmd->para1,
+					      RFREG_OFFSET_MASK,
+					      rtlphy->rfreg_chnlval[rfpath]);
+			}
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
+			break;
+		}
+
+		break;
+	} while (true);
+
+	(*delay) = currentcmd->msdelay;
+	(*step)++;
+	return false;
+}
+
+static bool _rtl88e_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
+					     u32 cmdtableidx, u32 cmdtablesz,
+					     enum swchnlcmd_id cmdid,
+					     u32 para1, u32 para2, u32 msdelay)
+{
+	struct swchnlcmd *pcmd;
+
+	if (cmdtable == NULL) {
+		RT_ASSERT(false, "cmdtable cannot be NULL.\n");
+		return false;
+	}
+
+	if (cmdtableidx >= cmdtablesz)
+		return false;
+
+	pcmd = cmdtable + cmdtableidx;
+	pcmd->cmdid = cmdid;
+	pcmd->para1 = para1;
+	pcmd->para2 = para2;
+	pcmd->msdelay = msdelay;
+	return true;
+}
+
 static u8 _rtl88e_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
 {
-	u32 reg_eac, reg_e94, reg_e9c;
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
 	u8 result = 0x00;
 
 	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c1c);
@@ -1256,6 +1399,7 @@ static u8 _rtl88e_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
 	reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
 	reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, 0xea4, MASKDWORD);
 
 	if (!(reg_eac & BIT(28)) &&
 	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
@@ -1295,15 +1439,14 @@ static u8 _rtl88e_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 {
 	u32 reg_eac, reg_e94, reg_e9c, reg_ea4, u32temp;
 	u8 result = 0x00;
-	int jj = RF90_PATH_A;
 
 	/*Get TXIMR Setting*/
 	/*Modify RX IQK mode table*/
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
-	rtl_set_rfreg(hw, jj, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
-	rtl_set_rfreg(hw, jj, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
-	rtl_set_rfreg(hw, jj, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
-	rtl_set_rfreg(hw, jj, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
 
 	/*IQK Setting*/
@@ -1318,7 +1461,7 @@ static u8 _rtl88e_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 
 	/*LO calibration Setting*/
 	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
-	/*one shot, path A LOK & iqk*/
+	/*one shot,path A LOK & iqk*/
 	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
 	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
@@ -1336,16 +1479,16 @@ static u8 _rtl88e_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	else
 		return result;
 
-	u32temp = 0x80007C00 | (reg_e94&0x3FF0000)  |
+	u32temp = 0x80007C00 | (reg_e94&0x3FF0000) |
 		  ((reg_e9c&0x3FF0000) >> 16);
 	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32temp);
 	/*RX IQK*/
 	/*Modify RX IQK mode table*/
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
-	rtl_set_rfreg(hw, jj, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
-	rtl_set_rfreg(hw, jj, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
-	rtl_set_rfreg(hw, jj, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
-	rtl_set_rfreg(hw, jj, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
 
 	/*IQK Setting*/
@@ -1359,7 +1502,7 @@ static u8 _rtl88e_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 
 	/*LO calibration Setting*/
 	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
-	/*one shot, path A LOK & iqk*/
+	/*one shot,path A LOK & iqk*/
 	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
 	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
@@ -1377,57 +1520,58 @@ static u8 _rtl88e_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	return result;
 }
 
-static void fill_iqk(struct ieee80211_hw *hw, bool iqk_ok, long result[][8],
-		     u8 final, bool btxonly)
+static void _rtl88e_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
+					       bool iqk_ok, long result[][8],
+					       u8 final_candidate, bool btxonly)
 {
 	u32 oldval_0, x, tx0_a, reg;
 	long y, tx0_c;
 
-	if (final == 0xFF) {
+	if (final_candidate == 0xFF) {
 		return;
 	} else if (iqk_ok) {
-		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBAL,
+		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
 					  MASKDWORD) >> 22) & 0x3FF;
-		x = result[final][0];
+		x = result[final_candidate][0];
 		if ((x & 0x00000200) != 0)
 			x = x | 0xFFFFFC00;
 		tx0_a = (x * oldval_0) >> 8;
-		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, 0x3FF, tx0_a);
-		rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(31),
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x3FF, tx0_a);
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(31),
 			      ((x * oldval_0 >> 7) & 0x1));
-		y = result[final][1];
+		y = result[final_candidate][1];
 		if ((y & 0x00000200) != 0)
-			y |= 0xFFFFFC00;
+			y = y | 0xFFFFFC00;
 		tx0_c = (y * oldval_0) >> 8;
 		rtl_set_bbreg(hw, ROFDM0_XCTXAFE, 0xF0000000,
 			      ((tx0_c & 0x3C0) >> 6));
-		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBAL, 0x003F0000,
+		rtl_set_bbreg(hw, ROFDM0_XATXIQIMBALANCE, 0x003F0000,
 			      (tx0_c & 0x3F));
-		rtl_set_bbreg(hw, ROFDM0_ECCATHRES, BIT(29),
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(29),
 			      ((y * oldval_0 >> 7) & 0x1));
 		if (btxonly)
 			return;
-		reg = result[final][2];
-		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBAL, 0x3FF, reg);
-		reg = result[final][3] & 0x3F;
-		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBAL, 0xFC00, reg);
-		reg = (result[final][3] >> 6) & 0xF;
+		reg = result[final_candidate][2];
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0x3FF, reg);
+		reg = result[final_candidate][3] & 0x3F;
+		rtl_set_bbreg(hw, ROFDM0_XARXIQIMBALANCE, 0xFC00, reg);
+		reg = (result[final_candidate][3] >> 6) & 0xF;
 		rtl_set_bbreg(hw, 0xca0, 0xF0000000, reg);
 	}
 }
 
-static void save_adda_reg(struct ieee80211_hw *hw,
-			  const u32 *addareg, u32 *backup,
-			  u32 registernum)
+static void _rtl88e_phy_save_adda_registers(struct ieee80211_hw *hw,
+					    u32 *addareg, u32 *addabackup,
+					    u32 registernum)
 {
 	u32 i;
 
 	for (i = 0; i < registernum; i++)
-		backup[i] = rtl_get_bbreg(hw, addareg[i], MASKDWORD);
+		addabackup[i] = rtl_get_bbreg(hw, addareg[i], MASKDWORD);
 }
 
-static void save_mac_reg(struct ieee80211_hw *hw, const u32 *macreg,
-			 u32 *macbackup)
+static void _rtl88e_phy_save_mac_registers(struct ieee80211_hw *hw,
+					   u32 *macreg, u32 *macbackup)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 i;
@@ -1437,17 +1581,18 @@ static void save_mac_reg(struct ieee80211_hw *hw, const u32 *macreg,
 	macbackup[i] = rtl_read_dword(rtlpriv, macreg[i]);
 }
 
-static void reload_adda(struct ieee80211_hw *hw, const u32 *addareg,
-		        u32 *backup, u32 reg_num)
+static void _rtl88e_phy_reload_adda_registers(struct ieee80211_hw *hw,
+					      u32 *addareg, u32 *addabackup,
+					      u32 regiesternum)
 {
 	u32 i;
 
-	for (i = 0; i < reg_num; i++)
-		rtl_set_bbreg(hw, addareg[i], MASKDWORD, backup[i]);
+	for (i = 0; i < regiesternum; i++)
+		rtl_set_bbreg(hw, addareg[i], MASKDWORD, addabackup[i]);
 }
 
-static void reload_mac(struct ieee80211_hw *hw, const u32 *macreg,
-		       u32 *macbackup)
+static void _rtl88e_phy_reload_mac_registers(struct ieee80211_hw *hw,
+					     u32 *macreg, u32 *macbackup)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 i;
@@ -1458,8 +1603,7 @@ static void reload_mac(struct ieee80211_hw *hw, const u32 *macreg,
 }
 
 static void _rtl88e_phy_path_adda_on(struct ieee80211_hw *hw,
-				     const u32 *addareg, bool is_patha_on,
-				     bool is2t)
+				     u32 *addareg, bool is_patha_on, bool is2t)
 {
 	u32 pathon;
 	u32 i;
@@ -1477,8 +1621,7 @@ static void _rtl88e_phy_path_adda_on(struct ieee80211_hw *hw,
 }
 
 static void _rtl88e_phy_mac_setting_calibration(struct ieee80211_hw *hw,
-						const u32 *macreg,
-						u32 *macbackup)
+						u32 *macreg, u32 *macbackup)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 i = 0;
@@ -1507,12 +1650,13 @@ static void _rtl88e_phy_pi_mode_switch(struct ieee80211_hw *hw, bool pi_mode)
 	rtl_set_bbreg(hw, 0x828, MASKDWORD, mode);
 }
 
-static bool sim_comp(struct ieee80211_hw *hw, long result[][8], u8 c1, u8 c2)
+static bool _rtl88e_phy_simularity_compare(struct ieee80211_hw *hw,
+					   long result[][8], u8 c1, u8 c2)
 {
-	u32 i, j, diff, bitmap, bound;
+	u32 i, j, diff, simularity_bitmap, bound;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
-	u8 final[2] = {0xFF, 0xFF};
+	u8 final_candidate[2] = { 0xFF, 0xFF };
 	bool bresult = true, is2t = IS_92C_SERIAL(rtlhal->version);
 
 	if (is2t)
@@ -1520,81 +1664,88 @@ static bool sim_comp(struct ieee80211_hw *hw, long result[][8], u8 c1, u8 c2)
 	else
 		bound = 4;
 
-	bitmap = 0;
+	simularity_bitmap = 0;
 
 	for (i = 0; i < bound; i++) {
 		diff = (result[c1][i] > result[c2][i]) ?
-		       (result[c1][i] - result[c2][i]) :
-		       (result[c2][i] - result[c1][i]);
+		    (result[c1][i] - result[c2][i]) :
+		    (result[c2][i] - result[c1][i]);
 
 		if (diff > MAX_TOLERANCE) {
-			if ((i == 2 || i == 6) && !bitmap) {
+			if ((i == 2 || i == 6) && !simularity_bitmap) {
 				if (result[c1][i] + result[c1][i + 1] == 0)
-					final[(i / 4)] = c2;
+					final_candidate[(i / 4)] = c2;
 				else if (result[c2][i] + result[c2][i + 1] == 0)
-					final[(i / 4)] = c1;
+					final_candidate[(i / 4)] = c1;
 				else
-					bitmap = bitmap | (1 << i);
-			} else {
-				bitmap = bitmap | (1 << i);
-			}
+					simularity_bitmap = simularity_bitmap |
+					    (1 << i);
+			} else
+				simularity_bitmap =
+				    simularity_bitmap | (1 << i);
 		}
 	}
 
-	if (bitmap == 0) {
+	if (simularity_bitmap == 0) {
 		for (i = 0; i < (bound / 4); i++) {
-			if (final[i] != 0xFF) {
+			if (final_candidate[i] != 0xFF) {
 				for (j = i * 4; j < (i + 1) * 4 - 2; j++)
-					result[3][j] = result[final[i]][j];
+					result[3][j] =
+					    result[final_candidate[i]][j];
 				bresult = false;
 			}
 		}
 		return bresult;
-	} else if (!(bitmap & 0x0F)) {
+	} else if (!(simularity_bitmap & 0x0F)) {
 		for (i = 0; i < 4; i++)
 			result[3][i] = result[c1][i];
 		return false;
-	} else if (!(bitmap & 0xF0) && is2t) {
+	} else if (!(simularity_bitmap & 0xF0) && is2t) {
 		for (i = 4; i < 8; i++)
 			result[3][i] = result[c1][i];
 		return false;
 	} else {
 		return false;
 	}
+
 }
 
 static void _rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw,
 				     long result[][8], u8 t, bool is2t)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 i;
 	u8 patha_ok, pathb_ok;
-	const u32 adda_reg[IQK_ADDA_REG_NUM] = {
+	u32 adda_reg[IQK_ADDA_REG_NUM] = {
 		0x85c, 0xe6c, 0xe70, 0xe74,
 		0xe78, 0xe7c, 0xe80, 0xe84,
 		0xe88, 0xe8c, 0xed0, 0xed4,
 		0xed8, 0xedc, 0xee0, 0xeec
 	};
-	const u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
+	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
 		0x522, 0x550, 0x551, 0x040
 	};
-	const u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
-		ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR, RFPGA0_XCD_RFINTERFACESW,
-		0xb68, 0xb6c, 0x870, 0x860, 0x864, 0x800
+	u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
+		ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR,
+		RFPGA0_XCD_RFINTERFACESW, 0xb68, 0xb6c,
+		0x870, 0x860, 0x864, 0x800
 	};
 	const u32 retrycount = 2;
 
 	if (t == 0) {
-		save_adda_reg(hw, adda_reg, rtlphy->adda_backup, 16);
-		save_mac_reg(hw, iqk_mac_reg, rtlphy->iqk_mac_backup);
-		save_adda_reg(hw, iqk_bb_reg, rtlphy->iqk_bb_backup,
-			      IQK_BB_REG_NUM);
+		_rtl88e_phy_save_adda_registers(hw, adda_reg,
+						rtlphy->adda_backup, 16);
+		_rtl88e_phy_save_mac_registers(hw, iqk_mac_reg,
+					       rtlphy->iqk_mac_backup);
+		_rtl88e_phy_save_adda_registers(hw, iqk_bb_reg,
+						rtlphy->iqk_bb_backup,
+						IQK_BB_REG_NUM);
 	}
 	_rtl88e_phy_path_adda_on(hw, adda_reg, true, is2t);
 	if (t == 0) {
-		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
-					   RFPGA0_XA_HSSIPARAMETER1, BIT(8));
+		rtlphy->rfpi_enable =
+		  (u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1, BIT(8));
 	}
 
 	if (!rtlphy->rfpi_enable)
@@ -1652,10 +1803,9 @@ static void _rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw,
 		}
 	}
 
-	if (0 == patha_ok) {
+	if (0 == patha_ok)
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 			 "Path A IQK Success!!\n");
-	}
 	if (is2t) {
 		_rtl88e_phy_path_a_standby(hw);
 		_rtl88e_phy_path_adda_on(hw, adda_reg, false, is2t);
@@ -1663,21 +1813,23 @@ static void _rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw,
 			pathb_ok = _rtl88e_phy_path_b_iqk(hw);
 			if (pathb_ok == 0x03) {
 				result[t][4] = (rtl_get_bbreg(hw,
-						0xeb4, MASKDWORD) &
+							      0xeb4,
+							      MASKDWORD) &
 						0x3FF0000) >> 16;
 				result[t][5] =
 				    (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
-						   0x3FF0000) >> 16;
+				     0x3FF0000) >> 16;
 				result[t][6] =
 				    (rtl_get_bbreg(hw, 0xec4, MASKDWORD) &
-						   0x3FF0000) >> 16;
+				     0x3FF0000) >> 16;
 				result[t][7] =
 				    (rtl_get_bbreg(hw, 0xecc, MASKDWORD) &
-						   0x3FF0000) >> 16;
+				     0x3FF0000) >> 16;
 				break;
 			} else if (i == (retrycount - 1) && pathb_ok == 0x01) {
 				result[t][4] = (rtl_get_bbreg(hw,
-						0xeb4, MASKDWORD) &
+							      0xeb4,
+							      MASKDWORD) &
 						0x3FF0000) >> 16;
 			}
 			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
@@ -1690,10 +1842,13 @@ static void _rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw,
 	if (t != 0) {
 		if (!rtlphy->rfpi_enable)
 			_rtl88e_phy_pi_mode_switch(hw, false);
-		reload_adda(hw, adda_reg, rtlphy->adda_backup, 16);
-		reload_mac(hw, iqk_mac_reg, rtlphy->iqk_mac_backup);
-		reload_adda(hw, iqk_bb_reg, rtlphy->iqk_bb_backup,
-			    IQK_BB_REG_NUM);
+		_rtl88e_phy_reload_adda_registers(hw, adda_reg,
+						  rtlphy->adda_backup, 16);
+		_rtl88e_phy_reload_mac_registers(hw, iqk_mac_reg,
+						 rtlphy->iqk_mac_backup);
+		_rtl88e_phy_reload_adda_registers(hw, iqk_bb_reg,
+						  rtlphy->iqk_bb_backup,
+						  IQK_BB_REG_NUM);
 
 		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00032ed3);
 		if (is2t)
@@ -1709,8 +1864,6 @@ static void _rtl88e_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 	u8 tmpreg;
 	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int jj = RF90_PATH_A;
-	int kk = RF90_PATH_B;
 
 	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
 
@@ -1720,51 +1873,52 @@ static void _rtl88e_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
 
 	if ((tmpreg & 0x70) != 0) {
-		rf_a_mode = rtl_get_rfreg(hw, jj, 0x00, MASK12BITS);
+		rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
 
 		if (is2t)
-			rf_b_mode = rtl_get_rfreg(hw, kk, 0x00,
+			rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
 						  MASK12BITS);
 
-		rtl_set_rfreg(hw, jj, 0x00, MASK12BITS,
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
 			      (rf_a_mode & 0x8FFFF) | 0x10000);
 
 		if (is2t)
-			rtl_set_rfreg(hw, kk, 0x00, MASK12BITS,
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
 				      (rf_b_mode & 0x8FFFF) | 0x10000);
 	}
-	lc_cal = rtl_get_rfreg(hw, jj, 0x18, MASK12BITS);
+	lc_cal = rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
 
-	rtl_set_rfreg(hw, jj, 0x18, MASK12BITS, lc_cal | 0x08000);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, lc_cal | 0x08000);
 
 	mdelay(100);
 
 	if ((tmpreg & 0x70) != 0) {
 		rtl_write_byte(rtlpriv, 0xd03, tmpreg);
-		rtl_set_rfreg(hw, jj, 0x00, MASK12BITS, rf_a_mode);
+		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
 
 		if (is2t)
-			rtl_set_rfreg(hw, kk, 0x00, MASK12BITS,
+			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
 				      rf_b_mode);
 	} else {
 		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
 	}
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+
 }
 
-static void rfpath_switch(struct ieee80211_hw *hw,
-			  bool bmain, bool is2t)
+static void _rtl88e_phy_set_rfpath_switch(struct ieee80211_hw *hw,
+					  bool bmain, bool is2t)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_efuse *fuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
 
 	if (is_hal_stop(rtlhal)) {
 		u8 u1btmp;
 		u1btmp = rtl_read_byte(rtlpriv, REG_LEDCFG0);
 		rtl_write_byte(rtlpriv, REG_LEDCFG0, u1btmp | BIT(7));
-		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+		rtl_set_bbreg(hw, RFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
 	}
 	if (is2t) {
 		if (bmain)
@@ -1777,24 +1931,24 @@ static void rfpath_switch(struct ieee80211_hw *hw,
 		rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(8) | BIT(9), 0);
 		rtl_set_bbreg(hw, 0x914, MASKLWORD, 0x0201);
 
-		/* We use the RF definition of MAIN and AUX, left antenna and
-		 * right antenna repectively.
+		/* We use the RF definition of MAIN and AUX,
+		 * left antenna and right antenna repectively.
 		 * Default output at AUX.
 		 */
 		if (bmain) {
-			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BIT(14) |
-				      BIT(13) | BIT(12), 0);
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BIT(5) |
-				      BIT(4) | BIT(3), 0);
-			if (fuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
-				rtl_set_bbreg(hw, RCONFIG_RAM64X16, BIT(31), 0);
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
+				      BIT(14) | BIT(13) | BIT(12), 0);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(4) | BIT(3), 0);
+			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, RCONFIG_RAM64x16, BIT(31), 0);
 		} else {
-			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, BIT(14) |
-				      BIT(13) | BIT(12), 1);
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE, BIT(5) |
-				      BIT(4) | BIT(3), 1);
-			if (fuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
-				rtl_set_bbreg(hw, RCONFIG_RAM64X16, BIT(31), 1);
+			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
+				      BIT(14) | BIT(13) | BIT(12), 1);
+			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
+				      BIT(5) | BIT(4) | BIT(3), 1);
+			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
+				rtl_set_bbreg(hw, RCONFIG_RAM64x16, BIT(31), 1);
 		}
 	}
 }
@@ -1802,35 +1956,44 @@ static void rfpath_switch(struct ieee80211_hw *hw,
 #undef IQK_ADDA_REG_NUM
 #undef IQK_DELAY_TIME
 
-void rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
+void rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	long result[4][8];
-	u8 i, final;
-	bool patha_ok;
-	long reg_e94, reg_e9c, reg_ea4, reg_eb4, reg_ebc, reg_tmp = 0;
+	u8 i, final_candidate;
+	bool b_patha_ok, b_pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
+	    reg_ecc, reg_tmp = 0;
 	bool is12simular, is13simular, is23simular;
 	u32 iqk_bb_reg[9] = {
-		ROFDM0_XARXIQIMBAL,
-		ROFDM0_XBRXIQIMBAL,
-		ROFDM0_ECCATHRES,
+		ROFDM0_XARXIQIMBALANCE,
+		ROFDM0_XBRXIQIMBALANCE,
+		ROFDM0_ECCATHRESHOLD,
 		ROFDM0_AGCRSSITABLE,
-		ROFDM0_XATXIQIMBAL,
-		ROFDM0_XBTXIQIMBAL,
+		ROFDM0_XATXIQIMBALANCE,
+		ROFDM0_XBTXIQIMBALANCE,
 		ROFDM0_XCTXAFE,
 		ROFDM0_XDTXAFE,
 		ROFDM0_RXIQEXTANTA
 	};
 
-	if (recovery) {
-		reload_adda(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 9);
+	if (b_recovery) {
+		_rtl88e_phy_reload_adda_registers(hw,
+						  iqk_bb_reg,
+						  rtlphy->iqk_bb_backup, 9);
 		return;
 	}
 
-	memset(result, 0, 32 * sizeof(long));
-	final = 0xff;
-	patha_ok = false;
+	for (i = 0; i < 8; i++) {
+		result[0][i] = 0;
+		result[1][i] = 0;
+		result[2][i] = 0;
+		result[3][i] = 0;
+	}
+	final_candidate = 0xff;
+	b_patha_ok = false;
+	b_pathb_ok = false;
 	is12simular = false;
 	is23simular = false;
 	is13simular = false;
@@ -1840,29 +2003,32 @@ void rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		else
 			_rtl88e_phy_iq_calibrate(hw, result, i, false);
 		if (i == 1) {
-			is12simular = sim_comp(hw, result, 0, 1);
+			is12simular =
+			  _rtl88e_phy_simularity_compare(hw, result, 0, 1);
 			if (is12simular) {
-				final = 0;
+				final_candidate = 0;
 				break;
 			}
 		}
 		if (i == 2) {
-			is13simular = sim_comp(hw, result, 0, 2);
+			is13simular =
+			  _rtl88e_phy_simularity_compare(hw, result, 0, 2);
 			if (is13simular) {
-				final = 0;
+				final_candidate = 0;
 				break;
 			}
-			is23simular = sim_comp(hw, result, 1, 2);
+			is23simular =
+			   _rtl88e_phy_simularity_compare(hw, result, 1, 2);
 			if (is23simular) {
-				final = 1;
+				final_candidate = 1;
 			} else {
 				for (i = 0; i < 8; i++)
 					reg_tmp += result[3][i];
 
 				if (reg_tmp != 0)
-					final = 3;
+					final_candidate = 3;
 				else
-					final = 0xFF;
+					final_candidate = 0xFF;
 			}
 		}
 	}
@@ -1870,47 +2036,55 @@ void rtl88e_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		reg_e94 = result[i][0];
 		reg_e9c = result[i][1];
 		reg_ea4 = result[i][2];
+		reg_eac = result[i][3];
 		reg_eb4 = result[i][4];
 		reg_ebc = result[i][5];
+		reg_ec4 = result[i][6];
+		reg_ecc = result[i][7];
 	}
-	if (final != 0xff) {
-		reg_e94 = result[final][0];
-		rtlphy->reg_e94 = reg_e94;
-		reg_e9c = result[final][1];
-		rtlphy->reg_e9c = reg_e9c;
-		reg_ea4 = result[final][2];
-		reg_eb4 = result[final][4];
+	if (final_candidate != 0xff) {
+		reg_e94 = result[final_candidate][0];
+		reg_e9c = result[final_candidate][1];
+		reg_ea4 = result[final_candidate][2];
+		reg_eac = result[final_candidate][3];
+		reg_eb4 = result[final_candidate][4];
+		reg_ebc = result[final_candidate][5];
+		reg_ec4 = result[final_candidate][6];
+		reg_ecc = result[final_candidate][7];
 		rtlphy->reg_eb4 = reg_eb4;
-		reg_ebc = result[final][5];
 		rtlphy->reg_ebc = reg_ebc;
-		patha_ok = true;
+		rtlphy->reg_e94 = reg_e94;
+		rtlphy->reg_e9c = reg_e9c;
+		b_patha_ok = true;
+		b_pathb_ok = true;
 	} else {
 		rtlphy->reg_e94 = 0x100;
 		rtlphy->reg_eb4 = 0x100;
-		rtlphy->reg_ebc = 0x0;
 		rtlphy->reg_e9c = 0x0;
+		rtlphy->reg_ebc = 0x0;
 	}
 	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
-		fill_iqk(hw, patha_ok, result, final, (reg_ea4 == 0));
-	if (final != 0xFF) {
+		_rtl88e_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
+						   final_candidate,
+						   (reg_ea4 == 0));
+	if (final_candidate != 0xFF) {
 		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
-			rtlphy->iqk_matrix[0].value[0][i] = result[final][i];
+			rtlphy->iqk_matrix[0].value[0][i] =
+				result[final_candidate][i];
 		rtlphy->iqk_matrix[0].iqk_done = true;
+
 	}
-	save_adda_reg(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 9);
+	_rtl88e_phy_save_adda_registers(hw, iqk_bb_reg,
+					rtlphy->iqk_bb_backup, 9);
 }
 
 void rtl88e_phy_lc_calibrate(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
-	bool start_conttx = false, singletone = false;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = &rtlpriv->rtlhal;
 	u32 timeout = 2000, timecount = 0;
 
-	if (start_conttx || singletone)
-		return;
-
 	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
 		udelay(50);
 		timecount += 50;
@@ -1928,18 +2102,18 @@ void rtl88e_phy_lc_calibrate(struct ieee80211_hw *hw)
 
 void rtl88e_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
 {
-	rfpath_switch(hw, bmain, false);
+	_rtl88e_phy_set_rfpath_switch(hw, bmain, false);
 }
 
 bool rtl88e_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	bool postprocessing = false;
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
-		 iotype, rtlphy->set_io_inprogress);
+		  iotype, rtlphy->set_io_inprogress);
 	do {
 		switch (iotype) {
 		case IO_CMD_RESUME_DM_BY_SCAN:
@@ -1947,14 +2121,14 @@ bool rtl88e_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 				 "[IO CMD] Resume DM after scan.\n");
 			postprocessing = true;
 			break;
-		case IO_CMD_PAUSE_DM_BY_SCAN:
+		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 				 "[IO CMD] Pause DM before scan.\n");
 			postprocessing = true;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 	} while (false);
@@ -1969,6 +2143,37 @@ bool rtl88e_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 	return true;
 }
 
+static void rtl88e_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
+		/*rtl92c_dm_write_dig(hw);*/
+		rtl88e_phy_set_txpower_level(hw, rtlphy->current_channel);
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
+		break;
+	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
+		dm_digtable->cur_igvalue = 0x17;
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "(%#x)\n", rtlphy->current_io_type);
+}
+
 static void rtl88ee_phy_set_rf_on(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1984,10 +2189,9 @@ static void rtl88ee_phy_set_rf_on(struct ieee80211_hw *hw)
 static void _rtl88ee_phy_set_rf_sleep(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int jj = RF90_PATH_A;
 
 	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
-	rtl_set_rfreg(hw, jj, 0x00, RFREG_OFFSET_MASK, 0x00);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
 	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
 }
@@ -1999,42 +2203,49 @@ static bool _rtl88ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl8192_tx_ring *ring = NULL;
 	bool bresult = true;
 	u8 i, queue_id;
+	struct rtl8192_tx_ring *ring = NULL;
 
 	switch (rfpwr_state) {
-	case ERFON:{
+	case ERFON:
 		if ((ppsc->rfpwr_state == ERFOFF) &&
 		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
 			bool rtstatus;
-			u32 init = 0;
+			u32 initializecount = 0;
+
 			do {
-				init++;
+				initializecount++;
 				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 					 "IPS Set eRf nic enable\n");
 				rtstatus = rtl_ps_enable_nic(hw);
-			} while ((rtstatus != true) && (init < 10));
+			} while (!rtstatus &&
+				 (initializecount < 10));
 			RT_CLEAR_PS_LEVEL(ppsc,
 					  RT_RF_OFF_LEVL_HALT_NIC);
 		} else {
 			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 				 "Set ERFON sleeped:%d ms\n",
-				 jiffies_to_msecs(jiffies - ppsc->
-						  last_sleep_jiffies));
+				  jiffies_to_msecs(jiffies -
+						   ppsc->
+						   last_sleep_jiffies));
 			ppsc->last_awake_jiffies = jiffies;
 			rtl88ee_phy_set_rf_on(hw);
 		}
-		if (mac->link_state == MAC80211_LINKED)
-			rtlpriv->cfg->ops->led_control(hw, LED_CTL_LINK);
-		else
-			rtlpriv->cfg->ops->led_control(hw, LED_CTL_NO_LINK);
-		break; }
-	case ERFOFF:{
+		if (mac->link_state == MAC80211_LINKED) {
+			rtlpriv->cfg->ops->led_control(hw,
+						       LED_CTL_LINK);
+		} else {
+			rtlpriv->cfg->ops->led_control(hw,
+						       LED_CTL_NO_LINK);
+		}
+		break;
+	case ERFOFF:
 		for (queue_id = 0, i = 0;
 		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
 			ring = &pcipriv->dev.tx_ring[queue_id];
-			if (skb_queue_len(&ring->queue) == 0) {
+			if (queue_id == BEACON_QUEUE ||
+			    skb_queue_len(&ring->queue) == 0) {
 				queue_id++;
 				continue;
 			} else {
@@ -2055,6 +2266,7 @@ static bool _rtl88ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
 				break;
 			}
 		}
+
 		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
 			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 				 "IPS Set eRf nic disable\n");
@@ -2063,49 +2275,51 @@ static bool _rtl88ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		} else {
 			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
 				rtlpriv->cfg->ops->led_control(hw,
-						LED_CTL_NO_LINK);
+							       LED_CTL_NO_LINK);
 			} else {
 				rtlpriv->cfg->ops->led_control(hw,
-						LED_CTL_POWER_OFF);
+							       LED_CTL_POWER_OFF);
 			}
 		}
-		break; }
+		break;
 	case ERFSLEEP:{
-		if (ppsc->rfpwr_state == ERFOFF)
-			break;
-		for (queue_id = 0, i = 0;
-		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
-			ring = &pcipriv->dev.tx_ring[queue_id];
-			if (skb_queue_len(&ring->queue) == 0) {
-				queue_id++;
-				continue;
-			} else {
-				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
-					 (i + 1), queue_id,
-					 skb_queue_len(&ring->queue));
-
-				udelay(10);
-				i++;
-			}
-			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
-				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
-					 MAX_DOZE_WAITING_TIMES_9x,
-					 queue_id,
-					 skb_queue_len(&ring->queue));
+			if (ppsc->rfpwr_state == ERFOFF)
 				break;
+			for (queue_id = 0, i = 0;
+			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+				ring = &pcipriv->dev.tx_ring[queue_id];
+				if (skb_queue_len(&ring->queue) == 0) {
+					queue_id++;
+					continue;
+				} else {
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+						 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+						 (i + 1), queue_id,
+						 skb_queue_len(&ring->queue));
+
+					udelay(10);
+					i++;
+				}
+				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+						 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+						 MAX_DOZE_WAITING_TIMES_9x,
+						 queue_id,
+						 skb_queue_len(&ring->queue));
+					break;
+				}
 			}
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "Set ERFSLEEP awaked:%d ms\n",
+				  jiffies_to_msecs(jiffies -
+				  ppsc->last_awake_jiffies));
+			ppsc->last_sleep_jiffies = jiffies;
+			_rtl88ee_phy_set_rf_sleep(hw);
+			break;
 		}
-		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
-			 "Set ERFSLEEP awaked:%d ms\n",
-			 jiffies_to_msecs(jiffies - ppsc->last_awake_jiffies));
-		ppsc->last_sleep_jiffies = jiffies;
-		_rtl88ee_phy_set_rf_sleep(hw);
-		break; }
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		bresult = false;
 		break;
 	}
@@ -2118,10 +2332,11 @@ bool rtl88e_phy_set_rf_power_state(struct ieee80211_hw *hw,
 				   enum rf_pwrstate rfpwr_state)
 {
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool bresult;
+
+	bool bresult = false;
 
 	if (rfpwr_state == ppsc->rfpwr_state)
-		return false;
+		return bresult;
 	bresult = _rtl88ee_phy_set_rf_power_state(hw, rfpwr_state);
 	return bresult;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.h b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.h
index 89f0f1ef1465..b29bd77210f4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/phy.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,33 +26,35 @@
 #ifndef __RTL92C_PHY_H__
 #define __RTL92C_PHY_H__
 
-/*It must always set to 4, otherwise read efuse table secquence will be wrong.*/
-#define	MAX_TX_COUNT				4
+/* MAX_TX_COUNT must always set to 4, otherwise read efuse
+ * table secquence will be wrong.
+ */
+#define		MAX_TX_COUNT				4
 
 #define MAX_PRECMD_CNT				16
-#define MAX_RFDEPENDCMD_CNT			16
+#define MAX_RFDEPENDCMD_CNT		16
 #define MAX_POSTCMD_CNT				16
 
-#define MAX_DOZE_WAITING_TIMES_9x		64
+#define MAX_DOZE_WAITING_TIMES_9x	64
 
 #define RT_CANNOT_IO(hw)			false
-#define HIGHPOWER_RADIOA_ARRAYLEN		22
+#define HIGHPOWER_RADIOA_ARRAYLEN	22
 
 #define IQK_ADDA_REG_NUM			16
 #define IQK_BB_REG_NUM				9
 #define MAX_TOLERANCE				5
 #define	IQK_DELAY_TIME				10
-#define	IDX_MAP					15
+#define	INDEX_MAPPING_NUM	15
 
 #define	APK_BB_REG_NUM				5
 #define	APK_AFE_REG_NUM				16
 #define	APK_CURVE_REG_NUM			4
-#define	PATH_NUM				2
+#define	PATH_NUM					2
 
-#define LOOP_LIMIT				5
+#define LOOP_LIMIT					5
 #define MAX_STALL_TIME				50
-#define ANTENNADIVERSITYVALUE			0x80
-#define MAX_TXPWR_IDX_NMODE_92S			63
+#define ANTENNADIVERSITYVALUE		0x80
+#define MAX_TXPWR_IDX_NMODE_92S		63
 #define RESET_CNT_LIMIT				3
 
 #define IQK_ADDA_REG_NUM			16
@@ -66,8 +64,8 @@
 
 #define CT_OFFSET_MAC_ADDR			0X16
 
-#define CT_OFFSET_CCK_TX_PWR_IDX		0x5A
-#define CT_OFFSET_HT401S_TX_PWR_IDX		0x60
+#define CT_OFFSET_CCK_TX_PWR_IDX			0x5A
+#define CT_OFFSET_HT401S_TX_PWR_IDX			0x60
 #define CT_OFFSET_HT402S_TX_PWR_IDX_DIFF	0x66
 #define CT_OFFSET_HT20_TX_PWR_IDX_DIFF		0x69
 #define CT_OFFSET_OFDM_TX_PWR_IDX_DIFF		0x6C
@@ -75,13 +73,13 @@
 #define CT_OFFSET_HT40_MAX_PWR_OFFSET		0x6F
 #define CT_OFFSET_HT20_MAX_PWR_OFFSET		0x72
 
-#define CT_OFFSET_CHANNEL_PLAH			0x75
-#define CT_OFFSET_THERMAL_METER			0x78
-#define CT_OFFSET_RF_OPTION			0x79
-#define CT_OFFSET_VERSION			0x7E
-#define CT_OFFSET_CUSTOMER_ID			0x7F
+#define CT_OFFSET_CHANNEL_PLAH				0x75
+#define CT_OFFSET_THERMAL_METER				0x78
+#define CT_OFFSET_RF_OPTION					0x79
+#define CT_OFFSET_VERSION					0x7E
+#define CT_OFFSET_CUSTOMER_ID				0x7F
 
-#define RTL92C_MAX_PATH_NUM			2
+#define RTL92C_MAX_PATH_NUM					2
 
 enum swchnlcmd_id {
 	CMDID_END,
@@ -160,7 +158,6 @@ struct r_antenna_select_cck {
 	u8 r_ccktx_enable:4;
 };
 
-
 struct efuse_contents {
 	u8 mac_addr[ETH_ALEN];
 	u8 cck_tx_power_idx[6];
@@ -192,10 +189,10 @@ struct tx_power_struct {
 };
 
 enum _ANT_DIV_TYPE {
-	NO_ANTDIV			= 0xFF,
+	NO_ANTDIV				= 0xFF,
 	CG_TRX_HW_ANTDIV		= 0x01,
 	CGCS_RX_HW_ANTDIV		= 0x02,
-	FIXED_HW_ANTDIV			= 0x03,
+	FIXED_HW_ANTDIV         = 0x03,
 	CG_TRX_SMART_ANTDIV		= 0x04,
 	CGCS_RX_SW_ANTDIV		= 0x05,
 };
@@ -217,6 +214,8 @@ void rtl88e_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
 void rtl88e_phy_get_txpower_level(struct ieee80211_hw *hw,
 				  long *powerlevel);
 void rtl88e_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
+void rtl88e_phy_scan_operation_backup(struct ieee80211_hw *hw,
+				      u8 operation);
 void rtl88e_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
 void rtl88e_phy_set_bw_mode(struct ieee80211_hw *hw,
 			    enum nl80211_channel_type ch_type);
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
index 6dc4e3a954f6..ef28c8ea1e84 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -32,78 +28,78 @@
 
 /* drivers should parse below arrays and do the corresponding actions */
 /*3 Power on  Array*/
-struct wlan_pwr_cfg rtl8188e_power_on_flow[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS +
-					   RTL8188E_TRANS_END_STEPS] = {
-	RTL8188E_TRANS_CARDEMU_TO_ACT
-	RTL8188E_TRANS_END
+struct wlan_pwr_cfg rtl8188ee_power_on_flow[RTL8188EE_TRANS_CARDEMU_TO_ACT_STEPS
+					+ RTL8188EE_TRANS_END_STEPS] = {
+	RTL8188EE_TRANS_CARDEMU_TO_ACT
+	RTL8188EE_TRANS_END
 };
 
 /*3Radio off GPIO Array */
-struct wlan_pwr_cfg rtl8188e_radio_off_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
-					    + RTL8188E_TRANS_END_STEPS] = {
-	RTL8188E_TRANS_ACT_TO_CARDEMU
-	RTL8188E_TRANS_END
+struct wlan_pwr_cfg rtl8188ee_radio_off_flow[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8188EE_TRANS_END_STEPS] = {
+	RTL8188EE_TRANS_ACT_TO_CARDEMU
+	RTL8188EE_TRANS_END
 };
 
 /*3Card Disable Array*/
-struct wlan_pwr_cfg rtl8188e_card_disable_flow
-	[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-	RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
-	RTL8188E_TRANS_END_STEPS] = {
-		RTL8188E_TRANS_ACT_TO_CARDEMU
-		RTL8188E_TRANS_CARDEMU_TO_CARDDIS
-		RTL8188E_TRANS_END
+struct wlan_pwr_cfg rtl8188ee_card_disable_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8188EE_TRANS_END_STEPS] = {
+	RTL8188EE_TRANS_ACT_TO_CARDEMU
+	RTL8188EE_TRANS_CARDEMU_TO_CARDDIS
+	RTL8188EE_TRANS_END
 };
 
 /*3 Card Enable Array*/
-struct wlan_pwr_cfg rtl8188e_card_enable_flow
-	[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-	RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
-	RTL8188E_TRANS_END_STEPS] = {
-		RTL8188E_TRANS_CARDDIS_TO_CARDEMU
-		RTL8188E_TRANS_CARDEMU_TO_ACT
-		RTL8188E_TRANS_END
+struct wlan_pwr_cfg rtl8188ee_card_enable_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8188EE_TRANS_END_STEPS] = {
+	RTL8188EE_TRANS_CARDDIS_TO_CARDEMU
+	RTL8188EE_TRANS_CARDEMU_TO_ACT
+	RTL8188EE_TRANS_END
 };
 
 /*3Suspend Array*/
-struct wlan_pwr_cfg rtl8188e_suspend_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
-					+ RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS
-					+ RTL8188E_TRANS_END_STEPS] = {
-	RTL8188E_TRANS_ACT_TO_CARDEMU
-	RTL8188E_TRANS_CARDEMU_TO_SUS
-	RTL8188E_TRANS_END
+struct wlan_pwr_cfg rtl8188ee_suspend_flow[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8188EE_TRANS_CARDEMU_TO_SUS_STEPS
+					+ RTL8188EE_TRANS_END_STEPS] = {
+	RTL8188EE_TRANS_ACT_TO_CARDEMU
+	RTL8188EE_TRANS_CARDEMU_TO_SUS
+	RTL8188EE_TRANS_END
 };
 
 /*3 Resume Array*/
-struct wlan_pwr_cfg rtl8188e_resume_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
-					+ RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS
-					+ RTL8188E_TRANS_END_STEPS] = {
-	RTL8188E_TRANS_SUS_TO_CARDEMU
-	RTL8188E_TRANS_CARDEMU_TO_ACT
-	RTL8188E_TRANS_END
+struct wlan_pwr_cfg rtl8188ee_resume_flow[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8188EE_TRANS_CARDEMU_TO_SUS_STEPS
+					+ RTL8188EE_TRANS_END_STEPS] = {
+	RTL8188EE_TRANS_SUS_TO_CARDEMU
+	RTL8188EE_TRANS_CARDEMU_TO_ACT
+	RTL8188EE_TRANS_END
 };
 
 /*3HWPDN Array*/
-struct wlan_pwr_cfg rtl8188e_hwpdn_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS
-				+ RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS
-				+ RTL8188E_TRANS_END_STEPS] = {
-	RTL8188E_TRANS_ACT_TO_CARDEMU
-	RTL8188E_TRANS_CARDEMU_TO_PDN
-	RTL8188E_TRANS_END
+struct wlan_pwr_cfg rtl8188ee_hwpdn_flow[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS
+				+ RTL8188EE_TRANS_CARDEMU_TO_PDN_STEPS
+				+ RTL8188EE_TRANS_END_STEPS] = {
+	RTL8188EE_TRANS_ACT_TO_CARDEMU
+	RTL8188EE_TRANS_CARDEMU_TO_PDN
+	RTL8188EE_TRANS_END
 };
 
 /*3 Enter LPS */
-struct wlan_pwr_cfg rtl8188e_enter_lps_flow[RTL8188E_TRANS_ACT_TO_LPS_STEPS
-					+ RTL8188E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8188ee_enter_lps_flow[RTL8188EE_TRANS_ACT_TO_LPS_STEPS
+					+ RTL8188EE_TRANS_END_STEPS] = {
 	/*FW behavior*/
-	RTL8188E_TRANS_ACT_TO_LPS
-	RTL8188E_TRANS_END
+	RTL8188EE_TRANS_ACT_TO_LPS
+	RTL8188EE_TRANS_END
 };
 
 /*3 Leave LPS */
-struct wlan_pwr_cfg rtl8188e_leave_lps_flow[RTL8188E_TRANS_LPS_TO_ACT_STEPS
-					+ RTL8188E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8188ee_leave_lps_flow[RTL8188EE_TRANS_LPS_TO_ACT_STEPS
+					+ RTL8188EE_TRANS_END_STEPS] = {
 	/*FW behavior*/
-	RTL8188E_TRANS_LPS_TO_ACT
-	RTL8188E_TRANS_END
+	RTL8188EE_TRANS_LPS_TO_ACT
+	RTL8188EE_TRANS_END
 };
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
index 32e135ab9a63..79103347d967 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseq.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,297 +26,286 @@
 #ifndef __RTL8723E_PWRSEQ_H__
 #define __RTL8723E_PWRSEQ_H__
 
-/*
-	Check document WM-20110607-Paul-RTL8188E_Power_Architecture-R02.vsd
-	There are 6 HW Power States:
-	0: POFF--Power Off
-	1: PDN--Power Down
-	2: CARDEMU--Card Emulation
-	3: ACT--Active Mode
-	4: LPS--Low Power State
-	5: SUS--Suspend
-
-	The transision from different states are defined below
-	TRANS_CARDEMU_TO_ACT
-	TRANS_ACT_TO_CARDEMU
-	TRANS_CARDEMU_TO_SUS
-	TRANS_SUS_TO_CARDEMU
-	TRANS_CARDEMU_TO_PDN
-	TRANS_ACT_TO_LPS
-	TRANS_LPS_TO_ACT
-
-	TRANS_END
-	PWR SEQ Version: rtl8188e_PwrSeq_V09.h
-*/
-
-#define	RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS	10
-#define	RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS	10
-#define	RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS	10
-#define	RTL8188E_TRANS_SUS_TO_CARDEMU_STEPS	10
-#define	RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS	10
-#define	RTL8188E_TRANS_PDN_TO_CARDEMU_STEPS	10
-#define	RTL8188E_TRANS_ACT_TO_LPS_STEPS		15
-#define	RTL8188E_TRANS_LPS_TO_ACT_STEPS		15
-#define	RTL8188E_TRANS_END_STEPS		1
+#include "pwrseqcmd.h"
+/* Check document WM-20110607-Paul-RTL8188EE_Power_Architecture-R02.vsd
+ *	There are 6 HW Power States:
+ *	0: POFF--Power Off
+ *	1: PDN--Power Down
+ *	2: CARDEMU--Card Emulation
+ *	3: ACT--Active Mode
+ *	4: LPS--Low Power State
+ *	5: SUS--Suspend
+ *
+ *	The transision from different states are defined below
+ *	TRANS_CARDEMU_TO_ACT
+ *	TRANS_ACT_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_SUS
+ *	TRANS_SUS_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_PDN
+ *	TRANS_ACT_TO_LPS
+ *	TRANS_LPS_TO_ACT
+ *
+ *	TRANS_END
+ *	PWR SEQ Version: rtl8188ee_PwrSeq_V09.h
+ */
 
+#define	RTL8188EE_TRANS_CARDEMU_TO_ACT_STEPS	10
+#define	RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS	10
+#define	RTL8188EE_TRANS_CARDEMU_TO_SUS_STEPS	10
+#define	RTL8188EE_TRANS_SUS_TO_CARDEMU_STEPS	10
+#define	RTL8188EE_TRANS_CARDEMU_TO_PDN_STEPS	10
+#define	RTL8188EE_TRANS_PDN_TO_CARDEMU_STEPS	10
+#define	RTL8188EE_TRANS_ACT_TO_LPS_STEPS		15
+#define	RTL8188EE_TRANS_LPS_TO_ACT_STEPS		15
+#define	RTL8188EE_TRANS_END_STEPS		1
 
-#define RTL8188E_TRANS_CARDEMU_TO_ACT					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+/* The following macros have the following format:
+ * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value
+ *   comments },
+ */
+#define RTL8188EE_TRANS_CARDEMU_TO_ACT					\
 	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/* wait till 0x04[17] = 1    power ready*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)		\
+	/* wait till 0x04[17] = 1    power ready*/},			\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/* 0x02[1:0] = 0	reset BB*/				\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0)|BIT(1), 0},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0)|BIT(1), 0		\
+	/* 0x02[1:0] = 0	reset BB*/},				\
 	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*0x24[23] = 2b'01 schmit trigger */				\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)			\
+	/*0x24[23] = 2b'01 schmit trigger */},				\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/* 0x04[15] = 0 disable HWPDN (control by DRV)*/		\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0			\
+	/* 0x04[15] = 0 disable HWPDN (control by DRV)*/},		\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*0x04[12:11] = 2b'00 disable WL suspend*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4)|BIT(3), 0},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4)|BIT(3), 0		\
+	/*0x04[12:11] = 2b'00 disable WL suspend*/},			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*0x04[8] = 1 polling until return 0*/				\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)			\
+	/*0x04[8] = 1 polling until return 0*/},			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*wait till 0x04[8] = 0*/					\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(0), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(0), 0			\
+	/*wait till 0x04[8] = 0*/},					\
 	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0}, /*LDO normal mode*/\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0			\
+	/*LDO normal mode*/},						\
 	{0x0074, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*SDIO Driving*/\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)			\
+	/*SDIO Driving*/},
 
-#define RTL8188E_TRANS_ACT_TO_CARDEMU					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+#define RTL8188EE_TRANS_ACT_TO_CARDEMU					\
 	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*0x1F[7:0] = 0 turn off RF*/\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0			\
+	/*0x1F[7:0] = 0 turn off RF*/},					\
 	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)}, /*LDO Sleep mode*/\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)			\
+	/*LDO Sleep mode*/},						\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*0x04[9] = 1 turn off MAC by HW state machine*/		\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)			\
+	/*0x04[9] = 1 turn off MAC by HW state machine*/},		\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0			\
+	/*wait till 0x04[9] = 0 polling until return 0 to disable*/},
 
-
-#define RTL8188E_TRANS_CARDEMU_TO_SUS					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+#define RTL8188EE_TRANS_CARDEMU_TO_SUS					\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-	/*0x04[12:11] = 2b'01enable WL suspend*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)		\
+	/*0x04[12:11] = 2b'01enable WL suspend*/},			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-	/*0x04[12:11] = 2b'11enable WL suspend for PCIe*/		\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)|BIT(4)},\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)|BIT(4)	\
+	/*0x04[12:11] = 2b'11enable WL suspend for PCIe*/},		\
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-	/*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, BIT(7)},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, BIT(7)			\
+	/*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */},\
 	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-	/*Clear SIC_EN register 0x40[12] = 1'b0 */			\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0			\
+	/*Clear SIC_EN register 0x40[12] = 1'b0 */},			\
 	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-	/*Set USB suspend enable local register  0xfe10[4]= 1 */	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)			\
+	/*Set USB suspend enable local register  0xfe10[4]=1 */},	\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	/*Set SDIO suspend local register*/				\
-	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)		\
+	/*Set SDIO suspend local register*/},				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	/*wait power state to suspend*/					\
-	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0},
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0			\
+	/*wait power state to suspend*/},
 
-#define RTL8188E_TRANS_SUS_TO_CARDEMU					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+#define RTL8188EE_TRANS_SUS_TO_CARDEMU					\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	/*Set SDIO suspend local register*/				\
-	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},			\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0			\
+	/*Set SDIO suspend local register*/},				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	/*wait power state to suspend*/					\
-	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)		\
+	/*wait power state to suspend*/},				\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*0x04[12:11] = 2b'01enable WL suspend*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(4), 0		\
+	/*0x04[12:11] = 2b'01enable WL suspend*/},
 
-#define RTL8188E_TRANS_CARDEMU_TO_CARDDIS				\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+#define RTL8188EE_TRANS_CARDEMU_TO_CARDDIS				\
 	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*0x24[23] = 2b'01 schmit trigger */				\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)			\
+	/*0x24[23] = 2b'01 schmit trigger */},				\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(4), BIT(3)	\
+	/*0x04[12:11] = 2b'01 enable WL suspend*/},			\
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-	/*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0			\
+	/*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */},\
 	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-	/*Clear SIC_EN register 0x40[12] = 1'b0 */			\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0			\
+	/*Clear SIC_EN register 0x40[12] = 1'b0 */},			\
 	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
-	/*Set USB suspend enable local register  0xfe10[4]= 1 */	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)			\
+	/*Set USB suspend enable local register  0xfe10[4]=1 */},	\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	/*Set SDIO suspend local register*/				\
-	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)		\
+	/*Set SDIO suspend local register*/},				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	PWR_CMD_POLLING, BIT(1), 0}, /*wait power state to suspend*/
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0			\
+	/*wait power state to suspend*/},
 
-#define RTL8188E_TRANS_CARDDIS_TO_CARDEMU				\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+#define RTL8188EE_TRANS_CARDDIS_TO_CARDEMU				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	PWR_BASEADDR_SDIO,\
-	PWR_CMD_WRITE, BIT(0), 0}, /*Set SDIO suspend local register*/	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0			\
+	/*Set SDIO suspend local register*/},				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	PWR_BASEADDR_SDIO,\
-	PWR_CMD_POLLING, BIT(1), BIT(1)}, /*wait power state to suspend*/\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)		\
+	/*wait power state to suspend*/},				\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC,						\
-	PWR_CMD_WRITE, BIT(3)|BIT(4), 0},				\
-	/*0x04[12:11] = 2b'01enable WL suspend*/
-
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0		\
+	/*0x04[12:11] = 2b'01enable WL suspend*/},
 
-#define RTL8188E_TRANS_CARDEMU_TO_PDN					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+#define RTL8188EE_TRANS_CARDEMU_TO_PDN					\
 	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},/* 0x04[16] = 0*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0/* 0x04[16] = 0*/},	\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},/* 0x04[15] = 1*/
-
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)			\
+	/* 0x04[15] = 1*/},
 
-#define RTL8188E_TRANS_PDN_TO_CARDEMU					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+#define RTL8188EE_TRANS_PDN_TO_CARDEMU					\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},/* 0x04[15] = 0*/
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0/* 0x04[15] = 0*/},
 
-
-#define RTL8188E_TRANS_ACT_TO_LPS					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+#define RTL8188EE_TRANS_ACT_TO_LPS					\
 	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x7F},/*Tx Pause*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x7F			\
+	/*Tx Pause*/},							\
 	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*zero if no pkt is tx*/\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0			\
+	/*Should be zero if no packet is transmitting*/},		\
 	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*Should be zero if no packet is transmitting*/	\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0			\
+	/*Should be zero if no packet is transmitting*/},		\
 	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*Should be zero if no packet is transmitting*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0			\
+	/*Should be zero if no packet is transmitting*/},		\
 	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*Should be zero if no packet is transmitting*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0			\
+	/*Should be zero if no packet is transmitting*/},		\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*CCK and OFDM are disabled, and clock are gated*/		\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0			\
+	/*CCK and OFDM are disabled,and clock are gated*/},		\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US		\
+	/*Delay 1us*/},							\
 	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3F},/*Reset MAC TRX*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3F			\
+	/*Reset MAC TRX*/},						\
 	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*check if removed later*/					\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0			\
+	/*check if removed later*/},					\
 	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*Respond TxOK to scheduler*/					\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)			\
+	/*Respond TxOK to scheduler*/},
 
 
-#define RTL8188E_TRANS_LPS_TO_ACT					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+#define RTL8188EE_TRANS_LPS_TO_ACT					\
 	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84			\
+	/*SDIO RPWM*/},							\
 	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84			\
+	/*USB RPWM*/},							\
 	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84			\
+	/*PCIe RPWM*/},							\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/	\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS		\
+	/*Delay*/},							\
 	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*.	0x08[4] = 0		 switch TSF to 40M*/		\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0			\
+	/*.	0x08[4] = 0		 switch TSF to 40M*/},		\
 	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*Polling 0x109[7]= 0  TSF in 40M*/				\
-	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0},			\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0			\
+	/*Polling 0x109[7]=0  TSF in 40M*/},				\
 	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*. 0x29[7:6] = 2b'00	 enable BB clock*/			\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6)|BIT(7), 0},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6)|BIT(7), 0		\
+	/*.	0x29[7:6] = 2b'00	 enable BB clock*/},		\
 	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*.	0x101[1] = 1*/\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)			\
+	/*.	0x101[1] = 1*/},					\
 	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF			\
+	/*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/},		\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	/*. 0x02[1:0] = 2b'11  enable BB macro*/\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1)|BIT(0), BIT(1)|BIT(0)},	\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
-	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
-
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1)|BIT(0), BIT(1)|BIT(0)	\
+	/*.	0x02[1:0] = 2b'11	 enable BB macro*/},		\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0			\
+	/*.	0x522 = 0*/},
 
-#define RTL8188E_TRANS_END						\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+#define RTL8188EE_TRANS_END		\
 	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
 	0, PWR_CMD_END, 0, 0}
 
-extern struct wlan_pwr_cfg rtl8188e_power_on_flow
-		[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_radio_off_flow
-		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_card_disable_flow
-		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_card_enable_flow
-		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_suspend_flow
-		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-		RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_resume_flow
-		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-		RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_hwpdn_flow
-		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
-		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_enter_lps_flow
-		[RTL8188E_TRANS_ACT_TO_LPS_STEPS +
-		RTL8188E_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8188e_leave_lps_flow
-		[RTL8188E_TRANS_LPS_TO_ACT_STEPS +
-		RTL8188E_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_power_on_flow
+		[RTL8188EE_TRANS_CARDEMU_TO_ACT_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_radio_off_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_card_disable_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_card_enable_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_suspend_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_resume_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_hwpdn_flow
+		[RTL8188EE_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8188EE_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_enter_lps_flow
+		[RTL8188EE_TRANS_ACT_TO_LPS_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8188ee_leave_lps_flow
+		[RTL8188EE_TRANS_LPS_TO_ACT_STEPS +
+		 RTL8188EE_TRANS_END_STEPS];
 
 /* RTL8723 Power Configuration CMDs for PCIe interface */
-#define Rtl8188E_NIC_PWR_ON_FLOW	rtl8188e_power_on_flow
-#define Rtl8188E_NIC_RF_OFF_FLOW	rtl8188e_radio_off_flow
-#define Rtl8188E_NIC_DISABLE_FLOW	rtl8188e_card_disable_flow
-#define Rtl8188E_NIC_ENABLE_FLOW	rtl8188e_card_enable_flow
-#define Rtl8188E_NIC_SUSPEND_FLOW	rtl8188e_suspend_flow
-#define Rtl8188E_NIC_RESUME_FLOW	rtl8188e_resume_flow
-#define Rtl8188E_NIC_PDN_FLOW		rtl8188e_hwpdn_flow
-#define Rtl8188E_NIC_LPS_ENTER_FLOW	rtl8188e_enter_lps_flow
-#define Rtl8188E_NIC_LPS_LEAVE_FLOW	rtl8188e_leave_lps_flow
+#define RTL8188EE_NIC_PWR_ON_FLOW	rtl8188ee_power_on_flow
+#define RTL8188EE_NIC_RF_OFF_FLOW	rtl8188ee_radio_off_flow
+#define RTL8188EE_NIC_DISABLE_FLOW	rtl8188ee_card_disable_flow
+#define RTL8188EE_NIC_ENABLE_FLOW	rtl8188ee_card_enable_flow
+#define RTL8188EE_NIC_SUSPEND_FLOW	rtl8188ee_suspend_flow
+#define RTL8188EE_NIC_RESUME_FLOW	rtl8188ee_resume_flow
+#define RTL8188EE_NIC_PDN_FLOW		rtl8188ee_hwpdn_flow
+#define RTL8188EE_NIC_LPS_ENTER_FLOW	rtl8188ee_enter_lps_flow
+#define RTL8188EE_NIC_LPS_LEAVE_FLOW	rtl8188ee_leave_lps_flow
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.c b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.c
deleted file mode 100644
index 0f9314205526..000000000000
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2013  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "pwrseq.h"
-
-
-/*	Description:
- *		This routine deal with the Power Configuration CMDs
- *		 parsing for RTL8723/RTL8188E Series IC.
- *	Assumption:
- *		We should follow specific format which was released from HW SD.
- *
- *	2011.07.07, added by Roger.
- */
-
-bool rtl88_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-				u8 fab_version, u8 interface_type,
-				struct wlan_pwr_cfg pwrcfgcmd[])
-{
-	struct wlan_pwr_cfg cmd = {0};
-	bool polling_bit = false;
-	u32 ary_idx = 0;
-	u8 val = 0;
-	u32 offset = 0;
-	u32 polling_count = 0;
-	u32 max_polling_cnt = 5000;
-
-	do {
-		cmd = pwrcfgcmd[ary_idx];
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "rtl88_hal_pwrseqcmdparsing(): offset(%#x), cut_msk(%#x), fab_msk(%#x),"
-			 "interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), val(%#x)\n",
-			 GET_PWR_CFG_OFFSET(cmd),
-			 GET_PWR_CFG_CUT_MASK(cmd),
-			 GET_PWR_CFG_FAB_MASK(cmd),
-			 GET_PWR_CFG_INTF_MASK(cmd),
-			 GET_PWR_CFG_BASE(cmd),
-			 GET_PWR_CFG_CMD(cmd),
-			 GET_PWR_CFG_MASK(cmd),
-			 GET_PWR_CFG_VALUE(cmd));
-
-		if ((GET_PWR_CFG_FAB_MASK(cmd) & fab_version) &&
-		    (GET_PWR_CFG_CUT_MASK(cmd) & cut_version) &&
-		    (GET_PWR_CFG_INTF_MASK(cmd) & interface_type)) {
-			switch (GET_PWR_CFG_CMD(cmd)) {
-			case PWR_CMD_READ:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_READ\n");
-				break;
-			case PWR_CMD_WRITE: {
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n");
-				offset = GET_PWR_CFG_OFFSET(cmd);
-
-					/*Read the val from system register*/
-					val = rtl_read_byte(rtlpriv, offset);
-					val &= (~(GET_PWR_CFG_MASK(cmd)));
-					val |= (GET_PWR_CFG_VALUE(cmd) &
-						GET_PWR_CFG_MASK(cmd));
-
-					/*Write the val back to sytem register*/
-					rtl_write_byte(rtlpriv, offset, val);
-				}
-				break;
-			case PWR_CMD_POLLING:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n");
-				polling_bit = false;
-				offset = GET_PWR_CFG_OFFSET(cmd);
-
-				do {
-					val = rtl_read_byte(rtlpriv, offset);
-
-					val = val & GET_PWR_CFG_MASK(cmd);
-					if (val == (GET_PWR_CFG_VALUE(cmd) &
-						    GET_PWR_CFG_MASK(cmd)))
-						polling_bit = true;
-					else
-						udelay(10);
-
-					if (polling_count++ > max_polling_cnt) {
-						RT_TRACE(rtlpriv, COMP_INIT,
-							 DBG_LOUD,
-							 "polling fail in pwrseqcmd\n");
-						return false;
-					}
-				} while (!polling_bit);
-
-				break;
-			case PWR_CMD_DELAY:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n");
-				if (GET_PWR_CFG_VALUE(cmd) == PWRSEQ_DELAY_US)
-					udelay(GET_PWR_CFG_OFFSET(cmd));
-				else
-					mdelay(GET_PWR_CFG_OFFSET(cmd));
-				break;
-			case PWR_CMD_END:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtl88_hal_pwrseqcmdparsing(): PWR_CMD_END\n");
-				return true;
-			default:
-				RT_ASSERT(false,
-					  "rtl88_hal_pwrseqcmdparsing(): Unknown CMD!!\n");
-				break;
-			}
-		}
-
-		ary_idx++;
-	} while (1);
-
-	return true;
-}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.h b/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.h
deleted file mode 100644
index d9ae280bb1a2..000000000000
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/pwrseqcmd.h
+++ /dev/null
@@ -1,97 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2013  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8723E_PWRSEQCMD_H__
-#define __RTL8723E_PWRSEQCMD_H__
-
-#include "../wifi.h"
-/*---------------------------------------------*/
-/* The value of cmd: 4 bits */
-/*---------------------------------------------*/
-#define PWR_CMD_READ		0x00
-#define PWR_CMD_WRITE		0x01
-#define PWR_CMD_POLLING		0x02
-#define PWR_CMD_DELAY		0x03
-#define PWR_CMD_END		0x04
-
-/* define the base address of each block */
-#define PWR_BASEADDR_MAC	0x00
-#define PWR_BASEADDR_USB	0x01
-#define PWR_BASEADDR_PCIE	0x02
-#define PWR_BASEADDR_SDIO	0x03
-
-#define PWR_INTF_SDIO_MSK	BIT(0)
-#define PWR_INTF_USB_MSK	BIT(1)
-#define PWR_INTF_PCI_MSK	BIT(2)
-#define PWR_INTF_ALL_MSK	(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-
-#define	PWR_FAB_TSMC_MSK	BIT(0)
-#define	PWR_FAB_UMC_MSK		BIT(1)
-#define	PWR_FAB_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-
-#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
-#define	PWR_CUT_A_MSK		BIT(1)
-#define	PWR_CUT_B_MSK		BIT(2)
-#define	PWR_CUT_C_MSK		BIT(3)
-#define	PWR_CUT_D_MSK		BIT(4)
-#define	PWR_CUT_E_MSK		BIT(5)
-#define	PWR_CUT_F_MSK		BIT(6)
-#define	PWR_CUT_G_MSK		BIT(7)
-#define	PWR_CUT_ALL_MSK		0xFF
-
-enum pwrseq_delay_unit {
-	PWRSEQ_DELAY_US,
-	PWRSEQ_DELAY_MS,
-};
-
-struct wlan_pwr_cfg {
-	u16 offset;
-	u8 cut_msk;
-	u8 fab_msk:4;
-	u8 interface_msk:4;
-	u8 base:4;
-	u8 cmd:4;
-	u8 msk;
-	u8 value;
-};
-
-#define	GET_PWR_CFG_OFFSET(__PWR)	(__PWR.offset)
-#define	GET_PWR_CFG_CUT_MASK(__PWR)	(__PWR.cut_msk)
-#define	GET_PWR_CFG_FAB_MASK(__PWR)	(__PWR.fab_msk)
-#define	GET_PWR_CFG_INTF_MASK(__PWR)	(__PWR.interface_msk)
-#define	GET_PWR_CFG_BASE(__PWR)		(__PWR.base)
-#define	GET_PWR_CFG_CMD(__PWR)		(__PWR.cmd)
-#define	GET_PWR_CFG_MASK(__PWR)		(__PWR.msk)
-#define	GET_PWR_CFG_VALUE(__PWR)	(__PWR.value)
-
-bool rtl88_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-				u8 fab_version, u8 interface_type,
-				struct wlan_pwr_cfg pwrcfgcmd[]);
-
-#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h b/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
index cd7e7a527133..15400ee6c04b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/reg.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,62 +26,60 @@
 #ifndef __RTL92C_REG_H__
 #define __RTL92C_REG_H__
 
-#define TXPKT_BUF_SELECT			0x69
-#define RXPKT_BUF_SELECT			0xA5
-#define DISABLE_TRXPKT_BUF_ACCESS		0x0
+#define TXPKT_BUF_SELECT				0x69
+#define RXPKT_BUF_SELECT				0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS			0x0
 
 #define REG_SYS_ISO_CTRL			0x0000
 #define REG_SYS_FUNC_EN				0x0002
 #define REG_APS_FSMCO				0x0004
 #define REG_SYS_CLKR				0x0008
-#define REG_9346CR				0x000A
-#define REG_EE_VPD				0x000C
+#define REG_9346CR					0x000A
+#define REG_EE_VPD					0x000C
 #define REG_AFE_MISC				0x0010
 #define REG_SPS0_CTRL				0x0011
 #define REG_SPS_OCP_CFG				0x0018
 #define REG_RSV_CTRL				0x001C
-#define REG_RF_CTRL				0x001F
+#define REG_RF_CTRL					0x001F
 #define REG_LDOA15_CTRL				0x0020
 #define REG_LDOV12D_CTRL			0x0021
 #define REG_LDOHCI12_CTRL			0x0022
 #define REG_LPLDO_CTRL				0x0023
 #define REG_AFE_XTAL_CTRL			0x0024
-#define REG_AFE_LDO_CTRL			0x0027 /* 1.5v for 8188EE test
-							* chip, 1.4v for MP chip
-							*/
+/* 1.5v for 8188EE test chip, 1.4v for MP chip */
+#define REG_AFE_LDO_CTRL			0x0027
 #define REG_AFE_PLL_CTRL			0x0028
 #define REG_EFUSE_CTRL				0x0030
 #define REG_EFUSE_TEST				0x0034
 #define REG_PWR_DATA				0x0038
 #define REG_CAL_TIMER				0x003C
 #define REG_ACLK_MON				0x003E
-#define REG_GPIO_MUXCFG				0x0040
+#define REG_GPIO_MUXCFG			0x0040
 #define REG_GPIO_IO_SEL				0x0042
-#define REG_MAC_PINMUX_CFG			0x0043
+#define REG_MAC_PINMUX_CFG		0x0043
 #define REG_GPIO_PIN_CTRL			0x0044
 #define REG_GPIO_INTM				0x0048
-#define REG_LEDCFG0				0x004C
-#define REG_LEDCFG1				0x004D
-#define REG_LEDCFG2				0x004E
-#define REG_LEDCFG3				0x004F
-#define REG_FSIMR				0x0050
-#define REG_FSISR				0x0054
-#define REG_HSIMR				0x0058
-#define REG_HSISR				0x005c
-#define REG_GPIO_PIN_CTRL_2			0x0060
+#define REG_LEDCFG0					0x004C
+#define REG_LEDCFG1					0x004D
+#define REG_LEDCFG2					0x004E
+#define REG_LEDCFG3					0x004F
+#define REG_FSIMR					0x0050
+#define REG_FSISR					0x0054
+#define REG_HSIMR					0x0058
+#define REG_HSISR					0x005c
+#define REG_GPIO_PIN_CTRL_2		0x0060
 #define REG_GPIO_IO_SEL_2			0x0062
-#define REG_GPIO_OUTPUT				0x006c
-#define REG_AFE_XTAL_CTRL_EXT			0x0078
+#define REG_GPIO_OUTPUT			0x006c
+#define REG_AFE_XTAL_CTRL_EXT		0x0078
 #define REG_XCK_OUT_CTRL			0x007c
 #define REG_MCUFWDL				0x0080
 #define REG_WOL_EVENT				0x0081
 #define REG_MCUTSTCFG				0x0084
 
-
-#define REG_HIMR				0x00B0
-#define REG_HISR				0x00B4
-#define REG_HIMRE				0x00B8
-#define REG_HISRE				0x00BC
+#define REG_HIMR					0x00B0
+#define REG_HISR					0x00B4
+#define REG_HIMRE					0x00B8
+#define REG_HISRE					0x00BC
 
 #define REG_EFUSE_ACCESS			0x00CF
 
@@ -96,23 +90,23 @@
 #define REG_PCIE_MIO_INTF			0x00E4
 #define REG_PCIE_MIO_INTD			0x00E8
 #define REG_HPON_FSM				0x00EC
-#define REG_SYS_CFG				0x00F0
+#define REG_SYS_CFG					0x00F0
 
-#define REG_CR					0x0100
-#define REG_PBP					0x0104
-#define REG_PKT_BUFF_ACCESS_CTRL		0x0106
+#define REG_CR						0x0100
+#define REG_PBP						0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL	0x0106
 #define REG_TRXDMA_CTRL				0x010C
 #define REG_TRXFF_BNDY				0x0114
 #define REG_TRXFF_STATUS			0x0118
 #define REG_RXFF_PTR				0x011C
 
-#define REG_CPWM				0x012F
-#define REG_FWIMR				0x0130
-#define REG_FWISR				0x0134
+#define REG_CPWM					0x012F
+#define REG_FWIMR					0x0130
+#define REG_FWISR					0x0134
 #define REG_PKTBUF_DBG_CTRL			0x0140
-#define REG_PKTBUF_DBG_DATA_L			0x0144
-#define REG_PKTBUF_DBG_DATA_H			0x0148
-#define REG_RXPKTBUF_CTRL			(REG_PKTBUF_DBG_CTRL+2)
+#define REG_PKTBUF_DBG_DATA_L		0x0144
+#define REG_PKTBUF_DBG_DATA_H		0x0148
+#define REG_RXPKTBUF_CTRL	(REG_PKTBUF_DBG_CTRL+2)
 
 #define REG_TC0_CTRL				0x0150
 #define REG_TC1_CTRL				0x0154
@@ -123,13 +117,13 @@
 #define REG_MBIST_START				0x0174
 #define REG_MBIST_DONE				0x0178
 #define REG_MBIST_FAIL				0x017C
-#define REG_32K_CTRL				0x0194
-#define REG_C2HEVT_MSG_NORMAL			0x01A0
+#define REG_32K_CTRL					0x0194
+#define REG_C2HEVT_MSG_NORMAL		0x01A0
 #define REG_C2HEVT_CLEAR			0x01AF
 #define REG_C2HEVT_MSG_TEST			0x01B8
 #define REG_MCUTST_1				0x01c0
-#define REG_FMETHR				0x01C8
-#define REG_HMETFR				0x01CC
+#define REG_FMETHR					0x01C8
+#define REG_HMETFR					0x01CC
 #define REG_HMEBOX_0				0x01D0
 #define REG_HMEBOX_1				0x01D4
 #define REG_HMEBOX_2				0x01D8
@@ -144,36 +138,37 @@
 #define REG_HMEBOX_EXT_2			0x01F8
 #define REG_HMEBOX_EXT_3			0x01FC
 
-#define REG_RQPN				0x0200
+#define REG_RQPN					0x0200
 #define REG_FIFOPAGE				0x0204
-#define REG_TDECTRL				0x0208
-#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TDECTRL					0x0208
+#define REG_TXDMA_OFFSET_CHK		0x020C
 #define REG_TXDMA_STATUS			0x0210
 #define REG_RQPN_NPQ				0x0214
 
-#define REG_RXDMA_AGG_PG_TH			0x0280
-#define REG_FW_UPD_RDPTR			0x0284 /* FW shall update this
-						* register before FW * write
-						* RXPKT_RELEASE_POLL to 1
-						*/
-#define REG_RXDMA_CONTROL			0x0286 /* Control the RX DMA.*/
-#define REG_RXPKT_NUM				0x0287 /* The number of packets
-							* in RXPKTBUF.
-							 */
+#define REG_RXDMA_AGG_PG_TH		0x0280
+/* FW shall update this register before
+ * FW write RXPKT_RELEASE_POLL to 1
+ */
+#define REG_FW_UPD_RDPTR			0x0284
+/* Control the RX DMA.*/
+#define REG_RXDMA_CONTROL			0x0286
+/* The number of packets in RXPKTBUF.	*/
+#define REG_RXPKT_NUM				0x0287
+
 #define	REG_PCIE_CTRL_REG			0x0300
-#define	REG_INT_MIG				0x0304
+#define	REG_INT_MIG					0x0304
 #define	REG_BCNQ_DESA				0x0308
-#define	REG_HQ_DESA				0x0310
+#define	REG_HQ_DESA					0x0310
 #define	REG_MGQ_DESA				0x0318
 #define	REG_VOQ_DESA				0x0320
 #define	REG_VIQ_DESA				0x0328
 #define	REG_BEQ_DESA				0x0330
 #define	REG_BKQ_DESA				0x0338
-#define	REG_RX_DESA				0x0340
+#define	REG_RX_DESA					0x0340
 
-#define	REG_DBI					0x0348
-#define	REG_MDIO				0x0354
-#define	REG_DBG_SEL				0x0360
+#define	REG_DBI						0x0348
+#define	REG_MDIO					0x0354
+#define	REG_DBG_SEL					0x0360
 #define	REG_PCIE_HRPWM				0x0361
 #define	REG_PCIE_HCPWM				0x0363
 #define	REG_UART_CTRL				0x0364
@@ -181,7 +176,6 @@
 #define	REG_UART_TX_DESA			0x0370
 #define	REG_UART_RX_DESA			0x0378
 
-
 #define	REG_HDAQ_DESA_NODEF			0x0000
 #define	REG_CMDQ_DESA_NODEF			0x0000
 
@@ -191,33 +185,32 @@
 #define REG_BKQ_INFORMATION			0x040C
 #define REG_MGQ_INFORMATION			0x0410
 #define REG_HGQ_INFORMATION			0x0414
-#define REG_BCNQ_INFORMATION			0x0418
+#define REG_BCNQ_INFORMATION		0x0418
 #define REG_TXPKT_EMPTY				0x041A
 
-
-#define REG_CPU_MGQ_INFORMATION			0x041C
+#define REG_CPU_MGQ_INFORMATION		0x041C
 #define REG_FWHW_TXQ_CTRL			0x0420
 #define REG_HWSEQ_CTRL				0x0423
-#define REG_TXPKTBUF_BCNQ_BDNY			0x0424
-#define REG_TXPKTBUF_MGQ_BDNY			0x0425
+#define REG_TXPKTBUF_BCNQ_BDNY		0x0424
+#define REG_TXPKTBUF_MGQ_BDNY		0x0425
 #define REG_MULTI_BCNQ_EN			0x0426
-#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_MULTI_BCNQ_OFFSET		0x0427
 #define REG_SPEC_SIFS				0x0428
-#define REG_RL					0x042A
-#define REG_DARFRC				0x0430
-#define REG_RARFRC				0x0438
-#define REG_RRSR				0x0440
-#define REG_ARFR0				0x0444
-#define REG_ARFR1				0x0448
-#define REG_ARFR2				0x044C
-#define REG_ARFR3				0x0450
+#define REG_RL						0x042A
+#define REG_DARFRC					0x0430
+#define REG_RARFRC					0x0438
+#define REG_RRSR					0x0440
+#define REG_ARFR0					0x0444
+#define REG_ARFR1					0x0448
+#define REG_ARFR2					0x044C
+#define REG_ARFR3					0x0450
 #define REG_AGGLEN_LMT				0x0458
 #define REG_AMPDU_MIN_SPACE			0x045C
-#define REG_TXPKTBUF_WMAC_LBK_BF_HD		0x045D
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD	0x045D
 #define REG_FAST_EDCA_CTRL			0x0460
 #define REG_RD_RESP_PKT_TH			0x0463
 #define REG_INIRTS_RATE_SEL			0x0480
-#define REG_INIDATA_RATE_SEL			0x0484
+#define REG_INIDATA_RATE_SEL		0x0484
 #define REG_POWER_STATUS			0x04A4
 #define REG_POWER_STAGE1			0x04B4
 #define REG_POWER_STAGE2			0x04B8
@@ -225,32 +218,32 @@
 #define REG_STBC_SETTING			0x04C4
 #define REG_PROT_MODE_CTRL			0x04C8
 #define REG_BAR_MODE_CTRL			0x04CC
-#define REG_RA_TRY_RATE_AGG_LMT			0x04CF
-#define REG_EARLY_MODE_CONTROL			0x04D0
+#define REG_RA_TRY_RATE_AGG_LMT		0x04CF
+#define REG_EARLY_MODE_CONTROL		0x04D0
 #define REG_NQOS_SEQ				0x04DC
-#define REG_QOS_SEQ				0x04DE
+#define REG_QOS_SEQ					0x04DE
 #define REG_NEED_CPU_HANDLE			0x04E0
 #define REG_PKT_LOSE_RPT			0x04E1
 #define REG_PTCL_ERR_STATUS			0x04E2
 #define REG_TX_RPT_CTRL				0x04EC
 #define REG_TX_RPT_TIME				0x04F0
-#define REG_DUMMY				0x04FC
+#define REG_DUMMY					0x04FC
 
 #define REG_EDCA_VO_PARAM			0x0500
 #define REG_EDCA_VI_PARAM			0x0504
 #define REG_EDCA_BE_PARAM			0x0508
 #define REG_EDCA_BK_PARAM			0x050C
-#define REG_BCNTCFG				0x0510
-#define REG_PIFS				0x0512
+#define REG_BCNTCFG					0x0510
+#define REG_PIFS					0x0512
 #define REG_RDG_PIFS				0x0513
 #define REG_SIFS_CTX				0x0514
 #define REG_SIFS_TRX				0x0516
 #define REG_AGGR_BREAK_TIME			0x051A
-#define REG_SLOT				0x051B
+#define REG_SLOT					0x051B
 #define REG_TX_PTCL_CTRL			0x0520
-#define REG_TXPAUSE				0x0522
+#define REG_TXPAUSE					0x0522
 #define REG_DIS_TXREQ_CLR			0x0523
-#define REG_RD_CTRL				0x0524
+#define REG_RD_CTRL					0x0524
 #define REG_TBTT_PROHIBIT			0x0540
 #define REG_RD_NAV_NXT				0x0544
 #define REG_NAV_PROT_LEN			0x0546
@@ -259,21 +252,21 @@
 #define REG_MBID_NUM				0x0552
 #define REG_DUAL_TSF_RST			0x0553
 #define REG_BCN_INTERVAL			0x0554
-#define REG_MBSSID_BCN_SPACE			0x0554
+#define REG_MBSSID_BCN_SPACE		0x0554
 #define REG_DRVERLYINT				0x0558
 #define REG_BCNDMATIM				0x0559
-#define REG_ATIMWND				0x055A
+#define REG_ATIMWND					0x055A
 #define REG_BCN_MAX_ERR				0x055D
-#define REG_RXTSF_OFFSET_CCK			0x055E
-#define REG_RXTSF_OFFSET_OFDM			0x055F
-#define REG_TSFTR				0x0560
+#define REG_RXTSF_OFFSET_CCK		0x055E
+#define REG_RXTSF_OFFSET_OFDM		0x055F
+#define REG_TSFTR					0x0560
 #define REG_INIT_TSFTR				0x0564
-#define REG_PSTIMER				0x0580
-#define REG_TIMER0				0x0584
-#define REG_TIMER1				0x0588
+#define REG_PSTIMER					0x0580
+#define REG_TIMER0					0x0584
+#define REG_TIMER1					0x0588
 #define REG_ACMHWCTRL				0x05C0
 #define REG_ACMRSTCTRL				0x05C1
-#define REG_ACMAVG				0x05C2
+#define REG_ACMAVG					0x05C2
 #define REG_VO_ADMTIME				0x05C4
 #define REG_VI_ADMTIME				0x05C6
 #define REG_BE_ADMTIME				0x05C8
@@ -282,38 +275,38 @@
 
 #define REG_APSD_CTRL				0x0600
 #define REG_BWOPMODE				0x0603
-#define REG_TCR					0x0604
-#define REG_RCR					0x0608
+#define REG_TCR						0x0604
+#define REG_RCR						0x0608
 #define REG_RX_PKT_LIMIT			0x060C
 #define REG_RX_DLK_TIME				0x060D
 #define REG_RX_DRVINFO_SZ			0x060F
 
-#define REG_MACID				0x0610
-#define REG_BSSID				0x0618
-#define REG_MAR					0x0620
+#define REG_MACID					0x0610
+#define REG_BSSID					0x0618
+#define REG_MAR						0x0620
 #define REG_MBIDCAMCFG				0x0628
 
 #define REG_USTIME_EDCA				0x0638
 #define REG_MAC_SPEC_SIFS			0x063A
 #define REG_RESP_SIFS_CCK			0x063C
 #define REG_RESP_SIFS_OFDM			0x063E
-#define REG_ACKTO				0x0640
-#define REG_CTS2TO				0x0641
-#define REG_EIFS				0x0642
+#define REG_ACKTO					0x0640
+#define REG_CTS2TO					0x0641
+#define REG_EIFS					0x0642
 
 #define REG_NAV_CTRL				0x0650
 #define REG_BACAMCMD				0x0654
 #define REG_BACAMCONTENT			0x0658
-#define REG_LBDLY				0x0660
-#define REG_FWDLY				0x0661
+#define REG_LBDLY					0x0660
+#define REG_FWDLY					0x0661
 #define REG_RXERR_RPT				0x0664
 #define REG_TRXPTCL_CTL				0x0668
 
-#define REG_CAMCMD				0x0670
+#define REG_CAMCMD					0x0670
 #define REG_CAMWRITE				0x0674
-#define REG_CAMREAD				0x0678
-#define REG_CAMDBG				0x067C
-#define REG_SECCFG				0x0680
+#define REG_CAMREAD					0x0678
+#define REG_CAMDBG					0x067C
+#define REG_SECCFG					0x0680
 
 #define REG_WOW_CTRL				0x0690
 #define REG_PSSTATUS				0x0691
@@ -329,10 +322,10 @@
 #define REG_CALB32K_CTRL			0x06AC
 #define REG_PKT_MON_CTRL			0x06B4
 #define REG_BT_COEX_TABLE			0x06C0
-#define REG_WMAC_RESP_TXINFO			0x06D8
+#define REG_WMAC_RESP_TXINFO		0x06D8
 
 #define REG_USB_INFO				0xFE17
-#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_SPECIAL_OPTION		0xFE55
 #define REG_USB_DMA_AGG_TO			0xFE5B
 #define REG_USB_AGG_TO				0xFE5C
 #define REG_USB_AGG_TH				0xFE5D
@@ -340,523 +333,545 @@
 #define REG_TEST_USB_TXQS			0xFE48
 #define REG_TEST_SIE_VID			0xFE60
 #define REG_TEST_SIE_PID			0xFE62
-#define REG_TEST_SIE_OPTIONAL			0xFE64
-#define REG_TEST_SIE_CHIRP_K			0xFE65
+#define REG_TEST_SIE_OPTIONAL		0xFE64
+#define REG_TEST_SIE_CHIRP_K		0xFE65
 #define REG_TEST_SIE_PHY			0xFE66
-#define REG_TEST_SIE_MAC_ADDR			0xFE70
+#define REG_TEST_SIE_MAC_ADDR		0xFE70
 #define REG_TEST_SIE_STRING			0xFE80
 
 #define REG_NORMAL_SIE_VID			0xFE60
 #define REG_NORMAL_SIE_PID			0xFE62
-#define REG_NORMAL_SIE_OPTIONAL			0xFE64
+#define REG_NORMAL_SIE_OPTIONAL		0xFE64
 #define REG_NORMAL_SIE_EP			0xFE65
 #define REG_NORMAL_SIE_PHY			0xFE68
-#define REG_NORMAL_SIE_MAC_ADDR			0xFE70
-#define REG_NORMAL_SIE_STRING			0xFE80
+#define REG_NORMAL_SIE_MAC_ADDR		0xFE70
+#define REG_NORMAL_SIE_STRING		0xFE80
 
-#define	CR9346					REG_9346CR
-#define	MSR					(REG_CR + 2)
-#define	ISR					REG_HISR
-#define	TSFR					REG_TSFTR
+#define	CR9346				REG_9346CR
+#define	MSR				(REG_CR + 2)
+#define	ISR				REG_HISR
+#define	TSFR				REG_TSFTR
 
-#define	MACIDR0					REG_MACID
-#define	MACIDR4					(REG_MACID + 4)
+#define	MACIDR0				REG_MACID
+#define	MACIDR4				(REG_MACID + 4)
 
-#define PBP					REG_PBP
+#define PBP				REG_PBP
 
-#define	IDR0					MACIDR0
-#define	IDR4					MACIDR4
+#define	IDR0				MACIDR0
+#define	IDR4				MACIDR4
 
-#define	UNUSED_REGISTER				0x1BF
-#define	DCAM					UNUSED_REGISTER
-#define	PSR					UNUSED_REGISTER
-#define BBADDR					UNUSED_REGISTER
-#define	PHYDATAR				UNUSED_REGISTER
+#define	UNUSED_REGISTER			0x1BF
+#define	DCAM				UNUSED_REGISTER
+#define	PSR				UNUSED_REGISTER
+#define BBADDR				UNUSED_REGISTER
+#define	PHYDATAR			UNUSED_REGISTER
 
-#define	INVALID_BBRF_VALUE			0x12345678
+#define	INVALID_BBRF_VALUE		0x12345678
 
-#define	MAX_MSS_DENSITY_2T			0x13
-#define	MAX_MSS_DENSITY_1T			0x0A
+#define	MAX_MSS_DENSITY_2T		0x13
+#define	MAX_MSS_DENSITY_1T		0x0A
 
-#define	CMDEEPROM_EN				BIT(5)
-#define	CMDEEPROM_SEL				BIT(4)
-#define	CMD9346CR_9356SEL			BIT(4)
-#define	AUTOLOAD_EEPROM				(CMDEEPROM_EN|CMDEEPROM_SEL)
-#define	AUTOLOAD_EFUSE				CMDEEPROM_EN
+#define	CMDEEPROM_EN			BIT(5)
+#define	CMDEEPROM_SEL			BIT(4)
+#define	CMD9346CR_9356SEL		BIT(4)
+#define	AUTOLOAD_EEPROM			(CMDEEPROM_EN|CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE			CMDEEPROM_EN
 
-#define	GPIOSEL_GPIO				0
-#define	GPIOSEL_ENBT				BIT(5)
+#define	GPIOSEL_GPIO			0
+#define	GPIOSEL_ENBT			BIT(5)
 
-#define	GPIO_IN					REG_GPIO_PIN_CTRL
-#define	GPIO_OUT				(REG_GPIO_PIN_CTRL+1)
-#define	GPIO_IO_SEL				(REG_GPIO_PIN_CTRL+2)
-#define	GPIO_MOD				(REG_GPIO_PIN_CTRL+3)
+#define	GPIO_IN				REG_GPIO_PIN_CTRL
+#define	GPIO_OUT			(REG_GPIO_PIN_CTRL+1)
+#define	GPIO_IO_SEL			(REG_GPIO_PIN_CTRL+2)
+#define	GPIO_MOD			(REG_GPIO_PIN_CTRL+3)
 
-/* 8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
+/*8723/8188E Host System Interrupt
+ *Mask Register (offset 0x58, 32 byte)
+ */
 #define	HSIMR_GPIO12_0_INT_EN			BIT(0)
 #define	HSIMR_SPS_OCP_INT_EN			BIT(5)
 #define	HSIMR_RON_INT_EN			BIT(6)
 #define	HSIMR_PDN_INT_EN			BIT(7)
 #define	HSIMR_GPIO9_INT_EN			BIT(25)
 
-
-/* 8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
+/*       8723/8188E Host System Interrupt
+ *		Status Register (offset 0x5C, 32 byte)
+ */
 #define	HSISR_GPIO12_0_INT			BIT(0)
 #define	HSISR_SPS_OCP_INT			BIT(5)
 #define	HSISR_RON_INT_EN			BIT(6)
 #define	HSISR_PDNINT				BIT(7)
 #define	HSISR_GPIO9_INT				BIT(25)
 
-#define	MSR_NOLINK				0x00
-#define	MSR_ADHOC				0x01
-#define	MSR_INFRA				0x02
-#define	MSR_AP					0x03
-#define	MSR_MASK				0x03
+#define	MSR_NOLINK					0x00
+#define	MSR_ADHOC					0x01
+#define	MSR_INFRA					0x02
+#define	MSR_AP						0x03
 
 #define	RRSR_RSC_OFFSET				21
 #define	RRSR_SHORT_OFFSET			23
 #define	RRSR_RSC_BW_40M				0x600000
 #define	RRSR_RSC_UPSUBCHNL			0x400000
 #define	RRSR_RSC_LOWSUBCHNL			0x200000
-#define	RRSR_SHORT				0x800000
-#define	RRSR_1M					BIT(0)
-#define	RRSR_2M					BIT(1)
-#define	RRSR_5_5M				BIT(2)
-#define	RRSR_11M				BIT(3)
-#define	RRSR_6M					BIT(4)
-#define	RRSR_9M					BIT(5)
-#define	RRSR_12M				BIT(6)
-#define	RRSR_18M				BIT(7)
-#define	RRSR_24M				BIT(8)
-#define	RRSR_36M				BIT(9)
-#define	RRSR_48M				BIT(10)
-#define	RRSR_54M				BIT(11)
-#define	RRSR_MCS0				BIT(12)
-#define	RRSR_MCS1				BIT(13)
-#define	RRSR_MCS2				BIT(14)
-#define	RRSR_MCS3				BIT(15)
-#define	RRSR_MCS4				BIT(16)
-#define	RRSR_MCS5				BIT(17)
-#define	RRSR_MCS6				BIT(18)
-#define	RRSR_MCS7				BIT(19)
+#define	RRSR_SHORT					0x800000
+#define	RRSR_1M						BIT(0)
+#define	RRSR_2M						BIT(1)
+#define	RRSR_5_5M					BIT(2)
+#define	RRSR_11M					BIT(3)
+#define	RRSR_6M						BIT(4)
+#define	RRSR_9M						BIT(5)
+#define	RRSR_12M					BIT(6)
+#define	RRSR_18M					BIT(7)
+#define	RRSR_24M					BIT(8)
+#define	RRSR_36M					BIT(9)
+#define	RRSR_48M					BIT(10)
+#define	RRSR_54M					BIT(11)
+#define	RRSR_MCS0					BIT(12)
+#define	RRSR_MCS1					BIT(13)
+#define	RRSR_MCS2					BIT(14)
+#define	RRSR_MCS3					BIT(15)
+#define	RRSR_MCS4					BIT(16)
+#define	RRSR_MCS5					BIT(17)
+#define	RRSR_MCS6					BIT(18)
+#define	RRSR_MCS7					BIT(19)
 #define	BRSR_ACKSHORTPMB			BIT(23)
 
-#define	RATR_1M					0x00000001
-#define	RATR_2M					0x00000002
-#define	RATR_55M				0x00000004
-#define	RATR_11M				0x00000008
-#define	RATR_6M					0x00000010
-#define	RATR_9M					0x00000020
-#define	RATR_12M				0x00000040
-#define	RATR_18M				0x00000080
-#define	RATR_24M				0x00000100
-#define	RATR_36M				0x00000200
-#define	RATR_48M				0x00000400
-#define	RATR_54M				0x00000800
-#define	RATR_MCS0				0x00001000
-#define	RATR_MCS1				0x00002000
-#define	RATR_MCS2				0x00004000
-#define	RATR_MCS3				0x00008000
-#define	RATR_MCS4				0x00010000
-#define	RATR_MCS5				0x00020000
-#define	RATR_MCS6				0x00040000
-#define	RATR_MCS7				0x00080000
-#define	RATR_MCS8				0x00100000
-#define	RATR_MCS9				0x00200000
-#define	RATR_MCS10				0x00400000
-#define	RATR_MCS11				0x00800000
-#define	RATR_MCS12				0x01000000
-#define	RATR_MCS13				0x02000000
-#define	RATR_MCS14				0x04000000
-#define	RATR_MCS15				0x08000000
-
-#define RATE_1M					BIT(0)
-#define RATE_2M					BIT(1)
-#define RATE_5_5M				BIT(2)
-#define RATE_11M				BIT(3)
-#define RATE_6M					BIT(4)
-#define RATE_9M					BIT(5)
-#define RATE_12M				BIT(6)
-#define RATE_18M				BIT(7)
-#define RATE_24M				BIT(8)
-#define RATE_36M				BIT(9)
-#define RATE_48M				BIT(10)
-#define RATE_54M				BIT(11)
-#define RATE_MCS0				BIT(12)
-#define RATE_MCS1				BIT(13)
-#define RATE_MCS2				BIT(14)
-#define RATE_MCS3				BIT(15)
-#define RATE_MCS4				BIT(16)
-#define RATE_MCS5				BIT(17)
-#define RATE_MCS6				BIT(18)
-#define RATE_MCS7				BIT(19)
-#define RATE_MCS8				BIT(20)
-#define RATE_MCS9				BIT(21)
-#define RATE_MCS10				BIT(22)
-#define RATE_MCS11				BIT(23)
-#define RATE_MCS12				BIT(24)
-#define RATE_MCS13				BIT(25)
-#define RATE_MCS14				BIT(26)
-#define RATE_MCS15				BIT(27)
+#define	RATR_1M						0x00000001
+#define	RATR_2M						0x00000002
+#define	RATR_55M					0x00000004
+#define	RATR_11M					0x00000008
+#define	RATR_6M						0x00000010
+#define	RATR_9M						0x00000020
+#define	RATR_12M					0x00000040
+#define	RATR_18M					0x00000080
+#define	RATR_24M					0x00000100
+#define	RATR_36M					0x00000200
+#define	RATR_48M					0x00000400
+#define	RATR_54M					0x00000800
+#define	RATR_MCS0					0x00001000
+#define	RATR_MCS1					0x00002000
+#define	RATR_MCS2					0x00004000
+#define	RATR_MCS3					0x00008000
+#define	RATR_MCS4					0x00010000
+#define	RATR_MCS5					0x00020000
+#define	RATR_MCS6					0x00040000
+#define	RATR_MCS7					0x00080000
+#define	RATR_MCS8					0x00100000
+#define	RATR_MCS9					0x00200000
+#define	RATR_MCS10					0x00400000
+#define	RATR_MCS11					0x00800000
+#define	RATR_MCS12					0x01000000
+#define	RATR_MCS13					0x02000000
+#define	RATR_MCS14					0x04000000
+#define	RATR_MCS15					0x08000000
+
+#define RATE_1M						BIT(0)
+#define RATE_2M						BIT(1)
+#define RATE_5_5M					BIT(2)
+#define RATE_11M					BIT(3)
+#define RATE_6M						BIT(4)
+#define RATE_9M						BIT(5)
+#define RATE_12M					BIT(6)
+#define RATE_18M					BIT(7)
+#define RATE_24M					BIT(8)
+#define RATE_36M					BIT(9)
+#define RATE_48M					BIT(10)
+#define RATE_54M					BIT(11)
+#define RATE_MCS0					BIT(12)
+#define RATE_MCS1					BIT(13)
+#define RATE_MCS2					BIT(14)
+#define RATE_MCS3					BIT(15)
+#define RATE_MCS4					BIT(16)
+#define RATE_MCS5					BIT(17)
+#define RATE_MCS6					BIT(18)
+#define RATE_MCS7					BIT(19)
+#define RATE_MCS8					BIT(20)
+#define RATE_MCS9					BIT(21)
+#define RATE_MCS10					BIT(22)
+#define RATE_MCS11					BIT(23)
+#define RATE_MCS12					BIT(24)
+#define RATE_MCS13					BIT(25)
+#define RATE_MCS14					BIT(26)
+#define RATE_MCS15					BIT(27)
 
 #define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
-#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M | \
+#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
 				RATR_24M | RATR_36M | RATR_48M | RATR_54M)
-#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 | \
-				RATR_MCS3 | RATR_MCS4 | RATR_MCS5 | \
+#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |\
+				RATR_MCS3 | RATR_MCS4 | RATR_MCS5 |\
 				RATR_MCS6 | RATR_MCS7)
-#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 | \
-				RATR_MCS11 | RATR_MCS12 | RATR_MCS13 | \
+#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 |\
+				RATR_MCS11 | RATR_MCS12 | RATR_MCS13 |\
 				RATR_MCS14 | RATR_MCS15)
 
 #define	BW_OPMODE_20MHZ				BIT(2)
 #define	BW_OPMODE_5G				BIT(1)
 #define	BW_OPMODE_11J				BIT(0)
 
-#define	CAM_VALID				BIT(15)
+#define	CAM_VALID					BIT(15)
 #define	CAM_NOTVALID				0x0000
-#define	CAM_USEDK				BIT(5)
+#define	CAM_USEDK					BIT(5)
 
-#define	CAM_NONE				0x0
-#define	CAM_WEP40				0x01
-#define	CAM_TKIP				0x02
-#define	CAM_AES					0x04
-#define	CAM_WEP104				0x05
+#define	CAM_NONE					0x0
+#define	CAM_WEP40					0x01
+#define	CAM_TKIP					0x02
+#define	CAM_AES						0x04
+#define	CAM_WEP104					0x05
 
 #define	TOTAL_CAM_ENTRY				32
 #define	HALF_CAM_ENTRY				16
 
-#define	CAM_WRITE				BIT(16)
-#define	CAM_READ				0x00000000
+#define	CAM_WRITE					BIT(16)
+#define	CAM_READ					0x00000000
 #define	CAM_POLLINIG				BIT(31)
 
-#define	SCR_USEDK				0x01
+#define	SCR_USEDK					0x01
 #define	SCR_TXSEC_ENABLE			0x02
 #define	SCR_RXSEC_ENABLE			0x04
 
-#define	WOW_PMEN				BIT(0)
-#define	WOW_WOMEN				BIT(1)
-#define	WOW_MAGIC				BIT(2)
-#define	WOW_UWF					BIT(3)
+#define	WOW_PMEN					BIT(0)
+#define	WOW_WOMEN					BIT(1)
+#define	WOW_MAGIC					BIT(2)
+#define	WOW_UWF						BIT(3)
 
 /*********************************************
 *       8188 IMR/ISR bits
 **********************************************/
-#define	IMR_DISABLED				0x0
+#define	IMR_DISABLED			0x0
 /* IMR DW0(0x0060-0063) Bit 0-31 */
-#define	IMR_TXCCK		BIT(30) /* TXRPT interrupt when CCX bit of
-					 * the packet is set
-					 */
-#define	IMR_PSTIMEOUT		BIT(29)	/* Power Save Time Out Interrupt */
-#define	IMR_GTINT4		BIT(28)	/* When GTIMER4 expires,
-					 * this bit is set to 1
-					 */
-#define	IMR_GTINT3		BIT(27)	/* When GTIMER3 expires,
-					 * this bit is set to 1
-					 */
-#define	IMR_TBDER		BIT(26)	/* Transmit Beacon0 Error */
-#define	IMR_TBDOK		BIT(25)	/* Transmit Beacon0 OK	*/
-#define	IMR_TSF_BIT32_TOGGLE	BIT(24)	/* TSF Timer BIT32 toggle ind int */
-#define	IMR_BCNDMAINT0		BIT(20)	/* Beacon DMA Interrupt 0 */
-#define	IMR_BCNDOK0		BIT(16)	/* Beacon Queue DMA OK0	*/
-#define	IMR_HSISR_IND_ON_INT	BIT(15)	/* HSISR Indicator (HSIMR & HSISR is
-					 * true, this bit is set to 1)
-					 */
-#define	IMR_BCNDMAINT_E		BIT(14)	/* Beacon DMA Int Extension for Win7 */
-#define	IMR_ATIMEND		BIT(12)	/* CTWidnow End or ATIM Window End */
-#define	IMR_HISR1_IND_INT	BIT(11)	/* HISR1 Indicator (HISR1 & HIMR1 is
-					 * true, this bit is set to 1)
-					 */
-#define	IMR_C2HCMD		BIT(10)	/* CPU to Host Command INT Status,
-					 * Write 1 clear
-					 */
-#define	IMR_CPWM2		BIT(9)	/* CPU power Mode exchange INT Status,
-					 * Write 1 clear
-					 */
-#define	IMR_CPWM		BIT(8)	/* CPU power Mode exchange INT Status,
-					 * Write 1 clear
-					 */
-#define	IMR_HIGHDOK		BIT(7)	/* High Queue DMA OK	*/
-#define	IMR_MGNTDOK		BIT(6)	/* Management Queue DMA OK */
-#define	IMR_BKDOK		BIT(5)	/* AC_BK DMA OK		*/
-#define	IMR_BEDOK		BIT(4)	/* AC_BE DMA OK	*/
-#define	IMR_VIDOK		BIT(3)	/* AC_VI DMA OK	*/
-#define	IMR_VODOK		BIT(2)	/* AC_VO DMA OK	*/
-#define	IMR_RDU			BIT(1)	/* Rx Descriptor Unavailable */
-#define	IMR_ROK			BIT(0)	/* Receive DMA OK */
+/* TXRPT interrupt when CCX bit of the packet is set	*/
+#define	IMR_TXCCK				BIT(30)
+/* Power Save Time Out Interrupt */
+#define	IMR_PSTIMEOUT			BIT(29)
+/* When GTIMER4 expires, this bit is set to 1	*/
+#define	IMR_GTINT4				BIT(28)
+/* When GTIMER3 expires, this bit is set to 1	*/
+#define	IMR_GTINT3				BIT(27)
+/* Transmit Beacon0 Error			*/
+#define	IMR_TBDER				BIT(26)
+/* Transmit Beacon0 OK			*/
+#define	IMR_TBDOK				BIT(25)
+/* TSF Timer BIT32 toggle indication interrupt	*/
+#define	IMR_TSF_BIT32_TOGGLE		BIT(24)
+/* Beacon DMA Interrupt 0			*/
+#define	IMR_BCNDMAINT0			BIT(20)
+/* Beacon Queue DMA OK0			*/
+#define	IMR_BCNDOK0				BIT(16)
+/* HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1)	*/
+#define	IMR_HSISR_IND_ON_INT		BIT(15)
+/* Beacon DMA Interrupt Extension for Win7			*/
+#define	IMR_BCNDMAINT_E			BIT(14)
+/* CTWidnow End or ATIM Window End */
+#define	IMR_ATIMEND				BIT(12)
+/* HISR1 Indicator (HISR1 & HIMR1 is true, this bit is set to 1)*/
+#define	IMR_HISR1_IND_INT			BIT(11)
+/* CPU to Host Command INT Status, Write 1 clear	*/
+#define	IMR_C2HCMD				BIT(10)
+/* CPU power Mode exchange INT Status, Write 1 clear	*/
+#define	IMR_CPWM2			BIT(9)
+/* CPU power Mode exchange INT Status, Write 1 clear	*/
+#define	IMR_CPWM				BIT(8)
+/* High Queue DMA OK	*/
+#define	IMR_HIGHDOK				BIT(7)
+/* Management Queue DMA OK	*/
+#define	IMR_MGNTDOK				BIT(6)
+/* AC_BK DMA OK		*/
+#define	IMR_BKDOK				BIT(5)
+/* AC_BE DMA OK	*/
+#define	IMR_BEDOK				BIT(4)
+/* AC_VI DMA OK	*/
+#define	IMR_VIDOK				BIT(3)
+/* AC_VO DMA OK	*/
+#define	IMR_VODOK				BIT(2)
+/* Rx Descriptor Unavailable	*/
+#define	IMR_RDU				BIT(1)
+/* Receive DMA OK */
+#define	IMR_ROK				BIT(0)
 
 /* IMR DW1(0x00B4-00B7) Bit 0-31 */
-#define	IMR_BCNDMAINT7		BIT(27)	/* Beacon DMA Interrupt 7 */
-#define	IMR_BCNDMAINT6		BIT(26)	/* Beacon DMA Interrupt 6 */
-#define	IMR_BCNDMAINT5		BIT(25)	/* Beacon DMA Interrupt 5 */
-#define	IMR_BCNDMAINT4		BIT(24)	/* Beacon DMA Interrupt 4 */
-#define	IMR_BCNDMAINT3		BIT(23)	/* Beacon DMA Interrupt 3 */
-#define	IMR_BCNDMAINT2		BIT(22)	/* Beacon DMA Interrupt 2 */
-#define	IMR_BCNDMAINT1		BIT(21)	/* Beacon DMA Interrupt 1 */
-#define	IMR_BCNDOK7		BIT(20)	/* Beacon Queue DMA OK Interrup 7 */
-#define	IMR_BCNDOK6		BIT(19)	/* Beacon Queue DMA OK Interrup 6 */
-#define	IMR_BCNDOK5		BIT(18)	/* Beacon Queue DMA OK Interrup 5 */
-#define	IMR_BCNDOK4		BIT(17)	/* Beacon Queue DMA OK Interrup 4 */
-#define	IMR_BCNDOK3		BIT(16)	/* Beacon Queue DMA OK Interrup 3 */
-#define	IMR_BCNDOK2		BIT(15)	/* Beacon Queue DMA OK Interrup 2 */
-#define	IMR_BCNDOK1		BIT(14)	/* Beacon Queue DMA OK Interrup 1 */
-#define	IMR_ATIMEND_E		BIT(13)	/* ATIM Window End Extension for Win7 */
-#define	IMR_TXERR		BIT(11)	/* Tx Err Flag Int Status,
-					 * write 1 clear.
-					 */
-#define	IMR_RXERR		BIT(10)	/* Rx Err Flag INT Status,
-					 * Write 1 clear
-					 */
-#define	IMR_TXFOVW		BIT(9)	/* Transmit FIFO Overflow */
-#define	IMR_RXFOVW		BIT(8)	/* Receive FIFO Overflow */
-
+/* Beacon DMA Interrupt 7	*/
+#define	IMR_BCNDMAINT7			BIT(27)
+/* Beacon DMA Interrupt 6		*/
+#define	IMR_BCNDMAINT6			BIT(26)
+/* Beacon DMA Interrupt 5		*/
+#define	IMR_BCNDMAINT5			BIT(25)
+/* Beacon DMA Interrupt 4		*/
+#define	IMR_BCNDMAINT4			BIT(24)
+/* Beacon DMA Interrupt 3		*/
+#define	IMR_BCNDMAINT3			BIT(23)
+/* Beacon DMA Interrupt 2		*/
+#define	IMR_BCNDMAINT2			BIT(22)
+/* Beacon DMA Interrupt 1		*/
+#define	IMR_BCNDMAINT1			BIT(21)
+/* Beacon Queue DMA OK Interrup 7 */
+#define	IMR_BCNDOK7				BIT(20)
+/* Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK6				BIT(19)
+/* Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK5				BIT(18)
+/* Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK4				BIT(17)
+/* Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK3				BIT(16)
+/* Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK2				BIT(15)
+/* Beacon Queue DMA OK Interrup 1 */
+#define	IMR_BCNDOK1				BIT(14)
+/* ATIM Window End Extension for Win7 */
+#define	IMR_ATIMEND_E		BIT(13)
+/* Tx Error Flag Interrupt Status, write 1 clear. */
+#define	IMR_TXERR				BIT(11)
+/* Rx Error Flag INT Status, Write 1 clear */
+#define	IMR_RXERR				BIT(10)
+/* Transmit FIFO Overflow */
+#define	IMR_TXFOVW				BIT(9)
+/* Receive FIFO Overflow */
+#define	IMR_RXFOVW				BIT(8)
 
 #define	HWSET_MAX_SIZE				512
 #define   EFUSE_MAX_SECTION			64
-#define   EFUSE_REAL_CONTENT_LEN		256
-#define	EFUSE_OOB_PROTECT_BYTES			18 /* PG data exclude header,
-						    * dummy 7 bytes frome CP
-						    * test and reserved 1byte.
-						    */
-
-#define	EEPROM_DEFAULT_TSSI			0x0
-#define EEPROM_DEFAULT_TXPOWERDIFF		0x0
-#define EEPROM_DEFAULT_CRYSTALCAP		0x5
-#define EEPROM_DEFAULT_BOARDTYPE		0x02
-#define EEPROM_DEFAULT_TXPOWER			0x1010
-#define	EEPROM_DEFAULT_HT2T_TXPWR		0x10
+#define   EFUSE_REAL_CONTENT_LEN			256
+/* PG data exclude header, dummy 7 bytes frome CP test and reserved 1byte.*/
+#define		EFUSE_OOB_PROTECT_BYTES		18
+
+#define	EEPROM_DEFAULT_TSSI					0x0
+#define EEPROM_DEFAULT_TXPOWERDIFF			0x0
+#define EEPROM_DEFAULT_CRYSTALCAP			0x5
+#define EEPROM_DEFAULT_BOARDTYPE			0x02
+#define EEPROM_DEFAULT_TXPOWER				0x1010
+#define	EEPROM_DEFAULT_HT2T_TXPWR			0x10
 
 #define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
-#define	EEPROM_DEFAULT_THERMALMETER		0x18
+#define	EEPROM_DEFAULT_THERMALMETER			0x18
 #define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
 #define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
-#define	EEPROM_DEFAULT_TXPOWERLEVEL		0x22
-#define	EEPROM_DEFAULT_HT40_2SDIFF		0x0
-#define EEPROM_DEFAULT_HT20_DIFF		2
+#define	EEPROM_DEFAULT_TXPOWERLEVEL			0x22
+#define	EEPROM_DEFAULT_HT40_2SDIFF			0x0
+#define EEPROM_DEFAULT_HT20_DIFF			2
 #define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
 #define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
 #define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
 
-#define RF_OPTION1				0x79
-#define RF_OPTION2				0x7A
-#define RF_OPTION3				0x7B
-#define RF_OPTION4				0x7C
+#define RF_OPTION1							0x79
+#define RF_OPTION2							0x7A
+#define RF_OPTION3							0x7B
+#define RF_OPTION4							0x7C
 
-#define EEPROM_DEFAULT_PID			0x1234
-#define EEPROM_DEFAULT_VID			0x5678
-#define EEPROM_DEFAULT_CUSTOMERID		0xAB
+#define EEPROM_DEFAULT_PID					0x1234
+#define EEPROM_DEFAULT_VID					0x5678
+#define EEPROM_DEFAULT_CUSTOMERID			0xAB
 #define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
-#define EEPROM_DEFAULT_VERSION			0
-
-#define	EEPROM_CHANNEL_PLAN_FCC			0x0
-#define	EEPROM_CHANNEL_PLAN_IC			0x1
-#define	EEPROM_CHANNEL_PLAN_ETSI		0x2
-#define	EEPROM_CHANNEL_PLAN_SPAIN		0x3
-#define	EEPROM_CHANNEL_PLAN_FRANCE		0x4
-#define	EEPROM_CHANNEL_PLAN_MKK			0x5
-#define	EEPROM_CHANNEL_PLAN_MKK1		0x6
-#define	EEPROM_CHANNEL_PLAN_ISRAEL		0x7
-#define	EEPROM_CHANNEL_PLAN_TELEC		0x8
+#define EEPROM_DEFAULT_VERSION				0
+
+#define	EEPROM_CHANNEL_PLAN_FCC				0x0
+#define	EEPROM_CHANNEL_PLAN_IC				0x1
+#define	EEPROM_CHANNEL_PLAN_ETSI			0x2
+#define	EEPROM_CHANNEL_PLAN_SPAIN			0x3
+#define	EEPROM_CHANNEL_PLAN_FRANCE			0x4
+#define	EEPROM_CHANNEL_PLAN_MKK				0x5
+#define	EEPROM_CHANNEL_PLAN_MKK1			0x6
+#define	EEPROM_CHANNEL_PLAN_ISRAEL			0x7
+#define	EEPROM_CHANNEL_PLAN_TELEC			0x8
 #define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
 #define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
-#define	EEPROM_CHANNEL_PLAN_NCC			0xB
+#define	EEPROM_CHANNEL_PLAN_NCC				0xB
 #define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
 
-#define EEPROM_CID_DEFAULT			0x0
-#define EEPROM_CID_TOSHIBA			0x4
-#define	EEPROM_CID_CCX				0x10
-#define	EEPROM_CID_QMI				0x0D
-#define EEPROM_CID_WHQL				0xFE
+#define EEPROM_CID_DEFAULT					0x0
+#define EEPROM_CID_TOSHIBA					0x4
+#define	EEPROM_CID_CCX						0x10
+#define	EEPROM_CID_QMI						0x0D
+#define EEPROM_CID_WHQL						0xFE
 
-#define	RTL8188E_EEPROM_ID			0x8129
+#define	RTL8188E_EEPROM_ID					0x8129
 
-#define EEPROM_HPON				0x02
-#define EEPROM_CLK				0x06
-#define EEPROM_TESTR				0x08
+#define EEPROM_HPON							0x02
+#define EEPROM_CLK							0x06
+#define EEPROM_TESTR						0x08
 
 #define EEPROM_TXPOWERCCK			0x10
-#define	EEPROM_TXPOWERHT40_1S			0x16
-#define EEPROM_TXPOWERHT20DIFF			0x1B
-#define EEPROM_TXPOWER_OFDMDIFF			0x1B
+#define	EEPROM_TXPOWERHT40_1S		0x16
+#define EEPROM_TXPOWERHT20DIFF		0x1B
+#define EEPROM_TXPOWER_OFDMDIFF		0x1B
 
-#define	EEPROM_TX_PWR_INX			0x10
+#define	EEPROM_TX_PWR_INX				0x10
 
-#define	EEPROM_CHANNELPLAN			0xB8
-#define	EEPROM_XTAL_88E				0xB9
-#define	EEPROM_THERMAL_METER_88E		0xBA
-#define	EEPROM_IQK_LCK_88E			0xBB
+#define	EEPROM_CHANNELPLAN					0xB8
+#define	EEPROM_XTAL_88E						0xB9
+#define	EEPROM_THERMAL_METER_88E			0xBA
+#define	EEPROM_IQK_LCK_88E					0xBB
 
-#define	EEPROM_RF_BOARD_OPTION_88E		0xC1
+#define	EEPROM_RF_BOARD_OPTION_88E			0xC1
 #define	EEPROM_RF_FEATURE_OPTION_88E		0xC2
-#define	EEPROM_RF_BT_SETTING_88E		0xC3
-#define	EEPROM_VERSION				0xC4
-#define	EEPROM_CUSTOMER_ID			0xC5
-#define	EEPROM_RF_ANTENNA_OPT_88E		0xC9
-
-#define	EEPROM_MAC_ADDR				0xD0
-#define EEPROM_VID				0xD6
-#define EEPROM_DID				0xD8
-#define EEPROM_SVID				0xDA
-#define EEPROM_SMID				0xDC
-
-#define	STOPBECON				BIT(6)
-#define	STOPHIGHT				BIT(5)
-#define	STOPMGT					BIT(4)
-#define	STOPVO					BIT(3)
-#define	STOPVI					BIT(2)
-#define	STOPBE					BIT(1)
-#define	STOPBK					BIT(0)
-
-#define	RCR_APPFCS				BIT(31)
-#define	RCR_APP_MIC				BIT(30)
-#define	RCR_APP_ICV				BIT(29)
+#define	EEPROM_RF_BT_SETTING_88E				0xC3
+#define	EEPROM_VERSION					0xC4
+#define	EEPROM_CUSTOMER_ID					0xC5
+#define	EEPROM_RF_ANTENNA_OPT_88E			0xC9
+
+#define	EEPROM_MAC_ADDR					0xD0
+#define EEPROM_VID							0xD6
+#define EEPROM_DID							0xD8
+#define EEPROM_SVID							0xDA
+#define EEPROM_SMID						0xDC
+
+#define	STOPBECON					BIT(6)
+#define	STOPHIGHT					BIT(5)
+#define	STOPMGT						BIT(4)
+#define	STOPVO						BIT(3)
+#define	STOPVI						BIT(2)
+#define	STOPBE						BIT(1)
+#define	STOPBK						BIT(0)
+
+#define	RCR_APPFCS					BIT(31)
+#define	RCR_APP_MIC					BIT(30)
+#define	RCR_APP_ICV					BIT(29)
 #define	RCR_APP_PHYST_RXFF			BIT(28)
 #define	RCR_APP_BA_SSN				BIT(27)
-#define	RCR_ENMBID				BIT(24)
-#define	RCR_LSIGEN				BIT(23)
-#define	RCR_MFBEN				BIT(22)
+#define	RCR_ENMBID					BIT(24)
+#define	RCR_LSIGEN					BIT(23)
+#define	RCR_MFBEN					BIT(22)
 #define	RCR_HTC_LOC_CTRL			BIT(14)
-#define	RCR_AMF					BIT(13)
-#define	RCR_ACF					BIT(12)
-#define	RCR_ADF					BIT(11)
-#define	RCR_AICV				BIT(9)
-#define	RCR_ACRC32				BIT(8)
+#define	RCR_AMF						BIT(13)
+#define	RCR_ACF						BIT(12)
+#define	RCR_ADF						BIT(11)
+#define	RCR_AICV					BIT(9)
+#define	RCR_ACRC32					BIT(8)
 #define	RCR_CBSSID_BCN				BIT(7)
 #define	RCR_CBSSID_DATA				BIT(6)
-#define	RCR_CBSSID				RCR_CBSSID_DATA
-#define	RCR_APWRMGT				BIT(5)
-#define	RCR_ADD3				BIT(4)
-#define	RCR_AB					BIT(3)
-#define	RCR_AM					BIT(2)
-#define	RCR_APM					BIT(1)
-#define	RCR_AAP					BIT(0)
+#define	RCR_CBSSID					RCR_CBSSID_DATA
+#define	RCR_APWRMGT					BIT(5)
+#define	RCR_ADD3					BIT(4)
+#define	RCR_AB						BIT(3)
+#define	RCR_AM						BIT(2)
+#define	RCR_APM						BIT(1)
+#define	RCR_AAP						BIT(0)
 #define	RCR_MXDMA_OFFSET			8
 #define	RCR_FIFO_OFFSET				13
 
-#define RSV_CTRL				0x001C
-#define RD_CTRL					0x0524
+#define RSV_CTRL					0x001C
+#define RD_CTRL						0x0524
 
 #define REG_USB_INFO				0xFE17
-#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_SPECIAL_OPTION		0xFE55
 #define REG_USB_DMA_AGG_TO			0xFE5B
 #define REG_USB_AGG_TO				0xFE5C
 #define REG_USB_AGG_TH				0xFE5D
 
-#define REG_USB_VID				0xFE60
-#define REG_USB_PID				0xFE62
+#define REG_USB_VID					0xFE60
+#define REG_USB_PID					0xFE62
 #define REG_USB_OPTIONAL			0xFE64
 #define REG_USB_CHIRP_K				0xFE65
-#define REG_USB_PHY				0xFE66
+#define REG_USB_PHY					0xFE66
 #define REG_USB_MAC_ADDR			0xFE70
 #define REG_USB_HRPWM				0xFE58
 #define REG_USB_HCPWM				0xFE57
 
-#define SW18_FPWM				BIT(3)
+#define SW18_FPWM					BIT(3)
 
-#define ISO_MD2PP				BIT(0)
-#define ISO_UA2USB				BIT(1)
-#define ISO_UD2CORE				BIT(2)
-#define ISO_PA2PCIE				BIT(3)
-#define ISO_PD2CORE				BIT(4)
-#define ISO_IP2MAC				BIT(5)
-#define ISO_DIOP				BIT(6)
-#define ISO_DIOE				BIT(7)
-#define ISO_EB2CORE				BIT(8)
-#define ISO_DIOR				BIT(9)
+#define ISO_MD2PP					BIT(0)
+#define ISO_UA2USB					BIT(1)
+#define ISO_UD2CORE					BIT(2)
+#define ISO_PA2PCIE					BIT(3)
+#define ISO_PD2CORE					BIT(4)
+#define ISO_IP2MAC					BIT(5)
+#define ISO_DIOP					BIT(6)
+#define ISO_DIOE					BIT(7)
+#define ISO_EB2CORE					BIT(8)
+#define ISO_DIOR					BIT(9)
 
-#define PWC_EV25V				BIT(14)
-#define PWC_EV12V				BIT(15)
+#define PWC_EV25V					BIT(14)
+#define PWC_EV12V					BIT(15)
 
-#define FEN_BBRSTB				BIT(0)
+#define FEN_BBRSTB					BIT(0)
 #define FEN_BB_GLB_RSTN				BIT(1)
-#define FEN_USBA				BIT(2)
-#define FEN_UPLL				BIT(3)
-#define FEN_USBD				BIT(4)
+#define FEN_USBA					BIT(2)
+#define FEN_UPLL					BIT(3)
+#define FEN_USBD					BIT(4)
 #define FEN_DIO_PCIE				BIT(5)
-#define FEN_PCIEA				BIT(6)
-#define FEN_PPLL				BIT(7)
-#define FEN_PCIED				BIT(8)
-#define FEN_DIOE				BIT(9)
-#define FEN_CPUEN				BIT(10)
-#define FEN_DCORE				BIT(11)
-#define FEN_ELDR				BIT(12)
-#define FEN_DIO_RF				BIT(13)
-#define FEN_HWPDN				BIT(14)
-#define FEN_MREGEN				BIT(15)
-
-#define PFM_LDALL				BIT(0)
-#define PFM_ALDN				BIT(1)
-#define PFM_LDKP				BIT(2)
-#define PFM_WOWL				BIT(3)
-#define ENPDN					BIT(4)
-#define PDN_PL					BIT(5)
-#define APFM_ONMAC				BIT(8)
-#define APFM_OFF				BIT(9)
-#define APFM_RSM				BIT(10)
-#define AFSM_HSUS				BIT(11)
-#define AFSM_PCIE				BIT(12)
-#define APDM_MAC				BIT(13)
-#define APDM_HOST				BIT(14)
-#define APDM_HPDN				BIT(15)
-#define RDY_MACON				BIT(16)
-#define SUS_HOST				BIT(17)
-#define ROP_ALD					BIT(20)
-#define ROP_PWR					BIT(21)
-#define ROP_SPS					BIT(22)
-#define SOP_MRST				BIT(25)
-#define SOP_FUSE				BIT(26)
-#define SOP_ABG					BIT(27)
-#define SOP_AMB					BIT(28)
-#define SOP_RCK					BIT(29)
-#define SOP_A8M					BIT(30)
-#define XOP_BTCK				BIT(31)
-
-#define ANAD16V_EN				BIT(0)
-#define ANA8M					BIT(1)
-#define MACSLP					BIT(4)
+#define FEN_PCIEA					BIT(6)
+#define FEN_PPLL					BIT(7)
+#define FEN_PCIED					BIT(8)
+#define FEN_DIOE					BIT(9)
+#define FEN_CPUEN					BIT(10)
+#define FEN_DCORE					BIT(11)
+#define FEN_ELDR					BIT(12)
+#define FEN_DIO_RF					BIT(13)
+#define FEN_HWPDN					BIT(14)
+#define FEN_MREGEN					BIT(15)
+
+#define PFM_LDALL					BIT(0)
+#define PFM_ALDN					BIT(1)
+#define PFM_LDKP					BIT(2)
+#define PFM_WOWL					BIT(3)
+#define ENPDN						BIT(4)
+#define PDN_PL						BIT(5)
+#define APFM_ONMAC					BIT(8)
+#define APFM_OFF					BIT(9)
+#define APFM_RSM					BIT(10)
+#define AFSM_HSUS					BIT(11)
+#define AFSM_PCIE					BIT(12)
+#define APDM_MAC					BIT(13)
+#define APDM_HOST					BIT(14)
+#define APDM_HPDN					BIT(15)
+#define RDY_MACON					BIT(16)
+#define SUS_HOST					BIT(17)
+#define ROP_ALD						BIT(20)
+#define ROP_PWR						BIT(21)
+#define ROP_SPS						BIT(22)
+#define SOP_MRST					BIT(25)
+#define SOP_FUSE					BIT(26)
+#define SOP_ABG						BIT(27)
+#define SOP_AMB						BIT(28)
+#define SOP_RCK						BIT(29)
+#define SOP_A8M						BIT(30)
+#define XOP_BTCK					BIT(31)
+
+#define ANAD16V_EN					BIT(0)
+#define ANA8M						BIT(1)
+#define MACSLP						BIT(4)
 #define LOADER_CLK_EN				BIT(5)
 #define _80M_SSC_DIS				BIT(7)
 #define _80M_SSC_EN_HO				BIT(8)
 #define PHY_SSC_RSTB				BIT(9)
-#define SEC_CLK_EN				BIT(10)
-#define MAC_CLK_EN				BIT(11)
-#define SYS_CLK_EN				BIT(12)
-#define RING_CLK_EN				BIT(13)
+#define SEC_CLK_EN					BIT(10)
+#define MAC_CLK_EN					BIT(11)
+#define SYS_CLK_EN					BIT(12)
+#define RING_CLK_EN					BIT(13)
 
 #define	BOOT_FROM_EEPROM			BIT(4)
-#define	EEPROM_EN				BIT(5)
+#define	EEPROM_EN					BIT(5)
 
-#define AFE_BGEN				BIT(0)
-#define AFE_MBEN				BIT(1)
-#define MAC_ID_EN				BIT(7)
+#define AFE_BGEN					BIT(0)
+#define AFE_MBEN					BIT(1)
+#define MAC_ID_EN					BIT(7)
 
-#define WLOCK_ALL				BIT(0)
-#define WLOCK_00				BIT(1)
-#define WLOCK_04				BIT(2)
-#define WLOCK_08				BIT(3)
-#define WLOCK_40				BIT(4)
+#define WLOCK_ALL					BIT(0)
+#define WLOCK_00					BIT(1)
+#define WLOCK_04					BIT(2)
+#define WLOCK_08					BIT(3)
+#define WLOCK_40					BIT(4)
 #define R_DIS_PRST_0				BIT(5)
 #define R_DIS_PRST_1				BIT(6)
-#define LOCK_ALL_EN				BIT(7)
+#define LOCK_ALL_EN					BIT(7)
 
-#define RF_EN					BIT(0)
-#define RF_RSTB					BIT(1)
-#define RF_SDMRSTB				BIT(2)
+#define RF_EN						BIT(0)
+#define RF_RSTB						BIT(1)
+#define RF_SDMRSTB					BIT(2)
 
-#define LDA15_EN				BIT(0)
-#define LDA15_STBY				BIT(1)
-#define LDA15_OBUF				BIT(2)
+#define LDA15_EN					BIT(0)
+#define LDA15_STBY					BIT(1)
+#define LDA15_OBUF					BIT(2)
 #define LDA15_REG_VOS				BIT(3)
 #define _LDA15_VOADJ(x)				(((x) & 0x7) << 4)
 
-#define LDV12_EN				BIT(0)
-#define LDV12_SDBY				BIT(1)
-#define LPLDO_HSM				BIT(2)
+#define LDV12_EN					BIT(0)
+#define LDV12_SDBY					BIT(1)
+#define LPLDO_HSM					BIT(2)
 #define LPLDO_LSM_DIS				BIT(3)
 #define _LDV12_VADJ(x)				(((x) & 0xF) << 4)
 
-#define XTAL_EN					BIT(0)
-#define XTAL_BSEL				BIT(1)
+#define XTAL_EN						BIT(0)
+#define XTAL_BSEL					BIT(1)
 #define _XTAL_BOSC(x)				(((x) & 0x3) << 2)
 #define _XTAL_CADJ(x)				(((x) & 0xF) << 4)
 #define XTAL_GATE_USB				BIT(8)
@@ -871,145 +886,145 @@
 #define _XTAL_BT_DRV(x)				(((x) & 0x3) << 21)
 #define _XTAL_GPIO(x)				(((x) & 0x7) << 23)
 
-#define CKDLY_AFE				BIT(26)
-#define CKDLY_USB				BIT(27)
-#define CKDLY_DIG				BIT(28)
-#define CKDLY_BT				BIT(29)
+#define CKDLY_AFE					BIT(26)
+#define CKDLY_USB					BIT(27)
+#define CKDLY_DIG					BIT(28)
+#define CKDLY_BT					BIT(29)
 
-#define APLL_EN					BIT(0)
-#define APLL_320_EN				BIT(1)
+#define APLL_EN						BIT(0)
+#define APLL_320_EN					BIT(1)
 #define APLL_FREF_SEL				BIT(2)
 #define APLL_EDGE_SEL				BIT(3)
-#define APLL_WDOGB				BIT(4)
-#define APLL_LPFEN				BIT(5)
+#define APLL_WDOGB					BIT(4)
+#define APLL_LPFEN					BIT(5)
 
 #define APLL_REF_CLK_13MHZ			0x1
-#define APLL_REF_CLK_19_2MHZ			0x2
+#define APLL_REF_CLK_19_2MHZ		0x2
 #define APLL_REF_CLK_20MHZ			0x3
 #define APLL_REF_CLK_25MHZ			0x4
 #define APLL_REF_CLK_26MHZ			0x5
-#define APLL_REF_CLK_38_4MHZ			0x6
+#define APLL_REF_CLK_38_4MHZ		0x6
 #define APLL_REF_CLK_40MHZ			0x7
 
-#define APLL_320EN				BIT(14)
-#define APLL_80EN				BIT(15)
-#define APLL_1MEN				BIT(24)
-
-#define ALD_EN					BIT(18)
-#define EF_PD					BIT(19)
-#define EF_FLAG					BIT(31)
-
-#define EF_TRPT					BIT(7)
-#define LDOE25_EN				BIT(31)
-
-#define RSM_EN					BIT(0)
-#define TIMER_EN				BIT(4)
-
-#define TRSW0EN					BIT(2)
-#define TRSW1EN					BIT(3)
-#define EROM_EN					BIT(4)
-#define ENBT					BIT(5)
-#define ENUART					BIT(8)
-#define UART_910				BIT(9)
-#define ENPMAC					BIT(10)
-#define SIC_SWRST				BIT(11)
-#define ENSIC					BIT(12)
-#define SIC_23					BIT(13)
-#define ENHDP					BIT(14)
-#define SIC_LBK					BIT(15)
-
-#define LED0PL					BIT(4)
-#define LED1PL					BIT(12)
-#define LED0DIS					BIT(7)
-
-#define MCUFWDL_EN				BIT(0)
-#define MCUFWDL_RDY				BIT(1)
+#define APLL_320EN					BIT(14)
+#define APLL_80EN					BIT(15)
+#define APLL_1MEN					BIT(24)
+
+#define ALD_EN						BIT(18)
+#define EF_PD						BIT(19)
+#define EF_FLAG						BIT(31)
+
+#define EF_TRPT						BIT(7)
+#define LDOE25_EN					BIT(31)
+
+#define RSM_EN						BIT(0)
+#define TIMER_EN					BIT(4)
+
+#define TRSW0EN						BIT(2)
+#define TRSW1EN						BIT(3)
+#define EROM_EN						BIT(4)
+#define ENBT						BIT(5)
+#define ENUART						BIT(8)
+#define UART_910					BIT(9)
+#define ENPMAC						BIT(10)
+#define SIC_SWRST					BIT(11)
+#define ENSIC						BIT(12)
+#define SIC_23						BIT(13)
+#define ENHDP						BIT(14)
+#define SIC_LBK						BIT(15)
+
+#define LED0PL						BIT(4)
+#define LED1PL						BIT(12)
+#define LED0DIS						BIT(7)
+
+#define MCUFWDL_EN					BIT(0)
+#define MCUFWDL_RDY					BIT(1)
 #define FWDL_CHKSUM_RPT				BIT(2)
-#define MACINI_RDY				BIT(3)
-#define BBINI_RDY				BIT(4)
-#define RFINI_RDY				BIT(5)
-#define WINTINI_RDY				BIT(6)
-#define CPRST					BIT(23)
-
-#define XCLK_VLD				BIT(0)
-#define ACLK_VLD				BIT(1)
-#define UCLK_VLD				BIT(2)
-#define PCLK_VLD				BIT(3)
-#define PCIRSTB					BIT(4)
-#define V15_VLD					BIT(5)
-#define TRP_B15V_EN				BIT(7)
-#define SIC_IDLE				BIT(8)
-#define BD_MAC2					BIT(9)
-#define BD_MAC1					BIT(10)
+#define MACINI_RDY					BIT(3)
+#define BBINI_RDY					BIT(4)
+#define RFINI_RDY					BIT(5)
+#define WINTINI_RDY					BIT(6)
+#define CPRST						BIT(23)
+
+#define XCLK_VLD					BIT(0)
+#define ACLK_VLD					BIT(1)
+#define UCLK_VLD					BIT(2)
+#define PCLK_VLD					BIT(3)
+#define PCIRSTB						BIT(4)
+#define V15_VLD						BIT(5)
+#define TRP_B15V_EN					BIT(7)
+#define SIC_IDLE					BIT(8)
+#define BD_MAC2						BIT(9)
+#define BD_MAC1						BIT(10)
 #define IC_MACPHY_MODE				BIT(11)
-#define VENDOR_ID				BIT(19)
+#define VENDOR_ID					BIT(19)
 #define PAD_HWPD_IDN				BIT(22)
-#define TRP_VAUX_EN				BIT(23)
-#define TRP_BT_EN				BIT(24)
-#define BD_PKG_SEL				BIT(25)
-#define BD_HCI_SEL				BIT(26)
-#define TYPE_ID					BIT(27)
+#define TRP_VAUX_EN					BIT(23)
+#define TRP_BT_EN					BIT(24)
+#define BD_PKG_SEL					BIT(25)
+#define BD_HCI_SEL					BIT(26)
+#define TYPE_ID						BIT(27)
 
 #define CHIP_VER_RTL_MASK			0xF000
 #define CHIP_VER_RTL_SHIFT			12
 
-#define REG_LBMODE				(REG_CR + 3)
+#define REG_LBMODE					(REG_CR + 3)
 
 #define HCI_TXDMA_EN				BIT(0)
 #define HCI_RXDMA_EN				BIT(1)
-#define TXDMA_EN				BIT(2)
-#define RXDMA_EN				BIT(3)
-#define PROTOCOL_EN				BIT(4)
-#define SCHEDULE_EN				BIT(5)
-#define MACTXEN					BIT(6)
-#define MACRXEN					BIT(7)
-#define ENSWBCN					BIT(8)
-#define ENSEC					BIT(9)
-
-#define _NETTYPE(x)				(((x) & 0x3) << 16)
+#define TXDMA_EN					BIT(2)
+#define RXDMA_EN					BIT(3)
+#define PROTOCOL_EN					BIT(4)
+#define SCHEDULE_EN					BIT(5)
+#define MACTXEN						BIT(6)
+#define MACRXEN						BIT(7)
+#define ENSWBCN						BIT(8)
+#define ENSEC						BIT(9)
+
+#define _NETTYPE(x)					(((x) & 0x3) << 16)
 #define MASK_NETTYPE				0x30000
-#define NT_NO_LINK				0x0
+#define NT_NO_LINK					0x0
 #define NT_LINK_AD_HOC				0x1
-#define NT_LINK_AP				0x2
-#define NT_AS_AP				0x3
+#define NT_LINK_AP					0x2
+#define NT_AS_AP					0x3
 
-#define _LBMODE(x)				(((x) & 0xF) << 24)
-#define MASK_LBMODE				0xF000000
+#define _LBMODE(x)					(((x) & 0xF) << 24)
+#define MASK_LBMODE					0xF000000
 #define LOOPBACK_NORMAL				0x0
-#define LOOPBACK_IMMEDIATELY			0xB
+#define LOOPBACK_IMMEDIATELY		0xB
 #define LOOPBACK_MAC_DELAY			0x3
 #define LOOPBACK_PHY				0x1
 #define LOOPBACK_DMA				0x7
 
 #define GET_RX_PAGE_SIZE(value)		((value) & 0xF)
 #define GET_TX_PAGE_SIZE(value)		(((value) & 0xF0) >> 4)
-#define _PSRX_MASK				0xF
-#define _PSTX_MASK				0xF0
-#define _PSRX(x)				(x)
-#define _PSTX(x)				((x) << 4)
+#define _PSRX_MASK					0xF
+#define _PSTX_MASK					0xF0
+#define _PSRX(x)					(x)
+#define _PSTX(x)					((x) << 4)
 
-#define PBP_64					0x0
-#define PBP_128					0x1
-#define PBP_256					0x2
-#define PBP_512					0x3
-#define PBP_1024				0x4
+#define PBP_64						0x0
+#define PBP_128						0x1
+#define PBP_256						0x2
+#define PBP_512						0x3
+#define PBP_1024					0x4
 
 #define RXDMA_ARBBW_EN				BIT(0)
-#define RXSHFT_EN				BIT(1)
+#define RXSHFT_EN					BIT(1)
 #define RXDMA_AGG_EN				BIT(2)
-#define QS_VO_QUEUE				BIT(8)
-#define QS_VI_QUEUE				BIT(9)
-#define QS_BE_QUEUE				BIT(10)
-#define QS_BK_QUEUE				BIT(11)
+#define QS_VO_QUEUE					BIT(8)
+#define QS_VI_QUEUE					BIT(9)
+#define QS_BE_QUEUE					BIT(10)
+#define QS_BK_QUEUE					BIT(11)
 #define QS_MANAGER_QUEUE			BIT(12)
 #define QS_HIGH_QUEUE				BIT(13)
 
-#define HQSEL_VOQ				BIT(0)
-#define HQSEL_VIQ				BIT(1)
-#define HQSEL_BEQ				BIT(2)
-#define HQSEL_BKQ				BIT(3)
-#define HQSEL_MGTQ				BIT(4)
-#define HQSEL_HIQ				BIT(5)
+#define HQSEL_VOQ					BIT(0)
+#define HQSEL_VIQ					BIT(1)
+#define HQSEL_BEQ					BIT(2)
+#define HQSEL_BKQ					BIT(3)
+#define HQSEL_MGTQ					BIT(4)
+#define HQSEL_HIQ					BIT(5)
 
 #define _TXDMA_HIQ_MAP(x)			(((x)&0x3) << 14)
 #define _TXDMA_MGQ_MAP(x)			(((x)&0x3) << 12)
@@ -1018,9 +1033,9 @@
 #define _TXDMA_VIQ_MAP(x)			(((x)&0x3) << 6)
 #define _TXDMA_VOQ_MAP(x)			(((x)&0x3) << 4)
 
-#define QUEUE_LOW				1
+#define QUEUE_LOW					1
 #define QUEUE_NORMAL				2
-#define QUEUE_HIGH				3
+#define QUEUE_HIGH					3
 
 #define _LLT_NO_ACTIVE				0x0
 #define _LLT_WRITE_ACCESS			0x1
@@ -1028,25 +1043,25 @@
 
 #define _LLT_INIT_DATA(x)			((x) & 0xFF)
 #define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
-#define _LLT_OP(x)				(((x) & 0x3) << 30)
+#define _LLT_OP(x)					(((x) & 0x3) << 30)
 #define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
 
 #define BB_WRITE_READ_MASK			(BIT(31) | BIT(30))
-#define BB_WRITE_EN				BIT(30)
-#define BB_READ_EN				BIT(31)
+#define BB_WRITE_EN					BIT(30)
+#define BB_READ_EN					BIT(31)
 
-#define _HPQ(x)					((x) & 0xFF)
-#define _LPQ(x)					(((x) & 0xFF) << 8)
-#define _PUBQ(x)				(((x) & 0xFF) << 16)
-#define _NPQ(x)					((x) & 0xFF)
+#define _HPQ(x)			((x) & 0xFF)
+#define _LPQ(x)			(((x) & 0xFF) << 8)
+#define _PUBQ(x)		(((x) & 0xFF) << 16)
+#define _NPQ(x)			((x) & 0xFF)
 
-#define HPQ_PUBLIC_DIS				BIT(24)
-#define LPQ_PUBLIC_DIS				BIT(25)
-#define LD_RQPN					BIT(31)
+#define HPQ_PUBLIC_DIS		BIT(24)
+#define LPQ_PUBLIC_DIS		BIT(25)
+#define LD_RQPN			BIT(31)
 
-#define BCN_VALID				BIT(16)
-#define BCN_HEAD(x)				(((x) & 0xFF) << 8)
-#define	BCN_HEAD_MASK				0xFF00
+#define BCN_VALID		BIT(16)
+#define BCN_HEAD(x)		(((x) & 0xFF) << 8)
+#define	BCN_HEAD_MASK		0xFF00
 
 #define BLK_DESC_NUM_SHIFT			4
 #define BLK_DESC_NUM_MASK			0xF
@@ -1066,9 +1081,9 @@
 
 #define _RRSR_RSC(x)				(((x) & 0x3) << 21)
 #define RRSR_RSC_RESERVED			0x0
-#define RRSR_RSC_UPPER_SUBCHANNEL		0x1
-#define RRSR_RSC_LOWER_SUBCHANNEL		0x2
-#define RRSR_RSC_DUPLICATE_MODE			0x3
+#define RRSR_RSC_UPPER_SUBCHANNEL	0x1
+#define RRSR_RSC_LOWER_SUBCHANNEL	0x2
+#define RRSR_RSC_DUPLICATE_MODE		0x3
 
 #define USE_SHORT_G1				BIT(20)
 
@@ -1081,8 +1096,8 @@
 #define _AGGLMT_MCS6(x)				(((x) & 0xF) << 24)
 #define _AGGLMT_MCS7(x)				(((x) & 0xF) << 28)
 
-#define	RETRY_LIMIT_SHORT_SHIFT			8
-#define	RETRY_LIMIT_LONG_SHIFT			0
+#define	RETRY_LIMIT_SHORT_SHIFT		8
+#define	RETRY_LIMIT_LONG_SHIFT		0
 
 #define _DARF_RC1(x)				((x) & 0x1F)
 #define _DARF_RC2(x)				(((x) & 0x1F) << 8)
@@ -1102,20 +1117,20 @@
 #define _RARF_RC7(x)				(((x) & 0x1F) << 16)
 #define _RARF_RC8(x)				(((x) & 0x1F) << 24)
 
-#define AC_PARAM_TXOP_LIMIT_OFFSET		16
-#define AC_PARAM_ECW_MAX_OFFSET			12
-#define AC_PARAM_ECW_MIN_OFFSET			8
-#define AC_PARAM_AIFS_OFFSET			0
+#define AC_PARAM_TXOP_LIMIT_OFFSET	16
+#define AC_PARAM_ECW_MAX_OFFSET		12
+#define AC_PARAM_ECW_MIN_OFFSET		8
+#define AC_PARAM_AIFS_OFFSET		0
 
-#define _AIFS(x)				(x)
+#define _AIFS(x)					(x)
 #define _ECW_MAX_MIN(x)				((x) << 8)
 #define _TXOP_LIMIT(x)				((x) << 16)
 
-#define _BCNIFS(x)				((x) & 0xFF)
-#define _BCNECW(x)				((((x) & 0xF)) << 8)
+#define _BCNIFS(x)					((x) & 0xFF)
+#define _BCNECW(x)					((((x) & 0xF)) << 8)
 
-#define _LRL(x)					((x) & 0x3F)
-#define _SRL(x)					(((x) & 0x3F) << 8)
+#define _LRL(x)						((x) & 0x3F)
+#define _SRL(x)						(((x) & 0x3F) << 8)
 
 #define _SIFS_CCK_CTX(x)			((x) & 0xFF)
 #define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8);
@@ -1123,102 +1138,102 @@
 #define _SIFS_OFDM_CTX(x)			((x) & 0xFF)
 #define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8);
 
-#define _TBTT_PROHIBIT_HOLD(x)			(((x) & 0xFF) << 8)
+#define _TBTT_PROHIBIT_HOLD(x)		(((x) & 0xFF) << 8)
 
 #define DIS_EDCA_CNT_DWN			BIT(11)
 
-#define EN_MBSSID				BIT(1)
+#define EN_MBSSID					BIT(1)
 #define EN_TXBCN_RPT				BIT(2)
 #define	EN_BCN_FUNCTION				BIT(3)
 
-#define TSFTR_RST				BIT(0)
-#define TSFTR1_RST				BIT(1)
+#define TSFTR_RST					BIT(0)
+#define TSFTR1_RST					BIT(1)
 
-#define STOP_BCNQ				BIT(6)
+#define STOP_BCNQ					BIT(6)
 
-#define	DIS_TSF_UDT0_NORMAL_CHIP		BIT(4)
-#define	DIS_TSF_UDT0_TEST_CHIP			BIT(5)
+#define	DIS_TSF_UDT0_NORMAL_CHIP	BIT(4)
+#define	DIS_TSF_UDT0_TEST_CHIP		BIT(5)
 
-#define	ACMHW_HWEN				BIT(0)
-#define	ACMHW_BEQEN				BIT(1)
-#define	ACMHW_VIQEN				BIT(2)
-#define	ACMHW_VOQEN				BIT(3)
+#define	ACMHW_HWEN					BIT(0)
+#define	ACMHW_BEQEN					BIT(1)
+#define	ACMHW_VIQEN					BIT(2)
+#define	ACMHW_VOQEN					BIT(3)
 #define	ACMHW_BEQSTATUS				BIT(4)
 #define	ACMHW_VIQSTATUS				BIT(5)
 #define	ACMHW_VOQSTATUS				BIT(6)
 
-#define APSDOFF					BIT(6)
+#define APSDOFF						BIT(6)
 #define APSDOFF_STATUS				BIT(7)
 
-#define BW_20MHZ				BIT(2)
+#define BW_20MHZ					BIT(2)
 
 #define RATE_BITMAP_ALL				0xFFFFF
 
-#define RATE_RRSR_CCK_ONLY_1M			0xFFFF1
+#define RATE_RRSR_CCK_ONLY_1M		0xFFFF1
 
-#define TSFRST					BIT(0)
-#define DIS_GCLK				BIT(1)
-#define PAD_SEL					BIT(2)
-#define PWR_ST					BIT(6)
+#define TSFRST						BIT(0)
+#define DIS_GCLK					BIT(1)
+#define PAD_SEL						BIT(2)
+#define PWR_ST						BIT(6)
 #define PWRBIT_OW_EN				BIT(7)
-#define ACRC					BIT(8)
-#define CFENDFORM				BIT(9)
-#define ICV					BIT(10)
-
-#define AAP					BIT(0)
-#define APM					BIT(1)
-#define AM					BIT(2)
-#define AB					BIT(3)
-#define ADD3					BIT(4)
-#define APWRMGT					BIT(5)
-#define CBSSID					BIT(6)
-#define CBSSID_DATA				BIT(6)
-#define CBSSID_BCN				BIT(7)
-#define ACRC32					BIT(8)
-#define AICV					BIT(9)
-#define ADF					BIT(11)
-#define ACF					BIT(12)
-#define AMF					BIT(13)
+#define ACRC						BIT(8)
+#define CFENDFORM					BIT(9)
+#define ICV							BIT(10)
+
+#define AAP							BIT(0)
+#define APM							BIT(1)
+#define AM							BIT(2)
+#define AB							BIT(3)
+#define ADD3						BIT(4)
+#define APWRMGT						BIT(5)
+#define CBSSID						BIT(6)
+#define CBSSID_DATA					BIT(6)
+#define CBSSID_BCN					BIT(7)
+#define ACRC32						BIT(8)
+#define AICV						BIT(9)
+#define ADF							BIT(11)
+#define ACF							BIT(12)
+#define AMF							BIT(13)
 #define HTC_LOC_CTRL				BIT(14)
-#define UC_DATA_EN				BIT(16)
-#define BM_DATA_EN				BIT(17)
-#define MFBEN					BIT(22)
-#define LSIGEN					BIT(23)
-#define ENMBID					BIT(24)
-#define APP_BASSN				BIT(27)
-#define APP_PHYSTS				BIT(28)
-#define APP_ICV					BIT(29)
-#define APP_MIC					BIT(30)
-#define APP_FCS					BIT(31)
+#define UC_DATA_EN					BIT(16)
+#define BM_DATA_EN					BIT(17)
+#define MFBEN						BIT(22)
+#define LSIGEN						BIT(23)
+#define ENMBID						BIT(24)
+#define APP_BASSN					BIT(27)
+#define APP_PHYSTS					BIT(28)
+#define APP_ICV						BIT(29)
+#define APP_MIC						BIT(30)
+#define APP_FCS						BIT(31)
 
 #define _MIN_SPACE(x)				((x) & 0x7)
-#define _SHORT_GI_PADDING(x)			(((x) & 0x1F) << 3)
+#define _SHORT_GI_PADDING(x)		(((x) & 0x1F) << 3)
 
-#define RXERR_TYPE_OFDM_PPDU			0
-#define RXERR_TYPE_OFDM_FALSE_ALARM		1
-#define	RXERR_TYPE_OFDM_MPDU_OK			2
-#define RXERR_TYPE_OFDM_MPDU_FAIL		3
+#define RXERR_TYPE_OFDM_PPDU		0
+#define RXERR_TYPE_OFDM_FALSE_ALARM	1
+#define	RXERR_TYPE_OFDM_MPDU_OK		2
+#define RXERR_TYPE_OFDM_MPDU_FAIL	3
 #define RXERR_TYPE_CCK_PPDU			4
-#define RXERR_TYPE_CCK_FALSE_ALARM		5
-#define RXERR_TYPE_CCK_MPDU_OK			6
-#define RXERR_TYPE_CCK_MPDU_FAIL		7
+#define RXERR_TYPE_CCK_FALSE_ALARM	5
+#define RXERR_TYPE_CCK_MPDU_OK		6
+#define RXERR_TYPE_CCK_MPDU_FAIL	7
 #define RXERR_TYPE_HT_PPDU			8
-#define RXERR_TYPE_HT_FALSE_ALARM		9
-#define RXERR_TYPE_HT_MPDU_TOTAL		10
-#define RXERR_TYPE_HT_MPDU_OK			11
-#define RXERR_TYPE_HT_MPDU_FAIL			12
-#define RXERR_TYPE_RX_FULL_DROP			15
+#define RXERR_TYPE_HT_FALSE_ALARM	9
+#define RXERR_TYPE_HT_MPDU_TOTAL	10
+#define RXERR_TYPE_HT_MPDU_OK		11
+#define RXERR_TYPE_HT_MPDU_FAIL		12
+#define RXERR_TYPE_RX_FULL_DROP		15
 
 #define RXERR_COUNTER_MASK			0xFFFFF
 #define RXERR_RPT_RST				BIT(27)
-#define _RXERR_RPT_SEL(type)			((type) << 28)
+#define _RXERR_RPT_SEL(type)		((type) << 28)
 
-#define	SCR_TXUSEDK				BIT(0)
-#define	SCR_RXUSEDK				BIT(1)
+#define	SCR_TXUSEDK					BIT(0)
+#define	SCR_RXUSEDK					BIT(1)
 #define	SCR_TXENCENABLE				BIT(2)
 #define	SCR_RXDECENABLE				BIT(3)
-#define	SCR_SKBYA2				BIT(4)
-#define	SCR_NOSKMC				BIT(5)
+#define	SCR_SKBYA2					BIT(4)
+#define	SCR_NOSKMC					BIT(5)
 #define SCR_TXBCUSEDK				BIT(6)
 #define SCR_RXBCUSEDK				BIT(7)
 
@@ -1226,32 +1241,32 @@
 #define USB_IS_FULL_SPEED			1
 #define USB_SPEED_MASK				BIT(5)
 
-#define USB_NORMAL_SIE_EP_MASK			0xF
-#define USB_NORMAL_SIE_EP_SHIFT			4
+#define USB_NORMAL_SIE_EP_MASK		0xF
+#define USB_NORMAL_SIE_EP_SHIFT		4
 
 #define USB_TEST_EP_MASK			0x30
 #define USB_TEST_EP_SHIFT			4
 
-#define USB_AGG_EN				BIT(3)
+#define USB_AGG_EN					BIT(3)
 
 #define MAC_ADDR_LEN				6
-#define LAST_ENTRY_OF_TX_PKT_BUFFER		175/*255    88e*/
+#define LAST_ENTRY_OF_TX_PKT_BUFFER	175/*255    88e*/
 
-#define POLLING_LLT_THRESHOLD			20
+#define POLLING_LLT_THRESHOLD		20
 #define POLLING_READY_TIMEOUT_COUNT		3000
 
 #define	MAX_MSS_DENSITY_2T			0x13
 #define	MAX_MSS_DENSITY_1T			0x0A
 
-#define EPROM_CMD_OPERATING_MODE_MASK		((1<<7)|(1<<6))
+#define EPROM_CMD_OPERATING_MODE_MASK	((1<<7)|(1<<6))
 #define EPROM_CMD_CONFIG			0x3
 #define EPROM_CMD_LOAD				1
 
 #define	HWSET_MAX_SIZE_92S			HWSET_MAX_SIZE
 
-#define	HAL_8192C_HW_GPIO_WPS_BIT		BIT(2)
+#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT(2)
 
-#define	RPMAC_RESET				0x100
+#define	RPMAC_RESET					0x100
 #define	RPMAC_TXSTART				0x104
 #define	RPMAC_TXLEGACYSIG			0x108
 #define	RPMAC_TXHTSIG1				0x10c
@@ -1267,12 +1282,12 @@
 #define	RPMAC_TXMACHEADER5			0x134
 #define	RPMAC_TXDADATYPE			0x138
 #define	RPMAC_TXRANDOMSEED			0x13c
-#define	RPMAC_CCKPLCPPREAMBLE			0x140
+#define	RPMAC_CCKPLCPPREAMBLE		0x140
 #define	RPMAC_CCKPLCPHEADER			0x144
 #define	RPMAC_CCKCRC16				0x148
 #define	RPMAC_OFDMRXCRC32OK			0x170
-#define	RPMAC_OFDMRXCRC32Er			0x174
-#define	RPMAC_OFDMRXPARITYER			0x178
+#define	RPMAC_OFDMRXCRC32ER			0x174
+#define	RPMAC_OFDMRXPARITYER		0x178
 #define	RPMAC_OFDMRXCRC8ER			0x17c
 #define	RPMAC_CCKCRXRC16ER			0x180
 #define	RPMAC_CCKCRXRC32ER			0x184
@@ -1289,45 +1304,45 @@
 #define	RFPGA0_RFTIMING1			0x810
 #define	RFPGA0_RFTIMING2			0x814
 
-#define	RFPGA0_XA_HSSIPARAMETER1		0x820
-#define	RFPGA0_XA_HSSIPARAMETER2		0x824
-#define	RFPGA0_XB_HSSIPARAMETER1		0x828
-#define	RFPGA0_XB_HSSIPARAMETER2		0x82c
+#define	RFPGA0_XA_HSSIPARAMETER1	0x820
+#define	RFPGA0_XA_HSSIPARAMETER2	0x824
+#define	RFPGA0_XB_HSSIPARAMETER1	0x828
+#define	RFPGA0_XB_HSSIPARAMETER2	0x82c
 
-#define	RFPGA0_XA_LSSIPARAMETER			0x840
-#define	RFPGA0_XB_LSSIPARAMETER			0x844
+#define	RFPGA0_XA_LSSIPARAMETER		0x840
+#define	RFPGA0_XB_LSSIPARAMETER		0x844
 
-#define	RFPGA0_RFWAKEUPPARAMETER		0x850
-#define	RFPGA0_RFSLEEPUPPARAMETER		0x854
+#define	RFPGA0_RFWAKEUPPARAMETER	0x850
+#define	RFPGA0_RFSLEEPUPPARAMETER	0x854
 
-#define	RFPGA0_XAB_SWITCHCONTROL		0x858
-#define	RFPGA0_XCD_SWITCHCONTROL		0x85c
+#define	RFPGA0_XAB_SWITCHCONTROL	0x858
+#define	RFPGA0_XCD_SWITCHCONTROL	0x85c
 
-#define	RFPGA0_XA_RFINTERFACEOE			0x860
-#define	RFPGA0_XB_RFINTERFACEOE			0x864
+#define	RFPGA0_XA_RFINTERFACEOE		0x860
+#define	RFPGA0_XB_RFINTERFACEOE		0x864
 
-#define	RFPGA0_XAB_RFINTERFACESW		0x870
-#define	RFPGA0_XCD_RFINTERFACESW		0x874
+#define	RFPGA0_XAB_RFINTERFACESW	0x870
+#define	RFPGA0_XCD_RFINTERFACESW	0x874
 
-#define	rFPGA0_XAB_RFPARAMETER			0x878
-#define	rFPGA0_XCD_RFPARAMETER			0x87c
+#define	RFPGA0_XAB_RFPARAMETER		0x878
+#define	RFPGA0_XCD_RFPARAMETER		0x87c
 
-#define	RFPGA0_ANALOGPARAMETER1			0x880
-#define	RFPGA0_ANALOGPARAMETER2			0x884
-#define	RFPGA0_ANALOGPARAMETER3			0x888
-#define	RFPGA0_ANALOGPARAMETER4			0x88c
+#define	RFPGA0_ANALOGPARAMETER1		0x880
+#define	RFPGA0_ANALOGPARAMETER2		0x884
+#define	RFPGA0_ANALOGPARAMETER3		0x888
+#define	RFPGA0_ANALOGPARAMETER4		0x88c
 
-#define	RFPGA0_XA_LSSIREADBACK			0x8a0
-#define	RFPGA0_XB_LSSIREADBACK			0x8a4
-#define	RFPGA0_XC_LSSIREADBACK			0x8a8
-#define	RFPGA0_XD_LSSIREADBACK			0x8ac
+#define	RFPGA0_XA_LSSIREADBACK		0x8a0
+#define	RFPGA0_XB_LSSIREADBACK		0x8a4
+#define	RFPGA0_XC_LSSIREADBACK		0x8a8
+#define	RFPGA0_XD_LSSIREADBACK		0x8ac
 
 #define	RFPGA0_PSDREPORT			0x8b4
-#define	TRANSCEIVEA_HSPI_READBACK		0x8b8
-#define	TRANSCEIVEB_HSPI_READBACK		0x8bc
-#define	REG_SC_CNT				0x8c4
-#define	RFPGA0_XAB_RFINTERFACERB		0x8e0
-#define	RFPGA0_XCD_RFINTERFACERB		0x8e4
+#define	TRANSCEIVEA_HSPI_READBACK	0x8b8
+#define	TRANSCEIVEB_HSPI_READBACK	0x8bc
+#define	REG_SC_CNT						0x8c4
+#define	RFPGA0_XAB_RFINTERFACERB	0x8e0
+#define	RFPGA0_XCD_RFINTERFACERB	0x8e4
 
 #define	RFPGA1_RFMOD				0x900
 
@@ -1338,12 +1353,12 @@
 #define	RCCK0_SYSTEM				0xa00
 
 #define	RCCK0_AFESETTING			0xa04
-#define	RCCK0_CCA				0xa08
+#define	RCCK0_CCA					0xa08
 
 #define	RCCK0_RXAGC1				0xa0c
 #define	RCCK0_RXAGC2				0xa10
 
-#define	RCCK0_RXHP				0xa14
+#define	RCCK0_RXHP					0xa14
 
 #define	RCCK0_DSPPARAMETER1			0xa18
 #define	RCCK0_DSPPARAMETER2			0xa1c
@@ -1351,75 +1366,74 @@
 #define	RCCK0_TXFILTER1				0xa20
 #define	RCCK0_TXFILTER2				0xa24
 #define	RCCK0_DEBUGPORT				0xa28
-#define	RCCK0_FALSEALARMREPORT			0xa2c
-#define	RCCK0_TRSSIREPORT			0xa50
-#define	RCCK0_RXREPORT				0xa54
-#define	RCCK0_FACOUNTERLOWER			0xa5c
-#define	RCCK0_FACOUNTERUPPER			0xa58
-#define	RCCK0_CCA_CNT				0xa60
-
+#define	RCCK0_FALSEALARMREPORT		0xa2c
+#define	RCCK0_TRSSIREPORT		0xa50
+#define	RCCK0_RXREPORT			0xa54
+#define	RCCK0_FACOUNTERLOWER		0xa5c
+#define	RCCK0_FACOUNTERUPPER		0xa58
+#define	RCCK0_CCA_CNT			0xa60
 
 /* PageB(0xB00) */
-#define	RPDP_ANTA				0xb00
+#define	RPDP_ANTA					0xb00
 #define	RPDP_ANTA_4				0xb04
 #define	RPDP_ANTA_8				0xb08
 #define	RPDP_ANTA_C				0xb0c
-#define	RPDP_ANTA_10				0xb10
-#define	RPDP_ANTA_14				0xb14
-#define	RPDP_ANTA_18				0xb18
-#define	RPDP_ANTA_1C				0xb1c
-#define	RPDP_ANTA_20				0xb20
-#define	RPDP_ANTA_24				0xb24
+#define	RPDP_ANTA_10					0xb10
+#define	RPDP_ANTA_14					0xb14
+#define	RPDP_ANTA_18					0xb18
+#define	RPDP_ANTA_1C					0xb1c
+#define	RPDP_ANTA_20					0xb20
+#define	RPDP_ANTA_24					0xb24
 
 #define	RCONFIG_PMPD_ANTA			0xb28
-#define	RCONFIG_RAM64X16			0xb2c
+#define	RCONFIG_RAM64x16				0xb2c
 
-#define	RBNDA					0xb30
-#define	RHSSIPAR				0xb34
+#define	RBNDA						0xb30
+#define	RHSSIPAR						0xb34
 
-#define	RCONFIG_ANTA				0xb68
-#define	RCONFIG_ANTB				0xb6c
+#define	RCONFIG_ANTA					0xb68
+#define	RCONFIG_ANTB					0xb6c
 
-#define	RPDP_ANTB				0xb70
-#define	RPDP_ANTB_4				0xb74
-#define	RPDP_ANTB_8				0xb78
-#define	RPDP_ANTB_C				0xb7c
-#define	RPDP_ANTB_10				0xb80
-#define	RPDP_ANTB_14				0xb84
-#define	RPDP_ANTB_18				0xb88
-#define	RPDP_ANTB_1C				0xb8c
-#define	RPDP_ANTB_20				0xb90
-#define	RPDP_ANTB_24				0xb94
+#define	RPDP_ANTB					0xb70
+#define	RPDP_ANTB_4					0xb74
+#define	RPDP_ANTB_8					0xb78
+#define	RPDP_ANTB_C					0xb7c
+#define	RPDP_ANTB_10					0xb80
+#define	RPDP_ANTB_14					0xb84
+#define	RPDP_ANTB_18					0xb88
+#define	RPDP_ANTB_1C					0xb8c
+#define	RPDP_ANTB_20					0xb90
+#define	RPDP_ANTB_24					0xb94
 
 #define	RCONFIG_PMPD_ANTB			0xb98
 
-#define	RBNDB					0xba0
+#define	RBNDB						0xba0
 
-#define	RAPK					0xbd8
-#define	rPm_Rx0_AntA				0xbdc
-#define	rPm_Rx1_AntA				0xbe0
-#define	rPm_Rx2_AntA				0xbe4
-#define	rPm_Rx3_AntA				0xbe8
-#define	rPm_Rx0_AntB				0xbec
-#define	rPm_Rx1_AntB				0xbf0
-#define	rPm_Rx2_AntB				0xbf4
-#define	rPm_Rx3_AntB				0xbf8
+#define	RAPK							0xbd8
+#define	RPM_RX0_ANTA				0xbdc
+#define	RPM_RX1_ANTA				0xbe0
+#define	RPM_RX2_ANTA				0xbe4
+#define	RPM_RX3_ANTA				0xbe8
+#define	RPM_RX0_ANTB				0xbec
+#define	RPM_RX1_ANTB				0xbf0
+#define	RPM_RX2_ANTB				0xbf4
+#define	RPM_RX3_ANTB				0xbf8
 
 /*Page C*/
-#define	ROFDM0_LSTF				0xc00
+#define	ROFDM0_LSTF					0xc00
 
-#define	ROFDM0_TRXPATHENABLE			0xc04
+#define	ROFDM0_TRXPATHENABLE		0xc04
 #define	ROFDM0_TRMUXPAR				0xc08
-#define	ROFDM0_TRSWISOLATION			0xc0c
+#define	ROFDM0_TRSWISOLATION		0xc0c
 
 #define	ROFDM0_XARXAFE				0xc10
-#define	ROFDM0_XARXIQIMBAL			0xc14
-#define	ROFDM0_XBRXAFE				0xc18
-#define	ROFDM0_XBRXIQIMBAL			0xc1c
-#define	ROFDM0_XCRXAFE				0xc20
-#define	ROFDM0_XCRXIQIMBAL			0xc24
-#define	ROFDM0_XDRXAFE				0xc28
-#define	ROFDM0_XDRXIQIMBAL			0xc2c
+#define	ROFDM0_XARXIQIMBALANCE		0xc14
+#define	ROFDM0_XBRXAFE			0xc18
+#define	ROFDM0_XBRXIQIMBALANCE		0xc1c
+#define	ROFDM0_XCRXAFE			0xc20
+#define	ROFDM0_XCRXIQIMBANLANCE		0xc24
+#define	ROFDM0_XDRXAFE			0xc28
+#define	ROFDM0_XDRXIQIMBALANCE		0xc2c
 
 #define	ROFDM0_RXDETECTOR1			0xc30
 #define	ROFDM0_RXDETECTOR2			0xc34
@@ -1428,8 +1442,8 @@
 
 #define	ROFDM0_RXDSP				0xc40
 #define	ROFDM0_CFOANDDAGC			0xc44
-#define	ROFDM0_CCADROPTHRES			0xc48
-#define	ROFDM0_ECCATHRES			0xc4c
+#define	ROFDM0_CCADROPTHRESHOLD		0xc48
+#define	ROFDM0_ECCATHRESHOLD		0xc4c
 
 #define	ROFDM0_XAAGCCORE1			0xc50
 #define	ROFDM0_XAAGCCORE2			0xc54
@@ -1440,18 +1454,18 @@
 #define	ROFDM0_XDAGCCORE1			0xc68
 #define	ROFDM0_XDAGCCORE2			0xc6c
 
-#define	ROFDM0_AGCPARAMETER1			0xc70
-#define	ROFDM0_AGCPARAMETER2			0xc74
+#define	ROFDM0_AGCPARAMETER1		0xc70
+#define	ROFDM0_AGCPARAMETER2		0xc74
 #define	ROFDM0_AGCRSSITABLE			0xc78
 #define	ROFDM0_HTSTFAGC				0xc7c
 
-#define	ROFDM0_XATXIQIMBAL			0xc80
+#define	ROFDM0_XATXIQIMBALANCE		0xc80
 #define	ROFDM0_XATXAFE				0xc84
-#define	ROFDM0_XBTXIQIMBAL			0xc88
+#define	ROFDM0_XBTXIQIMBALANCE		0xc88
 #define	ROFDM0_XBTXAFE				0xc8c
-#define	ROFDM0_XCTXIQIMBAL			0xc90
-#define	ROFDM0_XCTXAFE				0xc94
-#define	ROFDM0_XDTXIQIMBAL			0xc98
+#define	ROFDM0_XCTXIQIMBALANCE		0xc90
+#define	ROFDM0_XCTXAFE			0xc94
+#define	ROFDM0_XDTXIQIMBALANCE		0xc98
 #define	ROFDM0_XDTXAFE				0xc9c
 
 #define ROFDM0_RXIQEXTANTA			0xca0
@@ -1462,25 +1476,24 @@
 #define	ROFDM0_TXCOEFF5				0xcb4
 #define	ROFDM0_TXCOEFF6				0xcb8
 
-#define	ROFDM0_RXHPPARAMETER			0xce0
-#define	ROFDM0_TXPSEUDONOISEWGT			0xce4
+#define	ROFDM0_RXHPPARAMETER		0xce0
+#define	ROFDM0_TXPSEUDONOISEWGT		0xce4
 #define	ROFDM0_FRAMESYNC			0xcf0
 #define	ROFDM0_DFSREPORT			0xcf4
 
+#define	ROFDM1_LSTF					0xd00
+#define	ROFDM1_TRXPATHENABLE		0xd04
 
-#define	ROFDM1_LSTF				0xd00
-#define	ROFDM1_TRXPATHENABLE			0xd04
-
-#define	ROFDM1_CF0				0xd08
-#define	ROFDM1_CSI1				0xd10
-#define	ROFDM1_SBD				0xd14
-#define	ROFDM1_CSI2				0xd18
+#define	ROFDM1_CF0					0xd08
+#define	ROFDM1_CSI1					0xd10
+#define	ROFDM1_SBD					0xd14
+#define	ROFDM1_CSI2					0xd18
 #define	ROFDM1_CFOTRACKING			0xd2c
 #define	ROFDM1_TRXMESAURE1			0xd34
 #define	ROFDM1_INTFDET				0xd3c
-#define	ROFDM1_PSEUDONOISESTATEAB		0xd50
-#define	ROFDM1_PSEUDONOISESTATECD		0xd54
-#define	ROFDM1_RXPSEUDONOISEWGT			0xd58
+#define	ROFDM1_PSEUDONOISESTATEAB	0xd50
+#define	ROFDM1_PSEUDONOISESTATECD	0xd54
+#define	ROFDM1_RXPSEUDONOISEWGT		0xd58
 
 #define	ROFDM_PHYCOUNTER1			0xda0
 #define	ROFDM_PHYCOUNTER2			0xda4
@@ -1492,84 +1505,84 @@
 #define	ROFDM_LONGCFOCD				0xdb8
 #define	ROFDM_TAILCF0AB				0xdbc
 #define	ROFDM_TAILCF0CD				0xdc0
-#define	ROFDM_PWMEASURE1			0xdc4
-#define	ROFDM_PWMEASURE2			0xdc8
+#define	ROFDM_PWMEASURE1		0xdc4
+#define	ROFDM_PWMEASURE2		0xdc8
 #define	ROFDM_BWREPORT				0xdcc
 #define	ROFDM_AGCREPORT				0xdd0
-#define	ROFDM_RXSNR				0xdd4
+#define	ROFDM_RXSNR					0xdd4
 #define	ROFDM_RXEVMCSI				0xdd8
 #define	ROFDM_SIGREPORT				0xddc
 
 #define	RTXAGC_A_RATE18_06			0xe00
 #define	RTXAGC_A_RATE54_24			0xe04
 #define	RTXAGC_A_CCK1_MCS32			0xe08
-#define	RTXAGC_A_MCS03_MCS00			0xe10
-#define	RTXAGC_A_MCS07_MCS04			0xe14
-#define	RTXAGC_A_MCS11_MCS08			0xe18
-#define	RTXAGC_A_MCS15_MCS12			0xe1c
+#define	RTXAGC_A_MCS03_MCS00		0xe10
+#define	RTXAGC_A_MCS07_MCS04		0xe14
+#define	RTXAGC_A_MCS11_MCS08		0xe18
+#define	RTXAGC_A_MCS15_MCS12		0xe1c
 
 #define	RTXAGC_B_RATE18_06			0x830
 #define	RTXAGC_B_RATE54_24			0x834
-#define	RTXAGC_B_CCK1_55_MCS32			0x838
-#define	RTXAGC_B_MCS03_MCS00			0x83c
-#define	RTXAGC_B_MCS07_MCS04			0x848
-#define	RTXAGC_B_MCS11_MCS08			0x84c
-#define	RTXAGC_B_MCS15_MCS12			0x868
-#define	RTXAGC_B_CCK11_A_CCK2_11		0x86c
-
-#define	RFPGA0_IQK				0xe28
+#define	RTXAGC_B_CCK1_55_MCS32		0x838
+#define	RTXAGC_B_MCS03_MCS00		0x83c
+#define	RTXAGC_B_MCS07_MCS04		0x848
+#define	RTXAGC_B_MCS11_MCS08		0x84c
+#define	RTXAGC_B_MCS15_MCS12		0x868
+#define	RTXAGC_B_CCK11_A_CCK2_11	0x86c
+
+#define	RFPGA0_IQK					0xe28
 #define	RTX_IQK_TONE_A				0xe30
 #define	RRX_IQK_TONE_A				0xe34
-#define	RTX_IQK_PI_A				0xe38
-#define	RRX_IQK_PI_A				0xe3c
+#define	RTX_IQK_PI_A					0xe38
+#define	RRX_IQK_PI_A					0xe3c
 
-#define	RTX_IQK					0xe40
-#define	RRX_IQK					0xe44
-#define	RIQK_AGC_PTS				0xe48
-#define	RIQK_AGC_RSP				0xe4c
+#define	RTX_IQK							0xe40
+#define	RRX_IQK						0xe44
+#define	RIQK_AGC_PTS					0xe48
+#define	RIQK_AGC_RSP					0xe4c
 #define	RTX_IQK_TONE_B				0xe50
 #define	RRX_IQK_TONE_B				0xe54
-#define	RTX_IQK_PI_B				0xe58
-#define	RRX_IQK_PI_B				0xe5c
+#define	RTX_IQK_PI_B					0xe58
+#define	RRX_IQK_PI_B					0xe5c
 #define	RIQK_AGC_CONT				0xe60
 
-#define	RBLUE_TOOTH				0xe6c
-#define	RRX_WAIT_CCA				0xe70
-#define	RTX_CCK_RFON				0xe74
+#define	RBLUE_TOOTH					0xe6c
+#define	RRX_WAIT_CCA					0xe70
+#define	RTX_CCK_RFON					0xe74
 #define	RTX_CCK_BBON				0xe78
 #define	RTX_OFDM_RFON				0xe7c
 #define	RTX_OFDM_BBON				0xe80
-#define	RTX_TO_RX				0xe84
-#define	RTX_TO_TX				0xe88
-#define	RRX_CCK					0xe8c
+#define	RTX_TO_RX					0xe84
+#define	RTX_TO_TX					0xe88
+#define	RRX_CCK						0xe8c
 
-#define	RTX_POWER_BEFORE_IQK_A			0xe94
+#define	RTX_POWER_BEFORE_IQK_A		0xe94
 #define	RTX_POWER_AFTER_IQK_A			0xe9c
 
-#define	RRX_POWER_BEFORE_IQK_A			0xea0
+#define	RRX_POWER_BEFORE_IQK_A		0xea0
 #define	RRX_POWER_BEFORE_IQK_A_2		0xea4
 #define	RRX_POWER_AFTER_IQK_A			0xea8
-#define	RRX_POWER_AFTER_IQK_A_2			0xeac
+#define	RRX_POWER_AFTER_IQK_A_2		0xeac
 
-#define	RTX_POWER_BEFORE_IQK_B			0xeb4
+#define	RTX_POWER_BEFORE_IQK_B		0xeb4
 #define	RTX_POWER_AFTER_IQK_B			0xebc
 
-#define	RRX_POWER_BEFORE_IQK_B			0xec0
+#define	RRX_POWER_BEFORE_IQK_B		0xec0
 #define	RRX_POWER_BEFORE_IQK_B_2		0xec4
 #define	RRX_POWER_AFTER_IQK_B			0xec8
-#define	RRX_POWER_AFTER_IQK_B_2			0xecc
+#define	RRX_POWER_AFTER_IQK_B_2		0xecc
 
-#define	RRX_OFDM				0xed0
+#define	RRX_OFDM					0xed0
 #define	RRX_WAIT_RIFS				0xed4
-#define	RRX_TO_RX				0xed8
-#define	RSTANDBY				0xedc
-#define	RSLEEP					0xee0
+#define	RRX_TO_RX					0xed8
+#define	RSTANDBY						0xedc
+#define	RSLEEP						0xee0
 #define	RPMPD_ANAEN				0xeec
 
 #define	RZEBRA1_HSSIENABLE			0x0
 #define	RZEBRA1_TRXENABLE1			0x1
 #define	RZEBRA1_TRXENABLE2			0x2
-#define	RZEBRA1_AGC				0x4
+#define	RZEBRA1_AGC					0x4
 #define	RZEBRA1_CHARGEPUMP			0x5
 #define	RZEBRA1_CHANNEL				0x7
 
@@ -1578,666 +1591,681 @@
 #define	RZEBRA1_RXLPF				0xb
 #define	RZEBRA1_RXHPFCORNER			0xc
 
-#define	RGLOBALCTRL				0
+#define	RGLOBALCTRL					0
 #define	RRTL8256_TXLPF				19
 #define	RRTL8256_RXLPF				11
 #define	RRTL8258_TXLPF				0x11
 #define	RRTL8258_RXLPF				0x13
 #define	RRTL8258_RSSILPF			0xa
 
-#define	RF_AC					0x00
+#define	RF_AC						0x00
 
-#define	RF_IQADJ_G1				0x01
-#define	RF_IQADJ_G2				0x02
-#define	RF_POW_TRSW				0x05
+#define	RF_IQADJ_G1					0x01
+#define	RF_IQADJ_G2					0x02
+#define	RF_POW_TRSW					0x05
 
-#define	RF_GAIN_RX				0x06
-#define	RF_GAIN_TX				0x07
+#define	RF_GAIN_RX					0x06
+#define	RF_GAIN_TX					0x07
 
-#define	RF_TXM_IDAC				0x08
-#define	RF_BS_IQGEN				0x0F
+#define	RF_TXM_IDAC					0x08
+#define	RF_BS_IQGEN					0x0F
 
-#define	RF_MODE1				0x10
-#define	RF_MODE2				0x11
+#define	RF_MODE1					0x10
+#define	RF_MODE2					0x11
 
 #define	RF_RX_AGC_HP				0x12
-#define	RF_TX_AGC				0x13
-#define	RF_BIAS					0x14
-#define	RF_IPA					0x15
+#define	RF_TX_AGC					0x13
+#define	RF_BIAS						0x14
+#define	RF_IPA						0x15
 #define	RF_POW_ABILITY				0x17
-#define	RF_MODE_AG				0x18
-#define	RRFCHANNEL				0x18
-#define	RF_CHNLBW				0x18
-#define	RF_TOP					0x19
-
-#define	RF_RX_G1				0x1A
-#define	RF_RX_G2				0x1B
-
-#define	RF_RX_BB2				0x1C
-#define	RF_RX_BB1				0x1D
-
-#define	RF_RCK1					0x1E
-#define	RF_RCK2					0x1F
-
-#define	RF_TX_G1				0x20
-#define	RF_TX_G2				0x21
-#define	RF_TX_G3				0x22
-
-#define	RF_TX_BB1				0x23
-#define	RF_T_METER				0x42
-
-#define	RF_SYN_G1				0x25
-#define	RF_SYN_G2				0x26
-#define	RF_SYN_G3				0x27
-#define	RF_SYN_G4				0x28
-#define	RF_SYN_G5				0x29
-#define	RF_SYN_G6				0x2A
-#define	RF_SYN_G7				0x2B
-#define	RF_SYN_G8				0x2C
-
-#define	RF_RCK_OS				0x30
-#define	RF_TXPA_G1				0x31
-#define	RF_TXPA_G2				0x32
-#define	RF_TXPA_G3				0x33
-
-#define	RF_TX_BIAS_A				0x35
-#define	RF_TX_BIAS_D				0x36
-#define	RF_LOBF_9				0x38
-#define	RF_RXRF_A3				0x3C
-#define	RF_TRSW					0x3F
-
-#define	RF_TXRF_A2				0x41
-#define	RF_TXPA_G4				0x46
-#define	RF_TXPA_A4				0x4B
-
-#define	RF_WE_LUT				0xEF
-
-#define	BBBRESETB				0x100
+#define	RF_MODE_AG					0x18
+#define	RRFCHANNEL					0x18
+#define	RF_CHNLBW					0x18
+#define	RF_TOP						0x19
+
+#define	RF_RX_G1					0x1A
+#define	RF_RX_G2					0x1B
+
+#define	RF_RX_BB2					0x1C
+#define	RF_RX_BB1					0x1D
+
+#define	RF_RCK1						0x1E
+#define	RF_RCK2						0x1F
+
+#define	RF_TX_G1					0x20
+#define	RF_TX_G2					0x21
+#define	RF_TX_G3					0x22
+
+#define	RF_TX_BB1					0x23
+#define	RF_T_METER					0x42
+
+#define	RF_SYN_G1					0x25
+#define	RF_SYN_G2					0x26
+#define	RF_SYN_G3					0x27
+#define	RF_SYN_G4					0x28
+#define	RF_SYN_G5					0x29
+#define	RF_SYN_G6					0x2A
+#define	RF_SYN_G7					0x2B
+#define	RF_SYN_G8					0x2C
+
+#define	RF_RCK_OS					0x30
+#define	RF_TXPA_G1					0x31
+#define	RF_TXPA_G2					0x32
+#define	RF_TXPA_G3					0x33
+
+#define	RF_TX_BIAS_A					0x35
+#define	RF_TX_BIAS_D					0x36
+#define	RF_LOBF_9					0x38
+#define	RF_RXRF_A3					0x3C
+#define	RF_TRSW						0x3F
+
+#define	RF_TXRF_A2					0x41
+#define	RF_TXPA_G4					0x46
+#define	RF_TXPA_A4					0x4B
+
+#define	RF_WE_LUT					0xEF
+
+#define	BBBRESETB					0x100
 #define	BGLOBALRESETB				0x200
 #define	BOFDMTXSTART				0x4
-#define	BCCKTXSTART				0x8
-#define	BCRC32DEBUG				0x100
+#define	BCCKTXSTART					0x8
+#define	BCRC32DEBUG					0x100
 #define	BPMACLOOPBACK				0x10
-#define	BTXLSIG					0xffffff
-#define	BOFDMTXRATE				0xf
+#define	BTXLSIG						0xffffff
+#define	BOFDMTXRATE					0xf
 #define	BOFDMTXRESERVED				0x10
 #define	BOFDMTXLENGTH				0x1ffe0
 #define	BOFDMTXPARITY				0x20000
-#define	BTXHTSIG1				0xffffff
+#define	BTXHTSIG1					0xffffff
 #define	BTXHTMCSRATE				0x7f
-#define	BTXHTBW					0x80
-#define	BTXHTLENGTH				0xffff00
-#define	BTXHTSIG2				0xffffff
+#define	BTXHTBW						0x80
+#define	BTXHTLENGTH					0xffff00
+#define	BTXHTSIG2					0xffffff
 #define	BTXHTSMOOTHING				0x1
 #define	BTXHTSOUNDING				0x2
 #define	BTXHTRESERVED				0x4
 #define	BTXHTAGGREATION				0x8
-#define	BTXHTSTBC				0x30
+#define	BTXHTSTBC					0x30
 #define	BTXHTADVANCECODING			0x40
 #define	BTXHTSHORTGI				0x80
 #define	BTXHTNUMBERHT_LTF			0x300
-#define	BTXHTCRC8				0x3fc00
+#define	BTXHTCRC8					0x3fc00
 #define	BCOUNTERRESET				0x10000
 #define	BNUMOFOFDMTX				0xffff
-#define	BNUMOFCCKTX				0xffff0000
+#define	BNUMOFCCKTX					0xffff0000
 #define	BTXIDLEINTERVAL				0xffff
 #define	BOFDMSERVICE				0xffff0000
 #define	BTXMACHEADER				0xffffffff
-#define	BTXDATAINIT				0xff
-#define	BTXHTMODE				0x100
-#define	BTXDATATYPE				0x30000
+#define	BTXDATAINIT					0xff
+#define	BTXHTMODE					0x100
+#define	BTXDATATYPE					0x30000
 #define	BTXRANDOMSEED				0xffffffff
 #define	BCCKTXPREAMBLE				0x1
-#define	BCCKTXSFD				0xffff0000
-#define	BCCKTXSIG				0xff
+#define	BCCKTXSFD					0xffff0000
+#define	BCCKTXSIG					0xff
 #define	BCCKTXSERVICE				0xff00
 #define	BCCKLENGTHEXT				0x8000
 #define	BCCKTXLENGHT				0xffff0000
-#define	BCCKTXCRC16				0xffff
+#define	BCCKTXCRC16					0xffff
 #define	BCCKTXSTATUS				0x1
 #define	BOFDMTXSTATUS				0x2
 #define IS_BB_REG_OFFSET_92S(_offset)	\
 	((_offset >= 0x800) && (_offset <= 0xfff))
 
-#define	BRFMOD					0x1
-#define	BJAPANMODE				0x2
-#define	BCCKTXSC				0x30
-#define	BCCKEN					0x1000000
-#define	BOFDMEN					0x2000000
-
-#define	BOFDMRXADCPHASE				0x10000
-#define	BOFDMTXDACPHASE				0x40000
-#define	BXATXAGC				0x3f
-
-#define	BXBTXAGC				0xf00
-#define	BXCTXAGC				0xf000
-#define	BXDTXAGC				0xf0000
-
-#define	BPASTART				0xf0000000
-#define	BTRSTART				0x00f00000
-#define	BRFSTART				0x0000f000
-#define	BBBSTART				0x000000f0
-#define	BBBCCKSTART				0x0000000f
-#define	BPAEND					0xf
-#define	BTREND					0x0f000000
-#define	BRFEND					0x000f0000
-#define	BCCAMASK				0x000000f0
-#define	BR2RCCAMASK				0x00000f00
-#define	BHSSI_R2TDELAY				0xf8000000
-#define	BHSSI_T2RDELAY				0xf80000
-#define	BCONTXHSSI				0x400
-#define	BIGFROMCCK				0x200
-#define	BAGCADDRESS				0x3f
-#define	BRXHPTX					0x7000
-#define	BRXHP2RX				0x38000
-#define	BRXHPCCKINI				0xc0000
-#define	BAGCTXCODE				0xc00000
-#define	BAGCRXCODE				0x300000
-
-#define	B3WIREDATALENGTH			0x800
-#define	B3WIREADDREAALENGTH			0x400
-
-#define	B3WIRERFPOWERDOWN			0x1
-#define	B5GPAPEPOLARITY				0x40000000
-#define	B2GPAPEPOLARITY				0x80000000
-#define	BRFSW_TXDEFAULTANT			0x3
-#define	BRFSW_TXOPTIONANT			0x30
-#define	BRFSW_RXDEFAULTANT			0x300
-#define	BRFSW_RXOPTIONANT			0x3000
-#define	BRFSI_3WIREDATA				0x1
-#define	BRFSI_3WIRECLOCK			0x2
-#define	BRFSI_3WIRELOAD				0x4
-#define	BRFSI_3WIRERW				0x8
-#define	BRFSI_3WIRE				0xf
-
-#define	BRFSI_RFENV				0x10
-
-#define	BRFSI_TRSW				0x20
-#define	BRFSI_TRSWB				0x40
-#define	BRFSI_ANTSW				0x100
-#define	BRFSI_ANTSWB				0x200
-#define	BRFSI_PAPE				0x400
-#define	BRFSI_PAPE5G				0x800
-#define	BBANDSELECT				0x1
-#define	BHTSIG2_GI				0x80
-#define	BHTSIG2_SMOOTHING			0x01
-#define	BHTSIG2_SOUNDING			0x02
-#define	BHTSIG2_AGGREATON			0x08
-#define	BHTSIG2_STBC				0x30
-#define	BHTSIG2_ADVCODING			0x40
-#define	BHTSIG2_NUMOFHTLTF			0x300
-#define	BHTSIG2_CRC8				0x3fc
-#define	BHTSIG1_MCS				0x7f
-#define	BHTSIG1_BANDWIDTH			0x80
-#define	BHTSIG1_HTLENGTH			0xffff
-#define	BLSIG_RATE				0xf
-#define	BLSIG_RESERVED				0x10
-#define	BLSIG_LENGTH				0x1fffe
-#define	BLSIG_PARITY				0x20
-#define	BCCKRXPHASE				0x4
-
-#define	BLSSIREADADDRESS			0x7f800000
-#define	BLSSIREADEDGE				0x80000000
-
-#define	BLSSIREADBACKDATA			0xfffff
-
-#define	BLSSIREADOKFLAG				0x1000
-#define	BCCKSAMPLERATE				0x8
-#define	BREGULATOR0STANDBY			0x1
-#define	BREGULATORPLLSTANDBY			0x2
-#define	BREGULATOR1STANDBY			0x4
-#define	BPLLPOWERUP				0x8
-#define	BDPLLPOWERUP				0x10
-#define	BDA10POWERUP				0x20
-#define	BAD7POWERUP				0x200
-#define	BDA6POWERUP				0x2000
-#define	BXTALPOWERUP				0x4000
-#define	B40MDCLKPOWERUP				0x8000
-#define	BDA6DEBUGMODE				0x20000
-#define	BDA6SWING				0x380000
-
-#define	BADCLKPHASE				0x4000000
-#define	B80MCLKDELAY				0x18000000
-#define	BAFEWATCHDOGENABLE			0x20000000
-
-#define	BXTALCAP01				0xc0000000
-#define	BXTALCAP23				0x3
-#define	BXTALCAP92X				0x0f000000
-#define BXTALCAP				0x0f000000
-
-#define	BINTDIFCLKENABLE			0x400
-#define	BEXTSIGCLKENABLE			0x800
-#define	BBANDGAP_MBIAS_POWERUP			0x10000
-#define	BAD11SH_GAIN				0xc0000
-#define	BAD11NPUT_RANGE				0x700000
-#define	BAD110P_CURRENT				0x3800000
-#define	BLPATH_LOOPBACK				0x4000000
-#define	BQPATH_LOOPBACK				0x8000000
-#define	BAFE_LOOPBACK				0x10000000
-#define	BDA10_SWING				0x7e0
-#define	BDA10_REVERSE				0x800
-#define	BDA_CLK_SOURCE				0x1000
-#define	BDA7INPUT_RANGE				0x6000
-#define	BDA7_GAIN				0x38000
-#define	BDA7OUTPUT_CM_MODE			0x40000
-#define	BDA7INPUT_CM_MODE			0x380000
-#define	BDA7CURRENT				0xc00000
-#define	BREGULATOR_ADJUST			0x7000000
-#define	BAD11POWERUP_ATTX			0x1
-#define	BDA10PS_ATTX				0x10
-#define	BAD11POWERUP_ATRX			0x100
-#define	BDA10PS_ATRX				0x1000
-#define	BCCKRX_AGC_FORMAT			0x200
-#define	BPSDFFT_SAMPLE_POINT			0xc000
-#define	BPSD_AVERAGE_NUM			0x3000
-#define	BIQPATH_CONTROL				0xc00
-#define	BPSD_FREQ				0x3ff
-#define	BPSD_ANTENNA_PATH			0x30
-#define	BPSD_IQ_SWITCH				0x40
-#define	BPSD_RX_TRIGGER				0x400000
-#define	BPSD_TX_TRIGGERCW			0x80000000
-#define	BPSD_SINE_TONE_SCALE			0x7f000000
-#define	BPSD_REPORT				0xffff
-
-#define	BOFDM_TXSC				0x30000000
-#define	BCCK_TXON				0x1
-#define	BOFDM_TXON				0x2
-#define	BDEBUG_PAGE				0xfff
-#define	BDEBUG_ITEM				0xff
-#define	BANTL					0x10
-#define	BANT_NONHT				0x100
-#define	BANT_HT1				0x1000
-#define	BANT_HT2				0x10000
-#define	BANT_HT1S1				0x100000
-#define	BANT_NONHTS1				0x1000000
-
-#define	BCCK_BBMODE				0x3
-#define	BCCK_TXPOWERSAVING			0x80
-#define	BCCK_RXPOWERSAVING			0x40
-
-#define	BCCK_SIDEBAND				0x10
-
-#define	BCCK_SCRAMBLE				0x8
-#define	BCCK_ANTDIVERSITY			0x8000
-#define	BCCK_CARRIER_RECOVERY			0x4000
-#define	BCCK_TXRATE				0x3000
-#define	BCCK_DCCANCEL				0x0800
-#define	BCCK_ISICANCEL				0x0400
-#define	BCCK_MATCH_FILTER			0x0200
-#define	BCCK_EQUALIZER				0x0100
-#define	BCCK_PREAMBLE_DETECT			0x800000
-#define	BCCK_FAST_FALSECCA			0x400000
-#define	BCCK_CH_ESTSTART			0x300000
-#define	BCCK_CCA_COUNT				0x080000
-#define	BCCK_CS_LIM				0x070000
-#define	BCCK_BIST_MODE				0x80000000
-#define	BCCK_CCAMASK				0x40000000
-#define	BCCK_TX_DAC_PHASE			0x4
-#define	BCCK_RX_ADC_PHASE			0x20000000
-#define	BCCKR_CP_MODE				0x0100
-#define	BCCK_TXDC_OFFSET			0xf0
-#define	BCCK_RXDC_OFFSET			0xf
-#define	BCCK_CCA_MODE				0xc000
-#define	BCCK_FALSECS_LIM			0x3f00
-#define	BCCK_CS_RATIO				0xc00000
-#define	BCCK_CORGBIT_SEL			0x300000
-#define	BCCK_PD_LIM				0x0f0000
-#define	BCCK_NEWCCA				0x80000000
-#define	BCCK_RXHP_OF_IG				0x8000
-#define	BCCK_RXIG				0x7f00
-#define	BCCK_LNA_POLARITY			0x800000
-#define	BCCK_RX1ST_BAIN				0x7f0000
-#define	BCCK_RF_EXTEND				0x20000000
-#define	BCCK_RXAGC_SATLEVEL			0x1f000000
-#define	BCCK_RXAGC_SATCOUNT			0xe0
-#define	BCCKRXRFSETTLE				0x1f
-#define	BCCK_FIXED_RXAGC			0x8000
-#define	BCCK_ANTENNA_POLARITY			0x2000
-#define	BCCK_TXFILTER_TYPE			0x0c00
-#define	BCCK_RXAGC_REPORTTYPE			0x0300
-#define	BCCK_RXDAGC_EN				0x80000000
-#define	BCCK_RXDAGC_PERIOD			0x20000000
-#define	BCCK_RXDAGC_SATLEVEL			0x1f000000
-#define	BCCK_TIMING_RECOVERY			0x800000
-#define	BCCK_TXC0				0x3f0000
-#define	BCCK_TXC1				0x3f000000
-#define	BCCK_TXC2				0x3f
-#define	BCCK_TXC3				0x3f00
-#define	BCCK_TXC4				0x3f0000
-#define	BCCK_TXC5				0x3f000000
-#define	BCCK_TXC6				0x3f
-#define	BCCK_TXC7				0x3f00
-#define	BCCK_DEBUGPORT				0xff0000
-#define	BCCK_DAC_DEBUG				0x0f000000
-#define	BCCK_FALSEALARM_ENABLE			0x8000
-#define	BCCK_FALSEALARM_READ			0x4000
-#define	BCCK_TRSSI				0x7f
-#define	BCCK_RXAGC_REPORT			0xfe
-#define	BCCK_RXREPORT_ANTSEL			0x80000000
-#define	BCCK_RXREPORT_MFOFF			0x40000000
-#define	BCCK_RXREPORT_SQLOSS			0x20000000
-#define	BCCK_RXREPORT_PKTLOSS			0x10000000
-#define	BCCK_RXREPORT_LOCKEDBIT			0x08000000
-#define	BCCK_RXREPORT_RATEERROR			0x04000000
-#define	BCCK_RXREPORT_RXRATE			0x03000000
-#define	BCCK_RXFA_COUNTER_LOWER			0xff
-#define	BCCK_RXFA_COUNTER_UPPER			0xff000000
-#define	BCCK_RXHPAGC_START			0xe000
-#define	BCCK_RXHPAGC_FINAL			0x1c00
-#define	BCCK_RXFALSEALARM_ENABLE		0x8000
-#define	BCCK_FACOUNTER_FREEZE			0x4000
-#define	BCCK_TXPATH_SEL				0x10000000
-#define	BCCK_DEFAULT_RXPATH			0xc000000
-#define	BCCK_OPTION_RXPATH			0x3000000
-
-#define	BNUM_OFSTF				0x3
-#define	BSHIFT_L				0xc0
-#define	BGI_TH					0xc
-#define	BRXPATH_A				0x1
-#define	BRXPATH_B				0x2
-#define	BRXPATH_C				0x4
-#define	BRXPATH_D				0x8
-#define	BTXPATH_A				0x1
-#define	BTXPATH_B				0x2
-#define	BTXPATH_C				0x4
-#define	BTXPATH_D				0x8
-#define	BTRSSI_FREQ				0x200
-#define	BADC_BACKOFF				0x3000
-#define	BDFIR_BACKOFF				0xc000
-#define	BTRSSI_LATCH_PHASE			0x10000
-#define	BRX_LDC_OFFSET				0xff
-#define	BRX_QDC_OFFSET				0xff00
-#define	BRX_DFIR_MODE				0x1800000
-#define	BRX_DCNF_TYPE				0xe000000
-#define	BRXIQIMB_A				0x3ff
-#define	BRXIQIMB_B				0xfc00
-#define	BRXIQIMB_C				0x3f0000
-#define	BRXIQIMB_D				0xffc00000
-#define	BDC_DC_NOTCH				0x60000
-#define	BRXNB_NOTCH				0x1f000000
-#define	BPD_TH					0xf
-#define	BPD_TH_OPT2				0xc000
-#define	BPWED_TH				0x700
-#define	BIFMF_WIN_L				0x800
-#define	BPD_OPTION				0x1000
-#define	BMF_WIN_L				0xe000
-#define	BBW_SEARCH_L				0x30000
-#define	BWIN_ENH_L				0xc0000
-#define	BBW_TH					0x700000
-#define	BED_TH2					0x3800000
-#define	BBW_OPTION				0x4000000
-#define	BRADIO_TH				0x18000000
-#define	BWINDOW_L				0xe0000000
-#define	BSBD_OPTION				0x1
-#define	BFRAME_TH				0x1c
-#define	BFS_OPTION				0x60
-#define	BDC_SLOPE_CHECK				0x80
-#define	BFGUARD_COUNTER_DC_L			0xe00
-#define	BFRAME_WEIGHT_SHORT			0x7000
-#define	BSUB_TUNE				0xe00000
-#define	BFRAME_DC_LENGTH			0xe000000
-#define	BSBD_START_OFFSET			0x30000000
-#define	BFRAME_TH_2				0x7
-#define	BFRAME_GI2_TH				0x38
-#define	BGI2_SYNC_EN				0x40
-#define	BSARCH_SHORT_EARLY			0x300
-#define	BSARCH_SHORT_LATE			0xc00
-#define	BSARCH_GI2_LATE				0x70000
-#define	BCFOANTSUM				0x1
-#define	BCFOACC					0x2
-#define	BCFOSTARTOFFSET				0xc
-#define	BCFOLOOPBACK				0x70
-#define	BCFOSUMWEIGHT				0x80
-#define	BDAGCENABLE				0x10000
-#define	BTXIQIMB_A				0x3ff
-#define	BTXIQIMB_B				0xfc00
-#define	BTXIQIMB_C				0x3f0000
-#define	BTXIQIMB_D				0xffc00000
-#define	BTXIDCOFFSET				0xff
-#define	BTXIQDCOFFSET				0xff00
-#define	BTXDFIRMODE				0x10000
-#define	BTXPESUDO_NOISEON			0x4000000
-#define	BTXPESUDO_NOISE_A			0xff
-#define	BTXPESUDO_NOISE_B			0xff00
-#define	BTXPESUDO_NOISE_C			0xff0000
-#define	BTXPESUDO_NOISE_D			0xff000000
-#define	BCCA_DROPOPTION				0x20000
-#define	BCCA_DROPTHRES				0xfff00000
-#define	BEDCCA_H				0xf
-#define	BEDCCA_L				0xf0
-#define	BLAMBDA_ED				0x300
-#define	BRX_INITIALGAIN				0x7f
-#define	BRX_ANTDIV_EN				0x80
-#define	BRX_AGC_ADDRESS_FOR_LNA			0x7f00
-#define	BRX_HIGHPOWER_FLOW			0x8000
-#define	BRX_AGC_FREEZE_THRES			0xc0000
-#define	BRX_FREEZESTEP_AGC1			0x300000
-#define	BRX_FREEZESTEP_AGC2			0xc00000
-#define	BRX_FREEZESTEP_AGC3			0x3000000
-#define	BRX_FREEZESTEP_AGC0			0xc000000
-#define	BRXRSSI_CMP_EN				0x10000000
-#define	BRXQUICK_AGCEN				0x20000000
-#define	BRXAGC_FREEZE_THRES_MODE		0x40000000
-#define	BRX_OVERFLOW_CHECKTYPE			0x80000000
-#define	BRX_AGCSHIFT				0x7f
-#define	BTRSW_TRI_ONLY				0x80
-#define	BPOWER_THRES				0x300
-#define	BRXAGC_EN				0x1
-#define	BRXAGC_TOGETHER_EN			0x2
-#define	BRXAGC_MIN				0x4
-#define	BRXHP_INI				0x7
-#define	BRXHP_TRLNA				0x70
-#define	BRXHP_RSSI				0x700
-#define	BRXHP_BBP1				0x7000
-#define	BRXHP_BBP2				0x70000
-#define	BRXHP_BBP3				0x700000
-#define	BRSSI_H					0x7f0000
-#define	BRSSI_GEN				0x7f000000
-#define	BRXSETTLE_TRSW				0x7
-#define	BRXSETTLE_LNA				0x38
-#define	BRXSETTLE_RSSI				0x1c0
-#define	BRXSETTLE_BBP				0xe00
-#define	BRXSETTLE_RXHP				0x7000
-#define	BRXSETTLE_ANTSW_RSSI			0x38000
-#define	BRXSETTLE_ANTSW				0xc0000
-#define	BRXPROCESS_TIME_DAGC			0x300000
-#define	BRXSETTLE_HSSI				0x400000
-#define	BRXPROCESS_TIME_BBPPW			0x800000
-#define	BRXANTENNA_POWER_SHIFT			0x3000000
-#define	BRSSI_TABLE_SELECT			0xc000000
-#define	BRXHP_FINAL				0x7000000
-#define	BRXHPSETTLE_BBP				0x7
-#define	BRXHTSETTLE_HSSI			0x8
-#define	BRXHTSETTLE_RXHP			0x70
-#define	BRXHTSETTLE_BBPPW			0x80
-#define	BRXHTSETTLE_IDLE			0x300
-#define	BRXHTSETTLE_RESERVED			0x1c00
-#define	BRXHT_RXHP_EN				0x8000
-#define	BRXAGC_FREEZE_THRES			0x30000
-#define	BRXAGC_TOGETHEREN			0x40000
-#define	BRXHTAGC_MIN				0x80000
-#define	BRXHTAGC_EN				0x100000
-#define	BRXHTDAGC_EN				0x200000
-#define	BRXHT_RXHP_BBP				0x1c00000
-#define	BRXHT_RXHP_FINAL			0xe0000000
-#define	BRXPW_RADIO_TH				0x3
-#define	BRXPW_RADIO_EN				0x4
-#define	BRXMF_HOLD				0x3800
-#define	BRXPD_DELAY_TH1				0x38
-#define	BRXPD_DELAY_TH2				0x1c0
-#define	BRXPD_DC_COUNT_MAX			0x600
-#define	BRXPD_DELAY_TH				0x8000
-#define	BRXPROCESS_DELAY			0xf0000
-#define	BRXSEARCHRANGE_GI2_EARLY		0x700000
-#define	BRXFRAME_FUARD_COUNTER_L		0x3800000
-#define	BRXSGI_GUARD_L				0xc000000
-#define	BRXSGI_SEARCH_L				0x30000000
-#define	BRXSGI_TH				0xc0000000
-#define	BDFSCNT0				0xff
-#define	BDFSCNT1				0xff00
-#define	BDFSFLAG				0xf0000
-#define	BMF_WEIGHT_SUM				0x300000
-#define	BMINIDX_TH				0x7f000000
-#define	BDAFORMAT				0x40000
-#define	BTXCH_EMU_ENABLE			0x01000000
-#define	BTRSW_ISOLATION_A			0x7f
-#define	BTRSW_ISOLATION_B			0x7f00
-#define	BTRSW_ISOLATION_C			0x7f0000
-#define	BTRSW_ISOLATION_D			0x7f000000
-#define	BEXT_LNA_GAIN				0x7c00
-
-#define	BSTBC_EN				0x4
-#define	BANTENNA_MAPPING			0x10
-#define	BNSS					0x20
-#define	BCFO_ANTSUM_ID				0x200
-#define	BPHY_COUNTER_RESET			0x8000000
-#define	BCFO_REPORT_GET				0x4000000
-#define	BOFDM_CONTINUE_TX			0x10000000
-#define	BOFDM_SINGLE_CARRIER			0x20000000
-#define	BOFDM_SINGLE_TONE			0x40000000
-#define	BHT_DETECT				0x100
-#define	BCFOEN					0x10000
-#define	BCFOVALUE				0xfff00000
-#define	BSIGTONE_RE				0x3f
-#define	BSIGTONE_IM				0x7f00
-#define	BCOUNTER_CCA				0xffff
-#define	BCOUNTER_PARITYFAIL			0xffff0000
-#define	BCOUNTER_RATEILLEGAL			0xffff
-#define	BCOUNTER_CRC8FAIL			0xffff0000
-#define	BCOUNTER_MCSNOSUPPORT			0xffff
-#define	BCOUNTER_FASTSYNC			0xffff
-#define	BSHORTCFO				0xfff
-#define	BSHORTCFOT_LENGTH			12
-#define	BSHORTCFOF_LENGTH			11
-#define	BLONGCFO				0x7ff
-#define	BLONGCFOT_LENGTH			11
-#define	BLONGCFOF_LENGTH			11
-#define	BTAILCFO				0x1fff
-#define	BTAILCFOT_LENGTH			13
-#define	BTAILCFOF_LENGTH			12
-#define	BNOISE_EN_PWDB				0xffff
-#define	BCC_POWER_DB				0xffff0000
-#define	BMOISE_PWDB				0xffff
-#define	BPOWERMEAST_LENGTH			10
-#define	BPOWERMEASF_LENGTH			3
-#define	BRX_HT_BW				0x1
-#define	BRXSC					0x6
-#define	BRX_HT					0x8
-#define	BNB_INTF_DET_ON				0x1
-#define	BINTF_WIN_LEN_CFG			0x30
-#define	BNB_INTF_TH_CFG				0x1c0
-#define	BRFGAIN					0x3f
-#define	BTABLESEL				0x40
-#define	BTRSW					0x80
-#define	BRXSNR_A				0xff
-#define	BRXSNR_B				0xff00
-#define	BRXSNR_C				0xff0000
-#define	BRXSNR_D				0xff000000
-#define	BSNR_EVMT_LENGTH			8
-#define	BSNR_EVMF_LENGTH			1
-#define	BCSI1ST					0xff
-#define	BCSI2ND					0xff00
-#define	BRXEVM1ST				0xff0000
-#define	BRXEVM2ND				0xff000000
-#define	BSIGEVM					0xff
-#define	BPWDB					0xff00
-#define	BSGIEN					0x10000
-
-#define	BSFACTOR_QMA1				0xf
-#define	BSFACTOR_QMA2				0xf0
-#define	BSFACTOR_QMA3				0xf00
-#define	BSFACTOR_QMA4				0xf000
-#define	BSFACTOR_QMA5				0xf0000
-#define	BSFACTOR_QMA6				0xf0000
-#define	BSFACTOR_QMA7				0xf00000
-#define	BSFACTOR_QMA8				0xf000000
-#define	BSFACTOR_QMA9				0xf0000000
-#define	BCSI_SCHEME				0x100000
-
-#define	BNOISE_LVL_TOP_SET			0x3
-#define	BCHSMOOTH				0x4
-#define	BCHSMOOTH_CFG1				0x38
-#define	BCHSMOOTH_CFG2				0x1c0
-#define	BCHSMOOTH_CFG3				0xe00
-#define	BCHSMOOTH_CFG4				0x7000
-#define	BMRCMODE				0x800000
-#define	BTHEVMCFG				0x7000000
-
-#define	BLOOP_FIT_TYPE				0x1
-#define	BUPD_CFO				0x40
-#define	BUPD_CFO_OFFDATA			0x80
-#define	BADV_UPD_CFO				0x100
-#define	BADV_TIME_CTRL				0x800
-#define	BUPD_CLKO				0x1000
-#define	BFC					0x6000
-#define	BTRACKING_MODE				0x8000
-#define	BPHCMP_ENABLE				0x10000
-#define	BUPD_CLKO_LTF				0x20000
-#define	BCOM_CH_CFO				0x40000
-#define	BCSI_ESTI_MODE				0x80000
-#define	BADV_UPD_EQZ				0x100000
-#define	BUCHCFG					0x7000000
-#define	BUPDEQZ					0x8000000
-
-#define	BRX_PESUDO_NOISE_ON			0x20000000
-#define	BRX_PESUDO_NOISE_A			0xff
-#define	BRX_PESUDO_NOISE_B			0xff00
-#define	BRX_PESUDO_NOISE_C			0xff0000
-#define	BRX_PESUDO_NOISE_D			0xff000000
-#define	BRX_PESUDO_NOISESTATE_A			0xffff
-#define	BRX_PESUDO_NOISESTATE_B			0xffff0000
-#define	BRX_PESUDO_NOISESTATE_C			0xffff
-#define	BRX_PESUDO_NOISESTATE_D			0xffff0000
-
-#define	BZEBRA1_HSSIENABLE			0x8
-#define	BZEBRA1_TRXCONTROL			0xc00
-#define	BZEBRA1_TRXGAINSETTING			0x07f
-#define	BZEBRA1_RXCOUNTER			0xc00
-#define	BZEBRA1_TXCHANGEPUMP			0x38
-#define	BZEBRA1_RXCHANGEPUMP			0x7
-#define	BZEBRA1_CHANNEL_NUM			0xf80
-#define	BZEBRA1_TXLPFBW				0x400
-#define	BZEBRA1_RXLPFBW				0x600
-
-#define	BRTL8256REG_MODE_CTRL1			0x100
-#define	BRTL8256REG_MODE_CTRL0			0x40
-#define	BRTL8256REG_TXLPFBW			0x18
-#define	BRTL8256REG_RXLPFBW			0x600
-
-#define	BRTL8258_TXLPFBW			0xc
-#define	BRTL8258_RXLPFBW			0xc00
-#define	BRTL8258_RSSILPFBW			0xc0
-
-#define	BBYTE0					0x1
-#define	BBYTE1					0x2
-#define	BBYTE2					0x4
-#define	BBYTE3					0x8
-#define	BWORD0					0x3
-#define	BWORD1					0xc
-#define	BWORD					0xf
-
-#define	BENABLE					0x1
-#define	BDISABLE				0x0
-
-#define	LEFT_ANTENNA				0x0
-#define	RIGHT_ANTENNA				0x1
-
-#define	TCHECK_TXSTATUS				500
-#define	TUPDATE_RXCOUNTER			100
-
-#define	REG_UN_USED_REGISTER			0x01bf
+#define	BRFMOD						0x1
+#define	BJAPANMODE					0x2
+#define	BCCKTXSC					0x30
+#define	BCCKEN						0x1000000
+#define	BOFDMEN						0x2000000
+
+#define	BOFDMRXADCPHASE			0x10000
+#define	BOFDMTXDACPHASE			0x40000
+#define	BXATXAGC			0x3f
+
+#define	BXBTXAGC			0xf00
+#define	BXCTXAGC			0xf000
+#define	BXDTXAGC			0xf0000
+
+#define	BPASTART			0xf0000000
+#define	BTRSTART			0x00f00000
+#define	BRFSTART			0x0000f000
+#define	BBBSTART			0x000000f0
+#define	BBBCCKSTART			0x0000000f
+#define	BPAEND				0xf
+#define	BTREND				0x0f000000
+#define	BRFEND				0x000f0000
+#define	BCCAMASK			0x000000f0
+#define	BR2RCCAMASK			0x00000f00
+#define	BHSSI_R2TDELAY			0xf8000000
+#define	BHSSI_T2RDELAY			0xf80000
+#define	BCONTXHSSI			0x400
+#define	BIGFROMCCK			0x200
+#define	BAGCADDRESS			0x3f
+#define	BRXHPTX				0x7000
+#define	BRXHP2RX			0x38000
+#define	BRXHPCCKINI			0xc0000
+#define	BAGCTXCODE			0xc00000
+#define	BAGCRXCODE			0x300000
+
+#define	B3WIREDATALENGTH		0x800
+#define	B3WIREADDREAALENGTH		0x400
+
+#define	B3WIRERFPOWERDOWN		0x1
+#define	B5GPAPEPOLARITY			0x40000000
+#define	B2GPAPEPOLARITY			0x80000000
+#define	BRFSW_TXDEFAULTANT		0x3
+#define	BRFSW_TXOPTIONANT		0x30
+#define	BRFSW_RXDEFAULTANT		0x300
+#define	BRFSW_RXOPTIONANT		0x3000
+#define	BRFSI_3WIREDATA			0x1
+#define	BRFSI_3WIRECLOCK		0x2
+#define	BRFSI_3WIRELOAD			0x4
+#define	BRFSI_3WIRERW			0x8
+#define	BRFSI_3WIRE			0xf
+
+#define	BRFSI_RFENV			0x10
+
+#define	BRFSI_TRSW			0x20
+#define	BRFSI_TRSWB			0x40
+#define	BRFSI_ANTSW			0x100
+#define	BRFSI_ANTSWB			0x200
+#define	BRFSI_PAPE			0x400
+#define	BRFSI_PAPE5G			0x800
+#define	BBANDSELECT			0x1
+#define	BHTSIG2_GI			0x80
+#define	BHTSIG2_SMOOTHING		0x01
+#define	BHTSIG2_SOUNDING		0x02
+#define	BHTSIG2_AGGREATON		0x08
+#define	BHTSIG2_STBC			0x30
+#define	BHTSIG2_ADVCODING		0x40
+#define	BHTSIG2_NUMOFHTLTF		0x300
+#define	BHTSIG2_CRC8			0x3fc
+#define	BHTSIG1_MCS			0x7f
+#define	BHTSIG1_BANDWIDTH		0x80
+#define	BHTSIG1_HTLENGTH		0xffff
+#define	BLSIG_RATE			0xf
+#define	BLSIG_RESERVED			0x10
+#define	BLSIG_LENGTH			0x1fffe
+#define	BLSIG_PARITY			0x20
+#define	BCCKRXPHASE			0x4
+
+#define	BLSSIREADADDRESS		0x7f800000
+#define	BLSSIREADEDGE			0x80000000
+
+#define	BLSSIREADBACKDATA		0xfffff
+
+#define	BLSSIREADOKFLAG			0x1000
+#define	BCCKSAMPLERATE			0x8
+#define	BREGULATOR0STANDBY		0x1
+#define	BREGULATORPLLSTANDBY		0x2
+#define	BREGULATOR1STANDBY		0x4
+#define	BPLLPOWERUP			0x8
+#define	BDPLLPOWERUP			0x10
+#define	BDA10POWERUP			0x20
+#define	BAD7POWERUP			0x200
+#define	BDA6POWERUP			0x2000
+#define	BXTALPOWERUP			0x4000
+#define	B40MDCLKPOWERUP			0x8000
+#define	BDA6DEBUGMODE			0x20000
+#define	BDA6SWING			0x380000
+
+#define	BADCLKPHASE			0x4000000
+#define	B80MCLKDELAY			0x18000000
+#define	BAFEWATCHDOGENABLE		0x20000000
+
+#define	BXTALCAP01			0xc0000000
+#define	BXTALCAP23			0x3
+#define	BXTALCAP92X					0x0f000000
+#define BXTALCAP			0x0f000000
+
+#define	BINTDIFCLKENABLE		0x400
+#define	BEXTSIGCLKENABLE		0x800
+#define	BBANDGAP_MBIAS_POWERUP      0x10000
+#define	BAD11SH_GAIN			0xc0000
+#define	BAD11NPUT_RANGE			0x700000
+#define	BAD110P_CURRENT			0x3800000
+#define	BLPATH_LOOPBACK			0x4000000
+#define	BQPATH_LOOPBACK			0x8000000
+#define	BAFE_LOOPBACK			0x10000000
+#define	BDA10_SWING			0x7e0
+#define	BDA10_REVERSE			0x800
+#define	BDA_CLK_SOURCE              0x1000
+#define	BDA7INPUT_RANGE			0x6000
+#define	BDA7_GAIN			0x38000
+#define	BDA7OUTPUT_CM_MODE          0x40000
+#define	BDA7INPUT_CM_MODE           0x380000
+#define	BDA7CURRENT			0xc00000
+#define	BREGULATOR_ADJUST		0x7000000
+#define	BAD11POWERUP_ATTX		0x1
+#define	BDA10PS_ATTX			0x10
+#define	BAD11POWERUP_ATRX		0x100
+#define	BDA10PS_ATRX			0x1000
+#define	BCCKRX_AGC_FORMAT           0x200
+#define	BPSDFFT_SAMPLE_POINT		0xc000
+#define	BPSD_AVERAGE_NUM            0x3000
+#define	BIQPATH_CONTROL			0xc00
+#define	BPSD_FREQ			0x3ff
+#define	BPSD_ANTENNA_PATH           0x30
+#define	BPSD_IQ_SWITCH              0x40
+#define	BPSD_RX_TRIGGER             0x400000
+#define	BPSD_TX_TRIGGER             0x80000000
+#define	BPSD_SINE_TONE_SCALE        0x7f000000
+#define	BPSD_REPORT			0xffff
+
+#define	BOFDM_TXSC			0x30000000
+#define	BCCK_TXON			0x1
+#define	BOFDM_TXON			0x2
+#define	BDEBUG_PAGE			0xfff
+#define	BDEBUG_ITEM			0xff
+#define	BANTL				0x10
+#define	BANT_NONHT		    0x100
+#define	BANT_HT1			0x1000
+#define	BANT_HT2			0x10000
+#define	BANT_HT1S1			0x100000
+#define	BANT_NONHTS1			0x1000000
+
+#define	BCCK_BBMODE			0x3
+#define	BCCK_TXPOWERSAVING		0x80
+#define	BCCK_RXPOWERSAVING		0x40
+
+#define	BCCK_SIDEBAND			0x10
+
+#define	BCCK_SCRAMBLE			0x8
+#define	BCCK_ANTDIVERSITY		0x8000
+#define	BCCK_CARRIER_RECOVERY		0x4000
+#define	BCCK_TXRATE			0x3000
+#define	BCCK_DCCANCEL			0x0800
+#define	BCCK_ISICANCEL			0x0400
+#define	BCCK_MATCH_FILTER           0x0200
+#define	BCCK_EQUALIZER			0x0100
+#define	BCCK_PREAMBLE_DETECT		0x800000
+#define	BCCK_FAST_FALSECCA          0x400000
+#define	BCCK_CH_ESTSTART            0x300000
+#define	BCCK_CCA_COUNT              0x080000
+#define	BCCK_CS_LIM			0x070000
+#define	BCCK_BIST_MODE              0x80000000
+#define	BCCK_CCAMASK			0x40000000
+#define	BCCK_TX_DAC_PHASE		0x4
+#define	BCCK_RX_ADC_PHASE		0x20000000
+#define	BCCKR_CP_MODE			0x0100
+#define	BCCK_TXDC_OFFSET		0xf0
+#define	BCCK_RXDC_OFFSET		0xf
+#define	BCCK_CCA_MODE			0xc000
+#define	BCCK_FALSECS_LIM		0x3f00
+#define	BCCK_CS_RATIO			0xc00000
+#define	BCCK_CORGBIT_SEL		0x300000
+#define	BCCK_PD_LIM			0x0f0000
+#define	BCCK_NEWCCA			0x80000000
+#define	BCCK_RXHP_OF_IG             0x8000
+#define	BCCK_RXIG			0x7f00
+#define	BCCK_LNA_POLARITY           0x800000
+#define	BCCK_RX1ST_BAIN             0x7f0000
+#define	BCCK_RF_EXTEND              0x20000000
+#define	BCCK_RXAGC_SATLEVEL		0x1f000000
+#define	BCCK_RXAGC_SATCOUNT		0xe0
+#define	BCCKRXRFSETTLE			0x1f
+#define	BCCK_FIXED_RXAGC		0x8000
+#define	BCCK_ANTENNA_POLARITY		0x2000
+#define	BCCK_TXFILTER_TYPE          0x0c00
+#define	BCCK_RXAGC_REPORTTYPE		0x0300
+#define	BCCK_RXDAGC_EN              0x80000000
+#define	BCCK_RXDAGC_PERIOD		0x20000000
+#define	BCCK_RXDAGC_SATLEVEL		0x1f000000
+#define	BCCK_TIMING_RECOVERY		0x800000
+#define	BCCK_TXC0			0x3f0000
+#define	BCCK_TXC1			0x3f000000
+#define	BCCK_TXC2			0x3f
+#define	BCCK_TXC3			0x3f00
+#define	BCCK_TXC4			0x3f0000
+#define	BCCK_TXC5			0x3f000000
+#define	BCCK_TXC6			0x3f
+#define	BCCK_TXC7			0x3f00
+#define	BCCK_DEBUGPORT			0xff0000
+#define	BCCK_DAC_DEBUG              0x0f000000
+#define	BCCK_FALSEALARM_ENABLE      0x8000
+#define	BCCK_FALSEALARM_READ        0x4000
+#define	BCCK_TRSSI			0x7f
+#define	BCCK_RXAGC_REPORT           0xfe
+#define	BCCK_RXREPORT_ANTSEL		0x80000000
+#define	BCCK_RXREPORT_MFOFF		0x40000000
+#define	BCCK_RXREPORT_SQLOSS		0x20000000
+#define	BCCK_RXREPORT_PKTLOSS		0x10000000
+#define	BCCK_RXREPORT_LOCKEDBIT		0x08000000
+#define	BCCK_RXREPORT_RATEERROR		0x04000000
+#define	BCCK_RXREPORT_RXRATE		0x03000000
+#define	BCCK_RXFA_COUNTER_LOWER     0xff
+#define	BCCK_RXFA_COUNTER_UPPER     0xff000000
+#define	BCCK_RXHPAGC_START          0xe000
+#define	BCCK_RXHPAGC_FINAL          0x1c00
+#define	BCCK_RXFALSEALARM_ENABLE    0x8000
+#define	BCCK_FACOUNTER_FREEZE       0x4000
+#define	BCCK_TXPATH_SEL             0x10000000
+#define	BCCK_DEFAULT_RXPATH         0xc000000
+#define	BCCK_OPTION_RXPATH          0x3000000
+
+#define	BNUM_OFSTF			0x3
+#define	BSHIFT_L			0xc0
+#define	BGI_TH				0xc
+#define	BRXPATH_A			0x1
+#define	BRXPATH_B			0x2
+#define	BRXPATH_C			0x4
+#define	BRXPATH_D			0x8
+#define	BTXPATH_A			0x1
+#define	BTXPATH_B			0x2
+#define	BTXPATH_C			0x4
+#define	BTXPATH_D			0x8
+#define	BTRSSI_FREQ			0x200
+#define	BADC_BACKOFF			0x3000
+#define	BDFIR_BACKOFF			0xc000
+#define	BTRSSI_LATCH_PHASE		0x10000
+#define	BRX_LDC_OFFSET			0xff
+#define	BRX_QDC_OFFSET			0xff00
+#define	BRX_DFIR_MODE			0x1800000
+#define	BRX_DCNF_TYPE			0xe000000
+#define	BRXIQIMB_A			0x3ff
+#define	BRXIQIMB_B			0xfc00
+#define	BRXIQIMB_C			0x3f0000
+#define	BRXIQIMB_D			0xffc00000
+#define	BDC_DC_NOTCH			0x60000
+#define	BRXNB_NOTCH			0x1f000000
+#define	BPD_TH				0xf
+#define	BPD_TH_OPT2			0xc000
+#define	BPWED_TH			0x700
+#define	BIFMF_WIN_L			0x800
+#define	BPD_OPTION			0x1000
+#define	BMF_WIN_L			0xe000
+#define	BBW_SEARCH_L			0x30000
+#define	BWIN_ENH_L			0xc0000
+#define	BBW_TH				0x700000
+#define	BED_TH2				0x3800000
+#define	BBW_OPTION			0x4000000
+#define	BRADIO_TH			0x18000000
+#define	BWINDOW_L			0xe0000000
+#define	BSBD_OPTION			0x1
+#define	BFRAME_TH			0x1c
+#define	BFS_OPTION			0x60
+#define	BDC_SLOPE_CHECK			0x80
+#define	BFGUARD_COUNTER_DC_L		0xe00
+#define	BFRAME_WEIGHT_SHORT		0x7000
+#define	BSUB_TUNE			0xe00000
+#define	BFRAME_DC_LENGTH		0xe000000
+#define	BSBD_START_OFFSET		0x30000000
+#define	BFRAME_TH_2			0x7
+#define	BFRAME_GI2_TH			0x38
+#define	BGI2_SYNC_EN			0x40
+#define	BSARCH_SHORT_EARLY		0x300
+#define	BSARCH_SHORT_LATE		0xc00
+#define	BSARCH_GI2_LATE			0x70000
+#define	BCFOANTSUM			0x1
+#define	BCFOACC				0x2
+#define	BCFOSTARTOFFSET			0xc
+#define	BCFOLOOPBACK			0x70
+#define	BCFOSUMWEIGHT			0x80
+#define	BDAGCENABLE			0x10000
+#define	BTXIQIMB_A			0x3ff
+#define	BTXIQIMB_b			0xfc00
+#define	BTXIQIMB_C			0x3f0000
+#define	BTXIQIMB_D			0xffc00000
+#define	BTXIDCOFFSET			0xff
+#define	BTXIQDCOFFSET			0xff00
+#define	BTXDFIRMODE			0x10000
+#define	BTXPESUDO_NOISEON		0x4000000
+#define	BTXPESUDO_NOISE_A		0xff
+#define	BTXPESUDO_NOISE_B		0xff00
+#define	BTXPESUDO_NOISE_C		0xff0000
+#define	BTXPESUDO_NOISE_D		0xff000000
+#define	BCCA_DROPOPTION			0x20000
+#define	BCCA_DROPTHRES			0xfff00000
+#define	BEDCCA_H			0xf
+#define	BEDCCA_L			0xf0
+#define	BLAMBDA_ED			0x300
+#define	BRX_INITIALGAIN			0x7f
+#define	BRX_ANTDIV_EN			0x80
+#define	BRX_AGC_ADDRESS_FOR_LNA     0x7f00
+#define	BRX_HIGHPOWER_FLOW		0x8000
+#define	BRX_AGC_FREEZE_THRES        0xc0000
+#define	BRX_FREEZESTEP_AGC1		0x300000
+#define	BRX_FREEZESTEP_AGC2		0xc00000
+#define	BRX_FREEZESTEP_AGC3		0x3000000
+#define	BRX_FREEZESTEP_AGC0		0xc000000
+#define	BRXRSSI_CMP_EN			0x10000000
+#define	BRXQUICK_AGCEN			0x20000000
+#define	BRXAGC_FREEZE_THRES_MODE    0x40000000
+#define	BRX_OVERFLOW_CHECKTYPE		0x80000000
+#define	BRX_AGCSHIFT			0x7f
+#define	BTRSW_TRI_ONLY			0x80
+#define	BPOWER_THRES			0x300
+#define	BRXAGC_EN			0x1
+#define	BRXAGC_TOGETHER_EN		0x2
+#define	BRXAGC_MIN			0x4
+#define	BRXHP_INI			0x7
+#define	BRXHP_TRLNA			0x70
+#define	BRXHP_RSSI			0x700
+#define	BRXHP_BBP1			0x7000
+#define	BRXHP_BBP2			0x70000
+#define	BRXHP_BBP3			0x700000
+#define	BRSSI_H				0x7f0000
+#define	BRSSI_GEN			0x7f000000
+#define	BRXSETTLE_TRSW			0x7
+#define	BRXSETTLE_LNA			0x38
+#define	BRXSETTLE_RSSI			0x1c0
+#define	BRXSETTLE_BBP			0xe00
+#define	BRXSETTLE_RXHP			0x7000
+#define	BRXSETTLE_ANTSW_RSSI		0x38000
+#define	BRXSETTLE_ANTSW			0xc0000
+#define	BRXPROCESS_TIME_DAGC		0x300000
+#define	BRXSETTLE_HSSI			0x400000
+#define	BRXPROCESS_TIME_BBPPW		0x800000
+#define	BRXANTENNA_POWER_SHIFT		0x3000000
+#define	BRSSI_TABLE_SELECT		0xc000000
+#define	BRXHP_FINAL			0x7000000
+#define	BRXHPSETTLE_BBP			0x7
+#define	BRXHTSETTLE_HSSI		0x8
+#define	BRXHTSETTLE_RXHP		0x70
+#define	BRXHTSETTLE_BBPPW		0x80
+#define	BRXHTSETTLE_IDLE		0x300
+#define	BRXHTSETTLE_RESERVED		0x1c00
+#define	BRXHT_RXHP_EN			0x8000
+#define	BRXAGC_FREEZE_THRES		0x30000
+#define	BRXAGC_TOGETHEREN		0x40000
+#define	BRXHTAGC_MIN			0x80000
+#define	BRXHTAGC_EN			0x100000
+#define	BRXHTDAGC_EN			0x200000
+#define	BRXHT_RXHP_BBP			0x1c00000
+#define	BRXHT_RXHP_FINAL		0xe0000000
+#define	BRXPW_RADIO_TH			0x3
+#define	BRXPW_RADIO_EN			0x4
+#define	BRXMF_HOLD			0x3800
+#define	BRXPD_DELAY_TH1			0x38
+#define	BRXPD_DELAY_TH2			0x1c0
+#define	BRXPD_DC_COUNT_MAX		0x600
+#define	BRXPD_DELAY_TH			0x8000
+#define	BRXPROCESS_DELAY		0xf0000
+#define	BRXSEARCHRANGE_GI2_EARLY	0x700000
+#define	BRXFRAME_FUARD_COUNTER_L	0x3800000
+#define	BRXSGI_GUARD_L			0xc000000
+#define	BRXSGI_SEARCH_L			0x30000000
+#define	BRXSGI_TH			0xc0000000
+#define	BDFSCNT0			0xff
+#define	BDFSCNT1			0xff00
+#define	BDFSFLAG			0xf0000
+#define	BMF_WEIGHT_SUM			0x300000
+#define	BMINIDX_TH			0x7f000000
+#define	BDAFORMAT			0x40000
+#define	BTXCH_EMU_ENABLE		0x01000000
+#define	BTRSW_ISOLATION_A		0x7f
+#define	BTRSW_ISOLATION_B		0x7f00
+#define	BTRSW_ISOLATION_C		0x7f0000
+#define	BTRSW_ISOLATION_D		0x7f000000
+#define	BEXT_LNA_GAIN			0x7c00
+
+#define	BSTBC_EN			0x4
+#define	BANTENNA_MAPPING		0x10
+#define	BNSS				0x20
+#define	BCFO_ANTSUM_ID              0x200
+#define	BPHY_COUNTER_RESET		0x8000000
+#define	BCFO_REPORT_GET			0x4000000
+#define	BOFDM_CONTINUE_TX		0x10000000
+#define	BOFDM_SINGLE_CARRIER		0x20000000
+#define	BOFDM_SINGLE_TONE		0x40000000
+#define	BHT_DETECT			0x100
+#define	BCFOEN				0x10000
+#define	BCFOVALUE			0xfff00000
+#define	BSIGTONE_RE			0x3f
+#define	BSIGTONE_IM			0x7f00
+#define	BCOUNTER_CCA			0xffff
+#define	BCOUNTER_PARITYFAIL		0xffff0000
+#define	BCOUNTER_RATEILLEGAL		0xffff
+#define	BCOUNTER_CRC8FAIL		0xffff0000
+#define	BCOUNTER_MCSNOSUPPORT		0xffff
+#define	BCOUNTER_FASTSYNC		0xffff
+#define	BSHORTCFO			0xfff
+#define	BSHORTCFOT_LENGTH		12
+#define	BSHORTCFOF_LENGTH		11
+#define	BLONGCFO			0x7ff
+#define	BLONGCFOT_LENGTH		11
+#define	BLONGCFOF_LENGTH		11
+#define	BTAILCFO			0x1fff
+#define	BTAILCFOT_LENGTH		13
+#define	BTAILCFOF_LENGTH		12
+#define	BNOISE_EN_PWDB			0xffff
+#define	BCC_POWER_DB			0xffff0000
+#define	BMOISE_PWDB			0xffff
+#define	BPOWERMEAST_LENGTH		10
+#define	BPOWERMEASF_LENGTH		3
+#define	BRX_HT_BW			0x1
+#define	BRXSC				0x6
+#define	BRX_HT				0x8
+#define	BNB_INTF_DET_ON			0x1
+#define	BINTF_WIN_LEN_CFG		0x30
+#define	BNB_INTF_TH_CFG			0x1c0
+#define	BRFGAIN				0x3f
+#define	BTABLESEL			0x40
+#define	BTRSW				0x80
+#define	BRXSNR_A			0xff
+#define	BRXSNR_B			0xff00
+#define	BRXSNR_C			0xff0000
+#define	BRXSNR_D			0xff000000
+#define	BSNR_EVMT_LENGTH		8
+#define	BSNR_EVMF_LENGTH		1
+#define	BCSI1ST				0xff
+#define	BCSI2ND				0xff00
+#define	BRXEVM1ST			0xff0000
+#define	BRXEVM2ND			0xff000000
+#define	BSIGEVM				0xff
+#define	BPWDB				0xff00
+#define	BSGIEN				0x10000
+
+#define	BSFACTOR_QMA1			0xf
+#define	BSFACTOR_QMA2			0xf0
+#define	BSFACTOR_QMA3			0xf00
+#define	BSFACTOR_QMA4			0xf000
+#define	BSFACTOR_QMA5			0xf0000
+#define	BSFACTOR_QMA6			0xf0000
+#define	BSFACTOR_QMA7			0xf00000
+#define	BSFACTOR_QMA8			0xf000000
+#define	BSFACTOR_QMA9			0xf0000000
+#define	BCSI_SCHEME			0x100000
+
+#define	BNOISE_LVL_TOP_SET          0x3
+#define	BCHSMOOTH			0x4
+#define	BCHSMOOTH_CFG1			0x38
+#define	BCHSMOOTH_CFG2			0x1c0
+#define	BCHSMOOTH_CFG3			0xe00
+#define	BCHSMOOTH_CFG4			0x7000
+#define	BMRCMODE			0x800000
+#define	BTHEVMCFG			0x7000000
+
+#define	BLOOP_FIT_TYPE			0x1
+#define	BUPD_CFO			0x40
+#define	BUPD_CFO_OFFDATA		0x80
+#define	BADV_UPD_CFO			0x100
+#define	BADV_TIME_CTRL			0x800
+#define	BUPD_CLKO			0x1000
+#define	BFC				0x6000
+#define	BTRACKING_MODE			0x8000
+#define	BPHCMP_ENABLE			0x10000
+#define	BUPD_CLKO_LTF			0x20000
+#define	BCOM_CH_CFO			0x40000
+#define	BCSI_ESTI_MODE			0x80000
+#define	BADV_UPD_EQZ			0x100000
+#define	BUCHCFG				0x7000000
+#define	BUPDEQZ				0x8000000
+
+#define	BRX_PESUDO_NOISE_ON         0x20000000
+#define	BRX_PESUDO_NOISE_A		0xff
+#define	BRX_PESUDO_NOISE_B		0xff00
+#define	BRX_PESUDO_NOISE_C		0xff0000
+#define	BRX_PESUDO_NOISE_D		0xff000000
+#define	BRX_PESUDO_NOISESTATE_A     0xffff
+#define	BRX_PESUDO_NOISESTATE_B     0xffff0000
+#define	BRX_PESUDO_NOISESTATE_C     0xffff
+#define	BRX_PESUDO_NOISESTATE_D     0xffff0000
+
+#define	BZEBRA1_HSSIENABLE		0x8
+#define	BZEBRA1_TRXCONTROL		0xc00
+#define	BZEBRA1_TRXGAINSETTING		0x07f
+#define	BZEBRA1_RXCOUNTER		0xc00
+#define	BZEBRA1_TXCHANGEPUMP		0x38
+#define	BZEBRA1_RXCHANGEPUMP		0x7
+#define	BZEBRA1_CHANNEL_NUM		0xf80
+#define	BZEBRA1_TXLPFBW			0x400
+#define	BZEBRA1_RXLPFBW			0x600
+
+#define	BRTL8256REG_MODE_CTRL1      0x100
+#define	BRTL8256REG_MODE_CTRL0      0x40
+#define	BRTL8256REG_TXLPFBW         0x18
+#define	BRTL8256REG_RXLPFBW         0x600
+
+#define	BRTL8258_TXLPFBW		0xc
+#define	BRTL8258_RXLPFBW		0xc00
+#define	BRTL8258_RSSILPFBW		0xc0
+
+#define	BBYTE0				0x1
+#define	BBYTE1				0x2
+#define	BBYTE2				0x4
+#define	BBYTE3				0x8
+#define	BWORD0				0x3
+#define	BWORD1				0xc
+#define	BWORD				0xf
+
+#define	MASKBYTE0			0xff
+#define	MASKBYTE1			0xff00
+#define	MASKBYTE2			0xff0000
+#define	MASKBYTE3			0xff000000
+#define	MASKHWORD			0xffff0000
+#define	MASKLWORD			0x0000ffff
+#define	MASKDWORD					0xffffffff
+#define	MASK12BITS					0xfff
+#define	MASKH4BITS					0xf0000000
+#define MASKOFDM_D					0xffc00000
+#define	MASKCCK						0x3f3f3f3f
+
+#define	MASK4BITS			0x0f
+#define	MASK20BITS			0xfffff
+#define RFREG_OFFSET_MASK			0xfffff
+
+#define	BENABLE				0x1
+#define	BDISABLE			0x0
+
+#define	LEFT_ANTENNA			0x0
+#define	RIGHT_ANTENNA			0x1
+
+#define	TCHECK_TXSTATUS			500
+#define	TUPDATE_RXCOUNTER		100
+
+#define	REG_UN_used_register		0x01bf
 
 /* WOL bit information */
 #define	HAL92C_WOL_PTK_UPDATE_EVENT		BIT(0)
 #define	HAL92C_WOL_GTK_UPDATE_EVENT		BIT(1)
 #define	HAL92C_WOL_DISASSOC_EVENT		BIT(2)
 #define	HAL92C_WOL_DEAUTH_EVENT			BIT(3)
-#define	HAL92C_WOL_FW_DISCONNECT_EVENT		BIT(4)
+#define	HAL92C_WOL_FW_DISCONNECT_EVENT	BIT(4)
 
 #define		WOL_REASON_PTK_UPDATE		BIT(0)
 #define		WOL_REASON_GTK_UPDATE		BIT(1)
-#define		WOL_REASON_DISASSOC		BIT(2)
-#define		WOL_REASON_DEAUTH		BIT(3)
+#define		WOL_REASON_DISASSOC			BIT(2)
+#define		WOL_REASON_DEAUTH			BIT(3)
 #define		WOL_REASON_FW_DISCONNECT	BIT(4)
-
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.c b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.c
index 4faafdbab9c6..40893cef7dfe 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -34,6 +30,8 @@
 #include "rf.h"
 #include "dm.h"
 
+static bool _rtl88e_phy_rf6052_config_parafile(struct ieee80211_hw *hw);
+
 void rtl88e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -60,7 +58,7 @@ void rtl88e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 }
 
 void rtl88e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
-				       u8 *plevel)
+				       u8 *ppowerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -82,32 +80,36 @@ void rtl88e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 
 		if (turbo_scanoff) {
 			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
-				tx_agc[idx1] = plevel[idx1] |
-					       (plevel[idx1] << 8) |
-					       (plevel[idx1] << 16) |
-					       (plevel[idx1] << 24);
+				tx_agc[idx1] = ppowerlevel[idx1] |
+				    (ppowerlevel[idx1] << 8) |
+				    (ppowerlevel[idx1] << 16) |
+				    (ppowerlevel[idx1] << 24);
 			}
 		}
 	} else {
 		for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
-			tx_agc[idx1] = plevel[idx1] | (plevel[idx1] << 8) |
-				       (plevel[idx1] << 16) |
-				       (plevel[idx1] << 24);
+			tx_agc[idx1] = ppowerlevel[idx1] |
+			    (ppowerlevel[idx1] << 8) |
+			    (ppowerlevel[idx1] << 16) |
+			    (ppowerlevel[idx1] << 24);
 		}
 
 		if (rtlefuse->eeprom_regulatory == 0) {
-			tmpval = (rtlphy->mcs_offset[0][6]) +
-				 (rtlphy->mcs_offset[0][7] << 8);
+			tmpval =
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][6]) +
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][7] <<
+			     8);
 			tx_agc[RF90_PATH_A] += tmpval;
 
-			tmpval = (rtlphy->mcs_offset[0][14]) +
-				 (rtlphy->mcs_offset[0][15] << 24);
+			tmpval = (rtlphy->mcs_txpwrlevel_origoffset[0][14]) +
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][15] <<
+			     24);
 			tx_agc[RF90_PATH_B] += tmpval;
 		}
 	}
 
 	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
-		ptr = (u8 *)(&(tx_agc[idx1]));
+		ptr = (u8 *)(&tx_agc[idx1]);
 		for (idx2 = 0; idx2 < 4; idx2++) {
 			if (*ptr > RF6052_MAX_TX_PWR)
 				*ptr = RF6052_MAX_TX_PWR;
@@ -127,10 +129,12 @@ void rtl88e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 		"CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
-		RTXAGC_A_CCK1_MCS32);
+		 RTXAGC_A_CCK1_MCS32);
 
 	tmpval = tx_agc[RF90_PATH_A] >> 8;
 
+	/*tmpval = tmpval & 0xff00ffff;*/
+
 	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
@@ -153,148 +157,180 @@ void rtl88e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 }
 
 static void rtl88e_phy_get_power_base(struct ieee80211_hw *hw,
-				      u8 *pwrlvlofdm, u8 *pwrlvlbw20,
-				      u8 *pwrlvlbw40, u8 channel,
+				      u8 *ppowerlevel_ofdm,
+				      u8 *ppowerlevel_bw20,
+				      u8 *ppowerlevel_bw40, u8 channel,
 				      u32 *ofdmbase, u32 *mcsbase)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 base0, base1;
+	u32 powerbase0, powerbase1;
 	u8 i, powerlevel[2];
 
 	for (i = 0; i < 2; i++) {
-		base0 = pwrlvlofdm[i];
+		powerbase0 = ppowerlevel_ofdm[i];
 
-		base0 = (base0 << 24) | (base0 << 16) |
-			     (base0 << 8) | base0;
-		*(ofdmbase + i) = base0;
+		powerbase0 = (powerbase0 << 24) | (powerbase0 << 16) |
+		    (powerbase0 << 8) | powerbase0;
+		*(ofdmbase + i) = powerbase0;
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-			"[OFDM power base index rf(%c) = 0x%x]\n",
-			((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
+			" [OFDM power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
 	}
 
 	for (i = 0; i < 2; i++) {
 		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20)
-			powerlevel[i] = pwrlvlbw20[i];
+			powerlevel[i] = ppowerlevel_bw20[i];
 		else
-			powerlevel[i] = pwrlvlbw40[i];
-		base1 = powerlevel[i];
-		base1 = (base1 << 24) |
-		    (base1 << 16) | (base1 << 8) | base1;
+			powerlevel[i] = ppowerlevel_bw40[i];
 
-		*(mcsbase + i) = base1;
+		powerbase1 = powerlevel[i];
+		powerbase1 = (powerbase1 << 24) |
+		    (powerbase1 << 16) | (powerbase1 << 8) | powerbase1;
+
+		*(mcsbase + i) = powerbase1;
 
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-			"[MCS power base index rf(%c) = 0x%x]\n",
-			((i == 0) ? 'A' : 'B'), *(mcsbase + i));
+			" [MCS power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(mcsbase + i));
 	}
 }
 
-static void get_txpwr_by_reg(struct ieee80211_hw *hw, u8 chan, u8 index,
-			     u32 *base0, u32 *base1, u32 *outval)
+static void _rtl88e_get_txpower_writeval_by_regulatory(struct ieee80211_hw *hw,
+						       u8 channel, u8 index,
+						       u32 *powerbase0,
+						       u32 *powerbase1,
+						       u32 *p_outwriteval)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 i, chg = 0, pwr_lim[4], pwr_diff = 0, cust_pwr_dif;
-	u32 writeval, cust_lim, rf, tmp;
-	u8 ch = chan - 1;
-	u8 j;
+	u8 i, chnlgroup = 0, pwr_diff_limit[4], pwr_diff = 0, customer_pwr_diff;
+	u32 writeval, customer_limit, rf;
 
 	for (rf = 0; rf < 2; rf++) {
-		j = index + (rf ? 8 : 0);
-		tmp = ((index < 2) ? base0[rf] : base1[rf]);
 		switch (rtlefuse->eeprom_regulatory) {
 		case 0:
-			chg = 0;
+			chnlgroup = 0;
 
-			writeval = rtlphy->mcs_offset[chg][j] + tmp;
+			writeval =
+			    rtlphy->mcs_txpwrlevel_origoffset
+				[chnlgroup][index + (rf ? 8 : 0)]
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				"RTK better performance, "
-				"writeval(%c) = 0x%x\n",
+				"RTK better performance, writeval(%c) = 0x%x\n",
 				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 1:
 			if (rtlphy->pwrgroup_cnt == 1) {
-				chg = 0;
+				chnlgroup = 0;
 			} else {
-				chg = chan / 3;
-				if (chan == 14)
-					chg = 5;
+				if (channel < 3)
+					chnlgroup = 0;
+				else if (channel < 6)
+					chnlgroup = 1;
+				else if (channel < 9)
+					chnlgroup = 2;
+				else if (channel < 12)
+					chnlgroup = 3;
+				else if (channel < 14)
+					chnlgroup = 4;
+				else if (channel == 14)
+					chnlgroup = 5;
 			}
-			writeval = rtlphy->mcs_offset[chg][j] + tmp;
+
+			writeval =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
+			    [index + (rf ? 8 : 0)] + ((index < 2) ?
+						      powerbase0[rf] :
+						      powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 				"Realtek regulatory, 20MHz, writeval(%c) = 0x%x\n",
 				 ((rf == 0) ? 'A' : 'B'), writeval);
+
 			break;
 		case 2:
-			writeval = ((index < 2) ? base0[rf] : base1[rf]);
+			writeval =
+			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 				"Better regulatory, writeval(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeval);
+				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 3:
-			chg = 0;
+			chnlgroup = 0;
 
 			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 					"customer's limit, 40MHz rf(%c) = 0x%x\n",
-					 ((rf == 0) ? 'A' : 'B'),
-					 rtlefuse->pwrgroup_ht40[rf][ch]);
+					((rf == 0) ? 'A' : 'B'),
+					rtlefuse->pwrgroup_ht40[rf][channel -
+								    1]);
 			} else {
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 					"customer's limit, 20MHz rf(%c) = 0x%x\n",
-					 ((rf == 0) ? 'A' : 'B'),
-					 rtlefuse->pwrgroup_ht20[rf][ch]);
+					((rf == 0) ? 'A' : 'B'),
+					rtlefuse->pwrgroup_ht20[rf][channel -
+								    1]);
 			}
 
 			if (index < 2)
-				pwr_diff = rtlefuse->txpwr_legacyhtdiff[rf][ch];
+				pwr_diff =
+				   rtlefuse->txpwr_legacyhtdiff[rf][channel-1];
 			else if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20)
-				pwr_diff = rtlefuse->txpwr_ht20diff[rf][ch];
+				pwr_diff =
+					rtlefuse->txpwr_ht20diff[rf][channel-1];
 
 			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
-				cust_pwr_dif = rtlefuse->pwrgroup_ht40[rf][ch];
+				customer_pwr_diff =
+					rtlefuse->pwrgroup_ht40[rf][channel-1];
 			else
-				cust_pwr_dif = rtlefuse->pwrgroup_ht20[rf][ch];
+				customer_pwr_diff =
+					rtlefuse->pwrgroup_ht20[rf][channel-1];
 
-			if (pwr_diff > cust_pwr_dif)
+			if (pwr_diff > customer_pwr_diff)
 				pwr_diff = 0;
 			else
-				pwr_diff = cust_pwr_dif - pwr_diff;
+				pwr_diff = customer_pwr_diff - pwr_diff;
 
 			for (i = 0; i < 4; i++) {
-				pwr_lim[i] = (u8)((rtlphy->mcs_offset[chg][j] &
-					     (0x7f << (i * 8))) >> (i * 8));
-
-				if (pwr_lim[i] > pwr_diff)
-					pwr_lim[i] = pwr_diff;
+				pwr_diff_limit[i] =
+				    (u8)((rtlphy->mcs_txpwrlevel_origoffset
+					  [chnlgroup][index +
+					  (rf ? 8 : 0)] & (0x7f <<
+					  (i * 8))) >> (i * 8));
+
+				if (pwr_diff_limit[i] > pwr_diff)
+					pwr_diff_limit[i] = pwr_diff;
 			}
 
-			cust_lim = (pwr_lim[3] << 24) | (pwr_lim[2] << 16) |
-				   (pwr_lim[1] << 8) | (pwr_lim[0]);
+			customer_limit = (pwr_diff_limit[3] << 24) |
+			    (pwr_diff_limit[2] << 16) |
+			    (pwr_diff_limit[1] << 8) | (pwr_diff_limit[0]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 				"Customer's limit rf(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), cust_lim);
+				 ((rf == 0) ? 'A' : 'B'), customer_limit);
 
-			writeval = cust_lim + tmp;
+			writeval = customer_limit +
+			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				"Customer, writeval rf(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), writeval);
+				"Customer, writeval rf(%c)= 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		default:
-			chg = 0;
-			writeval = rtlphy->mcs_offset[chg][j] + tmp;
+			chnlgroup = 0;
+			writeval =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
+			    [index + (rf ? 8 : 0)]
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				"RTK better performance, writeval "
-				"rf(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), writeval);
+				"RTK better performance, writeval rf(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		}
 
@@ -302,12 +338,13 @@ static void get_txpwr_by_reg(struct ieee80211_hw *hw, u8 chan, u8 index,
 			writeval = writeval - 0x06060606;
 		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
 			 TXHIGHPWRLEVEL_BT2)
-			writeval -= 0x0c0c0c0c;
-		*(outval + rf) = writeval;
+			writeval = writeval - 0x0c0c0c0c;
+		*(p_outwriteval + rf) = writeval;
 	}
 }
 
-static void write_ofdm_pwr(struct ieee80211_hw *hw, u8 index, u32 *pvalue)
+static void _rtl88e_write_ofdm_power_reg(struct ieee80211_hw *hw,
+					 u8 index, u32 *value)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 regoffset_a[6] = {
@@ -325,16 +362,16 @@ static void write_ofdm_pwr(struct ieee80211_hw *hw, u8 index, u32 *pvalue)
 	u16 regoffset;
 
 	for (rf = 0; rf < 2; rf++) {
-		writeval = pvalue[rf];
+		writeval = value[rf];
 		for (i = 0; i < 4; i++) {
-			pwr_val[i] = (u8) ((writeval & (0x7f <<
-				     (i * 8))) >> (i * 8));
+			pwr_val[i] = (u8)((writeval & (0x7f <<
+					   (i * 8))) >> (i * 8));
 
 			if (pwr_val[i] > RF6052_MAX_TX_PWR)
 				pwr_val[i] = RF6052_MAX_TX_PWR;
 		}
 		writeval = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
-			   (pwr_val[1] << 8) | pwr_val[0];
+		    (pwr_val[1] << 8) | pwr_val[0];
 
 		if (rf == 0)
 			regoffset = regoffset_a[index];
@@ -348,24 +385,27 @@ static void write_ofdm_pwr(struct ieee80211_hw *hw, u8 index, u32 *pvalue)
 }
 
 void rtl88e_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
-					u8 *pwrlvlofdm,
-					u8 *pwrlvlbw20,
-					u8 *pwrlvlbw40, u8 chan)
+					u8 *ppowerlevel_ofdm,
+					u8 *ppowerlevel_bw20,
+					u8 *ppowerlevel_bw40, u8 channel)
 {
-	u32 writeval[2], base0[2], base1[2];
+	u32 writeval[2], powerbase0[2], powerbase1[2];
 	u8 index;
 	u8 direction;
 	u32 pwrtrac_value;
 
-	rtl88e_phy_get_power_base(hw, pwrlvlofdm, pwrlvlbw20,
-				  pwrlvlbw40, chan, &base0[0],
-				  &base1[0]);
+	rtl88e_phy_get_power_base(hw, ppowerlevel_ofdm,
+				  ppowerlevel_bw20, ppowerlevel_bw40,
+				  channel, &powerbase0[0], &powerbase1[0]);
 
 	rtl88e_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
 
 	for (index = 0; index < 6; index++) {
-		get_txpwr_by_reg(hw, chan, index, &base0[0], &base1[0],
-				 &writeval[0]);
+		_rtl88e_get_txpower_writeval_by_regulatory(hw,
+							   channel, index,
+							   &powerbase0[0],
+							   &powerbase1[0],
+							   &writeval[0]);
 		if (direction == 1) {
 			writeval[0] += pwrtrac_value;
 			writeval[1] += pwrtrac_value;
@@ -373,15 +413,28 @@ void rtl88e_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
 			writeval[0] -= pwrtrac_value;
 			writeval[1] -= pwrtrac_value;
 		}
-		write_ofdm_pwr(hw, index, &writeval[0]);
+		_rtl88e_write_ofdm_power_reg(hw, index, &writeval[0]);
 	}
 }
 
-static bool rf6052_conf_para(struct ieee80211_hw *hw)
+bool rtl88e_phy_rf6052_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 u4val = 0;
+
+	if (rtlphy->rf_type == RF_1T1R)
+		rtlphy->num_total_rfpath = 1;
+	else
+		rtlphy->num_total_rfpath = 2;
+
+	return _rtl88e_phy_rf6052_config_parafile(hw);
+}
+
+static bool _rtl88e_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u32 u4_regvalue = 0;
 	u8 rfpath;
 	bool rtstatus = true;
 	struct bb_reg_def *pphyreg;
@@ -392,12 +445,12 @@ static bool rf6052_conf_para(struct ieee80211_hw *hw)
 		switch (rfpath) {
 		case RF90_PATH_A:
 		case RF90_PATH_C:
-			u4val = rtl_get_bbreg(hw, pphyreg->rfintfs,
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
 						    BRFSI_RFENV);
 			break;
 		case RF90_PATH_B:
 		case RF90_PATH_D:
-			u4val = rtl_get_bbreg(hw, pphyreg->rfintfs,
+			u4_regvalue = rtl_get_bbreg(hw, pphyreg->rfintfs,
 						    BRFSI_RFENV << 16);
 			break;
 		}
@@ -418,11 +471,11 @@ static bool rf6052_conf_para(struct ieee80211_hw *hw)
 		switch (rfpath) {
 		case RF90_PATH_A:
 			rtstatus = rtl88e_phy_config_rf_with_headerfile(hw,
-					(enum radio_path)rfpath);
+						(enum radio_path)rfpath);
 			break;
 		case RF90_PATH_B:
 			rtstatus = rtl88e_phy_config_rf_with_headerfile(hw,
-					(enum radio_path)rfpath);
+						(enum radio_path)rfpath);
 			break;
 		case RF90_PATH_C:
 			break;
@@ -433,12 +486,13 @@ static bool rf6052_conf_para(struct ieee80211_hw *hw)
 		switch (rfpath) {
 		case RF90_PATH_A:
 		case RF90_PATH_C:
-			rtl_set_bbreg(hw, pphyreg->rfintfs, BRFSI_RFENV, u4val);
+			rtl_set_bbreg(hw, pphyreg->rfintfs,
+				      BRFSI_RFENV, u4_regvalue);
 			break;
 		case RF90_PATH_B:
 		case RF90_PATH_D:
-			rtl_set_bbreg(hw, pphyreg->rfintfs, BRFSI_RFENV << 16,
-				      u4val);
+			rtl_set_bbreg(hw, pphyreg->rfintfs,
+				      BRFSI_RFENV << 16, u4_regvalue);
 			break;
 		}
 
@@ -447,21 +501,9 @@ static bool rf6052_conf_para(struct ieee80211_hw *hw)
 				 "Radio[%d] Fail!!", rfpath);
 			return false;
 		}
+
 	}
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "\n");
 	return rtstatus;
 }
-
-bool rtl88e_phy_rf6052_config(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (rtlphy->rf_type == RF_1T1R)
-		rtlphy->num_total_rfpath = 1;
-	else
-		rtlphy->num_total_rfpath = 2;
-
-	return rf6052_conf_para(hw);
-}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
index a39a2a3dbcc9..5c1472d88fd4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/rf.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -40,7 +36,8 @@ void rtl88e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 void rtl88e_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
 					u8 *ppowerlevel_ofdm,
 					u8 *ppowerlevel_bw20,
-					u8 *ppowerlevel_bw40, u8 channel);
+					u8 *ppowerlevel_bw40,
+					u8 channel);
 bool rtl88e_phy_rf6052_config(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/sw.c b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.c
index 631b6907c17d..11344121c55e 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,7 +26,6 @@
 #include "../wifi.h"
 #include "../core.h"
 #include "../pci.h"
-#include "../base.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -122,7 +117,7 @@ int rtl88e_init_sw_vars(struct ieee80211_hw *hw)
 				  0);
 
 	rtlpci->irq_mask[0] =
-				(u32) (IMR_PSTIMEOUT	|
+				(u32)(IMR_PSTIMEOUT	|
 				IMR_HSISR_IND_ON_INT	|
 				IMR_C2HCMD		|
 				IMR_HIGHDOK		|
@@ -143,6 +138,8 @@ int rtl88e_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	if (rtlpriv->cfg->mod_params->disable_watchdog)
+		pr_info("watchdog disabled\n");
 	if (!rtlpriv->psc.inactiveps)
 		pr_info("rtl8188ee: Power Save off (module option)\n");
 	if (!rtlpriv->psc.fwctrl_lps)
@@ -162,7 +159,7 @@ int rtl88e_init_sw_vars(struct ieee80211_hw *hw)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
 
 	/* for firmware buf */
-	rtlpriv->rtlhal.pfirmware = vmalloc(0x8000);
+	rtlpriv->rtlhal.pfirmware = vzalloc(0x8000);
 	if (!rtlpriv->rtlhal.pfirmware) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Can't alloc buffer for fw.\n");
@@ -199,7 +196,7 @@ int rtl88e_init_sw_vars(struct ieee80211_hw *hw)
 	init_timer(&rtlpriv->works.fast_antenna_training_timer);
 	setup_timer(&rtlpriv->works.fast_antenna_training_timer,
 		    rtl88e_dm_fast_antenna_training_callback,
-		    (unsigned long)hw);
+			(unsigned long)hw);
 	return err;
 }
 
@@ -218,6 +215,12 @@ void rtl88e_deinit_sw_vars(struct ieee80211_hw *hw)
 	del_timer_sync(&rtlpriv->works.fast_antenna_training_timer);
 }
 
+/* get bt coexist status */
+bool rtl88e_get_btc_status(void)
+{
+	return false;
+}
+
 static struct rtl_hal_ops rtl8188ee_hal_ops = {
 	.init_sw_vars = rtl88e_init_sw_vars,
 	.deinit_sw_vars = rtl88e_deinit_sw_vars,
@@ -246,11 +249,12 @@ static struct rtl_hal_ops rtl8188ee_hal_ops = {
 	.set_bw_mode = rtl88e_phy_set_bw_mode,
 	.switch_channel = rtl88e_phy_sw_chnl,
 	.dm_watchdog = rtl88e_dm_watchdog,
-	.scan_operation_backup = rtl_phy_scan_operation_backup,
+	.scan_operation_backup = rtl88e_phy_scan_operation_backup,
 	.set_rf_power_state = rtl88e_phy_set_rf_power_state,
 	.led_control = rtl88ee_led_control,
 	.set_desc = rtl88ee_set_desc,
 	.get_desc = rtl88ee_get_desc,
+	.is_tx_desc_closed = rtl88ee_is_tx_desc_closed,
 	.tx_polling = rtl88ee_tx_polling,
 	.enable_hw_sec = rtl88ee_enable_hw_security_config,
 	.set_key = rtl88ee_set_key,
@@ -259,14 +263,17 @@ static struct rtl_hal_ops rtl8188ee_hal_ops = {
 	.set_bbreg = rtl88e_phy_set_bb_reg,
 	.get_rfreg = rtl88e_phy_query_rf_reg,
 	.set_rfreg = rtl88e_phy_set_rf_reg,
+	.get_btc_status = rtl88e_get_btc_status,
+	.rx_command_packet = rtl88ee_rx_command_packet,
+
 };
 
 static struct rtl_mod_params rtl88ee_mod_params = {
 	.sw_crypto = false,
-	.inactiveps = true,
+	.inactiveps = false,
 	.swctrl_lps = false,
-	.fwctrl_lps = true,
-	.msi_support = false,
+	.fwctrl_lps = false,
+	.msi_support = true,
 	.debug = DBG_EMERG,
 };
 
@@ -274,6 +281,7 @@ static struct rtl_hal_cfg rtl88ee_hal_cfg = {
 	.bar_id = 2,
 	.write_readback = true,
 	.name = "rtl88e_pci",
+	.fw_name = "rtlwifi/rtl8188efw.bin",
 	.ops = &rtl8188ee_hal_ops,
 	.mod_params = &rtl88ee_mod_params,
 
@@ -285,6 +293,9 @@ static struct rtl_hal_cfg rtl88ee_hal_cfg = {
 	.maps[MAC_RCR_ACRC32] = ACRC32,
 	.maps[MAC_RCR_ACF] = ACF,
 	.maps[MAC_RCR_AAP] = AAP,
+	.maps[MAC_HIMR] = REG_HIMR,
+	.maps[MAC_HIMRE] = REG_HIMRE,
+	.maps[MAC_HSISR] = REG_HSISR,
 
 	.maps[EFUSE_ACCESS] = REG_EFUSE_ACCESS,
 
@@ -345,6 +356,7 @@ static struct rtl_hal_cfg rtl88ee_hal_cfg = {
 	.maps[RTL_IMR_VIDOK] = IMR_VIDOK,
 	.maps[RTL_IMR_VODOK] = IMR_VODOK,
 	.maps[RTL_IMR_ROK] = IMR_ROK,
+	.maps[RTL_IMR_HSISR_IND] = IMR_HSISR_IND_ON_INT,
 	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNDMAINT0 | IMR_TBDOK | IMR_TBDER),
 
 	.maps[RTL_RC_CCK_RATE1M] = DESC92C_RATE1M,
@@ -364,7 +376,7 @@ static struct rtl_hal_cfg rtl88ee_hal_cfg = {
 	.maps[RTL_RC_HT_RATEMCS15] = DESC92C_RATEMCS15,
 };
 
-static const struct pci_device_id rtl88ee_pci_ids[] = {
+static struct pci_device_id rtl88ee_pci_ids[] = {
 	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8179, rtl88ee_hal_cfg)},
 	{},
 };
@@ -384,12 +396,15 @@ module_param_named(ips, rtl88ee_mod_params.inactiveps, bool, 0444);
 module_param_named(swlps, rtl88ee_mod_params.swctrl_lps, bool, 0444);
 module_param_named(fwlps, rtl88ee_mod_params.fwctrl_lps, bool, 0444);
 module_param_named(msi, rtl88ee_mod_params.msi_support, bool, 0444);
+module_param_named(disable_watchdog, rtl88ee_mod_params.disable_watchdog,
+		   bool, 0444);
 MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
 MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
 MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
 MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
-MODULE_PARM_DESC(msi, "Set to 1 to use MSI interrupts mode (default 0)\n");
+MODULE_PARM_DESC(msi, "Set to 1 to use MSI interrupts mode (default 1)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+MODULE_PARM_DESC(disable_watchdog, "Set to 1 to disable the watchdog (default 0)\n");
 
 static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/sw.h b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.h
index 85e02b3bdff8..22398c3753a6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/sw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/sw.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -32,5 +28,7 @@
 
 int rtl88e_init_sw_vars(struct ieee80211_hw *hw);
 void rtl88e_deinit_sw_vars(struct ieee80211_hw *hw);
+bool rtl88e_get_btc_status(void);
+
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/table.c b/drivers/net/wireless/rtlwifi/rtl8188ee/table.c
index fad373f97b2c..68bcb7fe6a65 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/table.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/table.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,7 +26,6 @@
  *****************************************************************************/
 
 #include "table.h"
-
 u32 RTL8188EEPHY_REG_1TARRAY[] = {
 		0x800, 0x80040000,
 		0x804, 0x00000003,
@@ -640,4 +635,5 @@ u32 RTL8188EEAGCTAB_1TARRAY[] = {
 		0xC78, 0x407D0001,
 		0xC78, 0x407E0001,
 		0xC78, 0x407F0001,
+
 };
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/table.h b/drivers/net/wireless/rtlwifi/rtl8188ee/table.h
index c1218e835129..403c4ddd236f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/table.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/table.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -35,13 +31,13 @@
 #include <linux/types.h>
 #define  RTL8188EEPHY_REG_1TARRAYLEN	382
 extern u32 RTL8188EEPHY_REG_1TARRAY[];
-#define RTL8188EEPHY_REG_ARRAY_PGLEN	264
+#define RTL8188EEPHY_REG_ARRAY_PGLEN 264
 extern u32 RTL8188EEPHY_REG_ARRAY_PG[];
-#define	RTL8188EE_RADIOA_1TARRAYLEN	190
+#define	RTL8188EE_RADIOA_1TARRAYLEN 190
 extern u32 RTL8188EE_RADIOA_1TARRAY[];
-#define RTL8188EEMAC_1T_ARRAYLEN	180
+#define RTL8188EEMAC_1T_ARRAYLEN  180
 extern u32 RTL8188EEMAC_1T_ARRAY[];
-#define RTL8188EEAGCTAB_1TARRAYLEN	256
+#define RTL8188EEAGCTAB_1TARRAYLEN 256
 extern u32 RTL8188EEAGCTAB_1TARRAY[];
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
index 5b4c225396f2..df549c96adef 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -37,6 +33,7 @@
 #include "trx.h"
 #include "led.h"
 #include "dm.h"
+#include "phy.h"
 
 static u8 _rtl88ee_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 {
@@ -50,6 +47,164 @@ static u8 _rtl88ee_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 	return skb->priority;
 }
 
+/* mac80211's rate_idx is like this:
+ *
+ * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
+ *
+ * B/G rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC92C_RATE1M-->DESC92C_RATE54M ==> idx is 0-->11,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
+ *
+ * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
+ * A rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC92C_RATE6M-->DESC92C_RATE54M ==> idx is 0-->7,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
+ */
+static int _rtl88ee_rate_mapping(struct ieee80211_hw *hw,
+				 bool isht, u8 desc_rate)
+{
+	int rate_idx;
+
+	if (!isht) {
+		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
+			switch (desc_rate) {
+			case DESC92C_RATE1M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE2M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE5_5M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE11M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE6M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 7;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 8;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 9;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 10;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 11;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		} else {
+			switch (desc_rate) {
+			case DESC92C_RATE6M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 7;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		}
+	} else {
+		switch (desc_rate) {
+		case DESC92C_RATEMCS0:
+			rate_idx = 0;
+			break;
+		case DESC92C_RATEMCS1:
+			rate_idx = 1;
+			break;
+		case DESC92C_RATEMCS2:
+			rate_idx = 2;
+			break;
+		case DESC92C_RATEMCS3:
+			rate_idx = 3;
+			break;
+		case DESC92C_RATEMCS4:
+			rate_idx = 4;
+			break;
+		case DESC92C_RATEMCS5:
+			rate_idx = 5;
+			break;
+		case DESC92C_RATEMCS6:
+			rate_idx = 6;
+			break;
+		case DESC92C_RATEMCS7:
+			rate_idx = 7;
+			break;
+		case DESC92C_RATEMCS8:
+			rate_idx = 8;
+			break;
+		case DESC92C_RATEMCS9:
+			rate_idx = 9;
+			break;
+		case DESC92C_RATEMCS10:
+			rate_idx = 10;
+			break;
+		case DESC92C_RATEMCS11:
+			rate_idx = 11;
+			break;
+		case DESC92C_RATEMCS12:
+			rate_idx = 12;
+			break;
+		case DESC92C_RATEMCS13:
+			rate_idx = 13;
+			break;
+		case DESC92C_RATEMCS14:
+			rate_idx = 14;
+			break;
+		case DESC92C_RATEMCS15:
+			rate_idx = 15;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+	}
+	return rate_idx;
+}
+
 static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 			struct rtl_stats *pstatus, u8 *pdesc,
 			struct rx_fwinfo_88e *p_drvinfo,
@@ -59,7 +214,8 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
 	struct phy_sts_cck_8192s_t *cck_buf;
-	struct phy_status_rpt *phystrpt = (struct phy_status_rpt *)p_drvinfo;
+	struct phy_status_rpt *phystrpt =
+		(struct phy_status_rpt *)p_drvinfo;
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
 	char rx_pwr_all = 0, rx_pwr[4];
 	u8 rf_rx_num = 0, evm, pwdb_all;
@@ -72,11 +228,11 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 	pstatus->packet_matchbssid = bpacket_match_bssid;
 	pstatus->packet_toself = bpacket_toself;
 	pstatus->packet_beacon = packet_beacon;
-	pstatus->rx_mimo_sig_qual[0] = -1;
-	pstatus->rx_mimo_sig_qual[1] = -1;
+	pstatus->rx_mimo_signalquality[0] = -1;
+	pstatus->rx_mimo_signalquality[1] = -1;
 
 	if (is_cck) {
-		u8 cck_hipwr;
+		u8 cck_highpwr;
 		u8 cck_agc_rpt;
 		/* CCK Driver info Structure is not the same as OFDM packet. */
 		cck_buf = (struct phy_sts_cck_8192s_t *)p_drvinfo;
@@ -87,53 +243,58 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 		 * hardware (for rate adaptive)
 		 */
 		if (ppsc->rfpwr_state == ERFON)
-			cck_hipwr = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
+			cck_highpwr =
+				(u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
 						  BIT(9));
 		else
-			cck_hipwr = false;
+			cck_highpwr = false;
 
 		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
 		vga_idx = (cck_agc_rpt & 0x1f);
 		switch (lan_idx) {
 		case 7:
 			if (vga_idx <= 27)
-				rx_pwr_all = -100 + 2 * (27 - vga_idx);
+				/*VGA_idx = 27~2*/
+				rx_pwr_all = -100 + 2*(27-vga_idx);
 			else
 				rx_pwr_all = -100;
 			break;
 		case 6:
-			rx_pwr_all = -48 + 2 * (2 - vga_idx); /*VGA_idx = 2~0*/
+			/*VGA_idx = 2~0*/
+			rx_pwr_all = -48 + 2*(2-vga_idx);
 			break;
 		case 5:
-			rx_pwr_all = -42 + 2 * (7 - vga_idx); /*VGA_idx = 7~5*/
+			/*VGA_idx = 7~5*/
+			rx_pwr_all = -42 + 2*(7-vga_idx);
 			break;
 		case 4:
-			rx_pwr_all = -36 + 2 * (7 - vga_idx); /*VGA_idx = 7~4*/
+			/*VGA_idx = 7~4*/
+			rx_pwr_all = -36 + 2*(7-vga_idx);
 			break;
 		case 3:
-			rx_pwr_all = -24 + 2 * (7 - vga_idx); /*VGA_idx = 7~0*/
+			/*VGA_idx = 7~0*/
+			rx_pwr_all = -24 + 2*(7-vga_idx);
 			break;
 		case 2:
-			if (cck_hipwr)
-				rx_pwr_all = -12 + 2 * (5 - vga_idx);
+			if (cck_highpwr)
+				/*VGA_idx = 5~0*/
+				rx_pwr_all = -12 + 2*(5-vga_idx);
 			else
-				rx_pwr_all = -6 + 2 * (5 - vga_idx);
+				rx_pwr_all = -6 + 2*(5-vga_idx);
 			break;
 		case 1:
-			rx_pwr_all = 8 - 2 * vga_idx;
+			rx_pwr_all = 8-2*vga_idx;
 			break;
 		case 0:
-			rx_pwr_all = 14 - 2 * vga_idx;
+			rx_pwr_all = 14-2*vga_idx;
 			break;
 		default:
 			break;
 		}
 		rx_pwr_all += 6;
 		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
-		/* CCK gain is smaller than OFDM/MCS gain,
-		 * so we add gain diff by experiences,
-		 * the val is 6
-		 */
+		/* CCK gain is smaller than OFDM/MCS gain,  */
+		/* so we add gain diff by experiences, the val is 6 */
 		pwdb_all += 6;
 		if (pwdb_all > 100)
 			pwdb_all = 100;
@@ -148,10 +309,10 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 			pwdb_all -= 8;
 		else if (pwdb_all > 4 && pwdb_all <= 14)
 			pwdb_all -= 4;
-		if (cck_hipwr == false) {
+		if (!cck_highpwr) {
 			if (pwdb_all >= 80)
-				pwdb_all = ((pwdb_all - 80)<<1) +
-					   ((pwdb_all - 80)>>1) + 80;
+				pwdb_all = ((pwdb_all-80)<<1) +
+					   ((pwdb_all-80)>>1) + 80;
 			else if ((pwdb_all <= 78) && (pwdb_all >= 20))
 				pwdb_all += 3;
 			if (pwdb_all > 100)
@@ -165,9 +326,9 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 		if (bpacket_match_bssid) {
 			u8 sq;
 
-			if (pstatus->rx_pwdb_all > 40) {
+			if (pstatus->rx_pwdb_all > 40)
 				sq = 100;
-			} else {
+			else {
 				sq = cck_buf->sq_rpt;
 				if (sq > 64)
 					sq = 0;
@@ -178,8 +339,8 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 			}
 
 			pstatus->signalquality = sq;
-			pstatus->rx_mimo_sig_qual[0] = sq;
-			pstatus->rx_mimo_sig_qual[1] = -1;
+			pstatus->rx_mimo_signalquality[0] = sq;
+			pstatus->rx_mimo_signalquality[1] = -1;
 		}
 	} else {
 		rtlpriv->dm.rfpath_rxenable[0] =
@@ -191,18 +352,20 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 			if (rtlpriv->dm.rfpath_rxenable[i])
 				rf_rx_num++;
 
-			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] & 0x3f) * 2)-110;
+			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] &
+				      0x3f) * 2) - 110;
 
 			/* Translate DBM to percentage. */
 			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
 			total_rssi += rssi;
 
 			/* Get Rx snr value in DB */
-			rtlpriv->stats.rx_snr_db[i] = p_drvinfo->rxsnr[i] / 2;
+			rtlpriv->stats.rx_snr_db[i] =
+				(long)(p_drvinfo->rxsnr[i] / 2);
 
 			/* Record Signal Strength for next packet */
 			if (bpacket_match_bssid)
-				pstatus->rx_mimo_signalstrength[i] = (u8) rssi;
+				pstatus->rx_mimo_signalstrength[i] = (u8)rssi;
 		}
 
 		/* (2)PWDB, Average PWDB cacluated by
@@ -227,11 +390,13 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 
 			if (bpacket_match_bssid) {
 				/* Fill value in RFD, Get the first
-				 * spatial stream only
+				 * spatial stream onlyi
 				 */
 				if (i == 0)
-					pstatus->signalquality = evm & 0xff;
-				pstatus->rx_mimo_sig_qual[i] = evm & 0xff;
+					pstatus->signalquality =
+						(u8)(evm & 0xff);
+				pstatus->rx_mimo_signalquality[i] =
+					(u8)(evm & 0xff);
 			}
 		}
 	}
@@ -241,10 +406,10 @@ static void _rtl88ee_query_rxphystatus(struct ieee80211_hw *hw,
 	 */
 	if (is_cck)
 		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
-					  pwdb_all));
+			pwdb_all));
 	else if (rf_rx_num != 0)
 		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
-					  total_rssi /= rf_rx_num));
+			total_rssi /= rf_rx_num));
 	/*HW antenna diversity*/
 	rtldm->fat_table.antsel_rx_keep_0 = phystrpt->ant_sel;
 	rtldm->fat_table.antsel_rx_keep_1 = phystrpt->ant_sel_b;
@@ -256,34 +421,39 @@ static void _rtl88ee_smart_antenna(struct ieee80211_hw *hw,
 {
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 ant_mux;
-	struct fast_ant_training *pfat = &(rtldm->fat_table);
+	u8 antsel_tr_mux;
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
 
 	if (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV) {
-		if (pfat->fat_state == FAT_TRAINING_STATE) {
+		if (pfat_table->fat_state == FAT_TRAINING_STATE) {
 			if (pstatus->packet_toself) {
-				ant_mux = (pfat->antsel_rx_keep_2 << 2) |
-						(pfat->antsel_rx_keep_1 << 1) |
-						 pfat->antsel_rx_keep_0;
-				pfat->ant_sum[ant_mux] += pstatus->rx_pwdb_all;
-				pfat->ant_cnt[ant_mux]++;
+				antsel_tr_mux =
+					(pfat_table->antsel_rx_keep_2 << 2) |
+					(pfat_table->antsel_rx_keep_1 << 1) |
+					pfat_table->antsel_rx_keep_0;
+				pfat_table->ant_sum[antsel_tr_mux] +=
+					pstatus->rx_pwdb_all;
+				pfat_table->ant_cnt[antsel_tr_mux]++;
 			}
 		}
 	} else if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
-		   (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)) {
+	(rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)) {
 		if (pstatus->packet_toself || pstatus->packet_matchbssid) {
-			ant_mux = (pfat->antsel_rx_keep_2 << 2) |
-				  (pfat->antsel_rx_keep_1 << 1) |
-				   pfat->antsel_rx_keep_0;
-			rtl88e_dm_ant_sel_statistics(hw, ant_mux, 0,
+			antsel_tr_mux = (pfat_table->antsel_rx_keep_2 << 2) |
+					(pfat_table->antsel_rx_keep_1 << 1) |
+					pfat_table->antsel_rx_keep_0;
+			rtl88e_dm_ant_sel_statistics(hw, antsel_tr_mux, 0,
 						     pstatus->rx_pwdb_all);
 		}
+
 	}
 }
 
 static void _rtl88ee_translate_rx_signal_stuff(struct ieee80211_hw *hw,
-		struct sk_buff *skb, struct rtl_stats *pstatus,
-		u8 *pdesc, struct rx_fwinfo_88e *p_drvinfo)
+					       struct sk_buff *skb,
+					       struct rtl_stats *pstatus,
+					       u8 *pdesc,
+					       struct rx_fwinfo_88e *p_drvinfo)
 {
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
@@ -292,42 +462,42 @@ static void _rtl88ee_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	u8 *praddr;
 	u8 *psaddr;
 	__le16 fc;
-	u16 type, ufc;
-	bool match_bssid, packet_toself, packet_beacon = false, addr;
+	bool packet_matchbssid, packet_toself, packet_beacon;
 
 	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
 
 	hdr = (struct ieee80211_hdr *)tmp_buf;
 	fc = hdr->frame_control;
-	ufc = le16_to_cpu(fc);
-	type = WLAN_FC_GET_TYPE(fc);
 	praddr = hdr->addr1;
 	psaddr = ieee80211_get_SA(hdr);
 	memcpy(pstatus->psaddr, psaddr, ETH_ALEN);
 
-	addr = ether_addr_equal(mac->bssid,
-				(ufc & IEEE80211_FCTL_TODS) ? hdr->addr1 :
-				(ufc & IEEE80211_FCTL_FROMDS) ? hdr->addr2 :
-				hdr->addr3);
-	match_bssid = ((IEEE80211_FTYPE_CTL != type) && (!pstatus->hwerror) &&
-		       (!pstatus->crc) && (!pstatus->icv)) && addr;
+	packet_matchbssid = ((!ieee80211_is_ctl(fc)) &&
+	     (ether_addr_equal(mac->bssid, ieee80211_has_tods(fc) ?
+			       hdr->addr1 : ieee80211_has_fromds(fc) ?
+			       hdr->addr2 : hdr->addr3)) &&
+			       (!pstatus->hwerror) &&
+			       (!pstatus->crc) && (!pstatus->icv));
 
-	addr = ether_addr_equal(praddr, rtlefuse->dev_addr);
-	packet_toself = match_bssid && addr;
+	packet_toself = packet_matchbssid &&
+	    (ether_addr_equal(praddr, rtlefuse->dev_addr));
 
-	if (ieee80211_is_beacon(fc))
+	if (ieee80211_is_beacon(hdr->frame_control))
 		packet_beacon = true;
+	else
+		packet_beacon = false;
 
 	_rtl88ee_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
-				   match_bssid, packet_toself, packet_beacon);
+				   packet_matchbssid, packet_toself,
+				   packet_beacon);
 	_rtl88ee_smart_antenna(hw, pstatus);
 	rtl_process_phyinfo(hw, tmp_buf, pstatus);
 }
 
-static void insert_em(struct rtl_tcb_desc *ptcb_desc, u8 *virtualaddress)
+static void _rtl88ee_insert_emcontent(struct rtl_tcb_desc *ptcb_desc,
+				      u8 *virtualaddress)
 {
 	u32 dwtmp = 0;
-
 	memset(virtualaddress, 0, 8);
 
 	SET_EARLYMODE_PKTNUM(virtualaddress, ptcb_desc->empkt_num);
@@ -335,7 +505,7 @@ static void insert_em(struct rtl_tcb_desc *ptcb_desc, u8 *virtualaddress)
 		dwtmp = ptcb_desc->empkt_len[0];
 	} else {
 		dwtmp = ptcb_desc->empkt_len[0];
-		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ((dwtmp%4) ? (4-dwtmp%4) : 0)+4;
 		dwtmp += ptcb_desc->empkt_len[1];
 	}
 	SET_EARLYMODE_LEN0(virtualaddress, dwtmp);
@@ -344,7 +514,7 @@ static void insert_em(struct rtl_tcb_desc *ptcb_desc, u8 *virtualaddress)
 		dwtmp = ptcb_desc->empkt_len[2];
 	} else {
 		dwtmp = ptcb_desc->empkt_len[2];
-		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ((dwtmp%4) ? (4-dwtmp%4) : 0)+4;
 		dwtmp += ptcb_desc->empkt_len[3];
 	}
 	SET_EARLYMODE_LEN1(virtualaddress, dwtmp);
@@ -352,7 +522,7 @@ static void insert_em(struct rtl_tcb_desc *ptcb_desc, u8 *virtualaddress)
 		dwtmp = ptcb_desc->empkt_len[4];
 	} else {
 		dwtmp = ptcb_desc->empkt_len[4];
-		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ((dwtmp%4) ? (4-dwtmp%4) : 0)+4;
 		dwtmp += ptcb_desc->empkt_len[5];
 	}
 	SET_EARLYMODE_LEN2_1(virtualaddress, dwtmp & 0xF);
@@ -361,7 +531,7 @@ static void insert_em(struct rtl_tcb_desc *ptcb_desc, u8 *virtualaddress)
 		dwtmp = ptcb_desc->empkt_len[6];
 	} else {
 		dwtmp = ptcb_desc->empkt_len[6];
-		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ((dwtmp%4) ? (4-dwtmp%4) : 0)+4;
 		dwtmp += ptcb_desc->empkt_len[7];
 	}
 	SET_EARLYMODE_LEN3(virtualaddress, dwtmp);
@@ -369,7 +539,7 @@ static void insert_em(struct rtl_tcb_desc *ptcb_desc, u8 *virtualaddress)
 		dwtmp = ptcb_desc->empkt_len[8];
 	} else {
 		dwtmp = ptcb_desc->empkt_len[8];
-		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0) + 4;
+		dwtmp += ((dwtmp%4) ? (4-dwtmp%4) : 0)+4;
 		dwtmp += ptcb_desc->empkt_len[9];
 	}
 	SET_EARLYMODE_LEN4(virtualaddress, dwtmp);
@@ -387,21 +557,21 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
 	status->packet_report_type = (u8)GET_RX_STATUS_DESC_RPT_SEL(pdesc);
 	if (status->packet_report_type == TX_REPORT2)
-		status->length = (u16) GET_RX_RPT2_DESC_PKT_LEN(pdesc);
+		status->length = (u16)GET_RX_RPT2_DESC_PKT_LEN(pdesc);
 	else
-		status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
-	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
-				       RX_DRV_INFO_SIZE_UNIT;
-	status->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
-	status->icv = (u16) GET_RX_DESC_ICV(pdesc);
-	status->crc = (u16) GET_RX_DESC_CRC32(pdesc);
+		status->length = (u16)GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8)GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+	    RX_DRV_INFO_SIZE_UNIT;
+	status->rx_bufshift = (u8)(GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->icv = (u16)GET_RX_DESC_ICV(pdesc);
+	status->crc = (u16)GET_RX_DESC_CRC32(pdesc);
 	status->hwerror = (status->crc | status->icv);
 	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
-	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
-	status->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
+	status->rate = (u8)GET_RX_DESC_RXMCS(pdesc);
+	status->shortpreamble = (u16)GET_RX_DESC_SPLCP(pdesc);
 	status->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	status->isfirst_ampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1) &&
-					(GET_RX_DESC_FAGGR(pdesc) == 1));
+	status->isfirst_ampdu = (bool)((GET_RX_DESC_PAGGR(pdesc) == 1) &&
+				(GET_RX_DESC_FAGGR(pdesc) == 1));
 	if (status->packet_report_type == NORMAL_RX)
 		status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
 	status->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
@@ -420,11 +590,14 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 		status->wake_match = 0;
 	if (status->wake_match)
 		RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD,
-			 "Get Wakeup Packet!! WakeMatch =%d\n",
-			 status->wake_match);
+		"GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",
+		status->wake_match);
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
 
+	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size
+			+ status->rx_bufshift);
+
 	if (status->crc)
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
 
@@ -445,18 +618,11 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 	 * to decrypt it
 	 */
 	if (status->decrypted) {
-		hdr = (struct ieee80211_hdr *)(skb->data +
-		       status->rx_drvinfo_size + status->rx_bufshift);
-
-		if (!hdr) {
-			/* During testing, hdr was NULL */
-			return false;
-		}
-		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
+		if ((!_ieee80211_is_robust_mgmt_frame(hdr)) &&
 		    (ieee80211_has_protected(hdr->frame_control)))
-			rx_status->flag &= ~RX_FLAG_DECRYPTED;
-		else
 			rx_status->flag |= RX_FLAG_DECRYPTED;
+		else
+			rx_status->flag &= ~RX_FLAG_DECRYPTED;
 	}
 
 	/* rate_idx: index of data rate into band's
@@ -464,19 +630,18 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 	 * are use (RX_FLAG_HT)
 	 * Notice: this is diff with windows define
 	 */
-	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
-						   status->rate, false);
+	rx_status->rate_idx = _rtl88ee_rate_mapping(hw,
+				status->is_ht, status->rate);
 
 	rx_status->mactime = status->timestamp_low;
 	if (phystatus == true) {
 		p_drvinfo = (struct rx_fwinfo_88e *)(skb->data +
 						     status->rx_bufshift);
 
-		_rtl88ee_translate_rx_signal_stuff(hw, skb, status, pdesc,
+		_rtl88ee_translate_rx_signal_stuff(hw,
+						   skb, status, pdesc,
 						   p_drvinfo);
 	}
-
-	/*rx_status->qual = status->signal; */
 	rx_status->signal = status->recvsignalpower + 10;
 	if (status->packet_report_type == TX_REPORT2) {
 		status->macid_valid_entry[0] =
@@ -489,15 +654,17 @@ bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 
 void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta, struct sk_buff *skb,
+			  u8 *txbd, struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
 			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
+
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 *pdesc = pdesc_tx;
+	u8 *pdesc = (u8 *)pdesc_tx;
 	u16 seq_number;
 	__le16 fc = hdr->frame_control;
 	unsigned int buf_len = 0;
@@ -547,8 +714,9 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			if (ptcb_desc->empkt_num) {
 				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
 					 "Insert 8 byte.pTcb->EMPktNum:%d\n",
-					 ptcb_desc->empkt_num);
-				insert_em(ptcb_desc, (u8 *)(skb->data));
+					  ptcb_desc->empkt_num);
+				_rtl88ee_insert_emcontent(ptcb_desc,
+							  (u8 *)(skb->data));
 			}
 		} else {
 			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
@@ -560,6 +728,7 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
 		else
 			short_gi = (ptcb_desc->use_shortpreamble) ? 1 : 0;
+
 		SET_TX_DESC_DATA_SHORTGI(pdesc, short_gi);
 
 		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
@@ -568,7 +737,7 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		SET_TX_DESC_SEQ(pdesc, seq_number);
 		SET_TX_DESC_RTS_ENABLE(pdesc, ((ptcb_desc->rts_enable &&
-					      !ptcb_desc->cts_enable) ? 1 : 0));
+						!ptcb_desc->cts_enable) ? 1 : 0));
 		SET_TX_DESC_HW_RTS_ENABLE(pdesc, 0);
 		SET_TX_DESC_CTS2SELF(pdesc, ((ptcb_desc->cts_enable) ? 1 : 0));
 		SET_TX_DESC_RTS_STBC(pdesc, ((ptcb_desc->rts_stbc) ? 1 : 0));
@@ -581,17 +750,17 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			(ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
 			(ptcb_desc->rts_use_shortgi ? 1 : 0)));
 
-		if (ptcb_desc->btx_enable_sw_calc_duration)
+		if (ptcb_desc->tx_enable_sw_calc_duration)
 			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
 
 		if (bw_40) {
-			if (ptcb_desc->packet_bw) {
+			if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40) {
 				SET_TX_DESC_DATA_BW(pdesc, 1);
 				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
 			} else {
 				SET_TX_DESC_DATA_BW(pdesc, 0);
 				SET_TX_DESC_TX_SUB_CARRIER(pdesc,
-						   mac->cur_40_prime_sc);
+							   mac->cur_40_prime_sc);
 			}
 		} else {
 			SET_TX_DESC_DATA_BW(pdesc, 0);
@@ -599,13 +768,14 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 
 		SET_TX_DESC_LINIP(pdesc, 0);
-		SET_TX_DESC_PKT_SIZE(pdesc, (u16) skb_len);
+		SET_TX_DESC_PKT_SIZE(pdesc, (u16)skb_len);
 		if (sta) {
 			u8 ampdu_density = sta->ht_cap.ampdu_density;
 			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
 		}
 		if (info->control.hw_key) {
 			struct ieee80211_key_conf *keyconf;
+
 			keyconf = info->control.hw_key;
 			switch (keyconf->cipher) {
 			case WLAN_CIPHER_SUITE_WEP40:
@@ -619,6 +789,7 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			default:
 				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
 				break;
+
 			}
 		}
 
@@ -629,6 +800,7 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 				       1 : 0);
 		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
 
+		/*SET_TX_DESC_PWR_STATUS(pdesc, pwr_status);*/
 		/* Set TxRate and RTSRate in TxDesc  */
 		/* This prevent Tx initial rate of new-coming packets */
 		/* from being overwritten by retried  packet rate.*/
@@ -639,7 +811,7 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 		if (ieee80211_is_data_qos(fc)) {
 			if (mac->rdg_en) {
 				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
-					 "Enable RDG function.\n");
+					"Enable RDG function.\n");
 				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
 				SET_TX_DESC_HTC(pdesc, 1);
 			}
@@ -648,7 +820,7 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 
 	SET_TX_DESC_FIRST_SEG(pdesc, (firstseg ? 1 : 0));
 	SET_TX_DESC_LAST_SEG(pdesc, (lastseg ? 1 : 0));
-	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) buf_len);
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16)buf_len);
 	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
 	if (rtlpriv->dm.useramask) {
 		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
@@ -664,8 +836,9 @@ void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
 	SET_TX_DESC_MORE_FRAG(pdesc, (lastseg ? 0 : 1));
 	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
-	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
 		SET_TX_DESC_BMC(pdesc, 1);
+	}
 
 	rtl88e_dm_set_tx_ant_by_tx_info(hw, pdesc, ptcb_desc->mac_id);
 	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "\n");
@@ -733,8 +906,8 @@ void rtl88ee_tx_fill_cmddesc(struct ieee80211_hw *hw,
 		      pdesc, TX_DESC_SIZE);
 }
 
-void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-		      u8 desc_name, u8 *val)
+void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc,
+		      bool istx, u8 desc_name, u8 *val)
 {
 	if (istx == true) {
 		switch (desc_name) {
@@ -745,7 +918,7 @@ void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *)val);
 			break;
 		default:
-			RT_ASSERT(false, "ERR txdesc :%d not processed\n",
+			RT_ASSERT(false, "ERR txdesc :%d not process\n",
 				  desc_name);
 			break;
 		}
@@ -764,7 +937,7 @@ void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 			SET_RX_DESC_EOR(pdesc, 1);
 			break;
 		default:
-			RT_ASSERT(false, "ERR rxdesc :%d not processed\n",
+			RT_ASSERT(false, "ERR rxdesc :%d not process\n",
 				  desc_name);
 			break;
 		}
@@ -784,7 +957,7 @@ u32 rtl88ee_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 			ret = GET_TX_DESC_TX_BUFFER_ADDRESS(pdesc);
 			break;
 		default:
-			RT_ASSERT(false, "ERR txdesc :%d not processed\n",
+			RT_ASSERT(false, "ERR txdesc :%d not process\n",
 				  desc_name);
 			break;
 		}
@@ -796,8 +969,11 @@ u32 rtl88ee_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 		case HW_DESC_RXPKT_LEN:
 			ret = GET_RX_DESC_PKT_LEN(pdesc);
 			break;
+		case HW_DESC_RXBUFF_ADDR:
+			ret = GET_RX_DESC_BUFF_ADDR(pdesc);
+			break;
 		default:
-			RT_ASSERT(false, "ERR rxdesc :%d not processed\n",
+			RT_ASSERT(false, "ERR rxdesc :%d not process\n",
 				  desc_name);
 			break;
 		}
@@ -805,6 +981,22 @@ u32 rtl88ee_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 	return ret;
 }
 
+bool rtl88ee_is_tx_desc_closed(struct ieee80211_hw *hw, u8 hw_queue, u16 index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+	u8 *entry = (u8 *)(&ring->desc[ring->idx]);
+	u8 own = (u8)rtl88ee_get_desc(entry, true, HW_DESC_OWN);
+
+	/*beacon packet will only use the first
+	 *descriptor defautly,and the own may not
+	 *be cleared by the hardware
+	 */
+	if (own)
+		return false;
+	return true;
+}
+
 void rtl88ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -815,3 +1007,10 @@ void rtl88ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
 			       BIT(0) << (hw_queue));
 	}
 }
+
+u32 rtl88ee_rx_command_packet(struct ieee80211_hw *hw,
+			      struct rtl_stats status,
+			      struct sk_buff *skb)
+{
+	return 0;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
index 8c2609412d2c..eab5ae0eb46c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8188ee/trx.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,59 +26,59 @@
 #ifndef __RTL92CE_TRX_H__
 #define __RTL92CE_TRX_H__
 
-#define TX_DESC_SIZE				64
+#define TX_DESC_SIZE					64
 #define TX_DESC_AGGR_SUBFRAME_SIZE		32
 
-#define RX_DESC_SIZE				32
+#define RX_DESC_SIZE					32
 #define RX_DRV_INFO_SIZE_UNIT			8
 
 #define	TX_DESC_NEXT_DESC_OFFSET		40
 #define USB_HWDESC_HEADER_LEN			32
-#define CRCLENGTH				4
+#define CRCLENGTH						4
 
 #define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
-#define SET_TX_DESC_OFFSET(__pdesc, __val)		\
+#define SET_TX_DESC_OFFSET(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 8, __val)
-#define SET_TX_DESC_BMC(__pdesc, __val)			\
+#define SET_TX_DESC_BMC(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 24, 1, __val)
-#define SET_TX_DESC_HTC(__pdesc, __val)			\
+#define SET_TX_DESC_HTC(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 25, 1, __val)
 #define SET_TX_DESC_LAST_SEG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 26, 1, __val)
 #define SET_TX_DESC_FIRST_SEG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 27, 1, __val)
-#define SET_TX_DESC_LINIP(__pdesc, __val)		\
+#define SET_TX_DESC_LINIP(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 28, 1, __val)
-#define SET_TX_DESC_NO_ACM(__pdesc, __val)		\
+#define SET_TX_DESC_NO_ACM(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 29, 1, __val)
-#define SET_TX_DESC_GF(__pdesc, __val)			\
+#define SET_TX_DESC_GF(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
-#define SET_TX_DESC_OWN(__pdesc, __val)			\
+#define SET_TX_DESC_OWN(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
 
-#define GET_TX_DESC_PKT_SIZE(__pdesc)			\
+#define GET_TX_DESC_PKT_SIZE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 0, 16)
-#define GET_TX_DESC_OFFSET(__pdesc)			\
+#define GET_TX_DESC_OFFSET(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 16, 8)
-#define GET_TX_DESC_BMC(__pdesc)			\
+#define GET_TX_DESC_BMC(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 24, 1)
-#define GET_TX_DESC_HTC(__pdesc)			\
+#define GET_TX_DESC_HTC(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 25, 1)
-#define GET_TX_DESC_LAST_SEG(__pdesc)			\
+#define GET_TX_DESC_LAST_SEG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
-#define GET_TX_DESC_FIRST_SEG(__pdesc)			\
+#define GET_TX_DESC_FIRST_SEG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
-#define GET_TX_DESC_LINIP(__pdesc)			\
+#define GET_TX_DESC_LINIP(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
-#define GET_TX_DESC_NO_ACM(__pdesc)			\
+#define GET_TX_DESC_NO_ACM(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
-#define GET_TX_DESC_GF(__pdesc)				\
+#define GET_TX_DESC_GF(__pdesc)						\
 	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
-#define GET_TX_DESC_OWN(__pdesc)			\
+#define GET_TX_DESC_OWN(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
 
-#define SET_TX_DESC_MACID(__pdesc, __val)		\
+#define SET_TX_DESC_MACID(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 6, __val)
 #define SET_TX_DESC_QUEUE_SEL(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 8, 5, __val)
@@ -90,11 +86,11 @@
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 13, 1, __val)
 #define SET_TX_DESC_LSIG_TXOP_EN(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 14, 1, __val)
-#define SET_TX_DESC_PIFS(__pdesc, __val)		\
+#define SET_TX_DESC_PIFS(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 15, 1, __val)
 #define SET_TX_DESC_RATE_ID(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 16, 4, __val)
-#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)	\
+#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 20, 1, __val)
 #define SET_TX_DESC_EN_DESC_ID(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 21, 1, __val)
@@ -102,10 +98,10 @@
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 22, 2, __val)
 #define SET_TX_DESC_PKT_OFFSET(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 26, 5, __val)
-#define SET_TX_DESC_PADDING_LEN(__pdesc, __val)		\
+#define SET_TX_DESC_PADDING_LEN(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 24, 8, __val)
 
-#define GET_TX_DESC_MACID(__pdesc)				\
+#define GET_TX_DESC_MACID(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
 #define GET_TX_DESC_AGG_ENABLE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 5, 1)
@@ -119,7 +115,7 @@
 	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
 #define GET_TX_DESC_LSIG_TXOP_EN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
-#define GET_TX_DESC_PIFS(__pdesc)				\
+#define GET_TX_DESC_PIFS(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
 #define GET_TX_DESC_RATE_ID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
@@ -205,7 +201,6 @@
 #define SET_TX_DESC_HWSEQ_EN(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 31, 1, __val)
 
-
 #define GET_TX_DESC_NEXT_HEAP_PAGE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+12, 0, 8)
 #define GET_TX_DESC_TAIL_PAGE(__pdesc)				\
@@ -213,7 +208,6 @@
 #define GET_TX_DESC_SEQ(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+12, 16, 12)
 
-
 #define SET_TX_DESC_RTS_RATE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 5, __val)
 #define SET_TX_DESC_AP_DCFE(__pdesc, __val)		\
@@ -386,7 +380,6 @@
 #define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
 
-
 #define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+32, 0, 32, __val)
 #define SET_TX_DESC_TX_BUFFER_ADDRESS64(__pdesc, __val) \
@@ -549,8 +542,10 @@ do {								\
 	 rxmcs == DESC92C_RATE5_5M ||\
 	 rxmcs == DESC92C_RATE11M)
 
+#define IS_LITTLE_ENDIAN	1
+
 struct phy_rx_agc_info_t {
-	#ifdef __LITTLE_ENDIAN
+	#if IS_LITTLE_ENDIAN
 		u8	gain:7, trsw:1;
 	#else
 		u8	trsw:1, gain:7;
@@ -562,7 +557,7 @@ struct phy_status_rpt {
 	u8	cck_sig_qual_ofdm_pwdb_all;
 	u8	cck_agc_rpt_ofdm_cfosho_a;
 	u8	cck_rpt_b_ofdm_cfosho_b;
-	u8	rsvd_1;
+	u8	rsvd_1;/* ch_corr_msb; */
 	u8	noise_power_db_msb;
 	u8	path_cfotail[2];
 	u8	pcts_mask[2];
@@ -574,7 +569,7 @@ struct phy_status_rpt {
 	u8	stream_target_csi[2];
 	u8	sig_evm;
 	u8	rsvd_3;
-#ifdef __LITTLE_ENDIAN
+#if IS_LITTLE_ENDIAN
 	u8	antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
 	u8	sgi_en:1;
 	u8	rxsc:2;
@@ -777,19 +772,25 @@ struct rx_desc_88e {
 
 void rtl88ee_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			  u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta, struct sk_buff *skb,
+			  u8 *txbd, struct ieee80211_tx_info *info,
+			  struct ieee80211_sta *sta,
+			  struct sk_buff *skb,
 			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
 bool rtl88ee_rx_query_desc(struct ieee80211_hw *hw,
 			   struct rtl_stats *status,
 			   struct ieee80211_rx_status *rx_status,
 			   u8 *pdesc, struct sk_buff *skb);
-void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-		      u8 desc_name, u8 *val);
+void rtl88ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc,
+		      bool istx, u8 desc_name, u8 *val);
 u32 rtl88ee_get_desc(u8 *pdesc, bool istx, u8 desc_name);
+bool rtl88ee_is_tx_desc_closed(struct ieee80211_hw *hw,
+			       u8 hw_queue, u16 index);
 void rtl88ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl88ee_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
-			     bool b_firstseg, bool b_lastseg,
+			     bool firstseg, bool lastseg,
 			     struct sk_buff *skb);
+u32 rtl88ee_rx_command_packet(struct ieee80211_hw *hw,
+			      struct rtl_stats status,
+			      struct sk_buff *skb);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
index eb78fd8607f7..f6cb5aedfdd1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/dm_common.c
@@ -1771,7 +1771,7 @@ static void rtl92c_check_bt_change(struct ieee80211_hw *hw)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 tmp1byte = 0;
 
-	if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version) &&
+	if (IS_81XXC_VENDOR_UMC_B_CUT(rtlhal->version) &&
 	    rtlpcipriv->bt_coexist.bt_coexistence)
 		tmp1byte |= BIT(5);
 	if (rtlpcipriv->bt_coexist.bt_cur_state) {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
index 04a41628ceed..a00861b26ece 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,6 +26,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../core.h"
 #include "../rtl8192ce/reg.h"
 #include "../rtl8192ce/def.h"
 #include "fw_common.h"
@@ -71,66 +68,31 @@ static void _rtl92c_enable_fw_download(struct ieee80211_hw *hw, bool enable)
 	}
 }
 
-static void rtl_block_fw_writeN(struct ieee80211_hw *hw, const u8 *buffer,
-				u32 size)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 blockSize = REALTEK_USB_VENQT_MAX_BUF_SIZE - 20;
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 i, offset, blockCount, remainSize;
-
-	blockCount = size / blockSize;
-	remainSize = size % blockSize;
-
-	for (i = 0; i < blockCount; i++) {
-		offset = i * blockSize;
-		rtlpriv->io.writeN_sync(rtlpriv,
-					(FW_8192C_START_ADDRESS + offset),
-					(void *)(bufferPtr + offset),
-					blockSize);
-	}
-
-	if (remainSize) {
-		offset = blockCount * blockSize;
-		rtlpriv->io.writeN_sync(rtlpriv,
-					(FW_8192C_START_ADDRESS + offset),
-					(void *)(bufferPtr + offset),
-					remainSize);
-	}
-}
-
 static void _rtl92c_fw_block_write(struct ieee80211_hw *hw,
 				   const u8 *buffer, u32 size)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 blockSize = sizeof(u32);
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 *pu4BytePtr = (u32 *) buffer;
-	u32 i, offset, blockCount, remainSize;
-	u32 data;
-
-	if (rtlpriv->io.writeN_sync) {
-		rtl_block_fw_writeN(hw, buffer, size);
-		return;
-	}
-	blockCount = size / blockSize;
-	remainSize = size % blockSize;
-	if (remainSize) {
-		/* the last word is < 4 bytes - pad it with zeros */
-		for (i = 0; i < 4 - remainSize; i++)
-			*(bufferPtr + size + i) = 0;
-		blockCount++;
-	}
+	u32 blocksize = sizeof(u32);
+	u8 *bufferptr = (u8 *)buffer;
+	u32 *pu4byteptr = (u32 *)buffer;
+	u32 i, offset, blockcount, remainsize;
 
-	for (i = 0; i < blockCount; i++) {
-		offset = i * blockSize;
-		/* for big-endian platforms, the firmware data need to be byte
-		 * swapped as it was read as a byte string and will be written
-		 * as 32-bit dwords and byte swapped when written
-		 */
-		data = le32_to_cpu(*(__le32 *)(pu4BytePtr + i));
+	blockcount = size / blocksize;
+	remainsize = size % blocksize;
+
+	for (i = 0; i < blockcount; i++) {
+		offset = i * blocksize;
 		rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
-				data);
+				*(pu4byteptr + i));
+	}
+
+	if (remainsize) {
+		offset = blockcount * blocksize;
+		bufferptr += offset;
+		for (i = 0; i < remainsize; i++) {
+			rtl_write_byte(rtlpriv, (FW_8192C_START_ADDRESS +
+						 offset + i), *(bufferptr + i));
+		}
 	}
 }
 
@@ -168,19 +130,20 @@ static void _rtl92c_write_fw(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 *bufferPtr = buffer;
+	bool is_version_b;
+	u8 *bufferptr = (u8 *)buffer;
 
-	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW size is %d bytes\n", size);
-
-	if (IS_CHIP_VER_B(version)) {
-		u32 pageNums, remainSize;
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW size is %d bytes,\n", size);
+	is_version_b = IS_NORMAL_CHIP(version);
+	if (is_version_b) {
+		u32 pageNums, remainsize;
 		u32 page, offset;
 
-		if (IS_HARDWARE_TYPE_8192CE(rtlhal))
-			_rtl92c_fill_dummy(bufferPtr, &size);
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192CE)
+			_rtl92c_fill_dummy(bufferptr, &size);
 
 		pageNums = size / FW_8192C_PAGE_SIZE;
-		remainSize = size % FW_8192C_PAGE_SIZE;
+		remainsize = size % FW_8192C_PAGE_SIZE;
 
 		if (pageNums > 4) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -189,15 +152,15 @@ static void _rtl92c_write_fw(struct ieee80211_hw *hw,
 
 		for (page = 0; page < pageNums; page++) {
 			offset = page * FW_8192C_PAGE_SIZE;
-			_rtl92c_fw_page_write(hw, page, (bufferPtr + offset),
+			_rtl92c_fw_page_write(hw, page, (bufferptr + offset),
 					      FW_8192C_PAGE_SIZE);
 		}
 
-		if (remainSize) {
+		if (remainsize) {
 			offset = pageNums * FW_8192C_PAGE_SIZE;
 			page = pageNums;
-			_rtl92c_fw_page_write(hw, page, (bufferPtr + offset),
-					      remainSize);
+			_rtl92c_fw_page_write(hw, page, (bufferptr + offset),
+					      remainsize);
 		}
 	} else {
 		_rtl92c_fw_block_write(hw, buffer, size);
@@ -207,6 +170,7 @@ static void _rtl92c_write_fw(struct ieee80211_hw *hw,
 static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err = -EIO;
 	u32 counter = 0;
 	u32 value32;
 
@@ -217,12 +181,13 @@ static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 
 	if (counter >= FW_8192C_POLLING_TIMEOUT_COUNT) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "chksum report faill ! REG_MCUFWDL:0x%08x\n", value32);
-		return -EIO;
+			 "chksum report faill ! REG_MCUFWDL:0x%08x .\n",
+			  value32);
+		goto exit;
 	}
 
 	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-		 "Checksum report OK ! REG_MCUFWDL:0x%08x\n", value32);
+		 "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
 
 	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 	value32 |= MCUFWDL_RDY;
@@ -235,9 +200,10 @@ static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 		if (value32 & WINTINI_RDY) {
 			RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-				 "Polling FW ready success!! REG_MCUFWDL:0x%08x\n",
-				 value32);
-			return 0;
+				 "Polling FW ready success!! REG_MCUFWDL:0x%08x .\n",
+					value32);
+			err = 0;
+			goto exit;
 		}
 
 		mdelay(FW_8192C_POLLING_DELAY);
@@ -245,8 +211,10 @@ static int _rtl92c_fw_free_to_go(struct ieee80211_hw *hw)
 	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
 
 	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x\n", value32);
-	return -EIO;
+		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n", value32);
+
+exit:
+	return err;
 }
 
 int rtl92c_download_fw(struct ieee80211_hw *hw)
@@ -256,21 +224,21 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 	struct rtl92c_firmware_header *pfwheader;
 	u8 *pfwdata;
 	u32 fwsize;
+	int err;
 	enum version_8192c version = rtlhal->version;
 
-	if (rtlpriv->max_fw_size == 0 || !rtlhal->pfirmware)
+	if (!rtlhal->pfirmware)
 		return 1;
 
 	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
-	pfwdata = rtlhal->pfirmware;
+	pfwdata = (u8 *)rtlhal->pfirmware;
 	fwsize = rtlhal->fwsize;
 
 	if (IS_FW_HEADER_EXIST(pfwheader)) {
 		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 			 "Firmware Version(%d), Signature(%#x),Size(%d)\n",
-			 le16_to_cpu(pfwheader->version),
-			 le16_to_cpu(pfwheader->signature),
-			 (uint)sizeof(struct rtl92c_firmware_header));
+			  pfwheader->version, pfwheader->signature,
+			  (int)sizeof(struct rtl92c_firmware_header));
 
 		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
 		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
@@ -280,7 +248,8 @@ int rtl92c_download_fw(struct ieee80211_hw *hw)
 	_rtl92c_write_fw(hw, version, pfwdata, fwsize);
 	_rtl92c_enable_fw_download(hw, false);
 
-	if (_rtl92c_fw_free_to_go(hw)) {
+	err = _rtl92c_fw_free_to_go(hw);
+	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Firmware is not ready to run!\n");
 	} else {
@@ -307,7 +276,7 @@ static bool _rtl92c_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
 }
 
 static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
-			      u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+			      u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -315,7 +284,8 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 	u16 box_reg = 0, box_extreg = 0;
 	u8 u1b_tmp;
 	bool isfw_read = false;
-	bool bwrite_success = false;
+	u8 buf_index = 0;
+	bool bwrite_sucess = false;
 	u8 wait_h2c_limmit = 100;
 	u8 wait_writeh2c_limmit = 100;
 	u8 boxcontent[4], boxextcontent[2];
@@ -329,16 +299,15 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 		if (rtlhal->h2c_setinprogress) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "H2C set in progress! Wait to set..element_id(%d)\n",
+				 "H2C set in progress! Wait to set..element_id(%d).\n",
 				 element_id);
-
 			while (rtlhal->h2c_setinprogress) {
 				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
 						       flag);
 				h2c_waitcounter++;
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 					 "Wait 100 us (%d times)...\n",
-					 h2c_waitcounter);
+					  h2c_waitcounter);
 				udelay(100);
 
 				if (h2c_waitcounter > 1000)
@@ -354,7 +323,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 		}
 	}
 
-	while (!bwrite_success) {
+	while (!bwrite_sucess) {
 		wait_writeh2c_limmit--;
 		if (wait_writeh2c_limmit == 0) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
@@ -381,14 +350,13 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			box_extreg = REG_HMEBOX_EXT_3;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
 		isfw_read = _rtl92c_check_fw_read_last_h2c(hw, boxnum);
 		while (!isfw_read) {
-
 			wait_h2c_limmit--;
 			if (wait_h2c_limmit == 0) {
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
@@ -408,7 +376,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 
 		if (!isfw_read) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Write H2C register BOX[%d] fail!!!!! Fw do not read\n",
+				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
 				 boxnum);
 			break;
 		}
@@ -418,13 +386,13 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 		boxcontent[0] = element_id;
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "Write element_id box_reg(%4x) = %2x\n",
-			 box_reg, element_id);
+			  box_reg, element_id);
 
 		switch (cmd_len) {
 		case 1:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 1);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 1);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -433,8 +401,8 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 2:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 2);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -443,8 +411,8 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 3:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 3);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 3);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -453,10 +421,10 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 4:
 			boxcontent[0] |= (BIT(7));
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer, 2);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + 2, 2);
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index + 2, 2);
 
 			for (idx = 0; idx < 2; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
@@ -470,10 +438,10 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 5:
 			boxcontent[0] |= (BIT(7));
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer, 2);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + 2, 3);
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index + 2, 3);
 
 			for (idx = 0; idx < 2; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
@@ -486,12 +454,12 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 			}
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
-		bwrite_success = true;
+		bwrite_sucess = true;
 
 		rtlhal->last_hmeboxnum = boxnum + 1;
 		if (rtlhal->last_hmeboxnum == 4)
@@ -499,7 +467,7 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "pHalData->last_hmeboxnum  = %d\n",
-			 rtlhal->last_hmeboxnum);
+			  rtlhal->last_hmeboxnum);
 	}
 
 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
@@ -510,12 +478,19 @@ static void _rtl92c_fill_h2c_command(struct ieee80211_hw *hw,
 }
 
 void rtl92c_fill_h2c_cmd(struct ieee80211_hw *hw,
-			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+			 u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 {
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 tmp_cmdbuf[2];
 
+	if (!rtlhal->fw_ready) {
+		RT_ASSERT(false,
+			  "return H2C cmd because of Fw download fail!!!\n");
+		return;
+	}
+
 	memset(tmp_cmdbuf, 0, 8);
-	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
+	memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
 	_rtl92c_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
 
 	return;
@@ -534,7 +509,7 @@ void rtl92c_firmware_selfreset(struct ieee80211_hw *hw)
 	while (u1b_tmp & BIT(2)) {
 		delay--;
 		if (delay == 0) {
-			RT_ASSERT(false, "8051 reset fail\n");
+			RT_ASSERT(false, "8051 reset fail.\n");
 			break;
 		}
 		udelay(50);
@@ -546,56 +521,24 @@ EXPORT_SYMBOL(rtl92c_firmware_selfreset);
 void rtl92c_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 u1_h2c_set_pwrmode[3] = {0};
+	u8 u1_h2c_set_pwrmode[3] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
-					 (rtlpriv->mac80211.p2p) ?
-					 ppsc->smart_ps : 1);
+		(rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
 	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
 					      ppsc->reg_max_lps_awakeintvl);
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl92c_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode",
+		      "rtl92c_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
 		      u1_h2c_set_pwrmode, 3);
 	rtl92c_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
-
 }
 EXPORT_SYMBOL(rtl92c_set_fw_pwrmode_cmd);
 
-static bool _rtl92c_cmd_send_packet(struct ieee80211_hw *hw,
-				struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	unsigned long flags;
-	struct sk_buff *pskb = NULL;
-
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	pskb = __skb_dequeue(&ring->queue);
-	kfree_skb(pskb);
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-
-	pdesc = &ring->desc[0];
-
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
-
-	__skb_queue_tail(&ring->queue, skb);
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
-
-	return true;
-}
-
 #define BEACON_PG		0 /*->1*/
 #define PSPOLL_PG		2
 #define NULL_PG			3
@@ -713,7 +656,7 @@ static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 };
 
-void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
+void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -721,13 +664,13 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 
 	u32 totalpacketlen;
 	bool rtstatus;
-	u8 u1RsvdPageLoc[3] = {0};
-	bool dlok = false;
+	u8 u1rsvdpageloc[3] = { 0 };
+	bool b_dlok = false;
 
 	u8 *beacon;
-	u8 *pspoll;
+	u8 *p_pspoll;
 	u8 *nullfunc;
-	u8 *probersp;
+	u8 *p_probersp;
 	/*---------------------------------------------------------
 				(1) beacon
 	---------------------------------------------------------*/
@@ -738,12 +681,12 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 	/*-------------------------------------------------------
 				(2) ps-poll
 	--------------------------------------------------------*/
-	pspoll = &reserved_page_packet[PSPOLL_PG * 128];
-	SET_80211_PS_POLL_AID(pspoll, (mac->assoc_id | 0xc000));
-	SET_80211_PS_POLL_BSSID(pspoll, mac->bssid);
-	SET_80211_PS_POLL_TA(pspoll, mac->mac_addr);
+	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 
 	/*--------------------------------------------------------
 				(3) null data
@@ -753,57 +696,54 @@ void rtl92c_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
 	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 
 	/*---------------------------------------------------------
 				(4) probe response
 	----------------------------------------------------------*/
-	probersp = &reserved_page_packet[PROBERSP_PG * 128];
-	SET_80211_HDR_ADDRESS1(probersp, mac->bssid);
-	SET_80211_HDR_ADDRESS2(probersp, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(probersp, mac->bssid);
+	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
+	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 
 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
-		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL",
+		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 		      &reserved_page_packet[0], totalpacketlen);
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL",
-		      u1RsvdPageLoc, 3);
+		      "rtl92c_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      u1rsvdpageloc, 3);
 
 
 	skb = dev_alloc_skb(totalpacketlen);
-	if (!skb)
-		return;
-	kmemleak_not_leak(skb);
-
-	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	memcpy((u8 *)skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
-	rtstatus = _rtl92c_cmd_send_packet(hw, skb);
+	rtstatus = rtl_cmd_send_packet(hw, skb);
 
 	if (rtstatus)
-		dlok = true;
+		b_dlok = true;
 
-	if (dlok) {
+	if (b_dlok) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
-			 "Set RSVD page location to Fw\n");
+			 "Set RSVD page location to Fw.\n");
 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-			      "H2C_RSVDPAGE", u1RsvdPageLoc, 3);
+				"H2C_RSVDPAGE:\n",
+				u1rsvdpageloc, 3);
 		rtl92c_fill_h2c_cmd(hw, H2C_RSVDPAGE,
-				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+				    sizeof(u1rsvdpageloc), u1rsvdpageloc);
 	} else
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "Set RSVD page location to Fw FAIL!!!!!!\n");
+			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
 }
 EXPORT_SYMBOL(rtl92c_set_fw_rsvdpagepkt);
 
 void rtl92c_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 {
-	u8 u1_joinbssrpt_parm[1] = {0};
+	u8 u1_joinbssrpt_parm[1] = { 0 };
 
 	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
 
@@ -813,11 +753,51 @@ EXPORT_SYMBOL(rtl92c_set_fw_joinbss_report_cmd);
 
 static void rtl92c_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
 {
-	u8 u1_ctwindow_period[1] = {ctwindow};
+	u8 u1_ctwindow_period[1] = { ctwindow};
 
 	rtl92c_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
 }
 
+/* refactored routine */
+static void set_noa_data(struct rtl_priv *rtlpriv,
+			 struct rtl_p2p_ps_info *p2pinfo,
+			 struct p2p_ps_offload_t *p2p_ps_offload)
+{
+	int i;
+	u32	start_time, tsf_low;
+
+	/* hw only support 2 set of NoA */
+	for (i = 0 ; i < p2pinfo->noa_num ; i++) {
+		/* To control the reg setting for which NOA*/
+		rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+		if (i == 0)
+			p2p_ps_offload->noa0_en = 1;
+		else
+			p2p_ps_offload->noa1_en = 1;
+
+		/* config P2P NoA Descriptor Register */
+		rtl_write_dword(rtlpriv, 0x5E0,
+				p2pinfo->noa_duration[i]);
+		rtl_write_dword(rtlpriv, 0x5E4,
+				p2pinfo->noa_interval[i]);
+
+		/*Get Current TSF value */
+		tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+		start_time = p2pinfo->noa_start_time[i];
+		if (p2pinfo->noa_count_type[i] != 1) {
+			while (start_time <= (tsf_low+(50*1024))) {
+				start_time += p2pinfo->noa_interval[i];
+				if (p2pinfo->noa_count_type[i] != 255)
+					p2pinfo->noa_count_type[i]--;
+			}
+		}
+		rtl_write_dword(rtlpriv, 0x5E8, start_time);
+		rtl_write_dword(rtlpriv, 0x5EC,
+				p2pinfo->noa_count_type[i]);
+	}
+}
+
 void rtl92c_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -825,83 +805,58 @@ void rtl92c_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
 	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
-	u8	i;
 	u16	ctwindow;
-	u32	start_time, tsf_low;
 
 	switch (p2p_ps_state) {
 	case P2P_PS_DISABLE:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
-		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
-		break;
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+				 "P2P_PS_DISABLE\n");
+			memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
+			break;
 	case P2P_PS_ENABLE:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
-		/* update CTWindow value. */
-		if (p2pinfo->ctwindow > 0) {
-			p2p_ps_offload->ctwindow_en = 1;
-			ctwindow = p2pinfo->ctwindow;
-			rtl92c_set_p2p_ctw_period_cmd(hw, ctwindow);
-		}
-		/* hw only support 2 set of NoA */
-		for (i = 0; i < p2pinfo->noa_num; i++) {
-			/* To control the register setting for which NOA*/
-			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
-			if (i == 0)
-				p2p_ps_offload->noa0_en = 1;
-			else
-				p2p_ps_offload->noa1_en = 1;
-
-			/* config P2P NoA Descriptor Register */
-			rtl_write_dword(rtlpriv, 0x5E0,
-					p2pinfo->noa_duration[i]);
-			rtl_write_dword(rtlpriv, 0x5E4,
-					p2pinfo->noa_interval[i]);
-
-			/*Get Current TSF value */
-			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
-
-			start_time = p2pinfo->noa_start_time[i];
-			if (p2pinfo->noa_count_type[i] != 1) {
-				while (start_time <= (tsf_low+(50*1024))) {
-					start_time += p2pinfo->noa_interval[i];
-					if (p2pinfo->noa_count_type[i] != 255)
-						p2pinfo->noa_count_type[i]--;
-				}
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+				 "P2P_PS_ENABLE\n");
+			/* update CTWindow value. */
+			if (p2pinfo->ctwindow > 0) {
+				p2p_ps_offload->ctwindow_en = 1;
+				ctwindow = p2pinfo->ctwindow;
+				rtl92c_set_p2p_ctw_period_cmd(hw, ctwindow);
 			}
-			rtl_write_dword(rtlpriv, 0x5E8, start_time);
-			rtl_write_dword(rtlpriv, 0x5EC,
-					p2pinfo->noa_count_type[i]);
-		}
+			/* call refactored routine */
+			set_noa_data(rtlpriv, p2pinfo, p2p_ps_offload);
 
-		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
-			/* rst p2p circuit */
-			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+			if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
+				/* rst p2p circuit */
+				rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST,
+					       BIT(4));
 
-			p2p_ps_offload->offload_en = 1;
+				p2p_ps_offload->offload_en = 1;
 
-			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
-				p2p_ps_offload->role = 1;
-				p2p_ps_offload->allstasleep = 0;
-			} else {
-				p2p_ps_offload->role = 0;
-			}
+				if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+					p2p_ps_offload->role = 1;
+					p2p_ps_offload->allstasleep = 0;
+				} else {
+					p2p_ps_offload->role = 0;
+				}
 
-			p2p_ps_offload->discovery = 0;
-		}
-		break;
+				p2p_ps_offload->discovery = 0;
+			}
+			break;
 	case P2P_PS_SCAN:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
-		p2p_ps_offload->discovery = 1;
-		break;
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
+			p2p_ps_offload->discovery = 1;
+			break;
 	case P2P_PS_SCAN_DONE:
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
-		p2p_ps_offload->discovery = 0;
-		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
-		break;
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+				 "P2P_PS_SCAN_DONE\n");
+			p2p_ps_offload->discovery = 0;
+			p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+			break;
 	default:
-		break;
+			break;
 	}
 
 	rtl92c_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
+
 }
 EXPORT_SYMBOL_GPL(rtl92c_set_p2p_ps_offload_cmd);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
index 15b2055e6212..a815bd6273da 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/fw_common.h
@@ -36,11 +36,38 @@
 #define FW_8192C_PAGE_SIZE			4096
 #define FW_8192C_POLLING_DELAY			5
 #define FW_8192C_POLLING_TIMEOUT_COUNT		100
+#define NORMAL_CHIP				BIT(4)
 
 #define IS_FW_HEADER_EXIST(_pfwhdr)	\
 	((le16_to_cpu(_pfwhdr->signature)&0xFFF0) == 0x92C0 ||\
 	(le16_to_cpu(_pfwhdr->signature)&0xFFF0) == 0x88C0)
 
+#define CUT_VERSION_MASK		(BIT(6)|BIT(7))
+#define CHIP_VENDOR_UMC			BIT(5)
+#define CHIP_VENDOR_UMC_B_CUT		BIT(6) /* Chip version for ECO */
+#define IS_CHIP_VER_B(version)  ((version & CHIP_VER_B) ? true : false)
+#define RF_TYPE_MASK			(BIT(0)|BIT(1))
+#define GET_CVID_RF_TYPE(version)	\
+	((version) & RF_TYPE_MASK)
+#define GET_CVID_CUT_VERSION(version) \
+	((version) & CUT_VERSION_MASK)
+#define IS_NORMAL_CHIP(version)	\
+	((version & NORMAL_CHIP) ? true : false)
+#define IS_2T2R(version) \
+	(((GET_CVID_RF_TYPE(version)) == \
+	CHIP_92C_BITMASK) ? true : false)
+#define IS_92C_SERIAL(version) \
+	((IS_2T2R(version)) ? true : false)
+#define IS_CHIP_VENDOR_UMC(version)	\
+	((version & CHIP_VENDOR_UMC) ? true : false)
+#define IS_VENDOR_UMC_A_CUT(version) \
+	((IS_CHIP_VENDOR_UMC(version)) ? \
+	((GET_CVID_CUT_VERSION(version)) ? false : true) : false)
+#define IS_81XXC_VENDOR_UMC_B_CUT(version)	\
+	((IS_CHIP_VENDOR_UMC(version)) ? \
+	((GET_CVID_CUT_VERSION(version) == \
+		CHIP_VENDOR_UMC_B_CUT) ? true : false) : false)
+
 struct rtl92c_firmware_header {
 	__le16 signature;
 	u8 category;
@@ -60,19 +87,6 @@ struct rtl92c_firmware_header {
 	__le32 rsvd5;
 };
 
-enum rtl8192c_h2c_cmd {
-	H2C_AP_OFFLOAD = 0,
-	H2C_SETPWRMODE = 1,
-	H2C_JOINBSSRPT = 2,
-	H2C_RSVDPAGE = 3,
-	H2C_RSSI_REPORT = 5,
-	H2C_RA_MASK = 6,
-	H2C_MACID_PS_MODE = 7,
-	H2C_P2P_PS_OFFLOAD = 8,
-	H2C_P2P_PS_CTW_CMD = 32,
-	MAX_H2CCMD
-};
-
 #define pagenum_128(_len)	(u32)(((_len)>>7) + ((_len)&0x7F ? 1 : 0))
 
 #define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
index 9e32ac8a4425..77e61b19bf36 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.c
@@ -27,12 +27,13 @@
  *
  *****************************************************************************/
 
-#include <linux/export.h>
 #include "../wifi.h"
 #include "../rtl8192ce/reg.h"
 #include "../rtl8192ce/def.h"
 #include "dm_common.h"
+#include "fw_common.h"
 #include "phy_common.h"
+#include <linux/export.h>
 
 u32 rtl92c_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
 {
@@ -50,7 +51,6 @@ u32 rtl92c_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
 		 bitmask, regaddr, originalvalue);
 
 	return returnvalue;
-
 }
 EXPORT_SYMBOL(rtl92c_phy_query_bb_reg);
 
@@ -75,7 +75,6 @@ void rtl92c_phy_set_bb_reg(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
 		 regaddr, bitmask, data);
-
 }
 EXPORT_SYMBOL(rtl92c_phy_set_bb_reg);
 
@@ -84,7 +83,6 @@ u32 _rtl92c_phy_fw_rf_serial_read(struct ieee80211_hw *hw,
 {
 	RT_ASSERT(false, "deprecated!\n");
 	return 0;
-
 }
 EXPORT_SYMBOL(_rtl92c_phy_fw_rf_serial_read);
 
@@ -129,10 +127,10 @@ u32 _rtl92c_phy_rf_serial_read(struct ieee80211_hw *hw,
 		      tmplong | BLSSIREADEDGE);
 	mdelay(1);
 	if (rfpath == RF90_PATH_A)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+		rfpi_enable = (u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
 						 BIT(8));
 	else if (rfpath == RF90_PATH_B)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+		rfpi_enable = (u8)rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
 						 BIT(8));
 	if (rfpi_enable)
 		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rbpi,
@@ -141,7 +139,8 @@ u32 _rtl92c_phy_rf_serial_read(struct ieee80211_hw *hw,
 		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rb,
 					 BLSSIREADBACKDATA);
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFR-%d Addr[0x%x]=0x%x\n",
-		 rfpath, pphyreg->rf_rb, retvalue);
+					       rfpath, pphyreg->rf_rb,
+					       retvalue);
 	return retvalue;
 }
 EXPORT_SYMBOL(_rtl92c_phy_rf_serial_read);
@@ -165,7 +164,8 @@ void _rtl92c_phy_rf_serial_write(struct ieee80211_hw *hw,
 	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
 	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE, "RFW-%d Addr[0x%x]=0x%x\n",
-		 rfpath, pphyreg->rf3wire_offset, data_and_addr);
+					       rfpath, pphyreg->rf3wire_offset,
+					       data_and_addr);
 }
 EXPORT_SYMBOL(_rtl92c_phy_rf_serial_write);
 
@@ -174,7 +174,7 @@ u32 _rtl92c_phy_calculate_bit_shift(u32 bitmask)
 	u32 i;
 
 	for (i = 0; i <= 31; i++) {
-		if ((bitmask >> i) & 0x1)
+		if (((bitmask >> i) & 0x1) == 1)
 			break;
 	}
 	return i;
@@ -210,11 +210,10 @@ bool _rtl92c_phy_bb8192c_config_parafile(struct ieee80211_hw *hw)
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	bool rtstatus;
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "==>\n");
 	rtstatus = rtlpriv->cfg->ops->config_bb_with_headerfile(hw,
 						 BASEBAND_CONFIG_PHY_REG);
 	if (!rtstatus) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
 		return false;
 	}
 	if (rtlphy->rf_type == RF_1T2R) {
@@ -227,7 +226,7 @@ bool _rtl92c_phy_bb8192c_config_parafile(struct ieee80211_hw *hw)
 						   BASEBAND_CONFIG_PHY_REG);
 	}
 	if (!rtstatus) {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
 		return false;
 	}
 	rtstatus = rtlpriv->cfg->ops->config_bb_with_headerfile(hw,
@@ -236,12 +235,12 @@ bool _rtl92c_phy_bb8192c_config_parafile(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
 		return false;
 	}
-	rtlphy->cck_high_power = (bool) (rtl_get_bbreg(hw,
-						RFPGA0_XA_HSSIPARAMETER2,
-						0x200));
+	rtlphy->cck_high_power =
+		(bool)(rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, 0x200));
 
 	return true;
 }
+
 EXPORT_SYMBOL(_rtl92c_phy_bb8192c_config_parafile);
 
 void _rtl92c_store_pwrIndex_diffrate_offset(struct ieee80211_hw *hw,
@@ -250,51 +249,153 @@ void _rtl92c_store_pwrIndex_diffrate_offset(struct ieee80211_hw *hw,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	int index;
-
-	if (regaddr == RTXAGC_A_RATE18_06)
-		index = 0;
-	else if (regaddr == RTXAGC_A_RATE54_24)
-		index = 1;
-	else if (regaddr == RTXAGC_A_CCK1_MCS32)
-		index = 6;
-	else if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00)
-		index = 7;
-	else if (regaddr == RTXAGC_A_MCS03_MCS00)
-		index = 2;
-	else if (regaddr == RTXAGC_A_MCS07_MCS04)
-		index = 3;
-	else if (regaddr == RTXAGC_A_MCS11_MCS08)
-		index = 4;
-	else if (regaddr == RTXAGC_A_MCS15_MCS12)
-		index = 5;
-	else if (regaddr == RTXAGC_B_RATE18_06)
-		index = 8;
-	else if (regaddr == RTXAGC_B_RATE54_24)
-		index = 9;
-	else if (regaddr == RTXAGC_B_CCK1_55_MCS32)
-		index = 14;
-	else if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff)
-		index = 15;
-	else if (regaddr == RTXAGC_B_MCS03_MCS00)
-		index = 10;
-	else if (regaddr == RTXAGC_B_MCS07_MCS04)
-		index = 11;
-	else if (regaddr == RTXAGC_B_MCS11_MCS08)
-		index = 12;
-	else if (regaddr == RTXAGC_B_MCS15_MCS12)
-		index = 13;
-	else
-		return;
 
-	rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][index] = data;
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-		 "MCSTxPowerLevelOriginalOffset[%d][%d] = 0x%x\n",
-		 rtlphy->pwrgroup_cnt, index,
-		 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][index]);
+	if (regaddr == RTXAGC_A_RATE18_06) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][0] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][0]);
+	}
+	if (regaddr == RTXAGC_A_RATE54_24) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][1] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][1] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][1]);
+	}
+	if (regaddr == RTXAGC_A_CCK1_MCS32) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][6] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][6] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][6]);
+	}
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][7] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][7] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][7]);
+	}
+	if (regaddr == RTXAGC_A_MCS03_MCS00) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][2] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][2] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][2]);
+	}
+	if (regaddr == RTXAGC_A_MCS07_MCS04) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][3] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][3] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][3]);
+	}
+	if (regaddr == RTXAGC_A_MCS11_MCS08) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][4] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][4] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][4]);
+	}
+	if (regaddr == RTXAGC_A_MCS15_MCS12) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][5] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][5] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][5]);
+	}
+	if (regaddr == RTXAGC_B_RATE18_06) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][8] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][8] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][8]);
+	}
+	if (regaddr == RTXAGC_B_RATE54_24) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][9] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][9] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][9]);
+	}
+	if (regaddr == RTXAGC_B_CCK1_55_MCS32) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][14] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][14] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][14]);
+	}
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][15] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][15] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][15]);
+	}
+	if (regaddr == RTXAGC_B_MCS03_MCS00) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][10] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][10] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][10]);
+	}
+	if (regaddr == RTXAGC_B_MCS07_MCS04) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][11] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][11] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][11]);
+	}
+	if (regaddr == RTXAGC_B_MCS11_MCS08) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][12] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][12] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][12]);
+	}
+	if (regaddr == RTXAGC_B_MCS15_MCS12) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][13] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][13] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][13]);
 
-	if (index == 13)
 		rtlphy->pwrgroup_cnt++;
+	}
 }
 EXPORT_SYMBOL(_rtl92c_store_pwrIndex_diffrate_offset);
 
@@ -304,29 +405,29 @@ void rtl92c_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 
 	rtlphy->default_initialgain[0] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[1] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[2] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[3] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
-		 rtlphy->default_initialgain[0],
-		 rtlphy->default_initialgain[1],
-		 rtlphy->default_initialgain[2],
-		 rtlphy->default_initialgain[3]);
+		  rtlphy->default_initialgain[0],
+		  rtlphy->default_initialgain[1],
+		  rtlphy->default_initialgain[2],
+		  rtlphy->default_initialgain[3]);
 
-	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
+	rtlphy->framesync = (u8)rtl_get_bbreg(hw,
 					       ROFDM0_RXDETECTOR3, MASKBYTE0);
 	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
 					      ROFDM0_RXDETECTOR2, MASKDWORD);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 		 "Default framesync (0x%x) = 0x%x\n",
-		 ROFDM0_RXDETECTOR3, rtlphy->framesync);
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync);
 }
 
 void _rtl92c_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
@@ -426,19 +527,17 @@ void rtl92c_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
 	long txpwr_dbm;
 
 	txpwr_level = rtlphy->cur_cck_txpwridx;
-	txpwr_dbm = _rtl92c_phy_txpwr_idx_to_dbm(hw,
-						 WIRELESS_MODE_B, txpwr_level);
+	txpwr_dbm = _rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B,
+						 txpwr_level);
 	txpwr_level = rtlphy->cur_ofdm24g_txpwridx +
 	    rtlefuse->legacy_ht_txpowerdiff;
-	if (_rtl92c_phy_txpwr_idx_to_dbm(hw,
-					 WIRELESS_MODE_G,
+	if (_rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
 					 txpwr_level) > txpwr_dbm)
 		txpwr_dbm =
 		    _rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
 						 txpwr_level);
 	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (_rtl92c_phy_txpwr_idx_to_dbm(hw,
-					 WIRELESS_MODE_N_24G,
+	if (_rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
 					 txpwr_level) > txpwr_dbm)
 		txpwr_dbm =
 		    _rtl92c_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
@@ -480,21 +579,19 @@ static void _rtl92c_ccxpower_index_check(struct ieee80211_hw *hw,
 
 	rtlphy->cur_cck_txpwridx = cckpowerlevel[0];
 	rtlphy->cur_ofdm24g_txpwridx = ofdmpowerlevel[0];
-
 }
 
 void rtl92c_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 cckpowerlevel[2], ofdmpowerlevel[2];
 
 	if (!rtlefuse->txpwr_fromeprom)
 		return;
 	_rtl92c_get_txpower_index(hw, channel,
 				  &cckpowerlevel[0], &ofdmpowerlevel[0]);
-	_rtl92c_ccxpower_index_check(hw,
-				     channel, &cckpowerlevel[0],
+	_rtl92c_ccxpower_index_check(hw, channel, &cckpowerlevel[0],
 				     &ofdmpowerlevel[0]);
 	rtlpriv->cfg->ops->phy_rf6052_set_cck_txpower(hw, &cckpowerlevel[0]);
 	rtlpriv->cfg->ops->phy_rf6052_set_ofdm_txpower(hw, &ofdmpowerlevel[0],
@@ -509,11 +606,9 @@ bool rtl92c_phy_update_txpower_dbm(struct ieee80211_hw *hw, long power_indbm)
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 idx;
 	u8 rf_path;
-	u8 ccktxpwridx = _rtl92c_phy_dbm_to_txpwr_Idx(hw,
-						      WIRELESS_MODE_B,
+	u8 ccktxpwridx = _rtl92c_phy_dbm_to_txpwr_idx(hw, WIRELESS_MODE_B,
 						      power_indbm);
-	u8 ofdmtxpwridx = _rtl92c_phy_dbm_to_txpwr_Idx(hw,
-						       WIRELESS_MODE_N_24G,
+	u8 ofdmtxpwridx = _rtl92c_phy_dbm_to_txpwr_idx(hw, WIRELESS_MODE_N_24G,
 						       power_indbm);
 	if (ofdmtxpwridx - rtlefuse->legacy_ht_txpowerdiff > 0)
 		ofdmtxpwridx -= rtlefuse->legacy_ht_txpowerdiff;
@@ -521,7 +616,7 @@ bool rtl92c_phy_update_txpower_dbm(struct ieee80211_hw *hw, long power_indbm)
 		ofdmtxpwridx = 0;
 	RT_TRACE(rtlpriv, COMP_TXAGC, DBG_TRACE,
 		 "%lx dBm, ccktxpwridx = %d, ofdmtxpwridx = %d\n",
-		 power_indbm, ccktxpwridx, ofdmtxpwridx);
+		  power_indbm, ccktxpwridx, ofdmtxpwridx);
 	for (idx = 0; idx < 14; idx++) {
 		for (rf_path = 0; rf_path < 2; rf_path++) {
 			rtlefuse->txpwrlevel_cck[rf_path][idx] = ccktxpwridx;
@@ -536,7 +631,7 @@ bool rtl92c_phy_update_txpower_dbm(struct ieee80211_hw *hw, long power_indbm)
 }
 EXPORT_SYMBOL(rtl92c_phy_update_txpower_dbm);
 
-u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
+u8 _rtl92c_phy_dbm_to_txpwr_idx(struct ieee80211_hw *hw,
 				enum wireless_mode wirelessmode,
 				long power_indbm)
 {
@@ -557,7 +652,7 @@ u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
 	}
 
 	if ((power_indbm - offset) > 0)
-		txpwridx = (u8) ((power_indbm - offset) * 2);
+		txpwridx = (u8)((power_indbm - offset) * 2);
 	else
 		txpwridx = 0;
 
@@ -566,7 +661,7 @@ u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
 
 	return txpwridx;
 }
-EXPORT_SYMBOL(_rtl92c_phy_dbm_to_txpwr_Idx);
+EXPORT_SYMBOL(_rtl92c_phy_dbm_to_txpwr_idx);
 
 long _rtl92c_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
 				  enum wireless_mode wirelessmode,
@@ -607,7 +702,7 @@ void rtl92c_phy_set_bw_mode(struct ieee80211_hw *hw,
 		rtlpriv->cfg->ops->phy_set_bw_mode_callback(hw);
 	} else {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "FALSE driver sleep or unload\n");
+			 "false driver sleep or unload\n");
 		rtlphy->set_bwmode_inprogress = false;
 		rtlphy->current_chan_bw = tmp_bw;
 	}
@@ -640,7 +735,7 @@ void rtl92c_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 		}
 		break;
 	} while (true);
-	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "<==\n");
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
 }
 EXPORT_SYMBOL(rtl92c_phy_sw_chnl_callback);
 
@@ -655,14 +750,14 @@ u8 rtl92c_phy_sw_chnl(struct ieee80211_hw *hw)
 	if (rtlphy->set_bwmode_inprogress)
 		return 0;
 	RT_ASSERT((rtlphy->current_channel <= 14),
-		  "WIRELESS_MODE_G but channel>14\n");
+		  "WIRELESS_MODE_G but channel>14");
 	rtlphy->sw_chnl_inprogress = true;
 	rtlphy->sw_chnl_stage = 0;
 	rtlphy->sw_chnl_step = 0;
 	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
 		rtl92c_phy_sw_chnl_callback(hw);
 		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
-			 "sw_chnl_inprogress false schedule workitem\n");
+			 "sw_chnl_inprogress false schdule workitem\n");
 		rtlphy->sw_chnl_inprogress = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
@@ -673,22 +768,22 @@ u8 rtl92c_phy_sw_chnl(struct ieee80211_hw *hw)
 }
 EXPORT_SYMBOL(rtl92c_phy_sw_chnl);
 
-static void _rtl92c_phy_sw_rf_setting(struct ieee80211_hw *hw, u8 channel)
+static void _rtl92c_phy_sw_rf_seting(struct ieee80211_hw *hw, u8 channel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version)) {
-		if (channel == 6 && rtlphy->current_chan_bw ==
-		    HT_CHANNEL_WIDTH_20)
-			rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1, MASKDWORD,
-				      0x00255);
-		else{
-			u32 backupRF0x1A = (u32)rtl_get_rfreg(hw, RF90_PATH_A,
-					    RF_RX_G1, RFREG_OFFSET_MASK);
+	if (IS_81XXC_VENDOR_UMC_B_CUT(rtlhal->version)) {
+		if (channel == 6 &&
+		    rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20) {
+			rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1,
+				      MASKDWORD, 0x00255);
+		} else {
+			u32 backuprf0x1A =
+			  (u32)rtl_get_rfreg(hw, RF90_PATH_A, RF_RX_G1,
+					     RFREG_OFFSET_MASK);
 			rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1, MASKDWORD,
-				      backupRF0x1A);
+				      backuprf0x1A);
 		}
 	}
 }
@@ -701,7 +796,7 @@ static bool _rtl92c_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
 	struct swchnlcmd *pcmd;
 
 	if (cmdtable == NULL) {
-		RT_ASSERT(false, "cmdtable cannot be NULL\n");
+		RT_ASSERT(false, "cmdtable cannot be NULL.\n");
 		return false;
 	}
 
@@ -747,7 +842,7 @@ bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 	rfdependcmdcnt = 0;
 
 	RT_ASSERT((channel >= 1 && channel <= 14),
-		  "invalid channel for Zebra: %d\n", channel);
+		  "illegal channel for Zebra: %d\n", channel);
 
 	_rtl92c_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
 					 MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
@@ -768,6 +863,10 @@ bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 		case 2:
 			currentcmd = &postcommoncmd[*step];
 			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Invalid 'stage' = %d, Check it!\n", *stage);
+			return true;
 		}
 
 		if (currentcmd->cmdid == CMDID_END) {
@@ -794,7 +893,7 @@ bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 			break;
 		case CMDID_WRITEPORT_UCHAR:
 			rtl_write_byte(rtlpriv, currentcmd->para1,
-				       (u8) currentcmd->para2);
+				       (u8)currentcmd->para2);
 			break;
 		case CMDID_RF_WRITEREG:
 			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
@@ -806,12 +905,12 @@ bool _rtl92c_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 					      currentcmd->para1,
 					      RFREG_OFFSET_MASK,
 					      rtlphy->rfreg_chnlval[rfpath]);
-			_rtl92c_phy_sw_rf_setting(hw, channel);
 			}
+			_rtl92c_phy_sw_rf_seting(hw, channel);
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
@@ -900,7 +999,7 @@ static u8 _rtl92c_phy_path_b_iqk(struct ieee80211_hw *hw)
 }
 
 static void _rtl92c_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
-					       bool iqk_ok, long result[][8],
+					       bool b_iqk_ok, long result[][8],
 					       u8 final_candidate, bool btxonly)
 {
 	u32 oldval_0, x, tx0_a, reg;
@@ -908,7 +1007,7 @@ static void _rtl92c_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
 
 	if (final_candidate == 0xFF) {
 		return;
-	} else if (iqk_ok) {
+	} else if (b_iqk_ok) {
 		oldval_0 = (rtl_get_bbreg(hw, ROFDM0_XATXIQIMBALANCE,
 					  MASKDWORD) >> 22) & 0x3FF;
 		x = result[final_candidate][0];
@@ -940,7 +1039,7 @@ static void _rtl92c_phy_path_a_fill_iqk_matrix(struct ieee80211_hw *hw,
 }
 
 static void _rtl92c_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
-					       bool iqk_ok, long result[][8],
+					       bool b_iqk_ok, long result[][8],
 					       u8 final_candidate, bool btxonly)
 {
 	u32 oldval_1, x, tx1_a, reg;
@@ -948,7 +1047,7 @@ static void _rtl92c_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
 
 	if (final_candidate == 0xFF) {
 		return;
-	} else if (iqk_ok) {
+	} else if (b_iqk_ok) {
 		oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
 					  MASKDWORD) >> 22) & 0x3FF;
 		x = result[final_candidate][4];
@@ -1017,7 +1116,7 @@ static void _rtl92c_phy_reload_mac_registers(struct ieee80211_hw *hw,
 	u32 i;
 
 	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
-		rtl_write_byte(rtlpriv, macreg[i], (u8) macbackup[i]);
+		rtl_write_byte(rtlpriv, macreg[i], (u8)macbackup[i]);
 	rtl_write_dword(rtlpriv, macreg[i], macbackup[i]);
 }
 
@@ -1043,14 +1142,14 @@ static void _rtl92c_phy_mac_setting_calibration(struct ieee80211_hw *hw,
 						u32 *macreg, u32 *macbackup)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
+	u32 i = 0;
 
-	rtl_write_byte(rtlpriv, macreg[0], 0x3F);
+	rtl_write_byte(rtlpriv, macreg[i], 0x3F);
 
 	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++)
 		rtl_write_byte(rtlpriv, macreg[i],
-			       (u8) (macbackup[i] & (~BIT(3))));
-	rtl_write_byte(rtlpriv, macreg[i], (u8) (macbackup[i] & (~BIT(5))));
+			       (u8)(macbackup[i] & (~BIT(3))));
+	rtl_write_byte(rtlpriv, macreg[i], (u8)(macbackup[i] & (~BIT(5))));
 }
 
 static void _rtl92c_phy_path_a_standby(struct ieee80211_hw *hw)
@@ -1126,7 +1225,6 @@ static bool _rtl92c_phy_simularity_compare(struct ieee80211_hw *hw,
 	} else {
 		return false;
 	}
-
 }
 
 static void _rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw,
@@ -1142,51 +1240,37 @@ static void _rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw,
 		0xe88, 0xe8c, 0xed0, 0xed4,
 		0xed8, 0xedc, 0xee0, 0xeec
 	};
-
 	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
 		0x522, 0x550, 0x551, 0x040
 	};
-
-	u32 iqk_bb_reg_92C[9] = {
-		0xc04, 0xc08, 0x874, 0xb68,
-		0xb6c, 0x870, 0x860, 0x864,
-		0x800
-	};
-
 	const u32 retrycount = 2;
+	u32 bbvalue;
 
 	if (t == 0) {
-		/* dummy read */
-		rtl_get_bbreg(hw, 0x800, MASKDWORD);
+		bbvalue = rtl_get_bbreg(hw, 0x800, MASKDWORD);
 
 		_rtl92c_phy_save_adda_registers(hw, adda_reg,
 						rtlphy->adda_backup, 16);
 		_rtl92c_phy_save_mac_registers(hw, iqk_mac_reg,
 					       rtlphy->iqk_mac_backup);
-		_rtl92c_phy_save_adda_registers(hw, iqk_bb_reg_92C,
-						rtlphy->iqk_bb_backup, 9);
 	}
 	_rtl92c_phy_path_adda_on(hw, adda_reg, true, is2t);
 	if (t == 0) {
-		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
-						   RFPGA0_XA_HSSIPARAMETER1,
-						   BIT(8));
+		rtlphy->rfpi_enable =
+		   (u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+				     BIT(8));
 	}
 
 	if (!rtlphy->rfpi_enable)
 		_rtl92c_phy_pi_mode_switch(hw, true);
-
-	rtl_set_bbreg(hw, 0x800, BIT(24), 0x0);
-
+	if (t == 0) {
+		rtlphy->reg_c04 = rtl_get_bbreg(hw, 0xc04, MASKDWORD);
+		rtlphy->reg_c08 = rtl_get_bbreg(hw, 0xc08, MASKDWORD);
+		rtlphy->reg_874 = rtl_get_bbreg(hw, 0x874, MASKDWORD);
+	}
 	rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
 	rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
 	rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
-
-	rtl_set_bbreg(hw, 0x870, BIT(10), 0x1);
-	rtl_set_bbreg(hw, 0x870, BIT(26), 0x1);
-	rtl_set_bbreg(hw, 0x860, BIT(10), 0x0);
-	rtl_set_bbreg(hw, 0x864, BIT(10), 0x0);
-
 	if (is2t) {
 		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00010000);
 		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00010000);
@@ -1228,8 +1312,8 @@ static void _rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw,
 			pathb_ok = _rtl92c_phy_path_b_iqk(hw);
 			if (pathb_ok == 0x03) {
 				result[t][4] = (rtl_get_bbreg(hw,
-						      0xeb4,
-						      MASKDWORD) &
+							      0xeb4,
+							      MASKDWORD) &
 						0x3FF0000) >> 16;
 				result[t][5] =
 				    (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
@@ -1243,17 +1327,21 @@ static void _rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw,
 				break;
 			} else if (i == (retrycount - 1) && pathb_ok == 0x01) {
 				result[t][4] = (rtl_get_bbreg(hw,
-						      0xeb4,
-						      MASKDWORD) &
+							      0xeb4,
+							      MASKDWORD) &
 						0x3FF0000) >> 16;
 			}
 			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
 					0x3FF0000) >> 16;
 		}
 	}
-
+	rtl_set_bbreg(hw, 0xc04, MASKDWORD, rtlphy->reg_c04);
+	rtl_set_bbreg(hw, 0x874, MASKDWORD, rtlphy->reg_874);
+	rtl_set_bbreg(hw, 0xc08, MASKDWORD, rtlphy->reg_c08);
 	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
-
+	rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00032ed3);
+	if (is2t)
+		rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00032ed3);
 	if (t != 0) {
 		if (!rtlphy->rfpi_enable)
 			_rtl92c_phy_pi_mode_switch(hw, false);
@@ -1261,379 +1349,12 @@ static void _rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw,
 						  rtlphy->adda_backup, 16);
 		_rtl92c_phy_reload_mac_registers(hw, iqk_mac_reg,
 						 rtlphy->iqk_mac_backup);
-		_rtl92c_phy_reload_adda_registers(hw, iqk_bb_reg_92C,
-						  rtlphy->iqk_bb_backup, 9);
-
-		rtl_set_bbreg(hw, 0x840, MASKDWORD, 0x00032ed3);
-		if (is2t)
-			rtl_set_bbreg(hw, 0x844, MASKDWORD, 0x00032ed3);
-
-		rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x01008c00);
-		rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x01008c00);
 	}
 }
 
 static void _rtl92c_phy_ap_calibrate(struct ieee80211_hw *hw,
 				     char delta, bool is2t)
 {
-#if 0 /* This routine is deliberately dummied out for later fixes */
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	u32 reg_d[PATH_NUM];
-	u32 tmpreg, index, offset, path, i, pathbound = PATH_NUM, apkbound;
-
-	u32 bb_backup[APK_BB_REG_NUM];
-	u32 bb_reg[APK_BB_REG_NUM] = {
-		0x904, 0xc04, 0x800, 0xc08, 0x874
-	};
-	u32 bb_ap_mode[APK_BB_REG_NUM] = {
-		0x00000020, 0x00a05430, 0x02040000,
-		0x000800e4, 0x00204000
-	};
-	u32 bb_normal_ap_mode[APK_BB_REG_NUM] = {
-		0x00000020, 0x00a05430, 0x02040000,
-		0x000800e4, 0x22204000
-	};
-
-	u32 afe_backup[APK_AFE_REG_NUM];
-	u32 afe_reg[APK_AFE_REG_NUM] = {
-		0x85c, 0xe6c, 0xe70, 0xe74, 0xe78,
-		0xe7c, 0xe80, 0xe84, 0xe88, 0xe8c,
-		0xed0, 0xed4, 0xed8, 0xedc, 0xee0,
-		0xeec
-	};
-
-	u32 mac_backup[IQK_MAC_REG_NUM];
-	u32 mac_reg[IQK_MAC_REG_NUM] = {
-		0x522, 0x550, 0x551, 0x040
-	};
-
-	u32 apk_rf_init_value[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x0852c, 0x1852c, 0x5852c, 0x1852c, 0x5852c},
-		{0x2852e, 0x0852e, 0x3852e, 0x0852e, 0x0852e}
-	};
-
-	u32 apk_normal_rf_init_value[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x0852c, 0x0a52c, 0x3a52c, 0x5a52c, 0x5a52c},
-		{0x0852c, 0x0a52c, 0x5a52c, 0x5a52c, 0x5a52c}
-	};
-
-	u32 apk_rf_value_0[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x52019, 0x52014, 0x52013, 0x5200f, 0x5208d},
-		{0x5201a, 0x52019, 0x52016, 0x52033, 0x52050}
-	};
-
-	u32 apk_normal_rf_value_0[PATH_NUM][APK_BB_REG_NUM] = {
-		{0x52019, 0x52017, 0x52010, 0x5200d, 0x5206a},
-		{0x52019, 0x52017, 0x52010, 0x5200d, 0x5206a}
-	};
-
-	u32 afe_on_off[PATH_NUM] = {
-		0x04db25a4, 0x0b1b25a4
-	};
-
-	const u32 apk_offset[PATH_NUM] = { 0xb68, 0xb6c };
-
-	u32 apk_normal_offset[PATH_NUM] = { 0xb28, 0xb98 };
-
-	u32 apk_value[PATH_NUM] = { 0x92fc0000, 0x12fc0000 };
-
-	u32 apk_normal_value[PATH_NUM] = { 0x92680000, 0x12680000 };
-
-	const char apk_delta_mapping[APK_BB_REG_NUM][13] = {
-		{-4, -3, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-4, -3, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-6, -4, -2, -2, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-1, -1, -1, -1, -1, -1, 0, 1, 2, 3, 4, 5, 6},
-		{-11, -9, -7, -5, -3, -1, 0, 0, 0, 0, 0, 0, 0}
-	};
-
-	const u32 apk_normal_setting_value_1[13] = {
-		0x01017018, 0xf7ed8f84, 0x1b1a1816, 0x2522201e, 0x322e2b28,
-		0x433f3a36, 0x5b544e49, 0x7b726a62, 0xa69a8f84, 0xdfcfc0b3,
-		0x12680000, 0x00880000, 0x00880000
-	};
-
-	const u32 apk_normal_setting_value_2[16] = {
-		0x01c7021d, 0x01670183, 0x01000123, 0x00bf00e2, 0x008d00a3,
-		0x0068007b, 0x004d0059, 0x003a0042, 0x002b0031, 0x001f0025,
-		0x0017001b, 0x00110014, 0x000c000f, 0x0009000b, 0x00070008,
-		0x00050006
-	};
-
-	u32 apk_result[PATH_NUM][APK_BB_REG_NUM];
-
-	long bb_offset, delta_v, delta_offset;
-
-	if (!is2t)
-		pathbound = 1;
-
-	return;
-
-	for (index = 0; index < PATH_NUM; index++) {
-		apk_offset[index] = apk_normal_offset[index];
-		apk_value[index] = apk_normal_value[index];
-		afe_on_off[index] = 0x6fdb25a4;
-	}
-
-	for (index = 0; index < APK_BB_REG_NUM; index++) {
-		for (path = 0; path < pathbound; path++) {
-			apk_rf_init_value[path][index] =
-			    apk_normal_rf_init_value[path][index];
-			apk_rf_value_0[path][index] =
-			    apk_normal_rf_value_0[path][index];
-		}
-		bb_ap_mode[index] = bb_normal_ap_mode[index];
-
-		apkbound = 6;
-	}
-
-	for (index = 0; index < APK_BB_REG_NUM; index++) {
-		if (index == 0)
-			continue;
-		bb_backup[index] = rtl_get_bbreg(hw, bb_reg[index], MASKDWORD);
-	}
-
-	_rtl92c_phy_save_mac_registers(hw, mac_reg, mac_backup);
-
-	_rtl92c_phy_save_adda_registers(hw, afe_reg, afe_backup, 16);
-
-	for (path = 0; path < pathbound; path++) {
-		if (path == RF90_PATH_A) {
-			offset = 0xb00;
-			for (index = 0; index < 11; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-
-			rtl_set_bbreg(hw, 0xb98, MASKDWORD, 0x12680000);
-
-			offset = 0xb68;
-			for (; index < 13; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x40000000);
-
-			offset = 0xb00;
-			for (index = 0; index < 16; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_2
-					      [index]);
-
-				offset += 0x04;
-			}
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
-		} else if (path == RF90_PATH_B) {
-			offset = 0xb70;
-			for (index = 0; index < 10; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-			rtl_set_bbreg(hw, 0xb28, MASKDWORD, 0x12680000);
-			rtl_set_bbreg(hw, 0xb98, MASKDWORD, 0x12680000);
-
-			offset = 0xb68;
-			index = 11;
-			for (; index < 13; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_1
-					      [index]);
-
-				offset += 0x04;
-			}
-
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x40000000);
-
-			offset = 0xb60;
-			for (index = 0; index < 16; index++) {
-				rtl_set_bbreg(hw, offset, MASKDWORD,
-					      apk_normal_setting_value_2
-					      [index]);
-
-				offset += 0x04;
-			}
-			rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
-		}
-
-		reg_d[path] = rtl_get_rfreg(hw, (enum radio_path)path,
-					    0xd, MASKDWORD);
-
-		for (index = 0; index < APK_AFE_REG_NUM; index++)
-			rtl_set_bbreg(hw, afe_reg[index], MASKDWORD,
-				      afe_on_off[path]);
-
-		if (path == RF90_PATH_A) {
-			for (index = 0; index < APK_BB_REG_NUM; index++) {
-				if (index == 0)
-					continue;
-				rtl_set_bbreg(hw, bb_reg[index], MASKDWORD,
-					      bb_ap_mode[index]);
-			}
-		}
-
-		_rtl92c_phy_mac_setting_calibration(hw, mac_reg, mac_backup);
-
-		if (path == 0) {
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x0, MASKDWORD, 0x10000);
-		} else {
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASKDWORD,
-				      0x10000);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x10, MASKDWORD,
-				      0x1000f);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x11, MASKDWORD,
-				      0x20103);
-		}
-
-		delta_offset = ((delta + 14) / 2);
-		if (delta_offset < 0)
-			delta_offset = 0;
-		else if (delta_offset > 12)
-			delta_offset = 12;
-
-		for (index = 0; index < APK_BB_REG_NUM; index++) {
-			if (index != 1)
-				continue;
-
-			tmpreg = apk_rf_init_value[path][index];
-
-			if (!rtlefuse->apk_thermalmeterignore) {
-				bb_offset = (tmpreg & 0xF0000) >> 16;
-
-				if (!(tmpreg & BIT(15)))
-					bb_offset = -bb_offset;
-
-				delta_v =
-				    apk_delta_mapping[index][delta_offset];
-
-				bb_offset += delta_v;
-
-				if (bb_offset < 0) {
-					tmpreg = tmpreg & (~BIT(15));
-					bb_offset = -bb_offset;
-				} else {
-					tmpreg = tmpreg | BIT(15);
-				}
-
-				tmpreg =
-				    (tmpreg & 0xFFF0FFFF) | (bb_offset << 16);
-			}
-
-			rtl_set_rfreg(hw, (enum radio_path)path, 0xc,
-				      MASKDWORD, 0x8992e);
-			rtl_set_rfreg(hw, (enum radio_path)path, 0x0,
-				      MASKDWORD, apk_rf_value_0[path][index]);
-			rtl_set_rfreg(hw, (enum radio_path)path, 0xd,
-				      MASKDWORD, tmpreg);
-
-			i = 0;
-			do {
-				rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80000000);
-				rtl_set_bbreg(hw, apk_offset[path],
-					      MASKDWORD, apk_value[0]);
-				RTPRINT(rtlpriv, FINIT, INIT_IQK,
-					("PHY_APCalibrate() offset 0x%x "
-					 "value 0x%x\n",
-					 apk_offset[path],
-					 rtl_get_bbreg(hw, apk_offset[path],
-						       MASKDWORD)));
-
-				mdelay(3);
-
-				rtl_set_bbreg(hw, apk_offset[path],
-					      MASKDWORD, apk_value[1]);
-				RTPRINT(rtlpriv, FINIT, INIT_IQK,
-					("PHY_APCalibrate() offset 0x%x "
-					 "value 0x%x\n",
-					 apk_offset[path],
-					 rtl_get_bbreg(hw, apk_offset[path],
-						       MASKDWORD)));
-
-				mdelay(20);
-
-				rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
-
-				if (path == RF90_PATH_A)
-					tmpreg = rtl_get_bbreg(hw, 0xbd8,
-							       0x03E00000);
-				else
-					tmpreg = rtl_get_bbreg(hw, 0xbd8,
-							       0xF8000000);
-
-				RTPRINT(rtlpriv, FINIT, INIT_IQK,
-					("PHY_APCalibrate() offset "
-					 "0xbd8[25:21] %x\n", tmpreg));
-
-				i++;
-
-			} while (tmpreg > apkbound && i < 4);
-
-			apk_result[path][index] = tmpreg;
-		}
-	}
-
-	_rtl92c_phy_reload_mac_registers(hw, mac_reg, mac_backup);
-
-	for (index = 0; index < APK_BB_REG_NUM; index++) {
-		if (index == 0)
-			continue;
-		rtl_set_bbreg(hw, bb_reg[index], MASKDWORD, bb_backup[index]);
-	}
-
-	_rtl92c_phy_reload_adda_registers(hw, afe_reg, afe_backup, 16);
-
-	for (path = 0; path < pathbound; path++) {
-		rtl_set_rfreg(hw, (enum radio_path)path, 0xd,
-			      MASKDWORD, reg_d[path]);
-
-		if (path == RF90_PATH_B) {
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x10, MASKDWORD,
-				      0x1000f);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x11, MASKDWORD,
-				      0x20101);
-		}
-
-		if (apk_result[path][1] > 6)
-			apk_result[path][1] = 6;
-	}
-
-	for (path = 0; path < pathbound; path++) {
-		rtl_set_rfreg(hw, (enum radio_path)path, 0x3, MASKDWORD,
-			      ((apk_result[path][1] << 15) |
-			       (apk_result[path][1] << 10) |
-			       (apk_result[path][1] << 5) |
-			       apk_result[path][1]));
-
-		if (path == RF90_PATH_A)
-			rtl_set_rfreg(hw, (enum radio_path)path, 0x4, MASKDWORD,
-				      ((apk_result[path][1] << 15) |
-				       (apk_result[path][1] << 10) |
-				       (0x00 << 5) | 0x05));
-		else
-			rtl_set_rfreg(hw, (enum radio_path)path, 0x4, MASKDWORD,
-				      ((apk_result[path][1] << 15) |
-				       (apk_result[path][1] << 10) |
-				       (0x02 << 5) | 0x05));
-
-		rtl_set_rfreg(hw, (enum radio_path)path, 0xe, MASKDWORD,
-			      ((0x08 << 15) | (0x08 << 10) | (0x08 << 5) |
-			       0x08));
-
-	}
-	rtlphy->b_apk_done = true;
-#endif
 }
 
 static void _rtl92c_phy_set_rfpath_switch(struct ieee80211_hw *hw,
@@ -1657,15 +1378,13 @@ static void _rtl92c_phy_set_rfpath_switch(struct ieee80211_hw *hw,
 			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300, 0x2);
 		else
 			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300, 0x1);
-
 	}
-
 }
 
 #undef IQK_ADDA_REG_NUM
 #undef IQK_DELAY_TIME
 
-void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
+void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -1673,10 +1392,10 @@ void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 
 	long result[4][8];
 	u8 i, final_candidate;
-	bool patha_ok, pathb_ok;
-	long reg_e94, reg_e9c, reg_ea4, reg_eb4, reg_ebc, reg_ec4, reg_tmp = 0;
+	bool b_patha_ok, b_pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
+	    reg_ecc, reg_tmp = 0;
 	bool is12simular, is13simular, is23simular;
-	bool start_conttx = false, singletone = false;
 	u32 iqk_bb_reg[10] = {
 		ROFDM0_XARXIQIMBALANCE,
 		ROFDM0_XBRXIQIMBALANCE,
@@ -1690,14 +1409,12 @@ void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		ROFDM0_RXIQEXTANTA
 	};
 
-	if (recovery) {
+	if (b_recovery) {
 		_rtl92c_phy_reload_adda_registers(hw,
 						  iqk_bb_reg,
 						  rtlphy->iqk_bb_backup, 10);
 		return;
 	}
-	if (start_conttx || singletone)
-		return;
 	for (i = 0; i < 8; i++) {
 		result[0][i] = 0;
 		result[1][i] = 0;
@@ -1705,8 +1422,8 @@ void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		result[3][i] = 0;
 	}
 	final_candidate = 0xff;
-	patha_ok = false;
-	pathb_ok = false;
+	b_patha_ok = false;
+	b_pathb_ok = false;
 	is12simular = false;
 	is23simular = false;
 	is13simular = false;
@@ -1752,29 +1469,34 @@ void rtl92c_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		reg_e94 = result[i][0];
 		reg_e9c = result[i][1];
 		reg_ea4 = result[i][2];
+		reg_eac = result[i][3];
 		reg_eb4 = result[i][4];
 		reg_ebc = result[i][5];
 		reg_ec4 = result[i][6];
+		reg_ecc = result[i][7];
 	}
 	if (final_candidate != 0xff) {
 		rtlphy->reg_e94 = reg_e94 = result[final_candidate][0];
 		rtlphy->reg_e9c = reg_e9c = result[final_candidate][1];
 		reg_ea4 = result[final_candidate][2];
+		reg_eac = result[final_candidate][3];
 		rtlphy->reg_eb4 = reg_eb4 = result[final_candidate][4];
 		rtlphy->reg_ebc = reg_ebc = result[final_candidate][5];
 		reg_ec4 = result[final_candidate][6];
-		patha_ok = pathb_ok = true;
+		reg_ecc = result[final_candidate][7];
+		b_patha_ok = true;
+		b_pathb_ok = true;
 	} else {
 		rtlphy->reg_e94 = rtlphy->reg_eb4 = 0x100;
 		rtlphy->reg_e9c = rtlphy->reg_ebc = 0x0;
 	}
 	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
-		_rtl92c_phy_path_a_fill_iqk_matrix(hw, patha_ok, result,
+		_rtl92c_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
 						   final_candidate,
 						   (reg_ea4 == 0));
 	if (IS_92C_SERIAL(rtlhal->version)) {
 		if (reg_eb4 != 0) /*&&(reg_ec4 != 0) */
-			_rtl92c_phy_path_b_fill_iqk_matrix(hw, pathb_ok,
+			_rtl92c_phy_path_b_fill_iqk_matrix(hw, b_pathb_ok,
 							   result,
 							   final_candidate,
 							   (reg_ec4 == 0));
@@ -1788,10 +1510,7 @@ void rtl92c_phy_lc_calibrate(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool start_conttx = false, singletone = false;
 
-	if (start_conttx || singletone)
-		return;
 	if (IS_92C_SERIAL(rtlhal->version))
 		rtlpriv->cfg->ops->phy_lc_calibrate(hw, true);
 	else
@@ -1833,22 +1552,22 @@ bool rtl92c_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
-		 iotype, rtlphy->set_io_inprogress);
+		  iotype, rtlphy->set_io_inprogress);
 	do {
 		switch (iotype) {
 		case IO_CMD_RESUME_DM_BY_SCAN:
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-				 "[IO CMD] Resume DM after scan\n");
+				 "[IO CMD] Resume DM after scan.\n");
 			postprocessing = true;
 			break;
-		case IO_CMD_PAUSE_DM_BY_SCAN:
+		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-				 "[IO CMD] Pause DM before scan\n");
+				 "[IO CMD] Pause DM before scan.\n");
 			postprocessing = true;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 	} while (false);
@@ -1859,7 +1578,7 @@ bool rtl92c_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 		return false;
 	}
 	rtl92c_phy_set_io(hw);
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "<--IO Type(%#x)\n", iotype);
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
 	return true;
 }
 EXPORT_SYMBOL(rtl92c_phy_set_io_cmd);
@@ -1868,30 +1587,30 @@ void rtl92c_phy_set_io(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct dig_t dm_digtable = rtlpriv->dm_digtable;
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
-		 rtlphy->current_io_type, rtlphy->set_io_inprogress);
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
 	switch (rtlphy->current_io_type) {
 	case IO_CMD_RESUME_DM_BY_SCAN:
-		dm_digtable.cur_igvalue = rtlphy->initgain_backup.xaagccore1;
+		dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
 		rtl92c_dm_write_dig(hw);
 		rtl92c_phy_set_txpower_level(hw, rtlphy->current_channel);
 		break;
-	case IO_CMD_PAUSE_DM_BY_SCAN:
-		rtlphy->initgain_backup.xaagccore1 = dm_digtable.cur_igvalue;
-		dm_digtable.cur_igvalue = 0x37;
+	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
+		dm_digtable->cur_igvalue = 0x17;
 		rtl92c_dm_write_dig(hw);
 		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
 	rtlphy->set_io_inprogress = false;
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "<---(%#x)\n",
-		 rtlphy->current_io_type);
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "(%#x)\n", rtlphy->current_io_type);
 }
 EXPORT_SYMBOL(rtl92c_phy_set_io);
 
@@ -1931,7 +1650,7 @@ void _rtl92c_phy_set_rf_sleep(struct ieee80211_hw *hw)
 		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
 		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
-			 "Switch RF timeout !!!\n");
+			 "Switch RF timeout !!!.\n");
 		return;
 	}
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.h b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.h
index e79dabe9ba1d..64bc49f4dbc6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192c/phy_common.h
@@ -226,7 +226,7 @@ u32 _rtl92c_phy_calculate_bit_shift(u32 bitmask);
 long _rtl92c_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
 				  enum wireless_mode wirelessmode,
 				  u8 txpwridx);
-u8 _rtl92c_phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
+u8 _rtl92c_phy_dbm_to_txpwr_idx(struct ieee80211_hw *hw,
 				enum wireless_mode wirelessmode,
 				long power_indbm);
 void _rtl92c_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
index fa24de43ce79..831df101d7b7 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/def.h
@@ -146,21 +146,6 @@ enum version_8192c {
 	VERSION_UNKNOWN = 0x88,
 };
 
-#define CUT_VERSION_MASK		(BIT(6)|BIT(7))
-#define CHIP_VENDOR_UMC			BIT(5)
-#define CHIP_VENDOR_UMC_B_CUT		BIT(6) /* Chip version for ECO */
-#define IS_VENDOR_UMC_A_CUT(version)	((IS_CHIP_VENDOR_UMC(version)) ? \
-	((GET_CVID_CUT_VERSION(version)) ? false : true) : false)
-#define IS_CHIP_VER_B(version)  ((version & CHIP_VER_B) ? true : false)
-#define IS_92C_SERIAL(version)  ((version & CHIP_92C_BITMASK) ? true : false)
-#define IS_CHIP_VENDOR_UMC(version)		\
-	((version & CHIP_VENDOR_UMC) ? true : false)
-#define GET_CVID_CUT_VERSION(version)	((version) & CUT_VERSION_MASK)
-#define IS_81xxC_VENDOR_UMC_B_CUT(version)		\
-	((IS_CHIP_VENDOR_UMC(version)) ? \
-	((GET_CVID_CUT_VERSION(version) == CHIP_VENDOR_UMC_B_CUT) ?	\
-	true : false) : false)
-
 enum rtl819x_loopback_e {
 	RTL819X_NO_LOOPBACK = 0,
 	RTL819X_MAC_LOOPBACK = 1,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
index d4a3d032c7bf..9c5311c299fd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/dm.h
@@ -86,70 +86,6 @@
 #define TX_POWER_NEAR_FIELD_THRESH_LVL2		74
 #define TX_POWER_NEAR_FIELD_THRESH_LVL1		67
 
-struct swat_t {
-	u8 failure_cnt;
-	u8 try_flag;
-	u8 stop_trying;
-	long pre_rssi;
-	long trying_threshold;
-	u8 cur_antenna;
-	u8 pre_antenna;
-};
-
-enum tag_dynamic_init_gain_operation_type_definition {
-	DIG_TYPE_THRESH_HIGH = 0,
-	DIG_TYPE_THRESH_LOW = 1,
-	DIG_TYPE_BACKOFF = 2,
-	DIG_TYPE_RX_GAIN_MIN = 3,
-	DIG_TYPE_RX_GAIN_MAX = 4,
-	DIG_TYPE_ENABLE = 5,
-	DIG_TYPE_DISABLE = 6,
-	DIG_OP_TYPE_MAX
-};
-
-enum tag_cck_packet_detection_threshold_type_definition {
-	CCK_PD_STAGE_LowRssi = 0,
-	CCK_PD_STAGE_HighRssi = 1,
-	CCK_FA_STAGE_Low = 2,
-	CCK_FA_STAGE_High = 3,
-	CCK_PD_STAGE_MAX = 4,
-};
-
-enum dm_1r_cca_e {
-	CCA_1R = 0,
-	CCA_2R = 1,
-	CCA_MAX = 2,
-};
-
-enum dm_rf_e {
-	RF_SAVE = 0,
-	RF_NORMAL = 1,
-	RF_MAX = 2,
-};
-
-enum dm_sw_ant_switch_e {
-	ANS_ANTENNA_B = 1,
-	ANS_ANTENNA_A = 2,
-	ANS_ANTENNA_MAX = 3,
-};
-
-enum dm_dig_ext_port_alg_e {
-	DIG_EXT_PORT_STAGE_0 = 0,
-	DIG_EXT_PORT_STAGE_1 = 1,
-	DIG_EXT_PORT_STAGE_2 = 2,
-	DIG_EXT_PORT_STAGE_3 = 3,
-	DIG_EXT_PORT_STAGE_MAX = 4,
-};
-
-enum dm_dig_connect_e {
-	DIG_STA_DISCONNECT = 0,
-	DIG_STA_CONNECT = 1,
-	DIG_STA_BEFORE_CONNECT = 2,
-	DIG_MULTISTA_DISCONNECT = 3,
-	DIG_MULTISTA_CONNECT = 4,
-	DIG_CONNECT_MAX
-};
-
 void rtl92c_dm_init(struct ieee80211_hw *hw);
 void rtl92c_dm_watchdog(struct ieee80211_hw *hw);
 void rtl92c_dm_write_dig(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
index df98a5e4729a..8ec0f031f48a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.c
@@ -37,7 +37,9 @@
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
+#include "../rtl8192c/dm_common.h"
 #include "../rtl8192c/fw_common.h"
+#include "../rtl8192c/phy_common.h"
 #include "dm.h"
 #include "led.h"
 #include "hw.h"
@@ -53,7 +55,7 @@ static void _rtl92ce_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
 	rtlpci->reg_bcn_ctrl_val |= set_bits;
 	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
 
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlpci->reg_bcn_ctrl_val);
 }
 
 static void _rtl92ce_stop_tx_beacon(struct ieee80211_hw *hw)
@@ -985,7 +987,7 @@ int rtl92ce_hw_init(struct ieee80211_hw *hw)
 	    !IS_92C_SERIAL(rtlhal->version)) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1, MASKDWORD, 0x30255);
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G2, MASKDWORD, 0x50a00);
-	} else if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version)) {
+	} else if (IS_81XXC_VENDOR_UMC_B_CUT(rtlhal->version)) {
 		rtl_set_rfreg(hw, RF90_PATH_A, 0x0C, MASKDWORD, 0x894AE);
 		rtl_set_rfreg(hw, RF90_PATH_A, 0x0A, MASKDWORD, 0x1AF31);
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_IPA, MASKDWORD, 0x8F425);
@@ -1330,7 +1332,7 @@ static void _rtl92ce_poweroff_adapter(struct ieee80211_hw *hw)
 	rtl_write_word(rtlpriv, REG_GPIO_IO_SEL, 0x0790);
 	rtl_write_word(rtlpriv, REG_LEDCFG0, 0x8080);
 	rtl_write_byte(rtlpriv, REG_AFE_PLL_CTRL, 0x80);
-	if (!IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version))
+	if (!IS_81XXC_VENDOR_UMC_B_CUT(rtlhal->version))
 		rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x23);
 	if (rtlpcipriv->bt_coexist.bt_coexistence) {
 		u4b_tmp = rtl_read_dword(rtlpriv, REG_AFE_XTAL_CTRL);
@@ -1494,7 +1496,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 
 	for (rf_path = 0; rf_path < 2; rf_path++) {
 		for (i = 0; i < 14; i++) {
-			index = _rtl92c_get_chnl_group((u8) i);
+			index = rtl92c_get_chnl_group((u8)i);
 
 			rtlefuse->txpwrlevel_cck[rf_path][i] =
 			    rtlefuse->eeprom_chnlarea_txpwr_cck[rf_path][index];
@@ -1543,7 +1545,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 
 	for (rf_path = 0; rf_path < 2; rf_path++) {
 		for (i = 0; i < 14; i++) {
-			index = _rtl92c_get_chnl_group((u8) i);
+			index = rtl92c_get_chnl_group((u8)i);
 
 			if (rf_path == RF90_PATH_A) {
 				rtlefuse->pwrgroup_ht20[rf_path][i] =
@@ -1573,7 +1575,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	}
 
 	for (i = 0; i < 14; i++) {
-		index = _rtl92c_get_chnl_group((u8) i);
+		index = rtl92c_get_chnl_group((u8)i);
 
 		if (!autoload_fail)
 			tempval = hwinfo[EEPROM_TXPOWERHT20DIFF + index];
@@ -1590,7 +1592,7 @@ static void _rtl92ce_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		if (rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] & BIT(3))
 			rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] |= 0xF0;
 
-		index = _rtl92c_get_chnl_group((u8) i);
+		index = rtl92c_get_chnl_group((u8)i);
 
 		if (!autoload_fail)
 			tempval = hwinfo[EEPROM_TXPOWER_OFDMDIFF + index];
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
index 5533070f266c..98a086822aac 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/hw.h
@@ -30,7 +30,7 @@
 #ifndef __RTL92CE_HW_H__
 #define __RTL92CE_HW_H__
 
-static inline u8 _rtl92c_get_chnl_group(u8 chnl)
+static inline u8 rtl92c_get_chnl_group(u8 chnl)
 {
 	u8 group;
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
index 98b22303c84d..bc5ca989b915 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.c
@@ -35,8 +35,11 @@
 #include "def.h"
 #include "hw.h"
 #include "phy.h"
+#include "../rtl8192c/phy_common.h"
 #include "rf.h"
 #include "dm.h"
+#include "../rtl8192c/dm_common.h"
+#include "../rtl8192c/fw_common.h"
 #include "table.h"
 
 static bool _rtl92c_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h
index 94486cca4000..e5e1353a94c3 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/phy.h
@@ -78,113 +78,6 @@
 
 #define RTL92C_MAX_PATH_NUM		2
 
-enum swchnlcmd_id {
-	CMDID_END,
-	CMDID_SET_TXPOWEROWER_LEVEL,
-	CMDID_BBREGWRITE10,
-	CMDID_WRITEPORT_ULONG,
-	CMDID_WRITEPORT_USHORT,
-	CMDID_WRITEPORT_UCHAR,
-	CMDID_RF_WRITEREG,
-};
-
-struct swchnlcmd {
-	enum swchnlcmd_id cmdid;
-	u32 para1;
-	u32 para2;
-	u32 msdelay;
-};
-
-enum hw90_block_e {
-	HW90_BLOCK_MAC = 0,
-	HW90_BLOCK_PHY0 = 1,
-	HW90_BLOCK_PHY1 = 2,
-	HW90_BLOCK_RF = 3,
-	HW90_BLOCK_MAXIMUM = 4,
-};
-
-enum baseband_config_type {
-	BASEBAND_CONFIG_PHY_REG = 0,
-	BASEBAND_CONFIG_AGC_TAB = 1,
-};
-
-enum ra_offset_area {
-	RA_OFFSET_LEGACY_OFDM1,
-	RA_OFFSET_LEGACY_OFDM2,
-	RA_OFFSET_HT_OFDM1,
-	RA_OFFSET_HT_OFDM2,
-	RA_OFFSET_HT_OFDM3,
-	RA_OFFSET_HT_OFDM4,
-	RA_OFFSET_HT_CCK,
-};
-
-enum antenna_path {
-	ANTENNA_NONE,
-	ANTENNA_D,
-	ANTENNA_C,
-	ANTENNA_CD,
-	ANTENNA_B,
-	ANTENNA_BD,
-	ANTENNA_BC,
-	ANTENNA_BCD,
-	ANTENNA_A,
-	ANTENNA_AD,
-	ANTENNA_AC,
-	ANTENNA_ACD,
-	ANTENNA_AB,
-	ANTENNA_ABD,
-	ANTENNA_ABC,
-	ANTENNA_ABCD
-};
-
-struct r_antenna_select_ofdm {
-	u32 r_tx_antenna:4;
-	u32 r_ant_l:4;
-	u32 r_ant_non_ht:4;
-	u32 r_ant_ht1:4;
-	u32 r_ant_ht2:4;
-	u32 r_ant_ht_s1:4;
-	u32 r_ant_non_ht_s1:4;
-	u32 ofdm_txsc:2;
-	u32 reserved:2;
-};
-
-struct r_antenna_select_cck {
-	u8 r_cckrx_enable_2:2;
-	u8 r_cckrx_enable:2;
-	u8 r_ccktx_enable:4;
-};
-
-struct efuse_contents {
-	u8 mac_addr[ETH_ALEN];
-	u8 cck_tx_power_idx[6];
-	u8 ht40_1s_tx_power_idx[6];
-	u8 ht40_2s_tx_power_idx_diff[3];
-	u8 ht20_tx_power_idx_diff[3];
-	u8 ofdm_tx_power_idx_diff[3];
-	u8 ht40_max_power_offset[3];
-	u8 ht20_max_power_offset[3];
-	u8 channel_plan;
-	u8 thermal_meter;
-	u8 rf_option[5];
-	u8 version;
-	u8 oem_id;
-	u8 regulatory;
-};
-
-struct tx_power_struct {
-	u8 cck[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 ht40_1s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 ht40_2s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 ht20_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 legacy_ht_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 legacy_ht_txpowerdiff;
-	u8 groupht20[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 groupht40[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 pwrgroup_cnt;
-	u32 mcs_original_offset[4][16];
-};
-
 bool rtl92c_phy_bb_config(struct ieee80211_hw *hw);
 u32 rtl92c_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask);
 void rtl92c_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
index 4bbdfb2df363..d86b5b566444 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/sw.c
@@ -35,6 +35,9 @@
 #include "def.h"
 #include "phy.h"
 #include "dm.h"
+#include "../rtl8192c/dm_common.h"
+#include "../rtl8192c/fw_common.h"
+#include "../rtl8192c/phy_common.h"
 #include "hw.h"
 #include "rf.h"
 #include "sw.h"
@@ -165,7 +168,7 @@ int rtl92c_init_sw_vars(struct ieee80211_hw *hw)
 	if (IS_VENDOR_UMC_A_CUT(rtlhal->version) &&
 	    !IS_92C_SERIAL(rtlhal->version))
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8192cfwU.bin";
-	else if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version))
+	else if (IS_81XXC_VENDOR_UMC_B_CUT(rtlhal->version))
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8192cfwU_B.bin";
 
 	rtlpriv->max_fw_size = 0x4000;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
index 8f04817cb7ec..2fb9c7acb76a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ce/trx.c
@@ -125,7 +125,7 @@ static void _rtl92ce_query_rxphystatus(struct ieee80211_hw *hw,
 	u32 rssi, total_rssi = 0;
 	bool is_cck_rate;
 
-	is_cck_rate = RX_HAL_IS_CCK_RATE(pdesc);
+	is_cck_rate = RX_HAL_IS_CCK_RATE(pdesc->rxmcs);
 	pstats->packet_matchbssid = packet_match_bssid;
 	pstats->packet_toself = packet_toself;
 	pstats->is_cck = is_cck_rate;
@@ -361,7 +361,7 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 	stats->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
 	stats->is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
 
-	stats->is_cck = RX_HAL_IS_CCK_RATE(pdesc);
+	stats->is_cck = RX_HAL_IS_CCK_RATE(pdesc->rxmcs);
 
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
@@ -389,10 +389,6 @@ bool rtl92ce_rx_query_desc(struct ieee80211_hw *hw,
 	 * to decrypt it
 	 */
 	if (stats->decrypted) {
-		if (!hdr) {
-			/* In testing, hdr was NULL here */
-			return false;
-		}
 		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
 		    (ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/def.h b/drivers/net/wireless/rtlwifi/rtl8192cu/def.h
index f916555e6311..c940a87175ca 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/def.h
@@ -38,9 +38,6 @@
 #define CHIP_VENDOR_UMC			BIT(5)
 #define CHIP_VENDOR_UMC_B_CUT		BIT(6)
 
-#define IS_NORMAL_CHIP(version)		\
-	(((version) & NORMAL_CHIP) ? true : false)
-
 #define IS_8723_SERIES(version)		\
 	(((version) & CHIP_8723) ? true : false)
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
index 270cbffcac70..04aa0b5f5b3d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/hw.c
@@ -36,8 +36,11 @@
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
+#include "../rtl8192c/phy_common.h"
 #include "mac.h"
 #include "dm.h"
+#include "../rtl8192c/dm_common.h"
+#include "../rtl8192c/fw_common.h"
 #include "hw.h"
 #include "../rtl8192ce/hw.h"
 #include "trx.h"
@@ -180,7 +183,7 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 				eprom_chnl_txpwr_ht40_2sdf[rf_path][i]);
 	for (rf_path = 0; rf_path < 2; rf_path++) {
 		for (i = 0; i < 14; i++) {
-			index = _rtl92c_get_chnl_group((u8) i);
+			index = rtl92c_get_chnl_group((u8)i);
 			rtlefuse->txpwrlevel_cck[rf_path][i] =
 			    rtlefuse->eeprom_chnlarea_txpwr_cck[rf_path][index];
 			rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
@@ -222,7 +225,7 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	}
 	for (rf_path = 0; rf_path < 2; rf_path++) {
 		for (i = 0; i < 14; i++) {
-			index = _rtl92c_get_chnl_group((u8) i);
+			index = rtl92c_get_chnl_group((u8)i);
 			if (rf_path == RF90_PATH_A) {
 				rtlefuse->pwrgroup_ht20[rf_path][i] =
 				    (rtlefuse->eeprom_pwrlimit_ht20[index]
@@ -249,7 +252,7 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		}
 	}
 	for (i = 0; i < 14; i++) {
-		index = _rtl92c_get_chnl_group((u8) i);
+		index = rtl92c_get_chnl_group((u8)i);
 		if (!autoload_fail)
 			tempval = hwinfo[EEPROM_TXPOWERHT20DIFF + index];
 		else
@@ -261,7 +264,7 @@ static void _rtl92cu_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 			rtlefuse->txpwr_ht20diff[RF90_PATH_A][i] |= 0xF0;
 		if (rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] & BIT(3))
 			rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] |= 0xF0;
-		index = _rtl92c_get_chnl_group((u8) i);
+		index = rtl92c_get_chnl_group((u8)i);
 		if (!autoload_fail)
 			tempval = hwinfo[EEPROM_TXPOWER_OFDMDIFF + index];
 		else
@@ -1169,13 +1172,13 @@ n. LEDCFG 0x4C[15:0] = 0x8080
 	/* 1. Disable GPIO[7:0] */
 	rtl_write_word(rtlpriv, REG_GPIO_PIN_CTRL+2, 0x0000);
 	value32 = rtl_read_dword(rtlpriv, REG_GPIO_PIN_CTRL) & 0xFFFF00FF;
-	value8 = (u8) (value32&0x000000FF);
+	value8 = (u8)(value32&0x000000FF);
 	value32 |= ((value8<<8) | 0x00FF0000);
 	rtl_write_dword(rtlpriv, REG_GPIO_PIN_CTRL, value32);
 	/* 2. Disable GPIO[10:8] */
 	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG+3, 0x00);
 	value16 = rtl_read_word(rtlpriv, REG_GPIO_MUXCFG+2) & 0xFF0F;
-	value8 = (u8) (value16&0x000F);
+	value8 = (u8)(value16&0x000F);
 	value16 |= ((value8<<4) | 0x0780);
 	rtl_write_word(rtlpriv, REG_GPIO_PIN_CTRL+2, value16);
 	/* 3. Disable LED0 & 1 */
@@ -1245,7 +1248,7 @@ static void _rtl92cu_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
 
 	rtlusb->reg_bcn_ctrl_val |= set_bits;
 	rtlusb->reg_bcn_ctrl_val &= ~clear_bits;
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlusb->reg_bcn_ctrl_val);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlusb->reg_bcn_ctrl_val);
 }
 
 static void _rtl92cu_stop_tx_beacon(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
index e26312fb4356..c2d8ec6afcda 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/mac.c
@@ -40,6 +40,7 @@
 #include "dm.h"
 #include "mac.h"
 #include "trx.h"
+#include "../rtl8192c/fw_common.h"
 
 #include <linux/module.h>
 
@@ -786,7 +787,7 @@ static void _rtl92c_query_rxphystatus(struct ieee80211_hw *hw,
 	bool is_cck_rate;
 	u8 *pdesc = (u8 *)p_desc;
 
-	is_cck_rate = RX_HAL_IS_CCK_RATE(p_desc);
+	is_cck_rate = RX_HAL_IS_CCK_RATE(p_desc->rxmcs);
 	pstats->packet_matchbssid = packet_match_bssid;
 	pstats->packet_toself = packet_toself;
 	pstats->packet_beacon = packet_beacon;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
index 9831ff1128ca..12f6d474b492 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/phy.c
@@ -34,8 +34,11 @@
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
+#include "../rtl8192c/phy_common.h"
 #include "rf.h"
 #include "dm.h"
+#include "../rtl8192c/dm_common.h"
+#include "../rtl8192c/fw_common.h"
 #include "table.h"
 
 u32 rtl92cu_phy_query_rf_reg(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
index 361435f8608a..7c5fbaf5fee0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/sw.c
@@ -42,6 +42,7 @@
 #include "trx.h"
 #include "led.h"
 #include "hw.h"
+#include "../rtl8192c/fw_common.h"
 #include <linux/module.h>
 
 MODULE_AUTHOR("Georgia		<georgia@realtek.com>");
@@ -75,7 +76,7 @@ static int rtl92cu_init_sw_vars(struct ieee80211_hw *hw)
 	if (IS_VENDOR_UMC_A_CUT(rtlpriv->rtlhal.version) &&
 	    !IS_92C_SERIAL(rtlpriv->rtlhal.version)) {
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8192cufw_A.bin";
-	} else if (IS_81xxC_VENDOR_UMC_B_CUT(rtlpriv->rtlhal.version)) {
+	} else if (IS_81XXC_VENDOR_UMC_B_CUT(rtlpriv->rtlhal.version)) {
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8192cufw_B.bin";
 	} else {
 		rtlpriv->cfg->fw_name = "rtlwifi/rtl8192cufw_TMSC.bin";
@@ -121,7 +122,6 @@ static struct rtl_hal_ops rtl8192cu_hal_ops = {
 	.fill_tx_desc = rtl92cu_tx_fill_desc,
 	.fill_fake_txdesc = rtl92cu_fill_fake_txdesc,
 	.fill_tx_cmddesc = rtl92cu_tx_fill_cmddesc,
-	.cmd_send_packet = rtl92cu_cmd_send_packet,
 	.query_rx_desc = rtl92cu_rx_query_desc,
 	.set_channel_access = rtl92cu_update_channel_access_setting,
 	.radio_onoff_checking = rtl92cu_gpio_radio_on_off_checking,
@@ -317,6 +317,7 @@ static struct usb_device_id rtl8192c_usb_ids[] = {
 	{RTL_USB_DEVICE(0x0bda, 0x5088, rtl92cu_hal_cfg)}, /*Thinkware-CC&C*/
 	{RTL_USB_DEVICE(0x0df6, 0x0052, rtl92cu_hal_cfg)}, /*Sitecom - Edimax*/
 	{RTL_USB_DEVICE(0x0df6, 0x005c, rtl92cu_hal_cfg)}, /*Sitecom - Edimax*/
+	{RTL_USB_DEVICE(0x0df6, 0x0070, rtl92cu_hal_cfg)}, /*Sitecom - 150N */
 	{RTL_USB_DEVICE(0x0df6, 0x0077, rtl92cu_hal_cfg)}, /*Sitecom-WLA2100V2*/
 	{RTL_USB_DEVICE(0x0eb0, 0x9071, rtl92cu_hal_cfg)}, /*NO Brand - Etop*/
 	{RTL_USB_DEVICE(0x4856, 0x0091, rtl92cu_hal_cfg)}, /*NetweeN - Feixun*/
diff --git a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
index 035e0dc3922c..f383d5f1fed5 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192cu/trx.c
@@ -38,6 +38,7 @@
 #include "dm.h"
 #include "mac.h"
 #include "trx.h"
+#include "../rtl8192c/fw_common.h"
 
 static int _ConfigVerTOutEP(struct ieee80211_hw *hw)
 {
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/fw.h b/drivers/net/wireless/rtlwifi/rtl8192de/fw.h
index 1ffacdda734c..a55a803a0b4d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/fw.h
@@ -132,18 +132,6 @@ struct rtl92d_firmware_header {
 	u32 rsvd5;
 };
 
-enum rtl8192d_h2c_cmd {
-	H2C_AP_OFFLOAD = 0,
-	H2C_SETPWRMODE = 1,
-	H2C_JOINBSSRPT = 2,
-	H2C_RSVDPAGE = 3,
-	H2C_RSSI_REPORT = 5,
-	H2C_RA_MASK = 6,
-	H2C_MAC_MODE_SEL = 9,
-	H2C_PWRM = 15,
-	MAX_H2CCMD
-};
-
 int rtl92d_download_fw(struct ieee80211_hw *hw);
 void rtl92d_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 			 u32 cmd_len, u8 *p_cmdbuffer);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
index 592125a5f19c..1961b8e28dc1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/phy.c
@@ -677,7 +677,7 @@ static void _rtl92d_store_pwrindex_diffrate_offset(struct ieee80211_hw *hw,
 
 	rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][index] = data;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-		 "MCSTxPowerLevelOriginalOffset[%d][%d] = 0x%ulx\n",
+		 "MCSTxPowerLevelOriginalOffset[%d][%d] = 0x%x\n",
 		 rtlphy->pwrgroup_cnt, index,
 		 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][index]);
 	if (index == 13)
@@ -2531,7 +2531,7 @@ static void _rtl92d_phy_reload_lck_setting(struct ieee80211_hw *hw,
 	if (rtlpriv->rtlhal.current_bandtype == BAND_ON_5G) {/* Path-A for 5G */
 		u4tmp = curveindex_5g[channel-1];
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
-			"ver 1 set RF-A, 5G,	0x28 = 0x%ulx !!\n", u4tmp);
+			"ver 1 set RF-A, 5G,	0x28 = 0x%x !!\n", u4tmp);
 		if (rtlpriv->rtlhal.macphymode == DUALMAC_DUALPHY &&
 			rtlpriv->rtlhal.interfaceindex == 1) {
 			bneed_powerdown_radio =
@@ -2550,7 +2550,7 @@ static void _rtl92d_phy_reload_lck_setting(struct ieee80211_hw *hw,
 	} else if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G) {
 		u4tmp = curveindex_2g[channel-1];
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
-			"ver 3 set RF-B, 2G, 0x28 = 0x%ulx !!\n", u4tmp);
+			"ver 3 set RF-B, 2G, 0x28 = 0x%x !!\n", u4tmp);
 		if (rtlpriv->rtlhal.macphymode == DUALMAC_DUALPHY &&
 			rtlpriv->rtlhal.interfaceindex == 0) {
 			bneed_powerdown_radio =
@@ -2562,7 +2562,7 @@ static void _rtl92d_phy_reload_lck_setting(struct ieee80211_hw *hw,
 		}
 		rtl_set_rfreg(hw, erfpath, RF_SYN_G4, 0x3f800, u4tmp);
 		RTPRINT(rtlpriv, FINIT, INIT_IQK,
-			"ver 3 set RF-B, 2G, 0x28 = 0x%ulx !!\n",
+			"ver 3 set RF-B, 2G, 0x28 = 0x%x !!\n",
 			rtl_get_rfreg(hw,  erfpath, RF_SYN_G4, 0x3f800));
 		if (bneed_powerdown_radio)
 			_rtl92d_phy_restore_rf_env(hw, erfpath, &u4regvalue);
diff --git a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
index 99c2ab5dfceb..8efbcc7af250 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192de/trx.c
@@ -127,7 +127,7 @@ static void _rtl92de_query_rxphystatus(struct ieee80211_hw *hw,
 	u32 rssi, total_rssi = 0;
 	bool is_cck_rate;
 
-	is_cck_rate = RX_HAL_IS_CCK_RATE(pdesc);
+	is_cck_rate = RX_HAL_IS_CCK_RATE(pdesc->rxmcs);
 	pstats->packet_matchbssid = packet_match_bssid;
 	pstats->packet_toself = packet_toself;
 	pstats->packet_beacon = packet_beacon;
diff --git a/drivers/net/wireless/rtlwifi/rtl8192ee/Makefile b/drivers/net/wireless/rtlwifi/rtl8192ee/Makefile
new file mode 100644
index 000000000000..11952b99daf8
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/Makefile
@@ -0,0 +1,19 @@
+obj-m := rtl8192ee.o
+
+
+rtl8192ee-objs :=		\
+		dm.o		\
+		fw.o		\
+		hw.o		\
+		led.o		\
+		phy.o		\
+		pwrseq.o	\
+		rf.o		\
+		sw.o		\
+		table.o		\
+		trx.o		\
+
+
+obj-$(CONFIG_RTL8821AE) += rtl8192ee.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/def.h b/drivers/net/wireless/rtlwifi/rtl8192ee/def.h
index 7566c1e8baeb..60f5728b4e2d 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/def.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -45,7 +41,6 @@
 #define IS_OFDM_RATE(_rate)	\
 	(_rate >= DESC92C_RATE6M && _rate <= DESC92C_RATE54M)
 
-
 enum version_8192e {
 	VERSION_TEST_CHIP_2T2R_8192E = 0x0024,
 	VERSION_NORMAL_CHIP_2T2R_8192E = 0x102C,
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/dm.c b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.c
index 41c2d98e81db..77deedf79d1d 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -33,8 +33,6 @@
 #include "fw.h"
 #include "trx.h"
 
-struct dig_t dm_dig;
-
 static const u32 ofdmswing_table[OFDM_TABLE_SIZE] = {
 	0x7f8001fe,		/* 0, +6.0dB */
 	0x788001e2,		/* 1, +5.5dB */
@@ -156,35 +154,37 @@ static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
 static void rtl92ee_dm_diginit(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	dm_dig.cur_igvalue = rtl_get_bbreg(hw, DM_REG_IGI_A_11N,
-						DM_BIT_IGI_11N);
-	dm_dig.rssi_lowthresh = DM_DIG_THRESH_LOW;
-	dm_dig.rssi_highthresh = DM_DIG_THRESH_HIGH;
-	dm_dig.fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
-	dm_dig.fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	dm_dig.rx_gain_range_max = DM_DIG_MAX;
-	dm_dig.rx_gain_range_min = DM_DIG_MIN;
-	dm_dig.backoff_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_dig.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
-	dm_dig.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
-	dm_dig.pre_cck_cca_thres = 0xff;
-	dm_dig.cur_cck_cca_thres = 0x83;
-	dm_dig.forbidden_igi = DM_DIG_MIN;
-	dm_dig.large_fa_hit = 0;
-	dm_dig.recover_cnt = 0;
-	dm_dig.dig_dynamic_min_0 = DM_DIG_MIN;
-	dm_dig.dig_dynamic_min_1 = DM_DIG_MIN;
-	dm_dig.b_media_connect_0 = false;
-	dm_dig.b_media_connect_1 = false;
-	rtlpriv->dm.b_dm_initialgain_enable = true;
-	dm_dig.bt30_cur_igi = 0x32;
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+
+	dm_dig->cur_igvalue = rtl_get_bbreg(hw, DM_REG_IGI_A_11N,
+					    DM_BIT_IGI_11N);
+	dm_dig->rssi_lowthresh = DM_DIG_THRESH_LOW;
+	dm_dig->rssi_highthresh = DM_DIG_THRESH_HIGH;
+	dm_dig->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	dm_dig->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	dm_dig->rx_gain_max = DM_DIG_MAX;
+	dm_dig->rx_gain_min = DM_DIG_MIN;
+	dm_dig->back_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_dig->back_range_max = DM_DIG_BACKOFF_MAX;
+	dm_dig->back_range_min = DM_DIG_BACKOFF_MIN;
+	dm_dig->pre_cck_cca_thres = 0xff;
+	dm_dig->cur_cck_cca_thres = 0x83;
+	dm_dig->forbidden_igi = DM_DIG_MIN;
+	dm_dig->large_fa_hit = 0;
+	dm_dig->recover_cnt = 0;
+	dm_dig->dig_dynamic_min = DM_DIG_MIN;
+	dm_dig->dig_dynamic_min_1 = DM_DIG_MIN;
+	dm_dig->media_connect_0 = false;
+	dm_dig->media_connect_1 = false;
+	rtlpriv->dm.dm_initialgain_enable = true;
+	dm_dig->bt30_cur_igi = 0x32;
 }
 
 static void rtl92ee_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 {
 	u32 ret_value;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
+	struct false_alarm_statistics *falsealm_cnt = &rtlpriv->falsealm_cnt;
 
 	rtl_set_bbreg(hw, DM_REG_OFDM_FA_HOLDC_11N, BIT(31), 1);
 	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(31), 1);
@@ -254,29 +254,29 @@ static void rtl92ee_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(15) | BIT(14), 0);
 	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(15) | BIT(14), 2);
 
-
-	RT_TRACE(COMP_DIG, DBG_TRACE,
-		 ("cnt_parity_fail = %d, cnt_rate_illegal = %d, "
-		  "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
 		  falsealm_cnt->cnt_parity_fail,
 		  falsealm_cnt->cnt_rate_illegal,
-		  falsealm_cnt->cnt_crc8_fail, falsealm_cnt->cnt_mcs_fail));
+		  falsealm_cnt->cnt_crc8_fail, falsealm_cnt->cnt_mcs_fail);
 
-	RT_TRACE(COMP_DIG, DBG_TRACE,
-		 ("cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
 		  falsealm_cnt->cnt_ofdm_fail,
-		  falsealm_cnt->cnt_cck_fail, falsealm_cnt->cnt_all));
+		  falsealm_cnt->cnt_cck_fail, falsealm_cnt->cnt_all);
 }
 
 static void rtl92ee_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 	u8 cur_cck_cca_thresh;
+
 	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-		if (dm_dig.rssi_val_min > 25) {
+		if (dm_dig->rssi_val_min > 25) {
 			cur_cck_cca_thresh = 0xcd;
-		} else if ((dm_dig.rssi_val_min <= 25) &&
-			   (dm_dig.rssi_val_min > 10)) {
+		} else if ((dm_dig->rssi_val_min <= 25) &&
+			   (dm_dig->rssi_val_min > 10)) {
 			cur_cck_cca_thresh = 0x83;
 		} else {
 			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
@@ -297,105 +297,106 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 	u8 dig_dynamic_min , dig_maxofmin;
 	bool bfirstconnect , bfirstdisconnect;
 	u8 dm_dig_max, dm_dig_min;
-	u8 current_igi = dm_dig.cur_igvalue;
+	u8 current_igi = dm_dig->cur_igvalue;
 	u8 offset;
 
-	/* AP, BT */
-	if (mac->act_scanning == true)
+	/* AP,BT */
+	if (mac->act_scanning)
 		return;
 
-	dig_dynamic_min = dm_dig.dig_dynamic_min_0;
+	dig_dynamic_min = dm_dig->dig_dynamic_min;
 	bfirstconnect = (mac->link_state >= MAC80211_LINKED) &&
-			(dm_dig.b_media_connect_0 == false);
+			!dm_dig->media_connect_0;
 	bfirstdisconnect = (mac->link_state < MAC80211_LINKED) &&
-			   (dm_dig.b_media_connect_0 == true);
+			   dm_dig->media_connect_0;
 
 	dm_dig_max = 0x5a;
 	dm_dig_min = DM_DIG_MIN;
 	dig_maxofmin = DM_DIG_MAX_AP;
 
 	if (mac->link_state >= MAC80211_LINKED) {
-		if ((dm_dig.rssi_val_min + 10) > dm_dig_max)
-			dm_dig.rx_gain_range_max = dm_dig_max;
-		else if ((dm_dig.rssi_val_min + 10) < dm_dig_min)
-			dm_dig.rx_gain_range_max = dm_dig_min;
+		if ((dm_dig->rssi_val_min + 10) > dm_dig_max)
+			dm_dig->rx_gain_max = dm_dig_max;
+		else if ((dm_dig->rssi_val_min + 10) < dm_dig_min)
+			dm_dig->rx_gain_max = dm_dig_min;
 		else
-			dm_dig.rx_gain_range_max = dm_dig.rssi_val_min + 10;
+			dm_dig->rx_gain_max = dm_dig->rssi_val_min + 10;
 
-		if (rtlpriv->dm.b_one_entry_only) {
+		if (rtlpriv->dm.one_entry_only) {
 			offset = 0;
-			if (dm_dig.rssi_val_min - offset < dm_dig_min)
+			if (dm_dig->rssi_val_min - offset < dm_dig_min)
 				dig_dynamic_min = dm_dig_min;
-			else if (dm_dig.rssi_val_min - offset >
+			else if (dm_dig->rssi_val_min - offset >
 				 dig_maxofmin)
 				dig_dynamic_min = dig_maxofmin;
 			else
-				dig_dynamic_min = dm_dig.rssi_val_min - offset;
+				dig_dynamic_min = dm_dig->rssi_val_min - offset;
 		} else {
 			dig_dynamic_min = dm_dig_min;
 		}
 
 	} else {
-		dm_dig.rx_gain_range_max = dm_dig_max;
+		dm_dig->rx_gain_max = dm_dig_max;
 		dig_dynamic_min = dm_dig_min;
-		RT_TRACE(COMP_DIG, DBG_LOUD, ("no link\n"));
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "no link\n");
 	}
 
 	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
-		if (dm_dig.large_fa_hit != 3)
-			dm_dig.large_fa_hit++;
-		if (dm_dig.forbidden_igi < current_igi) {
-			dm_dig.forbidden_igi = current_igi;
-			dm_dig.large_fa_hit = 1;
+		if (dm_dig->large_fa_hit != 3)
+			dm_dig->large_fa_hit++;
+		if (dm_dig->forbidden_igi < current_igi) {
+			dm_dig->forbidden_igi = current_igi;
+			dm_dig->large_fa_hit = 1;
 		}
 
-		if (dm_dig.large_fa_hit >= 3) {
-			if (dm_dig.forbidden_igi + 1 > dm_dig.rx_gain_range_max)
-				dm_dig.rx_gain_range_min =
-						dm_dig.rx_gain_range_max;
+		if (dm_dig->large_fa_hit >= 3) {
+			if (dm_dig->forbidden_igi + 1 > dm_dig->rx_gain_max)
+				dm_dig->rx_gain_min =
+						dm_dig->rx_gain_max;
 			else
-				dm_dig.rx_gain_range_min =
-						dm_dig.forbidden_igi + 1;
-			dm_dig.recover_cnt = 3600;
+				dm_dig->rx_gain_min =
+						dm_dig->forbidden_igi + 1;
+			dm_dig->recover_cnt = 3600;
 		}
 	} else {
-		if (dm_dig.recover_cnt != 0) {
-			dm_dig.recover_cnt--;
+		if (dm_dig->recover_cnt != 0) {
+			dm_dig->recover_cnt--;
 		} else {
-			if (dm_dig.large_fa_hit < 3) {
-				if ((dm_dig.forbidden_igi - 1) <
+			if (dm_dig->large_fa_hit < 3) {
+				if ((dm_dig->forbidden_igi - 1) <
 				    dig_dynamic_min) {
-					dm_dig.forbidden_igi = dig_dynamic_min;
-					dm_dig.rx_gain_range_min =
+					dm_dig->forbidden_igi = dig_dynamic_min;
+					dm_dig->rx_gain_min =
 								dig_dynamic_min;
 				} else {
-					dm_dig.forbidden_igi--;
-					dm_dig.rx_gain_range_min =
-						dm_dig.forbidden_igi + 1;
+					dm_dig->forbidden_igi--;
+					dm_dig->rx_gain_min =
+						dm_dig->forbidden_igi + 1;
 				}
 			} else {
-				dm_dig.large_fa_hit = 0;
+				dm_dig->large_fa_hit = 0;
 			}
 		}
 	}
 
 	if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 5)
-		dm_dig.rx_gain_range_min = dm_dig_min;
+		dm_dig->rx_gain_min = dm_dig_min;
 
-	if (dm_dig.rx_gain_range_min > dm_dig.rx_gain_range_max)
-		dm_dig.rx_gain_range_min = dm_dig.rx_gain_range_max;
+	if (dm_dig->rx_gain_min > dm_dig->rx_gain_max)
+		dm_dig->rx_gain_min = dm_dig->rx_gain_max;
 
 	if (mac->link_state >= MAC80211_LINKED) {
 		if (bfirstconnect) {
-			if (dm_dig.rssi_val_min <= dig_maxofmin)
-				current_igi = dm_dig.rssi_val_min;
+			if (dm_dig->rssi_val_min <= dig_maxofmin)
+				current_igi = dm_dig->rssi_val_min;
 			else
 				current_igi = dig_maxofmin;
 
-			dm_dig.large_fa_hit = 0;
+			dm_dig->large_fa_hit = 0;
 		} else {
 			if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH2)
 				current_igi += 4;
@@ -406,11 +407,11 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 
 			if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 5 &&
 			    rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH1)
-				current_igi = dm_dig.rx_gain_range_min;
+				current_igi = dm_dig->rx_gain_min;
 		}
 	} else {
 		if (bfirstdisconnect) {
-			current_igi = dm_dig.rx_gain_range_min;
+			current_igi = dm_dig->rx_gain_min;
 		} else {
 			if (rtlpriv->falsealm_cnt.cnt_all > 10000)
 				current_igi += 4;
@@ -421,52 +422,58 @@ static void rtl92ee_dm_dig(struct ieee80211_hw *hw)
 		}
 	}
 
-	if (current_igi > dm_dig.rx_gain_range_max)
-		current_igi = dm_dig.rx_gain_range_max;
-	if (current_igi < dm_dig.rx_gain_range_min)
-		current_igi = dm_dig.rx_gain_range_min;
+	if (current_igi > dm_dig->rx_gain_max)
+		current_igi = dm_dig->rx_gain_max;
+	if (current_igi < dm_dig->rx_gain_min)
+		current_igi = dm_dig->rx_gain_min;
 
 	rtl92ee_dm_write_dig(hw , current_igi);
-	dm_dig.b_media_connect_0 = ((mac->link_state >= MAC80211_LINKED) ?
+	dm_dig->media_connect_0 = ((mac->link_state >= MAC80211_LINKED) ?
 				   true : false);
-	dm_dig.dig_dynamic_min_0 = dig_dynamic_min;
+	dm_dig->dig_dynamic_min = dig_dynamic_min;
 }
 
 void rtl92ee_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 cur_thres)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	if (dm_dig.cur_cck_cca_thres != cur_thres)
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+
+	if (dm_dig->cur_cck_cca_thres != cur_thres)
 		rtl_write_byte(rtlpriv, DM_REG_CCK_CCA_11N, cur_thres);
 
-	dm_dig.pre_cck_cca_thres = dm_dig.cur_cck_cca_thres;
-	dm_dig.cur_cck_cca_thres = cur_thres;
+	dm_dig->pre_cck_cca_thres = dm_dig->cur_cck_cca_thres;
+	dm_dig->cur_cck_cca_thres = cur_thres;
 }
 
 void rtl92ee_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	if (dm_dig.stop_dig)
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
+
+	if (dm_dig->stop_dig)
 		return;
 
-	if (dm_dig.cur_igvalue != current_igi) {
+	if (dm_dig->cur_igvalue != current_igi) {
 		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f, current_igi);
 		if (rtlpriv->phy.rf_type != RF_1T1R)
 			rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f, current_igi);
 	}
-	dm_dig.pre_igvalue = dm_dig.cur_igvalue;
-	dm_dig.cur_igvalue = current_igi;
+	dm_dig->pre_igvalue = dm_dig->cur_igvalue;
+	dm_dig->cur_igvalue = current_igi;
 }
 
 static void rtl92ee_rssi_dump_to_register(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
 	rtl_write_byte(rtlpriv, RA_RSSIDUMP,
 		       rtlpriv->stats.rx_rssi_percentage[0]);
 	rtl_write_byte(rtlpriv, RB_RSSIDUMP,
 		       rtlpriv->stats.rx_rssi_percentage[1]);
-	/*It seems the following values is not initialized.
+	/*It seems the following values are not initialized.
 	  *According to Windows code,
-	  *these value will only be valid when JAGUAR chips*/
+	  *these value will only be valid with JAGUAR chips
+	  */
 	/* Rx EVM */
 	rtl_write_byte(rtlpriv, RS1_RXEVMDUMP, rtlpriv->stats.rx_evm_dbm[0]);
 	rtl_write_byte(rtlpriv, RS2_RXEVMDUMP, rtlpriv->stats.rx_evm_dbm[1]);
@@ -488,45 +495,47 @@ static void rtl92ee_rssi_dump_to_register(struct ieee80211_hw *hw)
 static void rtl92ee_dm_find_minimum_rssi(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dig *rtl_dm_dig = &(rtlpriv->dm.dm_digtable);
+	struct dig_t *rtl_dm_dig = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
 
 	/* Determine the minimum RSSI  */
 	if ((mac->link_state < MAC80211_LINKED) &&
-	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
-		rtl_dm_dig->min_undecorated_pwdb_for_dm = 0;
-		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-			 ("Not connected to any\n"));
+	    (rtlpriv->dm.entry_min_undec_sm_pwdb == 0)) {
+		rtl_dm_dig->min_undec_pwdb_for_dm = 0;
+		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+			 "Not connected to any\n");
 	}
 	if (mac->link_state >= MAC80211_LINKED) {
 		if (mac->opmode == NL80211_IFTYPE_AP ||
 		    mac->opmode == NL80211_IFTYPE_ADHOC) {
-			rtl_dm_dig->min_undecorated_pwdb_for_dm =
-				rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-			      ("AP Client PWDB = 0x%lx\n",
-			       rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb));
+			rtl_dm_dig->min_undec_pwdb_for_dm =
+				rtlpriv->dm.entry_min_undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+				 "AP Client PWDB = 0x%lx\n",
+				 rtlpriv->dm.entry_min_undec_sm_pwdb);
 		} else {
-			rtl_dm_dig->min_undecorated_pwdb_for_dm =
-			    rtlpriv->dm.undecorated_smoothed_pwdb;
-			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-				("STA Default Port PWDB = 0x%x\n",
-				rtl_dm_dig->min_undecorated_pwdb_for_dm));
+			rtl_dm_dig->min_undec_pwdb_for_dm =
+			    rtlpriv->dm.undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+				 "STA Default Port PWDB = 0x%x\n",
+				 rtl_dm_dig->min_undec_pwdb_for_dm);
 		}
 	} else {
-		rtl_dm_dig->min_undecorated_pwdb_for_dm =
-			rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-			("AP Ext Port or disconnet PWDB = 0x%x\n",
-			rtl_dm_dig->min_undecorated_pwdb_for_dm));
+		rtl_dm_dig->min_undec_pwdb_for_dm =
+			rtlpriv->dm.entry_min_undec_sm_pwdb;
+		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+			 "AP Ext Port or disconnet PWDB = 0x%x\n",
+			 rtl_dm_dig->min_undec_pwdb_for_dm);
 	}
-	RT_TRACE(COMP_DIG, DBG_LOUD, ("MinUndecoratedPWDBForDM =%d\n",
-		rtl_dm_dig->min_undecorated_pwdb_for_dm));
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "MinUndecoratedPWDBForDM =%d\n",
+		 rtl_dm_dig->min_undec_pwdb_for_dm);
 }
 
 static void rtl92ee_dm_check_rssi_monitor(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
 	struct rtl_dm *dm = rtl_dm(rtlpriv);
 	struct rtl_sta_info *drv_priv;
@@ -540,14 +549,15 @@ static void rtl92ee_dm_check_rssi_monitor(struct ieee80211_hw *hw)
 		/* AP & ADHOC & MESH */
 		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
 		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
-			struct rssi_sta *stat = &(drv_priv->rssi_stat);
-			if (stat->undecorated_smoothed_pwdb < min)
-				min = stat->undecorated_smoothed_pwdb;
-			if (stat->undecorated_smoothed_pwdb > max)
-				max = stat->undecorated_smoothed_pwdb;
+			struct rssi_sta *stat = &drv_priv->rssi_stat;
+
+			if (stat->undec_sm_pwdb < min)
+				min = stat->undec_sm_pwdb;
+			if (stat->undec_sm_pwdb > max)
+				max = stat->undec_sm_pwdb;
 
 			h2c[3] = 0;
-			h2c[2] = (u8) (dm->undecorated_smoothed_pwdb & 0xFF);
+			h2c[2] = (u8)(dm->undec_sm_pwdb & 0xFF);
 			h2c[1] = 0x20;
 			h2c[0] = ++i;
 			rtl92ee_fill_h2c_cmd(hw, H2C_92E_RSSI_REPORT, 4, h2c);
@@ -556,42 +566,42 @@ static void rtl92ee_dm_check_rssi_monitor(struct ieee80211_hw *hw)
 
 		/* If associated entry is found */
 		if (max != 0) {
-			dm->entry_max_undecoratedsmoothed_pwdb = max;
+			dm->entry_max_undec_sm_pwdb = max;
 			RTPRINT(rtlpriv, FDM, DM_PWDB,
 				"EntryMaxPWDB = 0x%lx(%ld)\n", max, max);
 		} else {
-			dm->entry_max_undecoratedsmoothed_pwdb = 0;
+			dm->entry_max_undec_sm_pwdb = 0;
 		}
 		/* If associated entry is found */
 		if (min != 0xff) {
-			dm->entry_min_undecoratedsmoothed_pwdb = min;
+			dm->entry_min_undec_sm_pwdb = min;
 			RTPRINT(rtlpriv, FDM, DM_PWDB,
 				"EntryMinPWDB = 0x%lx(%ld)\n", min, min);
 		} else {
-			dm->entry_min_undecoratedsmoothed_pwdb = 0;
+			dm->entry_min_undec_sm_pwdb = 0;
 		}
 	}
 
 	/* Indicate Rx signal strength to FW. */
-	if (dm->b_useramask) {
+	if (dm->useramask) {
 		h2c[3] = 0;
-		h2c[2] = (u8) (dm->undecorated_smoothed_pwdb & 0xFF);
+		h2c[2] = (u8)(dm->undec_sm_pwdb & 0xFF);
 		h2c[1] = 0x20;
 		h2c[0] = 0;
 		rtl92ee_fill_h2c_cmd(hw, H2C_92E_RSSI_REPORT, 4, h2c);
 	} else {
-		rtl_write_byte(rtlpriv, 0x4fe, dm->undecorated_smoothed_pwdb);
+		rtl_write_byte(rtlpriv, 0x4fe, dm->undec_sm_pwdb);
 	}
 	rtl92ee_rssi_dump_to_register(hw);
 	rtl92ee_dm_find_minimum_rssi(hw);
-	dm_dig.rssi_val_min = dm->dm_digtable.min_undecorated_pwdb_for_dm;
+	dm_dig->rssi_val_min = rtlpriv->dm_digtable.min_undec_pwdb_for_dm;
 }
 
 static void rtl92ee_dm_init_primary_cca_check(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct dynamic_primary_cca *primarycca = &(rtlpriv->primarycca);
+	struct dynamic_primary_cca *primarycca = &rtlpriv->primarycca;
 
 	rtlhal->rts_en = 0;
 	primarycca->dup_rts_flag = 0;
@@ -616,9 +626,9 @@ void rtl92ee_dm_init_edca_turbo(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	rtlpriv->dm.bcurrent_turbo_edca = false;
-	rtlpriv->dm.bis_cur_rdlstate = false;
-	rtlpriv->dm.bis_any_nonbepkts = false;
+	rtlpriv->dm.current_turbo_edca = false;
+	rtlpriv->dm.is_cur_rdlstate = false;
+	rtlpriv->dm.is_any_nonbepkts = false;
 }
 
 static void rtl92ee_dm_check_edca_turbo(struct ieee80211_hw *hw)
@@ -632,43 +642,44 @@ static void rtl92ee_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	u32 edca_be_ul = 0x5ea42b;
 	u32 edca_be_dl = 0x5ea42b; /*not sure*/
 	u32 edca_be = 0x5ea42b;
-	bool b_is_cur_rdlstate;
+	bool is_cur_rdlstate;
 	bool b_edca_turbo_on = false;
 
 	if (rtlpriv->dm.dbginfo.num_non_be_pkt > 0x100)
-		rtlpriv->dm.bis_any_nonbepkts = true;
+		rtlpriv->dm.is_any_nonbepkts = true;
 	rtlpriv->dm.dbginfo.num_non_be_pkt = 0;
 
 	cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
 	cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
 
 	/*b_bias_on_rx = false;*/
-	b_edca_turbo_on = ((!rtlpriv->dm.bis_any_nonbepkts) &&
-			   (!rtlpriv->dm.b_disable_framebursting)) ?
+	b_edca_turbo_on = ((!rtlpriv->dm.is_any_nonbepkts) &&
+			   (!rtlpriv->dm.disable_framebursting)) ?
 			  true : false;
 
 	if (rtl92ee_dm_is_edca_turbo_disable(hw))
-		goto dm_CheckEdcaTurbo_EXIT;
+		goto check_exit;
 
 	if (b_edca_turbo_on) {
-		b_is_cur_rdlstate = (cur_rxok_cnt > cur_txok_cnt * 4) ?
+		is_cur_rdlstate = (cur_rxok_cnt > cur_txok_cnt * 4) ?
 				    true : false;
 
-		edca_be = b_is_cur_rdlstate ? edca_be_dl : edca_be_ul;
+		edca_be = is_cur_rdlstate ? edca_be_dl : edca_be_ul;
 		rtl_write_dword(rtlpriv , REG_EDCA_BE_PARAM , edca_be);
-		rtlpriv->dm.bis_cur_rdlstate = b_is_cur_rdlstate;
-		rtlpriv->dm.bcurrent_turbo_edca = true;
+		rtlpriv->dm.is_cur_rdlstate = is_cur_rdlstate;
+		rtlpriv->dm.current_turbo_edca = true;
 	} else {
-		if (rtlpriv->dm.bcurrent_turbo_edca) {
+		if (rtlpriv->dm.current_turbo_edca) {
 			u8 tmp = AC0_BE;
+
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      (u8 *) (&tmp));
+						      (u8 *)(&tmp));
 		}
-		rtlpriv->dm.bcurrent_turbo_edca = false;
+		rtlpriv->dm.current_turbo_edca = false;
 	}
 
-dm_CheckEdcaTurbo_EXIT:
-	rtlpriv->dm.bis_any_nonbepkts = false;
+check_exit:
+	rtlpriv->dm.is_any_nonbepkts = false;
 	last_txok_cnt = rtlpriv->stats.txbytesunicast;
 	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
 }
@@ -677,27 +688,27 @@ static void rtl92ee_dm_dynamic_edcca(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 reg_c50 , reg_c58;
-	bool b_fw_current_in_ps_mode = false;
+	bool fw_current_in_ps_mode = false;
 
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				      (u8 *)(&b_fw_current_in_ps_mode));
-	if (b_fw_current_in_ps_mode)
+				      (u8 *)(&fw_current_in_ps_mode));
+	if (fw_current_in_ps_mode)
 		return;
 
 	reg_c50 = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
 	reg_c58 = rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
 
 	if (reg_c50 > 0x28 && reg_c58 > 0x28) {
-		if (!rtlpriv->rtlhal.b_pre_edcca_enable) {
+		if (!rtlpriv->rtlhal.pre_edcca_enable) {
 			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD, 0x03);
 			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD + 2, 0x00);
-			rtlpriv->rtlhal.b_pre_edcca_enable = true;
+			rtlpriv->rtlhal.pre_edcca_enable = true;
 		}
 	} else if (reg_c50 < 0x25 && reg_c58 < 0x25) {
-		if (rtlpriv->rtlhal.b_pre_edcca_enable) {
+		if (rtlpriv->rtlhal.pre_edcca_enable) {
 			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD, 0x7f);
 			rtl_write_byte(rtlpriv, ROFDM0_ECCATHRESHOLD + 2, 0x7f);
-			rtlpriv->rtlhal.b_pre_edcca_enable = false;
+			rtlpriv->rtlhal.pre_edcca_enable = false;
 		}
 	}
 }
@@ -707,9 +718,10 @@ static void rtl92ee_dm_adaptivity(struct ieee80211_hw *hw)
 	rtl92ee_dm_dynamic_edcca(hw);
 }
 
-static void rtl92ee_dm_write_dynamic_cca(struct ieee80211_hw *hw, u8 cur_mf_state)
+static void rtl92ee_dm_write_dynamic_cca(struct ieee80211_hw *hw,
+					 u8 cur_mf_state)
 {
-	struct dynamic_primary_cca *primarycca = &(rtl_priv(hw)->primarycca);
+	struct dynamic_primary_cca *primarycca = &rtl_priv(hw)->primarycca;
 
 	if (primarycca->mf_state != cur_mf_state)
 		rtl_set_bbreg(hw, DM_REG_L1SBD_PD_CH_11N, BIT(8) | BIT(7),
@@ -721,8 +733,8 @@ static void rtl92ee_dm_write_dynamic_cca(struct ieee80211_hw *hw, u8 cur_mf_stat
 static void rtl92ee_dm_dynamic_primary_cca_ckeck(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
-	struct dynamic_primary_cca *primarycca = &(rtlpriv->primarycca);
+	struct false_alarm_statistics *falsealm_cnt = &rtlpriv->falsealm_cnt;
+	struct dynamic_primary_cca *primarycca = &rtlpriv->primarycca;
 	bool is40mhz = false;
 	u64 ofdm_cca, ofdm_fa, bw_usc_cnt, bw_lsc_cnt;
 	u8 sec_ch_offset;
@@ -751,7 +763,8 @@ static void rtl92ee_dm_dynamic_primary_cca_ckeck(struct ieee80211_hw *hw)
 
 	if (primarycca->pricca_flag == 0) {
 		/* Primary channel is above
-		 * NOTE: duplicate CTS can remove this condition*/
+		 * NOTE: duplicate CTS can remove this condition
+		 */
 		if (sec_ch_offset == 2) {
 			if ((ofdm_cca > OFDMCCA_TH) &&
 			    (bw_lsc_cnt > (bw_usc_cnt + BW_IND_BIAS)) &&
@@ -807,7 +820,7 @@ static void rtl92ee_dm_dynamic_primary_cca_ckeck(struct ieee80211_hw *hw)
 				primarycca->dup_rts_flag = 0;
 			}
 		}
-	} else {/* PrimaryCCA->PriCCA_flag == 1 */
+	} else {/* PrimaryCCA->PriCCA_flag==1 */
 		count_down--;
 		if (count_down == 0) {
 			count_down = MONITOR_TIME;
@@ -840,10 +853,10 @@ static void rtl92ee_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
 		}
 		/* Disable CFO tracking for BT */
 		if (rtlpriv->cfg->ops->get_btc_status()) {
-			if (!rtlpriv->btcoexist.btc_ops->btc_is_bt_disabled(rtlpriv)) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_LOUD,
-					("odm_DynamicATCSwitch(): "
-					"Disable CFO tracking for BT!!\n"));
+			if (!rtlpriv->btcoexist.btc_ops->
+			    btc_is_bt_disabled(rtlpriv)) {
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
+					 "odm_DynamicATCSwitch(): Disable CFO tracking for BT!!\n");
 				return;
 			}
 		}
@@ -876,9 +889,8 @@ static void rtl92ee_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
 		if (cfo_ave_diff > 20 && rtldm->large_cfo_hit == 0) {
 			rtldm->large_cfo_hit = 1;
 			return;
-		} else {
-			rtldm->large_cfo_hit = 0;
 		}
+		rtldm->large_cfo_hit = 0;
 
 		rtldm->cfo_ave_pre = cfo_ave;
 
@@ -935,15 +947,15 @@ static void rtl92ee_dm_init_txpower_tracking(struct ieee80211_hw *hw)
 	struct rtl_dm *dm = rtl_dm(rtlpriv);
 	u8 path;
 
-	dm->btxpower_tracking = true;
+	dm->txpower_tracking = true;
 	dm->default_ofdm_index = 30;
 	dm->default_cck_index = 20;
 
-	dm->bb_swing_idx_cck_base = dm->default_cck_index;
+	dm->swing_idx_cck_base = dm->default_cck_index;
 	dm->cck_index = dm->default_cck_index;
 
 	for (path = RF90_PATH_A; path < MAX_RF_PATH; path++) {
-		dm->bb_swing_idx_ofdm_base[path] = dm->default_ofdm_index;
+		dm->swing_idx_ofdm_base[path] = dm->default_ofdm_index;
 		dm->ofdm_index[path] = dm->default_ofdm_index;
 		dm->delta_power_index[path] = 0;
 		dm->delta_power_index_last[path] = 0;
@@ -954,31 +966,30 @@ static void rtl92ee_dm_init_txpower_tracking(struct ieee80211_hw *hw)
 void rtl92ee_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
 
 	p_ra->ratr_state = DM_RATR_STA_INIT;
 	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
 
 	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
-		rtlpriv->dm.b_useramask = true;
+		rtlpriv->dm.useramask = true;
 	else
-		rtlpriv->dm.b_useramask = false;
+		rtlpriv->dm.useramask = false;
 
 	p_ra->ldpc_thres = 35;
 	p_ra->use_ldpc = false;
 	p_ra->high_rssi_thresh_for_ra = 50;
-	p_ra->low_rssi_thresh_for_ra = 20;
-
+	p_ra->low_rssi_thresh_for_ra40m = 20;
 }
 
 static bool _rtl92ee_dm_ra_state_check(struct ieee80211_hw *hw,
-				      s32 rssi, u8 *ratr_state)
+				       s32 rssi, u8 *ratr_state)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
 	const u8 go_up_gap = 5;
 	u32 high_rssithresh_for_ra = p_ra->high_rssi_thresh_for_ra;
-	u32 low_rssithresh_for_ra = p_ra->low_rssi_thresh_for_ra;
+	u32 low_rssithresh_for_ra = p_ra->low_rssi_thresh_for_ra40m;
 	u8 state;
 
 	/* Threshold Adjustment:
@@ -990,21 +1001,18 @@ static bool _rtl92ee_dm_ra_state_check(struct ieee80211_hw *hw,
 	switch (*ratr_state) {
 	case DM_RATR_STA_INIT:
 	case DM_RATR_STA_HIGH:
-			break;
-
+		break;
 	case DM_RATR_STA_MIDDLE:
-			high_rssithresh_for_ra += go_up_gap;
-			break;
-
+		high_rssithresh_for_ra += go_up_gap;
+		break;
 	case DM_RATR_STA_LOW:
-			high_rssithresh_for_ra += go_up_gap;
-			low_rssithresh_for_ra += go_up_gap;
-			break;
-
+		high_rssithresh_for_ra += go_up_gap;
+		low_rssithresh_for_ra += go_up_gap;
+		break;
 	default:
-			RT_TRACE(COMP_RATR, DBG_DMESG,
-				("wrong rssi level setting %d !", *ratr_state));
-			break;
+		RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+			 "wrong rssi level setting %d !", *ratr_state);
+		break;
 	}
 
 	/* Decide RATRState by RSSI. */
@@ -1028,36 +1036,33 @@ static void rtl92ee_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
 	struct ieee80211_sta *sta = NULL;
 
 	if (is_hal_stop(rtlhal)) {
-		RT_TRACE(COMP_RATE, DBG_LOUD,
-			 ("driver is going to unload\n"));
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver is going to unload\n");
 		return;
 	}
 
-	if (!rtlpriv->dm.b_useramask) {
-		RT_TRACE(COMP_RATE, DBG_LOUD,
-			 ("driver does not control rate adaptive mask\n"));
+	if (!rtlpriv->dm.useramask) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver does not control rate adaptive mask\n");
 		return;
 	}
 
 	if (mac->link_state == MAC80211_LINKED &&
-		mac->opmode == NL80211_IFTYPE_STATION) {
-
-		if (rtlpriv->dm.undecorated_smoothed_pwdb < p_ra->ldpc_thres) {
+	    mac->opmode == NL80211_IFTYPE_STATION) {
+		if (rtlpriv->dm.undec_sm_pwdb < p_ra->ldpc_thres) {
 			p_ra->use_ldpc = true;
 			p_ra->lower_rts_rate = true;
-		} else if (rtlpriv->dm.undecorated_smoothed_pwdb >
+		} else if (rtlpriv->dm.undec_sm_pwdb >
 			   (p_ra->ldpc_thres - 5)) {
 			p_ra->use_ldpc = false;
 			p_ra->lower_rts_rate = false;
 		}
-		if (_rtl92ee_dm_ra_state_check(hw,
-					rtlpriv->dm.undecorated_smoothed_pwdb,
-					&(p_ra->ratr_state))) {
-
+		if (_rtl92ee_dm_ra_state_check(hw, rtlpriv->dm.undec_sm_pwdb,
+					       &p_ra->ratr_state)) {
 			rcu_read_lock();
 			sta = rtl_find_sta(hw, mac->bssid);
 			if (sta)
@@ -1097,14 +1102,14 @@ void rtl92ee_dm_init(struct ieee80211_hw *hw)
 static void rtl92ee_dm_common_info_self_update(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 cnt = 0;
 	struct rtl_sta_info *drv_priv;
+	u8 cnt = 0;
 
-	rtlpriv->dm.b_one_entry_only = false;
+	rtlpriv->dm.one_entry_only = false;
 
 	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
-		rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-		rtlpriv->dm.b_one_entry_only = true;
+	    rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		rtlpriv->dm.one_entry_only = true;
 		return;
 	}
 
@@ -1118,7 +1123,7 @@ static void rtl92ee_dm_common_info_self_update(struct ieee80211_hw *hw)
 		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
 
 		if (cnt == 1)
-			rtlpriv->dm.b_one_entry_only = true;
+			rtlpriv->dm.one_entry_only = true;
 	}
 }
 
@@ -1231,18 +1236,18 @@ void rtl92ee_dm_watchdog(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool b_fw_current_inpsmode = false;
-	bool b_fw_ps_awake = true;
+	bool fw_current_inpsmode = false;
+	bool fw_ps_awake = true;
 
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				      (u8 *) (&b_fw_current_inpsmode));
+				      (u8 *)(&fw_current_inpsmode));
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
-				      (u8 *) (&b_fw_ps_awake));
+				      (u8 *)(&fw_ps_awake));
 	if (ppsc->p2p_ps_info.p2p_ps_mode)
-		b_fw_ps_awake = false;
+		fw_ps_awake = false;
 
 	if ((ppsc->rfpwr_state == ERFON) &&
-	    ((!b_fw_current_inpsmode) && b_fw_ps_awake) &&
+	    ((!fw_current_inpsmode) && fw_ps_awake) &&
 	    (!ppsc->rfchange_inprogress)) {
 		rtl92ee_dm_common_info_self_update(hw);
 		rtl92ee_dm_false_alarm_counter_statistics(hw);
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/dm.h b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.h
index 30b8fa6d3f78..881db7d6fef7 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/dm.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -58,8 +54,6 @@
 #define	DM_REG_T_METER_92D_11N			0x42
 #define	DM_REG_T_METER_92E_11N			0x42
 
-
-
 /*BB REG LIST*/
 /*PAGE 8 */
 #define	DM_REG_BB_CTRL_11N			0x800
@@ -166,7 +160,6 @@
 #define	DM_REG_SLEEP_11N			0xEE0
 #define	DM_REG_PMPD_ANAEN_11N			0xEEC
 
-
 /*MAC REG LIST*/
 #define	DM_REG_BB_RST_11N			0x02
 #define	DM_REG_ANTSEL_PIN_11N			0x4C
@@ -181,12 +174,9 @@
 #define	DM_REG_ANT_TRAIN_PARA1_11N		0x7b0
 #define	DM_REG_ANT_TRAIN_PARA2_11N		0x7b4
 
-
 /*DIG Related*/
 #define	DM_BIT_IGI_11N				0x0000007F
 
-
-
 #define HAL_DM_DIG_DISABLE			BIT(0)
 #define HAL_DM_HIPWR_DISABLE			BIT(1)
 
@@ -221,7 +211,7 @@
 #define DM_DIG_BACKOFF_MIN			-4
 #define DM_DIG_BACKOFF_DEFAULT			10
 
-#define RXPATHSELECTION_SS_TH_lOW		30
+#define RXPATHSELECTION_SS_TH_LOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
 #define DM_RATR_STA_INIT			0
@@ -232,7 +222,7 @@
 #define CTS2SELF_THVAL				30
 #define REGC38_TH				20
 
-#define WAIOTTHVal				25
+#define WAIOTTHVAL				25
 
 #define TXHIGHPWRLEVEL_NORMAL			0
 #define TXHIGHPWRLEVEL_LEVEL1			1
@@ -265,72 +255,6 @@
 #define RA_CFOLONGDUMP				0xcba
 #define RB_CFOLONGDUMP				0xcbc
 
-struct ps_t {
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-	u8 pre_rfstate;
-	u8 cur_rfstate;
-	long rssi_val_min;
-
-};
-
-struct dig_t {
-	u8 dig_enable_flag;
-	u8 dig_ext_port_stage;
-	u32 rssi_lowthresh;
-	u32 rssi_highthresh;
-
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-
-	u8 cursta_connectctate;
-	u8 presta_connectstate;
-	u8 curmultista_connectstate;
-
-	u8 pre_igvalue;
-	u8 cur_igvalue;
-	u8 backup_igvalue;
-	u8 bt30_cur_igi;
-	u8 stop_dig;
-
-	char backoff_val;
-	char backoff_val_range_max;
-	char backoff_val_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-	u8 rssi_val_min;
-
-	u8 pre_cck_cca_thres;
-	u8 cur_cck_cca_thres;
-	u8 pre_cck_pd_state;
-	u8 cur_cck_pd_state;
-
-	u8 large_fa_hit;
-	u8 forbidden_igi;
-	u32 recover_cnt;
-
-	char th_l2h_ini;
-	char th_edcca_hl_diff;
-	char igi_base;
-	u8 igi_target;
-	bool force_edcca;
-	u8 adapen_rssi;
-
-	u8 dig_dynamic_min_0;
-	u8 dig_dynamic_min_1;
-	bool b_media_connect_0;
-	bool b_media_connect_1;
-
-	u32 antdiv_rssi_max;
-	u32 rssi_max;
-};
-
-enum pwr_track_control_method {
-	BBSWING,
-	TXAGC
-};
-
-extern struct dig_t dm_dig;
 void rtl92ee_dm_init(struct ieee80211_hw *hw);
 void rtl92ee_dm_watchdog(struct ieee80211_hw *hw);
 void rtl92ee_dm_write_cck_cca_thres(struct ieee80211_hw *hw,
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/fw.c b/drivers/net/wireless/rtlwifi/rtl8192ee/fw.c
index ea6cafa80f48..45c128b91f7f 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/fw.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -26,6 +26,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "fw.h"
@@ -42,7 +43,6 @@ static void _rtl92ee_enable_fw_download(struct ieee80211_hw *hw, bool enable)
 		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL + 2);
 		rtl_write_byte(rtlpriv, REG_MCUFWDL + 2, tmp & 0xf7);
 	} else {
-
 		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
 		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
 	}
@@ -52,27 +52,27 @@ static void _rtl92ee_fw_block_write(struct ieee80211_hw *hw,
 				    const u8 *buffer, u32 size)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 blockSize = sizeof(u32);
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 *pu4BytePtr = (u32 *) buffer;
-	u32 i, offset, blockCount, remainSize;
+	u32 blocksize = sizeof(u32);
+	u8 *bufferptr = (u8 *)buffer;
+	u32 *pu4byteptr = (u32 *)buffer;
+	u32 i, offset, blockcount, remainsize;
 
-	blockCount = size / blockSize;
-	remainSize = size % blockSize;
+	blockcount = size / blocksize;
+	remainsize = size % blocksize;
 
-	for (i = 0; i < blockCount; i++) {
-		offset = i * blockSize;
+	for (i = 0; i < blockcount; i++) {
+		offset = i * blocksize;
 		rtl_write_dword(rtlpriv, (FW_8192C_START_ADDRESS + offset),
-				*(pu4BytePtr + i));
+				*(pu4byteptr + i));
 	}
 
-	if (remainSize) {
-		offset = blockCount * blockSize;
-		bufferPtr += offset;
-		for (i = 0; i < remainSize; i++) {
+	if (remainsize) {
+		offset = blockcount * blocksize;
+		bufferptr += offset;
+		for (i = 0; i < remainsize; i++) {
 			rtl_write_byte(rtlpriv,
 				       (FW_8192C_START_ADDRESS + offset + i),
-				       *(bufferPtr + i));
+				       *(bufferptr + i));
 		}
 	}
 }
@@ -82,7 +82,7 @@ static void _rtl92ee_fw_page_write(struct ieee80211_hw *hw, u32 page,
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 value8;
-	u8 u8page = (u8) (page & 0x07);
+	u8 u8page = (u8)(page & 0x07);
 
 	value8 = (rtl_read_byte(rtlpriv, REG_MCUFWDL + 2) & 0xF8) | u8page;
 	rtl_write_byte(rtlpriv, (REG_MCUFWDL + 2), value8);
@@ -93,7 +93,7 @@ static void _rtl92ee_fw_page_write(struct ieee80211_hw *hw, u32 page,
 static void _rtl92ee_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
 {
 	u32 fwlen = *pfwlen;
-	u8 remain = (u8) (fwlen % 4);
+	u8 remain = (u8)(fwlen % 4);
 
 	remain = (remain == 0) ? 0 : (4 - remain);
 
@@ -111,36 +111,35 @@ static void _rtl92ee_write_fw(struct ieee80211_hw *hw,
 			      u8 *buffer, u32 size)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 pageNums, remainSize;
+	u8 *bufferptr = (u8 *)buffer;
+	u32 pagenums, remainsize;
 	u32 page, offset;
 
-	RT_TRACE(COMP_FW, DBG_LOUD , ("FW size is %d bytes,\n", size));
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD , "FW size is %d bytes,\n", size);
 
-	_rtl92ee_fill_dummy(bufferPtr, &size);
+	_rtl92ee_fill_dummy(bufferptr, &size);
 
-	pageNums = size / FW_8192C_PAGE_SIZE;
-	remainSize = size % FW_8192C_PAGE_SIZE;
+	pagenums = size / FW_8192C_PAGE_SIZE;
+	remainsize = size % FW_8192C_PAGE_SIZE;
 
-	if (pageNums > 8) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Page numbers should not greater then 8\n"));
+	if (pagenums > 8) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Page numbers should not greater then 8\n");
 	}
 
-	for (page = 0; page < pageNums; page++) {
+	for (page = 0; page < pagenums; page++) {
 		offset = page * FW_8192C_PAGE_SIZE;
-		_rtl92ee_fw_page_write(hw, page, (bufferPtr + offset),
-				      FW_8192C_PAGE_SIZE);
+		_rtl92ee_fw_page_write(hw, page, (bufferptr + offset),
+				       FW_8192C_PAGE_SIZE);
 		udelay(2);
 	}
 
-	if (remainSize) {
-		offset = pageNums * FW_8192C_PAGE_SIZE;
-		page = pageNums;
-		_rtl92ee_fw_page_write(hw, page, (bufferPtr + offset),
-				       remainSize);
+	if (remainsize) {
+		offset = pagenums * FW_8192C_PAGE_SIZE;
+		page = pagenums;
+		_rtl92ee_fw_page_write(hw, page, (bufferptr + offset),
+				       remainsize);
 	}
-
 }
 
 static int _rtl92ee_fw_free_to_go(struct ieee80211_hw *hw)
@@ -153,17 +152,17 @@ static int _rtl92ee_fw_free_to_go(struct ieee80211_hw *hw)
 	do {
 		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 	} while ((counter++ < FW_8192C_POLLING_TIMEOUT_COUNT) &&
-		 (!(value32 & FWDL_ChkSum_rpt)));
+		 (!(value32 & FWDL_CHKSUM_RPT)));
 
 	if (counter >= FW_8192C_POLLING_TIMEOUT_COUNT) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("chksum report faill ! REG_MCUFWDL:0x%08x .\n",
-			  value32));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "chksum report faill ! REG_MCUFWDL:0x%08x .\n",
+			  value32);
 		goto exit;
 	}
 
-	RT_TRACE(COMP_FW, DBG_TRACE,
-		 ("Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32));
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+		 "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
 
 	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 	value32 |= MCUFWDL_RDY;
@@ -176,9 +175,9 @@ static int _rtl92ee_fw_free_to_go(struct ieee80211_hw *hw)
 	do {
 		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 		if (value32 & WINTINI_RDY) {
-			RT_TRACE(COMP_FW, DBG_LOUD ,
-				("Polling FW ready success!! REG_MCUFWDL:"
-				"0x%08x. count = %d\n", value32, counter));
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD ,
+				 "Polling FW ready success!! REG_MCUFWDL:0x%08x. count = %d\n",
+				 value32, counter);
 			err = 0;
 			goto exit;
 		}
@@ -187,9 +186,9 @@ static int _rtl92ee_fw_free_to_go(struct ieee80211_hw *hw)
 
 	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
 
-	RT_TRACE(COMP_ERR, DBG_EMERG,
-		 ("Polling FW ready fail!! REG_MCUFWDL:0x%08x. count = %d\n",
-		 value32, counter));
+	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x. count = %d\n",
+		 value32, counter);
 
 exit:
 	return err;
@@ -211,26 +210,26 @@ int rtl92ee_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
 	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
 	rtlhal->fw_version = pfwheader->version;
 	rtlhal->fw_subversion = pfwheader->subversion;
-	pfwdata = (u8 *) rtlhal->pfirmware;
+	pfwdata = (u8 *)rtlhal->pfirmware;
 	fwsize = rtlhal->fwsize;
-	RT_TRACE(COMP_FW, DBG_DMESG,
-		 ("normal Firmware SIZE %d\n" , fwsize));
+	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+		 "normal Firmware SIZE %d\n" , fwsize);
 
 	if (IS_FW_HEADER_EXIST(pfwheader)) {
-		RT_TRACE(COMP_FW, DBG_DMESG,
-			 ("Firmware Version(%d), Signature(%#x), Size(%d)\n",
+		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+			 "Firmware Version(%d), Signature(%#x),Size(%d)\n",
 			  pfwheader->version, pfwheader->signature,
-			  (int)sizeof(struct rtl92c_firmware_header)));
+			  (int)sizeof(struct rtl92c_firmware_header));
 
 		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
 		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
 	} else {
-		RT_TRACE(COMP_FW, DBG_DMESG,
-			 ("Firmware no Header, Signature(%#x)\n",
-			  pfwheader->signature));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+			 "Firmware no Header, Signature(%#x)\n",
+			  pfwheader->signature);
 	}
 
-	if (rtlhal->b_mac_func_enable) {
+	if (rtlhal->mac_func_enable) {
 		if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
 			rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
 			rtl92ee_firmware_selfreset(hw);
@@ -242,11 +241,11 @@ int rtl92ee_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
 
 	err = _rtl92ee_fw_free_to_go(hw);
 	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Firmware is not ready to run!\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Firmware is not ready to run!\n");
 	} else {
-		RT_TRACE(COMP_FW, DBG_LOUD ,
-			 ("Firmware is ready to run!\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD ,
+			 "Firmware is ready to run!\n");
 	}
 
 	return 0;
@@ -265,7 +264,7 @@ static bool _rtl92ee_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
 }
 
 static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
-				      u32 cmd_len, u8 *p_cmdbuffer)
+				      u32 cmd_len, u8 *cmdbuffer)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -283,32 +282,31 @@ static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 	u8 idx;
 
 	if (ppsc->dot11_psmode != EACTIVE ||
-		ppsc->inactive_pwrstate == ERFOFF) {
-		RT_TRACE(COMP_CMD, DBG_LOUD ,
-			("FillH2CCommand8192E(): "
-			"Return because RF is off!!!\n"));
+	    ppsc->inactive_pwrstate == ERFOFF) {
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+			 "FillH2CCommand8192E(): Return because RF is off!!!\n");
 		return;
 	}
 
-	RT_TRACE(COMP_CMD, DBG_LOUD , ("come in\n"));
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD , "come in\n");
 
 	/* 1. Prevent race condition in setting H2C cmd.
 	 * (copy from MgntActSet_RF_State().)
 	 */
 	while (true) {
 		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
-		if (rtlhal->b_h2c_setinprogress) {
-			RT_TRACE(COMP_CMD, DBG_LOUD ,
-				 ("H2C set in progress! Wait to set.."
-				  "element_id(%d).\n", element_id));
+		if (rtlhal->h2c_setinprogress) {
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+				 "H2C set in progress! Wait to set..element_id(%d).\n",
+				  element_id);
 
-			while (rtlhal->b_h2c_setinprogress) {
+			while (rtlhal->h2c_setinprogress) {
 				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
 						       flag);
 				h2c_waitcounter++;
-				RT_TRACE(COMP_CMD, DBG_LOUD ,
-					 ("Wait 100 us (%d times)...\n",
-					  h2c_waitcounter));
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+					 "Wait 100 us (%d times)...\n",
+					  h2c_waitcounter);
 				udelay(100);
 
 				if (h2c_waitcounter > 1000)
@@ -318,22 +316,13 @@ static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 			}
 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 		} else {
-			rtlhal->b_h2c_setinprogress = true;
+			rtlhal->h2c_setinprogress = true;
 			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 			break;
 		}
 	}
 
 	while (!bwrite_sucess) {
-		/*	cosa remove this because never reach this. */
-		/*wait_writeh2c_limmit--;
-		if (wait_writeh2c_limmit == 0) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Write H2C fail because no trigger "
-				  "for FW INT!\n"));
-			break;
-		}
-		*/
 		/* 2. Find the last BOX number which has been writen. */
 		boxnum = rtlhal->last_hmeboxnum;
 		switch (boxnum) {
@@ -354,8 +343,8 @@ static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 			box_extreg = REG_HMEBOX_EXT_3;
 			break;
 		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("switch case not process\n"));
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
@@ -371,51 +360,51 @@ static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 				rtl_write_byte(rtlpriv, REG_SYS_CFG1 + 3, 0xff);
 		}
 
-		if (isfw_read == true) {
+		if (isfw_read) {
 			wait_h2c_limmit = 100;
 			isfw_read = _rtl92ee_check_fw_read_last_h2c(hw, boxnum);
 			while (!isfw_read) {
 				wait_h2c_limmit--;
 				if (wait_h2c_limmit == 0) {
-					RT_TRACE(COMP_CMD, DBG_LOUD ,
-						("Wating too long for FW"
-						"read clear HMEBox(%d)!!!\n",
-						boxnum));
+					RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+						 "Waiting too long for FW read clear HMEBox(%d)!!!\n",
+						 boxnum);
 					break;
 				}
 				udelay(10);
-				isfw_read = _rtl92ee_check_fw_read_last_h2c(hw,
-									boxnum);
+				isfw_read =
+				  _rtl92ee_check_fw_read_last_h2c(hw, boxnum);
 				u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
-				RT_TRACE(COMP_CMD, DBG_LOUD ,
-					 ("Wating for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
-					 boxnum, u1b_tmp));
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+					 "Waiting for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
+					 boxnum, u1b_tmp);
 			}
 		}
 
 		/* If Fw has not read the last
-		 H2C cmd, break and give up this H2C. */
+		 * H2C cmd, break and give up this H2C.
+		 */
 		if (!isfw_read) {
-			RT_TRACE(COMP_CMD, DBG_LOUD ,
-				 ("Write H2C reg BOX[%d] fail, Fw don't read.\n",
-				 boxnum));
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+				 "Write H2C reg BOX[%d] fail,Fw don't read.\n",
+				 boxnum);
 			break;
 		}
 		/* 4. Fill the H2C cmd into box */
 		memset(boxcontent, 0, sizeof(boxcontent));
 		memset(boxextcontent, 0, sizeof(boxextcontent));
 		boxcontent[0] = element_id;
-		RT_TRACE(COMP_CMD, DBG_LOUD ,
-			 ("Write element_id box_reg(%4x) = %2x\n",
-			  box_reg, element_id));
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+			 "Write element_id box_reg(%4x) = %2x\n",
+			  box_reg, element_id);
 
 		switch (cmd_len) {
 		case 1:
 		case 2:
 		case 3:
 			/*boxcontent[0] &= ~(BIT(7));*/
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + buf_index, cmd_len);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, cmd_len);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -427,10 +416,10 @@ static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 		case 6:
 		case 7:
 			/*boxcontent[0] |= (BIT(7));*/
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer + buf_index+3, cmd_len-3);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + buf_index, 3);
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index+3, cmd_len-3);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 3);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
@@ -443,8 +432,8 @@ static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 			}
 			break;
 		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("switch case not process\n"));
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
@@ -454,35 +443,33 @@ static void _rtl92ee_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 		if (rtlhal->last_hmeboxnum == 4)
 			rtlhal->last_hmeboxnum = 0;
 
-		RT_TRACE(COMP_CMD, DBG_LOUD ,
-			 ("pHalData->last_hmeboxnum  = %d\n",
-			  rtlhal->last_hmeboxnum));
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD ,
+			 "pHalData->last_hmeboxnum  = %d\n",
+			  rtlhal->last_hmeboxnum);
 	}
 
 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
-	rtlhal->b_h2c_setinprogress = false;
+	rtlhal->h2c_setinprogress = false;
 	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
 
-	RT_TRACE(COMP_CMD, DBG_LOUD , ("go out\n"));
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD , "go out\n");
 }
 
 void rtl92ee_fill_h2c_cmd(struct ieee80211_hw *hw,
-			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+			  u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 {
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 tmp_cmdbuf[2];
 
-	if (rtlhal->bfw_ready == false) {
-		RT_ASSERT(false, ("return H2C cmd because of Fw "
-				  "download fail!!!\n"));
+	if (!rtlhal->fw_ready) {
+		RT_ASSERT(false,
+			  "return H2C cmd because of Fw download fail!!!\n");
 		return;
 	}
 
 	memset(tmp_cmdbuf, 0, 8);
-	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
+	memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
 	_rtl92ee_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
-
-	return;
 }
 
 void rtl92ee_firmware_selfreset(struct ieee80211_hw *hw)
@@ -504,8 +491,8 @@ void rtl92ee_firmware_selfreset(struct ieee80211_hw *hw)
 	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp | BIT(2)));
 
-	RT_TRACE(COMP_INIT, DBG_LOUD ,
-		 ("  _8051Reset92E(): 8051 reset success .\n"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD ,
+		 "  _8051Reset92E(): 8051 reset success .\n");
 }
 
 void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
@@ -514,10 +501,11 @@ void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 	u8 u1_h2c_set_pwrmode[H2C_92E_PWEMODE_LENGTH] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	u8 rlbm , power_state = 0;
-	RT_TRACE(COMP_POWER, DBG_LOUD , ("FW LPS mode = %d\n", mode));
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD , "FW LPS mode = %d\n", mode);
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
-	rlbm = 0;/*YJ, temp, 120316. FW now not support RLBM = 2.*/
+	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
 	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
 					 (rtlpriv->mac80211.p2p) ?
@@ -536,17 +524,17 @@ void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 		      u1_h2c_set_pwrmode, H2C_92E_PWEMODE_LENGTH);
 	rtl92ee_fill_h2c_cmd(hw, H2C_92E_SETPWRMODE, H2C_92E_PWEMODE_LENGTH,
 			     u1_h2c_set_pwrmode);
-
 }
 
 void rtl92ee_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus)
 {
 	u8 parm[3] = { 0 , 0 , 0 };
-	/* parm[0]: bit0 = 0-->Disconnect, bit0 = 1-->Connect
-	 *          bit1 = 0-->update Media Status to MACID
-	 *          bit1 = 1-->update Media Status from MACID to MACID_End
+	/* parm[0]: bit0=0-->Disconnect, bit0=1-->Connect
+	 *          bit1=0-->update Media Status to MACID
+	 *          bit1=1-->update Media Status from MACID to MACID_End
 	 * parm[1]: MACID, if this is INFRA_STA, MacID = 0
-	 * parm[2]: MACID_End*/
+	 * parm[2]: MACID_End
+	 */
 
 	SET_H2CCMD_MSRRPT_PARM_OPMODE(parm, mstatus);
 	SET_H2CCMD_MSRRPT_PARM_MACID_IND(parm, 0);
@@ -554,35 +542,6 @@ void rtl92ee_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus)
 	rtl92ee_fill_h2c_cmd(hw, H2C_92E_MSRRPT, 3, parm);
 }
 
-static bool _rtl92ee_cmd_send_packet(struct ieee80211_hw *hw,
-				     struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	unsigned long flags;
-	struct sk_buff *pskb = NULL;
-
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-	/*this is wrong, fill_tx_cmddesc needs update*/
-	pdesc = &ring->desc[0];
-
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
-
-	__skb_queue_tail(&ring->queue, skb);
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	return true;
-}
-
 #define BEACON_PG		0 /* ->1 */
 #define PSPOLL_PG		2
 #define NULL_PG			3
@@ -590,8 +549,6 @@ static bool _rtl92ee_cmd_send_packet(struct ieee80211_hw *hw,
 
 #define TOTAL_RESERVED_PKT_LEN	768
 
-
-
 static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 	/* page 0 beacon */
 	0x80, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF,
@@ -702,8 +659,6 @@ static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 };
 
-
-
 void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -712,7 +667,7 @@ void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 
 	u32 totalpacketlen;
 	bool rtstatus;
-	u8 u1RsvdPageLoc[5] = { 0 };
+	u8 u1rsvdpageloc[5] = { 0 };
 	bool b_dlok = false;
 
 	u8 *beacon;
@@ -720,41 +675,45 @@ void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 	u8 *nullfunc;
 	u8 *p_probersp;
 	/*---------------------------------------------------------
-				(1) beacon
-	---------------------------------------------------------*/
+	 *			(1) beacon
+	 *---------------------------------------------------------
+	 */
 	beacon = &reserved_page_packet[BEACON_PG * 128];
 	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
 
 	/*-------------------------------------------------------
-				(2) ps-poll
-	--------------------------------------------------------*/
+	 *			(2) ps-poll
+	 *--------------------------------------------------------
+	 */
 	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
 	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
 	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
 	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 
 	/*--------------------------------------------------------
-				(3) null data
-	---------------------------------------------------------*/
+	 *			(3) null data
+	 *---------------------------------------------------------
+	 */
 	nullfunc = &reserved_page_packet[NULL_PG * 128];
 	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
 	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 
 	/*---------------------------------------------------------
-				(4) probe response
-	----------------------------------------------------------*/
+	 *			(4) probe response
+	 *----------------------------------------------------------
+	 */
 	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
 	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
 	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 
 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 
@@ -763,28 +722,28 @@ void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 		      &reserved_page_packet[0], totalpacketlen);
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD ,
 		      "rtl92ee_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
-		      u1RsvdPageLoc, 3);
-
+		      u1rsvdpageloc, 3);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	memcpy((u8 *)skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
-	rtstatus = _rtl92ee_cmd_send_packet(hw, skb);
+	rtstatus = rtl_cmd_send_packet(hw, skb);
 
 	if (rtstatus)
 		b_dlok = true;
 
 	if (b_dlok) {
-		RT_TRACE(COMP_POWER, DBG_LOUD ,
-			 ("Set RSVD page location to Fw.\n"));
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD ,
+			 "Set RSVD page location to Fw.\n");
 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD ,
-			      "H2C_RSVDPAGE:\n", u1RsvdPageLoc, 3);
+			      "H2C_RSVDPAGE:\n", u1rsvdpageloc, 3);
 		rtl92ee_fill_h2c_cmd(hw, H2C_92E_RSVDPAGE,
-				     sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
-	} else
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Set RSVD page location to Fw FAIL!!!!!!.\n"));
+				     sizeof(u1rsvdpageloc), u1rsvdpageloc);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
+	}
 }
 
 /*Shoud check FW support p2p or not.*/
@@ -793,7 +752,6 @@ static void rtl92ee_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
 	u8 u1_ctwindow_period[1] = {ctwindow};
 
 	rtl92ee_fill_h2c_cmd(hw, H2C_92E_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
-
 }
 
 void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
@@ -801,7 +759,7 @@ void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
+	struct rtl_p2p_ps_info *p2pinfo = &rtlps->p2p_ps_info;
 	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
 	u8 i;
 	u16 ctwindow;
@@ -809,14 +767,14 @@ void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 
 	switch (p2p_ps_state) {
 	case P2P_PS_DISABLE:
-		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_DISABLE\n"));
-		memset(p2p_ps_offload, 0, 1);
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD , "P2P_PS_DISABLE\n");
+		memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
 		break;
 	case P2P_PS_ENABLE:
-		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_ENABLE\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD , "P2P_PS_ENABLE\n");
 		/* update CTWindow value. */
 		if (p2pinfo->ctwindow > 0) {
-			p2p_ps_offload->CTWindow_En = 1;
+			p2p_ps_offload->ctwindow_en = 1;
 			ctwindow = p2pinfo->ctwindow;
 			rtl92ee_set_p2p_ctw_period_cmd(hw, ctwindow);
 		}
@@ -825,16 +783,16 @@ void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 			/* To control the register setting for which NOA*/
 			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
 			if (i == 0)
-				p2p_ps_offload->NoA0_En = 1;
+				p2p_ps_offload->noa0_en = 1;
 			else
-				p2p_ps_offload->NoA1_En = 1;
+				p2p_ps_offload->noa1_en = 1;
 			/* config P2P NoA Descriptor Register */
 			rtl_write_dword(rtlpriv, 0x5E0,
 					p2pinfo->noa_duration[i]);
 			rtl_write_dword(rtlpriv, 0x5E4,
 					p2pinfo->noa_interval[i]);
 
-			/*Get Current TSF value */
+			/*Get Current TSF value */
 			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 
 			start_time = p2pinfo->noa_start_time[i];
@@ -852,11 +810,11 @@ void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
 			/* rst p2p circuit */
 			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
-			p2p_ps_offload->Offload_En = 1;
+			p2p_ps_offload->offload_en = 1;
 
 			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
 				p2p_ps_offload->role = 1;
-				p2p_ps_offload->AllStaSleep = 0;
+				p2p_ps_offload->allstasleep = 0;
 			} else {
 				p2p_ps_offload->role = 0;
 			}
@@ -864,21 +822,19 @@ void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 		}
 		break;
 	case P2P_PS_SCAN:
-		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_SCAN\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD , "P2P_PS_SCAN\n");
 		p2p_ps_offload->discovery = 1;
 		break;
 	case P2P_PS_SCAN_DONE:
-		RT_TRACE(COMP_FW, DBG_LOUD , ("P2P_PS_SCAN_DONE\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD , "P2P_PS_SCAN_DONE\n");
 		p2p_ps_offload->discovery = 0;
 		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
 		break;
 	default:
 		break;
 	}
-
 	rtl92ee_fill_h2c_cmd(hw, H2C_92E_P2P_PS_OFFLOAD, 1,
 			     (u8 *)p2p_ps_offload);
-
 }
 
 static void _rtl92ee_c2h_ra_report_handler(struct ieee80211_hw *hw,
@@ -897,28 +853,33 @@ static void _rtl92ee_c2h_content_parsing(struct ieee80211_hw *hw, u8 c2h_cmd_id,
 
 	switch (c2h_cmd_id) {
 	case C2H_8192E_DBG:
-		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8723BE_DBG!!\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8723BE_DBG!!\n");
 		break;
 	case C2H_8192E_TXBF:
-		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8192E_TXBF!!\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8192E_TXBF!!\n");
 		break;
 	case C2H_8192E_TX_REPORT:
-		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8723BE_TX_REPORT!\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE ,
+			 "[C2H], C2H_8723BE_TX_REPORT!\n");
 		break;
 	case C2H_8192E_BT_INFO:
-		RT_TRACE(COMP_FW, DBG_TRACE , ("[C2H], C2H_8723BE_BT_INFO!!\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8723BE_BT_INFO!!\n");
 		rtlpriv->btcoexist.btc_ops->btc_btinfo_notify(rtlpriv, tmp_buf,
 							      c2h_cmd_len);
 		break;
 	case C2H_8192E_BT_MP:
-		RT_TRACE(COMP_FW, DBG_TRACE, ("[C2H], C2H_8723BE_BT_MP!!\n"));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8723BE_BT_MP!!\n");
 		break;
 	case C2H_8192E_RA_RPT:
 		_rtl92ee_c2h_ra_report_handler(hw, tmp_buf, c2h_cmd_len);
 		break;
 	default:
-		RT_TRACE(COMP_FW, DBG_TRACE,
-			 ("[C2H], Unkown packet!! CmdId(%#X)!\n", c2h_cmd_id));
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], Unkown packet!! CmdId(%#X)!\n", c2h_cmd_id);
 		break;
 	}
 }
@@ -934,9 +895,9 @@ void rtl92ee_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 len)
 	c2h_cmd_len = len - 2;
 	tmp_buf = buffer + 2;
 
-	RT_TRACE(COMP_FW, DBG_TRACE,
-		("[C2H packet], c2hCmdId = 0x%x, c2hCmdSeq = 0x%x, c2hCmdLen =%d\n",
-		c2h_cmd_id, c2h_cmd_seq, c2h_cmd_len));
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+		 "[C2H packet], c2hCmdId=0x%x, c2hCmdSeq=0x%x, c2hCmdLen=%d\n",
+		 c2h_cmd_id, c2h_cmd_seq, c2h_cmd_len);
 
 	RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_TRACE,
 		      "[C2H packet], Content Hex:\n", tmp_buf, c2h_cmd_len);
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/fw.h b/drivers/net/wireless/rtlwifi/rtl8192ee/fw.h
index 143992d5f622..3e2a48e5fb4d 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/fw.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -54,7 +50,6 @@
 #define H2C_92E_AOAC_GLOBAL_INFO_LEN		2
 #define H2C_92E_AOAC_RSVDPAGE_LOC_LEN		7
 
-
 /* Fw PS state for RPWM.
 *BIT[2:0] = HW state
 *BIT[3] = Protocol PS state,  1: register active state, 0: register sleep state
@@ -88,8 +83,8 @@
 
 #define	FW_PS_IS_ACK(x)		((x) & FW_PS_ACK)
 
-#define	IS_IN_LOW_POWER_STATE_92E(FwPSState)		\
-	(FW_PS_STATE(FwPSState) == FW_PS_CLOCK_OFF)
+#define	IS_IN_LOW_POWER_STATE_92E(__state)		\
+	(FW_PS_STATE(__state) == FW_PS_CLOCK_OFF)
 
 #define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
 #define	FW_PWR_STATE_RF_OFF	0
@@ -105,7 +100,7 @@ struct rtl92c_firmware_header {
 	u8 date;
 	u8 hour;
 	u8 minute;
-	u16 ramcodeSize;
+	u16 ramcodesize;
 	u16 rsvd2;
 	u32 svnindex;
 	u32 rsvd3;
@@ -113,7 +108,7 @@ struct rtl92c_firmware_header {
 	u32 rsvd5;
 };
 
-enum rtl8192c_h2c_cmd {
+enum rtl8192e_h2c_cmd {
 	H2C_92E_RSVDPAGE = 0,
 	H2C_92E_MSRRPT = 1,
 	H2C_92E_SCAN = 2,
@@ -168,18 +163,18 @@ enum rtl8192e_c2h_evt {
 
 #define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
 	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_H2CCMD_PWRMODE_PARM_RLBM(__pH2CCmd, __val)			\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 4, __val)
-#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__pH2CCmd, __val)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 4, 4, __val)
-#define SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(__pH2CCmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __val)
-#define SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__pH2CCmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __val)
-#define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__pH2CCmd, __val)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+4, 0, 8, __val)
-#define GET_92E_H2CCMD_PWRMODE_PARM_MODE(__pH2CCmd)			\
-	LE_BITS_TO_1BYTE(__pH2CCmd, 0, 8)
+#define SET_H2CCMD_PWRMODE_PARM_RLBM(__cmd, __val)			\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 4, __val)
+#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__cmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 4, 4, __val)
+#define SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(__cmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__cmd, __val)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__cmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+4, 0, 8, __val)
+#define GET_92E_H2CCMD_PWRMODE_PARM_MODE(__cmd)			\
+	LE_BITS_TO_1BYTE(__cmd, 0, 8)
 
 #define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
@@ -191,23 +186,23 @@ enum rtl8192e_c2h_evt {
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
 
 /* _MEDIA_STATUS_RPT_PARM_CMD1 */
-#define SET_H2CCMD_MSRRPT_PARM_OPMODE(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
-#define SET_H2CCMD_MSRRPT_PARM_MACID_IND(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
-#define SET_H2CCMD_MSRRPT_PARM_MACID(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd+1, 0, 8, __Value)
-#define SET_H2CCMD_MSRRPT_PARM_MACID_END(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd+2, 0, 8, __Value)
-
+#define SET_H2CCMD_MSRRPT_PARM_OPMODE(__cmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __val)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_IND(__cmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __val)
+#define SET_H2CCMD_MSRRPT_PARM_MACID(__cmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__cmd+1, 0, 8, __val)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_END(__cmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__cmd+2, 0, 8, __val)
 
 int rtl92ee_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw);
 void rtl92ee_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
-			  u32 cmd_len, u8 *p_cmdbuffer);
+			  u32 cmd_len, u8 *cmdbuffer);
 void rtl92ee_firmware_selfreset(struct ieee80211_hw *hw);
 void rtl92ee_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
 void rtl92ee_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus);
 void rtl92ee_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
 void rtl92ee_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
 void rtl92ee_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 len);
+
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/hw.c b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
index 26af119e2ca4..dfdc9b20e4ad 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -37,7 +37,7 @@
 #include "fw.h"
 #include "led.h"
 #include "hw.h"
-#include "pwrseqcmd.h"
+#include "../pwrseqcmd.h"
 #include "pwrseq.h"
 
 #define LLT_CONFIG	5
@@ -51,7 +51,7 @@ static void _rtl92ee_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
 	rtlpci->reg_bcn_ctrl_val |= set_bits;
 	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
 
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlpci->reg_bcn_ctrl_val);
 }
 
 static void _rtl92ee_stop_tx_beacon(struct ieee80211_hw *hw)
@@ -90,7 +90,9 @@ static void _rtl92ee_return_beacon_queue_skb(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+	unsigned long flags;
 
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 	while (skb_queue_len(&ring->queue)) {
 		struct rtl_tx_buffer_desc *entry =
 						&ring->buffer_desc[ring->idx];
@@ -98,38 +100,40 @@ static void _rtl92ee_return_beacon_queue_skb(struct ieee80211_hw *hw)
 
 		pci_unmap_single(rtlpci->pdev,
 				 rtlpriv->cfg->ops->get_desc(
-				 (u8 *) entry, true, HW_DESC_TXBUFF_ADDR),
+				 (u8 *)entry, true, HW_DESC_TXBUFF_ADDR),
 				 skb->len, PCI_DMA_TODEVICE);
 		kfree_skb(skb);
 		ring->idx = (ring->idx + 1) % ring->entries;
 	}
-
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
 }
+
 static void _rtl92ee_disable_bcn_sub_func(struct ieee80211_hw *hw)
 {
 	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(1), 0);
 }
 
 static void _rtl92ee_set_fw_clock_on(struct ieee80211_hw *hw,
-	u8 rpwm_val, bool b_need_turn_off_ckk)
+				     u8 rpwm_val, bool b_need_turn_off_ckk)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	bool b_support_remote_wake_up;
-	u32 count = 0 , isr_regaddr , content;
+	u32 count = 0, isr_regaddr, content;
 	bool b_schedule_timer = b_need_turn_off_ckk;
+
 	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
-				      (u8 *) (&b_support_remote_wake_up));
+				      (u8 *)(&b_support_remote_wake_up));
 
-	if (!rtlhal->bfw_ready)
+	if (!rtlhal->fw_ready)
 		return;
-	if (!rtlpriv->psc.b_fw_current_inpsmode)
+	if (!rtlpriv->psc.fw_current_inpsmode)
 		return;
 
 	while (1) {
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		if (rtlhal->bfw_clk_change_in_progress) {
-			while (rtlhal->bfw_clk_change_in_progress) {
+		if (rtlhal->fw_clk_change_in_progress) {
+			while (rtlhal->fw_clk_change_in_progress) {
 				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
 				count++;
 				udelay(100);
@@ -139,7 +143,7 @@ static void _rtl92ee_set_fw_clock_on(struct ieee80211_hw *hw,
 			}
 			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
 		} else {
-			rtlhal->bfw_clk_change_in_progress = false;
+			rtlhal->fw_clk_change_in_progress = false;
 			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
 			break;
 		}
@@ -147,7 +151,7 @@ static void _rtl92ee_set_fw_clock_on(struct ieee80211_hw *hw,
 
 	if (IS_IN_LOW_POWER_STATE_92E(rtlhal->fw_ps_state)) {
 		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_SET_RPWM,
-					      (u8 *) (&rpwm_val));
+					      (u8 *)(&rpwm_val));
 		if (FW_PS_IS_ACK(rpwm_val)) {
 			isr_regaddr = REG_HISR;
 			content = rtl_read_dword(rtlpriv, isr_regaddr);
@@ -158,29 +162,26 @@ static void _rtl92ee_set_fw_clock_on(struct ieee80211_hw *hw,
 			}
 
 			if (content & IMR_CPWM) {
-				rtl_write_word(rtlpriv , isr_regaddr, 0x0100);
+				rtl_write_word(rtlpriv, isr_regaddr, 0x0100);
 				rtlhal->fw_ps_state = FW_PS_STATE_RF_ON_92E;
-				RT_TRACE(COMP_POWER, DBG_LOUD,
-					 ("Receive CPWM INT!!! PSState = %X\n",
-					 rtlhal->fw_ps_state));
+				RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+					 "Receive CPWM INT!!! PSState = %X\n",
+					 rtlhal->fw_ps_state);
 			}
 		}
 
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		rtlhal->bfw_clk_change_in_progress = false;
+		rtlhal->fw_clk_change_in_progress = false;
 		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
 		if (b_schedule_timer) {
 			mod_timer(&rtlpriv->works.fw_clockoff_timer,
 				  jiffies + MSECS(10));
 		}
-
 	} else  {
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		rtlhal->bfw_clk_change_in_progress = false;
+		rtlhal->fw_clk_change_in_progress = false;
 		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
 	}
-
-
 }
 
 static void _rtl92ee_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
@@ -193,14 +194,14 @@ static void _rtl92ee_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
 	bool b_schedule_timer = false;
 	u8 queue;
 
-	if (!rtlhal->bfw_ready)
+	if (!rtlhal->fw_ready)
 		return;
-	if (!rtlpriv->psc.b_fw_current_inpsmode)
+	if (!rtlpriv->psc.fw_current_inpsmode)
 		return;
-	if (!rtlhal->ballow_sw_to_change_hwclc)
+	if (!rtlhal->allow_sw_to_change_hwclc)
 		return;
 
-	rtlpriv->cfg->ops->get_hw_reg(hw , HW_VAR_RF_STATE , (u8 *)(&rtstate));
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE, (u8 *)(&rtstate));
 	if (rtstate == ERFOFF || rtlpriv->psc.inactive_pwrstate == ERFOFF)
 		return;
 
@@ -220,15 +221,15 @@ static void _rtl92ee_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
 
 	if (FW_PS_STATE(rtlhal->fw_ps_state) != FW_PS_STATE_RF_OFF_LOW_PWR) {
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		if (!rtlhal->bfw_clk_change_in_progress) {
-			rtlhal->bfw_clk_change_in_progress = true;
+		if (!rtlhal->fw_clk_change_in_progress) {
+			rtlhal->fw_clk_change_in_progress = true;
 			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
 			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
 			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-						      (u8 *) (&rpwm_val));
+						      (u8 *)(&rpwm_val));
 			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-			rtlhal->bfw_clk_change_in_progress = false;
+			rtlhal->fw_clk_change_in_progress = false;
 			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
 		} else {
 			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
@@ -241,6 +242,7 @@ static void _rtl92ee_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
 static void _rtl92ee_set_fw_ps_rf_on(struct ieee80211_hw *hw)
 {
 	u8 rpwm_val = 0;
+
 	rpwm_val |= (FW_PS_STATE_RF_OFF_92E | FW_PS_ACK);
 	_rtl92ee_set_fw_clock_on(hw, rpwm_val, true);
 }
@@ -248,9 +250,11 @@ static void _rtl92ee_set_fw_ps_rf_on(struct ieee80211_hw *hw)
 static void _rtl92ee_set_fw_ps_rf_off_low_power(struct ieee80211_hw *hw)
 {
 	u8 rpwm_val = 0;
+
 	rpwm_val |= FW_PS_STATE_RF_OFF_LOW_PWR;
 	_rtl92ee_set_fw_clock_off(hw, rpwm_val);
 }
+
 void rtl92ee_fw_clk_off_timer_callback(unsigned long data)
 {
 	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
@@ -263,25 +267,25 @@ static void _rtl92ee_fwlps_leave(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_fw_current_inps = false;
+	bool fw_current_inps = false;
 	u8 rpwm_val = 0, fw_pwrmode = FW_PS_ACTIVE_MODE;
 
-	if (ppsc->b_low_power_enable) {
+	if (ppsc->low_power_enable) {
 		rpwm_val = (FW_PS_STATE_ALL_ON_92E | FW_PS_ACK);/* RF on */
 		_rtl92ee_set_fw_clock_on(hw, rpwm_val, false);
-		rtlhal->ballow_sw_to_change_hwclc = false;
+		rtlhal->allow_sw_to_change_hwclc = false;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      (u8 *) (&fw_pwrmode));
+					      (u8 *)(&fw_pwrmode));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-					      (u8 *) (&b_fw_current_inps));
+					      (u8 *)(&fw_current_inps));
 	} else {
 		rpwm_val = FW_PS_STATE_ALL_ON_92E;	/* RF on */
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-					      (u8 *) (&rpwm_val));
+					      (u8 *)(&rpwm_val));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      (u8 *) (&fw_pwrmode));
+					      (u8 *)(&fw_pwrmode));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-					      (u8 *) (&b_fw_current_inps));
+					      (u8 *)(&fw_current_inps));
 	}
 }
 
@@ -290,25 +294,25 @@ static void _rtl92ee_fwlps_enter(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_fw_current_inps = true;
+	bool fw_current_inps = true;
 	u8 rpwm_val;
 
-	if (ppsc->b_low_power_enable) {
+	if (ppsc->low_power_enable) {
 		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR;	/* RF off */
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-					      (u8 *) (&b_fw_current_inps));
+					      (u8 *)(&fw_current_inps));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      (u8 *) (&ppsc->fwctrl_psmode));
-		rtlhal->ballow_sw_to_change_hwclc = true;
+					      (u8 *)(&ppsc->fwctrl_psmode));
+		rtlhal->allow_sw_to_change_hwclc = true;
 		_rtl92ee_set_fw_clock_off(hw, rpwm_val);
 	} else {
 		rpwm_val = FW_PS_STATE_RF_OFF_92E;	/* RF off */
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-					      (u8 *) (&b_fw_current_inps));
+					      (u8 *)(&fw_current_inps));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      (u8 *) (&ppsc->fwctrl_psmode));
+					      (u8 *)(&ppsc->fwctrl_psmode));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-					      (u8 *) (&rpwm_val));
+					      (u8 *)(&rpwm_val));
 	}
 }
 
@@ -320,45 +324,46 @@ void rtl92ee_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 	switch (variable) {
 	case HW_VAR_RCR:
-		*((u32 *) (val)) = rtlpci->receive_config;
+		*((u32 *)(val)) = rtlpci->receive_config;
 		break;
 	case HW_VAR_RF_STATE:
 		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
 		break;
 	case HW_VAR_FWLPS_RF_ON:{
-		enum rf_pwrstate rfState;
-		u32 val_rcr;
+			enum rf_pwrstate rfstate;
+			u32 val_rcr;
 
-		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE,
-					      (u8 *) (&rfState));
-		if (rfState == ERFOFF) {
-			*((bool *) (val)) = true;
-		} else {
-			val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
-			val_rcr &= 0x00070000;
-			if (val_rcr)
-				*((bool *) (val)) = false;
-			else
-				*((bool *) (val)) = true;
+			rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE,
+						      (u8 *)(&rfstate));
+			if (rfstate == ERFOFF) {
+				*((bool *)(val)) = true;
+			} else {
+				val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+				val_rcr &= 0x00070000;
+				if (val_rcr)
+					*((bool *)(val)) = false;
+				else
+					*((bool *)(val)) = true;
+			}
 		}
-		break; }
+		break;
 	case HW_VAR_FW_PSMODE_STATUS:
-		*((bool *) (val)) = ppsc->b_fw_current_inpsmode;
+		*((bool *)(val)) = ppsc->fw_current_inpsmode;
 		break;
 	case HW_VAR_CORRECT_TSF:{
 		u64 tsf;
-		u32 *ptsf_low = (u32 *) &tsf;
-		u32 *ptsf_high = ((u32 *) &tsf) + 1;
+		u32 *ptsf_low = (u32 *)&tsf;
+		u32 *ptsf_high = ((u32 *)&tsf) + 1;
 
 		*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
 		*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 
-		*((u64 *) (val)) = tsf;
-
-		break; }
+		*((u64 *)(val)) = tsf;
+		}
+		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_LOUD,
-			 ("switch case not process %x\n", variable));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process %x\n", variable);
 		break;
 	}
 }
@@ -378,11 +383,12 @@ static void _rtl92ee_download_rsvd_page(struct ieee80211_hw *hw)
 	/* Disable Hw protection for a time which revserd for Hw sending beacon.
 	 * Fix download reserved page packet fail
 	 * that access collision with the protection time.
-	 * 2010.05.11. Added by tynli. */
+	 * 2010.05.11. Added by tynli.
+	 */
 	_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
 	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(4), 0);
 
-	/* Set FWHW_TXQ_CTRL 0x422[6]= 0 to
+	/* Set FWHW_TXQ_CTRL 0x422[6]=0 to
 	 * tell Hw the packet is not a real beacon frame.
 	 */
 	tmp_reg422 = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
@@ -430,7 +436,8 @@ static void _rtl92ee_download_rsvd_page(struct ieee80211_hw *hw)
 	} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
 
 	if (!(bcnvalid_reg & BIT(0)))
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Download RSVD page failed!\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Download RSVD page failed!\n");
 
 	/* Enable Bcn */
 	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
@@ -441,7 +448,6 @@ static void _rtl92ee_download_rsvd_page(struct ieee80211_hw *hw)
 
 	tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
 	rtl_write_byte(rtlpriv, REG_CR + 1, tmp_regcr & (~BIT(0)));
-
 }
 
 void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
@@ -454,13 +460,13 @@ void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	u8 idx;
 
 	switch (variable) {
-	case HW_VAR_ETHER_ADDR:{
-		for (idx = 0; idx < ETH_ALEN; idx++) {
+	case HW_VAR_ETHER_ADDR:
+		for (idx = 0; idx < ETH_ALEN; idx++)
 			rtl_write_byte(rtlpriv, (REG_MACID + idx), val[idx]);
-		}
-		break; }
+		break;
 	case HW_VAR_BASIC_RATE:{
-		u16 b_rate_cfg = ((u16 *) val)[0];
+		u16 b_rate_cfg = ((u16 *)val)[0];
+
 		b_rate_cfg = b_rate_cfg & 0x15f;
 		b_rate_cfg |= 0x01;
 		b_rate_cfg = (b_rate_cfg | 0xd) & (~BIT(1));
@@ -468,9 +474,8 @@ void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtl_write_byte(rtlpriv, REG_RRSR + 1, (b_rate_cfg >> 8) & 0xff);
 		break; }
 	case HW_VAR_BSSID:
-		for (idx = 0; idx < ETH_ALEN; idx++) {
+		for (idx = 0; idx < ETH_ALEN; idx++)
 			rtl_write_byte(rtlpriv, (REG_BSSID + idx), val[idx]);
-		}
 		break;
 	case HW_VAR_SIFS:
 		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
@@ -483,32 +488,34 @@ void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM, 0x0e0e);
 		else
 			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
-				       *((u16 *) val));
+				       *((u16 *)val));
 		break;
 	case HW_VAR_SLOT_TIME:{
 		u8 e_aci;
 
-		RT_TRACE(COMP_MLME, DBG_TRACE,
-			 ("HW_VAR_SLOT_TIME %x\n", val[0]));
+		RT_TRACE(rtlpriv, COMP_MLME, DBG_TRACE,
+			 "HW_VAR_SLOT_TIME %x\n", val[0]);
 
 		rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
 
 		for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      (u8 *) (&e_aci));
+						      (u8 *)(&e_aci));
 		}
 		break; }
 	case HW_VAR_ACK_PREAMBLE:{
 		u8 reg_tmp;
-		u8 short_preamble = (bool) (*(u8 *) val);
+		u8 short_preamble = (bool)(*(u8 *)val);
+
 		reg_tmp = (rtlpriv->mac80211.cur_40_prime_sc) << 5;
 		if (short_preamble)
 			reg_tmp |= 0x80;
 		rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_tmp);
 		rtlpriv->mac80211.short_preamble = short_preamble;
-		break; }
+		}
+		break;
 	case HW_VAR_WPA_CONFIG:
-		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *) val));
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *)val));
 		break;
 	case HW_VAR_AMPDU_FACTOR:{
 		u8 regtoset_normal[4] = { 0x41, 0xa8, 0x72, 0xb9 };
@@ -518,34 +525,37 @@ void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 		reg = regtoset_normal;
 
-		fac = *((u8 *) val);
+		fac = *((u8 *)val);
 		if (fac <= 3) {
 			fac = (1 << (fac + 2));
 			if (fac > 0xf)
 				fac = 0xf;
-				for (i = 0; i < 4; i++) {
+			for (i = 0; i < 4; i++) {
 				if ((reg[i] & 0xf0) > (fac << 4))
-					reg[i] = (reg[i] & 0x0f) | (fac << 4);
+					reg[i] = (reg[i] & 0x0f) |
+						(fac << 4);
 				if ((reg[i] & 0x0f) > fac)
 					reg[i] = (reg[i] & 0xf0) | fac;
-					rtl_write_byte(rtlpriv,
-						       (REG_AGGLEN_LMT + i),
-						       reg[i]);
-				}
-				RT_TRACE(COMP_MLME, DBG_LOUD,
-					("Set HW_VAR_AMPDU_FACTOR:%#x\n", fac));
+				rtl_write_byte(rtlpriv,
+					       (REG_AGGLEN_LMT + i),
+					       reg[i]);
+			}
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "Set HW_VAR_AMPDU_FACTOR:%#x\n", fac);
 		}
-		break; }
+		}
+		break;
 	case HW_VAR_AC_PARAM:{
-		u8 e_aci = *((u8 *) val);
+		u8 e_aci = *((u8 *)val);
 
-		if (rtlpci->acm_method != eAcmWay2_SW)
+		if (rtlpci->acm_method != EACMWAY2_SW)
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
-						      (u8 *) (&e_aci));
-		break; }
+						      (u8 *)(&e_aci));
+		}
+		break;
 	case HW_VAR_ACM_CTRL:{
-		u8 e_aci = *((u8 *) val);
-		union aci_aifsn *aifs = (union aci_aifsn *)(&(mac->ac[0].aifs));
+		u8 e_aci = *((u8 *)val);
+		union aci_aifsn *aifs = (union aci_aifsn *)(&mac->ac[0].aifs);
 
 		u8 acm = aifs->f.acm;
 		u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
@@ -555,62 +565,65 @@ void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		if (acm) {
 			switch (e_aci) {
 			case AC0_BE:
-				acm_ctrl |= AcmHw_BeqEn;
+				acm_ctrl |= ACMHW_BEQEN;
 				break;
 			case AC2_VI:
-				acm_ctrl |= AcmHw_ViqEn;
+				acm_ctrl |= ACMHW_VIQEN;
 				break;
 			case AC3_VO:
-				acm_ctrl |= AcmHw_VoqEn;
+				acm_ctrl |= ACMHW_VOQEN;
 				break;
 			default:
-				RT_TRACE(COMP_ERR, DBG_WARNING,
-					 ("HW_VAR_ACM_CTRL acm set "
-					  "failed: eACI is %d\n", acm));
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "HW_VAR_ACM_CTRL acm set failed: eACI is %d\n",
+					 acm);
 				break;
 			}
 		} else {
 			switch (e_aci) {
 			case AC0_BE:
-				acm_ctrl &= (~AcmHw_BeqEn);
+				acm_ctrl &= (~ACMHW_BEQEN);
 				break;
 			case AC2_VI:
-				acm_ctrl &= (~AcmHw_ViqEn);
+				acm_ctrl &= (~ACMHW_VIQEN);
 				break;
 			case AC3_VO:
-				acm_ctrl &= (~AcmHw_BeqEn);
+				acm_ctrl &= (~ACMHW_BEQEN);
 				break;
 			default:
-				RT_TRACE(COMP_ERR, DBG_LOUD,
-					 ("switch case not process \n"));
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+					 "switch case not process\n");
 				break;
 			}
 		}
 
-		RT_TRACE(COMP_QOS, DBG_TRACE,
-			 ("SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
-			  acm_ctrl));
+		RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
+			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
+			  acm_ctrl);
 		rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
-		break; }
+		}
+		break;
 	case HW_VAR_RCR:{
-		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *) (val))[0]);
-		rtlpci->receive_config = ((u32 *) (val))[0];
-		break; }
+		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *)(val))[0]);
+		rtlpci->receive_config = ((u32 *)(val))[0];
+		}
+		break;
 	case HW_VAR_RETRY_LIMIT:{
-		u8 retry_limit = ((u8 *) (val))[0];
+		u8 retry_limit = ((u8 *)(val))[0];
 
 		rtl_write_word(rtlpriv, REG_RETRY_LIMIT,
 			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
 			       retry_limit << RETRY_LIMIT_LONG_SHIFT);
-		break; }
+		}
+		break;
 	case HW_VAR_DUAL_TSF_RST:
 		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
 		break;
 	case HW_VAR_EFUSE_BYTES:
-		efuse->efuse_usedbytes = *((u16 *) val);
+		efuse->efuse_usedbytes = *((u16 *)val);
 		break;
 	case HW_VAR_EFUSE_USAGE:
-		efuse->efuse_usedpercentage = *((u8 *) val);
+		efuse->efuse_usedpercentage = *((u8 *)val);
 		break;
 	case HW_VAR_IO_CMD:
 		rtl92ee_phy_set_io_cmd(hw, (*(enum io_type *)val));
@@ -622,75 +635,83 @@ void rtl92ee_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		udelay(1);
 
 		if (rpwm_val & BIT(7)) {
-			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, (*(u8 *) val));
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, (*(u8 *)val));
 		} else {
 			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
-				       ((*(u8 *) val) | BIT(7)));
+				       ((*(u8 *)val) | BIT(7)));
 		}
-		break; }
+		}
+		break;
 	case HW_VAR_H2C_FW_PWRMODE:
-		rtl92ee_set_fw_pwrmode_cmd(hw, (*(u8 *) val));
+		rtl92ee_set_fw_pwrmode_cmd(hw, (*(u8 *)val));
 		break;
 	case HW_VAR_FW_PSMODE_STATUS:
-		ppsc->b_fw_current_inpsmode = *((bool *) val);
+		ppsc->fw_current_inpsmode = *((bool *)val);
 		break;
 	case HW_VAR_RESUME_CLK_ON:
 		_rtl92ee_set_fw_ps_rf_on(hw);
 		break;
 	case HW_VAR_FW_LPS_ACTION:{
-		bool b_enter_fwlps = *((bool *) val);
+		bool b_enter_fwlps = *((bool *)val);
 
 		if (b_enter_fwlps)
 			_rtl92ee_fwlps_enter(hw);
 		else
 			_rtl92ee_fwlps_leave(hw);
-		break; }
+		}
+		break;
 	case HW_VAR_H2C_FW_JOINBSSRPT:{
-		u8 mstatus = (*(u8 *) val);
+		u8 mstatus = (*(u8 *)val);
 
 		if (mstatus == RT_MEDIA_CONNECT) {
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
 			_rtl92ee_download_rsvd_page(hw);
 		}
 		rtl92ee_set_fw_media_status_rpt_cmd(hw, mstatus);
-		break; }
+		}
+		break;
 	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
-		rtl92ee_set_p2p_ps_offload_cmd(hw , (*(u8 *) val));
+		rtl92ee_set_p2p_ps_offload_cmd(hw, (*(u8 *)val));
 		break;
 	case HW_VAR_AID:{
 		u16 u2btmp;
+
 		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
 		u2btmp &= 0xC000;
 		rtl_write_word(rtlpriv, REG_BCN_PSR_RPT,
 			       (u2btmp | mac->assoc_id));
-		break; }
+		}
+		break;
 	case HW_VAR_CORRECT_TSF:{
-		u8 btype_ibss = ((u8 *) (val))[0];
+		u8 btype_ibss = ((u8 *)(val))[0];
 
-		if (btype_ibss == true)
+		if (btype_ibss)
 			_rtl92ee_stop_tx_beacon(hw);
 
 		_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(3));
 
 		rtl_write_dword(rtlpriv, REG_TSFTR,
-				(u32) (mac->tsf & 0xffffffff));
+				(u32)(mac->tsf & 0xffffffff));
 		rtl_write_dword(rtlpriv, REG_TSFTR + 4,
-				(u32) ((mac->tsf >> 32) & 0xffffffff));
+				(u32)((mac->tsf >> 32) & 0xffffffff));
 
 		_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
 
-		if (btype_ibss == true)
+		if (btype_ibss)
 			_rtl92ee_resume_tx_beacon(hw);
-		break; }
+		}
+		break;
 	case HW_VAR_KEEP_ALIVE: {
 		u8 array[2];
+
 		array[0] = 0xff;
 		array[1] = *((u8 *)val);
 		rtl92ee_fill_h2c_cmd(hw, H2C_92E_KEEP_ALIVE_CTRL, 2, array);
-		break; }
+		}
+		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_LOUD,
-			 ("switch case not process %x\n", variable));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process %x\n", variable);
 		break;
 	}
 }
@@ -739,17 +760,17 @@ static void _rtl92ee_gen_refresh_led_state(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+	struct rtl_led *pled0 = &pcipriv->ledctl.sw_led0;
 
 	if (rtlpriv->rtlhal.up_first_time)
 		return;
 
 	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
-		rtl92ee_sw_led_on(hw, pLed0);
+		rtl92ee_sw_led_on(hw, pled0);
 	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
-		rtl92ee_sw_led_on(hw, pLed0);
+		rtl92ee_sw_led_on(hw, pled0);
 	else
-		rtl92ee_sw_led_off(hw, pLed0);
+		rtl92ee_sw_led_off(hw, pled0);
 }
 
 static bool _rtl92ee_init_mac(struct ieee80211_hw *hw)
@@ -782,7 +803,8 @@ static bool _rtl92ee_init_mac(struct ieee80211_hw *hw)
 	rtl_write_dword(rtlpriv, REG_AFE_CTRL4, dwordtmp);
 
 	/* 2. 92E AFE parameter
-	  *MP chip then check version */
+	 * MP chip then check version
+	 */
 	bytetmp = rtl_read_byte(rtlpriv, REG_AFE_CTRL2);
 	bytetmp &= 0xbf;
 	rtl_write_byte(rtlpriv, REG_AFE_CTRL2, bytetmp);
@@ -792,10 +814,11 @@ static bool _rtl92ee_init_mac(struct ieee80211_hw *hw)
 	rtl_write_dword(rtlpriv, REG_AFE_CTRL4, dwordtmp);
 
 	/* HW Power on sequence */
-	if (!rtl92e_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-		PWR_INTF_PCI_MSK, Rtl8192E_NIC_ENABLE_FLOW)) {
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			 ("init MAC Fail as rtl92e_hal_pwrseqcmdparsing\n"));
+	if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				      PWR_INTF_PCI_MSK,
+				      RTL8192E_NIC_ENABLE_FLOW)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "init MAC Fail as rtl_hal_pwrseqcmdparsing\n");
 		return false;
 	}
 
@@ -816,10 +839,10 @@ static bool _rtl92ee_init_mac(struct ieee80211_hw *hw)
 	/* Release MAC IO register reset */
 	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
 
-	if (!rtlhal->b_mac_func_enable) {
+	if (!rtlhal->mac_func_enable) {
 		if (_rtl92ee_llt_table_init(hw) == false) {
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				 ("LLT table init fail \n"));
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "LLT table init fail\n");
 			return false;
 		}
 	}
@@ -843,38 +866,38 @@ static bool _rtl92ee_init_mac(struct ieee80211_hw *hw)
 
 	/* Set TX/RX descriptor physical address(from OS API). */
 	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
-			((u64) rtlpci->tx_ring[BEACON_QUEUE].buffer_desc_dma) &
+			((u64)rtlpci->tx_ring[BEACON_QUEUE].buffer_desc_dma) &
 			DMA_BIT_MASK(32));
 	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
-			(u64) rtlpci->tx_ring[MGNT_QUEUE].buffer_desc_dma &
+			(u64)rtlpci->tx_ring[MGNT_QUEUE].buffer_desc_dma &
 			DMA_BIT_MASK(32));
 	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
-			(u64) rtlpci->tx_ring[VO_QUEUE].buffer_desc_dma &
+			(u64)rtlpci->tx_ring[VO_QUEUE].buffer_desc_dma &
 			DMA_BIT_MASK(32));
 	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
-			(u64) rtlpci->tx_ring[VI_QUEUE].buffer_desc_dma &
+			(u64)rtlpci->tx_ring[VI_QUEUE].buffer_desc_dma &
 			DMA_BIT_MASK(32));
 
 	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
-			(u64) rtlpci->tx_ring[BE_QUEUE].buffer_desc_dma &
+			(u64)rtlpci->tx_ring[BE_QUEUE].buffer_desc_dma &
 			DMA_BIT_MASK(32));
 
 	dwordtmp = rtl_read_dword(rtlpriv, REG_BEQ_DESA);
 
 	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
-			(u64) rtlpci->tx_ring[BK_QUEUE].buffer_desc_dma &
+			(u64)rtlpci->tx_ring[BK_QUEUE].buffer_desc_dma &
 			DMA_BIT_MASK(32));
 	rtl_write_dword(rtlpriv, REG_HQ0_DESA,
-			(u64) rtlpci->tx_ring[HIGH_QUEUE].buffer_desc_dma &
+			(u64)rtlpci->tx_ring[HIGH_QUEUE].buffer_desc_dma &
 			DMA_BIT_MASK(32));
 
 	rtl_write_dword(rtlpriv, REG_RX_DESA,
-			(u64) rtlpci->rx_ring[RX_MPDU_QUEUE].dma &
+			(u64)rtlpci->rx_ring[RX_MPDU_QUEUE].dma &
 			DMA_BIT_MASK(32));
 
 	/* if we want to support 64 bit DMA, we should set it here,
-	 * but now we do not support 64 bit DMA*/
-
+	 * but now we do not support 64 bit DMA
+	 */
 
 	rtl_write_dword(rtlpriv, REG_TSFTIMER_HCI, 0x3fffffff);
 
@@ -981,7 +1004,7 @@ static void _rtl92ee_hw_configure(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_BCN_CTRL_1, 0);
 
 	/* TBTT prohibit hold time. Suggested by designer TimChen. */
-	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1 , 0xff); /* 8 ms */
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff); /* 8 ms */
 
 	rtl_write_byte(rtlpriv, REG_PIFS, 0);
 	rtl_write_byte(rtlpriv, REG_AGGR_BREAK_TIME, 0x16);
@@ -1043,7 +1066,8 @@ static void _rtl92ee_enable_aspm_back_door(struct ieee80211_hw *hw)
 			rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0xf078);
 			rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x1);
 
-			tmp8 = rtl_read_byte(rtlpriv , REG_BACKDOOR_DBI_DATA + 2);
+			tmp8 = rtl_read_byte(rtlpriv,
+					     REG_BACKDOOR_DBI_DATA + 2);
 			count = 0;
 			while (tmp8 && count < 20) {
 				udelay(10);
@@ -1079,7 +1103,6 @@ static void _rtl92ee_enable_aspm_back_door(struct ieee80211_hw *hw)
 		count++;
 	}
 
-
 	rtl_write_word(rtlpriv, REG_BACKDOOR_DBI_DATA, 0x718);
 	rtl_write_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2, 0x2);
 	tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
@@ -1089,7 +1112,7 @@ static void _rtl92ee_enable_aspm_back_door(struct ieee80211_hw *hw)
 		tmp8 = rtl_read_byte(rtlpriv, REG_BACKDOOR_DBI_DATA + 2);
 		count++;
 	}
-	if (ppsc->b_support_backdoor || (0 == tmp8)) {
+	if (ppsc->support_backdoor || (0 == tmp8)) {
 		tmp32 = rtl_read_dword(rtlpriv, REG_BACKDOOR_DBI_RDATA);
 		rtl_write_dword(rtlpriv, REG_BACKDOOR_DBI_WDATA,
 				tmp32 | BIT(11) | BIT(12));
@@ -1111,21 +1134,22 @@ void rtl92ee_enable_hw_security_config(struct ieee80211_hw *hw)
 	u8 sec_reg_value;
 	u8 tmp;
 
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
 		  rtlpriv->sec.pairwise_enc_algorithm,
-		  rtlpriv->sec.group_enc_algorithm));
+		  rtlpriv->sec.group_enc_algorithm);
 
 	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("not open hw encryption\n"));
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+			 "not open hw encryption\n");
 		return;
 	}
 
-	sec_reg_value = SCR_TxEncEnable | SCR_RxDecEnable;
+	sec_reg_value = SCR_TXENCENABLE | SCR_RXDECENABLE;
 
 	if (rtlpriv->sec.use_defaultkey) {
-		sec_reg_value |= SCR_TxUseDK;
-		sec_reg_value |= SCR_RxUseDK;
+		sec_reg_value |= SCR_TXUSEDK;
+		sec_reg_value |= SCR_RXUSEDK;
 	}
 
 	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
@@ -1133,11 +1157,10 @@ void rtl92ee_enable_hw_security_config(struct ieee80211_hw *hw)
 	tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
 	rtl_write_byte(rtlpriv, REG_CR + 1, tmp | BIT(1));
 
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("The SECR-value %x \n", sec_reg_value));
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+		 "The SECR-value %x\n", sec_reg_value);
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
-
 }
 
 int rtl92ee_hw_init(struct ieee80211_hw *hw)
@@ -1145,23 +1168,23 @@ int rtl92ee_hw_init(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	bool rtstatus = true;
 	int err = 0;
-	u8 tmp_u1b , u1byte;
+	u8 tmp_u1b, u1byte;
 	u32 tmp_u4b;
 
-	RT_TRACE(COMP_INIT , DBG_LOUD , (" Rtl8192EE hw init\n"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, " Rtl8192EE hw init\n");
 	rtlpriv->rtlhal.being_init_adapter = true;
 	rtlpriv->intf_ops->disable_aspm(hw);
 
 	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CLKR+1);
 	u1byte = rtl_read_byte(rtlpriv, REG_CR);
 	if ((tmp_u1b & BIT(3)) && (u1byte != 0 && u1byte != 0xEA)) {
-		rtlhal->b_mac_func_enable = true;
+		rtlhal->mac_func_enable = true;
 	} else {
-		rtlhal->b_mac_func_enable = false;
+		rtlhal->mac_func_enable = false;
 		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_92E;
 	}
 
@@ -1179,31 +1202,29 @@ int rtl92ee_hw_init(struct ieee80211_hw *hw)
 		rtl_write_byte(rtlpriv, 0x64, 0);
 		rtl_write_byte(rtlpriv, 0x65, 1);
 	}
-	if (rtstatus != true) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Init MAC failed\n"));
+	if (!rtstatus) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
 		err = 1;
 		return err;
 	}
 	rtlhal->rx_tag = 0;
 	rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, 0x8000);
-	err = rtl92ee_download_fw(hw , false);
+	err = rtl92ee_download_fw(hw, false);
 	if (err) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Failed to download FW. Init HW without FW now..\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
-		rtlhal->bfw_ready = false;
+		rtlhal->fw_ready = false;
 		return err;
-	} else {
-		rtlhal->bfw_ready = true;
 	}
+	rtlhal->fw_ready = true;
 	/*fw related variable initialize */
-	ppsc->b_fw_current_inpsmode = false;
+	ppsc->fw_current_inpsmode = false;
 	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_92E;
-	rtlhal->bfw_clk_change_in_progress = false;
-	rtlhal->ballow_sw_to_change_hwclc = false;
+	rtlhal->fw_clk_change_in_progress = false;
+	rtlhal->allow_sw_to_change_hwclc = false;
 	rtlhal->last_hmeboxnum = 0;
 
-
 	rtl92ee_phy_mac_config(hw);
 
 	rtl92ee_phy_bb_config(hw);
@@ -1214,8 +1235,8 @@ int rtl92ee_hw_init(struct ieee80211_hw *hw)
 						 RF_CHNLBW, RFREG_OFFSET_MASK);
 	rtlphy->rfreg_chnlval[1] = rtl_get_rfreg(hw, RF90_PATH_B,
 						 RF_CHNLBW, RFREG_OFFSET_MASK);
-	rtlphy->backup_rf_0x1a = (u32) rtl_get_rfreg(hw, RF90_PATH_A, RF_RX_G1,
-						     RFREG_OFFSET_MASK);
+	rtlphy->backup_rf_0x1a = (u32)rtl_get_rfreg(hw, RF90_PATH_A, RF_RX_G1,
+						    RFREG_OFFSET_MASK);
 	rtlphy->rfreg_chnlval[0] = (rtlphy->rfreg_chnlval[0] & 0xfffff3ff) |
 				   BIT(10) | BIT(11);
 
@@ -1236,9 +1257,9 @@ int rtl92ee_hw_init(struct ieee80211_hw *hw)
 	/*Set Hardware(MAC default setting.)*/
 	_rtl92ee_hw_configure(hw);
 
-	rtlhal->b_mac_func_enable = true;
+	rtlhal->mac_func_enable = true;
 
-	stg_rtl_cam_reset_all_entry(hw);
+	rtl_cam_reset_all_entry(hw);
 	rtl92ee_enable_hw_security_config(hw);
 
 	ppsc->rfpwr_state = ERFON;
@@ -1264,15 +1285,15 @@ int rtl92ee_hw_init(struct ieee80211_hw *hw)
 	if (rtlphy->rf_type == RF_2T2R)
 		rtlphy->rfpath_rx_enable[1] = true;
 
-	stg_efuse_one_byte_read(hw, 0x1FA, &tmp_u1b);
+	efuse_one_byte_read(hw, 0x1FA, &tmp_u1b);
 	if (!(tmp_u1b & BIT(0))) {
 		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0F, 0x05);
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("PA BIAS path A\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "PA BIAS path A\n");
 	}
 
 	if ((!(tmp_u1b & BIT(1))) && (rtlphy->rf_type == RF_2T2R)) {
 		rtl_set_rfreg(hw, RF90_PATH_B, 0x15, 0x0F, 0x05);
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("PA BIAS path B\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "PA BIAS path B\n");
 	}
 
 	rtl_write_byte(rtlpriv, REG_NAV_UPPER, ((30000 + 127) / 128));
@@ -1287,14 +1308,15 @@ int rtl92ee_hw_init(struct ieee80211_hw *hw)
 
 	rtl_write_dword(rtlpriv, 0x4fc, 0);
 
-	RT_TRACE(COMP_INIT , DBG_LOUD , ("end of Rtl8192EE hw init %x\n" , err));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "end of Rtl8192EE hw init %x\n", err);
 	return 0;
 }
 
 static enum version_8192e _rtl92ee_read_chip_version(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	enum version_8192e version = VERSION_UNKNOWN;
 	u32 value32;
 
@@ -1302,13 +1324,13 @@ static enum version_8192e _rtl92ee_read_chip_version(struct ieee80211_hw *hw)
 
 	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
 	if (value32 & TRP_VAUX_EN)
-		version = (enum version_8192e) VERSION_TEST_CHIP_2T2R_8192E;
+		version = (enum version_8192e)VERSION_TEST_CHIP_2T2R_8192E;
 	else
-		version = (enum version_8192e) VERSION_NORMAL_CHIP_2T2R_8192E;
+		version = (enum version_8192e)VERSION_NORMAL_CHIP_2T2R_8192E;
 
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
-		  "RF_2T2R" : "RF_1T1R"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
+		  "RF_2T2R" : "RF_1T1R");
 
 	return version;
 }
@@ -1317,40 +1339,38 @@ static int _rtl92ee_set_media_status(struct ieee80211_hw *hw,
 				     enum nl80211_iftype type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR) & 0xfc;
 	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
 	u8 mode = MSR_NOLINK;
-	bt_msr &= 0xfc;
 
 	switch (type) {
 	case NL80211_IFTYPE_UNSPECIFIED:
 		mode = MSR_NOLINK;
-		RT_TRACE(COMP_INIT, DBG_TRACE,
-			 ("Set Network type to NO LINK!\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to NO LINK!\n");
 		break;
 	case NL80211_IFTYPE_ADHOC:
 	case NL80211_IFTYPE_MESH_POINT:
 		mode = MSR_ADHOC;
-		RT_TRACE(COMP_INIT, DBG_TRACE,
-			 ("Set Network type to Ad Hoc!\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to Ad Hoc!\n");
 		break;
 	case NL80211_IFTYPE_STATION:
 		mode = MSR_INFRA;
 		ledaction = LED_CTL_LINK;
-		RT_TRACE(COMP_INIT, DBG_TRACE,
-			 ("Set Network type to STA!\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to STA!\n");
 		break;
 	case NL80211_IFTYPE_AP:
 		mode = MSR_AP;
 		ledaction = LED_CTL_LINK;
-		RT_TRACE(COMP_INIT, DBG_TRACE,
-			 ("Set Network type to AP!\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to AP!\n");
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Network type %d not support!\n", type));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Network type %d not support!\n", type);
 		return 1;
-		break;
 	}
 
 	/* MSR_INFRA == Link in infrastructure network;
@@ -1371,9 +1391,9 @@ static int _rtl92ee_set_media_status(struct ieee80211_hw *hw,
 		_rtl92ee_resume_tx_beacon(hw);
 		_rtl92ee_disable_bcn_sub_func(hw);
 	} else {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Set HW_VAR_MEDIA_STATUS: "
-			  "No such media status(%x).\n", mode));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
+			 mode);
 	}
 
 	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
@@ -1394,16 +1414,16 @@ void rtl92ee_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
-	if (check_bssid == true) {
+	if (check_bssid) {
 		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
-					      (u8 *) (&reg_rcr));
+					      (u8 *)(&reg_rcr));
 		_rtl92ee_set_bcn_ctrl_reg(hw, 0, BIT(4));
-	} else if (check_bssid == false) {
+	} else {
 		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
 		_rtl92ee_set_bcn_ctrl_reg(hw, BIT(4), 0);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
-					      (u8 *) (&reg_rcr));
+					      (u8 *)(&reg_rcr));
 	}
 }
 
@@ -1429,6 +1449,7 @@ int rtl92ee_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
 void rtl92ee_set_qos(struct ieee80211_hw *hw, int aci)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
 	rtl92ee_dm_init_edca_turbo(hw);
 	switch (aci) {
 	case AC1_BK:
@@ -1444,7 +1465,7 @@ void rtl92ee_set_qos(struct ieee80211_hw *hw, int aci)
 		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x2f3222);
 		break;
 	default:
-		RT_ASSERT(false, ("invalid aci: %d !\n", aci));
+		RT_ASSERT(false, "invalid aci: %d !\n", aci);
 		break;
 	}
 }
@@ -1492,18 +1513,19 @@ static void _rtl92ee_poweroff_adapter(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 u1b_tmp;
-	rtlhal->b_mac_func_enable = false;
 
-	RT_TRACE(COMP_INIT , DBG_LOUD , ("POWER OFF adapter \n"));
+	rtlhal->mac_func_enable = false;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "POWER OFF adapter\n");
 
 	/* Run LPS WL RFOFF flow */
-	rtl92e_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-				 PWR_INTF_PCI_MSK, Rtl8192E_NIC_LPS_ENTER_FLOW);
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, RTL8192E_NIC_LPS_ENTER_FLOW);
 	/* turn off RF */
 	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
 
 	/* ==== Reset digital sequence   ======  */
-	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) && rtlhal->bfw_ready)
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) && rtlhal->fw_ready)
 		rtl92ee_firmware_selfreset(hw);
 
 	/* Reset MCU  */
@@ -1514,8 +1536,8 @@ static void _rtl92ee_poweroff_adapter(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
 
 	/* HW card disable configuration. */
-	rtl92e_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-				 PWR_INTF_PCI_MSK, Rtl8192E_NIC_DISABLE_FLOW);
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, RTL8192E_NIC_DISABLE_FLOW);
 
 	/* Reset MCU IO Wrapper */
 	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
@@ -1534,7 +1556,7 @@ void rtl92ee_card_disable(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	enum nl80211_iftype opmode;
 
-	RT_TRACE(COMP_INIT , DBG_LOUD , ("RTL8192ee card disable\n"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "RTL8192ee card disable\n");
 
 	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
 
@@ -1568,7 +1590,6 @@ void rtl92ee_interrupt_recognized(struct ieee80211_hw *hw,
 
 void rtl92ee_set_beacon_related_registers(struct ieee80211_hw *hw)
 {
-
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -1584,7 +1605,7 @@ void rtl92ee_set_beacon_related_registers(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x18);
 	rtl_write_byte(rtlpriv, 0x606, 0x30);
 	rtlpci->reg_bcn_ctrl_val |= BIT(3);
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlpci->reg_bcn_ctrl_val);
 }
 
 void rtl92ee_set_beacon_interval(struct ieee80211_hw *hw)
@@ -1593,8 +1614,8 @@ void rtl92ee_set_beacon_interval(struct ieee80211_hw *hw)
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u16 bcn_interval = mac->beacon_interval;
 
-	RT_TRACE(COMP_BEACON, DBG_DMESG,
-		 ("beacon_interval:%d\n", bcn_interval));
+	RT_TRACE(rtlpriv, COMP_BEACON, DBG_DMESG,
+		 "beacon_interval:%d\n", bcn_interval);
 	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
 }
 
@@ -1604,8 +1625,8 @@ void rtl92ee_update_interrupt_mask(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
-	RT_TRACE(COMP_INTR, DBG_LOUD,
-		 ("add_msr:%x, rm_msr:%x\n", add_msr, rm_msr));
+	RT_TRACE(rtlpriv, COMP_INTR, DBG_LOUD,
+		 "add_msr:%x, rm_msr:%x\n", add_msr, rm_msr);
 
 	if (add_msr)
 		rtlpci->irq_mask[0] |= add_msr;
@@ -1671,15 +1692,15 @@ static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 rf, addr = EEPROM_TX_PWR_INX, group, i = 0;
 
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("hal_ReadPowerValueFromPROM92E(): PROMContent[0x%x]= 0x%x\n",
-		 (addr + 1), hwinfo[addr + 1]));
-	if (0xFF == hwinfo[addr+1])  /*YJ, add, 120316*/
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "hal_ReadPowerValueFromPROM92E(): PROMContent[0x%x]=0x%x\n",
+		 (addr + 1), hwinfo[addr + 1]);
+	if (0xFF == hwinfo[addr+1])  /*YJ,add,120316*/
 		autoload_fail = true;
 
 	if (autoload_fail) {
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			 ("auto load fail : Use Default value!\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "auto load fail : Use Default value!\n");
 		for (rf = 0 ; rf < MAX_RF_PATH ; rf++) {
 			/* 2.4G default value */
 			for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
@@ -1720,7 +1741,7 @@ static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
 		return;
 	}
 
-	rtl_priv(hw)->efuse.b_txpwr_fromeprom = true;
+	rtl_priv(hw)->efuse.txpwr_fromeprom = true;
 
 	for (rf = 0 ; rf < MAX_RF_PATH ; rf++) {
 		/*2.4G default value*/
@@ -1728,7 +1749,6 @@ static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
 			pwr2g->index_cck_base[rf][group] = hwinfo[addr++];
 			if (pwr2g->index_cck_base[rf][group] == 0xFF)
 				pwr2g->index_cck_base[rf][group] = 0x2D;
-
 		}
 		for (group = 0 ; group < MAX_CHNL_GROUP_24G - 1; group++) {
 			pwr2g->index_bw40_base[rf][group] = hwinfo[addr++];
@@ -1834,7 +1854,7 @@ static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
 					pwr5g->bw40_diff[rf][i] = (hwinfo[addr]
 								  & 0xf0) >> 4;
 					if (pwr5g->bw40_diff[rf][i] & BIT(3))
-					pwr5g->bw40_diff[rf][i] |= 0xF0;
+						pwr5g->bw40_diff[rf][i] |= 0xF0;
 				}
 
 				if (hwinfo[addr] == 0xFF) {
@@ -1843,7 +1863,7 @@ static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
 					pwr5g->bw20_diff[rf][i] = (hwinfo[addr]
 								   & 0x0f);
 					if (pwr5g->bw20_diff[rf][i] & BIT(3))
-					pwr5g->bw20_diff[rf][i] |= 0xF0;
+						pwr5g->bw20_diff[rf][i] |= 0xF0;
 				}
 				addr++;
 			}
@@ -1884,7 +1904,8 @@ static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
 			if (hwinfo[addr] == 0xFF) {
 				pwr5g->bw160_diff[rf][i] = 0xFE;
 			} else {
-				pwr5g->bw160_diff[rf][i] = (hwinfo[addr] & 0x0f);
+				pwr5g->bw160_diff[rf][i] =
+				  (hwinfo[addr] & 0x0f);
 				if (pwr5g->bw160_diff[rf][i] & BIT(3))
 					pwr5g->bw160_diff[rf][i] |= 0xF0;
 			}
@@ -1892,6 +1913,7 @@ static void _rtl8192ee_read_power_value_fromprom(struct ieee80211_hw *hw,
 		}
 	}
 }
+
 static void _rtl92ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 						 bool autoload_fail, u8 *hwinfo)
 {
@@ -1906,9 +1928,11 @@ static void _rtl92ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		124, 126, 128, 130, 132, 134, 136, 138,
 		140, 142, 144, 149, 151, 153, 155, 157,
 		159, 161, 163, 165, 167, 168, 169, 171,
-		173, 175, 177};
+		173, 175, 177
+	};
 	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {
-		42, 58, 106, 122, 138, 155, 171};
+		42, 58, 106, 122, 138, 155, 171
+	};
 	u8 rf, idx;
 	u8 i;
 
@@ -1938,6 +1962,7 @@ static void _rtl92ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		}
 		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
 			u8 upper, lower;
+
 			idx = _rtl92ee_get_chnl_group(channel5g_80m[i]);
 			upper = pwr5g.index_bw40_base[rf][idx];
 			lower = pwr5g.index_bw40_base[rf][idx + 1];
@@ -1963,12 +1988,12 @@ static void _rtl92ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		efu->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
 
 	if (efu->eeprom_thermalmeter == 0xff || autoload_fail) {
-		efu->b_apk_thermalmeterignore = true;
+		efu->apk_thermalmeterignore = true;
 		efu->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
 	}
 
 	efu->thermalmeter[0] = efu->eeprom_thermalmeter;
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"thermalmeter = 0x%x\n", efu->eeprom_thermalmeter);
 
 	if (!autoload_fail) {
@@ -1979,7 +2004,7 @@ static void _rtl92ee_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	} else {
 		efu->eeprom_regulatory = 0;
 	}
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"eeprom_regulatory = 0x%x\n", efu->eeprom_regulatory);
 }
 
@@ -1993,68 +2018,68 @@ static void _rtl92ee_read_adapter_info(struct ieee80211_hw *hw)
 	u16 eeprom_id;
 
 	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
-		stg_rtl_efuse92e_shadow_map_update(hw);
+		rtl_efuse_shadow_map_update(hw);
 
 		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
 		       HWSET_MAX_SIZE);
 	} else if (rtlefuse->epromtype == EEPROM_93C46) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("RTL819X Not boot from eeprom, check it !!"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "RTL819X Not boot from eeprom, check it !!");
 		return;
-	}  else {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("boot from neither eeprom nor efuse, check it !!"));
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "boot from neither eeprom nor efuse, check it !!");
 		return;
 	}
 
-	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD, "MAP \n",
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "MAP\n",
 		      hwinfo, HWSET_MAX_SIZE);
 
-	eeprom_id = *((u16 *) &hwinfo[0]);
+	eeprom_id = *((u16 *)&hwinfo[0]);
 	if (eeprom_id != RTL8192E_EEPROM_ID) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("EEPROM ID(%#x) is invalid!!\n", eeprom_id));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "EEPROM ID(%#x) is invalid!!\n", eeprom_id);
 		rtlefuse->autoload_failflag = true;
 	} else {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
 		rtlefuse->autoload_failflag = false;
 	}
 
-	if (rtlefuse->autoload_failflag == true)
+	if (rtlefuse->autoload_failflag)
 		return;
 	/*VID DID SVID SDID*/
-	rtlefuse->eeprom_vid = *(u16 *) &hwinfo[EEPROM_VID];
-	rtlefuse->eeprom_did = *(u16 *) &hwinfo[EEPROM_DID];
-	rtlefuse->eeprom_svid = *(u16 *) &hwinfo[EEPROM_SVID];
-	rtlefuse->eeprom_smid = *(u16 *) &hwinfo[EEPROM_SMID];
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("EEPROMId = 0x%4x\n", eeprom_id));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid));
+	rtlefuse->eeprom_vid = *(u16 *)&hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *)&hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *)&hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *)&hwinfo[EEPROM_SMID];
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "EEPROMId = 0x%4x\n", eeprom_id);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid);
 	/*customer ID*/
-	rtlefuse->eeprom_oemid = *(u8 *) &hwinfo[EEPROM_CUSTOMER_ID];
+	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
 	if (rtlefuse->eeprom_oemid == 0xFF)
 		rtlefuse->eeprom_oemid = 0;
 
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid);
 	/*EEPROM version*/
-	rtlefuse->eeprom_version = *(u8 *) &hwinfo[EEPROM_VERSION];
+	rtlefuse->eeprom_version = *(u8 *)&hwinfo[EEPROM_VERSION];
 	/*mac address*/
 	for (i = 0; i < 6; i += 2) {
-		usvalue = *(u16 *) &hwinfo[EEPROM_MAC_ADDR + i];
-		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
+		usvalue = *(u16 *)&hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *)(&rtlefuse->dev_addr[i])) = usvalue;
 	}
 
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("dev_addr: %pM\n", rtlefuse->dev_addr));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "dev_addr: %pM\n", rtlefuse->dev_addr);
 	/*channel plan */
-	rtlefuse->eeprom_channelplan = *(u8 *) &hwinfo[EEPROM_CHANNELPLAN];
+	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
 	/* set channel paln to world wide 13 */
 	rtlefuse->channel_plan = COUNTRY_CODE_WORLD_WIDE_13;
 	/*tx power*/
@@ -2065,12 +2090,12 @@ static void _rtl92ee_read_adapter_info(struct ieee80211_hw *hw)
 					       hwinfo);
 
 	/*board type*/
-	rtlefuse->board_type = (((*(u8 *) &hwinfo[EEPROM_RF_BOARD_OPTION_92E])
+	rtlefuse->board_type = (((*(u8 *)&hwinfo[EEPROM_RF_BOARD_OPTION_92E])
 				& 0xE0) >> 5);
-	if ((*(u8 *) &hwinfo[EEPROM_RF_BOARD_OPTION_92E]) == 0xFF)
+	if ((*(u8 *)&hwinfo[EEPROM_RF_BOARD_OPTION_92E]) == 0xFF)
 		rtlefuse->board_type = 0;
 
-	rtlhal->boad_type = rtlefuse->board_type;
+	rtlhal->board_type = rtlefuse->board_type;
 	/*parse xtal*/
 	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_92E];
 	if (hwinfo[EEPROM_XTAL_92E] == 0xFF)
@@ -2086,7 +2111,7 @@ static void _rtl92ee_read_adapter_info(struct ieee80211_hw *hw)
 			if (rtlefuse->eeprom_did == 0x818B) {
 				if ((rtlefuse->eeprom_svid == 0x10EC) &&
 				    (rtlefuse->eeprom_smid == 0x001B))
-					rtlhal->oem_id = RT_CID_819x_Lenovo;
+					rtlhal->oem_id = RT_CID_819X_LENOVO;
 			} else {
 				rtlhal->oem_id = RT_CID_DEFAULT;
 			}
@@ -2104,42 +2129,43 @@ static void _rtl92ee_hal_customized_behavior(struct ieee80211_hw *hw)
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
-	pcipriv->ledctl.bled_opendrain = true;
+	pcipriv->ledctl.led_opendrain = true;
 
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("RT Customized ID: 0x%02X\n", rtlhal->oem_id));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "RT Customized ID: 0x%02X\n", rtlhal->oem_id);
 }
 
 void rtl92ee_read_eeprom_info(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 tmp_u1b;
 
 	rtlhal->version = _rtl92ee_read_chip_version(hw);
-	if (get_rf_type(rtlphy) == RF_1T1R)
-		rtlpriv->dm.brfpath_rxenable[0] = true;
-	else
-		rtlpriv->dm.brfpath_rxenable[0] =
-		    rtlpriv->dm.brfpath_rxenable[1] = true;
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("VersionID = 0x%4x\n",
-						rtlhal->version));
+	if (get_rf_type(rtlphy) == RF_1T1R) {
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+	} else {
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+		rtlpriv->dm.rfpath_rxenable[1] = true;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "VersionID = 0x%4x\n",
+		 rtlhal->version);
 	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
 	if (tmp_u1b & BIT(4)) {
-		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EEPROM\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EEPROM\n");
 		rtlefuse->epromtype = EEPROM_93C46;
 	} else {
-		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EFUSE\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EFUSE\n");
 		rtlefuse->epromtype = EEPROM_BOOT_EFUSE;
 	}
 	if (tmp_u1b & BIT(5)) {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
 		rtlefuse->autoload_failflag = false;
 		_rtl92ee_read_adapter_info(hw);
 	} else {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Autoload ERR!!\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Autoload ERR!!\n");
 	}
 	_rtl92ee_hal_customized_behavior(hw);
 
@@ -2184,12 +2210,12 @@ static void rtl92ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 					 u8 rssi_level)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_sta_info *sta_entry = NULL;
 	u32 ratr_bitmap;
 	u8 ratr_index;
-	u8 b_curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
+	u8 curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
 			     ? 1 : 0;
 	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
 				1 : 0;
@@ -2200,11 +2226,11 @@ static void rtl92ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 	u8 rate_mask[7] = {0};
 	u8 macid = 0;
 	/*u8 mimo_ps = IEEE80211_SMPS_OFF;*/
-	sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 	wirelessmode = sta_entry->wireless_mode;
 	if (mac->opmode == NL80211_IFTYPE_STATION ||
 	    mac->opmode == NL80211_IFTYPE_MESH_POINT)
-		b_curtxbw_40mhz = mac->bw_40;
+		curtxbw_40mhz = mac->bw_40;
 	else if (mac->opmode == NL80211_IFTYPE_AP ||
 		 mac->opmode == NL80211_IFTYPE_ADHOC)
 		macid = sta->aid + 1;
@@ -2235,13 +2261,13 @@ static void rtl92ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 			ratr_bitmap &= 0x00000ff5;
 		break;
 	case WIRELESS_MODE_N_24G:
-		if (b_curtxbw_40mhz)
+		if (curtxbw_40mhz)
 			ratr_index = RATR_INX_WIRELESS_NGB;
 		else
 			ratr_index = RATR_INX_WIRELESS_NB;
 
 		if (rtlphy->rf_type == RF_1T1R) {
-			if (b_curtxbw_40mhz) {
+			if (curtxbw_40mhz) {
 				if (rssi_level == 1)
 					ratr_bitmap &= 0x000f0000;
 				else if (rssi_level == 2)
@@ -2257,7 +2283,7 @@ static void rtl92ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 					ratr_bitmap &= 0x000ff005;
 			}
 		} else {
-			if (b_curtxbw_40mhz) {
+			if (curtxbw_40mhz) {
 				if (rssi_level == 1)
 					ratr_bitmap &= 0x0f8f0000;
 				else if (rssi_level == 2)
@@ -2274,9 +2300,8 @@ static void rtl92ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 			}
 		}
 
-		if ((b_curtxbw_40mhz && b_curshortgi_40mhz) ||
-		    (!b_curtxbw_40mhz && b_curshortgi_20mhz)) {
-
+		if ((curtxbw_40mhz && b_curshortgi_40mhz) ||
+		    (!curtxbw_40mhz && b_curshortgi_20mhz)) {
 			if (macid == 0)
 				b_shortgi = true;
 			else if (macid == 1)
@@ -2295,33 +2320,32 @@ static void rtl92ee_update_hal_rate_mask(struct ieee80211_hw *hw,
 	ratr_index = _rtl92ee_mrate_idx_to_arfr_id(hw, ratr_index);
 	sta_entry->ratr_index = ratr_index;
 
-	RT_TRACE(COMP_RATR, DBG_DMESG,
-		 ("ratr_bitmap :%x\n", ratr_bitmap));
-	*(u32 *) &rate_mask = (ratr_bitmap & 0x0fffffff) |
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "ratr_bitmap :%x\n", ratr_bitmap);
+	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) |
 				       (ratr_index << 28);
 	rate_mask[0] = macid;
 	rate_mask[1] = ratr_index | (b_shortgi ? 0x80 : 0x00);
-	rate_mask[2] = b_curtxbw_40mhz;
+	rate_mask[2] = curtxbw_40mhz;
 	rate_mask[3] = (u8)(ratr_bitmap & 0x000000ff);
 	rate_mask[4] = (u8)((ratr_bitmap & 0x0000ff00) >> 8);
 	rate_mask[5] = (u8)((ratr_bitmap & 0x00ff0000) >> 16);
 	rate_mask[6] = (u8)((ratr_bitmap & 0xff000000) >> 24);
-	RT_TRACE(COMP_RATR, DBG_DMESG,
-		 ("Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x:%x:%x\n",
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x:%x:%x\n",
 		  ratr_index, ratr_bitmap, rate_mask[0], rate_mask[1],
 		  rate_mask[2], rate_mask[3], rate_mask[4],
-		  rate_mask[5], rate_mask[6]));
+		  rate_mask[5], rate_mask[6]);
 	rtl92ee_fill_h2c_cmd(hw, H2C_92E_RA_MASK, 7, rate_mask);
 	_rtl92ee_set_bcn_ctrl_reg(hw, BIT(3), 0);
 }
 
-
 void rtl92ee_update_hal_rate_tbl(struct ieee80211_hw *hw,
 				 struct ieee80211_sta *sta, u8 rssi_level)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	if (rtlpriv->dm.b_useramask)
+	if (rtlpriv->dm.useramask)
 		rtl92ee_update_hal_rate_mask(hw, sta, rssi_level);
 }
 
@@ -2332,12 +2356,12 @@ void rtl92ee_update_channel_access_setting(struct ieee80211_hw *hw)
 	u16 sifs_timer;
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-				      (u8 *) &mac->slot_time);
+				      (u8 *)&mac->slot_time);
 	if (!mac->ht_enable)
 		sifs_timer = 0x0a0a;
 	else
 		sifs_timer = 0x0e0e;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *) &sifs_timer);
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *)&sifs_timer);
 }
 
 bool rtl92ee_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
@@ -2372,11 +2396,11 @@ void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
 		u8 cam_offset = 0;
 		u8 clear_number = 5;
 
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("clear_all\n"));
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "clear_all\n");
 
 		for (idx = 0; idx < clear_number; idx++) {
-			stg_rtl_cam_mark_invalid(hw, cam_offset + idx);
-			stg_rtl_cam_empty_entry(hw, cam_offset + idx);
+			rtl_cam_mark_invalid(hw, cam_offset + idx);
+			rtl_cam_empty_entry(hw, cam_offset + idx);
 
 			if (idx < 5) {
 				memset(rtlpriv->sec.key_buf[idx], 0,
@@ -2400,8 +2424,8 @@ void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
 			enc_algo = CAM_AES;
 			break;
 		default:
-			RT_TRACE(COMP_ERR, DBG_LOUD,
-				 ("switch case not process \n"));
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not process\n");
 			enc_algo = CAM_TKIP;
 			break;
 		}
@@ -2416,11 +2440,12 @@ void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
 			} else {
 				if (mac->opmode == NL80211_IFTYPE_AP ||
 				    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-					entry_id = stg_rtl_cam_get_free_entry(hw,
+					entry_id = rtl_cam_get_free_entry(hw,
 								     p_macaddr);
 					if (entry_id >=  TOTAL_CAM_ENTRY) {
-						RT_TRACE(COMP_SEC, DBG_EMERG,
-							 ("Can not find free hw security cam entry\n"));
+						RT_TRACE(rtlpriv, COMP_SEC,
+							 DBG_EMERG,
+							 "Can not find free hw security cam entry\n");
 						return;
 					}
 				} else {
@@ -2433,29 +2458,30 @@ void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
 		}
 
 		if (rtlpriv->sec.key_len[key_index] == 0) {
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("delete one entry, entry_id is %d\n",
-				 entry_id));
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "delete one entry, entry_id is %d\n",
+				 entry_id);
 			if (mac->opmode == NL80211_IFTYPE_AP ||
 			    mac->opmode == NL80211_IFTYPE_MESH_POINT)
-				stg_rtl_cam_del_entry(hw, p_macaddr);
-			stg_rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
+				rtl_cam_del_entry(hw, p_macaddr);
+			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
 		} else {
-			RT_TRACE(COMP_SEC, DBG_DMESG, ("add one entry\n"));
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "add one entry\n");
 			if (is_pairwise) {
-				RT_TRACE(COMP_SEC, DBG_DMESG,
-					 ("set Pairwiase key\n"));
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set Pairwiase key\n");
 
-				stg_rtl_cam_add_one_entry(hw, macaddr, key_index,
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
 					       entry_id, enc_algo,
 					       CAM_CONFIG_NO_USEDK,
 					       rtlpriv->sec.key_buf[key_index]);
 			} else {
-				RT_TRACE(COMP_SEC, DBG_DMESG,
-					 ("set group key\n"));
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set group key\n");
 
 				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-					stg_rtl_cam_add_one_entry(hw,
+					rtl_cam_add_one_entry(hw,
 						rtlefuse->dev_addr,
 						PAIRWISE_KEYIDX,
 						CAM_PAIRWISE_KEY_POSITION,
@@ -2463,7 +2489,7 @@ void rtl92ee_set_key(struct ieee80211_hw *hw, u32 key_index,
 						rtlpriv->sec.key_buf[entry_id]);
 				}
 
-				stg_rtl_cam_add_one_entry(hw, macaddr, key_index,
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
 						entry_id, enc_algo,
 						CAM_CONFIG_NO_USEDK,
 						rtlpriv->sec.key_buf[entry_id]);
@@ -2496,23 +2522,22 @@ void rtl92ee_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
 
 void rtl92ee_bt_reg_init(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	/* 0:Low, 1:High, 2:From Efuse. */
-	rtlpcipriv->btcoexist.b_reg_bt_iso = 2;
+	rtlpriv->btcoexist.reg_bt_iso = 2;
 	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
-	rtlpcipriv->btcoexist.b_reg_bt_sco = 3;
+	rtlpriv->btcoexist.reg_bt_sco = 3;
 	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
-	rtlpcipriv->btcoexist.b_reg_bt_sco = 0;
+	rtlpriv->btcoexist.reg_bt_sco = 0;
 }
 
 void rtl92ee_bt_hw_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	if (rtlpriv->cfg->ops->get_btc_status()) {
+	if (rtlpriv->cfg->ops->get_btc_status())
 		rtlpriv->btcoexist.btc_ops->btc_init_hw_config(rtlpriv);
-	}
 }
 
 void rtl92ee_suspend(struct ieee80211_hw *hw)
@@ -2538,7 +2563,7 @@ void rtl92ee_allow_all_destaddr(struct ieee80211_hw *hw,
 	if (write_into_reg)
 		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
 
-	RT_TRACE(COMP_TURBO | COMP_INIT, DBG_LOUD,
-		 ("receive_config = 0x%08X, write_into_reg =%d\n",
-		  rtlpci->receive_config, write_into_reg));
+	RT_TRACE(rtlpriv, COMP_TURBO | COMP_INIT, DBG_LOUD,
+		 "receive_config=0x%08X, write_into_reg=%d\n",
+		  rtlpci->receive_config, write_into_reg);
 }
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/hw.h b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.h
index e99d7bde1c6f..05413f189685 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/hw.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,7 +26,6 @@
 #ifndef __RTL92E_HW_H__
 #define __RTL92E_HW_H__
 
-
 void rtl92ee_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
 void rtl92ee_read_eeprom_info(struct ieee80211_hw *hw);
 void rtl92ee_interrupt_recognized(struct ieee80211_hw *hw,
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/led.c b/drivers/net/wireless/rtlwifi/rtl8192ee/led.c
index 3b459c93a843..8388e371c8e2 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/led.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/led.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -25,15 +25,15 @@
 
 #include "../wifi.h"
 #include "../pci.h"
-#include "led.h"
 #include "reg.h"
+#include "led.h"
 
 static void _rtl92ee_init_led(struct ieee80211_hw *hw,
 			      struct rtl_led *pled, enum rtl_led_pin ledpin)
 {
 	pled->hw = hw;
 	pled->ledpin = ledpin;
-	pled->b_ledon = false;
+	pled->ledon = false;
 }
 
 void rtl92ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
@@ -41,25 +41,30 @@ void rtl92ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 	u32 ledcfg;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	RT_TRACE(COMP_LED, DBG_LOUD,
-		 ("LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin));
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	switch (pled->ledpin) {
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
-		ledcfg = rtl_read_dword(rtlpriv, REG_GPIO_PIN_CTRL) | BIT(21);
-		ledcfg &= ~BIT(13) & ~BIT(29);
+		ledcfg = rtl_read_dword(rtlpriv , REG_GPIO_PIN_CTRL);
+		ledcfg &= ~BIT(13);
+		ledcfg |= BIT(21);
+		ledcfg &= ~BIT(29);
+
 		rtl_write_dword(rtlpriv, REG_GPIO_PIN_CTRL, ledcfg);
+
 		break;
 	case LED_PIN_LED1:
+
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
-	pled->b_ledon = true;
+	pled->ledon = true;
 }
 
 void rtl92ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
@@ -67,39 +72,45 @@ void rtl92ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 ledcfg;
 
-	RT_TRACE(COMP_LED, DBG_LOUD,
-		 ("LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin));
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	switch (pled->ledpin) {
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
-		ledcfg = rtl_read_dword(rtlpriv , REG_GPIO_PIN_CTRL) | ~BIT(21);
+
+		ledcfg = rtl_read_dword(rtlpriv , REG_GPIO_PIN_CTRL);
+		ledcfg |= ~BIT(21);
 		ledcfg &= ~BIT(29);
 		rtl_write_dword(rtlpriv, REG_GPIO_PIN_CTRL, ledcfg);
+
 		break;
 	case LED_PIN_LED1:
+
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_LOUD,
-			 ("switch case not process\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
-	pled->b_ledon = false;
+	pled->ledon = false;
 }
 
 void rtl92ee_init_sw_leds(struct ieee80211_hw *hw)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	_rtl92ee_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
-	_rtl92ee_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+
+	_rtl92ee_init_led(hw, &pcipriv->ledctl.sw_led0, LED_PIN_LED0);
+	_rtl92ee_init_led(hw, &pcipriv->ledctl.sw_led1, LED_PIN_LED1);
 }
 
 static void _rtl92ee_sw_led_control(struct ieee80211_hw *hw,
 				    enum led_ctl_mode ledaction)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+	struct rtl_led *pLed0 = &pcipriv->ledctl.sw_led0;
+
 	switch (ledaction) {
 	case LED_CTL_POWER_ON:
 	case LED_CTL_LINK:
@@ -129,6 +140,6 @@ void rtl92ee_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction)
 	     ledaction == LED_CTL_POWER_ON)) {
 		return;
 	}
-	RT_TRACE(COMP_LED, DBG_TRACE, ("ledaction %d,\n", ledaction));
+	RT_TRACE(rtlpriv, COMP_LED, DBG_TRACE, "ledaction %d,\n", ledaction);
 	_rtl92ee_sw_led_control(hw, ledaction);
 }
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/led.h b/drivers/net/wireless/rtlwifi/rtl8192ee/led.h
index 7302eda535d9..8ef640a2ef7f 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/led.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/led.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -34,4 +30,5 @@ void rtl92ee_init_sw_leds(struct ieee80211_hw *hw);
 void rtl92ee_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
 void rtl92ee_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
 void rtl92ee_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction);
+
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/phy.c b/drivers/net/wireless/rtlwifi/rtl8192ee/phy.c
index beef284615e5..a863a44f9e16 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/phy.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -41,11 +41,11 @@ static void _rtl92ee_phy_rf_serial_write(struct ieee80211_hw *hw,
 static u32 _rtl92ee_phy_calculate_bit_shift(u32 bitmask);
 static bool _rtl92ee_phy_bb8192ee_config_parafile(struct ieee80211_hw *hw);
 static bool _rtl92ee_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
-static bool _rtl92ee_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						   u8 configtype);
-static bool _rtl92ee_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
-						     u8 configtype);
-static void _rtl92ee_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
+static bool phy_config_bb_with_hdr_file(struct ieee80211_hw *hw,
+					u8 configtype);
+static bool phy_config_bb_with_pghdrfile(struct ieee80211_hw *hw,
+					 u8 configtype);
+static void phy_init_bb_rf_register_def(struct ieee80211_hw *hw);
 static bool _rtl92ee_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
 					      u32 cmdtableidx, u32 cmdtablesz,
 					      enum swchnlcmd_id cmdid,
@@ -65,15 +65,15 @@ u32 rtl92ee_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 returnvalue, originalvalue, bitshift;
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), bitmask(%#x)\n", regaddr, bitmask));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x)\n", regaddr, bitmask);
 	originalvalue = rtl_read_dword(rtlpriv, regaddr);
 	bitshift = _rtl92ee_phy_calculate_bit_shift(bitmask);
 	returnvalue = (originalvalue & bitmask) >> bitshift;
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("BBR MASK = 0x%x Addr[0x%x]= 0x%x\n",
-		  bitmask, regaddr, originalvalue));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "BBR MASK=0x%x Addr[0x%x]=0x%x\n",
+		  bitmask, regaddr, originalvalue);
 
 	return returnvalue;
 }
@@ -84,9 +84,9 @@ void rtl92ee_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 originalvalue, bitshift;
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), bitmask(%#x), data(%#x)\n",
-		  regaddr, bitmask, data));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		  regaddr, bitmask, data);
 
 	if (bitmask != MASKDWORD) {
 		originalvalue = rtl_read_dword(rtlpriv, regaddr);
@@ -96,9 +96,9 @@ void rtl92ee_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
 
 	rtl_write_dword(rtlpriv, regaddr, data);
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), bitmask(%#x), data(%#x)\n",
-		  regaddr, bitmask, data));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		  regaddr, bitmask, data);
 }
 
 u32 rtl92ee_phy_query_rf_reg(struct ieee80211_hw *hw,
@@ -108,22 +108,21 @@ u32 rtl92ee_phy_query_rf_reg(struct ieee80211_hw *hw,
 	u32 original_value, readback_value, bitshift;
 	unsigned long flags;
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
-		  regaddr, rfpath, bitmask));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
+		  regaddr, rfpath, bitmask);
 
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 
-
 	original_value = _rtl92ee_phy_rf_serial_read(hw , rfpath, regaddr);
 	bitshift = _rtl92ee_phy_calculate_bit_shift(bitmask);
 	readback_value = (original_value & bitmask) >> bitshift;
 
 	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
-		  regaddr, rfpath, bitmask, original_value));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x),rfpath(%#x),bitmask(%#x),original_value(%#x)\n",
+		  regaddr, rfpath, bitmask, original_value);
 
 	return readback_value;
 }
@@ -136,9 +135,9 @@ void rtl92ee_phy_set_rf_reg(struct ieee80211_hw *hw,
 	u32 original_value, bitshift;
 	unsigned long flags;
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
-		  addr, bitmask, data, rfpath));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  addr, bitmask, data, rfpath);
 
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 
@@ -152,16 +151,16 @@ void rtl92ee_phy_set_rf_reg(struct ieee80211_hw *hw,
 
 	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
 
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
-		  addr, bitmask, data, rfpath));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  addr, bitmask, data, rfpath);
 }
 
 static u32 _rtl92ee_phy_rf_serial_read(struct ieee80211_hw *hw,
 				       enum radio_path rfpath, u32 offset)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
 	u32 newoffset;
 	u32 tmplong, tmplong2;
@@ -171,7 +170,7 @@ static u32 _rtl92ee_phy_rf_serial_read(struct ieee80211_hw *hw,
 	offset &= 0xff;
 	newoffset = offset;
 	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("return all one\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "return all one\n");
 		return 0xFFFFFFFF;
 	}
 	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
@@ -185,23 +184,22 @@ static u32 _rtl92ee_phy_rf_serial_read(struct ieee80211_hw *hw,
 		      tmplong & (~BLSSIREADEDGE));
 	mdelay(1);
 	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
-	mdelay(1);
-	mdelay(1);
+	mdelay(2);
 	if (rfpath == RF90_PATH_A)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
-						 BIT(8));
+		rfpi_enable = (u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
+						BIT(8));
 	else if (rfpath == RF90_PATH_B)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
-						 BIT(8));
+		rfpi_enable = (u8)rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
+						BIT(8));
 	if (rfpi_enable)
-		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readbackpi,
+		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rbpi,
 					 BLSSIREADBACKDATA);
 	else
-		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readback,
+		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rb,
 					 BLSSIREADBACKDATA);
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("RFR-%d Addr[0x%x]= 0x%x\n",
-		  rfpath, pphyreg->rflssi_readback, retvalue));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "RFR-%d Addr[0x%x]=0x%x\n",
+		  rfpath, pphyreg->rf_rb, retvalue);
 	return retvalue;
 }
 
@@ -212,20 +210,20 @@ static void _rtl92ee_phy_rf_serial_write(struct ieee80211_hw *hw,
 	u32 data_and_addr;
 	u32 newoffset;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
 
 	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("stop\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "stop\n");
 		return;
 	}
 	offset &= 0xff;
 	newoffset = offset;
 	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
 	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("RFW-%d Addr[0x%x]= 0x%x\n", rfpath,
-		  pphyreg->rf3wire_offset, data_and_addr));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "RFW-%d Addr[0x%x]=0x%x\n", rfpath,
+		 pphyreg->rf3wire_offset, data_and_addr);
 }
 
 static u32 _rtl92ee_phy_calculate_bit_shift(u32 bitmask)
@@ -241,20 +239,18 @@ static u32 _rtl92ee_phy_calculate_bit_shift(u32 bitmask)
 
 bool rtl92ee_phy_mac_config(struct ieee80211_hw *hw)
 {
-	bool rtstatus = _rtl92ee_phy_config_mac_with_headerfile(hw);
-
-	return rtstatus;
+	return _rtl92ee_phy_config_mac_with_headerfile(hw);
 }
 
 bool rtl92ee_phy_bb_config(struct ieee80211_hw *hw)
 {
-	bool rtstatus = true;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool rtstatus = true;
 	u16 regval;
 	u32 tmp;
 	u8 crystal_cap;
 
-	_rtl92ee_phy_init_bb_rf_register_definition(hw);
+	phy_init_bb_rf_register_def(hw);
 	regval = rtl_read_word(rtlpriv, REG_SYS_FUNC_EN);
 	rtl_write_word(rtlpriv, REG_SYS_FUNC_EN,
 		       regval | BIT(13) | BIT(0) | BIT(1));
@@ -262,7 +258,7 @@ bool rtl92ee_phy_bb_config(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_RF_CTRL, RF_EN | RF_RSTB | RF_SDMRSTB);
 	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
 		       FEN_PPLL | FEN_PCIEA | FEN_DIO_PCIE |
-		       FEN_BB_GLB_RSTn | FEN_BBRSTB);
+		       FEN_BB_GLB_RSTN | FEN_BBRSTB);
 
 	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
 
@@ -283,7 +279,7 @@ bool rtl92ee_phy_rf_config(struct ieee80211_hw *hw)
 }
 
 static bool _check_condition(struct ieee80211_hw *hw,
-				     const u32  condition)
+			     const u32  condition)
 {
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
@@ -294,17 +290,21 @@ static bool _check_condition(struct ieee80211_hw *hw,
 
 	if (condition == 0xCDCDCDCD)
 		return true;
+
 	cond = condition & 0xFF;
 	if ((_board != cond) && (cond != 0xFF))
 		return false;
+
 	cond = condition & 0xFF00;
 	cond = cond >> 8;
 	if ((_interface & cond) == 0 && cond != 0x07)
 		return false;
+
 	cond = condition & 0xFF0000;
 	cond = cond >> 16;
 	if ((_platform & cond) == 0 && cond != 0x0F)
 		return false;
+
 	return true;
 }
 
@@ -320,6 +320,7 @@ static void _rtl92ee_config_rf_reg(struct ieee80211_hw *hw, u32 addr, u32 data,
 		if (addr == 0xb6) {
 			u32 getvalue;
 			u8 count = 0;
+
 			getvalue = rtl_get_rfreg(hw, rfpath, addr, MASKDWORD);
 			udelay(1);
 
@@ -338,8 +339,8 @@ static void _rtl92ee_config_rf_reg(struct ieee80211_hw *hw, u32 addr, u32 data,
 		if (addr == 0xb2) {
 			u32 getvalue;
 			u8 count = 0;
-			getvalue = rtl_get_rfreg(hw, rfpath, addr, MASKDWORD);
 
+			getvalue = rtl_get_rfreg(hw, rfpath, addr, MASKDWORD);
 			udelay(1);
 
 			while (getvalue != data) {
@@ -403,7 +404,7 @@ static void _rtl92ee_config_bb_reg(struct ieee80211_hw *hw,
 static void _rtl92ee_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	u8 band = BAND_ON_2_4G, rf = 0, txnum = 0, sec = 0;
 
@@ -412,20 +413,23 @@ static void _rtl92ee_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
 			for (; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum)
 				for (; sec < TX_PWR_BY_RATE_NUM_SECTION; ++sec)
 					rtlphy->tx_power_by_rate_offset
-						     [band][rf][txnum][sec] = 0;
+					     [band][rf][txnum][sec] = 0;
 }
 
 static void _rtl92ee_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
 						  u8 band, u8 path,
-						  u8 rate_section, u8 txnum, u8 value)
+						  u8 rate_section, u8 txnum,
+						  u8 value)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	if (path > RF90_PATH_D) {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Rf Path %d\n", path));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Rf Path %d\n", path);
 		return;
 	}
+
 	if (band == BAND_ON_2_4G) {
 		switch (rate_section) {
 		case CCK:
@@ -441,26 +445,31 @@ static void _rtl92ee_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
 			rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
 			break;
 		default:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				 ("Invalid RateSection %d in 2.4G, Rf %d,%dTx\n",
-				  rate_section, path, txnum));
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Invalid RateSection %d in 2.4G,Rf %d,%dTx\n",
+				  rate_section, path, txnum);
 			break;
 		};
 	} else {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Band %d\n", band));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Band %d\n", band);
 	}
 }
 
-static u8 _rtl92ee_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw, u8 band,
-						u8 path, u8 txnum, u8 rate_section)
+static u8 _rtl92ee_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw,
+						u8 band, u8 path, u8 txnum,
+						u8 rate_section)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 value = 0;
+
 	if (path > RF90_PATH_D) {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Rf Path %d\n", path));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Rf Path %d\n", path);
 		return 0;
 	}
+
 	if (band == BAND_ON_2_4G) {
 		switch (rate_section) {
 		case CCK:
@@ -476,13 +485,14 @@ static u8 _rtl92ee_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw, u8 band
 			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
 			break;
 		default:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				 ("Invalid RateSection %d in 2.4G, Rf %d,%dTx\n",
-				  rate_section, path, txnum));
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Invalid RateSection %d in 2.4G,Rf %d,%dTx\n",
+				  rate_section, path, txnum);
 			break;
 		};
 	} else {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Invalid Band %d()\n", band));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Band %d()\n", band);
 	}
 	return value;
 }
@@ -490,36 +500,43 @@ static u8 _rtl92ee_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw, u8 band
 static void _rtl92ee_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u16 raw = 0;
 	u8 base = 0, path = 0;
 
 	for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
 		if (path == RF90_PATH_A) {
-			raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][3] >> 24) & 0xFF;
+			raw = (u16)(rtlphy->tx_power_by_rate_offset
+				    [BAND_ON_2_4G][path][RF_1TX][3] >> 24) &
+				    0xFF;
 			base = (raw >> 4) * 10 + (raw & 0xF);
 			_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
 							      path, CCK, RF_1TX,
 							      base);
 		} else if (path == RF90_PATH_B) {
-			raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][3] >> 0) & 0xFF;
+			raw = (u16)(rtlphy->tx_power_by_rate_offset
+				    [BAND_ON_2_4G][path][RF_1TX][3] >> 0) &
+				    0xFF;
 			base = (raw >> 4) * 10 + (raw & 0xF);
 			_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
 							      path, CCK, RF_1TX,
 							      base);
 		}
-		raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][1] >> 24) & 0xFF;
+		raw = (u16)(rtlphy->tx_power_by_rate_offset
+			    [BAND_ON_2_4G][path][RF_1TX][1] >> 24) & 0xFF;
 		base = (raw >> 4) * 10 + (raw & 0xF);
 		_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path,
 						      OFDM, RF_1TX, base);
 
-		raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][5] >> 24) & 0xFF;
+		raw = (u16)(rtlphy->tx_power_by_rate_offset
+			    [BAND_ON_2_4G][path][RF_1TX][5] >> 24) & 0xFF;
 		base = (raw >> 4) * 10 + (raw & 0xF);
 		_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path,
 						      HT_MCS0_MCS7, RF_1TX,
 						      base);
 
-		raw = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][7] >> 24) & 0xFF;
+		raw = (u16)(rtlphy->tx_power_by_rate_offset
+			    [BAND_ON_2_4G][path][RF_2TX][7] >> 24) & 0xFF;
 		base = (raw >> 4) * 10 + (raw & 0xF);
 		_rtl92ee_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path,
 						      HT_MCS8_MCS15, RF_2TX,
@@ -537,13 +554,13 @@ static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
 	for (i = 3; i >= 0; --i) {
 		if (i >= start && i <= end) {
 			/* Get the exact value */
-			tmp = (u8) (*data >> (i * 8)) & 0xF;
-			tmp += ((u8) ((*data >> (i * 8 + 4)) & 0xF)) * 10;
+			tmp = (u8)(*data >> (i * 8)) & 0xF;
+			tmp += ((u8)((*data >> (i * 8 + 4)) & 0xF)) * 10;
 
 			/* Change the value to a relative value */
 			tmp = (tmp > base) ? tmp - base : base - tmp;
 		} else {
-			tmp = (u8) (*data >> (i * 8)) & 0xFF;
+			tmp = (u8)(*data >> (i * 8)) & 0xFF;
 		}
 		temp_data <<= 8;
 		temp_data |= tmp;
@@ -551,10 +568,10 @@ static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
 	*data = temp_data;
 }
 
-static void _rtl92ee_phy_convert_txpower_dbm_to_relative_value(struct ieee80211_hw *hw)
+static void phy_convert_txpwr_dbm_to_rel_val(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 base = 0, rf = 0, band = BAND_ON_2_4G;
 
 	for (rf = RF90_PATH_A; rf <= RF90_PATH_B; ++rf) {
@@ -563,94 +580,99 @@ static void _rtl92ee_phy_convert_txpower_dbm_to_relative_value(struct ieee80211_
 								     rf, RF_1TX,
 								     CCK);
 			_phy_convert_txpower_dbm_to_relative_value(
-				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][2]),
+				&rtlphy->tx_power_by_rate_offset
+				[band][rf][RF_1TX][2],
 				1, 1, base);
 			_phy_convert_txpower_dbm_to_relative_value(
-				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][3]),
+				&rtlphy->tx_power_by_rate_offset
+				[band][rf][RF_1TX][3],
 				1, 3, base);
 		} else if (rf == RF90_PATH_B) {
 			base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band,
 								     rf, RF_1TX,
 								     CCK);
 			_phy_convert_txpower_dbm_to_relative_value(
-				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][3]),
+				&rtlphy->tx_power_by_rate_offset
+				[band][rf][RF_1TX][3],
 				0, 0, base);
 			_phy_convert_txpower_dbm_to_relative_value(
-				&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][2]),
+				&rtlphy->tx_power_by_rate_offset
+				[band][rf][RF_1TX][2],
 				1, 3, base);
 		}
 		base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band, rf,
 							     RF_1TX, OFDM);
 		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][0]),
+			&rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][0],
 			0, 3, base);
 		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][1]),
+			&rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][1],
 			0, 3, base);
 
 		base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band, rf,
 							     RF_1TX,
 							     HT_MCS0_MCS7);
 		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][4]),
+			&rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][4],
 			0, 3, base);
 		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][5]),
+			&rtlphy->tx_power_by_rate_offset[band][rf][RF_1TX][5],
 			0, 3, base);
+
 		base = _rtl92ee_phy_get_txpower_by_rate_base(hw, band, rf,
 							     RF_2TX,
 							     HT_MCS8_MCS15);
 		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_2TX][6]),
+			&rtlphy->tx_power_by_rate_offset[band][rf][RF_2TX][6],
 			0, 3, base);
+
 		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[band][rf][RF_2TX][7]),
+			&rtlphy->tx_power_by_rate_offset[band][rf][RF_2TX][7],
 			0, 3, base);
 	}
-	RT_TRACE(COMP_POWER, DBG_TRACE,
-		 ("<== _rtl92ee_phy_convert_txpower_dbm_to_relative_value()\n"));
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+		 "<==phy_convert_txpwr_dbm_to_rel_val()\n");
 }
 
 static void _rtl92ee_phy_txpower_by_rate_configuration(struct ieee80211_hw *hw)
 {
 	_rtl92ee_phy_store_txpower_by_rate_base(hw);
-	_rtl92ee_phy_convert_txpower_dbm_to_relative_value(hw);
+	phy_convert_txpwr_dbm_to_rel_val(hw);
 }
 
 static bool _rtl92ee_phy_bb8192ee_config_parafile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	bool rtstatus;
 
-	rtstatus = _rtl92ee_phy_config_bb_with_headerfile(hw,
-						       BASEBAND_CONFIG_PHY_REG);
+	rtstatus = phy_config_bb_with_hdr_file(hw, BASEBAND_CONFIG_PHY_REG);
 	if (!rtstatus) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Write BB Reg Fail!!"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
 		return false;
 	}
 
 	_rtl92ee_phy_init_tx_power_by_rate(hw);
-	if (rtlefuse->autoload_failflag == false) {
+	if (!rtlefuse->autoload_failflag) {
 		rtlphy->pwrgroup_cnt = 0;
-		rtstatus = _rtl92ee_phy_config_bb_with_pgheaderfile(hw,
-						       BASEBAND_CONFIG_PHY_REG);
+		rtstatus =
+		  phy_config_bb_with_pghdrfile(hw, BASEBAND_CONFIG_PHY_REG);
 	}
 	_rtl92ee_phy_txpower_by_rate_configuration(hw);
 	if (!rtstatus) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("BB_PG Reg Fail!!"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
 		return false;
 	}
-	rtstatus = _rtl92ee_phy_config_bb_with_headerfile(hw,
-						       BASEBAND_CONFIG_AGC_TAB);
+	rtstatus = phy_config_bb_with_hdr_file(hw, BASEBAND_CONFIG_AGC_TAB);
 	if (!rtstatus) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("AGC Table Fail\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
 		return false;
 	}
-	rtlphy->bcck_high_power = (bool) (rtl_get_bbreg(hw,
-						       RFPGA0_XA_HSSIPARAMETER2,
-						       0x200));
+	rtlphy->cck_high_power = (bool)(rtl_get_bbreg(hw,
+						      RFPGA0_XA_HSSIPARAMETER2,
+						      0x200));
 
 	return true;
 }
@@ -662,26 +684,26 @@ static bool _rtl92ee_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
 	u32 arraylength;
 	u32 *ptrarray;
 
-	RT_TRACE(COMP_INIT, DBG_TRACE, ("Read Rtl8192EMACPHY_Array\n"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read Rtl8192EMACPHY_Array\n");
 	arraylength = RTL8192EE_MAC_ARRAY_LEN;
 	ptrarray = RTL8192EE_MAC_ARRAY;
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("Img:RTL8192EE_MAC_ARRAY LEN %d\n" , arraylength));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Img:RTL8192EE_MAC_ARRAY LEN %d\n" , arraylength);
 	for (i = 0; i < arraylength; i = i + 2)
-		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
+		rtl_write_byte(rtlpriv, ptrarray[i], (u8)ptrarray[i + 1]);
 	return true;
 }
 
-static bool _rtl92ee_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						   u8 configtype)
-{
-	#define READ_NEXT_PAIR(v1, v2, i) \
-		do { \
-			i += 2; \
-			v1 = array[i]; \
-			v2 = array[i+1]; \
-		} while (0)
+#define READ_NEXT_PAIR(v1, v2, i) \
+	do { \
+		i += 2; \
+		v1 = array[i]; \
+		v2 = array[i+1]; \
+	} while (0)
 
+static bool phy_config_bb_with_hdr_file(struct ieee80211_hw *hw,
+					u8 configtype)
+{
 	int i;
 	u32 *array;
 	u16 len;
@@ -707,11 +729,14 @@ static bool _rtl92ee_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 					READ_NEXT_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
 					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < len - 2)
-					    READ_NEXT_PAIR(v1, v2, i);
+					       v2 != 0xCDCD && i < len - 2) {
+						READ_NEXT_PAIR(v1, v2, i);
+					}
 					i -= 2; /* prevent from for-loop += 2*/
-				} else{/* Configure matched pairs and
-				       * skip to end of if-else. */
+				} else {
+					/* Configure matched pairs and
+					 * skip to end of if-else.
+					 */
 					READ_NEXT_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
 					       v2 != 0xCDEF &&
@@ -727,55 +752,59 @@ static bool _rtl92ee_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
 			}
 		}
 	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
-			len = RTL8192EE_AGC_TAB_ARRAY_LEN;
-			array = RTL8192EE_AGC_TAB_ARRAY;
-
-			for (i = 0; i < len; i = i + 2) {
-				v1 = array[i];
-				v2 = array[i+1];
-				if (v1 < 0xCDCDCDCD) {
-					rtl_set_bbreg(hw, array[i], MASKDWORD,
-						      array[i + 1]);
-					udelay(1);
-					continue;
-			    } else{/*This line is the start line of branch.*/
-				  /* to protect READ_NEXT_PAIR not overrun */
-					if (i >= len - 2)
-						break;
-
-					if (!_check_condition(hw , array[i])) {
-						/*Discard the following pairs*/
-						READ_NEXT_PAIR(v1, v2, i);
-						while (v2 != 0xDEAD &&
-						       v2 != 0xCDEF &&
-						       v2 != 0xCDCD &&
-						       i < len - 2)
-						    READ_NEXT_PAIR(v1, v2, i);
-						i -= 2; /* prevent from for-loop += 2*/
-					} else {/* Configure matched pairs and
-					       * skip to end of if-else.*/
+		len = RTL8192EE_AGC_TAB_ARRAY_LEN;
+		array = RTL8192EE_AGC_TAB_ARRAY;
+
+		for (i = 0; i < len; i = i + 2) {
+			v1 = array[i];
+			v2 = array[i+1];
+			if (v1 < 0xCDCDCDCD) {
+				rtl_set_bbreg(hw, array[i], MASKDWORD,
+					      array[i + 1]);
+				udelay(1);
+				continue;
+		    } else{/*This line is the start line of branch.*/
+			  /* to protect READ_NEXT_PAIR not overrun */
+				if (i >= len - 2)
+					break;
+
+				if (!_check_condition(hw , array[i])) {
+					/*Discard the following pairs*/
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < len - 2) {
 						READ_NEXT_PAIR(v1, v2, i);
-						while (v2 != 0xDEAD &&
-						       v2 != 0xCDEF &&
-						       v2 != 0xCDCD &&
-						       i < len - 2) {
-							rtl_set_bbreg(hw,
-								      array[i],
-								      MASKDWORD,
-								      array[i + 1]);
-							udelay(1);
-							READ_NEXT_PAIR(v1 , v2 , i);
-						}
-
-						while (v2 != 0xDEAD &&
-						       i < len - 2)
-							READ_NEXT_PAIR(v1 , v2 , i);
+					}
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {
+					/* Configure matched pairs and
+					 * skip to end of if-else.
+					 */
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < len - 2) {
+						rtl_set_bbreg(hw,
+							      array[i],
+							      MASKDWORD,
+							      array[i + 1]);
+						udelay(1);
+						READ_NEXT_PAIR(v1 , v2 , i);
+					}
+
+					while (v2 != 0xDEAD &&
+					       i < len - 2) {
+						READ_NEXT_PAIR(v1 , v2 , i);
 					}
 				}
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					 ("The agctab_array_table[0] is %x Rtl818EEPHY_REGArray[1] is %x\n",
-					  array[i],
-					  array[i + 1]));
+			}
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 "The agctab_array_table[0] is %x Rtl818EEPHY_REGArray[1] is %x\n",
+				 array[i],
+				 array[i + 1]);
 		}
 	}
 	return true;
@@ -787,56 +816,42 @@ static u8 _rtl92ee_get_rate_section_index(u32 regaddr)
 
 	switch (regaddr) {
 	case RTXAGC_A_RATE18_06:
+	case RTXAGC_B_RATE18_06:
 		index = 0;
 		break;
 	case RTXAGC_A_RATE54_24:
+	case RTXAGC_B_RATE54_24:
 		index = 1;
 		break;
 	case RTXAGC_A_CCK1_MCS32:
+	case RTXAGC_B_CCK1_55_MCS32:
 		index = 2;
 		break;
 	case RTXAGC_B_CCK11_A_CCK2_11:
 		index = 3;
 		break;
 	case RTXAGC_A_MCS03_MCS00:
-		index = 4;
-		break;
-	case RTXAGC_A_MCS07_MCS04:
-		index = 5;
-		break;
-	case RTXAGC_A_MCS11_MCS08:
-		index = 6;
-		break;
-	case RTXAGC_A_MCS15_MCS12:
-		index = 7;
-		break;
-	case RTXAGC_B_RATE18_06:
-		index = 0;
-		break;
-	case RTXAGC_B_RATE54_24:
-		index = 1;
-		break;
-	case RTXAGC_B_CCK1_55_MCS32:
-		index = 2;
-		break;
 	case RTXAGC_B_MCS03_MCS00:
 		index = 4;
 		break;
+	case RTXAGC_A_MCS07_MCS04:
 	case RTXAGC_B_MCS07_MCS04:
 		index = 5;
 		break;
+	case RTXAGC_A_MCS11_MCS08:
 	case RTXAGC_B_MCS11_MCS08:
 		index = 6;
 		break;
+	case RTXAGC_A_MCS15_MCS12:
 	case RTXAGC_B_MCS15_MCS12:
 		index = 7;
 		break;
 	default:
 		regaddr &= 0xFFF;
 		if (regaddr >= 0xC20 && regaddr <= 0xC4C)
-			index = (u8) ((regaddr - 0xC20) / 4);
+			index = (u8)((regaddr - 0xC20) / 4);
 		else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
-			index = (u8) ((regaddr - 0xE20) / 4);
+			index = (u8)((regaddr - 0xE20) / 4);
 		break;
 	};
 	return index;
@@ -849,28 +864,29 @@ static void _rtl92ee_store_tx_power_by_rate(struct ieee80211_hw *hw,
 					    u32 bitmask, u32 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 section = _rtl92ee_get_rate_section_index(regaddr);
 
 	if (band != BAND_ON_2_4G && band != BAND_ON_5G) {
-		RT_TRACE(FPHY, PHY_TXPWR, ("Invalid Band %d\n", band));
+		RT_TRACE(rtlpriv, FPHY, PHY_TXPWR, "Invalid Band %d\n", band);
 		return;
 	}
 
 	if (rfpath > MAX_RF_PATH - 1) {
-		RT_TRACE(FPHY, PHY_TXPWR, ("Invalid RfPath %d\n", rfpath));
+		RT_TRACE(rtlpriv, FPHY, PHY_TXPWR,
+			 "Invalid RfPath %d\n", rfpath);
 		return;
 	}
 	if (txnum > MAX_RF_PATH - 1) {
-		RT_TRACE(FPHY, PHY_TXPWR, ("Invalid TxNum %d\n", txnum));
+		RT_TRACE(rtlpriv, FPHY, PHY_TXPWR, "Invalid TxNum %d\n", txnum);
 		return;
 	}
 
 	rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][section] = data;
 }
 
-static bool _rtl92ee_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
-						     u8 configtype)
+static bool phy_config_bb_with_pghdrfile(struct ieee80211_hw *hw,
+					 u8 configtype)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int i;
@@ -897,35 +913,35 @@ static bool _rtl92ee_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 			}
 		}
 	} else {
-		RT_TRACE(COMP_SEND, DBG_TRACE,
-			 ("configtype != BaseBand_Config_PHY_REG\n"));
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "configtype != BaseBand_Config_PHY_REG\n");
 	}
 	return true;
 }
 
+#define READ_NEXT_RF_PAIR(v1, v2, i) \
+	do { \
+		i += 2; \
+		v1 = array[i]; \
+		v2 = array[i+1]; \
+	} while (0)
+
 bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw  *hw,
 					   enum radio_path rfpath)
 {
-	#define READ_NEXT_RF_PAIR(v1, v2, i) \
-		do { \
-			i += 2; \
-			v1 = array[i]; \
-			v2 = array[i+1]; \
-		} while (0)
-
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int i;
 	u32 *array;
 	u16 len;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 v1 = 0, v2 = 0;
 
 	switch (rfpath) {
 	case RF90_PATH_A:
 		len = RTL8192EE_RADIOA_ARRAY_LEN;
 		array = RTL8192EE_RADIOA_ARRAY;
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			 ("Radio_A:RTL8192EE_RADIOA_ARRAY %d\n" , len));
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Radio_A:RTL8192EE_RADIOA_ARRAY %d\n" , len);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
 		for (i = 0; i < len; i = i + 2) {
 			v1 = array[i];
 			v2 = array[i+1];
@@ -942,11 +958,14 @@ bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw  *hw,
 					READ_NEXT_RF_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
 					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < len - 2)
+					       v2 != 0xCDCD && i < len - 2) {
 						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
 					i -= 2; /* prevent from for-loop += 2*/
-				} else {/* Configure matched pairs and
-					 * skip to end of if-else.*/
+				} else {
+					/* Configure matched pairs and
+					 * skip to end of if-else.
+					 */
 					READ_NEXT_RF_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
 					       v2 != 0xCDEF &&
@@ -967,9 +986,9 @@ bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw  *hw,
 	case RF90_PATH_B:
 		len = RTL8192EE_RADIOB_ARRAY_LEN;
 		array = RTL8192EE_RADIOB_ARRAY;
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			 ("Radio_A:RTL8192EE_RADIOB_ARRAY %d\n" , len));
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Radio_A:RTL8192EE_RADIOB_ARRAY %d\n" , len);
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
 		for (i = 0; i < len; i = i + 2) {
 			v1 = array[i];
 			v2 = array[i+1];
@@ -986,11 +1005,14 @@ bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw  *hw,
 					READ_NEXT_RF_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
 					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < len - 2)
+					       v2 != 0xCDCD && i < len - 2) {
 						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
 					i -= 2; /* prevent from for-loop += 2*/
-				} else {/* Configure matched pairs and
-					 * skip to end of if-else.*/
+				} else {
+					/* Configure matched pairs and
+					 * skip to end of if-else.
+					 */
 					READ_NEXT_RF_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
 					       v2 != 0xCDEF &&
@@ -1008,12 +1030,7 @@ bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw  *hw,
 		}
 		break;
 	case RF90_PATH_C:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process\n"));
-		break;
 	case RF90_PATH_D:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process\n"));
 		break;
 	}
 	return true;
@@ -1022,38 +1039,38 @@ bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw  *hw,
 void rtl92ee_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	rtlphy->default_initialgain[0] =
-		(u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+		(u8)rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[1] =
-		(u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+		(u8)rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[2] =
-		(u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+		(u8)rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[3] =
-		(u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+		(u8)rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
 
-	RT_TRACE(COMP_INIT, DBG_TRACE,
-		 ("Default initial gain (c50 = 0x%x, c58 = 0x%x, c60 = 0x%x, c68 = 0x%x\n",
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
 		  rtlphy->default_initialgain[0],
 		  rtlphy->default_initialgain[1],
 		  rtlphy->default_initialgain[2],
-		  rtlphy->default_initialgain[3]));
+		  rtlphy->default_initialgain[3]);
 
-	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
-					       ROFDM0_RXDETECTOR3, MASKBYTE0);
+	rtlphy->framesync = (u8)rtl_get_bbreg(hw,
+					      ROFDM0_RXDETECTOR3, MASKBYTE0);
 	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
 					      ROFDM0_RXDETECTOR2, MASKDWORD);
 
-	RT_TRACE(COMP_INIT, DBG_TRACE,
-		 ("Default framesync (0x%x) = 0x%x\n",
-		  ROFDM0_RXDETECTOR3, rtlphy->framesync));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default framesync (0x%x) = 0x%x\n",
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync);
 }
 
-static void _rtl92ee_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
+static void phy_init_bb_rf_register_def(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
 	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
@@ -1072,21 +1089,17 @@ static void _rtl92ee_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
 	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RFPGA0_XA_HSSIPARAMETER2;
 	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RFPGA0_XB_HSSIPARAMETER2;
 
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readback =
-							 RFPGA0_XA_LSSIREADBACK;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readback =
-							 RFPGA0_XB_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rb = RFPGA0_XA_LSSIREADBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rb = RFPGA0_XB_LSSIREADBACK;
 
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readbackpi =
-						      TRANSCEIVEA_HSPI_READBACK;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readbackpi =
-						      TRANSCEIVEB_HSPI_READBACK;
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rbpi = TRANSCEIVEA_HSPI_READBACK;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rbpi = TRANSCEIVEB_HSPI_READBACK;
 }
 
 void rtl92ee_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 txpwr_level;
 	long txpwr_dbm;
 
@@ -1116,7 +1129,6 @@ static u8 _rtl92ee_phy_get_ratesection_intxpower_byrate(enum radio_path path,
 	case DESC92C_RATE1M:
 		rate_section = 2;
 		break;
-
 	case DESC92C_RATE2M:
 	case DESC92C_RATE5_5M:
 		if (path == RF90_PATH_A)
@@ -1124,58 +1136,49 @@ static u8 _rtl92ee_phy_get_ratesection_intxpower_byrate(enum radio_path path,
 		else if (path == RF90_PATH_B)
 			rate_section = 2;
 		break;
-
 	case DESC92C_RATE11M:
 		rate_section = 3;
 		break;
-
 	case DESC92C_RATE6M:
 	case DESC92C_RATE9M:
 	case DESC92C_RATE12M:
 	case DESC92C_RATE18M:
 		rate_section = 0;
 		break;
-
 	case DESC92C_RATE24M:
 	case DESC92C_RATE36M:
 	case DESC92C_RATE48M:
 	case DESC92C_RATE54M:
 		rate_section = 1;
 		break;
-
 	case DESC92C_RATEMCS0:
 	case DESC92C_RATEMCS1:
 	case DESC92C_RATEMCS2:
 	case DESC92C_RATEMCS3:
 		rate_section = 4;
 		break;
-
 	case DESC92C_RATEMCS4:
 	case DESC92C_RATEMCS5:
 	case DESC92C_RATEMCS6:
 	case DESC92C_RATEMCS7:
 		rate_section = 5;
 		break;
-
 	case DESC92C_RATEMCS8:
 	case DESC92C_RATEMCS9:
 	case DESC92C_RATEMCS10:
 	case DESC92C_RATEMCS11:
 		rate_section = 6;
 		break;
-
 	case DESC92C_RATEMCS12:
 	case DESC92C_RATEMCS13:
 	case DESC92C_RATEMCS14:
 	case DESC92C_RATEMCS15:
 		rate_section = 7;
 		break;
-
 	default:
-		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
+		RT_ASSERT(true, "Rate_Section is Illegal\n");
 		break;
 	}
-
 	return rate_section;
 }
 
@@ -1184,7 +1187,7 @@ static u8 _rtl92ee_get_txpower_by_rate(struct ieee80211_hw *hw,
 				       enum radio_path rf, u8 rate)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 shift = 0, sec, tx_num;
 	char diff = 0;
 
@@ -1200,103 +1203,48 @@ static u8 _rtl92ee_get_txpower_by_rate(struct ieee80211_hw *hw,
 
 	switch (rate) {
 	case DESC92C_RATE1M:
-		shift = 0;
-		break;
-	case DESC92C_RATE2M:
-		shift = 8;
-		break;
-	case DESC92C_RATE5_5M:
-		shift = 16;
-		break;
-	case DESC92C_RATE11M:
-		shift = 24;
-		break;
-
 	case DESC92C_RATE6M:
-		shift = 0;
-		break;
-	case DESC92C_RATE9M:
-		shift = 8;
-		break;
-	case DESC92C_RATE12M:
-		shift = 16;
-		break;
-	case DESC92C_RATE18M:
-		shift = 24;
-		break;
-
 	case DESC92C_RATE24M:
-		shift = 0;
-		break;
-	case DESC92C_RATE36M:
-		shift = 8;
-		break;
-	case DESC92C_RATE48M:
-		shift = 16;
-		break;
-	case DESC92C_RATE54M:
-		shift = 24;
-		break;
-
 	case DESC92C_RATEMCS0:
-		shift = 0;
-		break;
-	case DESC92C_RATEMCS1:
-		shift = 8;
-		break;
-	case DESC92C_RATEMCS2:
-		shift = 16;
-		break;
-	case DESC92C_RATEMCS3:
-		shift = 24;
-		break;
-
 	case DESC92C_RATEMCS4:
-		shift = 0;
-		break;
-	case DESC92C_RATEMCS5:
-		shift = 8;
-		break;
-	case DESC92C_RATEMCS6:
-		shift = 16;
-		break;
-	case DESC92C_RATEMCS7:
-		shift = 24;
-		break;
-
 	case DESC92C_RATEMCS8:
-		shift = 0;
-		break;
-	case DESC92C_RATEMCS9:
-		shift = 8;
-		break;
-	case DESC92C_RATEMCS10:
-		shift = 16;
-		break;
-	case DESC92C_RATEMCS11:
-		shift = 24;
-		break;
-
 	case DESC92C_RATEMCS12:
 		shift = 0;
 		break;
+	case DESC92C_RATE2M:
+	case DESC92C_RATE9M:
+	case DESC92C_RATE36M:
+	case DESC92C_RATEMCS1:
+	case DESC92C_RATEMCS5:
+	case DESC92C_RATEMCS9:
 	case DESC92C_RATEMCS13:
 		shift = 8;
 		break;
+	case DESC92C_RATE5_5M:
+	case DESC92C_RATE12M:
+	case DESC92C_RATE48M:
+	case DESC92C_RATEMCS2:
+	case DESC92C_RATEMCS6:
+	case DESC92C_RATEMCS10:
 	case DESC92C_RATEMCS14:
 		shift = 16;
 		break;
+	case DESC92C_RATE11M:
+	case DESC92C_RATE18M:
+	case DESC92C_RATE54M:
+	case DESC92C_RATEMCS3:
+	case DESC92C_RATEMCS7:
+	case DESC92C_RATEMCS11:
 	case DESC92C_RATEMCS15:
 		shift = 24;
 		break;
-
 	default:
-		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
+		RT_ASSERT(true, "Rate_Section is Illegal\n");
 		break;
 	}
 
-	diff = (u8) (rtlphy->tx_power_by_rate_offset[band][rf][tx_num][sec] >>
-		     shift) & 0xff;
+	diff = (u8)(rtlphy->tx_power_by_rate_offset[band][rf][tx_num][sec] >>
+		    shift) & 0xff;
 
 	return	diff;
 }
@@ -1313,8 +1261,8 @@ static u8 _rtl92ee_get_txpower_index(struct ieee80211_hw *hw,
 
 	if (channel < 1 || channel > 14) {
 		index = 0;
-		RT_TRACE(COMP_POWER_TRACKING, DBG_DMESG,
-			 ("Illegal channel!!\n"));
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_DMESG,
+			 "Illegal channel!!\n");
 	}
 
 	if (IS_CCK_RATE(rate))
@@ -1360,250 +1308,253 @@ static void _rtl92ee_set_txpower_index(struct ieee80211_hw *hw, u8 pwr_idx,
 	if (rfpath == RF90_PATH_A) {
 		switch (rate) {
 		case DESC92C_RATE1M:
-			rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE2M:
-			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE5_5M:
-			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE11M:
-			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE6M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE9M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE12M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE18M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE18_06, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE24M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE36M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE48M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE54M:
-			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_RATE54_24, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS0:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS1:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS2:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS3:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS4:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS5:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS6:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS7:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS8:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS9:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS10:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS11:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS12:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS13:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS14:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS15:
-			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE3,
+				      pwr_idx);
 			break;
 		default:
-			RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 "Invalid Rate!!\n");
 			break;
 		}
 	} else if (rfpath == RF90_PATH_B) {
 		switch (rate) {
 		case DESC92C_RATE1M:
-			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE2M:
-			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE5_5M:
-			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE11M:
-			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE6M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE9M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE12M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE18M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE18_06, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE24M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE36M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE48M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATE54M:
-			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_RATE54_24, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS0:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS1:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS2:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS3:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS4:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS5:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS6:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS7:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS8:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS9:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS10:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS11:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE3,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS12:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
-				      MASKBYTE0, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE0,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS13:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
-				      MASKBYTE1, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE1,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS14:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
-				      MASKBYTE2, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE2,
+				      pwr_idx);
 			break;
 		case DESC92C_RATEMCS15:
-			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
-				      MASKBYTE3, pwr_idx);
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE3,
+				      pwr_idx);
 			break;
 		default:
-			RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 "Invalid Rate!!\n");
 			break;
 		}
 	} else {
-		RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid RFPath!!\n"));
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid RFPath!!\n");
 	}
 }
 
-static void rtl92ee_phy_set_txpower_index_by_rate_array(struct ieee80211_hw *hw,
-							enum radio_path rfpath, u8 bw,
-							u8 channel, u8 *rates, u8 size)
+static void phy_set_txpower_index_by_rate_array(struct ieee80211_hw *hw,
+						enum radio_path rfpath, u8 bw,
+						u8 channel, u8 *rates, u8 size)
 {
 	u8 i;
 	u8 power_index;
+
 	for (i = 0; i < size; i++) {
 		power_index = _rtl92ee_get_txpower_index(hw, rfpath, rates[i],
 							 bw, channel);
@@ -1611,20 +1562,20 @@ static void rtl92ee_phy_set_txpower_index_by_rate_array(struct ieee80211_hw *hw,
 	}
 }
 
-static void rtl92ee_phy_set_txpower_index_by_rate_section(struct ieee80211_hw *hw,
-							  enum radio_path rfpath,
-							  u8 channel,
-							  enum rate_section section)
+static void phy_set_txpower_index_by_rate_section(struct ieee80211_hw *hw,
+						  enum radio_path rfpath,
+						  u8 channel,
+						  enum rate_section section)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	if (section == CCK) {
 		u8 cck_rates[] = {DESC92C_RATE1M, DESC92C_RATE2M,
 				  DESC92C_RATE5_5M, DESC92C_RATE11M};
 		if (rtlhal->current_bandtype == BAND_ON_2_4G)
-			rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
+			phy_set_txpower_index_by_rate_array(hw, rfpath,
 							rtlphy->current_chan_bw,
 							channel, cck_rates, 4);
 	} else if (section == OFDM) {
@@ -1632,51 +1583,52 @@ static void rtl92ee_phy_set_txpower_index_by_rate_section(struct ieee80211_hw *h
 				   DESC92C_RATE12M, DESC92C_RATE18M,
 				   DESC92C_RATE24M, DESC92C_RATE36M,
 				   DESC92C_RATE48M, DESC92C_RATE54M};
-		rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
-							rtlphy->current_chan_bw,
-							channel, ofdm_rates, 8);
+		phy_set_txpower_index_by_rate_array(hw, rfpath,
+						    rtlphy->current_chan_bw,
+						    channel, ofdm_rates, 8);
 	} else if (section == HT_MCS0_MCS7) {
 		u8 ht_rates1t[]  = {DESC92C_RATEMCS0, DESC92C_RATEMCS1,
 				    DESC92C_RATEMCS2, DESC92C_RATEMCS3,
 				    DESC92C_RATEMCS4, DESC92C_RATEMCS5,
 				    DESC92C_RATEMCS6, DESC92C_RATEMCS7};
-		rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
-							rtlphy->current_chan_bw,
-							channel, ht_rates1t, 8);
+		phy_set_txpower_index_by_rate_array(hw, rfpath,
+						    rtlphy->current_chan_bw,
+						    channel, ht_rates1t, 8);
 	} else if (section == HT_MCS8_MCS15) {
 		u8 ht_rates2t[]  = {DESC92C_RATEMCS8, DESC92C_RATEMCS9,
 				    DESC92C_RATEMCS10, DESC92C_RATEMCS11,
 				    DESC92C_RATEMCS12, DESC92C_RATEMCS13,
 				    DESC92C_RATEMCS14, DESC92C_RATEMCS15};
-		rtl92ee_phy_set_txpower_index_by_rate_array(hw, rfpath,
-							rtlphy->current_chan_bw,
-							channel, ht_rates2t, 8);
+		phy_set_txpower_index_by_rate_array(hw, rfpath,
+						    rtlphy->current_chan_bw,
+						    channel, ht_rates2t, 8);
 	} else
-		RT_TRACE(FPHY, PHY_TXPWR,
-			 ("Invalid RateSection %d\n", section));
+		RT_TRACE(rtlpriv, FPHY, PHY_TXPWR,
+			 "Invalid RateSection %d\n", section);
 }
 
 void rtl92ee_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
 {
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
+	struct rtl_phy *rtlphy = &rtl_priv(hw)->phy;
 	enum radio_path rfpath;
 
-	if (rtlefuse->b_txpwr_fromeprom == false)
+	if (!rtlefuse->txpwr_fromeprom)
 		return;
 	for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
 	     rfpath++) {
-		rtl92ee_phy_set_txpower_index_by_rate_section(hw, rfpath,
-							      channel, CCK);
-		rtl92ee_phy_set_txpower_index_by_rate_section(hw, rfpath,
-							      channel, OFDM);
-		rtl92ee_phy_set_txpower_index_by_rate_section(hw, rfpath,
-							      channel,
-							      HT_MCS0_MCS7);
+		phy_set_txpower_index_by_rate_section(hw, rfpath,
+						      channel, CCK);
+		phy_set_txpower_index_by_rate_section(hw, rfpath,
+						      channel, OFDM);
+		phy_set_txpower_index_by_rate_section(hw, rfpath,
+						      channel,
+						      HT_MCS0_MCS7);
+
 		if (rtlphy->num_total_rfpath >= 2)
-			rtl92ee_phy_set_txpower_index_by_rate_section(hw,
-								rfpath, channel,
-								HT_MCS8_MCS15);
+			phy_set_txpower_index_by_rate_section(hw,
+							      rfpath, channel,
+							      HT_MCS8_MCS15);
 	}
 }
 
@@ -1723,8 +1675,8 @@ void rtl92ee_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
 						      (u8 *)&iotype);
 			break;
 		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Unknown Scan Backup operation.\n"));
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Unknown Scan Backup operation.\n");
 			break;
 		}
 	}
@@ -1734,15 +1686,15 @@ void rtl92ee_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u8 reg_bw_opmode;
 	u8 reg_prsr_rsc;
 
-	RT_TRACE(COMP_SCAN, DBG_TRACE,
-		 ("Switch to %s bandwidth\n",
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "Switch to %s bandwidth\n",
 		  rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
-		  "20MHz" : "40MHz"));
+		  "20MHz" : "40MHz");
 
 	if (is_hal_stop(rtlhal)) {
 		rtlphy->set_bwmode_inprogress = false;
@@ -1765,8 +1717,8 @@ void rtl92ee_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 		rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_prsr_rsc);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", rtlphy->current_chan_bw);
 		break;
 	}
 
@@ -1790,20 +1742,20 @@ void rtl92ee_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 			       HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", rtlphy->current_chan_bw);
 		break;
 	}
 	rtl92ee_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
 	rtlphy->set_bwmode_inprogress = false;
-	RT_TRACE(COMP_SCAN, DBG_LOUD, ("\n"));
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD, "\n");
 }
 
 void rtl92ee_phy_set_bw_mode(struct ieee80211_hw *hw,
 			     enum nl80211_channel_type ch_type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 tmp_bw = rtlphy->current_chan_bw;
 
@@ -1813,8 +1765,8 @@ void rtl92ee_phy_set_bw_mode(struct ieee80211_hw *hw,
 	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
 		rtl92ee_phy_set_bw_mode_callback(hw);
 	} else {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("false driver sleep or unload\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "false driver sleep or unload\n");
 		rtlphy->set_bwmode_inprogress = false;
 		rtlphy->current_chan_bw = tmp_bw;
 	}
@@ -1824,11 +1776,11 @@ void rtl92ee_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 delay;
 
-	RT_TRACE(COMP_SCAN, DBG_TRACE,
-		 ("switch to channel%d\n", rtlphy->current_channel));
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "switch to channel%d\n", rtlphy->current_channel);
 	if (is_hal_stop(rtlhal))
 		return;
 	do {
@@ -1846,13 +1798,13 @@ void rtl92ee_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 		}
 		break;
 	} while (true);
-	RT_TRACE(COMP_SCAN, DBG_TRACE, ("\n"));
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
 }
 
 u8 rtl92ee_phy_sw_chnl(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	if (rtlphy->sw_chnl_inprogress)
@@ -1860,19 +1812,19 @@ u8 rtl92ee_phy_sw_chnl(struct ieee80211_hw *hw)
 	if (rtlphy->set_bwmode_inprogress)
 		return 0;
 	RT_ASSERT((rtlphy->current_channel <= 14),
-		  ("WIRELESS_MODE_G but channel>14"));
+		  "WIRELESS_MODE_G but channel>14");
 	rtlphy->sw_chnl_inprogress = true;
 	rtlphy->sw_chnl_stage = 0;
 	rtlphy->sw_chnl_step = 0;
 	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
 		rtl92ee_phy_sw_chnl_callback(hw);
-		RT_TRACE(COMP_CHAN, DBG_LOUD,
-			 ("sw_chnl_inprogress false schdule workitem current channel %d\n",
-			 rtlphy->current_channel));
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 "sw_chnl_inprogress false schdule workitem current channel %d\n",
+			 rtlphy->current_channel);
 		rtlphy->sw_chnl_inprogress = false;
 	} else {
-		RT_TRACE(COMP_CHAN, DBG_LOUD,
-			 ("sw_chnl_inprogress false driver sleep or unload\n"));
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 "sw_chnl_inprogress false driver sleep or unload\n");
 		rtlphy->sw_chnl_inprogress = false;
 	}
 	return 1;
@@ -1883,7 +1835,7 @@ static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 					      u32 *delay)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
 	u32 precommoncmdcnt;
 	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
@@ -1909,7 +1861,7 @@ static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 	rfdependcmdcnt = 0;
 
 	RT_ASSERT((channel >= 1 && channel <= 14),
-		  ("illegal channel for Zebra: %d\n", channel));
+		  "illegal channel for Zebra: %d\n", channel);
 
 	_rtl92ee_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
 					  MAX_RFDEPENDCMD_CNT,
@@ -1932,21 +1884,17 @@ static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 			currentcmd = &postcommoncmd[*step];
 			break;
 		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Invalid 'stage' = %d, Check it!\n" ,
-				  *stage));
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Invalid 'stage' = %d, Check it!\n" , *stage);
 			return true;
-			break;
 		}
 
 		if (currentcmd->cmdid == CMDID_END) {
-			if ((*stage) == 2) {
+			if ((*stage) == 2)
 				return true;
-			} else {
-				(*stage)++;
-				(*step) = 0;
-				continue;
-			}
+			(*stage)++;
+			(*step) = 0;
+			continue;
 		}
 
 		switch (currentcmd->cmdid) {
@@ -1959,11 +1907,11 @@ static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 			break;
 		case CMDID_WRITEPORT_USHORT:
 			rtl_write_word(rtlpriv, currentcmd->para1,
-				       (u16) currentcmd->para2);
+				       (u16)currentcmd->para2);
 			break;
 		case CMDID_WRITEPORT_UCHAR:
 			rtl_write_byte(rtlpriv, currentcmd->para1,
-				       (u8) currentcmd->para2);
+				       (u8)currentcmd->para2);
 			break;
 		case CMDID_RF_WRITEREG:
 			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
@@ -1978,8 +1926,8 @@ static bool _rtl92ee_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
 			}
 			break;
 		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("switch case not process\n"));
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 
@@ -1999,7 +1947,7 @@ static bool _rtl92ee_phy_set_sw_chnl_cmdarray(struct swchnlcmd *cmdtable,
 	struct swchnlcmd *pcmd;
 
 	if (cmdtable == NULL) {
-		RT_ASSERT(false, ("cmdtable cannot be NULL.\n"));
+		RT_ASSERT(false, "cmdtable cannot be NULL.\n");
 		return false;
 	}
 
@@ -2024,20 +1972,20 @@ static u8 _rtl92ee_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x180);
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
 
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x18008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
 
-	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82140303);
-	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x68160000);
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82140303);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x68160000);
 
 	/*LO calibration setting*/
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x00462911);
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
 
 	/*One shot, path A LOK & IQK*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf9000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
 	mdelay(IQK_DELAY_TIME);
 
@@ -2068,20 +2016,20 @@ static u8 _rtl92ee_phy_path_b_iqk(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x00000000);
 	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
 
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x18008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
 
-	rtl_set_bbreg(hw, RTx_IQK_PI_B, MASKDWORD, 0x821403e2);
-	rtl_set_bbreg(hw, RRx_IQK_PI_B, MASKDWORD, 0x68160000);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x821403e2);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x68160000);
 
 	/* LO calibration setting */
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x00462911);
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
 
 	/*One shot, path B LOK & IQK*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
 	mdelay(IQK_DELAY_TIME);
 
@@ -2121,31 +2069,31 @@ static u8 _rtl92ee_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
 
 	/*IQK Setting*/
-	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
-	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
 
 	/*path a IQK setting*/
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x18008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
 
-	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160c1f);
-	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x68160c1f);
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x68160c1f);
 
 	/*LO calibration Setting*/
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
 
-	/*one shot, path A LOK & iqk*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+	/*one shot,path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
 	mdelay(IQK_DELAY_TIME);
 
 	/* Check failed */
-	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
-	reg_e94 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_A, MASKDWORD);
-	reg_e9c = rtl_get_bbreg(hw, RTx_Power_After_IQK_A, MASKDWORD);
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
 
 	if (!(reg_eac & BIT(28)) &&
 	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
@@ -2160,7 +2108,7 @@ static u8 _rtl92ee_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 
 	u32temp = 0x80007C00 | (reg_e94 & 0x3FF0000)  |
 		  ((reg_e9c & 0x3FF0000) >> 16);
-	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, u32temp);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32temp);
 	/*RX IQK*/
 	/*Modify RX IQK mode table*/
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
@@ -2179,27 +2127,27 @@ static u8 _rtl92ee_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
 
 	/*IQK Setting*/
-	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
 
 	/*path a IQK setting*/
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x18008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
 
-	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160c1f);
-	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x28160c1f);
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28160c1f);
 
 	/*LO calibration Setting*/
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a891);
-	/*one shot, path A LOK & iqk*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a891);
+	/*one shot,path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
 	mdelay(IQK_DELAY_TIME);
 	/*Check failed*/
-	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
-	reg_ea4 = rtl_get_bbreg(hw, RRx_Power_Before_IQK_A_2, MASKDWORD);
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
 
 	/*PA/PAD controlled by 0x0*/
 	/*leave IQK mode*/
@@ -2236,31 +2184,31 @@ static u8 _rtl92ee_phy_path_b_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
 
 	/*IQK Setting*/
-	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
-	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
 
 	/*path a IQK setting*/
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x18008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
 
-	rtl_set_bbreg(hw, RTx_IQK_PI_B, MASKDWORD, 0x82160c1f);
-	rtl_set_bbreg(hw, RRx_IQK_PI_B, MASKDWORD, 0x68160c1f);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x68160c1f);
 
 	/*LO calibration Setting*/
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
 
-	/*one shot, path A LOK & iqk*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+	/*one shot,path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
 	mdelay(IQK_DELAY_TIME);
 
 	/* Check failed */
-	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
-	reg_eb4 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_B, MASKDWORD);
-	reg_ebc = rtl_get_bbreg(hw, RTx_Power_After_IQK_B, MASKDWORD);
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_eb4 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_B, MASKDWORD);
+	reg_ebc = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_B, MASKDWORD);
 
 	if (!(reg_eac & BIT(31)) &&
 	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
@@ -2275,7 +2223,7 @@ static u8 _rtl92ee_phy_path_b_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 
 	u32temp = 0x80007C00 | (reg_eb4 & 0x3FF0000) |
 		  ((reg_ebc & 0x3FF0000) >> 16);
-	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, u32temp);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32temp);
 	/*RX IQK*/
 	/*Modify RX IQK mode table*/
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
@@ -2293,28 +2241,28 @@ static u8 _rtl92ee_phy_path_b_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
 
 	/*IQK Setting*/
-	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
 
 	/*path b IQK setting*/
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_Tone_B, MASKDWORD, 0x38008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_B, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x18008c1c);
 
-	rtl_set_bbreg(hw, RTx_IQK_PI_B, MASKDWORD, 0x82160c1f);
-	rtl_set_bbreg(hw, RRx_IQK_PI_B, MASKDWORD, 0x28160c1f);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82160c1f);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28160c1f);
 
 	/*LO calibration Setting*/
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a891);
-	/*one shot, path A LOK & iqk*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xfa000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a891);
+	/*one shot,path A LOK & iqk*/
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xfa000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
 	mdelay(IQK_DELAY_TIME);
 	/*Check failed*/
-	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
-	reg_ec4 = rtl_get_bbreg(hw, RRx_Power_Before_IQK_B_2, MASKDWORD);
-	reg_ecc = rtl_get_bbreg(hw, RRx_Power_After_IQK_B_2, MASKDWORD);
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_ec4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_B_2, MASKDWORD);
+	reg_ecc = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_B_2, MASKDWORD);
 	/*PA/PAD controlled by 0x0*/
 	/*leave IQK mode*/
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
@@ -2325,7 +2273,7 @@ static u8 _rtl92ee_phy_path_b_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
 		result |= 0x02;
 	else
-		RT_TRACE(COMP_RF, DBG_LOUD, ("Path B Rx IQK fail!!\n"));
+		RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD, "Path B Rx IQK fail!!\n");
 
 	return result;
 }
@@ -2459,7 +2407,7 @@ static void _rtl92ee_phy_reload_mac_registers(struct ieee80211_hw *hw,
 	u32 i;
 
 	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++)
-		rtl_write_byte(rtlpriv, macreg[i], (u8) macbackup[i]);
+		rtl_write_byte(rtlpriv, macreg[i], (u8)macbackup[i]);
 	rtl_write_dword(rtlpriv, macreg[i], macbackup[i]);
 }
 
@@ -2549,57 +2497,52 @@ static bool _rtl92ee_phy_simularity_compare(struct ieee80211_hw *hw,
 			}
 		}
 		return bresult;
-	} else {
-		if (!(simularity_bitmap & 0x03)) {/*path A TX OK*/
-			for (i = 0; i < 2; i++)
-				result[3][i] = result[c1][i];
-		}
-		if (!(simularity_bitmap & 0x0c)) {/*path A RX OK*/
-			for (i = 2; i < 4; i++)
-				result[3][i] = result[c1][i];
-		}
-		if (!(simularity_bitmap & 0x30)) {/*path B TX OK*/
-			for (i = 4; i < 6; i++)
-				result[3][i] = result[c1][i];
-		}
-		if (!(simularity_bitmap & 0xc0)) {/*path B RX OK*/
-			for (i = 6; i < 8; i++)
-				result[3][i] = result[c1][i];
-		}
-		return false;
 	}
+	if (!(simularity_bitmap & 0x03)) {/*path A TX OK*/
+		for (i = 0; i < 2; i++)
+			result[3][i] = result[c1][i];
+	}
+	if (!(simularity_bitmap & 0x0c)) {/*path A RX OK*/
+		for (i = 2; i < 4; i++)
+			result[3][i] = result[c1][i];
+	}
+	if (!(simularity_bitmap & 0x30)) {/*path B TX OK*/
+		for (i = 4; i < 6; i++)
+			result[3][i] = result[c1][i];
+	}
+	if (!(simularity_bitmap & 0xc0)) {/*path B RX OK*/
+		for (i = 6; i < 8; i++)
+			result[3][i] = result[c1][i];
+	}
+	return false;
 }
 
 static void _rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw,
 				      long result[][8], u8 t, bool is2t)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 i;
 	u8 patha_ok, pathb_ok;
-	u8 tmp_0xc50 = (u8) rtl_get_bbreg(hw, 0xc50, MASKBYTE0);
-	u8 tmp_0xc58 = (u8) rtl_get_bbreg(hw, 0xc58, MASKBYTE0);
+	u8 tmp_0xc50 = (u8)rtl_get_bbreg(hw, 0xc50, MASKBYTE0);
+	u8 tmp_0xc58 = (u8)rtl_get_bbreg(hw, 0xc58, MASKBYTE0);
 	u32 adda_reg[IQK_ADDA_REG_NUM] = {
 		0x85c, 0xe6c, 0xe70, 0xe74,
 		0xe78, 0xe7c, 0xe80, 0xe84,
 		0xe88, 0xe8c, 0xed0, 0xed4,
 		0xed8, 0xedc, 0xee0, 0xeec
 	};
-
 	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
 		0x522, 0x550, 0x551, 0x040
 	};
-
 	u32 iqk_bb_reg[IQK_BB_REG_NUM] = {
 		ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR,
 		RFPGA0_XCD_RFINTERFACESW, 0xb68, 0xb6c,
 		0x870, 0x860,
 		0x864, 0x800
 	};
-
 	const u32 retrycount = 2;
 
-
 	if (t == 0) {
 		_rtl92ee_phy_save_adda_registers(hw, adda_reg,
 						 rtlphy->adda_backup,
@@ -2629,55 +2572,53 @@ static void _rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw,
 	/* Page B init*/
 	/* IQ calibration setting*/
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
-	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
-	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
 
-	for (i = 0; i < retrycount; i++) {
+	for (i = 0 ; i < retrycount ; i++) {
 		patha_ok = _rtl92ee_phy_path_a_iqk(hw, is2t);
 
 		if (patha_ok == 0x01) {
-			RT_TRACE(COMP_RF, DBG_LOUD,
-				 ("Path A Tx IQK Success!!\n"));
+			RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+				 "Path A Tx IQK Success!!\n");
 			result[t][0] = (rtl_get_bbreg(hw,
-						      RTx_Power_Before_IQK_A,
+						      RTX_POWER_BEFORE_IQK_A,
 						      MASKDWORD) & 0x3FF0000)
 						      >> 16;
-			result[t][1] = (rtl_get_bbreg(hw, RTx_Power_After_IQK_A,
+			result[t][1] = (rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A,
 						      MASKDWORD) & 0x3FF0000)
 						      >> 16;
 			break;
-		} else {
-			RT_TRACE(COMP_RF, DBG_LOUD,
-				 ("Path A Tx IQK Fail!!, ret = 0x%x\n",
-				  patha_ok));
 		}
+		RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+			 "Path A Tx IQK Fail!!, ret = 0x%x\n",
+			 patha_ok);
 	}
 
 	for (i = 0 ; i < retrycount ; i++) {
 		patha_ok = _rtl92ee_phy_path_a_rx_iqk(hw, is2t);
 
 		if (patha_ok == 0x03) {
-			RT_TRACE(COMP_RF, DBG_LOUD,
-				 ("Path A Rx IQK Success!!\n"));
+			RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+				 "Path A Rx IQK Success!!\n");
 			result[t][2] = (rtl_get_bbreg(hw,
-						      RRx_Power_Before_IQK_A_2,
+						      RRX_POWER_BEFORE_IQK_A_2,
 						      MASKDWORD) & 0x3FF0000)
 						      >> 16;
 			result[t][3] = (rtl_get_bbreg(hw,
-						      RRx_Power_After_IQK_A_2,
+						      RRX_POWER_AFTER_IQK_A_2,
 						      MASKDWORD) & 0x3FF0000)
 						      >> 16;
 			break;
-		} else {
-			RT_TRACE(COMP_RF, DBG_LOUD,
-				 ("Path A Rx IQK Fail!!, ret = 0x%x\n",
-				  patha_ok));
 		}
+		RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+			 "Path A Rx IQK Fail!!, ret = 0x%x\n",
+			  patha_ok);
 	}
 
 	if (0x00 == patha_ok)
-		RT_TRACE(COMP_RF, DBG_LOUD, ("Path A IQK failed!!, ret = 0\n"));
-
+		RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+			 "Path A IQK failed!!, ret = 0\n");
 	if (is2t) {
 		_rtl92ee_phy_path_a_standby(hw);
 		/* Turn Path B ADDA on */
@@ -2685,58 +2626,56 @@ static void _rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw,
 
 		/* IQ calibration setting */
 		rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
-		rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
-		rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
+		rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+		rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
 
 		for (i = 0 ; i < retrycount ; i++) {
 			pathb_ok = _rtl92ee_phy_path_b_iqk(hw);
 			if (pathb_ok == 0x01) {
-				RT_TRACE(COMP_RF, DBG_LOUD,
-					 ("Path B Tx IQK Success!!\n"));
+				RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+					 "Path B Tx IQK Success!!\n");
 				result[t][4] = (rtl_get_bbreg(hw,
-							RTx_Power_Before_IQK_B,
+							RTX_POWER_BEFORE_IQK_B,
 							MASKDWORD) & 0x3FF0000)
 							>> 16;
 				result[t][5] = (rtl_get_bbreg(hw,
-							RTx_Power_After_IQK_B,
+							RTX_POWER_AFTER_IQK_B,
 							MASKDWORD) & 0x3FF0000)
 							>> 16;
 				break;
-			} else {
-				RT_TRACE(COMP_RF, DBG_LOUD,
-					 ("Path B Tx IQK Fail!!, ret = 0x%x\n",
-					   pathb_ok));
 			}
+			RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+				 "Path B Tx IQK Fail!!, ret = 0x%x\n",
+				 pathb_ok);
 		}
 
 		for (i = 0 ; i < retrycount ; i++) {
 			pathb_ok = _rtl92ee_phy_path_b_rx_iqk(hw, is2t);
 			if (pathb_ok == 0x03) {
-				RT_TRACE(COMP_RF, DBG_LOUD,
-					 ("Path B Rx IQK Success!!\n"));
+				RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+					 "Path B Rx IQK Success!!\n");
 				result[t][6] = (rtl_get_bbreg(hw,
-						       RRx_Power_Before_IQK_B_2,
+						       RRX_POWER_BEFORE_IQK_B_2,
 						       MASKDWORD) & 0x3FF0000)
 						       >> 16;
 				result[t][7] = (rtl_get_bbreg(hw,
-						       RRx_Power_After_IQK_B_2,
+						       RRX_POWER_AFTER_IQK_B_2,
 						       MASKDWORD) & 0x3FF0000)
 						       >> 16;
 				break;
-			} else {
-				RT_TRACE(COMP_RF, DBG_LOUD,
-					 ("Path B Rx IQK Fail!!, ret = 0x%x\n",
-					 pathb_ok));
 			}
+			RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+				 "Path B Rx IQK Fail!!, ret = 0x%x\n",
+				 pathb_ok);
 		}
 
 		if (0x00 == pathb_ok)
-			RT_TRACE(COMP_RF, DBG_LOUD,
-				 ("Path B IQK failed!!, ret = 0\n"));
+			RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+				 "Path B IQK failed!!, ret = 0\n");
 	}
 	/* Back to BB mode, load original value */
-	RT_TRACE(COMP_RF, DBG_LOUD,
-		 ("IQK:Back to BB mode, load original value!\n"));
+	RT_TRACE(rtlpriv, COMP_RF, DBG_LOUD,
+		 "IQK:Back to BB mode, load original value!\n");
 	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0);
 
 	if (t != 0) {
@@ -2762,10 +2701,9 @@ static void _rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw,
 		}
 
 		/* load 0xe30 IQC default value */
-		rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x01008c00);
-		rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x01008c00);
+		rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x01008c00);
+		rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x01008c00);
 	}
-	RT_TRACE(COMP_RF, DBG_LOUD, ("_rtl92ee_phy_iq_calibrate() <==\n"));
 }
 
 static void _rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
@@ -2811,7 +2749,6 @@ static void _rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 	} else {
 		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
 	}
-	RT_TRACE(COMP_INIT , DBG_LOUD , ("\n"));
 }
 
 static void _rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw,
@@ -2820,13 +2757,15 @@ static void _rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	RT_TRACE(COMP_INIT , DBG_LOUD , ("\n"));
+
+	RT_TRACE(rtlpriv, COMP_INIT , DBG_LOUD , "\n");
 
 	if (is_hal_stop(rtlhal)) {
 		u8 u1btmp;
+
 		u1btmp = rtl_read_byte(rtlpriv, REG_LEDCFG0);
 		rtl_write_byte(rtlpriv, REG_LEDCFG0, u1btmp | BIT(7));
-		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+		rtl_set_bbreg(hw, RFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
 	}
 	if (is2t) {
 		if (bmain)
@@ -2841,21 +2780,22 @@ static void _rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw,
 
 		/* We use the RF definition of MAIN and AUX,
 		 * left antenna and right antenna repectively.
-		 * Default output at AUX.*/
+		 * Default output at AUX.
+		 */
 		if (bmain) {
 			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
 				      BIT(14) | BIT(13) | BIT(12), 0);
 			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
 				      BIT(5) | BIT(4) | BIT(3), 0);
 			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
-				rtl_set_bbreg(hw, rConfig_ram64x16, BIT(31), 0);
+				rtl_set_bbreg(hw, RCONFIG_RAM64x16, BIT(31), 0);
 		} else {
 			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
 				      BIT(14) | BIT(13) | BIT(12), 1);
 			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
 				      BIT(5) | BIT(4) | BIT(3), 1);
 			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
-				rtl_set_bbreg(hw, rConfig_ram64x16, BIT(31), 1);
+				rtl_set_bbreg(hw, RCONFIG_RAM64x16, BIT(31), 1);
 		}
 	}
 }
@@ -2888,7 +2828,7 @@ static u8 rtl92ee_get_rightchnlplace_for_iqk(u8 chnl)
 void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	long result[4][8];
 	u8 i, final_candidate;
 	bool b_patha_ok, b_pathb_ok;
@@ -2959,6 +2899,7 @@ void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 				final_candidate = 3;
 		}
 	}
+
 	for (i = 0; i < 4; i++) {
 		reg_e94 = result[i][0];
 		reg_e9c = result[i][1];
@@ -2969,6 +2910,7 @@ void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 		reg_ec4 = result[i][6];
 		reg_ecc = result[i][7];
 	}
+
 	if (final_candidate != 0xff) {
 		reg_e94 = result[final_candidate][0];
 		rtlphy->reg_e94 = reg_e94;
@@ -2991,7 +2933,7 @@ void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 		rtlphy->reg_ebc = 0x0;
 	}
 
-	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
+	if (reg_e94 != 0)
 		_rtl92ee_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
 						    final_candidate,
 						    (reg_ea4 == 0));
@@ -3002,14 +2944,13 @@ void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 
 	idx = rtl92ee_get_rightchnlplace_for_iqk(rtlphy->current_channel);
 
-	/* To Fix BSOD when final_candidate is 0xff
-	 * by sherry 20120321 */
+	/* To Fix BSOD when final_candidate is 0xff */
 	if (final_candidate < 4) {
 		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
-			rtlphy->iqk_matrix_regsetting[idx].value[0][i] =
+			rtlphy->iqk_matrix[idx].value[0][i] =
 				result[final_candidate][i];
 
-		rtlphy->iqk_matrix_regsetting[idx].b_iqk_done = true;
+		rtlphy->iqk_matrix[idx].iqk_done = true;
 	}
 	_rtl92ee_phy_save_adda_registers(hw, iqk_bb_reg,
 					 rtlphy->iqk_bb_backup, 9);
@@ -3018,8 +2959,8 @@ void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 void rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = &rtlpriv->rtlhal;
 	u32 timeout = 2000, timecount = 0;
 
 	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
@@ -3039,13 +2980,6 @@ void rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw)
 
 void rtl92ee_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (rtlphy->b_apk_done)
-		return;
-
-	return;
 }
 
 void rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
@@ -3056,71 +2990,72 @@ void rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
 bool rtl92ee_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	bool b_postprocessing = false;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	bool postprocessing = false;
 
-	RT_TRACE(COMP_CMD, DBG_TRACE,
-		 ("-->IO Cmd(%#x), set_io_inprogress(%d)\n",
-		  iotype, rtlphy->set_io_inprogress));
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
+		  iotype, rtlphy->set_io_inprogress);
 	do {
 		switch (iotype) {
 		case IO_CMD_RESUME_DM_BY_SCAN:
-			RT_TRACE(COMP_CMD, DBG_TRACE,
-				 ("[IO CMD] Resume DM after scan.\n"));
-			b_postprocessing = true;
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Resume DM after scan.\n");
+			postprocessing = true;
 			break;
 		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
-			RT_TRACE(COMP_CMD, DBG_TRACE,
-				 ("[IO CMD] Pause DM before scan.\n"));
-			b_postprocessing = true;
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Pause DM before scan.\n");
+			postprocessing = true;
 			break;
 		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("switch case not process\n"));
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			break;
 		}
 	} while (false);
-	if (b_postprocessing && !rtlphy->set_io_inprogress) {
+	if (postprocessing && !rtlphy->set_io_inprogress) {
 		rtlphy->set_io_inprogress = true;
 		rtlphy->current_io_type = iotype;
 	} else {
 		return false;
 	}
 	rtl92ee_phy_set_io(hw);
-	RT_TRACE(COMP_CMD, DBG_TRACE, ("IO Type(%#x)\n", iotype));
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
 	return true;
 }
 
 static void rtl92ee_phy_set_io(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct dig_t *dm_dig = &rtlpriv->dm_digtable;
 
-	RT_TRACE(COMP_CMD, DBG_TRACE,
-		 ("--->Cmd(%#x), set_io_inprogress(%d)\n",
-		  rtlphy->current_io_type, rtlphy->set_io_inprogress));
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
 	switch (rtlphy->current_io_type) {
 	case IO_CMD_RESUME_DM_BY_SCAN:
-		rtl92ee_dm_write_dig(hw , rtlphy->initgain_backup.xaagccore1);
+		rtl92ee_dm_write_dig(hw, rtlphy->initgain_backup.xaagccore1);
 		rtl92ee_dm_write_cck_cca_thres(hw, rtlphy->initgain_backup.cca);
-		RT_TRACE(COMP_CMD, DBG_TRACE , ("no set txpower\n"));
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE , "no set txpower\n");
 		rtl92ee_phy_set_txpower_level(hw, rtlphy->current_channel);
 		break;
 	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
 		/* 8192eebt */
-		rtlphy->initgain_backup.xaagccore1 = dm_dig.cur_igvalue;
+		rtlphy->initgain_backup.xaagccore1 = dm_dig->cur_igvalue;
 		rtl92ee_dm_write_dig(hw, 0x17);
-		rtlphy->initgain_backup.cca = dm_dig.cur_cck_cca_thres;
+		rtlphy->initgain_backup.cca = dm_dig->cur_cck_cca_thres;
 		rtl92ee_dm_write_cck_cca_thres(hw, 0x40);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
 	rtlphy->set_io_inprogress = false;
-	RT_TRACE(COMP_CMD, DBG_TRACE,
-		 ("(%#x)\n", rtlphy->current_io_type));
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "(%#x)\n", rtlphy->current_io_type);
 }
 
 static void rtl92ee_phy_set_rf_on(struct ieee80211_hw *hw)
@@ -3162,19 +3097,20 @@ static bool _rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		if ((ppsc->rfpwr_state == ERFOFF) &&
 		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
 			bool rtstatus;
-			u32 init_count = 0;
+			u32 initializecount = 0;
+
 			do {
-				init_count++;
-				RT_TRACE(COMP_RF, DBG_DMESG,
-					 ("IPS Set eRf nic enable\n"));
-				rtstatus = stg_rtl_ps_enable_nic(hw);
-			} while (!rtstatus && (init_count < 10));
+				initializecount++;
+				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+					 "IPS Set eRf nic enable\n");
+				rtstatus = rtl_ps_enable_nic(hw);
+			} while (!rtstatus && (initializecount < 10));
 			RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
 		} else {
-			RT_TRACE(COMP_RF, DBG_DMESG,
-				 ("Set ERFON sleeped:%d ms\n",
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "Set ERFON sleeping:%d ms\n",
 				  jiffies_to_msecs(jiffies -
-						   ppsc->last_sleep_jiffies)));
+						   ppsc->last_sleep_jiffies));
 			ppsc->last_awake_jiffies = jiffies;
 			rtl92ee_phy_set_rf_on(hw);
 		}
@@ -3187,40 +3123,41 @@ static bool _rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		for (queue_id = 0, i = 0;
 		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
 			ring = &pcipriv->dev.tx_ring[queue_id];
-			if (skb_queue_len(&ring->queue) == 0) {
+			if (queue_id == BEACON_QUEUE ||
+			    skb_queue_len(&ring->queue) == 0) {
 				queue_id++;
 				continue;
 			} else {
-				RT_TRACE(COMP_ERR, DBG_WARNING,
-					 ("eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before "
-					  "doze!\n", (i + 1), queue_id,
-					  skb_queue_len(&ring->queue)));
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					 (i + 1), queue_id,
+					 skb_queue_len(&ring->queue));
 
 				udelay(10);
 				i++;
 			}
 			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
-				RT_TRACE(COMP_ERR, DBG_WARNING,
-					 ("\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
 					  MAX_DOZE_WAITING_TIMES_9x,
 					  queue_id,
-					  skb_queue_len(&ring->queue)));
+					  skb_queue_len(&ring->queue));
 				break;
 			}
 		}
 
 		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
-			RT_TRACE(COMP_RF, DBG_DMESG,
-				 ("IPS Set eRf nic disable\n"));
-			stg_rtl_ps_disable_nic(hw);
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "IPS Set eRf nic disable\n");
+			rtl_ps_disable_nic(hw);
 			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
 		} else {
 			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
 				rtlpriv->cfg->ops->led_control(hw,
-							       LED_CTL_NO_LINK);
+							LED_CTL_NO_LINK);
 			} else {
 				rtlpriv->cfg->ops->led_control(hw,
-							     LED_CTL_POWER_OFF);
+							LED_CTL_POWER_OFF);
 			}
 		}
 		break;
@@ -3234,32 +3171,32 @@ static bool _rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
 				queue_id++;
 				continue;
 			} else {
-				RT_TRACE(COMP_ERR, DBG_WARNING,
-					 ("eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
-					  (i + 1), queue_id,
-					  skb_queue_len(&ring->queue)));
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					 (i + 1), queue_id,
+					 skb_queue_len(&ring->queue));
 				udelay(10);
 				i++;
 			}
 			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
-				RT_TRACE(COMP_ERR, DBG_WARNING,
-					 ("\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
 					  MAX_DOZE_WAITING_TIMES_9x,
 					  queue_id,
-					  skb_queue_len(&ring->queue)));
+					  skb_queue_len(&ring->queue));
 				break;
 			}
 		}
-		RT_TRACE(COMP_RF, DBG_DMESG,
-			 ("Set ERFSLEEP awaked:%d ms\n",
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "Set ERFSLEEP awaked:%d ms\n",
 			  jiffies_to_msecs(jiffies -
-					   ppsc->last_awake_jiffies)));
+					   ppsc->last_awake_jiffies));
 		ppsc->last_sleep_jiffies = jiffies;
 		_rtl92ee_phy_set_rf_sleep(hw);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		bresult = false;
 		break;
 	}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/phy.h b/drivers/net/wireless/rtlwifi/rtl8192ee/phy.h
index 5be6c4866e14..c6e97c8df54c 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/phy.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,7 +26,9 @@
 #ifndef __RTL92E_PHY_H__
 #define __RTL92E_PHY_H__
 
-/*It must always set to 4, otherwise read efuse table secquence will be wrong.*/
+/* MAX_TX_COUNT must always set to 4, otherwise read efuse table sequence
+ * will be wrong.
+ */
 #define MAX_TX_COUNT				4
 #define TX_1S					0
 #define TX_2S					1
@@ -62,9 +60,9 @@
 
 #define LOOP_LIMIT				5
 #define MAX_STALL_TIME				50
-#define AntennaDiversityValue			0x80
+#define ANTENNADIVERSITYVALUE			0x80
 #define MAX_TXPWR_IDX_NMODE_92S			63
-#define Reset_Cnt_Limit				3
+#define RESET_CNT_LIMIT				3
 
 #define RF6052_MAX_PATH				2
 
@@ -116,39 +114,40 @@ enum ant_div_type {
 	FIXED_HW_ANTDIV = 0x03,
 	CG_TRX_SMART_ANTDIV = 0x04,
 	CGCS_RX_SW_ANTDIV = 0x05,
-
 };
-extern u32 rtl92ee_phy_query_bb_reg(struct ieee80211_hw *hw,
-				    u32 regaddr, u32 bitmask);
-extern void rtl92ee_phy_set_bb_reg(struct ieee80211_hw *hw,
-				   u32 regaddr, u32 bitmask, u32 data);
-extern u32 rtl92ee_phy_query_rf_reg(struct ieee80211_hw *hw,
-				    enum radio_path rfpath, u32 regaddr,
-				    u32 bitmask);
-extern void rtl92ee_phy_set_rf_reg(struct ieee80211_hw *hw,
-				   enum radio_path rfpath, u32 regaddr,
-				   u32 bitmask, u32 data);
-extern bool rtl92ee_phy_mac_config(struct ieee80211_hw *hw);
-extern bool rtl92ee_phy_bb_config(struct ieee80211_hw *hw);
-extern bool rtl92ee_phy_rf_config(struct ieee80211_hw *hw);
-extern void rtl92ee_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
-extern void rtl92ee_phy_get_txpower_level(struct ieee80211_hw *hw,
-					  long *powerlevel);
-extern void rtl92ee_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
-extern void rtl92ee_phy_scan_operation_backup(struct ieee80211_hw *hw,
-					      u8 operation);
-extern void rtl92ee_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
-extern void rtl92ee_phy_set_bw_mode(struct ieee80211_hw *hw,
-				    enum nl80211_channel_type ch_type);
-extern void rtl92ee_phy_sw_chnl_callback(struct ieee80211_hw *hw);
-extern u8 rtl92ee_phy_sw_chnl(struct ieee80211_hw *hw);
-extern void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+
+u32 rtl92ee_phy_query_bb_reg(struct ieee80211_hw *hw,
+			     u32 regaddr, u32 bitmask);
+void rtl92ee_phy_set_bb_reg(struct ieee80211_hw *hw,
+			    u32 regaddr, u32 bitmask, u32 data);
+u32 rtl92ee_phy_query_rf_reg(struct ieee80211_hw *hw,
+			     enum radio_path rfpath, u32 regaddr,
+			     u32 bitmask);
+void rtl92ee_phy_set_rf_reg(struct ieee80211_hw *hw,
+			    enum radio_path rfpath, u32 regaddr,
+			    u32 bitmask, u32 data);
+bool rtl92ee_phy_mac_config(struct ieee80211_hw *hw);
+bool rtl92ee_phy_bb_config(struct ieee80211_hw *hw);
+bool rtl92ee_phy_rf_config(struct ieee80211_hw *hw);
+void rtl92ee_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+void rtl92ee_phy_get_txpower_level(struct ieee80211_hw *hw,
+				   long *powerlevel);
+void rtl92ee_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
+void rtl92ee_phy_scan_operation_backup(struct ieee80211_hw *hw,
+				       u8 operation);
+void rtl92ee_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+void rtl92ee_phy_set_bw_mode(struct ieee80211_hw *hw,
+			     enum nl80211_channel_type ch_type);
+void rtl92ee_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+u8 rtl92ee_phy_sw_chnl(struct ieee80211_hw *hw);
+void rtl92ee_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
 void rtl92ee_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
 void rtl92ee_phy_lc_calibrate(struct ieee80211_hw *hw);
 void rtl92ee_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
 bool rtl92ee_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 					   enum radio_path rfpath);
 bool rtl92ee_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
-extern bool rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
-					   enum rf_pwrstate rfpwr_state);
+bool rtl92ee_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				    enum rf_pwrstate rfpwr_state);
+
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8192ee/pwrseq.c
index 08a2df8dfd21..1a701d007f0c 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/pwrseq.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -23,32 +23,31 @@
  *
  *****************************************************************************/
 
-#include "pwrseqcmd.h"
 #include "pwrseq.h"
 
+/* drivers should parse below arrays and do the corresponding actions */
 
-/*
-    drivers should parse below arrays and do the corresponding actions
-*/
 /*3 Power on  Array*/
-struct wlan_pwr_cfg rtl8192E_power_on_flow[RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS +
-					   RTL8192E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8192E_power_on_flow
+		[RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	RTL8192E_TRANS_CARDEMU_TO_ACT
 	RTL8192E_TRANS_END
 };
 
 /*3Radio off GPIO Array */
-struct wlan_pwr_cfg rtl8192E_radio_off_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS
-					    + RTL8192E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8192E_radio_off_flow
+		[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS
+		+ RTL8192E_TRANS_END_STEPS] = {
 	RTL8192E_TRANS_ACT_TO_CARDEMU
 	RTL8192E_TRANS_END
 };
 
 /*3Card Disable Array*/
 struct wlan_pwr_cfg rtl8192E_card_disable_flow
-					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
-					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
-					 RTL8192E_TRANS_END_STEPS] = {
+		[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	RTL8192E_TRANS_ACT_TO_CARDEMU
 	RTL8192E_TRANS_CARDEMU_TO_CARDDIS
 	RTL8192E_TRANS_END
@@ -56,52 +55,57 @@ struct wlan_pwr_cfg rtl8192E_card_disable_flow
 
 /*3 Card Enable Array*/
 struct wlan_pwr_cfg rtl8192E_card_enable_flow
-					[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
-					 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
-					 RTL8192E_TRANS_END_STEPS] = {
+		[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	RTL8192E_TRANS_CARDDIS_TO_CARDEMU
 	RTL8192E_TRANS_CARDEMU_TO_ACT
 	RTL8192E_TRANS_END
 };
 
 /*3Suspend Array*/
-struct wlan_pwr_cfg rtl8192E_suspend_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
-					  RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
-					  RTL8192E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8192E_suspend_flow
+		[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	RTL8192E_TRANS_ACT_TO_CARDEMU
 	RTL8192E_TRANS_CARDEMU_TO_SUS
 	RTL8192E_TRANS_END
 };
 
 /*3 Resume Array*/
-struct wlan_pwr_cfg rtl8192E_resume_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
-					 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
-					 RTL8192E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8192E_resume_flow
+		[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8192E_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	RTL8192E_TRANS_SUS_TO_CARDEMU
 	RTL8192E_TRANS_CARDEMU_TO_ACT
 	RTL8192E_TRANS_END
 };
 
 /*3HWPDN Array*/
-struct wlan_pwr_cfg rtl8192E_hwpdn_flow[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
-					RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
-					RTL8192E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8192E_hwpdn_flow
+		[RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8192E_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	RTL8192E_TRANS_ACT_TO_CARDEMU
 	RTL8192E_TRANS_CARDEMU_TO_PDN
 	RTL8192E_TRANS_END
 };
 
 /*3 Enter LPS */
-struct wlan_pwr_cfg rtl8192E_enter_lps_flow[RTL8192E_TRANS_ACT_TO_LPS_STEPS +
-					    RTL8192E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8192E_enter_lps_flow
+		[RTL8192E_TRANS_ACT_TO_LPS_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	/*FW behavior*/
 	RTL8192E_TRANS_ACT_TO_LPS
 	RTL8192E_TRANS_END
 };
 
 /*3 Leave LPS */
-struct wlan_pwr_cfg rtl8192E_leave_lps_flow[RTL8192E_TRANS_LPS_TO_ACT_STEPS +
-					    RTL8192E_TRANS_END_STEPS] = {
+struct wlan_pwr_cfg rtl8192E_leave_lps_flow
+		[RTL8192E_TRANS_LPS_TO_ACT_STEPS +
+		 RTL8192E_TRANS_END_STEPS] = {
 	/*FW behavior*/
 	RTL8192E_TRANS_LPS_TO_ACT
 	RTL8192E_TRANS_END
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8192ee/pwrseq.h
index 528897213224..781eeaa6af49 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/pwrseq.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,29 +26,29 @@
 #ifndef __RTL92E_PWRSEQ_H__
 #define __RTL92E_PWRSEQ_H__
 
-#include "pwrseqcmd.h"
-/*
-	Check document WM-20110607-Paul-RTL8192E_Power_Architecture-R02.vsd
-	There are 6 HW Power States:
-	0: POFF--Power Off
-	1: PDN--Power Down
-	2: CARDEMU--Card Emulation
-	3: ACT--Active Mode
-	4: LPS--Low Power State
-	5: SUS--Suspend
-
-	The transision from different states are defined below
-	TRANS_CARDEMU_TO_ACT
-	TRANS_ACT_TO_CARDEMU
-	TRANS_CARDEMU_TO_SUS
-	TRANS_SUS_TO_CARDEMU
-	TRANS_CARDEMU_TO_PDN
-	TRANS_ACT_TO_LPS
-	TRANS_LPS_TO_ACT
-
-	TRANS_END
-	PWR SEQ Version: rtl8192E_PwrSeq_V09.h
-*/
+#include "../pwrseqcmd.h"
+/**
+ *	Check document WM-20110607-Paul-RTL8192E_Power_Architecture-R02.vsd
+ *	There are 6 HW Power States:
+ *	0: POFF--Power Off
+ *	1: PDN--Power Down
+ *	2: CARDEMU--Card Emulation
+ *	3: ACT--Active Mode
+ *	4: LPS--Low Power State
+ *	5: SUS--Suspend
+ *
+ *	The transision from different states are defined below
+ *	TRANS_CARDEMU_TO_ACT
+ *	TRANS_ACT_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_SUS
+ *	TRANS_SUS_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_PDN
+ *	TRANS_ACT_TO_LPS
+ *	TRANS_LPS_TO_ACT
+ *
+ *	TRANS_END
+ *	PWR SEQ Version: rtl8192E_PwrSeq_V09.h
+ */
 
 #define	RTL8192E_TRANS_CARDEMU_TO_ACT_STEPS	18
 #define	RTL8192E_TRANS_ACT_TO_CARDEMU_STEPS	18
@@ -64,7 +60,6 @@
 #define	RTL8192E_TRANS_LPS_TO_ACT_STEPS		23
 #define	RTL8192E_TRANS_END_STEPS		1
 
-
 #define RTL8192E_TRANS_CARDEMU_TO_ACT					\
 	/* format */							\
 	/* comments here */						\
@@ -91,7 +86,6 @@
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(0), 0},
 
-
 #define RTL8192E_TRANS_ACT_TO_CARDEMU					\
 	/* format */							\
 	/* comments here */						\
@@ -109,7 +103,6 @@
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(1), 0},
 
-
 #define RTL8192E_TRANS_CARDEMU_TO_SUS					\
 	/* format */							\
 	/* comments here */						\
@@ -131,7 +124,6 @@
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO , PWR_CMD_POLLING, BIT(1), 0},
 
-
 #define RTL8192E_TRANS_SUS_TO_CARDEMU					\
 	/* format */							\
 	/* comments here */						\
@@ -146,7 +138,6 @@
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(3) | BIT(4), 0},
 
-
 #define RTL8192E_TRANS_CARDEMU_TO_CARDDIS				\
 	/* format */							\
 	/* comments here */						\
@@ -162,7 +153,7 @@
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(0), 0},			\
 	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
-	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,		\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
 	 PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},				\
 	/*0x04[10] = 1, enable SW LPS*/					\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
@@ -174,7 +165,6 @@
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO , PWR_CMD_POLLING, BIT(1), 0},
 
-
 #define RTL8192E_TRANS_CARDDIS_TO_CARDEMU				\
 	/* format */							\
 	/* comments here */						\
@@ -195,7 +185,6 @@
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(3) | BIT(4), 0},
 
-
 #define RTL8192E_TRANS_CARDEMU_TO_PDN					\
 	/* format */							\
 	/* comments here */						\
@@ -207,7 +196,6 @@
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(7), BIT(7)},
 
-
 #define RTL8192E_TRANS_PDN_TO_CARDEMU					\
 	/* format */							\
 	/* comments here */						\
@@ -216,7 +204,6 @@
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(7), 0},
 
-
 #define RTL8192E_TRANS_ACT_TO_LPS					\
 	/* format */							\
 	/* comments here */						\
@@ -261,14 +248,13 @@
 	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(5), BIT(5)},
 
-
 #define RTL8192E_TRANS_LPS_TO_ACT					\
 	/* format */							\
 	/* comments here */						\
 	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
 	/*SDIO RPWM, For Repeatly In and out, Taggle bit should be changed*/\
 	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	 PWR_BASEADDR_SDIO , PWR_CMD_WRITE, 0xFF, 0x84},			\
+	 PWR_BASEADDR_SDIO , PWR_CMD_WRITE, 0xFF, 0x84},		\
 	/*USB RPWM*/							\
 	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0x84},			\
@@ -283,7 +269,7 @@
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(4), 0},			\
 	/*Polling 0x109[7]=0  TSF in 40M*/				\
 	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(7), 0},			\
+	 PWR_BASEADDR_MAC , PWR_CMD_POLLING, BIT(7), 0},		\
 	/*0x101[1] = 1*/						\
 	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, BIT(1), BIT(1)},		\
@@ -300,7 +286,6 @@
 	{0x013D, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC , PWR_CMD_WRITE, 0xFF, 0xFF},
 
-
 #define RTL8192E_TRANS_END						\
 	/* format */							\
 	/* comments here */						\
@@ -341,15 +326,15 @@ extern struct wlan_pwr_cfg rtl8192E_leave_lps_flow
 					[RTL8192E_TRANS_LPS_TO_ACT_STEPS +
 					 RTL8192E_TRANS_END_STEPS];
 
-
 /* RTL8192EE Power Configuration CMDs for PCIe interface */
-#define Rtl8192E_NIC_PWR_ON_FLOW	rtl8192E_power_on_flow
-#define Rtl8192E_NIC_RF_OFF_FLOW	rtl8192E_radio_off_flow
-#define Rtl8192E_NIC_DISABLE_FLOW	rtl8192E_card_disable_flow
-#define Rtl8192E_NIC_ENABLE_FLOW	rtl8192E_card_enable_flow
-#define Rtl8192E_NIC_SUSPEND_FLOW	rtl8192E_suspend_flow
-#define Rtl8192E_NIC_RESUME_FLOW	rtl8192E_resume_flow
-#define Rtl8192E_NIC_PDN_FLOW		rtl8192E_hwpdn_flow
-#define Rtl8192E_NIC_LPS_ENTER_FLOW	rtl8192E_enter_lps_flow
-#define Rtl8192E_NIC_LPS_LEAVE_FLOW	rtl8192E_leave_lps_flow
+#define RTL8192E_NIC_PWR_ON_FLOW	rtl8192E_power_on_flow
+#define RTL8192E_NIC_RF_OFF_FLOW	rtl8192E_radio_off_flow
+#define RTL8192E_NIC_DISABLE_FLOW	rtl8192E_card_disable_flow
+#define RTL8192E_NIC_ENABLE_FLOW	rtl8192E_card_enable_flow
+#define RTL8192E_NIC_SUSPEND_FLOW	rtl8192E_suspend_flow
+#define RTL8192E_NIC_RESUME_FLOW	rtl8192E_resume_flow
+#define RTL8192E_NIC_PDN_FLOW		rtl8192E_hwpdn_flow
+#define RTL8192E_NIC_LPS_ENTER_FLOW	rtl8192E_enter_lps_flow
+#define RTL8192E_NIC_LPS_LEAVE_FLOW	rtl8192E_leave_lps_flow
+
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/reg.h b/drivers/net/wireless/rtlwifi/rtl8192ee/reg.h
index 08c07577239f..3f2a9596e7cd 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/reg.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -86,7 +82,6 @@
 #define REG_SYS_CFG1				0x00F0
 #define REG_SYS_CFG2				0x00FC
 
-
 #define REG_CR					0x0100
 #define REG_PBP					0x0104
 #define REG_PKT_BUFF_ACCESS_CTRL		0x0106
@@ -132,7 +127,8 @@
  *
  *	0x0200h ~ 0x027Fh	TXDMA Configuration
  *
- *-----------------------------------------------------*/
+ *-----------------------------------------------------
+ */
 #define REG_RQPN				0x0200
 #define REG_FIFOPAGE				0x0204
 #define REG_DWBCN0_CTRL				0x0208
@@ -146,7 +142,8 @@
  *
  *	0x0280h ~ 0x02FFh	RXDMA Configuration
  *
- *-----------------------------------------------------*/
+ *-----------------------------------------------------
+ */
 #define REG_RXDMA_AGG_PG_TH			0x0280
 #define REG_FW_UPD_RDPTR			0x0284
 #define REG_RXDMA_CONTROL			0x0286
@@ -161,7 +158,8 @@
  *
  *	0x0300h ~ 0x03FFh	PCIe
  *
- *-----------------------------------------------------*/
+ *-----------------------------------------------------
+ */
 #define	REG_PCIE_CTRL_REG			0x0300
 #define	REG_INT_MIG				0x0304
 #define	REG_BCNQ_DESA				0x0308
@@ -229,7 +227,8 @@
  *
  *	0x0400h ~ 0x047Fh	Protocol Configuration
  *
- *-----------------------------------------------------*/
+ *-----------------------------------------------------
+ */
 #define REG_VOQ_INFORMATION			0x0400
 #define REG_VIQ_INFORMATION			0x0404
 #define REG_BEQ_INFORMATION			0x0408
@@ -239,7 +238,6 @@
 #define REG_BCNQ_INFORMATION			0x0418
 #define REG_TXPKT_EMPTY				0x041A
 
-
 #define REG_FWHW_TXQ_CTRL			0x0420
 #define REG_HWSEQ_CTRL				0x0423
 #define REG_BCNQ_BDNY				0x0424
@@ -279,7 +277,8 @@
  *
  *	0x0500h ~ 0x05FFh	EDCA Configuration
  *
- *-----------------------------------------------------*/
+ *-----------------------------------------------------
+ */
 #define REG_EDCA_VO_PARAM			0x0500
 #define REG_EDCA_VI_PARAM			0x0504
 #define REG_EDCA_BE_PARAM			0x0508
@@ -322,7 +321,8 @@
  *
  *	0x0600h ~ 0x07FFh	WMAC Configuration
  *
- *-----------------------------------------------------*/
+ *-----------------------------------------------------
+ */
 #define REG_MAC_CR				0x0600
 #define REG_BWOPMODE				0x0603
 #define REG_TCR					0x0604
@@ -375,7 +375,6 @@
 #define REG_BFMEE_SEL				0x0714
 #define REG_SND_PTCL_CTRL			0x0718
 
-
 #define	CR9346					REG_9346CR
 #define	MSR					(REG_CR + 2)
 #define	ISR					REG_HISR
@@ -538,7 +537,6 @@
 #define	SCR_TXSEC_ENABLE			0x02
 #define	SCR_RXSEC_ENABLE			0x04
 
-
 /*********************************************
 *       8192EE IMR/ISR bits
 **********************************************/
@@ -591,13 +589,11 @@
 #define	IMR_TXFOVW				BIT(9)
 #define	IMR_RXFOVW				BIT(8)
 
-
 #define	HWSET_MAX_SIZE				512
 #define EFUSE_MAX_SECTION			64
 #define EFUSE_REAL_CONTENT_LEN			256
 #define EFUSE_OOB_PROTECT_BYTES			18
 
-
 #define	EEPROM_DEFAULT_TSSI			0x0
 #define EEPROM_DEFAULT_TXPOWERDIFF		0x0
 #define EEPROM_DEFAULT_CRYSTALCAP		0x5
@@ -653,14 +649,11 @@
 #define EEPROM_CLK				0x06
 #define EEPROM_TESTR				0x08
 
-
 #define EEPROM_TXPOWERCCK			0x10
 #define	EEPROM_TXPOWERHT40_1S			0x16
 #define EEPROM_TXPOWERHT20DIFF			0x1B
 #define EEPROM_TXPOWER_OFDMDIFF			0x1B
 
-
-
 #define	EEPROM_TX_PWR_INX			0x10
 
 #define	EEPROM_CHANNELPLAN			0xB8
@@ -750,7 +743,7 @@
 #define PWC_EV12V				BIT(15)
 
 #define FEN_BBRSTB				BIT(0)
-#define FEN_BB_GLB_RSTn				BIT(1)
+#define FEN_BB_GLB_RSTN				BIT(1)
 #define FEN_USBA				BIT(2)
 #define FEN_UPLL				BIT(3)
 #define FEN_USBD				BIT(4)
@@ -770,7 +763,7 @@
 #define PFM_ALDN				BIT(1)
 #define PFM_LDKP				BIT(2)
 #define PFM_WOWL				BIT(3)
-#define EnPDN					BIT(4)
+#define ENPDN					BIT(4)
 #define PDN_PL					BIT(5)
 #define APFM_ONMAC				BIT(8)
 #define APFM_OFF				BIT(9)
@@ -885,19 +878,19 @@
 #define LDOE25_EN				BIT(31)
 
 #define RSM_EN					BIT(0)
-#define Timer_EN				BIT(4)
+#define TIMER_EN				BIT(4)
 
 #define TRSW0EN					BIT(2)
 #define TRSW1EN					BIT(3)
 #define EROM_EN					BIT(4)
-#define EnBT					BIT(5)
-#define EnUart					BIT(8)
-#define Uart_910				BIT(9)
-#define EnPMAC					BIT(10)
+#define ENBT					BIT(5)
+#define ENUART					BIT(8)
+#define UART_910				BIT(9)
+#define ENPMAC					BIT(10)
 #define SIC_SWRST				BIT(11)
-#define EnSIC					BIT(12)
+#define ENSIC					BIT(12)
 #define SIC_23					BIT(13)
-#define EnHDP					BIT(14)
+#define ENHDP					BIT(14)
 #define SIC_LBK					BIT(15)
 
 #define LED0PL					BIT(4)
@@ -906,7 +899,7 @@
 
 #define MCUFWDL_EN				BIT(0)
 #define MCUFWDL_RDY				BIT(1)
-#define FWDL_ChkSum_rpt				BIT(2)
+#define FWDL_CHKSUM_RPT				BIT(2)
 #define MACINI_RDY				BIT(3)
 #define BBINI_RDY				BIT(4)
 #define RFINI_RDY				BIT(5)
@@ -1100,10 +1093,10 @@
 #define _SRL(x)					(((x) & 0x3F) << 8)
 
 #define _SIFS_CCK_CTX(x)			((x) & 0xFF)
-#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8);
+#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8)
 
 #define _SIFS_OFDM_CTX(x)			((x) & 0xFF)
-#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8);
+#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8)
 
 #define _TBTT_PROHIBIT_HOLD(x)			(((x) & 0xFF) << 8)
 
@@ -1121,13 +1114,13 @@
 #define	DIS_TSF_UDT0_NORMAL_CHIP		BIT(4)
 #define	DIS_TSF_UDT0_TEST_CHIP			BIT(5)
 
-#define	AcmHw_HwEn				BIT(0)
-#define	AcmHw_BeqEn				BIT(1)
-#define	AcmHw_ViqEn				BIT(2)
-#define	AcmHw_VoqEn				BIT(3)
-#define	AcmHw_BeqStatus				BIT(4)
-#define	AcmHw_ViqStatus				BIT(5)
-#define	AcmHw_VoqStatus				BIT(6)
+#define	ACMHW_HWEN				BIT(0)
+#define	ACMHW_BEQEN				BIT(1)
+#define	ACMHW_VIQEN				BIT(2)
+#define	ACMHW_VOQEN				BIT(3)
+#define	ACMHW_BEQSTATUS				BIT(4)
+#define	ACMHW_VIQSTATUS				BIT(5)
+#define	ACMHW_VOQSTATUS				BIT(6)
 
 #define APSDOFF					BIT(6)
 #define APSDOFF_STATUS				BIT(7)
@@ -1166,7 +1159,7 @@
 #define BM_DATA_EN				BIT(17)
 #define MFBEN					BIT(22)
 #define LSIGEN					BIT(23)
-#define EnMBID					BIT(24)
+#define ENMBID					BIT(24)
 #define APP_BASSN				BIT(27)
 #define APP_PHYSTS				BIT(28)
 #define APP_ICV					BIT(29)
@@ -1195,12 +1188,12 @@
 #define RXERR_RPT_RST				BIT(27)
 #define _RXERR_RPT_SEL(type)			((type) << 28)
 
-#define	SCR_TxUseDK				BIT(0)
-#define	SCR_RxUseDK				BIT(1)
-#define	SCR_TxEncEnable				BIT(2)
-#define	SCR_RxDecEnable				BIT(3)
-#define	SCR_SKByA2				BIT(4)
-#define	SCR_NoSKMC				BIT(5)
+#define	SCR_TXUSEDK				BIT(0)
+#define	SCR_RXUSEDK				BIT(1)
+#define	SCR_TXENCENABLE				BIT(2)
+#define	SCR_RXDECENABLE				BIT(3)
+#define	SCR_SKBYA2				BIT(4)
+#define	SCR_NOSKMC				BIT(5)
 #define SCR_TXBCUSEDK				BIT(6)
 #define SCR_RXBCUSEDK				BIT(7)
 
@@ -1253,7 +1246,7 @@
 #define	RPMAC_CCKPLCPHEADER			0x144
 #define	RPMAC_CCKCRC16				0x148
 #define	RPMAC_OFDMRXCRC32OK			0x170
-#define	RPMAC_OFDMRXCRC32Er			0x174
+#define	RPMAC_OFDMRXCRC32ER			0x174
 #define	RPMAC_OFDMRXPARITYER			0x178
 #define	RPMAC_OFDMRXCRC8ER			0x17c
 #define	RPMAC_CCKCRXRC16ER			0x180
@@ -1291,8 +1284,8 @@
 #define	RFPGA0_XAB_RFINTERFACESW		0x870
 #define	RFPGA0_XCD_RFINTERFACESW		0x874
 
-#define	rFPGA0_XAB_RFPARAMETER			0x878
-#define	rFPGA0_XCD_RFPARAMETER			0x87c
+#define	RFPGA0_XAB_RFPARAMETER			0x878
+#define	RFPGA0_XCD_RFPARAMETER			0x87c
 
 #define	RFPGA0_ANALOGPARAMETER1			0x880
 #define	RFPGA0_ANALOGPARAMETER2			0x884
@@ -1340,52 +1333,51 @@
 #define	RCCK0_FACOUNTERUPPER			0xa58
 #define	RCCK0_CCA_CNT				0xa60
 
-
 /* PageB(0xB00) */
-#define	rPdp_AntA				0xb00
-#define	rPdp_AntA_4				0xb04
-#define	rPdp_AntA_8				0xb08
-#define	rPdp_AntA_C				0xb0c
-#define	rPdp_AntA_10				0xb10
-#define	rPdp_AntA_14				0xb14
-#define	rPdp_AntA_18				0xb18
-#define	rPdp_AntA_1C				0xb1c
-#define	rPdp_AntA_20				0xb20
-#define	rPdp_AntA_24				0xb24
-
-#define	rConfig_Pmpd_AntA			0xb28
-#define	rConfig_ram64x16			0xb2c
-
-#define	rBndA					0xb30
-#define	rHssiPar				0xb34
-
-#define	rConfig_AntA				0xb68
-#define	rConfig_AntB				0xb6c
-
-#define	rPdp_AntB				0xb70
-#define	rPdp_AntB_4				0xb74
-#define	rPdp_AntB_8				0xb78
-#define	rPdp_AntB_C				0xb7c
-#define	rPdp_AntB_10				0xb80
-#define	rPdp_AntB_14				0xb84
-#define	rPdp_AntB_18				0xb88
-#define	rPdp_AntB_1C				0xb8c
-#define	rPdp_AntB_20				0xb90
-#define	rPdp_AntB_24				0xb94
-
-#define	rConfig_Pmpd_AntB			0xb98
-
-#define	rBndB					0xba0
-
-#define	rAPK					0xbd8
-#define	rPm_Rx0_AntA				0xbdc
-#define	rPm_Rx1_AntA				0xbe0
-#define	rPm_Rx2_AntA				0xbe4
-#define	rPm_Rx3_AntA				0xbe8
-#define	rPm_Rx0_AntB				0xbec
-#define	rPm_Rx1_AntB				0xbf0
-#define	rPm_Rx2_AntB				0xbf4
-#define	rPm_Rx3_AntB				0xbf8
+#define	RPDP_ANTA				0xb00
+#define	RPDP_ANTA_4				0xb04
+#define	RPDP_ANTA_8				0xb08
+#define	RPDP_ANTA_C				0xb0c
+#define	RPDP_ANTA_10				0xb10
+#define	RPDP_ANTA_14				0xb14
+#define	RPDP_ANTA_18				0xb18
+#define	RPDP_ANTA_1C				0xb1c
+#define	RPDP_ANTA_20				0xb20
+#define	RPDP_ANTA_24				0xb24
+
+#define	RCONFIG_PMPD_ANTA			0xb28
+#define	RCONFIG_RAM64x16			0xb2c
+
+#define	RBNDA					0xb30
+#define	RHSSIPAR				0xb34
+
+#define	RCONFIG_ANTA				0xb68
+#define	RCONFIG_ANTB				0xb6c
+
+#define	RPDP_ANTB				0xb70
+#define	RPDP_ANTB_4				0xb74
+#define	RPDP_ANTB_8				0xb78
+#define	RPDP_ANTB_C				0xb7c
+#define	RPDP_ANTB_10				0xb80
+#define	RPDP_ANTB_14				0xb84
+#define	RPDP_ANTB_18				0xb88
+#define	RPDP_ANTB_1C				0xb8c
+#define	RPDP_ANTB_20				0xb90
+#define	RPDP_ANTB_24				0xb94
+
+#define	RCONFIG_PMPD_ANTB			0xb98
+
+#define	RBNDB					0xba0
+
+#define	RAPK					0xbd8
+#define	RPM_RX0_ANTA				0xbdc
+#define	RPM_RX1_ANTA				0xbe0
+#define	RPM_RX2_ANTA				0xbe4
+#define	RPM_RX3_ANTA				0xbe8
+#define	RPM_RX0_ANTB				0xbec
+#define	RPM_RX1_ANTB				0xbf0
+#define	RPM_RX2_ANTB				0xbf4
+#define	RPM_RX3_ANTB				0xbf8
 
 /*Page C*/
 #define	ROFDM0_LSTF				0xc00
@@ -1449,7 +1441,6 @@
 #define	ROFDM0_FRAMESYNC			0xcf0
 #define	ROFDM0_DFSREPORT			0xcf4
 
-
 #define	ROFDM1_LSTF				0xd00
 #define	ROFDM1_TRXPATHENABLE			0xd04
 
@@ -1500,52 +1491,52 @@
 #define	RTXAGC_B_CCK11_A_CCK2_11		0x86c
 
 #define	RFPGA0_IQK				0xe28
-#define	RTx_IQK_Tone_A				0xe30
-#define	RRx_IQK_Tone_A				0xe34
-#define	RTx_IQK_PI_A				0xe38
-#define	RRx_IQK_PI_A				0xe3c
-
-#define	RTx_IQK					0xe40
-#define	RRx_IQK					0xe44
-#define	RIQK_AGC_Pts				0xe48
-#define	RIQK_AGC_Rsp				0xe4c
-#define	RTx_IQK_Tone_B				0xe50
-#define	RRx_IQK_Tone_B				0xe54
-#define	RTx_IQK_PI_B				0xe58
-#define	RRx_IQK_PI_B				0xe5c
-#define	RIQK_AGC_Cont				0xe60
-
-#define	RBlue_Tooth				0xe6c
-#define	RRx_Wait_CCA				0xe70
-#define	RTx_CCK_RFON				0xe74
-#define	RTx_CCK_BBON				0xe78
-#define	RTx_OFDM_RFON				0xe7c
-#define	RTx_OFDM_BBON				0xe80
-#define	RTx_To_Rx				0xe84
-#define	RTx_To_Tx				0xe88
-#define	RRx_CCK					0xe8c
-
-#define	RTx_Power_Before_IQK_A			0xe94
-#define	RTx_Power_After_IQK_A			0xe9c
-
-#define	RRx_Power_Before_IQK_A			0xea0
-#define	RRx_Power_Before_IQK_A_2		0xea4
-#define	RRx_Power_After_IQK_A			0xea8
-#define	RRx_Power_After_IQK_A_2			0xeac
-
-#define	RTx_Power_Before_IQK_B			0xeb4
-#define	RTx_Power_After_IQK_B			0xebc
-
-#define	RRx_Power_Before_IQK_B			0xec0
-#define	RRx_Power_Before_IQK_B_2		0xec4
-#define	RRx_Power_After_IQK_B			0xec8
-#define	RRx_Power_After_IQK_B_2			0xecc
-
-#define	RRx_OFDM				0xed0
-#define	RRx_Wait_RIFS				0xed4
-#define	RRx_TO_Rx				0xed8
-#define	RStandby				0xedc
-#define	RSleep					0xee0
+#define	RTX_IQK_TONE_A				0xe30
+#define	RRX_IQK_TONE_A				0xe34
+#define	RTX_IQK_PI_A				0xe38
+#define	RRX_IQK_PI_A				0xe3c
+
+#define	RTX_IQK					0xe40
+#define	RRX_IQK					0xe44
+#define	RIQK_AGC_PTS				0xe48
+#define	RIQK_AGC_RSP				0xe4c
+#define	RTX_IQK_TONE_B				0xe50
+#define	RRX_IQK_TONE_B				0xe54
+#define	RTX_IQK_PI_B				0xe58
+#define	RRX_IQK_PI_B				0xe5c
+#define	RIQK_AGC_CONT				0xe60
+
+#define	RBLUE_TOOTH				0xe6c
+#define	RRX_WAIT_CCA				0xe70
+#define	RTX_CCK_RFON				0xe74
+#define	RTX_CCK_BBON				0xe78
+#define	RTX_OFDM_RFON				0xe7c
+#define	RTX_OFDM_BBON				0xe80
+#define	RTX_TO_RX				0xe84
+#define	RTX_TO_TX				0xe88
+#define	RRX_CCK					0xe8c
+
+#define	RTX_POWER_BEFORE_IQK_A			0xe94
+#define	RTX_POWER_AFTER_IQK_A			0xe9c
+
+#define	RRX_POWER_BEFORE_IQK_A			0xea0
+#define	RRX_POWER_BEFORE_IQK_A_2		0xea4
+#define	RRX_POWER_AFTER_IQK_A			0xea8
+#define	RRX_POWER_AFTER_IQK_A_2			0xeac
+
+#define	RTX_POWER_BEFORE_IQK_B			0xeb4
+#define	RTX_POWER_AFTER_IQK_B			0xebc
+
+#define	RRX_POWER_BEFORE_IQK_B			0xec0
+#define	RRX_POWER_BEFORE_IQK_B_2		0xec4
+#define	RRX_POWER_AFTER_IQK_B			0xec8
+#define	RRX_POWER_AFTER_IQK_B_2			0xecc
+
+#define	RRX_OFDM				0xed0
+#define	RRX_WAIT_RIFS				0xed4
+#define	RRX_TO_RX				0xed8
+#define	RSTANDBY				0xedc
+#define	RSLEEP					0xee0
 #define	RPMPD_ANAEN				0xeec
 
 #define	RZEBRA1_HSSIENABLE			0x0
@@ -1678,8 +1669,8 @@
 #define	BCCKTXCRC16				0xffff
 #define	BCCKTXSTATUS				0x1
 #define	BOFDMTXSTATUS				0x2
-#define IS_BB_REG_OFFSET_92S(_Offset)	\
-	((_Offset >= 0x800) && (_Offset <= 0xfff))
+#define IS_BB_REG_OFFSET_92S(_offset)	\
+	((_offset >= 0x800) && (_offset <= 0xfff))
 
 #define	BRFMOD					0x1
 #define	BJAPANMODE				0x2
@@ -1872,7 +1863,7 @@
 #define	BCCK_RF_EXTEND				0x20000000
 #define	BCCK_RXAGC_SATLEVEL			0x1f000000
 #define	BCCK_RXAGC_SATCOUNT			0xe0
-#define	bCCKRxRFSettle				0x1f
+#define	BCCKRXRFSETTLE				0x1f
 #define	BCCK_FIXED_RXAGC			0x8000
 #define	BCCK_ANTENNA_POLARITY			0x2000
 #define	BCCK_TXFILTER_TYPE			0x0c00
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/rf.c b/drivers/net/wireless/rtlwifi/rtl8192ee/rf.c
index 4f5a49ebca10..c9bc33cd1090 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/rf.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -35,7 +35,7 @@ static bool _rtl92ee_phy_rf6052_config_parafile(struct ieee80211_hw *hw);
 void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	switch (bandwidth) {
 	case HT_CHANNEL_WIDTH_20:
@@ -55,8 +55,8 @@ void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 			      rtlphy->rfreg_chnlval[0]);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("unknown bandwidth: %#X\n", bandwidth));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", bandwidth);
 		break;
 	}
 }
@@ -64,19 +64,20 @@ void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 bool rtl92ee_phy_rf6052_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	if (rtlphy->rf_type == RF_1T1R)
 		rtlphy->num_total_rfpath = 1;
 	else
 		rtlphy->num_total_rfpath = 2;
+
 	return _rtl92ee_phy_rf6052_config_parafile(hw);
 }
 
 static bool _rtl92ee_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 u4_regvalue = 0;
 	u8 rfpath;
 	bool rtstatus = true;
@@ -138,13 +139,14 @@ static bool _rtl92ee_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 				      BRFSI_RFENV << 16, u4_regvalue);
 			break;
 		}
+
 		if (!rtstatus) {
-			RT_TRACE(COMP_INIT, DBG_TRACE,
-				 ("Radio[%d] Fail!!", rfpath));
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 "Radio[%d] Fail!!", rfpath);
 			return false;
 		}
 	}
 
-	RT_TRACE(COMP_INIT, DBG_TRACE, ("\n"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "\n");
 	return rtstatus;
 }
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/rf.h b/drivers/net/wireless/rtlwifi/rtl8192ee/rf.h
index 5bc394af4761..8bdeed3c064e 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/rf.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -33,7 +29,8 @@
 #define RF6052_MAX_TX_PWR		0x3F
 #define RF6052_MAX_REG			0x3F
 
-extern void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
-					     u8 bandwidth);
-extern bool rtl92ee_phy_rf6052_config(struct ieee80211_hw *hw);
+void rtl92ee_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+				      u8 bandwidth);
+bool rtl92ee_phy_rf6052_config(struct ieee80211_hw *hw);
+
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/sw.c b/drivers/net/wireless/rtlwifi/rtl8192ee/sw.c
index f9c5729e2cd9..9b5a7d5be121 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/sw.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -23,9 +23,6 @@
  *
  *****************************************************************************/
 
-#include <linux/vmalloc.h>
-#include <linux/module.h>
-
 #include "../wifi.h"
 #include "../core.h"
 #include "../pci.h"
@@ -42,6 +39,8 @@
 
 #include "../btcoexist/rtl_btc.h"
 
+#include <linux/vmalloc.h>
+#include <linux/module.h>
 
 static void rtl92ee_init_aspm_vars(struct ieee80211_hw *hw)
 {
@@ -50,7 +49,7 @@ static void rtl92ee_init_aspm_vars(struct ieee80211_hw *hw)
 	/*close ASPM for AMD defaultly */
 	rtlpci->const_amdpci_aspm = 0;
 
-	/*
+	/**
 	 * ASPM PS mode.
 	 * 0 - Disable ASPM,
 	 * 1 - Enable ASPM without Clock Req,
@@ -58,7 +57,7 @@ static void rtl92ee_init_aspm_vars(struct ieee80211_hw *hw)
 	 * 3 - Alwyas Enable ASPM with Clock Req,
 	 * 4 - Always Enable ASPM without Clock Req.
 	 * set defult to RTL8192CE:3 RTL8192E:2
-	 * */
+	 */
 	rtlpci->const_pci_aspm = 3;
 
 	/*Setting for PCI-E device */
@@ -67,7 +66,7 @@ static void rtl92ee_init_aspm_vars(struct ieee80211_hw *hw)
 	/*Setting for PCI-E bridge */
 	rtlpci->const_hostpci_aspm_setting = 0x02;
 
-	/*
+	/**
 	 * In Hw/Sw Radio Off situation.
 	 * 0 - Default,
 	 * 1 - From ASPM setting without low Mac Pwr,
@@ -77,7 +76,7 @@ static void rtl92ee_init_aspm_vars(struct ieee80211_hw *hw)
 	 */
 	rtlpci->const_hwsw_rfoff_d3 = 0;
 
-	/*
+	/**
 	 * This setting works for those device with
 	 * backdoor ASPM setting such as EPHY setting.
 	 * 0 - Not support ASPM,
@@ -89,21 +88,17 @@ static void rtl92ee_init_aspm_vars(struct ieee80211_hw *hw)
 
 int rtl92ee_init_sw_vars(struct ieee80211_hw *hw)
 {
-	int err = 0;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	const struct firmware *firmware;
-	char *fw_name = NULL;
+	int err = 0;
 
 	rtl92ee_bt_reg_init(hw);
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+	rtlpriv->btcoexist.btc_ops = rtl_btc_get_ops_pointer();
 
-	rtlpci->msi_support = true;
-	rtlpriv->btcoexist.btc_ops = stg_rtl_btc_get_ops_pointer();
-
-	rtlpriv->dm.b_dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_initialgain_enable = 1;
 	rtlpriv->dm.dm_flag = 0;
-	rtlpriv->dm.b_disable_framebursting = 0;
-	/*rtlpriv->dm.thermalvalue = 0;*/
+	rtlpriv->dm.disable_framebursting = 0;
 	rtlpci->transmit_config = CFENDFORM | BIT(15);
 
 	/*just 2.4G band*/
@@ -126,10 +121,7 @@ int rtl92ee_init_sw_vars(struct ieee80211_hw *hw)
 				  RCR_APM			|
 				  0);
 
-	rtlpci->irq_mask[0] = (u32) (IMR_PSTIMEOUT		|
-				/*   IMR_TBDER			|
-				     IMR_TBDOK			|
-				     IMR_BCNDMAINT0		|*/
+	rtlpci->irq_mask[0] = (u32)(IMR_PSTIMEOUT		|
 				     IMR_C2HCMD			|
 				     IMR_HIGHDOK		|
 				     IMR_MGNTDOK		|
@@ -140,61 +132,60 @@ int rtl92ee_init_sw_vars(struct ieee80211_hw *hw)
 				     IMR_RDU			|
 				     IMR_ROK			|
 				     0);
-	rtlpci->irq_mask[1] = (u32) (IMR_RXFOVW | 0);
+	rtlpci->irq_mask[1] = (u32)(IMR_RXFOVW | 0);
 
 	/* for debug level */
 	rtlpriv->dbg.global_debuglevel = rtlpriv->cfg->mod_params->debug;
 	/* for LPS & IPS */
-	rtlpriv->psc.b_inactiveps = rtlpriv->cfg->mod_params->b_inactiveps;
-	rtlpriv->psc.b_swctrl_lps = rtlpriv->cfg->mod_params->b_swctrl_lps;
-	rtlpriv->psc.b_fwctrl_lps = rtlpriv->cfg->mod_params->b_fwctrl_lps;
-	rtlpriv->psc.b_reg_fwctrl_lps = 3;
+	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
+	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
+	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+	if (rtlpriv->cfg->mod_params->disable_watchdog)
+		pr_info("watchdog disabled\n");
+	rtlpriv->psc.reg_fwctrl_lps = 3;
 	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
 	/* for ASPM, you can close aspm through
-	 * set const_support_pciaspm = 0 */
+	 * set const_support_pciaspm = 0
+	 */
 	rtl92ee_init_aspm_vars(hw);
 
-	if (rtlpriv->psc.b_reg_fwctrl_lps == 1)
+	if (rtlpriv->psc.reg_fwctrl_lps == 1)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_MIN_MODE;
-	else if (rtlpriv->psc.b_reg_fwctrl_lps == 2)
+	else if (rtlpriv->psc.reg_fwctrl_lps == 2)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_MAX_MODE;
-	else if (rtlpriv->psc.b_reg_fwctrl_lps == 3)
+	else if (rtlpriv->psc.reg_fwctrl_lps == 3)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
 
 	/* for early mode */
-	rtlpriv->rtlhal.b_earlymode_enable = false;
+	rtlpriv->rtlhal.earlymode_enable = false;
 
 	/*low power */
-	rtlpriv->psc.b_low_power_enable = false;
-
+	rtlpriv->psc.low_power_enable = false;
 
 	/* for firmware buf */
-	rtlpriv->rtlhal.pfirmware = vmalloc(0x8000);
+	rtlpriv->rtlhal.pfirmware = vzalloc(0x8000);
 	if (!rtlpriv->rtlhal.pfirmware) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Can't alloc buffer for fw.\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Can't alloc buffer for fw\n");
 		return 1;
 	}
 
-	fw_name = "rtlwifi/rtl8192eefw.bin";
-	err = request_firmware(&firmware, fw_name, rtlpriv->io.dev);
+	/* request fw */
+	rtlpriv->cfg->fw_name = "rtlwifi/rtl8192eefw.bin";
 
+	rtlpriv->max_fw_size = 0x8000;
+	pr_info("Using firmware %s\n", rtlpriv->cfg->fw_name);
+	err = request_firmware_nowait(THIS_MODULE, 1, rtlpriv->cfg->fw_name,
+				      rtlpriv->io.dev, GFP_KERNEL, hw,
+				      rtl_fw_cb);
 	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Failed to request firmware!\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Failed to request firmware!\n");
 		return 1;
 	}
-	if (firmware->size > 0x8000) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Firmware is too big!\n"));
-		release_firmware(firmware);
-		return 1;
-	}
-	memcpy(rtlpriv->rtlhal.pfirmware, firmware->data, firmware->size);
-	rtlpriv->rtlhal.fwsize = firmware->size;
-	release_firmware(firmware);
 
-	return err;
+	return 0;
 }
 
 void rtl92ee_deinit_sw_vars(struct ieee80211_hw *hw)
@@ -213,7 +204,6 @@ bool rtl92ee_get_btc_status(void)
 	return true;
 }
 
-
 static struct rtl_hal_ops rtl8192ee_hal_ops = {
 	.init_sw_vars = rtl92ee_init_sw_vars,
 	.deinit_sw_vars = rtl92ee_deinit_sw_vars,
@@ -251,10 +241,10 @@ static struct rtl_hal_ops rtl8192ee_hal_ops = {
 	.set_desc = rtl92ee_set_desc,
 	.get_desc = rtl92ee_get_desc,
 	.is_tx_desc_closed = rtl92ee_is_tx_desc_closed,
+	.tx_polling = rtl92ee_tx_polling,
 	.enable_hw_sec = rtl92ee_enable_hw_security_config,
 	.set_key = rtl92ee_set_key,
 	.init_sw_leds = rtl92ee_init_sw_leds,
-	.allow_all_destaddr = rtl92ee_allow_all_destaddr,
 	.get_bbreg = rtl92ee_phy_query_bb_reg,
 	.set_bbreg = rtl92ee_phy_set_bb_reg,
 	.get_rfreg = rtl92ee_phy_query_rf_reg,
@@ -266,9 +256,10 @@ static struct rtl_hal_ops rtl8192ee_hal_ops = {
 
 static struct rtl_mod_params rtl92ee_mod_params = {
 	.sw_crypto = false,
-	.b_inactiveps = true,
-	.b_swctrl_lps = false,
-	.b_fwctrl_lps = true,
+	.inactiveps = false,
+	.swctrl_lps = false,
+	.fwctrl_lps = true,
+	.msi_support = true,
 	.debug = DBG_EMERG,
 };
 
@@ -323,7 +314,6 @@ static struct rtl_hal_cfg rtl92ee_hal_cfg = {
 	.maps[RTL_IMR_BCNDMAINT3] = IMR_BCNDMAINT3,
 	.maps[RTL_IMR_BCNDMAINT2] = IMR_BCNDMAINT2,
 	.maps[RTL_IMR_BCNDMAINT1] = IMR_BCNDMAINT1,
-/*	.maps[RTL_IMR_BCNDOK8] = IMR_BCNDOK8,     */   /*need check*/
 	.maps[RTL_IMR_BCNDOK7] = IMR_BCNDOK7,
 	.maps[RTL_IMR_BCNDOK6] = IMR_BCNDOK6,
 	.maps[RTL_IMR_BCNDOK5] = IMR_BCNDOK5,
@@ -331,12 +321,10 @@ static struct rtl_hal_cfg rtl92ee_hal_cfg = {
 	.maps[RTL_IMR_BCNDOK3] = IMR_BCNDOK3,
 	.maps[RTL_IMR_BCNDOK2] = IMR_BCNDOK2,
 	.maps[RTL_IMR_BCNDOK1] = IMR_BCNDOK1,
-/*	.maps[RTL_IMR_TIMEOUT2] = IMR_TIMEOUT2,*/
-/*	.maps[RTL_IMR_TIMEOUT1] = IMR_TIMEOUT1,*/
 
 	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
-	.maps[RTL_IMR_BcnInt] = IMR_BCNDMAINT0,
+	.maps[RTL_IMR_BCNINT] = IMR_BCNDMAINT0,
 	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
 	.maps[RTL_IMR_RDU] = IMR_RDU,
 	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
@@ -379,50 +367,33 @@ MODULE_DEVICE_TABLE(pci, rtl92ee_pci_ids);
 MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
 MODULE_AUTHOR("Larry Finger	<Larry.Finger@lwfinger.net>");
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Realtek 8192E 802.11n PCI wireless");
+MODULE_DESCRIPTION("Realtek 8192EE 802.11n PCI wireless");
 MODULE_FIRMWARE("rtlwifi/rtl8192eefw.bin");
 
 module_param_named(swenc, rtl92ee_mod_params.sw_crypto, bool, 0444);
 module_param_named(debug, rtl92ee_mod_params.debug, int, 0444);
-module_param_named(ips, rtl92ee_mod_params.b_inactiveps, bool, 0444);
-module_param_named(swlps, rtl92ee_mod_params.b_swctrl_lps, bool, 0444);
-module_param_named(fwlps, rtl92ee_mod_params.b_fwctrl_lps, bool, 0444);
-MODULE_PARM_DESC(swenc, "using hardware crypto (default 0 [hardware])\n");
-MODULE_PARM_DESC(ips, "using no link power save (default 1 is open)\n");
-MODULE_PARM_DESC(fwlps, "using linked fw control power save (default 1 is open)\n");
+module_param_named(ips, rtl92ee_mod_params.inactiveps, bool, 0444);
+module_param_named(swlps, rtl92ee_mod_params.swctrl_lps, bool, 0444);
+module_param_named(fwlps, rtl92ee_mod_params.fwctrl_lps, bool, 0444);
+module_param_named(msi, rtl92ee_mod_params.msi_support, bool, 0444);
+module_param_named(disable_watchdog, rtl92ee_mod_params.disable_watchdog,
+		   bool, 0444);
+MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
+MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
+MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
+MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
+MODULE_PARM_DESC(msi, "Set to 1 to use MSI interrupts mode (default 1)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+MODULE_PARM_DESC(disable_watchdog, "Set to 1 to disable the watchdog (default 0)\n");
 
-static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, stg_rtl_pci_suspend,
-			 stg_rtl_pci_resume);
+static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
 static struct pci_driver rtl92ee_driver = {
 	.name = KBUILD_MODNAME,
 	.id_table = rtl92ee_pci_ids,
-	.probe = stg_rtl_pci_probe,
-	.remove = stg_rtl_pci_disconnect,
-
+	.probe = rtl_pci_probe,
+	.remove = rtl_pci_disconnect,
 	.driver.pm = &rtlwifi_pm_ops,
 };
 
-static int __init rtl92ee_module_init(void)
-{
-	int ret;
-	ret = rtl_core_module_init();
-	if (ret)
-		return ret;
-
-	ret = pci_register_driver(&rtl92ee_driver);
-	if (ret)
-		RT_ASSERT(false, (": No device found\n"));
-
-	return ret;
-}
-
-static void __exit rtl92ee_module_exit(void)
-{
-	pci_unregister_driver(&rtl92ee_driver);
-	rtl_core_module_exit();
-}
-
-module_init(rtl92ee_module_init);
-module_exit(rtl92ee_module_exit);
+module_pci_driver(rtl92ee_driver);
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/sw.h b/drivers/net/wireless/rtlwifi/rtl8192ee/sw.h
index 0170257a20c9..21433d0332d0 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/sw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/sw.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -33,7 +29,5 @@
 int rtl92ee_init_sw_vars(struct ieee80211_hw *hw);
 void rtl92ee_deinit_sw_vars(struct ieee80211_hw *hw);
 bool rtl92ee_get_btc_status(void);
-int rtl_core_module_init(void);
-void rtl_core_module_exit(void);
 
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/table.c b/drivers/net/wireless/rtlwifi/rtl8192ee/table.c
index c7eb9be53cc4..abcdd0670fd8 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/table.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/table.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/table.h b/drivers/net/wireless/rtlwifi/rtl8192ee/table.h
index e0f419823b36..bff9df88815d 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/table.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/table.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -45,4 +41,5 @@ extern u32 RTL8192EE_RADIOB_ARRAY[];
 extern u32 RTL8192EE_MAC_ARRAY[];
 #define RTL8192EE_AGC_TAB_ARRAY_LEN	532
 extern u32 RTL8192EE_AGC_TAB_ARRAY[];
+
 #endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/trx.c b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.c
index 1190c8bdb0d3..2fcbef1d029f 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.c
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -73,7 +73,7 @@ static int _rtl92ee_rate_mapping(struct ieee80211_hw *hw,
 {
 	int rate_idx;
 
-	if (!false) {
+	if (!isht) {
 		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
 			switch (desc_rate) {
 			case DESC92C_RATE1M:
@@ -210,7 +210,7 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 				       struct rx_fwinfo *p_drvinfo,
 				       bool bpacket_match_bssid,
 				       bool bpacket_toself,
-				       bool b_packet_beacon)
+				       bool packet_beacon)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct phy_status_rpt *p_phystrpt = (struct phy_status_rpt *)p_drvinfo;
@@ -218,26 +218,27 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 	u8 rf_rx_num = 0, evm, pwdb_all;
 	u8 i, max_spatial_stream;
 	u32 rssi, total_rssi = 0;
-	bool b_is_cck = pstatus->b_is_cck;
-	u8 lan_idx , vga_idx;
+	bool is_cck = pstatus->is_cck;
+	u8 lan_idx, vga_idx;
 
 	/* Record it for next packet processing */
-	pstatus->b_packet_matchbssid = bpacket_match_bssid;
-	pstatus->b_packet_toself = bpacket_toself;
-	pstatus->b_packet_beacon = b_packet_beacon;
+	pstatus->packet_matchbssid = bpacket_match_bssid;
+	pstatus->packet_toself = bpacket_toself;
+	pstatus->packet_beacon = packet_beacon;
 	pstatus->rx_mimo_signalquality[0] = -1;
 	pstatus->rx_mimo_signalquality[1] = -1;
 
-	if (b_is_cck) {
+	if (is_cck) {
 		u8 cck_highpwr;
 		u8 cck_agc_rpt;
 		/* CCK Driver info Structure is not the same as OFDM packet. */
 		cck_agc_rpt = p_phystrpt->cck_agc_rpt_ofdm_cfosho_a;
 
-		/* (1)Hardware does not provide RSSI for CCK */
-		/* (2)PWDB, Average PWDB cacluated by
-		 * hardware (for rate adaptive) */
-		cck_highpwr = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
+		/* (1)Hardware does not provide RSSI for CCK
+		 * (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
+		cck_highpwr = (u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
 						 BIT(9));
 
 		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
@@ -277,7 +278,7 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 				break;
 		}
 		rx_pwr_all += 16;
-		pwdb_all = stg_rtl_query_rxpwrpercentage(rx_pwr_all);
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
 
 		if (!cck_highpwr) {
 			if (pwdb_all >= 80)
@@ -317,7 +318,7 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 		/* (1)Get RSSI for HT rate */
 		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
 			/* we will judge RF RX path now. */
-			if (rtlpriv->dm.brfpath_rxenable[i])
+			if (rtlpriv->dm.rfpath_rxenable[i])
 				rf_rx_num++;
 
 			rx_pwr[i] = ((p_phystrpt->path_agc[i].gain & 0x3f) * 2)
@@ -325,18 +326,19 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 
 			pstatus->rx_pwr[i] = rx_pwr[i];
 			/* Translate DBM to percentage. */
-			rssi = stg_rtl_query_rxpwrpercentage(rx_pwr[i]);
+			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
 			total_rssi += rssi;
 
 			pstatus->rx_mimo_signalstrength[i] = (u8)rssi;
 		}
 
 		/* (2)PWDB, Average PWDB cacluated by
-		 * hardware (for rate adaptive) */
+		 * hardware (for rate adaptive)
+		 */
 		rx_pwr_all = ((p_phystrpt->cck_sig_qual_ofdm_pwdb_all >> 1)
 			      & 0x7f) - 110;
 
-		pwdb_all = stg_rtl_query_rxpwrpercentage(rx_pwr_all);
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
 		pstatus->rx_pwdb_all = pwdb_all;
 		pstatus->bt_rx_rssi_percentage = pwdb_all;
 		pstatus->rxpower = rx_pwr_all;
@@ -350,17 +352,18 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 			max_spatial_stream = 1;
 
 		for (i = 0; i < max_spatial_stream; i++) {
-			evm = stg_rtl_evm_db_to_percentage(
+			evm = rtl_evm_db_to_percentage(
 						p_phystrpt->stream_rxevm[i]);
 
 			if (bpacket_match_bssid) {
 				/* Fill value in RFD, Get the first
-				 * spatial stream only */
+				 * spatial stream only
+				 */
 				if (i == 0)
 					pstatus->signalquality = (u8)(evm &
-								      0xff);
+								       0xff);
 				pstatus->rx_mimo_signalquality[i] = (u8)(evm &
-									 0xff);
+									  0xff);
 			}
 		}
 
@@ -377,12 +380,13 @@ static void _rtl92ee_query_rxphystatus(struct ieee80211_hw *hw,
 	}
 
 	/* UI BSS List signal strength(in percentage),
-	 * make it good looking, from 0~100. */
-	if (b_is_cck)
-		pstatus->signalstrength = (u8)(stg_rtl_signal_scale_mapping(hw,
+	 * make it good looking, from 0~100.
+	 */
+	if (is_cck)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
 								     pwdb_all));
 	else if (rf_rx_num != 0)
-		pstatus->signalstrength = (u8)(stg_rtl_signal_scale_mapping(hw,
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
 						      total_rssi /= rf_rx_num));
 }
 
@@ -399,58 +403,58 @@ static void _rtl92ee_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	u8 *praddr;
 	u8 *psaddr;
 	__le16 fc;
-	u16 cpu_fc, type;
-	bool b_packet_matchbssid, b_packet_toself, b_packet_beacon;
+	bool packet_matchbssid, packet_toself, packet_beacon;
 
 	tmp_buf = skb->data + pstatus->rx_drvinfo_size +
 		  pstatus->rx_bufshift + 24;
 
 	hdr = (struct ieee80211_hdr *)tmp_buf;
 	fc = hdr->frame_control;
-	cpu_fc = le16_to_cpu(fc);
-	type = WLAN_FC_GET_TYPE(fc);
 	praddr = hdr->addr1;
 	psaddr = ieee80211_get_SA(hdr);
 	ether_addr_copy(pstatus->psaddr, psaddr);
 
-	b_packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
+	packet_matchbssid = (!ieee80211_is_ctl(fc) &&
 			       (ether_addr_equal(mac->bssid,
-						(cpu_fc & IEEE80211_FCTL_TODS) ?
+						ieee80211_has_tods(fc) ?
 						hdr->addr1 :
-						(cpu_fc & IEEE80211_FCTL_FROMDS) ?
+						ieee80211_has_fromds(fc) ?
 						hdr->addr2 : hdr->addr3)) &&
-				(!pstatus->b_hwerror) && (!pstatus->b_crc) &&
-				(!pstatus->b_icv));
+				(!pstatus->hwerror) && (!pstatus->crc) &&
+				(!pstatus->icv));
 
-	b_packet_toself = b_packet_matchbssid &&
+	packet_toself = packet_matchbssid &&
 			 (ether_addr_equal(praddr, rtlefuse->dev_addr));
 
 	if (ieee80211_is_beacon(fc))
-		b_packet_beacon = true;
+		packet_beacon = true;
 	else
-		b_packet_beacon = false;
+		packet_beacon = false;
 
-	if (b_packet_beacon && b_packet_matchbssid)
+	if (packet_beacon && packet_matchbssid)
 		rtl_priv(hw)->dm.dbginfo.num_qry_beacon_pkt++;
 
-	if (b_packet_matchbssid && ieee80211_is_data_qos(fc) &&
+	if (packet_matchbssid && ieee80211_is_data_qos(hdr->frame_control) &&
 	    !is_multicast_ether_addr(ieee80211_get_DA(hdr))) {
 		struct ieee80211_qos_hdr *hdr_qos =
 					    (struct ieee80211_qos_hdr *)tmp_buf;
 		u16 tid = le16_to_cpu(hdr_qos->qos_ctrl) & 0xf;
+
 		if (tid != 0 && tid != 3)
 			rtl_priv(hw)->dm.dbginfo.num_non_be_pkt++;
 	}
+
 	_rtl92ee_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
-				   b_packet_matchbssid, b_packet_toself,
-				   b_packet_beacon);
-	stg_rtl_process_phyinfo(hw, tmp_buf, pstatus);
+				   packet_matchbssid, packet_toself,
+				   packet_beacon);
+	rtl_process_phyinfo(hw, tmp_buf, pstatus);
 }
 
 static void _rtl92ee_insert_emcontent(struct rtl_tcb_desc *ptcb_desc,
 				      u8 *virtualaddress)
 {
 	u32 dwtmp = 0;
+
 	memset(virtualaddress, 0, 8);
 
 	SET_EARLYMODE_PKTNUM(virtualaddress, ptcb_desc->empkt_num);
@@ -506,20 +510,20 @@ bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rx_fwinfo *p_drvinfo;
 	struct ieee80211_hdr *hdr;
-
 	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
-	status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
-	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+
+	status->length = (u16)GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8)GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
 				  RX_DRV_INFO_SIZE_UNIT;
 	status->rx_bufshift = (u8)(GET_RX_DESC_SHIFT(pdesc) & 0x03);
-	status->b_icv = (u16) GET_RX_DESC_ICV(pdesc);
-	status->b_crc = (u16) GET_RX_DESC_CRC32(pdesc);
-	status->b_hwerror = (status->b_crc | status->b_icv);
+	status->icv = (u16)GET_RX_DESC_ICV(pdesc);
+	status->crc = (u16)GET_RX_DESC_CRC32(pdesc);
+	status->hwerror = (status->crc | status->icv);
 	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
-	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
-	status->b_isampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->rate = (u8)GET_RX_DESC_RXMCS(pdesc);
+	status->isampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
 		status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
-	status->b_is_cck = RX_HAL_IS_CCK_RATE(status->rate);
+	status->is_cck = RTL92EE_RX_HAL_IS_CCK_RATE(status->rate);
 
 	status->macid = GET_RX_DESC_MACID(pdesc);
 	if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
@@ -531,41 +535,35 @@ bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
 	else
 		status->wake_match = 0;
 	if (status->wake_match)
-		RT_TRACE(COMP_RXDESC , DBG_LOUD,
-			 ("GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch =%d\n",
-			 status->wake_match));
+		RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD,
+			 "GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",
+			 status->wake_match);
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
 
 	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size +
 				       status->rx_bufshift + 24);
 
-	if (status->b_crc)
+	if (status->crc)
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
 
 	if (status->rx_is40Mhzpacket)
 		rx_status->flag |= RX_FLAG_40MHZ;
 
-	if (status->b_is_ht)
+	if (status->is_ht)
 		rx_status->flag |= RX_FLAG_HT;
 
-	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
+	rx_status->flag |= RX_FLAG_MACTIME_START;
 
 	/* hw will set status->decrypted true, if it finds the
-	 * frame is open data frame or mgmt frame. */
-	/* So hw will not decryption robust managment frame
+	 * frame is open data frame or mgmt frame.
+	 * So hw will not decryption robust managment frame
 	 * for IEEE80211w but still set status->decrypted
 	 * true, so here we should set it back to undecrypted
 	 * for IEEE80211w frame, and mac80211 sw will help
-	 * to decrypt it */
+	 * to decrypt it
+	 */
 	if (status->decrypted) {
-		if (!hdr) {
-			WARN_ON_ONCE(true);
-			pr_err("decrypted is true but hdr NULL, from skb %p\n",
-			       rtl_get_hdr(skb));
-			return false;
-		}
-
 		if ((!_ieee80211_is_robust_mgmt_frame(hdr)) &&
 		    (ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag |= RX_FLAG_DECRYPTED;
@@ -575,10 +573,11 @@ bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
 
 	/* rate_idx: index of data rate into band's
 	 * supported rates or MCS index if HT rates
-	 * are use (RX_FLAG_HT)*/
-	/* Notice: this is diff with windows define */
+	 * are use (RX_FLAG_HT)
+	 * Notice: this is diff with windows define
+	 */
 	rx_status->rate_idx = _rtl92ee_rate_mapping(hw,
-						    status->b_is_ht,
+						    status->is_ht,
 						    status->rate);
 
 	rx_status->mactime = status->timestamp_low;
@@ -589,10 +588,7 @@ bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
 		_rtl92ee_translate_rx_signal_stuff(hw, skb, status, pdesc,
 						   p_drvinfo);
 	}
-
-	/*rx_status->qual = status->signal; */
 	rx_status->signal = status->recvsignalpower + 10;
-	/*rx_status->noise = -status->noise; */
 	if (status->packet_report_type == TX_REPORT2) {
 		status->macid_valid_entry[0] =
 			GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
@@ -606,10 +602,11 @@ bool rtl92ee_rx_query_desc(struct ieee80211_hw *hw,
 void rtl92ee_rx_check_dma_ok(struct ieee80211_hw *hw, u8 *header_desc,
 			     u8 queue_index)
 {
-	u8 first_seg;
-	u8 last_seg;
-	u16 total_len;
+	u8 first_seg = 0;
+	u8 last_seg = 0;
+	u16 total_len = 0;
 	u16 read_cnt = 0;
+
 	if (header_desc == NULL)
 		return;
 
@@ -630,11 +627,11 @@ void rtl92ee_rx_check_dma_ok(struct ieee80211_hw *hw, u8 *header_desc,
 	}
 }
 
-u16 rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw , u8 queue_index)
+u16 rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw, u8 queue_index)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 read_point = 0 , write_point = 0 , remind_cnt = 0;
+	u16 read_point = 0, write_point = 0, remind_cnt = 0;
 	u32 tmp_4byte = 0;
 	static u16 last_read_point;
 	static bool start_rx;
@@ -644,9 +641,9 @@ u16 rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw , u8 queue_index)
 	write_point = (u16)(tmp_4byte & 0x7ff);
 
 	if (write_point != rtlpci->rx_ring[queue_index].next_rx_rp) {
-		RT_TRACE(COMP_RXDESC, DBG_DMESG,
-			 ("!!!write point is 0x%x, reg 0x3B4 value is 0x%x\n",
-			  write_point, tmp_4byte));
+		RT_TRACE(rtlpriv, COMP_RXDESC, DBG_DMESG,
+			 "!!!write point is 0x%x, reg 0x3B4 value is 0x%x\n",
+			  write_point, tmp_4byte);
 		tmp_4byte = rtl_read_dword(rtlpriv, REG_RXQ_TXBD_IDX);
 		read_point = (u16)((tmp_4byte>>16) & 0x7ff);
 		write_point = (u16)(tmp_4byte & 0x7ff);
@@ -675,53 +672,54 @@ u16 rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw , u8 queue_index)
 	return remind_cnt;
 }
 
-static u16 get_desc_addr_fr_q(u16 queue_index)
+static u16 get_desc_addr_fr_q_idx(u16 queue_index)
 {
 	u16 desc_address = REG_BEQ_TXBD_IDX;
 
 	switch (queue_index) {
 	case BK_QUEUE:
-			desc_address = REG_BKQ_TXBD_IDX;
-			break;
+		desc_address = REG_BKQ_TXBD_IDX;
+		break;
 	case BE_QUEUE:
-			desc_address = REG_BEQ_TXBD_IDX;
-			break;
+		desc_address = REG_BEQ_TXBD_IDX;
+		break;
 	case VI_QUEUE:
-			desc_address = REG_VIQ_TXBD_IDX;
-			break;
+		desc_address = REG_VIQ_TXBD_IDX;
+		break;
 	case VO_QUEUE:
-			desc_address = REG_VOQ_TXBD_IDX;
-			break;
+		desc_address = REG_VOQ_TXBD_IDX;
+		break;
 	case BEACON_QUEUE:
-			desc_address = REG_BEQ_TXBD_IDX;
-			break;
+		desc_address = REG_BEQ_TXBD_IDX;
+		break;
 	case TXCMD_QUEUE:
-			desc_address = REG_BEQ_TXBD_IDX;
-			break;
+		desc_address = REG_BEQ_TXBD_IDX;
+		break;
 	case MGNT_QUEUE:
-			desc_address = REG_MGQ_TXBD_IDX;
-			break;
+		desc_address = REG_MGQ_TXBD_IDX;
+		break;
 	case HIGH_QUEUE:
-			desc_address = REG_HI0Q_TXBD_IDX;
-			break;
+		desc_address = REG_HI0Q_TXBD_IDX;
+		break;
 	case HCCA_QUEUE:
-			desc_address = REG_BEQ_TXBD_IDX;
-			break;
+		desc_address = REG_BEQ_TXBD_IDX;
+		break;
 	default:
-			break;
+		break;
 	}
 	return desc_address;
 }
 
-void rtl92ee_get_available_desc(struct ieee80211_hw *hw , u8 q_idx)
+void rtl92ee_get_available_desc(struct ieee80211_hw *hw, u8 q_idx)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 point_diff = 0;
 	u16 current_tx_read_point = 0, current_tx_write_point = 0;
 	u32 tmp_4byte;
+
 	tmp_4byte = rtl_read_dword(rtlpriv,
-				   get_desc_addr_fr_q(q_idx));
+				   get_desc_addr_fr_q_idx(q_idx));
 	current_tx_read_point = (u16)((tmp_4byte >> 16) & 0x0fff);
 	current_tx_write_point = (u16)((tmp_4byte) & 0x0fff);
 
@@ -759,10 +757,9 @@ void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
 	tx_page_size = 2;
 	current_bd_desc = rtlpci->tx_ring[queue_index].cur_tx_wp;
 
-
 	total_packet_size = desc_size+pkt_len;
 
-	if (rtlpriv->rtlhal.b_earlymode_enable)	{
+	if (rtlpriv->rtlhal.earlymode_enable)	{
 		if (queue_index < BEACON_QUEUE) {
 			append_early_mode_size = 8;
 			total_packet_size += append_early_mode_size;
@@ -778,9 +775,9 @@ void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
 	}
 
 	/* Reset */
-	SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , 0);
-	SET_TX_BUFF_DESC_PSB(tx_bd_desc , 0);
-	SET_TX_BUFF_DESC_OWN(tx_bd_desc , 0);
+	SET_TX_BUFF_DESC_LEN_0(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_PSB(tx_bd_desc, 0);
+	SET_TX_BUFF_DESC_OWN(tx_bd_desc, 0);
 
 	for (i = 1; i < segmentnum; i++) {
 		SET_TXBUFFER_DESC_LEN_WITH_OFFSET(tx_bd_desc, i, 0);
@@ -800,17 +797,17 @@ void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
 	/* Clear all status */
 	CLEAR_PCI_TX_DESC_CONTENT(desc, TX_DESC_SIZE);
 
-	if (rtlpriv->rtlhal.b_earlymode_enable) {
+	if (rtlpriv->rtlhal.earlymode_enable) {
 		if (queue_index < BEACON_QUEUE) {
-			/* These macros need braces */
-			SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , desc_size + 8);
+			/* This if needs braces */
+			SET_TX_BUFF_DESC_LEN_0(tx_bd_desc, desc_size + 8);
 		} else {
-			SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , desc_size);
+			SET_TX_BUFF_DESC_LEN_0(tx_bd_desc, desc_size);
 		}
 	} else {
-		SET_TX_BUFF_DESC_LEN_0(tx_bd_desc , desc_size);
+		SET_TX_BUFF_DESC_LEN_0(tx_bd_desc, desc_size);
 	}
-	SET_TX_BUFF_DESC_PSB(tx_bd_desc , psblen);
+	SET_TX_BUFF_DESC_PSB(tx_bd_desc, psblen);
 	SET_TX_BUFF_DESC_ADDR_LOW_0(tx_bd_desc,
 				    rtlpci->tx_ring[queue_index].dma +
 				    (current_bd_desc * TX_DESC_SIZE));
@@ -841,9 +838,9 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 	__le16 fc = hdr->frame_control;
 	unsigned int buf_len = 0;
 	u8 fw_qsel = _rtl92ee_map_hwqueue_to_fwqueue(skb, hw_queue);
-	bool b_firstseg = ((hdr->seq_ctrl &
+	bool firstseg = ((hdr->seq_ctrl &
 			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
-	bool b_lastseg = ((hdr->frame_control &
+	bool lastseg = ((hdr->frame_control &
 			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
 	dma_addr_t mapping;
 	u8 bw_40 = 0;
@@ -858,9 +855,9 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 				IEEE80211_HT_CAP_SUP_WIDTH_20_40;
 	}
 	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
-	stg_rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
+	rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
 	/* reserve 8 byte for AMPDU early mode */
-	if (rtlhal->b_earlymode_enable) {
+	if (rtlhal->earlymode_enable) {
 		skb_push(skb, EM_HDR_LEN);
 		memset(skb->data, 0, EM_HDR_LEN);
 	}
@@ -868,27 +865,28 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 	mapping = pci_map_single(rtlpci->pdev, skb->data, skb->len,
 				 PCI_DMA_TODEVICE);
 	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
-		RT_TRACE(COMP_SEND, DBG_TRACE,
-			 ("DMA mapping error"));
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "DMA mapping error");
 		return;
 	}
+
 	if (pbd_desc_tx != NULL)
 		rtl92ee_pre_fill_tx_bd_desc(hw, pbd_desc_tx, pdesc, hw_queue,
 					    skb, mapping);
 
 	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
-		b_firstseg = true;
-		b_lastseg = true;
+		firstseg = true;
+		lastseg = true;
 	}
-	if (b_firstseg) {
-		if (rtlhal->b_earlymode_enable) {
+	if (firstseg) {
+		if (rtlhal->earlymode_enable) {
 			SET_TX_DESC_PKT_OFFSET(pdesc, 1);
 			SET_TX_DESC_OFFSET(pdesc,
 					   USB_HWDESC_HEADER_LEN + EM_HDR_LEN);
 			if (ptcb_desc->empkt_num) {
-				RT_TRACE(COMP_SEND, DBG_TRACE,
-					 ("Insert 8 byte.pTcb->EMPktNum:%d\n",
-					  ptcb_desc->empkt_num));
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Insert 8 byte.pTcb->EMPktNum:%d\n",
+					  ptcb_desc->empkt_num);
 				_rtl92ee_insert_emcontent(ptcb_desc,
 							  (u8 *)(skb->data));
 			}
@@ -920,20 +918,20 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		SET_TX_DESC_SEQ(pdesc, seq_number);
 		SET_TX_DESC_RTS_ENABLE(pdesc,
-				       ((ptcb_desc->b_rts_enable &&
-					 !ptcb_desc->b_cts_enable) ? 1 : 0));
-		SET_TX_DESC_HW_RTS_ENABLE(pdesc , 0);
+				       ((ptcb_desc->rts_enable &&
+					 !ptcb_desc->cts_enable) ? 1 : 0));
+		SET_TX_DESC_HW_RTS_ENABLE(pdesc, 0);
 		SET_TX_DESC_CTS2SELF(pdesc,
-				     ((ptcb_desc->b_cts_enable) ? 1 : 0));
+				     ((ptcb_desc->cts_enable) ? 1 : 0));
 
 		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
 		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
 		SET_TX_DESC_RTS_SHORT(pdesc,
 				((ptcb_desc->rts_rate <= DESC92C_RATE54M) ?
-				 (ptcb_desc->b_rts_use_shortpreamble ? 1 : 0) :
-				 (ptcb_desc->b_rts_use_shortgi ? 1 : 0)));
+				 (ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
+				 (ptcb_desc->rts_use_shortgi ? 1 : 0)));
 
-		if (ptcb_desc->btx_enable_sw_calc_duration)
+		if (ptcb_desc->tx_enable_sw_calc_duration)
 			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
 
 		if (bw_40) {
@@ -953,10 +951,12 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_LINIP(pdesc, 0);
 		if (sta) {
 			u8 ampdu_density = sta->ht_cap.ampdu_density;
+
 			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
 		}
 		if (info->control.hw_key) {
 			struct ieee80211_key_conf *key = info->control.hw_key;
+
 			switch (key->cipher) {
 			case WLAN_CIPHER_SUITE_WEP40:
 			case WLAN_CIPHER_SUITE_WEP104:
@@ -989,18 +989,18 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		if (ieee80211_is_data_qos(fc)) {
 			if (mac->rdg_en) {
-				RT_TRACE(COMP_SEND, DBG_TRACE,
-					 ("Enable RDG function.\n"));
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Enable RDG function.\n");
 				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
 				SET_TX_DESC_HTC(pdesc, 1);
 			}
 		}
 	}
 
-	SET_TX_DESC_FIRST_SEG(pdesc, (b_firstseg ? 1 : 0));
-	SET_TX_DESC_LAST_SEG(pdesc, (b_lastseg ? 1 : 0));
+	SET_TX_DESC_FIRST_SEG(pdesc, (firstseg ? 1 : 0));
+	SET_TX_DESC_LAST_SEG(pdesc, (lastseg ? 1 : 0));
 	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
-	if (rtlpriv->dm.b_useramask) {
+	if (rtlpriv->dm.useramask) {
 		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
 		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
 	} else {
@@ -1008,37 +1008,34 @@ void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_MACID(pdesc, ptcb_desc->ratr_index);
 	}
 
-	SET_TX_DESC_MORE_FRAG(pdesc, (b_lastseg ? 0 : 1));
+	SET_TX_DESC_MORE_FRAG(pdesc, (lastseg ? 0 : 1));
 	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
 	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
 		SET_TX_DESC_BMC(pdesc, 1);
 	}
-	RT_TRACE(COMP_SEND, DBG_TRACE, ("\n"));
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "\n");
 }
 
 void rtl92ee_tx_fill_cmddesc(struct ieee80211_hw *hw,
-			     u8 *pdesc, bool b_firstseg,
-			     bool b_lastseg, struct sk_buff *skb)
+			     u8 *pdesc, bool firstseg,
+			     bool lastseg, struct sk_buff *skb)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	u8 fw_queue = QSLT_BEACON;
-
 	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
 					    skb->data, skb->len,
 					    PCI_DMA_TODEVICE);
-
-
 	u8 txdesc_len = 40;
 
 	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
-		RT_TRACE(COMP_SEND, DBG_TRACE,
-			 ("DMA mapping error"));
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "DMA mapping error");
 		return;
 	}
 	CLEAR_PCI_TX_DESC_CONTENT(pdesc, txdesc_len);
 
-	if (b_firstseg)
+	if (firstseg)
 		SET_TX_DESC_OFFSET(pdesc, txdesc_len);
 
 	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
@@ -1074,7 +1071,6 @@ void rtl92ee_tx_fill_cmddesc(struct ieee80211_hw *hw,
 		      "H2C Tx Cmd Content\n", pdesc, txdesc_len);
 }
 
-
 void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 		      u8 desc_name, u8 *val)
 {
@@ -1095,12 +1091,14 @@ void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 			struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[q_idx];
 			u16 max_tx_desc = ring->entries;
+
 			if (q_idx == BEACON_QUEUE) {
 				ring->cur_tx_wp = 0;
 				ring->cur_tx_rp = 0;
 				SET_TX_BUFF_DESC_OWN(pdesc, 1);
 				return;
 			}
+
 			ring->cur_tx_wp = ((ring->cur_tx_wp + 1) % max_tx_desc);
 
 			if (over_run) {
@@ -1111,8 +1109,8 @@ void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 				ring->avl_desc--;
 
 				rtl_write_word(rtlpriv,
-					       get_desc_addr_fr_q(q_idx),
-				       ring->cur_tx_wp);
+					       get_desc_addr_fr_q_idx(q_idx),
+					       ring->cur_tx_wp);
 
 				if (q_idx == 1)
 					last_txw_point = cur_tx_wp;
@@ -1120,8 +1118,10 @@ void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 
 			if (ring->avl_desc < (max_tx_desc - 15)) {
 				u16 point_diff = 0;
-				tmp = rtl_read_dword(rtlpriv,
-						     get_desc_addr_fr_q(q_idx));
+
+				tmp =
+				  rtl_read_dword(rtlpriv,
+						 get_desc_addr_fr_q_idx(q_idx));
 				cur_tx_rp = (u16)((tmp >> 16) & 0x0fff);
 				cur_tx_wp = (u16)(tmp & 0x0fff);
 
@@ -1140,9 +1140,9 @@ void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 	} else {
 		switch (desc_name) {
 		case HW_DESC_RX_PREPARE:
-			SET_RX_BUFFER_DESC_LS(pdesc , 0);
-			SET_RX_BUFFER_DESC_FS(pdesc , 0);
-			SET_RX_BUFFER_DESC_TOTAL_LENGTH(pdesc , 0);
+			SET_RX_BUFFER_DESC_LS(pdesc, 0);
+			SET_RX_BUFFER_DESC_FS(pdesc, 0);
+			SET_RX_BUFFER_DESC_TOTAL_LENGTH(pdesc, 0);
 
 			SET_RX_BUFFER_DESC_DATA_LENGTH(pdesc,
 						       MAX_RECEIVE_BUFFER_SIZE +
@@ -1155,7 +1155,7 @@ void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 			break;
 		default:
 			RT_ASSERT(false,
-				  ("ERR rxdesc :%d not process\n", desc_name));
+				  "ERR rxdesc :%d not process\n", desc_name);
 			break;
 		}
 	}
@@ -1175,7 +1175,7 @@ u32 rtl92ee_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 			break;
 		default:
 			RT_ASSERT(false,
-				  ("ERR txdesc :%d not process\n", desc_name));
+				  "ERR txdesc :%d not process\n", desc_name);
 			break;
 		}
 	} else {
@@ -1191,7 +1191,7 @@ u32 rtl92ee_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 			break;
 		default:
 			RT_ASSERT(false,
-				  ("ERR rxdesc :%d not process\n", desc_name));
+				  "ERR rxdesc :%d not process\n", desc_name);
 			break;
 		}
 	}
@@ -1207,13 +1207,15 @@ bool rtl92ee_is_tx_desc_closed(struct ieee80211_hw *hw, u8 hw_queue, u16 index)
 	static u8 stop_report_cnt;
 	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
 
-	/*checking Read/Write Point each interrupt wastes CPU utilization.*/
-	if (stop_report_cnt > 15 || !rtlpriv->link_info.b_busytraffic) {
+	/*checking Read/Write Point each interrupt wastes CPU */
+	if (stop_report_cnt > 15 || !rtlpriv->link_info.busytraffic) {
 		u16 point_diff = 0;
 		u16 cur_tx_rp, cur_tx_wp;
-		u32 tmpu32;
+		u32 tmpu32 = 0;
 
-		tmpu32 = rtl_read_dword(rtlpriv, get_desc_addr_fr_q(hw_queue));
+		tmpu32 =
+		  rtl_read_dword(rtlpriv,
+				 get_desc_addr_fr_q_idx(hw_queue));
 		cur_tx_rp = (u16)((tmpu32 >> 16) & 0x0fff);
 		cur_tx_wp = (u16)(tmpu32 & 0x0fff);
 
@@ -1262,23 +1264,28 @@ bool rtl92ee_is_tx_desc_closed(struct ieee80211_hw *hw, u8 hw_queue, u16 index)
 	return ret;
 }
 
+void rtl92ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
+{
+}
+
 u32 rtl92ee_rx_command_packet(struct ieee80211_hw *hw,
-			      const struct rtl_stats *status,
+			      struct rtl_stats status,
 			      struct sk_buff *skb)
 {
 	u32 result = 0;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	switch (status->packet_report_type) {
+	switch (status.packet_report_type) {
 	case NORMAL_RX:
 		result = 0;
 		break;
 	case C2H_PACKET:
-		rtl92ee_c2h_packet_handler(hw, skb->data, (u8) skb->len);
+		rtl92ee_c2h_packet_handler(hw, skb->data, (u8)skb->len);
 		result = 1;
 		break;
 	default:
-		RT_TRACE(COMP_RECV, DBG_TRACE, ("No this packet type!!\n"));
+		RT_TRACE(rtlpriv, COMP_RECV, DBG_TRACE,
+			 "Unknown packet type %d\n", status.packet_report_type);
 		break;
 	}
 
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/trx.h b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.h
index e04ee7e6d9b2..6f9be1c7515c 100644
--- a/drivers/staging/rtl8192ee/rtl8192ee/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192ee/trx.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2014  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,8 +26,6 @@
 #ifndef __RTL92E_TRX_H__
 #define __RTL92E_TRX_H__
 
-
-
 #if (DMA_IS_64BIT == 1)
 #if (RTL8192EE_SEG_NUM == 2)
 #define TX_BD_DESC_SIZE					128
@@ -127,7 +121,6 @@
 #define SET_TX_DESC_TXOP_PS_MODE(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 31, 1, __val)
 
-
 #define GET_TX_DESC_MACID(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
 #define GET_TX_DESC_AGG_ENABLE(__pdesc)			\
@@ -215,7 +208,6 @@
 #define SET_TX_DESC_AMPDU_MAX_TIME(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 24, 8, __val)
 
-
 /* Dword 4 */
 #define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 7, __val)
@@ -236,7 +228,6 @@
 #define SET_TX_DESC_PCTS_MASK_IDX(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 30, 2, __val)
 
-
 /* Dword 5 */
 #define SET_TX_DESC_TX_SUB_CARRIER(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 4, __val)
@@ -254,9 +245,9 @@
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 12, 1, __val)
 #define SET_TX_DESC_RTS_SC(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __val)
-#define SET_TX_DESC_TX_ANT(__pdesc , __val)		\
+#define SET_TX_DESC_TX_ANT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 24, 4, __val)
-#define SET_TX_DESC_TX_POWER_0_PSET(__pdesc , __val)	\
+#define SET_TX_DESC_TX_POWER_0_PSET(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 28, 3, __val)
 
 /* Dword 6 */
@@ -279,70 +270,65 @@
 
 /* Dword 8 */
 #define SET_TX_DESC_RTS_RC(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 0 , 6 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 0, 6, __val)
 #define SET_TX_DESC_BAR_RTY_TH(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 6 , 2 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 6, 2, __val)
 #define SET_TX_DESC_DATA_RC(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 8 , 6 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 8, 6, __val)
 #define SET_TX_DESC_ENABLE_HW_SELECT(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 15 , 1 , __val)
-#define SET_TX_DESC_NEXT_HEAD_PAGE(__pdesc , __val)(__pdesc, __val) \
-	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 16 , 8 , __val)
-#define SET_TX_DESC_TAIL_PAGE(__pdesc , __val)(__pdesc, __val)\
-	SET_BITS_TO_LE_4BYTE(__pdesc+32 , 24 , 8 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 15, 1, __val)
+#define SET_TX_DESC_NEXT_HEAD_PAGE(__pdesc, __val)	\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 16, 8, __val)
+#define SET_TX_DESC_TAIL_PAGE(__pdesc, __val)		\
+	SET_BITS_TO_LE_4BYTE(__pdesc+32, 24, 8, __val)
 
 /* Dword 9 */
 #define SET_TX_DESC_PADDING_LENGTH(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 0 , 11 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 0, 11, __val)
 #define SET_TX_DESC_TXBF_PATH(__pdesc, __val)		\
-	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 11 , 1 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 11, 1, __val)
 #define SET_TX_DESC_SEQ(__pdesc, __val)			\
-	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 12 , 12 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 12, 12, __val)
 #define SET_TX_DESC_FINAL_DATA_RATE(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+36 , 24 , 8 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+36, 24, 8, __val)
 
 /* Dword 10 */
 #define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+40 , 0 , 32 , __val)
-
+	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
 
 /* Dword 11*/
 #define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+48 , 0 , 32 , __val)
-
-
-#define SET_EARLYMODE_PKTNUM(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr , 0 , 4 , __val)
-#define SET_EARLYMODE_LEN0(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr , 4 , 15 , __val)
-#define SET_EARLYMODE_LEN1(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr , 16 , 2 , __val)
-#define SET_EARLYMODE_LEN1_1(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr , 19 , 13 , __val)
-#define SET_EARLYMODE_LEN1_2(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr+4 , 0 , 2 , __val)
-#define SET_EARLYMODE_LEN2(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr+4 , 2 , 15 ,  __val)
-#define SET_EARLYMODE_LEN2_1(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr , 2 , 4 ,  __val)
-#define SET_EARLYMODE_LEN2_2(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr+4 , 0 , 8 ,  __val)
-#define SET_EARLYMODE_LEN3(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr+4 , 17 , 15, __val)
-#define SET_EARLYMODE_LEN4(__paddr , __val)		\
-	SET_BITS_TO_LE_4BYTE(__paddr+4 , 20 , 12 , __val)
-
+	SET_BITS_TO_LE_4BYTE(__pdesc+48, 0, 32, __val)
+
+#define SET_EARLYMODE_PKTNUM(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 0, 4, __val)
+#define SET_EARLYMODE_LEN0(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 4, 15, __val)
+#define SET_EARLYMODE_LEN1(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 16, 2, __val)
+#define SET_EARLYMODE_LEN1_1(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 19, 13, __val)
+#define SET_EARLYMODE_LEN1_2(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 0, 2, __val)
+#define SET_EARLYMODE_LEN2(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 2, 15,  __val)
+#define SET_EARLYMODE_LEN2_1(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr, 2, 4,  __val)
+#define SET_EARLYMODE_LEN2_2(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 0, 8,  __val)
+#define SET_EARLYMODE_LEN3(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 17, 15, __val)
+#define SET_EARLYMODE_LEN4(__paddr, __val)		\
+	SET_BITS_TO_LE_4BYTE(__paddr+4, 20, 12, __val)
 
 /* TX/RX buffer descriptor */
 
 #define SET_TX_EXTBUFF_DESC_LEN(__pdesc, __val, __set)	\
-	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16) , 0 , 16 , __val)
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16), 0, 16, __val)
 #define SET_TX_EXTBUFF_DESC_ADDR_LOW(__pdesc, __val, __set)\
-	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16)+4 , 0 , 32 , __val)
-#define SET_TX_EXTBUFF_DESC_ADDR_HIGH(__pdesc, __val , __set)\
-	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16)+8 , 0 , 32 , __val)
-
-
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16)+4, 0, 32, __val)
+#define SET_TX_EXTBUFF_DESC_ADDR_HIGH(__pdesc, __val, __set)\
+	SET_BITS_TO_LE_4BYTE(__pdesc+(__set*16)+8, 0, 32, __val)
 
 /* for Txfilldescroptor92ee, fill the desc content. */
 #if (DMA_IS_64BIT == 1)
@@ -454,32 +440,31 @@
 /* RX buffer  */
 
 /* DWORD 0 */
-#define SET_RX_BUFFER_DESC_DATA_LENGTH(__pRxStatusDesc , __val)	\
-	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 0, 14, __val)
-#define SET_RX_BUFFER_DESC_LS(__pRxStatusDesc , __val)		\
-	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 15, 1, __val)
-#define SET_RX_BUFFER_DESC_FS(__pRxStatusDesc , __val)		\
-	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 16, 1, __val)
-#define SET_RX_BUFFER_DESC_TOTAL_LENGTH(__pRxStatusDesc , __val)	\
-	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc , 16, 15, __val)
-
-#define GET_RX_BUFFER_DESC_OWN(__pRxStatusDesc)			\
-	LE_BITS_TO_4BYTE(__pRxStatusDesc , 31, 1)
-#define GET_RX_BUFFER_DESC_LS(__pRxStatusDesc)			\
-	LE_BITS_TO_4BYTE(__pRxStatusDesc , 15, 1)
-#define GET_RX_BUFFER_DESC_FS(__pRxStatusDesc)			\
-	LE_BITS_TO_4BYTE(__pRxStatusDesc , 16, 1)
-#define GET_RX_BUFFER_DESC_TOTAL_LENGTH(__pRxStatusDesc)	\
-	LE_BITS_TO_4BYTE(__pRxStatusDesc , 16, 15)
-
+#define SET_RX_BUFFER_DESC_DATA_LENGTH(__status, __val)	\
+	SET_BITS_TO_LE_4BYTE(__status, 0, 14, __val)
+#define SET_RX_BUFFER_DESC_LS(__status, __val)		\
+	SET_BITS_TO_LE_4BYTE(__status, 15, 1, __val)
+#define SET_RX_BUFFER_DESC_FS(__status, __val)		\
+	SET_BITS_TO_LE_4BYTE(__status, 16, 1, __val)
+#define SET_RX_BUFFER_DESC_TOTAL_LENGTH(__status, __val)	\
+	SET_BITS_TO_LE_4BYTE(__status, 16, 15, __val)
+
+#define GET_RX_BUFFER_DESC_OWN(__status)			\
+	LE_BITS_TO_4BYTE(__status, 31, 1)
+#define GET_RX_BUFFER_DESC_LS(__status)			\
+	LE_BITS_TO_4BYTE(__status, 15, 1)
+#define GET_RX_BUFFER_DESC_FS(__status)			\
+	LE_BITS_TO_4BYTE(__status, 16, 1)
+#define GET_RX_BUFFER_DESC_TOTAL_LENGTH(__status)	\
+	LE_BITS_TO_4BYTE(__status, 16, 15)
 
 /* DWORD 1 */
-#define SET_RX_BUFFER_PHYSICAL_LOW(__pRxStatusDesc , __val)	\
-	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc+4, 0, 32, __val)
+#define SET_RX_BUFFER_PHYSICAL_LOW(__status, __val)	\
+	SET_BITS_TO_LE_4BYTE(__status+4, 0, 32, __val)
 
 /* DWORD 2 */
-#define SET_RX_BUFFER_PHYSICAL_HIGH(__pRxStatusDesc , __val)	\
-	SET_BITS_TO_LE_4BYTE(__pRxStatusDesc+8, 0, 32, __val)
+#define SET_RX_BUFFER_PHYSICAL_HIGH(__status, __val)	\
+	SET_BITS_TO_LE_4BYTE(__status+8, 0, 32, __val)
 
 #define GET_RX_DESC_PKT_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc, 0, 14)
@@ -575,7 +560,6 @@
 #define GET_RX_STATUS_DESC_MAGIC_MATCH(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+12, 31, 1)
 
-
 #define GET_RX_DESC_TSFL(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+20, 0, 32)
 
@@ -589,16 +573,14 @@
 #define SET_RX_DESC_BUFF_ADDR64(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
 
-
 /* TX report 2 format in Rx desc*/
 
-#define GET_RX_RPT2_DESC_PKT_LEN(__pRxStatusDesc)	\
-	LE_BITS_TO_4BYTE(__pRxStatusDesc , 0, 9)
-#define GET_RX_RPT2_DESC_MACID_VALID_1(__pRxStatusDesc)	\
-	LE_BITS_TO_4BYTE(__pRxStatusDesc+16, 0, 32)
-#define GET_RX_RPT2_DESC_MACID_VALID_2(__pRxStatusDesc)	\
-	LE_BITS_TO_4BYTE(__pRxStatusDesc+20, 0, 32)
-
+#define GET_RX_RPT2_DESC_PKT_LEN(__status)	\
+	LE_BITS_TO_4BYTE(__status, 0, 9)
+#define GET_RX_RPT2_DESC_MACID_VALID_1(__status)	\
+	LE_BITS_TO_4BYTE(__status+16, 0, 32)
+#define GET_RX_RPT2_DESC_MACID_VALID_2(__status)	\
+	LE_BITS_TO_4BYTE(__status+20, 0, 32)
 
 #define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)		\
 do {								\
@@ -608,7 +590,7 @@ do {								\
 		memset(__pdesc, 0, _size);			\
 } while (0)
 
-#define RX_HAL_IS_CCK_RATE(rxmcs)\
+#define RTL92EE_RX_HAL_IS_CCK_RATE(rxmcs)\
 	(rxmcs == DESC92C_RATE1M ||\
 	 rxmcs == DESC92C_RATE2M ||\
 	 rxmcs == DESC92C_RATE5_5M ||\
@@ -618,11 +600,12 @@ do {								\
 
 struct phy_rx_agc_info_t {
 	#if IS_LITTLE_ENDIAN
-		u8 gain:7 , trsw:1;
+		u8 gain:7, trsw:1;
 	#else
-		u8 trsw:1 , gain:7;
+		u8 trsw:1, gain:7;
 	#endif
 };
+
 struct phy_status_rpt {
 	struct phy_rx_agc_info_t path_agc[2];
 	u8 ch_corr[2];
@@ -846,12 +829,11 @@ void rtl92ee_rx_check_dma_ok(struct ieee80211_hw *hw, u8 *header_desc,
 			     u8 queue_index);
 u16	rtl92ee_rx_desc_buff_remained_cnt(struct ieee80211_hw *hw,
 					  u8 queue_index);
-void rtl92ee_get_available_desc(struct ieee80211_hw *hw , u8 queue_index);
+void rtl92ee_get_available_desc(struct ieee80211_hw *hw, u8 queue_index);
 void rtl92ee_pre_fill_tx_bd_desc(struct ieee80211_hw *hw,
 				 u8 *tx_bd_desc, u8 *desc, u8 queue_index,
 				 struct sk_buff *skb, dma_addr_t addr);
 
-
 void rtl92ee_tx_fill_desc(struct ieee80211_hw *hw,
 			  struct ieee80211_hdr *hdr, u8 *pdesc_tx,
 			  u8 *pbd_desc_tx,
@@ -868,10 +850,11 @@ void rtl92ee_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 
 u32 rtl92ee_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 bool rtl92ee_is_tx_desc_closed(struct ieee80211_hw *hw, u8 hw_queue, u16 index);
+void rtl92ee_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl92ee_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
-			     bool b_firstseg, bool b_lastseg,
+			     bool firstseg, bool lastseg,
 			     struct sk_buff *skb);
 u32 rtl92ee_rx_command_packet(struct ieee80211_hw *hw,
-			      const struct rtl_stats *status,
+			      struct rtl_stats status,
 			      struct sk_buff *skb);
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/fw.h b/drivers/net/wireless/rtlwifi/rtl8192se/fw.h
index d53f4332464d..b1e44b86e8ed 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/fw.h
@@ -336,7 +336,6 @@ enum fw_h2c_cmd {
 	H2C_TMP3,
 	H2C_WOWLAN_UPDATE_IV_CMD,			/*50*/
 	H2C_TMP4,
-	MAX_H2CCMD					/*52*/
 };
 
 /* The following macros are used for FW
diff --git a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
index 2b3c78baa9f8..b358ebce8942 100644
--- a/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8192se/trx.c
@@ -312,10 +312,6 @@ bool rtl92se_rx_query_desc(struct ieee80211_hw *hw, struct rtl_stats *stats,
 		hdr = (struct ieee80211_hdr *)(skb->data +
 		       stats->rx_drvinfo_size + stats->rx_bufshift);
 
-		if (!hdr) {
-			/* during testing, hdr was NULL here */
-			return false;
-		}
 		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
 			(ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag &= ~RX_FLAG_DECRYPTED;
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/btc.h b/drivers/net/wireless/rtlwifi/rtl8723ae/btc.h
index 417afeed36af..06c448c010fd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/btc.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/btc.h
@@ -11,10 +11,6 @@
  ** FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  ** more details.
  **
- ** You should have received a copy of the GNU General Public License along with
- ** this program; if not, write to the Free Software Foundation, Inc.,
- ** 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- **
  ** The full GNU General Public License is included in this distribution in the
  ** file called LICENSE.
  **
@@ -24,8 +20,7 @@
  ** Hsinchu 300, Taiwan.
  ** Larry Finger <Larry.Finger@lwfinger.net>
  **
- *****************************************************************************
- */
+ ******************************************************************************/
 
 #ifndef __RTL8723E_BTC_H__
 #define __RTL8723E_BTC_H__
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/def.h b/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
index debe261a7eeb..94bdd4bbca5d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/def.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -25,55 +21,145 @@
  *
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
- ****************************************************************************
- */
+ *****************************************************************************/
 
 #ifndef __RTL8723E_DEF_H__
 #define __RTL8723E_DEF_H__
 
+#define HAL_RETRY_LIMIT_INFRA				48
+#define HAL_RETRY_LIMIT_AP_ADHOC			7
+
+#define RESET_DELAY_8185					20
+
+#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
+#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
+
+#define NUM_OF_FIRMWARE_QUEUE				10
+#define NUM_OF_PAGES_IN_FW					0x100
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO			0x07
+#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA		0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_CMD			0x0
+#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT		0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH		0x02
+#define NUM_OF_PAGE_IN_FW_QUEUE_BCN			0x2
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB			0xA1
+
+#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM		0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM		0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM		0x048
+#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM		0x026
+#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM		0x00
+
+#define MAX_LINES_HWCONFIG_TXT				1000
+#define MAX_BYTES_LINE_HWCONFIG_TXT			256
+
+#define SW_THREE_WIRE						0
+#define HW_THREE_WIRE						2
+
+#define BT_DEMO_BOARD						0
+#define BT_QA_BOARD							1
+#define BT_FPGA								2
+
+#define HAL_PRIME_CHNL_OFFSET_DONT_CARE		0
 #define HAL_PRIME_CHNL_OFFSET_LOWER			1
+#define HAL_PRIME_CHNL_OFFSET_UPPER			2
 
-#define RX_MPDU_QUEUE					0
+#define MAX_H2C_QUEUE_NUM					10
 
-#define CHIP_8723			BIT(0)
-#define NORMAL_CHIP			BIT(3)
-#define RF_TYPE_1T2R			BIT(4)
-#define RF_TYPE_2T2R			BIT(5)
-#define CHIP_VENDOR_UMC			BIT(7)
-#define B_CUT_VERSION			BIT(12)
-#define C_CUT_VERSION			BIT(13)
-#define D_CUT_VERSION			((BIT(12)|BIT(13)))
-#define E_CUT_VERSION			BIT(14)
-#define	RF_RL_ID			(BIT(31)|BIT(30)|BIT(29)|BIT(28))
+#define RX_MPDU_QUEUE						0
+#define RX_CMD_QUEUE						1
+#define RX_MAX_QUEUE						2
+#define AC2QUEUEID(_AC)						(_AC)
 
+#define	C2H_RX_CMD_HDR_LEN					8
+#define	GET_C2H_CMD_CMD_LEN(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 0, 16)
+#define	GET_C2H_CMD_ELEMENT_ID(__prxhdr)	\
+	LE_BITS_TO_4BYTE((__prxhdr), 16, 8)
+#define	GET_C2H_CMD_CMD_SEQ(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 24, 7)
+#define	GET_C2H_CMD_CONTINUE(__prxhdr)		\
+	LE_BITS_TO_4BYTE((__prxhdr), 31, 1)
+#define	GET_C2H_CMD_CONTENT(__prxhdr)		\
+	((u8 *)(__prxhdr) + C2H_RX_CMD_HDR_LEN)
+
+#define	GET_C2H_CMD_FEEDBACK_ELEMENT_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 0, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_LEN(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 8, 8)
+#define	GET_C2H_CMD_FEEDBACK_CCX_CMD_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE((__pcmdfbhdr), 16, 16)
+#define	GET_C2H_CMD_FEEDBACK_CCX_MAC_ID(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 0, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_VALID(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 7, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_RETRY_CNT(__pcmdfbhdr)	\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 8, 5)
+#define	GET_C2H_CMD_FEEDBACK_CCX_TOK(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 15, 1)
+#define	GET_C2H_CMD_FEEDBACK_CCX_QSEL(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 16, 4)
+#define	GET_C2H_CMD_FEEDBACK_CCX_SEQ(__pcmdfbhdr)		\
+	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 20, 12)
+
+#define CHIP_BONDING_IDENTIFIER(_value)	(((_value)>>22)&0x3)
+#define	CHIP_BONDING_92C_1T2R		0x1
+
+#define CHIP_8723		BIT(0)
+#define NORMAL_CHIP		BIT(3)
+#define RF_TYPE_1T1R		(~(BIT(4)|BIT(5)|BIT(6)))
+#define RF_TYPE_1T2R		BIT(4)
+#define RF_TYPE_2T2R		BIT(5)
+#define CHIP_VENDOR_UMC		BIT(7)
+#define B_CUT_VERSION		BIT(12)
+#define C_CUT_VERSION		BIT(13)
+#define D_CUT_VERSION		((BIT(12)|BIT(13)))
+#define E_CUT_VERSION		BIT(14)
+#define	RF_RL_ID		(BIT(31)|BIT(30)|BIT(29)|BIT(28))
 
 /* MASK */
-#define IC_TYPE_MASK			(BIT(0)|BIT(1)|BIT(2))
-#define CHIP_TYPE_MASK			BIT(3)
-#define RF_TYPE_MASK			(BIT(4)|BIT(5)|BIT(6))
-#define MANUFACTUER_MASK		BIT(7)
-#define ROM_VERSION_MASK		(BIT(11)|BIT(10)|BIT(9)|BIT(8))
-#define CUT_VERSION_MASK		(BIT(15)|BIT(14)|BIT(13)|BIT(12))
+#define IC_TYPE_MASK		(BIT(0)|BIT(1)|BIT(2))
+#define CHIP_TYPE_MASK		BIT(3)
+#define RF_TYPE_MASK		(BIT(4)|BIT(5)|BIT(6))
+#define MANUFACTUER_MASK	BIT(7)
+#define ROM_VERSION_MASK	(BIT(11)|BIT(10)|BIT(9)|BIT(8))
+#define CUT_VERSION_MASK	(BIT(15)|BIT(14)|BIT(13)|BIT(12))
 
 /* Get element */
 #define GET_CVID_IC_TYPE(version)	((version) & IC_TYPE_MASK)
+#define GET_CVID_CHIP_TYPE(version)	((version) & CHIP_TYPE_MASK)
+#define GET_CVID_RF_TYPE(version)	((version) & RF_TYPE_MASK)
 #define GET_CVID_MANUFACTUER(version)	((version) & MANUFACTUER_MASK)
+#define GET_CVID_ROM_VERSION(version)	((version) & ROM_VERSION_MASK)
 #define GET_CVID_CUT_VERSION(version)	((version) & CUT_VERSION_MASK)
 
-#define IS_81XXC(version)		((GET_CVID_IC_TYPE(version) == 0) ?\
-					true : false)
-#define IS_8723_SERIES(version)						\
-		((GET_CVID_IC_TYPE(version) == CHIP_8723) ? true : false)
-#define IS_CHIP_VENDOR_UMC(version)					\
-		((GET_CVID_MANUFACTUER(version)) ? true : false)
-
-#define IS_VENDOR_UMC_A_CUT(version)	((IS_CHIP_VENDOR_UMC(version)) ? \
-		((GET_CVID_CUT_VERSION(version)) ? false : true) : false)
-#define IS_VENDOR_8723_A_CUT(version)	((IS_8723_SERIES(version)) ?	\
-		((GET_CVID_CUT_VERSION(version)) ? false : true) : false)
-#define IS_81xxC_VENDOR_UMC_B_CUT(version)	((IS_CHIP_VENDOR_UMC(version)) \
-		? ((GET_CVID_CUT_VERSION(version) == B_CUT_VERSION) ? \
-		true : false) : false)
+#define IS_81XXC(version)	((GET_CVID_IC_TYPE(version) == 0) ?\
+						true : false)
+#define IS_8723_SERIES(version)	((GET_CVID_IC_TYPE(version) == CHIP_8723) ? \
+						true : false)
+#define IS_1T1R(version)	((GET_CVID_RF_TYPE(version)) ? false : true)
+#define IS_1T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R)\
+						? true : false)
+#define IS_2T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R)\
+						? true : false)
+#define IS_CHIP_VENDOR_UMC(version)	((GET_CVID_MANUFACTUER(version)) ? \
+						true : false)
+
+#define IS_VENDOR_UMC_A_CUT(version)	((IS_CHIP_VENDOR_UMC(version))\
+					? ((GET_CVID_CUT_VERSION(version)) ? \
+					false : true) : false)
+#define IS_VENDOR_8723_A_CUT(version)	((IS_8723_SERIES(version))\
+					? ((GET_CVID_CUT_VERSION(version)) ? \
+					false : true) : false)
+#define IS_VENDOR_8723A_B_CUT(version)	((IS_8723_SERIES(version))\
+		? ((GET_CVID_CUT_VERSION(version) == \
+		B_CUT_VERSION) ? true : false) : false)
+#define IS_81xxC_VENDOR_UMC_B_CUT(version)	((IS_CHIP_VENDOR_UMC(version))\
+		? ((GET_CVID_CUT_VERSION(version) == \
+		B_CUT_VERSION) ? true : false) : false)
 
 enum rf_optype {
 	RF_OP_BY_SW_3WIRE = 0,
@@ -93,7 +179,7 @@ enum power_save_mode {
 	POWER_SAVE_MODE_SAVE,
 };
 
-enum power_polocy_config {
+enum power_policy_config {
 	POWERCFG_MAX_POWER_SAVINGS,
 	POWERCFG_GLOBAL_POWER_SAVINGS,
 	POWERCFG_LOCAL_POWER_SAVINGS,
@@ -143,6 +229,41 @@ enum rtl_desc_qsel {
 	QSLT_CMD = 0x13,
 };
 
+enum rtl_desc8723e_rate {
+	DESC92C_RATE1M = 0x00,
+	DESC92C_RATE2M = 0x01,
+	DESC92C_RATE5_5M = 0x02,
+	DESC92C_RATE11M = 0x03,
+
+	DESC92C_RATE6M = 0x04,
+	DESC92C_RATE9M = 0x05,
+	DESC92C_RATE12M = 0x06,
+	DESC92C_RATE18M = 0x07,
+	DESC92C_RATE24M = 0x08,
+	DESC92C_RATE36M = 0x09,
+	DESC92C_RATE48M = 0x0a,
+	DESC92C_RATE54M = 0x0b,
+
+	DESC92C_RATEMCS0 = 0x0c,
+	DESC92C_RATEMCS1 = 0x0d,
+	DESC92C_RATEMCS2 = 0x0e,
+	DESC92C_RATEMCS3 = 0x0f,
+	DESC92C_RATEMCS4 = 0x10,
+	DESC92C_RATEMCS5 = 0x11,
+	DESC92C_RATEMCS6 = 0x12,
+	DESC92C_RATEMCS7 = 0x13,
+	DESC92C_RATEMCS8 = 0x14,
+	DESC92C_RATEMCS9 = 0x15,
+	DESC92C_RATEMCS10 = 0x16,
+	DESC92C_RATEMCS11 = 0x17,
+	DESC92C_RATEMCS12 = 0x18,
+	DESC92C_RATEMCS13 = 0x19,
+	DESC92C_RATEMCS14 = 0x1a,
+	DESC92C_RATEMCS15 = 0x1b,
+	DESC92C_RATEMCS15_SG = 0x1c,
+	DESC92C_RATEMCS32 = 0x20,
+};
+
 struct phy_sts_cck_8723e_t {
 	u8 adc_pwdb_X[4];
 	u8 sq_rpt;
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
index 25cc83058b01..a0e86922780a 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -25,8 +21,7 @@
  *
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
- ****************************************************************************
- */
+ *****************************************************************************/
 
 #include "../wifi.h"
 #include "../base.h"
@@ -151,7 +146,7 @@ static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
 	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}
 };
 
-static void rtl8723ae_dm_diginit(struct ieee80211_hw *hw)
+static void rtl8723e_dm_diginit(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
@@ -176,7 +171,7 @@ static void rtl8723ae_dm_diginit(struct ieee80211_hw *hw)
 	dm_digtable->cur_cck_pd_state = CCK_PD_STAGE_MAX;
 }
 
-static u8 rtl_init_gain_min_pwdb(struct ieee80211_hw *hw)
+static u8 rtl8723e_dm_initial_gain_min_pwdb(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
@@ -195,14 +190,15 @@ static u8 rtl_init_gain_min_pwdb(struct ieee80211_hw *hw)
 	} else if (dm_digtable->cursta_cstate == DIG_STA_CONNECT ||
 		   dm_digtable->cursta_cstate == DIG_STA_BEFORE_CONNECT) {
 		rssi_val_min = rtlpriv->dm.undec_sm_pwdb;
-	} else if (dm_digtable->curmultista_cstate == DIG_MULTISTA_CONNECT) {
+	} else if (dm_digtable->curmultista_cstate ==
+		DIG_MULTISTA_CONNECT) {
 		rssi_val_min = rtlpriv->dm.entry_min_undec_sm_pwdb;
 	}
 
 	return (u8) rssi_val_min;
 }
 
-static void rtl8723ae_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+static void rtl8723e_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 {
 	u32 ret_value;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -239,8 +235,7 @@ static void rtl8723ae_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, RCCK0_FALSEALARMREPORT, 0x0000c000, 2);
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, "
-		 "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
 		 falsealm_cnt->cnt_parity_fail,
 		 falsealm_cnt->cnt_rate_illegal,
 		 falsealm_cnt->cnt_crc8_fail, falsealm_cnt->cnt_mcs_fail);
@@ -263,52 +258,60 @@ static void rtl92c_dm_ctrl_initgain_by_fa(struct ieee80211_hw *hw)
 		value_igi += 0;
 	else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH2)
 		value_igi++;
-	else
+	else if (rtlpriv->falsealm_cnt.cnt_all >= DM_DIG_FA_TH2)
 		value_igi += 2;
-
-	value_igi = clamp(value_igi, (u8)DM_DIG_FA_LOWER, (u8)DM_DIG_FA_UPPER);
+	if (value_igi > DM_DIG_FA_UPPER)
+		value_igi = DM_DIG_FA_UPPER;
+	else if (value_igi < DM_DIG_FA_LOWER)
+		value_igi = DM_DIG_FA_LOWER;
 	if (rtlpriv->falsealm_cnt.cnt_all > 10000)
 		value_igi = 0x32;
 
 	dm_digtable->cur_igvalue = value_igi;
-	rtl8723ae_dm_write_dig(hw);
+	rtl8723e_dm_write_dig(hw);
 }
 
 static void rtl92c_dm_ctrl_initgain_by_rssi(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *dgtbl = &rtlpriv->dm_digtable;
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
-	if (rtlpriv->falsealm_cnt.cnt_all > dgtbl->fa_highthresh) {
-		if ((dgtbl->back_val - 2) < dgtbl->back_range_min)
-			dgtbl->back_val = dgtbl->back_range_min;
+	if (rtlpriv->falsealm_cnt.cnt_all > dm_digtable->fa_highthresh) {
+		if ((dm_digtable->back_val - 2) <
+		    dm_digtable->back_range_min)
+			dm_digtable->back_val =
+			    dm_digtable->back_range_min;
 		else
-			dgtbl->back_val -= 2;
-	} else if (rtlpriv->falsealm_cnt.cnt_all < dgtbl->fa_lowthresh) {
-		if ((dgtbl->back_val + 2) > dgtbl->back_range_max)
-			dgtbl->back_val = dgtbl->back_range_max;
+			dm_digtable->back_val -= 2;
+	} else if (rtlpriv->falsealm_cnt.cnt_all < dm_digtable->fa_lowthresh) {
+		if ((dm_digtable->back_val + 2) >
+		    dm_digtable->back_range_max)
+			dm_digtable->back_val =
+			    dm_digtable->back_range_max;
 		else
-			dgtbl->back_val += 2;
+			dm_digtable->back_val += 2;
 	}
 
-	if ((dgtbl->rssi_val_min + 10 - dgtbl->back_val) >
-	    dgtbl->rx_gain_max)
-		dgtbl->cur_igvalue = dgtbl->rx_gain_max;
-	else if ((dgtbl->rssi_val_min + 10 -
-		  dgtbl->back_val) < dgtbl->rx_gain_min)
-		dgtbl->cur_igvalue = dgtbl->rx_gain_min;
+	if ((dm_digtable->rssi_val_min + 10 - dm_digtable->back_val) >
+	    dm_digtable->rx_gain_max)
+		dm_digtable->cur_igvalue = dm_digtable->rx_gain_max;
+	else if ((dm_digtable->rssi_val_min + 10 -
+		  dm_digtable->back_val) < dm_digtable->rx_gain_min)
+		dm_digtable->cur_igvalue = dm_digtable->rx_gain_min;
 	else
-		dgtbl->cur_igvalue = dgtbl->rssi_val_min + 10 - dgtbl->back_val;
+		dm_digtable->cur_igvalue = dm_digtable->rssi_val_min + 10 -
+		    dm_digtable->back_val;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
 		 "rssi_val_min = %x back_val %x\n",
-		 dgtbl->rssi_val_min, dgtbl->back_val);
+		  dm_digtable->rssi_val_min, dm_digtable->back_val);
 
-	rtl8723ae_dm_write_dig(hw);
+	rtl8723e_dm_write_dig(hw);
 }
 
-static void rtl8723ae_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
+static void rtl8723e_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
 {
+	static u8 binitialized;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
@@ -318,16 +321,15 @@ static void rtl8723ae_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
 	if (mac->opmode == NL80211_IFTYPE_ADHOC)
 		multi_sta = true;
 
-	if ((!multi_sta) ||
-	    (dm_digtable->cursta_cstate != DIG_STA_DISCONNECT)) {
-		rtlpriv->initialized = false;
+	if (!multi_sta || (dm_digtable->cursta_cstate != DIG_STA_DISCONNECT)) {
+		binitialized = false;
 		dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_MAX;
 		return;
-	} else if (!rtlpriv->initialized) {
-		rtlpriv->initialized = true;
+	} else if (!binitialized) {
+		binitialized = true;
 		dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
 		dm_digtable->cur_igvalue = 0x20;
-		rtl8723ae_dm_write_dig(hw);
+		rtl8723e_dm_write_dig(hw);
 	}
 
 	if (dm_digtable->curmultista_cstate == DIG_MULTISTA_CONNECT) {
@@ -337,7 +339,7 @@ static void rtl8723ae_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
 			if (dm_digtable->dig_ext_port_stage ==
 			    DIG_EXT_PORT_STAGE_2) {
 				dm_digtable->cur_igvalue = 0x20;
-				rtl8723ae_dm_write_dig(hw);
+				rtl8723e_dm_write_dig(hw);
 			}
 
 			dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_1;
@@ -348,7 +350,7 @@ static void rtl8723ae_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
 	} else if (dm_digtable->dig_ext_port_stage != DIG_EXT_PORT_STAGE_0) {
 		dm_digtable->dig_ext_port_stage = DIG_EXT_PORT_STAGE_0;
 		dm_digtable->cur_igvalue = 0x20;
-		rtl8723ae_dm_write_dig(hw);
+		rtl8723e_dm_write_dig(hw);
 	}
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
@@ -357,22 +359,22 @@ static void rtl8723ae_dm_initial_gain_multi_sta(struct ieee80211_hw *hw)
 		 dm_digtable->dig_ext_port_stage);
 }
 
-static void rtl8723ae_dm_initial_gain_sta(struct ieee80211_hw *hw)
+static void rtl8723e_dm_initial_gain_sta(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
 		 "presta_cstate = %x, cursta_cstate = %x\n",
-		 dm_digtable->presta_cstate,
-		 dm_digtable->cursta_cstate);
+		  dm_digtable->presta_cstate,
+		  dm_digtable->cursta_cstate);
 
 	if (dm_digtable->presta_cstate == dm_digtable->cursta_cstate ||
 	    dm_digtable->cursta_cstate == DIG_STA_BEFORE_CONNECT ||
 	    dm_digtable->cursta_cstate == DIG_STA_CONNECT) {
-
 		if (dm_digtable->cursta_cstate != DIG_STA_DISCONNECT) {
-			dm_digtable->rssi_val_min = rtl_init_gain_min_pwdb(hw);
+			dm_digtable->rssi_val_min =
+			    rtl8723e_dm_initial_gain_min_pwdb(hw);
 			rtl92c_dm_ctrl_initgain_by_rssi(hw);
 		}
 	} else {
@@ -381,16 +383,17 @@ static void rtl8723ae_dm_initial_gain_sta(struct ieee80211_hw *hw)
 		dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
 		dm_digtable->cur_igvalue = 0x20;
 		dm_digtable->pre_igvalue = 0;
-		rtl8723ae_dm_write_dig(hw);
+		rtl8723e_dm_write_dig(hw);
 	}
 }
-static void rtl8723ae_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
+
+static void rtl8723e_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	if (dm_digtable->cursta_cstate == DIG_STA_CONNECT) {
-		dm_digtable->rssi_val_min = rtl_init_gain_min_pwdb(hw);
+		dm_digtable->rssi_val_min = rtl8723e_dm_initial_gain_min_pwdb(hw);
 
 		if (dm_digtable->pre_cck_pd_state == CCK_PD_STAGE_LowRssi) {
 			if (dm_digtable->rssi_val_min <= 25)
@@ -418,12 +421,11 @@ static void rtl8723ae_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 				    CCK_FA_STAGE_High;
 			else
 				dm_digtable->cur_cck_fa_state =
-							 CCK_FA_STAGE_Low;
-
+				    CCK_FA_STAGE_LOW;
 			if (dm_digtable->pre_cck_fa_state !=
 			    dm_digtable->cur_cck_fa_state) {
 				if (dm_digtable->cur_cck_fa_state ==
-				    CCK_FA_STAGE_Low)
+				    CCK_FA_STAGE_LOW)
 					rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2,
 						      0x83);
 				else
@@ -449,13 +451,13 @@ static void rtl8723ae_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 
 }
 
-static void rtl8723ae_dm_ctrl_initgain_by_twoport(struct ieee80211_hw *hw)
+static void rtl8723e_dm_ctrl_initgain_by_twoport(struct ieee80211_hw *hw)
 {
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
-	if (mac->act_scanning == true)
+	if (mac->act_scanning)
 		return;
 
 	if (mac->link_state >= MAC80211_LINKED)
@@ -463,28 +465,29 @@ static void rtl8723ae_dm_ctrl_initgain_by_twoport(struct ieee80211_hw *hw)
 	else
 		dm_digtable->cursta_cstate = DIG_STA_DISCONNECT;
 
-	rtl8723ae_dm_initial_gain_sta(hw);
-	rtl8723ae_dm_initial_gain_multi_sta(hw);
-	rtl8723ae_dm_cck_packet_detection_thresh(hw);
+	rtl8723e_dm_initial_gain_sta(hw);
+	rtl8723e_dm_initial_gain_multi_sta(hw);
+	rtl8723e_dm_cck_packet_detection_thresh(hw);
 
 	dm_digtable->presta_cstate = dm_digtable->cursta_cstate;
 
 }
 
-static void rtl8723ae_dm_dig(struct ieee80211_hw *hw)
+static void rtl8723e_dm_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
-	if (rtlpriv->dm.dm_initialgain_enable == false)
+	if (!rtlpriv->dm.dm_initialgain_enable)
 		return;
-	if (dm_digtable->dig_enable_flag == false)
+	if (!dm_digtable->dig_enable_flag)
 		return;
 
-	rtl8723ae_dm_ctrl_initgain_by_twoport(hw);
+	rtl8723e_dm_ctrl_initgain_by_twoport(hw);
+
 }
 
-static void rtl8723ae_dm_dynamic_txpower(struct ieee80211_hw *hw)
+static void rtl8723e_dm_dynamic_txpower(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -502,7 +505,7 @@ static void rtl8723ae_dm_dynamic_txpower(struct ieee80211_hw *hw)
 	if ((mac->link_state < MAC80211_LINKED) &&
 	    (rtlpriv->dm.entry_min_undec_sm_pwdb == 0)) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
-			 "Not connected\n");
+			 "Not connected to any\n");
 
 		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
 
@@ -512,18 +515,21 @@ static void rtl8723ae_dm_dynamic_txpower(struct ieee80211_hw *hw)
 
 	if (mac->link_state >= MAC80211_LINKED) {
 		if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-			undec_sm_pwdb = rtlpriv->dm.entry_min_undec_sm_pwdb;
+			undec_sm_pwdb =
+			    rtlpriv->dm.entry_min_undec_sm_pwdb;
 			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 				 "AP Client PWDB = 0x%lx\n",
-				 undec_sm_pwdb);
+				  undec_sm_pwdb);
 		} else {
-			undec_sm_pwdb = rtlpriv->dm.undec_sm_pwdb;
+			undec_sm_pwdb =
+			    rtlpriv->dm.undec_sm_pwdb;
 			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 				 "STA Default Port PWDB = 0x%lx\n",
-				 undec_sm_pwdb);
+				  undec_sm_pwdb);
 		}
 	} else {
-		undec_sm_pwdb = rtlpriv->dm.entry_min_undec_sm_pwdb;
+		undec_sm_pwdb =
+		    rtlpriv->dm.entry_min_undec_sm_pwdb;
 
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "AP Ext Port PWDB = 0x%lx\n",
@@ -534,37 +540,39 @@ static void rtl8723ae_dm_dynamic_txpower(struct ieee80211_hw *hw)
 		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_LEVEL1;
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "TXHIGHPWRLEVEL_LEVEL1 (TxPwr=0x0)\n");
-	} else if ((undec_sm_pwdb < (TX_POWER_NEAR_FIELD_THRESH_LVL2 - 3)) &&
-		   (undec_sm_pwdb >= TX_POWER_NEAR_FIELD_THRESH_LVL1)) {
+	} else if ((undec_sm_pwdb <
+		    (TX_POWER_NEAR_FIELD_THRESH_LVL2 - 3)) &&
+		   (undec_sm_pwdb >=
+		    TX_POWER_NEAR_FIELD_THRESH_LVL1)) {
 		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_LEVEL1;
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "TXHIGHPWRLEVEL_LEVEL1 (TxPwr=0x10)\n");
-	} else if (undec_sm_pwdb < (TX_POWER_NEAR_FIELD_THRESH_LVL1 - 5)) {
+	} else if (undec_sm_pwdb <
+		   (TX_POWER_NEAR_FIELD_THRESH_LVL1 - 5)) {
 		rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "TXHIGHPWRLEVEL_NORMAL\n");
 	}
 
-	if ((rtlpriv->dm.dynamic_txhighpower_lvl != rtlpriv->dm.last_dtp_lvl)) {
+	if (rtlpriv->dm.dynamic_txhighpower_lvl != rtlpriv->dm.last_dtp_lvl) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "PHY_SetTxPowerLevel8192S() Channel = %d\n",
 			  rtlphy->current_channel);
-		rtl8723ae_phy_set_txpower_level(hw, rtlphy->current_channel);
+		rtl8723e_phy_set_txpower_level(hw, rtlphy->current_channel);
 	}
 
 	rtlpriv->dm.last_dtp_lvl = rtlpriv->dm.dynamic_txhighpower_lvl;
 }
 
-void rtl8723ae_dm_write_dig(struct ieee80211_hw *hw)
+void rtl8723e_dm_write_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
-		 "cur_igvalue = 0x%x, "
-		 "pre_igvalue = 0x%x, back_val = %d\n",
-		 dm_digtable->cur_igvalue, dm_digtable->pre_igvalue,
-		 dm_digtable->back_val);
+		 "cur_igvalue = 0x%x, pre_igvalue = 0x%x, back_val = %d\n",
+		  dm_digtable->cur_igvalue, dm_digtable->pre_igvalue,
+		  dm_digtable->back_val);
 
 	if (dm_digtable->pre_igvalue != dm_digtable->cur_igvalue) {
 		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f,
@@ -576,32 +584,39 @@ void rtl8723ae_dm_write_dig(struct ieee80211_hw *hw)
 	}
 }
 
-static void rtl8723ae_dm_check_edca_turbo(struct ieee80211_hw *hw)
+static void rtl8723e_dm_pwdb_monitor(struct ieee80211_hw *hw)
+{
+}
+
+static void rtl8723e_dm_check_edca_turbo(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 
+	static u64 last_txok_cnt;
+	static u64 last_rxok_cnt;
+	static u32 last_bt_edca_ul;
+	static u32 last_bt_edca_dl;
 	u64 cur_txok_cnt = 0;
 	u64 cur_rxok_cnt = 0;
 	u32 edca_be_ul = 0x5ea42b;
 	u32 edca_be_dl = 0x5ea42b;
 	bool bt_change_edca = false;
 
-	if ((mac->last_bt_edca_ul != rtlpcipriv->bt_coexist.bt_edca_ul) ||
-	    (mac->last_bt_edca_dl != rtlpcipriv->bt_coexist.bt_edca_dl)) {
+	if ((last_bt_edca_ul != rtlpriv->btcoexist.bt_edca_ul) ||
+	    (last_bt_edca_dl != rtlpriv->btcoexist.bt_edca_dl)) {
 		rtlpriv->dm.current_turbo_edca = false;
-		mac->last_bt_edca_ul = rtlpcipriv->bt_coexist.bt_edca_ul;
-		mac->last_bt_edca_dl = rtlpcipriv->bt_coexist.bt_edca_dl;
+		last_bt_edca_ul = rtlpriv->btcoexist.bt_edca_ul;
+		last_bt_edca_dl = rtlpriv->btcoexist.bt_edca_dl;
 	}
 
-	if (rtlpcipriv->bt_coexist.bt_edca_ul != 0) {
-		edca_be_ul = rtlpcipriv->bt_coexist.bt_edca_ul;
+	if (rtlpriv->btcoexist.bt_edca_ul != 0) {
+		edca_be_ul = rtlpriv->btcoexist.bt_edca_ul;
 		bt_change_edca = true;
 	}
 
-	if (rtlpcipriv->bt_coexist.bt_edca_dl != 0) {
-		edca_be_ul = rtlpcipriv->bt_coexist.bt_edca_dl;
+	if (rtlpriv->btcoexist.bt_edca_dl != 0) {
+		edca_be_ul = rtlpriv->btcoexist.bt_edca_dl;
 		bt_change_edca = true;
 	}
 
@@ -609,22 +624,11 @@ static void rtl8723ae_dm_check_edca_turbo(struct ieee80211_hw *hw)
 		rtlpriv->dm.current_turbo_edca = false;
 		return;
 	}
-
-	if ((!mac->ht_enable) && (!rtlpcipriv->bt_coexist.bt_coexistence)) {
-		if (!(edca_be_ul & 0xffff0000))
-			edca_be_ul |= 0x005e0000;
-
-		if (!(edca_be_dl & 0xffff0000))
-			edca_be_dl |= 0x005e0000;
-	}
-
 	if ((bt_change_edca) || ((!rtlpriv->dm.is_any_nonbepkts) &&
 	     (!rtlpriv->dm.disable_framebursting))) {
 
-		cur_txok_cnt = rtlpriv->stats.txbytesunicast -
-			       mac->last_txok_cnt;
-		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast -
-			       mac->last_rxok_cnt;
+		cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
+		cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
 
 		if (cur_rxok_cnt > 4 * cur_txok_cnt) {
 			if (!rtlpriv->dm.is_cur_rdlstate ||
@@ -647,18 +651,20 @@ static void rtl8723ae_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	} else {
 		if (rtlpriv->dm.current_turbo_edca) {
 			u8 tmp = AC0_BE;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      &tmp);
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_AC_PARAM,
+						      (u8 *)(&tmp));
 			rtlpriv->dm.current_turbo_edca = false;
 		}
 	}
 
 	rtlpriv->dm.is_any_nonbepkts = false;
-	mac->last_txok_cnt = rtlpriv->stats.txbytesunicast;
-	mac->last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
+	last_txok_cnt = rtlpriv->stats.txbytesunicast;
+	last_rxok_cnt = rtlpriv->stats.rxbytesunicast;
 }
 
-static void rtl8723ae_dm_initialize_txpower_tracking(struct ieee80211_hw *hw)
+static void rtl8723e_dm_initialize_txpower_tracking_thermalmeter(
+				struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -667,10 +673,20 @@ static void rtl8723ae_dm_initialize_txpower_tracking(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 		 "pMgntInfo->txpower_tracking = %d\n",
-		 rtlpriv->dm.txpower_tracking);
+		  rtlpriv->dm.txpower_tracking);
 }
 
-void rtl8723ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
+static void rtl8723e_dm_initialize_txpower_tracking(struct ieee80211_hw *hw)
+{
+	rtl8723e_dm_initialize_txpower_tracking_thermalmeter(hw);
+}
+
+void rtl8723e_dm_check_txpower_tracking(struct ieee80211_hw *hw)
+{
+	return;
+}
+
+void rtl8723e_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rate_adaptive *p_ra = &(rtlpriv->ra);
@@ -682,101 +698,32 @@ void rtl8723ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 		rtlpriv->dm.useramask = true;
 	else
 		rtlpriv->dm.useramask = false;
-}
 
-static void rtl8723ae_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
-	u32 low_rssithresh_for_ra, high_rssithresh_for_ra;
-	struct ieee80211_sta *sta = NULL;
-
-	if (is_hal_stop(rtlhal)) {
-		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-			 " driver is going to unload\n");
-		return;
-	}
-
-	if (!rtlpriv->dm.useramask) {
-		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-			 " driver does not control rate adaptive mask\n");
-		return;
-	}
-
-	if (mac->link_state == MAC80211_LINKED &&
-	    mac->opmode == NL80211_IFTYPE_STATION) {
-		switch (p_ra->pre_ratr_state) {
-		case DM_RATR_STA_HIGH:
-			high_rssithresh_for_ra = 50;
-			low_rssithresh_for_ra = 20;
-			break;
-		case DM_RATR_STA_MIDDLE:
-			high_rssithresh_for_ra = 55;
-			low_rssithresh_for_ra = 20;
-			break;
-		case DM_RATR_STA_LOW:
-			high_rssithresh_for_ra = 50;
-			low_rssithresh_for_ra = 25;
-			break;
-		default:
-			high_rssithresh_for_ra = 50;
-			low_rssithresh_for_ra = 20;
-			break;
-		}
-
-		if (rtlpriv->dm.undec_sm_pwdb > high_rssithresh_for_ra)
-			p_ra->ratr_state = DM_RATR_STA_HIGH;
-		else if (rtlpriv->dm.undec_sm_pwdb > low_rssithresh_for_ra)
-			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
-		else
-			p_ra->ratr_state = DM_RATR_STA_LOW;
-
-		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 "RSSI = %ld\n",
-				 rtlpriv->dm.undec_sm_pwdb);
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 "RSSI_LEVEL = %d\n", p_ra->ratr_state);
-			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 "PreState = %d, CurState = %d\n",
-				 p_ra->pre_ratr_state, p_ra->ratr_state);
-
-			rcu_read_lock();
-			sta = rtl_find_sta(hw, mac->bssid);
-			if (sta)
-				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
-							   p_ra->ratr_state);
-			rcu_read_unlock();
-
-			p_ra->pre_ratr_state = p_ra->ratr_state;
-		}
-	}
 }
 
-void rtl8723ae_dm_rf_saving(struct ieee80211_hw *hw, u8 force_in_normal)
+void rtl8723e_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct ps_t *dm_pstable = &rtlpriv->dm_pstable;
+	static u8 initialize;
+	static u32 reg_874, reg_c70, reg_85c, reg_a74;
 
-	if (!rtlpriv->reg_init) {
-		rtlpriv->reg_874 = (rtl_get_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
-				    MASKDWORD) & 0x1CC000) >> 14;
+	if (initialize == 0) {
+		reg_874 = (rtl_get_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
+					 MASKDWORD) & 0x1CC000) >> 14;
 
-		rtlpriv->reg_c70 = (rtl_get_bbreg(hw, ROFDM0_AGCPARAMETER1,
-				    MASKDWORD) & BIT(3)) >> 3;
+		reg_c70 = (rtl_get_bbreg(hw, ROFDM0_AGCPARAMETER1,
+					 MASKDWORD) & BIT(3)) >> 3;
 
-		rtlpriv->reg_85c = (rtl_get_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL,
-				    MASKDWORD) & 0xFF000000) >> 24;
+		reg_85c = (rtl_get_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL,
+					 MASKDWORD) & 0xFF000000) >> 24;
 
-		rtlpriv->reg_a74 = (rtl_get_bbreg(hw, 0xa74, MASKDWORD) &
-				   0xF000) >> 12;
+		reg_a74 = (rtl_get_bbreg(hw, 0xa74, MASKDWORD) & 0xF000) >> 12;
 
-		rtlpriv->reg_init = true;
+		initialize = 1;
 	}
 
-	if (!force_in_normal) {
+	if (!bforce_in_normal) {
 		if (dm_pstable->rssi_val_min != 0) {
 			if (dm_pstable->pre_rfstate == RF_NORMAL) {
 				if (dm_pstable->rssi_val_min >= 30)
@@ -798,7 +745,6 @@ void rtl8723ae_dm_rf_saving(struct ieee80211_hw *hw, u8 force_in_normal)
 
 	if (dm_pstable->pre_rfstate != dm_pstable->cur_rfstate) {
 		if (dm_pstable->cur_rfstate == RF_SAVE) {
-
 			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
 				      BIT(5), 0x1);
 			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
@@ -813,12 +759,12 @@ void rtl8723ae_dm_rf_saving(struct ieee80211_hw *hw, u8 force_in_normal)
 			rtl_set_bbreg(hw, 0x818, BIT(28), 0x1);
 		} else {
 			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
-				      0x1CC000, rtlpriv->reg_874);
+				      0x1CC000, reg_874);
 			rtl_set_bbreg(hw, ROFDM0_AGCPARAMETER1, BIT(3),
-				      rtlpriv->reg_c70);
+				      reg_c70);
 			rtl_set_bbreg(hw, RFPGA0_XCD_SWITCHCONTROL, 0xFF000000,
-				      rtlpriv->reg_85c);
-			rtl_set_bbreg(hw, 0xa74, 0xF000, rtlpriv->reg_a74);
+				      reg_85c);
+			rtl_set_bbreg(hw, 0xa74, 0xF000, reg_a74);
 			rtl_set_bbreg(hw, 0x818, BIT(28), 0x0);
 			rtl_set_bbreg(hw, RFPGA0_XCD_RFINTERFACESW,
 				      BIT(5), 0x0);
@@ -828,7 +774,7 @@ void rtl8723ae_dm_rf_saving(struct ieee80211_hw *hw, u8 force_in_normal)
 	}
 }
 
-static void rtl8723ae_dm_dynamic_bpowersaving(struct ieee80211_hw *hw)
+static void rtl8723e_dm_dynamic_bb_powersaving(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -847,48 +793,49 @@ static void rtl8723ae_dm_dynamic_bpowersaving(struct ieee80211_hw *hw)
 			    rtlpriv->dm.entry_min_undec_sm_pwdb;
 			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
 				 "AP Client PWDB = 0x%lx\n",
-				 dm_pstable->rssi_val_min);
+				  dm_pstable->rssi_val_min);
 		} else {
-			dm_pstable->rssi_val_min = rtlpriv->dm.undec_sm_pwdb;
+			dm_pstable->rssi_val_min =
+			    rtlpriv->dm.undec_sm_pwdb;
 			RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
 				 "STA Default Port PWDB = 0x%lx\n",
-				 dm_pstable->rssi_val_min);
+				  dm_pstable->rssi_val_min);
 		}
 	} else {
-		dm_pstable->rssi_val_min = rtlpriv->dm.entry_min_undec_sm_pwdb;
+		dm_pstable->rssi_val_min =
+		    rtlpriv->dm.entry_min_undec_sm_pwdb;
 
 		RT_TRACE(rtlpriv, DBG_LOUD, DBG_LOUD,
 			 "AP Ext Port PWDB = 0x%lx\n",
-			 dm_pstable->rssi_val_min);
+			  dm_pstable->rssi_val_min);
 	}
 
-	rtl8723ae_dm_rf_saving(hw, false);
+	rtl8723e_dm_rf_saving(hw, false);
 }
 
-void rtl8723ae_dm_init(struct ieee80211_hw *hw)
+void rtl8723e_dm_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	rtl8723ae_dm_diginit(hw);
+	rtl8723e_dm_diginit(hw);
 	rtl8723_dm_init_dynamic_txpower(hw);
 	rtl8723_dm_init_edca_turbo(hw);
-	rtl8723ae_dm_init_rate_adaptive_mask(hw);
-	rtl8723ae_dm_initialize_txpower_tracking(hw);
+	rtl8723e_dm_init_rate_adaptive_mask(hw);
+	rtl8723e_dm_initialize_txpower_tracking(hw);
 	rtl8723_dm_init_dynamic_bb_powersaving(hw);
 }
 
-void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw)
+void rtl8723e_dm_watchdog(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	bool fw_current_inpsmode = false;
 	bool fw_ps_awake = true;
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				      (u8 *) (&fw_current_inpsmode));
+				      (u8 *)(&fw_current_inpsmode));
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
-				      (u8 *) (&fw_ps_awake));
+				      (u8 *)(&fw_ps_awake));
 
 	if (ppsc->p2p_ps_info.p2p_ps_mode)
 		fw_ps_awake = false;
@@ -896,58 +843,57 @@ void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw)
 	if ((ppsc->rfpwr_state == ERFON) &&
 	    ((!fw_current_inpsmode) && fw_ps_awake) &&
 	    (!ppsc->rfchange_inprogress)) {
-		rtl8723ae_dm_dig(hw);
-		rtl8723ae_dm_false_alarm_counter_statistics(hw);
-		rtl8723ae_dm_dynamic_bpowersaving(hw);
-		rtl8723ae_dm_dynamic_txpower(hw);
-		rtl8723ae_dm_refresh_rate_adaptive_mask(hw);
-		rtl8723ae_dm_bt_coexist(hw);
-		rtl8723ae_dm_check_edca_turbo(hw);
+		rtl8723e_dm_pwdb_monitor(hw);
+		rtl8723e_dm_dig(hw);
+		rtl8723e_dm_false_alarm_counter_statistics(hw);
+		rtl8723e_dm_dynamic_bb_powersaving(hw);
+		rtl8723e_dm_dynamic_txpower(hw);
+		rtl8723e_dm_check_txpower_tracking(hw);
+		/* rtl92c_dm_refresh_rate_adaptive_mask(hw); */
+		rtl8723e_dm_bt_coexist(hw);
+		rtl8723e_dm_check_edca_turbo(hw);
 	}
-	if (rtlpcipriv->bt_coexist.init_set)
+	if (rtlpriv->btcoexist.init_set)
 		rtl_write_byte(rtlpriv, 0x76e, 0xc);
 }
 
-static void rtl8723ae_dm_init_bt_coexist(struct ieee80211_hw *hw)
+static void rtl8723e_dm_init_bt_coexist(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 
-	rtlpcipriv->bt_coexist.bt_rfreg_origin_1e
+	rtlpriv->btcoexist.bt_rfreg_origin_1e
 		= rtl_get_rfreg(hw, (enum radio_path)0, RF_RCK1, 0xfffff);
-	rtlpcipriv->bt_coexist.bt_rfreg_origin_1f
+	rtlpriv->btcoexist.bt_rfreg_origin_1f
 		= rtl_get_rfreg(hw, (enum radio_path)0, RF_RCK2, 0xf0);
 
-	rtlpcipriv->bt_coexist.cstate = 0;
-	rtlpcipriv->bt_coexist.previous_state = 0;
-	rtlpcipriv->bt_coexist.cstate_h = 0;
-	rtlpcipriv->bt_coexist.previous_state_h = 0;
-	rtlpcipriv->bt_coexist.lps_counter = 0;
+	rtlpriv->btcoexist.cstate = 0;
+	rtlpriv->btcoexist.previous_state = 0;
+	rtlpriv->btcoexist.cstate_h = 0;
+	rtlpriv->btcoexist.previous_state_h = 0;
+	rtlpriv->btcoexist.lps_counter = 0;
 
 	/*  Enable counter statistics */
 	rtl_write_byte(rtlpriv, 0x76e, 0x4);
 	rtl_write_byte(rtlpriv, 0x778, 0x3);
 	rtl_write_byte(rtlpriv, 0x40, 0x20);
 
-	rtlpcipriv->bt_coexist.init_set = true;
+	rtlpriv->btcoexist.init_set = true;
 }
 
-void rtl8723ae_dm_bt_coexist(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_coexist(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	u8 tmp_byte = 0;
-	if (!rtlpcipriv->bt_coexist.bt_coexistence) {
+	if (!rtlpriv->btcoexist.bt_coexistence) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
 			 "[DM]{BT], BT not exist!!\n");
 		return;
 	}
 
-	if (!rtlpcipriv->bt_coexist.init_set) {
+	if (!rtlpriv->btcoexist.init_set) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-			 "[DM][BT], rtl8723ae_dm_bt_coexist()\n");
-
-		rtl8723ae_dm_init_bt_coexist(hw);
+			 "[DM][BT], rtl8723e_dm_bt_coexist()\n");
+		rtl8723e_dm_init_bt_coexist(hw);
 	}
 
 	tmp_byte = rtl_read_byte(rtlpriv, 0x40);
@@ -955,5 +901,5 @@ void rtl8723ae_dm_bt_coexist(struct ieee80211_hw *hw)
 		 "[DM][BT], 0x40 is 0x%x", tmp_byte);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 		 "[DM][BT], bt_dm_coexist start");
-	rtl8723ae_dm_bt_coexist_8723(hw);
+	rtl8723e_dm_bt_coexist_8723(hw);
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
index d253bb53d03e..6fa0feb05f6d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/dm.h
@@ -25,17 +25,23 @@
  *
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
- ****************************************************************************
- */
+ *****************************************************************************/
 
 #ifndef	__RTL8723E_DM_H__
 #define __RTL8723E_DM_H__
 
+#define HAL_DM_DIG_DISABLE			BIT(0)
 #define HAL_DM_HIPWR_DISABLE			BIT(1)
 
+#define OFDM_TABLE_LENGTH			37
+#define CCK_TABLE_LENGTH			33
+
 #define OFDM_TABLE_SIZE				37
 #define CCK_TABLE_SIZE				33
 
+#define BW_AUTO_SWITCH_HIGH_LOW			25
+#define BW_AUTO_SWITCH_LOW_HIGH			30
+
 #define DM_DIG_THRESH_HIGH			40
 #define DM_DIG_THRESH_LOW			35
 
@@ -63,12 +69,18 @@
 #define DM_RATR_STA_MIDDLE			2
 #define DM_RATR_STA_LOW				3
 
+#define CTS2SELF_THVAL				30
+#define REGC38_TH				20
+
+#define WAIOTTHVAL				25
+
 #define TXHIGHPWRLEVEL_NORMAL			0
 #define TXHIGHPWRLEVEL_LEVEL1			1
 #define TXHIGHPWRLEVEL_LEVEL2			2
 #define TXHIGHPWRLEVEL_BT1			3
 #define TXHIGHPWRLEVEL_BT2			4
 
+#define DM_TYPE_BYFW				0
 #define DM_TYPE_BYDRIVER			1
 
 #define TX_POWER_NEAR_FIELD_THRESH_LVL2		74
@@ -82,6 +94,7 @@ struct swat_t {
 	long trying_threshold;
 	u8 cur_antenna;
 	u8 pre_antenna;
+
 };
 
 enum tag_dynamic_init_gain_operation_type_definition {
@@ -98,7 +111,7 @@ enum tag_dynamic_init_gain_operation_type_definition {
 enum tag_cck_packet_detection_threshold_type_definition {
 	CCK_PD_STAGE_LowRssi = 0,
 	CCK_PD_STAGE_HighRssi = 1,
-	CCK_FA_STAGE_Low = 2,
+	CCK_FA_STAGE_LOW = 2,
 	CCK_FA_STAGE_High = 3,
 	CCK_PD_STAGE_MAX = 4,
 };
@@ -138,17 +151,24 @@ enum dm_dig_connect_e {
 	DIG_CONNECT_MAX
 };
 
+#define BT_RSSI_STATE_NORMAL_POWER      BIT_OFFSET_LEN_MASK_32(0, 1)
+#define BT_RSSI_STATE_AMDPU_OFF         BIT_OFFSET_LEN_MASK_32(1, 1)
+#define BT_RSSI_STATE_SPECIAL_LOW       BIT_OFFSET_LEN_MASK_32(2, 1)
+#define BT_RSSI_STATE_BG_EDCA_LOW       BIT_OFFSET_LEN_MASK_32(3, 1)
+#define BT_RSSI_STATE_TXPOWER_LOW       BIT_OFFSET_LEN_MASK_32(4, 1)
 #define GET_UNDECORATED_AVERAGE_RSSI(_priv)     \
-	((((struct rtl_priv *)(_priv))->mac80211.opmode ==	\
-	NL80211_IFTYPE_ADHOC) ?  \
-	(((struct rtl_priv *)(_priv))->dm.entry_min_undec_sm_pwdb) \
-	: (((struct rtl_priv *)(_priv))->dm.undec_sm_pwdb))
-
-void rtl8723ae_dm_init(struct ieee80211_hw *hw);
-void rtl8723ae_dm_watchdog(struct ieee80211_hw *hw);
-void rtl8723ae_dm_write_dig(struct ieee80211_hw *hw);
-void rtl8723ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
-void rtl8723ae_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal);
-void rtl8723ae_dm_bt_coexist(struct ieee80211_hw *hw);
-
+	( \
+	(((struct rtl_priv *)(_priv))->mac80211.opmode ==		\
+			     NL80211_IFTYPE_ADHOC) ?			\
+	(((struct rtl_priv *)(_priv))->dm.entry_min_undec_sm_pwdb) :	\
+	(((struct rtl_priv *)(_priv))->dm.undec_sm_pwdb)		\
+	)
+
+void rtl8723e_dm_init(struct ieee80211_hw *hw);
+void rtl8723e_dm_watchdog(struct ieee80211_hw *hw);
+void rtl8723e_dm_write_dig(struct ieee80211_hw *hw);
+void rtl8723e_dm_check_txpower_tracking(struct ieee80211_hw *hw);
+void rtl8723e_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
+void rtl8723e_dm_rf_saving(struct ieee80211_hw *hw, u8 bforce_in_normal);
+void rtl8723e_dm_bt_coexist(struct ieee80211_hw *hw);
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
index 728b7563ad36..b7c0d38ee5b5 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -25,18 +21,19 @@
  *
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
- ****************************************************************************
- */
+ *****************************************************************************/
 
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "fw.h"
 #include "../rtl8723com/fw_common.h"
 
-static bool rtl8723ae_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
+static bool _rtl8723e_check_fw_read_last_h2c(struct ieee80211_hw *hw,
+					     u8 boxnum)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 val_hmetfr, val_mcutst_1;
@@ -50,17 +47,17 @@ static bool rtl8723ae_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
 	return result;
 }
 
-static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
-					u8 element_id, u32 cmd_len,
-					u8 *p_cmdbuffer)
+static void _rtl8723e_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
+				       u32 cmd_len, u8 *cmdbuffer)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 boxnum;
 	u16 box_reg = 0, box_extreg = 0;
-	u8 u1tmp;
-	bool isfw_rd = false;
-	bool bwrite_success = false;
+	u8 u1b_tmp;
+	bool isfw_read = false;
+	u8 buf_index = 0;
+	bool bwrite_sucess = false;
 	u8 wait_h2c_limmit = 100;
 	u8 wait_writeh2c_limmit = 100;
 	u8 boxcontent[4], boxextcontent[2];
@@ -83,7 +80,7 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 				h2c_waitcounter++;
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 					 "Wait 100 us (%d times)...\n",
-					 h2c_waitcounter);
+					  h2c_waitcounter);
 				udelay(100);
 
 				if (h2c_waitcounter > 1000)
@@ -99,12 +96,11 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 		}
 	}
 
-	while (!bwrite_success) {
+	while (!bwrite_sucess) {
 		wait_writeh2c_limmit--;
 		if (wait_writeh2c_limmit == 0) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "Write H2C fail because no trigger "
-				 "for FW INT!\n");
+				 "Write H2C fail because no trigger for FW INT!\n");
 			break;
 		}
 
@@ -128,34 +124,35 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		default:
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+				 "switch case not process\n");
 			break;
 		}
 
-		isfw_rd = rtl8723ae_check_fw_read_last_h2c(hw, boxnum);
-		while (!isfw_rd) {
+		isfw_read = _rtl8723e_check_fw_read_last_h2c(hw, boxnum);
+		while (!isfw_read) {
 
 			wait_h2c_limmit--;
 			if (wait_h2c_limmit == 0) {
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-					 "Waiting too long for FW read clear HMEBox(%d)!\n",
+					 "Wating too long for FW read clear HMEBox(%d)!\n",
 					 boxnum);
 				break;
 			}
 
 			udelay(10);
 
-			isfw_rd = rtl8723ae_check_fw_read_last_h2c(hw, boxnum);
-			u1tmp = rtl_read_byte(rtlpriv, 0x1BF);
+			isfw_read = _rtl8723e_check_fw_read_last_h2c(hw,
+								boxnum);
+			u1b_tmp = rtl_read_byte(rtlpriv, 0x1BF);
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Waiting for FW read clear HMEBox(%d)!!! "
-				 "0x1BF = %2x\n", boxnum, u1tmp);
+				 "Waiting for FW read clear HMEBox(%d)!!! 0x1BF = %2x\n",
+				 boxnum, u1b_tmp);
 		}
 
-		if (!isfw_rd) {
+		if (!isfw_read) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Write H2C register BOX[%d] fail!!!!! "
-				 "Fw do not read.\n", boxnum);
+				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
+				 boxnum);
 			break;
 		}
 
@@ -169,8 +166,8 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 		switch (cmd_len) {
 		case 1:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 1);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 1);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -179,8 +176,8 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 2:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 2);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -189,8 +186,8 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 3:
 			boxcontent[0] &= ~(BIT(7));
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer, 3);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 3);
 
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
@@ -199,10 +196,10 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 4:
 			boxcontent[0] |= (BIT(7));
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer, 2);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + 2, 2);
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index + 2, 2);
 
 			for (idx = 0; idx < 2; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
@@ -216,10 +213,10 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		case 5:
 			boxcontent[0] |= (BIT(7));
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer, 2);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + 2, 3);
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index, 2);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index + 2, 3);
 
 			for (idx = 0; idx < 2; idx++) {
 				rtl_write_byte(rtlpriv, box_extreg + idx,
@@ -237,7 +234,7 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 			break;
 		}
 
-		bwrite_success = true;
+		bwrite_sucess = true;
 
 		rtlhal->last_hmeboxnum = boxnum + 1;
 		if (rtlhal->last_hmeboxnum == 4)
@@ -245,7 +242,7 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "pHalData->last_hmeboxnum  = %d\n",
-			 rtlhal->last_hmeboxnum);
+			  rtlhal->last_hmeboxnum);
 	}
 
 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
@@ -255,52 +252,49 @@ static void _rtl8723ae_fill_h2c_command(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
 }
 
-void rtl8723ae_fill_h2c_cmd(struct ieee80211_hw *hw,
-			    u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
+void rtl8723e_fill_h2c_cmd(struct ieee80211_hw *hw,
+			   u8 element_id, u32 cmd_len, u8 *cmdbuffer)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 tmp_cmdbuf[2];
 
-	if (rtlhal->fw_ready == false) {
+	if (!rtlhal->fw_ready) {
 		RT_ASSERT(false,
-			 "return H2C cmd because of Fw download fail!!!\n");
+			  "return H2C cmd because of Fw download fail!!!\n");
 		return;
 	}
-
-	_rtl8723ae_fill_h2c_command(hw, element_id, cmd_len, p_cmdbuffer);
-	return;
+	memset(tmp_cmdbuf, 0, 8);
+	memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
+	_rtl8723e_fill_h2c_command(hw, element_id, cmd_len,
+				   (u8 *)&tmp_cmdbuf);
 }
 
-static bool _rtl8723ae_cmd_send_packet(struct ieee80211_hw *hw,
-				       struct sk_buff *skb)
+void rtl8723e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	unsigned long flags;
-	struct sk_buff *pskb = NULL;
-
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-
-	pdesc = &ring->desc[0];
+	u8 u1_h2c_set_pwrmode[3] = { 0 };
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 
-	__skb_queue_tail(&ring->queue, skb);
+	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
+		(rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
+	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
+					      ppsc->reg_max_lps_awakeintvl);
 
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl8723e_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
+		      u1_h2c_set_pwrmode, 3);
+	rtl8723e_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
+}
 
-	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
+#define BEACON_PG		0 /* ->1 */
+#define PSPOLL_PG		2
+#define NULL_PG			3
+#define PROBERSP_PG		4 /* ->5 */
 
-	return true;
-}
+#define TOTAL_RESERVED_PKT_LEN	768
 
 static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 	/* page 0 beacon */
@@ -412,111 +406,111 @@ static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 };
 
-void rtl8723ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool dl_finished)
+void rtl8723e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct sk_buff *skb = NULL;
-
 	u32 totalpacketlen;
 	bool rtstatus;
-	u8 u1RsvdPageLoc[3] = { 0 };
-	bool dlok = false;
-
+	u8 u1rsvdpageloc[3] = { 0 };
+	bool b_dlok = false;
 	u8 *beacon;
 	u8 *p_pspoll;
 	u8 *nullfunc;
 	u8 *p_probersp;
+
 	/*---------------------------------------------------------
-				(1) beacon
-	---------------------------------------------------------
-	*/
+	 *			(1) beacon
+	 *---------------------------------------------------------
+	 */
 	beacon = &reserved_page_packet[BEACON_PG * 128];
 	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
 
 	/*-------------------------------------------------------
-				(2) ps-poll
-	--------------------------------------------------------
-	*/
+	 *			(2) ps-poll
+	 *--------------------------------------------------------
+	 */
 	p_pspoll = &reserved_page_packet[PSPOLL_PG * 128];
 	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
 	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
 	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1rsvdpageloc, PSPOLL_PG);
 
 	/*--------------------------------------------------------
-				(3) null data
-	---------------------------------------------------------i
-	*/
+	 *			(3) null data
+	 *---------------------------------------------------------
+	 */
 	nullfunc = &reserved_page_packet[NULL_PG * 128];
 	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
 	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1rsvdpageloc, NULL_PG);
 
 	/*---------------------------------------------------------
-				(4) probe response
-	----------------------------------------------------------
-	*/
+	 *			(4) probe response
+	 *----------------------------------------------------------
+	 */
 	p_probersp = &reserved_page_packet[PROBERSP_PG * 128];
 	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
 	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
 	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
 
-	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG);
+	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1rsvdpageloc, PROBERSP_PG);
 
 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
-		      "rtl8723ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      "rtl8723e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 		      &reserved_page_packet[0], totalpacketlen);
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl8723ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
-		      u1RsvdPageLoc, 3);
+		      "rtl8723e_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      u1rsvdpageloc, 3);
 
 	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *) skb_put(skb, totalpacketlen),
+	memcpy((u8 *)skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
-	rtstatus = _rtl8723ae_cmd_send_packet(hw, skb);
+	rtstatus = rtl_cmd_send_packet(hw, skb);
 
 	if (rtstatus)
-		dlok = true;
+		b_dlok = true;
 
-	if (dlok) {
+	if (b_dlok) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "Set RSVD page location to Fw.\n");
 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-				"H2C_RSVDPAGE:\n",
-				u1RsvdPageLoc, 3);
-		rtl8723ae_fill_h2c_cmd(hw, H2C_RSVDPAGE,
-				       sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+			      "H2C_RSVDPAGE:\n",
+			      u1rsvdpageloc, 3);
+		rtl8723e_fill_h2c_cmd(hw, H2C_RSVDPAGE,
+				      sizeof(u1rsvdpageloc), u1rsvdpageloc);
 	} else
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
 }
 
-void rtl8723ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
+void rtl8723e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
 {
 	u8 u1_joinbssrpt_parm[1] = { 0 };
 
 	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
 
-	rtl8723ae_fill_h2c_cmd(hw, H2C_JOINBSSRPT, 1, u1_joinbssrpt_parm);
+	rtl8723e_fill_h2c_cmd(hw, H2C_JOINBSSRPT, 1, u1_joinbssrpt_parm);
 }
 
 static void rtl8723e_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
 					    u8 ctwindow)
 {
-	u8 u1_ctwindow_period[1] = {ctwindow};
+	u8 u1_ctwindow_period[1] = { ctwindow};
+
+	rtl8723e_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
 
-	rtl8723ae_fill_h2c_cmd(hw, H2C_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
 }
 
-void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+void rtl8723e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
@@ -530,7 +524,7 @@ void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 	switch (p2p_ps_state) {
 	case P2P_PS_DISABLE:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
-		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
+		memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
 		break;
 	case P2P_PS_ENABLE:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
@@ -542,7 +536,7 @@ void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 		}
 
 		/* hw only support 2 set of NoA */
-		for (i = 0; i < p2pinfo->noa_num; i++) {
+		for (i = 0 ; i < p2pinfo->noa_num ; i++) {
 			/* To control the register setting for which NOA*/
 			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
 			if (i == 0)
@@ -561,27 +555,33 @@ void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 
 			start_time = p2pinfo->noa_start_time[i];
 			if (p2pinfo->noa_count_type[i] != 1) {
-				while (start_time <= (tsf_low+(50*1024))) {
-					start_time += p2pinfo->noa_interval[i];
+				while (start_time <=
+					(tsf_low+(50*1024))) {
+					start_time +=
+						p2pinfo->noa_interval[i];
 					if (p2pinfo->noa_count_type[i] != 255)
 						p2pinfo->noa_count_type[i]--;
 				}
 			}
 			rtl_write_dword(rtlpriv, 0x5E8, start_time);
 			rtl_write_dword(rtlpriv, 0x5EC,
-					p2pinfo->noa_count_type[i]);
+				p2pinfo->noa_count_type[i]);
+
 		}
+
 		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
 			/* rst p2p circuit */
 			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
 
 			p2p_ps_offload->offload_en = 1;
+
 			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
 				p2p_ps_offload->role = 1;
 				p2p_ps_offload->allstasleep = 0;
 			} else {
 				p2p_ps_offload->role = 0;
 			}
+
 			p2p_ps_offload->discovery = 0;
 		}
 		break;
@@ -597,26 +597,7 @@ void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
 	default:
 		break;
 	}
-	rtl8723ae_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
-}
-
-void rtl8723ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 u1_h2c_set_pwrmode[3] = { 0 };
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 
-	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, mode);
-	SET_H2CCMD_PWRMODE_PARM_SMART_PS_23A(u1_h2c_set_pwrmode,
-					     (rtlpriv->mac80211.p2p) ?
-					     ppsc->smart_ps : 1);
-	SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(u1_h2c_set_pwrmode,
-					      ppsc->reg_max_lps_awakeintvl);
+	rtl8723e_fill_h2c_cmd(hw, H2C_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
 
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl8723ae_set_fw_rsvdpagepkt(): u1_h2c_set_pwrmode\n",
-		      u1_h2c_set_pwrmode, 3);
-	rtl8723ae_fill_h2c_cmd(hw, H2C_SETPWRMODE, 3, u1_h2c_set_pwrmode);
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
index d355b85dd9fe..9d1fe25db953 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/fw.h
@@ -24,50 +24,27 @@
  * Hsinchu 300, Taiwan.
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
- ****************************************************************************
- */
+ *****************************************************************************/
 
 #ifndef __RTL92C__FW__H__
 #define __RTL92C__FW__H__
 
+#define FW_8192C_SIZE					0x3000
 #define FW_8192C_START_ADDRESS			0x1000
 #define FW_8192C_END_ADDRESS			0x3FFF
-#define FW_8192C_PAGE_SIZE			4096
+#define FW_8192C_PAGE_SIZE				4096
 #define FW_8192C_POLLING_DELAY			5
-#define FW_8192C_POLLING_TIMEOUT_COUNT		6000
 
-#define BEACON_PG				0
-#define PSPOLL_PG				2
-#define NULL_PG					3
-#define PROBERSP_PG				4 /* ->5 */
+#define IS_FW_HEADER_EXIST(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFFF) == 0x2300 ||\
+	(_pfwhdr->signature&0xFFFF) == 0x2301 ||\
+	(_pfwhdr->signature&0xFFFF) == 0x2302)
 
-#define TOTAL_RESERVED_PKT_LEN			768
-
-#define IS_FW_HEADER_EXIST(_pfwhdr)		\
-	((_pfwhdr->signature&0xFF00) == 0x2300)
-
-struct rtl8723ae_firmware_header {
-	u16 signature;
-	u8 category;
-	u8 function;
-	u16 version;
-	u8 subversion;
-	u8 rsvd1;
-	u8 month;
-	u8 date;
-	u8 hour;
-	u8 minute;
-	u16 ramcodeSize;
-	u16 rsvd2;
-	u32 svnindex;
-	u32 rsvd3;
-	u32 rsvd4;
-	u32 rsvd5;
-};
+#define pagenum_128(_len)	(u32)(((_len)>>7) + ((_len)&0x7F ? 1 : 0))
 
 #define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
 	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_H2CCMD_PWRMODE_PARM_SMART_PS_23A(__ph2ccmd, __val)		\
+#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
 #define SET_H2CCMD_PWRMODE_PARM_BCN_PASS_TIME(__ph2ccmd, __val)	\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
@@ -80,11 +57,10 @@ struct rtl8723ae_firmware_header {
 #define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
 
-void rtl8723ae_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
-			    u32 cmd_len, u8 *p_cmdbuffer);
-void rtl8723ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
-void rtl8723ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
-void rtl8723ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
-void rtl8723ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
-
+void rtl8723e_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
+			   u32 cmd_len, u8 *p_cmdbuffer);
+void rtl8723e_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl8723e_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
+void rtl8723e_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
+void rtl8723e_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c
index 5b4a714f3c8c..5aac45d5a974 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -31,96 +27,102 @@
 #include "../pci.h"
 #include "dm.h"
 #include "fw.h"
-#include "../rtl8723com/fw_common.h"
 #include "phy.h"
 #include "reg.h"
 #include "hal_btc.h"
 
-void rtl8723ae_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw,
-						 bool reject)
+static bool bt_operation_on;
+
+void rtl8723e_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw,
+						bool b_reject)
 {
 }
 
 void _rtl8723_dm_bt_check_wifi_state(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 
 	if (rtlpriv->link_info.busytraffic) {
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_WIFI_IDLE;
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_IDLE;
 
 		if (rtlpriv->link_info.tx_busy_traffic)
-			rtlpcipriv->bt_coexist.cstate |=
-					BT_COEX_STATE_WIFI_UPLINK;
+			rtlpriv->btcoexist.cstate |=
+				BT_COEX_STATE_WIFI_UPLINK;
 		else
-			rtlpcipriv->bt_coexist.cstate &=
-					~BT_COEX_STATE_WIFI_UPLINK;
+			rtlpriv->btcoexist.cstate &=
+				~BT_COEX_STATE_WIFI_UPLINK;
 
 		if (rtlpriv->link_info.rx_busy_traffic)
-			rtlpcipriv->bt_coexist.cstate |=
-					BT_COEX_STATE_WIFI_DOWNLINK;
+			rtlpriv->btcoexist.cstate |=
+				BT_COEX_STATE_WIFI_DOWNLINK;
 		else
-			rtlpcipriv->bt_coexist.cstate &=
-					~BT_COEX_STATE_WIFI_DOWNLINK;
+			rtlpriv->btcoexist.cstate &=
+				~BT_COEX_STATE_WIFI_DOWNLINK;
 	} else {
-		rtlpcipriv->bt_coexist.cstate |= BT_COEX_STATE_WIFI_IDLE;
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_WIFI_UPLINK;
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_WIFI_DOWNLINK;
+		rtlpriv->btcoexist.cstate |= BT_COEX_STATE_WIFI_IDLE;
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_UPLINK;
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_DOWNLINK;
 	}
 
 	if (rtlpriv->mac80211.mode == WIRELESS_MODE_G ||
 	    rtlpriv->mac80211.mode == WIRELESS_MODE_B) {
-		rtlpcipriv->bt_coexist.cstate |= BT_COEX_STATE_WIFI_LEGACY;
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_WIFI_HT20;
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_WIFI_HT40;
+		rtlpriv->btcoexist.cstate |=
+			BT_COEX_STATE_WIFI_LEGACY;
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_HT20;
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_HT40;
 	} else {
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_WIFI_LEGACY;
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_LEGACY;
 		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-			rtlpcipriv->bt_coexist.cstate |=
-					BT_COEX_STATE_WIFI_HT40;
-			rtlpcipriv->bt_coexist.cstate &=
-					~BT_COEX_STATE_WIFI_HT20;
+			rtlpriv->btcoexist.cstate |=
+				BT_COEX_STATE_WIFI_HT40;
+			rtlpriv->btcoexist.cstate &=
+				~BT_COEX_STATE_WIFI_HT20;
 		} else {
-			rtlpcipriv->bt_coexist.cstate |=
-					BT_COEX_STATE_WIFI_HT20;
-			rtlpcipriv->bt_coexist.cstate &=
-					~BT_COEX_STATE_WIFI_HT40;
+			rtlpriv->btcoexist.cstate |=
+				BT_COEX_STATE_WIFI_HT20;
+			rtlpriv->btcoexist.cstate &=
+				~BT_COEX_STATE_WIFI_HT40;
 		}
 	}
 
-	if (rtlpriv->bt_operation_on)
-		rtlpcipriv->bt_coexist.cstate |= BT_COEX_STATE_BT30;
+	if (bt_operation_on)
+		rtlpriv->btcoexist.cstate |= BT_COEX_STATE_BT30;
 	else
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_BT30;
+		rtlpriv->btcoexist.cstate &= ~BT_COEX_STATE_BT30;
 }
 
-u8 rtl8723ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
-					  u8 level_num, u8 rssi_thresh,
-					  u8 rssi_thresh1)
+u8 rtl8723e_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
+					 u8 level_num, u8 rssi_thresh,
+					 u8 rssi_thresh1)
 
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long smooth;
+	long undecoratedsmoothed_pwdb;
 	u8 bt_rssi_state = 0;
 
-	smooth =  rtl8723ae_dm_bt_get_rx_ss(hw);
+	undecoratedsmoothed_pwdb = rtl8723e_dm_bt_get_rx_ss(hw);
 
 	if (level_num == 2) {
-		rtlpcipriv->bt_coexist.cstate &=
-				~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-
-		if ((rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_LOW) ||
-		    (rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_STAY_LOW)) {
-			if (smooth >= (rssi_thresh +
-			    BT_FW_COEX_THRESH_TOL)) {
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+
+		if ((rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_LOW) ||
+		    (rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_STAY_LOW)) {
+			if (undecoratedsmoothed_pwdb >=
+			    (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
 				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->bt_coexist.cstate |=
+				rtlpriv->btcoexist.cstate |=
 					BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->bt_coexist.cstate &=
+				rtlpriv->btcoexist.cstate &=
 					~BT_COEX_STATE_WIFI_RSSI_1_LOW;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI_1 state switch to High\n");
@@ -130,12 +132,12 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
 					 "[DM][BT], RSSI_1 state stay at Low\n");
 			}
 		} else {
-			if (smooth < rssi_thresh) {
+			if (undecoratedsmoothed_pwdb < rssi_thresh) {
 				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->bt_coexist.cstate |=
-					 BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->bt_coexist.cstate &=
-					 ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				rtlpriv->btcoexist.cstate |=
+					BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				rtlpriv->btcoexist.cstate &=
+					~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI_1 state switch to Low\n");
 			} else {
@@ -148,22 +150,22 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
 		if (rssi_thresh > rssi_thresh1) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 				 "[DM][BT], RSSI_1 thresh error!!\n");
-			return rtlpcipriv->bt_coexist.bt_pre_rssi_state;
+			return rtlpriv->btcoexist.bt_pre_rssi_state;
 		}
 
-		if ((rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_LOW) ||
-		    (rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_STAY_LOW)) {
-			if (smooth >=
+		if ((rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_LOW) ||
+		    (rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_STAY_LOW)) {
+			if (undecoratedsmoothed_pwdb >=
 			    (rssi_thresh+BT_FW_COEX_THRESH_TOL)) {
 				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate |=
-					 BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate &=
-					 ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->bt_coexist.cstate &=
-					 ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				rtlpriv->btcoexist.cstate |=
+					BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+				rtlpriv->btcoexist.cstate &=
+					~BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				rtlpriv->btcoexist.cstate &=
+					~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI_1 state switch to Medium\n");
 			} else {
@@ -171,28 +173,28 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI_1 state stay at Low\n");
 			}
-		} else if ((rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-			   BT_RSSI_STATE_MEDIUM) ||
-			   (rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-			   BT_RSSI_STATE_STAY_MEDIUM)) {
-			if (smooth >= (rssi_thresh1 +
-			    BT_FW_COEX_THRESH_TOL)) {
+		} else if ((rtlpriv->btcoexist.bt_pre_rssi_state ==
+			    BT_RSSI_STATE_MEDIUM) ||
+			   (rtlpriv->btcoexist.bt_pre_rssi_state ==
+			    BT_RSSI_STATE_STAY_MEDIUM)) {
+			if (undecoratedsmoothed_pwdb >=
+			    (rssi_thresh1 + BT_FW_COEX_THRESH_TOL)) {
 				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->bt_coexist.cstate |=
-					 BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->bt_coexist.cstate &=
-					 ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->bt_coexist.cstate &=
-					 ~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
+				rtlpriv->btcoexist.cstate |=
+					BT_COEX_STATE_WIFI_RSSI_1_HIGH;
+				rtlpriv->btcoexist.cstate &=
+					~BT_COEX_STATE_WIFI_RSSI_1_LOW;
+				rtlpriv->btcoexist.cstate &=
+					~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI_1 state switch to High\n");
-			} else if (smooth < rssi_thresh) {
+			} else if (undecoratedsmoothed_pwdb < rssi_thresh) {
 				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->bt_coexist.cstate |=
+				rtlpriv->btcoexist.cstate |=
 					BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->bt_coexist.cstate &=
+				rtlpriv->btcoexist.cstate &=
 					~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->bt_coexist.cstate &=
+				rtlpriv->btcoexist.cstate &=
 					~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI_1 state switch to Low\n");
@@ -202,13 +204,13 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
 					 "[DM][BT], RSSI_1 state stay at Medium\n");
 			}
 		} else {
-			if (smooth < rssi_thresh1) {
+			if (undecoratedsmoothed_pwdb < rssi_thresh1) {
 				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate |=
+				rtlpriv->btcoexist.cstate |=
 					BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate &=
+				rtlpriv->btcoexist.cstate &=
 					~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->bt_coexist.cstate &=
+				rtlpriv->btcoexist.cstate &=
 					~BT_COEX_STATE_WIFI_RSSI_1_LOW;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI_1 state switch to Medium\n");
@@ -219,38 +221,37 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
 			}
 		}
 	}
-
-	rtlpcipriv->bt_coexist.bt_pre_rssi_state1 = bt_rssi_state;
+	rtlpriv->btcoexist.bt_pre_rssi_state1 = bt_rssi_state;
 
 	return bt_rssi_state;
 }
 
-u8 rtl8723ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
-					 u8 level_num, u8 rssi_thresh,
-					 u8 rssi_thresh1)
+u8 rtl8723e_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
+					u8 level_num,
+					u8 rssi_thresh,
+					u8 rssi_thresh1)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long smooth;
+	long undecoratedsmoothed_pwdb = 0;
 	u8 bt_rssi_state = 0;
 
-	smooth = rtl8723ae_dm_bt_get_rx_ss(hw);
+	undecoratedsmoothed_pwdb = rtl8723e_dm_bt_get_rx_ss(hw);
 
 	if (level_num == 2) {
-		rtlpcipriv->bt_coexist.cstate &=
-					 ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-
-		if ((rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_LOW) ||
-		    (rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_STAY_LOW)){
-			if (smooth >=
+		rtlpriv->btcoexist.cstate &=
+			~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
+
+		if ((rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_LOW) ||
+		    (rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_STAY_LOW)) {
+			if (undecoratedsmoothed_pwdb >=
 			    (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
 				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->bt_coexist.cstate |=
-					BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->bt_coexist.cstate &=
-					~BT_COEX_STATE_WIFI_RSSI_LOW;
+				rtlpriv->btcoexist.cstate
+					|= BT_COEX_STATE_WIFI_RSSI_HIGH;
+				rtlpriv->btcoexist.cstate
+					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI state switch to High\n");
 			} else {
@@ -259,12 +260,12 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
 					 "[DM][BT], RSSI state stay at Low\n");
 			}
 		} else {
-			if (smooth < rssi_thresh) {
+			if (undecoratedsmoothed_pwdb < rssi_thresh) {
 				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->bt_coexist.cstate |=
-					BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->bt_coexist.cstate &=
-					~BT_COEX_STATE_WIFI_RSSI_HIGH;
+				rtlpriv->btcoexist.cstate
+					|= BT_COEX_STATE_WIFI_RSSI_LOW;
+				rtlpriv->btcoexist.cstate
+					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI state switch to Low\n");
 			} else {
@@ -277,20 +278,20 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
 		if (rssi_thresh > rssi_thresh1) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 				 "[DM][BT], RSSI thresh error!!\n");
-			return rtlpcipriv->bt_coexist.bt_pre_rssi_state;
+			return rtlpriv->btcoexist.bt_pre_rssi_state;
 		}
-		if ((rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_LOW) ||
-		    (rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-		    BT_RSSI_STATE_STAY_LOW)) {
-			if (smooth >=
+		if ((rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_LOW) ||
+		    (rtlpriv->btcoexist.bt_pre_rssi_state ==
+		     BT_RSSI_STATE_STAY_LOW)) {
+			if (undecoratedsmoothed_pwdb >=
 			    (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
 				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					|= BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI state switch to Medium\n");
@@ -299,28 +300,28 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI state stay at Low\n");
 			}
-		} else if ((rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-			   BT_RSSI_STATE_MEDIUM) ||
-			   (rtlpcipriv->bt_coexist.bt_pre_rssi_state ==
-			   BT_RSSI_STATE_STAY_MEDIUM)) {
-			if (smooth >=
+		} else if ((rtlpriv->btcoexist.bt_pre_rssi_state ==
+				BT_RSSI_STATE_MEDIUM) ||
+			(rtlpriv->btcoexist.bt_pre_rssi_state ==
+				BT_RSSI_STATE_STAY_MEDIUM)) {
+			if (undecoratedsmoothed_pwdb >=
 			    (rssi_thresh1 + BT_FW_COEX_THRESH_TOL)) {
 				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					|= BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI state switch to High\n");
-			} else if (smooth < rssi_thresh) {
+			} else if (undecoratedsmoothed_pwdb < rssi_thresh) {
 				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					|= BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI state switch to Low\n");
@@ -330,13 +331,13 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
 					 "[DM][BT], RSSI state stay at Medium\n");
 			}
 		} else {
-			if (smooth < rssi_thresh1) {
+			if (undecoratedsmoothed_pwdb < rssi_thresh1) {
 				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					|= BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->bt_coexist.cstate
+				rtlpriv->btcoexist.cstate
 					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 					 "[DM][BT], RSSI state switch to Medium\n");
@@ -347,31 +348,32 @@ u8 rtl8723ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
 			}
 		}
 	}
-
-	rtlpcipriv->bt_coexist.bt_pre_rssi_state = bt_rssi_state;
+	rtlpriv->btcoexist.bt_pre_rssi_state = bt_rssi_state;
 	return bt_rssi_state;
 }
 
-long rtl8723ae_dm_bt_get_rx_ss(struct ieee80211_hw *hw)
+long rtl8723e_dm_bt_get_rx_ss(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long smooth = 0;
-
-	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
-		smooth = GET_UNDECORATED_AVERAGE_RSSI(rtlpriv);
-	else
-		smooth = rtlpriv->dm.entry_min_undec_sm_pwdb;
+	long undecoratedsmoothed_pwdb = 0;
 
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		undecoratedsmoothed_pwdb =
+			GET_UNDECORATED_AVERAGE_RSSI(rtlpriv);
+	} else {
+		undecoratedsmoothed_pwdb
+			= rtlpriv->dm.entry_min_undec_sm_pwdb;
+	}
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "rtl8723ae_dm_bt_get_rx_ss() = %ld\n", smooth);
+		 "rtl8723e_dm_bt_get_rx_ss() = %ld\n",
+		 undecoratedsmoothed_pwdb);
 
-	return smooth;
+	return undecoratedsmoothed_pwdb;
 }
 
-void rtl8723ae_dm_bt_balance(struct ieee80211_hw *hw,
-			     bool balance_on, u8 ms0, u8 ms1)
+void rtl8723e_dm_bt_balance(struct ieee80211_hw *hw,
+			    bool balance_on, u8 ms0, u8 ms1)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 h2c_parameter[3] = {0};
 
@@ -379,27 +381,26 @@ void rtl8723ae_dm_bt_balance(struct ieee80211_hw *hw,
 		h2c_parameter[2] = 1;
 		h2c_parameter[1] = ms1;
 		h2c_parameter[0] = ms0;
-		rtlpcipriv->bt_coexist.fw_coexist_all_off = false;
+		rtlpriv->btcoexist.fw_coexist_all_off = false;
 	} else {
 		h2c_parameter[2] = 0;
 		h2c_parameter[1] = 0;
 		h2c_parameter[0] = 0;
 	}
-	rtlpcipriv->bt_coexist.balance_on = balance_on;
+	rtlpriv->btcoexist.balance_on = balance_on;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 		 "[DM][BT], Balance=[%s:%dms:%dms], write 0xc=0x%x\n",
-		 balance_on ? "ON" : "OFF", ms0, ms1,
-		 h2c_parameter[0]<<16 | h2c_parameter[1]<<8 | h2c_parameter[2]);
+		 balance_on ? "ON" : "OFF", ms0, ms1, h2c_parameter[0]<<16 |
+		 h2c_parameter[1]<<8 | h2c_parameter[2]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0xc, 3, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0xc, 3, h2c_parameter);
 }
 
 
-void rtl8723ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type)
+void rtl8723e_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 
 	if (type == BT_AGCTABLE_OFF) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
@@ -410,15 +411,15 @@ void rtl8723ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type)
 		rtl_write_dword(rtlpriv, 0xc78, 0x611f0001);
 		rtl_write_dword(rtlpriv, 0xc78, 0x60200001);
 
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0x32000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0x71000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0xb0000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0xfc000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_G1, 0xfffff, 0x30355);
 	} else if (type == BT_AGCTABLE_ON) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
@@ -429,25 +430,24 @@ void rtl8723ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type)
 		rtl_write_dword(rtlpriv, 0xc78, 0x4b1f0001);
 		rtl_write_dword(rtlpriv, 0xc78, 0x4a200001);
 
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0xdc000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0x90000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0x51000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_AGC_HP, 0xfffff, 0x12000);
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A,
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A,
 					RF_RX_G1, 0xfffff, 0x00355);
 
-		rtlpcipriv->bt_coexist.sw_coexist_all_off = false;
+		rtlpriv->btcoexist.sw_coexist_all_off = false;
 	}
 }
 
-void rtl8723ae_dm_bt_bback_off_level(struct ieee80211_hw *hw, u8 type)
+void rtl8723e_dm_bt_bb_back_off_level(struct ieee80211_hw *hw, u8 type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 
 	if (type == BT_BB_BACKOFF_OFF) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
@@ -457,87 +457,81 @@ void rtl8723ae_dm_bt_bback_off_level(struct ieee80211_hw *hw, u8 type)
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "[BT]BBBackOffLevel On!\n");
 		rtl_write_dword(rtlpriv, 0xc04, 0x3a07611);
-		rtlpcipriv->bt_coexist.sw_coexist_all_off = false;
+		rtlpriv->btcoexist.sw_coexist_all_off = false;
 	}
 }
 
-void rtl8723ae_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "rtl8723ae_dm_bt_fw_coex_all_off()\n");
+		 "rtl8723e_dm_bt_fw_coex_all_off()\n");
 
-	if (rtlpcipriv->bt_coexist.fw_coexist_all_off)
+	if (rtlpriv->btcoexist.fw_coexist_all_off)
 		return;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "rtl8723ae_dm_bt_fw_coex_all_off(), real Do\n");
-	rtl8723ae_dm_bt_fw_coex_all_off_8723a(hw);
-	rtlpcipriv->bt_coexist.fw_coexist_all_off = true;
+		 "rtl8723e_dm_bt_fw_coex_all_off(), real Do\n");
+	rtl8723e_dm_bt_fw_coex_all_off_8723a(hw);
+	rtlpriv->btcoexist.fw_coexist_all_off = true;
 }
 
-void rtl8723ae_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "rtl8723ae_dm_bt_sw_coex_all_off()\n");
+		 "rtl8723e_dm_bt_sw_coex_all_off()\n");
 
-	if (rtlpcipriv->bt_coexist.sw_coexist_all_off)
+	if (rtlpriv->btcoexist.sw_coexist_all_off)
 		return;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "rtl8723ae_dm_bt_sw_coex_all_off(), real Do\n");
-	rtl8723ae_dm_bt_sw_coex_all_off_8723a(hw);
-	rtlpcipriv->bt_coexist.sw_coexist_all_off = true;
+		 "rtl8723e_dm_bt_sw_coex_all_off(), real Do\n");
+	rtl8723e_dm_bt_sw_coex_all_off_8723a(hw);
+	rtlpriv->btcoexist.sw_coexist_all_off = true;
 }
 
-void rtl8723ae_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "rtl8723ae_dm_bt_hw_coex_all_off()\n");
+		 "rtl8723e_dm_bt_hw_coex_all_off()\n");
 
-	if (rtlpcipriv->bt_coexist.hw_coexist_all_off)
+	if (rtlpriv->btcoexist.hw_coexist_all_off)
 		return;
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "rtl8723ae_dm_bt_hw_coex_all_off(), real Do\n");
+		 "rtl8723e_dm_bt_hw_coex_all_off(), real Do\n");
 
-	rtl8723ae_dm_bt_hw_coex_all_off_8723a(hw);
+	rtl8723e_dm_bt_hw_coex_all_off_8723a(hw);
 
-	rtlpcipriv->bt_coexist.hw_coexist_all_off = true;
+	rtlpriv->btcoexist.hw_coexist_all_off = true;
 }
 
-void rtl8723ae_btdm_coex_all_off(struct ieee80211_hw *hw)
+void rtl8723e_btdm_coex_all_off(struct ieee80211_hw *hw)
 {
-	rtl8723ae_dm_bt_fw_coex_all_off(hw);
-	rtl8723ae_dm_bt_sw_coex_all_off(hw);
-	rtl8723ae_dm_bt_hw_coex_all_off(hw);
+	rtl8723e_dm_bt_fw_coex_all_off(hw);
+	rtl8723e_dm_bt_sw_coex_all_off(hw);
+	rtl8723e_dm_bt_hw_coex_all_off(hw);
 }
 
-bool rtl8723ae_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw)
+bool rtl8723e_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	if ((rtlpcipriv->bt_coexist.previous_state ==
-	    rtlpcipriv->bt_coexist.cstate) &&
-	    (rtlpcipriv->bt_coexist.previous_state_h ==
-	    rtlpcipriv->bt_coexist.cstate_h))
+	if ((rtlpriv->btcoexist.previous_state == rtlpriv->btcoexist.cstate) &&
+	    (rtlpriv->btcoexist.previous_state_h ==
+	     rtlpriv->btcoexist.cstate_h))
 		return false;
-	else
-		return true;
+	return true;
 }
 
-bool rtl8723ae_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw)
+bool rtl8723e_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	if (rtlpriv->link_info.tx_busy_traffic)
 		return true;
-	else
-		return false;
+	return false;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.h b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.h
index 76f4d122dbc1..bcd64a22acc0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_bt_coexist.h
@@ -53,8 +53,8 @@
 #define BT_COEX_STATE_WIFI_LEGACY		BIT(3)
 
 #define BT_COEX_STATE_WIFI_RSSI_LOW		BIT(4)
-#define BT_COEX_STATE_WIFI_RSSI_MEDIUM		BIT(5)
-#define BT_COEX_STATE_WIFI_RSSI_HIGH		BIT(6)
+#define BT_COEX_STATE_WIFI_RSSI_MEDIUM	BIT(5)
+#define BT_COEX_STATE_WIFI_RSSI_HIGH	BIT(6)
 #define BT_COEX_STATE_DEC_BT_POWER		BIT(7)
 
 #define BT_COEX_STATE_WIFI_IDLE			BIT(8)
@@ -78,7 +78,7 @@
 #define BT_COEX_STATE_WIFI_RSSI_1_MEDIUM	BIT(25)
 #define BT_COEX_STATE_WIFI_RSSI_1_HIGH		BIT(26)
 
-#define BT_COEX_STATE_BTINFO_COMMON		BIT(30)
+#define BT_COEX_STATE_BTINFO_COMMON			BIT(30)
 #define BT_COEX_STATE_BTINFO_B_HID_SCOESCO	BIT(31)
 #define BT_COEX_STATE_BTINFO_B_FTP_A2DP		BIT(29)
 
@@ -133,28 +133,26 @@
 #define	BTINFO_B_SCO_ESCO			BIT(1)
 #define	BTINFO_B_CONNECTION			BIT(0)
 
+void rtl8723e_btdm_coex_all_off(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw);
 
-void rtl8723ae_btdm_coex_all_off(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw);
-
-void rtl8723ae_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw);
-long rtl8723ae_dm_bt_get_rx_ss(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_balance(struct ieee80211_hw *hw,
+void rtl8723e_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw);
+long rtl8723e_dm_bt_get_rx_ss(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_balance(struct ieee80211_hw *hw,
 			    bool balance_on, u8 ms0, u8 ms1);
-void rtl8723ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type);
-void rtl8723ae_dm_bt_bback_off_level(struct ieee80211_hw *hw, u8 type);
-u8 rtl8723ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
+void rtl8723e_dm_bt_agc_table(struct ieee80211_hw *hw, u8 tyep);
+void rtl8723e_dm_bt_bb_back_off_level(struct ieee80211_hw *hw, u8 type);
+u8 rtl8723e_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
 					u8 level_num, u8 rssi_thresh,
 					u8 rssi_thresh1);
-u8 rtl8723ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
-					 u8  level_num, u8 rssi_thresh,
+u8 rtl8723e_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
+					 u8 level_num, u8 rssi_thresh,
 					 u8 rssi_thresh1);
 void _rtl8723_dm_bt_check_wifi_state(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw,
-						bool reject);
-
-bool rtl8723ae_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw);
-bool rtl8723ae_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw,
+						bool b_reject);
+bool rtl8723e_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw);
+bool rtl8723e_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c
index 5d534df8d90c..00a0531cc5f4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -25,45 +21,42 @@
  *
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
- ****************************************************************************
- */
+ *****************************************************************************/
 #include "hal_btc.h"
 #include "../pci.h"
 #include "phy.h"
-#include "../rtl8723com/phy_common.h"
 #include "fw.h"
-#include "../rtl8723com/fw_common.h"
 #include "reg.h"
 #include "def.h"
+#include "../rtl8723com/phy_common.h"
+
+static struct bt_coexist_8723 hal_coex_8723;
 
-void rtl8723ae_bt_coex_off_before_lps(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_turn_off_bt_coexist_before_enter_lps(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 
-	if (!rtlpcipriv->bt_coexist.bt_coexistence)
+	if (!rtlpriv->btcoexist.bt_coexistence)
 		return;
 
 	if (ppsc->inactiveps) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BT][DM], Before enter IPS, turn off all Coexist DM\n");
-		rtlpcipriv->bt_coexist.cstate = 0;
-		rtlpcipriv->bt_coexist.previous_state = 0;
-		rtlpcipriv->bt_coexist.cstate_h = 0;
-		rtlpcipriv->bt_coexist.previous_state_h = 0;
-		rtl8723ae_btdm_coex_all_off(hw);
+			"[BT][DM], Before enter IPS, turn off all Coexist DM\n");
+		rtlpriv->btcoexist.cstate = 0;
+		rtlpriv->btcoexist.previous_state = 0;
+		rtlpriv->btcoexist.cstate_h = 0;
+		rtlpriv->btcoexist.previous_state_h = 0;
+		rtl8723e_btdm_coex_all_off(hw);
 	}
 }
 
-static enum _RT_MEDIA_STATUS mgnt_link_status_query(struct ieee80211_hw *hw)
+static enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	enum _RT_MEDIA_STATUS m_status = RT_MEDIA_DISCONNECT;
-
+	enum rt_media_status    m_status = RT_MEDIA_DISCONNECT;
 	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
-
 	if (bibss || rtlpriv->mac80211.link_state >= MAC80211_LINKED)
 		m_status = RT_MEDIA_CONNECT;
 
@@ -71,15 +64,14 @@ static enum _RT_MEDIA_STATUS mgnt_link_status_query(struct ieee80211_hw *hw)
 }
 
 void rtl_8723e_bt_wifi_media_status_notify(struct ieee80211_hw *hw,
-					   bool mstatus)
+						bool mstatus)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	u8 h2c_parameter[3] = {0};
 	u8 chnl;
 
-	if (!rtlpcipriv->bt_coexist.bt_coexistence)
+	if (!rtlpriv->btcoexist.bt_coexistence)
 		return;
 
 	if (RT_MEDIA_CONNECT == mstatus)
@@ -98,14 +90,13 @@ void rtl_8723e_bt_wifi_media_status_notify(struct ieee80211_hw *hw,
 		h2c_parameter[2] = 0x20;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-		 "[BTCoex], FW write 0x19 = 0x%x\n",
+		 "[BTCoex], FW write 0x19=0x%x\n",
 		 h2c_parameter[0]<<16|h2c_parameter[1]<<8|h2c_parameter[2]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x19, 3, h2c_parameter);
-
+	rtl8723e_fill_h2c_cmd(hw, 0x19, 3, h2c_parameter);
 }
 
-static bool rtl8723ae_dm_bt_is_wifi_busy(struct ieee80211_hw *hw)
+static bool rtl8723e_dm_bt_is_wifi_busy(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	if (rtlpriv->link_info.busytraffic ||
@@ -116,12 +107,12 @@ static bool rtl8723ae_dm_bt_is_wifi_busy(struct ieee80211_hw *hw)
 		return false;
 }
 
-static void rtl8723ae_dm_bt_set_fw_3a(struct ieee80211_hw *hw,
-				      u8 byte1, u8 byte2, u8 byte3,
-				      u8 byte4, u8 byte5)
+static void rtl8723e_dm_bt_set_fw_3a(struct ieee80211_hw *hw,
+				     u8 byte1, u8 byte2, u8 byte3, u8 byte4,
+				     u8 byte5)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[5] = {0};
+	u8 h2c_parameter[5];
 
 	h2c_parameter[0] = byte1;
 	h2c_parameter[1] = byte2;
@@ -129,37 +120,37 @@ static void rtl8723ae_dm_bt_set_fw_3a(struct ieee80211_hw *hw,
 	h2c_parameter[3] = byte4;
 	h2c_parameter[4] = byte5;
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], FW write 0x3a(4bytes) = 0x%x%8x\n",
-		 h2c_parameter[0], h2c_parameter[1]<<24 | h2c_parameter[2]<<16 |
-		 h2c_parameter[3]<<8 | h2c_parameter[4]);
-	rtl8723ae_fill_h2c_cmd(hw, 0x3a, 5, h2c_parameter);
+		"[BTCoex], FW write 0x3a(4bytes)=0x%x%8x\n",
+		h2c_parameter[0], h2c_parameter[1]<<24 |
+		h2c_parameter[2]<<16 | h2c_parameter[3]<<8 |
+		h2c_parameter[4]);
+	rtl8723e_fill_h2c_cmd(hw, 0x3a, 5, h2c_parameter);
 }
 
-static bool rtl8723ae_dm_bt_need_to_dec_bt_pwr(struct ieee80211_hw *hw)
+static bool rtl8723e_dm_bt_need_to_dec_bt_pwr(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	if (mgnt_link_status_query(hw) == RT_MEDIA_CONNECT) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "Need to decrease bt power\n");
-		rtlpcipriv->bt_coexist.cstate |= BT_COEX_STATE_DEC_BT_POWER;
-		return true;
+			"Need to decrease bt power\n");
+			rtlpriv->btcoexist.cstate |=
+			BT_COEX_STATE_DEC_BT_POWER;
+			return true;
 	}
 
-	rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_DEC_BT_POWER;
+	rtlpriv->btcoexist.cstate &= ~BT_COEX_STATE_DEC_BT_POWER;
 	return false;
 }
 
-static bool rtl8723ae_dm_bt_is_same_coexist_state(struct ieee80211_hw *hw)
+static bool rtl8723e_dm_bt_is_same_coexist_state(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 
-	if ((rtlpcipriv->bt_coexist.previous_state ==
-	    rtlpcipriv->bt_coexist.cstate) &&
-	    (rtlpcipriv->bt_coexist.previous_state_h ==
-	    rtlpcipriv->bt_coexist.cstate_h)) {
+	if ((rtlpriv->btcoexist.previous_state ==
+	     rtlpriv->btcoexist.cstate) &&
+	    (rtlpriv->btcoexist.previous_state_h ==
+	     rtlpriv->btcoexist.cstate_h)) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[DM][BT], Coexist state do not chang!!\n");
 		return true;
@@ -170,86 +161,84 @@ static bool rtl8723ae_dm_bt_is_same_coexist_state(struct ieee80211_hw *hw)
 	}
 }
 
-static void rtl8723ae_dm_bt_set_coex_table(struct ieee80211_hw *hw,
-					   u32 val_0x6c0, u32 val_0x6c8,
-					   u32 val_0x6cc)
+static void rtl8723e_dm_bt_set_coex_table(struct ieee80211_hw *hw,
+					  u32 val_0x6c0, u32 val_0x6c8,
+					  u32 val_0x6cc)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "set coex table, set 0x6c0 = 0x%x\n", val_0x6c0);
+		 "set coex table, set 0x6c0=0x%x\n", val_0x6c0);
 	rtl_write_dword(rtlpriv, 0x6c0, val_0x6c0);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "set coex table, set 0x6c8 = 0x%x\n", val_0x6c8);
+		 "set coex table, set 0x6c8=0x%x\n", val_0x6c8);
 	rtl_write_dword(rtlpriv, 0x6c8, val_0x6c8);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "set coex table, set 0x6cc = 0x%x\n", val_0x6cc);
+		 "set coex table, set 0x6cc=0x%x\n", val_0x6cc);
 	rtl_write_byte(rtlpriv, 0x6cc, val_0x6cc);
 }
 
-static void rtl8723ae_dm_bt_set_hw_pta_mode(struct ieee80211_hw *hw, bool mode)
+static void rtl8723e_dm_bt_set_hw_pta_mode(struct ieee80211_hw *hw, bool b_mode)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	if (BT_PTA_MODE_ON == mode) {
+	if (BT_PTA_MODE_ON == b_mode) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE, "PTA mode on, ");
 		/*  Enable GPIO 0/1/2/3/8 pins for bt */
 		rtl_write_byte(rtlpriv, 0x40, 0x20);
-		rtlpcipriv->bt_coexist.hw_coexist_all_off = false;
+		rtlpriv->btcoexist.hw_coexist_all_off = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE, "PTA mode off\n");
 		rtl_write_byte(rtlpriv, 0x40, 0x0);
 	}
 }
 
-static void rtl8723ae_dm_bt_set_sw_rf_rx_lpf_corner(struct ieee80211_hw *hw,
-						    u8 type)
+static void rtl8723e_dm_bt_set_sw_rf_rx_lpf_corner(struct ieee80211_hw *hw,
+						   u8 type)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	if (BT_RF_RX_LPF_CORNER_SHRINK == type) {
-		/* Shrink RF Rx LPF corner, 0x1e[7:4]=1111 ==> [11:4] by Jenyu*/
+		/* Shrink RF Rx LPF corner, 0x1e[7:4]=1111 ==> [11:4] */
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "Shrink RF Rx LPF corner!!\n");
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e, 0xfffff,
-					0xf0ff7);
-		rtlpcipriv->bt_coexist.sw_coexist_all_off = false;
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e,
+					0xfffff, 0xf0ff7);
+		rtlpriv->btcoexist.sw_coexist_all_off = false;
 	} else if (BT_RF_RX_LPF_CORNER_RESUME == type) {
 		/*Resume RF Rx LPF corner*/
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "Resume RF Rx LPF corner!!\n");
-		rtl8723ae_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e, 0xfffff,
-			rtlpcipriv->bt_coexist.bt_rfreg_origin_1e);
+		rtl8723e_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e, 0xfffff,
+					rtlpriv->btcoexist.bt_rfreg_origin_1e);
 	}
 }
 
-static void rtl8723ae_bt_set_penalty_tx_rate_adap(struct ieee80211_hw *hw,
-						  u8 ra_type)
+static void dm_bt_set_sw_penalty_tx_rate_adapt(struct ieee80211_hw *hw,
+					       u8 ra_type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u8 tmu1;
+	u8 tmp_u1;
 
-	tmu1 = rtl_read_byte(rtlpriv, 0x4fd);
-	tmu1 |= BIT(0);
+	tmp_u1 = rtl_read_byte(rtlpriv, 0x4fd);
+	tmp_u1 |= BIT(0);
 	if (BT_TX_RATE_ADAPTIVE_LOW_PENALTY == ra_type) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "Tx rate adaptive, set low penalty!!\n");
-		tmu1 &= ~BIT(2);
-		rtlpcipriv->bt_coexist.sw_coexist_all_off = false;
+			"Tx rate adaptive, set low penalty!!\n");
+		tmp_u1 &= ~BIT(2);
+		rtlpriv->btcoexist.sw_coexist_all_off = false;
 	} else if (BT_TX_RATE_ADAPTIVE_NORMAL == ra_type) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "Tx rate adaptive, set normal!!\n");
-		tmu1 |= BIT(2);
+			"Tx rate adaptive, set normal!!\n");
+		tmp_u1 |= BIT(2);
 	}
-	rtl_write_byte(rtlpriv, 0x4fd, tmu1);
+
+	rtl_write_byte(rtlpriv, 0x4fd, tmp_u1);
 }
 
-static void rtl8723ae_dm_bt_btdm_structure_reload(struct ieee80211_hw *hw,
+static void rtl8723e_dm_bt_btdm_structure_reload(struct ieee80211_hw *hw,
 						 struct btdm_8723 *btdm)
 {
 	btdm->all_off = false;
@@ -292,32 +281,31 @@ static void rtl8723ae_dm_bt_btdm_structure_reload(struct ieee80211_hw *hw,
 	btdm->dec_bt_pwr = false;
 }
 
-static void dm_bt_btdm_structure_reload_all_off(struct ieee80211_hw *hw,
-						struct btdm_8723 *btdm)
+static void rtl8723e_dm_bt_btdm_structure_reload_all_off(struct ieee80211_hw *hw,
+							 struct btdm_8723 *btdm)
 {
-	rtl8723ae_dm_bt_btdm_structure_reload(hw, btdm);
+	rtl8723e_dm_bt_btdm_structure_reload(hw, btdm);
 	btdm->all_off = true;
 	btdm->pta_on = false;
 	btdm->wlan_act_hi = 0x10;
 }
 
-static bool rtl8723ae_dm_bt_is_2_ant_common_action(struct ieee80211_hw *hw)
+static bool rtl8723e_dm_bt_is_2_ant_common_action(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct btdm_8723 btdm8723;
-	bool common = false;
+	bool b_common = false;
 
-	rtl8723ae_dm_bt_btdm_structure_reload(hw, &btdm8723);
+	rtl8723e_dm_bt_btdm_structure_reload(hw, &btdm8723);
 
-	if (!rtl8723ae_dm_bt_is_wifi_busy(hw)
-	    && !rtlpcipriv->bt_coexist.bt_busy) {
+	if (!rtl8723e_dm_bt_is_wifi_busy(hw) &&
+	    !rtlpriv->btcoexist.bt_busy) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "Wifi idle + Bt idle, bt coex mechanism always off!!\n");
-		dm_bt_btdm_structure_reload_all_off(hw, &btdm8723);
-		common = true;
-	} else if (rtl8723ae_dm_bt_is_wifi_busy(hw)
-		   && !rtlpcipriv->bt_coexist.bt_busy) {
+		rtl8723e_dm_bt_btdm_structure_reload_all_off(hw, &btdm8723);
+		b_common = true;
+	} else if (rtl8723e_dm_bt_is_wifi_busy(hw) &&
+		   !rtlpriv->btcoexist.bt_busy) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "Wifi non-idle + Bt disabled/idle!!\n");
 		btdm8723.low_penalty_rate_adaptive = true;
@@ -338,17 +326,17 @@ static bool rtl8723ae_dm_bt_is_2_ant_common_action(struct ieee80211_hw *hw)
 		btdm8723.tdma_dac_swing = TDMA_DAC_SWING_OFF;
 		btdm8723.b2_ant_hid_en = false;
 
-		common = true;
-	} else if (rtlpcipriv->bt_coexist.bt_busy) {
+		b_common = true;
+	} else if (rtlpriv->btcoexist.bt_busy) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "Bt non-idle!\n");
+			"Bt non-idle!\n");
 		if (mgnt_link_status_query(hw) == RT_MEDIA_CONNECT) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "Wifi connection exist\n");
-			common = false;
+				"Wifi connection exist\n");
+			b_common = false;
 		} else {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "No Wifi connection!\n");
+				"No Wifi connection!\n");
 			btdm8723.rf_rx_lpf_shrink = true;
 			btdm8723.low_penalty_rate_adaptive = false;
 			btdm8723.reject_aggre_pkt = false;
@@ -367,27 +355,28 @@ static bool rtl8723ae_dm_bt_is_2_ant_common_action(struct ieee80211_hw *hw)
 			btdm8723.tdma_dac_swing = TDMA_DAC_SWING_OFF;
 			btdm8723.b2_ant_hid_en = false;
 
-			common = true;
+			b_common = true;
 		}
 	}
 
-	if (rtl8723ae_dm_bt_need_to_dec_bt_pwr(hw))
+	if (rtl8723e_dm_bt_need_to_dec_bt_pwr(hw))
 		btdm8723.dec_bt_pwr = true;
 
-	if (common)
-		rtlpcipriv->bt_coexist.cstate |= BT_COEX_STATE_BTINFO_COMMON;
+	if (b_common)
+		rtlpriv->btcoexist.cstate |=
+			BT_COEX_STATE_BTINFO_COMMON;
 
-	if (common && rtl8723ae_dm_bt_is_coexist_state_changed(hw))
-		rtl8723ae_dm_bt_set_bt_dm(hw, &btdm8723);
+	if (b_common && rtl8723e_dm_bt_is_coexist_state_changed(hw))
+		rtl8723e_dm_bt_set_bt_dm(hw, &btdm8723);
 
-	return common;
+	return b_common;
 }
 
-static void rtl8723ae_dm_bt_set_sw_full_time_dac_swing(struct ieee80211_hw *hw,
-						       bool sw_dac_swing_on,
-						       u32 sw_dac_swing_lvl)
+static void rtl8723e_dm_bt_set_sw_full_time_dac_swing(
+		struct ieee80211_hw *hw,
+		bool sw_dac_swing_on,
+		u32 sw_dac_swing_lvl)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	if (sw_dac_swing_on) {
@@ -395,7 +384,7 @@ static void rtl8723ae_dm_bt_set_sw_full_time_dac_swing(struct ieee80211_hw *hw,
 			 "[BTCoex], SwDacSwing = 0x%x\n", sw_dac_swing_lvl);
 		rtl8723_phy_set_bb_reg(hw, 0x880, 0xff000000,
 				       sw_dac_swing_lvl);
-		rtlpcipriv->bt_coexist.sw_coexist_all_off = false;
+		rtlpriv->btcoexist.sw_coexist_all_off = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "[BTCoex], SwDacSwing Off!\n");
@@ -403,10 +392,9 @@ static void rtl8723ae_dm_bt_set_sw_full_time_dac_swing(struct ieee80211_hw *hw,
 	}
 }
 
-static void rtl8723ae_dm_bt_set_fw_dec_bt_pwr(struct ieee80211_hw *hw,
-					      bool dec_bt_pwr)
+static void rtl8723e_dm_bt_set_fw_dec_bt_pwr(
+		struct ieee80211_hw *hw, bool dec_bt_pwr)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 h2c_parameter[1] = {0};
 
@@ -414,87 +402,86 @@ static void rtl8723ae_dm_bt_set_fw_dec_bt_pwr(struct ieee80211_hw *hw,
 
 	if (dec_bt_pwr) {
 		h2c_parameter[0] |= BIT(1);
-		rtlpcipriv->bt_coexist.fw_coexist_all_off = false;
+		rtlpriv->btcoexist.fw_coexist_all_off = false;
 	}
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], decrease Bt Power : %s, write 0x21 = 0x%x\n",
+		 "[BTCoex], decrease Bt Power : %s, write 0x21=0x%x\n",
 		 (dec_bt_pwr ? "Yes!!" : "No!!"), h2c_parameter[0]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x21, 1, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x21, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_2_ant_hid(struct ieee80211_hw *hw,
-					    bool enable, bool dac_swing_on)
+static void rtl8723e_dm_bt_set_fw_2_ant_hid(struct ieee80211_hw *hw,
+					    bool b_enable, bool b_dac_swing_on)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 h2c_parameter[1] = {0};
 
-	if (enable) {
+	if (b_enable) {
 		h2c_parameter[0] |= BIT(0);
-		rtlpcipriv->bt_coexist.fw_coexist_all_off = false;
+		rtlpriv->btcoexist.fw_coexist_all_off = false;
 	}
-	if (dac_swing_on)
+	if (b_dac_swing_on)
 		h2c_parameter[0] |= BIT(1); /* Dac Swing default enable */
+
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], turn 2-Ant+HID mode %s, DACSwing:%s, write 0x15 = 0x%x\n",
-		 (enable ? "ON!!" : "OFF!!"), (dac_swing_on ? "ON" : "OFF"),
+		 "[BTCoex], turn 2-Ant+HID mode %s, DACSwing:%s, write 0x15=0x%x\n",
+		 (b_enable ? "ON!!" : "OFF!!"), (b_dac_swing_on ? "ON" : "OFF"),
 		 h2c_parameter[0]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x15, 1, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x15, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_tdma_ctrl(struct ieee80211_hw *hw,
-					     bool enable, u8 ant_num, u8 nav_en,
-					     u8 dac_swing_en)
+static void rtl8723e_dm_bt_set_fw_tdma_ctrl(struct ieee80211_hw *hw,
+					    bool b_enable, u8 ant_num,
+					    u8 nav_en, u8 dac_swing_en)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	u8 h2c_parameter[1] = {0};
 	u8 h2c_parameter1[1] = {0};
 
 	h2c_parameter[0] = 0;
 	h2c_parameter1[0] = 0;
 
-	if (enable) {
+	if (b_enable) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "[BTCoex], set BT PTA update manager to trigger update!!\n");
 		h2c_parameter1[0] |= BIT(0);
 
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "[BTCoex], turn TDMA mode ON!!\n");
+			"[BTCoex], turn TDMA mode ON!!\n");
 		h2c_parameter[0] |= BIT(0);		/* function enable */
 		if (TDMA_1ANT == ant_num) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TDMA_1ANT\n");
+			"[BTCoex], TDMA_1ANT\n");
 			h2c_parameter[0] |= BIT(1);
 		} else if (TDMA_2ANT == ant_num) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TDMA_2ANT\n");
+			"[BTCoex], TDMA_2ANT\n");
 		} else {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], Unknown Ant\n");
+			"[BTCoex], Unknown Ant\n");
 		}
 
 		if (TDMA_NAV_OFF == nav_en) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TDMA_NAV_OFF\n");
+			"[BTCoex], TDMA_NAV_OFF\n");
 		} else if (TDMA_NAV_ON == nav_en) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TDMA_NAV_ON\n");
+			"[BTCoex], TDMA_NAV_ON\n");
 			h2c_parameter[0] |= BIT(2);
 		}
 
 		if (TDMA_DAC_SWING_OFF == dac_swing_en) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TDMA_DAC_SWING_OFF\n");
+				"[BTCoex], TDMA_DAC_SWING_OFF\n");
 		} else if (TDMA_DAC_SWING_ON == dac_swing_en) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TDMA_DAC_SWING_ON\n");
+				"[BTCoex], TDMA_DAC_SWING_ON\n");
 			h2c_parameter[0] |= BIT(4);
 		}
-		rtlpcipriv->bt_coexist.fw_coexist_all_off = false;
+		rtlpriv->btcoexist.fw_coexist_all_off = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "[BTCoex], set BT PTA update manager to no update!!\n");
@@ -503,46 +490,46 @@ static void rtl8723ae_dm_bt_set_fw_tdma_ctrl(struct ieee80211_hw *hw,
 	}
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], FW2AntTDMA, write 0x26 = 0x%x\n",
+		 "[BTCoex], FW2AntTDMA, write 0x26=0x%x\n",
 		 h2c_parameter1[0]);
-	rtl8723ae_fill_h2c_cmd(hw, 0x26, 1, h2c_parameter1);
+	rtl8723e_fill_h2c_cmd(hw, 0x26, 1, h2c_parameter1);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], FW2AntTDMA, write 0x14 = 0x%x\n", h2c_parameter[0]);
-	rtl8723ae_fill_h2c_cmd(hw, 0x14, 1, h2c_parameter);
+		"[BTCoex], FW2AntTDMA, write 0x14=0x%x\n",
+		h2c_parameter[0]);
+	rtl8723e_fill_h2c_cmd(hw, 0x14, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_ignore_wlan_act(struct ieee80211_hw *hw,
-						   bool enable)
+static void rtl8723e_dm_bt_set_fw_ignore_wlan_act(struct ieee80211_hw *hw,
+						  bool b_enable)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	u8 h2c_parameter[1] = {0};
 
-	if (enable) {
+	if (b_enable) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "[BTCoex], BT Ignore Wlan_Act !!\n");
+			"[BTCoex], BT Ignore Wlan_Act !!\n");
 		h2c_parameter[0] |= BIT(0);		/* function enable */
-		rtlpcipriv->bt_coexist.fw_coexist_all_off = false;
+		rtlpriv->btcoexist.fw_coexist_all_off = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "[BTCoex], BT don't ignore Wlan_Act !!\n");
+			"[BTCoex], BT don't ignore Wlan_Act !!\n");
 	}
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], set FW for BT Ignore Wlan_Act, write 0x25 = 0x%x\n",
+		 "[BTCoex], set FW for BT Ignore Wlan_Act, write 0x25=0x%x\n",
 		 h2c_parameter[0]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x25, 1, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x25, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_tra_tdma_ctrl(struct ieee80211_hw *hw,
-						 bool enable, u8 ant_num,
-						 u8 nav_en)
+static void rtl8723e_dm_bt_set_fw_tra_tdma_ctrl(struct ieee80211_hw *hw,
+						bool b_enable, u8 ant_num,
+						u8 nav_en)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
 	u8 h2c_parameter[2] = {0};
 
 	/* Only 8723 B cut should do this */
@@ -552,460 +539,467 @@ static void rtl8723ae_dm_bt_set_fw_tra_tdma_ctrl(struct ieee80211_hw *hw,
 		return;
 	}
 
-	if (enable) {
+	if (b_enable) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "[BTCoex], turn TTDMA mode ON!!\n");
-		h2c_parameter[0] |= BIT(0);		/* function enable */
+		h2c_parameter[0] |= BIT(0);	/* function enable */
 		if (TDMA_1ANT == ant_num) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 				 "[BTCoex], TTDMA_1ANT\n");
 			h2c_parameter[0] |= BIT(1);
 		} else if (TDMA_2ANT == ant_num) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TTDMA_2ANT\n");
+			"[BTCoex], TTDMA_2ANT\n");
 		} else {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], Unknown Ant\n");
+			"[BTCoex], Unknown Ant\n");
 		}
 
 		if (TDMA_NAV_OFF == nav_en) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TTDMA_NAV_OFF\n");
+			"[BTCoex], TTDMA_NAV_OFF\n");
 		} else if (TDMA_NAV_ON == nav_en) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex], TTDMA_NAV_ON\n");
+			"[BTCoex], TTDMA_NAV_ON\n");
 			h2c_parameter[1] |= BIT(0);
 		}
 
-		rtlpcipriv->bt_coexist.fw_coexist_all_off = false;
+		rtlpriv->btcoexist.fw_coexist_all_off = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "[BTCoex], turn TTDMA mode OFF!!\n");
+			"[BTCoex], turn TTDMA mode OFF!!\n");
 	}
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], FW Traditional TDMA, write 0x33 = 0x%x\n",
-		 h2c_parameter[0] << 8 | h2c_parameter[1]);
+		"[BTCoex], FW Traditional TDMA, write 0x33=0x%x\n",
+		h2c_parameter[0] << 8 | h2c_parameter[1]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x33, 2, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x33, 2, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_dac_swing_level(struct ieee80211_hw *hw,
-						   u8 dac_swing_lvl)
+static void rtl8723e_dm_bt_set_fw_dac_swing_level(struct ieee80211_hw *hw,
+						  u8 dac_swing_lvl)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 h2c_parameter[1] = {0};
-
 	h2c_parameter[0] = dac_swing_lvl;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], Set Dac Swing Level = 0x%x\n", dac_swing_lvl);
+		"[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], write 0x29 = 0x%x\n", h2c_parameter[0]);
+		"[BTCoex], write 0x29=0x%x\n", h2c_parameter[0]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x29, 1, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x29, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_bt_hid_info(struct ieee80211_hw *hw,
-					       bool enable)
+static void rtl8723e_dm_bt_set_fw_bt_hid_info(struct ieee80211_hw *hw,
+					      bool b_enable)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 h2c_parameter[1] = {0};
-
 	h2c_parameter[0] = 0;
 
-	if (enable) {
+	if (b_enable) {
 		h2c_parameter[0] |= BIT(0);
-		rtlpcipriv->bt_coexist.fw_coexist_all_off = false;
+		rtlpriv->btcoexist.fw_coexist_all_off = false;
 	}
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], Set BT HID information = 0x%x\n", enable);
+		"[BTCoex], Set BT HID information=0x%x\n", b_enable);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], write 0x24 = 0x%x\n", h2c_parameter[0]);
+		"[BTCoex], write 0x24=0x%x\n", h2c_parameter[0]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x24, 1, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x24, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_bt_retry_index(struct ieee80211_hw *hw,
-						  u8 retry_index)
+static void rtl8723e_dm_bt_set_fw_bt_retry_index(struct ieee80211_hw *hw,
+						 u8 retry_index)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 h2c_parameter[1] = {0};
-
 	h2c_parameter[0] = retry_index;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], Set BT Retry Index=%d\n", retry_index);
+		"[BTCoex], Set BT Retry Index=%d\n", retry_index);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], write 0x23 = 0x%x\n", h2c_parameter[0]);
+		"[BTCoex], write 0x23=0x%x\n", h2c_parameter[0]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x23, 1, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x23, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_set_fw_wlan_act(struct ieee80211_hw *hw,
-					    u8 wlan_act_hi, u8 wlan_act_lo)
+static void rtl8723e_dm_bt_set_fw_wlan_act(struct ieee80211_hw *hw,
+					   u8 wlan_act_hi, u8 wlan_act_lo)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 h2c_parameter_hi[1] = {0};
 	u8 h2c_parameter_lo[1] = {0};
-
 	h2c_parameter_hi[0] = wlan_act_hi;
 	h2c_parameter_lo[0] = wlan_act_lo;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], Set WLAN_ACT Hi:Lo = 0x%x/0x%x\n", wlan_act_hi,
-		 wlan_act_lo);
+		"[BTCoex], Set WLAN_ACT Hi:Lo=0x%x/0x%x\n",
+		wlan_act_hi, wlan_act_lo);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], write 0x22 = 0x%x\n", h2c_parameter_hi[0]);
+		"[BTCoex], write 0x22=0x%x\n", h2c_parameter_hi[0]);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "[BTCoex], write 0x11 = 0x%x\n", h2c_parameter_lo[0]);
+		"[BTCoex], write 0x11=0x%x\n", h2c_parameter_lo[0]);
 
 	/* WLAN_ACT = High duration, unit:ms */
-	rtl8723ae_fill_h2c_cmd(hw, 0x22, 1, h2c_parameter_hi);
+	rtl8723e_fill_h2c_cmd(hw, 0x22, 1, h2c_parameter_hi);
 	/*  WLAN_ACT = Low duration, unit:3*625us */
-	rtl8723ae_fill_h2c_cmd(hw, 0x11, 1, h2c_parameter_lo);
+	rtl8723e_fill_h2c_cmd(hw, 0x11, 1, h2c_parameter_lo);
 }
 
-void rtl8723ae_dm_bt_set_bt_dm(struct ieee80211_hw *hw, struct btdm_8723 *btdm)
+void rtl8723e_dm_bt_set_bt_dm(struct ieee80211_hw *hw,
+			      struct btdm_8723 *btdm)
 {
-	struct rtl_pci_priv	*rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv	*rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct btdm_8723 *btdm_8723 = &rtlhal->hal_coex_8723.btdm;
+	struct btdm_8723 *btdm_8723 = &hal_coex_8723.btdm;
 	u8 i;
+
 	bool fw_current_inpsmode = false;
 	bool fw_ps_awake = true;
 
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				      (u8 *)(&fw_current_inpsmode));
+					      (u8 *)(&fw_current_inpsmode));
 	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
-				      (u8 *)(&fw_ps_awake));
+					      (u8 *)(&fw_ps_awake));
 
-	/* check new setting is different than the old one,
-	 * if all the same, don't do the setting again.
-	 */
+	/* check new setting is different with the old one, */
+	/* if all the same, don't do the setting again. */
 	if (memcmp(btdm_8723, btdm, sizeof(struct btdm_8723)) == 0) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], the same coexist setting, return!!\n");
+			"[BTCoex], the same coexist setting, return!!\n");
 		return;
 	} else {	/* save the new coexist setting */
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], UPDATE TO NEW COEX SETTING!!\n");
+			"[BTCoex], UPDATE TO NEW COEX SETTING!!\n");
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bAllOff = 0x%x/ 0x%x\n",
-			 btdm_8723->all_off, btdm->all_off);
+			"[BTCoex], original/new bAllOff=0x%x/ 0x%x\n",
+			btdm_8723->all_off, btdm->all_off);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new agc_table_en = 0x%x/ 0x%x\n",
-			 btdm_8723->agc_table_en, btdm->agc_table_en);
+			"[BTCoex], original/new agc_table_en=0x%x/ 0x%x\n",
+			btdm_8723->agc_table_en, btdm->agc_table_en);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new adc_back_off_on = 0x%x/ 0x%x\n",
-			 btdm_8723->adc_back_off_on, btdm->adc_back_off_on);
+			 "[BTCoex], original/new adc_back_off_on=0x%x/ 0x%x\n",
+			 btdm_8723->adc_back_off_on,
+			 btdm->adc_back_off_on);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new b2_ant_hid_en = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new b2_ant_hid_en=0x%x/ 0x%x\n",
 			 btdm_8723->b2_ant_hid_en, btdm->b2_ant_hid_en);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bLowPenaltyRateAdaptive = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new bLowPenaltyRateAdaptive=0x%x/ 0x%x\n",
 			 btdm_8723->low_penalty_rate_adaptive,
 			 btdm->low_penalty_rate_adaptive);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bRfRxLpfShrink = 0x%x/ 0x%x\n",
-			 btdm_8723->rf_rx_lpf_shrink, btdm->rf_rx_lpf_shrink);
+			 "[BTCoex], original/new bRfRxLpfShrink=0x%x/ 0x%x\n",
+			 btdm_8723->rf_rx_lpf_shrink,
+			 btdm->rf_rx_lpf_shrink);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bRejectAggrePkt = 0x%x/ 0x%x\n",
-			 btdm_8723->reject_aggre_pkt, btdm->reject_aggre_pkt);
+			 "[BTCoex], original/new bRejectAggrePkt=0x%x/ 0x%x\n",
+			 btdm_8723->reject_aggre_pkt,
+			 btdm->reject_aggre_pkt);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new tdma_on = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new tdma_on=0x%x/ 0x%x\n",
 			 btdm_8723->tdma_on, btdm->tdma_on);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new tdmaAnt = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new tdmaAnt=0x%x/ 0x%x\n",
 			 btdm_8723->tdma_ant, btdm->tdma_ant);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new tdmaNav = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new tdmaNav=0x%x/ 0x%x\n",
 			 btdm_8723->tdma_nav, btdm->tdma_nav);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new tdma_dac_swing = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new tdma_dac_swing=0x%x/ 0x%x\n",
 			 btdm_8723->tdma_dac_swing, btdm->tdma_dac_swing);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new fwDacSwingLvl = 0x%x/ 0x%x\n",
-			 btdm_8723->fw_dac_swing_lvl, btdm->fw_dac_swing_lvl);
+			 "[BTCoex], original/new fw_dac_swing_lvl=0x%x/ 0x%x\n",
+			 btdm_8723->fw_dac_swing_lvl,
+			 btdm->fw_dac_swing_lvl);
 
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bTraTdmaOn = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new bTraTdmaOn=0x%x/ 0x%x\n",
 			 btdm_8723->tra_tdma_on, btdm->tra_tdma_on);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new traTdmaAnt = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new traTdmaAnt=0x%x/ 0x%x\n",
 			 btdm_8723->tra_tdma_ant, btdm->tra_tdma_ant);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new traTdmaNav = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new traTdmaNav=0x%x/ 0x%x\n",
 			 btdm_8723->tra_tdma_nav, btdm->tra_tdma_nav);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bPsTdmaOn = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new bPsTdmaOn=0x%x/ 0x%x\n",
 			 btdm_8723->ps_tdma_on, btdm->ps_tdma_on);
 		for (i = 0; i < 5; i++) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "[BTCoex], original/new psTdmaByte[i] = 0x%x/ 0x%x\n",
+				 "[BTCoex], original/new psTdmaByte[i]=0x%x/ 0x%x\n",
 				 btdm_8723->ps_tdma_byte[i],
 				 btdm->ps_tdma_byte[i]);
 		}
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bIgnoreWlanAct = 0x%x/ 0x%x\n",
-			 btdm_8723->ignore_wlan_act, btdm->ignore_wlan_act);
+			"[BTCoex], original/new bIgnoreWlanAct=0x%x/ 0x%x\n",
+			btdm_8723->ignore_wlan_act,
+			btdm->ignore_wlan_act);
+
 
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new bPtaOn = 0x%x/ 0x%x\n",
-			 btdm_8723->pta_on, btdm->pta_on);
+			"[BTCoex], original/new bPtaOn=0x%x/ 0x%x\n",
+			btdm_8723->pta_on, btdm->pta_on);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new val_0x6c0 = 0x%x/ 0x%x\n",
-			 btdm_8723->val_0x6c0, btdm->val_0x6c0);
+			"[BTCoex], original/new val_0x6c0=0x%x/ 0x%x\n",
+			btdm_8723->val_0x6c0, btdm->val_0x6c0);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new val_0x6c8 = 0x%x/ 0x%x\n",
-			 btdm_8723->val_0x6c8, btdm->val_0x6c8);
+			"[BTCoex], original/new val_0x6c8=0x%x/ 0x%x\n",
+			btdm_8723->val_0x6c8, btdm->val_0x6c8);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new val_0x6cc = 0x%x/ 0x%x\n",
-			 btdm_8723->val_0x6cc, btdm->val_0x6cc);
+			"[BTCoex], original/new val_0x6cc=0x%x/ 0x%x\n",
+			btdm_8723->val_0x6cc, btdm->val_0x6cc);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new sw_dac_swing_on = 0x%x/ 0x%x\n",
-			 btdm_8723->sw_dac_swing_on, btdm->sw_dac_swing_on);
+			 "[BTCoex], original/new sw_dac_swing_on=0x%x/ 0x%x\n",
+			 btdm_8723->sw_dac_swing_on,
+			 btdm->sw_dac_swing_on);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new sw_dac_swing_lvl = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new sw_dac_swing_lvl=0x%x/ 0x%x\n",
 			 btdm_8723->sw_dac_swing_lvl,
 			 btdm->sw_dac_swing_lvl);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new wlanActHi = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new wlanActHi=0x%x/ 0x%x\n",
 			 btdm_8723->wlan_act_hi, btdm->wlan_act_hi);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new wlanActLo = 0x%x/ 0x%x\n",
+			 "[BTCoex], original/new wlanActLo=0x%x/ 0x%x\n",
 			 btdm_8723->wlan_act_lo, btdm->wlan_act_lo);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], original/new btRetryIndex = 0x%x/ 0x%x\n",
-			btdm_8723->bt_retry_index, btdm->bt_retry_index);
+			 "[BTCoex], original/new btRetryIndex=0x%x/ 0x%x\n",
+			 btdm_8723->bt_retry_index, btdm->bt_retry_index);
 
 		memcpy(btdm_8723, btdm, sizeof(struct btdm_8723));
 	}
-	/*
-	 * Here we only consider when Bt Operation
+	/* Here we only consider when Bt Operation
 	 * inquiry/paging/pairing is ON
 	 * we only need to turn off TDMA
 	 */
 
-	if (rtlpcipriv->bt_coexist.hold_for_bt_operation) {
+	if (rtlpriv->btcoexist.hold_for_bt_operation) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "[BTCoex], set to ignore wlanAct for BT OP!!\n");
-		rtl8723ae_dm_bt_set_fw_ignore_wlan_act(hw, true);
+			"[BTCoex], set to ignore wlanAct for BT OP!!\n");
+		rtl8723e_dm_bt_set_fw_ignore_wlan_act(hw, true);
 		return;
 	}
 
 	if (btdm->all_off) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "[BTCoex], disable all coexist mechanism !!\n");
-		rtl8723ae_btdm_coex_all_off(hw);
+			"[BTCoex], disable all coexist mechanism !!\n");
+		rtl8723e_btdm_coex_all_off(hw);
 		return;
 	}
 
-	rtl8723ae_dm_bt_reject_ap_aggregated_packet(hw, btdm->reject_aggre_pkt);
+	rtl8723e_dm_bt_reject_ap_aggregated_packet(hw, btdm->reject_aggre_pkt);
 
 	if (btdm->low_penalty_rate_adaptive)
-		rtl8723ae_bt_set_penalty_tx_rate_adap(hw,
-			BT_TX_RATE_ADAPTIVE_LOW_PENALTY);
+		dm_bt_set_sw_penalty_tx_rate_adapt(hw, BT_TX_RATE_ADAPTIVE_LOW_PENALTY);
 	else
-		rtl8723ae_bt_set_penalty_tx_rate_adap(hw,
-			BT_TX_RATE_ADAPTIVE_NORMAL);
+		dm_bt_set_sw_penalty_tx_rate_adapt(hw,
+						   BT_TX_RATE_ADAPTIVE_NORMAL);
 
 	if (btdm->rf_rx_lpf_shrink)
-		rtl8723ae_dm_bt_set_sw_rf_rx_lpf_corner(hw,
-					 BT_RF_RX_LPF_CORNER_SHRINK);
+		rtl8723e_dm_bt_set_sw_rf_rx_lpf_corner(hw,
+				BT_RF_RX_LPF_CORNER_SHRINK);
 	else
-		rtl8723ae_dm_bt_set_sw_rf_rx_lpf_corner(hw,
-					 BT_RF_RX_LPF_CORNER_RESUME);
+		rtl8723e_dm_bt_set_sw_rf_rx_lpf_corner(hw,
+				BT_RF_RX_LPF_CORNER_RESUME);
 
 	if (btdm->agc_table_en)
-		rtl8723ae_dm_bt_agc_table(hw, BT_AGCTABLE_ON);
+		rtl8723e_dm_bt_agc_table(hw, BT_AGCTABLE_ON);
 	else
-		rtl8723ae_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
+		rtl8723e_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
 
 	if (btdm->adc_back_off_on)
-		rtl8723ae_dm_bt_bback_off_level(hw, BT_BB_BACKOFF_ON);
+		rtl8723e_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_ON);
 	else
-		rtl8723ae_dm_bt_bback_off_level(hw, BT_BB_BACKOFF_OFF);
+		rtl8723e_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_OFF);
 
-	rtl8723ae_dm_bt_set_fw_bt_retry_index(hw, btdm->bt_retry_index);
+	rtl8723e_dm_bt_set_fw_bt_retry_index(hw, btdm->bt_retry_index);
 
-	rtl8723ae_dm_bt_set_fw_dac_swing_level(hw, btdm->fw_dac_swing_lvl);
-	rtl8723ae_dm_bt_set_fw_wlan_act(hw, btdm->wlan_act_hi,
+	rtl8723e_dm_bt_set_fw_dac_swing_level(hw, btdm->fw_dac_swing_lvl);
+	rtl8723e_dm_bt_set_fw_wlan_act(hw, btdm->wlan_act_hi,
 				       btdm->wlan_act_lo);
 
-	rtl8723ae_dm_bt_set_coex_table(hw, btdm->val_0x6c0,
-		btdm->val_0x6c8, btdm->val_0x6cc);
-	rtl8723ae_dm_bt_set_hw_pta_mode(hw, btdm->pta_on);
+	rtl8723e_dm_bt_set_coex_table(hw, btdm->val_0x6c0,
+				      btdm->val_0x6c8, btdm->val_0x6cc);
+	rtl8723e_dm_bt_set_hw_pta_mode(hw, btdm->pta_on);
 
 	/* Note: There is a constraint between TDMA and 2AntHID
-	 * Only one of 2AntHid and tdma can be turned on
-	 * We should turn off those mechanisms first
-	 * and then turn on them on.
+	 * Only one of 2AntHid and tdma can be turn on
+	 * We should turn off those mechanisms should be turned off first
+	 * and then turn on those mechanisms should be turned on.
 	*/
 	if (btdm->b2_ant_hid_en) {
 		/* turn off tdma */
-		rtl8723ae_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
+		rtl8723e_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
 						    btdm->tra_tdma_ant,
 						    btdm->tra_tdma_nav);
-		rtl8723ae_dm_bt_set_fw_tdma_ctrl(hw, false, btdm->tdma_ant,
+		rtl8723e_dm_bt_set_fw_tdma_ctrl(hw, false, btdm->tdma_ant,
 						btdm->tdma_nav,
 						btdm->tdma_dac_swing);
 
 		/* turn off Pstdma */
-		rtl8723ae_dm_bt_set_fw_ignore_wlan_act(hw,
+		rtl8723e_dm_bt_set_fw_ignore_wlan_act(hw,
 						      btdm->ignore_wlan_act);
 		/* Antenna control by PTA, 0x870 = 0x300. */
-		rtl8723ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
+		rtl8723e_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
 
 		/* turn on 2AntHid */
-		rtl8723ae_dm_bt_set_fw_bt_hid_info(hw, true);
-		rtl8723ae_dm_bt_set_fw_2_ant_hid(hw, true, true);
+		rtl8723e_dm_bt_set_fw_bt_hid_info(hw, true);
+		rtl8723e_dm_bt_set_fw_2_ant_hid(hw, true, true);
 	} else if (btdm->tdma_on) {
 		/* turn off 2AntHid */
-		rtl8723ae_dm_bt_set_fw_bt_hid_info(hw, false);
-		rtl8723ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+		rtl8723e_dm_bt_set_fw_bt_hid_info(hw, false);
+		rtl8723e_dm_bt_set_fw_2_ant_hid(hw, false, false);
 
 		/* turn off pstdma */
-		rtl8723ae_dm_bt_set_fw_ignore_wlan_act(hw,
+		rtl8723e_dm_bt_set_fw_ignore_wlan_act(hw,
 						      btdm->ignore_wlan_act);
 		/* Antenna control by PTA, 0x870 = 0x300. */
-		rtl8723ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
+		rtl8723e_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
 
 		/* turn on tdma */
-		rtl8723ae_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
-				 btdm->tra_tdma_ant, btdm->tra_tdma_nav);
-		rtl8723ae_dm_bt_set_fw_tdma_ctrl(hw, true, btdm->tdma_ant,
-				 btdm->tdma_nav, btdm->tdma_dac_swing);
+		rtl8723e_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
+						    btdm->tra_tdma_ant,
+						    btdm->tra_tdma_nav);
+		rtl8723e_dm_bt_set_fw_tdma_ctrl(hw, true, btdm->tdma_ant,
+						btdm->tdma_nav,
+						btdm->tdma_dac_swing);
 	} else if (btdm->ps_tdma_on) {
 		/* turn off 2AntHid */
-		rtl8723ae_dm_bt_set_fw_bt_hid_info(hw, false);
-		rtl8723ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+		rtl8723e_dm_bt_set_fw_bt_hid_info(hw, false);
+		rtl8723e_dm_bt_set_fw_2_ant_hid(hw, false, false);
 
 		/* turn off tdma */
-		rtl8723ae_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
-				 btdm->tra_tdma_ant, btdm->tra_tdma_nav);
-		rtl8723ae_dm_bt_set_fw_tdma_ctrl(hw, false, btdm->tdma_ant,
-				 btdm->tdma_nav, btdm->tdma_dac_swing);
+		rtl8723e_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
+						    btdm->tra_tdma_ant,
+						    btdm->tra_tdma_nav);
+		rtl8723e_dm_bt_set_fw_tdma_ctrl(hw, false, btdm->tdma_ant,
+						btdm->tdma_nav,
+						btdm->tdma_dac_swing);
 
 		/* turn on pstdma */
-		rtl8723ae_dm_bt_set_fw_ignore_wlan_act(hw,
-				 btdm->ignore_wlan_act);
-		rtl8723ae_dm_bt_set_fw_3a(hw,
-			btdm->ps_tdma_byte[0],
-			btdm->ps_tdma_byte[1],
-			btdm->ps_tdma_byte[2],
-			btdm->ps_tdma_byte[3],
-			btdm->ps_tdma_byte[4]);
+		rtl8723e_dm_bt_set_fw_ignore_wlan_act(hw,
+						      btdm->ignore_wlan_act);
+		rtl8723e_dm_bt_set_fw_3a(hw, btdm->ps_tdma_byte[0],
+					 btdm->ps_tdma_byte[1],
+					 btdm->ps_tdma_byte[2],
+					 btdm->ps_tdma_byte[3],
+					 btdm->ps_tdma_byte[4]);
 	} else {
 		/* turn off 2AntHid */
-		rtl8723ae_dm_bt_set_fw_bt_hid_info(hw, false);
-		rtl8723ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
+		rtl8723e_dm_bt_set_fw_bt_hid_info(hw, false);
+		rtl8723e_dm_bt_set_fw_2_ant_hid(hw, false, false);
 
 		/* turn off tdma */
-		rtl8723ae_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
-				 btdm->tra_tdma_ant, btdm->tra_tdma_nav);
-		rtl8723ae_dm_bt_set_fw_tdma_ctrl(hw, false, btdm->tdma_ant,
-				 btdm->tdma_nav, btdm->tdma_dac_swing);
+		rtl8723e_dm_bt_set_fw_tra_tdma_ctrl(hw, btdm->tra_tdma_on,
+						    btdm->tra_tdma_ant,
+						    btdm->tra_tdma_nav);
+		rtl8723e_dm_bt_set_fw_tdma_ctrl(hw, false, btdm->tdma_ant,
+						btdm->tdma_nav,
+						btdm->tdma_dac_swing);
 
 		/* turn off pstdma */
-		rtl8723ae_dm_bt_set_fw_ignore_wlan_act(hw,
-						      btdm->ignore_wlan_act);
+		rtl8723e_dm_bt_set_fw_ignore_wlan_act(hw,
+						btdm->ignore_wlan_act);
 		/* Antenna control by PTA, 0x870 = 0x300. */
-		rtl8723ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
+		rtl8723e_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
 	}
 
 	/* Note:
-	 * We should add delay for making sure sw DacSwing can be set
-	 *  sucessfully. Because of that rtl8723ae_dm_bt_set_fw_2_ant_hid()
-	 * and rtl8723ae_dm_bt_set_fw_tdma_ctrl()
+	 * We should add delay for making sure
+	 *	sw DacSwing can be set sucessfully.
+	 * because of that rtl8723e_dm_bt_set_fw_2_ant_hid()
+	 *	and rtl8723e_dm_bt_set_fw_tdma_ctrl()
 	 * will overwrite the reg 0x880.
 	*/
 	mdelay(30);
-	rtl8723ae_dm_bt_set_sw_full_time_dac_swing(hw,
-		btdm->sw_dac_swing_on, btdm->sw_dac_swing_lvl);
-	rtl8723ae_dm_bt_set_fw_dec_bt_pwr(hw, btdm->dec_bt_pwr);
+	rtl8723e_dm_bt_set_sw_full_time_dac_swing(hw, btdm->sw_dac_swing_on,
+						  btdm->sw_dac_swing_lvl);
+	rtl8723e_dm_bt_set_fw_dec_bt_pwr(hw, btdm->dec_bt_pwr);
 }
 
-/*============================================================
- * extern function start with BTDM_
- *============================================================
+/* ============================================================ */
+/* extern function start with BTDM_ */
+/* ============================================================i
  */
-static u32 rtl8723ae_dm_bt_tx_rx_couter_h(struct ieee80211_hw *hw)
+static u32 rtl8723e_dm_bt_tx_rx_couter_h(struct ieee80211_hw *hw)
 {
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u32 counters = 0;
+	u32	counters = 0;
 
-	counters = rtlhal->hal_coex_8723.high_priority_tx +
-		   rtlhal->hal_coex_8723.high_priority_rx;
+	counters = hal_coex_8723.high_priority_tx +
+			hal_coex_8723.high_priority_rx;
 	return counters;
 }
 
-static u32 rtl8723ae_dm_bt_tx_rx_couter_l(struct ieee80211_hw *hw)
+static u32 rtl8723e_dm_bt_tx_rx_couter_l(struct ieee80211_hw *hw)
 {
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 counters = 0;
 
-	return rtlhal->hal_coex_8723.low_priority_tx +
-	       rtlhal->hal_coex_8723.low_priority_rx;
+	counters = hal_coex_8723.low_priority_tx +
+			hal_coex_8723.low_priority_rx;
+	return counters;
 }
 
-static u8 rtl8723ae_dm_bt_bt_tx_rx_counter_level(struct ieee80211_hw *hw)
+static u8 rtl8723e_dm_bt_bt_tx_rx_counter_level(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u32 bt_tx_rx_cnt = 0;
-	u8 bt_tx_rx_cnt_lvl = 0;
+	u32	bt_tx_rx_cnt = 0;
+	u8	bt_tx_rx_cnt_lvl = 0;
 
-	bt_tx_rx_cnt = rtl8723ae_dm_bt_tx_rx_couter_h(hw) +
-		       rtl8723ae_dm_bt_tx_rx_couter_l(hw);
+	bt_tx_rx_cnt = rtl8723e_dm_bt_tx_rx_couter_h(hw)
+				+ rtl8723e_dm_bt_tx_rx_couter_l(hw);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 		 "[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt);
 
-	rtlpcipriv->bt_coexist.cstate_h &=
-		 ~(BT_COEX_STATE_BT_CNT_LEVEL_0 | BT_COEX_STATE_BT_CNT_LEVEL_1 |
+	rtlpriv->btcoexist.cstate_h &= ~
+		 (BT_COEX_STATE_BT_CNT_LEVEL_0 | BT_COEX_STATE_BT_CNT_LEVEL_1|
 		  BT_COEX_STATE_BT_CNT_LEVEL_2);
 
 	if (bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_3) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[BTCoex], BT TxRx Counters at level 3\n");
 		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_3;
-		rtlpcipriv->bt_coexist.cstate_h |= BT_COEX_STATE_BT_CNT_LEVEL_3;
+		rtlpriv->btcoexist.cstate_h |=
+			BT_COEX_STATE_BT_CNT_LEVEL_3;
 	} else if (bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_2) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[BTCoex], BT TxRx Counters at level 2\n");
 		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_2;
-		rtlpcipriv->bt_coexist.cstate_h |= BT_COEX_STATE_BT_CNT_LEVEL_2;
+		rtlpriv->btcoexist.cstate_h |=
+			BT_COEX_STATE_BT_CNT_LEVEL_2;
 	} else if (bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_1) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[BTCoex], BT TxRx Counters at level 1\n");
 		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_1;
-		rtlpcipriv->bt_coexist.cstate_h |= BT_COEX_STATE_BT_CNT_LEVEL_1;
+		rtlpriv->btcoexist.cstate_h  |=
+			BT_COEX_STATE_BT_CNT_LEVEL_1;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[BTCoex], BT TxRx Counters at level 0\n");
 		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_0;
-		rtlpcipriv->bt_coexist.cstate_h |= BT_COEX_STATE_BT_CNT_LEVEL_0;
+		rtlpriv->btcoexist.cstate_h |=
+			BT_COEX_STATE_BT_CNT_LEVEL_0;
 	}
 	return bt_tx_rx_cnt_lvl;
 }
 
-static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
+static void rtl8723e_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct btdm_8723 btdm8723;
 	u8 bt_rssi_state, bt_rssi_state1;
-	u8 bt_tx_rx_cnt_lvl;
+	u8	bt_tx_rx_cnt_lvl = 0;
 
-	rtl8723ae_dm_bt_btdm_structure_reload(hw, &btdm8723);
+	rtl8723e_dm_bt_btdm_structure_reload(hw, &btdm8723);
 
 	btdm8723.rf_rx_lpf_shrink = true;
 	btdm8723.low_penalty_rate_adaptive = true;
 	btdm8723.reject_aggre_pkt = false;
 
-	bt_tx_rx_cnt_lvl = rtl8723ae_dm_bt_bt_tx_rx_counter_level(hw);
+	bt_tx_rx_cnt_lvl = rtl8723e_dm_bt_bt_tx_rx_counter_level(hw);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 		 "[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt_lvl);
 
@@ -1051,10 +1045,10 @@ static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "HT20 or Legacy\n");
-		bt_rssi_state = rtl8723ae_dm_bt_check_coex_rssi_state(hw, 2,
-								     47, 0);
-		bt_rssi_state1 = rtl8723ae_dm_bt_check_coex_rssi_state1(hw, 2,
-								       27, 0);
+		bt_rssi_state =
+		  rtl8723e_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
+		bt_rssi_state1 =
+		  rtl8723e_dm_bt_check_coex_rssi_state1(hw, 2, 27, 0);
 
 		/* coex table */
 		btdm8723.val_0x6c0 = 0x55555555;
@@ -1063,15 +1057,15 @@ static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 
 		/* sw mechanism */
 		if ((bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BT_RSSI_STATE_STAY_HIGH)) {
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH)) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "Wifi rssi high\n");
+					"Wifi rssi high\n");
 			btdm8723.agc_table_en = true;
 			btdm8723.adc_back_off_on = true;
 			btdm8723.sw_dac_swing_on = false;
 		} else {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "Wifi rssi low\n");
+					"Wifi rssi low\n");
 			btdm8723.agc_table_en = false;
 			btdm8723.adc_back_off_on = false;
 			btdm8723.sw_dac_swing_on = false;
@@ -1080,16 +1074,15 @@ static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 		/* fw mechanism */
 		btdm8723.ps_tdma_on = true;
 		if ((bt_rssi_state1 == BT_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH)) {
+			(bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH)) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 				 "Wifi rssi-1 high\n");
-			/* only rssi high we need to do this,
-			 * when rssi low, the value will modified by fw
-			 */
+			/* only rssi high we need to do this, */
+			/* when rssi low, the value will modified by fw */
 			rtl_write_byte(rtlpriv, 0x883, 0x40);
 			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-					 "[BTCoex], BT TxRx Counters >= 1400\n");
+				"[BTCoex], BT TxRx Counters >= 1400\n");
 				btdm8723.ps_tdma_byte[0] = 0xa3;
 				btdm8723.ps_tdma_byte[1] = 0x5;
 				btdm8723.ps_tdma_byte[2] = 0x5;
@@ -1097,7 +1090,7 @@ static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 				btdm8723.ps_tdma_byte[4] = 0x80;
 			} else if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-					 "[BTCoex], BT TxRx Counters >= 1200 && < 1400\n");
+					 "[BTCoex], BT TxRx Counters>= 1200 && < 1400\n");
 				btdm8723.ps_tdma_byte[0] = 0xa3;
 				btdm8723.ps_tdma_byte[1] = 0xa;
 				btdm8723.ps_tdma_byte[2] = 0xa;
@@ -1114,7 +1107,7 @@ static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 			}
 		} else {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "Wifi rssi-1 low\n");
+					"Wifi rssi-1 low\n");
 			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 					 "[BTCoex], BT TxRx Counters >= 1400\n");
@@ -1143,16 +1136,15 @@ static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 		}
 	}
 
-	if (rtl8723ae_dm_bt_need_to_dec_bt_pwr(hw))
+	if (rtl8723e_dm_bt_need_to_dec_bt_pwr(hw))
 		btdm8723.dec_bt_pwr = true;
 
 	/* Always ignore WlanAct if bHid|bSCOBusy|bSCOeSCO */
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 		 "[BTCoex], BT btInqPageStartTime = 0x%x, btTxRxCntLvl = %d\n",
-		 rtlhal->hal_coex_8723.bt_inq_page_start_time,
-		 bt_tx_rx_cnt_lvl);
-	if ((rtlhal->hal_coex_8723.bt_inq_page_start_time) ||
+		 hal_coex_8723.bt_inq_page_start_time, bt_tx_rx_cnt_lvl);
+	if ((hal_coex_8723.bt_inq_page_start_time) ||
 	    (BT_TXRX_CNT_LEVEL_3 == bt_tx_rx_cnt_lvl)) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[BTCoex], Set BT inquiry / page scan 0x3a setting\n");
@@ -1164,33 +1156,35 @@ static void rtl8723ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
 		btdm8723.ps_tdma_byte[4] = 0x80;
 	}
 
-	if (rtl8723ae_dm_bt_is_coexist_state_changed(hw))
-		rtl8723ae_dm_bt_set_bt_dm(hw, &btdm8723);
+	if (rtl8723e_dm_bt_is_coexist_state_changed(hw))
+		rtl8723e_dm_bt_set_bt_dm(hw, &btdm8723);
+
 }
 
-static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
+static void rtl8723e_dm_bt_2_ant_ftp_a2dp(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct btdm_8723 btdm8723;
+
 	u8 bt_rssi_state, bt_rssi_state1;
-	u32 bt_tx_rx_cnt_lvl;
+	u32 bt_tx_rx_cnt_lvl = 0;
+
+	rtl8723e_dm_bt_btdm_structure_reload(hw, &btdm8723);
 
-	rtl8723ae_dm_bt_btdm_structure_reload(hw, &btdm8723);
 	btdm8723.rf_rx_lpf_shrink = true;
 	btdm8723.low_penalty_rate_adaptive = true;
 	btdm8723.reject_aggre_pkt = false;
 
-	bt_tx_rx_cnt_lvl = rtl8723ae_dm_bt_bt_tx_rx_counter_level(hw);
+	bt_tx_rx_cnt_lvl = rtl8723e_dm_bt_bt_tx_rx_counter_level(hw);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-		 "[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt_lvl);
+	"[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt_lvl);
 
 	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG, "HT40\n");
-		bt_rssi_state = rtl8723ae_dm_bt_check_coex_rssi_state(hw, 2,
-								     37, 0);
+		bt_rssi_state =
+		  rtl8723e_dm_bt_check_coex_rssi_state(hw, 2, 37, 0);
 
 		/* coex table */
 		btdm8723.val_0x6c0 = 0x55555555;
@@ -1205,12 +1199,12 @@ static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
 		/* fw mechanism */
 		btdm8723.ps_tdma_on = true;
 		if ((bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BT_RSSI_STATE_STAY_HIGH)) {
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH)) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "Wifi rssi high\n");
+						"Wifi rssi high\n");
 			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-					 "[BTCoex], BT TxRx Counters >= 1400\n");
+				"[BTCoex], BT TxRx Counters >= 1400\n");
 				btdm8723.ps_tdma_byte[0] = 0xa3;
 				btdm8723.ps_tdma_byte[1] = 0x5;
 				btdm8723.ps_tdma_byte[2] = 0x5;
@@ -1244,7 +1238,8 @@ static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
 				btdm8723.ps_tdma_byte[2] = 0x5;
 				btdm8723.ps_tdma_byte[3] = 0x0;
 				btdm8723.ps_tdma_byte[4] = 0x80;
-			} else if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
+			} else if (bt_tx_rx_cnt_lvl ==
+				BT_TXRX_CNT_LEVEL_1) {
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 					 "[BTCoex], BT TxRx Counters >= 1200 && < 1400\n");
 				btdm8723.ps_tdma_byte[0] = 0xa3;
@@ -1265,10 +1260,10 @@ static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "HT20 or Legacy\n");
-		bt_rssi_state = rtl8723ae_dm_bt_check_coex_rssi_state(hw, 2,
-								     47, 0);
-		bt_rssi_state1 = rtl8723ae_dm_bt_check_coex_rssi_state1(hw, 2,
-								       27, 0);
+		bt_rssi_state =
+		  rtl8723e_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
+		bt_rssi_state1 =
+		  rtl8723e_dm_bt_check_coex_rssi_state1(hw, 2, 27, 0);
 
 		/* coex table */
 		btdm8723.val_0x6c0 = 0x55555555;
@@ -1277,7 +1272,7 @@ static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
 
 		/* sw mechanism */
 		if ((bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BT_RSSI_STATE_STAY_HIGH)) {
+			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH)) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 				 "Wifi rssi high\n");
 			btdm8723.agc_table_en = true;
@@ -1294,12 +1289,11 @@ static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
 		/* fw mechanism */
 		btdm8723.ps_tdma_on = true;
 		if ((bt_rssi_state1 == BT_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH)) {
+			(bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH)) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 				 "Wifi rssi-1 high\n");
-			/* only rssi high we need to do this,
-			 * when rssi low, the value will modified by fw
-			 */
+			/* only rssi high we need to do this, */
+			/* when rssi low, the value will modified by fw */
 			rtl_write_byte(rtlpriv, 0x883, 0x40);
 			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
@@ -1357,15 +1351,14 @@ static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
 		}
 	}
 
-	if (rtl8723ae_dm_bt_need_to_dec_bt_pwr(hw))
+	if (rtl8723e_dm_bt_need_to_dec_bt_pwr(hw))
 		btdm8723.dec_bt_pwr = true;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 		 "[BTCoex], BT btInqPageStartTime = 0x%x, btTxRxCntLvl = %d\n",
-		 rtlhal->hal_coex_8723.bt_inq_page_start_time,
-		 bt_tx_rx_cnt_lvl);
+		 hal_coex_8723.bt_inq_page_start_time, bt_tx_rx_cnt_lvl);
 
-	if ((rtlhal->hal_coex_8723.bt_inq_page_start_time) ||
+	if ((hal_coex_8723.bt_inq_page_start_time) ||
 	    (BT_TXRX_CNT_LEVEL_3 == bt_tx_rx_cnt_lvl)) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[BTCoex], Set BT inquiry / page scan 0x3a setting\n");
@@ -1377,379 +1370,373 @@ static void rtl8723ae_dm_bt_2_ant_fta2dp(struct ieee80211_hw *hw)
 		btdm8723.ps_tdma_byte[4] = 0x80;
 	}
 
-	if (rtl8723ae_dm_bt_is_coexist_state_changed(hw))
-		rtl8723ae_dm_bt_set_bt_dm(hw, &btdm8723);
+	if (rtl8723e_dm_bt_is_coexist_state_changed(hw))
+		rtl8723e_dm_bt_set_bt_dm(hw, &btdm8723);
+
 }
 
-static void rtl8723ae_dm_bt_inq_page_monitor(struct ieee80211_hw *hw)
+static void rtl8723e_dm_bt_inq_page_monitor(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u32 cur_time = jiffies;
+	u32 cur_time;
 
-	if (rtlhal->hal_coex_8723.c2h_bt_inquiry_page) {
+	cur_time = jiffies;
+	if (hal_coex_8723.c2h_bt_inquiry_page) {
 		/* bt inquiry or page is started. */
-		if (rtlhal->hal_coex_8723.bt_inq_page_start_time == 0) {
-			rtlpcipriv->bt_coexist.cstate |=
-					 BT_COEX_STATE_BT_INQ_PAGE;
-			rtlhal->hal_coex_8723.bt_inq_page_start_time = cur_time;
+		if (hal_coex_8723.bt_inq_page_start_time == 0) {
+			rtlpriv->btcoexist.cstate  |=
+			BT_COEX_STATE_BT_INQ_PAGE;
+			hal_coex_8723.bt_inq_page_start_time = cur_time;
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 				 "[BTCoex], BT Inquiry/page is started at time : 0x%x\n",
-				 rtlhal->hal_coex_8723.bt_inq_page_start_time);
+				 hal_coex_8723.bt_inq_page_start_time);
 		}
 	}
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 		 "[BTCoex], BT Inquiry/page started time : 0x%x, cur_time : 0x%x\n",
-		 rtlhal->hal_coex_8723.bt_inq_page_start_time, cur_time);
+		 hal_coex_8723.bt_inq_page_start_time, cur_time);
 
-	if (rtlhal->hal_coex_8723.bt_inq_page_start_time) {
+	if (hal_coex_8723.bt_inq_page_start_time) {
 		if ((((long)cur_time -
-		    (long)rtlhal->hal_coex_8723.bt_inq_page_start_time) / HZ) >=
-		    10) {
+			(long)hal_coex_8723.bt_inq_page_start_time) / HZ)
+			>= 10) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "[BTCoex], BT Inquiry/page >= 10sec!!!");
-			rtlhal->hal_coex_8723.bt_inq_page_start_time = 0;
-			rtlpcipriv->bt_coexist.cstate &=
-						 ~BT_COEX_STATE_BT_INQ_PAGE;
+				"[BTCoex], BT Inquiry/page >= 10sec!!!");
+			hal_coex_8723.bt_inq_page_start_time = 0;
+			rtlpriv->btcoexist.cstate &=
+				~BT_COEX_STATE_BT_INQ_PAGE;
 		}
 	}
 }
 
-static void rtl8723ae_dm_bt_reset_action_profile_state(struct ieee80211_hw *hw)
+static void rtl8723e_dm_bt_reset_action_profile_state(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	rtlpcipriv->bt_coexist.cstate &=
-		~(BT_COEX_STATE_PROFILE_HID | BT_COEX_STATE_PROFILE_A2DP |
+	rtlpriv->btcoexist.cstate &= ~
+		(BT_COEX_STATE_PROFILE_HID | BT_COEX_STATE_PROFILE_A2DP|
 		BT_COEX_STATE_PROFILE_PAN | BT_COEX_STATE_PROFILE_SCO);
 
-	rtlpcipriv->bt_coexist.cstate &=
-		~(BT_COEX_STATE_BTINFO_COMMON |
-		BT_COEX_STATE_BTINFO_B_HID_SCOESCO |
+	rtlpriv->btcoexist.cstate &= ~
+		(BT_COEX_STATE_BTINFO_COMMON |
+		BT_COEX_STATE_BTINFO_B_HID_SCOESCO|
 		BT_COEX_STATE_BTINFO_B_FTP_A2DP);
 }
 
-static void _rtl8723ae_dm_bt_coexist_2_ant(struct ieee80211_hw *hw)
+static void _rtl8723e_dm_bt_coexist_2_ant(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
+	u8 bt_retry_cnt;
 	u8 bt_info_original;
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-		 "[BTCoex] Get bt info by fw!!\n");
+		"[BTCoex] Get bt info by fw!!\n");
 
 	_rtl8723_dm_bt_check_wifi_state(hw);
 
-	if (rtlhal->hal_coex_8723.c2h_bt_info_req_sent) {
+	if (hal_coex_8723.c2h_bt_info_req_sent) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "[BTCoex] c2h for btInfo not rcvd yet!!\n");
+				"[BTCoex] c2h for bt_info not rcvd yet!!\n");
 	}
 
-	bt_info_original = rtlhal->hal_coex_8723.c2h_bt_info_original;
+	bt_retry_cnt = hal_coex_8723.bt_retry_cnt;
+	bt_info_original = hal_coex_8723.c2h_bt_info_original;
 
-	/* when bt inquiry or page scan, we have to set h2c 0x25
-	 * ignore wlanact for continuous 4x2secs
-	 */
-	rtl8723ae_dm_bt_inq_page_monitor(hw);
-	rtl8723ae_dm_bt_reset_action_profile_state(hw);
-
-	if (rtl8723ae_dm_bt_is_2_ant_common_action(hw)) {
-		rtlpcipriv->bt_coexist.bt_profile_case = BT_COEX_MECH_COMMON;
-		rtlpcipriv->bt_coexist.bt_profile_action = BT_COEX_MECH_COMMON;
+	/* when bt inquiry or page scan, we have to set h2c 0x25 */
+	/* ignore wlanact for continuous 4x2secs */
+	rtl8723e_dm_bt_inq_page_monitor(hw);
+	rtl8723e_dm_bt_reset_action_profile_state(hw);
 
+	if (rtl8723e_dm_bt_is_2_ant_common_action(hw)) {
+		rtlpriv->btcoexist.bt_profile_case = BT_COEX_MECH_COMMON;
+		rtlpriv->btcoexist.bt_profile_action = BT_COEX_MECH_COMMON;
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "Action 2-Ant common.\n");
+		"Action 2-Ant common.\n");
 	} else {
 		if ((bt_info_original & BTINFO_B_HID) ||
-		    (bt_info_original & BTINFO_B_SCO_BUSY) ||
-		    (bt_info_original & BTINFO_B_SCO_ESCO)) {
-			rtlpcipriv->bt_coexist.cstate |=
+			(bt_info_original & BTINFO_B_SCO_BUSY) ||
+			(bt_info_original & BTINFO_B_SCO_ESCO)) {
+				rtlpriv->btcoexist.cstate |=
 					BT_COEX_STATE_BTINFO_B_HID_SCOESCO;
-			rtlpcipriv->bt_coexist.bt_profile_case =
+				rtlpriv->btcoexist.bt_profile_case =
 					BT_COEX_MECH_HID_SCO_ESCO;
-			rtlpcipriv->bt_coexist.bt_profile_action =
+				rtlpriv->btcoexist.bt_profile_action =
 					BT_COEX_MECH_HID_SCO_ESCO;
-			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "[BTCoex], BTInfo: bHid|bSCOBusy|bSCOeSCO\n");
-			rtl8723ae_dm_bt_2_ant_hid_sco_esco(hw);
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
+					 "[BTCoex], BTInfo: bHid|bSCOBusy|bSCOeSCO\n");
+				rtl8723e_dm_bt_2_ant_hid_sco_esco(hw);
 		} else if ((bt_info_original & BTINFO_B_FTP) ||
-			   (bt_info_original & BTINFO_B_A2DP)) {
-			rtlpcipriv->bt_coexist.cstate |=
+				(bt_info_original & BTINFO_B_A2DP)) {
+				rtlpriv->btcoexist.cstate |=
 					BT_COEX_STATE_BTINFO_B_FTP_A2DP;
-			rtlpcipriv->bt_coexist.bt_profile_case =
+				rtlpriv->btcoexist.bt_profile_case =
 					BT_COEX_MECH_FTP_A2DP;
-			rtlpcipriv->bt_coexist.bt_profile_action =
+				rtlpriv->btcoexist.bt_profile_action =
 					BT_COEX_MECH_FTP_A2DP;
-			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "BTInfo: bFTP|bA2DP\n");
-			rtl8723ae_dm_bt_2_ant_fta2dp(hw);
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
+					 "BTInfo: bFTP|bA2DP\n");
+				rtl8723e_dm_bt_2_ant_ftp_a2dp(hw);
 		} else {
-			rtlpcipriv->bt_coexist.cstate |=
-					 BT_COEX_STATE_BTINFO_B_HID_SCOESCO;
-			rtlpcipriv->bt_coexist.bt_profile_case =
-					 BT_COEX_MECH_NONE;
-			rtlpcipriv->bt_coexist.bt_profile_action =
-					 BT_COEX_MECH_NONE;
-			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-				 "[BTCoex], BTInfo: undefined case!!!!\n");
-			rtl8723ae_dm_bt_2_ant_hid_sco_esco(hw);
+				rtlpriv->btcoexist.cstate |=
+					BT_COEX_STATE_BTINFO_B_HID_SCOESCO;
+				rtlpriv->btcoexist.bt_profile_case =
+					BT_COEX_MECH_NONE;
+				rtlpriv->btcoexist.bt_profile_action =
+					BT_COEX_MECH_NONE;
+				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
+					 "[BTCoex], BTInfo: undefined case!!!!\n");
+				rtl8723e_dm_bt_2_ant_hid_sco_esco(hw);
 		}
 	}
 }
 
-static void _rtl8723ae_dm_bt_coexist_1_ant(struct ieee80211_hw *hw)
+static void _rtl8723e_dm_bt_coexist_1_ant(struct ieee80211_hw *hw)
 {
+	return;
 }
 
-void rtl8723ae_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw)
 {
-	rtl8723ae_dm_bt_set_coex_table(hw, 0x5a5aaaaa, 0xcc, 0x3);
-	rtl8723ae_dm_bt_set_hw_pta_mode(hw, true);
+	rtl8723e_dm_bt_set_coex_table(hw, 0x5a5aaaaa, 0xcc, 0x3);
+	rtl8723e_dm_bt_set_hw_pta_mode(hw, true);
 }
 
-void rtl8723ae_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw)
 {
-	rtl8723ae_dm_bt_set_fw_ignore_wlan_act(hw, false);
-	rtl8723ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
-	rtl8723ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
-	rtl8723ae_dm_bt_set_fw_tra_tdma_ctrl(hw, false,
-					     TDMA_2ANT, TDMA_NAV_OFF);
-	rtl8723ae_dm_bt_set_fw_tdma_ctrl(hw, false, TDMA_2ANT,
-				TDMA_NAV_OFF, TDMA_DAC_SWING_OFF);
-	rtl8723ae_dm_bt_set_fw_dac_swing_level(hw, 0);
-	rtl8723ae_dm_bt_set_fw_bt_hid_info(hw, false);
-	rtl8723ae_dm_bt_set_fw_bt_retry_index(hw, 2);
-	rtl8723ae_dm_bt_set_fw_wlan_act(hw, 0x10, 0x10);
-	rtl8723ae_dm_bt_set_fw_dec_bt_pwr(hw, false);
+	rtl8723e_dm_bt_set_fw_ignore_wlan_act(hw, false);
+	rtl8723e_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
+	rtl8723e_dm_bt_set_fw_2_ant_hid(hw, false, false);
+	rtl8723e_dm_bt_set_fw_tra_tdma_ctrl(hw, false, TDMA_2ANT,
+					    TDMA_NAV_OFF);
+	rtl8723e_dm_bt_set_fw_tdma_ctrl(hw, false, TDMA_2ANT, TDMA_NAV_OFF,
+					TDMA_DAC_SWING_OFF);
+	rtl8723e_dm_bt_set_fw_dac_swing_level(hw, 0);
+	rtl8723e_dm_bt_set_fw_bt_hid_info(hw, false);
+	rtl8723e_dm_bt_set_fw_bt_retry_index(hw, 2);
+	rtl8723e_dm_bt_set_fw_wlan_act(hw, 0x10, 0x10);
+	rtl8723e_dm_bt_set_fw_dec_bt_pwr(hw, false);
 }
 
-void rtl8723ae_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw)
 {
-	rtl8723ae_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
-	rtl8723ae_dm_bt_bback_off_level(hw, BT_BB_BACKOFF_OFF);
-	rtl8723ae_dm_bt_reject_ap_aggregated_packet(hw, false);
+	rtl8723e_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
+	rtl8723e_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_OFF);
+	rtl8723e_dm_bt_reject_ap_aggregated_packet(hw, false);
 
-	rtl8723ae_bt_set_penalty_tx_rate_adap(hw, BT_TX_RATE_ADAPTIVE_NORMAL);
-	rtl8723ae_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_RESUME);
-	rtl8723ae_dm_bt_set_sw_full_time_dac_swing(hw, false, 0xc0);
+	dm_bt_set_sw_penalty_tx_rate_adapt(hw, BT_TX_RATE_ADAPTIVE_NORMAL);
+	rtl8723e_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_RESUME);
+	rtl8723e_dm_bt_set_sw_full_time_dac_swing(hw, false, 0xc0);
 }
 
-static void rtl8723ae_dm_bt_query_bt_information(struct ieee80211_hw *hw)
+static void rtl8723e_dm_bt_query_bt_information(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	u8 h2c_parameter[1] = {0};
 
-	rtlhal->hal_coex_8723.c2h_bt_info_req_sent = true;
+	hal_coex_8723.c2h_bt_info_req_sent = true;
 
 	h2c_parameter[0] |=  BIT(0);
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-		 "Query Bt information, write 0x38 = 0x%x\n",
-		 h2c_parameter[0]);
+		"Query Bt information, write 0x38=0x%x\n", h2c_parameter[0]);
 
-	rtl8723ae_fill_h2c_cmd(hw, 0x38, 1, h2c_parameter);
+	rtl8723e_fill_h2c_cmd(hw, 0x38, 1, h2c_parameter);
 }
 
-static void rtl8723ae_dm_bt_bt_hw_counters_monitor(struct ieee80211_hw *hw)
+static void rtl8723e_dm_bt_bt_hw_counters_monitor(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u32 reg_htx_rx, reg_ltx_rx, u32_tmp;
-	u32 reg_htx, reg_hrx, reg_ltx, reg_lrx;
-
-	reg_htx_rx = REG_HIGH_PRIORITY_TXRX;
-	reg_ltx_rx = REG_LOW_PRIORITY_TXRX;
-
-	u32_tmp = rtl_read_dword(rtlpriv, reg_htx_rx);
-	reg_htx = u32_tmp & MASKLWORD;
-	reg_hrx = (u32_tmp & MASKHWORD)>>16;
-
-	u32_tmp = rtl_read_dword(rtlpriv, reg_ltx_rx);
-	reg_ltx = u32_tmp & MASKLWORD;
-	reg_lrx = (u32_tmp & MASKHWORD)>>16;
-
-	if (rtlpcipriv->bt_coexist.lps_counter > 1) {
-		reg_htx %= rtlpcipriv->bt_coexist.lps_counter;
-		reg_hrx %= rtlpcipriv->bt_coexist.lps_counter;
-		reg_ltx %= rtlpcipriv->bt_coexist.lps_counter;
-		reg_lrx %= rtlpcipriv->bt_coexist.lps_counter;
+	u32 reg_hp_tx_rx, reg_lp_tx_rx, u32_tmp;
+	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
+
+	reg_hp_tx_rx = REG_HIGH_PRIORITY_TXRX;
+	reg_lp_tx_rx = REG_LOW_PRIORITY_TXRX;
+
+	u32_tmp = rtl_read_dword(rtlpriv, reg_hp_tx_rx);
+	reg_hp_tx = u32_tmp & MASKLWORD;
+	reg_hp_rx = (u32_tmp & MASKHWORD)>>16;
+
+	u32_tmp = rtl_read_dword(rtlpriv, reg_lp_tx_rx);
+	reg_lp_tx = u32_tmp & MASKLWORD;
+	reg_lp_rx = (u32_tmp & MASKHWORD)>>16;
+
+	if (rtlpriv->btcoexist.lps_counter > 1) {
+		reg_hp_tx %= rtlpriv->btcoexist.lps_counter;
+		reg_hp_rx %= rtlpriv->btcoexist.lps_counter;
+		reg_lp_tx %= rtlpriv->btcoexist.lps_counter;
+		reg_lp_rx %= rtlpriv->btcoexist.lps_counter;
 	}
 
-	rtlhal->hal_coex_8723.high_priority_tx = reg_htx;
-	rtlhal->hal_coex_8723.high_priority_rx = reg_hrx;
-	rtlhal->hal_coex_8723.low_priority_tx = reg_ltx;
-	rtlhal->hal_coex_8723.low_priority_rx = reg_lrx;
+	hal_coex_8723.high_priority_tx = reg_hp_tx;
+	hal_coex_8723.high_priority_rx = reg_hp_rx;
+	hal_coex_8723.low_priority_tx = reg_lp_tx;
+	hal_coex_8723.low_priority_rx = reg_lp_rx;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-		 "High Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
-		 reg_htx_rx, reg_htx, reg_htx, reg_hrx, reg_hrx);
+		"High Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
+		reg_hp_tx_rx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx);
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-		 "Low Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
-		 reg_ltx_rx, reg_ltx, reg_ltx, reg_lrx, reg_lrx);
-	rtlpcipriv->bt_coexist.lps_counter = 0;
+		"Low Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
+		reg_lp_tx_rx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
+	rtlpriv->btcoexist.lps_counter = 0;
+	/* rtl_write_byte(rtlpriv, 0x76e, 0xc); */
 }
 
-static void rtl8723ae_dm_bt_bt_enable_disable_check(struct ieee80211_hw *hw)
+static void rtl8723e_dm_bt_bt_enable_disable_check(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	bool bt_alife = true;
 
-	if (rtlhal->hal_coex_8723.high_priority_tx == 0 &&
-	    rtlhal->hal_coex_8723.high_priority_rx == 0 &&
-	    rtlhal->hal_coex_8723.low_priority_tx == 0 &&
-	    rtlhal->hal_coex_8723.low_priority_rx == 0)
+	if (hal_coex_8723.high_priority_tx == 0 &&
+	    hal_coex_8723.high_priority_rx == 0 &&
+	    hal_coex_8723.low_priority_tx == 0 &&
+	    hal_coex_8723.low_priority_rx == 0) {
 		bt_alife = false;
-	if (rtlhal->hal_coex_8723.high_priority_tx == 0xeaea &&
-	    rtlhal->hal_coex_8723.high_priority_rx == 0xeaea &&
-	    rtlhal->hal_coex_8723.low_priority_tx == 0xeaea &&
-	    rtlhal->hal_coex_8723.low_priority_rx == 0xeaea)
+	}
+	if (hal_coex_8723.high_priority_tx == 0xeaea &&
+	    hal_coex_8723.high_priority_rx == 0xeaea &&
+	    hal_coex_8723.low_priority_tx == 0xeaea &&
+	    hal_coex_8723.low_priority_rx == 0xeaea) {
 		bt_alife = false;
-	if (rtlhal->hal_coex_8723.high_priority_tx == 0xffff &&
-	    rtlhal->hal_coex_8723.high_priority_rx == 0xffff &&
-	    rtlhal->hal_coex_8723.low_priority_tx == 0xffff &&
-	    rtlhal->hal_coex_8723.low_priority_rx == 0xffff)
+	}
+	if (hal_coex_8723.high_priority_tx == 0xffff &&
+	    hal_coex_8723.high_priority_rx == 0xffff &&
+	    hal_coex_8723.low_priority_tx == 0xffff &&
+	    hal_coex_8723.low_priority_rx == 0xffff) {
 		bt_alife = false;
+	}
 	if (bt_alife) {
-		rtlpcipriv->bt_coexist.bt_active_zero_cnt = 0;
-		rtlpcipriv->bt_coexist.cur_bt_disabled = false;
+		rtlpriv->btcoexist.bt_active_zero_cnt = 0;
+		rtlpriv->btcoexist.cur_bt_disabled = false;
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "8723A BT is enabled !!\n");
 	} else {
-		rtlpcipriv->bt_coexist.bt_active_zero_cnt++;
+		rtlpriv->btcoexist.bt_active_zero_cnt++;
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "8723A bt all counters = 0, %d times!!\n",
-			 rtlpcipriv->bt_coexist.bt_active_zero_cnt);
-		if (rtlpcipriv->bt_coexist.bt_active_zero_cnt >= 2) {
-			rtlpcipriv->bt_coexist.cur_bt_disabled = true;
+			 "8723A bt all counters=0, %d times!!\n",
+			 rtlpriv->btcoexist.bt_active_zero_cnt);
+		if (rtlpriv->btcoexist.bt_active_zero_cnt >= 2) {
+			rtlpriv->btcoexist.cur_bt_disabled = true;
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 				 "8723A BT is disabled !!\n");
 		}
 	}
-	if (rtlpcipriv->bt_coexist.pre_bt_disabled !=
-		rtlpcipriv->bt_coexist.cur_bt_disabled) {
-		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "8723A BT is from %s to %s!!\n",
-			 (rtlpcipriv->bt_coexist.pre_bt_disabled ?
-			 "disabled" : "enabled"),
-			 (rtlpcipriv->bt_coexist.cur_bt_disabled ?
-			 "disabled" : "enabled"));
-		rtlpcipriv->bt_coexist.pre_bt_disabled
-			= rtlpcipriv->bt_coexist.cur_bt_disabled;
+	if (rtlpriv->btcoexist.pre_bt_disabled !=
+		rtlpriv->btcoexist.cur_bt_disabled) {
+		RT_TRACE(rtlpriv, COMP_BT_COEXIST,
+			 DBG_TRACE, "8723A BT is from %s to %s!!\n",
+			 (rtlpriv->btcoexist.pre_bt_disabled ?
+				"disabled" : "enabled"),
+			 (rtlpriv->btcoexist.cur_bt_disabled ?
+				"disabled" : "enabled"));
+		rtlpriv->btcoexist.pre_bt_disabled
+			= rtlpriv->btcoexist.cur_bt_disabled;
 	}
 }
 
 
-void rtl8723ae_dm_bt_coexist_8723(struct ieee80211_hw *hw)
+void rtl8723e_dm_bt_coexist_8723(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 
-	rtl8723ae_dm_bt_query_bt_information(hw);
-	rtl8723ae_dm_bt_bt_hw_counters_monitor(hw);
-	rtl8723ae_dm_bt_bt_enable_disable_check(hw);
+	rtl8723e_dm_bt_query_bt_information(hw);
+	rtl8723e_dm_bt_bt_hw_counters_monitor(hw);
+	rtl8723e_dm_bt_bt_enable_disable_check(hw);
 
-	if (rtlpcipriv->bt_coexist.bt_ant_num == ANT_X2) {
+	if (rtlpriv->btcoexist.bt_ant_num == ANT_X2) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTCoex], 2 Ant mechanism\n");
-		_rtl8723ae_dm_bt_coexist_2_ant(hw);
+			"[BTCoex], 2 Ant mechanism\n");
+		_rtl8723e_dm_bt_coexist_2_ant(hw);
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-			 "[BTCoex], 1 Ant mechanism\n");
-		_rtl8723ae_dm_bt_coexist_1_ant(hw);
+			"[BTCoex], 1 Ant mechanism\n");
+		_rtl8723e_dm_bt_coexist_1_ant(hw);
 	}
 
-	if (!rtl8723ae_dm_bt_is_same_coexist_state(hw)) {
+	if (!rtl8723e_dm_bt_is_same_coexist_state(hw)) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
 			 "[BTCoex], Coexist State[bitMap] change from 0x%x%8x to 0x%x%8x\n",
-			 rtlpcipriv->bt_coexist.previous_state_h,
-			 rtlpcipriv->bt_coexist.previous_state,
-			 rtlpcipriv->bt_coexist.cstate_h,
-			 rtlpcipriv->bt_coexist.cstate);
-		rtlpcipriv->bt_coexist.previous_state
-			= rtlpcipriv->bt_coexist.cstate;
-		rtlpcipriv->bt_coexist.previous_state_h
-			= rtlpcipriv->bt_coexist.cstate_h;
+			 rtlpriv->btcoexist.previous_state_h,
+			 rtlpriv->btcoexist.previous_state,
+			 rtlpriv->btcoexist.cstate_h,
+			 rtlpriv->btcoexist.cstate);
+		rtlpriv->btcoexist.previous_state
+			= rtlpriv->btcoexist.cstate;
+		rtlpriv->btcoexist.previous_state_h
+			= rtlpriv->btcoexist.cstate_h;
 	}
 }
 
-static void rtl8723ae_dm_bt_parse_bt_info(struct ieee80211_hw *hw,
-					  u8 *tmbuf, u8 len)
+static void rtl8723e_dm_bt_parse_bt_info(struct ieee80211_hw *hw,
+					 u8 *tmp_buf, u8 len)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
 	u8 bt_info;
 	u8 i;
 
-	rtlhal->hal_coex_8723.c2h_bt_info_req_sent = false;
-	rtlhal->hal_coex_8723.bt_retry_cnt = 0;
+	hal_coex_8723.c2h_bt_info_req_sent = false;
+	hal_coex_8723.bt_retry_cnt = 0;
 	for (i = 0; i < len; i++) {
 		if (i == 0)
-			rtlhal->hal_coex_8723.c2h_bt_info_original = tmbuf[i];
+			hal_coex_8723.c2h_bt_info_original = tmp_buf[i];
 		else if (i == 1)
-			rtlhal->hal_coex_8723.bt_retry_cnt = tmbuf[i];
-		if (i == len-1) {
+			hal_coex_8723.bt_retry_cnt = tmp_buf[i];
+		if (i == len-1)
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "0x%2x]", tmbuf[i]);
-		} else {
+				 "0x%2x]", tmp_buf[i]);
+		else
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
-				 "0x%2x, ", tmbuf[i]);
-		}
+				 "0x%2x, ", tmp_buf[i]);
+
 	}
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-		 "BT info bt_info (Data)= 0x%x\n",
-		 rtlhal->hal_coex_8723.c2h_bt_info_original);
-	bt_info = rtlhal->hal_coex_8723.c2h_bt_info_original;
+		"BT info bt_info (Data)= 0x%x\n",
+			hal_coex_8723.c2h_bt_info_original);
+	bt_info = hal_coex_8723.c2h_bt_info_original;
 
 	if (bt_info & BIT(2))
-		rtlhal->hal_coex_8723.c2h_bt_inquiry_page = true;
+		hal_coex_8723.c2h_bt_inquiry_page = true;
 	else
-		rtlhal->hal_coex_8723.c2h_bt_inquiry_page = false;
+		hal_coex_8723.c2h_bt_inquiry_page = false;
+
 
 	if (bt_info & BTINFO_B_CONNECTION) {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTC2H], BTInfo: bConnect=true\n");
-		rtlpcipriv->bt_coexist.bt_busy = true;
-		rtlpcipriv->bt_coexist.cstate &= ~BT_COEX_STATE_BT_IDLE;
+			"[BTC2H], BTInfo: bConnect=true\n");
+		rtlpriv->btcoexist.bt_busy = true;
+		rtlpriv->btcoexist.cstate &= ~BT_COEX_STATE_BT_IDLE;
 	} else {
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_DMESG,
-			 "[BTC2H], BTInfo: bConnect=false\n");
-		rtlpcipriv->bt_coexist.bt_busy = false;
-		rtlpcipriv->bt_coexist.cstate |= BT_COEX_STATE_BT_IDLE;
+			"[BTC2H], BTInfo: bConnect=false\n");
+		rtlpriv->btcoexist.bt_busy = false;
+		rtlpriv->btcoexist.cstate |= BT_COEX_STATE_BT_IDLE;
 	}
 }
 void rtl_8723e_c2h_command_handle(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct c2h_evt_hdr c2h_event;
-	u8 *ptmbuf;
-	u8 index;
-	u8 u1tmp;
-
+	u8 *ptmp_buf = NULL;
+	u8 index = 0;
+	u8 u1b_tmp = 0;
 	memset(&c2h_event, 0, sizeof(c2h_event));
-	u1tmp = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL);
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL);
 	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
-		 "&&&&&&: REG_C2HEVT_MSG_NORMAL is 0x%x\n", u1tmp);
-	c2h_event.cmd_id = u1tmp & 0xF;
-	c2h_event.cmd_len = (u1tmp & 0xF0) >> 4;
+		"&&&&&&: REG_C2HEVT_MSG_NORMAL is 0x%x\n", u1b_tmp);
+	c2h_event.cmd_id = u1b_tmp & 0xF;
+	c2h_event.cmd_len = (u1b_tmp & 0xF0) >> 4;
 	c2h_event.cmd_seq = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL + 1);
 	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
 		 "cmd_id: %d, cmd_len: %d, cmd_seq: %d\n",
 		 c2h_event.cmd_id , c2h_event.cmd_len, c2h_event.cmd_seq);
-	u1tmp = rtl_read_byte(rtlpriv, 0x01AF);
-	if (u1tmp == C2H_EVT_HOST_CLOSE) {
+	u1b_tmp = rtl_read_byte(rtlpriv, 0x01AF);
+	if (u1b_tmp == C2H_EVT_HOST_CLOSE) {
 		return;
-	} else if (u1tmp != C2H_EVT_FW_CLOSE) {
+	} else if (u1b_tmp != C2H_EVT_FW_CLOSE) {
 		rtl_write_byte(rtlpriv, 0x1AF, 0x00);
 		return;
 	}
-	ptmbuf = kmalloc(c2h_event.cmd_len, GFP_KERNEL);
-	if (ptmbuf == NULL) {
+	ptmp_buf = kzalloc(c2h_event.cmd_len, GFP_KERNEL);
+	if (ptmp_buf == NULL) {
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
 			 "malloc cmd buf failed\n");
 		return;
@@ -1757,30 +1744,37 @@ void rtl_8723e_c2h_command_handle(struct ieee80211_hw *hw)
 
 	/* Read the content */
 	for (index = 0; index < c2h_event.cmd_len; index++)
-		ptmbuf[index] = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL +
-				  2 + index);
+		ptmp_buf[index] = rtl_read_byte(rtlpriv,
+					REG_C2HEVT_MSG_NORMAL + 2 + index);
+
 
 	switch (c2h_event.cmd_id) {
 	case C2H_BT_RSSI:
-		break;
+			break;
 
 	case C2H_BT_OP_MODE:
 			break;
 
 	case BT_INFO:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-			 "BT info Byte[0] (ID) is 0x%x\n", c2h_event.cmd_id);
+			"BT info Byte[0] (ID) is 0x%x\n",
+			c2h_event.cmd_id);
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-			 "BT info Byte[1] (Seq) is 0x%x\n", c2h_event.cmd_seq);
+			"BT info Byte[1] (Seq) is 0x%x\n",
+			c2h_event.cmd_seq);
 		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-			 "BT info Byte[2] (Data)= 0x%x\n", ptmbuf[0]);
+			"BT info Byte[2] (Data)= 0x%x\n", ptmp_buf[0]);
+
+		rtl8723e_dm_bt_parse_bt_info(hw, ptmp_buf, c2h_event.cmd_len);
+
+		if (rtlpriv->cfg->ops->get_btc_status())
+			rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
 
-		rtl8723ae_dm_bt_parse_bt_info(hw, ptmbuf, c2h_event.cmd_len);
 		break;
 	default:
 		break;
 	}
-	kfree(ptmbuf);
+	kfree(ptmp_buf);
 
 	rtl_write_byte(rtlpriv, 0x01AF, C2H_EVT_HOST_CLOSE);
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.h b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.h
index 4325ecd58f0c..3723d7476717 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hal_btc.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -24,8 +20,7 @@
  * Hsinchu 300, Taiwan.
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
- ****************************************************************************
- */
+ *****************************************************************************/
 
 #ifndef __RTL8723E_HAL_BTC_H__
 #define __RTL8723E_HAL_BTC_H__
@@ -34,21 +29,31 @@
 #include "btc.h"
 #include "hal_bt_coexist.h"
 
-#define	BT_TXRX_CNT_THRES_1		1200
-#define	BT_TXRX_CNT_THRES_2		1400
-#define	BT_TXRX_CNT_THRES_3		3000
-#define	BT_TXRX_CNT_LEVEL_0		0	/* < 1200 */
-#define	BT_TXRX_CNT_LEVEL_1		1	/* >= 1200 && < 1400 */
-#define	BT_TXRX_CNT_LEVEL_2		2	/* >= 1400 */
-#define	BT_TXRX_CNT_LEVEL_3		3
+#define	BT_TXRX_CNT_THRES_1			1200
+#define	BT_TXRX_CNT_THRES_2			1400
+#define	BT_TXRX_CNT_THRES_3			3000
+/* < 1200 */
+#define	BT_TXRX_CNT_LEVEL_0			0
+/* >= 1200 && < 1400 */
+#define	BT_TXRX_CNT_LEVEL_1			1
+/* >= 1400 */
+#define	BT_TXRX_CNT_LEVEL_2			2
+#define	BT_TXRX_CNT_LEVEL_3			3
+
+#define	BT_COEX_DISABLE			0
+#define	BT_Q_PKT_OFF			0
+#define	BT_Q_PKT_ON			1
+
+#define	BT_TX_PWR_OFF			0
+#define	BT_TX_PWR_ON			1
 
 /* TDMA mode definition */
-#define	TDMA_2ANT		0
-#define	TDMA_1ANT		1
-#define	TDMA_NAV_OFF		0
-#define	TDMA_NAV_ON		1
-#define	TDMA_DAC_SWING_OFF	0
-#define	TDMA_DAC_SWING_ON	1
+#define	TDMA_2ANT			0
+#define	TDMA_1ANT			1
+#define	TDMA_NAV_OFF			0
+#define	TDMA_NAV_ON			1
+#define	TDMA_DAC_SWING_OFF		0
+#define	TDMA_DAC_SWING_ON		1
 
 /* PTA mode related definition */
 #define	BT_PTA_MODE_OFF		0
@@ -80,6 +85,7 @@ enum bt_traffic_mode_profile {
 	BT_PROFILE_SCO
 };
 
+/*
 enum hci_ext_bt_operation {
 	HCI_BT_OP_NONE = 0x0,
 	HCI_BT_OP_INQUIRE_START	= 0x1,
@@ -93,6 +99,7 @@ enum hci_ext_bt_operation {
 	HCI_BT_OP_BT_DEV_DISABLE = 0x9,
 	HCI_BT_OP_MAX,
 };
+*/
 
 enum bt_spec {
 	BT_SPEC_1_0_b = 0x00,
@@ -123,12 +130,12 @@ enum bt_state {
 	BT_INFO_STATE_MAX = 7
 };
 
-enum rtl8723ae_c2h_evt {
+enum rtl8723e_c2h_evt {
 	C2H_DBG = 0,
 	C2H_TSF = 1,
 	C2H_AP_RPT_RSP = 2,
-	C2H_CCX_TX_RPT = 3,	/* The FW notify the report of the specific */
-				/* tx packet. */
+	/* The FW notify the report of the specific tx packet. */
+	C2H_CCX_TX_RPT = 3,
 	C2H_BT_RSSI = 4,
 	C2H_BT_OP_MODE = 5,
 	C2H_HW_INFO_EXCH = 10,
@@ -137,15 +144,16 @@ enum rtl8723ae_c2h_evt {
 	MAX_C2HEVENT
 };
 
-void rtl8723ae_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_coexist_8723(struct ieee80211_hw *hw);
-void rtl8723ae_dm_bt_set_bt_dm(struct ieee80211_hw *hw,
+void rtl8723e_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_coexist_8723(struct ieee80211_hw *hw);
+void rtl8723e_dm_bt_set_bt_dm(struct ieee80211_hw *hw,
 			      struct btdm_8723 *p_btdm);
 void rtl_8723e_c2h_command_handle(struct ieee80211_hw *hw);
 void rtl_8723e_bt_wifi_media_status_notify(struct ieee80211_hw *hw,
-					   bool mstatus);
-void rtl8723ae_bt_coex_off_before_lps(struct ieee80211_hw *hw);
+					bool mstatus);
+void rtl8723e_dm_bt_turn_off_bt_coexist_before_enter_lps(
+				struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
index 662a079f76f3..aa085462d0e9 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -37,17 +33,21 @@
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
+#include "../rtl8723com/phy_common.h"
 #include "dm.h"
 #include "../rtl8723com/dm_common.h"
 #include "fw.h"
 #include "../rtl8723com/fw_common.h"
 #include "led.h"
 #include "hw.h"
+#include "../pwrseqcmd.h"
 #include "pwrseq.h"
 #include "btc.h"
 
-static void _rtl8723ae_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
-					u8 set_bits, u8 clear_bits)
+#define LLT_CONFIG	5
+
+static void _rtl8723e_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
+				       u8 set_bits, u8 clear_bits)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -58,7 +58,7 @@ static void _rtl8723ae_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
 	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
 }
 
-static void _rtl8723ae_stop_tx_beacon(struct ieee80211_hw *hw)
+static void _rtl8723e_stop_tx_beacon(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmp1byte;
@@ -71,7 +71,7 @@ static void _rtl8723ae_stop_tx_beacon(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
 }
 
-static void _rtl8723ae_resume_tx_beacon(struct ieee80211_hw *hw)
+static void _rtl8723e_resume_tx_beacon(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmp1byte;
@@ -84,17 +84,17 @@ static void _rtl8723ae_resume_tx_beacon(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
 }
 
-static void _rtl8723ae_enable_bcn_sufunc(struct ieee80211_hw *hw)
+static void _rtl8723e_enable_bcn_sub_func(struct ieee80211_hw *hw)
 {
-	_rtl8723ae_set_bcn_ctrl_reg(hw, 0, BIT(1));
+	_rtl8723e_set_bcn_ctrl_reg(hw, 0, BIT(1));
 }
 
-static void _rtl8723ae_disable_bcn_sufunc(struct ieee80211_hw *hw)
+static void _rtl8723e_disable_bcn_sub_func(struct ieee80211_hw *hw)
 {
-	_rtl8723ae_set_bcn_ctrl_reg(hw, BIT(1), 0);
+	_rtl8723e_set_bcn_ctrl_reg(hw, BIT(1), 0);
 }
 
-void rtl8723ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+void rtl8723e_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
@@ -102,54 +102,55 @@ void rtl8723ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 	switch (variable) {
 	case HW_VAR_RCR:
-		*((u32 *) (val)) = rtlpci->receive_config;
+		*((u32 *)(val)) = rtlpci->receive_config;
 		break;
 	case HW_VAR_RF_STATE:
 		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
 		break;
 	case HW_VAR_FWLPS_RF_ON:{
-		enum rf_pwrstate rfState;
-		u32 val_rcr;
-
-		rtlpriv->cfg->ops->get_hw_reg(hw,
-					      HW_VAR_RF_STATE,
-					      (u8 *) (&rfState));
-		if (rfState == ERFOFF) {
-			*((bool *) (val)) = true;
-		} else {
-			val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
-			val_rcr &= 0x00070000;
-			if (val_rcr)
-				*((bool *) (val)) = false;
-			else
-				*((bool *) (val)) = true;
+			enum rf_pwrstate rfstate;
+			u32 val_rcr;
+
+			rtlpriv->cfg->ops->get_hw_reg(hw,
+						      HW_VAR_RF_STATE,
+						      (u8 *)(&rfstate));
+			if (rfstate == ERFOFF) {
+				*((bool *)(val)) = true;
+			} else {
+				val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+				val_rcr &= 0x00070000;
+				if (val_rcr)
+					*((bool *)(val)) = false;
+				else
+					*((bool *)(val)) = true;
+			}
+			break;
 		}
-		break; }
 	case HW_VAR_FW_PSMODE_STATUS:
-		*((bool *) (val)) = ppsc->fw_current_inpsmode;
+		*((bool *)(val)) = ppsc->fw_current_inpsmode;
 		break;
 	case HW_VAR_CORRECT_TSF:{
-		u64 tsf;
-		u32 *ptsf_low = (u32 *)&tsf;
-		u32 *ptsf_high = ((u32 *)&tsf) + 1;
+			u64 tsf;
+			u32 *ptsf_low = (u32 *)&tsf;
+			u32 *ptsf_high = ((u32 *)&tsf) + 1;
 
-		*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
-		*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+			*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
+			*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 
-		*((u64 *) (val)) = tsf;
+			*((u64 *)(val)) = tsf;
 
-		break; }
+			break;
+		}
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 			 "switch case not process\n");
 		break;
 	}
 }
 
-void rtl8723ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+void rtl8723e_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
@@ -157,362 +158,400 @@ void rtl8723ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	u8 idx;
 
 	switch (variable) {
-	case HW_VAR_ETHER_ADDR:
-		for (idx = 0; idx < ETH_ALEN; idx++) {
-			rtl_write_byte(rtlpriv, (REG_MACID + idx),
-				       val[idx]);
+	case HW_VAR_ETHER_ADDR:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_MACID + idx),
+					       val[idx]);
+			}
+			break;
 		}
-		break;
 	case HW_VAR_BASIC_RATE:{
-		u16 rate_cfg = ((u16 *) val)[0];
-		u8 rate_index = 0;
-		rate_cfg = rate_cfg & 0x15f;
-		rate_cfg |= 0x01;
-		rtl_write_byte(rtlpriv, REG_RRSR, rate_cfg & 0xff);
-		rtl_write_byte(rtlpriv, REG_RRSR + 1,
-			       (rate_cfg >> 8) & 0xff);
-		while (rate_cfg > 0x1) {
-			rate_cfg = (rate_cfg >> 1);
-			rate_index++;
+			u16 b_rate_cfg = ((u16 *)val)[0];
+			u8 rate_index = 0;
+
+			b_rate_cfg = b_rate_cfg & 0x15f;
+			b_rate_cfg |= 0x01;
+			rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
+			rtl_write_byte(rtlpriv, REG_RRSR + 1,
+				       (b_rate_cfg >> 8) & 0xff);
+			while (b_rate_cfg > 0x1) {
+				b_rate_cfg = (b_rate_cfg >> 1);
+				rate_index++;
+			}
+			rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL,
+				       rate_index);
+			break;
 		}
-		rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL,
-			       rate_index);
-		break; }
-	case HW_VAR_BSSID:
-		for (idx = 0; idx < ETH_ALEN; idx++) {
-			rtl_write_byte(rtlpriv, (REG_BSSID + idx),
-				       val[idx]);
+	case HW_VAR_BSSID:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_BSSID + idx),
+					       val[idx]);
+			}
+			break;
 		}
-		break;
-	case HW_VAR_SIFS:
-		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
-		rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[1]);
+	case HW_VAR_SIFS:{
+			rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[1]);
 
-		rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
-		rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
+			rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
 
-		if (!mac->ht_enable)
-			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
-				       0x0e0e);
-		else
-			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
-				       *((u16 *) val));
-		break;
+			if (!mac->ht_enable)
+				rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+					       0x0e0e);
+			else
+				rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
+					       *((u16 *)val));
+			break;
+		}
 	case HW_VAR_SLOT_TIME:{
-		u8 e_aci;
+			u8 e_aci;
 
-		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
-			 "HW_VAR_SLOT_TIME %x\n", val[0]);
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "HW_VAR_SLOT_TIME %x\n", val[0]);
 
-		rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
+			rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
 
-		for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      &e_aci);
+			for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							      HW_VAR_AC_PARAM,
+							      (u8 *)(&e_aci));
+			}
+			break;
 		}
-		break; }
 	case HW_VAR_ACK_PREAMBLE:{
-		u8 reg_tmp;
-		u8 short_preamble = (bool)*val;
-		reg_tmp = (mac->cur_40_prime_sc) << 5;
-		if (short_preamble)
-			reg_tmp |= 0x80;
-
-		rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_tmp);
-		break; }
+			u8 reg_tmp;
+			u8 short_preamble = (bool)(*(u8 *)val);
+
+			reg_tmp = (mac->cur_40_prime_sc) << 5;
+			if (short_preamble)
+				reg_tmp |= 0x80;
+
+			rtl_write_byte(rtlpriv, REG_RRSR + 2, reg_tmp);
+			break;
+		}
 	case HW_VAR_AMPDU_MIN_SPACE:{
-		u8 min_spacing_to_set;
-		u8 sec_min_space;
+			u8 min_spacing_to_set;
+			u8 sec_min_space;
 
-		min_spacing_to_set = *val;
-		if (min_spacing_to_set <= 7) {
-			sec_min_space = 0;
+			min_spacing_to_set = *((u8 *)val);
+			if (min_spacing_to_set <= 7) {
+				sec_min_space = 0;
 
-			if (min_spacing_to_set < sec_min_space)
-				min_spacing_to_set = sec_min_space;
+				if (min_spacing_to_set < sec_min_space)
+					min_spacing_to_set = sec_min_space;
 
-			mac->min_space_cfg = ((mac->min_space_cfg &
-					       0xf8) |
-					      min_spacing_to_set);
+				mac->min_space_cfg = ((mac->min_space_cfg &
+						       0xf8) |
+						      min_spacing_to_set);
 
-			*val = min_spacing_to_set;
+				*val = min_spacing_to_set;
 
-			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
-				 "Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
-				  mac->min_space_cfg);
+				RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+					 "Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
+					  mac->min_space_cfg);
 
-			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
-				       mac->min_space_cfg);
+				rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+					       mac->min_space_cfg);
+			}
+			break;
 		}
-		break; }
 	case HW_VAR_SHORTGI_DENSITY:{
-		u8 density_to_set;
+			u8 density_to_set;
 
-		density_to_set = *val;
-		mac->min_space_cfg |= (density_to_set << 3);
+			density_to_set = *((u8 *)val);
+			mac->min_space_cfg |= (density_to_set << 3);
 
-		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
-			 "Set HW_VAR_SHORTGI_DENSITY: %#x\n",
-			 mac->min_space_cfg);
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "Set HW_VAR_SHORTGI_DENSITY: %#x\n",
+				  mac->min_space_cfg);
 
-		rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
-			       mac->min_space_cfg);
+			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+				       mac->min_space_cfg);
 
-		break; }
+			break;
+		}
 	case HW_VAR_AMPDU_FACTOR:{
-		u8 regtoset_normal[4] = {0x41, 0xa8, 0x72, 0xb9};
-		u8 regtoset_bt[4] = {0x31, 0x74, 0x42, 0x97};
-		u8 factor_toset;
-		u8 *p_regtoset = NULL;
-		u8 index;
-
-		if ((pcipriv->bt_coexist.bt_coexistence) &&
-		    (pcipriv->bt_coexist.bt_coexist_type == BT_CSR_BC4))
-			p_regtoset = regtoset_bt;
-		else
-			p_regtoset = regtoset_normal;
-
-		factor_toset = *val;
-		if (factor_toset <= 3) {
-			factor_toset = (1 << (factor_toset + 2));
-			if (factor_toset > 0xf)
-				factor_toset = 0xf;
-
-			for (index = 0; index < 4; index++) {
-				if ((p_regtoset[index] & 0xf0) >
-				    (factor_toset << 4))
-					p_regtoset[index] =
-					    (p_regtoset[index] & 0x0f) |
-					    (factor_toset << 4);
-
-				if ((p_regtoset[index] & 0x0f) >
-				    factor_toset)
-					p_regtoset[index] =
-					    (p_regtoset[index] & 0xf0) |
-					    (factor_toset);
-
-				rtl_write_byte(rtlpriv,
-					       (REG_AGGLEN_LMT + index),
-					       p_regtoset[index]);
+			u8 regtoset_normal[4] = { 0x41, 0xa8, 0x72, 0xb9 };
+			u8 regtoset_bt[4] = {0x31, 0x74, 0x42, 0x97};
+			u8 factor_toset;
+			u8 *p_regtoset = NULL;
+			u8 index = 0;
+
+			if ((rtlpriv->btcoexist.bt_coexistence) &&
+			    (rtlpriv->btcoexist.bt_coexist_type ==
+				BT_CSR_BC4))
+				p_regtoset = regtoset_bt;
+			else
+				p_regtoset = regtoset_normal;
+
+			factor_toset = *((u8 *)val);
+			if (factor_toset <= 3) {
+				factor_toset = (1 << (factor_toset + 2));
+				if (factor_toset > 0xf)
+					factor_toset = 0xf;
+
+				for (index = 0; index < 4; index++) {
+					if ((p_regtoset[index] & 0xf0) >
+					    (factor_toset << 4))
+						p_regtoset[index] =
+						    (p_regtoset[index] & 0x0f) |
+						    (factor_toset << 4);
+
+					if ((p_regtoset[index] & 0x0f) >
+					    factor_toset)
+						p_regtoset[index] =
+						    (p_regtoset[index] & 0xf0) |
+						    (factor_toset);
+
+					rtl_write_byte(rtlpriv,
+						       (REG_AGGLEN_LMT + index),
+						       p_regtoset[index]);
+				}
 
+				RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+					 "Set HW_VAR_AMPDU_FACTOR: %#x\n",
+					  factor_toset);
 			}
-
-			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
-				 "Set HW_VAR_AMPDU_FACTOR: %#x\n",
-				 factor_toset);
+			break;
 		}
-		break; }
 	case HW_VAR_AC_PARAM:{
-		u8 e_aci = *val;
-		rtl8723_dm_init_edca_turbo(hw);
+			u8 e_aci = *((u8 *)val);
 
-		if (rtlpci->acm_method != EACMWAY2_SW)
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
-						      &e_aci);
-		break; }
+			rtl8723_dm_init_edca_turbo(hw);
+
+			if (rtlpci->acm_method != EACMWAY2_SW)
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							      HW_VAR_ACM_CTRL,
+							      (u8 *)(&e_aci));
+			break;
+		}
 	case HW_VAR_ACM_CTRL:{
-		u8 e_aci = *val;
-		union aci_aifsn *p_aci_aifsn =
-		    (union aci_aifsn *)(&(mac->ac[0].aifs));
-		u8 acm = p_aci_aifsn->f.acm;
-		u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
-
-		acm_ctrl |= ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
-
-		if (acm) {
-			switch (e_aci) {
-			case AC0_BE:
-				acm_ctrl |= AcmHw_BeqEn;
-				break;
-			case AC2_VI:
-				acm_ctrl |= AcmHw_ViqEn;
-				break;
-			case AC3_VO:
-				acm_ctrl |= AcmHw_VoqEn;
-				break;
-			default:
-				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "HW_VAR_ACM_CTRL acm set failed: eACI is %d\n",
-					 acm);
-				break;
-			}
-		} else {
-			switch (e_aci) {
-			case AC0_BE:
-				acm_ctrl &= (~AcmHw_BeqEn);
-				break;
-			case AC2_VI:
-				acm_ctrl &= (~AcmHw_ViqEn);
-				break;
-			case AC3_VO:
-				acm_ctrl &= (~AcmHw_BeqEn);
-				break;
-			default:
-				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-					 "switch case not processed\n");
-				break;
+			u8 e_aci = *((u8 *)val);
+			union aci_aifsn *p_aci_aifsn =
+			    (union aci_aifsn *)(&mac->ac[0].aifs);
+			u8 acm = p_aci_aifsn->f.acm;
+			u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
+
+			acm_ctrl =
+			    acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
+
+			if (acm) {
+				switch (e_aci) {
+				case AC0_BE:
+					acm_ctrl |= ACMHW_BEQEN;
+					break;
+				case AC2_VI:
+					acm_ctrl |= ACMHW_VIQEN;
+					break;
+				case AC3_VO:
+					acm_ctrl |= ACMHW_VOQEN;
+					break;
+				default:
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+						 "HW_VAR_ACM_CTRL acm set failed: eACI is %d\n",
+						 acm);
+					break;
+				}
+			} else {
+				switch (e_aci) {
+				case AC0_BE:
+					acm_ctrl &= (~ACMHW_BEQEN);
+					break;
+				case AC2_VI:
+					acm_ctrl &= (~ACMHW_VIQEN);
+					break;
+				case AC3_VO:
+					acm_ctrl &= (~ACMHW_BEQEN);
+					break;
+				default:
+					RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+						 "switch case not process\n");
+					break;
+				}
 			}
-		}
 
-		RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
-			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
-			 acm_ctrl);
-		rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
-		break; }
-	case HW_VAR_RCR:
-		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *) (val))[0]);
-		rtlpci->receive_config = ((u32 *) (val))[0];
-		break;
+			RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
+				 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
+				 acm_ctrl);
+			rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
+			break;
+		}
+	case HW_VAR_RCR:{
+			rtl_write_dword(rtlpriv, REG_RCR, ((u32 *)(val))[0]);
+			rtlpci->receive_config = ((u32 *)(val))[0];
+			break;
+		}
 	case HW_VAR_RETRY_LIMIT:{
-		u8 retry_limit = *val;
+			u8 retry_limit = ((u8 *)(val))[0];
 
-		rtl_write_word(rtlpriv, REG_RL,
-			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
-			       retry_limit << RETRY_LIMIT_LONG_SHIFT);
-		break; }
+			rtl_write_word(rtlpriv, REG_RL,
+				       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
+				       retry_limit << RETRY_LIMIT_LONG_SHIFT);
+			break;
+		}
 	case HW_VAR_DUAL_TSF_RST:
 		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
 		break;
 	case HW_VAR_EFUSE_BYTES:
-		rtlefuse->efuse_usedbytes = *((u16 *) val);
+		rtlefuse->efuse_usedbytes = *((u16 *)val);
 		break;
 	case HW_VAR_EFUSE_USAGE:
-		rtlefuse->efuse_usedpercentage = *val;
+		rtlefuse->efuse_usedpercentage = *((u8 *)val);
 		break;
 	case HW_VAR_IO_CMD:
-		rtl8723ae_phy_set_io_cmd(hw, (*(enum io_type *)val));
+		rtl8723e_phy_set_io_cmd(hw, (*(enum io_type *)val));
 		break;
 	case HW_VAR_WPA_CONFIG:
-		rtl_write_byte(rtlpriv, REG_SECCFG, *val);
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *)val));
 		break;
 	case HW_VAR_SET_RPWM:{
-		u8 rpwm_val;
+			u8 rpwm_val;
 
-		rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
-		udelay(1);
+			rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
+			udelay(1);
 
-		if (rpwm_val & BIT(7)) {
-			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, *val);
-		} else {
-			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, *val | BIT(7));
-		}
+			if (rpwm_val & BIT(7)) {
+				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+					       (*(u8 *)val));
+			} else {
+				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+					       ((*(u8 *)val) | BIT(7)));
+			}
 
-		break; }
+			break;
+		}
 	case HW_VAR_H2C_FW_PWRMODE:{
-		u8 psmode = *val;
+			u8 psmode = (*(u8 *)val);
 
-		if (psmode != FW_PS_ACTIVE_MODE)
-			rtl8723ae_dm_rf_saving(hw, true);
+			if (psmode != FW_PS_ACTIVE_MODE)
+				rtl8723e_dm_rf_saving(hw, true);
 
-		rtl8723ae_set_fw_pwrmode_cmd(hw, *val);
-		break; }
+			rtl8723e_set_fw_pwrmode_cmd(hw, (*(u8 *)val));
+			break;
+		}
 	case HW_VAR_FW_PSMODE_STATUS:
-		ppsc->fw_current_inpsmode = *((bool *) val);
+		ppsc->fw_current_inpsmode = *((bool *)val);
 		break;
 	case HW_VAR_H2C_FW_JOINBSSRPT:{
-		u8 mstatus = *val;
-		u8 tmp_regcr, tmp_reg422;
-		bool recover = false;
-
-		if (mstatus == RT_MEDIA_CONNECT) {
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
-
-			tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
-			rtl_write_byte(rtlpriv, REG_CR + 1,
-				       (tmp_regcr | BIT(0)));
-
-			_rtl8723ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
-			_rtl8723ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
+			u8 mstatus = (*(u8 *)val);
+			u8 tmp_regcr, tmp_reg422;
+			bool b_recover = false;
+
+			if (mstatus == RT_MEDIA_CONNECT) {
+				rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID,
+							      NULL);
+
+				tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+				rtl_write_byte(rtlpriv, REG_CR + 1,
+					       (tmp_regcr | BIT(0)));
+
+				_rtl8723e_set_bcn_ctrl_reg(hw, 0, BIT(3));
+				_rtl8723e_set_bcn_ctrl_reg(hw, BIT(4), 0);
+
+				tmp_reg422 =
+				    rtl_read_byte(rtlpriv,
+						  REG_FWHW_TXQ_CTRL + 2);
+				if (tmp_reg422 & BIT(6))
+					b_recover = true;
+				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+					       tmp_reg422 & (~BIT(6)));
 
-			tmp_reg422 = rtl_read_byte(rtlpriv,
-				     REG_FWHW_TXQ_CTRL + 2);
-			if (tmp_reg422 & BIT(6))
-				recover = true;
-			rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
-				       tmp_reg422 & (~BIT(6)));
+				rtl8723e_set_fw_rsvdpagepkt(hw, 0);
 
-			rtl8723ae_set_fw_rsvdpagepkt(hw, 0);
+				_rtl8723e_set_bcn_ctrl_reg(hw, BIT(3), 0);
+				_rtl8723e_set_bcn_ctrl_reg(hw, 0, BIT(4));
 
-			_rtl8723ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
-			_rtl8723ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
+				if (b_recover) {
+					rtl_write_byte(rtlpriv,
+						       REG_FWHW_TXQ_CTRL + 2,
+						       tmp_reg422);
+				}
 
-			if (recover)
-				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
-					       tmp_reg422);
+				rtl_write_byte(rtlpriv, REG_CR + 1,
+					       (tmp_regcr & ~(BIT(0))));
+			}
+			rtl8723e_set_fw_joinbss_report_cmd(hw, (*(u8 *)val));
 
-			rtl_write_byte(rtlpriv, REG_CR + 1,
-				       (tmp_regcr & ~(BIT(0))));
+			break;
 		}
-		rtl8723ae_set_fw_joinbss_report_cmd(hw, *val);
-
-		break; }
-	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
-		rtl8723ae_set_p2p_ps_offload_cmd(hw, *val);
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:{
+		rtl8723e_set_p2p_ps_offload_cmd(hw, (*(u8 *)val));
 		break;
+	}
 	case HW_VAR_AID:{
-		u16 u2btmp;
-		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
-		u2btmp &= 0xC000;
-		rtl_write_word(rtlpriv, REG_BCN_PSR_RPT, (u2btmp |
-				mac->assoc_id));
-		break; }
+			u16 u2btmp;
+
+			u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
+			u2btmp &= 0xC000;
+			rtl_write_word(rtlpriv, REG_BCN_PSR_RPT,
+				       (u2btmp | mac->assoc_id));
+
+			break;
+		}
 	case HW_VAR_CORRECT_TSF:{
-		u8 btype_ibss = *val;
-
-		if (btype_ibss == true)
-			_rtl8723ae_stop_tx_beacon(hw);
-
-		_rtl8723ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
-
-		rtl_write_dword(rtlpriv, REG_TSFTR,
-				(u32) (mac->tsf & 0xffffffff));
-		rtl_write_dword(rtlpriv, REG_TSFTR + 4,
-				(u32) ((mac->tsf >> 32) & 0xffffffff));
-
-		_rtl8723ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
-
-		if (btype_ibss == true)
-			_rtl8723ae_resume_tx_beacon(hw);
-		break; }
-	case HW_VAR_FW_LPS_ACTION: {
-		bool enter_fwlps = *((bool *)val);
-		u8 rpwm_val, fw_pwrmode;
-		bool fw_current_inps;
-
-		if (enter_fwlps) {
-			rpwm_val = 0x02;	/* RF off */
-			fw_current_inps = true;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-					HW_VAR_FW_PSMODE_STATUS,
-					(u8 *)(&fw_current_inps));
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-					HW_VAR_H2C_FW_PWRMODE,
-					&ppsc->fwctrl_psmode);
-
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-						      &rpwm_val);
-		} else {
-			rpwm_val = 0x0C;	/* RF on */
-			fw_pwrmode = FW_PS_ACTIVE_MODE;
-			fw_current_inps = false;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-						      &rpwm_val);
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-						      &fw_pwrmode);
-
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-					HW_VAR_FW_PSMODE_STATUS,
-					(u8 *)(&fw_current_inps));
+			u8 btype_ibss = ((u8 *)(val))[0];
+
+			if (btype_ibss)
+				_rtl8723e_stop_tx_beacon(hw);
+
+			_rtl8723e_set_bcn_ctrl_reg(hw, 0, BIT(3));
+
+			rtl_write_dword(rtlpriv, REG_TSFTR,
+					(u32)(mac->tsf & 0xffffffff));
+			rtl_write_dword(rtlpriv, REG_TSFTR + 4,
+					(u32)((mac->tsf >> 32) & 0xffffffff));
+
+			_rtl8723e_set_bcn_ctrl_reg(hw, BIT(3), 0);
+
+			if (btype_ibss)
+				_rtl8723e_resume_tx_beacon(hw);
+
+			break;
+		}
+	case HW_VAR_FW_LPS_ACTION:{
+			bool b_enter_fwlps = *((bool *)val);
+			u8 rpwm_val, fw_pwrmode;
+			bool fw_current_inps;
+
+			if (b_enter_fwlps) {
+				rpwm_val = 0x02;	/* RF off */
+				fw_current_inps = true;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_FW_PSMODE_STATUS,
+						(u8 *)(&fw_current_inps));
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_PWRMODE,
+						(u8 *)(&ppsc->fwctrl_psmode));
+
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_SET_RPWM,
+						(u8 *)(&rpwm_val));
+			} else {
+				rpwm_val = 0x0C;	/* RF on */
+				fw_pwrmode = FW_PS_ACTIVE_MODE;
+				fw_current_inps = false;
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+							      HW_VAR_SET_RPWM,
+							      (u8 *)(&rpwm_val));
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_H2C_FW_PWRMODE,
+						(u8 *)(&fw_pwrmode));
+
+				rtlpriv->cfg->ops->set_hw_reg(hw,
+						HW_VAR_FW_PSMODE_STATUS,
+						(u8 *)(&fw_current_inps));
+			}
+			 break;
 		}
-		break; }
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
 }
 
-static bool _rtl8723ae_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
+static bool _rtl8723e_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	bool status = true;
@@ -539,24 +578,49 @@ static bool _rtl8723ae_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
 	return status;
 }
 
-static bool _rtl8723ae_llt_table_init(struct ieee80211_hw *hw)
+static bool _rtl8723e_llt_table_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	unsigned short i;
 	u8 txpktbuf_bndy;
-	u8 maxPage;
+	u8 maxpage;
 	bool status;
 	u8 ubyte;
 
-	maxPage = 255;
+#if LLT_CONFIG == 1
+	maxpage = 255;
+	txpktbuf_bndy = 252;
+#elif LLT_CONFIG == 2
+	maxpage = 127;
+	txpktbuf_bndy = 124;
+#elif LLT_CONFIG == 3
+	maxpage = 255;
+	txpktbuf_bndy = 174;
+#elif LLT_CONFIG == 4
+	maxpage = 255;
 	txpktbuf_bndy = 246;
+#elif LLT_CONFIG == 5
+	maxpage = 255;
+	txpktbuf_bndy = 246;
+#endif
 
 	rtl_write_byte(rtlpriv, REG_CR, 0x8B);
 
+#if LLT_CONFIG == 1
+	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x1c);
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x80a71c1c);
+#elif LLT_CONFIG == 2
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x845B1010);
+#elif LLT_CONFIG == 3
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x84838484);
+#elif LLT_CONFIG == 4
+	rtl_write_dword(rtlpriv, REG_RQPN, 0x80bd1c1c);
+#elif LLT_CONFIG == 5
 	rtl_write_word(rtlpriv, REG_RQPN_NPQ, 0x0000);
 
 	rtl_write_dword(rtlpriv, REG_RQPN, 0x80ac1c29);
 	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x03);
+#endif
 
 	rtl_write_dword(rtlpriv, REG_TRXFF_BNDY, (0x27FF0000 | txpktbuf_bndy));
 	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, txpktbuf_bndy);
@@ -569,22 +633,22 @@ static bool _rtl8723ae_llt_table_init(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
 
 	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
-		status = _rtl8723ae_llt_write(hw, i, i + 1);
+		status = _rtl8723e_llt_write(hw, i, i + 1);
 		if (true != status)
 			return status;
 	}
 
-	status = _rtl8723ae_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
+	status = _rtl8723e_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
 	if (true != status)
 		return status;
 
-	for (i = txpktbuf_bndy; i < maxPage; i++) {
-		status = _rtl8723ae_llt_write(hw, i, (i + 1));
+	for (i = txpktbuf_bndy; i < maxpage; i++) {
+		status = _rtl8723e_llt_write(hw, i, (i + 1));
 		if (true != status)
 			return status;
 	}
 
-	status = _rtl8723ae_llt_write(hw, maxPage, txpktbuf_bndy);
+	status = _rtl8723e_llt_write(hw, maxpage, txpktbuf_bndy);
 	if (true != status)
 		return status;
 
@@ -595,28 +659,29 @@ static bool _rtl8723ae_llt_table_init(struct ieee80211_hw *hw)
 	return true;
 }
 
-static void _rtl8723ae_gen_refresh_led_state(struct ieee80211_hw *hw)
+static void _rtl8723e_gen_refresh_led_state(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+	struct rtl_led *pled0 = &pcipriv->ledctl.sw_led0;
 
 	if (rtlpriv->rtlhal.up_first_time)
 		return;
 
 	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
-		rtl8723ae_sw_led_on(hw, pLed0);
+		rtl8723e_sw_led_on(hw, pled0);
 	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
-		rtl8723ae_sw_led_on(hw, pLed0);
+		rtl8723e_sw_led_on(hw, pled0);
 	else
-		rtl8723ae_sw_led_off(hw, pLed0);
+		rtl8723e_sw_led_off(hw, pled0);
 }
 
 static bool _rtl8712e_init_mac(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
 	unsigned char bytetmp;
 	unsigned short wordtmp;
 	u16 retry = 0;
@@ -630,7 +695,6 @@ static bool _rtl8712e_init_mac(struct ieee80211_hw *hw)
 	else
 		mac_func_enable = false;
 
-
 	/* HW Power on sequence */
 	if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
 		PWR_INTF_PCI_MSK, Rtl8723_NIC_ENABLE_FLOW))
@@ -669,7 +733,7 @@ static bool _rtl8712e_init_mac(struct ieee80211_hw *hw)
 	rtl_write_word(rtlpriv, REG_CR + 1, 0x06);
 
 	if (!mac_func_enable) {
-		if (_rtl8723ae_llt_table_init(hw) == false)
+		if (!_rtl8723e_llt_table_init(hw))
 			return false;
 	}
 
@@ -678,7 +742,8 @@ static bool _rtl8712e_init_mac(struct ieee80211_hw *hw)
 
 	rtl_write_word(rtlpriv, REG_TRXFF_BNDY + 2, 0x27ff);
 
-	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL) & 0xf;
+	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
+	wordtmp &= 0xf;
 	wordtmp |= 0xF771;
 	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
 
@@ -721,22 +786,23 @@ static bool _rtl8712e_init_mac(struct ieee80211_hw *hw)
 		bytetmp = rtl_read_byte(rtlpriv, REG_APSD_CTRL);
 	} while ((retry < 200) && (bytetmp & BIT(7)));
 
-	_rtl8723ae_gen_refresh_led_state(hw);
+	_rtl8723e_gen_refresh_led_state(hw);
 
 	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
 
 	return true;
 }
 
-static void _rtl8723ae_hw_configure(struct ieee80211_hw *hw)
+static void _rtl8723e_hw_configure(struct ieee80211_hw *hw)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	u8 reg_bw_opmode;
-	u32 reg_prsr;
+	u32 reg_ratr, reg_prsr;
 
 	reg_bw_opmode = BW_OPMODE_20MHZ;
+	reg_ratr = RATE_ALL_CCK | RATE_ALL_OFDM_AG |
+	    RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
 	reg_prsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
 
 	rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL, 0x8);
@@ -762,8 +828,8 @@ static void _rtl8723ae_hw_configure(struct ieee80211_hw *hw)
 	rtl_write_dword(rtlpriv, REG_RARFRC, 0x01000000);
 	rtl_write_dword(rtlpriv, REG_RARFRC + 4, 0x07060504);
 
-	if ((pcipriv->bt_coexist.bt_coexistence) &&
-	    (pcipriv->bt_coexist.bt_coexist_type == BT_CSR_BC4))
+	if ((rtlpriv->btcoexist.bt_coexistence) &&
+	    (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4))
 		rtl_write_dword(rtlpriv, REG_AGGLEN_LMT, 0x97427431);
 	else
 		rtl_write_dword(rtlpriv, REG_AGGLEN_LMT, 0xb972a841);
@@ -782,8 +848,8 @@ static void _rtl8723ae_hw_configure(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_PIFS, 0x1C);
 	rtl_write_byte(rtlpriv, REG_AGGR_BREAK_TIME, 0x16);
 
-	if ((pcipriv->bt_coexist.bt_coexistence) &&
-	    (pcipriv->bt_coexist.bt_coexist_type == BT_CSR_BC4)) {
+	if ((rtlpriv->btcoexist.bt_coexistence) &&
+	    (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4)) {
 		rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0020);
 		rtl_write_word(rtlpriv, REG_PROT_MODE_CTRL, 0x0402);
 	} else {
@@ -791,8 +857,8 @@ static void _rtl8723ae_hw_configure(struct ieee80211_hw *hw)
 		rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0020);
 	}
 
-	if ((pcipriv->bt_coexist.bt_coexistence) &&
-	     (pcipriv->bt_coexist.bt_coexist_type == BT_CSR_BC4))
+	if ((rtlpriv->btcoexist.bt_coexistence) &&
+	    (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4))
 		rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x03086666);
 	else
 		rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x086666);
@@ -812,7 +878,7 @@ static void _rtl8723ae_hw_configure(struct ieee80211_hw *hw)
 	rtl_write_dword(rtlpriv, 0x394, 0x1);
 }
 
-static void _rtl8723ae_enable_aspm_back_door(struct ieee80211_hw *hw)
+static void _rtl8723e_enable_aspm_back_door(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
@@ -830,15 +896,15 @@ static void _rtl8723ae_enable_aspm_back_door(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, 0x352, 0x1);
 }
 
-void rtl8723ae_enable_hw_security_config(struct ieee80211_hw *hw)
+void rtl8723e_enable_hw_security_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 sec_reg_value;
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
-		 rtlpriv->sec.pairwise_enc_algorithm,
-		 rtlpriv->sec.group_enc_algorithm);
+		  rtlpriv->sec.pairwise_enc_algorithm,
+		  rtlpriv->sec.group_enc_algorithm);
 
 	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
@@ -846,11 +912,11 @@ void rtl8723ae_enable_hw_security_config(struct ieee80211_hw *hw)
 		return;
 	}
 
-	sec_reg_value = SCR_TxEncEnable | SCR_RxDecEnable;
+	sec_reg_value = SCR_TXENCENABLE | SCR_RXDECENABLE;
 
 	if (rtlpriv->sec.use_defaultkey) {
-		sec_reg_value |= SCR_TxUseDK;
-		sec_reg_value |= SCR_RxUseDK;
+		sec_reg_value |= SCR_TXUSEDK;
+		sec_reg_value |= SCR_RXUSEDK;
 	}
 
 	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
@@ -864,7 +930,7 @@ void rtl8723ae_enable_hw_security_config(struct ieee80211_hw *hw)
 
 }
 
-int rtl8723ae_hw_init(struct ieee80211_hw *hw)
+int rtl8723e_hw_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
@@ -887,6 +953,7 @@ int rtl8723ae_hw_init(struct ieee80211_hw *hw)
 	 */
 	local_save_flags(flags);
 	local_irq_enable();
+	rtlhal->fw_ready = false;
 
 	rtlpriv->intf_ops->disable_aspm(hw);
 	rtstatus = _rtl8712e_init_mac(hw);
@@ -896,20 +963,19 @@ int rtl8723ae_hw_init(struct ieee80211_hw *hw)
 		goto exit;
 	}
 
-	err = rtl8723_download_fw(hw, false);
+	err = rtl8723_download_fw(hw, false, FW_8723A_POLLING_TIMEOUT_COUNT);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
 		goto exit;
-	} else {
-		rtlhal->fw_ready = true;
 	}
+	rtlhal->fw_ready = true;
 
 	rtlhal->last_hmeboxnum = 0;
-	rtl8723ae_phy_mac_config(hw);
-	/* because the last function modifies RCR, we update
-	 * rcr var here, or TP will be unstable as ther receive_config
+	rtl8723e_phy_mac_config(hw);
+	/* because last function modify RCR, so we update
+	 * rcr var here, or TP will unstable for receive_config
 	 * is wrong, RX RCR_ACRC32 will cause TP unstable & Rx
 	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
 	 */
@@ -917,9 +983,9 @@ int rtl8723ae_hw_init(struct ieee80211_hw *hw)
 	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
 	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
 
-	rtl8723ae_phy_bb_config(hw);
+	rtl8723e_phy_bb_config(hw);
 	rtlphy->rf_mode = RF_OP_BY_SW_3WIRE;
-	rtl8723ae_phy_rf_config(hw);
+	rtl8723e_phy_rf_config(hw);
 	if (IS_VENDOR_UMC_A_CUT(rtlhal->version)) {
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1, MASKDWORD, 0x30255);
 		rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G2, MASKDWORD, 0x50a00);
@@ -938,28 +1004,29 @@ int rtl8723ae_hw_init(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0x1);
 	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0x1);
 	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);
-	_rtl8723ae_hw_configure(hw);
+	_rtl8723e_hw_configure(hw);
 	rtl_cam_reset_all_entry(hw);
-	rtl8723ae_enable_hw_security_config(hw);
+	rtl8723e_enable_hw_security_config(hw);
 
 	ppsc->rfpwr_state = ERFON;
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
-	_rtl8723ae_enable_aspm_back_door(hw);
+	_rtl8723e_enable_aspm_back_door(hw);
 	rtlpriv->intf_ops->enable_aspm(hw);
 
-	rtl8723ae_bt_hw_init(hw);
+	rtl8723e_bt_hw_init(hw);
 
 	if (ppsc->rfpwr_state == ERFON) {
-		rtl8723ae_phy_set_rfpath_switch(hw, 1);
+		rtl8723e_phy_set_rfpath_switch(hw, 1);
 		if (rtlphy->iqk_initialized) {
-			rtl8723ae_phy_iq_calibrate(hw, true);
+			rtl8723e_phy_iq_calibrate(hw, true);
 		} else {
-			rtl8723ae_phy_iq_calibrate(hw, false);
+			rtl8723e_phy_iq_calibrate(hw, false);
 			rtlphy->iqk_initialized = true;
 		}
 
-		rtl8723ae_phy_lc_calibrate(hw);
+		rtl8723e_dm_check_txpower_tracking(hw);
+		rtl8723e_phy_lc_calibrate(hw);
 	}
 
 	tmp_u1b = efuse_read_1byte(hw, 0x1FA);
@@ -969,20 +1036,21 @@ int rtl8723ae_hw_init(struct ieee80211_hw *hw)
 	}
 
 	if (!(tmp_u1b & BIT(4))) {
-		tmp_u1b = rtl_read_byte(rtlpriv, 0x16) & 0x0F;
+		tmp_u1b = rtl_read_byte(rtlpriv, 0x16);
+		tmp_u1b &= 0x0F;
 		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x80);
 		udelay(10);
 		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x90);
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "under 1.5V\n");
 	}
-	rtl8723ae_dm_init(hw);
+	rtl8723e_dm_init(hw);
 exit:
 	local_irq_restore(flags);
 	rtlpriv->rtlhal.being_init_adapter = false;
 	return err;
 }
 
-static enum version_8723e _rtl8723ae_read_chip_version(struct ieee80211_hw *hw)
+static enum version_8723e _rtl8723e_read_chip_version(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -992,41 +1060,41 @@ static enum version_8723e _rtl8723ae_read_chip_version(struct ieee80211_hw *hw)
 	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG);
 	if (value32 & TRP_VAUX_EN) {
 		version = (enum version_8723e)(version |
-			  ((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : 0));
+			((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : 0));
 		/* RTL8723 with BT function. */
 		version = (enum version_8723e)(version |
-			  ((value32 & BT_FUNC) ? CHIP_8723 : 0));
+			((value32 & BT_FUNC) ? CHIP_8723 : 0));
 
 	} else {
 		/* Normal mass production chip. */
 		version = (enum version_8723e) NORMAL_CHIP;
 		version = (enum version_8723e)(version |
-			  ((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : 0));
+			((value32 & VENDOR_ID) ? CHIP_VENDOR_UMC : 0));
 		/* RTL8723 with BT function. */
 		version = (enum version_8723e)(version |
-			  ((value32 & BT_FUNC) ? CHIP_8723 : 0));
+			((value32 & BT_FUNC) ? CHIP_8723 : 0));
 		if (IS_CHIP_VENDOR_UMC(version))
 			version = (enum version_8723e)(version |
 			((value32 & CHIP_VER_RTL_MASK)));/* IC version (CUT) */
 		if (IS_8723_SERIES(version)) {
 			value32 = rtl_read_dword(rtlpriv, REG_GPIO_OUTSTS);
-			/* ROM code version */
+			/* ROM code version. */
 			version = (enum version_8723e)(version |
-				  ((value32 & RF_RL_ID)>>20));
+				((value32 & RF_RL_ID)>>20));
 		}
 	}
 
 	if (IS_8723_SERIES(version)) {
 		value32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
 		rtlphy->polarity_ctl = ((value32 & WL_HWPDN_SL) ?
-				       RT_POLARITY_HIGH_ACT :
-				       RT_POLARITY_LOW_ACT);
+					RT_POLARITY_HIGH_ACT :
+					RT_POLARITY_LOW_ACT);
 	}
 	switch (version) {
 	case VERSION_TEST_UMC_CHIP_8723:
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Chip Version ID: VERSION_TEST_UMC_CHIP_8723.\n");
-		break;
+			break;
 	case VERSION_NORMAL_UMC_CHIP_8723_1T1R_A_CUT:
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Chip Version ID: VERSION_NORMAL_UMC_CHIP_8723_1T1R_A_CUT.\n");
@@ -1050,113 +1118,124 @@ static enum version_8723e _rtl8723ae_read_chip_version(struct ieee80211_hw *hw)
 	return version;
 }
 
-static int _rtl8723ae_set_media_status(struct ieee80211_hw *hw,
-				     enum nl80211_iftype type)
+static int _rtl8723e_set_media_status(struct ieee80211_hw *hw,
+				      enum nl80211_iftype type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 bt_msr = rtl_read_byte(rtlpriv, MSR) & 0xfc;
 	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+	u8 mode = MSR_NOLINK;
 
 	rtl_write_dword(rtlpriv, REG_BCN_CTRL, 0);
 	RT_TRACE(rtlpriv, COMP_BEACON, DBG_LOUD,
-		 "clear 0x550 when set HW_VAR_MEDIA_STATUS\n");
-
-	if (type == NL80211_IFTYPE_UNSPECIFIED ||
-	    type == NL80211_IFTYPE_STATION) {
-		_rtl8723ae_stop_tx_beacon(hw);
-		_rtl8723ae_enable_bcn_sufunc(hw);
-	} else if (type == NL80211_IFTYPE_ADHOC ||
-		type == NL80211_IFTYPE_AP) {
-		_rtl8723ae_resume_tx_beacon(hw);
-		_rtl8723ae_disable_bcn_sufunc(hw);
-	} else {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
-			 type);
-	}
+		"clear 0x550 when set HW_VAR_MEDIA_STATUS\n");
 
 	switch (type) {
 	case NL80211_IFTYPE_UNSPECIFIED:
-		bt_msr |= MSR_NOLINK;
-		ledaction = LED_CTL_LINK;
+		mode = MSR_NOLINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Set Network type to NO LINK!\n");
+			"Set Network type to NO LINK!\n");
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		bt_msr |= MSR_ADHOC;
+		mode = MSR_ADHOC;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Set Network type to Ad Hoc!\n");
+			"Set Network type to Ad Hoc!\n");
 		break;
 	case NL80211_IFTYPE_STATION:
-		bt_msr |= MSR_INFRA;
+		mode = MSR_INFRA;
 		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Set Network type to STA!\n");
+			"Set Network type to STA!\n");
 		break;
 	case NL80211_IFTYPE_AP:
-		bt_msr |= MSR_AP;
+		mode = MSR_AP;
+		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "Set Network type to AP!\n");
+			"Set Network type to AP!\n");
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Network type %d not supported!\n",
-			 type);
+			"Network type %d not support!\n", type);
 		return 1;
+		break;
+	}
 
+	/* MSR_INFRA == Link in infrastructure network;
+	 * MSR_ADHOC == Link in ad hoc network;
+	 * Therefore, check link state is necessary.
+	 *
+	 * MSR_AP == AP mode; link state is not cared here.
+	 */
+	if (mode != MSR_AP &&
+	    rtlpriv->mac80211.link_state < MAC80211_LINKED) {
+		mode = MSR_NOLINK;
+		ledaction = LED_CTL_NO_LINK;
+	}
+	if (mode == MSR_NOLINK || mode == MSR_INFRA) {
+		_rtl8723e_stop_tx_beacon(hw);
+		_rtl8723e_enable_bcn_sub_func(hw);
+	} else if (mode == MSR_ADHOC || mode == MSR_AP) {
+		_rtl8723e_resume_tx_beacon(hw);
+		_rtl8723e_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
+			 mode);
 	}
 
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
-	if ((bt_msr & MSR_MASK) == MSR_AP)
+	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
 	else
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
 	return 0;
 }
 
-void rtl8723ae_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
+void rtl8723e_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 reg_rcr;
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rcr = rtlpci->receive_config;
 
 	if (rtlpriv->psc.rfpwr_state != ERFON)
 		return;
 
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&reg_rcr));
-
-	if (check_bssid == true) {
+	if (check_bssid) {
 		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
 					      (u8 *)(&reg_rcr));
-		_rtl8723ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
-	} else if (check_bssid == false) {
+		_rtl8723e_set_bcn_ctrl_reg(hw, 0, BIT(4));
+	} else if (!check_bssid) {
 		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
-		_rtl8723ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
+		_rtl8723e_set_bcn_ctrl_reg(hw, BIT(4), 0);
 		rtlpriv->cfg->ops->set_hw_reg(hw,
-			HW_VAR_RCR, (u8 *) (&reg_rcr));
+			HW_VAR_RCR, (u8 *)(&reg_rcr));
 	}
 }
 
-int rtl8723ae_set_network_type(struct ieee80211_hw *hw,
-			       enum nl80211_iftype type)
+int rtl8723e_set_network_type(struct ieee80211_hw *hw,
+			      enum nl80211_iftype type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	if (_rtl8723ae_set_media_status(hw, type))
+	if (_rtl8723e_set_media_status(hw, type))
 		return -EOPNOTSUPP;
 
 	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
 		if (type != NL80211_IFTYPE_AP)
-			rtl8723ae_set_check_bssid(hw, true);
+			rtl8723e_set_check_bssid(hw, true);
 	} else {
-		rtl8723ae_set_check_bssid(hw, false);
+		rtl8723e_set_check_bssid(hw, false);
 	}
+
 	return 0;
 }
 
-/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
-void rtl8723ae_set_qos(struct ieee80211_hw *hw, int aci)
+/* don't set REG_EDCA_BE_PARAM here
+ * because mac80211 will send pkt when scan
+ */
+void rtl8723e_set_qos(struct ieee80211_hw *hw, int aci)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -1166,7 +1245,6 @@ void rtl8723ae_set_qos(struct ieee80211_hw *hw, int aci)
 		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
 		break;
 	case AC0_BE:
-		/* rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, u4ac_param); */
 		break;
 	case AC2_VI:
 		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x5e4322);
@@ -1180,7 +1258,19 @@ void rtl8723ae_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
-void rtl8723ae_enable_interrupt(struct ieee80211_hw *hw)
+static void rtl8723e_clear_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 tmp;
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISR);
+	rtl_write_dword(rtlpriv, REG_HISR, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
+	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
+}
+
+void rtl8723e_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -1190,37 +1280,39 @@ void rtl8723ae_enable_interrupt(struct ieee80211_hw *hw)
 	rtlpci->irq_enabled = true;
 }
 
-void rtl8723ae_disable_interrupt(struct ieee80211_hw *hw)
+void rtl8723e_disable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
+	rtl8723e_clear_interrupt(hw);/*clear it here first*/
 	rtl_write_dword(rtlpriv, 0x3a8, IMR8190_DISABLED);
 	rtl_write_dword(rtlpriv, 0x3ac, IMR8190_DISABLED);
 	rtlpci->irq_enabled = false;
-	synchronize_irq(rtlpci->pdev->irq);
+	/*synchronize_irq(rtlpci->pdev->irq);*/
 }
 
-static void _rtl8723ae_poweroff_adapter(struct ieee80211_hw *hw)
+static void _rtl8723e_poweroff_adapter(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 u1tmp;
+	u8 u1b_tmp;
 
 	/* Combo (PCIe + USB) Card and PCIe-MF Card */
 	/* 1. Run LPS WL RFOFF flow */
 	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-		PWR_INTF_PCI_MSK, Rtl8723_NIC_LPS_ENTER_FLOW);
+				 PWR_INTF_PCI_MSK, Rtl8723_NIC_LPS_ENTER_FLOW);
 
 	/* 2. 0x1F[7:0] = 0 */
 	/* turn off RF */
 	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
-	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) && rtlhal->fw_ready)
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) &&
+	    rtlhal->fw_ready) {
 		rtl8723ae_firmware_selfreset(hw);
+	}
 
 	/* Reset MCU. Suggested by Filen. */
-	u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1tmp & (~BIT(2))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
 
 	/* g.	MCUFWDL 0x80[1:0]=0	 */
 	/* reset MCU ready status */
@@ -1231,39 +1323,38 @@ static void _rtl8723ae_poweroff_adapter(struct ieee80211_hw *hw)
 		PWR_INTF_PCI_MSK, Rtl8723_NIC_DISABLE_FLOW);
 
 	/* Reset MCU IO Wrapper */
-	u1tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1tmp & (~BIT(0))));
-	u1tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1tmp | BIT(0));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1b_tmp | BIT(0));
 
 	/* 7. RSV_CTRL 0x1C[7:0] = 0x0E */
 	/* lock ISO/CLK/Power control register */
 	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0e);
 }
 
-void rtl8723ae_card_disable(struct ieee80211_hw *hw)
+void rtl8723e_card_disable(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	enum nl80211_iftype opmode;
 
 	mac->link_state = MAC80211_NOLINK;
 	opmode = NL80211_IFTYPE_UNSPECIFIED;
-	_rtl8723ae_set_media_status(hw, opmode);
-	if (rtlpci->driver_is_goingto_unload ||
+	_rtl8723e_set_media_status(hw, opmode);
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
 	    ppsc->rfoff_reason > RF_CHANGE_BY_PS)
 		rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
 	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-	_rtl8723ae_poweroff_adapter(hw);
+	_rtl8723e_poweroff_adapter(hw);
 
 	/* after power off we should do iqk again */
 	rtlpriv->phy.iqk_initialized = false;
 }
 
-void rtl8723ae_interrupt_recognized(struct ieee80211_hw *hw,
-				    u32 *p_inta, u32 *p_intb)
+void rtl8723e_interrupt_recognized(struct ieee80211_hw *hw,
+				   u32 *p_inta, u32 *p_intb)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -1272,7 +1363,7 @@ void rtl8723ae_interrupt_recognized(struct ieee80211_hw *hw,
 	rtl_write_dword(rtlpriv, 0x3a0, *p_inta);
 }
 
-void rtl8723ae_set_beacon_related_registers(struct ieee80211_hw *hw)
+void rtl8723e_set_beacon_related_registers(struct ieee80211_hw *hw)
 {
 
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1281,17 +1372,17 @@ void rtl8723ae_set_beacon_related_registers(struct ieee80211_hw *hw)
 
 	bcn_interval = mac->beacon_interval;
 	atim_window = 2;	/*FIX MERGE */
-	rtl8723ae_disable_interrupt(hw);
+	rtl8723e_disable_interrupt(hw);
 	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
 	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
 	rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660f);
 	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x18);
 	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x18);
 	rtl_write_byte(rtlpriv, 0x606, 0x30);
-	rtl8723ae_enable_interrupt(hw);
+	rtl8723e_enable_interrupt(hw);
 }
 
-void rtl8723ae_set_beacon_interval(struct ieee80211_hw *hw)
+void rtl8723e_set_beacon_interval(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -1299,13 +1390,13 @@ void rtl8723ae_set_beacon_interval(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_BEACON, DBG_DMESG,
 		 "beacon_interval:%d\n", bcn_interval);
-	rtl8723ae_disable_interrupt(hw);
+	rtl8723e_disable_interrupt(hw);
 	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
-	rtl8723ae_enable_interrupt(hw);
+	rtl8723e_enable_interrupt(hw);
 }
 
-void rtl8723ae_update_interrupt_mask(struct ieee80211_hw *hw,
-				     u32 add_msr, u32 rm_msr)
+void rtl8723e_update_interrupt_mask(struct ieee80211_hw *hw,
+				    u32 add_msr, u32 rm_msr)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -1317,11 +1408,11 @@ void rtl8723ae_update_interrupt_mask(struct ieee80211_hw *hw,
 		rtlpci->irq_mask[0] |= add_msr;
 	if (rm_msr)
 		rtlpci->irq_mask[0] &= (~rm_msr);
-	rtl8723ae_disable_interrupt(hw);
-	rtl8723ae_enable_interrupt(hw);
+	rtl8723e_disable_interrupt(hw);
+	rtl8723e_enable_interrupt(hw);
 }
 
-static u8 _rtl8723ae_get_chnl_group(u8 chnl)
+static u8 _rtl8723e_get_chnl_group(u8 chnl)
 {
 	u8 group;
 
@@ -1334,9 +1425,9 @@ static u8 _rtl8723ae_get_chnl_group(u8 chnl)
 	return group;
 }
 
-static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
-						   bool autoload_fail,
-						   u8 *hwinfo)
+static void _rtl8723e_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						  bool autoload_fail,
+						  u8 *hwinfo)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
@@ -1346,19 +1437,14 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	for (rf_path = 0; rf_path < 1; rf_path++) {
 		for (i = 0; i < 3; i++) {
 			if (!autoload_fail) {
-				rtlefuse->eeprom_chnlarea_txpwr_cck
-				    [rf_path][i] =
+				rtlefuse->eeprom_chnlarea_txpwr_cck[rf_path][i] =
 				    hwinfo[EEPROM_TXPOWERCCK + rf_path * 3 + i];
-				rtlefuse->eeprom_chnlarea_txpwr_ht40_1s
-				    [rf_path][i] =
-				    hwinfo[EEPROM_TXPOWERHT40_1S + rf_path *
-				    3 + i];
+				rtlefuse->eeprom_chnlarea_txpwr_ht40_1s[rf_path][i] =
+				    hwinfo[EEPROM_TXPOWERHT40_1S + rf_path * 3 + i];
 			} else {
-				rtlefuse->eeprom_chnlarea_txpwr_cck
-				    [rf_path][i] =
+				rtlefuse->eeprom_chnlarea_txpwr_cck[rf_path][i] =
 				    EEPROM_DEFAULT_TXPOWERLEVEL;
-				rtlefuse->eeprom_chnlarea_txpwr_ht40_1s
-				    [rf_path][i] =
+				rtlefuse->eeprom_chnlarea_txpwr_ht40_1s[rf_path][i] =
 				    EEPROM_DEFAULT_TXPOWERLEVEL;
 			}
 		}
@@ -1379,43 +1465,43 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		for (i = 0; i < 3; i++)
 			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
 				"RF(%d) EEPROM CCK Area(%d) = 0x%x\n", rf_path,
-				i, rtlefuse->eeprom_chnlarea_txpwr_cck
-				[rf_path][i]);
+				 i, rtlefuse->eeprom_chnlarea_txpwr_cck
+					[rf_path][i]);
 	for (rf_path = 0; rf_path < 2; rf_path++)
 		for (i = 0; i < 3; i++)
 			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
 				"RF(%d) EEPROM HT40 1S Area(%d) = 0x%x\n",
 				rf_path, i,
 				rtlefuse->eeprom_chnlarea_txpwr_ht40_1s
-				[rf_path][i]);
+					[rf_path][i]);
 	for (rf_path = 0; rf_path < 2; rf_path++)
 		for (i = 0; i < 3; i++)
 			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
 				"RF(%d) EEPROM HT40 2S Diff Area(%d) = 0x%x\n",
-				rf_path, i,
-				rtlefuse->eprom_chnl_txpwr_ht40_2sdf
-				[rf_path][i]);
+				 rf_path, i,
+				 rtlefuse->eprom_chnl_txpwr_ht40_2sdf
+					[rf_path][i]);
 
 	for (rf_path = 0; rf_path < 2; rf_path++) {
 		for (i = 0; i < 14; i++) {
-			index = _rtl8723ae_get_chnl_group((u8) i);
+			index = _rtl8723e_get_chnl_group((u8)i);
 
 			rtlefuse->txpwrlevel_cck[rf_path][i] =
 				rtlefuse->eeprom_chnlarea_txpwr_cck
-							[rf_path][index];
+					[rf_path][index];
 			rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
 				rtlefuse->eeprom_chnlarea_txpwr_ht40_1s
-							[rf_path][index];
+					[rf_path][index];
 
 			if ((rtlefuse->eeprom_chnlarea_txpwr_ht40_1s
-			    [rf_path][index] -
-			    rtlefuse->eprom_chnl_txpwr_ht40_2sdf[rf_path]
-			    [index]) > 0) {
-				rtlefuse->txpwrlevel_ht40_2s[rf_path][i] =
-					rtlefuse->eeprom_chnlarea_txpwr_ht40_1s
 					[rf_path][index] -
-					rtlefuse->eprom_chnl_txpwr_ht40_2sdf
-					[rf_path][index];
+			     rtlefuse->eprom_chnl_txpwr_ht40_2sdf
+					[rf_path][index]) > 0) {
+				rtlefuse->txpwrlevel_ht40_2s[rf_path][i] =
+				  rtlefuse->eeprom_chnlarea_txpwr_ht40_1s
+				  [rf_path][index] -
+				  rtlefuse->eprom_chnl_txpwr_ht40_2sdf
+				  [rf_path][index];
 			} else {
 				rtlefuse->txpwrlevel_ht40_2s[rf_path][i] = 0;
 			}
@@ -1423,8 +1509,8 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 
 		for (i = 0; i < 14; i++) {
 			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
-				"RF(%d)-Ch(%d) [CCK / HT40_1S / HT40_2S] = "
-				"[0x%x / 0x%x / 0x%x]\n", rf_path, i,
+				"RF(%d)-Ch(%d) [CCK / HT40_1S / HT40_2S] = [0x%x / 0x%x / 0x%x]\n",
+				rf_path, i,
 				rtlefuse->txpwrlevel_cck[rf_path][i],
 				rtlefuse->txpwrlevel_ht40_1s[rf_path][i],
 				rtlefuse->txpwrlevel_ht40_2s[rf_path][i]);
@@ -1445,22 +1531,20 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 
 	for (rf_path = 0; rf_path < 2; rf_path++) {
 		for (i = 0; i < 14; i++) {
-			index = _rtl8723ae_get_chnl_group((u8) i);
+			index = _rtl8723e_get_chnl_group((u8)i);
 
 			if (rf_path == RF90_PATH_A) {
 				rtlefuse->pwrgroup_ht20[rf_path][i] =
-				    (rtlefuse->eeprom_pwrlimit_ht20[index] &
-				    0xf);
+				  (rtlefuse->eeprom_pwrlimit_ht20[index] & 0xf);
 				rtlefuse->pwrgroup_ht40[rf_path][i] =
-				    (rtlefuse->eeprom_pwrlimit_ht40[index] &
-				    0xf);
+				  (rtlefuse->eeprom_pwrlimit_ht40[index] & 0xf);
 			} else if (rf_path == RF90_PATH_B) {
 				rtlefuse->pwrgroup_ht20[rf_path][i] =
-				    ((rtlefuse->eeprom_pwrlimit_ht20[index] &
-				    0xf0) >> 4);
+				  ((rtlefuse->eeprom_pwrlimit_ht20[index] &
+				   0xf0) >> 4);
 				rtlefuse->pwrgroup_ht40[rf_path][i] =
-				    ((rtlefuse->eeprom_pwrlimit_ht40[index] &
-				    0xf0) >> 4);
+				  ((rtlefuse->eeprom_pwrlimit_ht40[index] &
+				   0xf0) >> 4);
 			}
 
 			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
@@ -1473,7 +1557,7 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	}
 
 	for (i = 0; i < 14; i++) {
-		index = _rtl8723ae_get_chnl_group((u8) i);
+		index = _rtl8723e_get_chnl_group((u8)i);
 
 		if (!autoload_fail)
 			tempval = hwinfo[EEPROM_TXPOWERHT20DIFF + index];
@@ -1490,7 +1574,7 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		if (rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] & BIT(3))
 			rtlefuse->txpwr_ht20diff[RF90_PATH_B][i] |= 0xF0;
 
-		index = _rtl8723ae_get_chnl_group((u8) i);
+		index = _rtl8723e_get_chnl_group((u8)i);
 
 		if (!autoload_fail)
 			tempval = hwinfo[EEPROM_TXPOWER_OFDMDIFF + index];
@@ -1508,19 +1592,19 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	for (i = 0; i < 14; i++)
 		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Ht20 to HT40 Diff[%d] = 0x%x\n", i,
-			rtlefuse->txpwr_ht20diff[RF90_PATH_A][i]);
+			 rtlefuse->txpwr_ht20diff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
 		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-A Legacy to Ht40 Diff[%d] = 0x%x\n", i,
-			rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i]);
+			 rtlefuse->txpwr_legacyhtdiff[RF90_PATH_A][i]);
 	for (i = 0; i < 14; i++)
 		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Ht20 to HT40 Diff[%d] = 0x%x\n", i,
-			rtlefuse->txpwr_ht20diff[RF90_PATH_B][i]);
+			 rtlefuse->txpwr_ht20diff[RF90_PATH_B][i]);
 	for (i = 0; i < 14; i++)
 		RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 			"RF-B Legacy to HT40 Diff[%d] = 0x%x\n", i,
-			rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i]);
+			 rtlefuse->txpwr_legacyhtdiff[RF90_PATH_B][i]);
 
 	if (!autoload_fail)
 		rtlefuse->eeprom_regulatory = (hwinfo[RF_OPTION1] & 0x7);
@@ -1533,10 +1617,11 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->eeprom_tssi[RF90_PATH_A] = hwinfo[EEPROM_TSSI_A];
 	else
 		rtlefuse->eeprom_tssi[RF90_PATH_A] = EEPROM_DEFAULT_TSSI;
+
 	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"TSSI_A = 0x%x, TSSI_B = 0x%x\n",
-		rtlefuse->eeprom_tssi[RF90_PATH_A],
-		rtlefuse->eeprom_tssi[RF90_PATH_B]);
+		 rtlefuse->eeprom_tssi[RF90_PATH_A],
+		 rtlefuse->eeprom_tssi[RF90_PATH_B]);
 
 	if (!autoload_fail)
 		tempval = hwinfo[EEPROM_THERMAL_METER];
@@ -1552,8 +1637,8 @@ static void _rtl8723ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
 }
 
-static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
-					 bool pseudo_test)
+static void _rtl8723e_read_adapter_info(struct ieee80211_hw *hw,
+					bool b_pseudo_test)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
@@ -1562,7 +1647,7 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 	u8 hwinfo[HWSET_MAX_SIZE];
 	u16 eeprom_id;
 
-	if (pseudo_test) {
+	if (b_pseudo_test) {
 		/* need add */
 		return;
 	}
@@ -1576,7 +1661,7 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 			 "RTL819X Not boot from eeprom, check it !!");
 	}
 
-	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP\n"),
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "MAP\n",
 		      hwinfo, HWSET_MAX_SIZE);
 
 	eeprom_id = *((u16 *)&hwinfo[0]);
@@ -1589,13 +1674,13 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 		rtlefuse->autoload_failflag = false;
 	}
 
-	if (rtlefuse->autoload_failflag == true)
+	if (rtlefuse->autoload_failflag)
 		return;
 
-	rtlefuse->eeprom_vid = *(u16 *) &hwinfo[EEPROM_VID];
-	rtlefuse->eeprom_did = *(u16 *) &hwinfo[EEPROM_DID];
-	rtlefuse->eeprom_svid = *(u16 *) &hwinfo[EEPROM_SVID];
-	rtlefuse->eeprom_smid = *(u16 *) &hwinfo[EEPROM_SMID];
+	rtlefuse->eeprom_vid = *(u16 *)&hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *)&hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *)&hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *)&hwinfo[EEPROM_SMID];
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 		 "EEPROMId = 0x%4x\n", eeprom_id);
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
@@ -1609,16 +1694,16 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 
 	for (i = 0; i < 6; i += 2) {
 		usvalue = *(u16 *)&hwinfo[EEPROM_MAC_ADDR + i];
-		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
+		*((u16 *)(&rtlefuse->dev_addr[i])) = usvalue;
 	}
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "dev_addr: %pM\n", rtlefuse->dev_addr);
 
-	_rtl8723ae_read_txpower_info_from_hwpg(hw,
-			rtlefuse->autoload_failflag, hwinfo);
+	_rtl8723e_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					      hwinfo);
 
-	rtl8723ae_read_bt_coexist_info_from_hwpg(hw,
+	rtl8723e_read_bt_coexist_info_from_hwpg(hw,
 			rtlefuse->autoload_failflag, hwinfo);
 
 	rtlefuse->eeprom_channelplan = hwinfo[EEPROM_CHANNELPLAN];
@@ -1644,6 +1729,14 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 				    CHK_SVID_SMID(0x10EC, 0x6178) ||
 				    CHK_SVID_SMID(0x10EC, 0x6179) ||
 				    CHK_SVID_SMID(0x10EC, 0x6180) ||
+				    CHK_SVID_SMID(0x10EC, 0x7151) ||
+				    CHK_SVID_SMID(0x10EC, 0x7152) ||
+				    CHK_SVID_SMID(0x10EC, 0x7154) ||
+				    CHK_SVID_SMID(0x10EC, 0x7155) ||
+				    CHK_SVID_SMID(0x10EC, 0x7177) ||
+				    CHK_SVID_SMID(0x10EC, 0x7178) ||
+				    CHK_SVID_SMID(0x10EC, 0x7179) ||
+				    CHK_SVID_SMID(0x10EC, 0x7180) ||
 				    CHK_SVID_SMID(0x10EC, 0x8151) ||
 				    CHK_SVID_SMID(0x10EC, 0x8152) ||
 				    CHK_SVID_SMID(0x10EC, 0x8154) ||
@@ -1671,7 +1764,10 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 					 CHK_SVID_SMID(0x10EC, 0x7193) ||
 					 CHK_SVID_SMID(0x10EC, 0x8191) ||
 					 CHK_SVID_SMID(0x10EC, 0x8192) ||
-					 CHK_SVID_SMID(0x10EC, 0x8193))
+					 CHK_SVID_SMID(0x10EC, 0x8193) ||
+					 CHK_SVID_SMID(0x10EC, 0x9191) ||
+					 CHK_SVID_SMID(0x10EC, 0x9192) ||
+					 CHK_SVID_SMID(0x10EC, 0x9193))
 					rtlhal->oem_id = RT_CID_819X_SAMSUNG;
 				else if (CHK_SVID_SMID(0x10EC, 0x8195) ||
 					 CHK_SVID_SMID(0x10EC, 0x9195) ||
@@ -1728,7 +1824,7 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 				else
 					rtlhal->oem_id = RT_CID_DEFAULT;
 			} else {
-					rtlhal->oem_id = RT_CID_DEFAULT;
+				rtlhal->oem_id = RT_CID_DEFAULT;
 			}
 			break;
 		case EEPROM_CID_TOSHIBA:
@@ -1750,18 +1846,31 @@ static void _rtl8723ae_read_adapter_info(struct ieee80211_hw *hw,
 	}
 }
 
-static void _rtl8723ae_hal_customized_behavior(struct ieee80211_hw *hw)
+static void _rtl8723e_hal_customized_behavior(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	pcipriv->ledctl.led_opendrain = true;
+	switch (rtlhal->oem_id) {
+	case RT_CID_819X_HP:
+		pcipriv->ledctl.led_opendrain = true;
+		break;
+	case RT_CID_819X_LENOVO:
+	case RT_CID_DEFAULT:
+	case RT_CID_TOSHIBA:
+	case RT_CID_CCX:
+	case RT_CID_819X_ACER:
+	case RT_CID_WHQL:
+	default:
+		break;
+	}
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "RT Customized ID: 0x%02X\n", rtlhal->oem_id);
 }
 
-void rtl8723ae_read_eeprom_info(struct ieee80211_hw *hw)
+void rtl8723e_read_eeprom_info(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
@@ -1774,7 +1883,7 @@ void rtl8723ae_read_eeprom_info(struct ieee80211_hw *hw)
 	value32 = (value32 & ~EFUSE_SEL_MASK) | EFUSE_SEL(EFUSE_WIFI_SEL_0);
 	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST], value32);
 
-	rtlhal->version = _rtl8723ae_read_chip_version(hw);
+	rtlhal->version = _rtl8723e_read_chip_version(hw);
 
 	if (get_rf_type(rtlphy) == RF_1T1R)
 		rtlpriv->dm.rfpath_rxenable[0] = true;
@@ -1782,7 +1891,7 @@ void rtl8723ae_read_eeprom_info(struct ieee80211_hw *hw)
 		rtlpriv->dm.rfpath_rxenable[0] =
 		    rtlpriv->dm.rfpath_rxenable[1] = true;
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "VersionID = 0x%4x\n",
-		 rtlhal->version);
+						rtlhal->version);
 
 	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
 	if (tmp_u1b & BIT(4)) {
@@ -1795,33 +1904,34 @@ void rtl8723ae_read_eeprom_info(struct ieee80211_hw *hw)
 	if (tmp_u1b & BIT(5)) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
 		rtlefuse->autoload_failflag = false;
-		_rtl8723ae_read_adapter_info(hw, false);
+		_rtl8723e_read_adapter_info(hw, false);
 	} else {
 		rtlefuse->autoload_failflag = true;
-		_rtl8723ae_read_adapter_info(hw, false);
+		_rtl8723e_read_adapter_info(hw, false);
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Autoload ERR!!\n");
 	}
-	_rtl8723ae_hal_customized_behavior(hw);
+	_rtl8723e_hal_customized_behavior(hw);
 }
 
-static void rtl8723ae_update_hal_rate_table(struct ieee80211_hw *hw,
-					    struct ieee80211_sta *sta)
+static void rtl8723e_update_hal_rate_table(struct ieee80211_hw *hw,
+					   struct ieee80211_sta *sta)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 ratr_value;
 	u8 ratr_index = 0;
-	u8 nmode = mac->ht_enable;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	u8 b_nmode = mac->ht_enable;
+	u16 shortgi_rate;
+	u32 tmp_ratr_value;
 	u8 curtxbw_40mhz = mac->bw_40;
 	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
 				1 : 0;
 	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
 				1 : 0;
 	enum wireless_mode wirelessmode = mac->mode;
+	u32 ratr_mask;
 
 	if (rtlhal->current_bandtype == BAND_ON_5G)
 		ratr_value = sta->supp_rates[1] << 4;
@@ -1830,7 +1940,7 @@ static void rtl8723ae_update_hal_rate_table(struct ieee80211_hw *hw,
 	if (mac->opmode == NL80211_IFTYPE_ADHOC)
 		ratr_value = 0xfff;
 	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
-		       sta->ht_cap.mcs.rx_mask[0] << 12);
+			sta->ht_cap.mcs.rx_mask[0] << 12);
 	switch (wirelessmode) {
 	case WIRELESS_MODE_B:
 		if (ratr_value & 0x0000000c)
@@ -1843,20 +1953,14 @@ static void rtl8723ae_update_hal_rate_table(struct ieee80211_hw *hw,
 		break;
 	case WIRELESS_MODE_N_24G:
 	case WIRELESS_MODE_N_5G:
-		nmode = 1;
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			ratr_value &= 0x0007F005;
-		} else {
-			u32 ratr_mask;
-
-			if (get_rf_type(rtlphy) == RF_1T2R ||
-			    get_rf_type(rtlphy) == RF_1T1R)
-				ratr_mask = 0x000ff005;
-			else
-				ratr_mask = 0x0f0ff005;
+		b_nmode = 1;
+		if (get_rf_type(rtlphy) == RF_1T2R ||
+		    get_rf_type(rtlphy) == RF_1T1R)
+			ratr_mask = 0x000ff005;
+		else
+			ratr_mask = 0x0f0ff005;
 
-			ratr_value &= ratr_mask;
-		}
+		ratr_value &= ratr_mask;
 		break;
 	default:
 		if (rtlphy->rf_type == RF_1T2R)
@@ -1867,19 +1971,30 @@ static void rtl8723ae_update_hal_rate_table(struct ieee80211_hw *hw,
 		break;
 	}
 
-	if ((pcipriv->bt_coexist.bt_coexistence) &&
-	    (pcipriv->bt_coexist.bt_coexist_type == BT_CSR_BC4) &&
-	    (pcipriv->bt_coexist.bt_cur_state) &&
-	    (pcipriv->bt_coexist.bt_ant_isolation) &&
-	    ((pcipriv->bt_coexist.bt_service == BT_SCO) ||
-	    (pcipriv->bt_coexist.bt_service == BT_BUSY)))
+	if ((rtlpriv->btcoexist.bt_coexistence) &&
+	    (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4) &&
+	    (rtlpriv->btcoexist.bt_cur_state) &&
+	    (rtlpriv->btcoexist.bt_ant_isolation) &&
+	    ((rtlpriv->btcoexist.bt_service == BT_SCO) ||
+	    (rtlpriv->btcoexist.bt_service == BT_BUSY)))
 		ratr_value &= 0x0fffcfc0;
 	else
 		ratr_value &= 0x0FFFFFFF;
 
-	if (nmode && ((curtxbw_40mhz && curshortgi_40mhz) ||
-	   (!curtxbw_40mhz && curshortgi_20mhz)))
+	if (b_nmode &&
+	    ((curtxbw_40mhz && curshortgi_40mhz) ||
+	     (!curtxbw_40mhz && curshortgi_20mhz))) {
 		ratr_value |= 0x10000000;
+		tmp_ratr_value = (ratr_value >> 12);
+
+		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
+			if ((1 << shortgi_rate) & tmp_ratr_value)
+				break;
+		}
+
+		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
+		    (shortgi_rate << 4) | (shortgi_rate);
+	}
 
 	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
 
@@ -1887,8 +2002,9 @@ static void rtl8723ae_update_hal_rate_table(struct ieee80211_hw *hw,
 		 "%x\n", rtl_read_dword(rtlpriv, REG_ARFR0));
 }
 
-static void rtl8723ae_update_hal_rate_mask(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u8 rssi_level)
+static void rtl8723e_update_hal_rate_mask(struct ieee80211_hw *hw,
+					  struct ieee80211_sta *sta,
+					  u8 rssi_level)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
@@ -1897,7 +2013,8 @@ static void rtl8723ae_update_hal_rate_mask(struct ieee80211_hw *hw,
 	struct rtl_sta_info *sta_entry = NULL;
 	u32 ratr_bitmap;
 	u8 ratr_index;
-	u8 curtxbw_40mhz = (sta->bandwidth >= IEEE80211_STA_RX_BW_40) ? 1 : 0;
+	u8 curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
+				? 1 : 0;
 	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
 				1 : 0;
 	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
@@ -1906,9 +2023,9 @@ static void rtl8723ae_update_hal_rate_mask(struct ieee80211_hw *hw,
 	bool shortgi = false;
 	u8 rate_mask[5];
 	u8 macid = 0;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	/*u8 mimo_ps = IEEE80211_SMPS_OFF;*/
 
-	sta_entry = (struct rtl_sta_info *) sta->drv_priv;
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 	wirelessmode = sta_entry->wireless_mode;
 	if (mac->opmode == NL80211_IFTYPE_STATION)
 		curtxbw_40mhz = mac->bw_40;
@@ -1943,54 +2060,44 @@ static void rtl8723ae_update_hal_rate_mask(struct ieee80211_hw *hw,
 			ratr_bitmap &= 0x00000ff5;
 		break;
 	case WIRELESS_MODE_A:
-		ratr_index = RATR_INX_WIRELESS_A;
+		ratr_index = RATR_INX_WIRELESS_G;
 		ratr_bitmap &= 0x00000ff0;
 		break;
 	case WIRELESS_MODE_N_24G:
 	case WIRELESS_MODE_N_5G:
 		ratr_index = RATR_INX_WIRELESS_NGB;
-
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			if (rssi_level == 1)
-				ratr_bitmap &= 0x00070000;
-			else if (rssi_level == 2)
-				ratr_bitmap &= 0x0007f000;
-			else
-				ratr_bitmap &= 0x0007f005;
+		if (rtlphy->rf_type == RF_1T2R ||
+		    rtlphy->rf_type == RF_1T1R) {
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff015;
+			} else {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff005;
+			}
 		} else {
-			if (rtlphy->rf_type == RF_1T2R ||
-			    rtlphy->rf_type == RF_1T1R) {
-				if (curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff005;
-				}
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f0f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f0ff000;
+				else
+					ratr_bitmap &= 0x0f0ff015;
 			} else {
-				if (curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0f0f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0f0ff000;
-					else
-						ratr_bitmap &= 0x0f0ff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0f0f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0f0ff000;
-					else
-						ratr_bitmap &= 0x0f0ff005;
-				}
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f0f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f0ff000;
+				else
+					ratr_bitmap &= 0x0f0ff005;
 			}
 		}
 
@@ -2015,30 +2122,30 @@ static void rtl8723ae_update_hal_rate_mask(struct ieee80211_hw *hw,
 
 	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
 		 "ratr_bitmap :%x\n", ratr_bitmap);
-	/* convert ratr_bitmap to le byte array */
-	rate_mask[0] = ratr_bitmap;
-	rate_mask[1] = (ratr_bitmap >>= 8);
-	rate_mask[2] = (ratr_bitmap >>= 8);
-	rate_mask[3] = ((ratr_bitmap >> 8) & 0x0f) | (ratr_index << 4);
+	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) |
+			     (ratr_index << 28);
 	rate_mask[4] = macid | (shortgi ? 0x20 : 0x00) | 0x80;
 	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
-		 "Rate_index:%x, ratr_bitmap: %*phC\n",
-		 ratr_index, 5, rate_mask);
-	rtl8723ae_fill_h2c_cmd(hw, H2C_RA_MASK, 5, rate_mask);
+		 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x\n",
+		  ratr_index, ratr_bitmap,
+		  rate_mask[0], rate_mask[1],
+		  rate_mask[2], rate_mask[3],
+		  rate_mask[4]);
+	rtl8723e_fill_h2c_cmd(hw, H2C_RA_MASK, 5, rate_mask);
 }
 
-void rtl8723ae_update_hal_rate_tbl(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u8 rssi_level)
+void rtl8723e_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				  struct ieee80211_sta *sta, u8 rssi_level)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	if (rtlpriv->dm.useramask)
-		rtl8723ae_update_hal_rate_mask(hw, sta, rssi_level);
+		rtl8723e_update_hal_rate_mask(hw, sta, rssi_level);
 	else
-		rtl8723ae_update_hal_rate_table(hw, sta);
+		rtl8723e_update_hal_rate_table(hw, sta);
 }
 
-void rtl8723ae_update_channel_access_setting(struct ieee80211_hw *hw)
+void rtl8723e_update_channel_access_setting(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -2052,14 +2159,14 @@ void rtl8723ae_update_channel_access_setting(struct ieee80211_hw *hw)
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *)&sifs_timer);
 }
 
-bool rtl8723ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
+bool rtl8723e_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	enum rf_pwrstate e_rfpowerstate_toset;
+	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
 	u8 u1tmp;
-	bool actuallyset = false;
+	bool b_actuallyset = false;
 
 	if (rtlpriv->rtlhal.being_init_adapter)
 		return false;
@@ -2076,6 +2183,8 @@ bool rtl8723ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 		spin_unlock(&rtlpriv->locks.rf_ps_lock);
 	}
 
+	cur_rfstate = ppsc->rfpwr_state;
+
 	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL_2,
 		       rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL_2)&~(BIT(1)));
 
@@ -2086,24 +2195,23 @@ bool rtl8723ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 	else
 		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFON : ERFOFF;
 
-	if ((ppsc->hwradiooff == true) && (e_rfpowerstate_toset == ERFON)) {
+	if (ppsc->hwradiooff && (e_rfpowerstate_toset == ERFON)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 "GPIOChangeRF  - HW Radio ON, RF ON\n");
 
 		e_rfpowerstate_toset = ERFON;
 		ppsc->hwradiooff = false;
-		actuallyset = true;
-	} else if ((ppsc->hwradiooff == false)
-		   && (e_rfpowerstate_toset == ERFOFF)) {
+		b_actuallyset = true;
+	} else if (!ppsc->hwradiooff && (e_rfpowerstate_toset == ERFOFF)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 "GPIOChangeRF  - HW Radio OFF, RF OFF\n");
 
 		e_rfpowerstate_toset = ERFOFF;
 		ppsc->hwradiooff = true;
-		actuallyset = true;
+		b_actuallyset = true;
 	}
 
-	if (actuallyset) {
+	if (b_actuallyset) {
 		spin_lock(&rtlpriv->locks.rf_ps_lock);
 		ppsc->rfchange_inprogress = false;
 		spin_unlock(&rtlpriv->locks.rf_ps_lock);
@@ -2118,11 +2226,12 @@ bool rtl8723ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 
 	*valid = 1;
 	return !ppsc->hwradiooff;
+
 }
 
-void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
-		       u8 *p_macaddr, bool is_group, u8 enc_algo,
-		       bool is_wepkey, bool clear_all)
+void rtl8723e_set_key(struct ieee80211_hw *hw, u32 key_index,
+		      u8 *p_macaddr, bool is_group, u8 enc_algo,
+		      bool is_wepkey, bool clear_all)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -2130,6 +2239,7 @@ void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
 	u8 *macaddr = p_macaddr;
 	u32 entry_id = 0;
 	bool is_pairwise = false;
+
 	static u8 cam_const_addr[4][6] = {
 		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
 		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
@@ -2157,6 +2267,7 @@ void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
 				rtlpriv->sec.key_len[idx] = 0;
 			}
 		}
+
 	} else {
 		switch (enc_algo) {
 		case WEP40_ENCRYPTION:
@@ -2172,8 +2283,8 @@ void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
 			enc_algo = CAM_AES;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
 			enc_algo = CAM_TKIP;
 			break;
 		}
@@ -2187,8 +2298,8 @@ void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
 				entry_id = key_index;
 			} else {
 				if (mac->opmode == NL80211_IFTYPE_AP) {
-					entry_id = rtl_cam_get_free_entry(hw,
-								macaddr);
+					entry_id =
+					  rtl_cam_get_free_entry(hw, p_macaddr);
 					if (entry_id >=  TOTAL_CAM_ENTRY) {
 						RT_TRACE(rtlpriv, COMP_SEC,
 							 DBG_EMERG,
@@ -2219,22 +2330,22 @@ void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
 					 "set Pairwiase key\n");
 
 				rtl_cam_add_one_entry(hw, macaddr, key_index,
-					entry_id, enc_algo,
-					CAM_CONFIG_NO_USEDK,
-					rtlpriv->sec.key_buf[key_index]);
+						      entry_id, enc_algo,
+						      CAM_CONFIG_NO_USEDK,
+						      rtlpriv->sec.key_buf[key_index]);
 			} else {
 				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 					 "set group key\n");
 
 				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
 					rtl_cam_add_one_entry(hw,
-						rtlefuse->dev_addr,
-						PAIRWISE_KEYIDX,
-						CAM_PAIRWISE_KEY_POSITION,
-						enc_algo,
-						CAM_CONFIG_NO_USEDK,
-						rtlpriv->sec.key_buf
-						[entry_id]);
+							rtlefuse->dev_addr,
+							PAIRWISE_KEYIDX,
+							CAM_PAIRWISE_KEY_POSITION,
+							enc_algo,
+							CAM_CONFIG_NO_USEDK,
+							rtlpriv->sec.key_buf
+							[entry_id]);
 				}
 
 				rtl_cam_add_one_entry(hw, macaddr, key_index,
@@ -2247,45 +2358,43 @@ void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
 	}
 }
 
-static void rtl8723ae_bt_var_init(struct ieee80211_hw *hw)
+static void rtl8723e_bt_var_init(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	pcipriv->bt_coexist.bt_coexistence =
-					pcipriv->bt_coexist.eeprom_bt_coexist;
-	pcipriv->bt_coexist.bt_ant_num =
-					pcipriv->bt_coexist.eeprom_bt_ant_num;
-	pcipriv->bt_coexist.bt_coexist_type =
-					pcipriv->bt_coexist.eeprom_bt_type;
+	rtlpriv->btcoexist.bt_coexistence =
+		rtlpriv->btcoexist.eeprom_bt_coexist;
+	rtlpriv->btcoexist.bt_ant_num =
+		rtlpriv->btcoexist.eeprom_bt_ant_num;
+	rtlpriv->btcoexist.bt_coexist_type =
+		rtlpriv->btcoexist.eeprom_bt_type;
 
-		pcipriv->bt_coexist.bt_ant_isolation =
-				pcipriv->bt_coexist.eeprom_bt_ant_isol;
+	rtlpriv->btcoexist.bt_ant_isolation =
+		rtlpriv->btcoexist.eeprom_bt_ant_isol;
 
-	pcipriv->bt_coexist.bt_radio_shared_type =
-				pcipriv->bt_coexist.eeprom_bt_radio_shared;
+	rtlpriv->btcoexist.bt_radio_shared_type =
+		rtlpriv->btcoexist.eeprom_bt_radio_shared;
 
 	RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 		 "BT Coexistance = 0x%x\n",
-		 pcipriv->bt_coexist.bt_coexistence);
+		 rtlpriv->btcoexist.bt_coexistence);
 
-	if (pcipriv->bt_coexist.bt_coexistence) {
-		pcipriv->bt_coexist.bt_busy_traffic = false;
-		pcipriv->bt_coexist.bt_traffic_mode_set = false;
-		pcipriv->bt_coexist.bt_non_traffic_mode_set = false;
+	if (rtlpriv->btcoexist.bt_coexistence) {
+		rtlpriv->btcoexist.bt_busy_traffic = false;
+		rtlpriv->btcoexist.bt_traffic_mode_set = false;
+		rtlpriv->btcoexist.bt_non_traffic_mode_set = false;
 
-		pcipriv->bt_coexist.cstate = 0;
-		pcipriv->bt_coexist.previous_state = 0;
+		rtlpriv->btcoexist.cstate = 0;
+		rtlpriv->btcoexist.previous_state = 0;
 
-		if (pcipriv->bt_coexist.bt_ant_num == ANT_X2) {
+		if (rtlpriv->btcoexist.bt_ant_num == ANT_X2) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 				 "BlueTooth BT_Ant_Num = Antx2\n");
-		} else if (pcipriv->bt_coexist.bt_ant_num == ANT_X1) {
+		} else if (rtlpriv->btcoexist.bt_ant_num == ANT_X1) {
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 				 "BlueTooth BT_Ant_Num = Antx1\n");
 		}
-
-		switch (pcipriv->bt_coexist.bt_coexist_type) {
+		switch (rtlpriv->btcoexist.bt_coexist_type) {
 		case BT_2WIRE:
 			RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 				 "BlueTooth BT_CoexistType = BT_2Wire\n");
@@ -2317,20 +2426,19 @@ static void rtl8723ae_bt_var_init(struct ieee80211_hw *hw)
 		}
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "BlueTooth BT_Ant_isolation = %d\n",
-			 pcipriv->bt_coexist.bt_ant_isolation);
+			 rtlpriv->btcoexist.bt_ant_isolation);
 		RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_TRACE,
 			 "BT_RadioSharedType = 0x%x\n",
-			 pcipriv->bt_coexist.bt_radio_shared_type);
-		pcipriv->bt_coexist.bt_active_zero_cnt = 0;
-		pcipriv->bt_coexist.cur_bt_disabled = false;
-		pcipriv->bt_coexist.pre_bt_disabled = false;
+			 rtlpriv->btcoexist.bt_radio_shared_type);
+		rtlpriv->btcoexist.bt_active_zero_cnt = 0;
+		rtlpriv->btcoexist.cur_bt_disabled = false;
+		rtlpriv->btcoexist.pre_bt_disabled = false;
 	}
 }
 
-void rtl8723ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-					      bool auto_load_fail, u8 *hwinfo)
+void rtl8723e_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					     bool auto_load_fail, u8 *hwinfo)
 {
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 value;
 	u32 tmpu_32;
@@ -2338,47 +2446,50 @@ void rtl8723ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
 	if (!auto_load_fail) {
 		tmpu_32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
 		if (tmpu_32 & BIT(18))
-			pcipriv->bt_coexist.eeprom_bt_coexist = 1;
+			rtlpriv->btcoexist.eeprom_bt_coexist = 1;
 		else
-			pcipriv->bt_coexist.eeprom_bt_coexist = 0;
+			rtlpriv->btcoexist.eeprom_bt_coexist = 0;
 		value = hwinfo[RF_OPTION4];
-		pcipriv->bt_coexist.eeprom_bt_type = BT_RTL8723A;
-		pcipriv->bt_coexist.eeprom_bt_ant_num = (value & 0x1);
-		pcipriv->bt_coexist.eeprom_bt_ant_isol = ((value & 0x10) >> 4);
-		pcipriv->bt_coexist.eeprom_bt_radio_shared =
-				((value & 0x20) >> 5);
+		rtlpriv->btcoexist.eeprom_bt_type = BT_RTL8723A;
+		rtlpriv->btcoexist.eeprom_bt_ant_num = (value & 0x1);
+		rtlpriv->btcoexist.eeprom_bt_ant_isol = ((value & 0x10) >> 4);
+		rtlpriv->btcoexist.eeprom_bt_radio_shared =
+		  ((value & 0x20) >> 5);
 	} else {
-		pcipriv->bt_coexist.eeprom_bt_coexist = 0;
-		pcipriv->bt_coexist.eeprom_bt_type = BT_RTL8723A;
-		pcipriv->bt_coexist.eeprom_bt_ant_num = ANT_X2;
-		pcipriv->bt_coexist.eeprom_bt_ant_isol = 0;
-		pcipriv->bt_coexist.eeprom_bt_radio_shared = BT_RADIO_SHARED;
+		rtlpriv->btcoexist.eeprom_bt_coexist = 0;
+		rtlpriv->btcoexist.eeprom_bt_type = BT_RTL8723A;
+		rtlpriv->btcoexist.eeprom_bt_ant_num = ANT_X2;
+		rtlpriv->btcoexist.eeprom_bt_ant_isol = 0;
+		rtlpriv->btcoexist.eeprom_bt_radio_shared = BT_RADIO_SHARED;
 	}
 
-	rtl8723ae_bt_var_init(hw);
+	rtl8723e_bt_var_init(hw);
 }
 
-void rtl8723ae_bt_reg_init(struct ieee80211_hw *hw)
+void rtl8723e_bt_reg_init(struct ieee80211_hw *hw)
 {
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	/* 0:Low, 1:High, 2:From Efuse. */
-	pcipriv->bt_coexist.reg_bt_iso = 2;
+	rtlpriv->btcoexist.reg_bt_iso = 2;
 	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
-	pcipriv->bt_coexist.reg_bt_sco = 3;
+	rtlpriv->btcoexist.reg_bt_sco = 3;
 	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
-	pcipriv->bt_coexist.reg_bt_sco = 0;
+	rtlpriv->btcoexist.reg_bt_sco = 0;
 }
 
-
-void rtl8723ae_bt_hw_init(struct ieee80211_hw *hw)
+void rtl8723e_bt_hw_init(struct ieee80211_hw *hw)
 {
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_init_hw_config(rtlpriv);
 }
 
-void rtl8723ae_suspend(struct ieee80211_hw *hw)
+void rtl8723e_suspend(struct ieee80211_hw *hw)
 {
 }
 
-void rtl8723ae_resume(struct ieee80211_hw *hw)
+void rtl8723e_resume(struct ieee80211_hw *hw)
 {
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.h b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.h
index d3bc39fb27a5..32c1ace97c3f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/hw.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -34,38 +30,38 @@
 	((rtlefuse->eeprom_svid == (_val1)) &&			\
 	 (rtlefuse->eeprom_smid == (_val2)))
 
-void rtl8723ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
-void rtl8723ae_read_eeprom_info(struct ieee80211_hw *hw);
+void rtl8723e_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl8723e_read_eeprom_info(struct ieee80211_hw *hw);
 
-void rtl8723ae_interrupt_recognized(struct ieee80211_hw *hw,
-				    u32 *p_inta, u32 *p_intb);
-int rtl8723ae_hw_init(struct ieee80211_hw *hw);
-void rtl8723ae_card_disable(struct ieee80211_hw *hw);
-void rtl8723ae_enable_interrupt(struct ieee80211_hw *hw);
-void rtl8723ae_disable_interrupt(struct ieee80211_hw *hw);
-int rtl8723ae_set_network_type(struct ieee80211_hw *hw,
-			       enum nl80211_iftype type);
-void rtl8723ae_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
-void rtl8723ae_set_qos(struct ieee80211_hw *hw, int aci);
-void rtl8723ae_set_beacon_related_registers(struct ieee80211_hw *hw);
-void rtl8723ae_set_beacon_interval(struct ieee80211_hw *hw);
-void rtl8723ae_update_interrupt_mask(struct ieee80211_hw *hw,
-				     u32 add_msr, u32 rm_msr);
-void rtl8723ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
-void rtl8723ae_update_hal_rate_tbl(struct ieee80211_hw *hw,
-				   struct ieee80211_sta *sta, u8 rssi_level);
-void rtl8723ae_update_channel_access_setting(struct ieee80211_hw *hw);
-bool rtl8723ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
-void rtl8723ae_enable_hw_security_config(struct ieee80211_hw *hw);
-void rtl8723ae_set_key(struct ieee80211_hw *hw, u32 key_index,
-		       u8 *p_macaddr, bool is_group, u8 enc_algo,
-		       bool is_wepkey, bool clear_all);
+void rtl8723e_interrupt_recognized(struct ieee80211_hw *hw,
+				   u32 *p_inta, u32 *p_intb);
+int rtl8723e_hw_init(struct ieee80211_hw *hw);
+void rtl8723e_card_disable(struct ieee80211_hw *hw);
+void rtl8723e_enable_interrupt(struct ieee80211_hw *hw);
+void rtl8723e_disable_interrupt(struct ieee80211_hw *hw);
+int rtl8723e_set_network_type(struct ieee80211_hw *hw,
+			      enum nl80211_iftype type);
+void rtl8723e_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid);
+void rtl8723e_set_qos(struct ieee80211_hw *hw, int aci);
+void rtl8723e_set_beacon_related_registers(struct ieee80211_hw *hw);
+void rtl8723e_set_beacon_interval(struct ieee80211_hw *hw);
+void rtl8723e_update_interrupt_mask(struct ieee80211_hw *hw,
+				    u32 add_msr, u32 rm_msr);
+void rtl8723e_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val);
+void rtl8723e_update_hal_rate_tbl(struct ieee80211_hw *hw,
+				  struct ieee80211_sta *sta, u8 rssi_level);
+void rtl8723e_update_channel_access_setting(struct ieee80211_hw *hw);
+bool rtl8723e_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid);
+void rtl8723e_enable_hw_security_config(struct ieee80211_hw *hw);
+void rtl8723e_set_key(struct ieee80211_hw *hw, u32 key_index,
+		      u8 *p_macaddr, bool is_group, u8 enc_algo,
+		      bool is_wepkey, bool clear_all);
 
-void rtl8723ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-					      bool autoload_fail, u8 *hwinfo);
-void rtl8723ae_bt_reg_init(struct ieee80211_hw *hw);
-void rtl8723ae_bt_hw_init(struct ieee80211_hw *hw);
-void rtl8723ae_suspend(struct ieee80211_hw *hw);
-void rtl8723ae_resume(struct ieee80211_hw *hw);
+void rtl8723e_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					     bool autoload_fail, u8 *hwinfo);
+void rtl8723e_bt_reg_init(struct ieee80211_hw *hw);
+void rtl8723e_bt_hw_init(struct ieee80211_hw *hw);
+void rtl8723e_suspend(struct ieee80211_hw *hw);
+void rtl8723e_resume(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/led.c b/drivers/net/wireless/rtlwifi/rtl8723ae/led.c
index 061526fe6e2d..13173351cbfd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/led.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/led.c
@@ -32,44 +32,44 @@
 #include "reg.h"
 #include "led.h"
 
-static void _rtl8723ae_init_led(struct ieee80211_hw *hw,
-				struct rtl_led *pled, enum rtl_led_pin ledpin)
+static void _rtl8723e_init_led(struct ieee80211_hw *hw,
+			       struct rtl_led *pled, enum rtl_led_pin ledpin)
 {
 	pled->hw = hw;
 	pled->ledpin = ledpin;
 	pled->ledon = false;
 }
 
-void rtl8723ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
+void rtl8723e_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 ledcfg;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
 		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
-	ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
-
 	switch (pled->ledpin) {
 	case LED_PIN_GPIO0:
 		break;
 	case LED_PIN_LED0:
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
 		ledcfg &= ~BIT(6);
 		rtl_write_byte(rtlpriv,
 			       REG_LEDCFG2, (ledcfg & 0xf0) | BIT(5));
 		break;
 	case LED_PIN_LED1:
-		rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg & 0x0f) | BIT(5));
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg & 0x10);
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+			 "switch case not process\n");
 		break;
 	}
 	pled->ledon = true;
 }
 
-void rtl8723ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
+void rtl8723e_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
@@ -86,7 +86,7 @@ void rtl8723ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 	case LED_PIN_LED0:
 		ledcfg &= 0xf0;
 		if (pcipriv->ledctl.led_opendrain) {
-			ledcfg &= 0x90;
+			ledcfg &= 0x90; /* Set to software control. */
 			rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg|BIT(3)));
 			ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
 			ledcfg &= 0xFE;
@@ -94,50 +94,51 @@ void rtl8723ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 		} else {
 			ledcfg &= ~BIT(6);
 			rtl_write_byte(rtlpriv, REG_LEDCFG2,
-				       (ledcfg | BIT(3) | BIT(5)));
+					(ledcfg | BIT(3) | BIT(5)));
 		}
 		break;
 	case LED_PIN_LED1:
-		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1) & 0x10;
-		rtl_write_byte(rtlpriv, REG_LEDCFG1, (ledcfg | BIT(3)));
+		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
+		ledcfg &= 0x10; /* Set to software control. */
+		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg|BIT(3));
+
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+			 "switch case not process\n");
 		break;
 	}
 	pled->ledon = false;
 }
 
-void rtl8723ae_init_sw_leds(struct ieee80211_hw *hw)
+void rtl8723e_init_sw_leds(struct ieee80211_hw *hw)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-
-	_rtl8723ae_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
-	_rtl8723ae_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+	_rtl8723e_init_led(hw, &pcipriv->ledctl.sw_led0, LED_PIN_LED0);
+	_rtl8723e_init_led(hw, &pcipriv->ledctl.sw_led1, LED_PIN_LED1);
 }
 
-static void _rtl8723ae_sw_led_control(struct ieee80211_hw *hw,
-				    enum led_ctl_mode ledaction)
+static void _rtl8723e_sw_led_control(struct ieee80211_hw *hw,
+				     enum led_ctl_mode ledaction)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
-
 	switch (ledaction) {
 	case LED_CTL_POWER_ON:
 	case LED_CTL_LINK:
 	case LED_CTL_NO_LINK:
-		rtl8723ae_sw_led_on(hw, pLed0);
+		rtl8723e_sw_led_on(hw, pLed0);
 		break;
 	case LED_CTL_POWER_OFF:
-		rtl8723ae_sw_led_off(hw, pLed0);
+		rtl8723e_sw_led_off(hw, pLed0);
 		break;
 	default:
 		break;
 	}
 }
 
-void rtl8723ae_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction)
+void rtl8723e_led_control(struct ieee80211_hw *hw,
+			  enum led_ctl_mode ledaction)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
@@ -152,6 +153,7 @@ void rtl8723ae_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction)
 	     ledaction == LED_CTL_POWER_ON)) {
 		return;
 	}
-	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD, "ledaction %d,\n", ledaction);
-	_rtl8723ae_sw_led_control(hw, ledaction);
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD, "ledaction %d,\n",
+		 ledaction);
+	_rtl8723e_sw_led_control(hw, ledaction);
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/led.h b/drivers/net/wireless/rtlwifi/rtl8723ae/led.h
index 2cb88e78f62a..c22b19f542a6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/led.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/led.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,10 +26,9 @@
 #ifndef __RTL92CE_LED_H__
 #define __RTL92CE_LED_H__
 
-void rtl8723ae_init_sw_leds(struct ieee80211_hw *hw);
-void rtl8723ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
-void rtl8723ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
-void rtl8723ae_led_control(struct ieee80211_hw *hw,
-			   enum led_ctl_mode ledaction);
+void rtl8723e_init_sw_leds(struct ieee80211_hw *hw);
+void rtl8723e_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8723e_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
+void rtl8723e_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c
index 3ea78afdec73..d367097f490b 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,7 +26,6 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../ps.h"
-#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
@@ -39,29 +34,31 @@
 #include "table.h"
 #include "../rtl8723com/phy_common.h"
 
-/* static forward definitions */
-static u32 _phy_fw_rf_serial_read(struct ieee80211_hw *hw,
-				  enum radio_path rfpath, u32 offset);
-static void _phy_fw_rf_serial_write(struct ieee80211_hw *hw,
-				    enum radio_path rfpath,
-				    u32 offset, u32 data);
-static bool _phy_bb8192c_config_parafile(struct ieee80211_hw *hw);
-static bool _phy_cfg_mac_w_header(struct ieee80211_hw *hw);
-static bool _phy_cfg_bb_w_header(struct ieee80211_hw *hw, u8 configtype);
-static bool _phy_cfg_bb_w_pgheader(struct ieee80211_hw *hw, u8 configtype);
-static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
-				      u8 *stage, u8 *step, u32 *delay);
-static u8 _phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
-				enum wireless_mode wirelessmode,
-				long power_indbm);
-static void rtl8723ae_phy_set_io(struct ieee80211_hw *hw);
-
-u32 rtl8723ae_phy_query_rf_reg(struct ieee80211_hw *hw,
-			       enum radio_path rfpath, u32 regaddr, u32 bitmask)
+static void _rtl8723e_phy_fw_rf_serial_write(struct ieee80211_hw *hw,
+					     enum radio_path rfpath, u32 offset,
+					     u32 data);
+static bool _rtl8723e_phy_bb8192c_config_parafile(struct ieee80211_hw *hw);
+static bool _rtl8723e_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+static bool _rtl8723e_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						    u8 configtype);
+static bool _rtl8723e_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						      u8 configtype);
+static bool _rtl8723e_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					       u8 channel, u8 *stage, u8 *step,
+					       u32 *delay);
+static u8 _rtl8723e_phy_dbm_to_txpwr_idx(struct ieee80211_hw *hw,
+					 enum wireless_mode wirelessmode,
+					 long power_indbm);
+static void rtl8723e_phy_set_rf_on(struct ieee80211_hw *hw);
+static void rtl8723e_phy_set_io(struct ieee80211_hw *hw);
+
+u32 rtl8723e_phy_query_rf_reg(struct ieee80211_hw *hw,
+			      enum radio_path rfpath,
+			      u32 regaddr, u32 bitmask)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 original_value, readback_value, bitshift;
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	u32 original_value = 0, readback_value, bitshift;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	unsigned long flags;
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
@@ -70,10 +67,10 @@ u32 rtl8723ae_phy_query_rf_reg(struct ieee80211_hw *hw,
 
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 
-	if (rtlphy->rf_mode != RF_OP_BY_FW)
-		original_value = rtl8723_phy_rf_serial_read(hw, rfpath, regaddr);
-	else
-		original_value = _phy_fw_rf_serial_read(hw, rfpath, regaddr);
+	if (rtlphy->rf_mode != RF_OP_BY_FW) {
+		original_value = rtl8723_phy_rf_serial_read(hw,
+							    rfpath, regaddr);
+	}
 
 	bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
 	readback_value = (original_value & bitmask) >> bitshift;
@@ -82,45 +79,46 @@ u32 rtl8723ae_phy_query_rf_reg(struct ieee80211_hw *hw,
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
-		 regaddr, rfpath, bitmask, original_value);
+		  regaddr, rfpath, bitmask, original_value);
 
 	return readback_value;
 }
 
-void rtl8723ae_phy_set_rf_reg(struct ieee80211_hw *hw,
-			      enum radio_path rfpath,
-			      u32 regaddr, u32 bitmask, u32 data)
+void rtl8723e_phy_set_rf_reg(struct ieee80211_hw *hw,
+			     enum radio_path rfpath,
+			   u32 regaddr, u32 bitmask, u32 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 original_value, bitshift;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u32 original_value = 0, bitshift;
 	unsigned long flags;
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
-		 regaddr, bitmask, data, rfpath);
+		  regaddr, bitmask, data, rfpath);
 
 	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
 
 	if (rtlphy->rf_mode != RF_OP_BY_FW) {
 		if (bitmask != RFREG_OFFSET_MASK) {
-			original_value = rtl8723_phy_rf_serial_read(hw, rfpath,
+			original_value = rtl8723_phy_rf_serial_read(hw,
+								    rfpath,
 								    regaddr);
 			bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
-			data = ((original_value & (~bitmask)) |
-			       (data << bitshift));
+			data =
+			    ((original_value & (~bitmask)) |
+			     (data << bitshift));
 		}
 
 		rtl8723_phy_rf_serial_write(hw, rfpath, regaddr, data);
 	} else {
 		if (bitmask != RFREG_OFFSET_MASK) {
-			original_value = _phy_fw_rf_serial_read(hw, rfpath,
-								regaddr);
 			bitshift = rtl8723_phy_calculate_bit_shift(bitmask);
-			data = ((original_value & (~bitmask)) |
-			       (data << bitshift));
+			data =
+			    ((original_value & (~bitmask)) |
+			     (data << bitshift));
 		}
-		_phy_fw_rf_serial_write(hw, rfpath, regaddr, data);
+		_rtl8723e_phy_fw_rf_serial_write(hw, rfpath, regaddr, data);
 	}
 
 	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
@@ -128,23 +126,17 @@ void rtl8723ae_phy_set_rf_reg(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
 		 regaddr, bitmask, data, rfpath);
-}
 
-static u32 _phy_fw_rf_serial_read(struct ieee80211_hw *hw,
-					    enum radio_path rfpath, u32 offset)
-{
-	RT_ASSERT(false, "deprecated!\n");
-	return 0;
 }
 
-static void _phy_fw_rf_serial_write(struct ieee80211_hw *hw,
-				    enum radio_path rfpath,
-				    u32 offset, u32 data)
+static void _rtl8723e_phy_fw_rf_serial_write(struct ieee80211_hw *hw,
+					     enum radio_path rfpath, u32 offset,
+					     u32 data)
 {
 	RT_ASSERT(false, "deprecated!\n");
 }
 
-static void _rtl8723ae_phy_bb_config_1t(struct ieee80211_hw *hw)
+static void _rtl8723e_phy_bb_config_1t(struct ieee80211_hw *hw)
 {
 	rtl_set_bbreg(hw, RFPGA0_TXINFO, 0x3, 0x2);
 	rtl_set_bbreg(hw, RFPGA1_TXINFO, 0x300033, 0x200022);
@@ -158,20 +150,20 @@ static void _rtl8723ae_phy_bb_config_1t(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, 0xe88, 0x0c000000, 0x2);
 }
 
-bool rtl8723ae_phy_mac_config(struct ieee80211_hw *hw)
+bool rtl8723e_phy_mac_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool rtstatus = _phy_cfg_mac_w_header(hw);
+	bool rtstatus = _rtl8723e_phy_config_mac_with_headerfile(hw);
 	rtl_write_byte(rtlpriv, 0x04CA, 0x0A);
 	return rtstatus;
 }
 
-bool rtl8723ae_phy_bb_config(struct ieee80211_hw *hw)
+bool rtl8723e_phy_bb_config(struct ieee80211_hw *hw)
 {
 	bool rtstatus = true;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmpu1b;
-	u8 reg_hwparafile = 1;
+	u8 b_reg_hwparafile = 1;
 
 	rtl8723_phy_init_bb_rf_reg_def(hw);
 
@@ -186,67 +178,72 @@ bool rtl8723ae_phy_bb_config(struct ieee80211_hw *hw)
 
 	/* 3. 0x02[1:0] = 2b'11 */
 	tmpu1b = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, (tmpu1b |
-		       FEN_BB_GLB_RSTn | FEN_BBRSTB));
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
+		       (tmpu1b | FEN_BB_GLB_RSTN | FEN_BBRSTB));
 
 	/* 4. 0x25[6] = 0 */
 	tmpu1b = rtl_read_byte(rtlpriv, REG_AFE_XTAL_CTRL+1);
-	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL+1, (tmpu1b&(~BIT(6))));
+	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL+1, (tmpu1b & (~BIT(6))));
 
-	/* 5. 0x24[20] = 0	Advised by SD3 Alex Wang. 2011.02.09. */
+	/* 5. 0x24[20] = 0	//Advised by SD3 Alex Wang. 2011.02.09. */
 	tmpu1b = rtl_read_byte(rtlpriv, REG_AFE_XTAL_CTRL+2);
-	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL+2, (tmpu1b&(~BIT(4))));
+	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL+2, (tmpu1b & (~BIT(4))));
 
 	/* 6. 0x1f[7:0] = 0x07 */
 	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x07);
 
-	if (reg_hwparafile == 1)
-		rtstatus = _phy_bb8192c_config_parafile(hw);
+	if (b_reg_hwparafile == 1)
+		rtstatus = _rtl8723e_phy_bb8192c_config_parafile(hw);
 	return rtstatus;
 }
 
-bool rtl8723ae_phy_rf_config(struct ieee80211_hw *hw)
+bool rtl8723e_phy_rf_config(struct ieee80211_hw *hw)
 {
-	return rtl8723ae_phy_rf6052_config(hw);
+	return rtl8723e_phy_rf6052_config(hw);
 }
 
-static bool _phy_bb8192c_config_parafile(struct ieee80211_hw *hw)
+static bool _rtl8723e_phy_bb8192c_config_parafile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	bool rtstatus;
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "==>\n");
-	rtstatus = _phy_cfg_bb_w_header(hw, BASEBAND_CONFIG_PHY_REG);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "\n");
+	rtstatus = _rtl8723e_phy_config_bb_with_headerfile(hw,
+						BASEBAND_CONFIG_PHY_REG);
 	if (rtstatus != true) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
 		return false;
 	}
 
 	if (rtlphy->rf_type == RF_1T2R) {
-		_rtl8723ae_phy_bb_config_1t(hw);
+		_rtl8723e_phy_bb_config_1t(hw);
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Config to 1T!!\n");
 	}
 	if (rtlefuse->autoload_failflag == false) {
 		rtlphy->pwrgroup_cnt = 0;
-		rtstatus = _phy_cfg_bb_w_pgheader(hw, BASEBAND_CONFIG_PHY_REG);
+		rtstatus = _rtl8723e_phy_config_bb_with_pgheaderfile(hw,
+					BASEBAND_CONFIG_PHY_REG);
 	}
 	if (rtstatus != true) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
 		return false;
 	}
-	rtstatus = _phy_cfg_bb_w_header(hw, BASEBAND_CONFIG_AGC_TAB);
+	rtstatus =
+	  _rtl8723e_phy_config_bb_with_headerfile(hw, BASEBAND_CONFIG_AGC_TAB);
 	if (rtstatus != true) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
 		return false;
 	}
 	rtlphy->cck_high_power = (bool) (rtl_get_bbreg(hw,
-					 RFPGA0_XA_HSSIPARAMETER2, 0x200));
+					RFPGA0_XA_HSSIPARAMETER2,
+					0x200));
+
 	return true;
 }
 
-static bool _phy_cfg_mac_w_header(struct ieee80211_hw *hw)
+static bool _rtl8723e_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 i;
@@ -264,7 +261,8 @@ static bool _phy_cfg_mac_w_header(struct ieee80211_hw *hw)
 	return true;
 }
 
-static bool _phy_cfg_bb_w_header(struct ieee80211_hw *hw, u8 configtype)
+static bool _rtl8723e_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						    u8 configtype)
 {
 	int i;
 	u32 *phy_regarray_table;
@@ -278,13 +276,23 @@ static bool _phy_cfg_bb_w_header(struct ieee80211_hw *hw, u8 configtype)
 	phy_regarray_table = RTL8723EPHY_REG_1TARRAY;
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_reg_arraylen; i = i + 2) {
-			rtl_addr_delay(phy_regarray_table[i]);
+			if (phy_regarray_table[i] == 0xfe)
+				mdelay(50);
+			else if (phy_regarray_table[i] == 0xfd)
+				mdelay(5);
+			else if (phy_regarray_table[i] == 0xfc)
+				mdelay(1);
+			else if (phy_regarray_table[i] == 0xfb)
+				udelay(50);
+			else if (phy_regarray_table[i] == 0xfa)
+				udelay(5);
+			else if (phy_regarray_table[i] == 0xf9)
+				udelay(1);
 			rtl_set_bbreg(hw, phy_regarray_table[i], MASKDWORD,
 				      phy_regarray_table[i + 1]);
 			udelay(1);
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-				 "The phy_regarray_table[0] is %x"
-				 " Rtl819XPHY_REGArray[1] is %x\n",
+				 "The phy_regarray_table[0] is %x Rtl819XPHY_REGArray[1] is %x\n",
 				 phy_regarray_table[i],
 				 phy_regarray_table[i + 1]);
 		}
@@ -294,8 +302,7 @@ static bool _phy_cfg_bb_w_header(struct ieee80211_hw *hw, u8 configtype)
 				      agctab_array_table[i + 1]);
 			udelay(1);
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-				 "The agctab_array_table[0] is "
-				 "%x Rtl819XPHY_REGArray[1] is %x\n",
+				 "The agctab_array_table[0] is %x Rtl819XPHY_REGArray[1] is %x\n",
 				 agctab_array_table[i],
 				 agctab_array_table[i + 1]);
 		}
@@ -303,132 +310,163 @@ static bool _phy_cfg_bb_w_header(struct ieee80211_hw *hw, u8 configtype)
 	return true;
 }
 
-static void _st_pwrIdx_dfrate_off(struct ieee80211_hw *hw, u32 regaddr,
-				  u32 bitmask, u32 data)
+static void store_pwrindex_diffrate_offset(struct ieee80211_hw *hw,
+					   u32 regaddr, u32 bitmask,
+					   u32 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
-	switch (regaddr) {
-	case RTXAGC_A_RATE18_06:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][0] = data;
+	if (regaddr == RTXAGC_A_RATE18_06) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][0] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][0] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][0]);
-		break;
-	case RTXAGC_A_RATE54_24:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][1] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][0]);
+	}
+	if (regaddr == RTXAGC_A_RATE54_24) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][1] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][1] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][1]);
-		break;
-	case RTXAGC_A_CCK1_MCS32:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][6] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][1]);
+	}
+	if (regaddr == RTXAGC_A_CCK1_MCS32) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][6] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][6] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][6]);
-		break;
-	case RTXAGC_B_CCK11_A_CCK2_11:
-		if (bitmask == 0xffffff00) {
-			rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][7] = data;
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-				 "MCSTxPowerLevelOriginalOffset[%d][7] = 0x%x\n",
-				 rtlphy->pwrgroup_cnt,
-				 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][7]);
-		}
-		if (bitmask == 0x000000ff) {
-			rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][15] = data;
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-				 "MCSTxPowerLevelOriginalOffset[%d][15] = 0x%x\n",
-				 rtlphy->pwrgroup_cnt,
-				 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][15]);
-		}
-		break;
-	case RTXAGC_A_MCS03_MCS00:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][2] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][6]);
+	}
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][7] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][7] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][7]);
+	}
+	if (regaddr == RTXAGC_A_MCS03_MCS00) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][2] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][2] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][2]);
-		break;
-	case RTXAGC_A_MCS07_MCS04:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][3] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][2]);
+	}
+	if (regaddr == RTXAGC_A_MCS07_MCS04) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][3] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][3] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][3]);
-		break;
-	case RTXAGC_A_MCS11_MCS08:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][4] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][3]);
+	}
+	if (regaddr == RTXAGC_A_MCS11_MCS08) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][4] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][4] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][4]);
-		break;
-	case RTXAGC_A_MCS15_MCS12:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][5] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][4]);
+	}
+	if (regaddr == RTXAGC_A_MCS15_MCS12) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][5] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][5] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][5]);
-		break;
-	case RTXAGC_B_RATE18_06:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][8] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][5]);
+	}
+	if (regaddr == RTXAGC_B_RATE18_06) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][8] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][8] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][8]);
-		break;
-	case RTXAGC_B_RATE54_24:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][9] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][8]);
+	}
+	if (regaddr == RTXAGC_B_RATE54_24) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][9] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][9] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][9]);
-		break;
-	case RTXAGC_B_CCK1_55_MCS32:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][14] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][9]);
+	}
+	if (regaddr == RTXAGC_B_CCK1_55_MCS32) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][14] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][14] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][14]);
-		break;
-	case RTXAGC_B_MCS03_MCS00:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][10] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][14]);
+	}
+	if (regaddr == RTXAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][15] =
+		    data;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "MCSTxPowerLevelOriginalOffset[%d][15] = 0x%x\n",
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][15]);
+	}
+	if (regaddr == RTXAGC_B_MCS03_MCS00) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][10] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][10] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][10]);
-		break;
-	case RTXAGC_B_MCS07_MCS04:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][11] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][10]);
+	}
+	if (regaddr == RTXAGC_B_MCS07_MCS04) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][11] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][11] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][11]);
-		break;
-	case RTXAGC_B_MCS11_MCS08:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][12] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][11]);
+	}
+	if (regaddr == RTXAGC_B_MCS11_MCS08) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][12] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][12] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][12]);
-		break;
-	case RTXAGC_B_MCS15_MCS12:
-		rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][13] = data;
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][12]);
+	}
+	if (regaddr == RTXAGC_B_MCS15_MCS12) {
+		rtlphy->mcs_txpwrlevel_origoffset[rtlphy->pwrgroup_cnt][13] =
+		    data;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "MCSTxPowerLevelOriginalOffset[%d][13] = 0x%x\n",
-			 rtlphy->pwrgroup_cnt,
-			 rtlphy->mcs_offset[rtlphy->pwrgroup_cnt][13]);
+			  rtlphy->pwrgroup_cnt,
+			  rtlphy->mcs_txpwrlevel_origoffset[rtlphy->
+							    pwrgroup_cnt][13]);
+
 		rtlphy->pwrgroup_cnt++;
-		break;
 	}
 }
 
-static bool _phy_cfg_bb_w_pgheader(struct ieee80211_hw *hw, u8 configtype)
+static bool _rtl8723e_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						      u8 configtype)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int i;
@@ -440,11 +478,23 @@ static bool _phy_cfg_bb_w_pgheader(struct ieee80211_hw *hw, u8 configtype)
 
 	if (configtype == BASEBAND_CONFIG_PHY_REG) {
 		for (i = 0; i < phy_regarray_pg_len; i = i + 3) {
-			rtl_addr_delay(phy_regarray_table_pg[i]);
-
-			_st_pwrIdx_dfrate_off(hw, phy_regarray_table_pg[i],
-					      phy_regarray_table_pg[i + 1],
-					      phy_regarray_table_pg[i + 2]);
+			if (phy_regarray_table_pg[i] == 0xfe)
+				mdelay(50);
+			else if (phy_regarray_table_pg[i] == 0xfd)
+				mdelay(5);
+			else if (phy_regarray_table_pg[i] == 0xfc)
+				mdelay(1);
+			else if (phy_regarray_table_pg[i] == 0xfb)
+				udelay(50);
+			else if (phy_regarray_table_pg[i] == 0xfa)
+				udelay(5);
+			else if (phy_regarray_table_pg[i] == 0xf9)
+				udelay(1);
+
+			store_pwrindex_diffrate_offset(hw,
+						phy_regarray_table_pg[i],
+						phy_regarray_table_pg[i + 1],
+						phy_regarray_table_pg[i + 2]);
 		}
 	} else {
 		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
@@ -453,45 +503,57 @@ static bool _phy_cfg_bb_w_pgheader(struct ieee80211_hw *hw, u8 configtype)
 	return true;
 }
 
-bool rtl8723ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
-					     enum radio_path rfpath)
+bool rtl8723e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					    enum radio_path rfpath)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int i;
+	bool rtstatus = true;
 	u32 *radioa_array_table;
-	u16 radioa_arraylen;
+	u32 *radiob_array_table;
+	u16 radioa_arraylen, radiob_arraylen;
 
-	radioa_arraylen = Rtl8723ERADIOA_1TARRAYLENGTH;
+	radioa_arraylen = RTL8723ERADIOA_1TARRAYLENGTH;
 	radioa_array_table = RTL8723E_RADIOA_1TARRAY;
+	radiob_arraylen = RTL8723E_RADIOB_1TARRAYLENGTH;
+	radiob_array_table = RTL8723E_RADIOB_1TARRAY;
+
+	rtstatus = true;
 
 	switch (rfpath) {
 	case RF90_PATH_A:
 		for (i = 0; i < radioa_arraylen; i = i + 2) {
-			rtl_rfreg_delay(hw, rfpath, radioa_array_table[i],
-					RFREG_OFFSET_MASK,
-					radioa_array_table[i + 1]);
+			if (radioa_array_table[i] == 0xfe) {
+				mdelay(50);
+			} else if (radioa_array_table[i] == 0xfd) {
+				mdelay(5);
+			} else if (radioa_array_table[i] == 0xfc) {
+				mdelay(1);
+			} else if (radioa_array_table[i] == 0xfb) {
+				udelay(50);
+			} else if (radioa_array_table[i] == 0xfa) {
+				udelay(5);
+			} else if (radioa_array_table[i] == 0xf9) {
+				udelay(1);
+			} else {
+				rtl_set_rfreg(hw, rfpath, radioa_array_table[i],
+					      RFREG_OFFSET_MASK,
+					      radioa_array_table[i + 1]);
+				udelay(1);
+			}
 		}
 		break;
 	case RF90_PATH_B:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not process\n");
-		break;
 	case RF90_PATH_C:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not process\n");
-		break;
 	case RF90_PATH_D:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not process\n");
 		break;
 	}
 	return true;
 }
 
-void rtl8723ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
+void rtl8723e_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	rtlphy->default_initialgain[0] =
 	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
@@ -504,10 +566,10 @@ void rtl8723ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
-		  rtlphy->default_initialgain[0],
-		  rtlphy->default_initialgain[1],
-		  rtlphy->default_initialgain[2],
-		  rtlphy->default_initialgain[3]);
+		 rtlphy->default_initialgain[0],
+		 rtlphy->default_initialgain[1],
+		 rtlphy->default_initialgain[2],
+		 rtlphy->default_initialgain[3]);
 
 	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
 					       ROFDM0_RXDETECTOR3, MASKBYTE0);
@@ -516,37 +578,43 @@ void rtl8723ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 		 "Default framesync (0x%x) = 0x%x\n",
-		 ROFDM0_RXDETECTOR3, rtlphy->framesync);
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync);
 }
 
-void rtl8723ae_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
+void rtl8723e_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 txpwr_level;
 	long txpwr_dbm;
 
 	txpwr_level = rtlphy->cur_cck_txpwridx;
-	txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_B, txpwr_level);
+	txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw,
+						 WIRELESS_MODE_B, txpwr_level);
 	txpwr_level = rtlphy->cur_ofdm24g_txpwridx +
 	    rtlefuse->legacy_ht_txpowerdiff;
-	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G, txpwr_level) > txpwr_dbm)
-		txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
-						  txpwr_level);
+	if (rtl8723_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+						 txpwr_level);
 	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G, txpwr_level) >
-	    txpwr_dbm)
-		txpwr_dbm = rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
-							 txpwr_level);
+	if (rtl8723_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_N_24G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    rtl8723_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+						 txpwr_level);
 	*powerlevel = txpwr_dbm;
 }
 
-static void _rtl8723ae_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
-					 u8 *cckpowerlevel, u8 *ofdmpowerlevel)
+static void _rtl8723e_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
+					u8 *cckpowerlevel, u8 *ofdmpowerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 index = (channel - 1);
 
@@ -567,66 +635,70 @@ static void _rtl8723ae_get_txpower_index(struct ieee80211_hw *hw, u8 channel,
 	}
 }
 
-static void _rtl8723ae_ccxpower_index_check(struct ieee80211_hw *hw,
-					    u8 channel, u8 *cckpowerlevel,
-					    u8 *ofdmpowerlevel)
+static void _rtl8723e_ccxpower_index_check(struct ieee80211_hw *hw,
+					   u8 channel, u8 *cckpowerlevel,
+					   u8 *ofdmpowerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	rtlphy->cur_cck_txpwridx = cckpowerlevel[0];
 	rtlphy->cur_ofdm24g_txpwridx = ofdmpowerlevel[0];
+
 }
 
-void rtl8723ae_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
+void rtl8723e_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
 {
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 cckpowerlevel[2], ofdmpowerlevel[2];
 
 	if (rtlefuse->txpwr_fromeprom == false)
 		return;
-	_rtl8723ae_get_txpower_index(hw, channel, &cckpowerlevel[0],
-				     &ofdmpowerlevel[0]);
-	_rtl8723ae_ccxpower_index_check(hw, channel, &cckpowerlevel[0],
-					&ofdmpowerlevel[0]);
-	rtl8723ae_phy_rf6052_set_cck_txpower(hw, &cckpowerlevel[0]);
-	rtl8723ae_phy_rf6052_set_ofdm_txpower(hw, &ofdmpowerlevel[0], channel);
+	_rtl8723e_get_txpower_index(hw, channel,
+				    &cckpowerlevel[0], &ofdmpowerlevel[0]);
+	_rtl8723e_ccxpower_index_check(hw,
+				       channel, &cckpowerlevel[0],
+				       &ofdmpowerlevel[0]);
+	rtl8723e_phy_rf6052_set_cck_txpower(hw, &cckpowerlevel[0]);
+	rtl8723e_phy_rf6052_set_ofdm_txpower(hw, &ofdmpowerlevel[0], channel);
 }
 
-bool rtl8723ae_phy_update_txpower_dbm(struct ieee80211_hw *hw, long power_indbm)
+bool rtl8723e_phy_update_txpower_dbm(struct ieee80211_hw *hw, long power_indbm)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 idx;
 	u8 rf_path;
-	u8 ccktxpwridx = _phy_dbm_to_txpwr_Idx(hw, WIRELESS_MODE_B,
-					       power_indbm);
-	u8 ofdmtxpwridx = _phy_dbm_to_txpwr_Idx(hw, WIRELESS_MODE_N_24G,
-						power_indbm);
+	u8 ccktxpwridx = _rtl8723e_phy_dbm_to_txpwr_idx(hw,
+						      WIRELESS_MODE_B,
+						      power_indbm);
+	u8 ofdmtxpwridx = _rtl8723e_phy_dbm_to_txpwr_idx(hw,
+						       WIRELESS_MODE_N_24G,
+						       power_indbm);
 	if (ofdmtxpwridx - rtlefuse->legacy_ht_txpowerdiff > 0)
 		ofdmtxpwridx -= rtlefuse->legacy_ht_txpowerdiff;
 	else
 		ofdmtxpwridx = 0;
 	RT_TRACE(rtlpriv, COMP_TXAGC, DBG_TRACE,
 		 "%lx dBm, ccktxpwridx = %d, ofdmtxpwridx = %d\n",
-		 power_indbm, ccktxpwridx, ofdmtxpwridx);
+		  power_indbm, ccktxpwridx, ofdmtxpwridx);
 	for (idx = 0; idx < 14; idx++) {
 		for (rf_path = 0; rf_path < 2; rf_path++) {
 			rtlefuse->txpwrlevel_cck[rf_path][idx] = ccktxpwridx;
 			rtlefuse->txpwrlevel_ht40_1s[rf_path][idx] =
-							    ofdmtxpwridx;
+			    ofdmtxpwridx;
 			rtlefuse->txpwrlevel_ht40_2s[rf_path][idx] =
-							    ofdmtxpwridx;
+			    ofdmtxpwridx;
 		}
 	}
-	rtl8723ae_phy_set_txpower_level(hw, rtlphy->current_channel);
+	rtl8723e_phy_set_txpower_level(hw, rtlphy->current_channel);
 	return true;
 }
 
-static u8 _phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
-				enum wireless_mode wirelessmode,
-				long power_indbm)
+static u8 _rtl8723e_phy_dbm_to_txpwr_idx(struct ieee80211_hw *hw,
+					 enum wireless_mode wirelessmode,
+					 long power_indbm)
 {
 	u8 txpwridx;
 	long offset;
@@ -645,7 +717,7 @@ static u8 _phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
 	}
 
 	if ((power_indbm - offset) > 0)
-		txpwridx = (u8) ((power_indbm - offset) * 2);
+		txpwridx = (u8)((power_indbm - offset) * 2);
 	else
 		txpwridx = 0;
 
@@ -655,19 +727,48 @@ static u8 _phy_dbm_to_txpwr_Idx(struct ieee80211_hw *hw,
 	return txpwridx;
 }
 
-void rtl8723ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+void rtl8723e_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	enum io_type iotype;
+
+	if (!is_hal_stop(rtlhal)) {
+		switch (operation) {
+		case SCAN_OPT_BACKUP_BAND0:
+			iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Unknown Scan Backup operation.\n");
+			break;
+		}
+	}
+}
+
+void rtl8723e_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u8 reg_bw_opmode;
 	u8 reg_prsr_rsc;
 
 	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
 		 "Switch to %s bandwidth\n",
-		 rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
-		 "20MHz" : "40MHz");
+		  rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" : "40MHz");
 
 	if (is_hal_stop(rtlhal)) {
 		rtlphy->set_bwmode_inprogress = false;
@@ -719,16 +820,16 @@ void rtl8723ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 			 "unknown bandwidth: %#X\n", rtlphy->current_chan_bw);
 		break;
 	}
-	rtl8723ae_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
+	rtl8723e_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
 	rtlphy->set_bwmode_inprogress = false;
-	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "<==\n");
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
 }
 
-void rtl8723ae_phy_set_bw_mode(struct ieee80211_hw *hw,
-			       enum nl80211_channel_type ch_type)
+void rtl8723e_phy_set_bw_mode(struct ieee80211_hw *hw,
+			      enum nl80211_channel_type ch_type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 tmp_bw = rtlphy->current_chan_bw;
 
@@ -736,20 +837,20 @@ void rtl8723ae_phy_set_bw_mode(struct ieee80211_hw *hw,
 		return;
 	rtlphy->set_bwmode_inprogress = true;
 	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
-		rtl8723ae_phy_set_bw_mode_callback(hw);
+		rtl8723e_phy_set_bw_mode_callback(hw);
 	} else {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "FALSE driver sleep or unload\n");
+			 "false driver sleep or unload\n");
 		rtlphy->set_bwmode_inprogress = false;
 		rtlphy->current_chan_bw = tmp_bw;
 	}
 }
 
-void rtl8723ae_phy_sw_chnl_callback(struct ieee80211_hw *hw)
+void rtl8723e_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 delay;
 
 	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
@@ -759,7 +860,7 @@ void rtl8723ae_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 	do {
 		if (!rtlphy->sw_chnl_inprogress)
 			break;
-		if (!_phy_sw_chnl_step_by_step
+		if (!_rtl8723e_phy_sw_chnl_step_by_step
 		    (hw, rtlphy->current_channel, &rtlphy->sw_chnl_stage,
 		     &rtlphy->sw_chnl_step, &delay)) {
 			if (delay > 0)
@@ -771,13 +872,13 @@ void rtl8723ae_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 		}
 		break;
 	} while (true);
-	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "<==\n");
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
 }
 
-u8 rtl8723ae_phy_sw_chnl(struct ieee80211_hw *hw)
+u8 rtl8723e_phy_sw_chnl(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	if (rtlphy->sw_chnl_inprogress)
@@ -790,9 +891,9 @@ u8 rtl8723ae_phy_sw_chnl(struct ieee80211_hw *hw)
 	rtlphy->sw_chnl_stage = 0;
 	rtlphy->sw_chnl_step = 0;
 	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
-		rtl8723ae_phy_sw_chnl_callback(hw);
+		rtl8723e_phy_sw_chnl_callback(hw);
 		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
-			 "sw_chnl_inprogress false schedule workitem\n");
+			 "sw_chnl_inprogress false schdule workitem\n");
 		rtlphy->sw_chnl_inprogress = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
@@ -802,31 +903,33 @@ u8 rtl8723ae_phy_sw_chnl(struct ieee80211_hw *hw)
 	return 1;
 }
 
-static void _rtl8723ae_phy_sw_rf_seting(struct ieee80211_hw *hw, u8 channel)
+static void _rtl8723e_phy_sw_rf_seting(struct ieee80211_hw *hw, u8 channel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	if (IS_81xxC_VENDOR_UMC_B_CUT(rtlhal->version)) {
 		if (channel == 6 && rtlphy->current_chan_bw ==
-		    HT_CHANNEL_WIDTH_20)
-			rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1, MASKDWORD,
-				      0x00255);
+				HT_CHANNEL_WIDTH_20)
+			rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1,
+				      MASKDWORD, 0x00255);
 		else{
-			u32 backupRF0x1A = (u32)rtl_get_rfreg(hw, RF90_PATH_A,
-					   RF_RX_G1, RFREG_OFFSET_MASK);
-			rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1, MASKDWORD,
-				      backupRF0x1A);
+			u32 backuprf0x1a = (u32)rtl_get_rfreg(hw,
+					RF90_PATH_A, RF_RX_G1,
+					RFREG_OFFSET_MASK);
+			rtl_set_rfreg(hw, RF90_PATH_A, RF_RX_G1,
+				      MASKDWORD, backuprf0x1a);
 		}
 	}
 }
 
-static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
-				      u8 *stage, u8 *step, u32 *delay)
+static bool _rtl8723e_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+					       u8 channel, u8 *stage, u8 *step,
+					       u32 *delay)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
 	u32 precommoncmdcnt;
 	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
@@ -839,14 +942,16 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 
 	precommoncmdcnt = 0;
 	rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
-					 MAX_PRECMD_CNT, CMDID_SET_TXPOWEROWER_LEVEL,
-					 0, 0, 0);
+					 MAX_PRECMD_CNT,
+					 CMDID_SET_TXPOWEROWER_LEVEL, 0, 0, 0);
 	rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
 					 MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+
 	postcommoncmdcnt = 0;
 
 	rtl8723_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
 					 MAX_POSTCMD_CNT, CMDID_END, 0, 0, 0);
+
 	rfdependcmdcnt = 0;
 
 	RT_ASSERT((channel >= 1 && channel <= 14),
@@ -854,10 +959,11 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 
 	rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
 					 MAX_RFDEPENDCMD_CNT, CMDID_RF_WRITEREG,
-				  RF_CHNLBW, channel, 10);
+					 RF_CHNLBW, channel, 10);
 
 	rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
-					 MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0, 0);
+					 MAX_RFDEPENDCMD_CNT, CMDID_END, 0, 0,
+					 0);
 
 	do {
 		switch (*stage) {
@@ -870,6 +976,10 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 		case 2:
 			currentcmd = &postcommoncmd[*step];
 			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Invalid 'stage' = %d, Check it!\n", *stage);
+			return true;
 		}
 
 		if (currentcmd->cmdid == CMDID_END) {
@@ -884,7 +994,7 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 
 		switch (currentcmd->cmdid) {
 		case CMDID_SET_TXPOWEROWER_LEVEL:
-			rtl8723ae_phy_set_txpower_level(hw, channel);
+			rtl8723e_phy_set_txpower_level(hw, channel);
 			break;
 		case CMDID_WRITEPORT_ULONG:
 			rtl_write_dword(rtlpriv, currentcmd->para1,
@@ -909,10 +1019,10 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 					      RFREG_OFFSET_MASK,
 					      rtlphy->rfreg_chnlval[rfpath]);
 			}
-			_rtl8723ae_phy_sw_rf_seting(hw, channel);
+			_rtl8723e_phy_sw_rf_seting(hw, channel);
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 				 "switch case not process\n");
 			break;
 		}
@@ -925,7 +1035,7 @@ static bool _phy_sw_chnl_step_by_step(struct ieee80211_hw *hw, u8 channel,
 	return false;
 }
 
-static u8 _rtl8723ae_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
+static u8 _rtl8723e_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
 {
 	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
 	u8 result = 0x00;
@@ -968,7 +1078,7 @@ static u8 _rtl8723ae_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
 	return result;
 }
 
-static u8 _rtl8723ae_phy_path_b_iqk(struct ieee80211_hw *hw)
+static u8 _rtl8723e_phy_path_b_iqk(struct ieee80211_hw *hw)
 {
 	u32 reg_eac, reg_eb4, reg_ebc, reg_ec4, reg_ecc;
 	u8 result = 0x00;
@@ -995,8 +1105,8 @@ static u8 _rtl8723ae_phy_path_b_iqk(struct ieee80211_hw *hw)
 	return result;
 }
 
-static bool phy_simularity_comp(struct ieee80211_hw *hw, long result[][8],
-				u8 c1, u8 c2)
+static bool _rtl8723e_phy_simularity_compare(struct ieee80211_hw *hw,
+					     long result[][8], u8 c1, u8 c2)
 {
 	u32 i, j, diff, simularity_bitmap, bound;
 
@@ -1047,11 +1157,11 @@ static bool phy_simularity_comp(struct ieee80211_hw *hw, long result[][8],
 
 }
 
-static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
-					long result[][8], u8 t, bool is2t)
+static void _rtl8723e_phy_iq_calibrate(struct ieee80211_hw *hw,
+				       long result[][8], u8 t, bool is2t)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 i;
 	u8 patha_ok, pathb_ok;
 	u32 adda_reg[IQK_ADDA_REG_NUM] = {
@@ -1060,22 +1170,28 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 		0xe88, 0xe8c, 0xed0, 0xed4,
 		0xed8, 0xedc, 0xee0, 0xeec
 	};
+
 	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
 		0x522, 0x550, 0x551, 0x040
 	};
+
 	const u32 retrycount = 2;
 
+	u32 bbvalue;
+
 	if (t == 0) {
-		rtl8723_save_adda_registers(hw, adda_reg, rtlphy->adda_backup,
-					    16);
+		bbvalue = rtl_get_bbreg(hw, 0x800, MASKDWORD);
+
+		rtl8723_save_adda_registers(hw, adda_reg,
+					    rtlphy->adda_backup, 16);
 		rtl8723_phy_save_mac_registers(hw, iqk_mac_reg,
 					       rtlphy->iqk_mac_backup);
 	}
 	rtl8723_phy_path_adda_on(hw, adda_reg, true, is2t);
 	if (t == 0) {
 		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
-						 RFPGA0_XA_HSSIPARAMETER1,
-						 BIT(8));
+					RFPGA0_XA_HSSIPARAMETER1,
+					BIT(8));
 	}
 
 	if (!rtlphy->rfpi_enable)
@@ -1101,7 +1217,7 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x01007c00);
 	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x01004800);
 	for (i = 0; i < retrycount; i++) {
-		patha_ok = _rtl8723ae_phy_path_a_iqk(hw, is2t);
+		patha_ok = _rtl8723e_phy_path_a_iqk(hw, is2t);
 		if (patha_ok == 0x03) {
 			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
 					0x3FF0000) >> 16;
@@ -1115,7 +1231,8 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 		} else if (i == (retrycount - 1) && patha_ok == 0x01)
 
 			result[t][0] = (rtl_get_bbreg(hw, 0xe94,
-					MASKDWORD) & 0x3FF0000) >> 16;
+						      MASKDWORD) & 0x3FF0000) >>
+			    16;
 		result[t][1] =
 		    (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) & 0x3FF0000) >> 16;
 
@@ -1125,11 +1242,12 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 		rtl8723_phy_path_a_standby(hw);
 		rtl8723_phy_path_adda_on(hw, adda_reg, false, is2t);
 		for (i = 0; i < retrycount; i++) {
-			pathb_ok = _rtl8723ae_phy_path_b_iqk(hw);
+			pathb_ok = _rtl8723e_phy_path_b_iqk(hw);
 			if (pathb_ok == 0x03) {
-				result[t][4] =
-				    (rtl_get_bbreg(hw, 0xeb4, MASKDWORD) &
-				     0x3FF0000) >> 16;
+				result[t][4] = (rtl_get_bbreg(hw,
+							      0xeb4,
+							      MASKDWORD) &
+						0x3FF0000) >> 16;
 				result[t][5] =
 				    (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
 				     0x3FF0000) >> 16;
@@ -1141,9 +1259,10 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 				     0x3FF0000) >> 16;
 				break;
 			} else if (i == (retrycount - 1) && pathb_ok == 0x01) {
-				result[t][4] =
-				    (rtl_get_bbreg(hw, 0xeb4, MASKDWORD) &
-				     0x3FF0000) >> 16;
+				result[t][4] = (rtl_get_bbreg(hw,
+							      0xeb4,
+							      MASKDWORD) &
+						0x3FF0000) >> 16;
 			}
 			result[t][5] = (rtl_get_bbreg(hw, 0xebc, MASKDWORD) &
 					0x3FF0000) >> 16;
@@ -1166,11 +1285,11 @@ static void _rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw,
 	}
 }
 
-static void _rtl8723ae_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
+static void _rtl8723e_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmpreg;
 	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
 
@@ -1211,14 +1330,14 @@ static void _rtl8723ae_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 	}
 }
 
-static void _rtl8723ae_phy_set_rfpath_switch(struct ieee80211_hw *hw,
-					     bool bmain, bool is2t)
+static void _rtl8723e_phy_set_rfpath_switch(struct ieee80211_hw *hw,
+					    bool bmain, bool is2t)
 {
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	if (is_hal_stop(rtlhal)) {
 		rtl_set_bbreg(hw, REG_LEDCFG0, BIT(23), 0x01);
-		rtl_set_bbreg(hw, rFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
+		rtl_set_bbreg(hw, RFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
 	}
 	if (is2t) {
 		if (bmain)
@@ -1234,21 +1353,23 @@ static void _rtl8723ae_phy_set_rfpath_switch(struct ieee80211_hw *hw,
 			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE, 0x300, 0x1);
 
 	}
+
 }
 
 #undef IQK_ADDA_REG_NUM
 #undef IQK_DELAY_TIME
 
-void rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
+void rtl8723e_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
 	long result[4][8];
 	u8 i, final_candidate;
-	bool patha_ok, pathb_ok;
-	long reg_e94, reg_e9c, reg_ea4, reg_eb4, reg_ebc, reg_tmp = 0;
+	bool b_patha_ok, b_pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
+	    reg_ecc, reg_tmp = 0;
 	bool is12simular, is13simular, is23simular;
-	bool start_conttx = false, singletone = false;
 	u32 iqk_bb_reg[10] = {
 		ROFDM0_XARXIQIMBALANCE,
 		ROFDM0_XBRXIQIMBALANCE,
@@ -1262,13 +1383,12 @@ void rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		ROFDM0_RXIQEXTANTA
 	};
 
-	if (recovery) {
-		rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
+	if (b_recovery) {
+		rtl8723_phy_reload_adda_registers(hw,
+						  iqk_bb_reg,
 						  rtlphy->iqk_bb_backup, 10);
 		return;
 	}
-	if (start_conttx || singletone)
-		return;
 	for (i = 0; i < 8; i++) {
 		result[0][i] = 0;
 		result[1][i] = 0;
@@ -1276,30 +1396,33 @@ void rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		result[3][i] = 0;
 	}
 	final_candidate = 0xff;
-	patha_ok = false;
-	pathb_ok = false;
+	b_patha_ok = false;
+	b_pathb_ok = false;
 	is12simular = false;
 	is23simular = false;
 	is13simular = false;
 	for (i = 0; i < 3; i++) {
-		_rtl8723ae_phy_iq_calibrate(hw, result, i, false);
+		_rtl8723e_phy_iq_calibrate(hw, result, i, false);
 		if (i == 1) {
-			is12simular = phy_simularity_comp(hw, result, 0, 1);
+			is12simular =
+			  _rtl8723e_phy_simularity_compare(hw, result, 0, 1);
 			if (is12simular) {
 				final_candidate = 0;
 				break;
 			}
 		}
 		if (i == 2) {
-			is13simular = phy_simularity_comp(hw, result, 0, 2);
+			is13simular =
+			  _rtl8723e_phy_simularity_compare(hw, result, 0, 2);
 			if (is13simular) {
 				final_candidate = 0;
 				break;
 			}
-			is23simular = phy_simularity_comp(hw, result, 1, 2);
-			if (is23simular) {
+			is23simular =
+			  _rtl8723e_phy_simularity_compare(hw, result, 1, 2);
+			if (is23simular)
 				final_candidate = 1;
-			} else {
+			else {
 				for (i = 0; i < 8; i++)
 					reg_tmp += result[3][i];
 
@@ -1314,50 +1437,54 @@ void rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		reg_e94 = result[i][0];
 		reg_e9c = result[i][1];
 		reg_ea4 = result[i][2];
+		reg_eac = result[i][3];
 		reg_eb4 = result[i][4];
 		reg_ebc = result[i][5];
+		reg_ec4 = result[i][6];
+		reg_ecc = result[i][7];
 	}
 	if (final_candidate != 0xff) {
 		rtlphy->reg_e94 = reg_e94 = result[final_candidate][0];
 		rtlphy->reg_e9c = reg_e9c = result[final_candidate][1];
 		reg_ea4 = result[final_candidate][2];
+		reg_eac = result[final_candidate][3];
 		rtlphy->reg_eb4 = reg_eb4 = result[final_candidate][4];
 		rtlphy->reg_ebc = reg_ebc = result[final_candidate][5];
-		patha_ok = pathb_ok = true;
+		reg_ec4 = result[final_candidate][6];
+		reg_ecc = result[final_candidate][7];
+		b_patha_ok = true;
+		b_pathb_ok = true;
 	} else {
 		rtlphy->reg_e94 = rtlphy->reg_eb4 = 0x100;
 		rtlphy->reg_e9c = rtlphy->reg_ebc = 0x0;
 	}
-	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
-		rtl8723_phy_path_a_fill_iqk_matrix(hw, patha_ok, result,
+	if (reg_e94 != 0)
+		rtl8723_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
 						   final_candidate,
 						   (reg_ea4 == 0));
-	rtl8723_save_adda_registers(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 10);
+	rtl8723_save_adda_registers(hw, iqk_bb_reg,
+				    rtlphy->iqk_bb_backup, 10);
 }
 
-void rtl8723ae_phy_lc_calibrate(struct ieee80211_hw *hw)
+void rtl8723e_phy_lc_calibrate(struct ieee80211_hw *hw)
 {
-	bool start_conttx = false, singletone = false;
-
-	if (start_conttx || singletone)
-		return;
-	_rtl8723ae_phy_lc_calibrate(hw, false);
+	_rtl8723e_phy_lc_calibrate(hw, false);
 }
 
-void rtl8723ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
+void rtl8723e_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
 {
-	_rtl8723ae_phy_set_rfpath_switch(hw, bmain, false);
+	_rtl8723e_phy_set_rfpath_switch(hw, bmain, false);
 }
 
-bool rtl8723ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
+bool rtl8723e_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	bool postprocessing = false;
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
-		 iotype, rtlphy->set_io_inprogress);
+		  iotype, rtlphy->set_io_inprogress);
 	do {
 		switch (iotype) {
 		case IO_CMD_RESUME_DM_BY_SCAN:
@@ -1365,13 +1492,13 @@ bool rtl8723ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 				 "[IO CMD] Resume DM after scan.\n");
 			postprocessing = true;
 			break;
-		case IO_CMD_PAUSE_DM_BY_SCAN:
+		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 				 "[IO CMD] Pause DM before scan.\n");
 			postprocessing = true;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 				 "switch case not process\n");
 			break;
 		}
@@ -1382,42 +1509,42 @@ bool rtl8723ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 	} else {
 		return false;
 	}
-	rtl8723ae_phy_set_io(hw);
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "<--IO Type(%#x)\n", iotype);
+	rtl8723e_phy_set_io(hw);
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
 	return true;
 }
 
-static void rtl8723ae_phy_set_io(struct ieee80211_hw *hw)
+static void rtl8723e_phy_set_io(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
-		 rtlphy->current_io_type, rtlphy->set_io_inprogress);
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
 	switch (rtlphy->current_io_type) {
 	case IO_CMD_RESUME_DM_BY_SCAN:
 		dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
-		rtl8723ae_dm_write_dig(hw);
-		rtl8723ae_phy_set_txpower_level(hw, rtlphy->current_channel);
+		rtl8723e_dm_write_dig(hw);
+		rtl8723e_phy_set_txpower_level(hw, rtlphy->current_channel);
 		break;
-	case IO_CMD_PAUSE_DM_BY_SCAN:
+	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
 		rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
 		dm_digtable->cur_igvalue = 0x17;
-		rtl8723ae_dm_write_dig(hw);
+		rtl8723e_dm_write_dig(hw);
 		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 			 "switch case not process\n");
 		break;
 	}
 	rtlphy->set_io_inprogress = false;
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 "<---(%#x)\n", rtlphy->current_io_type);
+		 "(%#x)\n", rtlphy->current_io_type);
 }
 
-static void rtl8723ae_phy_set_rf_on(struct ieee80211_hw *hw)
+static void rtl8723e_phy_set_rf_on(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -1429,11 +1556,11 @@ static void rtl8723ae_phy_set_rf_on(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
 }
 
-static void _rtl8723ae_phy_set_rf_sleep(struct ieee80211_hw *hw)
+static void _rtl8723e_phy_set_rf_sleep(struct ieee80211_hw *hw)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u32 u4b_tmp;
 	u8 delay = 5;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
 	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
@@ -1459,45 +1586,47 @@ static void _rtl8723ae_phy_set_rf_sleep(struct ieee80211_hw *hw)
 	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
 }
 
-static bool _rtl8723ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
-					      enum rf_pwrstate rfpwr_state)
+static bool _rtl8723e_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					     enum rf_pwrstate rfpwr_state)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl8192_tx_ring *ring = NULL;
 	bool bresult = true;
 	u8 i, queue_id;
+	struct rtl8192_tx_ring *ring = NULL;
 
 	switch (rfpwr_state) {
 	case ERFON:
 		if ((ppsc->rfpwr_state == ERFOFF) &&
 		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
 			bool rtstatus;
-			u32 InitializeCount = 0;
+			u32 initializecount = 0;
+
 			do {
-				InitializeCount++;
+				initializecount++;
 				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 					 "IPS Set eRf nic enable\n");
 				rtstatus = rtl_ps_enable_nic(hw);
-			} while ((rtstatus != true) && (InitializeCount < 10));
+			} while (!rtstatus && (initializecount < 10));
 			RT_CLEAR_PS_LEVEL(ppsc,
 					  RT_RF_OFF_LEVL_HALT_NIC);
 		} else {
 			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 				 "Set ERFON sleeped:%d ms\n",
-				 jiffies_to_msecs(jiffies -
-				 ppsc->last_sleep_jiffies));
+				  jiffies_to_msecs(jiffies -
+						   ppsc->
+						   last_sleep_jiffies));
 			ppsc->last_awake_jiffies = jiffies;
-			rtl8723ae_phy_set_rf_on(hw);
+			rtl8723e_phy_set_rf_on(hw);
 		}
 		if (mac->link_state == MAC80211_LINKED) {
 			rtlpriv->cfg->ops->led_control(hw,
-					LED_CTL_LINK);
+						       LED_CTL_LINK);
 		} else {
 			rtlpriv->cfg->ops->led_control(hw,
-					LED_CTL_NO_LINK);
+						       LED_CTL_NO_LINK);
 		}
 		break;
 	case ERFOFF:
@@ -1509,10 +1638,10 @@ static bool _rtl8723ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		} else {
 			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
 				rtlpriv->cfg->ops->led_control(hw,
-					LED_CTL_NO_LINK);
+						LED_CTL_NO_LINK);
 			} else {
 				rtlpriv->cfg->ops->led_control(hw,
-					LED_CTL_POWER_OFF);
+						LED_CTL_POWER_OFF);
 			}
 		}
 		break;
@@ -1522,7 +1651,8 @@ static bool _rtl8723ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		for (queue_id = 0, i = 0;
 		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
 			ring = &pcipriv->dev.tx_ring[queue_id];
-			if (skb_queue_len(&ring->queue) == 0) {
+			if (queue_id == BEACON_QUEUE ||
+			    skb_queue_len(&ring->queue) == 0) {
 				queue_id++;
 				continue;
 			} else {
@@ -1536,22 +1666,23 @@ static bool _rtl8723ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
 			}
 			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
-					 MAX_DOZE_WAITING_TIMES_9x,
-					 queue_id,
-					 skb_queue_len(&ring->queue));
+					 "ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue));
 				break;
 			}
 		}
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 "Set ERFSLEEP awaked:%d ms\n",
-			 jiffies_to_msecs(jiffies - ppsc->last_awake_jiffies));
+			  jiffies_to_msecs(jiffies -
+					   ppsc->last_awake_jiffies));
 		ppsc->last_sleep_jiffies = jiffies;
-		_rtl8723ae_phy_set_rf_sleep(hw);
+		_rtl8723e_phy_set_rf_sleep(hw);
 		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		bresult = false;
 		break;
 	}
@@ -1560,14 +1691,15 @@ static bool _rtl8723ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
 	return bresult;
 }
 
-bool rtl8723ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
-				      enum rf_pwrstate rfpwr_state)
+bool rtl8723e_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				     enum rf_pwrstate rfpwr_state)
 {
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
 	bool bresult = false;
 
 	if (rfpwr_state == ppsc->rfpwr_state)
 		return bresult;
-	bresult = _rtl8723ae_phy_set_rf_power_state(hw, rfpwr_state);
+	bresult = _rtl8723e_phy_set_rf_power_state(hw, rfpwr_state);
 	return bresult;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h
index cd43139ed332..b85f5c7c5c01 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/phy.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -39,6 +35,7 @@
 #define RT_CANNOT_IO(hw)			false
 #define HIGHPOWER_RADIOA_ARRAYLEN		22
 
+#define IQK_ADDA_REG_NUM			16
 #define MAX_TOLERANCE				5
 #define	IQK_DELAY_TIME				1
 
@@ -49,12 +46,15 @@
 
 #define LOOP_LIMIT				5
 #define MAX_STALL_TIME				50
-#define AntennaDiversityValue			0x80
+#define ANTENNADIVERSITYVALUE			0x80
 #define MAX_TXPWR_IDX_NMODE_92S			63
 #define Reset_Cnt_Limit				3
 
+#define IQK_ADDA_REG_NUM			16
 #define IQK_MAC_REG_NUM				4
 
+#define IQK_DELAY_TIME				1
+
 #define RF6052_MAX_PATH				2
 
 #define CT_OFFSET_MAC_ADDR			0X16
@@ -166,36 +166,37 @@ struct tx_power_struct {
 	u32 mcs_original_offset[4][16];
 };
 
-u32 rtl8723ae_phy_query_rf_reg(struct ieee80211_hw *hw,
-			       enum radio_path rfpath, u32 regaddr,
-			       u32 bitmask);
-void rtl8723ae_phy_set_rf_reg(struct ieee80211_hw *hw,
+u32 rtl8723e_phy_query_rf_reg(struct ieee80211_hw *hw,
 			      enum radio_path rfpath, u32 regaddr,
-			      u32 bitmask, u32 data);
-bool rtl8723ae_phy_mac_config(struct ieee80211_hw *hw);
-bool rtl8723ae_phy_bb_config(struct ieee80211_hw *hw);
-bool rtl8723ae_phy_rf_config(struct ieee80211_hw *hw);
+			      u32 bitmask);
+void rtl8723e_phy_set_rf_reg(struct ieee80211_hw *hw,
+			     enum radio_path rfpath, u32 regaddr,
+			     u32 bitmask, u32 data);
+bool rtl8723e_phy_mac_config(struct ieee80211_hw *hw);
+bool rtl8723e_phy_bb_config(struct ieee80211_hw *hw);
+bool rtl8723e_phy_rf_config(struct ieee80211_hw *hw);
 bool rtl92c_phy_config_rf_with_feaderfile(struct ieee80211_hw *hw,
 					  enum radio_path rfpath);
-void rtl8723ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
-void rtl8723ae_phy_get_txpower_level(struct ieee80211_hw *hw,
-				     long *powerlevel);
-void rtl8723ae_phy_set_txpower_level(struct ieee80211_hw *hw,
-				     u8 channel);
-bool rtl8723ae_phy_update_txpower_dbm(struct ieee80211_hw *hw,
-				      long power_indbm);
-void rtl8723ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
-void rtl8723ae_phy_set_bw_mode(struct ieee80211_hw *hw,
-			       enum nl80211_channel_type ch_type);
-void rtl8723ae_phy_sw_chnl_callback(struct ieee80211_hw *hw);
-u8 rtl8723ae_phy_sw_chnl(struct ieee80211_hw *hw);
-void rtl8723ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery);
-void rtl8723ae_phy_lc_calibrate(struct ieee80211_hw *hw);
-void rtl8723ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
-bool rtl8723ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
-					     enum radio_path rfpath);
-bool rtl8723ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
-bool rtl8723ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
-				      enum rf_pwrstate rfpwr_state);
+void rtl8723e_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+void rtl8723e_phy_get_txpower_level(struct ieee80211_hw *hw,
+				    long *powerlevel);
+void rtl8723e_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
+bool rtl8723e_phy_update_txpower_dbm(struct ieee80211_hw *hw,
+				     long power_indbm);
+void rtl8723e_phy_scan_operation_backup(struct ieee80211_hw *hw,
+					u8 operation);
+void rtl8723e_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+void rtl8723e_phy_set_bw_mode(struct ieee80211_hw *hw,
+			      enum nl80211_channel_type ch_type);
+void rtl8723e_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+u8 rtl8723e_phy_sw_chnl(struct ieee80211_hw *hw);
+void rtl8723e_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+void rtl8723e_phy_lc_calibrate(struct ieee80211_hw *hw);
+void rtl8723e_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
+bool rtl8723e_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					    enum radio_path rfpath);
+bool rtl8723e_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
+bool rtl8723e_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				     enum rf_pwrstate rfpwr_state);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.c
index df6ca9a57f7f..2f7f81af8a55 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -27,83 +23,90 @@
  *
  *****************************************************************************/
 
-#include "pwrseqcmd.h"
+#include "../pwrseqcmd.h"
 #include "pwrseq.h"
 
-/* drivers should parse arrays below and do the corresponding actions */
-
+/* drivers should parse below arrays and do the corresponding actions */
 /*3 Power on  Array*/
-struct wlan_pwr_cfg rtl8723A_power_on_flow[RTL8723A_TRANS_CARDEMU_TO_ACT_STPS
-					+ RTL8723A_TRANS_END_STPS] = {
-	RTL8723A_TRANS_CARDEMU_TO_ACT,
+struct wlan_pwr_cfg rtl8723A_power_on_flow
+		[RTL8723A_TRANS_CARDEMU_TO_ACT_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
+	RTL8723A_TRANS_CARDEMU_TO_ACT
 	RTL8723A_TRANS_END
 };
 
 /*3Radio off GPIO Array */
-struct wlan_pwr_cfg rtl8723A_radio_off_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-					+ RTL8723A_TRANS_END_STPS] = {
-	RTL8723A_TRANS_ACT_TO_CARDEMU,
+struct wlan_pwr_cfg rtl8723A_radio_off_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
+	RTL8723A_TRANS_ACT_TO_CARDEMU
 	RTL8723A_TRANS_END
 };
 
 /*3Card Disable Array*/
-struct wlan_pwr_cfg
-rtl8723A_card_disable_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-			  + RTL8723A_TRANS_CARDEMU_TO_PDN_STPS
-			  + RTL8723A_TRANS_END_STPS] = {
-	RTL8723A_TRANS_ACT_TO_CARDEMU,
-	RTL8723A_TRANS_CARDEMU_TO_CARDDIS,
+struct wlan_pwr_cfg rtl8723A_card_disable_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
+	RTL8723A_TRANS_ACT_TO_CARDEMU
+	RTL8723A_TRANS_CARDEMU_TO_CARDDIS
 	RTL8723A_TRANS_END
 };
 
 /*3 Card Enable Array*/
-struct wlan_pwr_cfg rtl8723A_card_enable_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-					+ RTL8723A_TRANS_CARDEMU_TO_PDN_STPS
-					+ RTL8723A_TRANS_END_STPS] = {
-	RTL8723A_TRANS_CARDDIS_TO_CARDEMU,
-	RTL8723A_TRANS_CARDEMU_TO_ACT,
+struct wlan_pwr_cfg rtl8723A_card_enable_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
+	RTL8723A_TRANS_CARDDIS_TO_CARDEMU
+	RTL8723A_TRANS_CARDEMU_TO_ACT
 	RTL8723A_TRANS_END
 };
 
 /*3Suspend Array*/
-struct wlan_pwr_cfg rtl8723A_suspend_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-					+ RTL8723A_TRANS_CARDEMU_TO_SUS_STPS
-					+ RTL8723A_TRANS_END_STPS] = {
-	RTL8723A_TRANS_ACT_TO_CARDEMU,
-	RTL8723A_TRANS_CARDEMU_TO_SUS,
+struct wlan_pwr_cfg rtl8723A_suspend_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
+	RTL8723A_TRANS_ACT_TO_CARDEMU
+	RTL8723A_TRANS_CARDEMU_TO_SUS
 	RTL8723A_TRANS_END
 };
 
 /*3 Resume Array*/
-struct wlan_pwr_cfg rtl8723A_resume_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-					+ RTL8723A_TRANS_CARDEMU_TO_SUS_STPS
-					+ RTL8723A_TRANS_END_STPS] = {
-	RTL8723A_TRANS_SUS_TO_CARDEMU,
-	RTL8723A_TRANS_CARDEMU_TO_ACT,
+struct wlan_pwr_cfg rtl8723A_resume_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
+	RTL8723A_TRANS_SUS_TO_CARDEMU
+	RTL8723A_TRANS_CARDEMU_TO_ACT
 	RTL8723A_TRANS_END
 };
 
 /*3HWPDN Array*/
-struct wlan_pwr_cfg rtl8723A_hwpdn_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-				+ RTL8723A_TRANS_CARDEMU_TO_PDN_STPS
-				+ RTL8723A_TRANS_END_STPS] = {
-	RTL8723A_TRANS_ACT_TO_CARDEMU,
-	RTL8723A_TRANS_CARDEMU_TO_PDN,
+struct wlan_pwr_cfg rtl8723A_hwpdn_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
+	RTL8723A_TRANS_ACT_TO_CARDEMU
+	RTL8723A_TRANS_CARDEMU_TO_PDN
 	RTL8723A_TRANS_END
 };
 
 /*3 Enter LPS */
-struct wlan_pwr_cfg rtl8723A_enter_lps_flow[RTL8723A_TRANS_ACT_TO_LPS_STPS
-					+ RTL8723A_TRANS_END_STPS] = {
+struct wlan_pwr_cfg rtl8723A_enter_lps_flow
+		[RTL8723A_TRANS_ACT_TO_LPS_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
 	/*FW behavior*/
-	RTL8723A_TRANS_ACT_TO_LPS,
+	RTL8723A_TRANS_ACT_TO_LPS
 	RTL8723A_TRANS_END
 };
 
 /*3 Leave LPS */
-struct wlan_pwr_cfg rtl8723A_leave_lps_flow[RTL8723A_TRANS_LPS_TO_ACT_STPS
-					+ RTL8723A_TRANS_END_STPS] = {
+struct wlan_pwr_cfg rtl8723A_leave_lps_flow
+		[RTL8723A_TRANS_LPS_TO_ACT_STEPS +
+		 RTL8723A_TRANS_END_STEPS] = {
 	/*FW behavior*/
-	RTL8723A_TRANS_LPS_TO_ACT,
+	RTL8723A_TRANS_LPS_TO_ACT
 	RTL8723A_TRANS_END
 };
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h
index a418acb4d0ca..4ac7db526f15 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseq.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,282 +26,305 @@
 #ifndef __RTL8723E_PWRSEQ_H__
 #define __RTL8723E_PWRSEQ_H__
 
+#include "../pwrseqcmd.h"
 /*
-	Check document WM-20110607-Paul-RTL8723A_Power_Architecture-R02.vsd
-	There are 6 HW Power States:
-	0: POFF--Power Off
-	1: PDN--Power Down
-	2: CARDEMU--Card Emulation
-	3: ACT--Active Mode
-	4: LPS--Low Power State
-	5: SUS--Suspend
-
-	The transision from different states are defined below
-	TRANS_CARDEMU_TO_ACT
-	TRANS_ACT_TO_CARDEMU
-	TRANS_CARDEMU_TO_SUS
-	TRANS_SUS_TO_CARDEMU
-	TRANS_CARDEMU_TO_PDN
-	TRANS_ACT_TO_LPS
-	TRANS_LPS_TO_ACT
+ *	Check document WM-20110607-Paul-RTL8723A_Power_Architecture-R02.vsd
+ *	There are 6 HW Power States:
+ *	0: POFF--Power Off
+ *	1: PDN--Power Down
+ *	2: CARDEMU--Card Emulation
+ *	3: ACT--Active Mode
+ *	4: LPS--Low Power State
+ *	5: SUS--Suspend
+ *
+ *	The transision from different states are defined below
+ *	TRANS_CARDEMU_TO_ACT
+ *	TRANS_ACT_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_SUS
+ *	TRANS_SUS_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_PDN
+ *	TRANS_ACT_TO_LPS
+ *	TRANS_LPS_TO_ACT
+ *
+ *	TRANS_END
+ */
 
-	TRANS_END
-*/
+#define	RTL8723A_TRANS_CARDEMU_TO_ACT_STEPS	10
+#define	RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS	10
+#define	RTL8723A_TRANS_CARDEMU_TO_SUS_STEPS	10
+#define	RTL8723A_TRANS_SUS_TO_CARDEMU_STEPS	10
+#define	RTL8723A_TRANS_CARDEMU_TO_PDN_STEPS	10
+#define	RTL8723A_TRANS_PDN_TO_CARDEMU_STEPS	10
+#define	RTL8723A_TRANS_ACT_TO_LPS_STEPS		15
+#define	RTL8723A_TRANS_LPS_TO_ACT_STEPS		15
+#define	RTL8723A_TRANS_END_STEPS		1
 
-#define	RTL8723A_TRANS_CARDEMU_TO_ACT_STPS	10
-#define	RTL8723A_TRANS_ACT_TO_CARDEMU_STPS	10
-#define	RTL8723A_TRANS_CARDEMU_TO_SUS_STPS	10
-#define	RTL8723A_TRANS_SUS_TO_CARDEMU_STPS	10
-#define	RTL8723A_TRANS_CARDEMU_TO_PDN_STPS	10
-#define	RTL8723A_TRANS_PDN_TO_CARDEMU_STPS	10
-#define	RTL8723A_TRANS_ACT_TO_LPS_STPS		15
-#define	RTL8723A_TRANS_LPS_TO_ACT_STPS		15
-#define	RTL8723A_TRANS_END_STPS			1
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }*/
 
+#define RTL8723A_TRANS_CARDEMU_TO_ACT	\
+	/* disable SW LPS 0x04[10]=0*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(2), 0},\
+	/* wait till 0x04[17] = 1    power ready*/	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)},\
+	/* release WLON reset  0x04[16]=1*/	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},\
+	/* disable HWPDN 0x04[15]=0*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},\
+	/* disable WL suspend*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT(4)|BIT(3)), 0},\
+	/* polling until return 0*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(0), 0},
 
-#define RTL8723A_TRANS_CARDEMU_TO_ACT					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, \
-	 *  comments here*/						\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(2), 0},		\
-		/* disable SW LPS 0x04[10]=0*/				\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)},	\
-		/* wait till 0x04[17] = 1    power ready*/		\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},	\
-		/* release WLON reset  0x04[16]=1*/			\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},		\
-		/* disable HWPDN 0x04[15]=0*/				\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT(4)|BIT(3)), 0},	\
-	/* disable WL suspend*/						\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},	\
-		/* polling until return 0*/				\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(0), 0}
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
 
-#define RTL8723A_TRANS_ACT_TO_CARDEMU					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, \
-	 *  comments here*/						\
-	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},		\
-		/*0x1F[7:0] = 0 turn off RF*/				\
-	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},		\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0}
+#define RTL8723A_TRANS_ACT_TO_CARDEMU	\
+	/*0x1F[7:0] = 0 turn off RF*/ \
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},	\
+	{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)}, \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0},
 
-#define RTL8723A_TRANS_CARDEMU_TO_SUS					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, \
-	 *  comments here*/						\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4)|BIT(3),		\
-		(BIT(4)|BIT(3))},					\
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/
+#define RTL8723A_TRANS_CARDEMU_TO_SUS			\
 		/*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK |	\
-		PWR_INTF_SDIO_MSK,					\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},\
-		 /*0x04[12:11] = 2b'01 enable WL suspend*/		\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-		PWR_BASEADDR_MAC,					\
-		PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)|BIT(4)},		\
-		 /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO,					\
-		PWR_CMD_WRITE, BIT(0), BIT(0)},				\
-		/*Set SDIO suspend local register*/			\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO,					\
-		PWR_CMD_POLLING, BIT(1), 0}				\
-		/*wait power state to suspend*/
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, \
+		BIT(4)|BIT(3), (BIT(4)|BIT(3))},\
+/*0x04[12:11] = 2b'01 enable WL suspend*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK| \
+		PWR_INTF_SDIO_MSK,\
+		PWR_BASEADDR_MAC, \
+		PWR_CMD_WRITE, \
+		BIT(3)|BIT(4), BIT(3)}, \
+/*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, \
+		PWR_CMD_WRITE, BIT(3)|BIT(4), \
+		BIT(3)|BIT(4)}, \
+/*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO,\
+		PWR_CMD_WRITE, BIT(0), BIT(0)}, \
+/*wait power state to suspend*/ \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO,\
+		PWR_CMD_POLLING, BIT(1), 0},
+
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
+
+#define RTL8723A_TRANS_SUS_TO_CARDEMU	\
+ /*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+		PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},\
+ /*wait power state to suspend*/ \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+		PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},\
+ /*0x04[12:11] = 2b'01enable WL suspend*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},
+
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
+
+#define RTL8723A_TRANS_CARDEMU_TO_CARDDIS \
+ /*0x04[12:11] = 2b'01 enable WL suspend*/	 \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, \
+		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)}, \
+/*0x04[10] = 1, enable SW LPS*/	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(2), BIT(2)}, \
+/*Set SDIO suspend local register*/ \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO,\
+		PWR_CMD_WRITE, BIT(0), BIT(0)}, \
+ /*wait power state to suspend*/ \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO,\
+		PWR_CMD_POLLING, BIT(1), 0},
+
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
 
-#define RTL8723A_TRANS_SUS_TO_CARDEMU					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},		\
-		/*Set SDIO suspend local register*/			\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},	\
-		/*wait power state to suspend*/				\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0}	\
-		/*0x04[12:11] = 2b'01enable WL suspend*/
+#define RTL8723A_TRANS_CARDDIS_TO_CARDEMU\
+/*Set SDIO suspend local register*/	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO,\
+		PWR_CMD_WRITE, BIT(0), 0}, \
+ /*wait power state to suspend*/ \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO,\
+		PWR_CMD_POLLING, BIT(1), BIT(1)},\
+ /*0x04[12:11] = 2b'00enable WL suspend*/ \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(3)|BIT(4), 0},\
+/*PCIe DMA start*/ \
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0},
 
-#define RTL8723A_TRANS_CARDEMU_TO_CARDDIS				\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
-	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,				\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},\
-		/*0x04[12:11] = 2b'01 enable WL suspend*/		\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(2), BIT(2)},	\
-		/*0x04[10] = 1, enable SW LPS*/				\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},	\
-		/*Set SDIO suspend local register*/			\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0}		\
-		/*wait power state to suspend*/
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
+#define RTL8723A_TRANS_CARDEMU_TO_PDN	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(0), 0},/* 0x04[16] = 0*/\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(7), BIT(7)},/* 0x04[15] = 1*/
 
-#define RTL8723A_TRANS_CARDDIS_TO_CARDEMU				\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},		\
-		/*Set SDIO suspend local register*/			\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},	\
-		/*wait power state to suspend*/				\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},	\
-		/*0x04[12:11] = 2b'00enable WL suspend*/		\
-	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}		\
-		/*PCIe DMA start*/
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
+#define RTL8723A_TRANS_PDN_TO_CARDEMU	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(7), 0},/* 0x04[15] = 0*/
 
-#define RTL8723A_TRANS_CARDEMU_TO_PDN					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},		\
-		/* 0x04[16] = 0*/\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)}	\
-		/* 0x04[15] = 1*/
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
 
-#define RTL8723A_TRANS_PDN_TO_CARDEMU					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0}		\
-		/* 0x04[15] = 0*/
+#define RTL8723A_TRANS_ACT_TO_LPS	\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0x7F},/*Tx Pause*/	\
+	/*Should be zero if no packet is transmitting*/	\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_POLLING, 0xFF, 0},\
+	/*Should be zero if no packet is transmitting*/	\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_POLLING, 0xFF, 0},\
+	/*Should be zero if no packet is transmitting*/	\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_POLLING, 0xFF, 0},\
+	/*Should be zero if no packet is transmitting*/	\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_POLLING, 0xFF, 0},\
+	/*CCK and OFDM are disabled,and clock are gated*/ \
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(0), 0},\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(1), 0},/*Whole BB is reset*/	\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0x3F},/*Reset MAC TRX*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(1), 0},/*check if removed later*/	\
+	/*Respond TxOK to scheduler*/	\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(5), BIT(5)},\
 
-#define RTL8723A_TRANS_ACT_TO_LPS					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},		\
-		/*PCIe DMA stop*/					\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x7F},		\
-		/*Tx Pause*/						\
-	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},		\
-		/*Should be zero if no packet is transmitting*/		\
-	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},		\
-		/*Should be zero if no packet is transmitting*/		\
-	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},		\
-		/*Should be zero if no packet is transmitting*/		\
-	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},		\
-		/*Should be zero if no packet is transmitting*/		\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},		\
-		/*CCK and OFDM are disabled,and clock are gated*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},	\
-		/*Delay 1us*/						\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},		\
-		/*Whole BB is reset*/					\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3F},		\
-		/*Reset MAC TRX*/					\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},		\
-		/*check if removed later*/				\
-	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)}	\
-		/*Respond TxOK to scheduler*/
+#define RTL8723A_TRANS_LPS_TO_ACT\
+/* format */	\
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */	\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO,\
+		PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_USB_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/\
+	/*.	0x08[4] = 0		 switch TSF to 40M*/\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(4), 0},  \
+	/*Polling 0x109[7]=0  TSF in 40M*/\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_POLLING, BIT(7), 0}, \
+	/*.	0x29[7:6] = 2b'00	 enable BB clock*/\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(6)|BIT(7), 0},\
+	 /*.	0x101[1] = 1*/\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(1), BIT(1)},\
+	 /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0xFF},\
+	 /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, BIT(1)|BIT(0), BIT(1)|BIT(0)},\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, \
+		PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC,\
+		PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
 
-#define RTL8723A_TRANS_LPS_TO_ACT					\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-		 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84},		\
-		 /*SDIO RPWM*/						\
-	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84},		\
-		/*USB RPWM*/						\
-	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84},		\
-		/*PCIe RPWM*/						\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		 PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS},	\
-		/*Delay*/						\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},		\
-		/* 0x08[4] = 0 switch TSF to 40M*/			\
-	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0},		\
-		/*Polling 0x109[7]=0  TSF in 40M*/			\
-	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6)|BIT(7), 0},	\
-		/*.	0x29[7:6] = 2b'00	 enable BB clock*/	\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},	\
-		/*.	0x101[1] = 1*/					\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},		\
-		/* 0x100[7:0] = 0xFF enable WMAC TRX*/			\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1)|BIT(0),		\
-		BIT(1)|BIT(0)},						\
-		/* 0x02[1:0] = 2b'11  enable BB macro*/			\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-		PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}		\
-		/*.	0x522 = 0*/
+/* format */
+/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */
 
-#define RTL8723A_TRANS_END						\
-	/* format */							\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
-	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+#define RTL8723A_TRANS_END \
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
 	0, PWR_CMD_END, 0, 0}
 
-extern struct
-wlan_pwr_cfg rtl8723A_power_on_flow[RTL8723A_TRANS_CARDEMU_TO_ACT_STPS
-				    + RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_radio_off_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-				     + RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_card_disable_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-					+ RTL8723A_TRANS_CARDEMU_TO_PDN_STPS
-					+ RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_card_enable_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-				       + RTL8723A_TRANS_CARDEMU_TO_PDN_STPS
-				       + RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_suspend_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-				   + RTL8723A_TRANS_CARDEMU_TO_SUS_STPS
-				   + RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_resume_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-				  + RTL8723A_TRANS_CARDEMU_TO_SUS_STPS
-				  + RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_hwpdn_flow[RTL8723A_TRANS_ACT_TO_CARDEMU_STPS
-				 + RTL8723A_TRANS_CARDEMU_TO_PDN_STPS
-				 + RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_enter_lps_flow[RTL8723A_TRANS_ACT_TO_LPS_STPS
-				     + RTL8723A_TRANS_END_STPS];
-extern struct
-wlan_pwr_cfg rtl8723A_leave_lps_flow[RTL8723A_TRANS_LPS_TO_ACT_STPS
-				     + RTL8723A_TRANS_END_STPS];
+extern struct wlan_pwr_cfg rtl8723A_power_on_flow
+		[RTL8723A_TRANS_CARDEMU_TO_ACT_STEPS +
+		 RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_radio_off_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_card_disable_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_card_enable_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_suspend_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_resume_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_hwpdn_flow
+		[RTL8723A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8723A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_enter_lps_flow
+		[RTL8723A_TRANS_ACT_TO_LPS_STEPS + RTL8723A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8723A_leave_lps_flow
+		[RTL8723A_TRANS_LPS_TO_ACT_STEPS + RTL8723A_TRANS_END_STEPS];
 
 /* RTL8723 Power Configuration CMDs for PCIe interface */
 #define Rtl8723_NIC_PWR_ON_FLOW		rtl8723A_power_on_flow
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseqcmd.c b/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseqcmd.c
deleted file mode 100644
index 2044b5936b7f..000000000000
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/pwrseqcmd.c
+++ /dev/null
@@ -1,129 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2012  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "pwrseq.h"
-
-/*	Description:
- *		This routine deals with the Power Configuration CMD
- *		 parsing for RTL8723/RTL8188E Series IC.
- *	Assumption:
- *		We should follow specific format that was released from HW SD.
- */
-bool rtl_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-			      u8 faversion, u8 interface_type,
-			      struct wlan_pwr_cfg pwrcfgcmd[])
-{
-	struct wlan_pwr_cfg cfg_cmd = {0};
-	bool polling_bit = false;
-	u32 ary_idx = 0;
-	u8 value = 0;
-	u32 offset = 0;
-	u32 polling_count = 0;
-	u32 max_polling_cnt = 5000;
-
-	do {
-		cfg_cmd = pwrcfgcmd[ary_idx];
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			"rtl_hal_pwrseqcmdparsing(): offset(%#x),cut_msk(%#x), famsk(%#x),"
-			"interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
-			GET_PWR_CFG_OFFSET(cfg_cmd),
-					   GET_PWR_CFG_CUT_MASK(cfg_cmd),
-			GET_PWR_CFG_FAB_MASK(cfg_cmd),
-					     GET_PWR_CFG_INTF_MASK(cfg_cmd),
-			GET_PWR_CFG_BASE(cfg_cmd), GET_PWR_CFG_CMD(cfg_cmd),
-			GET_PWR_CFG_MASK(cfg_cmd), GET_PWR_CFG_VALUE(cfg_cmd));
-
-		if ((GET_PWR_CFG_FAB_MASK(cfg_cmd)&faversion) &&
-		    (GET_PWR_CFG_CUT_MASK(cfg_cmd)&cut_version) &&
-		    (GET_PWR_CFG_INTF_MASK(cfg_cmd)&interface_type)) {
-			switch (GET_PWR_CFG_CMD(cfg_cmd)) {
-			case PWR_CMD_READ:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_READ\n");
-				break;
-			case PWR_CMD_WRITE:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n");
-				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
-
-				/*Read the value from system register*/
-				value = rtl_read_byte(rtlpriv, offset);
-				value &= (~(GET_PWR_CFG_MASK(cfg_cmd)));
-				value |= (GET_PWR_CFG_VALUE(cfg_cmd) &
-					  GET_PWR_CFG_MASK(cfg_cmd));
-
-				/*Write the value back to sytem register*/
-				rtl_write_byte(rtlpriv, offset, value);
-				break;
-			case PWR_CMD_POLLING:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n");
-				polling_bit = false;
-				offset = GET_PWR_CFG_OFFSET(cfg_cmd);
-
-				do {
-					value = rtl_read_byte(rtlpriv, offset);
-
-					value &= GET_PWR_CFG_MASK(cfg_cmd);
-					if (value ==
-					    (GET_PWR_CFG_VALUE(cfg_cmd)
-					    & GET_PWR_CFG_MASK(cfg_cmd)))
-						polling_bit = true;
-					else
-						udelay(10);
-
-					if (polling_count++ > max_polling_cnt)
-						return false;
-				} while (!polling_bit);
-				break;
-			case PWR_CMD_DELAY:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					"rtl_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n");
-				if (GET_PWR_CFG_VALUE(cfg_cmd) ==
-				    PWRSEQ_DELAY_US)
-					udelay(GET_PWR_CFG_OFFSET(cfg_cmd));
-				else
-					mdelay(GET_PWR_CFG_OFFSET(cfg_cmd));
-				break;
-			case PWR_CMD_END:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtl_hal_pwrseqcmdparsing(): PWR_CMD_END\n");
-				return true;
-			default:
-				RT_ASSERT(false,
-					 "rtl_hal_pwrseqcmdparsing(): Unknown CMD!!\n");
-				break;
-			}
-
-		}
-		ary_idx++;
-	} while (1);
-
-	return true;
-}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h b/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h
index ce2c66fd9eee..306059f9b9cc 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/reg.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -34,13 +30,13 @@
 #define REG_SYS_FUNC_EN				0x0002
 #define REG_APS_FSMCO				0x0004
 #define REG_SYS_CLKR				0x0008
-#define REG_9346CR				0x000A
-#define REG_EE_VPD				0x000C
+#define REG_9346CR					0x000A
+#define REG_EE_VPD					0x000C
 #define REG_AFE_MISC				0x0010
 #define REG_SPS0_CTRL				0x0011
 #define REG_SPS_OCP_CFG				0x0018
 #define REG_RSV_CTRL				0x001C
-#define REG_RF_CTRL				0x001F
+#define REG_RF_CTRL					0x001F
 #define REG_LDOA15_CTRL				0x0020
 #define REG_LDOV12D_CTRL			0x0021
 #define REG_LDOHCI12_CTRL			0x0022
@@ -57,12 +53,12 @@
 #define REG_MAC_PINMUX_CFG			0x0043
 #define REG_GPIO_PIN_CTRL			0x0044
 #define REG_GPIO_INTM				0x0048
-#define REG_LEDCFG0				0x004C
-#define REG_LEDCFG1				0x004D
-#define REG_LEDCFG2				0x004E
-#define REG_LEDCFG3				0x004F
-#define REG_FSIMR				0x0050
-#define REG_FSISR				0x0054
+#define REG_LEDCFG0					0x004C
+#define REG_LEDCFG1					0x004D
+#define REG_LEDCFG2					0x004E
+#define REG_LEDCFG3					0x004F
+#define REG_FSIMR					0x0050
+#define REG_FSISR					0x0054
 #define REG_GPIO_PIN_CTRL_2			0x0060
 #define REG_GPIO_IO_SEL_2			0x0062
 #define REG_MULTI_FUNC_CTRL			0x0068
@@ -80,25 +76,25 @@
 #define REG_USB_SIE_INTF			0x00E0
 #define REG_PCIE_MIO_INTF			0x00E4
 #define REG_PCIE_MIO_INTD			0x00E8
-#define REG_SYS_CFG				0x00F0
+#define REG_SYS_CFG					0x00F0
 #define REG_GPIO_OUTSTS				0x00F4
 
-#define REG_CR					0x0100
-#define REG_PBP					0x0104
+#define REG_CR						0x0100
+#define REG_PBP						0x0104
 #define REG_TRXDMA_CTRL				0x010C
 #define REG_TRXFF_BNDY				0x0114
 #define REG_TRXFF_STATUS			0x0118
 #define REG_RXFF_PTR				0x011C
-#define REG_HIMR				0x0120
-#define REG_HISR				0x0124
-#define REG_HIMRE				0x0128
-#define REG_HISRE				0x012C
-#define REG_CPWM				0x012F
-#define REG_FWIMR				0x0130
-#define REG_FWISR				0x0134
+#define REG_HIMR					0x0120
+#define REG_HISR					0x0124
+#define REG_HIMRE					0x0128
+#define REG_HISRE					0x012C
+#define REG_CPWM					0x012F
+#define REG_FWIMR					0x0130
+#define REG_FWISR					0x0134
 #define REG_PKTBUF_DBG_CTRL			0x0140
-#define REG_PKTBUF_DBG_DATA_L			0x0144
-#define REG_PKTBUF_DBG_DATA_H			0x0148
+#define REG_PKTBUF_DBG_DATA_L		0x0144
+#define REG_PKTBUF_DBG_DATA_H		0x0148
 
 #define REG_TC0_CTRL				0x0150
 #define REG_TC1_CTRL				0x0154
@@ -109,11 +105,11 @@
 #define REG_MBIST_START				0x0174
 #define REG_MBIST_DONE				0x0178
 #define REG_MBIST_FAIL				0x017C
-#define REG_C2HEVT_MSG_NORMAL			0x01A0
+#define REG_C2HEVT_MSG_NORMAL		0x01A0
 #define REG_C2HEVT_MSG_TEST			0x01B8
 #define REG_MCUTST_1				0x01c0
-#define REG_FMETHR				0x01C8
-#define REG_HMETFR				0x01CC
+#define REG_FMETHR					0x01C8
+#define REG_HMETFR					0x01CC
 #define REG_HMEBOX_0				0x01D0
 #define REG_HMEBOX_1				0x01D4
 #define REG_HMEBOX_2				0x01D8
@@ -123,10 +119,10 @@
 #define REG_BB_ACCEESS_CTRL			0x01E8
 #define REG_BB_ACCESS_DATA			0x01EC
 
-#define REG_RQPN				0x0200
+#define REG_RQPN					0x0200
 #define REG_FIFOPAGE				0x0204
-#define REG_TDECTRL				0x0208
-#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TDECTRL					0x0208
+#define REG_TXDMA_OFFSET_CHK		0x020C
 #define REG_TXDMA_STATUS			0x0210
 #define REG_RQPN_NPQ				0x0214
 
@@ -135,18 +131,18 @@
 #define REG_RXDMA_STATUS			0x0288
 
 #define	REG_PCIE_CTRL_REG			0x0300
-#define	REG_INT_MIG				0x0304
+#define	REG_INT_MIG					0x0304
 #define	REG_BCNQ_DESA				0x0308
-#define	REG_HQ_DESA				0x0310
+#define	REG_HQ_DESA					0x0310
 #define	REG_MGQ_DESA				0x0318
 #define	REG_VOQ_DESA				0x0320
 #define	REG_VIQ_DESA				0x0328
 #define	REG_BEQ_DESA				0x0330
 #define	REG_BKQ_DESA				0x0338
-#define	REG_RX_DESA				0x0340
-#define	REG_DBI					0x0348
-#define	REG_MDIO				0x0354
-#define	REG_DBG_SEL				0x0360
+#define	REG_RX_DESA					0x0340
+#define	REG_DBI						0x0348
+#define	REG_MDIO					0x0354
+#define	REG_DBG_SEL					0x0360
 #define	REG_PCIE_HRPWM				0x0361
 #define	REG_PCIE_HCPWM				0x0363
 #define	REG_UART_CTRL				0x0364
@@ -162,31 +158,31 @@
 #define REG_BKQ_INFORMATION			0x040C
 #define REG_MGQ_INFORMATION			0x0410
 #define REG_HGQ_INFORMATION			0x0414
-#define REG_BCNQ_INFORMATION			0x0418
+#define REG_BCNQ_INFORMATION		0x0418
 
-#define REG_CPU_MGQ_INFORMATION			0x041C
+#define REG_CPU_MGQ_INFORMATION		0x041C
 #define REG_FWHW_TXQ_CTRL			0x0420
 #define REG_HWSEQ_CTRL				0x0423
-#define REG_TXPKTBUF_BCNQ_BDNY			0x0424
-#define REG_TXPKTBUF_MGQ_BDNY			0x0425
+#define REG_TXPKTBUF_BCNQ_BDNY		0x0424
+#define REG_TXPKTBUF_MGQ_BDNY		0x0425
 #define REG_MULTI_BCNQ_EN			0x0426
-#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_MULTI_BCNQ_OFFSET		0x0427
 #define REG_SPEC_SIFS				0x0428
-#define REG_RL					0x042A
-#define REG_DARFRC				0x0430
-#define REG_RARFRC				0x0438
-#define REG_RRSR				0x0440
-#define REG_ARFR0				0x0444
-#define REG_ARFR1				0x0448
-#define REG_ARFR2				0x044C
-#define REG_ARFR3				0x0450
+#define REG_RL						0x042A
+#define REG_DARFRC					0x0430
+#define REG_RARFRC					0x0438
+#define REG_RRSR					0x0440
+#define REG_ARFR0					0x0444
+#define REG_ARFR1					0x0448
+#define REG_ARFR2					0x044C
+#define REG_ARFR3					0x0450
 #define REG_AGGLEN_LMT				0x0458
 #define REG_AMPDU_MIN_SPACE			0x045C
-#define REG_TXPKTBUF_WMAC_LBK_BF_HD		0x045D
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD	0x045D
 #define REG_FAST_EDCA_CTRL			0x0460
 #define REG_RD_RESP_PKT_TH			0x0463
 #define REG_INIRTS_RATE_SEL			0x0480
-#define REG_INIDATA_RATE_SEL			0x0484
+#define REG_INIDATA_RATE_SEL		0x0484
 #define REG_POWER_STATUS			0x04A4
 #define REG_POWER_STAGE1			0x04B4
 #define REG_POWER_STAGE2			0x04B8
@@ -194,29 +190,29 @@
 #define REG_STBC_SETTING			0x04C4
 #define REG_PROT_MODE_CTRL			0x04C8
 #define REG_BAR_MODE_CTRL			0x04CC
-#define REG_RA_TRY_RATE_AGG_LMT			0x04CF
+#define REG_RA_TRY_RATE_AGG_LMT		0x04CF
 #define REG_NQOS_SEQ				0x04DC
-#define REG_QOS_SEQ				0x04DE
+#define REG_QOS_SEQ					0x04DE
 #define REG_NEED_CPU_HANDLE			0x04E0
 #define REG_PKT_LOSE_RPT			0x04E1
 #define REG_PTCL_ERR_STATUS			0x04E2
-#define REG_DUMMY				0x04FC
+#define REG_DUMMY					0x04FC
 
 #define REG_EDCA_VO_PARAM			0x0500
 #define REG_EDCA_VI_PARAM			0x0504
 #define REG_EDCA_BE_PARAM			0x0508
 #define REG_EDCA_BK_PARAM			0x050C
-#define REG_BCNTCFG				0x0510
-#define REG_PIFS				0x0512
+#define REG_BCNTCFG					0x0510
+#define REG_PIFS					0x0512
 #define REG_RDG_PIFS				0x0513
 #define REG_SIFS_CTX				0x0514
 #define REG_SIFS_TRX				0x0516
 #define REG_AGGR_BREAK_TIME			0x051A
-#define REG_SLOT				0x051B
+#define REG_SLOT					0x051B
 #define REG_TX_PTCL_CTRL			0x0520
-#define REG_TXPAUSE				0x0522
+#define REG_TXPAUSE					0x0522
 #define REG_DIS_TXREQ_CLR			0x0523
-#define REG_RD_CTRL				0x0524
+#define REG_RD_CTRL					0x0524
 #define REG_TBTT_PROHIBIT			0x0540
 #define REG_RD_NAV_NXT				0x0544
 #define REG_NAV_PROT_LEN			0x0546
@@ -225,21 +221,21 @@
 #define REG_MBID_NUM				0x0552
 #define REG_DUAL_TSF_RST			0x0553
 #define REG_BCN_INTERVAL			0x0554
-#define REG_MBSSID_BCN_SPACE			0x0554
+#define REG_MBSSID_BCN_SPACE		0x0554
 #define REG_DRVERLYINT				0x0558
 #define REG_BCNDMATIM				0x0559
-#define REG_ATIMWND				0x055A
+#define REG_ATIMWND					0x055A
 #define REG_BCN_MAX_ERR				0x055D
-#define REG_RXTSF_OFFSET_CCK			0x055E
-#define REG_RXTSF_OFFSET_OFDM			0x055F
-#define REG_TSFTR				0x0560
+#define REG_RXTSF_OFFSET_CCK		0x055E
+#define REG_RXTSF_OFFSET_OFDM		0x055F
+#define REG_TSFTR					0x0560
 #define REG_INIT_TSFTR				0x0564
-#define REG_PSTIMER				0x0580
-#define REG_TIMER0				0x0584
-#define REG_TIMER1				0x0588
+#define REG_PSTIMER					0x0580
+#define REG_TIMER0					0x0584
+#define REG_TIMER1					0x0588
 #define REG_ACMHWCTRL				0x05C0
 #define REG_ACMRSTCTRL				0x05C1
-#define REG_ACMAVG				0x05C2
+#define REG_ACMAVG					0x05C2
 #define REG_VO_ADMTIME				0x05C4
 #define REG_VI_ADMTIME				0x05C6
 #define REG_BE_ADMTIME				0x05C8
@@ -248,38 +244,38 @@
 
 #define REG_APSD_CTRL				0x0600
 #define REG_BWOPMODE				0x0603
-#define REG_TCR					0x0604
-#define REG_RCR					0x0608
+#define REG_TCR						0x0604
+#define REG_RCR						0x0608
 #define REG_RX_PKT_LIMIT			0x060C
 #define REG_RX_DLK_TIME				0x060D
 #define REG_RX_DRVINFO_SZ			0x060F
 
-#define REG_MACID				0x0610
-#define REG_BSSID				0x0618
-#define REG_MAR					0x0620
+#define REG_MACID					0x0610
+#define REG_BSSID					0x0618
+#define REG_MAR						0x0620
 #define REG_MBIDCAMCFG				0x0628
 
 #define REG_USTIME_EDCA				0x0638
 #define REG_MAC_SPEC_SIFS			0x063A
 #define REG_RESP_SIFS_CCK			0x063C
 #define REG_RESP_SIFS_OFDM			0x063E
-#define REG_ACKTO				0x0640
-#define REG_CTS2TO				0x0641
-#define REG_EIFS				0x0642
+#define REG_ACKTO					0x0640
+#define REG_CTS2TO					0x0641
+#define REG_EIFS					0x0642
 
 #define REG_NAV_CTRL				0x0650
 #define REG_BACAMCMD				0x0654
 #define REG_BACAMCONTENT			0x0658
-#define REG_LBDLY				0x0660
-#define REG_FWDLY				0x0661
+#define REG_LBDLY					0x0660
+#define REG_FWDLY					0x0661
 #define REG_RXERR_RPT				0x0664
-#define REG_WMAC_TRXPTCL_CTL			0x0668
+#define REG_WMAC_TRXPTCL_CTL		0x0668
 
-#define REG_CAMCMD				0x0670
+#define REG_CAMCMD					0x0670
 #define REG_CAMWRITE				0x0674
-#define REG_CAMREAD				0x0678
-#define REG_CAMDBG				0x067C
-#define REG_SECCFG				0x0680
+#define REG_CAMREAD					0x0678
+#define REG_CAMDBG					0x067C
+#define REG_SECCFG					0x0680
 
 #define REG_WOW_CTRL				0x0690
 #define REG_PSSTATUS				0x0691
@@ -294,10 +290,10 @@
 #define REG_CALB32K_CTRL			0x06AC
 #define REG_PKT_MON_CTRL			0x06B4
 #define REG_BT_COEX_TABLE			0x06C0
-#define REG_WMAC_RESP_TXINFO			0x06D8
+#define REG_WMAC_RESP_TXINFO		0x06D8
 
 #define REG_USB_INFO				0xFE17
-#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_SPECIAL_OPTION		0xFE55
 #define REG_USB_DMA_AGG_TO			0xFE5B
 #define REG_USB_AGG_TO				0xFE5C
 #define REG_USB_AGG_TH				0xFE5D
@@ -305,120 +301,148 @@
 #define REG_TEST_USB_TXQS			0xFE48
 #define REG_TEST_SIE_VID			0xFE60
 #define REG_TEST_SIE_PID			0xFE62
-#define REG_TEST_SIE_OPTIONAL			0xFE64
-#define REG_TEST_SIE_CHIRP_K			0xFE65
+#define REG_TEST_SIE_OPTIONAL		0xFE64
+#define REG_TEST_SIE_CHIRP_K		0xFE65
 #define REG_TEST_SIE_PHY			0xFE66
-#define REG_TEST_SIE_MAC_ADDR			0xFE70
+#define REG_TEST_SIE_MAC_ADDR		0xFE70
 #define REG_TEST_SIE_STRING			0xFE80
 
 #define REG_NORMAL_SIE_VID			0xFE60
 #define REG_NORMAL_SIE_PID			0xFE62
-#define REG_NORMAL_SIE_OPTIONAL			0xFE64
+#define REG_NORMAL_SIE_OPTIONAL		0xFE64
 #define REG_NORMAL_SIE_EP			0xFE65
 #define REG_NORMAL_SIE_PHY			0xFE68
-#define REG_NORMAL_SIE_MAC_ADDR			0xFE70
-#define REG_NORMAL_SIE_STRING			0xFE80
+#define REG_NORMAL_SIE_MAC_ADDR		0xFE70
+#define REG_NORMAL_SIE_STRING		0xFE80
 
-#define	CR9346					REG_9346CR
-#define	MSR					(REG_CR + 2)
-#define	ISR					REG_HISR
-#define	TSFR					REG_TSFTR
+#define	CR9346				REG_9346CR
+#define	MSR				(REG_CR + 2)
+#define	ISR				REG_HISR
+#define	TSFR				REG_TSFTR
 
-#define	MACIDR0					REG_MACID
-#define	MACIDR4					(REG_MACID + 4)
+#define	MACIDR0				REG_MACID
+#define	MACIDR4				(REG_MACID + 4)
 
-#define PBP					REG_PBP
+#define PBP				REG_PBP
 
-#define	IDR0					MACIDR0
-#define	IDR4					MACIDR4
+#define	IDR0				MACIDR0
+#define	IDR4				MACIDR4
 
-#define	UNUSED_REGISTER				0x1BF
-#define	DCAM					UNUSED_REGISTER
-#define	PSR					UNUSED_REGISTER
-#define BBADDR					UNUSED_REGISTER
-#define	PHYDATAR				UNUSED_REGISTER
+#define	UNUSED_REGISTER			0x1BF
+#define	DCAM				UNUSED_REGISTER
+#define	PSR				UNUSED_REGISTER
+#define BBADDR				UNUSED_REGISTER
+#define	PHYDATAR			UNUSED_REGISTER
 
-#define	INVALID_BBRF_VALUE			0x12345678
+#define	INVALID_BBRF_VALUE		0x12345678
 
-#define	MAX_MSS_DENSITY_2T			0x13
-#define	MAX_MSS_DENSITY_1T			0x0A
+#define	MAX_MSS_DENSITY_2T		0x13
+#define	MAX_MSS_DENSITY_1T		0x0A
 
-#define	CMDEEPROM_EN				BIT(5)
-#define	CMDEEPROM_SEL				BIT(4)
-#define	CMD9346CR_9356SEL			BIT(4)
-#define	AUTOLOAD_EEPROM				(CMDEEPROM_EN|CMDEEPROM_SEL)
-#define	AUTOLOAD_EFUSE				CMDEEPROM_EN
+#define	CMDEEPROM_EN			BIT(5)
+#define	CMDEEPROM_SEL			BIT(4)
+#define	CMD9346CR_9356SEL		BIT(4)
+#define	AUTOLOAD_EEPROM			(CMDEEPROM_EN|CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE			CMDEEPROM_EN
 
-#define	GPIOSEL_GPIO				0
-#define	GPIOSEL_ENBT				BIT(5)
+#define	GPIOSEL_GPIO			0
+#define	GPIOSEL_ENBT			BIT(5)
 
-#define	GPIO_IN					REG_GPIO_PIN_CTRL
-#define	GPIO_OUT				(REG_GPIO_PIN_CTRL+1)
-#define	GPIO_IO_SEL				(REG_GPIO_PIN_CTRL+2)
-#define	GPIO_MOD				(REG_GPIO_PIN_CTRL+3)
+#define	GPIO_IN				REG_GPIO_PIN_CTRL
+#define	GPIO_OUT			(REG_GPIO_PIN_CTRL+1)
+#define	GPIO_IO_SEL			(REG_GPIO_PIN_CTRL+2)
+#define	GPIO_MOD			(REG_GPIO_PIN_CTRL+3)
 
-#define	MSR_NOLINK				0x00
-#define	MSR_ADHOC				0x01
-#define	MSR_INFRA				0x02
-#define	MSR_AP					0x03
-#define	MSR_MASK				0x03
+#define	MSR_NOLINK					0x00
+#define	MSR_ADHOC					0x01
+#define	MSR_INFRA					0x02
+#define	MSR_AP						0x03
 
 #define	RRSR_RSC_OFFSET				21
 #define	RRSR_SHORT_OFFSET			23
 #define	RRSR_RSC_BW_40M				0x600000
 #define	RRSR_RSC_UPSUBCHNL			0x400000
 #define	RRSR_RSC_LOWSUBCHNL			0x200000
-#define	RRSR_SHORT				0x800000
-#define	RRSR_1M					BIT(0)
-#define	RRSR_2M					BIT(1)
-#define	RRSR_5_5M				BIT(2)
-#define	RRSR_11M				BIT(3)
-#define	RRSR_6M					BIT(4)
-#define	RRSR_9M					BIT(5)
-#define	RRSR_12M				BIT(6)
-#define	RRSR_18M				BIT(7)
-#define	RRSR_24M				BIT(8)
-#define	RRSR_36M				BIT(9)
-#define	RRSR_48M				BIT(10)
-#define	RRSR_54M				BIT(11)
-#define	RRSR_MCS0				BIT(12)
-#define	RRSR_MCS1				BIT(13)
-#define	RRSR_MCS2				BIT(14)
-#define	RRSR_MCS3				BIT(15)
-#define	RRSR_MCS4				BIT(16)
-#define	RRSR_MCS5				BIT(17)
-#define	RRSR_MCS6				BIT(18)
-#define	RRSR_MCS7				BIT(19)
+#define	RRSR_SHORT					0x800000
+#define	RRSR_1M						BIT(0)
+#define	RRSR_2M						BIT(1)
+#define	RRSR_5_5M					BIT(2)
+#define	RRSR_11M					BIT(3)
+#define	RRSR_6M						BIT(4)
+#define	RRSR_9M						BIT(5)
+#define	RRSR_12M					BIT(6)
+#define	RRSR_18M					BIT(7)
+#define	RRSR_24M					BIT(8)
+#define	RRSR_36M					BIT(9)
+#define	RRSR_48M					BIT(10)
+#define	RRSR_54M					BIT(11)
+#define	RRSR_MCS0					BIT(12)
+#define	RRSR_MCS1					BIT(13)
+#define	RRSR_MCS2					BIT(14)
+#define	RRSR_MCS3					BIT(15)
+#define	RRSR_MCS4					BIT(16)
+#define	RRSR_MCS5					BIT(17)
+#define	RRSR_MCS6					BIT(18)
+#define	RRSR_MCS7					BIT(19)
 #define	BRSR_ACKSHORTPMB			BIT(23)
 
-#define	RATR_1M					0x00000001
-#define	RATR_2M					0x00000002
-#define	RATR_55M				0x00000004
-#define	RATR_11M				0x00000008
-#define	RATR_6M					0x00000010
-#define	RATR_9M					0x00000020
-#define	RATR_12M				0x00000040
-#define	RATR_18M				0x00000080
-#define	RATR_24M				0x00000100
-#define	RATR_36M				0x00000200
-#define	RATR_48M				0x00000400
-#define	RATR_54M				0x00000800
-#define	RATR_MCS0				0x00001000
-#define	RATR_MCS1				0x00002000
-#define	RATR_MCS2				0x00004000
-#define	RATR_MCS3				0x00008000
-#define	RATR_MCS4				0x00010000
-#define	RATR_MCS5				0x00020000
-#define	RATR_MCS6				0x00040000
-#define	RATR_MCS7				0x00080000
-#define	RATR_MCS8				0x00100000
-#define	RATR_MCS9				0x00200000
-#define	RATR_MCS10				0x00400000
-#define	RATR_MCS11				0x00800000
-#define	RATR_MCS12				0x01000000
-#define	RATR_MCS13				0x02000000
-#define	RATR_MCS14				0x04000000
-#define	RATR_MCS15				0x08000000
+#define	RATR_1M						0x00000001
+#define	RATR_2M						0x00000002
+#define	RATR_55M					0x00000004
+#define	RATR_11M					0x00000008
+#define	RATR_6M						0x00000010
+#define	RATR_9M						0x00000020
+#define	RATR_12M					0x00000040
+#define	RATR_18M					0x00000080
+#define	RATR_24M					0x00000100
+#define	RATR_36M					0x00000200
+#define	RATR_48M					0x00000400
+#define	RATR_54M					0x00000800
+#define	RATR_MCS0					0x00001000
+#define	RATR_MCS1					0x00002000
+#define	RATR_MCS2					0x00004000
+#define	RATR_MCS3					0x00008000
+#define	RATR_MCS4					0x00010000
+#define	RATR_MCS5					0x00020000
+#define	RATR_MCS6					0x00040000
+#define	RATR_MCS7					0x00080000
+#define	RATR_MCS8					0x00100000
+#define	RATR_MCS9					0x00200000
+#define	RATR_MCS10					0x00400000
+#define	RATR_MCS11					0x00800000
+#define	RATR_MCS12					0x01000000
+#define	RATR_MCS13					0x02000000
+#define	RATR_MCS14					0x04000000
+#define	RATR_MCS15					0x08000000
+
+#define RATE_1M						BIT(0)
+#define RATE_2M						BIT(1)
+#define RATE_5_5M					BIT(2)
+#define RATE_11M					BIT(3)
+#define RATE_6M						BIT(4)
+#define RATE_9M						BIT(5)
+#define RATE_12M					BIT(6)
+#define RATE_18M					BIT(7)
+#define RATE_24M					BIT(8)
+#define RATE_36M					BIT(9)
+#define RATE_48M					BIT(10)
+#define RATE_54M					BIT(11)
+#define RATE_MCS0					BIT(12)
+#define RATE_MCS1					BIT(13)
+#define RATE_MCS2					BIT(14)
+#define RATE_MCS3					BIT(15)
+#define RATE_MCS4					BIT(16)
+#define RATE_MCS5					BIT(17)
+#define RATE_MCS6					BIT(18)
+#define RATE_MCS7					BIT(19)
+#define RATE_MCS8					BIT(20)
+#define RATE_MCS9					BIT(21)
+#define RATE_MCS10					BIT(22)
+#define RATE_MCS11					BIT(23)
+#define RATE_MCS12					BIT(24)
+#define RATE_MCS13					BIT(25)
+#define RATE_MCS14					BIT(26)
+#define RATE_MCS15					BIT(27)
 
 #define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
 #define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
@@ -434,31 +458,31 @@
 #define	BW_OPMODE_5G				BIT(1)
 #define	BW_OPMODE_11J				BIT(0)
 
-#define	CAM_VALID				BIT(15)
+#define	CAM_VALID					BIT(15)
 #define	CAM_NOTVALID				0x0000
-#define	CAM_USEDK				BIT(5)
+#define	CAM_USEDK					BIT(5)
 
-#define	CAM_NONE				0x0
-#define	CAM_WEP40				0x01
-#define	CAM_TKIP				0x02
-#define	CAM_AES					0x04
-#define	CAM_WEP104				0x05
+#define	CAM_NONE					0x0
+#define	CAM_WEP40					0x01
+#define	CAM_TKIP					0x02
+#define	CAM_AES						0x04
+#define	CAM_WEP104					0x05
 
 #define	TOTAL_CAM_ENTRY				32
 #define	HALF_CAM_ENTRY				16
 
-#define	CAM_WRITE				BIT(16)
-#define	CAM_READ				0x00000000
+#define	CAM_WRITE					BIT(16)
+#define	CAM_READ					0x00000000
 #define	CAM_POLLINIG				BIT(31)
 
-#define	SCR_USEDK				0x01
+#define	SCR_USEDK					0x01
 #define	SCR_TXSEC_ENABLE			0x02
 #define	SCR_RXSEC_ENABLE			0x04
 
-#define	WOW_PMEN				BIT(0)
-#define	WOW_WOMEN				BIT(1)
-#define	WOW_MAGIC				BIT(2)
-#define	WOW_UWF					BIT(3)
+#define	WOW_PMEN					BIT(0)
+#define	WOW_WOMEN					BIT(1)
+#define	WOW_MAGIC					BIT(2)
+#define	WOW_UWF						BIT(3)
 
 #define	IMR8190_DISABLED			0x0
 #define	IMR_BCNDMAINT6				BIT(31)
@@ -467,180 +491,179 @@
 #define	IMR_BCNDMAINT3				BIT(28)
 #define	IMR_BCNDMAINT2				BIT(27)
 #define	IMR_BCNDMAINT1				BIT(26)
-#define	IMR_BCNDOK8				BIT(25)
-#define	IMR_BCNDOK7				BIT(24)
-#define	IMR_BCNDOK6				BIT(23)
-#define	IMR_BCNDOK5				BIT(22)
-#define	IMR_BCNDOK4				BIT(21)
-#define	IMR_BCNDOK3				BIT(20)
-#define	IMR_BCNDOK2				BIT(19)
-#define	IMR_BCNDOK1				BIT(18)
+#define	IMR_BCNDOK8					BIT(25)
+#define	IMR_BCNDOK7					BIT(24)
+#define	IMR_BCNDOK6					BIT(23)
+#define	IMR_BCNDOK5					BIT(22)
+#define	IMR_BCNDOK4					BIT(21)
+#define	IMR_BCNDOK3					BIT(20)
+#define	IMR_BCNDOK2					BIT(19)
+#define	IMR_BCNDOK1					BIT(18)
 #define	IMR_TIMEOUT2				BIT(17)
 #define	IMR_TIMEOUT1				BIT(16)
-#define	IMR_TXFOVW				BIT(15)
+#define	IMR_TXFOVW					BIT(15)
 #define	IMR_PSTIMEOUT				BIT(14)
-#define	IMR_BCNINT				BIT(13)
-#define	IMR_RXFOVW				BIT(12)
-#define	IMR_RDU					BIT(11)
-#define	IMR_ATIMEND				BIT(10)
-#define	IMR_BDOK				BIT(9)
-#define	IMR_HIGHDOK				BIT(8)
-#define	IMR_TBDOK				BIT(7)
-#define	IMR_MGNTDOK				BIT(6)
-#define	IMR_TBDER				BIT(5)
-#define	IMR_BKDOK				BIT(4)
-#define	IMR_BEDOK				BIT(3)
-#define	IMR_VIDOK				BIT(2)
-#define	IMR_VODOK				BIT(1)
-#define	IMR_ROK					BIT(0)
-
-#define	IMR_TXERR				BIT(11)
-#define	IMR_RXERR				BIT(10)
-#define	IMR_CPWM				BIT(8)
-#define	IMR_OCPINT				BIT(1)
-#define	IMR_WLANOFF				BIT(0)
+#define	IMR_BCNINT					BIT(13)
+#define	IMR_RXFOVW					BIT(12)
+#define	IMR_RDU						BIT(11)
+#define	IMR_ATIMEND					BIT(10)
+#define	IMR_BDOK					BIT(9)
+#define	IMR_HIGHDOK					BIT(8)
+#define	IMR_TBDOK					BIT(7)
+#define	IMR_MGNTDOK					BIT(6)
+#define	IMR_TBDER					BIT(5)
+#define	IMR_BKDOK					BIT(4)
+#define	IMR_BEDOK					BIT(3)
+#define	IMR_VIDOK					BIT(2)
+#define	IMR_VODOK					BIT(1)
+#define	IMR_ROK						BIT(0)
+
+#define	IMR_TXERR					BIT(11)
+#define	IMR_RXERR					BIT(10)
+#define	IMR_CPWM					BIT(8)
+#define	IMR_OCPINT					BIT(1)
+#define	IMR_WLANOFF					BIT(0)
 
 /* 8723E series PCIE Host IMR/ISR bit */
 /* IMR DW0 Bit 0-31 */
-#define	PHIMR_TIMEOUT2				BIT(31)
-#define	PHIMR_TIMEOUT1				BIT(30)
+#define	PHIMR_TIMEOUT2					BIT(31)
+#define	PHIMR_TIMEOUT1					BIT(30)
 #define	PHIMR_PSTIMEOUT				BIT(29)
-#define	PHIMR_GTINT4				BIT(28)
-#define	PHIMR_GTINT3				BIT(27)
-#define	PHIMR_TXBCNERR				BIT(26)
-#define	PHIMR_TXBCNOK				BIT(25)
+#define	PHIMR_GTINT4					BIT(28)
+#define	PHIMR_GTINT3					BIT(27)
+#define	PHIMR_TXBCNERR					BIT(26)
+#define	PHIMR_TXBCNOK					BIT(25)
 #define	PHIMR_TSF_BIT32_TOGGLE			BIT(24)
-#define	PHIMR_BCNDMAINT3			BIT(23)
-#define	PHIMR_BCNDMAINT2			BIT(22)
-#define	PHIMR_BCNDMAINT1			BIT(21)
-#define	PHIMR_BCNDMAINT0			BIT(20)
-#define	PHIMR_BCNDOK3				BIT(19)
-#define	PHIMR_BCNDOK2				BIT(18)
-#define	PHIMR_BCNDOK1				BIT(17)
-#define	PHIMR_BCNDOK0				BIT(16)
+#define	PHIMR_BCNDMAINT3				BIT(23)
+#define	PHIMR_BCNDMAINT2				BIT(22)
+#define	PHIMR_BCNDMAINT1				BIT(21)
+#define	PHIMR_BCNDMAINT0				BIT(20)
+#define	PHIMR_BCNDOK3					BIT(19)
+#define	PHIMR_BCNDOK2					BIT(18)
+#define	PHIMR_BCNDOK1					BIT(17)
+#define	PHIMR_BCNDOK0					BIT(16)
 #define	PHIMR_HSISR_IND_ON			BIT(15)
-#define	PHIMR_BCNDMAINT_E			BIT(14)
+#define	PHIMR_BCNDMAINT_E				BIT(14)
 #define	PHIMR_ATIMEND_E				BIT(13)
 #define	PHIMR_ATIM_CTW_END			BIT(12)
 #define	PHIMR_HISRE_IND				BIT(11)
-#define	PHIMR_C2HCMD				BIT(10)
-#define	PHIMR_CPWM2				BIT(9)
-#define	PHIMR_CPWM				BIT(8)
-#define	PHIMR_HIGHDOK				BIT(7)
-#define	PHIMR_MGNTDOK				BIT(6)
-#define	PHIMR_BKDOK				BIT(5)
-#define	PHIMR_BEDOK				BIT(4)
-#define	PHIMR_VIDOK				BIT(3)
-#define	PHIMR_VODOK				BIT(2)
-#define	PHIMR_RDU				BIT(1)
-#define	PHIMR_ROK				BIT(0)
+#define	PHIMR_C2HCMD					BIT(10)
+#define	PHIMR_CPWM2					BIT(9)
+#define	PHIMR_CPWM					BIT(8)
+#define	PHIMR_HIGHDOK					BIT(7)
+#define	PHIMR_MGNTDOK					BIT(6)
+#define	PHIMR_BKDOK					BIT(5)
+#define	PHIMR_BEDOK					BIT(4)
+#define	PHIMR_VIDOK					BIT(3)
+#define	PHIMR_VODOK					BIT(2)
+#define	PHIMR_RDU						BIT(1)
+#define	PHIMR_ROK						BIT(0)
 
 /* PCIE Host Interrupt Status Extension bit */
-#define	PHIMR_BCNDMAINT7			BIT(23)
-#define	PHIMR_BCNDMAINT6			BIT(22)
-#define	PHIMR_BCNDMAINT5			BIT(21)
-#define	PHIMR_BCNDMAINT4			BIT(20)
-#define	PHIMR_BCNDOK7				BIT(19)
-#define	PHIMR_BCNDOK6				BIT(18)
-#define	PHIMR_BCNDOK5				BIT(17)
-#define	PHIMR_BCNDOK4				BIT(16)
+#define	PHIMR_BCNDMAINT7				BIT(23)
+#define	PHIMR_BCNDMAINT6				BIT(22)
+#define	PHIMR_BCNDMAINT5				BIT(21)
+#define	PHIMR_BCNDMAINT4				BIT(20)
+#define	PHIMR_BCNDOK7					BIT(19)
+#define	PHIMR_BCNDOK6					BIT(18)
+#define	PHIMR_BCNDOK5					BIT(17)
+#define	PHIMR_BCNDOK4					BIT(16)
 /* bit12-15: RSVD */
-#define	PHIMR_TXERR				BIT(11)
-#define	PHIMR_RXERR				BIT(10)
-#define	PHIMR_TXFOVW				BIT(9)
-#define	PHIMR_RXFOVW				BIT(8)
-/* bit2-7: RSV */
-#define	PHIMR_OCPINT				BIT(1)
+#define	PHIMR_TXERR					BIT(11)
+#define	PHIMR_RXERR					BIT(10)
+#define	PHIMR_TXFOVW					BIT(9)
+#define	PHIMR_RXFOVW					BIT(8)
+/* bit2-7: RSVD */
+#define	PHIMR_OCPINT					BIT(1)
 
 #define	HWSET_MAX_SIZE				256
 #define EFUSE_MAX_SECTION			32
 #define EFUSE_REAL_CONTENT_LEN			512
 #define EFUSE_OOB_PROTECT_BYTES			15
 
-#define	EEPROM_DEFAULT_TSSI			0x0
-#define EEPROM_DEFAULT_TXPOWERDIFF		0x0
-#define EEPROM_DEFAULT_CRYSTALCAP		0x5
-#define EEPROM_DEFAULT_BOARDTYPE		0x02
-#define EEPROM_DEFAULT_TXPOWER			0x1010
-#define	EEPROM_DEFAULT_HT2T_TXPWR		0x10
+#define	EEPROM_DEFAULT_TSSI					0x0
+#define EEPROM_DEFAULT_TXPOWERDIFF			0x0
+#define EEPROM_DEFAULT_CRYSTALCAP			0x5
+#define EEPROM_DEFAULT_BOARDTYPE			0x02
+#define EEPROM_DEFAULT_TXPOWER				0x1010
+#define	EEPROM_DEFAULT_HT2T_TXPWR			0x10
 
 #define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
-#define	EEPROM_DEFAULT_THERMALMETER		0x12
+#define	EEPROM_DEFAULT_THERMALMETER			0x12
 #define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
 #define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
-#define	EEPROM_DEFAULT_TXPOWERLEVEL		0x22
-#define	EEPROM_DEFAULT_HT40_2SDIFF		0x0
-#define EEPROM_DEFAULT_HT20_DIFF		2
+#define	EEPROM_DEFAULT_TXPOWERLEVEL			0x22
+#define	EEPROM_DEFAULT_HT40_2SDIFF			0x0
+#define EEPROM_DEFAULT_HT20_DIFF			2
 #define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
 #define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
 #define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
 
-
-#define EEPROM_DEFAULT_PID			0x1234
-#define EEPROM_DEFAULT_VID			0x5678
-#define EEPROM_DEFAULT_CUSTOMERID		0xAB
+#define EEPROM_DEFAULT_PID					0x1234
+#define EEPROM_DEFAULT_VID					0x5678
+#define EEPROM_DEFAULT_CUSTOMERID			0xAB
 #define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
-#define EEPROM_DEFAULT_VERSION			0
-
-#define	EEPROM_CHANNEL_PLAN_FCC			0x0
-#define	EEPROM_CHANNEL_PLAN_IC			0x1
-#define	EEPROM_CHANNEL_PLAN_ETSI		0x2
-#define	EEPROM_CHANNEL_PLAN_SPAIN		0x3
-#define	EEPROM_CHANNEL_PLAN_FRANCE		0x4
-#define	EEPROM_CHANNEL_PLAN_MKK			0x5
-#define	EEPROM_CHANNEL_PLAN_MKK1		0x6
-#define	EEPROM_CHANNEL_PLAN_ISRAEL		0x7
-#define	EEPROM_CHANNEL_PLAN_TELEC		0x8
+#define EEPROM_DEFAULT_VERSION				0
+
+#define	EEPROM_CHANNEL_PLAN_FCC				0x0
+#define	EEPROM_CHANNEL_PLAN_IC				0x1
+#define	EEPROM_CHANNEL_PLAN_ETSI			0x2
+#define	EEPROM_CHANNEL_PLAN_SPAIN			0x3
+#define	EEPROM_CHANNEL_PLAN_FRANCE			0x4
+#define	EEPROM_CHANNEL_PLAN_MKK				0x5
+#define	EEPROM_CHANNEL_PLAN_MKK1			0x6
+#define	EEPROM_CHANNEL_PLAN_ISRAEL			0x7
+#define	EEPROM_CHANNEL_PLAN_TELEC			0x8
 #define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
 #define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
-#define	EEPROM_CHANNEL_PLAN_NCC			0xB
+#define	EEPROM_CHANNEL_PLAN_NCC				0xB
 #define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
 
-#define EEPROM_CID_DEFAULT			0x0
-#define EEPROM_CID_TOSHIBA			0x4
-#define	EEPROM_CID_CCX				0x10
-#define	EEPROM_CID_QMI				0x0D
-#define EEPROM_CID_WHQL				0xFE
+#define EEPROM_CID_DEFAULT					0x0
+#define EEPROM_CID_TOSHIBA					0x4
+#define	EEPROM_CID_CCX						0x10
+#define	EEPROM_CID_QMI						0x0D
+#define EEPROM_CID_WHQL						0xFE
 
-#define	RTL8192_EEPROM_ID			0x8129
+#define	RTL8192_EEPROM_ID					0x8129
 
-#define RTL8190_EEPROM_ID			0x8129
-#define EEPROM_HPON				0x02
-#define EEPROM_CLK				0x06
-#define EEPROM_TESTR				0x08
+#define RTL8190_EEPROM_ID					0x8129
+#define EEPROM_HPON							0x02
+#define EEPROM_CLK							0x06
+#define EEPROM_TESTR						0x08
 
-#define EEPROM_VID				0x49
-#define EEPROM_DID				0x4B
-#define EEPROM_SVID				0x4D
-#define EEPROM_SMID				0x4F
+#define EEPROM_VID							0x49
+#define EEPROM_DID							0x4B
+#define EEPROM_SVID							0x4D
+#define EEPROM_SMID							0x4F
 
-#define EEPROM_MAC_ADDR				0x67
+#define EEPROM_MAC_ADDR						0x67
 
-#define EEPROM_CCK_TX_PWR_INX			0x5A
-#define EEPROM_HT40_1S_TX_PWR_INX		0x60
+#define EEPROM_CCK_TX_PWR_INX				0x5A
+#define EEPROM_HT40_1S_TX_PWR_INX			0x60
 #define EEPROM_HT40_2S_TX_PWR_INX_DIFF		0x66
-#define EEPROM_HT20_TX_PWR_INX_DIFF		0x69
-#define EEPROM_OFDM_TX_PWR_INX_DIFF		0x6C
-#define EEPROM_HT40_MAX_PWR_OFFSET		0x25
-#define EEPROM_HT20_MAX_PWR_OFFSET		0x22
-
-#define EEPROM_THERMAL_METER			0x2a
-#define EEPROM_XTAL_K				0x78
-#define EEPROM_RF_OPT1				0x79
-#define EEPROM_RF_OPT2				0x7A
-#define EEPROM_RF_OPT3				0x7B
-#define EEPROM_RF_OPT4				0x7C
-#define EEPROM_CHANNEL_PLAN			0x28
-#define EEPROM_VERSION				0x30
-#define EEPROM_CUSTOMER_ID			0x31
+#define EEPROM_HT20_TX_PWR_INX_DIFF			0x69
+#define EEPROM_OFDM_TX_PWR_INX_DIFF			0x6C
+#define EEPROM_HT40_MAX_PWR_OFFSET			0x25
+#define EEPROM_HT20_MAX_PWR_OFFSET			0x22
+
+#define EEPROM_THERMAL_METER				0x2a
+#define EEPROM_XTAL_K						0x78
+#define EEPROM_RF_OPT1						0x79
+#define EEPROM_RF_OPT2						0x7A
+#define EEPROM_RF_OPT3						0x7B
+#define EEPROM_RF_OPT4						0x7C
+#define EEPROM_CHANNEL_PLAN					0x28
+#define EEPROM_VERSION						0x30
+#define EEPROM_CUSTOMER_ID					0x31
 
 #define EEPROM_PWRDIFF				0x54
 
 #define EEPROM_TXPOWERCCK			0x10
-#define	EEPROM_TXPOWERHT40_1S			0x16
-#define	EEPROM_TXPOWERHT40_2SDIFF		0x66
-#define EEPROM_TXPOWERHT20DIFF			0x1C
-#define EEPROM_TXPOWER_OFDMDIFF			0x1F
+#define	EEPROM_TXPOWERHT40_1S		0x16
+#define	EEPROM_TXPOWERHT40_2SDIFF	0x66
+#define EEPROM_TXPOWERHT20DIFF		0x1C
+#define EEPROM_TXPOWER_OFDMDIFF		0x1F
 
 #define	EEPROM_TXPWR_GROUP			0x22
 
@@ -649,169 +672,169 @@
 
 #define EEPROM_CHANNELPLAN			0x28
 
-#define RF_OPTION1				0x2B
-#define RF_OPTION2				0x2C
-#define RF_OPTION3				0x2D
-#define RF_OPTION4				0x2E
-
-#define	STOPBECON				BIT(6)
-#define	STOPHIGHT				BIT(5)
-#define	STOPMGT					BIT(4)
-#define	STOPVO					BIT(3)
-#define	STOPVI					BIT(2)
-#define	STOPBE					BIT(1)
-#define	STOPBK					BIT(0)
-
-#define	RCR_APPFCS				BIT(31)
-#define	RCR_APP_MIC				BIT(30)
-#define	RCR_APP_ICV				BIT(29)
+#define RF_OPTION1					0x2B
+#define RF_OPTION2					0x2C
+#define RF_OPTION3					0x2D
+#define RF_OPTION4					0x2E
+
+#define	STOPBECON					BIT(6)
+#define	STOPHIGHT					BIT(5)
+#define	STOPMGT						BIT(4)
+#define	STOPVO						BIT(3)
+#define	STOPVI						BIT(2)
+#define	STOPBE						BIT(1)
+#define	STOPBK						BIT(0)
+
+#define	RCR_APPFCS					BIT(31)
+#define	RCR_APP_MIC					BIT(30)
+#define	RCR_APP_ICV					BIT(29)
 #define	RCR_APP_PHYST_RXFF			BIT(28)
 #define	RCR_APP_BA_SSN				BIT(27)
-#define	RCR_ENMBID				BIT(24)
-#define	RCR_LSIGEN				BIT(23)
-#define	RCR_MFBEN				BIT(22)
+#define	RCR_ENMBID					BIT(24)
+#define	RCR_LSIGEN					BIT(23)
+#define	RCR_MFBEN					BIT(22)
 #define	RCR_HTC_LOC_CTRL			BIT(14)
-#define	RCR_AMF					BIT(13)
-#define	RCR_ACF					BIT(12)
-#define	RCR_ADF					BIT(11)
-#define	RCR_AICV				BIT(9)
-#define	RCR_ACRC32				BIT(8)
+#define	RCR_AMF						BIT(13)
+#define	RCR_ACF						BIT(12)
+#define	RCR_ADF						BIT(11)
+#define	RCR_AICV					BIT(9)
+#define	RCR_ACRC32					BIT(8)
 #define	RCR_CBSSID_BCN				BIT(7)
 #define	RCR_CBSSID_DATA				BIT(6)
-#define	RCR_CBSSID				RCR_CBSSID_DATA
-#define	RCR_APWRMGT				BIT(5)
-#define	RCR_ADD3				BIT(4)
-#define	RCR_AB					BIT(3)
-#define	RCR_AM					BIT(2)
-#define	RCR_APM					BIT(1)
-#define	RCR_AAP					BIT(0)
+#define	RCR_CBSSID					RCR_CBSSID_DATA
+#define	RCR_APWRMGT					BIT(5)
+#define	RCR_ADD3					BIT(4)
+#define	RCR_AB						BIT(3)
+#define	RCR_AM						BIT(2)
+#define	RCR_APM						BIT(1)
+#define	RCR_AAP						BIT(0)
 #define	RCR_MXDMA_OFFSET			8
 #define	RCR_FIFO_OFFSET				13
 
-#define RSV_CTRL				0x001C
-#define RD_CTRL					0x0524
+#define RSV_CTRL					0x001C
+#define RD_CTRL						0x0524
 
 #define REG_USB_INFO				0xFE17
-#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_SPECIAL_OPTION		0xFE55
 #define REG_USB_DMA_AGG_TO			0xFE5B
 #define REG_USB_AGG_TO				0xFE5C
 #define REG_USB_AGG_TH				0xFE5D
 
-#define REG_USB_VID				0xFE60
-#define REG_USB_PID				0xFE62
+#define REG_USB_VID					0xFE60
+#define REG_USB_PID					0xFE62
 #define REG_USB_OPTIONAL			0xFE64
 #define REG_USB_CHIRP_K				0xFE65
-#define REG_USB_PHY				0xFE66
+#define REG_USB_PHY					0xFE66
 #define REG_USB_MAC_ADDR			0xFE70
 #define REG_USB_HRPWM				0xFE58
 #define REG_USB_HCPWM				0xFE57
 
-#define SW18_FPWM				BIT(3)
-
-#define ISO_MD2PP				BIT(0)
-#define ISO_UA2USB				BIT(1)
-#define ISO_UD2CORE				BIT(2)
-#define ISO_PA2PCIE				BIT(3)
-#define ISO_PD2CORE				BIT(4)
-#define ISO_IP2MAC				BIT(5)
-#define ISO_DIOP				BIT(6)
-#define ISO_DIOE				BIT(7)
-#define ISO_EB2CORE				BIT(8)
-#define ISO_DIOR				BIT(9)
-
-#define PWC_EV25V				BIT(14)
-#define PWC_EV12V				BIT(15)
-
-#define FEN_BBRSTB				BIT(0)
-#define FEN_BB_GLB_RSTn				BIT(1)
-#define FEN_USBA				BIT(2)
-#define FEN_UPLL				BIT(3)
-#define FEN_USBD				BIT(4)
+#define SW18_FPWM					BIT(3)
+
+#define ISO_MD2PP					BIT(0)
+#define ISO_UA2USB					BIT(1)
+#define ISO_UD2CORE					BIT(2)
+#define ISO_PA2PCIE					BIT(3)
+#define ISO_PD2CORE					BIT(4)
+#define ISO_IP2MAC					BIT(5)
+#define ISO_DIOP					BIT(6)
+#define ISO_DIOE					BIT(7)
+#define ISO_EB2CORE					BIT(8)
+#define ISO_DIOR					BIT(9)
+
+#define PWC_EV25V					BIT(14)
+#define PWC_EV12V					BIT(15)
+
+#define FEN_BBRSTB					BIT(0)
+#define FEN_BB_GLB_RSTN				BIT(1)
+#define FEN_USBA					BIT(2)
+#define FEN_UPLL					BIT(3)
+#define FEN_USBD					BIT(4)
 #define FEN_DIO_PCIE				BIT(5)
-#define FEN_PCIEA				BIT(6)
-#define FEN_PPLL				BIT(7)
-#define FEN_PCIED				BIT(8)
-#define FEN_DIOE				BIT(9)
-#define FEN_CPUEN				BIT(10)
-#define FEN_DCORE				BIT(11)
-#define FEN_ELDR				BIT(12)
-#define FEN_DIO_RF				BIT(13)
-#define FEN_HWPDN				BIT(14)
-#define FEN_MREGEN				BIT(15)
-
-#define PFM_LDALL				BIT(0)
-#define PFM_ALDN				BIT(1)
-#define PFM_LDKP				BIT(2)
-#define PFM_WOWL				BIT(3)
-#define EnPDN					BIT(4)
-#define PDN_PL					BIT(5)
-#define APFM_ONMAC				BIT(8)
-#define APFM_OFF				BIT(9)
-#define APFM_RSM				BIT(10)
-#define AFSM_HSUS				BIT(11)
-#define AFSM_PCIE				BIT(12)
-#define APDM_MAC				BIT(13)
-#define APDM_HOST				BIT(14)
-#define APDM_HPDN				BIT(15)
-#define RDY_MACON				BIT(16)
-#define SUS_HOST				BIT(17)
-#define ROP_ALD					BIT(20)
-#define ROP_PWR					BIT(21)
-#define ROP_SPS					BIT(22)
-#define SOP_MRST				BIT(25)
-#define SOP_FUSE				BIT(26)
-#define SOP_ABG					BIT(27)
-#define SOP_AMB					BIT(28)
-#define SOP_RCK					BIT(29)
-#define SOP_A8M					BIT(30)
-#define XOP_BTCK				BIT(31)
-
-#define ANAD16V_EN				BIT(0)
-#define ANA8M					BIT(1)
-#define MACSLP					BIT(4)
+#define FEN_PCIEA					BIT(6)
+#define FEN_PPLL					BIT(7)
+#define FEN_PCIED					BIT(8)
+#define FEN_DIOE					BIT(9)
+#define FEN_CPUEN					BIT(10)
+#define FEN_DCORE					BIT(11)
+#define FEN_ELDR					BIT(12)
+#define FEN_DIO_RF					BIT(13)
+#define FEN_HWPDN					BIT(14)
+#define FEN_MREGEN					BIT(15)
+
+#define PFM_LDALL					BIT(0)
+#define PFM_ALDN					BIT(1)
+#define PFM_LDKP					BIT(2)
+#define PFM_WOWL					BIT(3)
+#define ENPDN						BIT(4)
+#define PDN_PL						BIT(5)
+#define APFM_ONMAC					BIT(8)
+#define APFM_OFF					BIT(9)
+#define APFM_RSM					BIT(10)
+#define AFSM_HSUS					BIT(11)
+#define AFSM_PCIE					BIT(12)
+#define APDM_MAC					BIT(13)
+#define APDM_HOST					BIT(14)
+#define APDM_HPDN					BIT(15)
+#define RDY_MACON					BIT(16)
+#define SUS_HOST					BIT(17)
+#define ROP_ALD						BIT(20)
+#define ROP_PWR						BIT(21)
+#define ROP_SPS						BIT(22)
+#define SOP_MRST					BIT(25)
+#define SOP_FUSE					BIT(26)
+#define SOP_ABG						BIT(27)
+#define SOP_AMB						BIT(28)
+#define SOP_RCK						BIT(29)
+#define SOP_A8M						BIT(30)
+#define XOP_BTCK					BIT(31)
+
+#define ANAD16V_EN					BIT(0)
+#define ANA8M						BIT(1)
+#define MACSLP						BIT(4)
 #define LOADER_CLK_EN				BIT(5)
 #define _80M_SSC_DIS				BIT(7)
 #define _80M_SSC_EN_HO				BIT(8)
 #define PHY_SSC_RSTB				BIT(9)
-#define SEC_CLK_EN				BIT(10)
-#define MAC_CLK_EN				BIT(11)
-#define SYS_CLK_EN				BIT(12)
-#define RING_CLK_EN				BIT(13)
+#define SEC_CLK_EN					BIT(10)
+#define MAC_CLK_EN					BIT(11)
+#define SYS_CLK_EN					BIT(12)
+#define RING_CLK_EN					BIT(13)
 
 #define	BOOT_FROM_EEPROM			BIT(4)
-#define	EEPROM_EN				BIT(5)
+#define	EEPROM_EN					BIT(5)
 
-#define AFE_BGEN				BIT(0)
-#define AFE_MBEN				BIT(1)
-#define MAC_ID_EN				BIT(7)
+#define AFE_BGEN					BIT(0)
+#define AFE_MBEN					BIT(1)
+#define MAC_ID_EN					BIT(7)
 
-#define WLOCK_ALL				BIT(0)
-#define WLOCK_00				BIT(1)
-#define WLOCK_04				BIT(2)
-#define WLOCK_08				BIT(3)
-#define WLOCK_40				BIT(4)
+#define WLOCK_ALL					BIT(0)
+#define WLOCK_00					BIT(1)
+#define WLOCK_04					BIT(2)
+#define WLOCK_08					BIT(3)
+#define WLOCK_40					BIT(4)
 #define R_DIS_PRST_0				BIT(5)
 #define R_DIS_PRST_1				BIT(6)
-#define LOCK_ALL_EN				BIT(7)
+#define LOCK_ALL_EN					BIT(7)
 
-#define RF_EN					BIT(0)
-#define RF_RSTB					BIT(1)
-#define RF_SDMRSTB				BIT(2)
+#define RF_EN						BIT(0)
+#define RF_RSTB						BIT(1)
+#define RF_SDMRSTB					BIT(2)
 
-#define LDA15_EN				BIT(0)
-#define LDA15_STBY				BIT(1)
-#define LDA15_OBUF				BIT(2)
+#define LDA15_EN					BIT(0)
+#define LDA15_STBY					BIT(1)
+#define LDA15_OBUF					BIT(2)
 #define LDA15_REG_VOS				BIT(3)
 #define _LDA15_VOADJ(x)				(((x) & 0x7) << 4)
 
-#define LDV12_EN				BIT(0)
-#define LDV12_SDBY				BIT(1)
-#define LPLDO_HSM				BIT(2)
+#define LDV12_EN					BIT(0)
+#define LDV12_SDBY					BIT(1)
+#define LPLDO_HSM					BIT(2)
 #define LPLDO_LSM_DIS				BIT(3)
 #define _LDV12_VADJ(x)				(((x) & 0xF) << 4)
 
-#define XTAL_EN					BIT(0)
-#define XTAL_BSEL				BIT(1)
+#define XTAL_EN						BIT(0)
+#define XTAL_BSEL					BIT(1)
 #define _XTAL_BOSC(x)				(((x) & 0x3) << 2)
 #define _XTAL_CADJ(x)				(((x) & 0xF) << 4)
 #define XTAL_GATE_USB				BIT(8)
@@ -826,146 +849,146 @@
 #define _XTAL_BT_DRV(x)				(((x) & 0x3) << 21)
 #define _XTAL_GPIO(x)				(((x) & 0x7) << 23)
 
-#define CKDLY_AFE				BIT(26)
-#define CKDLY_USB				BIT(27)
-#define CKDLY_DIG				BIT(28)
-#define CKDLY_BT				BIT(29)
+#define CKDLY_AFE					BIT(26)
+#define CKDLY_USB					BIT(27)
+#define CKDLY_DIG					BIT(28)
+#define CKDLY_BT					BIT(29)
 
-#define APLL_EN					BIT(0)
-#define APLL_320_EN				BIT(1)
+#define APLL_EN						BIT(0)
+#define APLL_320_EN					BIT(1)
 #define APLL_FREF_SEL				BIT(2)
 #define APLL_EDGE_SEL				BIT(3)
-#define APLL_WDOGB				BIT(4)
-#define APLL_LPFEN				BIT(5)
+#define APLL_WDOGB					BIT(4)
+#define APLL_LPFEN					BIT(5)
 
 #define APLL_REF_CLK_13MHZ			0x1
-#define APLL_REF_CLK_19_2MHZ			0x2
+#define APLL_REF_CLK_19_2MHZ		0x2
 #define APLL_REF_CLK_20MHZ			0x3
 #define APLL_REF_CLK_25MHZ			0x4
 #define APLL_REF_CLK_26MHZ			0x5
-#define APLL_REF_CLK_38_4MHZ			0x6
+#define APLL_REF_CLK_38_4MHZ		0x6
 #define APLL_REF_CLK_40MHZ			0x7
 
-#define APLL_320EN				BIT(14)
-#define APLL_80EN				BIT(15)
-#define APLL_1MEN				BIT(24)
-
-#define ALD_EN					BIT(18)
-#define EF_PD					BIT(19)
-#define EF_FLAG					BIT(31)
-
-#define EF_TRPT					BIT(7)
-#define LDOE25_EN				BIT(31)
-
-#define RSM_EN					BIT(0)
-#define Timer_EN				BIT(4)
-
-#define TRSW0EN					BIT(2)
-#define TRSW1EN					BIT(3)
-#define EROM_EN					BIT(4)
-#define EnBT					BIT(5)
-#define EnUart					BIT(8)
-#define Uart_910				BIT(9)
-#define EnPMAC					BIT(10)
-#define SIC_SWRST				BIT(11)
-#define EnSIC					BIT(12)
-#define SIC_23					BIT(13)
-#define EnHDP					BIT(14)
-#define SIC_LBK					BIT(15)
-
-#define LED0PL					BIT(4)
-#define LED1PL					BIT(12)
-#define LED0DIS					BIT(7)
-
-#define MCUFWDL_EN				BIT(0)
-#define MCUFWDL_RDY				BIT(1)
-#define FWDL_ChkSum_rpt				BIT(2)
-#define MACINI_RDY				BIT(3)
-#define BBINI_RDY				BIT(4)
-#define RFINI_RDY				BIT(5)
-#define WINTINI_RDY				BIT(6)
-#define CPRST					BIT(23)
-
-#define XCLK_VLD				BIT(0)
-#define ACLK_VLD				BIT(1)
-#define UCLK_VLD				BIT(2)
-#define PCLK_VLD				BIT(3)
-#define PCIRSTB					BIT(4)
-#define V15_VLD					BIT(5)
-#define TRP_B15V_EN				BIT(7)
-#define SIC_IDLE				BIT(8)
-#define BD_MAC2					BIT(9)
-#define BD_MAC1					BIT(10)
+#define APLL_320EN					BIT(14)
+#define APLL_80EN					BIT(15)
+#define APLL_1MEN					BIT(24)
+
+#define ALD_EN						BIT(18)
+#define EF_PD						BIT(19)
+#define EF_FLAG						BIT(31)
+
+#define EF_TRPT						BIT(7)
+#define LDOE25_EN					BIT(31)
+
+#define RSM_EN						BIT(0)
+#define TIMER_EN					BIT(4)
+
+#define TRSW0EN						BIT(2)
+#define TRSW1EN						BIT(3)
+#define EROM_EN						BIT(4)
+#define ENBT						BIT(5)
+#define ENUART						BIT(8)
+#define UART_910					BIT(9)
+#define ENPMAC						BIT(10)
+#define SIC_SWRST					BIT(11)
+#define ENSIC						BIT(12)
+#define SIC_23						BIT(13)
+#define ENHDP						BIT(14)
+#define SIC_LBK						BIT(15)
+
+#define LED0PL						BIT(4)
+#define LED1PL						BIT(12)
+#define LED0DIS						BIT(7)
+
+#define MCUFWDL_EN					BIT(0)
+#define MCUFWDL_RDY					BIT(1)
+#define FWDL_CHKSUM_RPT				BIT(2)
+#define MACINI_RDY					BIT(3)
+#define BBINI_RDY					BIT(4)
+#define RFINI_RDY					BIT(5)
+#define WINTINI_RDY					BIT(6)
+#define CPRST						BIT(23)
+
+#define XCLK_VLD					BIT(0)
+#define ACLK_VLD					BIT(1)
+#define UCLK_VLD					BIT(2)
+#define PCLK_VLD					BIT(3)
+#define PCIRSTB						BIT(4)
+#define V15_VLD						BIT(5)
+#define TRP_B15V_EN					BIT(7)
+#define SIC_IDLE					BIT(8)
+#define BD_MAC2						BIT(9)
+#define BD_MAC1						BIT(10)
 #define IC_MACPHY_MODE				BIT(11)
-#define BT_FUNC					BIT(16)
-#define VENDOR_ID				BIT(19)
+#define BT_FUNC						BIT(16)
+#define VENDOR_ID					BIT(19)
 #define PAD_HWPD_IDN				BIT(22)
-#define TRP_VAUX_EN				BIT(23)
-#define TRP_BT_EN				BIT(24)
-#define BD_PKG_SEL				BIT(25)
-#define BD_HCI_SEL				BIT(26)
-#define TYPE_ID					BIT(27)
+#define TRP_VAUX_EN					BIT(23)
+#define TRP_BT_EN					BIT(24)
+#define BD_PKG_SEL					BIT(25)
+#define BD_HCI_SEL					BIT(26)
+#define TYPE_ID						BIT(27)
 
 #define CHIP_VER_RTL_MASK			0xF000
 #define CHIP_VER_RTL_SHIFT			12
 
-#define REG_LBMODE				(REG_CR + 3)
+#define REG_LBMODE					(REG_CR + 3)
 
 #define HCI_TXDMA_EN				BIT(0)
 #define HCI_RXDMA_EN				BIT(1)
-#define TXDMA_EN				BIT(2)
-#define RXDMA_EN				BIT(3)
-#define PROTOCOL_EN				BIT(4)
-#define SCHEDULE_EN				BIT(5)
-#define MACTXEN					BIT(6)
-#define MACRXEN					BIT(7)
-#define ENSWBCN					BIT(8)
-#define ENSEC					BIT(9)
-
-#define _NETTYPE(x)				(((x) & 0x3) << 16)
+#define TXDMA_EN					BIT(2)
+#define RXDMA_EN					BIT(3)
+#define PROTOCOL_EN					BIT(4)
+#define SCHEDULE_EN					BIT(5)
+#define MACTXEN						BIT(6)
+#define MACRXEN						BIT(7)
+#define ENSWBCN						BIT(8)
+#define ENSEC						BIT(9)
+
+#define _NETTYPE(x)					(((x) & 0x3) << 16)
 #define MASK_NETTYPE				0x30000
-#define NT_NO_LINK				0x0
+#define NT_NO_LINK					0x0
 #define NT_LINK_AD_HOC				0x1
-#define NT_LINK_AP				0x2
-#define NT_AS_AP				0x3
+#define NT_LINK_AP					0x2
+#define NT_AS_AP					0x3
 
-#define _LBMODE(x)				(((x) & 0xF) << 24)
-#define MASK_LBMODE				0xF000000
+#define _LBMODE(x)					(((x) & 0xF) << 24)
+#define MASK_LBMODE					0xF000000
 #define LOOPBACK_NORMAL				0x0
-#define LOOPBACK_IMMEDIATELY			0xB
+#define LOOPBACK_IMMEDIATELY		0xB
 #define LOOPBACK_MAC_DELAY			0x3
 #define LOOPBACK_PHY				0x1
 #define LOOPBACK_DMA				0x7
 
-#define GET_RX_PAGE_SIZE(value)			((value) & 0xF)
-#define GET_TX_PAGE_SIZE(value)			(((value) & 0xF0) >> 4)
-#define _PSRX_MASK				0xF
-#define _PSTX_MASK				0xF0
-#define _PSRX(x)				(x)
-#define _PSTX(x)				((x) << 4)
+#define GET_RX_PAGE_SIZE(value)		((value) & 0xF)
+#define GET_TX_PAGE_SIZE(value)		(((value) & 0xF0) >> 4)
+#define _PSRX_MASK					0xF
+#define _PSTX_MASK					0xF0
+#define _PSRX(x)					(x)
+#define _PSTX(x)					((x) << 4)
 
-#define PBP_64					0x0
-#define PBP_128					0x1
-#define PBP_256					0x2
-#define PBP_512					0x3
-#define PBP_1024				0x4
+#define PBP_64						0x0
+#define PBP_128						0x1
+#define PBP_256						0x2
+#define PBP_512						0x3
+#define PBP_1024					0x4
 
 #define RXDMA_ARBBW_EN				BIT(0)
-#define RXSHFT_EN				BIT(1)
+#define RXSHFT_EN					BIT(1)
 #define RXDMA_AGG_EN				BIT(2)
-#define QS_VO_QUEUE				BIT(8)
-#define QS_VI_QUEUE				BIT(9)
-#define QS_BE_QUEUE				BIT(10)
-#define QS_BK_QUEUE				BIT(11)
+#define QS_VO_QUEUE					BIT(8)
+#define QS_VI_QUEUE					BIT(9)
+#define QS_BE_QUEUE					BIT(10)
+#define QS_BK_QUEUE					BIT(11)
 #define QS_MANAGER_QUEUE			BIT(12)
 #define QS_HIGH_QUEUE				BIT(13)
 
-#define HQSEL_VOQ				BIT(0)
-#define HQSEL_VIQ				BIT(1)
-#define HQSEL_BEQ				BIT(2)
-#define HQSEL_BKQ				BIT(3)
-#define HQSEL_MGTQ				BIT(4)
-#define HQSEL_HIQ				BIT(5)
+#define HQSEL_VOQ					BIT(0)
+#define HQSEL_VIQ					BIT(1)
+#define HQSEL_BEQ					BIT(2)
+#define HQSEL_BKQ					BIT(3)
+#define HQSEL_MGTQ					BIT(4)
+#define HQSEL_HIQ					BIT(5)
 
 #define _TXDMA_HIQ_MAP(x)			(((x)&0x3) << 14)
 #define _TXDMA_MGQ_MAP(x)			(((x)&0x3) << 12)
@@ -974,9 +997,9 @@
 #define _TXDMA_VIQ_MAP(x)			(((x)&0x3) << 6)
 #define _TXDMA_VOQ_MAP(x)			(((x)&0x3) << 4)
 
-#define QUEUE_LOW				1
+#define QUEUE_LOW					1
 #define QUEUE_NORMAL				2
-#define QUEUE_HIGH				3
+#define QUEUE_HIGH					3
 
 #define _LLT_NO_ACTIVE				0x0
 #define _LLT_WRITE_ACCESS			0x1
@@ -984,25 +1007,25 @@
 
 #define _LLT_INIT_DATA(x)			((x) & 0xFF)
 #define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
-#define _LLT_OP(x)				(((x) & 0x3) << 30)
+#define _LLT_OP(x)					(((x) & 0x3) << 30)
 #define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
 
 #define BB_WRITE_READ_MASK			(BIT(31) | BIT(30))
-#define BB_WRITE_EN				BIT(30)
-#define BB_READ_EN				BIT(31)
+#define BB_WRITE_EN					BIT(30)
+#define BB_READ_EN					BIT(31)
 
-#define _HPQ(x)					((x) & 0xFF)
-#define _LPQ(x)					(((x) & 0xFF) << 8)
-#define _PUBQ(x)				(((x) & 0xFF) << 16)
-#define _NPQ(x)					((x) & 0xFF)
+#define _HPQ(x)			((x) & 0xFF)
+#define _LPQ(x)			(((x) & 0xFF) << 8)
+#define _PUBQ(x)		(((x) & 0xFF) << 16)
+#define _NPQ(x)			((x) & 0xFF)
 
-#define HPQ_PUBLIC_DIS				BIT(24)
-#define LPQ_PUBLIC_DIS				BIT(25)
-#define LD_RQPN					BIT(31)
+#define HPQ_PUBLIC_DIS		BIT(24)
+#define LPQ_PUBLIC_DIS		BIT(25)
+#define LD_RQPN			BIT(31)
 
-#define BCN_VALID				BIT(16)
-#define BCN_HEAD(x)				(((x) & 0xFF) << 8)
-#define	BCN_HEAD_MASK				0xFF00
+#define BCN_VALID		BIT(16)
+#define BCN_HEAD(x)		(((x) & 0xFF) << 8)
+#define	BCN_HEAD_MASK		0xFF00
 
 #define BLK_DESC_NUM_SHIFT			4
 #define BLK_DESC_NUM_MASK			0xF
@@ -1022,9 +1045,9 @@
 
 #define _RRSR_RSC(x)				(((x) & 0x3) << 21)
 #define RRSR_RSC_RESERVED			0x0
-#define RRSR_RSC_UPPER_SUBCHANNEL		0x1
-#define RRSR_RSC_LOWER_SUBCHANNEL		0x2
-#define RRSR_RSC_DUPLICATE_MODE			0x3
+#define RRSR_RSC_UPPER_SUBCHANNEL	0x1
+#define RRSR_RSC_LOWER_SUBCHANNEL	0x2
+#define RRSR_RSC_DUPLICATE_MODE		0x3
 
 #define USE_SHORT_G1				BIT(20)
 
@@ -1037,8 +1060,8 @@
 #define _AGGLMT_MCS6(x)				(((x) & 0xF) << 24)
 #define _AGGLMT_MCS7(x)				(((x) & 0xF) << 28)
 
-#define	RETRY_LIMIT_SHORT_SHIFT			8
-#define	RETRY_LIMIT_LONG_SHIFT			0
+#define	RETRY_LIMIT_SHORT_SHIFT		8
+#define	RETRY_LIMIT_LONG_SHIFT		0
 
 #define _DARF_RC1(x)				((x) & 0x1F)
 #define _DARF_RC2(x)				(((x) & 0x1F) << 8)
@@ -1058,123 +1081,123 @@
 #define _RARF_RC7(x)				(((x) & 0x1F) << 16)
 #define _RARF_RC8(x)				(((x) & 0x1F) << 24)
 
-#define AC_PARAM_TXOP_LIMIT_OFFSET		16
-#define AC_PARAM_ECW_MAX_OFFSET			12
-#define AC_PARAM_ECW_MIN_OFFSET			8
-#define AC_PARAM_AIFS_OFFSET			0
+#define AC_PARAM_TXOP_LIMIT_OFFSET	16
+#define AC_PARAM_ECW_MAX_OFFSET		12
+#define AC_PARAM_ECW_MIN_OFFSET		8
+#define AC_PARAM_AIFS_OFFSET		0
 
-#define _AIFS(x)				(x)
+#define _AIFS(x)					(x)
 #define _ECW_MAX_MIN(x)				((x) << 8)
 #define _TXOP_LIMIT(x)				((x) << 16)
 
-#define _BCNIFS(x)				((x) & 0xFF)
-#define _BCNECW(x)				((((x) & 0xF)) << 8)
+#define _BCNIFS(x)					((x) & 0xFF)
+#define _BCNECW(x)					((((x) & 0xF)) << 8)
 
-#define _LRL(x)					((x) & 0x3F)
-#define _SRL(x)					(((x) & 0x3F) << 8)
+#define _LRL(x)						((x) & 0x3F)
+#define _SRL(x)						(((x) & 0x3F) << 8)
 
 #define _SIFS_CCK_CTX(x)			((x) & 0xFF)
-#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8);
+#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8)
 
 #define _SIFS_OFDM_CTX(x)			((x) & 0xFF)
-#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8);
+#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8)
 
-#define _TBTT_PROHIBIT_HOLD(x)			(((x) & 0xFF) << 8)
+#define _TBTT_PROHIBIT_HOLD(x)		(((x) & 0xFF) << 8)
 
 #define DIS_EDCA_CNT_DWN			BIT(11)
 
-#define EN_MBSSID				BIT(1)
+#define EN_MBSSID					BIT(1)
 #define EN_TXBCN_RPT				BIT(2)
 #define	EN_BCN_FUNCTION				BIT(3)
 
-#define TSFTR_RST				BIT(0)
-#define TSFTR1_RST				BIT(1)
+#define TSFTR_RST					BIT(0)
+#define TSFTR1_RST					BIT(1)
 
-#define STOP_BCNQ				BIT(6)
+#define STOP_BCNQ					BIT(6)
 
-#define	DIS_TSF_UDT0_NORMAL_CHIP		BIT(4)
-#define	DIS_TSF_UDT0_TEST_CHIP			BIT(5)
+#define	DIS_TSF_UDT0_NORMAL_CHIP	BIT(4)
+#define	DIS_TSF_UDT0_TEST_CHIP		BIT(5)
 
-#define	AcmHw_HwEn				BIT(0)
-#define	AcmHw_BeqEn				BIT(1)
-#define	AcmHw_ViqEn				BIT(2)
-#define	AcmHw_VoqEn				BIT(3)
-#define	AcmHw_BeqStatus				BIT(4)
-#define	AcmHw_ViqStatus				BIT(5)
-#define	AcmHw_VoqStatus				BIT(6)
+#define	ACMHW_HWEN					BIT(0)
+#define	ACMHW_BEQEN					BIT(1)
+#define	ACMHW_VIQEN					BIT(2)
+#define	ACMHW_VOQEN					BIT(3)
+#define	ACMHW_BEQSTATUS				BIT(4)
+#define	ACMHW_VIQSTATUS				BIT(5)
+#define	ACMHW_VOQSTATUS				BIT(6)
 
-#define APSDOFF					BIT(6)
+#define APSDOFF						BIT(6)
 #define APSDOFF_STATUS				BIT(7)
 
-#define BW_20MHZ				BIT(2)
+#define BW_20MHZ					BIT(2)
 
 #define RATE_BITMAP_ALL				0xFFFFF
 
-#define RATE_RRSR_CCK_ONLY_1M			0xFFFF1
+#define RATE_RRSR_CCK_ONLY_1M		0xFFFF1
 
-#define TSFRST					BIT(0)
-#define DIS_GCLK				BIT(1)
-#define PAD_SEL					BIT(2)
-#define PWR_ST					BIT(6)
+#define TSFRST						BIT(0)
+#define DIS_GCLK					BIT(1)
+#define PAD_SEL						BIT(2)
+#define PWR_ST						BIT(6)
 #define PWRBIT_OW_EN				BIT(7)
-#define ACRC					BIT(8)
-#define CFENDFORM				BIT(9)
-#define ICV					BIT(10)
-
-#define AAP					BIT(0)
-#define APM					BIT(1)
-#define AM					BIT(2)
-#define AB					BIT(3)
-#define ADD3					BIT(4)
-#define APWRMGT					BIT(5)
-#define CBSSID					BIT(6)
-#define CBSSID_DATA				BIT(6)
-#define CBSSID_BCN				BIT(7)
-#define ACRC32					BIT(8)
-#define AICV					BIT(9)
-#define ADF					BIT(11)
-#define ACF					BIT(12)
-#define AMF					BIT(13)
+#define ACRC						BIT(8)
+#define CFENDFORM					BIT(9)
+#define ICV							BIT(10)
+
+#define AAP							BIT(0)
+#define APM							BIT(1)
+#define AM							BIT(2)
+#define AB							BIT(3)
+#define ADD3						BIT(4)
+#define APWRMGT						BIT(5)
+#define CBSSID						BIT(6)
+#define CBSSID_DATA					BIT(6)
+#define CBSSID_BCN					BIT(7)
+#define ACRC32						BIT(8)
+#define AICV						BIT(9)
+#define ADF							BIT(11)
+#define ACF							BIT(12)
+#define AMF							BIT(13)
 #define HTC_LOC_CTRL				BIT(14)
-#define UC_DATA_EN				BIT(16)
-#define BM_DATA_EN				BIT(17)
-#define MFBEN					BIT(22)
-#define LSIGEN					BIT(23)
-#define EnMBID					BIT(24)
-#define APP_BASSN				BIT(27)
-#define APP_PHYSTS				BIT(28)
-#define APP_ICV					BIT(29)
-#define APP_MIC					BIT(30)
-#define APP_FCS					BIT(31)
+#define UC_DATA_EN					BIT(16)
+#define BM_DATA_EN					BIT(17)
+#define MFBEN						BIT(22)
+#define LSIGEN						BIT(23)
+#define ENMBID						BIT(24)
+#define APP_BASSN					BIT(27)
+#define APP_PHYSTS					BIT(28)
+#define APP_ICV						BIT(29)
+#define APP_MIC						BIT(30)
+#define APP_FCS						BIT(31)
 
 #define _MIN_SPACE(x)				((x) & 0x7)
-#define _SHORT_GI_PADDING(x)			(((x) & 0x1F) << 3)
+#define _SHORT_GI_PADDING(x)		(((x) & 0x1F) << 3)
 
-#define RXERR_TYPE_OFDM_PPDU			0
-#define RXERR_TYPE_OFDM_FALSE_ALARM		1
-#define	RXERR_TYPE_OFDM_MPDU_OK			2
-#define RXERR_TYPE_OFDM_MPDU_FAIL		3
+#define RXERR_TYPE_OFDM_PPDU		0
+#define RXERR_TYPE_OFDM_FALSE_ALARM	1
+#define	RXERR_TYPE_OFDM_MPDU_OK		2
+#define RXERR_TYPE_OFDM_MPDU_FAIL	3
 #define RXERR_TYPE_CCK_PPDU			4
-#define RXERR_TYPE_CCK_FALSE_ALARM		5
-#define RXERR_TYPE_CCK_MPDU_OK			6
-#define RXERR_TYPE_CCK_MPDU_FAIL		7
+#define RXERR_TYPE_CCK_FALSE_ALARM	5
+#define RXERR_TYPE_CCK_MPDU_OK		6
+#define RXERR_TYPE_CCK_MPDU_FAIL	7
 #define RXERR_TYPE_HT_PPDU			8
-#define RXERR_TYPE_HT_FALSE_ALARM		9
-#define RXERR_TYPE_HT_MPDU_TOTAL		10
-#define RXERR_TYPE_HT_MPDU_OK			11
-#define RXERR_TYPE_HT_MPDU_FAIL			12
-#define RXERR_TYPE_RX_FULL_DROP			15
+#define RXERR_TYPE_HT_FALSE_ALARM	9
+#define RXERR_TYPE_HT_MPDU_TOTAL	10
+#define RXERR_TYPE_HT_MPDU_OK		11
+#define RXERR_TYPE_HT_MPDU_FAIL		12
+#define RXERR_TYPE_RX_FULL_DROP		15
 
 #define RXERR_COUNTER_MASK			0xFFFFF
 #define RXERR_RPT_RST				BIT(27)
-#define _RXERR_RPT_SEL(type)			((type) << 28)
-
-#define	SCR_TxUseDK				BIT(0)
-#define	SCR_RxUseDK				BIT(1)
-#define	SCR_TxEncEnable				BIT(2)
-#define	SCR_RxDecEnable				BIT(3)
-#define	SCR_SKByA2				BIT(4)
-#define	SCR_NoSKMC				BIT(5)
+#define _RXERR_RPT_SEL(type)		((type) << 28)
+
+#define	SCR_TXUSEDK					BIT(0)
+#define	SCR_RXUSEDK					BIT(1)
+#define	SCR_TXENCENABLE				BIT(2)
+#define	SCR_RXDECENABLE				BIT(3)
+#define	SCR_SKBYA2					BIT(4)
+#define	SCR_NOSKMC					BIT(5)
 #define SCR_TXBCUSEDK				BIT(6)
 #define SCR_RXBCUSEDK				BIT(7)
 
@@ -1182,32 +1205,32 @@
 #define USB_IS_FULL_SPEED			1
 #define USB_SPEED_MASK				BIT(5)
 
-#define USB_NORMAL_SIE_EP_MASK			0xF
-#define USB_NORMAL_SIE_EP_SHIFT			4
+#define USB_NORMAL_SIE_EP_MASK		0xF
+#define USB_NORMAL_SIE_EP_SHIFT		4
 
 #define USB_TEST_EP_MASK			0x30
 #define USB_TEST_EP_SHIFT			4
 
-#define USB_AGG_EN				BIT(3)
+#define USB_AGG_EN					BIT(3)
 
 #define MAC_ADDR_LEN				6
-#define LAST_ENTRY_OF_TX_PKT_BUFFER		255
+#define LAST_ENTRY_OF_TX_PKT_BUFFER	255
 
-#define POLLING_LLT_THRESHOLD			20
-#define POLLING_READY_TIMEOUT_COUNT		1000
+#define POLLING_LLT_THRESHOLD		20
+#define POLLING_READY_TIMEOUT_COUNT	1000
 
 #define	MAX_MSS_DENSITY_2T			0x13
 #define	MAX_MSS_DENSITY_1T			0x0A
 
-#define EPROM_CMD_OPERATING_MODE_MASK		((1<<7)|(1<<6))
+#define EPROM_CMD_OPERATING_MODE_MASK	((1<<7)|(1<<6))
 #define EPROM_CMD_CONFIG			0x3
 #define EPROM_CMD_LOAD				1
 
 #define	HWSET_MAX_SIZE_92S			HWSET_MAX_SIZE
 
-#define	HAL_8192C_HW_GPIO_WPS_BIT		BIT(2)
+#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT(2)
 
-#define	RPMAC_RESET				0x100
+#define	RPMAC_RESET					0x100
 #define	RPMAC_TXSTART				0x104
 #define	RPMAC_TXLEGACYSIG			0x108
 #define	RPMAC_TXHTSIG1				0x10c
@@ -1223,12 +1246,12 @@
 #define	RPMAC_TXMACHEADER5			0x134
 #define	RPMAC_TXDADATYPE			0x138
 #define	RPMAC_TXRANDOMSEED			0x13c
-#define	RPMAC_CCKPLCPPREAMBLE			0x140
+#define	RPMAC_CCKPLCPPREAMBLE		0x140
 #define	RPMAC_CCKPLCPHEADER			0x144
 #define	RPMAC_CCKCRC16				0x148
 #define	RPMAC_OFDMRXCRC32OK			0x170
-#define	RPMAC_OFDMRXCRC32Er			0x174
-#define	RPMAC_OFDMRXPARITYER			0x178
+#define	RPMAC_OFDMRXCRC32ER			0x174
+#define	RPMAC_OFDMRXPARITYER		0x178
 #define	RPMAC_OFDMRXCRC8ER			0x17c
 #define	RPMAC_CCKCRXRC16ER			0x180
 #define	RPMAC_CCKCRXRC32ER			0x184
@@ -1245,44 +1268,44 @@
 #define	RFPGA0_RFTIMING1			0x810
 #define	RFPGA0_RFTIMING2			0x814
 
-#define	RFPGA0_XA_HSSIPARAMETER1		0x820
-#define	RFPGA0_XA_HSSIPARAMETER2		0x824
-#define	RFPGA0_XB_HSSIPARAMETER1		0x828
-#define	RFPGA0_XB_HSSIPARAMETER2		0x82c
+#define	RFPGA0_XA_HSSIPARAMETER1	0x820
+#define	RFPGA0_XA_HSSIPARAMETER2	0x824
+#define	RFPGA0_XB_HSSIPARAMETER1	0x828
+#define	RFPGA0_XB_HSSIPARAMETER2	0x82c
 
-#define	RFPGA0_XA_LSSIPARAMETER			0x840
-#define	RFPGA0_XB_LSSIPARAMETER			0x844
+#define	RFPGA0_XA_LSSIPARAMETER		0x840
+#define	RFPGA0_XB_LSSIPARAMETER		0x844
 
-#define	RFPGA0_RFWAKEUPPARAMETER		0x850
-#define	RFPGA0_RFSLEEPUPPARAMETER		0x854
+#define	RFPGA0_RFWAKEUPPARAMETER	0x850
+#define	RFPGA0_RFSLEEPUPPARAMETER	0x854
 
-#define	RFPGA0_XAB_SWITCHCONTROL		0x858
-#define	RFPGA0_XCD_SWITCHCONTROL		0x85c
+#define	RFPGA0_XAB_SWITCHCONTROL	0x858
+#define	RFPGA0_XCD_SWITCHCONTROL	0x85c
 
-#define	RFPGA0_XA_RFINTERFACEOE			0x860
-#define	RFPGA0_XB_RFINTERFACEOE			0x864
+#define	RFPGA0_XA_RFINTERFACEOE		0x860
+#define	RFPGA0_XB_RFINTERFACEOE		0x864
 
-#define	RFPGA0_XAB_RFINTERFACESW		0x870
-#define	RFPGA0_XCD_RFINTERFACESW		0x874
+#define	RFPGA0_XAB_RFINTERFACESW	0x870
+#define	RFPGA0_XCD_RFINTERFACESW	0x874
 
-#define	rFPGA0_XAB_RFPARAMETER			0x878
-#define	rFPGA0_XCD_RFPARAMETER			0x87c
+#define	RFPGA0_XAB_RFPARAMETER		0x878
+#define	RFPGA0_XCD_RFPARAMETER		0x87c
 
-#define	RFPGA0_ANALOGPARAMETER1			0x880
-#define	RFPGA0_ANALOGPARAMETER2			0x884
-#define	RFPGA0_ANALOGPARAMETER3			0x888
-#define	RFPGA0_ANALOGPARAMETER4			0x88c
+#define	RFPGA0_ANALOGPARAMETER1		0x880
+#define	RFPGA0_ANALOGPARAMETER2		0x884
+#define	RFPGA0_ANALOGPARAMETER3		0x888
+#define	RFPGA0_ANALOGPARAMETER4		0x88c
 
-#define	RFPGA0_XA_LSSIREADBACK			0x8a0
-#define	RFPGA0_XB_LSSIREADBACK			0x8a4
-#define	RFPGA0_XC_LSSIREADBACK			0x8a8
-#define	RFPGA0_XD_LSSIREADBACK			0x8ac
+#define	RFPGA0_XA_LSSIREADBACK		0x8a0
+#define	RFPGA0_XB_LSSIREADBACK		0x8a4
+#define	RFPGA0_XC_LSSIREADBACK		0x8a8
+#define	RFPGA0_XD_LSSIREADBACK		0x8ac
 
 #define	RFPGA0_PSDREPORT			0x8b4
-#define	TRANSCEIVEA_HSPI_READBACK		0x8b8
-#define	TRANSCEIVEB_HSPI_READBACK		0x8bc
-#define	RFPGA0_XAB_RFINTERFACERB		0x8e0
-#define	RFPGA0_XCD_RFINTERFACERB		0x8e4
+#define	TRANSCEIVEA_HSPI_READBACK	0x8b8
+#define	TRANSCEIVEB_HSPI_READBACK	0x8bc
+#define	RFPGA0_XAB_RFINTERFACERB	0x8e0
+#define	RFPGA0_XCD_RFINTERFACERB	0x8e4
 
 #define	RFPGA1_RFMOD				0x900
 
@@ -1293,12 +1316,12 @@
 #define	RCCK0_SYSTEM				0xa00
 
 #define	RCCK0_AFESETTING			0xa04
-#define	RCCK0_CCA				0xa08
+#define	RCCK0_CCA					0xa08
 
 #define	RCCK0_RXAGC1				0xa0c
 #define	RCCK0_RXAGC2				0xa10
 
-#define	RCCK0_RXHP				0xa14
+#define	RCCK0_RXHP					0xa14
 
 #define	RCCK0_DSPPARAMETER1			0xa18
 #define	RCCK0_DSPPARAMETER2			0xa1c
@@ -1306,26 +1329,26 @@
 #define	RCCK0_TXFILTER1				0xa20
 #define	RCCK0_TXFILTER2				0xa24
 #define	RCCK0_DEBUGPORT				0xa28
-#define	RCCK0_FALSEALARMREPORT			0xa2c
-#define	RCCK0_TRSSIREPORT			0xa50
-#define	RCCK0_RXREPORT				0xa54
-#define	RCCK0_FACOUNTERLOWER			0xa5c
-#define	RCCK0_FACOUNTERUPPER			0xa58
+#define	RCCK0_FALSEALARMREPORT		0xa2c
+#define	RCCK0_TRSSIREPORT		0xa50
+#define	RCCK0_RXREPORT			0xa54
+#define	RCCK0_FACOUNTERLOWER		0xa5c
+#define	RCCK0_FACOUNTERUPPER		0xa58
 
-#define	ROFDM0_LSTF				0xc00
+#define	ROFDM0_LSTF					0xc00
 
-#define	ROFDM0_TRXPATHENABLE			0xc04
+#define	ROFDM0_TRXPATHENABLE		0xc04
 #define	ROFDM0_TRMUXPAR				0xc08
-#define	ROFDM0_TRSWISOLATION			0xc0c
+#define	ROFDM0_TRSWISOLATION		0xc0c
 
 #define	ROFDM0_XARXAFE				0xc10
-#define	ROFDM0_XARXIQIMBALANCE			0xc14
-#define	ROFDM0_XBRXAFE				0xc18
-#define	ROFDM0_XBRXIQIMBALANCE			0xc1c
-#define	ROFDM0_XCRXAFE				0xc20
-#define	ROFDM0_XCRXIQIMBANLANCE			0xc24
-#define	ROFDM0_XDRXAFE				0xc28
-#define	ROFDM0_XDRXIQIMBALANCE			0xc2c
+#define	ROFDM0_XARXIQIMBALANCE		0xc14
+#define	ROFDM0_XBRXAFE			0xc18
+#define	ROFDM0_XBRXIQIMBALANCE		0xc1c
+#define	ROFDM0_XCRXAFE			0xc20
+#define	ROFDM0_XCRXIQIMBANLANCE		0xc24
+#define	ROFDM0_XDRXAFE			0xc28
+#define	ROFDM0_XDRXIQIMBALANCE		0xc2c
 
 #define	ROFDM0_RXDETECTOR1			0xc30
 #define	ROFDM0_RXDETECTOR2			0xc34
@@ -1334,8 +1357,8 @@
 
 #define	ROFDM0_RXDSP				0xc40
 #define	ROFDM0_CFOANDDAGC			0xc44
-#define	ROFDM0_CCADROPTHRESHOLD			0xc48
-#define	ROFDM0_ECCATHRESHOLD			0xc4c
+#define	ROFDM0_CCADROPTHRESHOLD		0xc48
+#define	ROFDM0_ECCATHRESHOLD		0xc4c
 
 #define	ROFDM0_XAAGCCORE1			0xc50
 #define	ROFDM0_XAAGCCORE2			0xc54
@@ -1346,24 +1369,24 @@
 #define	ROFDM0_XDAGCCORE1			0xc68
 #define	ROFDM0_XDAGCCORE2			0xc6c
 
-#define	ROFDM0_AGCPARAMETER1			0xc70
-#define	ROFDM0_AGCPARAMETER2			0xc74
+#define	ROFDM0_AGCPARAMETER1		0xc70
+#define	ROFDM0_AGCPARAMETER2		0xc74
 #define	ROFDM0_AGCRSSITABLE			0xc78
 #define	ROFDM0_HTSTFAGC				0xc7c
 
-#define	ROFDM0_XATXIQIMBALANCE			0xc80
+#define	ROFDM0_XATXIQIMBALANCE		0xc80
 #define	ROFDM0_XATXAFE				0xc84
-#define	ROFDM0_XBTXIQIMBALANCE			0xc88
+#define	ROFDM0_XBTXIQIMBALANCE		0xc88
 #define	ROFDM0_XBTXAFE				0xc8c
-#define	ROFDM0_XCTXIQIMBALANCE			0xc90
-#define	ROFDM0_XCTXAFE				0xc94
-#define	ROFDM0_XDTXIQIMBALANCE			0xc98
+#define	ROFDM0_XCTXIQIMBALANCE		0xc90
+#define	ROFDM0_XCTXAFE			0xc94
+#define	ROFDM0_XDTXIQIMBALANCE		0xc98
 #define	ROFDM0_XDTXAFE				0xc9c
 
 #define ROFDM0_RXIQEXTANTA			0xca0
 
-#define	ROFDM0_RXHPPARAMETER			0xce0
-#define	ROFDM0_TXPSEUDONOISEWGT			0xce4
+#define	ROFDM0_RXHPPARAMETER		0xce0
+#define	ROFDM0_TXPSEUDONOISEWGT		0xce4
 #define	ROFDM0_FRAMESYNC			0xcf0
 #define	ROFDM0_DFSREPORT			0xcf4
 #define	ROFDM0_TXCOEFF1				0xca4
@@ -1373,19 +1396,19 @@
 #define	ROFDM0_TXCOEFF5				0xcb4
 #define	ROFDM0_TXCOEFF6				0xcb8
 
-#define	ROFDM1_LSTF				0xd00
-#define	ROFDM1_TRXPATHENABLE			0xd04
+#define	ROFDM1_LSTF					0xd00
+#define	ROFDM1_TRXPATHENABLE		0xd04
 
-#define	ROFDM1_CF0				0xd08
-#define	ROFDM1_CSI1				0xd10
-#define	ROFDM1_SBD				0xd14
-#define	ROFDM1_CSI2				0xd18
+#define	ROFDM1_CF0					0xd08
+#define	ROFDM1_CSI1					0xd10
+#define	ROFDM1_SBD					0xd14
+#define	ROFDM1_CSI2					0xd18
 #define	ROFDM1_CFOTRACKING			0xd2c
 #define	ROFDM1_TRXMESAURE1			0xd34
 #define	ROFDM1_INTFDET				0xd3c
-#define	ROFDM1_PSEUDONOISESTATEAB		0xd50
-#define	ROFDM1_PSEUDONOISESTATECD		0xd54
-#define	ROFDM1_RXPSEUDONOISEWGT			0xd58
+#define	ROFDM1_PSEUDONOISESTATEAB	0xd50
+#define	ROFDM1_PSEUDONOISESTATECD	0xd54
+#define	ROFDM1_RXPSEUDONOISEWGT		0xd58
 
 #define	ROFDM_PHYCOUNTER1			0xda0
 #define	ROFDM_PHYCOUNTER2			0xda4
@@ -1397,35 +1420,35 @@
 #define	ROFDM_LONGCFOCD				0xdb8
 #define	ROFDM_TAILCF0AB				0xdbc
 #define	ROFDM_TAILCF0CD				0xdc0
-#define	ROFDM_PWMEASURE1			0xdc4
-#define	ROFDM_PWMEASURE2			0xdc8
+#define	ROFDM_PWMEASURE1		0xdc4
+#define	ROFDM_PWMEASURE2		0xdc8
 #define	ROFDM_BWREPORT				0xdcc
 #define	ROFDM_AGCREPORT				0xdd0
-#define	ROFDM_RXSNR				0xdd4
+#define	ROFDM_RXSNR					0xdd4
 #define	ROFDM_RXEVMCSI				0xdd8
 #define	ROFDM_SIGREPORT				0xddc
 
 #define	RTXAGC_A_RATE18_06			0xe00
 #define	RTXAGC_A_RATE54_24			0xe04
 #define	RTXAGC_A_CCK1_MCS32			0xe08
-#define	RTXAGC_A_MCS03_MCS00			0xe10
-#define	RTXAGC_A_MCS07_MCS04			0xe14
-#define	RTXAGC_A_MCS11_MCS08			0xe18
-#define	RTXAGC_A_MCS15_MCS12			0xe1c
+#define	RTXAGC_A_MCS03_MCS00		0xe10
+#define	RTXAGC_A_MCS07_MCS04		0xe14
+#define	RTXAGC_A_MCS11_MCS08		0xe18
+#define	RTXAGC_A_MCS15_MCS12		0xe1c
 
 #define	RTXAGC_B_RATE18_06			0x830
 #define	RTXAGC_B_RATE54_24			0x834
-#define	RTXAGC_B_CCK1_55_MCS32			0x838
-#define	RTXAGC_B_MCS03_MCS00			0x83c
-#define	RTXAGC_B_MCS07_MCS04			0x848
-#define	RTXAGC_B_MCS11_MCS08			0x84c
-#define	RTXAGC_B_MCS15_MCS12			0x868
-#define	RTXAGC_B_CCK11_A_CCK2_11		0x86c
+#define	RTXAGC_B_CCK1_55_MCS32		0x838
+#define	RTXAGC_B_MCS03_MCS00		0x83c
+#define	RTXAGC_B_MCS07_MCS04		0x848
+#define	RTXAGC_B_MCS11_MCS08		0x84c
+#define	RTXAGC_B_MCS15_MCS12		0x868
+#define	RTXAGC_B_CCK11_A_CCK2_11	0x86c
 
 #define	RZEBRA1_HSSIENABLE			0x0
 #define	RZEBRA1_TRXENABLE1			0x1
 #define	RZEBRA1_TRXENABLE2			0x2
-#define	RZEBRA1_AGC				0x4
+#define	RZEBRA1_AGC					0x4
 #define	RZEBRA1_CHARGEPUMP			0x5
 #define	RZEBRA1_CHANNEL				0x7
 
@@ -1434,649 +1457,664 @@
 #define	RZEBRA1_RXLPF				0xb
 #define	RZEBRA1_RXHPFCORNER			0xc
 
-#define	RGLOBALCTRL				0
+#define	RGLOBALCTRL					0
 #define	RRTL8256_TXLPF				19
 #define	RRTL8256_RXLPF				11
 #define	RRTL8258_TXLPF				0x11
 #define	RRTL8258_RXLPF				0x13
 #define	RRTL8258_RSSILPF			0xa
 
-#define	RF_AC					0x00
+#define	RF_AC						0x00
 
-#define	RF_IQADJ_G1				0x01
-#define	RF_IQADJ_G2				0x02
-#define	RF_POW_TRSW				0x05
+#define	RF_IQADJ_G1					0x01
+#define	RF_IQADJ_G2					0x02
+#define	RF_POW_TRSW					0x05
 
-#define	RF_GAIN_RX				0x06
-#define	RF_GAIN_TX				0x07
+#define	RF_GAIN_RX					0x06
+#define	RF_GAIN_TX					0x07
 
-#define	RF_TXM_IDAC				0x08
-#define	RF_BS_IQGEN				0x0F
+#define	RF_TXM_IDAC					0x08
+#define	RF_BS_IQGEN					0x0F
 
-#define	RF_MODE1				0x10
-#define	RF_MODE2				0x11
+#define	RF_MODE1					0x10
+#define	RF_MODE2					0x11
 
 #define	RF_RX_AGC_HP				0x12
-#define	RF_TX_AGC				0x13
-#define	RF_BIAS					0x14
-#define	RF_IPA					0x15
+#define	RF_TX_AGC					0x13
+#define	RF_BIAS						0x14
+#define	RF_IPA						0x15
 #define	RF_POW_ABILITY				0x17
-#define	RF_MODE_AG				0x18
-#define	RRFCHANNEL				0x18
-#define	RF_CHNLBW				0x18
-#define	RF_TOP					0x19
-
-#define	RF_RX_G1				0x1A
-#define	RF_RX_G2				0x1B
-
-#define	RF_RX_BB2				0x1C
-#define	RF_RX_BB1				0x1D
-
-#define	RF_RCK1					0x1E
-#define	RF_RCK2					0x1F
-
-#define	RF_TX_G1				0x20
-#define	RF_TX_G2				0x21
-#define	RF_TX_G3				0x22
-
-#define	RF_TX_BB1				0x23
-#define	RF_T_METER				0x24
-
-#define	RF_SYN_G1				0x25
-#define	RF_SYN_G2				0x26
-#define	RF_SYN_G3				0x27
-#define	RF_SYN_G4				0x28
-#define	RF_SYN_G5				0x29
-#define	RF_SYN_G6				0x2A
-#define	RF_SYN_G7				0x2B
-#define	RF_SYN_G8				0x2C
-
-#define	RF_RCK_OS				0x30
-#define	RF_TXPA_G1				0x31
-#define	RF_TXPA_G2				0x32
-#define	RF_TXPA_G3				0x33
-
-#define	BBBRESETB				0x100
+#define	RF_MODE_AG					0x18
+#define	RRFCHANNEL					0x18
+#define	RF_CHNLBW					0x18
+#define	RF_TOP						0x19
+
+#define	RF_RX_G1					0x1A
+#define	RF_RX_G2					0x1B
+
+#define	RF_RX_BB2					0x1C
+#define	RF_RX_BB1					0x1D
+
+#define	RF_RCK1						0x1E
+#define	RF_RCK2						0x1F
+
+#define	RF_TX_G1					0x20
+#define	RF_TX_G2					0x21
+#define	RF_TX_G3					0x22
+
+#define	RF_TX_BB1					0x23
+#define	RF_T_METER					0x24
+
+#define	RF_SYN_G1					0x25
+#define	RF_SYN_G2					0x26
+#define	RF_SYN_G3					0x27
+#define	RF_SYN_G4					0x28
+#define	RF_SYN_G5					0x29
+#define	RF_SYN_G6					0x2A
+#define	RF_SYN_G7					0x2B
+#define	RF_SYN_G8					0x2C
+
+#define	RF_RCK_OS					0x30
+#define	RF_TXPA_G1					0x31
+#define	RF_TXPA_G2					0x32
+#define	RF_TXPA_G3					0x33
+
+#define	BBBRESETB					0x100
 #define	BGLOBALRESETB				0x200
 #define	BOFDMTXSTART				0x4
-#define	BCCKTXSTART				0x8
-#define	BCRC32DEBUG				0x100
+#define	BCCKTXSTART					0x8
+#define	BCRC32DEBUG					0x100
 #define	BPMACLOOPBACK				0x10
-#define	BTXLSIG					0xffffff
-#define	BOFDMTXRATE				0xf
+#define	BTXLSIG						0xffffff
+#define	BOFDMTXRATE					0xf
 #define	BOFDMTXRESERVED				0x10
 #define	BOFDMTXLENGTH				0x1ffe0
 #define	BOFDMTXPARITY				0x20000
-#define	BTXHTSIG1				0xffffff
+#define	BTXHTSIG1					0xffffff
 #define	BTXHTMCSRATE				0x7f
-#define	BTXHTBW					0x80
-#define	BTXHTLENGTH				0xffff00
-#define	BTXHTSIG2				0xffffff
+#define	BTXHTBW						0x80
+#define	BTXHTLENGTH					0xffff00
+#define	BTXHTSIG2					0xffffff
 #define	BTXHTSMOOTHING				0x1
 #define	BTXHTSOUNDING				0x2
 #define	BTXHTRESERVED				0x4
 #define	BTXHTAGGREATION				0x8
-#define	BTXHTSTBC				0x30
+#define	BTXHTSTBC					0x30
 #define	BTXHTADVANCECODING			0x40
 #define	BTXHTSHORTGI				0x80
 #define	BTXHTNUMBERHT_LTF			0x300
-#define	BTXHTCRC8				0x3fc00
+#define	BTXHTCRC8					0x3fc00
 #define	BCOUNTERRESET				0x10000
 #define	BNUMOFOFDMTX				0xffff
-#define	BNUMOFCCKTX				0xffff0000
+#define	BNUMOFCCKTX					0xffff0000
 #define	BTXIDLEINTERVAL				0xffff
 #define	BOFDMSERVICE				0xffff0000
 #define	BTXMACHEADER				0xffffffff
-#define	BTXDATAINIT				0xff
-#define	BTXHTMODE				0x100
-#define	BTXDATATYPE				0x30000
+#define	BTXDATAINIT					0xff
+#define	BTXHTMODE					0x100
+#define	BTXDATATYPE					0x30000
 #define	BTXRANDOMSEED				0xffffffff
 #define	BCCKTXPREAMBLE				0x1
-#define	BCCKTXSFD				0xffff0000
-#define	BCCKTXSIG				0xff
+#define	BCCKTXSFD					0xffff0000
+#define	BCCKTXSIG					0xff
 #define	BCCKTXSERVICE				0xff00
 #define	BCCKLENGTHEXT				0x8000
 #define	BCCKTXLENGHT				0xffff0000
-#define	BCCKTXCRC16				0xffff
+#define	BCCKTXCRC16					0xffff
 #define	BCCKTXSTATUS				0x1
 #define	BOFDMTXSTATUS				0x2
-#define IS_BB_REG_OFFSET_92S(_Offset)	\
-	((_Offset >= 0x800) && (_Offset <= 0xfff))
-
-#define	BRFMOD					0x1
-#define	BJAPANMODE				0x2
-#define	BCCKTXSC				0x30
-#define	BCCKEN					0x1000000
-#define	BOFDMEN					0x2000000
-
-#define	BOFDMRXADCPHASE				0x10000
-#define	BOFDMTXDACPHASE				0x40000
-#define	BXATXAGC				0x3f
-
-#define	BXBTXAGC				0xf00
-#define	BXCTXAGC				0xf000
-#define	BXDTXAGC				0xf0000
-
-#define	BPASTART				0xf0000000
-#define	BTRSTART				0x00f00000
-#define	BRFSTART				0x0000f000
-#define	BBBSTART				0x000000f0
-#define	BBBCCKSTART				0x0000000f
-#define	BPAEND					0xf
-#define	BTREND					0x0f000000
-#define	BRFEND					0x000f0000
-#define	BCCAMASK				0x000000f0
-#define	BR2RCCAMASK				0x00000f00
-#define	BHSSI_R2TDELAY				0xf8000000
-#define	BHSSI_T2RDELAY				0xf80000
-#define	BCONTXHSSI				0x400
-#define	BIGFROMCCK				0x200
-#define	BAGCADDRESS				0x3f
-#define	BRXHPTX					0x7000
-#define	BRXHP2RX				0x38000
-#define	BRXHPCCKINI				0xc0000
-#define	BAGCTXCODE				0xc00000
-#define	BAGCRXCODE				0x300000
-
-#define	B3WIREDATALENGTH			0x800
-#define	B3WIREADDREAALENGTH			0x400
-
-#define	B3WIRERFPOWERDOWN			0x1
-#define	B5GPAPEPOLARITY				0x40000000
-#define	B2GPAPEPOLARITY				0x80000000
-#define	BRFSW_TXDEFAULTANT			0x3
-#define	BRFSW_TXOPTIONANT			0x30
-#define	BRFSW_RXDEFAULTANT			0x300
-#define	BRFSW_RXOPTIONANT			0x3000
-#define	BRFSI_3WIREDATA				0x1
-#define	BRFSI_3WIRECLOCK			0x2
-#define	BRFSI_3WIRELOAD				0x4
-#define	BRFSI_3WIRERW				0x8
-#define	BRFSI_3WIRE				0xf
-
-#define	BRFSI_RFENV				0x10
-
-#define	BRFSI_TRSW				0x20
-#define	BRFSI_TRSWB				0x40
-#define	BRFSI_ANTSW				0x100
-#define	BRFSI_ANTSWB				0x200
-#define	BRFSI_PAPE				0x400
-#define	BRFSI_PAPE5G				0x800
-#define	BBANDSELECT				0x1
-#define	BHTSIG2_GI				0x80
-#define	BHTSIG2_SMOOTHING			0x01
-#define	BHTSIG2_SOUNDING			0x02
-#define	BHTSIG2_AGGREATON			0x08
-#define	BHTSIG2_STBC				0x30
-#define	BHTSIG2_ADVCODING			0x40
-#define	BHTSIG2_NUMOFHTLTF			0x300
-#define	BHTSIG2_CRC8				0x3fc
-#define	BHTSIG1_MCS				0x7f
-#define	BHTSIG1_BANDWIDTH			0x80
-#define	BHTSIG1_HTLENGTH			0xffff
-#define	BLSIG_RATE				0xf
-#define	BLSIG_RESERVED				0x10
-#define	BLSIG_LENGTH				0x1fffe
-#define	BLSIG_PARITY				0x20
-#define	BCCKRXPHASE				0x4
-
-#define	BLSSIREADADDRESS			0x7f800000
-#define	BLSSIREADEDGE				0x80000000
-
-#define	BLSSIREADBACKDATA			0xfffff
-
-#define	BLSSIREADOKFLAG				0x1000
-#define	BCCKSAMPLERATE				0x8
-#define	BREGULATOR0STANDBY			0x1
-#define	BREGULATORPLLSTANDBY			0x2
-#define	BREGULATOR1STANDBY			0x4
-#define	BPLLPOWERUP				0x8
-#define	BDPLLPOWERUP				0x10
-#define	BDA10POWERUP				0x20
-#define	BAD7POWERUP				0x200
-#define	BDA6POWERUP				0x2000
-#define	BXTALPOWERUP				0x4000
-#define	B40MDCLKPOWERUP				0x8000
-#define	BDA6DEBUGMODE				0x20000
-#define	BDA6SWING				0x380000
-
-#define	BADCLKPHASE				0x4000000
-#define	B80MCLKDELAY				0x18000000
-#define	BAFEWATCHDOGENABLE			0x20000000
-
-#define	BXTALCAP01				0xc0000000
-#define	BXTALCAP23				0x3
-#define	BXTALCAP92X				0x0f000000
-#define BXTALCAP				0x0f000000
-
-#define	BINTDIFCLKENABLE			0x400
-#define	BEXTSIGCLKENABLE			0x800
-#define	BBANDGAP_MBIAS_POWERUP			0x10000
-#define	BAD11SH_GAIN				0xc0000
-#define	BAD11NPUT_RANGE				0x700000
-#define	BAD110P_CURRENT				0x3800000
-#define	BLPATH_LOOPBACK				0x4000000
-#define	BQPATH_LOOPBACK				0x8000000
-#define	BAFE_LOOPBACK				0x10000000
-#define	BDA10_SWING				0x7e0
-#define	BDA10_REVERSE				0x800
-#define	BDA_CLK_SOURCE				0x1000
-#define	BDA7INPUT_RANGE				0x6000
-#define	BDA7_GAIN				0x38000
-#define	BDA7OUTPUT_CM_MODE			0x40000
-#define	BDA7INPUT_CM_MODE			0x380000
-#define	BDA7CURRENT				0xc00000
-#define	BREGULATOR_ADJUST			0x7000000
-#define	BAD11POWERUP_ATTX			0x1
-#define	BDA10PS_ATTX				0x10
-#define	BAD11POWERUP_ATRX			0x100
-#define	BDA10PS_ATRX				0x1000
-#define	BCCKRX_AGC_FORMAT			0x200
-#define	BPSDFFT_SAMPLE_POINT			0xc000
-#define	BPSD_AVERAGE_NUM			0x3000
-#define	BIQPATH_CONTROL				0xc00
-#define	BPSD_FREQ				0x3ff
-#define	BPSD_ANTENNA_PATH			0x30
-#define	BPSD_IQ_SWITCH				0x40
-#define	BPSD_RX_TRIGGER				0x400000
-#define	BPSD_TX_TRIGGER				0x80000000
-#define	BPSD_SINE_TONE_SCALE			0x7f000000
-#define	BPSD_REPORT				0xffff
-
-#define	BOFDM_TXSC				0x30000000
-#define	BCCK_TXON				0x1
-#define	BOFDM_TXON				0x2
-#define	BDEBUG_PAGE				0xfff
-#define	BDEBUG_ITEM				0xff
-#define	BANTL					0x10
-#define	BANT_NONHT				0x100
-#define	BANT_HT1				0x1000
-#define	BANT_HT2				0x10000
-#define	BANT_HT1S1				0x100000
-#define	BANT_NONHTS1				0x1000000
-
-#define	BCCK_BBMODE				0x3
-#define	BCCK_TXPOWERSAVING			0x80
-#define	BCCK_RXPOWERSAVING			0x40
-
-#define	BCCK_SIDEBAND				0x10
-
-#define	BCCK_SCRAMBLE				0x8
-#define	BCCK_ANTDIVERSITY			0x8000
-#define	BCCK_CARRIER_RECOVERY			0x4000
-#define	BCCK_TXRATE				0x3000
-#define	BCCK_DCCANCEL				0x0800
-#define	BCCK_ISICANCEL				0x0400
-#define	BCCK_MATCH_FILTER			0x0200
-#define	BCCK_EQUALIZER				0x0100
-#define	BCCK_PREAMBLE_DETECT			0x800000
-#define	BCCK_FAST_FALSECCAi			0x400000
-#define	BCCK_CH_ESTSTARTi			0x300000
-#define	BCCK_CCA_COUNTi				0x080000
-#define	BCCK_CS_LIM				0x070000
-#define	BCCK_BIST_MODEi				0x80000000
-#define	BCCK_CCAMASK				0x40000000
-#define	BCCK_TX_DAC_PHASE			0x4
-#define	BCCK_RX_ADC_PHASE			0x20000000
-#define	BCCKR_CP_MODE				0x0100
-#define	BCCK_TXDC_OFFSET			0xf0
-#define	BCCK_RXDC_OFFSET			0xf
-#define	BCCK_CCA_MODE				0xc000
-#define	BCCK_FALSECS_LIM			0x3f00
-#define	BCCK_CS_RATIO				0xc00000
-#define	BCCK_CORGBIT_SEL			0x300000
-#define	BCCK_PD_LIM				0x0f0000
-#define	BCCK_NEWCCA				0x80000000
-#define	BCCK_RXHP_OF_IG				0x8000
-#define	BCCK_RXIG				0x7f00
-#define	BCCK_LNA_POLARITY			0x800000
-#define	BCCK_RX1ST_BAIN				0x7f0000
-#define	BCCK_RF_EXTEND				0x20000000
-#define	BCCK_RXAGC_SATLEVEL			0x1f000000
-#define	BCCK_RXAGC_SATCOUNT			0xe0
-#define	bCCKRxRFSettle				0x1f
-#define	BCCK_FIXED_RXAGC			0x8000
-#define	BCCK_ANTENNA_POLARITY			0x2000
-#define	BCCK_TXFILTER_TYPE			0x0c00
-#define	BCCK_RXAGC_REPORTTYPE			0x0300
-#define	BCCK_RXDAGC_EN				0x80000000
-#define	BCCK_RXDAGC_PERIOD			0x20000000
-#define	BCCK_RXDAGC_SATLEVEL			0x1f000000
-#define	BCCK_TIMING_RECOVERY			0x800000
-#define	BCCK_TXC0				0x3f0000
-#define	BCCK_TXC1				0x3f000000
-#define	BCCK_TXC2				0x3f
-#define	BCCK_TXC3				0x3f00
-#define	BCCK_TXC4				0x3f0000
-#define	BCCK_TXC5				0x3f000000
-#define	BCCK_TXC6				0x3f
-#define	BCCK_TXC7				0x3f00
-#define	BCCK_DEBUGPORT				0xff0000
-#define	BCCK_DAC_DEBUG				0x0f000000
-#define	BCCK_FALSEALARM_ENABLE			0x8000
-#define	BCCK_FALSEALARM_READ			0x4000
-#define	BCCK_TRSSI				0x7f
-#define	BCCK_RXAGC_REPORT			0xfe
-#define	BCCK_RXREPORT_ANTSEL			0x80000000
-#define	BCCK_RXREPORT_MFOFF			0x40000000
-#define	BCCK_RXREPORT_SQLOSS			0x20000000
-#define	BCCK_RXREPORT_PKTLOSS			0x10000000
-#define	BCCK_RXREPORT_LOCKEDBIT			0x08000000
-#define	BCCK_RXREPORT_RATEERROR			0x04000000
-#define	BCCK_RXREPORT_RXRATE			0x03000000
-#define	BCCK_RXFA_COUNTER_LOWER			0xff
-#define	BCCK_RXFA_COUNTER_UPPER			0xff000000
-#define	BCCK_RXHPAGC_START			0xe000
-#define	BCCK_RXHPAGC_FINAL			0x1c00
-#define	BCCK_RXFALSEALARM_ENABLE		0x8000
-#define	BCCK_FACOUNTER_FREEZE			0x4000
-#define	BCCK_TXPATH_SEL				0x10000000
-#define	BCCK_DEFAULT_RXPATH			0xc000000
-#define	BCCK_OPTION_RXPATH			0x3000000
-
-#define	BNUM_OFSTF				0x3
-#define	BSHIFT_L				0xc0
-#define	BGI_TH					0xc
-#define	BRXPATH_A				0x1
-#define	BRXPATH_B				0x2
-#define	BRXPATH_C				0x4
-#define	BRXPATH_D				0x8
-#define	BTXPATH_A				0x1
-#define	BTXPATH_B				0x2
-#define	BTXPATH_C				0x4
-#define	BTXPATH_D				0x8
-#define	BTRSSI_FREQ				0x200
-#define	BADC_BACKOFF				0x3000
-#define	BDFIR_BACKOFF				0xc000
-#define	BTRSSI_LATCH_PHASE			0x10000
-#define	BRX_LDC_OFFSET				0xff
-#define	BRX_QDC_OFFSET				0xff00
-#define	BRX_DFIR_MODE				0x1800000
-#define	BRX_DCNF_TYPE				0xe000000
-#define	BRXIQIMB_A				0x3ff
-#define	BRXIQIMB_B				0xfc00
-#define	BRXIQIMB_C				0x3f0000
-#define	BRXIQIMB_D				0xffc00000
-#define	BDC_DC_NOTCH				0x60000
-#define	BRXNB_NOTCH				0x1f000000
-#define	BPD_TH					0xf
-#define	BPD_TH_OPT2				0xc000
-#define	BPWED_TH				0x700
-#define	BIFMF_WIN_L				0x800
-#define	BPD_OPTION				0x1000
-#define	BMF_WIN_L				0xe000
-#define	BBW_SEARCH_L				0x30000
-#define	BWIN_ENH_L				0xc0000
-#define	BBW_TH					0x700000
-#define	BED_TH2					0x3800000
-#define	BBW_OPTION				0x4000000
-#define	BRADIO_TH				0x18000000
-#define	BWINDOW_L				0xe0000000
-#define	BSBD_OPTION				0x1
-#define	BFRAME_TH				0x1c
-#define	BFS_OPTION				0x60
-#define	BDC_SLOPE_CHECK				0x80
-#define	BFGUARD_COUNTER_DC_L			0xe00
-#define	BFRAME_WEIGHT_SHORT			0x7000
-#define	BSUB_TUNE				0xe00000
-#define	BFRAME_DC_LENGTH			0xe000000
-#define	BSBD_START_OFFSET			0x30000000
-#define	BFRAME_TH_2				0x7
-#define	BFRAME_GI2_TH				0x38
-#define	BGI2_SYNC_EN				0x40
-#define	BSARCH_SHORT_EARLY			0x300
-#define	BSARCH_SHORT_LATE			0xc00
-#define	BSARCH_GI2_LATE				0x70000
-#define	BCFOANTSUM				0x1
-#define	BCFOACC					0x2
-#define	BCFOSTARTOFFSET				0xc
-#define	BCFOLOOPBACK				0x70
-#define	BCFOSUMWEIGHT				0x80
-#define	BDAGCENABLE				0x10000
-#define	BTXIQIMB_A				0x3ff
-#define	BTXIQIMB_b				0xfc00
-#define	BTXIQIMB_C				0x3f0000
-#define	BTXIQIMB_D				0xffc00000
-#define	BTXIDCOFFSET				0xff
-#define	BTXIQDCOFFSET				0xff00
-#define	BTXDFIRMODE				0x10000
-#define	BTXPESUDO_NOISEON			0x4000000
-#define	BTXPESUDO_NOISE_A			0xff
-#define	BTXPESUDO_NOISE_B			0xff00
-#define	BTXPESUDO_NOISE_C			0xff0000
-#define	BTXPESUDO_NOISE_D			0xff000000
-#define	BCCA_DROPOPTION				0x20000
-#define	BCCA_DROPTHRES				0xfff00000
-#define	BEDCCA_H				0xf
-#define	BEDCCA_L				0xf0
-#define	BLAMBDA_ED				0x300
-#define	BRX_INITIALGAIN				0x7f
-#define	BRX_ANTDIV_EN				0x80
-#define	BRX_AGC_ADDRESS_FOR_LNA			0x7f00
-#define	BRX_HIGHPOWER_FLOW			0x8000
-#define	BRX_AGC_FREEZE_THRES			0xc0000
-#define	BRX_FREEZESTEP_AGC1			0x300000
-#define	BRX_FREEZESTEP_AGC2			0xc00000
-#define	BRX_FREEZESTEP_AGC3			0x3000000
-#define	BRX_FREEZESTEP_AGC0			0xc000000
-#define	BRXRSSI_CMP_EN				0x10000000
-#define	BRXQUICK_AGCEN				0x20000000
-#define	BRXAGC_FREEZE_THRES_MODE		0x40000000
-#define	BRX_OVERFLOW_CHECKTYPE			0x80000000
-#define	BRX_AGCSHIFT				0x7f
-#define	BTRSW_TRI_ONLY				0x80
-#define	BPOWER_THRES				0x300
-#define	BRXAGC_EN				0x1
-#define	BRXAGC_TOGETHER_EN			0x2
-#define	BRXAGC_MIN				0x4
-#define	BRXHP_INI				0x7
-#define	BRXHP_TRLNA				0x70
-#define	BRXHP_RSSI				0x700
-#define	BRXHP_BBP1				0x7000
-#define	BRXHP_BBP2				0x70000
-#define	BRXHP_BBP3				0x700000
-#define	BRSSI_H					0x7f0000
-#define	BRSSI_GEN				0x7f000000
-#define	BRXSETTLE_TRSW				0x7
-#define	BRXSETTLE_LNA				0x38
-#define	BRXSETTLE_RSSI				0x1c0
-#define	BRXSETTLE_BBP				0xe00
-#define	BRXSETTLE_RXHP				0x7000
-#define	BRXSETTLE_ANTSW_RSSI			0x38000
-#define	BRXSETTLE_ANTSW				0xc0000
-#define	BRXPROCESS_TIME_DAGC			0x300000
-#define	BRXSETTLE_HSSI				0x400000
-#define	BRXPROCESS_TIME_BBPPW			0x800000
-#define	BRXANTENNA_POWER_SHIFT			0x3000000
-#define	BRSSI_TABLE_SELECT			0xc000000
-#define	BRXHP_FINAL				0x7000000
-#define	BRXHPSETTLE_BBP				0x7
-#define	BRXHTSETTLE_HSSI			0x8
-#define	BRXHTSETTLE_RXHP			0x70
-#define	BRXHTSETTLE_BBPPW			0x80
-#define	BRXHTSETTLE_IDLE			0x300
-#define	BRXHTSETTLE_RESERVED			0x1c00
-#define	BRXHT_RXHP_EN				0x8000
-#define	BRXAGC_FREEZE_THRES			0x30000
-#define	BRXAGC_TOGETHEREN			0x40000
-#define	BRXHTAGC_MIN				0x80000
-#define	BRXHTAGC_EN				0x100000
-#define	BRXHTDAGC_EN				0x200000
-#define	BRXHT_RXHP_BBP				0x1c00000
-#define	BRXHT_RXHP_FINAL			0xe0000000
-#define	BRXPW_RADIO_TH				0x3
-#define	BRXPW_RADIO_EN				0x4
-#define	BRXMF_HOLD				0x3800
-#define	BRXPD_DELAY_TH1				0x38
-#define	BRXPD_DELAY_TH2				0x1c0
-#define	BRXPD_DC_COUNT_MAX			0x600
-#define	BRXPD_DELAY_TH				0x8000
-#define	BRXPROCESS_DELAY			0xf0000
-#define	BRXSEARCHRANGE_GI2_EARLY		0x700000
-#define	BRXFRAME_FUARD_COUNTER_L		0x3800000
-#define	BRXSGI_GUARD_L				0xc000000
-#define	BRXSGI_SEARCH_L				0x30000000
-#define	BRXSGI_TH				0xc0000000
-#define	BDFSCNT0				0xff
-#define	BDFSCNT1				0xff00
-#define	BDFSFLAG				0xf0000
-#define	BMF_WEIGHT_SUM				0x300000
-#define	BMINIDX_TH				0x7f000000
-#define	BDAFORMAT				0x40000
-#define	BTXCH_EMU_ENABLE			0x01000000
-#define	BTRSW_ISOLATION_A			0x7f
-#define	BTRSW_ISOLATION_B			0x7f00
-#define	BTRSW_ISOLATION_C			0x7f0000
-#define	BTRSW_ISOLATION_D			0x7f000000
-#define	BEXT_LNA_GAIN				0x7c00
-
-#define	BSTBC_EN				0x4
-#define	BANTENNA_MAPPING			0x10
-#define	BNSS					0x20
-#define	BCFO_ANTSUM_ID				0x200
-#define	BPHY_COUNTER_RESET			0x8000000
-#define	BCFO_REPORT_GET				0x4000000
-#define	BOFDM_CONTINUE_TX			0x10000000
-#define	BOFDM_SINGLE_CARRIER			0x20000000
-#define	BOFDM_SINGLE_TONE			0x40000000
-#define	BHT_DETECT				0x100
-#define	BCFOEN					0x10000
-#define	BCFOVALUE				0xfff00000
-#define	BSIGTONE_RE				0x3f
-#define	BSIGTONE_IM				0x7f00
-#define	BCOUNTER_CCA				0xffff
-#define	BCOUNTER_PARITYFAIL			0xffff0000
-#define	BCOUNTER_RATEILLEGAL			0xffff
-#define	BCOUNTER_CRC8FAIL			0xffff0000
-#define	BCOUNTER_MCSNOSUPPORT			0xffff
-#define	BCOUNTER_FASTSYNC			0xffff
-#define	BSHORTCFO				0xfff
-#define	BSHORTCFOT_LENGTH			12
-#define	BSHORTCFOF_LENGTH			11
-#define	BLONGCFO				0x7ff
-#define	BLONGCFOT_LENGTH			11
-#define	BLONGCFOF_LENGTH			11
-#define	BTAILCFO				0x1fff
-#define	BTAILCFOT_LENGTH			13
-#define	BTAILCFOF_LENGTH			12
-#define	BNOISE_EN_PWDB				0xffff
-#define	BCC_POWER_DB				0xffff0000
-#define	BMOISE_PWDB				0xffff
-#define	BPOWERMEAST_LENGTH			10
-#define	BPOWERMEASF_LENGTH			3
-#define	BRX_HT_BW				0x1
-#define	BRXSC					0x6
-#define	BRX_HT					0x8
-#define	BNB_INTF_DET_ON				0x1
-#define	BINTF_WIN_LEN_CFG			0x30
-#define	BNB_INTF_TH_CFG				0x1c0
-#define	BRFGAIN					0x3f
-#define	BTABLESEL				0x40
-#define	BTRSW					0x80
-#define	BRXSNR_A				0xff
-#define	BRXSNR_B				0xff00
-#define	BRXSNR_C				0xff0000
-#define	BRXSNR_D				0xff000000
-#define	BSNR_EVMT_LENGTH			8
-#define	BSNR_EVMF_LENGTH			1
-#define	BCSI1ST					0xff
-#define	BCSI2ND					0xff00
-#define	BRXEVM1ST				0xff0000
-#define	BRXEVM2ND				0xff000000
-#define	BSIGEVM					0xff
-#define	BPWDB					0xff00
-#define	BSGIEN					0x10000
-
-#define	BSFACTOR_QMA1				0xf
-#define	BSFACTOR_QMA2				0xf0
-#define	BSFACTOR_QMA3				0xf00
-#define	BSFACTOR_QMA4				0xf000
-#define	BSFACTOR_QMA5				0xf0000
-#define	BSFACTOR_QMA6				0xf0000
-#define	BSFACTOR_QMA7				0xf00000
-#define	BSFACTOR_QMA8				0xf000000
-#define	BSFACTOR_QMA9				0xf0000000
-#define	BCSI_SCHEME				0x100000
-
-#define	BNOISE_LVL_TOP_SET			0x3
-#define	BCHSMOOTH				0x4
-#define	BCHSMOOTH_CFG1				0x38
-#define	BCHSMOOTH_CFG2				0x1c0
-#define	BCHSMOOTH_CFG3				0xe00
-#define	BCHSMOOTH_CFG4				0x7000
-#define	BMRCMODE				0x800000
-#define	BTHEVMCFG				0x7000000
-
-#define	BLOOP_FIT_TYPE				0x1
-#define	BUPD_CFO				0x40
-#define	BUPD_CFO_OFFDATA			0x80
-#define	BADV_UPD_CFO				0x100
-#define	BADV_TIME_CTRL				0x800
-#define	BUPD_CLKO				0x1000
-#define	BFC					0x6000
-#define	BTRACKING_MODE				0x8000
-#define	BPHCMP_ENABLE				0x10000
-#define	BUPD_CLKO_LTF				0x20000
-#define	BCOM_CH_CFO				0x40000
-#define	BCSI_ESTI_MODE				0x80000
-#define	BADV_UPD_EQZ				0x100000
-#define	BUCHCFG					0x7000000
-#define	BUPDEQZ					0x8000000
-
-#define	BRX_PESUDO_NOISE_ON			0x20000000
-#define	BRX_PESUDO_NOISE_A			0xff
-#define	BRX_PESUDO_NOISE_B			0xff00
-#define	BRX_PESUDO_NOISE_C			0xff0000
-#define	BRX_PESUDO_NOISE_D			0xff000000
-#define	BRX_PESUDO_NOISESTATE_A			0xffff
-#define	BRX_PESUDO_NOISESTATE_B			0xffff0000
-#define	BRX_PESUDO_NOISESTATE_C			0xffff
-#define	BRX_PESUDO_NOISESTATE_D			0xffff0000
-
-#define	BZEBRA1_HSSIENABLE			0x8
-#define	BZEBRA1_TRXCONTROL			0xc00
-#define	BZEBRA1_TRXGAINSETTING			0x07f
-#define	BZEBRA1_RXCOUNTER			0xc00
-#define	BZEBRA1_TXCHANGEPUMP			0x38
-#define	BZEBRA1_RXCHANGEPUMP			0x7
-#define	BZEBRA1_CHANNEL_NUM			0xf80
-#define	BZEBRA1_TXLPFBW				0x400
-#define	BZEBRA1_RXLPFBW				0x600
-
-#define	BRTL8256REG_MODE_CTRL1			0x100
-#define	BRTL8256REG_MODE_CTRL0			0x40
-#define	BRTL8256REG_TXLPFBW			0x18
-#define	BRTL8256REG_RXLPFBW			0x600
-
-#define	BRTL8258_TXLPFBW			0xc
-#define	BRTL8258_RXLPFBW			0xc00
-#define	BRTL8258_RSSILPFBW			0xc0
-
-#define	BBYTE0					0x1
-#define	BBYTE1					0x2
-#define	BBYTE2					0x4
-#define	BBYTE3					0x8
-#define	BWORD0					0x3
-#define	BWORD1					0xc
-#define	BWORD					0xf
-
-#define	BENABLE					0x1
-#define	BDISABLE				0x0
-
-#define	LEFT_ANTENNA				0x0
-#define	RIGHT_ANTENNA				0x1
-
-#define	TCHECK_TXSTATUS				500
-#define	TUPDATE_RXCOUNTER			100
+#define IS_BB_REG_OFFSET_92S(_offset)	\
+	((_offset >= 0x800) && (_offset <= 0xfff))
+
+#define	BRFMOD						0x1
+#define	BJAPANMODE					0x2
+#define	BCCKTXSC					0x30
+#define	BCCKEN						0x1000000
+#define	BOFDMEN						0x2000000
+
+#define	BOFDMRXADCPHASE			0x10000
+#define	BOFDMTXDACPHASE			0x40000
+#define	BXATXAGC			0x3f
+
+#define	BXBTXAGC			0xf00
+#define	BXCTXAGC			0xf000
+#define	BXDTXAGC			0xf0000
+
+#define	BPASTART			0xf0000000
+#define	BTRSTART			0x00f00000
+#define	BRFSTART			0x0000f000
+#define	BBBSTART			0x000000f0
+#define	BBBCCKSTART			0x0000000f
+#define	BPAEND				0xf
+#define	BTREND				0x0f000000
+#define	BRFEND				0x000f0000
+#define	BCCAMASK			0x000000f0
+#define	BR2RCCAMASK			0x00000f00
+#define	BHSSI_R2TDELAY			0xf8000000
+#define	BHSSI_T2RDELAY			0xf80000
+#define	BCONTXHSSI			0x400
+#define	BIGFROMCCK			0x200
+#define	BAGCADDRESS			0x3f
+#define	BRXHPTX				0x7000
+#define	BRXHP2RX			0x38000
+#define	BRXHPCCKINI			0xc0000
+#define	BAGCTXCODE			0xc00000
+#define	BAGCRXCODE			0x300000
+
+#define	B3WIREDATALENGTH		0x800
+#define	B3WIREADDREAALENGTH		0x400
+
+#define	B3WIRERFPOWERDOWN		0x1
+#define	B5GPAPEPOLARITY			0x40000000
+#define	B2GPAPEPOLARITY			0x80000000
+#define	BRFSW_TXDEFAULTANT		0x3
+#define	BRFSW_TXOPTIONANT		0x30
+#define	BRFSW_RXDEFAULTANT		0x300
+#define	BRFSW_RXOPTIONANT		0x3000
+#define	BRFSI_3WIREDATA			0x1
+#define	BRFSI_3WIRECLOCK		0x2
+#define	BRFSI_3WIRELOAD			0x4
+#define	BRFSI_3WIRERW			0x8
+#define	BRFSI_3WIRE			0xf
+
+#define	BRFSI_RFENV			0x10
+
+#define	BRFSI_TRSW			0x20
+#define	BRFSI_TRSWB			0x40
+#define	BRFSI_ANTSW			0x100
+#define	BRFSI_ANTSWB			0x200
+#define	BRFSI_PAPE			0x400
+#define	BRFSI_PAPE5G			0x800
+#define	BBANDSELECT			0x1
+#define	BHTSIG2_GI			0x80
+#define	BHTSIG2_SMOOTHING		0x01
+#define	BHTSIG2_SOUNDING		0x02
+#define	BHTSIG2_AGGREATON		0x08
+#define	BHTSIG2_STBC			0x30
+#define	BHTSIG2_ADVCODING		0x40
+#define	BHTSIG2_NUMOFHTLTF		0x300
+#define	BHTSIG2_CRC8			0x3fc
+#define	BHTSIG1_MCS			0x7f
+#define	BHTSIG1_BANDWIDTH		0x80
+#define	BHTSIG1_HTLENGTH		0xffff
+#define	BLSIG_RATE			0xf
+#define	BLSIG_RESERVED			0x10
+#define	BLSIG_LENGTH			0x1fffe
+#define	BLSIG_PARITY			0x20
+#define	BCCKRXPHASE			0x4
+
+#define	BLSSIREADADDRESS		0x7f800000
+#define	BLSSIREADEDGE			0x80000000
+
+#define	BLSSIREADBACKDATA		0xfffff
+
+#define	BLSSIREADOKFLAG			0x1000
+#define	BCCKSAMPLERATE			0x8
+#define	BREGULATOR0STANDBY		0x1
+#define	BREGULATORPLLSTANDBY		0x2
+#define	BREGULATOR1STANDBY		0x4
+#define	BPLLPOWERUP			0x8
+#define	BDPLLPOWERUP			0x10
+#define	BDA10POWERUP			0x20
+#define	BAD7POWERUP			0x200
+#define	BDA6POWERUP			0x2000
+#define	BXTALPOWERUP			0x4000
+#define	B40MDCLKPOWERUP			0x8000
+#define	BDA6DEBUGMODE			0x20000
+#define	BDA6SWING			0x380000
+
+#define	BADCLKPHASE			0x4000000
+#define	B80MCLKDELAY			0x18000000
+#define	BAFEWATCHDOGENABLE		0x20000000
+
+#define	BXTALCAP01			0xc0000000
+#define	BXTALCAP23			0x3
+#define	BXTALCAP92X					0x0f000000
+#define BXTALCAP			0x0f000000
+
+#define	BINTDIFCLKENABLE		0x400
+#define	BEXTSIGCLKENABLE		0x800
+#define	BBANDGAP_MBIAS_POWERUP      0x10000
+#define	BAD11SH_GAIN			0xc0000
+#define	BAD11NPUT_RANGE			0x700000
+#define	BAD110P_CURRENT			0x3800000
+#define	BLPATH_LOOPBACK			0x4000000
+#define	BQPATH_LOOPBACK			0x8000000
+#define	BAFE_LOOPBACK			0x10000000
+#define	BDA10_SWING			0x7e0
+#define	BDA10_REVERSE			0x800
+#define	BDA_CLK_SOURCE              0x1000
+#define	BDA7INPUT_RANGE			0x6000
+#define	BDA7_GAIN			0x38000
+#define	BDA7OUTPUT_CM_MODE          0x40000
+#define	BDA7INPUT_CM_MODE           0x380000
+#define	BDA7CURRENT			0xc00000
+#define	BREGULATOR_ADJUST		0x7000000
+#define	BAD11POWERUP_ATTX		0x1
+#define	BDA10PS_ATTX			0x10
+#define	BAD11POWERUP_ATRX		0x100
+#define	BDA10PS_ATRX			0x1000
+#define	BCCKRX_AGC_FORMAT           0x200
+#define	BPSDFFT_SAMPLE_POINT		0xc000
+#define	BPSD_AVERAGE_NUM            0x3000
+#define	BIQPATH_CONTROL			0xc00
+#define	BPSD_FREQ			0x3ff
+#define	BPSD_ANTENNA_PATH           0x30
+#define	BPSD_IQ_SWITCH              0x40
+#define	BPSD_RX_TRIGGER             0x400000
+#define	BPSD_TX_TRIGGER             0x80000000
+#define	BPSD_SINE_TONE_SCALE        0x7f000000
+#define	BPSD_REPORT			0xffff
+
+#define	BOFDM_TXSC			0x30000000
+#define	BCCK_TXON			0x1
+#define	BOFDM_TXON			0x2
+#define	BDEBUG_PAGE			0xfff
+#define	BDEBUG_ITEM			0xff
+#define	BANTL				0x10
+#define	BANT_NONHT		    0x100
+#define	BANT_HT1			0x1000
+#define	BANT_HT2			0x10000
+#define	BANT_HT1S1			0x100000
+#define	BANT_NONHTS1			0x1000000
+
+#define	BCCK_BBMODE			0x3
+#define	BCCK_TXPOWERSAVING		0x80
+#define	BCCK_RXPOWERSAVING		0x40
+
+#define	BCCK_SIDEBAND			0x10
+
+#define	BCCK_SCRAMBLE			0x8
+#define	BCCK_ANTDIVERSITY		0x8000
+#define	BCCK_CARRIER_RECOVERY		0x4000
+#define	BCCK_TXRATE			0x3000
+#define	BCCK_DCCANCEL			0x0800
+#define	BCCK_ISICANCEL			0x0400
+#define	BCCK_MATCH_FILTER           0x0200
+#define	BCCK_EQUALIZER			0x0100
+#define	BCCK_PREAMBLE_DETECT		0x800000
+#define	BCCK_FAST_FALSECCA          0x400000
+#define	BCCK_CH_ESTSTART            0x300000
+#define	BCCK_CCA_COUNT              0x080000
+#define	BCCK_CS_LIM			0x070000
+#define	BCCK_BIST_MODE              0x80000000
+#define	BCCK_CCAMASK			0x40000000
+#define	BCCK_TX_DAC_PHASE		0x4
+#define	BCCK_RX_ADC_PHASE		0x20000000
+#define	BCCKR_CP_MODE			0x0100
+#define	BCCK_TXDC_OFFSET		0xf0
+#define	BCCK_RXDC_OFFSET		0xf
+#define	BCCK_CCA_MODE			0xc000
+#define	BCCK_FALSECS_LIM		0x3f00
+#define	BCCK_CS_RATIO			0xc00000
+#define	BCCK_CORGBIT_SEL		0x300000
+#define	BCCK_PD_LIM			0x0f0000
+#define	BCCK_NEWCCA			0x80000000
+#define	BCCK_RXHP_OF_IG             0x8000
+#define	BCCK_RXIG			0x7f00
+#define	BCCK_LNA_POLARITY           0x800000
+#define	BCCK_RX1ST_BAIN             0x7f0000
+#define	BCCK_RF_EXTEND              0x20000000
+#define	BCCK_RXAGC_SATLEVEL		0x1f000000
+#define	BCCK_RXAGC_SATCOUNT		0xe0
+#define	BCCKRXRFSETTLE			0x1f
+#define	BCCK_FIXED_RXAGC		0x8000
+#define	BCCK_ANTENNA_POLARITY		0x2000
+#define	BCCK_TXFILTER_TYPE          0x0c00
+#define	BCCK_RXAGC_REPORTTYPE		0x0300
+#define	BCCK_RXDAGC_EN              0x80000000
+#define	BCCK_RXDAGC_PERIOD		0x20000000
+#define	BCCK_RXDAGC_SATLEVEL		0x1f000000
+#define	BCCK_TIMING_RECOVERY		0x800000
+#define	BCCK_TXC0			0x3f0000
+#define	BCCK_TXC1			0x3f000000
+#define	BCCK_TXC2			0x3f
+#define	BCCK_TXC3			0x3f00
+#define	BCCK_TXC4			0x3f0000
+#define	BCCK_TXC5			0x3f000000
+#define	BCCK_TXC6			0x3f
+#define	BCCK_TXC7			0x3f00
+#define	BCCK_DEBUGPORT			0xff0000
+#define	BCCK_DAC_DEBUG              0x0f000000
+#define	BCCK_FALSEALARM_ENABLE      0x8000
+#define	BCCK_FALSEALARM_READ        0x4000
+#define	BCCK_TRSSI			0x7f
+#define	BCCK_RXAGC_REPORT           0xfe
+#define	BCCK_RXREPORT_ANTSEL		0x80000000
+#define	BCCK_RXREPORT_MFOFF		0x40000000
+#define	BCCK_RXREPORT_SQLOSS		0x20000000
+#define	BCCK_RXREPORT_PKTLOSS		0x10000000
+#define	BCCK_RXREPORT_LOCKEDBIT		0x08000000
+#define	BCCK_RXREPORT_RATEERROR		0x04000000
+#define	BCCK_RXREPORT_RXRATE		0x03000000
+#define	BCCK_RXFA_COUNTER_LOWER     0xff
+#define	BCCK_RXFA_COUNTER_UPPER     0xff000000
+#define	BCCK_RXHPAGC_START          0xe000
+#define	BCCK_RXHPAGC_FINAL          0x1c00
+#define	BCCK_RXFALSEALARM_ENABLE    0x8000
+#define	BCCK_FACOUNTER_FREEZE       0x4000
+#define	BCCK_TXPATH_SEL             0x10000000
+#define	BCCK_DEFAULT_RXPATH         0xc000000
+#define	BCCK_OPTION_RXPATH          0x3000000
+
+#define	BNUM_OFSTF			0x3
+#define	BSHIFT_L			0xc0
+#define	BGI_TH				0xc
+#define	BRXPATH_A			0x1
+#define	BRXPATH_B			0x2
+#define	BRXPATH_C			0x4
+#define	BRXPATH_D			0x8
+#define	BTXPATH_A			0x1
+#define	BTXPATH_B			0x2
+#define	BTXPATH_C			0x4
+#define	BTXPATH_D			0x8
+#define	BTRSSI_FREQ			0x200
+#define	BADC_BACKOFF			0x3000
+#define	BDFIR_BACKOFF			0xc000
+#define	BTRSSI_LATCH_PHASE		0x10000
+#define	BRX_LDC_OFFSET			0xff
+#define	BRX_QDC_OFFSET			0xff00
+#define	BRX_DFIR_MODE			0x1800000
+#define	BRX_DCNF_TYPE			0xe000000
+#define	BRXIQIMB_A			0x3ff
+#define	BRXIQIMB_B			0xfc00
+#define	BRXIQIMB_C			0x3f0000
+#define	BRXIQIMB_D			0xffc00000
+#define	BDC_DC_NOTCH			0x60000
+#define	BRXNB_NOTCH			0x1f000000
+#define	BPD_TH				0xf
+#define	BPD_TH_OPT2			0xc000
+#define	BPWED_TH			0x700
+#define	BIFMF_WIN_L			0x800
+#define	BPD_OPTION			0x1000
+#define	BMF_WIN_L			0xe000
+#define	BBW_SEARCH_L			0x30000
+#define	BWIN_ENH_L			0xc0000
+#define	BBW_TH				0x700000
+#define	BED_TH2				0x3800000
+#define	BBW_OPTION			0x4000000
+#define	BRADIO_TH			0x18000000
+#define	BWINDOW_L			0xe0000000
+#define	BSBD_OPTION			0x1
+#define	BFRAME_TH			0x1c
+#define	BFS_OPTION			0x60
+#define	BDC_SLOPE_CHECK			0x80
+#define	BFGUARD_COUNTER_DC_L		0xe00
+#define	BFRAME_WEIGHT_SHORT		0x7000
+#define	BSUB_TUNE			0xe00000
+#define	BFRAME_DC_LENGTH		0xe000000
+#define	BSBD_START_OFFSET		0x30000000
+#define	BFRAME_TH_2			0x7
+#define	BFRAME_GI2_TH			0x38
+#define	BGI2_SYNC_EN			0x40
+#define	BSARCH_SHORT_EARLY		0x300
+#define	BSARCH_SHORT_LATE		0xc00
+#define	BSARCH_GI2_LATE			0x70000
+#define	BCFOANTSUM			0x1
+#define	BCFOACC				0x2
+#define	BCFOSTARTOFFSET			0xc
+#define	BCFOLOOPBACK			0x70
+#define	BCFOSUMWEIGHT			0x80
+#define	BDAGCENABLE			0x10000
+#define	BTXIQIMB_A			0x3ff
+#define	BTXIQIMB_b			0xfc00
+#define	BTXIQIMB_C			0x3f0000
+#define	BTXIQIMB_D			0xffc00000
+#define	BTXIDCOFFSET			0xff
+#define	BTXIQDCOFFSET			0xff00
+#define	BTXDFIRMODE			0x10000
+#define	BTXPESUDO_NOISEON		0x4000000
+#define	BTXPESUDO_NOISE_A		0xff
+#define	BTXPESUDO_NOISE_B		0xff00
+#define	BTXPESUDO_NOISE_C		0xff0000
+#define	BTXPESUDO_NOISE_D		0xff000000
+#define	BCCA_DROPOPTION			0x20000
+#define	BCCA_DROPTHRES			0xfff00000
+#define	BEDCCA_H			0xf
+#define	BEDCCA_L			0xf0
+#define	BLAMBDA_ED			0x300
+#define	BRX_INITIALGAIN			0x7f
+#define	BRX_ANTDIV_EN			0x80
+#define	BRX_AGC_ADDRESS_FOR_LNA     0x7f00
+#define	BRX_HIGHPOWER_FLOW		0x8000
+#define	BRX_AGC_FREEZE_THRES        0xc0000
+#define	BRX_FREEZESTEP_AGC1		0x300000
+#define	BRX_FREEZESTEP_AGC2		0xc00000
+#define	BRX_FREEZESTEP_AGC3		0x3000000
+#define	BRX_FREEZESTEP_AGC0		0xc000000
+#define	BRXRSSI_CMP_EN			0x10000000
+#define	BRXQUICK_AGCEN			0x20000000
+#define	BRXAGC_FREEZE_THRES_MODE    0x40000000
+#define	BRX_OVERFLOW_CHECKTYPE		0x80000000
+#define	BRX_AGCSHIFT			0x7f
+#define	BTRSW_TRI_ONLY			0x80
+#define	BPOWER_THRES			0x300
+#define	BRXAGC_EN			0x1
+#define	BRXAGC_TOGETHER_EN		0x2
+#define	BRXAGC_MIN			0x4
+#define	BRXHP_INI			0x7
+#define	BRXHP_TRLNA			0x70
+#define	BRXHP_RSSI			0x700
+#define	BRXHP_BBP1			0x7000
+#define	BRXHP_BBP2			0x70000
+#define	BRXHP_BBP3			0x700000
+#define	BRSSI_H				0x7f0000
+#define	BRSSI_GEN			0x7f000000
+#define	BRXSETTLE_TRSW			0x7
+#define	BRXSETTLE_LNA			0x38
+#define	BRXSETTLE_RSSI			0x1c0
+#define	BRXSETTLE_BBP			0xe00
+#define	BRXSETTLE_RXHP			0x7000
+#define	BRXSETTLE_ANTSW_RSSI		0x38000
+#define	BRXSETTLE_ANTSW			0xc0000
+#define	BRXPROCESS_TIME_DAGC		0x300000
+#define	BRXSETTLE_HSSI			0x400000
+#define	BRXPROCESS_TIME_BBPPW		0x800000
+#define	BRXANTENNA_POWER_SHIFT		0x3000000
+#define	BRSSI_TABLE_SELECT		0xc000000
+#define	BRXHP_FINAL			0x7000000
+#define	BRXHPSETTLE_BBP			0x7
+#define	BRXHTSETTLE_HSSI		0x8
+#define	BRXHTSETTLE_RXHP		0x70
+#define	BRXHTSETTLE_BBPPW		0x80
+#define	BRXHTSETTLE_IDLE		0x300
+#define	BRXHTSETTLE_RESERVED		0x1c00
+#define	BRXHT_RXHP_EN			0x8000
+#define	BRXAGC_FREEZE_THRES		0x30000
+#define	BRXAGC_TOGETHEREN		0x40000
+#define	BRXHTAGC_MIN			0x80000
+#define	BRXHTAGC_EN			0x100000
+#define	BRXHTDAGC_EN			0x200000
+#define	BRXHT_RXHP_BBP			0x1c00000
+#define	BRXHT_RXHP_FINAL		0xe0000000
+#define	BRXPW_RADIO_TH			0x3
+#define	BRXPW_RADIO_EN			0x4
+#define	BRXMF_HOLD			0x3800
+#define	BRXPD_DELAY_TH1			0x38
+#define	BRXPD_DELAY_TH2			0x1c0
+#define	BRXPD_DC_COUNT_MAX		0x600
+#define	BRXPD_DELAY_TH			0x8000
+#define	BRXPROCESS_DELAY		0xf0000
+#define	BRXSEARCHRANGE_GI2_EARLY	0x700000
+#define	BRXFRAME_FUARD_COUNTER_L	0x3800000
+#define	BRXSGI_GUARD_L			0xc000000
+#define	BRXSGI_SEARCH_L			0x30000000
+#define	BRXSGI_TH			0xc0000000
+#define	BDFSCNT0			0xff
+#define	BDFSCNT1			0xff00
+#define	BDFSFLAG			0xf0000
+#define	BMF_WEIGHT_SUM			0x300000
+#define	BMINIDX_TH			0x7f000000
+#define	BDAFORMAT			0x40000
+#define	BTXCH_EMU_ENABLE		0x01000000
+#define	BTRSW_ISOLATION_A		0x7f
+#define	BTRSW_ISOLATION_B		0x7f00
+#define	BTRSW_ISOLATION_C		0x7f0000
+#define	BTRSW_ISOLATION_D		0x7f000000
+#define	BEXT_LNA_GAIN			0x7c00
+
+#define	BSTBC_EN			0x4
+#define	BANTENNA_MAPPING		0x10
+#define	BNSS				0x20
+#define	BCFO_ANTSUM_ID              0x200
+#define	BPHY_COUNTER_RESET		0x8000000
+#define	BCFO_REPORT_GET			0x4000000
+#define	BOFDM_CONTINUE_TX		0x10000000
+#define	BOFDM_SINGLE_CARRIER		0x20000000
+#define	BOFDM_SINGLE_TONE		0x40000000
+#define	BHT_DETECT			0x100
+#define	BCFOEN				0x10000
+#define	BCFOVALUE			0xfff00000
+#define	BSIGTONE_RE			0x3f
+#define	BSIGTONE_IM			0x7f00
+#define	BCOUNTER_CCA			0xffff
+#define	BCOUNTER_PARITYFAIL		0xffff0000
+#define	BCOUNTER_RATEILLEGAL		0xffff
+#define	BCOUNTER_CRC8FAIL		0xffff0000
+#define	BCOUNTER_MCSNOSUPPORT		0xffff
+#define	BCOUNTER_FASTSYNC		0xffff
+#define	BSHORTCFO			0xfff
+#define	BSHORTCFOT_LENGTH		12
+#define	BSHORTCFOF_LENGTH		11
+#define	BLONGCFO			0x7ff
+#define	BLONGCFOT_LENGTH		11
+#define	BLONGCFOF_LENGTH		11
+#define	BTAILCFO			0x1fff
+#define	BTAILCFOT_LENGTH		13
+#define	BTAILCFOF_LENGTH		12
+#define	BNOISE_EN_PWDB			0xffff
+#define	BCC_POWER_DB			0xffff0000
+#define	BMOISE_PWDB			0xffff
+#define	BPOWERMEAST_LENGTH		10
+#define	BPOWERMEASF_LENGTH		3
+#define	BRX_HT_BW			0x1
+#define	BRXSC				0x6
+#define	BRX_HT				0x8
+#define	BNB_INTF_DET_ON			0x1
+#define	BINTF_WIN_LEN_CFG		0x30
+#define	BNB_INTF_TH_CFG			0x1c0
+#define	BRFGAIN				0x3f
+#define	BTABLESEL			0x40
+#define	BTRSW				0x80
+#define	BRXSNR_A			0xff
+#define	BRXSNR_B			0xff00
+#define	BRXSNR_C			0xff0000
+#define	BRXSNR_D			0xff000000
+#define	BSNR_EVMT_LENGTH		8
+#define	BSNR_EVMF_LENGTH		1
+#define	BCSI1ST				0xff
+#define	BCSI2ND				0xff00
+#define	BRXEVM1ST			0xff0000
+#define	BRXEVM2ND			0xff000000
+#define	BSIGEVM				0xff
+#define	BPWDB				0xff00
+#define	BSGIEN				0x10000
+
+#define	BSFACTOR_QMA1			0xf
+#define	BSFACTOR_QMA2			0xf0
+#define	BSFACTOR_QMA3			0xf00
+#define	BSFACTOR_QMA4			0xf000
+#define	BSFACTOR_QMA5			0xf0000
+#define	BSFACTOR_QMA6			0xf0000
+#define	BSFACTOR_QMA7			0xf00000
+#define	BSFACTOR_QMA8			0xf000000
+#define	BSFACTOR_QMA9			0xf0000000
+#define	BCSI_SCHEME			0x100000
+
+#define	BNOISE_LVL_TOP_SET          0x3
+#define	BCHSMOOTH			0x4
+#define	BCHSMOOTH_CFG1			0x38
+#define	BCHSMOOTH_CFG2			0x1c0
+#define	BCHSMOOTH_CFG3			0xe00
+#define	BCHSMOOTH_CFG4			0x7000
+#define	BMRCMODE			0x800000
+#define	BTHEVMCFG			0x7000000
+
+#define	BLOOP_FIT_TYPE			0x1
+#define	BUPD_CFO			0x40
+#define	BUPD_CFO_OFFDATA		0x80
+#define	BADV_UPD_CFO			0x100
+#define	BADV_TIME_CTRL			0x800
+#define	BUPD_CLKO			0x1000
+#define	BFC				0x6000
+#define	BTRACKING_MODE			0x8000
+#define	BPHCMP_ENABLE			0x10000
+#define	BUPD_CLKO_LTF			0x20000
+#define	BCOM_CH_CFO			0x40000
+#define	BCSI_ESTI_MODE			0x80000
+#define	BADV_UPD_EQZ			0x100000
+#define	BUCHCFG				0x7000000
+#define	BUPDEQZ				0x8000000
+
+#define	BRX_PESUDO_NOISE_ON         0x20000000
+#define	BRX_PESUDO_NOISE_A		0xff
+#define	BRX_PESUDO_NOISE_B		0xff00
+#define	BRX_PESUDO_NOISE_C		0xff0000
+#define	BRX_PESUDO_NOISE_D		0xff000000
+#define	BRX_PESUDO_NOISESTATE_A     0xffff
+#define	BRX_PESUDO_NOISESTATE_B     0xffff0000
+#define	BRX_PESUDO_NOISESTATE_C     0xffff
+#define	BRX_PESUDO_NOISESTATE_D     0xffff0000
+
+#define	BZEBRA1_HSSIENABLE		0x8
+#define	BZEBRA1_TRXCONTROL		0xc00
+#define	BZEBRA1_TRXGAINSETTING		0x07f
+#define	BZEBRA1_RXCOUNTER		0xc00
+#define	BZEBRA1_TXCHANGEPUMP		0x38
+#define	BZEBRA1_RXCHANGEPUMP		0x7
+#define	BZEBRA1_CHANNEL_NUM		0xf80
+#define	BZEBRA1_TXLPFBW			0x400
+#define	BZEBRA1_RXLPFBW			0x600
+
+#define	BRTL8256REG_MODE_CTRL1      0x100
+#define	BRTL8256REG_MODE_CTRL0      0x40
+#define	BRTL8256REG_TXLPFBW         0x18
+#define	BRTL8256REG_RXLPFBW         0x600
+
+#define	BRTL8258_TXLPFBW		0xc
+#define	BRTL8258_RXLPFBW		0xc00
+#define	BRTL8258_RSSILPFBW		0xc0
+
+#define	BBYTE0				0x1
+#define	BBYTE1				0x2
+#define	BBYTE2				0x4
+#define	BBYTE3				0x8
+#define	BWORD0				0x3
+#define	BWORD1				0xc
+#define	BWORD				0xf
+
+#define	MASKBYTE0			0xff
+#define	MASKBYTE1			0xff00
+#define	MASKBYTE2			0xff0000
+#define	MASKBYTE3			0xff000000
+#define	MASKHWORD			0xffff0000
+#define	MASKLWORD			0x0000ffff
+#define	MASKDWORD					0xffffffff
+#define	MASK12BITS					0xfff
+#define	MASKH4BITS					0xf0000000
+#define MASKOFDM_D					0xffc00000
+#define	MASKCCK						0x3f3f3f3f
+
+#define	MASK4BITS			0x0f
+#define	MASK20BITS			0xfffff
+#define RFREG_OFFSET_MASK			0xfffff
+
+#define	BENABLE				0x1
+#define	BDISABLE			0x0
+
+#define	LEFT_ANTENNA			0x0
+#define	RIGHT_ANTENNA			0x1
+
+#define	TCHECK_TXSTATUS			500
+#define	TUPDATE_RXCOUNTER		100
 
 /* 2 EFUSE_TEST (For RTL8723 partially) */
-#define EFUSE_SEL(x)				(((x) & 0x3) << 8)
+#define EFUSE_SEL(x)					(((x) & 0x3) << 8)
 #define EFUSE_SEL_MASK				0x300
-#define EFUSE_WIFI_SEL_0			0x0
-
+#define EFUSE_WIFI_SEL_0				0x0
 /* Enable GPIO[9] as WiFi HW PDn source*/
-#define	WL_HWPDN_EN				BIT(0)
+#define	WL_HWPDN_EN					BIT(0)
 /* WiFi HW PDn polarity control*/
-#define	WL_HWPDN_SL				BIT(1)
+#define	WL_HWPDN_SL					BIT(1)
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/rf.c b/drivers/net/wireless/rtlwifi/rtl8723ae/rf.c
index 50dd2fb2c93d..9ebc8281ff99 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/rf.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -34,10 +30,12 @@
 #include "rf.h"
 #include "dm.h"
 
-void rtl8723ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
+static bool _rtl8723e_phy_rf6052_config_parafile(struct ieee80211_hw *hw);
+
+void rtl8723e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	switch (bandwidth) {
 	case HT_CHANNEL_WIDTH_20:
@@ -59,11 +57,11 @@ void rtl8723ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 	}
 }
 
-void rtl8723ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
-					  u8 *ppowerlevel)
+void rtl8723e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					 u8 *ppowerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u32 tx_agc[2] = {0, 0}, tmpval;
@@ -79,7 +77,8 @@ void rtl8723ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 		tx_agc[RF90_PATH_B] = 0x3f3f3f3f;
 
 		if (turbo_scanoff) {
-			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B;
+				idx1++) {
 				tx_agc[idx1] = ppowerlevel[idx1] |
 				    (ppowerlevel[idx1] << 8) |
 				    (ppowerlevel[idx1] << 16) |
@@ -89,24 +88,27 @@ void rtl8723ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 	} else {
 		for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
 			tx_agc[idx1] = ppowerlevel[idx1] |
-				       (ppowerlevel[idx1] << 8) |
-				       (ppowerlevel[idx1] << 16) |
-				       (ppowerlevel[idx1] << 24);
+			    (ppowerlevel[idx1] << 8) |
+			    (ppowerlevel[idx1] << 16) |
+			    (ppowerlevel[idx1] << 24);
 		}
 
 		if (rtlefuse->eeprom_regulatory == 0) {
-			tmpval = (rtlphy->mcs_offset[0][6]) +
-				(rtlphy->mcs_offset[0][7] << 8);
+			tmpval =
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][6]) +
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][7] <<
+			     8);
 			tx_agc[RF90_PATH_A] += tmpval;
 
-			tmpval = (rtlphy->mcs_offset[0][14]) +
-			    (rtlphy->mcs_offset[0][15] << 24);
+			tmpval = (rtlphy->mcs_txpwrlevel_origoffset[0][14]) +
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][15] <<
+			     24);
 			tx_agc[RF90_PATH_B] += tmpval;
 		}
 	}
 
 	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
-		ptr = (u8 *) (&(tx_agc[idx1]));
+		ptr = (u8 *)&tx_agc[idx1];
 		for (idx2 = 0; idx2 < 4; idx2++) {
 			if (*ptr > RF6052_MAX_TX_PWR)
 				*ptr = RF6052_MAX_TX_PWR;
@@ -119,7 +121,7 @@ void rtl8723ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 		"CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
-		RTXAGC_A_CCK1_MCS32);
+		 RTXAGC_A_CCK1_MCS32);
 
 	tmpval = tx_agc[RF90_PATH_A] >> 8;
 
@@ -129,100 +131,99 @@ void rtl8723ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 		"CCK PWR 2~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
-		RTXAGC_B_CCK11_A_CCK2_11);
+		 RTXAGC_B_CCK11_A_CCK2_11);
 
 	tmpval = tx_agc[RF90_PATH_B] >> 24;
 	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE0, tmpval);
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 		"CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
-		RTXAGC_B_CCK11_A_CCK2_11);
+		 RTXAGC_B_CCK11_A_CCK2_11);
 
 	tmpval = tx_agc[RF90_PATH_B] & 0x00ffffff;
 	rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, 0xffffff00, tmpval);
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 		"CCK PWR 1~5.5M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
-		RTXAGC_B_CCK1_55_MCS32);
+		 RTXAGC_B_CCK1_55_MCS32);
 }
 
-static void rtl8723ae_phy_get_power_base(struct ieee80211_hw *hw,
-					 u8 *ppowerlevel, u8 channel,
-					 u32 *ofdmbase, u32 *mcsbase)
+static void rtl8723e_phy_get_power_base(struct ieee80211_hw *hw,
+					u8 *ppowerlevel, u8 channel,
+					u32 *ofdmbase, u32 *mcsbase)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u32 powerBase0, powerBase1;
+	u32 powerbase0, powerbase1;
 	u8 legacy_pwrdiff, ht20_pwrdiff;
 	u8 i, powerlevel[2];
 
 	for (i = 0; i < 2; i++) {
 		powerlevel[i] = ppowerlevel[i];
 		legacy_pwrdiff = rtlefuse->txpwr_legacyhtdiff[i][channel - 1];
-		powerBase0 = powerlevel[i] + legacy_pwrdiff;
+		powerbase0 = powerlevel[i] + legacy_pwrdiff;
 
-		powerBase0 = (powerBase0 << 24) | (powerBase0 << 16) |
-		    (powerBase0 << 8) | powerBase0;
-		*(ofdmbase + i) = powerBase0;
+		powerbase0 = (powerbase0 << 24) | (powerbase0 << 16) |
+		    (powerbase0 << 8) | powerbase0;
+		*(ofdmbase + i) = powerbase0;
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 			" [OFDM power base index rf(%c) = 0x%x]\n",
-			((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
+			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
 	}
 
 	for (i = 0; i < 2; i++) {
 		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20) {
-			ht20_pwrdiff = rtlefuse->txpwr_ht20diff[i][channel - 1];
+			ht20_pwrdiff =
+				rtlefuse->txpwr_ht20diff[i][channel - 1];
 			powerlevel[i] += ht20_pwrdiff;
 		}
-		powerBase1 = powerlevel[i];
-		powerBase1 = (powerBase1 << 24) |
-		    (powerBase1 << 16) | (powerBase1 << 8) | powerBase1;
+		powerbase1 = powerlevel[i];
+		powerbase1 = (powerbase1 << 24) |
+		    (powerbase1 << 16) | (powerbase1 << 8) | powerbase1;
 
-		*(mcsbase + i) = powerBase1;
+		*(mcsbase + i) = powerbase1;
 
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 			" [MCS power base index rf(%c) = 0x%x]\n",
-			((i == 0) ? 'A' : 'B'), *(mcsbase + i));
+			 ((i == 0) ? 'A' : 'B'), *(mcsbase + i));
 	}
 }
 
-static void rtl8723ae_get_txpwr_val_by_reg(struct ieee80211_hw *hw,
-					   u8 channel, u8 index,
-					   u32 *powerBase0,
-					   u32 *powerBase1,
-					   u32 *p_outwriteval)
+static void get_txpower_writeval_by_reg(struct ieee80211_hw *hw,
+					u8 channel, u8 index,
+					u32 *powerbase0,
+					u32 *powerbase1,
+					u32 *p_outwriteval)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u8 i, chnlgroup = 0, pwr_diff_limit[4];
-	u32 writeVal, customer_limit, rf;
+	u32 writeval, customer_limit, rf;
 
 	for (rf = 0; rf < 2; rf++) {
 		switch (rtlefuse->eeprom_regulatory) {
 		case 0:
 			chnlgroup = 0;
 
-			writeVal = rtlphy->mcs_offset[chnlgroup]
-				   [index + (rf ? 8 : 0)] +
-				   ((index < 2) ? powerBase0[rf] :
-				   powerBase1[rf]);
+			writeval =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup][index +
+								(rf ? 8 : 0)]
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				"RTK better performance, "
-				"writeVal(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), writeVal);
+				"RTK better performance, writeval(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 1:
 			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-				writeVal = ((index < 2) ? powerBase0[rf] :
-					    powerBase1[rf]);
+				writeval = ((index < 2) ? powerbase0[rf] :
+					    powerbase1[rf]);
 
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-					"Realtek regulatory, 40MHz, "
-					"writeVal(%c) = 0x%x\n",
-					((rf == 0) ? 'A' : 'B'), writeVal);
+					"Realtek regulatory, 40MHz, writeval(%c) = 0x%x\n",
+					((rf == 0) ? 'A' : 'B'), writeval);
 			} else {
 				if (rtlphy->pwrgroup_cnt == 1)
 					chnlgroup = 0;
@@ -234,29 +235,30 @@ static void rtl8723ae_get_txpwr_val_by_reg(struct ieee80211_hw *hw,
 					else if (channel > 9)
 						chnlgroup = 2;
 					if (rtlphy->current_chan_bw ==
-					    HT_CHANNEL_WIDTH_20)
+						HT_CHANNEL_WIDTH_20)
 						chnlgroup++;
 					else
 						chnlgroup += 4;
 				}
 
-				writeVal = rtlphy->mcs_offset[chnlgroup]
+				writeval =
+				    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
 				    [index + (rf ? 8 : 0)] + ((index < 2) ?
-							      powerBase0[rf] :
-							      powerBase1[rf]);
+							      powerbase0[rf] :
+							      powerbase1[rf]);
 
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-					"Realtek regulatory, 20MHz, writeVal(%c) = 0x%x\n",
-					((rf == 0) ? 'A' : 'B'), writeVal);
+					"Realtek regulatory, 20MHz, writeval(%c) = 0x%x\n",
+					((rf == 0) ? 'A' : 'B'), writeval);
 			}
 			break;
 		case 2:
-			writeVal =
-			    ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			writeval =
+			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				"Better regulatory, writeVal(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), writeVal);
+				"Better regulatory, writeval(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 3:
 			chnlgroup = 0;
@@ -265,18 +267,21 @@ static void rtl8723ae_get_txpwr_val_by_reg(struct ieee80211_hw *hw,
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 					"customer's limit, 40MHz rf(%c) = 0x%x\n",
 					((rf == 0) ? 'A' : 'B'),
-					rtlefuse->pwrgroup_ht40[rf][channel-1]);
+					rtlefuse->pwrgroup_ht40[rf][channel -
+								     1]);
 			} else {
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 					"customer's limit, 20MHz rf(%c) = 0x%x\n",
 					((rf == 0) ? 'A' : 'B'),
-					rtlefuse->pwrgroup_ht20[rf][channel-1]);
+					rtlefuse->pwrgroup_ht20[rf][channel -
+								     1]);
 			}
 			for (i = 0; i < 4; i++) {
 				pwr_diff_limit[i] =
-					(u8) ((rtlphy->mcs_offset
-					[chnlgroup][index + (rf ? 8 : 0)] &
-					(0x7f << (i * 8))) >> (i * 8));
+				    (u8)((rtlphy->mcs_txpwrlevel_origoffset
+					  [chnlgroup][index +
+						(rf ? 8 : 0)] & (0x7f <<
+						(i * 8))) >> (i * 8));
 
 				if (rtlphy->current_chan_bw ==
 				    HT_CHANNEL_WIDTH_20_40) {
@@ -302,41 +307,42 @@ static void rtl8723ae_get_txpwr_val_by_reg(struct ieee80211_hw *hw,
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 				"Customer's limit rf(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), customer_limit);
+				 ((rf == 0) ? 'A' : 'B'), customer_limit);
 
-			writeVal = customer_limit +
-			    ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			writeval = customer_limit +
+			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				"Customer, writeVal rf(%c)= 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), writeVal);
+				"Customer, writeval rf(%c)= 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		default:
 			chnlgroup = 0;
-			writeVal = rtlphy->mcs_offset[chnlgroup][index +
-			    (rf ? 8 : 0)] + ((index < 2) ? powerBase0[rf] :
-			    powerBase1[rf]);
+			writeval =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
+			    [index + (rf ? 8 : 0)]
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				"RTK better performance, writeVal rf(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), writeVal);
+				"RTK better performance, writeval rf(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		}
 
 		if (rtlpriv->dm.dynamic_txhighpower_lvl == TXHIGHPWRLEVEL_BT1)
-			writeVal = writeVal - 0x06060606;
+			writeval = writeval - 0x06060606;
 		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
 			 TXHIGHPWRLEVEL_BT2)
-			writeVal = writeVal - 0x0c0c0c0c;
-		*(p_outwriteval + rf) = writeVal;
+			writeval = writeval - 0x0c0c0c0c;
+		*(p_outwriteval + rf) = writeval;
 	}
 }
 
-static void _rtl8723ae_write_ofdm_power_reg(struct ieee80211_hw *hw,
-					    u8 index, u32 *pValue)
+static void _rtl8723e_write_ofdm_power_reg(struct ieee80211_hw *hw,
+					   u8 index, u32 *pvalue)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	u16 regoffset_a[6] = {
 		RTXAGC_A_RATE18_06, RTXAGC_A_RATE54_24,
@@ -349,29 +355,29 @@ static void _rtl8723ae_write_ofdm_power_reg(struct ieee80211_hw *hw,
 		RTXAGC_B_MCS11_MCS08, RTXAGC_B_MCS15_MCS12
 	};
 	u8 i, rf, pwr_val[4];
-	u32 writeVal;
+	u32 writeval;
 	u16 regoffset;
 
 	for (rf = 0; rf < 2; rf++) {
-		writeVal = pValue[rf];
+		writeval = pvalue[rf];
 		for (i = 0; i < 4; i++) {
-			pwr_val[i] = (u8) ((writeVal & (0x7f <<
-							(i * 8))) >> (i * 8));
+			pwr_val[i] = (u8)((writeval & (0x7f <<
+					   (i * 8))) >> (i * 8));
 
 			if (pwr_val[i] > RF6052_MAX_TX_PWR)
 				pwr_val[i] = RF6052_MAX_TX_PWR;
 		}
-		writeVal = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
+		writeval = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
 		    (pwr_val[1] << 8) | pwr_val[0];
 
 		if (rf == 0)
 			regoffset = regoffset_a[index];
 		else
 			regoffset = regoffset_b[index];
-		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeVal);
+		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeval);
 
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-			"Set 0x%x = %08x\n", regoffset, writeVal);
+			"Set 0x%x = %08x\n", regoffset, writeval);
 
 		if (((get_rf_type(rtlphy) == RF_2T2R) &&
 		     (regoffset == RTXAGC_A_MCS15_MCS12 ||
@@ -380,7 +386,7 @@ static void _rtl8723ae_write_ofdm_power_reg(struct ieee80211_hw *hw,
 		     (regoffset == RTXAGC_A_MCS07_MCS04 ||
 		      regoffset == RTXAGC_B_MCS07_MCS04))) {
 
-			writeVal = pwr_val[3];
+			writeval = pwr_val[3];
 			if (regoffset == RTXAGC_A_MCS15_MCS12 ||
 			    regoffset == RTXAGC_A_MCS07_MCS04)
 				regoffset = 0xc90;
@@ -389,37 +395,49 @@ static void _rtl8723ae_write_ofdm_power_reg(struct ieee80211_hw *hw,
 				regoffset = 0xc98;
 
 			for (i = 0; i < 3; i++) {
-				writeVal = (writeVal > 6) ? (writeVal - 6) : 0;
+				writeval = (writeval > 6) ? (writeval - 6) : 0;
 				rtl_write_byte(rtlpriv, (u32) (regoffset + i),
-					       (u8) writeVal);
+					       (u8)writeval);
 			}
 		}
 	}
 }
 
-void rtl8723ae_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
-					   u8 *ppowerlevel, u8 channel)
+void rtl8723e_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					  u8 *ppowerlevel, u8 channel)
 {
-	u32 writeVal[2], powerBase0[2], powerBase1[2];
+	u32 writeval[2], powerbase0[2], powerbase1[2];
 	u8 index;
 
-	rtl8723ae_phy_get_power_base(hw, ppowerlevel,
-				  channel, &powerBase0[0], &powerBase1[0]);
+	rtl8723e_phy_get_power_base(hw, ppowerlevel,
+				    channel, &powerbase0[0], &powerbase1[0]);
 
 	for (index = 0; index < 6; index++) {
-		rtl8723ae_get_txpwr_val_by_reg(hw, channel, index,
-					      &powerBase0[0],
-					      &powerBase1[0],
-					      &writeVal[0]);
+		get_txpower_writeval_by_reg(hw, channel, index, &powerbase0[0],
+					    &powerbase1[0],
+					    &writeval[0]);
 
-		_rtl8723ae_write_ofdm_power_reg(hw, index, &writeVal[0]);
+		_rtl8723e_write_ofdm_power_reg(hw, index, &writeval[0]);
 	}
 }
 
-static bool _rtl8723ae_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
+bool rtl8723e_phy_rf6052_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	if (rtlphy->rf_type == RF_1T1R)
+		rtlphy->num_total_rfpath = 1;
+	else
+		rtlphy->num_total_rfpath = 2;
+
+	return _rtl8723e_phy_rf6052_config_parafile(hw);
+}
+
+static bool _rtl8723e_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 u4_regvalue = 0;
 	u8 rfpath;
 	bool rtstatus = true;
@@ -457,11 +475,12 @@ static bool _rtl8723ae_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 
 		switch (rfpath) {
 		case RF90_PATH_A:
-			rtstatus = rtl8723ae_phy_config_rf_with_headerfile(hw,
+			rtstatus = rtl8723e_phy_config_rf_with_headerfile(hw,
 						(enum radio_path)rfpath);
 			break;
 		case RF90_PATH_B:
-			rtstatus = rtl8723ae_phy_config_rf_with_headerfile(hw,
+			rtstatus =
+			  rtl8723e_phy_config_rf_with_headerfile(hw,
 						(enum radio_path)rfpath);
 			break;
 		case RF90_PATH_C:
@@ -469,6 +488,7 @@ static bool _rtl8723ae_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 		case RF90_PATH_D:
 			break;
 		}
+
 		switch (rfpath) {
 		case RF90_PATH_A:
 		case RF90_PATH_C:
@@ -481,25 +501,14 @@ static bool _rtl8723ae_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 				      BRFSI_RFENV << 16, u4_regvalue);
 			break;
 		}
+
 		if (rtstatus != true) {
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 				 "Radio[%d] Fail!!", rfpath);
 			return false;
 		}
 	}
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "<---\n");
-	return rtstatus;
-}
-
-bool rtl8723ae_phy_rf6052_config(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 
-	if (rtlphy->rf_type == RF_1T1R)
-		rtlphy->num_total_rfpath = 1;
-	else
-		rtlphy->num_total_rfpath = 2;
-
-	return _rtl8723ae_phy_rf6052_config_parafile(hw);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "\n");
+	return rtstatus;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h b/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h
index 57f1933ee663..f3f45b16361f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/rf.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -31,12 +27,14 @@
 #define __RTL8723E_RF_H__
 
 #define RF6052_MAX_TX_PWR		0x3F
+#define RF6052_MAX_REG			0x3F
 
-void rtl8723ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth);
-void rtl8723ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
-					  u8 *ppowerlevel);
-void rtl8723ae_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
-					   u8 *ppowerlevel, u8 channel);
-bool rtl8723ae_phy_rf6052_config(struct ieee80211_hw *hw);
+void rtl8723e_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+				       u8 bandwidth);
+void rtl8723e_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					 u8 *ppowerlevel);
+void rtl8723e_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					  u8 *ppowerlevel, u8 channel);
+bool rtl8723e_phy_rf6052_config(struct ieee80211_hw *hw);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
index 73cba1eec8cf..8280bab43df4 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -28,34 +24,35 @@
  *****************************************************************************/
 
 #include "../wifi.h"
-#include <linux/vmalloc.h>
-#include <linux/module.h>
-
 #include "../core.h"
 #include "../pci.h"
-#include "../base.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
-#include "../rtl8723com/phy_common.h"
 #include "dm.h"
-#include "hw.h"
 #include "fw.h"
 #include "../rtl8723com/fw_common.h"
+#include "hw.h"
 #include "sw.h"
 #include "trx.h"
 #include "led.h"
 #include "table.h"
 #include "hal_btc.h"
+#include "../btcoexist/rtl_btc.h"
+#include "../rtl8723com/phy_common.h"
 
-static void rtl8723ae_init_aspm_vars(struct ieee80211_hw *hw)
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
+static void rtl8723e_init_aspm_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
 	/*close ASPM for AMD defaultly */
 	rtlpci->const_amdpci_aspm = 0;
 
-	/* ASPM PS mode.
+	/**
+	 * ASPM PS mode.
 	 * 0 - Disable ASPM,
 	 * 1 - Enable ASPM without Clock Req,
 	 * 2 - Enable ASPM with Clock Req,
@@ -71,7 +68,8 @@ static void rtl8723ae_init_aspm_vars(struct ieee80211_hw *hw)
 	/*Setting for PCI-E bridge */
 	rtlpci->const_hostpci_aspm_setting = 0x02;
 
-	/* In Hw/Sw Radio Off situation.
+	/**
+	 * In Hw/Sw Radio Off situation.
 	 * 0 - Default,
 	 * 1 - From ASPM setting without low Mac Pwr,
 	 * 2 - From ASPM setting with low Mac Pwr,
@@ -80,7 +78,8 @@ static void rtl8723ae_init_aspm_vars(struct ieee80211_hw *hw)
 	 */
 	rtlpci->const_hwsw_rfoff_d3 = 0;
 
-	/* This setting works for those device with
+	/**
+	 * This setting works for those device with
 	 * backdoor ASPM setting such as EPHY setting.
 	 * 0 - Not support ASPM,
 	 * 1 - Support ASPM,
@@ -89,14 +88,17 @@ static void rtl8723ae_init_aspm_vars(struct ieee80211_hw *hw)
 	rtlpci->const_support_pciaspm = 1;
 }
 
-int rtl8723ae_init_sw_vars(struct ieee80211_hw *hw)
+int rtl8723e_init_sw_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	int err;
+	int err = 0;
+
+	rtl8723e_bt_reg_init(hw);
+
+	rtlpriv->btcoexist.btc_ops = rtl_btc_get_ops_pointer();
 
-	rtl8723ae_bt_reg_init(hw);
 	rtlpriv->dm.dm_initialgain_enable = 1;
 	rtlpriv->dm.dm_flag = 0;
 	rtlpriv->dm.disable_framebursting = 0;
@@ -138,7 +140,9 @@ int rtl8723ae_init_sw_vars(struct ieee80211_hw *hw)
 		   PHIMR_PSTIMEOUT |
 		   0);
 
-	rtlpci->irq_mask[1] = (u32)(PHIMR_RXFOVW | 0);
+	rtlpci->irq_mask[1]	=
+		 (u32)(PHIMR_RXFOVW |
+				0);
 
 	/* for debug level */
 	rtlpriv->dbg.global_debuglevel = rtlpriv->cfg->mod_params->debug;
@@ -146,12 +150,11 @@ int rtl8723ae_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	if (rtlpriv->cfg->mod_params->disable_watchdog)
+		pr_info("watchdog disabled\n");
 	rtlpriv->psc.reg_fwctrl_lps = 3;
 	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
-	/* for ASPM, you can close aspm through
-	 * set const_support_pciaspm = 0
-	 */
-	rtl8723ae_init_aspm_vars(hw);
+	rtl8723e_init_aspm_vars(hw);
 
 	if (rtlpriv->psc.reg_fwctrl_lps == 1)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_MIN_MODE;
@@ -161,7 +164,7 @@ int rtl8723ae_init_sw_vars(struct ieee80211_hw *hw)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
 
 	/* for firmware buf */
-	rtlpriv->rtlhal.pfirmware = vmalloc(0x6000);
+	rtlpriv->rtlhal.pfirmware = vzalloc(0x6000);
 	if (!rtlpriv->rtlhal.pfirmware) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Can't alloc buffer for fw.\n");
@@ -186,7 +189,7 @@ int rtl8723ae_init_sw_vars(struct ieee80211_hw *hw)
 	return 0;
 }
 
-void rtl8723ae_deinit_sw_vars(struct ieee80211_hw *hw)
+void rtl8723e_deinit_sw_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
@@ -196,59 +199,69 @@ void rtl8723ae_deinit_sw_vars(struct ieee80211_hw *hw)
 	}
 }
 
-static bool is_fw_header(struct rtl92c_firmware_header *hdr)
+/* get bt coexist status */
+bool rtl8723e_get_btc_status(void)
+{
+	return true;
+}
+
+static bool is_fw_header(struct rtl8723e_firmware_header *hdr)
 {
 	return (hdr->signature & 0xfff0) == 0x2300;
 }
 
-static struct rtl_hal_ops rtl8723ae_hal_ops = {
-	.init_sw_vars = rtl8723ae_init_sw_vars,
-	.deinit_sw_vars = rtl8723ae_deinit_sw_vars,
-	.read_eeprom_info = rtl8723ae_read_eeprom_info,
-	.interrupt_recognized = rtl8723ae_interrupt_recognized,
-	.hw_init = rtl8723ae_hw_init,
-	.hw_disable = rtl8723ae_card_disable,
-	.hw_suspend = rtl8723ae_suspend,
-	.hw_resume = rtl8723ae_resume,
-	.enable_interrupt = rtl8723ae_enable_interrupt,
-	.disable_interrupt = rtl8723ae_disable_interrupt,
-	.set_network_type = rtl8723ae_set_network_type,
-	.set_chk_bssid = rtl8723ae_set_check_bssid,
-	.set_qos = rtl8723ae_set_qos,
-	.set_bcn_reg = rtl8723ae_set_beacon_related_registers,
-	.set_bcn_intv = rtl8723ae_set_beacon_interval,
-	.update_interrupt_mask = rtl8723ae_update_interrupt_mask,
-	.get_hw_reg = rtl8723ae_get_hw_reg,
-	.set_hw_reg = rtl8723ae_set_hw_reg,
-	.update_rate_tbl = rtl8723ae_update_hal_rate_tbl,
-	.fill_tx_desc = rtl8723ae_tx_fill_desc,
-	.fill_tx_cmddesc = rtl8723ae_tx_fill_cmddesc,
-	.query_rx_desc = rtl8723ae_rx_query_desc,
-	.set_channel_access = rtl8723ae_update_channel_access_setting,
-	.radio_onoff_checking = rtl8723ae_gpio_radio_on_off_checking,
-	.set_bw_mode = rtl8723ae_phy_set_bw_mode,
-	.switch_channel = rtl8723ae_phy_sw_chnl,
-	.dm_watchdog = rtl8723ae_dm_watchdog,
-	.scan_operation_backup = rtl_phy_scan_operation_backup,
-	.set_rf_power_state = rtl8723ae_phy_set_rf_power_state,
-	.led_control = rtl8723ae_led_control,
-	.set_desc = rtl8723ae_set_desc,
-	.get_desc = rtl8723ae_get_desc,
-	.tx_polling = rtl8723ae_tx_polling,
-	.enable_hw_sec = rtl8723ae_enable_hw_security_config,
-	.set_key = rtl8723ae_set_key,
-	.init_sw_leds = rtl8723ae_init_sw_leds,
+static struct rtl_hal_ops rtl8723e_hal_ops = {
+	.init_sw_vars = rtl8723e_init_sw_vars,
+	.deinit_sw_vars = rtl8723e_deinit_sw_vars,
+	.read_eeprom_info = rtl8723e_read_eeprom_info,
+	.interrupt_recognized = rtl8723e_interrupt_recognized,
+	.hw_init = rtl8723e_hw_init,
+	.hw_disable = rtl8723e_card_disable,
+	.hw_suspend = rtl8723e_suspend,
+	.hw_resume = rtl8723e_resume,
+	.enable_interrupt = rtl8723e_enable_interrupt,
+	.disable_interrupt = rtl8723e_disable_interrupt,
+	.set_network_type = rtl8723e_set_network_type,
+	.set_chk_bssid = rtl8723e_set_check_bssid,
+	.set_qos = rtl8723e_set_qos,
+	.set_bcn_reg = rtl8723e_set_beacon_related_registers,
+	.set_bcn_intv = rtl8723e_set_beacon_interval,
+	.update_interrupt_mask = rtl8723e_update_interrupt_mask,
+	.get_hw_reg = rtl8723e_get_hw_reg,
+	.set_hw_reg = rtl8723e_set_hw_reg,
+	.update_rate_tbl = rtl8723e_update_hal_rate_tbl,
+	.fill_tx_desc = rtl8723e_tx_fill_desc,
+	.fill_tx_cmddesc = rtl8723e_tx_fill_cmddesc,
+	.query_rx_desc = rtl8723e_rx_query_desc,
+	.set_channel_access = rtl8723e_update_channel_access_setting,
+	.radio_onoff_checking = rtl8723e_gpio_radio_on_off_checking,
+	.set_bw_mode = rtl8723e_phy_set_bw_mode,
+	.switch_channel = rtl8723e_phy_sw_chnl,
+	.dm_watchdog = rtl8723e_dm_watchdog,
+	.scan_operation_backup = rtl8723e_phy_scan_operation_backup,
+	.set_rf_power_state = rtl8723e_phy_set_rf_power_state,
+	.led_control = rtl8723e_led_control,
+	.set_desc = rtl8723e_set_desc,
+	.get_desc = rtl8723e_get_desc,
+	.is_tx_desc_closed = rtl8723e_is_tx_desc_closed,
+	.tx_polling = rtl8723e_tx_polling,
+	.enable_hw_sec = rtl8723e_enable_hw_security_config,
+	.set_key = rtl8723e_set_key,
+	.init_sw_leds = rtl8723e_init_sw_leds,
 	.get_bbreg = rtl8723_phy_query_bb_reg,
 	.set_bbreg = rtl8723_phy_set_bb_reg,
-	.get_rfreg = rtl8723ae_phy_query_rf_reg,
-	.set_rfreg = rtl8723ae_phy_set_rf_reg,
+	.get_rfreg = rtl8723e_phy_query_rf_reg,
+	.set_rfreg = rtl8723e_phy_set_rf_reg,
 	.c2h_command_handle = rtl_8723e_c2h_command_handle,
 	.bt_wifi_media_status_notify = rtl_8723e_bt_wifi_media_status_notify,
-	.bt_coex_off_before_lps = rtl8723ae_bt_coex_off_before_lps,
+	.bt_coex_off_before_lps =
+		rtl8723e_dm_bt_turn_off_bt_coexist_before_enter_lps,
+	.get_btc_status = rtl8723e_get_btc_status,
+	.rx_command_packet = rtl8723e_rx_command_packet,
 	.is_fw_header = is_fw_header,
 };
 
-static struct rtl_mod_params rtl8723ae_mod_params = {
+static struct rtl_mod_params rtl8723e_mod_params = {
 	.sw_crypto = false,
 	.inactiveps = true,
 	.swctrl_lps = false,
@@ -256,13 +269,13 @@ static struct rtl_mod_params rtl8723ae_mod_params = {
 	.debug = DBG_EMERG,
 };
 
-static struct rtl_hal_cfg rtl8723ae_hal_cfg = {
+static struct rtl_hal_cfg rtl8723e_hal_cfg = {
 	.bar_id = 2,
 	.write_readback = true,
-	.name = "rtl8723ae_pci",
-	.fw_name = "rtlwifi/rtl8723fw.bin",
-	.ops = &rtl8723ae_hal_ops,
-	.mod_params = &rtl8723ae_mod_params,
+	.name = "rtl8723e_pci",
+	.fw_name = "rtlwifi/rtl8723efw.bin",
+	.ops = &rtl8723e_hal_ops,
+	.mod_params = &rtl8723e_mod_params,
 	.maps[SYS_ISO_CTRL] = REG_SYS_ISO_CTRL,
 	.maps[SYS_FUNC_EN] = REG_SYS_FUNC_EN,
 	.maps[SYS_CLK] = REG_SYS_CLKR,
@@ -271,6 +284,8 @@ static struct rtl_hal_cfg rtl8723ae_hal_cfg = {
 	.maps[MAC_RCR_ACRC32] = ACRC32,
 	.maps[MAC_RCR_ACF] = ACF,
 	.maps[MAC_RCR_AAP] = AAP,
+	.maps[MAC_HIMR] = REG_HIMR,
+	.maps[MAC_HIMRE] = REG_HIMRE,
 	.maps[EFUSE_TEST] = REG_EFUSE_TEST,
 	.maps[EFUSE_CTRL] = REG_EFUSE_CTRL,
 	.maps[EFUSE_CLK] = 0,
@@ -328,62 +343,63 @@ static struct rtl_hal_cfg rtl8723ae_hal_cfg = {
 	.maps[RTL_IMR_VIDOK] = PHIMR_VIDOK,
 	.maps[RTL_IMR_VODOK] = PHIMR_VODOK,
 	.maps[RTL_IMR_ROK] = PHIMR_ROK,
-	.maps[RTL_IBSS_INT_MASKS] = (PHIMR_BCNDMAINT0 |
-				     PHIMR_TXBCNOK | PHIMR_TXBCNERR),
+	.maps[RTL_IBSS_INT_MASKS] =
+		(PHIMR_BCNDMAINT0 | PHIMR_TXBCNOK | PHIMR_TXBCNERR),
 	.maps[RTL_IMR_C2HCMD] = PHIMR_C2HCMD,
 
 
-	.maps[RTL_RC_CCK_RATE1M] = DESC92_RATE1M,
-	.maps[RTL_RC_CCK_RATE2M] = DESC92_RATE2M,
-	.maps[RTL_RC_CCK_RATE5_5M] = DESC92_RATE5_5M,
-	.maps[RTL_RC_CCK_RATE11M] = DESC92_RATE11M,
-	.maps[RTL_RC_OFDM_RATE6M] = DESC92_RATE6M,
-	.maps[RTL_RC_OFDM_RATE9M] = DESC92_RATE9M,
-	.maps[RTL_RC_OFDM_RATE12M] = DESC92_RATE12M,
-	.maps[RTL_RC_OFDM_RATE18M] = DESC92_RATE18M,
-	.maps[RTL_RC_OFDM_RATE24M] = DESC92_RATE24M,
-	.maps[RTL_RC_OFDM_RATE36M] = DESC92_RATE36M,
-	.maps[RTL_RC_OFDM_RATE48M] = DESC92_RATE48M,
-	.maps[RTL_RC_OFDM_RATE54M] = DESC92_RATE54M,
-
-	.maps[RTL_RC_HT_RATEMCS7] = DESC92_RATEMCS7,
-	.maps[RTL_RC_HT_RATEMCS15] = DESC92_RATEMCS15,
+	.maps[RTL_RC_CCK_RATE1M] = DESC92C_RATE1M,
+	.maps[RTL_RC_CCK_RATE2M] = DESC92C_RATE2M,
+	.maps[RTL_RC_CCK_RATE5_5M] = DESC92C_RATE5_5M,
+	.maps[RTL_RC_CCK_RATE11M] = DESC92C_RATE11M,
+	.maps[RTL_RC_OFDM_RATE6M] = DESC92C_RATE6M,
+	.maps[RTL_RC_OFDM_RATE9M] = DESC92C_RATE9M,
+	.maps[RTL_RC_OFDM_RATE12M] = DESC92C_RATE12M,
+	.maps[RTL_RC_OFDM_RATE18M] = DESC92C_RATE18M,
+	.maps[RTL_RC_OFDM_RATE24M] = DESC92C_RATE24M,
+	.maps[RTL_RC_OFDM_RATE36M] = DESC92C_RATE36M,
+	.maps[RTL_RC_OFDM_RATE48M] = DESC92C_RATE48M,
+	.maps[RTL_RC_OFDM_RATE54M] = DESC92C_RATE54M,
+
+	.maps[RTL_RC_HT_RATEMCS7] = DESC92C_RATEMCS7,
+	.maps[RTL_RC_HT_RATEMCS15] = DESC92C_RATEMCS15,
 };
 
-static struct pci_device_id rtl8723ae_pci_ids[] = {
-	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8723, rtl8723ae_hal_cfg)},
+static struct pci_device_id rtl8723e_pci_ids[] = {
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8723, rtl8723e_hal_cfg)},
 	{},
 };
 
-MODULE_DEVICE_TABLE(pci, rtl8723ae_pci_ids);
+MODULE_DEVICE_TABLE(pci, rtl8723e_pci_ids);
 
 MODULE_AUTHOR("lizhaoming	<chaoming_li@realsil.com.cn>");
 MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
-MODULE_AUTHOR("Larry Finger	<Larry.Finger@lwfinger.net>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Realtek 8723E 802.11n PCI wireless");
-MODULE_FIRMWARE("rtlwifi/rtl8723fw.bin");
-MODULE_FIRMWARE("rtlwifi/rtl8723fw_B.bin");
-
-module_param_named(swenc, rtl8723ae_mod_params.sw_crypto, bool, 0444);
-module_param_named(debug, rtl8723ae_mod_params.debug, int, 0444);
-module_param_named(ips, rtl8723ae_mod_params.inactiveps, bool, 0444);
-module_param_named(swlps, rtl8723ae_mod_params.swctrl_lps, bool, 0444);
-module_param_named(fwlps, rtl8723ae_mod_params.fwctrl_lps, bool, 0444);
+MODULE_FIRMWARE("rtlwifi/rtl8723efw.bin");
+
+module_param_named(swenc, rtl8723e_mod_params.sw_crypto, bool, 0444);
+module_param_named(debug, rtl8723e_mod_params.debug, int, 0444);
+module_param_named(ips, rtl8723e_mod_params.inactiveps, bool, 0444);
+module_param_named(swlps, rtl8723e_mod_params.swctrl_lps, bool, 0444);
+module_param_named(fwlps, rtl8723e_mod_params.fwctrl_lps, bool, 0444);
+module_param_named(disable_watchdog, rtl8723e_mod_params.disable_watchdog,
+		   bool, 0444);
 MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
 MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
 MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
 MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+MODULE_PARM_DESC(disable_watchdog, "Set to 1 to disable the watchdog (default 0)\n");
 
 static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
-static struct pci_driver rtl8723ae_driver = {
+static struct pci_driver rtl8723e_driver = {
 	.name = KBUILD_MODNAME,
-	.id_table = rtl8723ae_pci_ids,
+	.id_table = rtl8723e_pci_ids,
 	.probe = rtl_pci_probe,
 	.remove = rtl_pci_disconnect,
 	.driver.pm = &rtlwifi_pm_ops,
 };
 
-module_pci_driver(rtl8723ae_driver);
+module_pci_driver(rtl8723e_driver);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.h b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.h
index fc4fde5e3eb5..46478780d262 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/sw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/sw.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,8 +26,10 @@
 #ifndef __RTL8723E_SW_H__
 #define __RTL8723E_SW_H__
 
-int rtl8723ae_init_sw_vars(struct ieee80211_hw *hw);
-void rtl8723ae_deinit_sw_vars(struct ieee80211_hw *hw);
-void rtl8723ae_init_var_map(struct ieee80211_hw *hw);
+int rtl8723e_init_sw_vars(struct ieee80211_hw *hw);
+void rtl8723e_deinit_sw_vars(struct ieee80211_hw *hw);
+void rtl8723e_init_var_map(struct ieee80211_hw *hw);
+bool rtl8723e_get_btc_status(void);
+
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/table.c b/drivers/net/wireless/rtlwifi/rtl8723ae/table.c
index 9b0b50cc4ade..61e86045f15c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/table.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/table.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -335,7 +331,7 @@ u32 RTL8723EPHY_REG_ARRAY_PG[RTL8723E_PHY_REG_ARRAY_PGLENGTH] = {
 	0x868, 0xffffffff, 0x00000000,
 };
 
-u32 RTL8723E_RADIOA_1TARRAY[Rtl8723ERADIOA_1TARRAYLENGTH] = {
+u32 RTL8723E_RADIOA_1TARRAY[RTL8723ERADIOA_1TARRAYLENGTH] = {
 	0x000, 0x00030159,
 	0x001, 0x00031284,
 	0x002, 0x00098000,
@@ -479,12 +475,10 @@ u32 RTL8723E_RADIOA_1TARRAY[Rtl8723ERADIOA_1TARRAYLENGTH] = {
 	0x000, 0x00030159,
 };
 
-
 u32 RTL8723E_RADIOB_1TARRAY[RTL8723E_RADIOB_1TARRAYLENGTH] = {
 	0x0,
 };
 
-
 u32 RTL8723EMAC_ARRAY[RTL8723E_MACARRAYLENGTH] = {
 	0x420, 0x00000080,
 	0x423, 0x00000000,
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/table.h b/drivers/net/wireless/rtlwifi/rtl8723ae/table.h
index f5ce71375c20..57a548ceba7d 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/table.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/table.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -38,8 +34,8 @@
 extern u32 RTL8723EPHY_REG_1TARRAY[RTL8723E_PHY_REG_1TARRAY_LENGTH];
 #define RTL8723E_PHY_REG_ARRAY_PGLENGTH		336
 extern u32 RTL8723EPHY_REG_ARRAY_PG[RTL8723E_PHY_REG_ARRAY_PGLENGTH];
-#define Rtl8723ERADIOA_1TARRAYLENGTH		 282
-extern u32 RTL8723E_RADIOA_1TARRAY[Rtl8723ERADIOA_1TARRAYLENGTH];
+#define RTL8723ERADIOA_1TARRAYLENGTH		282
+extern u32 RTL8723E_RADIOA_1TARRAY[RTL8723ERADIOA_1TARRAYLENGTH];
 #define RTL8723E_RADIOB_1TARRAYLENGTH		1
 extern u32 RTL8723E_RADIOB_1TARRAY[RTL8723E_RADIOB_1TARRAYLENGTH];
 #define RTL8723E_MACARRAYLENGTH			172
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
index 10b7577b6ae5..d372ccaf3465 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -37,7 +33,7 @@
 #include "trx.h"
 #include "led.h"
 
-static u8 _rtl8723ae_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
+static u8 _rtl8723e_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 {
 	__le16 fc = rtl_get_fc(skb);
 
@@ -49,16 +45,174 @@ static u8 _rtl8723ae_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 	return skb->priority;
 }
 
-static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
-			struct rtl_stats *pstatus, u8 *pdesc,
-			struct rx_fwinfo_8723e *p_drvinfo,
-			bool bpacket_match_bssid,
-			bool bpacket_toself, bool packet_beacon)
+/* mac80211's rate_idx is like this:
+ *
+ * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
+ *
+ * B/G rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC92C_RATE1M-->DESC92C_RATE54M ==> idx is 0-->11,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
+ *
+ * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
+ * A rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC92C_RATE6M-->DESC92C_RATE54M ==> idx is 0-->7,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC92C_RATEMCS0-->DESC92C_RATEMCS15 ==> idx is 0-->15
+ */
+static int _rtl8723e_rate_mapping(struct ieee80211_hw *hw,
+				  bool isht, u8 desc_rate)
+{
+	int rate_idx;
+
+	if (!isht) {
+		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
+			switch (desc_rate) {
+			case DESC92C_RATE1M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE2M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE5_5M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE11M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE6M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 7;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 8;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 9;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 10;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 11;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		} else {
+			switch (desc_rate) {
+			case DESC92C_RATE6M:
+				rate_idx = 0;
+				break;
+			case DESC92C_RATE9M:
+				rate_idx = 1;
+				break;
+			case DESC92C_RATE12M:
+				rate_idx = 2;
+				break;
+			case DESC92C_RATE18M:
+				rate_idx = 3;
+				break;
+			case DESC92C_RATE24M:
+				rate_idx = 4;
+				break;
+			case DESC92C_RATE36M:
+				rate_idx = 5;
+				break;
+			case DESC92C_RATE48M:
+				rate_idx = 6;
+				break;
+			case DESC92C_RATE54M:
+				rate_idx = 7;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		}
+	} else {
+		switch (desc_rate) {
+		case DESC92C_RATEMCS0:
+			rate_idx = 0;
+			break;
+		case DESC92C_RATEMCS1:
+			rate_idx = 1;
+			break;
+		case DESC92C_RATEMCS2:
+			rate_idx = 2;
+			break;
+		case DESC92C_RATEMCS3:
+			rate_idx = 3;
+			break;
+		case DESC92C_RATEMCS4:
+			rate_idx = 4;
+			break;
+		case DESC92C_RATEMCS5:
+			rate_idx = 5;
+			break;
+		case DESC92C_RATEMCS6:
+			rate_idx = 6;
+			break;
+		case DESC92C_RATEMCS7:
+			rate_idx = 7;
+			break;
+		case DESC92C_RATEMCS8:
+			rate_idx = 8;
+			break;
+		case DESC92C_RATEMCS9:
+			rate_idx = 9;
+			break;
+		case DESC92C_RATEMCS10:
+			rate_idx = 10;
+			break;
+		case DESC92C_RATEMCS11:
+			rate_idx = 11;
+			break;
+		case DESC92C_RATEMCS12:
+			rate_idx = 12;
+			break;
+		case DESC92C_RATEMCS13:
+			rate_idx = 13;
+			break;
+		case DESC92C_RATEMCS14:
+			rate_idx = 14;
+			break;
+		case DESC92C_RATEMCS15:
+			rate_idx = 15;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+	}
+	return rate_idx;
+}
+
+static void _rtl8723e_query_rxphystatus(struct ieee80211_hw *hw,
+					struct rtl_stats *pstatus, u8 *pdesc,
+					struct rx_fwinfo_8723e *p_drvinfo,
+					bool bpacket_match_bssid,
+					bool bpacket_toself, bool packet_beacon)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
 	struct phy_sts_cck_8723e_t *cck_buf;
-	s8 rx_pwr_all, rx_pwr[4];
+	s8 rx_pwr_all = 0, rx_pwr[4];
 	u8 rf_rx_num = 0, evm, pwdb_all;
 	u8 i, max_spatial_stream;
 	u32 rssi, total_rssi = 0;
@@ -68,8 +222,8 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 	pstatus->packet_matchbssid = bpacket_match_bssid;
 	pstatus->packet_toself = bpacket_toself;
 	pstatus->packet_beacon = packet_beacon;
-	pstatus->rx_mimo_sig_qual[0] = -1;
-	pstatus->rx_mimo_sig_qual[1] = -1;
+	pstatus->rx_mimo_signalquality[0] = -1;
+	pstatus->rx_mimo_signalquality[1] = -1;
 
 	if (is_cck) {
 		u8 report, cck_highpwr;
@@ -77,14 +231,14 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 		/* CCK Driver info Structure is not the same as OFDM packet. */
 		cck_buf = (struct phy_sts_cck_8723e_t *)p_drvinfo;
 
-		/* (1)Hardware does not provide RSSI for CCK
-		 * (2)PWDB, Average PWDB cacluated by
+		/* (1)Hardware does not provide RSSI for CCK */
+		/* (2)PWDB, Average PWDB cacluated by
 		 * hardware (for rate adaptive)
 		 */
 		if (ppsc->rfpwr_state == ERFON)
-			cck_highpwr = (u8) rtl_get_bbreg(hw,
-						 RFPGA0_XA_HSSIPARAMETER2,
-						 BIT(9));
+			cck_highpwr = (u8)rtl_get_bbreg(hw,
+					RFPGA0_XA_HSSIPARAMETER2,
+					BIT(9));
 		else
 			cck_highpwr = false;
 
@@ -127,8 +281,9 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 		}
 
 		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
-		/* CCK gain is smaller than OFDM/MCS gain,
-		 * so we add gain diff. From experience, the val is 6
+		/* CCK gain is smaller than OFDM/MCS gain,  */
+		/* so we add gain diff by experiences,
+		 * the val is 6
 		 */
 		pwdb_all += 6;
 		if (pwdb_all > 100)
@@ -152,9 +307,9 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 		if (bpacket_match_bssid) {
 			u8 sq;
 
-			if (pstatus->rx_pwdb_all > 40) {
+			if (pstatus->rx_pwdb_all > 40)
 				sq = 100;
-			} else {
+			else {
 				sq = cck_buf->sq_rpt;
 				if (sq > 64)
 					sq = 0;
@@ -165,8 +320,8 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 			}
 
 			pstatus->signalquality = sq;
-			pstatus->rx_mimo_sig_qual[0] = sq;
-			pstatus->rx_mimo_sig_qual[1] = -1;
+			pstatus->rx_mimo_signalquality[0] = sq;
+			pstatus->rx_mimo_signalquality[1] = -1;
 		}
 	} else {
 		rtlpriv->dm.rfpath_rxenable[0] =
@@ -179,18 +334,20 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 			if (rtlpriv->dm.rfpath_rxenable[i])
 				rf_rx_num++;
 
-			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] & 0x3f)*2) - 110;
+			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] &
+				      0x3f) * 2) - 110;
 
 			/* Translate DBM to percentage. */
 			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
 			total_rssi += rssi;
 
 			/* Get Rx snr value in DB */
-			rtlpriv->stats.rx_snr_db[i] = (p_drvinfo->rxsnr[i] / 2);
+			rtlpriv->stats.rx_snr_db[i] =
+				(long)(p_drvinfo->rxsnr[i] / 2);
 
 			/* Record Signal Strength for next packet */
 			if (bpacket_match_bssid)
-				pstatus->rx_mimo_signalstrength[i] = (u8) rssi;
+				pstatus->rx_mimo_signalstrength[i] = (u8)rssi;
 		}
 
 		/* (2)PWDB, Average PWDB cacluated by
@@ -204,8 +361,8 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 		pstatus->recvsignalpower = rx_pwr_all;
 
 		/* (3)EVM of HT rate */
-		if (pstatus->is_ht && pstatus->rate >= DESC92_RATEMCS8 &&
-		    pstatus->rate <= DESC92_RATEMCS15)
+		if (pstatus->is_ht && pstatus->rate >= DESC92C_RATEMCS8 &&
+		    pstatus->rate <= DESC92C_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
@@ -218,8 +375,10 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 				 * spatial stream only
 				 */
 				if (i == 0)
-					pstatus->signalquality = (evm & 0xff);
-				pstatus->rx_mimo_sig_qual[i] = (evm & 0xff);
+					pstatus->signalquality =
+						(u8)(evm & 0xff);
+				pstatus->rx_mimo_signalquality[i] =
+					(u8)(evm & 0xff);
 			}
 		}
 	}
@@ -235,78 +394,83 @@ static void _rtl8723ae_query_rxphystatus(struct ieee80211_hw *hw,
 			total_rssi /= rf_rx_num));
 }
 
-static void _rtl8723ae_translate_rx_signal_stuff(struct ieee80211_hw *hw,
-		struct sk_buff *skb, struct rtl_stats *pstatus,
-		u8 *pdesc, struct rx_fwinfo_8723e *p_drvinfo)
+static void translate_rx_signal_stuff(struct ieee80211_hw *hw,
+				      struct sk_buff *skb,
+				      struct rtl_stats *pstatus, u8 *pdesc,
+				      struct rx_fwinfo_8723e *p_drvinfo)
 {
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	struct ieee80211_hdr *hdr;
 	u8 *tmp_buf;
 	u8 *praddr;
-	__le16 fc;
-	u16 type;
-	bool packet_matchbssid, packet_toself, packet_beacon = false;
+	/*u8 *psaddr;*/
+	u16 fc, type;
+	bool packet_matchbssid, packet_toself, packet_beacon;
 
 	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
 
 	hdr = (struct ieee80211_hdr *)tmp_buf;
-	fc = hdr->frame_control;
-	type = WLAN_FC_GET_TYPE(fc);
+	fc = le16_to_cpu(hdr->frame_control);
+	type = WLAN_FC_GET_TYPE(hdr->frame_control);
 	praddr = hdr->addr1;
 
-	packet_matchbssid =
-		((IEEE80211_FTYPE_CTL != type) &&
-		 ether_addr_equal(mac->bssid,
-				  (le16_to_cpu(fc) & IEEE80211_FCTL_TODS) ? hdr->addr1 :
-				  (le16_to_cpu(fc) & IEEE80211_FCTL_FROMDS) ? hdr->addr2 :
-				  hdr->addr3) &&
-		 (!pstatus->hwerror) && (!pstatus->crc) && (!pstatus->icv));
+	packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
+		(ether_addr_equal(mac->bssid, (fc & IEEE80211_FCTL_TODS) ?
+		 hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ?
+		 hdr->addr2 : hdr->addr3)) &&
+		 (!pstatus->hwerror) &&
+		 (!pstatus->crc) && (!pstatus->icv));
 
-	packet_toself = (packet_matchbssid &&
-			 ether_addr_equal(praddr, rtlefuse->dev_addr));
+	packet_toself = packet_matchbssid &&
+	    (ether_addr_equal(praddr, rtlefuse->dev_addr));
 
-	if (ieee80211_is_beacon(fc))
+	if (ieee80211_is_beacon(hdr->frame_control))
 		packet_beacon = true;
+	else
+		packet_beacon = false;
 
-	_rtl8723ae_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
-				   packet_matchbssid, packet_toself,
-				   packet_beacon);
+	_rtl8723e_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
+				    packet_matchbssid, packet_toself,
+				    packet_beacon);
 
 	rtl_process_phyinfo(hw, tmp_buf, pstatus);
 }
 
-bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
-			     struct rtl_stats *status,
-			     struct ieee80211_rx_status *rx_status,
-			     u8 *pdesc, struct sk_buff *skb)
+bool rtl8723e_rx_query_desc(struct ieee80211_hw *hw,
+			    struct rtl_stats *status,
+			    struct ieee80211_rx_status *rx_status,
+			    u8 *pdesc, struct sk_buff *skb)
 {
 	struct rx_fwinfo_8723e *p_drvinfo;
 	struct ieee80211_hdr *hdr;
 	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
 
-	status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
-	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
-				   RX_DRV_INFO_SIZE_UNIT;
-	status->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
-	status->icv = (u16) GET_RX_DESC_ICV(pdesc);
-	status->crc = (u16) GET_RX_DESC_CRC32(pdesc);
+	status->length = (u16)GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8)GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+	    RX_DRV_INFO_SIZE_UNIT;
+	status->rx_bufshift = (u8)(GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->icv = (u16)GET_RX_DESC_ICV(pdesc);
+	status->crc = (u16)GET_RX_DESC_CRC32(pdesc);
 	status->hwerror = (status->crc | status->icv);
 	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
-	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
-	status->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
-	status->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	status->isfirst_ampdu = (bool) ((GET_RX_DESC_PAGGR(pdesc) == 1)
-				 && (GET_RX_DESC_FAGGR(pdesc) == 1));
+	status->rate = (u8)GET_RX_DESC_RXMCS(pdesc);
+	status->shortpreamble = (u16)GET_RX_DESC_SPLCP(pdesc);
+	status->isampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->isfirst_ampdu = (bool)((GET_RX_DESC_PAGGR(pdesc) == 1) &&
+				       (GET_RX_DESC_FAGGR(pdesc) == 1));
 	status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
-	status->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	status->rx_is40Mhzpacket = (bool)GET_RX_DESC_BW(pdesc);
 	status->is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
 
-	status->is_cck = RTL8723E_RX_HAL_IS_CCK_RATE(status->rate);
+	status->is_cck = RX_HAL_IS_CCK_RATE(status->rate);
 
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
 
+	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size
+			+ status->rx_bufshift);
+
 	if (status->crc)
 		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
 
@@ -320,69 +484,62 @@ bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
 
 	/* hw will set status->decrypted true, if it finds the
 	 * frame is open data frame or mgmt frame.
-	 * Thus hw will not decrypt a robust managment frame
+	 * So hw will not decryption robust managment frame
 	 * for IEEE80211w but still set status->decrypted
 	 * true, so here we should set it back to undecrypted
 	 * for IEEE80211w frame, and mac80211 sw will help
 	 * to decrypt it
 	 */
 	if (status->decrypted) {
-		hdr = (struct ieee80211_hdr *)(skb->data +
-		       status->rx_drvinfo_size + status->rx_bufshift);
-
-		if (!hdr) {
-			/* during testing, hdr could be NULL here */
-			return false;
-		}
-		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
-			(ieee80211_has_protected(hdr->frame_control)))
-			rx_status->flag &= ~RX_FLAG_DECRYPTED;
-		else
+		if ((!_ieee80211_is_robust_mgmt_frame(hdr)) &&
+		    (ieee80211_has_protected(hdr->frame_control)))
 			rx_status->flag |= RX_FLAG_DECRYPTED;
+		else
+			rx_status->flag &= ~RX_FLAG_DECRYPTED;
 	}
 
 	/* rate_idx: index of data rate into band's
 	 * supported rates or MCS index if HT rates
 	 * are use (RX_FLAG_HT)
+	 * Notice: this is diff with windows define
 	 */
-	rx_status->rate_idx = rtlwifi_rate_mapping(hw, status->is_ht,
-						   status->rate, false);
+	rx_status->rate_idx = _rtl8723e_rate_mapping(hw,
+				status->is_ht, status->rate);
 
 	rx_status->mactime = status->timestamp_low;
 	if (phystatus == true) {
 		p_drvinfo = (struct rx_fwinfo_8723e *)(skb->data +
-			     status->rx_bufshift);
+						     status->rx_bufshift);
 
-		_rtl8723ae_translate_rx_signal_stuff(hw,
-			   skb, status, pdesc, p_drvinfo);
+		translate_rx_signal_stuff(hw, skb, status, pdesc, p_drvinfo);
 	}
-
-	/*rx_status->qual = status->signal; */
 	rx_status->signal = status->recvsignalpower + 10;
-
 	return true;
 }
 
-void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
-			    struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
-			    struct ieee80211_sta *sta,
-			    struct sk_buff *skb, u8 hw_queue,
-			    struct rtl_tcb_desc *ptcdesc)
+void rtl8723e_tx_fill_desc(struct ieee80211_hw *hw,
+			   struct ieee80211_hdr *hdr, u8 *pdesc_tx,
+			   u8 *txbd, struct ieee80211_tx_info *info,
+			   struct ieee80211_sta *sta,
+			   struct sk_buff *skb,
+			   u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool defaultadapter = true;
-	u8 *pdesc = pdesc_tx;
+	bool b_defaultadapter = true;
+	/* bool b_trigger_ac = false; */
+	u8 *pdesc = (u8 *)pdesc_tx;
 	u16 seq_number;
 	__le16 fc = hdr->frame_control;
-	u8 fw_qsel = _rtl8723ae_map_hwqueue_to_fwqueue(skb, hw_queue);
+	u8 fw_qsel = _rtl8723e_map_hwqueue_to_fwqueue(skb, hw_queue);
 	bool firstseg = ((hdr->seq_ctrl &
-			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+			  cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+
 	bool lastseg = ((hdr->frame_control &
-			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+			 cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+
 	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
 					    skb->data, skb->len,
 					    PCI_DMA_TODEVICE);
@@ -398,12 +555,13 @@ void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 	} else if (mac->opmode == NL80211_IFTYPE_AP ||
 		mac->opmode == NL80211_IFTYPE_ADHOC) {
 		if (sta)
-			bw_40 = sta->bandwidth >= IEEE80211_STA_RX_BW_40;
+			bw_40 = sta->ht_cap.cap &
+				IEEE80211_HT_CAP_SUP_WIDTH_20_40;
 	}
 
 	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
 
-	rtl_get_tcb_desc(hw, info, sta, skb, ptcdesc);
+	rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
 
 	CLEAR_PCI_TX_DESC_CONTENT(pdesc, sizeof(struct tx_desc_8723e));
 
@@ -415,9 +573,9 @@ void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 	if (firstseg) {
 		SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 
-		SET_TX_DESC_TX_RATE(pdesc, ptcdesc->hw_rate);
+		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
 
-		if (ptcdesc->use_shortgi || ptcdesc->use_shortpreamble)
+		if (ptcb_desc->use_shortgi || ptcb_desc->use_shortpreamble)
 			SET_TX_DESC_DATA_SHORTGI(pdesc, 1);
 
 		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
@@ -426,31 +584,33 @@ void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 		}
 		SET_TX_DESC_SEQ(pdesc, seq_number);
 
-		SET_TX_DESC_RTS_ENABLE(pdesc, ((ptcdesc->rts_enable &&
-						!ptcdesc->
-						cts_enable) ? 1 : 0));
+		SET_TX_DESC_RTS_ENABLE(pdesc,
+				       ((ptcb_desc->rts_enable &&
+					!ptcb_desc->cts_enable) ? 1 : 0));
 		SET_TX_DESC_HW_RTS_ENABLE(pdesc,
-					  ((ptcdesc->rts_enable
-					    || ptcdesc->cts_enable) ? 1 : 0));
-		SET_TX_DESC_CTS2SELF(pdesc, ((ptcdesc->cts_enable) ? 1 : 0));
-		SET_TX_DESC_RTS_STBC(pdesc, ((ptcdesc->rts_stbc) ? 1 : 0));
-
-		SET_TX_DESC_RTS_RATE(pdesc, ptcdesc->rts_rate);
+					  ((ptcb_desc->rts_enable ||
+					  ptcb_desc->cts_enable) ? 1 : 0));
+		SET_TX_DESC_CTS2SELF(pdesc,
+				     ((ptcb_desc->cts_enable) ? 1 : 0));
+		SET_TX_DESC_RTS_STBC(pdesc,
+				     ((ptcb_desc->rts_stbc) ? 1 : 0));
+
+		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
 		SET_TX_DESC_RTS_BW(pdesc, 0);
-		SET_TX_DESC_RTS_SC(pdesc, ptcdesc->rts_sc);
+		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
 		SET_TX_DESC_RTS_SHORT(pdesc,
-				      ((ptcdesc->rts_rate <= DESC92_RATE54M) ?
-				       (ptcdesc->rts_use_shortpreamble ? 1 : 0)
-				       : (ptcdesc->rts_use_shortgi ? 1 : 0)));
+				((ptcb_desc->rts_rate <= DESC92C_RATE54M) ?
+				(ptcb_desc->rts_use_shortpreamble ? 1 : 0)
+				: (ptcb_desc->rts_use_shortgi ? 1 : 0)));
 
 		if (bw_40) {
-			if (ptcdesc->packet_bw) {
+			if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40) {
 				SET_TX_DESC_DATA_BW(pdesc, 1);
 				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
 			} else {
 				SET_TX_DESC_DATA_BW(pdesc, 0);
 				SET_TX_DESC_TX_SUB_CARRIER(pdesc,
-							 mac->cur_40_prime_sc);
+					mac->cur_40_prime_sc);
 			}
 		} else {
 			SET_TX_DESC_DATA_BW(pdesc, 0);
@@ -481,6 +641,7 @@ void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 			default:
 				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
 				break;
+
 			}
 		}
 
@@ -490,7 +651,7 @@ void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
 		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
 		SET_TX_DESC_DISABLE_FB(pdesc, 0);
-		SET_TX_DESC_USE_RATE(pdesc, ptcdesc->use_driver_rate ? 1 : 0);
+		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
 
 		if (ieee80211_is_data_qos(fc)) {
 			if (mac->rdg_en) {
@@ -510,18 +671,21 @@ void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
 
 	if (rtlpriv->dm.useramask) {
-		SET_TX_DESC_RATE_ID(pdesc, ptcdesc->ratr_index);
-		SET_TX_DESC_MACID(pdesc, ptcdesc->mac_id);
+		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
 	} else {
-		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcdesc->ratr_index);
-		SET_TX_DESC_MACID(pdesc, ptcdesc->ratr_index);
+		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->ratr_index);
 	}
 
 	if ((!ieee80211_is_data_qos(fc)) && ppsc->fwctrl_lps) {
 		SET_TX_DESC_HWSEQ_EN_8723(pdesc, 1);
+		/* SET_TX_DESC_HWSEQ_EN(pdesc, 1); */
+		/* SET_TX_DESC_PKT_ID(pdesc, 8); */
 
-		if (!defaultadapter)
+		if (!b_defaultadapter)
 			SET_TX_DESC_HWSEQ_SEL_8723(pdesc, 1);
+	/* SET_TX_DESC_QOS(pdesc, 1); */
 	}
 
 	SET_TX_DESC_MORE_FRAG(pdesc, (lastseg ? 0 : 1));
@@ -534,17 +698,19 @@ void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "\n");
 }
 
-void rtl8723ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
+void rtl8723e_tx_fill_cmddesc(struct ieee80211_hw *hw,
 			      u8 *pdesc, bool firstseg,
 			      bool lastseg, struct sk_buff *skb)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
 	u8 fw_queue = QSLT_BEACON;
+
 	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
 					    skb->data, skb->len,
 					    PCI_DMA_TODEVICE);
+
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(skb->data);
 	__le16 fc = hdr->frame_control;
 
 	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
@@ -557,7 +723,7 @@ void rtl8723ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
 	if (firstseg)
 		SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
 
-	SET_TX_DESC_TX_RATE(pdesc, DESC92_RATE1M);
+	SET_TX_DESC_TX_RATE(pdesc, DESC92C_RATE1M);
 
 	SET_TX_DESC_SEQ(pdesc, 0);
 
@@ -577,7 +743,7 @@ void rtl8723ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
 
 	SET_TX_DESC_OWN(pdesc, 1);
 
-	SET_TX_DESC_PKT_SIZE(pdesc, (u16) (skb->len));
+	SET_TX_DESC_PKT_SIZE((u8 *)pdesc, (u16)(skb->len));
 
 	SET_TX_DESC_FIRST_SEG(pdesc, 1);
 	SET_TX_DESC_LAST_SEG(pdesc, 1);
@@ -597,8 +763,8 @@ void rtl8723ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
 		      pdesc, TX_DESC_SIZE);
 }
 
-void rtl8723ae_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-			u8 desc_name, u8 *val)
+void rtl8723e_set_desc(struct ieee80211_hw *hw, u8 *pdesc,
+		       bool istx, u8 desc_name, u8 *val)
 {
 	if (istx == true) {
 		switch (desc_name) {
@@ -635,7 +801,7 @@ void rtl8723ae_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 	}
 }
 
-u32 rtl8723ae_get_desc(u8 *pdesc, bool istx, u8 desc_name)
+u32 rtl8723e_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 {
 	u32 ret = 0;
 
@@ -660,6 +826,9 @@ u32 rtl8723ae_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 		case HW_DESC_RXPKT_LEN:
 			ret = GET_RX_DESC_PKT_LEN(pdesc);
 			break;
+		case HW_DESC_RXBUFF_ADDR:
+			ret = GET_RX_DESC_BUFF_ADDR(pdesc);
+			break;
 		default:
 			RT_ASSERT(false, "ERR rxdesc :%d not process\n",
 				  desc_name);
@@ -669,7 +838,25 @@ u32 rtl8723ae_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 	return ret;
 }
 
-void rtl8723ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
+bool rtl8723e_is_tx_desc_closed(struct ieee80211_hw *hw,
+				u8 hw_queue, u16 index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+	u8 *entry = (u8 *)(&ring->desc[ring->idx]);
+	u8 own = (u8)rtl8723e_get_desc(entry, true, HW_DESC_OWN);
+
+	/**
+	 *beacon packet will only use the first
+	 *descriptor defautly,and the own may not
+	 *be cleared by the hardware
+	 */
+	if (own)
+		return false;
+	return true;
+}
+
+void rtl8723e_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	if (hw_queue == BEACON_QUEUE) {
@@ -679,3 +866,10 @@ void rtl8723ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
 			       BIT(0) << (hw_queue));
 	}
 }
+
+u32 rtl8723e_rx_command_packet(struct ieee80211_hw *hw,
+			       struct rtl_stats status,
+			       struct sk_buff *skb)
+{
+	return 0;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h
index 4380b7d3a91a..017da7e194d8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723ae/trx.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,77 +26,77 @@
 #ifndef __RTL8723E_TRX_H__
 #define __RTL8723E_TRX_H__
 
-#define TX_DESC_SIZE				64
+#define TX_DESC_SIZE					64
 #define TX_DESC_AGGR_SUBFRAME_SIZE		32
 
-#define RX_DESC_SIZE				32
+#define RX_DESC_SIZE					32
 #define RX_DRV_INFO_SIZE_UNIT			8
 
 #define	TX_DESC_NEXT_DESC_OFFSET		40
 #define USB_HWDESC_HEADER_LEN			32
-#define CRCLENGTH				4
+#define CRCLENGTH						4
 
 #define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
-#define SET_TX_DESC_OFFSET(__pdesc, __val)		\
+#define SET_TX_DESC_OFFSET(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 8, __val)
-#define SET_TX_DESC_BMC(__pdesc, __val)		\
+#define SET_TX_DESC_BMC(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 24, 1, __val)
-#define SET_TX_DESC_HTC(__pdesc, __val)		\
+#define SET_TX_DESC_HTC(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 25, 1, __val)
 #define SET_TX_DESC_LAST_SEG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 26, 1, __val)
 #define SET_TX_DESC_FIRST_SEG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 27, 1, __val)
-#define SET_TX_DESC_LINIP(__pdesc, __val)		\
+#define SET_TX_DESC_LINIP(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 28, 1, __val)
-#define SET_TX_DESC_NO_ACM(__pdesc, __val)		\
+#define SET_TX_DESC_NO_ACM(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 29, 1, __val)
-#define SET_TX_DESC_GF(__pdesc, __val)			\
+#define SET_TX_DESC_GF(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
-#define SET_TX_DESC_OWN(__pdesc, __val)		\
+#define SET_TX_DESC_OWN(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
 
-#define GET_TX_DESC_PKT_SIZE(__pdesc)			\
+#define GET_TX_DESC_PKT_SIZE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 0, 16)
-#define GET_TX_DESC_OFFSET(__pdesc)			\
+#define GET_TX_DESC_OFFSET(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 16, 8)
-#define GET_TX_DESC_BMC(__pdesc)			\
+#define GET_TX_DESC_BMC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 24, 1)
-#define GET_TX_DESC_HTC(__pdesc)			\
+#define GET_TX_DESC_HTC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 25, 1)
-#define GET_TX_DESC_LAST_SEG(__pdesc)			\
+#define GET_TX_DESC_LAST_SEG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
-#define GET_TX_DESC_FIRST_SEG(__pdesc)			\
+#define GET_TX_DESC_FIRST_SEG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
-#define GET_TX_DESC_LINIP(__pdesc)			\
+#define GET_TX_DESC_LINIP(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
-#define GET_TX_DESC_NO_ACM(__pdesc)			\
+#define GET_TX_DESC_NO_ACM(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
-#define GET_TX_DESC_GF(__pdesc)				\
+#define GET_TX_DESC_GF(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
-#define GET_TX_DESC_OWN(__pdesc)			\
+#define GET_TX_DESC_OWN(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
 
-#define SET_TX_DESC_MACID(__pdesc, __val)		\
+#define SET_TX_DESC_MACID(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 5, __val)
 #define SET_TX_DESC_AGG_BREAK(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 5, 1, __val)
-#define SET_TX_DESC_BK(__pdesc, __val)			\
+#define SET_TX_DESC_BK(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 6, 1, __val)
 #define SET_TX_DESC_RDG_ENABLE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 7, 1, __val)
 #define SET_TX_DESC_QUEUE_SEL(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 8, 5, __val)
-#define SET_TX_DESC_RDG_NAV_EXT(__pdesc, __val)	\
+#define SET_TX_DESC_RDG_NAV_EXT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 13, 1, __val)
 #define SET_TX_DESC_LSIG_TXOP_EN(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 14, 1, __val)
-#define SET_TX_DESC_PIFS(__pdesc, __val)		\
+#define SET_TX_DESC_PIFS(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 15, 1, __val)
 #define SET_TX_DESC_RATE_ID(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 16, 4, __val)
-#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)	\
+#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 20, 1, __val)
 #define SET_TX_DESC_EN_DESC_ID(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 21, 1, __val)
@@ -109,34 +105,34 @@
 #define SET_TX_DESC_PKT_OFFSET(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 24, 8, __val)
 
-#define GET_TX_DESC_MACID(__pdesc)			\
+#define GET_TX_DESC_MACID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
-#define GET_TX_DESC_AGG_ENABLE(__pdesc)		\
+#define GET_TX_DESC_AGG_ENABLE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 5, 1)
-#define GET_TX_DESC_AGG_BREAK(__pdesc)			\
+#define GET_TX_DESC_AGG_BREAK(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 6, 1)
-#define GET_TX_DESC_RDG_ENABLE(__pdesc)		\
+#define GET_TX_DESC_RDG_ENABLE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 7, 1)
-#define GET_TX_DESC_QUEUE_SEL(__pdesc)			\
+#define GET_TX_DESC_QUEUE_SEL(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 8, 5)
-#define GET_TX_DESC_RDG_NAV_EXT(__pdesc)		\
+#define GET_TX_DESC_RDG_NAV_EXT(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
-#define GET_TX_DESC_LSIG_TXOP_EN(__pdesc)		\
+#define GET_TX_DESC_LSIG_TXOP_EN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
-#define GET_TX_DESC_PIFS(__pdesc)			\
+#define GET_TX_DESC_PIFS(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
-#define GET_TX_DESC_RATE_ID(__pdesc)			\
+#define GET_TX_DESC_RATE_ID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
-#define GET_TX_DESC_NAV_USE_HDR(__pdesc)		\
+#define GET_TX_DESC_NAV_USE_HDR(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
-#define GET_TX_DESC_EN_DESC_ID(__pdesc)		\
+#define GET_TX_DESC_EN_DESC_ID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
-#define GET_TX_DESC_SEC_TYPE(__pdesc)			\
+#define GET_TX_DESC_SEC_TYPE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 22, 2)
-#define GET_TX_DESC_PKT_OFFSET(__pdesc)		\
+#define GET_TX_DESC_PKT_OFFSET(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 24, 8)
 
-#define SET_TX_DESC_RTS_RC(__pdesc, __val)		\
+#define SET_TX_DESC_RTS_RC(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 6, __val)
 #define SET_TX_DESC_DATA_RC(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 6, 6, __val)
@@ -144,9 +140,9 @@
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 2, __val)
 #define SET_TX_DESC_MORE_FRAG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 17, 1, __val)
-#define SET_TX_DESC_RAW(__pdesc, __val)		\
+#define SET_TX_DESC_RAW(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
-#define SET_TX_DESC_CCX(__pdesc, __val)		\
+#define SET_TX_DESC_CCX(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
 #define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
@@ -161,62 +157,62 @@
 #define SET_TX_DESC_TX_ANT_HT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 30, 2, __val)
 
-#define GET_TX_DESC_RTS_RC(__pdesc)			\
+#define GET_TX_DESC_RTS_RC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 0, 6)
-#define GET_TX_DESC_DATA_RC(__pdesc)			\
+#define GET_TX_DESC_DATA_RC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 6, 6)
-#define GET_TX_DESC_BAR_RTY_TH(__pdesc)		\
+#define GET_TX_DESC_BAR_RTY_TH(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 14, 2)
-#define GET_TX_DESC_MORE_FRAG(__pdesc)			\
+#define GET_TX_DESC_MORE_FRAG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 17, 1)
-#define GET_TX_DESC_RAW(__pdesc)			\
+#define GET_TX_DESC_RAW(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 18, 1)
-#define GET_TX_DESC_CCX(__pdesc)			\
+#define GET_TX_DESC_CCX(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 19, 1)
-#define GET_TX_DESC_AMPDU_DENSITY(__pdesc)		\
+#define GET_TX_DESC_AMPDU_DENSITY(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+8, 20, 3)
-#define GET_TX_DESC_ANTSEL_A(__pdesc)			\
+#define GET_TX_DESC_ANTSEL_A(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 24, 1)
-#define GET_TX_DESC_ANTSEL_B(__pdesc)			\
+#define GET_TX_DESC_ANTSEL_B(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 25, 1)
-#define GET_TX_DESC_TX_ANT_CCK(__pdesc)		\
+#define GET_TX_DESC_TX_ANT_CCK(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 26, 2)
-#define GET_TX_DESC_TX_ANTL(__pdesc)			\
+#define GET_TX_DESC_TX_ANTL(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 28, 2)
-#define GET_TX_DESC_TX_ANT_HT(__pdesc)			\
+#define GET_TX_DESC_TX_ANT_HT(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 30, 2)
 
 #define SET_TX_DESC_NEXT_HEAP_PAGE(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 8, __val)
 #define SET_TX_DESC_TAIL_PAGE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 8, 8, __val)
-#define SET_TX_DESC_SEQ(__pdesc, __val)		\
+#define SET_TX_DESC_SEQ(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 16, 12, __val)
-#define SET_TX_DESC_PKT_ID(__pdesc, __val)		\
+#define SET_TX_DESC_PKT_ID(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 28, 4, __val)
 
 #define GET_TX_DESC_NEXT_HEAP_PAGE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+12, 0, 8)
-#define GET_TX_DESC_TAIL_PAGE(__pdesc)			\
+#define GET_TX_DESC_TAIL_PAGE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 8, 8)
-#define GET_TX_DESC_SEQ(__pdesc)			\
+#define GET_TX_DESC_SEQ(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 16, 12)
-#define GET_TX_DESC_PKT_ID(__pdesc)			\
+#define GET_TX_DESC_PKT_ID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 28, 4)
 
 /* For RTL8723 */
 #define SET_TX_DESC_TRIGGER_INT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 30, 1, __val)
-#define SET_TX_DESC_HWSEQ_EN_8723(__pdesc, __val)	\
+#define SET_TX_DESC_HWSEQ_EN_8723(__pdesc, __val)   \
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 31, 1, __val)
-#define SET_TX_DESC_HWSEQ_SEL_8723(__pTxDesc, __Value)	\
-	SET_BITS_TO_LE_4BYTE(__pTxDesc+16, 6, 2, __Value)
+#define SET_TX_DESC_HWSEQ_SEL_8723(__txdesc, __value) \
+	SET_BITS_TO_LE_4BYTE(__txdesc+16, 6, 2, __value)
 
 #define SET_TX_DESC_RTS_RATE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 5, __val)
 #define SET_TX_DESC_AP_DCFE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 5, 1, __val)
-#define SET_TX_DESC_QOS(__pdesc, __val)		\
+#define SET_TX_DESC_QOS(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 6, 1, __val)
 #define SET_TX_DESC_HWSEQ_EN(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 7, 1, __val)
@@ -248,54 +244,54 @@
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 25, 1, __val)
 #define SET_TX_DESC_RTS_SHORT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 26, 1, __val)
-#define SET_TX_DESC_RTS_BW(__pdesc, __val)		\
+#define SET_TX_DESC_RTS_BW(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 27, 1, __val)
-#define SET_TX_DESC_RTS_SC(__pdesc, __val)		\
+#define SET_TX_DESC_RTS_SC(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 28, 2, __val)
 #define SET_TX_DESC_RTS_STBC(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 30, 2, __val)
 
-#define GET_TX_DESC_RTS_RATE(__pdesc)			\
+#define GET_TX_DESC_RTS_RATE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 0, 5)
-#define GET_TX_DESC_AP_DCFE(__pdesc)			\
+#define GET_TX_DESC_AP_DCFE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 5, 1)
-#define GET_TX_DESC_QOS(__pdesc)			\
+#define GET_TX_DESC_QOS(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 6, 1)
-#define GET_TX_DESC_HWSEQ_EN(__pdesc)			\
+#define GET_TX_DESC_HWSEQ_EN(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 7, 1)
-#define GET_TX_DESC_USE_RATE(__pdesc)			\
+#define GET_TX_DESC_USE_RATE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 8, 1)
 #define GET_TX_DESC_DISABLE_RTS_FB(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+16, 9, 1)
-#define GET_TX_DESC_DISABLE_FB(__pdesc)		\
+#define GET_TX_DESC_DISABLE_FB(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 10, 1)
-#define GET_TX_DESC_CTS2SELF(__pdesc)			\
+#define GET_TX_DESC_CTS2SELF(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 11, 1)
-#define GET_TX_DESC_RTS_ENABLE(__pdesc)		\
+#define GET_TX_DESC_RTS_ENABLE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 12, 1)
-#define GET_TX_DESC_HW_RTS_ENABLE(__pdesc)		\
+#define GET_TX_DESC_HW_RTS_ENABLE(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+16, 13, 1)
-#define GET_TX_DESC_PORT_ID(__pdesc)			\
+#define GET_TX_DESC_PORT_ID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 14, 1)
-#define GET_TX_DESC_WAIT_DCTS(__pdesc)			\
+#define GET_TX_DESC_WAIT_DCTS(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 18, 1)
-#define GET_TX_DESC_CTS2AP_EN(__pdesc)			\
+#define GET_TX_DESC_CTS2AP_EN(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 19, 1)
 #define GET_TX_DESC_TX_SUB_CARRIER(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+16, 20, 2)
-#define GET_TX_DESC_TX_STBC(__pdesc)			\
+#define GET_TX_DESC_TX_STBC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 22, 2)
-#define GET_TX_DESC_DATA_SHORT(__pdesc)		\
+#define GET_TX_DESC_DATA_SHORT(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 24, 1)
-#define GET_TX_DESC_DATA_BW(__pdesc)			\
+#define GET_TX_DESC_DATA_BW(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 25, 1)
-#define GET_TX_DESC_RTS_SHORT(__pdesc)			\
+#define GET_TX_DESC_RTS_SHORT(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 26, 1)
-#define GET_TX_DESC_RTS_BW(__pdesc)			\
+#define GET_TX_DESC_RTS_BW(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 27, 1)
-#define GET_TX_DESC_RTS_SC(__pdesc)			\
+#define GET_TX_DESC_RTS_SC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 28, 2)
-#define GET_TX_DESC_RTS_STBC(__pdesc)			\
+#define GET_TX_DESC_RTS_STBC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 30, 2)
 
 #define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
@@ -315,17 +311,17 @@
 #define SET_TX_DESC_USB_TXAGG_NUM(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 24, 8, __val)
 
-#define GET_TX_DESC_TX_RATE(__pdesc)			\
+#define GET_TX_DESC_TX_RATE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+20, 0, 6)
-#define GET_TX_DESC_DATA_SHORTGI(__pdesc)		\
+#define GET_TX_DESC_DATA_SHORTGI(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+20, 6, 1)
-#define GET_TX_DESC_CCX_TAG(__pdesc)			\
+#define GET_TX_DESC_CCX_TAG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+20, 7, 1)
-#define GET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc)	\
+#define GET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+20, 8, 5)
 #define GET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+20, 13, 4)
-#define GET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc)	\
+#define GET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+20, 17, 1)
 #define GET_TX_DESC_DATA_RETRY_LIMIT(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+20, 18, 6)
@@ -336,9 +332,9 @@
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 5, __val)
 #define SET_TX_DESC_TXAGC_B(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 5, 5, __val)
-#define SET_TX_DESC_USE_MAX_LEN(__pdesc, __val)	\
+#define SET_TX_DESC_USE_MAX_LEN(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 10, 1, __val)
-#define SET_TX_DESC_MAX_AGG_NUM(__pdesc, __val)	\
+#define SET_TX_DESC_MAX_AGG_NUM(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 11, 5, __val)
 #define SET_TX_DESC_MCSG1_MAX_LEN(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 16, 4, __val)
@@ -349,19 +345,19 @@
 #define SET_TX_DESC_MCS7_SGI_MAX_LEN(__pdesc, __val)\
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 28, 4, __val)
 
-#define GET_TX_DESC_TXAGC_A(__pdesc)			\
+#define GET_TX_DESC_TXAGC_A(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+24, 0, 5)
-#define GET_TX_DESC_TXAGC_B(__pdesc)			\
+#define GET_TX_DESC_TXAGC_B(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+24, 5, 5)
-#define GET_TX_DESC_USE_MAX_LEN(__pdesc)		\
+#define GET_TX_DESC_USE_MAX_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+24, 10, 1)
-#define GET_TX_DESC_MAX_AGG_NUM(__pdesc)		\
+#define GET_TX_DESC_MAX_AGG_NUM(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+24, 11, 5)
-#define GET_TX_DESC_MCSG1_MAX_LEN(__pdesc)		\
+#define GET_TX_DESC_MCSG1_MAX_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+24, 16, 4)
-#define GET_TX_DESC_MCSG2_MAX_LEN(__pdesc)		\
+#define GET_TX_DESC_MCSG2_MAX_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+24, 20, 4)
-#define GET_TX_DESC_MCSG3_MAX_LEN(__pdesc)		\
+#define GET_TX_DESC_MCSG3_MAX_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+24, 24, 4)
 #define GET_TX_DESC_MCS7_SGI_MAX_LEN(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+24, 28, 4)
@@ -379,11 +375,11 @@
 
 #define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
-#define GET_TX_DESC_MCSG4_MAX_LEN(__pdesc)		\
+#define GET_TX_DESC_MCSG4_MAX_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+28, 16, 4)
-#define GET_TX_DESC_MCSG5_MAX_LEN(__pdesc)		\
+#define GET_TX_DESC_MCSG5_MAX_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+28, 20, 4)
-#define GET_TX_DESC_MCSG6_MAX_LEN(__pdesc)		\
+#define GET_TX_DESC_MCSG6_MAX_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+28, 24, 4)
 #define GET_TX_DESC_MCS15_SGI_MAX_LEN(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+28, 28, 4)
@@ -395,7 +391,7 @@
 
 #define GET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+32, 0, 32)
-#define GET_TX_DESC_TX_BUFFER_ADDRESS64(__pdesc)	\
+#define GET_TX_DESC_TX_BUFFER_ADDRESS64(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+36, 0, 32)
 
 #define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val)	\
@@ -410,97 +406,97 @@
 
 #define GET_RX_DESC_PKT_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc, 0, 14)
-#define GET_RX_DESC_CRC32(__pdesc)			\
+#define GET_RX_DESC_CRC32(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 14, 1)
-#define GET_RX_DESC_ICV(__pdesc)			\
+#define GET_RX_DESC_ICV(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 15, 1)
 #define GET_RX_DESC_DRV_INFO_SIZE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc, 16, 4)
 #define GET_RX_DESC_SECURITY(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc, 20, 3)
-#define GET_RX_DESC_QOS(__pdesc)			\
+#define GET_RX_DESC_QOS(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 23, 1)
-#define GET_RX_DESC_SHIFT(__pdesc)			\
+#define GET_RX_DESC_SHIFT(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 24, 2)
-#define GET_RX_DESC_PHYST(__pdesc)			\
+#define GET_RX_DESC_PHYST(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
-#define GET_RX_DESC_SWDEC(__pdesc)			\
+#define GET_RX_DESC_SWDEC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
-#define GET_RX_DESC_LS(__pdesc)				\
+#define GET_RX_DESC_LS(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
-#define GET_RX_DESC_FS(__pdesc)				\
+#define GET_RX_DESC_FS(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
-#define GET_RX_DESC_EOR(__pdesc)			\
+#define GET_RX_DESC_EOR(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
-#define GET_RX_DESC_OWN(__pdesc)			\
+#define GET_RX_DESC_OWN(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
 
-#define SET_RX_DESC_PKT_LEN(__pdesc, __val)		\
+#define SET_RX_DESC_PKT_LEN(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 14, __val)
 #define SET_RX_DESC_EOR(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
 #define SET_RX_DESC_OWN(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
 
-#define GET_RX_DESC_MACID(__pdesc)			\
+#define GET_RX_DESC_MACID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 0, 5)
-#define GET_RX_DESC_TID(__pdesc)			\
+#define GET_RX_DESC_TID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 5, 4)
 #define GET_RX_DESC_HWRSVD(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 9, 5)
-#define GET_RX_DESC_PAGGR(__pdesc)			\
+#define GET_RX_DESC_PAGGR(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
-#define GET_RX_DESC_FAGGR(__pdesc)			\
+#define GET_RX_DESC_FAGGR(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
 #define GET_RX_DESC_A1_FIT(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
 #define GET_RX_DESC_A2_FIT(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 20, 4)
-#define GET_RX_DESC_PAM(__pdesc)			\
+#define GET_RX_DESC_PAM(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 24, 1)
-#define GET_RX_DESC_PWR(__pdesc)			\
+#define GET_RX_DESC_PWR(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 25, 1)
-#define GET_RX_DESC_MD(__pdesc)				\
+#define GET_RX_DESC_MD(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 26, 1)
-#define GET_RX_DESC_MF(__pdesc)				\
+#define GET_RX_DESC_MF(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 27, 1)
-#define GET_RX_DESC_TYPE(__pdesc)			\
+#define GET_RX_DESC_TYPE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 28, 2)
-#define GET_RX_DESC_MC(__pdesc)				\
+#define GET_RX_DESC_MC(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 30, 1)
-#define GET_RX_DESC_BC(__pdesc)				\
+#define GET_RX_DESC_BC(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 31, 1)
-#define GET_RX_DESC_SEQ(__pdesc)			\
+#define GET_RX_DESC_SEQ(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 0, 12)
-#define GET_RX_DESC_FRAG(__pdesc)			\
+#define GET_RX_DESC_FRAG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 12, 4)
 #define GET_RX_DESC_NEXT_PKT_LEN(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+8, 16, 14)
 #define GET_RX_DESC_NEXT_IND(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+8, 30, 1)
-#define GET_RX_DESC_RSVD(__pdesc)			\
+#define GET_RX_DESC_RSVD(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 31, 1)
 
-#define GET_RX_DESC_RXMCS(__pdesc)			\
+#define GET_RX_DESC_RXMCS(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 0, 6)
-#define GET_RX_DESC_RXHT(__pdesc)			\
+#define GET_RX_DESC_RXHT(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 6, 1)
-#define GET_RX_DESC_SPLCP(__pdesc)			\
+#define GET_RX_DESC_SPLCP(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 8, 1)
-#define GET_RX_DESC_BW(__pdesc)				\
+#define GET_RX_DESC_BW(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+12, 9, 1)
-#define GET_RX_DESC_HTC(__pdesc)			\
+#define GET_RX_DESC_HTC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 10, 1)
 #define GET_RX_DESC_HWPC_ERR(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+12, 14, 1)
 #define GET_RX_DESC_HWPC_IND(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+12, 15, 1)
-#define GET_RX_DESC_IV0(__pdesc)			\
+#define GET_RX_DESC_IV0(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 16, 16)
 
-#define GET_RX_DESC_IV1(__pdesc)			\
+#define GET_RX_DESC_IV1(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 0, 32)
-#define GET_RX_DESC_TSFL(__pdesc)			\
+#define GET_RX_DESC_TSFL(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+20, 0, 32)
 
 #define GET_RX_DESC_BUFF_ADDR(__pdesc)			\
@@ -508,17 +504,17 @@
 #define GET_RX_DESC_BUFF_ADDR64(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+28, 0, 32)
 
-#define SET_RX_DESC_BUFF_ADDR(__pdesc, __val)		\
+#define SET_RX_DESC_BUFF_ADDR(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 32, __val)
-#define SET_RX_DESC_BUFF_ADDR64(__pdesc, __val)		\
+#define SET_RX_DESC_BUFF_ADDR64(__pdesc, __val) \
 	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
 
-#define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)	\
-do {							\
-	if (_size > TX_DESC_NEXT_DESC_OFFSET)		\
+#define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)		\
+do {								\
+	if (_size > TX_DESC_NEXT_DESC_OFFSET)			\
 		memset(__pdesc, 0, TX_DESC_NEXT_DESC_OFFSET);	\
-	else						\
-		memset(__pdesc, 0, _size);		\
+	else							\
+		memset(__pdesc, 0, _size);			\
 } while (0)
 
 struct rx_fwinfo_8723e {
@@ -699,22 +695,27 @@ struct rx_desc_8723e {
 
 } __packed;
 
-void rtl8723ae_tx_fill_desc(struct ieee80211_hw *hw,
-			    struct ieee80211_hdr *hdr, u8 *pdesc,
-			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
-			    struct ieee80211_sta *sta,
-			    struct sk_buff *skb, u8 hw_queue,
-			    struct rtl_tcb_desc *ptcb_desc);
-bool rtl8723ae_rx_query_desc(struct ieee80211_hw *hw,
-			     struct rtl_stats *status,
-			     struct ieee80211_rx_status *rx_status,
-			     u8 *pdesc, struct sk_buff *skb);
-void rtl8723ae_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-			u8 desc_name, u8 *val);
-u32 rtl8723ae_get_desc(u8 *pdesc, bool istx, u8 desc_name);
-void rtl8723ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
-void rtl8723ae_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
-			       bool b_firstseg, bool b_lastseg,
+void rtl8723e_tx_fill_desc(struct ieee80211_hw *hw,
+			   struct ieee80211_hdr *hdr,
+			   u8 *pdesc, u8 *txbd,
+			   struct ieee80211_tx_info *info,
+			   struct ieee80211_sta *sta,
+			   struct sk_buff *skb, u8 hw_queue,
+			   struct rtl_tcb_desc *ptcb_desc);
+bool rtl8723e_rx_query_desc(struct ieee80211_hw *hw,
+			    struct rtl_stats *status,
+			    struct ieee80211_rx_status *rx_status,
+			    u8 *pdesc, struct sk_buff *skb);
+void rtl8723e_set_desc(struct ieee80211_hw *hw,
+		       u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+u32 rtl8723e_get_desc(u8 *pdesc, bool istx, u8 desc_name);
+bool rtl8723e_is_tx_desc_closed(struct ieee80211_hw *hw,
+				u8 hw_queue, u16 index);
+void rtl8723e_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
+void rtl8723e_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
+			      bool firstseg, bool lastseg,
+			      struct sk_buff *skb);
+u32 rtl8723e_rx_command_packet(struct ieee80211_hw *hw,
+			       struct rtl_stats status,
 			       struct sk_buff *skb);
-
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/def.h b/drivers/net/wireless/rtlwifi/rtl8723be/def.h
index 3c30b74e983d..025ea5c0f3f6 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/def.h
@@ -26,158 +26,24 @@
 #ifndef __RTL8723BE_DEF_H__
 #define __RTL8723BE_DEF_H__
 
-#define HAL_RETRY_LIMIT_INFRA				48
-#define HAL_RETRY_LIMIT_AP_ADHOC			7
-
-#define RESET_DELAY_8185				20
-
-#define RT_IBSS_INT_MASKS	(IMR_BCNINT | IMR_TBDOK | IMR_TBDER)
-#define RT_AC_INT_MASKS		(IMR_VIDOK | IMR_VODOK | IMR_BEDOK|IMR_BKDOK)
-
-#define NUM_OF_FIRMWARE_QUEUE			10
-#define NUM_OF_PAGES_IN_FW			0x100
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK		0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE		0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI		0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO		0x07
-#define NUM_OF_PAGE_IN_FW_QUEUE_HCCA		0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_CMD		0x0
-#define NUM_OF_PAGE_IN_FW_QUEUE_MGNT		0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_HIGH		0x02
-#define NUM_OF_PAGE_IN_FW_QUEUE_BCN		0x2
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB		0xA1
-
-#define NUM_OF_PAGE_IN_FW_QUEUE_BK_DTM		0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_BE_DTM		0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VI_DTM		0x048
-#define NUM_OF_PAGE_IN_FW_QUEUE_VO_DTM		0x026
-#define NUM_OF_PAGE_IN_FW_QUEUE_PUB_DTM		0x00
-
-#define MAX_LINES_HWCONFIG_TXT			1000
-#define MAX_BYTES_LINE_HWCONFIG_TXT		256
-
-#define SW_THREE_WIRE				0
-#define HW_THREE_WIRE				2
-
-#define BT_DEMO_BOARD				0
-#define BT_QA_BOARD				1
-#define BT_FPGA					2
-
 #define HAL_PRIME_CHNL_OFFSET_DONT_CARE		0
 #define HAL_PRIME_CHNL_OFFSET_LOWER		1
 #define HAL_PRIME_CHNL_OFFSET_UPPER		2
 
-#define MAX_H2C_QUEUE_NUM			10
 
 #define RX_MPDU_QUEUE				0
-#define RX_CMD_QUEUE				1
-#define RX_MAX_QUEUE				2
-#define AC2QUEUEID(_AC)				(_AC)
-
-#define	C2H_RX_CMD_HDR_LEN			8
-#define	GET_C2H_CMD_CMD_LEN(__prxhdr)		\
-	LE_BITS_TO_4BYTE((__prxhdr), 0, 16)
-#define	GET_C2H_CMD_ELEMENT_ID(__prxhdr)	\
-	LE_BITS_TO_4BYTE((__prxhdr), 16, 8)
-#define	GET_C2H_CMD_CMD_SEQ(__prxhdr)		\
-	LE_BITS_TO_4BYTE((__prxhdr), 24, 7)
-#define	GET_C2H_CMD_CONTINUE(__prxhdr)		\
-	LE_BITS_TO_4BYTE((__prxhdr), 31, 1)
-#define	GET_C2H_CMD_CONTENT(__prxhdr)		\
-	((u8 *)(__prxhdr) + C2H_RX_CMD_HDR_LEN)
-
-#define	GET_C2H_CMD_FEEDBACK_ELEMENT_ID(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE((__pcmdfbhdr), 0, 8)
-#define	GET_C2H_CMD_FEEDBACK_CCX_LEN(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE((__pcmdfbhdr), 8, 8)
-#define	GET_C2H_CMD_FEEDBACK_CCX_CMD_CNT(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE((__pcmdfbhdr), 16, 16)
-#define	GET_C2H_CMD_FEEDBACK_CCX_MAC_ID(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 0, 5)
-#define	GET_C2H_CMD_FEEDBACK_CCX_VALID(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 7, 1)
-#define	GET_C2H_CMD_FEEDBACK_CCX_RETRY_CNT(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 8, 5)
-#define	GET_C2H_CMD_FEEDBACK_CCX_TOK(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 15, 1)
-#define	GET_C2H_CMD_FEEDBACK_CCX_QSEL(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 16, 4)
-#define	GET_C2H_CMD_FEEDBACK_CCX_SEQ(__pcmdfbhdr)	\
-	LE_BITS_TO_4BYTE(((__pcmdfbhdr) + 4), 20, 12)
-
-#define CHIP_BONDING_IDENTIFIER(_value)	(((_value)>>22)&0x3)
-#define	CHIP_BONDING_92C_1T2R		0x1
-
-#define CHIP_8723			BIT(0)
-#define CHIP_8723B			(BIT(1) | BIT(2))
-#define NORMAL_CHIP			BIT(3)
-#define RF_TYPE_1T1R			(~(BIT(4) | BIT(5) | BIT(6)))
-#define RF_TYPE_1T2R			BIT(4)
-#define RF_TYPE_2T2R			BIT(5)
-#define CHIP_VENDOR_UMC			BIT(7)
-#define B_CUT_VERSION			BIT(12)
-#define C_CUT_VERSION			BIT(13)
-#define D_CUT_VERSION			((BIT(12) | BIT(13)))
-#define E_CUT_VERSION			BIT(14)
-#define	RF_RL_ID			(BIT(31) | BIT(30) | BIT(29) | BIT(28))
-
-/* MASK */
-#define IC_TYPE_MASK			(BIT(0) | BIT(1) | BIT(2))
-#define CHIP_TYPE_MASK			BIT(3)
-#define RF_TYPE_MASK			(BIT(4) | BIT(5) | BIT(6))
-#define MANUFACTUER_MASK		BIT(7)
-#define ROM_VERSION_MASK		(BIT(11) | BIT(10) | BIT(9) | BIT(8))
-#define CUT_VERSION_MASK		(BIT(15) | BIT(14) | BIT(13) | BIT(12))
-
-/* Get element */
-#define GET_CVID_IC_TYPE(version)	((version) & IC_TYPE_MASK)
-#define GET_CVID_CHIP_TYPE(version)	((version) & CHIP_TYPE_MASK)
-#define GET_CVID_RF_TYPE(version)	((version) & RF_TYPE_MASK)
-#define GET_CVID_MANUFACTUER(version)	((version) & MANUFACTUER_MASK)
-#define GET_CVID_ROM_VERSION(version)	((version) & ROM_VERSION_MASK)
-#define GET_CVID_CUT_VERSION(version)	((version) & CUT_VERSION_MASK)
-
-#define IS_92C_SERIAL(version)   ((IS_81XXC(version) && IS_2T2R(version)) ?\
-								true : false)
-#define IS_81XXC(version)	((GET_CVID_IC_TYPE(version) == 0) ?\
-							true : false)
-#define IS_8723_SERIES(version)	((GET_CVID_IC_TYPE(version) == CHIP_8723) ?\
-							true : false)
-#define IS_1T1R(version)	((GET_CVID_RF_TYPE(version)) ? false : true)
-#define IS_1T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R)\
-							? true : false)
-#define IS_2T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R)\
-							? true : false)
-enum rf_optype {
-	RF_OP_BY_SW_3WIRE = 0,
-	RF_OP_BY_FW,
-	RF_OP_MAX
-};
-
-enum rf_power_state {
-	RF_ON,
-	RF_OFF,
-	RF_SLEEP,
-	RF_SHUT_DOWN,
-};
-
-enum power_save_mode {
-	POWER_SAVE_MODE_ACTIVE,
-	POWER_SAVE_MODE_SAVE,
-};
+#define CHIP_8723B				(BIT(1) | BIT(2))
+#define NORMAL_CHIP				BIT(3)
+#define CHIP_VENDOR_SMIC			BIT(8)
+/* Currently only for RTL8723B */
+#define EXT_VENDOR_ID				(BIT(18) | BIT(19))
 
-enum power_polocy_config {
-	POWERCFG_MAX_POWER_SAVINGS,
-	POWERCFG_GLOBAL_POWER_SAVINGS,
-	POWERCFG_LOCAL_POWER_SAVINGS,
-	POWERCFG_LENOVO,
-};
-
-enum interface_select_pci {
-	INTF_SEL1_MINICARD = 0,
-	INTF_SEL0_PCIE = 1,
-	INTF_SEL2_RSV = 2,
-	INTF_SEL3_RSV = 3,
+enum rx_packet_type {
+	NORMAL_RX,
+	TX_REPORT1,
+	TX_REPORT2,
+	HIS_REPORT,
+	C2H_PACKET,
 };
 
 enum rtl_desc_qsel {
@@ -222,27 +88,5 @@ enum rtl_desc8723e_rate {
 	DESC92C_RATEMCS13 = 0x19,
 	DESC92C_RATEMCS14 = 0x1a,
 	DESC92C_RATEMCS15 = 0x1b,
-	DESC92C_RATEMCS15_SG = 0x1c,
-	DESC92C_RATEMCS32 = 0x20,
 };
-
-enum rx_packet_type {
-	NORMAL_RX,
-	TX_REPORT1,
-	TX_REPORT2,
-	HIS_REPORT,
-};
-
-struct phy_sts_cck_8723e_t {
-	u8 adc_pwdb_X[4];
-	u8 sq_rpt;
-	u8 cck_agc_rpt;
-};
-
-struct h2c_cmd_8723e {
-	u8 element_id;
-	u32 cmd_len;
-	u8 *p_cmdbuffer;
-};
-
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
index 13d53a1df789..dd7eb4371f49 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.c
@@ -32,7 +32,6 @@
 #include "dm.h"
 #include "../rtl8723com/dm_common.h"
 #include "fw.h"
-#include "../rtl8723com/fw_common.h"
 #include "trx.h"
 #include "../btcoexist/rtl_btc.h"
 
@@ -209,7 +208,7 @@ void rtl8723be_dm_txpower_track_adjust(struct ieee80211_hw *hw, u8 type,
 		pwr_val = TXPWRTRACK_MAX_IDX;
 
 	*poutwrite_val = pwr_val | (pwr_val << 8) |
-			(pwr_val << 16) | (pwr_val << 24);
+		(pwr_val << 16) | (pwr_val << 24);
 }
 
 static void rtl8723be_dm_diginit(struct ieee80211_hw *hw)
@@ -218,8 +217,7 @@ static void rtl8723be_dm_diginit(struct ieee80211_hw *hw)
 	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
 	dm_digtable->dig_enable_flag = true;
-	dm_digtable->cur_igvalue = rtl_get_bbreg(hw,
-		ROFDM0_XAAGCCORE1, 0x7f);
+	dm_digtable->cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
 	dm_digtable->rssi_lowthresh = DM_DIG_THRESH_LOW;
 	dm_digtable->rssi_highthresh = DM_DIG_THRESH_HIGH;
 	dm_digtable->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
@@ -234,8 +232,8 @@ static void rtl8723be_dm_diginit(struct ieee80211_hw *hw)
 	dm_digtable->forbidden_igi = DM_DIG_MIN;
 	dm_digtable->large_fa_hit = 0;
 	dm_digtable->recover_cnt = 0;
-	dm_digtable->dig_min_0 = DM_DIG_MIN;
-	dm_digtable->dig_min_1 = DM_DIG_MIN;
+	dm_digtable->dig_dynamic_min = DM_DIG_MIN;
+	dm_digtable->dig_dynamic_min_1 = DM_DIG_MIN;
 	dm_digtable->media_connect_0 = false;
 	dm_digtable->media_connect_1 = false;
 	rtlpriv->dm.dm_initialgain_enable = true;
@@ -245,18 +243,18 @@ static void rtl8723be_dm_diginit(struct ieee80211_hw *hw)
 void rtl8723be_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rate_adaptive *ra = &(rtlpriv->ra);
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
 
-	ra->ratr_state = DM_RATR_STA_INIT;
-	ra->pre_ratr_state = DM_RATR_STA_INIT;
+	p_ra->ratr_state = DM_RATR_STA_INIT;
+	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
 
 	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
 		rtlpriv->dm.useramask = true;
 	else
 		rtlpriv->dm.useramask = false;
 
-	ra->high_rssi_thresh_for_ra = 50;
-	ra->low_rssi_thresh_for_ra40m = 20;
+	p_ra->high_rssi_thresh_for_ra = 50;
+	p_ra->low_rssi_thresh_for_ra40m = 20;
 }
 
 static void rtl8723be_dm_init_txpower_tracking(struct ieee80211_hw *hw)
@@ -279,7 +277,7 @@ static void rtl8723be_dm_init_txpower_tracking(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 		 "  rtlpriv->dm.txpower_tracking = %d\n",
-		 rtlpriv->dm.txpower_tracking);
+		  rtlpriv->dm.txpower_tracking);
 }
 
 static void rtl8723be_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
@@ -287,6 +285,7 @@ static void rtl8723be_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	rtlpriv->dm.crystal_cap = rtlpriv->efuse.crystalcap;
+
 	rtlpriv->dm.atc_status = rtl_get_bbreg(hw, ROFDM1_CFOTRACKING, 0x800);
 	rtlpriv->dm.cfo_threshold = CFO_THRESHOLD_XTAL;
 }
@@ -308,7 +307,7 @@ void rtl8723be_dm_init(struct ieee80211_hw *hw)
 static void rtl8723be_dm_find_minimum_rssi(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct dig_t *rtl_dm_dig = &(rtlpriv->dm_digtable);
+	struct dig_t *rtl_dm_dig = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtlpriv);
 
 	/* Determine the minimum RSSI  */
@@ -325,20 +324,20 @@ static void rtl8723be_dm_find_minimum_rssi(struct ieee80211_hw *hw)
 			    rtlpriv->dm.entry_min_undec_sm_pwdb;
 			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
 				 "AP Client PWDB = 0x%lx\n",
-				 rtlpriv->dm.entry_min_undec_sm_pwdb);
+			       rtlpriv->dm.entry_min_undec_sm_pwdb);
 		} else {
 			rtl_dm_dig->min_undec_pwdb_for_dm =
 			    rtlpriv->dm.undec_sm_pwdb;
 			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
 				 "STA Default Port PWDB = 0x%x\n",
-				 rtl_dm_dig->min_undec_pwdb_for_dm);
+				  rtl_dm_dig->min_undec_pwdb_for_dm);
 		}
 	} else {
 		rtl_dm_dig->min_undec_pwdb_for_dm =
 				rtlpriv->dm.entry_min_undec_sm_pwdb;
 		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
 			 "AP Ext Port or disconnet PWDB = 0x%x\n",
-			 rtl_dm_dig->min_undec_pwdb_for_dm);
+			  rtl_dm_dig->min_undec_pwdb_for_dm);
 	}
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "MinUndecoratedPWDBForDM =%d\n",
 		 rtl_dm_dig->min_undec_pwdb_for_dm);
@@ -347,6 +346,7 @@ static void rtl8723be_dm_find_minimum_rssi(struct ieee80211_hw *hw)
 static void rtl8723be_dm_check_rssi_monitor(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 	struct rtl_sta_info *drv_priv;
 	u8 h2c_parameter[3] = { 0 };
 	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
@@ -367,69 +367,78 @@ static void rtl8723be_dm_check_rssi_monitor(struct ieee80211_hw *hw)
 
 	/* If associated entry is found */
 	if (tmp_entry_max_pwdb != 0) {
-		rtlpriv->dm.entry_max_undec_sm_pwdb = tmp_entry_max_pwdb;
-		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-			 "EntryMaxPWDB = 0x%lx(%ld)\n",
+		rtlpriv->dm.entry_max_undec_sm_pwdb =
+							tmp_entry_max_pwdb;
+		RTPRINT(rtlpriv, FDM, DM_PWDB,
+			"EntryMaxPWDB = 0x%lx(%ld)\n",
 			 tmp_entry_max_pwdb, tmp_entry_max_pwdb);
 	} else {
 		rtlpriv->dm.entry_max_undec_sm_pwdb = 0;
 	}
 	/* If associated entry is found */
 	if (tmp_entry_min_pwdb != 0xff) {
-		rtlpriv->dm.entry_min_undec_sm_pwdb = tmp_entry_min_pwdb;
-		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-			 "EntryMinPWDB = 0x%lx(%ld)\n",
+		rtlpriv->dm.entry_min_undec_sm_pwdb =
+							tmp_entry_min_pwdb;
+		RTPRINT(rtlpriv, FDM, DM_PWDB,
+			"EntryMinPWDB = 0x%lx(%ld)\n",
 			 tmp_entry_min_pwdb, tmp_entry_min_pwdb);
 	} else {
 		rtlpriv->dm.entry_min_undec_sm_pwdb = 0;
 	}
 	/* Indicate Rx signal strength to FW. */
 	if (rtlpriv->dm.useramask) {
-		h2c_parameter[2] = (u8) (rtlpriv->dm.undec_sm_pwdb & 0xFF);
+		h2c_parameter[2] =
+			(u8)(rtlpriv->dm.undec_sm_pwdb & 0xFF);
 		h2c_parameter[1] = 0x20;
 		h2c_parameter[0] = 0;
-		rtl8723be_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);
+		rtl8723be_fill_h2c_cmd(hw, H2C_RSSIBE_REPORT, 3, h2c_parameter);
 	} else {
-		rtl_write_byte(rtlpriv, 0x4fe, rtlpriv->dm.undec_sm_pwdb);
+		rtl_write_byte(rtlpriv, 0x4fe,
+			       rtlpriv->dm.undec_sm_pwdb);
 	}
 	rtl8723be_dm_find_minimum_rssi(hw);
-	rtlpriv->dm_digtable.rssi_val_min =
-		rtlpriv->dm_digtable.min_undec_pwdb_for_dm;
+	dm_digtable->rssi_val_min =
+			rtlpriv->dm_digtable.min_undec_pwdb_for_dm;
 }
 
 void rtl8723be_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 
-	if (rtlpriv->dm_digtable.cur_igvalue != current_igi) {
+	if (dm_digtable->stop_dig)
+		return;
+
+	if (dm_digtable->cur_igvalue != current_igi) {
 		rtl_set_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f, current_igi);
 		if (rtlpriv->phy.rf_type != RF_1T1R)
-			rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1, 0x7f, current_igi);
+			rtl_set_bbreg(hw, ROFDM0_XBAGCCORE1,
+				      0x7f, current_igi);
 	}
-	rtlpriv->dm_digtable.pre_igvalue = rtlpriv->dm_digtable.cur_igvalue;
-	rtlpriv->dm_digtable.cur_igvalue = current_igi;
+	dm_digtable->pre_igvalue = dm_digtable->cur_igvalue;
+	dm_digtable->cur_igvalue = current_igi;
 }
 
 static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct dig_t *dm_digtable = &(rtlpriv->dm_digtable);
 	u8 dig_dynamic_min, dig_maxofmin;
-	bool firstconnect, firstdisconnect;
+	bool bfirstconnect, bfirstdisconnect;
 	u8 dm_dig_max, dm_dig_min;
 	u8 current_igi = dm_digtable->cur_igvalue;
 	u8 offset;
 
-	/* AP, BT */
+	/* AP,BT */
 	if (mac->act_scanning)
 		return;
 
-	dig_dynamic_min = dm_digtable->dig_min_0;
-	firstconnect = (mac->link_state >= MAC80211_LINKED) &&
+	dig_dynamic_min = dm_digtable->dig_dynamic_min;
+	bfirstconnect = (mac->link_state >= MAC80211_LINKED) &&
 			!dm_digtable->media_connect_0;
-	firstdisconnect = (mac->link_state < MAC80211_LINKED) &&
-			   dm_digtable->media_connect_0;
+	bfirstdisconnect = (mac->link_state < MAC80211_LINKED) &&
+			(dm_digtable->media_connect_0);
 
 	dm_dig_max = 0x5a;
 	dm_dig_min = DM_DIG_MIN;
@@ -457,6 +466,7 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 		} else {
 			dig_dynamic_min = dm_dig_min;
 		}
+
 	} else {
 		dm_digtable->rx_gain_max = dm_dig_max;
 		dig_dynamic_min = dm_dig_min;
@@ -506,7 +516,7 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 		dm_digtable->rx_gain_min = dm_digtable->rx_gain_max;
 
 	if (mac->link_state >= MAC80211_LINKED) {
-		if (firstconnect) {
+		if (bfirstconnect) {
 			if (dm_digtable->rssi_val_min <= dig_maxofmin)
 				current_igi = dm_digtable->rssi_val_min;
 			else
@@ -522,7 +532,7 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 				current_igi -= 2;
 		}
 	} else {
-		if (firstdisconnect) {
+		if (bfirstdisconnect) {
 			current_igi = dm_digtable->rx_gain_min;
 		} else {
 			if (rtlpriv->falsealm_cnt.cnt_all > 10000)
@@ -542,14 +552,15 @@ static void rtl8723be_dm_dig(struct ieee80211_hw *hw)
 	rtl8723be_dm_write_dig(hw, current_igi);
 	dm_digtable->media_connect_0 =
 		((mac->link_state >= MAC80211_LINKED) ? true : false);
-	dm_digtable->dig_min_0 = dig_dynamic_min;
+	dm_digtable->dig_dynamic_min = dig_dynamic_min;
 }
 
-static void rtl8723be_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+static void rtl8723be_dm_false_alarm_counter_statistics(
+					struct ieee80211_hw *hw)
 {
 	u32 ret_value;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
+	struct false_alarm_statistics *falsealm_cnt = &rtlpriv->falsealm_cnt;
 
 	rtl_set_bbreg(hw, DM_REG_OFDM_FA_HOLDC_11N, BIT(31), 1);
 	rtl_set_bbreg(hw, DM_REG_OFDM_FA_RSTD_11N, BIT(31), 1);
@@ -615,16 +626,14 @@ static void rtl8723be_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
 	rtl_set_bbreg(hw, DM_REG_CCK_FA_RST_11N, BIT(15) | BIT(14), 2);
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, "
-		 "cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
+		 "cnt_parity_fail = %d, cnt_rate_illegal = %d, cnt_crc8_fail = %d, cnt_mcs_fail = %d\n",
 		 falsealm_cnt->cnt_parity_fail,
 		 falsealm_cnt->cnt_rate_illegal,
 		 falsealm_cnt->cnt_crc8_fail,
 		 falsealm_cnt->cnt_mcs_fail);
 
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 "cnt_ofdm_fail = %x, cnt_cck_fail = %x,"
-		 " cnt_all = %x\n",
+		 "cnt_ofdm_fail = %x, cnt_cck_fail = %x, cnt_all = %x\n",
 		 falsealm_cnt->cnt_ofdm_fail,
 		 falsealm_cnt->cnt_cck_fail,
 		 falsealm_cnt->cnt_all);
@@ -690,7 +699,7 @@ static void rtl8723be_dm_tx_power_track_set_power(struct ieee80211_hw *hw,
 					u8 rfpath, u8 idx)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
 	u8 swing_idx_ofdm_limit = 36;
 
@@ -762,7 +771,8 @@ static void rtl8723be_dm_tx_power_track_set_power(struct ieee80211_hw *hw,
 	}
 }
 
-static void txpwr_track_cb_therm(struct ieee80211_hw *hw)
+static void rtl8723be_dm_txpower_tracking_callback_thermalmeter(
+							struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
@@ -773,30 +783,29 @@ static void txpwr_track_cb_therm(struct ieee80211_hw *hw)
 	int i = 0;
 
 	u8 ofdm_min_index = 6;
-	u8 index = 0;
+	u8 index_for_channel = 0;
 
-	char delta_swing_table_idx_tup_a[] = {
+	char delta_swing_table_idx_tup_a[TXSCALE_TABLE_SIZE] = {
 		0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,
 		5, 6, 6, 7, 7, 8, 8, 9, 9, 9, 10,
 		10, 11, 11, 12, 12, 13, 14, 15};
-	char delta_swing_table_idx_tdown_a[] = {
+	char delta_swing_table_idx_tdown_a[TXSCALE_TABLE_SIZE] = {
 		0, 0, 1, 2, 2, 2, 3, 3, 3, 4,  5,
 		5, 6, 6, 6, 6, 7, 7, 7, 8, 8,  9,
 		9, 10, 10, 11, 12, 13, 14, 15};
 
-	/*Initilization ( 7 steps in total)*/
+	/*Initilization ( 7 steps in total )*/
 	rtlpriv->dm.txpower_trackinginit = true;
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "rtl8723be_dm_txpower_tracking"
-		 "_callback_thermalmeter\n");
+		 "rtl8723be_dm_txpower_tracking_callback_thermalmeter\n");
 
-	thermalvalue = (u8)rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER, 0xfc00);
+	thermalvalue = (u8)rtl_get_rfreg(hw,
+		RF90_PATH_A, RF_T_METER, 0xfc00);
 	if (!rtlpriv->dm.txpower_track_control || thermalvalue == 0 ||
 	    rtlefuse->eeprom_thermalmeter == 0xFF)
 		return;
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
-		 "eeprom_thermalmeter 0x%x\n",
+		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x eeprom_thermalmeter 0x%x\n",
 		 thermalvalue, rtldm->thermalvalue,
 		 rtlefuse->eeprom_thermalmeter);
 	/*3 Initialize ThermalValues of RFCalibrateInfo*/
@@ -833,9 +842,7 @@ static void txpwr_track_cb_therm(struct ieee80211_hw *hw)
 		    (rtlpriv->dm.thermalvalue_iqk - thermalvalue);
 
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
-		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x "
-		 "eeprom_thermalmeter 0x%x delta 0x%x "
-		 "delta_lck 0x%x delta_iqk 0x%x\n",
+		 "Readback Thermal Meter = 0x%x pre thermal meter 0x%x eeprom_thermalmeter 0x%x delta 0x%x delta_lck 0x%x delta_iqk 0x%x\n",
 		 thermalvalue, rtlpriv->dm.thermalvalue,
 		 rtlefuse->eeprom_thermalmeter, delta, delta_lck, delta_iqk);
 	/* 6 If necessary, do LCK.*/
@@ -905,10 +912,10 @@ static void txpwr_track_cb_therm(struct ieee80211_hw *hw)
 		rtldm->done_txpower = true;
 		if (thermalvalue > rtlefuse->eeprom_thermalmeter)
 			rtl8723be_dm_tx_power_track_set_power(hw, BBSWING, 0,
-							      index);
+							     index_for_channel);
 		else
 			rtl8723be_dm_tx_power_track_set_power(hw, BBSWING, 0,
-							      index);
+							     index_for_channel);
 
 		rtldm->swing_idx_cck_base = rtldm->swing_idx_cck;
 		rtldm->swing_idx_ofdm_base[RF90_PATH_A] =
@@ -923,6 +930,7 @@ static void txpwr_track_cb_therm(struct ieee80211_hw *hw)
 
 	rtldm->txpowercount = 0;
 	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, "end\n");
+
 }
 
 void rtl8723be_dm_check_txpower_tracking(struct ieee80211_hw *hw)
@@ -943,7 +951,7 @@ void rtl8723be_dm_check_txpower_tracking(struct ieee80211_hw *hw)
 	} else {
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Schedule TxPowerTracking !!\n");
-		txpwr_track_cb_therm(hw);
+		rtl8723be_dm_txpower_tracking_callback_thermalmeter(hw);
 		tm_trigger = 0;
 	}
 }
@@ -953,11 +961,11 @@ static void rtl8723be_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rate_adaptive *ra = &(rtlpriv->ra);
-	struct ieee80211_sta *sta = NULL;
-	u32 low_rssithresh_for_ra = ra->low2high_rssi_thresh_for_ra40m;
-	u32 high_rssithresh_for_ra = ra->high_rssi_thresh_for_ra;
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
+	u32 low_rssithresh_for_ra = p_ra->low2high_rssi_thresh_for_ra40m;
+	u32 high_rssithresh_for_ra = p_ra->high_rssi_thresh_for_ra;
 	u8 go_up_gap = 5;
+	struct ieee80211_sta *sta = NULL;
 
 	if (is_hal_stop(rtlhal)) {
 		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
@@ -972,8 +980,8 @@ static void rtl8723be_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 	}
 
 	if (mac->link_state == MAC80211_LINKED &&
-	    mac->opmode == NL80211_IFTYPE_STATION) {
-		switch (ra->pre_ratr_state) {
+		mac->opmode == NL80211_IFTYPE_STATION) {
+		switch (p_ra->pre_ratr_state) {
 		case DM_RATR_STA_MIDDLE:
 			high_rssithresh_for_ra += go_up_gap;
 			break;
@@ -987,31 +995,31 @@ static void rtl8723be_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
 
 		if (rtlpriv->dm.undec_sm_pwdb >
 		    (long)high_rssithresh_for_ra)
-			ra->ratr_state = DM_RATR_STA_HIGH;
+			p_ra->ratr_state = DM_RATR_STA_HIGH;
 		else if (rtlpriv->dm.undec_sm_pwdb >
 			 (long)low_rssithresh_for_ra)
-			ra->ratr_state = DM_RATR_STA_MIDDLE;
+			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
 		else
-			ra->ratr_state = DM_RATR_STA_LOW;
+			p_ra->ratr_state = DM_RATR_STA_LOW;
 
-		if (ra->pre_ratr_state != ra->ratr_state) {
+		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
 			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
 				 "RSSI = %ld\n",
 				 rtlpriv->dm.undec_sm_pwdb);
 			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
-				 "RSSI_LEVEL = %d\n", ra->ratr_state);
+				 "RSSI_LEVEL = %d\n", p_ra->ratr_state);
 			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
 				 "PreState = %d, CurState = %d\n",
-				 ra->pre_ratr_state, ra->ratr_state);
+				  p_ra->pre_ratr_state, p_ra->ratr_state);
 
 			rcu_read_lock();
 			sta = rtl_find_sta(hw, mac->bssid);
 			if (sta)
 				rtlpriv->cfg->ops->update_rate_tbl(hw, sta,
-							   ra->ratr_state);
+							   p_ra->ratr_state);
 			rcu_read_unlock();
 
-			ra->pre_ratr_state = ra->ratr_state;
+			p_ra->pre_ratr_state = p_ra->ratr_state;
 		}
 	}
 }
@@ -1020,10 +1028,6 @@ static bool rtl8723be_dm_is_edca_turbo_disable(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	if (rtlpriv->cfg->ops->get_btc_status()) {
-		if (rtlpriv->btcoexist.btc_ops->btc_is_disable_edca_turbo(rtlpriv))
-			return true;
-	}
 	if (rtlpriv->mac80211.mode == WIRELESS_MODE_B)
 		return true;
 
@@ -1034,6 +1038,7 @@ static void rtl8723be_dm_check_edca_turbo(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
 	static u64 last_txok_cnt;
 	static u64 last_rxok_cnt;
 	u64 cur_txok_cnt = 0;
@@ -1042,22 +1047,22 @@ static void rtl8723be_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	u32 edca_be_dl = 0x6ea42b;/*not sure*/
 	u32 edca_be = 0x5ea42b;
 	u32 iot_peer = 0;
-	bool is_cur_rdlstate;
-	bool last_is_cur_rdlstate = false;
-	bool bias_on_rx = false;
-	bool edca_turbo_on = false;
+	bool b_is_cur_rdlstate;
+	bool b_last_is_cur_rdlstate = false;
+	bool b_bias_on_rx = false;
+	bool b_edca_turbo_on = false;
 
-	last_is_cur_rdlstate = rtlpriv->dm.is_cur_rdlstate;
+	b_last_is_cur_rdlstate = rtlpriv->dm.is_cur_rdlstate;
 
 	cur_txok_cnt = rtlpriv->stats.txbytesunicast - last_txok_cnt;
 	cur_rxok_cnt = rtlpriv->stats.rxbytesunicast - last_rxok_cnt;
 
 	iot_peer = rtlpriv->mac80211.vendor;
-	bias_on_rx = (iot_peer == PEER_RAL || iot_peer == PEER_ATH) ?
-		     true : false;
-	edca_turbo_on = ((!rtlpriv->dm.is_any_nonbepkts) &&
-			 (!rtlpriv->dm.disable_framebursting)) ?
-			 true : false;
+	b_bias_on_rx = (iot_peer == PEER_RAL || iot_peer == PEER_ATH) ?
+		       true : false;
+	b_edca_turbo_on = ((!rtlpriv->dm.is_any_nonbepkts) &&
+			   (!rtlpriv->dm.disable_framebursting)) ?
+			   true : false;
 
 	if ((iot_peer == PEER_CISCO) &&
 	    (mac->mode == WIRELESS_MODE_N_24G)) {
@@ -1067,23 +1072,23 @@ static void rtl8723be_dm_check_edca_turbo(struct ieee80211_hw *hw)
 	if (rtl8723be_dm_is_edca_turbo_disable(hw))
 		goto exit;
 
-	if (edca_turbo_on) {
-		if (bias_on_rx)
-			is_cur_rdlstate = (cur_txok_cnt > cur_rxok_cnt * 4) ?
-					  false : true;
+	if (b_edca_turbo_on) {
+		if (b_bias_on_rx)
+			b_is_cur_rdlstate = (cur_txok_cnt > cur_rxok_cnt * 4) ?
+					    false : true;
 		else
-			is_cur_rdlstate = (cur_rxok_cnt > cur_txok_cnt * 4) ?
-					  true : false;
+			b_is_cur_rdlstate = (cur_rxok_cnt > cur_txok_cnt * 4) ?
+					    true : false;
 
-		edca_be = (is_cur_rdlstate) ? edca_be_dl : edca_be_ul;
+		edca_be = (b_is_cur_rdlstate) ? edca_be_dl : edca_be_ul;
 		rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, edca_be);
-		rtlpriv->dm.is_cur_rdlstate = is_cur_rdlstate;
+		rtlpriv->dm.is_cur_rdlstate = b_is_cur_rdlstate;
 		rtlpriv->dm.current_turbo_edca = true;
 	} else {
 		if (rtlpriv->dm.current_turbo_edca) {
 			u8 tmp = AC0_BE;
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      &tmp);
+						      (u8 *)(&tmp));
 		}
 		rtlpriv->dm.current_turbo_edca = false;
 	}
@@ -1097,13 +1102,14 @@ exit:
 static void rtl8723be_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
 	u8 cur_cck_cca_thresh;
 
 	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-		if (rtlpriv->dm_digtable.rssi_val_min > 25) {
+		if (dm_digtable->rssi_val_min > 25) {
 			cur_cck_cca_thresh = 0xcd;
-		} else if ((rtlpriv->dm_digtable.rssi_val_min <= 25) &&
-			   (rtlpriv->dm_digtable.rssi_val_min > 10)) {
+		} else if ((dm_digtable->rssi_val_min <= 25) &&
+			   (dm_digtable->rssi_val_min > 10)) {
 			cur_cck_cca_thresh = 0x83;
 		} else {
 			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
@@ -1118,14 +1124,13 @@ static void rtl8723be_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
 			cur_cck_cca_thresh = 0x40;
 	}
 
-	if (rtlpriv->dm_digtable.cur_cck_cca_thres != cur_cck_cca_thresh)
+	if (dm_digtable->cur_cck_cca_thres != cur_cck_cca_thresh)
 		rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, cur_cck_cca_thresh);
 
-	rtlpriv->dm_digtable.pre_cck_cca_thres = rtlpriv->dm_digtable.cur_cck_cca_thres;
-	rtlpriv->dm_digtable.cur_cck_cca_thres = cur_cck_cca_thresh;
+	dm_digtable->pre_cck_cca_thres = dm_digtable->cur_cck_cca_thres;
+	dm_digtable->cur_cck_cca_thres = cur_cck_cca_thresh;
 	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
-		 "CCK cca thresh hold =%x\n",
-		 rtlpriv->dm_digtable.cur_cck_cca_thres);
+		 "CCK cca thresh hold =%x\n", dm_digtable->cur_cck_cca_thres);
 }
 
 static void rtl8723be_dm_dynamic_edcca(struct ieee80211_hw *hw)
@@ -1173,8 +1178,7 @@ static void rtl8723be_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
 		if (rtlpriv->cfg->ops->get_btc_status()) {
 			if (!rtlpriv->btcoexist.btc_ops->btc_is_bt_disabled(rtlpriv)) {
 				RT_TRACE(rtlpriv, COMP_BT_COEXIST, DBG_LOUD,
-					 "odm_DynamicATCSwitch(): Disable"
-					 " CFO tracking for BT!!\n");
+					 "odm_DynamicATCSwitch(): Disable CFO tracking for BT!!\n");
 				return;
 			}
 		}
@@ -1207,9 +1211,8 @@ static void rtl8723be_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
 		if (cfo_ave_diff > 20 && rtldm->large_cfo_hit == 0) {
 			rtldm->large_cfo_hit = 1;
 			return;
-		} else {
+		} else
 			rtldm->large_cfo_hit = 0;
-		}
 
 		rtldm->cfo_ave_pre = cfo_ave;
 
@@ -1263,20 +1266,20 @@ static void rtl8723be_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
 static void rtl8723be_dm_common_info_self_update(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *drv_priv;
 	u8 cnt = 0;
+	struct rtl_sta_info *drv_priv;
 
 	rtlpriv->dm.one_entry_only = false;
 
 	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
-	    rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
 		rtlpriv->dm.one_entry_only = true;
 		return;
 	}
 
 	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
-	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC ||
-	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_MESH_POINT) {
+		rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC ||
+		rtlpriv->mac80211.opmode == NL80211_IFTYPE_MESH_POINT) {
 		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
 		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
 			cnt++;
@@ -1305,8 +1308,8 @@ void rtl8723be_dm_watchdog(struct ieee80211_hw *hw)
 		fw_ps_awake = false;
 
 	if ((ppsc->rfpwr_state == ERFON) &&
-	    ((!fw_current_inpsmode) && fw_ps_awake) &&
-	    (!ppsc->rfchange_inprogress)) {
+		((!fw_current_inpsmode) && fw_ps_awake) &&
+		(!ppsc->rfchange_inprogress)) {
 		rtl8723be_dm_common_info_self_update(hw);
 		rtl8723be_dm_false_alarm_counter_statistics(hw);
 		rtl8723be_dm_check_rssi_monitor(hw);
@@ -1318,8 +1321,6 @@ void rtl8723be_dm_watchdog(struct ieee80211_hw *hw)
 		rtl8723be_dm_dynamic_atc_switch(hw);
 		rtl8723be_dm_check_txpower_tracking(hw);
 		rtl8723be_dm_dynamic_txpower(hw);
-		if (rtlpriv->cfg->ops->get_btc_status())
-			rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
 	}
 	rtlpriv->dm.dbginfo.num_qry_beacon_pkt = 0;
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/dm.h b/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
index c6c2f2a78a66..e4c0e8ae6f47 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/dm.h
@@ -141,7 +141,7 @@
 #define	DM_REG_TX_CCK_BBON_11N			0xE78
 #define	DM_REG_OFDM_RFON_11N			0xE7C
 #define	DM_REG_OFDM_BBON_11N			0xE80
-#define DM_REG_TX2RX_11N			0xE84
+#define		DM_REG_TX2RX_11N		0xE84
 #define	DM_REG_TX2TX_11N			0xE88
 #define	DM_REG_RX_CCK_11N			0xE8C
 #define	DM_REG_RX_OFDM_11N			0xED0
@@ -202,6 +202,7 @@
 #define DM_DIG_BACKOFF_MIN			-4
 #define DM_DIG_BACKOFF_DEFAULT			10
 
+#define RXPATHSELECTION_SS_TH_LOW		30
 #define RXPATHSELECTION_DIFF_TH			18
 
 #define DM_RATR_STA_INIT			0
@@ -212,6 +213,8 @@
 #define CTS2SELF_THVAL				30
 #define REGC38_TH				20
 
+#define WAIOTTHVAL				25
+
 #define TXHIGHPWRLEVEL_NORMAL			0
 #define TXHIGHPWRLEVEL_LEVEL1			1
 #define TXHIGHPWRLEVEL_LEVEL2			2
@@ -231,22 +234,6 @@
 #define	CFO_THRESHOLD_XTAL			10 /* kHz */
 #define	CFO_THRESHOLD_ATC			80 /* kHz */
 
-enum FAT_STATE {
-	FAT_NORMAL_STATE	= 0,
-	FAT_TRAINING_STATE	= 1,
-};
-
-enum tag_dynamic_init_gain_operation_type_definition {
-	DIG_TYPE_THRESH_HIGH	= 0,
-	DIG_TYPE_THRESH_LOW	= 1,
-	DIG_TYPE_BACKOFF	= 2,
-	DIG_TYPE_RX_GAIN_MIN	= 3,
-	DIG_TYPE_RX_GAIN_MAX	= 4,
-	DIG_TYPE_ENABLE		= 5,
-	DIG_TYPE_DISABLE	= 6,
-	DIG_OP_TYPE_MAX
-};
-
 enum dm_1r_cca_e {
 	CCA_1R		= 0,
 	CCA_2R		= 1,
@@ -292,12 +279,17 @@ enum pwr_track_control_method {
 #define BT_RSSI_STATE_SPECIAL_LOW       BIT_OFFSET_LEN_MASK_32(2, 1)
 #define BT_RSSI_STATE_BG_EDCA_LOW       BIT_OFFSET_LEN_MASK_32(3, 1)
 #define BT_RSSI_STATE_TXPOWER_LOW       BIT_OFFSET_LEN_MASK_32(4, 1)
+#define GET_UNDECORATED_AVERAGE_RSSI(_priv)     \
+	((((struct rtl_priv *)(_priv))->mac80211.opmode == \
+		NL80211_IFTYPE_ADHOC) ? \
+	(((struct rtl_priv *)(_priv))->dm.entry_min_undecoratedsmoothed_pwdb) :\
+	(((struct rtl_priv *)(_priv))->dm.undecorated_smoothed_pwdb))
 
 void rtl8723be_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw, u8 *pdesc,
 					u32 mac_id);
 void rtl8723be_dm_ant_sel_statistics(struct ieee80211_hw *hw, u8 antsel_tr_mux,
 				     u32 mac_id, u32 rx_pwdb_all);
-void rtl8723be_dm_fast_antenna_trainning_callback(unsigned long data);
+void rtl8723be_dm_fast_antenna_training_callback(unsigned long data);
 void rtl8723be_dm_init(struct ieee80211_hw *hw);
 void rtl8723be_dm_watchdog(struct ieee80211_hw *hw);
 void rtl8723be_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi);
@@ -305,6 +297,4 @@ void rtl8723be_dm_check_txpower_tracking(struct ieee80211_hw *hw);
 void rtl8723be_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
 void rtl8723be_dm_txpower_track_adjust(struct ieee80211_hw *hw, u8 type,
 				       u8 *pdirection, u32 *poutwrite_val);
-void rtl8723be_dm_init_edca_turbo(struct ieee80211_hw *hw);
-
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/fw.c b/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
index f856be6fc138..69d4f0fc1af1 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/fw.c
@@ -26,6 +26,7 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../base.h"
+#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "fw.h"
@@ -55,8 +56,8 @@ static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 	bool isfw_read = false;
 	u8 buf_index = 0;
 	bool bwrite_sucess = false;
-	u8 wait_h2c_limit = 100;
-	u8 wait_writeh2c_limit = 100;
+	u8 wait_h2c_limmit = 100;
+	u8 wait_writeh2c_limmit = 100;
 	u8 boxcontent[4], boxextcontent[4];
 	u32 h2c_waitcounter = 0;
 	unsigned long flag;
@@ -68,8 +69,8 @@ static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 		if (rtlhal->h2c_setinprogress) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "H2C set in progress! Wait to set.."
-				 "element_id(%d).\n", element_id);
+				 "H2C set in progress! Wait to set..element_id(%d).\n",
+				 element_id);
 
 			while (rtlhal->h2c_setinprogress) {
 				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
@@ -92,14 +93,15 @@ static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 			break;
 		}
 	}
+
 	while (!bwrite_sucess) {
-		wait_writeh2c_limit--;
-		if (wait_writeh2c_limit == 0) {
+		wait_writeh2c_limmit--;
+		if (wait_writeh2c_limmit == 0) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "Write H2C fail because no trigger "
-				 "for FW INT!\n");
+				 "Write H2C fail because no trigger for FW INT!\n");
 			break;
 		}
+
 		boxnum = rtlhal->last_hmeboxnum;
 		switch (boxnum) {
 		case 0:
@@ -120,39 +122,43 @@ static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 			break;
 		default:
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "switch case not processed\n");
+				 "switch case not process\n");
 			break;
 		}
+
 		isfw_read = _rtl8723be_check_fw_read_last_h2c(hw, boxnum);
 		while (!isfw_read) {
-			wait_h2c_limit--;
-			if (wait_h2c_limit == 0) {
+			wait_h2c_limmit--;
+			if (wait_h2c_limmit == 0) {
 				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-					 "Wating too long for FW read "
-					 "clear HMEBox(%d)!\n", boxnum);
+					 "Waiting too long for FW read clear HMEBox(%d)!\n",
+					 boxnum);
 				break;
 			}
+
 			udelay(10);
 
 			isfw_read = _rtl8723be_check_fw_read_last_h2c(hw,
 								boxnum);
 			u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Wating for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
+				 "Waiting for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
 				 boxnum, u1b_tmp);
 		}
+
 		if (!isfw_read) {
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
-				 "Write H2C register BOX[%d] fail!!!!! "
-				 "Fw do not read.\n", boxnum);
+				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
+				 boxnum);
 			break;
 		}
+
 		memset(boxcontent, 0, sizeof(boxcontent));
 		memset(boxextcontent, 0, sizeof(boxextcontent));
 		boxcontent[0] = element_id;
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "Write element_id box_reg(%4x) = %2x\n",
-			 box_reg, element_id);
+			  box_reg, element_id);
 
 		switch (cmd_len) {
 		case 1:
@@ -181,6 +187,7 @@ static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 				rtl_write_byte(rtlpriv, box_extreg + idx,
 					       boxextcontent[idx]);
 			}
+
 			for (idx = 0; idx < 4; idx++) {
 				rtl_write_byte(rtlpriv, box_reg + idx,
 					       boxcontent[idx]);
@@ -191,6 +198,7 @@ static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 				 "switch case not process\n");
 			break;
 		}
+
 		bwrite_sucess = true;
 
 		rtlhal->last_hmeboxnum = boxnum + 1;
@@ -199,8 +207,9 @@ static void _rtl8723be_fill_h2c_command(struct ieee80211_hw *hw, u8 element_id,
 
 		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
 			 "pHalData->last_hmeboxnum  = %d\n",
-			 rtlhal->last_hmeboxnum);
+			  rtlhal->last_hmeboxnum);
 	}
+
 	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
 	rtlhal->h2c_setinprogress = false;
 	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
@@ -219,6 +228,7 @@ void rtl8723be_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 			  "return H2C cmd because of Fw download fail!!!\n");
 		return;
 	}
+
 	memset(tmp_cmdbuf, 0, 8);
 	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
 	_rtl8723be_fill_h2c_command(hw, element_id, cmd_len,
@@ -229,17 +239,17 @@ void rtl8723be_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 u1_h2c_set_pwrmode[H2C_8723BE_PWEMODE_LENGTH] = { 0 };
+	u8 u1_h2c_set_pwrmode[H2C_PWEMODE_LENGTH] = { 0 };
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
 	u8 rlbm, power_state = 0;
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
 
 	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
-	rlbm = 0;/*YJ, temp, 120316. FW now not support RLBM = 2.*/
+	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
 	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
 	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
 					 (rtlpriv->mac80211.p2p) ?
-					 ppsc->smart_ps : 1);
+					  ppsc->smart_ps : 1);
 	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
 					       ppsc->reg_max_lps_awakeintvl);
 	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
@@ -251,44 +261,26 @@ void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
 		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
-		      u1_h2c_set_pwrmode, H2C_8723BE_PWEMODE_LENGTH);
-	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_SETPWRMODE,
-			       H2C_8723BE_PWEMODE_LENGTH,
+		      u1_h2c_set_pwrmode, H2C_PWEMODE_LENGTH);
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_SETPWRMODE, H2C_PWEMODE_LENGTH,
 			       u1_h2c_set_pwrmode);
 }
 
-static bool _rtl8723be_cmd_send_packet(struct ieee80211_hw *hw,
-				       struct sk_buff *skb)
+void rtl8723be_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	struct sk_buff *pskb = NULL;
-	u8 own;
-	unsigned long flags;
-
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-
-	pdesc = &ring->desc[0];
-	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc, true, HW_DESC_OWN);
-
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *)pdesc, 1, 1, skb);
-
-	__skb_queue_tail(&ring->queue, skb);
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
-
-	return true;
+	u8 parm[3] = { 0, 0, 0 };
+	/* parm[0]: bit0=0-->Disconnect, bit0=1-->Connect
+	 *          bit1=0-->update Media Status to MACID
+	 *          bit1=1-->update Media Status from MACID to MACID_End
+	 * parm[1]: MACID, if this is INFRA_STA, MacID = 0
+	 * parm[2]: MACID_End
+	*/
+	SET_H2CCMD_MSRRPT_PARM_OPMODE(parm, mstatus);
+	SET_H2CCMD_MSRRPT_PARM_MACID_IND(parm, 0);
+
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_MSRRPT, 3, parm);
 }
+
 #define BEACON_PG		0 /* ->1 */
 #define PSPOLL_PG		2
 #define NULL_PG			3
@@ -407,7 +399,7 @@ static u8 reserved_page_packet[TOTAL_RESERVED_PKT_LEN] = {
 };
 
 void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
-				  bool dl_finished)
+				  bool b_dl_finished)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
@@ -416,7 +408,7 @@ void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
 	u32 totalpacketlen;
 	bool rtstatus;
 	u8 u1rsvdpageloc[5] = { 0 };
-	bool dlok = false;
+	bool b_dlok = false;
 
 	u8 *beacon;
 	u8 *p_pspoll;
@@ -466,43 +458,40 @@ void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
 	totalpacketlen = TOTAL_RESERVED_PKT_LEN;
 
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
-		      "rtl8723be_set_fw_rsvdpagepkt(): "
-		      "HW_VAR_SET_TX_CMD: ALL\n",
+		      "rtl8723be_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
 		      &reserved_page_packet[0], totalpacketlen);
 	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl8723be_set_fw_rsvdpagepkt(): "
-		      "HW_VAR_SET_TX_CMD: ALL\n", u1rsvdpageloc, 3);
-
+		      "rtl8723be_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL\n",
+		      u1rsvdpageloc, 3);
 
 	skb = dev_alloc_skb(totalpacketlen);
 	memcpy((u8 *)skb_put(skb, totalpacketlen),
 	       &reserved_page_packet, totalpacketlen);
 
-	rtstatus = _rtl8723be_cmd_send_packet(hw, skb);
+	rtstatus = rtl_cmd_send_packet(hw, skb);
 
 	if (rtstatus)
-		dlok = true;
+		b_dlok = true;
 
-	if (dlok) {
+	if (b_dlok) {
 		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
 			 "Set RSVD page location to Fw.\n");
 		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG, "H2C_RSVDPAGE:\n",
 			      u1rsvdpageloc, 3);
-		rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_RSVDPAGE,
+		rtl8723be_fill_h2c_cmd(hw, H2C_8723B_RSVDPAGE,
 				       sizeof(u1rsvdpageloc), u1rsvdpageloc);
-	} else {
+	} else
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
-	}
 }
 
 /*Should check FW support p2p or not.*/
 static void rtl8723be_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw,
 					     u8 ctwindow)
 {
-	u8 u1_ctwindow_period[1] = {ctwindow};
+	u8 u1_ctwindow_period[1] = { ctwindow};
 
-	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_P2P_PS_CTW_CMD, 1,
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_P2P_PS_CTW_CMD, 1,
 			       u1_ctwindow_period);
 }
 
@@ -521,7 +510,7 @@ void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
 	switch (p2p_ps_state) {
 	case P2P_PS_DISABLE:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
-		memset(p2p_ps_offload, 0, sizeof(struct p2p_ps_offload_t));
+		memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
 		break;
 	case P2P_PS_ENABLE:
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
@@ -532,7 +521,7 @@ void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
 			rtl8723be_set_p2p_ctw_period_cmd(hw, ctwindow);
 		}
 		/* hw only support 2 set of NoA */
-		for (i = 0; i < p2pinfo->noa_num; i++) {
+		for (i = 0 ; i < p2pinfo->noa_num ; i++) {
 			/* To control the register setting
 			 * for which NOA
 			 */
@@ -563,6 +552,7 @@ void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
 			rtl_write_dword(rtlpriv, 0x5EC,
 					p2pinfo->noa_count_type[i]);
 		}
+
 		if ((p2pinfo->opp_ps == 1) ||
 		    (p2pinfo->noa_num > 0)) {
 			/* rst p2p circuit */
@@ -591,30 +581,60 @@ void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
 	default:
 		break;
 	}
-	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_P2P_PS_OFFLOAD, 1,
+
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_P2P_PS_OFFLOAD, 1,
 			       (u8 *)p2p_ps_offload);
 }
 
-void rtl8723be_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
+static void _rtl8723be_c2h_content_parsing(struct ieee80211_hw *hw,
+					   u8 c2h_cmd_id,
+					   u8 c2h_cmd_len, u8 *tmp_buf)
 {
-	u8 u1_joinbssrpt_parm[1] = { 0 };
-
-	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_JOINBSSRPT, 1,
-			       u1_joinbssrpt_parm);
+	switch (c2h_cmd_id) {
+	case C2H_8723B_DBG:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8723BE_DBG!!\n");
+		break;
+	case C2H_8723B_TX_REPORT:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8723BE_TX_REPORT!\n");
+		break;
+	case C2H_8723B_BT_INFO:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8723BE_BT_INFO!!\n");
+		rtlpriv->btcoexist.btc_ops->btc_btinfo_notify(rtlpriv, tmp_buf,
+							      c2h_cmd_len);
+		break;
+	case C2H_8723B_BT_MP:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], C2H_8723BE_BT_MP!!\n");
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+			 "[C2H], Unkown packet!! CmdId(%#X)!\n", c2h_cmd_id);
+		break;
+	}
 }
 
-void rtl8723be_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
-				      u8 ap_offload_enable)
+void rtl8723be_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 len)
 {
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u8 u1_apoffload_parm[H2C_8723BE_AP_OFFLOAD_LENGTH] = { 0 };
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 c2h_cmd_id = 0, c2h_cmd_seq = 0, c2h_cmd_len = 0;
+	u8 *tmp_buf = NULL;
+
+	c2h_cmd_id = buffer[0];
+	c2h_cmd_seq = buffer[1];
+	c2h_cmd_len = len - 2;
+	tmp_buf = buffer + 2;
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
+		 "[C2H packet], c2hCmdId=0x%x, c2hCmdSeq=0x%x, c2hCmdLen=%d\n",
+		 c2h_cmd_id, c2h_cmd_seq, c2h_cmd_len);
 
-	SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
-	SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->hiddenssid);
-	SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
+	RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_TRACE,
+		      "[C2H packet], Content Hex:\n", tmp_buf, c2h_cmd_len);
 
-	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_AP_OFFLOAD,
-			       H2C_8723BE_AP_OFFLOAD_LENGTH, u1_apoffload_parm);
+	_rtl8723be_c2h_content_parsing(hw, c2h_cmd_id, c2h_cmd_len, tmp_buf);
 }
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/fw.h b/drivers/net/wireless/rtlwifi/rtl8723be/fw.h
index 31eec281e446..067429669bda 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/fw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/fw.h
@@ -30,50 +30,23 @@
 #define FW_8192C_END_ADDRESS			0x5FFF
 #define FW_8192C_PAGE_SIZE			4096
 #define FW_8192C_POLLING_DELAY			5
-#define FW_8192C_POLLING_TIMEOUT_COUNT		6000
 
-#define IS_FW_HEADER_EXIST(_pfwhdr)	\
-	((_pfwhdr->signature&0xFFF0) == 0x5300)
 #define USE_OLD_WOWLAN_DEBUG_FW			0
 
-#define H2C_8723BE_RSVDPAGE_LOC_LEN		5
-#define H2C_8723BE_PWEMODE_LENGTH		5
-#define H2C_8723BE_JOINBSSRPT_LENGTH		1
-#define H2C_8723BE_AP_OFFLOAD_LENGTH		3
-#define H2C_8723BE_WOWLAN_LENGTH		3
-#define H2C_8723BE_KEEP_ALIVE_CTRL_LENGTH	3
-#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
-#define H2C_8723BE_REMOTE_WAKE_CTRL_LEN		1
-#else
-#define H2C_8723BE_REMOTE_WAKE_CTRL_LEN		3
-#endif
-#define H2C_8723BE_AOAC_GLOBAL_INFO_LEN		2
-#define H2C_8723BE_AOAC_RSVDPAGE_LOC_LEN	7
-
+#define H2C_PWEMODE_LENGTH			5
 
 /* Fw PS state for RPWM.
 *BIT[2:0] = HW state
 *BIT[3] = Protocol PS state, 1: register active state , 0: register sleep state
 *BIT[4] = sub-state
 */
-#define	FW_PS_GO_ON		BIT(0)
-#define	FW_PS_TX_NULL		BIT(1)
 #define	FW_PS_RF_ON		BIT(2)
 #define	FW_PS_REGISTER_ACTIVE	BIT(3)
 
-#define	FW_PS_DPS		BIT(0)
-#define	FW_PS_LCLK		(FW_PS_DPS)
-#define	FW_PS_RF_OFF		BIT(1)
-#define	FW_PS_ALL_ON		BIT(2)
-#define	FW_PS_ST_ACTIVE	BIT(3)
-#define	FW_PS_ISR_ENABLE	BIT(4)
-#define	FW_PS_IMR_ENABLE	BIT(5)
-
-
 #define	FW_PS_ACK		BIT(6)
 #define	FW_PS_TOGGLE		BIT(7)
 
- /* 88E RPWM value*/
+ /* 8723BE RPWM value*/
  /* BIT[0] = 1: 32k, 0: 40M*/
 #define	FW_PS_CLOCK_OFF		BIT(0)		/* 32k*/
 #define	FW_PS_CLOCK_ON		0		/*40M*/
@@ -83,75 +56,61 @@
 /*ISR_ENABLE, IMR_ENABLE, and PS mode should be inherited.*/
 #define	FW_PS_STATE_INT_MASK	(0x3F)
 
-#define	FW_PS_STATE(x)	(FW_PS_STATE_MASK & (x))
-#define	FW_PS_STATE_HW(x)	(FW_PS_STATE_HW_MASK & (x))
-#define	FW_PS_STATE_INT(x)	(FW_PS_STATE_INT_MASK & (x))
-#define	FW_PS_ISR_VAL(x)	((x) & 0x70)
-#define	FW_PS_IMR_MASK(x)	((x) & 0xDF)
-#define	FW_PS_KEEP_IMR(x)	((x) & 0x20)
-
-
-#define	FW_PS_STATE_S0		(FW_PS_DPS)
-#define	FW_PS_STATE_S1		(FW_PS_LCLK)
-#define	FW_PS_STATE_S2		(FW_PS_RF_OFF)
-#define	FW_PS_STATE_S3		(FW_PS_ALL_ON)
-#define	FW_PS_STATE_S4		((FW_PS_ST_ACTIVE) | (FW_PS_ALL_ON))
+#define	FW_PS_STATE(x)		(FW_PS_STATE_MASK & (x))
 
 /* ((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))*/
-#define	FW_PS_STATE_ALL_ON_88E	(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_ALL_ON	(FW_PS_CLOCK_ON)
 /* (FW_PS_RF_ON)*/
-#define	FW_PS_STATE_RF_ON_88E	(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_ON	(FW_PS_CLOCK_ON)
 /* 0x0*/
-#define	FW_PS_STATE_RF_OFF_88E	(FW_PS_CLOCK_ON)
+#define	FW_PS_STATE_RF_OFF	(FW_PS_CLOCK_ON)
 /* (FW_PS_STATE_RF_OFF)*/
-#define	FW_PS_STATE_RF_OFF_LOW_PWR_88E	(FW_PS_CLOCK_OFF)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR	(FW_PS_CLOCK_OFF)
 
-#define	FW_PS_STATE_ALL_ON_92C	(FW_PS_STATE_S4)
-#define	FW_PS_STATE_RF_ON_92C		(FW_PS_STATE_S3)
-#define	FW_PS_STATE_RF_OFF_92C	(FW_PS_STATE_S2)
-#define	FW_PS_STATE_RF_OFF_LOW_PWR_92C	(FW_PS_STATE_S1)
 
-
-/* For 88E H2C PwrMode Cmd ID 5.*/
+/* For 8723BE H2C PwrMode Cmd ID 5.*/
 #define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
 #define	FW_PWR_STATE_RF_OFF	0
 
-#define	FW_PS_IS_ACK(x)	((x) & FW_PS_ACK)
-#define	FW_PS_IS_CLK_ON(x)	((x) & (FW_PS_RF_OFF | FW_PS_ALL_ON))
-#define	FW_PS_IS_RF_ON(x)	((x) & (FW_PS_ALL_ON))
-#define	FW_PS_IS_ACTIVE(x)	((x) & (FW_PS_ST_ACTIVE))
-#define	FW_PS_IS_CPWM_INT(x)	((x) & 0x40)
-
-#define	FW_CLR_PS_STATE(x)	((x) = ((x) & (0xF0)))
+#define	FW_PS_IS_ACK(x)		((x) & FW_PS_ACK)
 
-#define	IS_IN_LOW_POWER_STATE_88E(fwpsstate)		\
-			(FW_PS_STATE(fwpsstate) == FW_PS_CLOCK_OFF)
+#define	IS_IN_LOW_POWER_STATE(__fwpsstate)	\
+	(FW_PS_STATE(__fwpsstate) == FW_PS_CLOCK_OFF)
 
 #define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
 #define	FW_PWR_STATE_RF_OFF	0
 
-#define pagenum_128(_len)	(u32)(((_len)>>7) + ((_len)&0x7F ? 1 : 0))
-
-#define SET_88E_H2CCMD_WOWLAN_FUNC_ENABLE(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 1, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 2, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 3, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_ALL_PKT_DROP(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 4, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_GPIO_ACTIVE(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 5, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_REKEY_WAKE_UP(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 6, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 7, 1, __val)
-#define SET_88E_H2CCMD_WOWLAN_GPIONUM(__ph2ccmd, __val)			\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
-#define SET_88E_H2CCMD_WOWLAN_GPIO_DURATION(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+enum rtl8723b_h2c_cmd {
+	H2C_8723B_RSVDPAGE = 0,
+	H2C_8723B_MSRRPT = 1,
+	H2C_8723B_SCAN = 2,
+	H2C_8723B_KEEP_ALIVE_CTRL = 3,
+	H2C_8723B_DISCONNECT_DECISION = 4,
+	H2C_8723B_BCN_RSVDPAGE = 9,
+	H2C_8723B_PROBERSP_RSVDPAGE = 10,
+
+	H2C_8723B_SETPWRMODE = 0x20,
+	H2C_8723B_PS_LPS_PARA = 0x23,
+	H2C_8723B_P2P_PS_OFFLOAD = 0x24,
+
+	H2C_8723B_RA_MASK = 0x40,
+	H2C_RSSIBE_REPORT = 0x42,
+	/*Not defined CTW CMD for P2P yet*/
+	H2C_8723B_P2P_PS_CTW_CMD,
+	MAX_8723B_H2CCMD
+};
+
+enum rtl8723b_c2h_evt {
+	C2H_8723B_DBG = 0,
+	C2H_8723B_LB = 1,
+	C2H_8723B_TXBF = 2,
+	C2H_8723B_TX_REPORT = 3,
+	C2H_8723B_BT_INFO = 9,
+	C2H_8723B_BT_MP = 11,
+	MAX_8723B_C2HEVENT
+};
+
+#define pagenum_128(_len) (u32)(((_len)>>7) + ((_len)&0x7F ? 1 : 0))
 
 
 #define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
@@ -169,8 +128,11 @@
 #define GET_88E_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd)			\
 	LE_BITS_TO_1BYTE(__ph2ccmd, 0, 8)
 
-#define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_MSRRPT_PARM_OPMODE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 1, __val)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_IND(__ph2ccmd, __val)	\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 1, 1, __val)
+
 #define SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
 #define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__ph2ccmd, __val)		\
@@ -178,71 +140,13 @@
 #define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
 	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
 
-/* AP_OFFLOAD */
-#define SET_H2CCMD_AP_OFFLOAD_ON(__ph2ccmd, __val)			\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_H2CCMD_AP_OFFLOAD_HIDDEN(__ph2ccmd, __val)			\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
-#define SET_H2CCMD_AP_OFFLOAD_DENYANY(__ph2ccmd, __val)			\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
-#define SET_H2CCMD_AP_OFFLOAD_WAKEUP_EVT_RPT(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+3, 0, 8, __val)
 
-/* Keep Alive Control*/
-#define SET_88E_H2CCMD_KEEP_ALIVE_ENABLE(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 1, __val)
-#define SET_88E_H2CCMD_KEEP_ALIVE_ACCPEPT_USER_DEFINED(__ph2ccmd, __val)\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 1, 1, __val)
-#define SET_88E_H2CCMD_KEEP_ALIVE_PERIOD(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
-
-/*REMOTE_WAKE_CTRL */
-#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_EN(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 1, __val)
-#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
-#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(__ph2ccmd, __val)\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 1, 1, __val)
-#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_NDP_OFFLOAD_EN(__ph2ccmd, __val)\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 2, 1, __val)
-#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(__ph2ccmd, __val)\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 3, 1, __val)
-#else
-#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_PAIRWISE_ENC_ALG(__ph2ccmd, __val)\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
-#define SET_88E_H2CCMD_REMOTE_WAKE_CTRL_GROUP_ENC_ALG(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
-#endif
-
-/* GTK_OFFLOAD */
-#define SET_88E_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(__ph2ccmd, __val)\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_88E_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
-
-/* AOAC_RSVDPAGE_LOC */
-#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_REM_WAKE_CTRL_INFO(__ph2ccmd, __val)\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd), 0, 8, __val)
-#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
-#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
-#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+3, 0, 8, __val)
-#define SET_88E_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(__ph2ccmd, __val)	\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+4, 0, 8, __val)
-
-void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
-void rtl8723be_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
-				      u8 ap_offload_enable);
 void rtl8723be_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
 			    u32 cmd_len, u8 *p_cmdbuffer);
-void rtl8723be_firmware_selfreset(struct ieee80211_hw *hw);
-void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
-				  bool dl_finished);
-void rtl8723be_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
-int rtl8723be_download_fw(struct ieee80211_hw *hw,
-			  bool buse_wake_on_wlan_fw);
-void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
-				      u8 p2p_ps_state);
+void rtl8723be_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl8723be_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw, u8 mstatus);
+void rtl8723be_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
+void rtl8723be_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
+void rtl8723be_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 len);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/hw.c b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
index 3cd286930fe0..6dad28e77bbb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/hw.c
@@ -33,12 +33,14 @@
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
+#include "../rtl8723com/phy_common.h"
 #include "dm.h"
 #include "../rtl8723com/dm_common.h"
 #include "fw.h"
 #include "../rtl8723com/fw_common.h"
 #include "led.h"
 #include "hw.h"
+#include "../pwrseqcmd.h"
 #include "pwrseq.h"
 #include "../btcoexist/rtl_btc.h"
 
@@ -49,7 +51,9 @@ static void _rtl8723be_return_beacon_queue_skb(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+	unsigned long flags;
 
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
 	while (skb_queue_len(&ring->queue)) {
 		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
 		struct sk_buff *skb = __skb_dequeue(&ring->queue);
@@ -61,6 +65,7 @@ static void _rtl8723be_return_beacon_queue_skb(struct ieee80211_hw *hw)
 		kfree_skb(skb);
 		ring->idx = (ring->idx + 1) % ring->entries;
 	}
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
 }
 
 static void _rtl8723be_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
@@ -72,7 +77,7 @@ static void _rtl8723be_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
 	rtlpci->reg_bcn_ctrl_val |= set_bits;
 	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
 
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlpci->reg_bcn_ctrl_val);
 }
 
 static void _rtl8723be_stop_tx_beacon(struct ieee80211_hw *hw)
@@ -112,15 +117,15 @@ static void _rtl8723be_disable_bcn_sub_func(struct ieee80211_hw *hw)
 }
 
 static void _rtl8723be_set_fw_clock_on(struct ieee80211_hw *hw, u8 rpwm_val,
-				       bool need_turn_off_ckk)
+				       bool b_need_turn_off_ckk)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool support_remote_wake_up;
+	bool b_support_remote_wake_up;
 	u32 count = 0, isr_regaddr, content;
-	bool schedule_timer = need_turn_off_ckk;
+	bool b_schedule_timer = b_need_turn_off_ckk;
 	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
-				      (u8 *)(&support_remote_wake_up));
+				      (u8 *)(&b_support_remote_wake_up));
 
 	if (!rtlhal->fw_ready)
 		return;
@@ -145,9 +150,10 @@ static void _rtl8723be_set_fw_clock_on(struct ieee80211_hw *hw, u8 rpwm_val,
 			break;
 		}
 	}
-	if (IS_IN_LOW_POWER_STATE_88E(rtlhal->fw_ps_state)) {
+
+	if (IS_IN_LOW_POWER_STATE(rtlhal->fw_ps_state)) {
 		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_SET_RPWM,
-					      &rpwm_val);
+					      (u8 *)(&rpwm_val));
 		if (FW_PS_IS_ACK(rpwm_val)) {
 			isr_regaddr = REG_HISR;
 			content = rtl_read_dword(rtlpriv, isr_regaddr);
@@ -159,20 +165,19 @@ static void _rtl8723be_set_fw_clock_on(struct ieee80211_hw *hw, u8 rpwm_val,
 
 			if (content & IMR_CPWM) {
 				rtl_write_word(rtlpriv, isr_regaddr, 0x0100);
-				rtlhal->fw_ps_state = FW_PS_STATE_RF_ON_88E;
+				rtlhal->fw_ps_state = FW_PS_STATE_RF_ON;
 				RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
-					 "Receive CPWM INT!!! Set "
-					 "pHalData->FwPSState = %X\n",
+					 "Receive CPWM INT!!! Set pHalData->FwPSState = %X\n",
 					 rtlhal->fw_ps_state);
 			}
 		}
+
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
 		rtlhal->fw_clk_change_in_progress = false;
 		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-		if (schedule_timer) {
+		if (b_schedule_timer)
 			mod_timer(&rtlpriv->works.fw_clockoff_timer,
 				  jiffies + MSECS(10));
-		}
 	} else  {
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
 		rtlhal->fw_clk_change_in_progress = false;
@@ -187,7 +192,7 @@ static void _rtl8723be_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl8192_tx_ring *ring;
 	enum rf_pwrstate rtstate;
-	bool schedule_timer = false;
+	bool b_schedule_timer = false;
 	u8 queue;
 
 	if (!rtlhal->fw_ready)
@@ -203,17 +208,18 @@ static void _rtl8723be_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
 	for (queue = 0; queue < RTL_PCI_MAX_TX_QUEUE_COUNT; queue++) {
 		ring = &rtlpci->tx_ring[queue];
 		if (skb_queue_len(&ring->queue)) {
-			schedule_timer = true;
+			b_schedule_timer = true;
 			break;
 		}
 	}
-	if (schedule_timer) {
+
+	if (b_schedule_timer) {
 		mod_timer(&rtlpriv->works.fw_clockoff_timer,
 			  jiffies + MSECS(10));
 		return;
 	}
-	if (FW_PS_STATE(rtlhal->fw_ps_state) !=
-	    FW_PS_STATE_RF_OFF_LOW_PWR_88E) {
+
+	if (FW_PS_STATE(rtlhal->fw_ps_state) != FW_PS_STATE_RF_OFF_LOW_PWR) {
 		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
 		if (!rtlhal->fw_clk_change_in_progress) {
 			rtlhal->fw_clk_change_in_progress = true;
@@ -221,7 +227,7 @@ static void _rtl8723be_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
 			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
 			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-						      &rpwm_val);
+						      (u8 *)(&rpwm_val));
 			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
 			rtlhal->fw_clk_change_in_progress = false;
 			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
@@ -231,12 +237,13 @@ static void _rtl8723be_set_fw_clock_off(struct ieee80211_hw *hw, u8 rpwm_val)
 				  jiffies + MSECS(10));
 		}
 	}
+
 }
 
 static void _rtl8723be_set_fw_ps_rf_on(struct ieee80211_hw *hw)
 {
 	u8 rpwm_val = 0;
-	rpwm_val |= (FW_PS_STATE_RF_OFF_88E | FW_PS_ACK);
+	rpwm_val |= (FW_PS_STATE_RF_OFF | FW_PS_ACK);
 	_rtl8723be_set_fw_clock_on(hw, rpwm_val, true);
 }
 
@@ -249,21 +256,23 @@ static void _rtl8723be_fwlps_leave(struct ieee80211_hw *hw)
 	u8 rpwm_val = 0, fw_pwrmode = FW_PS_ACTIVE_MODE;
 
 	if (ppsc->low_power_enable) {
-		rpwm_val = (FW_PS_STATE_ALL_ON_88E | FW_PS_ACK);/* RF on */
+		rpwm_val = (FW_PS_STATE_ALL_ON | FW_PS_ACK);/* RF on */
 		_rtl8723be_set_fw_clock_on(hw, rpwm_val, false);
 		rtlhal->allow_sw_to_change_hwclc = false;
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      &fw_pwrmode);
+					      (u8 *)(&fw_pwrmode));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
 					      (u8 *)(&fw_current_inps));
 	} else {
-		rpwm_val = FW_PS_STATE_ALL_ON_88E;	/* RF on */
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM, &rpwm_val);
+		rpwm_val = FW_PS_STATE_ALL_ON;	/* RF on */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *)(&rpwm_val));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      &fw_pwrmode);
+					      (u8 *)(&fw_pwrmode));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
 					      (u8 *)(&fw_current_inps));
 	}
+
 }
 
 static void _rtl8723be_fwlps_enter(struct ieee80211_hw *hw)
@@ -275,22 +284,23 @@ static void _rtl8723be_fwlps_enter(struct ieee80211_hw *hw)
 	u8 rpwm_val;
 
 	if (ppsc->low_power_enable) {
-		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR_88E;	/* RF off */
+		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR;	/* RF off */
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
 					      (u8 *)(&fw_current_inps));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      &ppsc->fwctrl_psmode);
+					      (u8 *)(&ppsc->fwctrl_psmode));
 		rtlhal->allow_sw_to_change_hwclc = true;
 		_rtl8723be_set_fw_clock_off(hw, rpwm_val);
-
 	} else {
-		rpwm_val = FW_PS_STATE_RF_OFF_88E;	/* RF off */
+		rpwm_val = FW_PS_STATE_RF_OFF;	/* RF off */
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
 					      (u8 *)(&fw_current_inps));
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-					      &ppsc->fwctrl_psmode);
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM, &rpwm_val);
+					      (u8 *)(&ppsc->fwctrl_psmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+					      (u8 *)(&rpwm_val));
 	}
+
 }
 
 void rtl8723be_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
@@ -306,13 +316,13 @@ void rtl8723be_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_RF_STATE:
 		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
 		break;
-	case HW_VAR_FWLPS_RF_ON: {
-		enum rf_pwrstate rfstate;
+	case HW_VAR_FWLPS_RF_ON:{
+		enum rf_pwrstate rfState;
 		u32 val_rcr;
 
 		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE,
-					      (u8 *)(&rfstate));
-		if (rfstate == ERFOFF) {
+					      (u8 *)(&rfState));
+		if (rfState == ERFOFF) {
 			*((bool *)(val)) = true;
 		} else {
 			val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
@@ -322,11 +332,12 @@ void rtl8723be_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			else
 				*((bool *)(val)) = true;
 		}
-		break; }
+		}
+		break;
 	case HW_VAR_FW_PSMODE_STATUS:
 		*((bool *)(val)) = ppsc->fw_current_inpsmode;
 		break;
-	case HW_VAR_CORRECT_TSF: {
+	case HW_VAR_CORRECT_TSF:{
 		u64 tsf;
 		u32 *ptsf_low = (u32 *)&tsf;
 		u32 *ptsf_high = ((u32 *)&tsf) + 1;
@@ -335,15 +346,65 @@ void rtl8723be_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
 
 		*((u64 *)(val)) = tsf;
-
-		break; }
+		}
+		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 			 "switch case not process %x\n", variable);
 		break;
 	}
 }
 
+static void _rtl8723be_download_rsvd_page(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp_regcr, tmp_reg422, bcnvalid_reg;
+	u8 count = 0, dlbcn_count = 0;
+	bool b_recover = false;
+
+	tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1,
+		       (tmp_regcr | BIT(0)));
+
+	_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(3));
+	_rtl8723be_set_bcn_ctrl_reg(hw, BIT(4), 0);
+
+	tmp_reg422 = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp_reg422 & (~BIT(6)));
+	if (tmp_reg422 & BIT(6))
+		b_recover = true;
+
+	do {
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL + 2);
+		rtl_write_byte(rtlpriv, REG_TDECTRL + 2,
+			       (bcnvalid_reg | BIT(0)));
+		_rtl8723be_return_beacon_queue_skb(hw);
+
+		rtl8723be_set_fw_rsvdpagepkt(hw, 0);
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL + 2);
+		count = 0;
+		while (!(bcnvalid_reg & BIT(0)) && count < 20) {
+			count++;
+			udelay(10);
+			bcnvalid_reg = rtl_read_byte(rtlpriv,
+						     REG_TDECTRL + 2);
+		}
+		dlbcn_count++;
+	} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
+
+	if (bcnvalid_reg & BIT(0))
+		rtl_write_byte(rtlpriv, REG_TDECTRL + 2, BIT(0));
+
+	_rtl8723be_set_bcn_ctrl_reg(hw, BIT(3), 0);
+	_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(4));
+
+	if (b_recover)
+		rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp_reg422);
+
+	tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, (tmp_regcr & ~(BIT(0))));
+}
+
 void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -358,22 +419,24 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		for (idx = 0; idx < ETH_ALEN; idx++)
 			rtl_write_byte(rtlpriv, (REG_MACID + idx), val[idx]);
 		break;
-	case HW_VAR_BASIC_RATE: {
-		u16 rate_cfg = ((u16 *)val)[0];
+	case HW_VAR_BASIC_RATE:{
+		u16 b_rate_cfg = ((u16 *)val)[0];
 		u8 rate_index = 0;
-		rate_cfg = rate_cfg & 0x15f;
-		rate_cfg |= 0x01;
-		rtl_write_byte(rtlpriv, REG_RRSR, rate_cfg & 0xff);
-		rtl_write_byte(rtlpriv, REG_RRSR + 1, (rate_cfg >> 8) & 0xff);
-		while (rate_cfg > 0x1) {
-			rate_cfg = (rate_cfg >> 1);
+		b_rate_cfg = b_rate_cfg & 0x15f;
+		b_rate_cfg |= 0x01;
+		rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
+		rtl_write_byte(rtlpriv, REG_RRSR + 1, (b_rate_cfg >> 8) & 0xff);
+		while (b_rate_cfg > 0x1) {
+			b_rate_cfg = (b_rate_cfg >> 1);
 			rate_index++;
 		}
 		rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL, rate_index);
-		break; }
+		}
+		break;
 	case HW_VAR_BSSID:
 		for (idx = 0; idx < ETH_ALEN; idx++)
 			rtl_write_byte(rtlpriv, (REG_BSSID + idx), val[idx]);
+
 		break;
 	case HW_VAR_SIFS:
 		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
@@ -388,7 +451,7 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
 				       *((u16 *)val));
 		break;
-	case HW_VAR_SLOT_TIME: {
+	case HW_VAR_SLOT_TIME:{
 		u8 e_aci;
 
 		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
@@ -398,12 +461,13 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 		for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      &e_aci);
+						      (u8 *)(&e_aci));
 		}
-		break; }
-	case HW_VAR_ACK_PREAMBLE: {
+		}
+		break;
+	case HW_VAR_ACK_PREAMBLE:{
 		u8 reg_tmp;
-		u8 short_preamble = (bool)*val;
+		u8 short_preamble = (bool)(*(u8 *)val);
 		reg_tmp = rtl_read_byte(rtlpriv, REG_TRXPTCL_CTL + 2);
 		if (short_preamble) {
 			reg_tmp |= 0x02;
@@ -412,15 +476,16 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 			reg_tmp &= 0xFD;
 			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
 		}
-		break; }
+		}
+		break;
 	case HW_VAR_WPA_CONFIG:
-		rtl_write_byte(rtlpriv, REG_SECCFG, *val);
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *)val));
 		break;
-	case HW_VAR_AMPDU_MIN_SPACE: {
+	case HW_VAR_AMPDU_MIN_SPACE:{
 		u8 min_spacing_to_set;
 		u8 sec_min_space;
 
-		min_spacing_to_set = *val;
+		min_spacing_to_set = *((u8 *)val);
 		if (min_spacing_to_set <= 7) {
 			sec_min_space = 0;
 
@@ -434,26 +499,28 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
 				 "Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
-				 mac->min_space_cfg);
+				  mac->min_space_cfg);
 
 			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
 				       mac->min_space_cfg);
 		}
-		break; }
-	case HW_VAR_SHORTGI_DENSITY: {
+		}
+		break;
+	case HW_VAR_SHORTGI_DENSITY:{
 		u8 density_to_set;
 
-		density_to_set = *val;
+		density_to_set = *((u8 *)val);
 		mac->min_space_cfg |= (density_to_set << 3);
 
 		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
 			 "Set HW_VAR_SHORTGI_DENSITY: %#x\n",
-			 mac->min_space_cfg);
+			  mac->min_space_cfg);
 
 		rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
 			       mac->min_space_cfg);
-		break; }
-	case HW_VAR_AMPDU_FACTOR: {
+		}
+		break;
+	case HW_VAR_AMPDU_FACTOR:{
 		u8 regtoset_normal[4] = {0x41, 0xa8, 0x72, 0xb9};
 		u8 factor_toset;
 		u8 *p_regtoset = NULL;
@@ -461,7 +528,7 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 		p_regtoset = regtoset_normal;
 
-		factor_toset = *val;
+		factor_toset = *((u8 *)val);
 		if (factor_toset <= 3) {
 			factor_toset = (1 << (factor_toset + 2));
 			if (factor_toset > 0xf)
@@ -482,22 +549,26 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				rtl_write_byte(rtlpriv,
 					       (REG_AGGLEN_LMT + index),
 					       p_regtoset[index]);
+
 			}
+
 			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
 				 "Set HW_VAR_AMPDU_FACTOR: %#x\n",
-				 factor_toset);
+				  factor_toset);
 		}
-		break; }
-	case HW_VAR_AC_PARAM: {
-		u8 e_aci = *val;
+		}
+		break;
+	case HW_VAR_AC_PARAM:{
+		u8 e_aci = *((u8 *)val);
 		rtl8723_dm_init_edca_turbo(hw);
 
 		if (rtlpci->acm_method != EACMWAY2_SW)
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACM_CTRL,
-						      &e_aci);
-		break; }
-	case HW_VAR_ACM_CTRL: {
-		u8 e_aci = *val;
+						      (u8 *)(&e_aci));
+		}
+		break;
+	case HW_VAR_ACM_CTRL:{
+		u8 e_aci = *((u8 *)val);
 		union aci_aifsn *p_aci_aifsn =
 				(union aci_aifsn *)(&(mac->ac[0].aifs));
 		u8 acm = p_aci_aifsn->f.acm;
@@ -519,8 +590,8 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				break;
 			default:
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "HW_VAR_ACM_CTRL acm set "
-					  "failed: eACI is %d\n", acm);
+					 "HW_VAR_ACM_CTRL acm set failed: eACI is %d\n",
+					 acm);
 				break;
 			}
 		} else {
@@ -535,27 +606,30 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 				acm_ctrl &= (~ACMHW_BEQEN);
 				break;
 			default:
-				RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 					 "switch case not process\n");
 				break;
 			}
 		}
+
 		RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
-			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] "
-			 "Write 0x%X\n", acm_ctrl);
+			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
+			 acm_ctrl);
 		rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
-		break; }
+		}
+		break;
 	case HW_VAR_RCR:
 		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *)(val))[0]);
 		rtlpci->receive_config = ((u32 *)(val))[0];
 		break;
-	case HW_VAR_RETRY_LIMIT: {
-		u8 retry_limit = *val;
+	case HW_VAR_RETRY_LIMIT:{
+		u8 retry_limit = ((u8 *)(val))[0];
 
 		rtl_write_word(rtlpriv, REG_RL,
 			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
 			       retry_limit << RETRY_LIMIT_LONG_SHIFT);
-		break; }
+		}
+		break;
 	case HW_VAR_DUAL_TSF_RST:
 		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
 		break;
@@ -563,25 +637,27 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 		rtlefuse->efuse_usedbytes = *((u16 *)val);
 		break;
 	case HW_VAR_EFUSE_USAGE:
-		rtlefuse->efuse_usedpercentage = *val;
+		rtlefuse->efuse_usedpercentage = *((u8 *)val);
 		break;
 	case HW_VAR_IO_CMD:
 		rtl8723be_phy_set_io_cmd(hw, (*(enum io_type *)val));
 		break;
-	case HW_VAR_SET_RPWM: {
+	case HW_VAR_SET_RPWM:{
 		u8 rpwm_val;
 
 		rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
 		udelay(1);
 
 		if (rpwm_val & BIT(7)) {
-			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, *val);
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, (*(u8 *)val));
 		} else {
-			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM, *val | BIT(7));
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+				       ((*(u8 *)val) | BIT(7)));
 		}
-		break; }
+		}
+		break;
 	case HW_VAR_H2C_FW_PWRMODE:
-		rtl8723be_set_fw_pwrmode_cmd(hw, *val);
+		rtl8723be_set_fw_pwrmode_cmd(hw, (*(u8 *)val));
 		break;
 	case HW_VAR_FW_PSMODE_STATUS:
 		ppsc->fw_current_inpsmode = *((bool *)val);
@@ -589,85 +665,38 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 	case HW_VAR_RESUME_CLK_ON:
 		_rtl8723be_set_fw_ps_rf_on(hw);
 		break;
-	case HW_VAR_FW_LPS_ACTION: {
-		bool enter_fwlps = *((bool *)val);
+	case HW_VAR_FW_LPS_ACTION:{
+		bool b_enter_fwlps = *((bool *)val);
 
-		if (enter_fwlps)
+		if (b_enter_fwlps)
 			_rtl8723be_fwlps_enter(hw);
 		else
 			_rtl8723be_fwlps_leave(hw);
-
-		break; }
-	case HW_VAR_H2C_FW_JOINBSSRPT: {
-		u8 mstatus = *val;
-		u8 tmp_regcr, tmp_reg422, bcnvalid_reg;
-		u8 count = 0, dlbcn_count = 0;
-		bool recover = false;
+		}
+		break;
+	case HW_VAR_H2C_FW_JOINBSSRPT:{
+		u8 mstatus = (*(u8 *)val);
 
 		if (mstatus == RT_MEDIA_CONNECT) {
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
-
-			tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
-			rtl_write_byte(rtlpriv, REG_CR + 1,
-				       (tmp_regcr | BIT(0)));
-
-			_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(3));
-			_rtl8723be_set_bcn_ctrl_reg(hw, BIT(4), 0);
-
-			tmp_reg422 = rtl_read_byte(rtlpriv,
-						   REG_FWHW_TXQ_CTRL + 2);
-			rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
-				       tmp_reg422 & (~BIT(6)));
-			if (tmp_reg422 & BIT(6))
-				recover = true;
-
-			do {
-				bcnvalid_reg = rtl_read_byte(rtlpriv,
-							     REG_TDECTRL + 2);
-				rtl_write_byte(rtlpriv, REG_TDECTRL + 2,
-					       (bcnvalid_reg | BIT(0)));
-				_rtl8723be_return_beacon_queue_skb(hw);
-
-				rtl8723be_set_fw_rsvdpagepkt(hw, 0);
-				bcnvalid_reg = rtl_read_byte(rtlpriv,
-							     REG_TDECTRL + 2);
-				count = 0;
-				while (!(bcnvalid_reg & BIT(0)) && count < 20) {
-					count++;
-					udelay(10);
-					bcnvalid_reg = rtl_read_byte(rtlpriv,
-							       REG_TDECTRL + 2);
-				}
-				dlbcn_count++;
-			} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
-
-			if (bcnvalid_reg & BIT(0))
-				rtl_write_byte(rtlpriv, REG_TDECTRL+2, BIT(0));
-
-			_rtl8723be_set_bcn_ctrl_reg(hw, BIT(3), 0);
-			_rtl8723be_set_bcn_ctrl_reg(hw, 0, BIT(4));
-
-			if (recover) {
-				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
-					       tmp_reg422);
-			}
-			rtl_write_byte(rtlpriv, REG_CR + 1,
-				       (tmp_regcr & ~(BIT(0))));
+			_rtl8723be_download_rsvd_page(hw);
+		}
+		rtl8723be_set_fw_media_status_rpt_cmd(hw, mstatus);
 		}
-		rtl8723be_set_fw_joinbss_report_cmd(hw, *val);
-		break; }
+		break;
 	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
-		rtl8723be_set_p2p_ps_offload_cmd(hw, *val);
+		rtl8723be_set_p2p_ps_offload_cmd(hw, (*(u8 *)val));
 		break;
-	case HW_VAR_AID: {
+	case HW_VAR_AID:{
 		u16 u2btmp;
 		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
 		u2btmp &= 0xC000;
 		rtl_write_word(rtlpriv, REG_BCN_PSR_RPT,
 			       (u2btmp | mac->assoc_id));
-		break; }
-	case HW_VAR_CORRECT_TSF: {
-		u8 btype_ibss = *val;
+		}
+		break;
+	case HW_VAR_CORRECT_TSF:{
+		u8 btype_ibss = ((u8 *)(val))[0];
 
 		if (btype_ibss)
 			_rtl8723be_stop_tx_beacon(hw);
@@ -683,16 +712,17 @@ void rtl8723be_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
 
 		if (btype_ibss)
 			_rtl8723be_resume_tx_beacon(hw);
-		break; }
-	case HW_VAR_KEEP_ALIVE: {
+		}
+		break;
+	case HW_VAR_KEEP_ALIVE:{
 		u8 array[2];
 		array[0] = 0xff;
-		array[1] = *val;
-		rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_KEEP_ALIVE_CTRL,
-				       2, array);
-		break; }
+		array[1] = *((u8 *)val);
+		rtl8723be_fill_h2c_cmd(hw, H2C_8723B_KEEP_ALIVE_CTRL, 2, array);
+		}
+		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 			 "switch case not process %x\n",
 			 variable);
 		break;
@@ -703,7 +733,7 @@ static bool _rtl8723be_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	bool status = true;
-	int count = 0;
+	long count = 0;
 	u32 value = _LLT_INIT_ADDR(address) | _LLT_INIT_DATA(data) |
 		    _LLT_OP(_LLT_WRITE_ACCESS);
 
@@ -716,8 +746,8 @@ static bool _rtl8723be_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
 
 		if (count > POLLING_LLT_THRESHOLD) {
 			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-				 "Failed to polling write LLT done at "
-				  "address %d!\n", address);
+				 "Failed to polling write LLT done at address %d!\n",
+				 address);
 			status = false;
 			break;
 		}
@@ -731,10 +761,10 @@ static bool _rtl8723be_llt_table_init(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	unsigned short i;
 	u8 txpktbuf_bndy;
-	u8 maxpage;
+	u8 maxPage;
 	bool status;
 
-	maxpage = 255;
+	maxPage = 255;
 	txpktbuf_bndy = 245;
 
 	rtl_write_dword(rtlpriv, REG_TRXFF_BNDY,
@@ -753,17 +783,19 @@ static bool _rtl8723be_llt_table_init(struct ieee80211_hw *hw)
 		if (!status)
 			return status;
 	}
+
 	status = _rtl8723be_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
 
 	if (!status)
 		return status;
 
-	for (i = txpktbuf_bndy; i < maxpage; i++) {
+	for (i = txpktbuf_bndy; i < maxPage; i++) {
 		status = _rtl8723be_llt_write(hw, i, (i + 1));
 		if (!status)
 			return status;
 	}
-	status = _rtl8723be_llt_write(hw, maxpage, txpktbuf_bndy);
+
+	status = _rtl8723be_llt_write(hw, maxPage, txpktbuf_bndy);
 	if (!status)
 		return status;
 
@@ -795,11 +827,9 @@ static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
 	unsigned char bytetmp;
 	unsigned short wordtmp;
-	u16 retry = 0;
-	bool mac_func_enable;
 
 	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x00);
 
@@ -807,12 +837,6 @@ static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
 	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1) & (~BIT(7));
 	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, bytetmp);
 
-	bytetmp = rtl_read_byte(rtlpriv, REG_CR);
-	if (bytetmp == 0xFF)
-		mac_func_enable = true;
-	else
-		mac_func_enable = false;
-
 	/* HW Power on sequence */
 	if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK,
 				      PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
@@ -821,6 +845,10 @@ static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
 			 "init MAC Fail as power on failure\n");
 		return false;
 	}
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_MULTI_FUNC_CTRL);
+	rtl_write_byte(rtlpriv, REG_MULTI_FUNC_CTRL, bytetmp | BIT(3));
+
 	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO) | BIT(4);
 	rtl_write_byte(rtlpriv, REG_APS_FSMCO, bytetmp);
 
@@ -837,25 +865,21 @@ static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
 	bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CFG + 3);
 	if (bytetmp & BIT(0)) {
 		bytetmp = rtl_read_byte(rtlpriv, 0x7c);
-		bytetmp |= BIT(6);
-		rtl_write_byte(rtlpriv, 0x7c, bytetmp);
+		rtl_write_byte(rtlpriv, 0x7c, bytetmp | BIT(6));
 	}
+
 	bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CLKR);
-	bytetmp |= BIT(3);
-	rtl_write_byte(rtlpriv, REG_SYS_CLKR, bytetmp);
+	rtl_write_byte(rtlpriv, REG_SYS_CLKR, bytetmp | BIT(3));
 	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG + 1);
-	bytetmp &= ~BIT(4);
-	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp);
-
-	bytetmp = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG+3);
-	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+3, bytetmp | 0x77);
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp & (~BIT(4)));
 
 	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
 
-	if (!mac_func_enable) {
-		if (!_rtl8723be_llt_table_init(hw))
+	if (!rtlhal->mac_func_enable) {
+		if (_rtl8723be_llt_table_init(hw) == false)
 			return false;
 	}
+
 	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
 	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
 
@@ -873,8 +897,6 @@ static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
 	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xFFFF);
 	rtl_write_dword(rtlpriv, REG_TCR, rtlpci->transmit_config);
 
-	rtl_write_byte(rtlpriv, 0x4d0, 0x0);
-
 	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
 			((u64) rtlpci->tx_ring[BEACON_QUEUE].dma) &
 			DMA_BIT_MASK(32));
@@ -901,57 +923,213 @@ static bool _rtl8723be_init_mac(struct ieee80211_hw *hw)
 
 	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
 
-	bytetmp = rtl_read_byte(rtlpriv, REG_APSD_CTRL);
-	rtl_write_byte(rtlpriv, REG_APSD_CTRL, bytetmp & ~BIT(6));
+	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
 
 	rtl_write_byte(rtlpriv, REG_SECONDARY_CCA_CTRL, 0x3);
 
-	do {
-		retry++;
-		bytetmp = rtl_read_byte(rtlpriv, REG_APSD_CTRL);
-	} while ((retry < 200) && (bytetmp & BIT(7)));
-
-	_rtl8723be_gen_refresh_led_state(hw);
-
-	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0x0);
+	/* <20130114, Kordan> The following setting is
+	 * only for DPDT and Fixed board type.
+	 * TODO:  A better solution is configure it
+	 * according EFUSE during the run-time.
+	 */
+	rtl_set_bbreg(hw, 0x64, BIT(20), 0x0);/* 0x66[4]=0 */
+	rtl_set_bbreg(hw, 0x64, BIT(24), 0x0);/* 0x66[8]=0 */
+	rtl_set_bbreg(hw, 0x40, BIT(4), 0x0)/* 0x40[4]=0 */;
+	rtl_set_bbreg(hw, 0x40, BIT(3), 0x1)/* 0x40[3]=1 */;
+	rtl_set_bbreg(hw, 0x4C, BIT(24) | BIT(23), 0x2)/* 0x4C[24:23]=10 */;
+	rtl_set_bbreg(hw, 0x944, BIT(1) | BIT(0), 0x3)/* 0x944[1:0]=11 */;
+	rtl_set_bbreg(hw, 0x930, MASKBYTE0, 0x77)/* 0x930[7:0]=77 */;
+	rtl_set_bbreg(hw, 0x38, BIT(11), 0x1)/* 0x38[11]=1 */;
 
 	bytetmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
-	rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, bytetmp & ~BIT(2));
+	rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, bytetmp & (~BIT(2)));
 
+	_rtl8723be_gen_refresh_led_state(hw);
 	return true;
 }
 
 static void _rtl8723be_hw_configure(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 reg_bw_opmode;
-	u32 reg_ratr, reg_prsr;
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rrsr;
+
+	reg_rrsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+	/* Init value for RRSR. */
+	rtl_write_dword(rtlpriv, REG_RRSR, reg_rrsr);
+
+	/* ARFB table 9 for 11ac 5G 2SS */
+	rtl_write_dword(rtlpriv, REG_ARFR0 + 4, 0xfffff000);
+
+	/* ARFB table 10 for 11ac 5G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR1 + 4, 0x003ff000);
+
+	/* CF-End setting. */
+	rtl_write_word(rtlpriv, REG_FWHW_TXQ_CTRL, 0x1F00);
+
+	/* 0x456 = 0x70, sugguested by Zhilin */
+	rtl_write_byte(rtlpriv, REG_AMPDU_MAX_TIME, 0x70);
+
+	/* Set retry limit */
+	rtl_write_word(rtlpriv, REG_RL, 0x0707);
+
+	/* Set Data / Response auto rate fallack retry count */
+	rtl_write_dword(rtlpriv, REG_DARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_DARFRC + 4, 0x07060504);
+	rtl_write_dword(rtlpriv, REG_RARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_RARFRC + 4, 0x07060504);
+
+	rtlpci->reg_bcn_ctrl_val = 0x1d;
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, rtlpci->reg_bcn_ctrl_val);
+
+	/* TBTT prohibit hold time. Suggested by designer TimChen. */
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff); /* 8 ms */
+
+	rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0040);
+
+	/*For Rx TP. Suggested by SD1 Richard. Added by tynli. 2010.04.12.*/
+	rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x03086666);
 
-	reg_bw_opmode = BW_OPMODE_20MHZ;
-	reg_ratr = RATE_ALL_CCK | RATE_ALL_OFDM_AG |
-		   RATE_ALL_OFDM_1SS | RATE_ALL_OFDM_2SS;
-	reg_prsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+	rtl_write_byte(rtlpriv, REG_HT_SINGLE_AMPDU, 0x80);
 
-	rtl_write_dword(rtlpriv, REG_RRSR, reg_prsr);
-	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, 0xFF);
+	rtl_write_byte(rtlpriv, REG_RX_PKT_LIMIT, 0x20);
+
+	rtl_write_byte(rtlpriv, REG_MAX_AGGR_NUM, 0x1F);
+}
+
+static u8 _rtl8723be_dbi_read(struct rtl_priv *rtlpriv, u16 addr)
+{
+	u16 read_addr = addr & 0xfffc;
+	u8 ret = 0, tmp = 0, count = 0;
+
+	rtl_write_word(rtlpriv, REG_DBI_ADDR, read_addr);
+	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x2);
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+		count++;
+	}
+	if (0 == tmp) {
+		read_addr = REG_DBI_RDATA + addr % 4;
+		ret = rtl_read_byte(rtlpriv, read_addr);
+	}
+
+	return ret;
+}
+
+static void _rtl8723be_dbi_write(struct rtl_priv *rtlpriv, u16 addr, u8 data)
+{
+	u8 tmp = 0, count = 0;
+	u16 write_addr = 0, remainder = addr % 4;
+
+	/* Write DBI 1Byte Data */
+	write_addr = REG_DBI_WDATA + remainder;
+	rtl_write_byte(rtlpriv, write_addr, data);
+
+	/* Write DBI 2Byte Address & Write Enable */
+	write_addr = (addr & 0xfffc) | (BIT(0) << (remainder + 12));
+	rtl_write_word(rtlpriv, REG_DBI_ADDR, write_addr);
+
+	/* Write DBI Write Flag */
+	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x1);
+
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+		count++;
+	}
+}
+
+static u16 _rtl8723be_mdio_read(struct rtl_priv *rtlpriv, u8 addr)
+{
+	u16 ret = 0;
+	u8 tmp = 0, count = 0;
+
+	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(6));
+	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6);
+		count++;
+	}
+
+	if (0 == tmp)
+		ret = rtl_read_word(rtlpriv, REG_MDIO_RDATA);
+
+	return ret;
+}
+
+static void _rtl8723be_mdio_write(struct rtl_priv *rtlpriv, u8 addr, u16 data)
+{
+	u8 tmp = 0, count = 0;
+
+	rtl_write_word(rtlpriv, REG_MDIO_WDATA, data);
+	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(5));
+	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5);
+		count++;
+	}
 }
 
 static void _rtl8723be_enable_aspm_back_door(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 tmp8 = 0;
+	u16 tmp16 = 0;
 
-	rtl_write_byte(rtlpriv, 0x34b, 0x93);
-	rtl_write_word(rtlpriv, 0x350, 0x870c);
-	rtl_write_byte(rtlpriv, 0x352, 0x1);
+	/* <Roger_Notes> Overwrite following ePHY parameter for
+	 * some platform compatibility issue,
+	 * especially when CLKReq is enabled, 2012.11.09.
+	 */
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x01);
+	if (tmp16 != 0x0663)
+		_rtl8723be_mdio_write(rtlpriv, 0x01, 0x0663);
 
-	if (ppsc->support_backdoor)
-		rtl_write_byte(rtlpriv, 0x349, 0x1b);
-	else
-		rtl_write_byte(rtlpriv, 0x349, 0x03);
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x04);
+	if (tmp16 != 0x7544)
+		_rtl8723be_mdio_write(rtlpriv, 0x04, 0x7544);
+
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x06);
+	if (tmp16 != 0xB880)
+		_rtl8723be_mdio_write(rtlpriv, 0x06, 0xB880);
+
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x07);
+	if (tmp16 != 0x4000)
+		_rtl8723be_mdio_write(rtlpriv, 0x07, 0x4000);
+
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x08);
+	if (tmp16 != 0x9003)
+		_rtl8723be_mdio_write(rtlpriv, 0x08, 0x9003);
+
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x09);
+	if (tmp16 != 0x0D03)
+		_rtl8723be_mdio_write(rtlpriv, 0x09, 0x0D03);
+
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x0A);
+	if (tmp16 != 0x4037)
+		_rtl8723be_mdio_write(rtlpriv, 0x0A, 0x4037);
 
-	rtl_write_word(rtlpriv, 0x350, 0x2718);
-	rtl_write_byte(rtlpriv, 0x352, 0x1);
+	tmp16 = _rtl8723be_mdio_read(rtlpriv, 0x0B);
+	if (tmp16 != 0x0070)
+		_rtl8723be_mdio_write(rtlpriv, 0x0B, 0x0070);
+
+	/* Configuration Space offset 0x70f BIT7 is used to control L0S */
+	tmp8 = _rtl8723be_dbi_read(rtlpriv, 0x70f);
+	_rtl8723be_dbi_write(rtlpriv, 0x70f, tmp8 | BIT(7));
+
+	/* Configuration Space offset 0x719 Bit3 is for L1
+	 * BIT4 is for clock request
+	 */
+	tmp8 = _rtl8723be_dbi_read(rtlpriv, 0x719);
+	_rtl8723be_dbi_write(rtlpriv, 0x719, tmp8 | BIT(3) | BIT(4));
 }
 
 void rtl8723be_enable_hw_security_config(struct ieee80211_hw *hw)
@@ -961,30 +1139,208 @@ void rtl8723be_enable_hw_security_config(struct ieee80211_hw *hw)
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
 		 "PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
-		 rtlpriv->sec.pairwise_enc_algorithm,
-		 rtlpriv->sec.group_enc_algorithm);
+		  rtlpriv->sec.pairwise_enc_algorithm,
+		  rtlpriv->sec.group_enc_algorithm);
 
 	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
 		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 			 "not open hw encryption\n");
 		return;
 	}
+
 	sec_reg_value = SCR_TXENCENABLE | SCR_RXDECENABLE;
 
 	if (rtlpriv->sec.use_defaultkey) {
 		sec_reg_value |= SCR_TXUSEDK;
 		sec_reg_value |= SCR_RXUSEDK;
 	}
+
 	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
 
 	rtl_write_byte(rtlpriv, REG_CR + 1, 0x02);
 
-	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "The SECR-value %x\n",
-		 sec_reg_value);
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+		 "The SECR-value %x\n", sec_reg_value);
 
 	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
 }
 
+static void _rtl8723be_poweroff_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 u1b_tmp;
+
+	rtlhal->mac_func_enable = false;
+	/* Combo (PCIe + USB) Card and PCIe-MF Card */
+	/* 1. Run LPS WL RFOFF flow */
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, RTL8723_NIC_LPS_ENTER_FLOW);
+
+	/* 2. 0x1F[7:0] = 0 */
+	/* turn off RF */
+	/* rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00); */
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) &&
+	    rtlhal->fw_ready) {
+		rtl8723be_firmware_selfreset(hw);
+	}
+
+	/* Reset MCU. Suggested by Filen. */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp & (~BIT(2))));
+
+	/* g.	MCUFWDL 0x80[1:0]=0	 */
+	/* reset MCU ready status */
+	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+
+	/* HW card disable configuration. */
+	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+				 PWR_INTF_PCI_MSK, RTL8723_NIC_DISABLE_FLOW);
+
+	/* Reset MCU IO Wrapper */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1b_tmp | BIT(0));
+
+	/* 7. RSV_CTRL 0x1C[7:0] = 0x0E */
+	/* lock ISO/CLK/Power control register */
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0e);
+}
+
+static bool _rtl8723be_check_pcie_dma_hang(struct rtl_priv *rtlpriv)
+{
+	u8 tmp;
+
+	/* write reg 0x350 Bit[26]=1. Enable debug port. */
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL + 3);
+	if (!(tmp & BIT(2))) {
+		rtl_write_byte(rtlpriv, REG_DBI_CTRL + 3, (tmp | BIT(2)));
+		mdelay(100); /* Suggested by DD Justin_tsai. */
+	}
+
+	/* read reg 0x350 Bit[25] if 1 : RX hang
+	 * read reg 0x350 Bit[24] if 1 : TX hang
+	 */
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL + 3);
+	if ((tmp & BIT(0)) || (tmp & BIT(1))) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "CheckPcieDMAHang8723BE(): true!!\n");
+		return true;
+	}
+	return false;
+}
+
+static void _rtl8723be_reset_pcie_interface_dma(struct rtl_priv *rtlpriv,
+						bool mac_power_on)
+{
+	u8 tmp;
+	bool release_mac_rx_pause;
+	u8 backup_pcie_dma_pause;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "ResetPcieInterfaceDMA8723BE()\n");
+
+	/* Revise Note: Follow the document "PCIe RX DMA Hang Reset Flow_v03"
+	 * released by SD1 Alan.
+	 * 2013.05.07, by tynli.
+	 */
+
+	/* 1. disable register write lock
+	 *	write 0x1C bit[1:0] = 2'h0
+	 *	write 0xCC bit[2] = 1'b1
+	 */
+	tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL);
+	tmp &= ~(BIT(1) | BIT(0));
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, tmp);
+	tmp = rtl_read_byte(rtlpriv, REG_PMC_DBG_CTRL2);
+	tmp |= BIT(2);
+	rtl_write_byte(rtlpriv, REG_PMC_DBG_CTRL2, tmp);
+
+	/* 2. Check and pause TRX DMA
+	 *	write 0x284 bit[18] = 1'b1
+	 *	write 0x301 = 0xFF
+	 */
+	tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	if (tmp & BIT(2)) {
+		/* Already pause before the function for another purpose. */
+		release_mac_rx_pause = false;
+	} else {
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, (tmp | BIT(2)));
+		release_mac_rx_pause = true;
+	}
+
+	backup_pcie_dma_pause = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG + 1);
+	if (backup_pcie_dma_pause != 0xFF)
+		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0xFF);
+
+	if (mac_power_on) {
+		/* 3. reset TRX function
+		 *	write 0x100 = 0x00
+		 */
+		rtl_write_byte(rtlpriv, REG_CR, 0);
+	}
+
+	/* 4. Reset PCIe DMA
+	 *	write 0x003 bit[0] = 0
+	 */
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	tmp &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp);
+
+	/* 5. Enable PCIe DMA
+	 *	write 0x003 bit[0] = 1
+	 */
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	tmp |= BIT(0);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp);
+
+	if (mac_power_on) {
+		/* 6. enable TRX function
+		 *	write 0x100 = 0xFF
+		 */
+		rtl_write_byte(rtlpriv, REG_CR, 0xFF);
+
+		/* We should init LLT & RQPN and
+		 * prepare Tx/Rx descrptor address later
+		 * because MAC function is reset.
+		 */
+	}
+
+	/* 7. Restore PCIe autoload down bit
+	 *	write 0xF8 bit[17] = 1'b1
+	 */
+	tmp = rtl_read_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL + 2);
+	tmp |= BIT(1);
+	rtl_write_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL + 2, tmp);
+
+	/* In MAC power on state, BB and RF maybe in ON state,
+	 * if we release TRx DMA here
+	 * it will cause packets to be started to Tx/Rx,
+	 * so we release Tx/Rx DMA later.
+	 */
+	if (!mac_power_on) {
+		/* 8. release TRX DMA
+		 *	write 0x284 bit[18] = 1'b0
+		 *	write 0x301 = 0x00
+		 */
+		if (release_mac_rx_pause) {
+			tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+			rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL,
+				       (tmp & (~BIT(2))));
+		}
+		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1,
+			       backup_pcie_dma_pause);
+	}
+
+	/* 9. lock system register
+	 *	write 0xCC bit[2] = 1'b0
+	 */
+	tmp = rtl_read_byte(rtlpriv, REG_PMC_DBG_CTRL2);
+	tmp &= ~(BIT(2));
+	rtl_write_byte(rtlpriv, REG_PMC_DBG_CTRL2, tmp);
+}
+
 int rtl8723be_hw_init(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -1002,33 +1358,51 @@ int rtl8723be_hw_init(struct ieee80211_hw *hw)
 	local_save_flags(flags);
 	local_irq_enable();
 
+	rtlhal->fw_ready = false;
 	rtlpriv->rtlhal.being_init_adapter = true;
 	rtlpriv->intf_ops->disable_aspm(hw);
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_CR);
+	if (tmp_u1b != 0 && tmp_u1b != 0xea) {
+		rtlhal->mac_func_enable = true;
+	} else {
+		rtlhal->mac_func_enable = false;
+		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON;
+	}
+
+	if (_rtl8723be_check_pcie_dma_hang(rtlpriv)) {
+		_rtl8723be_reset_pcie_interface_dma(rtlpriv,
+						    rtlhal->mac_func_enable);
+		rtlhal->mac_func_enable = false;
+	}
+	if (rtlhal->mac_func_enable) {
+		_rtl8723be_poweroff_adapter(hw);
+		rtlhal->mac_func_enable = false;
+	}
 	rtstatus = _rtl8723be_init_mac(hw);
 	if (!rtstatus) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
 		err = 1;
 		goto exit;
 	}
+
 	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CFG);
-	tmp_u1b &= 0x7F;
-	rtl_write_byte(rtlpriv, REG_SYS_CFG, tmp_u1b);
+	rtl_write_byte(rtlpriv, REG_SYS_CFG, tmp_u1b & 0x7F);
 
-	err = rtl8723_download_fw(hw, true);
+	err = rtl8723_download_fw(hw, true, FW_8723B_POLLING_TIMEOUT_COUNT);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
 			 "Failed to download FW. Init HW without FW now..\n");
 		err = 1;
-		rtlhal->fw_ready = false;
 		goto exit;
-	} else {
-		rtlhal->fw_ready = true;
 	}
+	rtlhal->fw_ready = true;
+
 	rtlhal->last_hmeboxnum = 0;
 	rtl8723be_phy_mac_config(hw);
 	/* because last function modify RCR, so we update
 	 * rcr var here, or TP will unstable for receive_config
-	 * is wrong, RX RCR_ACRC32 will cause TP unstabel & Rx
+	 * is wrong, RX RCR_ACRC32 will cause TP unstable & Rx
 	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
 	 */
 	rtlpci->receive_config = rtl_read_dword(rtlpriv, REG_RCR);
@@ -1036,7 +1410,6 @@ int rtl8723be_hw_init(struct ieee80211_hw *hw)
 	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
 
 	rtl8723be_phy_bb_config(hw);
-	rtlphy->rf_mode = RF_OP_BY_SW_3WIRE;
 	rtl8723be_phy_rf_config(hw);
 
 	rtlphy->rfreg_chnlval[0] = rtl_get_rfreg(hw, (enum radio_path)0,
@@ -1046,10 +1419,8 @@ int rtl8723be_hw_init(struct ieee80211_hw *hw)
 	rtlphy->rfreg_chnlval[0] &= 0xFFF03FF;
 	rtlphy->rfreg_chnlval[0] |= (BIT(10) | BIT(11));
 
-	rtl_set_bbreg(hw, RFPGA0_RFMOD, BCCKEN, 0x1);
-	rtl_set_bbreg(hw, RFPGA0_RFMOD, BOFDMEN, 0x1);
-	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);
 	_rtl8723be_hw_configure(hw);
+	rtlhal->mac_func_enable = true;
 	rtl_cam_reset_all_entry(hw);
 	rtl8723be_enable_hw_security_config(hw);
 
@@ -1061,36 +1432,32 @@ int rtl8723be_hw_init(struct ieee80211_hw *hw)
 
 	rtl8723be_bt_hw_init(hw);
 
-	rtl_set_bbreg(hw, 0x64, BIT(20), 0);
-	rtl_set_bbreg(hw, 0x64, BIT(24), 0);
-
-	rtl_set_bbreg(hw, 0x40, BIT(4), 0);
-	rtl_set_bbreg(hw, 0x40, BIT(3), 1);
-
-	rtl_set_bbreg(hw, 0x944, BIT(0)|BIT(1), 0x3);
-	rtl_set_bbreg(hw, 0x930, 0xff, 0x77);
-
-	rtl_set_bbreg(hw, 0x38, BIT(11), 0x1);
-
-	rtl_set_bbreg(hw, 0xb2c, 0xffffffff, 0x80000000);
-
 	if (ppsc->rfpwr_state == ERFON) {
+		rtl8723be_phy_set_rfpath_switch(hw, 1);
+		/* when use 1ant NIC, iqk will disturb BT music
+		 * root cause is not clear now, is something
+		 * related with 'mdelay' and Reg[0x948]
+		 */
+		if (rtlpriv->btcoexist.btc_info.ant_num == ANT_X2 ||
+		    !rtlpriv->cfg->ops->get_btc_status()) {
+			rtl8723be_phy_iq_calibrate(hw, false);
+			rtlphy->iqk_initialized = true;
+		}
 		rtl8723be_dm_check_txpower_tracking(hw);
 		rtl8723be_phy_lc_calibrate(hw);
 	}
-	tmp_u1b = efuse_read_1byte(hw, 0x1FA);
-	if (!(tmp_u1b & BIT(0))) {
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x15, 0x0F, 0x05);
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "PA BIAS path A\n");
-	}
-	if (!(tmp_u1b & BIT(4))) {
-		tmp_u1b = rtl_read_byte(rtlpriv, 0x16);
-		tmp_u1b &= 0x0F;
-		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x80);
-		udelay(10);
-		rtl_write_byte(rtlpriv, 0x16, tmp_u1b | 0x90);
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "under 1.5V\n");
+	rtl_write_byte(rtlpriv, REG_NAV_UPPER, ((30000 + 127) / 128));
+
+	/* Release Rx DMA. */
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	if (tmp_u1b & BIT(2)) {
+		/* Release Rx DMA if needed */
+		tmp_u1b &= (~BIT(2));
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, tmp_u1b);
 	}
+	/* Release Tx/Rx PCIE DMA. */
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0);
+
 	rtl8723be_dm_init(hw);
 exit:
 	local_irq_restore(flags);
@@ -1103,43 +1470,29 @@ static enum version_8723e _rtl8723be_read_chip_version(struct ieee80211_hw *hw)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	enum version_8723e version = VERSION_UNKNOWN;
-	u8 count = 0;
-	u8 value8;
 	u32 value32;
 
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0);
-
-	value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 2);
-	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 2, value8 | BIT(0));
-
-	value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1);
-	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, value8 | BIT(0));
-
-	value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1);
-	while (((value8 & BIT(0))) && (count++ < 100)) {
-		udelay(10);
-		value8 = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1);
-	}
-	count = 0;
-	value8 = rtl_read_byte(rtlpriv, REG_ROM_VERSION);
-	while ((value8 == 0) && (count++ < 50)) {
-		value8 = rtl_read_byte(rtlpriv, REG_ROM_VERSION);
-		mdelay(1);
-	}
 	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
 	if ((value32 & (CHIP_8723B)) != CHIP_8723B)
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "unkown chip version\n");
 	else
-		version = (enum version_8723e) VERSION_TEST_CHIP_1T1R_8723B;
+		version = (enum version_8723e)CHIP_8723B;
 
-		rtlphy->rf_type = RF_1T1R;
+	rtlphy->rf_type = RF_1T1R;
+
+	/* treat rtl8723be chip as  MP version in default */
+	version = (enum version_8723e)(version | NORMAL_CHIP);
+
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG);
+	/* cut version */
+	version |= (enum version_8723e)(value32 & CHIP_VER_RTL_MASK);
+	/* Manufacture */
+	if (((value32 & EXT_VENDOR_ID) >> 18) == 0x01)
+		version = (enum version_8723e)(version | CHIP_VENDOR_SMIC);
 
-	value8 = rtl_read_byte(rtlpriv, REG_ROM_VERSION);
-	if (value8 >= 0x02)
-		version |= BIT(3);
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 		 "Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
-		 "RF_2T2R" : "RF_1T1R");
+		  "RF_2T2R" : "RF_1T1R");
 
 	return version;
 }
@@ -1150,43 +1503,29 @@ static int _rtl8723be_set_media_status(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 bt_msr = rtl_read_byte(rtlpriv, MSR) & 0xfc;
 	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+	u8 mode = MSR_NOLINK;
 
-	rtl_write_dword(rtlpriv, REG_BCN_CTRL, 0);
-	RT_TRACE(rtlpriv, COMP_BEACON, DBG_LOUD,
-		 "clear 0x550 when set HW_VAR_MEDIA_STATUS\n");
-
-	if (type == NL80211_IFTYPE_UNSPECIFIED ||
-	    type == NL80211_IFTYPE_STATION) {
-		_rtl8723be_stop_tx_beacon(hw);
-		_rtl8723be_enable_bcn_sub_func(hw);
-	} else if (type == NL80211_IFTYPE_ADHOC || type == NL80211_IFTYPE_AP) {
-		_rtl8723be_resume_tx_beacon(hw);
-		_rtl8723be_disable_bcn_sub_func(hw);
-	} else {
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-			 "Set HW_VAR_MEDIA_STATUS: "
-			 "No such media status(%x).\n", type);
-	}
 	switch (type) {
 	case NL80211_IFTYPE_UNSPECIFIED:
-		bt_msr |= MSR_NOLINK;
-		ledaction = LED_CTL_LINK;
+		mode = MSR_NOLINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to NO LINK!\n");
 		break;
 	case NL80211_IFTYPE_ADHOC:
-		bt_msr |= MSR_ADHOC;
+	case NL80211_IFTYPE_MESH_POINT:
+		mode = MSR_ADHOC;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to Ad Hoc!\n");
 		break;
 	case NL80211_IFTYPE_STATION:
-		bt_msr |= MSR_INFRA;
+		mode = MSR_INFRA;
 		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to STA!\n");
 		break;
 	case NL80211_IFTYPE_AP:
-		bt_msr |= MSR_AP;
+		mode = MSR_AP;
+		ledaction = LED_CTL_LINK;
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
 			 "Set Network type to AP!\n");
 		break;
@@ -1195,9 +1534,33 @@ static int _rtl8723be_set_media_status(struct ieee80211_hw *hw,
 			 "Network type %d not support!\n", type);
 		return 1;
 	}
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+
+	/* MSR_INFRA == Link in infrastructure network;
+	 * MSR_ADHOC == Link in ad hoc network;
+	 * Therefore, check link state is necessary.
+	 *
+	 * MSR_AP == AP mode; link state is not cared here.
+	 */
+	if (mode != MSR_AP && rtlpriv->mac80211.link_state < MAC80211_LINKED) {
+		mode = MSR_NOLINK;
+		ledaction = LED_CTL_NO_LINK;
+	}
+
+	if (mode == MSR_NOLINK || mode == MSR_INFRA) {
+		_rtl8723be_stop_tx_beacon(hw);
+		_rtl8723be_enable_bcn_sub_func(hw);
+	} else if (mode == MSR_ADHOC || mode == MSR_AP) {
+		_rtl8723be_resume_tx_beacon(hw);
+		_rtl8723be_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
+			 mode);
+	}
+
+	rtl_write_byte(rtlpriv, (MSR), bt_msr | mode);
 	rtlpriv->cfg->ops->led_control(hw, ledaction);
-	if ((bt_msr & MSR_MASK) == MSR_AP)
+	if (mode == MSR_AP)
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
 	else
 		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
@@ -1224,6 +1587,7 @@ void rtl8723be_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
 		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
 					      (u8 *)(&reg_rcr));
 	}
+
 }
 
 int rtl8723be_set_network_type(struct ieee80211_hw *hw,
@@ -1240,6 +1604,7 @@ int rtl8723be_set_network_type(struct ieee80211_hw *hw,
 	} else {
 		rtl8723be_set_check_bssid(hw, false);
 	}
+
 	return 0;
 }
 
@@ -1249,6 +1614,7 @@ int rtl8723be_set_network_type(struct ieee80211_hw *hw,
 void rtl8723be_set_qos(struct ieee80211_hw *hw, int aci)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
 	rtl8723_dm_init_edca_turbo(hw);
 	switch (aci) {
 	case AC1_BK:
@@ -1268,20 +1634,32 @@ void rtl8723be_set_qos(struct ieee80211_hw *hw, int aci)
 	}
 }
 
+static void rtl8723be_clear_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 tmp;
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISR);
+	rtl_write_dword(rtlpriv, REG_HISR, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
+	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
+	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
+}
+
 void rtl8723be_enable_interrupt(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 
+	rtl8723be_clear_interrupt(hw);/*clear it here first*/
+
 	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
 	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
 	rtlpci->irq_enabled = true;
-	/* there are some C2H CMDs have been sent
-	 * before system interrupt is enabled, e.g., C2H, CPWM.
-	 * So we need to clear all C2H events that FW has notified,
-	 * otherwise FW won't schedule any commands anymore.
-	 */
-	rtl_write_byte(rtlpriv, REG_C2HEVT_CLEAR, 0);
+
 	/*enable system interrupt*/
 	rtl_write_dword(rtlpriv, REG_HSIMR, rtlpci->sys_irq_mask & 0xFFFFFFFF);
 }
@@ -1294,48 +1672,7 @@ void rtl8723be_disable_interrupt(struct ieee80211_hw *hw)
 	rtl_write_dword(rtlpriv, REG_HIMR, IMR_DISABLED);
 	rtl_write_dword(rtlpriv, REG_HIMRE, IMR_DISABLED);
 	rtlpci->irq_enabled = false;
-	synchronize_irq(rtlpci->pdev->irq);
-}
-
-static void _rtl8723be_poweroff_adapter(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 u1b_tmp;
-
-	/* Combo (PCIe + USB) Card and PCIe-MF Card */
-	/* 1. Run LPS WL RFOFF flow */
-	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-				 PWR_INTF_PCI_MSK, RTL8723_NIC_LPS_ENTER_FLOW);
-
-	/* 2. 0x1F[7:0] = 0 */
-	/* turn off RF */
-	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
-	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) &&
-	    rtlhal->fw_ready)
-		rtl8723be_firmware_selfreset(hw);
-
-	/* Reset MCU. Suggested by Filen. */
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, (u1b_tmp & (~BIT(2))));
-
-	/* g.	MCUFWDL 0x80[1:0]= 0	 */
-	/* reset MCU ready status */
-	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
-
-	/* HW card disable configuration. */
-	rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-				 PWR_INTF_PCI_MSK, RTL8723_NIC_DISABLE_FLOW);
-
-	/* Reset MCU IO Wrapper */
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1b_tmp | BIT(0));
-
-	/* 7. RSV_CTRL 0x1C[7:0] = 0x0E */
-	/* lock ISO/CLK/Power control register */
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0e);
+	/*synchronize_irq(rtlpci->pdev->irq);*/
 }
 
 void rtl8723be_card_disable(struct ieee80211_hw *hw)
@@ -1442,10 +1779,9 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 	u32 path, addr = EEPROM_TX_PWR_INX, group, cnt = 0;
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "hal_ReadPowerValueFromPROM8723BE(): "
-		 "PROMContent[0x%x]= 0x%x\n",
+		 "hal_ReadPowerValueFromPROM8723BE(): PROMContent[0x%x]=0x%x\n",
 		 (addr + 1), hwinfo[addr + 1]);
-	if (0xFF == hwinfo[addr + 1])  /*YJ, add, 120316*/
+	if (0xFF == hwinfo[addr + 1])  /*YJ,add,120316*/
 		autoload_fail = true;
 
 	if (autoload_fail) {
@@ -1453,7 +1789,7 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 			 "auto load fail : Use Default value!\n");
 		for (path = 0; path < MAX_RF_PATH; path++) {
 			/* 2.4G default value */
-			for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
+			for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
 				pw2g->index_cck_base[path][group] = 0x2D;
 				pw2g->index_bw40_base[path][group] = 0x2D;
 			}
@@ -1471,12 +1807,14 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 		}
 		return;
 	}
+
 	for (path = 0; path < MAX_RF_PATH; path++) {
 		/*2.4G default value*/
 		for (group = 0; group < MAX_CHNL_GROUP_24G; group++) {
 			pw2g->index_cck_base[path][group] = hwinfo[addr++];
 			if (pw2g->index_cck_base[path][group] == 0xFF)
 				pw2g->index_cck_base[path][group] = 0x2D;
+
 		}
 		for (group = 0; group < MAX_CHNL_GROUP_24G - 1; group++) {
 			pw2g->index_bw40_base[path][group] = hwinfo[addr++];
@@ -1493,8 +1831,10 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 						(hwinfo[addr] & 0xf0) >> 4;
 					/*bit sign number to 8 bit sign number*/
 					if (pw2g->bw20_diff[path][cnt] & BIT(3))
-						pw2g->bw20_diff[path][cnt] |= 0xF0;
+						pw2g->bw20_diff[path][cnt] |=
+									  0xF0;
 				}
+
 				if (hwinfo[addr] == 0xFF) {
 					pw2g->ofdm_diff[path][cnt] = 0x04;
 				} else {
@@ -1517,6 +1857,7 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 						pw2g->bw40_diff[path][cnt] |=
 									  0xF0;
 				}
+
 				if (hwinfo[addr] == 0xFF) {
 					pw2g->bw20_diff[path][cnt] = 0xFE;
 				} else {
@@ -1537,9 +1878,10 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 						pw2g->ofdm_diff[path][cnt] |=
 									  0xF0;
 				}
-				if (hwinfo[addr] == 0xFF) {
+
+				if (hwinfo[addr] == 0xFF)
 					pw2g->cck_diff[path][cnt] = 0xFE;
-				} else {
+				else {
 					pw2g->cck_diff[path][cnt] =
 							(hwinfo[addr] & 0x0f);
 					if (pw2g->cck_diff[path][cnt] & BIT(3))
@@ -1549,12 +1891,14 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 				addr++;
 			}
 		}
+
 		/*5G default value*/
 		for (group = 0; group < MAX_CHNL_GROUP_5G; group++) {
 			pw5g->index_bw40_base[path][group] = hwinfo[addr++];
 			if (pw5g->index_bw40_base[path][group] == 0xFF)
 				pw5g->index_bw40_base[path][group] = 0xFE;
 		}
+
 		for (cnt = 0; cnt < MAX_TX_COUNT; cnt++) {
 			if (cnt == 0) {
 				pw5g->bw40_diff[path][cnt] = 0;
@@ -1568,9 +1912,10 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 						pw5g->bw20_diff[path][cnt] |=
 									  0xF0;
 				}
-				if (hwinfo[addr] == 0xFF) {
+
+				if (hwinfo[addr] == 0xFF)
 					pw5g->ofdm_diff[path][cnt] = 0x04;
-				} else {
+				else {
 					pw5g->ofdm_diff[path][0] =
 							(hwinfo[addr] & 0x0f);
 					if (pw5g->ofdm_diff[path][cnt] & BIT(3))
@@ -1587,6 +1932,7 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 					if (pw5g->bw40_diff[path][cnt] & BIT(3))
 						pw5g->bw40_diff[path][cnt] |= 0xF0;
 				}
+
 				if (hwinfo[addr] == 0xFF) {
 					pw5g->bw20_diff[path][cnt] = 0xFE;
 				} else {
@@ -1598,6 +1944,7 @@ static void _rtl8723be_read_power_value_fromprom(struct ieee80211_hw *hw,
 				addr++;
 			}
 		}
+
 		if (hwinfo[addr] == 0xFF) {
 			pw5g->ofdm_diff[path][1] = 0xFE;
 			pw5g->ofdm_diff[path][2] = 0xFE;
@@ -1653,14 +2000,16 @@ static void _rtl8723be_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 			rtlefuse->txpwr_legacyhtdiff[rf_path][i] =
 						pw2g.ofdm_diff[rf_path][i];
 		}
+
 		for (i = 0; i < 14; i++) {
-			RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
-				"RF(%d)-Ch(%d) [CCK / HT40_1S ] = "
-				"[0x%x / 0x%x ]\n", rf_path, i,
+			RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
+				"RF(%d)-Ch(%d) [CCK / HT40_1S ] = [0x%x / 0x%x ]\n",
+				rf_path, i,
 				rtlefuse->txpwrlevel_cck[rf_path][i],
 				rtlefuse->txpwrlevel_ht40_1s[rf_path][i]);
 		}
 	}
+
 	if (!autoload_fail)
 		rtlefuse->eeprom_thermalmeter =
 					hwinfo[EEPROM_THERMAL_METER_88E];
@@ -1671,8 +2020,9 @@ static void _rtl8723be_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlefuse->apk_thermalmeterignore = true;
 		rtlefuse->eeprom_thermalmeter = EEPROM_DEFAULT_THERMALMETER;
 	}
+
 	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
-	RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
 
 	if (!autoload_fail) {
@@ -1683,7 +2033,7 @@ static void _rtl8723be_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
 	} else {
 		rtlefuse->eeprom_regulatory = 0;
 	}
-	RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
 		"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
 }
 
@@ -1743,6 +2093,7 @@ static void _rtl8723be_read_adapter_info(struct ieee80211_hw *hw,
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
 		rtlefuse->autoload_failflag = false;
 	}
+
 	if (rtlefuse->autoload_failflag)
 		return;
 
@@ -1958,100 +2309,10 @@ void rtl8723be_read_eeprom_info(struct ieee80211_hw *hw)
 	_rtl8723be_hal_customized_behavior(hw);
 }
 
-static void rtl8723be_update_hal_rate_table(struct ieee80211_hw *hw,
-					    struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u32 ratr_value;
-	u8 ratr_index = 0;
-	u8 nmode = mac->ht_enable;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
-	u16 shortgi_rate;
-	u32 tmp_ratr_value;
-	u8 curtxbw_40mhz = mac->bw_40;
-	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
-			       1 : 0;
-	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
-			       1 : 0;
-	enum wireless_mode wirelessmode = mac->mode;
-
-	if (rtlhal->current_bandtype == BAND_ON_5G)
-		ratr_value = sta->supp_rates[1] << 4;
-	else
-		ratr_value = sta->supp_rates[0];
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		ratr_value = 0xfff;
-	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
-		       sta->ht_cap.mcs.rx_mask[0] << 12);
-	switch (wirelessmode) {
-	case WIRELESS_MODE_B:
-		if (ratr_value & 0x0000000c)
-			ratr_value &= 0x0000000d;
-		else
-			ratr_value &= 0x0000000f;
-		break;
-	case WIRELESS_MODE_G:
-		ratr_value &= 0x00000FF5;
-		break;
-	case WIRELESS_MODE_N_24G:
-	case WIRELESS_MODE_N_5G:
-		nmode = 1;
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			ratr_value &= 0x0007F005;
-		} else {
-			u32 ratr_mask;
-
-			if (get_rf_type(rtlphy) == RF_1T2R ||
-			    get_rf_type(rtlphy) == RF_1T1R)
-				ratr_mask = 0x000ff005;
-			else
-				ratr_mask = 0x0f0ff005;
-			ratr_value &= ratr_mask;
-		}
-		break;
-	default:
-		if (rtlphy->rf_type == RF_1T2R)
-			ratr_value &= 0x000ff0ff;
-		else
-			ratr_value &= 0x0f0ff0ff;
-		break;
-	}
-	if ((rtlpriv->btcoexist.bt_coexistence) &&
-	    (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4) &&
-	    (rtlpriv->btcoexist.bt_cur_state) &&
-	    (rtlpriv->btcoexist.bt_ant_isolation) &&
-	    ((rtlpriv->btcoexist.bt_service == BT_SCO) ||
-	     (rtlpriv->btcoexist.bt_service == BT_BUSY)))
-		ratr_value &= 0x0fffcfc0;
-	else
-		ratr_value &= 0x0FFFFFFF;
-
-	if (nmode && ((curtxbw_40mhz && curshortgi_40mhz) ||
-		      (!curtxbw_40mhz && curshortgi_20mhz))) {
-		ratr_value |= 0x10000000;
-		tmp_ratr_value = (ratr_value >> 12);
-
-		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
-			if ((1 << shortgi_rate) & tmp_ratr_value)
-				break;
-		}
-		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
-			       (shortgi_rate << 4) | (shortgi_rate);
-	}
-	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
-
-	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
-		 "%x\n", rtl_read_dword(rtlpriv, REG_ARFR0));
-}
-
 static u8 _rtl8723be_mrate_idx_to_arfr_id(struct ieee80211_hw *hw,
 					  u8 rate_index)
 {
 	u8 ret = 0;
-
 	switch (rate_index) {
 	case RATR_INX_WIRELESS_NGB:
 		ret = 1;
@@ -2090,16 +2351,15 @@ static void rtl8723be_update_hal_rate_mask(struct ieee80211_hw *hw,
 	u32 ratr_bitmap;
 	u8 ratr_index;
 	u8 curtxbw_40mhz = (sta->ht_cap.cap &
-			    IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
+			      IEEE80211_HT_CAP_SUP_WIDTH_20_40) ? 1 : 0;
 	u8 curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
-			       1 : 0;
+				1 : 0;
 	u8 curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
-			       1 : 0;
+				1 : 0;
 	enum wireless_mode wirelessmode = 0;
 	bool shortgi = false;
 	u8 rate_mask[7];
 	u8 macid = 0;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
 
 	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
 	wirelessmode = sta_entry->wireless_mode;
@@ -2135,55 +2395,40 @@ static void rtl8723be_update_hal_rate_mask(struct ieee80211_hw *hw,
 		else
 			ratr_bitmap &= 0x00000ff5;
 		break;
-	case WIRELESS_MODE_A:
-		ratr_index = RATR_INX_WIRELESS_A;
-		ratr_bitmap &= 0x00000ff0;
-		break;
 	case WIRELESS_MODE_N_24G:
 	case WIRELESS_MODE_N_5G:
 		ratr_index = RATR_INX_WIRELESS_NGB;
-
-		if (mimo_ps == IEEE80211_SMPS_STATIC  ||
-		    mimo_ps == IEEE80211_SMPS_DYNAMIC) {
-			if (rssi_level == 1)
-				ratr_bitmap &= 0x00070000;
-			else if (rssi_level == 2)
-				ratr_bitmap &= 0x0007f000;
-			else
-				ratr_bitmap &= 0x0007f005;
+		if (rtlphy->rf_type == RF_1T1R) {
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff015;
+			} else {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x000f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x000ff000;
+				else
+					ratr_bitmap &= 0x000ff005;
+			}
 		} else {
-			if (rtlphy->rf_type == RF_1T1R) {
-				if (curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff005;
-				}
+			if (curtxbw_40mhz) {
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f8f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f8ff000;
+				else
+					ratr_bitmap &= 0x0f8ff015;
 			} else {
-				if (curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0f8f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0f8ff000;
-					else
-						ratr_bitmap &= 0x0f8ff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0f8f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0f8ff000;
-					else
-						ratr_bitmap &= 0x0f8ff005;
-				}
+				if (rssi_level == 1)
+					ratr_bitmap &= 0x0f8f0000;
+				else if (rssi_level == 2)
+					ratr_bitmap &= 0x0f8ff000;
+				else
+					ratr_bitmap &= 0x0f8ff005;
 			}
 		}
 		if ((curtxbw_40mhz && curshortgi_40mhz) ||
@@ -2203,18 +2448,17 @@ static void rtl8723be_update_hal_rate_mask(struct ieee80211_hw *hw,
 			ratr_bitmap &= 0x0f0ff0ff;
 		break;
 	}
+
 	sta_entry->ratr_index = ratr_index;
 
 	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
 		 "ratr_bitmap :%x\n", ratr_bitmap);
-	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) | (ratr_index << 28);
+	*(u32 *)&rate_mask = (ratr_bitmap & 0x0fffffff) |
+				       (ratr_index << 28);
 	rate_mask[0] = macid;
 	rate_mask[1] = _rtl8723be_mrate_idx_to_arfr_id(hw, ratr_index) |
-						       (shortgi ? 0x80 : 0x00);
+						      (shortgi ? 0x80 : 0x00);
 	rate_mask[2] = curtxbw_40mhz;
-	/* if (prox_priv->proxim_modeinfo->power_output > 0)
-	 *	rate_mask[2] |= BIT(6);
-	 */
 
 	rate_mask[3] = (u8)(ratr_bitmap & 0x000000ff);
 	rate_mask[4] = (u8)((ratr_bitmap & 0x0000ff00) >> 8);
@@ -2228,7 +2472,7 @@ static void rtl8723be_update_hal_rate_mask(struct ieee80211_hw *hw,
 		 rate_mask[2], rate_mask[3],
 		 rate_mask[4], rate_mask[5],
 		 rate_mask[6]);
-	rtl8723be_fill_h2c_cmd(hw, H2C_8723BE_RA_MASK, 7, rate_mask);
+	rtl8723be_fill_h2c_cmd(hw, H2C_8723B_RA_MASK, 7, rate_mask);
 	_rtl8723be_set_bcn_ctrl_reg(hw, BIT(3), 0);
 }
 
@@ -2239,8 +2483,6 @@ void rtl8723be_update_hal_rate_tbl(struct ieee80211_hw *hw,
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	if (rtlpriv->dm.useramask)
 		rtl8723be_update_hal_rate_mask(hw, sta, rssi_level);
-	else
-		rtl8723be_update_hal_rate_table(hw, sta);
 }
 
 void rtl8723be_update_channel_access_setting(struct ieee80211_hw *hw)
@@ -2264,7 +2506,7 @@ bool rtl8723be_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
 	u8 u1tmp;
-	bool actuallyset = false;
+	bool b_actuallyset = false;
 
 	if (rtlpriv->rtlhal.being_init_adapter)
 		return false;
@@ -2280,6 +2522,7 @@ bool rtl8723be_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 		ppsc->rfchange_inprogress = true;
 		spin_unlock(&rtlpriv->locks.rf_ps_lock);
 	}
+
 	cur_rfstate = ppsc->rfpwr_state;
 
 	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL_2,
@@ -2292,24 +2535,23 @@ bool rtl8723be_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 	else
 		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFON : ERFOFF;
 
-	if (ppsc->hwradiooff &&
-	    (e_rfpowerstate_toset == ERFON)) {
+	if ((ppsc->hwradiooff) && (e_rfpowerstate_toset == ERFON)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 "GPIOChangeRF  - HW Radio ON, RF ON\n");
 
 		e_rfpowerstate_toset = ERFON;
 		ppsc->hwradiooff = false;
-		actuallyset = true;
-	} else if (!ppsc->hwradiooff &&
-		   (e_rfpowerstate_toset == ERFOFF)) {
+		b_actuallyset = true;
+	} else if (!ppsc->hwradiooff && (e_rfpowerstate_toset == ERFOFF)) {
 		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 			 "GPIOChangeRF  - HW Radio OFF, RF OFF\n");
 
 		e_rfpowerstate_toset = ERFOFF;
 		ppsc->hwradiooff = true;
-		actuallyset = true;
+		b_actuallyset = true;
 	}
-	if (actuallyset) {
+
+	if (b_actuallyset) {
 		spin_lock(&rtlpriv->locks.rf_ps_lock);
 		ppsc->rfchange_inprogress = false;
 		spin_unlock(&rtlpriv->locks.rf_ps_lock);
@@ -2321,8 +2563,10 @@ bool rtl8723be_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
 		ppsc->rfchange_inprogress = false;
 		spin_unlock(&rtlpriv->locks.rf_ps_lock);
 	}
+
 	*valid = 1;
 	return !ppsc->hwradiooff;
+
 }
 
 void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
@@ -2363,6 +2607,7 @@ void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
 				rtlpriv->sec.key_len[idx] = 0;
 			}
 		}
+
 	} else {
 		switch (enc_algo) {
 		case WEP40_ENCRYPTION:
@@ -2378,7 +2623,7 @@ void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
 			enc_algo = CAM_AES;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 				 "switch case not process\n");
 			enc_algo = CAM_TKIP;
 			break;
@@ -2398,22 +2643,22 @@ void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
 					if (entry_id >=  TOTAL_CAM_ENTRY) {
 						RT_TRACE(rtlpriv, COMP_SEC,
 							 DBG_EMERG,
-							 "Can not find free"
-							 " hw security cam "
-							 "entry\n");
+							 "Can not find free hw security cam entry\n");
 						return;
 					}
 				} else {
 					entry_id = CAM_PAIRWISE_KEY_POSITION;
 				}
+
 				key_index = PAIRWISE_KEYIDX;
 				is_pairwise = true;
 			}
 		}
+
 		if (rtlpriv->sec.key_len[key_index] == 0) {
 			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 				 "delete one entry, entry_id is %d\n",
-				 entry_id);
+				  entry_id);
 			if (mac->opmode == NL80211_IFTYPE_AP)
 				rtl_cam_del_entry(hw, p_macaddr);
 			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
@@ -2422,12 +2667,12 @@ void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
 				 "add one entry\n");
 			if (is_pairwise) {
 				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
-					 "set Pairwise key\n");
+					 "set Pairwiase key\n");
 
 				rtl_cam_add_one_entry(hw, macaddr, key_index,
-						      entry_id, enc_algo,
-						      CAM_CONFIG_NO_USEDK,
-						      rtlpriv->sec.key_buf[key_index]);
+					       entry_id, enc_algo,
+					       CAM_CONFIG_NO_USEDK,
+					       rtlpriv->sec.key_buf[key_index]);
 			} else {
 				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
 					 "set group key\n");
@@ -2442,10 +2687,11 @@ void rtl8723be_set_key(struct ieee80211_hw *hw, u32 key_index,
 						rtlpriv->sec.key_buf
 						[entry_id]);
 				}
+
 				rtl_cam_add_one_entry(hw, macaddr, key_index,
-						      entry_id, enc_algo,
-						      CAM_CONFIG_NO_USEDK,
-						      rtlpriv->sec.key_buf[entry_id]);
+						entry_id, enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf[entry_id]);
 			}
 		}
 	}
@@ -2464,7 +2710,7 @@ void rtl8723be_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
 			rtlpriv->btcoexist.btc_info.btcoexist = 1;
 		else
 			rtlpriv->btcoexist.btc_info.btcoexist = 0;
-		value = hwinfo[RF_OPTION4];
+		value = hwinfo[EEPROM_RF_BT_SETTING_8723B];
 		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8723B;
 		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
 	} else {
@@ -2472,6 +2718,7 @@ void rtl8723be_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
 		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8723B;
 		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
 	}
+
 }
 
 void rtl8723be_bt_reg_init(struct ieee80211_hw *hw)
@@ -2492,6 +2739,7 @@ void rtl8723be_bt_hw_init(struct ieee80211_hw *hw)
 
 	if (rtlpriv->cfg->ops->get_btc_status())
 		rtlpriv->btcoexist.btc_ops->btc_init_hw_config(rtlpriv);
+
 }
 
 void rtl8723be_suspend(struct ieee80211_hw *hw)
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/hw.h b/drivers/net/wireless/rtlwifi/rtl8723be/hw.h
index 64c7551af6b7..eae863d08de8 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/hw.h
@@ -59,4 +59,5 @@ void rtl8723be_bt_reg_init(struct ieee80211_hw *hw);
 void rtl8723be_bt_hw_init(struct ieee80211_hw *hw);
 void rtl8723be_suspend(struct ieee80211_hw *hw);
 void rtl8723be_resume(struct ieee80211_hw *hw);
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/led.c b/drivers/net/wireless/rtlwifi/rtl8723be/led.c
index cb931a38dc48..4196efb723a2 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/led.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/led.c
@@ -42,7 +42,7 @@ void rtl8723be_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
-		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	switch (pled->ledpin) {
 	case LED_PIN_GPIO0:
@@ -71,7 +71,7 @@ void rtl8723be_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 	u8 ledcfg;
 
 	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
-		 "LedAddr:%X ledpin =%d\n", REG_LEDCFG2, pled->ledpin);
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
 
@@ -100,7 +100,7 @@ void rtl8723be_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 		break;
 	default:
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not processed\n");
+			 "switch case not process\n");
 		break;
 	}
 	pled->ledon = false;
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/phy.c b/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
index 1575ef9ece9f..20dcc25c506c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/phy.c
@@ -26,224 +26,28 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "../ps.h"
-#include "../core.h"
 #include "reg.h"
 #include "def.h"
 #include "phy.h"
 #include "../rtl8723com/phy_common.h"
 #include "rf.h"
 #include "dm.h"
+#include "../rtl8723com/dm_common.h"
 #include "table.h"
 #include "trx.h"
 
 static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw);
+static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						     u8 configtype);
 static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 						       u8 configtype);
-static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
-					      u8 channel, u8 *stage,
-					      u8 *step, u32 *delay);
-static bool _rtl8723be_check_condition(struct ieee80211_hw *hw,
-				       const u32  condition)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u32 _board = rtlefuse->board_type; /*need efuse define*/
-	u32 _interface = rtlhal->interface;
-	u32 _platform = 0x08;/*SupportPlatform */
-	u32 cond = condition;
-
-	if (condition == 0xCDCDCDCD)
-		return true;
-
-	cond = condition & 0xFF;
-	if ((_board & cond) == 0 && cond != 0x1F)
-		return false;
-
-	cond = condition & 0xFF00;
-	cond = cond >> 8;
-	if ((_interface & cond) == 0 && cond != 0x07)
-		return false;
-
-	cond = condition & 0xFF0000;
-	cond = cond >> 16;
-	if ((_platform & cond) == 0 && cond != 0x0F)
-		return false;
-	return true;
-}
-
-static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-	u32 arraylength;
-	u32 *ptrarray;
-
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read rtl8723beMACPHY_Array\n");
-	arraylength = RTL8723BEMAC_1T_ARRAYLEN;
-	ptrarray = RTL8723BEMAC_1T_ARRAY;
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "Img:RTL8723bEMAC_1T_ARRAY LEN %d\n", arraylength);
-	for (i = 0; i < arraylength; i = i + 2)
-		rtl_write_byte(rtlpriv, ptrarray[i], (u8) ptrarray[i + 1]);
-	return true;
-}
-
-static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						     u8 configtype)
-{
-	#define READ_NEXT_PAIR(v1, v2, i) \
-		do { \
-			i += 2; \
-			v1 = array_table[i];\
-			v2 = array_table[i+1]; \
-		} while (0)
-
-	int i;
-	u32 *array_table;
-	u16 arraylen;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 v1 = 0, v2 = 0;
-
-	if (configtype == BASEBAND_CONFIG_PHY_REG) {
-		arraylen = RTL8723BEPHY_REG_1TARRAYLEN;
-		array_table = RTL8723BEPHY_REG_1TARRAY;
-
-		for (i = 0; i < arraylen; i = i + 2) {
-			v1 = array_table[i];
-			v2 = array_table[i+1];
-			if (v1 < 0xcdcdcdcd) {
-				rtl_bb_delay(hw, v1, v2);
-			} else {/*This line is the start line of branch.*/
-				if (!_rtl8723be_check_condition(hw, array_table[i])) {
-					/*Discard the following (offset, data) pairs*/
-					READ_NEXT_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD &&
-					       i < arraylen - 2) {
-						READ_NEXT_PAIR(v1, v2, i);
-					}
-					i -= 2; /* prevent from for-loop += 2*/
-				/* Configure matched pairs and
-				 * skip to end of if-else.
-				 */
-				} else {
-					READ_NEXT_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD &&
-					       i < arraylen - 2) {
-						rtl_bb_delay(hw,
-								    v1, v2);
-						READ_NEXT_PAIR(v1, v2, i);
-					}
+static bool _rtl8723be_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+						u8 channel, u8 *stage,
+						u8 *step, u32 *delay);
 
-					while (v2 != 0xDEAD && i < arraylen - 2)
-						READ_NEXT_PAIR(v1, v2, i);
-				}
-			}
-		}
-	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
-		arraylen = RTL8723BEAGCTAB_1TARRAYLEN;
-		array_table = RTL8723BEAGCTAB_1TARRAY;
-
-		for (i = 0; i < arraylen; i = i + 2) {
-			v1 = array_table[i];
-			v2 = array_table[i+1];
-			if (v1 < 0xCDCDCDCD) {
-				rtl_set_bbreg(hw, array_table[i],
-					      MASKDWORD,
-					      array_table[i + 1]);
-				udelay(1);
-				continue;
-			} else {/*This line is the start line of branch.*/
-				if (!_rtl8723be_check_condition(hw, array_table[i])) {
-					/* Discard the following
-					 * (offset, data) pairs
-					 */
-					READ_NEXT_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD &&
-					       i < arraylen - 2) {
-						READ_NEXT_PAIR(v1, v2, i);
-					}
-					i -= 2; /* prevent from for-loop += 2*/
-				/*Configure matched pairs and
-				 *skip to end of if-else.
-				 */
-				} else {
-					READ_NEXT_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD &&
-					       i < arraylen - 2) {
-						rtl_set_bbreg(hw, array_table[i],
-							      MASKDWORD,
-							      array_table[i + 1]);
-						udelay(1);
-						READ_NEXT_PAIR(v1, v2, i);
-					}
-
-					while (v2 != 0xDEAD && i < arraylen - 2)
-						READ_NEXT_PAIR(v1, v2, i);
-				}
-			}
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-				 "The agctab_array_table[0] is "
-				  "%x Rtl818EEPHY_REGArray[1] is %x\n",
-				  array_table[i], array_table[i + 1]);
-		}
-	}
-	return true;
-}
-
-static u8 _rtl8723be_get_rate_section_index(u32 regaddr)
-{
-	u8 index = 0;
-
-	switch (regaddr) {
-	case RTXAGC_A_RATE18_06:
-	case RTXAGC_B_RATE18_06:
-		index = 0;
-		break;
-	case RTXAGC_A_RATE54_24:
-	case RTXAGC_B_RATE54_24:
-		index = 1;
-		break;
-	case RTXAGC_A_CCK1_MCS32:
-	case RTXAGC_B_CCK1_55_MCS32:
-		index = 2;
-		break;
-	case RTXAGC_B_CCK11_A_CCK2_11:
-		index = 3;
-		break;
-	case RTXAGC_A_MCS03_MCS00:
-	case RTXAGC_B_MCS03_MCS00:
-		index = 4;
-		break;
-	case RTXAGC_A_MCS07_MCS04:
-	case RTXAGC_B_MCS07_MCS04:
-		index = 5;
-		break;
-	case RTXAGC_A_MCS11_MCS08:
-	case RTXAGC_B_MCS11_MCS08:
-		index = 6;
-		break;
-	case RTXAGC_A_MCS15_MCS12:
-	case RTXAGC_B_MCS15_MCS12:
-		index = 7;
-		break;
-	default:
-		regaddr &= 0xFFF;
-		if (regaddr >= 0xC20 && regaddr <= 0xC4C)
-			index = (u8) ((regaddr - 0xC20) / 4);
-		else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
-			index = (u8) ((regaddr - 0xE20) / 4);
-		break;
-	};
-	return index;
-}
+static void rtl8723be_phy_set_rf_on(struct ieee80211_hw *hw);
+static void rtl8723be_phy_set_io(struct ieee80211_hw *hw);
 
 u32 rtl8723be_phy_query_rf_reg(struct ieee80211_hw *hw, enum radio_path rfpath,
 			       u32 regaddr, u32 bitmask)
@@ -265,9 +69,8 @@ u32 rtl8723be_phy_query_rf_reg(struct ieee80211_hw *hw, enum radio_path rfpath,
 	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "regaddr(%#x), rfpath(%#x), "
-		  "bitmask(%#x), original_value(%#x)\n",
-		  regaddr, rfpath, bitmask, original_value);
+		 "regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
+		 regaddr, rfpath, bitmask, original_value);
 
 	return readback_value;
 }
@@ -300,6 +103,7 @@ void rtl8723be_phy_set_rf_reg(struct ieee80211_hw *hw, enum radio_path path,
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
 		  regaddr, bitmask, data, path);
+
 }
 
 bool rtl8723be_phy_mac_config(struct ieee80211_hw *hw)
@@ -316,7 +120,7 @@ bool rtl8723be_phy_bb_config(struct ieee80211_hw *hw)
 	bool rtstatus = true;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 regval;
-	u8 reg_hwparafile = 1;
+	u8 b_reg_hwparafile = 1;
 	u32 tmp;
 	u8 crystalcap = rtlpriv->efuse.crystalcap;
 	rtl8723_phy_init_bb_rf_reg_def(hw);
@@ -333,7 +137,7 @@ bool rtl8723be_phy_bb_config(struct ieee80211_hw *hw)
 
 	rtl_write_byte(rtlpriv, REG_AFE_XTAL_CTRL + 1, 0x80);
 
-	if (reg_hwparafile == 1)
+	if (b_reg_hwparafile == 1)
 		rtstatus = _rtl8723be_phy_bb8723b_config_parafile(hw);
 
 	crystalcap = crystalcap & 0x3F;
@@ -348,18 +152,49 @@ bool rtl8723be_phy_rf_config(struct ieee80211_hw *hw)
 	return rtl8723be_phy_rf6052_config(hw);
 }
 
+static bool _rtl8723be_check_condition(struct ieee80211_hw *hw,
+				       const u32  condition)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 _board = rtlefuse->board_type; /*need efuse define*/
+	u32 _interface = rtlhal->interface;
+	u32 _platform = 0x08;/*SupportPlatform */
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+
+	cond = condition & 0xFF;
+	if ((_board & cond) == 0 && cond != 0x1F)
+		return false;
+
+	cond = condition & 0xFF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = condition & 0xFF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
 static void _rtl8723be_config_rf_reg(struct ieee80211_hw *hw, u32 addr,
 				     u32 data, enum radio_path rfpath,
 				     u32 regaddr)
 {
 	if (addr == 0xfe || addr == 0xffe) {
+		/* In order not to disturb BT music
+		 *	when wifi init.(1ant NIC only)
+		 */
 		mdelay(50);
 	} else {
 		rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
 		udelay(1);
 	}
 }
-
 static void _rtl8723be_config_rf_radio_a(struct ieee80211_hw *hw,
 					 u32 addr, u32 data)
 {
@@ -368,12 +203,13 @@ static void _rtl8723be_config_rf_radio_a(struct ieee80211_hw *hw,
 
 	_rtl8723be_config_rf_reg(hw, addr, data, RF90_PATH_A,
 				 addr | maskforphyset);
+
 }
 
 static void _rtl8723be_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	u8 band, path, txnum, section;
 
@@ -383,16 +219,38 @@ static void _rtl8723be_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
 				for (section = 0;
 				     section < TX_PWR_BY_RATE_NUM_SECTION;
 				     ++section)
-					rtlphy->tx_power_by_rate_offset[band]
-						[path][txnum][section] = 0;
+					rtlphy->tx_power_by_rate_offset
+					  [band][path][txnum][section] = 0;
+}
+
+static void _rtl8723be_config_bb_reg(struct ieee80211_hw *hw,
+				     u32 addr, u32 data)
+{
+	if (addr == 0xfe) {
+		mdelay(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		rtl_set_bbreg(hw, addr, MASKDWORD, data);
+		udelay(1);
+	}
 }
 
-static void phy_set_txpwr_by_rate_base(struct ieee80211_hw *hw, u8 band,
-				       u8 path, u8 rate_section,
-				       u8 txnum, u8 value)
+static void _rtl8723be_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
+						    u8 band,
+						    u8 path, u8 rate_section,
+						    u8 txnum, u8 value)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	if (path > RF90_PATH_D) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
@@ -417,23 +275,24 @@ static void phy_set_txpwr_by_rate_base(struct ieee80211_hw *hw, u8 band,
 			break;
 		default:
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "Invalid RateSection %d in Band 2.4G, Rf Path"
-				  " %d, %dTx in PHY_SetTxPowerByRateBase()\n",
-				  rate_section, path, txnum);
+				 "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_SetTxPowerByRateBase()\n",
+				 rate_section, path, txnum);
 			break;
 		};
 	} else {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 			 "Invalid Band %d in PHY_SetTxPowerByRateBase()\n",
-			  band);
+			 band);
 	}
+
 }
 
-static u8 phy_get_txpwr_by_rate_base(struct ieee80211_hw *hw, u8 band, u8 path,
-				     u8 txnum, u8 rate_section)
+static u8 _rtl8723be_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw,
+						  u8 band, u8 path, u8 txnum,
+						  u8 rate_section)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 value = 0;
 	if (path > RF90_PATH_D) {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
@@ -458,15 +317,14 @@ static u8 phy_get_txpwr_by_rate_base(struct ieee80211_hw *hw, u8 band, u8 path,
 			break;
 		default:
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "Invalid RateSection %d in Band 2.4G, Rf Path"
-				  " %d, %dTx in PHY_GetTxPowerByRateBase()\n",
-				  rate_section, path, txnum);
+				 "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+				 rate_section, path, txnum);
 			break;
 		};
 	} else {
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
 			 "Invalid Band %d in PHY_GetTxPowerByRateBase()\n",
-			  band);
+			 band);
 	}
 
 	return value;
@@ -475,45 +333,51 @@ static u8 phy_get_txpwr_by_rate_base(struct ieee80211_hw *hw, u8 band, u8 path,
 static void _rtl8723be_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u16 raw_value = 0;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u16 rawvalue = 0;
 	u8 base = 0, path = 0;
 
 	for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
 		if (path == RF90_PATH_A) {
-			raw_value = (u16) (rtlphy->tx_power_by_rate_offset
+			rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
 				[BAND_ON_2_4G][path][RF_1TX][3] >> 24) & 0xFF;
-			base = (raw_value >> 4) * 10 + (raw_value & 0xF);
-			phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path, CCK,
-						   RF_1TX, base);
+			base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
+			_rtl8723be_phy_set_txpower_by_rate_base(hw,
+				BAND_ON_2_4G, path, CCK, RF_1TX, base);
 		} else if (path == RF90_PATH_B) {
-			raw_value = (u16) (rtlphy->tx_power_by_rate_offset
+			rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
 				[BAND_ON_2_4G][path][RF_1TX][3] >> 0) & 0xFF;
-			base = (raw_value >> 4) * 10 + (raw_value & 0xF);
-			phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path,
-						   CCK, RF_1TX, base);
+			base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
+			_rtl8723be_phy_set_txpower_by_rate_base(hw,
+								BAND_ON_2_4G,
+								path, CCK,
+								RF_1TX, base);
 		}
-		raw_value = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-					  [path][RF_1TX][1] >> 24) & 0xFF;
-		base = (raw_value >> 4) * 10 + (raw_value & 0xF);
-		phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path, OFDM, RF_1TX,
-					   base);
-
-		raw_value = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-					  [path][RF_1TX][5] >> 24) & 0xFF;
-		base = (raw_value >> 4) * 10 + (raw_value & 0xF);
-		phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS0_MCS7,
-					   RF_1TX, base);
-
-		raw_value = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-					  [path][RF_2TX][7] >> 24) & 0xFF;
-		base = (raw_value >> 4) * 10 + (raw_value & 0xF);
-		phy_set_txpwr_by_rate_base(hw, BAND_ON_2_4G, path,
-					   HT_MCS8_MCS15, RF_2TX, base);
+		rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
+				[BAND_ON_2_4G][path][RF_1TX][1] >> 24) & 0xFF;
+		base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
+		_rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
+							path, OFDM, RF_1TX,
+							base);
+
+		rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
+				[BAND_ON_2_4G][path][RF_1TX][5] >> 24) & 0xFF;
+		base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
+		_rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
+							path, HT_MCS0_MCS7,
+							RF_1TX, base);
+
+		rawvalue = (u16)(rtlphy->tx_power_by_rate_offset
+				[BAND_ON_2_4G][path][RF_2TX][7] >> 24) & 0xFF;
+		base = (rawvalue >> 4) * 10 + (rawvalue & 0xF);
+		_rtl8723be_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G,
+							path, HT_MCS8_MCS15,
+							RF_2TX, base);
 	}
 }
 
-static void phy_conv_dbm_to_rel(u32 *data, u8 start, u8 end, u8 base_val)
+static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
+						u8 end, u8 base_val)
 {
 	char i = 0;
 	u8 temp_value = 0;
@@ -522,15 +386,15 @@ static void phy_conv_dbm_to_rel(u32 *data, u8 start, u8 end, u8 base_val)
 	for (i = 3; i >= 0; --i) {
 		if (i >= start && i <= end) {
 			/* Get the exact value */
-			temp_value = (u8) (*data >> (i * 8)) & 0xF;
-			temp_value += ((u8) ((*data >> (i*8 + 4)) & 0xF)) * 10;
+			temp_value = (u8)(*data >> (i * 8)) & 0xF;
+			temp_value += ((u8)((*data >> (i*8 + 4)) & 0xF)) * 10;
 
 			/* Change the value to a relative value */
 			temp_value = (temp_value > base_val) ?
 				     temp_value - base_val :
 				     base_val - temp_value;
 		} else {
-			temp_value = (u8) (*data >> (i * 8)) & 0xFF;
+			temp_value = (u8)(*data >> (i * 8)) & 0xFF;
 		}
 		temp_data <<= 8;
 		temp_data |= temp_value;
@@ -538,56 +402,65 @@ static void phy_conv_dbm_to_rel(u32 *data, u8 start, u8 end, u8 base_val)
 	*data = temp_data;
 }
 
-static void conv_dbm_to_rel(struct ieee80211_hw *hw)
+static void _rtl8723be_phy_convert_txpower_dbm_to_relative_value(
+							struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 base = 0, rfpath = RF90_PATH_A;
 
-	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
-					  RF_1TX, CCK);
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_1TX][2]), 1, 1, base);
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_1TX][3]), 1, 3, base);
-
-	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
-					  RF_1TX, OFDM);
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_1TX][0]), 0, 3, base);
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_1TX][1]), 0, 3, base);
-
-	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
-					  RF_1TX, HT_MCS0_MCS7);
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_1TX][4]), 0, 3, base);
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_1TX][5]), 0, 3, base);
-
-	base = phy_get_txpwr_by_rate_base(hw, BAND_ON_2_4G, rfpath,
-					  RF_2TX, HT_MCS8_MCS15);
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_2TX][6]), 0, 3, base);
-
-	phy_conv_dbm_to_rel(&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G]
-			    [rfpath][RF_2TX][7]), 0, 3, base);
+	base = _rtl8723be_phy_get_txpower_by_rate_base(hw,
+			BAND_ON_2_4G, rfpath, RF_1TX, CCK);
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][2],
+	    1, 1, base);
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][3],
+	    1, 3, base);
+
+	base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfpath,
+						       RF_1TX, OFDM);
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][0],
+	    0, 3, base);
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][1],
+	    0, 3, base);
+
+	base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G,
+						rfpath, RF_1TX, HT_MCS0_MCS7);
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][4],
+	    0, 3, base);
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_1TX][5],
+	    0, 3, base);
+
+	base = _rtl8723be_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G,
+						       rfpath, RF_2TX,
+						       HT_MCS8_MCS15);
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_2TX][6],
+	    0, 3, base);
+
+	_phy_convert_txpower_dbm_to_relative_value(
+	    &rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfpath][RF_2TX][7],
+	    0, 3, base);
 
 	RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
-		 "<=== conv_dbm_to_rel()\n");
+	    "<===_rtl8723be_phy_convert_txpower_dbm_to_relative_value()\n");
 }
 
-static void _rtl8723be_phy_txpower_by_rate_configuration(
-							struct ieee80211_hw *hw)
+static void phy_txpower_by_rate_config(struct ieee80211_hw *hw)
 {
 	_rtl8723be_phy_store_txpower_by_rate_base(hw);
-	conv_dbm_to_rel(hw);
+	_rtl8723be_phy_convert_txpower_dbm_to_relative_value(hw);
 }
 
 static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	bool rtstatus;
 
@@ -603,7 +476,7 @@ static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw)
 		rtstatus = _rtl8723be_phy_config_bb_with_pgheaderfile(hw,
 						BASEBAND_CONFIG_PHY_REG);
 	}
-	_rtl8723be_phy_txpower_by_rate_configuration(hw);
+	phy_txpower_by_rate_config(hw);
 	if (!rtstatus) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
 		return false;
@@ -614,39 +487,237 @@ static bool _rtl8723be_phy_bb8723b_config_parafile(struct ieee80211_hw *hw)
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
 		return false;
 	}
-	rtlphy->cck_high_power = (bool) (rtl_get_bbreg(hw,
-						       RFPGA0_XA_HSSIPARAMETER2,
-						       0x200));
+	rtlphy->cck_high_power = (bool)(rtl_get_bbreg(hw,
+						      RFPGA0_XA_HSSIPARAMETER2,
+						      0x200));
 	return true;
 }
 
+static bool _rtl8723be_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read rtl8723beMACPHY_Array\n");
+	arraylength = RTL8723BEMAC_1T_ARRAYLEN;
+	ptrarray = RTL8723BEMAC_1T_ARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Img:RTL8723bEMAC_1T_ARRAY LEN %d\n", arraylength);
+	for (i = 0; i < arraylength; i = i + 2)
+		rtl_write_byte(rtlpriv, ptrarray[i], (u8)ptrarray[i + 1]);
+	return true;
+}
+
+static bool _rtl8723be_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						     u8 configtype)
+{
+	#define READ_NEXT_PAIR(v1, v2, i) \
+		do { \
+			i += 2; \
+			v1 = array_table[i];\
+			v2 = array_table[i+1]; \
+		} while (0)
+
+	int i;
+	u32 *array_table;
+	u16 arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		arraylen = RTL8723BEPHY_REG_1TARRAYLEN;
+		array_table = RTL8723BEPHY_REG_1TARRAY;
+
+		for (i = 0; i < arraylen; i = i + 2) {
+			v1 = array_table[i];
+			v2 = array_table[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl8723be_config_bb_reg(hw, v1, v2);
+			} else {/*This line is the start line of branch.*/
+				/* to protect READ_NEXT_PAIR not overrun */
+				if (i >= arraylen - 2)
+					break;
+
+				if (!_rtl8723be_check_condition(hw,
+						array_table[i])) {
+					/*Discard the following
+					 *(offset, data) pairs
+					 */
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+					i -= 2; /* prevent from for-loop += 2*/
+				/*Configure matched pairs and
+				 *skip to end of if-else.
+				 */
+				} else {
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						_rtl8723be_config_bb_reg(hw,
+								    v1, v2);
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < arraylen - 2)
+						READ_NEXT_PAIR(v1, v2, i);
+				}
+			}
+		}
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+		arraylen = RTL8723BEAGCTAB_1TARRAYLEN;
+		array_table = RTL8723BEAGCTAB_1TARRAY;
+
+		for (i = 0; i < arraylen; i = i + 2) {
+			v1 = array_table[i];
+			v2 = array_table[i+1];
+			if (v1 < 0xCDCDCDCD) {
+				rtl_set_bbreg(hw, array_table[i],
+					      MASKDWORD,
+					      array_table[i + 1]);
+				udelay(1);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				/* to protect READ_NEXT_PAIR not overrun */
+				if (i >= arraylen - 2)
+					break;
+
+				if (!_rtl8723be_check_condition(hw,
+					array_table[i])) {
+					/*Discard the following
+					 *(offset, data) pairs
+					 */
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+					i -= 2; /* prevent from for-loop += 2*/
+				/*Configure matched pairs and
+				 *skip to end of if-else.
+				 */
+				} else {
+					READ_NEXT_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						rtl_set_bbreg(hw, array_table[i],
+							      MASKDWORD,
+							      array_table[i + 1]);
+						udelay(1);
+						READ_NEXT_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < arraylen - 2)
+						READ_NEXT_PAIR(v1, v2, i);
+				}
+			}
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 "The agctab_array_table[0] is %x Rtl818EEPHY_REGArray[1] is %x\n",
+				 array_table[i], array_table[i + 1]);
+		}
+	}
+	return true;
+}
+
+static u8 _rtl8723be_get_rate_section_index(u32 regaddr)
+{
+	u8 index = 0;
+
+	switch (regaddr) {
+	case RTXAGC_A_RATE18_06:
+		index = 0;
+	break;
+	case RTXAGC_A_RATE54_24:
+		index = 1;
+	break;
+	case RTXAGC_A_CCK1_MCS32:
+		index = 2;
+	break;
+	case RTXAGC_B_CCK11_A_CCK2_11:
+		index = 3;
+	break;
+	case RTXAGC_A_MCS03_MCS00:
+		index = 4;
+	break;
+	case RTXAGC_A_MCS07_MCS04:
+		index = 5;
+	break;
+	case RTXAGC_A_MCS11_MCS08:
+		index = 6;
+	break;
+	case RTXAGC_A_MCS15_MCS12:
+		index = 7;
+	break;
+	case RTXAGC_B_RATE18_06:
+		index = 0;
+	break;
+	case RTXAGC_B_RATE54_24:
+		index = 1;
+	break;
+	case RTXAGC_B_CCK1_55_MCS32:
+		index = 2;
+	break;
+	case RTXAGC_B_MCS03_MCS00:
+		index = 4;
+	break;
+	case RTXAGC_B_MCS07_MCS04:
+		index = 5;
+	break;
+	case RTXAGC_B_MCS11_MCS08:
+		index = 6;
+	break;
+	case RTXAGC_B_MCS15_MCS12:
+		index = 7;
+	break;
+	default:
+		regaddr &= 0xFFF;
+		if (regaddr >= 0xC20 && regaddr <= 0xC4C)
+			index = (u8)((regaddr - 0xC20) / 4);
+		else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
+			index = (u8)((regaddr - 0xE20) / 4);
+		break;
+	};
+	return index;
+}
+
 static void _rtl8723be_store_tx_power_by_rate(struct ieee80211_hw *hw,
 					      u32 band, u32 rfpath,
 					      u32 txnum, u32 regaddr,
 					      u32 bitmask, u32 data)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 rate_section = _rtl8723be_get_rate_section_index(regaddr);
 
 	if (band != BAND_ON_2_4G && band != BAND_ON_5G) {
-		RT_TRACE(rtlpriv, COMP_POWER, PHY_TXPWR,
-			 "Invalid Band %d\n", band);
+		RT_TRACE(rtlpriv, FPHY, PHY_TXPWR, "Invalid Band %d\n", band);
 		return;
 	}
-
-	if (rfpath > TX_PWR_BY_RATE_NUM_RF) {
-		RT_TRACE(rtlpriv, COMP_POWER, PHY_TXPWR,
+	if (rfpath > MAX_RF_PATH - 1) {
+		RT_TRACE(rtlpriv, FPHY, PHY_TXPWR,
 			 "Invalid RfPath %d\n", rfpath);
 		return;
 	}
-	if (txnum > TX_PWR_BY_RATE_NUM_RF) {
-		RT_TRACE(rtlpriv, COMP_POWER, PHY_TXPWR,
-			 "Invalid TxNum %d\n", txnum);
+	if (txnum > MAX_RF_PATH - 1) {
+		RT_TRACE(rtlpriv, FPHY, PHY_TXPWR, "Invalid TxNum %d\n", txnum);
 		return;
 	}
+
 	rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] =
 									data;
+
 }
 
 static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
@@ -678,21 +749,6 @@ static bool _rtl8723be_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
 					_rtl8723be_store_tx_power_by_rate(hw,
 							v1, v2, v3, v4, v5, v6);
 				continue;
-			} else {
-				/*don't need the hw_body*/
-				if (!_rtl8723be_check_condition(hw,
-						phy_regarray_table_pg[i])) {
-					i += 2; /* skip the pair of expression*/
-					v1 = phy_regarray_table_pg[i];
-					v2 = phy_regarray_table_pg[i+1];
-					v3 = phy_regarray_table_pg[i+2];
-					while (v2 != 0xDEAD) {
-						i += 3;
-						v1 = phy_regarray_table_pg[i];
-						v2 = phy_regarray_table_pg[i+1];
-						v3 = phy_regarray_table_pg[i+2];
-					}
-				}
 			}
 		}
 	} else {
@@ -733,22 +789,27 @@ bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 			v2 = radioa_array_table[i+1];
 			if (v1 < 0xcdcdcdcd) {
 				_rtl8723be_config_rf_radio_a(hw, v1, v2);
-			} else { /*This line is the start line of branch.*/
+			} else {/*This line is the start line of branch.*/
+				/* to protect READ_NEXT_PAIR not overrun */
+				if (i >= radioa_arraylen - 2)
+					break;
+
 				if (!_rtl8723be_check_condition(hw,
 						radioa_array_table[i])) {
-					/* Discard the following
-					 * (offset, data) pairs
+					/*Discard the following
+					 *(offset, data) pairs
 					 */
 					READ_NEXT_RF_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
 					       v2 != 0xCDEF &&
 					       v2 != 0xCDCD &&
-					       i < radioa_arraylen - 2)
+					       i < radioa_arraylen - 2) {
 						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
 					i -= 2; /* prevent from for-loop += 2*/
 				} else {
-					/* Configure matched pairs
-					 * and skip to end of if-else.
+					/*Configure matched pairs
+					 *and skip to end of if-else.
 					 */
 					READ_NEXT_RF_PAIR(v1, v2, i);
 					while (v2 != 0xDEAD &&
@@ -770,18 +831,12 @@ bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 
 		if (rtlhal->oem_id == RT_CID_819X_HP)
 			_rtl8723be_config_rf_radio_a(hw, 0x52, 0x7E4BD);
-
 		break;
 	case RF90_PATH_B:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not process\n");
-		break;
 	case RF90_PATH_C:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not process\n");
 		break;
 	case RF90_PATH_D:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 			 "switch case not process\n");
 		break;
 	}
@@ -791,26 +846,25 @@ bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
 void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	rtlphy->default_initialgain[0] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[1] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[2] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
 	rtlphy->default_initialgain[3] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
 
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-		 "Default initial gain (c50 = 0x%x, "
-		  "c58 = 0x%x, c60 = 0x%x, c68 = 0x%x\n",
-		  rtlphy->default_initialgain[0],
-		  rtlphy->default_initialgain[1],
-		  rtlphy->default_initialgain[2],
-		  rtlphy->default_initialgain[3]);
-
-	rtlphy->framesync = (u8) rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
+		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
+		 rtlphy->default_initialgain[0],
+		 rtlphy->default_initialgain[1],
+		 rtlphy->default_initialgain[2],
+		 rtlphy->default_initialgain[3]);
+
+	rtlphy->framesync = (u8)rtl_get_bbreg(hw, ROFDM0_RXDETECTOR3,
 					       MASKBYTE0);
 	rtlphy->framesync_c34 = rtl_get_bbreg(hw, ROFDM0_RXDETECTOR2,
 					      MASKDWORD);
@@ -823,7 +877,7 @@ void rtl8723be_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
 void rtl8723be_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 txpwr_level;
 	long txpwr_dbm;
 
@@ -854,6 +908,7 @@ static u8 _rtl8723be_phy_get_ratesection_intxpower_byrate(enum radio_path path,
 	case DESC92C_RATE1M:
 		rate_section = 2;
 		break;
+
 	case DESC92C_RATE2M:
 	case DESC92C_RATE5_5M:
 		if (path == RF90_PATH_A)
@@ -861,49 +916,58 @@ static u8 _rtl8723be_phy_get_ratesection_intxpower_byrate(enum radio_path path,
 		else if (path == RF90_PATH_B)
 			rate_section = 2;
 		break;
+
 	case DESC92C_RATE11M:
 		rate_section = 3;
 		break;
+
 	case DESC92C_RATE6M:
 	case DESC92C_RATE9M:
 	case DESC92C_RATE12M:
 	case DESC92C_RATE18M:
 		rate_section = 0;
 		break;
+
 	case DESC92C_RATE24M:
 	case DESC92C_RATE36M:
 	case DESC92C_RATE48M:
 	case DESC92C_RATE54M:
 		rate_section = 1;
 		break;
+
 	case DESC92C_RATEMCS0:
 	case DESC92C_RATEMCS1:
 	case DESC92C_RATEMCS2:
 	case DESC92C_RATEMCS3:
 		rate_section = 4;
 		break;
+
 	case DESC92C_RATEMCS4:
 	case DESC92C_RATEMCS5:
 	case DESC92C_RATEMCS6:
 	case DESC92C_RATEMCS7:
 		rate_section = 5;
 		break;
+
 	case DESC92C_RATEMCS8:
 	case DESC92C_RATEMCS9:
 	case DESC92C_RATEMCS10:
 	case DESC92C_RATEMCS11:
 		rate_section = 6;
 		break;
+
 	case DESC92C_RATEMCS12:
 	case DESC92C_RATEMCS13:
 	case DESC92C_RATEMCS14:
 	case DESC92C_RATEMCS15:
 		rate_section = 7;
 		break;
+
 	default:
 		RT_ASSERT(true, "Rate_Section is Illegal\n");
 		break;
 	}
+
 	return rate_section;
 }
 
@@ -912,7 +976,7 @@ static u8 _rtl8723be_get_txpower_by_rate(struct ieee80211_hw *hw,
 					 enum radio_path rfpath, u8 rate)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u8 shift = 0, rate_section, tx_num;
 	char tx_pwr_diff = 0;
 
@@ -988,7 +1052,7 @@ static u8 _rtl8723be_get_txpower_index(struct ieee80211_hw *hw, u8 path,
 		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
 			 "Illegal channel!\n");
 	}
-	if (RTL8723E_RX_HAL_IS_CCK_RATE(rate))
+	if (RX_HAL_IS_CCK_RATE(rate))
 		txpower = rtlefuse->txpwrlevel_cck[path][index];
 	else if (DESC92C_RATE6M <= rate)
 		txpower = rtlefuse->txpwrlevel_ht40_1s[path][index];
@@ -997,7 +1061,7 @@ static u8 _rtl8723be_get_txpower_index(struct ieee80211_hw *hw, u8 path,
 			 "invalid rate\n");
 
 	if (DESC92C_RATE6M <= rate && rate <= DESC92C_RATE54M &&
-	    !RTL8723E_RX_HAL_IS_CCK_RATE(rate))
+	    !RX_HAL_IS_CCK_RATE(rate))
 		txpower += rtlefuse->txpwr_legacyhtdiff[0][TX_1S];
 
 	if (bandwidth == HT_CHANNEL_WIDTH_20) {
@@ -1011,6 +1075,7 @@ static u8 _rtl8723be_get_txpower_index(struct ieee80211_hw *hw, u8 path,
 		if (DESC92C_RATEMCS8 <= rate && rate <= DESC92C_RATEMCS15)
 			txpower += rtlefuse->txpwr_ht40diff[0][TX_2S];
 	}
+
 	if (rtlefuse->eeprom_regulatory != 2)
 		power_diff_byrate = _rtl8723be_get_txpower_by_rate(hw,
 								   BAND_ON_2_4G,
@@ -1046,6 +1111,7 @@ static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_B_CCK11_A_CCK2_11,
 					       MASKBYTE3, power_index);
 			break;
+
 		case DESC92C_RATE6M:
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
 					       MASKBYTE0, power_index);
@@ -1062,6 +1128,7 @@ static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE18_06,
 					       MASKBYTE3, power_index);
 			break;
+
 		case DESC92C_RATE24M:
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
 					       MASKBYTE0, power_index);
@@ -1078,6 +1145,7 @@ static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_RATE54_24,
 					       MASKBYTE3, power_index);
 			break;
+
 		case DESC92C_RATEMCS0:
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
 					       MASKBYTE0, power_index);
@@ -1094,6 +1162,7 @@ static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS03_MCS00,
 					       MASKBYTE3, power_index);
 			break;
+
 		case DESC92C_RATEMCS4:
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
 					       MASKBYTE0, power_index);
@@ -1110,6 +1179,7 @@ static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS07_MCS04,
 					       MASKBYTE3, power_index);
 			break;
+
 		case DESC92C_RATEMCS8:
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
 					       MASKBYTE0, power_index);
@@ -1126,9 +1196,9 @@ static void _rtl8723be_phy_set_txpower_index(struct ieee80211_hw *hw,
 			rtl8723_phy_set_bb_reg(hw, RTXAGC_A_MCS11_MCS08,
 					       MASKBYTE3, power_index);
 			break;
+
 		default:
-			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
-				 "Invalid Rate!!\n");
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid Rate!!\n");
 			break;
 		}
 	} else {
@@ -1192,10 +1262,11 @@ void rtl8723be_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
 
 	if (!is_hal_stop(rtlhal)) {
 		switch (operation) {
-		case SCAN_OPT_BACKUP:
-			iotype = IO_CMD_PAUSE_DM_BY_SCAN;
+		case SCAN_OPT_BACKUP_BAND0:
+			iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
 			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_IO_CMD,
 						      (u8 *)&iotype);
+
 			break;
 		case SCAN_OPT_RESTORE:
 			iotype = IO_CMD_RESUME_DM_BY_SCAN;
@@ -1214,15 +1285,15 @@ void rtl8723be_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	u8 reg_bw_opmode;
 	u8 reg_prsr_rsc;
 
 	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
 		 "Switch to %s bandwidth\n",
-		 rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
-		 "20MHz" : "40MHz");
+		  rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" : "40MHz");
 
 	if (is_hal_stop(rtlhal)) {
 		rtlphy->set_bwmode_inprogress = false;
@@ -1254,13 +1325,17 @@ void rtl8723be_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
 	case HT_CHANNEL_WIDTH_20:
 		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x0);
 		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x0);
+	/*	rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 1);*/
 		break;
 	case HT_CHANNEL_WIDTH_20_40:
 		rtl_set_bbreg(hw, RFPGA0_RFMOD, BRFMOD, 0x1);
 		rtl_set_bbreg(hw, RFPGA1_RFMOD, BRFMOD, 0x1);
+
 		rtl_set_bbreg(hw, RCCK0_SYSTEM, BCCK_SIDEBAND,
 			      (mac->cur_40_prime_sc >> 1));
 		rtl_set_bbreg(hw, ROFDM1_LSTF, 0xC00, mac->cur_40_prime_sc);
+		/*rtl_set_bbreg(hw, RFPGA0_ANALOGPARAMETER2, BIT(10), 0);*/
+
 		rtl_set_bbreg(hw, 0x818, (BIT(26) | BIT(27)),
 			      (mac->cur_40_prime_sc ==
 			       HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
@@ -1279,7 +1354,7 @@ void rtl8723be_phy_set_bw_mode(struct ieee80211_hw *hw,
 			    enum nl80211_channel_type ch_type)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u8 tmp_bw = rtlphy->current_chan_bw;
 
@@ -1300,7 +1375,7 @@ void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 delay;
 
 	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
@@ -1310,11 +1385,11 @@ void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 	do {
 		if (!rtlphy->sw_chnl_inprogress)
 			break;
-		if (!rtl8723be_phy_sw_chn_step_by_step(hw,
-						       rtlphy->current_channel,
-						       &rtlphy->sw_chnl_stage,
-						       &rtlphy->sw_chnl_step,
-						       &delay)) {
+		if (!_rtl8723be_phy_sw_chnl_step_by_step(hw,
+							 rtlphy->current_channel,
+							 &rtlphy->sw_chnl_stage,
+							 &rtlphy->sw_chnl_step,
+							 &delay)) {
 			if (delay > 0)
 				mdelay(delay);
 			else
@@ -1330,7 +1405,7 @@ void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw)
 u8 rtl8723be_phy_sw_chnl(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	if (rtlphy->sw_chnl_inprogress)
@@ -1345,25 +1420,23 @@ u8 rtl8723be_phy_sw_chnl(struct ieee80211_hw *hw)
 	if (!(is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
 		rtl8723be_phy_sw_chnl_callback(hw);
 		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
-			 "sw_chnl_inprogress false schdule "
-			  "workitem current channel %d\n",
-			  rtlphy->current_channel);
+			 "sw_chnl_inprogress false schdule workitem current channel %d\n",
+			 rtlphy->current_channel);
 		rtlphy->sw_chnl_inprogress = false;
 	} else {
 		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
-			 "sw_chnl_inprogress false driver sleep or"
-			  " unload\n");
+			 "sw_chnl_inprogress false driver sleep or unload\n");
 		rtlphy->sw_chnl_inprogress = false;
 	}
 	return 1;
 }
 
-static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
-					      u8 channel, u8 *stage,
-					      u8 *step, u32 *delay)
+static bool _rtl8723be_phy_sw_chnl_step_by_step(struct ieee80211_hw *hw,
+						u8 channel, u8 *stage,
+						u8 *step, u32 *delay)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct swchnlcmd precommoncmd[MAX_PRECMD_CNT];
 	u32 precommoncmdcnt;
 	struct swchnlcmd postcommoncmd[MAX_POSTCMD_CNT];
@@ -1381,10 +1454,13 @@ static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
 					 0, 0, 0);
 	rtl8723_phy_set_sw_chnl_cmdarray(precommoncmd, precommoncmdcnt++,
 					 MAX_PRECMD_CNT, CMDID_END, 0, 0, 0);
+
 	postcommoncmdcnt = 0;
+
 	rtl8723_phy_set_sw_chnl_cmdarray(postcommoncmd, postcommoncmdcnt++,
 					 MAX_POSTCMD_CNT, CMDID_END,
-					 0, 0, 0);
+					    0, 0, 0);
+
 	rfdependcmdcnt = 0;
 
 	RT_ASSERT((channel >= 1 && channel <= 14),
@@ -1397,7 +1473,7 @@ static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
 
 	rtl8723_phy_set_sw_chnl_cmdarray(rfdependcmd, rfdependcmdcnt++,
 					 MAX_RFDEPENDCMD_CNT,
-					 CMDID_END, 0, 0, 0);
+					    CMDID_END, 0, 0, 0);
 
 	do {
 		switch (*stage) {
@@ -1410,6 +1486,10 @@ static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
 		case 2:
 			currentcmd = &postcommoncmd[*step];
 			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Invalid 'stage' = %d, Check it!\n", *stage);
+			return true;
 		}
 
 		if (currentcmd->cmdid == CMDID_END) {
@@ -1432,11 +1512,11 @@ static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
 			break;
 		case CMDID_WRITEPORT_USHORT:
 			rtl_write_word(rtlpriv, currentcmd->para1,
-				       (u16) currentcmd->para2);
+				       (u16)currentcmd->para2);
 			break;
 		case CMDID_WRITEPORT_UCHAR:
 			rtl_write_byte(rtlpriv, currentcmd->para1,
-				       (u8) currentcmd->para2);
+				       (u8)currentcmd->para2);
 			break;
 		case CMDID_RF_WRITEREG:
 			for (rfpath = 0; rfpath < num_total_rfpath; rfpath++) {
@@ -1451,7 +1531,7 @@ static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
 			}
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 				 "switch case not process\n");
 			break;
 		}
@@ -1464,54 +1544,515 @@ static bool rtl8723be_phy_sw_chn_step_by_step(struct ieee80211_hw *hw,
 	return false;
 }
 
-static u8 _rtl8723be_phy_path_a_iqk(struct ieee80211_hw *hw, bool config_pathb)
+static u8 _rtl8723be_phy_path_a_iqk(struct ieee80211_hw *hw)
 {
-	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
+	u32 reg_eac, reg_e94, reg_e9c, tmp;
 	u8 result = 0x00;
 
-	rtl_set_bbreg(hw, 0xe30, MASKDWORD, 0x10008c1c);
-	rtl_set_bbreg(hw, 0xe34, MASKDWORD, 0x30008c1c);
-	rtl_set_bbreg(hw, 0xe38, MASKDWORD, 0x8214032a);
-	rtl_set_bbreg(hw, 0xe3c, MASKDWORD, 0x28160000);
-
-	rtl_set_bbreg(hw, 0xe4c, MASKDWORD, 0x00462911);
-	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf9000000);
-	rtl_set_bbreg(hw, 0xe48, MASKDWORD, 0xf8000000);
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	/* switch to path A */
+	rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000000);
+	/* enable path A PA in TXIQK mode */
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x20000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0003f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xc7f87);
+
+	/* 1. TX IQK */
+	/* path-A IQK setting */
+	/* IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
+	/* path-A IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x821403ea);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28160000);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
+	/* LO calibration setting */
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
+	/* enter IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/* One shot, path A LOK & IQK */
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
 
 	mdelay(IQK_DELAY_TIME);
 
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	/* Check failed */
 	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
 	reg_e94 = rtl_get_bbreg(hw, 0xe94, MASKDWORD);
 	reg_e9c = rtl_get_bbreg(hw, 0xe9c, MASKDWORD);
-	reg_ea4 = rtl_get_bbreg(hw, 0xea4, MASKDWORD);
 
 	if (!(reg_eac & BIT(28)) &&
 	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
 	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
 		result |= 0x01;
+	else /* if Tx not OK, ignore Rx */
+		return result;
+
+	/* Allen 20131125 */
+	tmp = (reg_e9c & 0x03FF0000) >> 16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
+	    (tmp < 0xf))
+		result |= 0x01;
+	else /* if Tx not OK, ignore Rx */
+		return result;
+
 	return result;
 }
 
-static bool phy_similarity_cmp(struct ieee80211_hw *hw, long result[][8],
-			       u8 c1, u8 c2)
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 _rtl8723be_phy_path_a_rx_iqk(struct ieee80211_hw *hw)
 {
-	u32 i, j, diff, simularity_bitmap, bound;
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4, u32tmp, tmp;
+	u8 result = 0x00;
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	/* switch to path A */
+	rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000000);
+
+	/* 1 Get TXIMR setting */
+	/* modify RXIQK mode table */
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
+	/* LNA2 off, PA on for Dcut */
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7fb7);
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/* IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
+
+	/* path-A IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160ff0);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
+
+	/* LO calibration setting */
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
+
+	/* enter IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/* One shot, path A LOK & IQK */
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	/* Check failed */
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else /* if Tx not OK, ignore Rx */
+		return result;
+
+	/* Allen 20131125 */
+	tmp = (reg_e9c & 0x03FF0000) >> 16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
+	    (tmp < 0xf))
+		result |= 0x01;
+	else /* if Tx not OK, ignore Rx */
+		return result;
+
+	u32tmp = 0x80007C00 | (reg_e94 & 0x3FF0000) |
+		 ((reg_e9c & 0x3FF0000) >> 16);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32tmp);
+
+	/* 1 RX IQK */
+	/* modify RXIQK mode table */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
+	/* LAN2 on, PA off for Dcut */
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7d77);
+
+	/* PA, PAD setting */
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0xf80);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x55, RFREG_OFFSET_MASK, 0x4021f);
+
+	/* IQK setting */
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
+
+	/* path-A IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x2816001f);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
+
+	/* LO calibration setting */
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a8d1);
+
+	/* enter IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/* One shot, path A LOK & IQK */
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	/* Check failed */
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xdf, RFREG_OFFSET_MASK, 0x780);
+
+	/* Allen 20131125 */
+	tmp = (reg_eac & 0x03FF0000) >> 16;
+		if ((tmp & 0x200) > 0)
+			tmp = 0x400 - tmp;
+	/* if Tx is OK, check whether Rx is OK */
+	if (!(reg_eac & BIT(27)) &&
+	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	else if (!(reg_eac & BIT(27)) &&
+		 (((reg_ea4 & 0x03FF0000) >> 16) < 0x110) &&
+		 (((reg_ea4 & 0x03FF0000) >> 16) > 0xf0) &&
+		 (tmp < 0xf))
+		result |= 0x02;
+
+	return result;
+}
+
+static u8 _rtl8723be_phy_path_b_iqk(struct ieee80211_hw *hw)
+{
+	u32 reg_eac, reg_e94, reg_e9c, tmp;
+	u8 result = 0x00;
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	/* switch to path B */
+	rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000280);
+
+	/* enable path B PA in TXIQK mode */
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x40fc1);
+
+	/* 1 Tx IQK */
+	/* IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
+	/* path-A IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x821403ea);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
+
+	/* LO calibration setting */
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x00462911);
+
+	/* enter IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/* One shot, path B LOK & IQK */
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+
+	/* Check failed */
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	/* Allen 20131125 */
+	tmp = (reg_e9c & 0x03FF0000) >> 16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
+	    (tmp < 0xf))
+		result |= 0x01;
+	else
+		return result;
+
+	return result;
+}
+
+/* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
+static u8 _rtl8723be_phy_path_b_rx_iqk(struct ieee80211_hw *hw)
+{
+	u32 reg_e94, reg_e9c, reg_ea4, reg_eac, u32tmp, tmp;
+	u8 result = 0x00;
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	/* switch to path B */
+	rtl_set_bbreg(hw, 0x948, MASKDWORD, 0x00000280);
+
+	/* 1 Get TXIMR setting */
+	/* modify RXIQK mode table */
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ff7);
+
+	/* open PA S1 & SMIXER */
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x60fed);
+
+	/* IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, 0x01007c00);
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
+
+	/* path-B IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82160ff0);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x28110000);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
+
+	/* LO calibration setting */
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a911);
+	/* enter IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/* One shot, path B TXIQK @ RXIQK */
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	/* Check failed */
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_e94 = rtl_get_bbreg(hw, RTX_POWER_BEFORE_IQK_A, MASKDWORD);
+	reg_e9c = rtl_get_bbreg(hw, RTX_POWER_AFTER_IQK_A, MASKDWORD);
+
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
+		result |= 0x01;
+	else	/* if Tx not OK, ignore Rx */
+		return result;
 
-	u8 final_candidate[2] = { 0xFF, 0xFF };
-	bool bresult = true, is2t = IS_92C_SERIAL(rtlhal->version);
+	/* Allen 20131125 */
+	tmp = (reg_e9c & 0x03FF0000) >> 16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
 
-	if (is2t)
-		bound = 8;
+	if (!(reg_eac & BIT(28)) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) < 0x110) &&
+	    (((reg_e94 & 0x03FF0000) >> 16) > 0xf0) &&
+	    (tmp < 0xf))
+		result |= 0x01;
+	else
+		return result;
+
+	u32tmp = 0x80007C00 | (reg_e94 & 0x3FF0000)  |
+		 ((reg_e9c & 0x3FF0000) >> 16);
+	rtl_set_bbreg(hw, RTX_IQK, MASKDWORD, u32tmp);
+
+	/* 1 RX IQK */
+
+	/* <20121009, Kordan> RF Mode = 3 */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x1);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0001f);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7d77);
+	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, 0x80000, 0x0);
+
+	/* open PA S1 & close SMIXER */
+	rtl_set_rfreg(hw, RF90_PATH_A, 0xed, RFREG_OFFSET_MASK, 0x00020);
+	rtl_set_rfreg(hw, RF90_PATH_A, 0x43, RFREG_OFFSET_MASK, 0x60fbd);
+
+	/* IQK setting */
+	rtl_set_bbreg(hw, RRX_IQK, MASKDWORD, 0x01004800);
+
+	/* path-B IQK setting */
+	rtl_set_bbreg(hw, RTX_IQK_TONE_A, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_A, MASKDWORD, 0x18008c1c);
+	rtl_set_bbreg(hw, RTX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+	rtl_set_bbreg(hw, RRX_IQK_TONE_B, MASKDWORD, 0x38008c1c);
+
+	rtl_set_bbreg(hw, RTX_IQK_PI_A, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_A, MASKDWORD, 0x2816001f);
+	rtl_set_bbreg(hw, RTX_IQK_PI_B, MASKDWORD, 0x82110000);
+	rtl_set_bbreg(hw, RRX_IQK_PI_B, MASKDWORD, 0x28110000);
+
+	/* LO calibration setting */
+	rtl_set_bbreg(hw, RIQK_AGC_RSP, MASKDWORD, 0x0046a8d1);
+	/* enter IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
+
+	/* One shot, path B LOK & IQK */
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf9000000);
+	rtl_set_bbreg(hw, RIQK_AGC_PTS, MASKDWORD, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME);
+
+	/* leave IQK mode */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
+	/* Check failed */
+	reg_eac = rtl_get_bbreg(hw, RRX_POWER_AFTER_IQK_A_2, MASKDWORD);
+	reg_ea4 = rtl_get_bbreg(hw, RRX_POWER_BEFORE_IQK_A_2, MASKDWORD);
+
+	/* Allen 20131125 */
+	tmp = (reg_eac & 0x03FF0000) >> 16;
+	if ((tmp & 0x200) > 0)
+		tmp = 0x400 - tmp;
+
+	/* if Tx is OK, check whether Rx is OK */
+	if (!(reg_eac & BIT(27)) &&
+	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
+	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
+		result |= 0x02;
+	else if (!(reg_eac & BIT(27)) &&
+		 (((reg_ea4 & 0x03FF0000) >> 16) < 0x110) &&
+		 (((reg_ea4 & 0x03FF0000) >> 16) > 0xf0) &&
+		 (tmp < 0xf))
+		result |= 0x02;
 	else
-		bound = 4;
+		return result;
+
+	return result;
+}
+
+static void _rtl8723be_phy_path_b_fill_iqk_matrix(struct ieee80211_hw *hw,
+						  bool b_iqk_ok,
+						  long result[][8],
+						  u8 final_candidate,
+						  bool btxonly)
+{
+	u32 oldval_1, x, tx1_a, reg;
+	long y, tx1_c;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (b_iqk_ok) {
+		oldval_1 = (rtl_get_bbreg(hw, ROFDM0_XBTXIQIMBALANCE,
+					  MASKDWORD) >> 22) & 0x3FF;
+		x = result[final_candidate][4];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx1_a = (x * oldval_1) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x3FF, tx1_a);
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(27),
+			      ((x * oldval_1 >> 7) & 0x1));
+		y = result[final_candidate][5];
+		if ((y & 0x00000200) != 0)
+			y = y | 0xFFFFFC00;
+		tx1_c = (y * oldval_1) >> 8;
+		rtl_set_bbreg(hw, ROFDM0_XDTXAFE, 0xF0000000,
+			      ((tx1_c & 0x3C0) >> 6));
+		rtl_set_bbreg(hw, ROFDM0_XBTXIQIMBALANCE, 0x003F0000,
+			      (tx1_c & 0x3F));
+		rtl_set_bbreg(hw, ROFDM0_ECCATHRESHOLD, BIT(25),
+			      ((y * oldval_1 >> 7) & 0x1));
+		if (btxonly)
+			return;
+		reg = result[final_candidate][6];
+		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0x3FF, reg);
+		reg = result[final_candidate][7] & 0x3F;
+		rtl_set_bbreg(hw, ROFDM0_XBRXIQIMBALANCE, 0xFC00, reg);
+		reg = (result[final_candidate][7] >> 6) & 0xF;
+		/* rtl_set_bbreg(hw, 0xca0, 0xF0000000, reg); */
+	}
+}
+
+static bool _rtl8723be_phy_simularity_compare(struct ieee80211_hw *hw,
+					      long result[][8], u8 c1, u8 c2)
+{
+	u32 i, j, diff, simularity_bitmap, bound = 0;
+
+	u8 final_candidate[2] = {0xFF, 0xFF}; /* for path A and path B */
+	bool bresult = true; /* is2t = true*/
+	s32 tmp1 = 0, tmp2 = 0;
+
+	bound = 8;
 
 	simularity_bitmap = 0;
 
 	for (i = 0; i < bound; i++) {
-		diff = (result[c1][i] > result[c2][i]) ?
-		    (result[c1][i] - result[c2][i]) :
-		    (result[c2][i] - result[c1][i]);
+		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
+			if ((result[c1][i] & 0x00000200) != 0)
+				tmp1 = result[c1][i] | 0xFFFFFC00;
+			else
+				tmp1 = result[c1][i];
+
+			if ((result[c2][i] & 0x00000200) != 0)
+				tmp2 = result[c2][i] | 0xFFFFFC00;
+			else
+				tmp2 = result[c2][i];
+		} else {
+			tmp1 = result[c1][i];
+			tmp2 = result[c2][i];
+		}
+
+		diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
 
 		if (diff > MAX_TOLERANCE) {
 			if ((i == 2 || i == 6) && !simularity_bitmap) {
@@ -1521,9 +2062,8 @@ static bool phy_similarity_cmp(struct ieee80211_hw *hw, long result[][8],
 					final_candidate[(i / 4)] = c1;
 				else
 					simularity_bitmap |= (1 << i);
-			} else {
+			} else
 				simularity_bitmap |= (1 << i);
-			}
 		}
 	}
 
@@ -1537,15 +2077,23 @@ static bool phy_similarity_cmp(struct ieee80211_hw *hw, long result[][8],
 			}
 		}
 		return bresult;
-	} else if (!(simularity_bitmap & 0x0F)) {
-		for (i = 0; i < 4; i++)
-			result[3][i] = result[c1][i];
-		return false;
-	} else if (!(simularity_bitmap & 0xF0) && is2t) {
-		for (i = 4; i < 8; i++)
-			result[3][i] = result[c1][i];
-		return false;
 	} else {
+		if (!(simularity_bitmap & 0x03)) { /* path A TX OK */
+			for (i = 0; i < 2; i++)
+				result[3][i] = result[c1][i];
+		}
+		if (!(simularity_bitmap & 0x0c)) { /* path A RX OK */
+			for (i = 2; i < 4; i++)
+				result[3][i] = result[c1][i];
+		}
+		if (!(simularity_bitmap & 0x30)) { /* path B TX OK */
+			for (i = 4; i < 6; i++)
+				result[3][i] = result[c1][i];
+		}
+		if (!(simularity_bitmap & 0xc0)) { /* path B RX OK */
+			for (i = 6; i < 8; i++)
+				result[3][i] = result[c1][i];
+		}
 		return false;
 	}
 }
@@ -1554,9 +2102,9 @@ static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
 					long result[][8], u8 t, bool is2t)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	u32 i;
-	u8 patha_ok;
+	u8 patha_ok, pathb_ok;
 	u32 adda_reg[IQK_ADDA_REG_NUM] = {
 		0x85c, 0xe6c, 0xe70, 0xe74,
 		0xe78, 0xe7c, 0xe80, 0xe84,
@@ -1571,10 +2119,12 @@ static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
 		ROFDM0_TRXPATHENABLE, ROFDM0_TRMUXPAR,
 		RFPGA0_XCD_RFINTERFACESW, 0xb68, 0xb6c,
 		0x870, 0x860,
-		0x864, 0x800
+		0x864, 0xa04
 	};
 	const u32 retrycount = 2;
-	u32 path_sel_bb, path_sel_rf;
+
+	u32 path_sel_bb;/* path_sel_rf */
+
 	u8 tmp_reg_c50, tmp_reg_c58;
 
 	tmp_reg_c50 = rtl_get_bbreg(hw, 0xc50, MASKBYTE0);
@@ -1591,62 +2141,97 @@ static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
 	}
 	rtl8723_phy_path_adda_on(hw, adda_reg, true, is2t);
 	if (t == 0) {
-		rtlphy->rfpi_enable = (u8) rtl_get_bbreg(hw,
+		rtlphy->rfpi_enable = (u8)rtl_get_bbreg(hw,
 						RFPGA0_XA_HSSIPARAMETER1,
 						BIT(8));
 	}
-	if (!rtlphy->rfpi_enable)
-		rtl8723_phy_pi_mode_switch(hw, true);
 
 	path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
-	path_sel_rf = rtl_get_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff);
 
+	rtl8723_phy_mac_setting_calibration(hw, iqk_mac_reg,
+					    rtlphy->iqk_mac_backup);
 	/*BB Setting*/
-	rtl_set_bbreg(hw, 0x800, BIT(24), 0x00);
+	rtl_set_bbreg(hw, 0xa04, 0x0f000000, 0xf);
 	rtl_set_bbreg(hw, 0xc04, MASKDWORD, 0x03a05600);
 	rtl_set_bbreg(hw, 0xc08, MASKDWORD, 0x000800e4);
 	rtl_set_bbreg(hw, 0x874, MASKDWORD, 0x22204000);
 
-	rtl_set_bbreg(hw, 0x870, BIT(10), 0x01);
-	rtl_set_bbreg(hw, 0x870, BIT(26), 0x01);
-	rtl_set_bbreg(hw, 0x860, BIT(10), 0x00);
-	rtl_set_bbreg(hw, 0x864, BIT(10), 0x00);
-
-	if (is2t)
-		rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASKDWORD, 0x10000);
-	rtl8723_phy_mac_setting_calibration(hw, iqk_mac_reg,
-					    rtlphy->iqk_mac_backup);
-	rtl_set_bbreg(hw, 0xb68, MASKDWORD, 0x0f600000);
-
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0x80800000);
-	rtl_set_bbreg(hw, 0xe40, MASKDWORD, 0x01007c00);
-	rtl_set_bbreg(hw, 0xe44, MASKDWORD, 0x81004800);
+	/* path A TX IQK */
 	for (i = 0; i < retrycount; i++) {
-		patha_ok = _rtl8723be_phy_path_a_iqk(hw, is2t);
+		patha_ok = _rtl8723be_phy_path_a_iqk(hw);
 		if (patha_ok == 0x01) {
 			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "Path A Tx IQK Success!!\n");
+				"Path A Tx IQK Success!!\n");
 			result[t][0] = (rtl_get_bbreg(hw, 0xe94, MASKDWORD) &
 					0x3FF0000) >> 16;
 			result[t][1] = (rtl_get_bbreg(hw, 0xe9c, MASKDWORD) &
 					0x3FF0000) >> 16;
 			break;
+		} else {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path A Tx IQK Fail!!\n");
 		}
 	}
-
-	if (0 == patha_ok)
+	/* path A RX IQK */
+	for (i = 0; i < retrycount; i++) {
+		patha_ok = _rtl8723be_phy_path_a_rx_iqk(hw);
+		if (patha_ok == 0x03) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path A Rx IQK Success!!\n");
+			result[t][2] = (rtl_get_bbreg(hw, 0xea4, MASKDWORD) &
+					0x3FF0000) >> 16;
+			result[t][3] = (rtl_get_bbreg(hw, 0xeac, MASKDWORD) &
+					0x3FF0000) >> 16;
+			break;
+		}
 		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-			 "Path A IQK Success!!\n");
+			 "Path A Rx IQK Fail!!\n");
+	}
+
+	if (0x00 == patha_ok)
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Path A IQK Fail!!\n");
+
 	if (is2t) {
-		rtl8723_phy_path_a_standby(hw);
-		rtl8723_phy_path_adda_on(hw, adda_reg, false, is2t);
+		/* path B TX IQK */
+		for (i = 0; i < retrycount; i++) {
+			pathb_ok = _rtl8723be_phy_path_b_iqk(hw);
+			if (pathb_ok == 0x01) {
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "Path B Tx IQK Success!!\n");
+				result[t][4] = (rtl_get_bbreg(hw, 0xe94,
+							      MASKDWORD) &
+							      0x3FF0000) >> 16;
+				result[t][5] = (rtl_get_bbreg(hw, 0xe9c,
+							      MASKDWORD) &
+							      0x3FF0000) >> 16;
+				break;
+			}
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path B Tx IQK Fail!!\n");
+		}
+		/* path B RX IQK */
+		for (i = 0; i < retrycount; i++) {
+			pathb_ok = _rtl8723be_phy_path_b_rx_iqk(hw);
+			if (pathb_ok == 0x03) {
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "Path B Rx IQK Success!!\n");
+				result[t][6] = (rtl_get_bbreg(hw, 0xea4,
+							      MASKDWORD) &
+							      0x3FF0000) >> 16;
+				result[t][7] = (rtl_get_bbreg(hw, 0xeac,
+							      MASKDWORD) &
+							      0x3FF0000) >> 16;
+				break;
+			}
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Path B Rx IQK Fail!!\n");
+		}
 	}
 
-	rtl_set_bbreg(hw, 0xe28, MASKDWORD, 0);
+	/* Back to BB mode, load original value */
+	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0);
 
 	if (t != 0) {
-		if (!rtlphy->rfpi_enable)
-			rtl8723_phy_pi_mode_switch(hw, false);
 		rtl8723_phy_reload_adda_registers(hw, adda_reg,
 						  rtlphy->adda_backup, 16);
 		rtl8723_phy_reload_mac_registers(hw, iqk_mac_reg,
@@ -1656,7 +2241,7 @@ static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
 						  IQK_BB_REG_NUM);
 
 		rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
-		rtl_set_rfreg(hw, RF90_PATH_B, 0xb0, 0xfffff, path_sel_rf);
+		/*rtl_set_rfreg(hw, RF90_PATH_B, 0xb0, 0xfffff, path_sel_rf);*/
 
 		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, 0x50);
 		rtl_set_bbreg(hw, 0xc50, MASKBYTE0, tmp_reg_c50);
@@ -1670,11 +2255,33 @@ static void _rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "8723be IQK Finish!!\n");
 }
 
+static u8 _get_right_chnl_place_for_iqk(u8 chnl)
+{
+	u8 channel_all[TARGET_CHNL_NUM_2G_5G] = {
+			1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12,
+			13, 14, 36, 38, 40, 42, 44, 46,
+			48, 50, 52, 54, 56, 58, 60, 62, 64,
+			100, 102, 104, 106, 108, 110,
+			112, 114, 116, 118, 120, 122,
+			124, 126, 128, 130, 132, 134, 136,
+			138, 140, 149, 151, 153, 155, 157,
+			159, 161, 163, 165};
+	u8 place = chnl;
+
+	if (chnl > 14) {
+		for (place = 14; place < sizeof(channel_all); place++) {
+			if (channel_all[place] == chnl)
+				return place - 13;
+		}
+	}
+	return 0;
+}
+
 static void _rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 {
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 tmpreg;
 	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
 
@@ -1702,7 +2309,10 @@ static void _rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdfbe0);
 	rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, 0x8c0a);
 
-	mdelay(100);
+	/* In order not to disturb BT music when wifi init.(1ant NIC only) */
+	/*mdelay(100);*/
+	/* In order not to disturb BT music when wifi init.(1ant NIC only) */
+	mdelay(50);
 
 	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdffe0);
 
@@ -1716,68 +2326,34 @@ static void _rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
 	} else {
 		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
 	}
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+
 }
 
 static void _rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw,
 					     bool bmain, bool is2t)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
 
-	if (is_hal_stop(rtlhal)) {
-		u8 u1btmp;
-		u1btmp = rtl_read_byte(rtlpriv, REG_LEDCFG0);
-		rtl_write_byte(rtlpriv, REG_LEDCFG0, u1btmp | BIT(7));
-		rtl_set_bbreg(hw, RFPGA0_XAB_RFPARAMETER, BIT(13), 0x01);
-	}
-	if (is2t) {
-		if (bmain)
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
-				      BIT(5) | BIT(6), 0x1);
-		else
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
-				      BIT(5) | BIT(6), 0x2);
-	} else {
-		rtl_set_bbreg(hw, RFPGA0_XAB_RFINTERFACESW, BIT(8) | BIT(9), 0);
-		rtl_set_bbreg(hw, 0x914, MASKLWORD, 0x0201);
-
-		/* We use the RF definition of MAIN and AUX,
-		 * left antenna and right antenna repectively.
-		 * Default output at AUX.
-		 */
-		if (bmain) {
-			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
-				      BIT(14) | BIT(13) | BIT(12), 0);
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
-				      BIT(5) | BIT(4) | BIT(3), 0);
-			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
-				rtl_set_bbreg(hw, CONFIG_RAM64X16, BIT(31), 0);
-		} else {
-			rtl_set_bbreg(hw, RFPGA0_XA_RFINTERFACEOE,
-				      BIT(14) | BIT(13) | BIT(12), 1);
-			rtl_set_bbreg(hw, RFPGA0_XB_RFINTERFACEOE,
-				      BIT(5) | BIT(4) | BIT(3), 1);
-			if (rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)
-				rtl_set_bbreg(hw, CONFIG_RAM64X16, BIT(31), 1);
-		}
-	}
+	if (bmain) /* left antenna */
+		rtl_set_bbreg(hw, 0x92C, MASKDWORD, 0x1);
+	else
+		rtl_set_bbreg(hw, 0x92C, MASKDWORD, 0x2);
 }
 
 #undef IQK_ADDA_REG_NUM
 #undef IQK_DELAY_TIME
-
-void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
+/* IQK is merge from Merge Temp */
+void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	long result[4][8];
-	u8 i, final_candidate;
-	bool patha_ok, pathb_ok;
-	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
-	    reg_ecc, reg_tmp = 0;
+	u8 i, final_candidate, idx;
+	bool b_patha_ok, b_pathb_ok;
+	long reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4;
+	long reg_ecc, reg_tmp = 0;
 	bool is12simular, is13simular, is23simular;
 	u32 iqk_bb_reg[9] = {
 		ROFDM0_XARXIQIMBALANCE,
@@ -1790,12 +2366,23 @@ void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		ROFDM0_XDTXAFE,
 		ROFDM0_RXIQEXTANTA
 	};
+	u32 path_sel_bb = 0; /* path_sel_rf = 0 */
 
-	if (recovery) {
+	if (rtlphy->lck_inprogress)
+		return;
+
+	spin_lock(&rtlpriv->locks.iqk_lock);
+	rtlphy->lck_inprogress = true;
+	spin_unlock(&rtlpriv->locks.iqk_lock);
+
+	if (b_recovery) {
 		rtl8723_phy_reload_adda_registers(hw, iqk_bb_reg,
 						  rtlphy->iqk_bb_backup, 9);
 		return;
 	}
+	/* Save RF Path */
+	path_sel_bb = rtl_get_bbreg(hw, 0x948, MASKDWORD);
+	/* path_sel_rf = rtl_get_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff); */
 
 	for (i = 0; i < 8; i++) {
 		result[0][i] = 0;
@@ -1804,30 +2391,33 @@ void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		result[3][i] = 0;
 	}
 	final_candidate = 0xff;
-	patha_ok = false;
-	pathb_ok = false;
+	b_patha_ok = false;
+	b_pathb_ok = false;
 	is12simular = false;
 	is23simular = false;
 	is13simular = false;
 	for (i = 0; i < 3; i++) {
-		if (get_rf_type(rtlphy) == RF_2T2R)
-			_rtl8723be_phy_iq_calibrate(hw, result, i, true);
-		else
-			_rtl8723be_phy_iq_calibrate(hw, result, i, false);
+		_rtl8723be_phy_iq_calibrate(hw, result, i, true);
 		if (i == 1) {
-			is12simular = phy_similarity_cmp(hw, result, 0, 1);
+			is12simular = _rtl8723be_phy_simularity_compare(hw,
+									result,
+									0, 1);
 			if (is12simular) {
 				final_candidate = 0;
 				break;
 			}
 		}
 		if (i == 2) {
-			is13simular = phy_similarity_cmp(hw, result, 0, 2);
+			is13simular = _rtl8723be_phy_simularity_compare(hw,
+									result,
+									0, 2);
 			if (is13simular) {
 				final_candidate = 0;
 				break;
 			}
-			is23simular = phy_similarity_cmp(hw, result, 1, 2);
+			is23simular = _rtl8723be_phy_simularity_compare(hw,
+									result,
+									1, 2);
 			if (is23simular) {
 				final_candidate = 1;
 			} else {
@@ -1864,32 +2454,48 @@ void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw, bool recovery)
 		rtlphy->reg_ebc = reg_ebc;
 		reg_ec4 = result[final_candidate][6];
 		reg_ecc = result[final_candidate][7];
-		patha_ok = true;
-		pathb_ok = true;
+		b_patha_ok = true;
+		b_pathb_ok = true;
 	} else {
 		rtlphy->reg_e94 = 0x100;
 		rtlphy->reg_eb4 = 0x100;
 		rtlphy->reg_e9c = 0x0;
 		rtlphy->reg_ebc = 0x0;
 	}
-	if (reg_e94 != 0) /*&&(reg_ea4 != 0) */
-		rtl8723_phy_path_a_fill_iqk_matrix(hw, patha_ok, result,
+	if (reg_e94 != 0)
+		rtl8723_phy_path_a_fill_iqk_matrix(hw, b_patha_ok, result,
 						   final_candidate,
 						   (reg_ea4 == 0));
-	if (final_candidate != 0xFF) {
+	if (reg_eb4 != 0)
+		_rtl8723be_phy_path_b_fill_iqk_matrix(hw, b_pathb_ok, result,
+						      final_candidate,
+						      (reg_ec4 == 0));
+
+	idx = _get_right_chnl_place_for_iqk(rtlphy->current_channel);
+
+	if (final_candidate < 4) {
 		for (i = 0; i < IQK_MATRIX_REG_NUM; i++)
-			rtlphy->iqk_matrix[0].value[0][i] =
+			rtlphy->iqk_matrix[idx].value[0][i] =
 						result[final_candidate][i];
-		rtlphy->iqk_matrix[0].iqk_done = true;
+		rtlphy->iqk_matrix[idx].iqk_done = true;
+
 	}
-	rtl8723_save_adda_registers(hw, iqk_bb_reg, rtlphy->iqk_bb_backup, 9);
+	rtl8723_save_adda_registers(hw, iqk_bb_reg,
+				    rtlphy->iqk_bb_backup, 9);
+
+	rtl_set_bbreg(hw, 0x948, MASKDWORD, path_sel_bb);
+	/* rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, 0xfffff, path_sel_rf); */
+
+	spin_lock(&rtlpriv->locks.iqk_lock);
+	rtlphy->lck_inprogress = false;
+	spin_unlock(&rtlpriv->locks.iqk_lock);
 }
 
 void rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = &rtlpriv->rtlhal;
 	u32 timeout = 2000, timecount = 0;
 
 	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
@@ -1898,68 +2504,25 @@ void rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw)
 	}
 
 	rtlphy->lck_inprogress = true;
-	RTPRINT(rtlpriv, FINIT, INIT_EEPROM,
+	RTPRINT(rtlpriv, FINIT, INIT_IQK,
 		"LCK:Start!!! currentband %x delay %d ms\n",
-		rtlhal->current_bandtype, timecount);
+		 rtlhal->current_bandtype, timecount);
 
 	_rtl8723be_phy_lc_calibrate(hw, false);
 
 	rtlphy->lck_inprogress = false;
 }
 
-void rtl23b_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (rtlphy->apk_done)
-		return;
-
-	return;
-}
-
 void rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
 {
-	_rtl8723be_phy_set_rfpath_switch(hw, bmain, false);
-}
-
-static void rtl8723be_phy_set_io(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
-		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
-	switch (rtlphy->current_io_type) {
-	case IO_CMD_RESUME_DM_BY_SCAN:
-		rtlpriv->dm_digtable.cur_igvalue =
-				 rtlphy->initgain_backup.xaagccore1;
-		/*rtl92c_dm_write_dig(hw);*/
-		rtl8723be_phy_set_txpower_level(hw, rtlphy->current_channel);
-		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
-		break;
-	case IO_CMD_PAUSE_DM_BY_SCAN:
-		rtlphy->initgain_backup.xaagccore1 =
-				 rtlpriv->dm_digtable.cur_igvalue;
-		rtlpriv->dm_digtable.cur_igvalue = 0x17;
-		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
-		break;
-	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "switch case not process\n");
-		break;
-	}
-	rtlphy->set_io_inprogress = false;
-	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
-		 "(%#x)\n", rtlphy->current_io_type);
+	_rtl8723be_phy_set_rfpath_switch(hw, bmain, true);
 }
 
 bool rtl8723be_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	bool postprocessing = false;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	bool b_postprocessing = false;
 
 	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
@@ -1969,20 +2532,20 @@ bool rtl8723be_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 		case IO_CMD_RESUME_DM_BY_SCAN:
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 				 "[IO CMD] Resume DM after scan.\n");
-			postprocessing = true;
+			b_postprocessing = true;
 			break;
-		case IO_CMD_PAUSE_DM_BY_SCAN:
+		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
 			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
 				 "[IO CMD] Pause DM before scan.\n");
-			postprocessing = true;
+			b_postprocessing = true;
 			break;
 		default:
-			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 				 "switch case not process\n");
 			break;
 		}
 	} while (false);
-	if (postprocessing && !rtlphy->set_io_inprogress) {
+	if (b_postprocessing && !rtlphy->set_io_inprogress) {
 		rtlphy->set_io_inprogress = true;
 		rtlphy->current_io_type = iotype;
 	} else {
@@ -1993,6 +2556,37 @@ bool rtl8723be_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
 	return true;
 }
 
+static void rtl8723be_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		dm_digtable->cur_igvalue = rtlphy->initgain_backup.xaagccore1;
+		/*rtl92c_dm_write_dig(hw);*/
+		rtl8723be_phy_set_txpower_level(hw, rtlphy->current_channel);
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x83);
+		break;
+	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
+		dm_digtable->cur_igvalue = 0x17;
+		rtl_set_bbreg(hw, RCCK0_CCA, 0xff0000, 0x40);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "(%#x)\n", rtlphy->current_io_type);
+}
+
 static void rtl8723be_phy_set_rf_on(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -2028,15 +2622,15 @@ static bool _rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
 	switch (rfpwr_state) {
 	case ERFON:
 		if ((ppsc->rfpwr_state == ERFOFF) &&
-		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
+		     RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
 			bool rtstatus;
-			u32 initialize_count = 0;
+			u32 initializecount = 0;
 			do {
-				initialize_count++;
+				initializecount++;
 				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
 					 "IPS Set eRf nic enable\n");
 				rtstatus = rtl_ps_enable_nic(hw);
-			} while (!rtstatus && (initialize_count < 10));
+			} while (!rtstatus && (initializecount < 10));
 				RT_CLEAR_PS_LEVEL(ppsc,
 						  RT_RF_OFF_LEVL_HALT_NIC);
 		} else {
@@ -2051,28 +2645,33 @@ static bool _rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
 			rtlpriv->cfg->ops->led_control(hw, LED_CTL_LINK);
 		else
 			rtlpriv->cfg->ops->led_control(hw, LED_CTL_NO_LINK);
+
 		break;
+
 	case ERFOFF:
 		for (queue_id = 0, i = 0;
 		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
 			ring = &pcipriv->dev.tx_ring[queue_id];
-			if (skb_queue_len(&ring->queue) == 0) {
+			/* Don't check BEACON Q.
+			 * BEACON Q is always not empty,
+			 * because '_rtl8723be_cmd_send_packet'
+			 */
+			if (queue_id == BEACON_QUEUE ||
+			    skb_queue_len(&ring->queue) == 0) {
 				queue_id++;
 				continue;
 			} else {
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "eRf Off/Sleep: %d times "
-					  "TcbBusyQueue[%d] =%d before "
-					  "doze!\n", (i + 1), queue_id,
-					  skb_queue_len(&ring->queue));
+					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					 (i + 1), queue_id,
+					 skb_queue_len(&ring->queue));
 
 				udelay(10);
 				i++;
 			}
 			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "\n ERFSLEEP: %d times "
-					  "TcbBusyQueue[%d] = %d !\n",
+					 "ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
 					  MAX_DOZE_WAITING_TIMES_9x,
 					  queue_id,
 					  skb_queue_len(&ring->queue));
@@ -2095,6 +2694,7 @@ static bool _rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
 			}
 		}
 		break;
+
 	case ERFSLEEP:
 		if (ppsc->rfpwr_state == ERFOFF)
 			break;
@@ -2106,21 +2706,19 @@ static bool _rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
 				continue;
 			} else {
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "eRf Off/Sleep: %d times "
-					  "TcbBusyQueue[%d] =%d before "
-					  "doze!\n", (i + 1), queue_id,
-					  skb_queue_len(&ring->queue));
+					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					 (i + 1), queue_id,
+					 skb_queue_len(&ring->queue));
 
 				udelay(10);
 				i++;
 			}
 			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
 				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
-					 "\n ERFSLEEP: %d times "
-					  "TcbBusyQueue[%d] = %d !\n",
-					  MAX_DOZE_WAITING_TIMES_9x,
-					  queue_id,
-					  skb_queue_len(&ring->queue));
+					 "ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+					 MAX_DOZE_WAITING_TIMES_9x,
+					 queue_id,
+					 skb_queue_len(&ring->queue));
 				break;
 			}
 		}
@@ -2131,8 +2729,9 @@ static bool _rtl8723be_phy_set_rf_power_state(struct ieee80211_hw *hw,
 		ppsc->last_sleep_jiffies = jiffies;
 		_rtl8723be_phy_set_rf_sleep(hw);
 		break;
+
 	default:
-		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
 			 "switch case not process\n");
 		bresult = false;
 		break;
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/phy.h b/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
index 444ef95bb6af..6339738a0e33 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/phy.h
@@ -26,22 +26,28 @@
 #ifndef __RTL8723BE_PHY_H__
 #define __RTL8723BE_PHY_H__
 
-/*It must always set to 4, otherwise read efuse table secquence will be wrong.*/
+/* MAX_TX_COUNT must always set to 4, otherwise read efuse table sequence
+ * will be wrong.
+ */
 #define MAX_TX_COUNT		4
 #define	TX_1S			0
 #define	TX_2S			1
+#define	TX_3S			2
+#define	TX_4S			3
 
 #define	MAX_POWER_INDEX		0x3F
 
 #define MAX_PRECMD_CNT			16
 #define MAX_RFDEPENDCMD_CNT		16
-#define MAX_POSTCMD_CNT		16
+#define MAX_POSTCMD_CNT			16
 
 #define MAX_DOZE_WAITING_TIMES_9x	64
 
 #define RT_CANNOT_IO(hw)		false
 #define HIGHPOWER_RADIOA_ARRAYLEN	22
 
+#define TARGET_CHNL_NUM_2G_5G		59
+
 #define IQK_ADDA_REG_NUM		16
 #define IQK_BB_REG_NUM			9
 #define MAX_TOLERANCE			5
@@ -83,104 +89,19 @@
 
 #define RTL92C_MAX_PATH_NUM			2
 
-enum hw90_block_e {
-	HW90_BLOCK_MAC = 0,
-	HW90_BLOCK_PHY0 = 1,
-	HW90_BLOCK_PHY1 = 2,
-	HW90_BLOCK_RF = 3,
-	HW90_BLOCK_MAXIMUM = 4,
-};
-
 enum baseband_config_type {
 	BASEBAND_CONFIG_PHY_REG = 0,
 	BASEBAND_CONFIG_AGC_TAB = 1,
 };
 
-enum ra_offset_area {
-	RA_OFFSET_LEGACY_OFDM1,
-	RA_OFFSET_LEGACY_OFDM2,
-	RA_OFFSET_HT_OFDM1,
-	RA_OFFSET_HT_OFDM2,
-	RA_OFFSET_HT_OFDM3,
-	RA_OFFSET_HT_OFDM4,
-	RA_OFFSET_HT_CCK,
-};
-
-enum antenna_path {
-	ANTENNA_NONE,
-	ANTENNA_D,
-	ANTENNA_C,
-	ANTENNA_CD,
-	ANTENNA_B,
-	ANTENNA_BD,
-	ANTENNA_BC,
-	ANTENNA_BCD,
-	ANTENNA_A,
-	ANTENNA_AD,
-	ANTENNA_AC,
-	ANTENNA_ACD,
-	ANTENNA_AB,
-	ANTENNA_ABD,
-	ANTENNA_ABC,
-	ANTENNA_ABCD
-};
-
-struct r_antenna_select_ofdm {
-	u32 r_tx_antenna:4;
-	u32 r_ant_l:4;
-	u32 r_ant_non_ht:4;
-	u32 r_ant_ht1:4;
-	u32 r_ant_ht2:4;
-	u32 r_ant_ht_s1:4;
-	u32 r_ant_non_ht_s1:4;
-	u32 ofdm_txsc:2;
-	u32 reserved:2;
-};
-
-struct r_antenna_select_cck {
-	u8 r_cckrx_enable_2:2;
-	u8 r_cckrx_enable:2;
-	u8 r_ccktx_enable:4;
-};
-
-
-struct efuse_contents {
-	u8 mac_addr[ETH_ALEN];
-	u8 cck_tx_power_idx[6];
-	u8 ht40_1s_tx_power_idx[6];
-	u8 ht40_2s_tx_power_idx_diff[3];
-	u8 ht20_tx_power_idx_diff[3];
-	u8 ofdm_tx_power_idx_diff[3];
-	u8 ht40_max_power_offset[3];
-	u8 ht20_max_power_offset[3];
-	u8 channel_plan;
-	u8 thermal_meter;
-	u8 rf_option[5];
-	u8 version;
-	u8 oem_id;
-	u8 regulatory;
-};
-
-struct tx_power_struct {
-	u8 cck[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 ht40_1s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 ht40_2s[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 ht20_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 legacy_ht_diff[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 legacy_ht_txpowerdiff;
-	u8 groupht20[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 groupht40[RTL92C_MAX_PATH_NUM][CHANNEL_MAX_NUMBER];
-	u8 pwrgroup_cnt;
-	u32 mcs_original_offset[4][16];
-};
-
-enum _ANT_DIV_TYPE {
-	NO_ANTDIV				= 0xFF,
-	CG_TRX_HW_ANTDIV		= 0x01,
-	CGCS_RX_HW_ANTDIV		= 0x02,
+enum ant_div_type {
+	NO_ANTDIV		= 0xFF,
+	CG_TRX_HW_ANTDIV	= 0x01,
+	CGCS_RX_HW_ANTDIV	= 0x02,
 	FIXED_HW_ANTDIV         = 0x03,
-	CG_TRX_SMART_ANTDIV		= 0x04,
-	CGCS_RX_SW_ANTDIV		= 0x05,
+	CG_TRX_SMART_ANTDIV	= 0x04,
+	CGCS_RX_SW_ANTDIV	= 0x05,
+
 };
 
 u32 rtl8723be_phy_query_rf_reg(struct ieee80211_hw *hw,
@@ -206,7 +127,6 @@ void rtl8723be_phy_sw_chnl_callback(struct ieee80211_hw *hw);
 u8 rtl8723be_phy_sw_chnl(struct ieee80211_hw *hw);
 void rtl8723be_phy_iq_calibrate(struct ieee80211_hw *hw,
 				bool b_recovery);
-void rtl23b_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
 void rtl8723be_phy_lc_calibrate(struct ieee80211_hw *hw);
 void rtl8723be_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
 bool rtl8723be_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.c
index b5167e73fecf..a1bb1f6116fb 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.c
@@ -23,7 +23,7 @@
  *
  *****************************************************************************/
 
-#include "pwrseqcmd.h"
+#include "../pwrseqcmd.h"
 #include "pwrseq.h"
 
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.h
index a62f43ed8d32..0fee5e0e55c2 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseq.h
@@ -26,7 +26,9 @@
 #ifndef __RTL8723BE_PWRSEQ_H__
 #define __RTL8723BE_PWRSEQ_H__
 
-/*	Check document WM-20130425-JackieLau-RTL8723B_Power_Architecture v05.vsd
+#include "../pwrseqcmd.h"
+/**
+ *	Check document WM-20130425-JackieLau-RTL8723B_Power_Architecture v05.vsd
  *	There are 6 HW Power States:
  *	0: POFF--Power Off
  *	1: PDN--Power Down
@@ -35,7 +37,7 @@
  *	4: LPS--Low Power State
  *	5: SUS--Suspend
  *
- *	The transition from different states are defined below
+ *	The transision from different states are defined below
  *	TRANS_CARDEMU_TO_ACT
  *	TRANS_ACT_TO_CARDEMU
  *	TRANS_CARDEMU_TO_SUS
@@ -57,203 +59,320 @@
 #define	RTL8723B_TRANS_END_STEPS		1
 
 #define RTL8723B_TRANS_CARDEMU_TO_ACT					\
+	/* format */							\
+	/* comments here */						\
+	/* {offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value}, */\
+	/*0x20[0] = 1b'1 enable LDOA12 MACRO block for all interface*/  \
 	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK,				\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/*0x67[0] = 0 to disable BT_GPS_SEL pins*/			\
 	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK,				\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	/*Delay 1ms*/							\
 	{0x0001, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK,				\
 	 PWR_BASEADDR_MAC, PWR_CMD_DELAY, 1, PWRSEQ_DELAY_MS},		\
+	/*0x00[5] = 1b'0 release analog Ips to digital ,1:isolation*/   \
 	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK,				\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), 0},			\
+	/* disable SW LPS 0x04[10]=0 and WLSUS_EN 0x04[11]=0*/		\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT(4)|BIT(3)|BIT(2)), 0},	\
+	/* Disable USB suspend */					\
 	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0) , 0},			\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0) , BIT(0)},		\
+	/* wait till 0x04[17] = 1    power ready*/			\
 	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	/* Enable USB suspend */					\
 	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
-	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0) , BIT(0)},		\
+	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0) , 0},			\
+	/* release WLON reset  0x04[16]=1*/				\
 	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/* disable HWPDN 0x04[15]=0*/					\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},			\
+	/* disable WL suspend*/						\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT(4)|BIT(3)), 0},		\
+	/* polling until return 0*/					\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(0), 0},			\
+	/* Enable WL control XTAL setting*/				\
 	{0x0010, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6), BIT(6)},		\
+	/*Enable falling edge triggering interrupt*/			\
 	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	/*Enable GPIO9 interrupt mode*/					\
 	{0x0063, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	/*Enable GPIO9 input mode*/					\
 	{0x0062, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	/*Enable HSISR GPIO[C:0] interrupt*/				\
 	{0x0058, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/*Enable HSISR GPIO9 interrupt*/				\
 	{0x005A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	/*For GPIO9 internal pull high setting by test chip*/		\
 	{0x0068, PWR_CUT_TESTCHIP_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3), BIT(3)},		\
+	/*For GPIO9 internal pull high setting*/			\
 	{0x0069, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6), BIT(6)},
 
 #define RTL8723B_TRANS_ACT_TO_CARDEMU					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/*0x1F[7:0] = 0 turn off RF*/					\
 	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},			\
+	/*0x4C[24] = 0x4F[0] = 0, */					\
+	/*switch DPDT_SEL_P output from register 0x65[2] */		\
 	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	/*Enable rising edge triggering interrupt*/			\
 	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	 /*0x04[9] = 1 turn off MAC by HW state machine*/		\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	 /*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(1), 0},			\
+	/* Enable BT control XTAL setting*/				\
 	{0x0010, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6), 0},			\
+	/*0x00[5] = 1b'1 analog Ips to digital ,1:isolation*/		\
 	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,	\
 	 PWR_CMD_WRITE, BIT(5), BIT(5)},				\
+	/*0x20[0] = 1b'0 disable LDOA12 MACRO block*/			\
 	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,	\
 	 PWR_CMD_WRITE, BIT(0), 0},
 
 #define RTL8723B_TRANS_CARDEMU_TO_SUS					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },*/\
+	/*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/		\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4) | BIT(3), (BIT(4) | BIT(3))}, \
+	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,	\
 	 PWR_CMD_WRITE, BIT(3) | BIT(4), BIT(3)},			\
+	/*0x23[4] = 1b'1 12H LDO enter sleep mode*/			\
 	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	/*0x07[7:0] = 0x20 SDIO SOP option to disable BG/MB/ACK/SWR*/   \
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20},			\
+	/*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/		\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(4), BIT(3) | BIT(4)},\
+	/*Set SDIO suspend local register*/				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/*wait power state to suspend*/					\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0},
 
 #define RTL8723B_TRANS_SUS_TO_CARDEMU					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/*clear suspend enable and power down enable*/			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 0},		\
+	/*Set SDIO suspend local register*/				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},			\
+	/*wait power state to suspend*/					\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	/*0x23[4] = 1b'0 12H LDO enter normal mode*/			\
 	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	/*0x04[12:11] = 2b'01enable WL suspend*/			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},
 
 #define RTL8723B_TRANS_CARDEMU_TO_CARDDIS				\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/*0x07=0x20 , SOP option to disable BG/MB*/			\
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20},			\
+	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_USB_MSK | PWR_INTF_SDIO_MSK,				\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), BIT(3)},	\
+	/*0x04[10] = 1, enable SW LPS*/					\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(2), BIT(2)},		\
+	/*0x48[16] = 1 to enable GPIO9 as EXT WAKEUP*/			\
 	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 1},			\
+	/*0x23[4] = 1b'1 12H LDO enter sleep mode*/			\
 	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	/*Set SDIO suspend local register*/				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), BIT(0)},		\
+	/*wait power state to suspend*/					\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), 0},
 
 #define RTL8723B_TRANS_CARDDIS_TO_CARDEMU				\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/*clear suspend enable and power down enable*/			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3) | BIT(7), 0},		\
+	/*Set SDIO suspend local register*/				\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT(0), 0},			\
+	/*wait power state to suspend*/					\
 	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT(1), BIT(1)},		\
+	/*0x48[16] = 0 to disable GPIO9 as EXT WAKEUP*/			\
 	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	/*0x04[12:11] = 2b'01enable WL suspend*/			\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(3)|BIT(4), 0},		\
+	/*0x23[4] = 1b'0 12H LDO enter normal mode*/			\
 	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	/*PCIe DMA start*/						\
 	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},
 
 #define RTL8723B_TRANS_CARDEMU_TO_PDN					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/*0x23[4] = 1b'1 12H LDO enter sleep mode*/			\
 	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), BIT(4)},		\
+	/*0x07[7:0] = 0x20 SOP option to disable BG/MB/ACK/SWR*/	\
 	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
 	 PWR_INTF_SDIO_MSK | PWR_INTF_USB_MSK, PWR_BASEADDR_MAC,	\
 	 PWR_CMD_WRITE, 0xFF, 0x20},					\
+	/* 0x04[16] = 0*/						\
 	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	/* 0x04[15] = 1*/						\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), BIT(7)},
 
 #define RTL8723B_TRANS_PDN_TO_CARDEMU					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/* 0x04[15] = 0*/						\
 	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(7), 0},
 
 #define RTL8723B_TRANS_ACT_TO_LPS					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/*PCIe DMA stop*/						\
 	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	/*Tx Pause*/							\
 	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	/*Should be zero if no packet is transmitting*/			\
 	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
 	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
 	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
 	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*CCK and OFDM are disabled,and clock are gated*/		\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(0), 0},			\
+	/*Delay 1us*/							\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},		\
+	/*Whole BB is reset*/						\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	/*Reset MAC TRX*/						\
 	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x03},			\
+	/*check if removed later*/					\
 	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), 0},			\
+	/*When driver enter Sus/ Disable, enable LOP for BT*/		\
 	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00},			\
+	/*Respond TxOK to scheduler*/					\
 	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(5), BIT(5)},
 
 #define RTL8723B_TRANS_LPS_TO_ACT					\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
+	/*SDIO RPWM*/							\
 	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
-	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84},		\
+	 PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84},			\
+	/*USB RPWM*/							\
 	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84},			\
+	/*PCIe RPWM*/							\
 	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84},			\
+	/*Delay*/							\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS},		\
+	/*.	0x08[4] = 0		 switch TSF to 40M*/		\
 	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(4), 0},			\
+	/*Polling 0x109[7]=0  TSF in 40M*/				\
 	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
-	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0},		\
+	 PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT(7), 0},			\
+	/*.	0x29[7:6] = 2b'00	 enable BB clock*/		\
 	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(6)|BIT(7), 0},		\
+	/*.	0x101[1] = 1*/						\
 	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1), BIT(1)},		\
+	/*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/		\
 	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	/*.	0x02[1:0] = 2b'11	 enable BB macro*/		\
 	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT(1) | BIT(0), BIT(1) | BIT(0)}, \
+	/*.	0x522 = 0*/						\
 	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
 	 PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},
 
 #define RTL8723B_TRANS_END						\
+	/* format */							\
+	/* comments here */						\
+	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, */\
 	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, 0,	\
 	 PWR_CMD_END, 0, 0},
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.c b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.c
deleted file mode 100644
index 4573310c707f..000000000000
--- a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2014  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "pwrseq.h"
-
-/*	Description:
- *		This routine deal with the Power Configuration CMDs
- *		 parsing for RTL8723/RTL8188E Series IC.
- *	Assumption:
- *		We should follow specific format which was released from HW SD.
- *
- *	2011.07.07, added by Roger.
- */
-bool rtlbe_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-				 u8 fab_version, u8 interface_type,
-				 struct wlan_pwr_cfg pwrcfgcmd[])
-
-{
-	struct wlan_pwr_cfg pwr_cfg_cmd = {0};
-	bool b_polling_bit = false;
-	u32 ary_idx = 0;
-	u8 value = 0;
-	u32 offset = 0;
-	u32 polling_count = 0;
-	u32 max_polling_cnt = 5000;
-
-	do {
-		pwr_cfg_cmd = pwrcfgcmd[ary_idx];
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-			 "rtlbe_hal_pwrseqcmdparsing(): "
-			 "offset(%#x),cut_msk(%#x), fab_msk(%#x),"
-			 "interface_msk(%#x), base(%#x), "
-			 "cmd(%#x), msk(%#x), value(%#x)\n",
-			 GET_PWR_CFG_OFFSET(pwr_cfg_cmd),
-			 GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd),
-			 GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd),
-			 GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd),
-			 GET_PWR_CFG_BASE(pwr_cfg_cmd),
-			 GET_PWR_CFG_CMD(pwr_cfg_cmd),
-			 GET_PWR_CFG_MASK(pwr_cfg_cmd),
-			 GET_PWR_CFG_VALUE(pwr_cfg_cmd));
-
-		if ((GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd)&fab_version) &&
-		    (GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd)&cut_version) &&
-		    (GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd)&interface_type)) {
-			switch (GET_PWR_CFG_CMD(pwr_cfg_cmd)) {
-			case PWR_CMD_READ:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtlbe_hal_pwrseqcmdparsing(): "
-					  "PWR_CMD_READ\n");
-				break;
-			case PWR_CMD_WRITE:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtlbe_hal_pwrseqcmdparsing(): "
-					  "PWR_CMD_WRITE\n");
-				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
-
-				/*Read the value from system register*/
-				value = rtl_read_byte(rtlpriv, offset);
-				value &= (~(GET_PWR_CFG_MASK(pwr_cfg_cmd)));
-				value = value | (GET_PWR_CFG_VALUE(pwr_cfg_cmd)
-					& GET_PWR_CFG_MASK(pwr_cfg_cmd));
-
-				/*Write the value back to sytem register*/
-				rtl_write_byte(rtlpriv, offset, value);
-				break;
-			case PWR_CMD_POLLING:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtlbe_hal_pwrseqcmdparsing(): "
-					  "PWR_CMD_POLLING\n");
-				b_polling_bit = false;
-				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
-
-				do {
-					value = rtl_read_byte(rtlpriv, offset);
-
-					value &= GET_PWR_CFG_MASK(pwr_cfg_cmd);
-					if (value ==
-					    (GET_PWR_CFG_VALUE(pwr_cfg_cmd) &
-					     GET_PWR_CFG_MASK(pwr_cfg_cmd)))
-						b_polling_bit = true;
-					else
-						udelay(10);
-
-					if (polling_count++ > max_polling_cnt)
-						return false;
-
-				} while (!b_polling_bit);
-				break;
-			case PWR_CMD_DELAY:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtlbe_hal_pwrseqcmdparsing(): "
-					 "PWR_CMD_DELAY\n");
-				if (GET_PWR_CFG_VALUE(pwr_cfg_cmd) ==
-				    PWRSEQ_DELAY_US)
-					udelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
-				else
-					mdelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
-				break;
-			case PWR_CMD_END:
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
-					 "rtlbe_hal_pwrseqcmdparsing(): "
-					 "PWR_CMD_END\n");
-				return true;
-			default:
-				RT_ASSERT(false,
-					  "rtlbe_hal_pwrseqcmdparsing(): "
-					  "Unknown CMD!!\n");
-				break;
-			}
-		}
-
-		ary_idx++;
-	} while (1);
-
-	return true;
-}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.h b/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.h
deleted file mode 100644
index ce14a3b5cb71..000000000000
--- a/drivers/net/wireless/rtlwifi/rtl8723be/pwrseqcmd.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2014  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8723BE_PWRSEQCMD_H__
-#define __RTL8723BE_PWRSEQCMD_H__
-
-#include "../wifi.h"
-/*---------------------------------------------*/
-/*The value of cmd: 4 bits */
-/*---------------------------------------------*/
-#define PWR_CMD_READ		0x00
-#define PWR_CMD_WRITE		0x01
-#define PWR_CMD_POLLING		0x02
-#define PWR_CMD_DELAY		0x03
-#define PWR_CMD_END		0x04
-
-/* define the base address of each block */
-#define PWR_BASEADDR_MAC	0x00
-#define PWR_BASEADDR_USB	0x01
-#define PWR_BASEADDR_PCIE	0x02
-#define PWR_BASEADDR_SDIO	0x03
-
-#define	PWR_INTF_SDIO_MSK	BIT(0)
-#define	PWR_INTF_USB_MSK	BIT(1)
-#define	PWR_INTF_PCI_MSK	BIT(2)
-#define	PWR_INTF_ALL_MSK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
-
-#define	PWR_FAB_TSMC_MSK	BIT(0)
-#define	PWR_FAB_UMC_MSK		BIT(1)
-#define	PWR_FAB_ALL_MSK		(BIT(0) | BIT(1) | BIT(2) | BIT(3))
-
-#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
-#define	PWR_CUT_A_MSK		BIT(1)
-#define	PWR_CUT_B_MSK		BIT(2)
-#define	PWR_CUT_C_MSK		BIT(3)
-#define	PWR_CUT_D_MSK		BIT(4)
-#define	PWR_CUT_E_MSK		BIT(5)
-#define	PWR_CUT_F_MSK		BIT(6)
-#define	PWR_CUT_G_MSK		BIT(7)
-#define	PWR_CUT_ALL_MSK		0xFF
-
-
-enum pwrseq_delay_unit {
-	PWRSEQ_DELAY_US,
-	PWRSEQ_DELAY_MS,
-};
-
-struct wlan_pwr_cfg {
-	u16 offset;
-	u8 cut_msk;
-	u8 fab_msk:4;
-	u8 interface_msk:4;
-	u8 base:4;
-	u8 cmd:4;
-	u8 msk;
-	u8 value;
-
-};
-
-#define	GET_PWR_CFG_OFFSET(__PWR_CMD)	__PWR_CMD.offset
-#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)	__PWR_CMD.cut_msk
-#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)	__PWR_CMD.fab_msk
-#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
-#define	GET_PWR_CFG_BASE(__PWR_CMD)	__PWR_CMD.base
-#define	GET_PWR_CFG_CMD(__PWR_CMD)	__PWR_CMD.cmd
-#define	GET_PWR_CFG_MASK(__PWR_CMD)	__PWR_CMD.msk
-#define	GET_PWR_CFG_VALUE(__PWR_CMD)	__PWR_CMD.value
-
-bool rtlbe_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-				 u8 fab_version, u8 interface_type,
-				 struct wlan_pwr_cfg pwrcfgcmd[]);
-
-#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/reg.h b/drivers/net/wireless/rtlwifi/rtl8723be/reg.h
index 3006849ed439..03581d2a5da0 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/reg.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/reg.h
@@ -78,11 +78,11 @@
 #define REG_WOL_EVENT				0x0081
 #define REG_MCUTSTCFG				0x0084
 
-
 #define REG_HIMR				0x00B0
 #define REG_HISR				0x00B4
 #define REG_HIMRE				0x00B8
 #define REG_HISRE				0x00BC
+#define REG_PMC_DBG_CTRL2			0x00CC
 
 #define REG_EFUSE_ACCESS			0x00CF
 
@@ -95,7 +95,8 @@
 #define REG_HPON_FSM				0x00EC
 #define REG_SYS_CFG				0x00F0
 #define REG_GPIO_OUTSTS				0x00F4
-#define REG_SYS_CFG1				0x00F0
+#define REG_MAC_PHY_CTRL_NORMAL			0x00F8
+#define REG_SYS_CFG1				0x00FC
 #define REG_ROM_VERSION				0x00FD
 
 #define REG_CR					0x0100
@@ -170,8 +171,14 @@
 #define	REG_BKQ_DESA				0x0338
 #define	REG_RX_DESA				0x0340
 
-#define	REG_DBI					0x0348
-#define	REG_MDIO				0x0354
+#define	REG_DBI_WDATA				0x0348
+#define	REG_DBI_RDATA				0x034C
+#define	REG_DBI_CTRL				0x0350
+#define	REG_DBI_ADDR				0x0350
+#define	REG_DBI_FLAG				0x0352
+#define	REG_MDIO_WDATA				0x0354
+#define	REG_MDIO_RDATA				0x0356
+#define	REG_MDIO_CTL				0x0358
 #define	REG_DBG_SEL				0x0360
 #define	REG_PCIE_HRPWM				0x0361
 #define	REG_PCIE_HCPWM				0x0363
@@ -180,7 +187,6 @@
 #define	REG_UART_TX_DESA			0x0370
 #define	REG_UART_RX_DESA			0x0378
 
-
 #define	REG_HDAQ_DESA_NODEF			0x0000
 #define	REG_CMDQ_DESA_NODEF			0x0000
 
@@ -193,7 +199,6 @@
 #define REG_BCNQ_INFORMATION			0x0418
 #define REG_TXPKT_EMPTY				0x041A
 
-
 #define REG_CPU_MGQ_INFORMATION			0x041C
 #define REG_FWHW_TXQ_CTRL			0x0420
 #define REG_HWSEQ_CTRL				0x0423
@@ -207,9 +212,7 @@
 #define REG_RARFRC				0x0438
 #define REG_RRSR				0x0440
 #define REG_ARFR0				0x0444
-#define REG_ARFR1				0x0448
-#define REG_ARFR2				0x044C
-#define REG_ARFR3				0x0450
+#define REG_ARFR1				0x044C
 #define REG_AMPDU_MAX_TIME			0x0456
 #define REG_AGGLEN_LMT				0x0458
 #define REG_AMPDU_MIN_SPACE			0x045C
@@ -223,7 +226,10 @@
 #define REG_POWER_STAGE2			0x04B8
 #define REG_PKT_LIFE_TIME			0x04C0
 #define REG_STBC_SETTING			0x04C4
+#define REG_HT_SINGLE_AMPDU			0x04C7
+
 #define REG_PROT_MODE_CTRL			0x04C8
+#define REG_MAX_AGGR_NUM			0x04CA
 #define REG_BAR_MODE_CTRL			0x04CC
 #define REG_RA_TRY_RATE_AGG_LMT			0x04CF
 #define REG_EARLY_MODE_CONTROL			0x04D0
@@ -303,6 +309,7 @@
 #define REG_EIFS				0x0642
 
 #define REG_NAV_CTRL				0x0650
+#define	REG_NAV_UPPER				0x0652
 #define REG_BACAMCMD				0x0654
 #define REG_BACAMCONTENT			0x0658
 #define REG_LBDLY				0x0660
@@ -355,43 +362,43 @@
 #define REG_NORMAL_SIE_MAC_ADDR			0xFE70
 #define REG_NORMAL_SIE_STRING			0xFE80
 
-#define	CR9346					REG_9346CR
-#define	MSR					(REG_CR + 2)
-#define	ISR					REG_HISR
-#define	TSFR					REG_TSFTR
+#define	CR9346				REG_9346CR
+#define	MSR				(REG_CR + 2)
+#define	ISR				REG_HISR
+#define	TSFR				REG_TSFTR
 
-#define	MACIDR0					REG_MACID
-#define	MACIDR4					(REG_MACID + 4)
+#define	MACIDR0				REG_MACID
+#define	MACIDR4				(REG_MACID + 4)
 
-#define PBP					REG_PBP
+#define PBP				REG_PBP
 
-#define	IDR0					MACIDR0
-#define	IDR4					MACIDR4
+#define	IDR0				MACIDR0
+#define	IDR4				MACIDR4
 
-#define	UNUSED_REGISTER				0x1BF
-#define	DCAM					UNUSED_REGISTER
-#define	PSR					UNUSED_REGISTER
-#define BBADDR					UNUSED_REGISTER
-#define	PHYDATAR				UNUSED_REGISTER
+#define	UNUSED_REGISTER			0x1BF
+#define	DCAM				UNUSED_REGISTER
+#define	PSR				UNUSED_REGISTER
+#define BBADDR				UNUSED_REGISTER
+#define	PHYDATAR			UNUSED_REGISTER
 
-#define	INVALID_BBRF_VALUE			0x12345678
+#define	INVALID_BBRF_VALUE		0x12345678
 
-#define	MAX_MSS_DENSITY_2T			0x13
-#define	MAX_MSS_DENSITY_1T			0x0A
+#define	MAX_MSS_DENSITY_2T		0x13
+#define	MAX_MSS_DENSITY_1T		0x0A
 
-#define	CMDEEPROM_EN				BIT(5)
-#define	CMDEEPROM_SEL				BIT(4)
-#define	CMD9346CR_9356SEL			BIT(4)
-#define	AUTOLOAD_EEPROM				(CMDEEPROM_EN | CMDEEPROM_SEL)
-#define	AUTOLOAD_EFUSE				CMDEEPROM_EN
+#define	CMDEEPROM_EN			BIT(5)
+#define	CMDEEPROM_SEL			BIT(4)
+#define	CMD9346CR_9356SEL		BIT(4)
+#define	AUTOLOAD_EEPROM			(CMDEEPROM_EN | CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE			CMDEEPROM_EN
 
-#define	GPIOSEL_GPIO				0
-#define	GPIOSEL_ENBT				BIT(5)
+#define	GPIOSEL_GPIO			0
+#define	GPIOSEL_ENBT			BIT(5)
 
-#define	GPIO_IN					REG_GPIO_PIN_CTRL
-#define	GPIO_OUT				(REG_GPIO_PIN_CTRL + 1)
-#define	GPIO_IO_SEL				(REG_GPIO_PIN_CTRL + 2)
-#define	GPIO_MOD				(REG_GPIO_PIN_CTRL + 3)
+#define	GPIO_IN				REG_GPIO_PIN_CTRL
+#define	GPIO_OUT			(REG_GPIO_PIN_CTRL + 1)
+#define	GPIO_IO_SEL			(REG_GPIO_PIN_CTRL + 2)
+#define	GPIO_MOD			(REG_GPIO_PIN_CTRL + 3)
 
 /* 8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
 #define	HSIMR_GPIO12_0_INT_EN			BIT(0)
@@ -400,8 +407,7 @@
 #define	HSIMR_PDN_INT_EN			BIT(7)
 #define	HSIMR_GPIO9_INT_EN			BIT(25)
 
-/* 8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
-
+/*  8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
 #define	HSISR_GPIO12_0_INT			BIT(0)
 #define	HSISR_SPS_OCP_INT			BIT(5)
 #define	HSISR_RON_INT_EN			BIT(6)
@@ -412,7 +418,6 @@
 #define	MSR_ADHOC				0x01
 #define	MSR_INFRA				0x02
 #define	MSR_AP					0x03
-#define	MSR_MASK				0x03
 
 #define	RRSR_RSC_OFFSET				21
 #define	RRSR_SHORT_OFFSET			23
@@ -542,7 +547,8 @@
 
 /*********************************************
 *       8723BE IMR/ISR bits
-**********************************************/
+*********************************************
+*/
 #define	IMR_DISABLED				0x0
 /* IMR DW0(0x0060-0063) Bit 0-31 */
 #define	IMR_TXCCK		BIT(30)	/* TXRPT interrupt when
@@ -644,7 +650,7 @@
 #define RF_OPTION1				0x79
 #define RF_OPTION2				0x7A
 #define RF_OPTION3				0x7B
-#define RF_OPTION4				0xC3
+#define EEPROM_RF_BT_SETTING_8723B		0xC3
 
 #define EEPROM_DEFAULT_PID			0x1234
 #define EEPROM_DEFAULT_VID			0x5678
@@ -678,14 +684,11 @@
 #define EEPROM_CLK				0x06
 #define EEPROM_TESTR				0x08
 
-
 #define EEPROM_TXPOWERCCK			0x10
 #define	EEPROM_TXPOWERHT40_1S			0x16
 #define EEPROM_TXPOWERHT20DIFF			0x1B
 #define EEPROM_TXPOWER_OFDMDIFF			0x1B
 
-
-
 #define	EEPROM_TX_PWR_INX			0x10
 
 #define	EEPROM_CHANNELPLAN			0xB8
@@ -1198,7 +1201,7 @@
 #define APP_MIC					BIT(30)
 #define APP_FCS					BIT(31)
 
-#define _MIN_SPACE(x)			((x) & 0x7)
+#define _MIN_SPACE(x)				((x) & 0x7)
 #define _SHORT_GI_PADDING(x)		(((x) & 0x1F) << 3)
 
 #define RXERR_TYPE_OFDM_PPDU		0
@@ -1216,105 +1219,105 @@
 #define RXERR_TYPE_HT_MPDU_FAIL		12
 #define RXERR_TYPE_RX_FULL_DROP		15
 
-#define RXERR_COUNTER_MASK		0xFFFFF
-#define RXERR_RPT_RST			BIT(27)
-#define _RXERR_RPT_SEL(type)		((type) << 28)
-
-#define	SCR_TXUSEDK			BIT(0)
-#define	SCR_RXUSEDK			BIT(1)
-#define	SCR_TXENCENABLE			BIT(2)
-#define	SCR_RXDECENABLE			BIT(3)
-#define	SCR_SKBYA2			BIT(4)
-#define	SCR_NOSKMC			BIT(5)
-#define SCR_TXBCUSEDK			BIT(6)
-#define SCR_RXBCUSEDK			BIT(7)
-
-#define XCLK_VLD			BIT(0)
-#define ACLK_VLD			BIT(1)
-#define UCLK_VLD			BIT(2)
-#define PCLK_VLD			BIT(3)
-#define PCIRSTB				BIT(4)
-#define V15_VLD				BIT(5)
-#define TRP_B15V_EN			BIT(7)
-#define SIC_IDLE			BIT(8)
-#define BD_MAC2				BIT(9)
-#define BD_MAC1				BIT(10)
-#define IC_MACPHY_MODE			BIT(11)
-#define BT_FUNC				BIT(16)
-#define VENDOR_ID			BIT(19)
-#define PAD_HWPD_IDN			BIT(22)
-#define TRP_VAUX_EN			BIT(23)
-#define TRP_BT_EN			BIT(24)
-#define BD_PKG_SEL			BIT(25)
-#define BD_HCI_SEL			BIT(26)
-#define TYPE_ID				BIT(27)
-
-#define USB_IS_HIGH_SPEED		0
-#define USB_IS_FULL_SPEED		1
-#define USB_SPEED_MASK			BIT(5)
-
-#define USB_NORMAL_SIE_EP_MASK		0xF
-#define USB_NORMAL_SIE_EP_SHIFT		4
-
-#define USB_TEST_EP_MASK		0x30
-#define USB_TEST_EP_SHIFT		4
-
-#define USB_AGG_EN			BIT(3)
-
-#define MAC_ADDR_LEN			6
-#define LAST_ENTRY_OF_TX_PKT_BUFFER	175/*255    88e*/
-
-#define POLLING_LLT_THRESHOLD		20
-#define POLLING_READY_TIMEOUT_COUNT	3000
+#define RXERR_COUNTER_MASK			0xFFFFF
+#define RXERR_RPT_RST				BIT(27)
+#define _RXERR_RPT_SEL(type)			((type) << 28)
 
-#define	MAX_MSS_DENSITY_2T		0x13
-#define	MAX_MSS_DENSITY_1T		0x0A
+#define	SCR_TXUSEDK				BIT(0)
+#define	SCR_RXUSEDK				BIT(1)
+#define	SCR_TXENCENABLE				BIT(2)
+#define	SCR_RXDECENABLE				BIT(3)
+#define	SCR_SKBYA2				BIT(4)
+#define	SCR_NOSKMC				BIT(5)
+#define SCR_TXBCUSEDK				BIT(6)
+#define SCR_RXBCUSEDK				BIT(7)
+
+#define XCLK_VLD				BIT(0)
+#define ACLK_VLD				BIT(1)
+#define UCLK_VLD				BIT(2)
+#define PCLK_VLD				BIT(3)
+#define PCIRSTB					BIT(4)
+#define V15_VLD					BIT(5)
+#define TRP_B15V_EN				BIT(7)
+#define SIC_IDLE				BIT(8)
+#define BD_MAC2					BIT(9)
+#define BD_MAC1					BIT(10)
+#define IC_MACPHY_MODE				BIT(11)
+#define BT_FUNC					BIT(16)
+#define VENDOR_ID				BIT(19)
+#define PAD_HWPD_IDN				BIT(22)
+#define TRP_VAUX_EN				BIT(23)
+#define TRP_BT_EN				BIT(24)
+#define BD_PKG_SEL				BIT(25)
+#define BD_HCI_SEL				BIT(26)
+#define TYPE_ID					BIT(27)
+
+#define USB_IS_HIGH_SPEED			0
+#define USB_IS_FULL_SPEED			1
+#define USB_SPEED_MASK				BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK			0xF
+#define USB_NORMAL_SIE_EP_SHIFT			4
+
+#define USB_TEST_EP_MASK			0x30
+#define USB_TEST_EP_SHIFT			4
+
+#define USB_AGG_EN				BIT(3)
+
+#define MAC_ADDR_LEN				6
+#define LAST_ENTRY_OF_TX_PKT_BUFFER		175/*255    88e*/
+
+#define POLLING_LLT_THRESHOLD			20
+#define POLLING_READY_TIMEOUT_COUNT		3000
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
 
 #define EPROM_CMD_OPERATING_MODE_MASK	((1<<7)|(1<<6))
-#define EPROM_CMD_CONFIG		0x3
-#define EPROM_CMD_LOAD			1
-
-#define	HWSET_MAX_SIZE_92S		HWSET_MAX_SIZE
-
-#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT(2)
-
-#define	RPMAC_RESET			0x100
-#define	RPMAC_TXSTART			0x104
-#define	RPMAC_TXLEGACYSIG		0x108
-#define	RPMAC_TXHTSIG1			0x10c
-#define	RPMAC_TXHTSIG2			0x110
-#define	RPMAC_PHYDEBUG			0x114
-#define	RPMAC_TXPACKETNUM		0x118
-#define	RPMAC_TXIDLE			0x11c
-#define	RPMAC_TXMACHEADER0		0x120
-#define	RPMAC_TXMACHEADER1		0x124
-#define	RPMAC_TXMACHEADER2		0x128
-#define	RPMAC_TXMACHEADER3		0x12c
-#define	RPMAC_TXMACHEADER4		0x130
-#define	RPMAC_TXMACHEADER5		0x134
-#define	RPMAC_TXDADATYPE		0x138
-#define	RPMAC_TXRANDOMSEED		0x13c
-#define	RPMAC_CCKPLCPPREAMBLE		0x140
-#define	RPMAC_CCKPLCPHEADER		0x144
-#define	RPMAC_CCKCRC16			0x148
-#define	RPMAC_OFDMRXCRC32OK		0x170
-#define	RPMAC_OFDMRXCRC32ER		0x174
-#define	RPMAC_OFDMRXPARITYER		0x178
-#define	RPMAC_OFDMRXCRC8ER		0x17c
-#define	RPMAC_CCKCRXRC16ER		0x180
-#define	RPMAC_CCKCRXRC32ER		0x184
-#define	RPMAC_CCKCRXRC32OK		0x188
-#define	RPMAC_TXSTATUS			0x18c
-
-#define	RFPGA0_RFMOD			0x800
-
-#define	RFPGA0_TXINFO			0x804
-#define	RFPGA0_PSDFUNCTION		0x808
-
-#define	RFPGA0_TXGAINSTAGE		0x80c
-
-#define	RFPGA0_RFTIMING1		0x810
-#define	RFPGA0_RFTIMING2		0x814
+#define EPROM_CMD_CONFIG			0x3
+#define EPROM_CMD_LOAD				1
+
+#define	HWSET_MAX_SIZE_92S			HWSET_MAX_SIZE
+
+#define	HAL_8192C_HW_GPIO_WPS_BIT		BIT(2)
+
+#define	RPMAC_RESET				0x100
+#define	RPMAC_TXSTART				0x104
+#define	RPMAC_TXLEGACYSIG			0x108
+#define	RPMAC_TXHTSIG1				0x10c
+#define	RPMAC_TXHTSIG2				0x110
+#define	RPMAC_PHYDEBUG				0x114
+#define	RPMAC_TXPACKETNUM			0x118
+#define	RPMAC_TXIDLE				0x11c
+#define	RPMAC_TXMACHEADER0			0x120
+#define	RPMAC_TXMACHEADER1			0x124
+#define	RPMAC_TXMACHEADER2			0x128
+#define	RPMAC_TXMACHEADER3			0x12c
+#define	RPMAC_TXMACHEADER4			0x130
+#define	RPMAC_TXMACHEADER5			0x134
+#define	RPMAC_TXDADATYPE			0x138
+#define	RPMAC_TXRANDOMSEED			0x13c
+#define	RPMAC_CCKPLCPPREAMBLE			0x140
+#define	RPMAC_CCKPLCPHEADER			0x144
+#define	RPMAC_CCKCRC16				0x148
+#define	RPMAC_OFDMRXCRC32OK			0x170
+#define	RPMAC_OFDMRXCRC32ER			0x174
+#define	RPMAC_OFDMRXPARITYER			0x178
+#define	RPMAC_OFDMRXCRC8ER			0x17c
+#define	RPMAC_CCKCRXRC16ER			0x180
+#define	RPMAC_CCKCRXRC32ER			0x184
+#define	RPMAC_CCKCRXRC32OK			0x188
+#define	RPMAC_TXSTATUS				0x18c
+
+#define	RFPGA0_RFMOD				0x800
+
+#define	RFPGA0_TXINFO				0x804
+#define	RFPGA0_PSDFUNCTION			0x808
+
+#define	RFPGA0_TXGAINSTAGE			0x80c
+
+#define	RFPGA0_RFTIMING1			0x810
+#define	RFPGA0_RFTIMING2			0x814
 
 #define	RFPGA0_XA_HSSIPARAMETER1	0x820
 #define	RFPGA0_XA_HSSIPARAMETER2	0x824
@@ -1385,7 +1388,6 @@
 #define	RCCK0_FACOUNTERUPPER		0xa58
 #define	RCCK0_CCA_CNT			0xa60
 
-
 /* PageB(0xB00) */
 #define	RPDP_ANTA			0xb00
 #define	RPDP_ANTA_4			0xb04
@@ -1399,7 +1401,7 @@
 #define	RPDP_ANTA_24			0xb24
 
 #define	RCONFIG_PMPD_ANTA		0xb28
-#define	CONFIG_RAM64X16			0xb2c
+#define	RCONFIG_ram64x16		0xb2c
 
 #define	RBNDA				0xb30
 #define	RHSSIPAR			0xb34
@@ -1494,7 +1496,6 @@
 #define	ROFDM0_FRAMESYNC		0xcf0
 #define	ROFDM0_DFSREPORT		0xcf4
 
-
 #define	ROFDM1_LSTF			0xd00
 #define	ROFDM1_TRXPATHENABLE		0xd04
 
@@ -1593,144 +1594,144 @@
 #define	RSLEEP				0xee0
 #define	RPMPD_ANAEN			0xeec
 
-#define	RZEBRA1_HSSIENABLE		0x0
-#define	RZEBRA1_TRXENABLE1		0x1
-#define	RZEBRA1_TRXENABLE2		0x2
-#define	RZEBRA1_AGC			0x4
-#define	RZEBRA1_CHARGEPUMP		0x5
-#define	RZEBRA1_CHANNEL			0x7
-
-#define	RZEBRA1_TXGAIN			0x8
-#define	RZEBRA1_TXLPF			0x9
-#define	RZEBRA1_RXLPF			0xb
-#define	RZEBRA1_RXHPFCORNER		0xc
-
-#define	RGLOBALCTRL			0
-#define	RRTL8256_TXLPF			19
-#define	RRTL8256_RXLPF			11
-#define	RRTL8258_TXLPF			0x11
-#define	RRTL8258_RXLPF			0x13
-#define	RRTL8258_RSSILPF		0xa
-
-#define	RF_AC				0x00
-
-#define	RF_IQADJ_G1			0x01
-#define	RF_IQADJ_G2			0x02
-#define	RF_POW_TRSW			0x05
-
-#define	RF_GAIN_RX			0x06
-#define	RF_GAIN_TX			0x07
-
-#define	RF_TXM_IDAC			0x08
-#define	RF_BS_IQGEN			0x0F
-
-#define	RF_MODE1			0x10
-#define	RF_MODE2			0x11
-
-#define	RF_RX_AGC_HP			0x12
-#define	RF_TX_AGC			0x13
-#define	RF_BIAS				0x14
-#define	RF_IPA				0x15
-#define	RF_POW_ABILITY			0x17
-#define	RF_MODE_AG			0x18
-#define	RRFCHANNEL			0x18
-#define	RF_CHNLBW			0x18
-#define	RF_TOP				0x19
-
-#define	RF_RX_G1			0x1A
-#define	RF_RX_G2			0x1B
-
-#define	RF_RX_BB2			0x1C
-#define	RF_RX_BB1			0x1D
-
-#define	RF_RCK1				0x1E
-#define	RF_RCK2				0x1F
-
-#define	RF_TX_G1			0x20
-#define	RF_TX_G2			0x21
-#define	RF_TX_G3			0x22
-
-#define	RF_TX_BB1			0x23
-#define	RF_T_METER			0x42
-
-#define	RF_SYN_G1			0x25
-#define	RF_SYN_G2			0x26
-#define	RF_SYN_G3			0x27
-#define	RF_SYN_G4			0x28
-#define	RF_SYN_G5			0x29
-#define	RF_SYN_G6			0x2A
-#define	RF_SYN_G7			0x2B
-#define	RF_SYN_G8			0x2C
-
-#define	RF_RCK_OS			0x30
-#define	RF_TXPA_G1			0x31
-#define	RF_TXPA_G2			0x32
-#define	RF_TXPA_G3			0x33
-
-#define	RF_TX_BIAS_A			0x35
-#define	RF_TX_BIAS_D			0x36
-#define	RF_LOBF_9			0x38
-#define	RF_RXRF_A3			0x3C
-#define	RF_TRSW				0x3F
-
-#define	RF_TXRF_A2			0x41
-#define	RF_TXPA_G4			0x46
-#define	RF_TXPA_A4			0x4B
-
-#define	RF_WE_LUT			0xEF
-
-#define	BBBRESETB			0x100
-#define	BGLOBALRESETB			0x200
-#define	BOFDMTXSTART			0x4
-#define	BCCKTXSTART			0x8
-#define	BCRC32DEBUG			0x100
-#define	BPMACLOOPBACK			0x10
-#define	BTXLSIG				0xffffff
-#define	BOFDMTXRATE			0xf
-#define	BOFDMTXRESERVED			0x10
-#define	BOFDMTXLENGTH			0x1ffe0
-#define	BOFDMTXPARITY			0x20000
-#define	BTXHTSIG1			0xffffff
-#define	BTXHTMCSRATE			0x7f
-#define	BTXHTBW				0x80
-#define	BTXHTLENGTH			0xffff00
-#define	BTXHTSIG2			0xffffff
-#define	BTXHTSMOOTHING			0x1
-#define	BTXHTSOUNDING			0x2
-#define	BTXHTRESERVED			0x4
-#define	BTXHTAGGREATION			0x8
-#define	BTXHTSTBC			0x30
-#define	BTXHTADVANCECODING		0x40
-#define	BTXHTSHORTGI			0x80
-#define	BTXHTNUMBERHT_LTF		0x300
-#define	BTXHTCRC8			0x3fc00
-#define	BCOUNTERRESET			0x10000
-#define	BNUMOFOFDMTX			0xffff
-#define	BNUMOFCCKTX			0xffff0000
-#define	BTXIDLEINTERVAL			0xffff
-#define	BOFDMSERVICE			0xffff0000
-#define	BTXMACHEADER			0xffffffff
-#define	BTXDATAINIT			0xff
-#define	BTXHTMODE			0x100
-#define	BTXDATATYPE			0x30000
-#define	BTXRANDOMSEED			0xffffffff
-#define	BCCKTXPREAMBLE			0x1
-#define	BCCKTXSFD			0xffff0000
-#define	BCCKTXSIG			0xff
-#define	BCCKTXSERVICE			0xff00
-#define	BCCKLENGTHEXT			0x8000
-#define	BCCKTXLENGHT			0xffff0000
-#define	BCCKTXCRC16			0xffff
-#define	BCCKTXSTATUS			0x1
-#define	BOFDMTXSTATUS			0x2
+#define	RZEBRA1_HSSIENABLE			0x0
+#define	RZEBRA1_TRXENABLE1			0x1
+#define	RZEBRA1_TRXENABLE2			0x2
+#define	RZEBRA1_AGC				0x4
+#define	RZEBRA1_CHARGEPUMP			0x5
+#define	RZEBRA1_CHANNEL				0x7
+
+#define	RZEBRA1_TXGAIN				0x8
+#define	RZEBRA1_TXLPF				0x9
+#define	RZEBRA1_RXLPF				0xb
+#define	RZEBRA1_RXHPFCORNER			0xc
+
+#define	RGLOBALCTRL				0
+#define	RRTL8256_TXLPF				19
+#define	RRTL8256_RXLPF				11
+#define	RRTL8258_TXLPF				0x11
+#define	RRTL8258_RXLPF				0x13
+#define	RRTL8258_RSSILPF			0xa
+
+#define	RF_AC					0x00
+
+#define	RF_IQADJ_G1				0x01
+#define	RF_IQADJ_G2				0x02
+#define	RF_POW_TRSW				0x05
+
+#define	RF_GAIN_RX				0x06
+#define	RF_GAIN_TX				0x07
+
+#define	RF_TXM_IDAC				0x08
+#define	RF_BS_IQGEN				0x0F
+
+#define	RF_MODE1				0x10
+#define	RF_MODE2				0x11
+
+#define	RF_RX_AGC_HP				0x12
+#define	RF_TX_AGC				0x13
+#define	RF_BIAS					0x14
+#define	RF_IPA					0x15
+#define	RF_POW_ABILITY				0x17
+#define	RF_MODE_AG				0x18
+#define	RRFCHANNEL				0x18
+#define	RF_CHNLBW				0x18
+#define	RF_TOP					0x19
+
+#define	RF_RX_G1				0x1A
+#define	RF_RX_G2				0x1B
+
+#define	RF_RX_BB2				0x1C
+#define	RF_RX_BB1				0x1D
+
+#define	RF_RCK1					0x1E
+#define	RF_RCK2					0x1F
+
+#define	RF_TX_G1				0x20
+#define	RF_TX_G2				0x21
+#define	RF_TX_G3				0x22
+
+#define	RF_TX_BB1				0x23
+#define	RF_T_METER				0x42
+
+#define	RF_SYN_G1				0x25
+#define	RF_SYN_G2				0x26
+#define	RF_SYN_G3				0x27
+#define	RF_SYN_G4				0x28
+#define	RF_SYN_G5				0x29
+#define	RF_SYN_G6				0x2A
+#define	RF_SYN_G7				0x2B
+#define	RF_SYN_G8				0x2C
+
+#define	RF_RCK_OS				0x30
+#define	RF_TXPA_G1				0x31
+#define	RF_TXPA_G2				0x32
+#define	RF_TXPA_G3				0x33
+
+#define	RF_TX_BIAS_A				0x35
+#define	RF_TX_BIAS_D				0x36
+#define	RF_LOBF_9				0x38
+#define	RF_RXRF_A3				0x3C
+#define	RF_TRSW					0x3F
+
+#define	RF_TXRF_A2				0x41
+#define	RF_TXPA_G4				0x46
+#define	RF_TXPA_A4				0x4B
+
+#define	RF_WE_LUT				0xEF
+
+#define	BBBRESETB				0x100
+#define	BGLOBALRESETB				0x200
+#define	BOFDMTXSTART				0x4
+#define	BCCKTXSTART				0x8
+#define	BCRC32DEBUG				0x100
+#define	BPMACLOOPBACK				0x10
+#define	BTXLSIG					0xffffff
+#define	BOFDMTXRATE				0xf
+#define	BOFDMTXRESERVED				0x10
+#define	BOFDMTXLENGTH				0x1ffe0
+#define	BOFDMTXPARITY				0x20000
+#define	BTXHTSIG1				0xffffff
+#define	BTXHTMCSRATE				0x7f
+#define	BTXHTBW					0x80
+#define	BTXHTLENGTH				0xffff00
+#define	BTXHTSIG2				0xffffff
+#define	BTXHTSMOOTHING				0x1
+#define	BTXHTSOUNDING				0x2
+#define	BTXHTRESERVED				0x4
+#define	BTXHTAGGREATION				0x8
+#define	BTXHTSTBC				0x30
+#define	BTXHTADVANCECODING			0x40
+#define	BTXHTSHORTGI				0x80
+#define	BTXHTNUMBERHT_LTF			0x300
+#define	BTXHTCRC8				0x3fc00
+#define	BCOUNTERRESET				0x10000
+#define	BNUMOFOFDMTX				0xffff
+#define	BNUMOFCCKTX				0xffff0000
+#define	BTXIDLEINTERVAL				0xffff
+#define	BOFDMSERVICE				0xffff0000
+#define	BTXMACHEADER				0xffffffff
+#define	BTXDATAINIT				0xff
+#define	BTXHTMODE				0x100
+#define	BTXDATATYPE				0x30000
+#define	BTXRANDOMSEED				0xffffffff
+#define	BCCKTXPREAMBLE				0x1
+#define	BCCKTXSFD				0xffff0000
+#define	BCCKTXSIG				0xff
+#define	BCCKTXSERVICE				0xff00
+#define	BCCKLENGTHEXT				0x8000
+#define	BCCKTXLENGHT				0xffff0000
+#define	BCCKTXCRC16				0xffff
+#define	BCCKTXSTATUS				0x1
+#define	BOFDMTXSTATUS				0x2
 #define IS_BB_REG_OFFSET_92S(_offset)	\
 	((_offset >= 0x800) && (_offset <= 0xfff))
 
-#define	BRFMOD				0x1
-#define	BJAPANMODE			0x2
-#define	BCCKTXSC			0x30
-#define	BCCKEN				0x1000000
-#define	BOFDMEN				0x2000000
+#define	BRFMOD					0x1
+#define	BJAPANMODE				0x2
+#define	BCCKTXSC				0x30
+#define	BCCKEN					0x1000000
+#define	BOFDMEN					0x2000000
 
 #define	BOFDMRXADCPHASE			0x10000
 #define	BOFDMTXDACPHASE			0x40000
@@ -1824,13 +1825,13 @@
 #define	BDA6SWING			0x380000
 
 #define	BADCLKPHASE			0x4000000
-#define	B80MCLKDELAY	0x18000000
-#define	BAFEWATCHDOGENABLE	0x20000000
+#define	B80MCLKDELAY			0x18000000
+#define	BAFEWATCHDOGENABLE		0x20000000
 
-#define	BXTALCAP01	0xc0000000
-#define	BXTALCAP23	0x3
+#define	BXTALCAP01			0xc0000000
+#define	BXTALCAP23			0x3
 #define	BXTALCAP92X			0x0f000000
-#define BXTALCAP	0x0f000000
+#define BXTALCAP			0x0f000000
 
 #define	BINTDIFCLKENABLE		0x400
 #define	BEXTSIGCLKENABLE		0x800
@@ -1857,7 +1858,7 @@
 #define	BCCKRX_AGC_FORMAT		0x200
 #define	BPSDFFT_SAMPLE_POINT		0xc000
 #define	BPSD_AVERAGE_NUM		0x3000
-#define	BIQPATH_CONTROL	0xc00
+#define	BIQPATH_CONTROL			0xc00
 #define	BPSD_FREQ			0x3ff
 #define	BPSD_ANTENNA_PATH		0x30
 #define	BPSD_IQ_SWITCH			0x40
@@ -1957,300 +1958,316 @@
 #define	BCCK_DEFAULT_RXPATH		0xc000000
 #define	BCCK_OPTION_RXPATH		0x3000000
 
-#define	BNUM_OFSTF	0x3
-#define	BSHIFT_L	0xc0
-#define	BGI_TH	0xc
-#define	BRXPATH_A	0x1
-#define	BRXPATH_B	0x2
-#define	BRXPATH_C	0x4
-#define	BRXPATH_D	0x8
-#define	BTXPATH_A	0x1
-#define	BTXPATH_B	0x2
-#define	BTXPATH_C	0x4
-#define	BTXPATH_D	0x8
-#define	BTRSSI_FREQ	0x200
-#define	BADC_BACKOFF	0x3000
-#define	BDFIR_BACKOFF	0xc000
-#define	BTRSSI_LATCH_PHASE	0x10000
-#define	BRX_LDC_OFFSET	0xff
-#define	BRX_QDC_OFFSET	0xff00
-#define	BRX_DFIR_MODE	0x1800000
-#define	BRX_DCNF_TYPE	0xe000000
-#define	BRXIQIMB_A	0x3ff
-#define	BRXIQIMB_B	0xfc00
-#define	BRXIQIMB_C	0x3f0000
-#define	BRXIQIMB_D	0xffc00000
-#define	BDC_DC_NOTCH	0x60000
-#define	BRXNB_NOTCH	0x1f000000
-#define	BPD_TH	0xf
-#define	BPD_TH_OPT2	0xc000
-#define	BPWED_TH	0x700
-#define	BIFMF_WIN_L	0x800
-#define	BPD_OPTION	0x1000
-#define	BMF_WIN_L	0xe000
-#define	BBW_SEARCH_L	0x30000
-#define	BWIN_ENH_L	0xc0000
-#define	BBW_TH	0x700000
-#define	BED_TH2	0x3800000
-#define	BBW_OPTION	0x4000000
-#define	BRADIO_TH	0x18000000
-#define	BWINDOW_L	0xe0000000
-#define	BSBD_OPTION	0x1
-#define	BFRAME_TH	0x1c
-#define	BFS_OPTION	0x60
-#define	BDC_SLOPE_CHECK	0x80
-#define	BFGUARD_COUNTER_DC_L	0xe00
-#define	BFRAME_WEIGHT_SHORT	0x7000
-#define	BSUB_TUNE	0xe00000
-#define	BFRAME_DC_LENGTH	0xe000000
-#define	BSBD_START_OFFSET	0x30000000
-#define	BFRAME_TH_2	0x7
-#define	BFRAME_GI2_TH	0x38
-#define	BGI2_SYNC_EN	0x40
-#define	BSARCH_SHORT_EARLY	0x300
-#define	BSARCH_SHORT_LATE	0xc00
-#define	BSARCH_GI2_LATE	0x70000
-#define	BCFOANTSUM	0x1
-#define	BCFOACC	0x2
-#define	BCFOSTARTOFFSET	0xc
-#define	BCFOLOOPBACK	0x70
-#define	BCFOSUMWEIGHT	0x80
-#define	BDAGCENABLE	0x10000
-#define	BTXIQIMB_A	0x3ff
-#define	BTXIQIMB_b	0xfc00
-#define	BTXIQIMB_C	0x3f0000
-#define	BTXIQIMB_D	0xffc00000
-#define	BTXIDCOFFSET	0xff
-#define	BTXIQDCOFFSET	0xff00
-#define	BTXDFIRMODE	0x10000
-#define	BTXPESUDO_NOISEON	0x4000000
-#define	BTXPESUDO_NOISE_A	0xff
-#define	BTXPESUDO_NOISE_B	0xff00
-#define	BTXPESUDO_NOISE_C	0xff0000
-#define	BTXPESUDO_NOISE_D	0xff000000
-#define	BCCA_DROPOPTION	0x20000
-#define	BCCA_DROPTHRES	0xfff00000
-#define	BEDCCA_H	0xf
-#define	BEDCCA_L	0xf0
-#define	BLAMBDA_ED	0x300
-#define	BRX_INITIALGAIN	0x7f
-#define	BRX_ANTDIV_EN	0x80
+#define	BNUM_OFSTF			0x3
+#define	BSHIFT_L			0xc0
+#define	BGI_TH				0xc
+#define	BRXPATH_A			0x1
+#define	BRXPATH_B			0x2
+#define	BRXPATH_C			0x4
+#define	BRXPATH_D			0x8
+#define	BTXPATH_A			0x1
+#define	BTXPATH_B			0x2
+#define	BTXPATH_C			0x4
+#define	BTXPATH_D			0x8
+#define	BTRSSI_FREQ			0x200
+#define	BADC_BACKOFF			0x3000
+#define	BDFIR_BACKOFF			0xc000
+#define	BTRSSI_LATCH_PHASE		0x10000
+#define	BRX_LDC_OFFSET			0xff
+#define	BRX_QDC_OFFSET			0xff00
+#define	BRX_DFIR_MODE			0x1800000
+#define	BRX_DCNF_TYPE			0xe000000
+#define	BRXIQIMB_A			0x3ff
+#define	BRXIQIMB_B			0xfc00
+#define	BRXIQIMB_C			0x3f0000
+#define	BRXIQIMB_D			0xffc00000
+#define	BDC_DC_NOTCH			0x60000
+#define	BRXNB_NOTCH			0x1f000000
+#define	BPD_TH				0xf
+#define	BPD_TH_OPT2			0xc000
+#define	BPWED_TH			0x700
+#define	BIFMF_WIN_L			0x800
+#define	BPD_OPTION			0x1000
+#define	BMF_WIN_L			0xe000
+#define	BBW_SEARCH_L			0x30000
+#define	BWIN_ENH_L			0xc0000
+#define	BBW_TH				0x700000
+#define	BED_TH2				0x3800000
+#define	BBW_OPTION			0x4000000
+#define	BRADIO_TH			0x18000000
+#define	BWINDOW_L			0xe0000000
+#define	BSBD_OPTION			0x1
+#define	BFRAME_TH			0x1c
+#define	BFS_OPTION			0x60
+#define	BDC_SLOPE_CHECK			0x80
+#define	BFGUARD_COUNTER_DC_L		0xe00
+#define	BFRAME_WEIGHT_SHORT		0x7000
+#define	BSUB_TUNE			0xe00000
+#define	BFRAME_DC_LENGTH		0xe000000
+#define	BSBD_START_OFFSET		0x30000000
+#define	BFRAME_TH_2			0x7
+#define	BFRAME_GI2_TH			0x38
+#define	BGI2_SYNC_EN			0x40
+#define	BSARCH_SHORT_EARLY		0x300
+#define	BSARCH_SHORT_LATE		0xc00
+#define	BSARCH_GI2_LATE			0x70000
+#define	BCFOANTSUM			0x1
+#define	BCFOACC				0x2
+#define	BCFOSTARTOFFSET			0xc
+#define	BCFOLOOPBACK			0x70
+#define	BCFOSUMWEIGHT			0x80
+#define	BDAGCENABLE			0x10000
+#define	BTXIQIMB_A			0x3ff
+#define	BTXIQIMB_b			0xfc00
+#define	BTXIQIMB_C			0x3f0000
+#define	BTXIQIMB_D			0xffc00000
+#define	BTXIDCOFFSET			0xff
+#define	BTXIQDCOFFSET			0xff00
+#define	BTXDFIRMODE			0x10000
+#define	BTXPESUDO_NOISEON		0x4000000
+#define	BTXPESUDO_NOISE_A		0xff
+#define	BTXPESUDO_NOISE_B		0xff00
+#define	BTXPESUDO_NOISE_C		0xff0000
+#define	BTXPESUDO_NOISE_D		0xff000000
+#define	BCCA_DROPOPTION			0x20000
+#define	BCCA_DROPTHRES			0xfff00000
+#define	BEDCCA_H			0xf
+#define	BEDCCA_L			0xf0
+#define	BLAMBDA_ED			0x300
+#define	BRX_INITIALGAIN			0x7f
+#define	BRX_ANTDIV_EN			0x80
 #define	BRX_AGC_ADDRESS_FOR_LNA		0x7f00
-#define	BRX_HIGHPOWER_FLOW	0x8000
+#define	BRX_HIGHPOWER_FLOW		0x8000
 #define	BRX_AGC_FREEZE_THRES		0xc0000
-#define	BRX_FREEZESTEP_AGC1	0x300000
-#define	BRX_FREEZESTEP_AGC2	0xc00000
-#define	BRX_FREEZESTEP_AGC3	0x3000000
-#define	BRX_FREEZESTEP_AGC0	0xc000000
-#define	BRXRSSI_CMP_EN	0x10000000
-#define	BRXQUICK_AGCEN	0x20000000
+#define	BRX_FREEZESTEP_AGC1		0x300000
+#define	BRX_FREEZESTEP_AGC2		0xc00000
+#define	BRX_FREEZESTEP_AGC3		0x3000000
+#define	BRX_FREEZESTEP_AGC0		0xc000000
+#define	BRXRSSI_CMP_EN			0x10000000
+#define	BRXQUICK_AGCEN			0x20000000
 #define	BRXAGC_FREEZE_THRES_MODE	0x40000000
-#define	BRX_OVERFLOW_CHECKTYPE	0x80000000
-#define	BRX_AGCSHIFT	0x7f
-#define	BTRSW_TRI_ONLY	0x80
-#define	BPOWER_THRES	0x300
-#define	BRXAGC_EN	0x1
-#define	BRXAGC_TOGETHER_EN	0x2
-#define	BRXAGC_MIN	0x4
-#define	BRXHP_INI	0x7
-#define	BRXHP_TRLNA	0x70
-#define	BRXHP_RSSI	0x700
-#define	BRXHP_BBP1	0x7000
-#define	BRXHP_BBP2	0x70000
-#define	BRXHP_BBP3	0x700000
-#define	BRSSI_H	0x7f0000
-#define	BRSSI_GEN	0x7f000000
-#define	BRXSETTLE_TRSW	0x7
-#define	BRXSETTLE_LNA	0x38
-#define	BRXSETTLE_RSSI	0x1c0
-#define	BRXSETTLE_BBP	0xe00
-#define	BRXSETTLE_RXHP	0x7000
-#define	BRXSETTLE_ANTSW_RSSI	0x38000
-#define	BRXSETTLE_ANTSW	0xc0000
-#define	BRXPROCESS_TIME_DAGC	0x300000
-#define	BRXSETTLE_HSSI	0x400000
-#define	BRXPROCESS_TIME_BBPPW	0x800000
-#define	BRXANTENNA_POWER_SHIFT	0x3000000
-#define	BRSSI_TABLE_SELECT	0xc000000
-#define	BRXHP_FINAL	0x7000000
-#define	BRXHPSETTLE_BBP	0x7
-#define	BRXHTSETTLE_HSSI	0x8
-#define	BRXHTSETTLE_RXHP	0x70
-#define	BRXHTSETTLE_BBPPW	0x80
-#define	BRXHTSETTLE_IDLE	0x300
-#define	BRXHTSETTLE_RESERVED	0x1c00
-#define	BRXHT_RXHP_EN	0x8000
-#define	BRXAGC_FREEZE_THRES	0x30000
-#define	BRXAGC_TOGETHEREN	0x40000
-#define	BRXHTAGC_MIN	0x80000
-#define	BRXHTAGC_EN	0x100000
-#define	BRXHTDAGC_EN	0x200000
-#define	BRXHT_RXHP_BBP	0x1c00000
-#define	BRXHT_RXHP_FINAL	0xe0000000
-#define	BRXPW_RADIO_TH	0x3
-#define	BRXPW_RADIO_EN	0x4
-#define	BRXMF_HOLD	0x3800
-#define	BRXPD_DELAY_TH1	0x38
-#define	BRXPD_DELAY_TH2	0x1c0
-#define	BRXPD_DC_COUNT_MAX	0x600
-#define	BRXPD_DELAY_TH	0x8000
-#define	BRXPROCESS_DELAY	0xf0000
+#define	BRX_OVERFLOW_CHECKTYPE		0x80000000
+#define	BRX_AGCSHIFT			0x7f
+#define	BTRSW_TRI_ONLY			0x80
+#define	BPOWER_THRES			0x300
+#define	BRXAGC_EN			0x1
+#define	BRXAGC_TOGETHER_EN		0x2
+#define	BRXAGC_MIN			0x4
+#define	BRXHP_INI			0x7
+#define	BRXHP_TRLNA			0x70
+#define	BRXHP_RSSI			0x700
+#define	BRXHP_BBP1			0x7000
+#define	BRXHP_BBP2			0x70000
+#define	BRXHP_BBP3			0x700000
+#define	BRSSI_H				0x7f0000
+#define	BRSSI_GEN			0x7f000000
+#define	BRXSETTLE_TRSW			0x7
+#define	BRXSETTLE_LNA			0x38
+#define	BRXSETTLE_RSSI			0x1c0
+#define	BRXSETTLE_BBP			0xe00
+#define	BRXSETTLE_RXHP			0x7000
+#define	BRXSETTLE_ANTSW_RSSI		0x38000
+#define	BRXSETTLE_ANTSW			0xc0000
+#define	BRXPROCESS_TIME_DAGC		0x300000
+#define	BRXSETTLE_HSSI			0x400000
+#define	BRXPROCESS_TIME_BBPPW		0x800000
+#define	BRXANTENNA_POWER_SHIFT		0x3000000
+#define	BRSSI_TABLE_SELECT		0xc000000
+#define	BRXHP_FINAL			0x7000000
+#define	BRXHPSETTLE_BBP			0x7
+#define	BRXHTSETTLE_HSSI		0x8
+#define	BRXHTSETTLE_RXHP		0x70
+#define	BRXHTSETTLE_BBPPW		0x80
+#define	BRXHTSETTLE_IDLE		0x300
+#define	BRXHTSETTLE_RESERVED		0x1c00
+#define	BRXHT_RXHP_EN			0x8000
+#define	BRXAGC_FREEZE_THRES		0x30000
+#define	BRXAGC_TOGETHEREN		0x40000
+#define	BRXHTAGC_MIN			0x80000
+#define	BRXHTAGC_EN			0x100000
+#define	BRXHTDAGC_EN			0x200000
+#define	BRXHT_RXHP_BBP			0x1c00000
+#define	BRXHT_RXHP_FINAL		0xe0000000
+#define	BRXPW_RADIO_TH			0x3
+#define	BRXPW_RADIO_EN			0x4
+#define	BRXMF_HOLD			0x3800
+#define	BRXPD_DELAY_TH1			0x38
+#define	BRXPD_DELAY_TH2			0x1c0
+#define	BRXPD_DC_COUNT_MAX		0x600
+#define	BRXPD_DELAY_TH			0x8000
+#define	BRXPROCESS_DELAY		0xf0000
 #define	BRXSEARCHRANGE_GI2_EARLY	0x700000
 #define	BRXFRAME_FUARD_COUNTER_L	0x3800000
-#define	BRXSGI_GUARD_L	0xc000000
-#define	BRXSGI_SEARCH_L	0x30000000
-#define	BRXSGI_TH	0xc0000000
-#define	BDFSCNT0	0xff
-#define	BDFSCNT1	0xff00
-#define	BDFSFLAG	0xf0000
-#define	BMF_WEIGHT_SUM	0x300000
-#define	BMINIDX_TH	0x7f000000
-#define	BDAFORMAT	0x40000
-#define	BTXCH_EMU_ENABLE	0x01000000
-#define	BTRSW_ISOLATION_A	0x7f
-#define	BTRSW_ISOLATION_B	0x7f00
-#define	BTRSW_ISOLATION_C	0x7f0000
-#define	BTRSW_ISOLATION_D	0x7f000000
-#define	BEXT_LNA_GAIN	0x7c00
-
-#define	BSTBC_EN	0x4
-#define	BANTENNA_MAPPING	0x10
-#define	BNSS	0x20
+#define	BRXSGI_GUARD_L			0xc000000
+#define	BRXSGI_SEARCH_L			0x30000000
+#define	BRXSGI_TH			0xc0000000
+#define	BDFSCNT0			0xff
+#define	BDFSCNT1			0xff00
+#define	BDFSFLAG			0xf0000
+#define	BMF_WEIGHT_SUM			0x300000
+#define	BMINIDX_TH			0x7f000000
+#define	BDAFORMAT			0x40000
+#define	BTXCH_EMU_ENABLE		0x01000000
+#define	BTRSW_ISOLATION_A		0x7f
+#define	BTRSW_ISOLATION_B		0x7f00
+#define	BTRSW_ISOLATION_C		0x7f0000
+#define	BTRSW_ISOLATION_D		0x7f000000
+#define	BEXT_LNA_GAIN			0x7c00
+
+#define	BSTBC_EN			0x4
+#define	BANTENNA_MAPPING		0x10
+#define	BNSS				0x20
 #define	BCFO_ANTSUM_ID			0x200
-#define	BPHY_COUNTER_RESET	0x8000000
-#define	BCFO_REPORT_GET	0x4000000
-#define	BOFDM_CONTINUE_TX	0x10000000
-#define	BOFDM_SINGLE_CARRIER	0x20000000
-#define	BOFDM_SINGLE_TONE	0x40000000
-#define	BHT_DETECT	0x100
-#define	BCFOEN	0x10000
-#define	BCFOVALUE	0xfff00000
-#define	BSIGTONE_RE	0x3f
-#define	BSIGTONE_IM	0x7f00
-#define	BCOUNTER_CCA	0xffff
-#define	BCOUNTER_PARITYFAIL	0xffff0000
-#define	BCOUNTER_RATEILLEGAL	0xffff
-#define	BCOUNTER_CRC8FAIL	0xffff0000
-#define	BCOUNTER_MCSNOSUPPORT	0xffff
-#define	BCOUNTER_FASTSYNC	0xffff
-#define	BSHORTCFO	0xfff
-#define	BSHORTCFOT_LENGTH	12
-#define	BSHORTCFOF_LENGTH	11
-#define	BLONGCFO	0x7ff
-#define	BLONGCFOT_LENGTH	11
-#define	BLONGCFOF_LENGTH	11
-#define	BTAILCFO	0x1fff
-#define	BTAILCFOT_LENGTH	13
-#define	BTAILCFOF_LENGTH	12
-#define	BNOISE_EN_PWDB	0xffff
-#define	BCC_POWER_DB	0xffff0000
-#define	BMOISE_PWDB	0xffff
-#define	BPOWERMEAST_LENGTH	10
-#define	BPOWERMEASF_LENGTH	3
-#define	BRX_HT_BW	0x1
-#define	BRXSC	0x6
-#define	BRX_HT	0x8
-#define	BNB_INTF_DET_ON	0x1
-#define	BINTF_WIN_LEN_CFG	0x30
-#define	BNB_INTF_TH_CFG	0x1c0
-#define	BRFGAIN	0x3f
-#define	BTABLESEL	0x40
-#define	BTRSW	0x80
-#define	BRXSNR_A	0xff
-#define	BRXSNR_B	0xff00
-#define	BRXSNR_C	0xff0000
-#define	BRXSNR_D	0xff000000
-#define	BSNR_EVMT_LENGTH	8
-#define	BSNR_EVMF_LENGTH	1
-#define	BCSI1ST	0xff
-#define	BCSI2ND	0xff00
-#define	BRXEVM1ST	0xff0000
-#define	BRXEVM2ND	0xff000000
-#define	BSIGEVM	0xff
-#define	BPWDB	0xff00
-#define	BSGIEN	0x10000
-
-#define	BSFACTOR_QMA1	0xf
-#define	BSFACTOR_QMA2	0xf0
-#define	BSFACTOR_QMA3	0xf00
-#define	BSFACTOR_QMA4	0xf000
-#define	BSFACTOR_QMA5	0xf0000
-#define	BSFACTOR_QMA6	0xf0000
-#define	BSFACTOR_QMA7	0xf00000
-#define	BSFACTOR_QMA8	0xf000000
-#define	BSFACTOR_QMA9	0xf0000000
-#define	BCSI_SCHEME	0x100000
+#define	BPHY_COUNTER_RESET		0x8000000
+#define	BCFO_REPORT_GET			0x4000000
+#define	BOFDM_CONTINUE_TX		0x10000000
+#define	BOFDM_SINGLE_CARRIER		0x20000000
+#define	BOFDM_SINGLE_TONE		0x40000000
+#define	BHT_DETECT			0x100
+#define	BCFOEN				0x10000
+#define	BCFOVALUE			0xfff00000
+#define	BSIGTONE_RE			0x3f
+#define	BSIGTONE_IM			0x7f00
+#define	BCOUNTER_CCA			0xffff
+#define	BCOUNTER_PARITYFAIL		0xffff0000
+#define	BCOUNTER_RATEILLEGAL		0xffff
+#define	BCOUNTER_CRC8FAIL		0xffff0000
+#define	BCOUNTER_MCSNOSUPPORT		0xffff
+#define	BCOUNTER_FASTSYNC		0xffff
+#define	BSHORTCFO			0xfff
+#define	BSHORTCFOT_LENGTH		12
+#define	BSHORTCFOF_LENGTH		11
+#define	BLONGCFO			0x7ff
+#define	BLONGCFOT_LENGTH		11
+#define	BLONGCFOF_LENGTH		11
+#define	BTAILCFO			0x1fff
+#define	BTAILCFOT_LENGTH		13
+#define	BTAILCFOF_LENGTH		12
+#define	BNOISE_EN_PWDB			0xffff
+#define	BCC_POWER_DB			0xffff0000
+#define	BMOISE_PWDB			0xffff
+#define	BPOWERMEAST_LENGTH		10
+#define	BPOWERMEASF_LENGTH		3
+#define	BRX_HT_BW			0x1
+#define	BRXSC				0x6
+#define	BRX_HT				0x8
+#define	BNB_INTF_DET_ON			0x1
+#define	BINTF_WIN_LEN_CFG		0x30
+#define	BNB_INTF_TH_CFG			0x1c0
+#define	BRFGAIN				0x3f
+#define	BTABLESEL			0x40
+#define	BTRSW				0x80
+#define	BRXSNR_A			0xff
+#define	BRXSNR_B			0xff00
+#define	BRXSNR_C			0xff0000
+#define	BRXSNR_D			0xff000000
+#define	BSNR_EVMT_LENGTH		8
+#define	BSNR_EVMF_LENGTH		1
+#define	BCSI1ST				0xff
+#define	BCSI2ND				0xff00
+#define	BRXEVM1ST			0xff0000
+#define	BRXEVM2ND			0xff000000
+#define	BSIGEVM				0xff
+#define	BPWDB				0xff00
+#define	BSGIEN				0x10000
+
+#define	BSFACTOR_QMA1			0xf
+#define	BSFACTOR_QMA2			0xf0
+#define	BSFACTOR_QMA3			0xf00
+#define	BSFACTOR_QMA4			0xf000
+#define	BSFACTOR_QMA5			0xf0000
+#define	BSFACTOR_QMA6			0xf0000
+#define	BSFACTOR_QMA7			0xf00000
+#define	BSFACTOR_QMA8			0xf000000
+#define	BSFACTOR_QMA9			0xf0000000
+#define	BCSI_SCHEME			0x100000
 
 #define	BNOISE_LVL_TOP_SET		0x3
-#define	BCHSMOOTH	0x4
-#define	BCHSMOOTH_CFG1	0x38
-#define	BCHSMOOTH_CFG2	0x1c0
-#define	BCHSMOOTH_CFG3	0xe00
-#define	BCHSMOOTH_CFG4	0x7000
-#define	BMRCMODE	0x800000
-#define	BTHEVMCFG	0x7000000
-
-#define	BLOOP_FIT_TYPE	0x1
-#define	BUPD_CFO	0x40
-#define	BUPD_CFO_OFFDATA	0x80
-#define	BADV_UPD_CFO	0x100
-#define	BADV_TIME_CTRL	0x800
-#define	BUPD_CLKO	0x1000
-#define	BFC	0x6000
-#define	BTRACKING_MODE	0x8000
-#define	BPHCMP_ENABLE	0x10000
-#define	BUPD_CLKO_LTF	0x20000
-#define	BCOM_CH_CFO	0x40000
-#define	BCSI_ESTI_MODE	0x80000
-#define	BADV_UPD_EQZ	0x100000
-#define	BUCHCFG	0x7000000
-#define	BUPDEQZ	0x8000000
+#define	BCHSMOOTH			0x4
+#define	BCHSMOOTH_CFG1			0x38
+#define	BCHSMOOTH_CFG2			0x1c0
+#define	BCHSMOOTH_CFG3			0xe00
+#define	BCHSMOOTH_CFG4			0x7000
+#define	BMRCMODE			0x800000
+#define	BTHEVMCFG			0x7000000
+
+#define	BLOOP_FIT_TYPE			0x1
+#define	BUPD_CFO			0x40
+#define	BUPD_CFO_OFFDATA		0x80
+#define	BADV_UPD_CFO			0x100
+#define	BADV_TIME_CTRL			0x800
+#define	BUPD_CLKO			0x1000
+#define	BFC				0x6000
+#define	BTRACKING_MODE			0x8000
+#define	BPHCMP_ENABLE			0x10000
+#define	BUPD_CLKO_LTF			0x20000
+#define	BCOM_CH_CFO			0x40000
+#define	BCSI_ESTI_MODE			0x80000
+#define	BADV_UPD_EQZ			0x100000
+#define	BUCHCFG				0x7000000
+#define	BUPDEQZ				0x8000000
 
 #define	BRX_PESUDO_NOISE_ON		0x20000000
-#define	BRX_PESUDO_NOISE_A	0xff
-#define	BRX_PESUDO_NOISE_B	0xff00
-#define	BRX_PESUDO_NOISE_C	0xff0000
-#define	BRX_PESUDO_NOISE_D	0xff000000
+#define	BRX_PESUDO_NOISE_A		0xff
+#define	BRX_PESUDO_NOISE_B		0xff00
+#define	BRX_PESUDO_NOISE_C		0xff0000
+#define	BRX_PESUDO_NOISE_D		0xff000000
 #define	BRX_PESUDO_NOISESTATE_A		0xffff
 #define	BRX_PESUDO_NOISESTATE_B		0xffff0000
 #define	BRX_PESUDO_NOISESTATE_C		0xffff
 #define	BRX_PESUDO_NOISESTATE_D		0xffff0000
 
-#define	BZEBRA1_HSSIENABLE	0x8
-#define	BZEBRA1_TRXCONTROL	0xc00
-#define	BZEBRA1_TRXGAINSETTING	0x07f
-#define	BZEBRA1_RXCOUNTER	0xc00
-#define	BZEBRA1_TXCHANGEPUMP	0x38
-#define	BZEBRA1_RXCHANGEPUMP	0x7
-#define	BZEBRA1_CHANNEL_NUM	0xf80
-#define	BZEBRA1_TXLPFBW	0x400
-#define	BZEBRA1_RXLPFBW	0x600
+#define	BZEBRA1_HSSIENABLE		0x8
+#define	BZEBRA1_TRXCONTROL		0xc00
+#define	BZEBRA1_TRXGAINSETTING		0x07f
+#define	BZEBRA1_RXCOUNTER		0xc00
+#define	BZEBRA1_TXCHANGEPUMP		0x38
+#define	BZEBRA1_RXCHANGEPUMP		0x7
+#define	BZEBRA1_CHANNEL_NUM		0xf80
+#define	BZEBRA1_TXLPFBW			0x400
+#define	BZEBRA1_RXLPFBW			0x600
 
 #define	BRTL8256REG_MODE_CTRL1		0x100
 #define	BRTL8256REG_MODE_CTRL0		0x40
 #define	BRTL8256REG_TXLPFBW		0x18
 #define	BRTL8256REG_RXLPFBW		0x600
 
-#define	BRTL8258_TXLPFBW	0xc
-#define	BRTL8258_RXLPFBW	0xc00
-#define	BRTL8258_RSSILPFBW	0xc0
-
-#define	BBYTE0	0x1
-#define	BBYTE1	0x2
-#define	BBYTE2	0x4
-#define	BBYTE3	0x8
-#define	BWORD0	0x3
-#define	BWORD1	0xc
-#define	BWORD	0xf
-
-#define	BENABLE	0x1
-#define	BDISABLE	0x0
-
-#define	LEFT_ANTENNA	0x0
-#define	RIGHT_ANTENNA	0x1
-
-#define	TCHECK_TXSTATUS	500
-#define	TUPDATE_RXCOUNTER	100
+#define	BRTL8258_TXLPFBW		0xc
+#define	BRTL8258_RXLPFBW		0xc00
+#define	BRTL8258_RSSILPFBW		0xc0
+
+#define	BBYTE0				0x1
+#define	BBYTE1				0x2
+#define	BBYTE2				0x4
+#define	BBYTE3				0x8
+#define	BWORD0				0x3
+#define	BWORD1				0xc
+#define	BWORD				0xf
+
+#define	MASKBYTE0			0xff
+#define	MASKBYTE1			0xff00
+#define	MASKBYTE2			0xff0000
+#define	MASKBYTE3			0xff000000
+#define	MASKHWORD			0xffff0000
+#define	MASKLWORD			0x0000ffff
+#define	MASKDWORD			0xffffffff
+#define	MASK12BITS			0xfff
+#define	MASKH4BITS			0xf0000000
+#define MASKOFDM_D			0xffc00000
+#define	MASKCCK				0x3f3f3f3f
+
+#define	MASK4BITS			0x0f
+#define	MASK20BITS			0xfffff
+#define RFREG_OFFSET_MASK		0xfffff
+
+#define	BENABLE				0x1
+#define	BDISABLE			0x0
+
+#define	LEFT_ANTENNA			0x0
+#define	RIGHT_ANTENNA			0x1
+
+#define	TCHECK_TXSTATUS			500
+#define	TUPDATE_RXCOUNTER		100
 
 #define	REG_UN_used_register		0x01bf
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/rf.c b/drivers/net/wireless/rtlwifi/rtl8723be/rf.c
index 486294930a7b..5ed4492d3c80 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/rf.c
@@ -51,7 +51,7 @@ void rtl8723be_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 			      rtlphy->rfreg_chnlval[0]);
 		break;
 	default:
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_EMERG,
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "unknown bandwidth: %#X\n", bandwidth);
 		break;
 	}
@@ -93,18 +93,20 @@ void rtl8723be_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 				       (ppowerlevel[idx1] << 16) |
 				       (ppowerlevel[idx1] << 24);
 		}
+
 		if (rtlefuse->eeprom_regulatory == 0) {
 			tmpval =
-			    (rtlphy->mcs_offset[0][6]) +
-			    (rtlphy->mcs_offset[0][7] << 8);
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][6]) +
+			    (rtlphy->mcs_txpwrlevel_origoffset[0][7] << 8);
 			tx_agc[RF90_PATH_A] += tmpval;
 
-			tmpval = (rtlphy->mcs_offset[0][14]) +
-				 (rtlphy->mcs_offset[0][15] <<
+			tmpval = (rtlphy->mcs_txpwrlevel_origoffset[0][14]) +
+				 (rtlphy->mcs_txpwrlevel_origoffset[0][15] <<
 				  24);
 			tx_agc[RF90_PATH_B] += tmpval;
 		}
 	}
+
 	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
 		ptr = (u8 *)(&(tx_agc[idx1]));
 		for (idx2 = 0; idx2 < 4; idx2++) {
@@ -124,30 +126,32 @@ void rtl8723be_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 	tmpval = tx_agc[RF90_PATH_A] & 0xff;
 	rtl_set_bbreg(hw, RTXAGC_A_CCK1_MCS32, MASKBYTE1, tmpval);
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 1M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
 		 RTXAGC_A_CCK1_MCS32);
 
 	tmpval = tx_agc[RF90_PATH_A] >> 8;
 
+	/*tmpval = tmpval & 0xff00ffff;*/
+
 	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "CCK PWR 2~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 2~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
 		 RTXAGC_B_CCK11_A_CCK2_11);
 
 	tmpval = tx_agc[RF90_PATH_B] >> 24;
 	rtl_set_bbreg(hw, RTXAGC_B_CCK11_A_CCK2_11, MASKBYTE0, tmpval);
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
 		 RTXAGC_B_CCK11_A_CCK2_11);
 
 	tmpval = tx_agc[RF90_PATH_B] & 0x00ffffff;
 	rtl_set_bbreg(hw, RTXAGC_B_CCK1_55_MCS32, 0xffffff00, tmpval);
 
-	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-		 "CCK PWR 1~5.5M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+		"CCK PWR 1~5.5M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
 		 RTXAGC_B_CCK1_55_MCS32);
 }
 
@@ -169,8 +173,8 @@ static void rtl8723be_phy_get_power_base(struct ieee80211_hw *hw,
 		powerbase0 = (powerbase0 << 24) | (powerbase0 << 16) |
 		    (powerbase0 << 8) | powerbase0;
 		*(ofdmbase + i) = powerbase0;
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-			 " [OFDM power base index rf(%c) = 0x%x]\n",
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			" [OFDM power base index rf(%c) = 0x%x]\n",
 			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
 	}
 
@@ -179,27 +183,30 @@ static void rtl8723be_phy_get_power_base(struct ieee80211_hw *hw,
 			powerlevel[i] = ppowerlevel_bw20[i];
 		else
 			powerlevel[i] = ppowerlevel_bw40[i];
+
 		powerbase1 = powerlevel[i];
 		powerbase1 = (powerbase1 << 24) | (powerbase1 << 16) |
 			     (powerbase1 << 8) | powerbase1;
 
 		*(mcsbase + i) = powerbase1;
 
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 			" [MCS power base index rf(%c) = 0x%x]\n",
-			((i == 0) ? 'A' : 'B'), *(mcsbase + i));
+			 ((i == 0) ? 'A' : 'B'), *(mcsbase + i));
 	}
 }
 
-static void txpwr_by_regulatory(struct ieee80211_hw *hw, u8 channel, u8 index,
-				u32 *powerbase0, u32 *powerbase1,
-				u32 *p_outwriteval)
+static void _rtl8723be_get_txpower_writeval_by_regulatory(
+							struct ieee80211_hw *hw,
+							u8 channel, u8 index,
+							u32 *powerbase0,
+							u32 *powerbase1,
+							u32 *p_outwriteval)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 i, chnlgroup = 0, pwr_diff_limit[4];
-	u8 pwr_diff = 0, customer_pwr_diff;
+	u8 i, chnlgroup = 0, pwr_diff_limit[4], pwr_diff = 0, customer_pwr_diff;
 	u32 writeval, customer_limit, rf;
 
 	for (rf = 0; rf < 2; rf++) {
@@ -208,13 +215,13 @@ static void txpwr_by_regulatory(struct ieee80211_hw *hw, u8 channel, u8 index,
 			chnlgroup = 0;
 
 			writeval =
-			    rtlphy->mcs_offset[chnlgroup][index + (rf ? 8 : 0)]
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup][index +
+								(rf ? 8 : 0)]
 			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "RTK better performance, "
-				 "writeval(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeval);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"RTK better performance, writeval(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 1:
 			if (rtlphy->pwrgroup_cnt == 1) {
@@ -233,43 +240,41 @@ static void txpwr_by_regulatory(struct ieee80211_hw *hw, u8 channel, u8 index,
 				else if (channel == 14)
 					chnlgroup = 5;
 			}
-			writeval = rtlphy->mcs_offset[chnlgroup]
+
+			writeval =
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
 			    [index + (rf ? 8 : 0)] + ((index < 2) ?
 						      powerbase0[rf] :
 						      powerbase1[rf]);
 
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "Realtek regulatory, 20MHz, "
-				 "writeval(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeval);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"Realtek regulatory, 20MHz, writeval(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
 
 			break;
 		case 2:
 			writeval =
 			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "Better regulatory, "
-				 "writeval(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeval);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"Better regulatory, writeval(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 3:
 			chnlgroup = 0;
 
 			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-					 "customer's limit, 40MHz "
-					 "rf(%c) = 0x%x\n",
-					 ((rf == 0) ? 'A' : 'B'),
-					 rtlefuse->pwrgroup_ht40[rf]
-					 [channel-1]);
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					"customer's limit, 40MHz rf(%c) = 0x%x\n",
+					((rf == 0) ? 'A' : 'B'),
+					rtlefuse->pwrgroup_ht40
+					[rf][channel - 1]);
 			} else {
-				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-					 "customer's limit, 20MHz "
-					 "rf(%c) = 0x%x\n",
-					 ((rf == 0) ? 'A' : 'B'),
-					 rtlefuse->pwrgroup_ht20[rf]
-					 [channel-1]);
+				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+					"customer's limit, 20MHz rf(%c) = 0x%x\n",
+					((rf == 0) ? 'A' : 'B'),
+					rtlefuse->pwrgroup_ht20
+					[rf][channel - 1]);
 			}
 
 			if (index < 2)
@@ -294,9 +299,9 @@ static void txpwr_by_regulatory(struct ieee80211_hw *hw, u8 channel, u8 index,
 
 			for (i = 0; i < 4; i++) {
 				pwr_diff_limit[i] =
-				    (u8)((rtlphy->mcs_offset
-					  [chnlgroup][index + (rf ? 8 : 0)] &
-					  (0x7f << (i * 8))) >> (i * 8));
+				    (u8)((rtlphy->mcs_txpwrlevel_origoffset
+					   [chnlgroup][index + (rf ? 8 : 0)] &
+					      (0x7f << (i * 8))) >> (i * 8));
 
 					if (pwr_diff_limit[i] > pwr_diff)
 						pwr_diff_limit[i] = pwr_diff;
@@ -307,29 +312,28 @@ static void txpwr_by_regulatory(struct ieee80211_hw *hw, u8 channel, u8 index,
 					 (pwr_diff_limit[1] << 8) |
 					 (pwr_diff_limit[0]);
 
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
 				"Customer's limit rf(%c) = 0x%x\n",
-				((rf == 0) ? 'A' : 'B'), customer_limit);
+				 ((rf == 0) ? 'A' : 'B'), customer_limit);
 
 			writeval = customer_limit + ((index < 2) ?
 						      powerbase0[rf] :
 						      powerbase1[rf]);
 
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "Customer, writeval rf(%c)= 0x%x\n",
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"Customer, writeval rf(%c)= 0x%x\n",
 				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		default:
 			chnlgroup = 0;
 			writeval =
-			    rtlphy->mcs_offset[chnlgroup]
+			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
 			    [index + (rf ? 8 : 0)]
 			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
-			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-				 "RTK better performance, writeval "
-				 "rf(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeval);
+			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+				"RTK better performance, writeval rf(%c) = 0x%x\n",
+				((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		}
 
@@ -343,7 +347,7 @@ static void txpwr_by_regulatory(struct ieee80211_hw *hw, u8 channel, u8 index,
 }
 
 static void _rtl8723be_write_ofdm_power_reg(struct ieee80211_hw *hw,
-					 u8 index, u32 *value)
+					 u8 index, u32 *pvalue)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 regoffset_a[6] = {
@@ -361,9 +365,9 @@ static void _rtl8723be_write_ofdm_power_reg(struct ieee80211_hw *hw,
 	u16 regoffset;
 
 	for (rf = 0; rf < 2; rf++) {
-		writeval = value[rf];
+		writeval = pvalue[rf];
 		for (i = 0; i < 4; i++) {
-			pwr_val[i] = (u8) ((writeval & (0x7f <<
+			pwr_val[i] = (u8)((writeval & (0x7f <<
 							(i * 8))) >> (i * 8));
 
 			if (pwr_val[i] > RF6052_MAX_TX_PWR)
@@ -378,8 +382,8 @@ static void _rtl8723be_write_ofdm_power_reg(struct ieee80211_hw *hw,
 			regoffset = regoffset_b[index];
 		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeval);
 
-		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
-			 "Set 0x%x = %08x\n", regoffset, writeval);
+		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
+			"Set 0x%x = %08x\n", regoffset, writeval);
 	}
 }
 
@@ -400,8 +404,11 @@ void rtl8723be_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
 	rtl8723be_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
 
 	for (index = 0; index < 6; index++) {
-		txpwr_by_regulatory(hw, channel, index, &powerbase0[0],
-				    &powerbase1[0], &writeval[0]);
+		_rtl8723be_get_txpower_writeval_by_regulatory(hw,
+							      channel, index,
+							      &powerbase0[0],
+							      &powerbase1[0],
+							      &writeval[0]);
 		if (direction == 1) {
 			writeval[0] += pwrtrac_value;
 			writeval[1] += pwrtrac_value;
@@ -424,16 +431,17 @@ bool rtl8723be_phy_rf6052_config(struct ieee80211_hw *hw)
 		rtlphy->num_total_rfpath = 2;
 
 	return _rtl8723be_phy_rf6052_config_parafile(hw);
+
 }
 
 static bool _rtl8723be_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *pphyreg;
 	u32 u4_regvalue = 0;
 	u8 rfpath;
 	bool rtstatus = true;
+	struct bb_reg_def *pphyreg;
 
 	for (rfpath = 0; rfpath < rtlphy->num_total_rfpath; rfpath++) {
 		pphyreg = &rtlphy->phyreg_def[rfpath];
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/sw.c b/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
index 532913c6622a..223eb42992bd 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/sw.c
@@ -31,6 +31,7 @@
 #include "phy.h"
 #include "../rtl8723com/phy_common.h"
 #include "dm.h"
+#include "../rtl8723com/dm_common.h"
 #include "hw.h"
 #include "fw.h"
 #include "../rtl8723com/fw_common.h"
@@ -101,6 +102,8 @@ int rtl8723be_init_sw_vars(struct ieee80211_hw *hw)
 	rtlpriv->dm.thermalvalue = 0;
 	rtlpci->transmit_config = CFENDFORM | BIT(15) | BIT(24) | BIT(25);
 
+	rtlpriv->phy.lck_inprogress = false;
+
 	mac->ht_enable = true;
 
 	/* compatible 5G band 88ce just 2.4G band & smsp */
@@ -137,12 +140,19 @@ int rtl8723be_init_sw_vars(struct ieee80211_hw *hw)
 
 	rtlpci->irq_mask[1] = (u32)(IMR_RXFOVW | 0);
 
+	rtlpci->sys_irq_mask = (u32)(HSIMR_PDN_INT_EN	|
+				     HSIMR_RON_INT_EN	|
+				     0);
+
 	/* for debug level */
 	rtlpriv->dbg.global_debuglevel = rtlpriv->cfg->mod_params->debug;
 	/* for LPS & IPS */
 	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
 	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
 	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+	if (rtlpriv->cfg->mod_params->disable_watchdog)
+		pr_info("watchdog disabled\n");
 	rtlpriv->psc.reg_fwctrl_lps = 3;
 	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
 	/* for ASPM, you can close aspm through
@@ -157,6 +167,11 @@ int rtl8723be_init_sw_vars(struct ieee80211_hw *hw)
 	else if (rtlpriv->psc.reg_fwctrl_lps == 3)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
 
+	/*low power: Disable 32k */
+	rtlpriv->psc.low_power_enable = false;
+
+	rtlpriv->rtlhal.earlymode_enable = false;
+
 	/* for firmware buf */
 	rtlpriv->rtlhal.pfirmware = vzalloc(0x8000);
 	if (!rtlpriv->rtlhal.pfirmware) {
@@ -182,8 +197,6 @@ void rtl8723be_deinit_sw_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	if (rtlpriv->cfg->ops->get_btc_status())
-		rtlpriv->btcoexist.btc_ops->btc_halt_notify();
 	if (rtlpriv->rtlhal.pfirmware) {
 		vfree(rtlpriv->rtlhal.pfirmware);
 		rtlpriv->rtlhal.pfirmware = NULL;
@@ -196,7 +209,7 @@ bool rtl8723be_get_btc_status(void)
 	return true;
 }
 
-static bool is_fw_header(struct rtl92c_firmware_header *hdr)
+static bool is_fw_header(struct rtl8723e_firmware_header *hdr)
 {
 	return (hdr->signature & 0xfff0) == 0x5300;
 }
@@ -245,6 +258,7 @@ static struct rtl_hal_ops rtl8723be_hal_ops = {
 	.set_rfreg = rtl8723be_phy_set_rf_reg,
 	.fill_h2c_cmd = rtl8723be_fill_h2c_cmd,
 	.get_btc_status = rtl8723be_get_btc_status,
+	.rx_command_packet = rtl8723be_rx_command_packet,
 	.is_fw_header = is_fw_header,
 };
 
@@ -253,8 +267,6 @@ static struct rtl_mod_params rtl8723be_mod_params = {
 	.inactiveps = true,
 	.swctrl_lps = false,
 	.fwctrl_lps = true,
-	.msi_support = false,
-	.debug = DBG_EMERG,
 };
 
 static struct rtl_hal_cfg rtl8723be_hal_cfg = {
@@ -272,6 +284,9 @@ static struct rtl_hal_cfg rtl8723be_hal_cfg = {
 	.maps[MAC_RCR_ACRC32] = ACRC32,
 	.maps[MAC_RCR_ACF] = ACF,
 	.maps[MAC_RCR_AAP] = AAP,
+	.maps[MAC_HIMR] = REG_HIMR,
+	.maps[MAC_HIMRE] = REG_HIMRE,
+	.maps[MAC_HSISR] = REG_HSISR,
 
 	.maps[EFUSE_ACCESS] = REG_EFUSE_ACCESS,
 
@@ -305,6 +320,7 @@ static struct rtl_hal_cfg rtl8723be_hal_cfg = {
 	.maps[RTL_IMR_BCNDMAINT3] = IMR_BCNDMAINT3,
 	.maps[RTL_IMR_BCNDMAINT2] = IMR_BCNDMAINT2,
 	.maps[RTL_IMR_BCNDMAINT1] = IMR_BCNDMAINT1,
+/*	.maps[RTL_IMR_BCNDOK8] = IMR_BCNDOK8,     */   /*need check*/
 	.maps[RTL_IMR_BCNDOK7] = IMR_BCNDOK7,
 	.maps[RTL_IMR_BCNDOK6] = IMR_BCNDOK6,
 	.maps[RTL_IMR_BCNDOK5] = IMR_BCNDOK5,
@@ -312,6 +328,8 @@ static struct rtl_hal_cfg rtl8723be_hal_cfg = {
 	.maps[RTL_IMR_BCNDOK3] = IMR_BCNDOK3,
 	.maps[RTL_IMR_BCNDOK2] = IMR_BCNDOK2,
 	.maps[RTL_IMR_BCNDOK1] = IMR_BCNDOK1,
+/*	.maps[RTL_IMR_TIMEOUT2] = IMR_TIMEOUT2,*/
+/*	.maps[RTL_IMR_TIMEOUT1] = IMR_TIMEOUT1,*/
 
 	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
@@ -329,6 +347,7 @@ static struct rtl_hal_cfg rtl8723be_hal_cfg = {
 	.maps[RTL_IMR_VIDOK] = IMR_VIDOK,
 	.maps[RTL_IMR_VODOK] = IMR_VODOK,
 	.maps[RTL_IMR_ROK] = IMR_ROK,
+	.maps[RTL_IMR_HSISR_IND] = IMR_HSISR_IND_ON_INT,
 	.maps[RTL_IBSS_INT_MASKS] = (IMR_BCNDMAINT0 | IMR_TBDOK | IMR_TBDER),
 
 	.maps[RTL_RC_CCK_RATE1M] = DESC92C_RATE1M,
@@ -348,12 +367,12 @@ static struct rtl_hal_cfg rtl8723be_hal_cfg = {
 	.maps[RTL_RC_HT_RATEMCS15] = DESC92C_RATEMCS15,
 };
 
-static const struct pci_device_id rtl8723be_pci_id[] = {
-	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0xb723, rtl8723be_hal_cfg)},
+static struct pci_device_id rtl8723be_pci_ids[] = {
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0xB723, rtl8723be_hal_cfg)},
 	{},
 };
 
-MODULE_DEVICE_TABLE(pci, rtl8723be_pci_id);
+MODULE_DEVICE_TABLE(pci, rtl8723be_pci_ids);
 
 MODULE_AUTHOR("PageHe	<page_he@realsil.com.cn>");
 MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
@@ -366,21 +385,22 @@ module_param_named(debug, rtl8723be_mod_params.debug, int, 0444);
 module_param_named(ips, rtl8723be_mod_params.inactiveps, bool, 0444);
 module_param_named(swlps, rtl8723be_mod_params.swctrl_lps, bool, 0444);
 module_param_named(fwlps, rtl8723be_mod_params.fwctrl_lps, bool, 0444);
-module_param_named(msi, rtl8723be_mod_params.msi_support, bool, 0444);
+module_param_named(disable_watchdog, rtl8723be_mod_params.disable_watchdog,
+		   bool, 0444);
 MODULE_PARM_DESC(swenc, "using hardware crypto (default 0 [hardware])\n");
 MODULE_PARM_DESC(ips, "using no link power save (default 1 is open)\n");
 MODULE_PARM_DESC(fwlps, "using linked fw control power save (default 1 is open)\n");
 MODULE_PARM_DESC(msi, "Set to 1 to use MSI interrupts mode (default 0)\n");
 MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+MODULE_PARM_DESC(disable_watchdog, "Set to 1 to disable the watchdog (default 0)\n");
 
 static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
 static struct pci_driver rtl8723be_driver = {
 	.name = KBUILD_MODNAME,
-	.id_table = rtl8723be_pci_id,
+	.id_table = rtl8723be_pci_ids,
 	.probe = rtl_pci_probe,
 	.remove = rtl_pci_disconnect,
-
 	.driver.pm = &rtlwifi_pm_ops,
 };
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/table.c b/drivers/net/wireless/rtlwifi/rtl8723be/table.c
index 4b283cde042e..a180761e8810 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/table.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/table.c
@@ -27,200 +27,201 @@
 
 #include "table.h"
 u32 RTL8723BEPHY_REG_1TARRAY[] = {
-	0x800, 0x80040000,
-	0x804, 0x00000003,
-	0x808, 0x0000FC00,
-	0x80C, 0x0000000A,
-	0x810, 0x10001331,
-	0x814, 0x020C3D10,
-	0x818, 0x02200385,
-	0x81C, 0x00000000,
-	0x820, 0x01000100,
-	0x824, 0x00390204,
-	0x828, 0x00000000,
-	0x82C, 0x00000000,
-	0x830, 0x00000000,
-	0x834, 0x00000000,
-	0x838, 0x00000000,
-	0x83C, 0x00000000,
-	0x840, 0x00010000,
-	0x844, 0x00000000,
-	0x848, 0x00000000,
-	0x84C, 0x00000000,
-	0x850, 0x00000000,
-	0x854, 0x00000000,
-	0x858, 0x569A11A9,
-	0x85C, 0x01000014,
-	0x860, 0x66F60110,
-	0x864, 0x061F0649,
-	0x868, 0x00000000,
-	0x86C, 0x27272700,
-	0x870, 0x07000760,
-	0x874, 0x25004000,
-	0x878, 0x00000808,
-	0x87C, 0x00000000,
-	0x880, 0xB0000C1C,
-	0x884, 0x00000001,
-	0x888, 0x00000000,
-	0x88C, 0xCCC000C0,
-	0x890, 0x00000800,
-	0x894, 0xFFFFFFFE,
-	0x898, 0x40302010,
-	0x89C, 0x00706050,
-	0x900, 0x00000000,
-	0x904, 0x00000023,
-	0x908, 0x00000000,
-	0x90C, 0x81121111,
-	0x910, 0x00000002,
-	0x914, 0x00000201,
-	0x948, 0x00000000,
-	0xA00, 0x00D047C8,
-	0xA04, 0x80FF000C,
-	0xA08, 0x8C838300,
-	0xA0C, 0x2E7F120F,
-	0xA10, 0x9500BB78,
-	0xA14, 0x1114D028,
-	0xA18, 0x00881117,
-	0xA1C, 0x89140F00,
-	0xA20, 0x1A1B0000,
-	0xA24, 0x090E1317,
-	0xA28, 0x00000204,
-	0xA2C, 0x00D30000,
-	0xA70, 0x101FBF00,
-	0xA74, 0x00000007,
-	0xA78, 0x00000900,
-	0xA7C, 0x225B0606,
-	0xA80, 0x21806490,
-	0xB2C, 0x00000000,
-	0xC00, 0x48071D40,
-	0xC04, 0x03A05611,
-	0xC08, 0x000000E4,
-	0xC0C, 0x6C6C6C6C,
-	0xC10, 0x08800000,
-	0xC14, 0x40000100,
-	0xC18, 0x08800000,
-	0xC1C, 0x40000100,
-	0xC20, 0x00000000,
-	0xC24, 0x00000000,
-	0xC28, 0x00000000,
-	0xC2C, 0x00000000,
-	0xC30, 0x69E9AC44,
-	0xC34, 0x469652AF,
-	0xC38, 0x49795994,
-	0xC3C, 0x0A97971C,
-	0xC40, 0x1F7C403F,
-	0xC44, 0x000100B7,
-	0xC48, 0xEC020107,
-	0xC4C, 0x007F037F,
-	0xC50, 0x69553420,
-	0xC54, 0x43BC0094,
-	0xC58, 0x00023169,
-	0xC5C, 0x00250492,
-	0xC60, 0x00000000,
-	0xC64, 0x7112848B,
-	0xC68, 0x47C00BFF,
-	0xC6C, 0x00000036,
-	0xC70, 0x2C7F000D,
-	0xC74, 0x020610DB,
-	0xC78, 0x0000001F,
-	0xC7C, 0x00B91612,
-	0xC80, 0x390000E4,
-	0xC84, 0x20F60000,
-	0xC88, 0x40000100,
-	0xC8C, 0x20200000,
-	0xC90, 0x00020E1A,
-	0xC94, 0x00000000,
-	0xC98, 0x00020E1A,
-	0xC9C, 0x00007F7F,
-	0xCA0, 0x00000000,
-	0xCA4, 0x000300A0,
-	0xCA8, 0x00000000,
-	0xCAC, 0x00000000,
-	0xCB0, 0x00000000,
-	0xCB4, 0x00000000,
-	0xCB8, 0x00000000,
-	0xCBC, 0x28000000,
-	0xCC0, 0x00000000,
-	0xCC4, 0x00000000,
-	0xCC8, 0x00000000,
-	0xCCC, 0x00000000,
-	0xCD0, 0x00000000,
-	0xCD4, 0x00000000,
-	0xCD8, 0x64B22427,
-	0xCDC, 0x00766932,
-	0xCE0, 0x00222222,
-	0xCE4, 0x00000000,
-	0xCE8, 0x37644302,
-	0xCEC, 0x2F97D40C,
-	0xD00, 0x00000740,
-	0xD04, 0x40020401,
-	0xD08, 0x0000907F,
-	0xD0C, 0x20010201,
-	0xD10, 0xA0633333,
-	0xD14, 0x3333BC53,
-	0xD18, 0x7A8F5B6F,
-	0xD2C, 0xCC979975,
-	0xD30, 0x00000000,
-	0xD34, 0x80608000,
-	0xD38, 0x00000000,
-	0xD3C, 0x00127353,
-	0xD40, 0x00000000,
-	0xD44, 0x00000000,
-	0xD48, 0x00000000,
-	0xD4C, 0x00000000,
-	0xD50, 0x6437140A,
-	0xD54, 0x00000000,
-	0xD58, 0x00000282,
-	0xD5C, 0x30032064,
-	0xD60, 0x4653DE68,
-	0xD64, 0x04518A3C,
-	0xD68, 0x00002101,
-	0xD6C, 0x2A201C16,
-	0xD70, 0x1812362E,
-	0xD74, 0x322C2220,
-	0xD78, 0x000E3C24,
-	0xE00, 0x2D2D2D2D,
-	0xE04, 0x2D2D2D2D,
-	0xE08, 0x0390272D,
-	0xE10, 0x2D2D2D2D,
-	0xE14, 0x2D2D2D2D,
-	0xE18, 0x2D2D2D2D,
-	0xE1C, 0x2D2D2D2D,
-	0xE28, 0x00000000,
-	0xE30, 0x1000DC1F,
-	0xE34, 0x10008C1F,
-	0xE38, 0x02140102,
-	0xE3C, 0x681604C2,
-	0xE40, 0x01007C00,
-	0xE44, 0x01004800,
-	0xE48, 0xFB000000,
-	0xE4C, 0x000028D1,
-	0xE50, 0x1000DC1F,
-	0xE54, 0x10008C1F,
-	0xE58, 0x02140102,
-	0xE5C, 0x28160D05,
-	0xE60, 0x00000008,
-	0xE68, 0x001B2556,
-	0xE6C, 0x00C00096,
-	0xE70, 0x00C00096,
-	0xE74, 0x01000056,
-	0xE78, 0x01000014,
-	0xE7C, 0x01000056,
-	0xE80, 0x01000014,
-	0xE84, 0x00C00096,
-	0xE88, 0x01000056,
-	0xE8C, 0x00C00096,
-	0xED0, 0x00C00096,
-	0xED4, 0x00C00096,
-	0xED8, 0x00C00096,
-	0xEDC, 0x000000D6,
-	0xEE0, 0x000000D6,
-	0xEEC, 0x01C00016,
-	0xF14, 0x00000003,
-	0xF4C, 0x00000000,
-	0xF00, 0x00000300,
-	0x820, 0x01000100,
-	0x800, 0x83040000,
+		0x800, 0x80040000,
+		0x804, 0x00000003,
+		0x808, 0x0000FC00,
+		0x80C, 0x0000000A,
+		0x810, 0x10001331,
+		0x814, 0x020C3D10,
+		0x818, 0x02200385,
+		0x81C, 0x00000000,
+		0x820, 0x01000100,
+		0x824, 0x00390204,
+		0x828, 0x00000000,
+		0x82C, 0x00000000,
+		0x830, 0x00000000,
+		0x834, 0x00000000,
+		0x838, 0x00000000,
+		0x83C, 0x00000000,
+		0x840, 0x00010000,
+		0x844, 0x00000000,
+		0x848, 0x00000000,
+		0x84C, 0x00000000,
+		0x850, 0x00000000,
+		0x854, 0x00000000,
+		0x858, 0x569A11A9,
+		0x85C, 0x01000014,
+		0x860, 0x66F60110,
+		0x864, 0x061F0649,
+		0x868, 0x00000000,
+		0x86C, 0x27272700,
+		0x870, 0x07000760,
+		0x874, 0x25004000,
+		0x878, 0x00000808,
+		0x87C, 0x00000000,
+		0x880, 0xB0000C1C,
+		0x884, 0x00000001,
+		0x888, 0x00000000,
+		0x88C, 0xCCC000C0,
+		0x890, 0x00000800,
+		0x894, 0xFFFFFFFE,
+		0x898, 0x40302010,
+		0x89C, 0x00706050,
+		0x900, 0x00000000,
+		0x904, 0x00000023,
+		0x908, 0x00000000,
+		0x90C, 0x81121111,
+		0x910, 0x00000002,
+		0x914, 0x00000201,
+		0x948, 0x00000280,
+		0xA00, 0x00D047C8,
+		0xA04, 0x80FF000C,
+		0xA08, 0x8C838300,
+		0xA0C, 0x2E7F120F,
+		0xA10, 0x9500BB78,
+		0xA14, 0x1114D028,
+		0xA18, 0x00881117,
+		0xA1C, 0x89140F00,
+		0xA20, 0x1A1B0000,
+		0xA24, 0x090E1317,
+		0xA28, 0x00000204,
+		0xA2C, 0x00D30000,
+		0xA70, 0x101FBF00,
+		0xA74, 0x00000007,
+		0xA78, 0x00000900,
+		0xA7C, 0x225B0606,
+		0xA80, 0x21806490,
+		0xB2C, 0x00000000,
+		0xC00, 0x48071D40,
+		0xC04, 0x03A05611,
+		0xC08, 0x000000E4,
+		0xC0C, 0x6C6C6C6C,
+		0xC10, 0x08800000,
+		0xC14, 0x40000100,
+		0xC18, 0x08800000,
+		0xC1C, 0x40000100,
+		0xC20, 0x00000000,
+		0xC24, 0x00000000,
+		0xC28, 0x00000000,
+		0xC2C, 0x00000000,
+		0xC30, 0x69E9AC44,
+		0xC34, 0x469652AF,
+		0xC38, 0x49795994,
+		0xC3C, 0x0A97971C,
+		0xC40, 0x1F7C403F,
+		0xC44, 0x000100B7,
+		0xC48, 0xEC020107,
+		0xC4C, 0x007F037F,
+		0xC50, 0x69553420,
+		0xC54, 0x43BC0094,
+		0xC58, 0x00023169,
+		0xC5C, 0x00250492,
+		0xC60, 0x00000000,
+		0xC64, 0x7112848B,
+		0xC68, 0x47C00BFF,
+		0xC6C, 0x00000036,
+		0xC70, 0x2C7F000D,
+		0xC74, 0x020610DB,
+		0xC78, 0x0000001F,
+		0xC7C, 0x00B91612,
+		0xC80, 0x390000E4,
+		0xC84, 0x20F60000,
+		0xC88, 0x40000100,
+		0xC8C, 0x20200000,
+		0xC90, 0x00020E1A,
+		0xC94, 0x00000000,
+		0xC98, 0x00020E1A,
+		0xC9C, 0x00007F7F,
+		0xCA0, 0x00000000,
+		0xCA4, 0x000300A0,
+		0xCA8, 0x00000000,
+		0xCAC, 0x00000000,
+		0xCB0, 0x00000000,
+		0xCB4, 0x00000000,
+		0xCB8, 0x00000000,
+		0xCBC, 0x28000000,
+		0xCC0, 0x00000000,
+		0xCC4, 0x00000000,
+		0xCC8, 0x00000000,
+		0xCCC, 0x00000000,
+		0xCD0, 0x00000000,
+		0xCD4, 0x00000000,
+		0xCD8, 0x64B22427,
+		0xCDC, 0x00766932,
+		0xCE0, 0x00222222,
+		0xCE4, 0x00000000,
+		0xCE8, 0x37644302,
+		0xCEC, 0x2F97D40C,
+		0xD00, 0x00000740,
+		0xD04, 0x40020401,
+		0xD08, 0x0000907F,
+		0xD0C, 0x20010201,
+		0xD10, 0xA0633333,
+		0xD14, 0x3333BC53,
+		0xD18, 0x7A8F5B6F,
+		0xD2C, 0xCC979975,
+		0xD30, 0x00000000,
+		0xD34, 0x80608000,
+		0xD38, 0x00000000,
+		0xD3C, 0x00127353,
+		0xD40, 0x00000000,
+		0xD44, 0x00000000,
+		0xD48, 0x00000000,
+		0xD4C, 0x00000000,
+		0xD50, 0x6437140A,
+		0xD54, 0x00000000,
+		0xD58, 0x00000282,
+		0xD5C, 0x30032064,
+		0xD60, 0x4653DE68,
+		0xD64, 0x04518A3C,
+		0xD68, 0x00002101,
+		0xD6C, 0x2A201C16,
+		0xD70, 0x1812362E,
+		0xD74, 0x322C2220,
+		0xD78, 0x000E3C24,
+		0xE00, 0x2D2D2D2D,
+		0xE04, 0x2D2D2D2D,
+		0xE08, 0x0390272D,
+		0xE10, 0x2D2D2D2D,
+		0xE14, 0x2D2D2D2D,
+		0xE18, 0x2D2D2D2D,
+		0xE1C, 0x2D2D2D2D,
+		0xE28, 0x00000000,
+		0xE30, 0x1000DC1F,
+		0xE34, 0x10008C1F,
+		0xE38, 0x02140102,
+		0xE3C, 0x681604C2,
+		0xE40, 0x01007C00,
+		0xE44, 0x01004800,
+		0xE48, 0xFB000000,
+		0xE4C, 0x000028D1,
+		0xE50, 0x1000DC1F,
+		0xE54, 0x10008C1F,
+		0xE58, 0x02140102,
+		0xE5C, 0x28160D05,
+		0xE60, 0x00000008,
+		0xE68, 0x001B2556,
+		0xE6C, 0x00C00096,
+		0xE70, 0x00C00096,
+		0xE74, 0x01000056,
+		0xE78, 0x01000014,
+		0xE7C, 0x01000056,
+		0xE80, 0x01000014,
+		0xE84, 0x00C00096,
+		0xE88, 0x01000056,
+		0xE8C, 0x00C00096,
+		0xED0, 0x00C00096,
+		0xED4, 0x00C00096,
+		0xED8, 0x00C00096,
+		0xEDC, 0x000000D6,
+		0xEE0, 0x000000D6,
+		0xEEC, 0x01C00016,
+		0xF14, 0x00000003,
+		0xF4C, 0x00000000,
+		0xF00, 0x00000300,
+		0x820, 0x01000100,
+		0x800, 0x83040000,
+
 };
 
 u32 RTL8723BEPHY_REG_ARRAY_PG[] = {
@@ -233,340 +234,344 @@ u32 RTL8723BEPHY_REG_ARRAY_PG[] = {
 };
 
 u32 RTL8723BE_RADIOA_1TARRAY[] = {
-	0x000, 0x00010000,
-	0x0B0, 0x000DFFE0,
-	0x0FE, 0x00000000,
-	0x0FE, 0x00000000,
-	0x0FE, 0x00000000,
-	0x0B1, 0x00000018,
-	0x0FE, 0x00000000,
-	0x0FE, 0x00000000,
-	0x0FE, 0x00000000,
-	0x0B2, 0x00084C00,
-	0x0B5, 0x0000D2CC,
-	0x0B6, 0x000925AA,
-	0x0B7, 0x00000010,
-	0x0B8, 0x0000907F,
-	0x05C, 0x00000002,
-	0x07C, 0x00000002,
-	0x07E, 0x00000005,
-	0x08B, 0x0006FC00,
-	0x0B0, 0x000FF9F0,
-	0x01C, 0x000739D2,
-	0x01E, 0x00000000,
-	0x0DF, 0x00000780,
-	0x050, 0x00067435,
-	0x051, 0x0006B04E,
-	0x052, 0x000007D2,
-	0x053, 0x00000000,
-	0x054, 0x00050400,
-	0x055, 0x0004026E,
-	0x0DD, 0x0000004C,
-	0x070, 0x00067435,
-	0x071, 0x0006B04E,
-	0x072, 0x000007D2,
-	0x073, 0x00000000,
-	0x074, 0x00050400,
-	0x075, 0x0004026E,
-	0x0EF, 0x00000100,
-	0x034, 0x0000ADD7,
-	0x035, 0x00005C00,
-	0x034, 0x00009DD4,
-	0x035, 0x00005000,
-	0x034, 0x00008DD1,
-	0x035, 0x00004400,
-	0x034, 0x00007DCE,
-	0x035, 0x00003800,
-	0x034, 0x00006CD1,
-	0x035, 0x00004400,
-	0x034, 0x00005CCE,
-	0x035, 0x00003800,
-	0x034, 0x000048CE,
-	0x035, 0x00004400,
-	0x034, 0x000034CE,
-	0x035, 0x00003800,
-	0x034, 0x00002451,
-	0x035, 0x00004400,
-	0x034, 0x0000144E,
-	0x035, 0x00003800,
-	0x034, 0x00000051,
-	0x035, 0x00004400,
-	0x0EF, 0x00000000,
-	0x0EF, 0x00000100,
-	0x0ED, 0x00000010,
-	0x044, 0x0000ADD7,
-	0x044, 0x00009DD4,
-	0x044, 0x00008DD1,
-	0x044, 0x00007DCE,
-	0x044, 0x00006CC1,
-	0x044, 0x00005CCE,
-	0x044, 0x000044D1,
-	0x044, 0x000034CE,
-	0x044, 0x00002451,
-	0x044, 0x0000144E,
-	0x044, 0x00000051,
-	0x0EF, 0x00000000,
-	0x0ED, 0x00000000,
-	0x0EF, 0x00002000,
-	0x03B, 0x000380EF,
-	0x03B, 0x000302FE,
-	0x03B, 0x00028CE6,
-	0x03B, 0x000200BC,
-	0x03B, 0x000188A5,
-	0x03B, 0x00010FBC,
-	0x03B, 0x00008F71,
-	0x03B, 0x00000900,
-	0x0EF, 0x00000000,
-	0x0ED, 0x00000001,
-	0x040, 0x000380EF,
-	0x040, 0x000302FE,
-	0x040, 0x00028CE6,
-	0x040, 0x000200BC,
-	0x040, 0x000188A5,
-	0x040, 0x00010FBC,
-	0x040, 0x00008F71,
-	0x040, 0x00000900,
-	0x0ED, 0x00000000,
-	0x082, 0x00080000,
-	0x083, 0x00008000,
-	0x084, 0x00048D80,
-	0x085, 0x00068000,
-	0x0A2, 0x00080000,
-	0x0A3, 0x00008000,
-	0x0A4, 0x00048D80,
-	0x0A5, 0x00068000,
-	0x000, 0x00033D80,
+		0x000, 0x00010000,
+		0x0B0, 0x000DFFE0,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0B1, 0x00000018,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0FE, 0x00000000,
+		0x0B2, 0x00084C00,
+		0x0B5, 0x0000D2CC,
+		0x0B6, 0x000925AA,
+		0x0B7, 0x00000010,
+		0x0B8, 0x0000907F,
+		0x05C, 0x00000002,
+		0x07C, 0x00000002,
+		0x07E, 0x00000005,
+		0x08B, 0x0006FC00,
+		0x0B0, 0x000FF9F0,
+		0x01C, 0x000739D2,
+		0x01E, 0x00000000,
+		0x0DF, 0x00000780,
+		0x050, 0x00067435,
+		0x051, 0x0006B04E,
+		0x052, 0x000007D2,
+		0x053, 0x00000000,
+		0x054, 0x00050400,
+		0x055, 0x0004026E,
+		0x0DD, 0x0000004C,
+		0x070, 0x00067435,
+		0x071, 0x0006B04E,
+		0x072, 0x000007D2,
+		0x073, 0x00000000,
+		0x074, 0x00050400,
+		0x075, 0x0004026E,
+		0x0EF, 0x00000100,
+		0x034, 0x0000ADD7,
+		0x035, 0x00005C00,
+		0x034, 0x00009DD4,
+		0x035, 0x00005000,
+		0x034, 0x00008DD1,
+		0x035, 0x00004400,
+		0x034, 0x00007DCE,
+		0x035, 0x00003800,
+		0x034, 0x00006CD1,
+		0x035, 0x00004400,
+		0x034, 0x00005CCE,
+		0x035, 0x00003800,
+		0x034, 0x000048CE,
+		0x035, 0x00004400,
+		0x034, 0x000034CE,
+		0x035, 0x00003800,
+		0x034, 0x00002451,
+		0x035, 0x00004400,
+		0x034, 0x0000144E,
+		0x035, 0x00003800,
+		0x034, 0x00000051,
+		0x035, 0x00004400,
+		0x0EF, 0x00000000,
+		0x0EF, 0x00000100,
+		0x0ED, 0x00000010,
+		0x044, 0x0000ADD7,
+		0x044, 0x00009DD4,
+		0x044, 0x00008DD1,
+		0x044, 0x00007DCE,
+		0x044, 0x00006CC1,
+		0x044, 0x00005CCE,
+		0x044, 0x000044D1,
+		0x044, 0x000034CE,
+		0x044, 0x00002451,
+		0x044, 0x0000144E,
+		0x044, 0x00000051,
+		0x0EF, 0x00000000,
+		0x0ED, 0x00000000,
+		0x0EF, 0x00002000,
+		0x03B, 0x000380EF,
+		0x03B, 0x000302FE,
+		0x03B, 0x00028CE6,
+		0x03B, 0x000200BC,
+		0x03B, 0x000188A5,
+		0x03B, 0x00010FBC,
+		0x03B, 0x00008F71,
+		0x03B, 0x00000900,
+		0x0EF, 0x00000000,
+		0x0ED, 0x00000001,
+		0x040, 0x000380EF,
+		0x040, 0x000302FE,
+		0x040, 0x00028CE6,
+		0x040, 0x000200BC,
+		0x040, 0x000188A5,
+		0x040, 0x00010FBC,
+		0x040, 0x00008F71,
+		0x040, 0x00000900,
+		0x0ED, 0x00000000,
+		0x082, 0x00080000,
+		0x083, 0x00008000,
+		0x084, 0x00048D80,
+		0x085, 0x00068000,
+		0x0A2, 0x00080000,
+		0x0A3, 0x00008000,
+		0x0A4, 0x00048D80,
+		0x0A5, 0x00068000,
+		0x000, 0x00033D80,
+
 };
 
 u32 RTL8723BEMAC_1T_ARRAY[] = {
-	0x02F, 0x00000030,
-	0x035, 0x00000000,
-	0x428, 0x0000000A,
-	0x429, 0x00000010,
-	0x430, 0x00000000,
-	0x431, 0x00000000,
-	0x432, 0x00000000,
-	0x433, 0x00000001,
-	0x434, 0x00000004,
-	0x435, 0x00000005,
-	0x436, 0x00000007,
-	0x437, 0x00000008,
-	0x43C, 0x00000004,
-	0x43D, 0x00000005,
-	0x43E, 0x00000007,
-	0x43F, 0x00000008,
-	0x440, 0x0000005D,
-	0x441, 0x00000001,
-	0x442, 0x00000000,
-	0x444, 0x00000010,
-	0x445, 0x00000000,
-	0x446, 0x00000000,
-	0x447, 0x00000000,
-	0x448, 0x00000000,
-	0x449, 0x000000F0,
-	0x44A, 0x0000000F,
-	0x44B, 0x0000003E,
-	0x44C, 0x00000010,
-	0x44D, 0x00000000,
-	0x44E, 0x00000000,
-	0x44F, 0x00000000,
-	0x450, 0x00000000,
-	0x451, 0x000000F0,
-	0x452, 0x0000000F,
-	0x453, 0x00000000,
-	0x456, 0x0000005E,
-	0x460, 0x00000066,
-	0x461, 0x00000066,
-	0x4C8, 0x000000FF,
-	0x4C9, 0x00000008,
-	0x4CC, 0x000000FF,
-	0x4CD, 0x000000FF,
-	0x4CE, 0x00000001,
-	0x500, 0x00000026,
-	0x501, 0x000000A2,
-	0x502, 0x0000002F,
-	0x503, 0x00000000,
-	0x504, 0x00000028,
-	0x505, 0x000000A3,
-	0x506, 0x0000005E,
-	0x507, 0x00000000,
-	0x508, 0x0000002B,
-	0x509, 0x000000A4,
-	0x50A, 0x0000005E,
-	0x50B, 0x00000000,
-	0x50C, 0x0000004F,
-	0x50D, 0x000000A4,
-	0x50E, 0x00000000,
-	0x50F, 0x00000000,
-	0x512, 0x0000001C,
-	0x514, 0x0000000A,
-	0x516, 0x0000000A,
-	0x525, 0x0000004F,
-	0x550, 0x00000010,
-	0x551, 0x00000010,
-	0x559, 0x00000002,
-	0x55C, 0x00000050,
-	0x55D, 0x000000FF,
-	0x605, 0x00000030,
-	0x608, 0x0000000E,
-	0x609, 0x0000002A,
-	0x620, 0x000000FF,
-	0x621, 0x000000FF,
-	0x622, 0x000000FF,
-	0x623, 0x000000FF,
-	0x624, 0x000000FF,
-	0x625, 0x000000FF,
-	0x626, 0x000000FF,
-	0x627, 0x000000FF,
-	0x638, 0x00000050,
-	0x63C, 0x0000000A,
-	0x63D, 0x0000000A,
-	0x63E, 0x0000000E,
-	0x63F, 0x0000000E,
-	0x640, 0x00000040,
-	0x642, 0x00000040,
-	0x643, 0x00000000,
-	0x652, 0x000000C8,
-	0x66E, 0x00000005,
-	0x700, 0x00000021,
-	0x701, 0x00000043,
-	0x702, 0x00000065,
-	0x703, 0x00000087,
-	0x708, 0x00000021,
-	0x709, 0x00000043,
-	0x70A, 0x00000065,
-	0x70B, 0x00000087,
+		0x02F, 0x00000030,
+		0x035, 0x00000000,
+		0x067, 0x00000020,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000000,
+		0x432, 0x00000000,
+		0x433, 0x00000001,
+		0x434, 0x00000004,
+		0x435, 0x00000005,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000007,
+		0x43F, 0x00000008,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000010,
+		0x445, 0x00000000,
+		0x446, 0x00000000,
+		0x447, 0x00000000,
+		0x448, 0x00000000,
+		0x449, 0x000000F0,
+		0x44A, 0x0000000F,
+		0x44B, 0x0000003E,
+		0x44C, 0x00000010,
+		0x44D, 0x00000000,
+		0x44E, 0x00000000,
+		0x44F, 0x00000000,
+		0x450, 0x00000000,
+		0x451, 0x000000F0,
+		0x452, 0x0000000F,
+		0x453, 0x00000000,
+		0x456, 0x0000005E,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55C, 0x00000050,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x608, 0x0000000E,
+		0x609, 0x0000002A,
+		0x620, 0x000000FF,
+		0x621, 0x000000FF,
+		0x622, 0x000000FF,
+		0x623, 0x000000FF,
+		0x624, 0x000000FF,
+		0x625, 0x000000FF,
+		0x626, 0x000000FF,
+		0x627, 0x000000FF,
+		0x638, 0x00000050,
+		0x63C, 0x0000000A,
+		0x63D, 0x0000000A,
+		0x63E, 0x0000000E,
+		0x63F, 0x0000000E,
+		0x640, 0x00000040,
+		0x642, 0x00000040,
+		0x643, 0x00000000,
+		0x652, 0x000000C8,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+
 };
 
 u32 RTL8723BEAGCTAB_1TARRAY[] = {
-	0xC78, 0xFD000001,
-	0xC78, 0xFC010001,
-	0xC78, 0xFB020001,
-	0xC78, 0xFA030001,
-	0xC78, 0xF9040001,
-	0xC78, 0xF8050001,
-	0xC78, 0xF7060001,
-	0xC78, 0xF6070001,
-	0xC78, 0xF5080001,
-	0xC78, 0xF4090001,
-	0xC78, 0xF30A0001,
-	0xC78, 0xF20B0001,
-	0xC78, 0xF10C0001,
-	0xC78, 0xF00D0001,
-	0xC78, 0xEF0E0001,
-	0xC78, 0xEE0F0001,
-	0xC78, 0xED100001,
-	0xC78, 0xEC110001,
-	0xC78, 0xEB120001,
-	0xC78, 0xEA130001,
-	0xC78, 0xE9140001,
-	0xC78, 0xE8150001,
-	0xC78, 0xE7160001,
-	0xC78, 0xAA170001,
-	0xC78, 0xA9180001,
-	0xC78, 0xA8190001,
-	0xC78, 0xA71A0001,
-	0xC78, 0xA61B0001,
-	0xC78, 0xA51C0001,
-	0xC78, 0xA41D0001,
-	0xC78, 0xA31E0001,
-	0xC78, 0x671F0001,
-	0xC78, 0x66200001,
-	0xC78, 0x65210001,
-	0xC78, 0x64220001,
-	0xC78, 0x63230001,
-	0xC78, 0x62240001,
-	0xC78, 0x61250001,
-	0xC78, 0x47260001,
-	0xC78, 0x46270001,
-	0xC78, 0x45280001,
-	0xC78, 0x44290001,
-	0xC78, 0x432A0001,
-	0xC78, 0x422B0001,
-	0xC78, 0x292C0001,
-	0xC78, 0x282D0001,
-	0xC78, 0x272E0001,
-	0xC78, 0x262F0001,
-	0xC78, 0x25300001,
-	0xC78, 0x24310001,
-	0xC78, 0x09320001,
-	0xC78, 0x08330001,
-	0xC78, 0x07340001,
-	0xC78, 0x06350001,
-	0xC78, 0x05360001,
-	0xC78, 0x04370001,
-	0xC78, 0x03380001,
-	0xC78, 0x02390001,
-	0xC78, 0x013A0001,
-	0xC78, 0x003B0001,
-	0xC78, 0x003C0001,
-	0xC78, 0x003D0001,
-	0xC78, 0x003E0001,
-	0xC78, 0x003F0001,
-	0xC78, 0xFC400001,
-	0xC78, 0xFB410001,
-	0xC78, 0xFA420001,
-	0xC78, 0xF9430001,
-	0xC78, 0xF8440001,
-	0xC78, 0xF7450001,
-	0xC78, 0xF6460001,
-	0xC78, 0xF5470001,
-	0xC78, 0xF4480001,
-	0xC78, 0xF3490001,
-	0xC78, 0xF24A0001,
-	0xC78, 0xF14B0001,
-	0xC78, 0xF04C0001,
-	0xC78, 0xEF4D0001,
-	0xC78, 0xEE4E0001,
-	0xC78, 0xED4F0001,
-	0xC78, 0xEC500001,
-	0xC78, 0xEB510001,
-	0xC78, 0xEA520001,
-	0xC78, 0xE9530001,
-	0xC78, 0xE8540001,
-	0xC78, 0xE7550001,
-	0xC78, 0xE6560001,
-	0xC78, 0xE5570001,
-	0xC78, 0xAA580001,
-	0xC78, 0xA9590001,
-	0xC78, 0xA85A0001,
-	0xC78, 0xA75B0001,
-	0xC78, 0xA65C0001,
-	0xC78, 0xA55D0001,
-	0xC78, 0xA45E0001,
-	0xC78, 0x675F0001,
-	0xC78, 0x66600001,
-	0xC78, 0x65610001,
-	0xC78, 0x64620001,
-	0xC78, 0x63630001,
-	0xC78, 0x62640001,
-	0xC78, 0x61650001,
-	0xC78, 0x47660001,
-	0xC78, 0x46670001,
-	0xC78, 0x45680001,
-	0xC78, 0x44690001,
-	0xC78, 0x436A0001,
-	0xC78, 0x426B0001,
-	0xC78, 0x296C0001,
-	0xC78, 0x286D0001,
-	0xC78, 0x276E0001,
-	0xC78, 0x266F0001,
-	0xC78, 0x25700001,
-	0xC78, 0x24710001,
-	0xC78, 0x09720001,
-	0xC78, 0x08730001,
-	0xC78, 0x07740001,
-	0xC78, 0x06750001,
-	0xC78, 0x05760001,
-	0xC78, 0x04770001,
-	0xC78, 0x03780001,
-	0xC78, 0x02790001,
-	0xC78, 0x017A0001,
-	0xC78, 0x007B0001,
-	0xC78, 0x007C0001,
-	0xC78, 0x007D0001,
-	0xC78, 0x007E0001,
-	0xC78, 0x007F0001,
-	0xC50, 0x69553422,
-	0xC50, 0x69553420,
+		0xC78, 0xFD000001,
+		0xC78, 0xFC010001,
+		0xC78, 0xFB020001,
+		0xC78, 0xFA030001,
+		0xC78, 0xF9040001,
+		0xC78, 0xF8050001,
+		0xC78, 0xF7060001,
+		0xC78, 0xF6070001,
+		0xC78, 0xF5080001,
+		0xC78, 0xF4090001,
+		0xC78, 0xF30A0001,
+		0xC78, 0xF20B0001,
+		0xC78, 0xF10C0001,
+		0xC78, 0xF00D0001,
+		0xC78, 0xEF0E0001,
+		0xC78, 0xEE0F0001,
+		0xC78, 0xED100001,
+		0xC78, 0xEC110001,
+		0xC78, 0xEB120001,
+		0xC78, 0xEA130001,
+		0xC78, 0xE9140001,
+		0xC78, 0xE8150001,
+		0xC78, 0xE7160001,
+		0xC78, 0xAA170001,
+		0xC78, 0xA9180001,
+		0xC78, 0xA8190001,
+		0xC78, 0xA71A0001,
+		0xC78, 0xA61B0001,
+		0xC78, 0xA51C0001,
+		0xC78, 0xA41D0001,
+		0xC78, 0xA31E0001,
+		0xC78, 0x671F0001,
+		0xC78, 0x66200001,
+		0xC78, 0x65210001,
+		0xC78, 0x64220001,
+		0xC78, 0x63230001,
+		0xC78, 0x62240001,
+		0xC78, 0x61250001,
+		0xC78, 0x47260001,
+		0xC78, 0x46270001,
+		0xC78, 0x45280001,
+		0xC78, 0x44290001,
+		0xC78, 0x432A0001,
+		0xC78, 0x422B0001,
+		0xC78, 0x292C0001,
+		0xC78, 0x282D0001,
+		0xC78, 0x272E0001,
+		0xC78, 0x262F0001,
+		0xC78, 0x25300001,
+		0xC78, 0x24310001,
+		0xC78, 0x09320001,
+		0xC78, 0x08330001,
+		0xC78, 0x07340001,
+		0xC78, 0x06350001,
+		0xC78, 0x05360001,
+		0xC78, 0x04370001,
+		0xC78, 0x03380001,
+		0xC78, 0x02390001,
+		0xC78, 0x013A0001,
+		0xC78, 0x003B0001,
+		0xC78, 0x003C0001,
+		0xC78, 0x003D0001,
+		0xC78, 0x003E0001,
+		0xC78, 0x003F0001,
+		0xC78, 0xFC400001,
+		0xC78, 0xFB410001,
+		0xC78, 0xFA420001,
+		0xC78, 0xF9430001,
+		0xC78, 0xF8440001,
+		0xC78, 0xF7450001,
+		0xC78, 0xF6460001,
+		0xC78, 0xF5470001,
+		0xC78, 0xF4480001,
+		0xC78, 0xF3490001,
+		0xC78, 0xF24A0001,
+		0xC78, 0xF14B0001,
+		0xC78, 0xF04C0001,
+		0xC78, 0xEF4D0001,
+		0xC78, 0xEE4E0001,
+		0xC78, 0xED4F0001,
+		0xC78, 0xEC500001,
+		0xC78, 0xEB510001,
+		0xC78, 0xEA520001,
+		0xC78, 0xE9530001,
+		0xC78, 0xE8540001,
+		0xC78, 0xE7550001,
+		0xC78, 0xE6560001,
+		0xC78, 0xE5570001,
+		0xC78, 0xAA580001,
+		0xC78, 0xA9590001,
+		0xC78, 0xA85A0001,
+		0xC78, 0xA75B0001,
+		0xC78, 0xA65C0001,
+		0xC78, 0xA55D0001,
+		0xC78, 0xA45E0001,
+		0xC78, 0x675F0001,
+		0xC78, 0x66600001,
+		0xC78, 0x65610001,
+		0xC78, 0x64620001,
+		0xC78, 0x63630001,
+		0xC78, 0x62640001,
+		0xC78, 0x61650001,
+		0xC78, 0x47660001,
+		0xC78, 0x46670001,
+		0xC78, 0x45680001,
+		0xC78, 0x44690001,
+		0xC78, 0x436A0001,
+		0xC78, 0x426B0001,
+		0xC78, 0x296C0001,
+		0xC78, 0x286D0001,
+		0xC78, 0x276E0001,
+		0xC78, 0x266F0001,
+		0xC78, 0x25700001,
+		0xC78, 0x24710001,
+		0xC78, 0x09720001,
+		0xC78, 0x08730001,
+		0xC78, 0x07740001,
+		0xC78, 0x06750001,
+		0xC78, 0x05760001,
+		0xC78, 0x04770001,
+		0xC78, 0x03780001,
+		0xC78, 0x02790001,
+		0xC78, 0x017A0001,
+		0xC78, 0x007B0001,
+		0xC78, 0x007C0001,
+		0xC78, 0x007D0001,
+		0xC78, 0x007E0001,
+		0xC78, 0x007F0001,
+		0xC50, 0x69553422,
+		0xC50, 0x69553420,
+
 };
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/table.h b/drivers/net/wireless/rtlwifi/rtl8723be/table.h
index 932760a84827..dc17001632f7 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/table.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/table.h
@@ -35,7 +35,7 @@ extern u32 RTL8723BEPHY_REG_1TARRAY[];
 extern u32 RTL8723BEPHY_REG_ARRAY_PG[];
 #define	RTL8723BE_RADIOA_1TARRAYLEN	206
 extern u32 RTL8723BE_RADIOA_1TARRAY[];
-#define RTL8723BEMAC_1T_ARRAYLEN	194
+#define RTL8723BEMAC_1T_ARRAYLEN	196
 extern u32 RTL8723BEMAC_1T_ARRAY[];
 #define RTL8723BEAGCTAB_1TARRAYLEN	260
 extern u32 RTL8723BEAGCTAB_1TARRAY[];
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/trx.c b/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
index 969eaea5eddd..d6a1c70cb657 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/trx.c
@@ -33,6 +33,7 @@
 #include "trx.h"
 #include "led.h"
 #include "dm.h"
+#include "fw.h"
 
 static u8 _rtl8723be_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
 {
@@ -207,196 +208,150 @@ static int _rtl8723be_rate_mapping(struct ieee80211_hw *hw,
 static void _rtl8723be_query_rxphystatus(struct ieee80211_hw *hw,
 					 struct rtl_stats *pstatus, u8 *pdesc,
 					 struct rx_fwinfo_8723be *p_drvinfo,
-					 bool packet_match_bssid,
-					 bool packet_toself,
+					 bool bpacket_match_bssid,
+					 bool bpacket_toself,
 					 bool packet_beacon)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
-	struct phy_sts_cck_8723e_t *cck_buf;
 	struct phy_status_rpt *p_phystrpt = (struct phy_status_rpt *)p_drvinfo;
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
 	char rx_pwr_all = 0, rx_pwr[4];
-	u8 rf_rx_num = 0, evm, pwdb_all;
+	u8 rf_rx_num = 0, evm, pwdb_all, pwdb_all_bt = 0;
 	u8 i, max_spatial_stream;
 	u32 rssi, total_rssi = 0;
 	bool is_cck = pstatus->is_cck;
 	u8 lan_idx, vga_idx;
 
 	/* Record it for next packet processing */
-	pstatus->packet_matchbssid = packet_match_bssid;
-	pstatus->packet_toself = packet_toself;
+	pstatus->packet_matchbssid = bpacket_match_bssid;
+	pstatus->packet_toself = bpacket_toself;
 	pstatus->packet_beacon = packet_beacon;
-	pstatus->rx_mimo_sig_qual[0] = -1;
-	pstatus->rx_mimo_sig_qual[1] = -1;
+	pstatus->rx_mimo_signalquality[0] = -1;
+	pstatus->rx_mimo_signalquality[1] = -1;
 
 	if (is_cck) {
 		u8 cck_highpwr;
 		u8 cck_agc_rpt;
-		/* CCK Driver info Structure is not the same as OFDM packet. */
-		cck_buf = (struct phy_sts_cck_8723e_t *)p_drvinfo;
-		cck_agc_rpt = cck_buf->cck_agc_rpt;
 
-		/* (1)Hardware does not provide RSSI for CCK
-		 * (2)PWDB, Average PWDB cacluated by
+		cck_agc_rpt = p_phystrpt->cck_agc_rpt_ofdm_cfosho_a;
+
+		/* (1)Hardware does not provide RSSI for CCK */
+		/* (2)PWDB, Average PWDB cacluated by
 		 * hardware (for rate adaptive)
 		 */
-		if (ppsc->rfpwr_state == ERFON)
-			cck_highpwr = (u8) rtl_get_bbreg(hw,
-						       RFPGA0_XA_HSSIPARAMETER2,
-						       BIT(9));
-		else
-			cck_highpwr = false;
+		cck_highpwr = (u8)rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
+						 BIT(9));
 
 		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
 		vga_idx = (cck_agc_rpt & 0x1f);
+
 		switch (lan_idx) {
-		case 7:
-			if (vga_idx <= 27)/*VGA_idx = 27~2*/
-				rx_pwr_all = -100 + 2 * (27 - vga_idx);
-			else
-				rx_pwr_all = -100;
+		/* 46 53 73 95 201301231630 */
+		/* 46 53 77 99 201301241630 */
+		case 6:
+			rx_pwr_all = -34 - (2 * vga_idx);
 			break;
-		case 6:/*VGA_idx = 2~0*/
-			rx_pwr_all = -48 + 2 * (2 - vga_idx);
-			break;
-		case 5:/*VGA_idx = 7~5*/
-			rx_pwr_all = -42 + 2 * (7 - vga_idx);
-			break;
-		case 4:/*VGA_idx = 7~4*/
-			rx_pwr_all = -36 + 2 * (7 - vga_idx);
-			break;
-		case 3:/*VGA_idx = 7~0*/
-			rx_pwr_all = -24 + 2 * (7 - vga_idx);
-			break;
-		case 2:
-			if (cck_highpwr)/*VGA_idx = 5~0*/
-				rx_pwr_all = -12 + 2 * (5 - vga_idx);
-			else
-				rx_pwr_all = -6 + 2 * (5 - vga_idx);
+		case 4:
+			rx_pwr_all = -14 - (2 * vga_idx);
 			break;
 		case 1:
-			rx_pwr_all = 8 - 2 * vga_idx;
+			rx_pwr_all = 6 - (2 * vga_idx);
 			break;
 		case 0:
-			rx_pwr_all = 14 - 2 * vga_idx;
+			rx_pwr_all = 16 - (2 * vga_idx);
 			break;
 		default:
 			break;
 		}
-		rx_pwr_all += 6;
+
 		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
-		/* CCK gain is smaller than OFDM/MCS gain,  */
-		/* so we add gain diff by experiences,
-		 * the val is 6
-		 */
-		pwdb_all += 6;
 		if (pwdb_all > 100)
 			pwdb_all = 100;
-		/* modify the offset to make the same gain index with OFDM. */
-		if (pwdb_all > 34 && pwdb_all <= 42)
-			pwdb_all -= 2;
-		else if (pwdb_all > 26 && pwdb_all <= 34)
-			pwdb_all -= 6;
-		else if (pwdb_all > 14 && pwdb_all <= 26)
-			pwdb_all -= 8;
-		else if (pwdb_all > 4 && pwdb_all <= 14)
-			pwdb_all -= 4;
-		if (!cck_highpwr) {
-			if (pwdb_all >= 80)
-				pwdb_all = ((pwdb_all - 80) << 1) +
-					   ((pwdb_all - 80) >> 1) + 80;
-			else if ((pwdb_all <= 78) && (pwdb_all >= 20))
-				pwdb_all += 3;
-			if (pwdb_all > 100)
-				pwdb_all = 100;
-		}
 
 		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->bt_rx_rssi_percentage = pwdb_all;
 		pstatus->recvsignalpower = rx_pwr_all;
 
 		/* (3) Get Signal Quality (EVM) */
-		if (packet_match_bssid) {
-			u8 sq;
-
+		if (bpacket_match_bssid) {
+			u8 sq, sq_rpt;
 			if (pstatus->rx_pwdb_all > 40) {
 				sq = 100;
 			} else {
-				sq = cck_buf->sq_rpt;
-				if (sq > 64)
+				sq_rpt = p_phystrpt->cck_sig_qual_ofdm_pwdb_all;
+				if (sq_rpt > 64)
 					sq = 0;
-				else if (sq < 20)
+				else if (sq_rpt < 20)
 					sq = 100;
 				else
-					sq = ((64 - sq) * 100) / 44;
+					sq = ((64 - sq_rpt) * 100) / 44;
 			}
-
 			pstatus->signalquality = sq;
-			pstatus->rx_mimo_sig_qual[0] = sq;
-			pstatus->rx_mimo_sig_qual[1] = -1;
+			pstatus->rx_mimo_signalquality[0] = sq;
+			pstatus->rx_mimo_signalquality[1] = -1;
 		}
 	} else {
-		rtlpriv->dm.rfpath_rxenable[0] = true;
-		rtlpriv->dm.rfpath_rxenable[1] = true;
-
 		/* (1)Get RSSI for HT rate */
 		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
 			/* we will judge RF RX path now. */
 			if (rtlpriv->dm.rfpath_rxenable[i])
 				rf_rx_num++;
 
-			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] & 0x3f)*2) - 110;
+			rx_pwr[i] = ((p_phystrpt->path_agc[i].gain & 0x3f) * 2)
+				    - 110;
 
+			pstatus->rx_pwr[i] = rx_pwr[i];
 			/* Translate DBM to percentage. */
 			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
 			total_rssi += rssi;
 
-			/* Get Rx snr value in DB */
-			rtlpriv->stats.rx_snr_db[i] =
-					(long)(p_drvinfo->rxsnr[i] / 2);
-
-			/* Record Signal Strength for next packet */
-			if (packet_match_bssid)
-				pstatus->rx_mimo_signalstrength[i] = (u8) rssi;
+			pstatus->rx_mimo_signalstrength[i] = (u8)rssi;
 		}
 
-		/* (2)PWDB, Avg cacluated by hardware (for rate adaptive) */
-		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
+		rx_pwr_all = ((p_phystrpt->cck_sig_qual_ofdm_pwdb_all >> 1) &
+			     0x7f) - 110;
 
 		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		pwdb_all_bt = pwdb_all;
 		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->bt_rx_rssi_percentage = pwdb_all_bt;
 		pstatus->rxpower = rx_pwr_all;
 		pstatus->recvsignalpower = rx_pwr_all;
 
 		/* (3)EVM of HT rate */
-		if (pstatus->is_ht && pstatus->rate >= DESC92C_RATEMCS8 &&
+		if (pstatus->rate >= DESC92C_RATEMCS8 &&
 		    pstatus->rate <= DESC92C_RATEMCS15)
 			max_spatial_stream = 2;
 		else
 			max_spatial_stream = 1;
 
 		for (i = 0; i < max_spatial_stream; i++) {
-			evm = rtl_evm_db_to_percentage(p_drvinfo->rxevm[i]);
+			evm = rtl_evm_db_to_percentage(
+						p_phystrpt->stream_rxevm[i]);
 
-			if (packet_match_bssid) {
+			if (bpacket_match_bssid) {
 				/* Fill value in RFD, Get the first
 				 * spatial stream only
 				 */
 				if (i == 0)
 					pstatus->signalquality =
-							(u8) (evm & 0xff);
-				pstatus->rx_mimo_sig_qual[i] =
-							(u8) (evm & 0xff);
+							(u8)(evm & 0xff);
+				pstatus->rx_mimo_signalquality[i] =
+							(u8)(evm & 0xff);
 			}
 		}
-		if (packet_match_bssid) {
+
+		if (bpacket_match_bssid) {
 			for (i = RF90_PATH_A; i <= RF90_PATH_B; i++)
 				rtl_priv(hw)->dm.cfo_tail[i] =
-					(char)p_phystrpt->path_cfotail[i];
+					(int)p_phystrpt->path_cfotail[i];
 
-			rtl_priv(hw)->dm.packet_count++;
 			if (rtl_priv(hw)->dm.packet_count == 0xffffffff)
 				rtl_priv(hw)->dm.packet_count = 0;
+			else
+				rtl_priv(hw)->dm.packet_count++;
 		}
 	}
 
@@ -409,10 +364,6 @@ static void _rtl8723be_query_rxphystatus(struct ieee80211_hw *hw,
 	else if (rf_rx_num != 0)
 		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
 						total_rssi /= rf_rx_num));
-	/*HW antenna diversity*/
-	rtldm->fat_table.antsel_rx_keep_0 = p_phystrpt->ant_sel;
-	rtldm->fat_table.antsel_rx_keep_1 = p_phystrpt->ant_sel_b;
-	rtldm->fat_table.antsel_rx_keep_2 = p_phystrpt->antsel_rx_keep_2;
 }
 
 static void _rtl8723be_translate_rx_signal_stuff(struct ieee80211_hw *hw,
@@ -440,14 +391,14 @@ static void _rtl8723be_translate_rx_signal_stuff(struct ieee80211_hw *hw,
 	memcpy(pstatus->psaddr, psaddr, ETH_ALEN);
 
 	packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
-	     (!ether_addr_equal(mac->bssid, (fc & IEEE80211_FCTL_TODS) ?
-				hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ?
-				hdr->addr2 : hdr->addr3)) &&
-				(!pstatus->hwerror) &&
-				(!pstatus->crc) && (!pstatus->icv));
+	     (ether_addr_equal(mac->bssid, (fc & IEEE80211_FCTL_TODS) ?
+				  hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ?
+				  hdr->addr2 : hdr->addr3)) &&
+				  (!pstatus->hwerror) &&
+				  (!pstatus->crc) && (!pstatus->icv));
 
 	packet_toself = packet_matchbssid &&
-	    (!ether_addr_equal(praddr, rtlefuse->dev_addr));
+	    (ether_addr_equal(praddr, rtlefuse->dev_addr));
 
 	/* YP: packet_beacon is not initialized,
 	 * this assignment is neccesary,
@@ -531,30 +482,33 @@ bool rtl8723be_rx_query_desc(struct ieee80211_hw *hw,
 	struct ieee80211_hdr *hdr;
 
 	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
-	status->packet_report_type = (u8)GET_RX_STATUS_DESC_RPT_SEL(pdesc);
-	if (status->packet_report_type == TX_REPORT2)
-		status->length = (u16) GET_RX_RPT2_DESC_PKT_LEN(pdesc);
-	else
-		status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
-	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+
+	status->length = (u16)GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8)GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
 				  RX_DRV_INFO_SIZE_UNIT;
-	status->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->rx_bufshift = (u8)(GET_RX_DESC_SHIFT(pdesc) & 0x03);
 	status->icv = (u16) GET_RX_DESC_ICV(pdesc);
 	status->crc = (u16) GET_RX_DESC_CRC32(pdesc);
 	status->hwerror = (status->crc | status->icv);
 	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
-	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
-	status->shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
-	status->isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	status->isfirst_ampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	if (status->packet_report_type == NORMAL_RX)
-		status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
-	status->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
+	status->rate = (u8)GET_RX_DESC_RXMCS(pdesc);
+	status->shortpreamble = (u16)GET_RX_DESC_SPLCP(pdesc);
+	status->isampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->isfirst_ampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
+	status->rx_is40Mhzpacket = (bool)GET_RX_DESC_BW(pdesc);
+	status->bandwidth = (u8)GET_RX_DESC_BW(pdesc);
+	status->macid = GET_RX_DESC_MACID(pdesc);
 	status->is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
 
-	status->is_cck = RTL8723E_RX_HAL_IS_CCK_RATE(status->rate);
+	status->is_cck = RX_HAL_IS_CCK_RATE(status->rate);
+
+	if (GET_RX_STATUS_DESC_RPT_SEL(pdesc))
+		status->packet_report_type = C2H_PACKET;
+	else
+		status->packet_report_type = NORMAL_RX;
+
 
-	status->macid = GET_RX_DESC_MACID(pdesc);
 	if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
 		status->wake_match = BIT(2);
 	else if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
@@ -565,12 +519,11 @@ bool rtl8723be_rx_query_desc(struct ieee80211_hw *hw,
 		status->wake_match = 0;
 	if (status->wake_match)
 		RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD,
-			 "GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",
-			 status->wake_match);
+		"GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",
+		status->wake_match);
 	rx_status->freq = hw->conf.chandef.chan->center_freq;
 	rx_status->band = hw->conf.chandef.chan->band;
 
-
 	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size +
 				       status->rx_bufshift);
 
@@ -594,24 +547,16 @@ bool rtl8723be_rx_query_desc(struct ieee80211_hw *hw,
 	 * to decrypt it
 	 */
 	if (status->decrypted) {
-		if (!hdr) {
-			WARN_ON_ONCE(true);
-			pr_err("decrypted is true but hdr NULL in skb %p\n",
-			       rtl_get_hdr(skb));
-			return false;
-		}
-
-		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
+		if ((!_ieee80211_is_robust_mgmt_frame(hdr)) &&
 		    (ieee80211_has_protected(hdr->frame_control)))
-			rx_status->flag &= ~RX_FLAG_DECRYPTED;
-		else
 			rx_status->flag |= RX_FLAG_DECRYPTED;
+		else
+			rx_status->flag &= ~RX_FLAG_DECRYPTED;
 	}
 
 	/* rate_idx: index of data rate into band's
 	 * supported rates or MCS index if HT rates
 	 * are use (RX_FLAG_HT)
-	 * Notice: this is diff with windows define
 	 */
 	rx_status->rate_idx = _rtl8723be_rate_mapping(hw, status->is_ht,
 						      status->rate);
@@ -624,21 +569,19 @@ bool rtl8723be_rx_query_desc(struct ieee80211_hw *hw,
 		_rtl8723be_translate_rx_signal_stuff(hw, skb, status,
 						     pdesc, p_drvinfo);
 	}
-
-	/*rx_status->qual = status->signal; */
 	rx_status->signal = status->recvsignalpower + 10;
 	if (status->packet_report_type == TX_REPORT2) {
 		status->macid_valid_entry[0] =
-			 GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
+		  GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
 		status->macid_valid_entry[1] =
-			 GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
+		  GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
 	}
 	return true;
 }
 
 void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
 			    struct ieee80211_hdr *hdr, u8 *pdesc_tx,
-			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
+			    u8 *txbd, struct ieee80211_tx_info *info,
 			    struct ieee80211_sta *sta, struct sk_buff *skb,
 			    u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
 {
@@ -646,16 +589,16 @@ void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 *pdesc = pdesc_tx;
+	u8 *pdesc = (u8 *)pdesc_tx;
 	u16 seq_number;
 	__le16 fc = hdr->frame_control;
 	unsigned int buf_len = 0;
 	unsigned int skb_len = skb->len;
 	u8 fw_qsel = _rtl8723be_map_hwqueue_to_fwqueue(skb, hw_queue);
 	bool firstseg = ((hdr->seq_ctrl &
-			  cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
 	bool lastseg = ((hdr->frame_control &
-			 cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
 	dma_addr_t mapping;
 	u8 bw_40 = 0;
 	u8 short_gi = 0;
@@ -732,11 +675,11 @@ void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
 			 (ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
 			 (ptcb_desc->rts_use_shortgi ? 1 : 0)));
 
-		if (ptcb_desc->btx_enable_sw_calc_duration)
+		if (ptcb_desc->tx_enable_sw_calc_duration)
 			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
 
 		if (bw_40) {
-			if (ptcb_desc->packet_bw) {
+			if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40) {
 				SET_TX_DESC_DATA_BW(pdesc, 1);
 				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
 			} else {
@@ -776,9 +719,12 @@ void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
 		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
 		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
 		SET_TX_DESC_DISABLE_FB(pdesc, ptcb_desc->disable_ratefallback ?
-				       1 : 0);
+					      1 : 0);
 		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
 
+		/* Set TxRate and RTSRate in TxDesc  */
+		/* This prevent Tx initial rate of new-coming packets */
+		/* from being overwritten by retried  packet rate.*/
 		if (ieee80211_is_data_qos(fc)) {
 			if (mac->rdg_en) {
 				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
@@ -793,9 +739,14 @@ void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
 	SET_TX_DESC_LAST_SEG(pdesc, (lastseg ? 1 : 0));
 	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) buf_len);
 	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
-	SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
-	SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
-
+	/* if (rtlpriv->dm.useramask) { */
+	if (1) {
+		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	} else {
+		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	}
 	if (!ieee80211_is_data_qos(fc))  {
 		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
 		SET_TX_DESC_HWSEQ_SEL(pdesc, 0);
@@ -805,11 +756,12 @@ void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
 	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
 		SET_TX_DESC_BMC(pdesc, 1);
 	}
+
 	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "\n");
 }
 
 void rtl8723be_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
-			       bool b_firstseg, bool b_lastseg,
+			       bool firstseg, bool lastseg,
 			       struct sk_buff *skb)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
@@ -849,16 +801,19 @@ void rtl8723be_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
 
 	SET_TX_DESC_OWN(pdesc, 1);
 
-	SET_TX_DESC_PKT_SIZE(pdesc, (u16)(skb->len));
+	SET_TX_DESC_PKT_SIZE((u8 *)pdesc, (u16)(skb->len));
 
 	SET_TX_DESC_FIRST_SEG(pdesc, 1);
 	SET_TX_DESC_LAST_SEG(pdesc, 1);
 
 	SET_TX_DESC_USE_RATE(pdesc, 1);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "H2C Tx Cmd Content\n", pdesc, TX_DESC_SIZE);
 }
 
-void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-			u8 desc_name, u8 *val)
+void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc,
+			bool istx, u8 desc_name, u8 *val)
 {
 	if (istx) {
 		switch (desc_name) {
@@ -870,7 +825,7 @@ void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 			break;
 		default:
 			RT_ASSERT(false, "ERR txdesc :%d not process\n",
-				  desc_name);
+					  desc_name);
 			break;
 		}
 	} else {
@@ -889,7 +844,7 @@ void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
 			break;
 		default:
 			RT_ASSERT(false, "ERR rxdesc :%d not process\n",
-				  desc_name);
+					  desc_name);
 			break;
 		}
 	}
@@ -909,7 +864,7 @@ u32 rtl8723be_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 			break;
 		default:
 			RT_ASSERT(false, "ERR txdesc :%d not process\n",
-				  desc_name);
+					  desc_name);
 			break;
 		}
 	} else {
@@ -920,6 +875,9 @@ u32 rtl8723be_get_desc(u8 *pdesc, bool istx, u8 desc_name)
 		case HW_DESC_RXPKT_LEN:
 			ret = GET_RX_DESC_PKT_LEN(pdesc);
 			break;
+		case HW_DESC_RXBUFF_ADDR:
+			ret = GET_RX_DESC_BUFF_ADDR(pdesc);
+			break;
 		default:
 			RT_ASSERT(false, "ERR rxdesc :%d not process\n",
 				  desc_name);
@@ -935,16 +893,15 @@ bool rtl8723be_is_tx_desc_closed(struct ieee80211_hw *hw,
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
 	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
 	u8 *entry = (u8 *)(&ring->desc[ring->idx]);
-	u8 own = (u8) rtl8723be_get_desc(entry, true, HW_DESC_OWN);
+	u8 own = (u8)rtl8723be_get_desc(entry, true, HW_DESC_OWN);
 
 	/*beacon packet will only use the first
-	 *descriptor by default, and the own may not
+	 *descriptor defautly,and the own may not
 	 *be cleared by the hardware
 	 */
 	if (own)
 		return false;
-	else
-		return true;
+	return true;
 }
 
 void rtl8723be_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
@@ -957,3 +914,28 @@ void rtl8723be_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
 			       BIT(0) << (hw_queue));
 	}
 }
+
+u32 rtl8723be_rx_command_packet(struct ieee80211_hw *hw,
+				struct rtl_stats status,
+				struct sk_buff *skb)
+{
+	u32 result = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (status.packet_report_type) {
+	case NORMAL_RX:
+			result = 0;
+			break;
+	case C2H_PACKET:
+			rtl8723be_c2h_packet_handler(hw, skb->data,
+						     (u8)skb->len);
+			result = 1;
+			break;
+	default:
+			RT_TRACE(rtlpriv, COMP_RECV, DBG_TRACE,
+				 "No this packet type!!\n");
+			break;
+	}
+
+	return result;
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8723be/trx.h b/drivers/net/wireless/rtlwifi/rtl8723be/trx.h
index 102f33dcc988..45949ac4854c 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723be/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723be/trx.h
@@ -415,21 +415,25 @@ struct phy_status_rpt {
 } __packed;
 
 struct rx_fwinfo_8723be {
-	u8 gain_trsw[4];
+	u8 gain_trsw[2];
+	u16 chl_num:10;
+	u16 sub_chnl:4;
+	u16 r_rfmod:2;
 	u8 pwdb_all;
 	u8 cfosho[4];
 	u8 cfotail[4];
 	char rxevm[2];
-	char rxsnr[4];
+	char rxsnr[2];
+	u8 pcts_msk_rpt[2];
 	u8 pdsnr[2];
 	u8 csi_current[2];
-	u8 csi_target[2];
+	u8 rx_gain_c;
+	u8 rx_gain_d;
 	u8 sigevm;
-	u8 max_ex_pwr;
-	u8 ex_intf_flag:1;
-	u8 sgi_en:1;
-	u8 rxsc:2;
-	u8 reserve:4;
+	u8 resvd_0;
+	u8 antidx_anta:3;
+	u8 antidx_antb:3;
+	u8 resvd_1:2;
 } __packed;
 
 struct tx_desc_8723be {
@@ -597,21 +601,25 @@ struct rx_desc_8723be {
 } __packed;
 
 void rtl8723be_tx_fill_desc(struct ieee80211_hw *hw,
-			    struct ieee80211_hdr *hdr, u8 *pdesc,
-			    u8 *pbd_desc_tx, struct ieee80211_tx_info *info,
+			    struct ieee80211_hdr *hdr,
+			    u8 *pdesc_tx, u8 *txbd,
+			    struct ieee80211_tx_info *info,
 			    struct ieee80211_sta *sta, struct sk_buff *skb,
 			    u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
 bool rtl8723be_rx_query_desc(struct ieee80211_hw *hw,
 			     struct rtl_stats *status,
 			     struct ieee80211_rx_status *rx_status,
 			     u8 *pdesc, struct sk_buff *skb);
-void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-			u8 desc_name, u8 *val);
+void rtl8723be_set_desc(struct ieee80211_hw *hw, u8 *pdesc,
+			bool istx, u8 desc_name, u8 *val);
 u32 rtl8723be_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 bool rtl8723be_is_tx_desc_closed(struct ieee80211_hw *hw,
 				 u8 hw_queue, u16 index);
 void rtl8723be_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl8723be_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
-			       bool b_firstseg, bool b_lastseg,
+			       bool firstseg, bool lastseg,
 			       struct sk_buff *skb);
+u32 rtl8723be_rx_command_packet(struct ieee80211_hw *hw,
+				struct rtl_stats status,
+				struct sk_buff *skb);
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.c b/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.c
index 4e254b72bf45..064340641913 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/dm_common.c
@@ -44,7 +44,6 @@ EXPORT_SYMBOL_GPL(rtl8723_dm_init_dynamic_txpower);
 void rtl8723_dm_init_edca_turbo(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
 	rtlpriv->dm.current_turbo_edca = false;
 	rtlpriv->dm.is_any_nonbepkts = false;
 	rtlpriv->dm.is_cur_rdlstate = false;
@@ -54,12 +53,13 @@ EXPORT_SYMBOL_GPL(rtl8723_dm_init_edca_turbo);
 void rtl8723_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct ps_t *dm_pstable = &rtlpriv->dm_pstable;
 
-	rtlpriv->dm_pstable.pre_ccastate = CCA_MAX;
-	rtlpriv->dm_pstable.cur_ccasate = CCA_MAX;
-	rtlpriv->dm_pstable.pre_rfstate = RF_MAX;
-	rtlpriv->dm_pstable.cur_rfstate = RF_MAX;
-	rtlpriv->dm_pstable.rssi_val_min = 0;
-	rtlpriv->dm_pstable.initialize = 0;
+	dm_pstable->pre_ccastate = CCA_MAX;
+	dm_pstable->cur_ccasate = CCA_MAX;
+	dm_pstable->pre_rfstate = RF_MAX;
+	dm_pstable->cur_rfstate = RF_MAX;
+	dm_pstable->rssi_val_min = 0;
+	dm_pstable->initialize = 0;
 }
 EXPORT_SYMBOL_GPL(rtl8723_dm_init_dynamic_bb_powersaving);
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.c b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.c
index 540278ff462b..dd698e7e9ace 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.c
@@ -36,7 +36,8 @@ void rtl8723_enable_fw_download(struct ieee80211_hw *hw, bool enable)
 
 	if (enable) {
 		tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp | 0x04);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1,
+			       tmp | 0x04);
 
 		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
 		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp | 0x01);
@@ -95,7 +96,7 @@ void rtl8723_fw_page_write(struct ieee80211_hw *hw,
 }
 EXPORT_SYMBOL_GPL(rtl8723_fw_page_write);
 
-static void rtl8723_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
+void rtl8723_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
 {
 	u32 fwlen = *pfwlen;
 	u8 remain = (u8) (fwlen % 4);
@@ -109,60 +110,64 @@ static void rtl8723_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
 	}
 	*pfwlen = fwlen;
 }
+EXPORT_SYMBOL(rtl8723_fill_dummy);
 
 void rtl8723_write_fw(struct ieee80211_hw *hw,
 		      enum version_8723e version,
-		      u8 *buffer, u32 size)
+		      u8 *buffer, u32 size, u8 max_page)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u8 *bufferptr = buffer;
-	u32 pagenums, remainsize;
+	u32 page_nums, remain_size;
 	u32 page, offset;
 
-	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "FW size is %d bytes,\n", size);
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW size is %d bytes,\n", size);
 
 	rtl8723_fill_dummy(bufferptr, &size);
 
-	pagenums = size / FW_8192C_PAGE_SIZE;
-	remainsize = size % FW_8192C_PAGE_SIZE;
+	page_nums = size / FW_8192C_PAGE_SIZE;
+	remain_size = size % FW_8192C_PAGE_SIZE;
 
-	if (pagenums > 8) {
+	if (page_nums > max_page) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
-			 "Page numbers should not greater then 8\n");
+			 "Page numbers should not greater than %d\n", max_page);
 	}
-	for (page = 0; page < pagenums; page++) {
+	for (page = 0; page < page_nums; page++) {
 		offset = page * FW_8192C_PAGE_SIZE;
 		rtl8723_fw_page_write(hw, page, (bufferptr + offset),
 				      FW_8192C_PAGE_SIZE);
 	}
-	if (remainsize) {
-		offset = pagenums * FW_8192C_PAGE_SIZE;
-		page = pagenums;
+
+	if (remain_size) {
+		offset = page_nums * FW_8192C_PAGE_SIZE;
+		page = page_nums;
 		rtl8723_fw_page_write(hw, page, (bufferptr + offset),
-				      remainsize);
+				      remain_size);
 	}
+	RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE, "FW write done.\n");
 }
 EXPORT_SYMBOL_GPL(rtl8723_write_fw);
 
 void rtl8723ae_firmware_selfreset(struct ieee80211_hw *hw)
 {
-	u8 u1tmp;
+	u8 u1b_tmp;
 	u8 delay = 100;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	rtl_write_byte(rtlpriv, REG_HMETFR + 3, 0x20);
-	u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
 
-	while (u1tmp & BIT(2)) {
+	while (u1b_tmp & BIT(2)) {
 		delay--;
 		if (delay == 0)
 			break;
 		udelay(50);
-		u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
 	}
 	if (delay == 0) {
-		u1tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, u1tmp&(~BIT(2)));
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1,
+			       u1b_tmp&(~BIT(2)));
 	}
 }
 EXPORT_SYMBOL_GPL(rtl8723ae_firmware_selfreset);
@@ -190,7 +195,8 @@ void rtl8723be_firmware_selfreset(struct ieee80211_hw *hw)
 }
 EXPORT_SYMBOL_GPL(rtl8723be_firmware_selfreset);
 
-int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be)
+int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be,
+			  int max_count)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	int err = -EIO;
@@ -199,10 +205,10 @@ int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be)
 
 	do {
 		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
-	} while ((counter++ < FW_8192C_POLLING_TIMEOUT_COUNT) &&
+	} while ((counter++ < max_count) &&
 		 (!(value32 & FWDL_CHKSUM_RPT)));
 
-	if (counter >= FW_8192C_POLLING_TIMEOUT_COUNT) {
+	if (counter >= max_count) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "chksum report fail ! REG_MCUFWDL:0x%08x .\n",
 			 value32);
@@ -223,15 +229,15 @@ int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be)
 		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
 		if (value32 & WINTINI_RDY) {
 			RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
-				 "Polling FW ready success!! "
-				 "REG_MCUFWDL:0x%08x .\n",
+				 "Polling FW ready success!! REG_MCUFWDL:0x%08x .\n",
 				 value32);
 			err = 0;
 			goto exit;
 		}
-		udelay(FW_8192C_POLLING_DELAY);
 
-	} while (counter++ < FW_8192C_POLLING_TIMEOUT_COUNT);
+		mdelay(FW_8192C_POLLING_DELAY);
+
+	} while (counter++ < max_count);
 
 	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n",
@@ -243,51 +249,55 @@ exit:
 EXPORT_SYMBOL_GPL(rtl8723_fw_free_to_go);
 
 int rtl8723_download_fw(struct ieee80211_hw *hw,
-			bool is_8723be)
+			bool is_8723be, int max_count)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl92c_firmware_header *pfwheader;
+	struct rtl8723e_firmware_header *pfwheader;
 	u8 *pfwdata;
 	u32 fwsize;
 	int err;
 	enum version_8723e version = rtlhal->version;
+	int max_page;
 
 	if (!rtlhal->pfirmware)
 		return 1;
 
-	pfwheader = (struct rtl92c_firmware_header *)rtlhal->pfirmware;
+	pfwheader = (struct rtl8723e_firmware_header *)rtlhal->pfirmware;
 	pfwdata = rtlhal->pfirmware;
 	fwsize = rtlhal->fwsize;
-	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
-		 "normal Firmware SIZE %d\n", fwsize);
 
+	if (!is_8723be)
+		max_page = 6;
+	else
+		max_page = 8;
 	if (rtlpriv->cfg->ops->is_fw_header(pfwheader)) {
 		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
 			 "Firmware Version(%d), Signature(%#x), Size(%d)\n",
 			 pfwheader->version, pfwheader->signature,
-			 (int)sizeof(struct rtl92c_firmware_header));
+			 (int)sizeof(struct rtl8723e_firmware_header));
 
-		pfwdata = pfwdata + sizeof(struct rtl92c_firmware_header);
-		fwsize = fwsize - sizeof(struct rtl92c_firmware_header);
+		pfwdata = pfwdata + sizeof(struct rtl8723e_firmware_header);
+		fwsize = fwsize - sizeof(struct rtl8723e_firmware_header);
 	}
-	if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
-		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0);
+
+	if (rtl_read_byte(rtlpriv, REG_MCUFWDL)&BIT(7)) {
 		if (is_8723be)
 			rtl8723be_firmware_selfreset(hw);
 		else
 			rtl8723ae_firmware_selfreset(hw);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
 	}
 	rtl8723_enable_fw_download(hw, true);
-	rtl8723_write_fw(hw, version, pfwdata, fwsize);
+	rtl8723_write_fw(hw, version, pfwdata, fwsize, max_page);
 	rtl8723_enable_fw_download(hw, false);
 
-	err = rtl8723_fw_free_to_go(hw, is_8723be);
+	err = rtl8723_fw_free_to_go(hw, is_8723be, max_count);
 	if (err) {
 		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
 			 "Firmware is not ready to run!\n");
 	} else {
-		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+		RT_TRACE(rtlpriv, COMP_FW, DBG_TRACE,
 			 "Firmware is ready to run!\n");
 	}
 	return 0;
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.h b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.h
index cf1cc5804d06..3ebafc80972f 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.h
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/fw_common.h
@@ -30,7 +30,8 @@
 #define REG_MCUFWDL				0x0080
 #define FW_8192C_START_ADDRESS			0x1000
 #define FW_8192C_PAGE_SIZE			4096
-#define FW_8192C_POLLING_TIMEOUT_COUNT		6000
+#define FW_8723A_POLLING_TIMEOUT_COUNT		1000
+#define FW_8723B_POLLING_TIMEOUT_COUNT		6000
 #define FW_8192C_POLLING_DELAY			5
 
 #define MCUFWDL_RDY				BIT(1)
@@ -49,16 +50,23 @@ enum version_8723e {
 	VERSION_UNKNOWN = 0xFF,
 };
 
-enum rtl8723ae_h2c_cmd {
-	H2C_AP_OFFLOAD = 0,
-	H2C_SETPWRMODE = 1,
-	H2C_JOINBSSRPT = 2,
-	H2C_RSVDPAGE = 3,
-	H2C_RSSI_REPORT = 4,
-	H2C_P2P_PS_CTW_CMD = 5,
-	H2C_P2P_PS_OFFLOAD = 6,
-	H2C_RA_MASK = 7,
-	MAX_H2CCMD
+struct rtl8723e_firmware_header {
+	u16 signature;
+	u8 category;
+	u8 function;
+	u16 version;
+	u8 subversion;
+	u8 rsvd1;
+	u8 month;
+	u8 date;
+	u8 hour;
+	u8 minute;
+	u16 ramcodesize;
+	u16 rsvd2;
+	u32 svnindex;
+	u32 rsvd3;
+	u32 rsvd4;
+	u32 rsvd5;
 };
 
 enum rtl8723be_cmd {
@@ -92,25 +100,6 @@ enum rtl8723be_cmd {
 	MAX_8723BE_H2CCMD
 };
 
-struct rtl92c_firmware_header {
-	u16 signature;
-	u8 category;
-	u8 function;
-	u16 version;
-	u8 subversion;
-	u8 rsvd1;
-	u8 month;
-	u8 date;
-	u8 hour;
-	u8 minute;
-	u16 ramcodesize;
-	u16 rsvd2;
-	u32 svnindex;
-	u32 rsvd3;
-	u32 rsvd4;
-	u32 rsvd5;
-};
-
 void rtl8723ae_firmware_selfreset(struct ieee80211_hw *hw);
 void rtl8723be_firmware_selfreset(struct ieee80211_hw *hw);
 void rtl8723_enable_fw_download(struct ieee80211_hw *hw, bool enable);
@@ -120,7 +109,11 @@ void rtl8723_fw_page_write(struct ieee80211_hw *hw,
 			   u32 page, const u8 *buffer, u32 size);
 void rtl8723_write_fw(struct ieee80211_hw *hw,
 		      enum version_8723e version,
-		      u8 *buffer, u32 size);
-int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be);
-int rtl8723_download_fw(struct ieee80211_hw *hw, bool is_8723be);
+		      u8 *buffer, u32 size, u8 max_page);
+int rtl8723_fw_free_to_go(struct ieee80211_hw *hw, bool is_8723be, int count);
+int rtl8723_download_fw(struct ieee80211_hw *hw, bool is_8723be, int count);
+bool rtl8723_cmd_send_packet(struct ieee80211_hw *hw,
+			     struct sk_buff *skb);
+void rtl8723_fill_dummy(u8 *pfwbuf, u32 *pfwlen);
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.c b/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.c
index d73b659bd2b5..75cbd1509b52 100644
--- a/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.c
+++ b/drivers/net/wireless/rtlwifi/rtl8723com/phy_common.c
@@ -43,9 +43,8 @@ u32 rtl8723_phy_query_bb_reg(struct ieee80211_hw *hw,
 	returnvalue = (originalvalue & bitmask) >> bitshift;
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "BBR MASK = 0x%x Addr[0x%x]= 0x%x\n",
-		  bitmask, regaddr, originalvalue);
-
+		 "BBR MASK=0x%x Addr[0x%x]=0x%x\n", bitmask,
+		 regaddr, originalvalue);
 	return returnvalue;
 }
 EXPORT_SYMBOL_GPL(rtl8723_phy_query_bb_reg);
@@ -57,8 +56,8 @@ void rtl8723_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
 	u32 originalvalue, bitshift;
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
-		  regaddr, bitmask, data);
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n", regaddr, bitmask,
+		 data);
 
 	if (bitmask != MASKDWORD) {
 		originalvalue = rtl_read_dword(rtlpriv, regaddr);
@@ -70,7 +69,7 @@ void rtl8723_phy_set_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
 
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
 		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
-		  regaddr, bitmask, data);
+		 regaddr, bitmask, data);
 }
 EXPORT_SYMBOL_GPL(rtl8723_phy_set_bb_reg);
 
@@ -109,12 +108,15 @@ u32 rtl8723_phy_rf_serial_read(struct ieee80211_hw *hw,
 	else
 		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
 	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
-		   (newoffset << 23) | BLSSIREADEDGE;
+	    (newoffset << 23) | BLSSIREADEDGE;
 	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
 		      tmplong & (~BLSSIREADEDGE));
 	mdelay(1);
 	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
-	mdelay(2);
+	mdelay(1);
+	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
+		      tmplong | BLSSIREADEDGE);
+	mdelay(1);
 	if (rfpath == RF90_PATH_A)
 		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
 						 BIT(8));
@@ -128,8 +130,8 @@ u32 rtl8723_phy_rf_serial_read(struct ieee80211_hw *hw,
 		retvalue = rtl_get_bbreg(hw, pphyreg->rf_rb,
 					 BLSSIREADBACKDATA);
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "RFR-%d Addr[0x%x]= 0x%x\n",
-		  rfpath, pphyreg->rf_rb, retvalue);
+		 "RFR-%d Addr[0x%x]=0x%x\n",
+		 rfpath, pphyreg->rf_rb, retvalue);
 	return retvalue;
 }
 EXPORT_SYMBOL_GPL(rtl8723_phy_rf_serial_read);
@@ -153,8 +155,9 @@ void rtl8723_phy_rf_serial_write(struct ieee80211_hw *hw,
 	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
 	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
 	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
-		 "RFW-%d Addr[0x%x]= 0x%x\n", rfpath,
-		   pphyreg->rf3wire_offset, data_and_addr);
+		 "RFW-%d Addr[0x%x]=0x%x\n",
+		 rfpath, pphyreg->rf3wire_offset,
+		 data_and_addr);
 }
 EXPORT_SYMBOL_GPL(rtl8723_phy_rf_serial_write);
 
@@ -171,6 +174,8 @@ long rtl8723_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
 		break;
 	case WIRELESS_MODE_G:
 	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
 	default:
 		offset = -8;
 		break;
@@ -202,14 +207,14 @@ void rtl8723_phy_init_bb_rf_reg_def(struct ieee80211_hw *hw)
 	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
 
 	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset =
-			    RFPGA0_XA_LSSIPARAMETER;
+	    RFPGA0_XA_LSSIPARAMETER;
 	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset =
-			    RFPGA0_XB_LSSIPARAMETER;
+	    RFPGA0_XB_LSSIPARAMETER;
 
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_select = rFPGA0_XAB_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = rFPGA0_XCD_RFPARAMETER;
-	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = rFPGA0_XCD_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_A].rflssi_select = RFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_B].rflssi_select = RFPGA0_XAB_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_C].rflssi_select = RFPGA0_XCD_RFPARAMETER;
+	rtlphy->phyreg_def[RF90_PATH_D].rflssi_select = RFPGA0_XCD_RFPARAMETER;
 
 	rtlphy->phyreg_def[RF90_PATH_A].rftxgain_stage = RFPGA0_TXGAINSTAGE;
 	rtlphy->phyreg_def[RF90_PATH_B].rftxgain_stage = RFPGA0_TXGAINSTAGE;
@@ -264,6 +269,7 @@ void rtl8723_phy_init_bb_rf_reg_def(struct ieee80211_hw *hw)
 
 	rtlphy->phyreg_def[RF90_PATH_A].rf_rbpi = TRANSCEIVEA_HSPI_READBACK;
 	rtlphy->phyreg_def[RF90_PATH_B].rf_rbpi = TRANSCEIVEB_HSPI_READBACK;
+
 }
 EXPORT_SYMBOL_GPL(rtl8723_phy_init_bb_rf_reg_def);
 
@@ -384,14 +390,21 @@ EXPORT_SYMBOL_GPL(rtl8723_phy_reload_mac_registers);
 void rtl8723_phy_path_adda_on(struct ieee80211_hw *hw, u32 *addareg,
 			      bool is_patha_on, bool is2t)
 {
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 	u32 pathon;
 	u32 i;
 
-	pathon = is_patha_on ? 0x04db25a4 : 0x0b1b25a4;
-	if (!is2t) {
-		pathon = 0x0bdb25a0;
-		rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0b1b25a0);
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723AE) {
+		pathon = is_patha_on ? 0x04db25a4 : 0x0b1b25a4;
+		if (!is2t) {
+			pathon = 0x0bdb25a0;
+			rtl_set_bbreg(hw, addareg[0], MASKDWORD, 0x0b1b25a0);
+		} else {
+			rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathon);
+		}
 	} else {
+		/* rtl8723be */
+		pathon = 0x01c00014;
 		rtl_set_bbreg(hw, addareg[0], MASKDWORD, pathon);
 	}
 
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/Makefile b/drivers/net/wireless/rtlwifi/rtl8821ae/Makefile
new file mode 100644
index 000000000000..87ad604a1eb3
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/Makefile
@@ -0,0 +1,19 @@
+obj-m := rtl8821ae.o
+
+
+rtl8821ae-objs :=		\
+		dm.o		\
+		fw.o		\
+		hw.o		\
+		led.o		\
+		phy.o		\
+		pwrseq.o	\
+		rf.o		\
+		sw.o		\
+		table.o		\
+		trx.o		\
+
+
+obj-$(CONFIG_RTL8821AE) += rtl8821ae.o
+
+ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/def.h b/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
index 72ebdeaa6e7d..a730985ae81d 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/def.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/def.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -31,6 +27,8 @@
 #define __RTL8821AE_DEF_H__
 
 /*--------------------------Define -------------------------------------------*/
+#define	USE_SPECIFIC_FW_TO_SUPPORT_WOWLAN	1
+
 /* BIT 7 HT Rate*/
 /*TxHT = 0*/
 #define	MGN_1M				0x02
@@ -47,7 +45,7 @@
 #define	MGN_48M				0x60
 #define	MGN_54M				0x6c
 
-// TxHT = 1
+/* TxHT = 1 */
 #define	MGN_MCS0			0x80
 #define	MGN_MCS1			0x81
 #define	MGN_MCS2			0x82
@@ -64,7 +62,7 @@
 #define	MGN_MCS13			0x8d
 #define	MGN_MCS14			0x8e
 #define	MGN_MCS15			0x8f
-//VHT rate
+/* VHT rate */
 #define	MGN_VHT1SS_MCS0		0x90
 #define	MGN_VHT1SS_MCS1		0x91
 #define	MGN_VHT1SS_MCS2		0x92
@@ -116,7 +114,6 @@
 
 #define	MGN_UNKNOWN			0xff
 
-
 /* 30 ms */
 #define	WIFI_NAV_UPPER_US				30000
 #define HAL_92C_NAV_UPPER_UNIT			128
@@ -150,7 +147,6 @@
 
 #define MAX_RX_DMA_BUFFER_SIZE				0x3E80
 
-
 #define MAX_LINES_HWCONFIG_TXT				1000
 #define MAX_BYTES_LINE_HWCONFIG_TXT			256
 
@@ -172,6 +168,8 @@
 #define RX_MAX_QUEUE						2
 #define AC2QUEUEID(_AC)						(_AC)
 
+#define MAX_RX_DMA_BUFFER_SIZE_8812	0x3E80
+
 #define	C2H_RX_CMD_HDR_LEN					8
 #define	GET_C2H_CMD_CMD_LEN(__prxhdr)		\
 	LE_BITS_TO_4BYTE((__prxhdr), 0, 16)
@@ -182,7 +180,7 @@
 #define	GET_C2H_CMD_CONTINUE(__prxhdr)		\
 	LE_BITS_TO_4BYTE((__prxhdr), 31, 1)
 #define	GET_C2H_CMD_CONTENT(__prxhdr)		\
-	((u8*)(__prxhdr) + C2H_RX_CMD_HDR_LEN)
+	((u8 *)(__prxhdr) + C2H_RX_CMD_HDR_LEN)
 
 #define	GET_C2H_CMD_FEEDBACK_ELEMENT_ID(__pcmdfbhdr)	\
 	LE_BITS_TO_4BYTE((__pcmdfbhdr), 0, 8)
@@ -208,19 +206,17 @@
 #define CHIP_8812				BIT(2)
 #define CHIP_8821				(BIT(0)|BIT(2))
 
-#define CHIP_8821A						(BIT(0)|BIT(2))
-#define NORMAL_CHIP  					BIT(3)
-#define RF_TYPE_1T1R					(~(BIT(4)|BIT(5)|BIT(6)))
-#define RF_TYPE_1T2R					BIT(4)
-#define RF_TYPE_2T2R					BIT(5)
-#define CHIP_VENDOR_UMC					BIT(7)
-#define B_CUT_VERSION					BIT(12)
-#define C_CUT_VERSION					BIT(13)
-#define D_CUT_VERSION					((BIT(12)|BIT(13)))
-#define E_CUT_VERSION					BIT(14)
-#define	RF_RL_ID						(BIT(31)|BIT(30)|BIT(29)|BIT(28))
-
-
+#define CHIP_8821A				(BIT(0)|BIT(2))
+#define NORMAL_CHIP				BIT(3)
+#define RF_TYPE_1T1R				(~(BIT(4)|BIT(5)|BIT(6)))
+#define RF_TYPE_1T2R				BIT(4)
+#define RF_TYPE_2T2R				BIT(5)
+#define CHIP_VENDOR_UMC				BIT(7)
+#define B_CUT_VERSION				BIT(12)
+#define C_CUT_VERSION				BIT(13)
+#define D_CUT_VERSION				((BIT(12)|BIT(13)))
+#define E_CUT_VERSION				BIT(14)
+#define	RF_RL_ID			(BIT(31)|BIT(30)|BIT(29)|BIT(28))
 
 enum version_8821ae {
 	VERSION_TEST_CHIP_1T1R_8812 = 0x0004,
@@ -235,7 +231,7 @@ enum version_8821ae {
 	VERSION_UNKNOWN = 0xFF,
 };
 
-enum vht_data_sc{
+enum vht_data_sc {
 	VHT_DATA_SC_DONOT_CARE = 0,
 	VHT_DATA_SC_20_UPPER_OF_80MHZ = 1,
 	VHT_DATA_SC_20_LOWER_OF_80MHZ = 2,
@@ -249,54 +245,65 @@ enum vht_data_sc{
 	VHT_DATA_SC_40_LOWER_OF_80MHZ = 10,
 };
 
-
 /* MASK */
-#define IC_TYPE_MASK					(BIT(0)|BIT(1)|BIT(2))
-#define CHIP_TYPE_MASK 					BIT(3)
-#define RF_TYPE_MASK					(BIT(4)|BIT(5)|BIT(6))
-#define MANUFACTUER_MASK				BIT(7)
-#define ROM_VERSION_MASK				(BIT(11)|BIT(10)|BIT(9)|BIT(8))
-#define CUT_VERSION_MASK				(BIT(15)|BIT(14)|BIT(13)|BIT(12))
+#define IC_TYPE_MASK			(BIT(0)|BIT(1)|BIT(2))
+#define CHIP_TYPE_MASK			BIT(3)
+#define RF_TYPE_MASK			(BIT(4)|BIT(5)|BIT(6))
+#define MANUFACTUER_MASK		BIT(7)
+#define ROM_VERSION_MASK		(BIT(11)|BIT(10)|BIT(9)|BIT(8))
+#define CUT_VERSION_MASK		(BIT(15)|BIT(14)|BIT(13)|BIT(12))
 
 /* Get element */
-#define GET_CVID_IC_TYPE(version)			((version) & IC_TYPE_MASK)
-#define GET_CVID_CHIP_TYPE(version)			((version) & CHIP_TYPE_MASK)
-#define GET_CVID_RF_TYPE(version)			((version) & RF_TYPE_MASK)
-#define GET_CVID_MANUFACTUER(version)		((version) & MANUFACTUER_MASK)
-#define GET_CVID_ROM_VERSION(version)		((version) & ROM_VERSION_MASK)
-#define GET_CVID_CUT_VERSION(version)		((version) & CUT_VERSION_MASK)
-
-#define IS_1T1R(version)			((GET_CVID_RF_TYPE(version))? false : true)
-#define IS_1T2R(version)			((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R)\
-									? true : false)
-#define IS_2T2R(version)			((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R)\
-									? true : false)
-
-#define IS_8812_SERIES(version)			((GET_CVID_IC_TYPE(version) == CHIP_8812)? \
-										true : false)
-#define IS_8821_SERIES(version)			((GET_CVID_IC_TYPE(version) == CHIP_8821)? \
-										true : false)
+#define GET_CVID_IC_TYPE(version)	((version) & IC_TYPE_MASK)
+#define GET_CVID_CHIP_TYPE(version)	((version) & CHIP_TYPE_MASK)
+#define GET_CVID_RF_TYPE(version)	((version) & RF_TYPE_MASK)
+#define GET_CVID_MANUFACTUER(version)	((version) & MANUFACTUER_MASK)
+#define GET_CVID_ROM_VERSION(version)	((version) & ROM_VERSION_MASK)
+#define GET_CVID_CUT_VERSION(version)	((version) & CUT_VERSION_MASK)
+
+#define IS_1T1R(version)	((GET_CVID_RF_TYPE(version)) ? false : true)
+#define IS_1T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_1T2R)\
+							? true : false)
+#define IS_2T2R(version)	((GET_CVID_RF_TYPE(version) == RF_TYPE_2T2R)\
+							? true : false)
+
+#define IS_8812_SERIES(version)	((GET_CVID_IC_TYPE(version) == CHIP_8812) ? \
+								true : false)
+#define IS_8821_SERIES(version)	((GET_CVID_IC_TYPE(version) == CHIP_8821) ? \
+								true : false)
 
 #define IS_VENDOR_8812A_TEST_CHIP(version)	((IS_8812_SERIES(version)) ? \
-												((IS_NORMAL_CHIP(version)) ? \
-												false : true) : false)
-#define IS_VENDOR_8812A_MP_CHIP(version)		((IS_8812_SERIES(version)) ? \
-												((IS_NORMAL_CHIP(version)) ? \
-												true : false) : false)
+					((IS_NORMAL_CHIP(version)) ? \
+						false : true) : false)
+#define IS_VENDOR_8812A_MP_CHIP(version)	((IS_8812_SERIES(version)) ? \
+					((IS_NORMAL_CHIP(version)) ? \
+						true : false) : false)
 #define IS_VENDOR_8812A_C_CUT(version)		((IS_8812_SERIES(version)) ? \
-											((GET_CVID_CUT_VERSION(version) == C_CUT_VERSION) ? \
-											true : false) : false)
+					((GET_CVID_CUT_VERSION(version) == \
+					C_CUT_VERSION) ? \
+					true : false) : false)
 
 #define IS_VENDOR_8821A_TEST_CHIP(version)	((IS_8821_SERIES(version)) ? \
-												((IS_NORMAL_CHIP(version)) ? \
-												false : true) : false)
-#define IS_VENDOR_8821A_MP_CHIP(version)		((IS_8821_SERIES(version)) ? \
-												((IS_NORMAL_CHIP(version)) ? \
-												true : false) : false)
+					((IS_NORMAL_CHIP(version)) ? \
+					false : true) : false)
+#define IS_VENDOR_8821A_MP_CHIP(version)	((IS_8821_SERIES(version)) ? \
+					((IS_NORMAL_CHIP(version)) ? \
+						true : false) : false)
 #define IS_VENDOR_8821A_B_CUT(version)		((IS_8821_SERIES(version)) ? \
-												((GET_CVID_CUT_VERSION(version) == B_CUT_VERSION) ? \
-												true : false) : false)
-
+					((GET_CVID_CUT_VERSION(version) == \
+					B_CUT_VERSION) ? \
+					true : false) : false)
+enum board_type {
+	ODM_BOARD_DEFAULT = 0,	  /* The DEFAULT case. */
+	ODM_BOARD_MINICARD = BIT(0), /* 0 = non-mini card, 1 = mini card. */
+	ODM_BOARD_SLIM = BIT(1), /* 0 = non-slim card, 1 = slim card */
+	ODM_BOARD_BT = BIT(2), /* 0 = without BT card, 1 = with BT */
+	ODM_BOARD_EXT_PA = BIT(3), /* 1 = existing 2G ext-PA */
+	ODM_BOARD_EXT_LNA = BIT(4), /* 1 = existing 2G ext-LNA */
+	ODM_BOARD_EXT_TRSW = BIT(5), /* 1 = existing ext-TRSW */
+	ODM_BOARD_EXT_PA_5G = BIT(6), /* 1 = existing 5G ext-PA */
+	ODM_BOARD_EXT_LNA_5G = BIT(7), /* 1 = existing 5G ext-LNA */
+};
 
 enum rf_optype {
 	RF_OP_BY_SW_3WIRE = 0,
@@ -331,24 +338,24 @@ enum interface_select_pci {
 };
 
 enum hal_fw_c2h_cmd_id {
-	HAL_FW_C2H_CMD_Read_MACREG = 0,
-	HAL_FW_C2H_CMD_Read_BBREG = 1,
-	HAL_FW_C2H_CMD_Read_RFREG = 2,
-	HAL_FW_C2H_CMD_Read_EEPROM = 3,
-	HAL_FW_C2H_CMD_Read_EFUSE = 4,
-	HAL_FW_C2H_CMD_Read_CAM = 5,
-	HAL_FW_C2H_CMD_Get_BasicRate = 6,
-	HAL_FW_C2H_CMD_Get_DataRate = 7,
-	HAL_FW_C2H_CMD_Survey = 8,
-	HAL_FW_C2H_CMD_SurveyDone = 9,
-	HAL_FW_C2H_CMD_JoinBss = 10,
-	HAL_FW_C2H_CMD_AddSTA = 11,
-	HAL_FW_C2H_CMD_DelSTA = 12,
-	HAL_FW_C2H_CMD_AtimDone = 13,
-	HAL_FW_C2H_CMD_TX_Report = 14,
-	HAL_FW_C2H_CMD_CCX_Report = 15,
-	HAL_FW_C2H_CMD_DTM_Report = 16,
-	HAL_FW_C2H_CMD_TX_Rate_Statistics = 17,
+	HAL_FW_C2H_CMD_READ_MACREG = 0,
+	HAL_FW_C2H_CMD_READ_BBREG = 1,
+	HAL_FW_C2H_CMD_READ_RFREG = 2,
+	HAL_FW_C2H_CMD_READ_EEPROM = 3,
+	HAL_FW_C2H_CMD_READ_EFUSE = 4,
+	HAL_FW_C2H_CMD_READ_CAM = 5,
+	HAL_FW_C2H_CMD_GET_BASICRATE = 6,
+	HAL_FW_C2H_CMD_GET_DATARATE = 7,
+	HAL_FW_C2H_CMD_SURVEY = 8,
+	HAL_FW_C2H_CMD_SURVEYDONE = 9,
+	HAL_FW_C2H_CMD_JOINBSS = 10,
+	HAL_FW_C2H_CMD_ADDSTA = 11,
+	HAL_FW_C2H_CMD_DELSTA = 12,
+	HAL_FW_C2H_CMD_ATIMDONE = 13,
+	HAL_FW_C2H_CMD_TX_REPORT = 14,
+	HAL_FW_C2H_CMD_CCX_REPORT = 15,
+	HAL_FW_C2H_CMD_DTM_REPORT = 16,
+	HAL_FW_C2H_CMD_TX_RATE_STATISTICS = 17,
 	HAL_FW_C2H_CMD_C2HLBK = 18,
 	HAL_FW_C2H_CMD_C2HDBG = 19,
 	HAL_FW_C2H_CMD_C2HFEEDBACK = 20,
@@ -397,29 +404,30 @@ enum rtl_desc8821ae_rate {
 	DESC_RATEMCS13 = 0x19,
 	DESC_RATEMCS14 = 0x1a,
 	DESC_RATEMCS15 = 0x1b,
-	DESC_RATEVHT1SS_MCS0 = 0x1c,
-	DESC_RATEVHT1SS_MCS1 = 0x1d,
-	DESC_RATEVHT1SS_MCS2 = 0x1e,
-	DESC_RATEVHT1SS_MCS3 = 0x1f,
-	DESC_RATEVHT1SS_MCS4 = 0x20,
-	DESC_RATEVHT1SS_MCS5 = 0x21,
-	DESC_RATEVHT1SS_MCS6 = 0x22,
-	DESC_RATEVHT1SS_MCS7 = 0x23,
-	DESC_RATEVHT1SS_MCS8 = 0x24,
-	DESC_RATEVHT1SS_MCS9 = 0x25,
-	DESC_RATEVHT2SS_MCS0 = 0x26,
-	DESC_RATEVHT2SS_MCS1 = 0x27,
-	DESC_RATEVHT2SS_MCS2 = 0x28,
-	DESC_RATEVHT2SS_MCS3 = 0x29,
-	DESC_RATEVHT2SS_MCS4 = 0x2a,
-	DESC_RATEVHT2SS_MCS5 = 0x2b,
-	DESC_RATEVHT2SS_MCS6 = 0x2c,
-	DESC_RATEVHT2SS_MCS7 = 0x2d,
-	DESC_RATEVHT2SS_MCS8 = 0x2e,
-	DESC_RATEVHT2SS_MCS9 = 0x2f,
+
+	DESC_RATEVHT1SS_MCS0 = 0x2c,
+	DESC_RATEVHT1SS_MCS1 = 0x2d,
+	DESC_RATEVHT1SS_MCS2 = 0x2e,
+	DESC_RATEVHT1SS_MCS3 = 0x2f,
+	DESC_RATEVHT1SS_MCS4 = 0x30,
+	DESC_RATEVHT1SS_MCS5 = 0x31,
+	DESC_RATEVHT1SS_MCS6 = 0x32,
+	DESC_RATEVHT1SS_MCS7 = 0x33,
+	DESC_RATEVHT1SS_MCS8 = 0x34,
+	DESC_RATEVHT1SS_MCS9 = 0x35,
+	DESC_RATEVHT2SS_MCS0 = 0x36,
+	DESC_RATEVHT2SS_MCS1 = 0x37,
+	DESC_RATEVHT2SS_MCS2 = 0x38,
+	DESC_RATEVHT2SS_MCS3 = 0x39,
+	DESC_RATEVHT2SS_MCS4 = 0x3a,
+	DESC_RATEVHT2SS_MCS5 = 0x3b,
+	DESC_RATEVHT2SS_MCS6 = 0x3c,
+	DESC_RATEVHT2SS_MCS7 = 0x3d,
+	DESC_RATEVHT2SS_MCS8 = 0x3e,
+	DESC_RATEVHT2SS_MCS9 = 0x3f,
 };
 
-enum rx_packet_type{
+enum rx_packet_type {
 	NORMAL_RX,
 	TX_REPORT1,
 	TX_REPORT2,
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
new file mode 100644
index 000000000000..9be106109921
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.c
@@ -0,0 +1,3019 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../base.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "trx.h"
+#include "../btcoexist/rtl_btc.h"
+
+static const u32 txscaling_tbl[TXSCALE_TABLE_SIZE] = {
+	0x081, /* 0, -12.0dB */
+	0x088, /* 1, -11.5dB */
+	0x090, /* 2, -11.0dB */
+	0x099, /* 3, -10.5dB */
+	0x0A2, /* 4, -10.0dB */
+	0x0AC, /* 5, -9.5dB */
+	0x0B6, /* 6, -9.0dB */
+	0x0C0, /* 7, -8.5dB */
+	0x0CC, /* 8, -8.0dB */
+	0x0D8, /* 9, -7.5dB */
+	0x0E5, /* 10, -7.0dB */
+	0x0F2, /* 11, -6.5dB */
+	0x101, /* 12, -6.0dB */
+	0x110, /* 13, -5.5dB */
+	0x120, /* 14, -5.0dB */
+	0x131, /* 15, -4.5dB */
+	0x143, /* 16, -4.0dB */
+	0x156, /* 17, -3.5dB */
+	0x16A, /* 18, -3.0dB */
+	0x180, /* 19, -2.5dB */
+	0x197, /* 20, -2.0dB */
+	0x1AF, /* 21, -1.5dB */
+	0x1C8, /* 22, -1.0dB */
+	0x1E3, /* 23, -0.5dB */
+	0x200, /* 24, +0  dB */
+	0x21E, /* 25, +0.5dB */
+	0x23E, /* 26, +1.0dB */
+	0x261, /* 27, +1.5dB */
+	0x285, /* 28, +2.0dB */
+	0x2AB, /* 29, +2.5dB */
+	0x2D3, /* 30, +3.0dB */
+	0x2FE, /* 31, +3.5dB */
+	0x32B, /* 32, +4.0dB */
+	0x35C, /* 33, +4.5dB */
+	0x38E, /* 34, +5.0dB */
+	0x3C4, /* 35, +5.5dB */
+	0x3FE  /* 36, +6.0dB */
+};
+
+static const u32 rtl8821ae_txscaling_table[TXSCALE_TABLE_SIZE] = {
+	0x081, /* 0, -12.0dB */
+	0x088, /* 1, -11.5dB */
+	0x090, /* 2, -11.0dB */
+	0x099, /* 3, -10.5dB */
+	0x0A2, /* 4, -10.0dB */
+	0x0AC, /* 5, -9.5dB */
+	0x0B6, /* 6, -9.0dB */
+	0x0C0, /* 7, -8.5dB */
+	0x0CC, /* 8, -8.0dB */
+	0x0D8, /* 9, -7.5dB */
+	0x0E5, /* 10, -7.0dB */
+	0x0F2, /* 11, -6.5dB */
+	0x101, /* 12, -6.0dB */
+	0x110, /* 13, -5.5dB */
+	0x120, /* 14, -5.0dB */
+	0x131, /* 15, -4.5dB */
+	0x143, /* 16, -4.0dB */
+	0x156, /* 17, -3.5dB */
+	0x16A, /* 18, -3.0dB */
+	0x180, /* 19, -2.5dB */
+	0x197, /* 20, -2.0dB */
+	0x1AF, /* 21, -1.5dB */
+	0x1C8, /* 22, -1.0dB */
+	0x1E3, /* 23, -0.5dB */
+	0x200, /* 24, +0  dB */
+	0x21E, /* 25, +0.5dB */
+	0x23E, /* 26, +1.0dB */
+	0x261, /* 27, +1.5dB */
+	0x285, /* 28, +2.0dB */
+	0x2AB, /* 29, +2.5dB */
+	0x2D3, /* 30, +3.0dB */
+	0x2FE, /* 31, +3.5dB */
+	0x32B, /* 32, +4.0dB */
+	0x35C, /* 33, +4.5dB */
+	0x38E, /* 34, +5.0dB */
+	0x3C4, /* 35, +5.5dB */
+	0x3FE  /* 36, +6.0dB */
+};
+
+static const u32 ofdmswing_table[] = {
+	0x0b40002d, /* 0, -15.0dB */
+	0x0c000030, /* 1, -14.5dB */
+	0x0cc00033, /* 2, -14.0dB */
+	0x0d800036, /* 3, -13.5dB */
+	0x0e400039, /* 4, -13.0dB */
+	0x0f00003c, /* 5, -12.5dB */
+	0x10000040, /* 6, -12.0dB */
+	0x11000044, /* 7, -11.5dB */
+	0x12000048, /* 8, -11.0dB */
+	0x1300004c, /* 9, -10.5dB */
+	0x14400051, /* 10, -10.0dB */
+	0x15800056, /* 11, -9.5dB */
+	0x16c0005b, /* 12, -9.0dB */
+	0x18000060, /* 13, -8.5dB */
+	0x19800066, /* 14, -8.0dB */
+	0x1b00006c, /* 15, -7.5dB */
+	0x1c800072, /* 16, -7.0dB */
+	0x1e400079, /* 17, -6.5dB */
+	0x20000080, /* 18, -6.0dB */
+	0x22000088, /* 19, -5.5dB */
+	0x24000090, /* 20, -5.0dB */
+	0x26000098, /* 21, -4.5dB */
+	0x288000a2, /* 22, -4.0dB */
+	0x2ac000ab, /* 23, -3.5dB */
+	0x2d4000b5, /* 24, -3.0dB */
+	0x300000c0, /* 25, -2.5dB */
+	0x32c000cb, /* 26, -2.0dB */
+	0x35c000d7, /* 27, -1.5dB */
+	0x390000e4, /* 28, -1.0dB */
+	0x3c8000f2, /* 29, -0.5dB */
+	0x40000100, /* 30, +0dB */
+	0x43c0010f, /* 31, +0.5dB */
+	0x47c0011f, /* 32, +1.0dB */
+	0x4c000130, /* 33, +1.5dB */
+	0x50800142, /* 34, +2.0dB */
+	0x55400155, /* 35, +2.5dB */
+	0x5a400169, /* 36, +3.0dB */
+	0x5fc0017f, /* 37, +3.5dB */
+	0x65400195, /* 38, +4.0dB */
+	0x6b8001ae, /* 39, +4.5dB */
+	0x71c001c7, /* 40, +5.0dB */
+	0x788001e2, /* 41, +5.5dB */
+	0x7f8001fe  /* 42, +6.0dB */
+};
+
+static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01}, /* 0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01}, /* 1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01}, /* 2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01}, /* 3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01}, /* 4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01}, /* 5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01}, /* 6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01}, /* 7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01}, /* 8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01}, /* 9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01}, /* 11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01}, /* 13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01}, /* 14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02}, /* 15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01}, /* 16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02}, /* 17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02}, /* 18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02}, /* 19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02}, /* 20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02}, /* 21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02}, /* 22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02}, /* 23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02}, /* 24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03}, /* 25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03}, /* 26, -3.0dB */
+	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03}, /* 27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03}, /* 28, -2.0dB */
+	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03}, /* 29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03}, /* 30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04}, /* 31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04} /* 32, +0dB */
+};
+
+static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8] = {
+	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00}, /* 0, -16.0dB */
+	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 1, -15.5dB */
+	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 2, -15.0dB */
+	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 3, -14.5dB */
+	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00}, /* 4, -14.0dB */
+	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 5, -13.5dB */
+	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 6, -13.0dB */
+	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00}, /* 7, -12.5dB */
+	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /* 8, -12.0dB */
+	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00}, /* 9, -11.5dB */
+	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 10, -11.0dB */
+	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00}, /* 11, -10.5dB */
+	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 12, -10.0dB */
+	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00}, /* 13, -9.5dB */
+	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 14, -9.0dB */
+	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00}, /* 15, -8.5dB */
+	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 16, -8.0dB */
+	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00}, /* 17, -7.5dB */
+	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00}, /* 18, -7.0dB */
+	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00}, /* 19, -6.5dB */
+	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 20, -6.0dB */
+	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00}, /* 21, -5.5dB */
+	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00}, /* 22, -5.0dB */
+	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00}, /* 23, -4.5dB */
+	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00}, /* 24, -4.0dB */
+	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00}, /* 25, -3.5dB */
+	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00}, /* 26, -3.0dB */
+	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00}, /* 27, -2.5dB */
+	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00}, /* 28, -2.0dB */
+	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00}, /* 29, -1.5dB */
+	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00}, /* 30, -1.0dB */
+	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00}, /* 31, -0.5dB */
+	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00} /* 32, +0dB */
+};
+
+static const u32 edca_setting_dl[PEER_MAX] = {
+	0xa44f,		/* 0 UNKNOWN */
+	0x5ea44f,	/* 1 REALTEK_90 */
+	0x5e4322,	/* 2 REALTEK_92SE */
+	0x5ea42b,		/* 3 BROAD	*/
+	0xa44f,		/* 4 RAL */
+	0xa630,		/* 5 ATH */
+	0x5ea630,		/* 6 CISCO */
+	0x5ea42b,		/* 7 MARVELL */
+};
+
+static const u32 edca_setting_ul[PEER_MAX] = {
+	0x5e4322,	/* 0 UNKNOWN */
+	0xa44f,		/* 1 REALTEK_90 */
+	0x5ea44f,	/* 2 REALTEK_92SE */
+	0x5ea32b,	/* 3 BROAD */
+	0x5ea422,	/* 4 RAL */
+	0x5ea322,	/* 5 ATH */
+	0x3ea430,	/* 6 CISCO */
+	0x5ea44f,	/* 7 MARV */
+};
+
+static u8 rtl8818e_delta_swing_table_idx_24gb_p[] = {
+	0, 0, 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 4, 4,
+	4, 4, 4, 5, 5, 7, 7, 8, 8, 8, 9, 9, 9, 9, 9};
+
+static u8 rtl8818e_delta_swing_table_idx_24gb_n[] = {
+	0, 0, 0, 2, 2, 3, 3, 4, 4, 4, 4, 5, 5, 6, 6,
+	7, 7, 7, 7, 8, 8, 9, 9, 10, 10, 10, 11, 11, 11, 11};
+
+static u8 rtl8812ae_delta_swing_table_idx_24gb_n[]  = {
+	0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 5, 6,
+	6, 6, 7, 8, 9, 9, 9, 9, 10, 10, 10, 10, 11, 11};
+
+static u8 rtl8812ae_delta_swing_table_idx_24gb_p[] = {
+	0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 6,
+	6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 9, 9, 9};
+
+static u8 rtl8812ae_delta_swing_table_idx_24ga_n[] = {
+	0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 5, 6,
+	6, 6, 7, 8, 8, 9, 9, 9, 10, 10, 10, 10, 11, 11};
+
+static u8 rtl8812ae_delta_swing_table_idx_24ga_p[] = {
+	0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 6,
+	6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 9, 9, 9};
+
+static u8 rtl8812ae_delta_swing_table_idx_24gcckb_n[] = {
+	0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 5, 6,
+	6, 6, 7, 8, 9, 9, 9, 9, 10, 10, 10, 10, 11, 11};
+
+static u8 rtl8812ae_delta_swing_table_idx_24gcckb_p[] = {
+	0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 6,
+	6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 9, 9, 9};
+
+static u8 rtl8812ae_delta_swing_table_idx_24gccka_n[] = {
+	0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 5, 6,
+	6, 6, 7, 8, 8, 9, 9, 9, 10, 10, 10, 10, 11, 11};
+
+static u8 rtl8812ae_delta_swing_table_idx_24gccka_p[] = {
+	0, 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4, 5, 5, 6,
+	6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 9, 9, 9};
+
+static u8 rtl8812ae_delta_swing_table_idx_5gb_n[][DEL_SW_IDX_SZ] = {
+	{0, 1, 1, 2, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 6, 7, 7,
+	7, 8, 8, 9, 9, 9, 10, 10, 11, 11, 12, 12, 13},
+	{0, 1, 1, 2, 2, 2, 3, 3, 4, 4, 4, 5, 5, 6, 6, 7,
+	7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 13, 13},
+	{0, 1, 1, 2, 3, 3, 4, 4, 5, 6, 6, 7, 8, 9, 10, 11,
+	12, 12, 13, 14, 14, 14, 15, 16, 17, 17, 17, 18, 18, 18},
+};
+
+static u8 rtl8812ae_delta_swing_table_idx_5gb_p[][DEL_SW_IDX_SZ] = {
+	{0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 5, 5, 6, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 7, 7, 8, 8, 9,
+	9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
+};
+
+static u8 rtl8812ae_delta_swing_table_idx_5ga_n[][DEL_SW_IDX_SZ] = {
+	{0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 5, 5, 6, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 13, 13, 13},
+	{0, 1, 1, 2, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 8, 9,
+	9, 10, 10, 11, 11, 11, 12, 12, 12, 12, 12, 13, 13},
+	{0, 1, 1, 2, 2, 3, 3, 4, 5, 6, 7, 8, 8, 9, 10, 11,
+	12, 13, 14, 14, 15, 15, 15, 16, 16, 16, 17, 17, 18, 18},
+};
+
+static u8 rtl8812ae_delta_swing_table_idx_5ga_p[][DEL_SW_IDX_SZ] = {
+	{0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 4, 5, 5, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 2, 3, 3, 4, 4, 4, 5, 5, 6, 6, 7, 7, 8,
+	9, 9, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
+	{0, 1, 1, 2, 3, 3, 4, 4, 5, 6, 6, 7, 7, 8, 9, 9,
+	10, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
+};
+
+static u8 rtl8821ae_delta_swing_table_idx_24gb_n[] = {
+	0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6,
+	6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+
+static u8 rtl8821ae_delta_swing_table_idx_24gb_p[]  = {
+	0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+
+static u8 rtl8821ae_delta_swing_table_idx_24ga_n[]  = {
+	0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6,
+	6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+
+static u8 rtl8821ae_delta_swing_table_idx_24ga_p[] = {
+	0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+
+static u8 rtl8821ae_delta_swing_table_idx_24gcckb_n[] = {
+	0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6,
+	6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+
+static u8 rtl8821ae_delta_swing_table_idx_24gcckb_p[] = {
+	0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+
+static u8 rtl8821ae_delta_swing_table_idx_24gccka_n[] = {
+	0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6,
+	6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
+
+static u8 rtl8821ae_delta_swing_table_idx_24gccka_p[] = {
+	0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8,
+	8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
+
+static u8 rtl8821ae_delta_swing_table_idx_5gb_n[][DEL_SW_IDX_SZ] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+static u8 rtl8821ae_delta_swing_table_idx_5gb_p[][DEL_SW_IDX_SZ] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+static u8 rtl8821ae_delta_swing_table_idx_5ga_n[][DEL_SW_IDX_SZ] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+static u8 rtl8821ae_delta_swing_table_idx_5ga_p[][DEL_SW_IDX_SZ] = {
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11,
+	12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
+};
+
+void rtl8821ae_dm_txpower_track_adjust(struct ieee80211_hw *hw,
+				       u8 type, u8 *pdirection,
+				       u32 *poutwrite_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 pwr_val = 0;
+
+	if (type == 0) {
+		if (rtlpriv->dm.swing_idx_ofdm[RF90_PATH_A] <=
+			rtlpriv->dm.swing_idx_ofdm_base[RF90_PATH_A]) {
+			*pdirection = 1;
+			pwr_val = rtldm->swing_idx_ofdm_base[RF90_PATH_A] -
+					rtldm->swing_idx_ofdm[RF90_PATH_A];
+		} else {
+			*pdirection = 2;
+			pwr_val = rtldm->swing_idx_ofdm[RF90_PATH_A] -
+				rtldm->swing_idx_ofdm_base[RF90_PATH_A];
+		}
+	} else if (type == 1) {
+		if (rtldm->swing_idx_cck <= rtldm->swing_idx_cck_base) {
+			*pdirection = 1;
+			pwr_val = rtldm->swing_idx_cck_base -
+					rtldm->swing_idx_cck;
+		} else {
+			*pdirection = 2;
+			pwr_val = rtldm->swing_idx_cck -
+				rtldm->swing_idx_cck_base;
+		}
+	}
+
+	if (pwr_val >= TXPWRTRACK_MAX_IDX && (*pdirection == 1))
+		pwr_val = TXPWRTRACK_MAX_IDX;
+
+	*poutwrite_val = pwr_val | (pwr_val << 8)|
+				(pwr_val << 16)|
+				(pwr_val << 24);
+}
+
+void rtl8821ae_dm_clear_txpower_tracking_state(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	u8 p = 0;
+
+	rtldm->swing_idx_cck_base = rtldm->default_cck_index;
+	rtldm->swing_idx_cck = rtldm->default_cck_index;
+	rtldm->cck_index = 0;
+
+	for (p = RF90_PATH_A; p <= RF90_PATH_B; ++p) {
+		rtldm->swing_idx_ofdm_base[p] = rtldm->default_ofdm_index;
+		rtldm->swing_idx_ofdm[p] = rtldm->default_ofdm_index;
+		rtldm->ofdm_index[p] = rtldm->default_ofdm_index;
+
+		rtldm->power_index_offset[p] = 0;
+		rtldm->delta_power_index[p] = 0;
+		rtldm->delta_power_index_last[p] = 0;
+		/*Initial Mix mode power tracking*/
+		rtldm->absolute_ofdm_swing_idx[p] = 0;
+		rtldm->remnant_ofdm_swing_idx[p] = 0;
+	}
+	/*Initial at Modify Tx Scaling Mode*/
+	rtldm->modify_txagc_flag_path_a = false;
+	/*Initial at Modify Tx Scaling Mode*/
+	rtldm->modify_txagc_flag_path_b = false;
+	rtldm->remnant_cck_idx = 0;
+	rtldm->thermalvalue = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_iqk = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_lck = rtlefuse->eeprom_thermalmeter;
+}
+
+static u8  rtl8821ae_dm_get_swing_index(struct ieee80211_hw *hw)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 i = 0;
+	u32  bb_swing;
+
+	bb_swing = phy_get_tx_swing_8812A(hw, rtlhal->current_bandtype,
+					  RF90_PATH_A);
+
+	for (i = 0; i < TXSCALE_TABLE_SIZE; ++i)
+		if (bb_swing == rtl8821ae_txscaling_table[i])
+			break;
+
+	return i;
+}
+
+void rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(
+				struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	u8 default_swing_index  = 0;
+	u8 p = 0;
+
+	rtlpriv->dm.txpower_track_control = true;
+	rtldm->thermalvalue = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_iqk = rtlefuse->eeprom_thermalmeter;
+	rtldm->thermalvalue_lck = rtlefuse->eeprom_thermalmeter;
+	default_swing_index = rtl8821ae_dm_get_swing_index(hw);
+
+	rtldm->default_ofdm_index =
+		(default_swing_index == TXSCALE_TABLE_SIZE) ?
+		24 : default_swing_index;
+	rtldm->default_cck_index = 24;
+
+	rtldm->swing_idx_cck_base = rtldm->default_cck_index;
+	rtldm->cck_index = rtldm->default_cck_index;
+
+	for (p = RF90_PATH_A; p < MAX_RF_PATH; ++p) {
+		rtldm->swing_idx_ofdm_base[p] =
+			rtldm->default_ofdm_index;
+		rtldm->ofdm_index[p] = rtldm->default_ofdm_index;
+		rtldm->delta_power_index[p] = 0;
+		rtldm->power_index_offset[p] = 0;
+		rtldm->delta_power_index_last[p] = 0;
+	}
+}
+
+static void rtl8821ae_dm_diginit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+
+	dm_digtable->cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
+	dm_digtable->rssi_lowthresh = DM_DIG_THRESH_LOW;
+	dm_digtable->rssi_highthresh = DM_DIG_THRESH_HIGH;
+	dm_digtable->fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
+	dm_digtable->fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
+	dm_digtable->rx_gain_max = DM_DIG_MAX;
+	dm_digtable->rx_gain_min = DM_DIG_MIN;
+	dm_digtable->back_val = DM_DIG_BACKOFF_DEFAULT;
+	dm_digtable->back_range_max = DM_DIG_BACKOFF_MAX;
+	dm_digtable->back_range_min = DM_DIG_BACKOFF_MIN;
+	dm_digtable->pre_cck_cca_thres = 0xff;
+	dm_digtable->cur_cck_cca_thres = 0x83;
+	dm_digtable->forbidden_igi = DM_DIG_MIN;
+	dm_digtable->large_fa_hit = 0;
+	dm_digtable->recover_cnt = 0;
+	dm_digtable->dig_dynamic_min = DM_DIG_MIN;
+	dm_digtable->dig_dynamic_min_1 = DM_DIG_MIN;
+	dm_digtable->media_connect_0 = false;
+	dm_digtable->media_connect_1 = false;
+	rtlpriv->dm.dm_initialgain_enable = true;
+	dm_digtable->bt30_cur_igi = 0x32;
+}
+
+void rtl8821ae_dm_init_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.current_turbo_edca = false;
+	rtlpriv->dm.is_any_nonbepkts = false;
+	rtlpriv->dm.is_cur_rdlstate = false;
+}
+
+void rtl8821ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
+
+	p_ra->ratr_state = DM_RATR_STA_INIT;
+	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
+		rtlpriv->dm.useramask = true;
+	else
+		rtlpriv->dm.useramask = false;
+
+	p_ra->high_rssi_thresh_for_ra = 50;
+	p_ra->low_rssi_thresh_for_ra40m = 20;
+}
+
+static void rtl8821ae_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpriv->dm.crystal_cap = rtlpriv->efuse.crystalcap;
+
+	rtlpriv->dm.atc_status = rtl_get_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11));
+	rtlpriv->dm.cfo_threshold = CFO_THRESHOLD_XTAL;
+}
+
+static void rtl8821ae_dm_common_info_self_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 tmp;
+
+	rtlphy->cck_high_power =
+		(bool)rtl_get_bbreg(hw, ODM_REG_CCK_RPT_FORMAT_11AC,
+				    ODM_BIT_CCK_RPT_FORMAT_11AC);
+
+	tmp = (u8)rtl_get_bbreg(hw, ODM_REG_BB_RX_PATH_11AC,
+				ODM_BIT_BB_RX_PATH_11AC);
+	if (tmp & BIT(0))
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+	if (tmp & BIT(1))
+		rtlpriv->dm.rfpath_rxenable[1] = true;
+}
+
+void rtl8821ae_dm_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	spin_lock(&rtlpriv->locks.iqk_lock);
+	rtlphy->lck_inprogress = false;
+	spin_unlock(&rtlpriv->locks.iqk_lock);
+
+	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
+	rtl8821ae_dm_common_info_self_init(hw);
+	rtl8821ae_dm_diginit(hw);
+	rtl8821ae_dm_init_rate_adaptive_mask(hw);
+	rtl8821ae_dm_init_edca_turbo(hw);
+	rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(hw);
+	rtl8821ae_dm_init_dynamic_atc_switch(hw);
+}
+
+static void rtl8821ae_dm_find_minimum_rssi(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *rtl_dm_dig = &rtlpriv->dm_digtable;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+
+	/* Determine the minimum RSSI  */
+	if ((mac->link_state < MAC80211_LINKED) &&
+	    (rtlpriv->dm.entry_min_undec_sm_pwdb == 0)) {
+		rtl_dm_dig->min_undec_pwdb_for_dm = 0;
+		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+			 "Not connected to any\n");
+	}
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (mac->opmode == NL80211_IFTYPE_AP ||
+		    mac->opmode == NL80211_IFTYPE_ADHOC) {
+			rtl_dm_dig->min_undec_pwdb_for_dm =
+			    rtlpriv->dm.entry_min_undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+				 "AP Client PWDB = 0x%lx\n",
+				 rtlpriv->dm.entry_min_undec_sm_pwdb);
+		} else {
+			rtl_dm_dig->min_undec_pwdb_for_dm =
+			    rtlpriv->dm.undec_sm_pwdb;
+			RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+				 "STA Default Port PWDB = 0x%x\n",
+				 rtl_dm_dig->min_undec_pwdb_for_dm);
+		}
+	} else {
+		rtl_dm_dig->min_undec_pwdb_for_dm =
+		    rtlpriv->dm.entry_min_undec_sm_pwdb;
+		RT_TRACE(rtlpriv, COMP_BB_POWERSAVING, DBG_LOUD,
+			 "AP Ext Port or disconnet PWDB = 0x%x\n",
+			 rtl_dm_dig->min_undec_pwdb_for_dm);
+	}
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "MinUndecoratedPWDBForDM =%d\n",
+		 rtl_dm_dig->min_undec_pwdb_for_dm);
+}
+
+static void  rtl8812ae_dm_rssi_dump_to_register(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, RA_RSSI_DUMP,
+		       rtlpriv->stats.rx_rssi_percentage[0]);
+	rtl_write_byte(rtlpriv, RB_RSSI_DUMP,
+		       rtlpriv->stats.rx_rssi_percentage[1]);
+
+	/* Rx EVM*/
+	rtl_write_byte(rtlpriv, RS1_RX_EVM_DUMP,
+		       rtlpriv->stats.rx_evm_dbm[0]);
+	rtl_write_byte(rtlpriv, RS2_RX_EVM_DUMP,
+		       rtlpriv->stats.rx_evm_dbm[1]);
+
+	/*Rx SNR*/
+	rtl_write_byte(rtlpriv, RA_RX_SNR_DUMP,
+		       (u8)(rtlpriv->stats.rx_snr_db[0]));
+	rtl_write_byte(rtlpriv, RB_RX_SNR_DUMP,
+		       (u8)(rtlpriv->stats.rx_snr_db[1]));
+
+	/*Rx Cfo_Short*/
+	rtl_write_word(rtlpriv, RA_CFO_SHORT_DUMP,
+		       rtlpriv->stats.rx_cfo_short[0]);
+	rtl_write_word(rtlpriv, RB_CFO_SHORT_DUMP,
+		       rtlpriv->stats.rx_cfo_short[1]);
+
+	/*Rx Cfo_Tail*/
+	rtl_write_word(rtlpriv, RA_CFO_LONG_DUMP,
+		       rtlpriv->stats.rx_cfo_tail[0]);
+	rtl_write_word(rtlpriv, RB_CFO_LONG_DUMP,
+		       rtlpriv->stats.rx_cfo_tail[1]);
+}
+
+static void rtl8821ae_dm_check_rssi_monitor(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_sta_info *drv_priv;
+	u8 h2c_parameter[4] = { 0 };
+	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
+	u8 stbc_tx = 0;
+	u64 cur_txokcnt = 0, cur_rxokcnt = 0;
+	static u64 last_txokcnt = 0, last_rxokcnt;
+
+	cur_txokcnt = rtlpriv->stats.txbytesunicast - last_txokcnt;
+	cur_rxokcnt = rtlpriv->stats.rxbytesunicast - last_rxokcnt;
+	last_txokcnt = rtlpriv->stats.txbytesunicast;
+	last_rxokcnt = rtlpriv->stats.rxbytesunicast;
+	if (cur_rxokcnt > (last_txokcnt * 6))
+		h2c_parameter[3] = 0x01;
+	else
+		h2c_parameter[3] = 0x00;
+
+	/* AP & ADHOC & MESH */
+	if (mac->opmode == NL80211_IFTYPE_AP ||
+	    mac->opmode == NL80211_IFTYPE_ADHOC ||
+	    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
+			if (drv_priv->rssi_stat.undec_sm_pwdb <
+					tmp_entry_min_pwdb)
+				tmp_entry_min_pwdb =
+					drv_priv->rssi_stat.undec_sm_pwdb;
+			if (drv_priv->rssi_stat.undec_sm_pwdb >
+					tmp_entry_max_pwdb)
+				tmp_entry_max_pwdb =
+					drv_priv->rssi_stat.undec_sm_pwdb;
+		}
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+		/* If associated entry is found */
+		if (tmp_entry_max_pwdb != 0) {
+			rtlpriv->dm.entry_max_undec_sm_pwdb =
+				tmp_entry_max_pwdb;
+			RTPRINT(rtlpriv, FDM, DM_PWDB,
+				"EntryMaxPWDB = 0x%lx(%ld)\n",
+				tmp_entry_max_pwdb, tmp_entry_max_pwdb);
+		} else {
+			rtlpriv->dm.entry_max_undec_sm_pwdb = 0;
+		}
+		/* If associated entry is found */
+		if (tmp_entry_min_pwdb != 0xff) {
+			rtlpriv->dm.entry_min_undec_sm_pwdb =
+				tmp_entry_min_pwdb;
+			RTPRINT(rtlpriv, FDM, DM_PWDB,
+				"EntryMinPWDB = 0x%lx(%ld)\n",
+				tmp_entry_min_pwdb, tmp_entry_min_pwdb);
+		} else {
+			rtlpriv->dm.entry_min_undec_sm_pwdb = 0;
+		}
+	}
+	/* Indicate Rx signal strength to FW. */
+	if (rtlpriv->dm.useramask) {
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			if (mac->mode == WIRELESS_MODE_AC_24G ||
+			    mac->mode == WIRELESS_MODE_AC_5G ||
+			    mac->mode == WIRELESS_MODE_AC_ONLY)
+				stbc_tx = (mac->vht_cur_stbc &
+					   STBC_VHT_ENABLE_TX) ? 1 : 0;
+			else
+				stbc_tx = (mac->ht_cur_stbc &
+					   STBC_HT_ENABLE_TX) ? 1 : 0;
+			h2c_parameter[3] |= stbc_tx << 1;
+		}
+		h2c_parameter[2] =
+			(u8)(rtlpriv->dm.undec_sm_pwdb & 0xFF);
+		h2c_parameter[1] = 0x20;
+		h2c_parameter[0] = 0;
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8821ae_fill_h2c_cmd(hw, H2C_RSSI_21AE_REPORT, 4,
+					       h2c_parameter);
+		else
+			rtl8821ae_fill_h2c_cmd(hw, H2C_RSSI_21AE_REPORT, 3,
+					       h2c_parameter);
+	} else {
+		rtl_write_byte(rtlpriv, 0x4fe, rtlpriv->dm.undec_sm_pwdb);
+	}
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtl8812ae_dm_rssi_dump_to_register(hw);
+	rtl8821ae_dm_find_minimum_rssi(hw);
+	dm_digtable->rssi_val_min = rtlpriv->dm_digtable.min_undec_pwdb_for_dm;
+}
+
+void rtl8821ae_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 current_cca)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+
+	if (dm_digtable->cur_cck_cca_thres != current_cca)
+		rtl_write_byte(rtlpriv, DM_REG_CCK_CCA_11AC, current_cca);
+
+	dm_digtable->pre_cck_cca_thres = dm_digtable->cur_cck_cca_thres;
+	dm_digtable->cur_cck_cca_thres = current_cca;
+}
+
+void rtl8821ae_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+
+	if (dm_digtable->stop_dig)
+		return;
+
+	if (dm_digtable->cur_igvalue != current_igi) {
+		rtl_set_bbreg(hw, DM_REG_IGI_A_11AC,
+			      DM_BIT_IGI_11AC, current_igi);
+		if (rtlpriv->phy.rf_type != RF_1T1R)
+			rtl_set_bbreg(hw, DM_REG_IGI_B_11AC,
+				      DM_BIT_IGI_11AC, current_igi);
+	}
+	dm_digtable->cur_igvalue = current_igi;
+}
+
+static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 dig_dynamic_min;
+	u8 dig_max_of_min;
+	bool first_connect, first_disconnect;
+	u8 dm_dig_max, dm_dig_min, offset;
+	u8 current_igi = dm_digtable->cur_igvalue;
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "\n");
+
+	if (mac->act_scanning) {
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "Return: In Scan Progress\n");
+		return;
+	}
+
+	/*add by Neil Chen to avoid PSD is processing*/
+	dig_dynamic_min = dm_digtable->dig_dynamic_min;
+	first_connect = (mac->link_state >= MAC80211_LINKED) &&
+			(!dm_digtable->media_connect_0);
+	first_disconnect = (mac->link_state < MAC80211_LINKED) &&
+			(dm_digtable->media_connect_0);
+
+	/*1 Boundary Decision*/
+
+	dm_dig_max = 0x5A;
+
+	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8821AE)
+		dm_dig_min = DM_DIG_MIN;
+	else
+		dm_dig_min = 0x1C;
+
+	dig_max_of_min = DM_DIG_MAX_AP;
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8821AE)
+			offset = 20;
+		else
+			offset = 10;
+
+		if ((dm_digtable->rssi_val_min + offset) > dm_dig_max)
+			dm_digtable->rx_gain_max = dm_dig_max;
+		else if ((dm_digtable->rssi_val_min + offset) < dm_dig_min)
+			dm_digtable->rx_gain_max = dm_dig_min;
+		else
+			dm_digtable->rx_gain_max =
+				dm_digtable->rssi_val_min + offset;
+
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "dm_digtable->rssi_val_min=0x%x,dm_digtable->rx_gain_max = 0x%x",
+			 dm_digtable->rssi_val_min,
+			 dm_digtable->rx_gain_max);
+		if (rtlpriv->dm.one_entry_only) {
+			offset = 0;
+
+			if (dm_digtable->rssi_val_min - offset < dm_dig_min)
+				dig_dynamic_min = dm_dig_min;
+			else if (dm_digtable->rssi_val_min -
+				offset > dig_max_of_min)
+				dig_dynamic_min = dig_max_of_min;
+			else
+				dig_dynamic_min =
+					dm_digtable->rssi_val_min - offset;
+
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "bOneEntryOnly=TRUE, dig_dynamic_min=0x%x\n",
+				 dig_dynamic_min);
+		} else {
+			dig_dynamic_min = dm_dig_min;
+		}
+	} else {
+		dm_digtable->rx_gain_max = dm_dig_max;
+		dig_dynamic_min = dm_dig_min;
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "No Link\n");
+	}
+
+	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "Abnornally false alarm case.\n");
+
+		if (dm_digtable->large_fa_hit != 3)
+			dm_digtable->large_fa_hit++;
+		if (dm_digtable->forbidden_igi < current_igi) {
+			dm_digtable->forbidden_igi = current_igi;
+			dm_digtable->large_fa_hit = 1;
+		}
+
+		if (dm_digtable->large_fa_hit >= 3) {
+			if ((dm_digtable->forbidden_igi + 1) >
+				dm_digtable->rx_gain_max)
+				dm_digtable->rx_gain_min =
+					dm_digtable->rx_gain_max;
+			else
+				dm_digtable->rx_gain_min =
+					(dm_digtable->forbidden_igi + 1);
+			dm_digtable->recover_cnt = 3600;
+		}
+	} else {
+		/*Recovery mechanism for IGI lower bound*/
+		if (dm_digtable->recover_cnt != 0) {
+			dm_digtable->recover_cnt--;
+		} else {
+			if (dm_digtable->large_fa_hit < 3) {
+				if ((dm_digtable->forbidden_igi - 1) <
+				    dig_dynamic_min) {
+					dm_digtable->forbidden_igi =
+						dig_dynamic_min;
+					dm_digtable->rx_gain_min =
+						dig_dynamic_min;
+					RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+						 "Normal Case: At Lower Bound\n");
+				} else {
+					dm_digtable->forbidden_igi--;
+					dm_digtable->rx_gain_min =
+					  (dm_digtable->forbidden_igi + 1);
+					RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+						 "Normal Case: Approach Lower Bound\n");
+				}
+			} else {
+				dm_digtable->large_fa_hit = 0;
+			}
+		}
+	}
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "pDM_DigTable->LargeFAHit=%d\n",
+		 dm_digtable->large_fa_hit);
+
+	if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 10)
+		dm_digtable->rx_gain_min = dm_dig_min;
+
+	if (dm_digtable->rx_gain_min > dm_digtable->rx_gain_max)
+		dm_digtable->rx_gain_min = dm_digtable->rx_gain_max;
+
+	/*Adjust initial gain by false alarm*/
+	if (mac->link_state >= MAC80211_LINKED) {
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "DIG AfterLink\n");
+		if (first_connect) {
+			if (dm_digtable->rssi_val_min <= dig_max_of_min)
+				current_igi = dm_digtable->rssi_val_min;
+			else
+				current_igi = dig_max_of_min;
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "First Connect\n");
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH2)
+				current_igi = current_igi + 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH1)
+				current_igi = current_igi + 2;
+			else if (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
+				current_igi = current_igi - 2;
+
+			if ((rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 10) &&
+			    (rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH1)) {
+				current_igi = dm_digtable->rx_gain_min;
+				RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+					 "Beacon is less than 10 and FA is less than 768, IGI GOES TO 0x1E!!!!!!!!!!!!\n");
+			}
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "DIG BeforeLink\n");
+		if (first_disconnect) {
+			current_igi = dm_digtable->rx_gain_min;
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "First DisConnect\n");
+		} else {
+			/* 2012.03.30 LukeLee: enable DIG before
+			 * link but with very high thresholds
+			 */
+			if (rtlpriv->falsealm_cnt.cnt_all > 2000)
+				current_igi = current_igi + 4;
+			else if (rtlpriv->falsealm_cnt.cnt_all > 600)
+				current_igi = current_igi + 2;
+			else if (rtlpriv->falsealm_cnt.cnt_all < 300)
+				current_igi = current_igi - 2;
+
+			if (current_igi >= 0x3e)
+				current_igi = 0x3e;
+
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "England DIG\n");
+		}
+	}
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "DIG End Adjust IGI\n");
+	/* Check initial gain by upper/lower bound*/
+
+	if (current_igi > dm_digtable->rx_gain_max)
+		current_igi = dm_digtable->rx_gain_max;
+	if (current_igi < dm_digtable->rx_gain_min)
+		current_igi = dm_digtable->rx_gain_min;
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "rx_gain_max=0x%x, rx_gain_min=0x%x\n",
+		dm_digtable->rx_gain_max, dm_digtable->rx_gain_min);
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "TotalFA=%d\n", rtlpriv->falsealm_cnt.cnt_all);
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+		 "CurIGValue=0x%x\n", current_igi);
+
+	rtl8821ae_dm_write_dig(hw, current_igi);
+	dm_digtable->media_connect_0 =
+		((mac->link_state >= MAC80211_LINKED) ? true : false);
+	dm_digtable->dig_dynamic_min = dig_dynamic_min;
+}
+
+static void rtl8821ae_dm_common_info_self_update(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 cnt = 0;
+	struct rtl_sta_info *drv_priv;
+
+	rtlpriv->dm.tx_rate = 0xff;
+
+	rtlpriv->dm.one_entry_only = false;
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
+	    rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		rtlpriv->dm.one_entry_only = true;
+		return;
+	}
+
+	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC ||
+	    rtlpriv->mac80211.opmode == NL80211_IFTYPE_MESH_POINT) {
+		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
+		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list)
+			cnt++;
+		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
+
+		if (cnt == 1)
+			rtlpriv->dm.one_entry_only = true;
+	}
+}
+
+static void rtl8821ae_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct false_alarm_statistics *falsealm_cnt = &rtlpriv->falsealm_cnt;
+	u32 cck_enable = 0;
+
+	/*read OFDM FA counter*/
+	falsealm_cnt->cnt_ofdm_fail =
+		rtl_get_bbreg(hw, ODM_REG_OFDM_FA_11AC, BMASKLWORD);
+	falsealm_cnt->cnt_cck_fail =
+		rtl_get_bbreg(hw, ODM_REG_CCK_FA_11AC, BMASKLWORD);
+
+	cck_enable =  rtl_get_bbreg(hw, ODM_REG_BB_RX_PATH_11AC, BIT(28));
+	if (cck_enable)  /*if(pDM_Odm->pBandType == ODM_BAND_2_4G)*/
+		falsealm_cnt->cnt_all = falsealm_cnt->cnt_ofdm_fail +
+					falsealm_cnt->cnt_cck_fail;
+	else
+		falsealm_cnt->cnt_all = falsealm_cnt->cnt_ofdm_fail;
+
+	/*reset OFDM FA coutner*/
+	rtl_set_bbreg(hw, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 1);
+	rtl_set_bbreg(hw, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 0);
+	/* reset CCK FA counter*/
+	rtl_set_bbreg(hw, ODM_REG_CCK_FA_RST_11AC, BIT(15), 0);
+	rtl_set_bbreg(hw, ODM_REG_CCK_FA_RST_11AC, BIT(15), 1);
+
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "Cnt_Cck_fail=%d\n",
+		 falsealm_cnt->cnt_cck_fail);
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "cnt_ofdm_fail=%d\n",
+		 falsealm_cnt->cnt_ofdm_fail);
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "Total False Alarm=%d\n",
+		 falsealm_cnt->cnt_all);
+}
+
+static void rtl8812ae_dm_check_txpower_tracking_thermalmeter(
+		struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	static u8 tm_trigger;
+
+	if (!tm_trigger) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E,
+			      BIT(17) | BIT(16), 0x03);
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Trigger 8812 Thermal Meter!!\n");
+		tm_trigger = 1;
+		return;
+	}
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "Schedule TxPowerTracking direct call!!\n");
+	rtl8812ae_dm_txpower_tracking_callback_thermalmeter(hw);
+	tm_trigger = 0;
+}
+
+static void rtl8821ae_dm_iq_calibrate(struct ieee80211_hw *hw)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (mac->link_state >= MAC80211_LINKED) {
+		if (rtldm->linked_interval < 3)
+			rtldm->linked_interval++;
+
+		if (rtldm->linked_interval == 2) {
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+				rtl8812ae_phy_iq_calibrate(hw, false);
+			else
+				rtl8821ae_phy_iq_calibrate(hw, false);
+		}
+	} else {
+		rtldm->linked_interval = 0;
+	}
+}
+
+static void rtl8812ae_get_delta_swing_table(struct ieee80211_hw *hw,
+					    u8 **up_a, u8 **down_a,
+					    u8 **up_b, u8 **down_b)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 channel = rtlphy->current_channel;
+	u8 rate = rtldm->tx_rate;
+
+	if (1 <= channel && channel <= 14) {
+		if (RTL8821AE_RX_HAL_IS_CCK_RATE(rate)) {
+			*up_a = rtl8812ae_delta_swing_table_idx_24gccka_p;
+			*down_a = rtl8812ae_delta_swing_table_idx_24gccka_n;
+			*up_b = rtl8812ae_delta_swing_table_idx_24gcckb_p;
+			*down_b = rtl8812ae_delta_swing_table_idx_24gcckb_n;
+		} else {
+			*up_a = rtl8812ae_delta_swing_table_idx_24ga_p;
+			*down_a = rtl8812ae_delta_swing_table_idx_24ga_n;
+			*up_b = rtl8812ae_delta_swing_table_idx_24gb_p;
+			*down_b = rtl8812ae_delta_swing_table_idx_24gb_n;
+		}
+	} else if (36 <= channel && channel <= 64) {
+		*up_a = rtl8812ae_delta_swing_table_idx_5ga_p[0];
+		*down_a = rtl8812ae_delta_swing_table_idx_5ga_n[0];
+		*up_b = rtl8812ae_delta_swing_table_idx_5gb_p[0];
+		*down_b = rtl8812ae_delta_swing_table_idx_5gb_n[0];
+	} else if (100 <= channel && channel <= 140) {
+		*up_a = rtl8812ae_delta_swing_table_idx_5ga_p[1];
+		*down_a = rtl8812ae_delta_swing_table_idx_5ga_n[1];
+		*up_b = rtl8812ae_delta_swing_table_idx_5gb_p[1];
+		*down_b = rtl8812ae_delta_swing_table_idx_5gb_n[1];
+	} else if (149 <= channel && channel <= 173) {
+		*up_a = rtl8812ae_delta_swing_table_idx_5ga_p[2];
+		*down_a = rtl8812ae_delta_swing_table_idx_5ga_n[2];
+		*up_b = rtl8812ae_delta_swing_table_idx_5gb_p[2];
+		*down_b = rtl8812ae_delta_swing_table_idx_5gb_n[2];
+	} else {
+	    *up_a = (u8 *)rtl8818e_delta_swing_table_idx_24gb_p;
+	    *down_a = (u8 *)rtl8818e_delta_swing_table_idx_24gb_n;
+	    *up_b = (u8 *)rtl8818e_delta_swing_table_idx_24gb_p;
+	    *down_b = (u8 *)rtl8818e_delta_swing_table_idx_24gb_n;
+	}
+}
+
+void rtl8821ae_dm_update_init_rate(struct ieee80211_hw *hw, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 p = 0;
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "Get C2H Command! Rate=0x%x\n", rate);
+
+	rtldm->tx_rate = rate;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		rtl8821ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, RF90_PATH_A, 0);
+	} else {
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+			rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, 0);
+	}
+}
+
+u8 rtl8821ae_hw_rate_to_mrate(struct ieee80211_hw *hw, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 ret_rate = MGN_1M;
+
+	switch (rate) {
+	case DESC_RATE1M:
+		ret_rate = MGN_1M;
+		break;
+	case DESC_RATE2M:
+		ret_rate = MGN_2M;
+		break;
+	case DESC_RATE5_5M:
+		ret_rate = MGN_5_5M;
+		break;
+	case DESC_RATE11M:
+		ret_rate = MGN_11M;
+		break;
+	case DESC_RATE6M:
+		ret_rate = MGN_6M;
+		break;
+	case DESC_RATE9M:
+		ret_rate = MGN_9M;
+		break;
+	case DESC_RATE12M:
+		ret_rate = MGN_12M;
+		break;
+	case DESC_RATE18M:
+		ret_rate = MGN_18M;
+		break;
+	case DESC_RATE24M:
+		ret_rate = MGN_24M;
+		break;
+	case DESC_RATE36M:
+		ret_rate = MGN_36M;
+		break;
+	case DESC_RATE48M:
+		ret_rate = MGN_48M;
+		break;
+	case DESC_RATE54M:
+		ret_rate = MGN_54M;
+		break;
+	case DESC_RATEMCS0:
+		ret_rate = MGN_MCS0;
+		break;
+	case DESC_RATEMCS1:
+		ret_rate = MGN_MCS1;
+		break;
+	case DESC_RATEMCS2:
+		ret_rate = MGN_MCS2;
+		break;
+	case DESC_RATEMCS3:
+		ret_rate = MGN_MCS3;
+		break;
+	case DESC_RATEMCS4:
+		ret_rate = MGN_MCS4;
+		break;
+	case DESC_RATEMCS5:
+		ret_rate = MGN_MCS5;
+		break;
+	case DESC_RATEMCS6:
+		ret_rate = MGN_MCS6;
+		break;
+	case DESC_RATEMCS7:
+		ret_rate = MGN_MCS7;
+		break;
+	case DESC_RATEMCS8:
+		ret_rate = MGN_MCS8;
+		break;
+	case DESC_RATEMCS9:
+		ret_rate = MGN_MCS9;
+		break;
+	case DESC_RATEMCS10:
+		ret_rate = MGN_MCS10;
+		break;
+	case DESC_RATEMCS11:
+		ret_rate = MGN_MCS11;
+		break;
+	case DESC_RATEMCS12:
+		ret_rate = MGN_MCS12;
+		break;
+	case DESC_RATEMCS13:
+		ret_rate = MGN_MCS13;
+		break;
+	case DESC_RATEMCS14:
+		ret_rate = MGN_MCS14;
+		break;
+	case DESC_RATEMCS15:
+		ret_rate = MGN_MCS15;
+		break;
+	case DESC_RATEVHT1SS_MCS0:
+		ret_rate = MGN_VHT1SS_MCS0;
+		break;
+	case DESC_RATEVHT1SS_MCS1:
+		ret_rate = MGN_VHT1SS_MCS1;
+		break;
+	case DESC_RATEVHT1SS_MCS2:
+		ret_rate = MGN_VHT1SS_MCS2;
+		break;
+	case DESC_RATEVHT1SS_MCS3:
+		ret_rate = MGN_VHT1SS_MCS3;
+		break;
+	case DESC_RATEVHT1SS_MCS4:
+		ret_rate = MGN_VHT1SS_MCS4;
+		break;
+	case DESC_RATEVHT1SS_MCS5:
+		ret_rate = MGN_VHT1SS_MCS5;
+		break;
+	case DESC_RATEVHT1SS_MCS6:
+		ret_rate = MGN_VHT1SS_MCS6;
+		break;
+	case DESC_RATEVHT1SS_MCS7:
+		ret_rate = MGN_VHT1SS_MCS7;
+		break;
+	case DESC_RATEVHT1SS_MCS8:
+		ret_rate = MGN_VHT1SS_MCS8;
+		break;
+	case DESC_RATEVHT1SS_MCS9:
+		ret_rate = MGN_VHT1SS_MCS9;
+		break;
+	case DESC_RATEVHT2SS_MCS0:
+		ret_rate = MGN_VHT2SS_MCS0;
+		break;
+	case DESC_RATEVHT2SS_MCS1:
+		ret_rate = MGN_VHT2SS_MCS1;
+		break;
+	case DESC_RATEVHT2SS_MCS2:
+		ret_rate = MGN_VHT2SS_MCS2;
+		break;
+	case DESC_RATEVHT2SS_MCS3:
+		ret_rate = MGN_VHT2SS_MCS3;
+		break;
+	case DESC_RATEVHT2SS_MCS4:
+		ret_rate = MGN_VHT2SS_MCS4;
+		break;
+	case DESC_RATEVHT2SS_MCS5:
+		ret_rate = MGN_VHT2SS_MCS5;
+		break;
+	case DESC_RATEVHT2SS_MCS6:
+		ret_rate = MGN_VHT2SS_MCS6;
+		break;
+	case DESC_RATEVHT2SS_MCS7:
+		ret_rate = MGN_VHT2SS_MCS7;
+		break;
+	case DESC_RATEVHT2SS_MCS8:
+		ret_rate = MGN_VHT2SS_MCS8;
+		break;
+	case DESC_RATEVHT2SS_MCS9:
+		ret_rate = MGN_VHT2SS_MCS9;
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "HwRateToMRate8812(): Non supported Rate [%x]!!!\n",
+			 rate);
+		break;
+	}
+	return ret_rate;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	odm_TxPwrTrackSetPwr88E()
+ *
+ * Overview:	88E change all channel tx power accordign to flag.
+ *				OFDM & CCK are all different.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	04/23/2012	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------
+ */
+void rtl8812ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
+				      enum pwr_track_control_method method,
+				      u8 rf_path, u8 channel_mapped_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u32 final_swing_idx[2];
+	u8 pwr_tracking_limit = 26; /*+1.0dB*/
+	u8 tx_rate = 0xFF;
+	char final_ofdm_swing_index = 0;
+
+	if (rtldm->tx_rate != 0xFF)
+		tx_rate =
+			rtl8821ae_hw_rate_to_mrate(hw, rtldm->tx_rate);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "===>rtl8812ae_dm_txpwr_track_set_pwr\n");
+	/*20130429 Mimic Modify High Rate BBSwing Limit.*/
+	if (tx_rate != 0xFF) {
+		/*CCK*/
+		if ((tx_rate >= MGN_1M) && (tx_rate <= MGN_11M))
+			pwr_tracking_limit = 32; /*+4dB*/
+		/*OFDM*/
+		else if ((tx_rate >= MGN_6M) && (tx_rate <= MGN_48M))
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if (tx_rate == MGN_54M)
+			pwr_tracking_limit = 28; /*+2dB*/
+		/*HT*/
+		 /*QPSK/BPSK*/
+		else if ((tx_rate >= MGN_MCS0) && (tx_rate <= MGN_MCS2))
+			pwr_tracking_limit = 34; /*+5dB*/
+		 /*16QAM*/
+		else if ((tx_rate >= MGN_MCS3) && (tx_rate <= MGN_MCS4))
+			pwr_tracking_limit = 30; /*+3dB*/
+		 /*64QAM*/
+		else if ((tx_rate >= MGN_MCS5) && (tx_rate <= MGN_MCS7))
+			pwr_tracking_limit = 28; /*+2dB*/
+		 /*QPSK/BPSK*/
+		else if ((tx_rate >= MGN_MCS8) && (tx_rate <= MGN_MCS10))
+			pwr_tracking_limit = 34; /*+5dB*/
+		 /*16QAM*/
+		else if ((tx_rate >= MGN_MCS11) && (tx_rate <= MGN_MCS12))
+			pwr_tracking_limit = 30; /*+3dB*/
+		 /*64QAM*/
+		else if ((tx_rate >= MGN_MCS13) && (tx_rate <= MGN_MCS15))
+			pwr_tracking_limit = 28; /*+2dB*/
+
+		/*2 VHT*/
+		 /*QPSK/BPSK*/
+		else if ((tx_rate >= MGN_VHT1SS_MCS0) &&
+			 (tx_rate <= MGN_VHT1SS_MCS2))
+			pwr_tracking_limit = 34; /*+5dB*/
+		 /*16QAM*/
+		else if ((tx_rate >= MGN_VHT1SS_MCS3) &&
+			 (tx_rate <= MGN_VHT1SS_MCS4))
+			pwr_tracking_limit = 30; /*+3dB*/
+		 /*64QAM*/
+		else if ((tx_rate >= MGN_VHT1SS_MCS5) &&
+			 (tx_rate <= MGN_VHT1SS_MCS6))
+			pwr_tracking_limit = 28; /*+2dB*/
+		else if (tx_rate == MGN_VHT1SS_MCS7) /*64QAM*/
+			pwr_tracking_limit = 26; /*+1dB*/
+		else if (tx_rate == MGN_VHT1SS_MCS8) /*256QAM*/
+			pwr_tracking_limit = 24; /*+0dB*/
+		else if (tx_rate == MGN_VHT1SS_MCS9) /*256QAM*/
+			pwr_tracking_limit = 22; /*-1dB*/
+		 /*QPSK/BPSK*/
+		else if ((tx_rate >= MGN_VHT2SS_MCS0) &&
+			 (tx_rate <= MGN_VHT2SS_MCS2))
+			pwr_tracking_limit = 34; /*+5dB*/
+		 /*16QAM*/
+		else if ((tx_rate >= MGN_VHT2SS_MCS3) &&
+			 (tx_rate <= MGN_VHT2SS_MCS4))
+			pwr_tracking_limit = 30; /*+3dB*/
+		 /*64QAM*/
+		else if ((tx_rate >= MGN_VHT2SS_MCS5) &&
+			 (tx_rate <= MGN_VHT2SS_MCS6))
+			pwr_tracking_limit = 28; /*+2dB*/
+		else if (tx_rate == MGN_VHT2SS_MCS7) /*64QAM*/
+			pwr_tracking_limit = 26; /*+1dB*/
+		else if (tx_rate == MGN_VHT2SS_MCS8) /*256QAM*/
+			pwr_tracking_limit = 24; /*+0dB*/
+		else if (tx_rate == MGN_VHT2SS_MCS9) /*256QAM*/
+			pwr_tracking_limit = 22; /*-1dB*/
+		else
+			pwr_tracking_limit = 24;
+	}
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "TxRate=0x%x, PwrTrackingLimit=%d\n",
+		 tx_rate, pwr_tracking_limit);
+
+	if (method == BBSWING) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "===>rtl8812ae_dm_txpwr_track_set_pwr\n");
+
+		if (rf_path == RF90_PATH_A) {
+			u32 tmp;
+
+			final_swing_idx[RF90_PATH_A] =
+				(rtldm->ofdm_index[RF90_PATH_A] >
+				pwr_tracking_limit) ?
+				pwr_tracking_limit :
+				rtldm->ofdm_index[RF90_PATH_A];
+			tmp = final_swing_idx[RF90_PATH_A];
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_A]=%d,pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_A]=%d\n",
+				 rtldm->ofdm_index[RF90_PATH_A],
+				 final_swing_idx[RF90_PATH_A]);
+
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+				      txscaling_tbl[tmp]);
+		} else {
+			u32 tmp;
+
+			final_swing_idx[RF90_PATH_B] =
+				rtldm->ofdm_index[RF90_PATH_B] >
+				pwr_tracking_limit ?
+				pwr_tracking_limit :
+				rtldm->ofdm_index[RF90_PATH_B];
+			tmp = final_swing_idx[RF90_PATH_B];
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_B]=%d, pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_B]=%d\n",
+				 rtldm->ofdm_index[RF90_PATH_B],
+				 final_swing_idx[RF90_PATH_B]);
+
+			rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000,
+				      txscaling_tbl[tmp]);
+		}
+	} else if (method == MIX_MODE) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "pDM_Odm->DefaultOfdmIndex=%d, pDM_Odm->Aboslute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
+			 rtldm->default_ofdm_index,
+			 rtldm->absolute_ofdm_swing_idx[rf_path],
+			 rf_path);
+
+		final_ofdm_swing_index = rtldm->default_ofdm_index +
+				rtldm->absolute_ofdm_swing_idx[rf_path];
+
+		if (rf_path == RF90_PATH_A) {
+			/*BBSwing higher then Limit*/
+			if (final_ofdm_swing_index > pwr_tracking_limit) {
+				rtldm->remnant_cck_idx =
+					final_ofdm_swing_index -
+					pwr_tracking_limit;
+				/* CCK Follow the same compensation value
+				 * as Path A
+				 */
+				rtldm->remnant_ofdm_swing_idx[rf_path] =
+					final_ofdm_swing_index -
+					pwr_tracking_limit;
+
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+					      txscaling_tbl[pwr_tracking_limit]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw,
+					rtlphy->current_channel,
+					RF90_PATH_A);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_A Over BBSwing Limit ,PwrTrackingLimit = %d ,Remnant TxAGC Value = %d\n",
+					 pwr_tracking_limit,
+					 rtldm->remnant_ofdm_swing_idx[rf_path]);
+			} else if (final_ofdm_swing_index < 0) {
+				rtldm->remnant_cck_idx = final_ofdm_swing_index;
+				/* CCK Follow the same compensate value as Path A*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] =
+					final_ofdm_swing_index;
+
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+					txscaling_tbl[0]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw,
+					rtlphy->current_channel, RF90_PATH_A);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d\n",
+					 rtldm->remnant_ofdm_swing_idx[rf_path]);
+			} else {
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+					txscaling_tbl[(u8)final_ofdm_swing_index]);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_A Compensate with BBSwing, Final_OFDM_Swing_Index = %d\n",
+					final_ofdm_swing_index);
+				/*If TxAGC has changed, reset TxAGC again*/
+				if (rtldm->modify_txagc_flag_path_a) {
+					rtldm->remnant_cck_idx = 0;
+					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
+
+					/*Set TxAGC Page C{};*/
+					rtl8821ae_phy_set_txpower_level_by_path(hw,
+						rtlphy->current_channel, RF90_PATH_A);
+					rtldm->modify_txagc_flag_path_a = false;
+
+					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+						 DBG_LOUD,
+						 "******Path_A pDM_Odm->Modify_TxAGC_Flag = FALSE\n");
+				}
+			}
+		}
+		/*BBSwing higher then Limit*/
+		if (rf_path == RF90_PATH_B) {
+			if (final_ofdm_swing_index > pwr_tracking_limit) {
+				rtldm->remnant_ofdm_swing_idx[rf_path] =
+					final_ofdm_swing_index -
+					pwr_tracking_limit;
+
+				rtl_set_bbreg(hw, RB_TXSCALE,
+					0xFFE00000,
+					txscaling_tbl[pwr_tracking_limit]);
+
+				rtldm->modify_txagc_flag_path_b = true;
+
+				/*Set TxAGC Page E{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw,
+					rtlphy->current_channel, RF90_PATH_B);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_B Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d\n",
+					 pwr_tracking_limit,
+					 rtldm->remnant_ofdm_swing_idx[rf_path]);
+			} else if (final_ofdm_swing_index < 0) {
+				rtldm->remnant_ofdm_swing_idx[rf_path] =
+					final_ofdm_swing_index;
+
+				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000,
+					      txscaling_tbl[0]);
+
+				rtldm->modify_txagc_flag_path_b = true;
+
+				/*Set TxAGC Page E{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw,
+					rtlphy->current_channel, RF90_PATH_B);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_B Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d\n",
+					 rtldm->remnant_ofdm_swing_idx[rf_path]);
+			} else {
+				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000,
+					txscaling_tbl[(u8)final_ofdm_swing_index]);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_B Compensate with BBSwing ,Final_OFDM_Swing_Index = %d\n",
+					final_ofdm_swing_index);
+				 /*If TxAGC has changed, reset TxAGC again*/
+				if (rtldm->modify_txagc_flag_path_b) {
+					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
+
+					/*Set TxAGC Page E{};*/
+					rtl8821ae_phy_set_txpower_level_by_path(hw,
+					rtlphy->current_channel, RF90_PATH_B);
+
+					rtldm->modify_txagc_flag_path_b =
+						false;
+
+					RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+						 "******Path_B pDM_Odm->Modify_TxAGC_Flag = FALSE\n");
+				}
+			}
+		}
+	} else {
+		return;
+	}
+}
+
+void rtl8812ae_dm_txpower_tracking_callback_thermalmeter(
+	struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 thermal_value = 0, delta, delta_lck, delta_iqk, p = 0, i = 0;
+	u8 thermal_value_avg_count = 0;
+	u32 thermal_value_avg = 0;
+	/* OFDM BB Swing should be less than +3.0dB, */
+	u8 ofdm_min_index = 6;
+	 /* GetRightChnlPlaceforIQK(pHalData->CurrentChannel)*/
+	u8 index_for_channel = 0;
+	/* 1. The following TWO tables decide
+	 * the final index of OFDM/CCK swing table.
+	 */
+	u8 *delta_swing_table_idx_tup_a;
+	u8 *delta_swing_table_idx_tdown_a;
+	u8 *delta_swing_table_idx_tup_b;
+	u8 *delta_swing_table_idx_tdown_b;
+
+	/*2. Initilization ( 7 steps in total )*/
+	rtl8812ae_get_delta_swing_table(hw,
+		(u8 **)&delta_swing_table_idx_tup_a,
+		(u8 **)&delta_swing_table_idx_tdown_a,
+		(u8 **)&delta_swing_table_idx_tup_b,
+		(u8 **)&delta_swing_table_idx_tdown_b);
+
+	rtldm->txpower_trackinginit = true;
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "pDM_Odm->BbSwingIdxCckBase: %d, pDM_Odm->BbSwingIdxOfdmBase[A]:%d, pDM_Odm->DefaultOfdmIndex: %d\n",
+		 rtldm->swing_idx_cck_base,
+		 rtldm->swing_idx_ofdm_base[RF90_PATH_A],
+		 rtldm->default_ofdm_index);
+
+	thermal_value = (u8)rtl_get_rfreg(hw, RF90_PATH_A,
+		/*0x42: RF Reg[15:10] 88E*/
+		RF_T_METER_8812A, 0xfc00);
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
+		 thermal_value, rtlefuse->eeprom_thermalmeter);
+	if (!rtldm->txpower_track_control ||
+	    rtlefuse->eeprom_thermalmeter == 0 ||
+	    rtlefuse->eeprom_thermalmeter == 0xFF)
+		return;
+
+	/* 3. Initialize ThermalValues of RFCalibrateInfo*/
+
+	if (rtlhal->reloadtxpowerindex)
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "reload ofdm index for band switch\n");
+
+	/*4. Calculate average thermal meter*/
+	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermal_value;
+	rtldm->thermalvalue_avg_index++;
+	if (rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_8812A)
+		/*Average times =  c.AverageThermalNum*/
+		rtldm->thermalvalue_avg_index = 0;
+
+	for (i = 0; i < AVG_THERMAL_NUM_8812A; i++) {
+		if (rtldm->thermalvalue_avg[i]) {
+			thermal_value_avg += rtldm->thermalvalue_avg[i];
+			thermal_value_avg_count++;
+		}
+	}
+	/*Calculate Average ThermalValue after average enough times*/
+	if (thermal_value_avg_count) {
+		thermal_value = (u8)(thermal_value_avg /
+				thermal_value_avg_count);
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
+			 thermal_value, rtlefuse->eeprom_thermalmeter);
+	}
+
+	/*5. Calculate delta, delta_LCK, delta_IQK.
+	 *"delta" here is used to determine whether
+	 *thermal value changes or not.
+	 */
+	delta = (thermal_value > rtldm->thermalvalue) ?
+		(thermal_value - rtldm->thermalvalue) :
+		(rtldm->thermalvalue - thermal_value);
+	delta_lck = (thermal_value > rtldm->thermalvalue_lck) ?
+		(thermal_value - rtldm->thermalvalue_lck) :
+		(rtldm->thermalvalue_lck - thermal_value);
+	delta_iqk = (thermal_value > rtldm->thermalvalue_iqk) ?
+		(thermal_value - rtldm->thermalvalue_iqk) :
+		(rtldm->thermalvalue_iqk - thermal_value);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
+		 delta, delta_lck, delta_iqk);
+
+	/* 6. If necessary, do LCK.
+	 * Delta temperature is equal to or larger than 20 centigrade.
+	 */
+	if (delta_lck >= IQK_THRESHOLD) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "delta_LCK(%d) >= Threshold_IQK(%d)\n",
+			 delta_lck, IQK_THRESHOLD);
+		rtldm->thermalvalue_lck = thermal_value;
+		rtl8821ae_phy_lc_calibrate(hw);
+	}
+
+	/*7. If necessary, move the index of swing table to adjust Tx power.*/
+
+	if (delta > 0 && rtldm->txpower_track_control) {
+		/* "delta" here is used to record the
+		 * absolute value of differrence.
+		 */
+		delta = thermal_value > rtlefuse->eeprom_thermalmeter ?
+			(thermal_value - rtlefuse->eeprom_thermalmeter) :
+			(rtlefuse->eeprom_thermalmeter - thermal_value);
+
+		if (delta >= TXPWR_TRACK_TABLE_SIZE)
+			delta = TXPWR_TRACK_TABLE_SIZE - 1;
+
+		/*7.1 The Final Power Index = BaseIndex + PowerIndexOffset*/
+
+		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "delta_swing_table_idx_tup_a[%d] = %d\n",
+				 delta, delta_swing_table_idx_tup_a[delta]);
+			rtldm->delta_power_index_last[RF90_PATH_A] =
+				rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] =
+				delta_swing_table_idx_tup_a[delta];
+
+			rtldm->absolute_ofdm_swing_idx[RF90_PATH_A] =
+				delta_swing_table_idx_tup_a[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+			rtldm->absolute_ofdm_swing_idx[RF90_PATH_A]);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "delta_swing_table_idx_tup_b[%d] = %d\n",
+				 delta, delta_swing_table_idx_tup_b[delta]);
+			rtldm->delta_power_index_last[RF90_PATH_B] =
+				rtldm->delta_power_index[RF90_PATH_B];
+			rtldm->delta_power_index[RF90_PATH_B] =
+				delta_swing_table_idx_tup_b[delta];
+
+			rtldm->absolute_ofdm_swing_idx[RF90_PATH_B] =
+				delta_swing_table_idx_tup_b[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+				 rtldm->absolute_ofdm_swing_idx[RF90_PATH_B]);
+		} else {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "delta_swing_table_idx_tdown_a[%d] = %d\n",
+				 delta, delta_swing_table_idx_tdown_a[delta]);
+
+			rtldm->delta_power_index_last[RF90_PATH_A] =
+				rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] =
+				-1 * delta_swing_table_idx_tdown_a[delta];
+
+			rtldm->absolute_ofdm_swing_idx[RF90_PATH_A] =
+				-1 * delta_swing_table_idx_tdown_a[delta];
+			/* Record delta swing for mix mode power tracking*/
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+				 rtldm->absolute_ofdm_swing_idx[RF90_PATH_A]);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "deltaSwingTableIdx_TDOWN_B[%d] = %d\n",
+				 delta, delta_swing_table_idx_tdown_b[delta]);
+
+			rtldm->delta_power_index_last[RF90_PATH_B] =
+				rtldm->delta_power_index[RF90_PATH_B];
+			rtldm->delta_power_index[RF90_PATH_B] =
+				-1 * delta_swing_table_idx_tdown_b[delta];
+
+			rtldm->absolute_ofdm_swing_idx[RF90_PATH_B] =
+				-1 * delta_swing_table_idx_tdown_b[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
+				 rtldm->absolute_ofdm_swing_idx[RF90_PATH_B]);
+		}
+
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "============================= [Path-%c]Calculating PowerIndexOffset =============================\n",
+				 (p == RF90_PATH_A ? 'A' : 'B'));
+
+			if (rtldm->delta_power_index[p] ==
+				rtldm->delta_power_index_last[p])
+				/*If Thermal value changes but lookup
+				table value still the same*/
+				rtldm->power_index_offset[p] = 0;
+			else
+				rtldm->power_index_offset[p] =
+					rtldm->delta_power_index[p] -
+					rtldm->delta_power_index_last[p];
+				/* Power Index Diff between 2
+				 * times Power Tracking
+				 */
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "[Path-%c] PowerIndexOffset(%d) =DeltaPowerIndex(%d) -DeltaPowerIndexLast(%d)\n",
+				 (p == RF90_PATH_A ? 'A' : 'B'),
+				 rtldm->power_index_offset[p],
+				 rtldm->delta_power_index[p] ,
+				 rtldm->delta_power_index_last[p]);
+
+			rtldm->ofdm_index[p] =
+					rtldm->swing_idx_ofdm_base[p] +
+					rtldm->power_index_offset[p];
+			rtldm->cck_index =
+					rtldm->swing_idx_cck_base +
+					rtldm->power_index_offset[p];
+
+			rtldm->swing_idx_cck = rtldm->cck_index;
+			rtldm->swing_idx_ofdm[p] = rtldm->ofdm_index[p];
+
+			/****Print BB Swing Base and Index Offset */
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
+				 rtldm->swing_idx_cck,
+				rtldm->swing_idx_cck_base,
+				rtldm->power_index_offset[p]);
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
+				 rtldm->swing_idx_ofdm[p],
+				 (p == RF90_PATH_A ? 'A' : 'B'),
+				 rtldm->swing_idx_ofdm_base[p],
+				 rtldm->power_index_offset[p]);
+
+			/*7.1 Handle boundary conditions of index.*/
+
+			if (rtldm->ofdm_index[p] > TXSCALE_TABLE_SIZE - 1)
+				rtldm->ofdm_index[p] = TXSCALE_TABLE_SIZE - 1;
+			else if (rtldm->ofdm_index[p] < ofdm_min_index)
+				rtldm->ofdm_index[p] = ofdm_min_index;
+		}
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "\n\n====================================================================================\n");
+		if (rtldm->cck_index > TXSCALE_TABLE_SIZE - 1)
+			rtldm->cck_index = TXSCALE_TABLE_SIZE - 1;
+		else if (rtldm->cck_index < 0)
+			rtldm->cck_index = 0;
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "The thermal meter is unchanged or TxPowerTracking OFF(%d): ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
+			 rtldm->txpower_track_control,
+			 thermal_value,
+			 rtldm->thermalvalue);
+
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+			rtldm->power_index_offset[p] = 0;
+	}
+	/*Print Swing base & current*/
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "TxPowerTracking: [CCK] Swing Current Index: %d,Swing Base Index: %d\n",
+		 rtldm->cck_index, rtldm->swing_idx_cck_base);
+	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "TxPowerTracking: [OFDM] Swing Current Index: %d,Swing Base Index[%c]: %d\n",
+			 rtldm->ofdm_index[p],
+			 (p == RF90_PATH_A ? 'A' : 'B'),
+			 rtldm->swing_idx_ofdm_base[p]);
+	}
+
+	if ((rtldm->power_index_offset[RF90_PATH_A] != 0 ||
+		rtldm->power_index_offset[RF90_PATH_B] != 0) &&
+		rtldm->txpower_track_control) {
+		/*7.2 Configure the Swing Table to adjust Tx Power.
+		 *Always TRUE after Tx Power is adjusted by power tracking.
+		 *
+		 *2012/04/23 MH According to Luke's suggestion,
+		 *we can not write BB digital
+		 *to increase TX power. Otherwise, EVM will be bad.
+		 *
+		 *2012/04/25 MH Add for tx power tracking to set
+		 *tx power in tx agc for 88E.
+		 */
+		if (thermal_value > rtldm->thermalvalue) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature Increasing(A): delta_pi: %d , delta_t: %d, Now_t: %d,EFUSE_t: %d, Last_t: %d\n",
+				 rtldm->power_index_offset[RF90_PATH_A],
+				 delta, thermal_value,
+				 rtlefuse->eeprom_thermalmeter,
+				 rtldm->thermalvalue);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature Increasing(B): delta_pi: %d ,delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				 rtldm->power_index_offset[RF90_PATH_B],
+				 delta, thermal_value,
+				 rtlefuse->eeprom_thermalmeter,
+				 rtldm->thermalvalue);
+		} else if (thermal_value < rtldm->thermalvalue) { /*Low temperature*/
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				 rtldm->power_index_offset[RF90_PATH_A],
+				 delta, thermal_value,
+				 rtlefuse->eeprom_thermalmeter,
+				 rtldm->thermalvalue);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature Decreasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				 rtldm->power_index_offset[RF90_PATH_B],
+				 delta, thermal_value,
+				 rtlefuse->eeprom_thermalmeter,
+				 rtldm->thermalvalue);
+		}
+
+		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature(%d) higher than PG value(%d)\n",
+				 thermal_value, rtlefuse->eeprom_thermalmeter);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "**********Enter POWER Tracking MIX_MODE**********\n");
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+				rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE,
+								 p, 0);
+		} else {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature(%d) lower than PG value(%d)\n",
+				 thermal_value, rtlefuse->eeprom_thermalmeter);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "**********Enter POWER Tracking MIX_MODE**********\n");
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+				rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE,
+								 p, index_for_channel);
+		}
+		/*Record last time Power Tracking result as base.*/
+		rtldm->swing_idx_cck_base = rtldm->swing_idx_cck;
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
+				rtldm->swing_idx_ofdm_base[p] =
+					rtldm->swing_idx_ofdm[p];
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "pDM_Odm->RFCalibrateInfo.ThermalValue =%d ThermalValue= %d\n",
+				 rtldm->thermalvalue, thermal_value);
+		/*Record last Power Tracking Thermal Value*/
+		rtldm->thermalvalue = thermal_value;
+	}
+	/*Delta temperature is equal to or larger than
+	20 centigrade (When threshold is 8).*/
+	if (delta_iqk >= IQK_THRESHOLD)
+		rtl8812ae_do_iqk(hw, delta_iqk, thermal_value, 8);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "<===rtl8812ae_dm_txpower_tracking_callback_thermalmeter\n");
+}
+
+static void rtl8821ae_get_delta_swing_table(struct ieee80211_hw *hw, u8 **up_a,
+					    u8 **down_a, u8 **up_b, u8 **down_b)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 channel = rtlphy->current_channel;
+	u8 rate = rtldm->tx_rate;
+
+	if (1 <= channel && channel <= 14) {
+		if (RTL8821AE_RX_HAL_IS_CCK_RATE(rate)) {
+			*up_a = rtl8821ae_delta_swing_table_idx_24gccka_p;
+			*down_a = rtl8821ae_delta_swing_table_idx_24gccka_n;
+			*up_b = rtl8821ae_delta_swing_table_idx_24gcckb_p;
+			*down_b = rtl8821ae_delta_swing_table_idx_24gcckb_n;
+		} else {
+			*up_a = rtl8821ae_delta_swing_table_idx_24ga_p;
+			*down_a = rtl8821ae_delta_swing_table_idx_24ga_n;
+			*up_b = rtl8821ae_delta_swing_table_idx_24gb_p;
+			*down_b = rtl8821ae_delta_swing_table_idx_24gb_n;
+		}
+	} else if (36 <= channel && channel <= 64) {
+		*up_a = rtl8821ae_delta_swing_table_idx_5ga_p[0];
+		*down_a = rtl8821ae_delta_swing_table_idx_5ga_n[0];
+		*up_b = rtl8821ae_delta_swing_table_idx_5gb_p[0];
+		*down_b = rtl8821ae_delta_swing_table_idx_5gb_n[0];
+	} else if (100 <= channel && channel <= 140) {
+		*up_a = rtl8821ae_delta_swing_table_idx_5ga_p[1];
+		*down_a = rtl8821ae_delta_swing_table_idx_5ga_n[1];
+		*up_b = rtl8821ae_delta_swing_table_idx_5gb_p[1];
+		*down_b = rtl8821ae_delta_swing_table_idx_5gb_n[1];
+	} else if (149 <= channel && channel <= 173) {
+		*up_a = rtl8821ae_delta_swing_table_idx_5ga_p[2];
+		*down_a = rtl8821ae_delta_swing_table_idx_5ga_n[2];
+		*up_b = rtl8821ae_delta_swing_table_idx_5gb_p[2];
+		*down_b = rtl8821ae_delta_swing_table_idx_5gb_n[2];
+	} else {
+	    *up_a = (u8 *)rtl8818e_delta_swing_table_idx_24gb_p;
+	    *down_a = (u8 *)rtl8818e_delta_swing_table_idx_24gb_n;
+	    *up_b = (u8 *)rtl8818e_delta_swing_table_idx_24gb_p;
+	    *down_b = (u8 *)rtl8818e_delta_swing_table_idx_24gb_n;
+	}
+	return;
+}
+
+/*-----------------------------------------------------------------------------
+ * Function:	odm_TxPwrTrackSetPwr88E()
+ *
+ * Overview:	88E change all channel tx power accordign to flag.
+ *				OFDM & CCK are all different.
+ *
+ * Input:		NONE
+ *
+ * Output:		NONE
+ *
+ * Return:		NONE
+ *
+ * Revised History:
+ *	When		Who		Remark
+ *	04/23/2012	MHC		Create Version 0.
+ *
+ *---------------------------------------------------------------------------
+ */
+void rtl8821ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
+				      enum pwr_track_control_method method,
+				      u8 rf_path, u8 channel_mapped_index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u32 final_swing_idx[1];
+	u8 pwr_tracking_limit = 26; /*+1.0dB*/
+	u8 tx_rate = 0xFF;
+	char final_ofdm_swing_index = 0;
+
+	if (rtldm->tx_rate != 0xFF)
+		tx_rate = rtl8821ae_hw_rate_to_mrate(hw, rtldm->tx_rate);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "===>rtl8812ae_dm_txpwr_track_set_pwr\n");
+
+	if (tx_rate != 0xFF) { /* Mimic Modify High Rate BBSwing Limit.*/
+		/*CCK*/
+		if ((tx_rate >= MGN_1M) && (tx_rate <= MGN_11M))
+			pwr_tracking_limit = 32; /*+4dB*/
+		/*OFDM*/
+		else if ((tx_rate >= MGN_6M) && (tx_rate <= MGN_48M))
+			pwr_tracking_limit = 30; /*+3dB*/
+		else if (tx_rate == MGN_54M)
+			pwr_tracking_limit = 28; /*+2dB*/
+		/*HT*/
+		/*QPSK/BPSK*/
+		else if ((tx_rate >= MGN_MCS0) && (tx_rate <= MGN_MCS2))
+			pwr_tracking_limit = 34; /*+5dB*/
+		/*16QAM*/
+		else if ((tx_rate >= MGN_MCS3) && (tx_rate <= MGN_MCS4))
+			pwr_tracking_limit = 30; /*+3dB*/
+		/*64QAM*/
+		else if ((tx_rate >= MGN_MCS5) && (tx_rate <= MGN_MCS7))
+			pwr_tracking_limit = 28; /*+2dB*/
+		/*2 VHT*/
+		/*QPSK/BPSK*/
+		else if ((tx_rate >= MGN_VHT1SS_MCS0) &&
+			(tx_rate <= MGN_VHT1SS_MCS2))
+			pwr_tracking_limit = 34; /*+5dB*/
+		/*16QAM*/
+		else if ((tx_rate >= MGN_VHT1SS_MCS3) &&
+			(tx_rate <= MGN_VHT1SS_MCS4))
+			pwr_tracking_limit = 30; /*+3dB*/
+		/*64QAM*/
+		else if ((tx_rate >= MGN_VHT1SS_MCS5) &&
+			(tx_rate <= MGN_VHT1SS_MCS6))
+			pwr_tracking_limit = 28; /*+2dB*/
+		else if (tx_rate == MGN_VHT1SS_MCS7) /*64QAM*/
+			pwr_tracking_limit = 26; /*+1dB*/
+		else if (tx_rate == MGN_VHT1SS_MCS8) /*256QAM*/
+			pwr_tracking_limit = 24; /*+0dB*/
+		else if (tx_rate == MGN_VHT1SS_MCS9) /*256QAM*/
+			pwr_tracking_limit = 22; /*-1dB*/
+		else
+			pwr_tracking_limit = 24;
+	}
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "TxRate=0x%x, PwrTrackingLimit=%d\n",
+		 tx_rate, pwr_tracking_limit);
+
+	if (method == BBSWING) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "===>rtl8812ae_dm_txpwr_track_set_pwr\n");
+		if (rf_path == RF90_PATH_A) {
+			final_swing_idx[RF90_PATH_A] =
+				(rtldm->ofdm_index[RF90_PATH_A] >
+				pwr_tracking_limit) ?
+				pwr_tracking_limit :
+				rtldm->ofdm_index[RF90_PATH_A];
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_A]=%d,pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_A]=%d\n",
+				 rtldm->ofdm_index[RF90_PATH_A],
+				 final_swing_idx[RF90_PATH_A]);
+
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+				txscaling_tbl[final_swing_idx[RF90_PATH_A]]);
+		}
+	} else if (method == MIX_MODE) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "pDM_Odm->DefaultOfdmIndex=%d,pDM_Odm->Aboslute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
+			 rtldm->default_ofdm_index,
+			 rtldm->absolute_ofdm_swing_idx[rf_path],
+			 rf_path);
+
+		final_ofdm_swing_index =
+			rtldm->default_ofdm_index +
+			rtldm->absolute_ofdm_swing_idx[rf_path];
+		/*BBSwing higher then Limit*/
+		if (rf_path == RF90_PATH_A) {
+			if (final_ofdm_swing_index > pwr_tracking_limit) {
+				rtldm->remnant_cck_idx =
+					final_ofdm_swing_index -
+					pwr_tracking_limit;
+				/* CCK Follow the same compensate value as Path A*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] =
+					final_ofdm_swing_index -
+					pwr_tracking_limit;
+
+				rtl_set_bbreg(hw, RA_TXSCALE,
+					0xFFE00000,
+					txscaling_tbl[pwr_tracking_limit]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw,
+					rtlphy->current_channel,
+					RF90_PATH_A);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					" ******Path_A Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d\n",
+					 pwr_tracking_limit,
+					 rtldm->remnant_ofdm_swing_idx[rf_path]);
+			} else if (final_ofdm_swing_index < 0) {
+				rtldm->remnant_cck_idx = final_ofdm_swing_index;
+				/* CCK Follow the same compensate value as Path A*/
+				rtldm->remnant_ofdm_swing_idx[rf_path] =
+					final_ofdm_swing_index;
+
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+					txscaling_tbl[0]);
+
+				rtldm->modify_txagc_flag_path_a = true;
+
+				/*Set TxAGC Page C{};*/
+				rtl8821ae_phy_set_txpower_level_by_path(hw,
+					rtlphy->current_channel, RF90_PATH_A);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d\n",
+					 rtldm->remnant_ofdm_swing_idx[rf_path]);
+			} else {
+				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+					txscaling_tbl[(u8)final_ofdm_swing_index]);
+
+				RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+					 "******Path_A Compensate with BBSwing ,Final_OFDM_Swing_Index = %d\n",
+					 final_ofdm_swing_index);
+				/*If TxAGC has changed, reset TxAGC again*/
+				if (rtldm->modify_txagc_flag_path_a) {
+					rtldm->remnant_cck_idx = 0;
+					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
+
+					/*Set TxAGC Page C{};*/
+					rtl8821ae_phy_set_txpower_level_by_path(hw,
+						rtlphy->current_channel, RF90_PATH_A);
+
+					rtldm->modify_txagc_flag_path_a = false;
+
+					RT_TRACE(rtlpriv, COMP_POWER_TRACKING,
+						 DBG_LOUD,
+						 "******Path_A pDM_Odm->Modify_TxAGC_Flag= FALSE\n");
+				}
+			}
+		}
+	} else {
+		return;
+	}
+}
+
+void rtl8821ae_dm_txpower_tracking_callback_thermalmeter(
+	struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	u8 thermal_value = 0, delta, delta_lck, delta_iqk, p = 0, i = 0;
+	u8 thermal_value_avg_count = 0;
+	u32 thermal_value_avg = 0;
+
+	u8 ofdm_min_index = 6;  /*OFDM BB Swing should be less than +3.0dB */
+	/* GetRightChnlPlaceforIQK(pHalData->CurrentChannel)*/
+	u8 index_for_channel = 0;
+
+	/* 1. The following TWO tables decide the final
+	 * index of OFDM/CCK swing table.
+	 */
+	u8 *delta_swing_table_idx_tup_a;
+	u8 *delta_swing_table_idx_tdown_a;
+	u8 *delta_swing_table_idx_tup_b;
+	u8 *delta_swing_table_idx_tdown_b;
+
+	/*2. Initilization ( 7 steps in total )*/
+	rtl8821ae_get_delta_swing_table(hw, (u8 **)&delta_swing_table_idx_tup_a,
+					(u8 **)&delta_swing_table_idx_tdown_a,
+					(u8 **)&delta_swing_table_idx_tup_b,
+					(u8 **)&delta_swing_table_idx_tdown_b);
+
+	rtldm->txpower_trackinginit = true;
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "===>rtl8812ae_dm_txpower_tracking_callback_thermalmeter,\n pDM_Odm->BbSwingIdxCckBase: %d,pDM_Odm->BbSwingIdxOfdmBase[A]:%d, pDM_Odm->DefaultOfdmIndex: %d\n",
+		 rtldm->swing_idx_cck_base,
+		 rtldm->swing_idx_ofdm_base[RF90_PATH_A],
+		 rtldm->default_ofdm_index);
+	/*0x42: RF Reg[15:10] 88E*/
+	thermal_value = (u8)rtl_get_rfreg(hw,
+		RF90_PATH_A, RF_T_METER_8812A, 0xfc00);
+	if (!rtldm->txpower_track_control ||
+		rtlefuse->eeprom_thermalmeter == 0 ||
+		rtlefuse->eeprom_thermalmeter == 0xFF)
+		return;
+
+	/* 3. Initialize ThermalValues of RFCalibrateInfo*/
+
+	if (rtlhal->reloadtxpowerindex) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "reload ofdm index for band switch\n");
+	}
+
+	/*4. Calculate average thermal meter*/
+	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermal_value;
+	rtldm->thermalvalue_avg_index++;
+	if (rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_8812A)
+		/*Average times =  c.AverageThermalNum*/
+		rtldm->thermalvalue_avg_index = 0;
+
+	for (i = 0; i < AVG_THERMAL_NUM_8812A; i++) {
+		if (rtldm->thermalvalue_avg[i]) {
+			thermal_value_avg += rtldm->thermalvalue_avg[i];
+			thermal_value_avg_count++;
+		}
+	}
+	/*Calculate Average ThermalValue after average enough times*/
+	if (thermal_value_avg_count) {
+		thermal_value = (u8)(thermal_value_avg /
+				thermal_value_avg_count);
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
+			 thermal_value, rtlefuse->eeprom_thermalmeter);
+	}
+
+	/*5. Calculate delta, delta_LCK, delta_IQK.
+	 *"delta" here is used to determine whether
+	 * thermal value changes or not.
+	 */
+	delta = (thermal_value > rtldm->thermalvalue) ?
+		(thermal_value - rtldm->thermalvalue) :
+		(rtldm->thermalvalue - thermal_value);
+	delta_lck = (thermal_value > rtldm->thermalvalue_lck) ?
+		(thermal_value - rtldm->thermalvalue_lck) :
+		(rtldm->thermalvalue_lck - thermal_value);
+	delta_iqk = (thermal_value > rtldm->thermalvalue_iqk) ?
+		(thermal_value - rtldm->thermalvalue_iqk) :
+		(rtldm->thermalvalue_iqk - thermal_value);
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
+		 delta, delta_lck, delta_iqk);
+
+	/* 6. If necessary, do LCK.	*/
+	/*Delta temperature is equal to or larger than 20 centigrade.*/
+	if (delta_lck >= IQK_THRESHOLD) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "delta_LCK(%d) >= Threshold_IQK(%d)\n",
+			 delta_lck, IQK_THRESHOLD);
+		rtldm->thermalvalue_lck = thermal_value;
+		rtl8821ae_phy_lc_calibrate(hw);
+	}
+
+	/*7. If necessary, move the index of swing table to adjust Tx power.*/
+
+	if (delta > 0 && rtldm->txpower_track_control) {
+		/*"delta" here is used to record the
+		 * absolute value of differrence.
+		 */
+		delta = thermal_value > rtlefuse->eeprom_thermalmeter ?
+			(thermal_value - rtlefuse->eeprom_thermalmeter) :
+			(rtlefuse->eeprom_thermalmeter - thermal_value);
+
+		if (delta >= TXSCALE_TABLE_SIZE)
+			delta = TXSCALE_TABLE_SIZE - 1;
+
+		/*7.1 The Final Power Index = BaseIndex + PowerIndexOffset*/
+
+		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "delta_swing_table_idx_tup_a[%d] = %d\n",
+				 delta, delta_swing_table_idx_tup_a[delta]);
+			rtldm->delta_power_index_last[RF90_PATH_A] =
+				rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] =
+				delta_swing_table_idx_tup_a[delta];
+
+			rtldm->absolute_ofdm_swing_idx[RF90_PATH_A] =
+				delta_swing_table_idx_tup_a[delta];
+			/*Record delta swing for mix mode power tracking*/
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+				 rtldm->absolute_ofdm_swing_idx[RF90_PATH_A]);
+		} else {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "delta_swing_table_idx_tdown_a[%d] = %d\n",
+				 delta, delta_swing_table_idx_tdown_a[delta]);
+
+			rtldm->delta_power_index_last[RF90_PATH_A] =
+				rtldm->delta_power_index[RF90_PATH_A];
+			rtldm->delta_power_index[RF90_PATH_A] =
+				-1 * delta_swing_table_idx_tdown_a[delta];
+
+			rtldm->absolute_ofdm_swing_idx[RF90_PATH_A] =
+				-1 * delta_swing_table_idx_tdown_a[delta];
+			/* Record delta swing for mix mode power tracking*/
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
+				 rtldm->absolute_ofdm_swing_idx[RF90_PATH_A]);
+		}
+
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "\n\n================================ [Path-%c]Calculating PowerIndexOffset ================================\n",
+				 (p == RF90_PATH_A ? 'A' : 'B'));
+			/*If Thermal value changes but lookup table value
+			 * still the same
+			 */
+			if (rtldm->delta_power_index[p] ==
+				rtldm->delta_power_index_last[p])
+
+				rtldm->power_index_offset[p] = 0;
+			else
+				rtldm->power_index_offset[p] =
+					rtldm->delta_power_index[p] -
+					rtldm->delta_power_index_last[p];
+			/*Power Index Diff between 2 times Power Tracking*/
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "[Path-%c] PowerIndexOffset(%d) = DeltaPowerIndex(%d) - DeltaPowerIndexLast(%d)\n",
+				 (p == RF90_PATH_A ? 'A' : 'B'),
+				rtldm->power_index_offset[p],
+				rtldm->delta_power_index[p] ,
+				rtldm->delta_power_index_last[p]);
+
+			rtldm->ofdm_index[p] =
+					rtldm->swing_idx_ofdm_base[p] +
+					rtldm->power_index_offset[p];
+			rtldm->cck_index =
+					rtldm->swing_idx_cck_base +
+					rtldm->power_index_offset[p];
+
+			rtldm->swing_idx_cck = rtldm->cck_index;
+			rtldm->swing_idx_ofdm[p] = rtldm->ofdm_index[p];
+
+			/*********Print BB Swing Base and Index Offset********/
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
+				 rtldm->swing_idx_cck,
+				 rtldm->swing_idx_cck_base,
+				 rtldm->power_index_offset[p]);
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
+				 rtldm->swing_idx_ofdm[p],
+				 (p == RF90_PATH_A ? 'A' : 'B'),
+				 rtldm->swing_idx_ofdm_base[p],
+				 rtldm->power_index_offset[p]);
+
+			/*7.1 Handle boundary conditions of index.*/
+
+			if (rtldm->ofdm_index[p] > TXSCALE_TABLE_SIZE - 1)
+				rtldm->ofdm_index[p] = TXSCALE_TABLE_SIZE - 1;
+			else if (rtldm->ofdm_index[p] < ofdm_min_index)
+				rtldm->ofdm_index[p] = ofdm_min_index;
+		}
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "\n\n========================================================================================================\n");
+		if (rtldm->cck_index > TXSCALE_TABLE_SIZE - 1)
+			rtldm->cck_index = TXSCALE_TABLE_SIZE - 1;
+		else if (rtldm->cck_index < 0)
+			rtldm->cck_index = 0;
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "The thermal meter is unchanged or TxPowerTracking OFF(%d):ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
+			 rtldm->txpower_track_control,
+			 thermal_value,
+			 rtldm->thermalvalue);
+
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+			rtldm->power_index_offset[p] = 0;
+	}
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "TxPowerTracking: [CCK] Swing Current Index: %d, Swing Base Index: %d\n",
+		 /*Print Swing base & current*/
+		rtldm->cck_index, rtldm->swing_idx_cck_base);
+	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++) {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "TxPowerTracking: [OFDM] Swing Current Index: %d, Swing Base Index[%c]: %d\n",
+			 rtldm->ofdm_index[p],
+			 (p == RF90_PATH_A ? 'A' : 'B'),
+			 rtldm->swing_idx_ofdm_base[p]);
+	}
+
+	if ((rtldm->power_index_offset[RF90_PATH_A] != 0 ||
+		rtldm->power_index_offset[RF90_PATH_B] != 0) &&
+		rtldm->txpower_track_control) {
+		/*7.2 Configure the Swing Table to adjust Tx Power.*/
+		/*Always TRUE after Tx Power is adjusted by power tracking.*/
+		/*
+		 *  2012/04/23 MH According to Luke's suggestion,
+		 *  we can not write BB digital
+		 *  to increase TX power. Otherwise, EVM will be bad.
+		 *
+		 *  2012/04/25 MH Add for tx power tracking to
+		 *  set tx power in tx agc for 88E.
+		 */
+		if (thermal_value > rtldm->thermalvalue) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature Increasing(A): delta_pi: %d , delta_t: %d,Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				 rtldm->power_index_offset[RF90_PATH_A],
+				 delta, thermal_value,
+				 rtlefuse->eeprom_thermalmeter,
+				 rtldm->thermalvalue);
+		} else if (thermal_value < rtldm->thermalvalue) { /*Low temperature*/
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
+				 rtldm->power_index_offset[RF90_PATH_A],
+				 delta, thermal_value,
+				 rtlefuse->eeprom_thermalmeter,
+				 rtldm->thermalvalue);
+		}
+
+		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature(%d) higher than PG value(%d)\n",
+				 thermal_value, rtlefuse->eeprom_thermalmeter);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "****Enter POWER Tracking MIX_MODE****\n");
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+					rtl8821ae_dm_txpwr_track_set_pwr(hw,
+						MIX_MODE, p, index_for_channel);
+		} else {
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "Temperature(%d) lower than PG value(%d)\n",
+				 thermal_value, rtlefuse->eeprom_thermalmeter);
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "*****Enter POWER Tracking MIX_MODE*****\n");
+			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+				rtl8812ae_dm_txpwr_track_set_pwr(hw,
+					MIX_MODE, p, index_for_channel);
+		}
+		/*Record last time Power Tracking result as base.*/
+		rtldm->swing_idx_cck_base = rtldm->swing_idx_cck;
+		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
+			rtldm->swing_idx_ofdm_base[p] = rtldm->swing_idx_ofdm[p];
+
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+				 "pDM_Odm->RFCalibrateInfo.ThermalValue = %d ThermalValue= %d\n",
+				 rtldm->thermalvalue, thermal_value);
+		/*Record last Power Tracking Thermal Value*/
+		rtldm->thermalvalue = thermal_value;
+	}
+	/* Delta temperature is equal to or larger than
+	 * 20 centigrade (When threshold is 8).
+	 */
+	if (delta_iqk >= IQK_THRESHOLD) {
+		if (!rtlphy->lck_inprogress) {
+			spin_lock(&rtlpriv->locks.iqk_lock);
+			rtlphy->lck_inprogress = true;
+			spin_unlock(&rtlpriv->locks.iqk_lock);
+
+			rtl8821ae_do_iqk(hw, delta_iqk, thermal_value, 8);
+
+			spin_lock(&rtlpriv->locks.iqk_lock);
+			rtlphy->lck_inprogress = false;
+			spin_unlock(&rtlpriv->locks.iqk_lock);
+		}
+	}
+
+	RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+		 "<===rtl8812ae_dm_txpower_tracking_callback_thermalmeter\n");
+}
+
+void rtl8821ae_dm_check_txpower_tracking_thermalmeter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	static u8 tm_trigger;
+
+	if (!tm_trigger) {
+		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E, BIT(17)|BIT(16),
+			      0x03);
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Trigger 8821ae Thermal Meter!!\n");
+		tm_trigger = 1;
+		return;
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			 "Schedule TxPowerTracking !!\n");
+
+		rtl8821ae_dm_txpower_tracking_callback_thermalmeter(hw);
+		tm_trigger = 0;
+	}
+}
+
+static void rtl8821ae_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rate_adaptive *p_ra = &rtlpriv->ra;
+	u32 low_rssithresh_for_ra = p_ra->low2high_rssi_thresh_for_ra40m;
+	u32 high_rssithresh_for_ra = p_ra->high_rssi_thresh_for_ra;
+	u8 go_up_gap = 5;
+	struct ieee80211_sta *sta = NULL;
+
+	if (is_hal_stop(rtlhal)) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver is going to unload\n");
+		return;
+	}
+
+	if (!rtlpriv->dm.useramask) {
+		RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+			 "driver does not control rate adaptive mask\n");
+		return;
+	}
+
+	if (mac->link_state == MAC80211_LINKED &&
+		mac->opmode == NL80211_IFTYPE_STATION) {
+		switch (p_ra->pre_ratr_state) {
+		case DM_RATR_STA_MIDDLE:
+			high_rssithresh_for_ra += go_up_gap;
+			break;
+		case DM_RATR_STA_LOW:
+			high_rssithresh_for_ra += go_up_gap;
+			low_rssithresh_for_ra += go_up_gap;
+			break;
+		default:
+			break;
+		}
+
+		if (rtlpriv->dm.undec_sm_pwdb >
+		    (long)high_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_HIGH;
+		else if (rtlpriv->dm.undec_sm_pwdb >
+			 (long)low_rssithresh_for_ra)
+			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
+		else
+			p_ra->ratr_state = DM_RATR_STA_LOW;
+
+		if (p_ra->pre_ratr_state != p_ra->ratr_state) {
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI = %ld\n",
+				  rtlpriv->dm.undec_sm_pwdb);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "RSSI_LEVEL = %d\n", p_ra->ratr_state);
+			RT_TRACE(rtlpriv, COMP_RATE, DBG_LOUD,
+				 "PreState = %d, CurState = %d\n",
+				  p_ra->pre_ratr_state, p_ra->ratr_state);
+
+			rcu_read_lock();
+			sta = rtl_find_sta(hw, mac->bssid);
+			if (sta)
+				rtlpriv->cfg->ops->update_rate_tbl(hw,
+						sta, p_ra->ratr_state);
+			rcu_read_unlock();
+
+			p_ra->pre_ratr_state = p_ra->ratr_state;
+		}
+	}
+}
+
+static void rtl8821ae_dm_refresh_basic_rate_mask(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+	struct rtl_mac *mac = &rtlpriv->mac80211;
+	static u8 stage;
+	u8 cur_stage = 0;
+	u16 basic_rate = RRSR_1M | RRSR_2M | RRSR_5_5M | RRSR_11M | RRSR_6M;
+
+	if (mac->link_state < MAC80211_LINKED)
+		cur_stage = 0;
+	else if (dm_digtable->rssi_val_min < 25)
+		cur_stage = 1;
+	else if (dm_digtable->rssi_val_min > 30)
+		cur_stage = 3;
+	else
+		cur_stage = 2;
+
+	if (cur_stage != stage) {
+		if (cur_stage == 1) {
+			basic_rate &= (!(basic_rate ^ mac->basic_rates));
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_BASIC_RATE, (u8 *)&basic_rate);
+		} else if (cur_stage == 3 && (stage == 1 || stage == 2)) {
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_BASIC_RATE, (u8 *)&mac->basic_rates);
+		}
+	}
+	stage = cur_stage;
+}
+
+static void rtl8821ae_dm_edca_choose_traffic_idx(
+	struct ieee80211_hw *hw, u64 cur_tx_bytes,
+	u64 cur_rx_bytes, bool b_bias_on_rx,
+	bool *pb_is_cur_rdl_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (b_bias_on_rx) {
+		if (cur_tx_bytes > (cur_rx_bytes*4)) {
+			*pb_is_cur_rdl_state = false;
+			RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+				 "Uplink Traffic\n ");
+		} else {
+			*pb_is_cur_rdl_state = true;
+			RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+				 "Balance Traffic\n");
+		}
+	} else {
+		if (cur_rx_bytes > (cur_tx_bytes*4)) {
+			*pb_is_cur_rdl_state = true;
+			RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+				 "Downlink	Traffic\n");
+		} else {
+			*pb_is_cur_rdl_state = false;
+			RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+				 "Balance Traffic\n");
+		}
+	}
+	return;
+}
+
+static void rtl8821ae_dm_check_edca_turbo(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_dm *rtldm =  rtl_dm(rtl_priv(hw));
+
+	/*Keep past Tx/Rx packet count for RT-to-RT EDCA turbo.*/
+	u64 cur_tx_ok_cnt = 0;
+	u64 cur_rx_ok_cnt = 0;
+	u32 edca_be_ul = 0x5ea42b;
+	u32 edca_be_dl = 0x5ea42b;
+	u32 edca_be = 0x5ea42b;
+	u8 iot_peer = 0;
+	bool *pb_is_cur_rdl_state = NULL;
+	bool b_last_is_cur_rdl_state = false;
+	bool b_bias_on_rx = false;
+	bool b_edca_turbo_on = false;
+
+	RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+		 "rtl8821ae_dm_check_edca_turbo=====>");
+	RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+		 "Orginial BE PARAM: 0x%x\n",
+		 rtl_read_dword(rtlpriv, DM_REG_EDCA_BE_11N));
+
+	if (rtlpriv->dm.dbginfo.num_non_be_pkt > 0x100)
+		rtlpriv->dm.is_any_nonbepkts = true;
+	rtlpriv->dm.dbginfo.num_non_be_pkt = 0;
+
+	/*===============================
+	 * list paramter for different platform
+	 *===============================
+	 */
+	b_last_is_cur_rdl_state = rtlpriv->dm.is_cur_rdlstate;
+	pb_is_cur_rdl_state = &rtlpriv->dm.is_cur_rdlstate;
+
+	cur_tx_ok_cnt = rtlpriv->stats.txbytesunicast - rtldm->last_tx_ok_cnt;
+	cur_rx_ok_cnt = rtlpriv->stats.rxbytesunicast - rtldm->last_rx_ok_cnt;
+
+	rtldm->last_tx_ok_cnt = rtlpriv->stats.txbytesunicast;
+	rtldm->last_rx_ok_cnt = rtlpriv->stats.rxbytesunicast;
+
+	iot_peer = rtlpriv->mac80211.vendor;
+	b_bias_on_rx = false;
+	b_edca_turbo_on = ((!rtlpriv->dm.is_any_nonbepkts) &&
+			   (!rtlpriv->dm.disable_framebursting)) ?
+			   true : false;
+
+	if (rtlpriv->rtlhal.hw_type != HARDWARE_TYPE_RTL8812AE) {
+		if ((iot_peer == PEER_CISCO) &&
+			(mac->mode == WIRELESS_MODE_N_24G)) {
+			edca_be_dl = edca_setting_dl[iot_peer];
+			edca_be_ul = edca_setting_ul[iot_peer];
+		}
+	}
+
+	RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+		 "bIsAnyNonBEPkts : 0x%x  bDisableFrameBursting : 0x%x\n",
+		 rtlpriv->dm.is_any_nonbepkts,
+		 rtlpriv->dm.disable_framebursting);
+
+	RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+		 "bEdcaTurboOn : 0x%x bBiasOnRx : 0x%x\n",
+		 b_edca_turbo_on, b_bias_on_rx);
+
+	if (b_edca_turbo_on) {
+		RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+			 "curTxOkCnt : 0x%llx\n", cur_tx_ok_cnt);
+		RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+			 "curRxOkCnt : 0x%llx\n", cur_rx_ok_cnt);
+		if (b_bias_on_rx)
+			rtl8821ae_dm_edca_choose_traffic_idx(hw, cur_tx_ok_cnt,
+				cur_rx_ok_cnt, true, pb_is_cur_rdl_state);
+		else
+			rtl8821ae_dm_edca_choose_traffic_idx(hw, cur_tx_ok_cnt,
+				cur_rx_ok_cnt, false, pb_is_cur_rdl_state);
+
+		edca_be = (*pb_is_cur_rdl_state) ?  edca_be_dl : edca_be_ul;
+
+		rtl_write_dword(rtlpriv, DM_REG_EDCA_BE_11N, edca_be);
+
+		RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+			 "EDCA Turbo on: EDCA_BE:0x%x\n", edca_be);
+
+		rtlpriv->dm.current_turbo_edca = true;
+
+		RT_TRACE(rtlpriv, COMP_TURBO, DBG_LOUD,
+			 "EDCA_BE_DL : 0x%x  EDCA_BE_UL : 0x%x  EDCA_BE : 0x%x\n",
+			 edca_be_dl, edca_be_ul, edca_be);
+	} else {
+		if (rtlpriv->dm.current_turbo_edca) {
+			u8 tmp = AC0_BE;
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
+						      (u8 *)(&tmp));
+		}
+		rtlpriv->dm.current_turbo_edca = false;
+	}
+
+	rtlpriv->dm.is_any_nonbepkts = false;
+	rtldm->last_tx_ok_cnt = rtlpriv->stats.txbytesunicast;
+	rtldm->last_rx_ok_cnt = rtlpriv->stats.rxbytesunicast;
+}
+
+static void rtl8821ae_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+	u8 cur_cck_cca_thresh;
+
+	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
+		if (dm_digtable->rssi_val_min > 25) {
+			cur_cck_cca_thresh = 0xcd;
+		} else if ((dm_digtable->rssi_val_min <= 25) &&
+			   (dm_digtable->rssi_val_min > 10)) {
+			cur_cck_cca_thresh = 0x83;
+		} else {
+			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+				cur_cck_cca_thresh = 0x83;
+			else
+				cur_cck_cca_thresh = 0x40;
+		}
+	} else {
+		if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
+			cur_cck_cca_thresh = 0x83;
+		else
+			cur_cck_cca_thresh = 0x40;
+	}
+
+	if (dm_digtable->cur_cck_cca_thres != cur_cck_cca_thresh)
+		rtl_write_byte(rtlpriv, ODM_REG_CCK_CCA_11AC,
+			       cur_cck_cca_thresh);
+
+	dm_digtable->pre_cck_cca_thres = dm_digtable->cur_cck_cca_thres;
+	dm_digtable->cur_cck_cca_thres = cur_cck_cca_thresh;
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_TRACE,
+		 "CCK cca thresh hold =%x\n", dm_digtable->cur_cck_cca_thres);
+}
+
+static void rtl8821ae_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	u8 crystal_cap;
+	u32 packet_count;
+	int cfo_khz_a, cfo_khz_b, cfo_ave = 0, adjust_xtal = 0;
+	int cfo_ave_diff;
+
+	if (rtlpriv->mac80211.link_state < MAC80211_LINKED) {
+		/*1.Enable ATC*/
+		if (rtldm->atc_status == ATC_STATUS_OFF) {
+			rtl_set_bbreg(hw, RFC_AREA, BIT(14), ATC_STATUS_ON);
+			rtldm->atc_status = ATC_STATUS_ON;
+		}
+
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "No link!!\n");
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "atc_status = %d\n", rtldm->atc_status);
+
+		if (rtldm->crystal_cap != rtlpriv->efuse.crystalcap) {
+			rtldm->crystal_cap = rtlpriv->efuse.crystalcap;
+			crystal_cap = rtldm->crystal_cap & 0x3f;
+			crystal_cap = crystal_cap & 0x3f;
+			if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE)
+				rtl_set_bbreg(hw, REG_MAC_PHY_CTRL,
+					      0x7ff80000, (crystal_cap |
+					      (crystal_cap << 6)));
+			else
+				rtl_set_bbreg(hw, REG_MAC_PHY_CTRL,
+					      0xfff000, (crystal_cap |
+					      (crystal_cap << 6)));
+		}
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD, "crystal_cap = 0x%x\n",
+			 rtldm->crystal_cap);
+	} else{
+		/*1. Calculate CFO for path-A & path-B*/
+		cfo_khz_a = (int)(rtldm->cfo_tail[0] * 3125) / 1280;
+		cfo_khz_b = (int)(rtldm->cfo_tail[1] * 3125) / 1280;
+		packet_count = rtldm->packet_count;
+
+		/*2.No new packet*/
+		if (packet_count == rtldm->packet_count_pre) {
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "packet counter doesn't change\n");
+			return;
+		}
+
+		rtldm->packet_count_pre = packet_count;
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "packet counter = %d\n",
+			 rtldm->packet_count);
+
+		/*3.Average CFO*/
+		if (rtlpriv->phy.rf_type == RF_1T1R)
+			cfo_ave = cfo_khz_a;
+		else
+			cfo_ave = (cfo_khz_a + cfo_khz_b) >> 1;
+
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "cfo_khz_a = %dkHz, cfo_khz_b = %dkHz, cfo_ave = %dkHz\n",
+			 cfo_khz_a, cfo_khz_b, cfo_ave);
+
+		/*4.Avoid abnormal large CFO*/
+		cfo_ave_diff = (rtldm->cfo_ave_pre >= cfo_ave) ?
+						(rtldm->cfo_ave_pre - cfo_ave) :
+						(cfo_ave - rtldm->cfo_ave_pre);
+
+		if (cfo_ave_diff > 20 && rtldm->large_cfo_hit == 0) {
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "first large CFO hit\n");
+			rtldm->large_cfo_hit = 1;
+			return;
+		} else
+			rtldm->large_cfo_hit = 0;
+
+		rtldm->cfo_ave_pre = cfo_ave;
+
+		/*CFO tracking by adjusting Xtal cap.*/
+
+		/*1.Dynamic Xtal threshold*/
+		if (cfo_ave >= -rtldm->cfo_threshold &&
+			cfo_ave <= rtldm->cfo_threshold &&
+			rtldm->is_freeze == 0) {
+			if (rtldm->cfo_threshold == CFO_THRESHOLD_XTAL) {
+				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL + 10;
+				rtldm->is_freeze = 1;
+			} else {
+				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL;
+			}
+		}
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "Dynamic threshold = %d\n",
+			 rtldm->cfo_threshold);
+
+		/* 2.Calculate Xtal offset*/
+		if (cfo_ave > rtldm->cfo_threshold && rtldm->crystal_cap < 0x3f)
+			adjust_xtal = ((cfo_ave - CFO_THRESHOLD_XTAL) >> 2) + 1;
+		else if ((cfo_ave < -rtlpriv->dm.cfo_threshold) &&
+			 rtlpriv->dm.crystal_cap > 0)
+			adjust_xtal = ((cfo_ave + CFO_THRESHOLD_XTAL) >> 2) - 1;
+		RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+			 "Crystal cap = 0x%x, Crystal cap offset = %d\n",
+			 rtldm->crystal_cap, adjust_xtal);
+
+		/*3.Adjudt Crystal Cap.*/
+		if (adjust_xtal != 0) {
+			rtldm->is_freeze = 0;
+			rtldm->crystal_cap += adjust_xtal;
+
+			if (rtldm->crystal_cap > 0x3f)
+				rtldm->crystal_cap = 0x3f;
+			else if (rtldm->crystal_cap < 0)
+				rtldm->crystal_cap = 0;
+
+			crystal_cap = rtldm->crystal_cap & 0x3f;
+			crystal_cap = crystal_cap & 0x3f;
+			if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE)
+				rtl_set_bbreg(hw, REG_MAC_PHY_CTRL,
+					      0x7ff80000, (crystal_cap |
+					      (crystal_cap << 6)));
+			else
+				rtl_set_bbreg(hw, REG_MAC_PHY_CTRL,
+					      0xfff000, (crystal_cap |
+					      (crystal_cap << 6)));
+			RT_TRACE(rtlpriv, COMP_DIG, DBG_LOUD,
+				 "New crystal cap = 0x%x\n",
+				 rtldm->crystal_cap);
+		}
+	}
+}
+
+void rtl8821ae_dm_watchdog(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool fw_current_inpsmode = false;
+	bool fw_ps_awake = true;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				      (u8 *)(&fw_current_inpsmode));
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
+				      (u8 *)(&fw_ps_awake));
+
+	if (ppsc->p2p_ps_info.p2p_ps_mode)
+		fw_ps_awake = false;
+
+	if ((ppsc->rfpwr_state == ERFON) &&
+	    ((!fw_current_inpsmode) && fw_ps_awake) &&
+	    (!ppsc->rfchange_inprogress)) {
+		rtl8821ae_dm_common_info_self_update(hw);
+		rtl8821ae_dm_false_alarm_counter_statistics(hw);
+		rtl8821ae_dm_check_rssi_monitor(hw);
+		rtl8821ae_dm_dig(hw);
+		rtl8821ae_dm_cck_packet_detection_thresh(hw);
+		rtl8821ae_dm_refresh_rate_adaptive_mask(hw);
+		rtl8821ae_dm_refresh_basic_rate_mask(hw);
+		rtl8821ae_dm_check_edca_turbo(hw);
+		rtl8821ae_dm_dynamic_atc_switch(hw);
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_dm_check_txpower_tracking_thermalmeter(hw);
+		else
+			rtl8821ae_dm_check_txpower_tracking_thermalmeter(hw);
+		rtl8821ae_dm_iq_calibrate(hw);
+	}
+
+	rtlpriv->dm.dbginfo.num_qry_beacon_pkt = 0;
+	RT_TRACE(rtlpriv, COMP_DIG, DBG_DMESG, "\n");
+}
+
+void rtl8821ae_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
+					u8 *pdesc, u32 mac_id)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct fast_ant_training *pfat_table = &rtldm->fat_table;
+
+	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8812AE)
+		return;
+
+	if (rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)
+		SET_TX_DESC_TX_ANT(pdesc, pfat_table->antsel_a[mac_id]);
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/dm.h b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.h
index ebbff9b6cacf..9dd40dd316c1 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/dm.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/dm.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -53,8 +49,6 @@
 #define	DM_REG_T_METER_92D_11N			0x42
 #define	DM_REG_T_METER_88E_11N			0x42
 
-
-
 /*BB REG LIST*/
 /*PAGE 8 */
 #define	DM_REG_BB_CTRL_11N				0x800
@@ -163,7 +157,6 @@
 #define	DM_REG_SLEEP_11N				0xEE0
 #define	DM_REG_PMPD_ANAEN_11N			0xEEC
 
-
 /*MAC REG LIST*/
 #define	DM_REG_BB_RST_11N				0x02
 #define	DM_REG_ANTSEL_PIN_11N			0x4C
@@ -178,20 +171,17 @@
 #define	DM_REG_ANT_TRAIN_PARA1_11N	0x7b0
 #define	DM_REG_ANT_TRAIN_PARA2_11N	0x7b4
 
-
 /*DIG Related*/
 #define	DM_BIT_IGI_11N					0x0000007F
 #define	DM_BIT_IGI_11AC					0xFFFFFFFF
 
-
-
 #define HAL_DM_DIG_DISABLE			BIT(0)
 #define HAL_DM_HIPWR_DISABLE		BIT(1)
 
-#define OFDM_TABLE_LENGTH 			43
-#define CCK_TABLE_LENGTH 			33
+#define OFDM_TABLE_LENGTH			43
+#define CCK_TABLE_LENGTH			33
 
-#define OFDM_TABLE_SIZE 			37
+#define OFDM_TABLE_SIZE				37
 #define CCK_TABLE_SIZE				33
 
 #define BW_AUTO_SWITCH_HIGH_LOW		25
@@ -211,7 +201,7 @@
 
 #define DM_DIG_FA_UPPER				0x3e
 #define DM_DIG_FA_LOWER				0x1e
-#define DM_DIG_FA_TH0				0x200
+#define DM_DIG_FA_TH0				200
 #define DM_DIG_FA_TH1				0x300
 #define DM_DIG_FA_TH2				0x400
 
@@ -219,7 +209,7 @@
 #define DM_DIG_BACKOFF_MIN			-4
 #define DM_DIG_BACKOFF_DEFAULT		10
 
-#define RXPATHSELECTION_SS_TH_lOW	30
+#define RXPATHSELECTION_SS_TH_LOW	30
 #define RXPATHSELECTION_DIFF_TH		18
 
 #define DM_RATR_STA_INIT			0
@@ -230,7 +220,7 @@
 #define CTS2SELF_THVAL				30
 #define REGC38_TH					20
 
-#define WAIOTTHVal					25
+#define WAIOTTHVAL					25
 
 #define TXHIGHPWRLEVEL_NORMAL		0
 #define TXHIGHPWRLEVEL_LEVEL1		1
@@ -246,72 +236,16 @@
 #define TXPWRTRACK_MAX_IDX 6
 
 /* Dynamic ATC switch */
-#define ATC_STATUS_OFF				0x0			/* enable */
-#define	ATC_STATUS_ON				0x1			/* disable */
-#define	CFO_THRESHOLD_XTAL			10			/* kHz */
-#define	CFO_THRESHOLD_ATC			80			/* kHz */
+#define ATC_STATUS_OFF				0x0	/* enable */
+#define	ATC_STATUS_ON				0x1	/* disable */
+#define	CFO_THRESHOLD_XTAL			10	/* kHz */
+#define	CFO_THRESHOLD_ATC			80	/* kHz */
 
 #define AVG_THERMAL_NUM_8812A	4
-#define TXPWR_TRACK_TABLE_SIZE 	30
+#define TXPWR_TRACK_TABLE_SIZE	30
 #define MAX_PATH_NUM_8812A		2
 #define MAX_PATH_NUM_8821A		1
 
-
-struct ps_t {
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-	u8 pre_rfstate;
-	u8 cur_rfstate;
-	u8 initialize;
-	long rssi_val_min;
-
-};
-
-struct dig_t {
-	u8 dig_enable_flag;
-	u8 dig_ext_port_stage;
-	u32 rssi_lowthresh;
-	u32 rssi_highthresh;
-
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-
-	u8 cursta_connectctate;
-	u8 presta_connectstate;
-	u8 curmultista_connectstate;
-
-	u8 pre_igvalue;
-	u8 cur_igvalue;
-	u8 bt30_cur_igi;
-	u8 backup_igvalue;
-	u8 stop_dig;
-
-	char backoff_val;
-	char backoff_val_range_max;
-	char backoff_val_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-	u8 rssi_val_min;
-
-	u8 pre_cck_cca_thres;
-	u8 cur_cck_cca_thres;
-	u8 pre_cck_pd_state;
-	u8 cur_cck_pd_state;
-
-	u8 large_fa_hit;
-	u8 forbidden_igi;
-	u32 recover_cnt;
-
-	u8 dig_dynamic_min_0;
-	u8 dig_dynamic_min_1;
-	bool b_media_connect_0;
-	bool b_media_connect_1;
-
-	u32 antdiv_rssi_max;
-	u32 rssi_max;
-};
-
-
 enum FAT_STATE {
 	FAT_NORMAL_STATE	= 0,
 	FAT_TRAINING_STATE = 1,
@@ -329,10 +263,10 @@ enum tag_dynamic_init_gain_operation_type_definition {
 };
 
 enum tag_cck_packet_detection_threshold_type_definition {
-	CCK_PD_STAGE_LowRssi = 0,
-	CCK_PD_STAGE_HighRssi = 1,
-	CCK_FA_STAGE_Low = 2,
-	CCK_FA_STAGE_High = 3,
+	CCK_PD_STAGE_LOWRSSI = 0,
+	CCK_PD_STAGE_HIGHRSSI = 1,
+	CCK_FA_STAGE_LOW = 2,
+	CCK_FA_STAGE_HIGH = 3,
 	CCK_PD_STAGE_MAX = 4,
 };
 
@@ -383,17 +317,17 @@ enum pwr_track_control_method {
 #define BT_RSSI_STATE_BG_EDCA_LOW       BIT_OFFSET_LEN_MASK_32(3, 1)
 #define BT_RSSI_STATE_TXPOWER_LOW       BIT_OFFSET_LEN_MASK_32(4, 1)
 #define GET_UNDECORATED_AVERAGE_RSSI(_priv)     \
-        (((struct rtl_priv *)(_priv))->mac80211.opmode == NL80211_IFTYPE_ADHOC)?  \
-        (((struct rtl_priv *)(_priv))->dm.entry_min_undecoratedsmoothed_pwdb):  \
-        (((struct rtl_priv *)(_priv))->dm.undecorated_smoothed_pwdb)
+	((((struct rtl_priv *)(_priv))->mac80211.opmode ==	\
+			      NL80211_IFTYPE_ADHOC) ? \
+	(((struct rtl_priv *)(_priv))->dm.entry_min_undec_sm_pwdb) : \
+	(((struct rtl_priv *)(_priv))->dm.undec_sm_pwdb))
 
-extern struct dig_t dm_digtable;
 void rtl8821ae_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
-												   u8 *pdesc, u32 mac_id);
+					u8 *pdesc, u32 mac_id);
 void rtl8821ae_dm_ant_sel_statistics(struct ieee80211_hw *hw,
-											  u8 antsel_tr_mux, u32 mac_id,
-											  u32 rx_pwdb_all);
-void rtl8821ae_dm_fast_antenna_trainning_callback(unsigned long data);
+				     u8 antsel_tr_mux, u32 mac_id,
+				     u32 rx_pwdb_all);
+void rtl8821ae_dm_fast_antenna_training_callback(unsigned long data);
 void rtl8821ae_dm_init(struct ieee80211_hw *hw);
 void rtl8821ae_dm_watchdog(struct ieee80211_hw *hw);
 void rtl8821ae_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi);
@@ -401,26 +335,22 @@ void rtl8821ae_dm_init_edca_turbo(struct ieee80211_hw *hw);
 void rtl8821ae_dm_check_txpower_tracking_thermalmeter(struct ieee80211_hw *hw);
 void rtl8821ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw);
 void rtl8821ae_dm_txpower_track_adjust(struct ieee80211_hw *hw,
-												   u8 type,u8 *pdirection,
-												   u32 *poutwrite_val);
+				       u8 type, u8 *pdirection,
+				       u32 *poutwrite_val);
 void rtl8821ae_dm_clear_txpower_tracking_state(struct ieee80211_hw *hw);
 void rtl8821ae_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 current_cca);
 void rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(struct ieee80211_hw *hw);
-void rtl8812ae_dm_path_diversity(struct ieee80211_hw *hw);
-void rtl8812ae_dm_path_diversity_init(struct ieee80211_hw *hw);
-void rtl8812ae_dm_path_statistics(struct ieee80211_hw *hw,
-	u32 rssi_a, u32 rssi_b);
-void rtl812ae_dm_set_txpath_by_txinfo(struct ieee80211_hw *hw,
-	u8 *pdesc);
 void rtl8812ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
-												enum pwr_track_control_method method,
-												u8 rf_path,
-												u8 channel_mapped_index);
+				      enum pwr_track_control_method method,
+				      u8 rf_path,
+				      u8 channel_mapped_index);
 void rtl8821ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
-	enum pwr_track_control_method method, u8 rf_path, u8 channel_mapped_index);
+				      enum pwr_track_control_method method,
+				      u8 rf_path, u8 channel_mapped_index);
 
-void rtl8812ae_dm_update_init_rate(struct ieee80211_hw *hw, u8 rate);
-u8 rtl8812ae_hw_rate_to_mrate(struct ieee80211_hw *hw, u8 rate);
+void rtl8821ae_dm_update_init_rate(struct ieee80211_hw *hw, u8 rate);
+u8 rtl8821ae_hw_rate_to_mrate(struct ieee80211_hw *hw, u8 rate);
 void rtl8812ae_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw *hw);
 void rtl8821ae_dm_txpower_tracking_callback_thermalmeter(struct ieee80211_hw *hw);
+
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/fw.c b/drivers/net/wireless/rtlwifi/rtl8821ae/fw.c
new file mode 100644
index 000000000000..95e95626b632
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/fw.c
@@ -0,0 +1,1857 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "../core.h"
+#include "reg.h"
+#include "def.h"
+#include "fw.h"
+#include "dm.h"
+
+static void _rtl8821ae_enable_fw_download(struct ieee80211_hw *hw, bool enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	if (enable) {
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x05);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL + 2);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL + 2, tmp & 0xf7);
+
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+	} else {
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
+		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
+	}
+}
+
+static void _rtl8821ae_fw_block_write(struct ieee80211_hw *hw,
+				      const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 blocksize = sizeof(u32);
+	u8 *bufferptr = (u8 *)buffer;
+	u32 *pu4byteptr = (u32 *)buffer;
+	u32 i, offset, blockcount, remainsize;
+
+	blockcount = size / blocksize;
+	remainsize = size % blocksize;
+
+	for (i = 0; i < blockcount; i++) {
+		offset = i * blocksize;
+		rtl_write_dword(rtlpriv, (FW_8821AE_START_ADDRESS + offset),
+				*(pu4byteptr + i));
+	}
+
+	if (remainsize) {
+		offset = blockcount * blocksize;
+		bufferptr += offset;
+		for (i = 0; i < remainsize; i++) {
+			rtl_write_byte(rtlpriv, (FW_8821AE_START_ADDRESS +
+					offset + i), *(bufferptr + i));
+		}
+	}
+}
+
+static void _rtl8821ae_fw_page_write(struct ieee80211_hw *hw,
+				     u32 page, const u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value8;
+	u8 u8page = (u8)(page & 0x07);
+
+	value8 = (rtl_read_byte(rtlpriv, REG_MCUFWDL + 2) & 0xF8) | u8page;
+
+	rtl_write_byte(rtlpriv, (REG_MCUFWDL + 2), value8);
+	_rtl8821ae_fw_block_write(hw, buffer, size);
+}
+
+static void _rtl8821ae_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
+{
+	u32 fwlen = *pfwlen;
+	u8 remain = (u8)(fwlen % 4);
+
+	remain = (remain == 0) ? 0 : (4 - remain);
+
+	while (remain > 0) {
+		pfwbuf[fwlen] = 0;
+		fwlen++;
+		remain--;
+	}
+
+	*pfwlen = fwlen;
+}
+
+static void _rtl8821ae_write_fw(struct ieee80211_hw *hw,
+				enum version_8821ae version,
+				u8 *buffer, u32 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 *bufferptr = (u8 *)buffer;
+	u32 pagenums, remainsize;
+	u32 page, offset;
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "FW size is %d bytes,\n", size);
+
+	_rtl8821ae_fill_dummy(bufferptr, &size);
+
+	pagenums = size / FW_8821AE_PAGE_SIZE;
+	remainsize = size % FW_8821AE_PAGE_SIZE;
+
+	if (pagenums > 8) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Page numbers should not greater then 8\n");
+	}
+
+	for (page = 0; page < pagenums; page++) {
+		offset = page * FW_8821AE_PAGE_SIZE;
+		_rtl8821ae_fw_page_write(hw, page, (bufferptr + offset),
+					 FW_8821AE_PAGE_SIZE);
+	}
+
+	if (remainsize) {
+		offset = pagenums * FW_8821AE_PAGE_SIZE;
+		page = pagenums;
+		_rtl8821ae_fw_page_write(hw, page, (bufferptr + offset),
+					 remainsize);
+	}
+}
+
+static int _rtl8821ae_fw_free_to_go(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	int err = -EIO;
+	u32 counter = 0;
+	u32 value32;
+
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	} while ((counter++ < FW_8821AE_POLLING_TIMEOUT_COUNT) &&
+		 (!(value32 & FWDL_CHKSUM_RPT)));
+
+	if (counter >= FW_8821AE_POLLING_TIMEOUT_COUNT) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "chksum report faill ! REG_MCUFWDL:0x%08x .\n",
+			  value32);
+		goto exit;
+	}
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_EMERG,
+		 "Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32);
+
+	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
+
+	rtl8821ae_firmware_selfreset(hw);
+
+	counter = 0;
+	do {
+		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY) {
+			RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+				 "Polling FW ready success!! REG_MCUFWDL:0x%08x .\n",
+				  value32);
+			err = 0;
+			goto exit;
+		}
+
+		udelay(FW_8821AE_POLLING_DELAY);
+	} while (counter++ < FW_8821AE_POLLING_TIMEOUT_COUNT);
+
+	RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+		 "Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n",
+		 value32);
+
+exit:
+	return err;
+}
+
+static void _rtl8821ae_wait_for_h2c_cmd_finish(struct rtl_priv *rtlpriv)
+{
+	u8 val;
+	u16 count = 0;
+
+	do {
+		val = rtl_read_byte(rtlpriv, REG_HMETFR);
+		mdelay(1);
+		count++;
+	} while ((val & 0x0F) && (count < 1000));
+}
+
+int rtl8821ae_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl8821a_firmware_header *pfwheader;
+	u8 *pfwdata;
+	u32 fwsize;
+	int err;
+	bool support_remote_wakeup;
+	enum version_8821ae version = rtlhal->version;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&support_remote_wakeup));
+
+	if (support_remote_wakeup)
+		_rtl8821ae_wait_for_h2c_cmd_finish(rtlpriv);
+
+	if (buse_wake_on_wlan_fw) {
+		if (!rtlhal->wowlan_firmware)
+			return 1;
+
+		pfwheader =
+		  (struct rtl8821a_firmware_header *)rtlhal->wowlan_firmware;
+		rtlhal->fw_version = pfwheader->version;
+		rtlhal->fw_subversion = pfwheader->subversion;
+		pfwdata = (u8 *)rtlhal->wowlan_firmware;
+		fwsize = rtlhal->wowlan_fwsize;
+	} else {
+		if (!rtlhal->pfirmware)
+			return 1;
+
+		pfwheader =
+		  (struct rtl8821a_firmware_header *)rtlhal->pfirmware;
+		rtlhal->fw_version = pfwheader->version;
+		rtlhal->fw_subversion = pfwheader->subversion;
+		pfwdata = (u8 *)rtlhal->pfirmware;
+		fwsize = rtlhal->fwsize;
+	}
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+		 "%s Firmware SIZE %d\n",
+		 buse_wake_on_wlan_fw ? "Wowlan" : "Normal", fwsize);
+
+	if (IS_FW_HEADER_EXIST_8812(pfwheader) ||
+	    IS_FW_HEADER_EXIST_8821(pfwheader)) {
+		RT_TRACE(rtlpriv, COMP_FW, DBG_DMESG,
+			 "Firmware Version(%d), Signature(%#x)\n",
+			 pfwheader->version, pfwheader->signature);
+
+		pfwdata = pfwdata + sizeof(struct rtl8821a_firmware_header);
+		fwsize = fwsize - sizeof(struct rtl8821a_firmware_header);
+	}
+
+	if (rtlhal->mac_func_enable) {
+		if (rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) {
+			rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+			rtl8821ae_firmware_selfreset(hw);
+		}
+	}
+	_rtl8821ae_enable_fw_download(hw, true);
+	_rtl8821ae_write_fw(hw, version, pfwdata, fwsize);
+	_rtl8821ae_enable_fw_download(hw, false);
+
+	err = _rtl8821ae_fw_free_to_go(hw);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Firmware is not ready to run!\n");
+	} else {
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+			 "Firmware is ready to run!\n");
+	}
+
+	return 0;
+}
+
+#if (USE_SPECIFIC_FW_TO_SUPPORT_WOWLAN == 1)
+void rtl8821ae_set_fw_related_for_wowlan(struct ieee80211_hw *hw,
+					 bool used_wowlan_fw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	/* 1. Before WoWLAN or After WOWLAN we need to re-download Fw. */
+	if (rtl8821ae_download_fw(hw, used_wowlan_fw)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+			 "Re-Download Firmware failed!!\n");
+		rtlhal->fw_ready = false;
+		return;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "Re-Download Firmware Success !!\n");
+	rtlhal->fw_ready = true;
+
+	/* 2. Re-Init the variables about Fw related setting. */
+	ppsc->fw_current_inpsmode = false;
+	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_8821AE;
+	rtlhal->fw_clk_change_in_progress = false;
+	rtlhal->allow_sw_to_change_hwclc = false;
+	rtlhal->last_hmeboxnum = 0;
+}
+#endif
+
+static bool _rtl8821ae_check_fw_read_last_h2c(struct ieee80211_hw *hw,
+					      u8 boxnum)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 val_hmetfr;
+	bool result = false;
+
+	val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
+	if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
+		result = true;
+	return result;
+}
+
+static void _rtl8821ae_fill_h2c_command(struct ieee80211_hw *hw,
+					u8 element_id, u32 cmd_len,
+					u8 *cmdbuffer)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 boxnum = 0;
+	u16 box_reg = 0, box_extreg = 0;
+	u8 u1b_tmp = 0;
+	bool isfw_read = false;
+	u8 buf_index = 0;
+	bool bwrite_sucess = false;
+	u8 wait_h2c_limmit = 100;
+	/*u8 wait_writeh2c_limmit = 100;*/
+	u8 boxcontent[4], boxextcontent[4];
+	u32 h2c_waitcounter = 0;
+	unsigned long flag = 0;
+	u8 idx = 0;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "come in\n");
+
+	while (true) {
+		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+		if (rtlhal->h2c_setinprogress) {
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "H2C set in progress! Wait to set..element_id(%d).\n",
+				 element_id);
+
+			while (rtlhal->h2c_setinprogress) {
+				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
+						       flag);
+				h2c_waitcounter++;
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+					 "Wait 100 us (%d times)...\n",
+					  h2c_waitcounter);
+				udelay(100);
+
+				if (h2c_waitcounter > 1000)
+					return;
+				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
+						  flag);
+			}
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+		} else {
+			rtlhal->h2c_setinprogress = true;
+			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+			break;
+		}
+	}
+
+	while (!bwrite_sucess) {
+		boxnum = rtlhal->last_hmeboxnum;
+		switch (boxnum) {
+		case 0:
+			box_reg = REG_HMEBOX_0;
+			box_extreg = REG_HMEBOX_EXT_0;
+			break;
+		case 1:
+			box_reg = REG_HMEBOX_1;
+			box_extreg = REG_HMEBOX_EXT_1;
+			break;
+		case 2:
+			box_reg = REG_HMEBOX_2;
+			box_extreg = REG_HMEBOX_EXT_2;
+			break;
+		case 3:
+			box_reg = REG_HMEBOX_3;
+			box_extreg = REG_HMEBOX_EXT_3;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
+			break;
+		}
+
+		isfw_read = false;
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_CR);
+
+		if (u1b_tmp != 0xEA) {
+			isfw_read = true;
+		} else {
+			if (rtl_read_byte(rtlpriv, REG_TXDMA_STATUS) == 0xEA ||
+			    rtl_read_byte(rtlpriv, REG_TXPKT_EMPTY) == 0xEA)
+				rtl_write_byte(rtlpriv, REG_SYS_CFG1 + 3, 0xFF);
+		}
+
+		if (isfw_read) {
+			wait_h2c_limmit = 100;
+			isfw_read =
+			  _rtl8821ae_check_fw_read_last_h2c(hw, boxnum);
+			while (!isfw_read) {
+				/*wait until Fw read*/
+				wait_h2c_limmit--;
+				if (wait_h2c_limmit == 0) {
+					RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+						 "Waiting too long for FW read clear HMEBox(%d)!\n",
+						 boxnum);
+					break;
+				}
+
+				udelay(10);
+
+				isfw_read =
+				  _rtl8821ae_check_fw_read_last_h2c(hw, boxnum);
+				u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
+				RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+					 "Waiting for FW read clear HMEBox(%d)!!! 0x130 = %2x\n",
+					 boxnum, u1b_tmp);
+			}
+		}
+
+		if (!isfw_read) {
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+				 "Write H2C register BOX[%d] fail!!!!! Fw do not read.\n",
+				 boxnum);
+			break;
+		}
+
+		memset(boxcontent, 0, sizeof(boxcontent));
+		memset(boxextcontent, 0, sizeof(boxextcontent));
+		boxcontent[0] = element_id;
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+			 "Write element_id box_reg(%4x) = %2x\n",
+			 box_reg, element_id);
+
+		switch (cmd_len) {
+		case 1:
+		case 2:
+		case 3:
+			/*boxcontent[0] &= ~(BIT(7));*/
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, cmd_len);
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxcontent[idx]);
+			}
+			break;
+		case 4:
+		case 5:
+		case 6:
+		case 7:
+			/*boxcontent[0] |= (BIT(7));*/
+			memcpy((u8 *)(boxextcontent),
+			       cmdbuffer + buf_index+3, cmd_len-3);
+			memcpy((u8 *)(boxcontent) + 1,
+			       cmdbuffer + buf_index, 3);
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_extreg + idx,
+					       boxextcontent[idx]);
+			}
+
+			for (idx = 0; idx < 4; idx++) {
+				rtl_write_byte(rtlpriv, box_reg + idx,
+					       boxcontent[idx]);
+			}
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
+			break;
+		}
+
+		bwrite_sucess = true;
+
+		rtlhal->last_hmeboxnum = boxnum + 1;
+		if (rtlhal->last_hmeboxnum == 4)
+			rtlhal->last_hmeboxnum = 0;
+
+		RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD,
+			 "pHalData->last_hmeboxnum  = %d\n",
+			  rtlhal->last_hmeboxnum);
+	}
+
+	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
+	rtlhal->h2c_setinprogress = false;
+	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_LOUD, "go out\n");
+}
+
+void rtl8821ae_fill_h2c_cmd(struct ieee80211_hw *hw,
+			    u8 element_id, u32 cmd_len, u8 *cmdbuffer)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 tmp_cmdbuf[2];
+
+	if (!rtlhal->fw_ready) {
+		RT_ASSERT(false,
+			  "return H2C cmd because of Fw download fail!!!\n");
+		return;
+	}
+
+	memset(tmp_cmdbuf, 0, 8);
+	memcpy(tmp_cmdbuf, cmdbuffer, cmd_len);
+	_rtl8821ae_fill_h2c_command(hw, element_id, cmd_len, (u8 *)&tmp_cmdbuf);
+}
+
+void rtl8821ae_firmware_selfreset(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 u1b_tmp;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(3))));
+	} else {
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(0))));
+	}
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
+	udelay(50);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp | BIT(3)));
+	} else {
+		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp | BIT(0)));
+	}
+
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp | BIT(2)));
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "_8051Reset8812ae(): 8051 reset success .\n");
+}
+
+void rtl8821ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 u1_h2c_set_pwrmode[H2C_8821AE_PWEMODE_LENGTH] = { 0 };
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 rlbm, power_state = 0;
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "FW LPS mode = %d\n", mode);
+
+	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
+	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
+	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
+	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode,
+					 (rtlpriv->mac80211.p2p) ?
+					 ppsc->smart_ps : 1);
+	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode,
+					       ppsc->reg_max_lps_awakeintvl);
+	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
+	if (mode == FW_PS_ACTIVE_MODE)
+		power_state |= FW_PWR_STATE_ACTIVE;
+	else
+		power_state |= FW_PWR_STATE_RF_OFF;
+
+	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode\n",
+		      u1_h2c_set_pwrmode, H2C_8821AE_PWEMODE_LENGTH);
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_SETPWRMODE,
+			       H2C_8821AE_PWEMODE_LENGTH,
+			       u1_h2c_set_pwrmode);
+}
+
+void rtl8821ae_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw,
+					   u8 mstatus)
+{
+	u8 parm[3] = { 0, 0, 0 };
+	/* parm[0]: bit0=0-->Disconnect, bit0=1-->Connect
+	 *          bit1=0-->update Media Status to MACID
+	 *          bit1=1-->update Media Status from MACID to MACID_End
+	 * parm[1]: MACID, if this is INFRA_STA, MacID = 0
+	 * parm[2]: MACID_End
+	 */
+
+	SET_H2CCMD_MSRRPT_PARM_OPMODE(parm, mstatus);
+	SET_H2CCMD_MSRRPT_PARM_MACID_IND(parm, 0);
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_MSRRPT, 3, parm);
+}
+
+void rtl8821ae_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
+				      u8 ap_offload_enable)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 u1_apoffload_parm[H2C_8821AE_AP_OFFLOAD_LENGTH] = { 0 };
+
+	SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
+	SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->hiddenssid);
+	SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_AP_OFFLOAD,
+			       H2C_8821AE_AP_OFFLOAD_LENGTH,
+			       u1_apoffload_parm);
+}
+
+void rtl8821ae_set_fw_wowlan_mode(struct ieee80211_hw *hw, bool func_en)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	u8 fw_wowlan_info[H2C_8821AE_WOWLAN_LENGTH] = {0};
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "enable(%d)\n", func_en);
+
+	SET_8812_H2CCMD_WOWLAN_FUNC_ENABLE(fw_wowlan_info,
+					   (func_en ? true : false));
+
+	SET_8812_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(fw_wowlan_info,
+		((ppsc->wo_wlan_mode & WAKE_ON_PATTERN_MATCH) ? 1 : 0));
+	SET_8812_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(fw_wowlan_info,
+		((ppsc->wo_wlan_mode & WAKE_ON_MAGIC_PACKET) ? 1 : 0));
+
+	SET_8812_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(fw_wowlan_info, 0);
+	SET_8812_H2CCMD_WOWLAN_ALL_PKT_DROP(fw_wowlan_info, false);
+	SET_8812_H2CCMD_WOWLAN_GPIO_ACTIVE(fw_wowlan_info, 0);
+	SET_8812_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(fw_wowlan_info, 1);
+	SET_8812_H2CCMD_WOWLAN_GPIONUM(fw_wowlan_info, 0);
+	SET_8812_H2CCMD_WOWLAN_GPIO_DURATION(fw_wowlan_info, 0);
+
+	RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_DMESG,
+		      "wowlan mode: cmd 0x80: Content:\n",
+		      fw_wowlan_info, H2C_8821AE_WOWLAN_LENGTH);
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_WO_WLAN,
+			       H2C_8821AE_WOWLAN_LENGTH,
+			       fw_wowlan_info);
+}
+
+void rtl8821ae_set_fw_remote_wake_ctrl_cmd(struct ieee80211_hw *hw,
+					   u8 enable)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 remote_wake_ctrl_parm[H2C_8821AE_REMOTE_WAKE_CTRL_LEN] = {0};
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+		 "enable=%d, ARP offload=%d, GTK offload=%d\n",
+		 enable, ppsc->arp_offload_enable, ppsc->gtk_offload_enable);
+
+	SET_8812_H2CCMD_REMOTE_WAKECTRL_ENABLE(remote_wake_ctrl_parm, enable);
+	SET_8812_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(remote_wake_ctrl_parm,
+					(ppsc->arp_offload_enable ? 1 : 0));
+	SET_8812_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(remote_wake_ctrl_parm,
+					(ppsc->gtk_offload_enable ? 1 : 0));
+	SET_8812_H2CCMD_REMOTE_WAKE_CTRL_REALWOWV2_EN(remote_wake_ctrl_parm,
+					(rtlhal->real_wow_v2_enable ? 1 : 0));
+
+	RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+		      "remote_wake_ctrl: cmd 0x4: Content:\n",
+		      remote_wake_ctrl_parm, H2C_8821AE_REMOTE_WAKE_CTRL_LEN);
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_REMOTE_WAKE_CTRL,
+			       H2C_8821AE_REMOTE_WAKE_CTRL_LEN,
+			       remote_wake_ctrl_parm);
+}
+
+void rtl8821ae_set_fw_keep_alive_cmd(struct ieee80211_hw *hw,
+				     bool func_en)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 keep_alive_info[H2C_8821AE_KEEP_ALIVE_CTRL_LENGTH] = {0};
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Enable(%d)\n", func_en);
+
+	SET_8812_H2CCMD_KEEP_ALIVE_ENABLE(keep_alive_info, func_en);
+	/* 1: the period is controled by driver, 0: by Fw default */
+	SET_8812_H2CCMD_KEEP_ALIVE_ACCPEPT_USER_DEFINED(keep_alive_info, 1);
+	SET_8812_H2CCMD_KEEP_ALIVE_PERIOD(keep_alive_info, 10); /* 10 sec */
+
+	RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+		      "keep alive: cmd 0x3: Content:\n",
+		      keep_alive_info, H2C_8821AE_KEEP_ALIVE_CTRL);
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_KEEP_ALIVE_CTRL,
+			       H2C_8821AE_KEEP_ALIVE_CTRL_LENGTH,
+			       keep_alive_info);
+}
+
+void rtl8821ae_set_fw_disconnect_decision_ctrl_cmd(struct ieee80211_hw *hw,
+						   bool enabled)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 parm[H2C_8821AE_DISCONNECT_DECISION_CTRL_LEN] = {0};
+
+	SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_ENABLE(parm, enabled);
+	SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_USER_SETTING(parm, 1);
+	SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_CHECK_PERIOD(parm, 30);
+	SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_TRYPKT_NUM(parm, 3);
+
+	RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
+		      "disconnect_decision_ctrl: cmd 0x4: Content:\n",
+		      parm, H2C_8821AE_DISCONNECT_DECISION_CTRL_LEN);
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_DISCONNECT_DECISION,
+			       H2C_8821AE_DISCONNECT_DECISION_CTRL_LEN, parm);
+}
+
+void rtl8821ae_set_fw_global_info_cmd(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_security *sec = &rtlpriv->sec;
+	u8 remote_wakeup_sec_info[H2C_8821AE_AOAC_GLOBAL_INFO_LEN] = {0};
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+		 "PairwiseEncAlgorithm=%d, GroupEncAlgorithm=%d\n",
+		 sec->pairwise_enc_algorithm, sec->group_enc_algorithm);
+
+	SET_8812_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(
+						remote_wakeup_sec_info,
+						sec->pairwise_enc_algorithm);
+	SET_8812_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(remote_wakeup_sec_info,
+						      sec->group_enc_algorithm);
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_AOAC_GLOBAL_INFO,
+			       H2C_8821AE_AOAC_GLOBAL_INFO_LEN,
+			       remote_wakeup_sec_info);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_TRACE,
+		      "rtl8821ae_set_global_info: cmd 0x82:\n",
+		      remote_wakeup_sec_info, H2C_8821AE_AOAC_GLOBAL_INFO_LEN);
+}
+
+#define BEACON_PG		0
+#define PSPOLL_PG		1
+#define NULL_PG			2
+#define QOSNULL_PG		3
+#define ARPRESP_PG		4
+#define REMOTE_PG		5
+#define GTKEXT_PG		6
+
+#define TOTAL_RESERVED_PKT_LEN_8812	3584
+#define TOTAL_RESERVED_PKT_LEN_8821	1792
+
+static u8 reserved_page_packet_8821[TOTAL_RESERVED_PKT_LEN_8821] = {
+	/* page 0: beacon */
+	0x80, 0x00, 0x00, 0x00,  0xff, 0xff, 0xff, 0xff,
+	0xff, 0xff, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
+	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x20, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x64, 0x00, 0x20, 0x04,  0x00, 0x06, 0x64, 0x6c,
+	0x69, 0x6e, 0x6b, 0x31,  0x01, 0x08, 0x82, 0x84,
+	0x8b, 0x96, 0x0c, 0x18,  0x30, 0x48, 0x03, 0x01,
+	0x0b, 0x06, 0x02, 0x00,  0x00, 0x2a, 0x01, 0x8b,
+	0x32, 0x04, 0x12, 0x24,  0x60, 0x6c, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x10, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x81, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 1: ps-poll */
+	0xa4, 0x10, 0x01, 0xc0,  0x40, 0x16, 0x9f, 0x23,
+	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x18, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 2: null data */
+	0x48, 0x01, 0x00, 0x00,  0x40, 0x16, 0x9f, 0x23,
+	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
+	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x1A, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 3: qos null data */
+	0xC8, 0x01, 0x00, 0x00,  0x84, 0xC9, 0xB2, 0xA7,
+	0xB3, 0x6E, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x84, 0xC9, 0xB2, 0xA7,  0xB3, 0x6E, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x3C, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 4~6 is for wowlan */
+	/* page 4: ARP resp */
+	0x08, 0x01, 0x00, 0x00,  0x84, 0xC9, 0xB2, 0xA7,
+	0xB3, 0x6E, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x84, 0xC9, 0xB2, 0xA7,  0xB3, 0x6E, 0x00, 0x00,
+	0xAA, 0xAA, 0x03, 0x00,  0x00, 0x00, 0x08, 0x06,
+	0x00, 0x01, 0x08, 0x00,  0x06, 0x04, 0x00, 0x02,
+	0x00, 0xE0, 0x4C, 0x02,  0x51, 0x02, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 5: H2C_REMOTE_WAKE_CTRL_INFO */
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 6: Rsvd GTK extend memory (zero memory) */
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+};
+
+static u8 reserved_page_packet_8812[TOTAL_RESERVED_PKT_LEN_8812] = {
+	/* page 0: beacon */
+	0x80, 0x00, 0x00, 0x00,  0xFF, 0xFF, 0xFF, 0xFF,
+	0xFF, 0xFF, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x84, 0xC9, 0xB2, 0xA7,  0xB3, 0x6E, 0x60, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x64, 0x00, 0x20, 0x04,  0x00, 0x03, 0x32, 0x31,
+	0x35, 0x01, 0x08, 0x82,  0x84, 0x8B, 0x96, 0x0C,
+	0x12, 0x18, 0x24, 0x03,  0x01, 0x01, 0x06, 0x02,
+	0x00, 0x00, 0x2A, 0x01,  0x02, 0x32, 0x04, 0x30,
+	0x48, 0x60, 0x6C, 0x2D,  0x1A, 0xED, 0x09, 0x03,
+	0xFF, 0xFF, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x3D,
+	0x00, 0xDD, 0x07, 0x00,  0xE0, 0x4C, 0x02, 0x02,
+	0x08, 0x04, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x10, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x81, 0x00, 0x00,
+	0x04, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 1: ps-poll */
+	0xA4, 0x10, 0x09, 0xC0,  0x84, 0xC9, 0xB2, 0xA7,
+	0xB3, 0x6E, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x18, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 2: null data */
+	0x48, 0x01, 0x00, 0x00,  0x84, 0xC9, 0xB2, 0xA7,
+	0xB3, 0x6E, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x84, 0xC9, 0xB2, 0xA7,  0xB3, 0x6E, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x1A, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 3: Qos null data */
+	0xC8, 0x01, 0x00, 0x00,  0x84, 0xC9, 0xB2, 0xA7,
+	0xB3, 0x6E, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x84, 0xC9, 0xB2, 0xA7,  0xB3, 0x6E, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x3C, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 4~6 is for wowlan */
+	/* page 4: ARP resp */
+	0x08, 0x01, 0x00, 0x00,  0x84, 0xC9, 0xB2, 0xA7,
+	0xB3, 0x6E, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x84, 0xC9, 0xB2, 0xA7,  0xB3, 0x6E, 0x00, 0x00,
+	0xAA, 0xAA, 0x03, 0x00,  0x00, 0x00, 0x08, 0x06,
+	0x00, 0x01, 0x08, 0x00,  0x06, 0x04, 0x00, 0x02,
+	0x00, 0xE0, 0x4C, 0x02,  0x51, 0x02, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0xE0,  0x4C, 0x02, 0x51, 0x02,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 5: H2C_REMOTE_WAKE_CTRL_INFO */
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	/* page 6: Rsvd GTK extend memory (zero memory) */
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
+};
+
+void rtl8812ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
+				  bool b_dl_finished, bool dl_whole_packets)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	struct sk_buff *skb = NULL;
+	u32 totalpacketlen;
+	bool rtstatus;
+	u8 u1RsvdPageLoc[5] = { 0 };
+	u8 u1RsvdPageLoc2[7] = { 0 };
+	bool b_dlok = false;
+	u8 *beacon;
+	u8 *p_pspoll;
+	u8 *nullfunc;
+	u8 *qosnull;
+	u8 *arpresp;
+
+	/*---------------------------------------------------------
+	 *			(1) beacon
+	 *---------------------------------------------------------
+	 */
+	beacon = &reserved_page_packet_8812[BEACON_PG * 512];
+	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
+
+	if (b_dl_finished) {
+		totalpacketlen = 512 - 40;
+		goto out;
+	}
+	/*-------------------------------------------------------
+	 *			(2) ps-poll
+	 *--------------------------------------------------------
+	 */
+	p_pspoll = &reserved_page_packet_8812[PSPOLL_PG * 512];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+
+	/*--------------------------------------------------------
+	 *			(3) null data
+	 *---------------------------------------------------------
+	 */
+	nullfunc = &reserved_page_packet_8812[NULL_PG * 512];
+	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
+	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+
+	/*---------------------------------------------------------
+	 *			(4) Qos null data
+	 *----------------------------------------------------------
+	 */
+	qosnull = &reserved_page_packet_8812[QOSNULL_PG * 512];
+	SET_80211_HDR_ADDRESS1(qosnull, mac->bssid);
+	SET_80211_HDR_ADDRESS2(qosnull, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(qosnull, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(u1RsvdPageLoc, QOSNULL_PG);
+
+	if (!dl_whole_packets) {
+		totalpacketlen = 512 * (QOSNULL_PG + 1) - 40;
+		goto out;
+	}
+	/*---------------------------------------------------------
+	 *			(5) ARP Resp
+	 *----------------------------------------------------------
+	 */
+	arpresp = &reserved_page_packet_8812[ARPRESP_PG * 512];
+	SET_80211_HDR_ADDRESS1(arpresp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(arpresp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(arpresp, mac->bssid);
+
+	SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(u1RsvdPageLoc2, ARPRESP_PG);
+
+	/*---------------------------------------------------------
+	 *			(6) Remote Wake Ctrl
+	 *----------------------------------------------------------
+	 */
+	SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(u1RsvdPageLoc2,
+								REMOTE_PG);
+
+	/*---------------------------------------------------------
+	 *			(7) GTK Ext Memory
+	 *----------------------------------------------------------
+	 */
+	SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_EXT_MEM(u1RsvdPageLoc2, GTKEXT_PG);
+
+	totalpacketlen = TOTAL_RESERVED_PKT_LEN_8812 - 40;
+
+out:
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "rtl8812ae_set_fw_rsvdpagepkt(): packet data\n",
+		      &reserved_page_packet_8812[0], totalpacketlen);
+
+	skb = dev_alloc_skb(totalpacketlen);
+	memcpy((u8 *)skb_put(skb, totalpacketlen),
+	       &reserved_page_packet_8812, totalpacketlen);
+
+	rtstatus = rtl_cmd_send_packet(hw, skb);
+
+	if (rtstatus)
+		b_dlok = true;
+
+	if (!b_dl_finished && b_dlok) {
+		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+			      "H2C_RSVDPAGE:\n", u1RsvdPageLoc, 5);
+		rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RSVDPAGE,
+				       sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+		if (dl_whole_packets) {
+			RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+				      "wowlan H2C_RSVDPAGE:\n", u1RsvdPageLoc2, 7);
+			rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_AOAC_RSVDPAGE,
+					       sizeof(u1RsvdPageLoc2), u1RsvdPageLoc2);
+		}
+	}
+
+	if (!b_dlok)
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
+}
+
+void rtl8821ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
+				  bool b_dl_finished, bool dl_whole_packets)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct sk_buff *skb = NULL;
+	u32 totalpacketlen;
+	bool rtstatus;
+	u8 u1RsvdPageLoc[5] = { 0 };
+	u8 u1RsvdPageLoc2[7] = { 0 };
+	bool b_dlok = false;
+	u8 *beacon;
+	u8 *p_pspoll;
+	u8 *nullfunc;
+	u8 *qosnull;
+	u8 *arpresp;
+
+	/*---------------------------------------------------------
+	 *			(1) beacon
+	 *---------------------------------------------------------
+	 */
+	beacon = &reserved_page_packet_8821[BEACON_PG * 256];
+	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
+
+	if (b_dl_finished) {
+		totalpacketlen = 256 - 40;
+		goto out;
+	}
+	/*-------------------------------------------------------
+	 *			(2) ps-poll
+	 *--------------------------------------------------------
+	 */
+	p_pspoll = &reserved_page_packet_8821[PSPOLL_PG * 256];
+	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
+	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
+	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
+
+	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG);
+
+	/*--------------------------------------------------------
+	 *			(3) null data
+	 *---------------------------------------------------------i
+	 */
+	nullfunc = &reserved_page_packet_8821[NULL_PG * 256];
+	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
+	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG);
+
+	/*---------------------------------------------------------
+	 *			(4) Qos null data
+	 *----------------------------------------------------------
+	 */
+	qosnull = &reserved_page_packet_8821[QOSNULL_PG * 256];
+	SET_80211_HDR_ADDRESS1(qosnull, mac->bssid);
+	SET_80211_HDR_ADDRESS2(qosnull, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(qosnull, mac->bssid);
+
+	SET_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(u1RsvdPageLoc, QOSNULL_PG);
+
+	if (!dl_whole_packets) {
+		totalpacketlen = 256 * (QOSNULL_PG + 1) - 40;
+		goto out;
+	}
+	/*---------------------------------------------------------
+	 *			(5) ARP Resp
+	 *----------------------------------------------------------
+	 */
+	arpresp = &reserved_page_packet_8821[ARPRESP_PG * 256];
+	SET_80211_HDR_ADDRESS1(arpresp, mac->bssid);
+	SET_80211_HDR_ADDRESS2(arpresp, mac->mac_addr);
+	SET_80211_HDR_ADDRESS3(arpresp, mac->bssid);
+
+	SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(u1RsvdPageLoc2, ARPRESP_PG);
+
+	/*---------------------------------------------------------
+	 *			(6) Remote Wake Ctrl
+	 *----------------------------------------------------------
+	 */
+	SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(u1RsvdPageLoc2,
+									REMOTE_PG);
+
+	/*---------------------------------------------------------
+	 *			(7) GTK Ext Memory
+	 *----------------------------------------------------------
+	 */
+	SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_EXT_MEM(u1RsvdPageLoc2, GTKEXT_PG);
+
+	totalpacketlen = TOTAL_RESERVED_PKT_LEN_8821 - 40;
+
+out:
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "rtl8821ae_set_fw_rsvdpagepkt(): packet data\n",
+		      &reserved_page_packet_8821[0], totalpacketlen);
+
+	skb = dev_alloc_skb(totalpacketlen);
+	memcpy((u8 *)skb_put(skb, totalpacketlen),
+	       &reserved_page_packet_8821, totalpacketlen);
+
+	rtstatus = rtl_cmd_send_packet(hw, skb);
+
+	if (rtstatus)
+		b_dlok = true;
+
+	if (!b_dl_finished && b_dlok) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "Set RSVD page location to Fw.\n");
+		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+				"H2C_RSVDPAGE:\n", u1RsvdPageLoc, 5);
+		rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RSVDPAGE,
+				       sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
+		if (dl_whole_packets) {
+			RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
+				      "wowlan H2C_RSVDPAGE:\n",
+				      u1RsvdPageLoc2, 7);
+			rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_AOAC_RSVDPAGE,
+					       sizeof(u1RsvdPageLoc2),
+					       u1RsvdPageLoc2);
+		}
+	}
+
+	if (!b_dlok) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set RSVD page location to Fw FAIL!!!!!!.\n");
+	}
+}
+
+/*Should check FW support p2p or not.*/
+static void rtl8821ae_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
+{
+	u8 u1_ctwindow_period[1] = { ctwindow};
+
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_P2P_PS_CTW_CMD, 1,
+			       u1_ctwindow_period);
+}
+
+void rtl8821ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_p2p_ps_info *p2pinfo = &rtlps->p2p_ps_info;
+	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
+	u8	i;
+	u16	ctwindow;
+	u32	start_time, tsf_low;
+
+	switch (p2p_ps_state) {
+	case P2P_PS_DISABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_DISABLE\n");
+		memset(p2p_ps_offload, 0, sizeof(*p2p_ps_offload));
+		break;
+	case P2P_PS_ENABLE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_ENABLE\n");
+		/* update CTWindow value. */
+		if (p2pinfo->ctwindow > 0) {
+			p2p_ps_offload->ctwindow_en = 1;
+			ctwindow = p2pinfo->ctwindow;
+			rtl8821ae_set_p2p_ctw_period_cmd(hw, ctwindow);
+		}
+
+		/* hw only support 2 set of NoA */
+		for (i = 0 ; i < p2pinfo->noa_num ; i++) {
+			/* To control the register setting for which NOA*/
+			rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
+			if (i == 0)
+				p2p_ps_offload->noa0_en = 1;
+			else
+				p2p_ps_offload->noa1_en = 1;
+
+			/* config P2P NoA Descriptor Register */
+			rtl_write_dword(rtlpriv, 0x5E0, p2pinfo->noa_duration[i]);
+			rtl_write_dword(rtlpriv, 0x5E4, p2pinfo->noa_interval[i]);
+
+			/*Get Current TSF value */
+			tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+			start_time = p2pinfo->noa_start_time[i];
+			if (p2pinfo->noa_count_type[i] != 1) {
+				while (start_time <= (tsf_low+(50*1024))) {
+					start_time += p2pinfo->noa_interval[i];
+					if (p2pinfo->noa_count_type[i] != 255)
+						p2pinfo->noa_count_type[i]--;
+				}
+			}
+			rtl_write_dword(rtlpriv, 0x5E8, start_time);
+			rtl_write_dword(rtlpriv, 0x5EC,
+					p2pinfo->noa_count_type[i]);
+		}
+
+		if ((p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0)) {
+			/* rst p2p circuit */
+			rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
+
+			p2p_ps_offload->offload_en = 1;
+
+			if (P2P_ROLE_GO == rtlpriv->mac80211.p2p) {
+				p2p_ps_offload->role = 1;
+				p2p_ps_offload->allstasleep = 0;
+			} else {
+				p2p_ps_offload->role = 0;
+			}
+
+			p2p_ps_offload->discovery = 0;
+		}
+		break;
+	case P2P_PS_SCAN:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN\n");
+		p2p_ps_offload->discovery = 1;
+		break;
+	case P2P_PS_SCAN_DONE:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "P2P_PS_SCAN_DONE\n");
+		p2p_ps_offload->discovery = 0;
+		p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
+		break;
+	default:
+		break;
+	}
+
+	rtl8821ae_fill_h2c_cmd(hw,
+			H2C_8821AE_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
+}
+
+static void rtl8821ae_c2h_ra_report_handler(struct ieee80211_hw *hw,
+				     u8 *cmd_buf, u8 cmd_len)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 rate = cmd_buf[0] & 0x3F;
+
+	rtlhal->current_ra_rate = rtl8821ae_hw_rate_to_mrate(hw, rate);
+
+	rtl8821ae_dm_update_init_rate(hw, rate);
+}
+
+static void _rtl8821ae_c2h_content_parsing(struct ieee80211_hw *hw,
+					   u8 c2h_cmd_id, u8 c2h_cmd_len,
+					   u8 *tmp_buf)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (c2h_cmd_id) {
+	case C2H_8812_DBG:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD, "[C2H], C2H_8812_DBG!!\n");
+		break;
+	case C2H_8812_RA_RPT:
+		rtl8821ae_c2h_ra_report_handler(hw, tmp_buf, c2h_cmd_len);
+		break;
+	case C2H_8812_BT_INFO:
+		RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+			 "[C2H], C2H_8812_BT_INFO!!\n");
+		if (rtlpriv->cfg->ops->get_btc_status())
+			rtlpriv->btcoexist.btc_ops->btc_btinfo_notify(rtlpriv,
+								      tmp_buf,
+								      c2h_cmd_len);
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl8821ae_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer,
+				  u8 length)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 c2h_cmd_id = 0, c2h_cmd_seq = 0, c2h_cmd_len = 0;
+	u8 *tmp_buf = NULL;
+
+	c2h_cmd_id = buffer[0];
+	c2h_cmd_seq = buffer[1];
+	c2h_cmd_len = length - 2;
+	tmp_buf = buffer + 2;
+
+	RT_TRACE(rtlpriv, COMP_FW, DBG_LOUD,
+		 "[C2H packet], c2hCmdId=0x%x, c2hCmdSeq=0x%x, c2hCmdLen=%d\n",
+		 c2h_cmd_id, c2h_cmd_seq, c2h_cmd_len);
+
+	RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD,
+		      "[C2H packet], Content Hex:\n", tmp_buf, c2h_cmd_len);
+	_rtl8821ae_c2h_content_parsing(hw, c2h_cmd_id, c2h_cmd_len, tmp_buf);
+}
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/fw.h b/drivers/net/wireless/rtlwifi/rtl8821ae/fw.h
new file mode 100644
index 000000000000..591c14c0b9b5
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/fw.h
@@ -0,0 +1,351 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE__FW__H__
+#define __RTL8821AE__FW__H__
+#include "def.h"
+
+#define FW_8821AE_SIZE					0x8000
+#define FW_8821AE_START_ADDRESS			0x1000
+#define FW_8821AE_END_ADDRESS			0x5FFF
+#define FW_8821AE_PAGE_SIZE				4096
+#define FW_8821AE_POLLING_DELAY			5
+#define FW_8821AE_POLLING_TIMEOUT_COUNT	6000
+
+#define IS_FW_HEADER_EXIST_8812(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFF0) == 0x9500)
+
+#define IS_FW_HEADER_EXIST_8821(_pfwhdr)	\
+	((_pfwhdr->signature&0xFFF0) == 0x2100)
+
+#define USE_OLD_WOWLAN_DEBUG_FW 0
+
+#define H2C_8821AE_RSVDPAGE_LOC_LEN		5
+#define H2C_8821AE_PWEMODE_LENGTH			5
+#define H2C_8821AE_JOINBSSRPT_LENGTH		1
+#define H2C_8821AE_AP_OFFLOAD_LENGTH		3
+#define H2C_8821AE_WOWLAN_LENGTH			3
+#define H2C_8821AE_KEEP_ALIVE_CTRL_LENGTH	3
+#if (USE_OLD_WOWLAN_DEBUG_FW == 0)
+#define H2C_8821AE_REMOTE_WAKE_CTRL_LEN	1
+#else
+#define H2C_8821AE_REMOTE_WAKE_CTRL_LEN	3
+#endif
+#define H2C_8821AE_AOAC_GLOBAL_INFO_LEN	2
+#define H2C_8821AE_AOAC_RSVDPAGE_LOC_LEN	7
+#define H2C_8821AE_DISCONNECT_DECISION_CTRL_LEN	3
+
+/* Fw PS state for RPWM.
+*BIT[2:0] = HW state
+
+*BIT[3] = Protocol PS state,
+1: register active state ,
+0: register sleep state
+
+*BIT[4] = sub-state
+*/
+#define	FW_PS_GO_ON			BIT(0)
+#define	FW_PS_TX_NULL			BIT(1)
+#define	FW_PS_RF_ON			BIT(2)
+#define	FW_PS_REGISTER_ACTIVE	BIT(3)
+
+#define	FW_PS_DPS		BIT(0)
+#define	FW_PS_LCLK		(FW_PS_DPS)
+#define	FW_PS_RF_OFF		BIT(1)
+#define	FW_PS_ALL_ON		BIT(2)
+#define	FW_PS_ST_ACTIVE		BIT(3)
+#define	FW_PS_ISR_ENABLE	BIT(4)
+#define	FW_PS_IMR_ENABLE	BIT(5)
+
+#define	FW_PS_ACK		BIT(6)
+#define	FW_PS_TOGGLE		BIT(7)
+
+ /* 8821AE RPWM value*/
+ /* BIT[0] = 1: 32k, 0: 40M*/
+ /* 32k*/
+#define	FW_PS_CLOCK_OFF		BIT(0)
+/*40M*/
+#define	FW_PS_CLOCK_ON		0
+
+#define	FW_PS_STATE_MASK		(0x0F)
+#define	FW_PS_STATE_HW_MASK	(0x07)
+/*ISR_ENABLE, IMR_ENABLE, and PS mode should be inherited.*/
+#define	FW_PS_STATE_INT_MASK	(0x3F)
+
+#define	FW_PS_STATE(x)			(FW_PS_STATE_MASK & (x))
+#define	FW_PS_STATE_HW(x)		(FW_PS_STATE_HW_MASK & (x))
+#define	FW_PS_STATE_INT(x)	(FW_PS_STATE_INT_MASK & (x))
+#define	FW_PS_ISR_VAL(x)		((x) & 0x70)
+#define	FW_PS_IMR_MASK(x)	((x) & 0xDF)
+#define	FW_PS_KEEP_IMR(x)		((x) & 0x20)
+
+#define	FW_PS_STATE_S0		(FW_PS_DPS)
+#define	FW_PS_STATE_S1		(FW_PS_LCLK)
+#define	FW_PS_STATE_S2		(FW_PS_RF_OFF)
+#define	FW_PS_STATE_S3		(FW_PS_ALL_ON)
+#define	FW_PS_STATE_S4		((FW_PS_ST_ACTIVE) | (FW_PS_ALL_ON))
+ /* ((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))*/
+#define	FW_PS_STATE_ALL_ON_8821AE	(FW_PS_CLOCK_ON)
+ /* (FW_PS_RF_ON)*/
+#define	FW_PS_STATE_RF_ON_8821AE	(FW_PS_CLOCK_ON)
+ /* 0x0*/
+#define	FW_PS_STATE_RF_OFF_8821AE	(FW_PS_CLOCK_ON)
+ /* (FW_PS_STATE_RF_OFF)*/
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_8821AE	(FW_PS_CLOCK_OFF)
+
+#define	FW_PS_STATE_ALL_ON_92C	(FW_PS_STATE_S4)
+#define	FW_PS_STATE_RF_ON_92C		(FW_PS_STATE_S3)
+#define	FW_PS_STATE_RF_OFF_92C	(FW_PS_STATE_S2)
+#define	FW_PS_STATE_RF_OFF_LOW_PWR_92C	(FW_PS_STATE_S1)
+
+/* For 8821AE H2C PwrMode Cmd ID 5.*/
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+#define	FW_PS_IS_ACK(x)		((x) & FW_PS_ACK)
+#define	FW_PS_IS_CLK_ON(x)	((x) & (FW_PS_RF_OFF | FW_PS_ALL_ON))
+#define	FW_PS_IS_RF_ON(x)	((x) & (FW_PS_ALL_ON))
+#define	FW_PS_IS_ACTIVE(x)	((x) & (FW_PS_ST_ACTIVE))
+#define	FW_PS_IS_CPWM_INT(x)	((x) & 0x40)
+
+#define	FW_CLR_PS_STATE(x)	((x) = ((x) & (0xF0)))
+
+#define	IS_IN_LOW_POWER_STATE_8821AE(__state)		\
+			(FW_PS_STATE(__state) == FW_PS_CLOCK_OFF)
+
+#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
+#define	FW_PWR_STATE_RF_OFF	0
+
+struct rtl8821a_firmware_header {
+	u16 signature;
+	u8 category;
+	u8 function;
+	u16 version;
+	u8 subversion;
+	u8 rsvd1;
+	u8 month;
+	u8 date;
+	u8 hour;
+	u8 minute;
+	u16 ramcodeSize;
+	u16 rsvd2;
+	u32 svnindex;
+	u32 rsvd3;
+	u32 rsvd4;
+	u32 rsvd5;
+};
+
+enum rtl8812_c2h_evt {
+	C2H_8812_DBG = 0,
+	C2H_8812_LB = 1,
+	C2H_8812_TXBF = 2,
+	C2H_8812_TX_REPORT = 3,
+	C2H_8812_BT_INFO = 9,
+	C2H_8812_BT_MP = 11,
+	C2H_8812_RA_RPT = 12,
+
+	C2H_8812_FW_SWCHNL = 0x10,
+	C2H_8812_IQK_FINISH = 0x11,
+	MAX_8812_C2HEVENT
+};
+
+enum rtl8821a_h2c_cmd {
+	H2C_8821AE_RSVDPAGE = 0,
+	H2C_8821AE_MSRRPT = 1,
+	H2C_8821AE_SCAN = 2,
+	H2C_8821AE_KEEP_ALIVE_CTRL = 3,
+	H2C_8821AE_DISCONNECT_DECISION = 4,
+	H2C_8821AE_INIT_OFFLOAD = 6,
+	H2C_8821AE_AP_OFFLOAD = 8,
+	H2C_8821AE_BCN_RSVDPAGE = 9,
+	H2C_8821AE_PROBERSP_RSVDPAGE = 10,
+
+	H2C_8821AE_SETPWRMODE = 0x20,
+	H2C_8821AE_PS_TUNING_PARA = 0x21,
+	H2C_8821AE_PS_TUNING_PARA2 = 0x22,
+	H2C_8821AE_PS_LPS_PARA = 0x23,
+	H2C_8821AE_P2P_PS_OFFLOAD = 024,
+
+	H2C_8821AE_WO_WLAN = 0x80,
+	H2C_8821AE_REMOTE_WAKE_CTRL = 0x81,
+	H2C_8821AE_AOAC_GLOBAL_INFO = 0x82,
+	H2C_8821AE_AOAC_RSVDPAGE = 0x83,
+
+	H2C_RSSI_21AE_REPORT = 0x42,
+	H2C_8821AE_RA_MASK = 0x40,
+	H2C_8821AE_SELECTIVE_SUSPEND_ROF_CMD,
+	H2C_8821AE_P2P_PS_MODE,
+	H2C_8821AE_PSD_RESULT,
+	/*Not defined CTW CMD for P2P yet*/
+	H2C_8821AE_P2P_PS_CTW_CMD,
+	MAX_8821AE_H2CCMD
+};
+
+#define pagenum_128(_len)	(u32)(((_len)>>7) + ((_len)&0x7F ? 1 : 0))
+
+#define SET_8812_H2CCMD_WOWLAN_FUNC_ENABLE(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 2, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 3, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_ALL_PKT_DROP(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 4, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_GPIO_ACTIVE(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE(__cmd, 5, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_REKEY_WAKE_UP(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 6, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 7, 1, __value)
+#define SET_8812_H2CCMD_WOWLAN_GPIONUM(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd) + 1, 0, 8, __value)
+#define SET_8812_H2CCMD_WOWLAN_GPIO_DURATION(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd) + 2, 0, 8, __value)
+
+#define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_PWRMODE_PARM_RLBM(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 4, __value)
+#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 4, 4, __value)
+#define SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+#define SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__cmd, __value)		\
+	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __value)
+#define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+4, 0, 8, __value)
+#define GET_8821AE_H2CCMD_PWRMODE_PARM_MODE(__cmd)		\
+	LE_BITS_TO_1BYTE(__cmd, 0, 8)
+
+#define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
+#define SET_H2CCMD_RSVDPAGE_LOC_QOS_NULL_DATA(__ph2ccmd, __val)		\
+	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+3, 0, 8, __val)
+
+/* _MEDIA_STATUS_RPT_PARM_CMD1 */
+#define SET_H2CCMD_MSRRPT_PARM_OPMODE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_IND(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd+1, 0, 8, __value)
+#define SET_H2CCMD_MSRRPT_PARM_MACID_END(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd+2, 0, 8, __value)
+
+/* AP_OFFLOAD */
+#define SET_H2CCMD_AP_OFFLOAD_ON(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 8, __value)
+#define SET_H2CCMD_AP_OFFLOAD_HIDDEN(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+#define SET_H2CCMD_AP_OFFLOAD_DENYANY(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+#define SET_H2CCMD_AP_OFFLOAD_WAKEUP_EVT_RPT(__cmd, __value) \
+	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __value)
+
+/* Keep Alive Control*/
+#define SET_8812_H2CCMD_KEEP_ALIVE_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#define SET_8812_H2CCMD_KEEP_ALIVE_ACCPEPT_USER_DEFINED(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_8812_H2CCMD_KEEP_ALIVE_PERIOD(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+
+/*REMOTE_WAKE_CTRL */
+#define SET_8812_H2CCMD_REMOTE_WAKECTRL_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#define SET_8812_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_8812_H2CCMD_REMOTE_WAKE_CTRL_NDP_OFFLOAD_EN(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE(__cmd, 2, 1, __value)
+#define SET_8812_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE(__cmd, 3, 1, __value)
+#define SET_8812_H2CCMD_REMOTE_WAKE_CTRL_REALWOWV2_EN(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE(__cmd, 6, 1, __value)
+
+/* GTK_OFFLOAD */
+#define SET_8812_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 8, __value)
+#define SET_8812_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+
+/* AOAC_RSVDPAGE_LOC */
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd), 0, 8, __value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+3, 0, 8, __value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+4, 0, 8, __value)
+#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_EXT_MEM(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE((__cmd)+5, 0, 8, __value)
+
+/* Disconnect_Decision_Control */
+#define SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_ENABLE(__cmd, __value)	\
+	SET_BITS_TO_LE_1BYTE(__cmd, 0, 1, __value)
+#define SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_USER_SETTING(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE(__cmd, 1, 1, __value)
+#define SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_CHECK_PERIOD(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE((__cmd)+1, 0, 8, __value) /* unit: beacon period */
+#define SET_8812_H2CCMD_DISCONNECT_DECISION_CTRL_TRYPKT_NUM(__cmd, __value)\
+	SET_BITS_TO_LE_1BYTE((__cmd)+2, 0, 8, __value)
+
+int rtl8821ae_download_fw(struct ieee80211_hw *hw, bool buse_wake_on_wlan_fw);
+#if (USE_SPECIFIC_FW_TO_SUPPORT_WOWLAN == 1)
+void rtl8821ae_set_fw_related_for_wowlan(struct ieee80211_hw *hw,
+					 bool used_wowlan_fw);
+
+#endif
+void rtl8821ae_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
+			    u32 cmd_len, u8 *cmdbuffer);
+void rtl8821ae_firmware_selfreset(struct ieee80211_hw *hw);
+void rtl8821ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
+void rtl8821ae_set_fw_media_status_rpt_cmd(struct ieee80211_hw *hw,
+					   u8 mstatus);
+void rtl8821ae_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,
+				      u8 ap_offload_enable);
+void rtl8821ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
+				  bool b_dl_finished, bool dl_whole_packet);
+void rtl8812ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw,
+				  bool b_dl_finished, bool dl_whole_packet);
+void rtl8821ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw,
+				      u8 p2p_ps_state);
+void rtl8821ae_set_fw_wowlan_mode(struct ieee80211_hw *hw, bool func_en);
+void rtl8821ae_set_fw_remote_wake_ctrl_cmd(struct ieee80211_hw *hw,
+					   u8 enable);
+void rtl8821ae_set_fw_keep_alive_cmd(struct ieee80211_hw *hw, bool func_en);
+void rtl8821ae_set_fw_disconnect_decision_ctrl_cmd(struct ieee80211_hw *hw,
+						   bool enabled);
+void rtl8821ae_set_fw_global_info_cmd(struct ieee80211_hw *hw);
+void rtl8821ae_c2h_packet_handler(struct ieee80211_hw *hw,
+				  u8 *buffer, u8 length);
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
new file mode 100644
index 000000000000..310d3163dc5b
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.c
@@ -0,0 +1,4218 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../efuse.h"
+#include "../base.h"
+#include "../regd.h"
+#include "../cam.h"
+#include "../ps.h"
+#include "../pci.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "dm.h"
+#include "fw.h"
+#include "led.h"
+#include "hw.h"
+#include "../pwrseqcmd.h"
+#include "pwrseq.h"
+#include "../btcoexist/rtl_btc.h"
+
+#define LLT_CONFIG	5
+
+static void _rtl8821ae_return_beacon_queue_skb(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
+	unsigned long flags;
+
+	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
+	while (skb_queue_len(&ring->queue)) {
+		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
+		struct sk_buff *skb = __skb_dequeue(&ring->queue);
+
+		pci_unmap_single(rtlpci->pdev,
+				 rtlpriv->cfg->ops->get_desc(
+				 (u8 *)entry, true, HW_DESC_TXBUFF_ADDR),
+				 skb->len, PCI_DMA_TODEVICE);
+		kfree_skb(skb);
+		ring->idx = (ring->idx + 1) % ring->entries;
+	}
+	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
+}
+
+static void _rtl8821ae_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
+					u8 set_bits, u8 clear_bits)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtlpci->reg_bcn_ctrl_val |= set_bits;
+	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
+
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlpci->reg_bcn_ctrl_val);
+}
+
+void _rtl8821ae_stop_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte & (~BIT(6)));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+void _rtl8821ae_resume_tx_beacon(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp1byte;
+
+	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte | BIT(6));
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
+	tmp1byte |= BIT(0);
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
+}
+
+static void _rtl8821ae_enable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(1));
+}
+
+static void _rtl8821ae_disable_bcn_sub_func(struct ieee80211_hw *hw)
+{
+	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(1), 0);
+}
+
+static void _rtl8821ae_set_fw_clock_on(struct ieee80211_hw *hw,
+				       u8 rpwm_val, bool b_need_turn_off_ckk)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool b_support_remote_wake_up;
+	u32 count = 0, isr_regaddr, content;
+	bool b_schedule_timer = b_need_turn_off_ckk;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+					(u8 *)(&b_support_remote_wake_up));
+
+	if (!rtlhal->fw_ready)
+		return;
+	if (!rtlpriv->psc.fw_current_inpsmode)
+		return;
+
+	while (1) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (rtlhal->fw_clk_change_in_progress) {
+			while (rtlhal->fw_clk_change_in_progress) {
+				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+				count++;
+				udelay(100);
+				if (count > 1000)
+					goto change_done;
+				spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			}
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			rtlhal->fw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			goto change_done;
+		}
+	}
+change_done:
+	if (IS_IN_LOW_POWER_STATE_8821AE(rtlhal->fw_ps_state)) {
+		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_SET_RPWM,
+					(u8 *)(&rpwm_val));
+		if (FW_PS_IS_ACK(rpwm_val)) {
+			isr_regaddr = REG_HISR;
+			content = rtl_read_dword(rtlpriv, isr_regaddr);
+			while (!(content & IMR_CPWM) && (count < 500)) {
+				udelay(50);
+				count++;
+				content = rtl_read_dword(rtlpriv, isr_regaddr);
+			}
+
+			if (content & IMR_CPWM) {
+				rtl_write_word(rtlpriv, isr_regaddr, 0x0100);
+				rtlhal->fw_ps_state = FW_PS_STATE_RF_ON_8821AE;
+				RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+					 "Receive CPWM INT!!! Set rtlhal->FwPSState = %X\n",
+					 rtlhal->fw_ps_state);
+			}
+		}
+
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->fw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (b_schedule_timer)
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+	} else  {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		rtlhal->fw_clk_change_in_progress = false;
+		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+	}
+}
+
+static void _rtl8821ae_set_fw_clock_off(struct ieee80211_hw *hw,
+					u8 rpwm_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring;
+	enum rf_pwrstate rtstate;
+	bool b_schedule_timer = false;
+	u8 queue;
+
+	if (!rtlhal->fw_ready)
+		return;
+	if (!rtlpriv->psc.fw_current_inpsmode)
+		return;
+	if (!rtlhal->allow_sw_to_change_hwclc)
+		return;
+	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RF_STATE, (u8 *)(&rtstate));
+	if (rtstate == ERFOFF || rtlpriv->psc.inactive_pwrstate == ERFOFF)
+		return;
+
+	for (queue = 0; queue < RTL_PCI_MAX_TX_QUEUE_COUNT; queue++) {
+		ring = &rtlpci->tx_ring[queue];
+		if (skb_queue_len(&ring->queue)) {
+			b_schedule_timer = true;
+			break;
+		}
+	}
+
+	if (b_schedule_timer) {
+		mod_timer(&rtlpriv->works.fw_clockoff_timer,
+			  jiffies + MSECS(10));
+		return;
+	}
+
+	if (FW_PS_STATE(rtlhal->fw_ps_state) !=
+		FW_PS_STATE_RF_OFF_LOW_PWR_8821AE) {
+		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+		if (!rtlhal->fw_clk_change_in_progress) {
+			rtlhal->fw_clk_change_in_progress = true;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
+			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+						      (u8 *)(&rpwm_val));
+			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
+			rtlhal->fw_clk_change_in_progress = false;
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+		} else {
+			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
+			mod_timer(&rtlpriv->works.fw_clockoff_timer,
+				  jiffies + MSECS(10));
+		}
+	}
+}
+
+static void _rtl8821ae_set_fw_ps_rf_on(struct ieee80211_hw *hw)
+{
+	u8 rpwm_val = 0;
+
+	rpwm_val |= (FW_PS_STATE_RF_OFF_8821AE | FW_PS_ACK);
+	_rtl8821ae_set_fw_clock_on(hw, rpwm_val, true);
+}
+
+static void _rtl8821ae_fwlps_leave(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool fw_current_inps = false;
+	u8 rpwm_val = 0, fw_pwrmode = FW_PS_ACTIVE_MODE;
+
+	if (ppsc->low_power_enable) {
+		rpwm_val = (FW_PS_STATE_ALL_ON_8821AE|FW_PS_ACK);/* RF on */
+		_rtl8821ae_set_fw_clock_on(hw, rpwm_val, false);
+		rtlhal->allow_sw_to_change_hwclc = false;
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+				(u8 *)(&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				(u8 *)(&fw_current_inps));
+	} else {
+		rpwm_val = FW_PS_STATE_ALL_ON_8821AE;	/* RF on */
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
+				(u8 *)(&rpwm_val));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
+				(u8 *)(&fw_pwrmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
+				(u8 *)(&fw_current_inps));
+	}
+}
+
+static void _rtl8821ae_fwlps_enter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool fw_current_inps = true;
+	u8 rpwm_val;
+
+	if (ppsc->low_power_enable) {
+		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR_8821AE;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_FW_PSMODE_STATUS,
+				(u8 *)(&fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_H2C_FW_PWRMODE,
+				(u8 *)(&ppsc->fwctrl_psmode));
+		rtlhal->allow_sw_to_change_hwclc = true;
+		_rtl8821ae_set_fw_clock_off(hw, rpwm_val);
+	} else {
+		rpwm_val = FW_PS_STATE_RF_OFF_8821AE;	/* RF off */
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_FW_PSMODE_STATUS,
+				(u8 *)(&fw_current_inps));
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_H2C_FW_PWRMODE,
+				(u8 *)(&ppsc->fwctrl_psmode));
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+				HW_VAR_SET_RPWM,
+				(u8 *)(&rpwm_val));
+	}
+}
+
+static void _rtl8821ae_download_rsvd_page(struct ieee80211_hw *hw,
+					  bool dl_whole_packets)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 tmp_regcr, tmp_reg422, bcnvalid_reg;
+	u8 count = 0, dlbcn_count = 0;
+	bool send_beacon = false;
+
+	tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, (tmp_regcr | BIT(0)));
+
+	_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
+	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
+
+	tmp_reg422 = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
+		       tmp_reg422 & (~BIT(6)));
+	if (tmp_reg422 & BIT(6))
+		send_beacon = true;
+
+	do {
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL + 2);
+		rtl_write_byte(rtlpriv, REG_TDECTRL + 2,
+			       (bcnvalid_reg | BIT(0)));
+		_rtl8821ae_return_beacon_queue_skb(hw);
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_set_fw_rsvdpagepkt(hw, false,
+						     dl_whole_packets);
+		else
+			rtl8821ae_set_fw_rsvdpagepkt(hw, false,
+						     dl_whole_packets);
+
+		bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL + 2);
+		count = 0;
+		while (!(bcnvalid_reg & BIT(0)) && count < 20) {
+			count++;
+			udelay(10);
+			bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL + 2);
+		}
+		dlbcn_count++;
+	} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
+
+	if (!(bcnvalid_reg & BIT(0)))
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Download RSVD page failed!\n");
+	if (bcnvalid_reg & BIT(0) && rtlhal->enter_pnp_sleep) {
+		rtl_write_byte(rtlpriv, REG_TDECTRL + 2, bcnvalid_reg | BIT(0));
+		_rtl8821ae_return_beacon_queue_skb(hw);
+		if (send_beacon) {
+			dlbcn_count = 0;
+			do {
+				rtl_write_byte(rtlpriv, REG_TDECTRL + 2,
+					       bcnvalid_reg | BIT(0));
+
+				_rtl8821ae_return_beacon_queue_skb(hw);
+
+				if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+					rtl8812ae_set_fw_rsvdpagepkt(hw, true,
+								     false);
+				else
+					rtl8821ae_set_fw_rsvdpagepkt(hw, true,
+								     false);
+
+				/* check rsvd page download OK. */
+				bcnvalid_reg = rtl_read_byte(rtlpriv,
+							     REG_TDECTRL + 2);
+				count = 0;
+				while (!(bcnvalid_reg & BIT(0)) && count < 20) {
+					count++;
+					udelay(10);
+					bcnvalid_reg =
+					  rtl_read_byte(rtlpriv,
+							REG_TDECTRL + 2);
+				}
+				dlbcn_count++;
+			} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count < 5);
+
+			if (!(bcnvalid_reg & BIT(0)))
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "2 Download RSVD page failed!\n");
+		}
+	}
+
+	if (bcnvalid_reg & BIT(0))
+		rtl_write_byte(rtlpriv, REG_TDECTRL + 2, BIT(0));
+
+	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
+	_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
+
+	if (send_beacon)
+		rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp_reg422);
+
+	if (!rtlhal->enter_pnp_sleep) {
+		tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
+		rtl_write_byte(rtlpriv, REG_CR + 1, (tmp_regcr & ~(BIT(0))));
+	}
+}
+
+void rtl8821ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+
+	switch (variable) {
+	case HW_VAR_ETHER_ADDR:
+		*((u32 *)(val)) = rtl_read_dword(rtlpriv, REG_MACID);
+		*((u16 *)(val+4)) = rtl_read_word(rtlpriv, REG_MACID + 4);
+		break;
+	case HW_VAR_BSSID:
+		*((u32 *)(val)) = rtl_read_dword(rtlpriv, REG_BSSID);
+		*((u16 *)(val+4)) = rtl_read_word(rtlpriv, REG_BSSID+4);
+		break;
+	case HW_VAR_MEDIA_STATUS:
+		val[0] = rtl_read_byte(rtlpriv, REG_CR+2) & 0x3;
+		break;
+	case HW_VAR_SLOT_TIME:
+		*((u8 *)(val)) = mac->slot_time;
+		break;
+	case HW_VAR_BEACON_INTERVAL:
+		*((u16 *)(val)) = rtl_read_word(rtlpriv, REG_BCN_INTERVAL);
+		break;
+	case HW_VAR_ATIM_WINDOW:
+		*((u16 *)(val)) =  rtl_read_word(rtlpriv, REG_ATIMWND);
+		break;
+	case HW_VAR_RCR:
+		*((u32 *)(val)) = rtlpci->receive_config;
+		break;
+	case HW_VAR_RF_STATE:
+		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
+		break;
+	case HW_VAR_FWLPS_RF_ON:{
+		enum rf_pwrstate rfstate;
+		u32 val_rcr;
+
+		rtlpriv->cfg->ops->get_hw_reg(hw,
+					      HW_VAR_RF_STATE,
+					      (u8 *)(&rfstate));
+		if (rfstate == ERFOFF) {
+			*((bool *)(val)) = true;
+		} else {
+			val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
+			val_rcr &= 0x00070000;
+			if (val_rcr)
+				*((bool *)(val)) = false;
+			else
+				*((bool *)(val)) = true;
+		}
+		break; }
+	case HW_VAR_FW_PSMODE_STATUS:
+		*((bool *)(val)) = ppsc->fw_current_inpsmode;
+		break;
+	case HW_VAR_CORRECT_TSF:{
+		u64 tsf;
+		u32 *ptsf_low = (u32 *)&tsf;
+		u32 *ptsf_high = ((u32 *)&tsf) + 1;
+
+		*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
+		*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
+
+		*((u64 *)(val)) = tsf;
+
+		break; }
+	case HAL_DEF_WOWLAN:
+		if (ppsc->wo_wlan_mode)
+			*((bool *)(val)) = true;
+		else
+			*((bool *)(val)) = false;
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process %x\n", variable);
+		break;
+	}
+}
+
+void rtl8821ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 idx;
+
+	switch (variable) {
+	case HW_VAR_ETHER_ADDR:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_MACID + idx),
+					       val[idx]);
+			}
+			break;
+		}
+	case HW_VAR_BASIC_RATE:{
+			u16 b_rate_cfg = ((u16 *)val)[0];
+			b_rate_cfg = b_rate_cfg & 0x15f;
+			rtl_write_word(rtlpriv, REG_RRSR, b_rate_cfg);
+			break;
+		}
+	case HW_VAR_BSSID:{
+			for (idx = 0; idx < ETH_ALEN; idx++) {
+				rtl_write_byte(rtlpriv, (REG_BSSID + idx),
+					       val[idx]);
+			}
+			break;
+		}
+	case HW_VAR_SIFS:
+		rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[0]);
+
+		rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
+
+		rtl_write_byte(rtlpriv, REG_RESP_SIFS_OFDM + 1, val[0]);
+		rtl_write_byte(rtlpriv, REG_RESP_SIFS_OFDM, val[0]);
+		break;
+	case HW_VAR_R2T_SIFS:
+		rtl_write_byte(rtlpriv, REG_RESP_SIFS_OFDM + 1, val[0]);
+		break;
+	case HW_VAR_SLOT_TIME:{
+		u8 e_aci;
+
+		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+			 "HW_VAR_SLOT_TIME %x\n", val[0]);
+
+		rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
+
+		for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_AC_PARAM,
+						      (u8 *)(&e_aci));
+		}
+		break; }
+	case HW_VAR_ACK_PREAMBLE:{
+		u8 reg_tmp;
+		u8 short_preamble = (bool)(*(u8 *)val);
+
+		reg_tmp = rtl_read_byte(rtlpriv, REG_TRXPTCL_CTL+2);
+		if (short_preamble) {
+			reg_tmp |= BIT(1);
+			rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2,
+				       reg_tmp);
+		} else {
+			reg_tmp &= (~BIT(1));
+			rtl_write_byte(rtlpriv,
+				REG_TRXPTCL_CTL + 2,
+				reg_tmp);
+		}
+		break; }
+	case HW_VAR_WPA_CONFIG:
+		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *)val));
+		break;
+	case HW_VAR_AMPDU_MIN_SPACE:{
+		u8 min_spacing_to_set;
+		u8 sec_min_space;
+
+		min_spacing_to_set = *((u8 *)val);
+		if (min_spacing_to_set <= 7) {
+			sec_min_space = 0;
+
+			if (min_spacing_to_set < sec_min_space)
+				min_spacing_to_set = sec_min_space;
+
+			mac->min_space_cfg = ((mac->min_space_cfg &
+					       0xf8) |
+					      min_spacing_to_set);
+
+			*val = min_spacing_to_set;
+
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
+				  mac->min_space_cfg);
+
+			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+				       mac->min_space_cfg);
+		}
+		break; }
+	case HW_VAR_SHORTGI_DENSITY:{
+		u8 density_to_set;
+
+		density_to_set = *((u8 *)val);
+		mac->min_space_cfg |= (density_to_set << 3);
+
+		RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+			 "Set HW_VAR_SHORTGI_DENSITY: %#x\n",
+			  mac->min_space_cfg);
+
+		rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
+			       mac->min_space_cfg);
+
+		break; }
+	case HW_VAR_AMPDU_FACTOR:{
+		u32	ampdu_len =  (*((u8 *)val));
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			if (ampdu_len < VHT_AGG_SIZE_128K)
+				ampdu_len =
+					(0x2000 << (*((u8 *)val))) - 1;
+			else
+				ampdu_len = 0x1ffff;
+		} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			if (ampdu_len < HT_AGG_SIZE_64K)
+				ampdu_len =
+					(0x2000 << (*((u8 *)val))) - 1;
+			else
+				ampdu_len = 0xffff;
+		}
+		ampdu_len |= BIT(31);
+
+		rtl_write_dword(rtlpriv,
+			REG_AMPDU_MAX_LENGTH_8812, ampdu_len);
+		break; }
+	case HW_VAR_AC_PARAM:{
+		u8 e_aci = *((u8 *)val);
+
+		rtl8821ae_dm_init_edca_turbo(hw);
+		if (rtlpci->acm_method != EACMWAY2_SW)
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_ACM_CTRL,
+						      (u8 *)(&e_aci));
+		break; }
+	case HW_VAR_ACM_CTRL:{
+		u8 e_aci = *((u8 *)val);
+		union aci_aifsn *p_aci_aifsn =
+		    (union aci_aifsn *)(&mac->ac[0].aifs);
+		u8 acm = p_aci_aifsn->f.acm;
+		u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
+
+		acm_ctrl =
+		    acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
+
+		if (acm) {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl |= ACMHW_BEQEN;
+				break;
+			case AC2_VI:
+				acm_ctrl |= ACMHW_VIQEN;
+				break;
+			case AC3_VO:
+				acm_ctrl |= ACMHW_VOQEN;
+				break;
+			default:
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "HW_VAR_ACM_CTRL acm set failed: eACI is %d\n",
+					 acm);
+				break;
+			}
+		} else {
+			switch (e_aci) {
+			case AC0_BE:
+				acm_ctrl &= (~ACMHW_BEQEN);
+				break;
+			case AC2_VI:
+				acm_ctrl &= (~ACMHW_VIQEN);
+				break;
+			case AC3_VO:
+				acm_ctrl &= (~ACMHW_BEQEN);
+				break;
+			default:
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+					 "switch case not process\n");
+				break;
+			}
+		}
+
+		RT_TRACE(rtlpriv, COMP_QOS, DBG_TRACE,
+			 "SetHwReg8190pci(): [HW_VAR_ACM_CTRL] Write 0x%X\n",
+			 acm_ctrl);
+		rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
+		break; }
+	case HW_VAR_RCR:
+		rtl_write_dword(rtlpriv, REG_RCR, ((u32 *)(val))[0]);
+		rtlpci->receive_config = ((u32 *)(val))[0];
+		break;
+	case HW_VAR_RETRY_LIMIT:{
+		u8 retry_limit = ((u8 *)(val))[0];
+
+		rtl_write_word(rtlpriv, REG_RL,
+			       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
+			       retry_limit << RETRY_LIMIT_LONG_SHIFT);
+		break; }
+	case HW_VAR_DUAL_TSF_RST:
+		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
+		break;
+	case HW_VAR_EFUSE_BYTES:
+		rtlefuse->efuse_usedbytes = *((u16 *)val);
+		break;
+	case HW_VAR_EFUSE_USAGE:
+		rtlefuse->efuse_usedpercentage = *((u8 *)val);
+		break;
+	case HW_VAR_IO_CMD:
+		rtl8821ae_phy_set_io_cmd(hw, (*(enum io_type *)val));
+		break;
+	case HW_VAR_SET_RPWM:{
+		u8 rpwm_val;
+
+		rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
+		udelay(1);
+
+		if (rpwm_val & BIT(7)) {
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+				       (*(u8 *)val));
+		} else {
+			rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
+				       ((*(u8 *)val) | BIT(7)));
+		}
+
+		break; }
+	case HW_VAR_H2C_FW_PWRMODE:
+		rtl8821ae_set_fw_pwrmode_cmd(hw, (*(u8 *)val));
+		break;
+	case HW_VAR_FW_PSMODE_STATUS:
+		ppsc->fw_current_inpsmode = *((bool *)val);
+		break;
+	case HW_VAR_INIT_RTS_RATE:
+		break;
+	case HW_VAR_RESUME_CLK_ON:
+		_rtl8821ae_set_fw_ps_rf_on(hw);
+		break;
+	case HW_VAR_FW_LPS_ACTION:{
+		bool b_enter_fwlps = *((bool *)val);
+
+		if (b_enter_fwlps)
+			_rtl8821ae_fwlps_enter(hw);
+		 else
+			_rtl8821ae_fwlps_leave(hw);
+		 break; }
+	case HW_VAR_H2C_FW_JOINBSSRPT:{
+		u8 mstatus = (*(u8 *)val);
+
+		if (mstatus == RT_MEDIA_CONNECT) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID,
+						      NULL);
+			_rtl8821ae_download_rsvd_page(hw, false);
+		}
+		rtl8821ae_set_fw_media_status_rpt_cmd(hw, mstatus);
+
+		break; }
+	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:
+		rtl8821ae_set_p2p_ps_offload_cmd(hw, (*(u8 *)val));
+		break;
+	case HW_VAR_AID:{
+		u16 u2btmp;
+		u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
+		u2btmp &= 0xC000;
+		rtl_write_word(rtlpriv, REG_BCN_PSR_RPT, (u2btmp |
+			       mac->assoc_id));
+		break; }
+	case HW_VAR_CORRECT_TSF:{
+		u8 btype_ibss = ((u8 *)(val))[0];
+
+		if (btype_ibss)
+			_rtl8821ae_stop_tx_beacon(hw);
+
+		_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
+
+		rtl_write_dword(rtlpriv, REG_TSFTR,
+				(u32)(mac->tsf & 0xffffffff));
+		rtl_write_dword(rtlpriv, REG_TSFTR + 4,
+				(u32)((mac->tsf >> 32) & 0xffffffff));
+
+		_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
+
+		if (btype_ibss)
+			_rtl8821ae_resume_tx_beacon(hw);
+		break; }
+	case HW_VAR_NAV_UPPER: {
+		u32	us_nav_upper = ((u32)*val);
+
+		if (us_nav_upper > HAL_92C_NAV_UPPER_UNIT * 0xFF) {
+			RT_TRACE(rtlpriv, COMP_INIT , DBG_WARNING,
+				 "The setting value (0x%08X us) of NAV_UPPER is larger than (%d * 0xFF)!!!\n",
+				 us_nav_upper, HAL_92C_NAV_UPPER_UNIT);
+			break;
+		}
+		rtl_write_byte(rtlpriv, REG_NAV_UPPER,
+			       ((u8)((us_nav_upper +
+				HAL_92C_NAV_UPPER_UNIT - 1) /
+				HAL_92C_NAV_UPPER_UNIT)));
+		break; }
+	case HW_VAR_KEEP_ALIVE: {
+		u8 array[2];
+		array[0] = 0xff;
+		array[1] = *((u8 *)val);
+		rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_KEEP_ALIVE_CTRL, 2,
+				       array);
+		break; }
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process %x\n", variable);
+		break;
+	}
+}
+
+static bool _rtl8821ae_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	bool status = true;
+	long count = 0;
+	u32 value = _LLT_INIT_ADDR(address) | _LLT_INIT_DATA(data) |
+		    _LLT_OP(_LLT_WRITE_ACCESS);
+
+	rtl_write_dword(rtlpriv, REG_LLT_INIT, value);
+
+	do {
+		value = rtl_read_dword(rtlpriv, REG_LLT_INIT);
+		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
+			break;
+
+		if (count > POLLING_LLT_THRESHOLD) {
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Failed to polling write LLT done at address %d!\n",
+				 address);
+			status = false;
+			break;
+		}
+	} while (++count);
+
+	return status;
+}
+
+static bool _rtl8821ae_llt_table_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	unsigned short i;
+	u8 txpktbuf_bndy;
+	u32 rqpn;
+	u8 maxpage;
+	bool status;
+
+	maxpage = 255;
+	txpktbuf_bndy = 0xF8;
+	rqpn = 0x80e70808;
+	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE) {
+		txpktbuf_bndy = 0xFA;
+		rqpn = 0x80e90808;
+	}
+
+	rtl_write_byte(rtlpriv, REG_TRXFF_BNDY, txpktbuf_bndy);
+	rtl_write_word(rtlpriv, REG_TRXFF_BNDY + 2, MAX_RX_DMA_BUFFER_SIZE - 1);
+
+	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
+
+	rtl_write_byte(rtlpriv, REG_PBP, 0x31);
+	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
+
+	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
+		status = _rtl8821ae_llt_write(hw, i, i + 1);
+		if (!status)
+			return status;
+	}
+
+	status = _rtl8821ae_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
+	if (!status)
+		return status;
+
+	for (i = txpktbuf_bndy; i < maxpage; i++) {
+		status = _rtl8821ae_llt_write(hw, i, (i + 1));
+		if (!status)
+			return status;
+	}
+
+	status = _rtl8821ae_llt_write(hw, maxpage, txpktbuf_bndy);
+	if (!status)
+		return status;
+
+	rtl_write_dword(rtlpriv, REG_RQPN, rqpn);
+
+	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x00);
+
+	return true;
+}
+
+static void _rtl8821ae_gen_refresh_led_state(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_led *pled0 = &pcipriv->ledctl.sw_led0;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	if (rtlpriv->rtlhal.up_first_time)
+		return;
+
+	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_sw_led_on(hw, pled0);
+		else
+			rtl8821ae_sw_led_on(hw, pled0);
+	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_sw_led_on(hw, pled0);
+		else
+			rtl8821ae_sw_led_on(hw, pled0);
+	else
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+			rtl8812ae_sw_led_off(hw, pled0);
+		else
+			rtl8821ae_sw_led_off(hw, pled0);
+}
+
+static bool _rtl8821ae_init_mac(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u8 bytetmp = 0;
+	u16 wordtmp = 0;
+	bool mac_func_enable = rtlhal->mac_func_enable;
+
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x00);
+
+	/*Auto Power Down to CHIP-off State*/
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1) & (~BIT(7));
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, bytetmp);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		/* HW Power on sequence*/
+		if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK,
+					      PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
+					      RTL8812_NIC_ENABLE_FLOW)) {
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+					 "init 8812 MAC Fail as power on failure\n");
+				return false;
+		}
+	} else {
+		/* HW Power on sequence */
+		if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_A_MSK,
+					      PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,
+					      RTL8821A_NIC_ENABLE_FLOW)){
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				"init 8821 MAC Fail as power on failure\n");
+			return false;
+		}
+	}
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO) | BIT(4);
+	rtl_write_byte(rtlpriv, REG_APS_FSMCO, bytetmp);
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_CR);
+	bytetmp = 0xff;
+	rtl_write_byte(rtlpriv, REG_CR, bytetmp);
+	mdelay(2);
+
+	bytetmp = 0xff;
+	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, bytetmp);
+	mdelay(2);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CFG + 3);
+		if (bytetmp & BIT(0)) {
+			bytetmp = rtl_read_byte(rtlpriv, 0x7c);
+			bytetmp |= BIT(6);
+			rtl_write_byte(rtlpriv, 0x7c, bytetmp);
+		}
+	}
+
+	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG + 1);
+	bytetmp &= ~BIT(4);
+	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp);
+
+	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
+
+	if (!mac_func_enable) {
+		if (!_rtl8821ae_llt_table_init(hw))
+			return false;
+	}
+
+	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
+	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
+
+	/* Enable FW Beamformer Interrupt */
+	bytetmp = rtl_read_byte(rtlpriv, REG_FWIMR + 3);
+	rtl_write_byte(rtlpriv, REG_FWIMR + 3, bytetmp | BIT(6));
+
+	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
+	wordtmp &= 0xf;
+	wordtmp |= 0xF5B1;
+	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
+
+	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 1, 0x1F);
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xFFFF);
+	/*low address*/
+	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
+			rtlpci->tx_ring[BEACON_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
+			rtlpci->tx_ring[MGNT_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
+			rtlpci->tx_ring[VO_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
+			rtlpci->tx_ring[VI_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
+			rtlpci->tx_ring[BE_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
+			rtlpci->tx_ring[BK_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_HQ_DESA,
+			rtlpci->tx_ring[HIGH_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_RX_DESA,
+			rtlpci->rx_ring[RX_MPDU_QUEUE].dma & DMA_BIT_MASK(32));
+
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 3, 0x77);
+
+	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
+
+	rtl_write_dword(rtlpriv, REG_MCUTST_1, 0);
+
+	rtl_write_byte(rtlpriv, REG_SECONDARY_CCA_CTRL, 0x3);
+	_rtl8821ae_gen_refresh_led_state(hw);
+
+	return true;
+}
+
+static void _rtl8821ae_hw_configure(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rrsr;
+
+	reg_rrsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
+
+	rtl_write_dword(rtlpriv, REG_RRSR, reg_rrsr);
+	/* ARFB table 9 for 11ac 5G 2SS */
+	rtl_write_dword(rtlpriv, REG_ARFR0 + 4, 0xfffff000);
+	/* ARFB table 10 for 11ac 5G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR1 + 4, 0x003ff000);
+	/* ARFB table 11 for 11ac 24G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR2, 0x00000015);
+	rtl_write_dword(rtlpriv, REG_ARFR2 + 4, 0x003ff000);
+	/* ARFB table 12 for 11ac 24G 1SS */
+	rtl_write_dword(rtlpriv, REG_ARFR3, 0x00000015);
+	rtl_write_dword(rtlpriv, REG_ARFR3 + 4, 0xffcff000);
+	/* 0x420[7] = 0 , enable retry AMPDU in new AMPD not singal MPDU. */
+	rtl_write_word(rtlpriv, REG_FWHW_TXQ_CTRL, 0x1F00);
+	rtl_write_byte(rtlpriv, REG_AMPDU_MAX_TIME, 0x70);
+
+	/*Set retry limit*/
+	rtl_write_word(rtlpriv, REG_RL, 0x0707);
+
+	/* Set Data / Response auto rate fallack retry count*/
+	rtl_write_dword(rtlpriv, REG_DARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_DARFRC + 4, 0x07060504);
+	rtl_write_dword(rtlpriv, REG_RARFRC, 0x01000000);
+	rtl_write_dword(rtlpriv, REG_RARFRC + 4, 0x07060504);
+
+	rtlpci->reg_bcn_ctrl_val = 0x1d;
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, rtlpci->reg_bcn_ctrl_val);
+
+	/* TBTT prohibit hold time. Suggested by designer TimChen. */
+	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
+
+	/* AGGR_BK_TIME Reg51A 0x16 */
+	rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0040);
+
+	/*For Rx TP. Suggested by SD1 Richard. Added by tynli. 2010.04.12.*/
+	rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x03086666);
+
+	rtl_write_byte(rtlpriv, REG_HT_SINGLE_AMPDU, 0x80);
+	rtl_write_byte(rtlpriv, REG_RX_PKT_LIMIT, 0x20);
+	rtl_write_word(rtlpriv, REG_MAX_AGGR_NUM, 0x1F1F);
+}
+
+static u16 _rtl8821ae_mdio_read(struct rtl_priv *rtlpriv, u8 addr)
+{
+	u16 ret = 0;
+	u8 tmp = 0, count = 0;
+
+	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(6));
+	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6);
+		count++;
+	}
+	if (0 == tmp)
+		ret = rtl_read_word(rtlpriv, REG_MDIO_RDATA);
+
+	return ret;
+}
+
+static void _rtl8821ae_mdio_write(struct rtl_priv *rtlpriv, u8 addr, u16 data)
+{
+	u8 tmp = 0, count = 0;
+
+	rtl_write_word(rtlpriv, REG_MDIO_WDATA, data);
+	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(5));
+	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5);
+		count++;
+	}
+}
+
+static u8 _rtl8821ae_dbi_read(struct rtl_priv *rtlpriv, u16 addr)
+{
+	u16 read_addr = addr & 0xfffc;
+	u8 tmp = 0, count = 0, ret = 0;
+
+	rtl_write_word(rtlpriv, REG_DBI_ADDR, read_addr);
+	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x2);
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+		count++;
+	}
+	if (0 == tmp) {
+		read_addr = REG_DBI_RDATA + addr % 4;
+		ret = rtl_read_word(rtlpriv, read_addr);
+	}
+	return ret;
+}
+
+static void _rtl8821ae_dbi_write(struct rtl_priv *rtlpriv, u16 addr, u8 data)
+{
+	u8 tmp = 0, count = 0;
+	u16 wrtie_addr, remainder = addr % 4;
+
+	wrtie_addr = REG_DBI_WDATA + remainder;
+	rtl_write_byte(rtlpriv, wrtie_addr, data);
+
+	wrtie_addr = (addr & 0xfffc) | (BIT(0) << (remainder + 12));
+	rtl_write_word(rtlpriv, REG_DBI_ADDR, wrtie_addr);
+
+	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x1);
+
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+	count = 0;
+	while (tmp && count < 20) {
+		udelay(10);
+		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
+		count++;
+	}
+}
+
+static void _rtl8821ae_enable_aspm_back_door(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		if (_rtl8821ae_mdio_read(rtlpriv, 0x04) != 0x8544)
+			_rtl8821ae_mdio_write(rtlpriv, 0x04, 0x8544);
+
+		if (_rtl8821ae_mdio_read(rtlpriv, 0x0b) != 0x0070)
+			_rtl8821ae_mdio_write(rtlpriv, 0x0b, 0x0070);
+	}
+
+	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x70f);
+	_rtl8821ae_dbi_write(rtlpriv, 0x70f, tmp | BIT(7));
+
+	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x719);
+	_rtl8821ae_dbi_write(rtlpriv, 0x719, tmp | BIT(3) | BIT(4));
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		tmp  = _rtl8821ae_dbi_read(rtlpriv, 0x718);
+		_rtl8821ae_dbi_write(rtlpriv, 0x718, tmp|BIT(4));
+	}
+}
+
+void rtl8821ae_enable_hw_security_config(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 sec_reg_value;
+	u8 tmp;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
+		  rtlpriv->sec.pairwise_enc_algorithm,
+		  rtlpriv->sec.group_enc_algorithm);
+
+	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+			 "not open hw encryption\n");
+		return;
+	}
+
+	sec_reg_value = SCR_TXENCENABLE | SCR_RXDECENABLE;
+
+	if (rtlpriv->sec.use_defaultkey) {
+		sec_reg_value |= SCR_TXUSEDK;
+		sec_reg_value |= SCR_RXUSEDK;
+	}
+
+	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
+
+	tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, tmp | BIT(1));
+
+	RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+		 "The SECR-value %x\n", sec_reg_value);
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
+}
+
+/* Static MacID Mapping (cf. Used in MacIdDoStaticMapping) ---------- */
+#define MAC_ID_STATIC_FOR_DEFAULT_PORT				0
+#define MAC_ID_STATIC_FOR_BROADCAST_MULTICAST		1
+#define MAC_ID_STATIC_FOR_BT_CLIENT_START				2
+#define MAC_ID_STATIC_FOR_BT_CLIENT_END				3
+/* ----------------------------------------------------------- */
+
+static void rtl8821ae_macid_initialize_mediastatus(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8	media_rpt[4] = {RT_MEDIA_CONNECT, 1,
+		MAC_ID_STATIC_FOR_BROADCAST_MULTICAST,
+		MAC_ID_STATIC_FOR_BT_CLIENT_END};
+
+	rtlpriv->cfg->ops->set_hw_reg(hw,
+		HW_VAR_H2C_FW_MEDIASTATUSRPT, media_rpt);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Initialize MacId media status: from %d to %d\n",
+		 MAC_ID_STATIC_FOR_BROADCAST_MULTICAST,
+		 MAC_ID_STATIC_FOR_BT_CLIENT_END);
+}
+
+static bool _rtl8821ae_check_pcie_dma_hang(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+
+	/* write reg 0x350 Bit[26]=1. Enable debug port. */
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL + 3);
+	if (!(tmp & BIT(2))) {
+		rtl_write_byte(rtlpriv, REG_DBI_CTRL + 3, (tmp | BIT(2)));
+		mdelay(100);
+	}
+
+	/* read reg 0x350 Bit[25] if 1 : RX hang */
+	/* read reg 0x350 Bit[24] if 1 : TX hang */
+	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL + 3);
+	if ((tmp & BIT(0)) || (tmp & BIT(1))) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "CheckPcieDMAHang8821AE(): true! Reset PCIE DMA!\n");
+		return true;
+	} else {
+		return false;
+	}
+}
+
+static bool _rtl8821ae_reset_pcie_interface_dma(struct ieee80211_hw *hw,
+					 bool mac_power_on,
+					 bool in_watchdog)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp;
+	bool release_mac_rx_pause;
+	u8 backup_pcie_dma_pause;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+
+	/* 1. Disable register write lock. 0x1c[1] = 0 */
+	tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL);
+	tmp &= ~(BIT(1));
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, tmp);
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		/* write 0xCC bit[2] = 1'b1 */
+		tmp = rtl_read_byte(rtlpriv, REG_PMC_DBG_CTRL2);
+		tmp |= BIT(2);
+		rtl_write_byte(rtlpriv, REG_PMC_DBG_CTRL2, tmp);
+	}
+
+	/* 2. Check and pause TRX DMA */
+	/* write 0x284 bit[18] = 1'b1 */
+	/* write 0x301 = 0xFF */
+	tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	if (tmp & BIT(2)) {
+		/* Already pause before the function for another purpose. */
+		release_mac_rx_pause = false;
+	} else {
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, (tmp | BIT(2)));
+		release_mac_rx_pause = true;
+	}
+	backup_pcie_dma_pause = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG + 1);
+	if (backup_pcie_dma_pause != 0xFF)
+		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0xFF);
+
+	if (mac_power_on) {
+		/* 3. reset TRX function */
+		/* write 0x100 = 0x00 */
+		rtl_write_byte(rtlpriv, REG_CR, 0);
+	}
+
+	/* 4. Reset PCIe DMA. 0x3[0] = 0 */
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	tmp &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp);
+
+	/* 5. Enable PCIe DMA. 0x3[0] = 1 */
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN + 1);
+	tmp |= BIT(0);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN + 1, tmp);
+
+	if (mac_power_on) {
+		/* 6. enable TRX function */
+		/* write 0x100 = 0xFF */
+		rtl_write_byte(rtlpriv, REG_CR, 0xFF);
+
+		/* We should init LLT & RQPN and
+		 * prepare Tx/Rx descrptor address later
+		 * because MAC function is reset.*/
+	}
+
+	/* 7. Restore PCIe autoload down bit */
+	/* 8812AE does not has the defination. */
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		/* write 0xF8 bit[17] = 1'b1 */
+		tmp = rtl_read_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL + 2);
+		tmp |= BIT(1);
+		rtl_write_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL + 2, tmp);
+	}
+
+	/* In MAC power on state, BB and RF maybe in ON state,
+	 * if we release TRx DMA here.
+	 * it will cause packets to be started to Tx/Rx,
+	 * so we release Tx/Rx DMA later.*/
+	if (!mac_power_on/* || in_watchdog*/) {
+		/* 8. release TRX DMA */
+		/* write 0x284 bit[18] = 1'b0 */
+		/* write 0x301 = 0x00 */
+		if (release_mac_rx_pause) {
+			tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+			rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL,
+				       tmp & (~BIT(2)));
+		}
+		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1,
+			       backup_pcie_dma_pause);
+	}
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		/* 9. lock system register */
+		/* write 0xCC bit[2] = 1'b0 */
+		tmp = rtl_read_byte(rtlpriv, REG_PMC_DBG_CTRL2);
+		tmp &= ~(BIT(2));
+		rtl_write_byte(rtlpriv, REG_PMC_DBG_CTRL2, tmp);
+	}
+	return true;
+}
+
+static void _rtl8821ae_get_wakeup_reason(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+	u8 fw_reason = 0;
+	struct timeval ts;
+
+	fw_reason = rtl_read_byte(rtlpriv, REG_MCUTST_WOWLAN);
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "WOL Read 0x1c7 = %02X\n",
+		 fw_reason);
+
+	ppsc->wakeup_reason = 0;
+
+	rtlhal->last_suspend_sec = ts.tv_sec;
+
+	switch (fw_reason) {
+	case FW_WOW_V2_PTK_UPDATE_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_PTK_UPDATE;
+		do_gettimeofday(&ts);
+		ppsc->last_wakeup_time = ts.tv_sec*1000 + ts.tv_usec/1000;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's a WOL PTK Key update event!\n");
+		break;
+	case FW_WOW_V2_GTK_UPDATE_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_GTK_UPDATE;
+		do_gettimeofday(&ts);
+		ppsc->last_wakeup_time = ts.tv_sec*1000 + ts.tv_usec/1000;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's a WOL GTK Key update event!\n");
+		break;
+	case FW_WOW_V2_DISASSOC_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_DISASSOC;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's a disassociation event!\n");
+		break;
+	case FW_WOW_V2_DEAUTH_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_DEAUTH;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's a deauth event!\n");
+		break;
+	case FW_WOW_V2_FW_DISCONNECT_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_AP_LOST;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's a Fw disconnect decision (AP lost) event!\n");
+	break;
+	case FW_WOW_V2_MAGIC_PKT_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_MAGIC_PKT;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's a magic packet event!\n");
+		break;
+	case FW_WOW_V2_UNICAST_PKT_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_UNICAST_PKT;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's an unicast packet event!\n");
+		break;
+	case FW_WOW_V2_PATTERN_PKT_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_PATTERN_PKT;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's a pattern match event!\n");
+		break;
+	case FW_WOW_V2_RTD3_SSID_MATCH_EVENT:
+		ppsc->wakeup_reason = WOL_REASON_RTD3_SSID_MATCH;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's an RTD3 Ssid match event!\n");
+		break;
+	case FW_WOW_V2_REALWOW_V2_WAKEUPPKT:
+		ppsc->wakeup_reason = WOL_REASON_REALWOW_V2_WAKEUPPKT;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's an RealWoW wake packet event!\n");
+		break;
+	case FW_WOW_V2_REALWOW_V2_ACKLOST:
+		ppsc->wakeup_reason = WOL_REASON_REALWOW_V2_ACKLOST;
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "It's an RealWoW ack lost event!\n");
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_DMESG,
+			 "WOL Read 0x1c7 = %02X, Unknown reason!\n",
+			  fw_reason);
+		break;
+	}
+}
+
+static void _rtl8821ae_init_trx_desc_hw_address(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	/*low address*/
+	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
+			rtlpci->tx_ring[BEACON_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
+			rtlpci->tx_ring[MGNT_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
+			rtlpci->tx_ring[VO_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
+			rtlpci->tx_ring[VI_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
+			rtlpci->tx_ring[BE_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
+			rtlpci->tx_ring[BK_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_HQ_DESA,
+			rtlpci->tx_ring[HIGH_QUEUE].dma & DMA_BIT_MASK(32));
+	rtl_write_dword(rtlpriv, REG_RX_DESA,
+			rtlpci->rx_ring[RX_MPDU_QUEUE].dma & DMA_BIT_MASK(32));
+}
+
+static bool _rtl8821ae_init_llt_table(struct ieee80211_hw *hw, u32 boundary)
+{
+	bool status = true;
+	u32 i;
+	u32 txpktbuf_bndy = boundary;
+	u32 last_entry_of_txpktbuf = LAST_ENTRY_OF_TX_PKT_BUFFER;
+
+	for (i = 0 ; i < (txpktbuf_bndy - 1) ; i++) {
+		status = _rtl8821ae_llt_write(hw, i , i + 1);
+		if (!status)
+			return status;
+	}
+
+	status = _rtl8821ae_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
+	if (!status)
+		return status;
+
+	for (i = txpktbuf_bndy ; i < last_entry_of_txpktbuf ; i++) {
+		status = _rtl8821ae_llt_write(hw, i, (i + 1));
+		if (!status)
+			return status;
+	}
+
+	status = _rtl8821ae_llt_write(hw, last_entry_of_txpktbuf,
+				      txpktbuf_bndy);
+	if (!status)
+		return status;
+
+	return status;
+}
+
+static bool _rtl8821ae_dynamic_rqpn(struct ieee80211_hw *hw, u32 boundary,
+			     u16 npq_rqpn_value, u32 rqpn_val)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 tmp;
+	bool ret = true;
+	u16 count = 0, tmp16;
+	bool support_remote_wakeup;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&support_remote_wakeup));
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "boundary=0x%#X, NPQ_RQPNValue=0x%#X, RQPNValue=0x%#X\n",
+		  boundary, npq_rqpn_value, rqpn_val);
+
+	/* stop PCIe DMA
+	 * 1. 0x301[7:0] = 0xFE */
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0xFE);
+
+	/* wait TXFF empty
+	 * 2. polling till 0x41A[15:0]=0x07FF */
+	tmp16 = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
+	while ((tmp16 & 0x07FF) != 0x07FF) {
+		udelay(100);
+		tmp16 = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
+		count++;
+		if ((count % 200) == 0) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Tx queue is not empty for 20ms!\n");
+		}
+		if (count >= 1000) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Wait for Tx FIFO empty timeout!\n");
+			break;
+		}
+	}
+
+	/* TX pause
+	 * 3. reg 0x522=0xFF */
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
+
+	/* Wait TX State Machine OK
+	 * 4. polling till reg 0x5FB~0x5F8 = 0x00000000 for 50ms */
+	count = 0;
+	while (rtl_read_byte(rtlpriv, REG_SCH_TXCMD) != 0) {
+		udelay(100);
+		count++;
+		if (count >= 500) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Wait for TX State Machine ready timeout !!\n");
+			break;
+		}
+	}
+
+	/* stop RX DMA path
+	 * 5.	0x284[18] = 1
+	 * 6.	wait till 0x284[17] == 1
+	 * wait RX DMA idle */
+	count = 0;
+	tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, (tmp | BIT(2)));
+	do {
+		tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+		udelay(10);
+		count++;
+	} while (!(tmp & BIT(1)) && count < 100);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Wait until Rx DMA Idle. count=%d REG[0x286]=0x%x\n",
+		  count, tmp);
+
+	/* reset BB
+	 * 7.	0x02 [0] = 0 */
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN);
+	tmp &= ~(BIT(0));
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, tmp);
+
+	/* Reset TRX MAC
+	 * 8.	 0x100 = 0x00
+	 * Delay (1ms) */
+	rtl_write_byte(rtlpriv, REG_CR, 0x00);
+	udelay(1000);
+
+	/* Disable MAC Security Engine
+	 * 9.	0x100 bit[9]=0 */
+	tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
+	tmp &= ~(BIT(1));
+	rtl_write_byte(rtlpriv, REG_CR + 1, tmp);
+
+	/* To avoid DD-Tim Circuit hang
+	 * 10.	0x553 bit[5]=1 */
+	tmp = rtl_read_byte(rtlpriv, REG_DUAL_TSF_RST);
+	rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (tmp | BIT(5)));
+
+	/* Enable MAC Security Engine
+	 * 11.	0x100 bit[9]=1 */
+	tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
+	rtl_write_byte(rtlpriv, REG_CR + 1, (tmp | BIT(1)));
+
+	/* Enable TRX MAC
+	 * 12.	 0x100 = 0xFF
+	 *	Delay (1ms) */
+	rtl_write_byte(rtlpriv, REG_CR, 0xFF);
+	udelay(1000);
+
+	/* Enable BB
+	 * 13.	0x02 [0] = 1 */
+	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, (tmp | BIT(0)));
+
+	/* beacon setting
+	 * 14,15. set beacon head page (reg 0x209 and 0x424) */
+	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, (u8)boundary);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_BCNQ_BDNY, (u8)boundary);
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_MGQ_BDNY, (u8)boundary);
+
+	/* 16.	WMAC_LBK_BF_HD 0x45D[7:0]
+	 * WMAC_LBK_BF_HD */
+	rtl_write_byte(rtlpriv, REG_TXPKTBUF_WMAC_LBK_BF_HD,
+		       (u8)boundary);
+
+	rtl_write_word(rtlpriv, REG_TRXFF_BNDY, boundary);
+
+	/* init LLT
+	 * 17. init LLT */
+	if (!_rtl8821ae_init_llt_table(hw, boundary)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_WARNING,
+			 "Failed to init LLT table!\n");
+		return false;
+	}
+
+	/* reallocate RQPN
+	 * 18. reallocate RQPN and init LLT */
+	rtl_write_word(rtlpriv, REG_RQPN_NPQ, npq_rqpn_value);
+	rtl_write_dword(rtlpriv, REG_RQPN, rqpn_val);
+
+	/* release Tx pause
+	 * 19. 0x522=0x00 */
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+
+	/* enable PCIE DMA
+	 * 20. 0x301[7:0] = 0x00
+	 * 21. 0x284[18] = 0 */
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0x00);
+	tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, (tmp&~BIT(2)));
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "End.\n");
+	return ret;
+}
+
+static void _rtl8821ae_simple_initialize_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+
+#if (USE_SPECIFIC_FW_TO_SUPPORT_WOWLAN == 1)
+	/* Re-download normal Fw. */
+	rtl8821ae_set_fw_related_for_wowlan(hw, false);
+#endif
+
+	/* Re-Initialize LLT table. */
+	if (rtlhal->re_init_llt_table) {
+		u32 rqpn = 0x80e70808;
+		u8 rqpn_npq = 0, boundary = 0xF8;
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			rqpn = 0x80e90808;
+			boundary = 0xFA;
+		}
+		if (_rtl8821ae_dynamic_rqpn(hw, boundary, rqpn_npq, rqpn))
+			rtlhal->re_init_llt_table = false;
+	}
+
+	ppsc->rfpwr_state = ERFON;
+}
+
+static void _rtl8821ae_enable_l1off(struct ieee80211_hw *hw)
+{
+	u8 tmp  = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "--->\n");
+
+	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x160);
+	if (!(tmp & (BIT(2) | BIT(3)))) {
+		RT_TRACE(rtlpriv, COMP_POWER | COMP_INIT, DBG_LOUD,
+			 "0x160(%#x)return!!\n", tmp);
+		return;
+	}
+
+	tmp = _rtl8821ae_mdio_read(rtlpriv, 0x1b);
+	_rtl8821ae_mdio_write(rtlpriv, 0x1b, (tmp | BIT(4)));
+
+	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x718);
+	_rtl8821ae_dbi_write(rtlpriv, 0x718, tmp | BIT(5));
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "<---\n");
+}
+
+static void _rtl8821ae_enable_ltr(struct ieee80211_hw *hw)
+{
+	u8 tmp  = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "--->\n");
+
+	/* Check 0x98[10] */
+	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x99);
+	if (!(tmp & BIT(2))) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "<---0x99(%#x) return!!\n", tmp);
+		return;
+	}
+
+	/* LTR idle latency, 0x90 for 144us */
+	rtl_write_dword(rtlpriv, 0x798, 0x88908890);
+
+	/* LTR active latency, 0x3c for 60us */
+	rtl_write_dword(rtlpriv, 0x79c, 0x883c883c);
+
+	tmp = rtl_read_byte(rtlpriv, 0x7a4);
+	rtl_write_byte(rtlpriv, 0x7a4, (tmp | BIT(4)));
+
+	tmp = rtl_read_byte(rtlpriv, 0x7a4);
+	rtl_write_byte(rtlpriv, 0x7a4, (tmp & (~BIT(0))));
+	rtl_write_byte(rtlpriv, 0x7a4, (tmp | BIT(0)));
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "<---\n");
+}
+
+static bool _rtl8821ae_wowlan_initialize_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	bool init_finished = true;
+	u8 tmp = 0;
+
+	/* Get Fw wake up reason. */
+	_rtl8821ae_get_wakeup_reason(hw);
+
+	/* Patch Pcie Rx DMA hang after S3/S4 several times.
+	 * The root cause has not be found. */
+	if (_rtl8821ae_check_pcie_dma_hang(hw))
+		_rtl8821ae_reset_pcie_interface_dma(hw, true, false);
+
+	/* Prepare Tx/Rx Desc Hw address. */
+	_rtl8821ae_init_trx_desc_hw_address(hw);
+
+	/* Release Pcie Interface Rx DMA to allow wake packet DMA. */
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0xFE);
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Enable PCIE Rx DMA.\n");
+
+	/* Check wake up event.
+	 * We should check wake packet bit before disable wowlan by H2C or
+	 * Fw will clear the bit. */
+	tmp = rtl_read_byte(rtlpriv, REG_FTISR + 3);
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+		 "Read REG_FTISR 0x13f = %#X\n", tmp);
+
+	/* Set the WoWLAN related function control disable. */
+	rtl8821ae_set_fw_wowlan_mode(hw, false);
+	rtl8821ae_set_fw_remote_wake_ctrl_cmd(hw, 0);
+
+	if (rtlhal->hw_rof_enable) {
+		tmp = rtl_read_byte(rtlpriv, REG_HSISR + 3);
+		if (tmp & BIT(1)) {
+			/* Clear GPIO9 ISR */
+			rtl_write_byte(rtlpriv, REG_HSISR + 3, tmp | BIT(1));
+			init_finished = false;
+		} else {
+			init_finished = true;
+		}
+	}
+
+	if (init_finished) {
+		_rtl8821ae_simple_initialize_adapter(hw);
+
+		/* Release Pcie Interface Tx DMA. */
+		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0x00);
+		/* Release Pcie RX DMA */
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, 0x02);
+
+		tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
+		rtl_write_byte(rtlpriv, REG_CR + 1, (tmp & (~BIT(0))));
+
+		_rtl8821ae_enable_l1off(hw);
+		_rtl8821ae_enable_ltr(hw);
+	}
+
+	return init_finished;
+}
+
+static void _rtl8812ae_bb8812_config_1t(struct ieee80211_hw *hw)
+{
+	/* BB OFDM RX Path_A */
+	rtl_set_bbreg(hw, 0x808, 0xff, 0x11);
+	/* BB OFDM TX Path_A */
+	rtl_set_bbreg(hw, 0x80c, MASKLWORD, 0x1111);
+	/* BB CCK R/Rx Path_A */
+	rtl_set_bbreg(hw, 0xa04, 0x0c000000, 0x0);
+	/* MCS support */
+	rtl_set_bbreg(hw, 0x8bc, 0xc0000060, 0x4);
+	/* RF Path_B HSSI OFF */
+	rtl_set_bbreg(hw, 0xe00, 0xf, 0x4);
+	/* RF Path_B Power Down */
+	rtl_set_bbreg(hw, 0xe90, MASKDWORD, 0);
+	/* ADDA Path_B OFF */
+	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0);
+	rtl_set_bbreg(hw, 0xe64, MASKDWORD, 0);
+}
+
+static void _rtl8821ae_poweroff_adapter(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 u1b_tmp;
+
+	rtlhal->mac_func_enable = false;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		/* Combo (PCIe + USB) Card and PCIe-MF Card */
+		/* 1. Run LPS WL RFOFF flow */
+		/* RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		"=====>CardDisableRTL8812E,RTL8821A_NIC_LPS_ENTER_FLOW\n");
+		*/
+		rtl_hal_pwrseqcmdparsing(rtlpriv,
+			PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8821A_NIC_LPS_ENTER_FLOW);
+	}
+	/* 2. 0x1F[7:0] = 0 */
+	/* turn off RF */
+	/* rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00); */
+	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) &&
+		rtlhal->fw_ready) {
+		rtl8821ae_firmware_selfreset(hw);
+	}
+
+	/* Reset MCU. Suggested by Filen. */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
+
+	/* g.	MCUFWDL 0x80[1:0]=0	 */
+	/* reset MCU ready status */
+	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		/* HW card disable configuration. */
+		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8821A_NIC_DISABLE_FLOW);
+	} else {
+		/* HW card disable configuration. */
+		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
+			PWR_INTF_PCI_MSK, RTL8812_NIC_DISABLE_FLOW);
+	}
+
+	/* Reset MCU IO Wrapper */
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
+	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1b_tmp | BIT(0));
+
+	/* 7. RSV_CTRL 0x1C[7:0] = 0x0E */
+	/* lock ISO/CLK/Power control register */
+	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0e);
+}
+
+int rtl8821ae_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool rtstatus = true;
+	int err;
+	u8 tmp_u1b;
+	bool support_remote_wakeup;
+	u32 nav_upper = WIFI_NAV_UPPER_US;
+
+	rtlhal->being_init_adapter = true;
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&support_remote_wakeup));
+	rtlpriv->intf_ops->disable_aspm(hw);
+
+	/*YP wowlan not considered*/
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_CR);
+	if (tmp_u1b != 0 && tmp_u1b != 0xEA) {
+		rtlhal->mac_func_enable = true;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "MAC has already power on.\n");
+	} else {
+		rtlhal->mac_func_enable = false;
+		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_8821AE;
+	}
+
+	if (support_remote_wakeup &&
+		rtlhal->wake_from_pnp_sleep &&
+		rtlhal->mac_func_enable) {
+		if (_rtl8821ae_wowlan_initialize_adapter(hw)) {
+			rtlhal->being_init_adapter = false;
+			return 0;
+		}
+	}
+
+	if (_rtl8821ae_check_pcie_dma_hang(hw)) {
+		_rtl8821ae_reset_pcie_interface_dma(hw,
+						    rtlhal->mac_func_enable,
+						    false);
+		rtlhal->mac_func_enable = false;
+	}
+
+	/* Reset MAC/BB/RF status if it is not powered off
+	 * before calling initialize Hw flow to prevent
+	 * from interface and MAC status mismatch.
+	 * 2013.06.21, by tynli. Suggested by SD1 JackieLau. */
+	if (rtlhal->mac_func_enable) {
+		_rtl8821ae_poweroff_adapter(hw);
+		rtlhal->mac_func_enable = false;
+	}
+
+	rtstatus = _rtl8821ae_init_mac(hw);
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Init MAC failed\n");
+		err = 1;
+		return err;
+	}
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CFG);
+	tmp_u1b &= 0x7F;
+	rtl_write_byte(rtlpriv, REG_SYS_CFG, tmp_u1b);
+
+	err = rtl8821ae_download_fw(hw, false);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Failed to download FW. Init HW without FW now\n");
+		err = 1;
+		rtlhal->fw_ready = false;
+		return err;
+	} else {
+		rtlhal->fw_ready = true;
+	}
+	ppsc->fw_current_inpsmode = false;
+	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_8821AE;
+	rtlhal->fw_clk_change_in_progress = false;
+	rtlhal->allow_sw_to_change_hwclc = false;
+	rtlhal->last_hmeboxnum = 0;
+
+	/*SIC_Init(Adapter);
+	if(rtlhal->AMPDUBurstMode)
+		rtl_write_byte(rtlpriv,REG_AMPDU_BURST_MODE_8812,  0x7F);*/
+
+	rtl8821ae_phy_mac_config(hw);
+	/* because last function modify RCR, so we update
+	 * rcr var here, or TP will unstable for receive_config
+	 * is wrong, RX RCR_ACRC32 will cause TP unstabel & Rx
+	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
+	rtlpci->receive_config = rtl_read_dword(rtlpriv, REG_RCR);
+	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
+	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);*/
+	rtl8821ae_phy_bb_config(hw);
+
+	rtl8821ae_phy_rf_config(hw);
+
+	if (rtlpriv->phy.rf_type == RF_1T1R &&
+		rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		_rtl8812ae_bb8812_config_1t(hw);
+
+	_rtl8821ae_hw_configure(hw);
+
+	rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_2_4G);
+
+	/*set wireless mode*/
+
+	rtlhal->mac_func_enable = true;
+
+	rtl_cam_reset_all_entry(hw);
+
+	rtl8821ae_enable_hw_security_config(hw);
+
+	ppsc->rfpwr_state = ERFON;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
+	_rtl8821ae_enable_aspm_back_door(hw);
+	rtlpriv->intf_ops->enable_aspm(hw);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE &&
+	    (rtlhal->rfe_type == 1 || rtlhal->rfe_type == 5))
+		rtl_set_bbreg(hw, 0x900, 0x00000303, 0x0302);
+
+	rtl8821ae_bt_hw_init(hw);
+	rtlpriv->rtlhal.being_init_adapter = false;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_NAV_UPPER, (u8 *)&nav_upper);
+
+	/* rtl8821ae_dm_check_txpower_tracking(hw); */
+	/* rtl8821ae_phy_lc_calibrate(hw); */
+	if (support_remote_wakeup)
+		rtl_write_byte(rtlpriv, REG_WOW_CTRL, 0);
+
+	/* Release Rx DMA*/
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+	if (tmp_u1b & BIT(2)) {
+		/* Release Rx DMA if needed*/
+		tmp_u1b &= ~BIT(2);
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, tmp_u1b);
+	}
+
+	/* Release Tx/Rx PCIE DMA if*/
+	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0);
+
+	rtl8821ae_dm_init(hw);
+	rtl8821ae_macid_initialize_mediastatus(hw);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "rtl8821ae_hw_init() <====\n");
+	return err;
+}
+
+static enum version_8821ae _rtl8821ae_read_chip_version(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum version_8821ae version = VERSION_UNKNOWN;
+	u32 value32;
+
+	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "ReadChipVersion8812A 0xF0 = 0x%x\n", value32);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtlphy->rf_type = RF_2T2R;
+	else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
+		rtlphy->rf_type = RF_1T1R;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "RF_Type is %x!!\n", rtlphy->rf_type);
+
+	if (value32 & TRP_VAUX_EN) {
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			if (rtlphy->rf_type == RF_2T2R)
+				version = VERSION_TEST_CHIP_2T2R_8812;
+			else
+				version = VERSION_TEST_CHIP_1T1R_8812;
+		} else
+			version = VERSION_TEST_CHIP_8821;
+	} else {
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			u32 rtl_id = ((value32 & CHIP_VER_RTL_MASK) >> 12) + 1;
+
+			if (rtlphy->rf_type == RF_2T2R)
+				version =
+					(enum version_8821ae)(CHIP_8812
+					| NORMAL_CHIP |
+					RF_TYPE_2T2R);
+			else
+				version = (enum version_8821ae)(CHIP_8812
+					| NORMAL_CHIP);
+
+			version = (enum version_8821ae)(version | (rtl_id << 12));
+		} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			u32 rtl_id = value32 & CHIP_VER_RTL_MASK;
+
+			version = (enum version_8821ae)(CHIP_8821
+				| NORMAL_CHIP | rtl_id);
+		}
+	}
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		/*WL_HWROF_EN.*/
+		value32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
+		rtlhal->hw_rof_enable = ((value32 & WL_HWROF_EN) ? 1 : 0);
+	}
+
+	switch (version) {
+	case VERSION_TEST_CHIP_1T1R_8812:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_TEST_CHIP_1T1R_8812\n");
+		break;
+	case VERSION_TEST_CHIP_2T2R_8812:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_TEST_CHIP_2T2R_8812\n");
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_1T1R_8812:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID:VERSION_NORMAL_TSMC_CHIP_1T1R_8812\n");
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_2T2R_8812:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_NORMAL_TSMC_CHIP_2T2R_8812\n");
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_1T1R_8812_C_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_NORMAL_TSMC_CHIP_1T1R_8812 C CUT\n");
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_2T2R_8812_C_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_NORMAL_TSMC_CHIP_2T2R_8812 C CUT\n");
+		break;
+	case VERSION_TEST_CHIP_8821:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_TEST_CHIP_8821\n");
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_8821:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_NORMAL_TSMC_CHIP_8821 A CUT\n");
+		break;
+	case VERSION_NORMAL_TSMC_CHIP_8821_B_CUT:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: VERSION_NORMAL_TSMC_CHIP_8821 B CUT\n");
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Chip Version ID: Unknow (0x%X)\n", version);
+		break;
+	}
+
+	return version;
+}
+
+static int _rtl8821ae_set_media_status(struct ieee80211_hw *hw,
+				     enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
+	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
+	bt_msr &= 0xfc;
+
+	rtl_write_dword(rtlpriv, REG_BCN_CTRL, 0);
+	RT_TRACE(rtlpriv, COMP_BEACON, DBG_LOUD,
+		"clear 0x550 when set HW_VAR_MEDIA_STATUS\n");
+
+	if (type == NL80211_IFTYPE_UNSPECIFIED ||
+	    type == NL80211_IFTYPE_STATION) {
+		_rtl8821ae_stop_tx_beacon(hw);
+		_rtl8821ae_enable_bcn_sub_func(hw);
+	} else if (type == NL80211_IFTYPE_ADHOC ||
+		type == NL80211_IFTYPE_AP) {
+		_rtl8821ae_resume_tx_beacon(hw);
+		_rtl8821ae_disable_bcn_sub_func(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "Set HW_VAR_MEDIA_STATUS: No such media status(%x).\n",
+			 type);
+	}
+
+	switch (type) {
+	case NL80211_IFTYPE_UNSPECIFIED:
+		bt_msr |= MSR_NOLINK;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to NO LINK!\n");
+		break;
+	case NL80211_IFTYPE_ADHOC:
+		bt_msr |= MSR_ADHOC;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to Ad Hoc!\n");
+		break;
+	case NL80211_IFTYPE_STATION:
+		bt_msr |= MSR_INFRA;
+		ledaction = LED_CTL_LINK;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to STA!\n");
+		break;
+	case NL80211_IFTYPE_AP:
+		bt_msr |= MSR_AP;
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Set Network type to AP!\n");
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Network type %d not support!\n", type);
+		return 1;
+	}
+
+	rtl_write_byte(rtlpriv, (MSR), bt_msr);
+	rtlpriv->cfg->ops->led_control(hw, ledaction);
+	if ((bt_msr & 0xfc) == MSR_AP)
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
+	else
+		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
+
+	return 0;
+}
+
+void rtl8821ae_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u32 reg_rcr = rtlpci->receive_config;
+
+	if (rtlpriv->psc.rfpwr_state != ERFON)
+		return;
+
+	if (check_bssid) {
+		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
+		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
+					      (u8 *)(&reg_rcr));
+		_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
+	} else if (!check_bssid) {
+		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
+		_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
+		rtlpriv->cfg->ops->set_hw_reg(hw,
+			HW_VAR_RCR, (u8 *)(&reg_rcr));
+	}
+}
+
+int rtl8821ae_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "rtl8821ae_set_network_type!\n");
+
+	if (_rtl8821ae_set_media_status(hw, type))
+		return -EOPNOTSUPP;
+
+	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
+		if (type != NL80211_IFTYPE_AP)
+			rtl8821ae_set_check_bssid(hw, true);
+	} else {
+		rtl8821ae_set_check_bssid(hw, false);
+	}
+
+	return 0;
+}
+
+/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
+void rtl8821ae_set_qos(struct ieee80211_hw *hw, int aci)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	rtl8821ae_dm_init_edca_turbo(hw);
+	switch (aci) {
+	case AC1_BK:
+		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
+		break;
+	case AC0_BE:
+		/* rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, u4b_ac_param); */
+		break;
+	case AC2_VI:
+		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x5e4322);
+		break;
+	case AC3_VO:
+		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x2f3222);
+		break;
+	default:
+		RT_ASSERT(false, "invalid aci: %d !\n", aci);
+		break;
+	}
+}
+
+static void rtl8821ae_clear_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 tmp;
+	tmp = rtl_read_dword(rtlpriv, REG_HISR);
+	/*printk("clear interrupt first:\n");
+	printk("0x%x = 0x%08x\n",REG_HISR, tmp);*/
+	rtl_write_dword(rtlpriv, REG_HISR, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HISRE);
+	/*printk("0x%x = 0x%08x\n",REG_HISRE, tmp);*/
+	rtl_write_dword(rtlpriv, REG_HISRE, tmp);
+
+	tmp = rtl_read_dword(rtlpriv, REG_HSISR);
+	/*printk("0x%x = 0x%08x\n",REG_HSISR, tmp);*/
+	rtl_write_dword(rtlpriv, REG_HSISR, tmp);
+}
+
+void rtl8821ae_enable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl8821ae_clear_interrupt(hw);/*clear it here first*/
+
+	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
+	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
+	rtlpci->irq_enabled = true;
+	/* there are some C2H CMDs have been sent before
+	system interrupt is enabled, e.g., C2H, CPWM.
+	*So we need to clear all C2H events that FW has
+	notified, otherwise FW won't schedule any commands anymore.
+	*/
+	/* rtl_write_byte(rtlpriv, REG_C2HEVT_CLEAR, 0); */
+	/*enable system interrupt*/
+	rtl_write_dword(rtlpriv, REG_HSIMR, rtlpci->sys_irq_mask & 0xFFFFFFFF);
+}
+
+void rtl8821ae_disable_interrupt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	rtl_write_dword(rtlpriv, REG_HIMR, IMR_DISABLED);
+	rtl_write_dword(rtlpriv, REG_HIMRE, IMR_DISABLED);
+	rtlpci->irq_enabled = false;
+	/*synchronize_irq(rtlpci->pdev->irq);*/
+}
+
+static void _rtl8821ae_clear_pci_pme_status(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u16 cap_hdr;
+	u8 cap_pointer;
+	u8 cap_id = 0xff;
+	u8 pmcs_reg;
+	u8 cnt = 0;
+
+	/* Get the Capability pointer first,
+	 * the Capability Pointer is located at
+	 * offset 0x34 from the Function Header */
+
+	pci_read_config_byte(rtlpci->pdev, 0x34, &cap_pointer);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "PCI configration 0x34 = 0x%2x\n", cap_pointer);
+
+	do {
+		pci_read_config_word(rtlpci->pdev, cap_pointer, &cap_hdr);
+		cap_id = cap_hdr & 0xFF;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "in pci configration, cap_pointer%x = %x\n",
+			  cap_pointer, cap_id);
+
+		if (cap_id == 0x01) {
+			break;
+		} else {
+			/* point to next Capability */
+			cap_pointer = (cap_hdr >> 8) & 0xFF;
+			/* 0: end of pci capability, 0xff: invalid value */
+			if (cap_pointer == 0x00 || cap_pointer == 0xff) {
+				cap_id = 0xff;
+				break;
+			}
+		}
+	} while (cnt++ < 200);
+
+	if (cap_id == 0x01) {
+		/* Get the PM CSR (Control/Status Register),
+		 * The PME_Status is located at PM Capatibility offset 5, bit 7
+		 */
+		pci_read_config_byte(rtlpci->pdev, cap_pointer + 5, &pmcs_reg);
+
+		if (pmcs_reg & BIT(7)) {
+			/* PME event occured, clear the PM_Status by write 1 */
+			pmcs_reg = pmcs_reg | BIT(7);
+
+			pci_write_config_byte(rtlpci->pdev, cap_pointer + 5,
+					      pmcs_reg);
+			/* Read it back to check */
+			pci_read_config_byte(rtlpci->pdev, cap_pointer + 5,
+					     &pmcs_reg);
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+				 "Clear PME status 0x%2x to 0x%2x\n",
+				  cap_pointer + 5, pmcs_reg);
+		} else {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+				 "PME status(0x%2x) = 0x%2x\n",
+				  cap_pointer + 5, pmcs_reg);
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_WARNING,
+			 "Cannot find PME Capability\n");
+	}
+}
+
+void rtl8821ae_card_disable(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	enum nl80211_iftype opmode;
+	bool support_remote_wakeup;
+	u8 tmp;
+	u32 count = 0;
+
+	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
+				      (u8 *)(&support_remote_wakeup));
+
+	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+	if (!(support_remote_wakeup && mac->opmode == NL80211_IFTYPE_STATION)
+	    || !rtlhal->enter_pnp_sleep) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Normal Power off\n");
+		mac->link_state = MAC80211_NOLINK;
+		opmode = NL80211_IFTYPE_UNSPECIFIED;
+		_rtl8821ae_set_media_status(hw, opmode);
+		_rtl8821ae_poweroff_adapter(hw);
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Wowlan Supported.\n");
+		/* 3 <1> Prepare for configuring wowlan related infomations */
+		/* Clear Fw WoWLAN event. */
+		rtl_write_byte(rtlpriv, REG_MCUTST_WOWLAN, 0x0);
+
+#if (USE_SPECIFIC_FW_TO_SUPPORT_WOWLAN == 1)
+		rtl8821ae_set_fw_related_for_wowlan(hw, true);
+#endif
+		/* Dynamically adjust Tx packet boundary
+		 * for download reserved page packet.
+		 * reserve 30 pages for rsvd page */
+		if (_rtl8821ae_dynamic_rqpn(hw, 0xE0, 0x3, 0x80c20d0d))
+			rtlhal->re_init_llt_table = true;
+
+		/* 3 <2> Set Fw releted H2C cmd. */
+
+		/* Set WoWLAN related security information. */
+		rtl8821ae_set_fw_global_info_cmd(hw);
+
+		_rtl8821ae_download_rsvd_page(hw, true);
+
+		/* Just enable AOAC related functions when we connect to AP. */
+		printk("mac->link_state = %d\n", mac->link_state);
+		if (mac->link_state >= MAC80211_LINKED &&
+		    mac->opmode == NL80211_IFTYPE_STATION) {
+			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID, NULL);
+			rtl8821ae_set_fw_media_status_rpt_cmd(hw,
+							      RT_MEDIA_CONNECT);
+
+			rtl8821ae_set_fw_wowlan_mode(hw, true);
+			/* Enable Fw Keep alive mechanism. */
+			rtl8821ae_set_fw_keep_alive_cmd(hw, true);
+
+			/* Enable disconnect decision control. */
+			rtl8821ae_set_fw_disconnect_decision_ctrl_cmd(hw, true);
+		}
+
+		/* 3 <3> Hw Configutations */
+
+		/* Wait untill Rx DMA Finished before host sleep.
+		 * FW Pause Rx DMA may happens when received packet doing dma.
+		 */
+		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, BIT(2));
+
+		tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+		count = 0;
+		while (!(tmp & BIT(1)) && (count++ < 100)) {
+			udelay(10);
+			tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
+		}
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Wait Rx DMA Finished before host sleep. count=%d\n",
+			  count);
+
+		/* reset trx ring */
+		rtlpriv->intf_ops->reset_trx_ring(hw);
+
+		rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, 0x0);
+
+		_rtl8821ae_clear_pci_pme_status(hw);
+		tmp = rtl_read_byte(rtlpriv, REG_SYS_CLKR);
+		rtl_write_byte(rtlpriv, REG_SYS_CLKR, tmp | BIT(3));
+		/* prevent 8051 to be reset by PERST */
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x20);
+		rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x60);
+	}
+
+	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
+	    ppsc->rfoff_reason > RF_CHANGE_BY_PS)
+		rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
+	/* For wowlan+LPS+32k. */
+	if (support_remote_wakeup && rtlhal->enter_pnp_sleep) {
+		/* Set the WoWLAN related function control enable.
+		 * It should be the last H2C cmd in the WoWLAN flow. */
+		rtl8821ae_set_fw_remote_wake_ctrl_cmd(hw, 1);
+
+		/* Stop Pcie Interface Tx DMA. */
+		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0xff);
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Stop PCIE Tx DMA.\n");
+
+		/* Wait for TxDMA idle. */
+		count = 0;
+		do {
+			tmp = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG);
+			udelay(10);
+			count++;
+		} while ((tmp != 0) && (count < 100));
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Wait Tx DMA Finished before host sleep. count=%d\n",
+			  count);
+
+		if (rtlhal->hw_rof_enable) {
+			printk("hw_rof_enable\n");
+			tmp = rtl_read_byte(rtlpriv, REG_HSISR + 3);
+			rtl_write_byte(rtlpriv, REG_HSISR + 3, tmp | BIT(1));
+		}
+	}
+	/* after power off we should do iqk again */
+	rtlpriv->phy.iqk_initialized = false;
+}
+
+void rtl8821ae_interrupt_recognized(struct ieee80211_hw *hw,
+				  u32 *p_inta, u32 *p_intb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	*p_inta = rtl_read_dword(rtlpriv, ISR) & rtlpci->irq_mask[0];
+	rtl_write_dword(rtlpriv, ISR, *p_inta);
+
+	*p_intb = rtl_read_dword(rtlpriv, REG_HISRE) & rtlpci->irq_mask[1];
+	rtl_write_dword(rtlpriv, REG_HISRE, *p_intb);
+}
+
+void rtl8821ae_set_beacon_related_registers(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u16 bcn_interval, atim_window;
+
+	bcn_interval = mac->beacon_interval;
+	atim_window = 2;	/*FIX MERGE */
+	rtl8821ae_disable_interrupt(hw);
+	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660f);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x18);
+	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x18);
+	rtl_write_byte(rtlpriv, 0x606, 0x30);
+	rtlpci->reg_bcn_ctrl_val |= BIT(3);
+	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8)rtlpci->reg_bcn_ctrl_val);
+	rtl8821ae_enable_interrupt(hw);
+}
+
+void rtl8821ae_set_beacon_interval(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u16 bcn_interval = mac->beacon_interval;
+
+	RT_TRACE(rtlpriv, COMP_BEACON, DBG_DMESG,
+		 "beacon_interval:%d\n", bcn_interval);
+	rtl8821ae_disable_interrupt(hw);
+	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
+	rtl8821ae_enable_interrupt(hw);
+}
+
+void rtl8821ae_update_interrupt_mask(struct ieee80211_hw *hw,
+				   u32 add_msr, u32 rm_msr)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	RT_TRACE(rtlpriv, COMP_INTR, DBG_LOUD,
+		 "add_msr:%x, rm_msr:%x\n", add_msr, rm_msr);
+
+	if (add_msr)
+		rtlpci->irq_mask[0] |= add_msr;
+	if (rm_msr)
+		rtlpci->irq_mask[0] &= (~rm_msr);
+	rtl8821ae_disable_interrupt(hw);
+	rtl8821ae_enable_interrupt(hw);
+}
+
+static u8 _rtl8821ae_get_chnl_group(u8 chnl)
+{
+	u8 group = 0;
+
+	if (chnl <= 14) {
+		if (1 <= chnl && chnl <= 2)
+			group = 0;
+	else if (3 <= chnl && chnl <= 5)
+			group = 1;
+	else if (6 <= chnl && chnl <= 8)
+			group = 2;
+	else if (9 <= chnl && chnl <= 11)
+			group = 3;
+	else /*if (12 <= chnl && chnl <= 14)*/
+			group = 4;
+	} else {
+		if (36 <= chnl && chnl <= 42)
+			group = 0;
+	else if (44 <= chnl && chnl <= 48)
+			group = 1;
+	else if (50 <= chnl && chnl <= 58)
+			group = 2;
+	else if (60 <= chnl && chnl <= 64)
+			group = 3;
+	else if (100 <= chnl && chnl <= 106)
+			group = 4;
+	else if (108 <= chnl && chnl <= 114)
+			group = 5;
+	else if (116 <= chnl && chnl <= 122)
+			group = 6;
+	else if (124 <= chnl && chnl <= 130)
+			group = 7;
+	else if (132 <= chnl && chnl <= 138)
+			group = 8;
+	else if (140 <= chnl && chnl <= 144)
+			group = 9;
+	else if (149 <= chnl && chnl <= 155)
+			group = 10;
+	else if (157 <= chnl && chnl <= 161)
+			group = 11;
+	else if (165 <= chnl && chnl <= 171)
+			group = 12;
+	else if (173 <= chnl && chnl <= 177)
+			group = 13;
+		else
+			/*RT_TRACE(rtlpriv, COMP_EFUSE,DBG_LOUD,
+				"5G, Channel %d in Group not found\n",chnl);*/
+			RT_ASSERT(!COMP_EFUSE,
+				"5G, Channel %d in Group not found\n", chnl);
+	}
+	return group;
+}
+
+static void _rtl8821ae_read_power_value_fromprom(struct ieee80211_hw *hw,
+	struct txpower_info_2g *pwrinfo24g,
+	struct txpower_info_5g *pwrinfo5g,
+	bool autoload_fail,
+	u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 rfPath, eeAddr = EEPROM_TX_PWR_INX, group, TxCount = 0;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "hal_ReadPowerValueFromPROM8821ae(): hwinfo[0x%x]=0x%x\n",
+		 (eeAddr+1), hwinfo[eeAddr+1]);
+	if (0xFF == hwinfo[eeAddr+1])  /*YJ,add,120316*/
+		autoload_fail = true;
+
+	if (autoload_fail) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "auto load fail : Use Default value!\n");
+		for (rfPath = 0 ; rfPath < MAX_RF_PATH ; rfPath++) {
+			/*2.4G default value*/
+			for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+				pwrinfo24g->index_cck_base[rfPath][group] =	0x2D;
+				pwrinfo24g->index_bw40_base[rfPath][group] = 0x2D;
+			}
+			for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+				if (TxCount == 0) {
+					pwrinfo24g->bw20_diff[rfPath][0] = 0x02;
+					pwrinfo24g->ofdm_diff[rfPath][0] = 0x04;
+				} else {
+					pwrinfo24g->bw20_diff[rfPath][TxCount] = 0xFE;
+					pwrinfo24g->bw40_diff[rfPath][TxCount] = 0xFE;
+					pwrinfo24g->cck_diff[rfPath][TxCount] =	0xFE;
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] = 0xFE;
+				}
+			}
+			/*5G default value*/
+			for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++)
+				pwrinfo5g->index_bw40_base[rfPath][group] = 0x2A;
+
+			for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+				if (TxCount == 0) {
+					pwrinfo5g->ofdm_diff[rfPath][0] = 0x04;
+					pwrinfo5g->bw20_diff[rfPath][0] = 0x00;
+					pwrinfo5g->bw80_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw160_diff[rfPath][0] = 0xFE;
+				} else {
+					pwrinfo5g->ofdm_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw20_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw40_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw80_diff[rfPath][0] = 0xFE;
+					pwrinfo5g->bw160_diff[rfPath][0] = 0xFE;
+				}
+			}
+		}
+		return;
+	}
+
+	rtl_priv(hw)->efuse.txpwr_fromeprom = true;
+
+	for (rfPath = 0 ; rfPath < MAX_RF_PATH ; rfPath++) {
+		/*2.4G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
+			pwrinfo24g->index_cck_base[rfPath][group] = hwinfo[eeAddr++];
+			if (pwrinfo24g->index_cck_base[rfPath][group] == 0xFF)
+				pwrinfo24g->index_cck_base[rfPath][group] = 0x2D;
+		}
+		for (group = 0 ; group < MAX_CHNL_GROUP_24G - 1; group++) {
+			pwrinfo24g->index_bw40_base[rfPath][group] = hwinfo[eeAddr++];
+			if (pwrinfo24g->index_bw40_base[rfPath][group] == 0xFF)
+				pwrinfo24g->index_bw40_base[rfPath][group] = 0x2D;
+		}
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (TxCount == 0) {
+				pwrinfo24g->bw40_diff[rfPath][TxCount] = 0;
+				/*bit sign number to 8 bit sign number*/
+				pwrinfo24g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0xf0) >> 4;
+				if (pwrinfo24g->bw20_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo24g->bw20_diff[rfPath][TxCount] |= 0xF0;
+				/*bit sign number to 8 bit sign number*/
+				pwrinfo24g->ofdm_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
+				if (pwrinfo24g->ofdm_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+
+				pwrinfo24g->cck_diff[rfPath][TxCount] = 0;
+				eeAddr++;
+			} else {
+				pwrinfo24g->bw40_diff[rfPath][TxCount] = (hwinfo[eeAddr]&0xf0) >> 4;
+				if (pwrinfo24g->bw40_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo24g->bw40_diff[rfPath][TxCount] |= 0xF0;
+
+				pwrinfo24g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
+				if (pwrinfo24g->bw20_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo24g->bw20_diff[rfPath][TxCount] |= 0xF0;
+
+				eeAddr++;
+
+				pwrinfo24g->ofdm_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0xf0) >> 4;
+				if (pwrinfo24g->ofdm_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo24g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+
+				pwrinfo24g->cck_diff[rfPath][TxCount] =	(hwinfo[eeAddr] & 0x0f);
+				if (pwrinfo24g->cck_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo24g->cck_diff[rfPath][TxCount] |= 0xF0;
+
+				eeAddr++;
+			}
+		}
+
+		/*5G default value*/
+		for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++) {
+			pwrinfo5g->index_bw40_base[rfPath][group] = hwinfo[eeAddr++];
+			if (pwrinfo5g->index_bw40_base[rfPath][group] == 0xFF)
+				pwrinfo5g->index_bw40_base[rfPath][group] = 0xFE;
+		}
+
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (TxCount == 0) {
+				pwrinfo5g->bw40_diff[rfPath][TxCount] = 0;
+
+				pwrinfo5g->bw20_diff[rfPath][0] = (hwinfo[eeAddr] & 0xf0) >> 4;
+				if (pwrinfo5g->bw20_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo5g->bw20_diff[rfPath][TxCount] |= 0xF0;
+
+				pwrinfo5g->ofdm_diff[rfPath][0] = (hwinfo[eeAddr] & 0x0f);
+				if (pwrinfo5g->ofdm_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo5g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+
+				eeAddr++;
+			} else {
+				pwrinfo5g->bw40_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0xf0) >> 4;
+				if (pwrinfo5g->bw40_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo5g->bw40_diff[rfPath][TxCount] |= 0xF0;
+
+				pwrinfo5g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
+				if (pwrinfo5g->bw20_diff[rfPath][TxCount] & BIT(3))
+					pwrinfo5g->bw20_diff[rfPath][TxCount] |= 0xF0;
+
+				eeAddr++;
+			}
+		}
+
+		pwrinfo5g->ofdm_diff[rfPath][1] =	(hwinfo[eeAddr] & 0xf0) >> 4;
+		pwrinfo5g->ofdm_diff[rfPath][2] =	(hwinfo[eeAddr] & 0x0f);
+
+		eeAddr++;
+
+		pwrinfo5g->ofdm_diff[rfPath][3] = (hwinfo[eeAddr] & 0x0f);
+
+		eeAddr++;
+
+		for (TxCount = 1; TxCount < MAX_TX_COUNT; TxCount++) {
+			if (pwrinfo5g->ofdm_diff[rfPath][TxCount] & BIT(3))
+				pwrinfo5g->ofdm_diff[rfPath][TxCount] |= 0xF0;
+		}
+		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
+			pwrinfo5g->bw80_diff[rfPath][TxCount] =	(hwinfo[eeAddr] & 0xf0) >> 4;
+			/* 4bit sign number to 8 bit sign number */
+			if (pwrinfo5g->bw80_diff[rfPath][TxCount] & BIT(3))
+				pwrinfo5g->bw80_diff[rfPath][TxCount] |= 0xF0;
+			/* 4bit sign number to 8 bit sign number */
+			pwrinfo5g->bw160_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
+			if (pwrinfo5g->bw160_diff[rfPath][TxCount] & BIT(3))
+				pwrinfo5g->bw160_diff[rfPath][TxCount] |= 0xF0;
+
+			eeAddr++;
+		}
+	}
+}
+#if 0
+static void _rtl8812ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail,
+						 u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pwrinfo24g;
+	struct txpower_info_5g pwrinfo5g;
+	u8 channel5g[CHANNEL_MAX_NUMBER_5G] = {
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54,
+		56, 58, 60, 62, 64, 100, 102, 104, 106,
+		108, 110, 112, 114, 116, 118, 120, 122,
+		124, 126, 128, 130, 132, 134, 136, 138,
+		140, 142, 144, 149, 151, 153, 155, 157,
+		159, 161, 163, 165, 167, 168, 169, 171, 173, 175, 177};
+	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
+	u8 rf_path, index;
+	u8 i;
+
+	_rtl8821ae_read_power_value_fromprom(hw, &pwrinfo24g,
+					&pwrinfo5g, autoload_fail, hwinfo);
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < CHANNEL_MAX_NUMBER_2G; i++) {
+			index = _rtl8821ae_get_chnl_group(i + 1);
+
+			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
+				rtlefuse->txpwrlevel_cck[rf_path][i] =
+					pwrinfo24g.index_cck_base[rf_path][5];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+					pwrinfo24g.index_bw40_base[rf_path][index];
+			} else {
+				rtlefuse->txpwrlevel_cck[rf_path][i] =
+					pwrinfo24g.index_cck_base[rf_path][index];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+					pwrinfo24g.index_bw40_base[rf_path][index];
+			}
+		}
+
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
+			index = _rtl8821ae_get_chnl_group(channel5g[i]);
+			rtlefuse->txpwr_5g_bw40base[rf_path][i] =
+					pwrinfo5g.index_bw40_base[rf_path][index];
+		}
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
+			u8 upper, lower;
+			index = _rtl8821ae_get_chnl_group(channel5g_80m[i]);
+			upper = pwrinfo5g.index_bw40_base[rf_path][index];
+			lower = pwrinfo5g.index_bw40_base[rf_path][index + 1];
+
+			rtlefuse->txpwr_5g_bw80base[rf_path][i] = (upper + lower) / 2;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			rtlefuse->txpwr_cckdiff[rf_path][i] =
+				pwrinfo24g.cck_diff[rf_path][i];
+			rtlefuse->txpwr_legacyhtdiff[rf_path][i] =
+				pwrinfo24g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_ht20diff[rf_path][i] =
+				pwrinfo24g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_ht40diff[rf_path][i] =
+				pwrinfo24g.bw40_diff[rf_path][i];
+
+			rtlefuse->txpwr_5g_ofdmdiff[rf_path][i] =
+				pwrinfo5g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw20diff[rf_path][i] =
+				pwrinfo5g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw40diff[rf_path][i] =
+				pwrinfo5g.bw40_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw80diff[rf_path][i] =
+				pwrinfo5g.bw80_diff[rf_path][i];
+		}
+	}
+
+	if (!autoload_fail) {
+		rtlefuse->eeprom_regulatory =
+			hwinfo[EEPROM_RF_BOARD_OPTION] & 0x07;/*bit0~2*/
+		if (hwinfo[EEPROM_RF_BOARD_OPTION] == 0xFF)
+			rtlefuse->eeprom_regulatory = 0;
+	} else {
+		rtlefuse->eeprom_regulatory = 0;
+	}
+
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
+	"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
+}
+#endif
+static void _rtl8821ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
+						 bool autoload_fail,
+						 u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct txpower_info_2g pwrinfo24g;
+	struct txpower_info_5g pwrinfo5g;
+	u8 channel5g[CHANNEL_MAX_NUMBER_5G] = {
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54,
+		56, 58, 60, 62, 64, 100, 102, 104, 106,
+		108, 110, 112, 114, 116, 118, 120, 122,
+		124, 126, 128, 130, 132, 134, 136, 138,
+		140, 142, 144, 149, 151, 153, 155, 157,
+		159, 161, 163, 165, 167, 168, 169, 171,
+		173, 175, 177};
+	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {
+		42, 58, 106, 122, 138, 155, 171};
+	u8 rf_path, index;
+	u8 i;
+
+	_rtl8821ae_read_power_value_fromprom(hw, &pwrinfo24g,
+		&pwrinfo5g, autoload_fail, hwinfo);
+
+	for (rf_path = 0; rf_path < 2; rf_path++) {
+		for (i = 0; i < CHANNEL_MAX_NUMBER_2G; i++) {
+			index = _rtl8821ae_get_chnl_group(i + 1);
+
+			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
+				rtlefuse->txpwrlevel_cck[rf_path][i] =
+					pwrinfo24g.index_cck_base[rf_path][5];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+					pwrinfo24g.index_bw40_base[rf_path][index];
+			} else {
+				rtlefuse->txpwrlevel_cck[rf_path][i] =
+					pwrinfo24g.index_cck_base[rf_path][index];
+				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
+					pwrinfo24g.index_bw40_base[rf_path][index];
+			}
+		}
+
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
+			index = _rtl8821ae_get_chnl_group(channel5g[i]);
+			rtlefuse->txpwr_5g_bw40base[rf_path][i] =
+				pwrinfo5g.index_bw40_base[rf_path][index];
+		}
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
+			u8 upper, lower;
+			index = _rtl8821ae_get_chnl_group(channel5g_80m[i]);
+			upper = pwrinfo5g.index_bw40_base[rf_path][index];
+			lower = pwrinfo5g.index_bw40_base[rf_path][index + 1];
+
+			rtlefuse->txpwr_5g_bw80base[rf_path][i] = (upper + lower) / 2;
+		}
+		for (i = 0; i < MAX_TX_COUNT; i++) {
+			rtlefuse->txpwr_cckdiff[rf_path][i] =
+				pwrinfo24g.cck_diff[rf_path][i];
+			rtlefuse->txpwr_legacyhtdiff[rf_path][i] =
+				pwrinfo24g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_ht20diff[rf_path][i] =
+				pwrinfo24g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_ht40diff[rf_path][i] =
+				pwrinfo24g.bw40_diff[rf_path][i];
+
+			rtlefuse->txpwr_5g_ofdmdiff[rf_path][i] =
+				pwrinfo5g.ofdm_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw20diff[rf_path][i] =
+				pwrinfo5g.bw20_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw40diff[rf_path][i] =
+				pwrinfo5g.bw40_diff[rf_path][i];
+			rtlefuse->txpwr_5g_bw80diff[rf_path][i] =
+				pwrinfo5g.bw80_diff[rf_path][i];
+		}
+	}
+	/*bit0~2*/
+	if (!autoload_fail) {
+		rtlefuse->eeprom_regulatory = hwinfo[EEPROM_RF_BOARD_OPTION] & 0x07;
+		if (hwinfo[EEPROM_RF_BOARD_OPTION] == 0xFF)
+			rtlefuse->eeprom_regulatory = 0;
+	} else {
+		rtlefuse->eeprom_regulatory = 0;
+	}
+
+	RTPRINT(rtlpriv, FINIT, INIT_TXPOWER,
+	"eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory);
+}
+
+static void _rtl8812ae_read_pa_type(struct ieee80211_hw *hw, u8 *hwinfo,
+				    bool autoload_fail)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!autoload_fail) {
+		rtlhal->pa_type_2g = hwinfo[0xBC];
+		rtlhal->lna_type_2g = hwinfo[0xBD];
+		if (rtlhal->pa_type_2g == 0xFF && rtlhal->lna_type_2g == 0xFF) {
+			rtlhal->pa_type_2g = 0;
+			rtlhal->lna_type_2g = 0;
+		}
+		rtlhal->external_pa_2g = ((rtlhal->pa_type_2g & BIT(5)) &&
+					  (rtlhal->pa_type_2g & BIT(4))) ?
+					 1 : 0;
+		rtlhal->external_lna_2g = ((rtlhal->lna_type_2g & BIT(7)) &&
+					   (rtlhal->lna_type_2g & BIT(3))) ?
+					  1 : 0;
+
+		rtlhal->pa_type_5g = hwinfo[0xBC];
+		rtlhal->lna_type_5g = hwinfo[0xBF];
+		if (rtlhal->pa_type_5g == 0xFF && rtlhal->lna_type_5g == 0xFF) {
+			rtlhal->pa_type_5g = 0;
+			rtlhal->lna_type_5g = 0;
+		}
+		rtlhal->external_pa_5g = ((rtlhal->pa_type_5g & BIT(1)) &&
+					  (rtlhal->pa_type_5g & BIT(0))) ?
+					 1 : 0;
+		rtlhal->external_lna_5g = ((rtlhal->lna_type_5g & BIT(7)) &&
+					   (rtlhal->lna_type_5g & BIT(3))) ?
+					  1 : 0;
+	} else {
+		rtlhal->external_pa_2g  = 0;
+		rtlhal->external_lna_2g = 0;
+		rtlhal->external_pa_5g  = 0;
+		rtlhal->external_lna_5g = 0;
+	}
+}
+
+static void _rtl8821ae_read_pa_type(struct ieee80211_hw *hw, u8 *hwinfo,
+				    bool autoload_fail)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!autoload_fail) {
+		rtlhal->pa_type_2g = hwinfo[0xBC];
+		rtlhal->lna_type_2g = hwinfo[0xBD];
+		if (rtlhal->pa_type_2g == 0xFF && rtlhal->lna_type_2g == 0xFF) {
+			rtlhal->pa_type_2g = 0;
+			rtlhal->lna_type_2g = 0;
+		}
+		rtlhal->external_pa_2g = (rtlhal->pa_type_2g & BIT(5)) ? 1 : 0;
+		rtlhal->external_lna_2g = (rtlhal->lna_type_2g & BIT(7)) ? 1 : 0;
+
+		rtlhal->pa_type_5g = hwinfo[0xBC];
+		rtlhal->lna_type_5g = hwinfo[0xBF];
+		if (rtlhal->pa_type_5g == 0xFF && rtlhal->lna_type_5g == 0xFF) {
+			rtlhal->pa_type_5g = 0;
+			rtlhal->lna_type_5g = 0;
+		}
+		rtlhal->external_pa_5g = (rtlhal->pa_type_5g & BIT(1)) ? 1 : 0;
+		rtlhal->external_lna_5g = (rtlhal->lna_type_5g & BIT(7)) ? 1 : 0;
+	} else {
+		rtlhal->external_pa_2g  = 0;
+		rtlhal->external_lna_2g = 0;
+		rtlhal->external_pa_5g  = 0;
+		rtlhal->external_lna_5g = 0;
+	}
+}
+
+static void _rtl8821ae_read_rfe_type(struct ieee80211_hw *hw, u8 *hwinfo,
+			      bool autoload_fail)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+
+	if (!autoload_fail) {
+		if (hwinfo[EEPROM_RFE_OPTION] & BIT(7)) {
+			if (rtlhal->external_lna_5g) {
+				if (rtlhal->external_pa_5g) {
+					if (rtlhal->external_lna_2g &&
+					    rtlhal->external_pa_2g)
+						rtlhal->rfe_type = 3;
+					else
+						rtlhal->rfe_type = 0;
+				} else {
+					rtlhal->rfe_type = 2;
+				}
+			} else {
+				rtlhal->rfe_type = 4;
+			}
+		} else {
+			rtlhal->rfe_type = hwinfo[EEPROM_RFE_OPTION] & 0x3F;
+
+			if (rtlhal->rfe_type == 4 &&
+			    (rtlhal->external_pa_5g ||
+			     rtlhal->external_pa_2g ||
+			     rtlhal->external_lna_5g ||
+			     rtlhal->external_lna_2g)) {
+				if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+					rtlhal->rfe_type = 2;
+			}
+		}
+	} else {
+		rtlhal->rfe_type = 0x04;
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "RFE Type: 0x%2x\n", rtlhal->rfe_type);
+}
+
+static void _rtl8812ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool auto_load_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value;
+
+	if (!auto_load_fail) {
+		value = *(u8 *)&hwinfo[EEPROM_RF_BOARD_OPTION];
+		if (((value & 0xe0) >> 5) == 0x1)
+			rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		else
+			rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8812A;
+
+		value = hwinfo[EEPROM_RF_BT_SETTING];
+		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
+	} else {
+		rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8812A;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
+	}
+	/*move BT_InitHalVars() to init_sw_vars*/
+}
+
+static void _rtl8821ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
+					      bool auto_load_fail, u8 *hwinfo)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 value;
+	u32 tmpu_32;
+
+	if (!auto_load_fail) {
+		tmpu_32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
+		if (tmpu_32 & BIT(18))
+			rtlpriv->btcoexist.btc_info.btcoexist = 1;
+		else
+			rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8821A;
+
+		value = hwinfo[EEPROM_RF_BT_SETTING];
+		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
+	} else {
+		rtlpriv->btcoexist.btc_info.btcoexist = 0;
+		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8821A;
+		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
+	}
+	/*move BT_InitHalVars() to init_sw_vars*/
+}
+
+static void _rtl8821ae_read_adapter_info(struct ieee80211_hw *hw, bool b_pseudo_test)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	u16 i, usvalue;
+	u8 hwinfo[HWSET_MAX_SIZE];
+	u16 eeprom_id;
+
+	if (b_pseudo_test) {
+		;/* need add */
+	}
+
+	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
+		rtl_efuse_shadow_map_update(hw);
+		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
+		       HWSET_MAX_SIZE);
+	} else if (rtlefuse->epromtype == EEPROM_93C46) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "RTL819X Not boot from eeprom, check it !!");
+	}
+
+	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "MAP\n",
+		      hwinfo, HWSET_MAX_SIZE);
+
+	eeprom_id = *((u16 *)&hwinfo[0]);
+	if (eeprom_id != RTL_EEPROM_ID) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "EEPROM ID(%#x) is invalid!!\n", eeprom_id);
+		rtlefuse->autoload_failflag = true;
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
+		rtlefuse->autoload_failflag = false;
+	}
+
+	if (rtlefuse->autoload_failflag) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "RTL8812AE autoload_failflag, check it !!");
+		return;
+	}
+
+	rtlefuse->eeprom_version = *(u8 *)&hwinfo[EEPROM_VERSION];
+	if (rtlefuse->eeprom_version == 0xff)
+			rtlefuse->eeprom_version = 0;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM version: 0x%2x\n", rtlefuse->eeprom_version);
+
+	rtlefuse->eeprom_vid = *(u16 *)&hwinfo[EEPROM_VID];
+	rtlefuse->eeprom_did = *(u16 *)&hwinfo[EEPROM_DID];
+	rtlefuse->eeprom_svid = *(u16 *)&hwinfo[EEPROM_SVID];
+	rtlefuse->eeprom_smid = *(u16 *)&hwinfo[EEPROM_SMID];
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROMId = 0x%4x\n", eeprom_id);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid);
+
+	/*customer ID*/
+	rtlefuse->eeprom_oemid = *(u8 *)&hwinfo[EEPROM_CUSTOMER_ID];
+	if (rtlefuse->eeprom_oemid == 0xFF)
+		rtlefuse->eeprom_oemid = 0;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid);
+
+	for (i = 0; i < 6; i += 2) {
+		usvalue = *(u16 *)&hwinfo[EEPROM_MAC_ADDR + i];
+		*((u16 *)(&rtlefuse->dev_addr[i])) = usvalue;
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "dev_addr: %pM\n", rtlefuse->dev_addr);
+
+	_rtl8821ae_read_txpower_info_from_hwpg(hw, rtlefuse->autoload_failflag,
+					       hwinfo);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		_rtl8812ae_read_pa_type(hw, hwinfo, rtlefuse->autoload_failflag);
+		_rtl8812ae_read_bt_coexist_info_from_hwpg(hw,
+				rtlefuse->autoload_failflag, hwinfo);
+	} else {
+		_rtl8821ae_read_pa_type(hw, hwinfo, rtlefuse->autoload_failflag);
+		_rtl8821ae_read_bt_coexist_info_from_hwpg(hw,
+				rtlefuse->autoload_failflag, hwinfo);
+	}
+
+	_rtl8821ae_read_rfe_type(hw, hwinfo, rtlefuse->autoload_failflag);
+	/*board type*/
+	rtlefuse->board_type = ODM_BOARD_DEFAULT;
+	if (rtlhal->external_lna_2g != 0)
+		rtlefuse->board_type |= ODM_BOARD_EXT_LNA;
+	if (rtlhal->external_lna_5g != 0)
+		rtlefuse->board_type |= ODM_BOARD_EXT_LNA_5G;
+	if (rtlhal->external_pa_2g != 0)
+		rtlefuse->board_type |= ODM_BOARD_EXT_PA;
+	if (rtlhal->external_pa_5g != 0)
+		rtlefuse->board_type |= ODM_BOARD_EXT_PA_5G;
+
+	if (rtlpriv->btcoexist.btc_info.btcoexist == 1)
+		rtlefuse->board_type |= ODM_BOARD_BT;
+
+	rtlhal->board_type = rtlefuse->board_type;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "board_type = 0x%x\n", rtlefuse->board_type);
+
+	rtlefuse->eeprom_channelplan = *(u8 *)&hwinfo[EEPROM_CHANNELPLAN];
+	if (rtlefuse->eeprom_channelplan == 0xff)
+		rtlefuse->eeprom_channelplan = 0x7F;
+
+	/* set channel paln to world wide 13 */
+	/* rtlefuse->channel_plan = (u8)rtlefuse->eeprom_channelplan; */
+
+	/*parse xtal*/
+	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_8821AE];
+	if (rtlefuse->crystalcap == 0xFF)
+		rtlefuse->crystalcap = 0x20;
+
+	rtlefuse->eeprom_thermalmeter = *(u8 *)&hwinfo[EEPROM_THERMAL_METER];
+	if ((rtlefuse->eeprom_thermalmeter == 0xff) ||
+	    rtlefuse->autoload_failflag) {
+		rtlefuse->apk_thermalmeterignore = true;
+		rtlefuse->eeprom_thermalmeter = 0xff;
+	}
+
+	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter);
+
+	if (!rtlefuse->autoload_failflag) {
+		rtlefuse->antenna_div_cfg =
+		  (hwinfo[EEPROM_RF_BOARD_OPTION] & 0x18) >> 3;
+		if (hwinfo[EEPROM_RF_BOARD_OPTION] == 0xff)
+			rtlefuse->antenna_div_cfg = 0;
+
+		if (rtlpriv->btcoexist.btc_info.btcoexist == 1 &&
+		    rtlpriv->btcoexist.btc_info.ant_num == ANT_X1)
+			rtlefuse->antenna_div_cfg = 0;
+
+		rtlefuse->antenna_div_type = hwinfo[EEPROM_RF_ANTENNA_OPT_88E];
+		if (rtlefuse->antenna_div_type == 0xff)
+			rtlefuse->antenna_div_type = FIXED_HW_ANTDIV;
+	} else {
+		rtlefuse->antenna_div_cfg = 0;
+		rtlefuse->antenna_div_type = 0;
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		"SWAS: bHwAntDiv = %x, TRxAntDivType = %x\n",
+		rtlefuse->antenna_div_cfg, rtlefuse->antenna_div_type);
+
+	pcipriv->ledctl.led_opendrain = true;
+
+	if (rtlhal->oem_id == RT_CID_DEFAULT) {
+		switch (rtlefuse->eeprom_oemid) {
+		case RT_CID_DEFAULT:
+			break;
+		case EEPROM_CID_TOSHIBA:
+			rtlhal->oem_id = RT_CID_TOSHIBA;
+			break;
+		case EEPROM_CID_CCX:
+			rtlhal->oem_id = RT_CID_CCX;
+			break;
+		case EEPROM_CID_QMI:
+			rtlhal->oem_id = RT_CID_819X_QMI;
+			break;
+		case EEPROM_CID_WHQL:
+			break;
+		default:
+			break;
+		}
+	}
+}
+
+/*static void _rtl8821ae_hal_customized_behavior(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	pcipriv->ledctl.led_opendrain = true;
+	switch (rtlhal->oem_id) {
+	case RT_CID_819X_HP:
+		pcipriv->ledctl.led_opendrain = true;
+		break;
+	case RT_CID_819X_LENOVO:
+	case RT_CID_DEFAULT:
+	case RT_CID_TOSHIBA:
+	case RT_CID_CCX:
+	case RT_CID_819X_ACER:
+	case RT_CID_WHQL:
+	default:
+		break;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG,
+		 "RT Customized ID: 0x%02X\n", rtlhal->oem_id);
+}*/
+
+void rtl8821ae_read_eeprom_info(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_u1b;
+
+	rtlhal->version = _rtl8821ae_read_chip_version(hw);
+	if (get_rf_type(rtlphy) == RF_1T1R)
+		rtlpriv->dm.rfpath_rxenable[0] = true;
+	else
+		rtlpriv->dm.rfpath_rxenable[0] =
+		    rtlpriv->dm.rfpath_rxenable[1] = true;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "VersionID = 0x%4x\n",
+						rtlhal->version);
+
+	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
+	if (tmp_u1b & BIT(4)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EEPROM\n");
+		rtlefuse->epromtype = EEPROM_93C46;
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_DMESG, "Boot from EFUSE\n");
+		rtlefuse->epromtype = EEPROM_BOOT_EFUSE;
+	}
+
+	if (tmp_u1b & BIT(5)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Autoload OK\n");
+		rtlefuse->autoload_failflag = false;
+		_rtl8821ae_read_adapter_info(hw, false);
+	} else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Autoload ERR!!\n");
+	}
+	/*hal_ReadRFType_8812A()*/
+	/* _rtl8821ae_hal_customized_behavior(hw); */
+}
+
+static void rtl8821ae_update_hal_rate_table(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 ratr_value;
+	u8 ratr_index = 0;
+	u8 b_nmode = mac->ht_enable;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	u16 shortgi_rate;
+	u32 tmp_ratr_value;
+	u8 curtxbw_40mhz = mac->bw_40;
+	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+				1 : 0;
+	u8 b_curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+				1 : 0;
+	enum wireless_mode wirelessmode = mac->mode;
+
+	if (rtlhal->current_bandtype == BAND_ON_5G)
+		ratr_value = sta->supp_rates[1] << 4;
+	else
+		ratr_value = sta->supp_rates[0];
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_value = 0xfff;
+	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+			sta->ht_cap.mcs.rx_mask[0] << 12);
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		if (ratr_value & 0x0000000c)
+			ratr_value &= 0x0000000d;
+		else
+			ratr_value &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_value &= 0x00000FF5;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		b_nmode = 1;
+		if (mimo_ps == IEEE80211_SMPS_STATIC) {
+			ratr_value &= 0x0007F005;
+		} else {
+			u32 ratr_mask;
+
+			if (get_rf_type(rtlphy) == RF_1T2R ||
+			    get_rf_type(rtlphy) == RF_1T1R)
+				ratr_mask = 0x000ff005;
+			else
+				ratr_mask = 0x0f0ff005;
+
+			ratr_value &= ratr_mask;
+		}
+		break;
+	default:
+		if (rtlphy->rf_type == RF_1T2R)
+			ratr_value &= 0x000ff0ff;
+		else
+			ratr_value &= 0x0f0ff0ff;
+
+		break;
+	}
+
+	if ((rtlpriv->btcoexist.bt_coexistence) &&
+	     (rtlpriv->btcoexist.bt_coexist_type == BT_CSR_BC4) &&
+	     (rtlpriv->btcoexist.bt_cur_state) &&
+	     (rtlpriv->btcoexist.bt_ant_isolation) &&
+	     ((rtlpriv->btcoexist.bt_service == BT_SCO) ||
+	     (rtlpriv->btcoexist.bt_service == BT_BUSY)))
+		ratr_value &= 0x0fffcfc0;
+	else
+		ratr_value &= 0x0FFFFFFF;
+
+	if (b_nmode && ((curtxbw_40mhz &&
+			 b_curshortgi_40mhz) || (!curtxbw_40mhz &&
+						 b_curshortgi_20mhz))) {
+		ratr_value |= 0x10000000;
+		tmp_ratr_value = (ratr_value >> 12);
+
+		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
+			if ((1 << shortgi_rate) & tmp_ratr_value)
+				break;
+		}
+
+		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
+		    (shortgi_rate << 4) | (shortgi_rate);
+	}
+
+	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "%x\n", rtl_read_dword(rtlpriv, REG_ARFR0));
+}
+
+static u8 _rtl8821ae_mrate_idx_to_arfr_id(
+	struct ieee80211_hw *hw, u8 rate_index,
+	enum wireless_mode wirelessmode)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 ret = 0;
+	switch (rate_index) {
+	case RATR_INX_WIRELESS_NGB:
+		if (rtlphy->rf_type == RF_1T1R)
+			ret = 1;
+		else
+			ret = 0;
+		; break;
+	case RATR_INX_WIRELESS_N:
+	case RATR_INX_WIRELESS_NG:
+		if (rtlphy->rf_type == RF_1T1R)
+			ret = 5;
+		else
+			ret = 4;
+		; break;
+	case RATR_INX_WIRELESS_NB:
+		if (rtlphy->rf_type == RF_1T1R)
+			ret = 3;
+		else
+			ret = 2;
+		; break;
+	case RATR_INX_WIRELESS_GB:
+		ret = 6;
+		break;
+	case RATR_INX_WIRELESS_G:
+		ret = 7;
+		break;
+	case RATR_INX_WIRELESS_B:
+		ret = 8;
+		break;
+	case RATR_INX_WIRELESS_MC:
+		if ((wirelessmode == WIRELESS_MODE_B)
+			|| (wirelessmode == WIRELESS_MODE_G)
+			|| (wirelessmode == WIRELESS_MODE_N_24G)
+			|| (wirelessmode == WIRELESS_MODE_AC_24G))
+			ret = 6;
+		else
+			ret = 7;
+	case RATR_INX_WIRELESS_AC_5N:
+		if (rtlphy->rf_type == RF_1T1R)
+			ret = 10;
+		else
+			ret = 9;
+		break;
+	case RATR_INX_WIRELESS_AC_24N:
+		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80) {
+			if (rtlphy->rf_type == RF_1T1R)
+				ret = 10;
+			else
+				ret = 9;
+		} else {
+			if (rtlphy->rf_type == RF_1T1R)
+				ret = 11;
+			else
+				ret = 12;
+		}
+		break;
+	default:
+		ret = 0; break;
+	}
+	return ret;
+}
+
+static u32 _rtl8821ae_rate_to_bitmap_2ssvht(__le16 vht_rate)
+{
+	u8 i, j, tmp_rate;
+	u32 rate_bitmap = 0;
+
+	for (i = j = 0; i < 4; i += 2, j += 10) {
+		tmp_rate = (le16_to_cpu(vht_rate) >> i) & 3;
+
+		switch (tmp_rate) {
+		case 2:
+			rate_bitmap = rate_bitmap | (0x03ff << j);
+			break;
+		case 1:
+			rate_bitmap = rate_bitmap | (0x01ff << j);
+			break;
+		case 0:
+			rate_bitmap = rate_bitmap | (0x00ff << j);
+			break;
+		default:
+			break;
+		}
+	}
+
+	return rate_bitmap;
+}
+
+static u32 _rtl8821ae_set_ra_vht_ratr_bitmap(struct ieee80211_hw *hw,
+					     enum wireless_mode wirelessmode,
+					     u32 ratr_bitmap)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u32 ret_bitmap = ratr_bitmap;
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40
+		|| rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
+		ret_bitmap = ratr_bitmap;
+	else if (wirelessmode == WIRELESS_MODE_AC_5G
+		|| wirelessmode == WIRELESS_MODE_AC_24G) {
+		if (rtlphy->rf_type == RF_1T1R)
+			ret_bitmap = ratr_bitmap & (~BIT21);
+		else
+			ret_bitmap = ratr_bitmap & (~(BIT31|BIT21));
+	}
+
+	return ret_bitmap;
+}
+
+static u8 _rtl8821ae_get_vht_eni(enum wireless_mode wirelessmode,
+			u32 ratr_bitmap)
+{
+	u8 ret = 0;
+	if (wirelessmode < WIRELESS_MODE_N_24G)
+		ret =  0;
+	else if (wirelessmode == WIRELESS_MODE_AC_24G) {
+		if (ratr_bitmap & 0xfff00000)	/* Mix , 2SS */
+			ret = 3;
+		else					/* Mix, 1SS */
+			ret = 2;
+	} else if (wirelessmode == WIRELESS_MODE_AC_5G) {
+			ret = 1;
+	} /* VHT */
+
+	return ret << 4;
+}
+
+static u8 _rtl8821ae_get_ra_ldpc(struct ieee80211_hw *hw,
+			     u8 mac_id, struct rtl_sta_info *sta_entry,
+			     enum wireless_mode wirelessmode)
+{
+	u8 b_ldpc = 0;
+	/*not support ldpc, do not open*/
+	return b_ldpc << 2;
+}
+
+static u8 _rtl8821ae_get_ra_rftype(struct ieee80211_hw *hw,
+			  enum wireless_mode wirelessmode,
+			  u32 ratr_bitmap)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 rf_type = RF_1T1R;
+
+	if (rtlphy->rf_type == RF_1T1R)
+		rf_type = RF_1T1R;
+	else if (wirelessmode == WIRELESS_MODE_AC_5G
+		|| wirelessmode == WIRELESS_MODE_AC_24G
+		|| wirelessmode == WIRELESS_MODE_AC_ONLY) {
+		if (ratr_bitmap & 0xffc00000)
+			rf_type = RF_2T2R;
+	} else if (wirelessmode == WIRELESS_MODE_N_5G
+		|| wirelessmode == WIRELESS_MODE_N_24G) {
+		if (ratr_bitmap & 0xfff00000)
+			rf_type = RF_2T2R;
+	}
+
+	return rf_type;
+}
+
+static bool _rtl8821ae_get_ra_shortgi(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
+			      u8 mac_id)
+{
+	bool b_short_gi = false;
+	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
+				1 : 0;
+	u8 b_curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
+				1 : 0;
+	u8 b_curshortgi_80mhz = 0;
+	b_curshortgi_80mhz = (sta->vht_cap.cap &
+			      IEEE80211_VHT_CAP_SHORT_GI_80) ? 1 : 0;
+
+	if (mac_id == MAC_ID_STATIC_FOR_BROADCAST_MULTICAST)
+			b_short_gi = false;
+
+	if (b_curshortgi_40mhz || b_curshortgi_80mhz
+		|| b_curshortgi_20mhz)
+		b_short_gi = true;
+
+	return b_short_gi;
+}
+
+static void rtl8821ae_update_hal_rate_mask(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta, u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_sta_info *sta_entry = NULL;
+	u32 ratr_bitmap;
+	u8 ratr_index;
+	enum wireless_mode wirelessmode = 0;
+	u8 curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
+				? 1 : 0;
+	bool b_shortgi = false;
+	u8 rate_mask[7];
+	u8 macid = 0;
+	u8 mimo_ps = IEEE80211_SMPS_OFF;
+	u8 rf_type;
+
+	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
+	wirelessmode = sta_entry->wireless_mode;
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_LOUD,
+		 "wireless mode = 0x%x\n", wirelessmode);
+	if (mac->opmode == NL80211_IFTYPE_STATION ||
+		mac->opmode == NL80211_IFTYPE_MESH_POINT) {
+		curtxbw_40mhz = mac->bw_40;
+	} else if (mac->opmode == NL80211_IFTYPE_AP ||
+		mac->opmode == NL80211_IFTYPE_ADHOC)
+		macid = sta->aid + 1;
+	if (wirelessmode == WIRELESS_MODE_N_5G ||
+	    wirelessmode == WIRELESS_MODE_AC_5G)
+		ratr_bitmap = sta->supp_rates[NL80211_BAND_5GHZ];
+	else
+		ratr_bitmap = sta->supp_rates[NL80211_BAND_2GHZ];
+
+	if (mac->opmode == NL80211_IFTYPE_ADHOC)
+		ratr_bitmap = 0xfff;
+
+	if (wirelessmode == WIRELESS_MODE_N_24G
+		|| wirelessmode == WIRELESS_MODE_N_5G)
+		ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
+				sta->ht_cap.mcs.rx_mask[0] << 12);
+	else if (wirelessmode == WIRELESS_MODE_AC_24G
+		|| wirelessmode == WIRELESS_MODE_AC_5G
+		|| wirelessmode == WIRELESS_MODE_AC_ONLY)
+		ratr_bitmap |= _rtl8821ae_rate_to_bitmap_2ssvht(
+				sta->vht_cap.vht_mcs.rx_mcs_map) << 12;
+
+	b_shortgi = _rtl8821ae_get_ra_shortgi(hw, sta, macid);
+	rf_type = _rtl8821ae_get_ra_rftype(hw, wirelessmode, ratr_bitmap);
+
+/*mac id owner*/
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		ratr_index = RATR_INX_WIRELESS_B;
+		if (ratr_bitmap & 0x0000000c)
+			ratr_bitmap &= 0x0000000d;
+		else
+			ratr_bitmap &= 0x0000000f;
+		break;
+	case WIRELESS_MODE_G:
+		ratr_index = RATR_INX_WIRELESS_GB;
+
+		if (rssi_level == 1)
+			ratr_bitmap &= 0x00000f00;
+		else if (rssi_level == 2)
+			ratr_bitmap &= 0x00000ff0;
+		else
+			ratr_bitmap &= 0x00000ff5;
+		break;
+	case WIRELESS_MODE_A:
+		ratr_index = RATR_INX_WIRELESS_G;
+		ratr_bitmap &= 0x00000ff0;
+		break;
+	case WIRELESS_MODE_N_24G:
+	case WIRELESS_MODE_N_5G:
+		if (wirelessmode == WIRELESS_MODE_N_24G)
+			ratr_index = RATR_INX_WIRELESS_NGB;
+		else
+			ratr_index = RATR_INX_WIRELESS_NG;
+
+		if (mimo_ps == IEEE80211_SMPS_STATIC
+			|| mimo_ps == IEEE80211_SMPS_DYNAMIC) {
+			if (rssi_level == 1)
+				ratr_bitmap &= 0x000f0000;
+			else if (rssi_level == 2)
+				ratr_bitmap &= 0x000ff000;
+			else
+				ratr_bitmap &= 0x000ff005;
+		} else {
+			if (rf_type == RF_1T1R) {
+				if (curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x000f0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x000ff000;
+					else
+						ratr_bitmap &= 0x000ff005;
+				}
+			} else {
+				if (curtxbw_40mhz) {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0fff0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0ffff000;
+					else
+						ratr_bitmap &= 0x0ffff015;
+				} else {
+					if (rssi_level == 1)
+						ratr_bitmap &= 0x0fff0000;
+					else if (rssi_level == 2)
+						ratr_bitmap &= 0x0ffff000;
+					else
+						ratr_bitmap &= 0x0ffff005;
+				}
+			}
+		}
+		break;
+
+	case WIRELESS_MODE_AC_24G:
+		ratr_index = RATR_INX_WIRELESS_AC_24N;
+		if (rssi_level == 1)
+			ratr_bitmap &= 0xfc3f0000;
+		else if (rssi_level == 2)
+			ratr_bitmap &= 0xfffff000;
+		else
+			ratr_bitmap &= 0xffffffff;
+		break;
+
+	case WIRELESS_MODE_AC_5G:
+		ratr_index = RATR_INX_WIRELESS_AC_5N;
+
+		if (rf_type == RF_1T1R) {
+			if (rssi_level == 1)	/*add by Gary for ac-series*/
+				ratr_bitmap &= 0x003f8000;
+			else if (rssi_level == 2)
+				ratr_bitmap &= 0x003ff000;
+			else
+				ratr_bitmap &= 0x003ff010;
+		} else {
+			if (rssi_level == 1)
+				ratr_bitmap &= 0xfe3f8000;
+			else if (rssi_level == 2)
+				ratr_bitmap &= 0xfffff000;
+			else
+				ratr_bitmap &= 0xfffff010;
+		}
+		break;
+
+	default:
+		ratr_index = RATR_INX_WIRELESS_NGB;
+
+		if (rf_type == RF_1T2R)
+			ratr_bitmap &= 0x000ff0ff;
+		else
+			ratr_bitmap &= 0x0f8ff0ff;
+		break;
+	}
+
+	ratr_index = _rtl8821ae_mrate_idx_to_arfr_id(hw, ratr_index, wirelessmode);
+	sta_entry->ratr_index = ratr_index;
+	ratr_bitmap = _rtl8821ae_set_ra_vht_ratr_bitmap(hw, wirelessmode,
+							ratr_bitmap);
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_LOUD,
+		 "ratr_bitmap :%x\n", ratr_bitmap);
+
+	/* *(u32 *)& rate_mask = EF4BYTE((ratr_bitmap & 0x0fffffff) |
+				       (ratr_index << 28)); */
+
+	rate_mask[0] = macid;
+	rate_mask[1] = ratr_index | (b_shortgi ? 0x80 : 0x00);
+	rate_mask[2] = rtlphy->current_chan_bw
+			   | _rtl8821ae_get_vht_eni(wirelessmode, ratr_bitmap)
+			   | _rtl8821ae_get_ra_ldpc(hw, macid, sta_entry, wirelessmode);
+
+	rate_mask[3] = (u8)(ratr_bitmap & 0x000000ff);
+	rate_mask[4] = (u8)((ratr_bitmap & 0x0000ff00) >> 8);
+	rate_mask[5] = (u8)((ratr_bitmap & 0x00ff0000) >> 16);
+	rate_mask[6] = (u8)((ratr_bitmap & 0xff000000) >> 24);
+
+	RT_TRACE(rtlpriv, COMP_RATR, DBG_DMESG,
+		 "Rate_index:%x, ratr_val:%x, %x:%x:%x:%x:%x:%x:%x\n",
+		 ratr_index, ratr_bitmap,
+		 rate_mask[0], rate_mask[1],
+		 rate_mask[2], rate_mask[3],
+		 rate_mask[4], rate_mask[5],
+		 rate_mask[6]);
+	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RA_MASK, 7, rate_mask);
+	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
+}
+
+void rtl8821ae_update_hal_rate_tbl(struct ieee80211_hw *hw,
+		struct ieee80211_sta *sta, u8 rssi_level)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	if (rtlpriv->dm.useramask)
+		rtl8821ae_update_hal_rate_mask(hw, sta, rssi_level);
+	else
+		/*RT_TRACE(rtlpriv, COMP_RATR,DBG_LOUD,
+			   "rtl8821ae_update_hal_rate_tbl() Error! 8821ae FW RA Only");*/
+		rtl8821ae_update_hal_rate_table(hw, sta);
+}
+
+void rtl8821ae_update_channel_access_setting(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	u8 wireless_mode = mac->mode;
+	u8 sifs_timer, r2t_sifs;
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
+				      (u8 *)&mac->slot_time);
+	if (wireless_mode == WIRELESS_MODE_G)
+		sifs_timer = 0x0a;
+	else
+		sifs_timer = 0x0e;
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *)&sifs_timer);
+
+	r2t_sifs = 0xa;
+
+	if (wireless_mode == WIRELESS_MODE_AC_5G &&
+	    (mac->vht_ldpc_cap & LDPC_VHT_ENABLE_RX) &&
+	    (mac->vht_stbc_cap & STBC_VHT_ENABLE_RX)) {
+		if (mac->vendor == PEER_ATH)
+			r2t_sifs = 0x8;
+		else
+			r2t_sifs = 0xa;
+	} else if (wireless_mode == WIRELESS_MODE_AC_5G) {
+		r2t_sifs = 0xa;
+	}
+
+	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_R2T_SIFS, (u8 *)&r2t_sifs);
+}
+
+bool rtl8821ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 *valid)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
+	u8 u1tmp = 0;
+	bool b_actuallyset = false;
+
+	if (rtlpriv->rtlhal.being_init_adapter)
+		return false;
+
+	if (ppsc->swrf_processing)
+		return false;
+
+	spin_lock(&rtlpriv->locks.rf_ps_lock);
+	if (ppsc->rfchange_inprogress) {
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+		return false;
+	} else {
+		ppsc->rfchange_inprogress = true;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	cur_rfstate = ppsc->rfpwr_state;
+
+	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL_2,
+			rtl_read_byte(rtlpriv,
+					REG_GPIO_IO_SEL_2) & ~(BIT(1)));
+
+	u1tmp = rtl_read_byte(rtlpriv, REG_GPIO_PIN_CTRL_2);
+
+	if (rtlphy->polarity_ctl)
+		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFOFF : ERFON;
+	else
+		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFON : ERFOFF;
+
+	if ((ppsc->hwradiooff) && (e_rfpowerstate_toset == ERFON)) {
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "GPIOChangeRF  - HW Radio ON, RF ON\n");
+
+		e_rfpowerstate_toset = ERFON;
+		ppsc->hwradiooff = false;
+		b_actuallyset = true;
+	} else if ((!ppsc->hwradiooff)
+		   && (e_rfpowerstate_toset == ERFOFF)) {
+		RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+			 "GPIOChangeRF  - HW Radio OFF, RF OFF\n");
+
+		e_rfpowerstate_toset = ERFOFF;
+		ppsc->hwradiooff = true;
+		b_actuallyset = true;
+	}
+
+	if (b_actuallyset) {
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	} else {
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC)
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+
+		spin_lock(&rtlpriv->locks.rf_ps_lock);
+		ppsc->rfchange_inprogress = false;
+		spin_unlock(&rtlpriv->locks.rf_ps_lock);
+	}
+
+	*valid = 1;
+	return !ppsc->hwradiooff;
+}
+
+void rtl8821ae_set_key(struct ieee80211_hw *hw, u32 key_index,
+		     u8 *p_macaddr, bool is_group, u8 enc_algo,
+		     bool is_wepkey, bool clear_all)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 *macaddr = p_macaddr;
+	u32 entry_id = 0;
+	bool is_pairwise = false;
+
+	static u8 cam_const_addr[4][6] = {
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
+		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
+	};
+	static u8 cam_const_broad[] = {
+		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+	};
+
+	if (clear_all) {
+		u8 idx = 0;
+		u8 cam_offset = 0;
+		u8 clear_number = 5;
+
+		RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG, "clear_all\n");
+
+		for (idx = 0; idx < clear_number; idx++) {
+			rtl_cam_mark_invalid(hw, cam_offset + idx);
+			rtl_cam_empty_entry(hw, cam_offset + idx);
+
+			if (idx < 5) {
+				memset(rtlpriv->sec.key_buf[idx], 0,
+				       MAX_KEY_LEN);
+				rtlpriv->sec.key_len[idx] = 0;
+			}
+		}
+	} else {
+		switch (enc_algo) {
+		case WEP40_ENCRYPTION:
+			enc_algo = CAM_WEP40;
+			break;
+		case WEP104_ENCRYPTION:
+			enc_algo = CAM_WEP104;
+			break;
+		case TKIP_ENCRYPTION:
+			enc_algo = CAM_TKIP;
+			break;
+		case AESCCMP_ENCRYPTION:
+			enc_algo = CAM_AES;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+				 "switch case not process\n");
+			enc_algo = CAM_TKIP;
+			break;
+		}
+
+		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
+			macaddr = cam_const_addr[key_index];
+			entry_id = key_index;
+		} else {
+			if (is_group) {
+				macaddr = cam_const_broad;
+				entry_id = key_index;
+			} else {
+				if (mac->opmode == NL80211_IFTYPE_AP) {
+					entry_id = rtl_cam_get_free_entry(hw, p_macaddr);
+					if (entry_id >=  TOTAL_CAM_ENTRY) {
+						RT_TRACE(rtlpriv, COMP_SEC, DBG_EMERG,
+							 "Can not find free hwsecurity cam entry\n");
+						return;
+					}
+				} else {
+					entry_id = CAM_PAIRWISE_KEY_POSITION;
+				}
+
+				key_index = PAIRWISE_KEYIDX;
+				is_pairwise = true;
+			}
+		}
+
+		if (rtlpriv->sec.key_len[key_index] == 0) {
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "delete one entry, entry_id is %d\n",
+				 entry_id);
+			if (mac->opmode == NL80211_IFTYPE_AP)
+				rtl_cam_del_entry(hw, p_macaddr);
+			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
+		} else {
+			RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+				 "add one entry\n");
+			if (is_pairwise) {
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set Pairwise key\n");
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						      entry_id, enc_algo,
+						      CAM_CONFIG_NO_USEDK,
+						      rtlpriv->sec.key_buf[key_index]);
+			} else {
+				RT_TRACE(rtlpriv, COMP_SEC, DBG_DMESG,
+					 "set group key\n");
+
+				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
+					rtl_cam_add_one_entry(hw,
+							rtlefuse->dev_addr,
+							PAIRWISE_KEYIDX,
+							CAM_PAIRWISE_KEY_POSITION,
+							enc_algo,
+							CAM_CONFIG_NO_USEDK,
+							rtlpriv->sec.key_buf
+							[entry_id]);
+				}
+
+				rtl_cam_add_one_entry(hw, macaddr, key_index,
+						entry_id, enc_algo,
+						CAM_CONFIG_NO_USEDK,
+						rtlpriv->sec.key_buf[entry_id]);
+			}
+		}
+	}
+}
+
+void rtl8821ae_bt_reg_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	/* 0:Low, 1:High, 2:From Efuse. */
+	rtlpriv->btcoexist.reg_bt_iso = 2;
+	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
+	rtlpriv->btcoexist.reg_bt_sco = 3;
+	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
+	rtlpriv->btcoexist.reg_bt_sco = 0;
+}
+
+void rtl8821ae_bt_hw_init(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (rtlpriv->cfg->ops->get_btc_status())
+		rtlpriv->btcoexist.btc_ops->btc_init_hw_config(rtlpriv);
+}
+
+void rtl8821ae_suspend(struct ieee80211_hw *hw)
+{
+}
+
+void rtl8821ae_resume(struct ieee80211_hw *hw)
+{
+}
+
+/* Turn on AAP (RCR:bit 0) for promicuous mode. */
+void rtl8821ae_allow_all_destaddr(struct ieee80211_hw *hw,
+	bool allow_all_da, bool write_into_reg)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+
+	if (allow_all_da) /* Set BIT0 */
+		rtlpci->receive_config |= RCR_AAP;
+	else /* Clear BIT0 */
+		rtlpci->receive_config &= ~RCR_AAP;
+
+	if (write_into_reg)
+		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
+
+	RT_TRACE(rtlpriv, COMP_TURBO | COMP_INIT, DBG_LOUD,
+		"receive_config=0x%08X, write_into_reg=%d\n",
+		rtlpci->receive_config, write_into_reg);
+}
+
+/* WKFMCAMAddAllEntry8812 */
+void rtl8821ae_add_wowlan_pattern(struct ieee80211_hw *hw,
+				  struct rtl_wow_pattern *rtl_pattern,
+				  u8 index)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 cam = 0;
+	u8 addr = 0;
+	u16 rxbuf_addr;
+	u8 tmp, count = 0;
+	u16 cam_start;
+	u16 offset;
+
+	/* Count the WFCAM entry start offset. */
+
+	/* RX page size = 128 byte */
+	offset = MAX_RX_DMA_BUFFER_SIZE_8812 / 128;
+	/* We should start from the boundry */
+	cam_start = offset * 128;
+
+	/* Enable Rx packet buffer access. */
+	rtl_write_byte(rtlpriv, REG_PKT_BUFF_ACCESS_CTRL, RXPKT_BUF_SELECT);
+	for (addr = 0; addr < WKFMCAM_ADDR_NUM; addr++) {
+		/* Set Rx packet buffer offset.
+		 * RxBufer pointer increases 1,
+		 * we can access 8 bytes in Rx packet buffer.
+		 * CAM start offset (unit: 1 byte) =  index*WKFMCAM_SIZE
+		 * RxBufer addr = (CAM start offset +
+		 *                 per entry offset of a WKFM CAM)/8
+		 *	* index: The index of the wake up frame mask
+		 *	* WKFMCAM_SIZE: the total size of one WKFM CAM
+		 *	* per entry offset of a WKFM CAM: Addr*4 bytes
+		 */
+		rxbuf_addr = (cam_start + index * WKFMCAM_SIZE + addr * 4) >> 3;
+		/* Set R/W start offset */
+		rtl_write_word(rtlpriv, REG_PKTBUF_DBG_CTRL, rxbuf_addr);
+
+		if (addr == 0) {
+			cam = BIT(31) | rtl_pattern->crc;
+
+			if (rtl_pattern->type == UNICAST_PATTERN)
+				cam |= BIT(24);
+			else if (rtl_pattern->type == MULTICAST_PATTERN)
+				cam |= BIT(25);
+			else if (rtl_pattern->type == BROADCAST_PATTERN)
+				cam |= BIT(26);
+
+			rtl_write_dword(rtlpriv, REG_PKTBUF_DBG_DATA_L, cam);
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+				 "WRITE entry[%d] 0x%x: %x\n", addr,
+				  REG_PKTBUF_DBG_DATA_L, cam);
+
+			/* Write to Rx packet buffer. */
+			rtl_write_word(rtlpriv, REG_RXPKTBUF_CTRL, 0x0f01);
+		} else if (addr == 2 || addr == 4) {/* WKFM[127:0] */
+			cam = rtl_pattern->mask[addr - 2];
+
+			rtl_write_dword(rtlpriv, REG_PKTBUF_DBG_DATA_L, cam);
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+				 "WRITE entry[%d] 0x%x: %x\n", addr,
+				  REG_PKTBUF_DBG_DATA_L, cam);
+
+			rtl_write_word(rtlpriv, REG_RXPKTBUF_CTRL, 0x0f01);
+		} else if (addr == 3 || addr == 5) {/* WKFM[127:0] */
+			cam = rtl_pattern->mask[addr - 2];
+
+			rtl_write_dword(rtlpriv, REG_PKTBUF_DBG_DATA_H, cam);
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+				 "WRITE entry[%d] 0x%x: %x\n", addr,
+				  REG_PKTBUF_DBG_DATA_H, cam);
+
+			rtl_write_word(rtlpriv, REG_RXPKTBUF_CTRL, 0xf001);
+		}
+
+		count = 0;
+		do {
+			tmp = rtl_read_byte(rtlpriv, REG_RXPKTBUF_CTRL);
+			udelay(2);
+			count++;
+		} while (tmp && count < 100);
+
+		RT_ASSERT((count < 100),
+			  "Write wake up frame mask FAIL %d value!\n", tmp);
+	}
+	/* Disable Rx packet buffer access. */
+	rtl_write_byte(rtlpriv, REG_PKT_BUFF_ACCESS_CTRL,
+		       DISABLE_TRXPKT_BUF_ACCESS);
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hw.h b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.h
index 256e514f424a..a3553e3abaa1 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/hw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/hw.h
@@ -54,12 +54,6 @@ void rtl8821ae_set_key(struct ieee80211_hw *hw, u32 key_index,
 		       u8 *p_macaddr, bool is_group, u8 enc_algo,
 		       bool is_wepkey, bool clear_all);
 
-void rtl8821ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-					      bool autoload_fail,
-					      u8 *hwinfo);
-void rtl8812ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-					      bool autoload_fail,
-					      u8 *hwinfo);
 void rtl8821ae_bt_reg_init(struct ieee80211_hw *hw);
 void rtl8821ae_bt_hw_init(struct ieee80211_hw *hw);
 void rtl8821ae_suspend(struct ieee80211_hw *hw);
@@ -69,4 +63,8 @@ void rtl8821ae_allow_all_destaddr(struct ieee80211_hw *hw,
 				  bool write_into_reg);
 void _rtl8821ae_stop_tx_beacon(struct ieee80211_hw *hw);
 void _rtl8821ae_resume_tx_beacon(struct ieee80211_hw *hw);
+void rtl8821ae_add_wowlan_pattern(struct ieee80211_hw *hw,
+				  struct rtl_wow_pattern *rtl_pattern,
+				  u8 index);
+
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/led.c b/drivers/net/wireless/rtlwifi/rtl8821ae/led.c
index 130a4f4b24a2..ba1946a0280e 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/led.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/led.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,14 +26,15 @@
 #include "../wifi.h"
 #include "../pci.h"
 #include "reg.h"
+#include "led.h"
 
 static void _rtl8821ae_init_led(struct ieee80211_hw *hw,
-			      					 struct rtl_led *pled,
-			      					 enum rtl_led_pin ledpin)
+				struct rtl_led *pled,
+				enum rtl_led_pin ledpin)
 {
 	pled->hw = hw;
 	pled->ledpin = ledpin;
-	pled->b_ledon = false;
+	pled->ledon = false;
 }
 
 void rtl8821ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
@@ -45,8 +42,8 @@ void rtl8821ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 	u8 ledcfg;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	RT_TRACE(COMP_LED, DBG_LOUD,
-		 ("LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin));
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	switch (pled->ledpin) {
 	case LED_PIN_GPIO0:
@@ -62,41 +59,43 @@ void rtl8821ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg & 0x10);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
-	pled->b_ledon = true;
+	pled->ledon = true;
 }
 
 void rtl8812ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled)
 {
-	u16 	ledreg = REG_LEDCFG1;
-	u8 	ledcfg = 0;
+	u16	ledreg = REG_LEDCFG1;
+	u8	ledcfg = 0;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
 	switch (pled->ledpin) {
-		case LED_PIN_LED0:
-			ledreg = REG_LEDCFG1;
-			break;
+	case LED_PIN_LED0:
+		ledreg = REG_LEDCFG1;
+		break;
 
-		case LED_PIN_LED1:
-			ledreg = REG_LEDCFG2;
-			break;
+	case LED_PIN_LED1:
+		ledreg = REG_LEDCFG2;
+		break;
 
-		case LED_PIN_GPIO0:
-		default:
-			break;
+	case LED_PIN_GPIO0:
+	default:
+		break;
 	}
 
-	RT_TRACE(COMP_LED, DBG_LOUD, ("In SwLedOn, LedAddr:%X LEDPIN=%d \n", ledreg, pled->ledpin));
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "In SwLedOn, LedAddr:%X LEDPIN=%d\n",
+		 ledreg, pled->ledpin);
 
 	ledcfg =  rtl_read_byte(rtlpriv, ledreg);
 	ledcfg |= BIT(5); /*Set 0x4c[21]*/
-	ledcfg &= ~(BIT(7) | BIT(6) | BIT(3) |BIT(2) | BIT(1) |BIT(0));
+	ledcfg &= ~(BIT(7) | BIT(6) | BIT(3) | BIT(2) | BIT(1) | BIT(0));
 		/*Clear 0x4c[23:22] and 0x4c[19:16]*/
 	rtl_write_byte(rtlpriv, ledreg, ledcfg); /*SW control led0 on.*/
-	pled->b_ledon = true;
+	pled->ledon = true;
 }
 
 void rtl8821ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
@@ -105,8 +104,8 @@ void rtl8821ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 	u8 ledcfg;
 
-	RT_TRACE(COMP_LED, DBG_LOUD,
-		 ("LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin));
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "LedAddr:%X ledpin=%d\n", REG_LEDCFG2, pled->ledpin);
 
 	ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG2);
 
@@ -115,58 +114,57 @@ void rtl8821ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
 		break;
 	case LED_PIN_LED0:
 		ledcfg &= 0xf0;
-		if (pcipriv->ledctl.bled_opendrain == true) {
+		if (pcipriv->ledctl.led_opendrain) {
 			ledcfg &= 0x90; /* Set to software control. */
 			rtl_write_byte(rtlpriv, REG_LEDCFG2, (ledcfg|BIT(3)));
 			ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
 			ledcfg &= 0xFE;
 			rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, ledcfg);
-		}
-		else {
+		} else {
 			ledcfg &= ~BIT(6);
 			rtl_write_byte(rtlpriv, REG_LEDCFG2,
-					(ledcfg | BIT(3) | BIT(5)));
+				       (ledcfg | BIT(3) | BIT(5)));
 		}
 		break;
 	case LED_PIN_LED1:
 		ledcfg = rtl_read_byte(rtlpriv, REG_LEDCFG1);
 		ledcfg &= 0x10; /* Set to software control. */
 		rtl_write_byte(rtlpriv, REG_LEDCFG1, ledcfg|BIT(3));
-
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_LOUD,
+			 "switch case not process\n");
 		break;
 	}
-	pled->b_ledon = false;
+	pled->ledon = false;
 }
 
-void rtl8812ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled){
+void rtl8812ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled)
+{
 	u16 ledreg = REG_LEDCFG1;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
 
-	switch(pled->ledpin)
-	{
-		case LED_PIN_LED0:
-			ledreg = REG_LEDCFG1;
-			break;
+	switch (pled->ledpin) {
+	case LED_PIN_LED0:
+		ledreg = REG_LEDCFG1;
+		break;
 
-		case LED_PIN_LED1:
-			ledreg = REG_LEDCFG2;
-			break;
+	case LED_PIN_LED1:
+		ledreg = REG_LEDCFG2;
+		break;
 
-		case LED_PIN_GPIO0:
-		default:
-			break;
+	case LED_PIN_GPIO0:
+	default:
+		break;
 	}
 
-	RT_TRACE(COMP_LED,DBG_LOUD,("In SwLedOff,LedAddr:%X LEDPIN=%d\n", ledreg, pled->ledpin));
-
-	if(pcipriv->ledctl.bled_opendrain == true) /*Open-drain arrangement for controlling the LED*/
-	{
-		u8 ledcfg = rtl_read_byte(rtlpriv,  ledreg);
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD,
+		 "In SwLedOff,LedAddr:%X LEDPIN=%d\n",
+		 ledreg, pled->ledpin);
+	/*Open-drain arrangement for controlling the LED*/
+	if (pcipriv->ledctl.led_opendrain) {
+		u8 ledcfg = rtl_read_byte(rtlpriv, ledreg);
 
 		ledreg &= 0xd0; /* Set to software control.*/
 		rtl_write_byte(rtlpriv, ledreg, (ledcfg | BIT(3)));
@@ -175,28 +173,28 @@ void rtl8812ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled){
 		ledcfg = rtl_read_byte(rtlpriv, REG_MAC_PINMUX_CFG);
 		ledcfg &= 0xFE;/*Set GPIO[8] to input mode*/
 		rtl_write_byte(rtlpriv, REG_MAC_PINMUX_CFG, ledcfg);
-	}
-	else
-	{
+	} else {
 		rtl_write_byte(rtlpriv, ledreg, 0x28);
 	}
 
-	pled->b_ledon = false;
+	pled->ledon = false;
 }
 
 void rtl8821ae_init_sw_leds(struct ieee80211_hw *hw)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	_rtl8821ae_init_led(hw, &(pcipriv->ledctl.sw_led0), LED_PIN_LED0);
-	_rtl8821ae_init_led(hw, &(pcipriv->ledctl.sw_led1), LED_PIN_LED1);
+
+	_rtl8821ae_init_led(hw, &pcipriv->ledctl.sw_led0, LED_PIN_LED0);
+	_rtl8821ae_init_led(hw, &pcipriv->ledctl.sw_led1, LED_PIN_LED1);
 }
 
 static void _rtl8821ae_sw_led_control(struct ieee80211_hw *hw,
-				    enum led_ctl_mode ledaction)
+				      enum led_ctl_mode ledaction)
 {
 	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
+	struct rtl_led *pLed0 = &pcipriv->ledctl.sw_led0;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
 	switch (ledaction) {
 	case LED_CTL_POWER_ON:
 	case LED_CTL_LINK:
@@ -207,7 +205,7 @@ static void _rtl8821ae_sw_led_control(struct ieee80211_hw *hw,
 			rtl8821ae_sw_led_on(hw, pLed0);
 		break;
 	case LED_CTL_POWER_OFF:
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)\
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
 			rtl8812ae_sw_led_off(hw, pLed0);
 		else
 			rtl8821ae_sw_led_off(hw, pLed0);
@@ -218,7 +216,7 @@ static void _rtl8821ae_sw_led_control(struct ieee80211_hw *hw,
 }
 
 void rtl8821ae_led_control(struct ieee80211_hw *hw,
-			enum led_ctl_mode ledaction)
+			   enum led_ctl_mode ledaction)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
@@ -233,7 +231,7 @@ void rtl8821ae_led_control(struct ieee80211_hw *hw,
 	     ledaction == LED_CTL_POWER_ON)) {
 		return;
 	}
-	RT_TRACE(COMP_LED, DBG_LOUD, ("ledaction %d, \n",
-				ledaction));
+	RT_TRACE(rtlpriv, COMP_LED, DBG_LOUD, "ledaction %d,\n",
+		 ledaction);
 	_rtl8821ae_sw_led_control(hw, ledaction);
 }
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/led.h b/drivers/net/wireless/rtlwifi/rtl8821ae/led.h
index 44be401ba21f..038e64e18ae8 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/led.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/led.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -35,6 +31,7 @@ void rtl8821ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
 void rtl8812ae_sw_led_on(struct ieee80211_hw *hw, struct rtl_led *pled);
 void rtl8821ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
 void rtl8812ae_sw_led_off(struct ieee80211_hw *hw, struct rtl_led *pled);
-void rtl8821ae_led_control(struct ieee80211_hw *hw, enum led_ctl_mode ledaction);
+void rtl8821ae_led_control(struct ieee80211_hw *hw,
+			   enum led_ctl_mode ledaction);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/phy.c b/drivers/net/wireless/rtlwifi/rtl8821ae/phy.c
new file mode 100644
index 000000000000..9786313dc62f
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/phy.c
@@ -0,0 +1,4855 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../ps.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "rf.h"
+#include "dm.h"
+#include "table.h"
+#include "trx.h"
+#include "../btcoexist/halbt_precomp.h"
+#include "hw.h"
+#include "../efuse.h"
+
+#define READ_NEXT_PAIR(array_table, v1, v2, i) \
+	do { \
+		i += 2; \
+		v1 = array_table[i]; \
+		v2 = array_table[i+1]; \
+	} while (0)
+
+static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
+					 enum radio_path rfpath, u32 offset);
+static void _rtl8821ae_phy_rf_serial_write(struct ieee80211_hw *hw,
+					   enum radio_path rfpath, u32 offset,
+					   u32 data);
+static u32 _rtl8821ae_phy_calculate_bit_shift(u32 bitmask);
+static bool _rtl8821ae_phy_bb8821a_config_parafile(struct ieee80211_hw *hw);
+/*static bool _rtl8812ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);*/
+static bool _rtl8821ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
+static bool _rtl8821ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						     u8 configtype);
+static bool _rtl8821ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+						       u8 configtype);
+static void phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
+
+static long _rtl8821ae_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					    enum wireless_mode wirelessmode,
+					    u8 txpwridx);
+static void rtl8821ae_phy_set_rf_on(struct ieee80211_hw *hw);
+static void rtl8821ae_phy_set_io(struct ieee80211_hw *hw);
+
+static void rtl8812ae_fixspur(struct ieee80211_hw *hw,
+			      enum ht_channel_width band_width, u8 channel)
+{
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	/*C cut Item12 ADC FIFO CLOCK*/
+	if (IS_VENDOR_8812A_C_CUT(rtlhal->version)) {
+		if (band_width == HT_CHANNEL_WIDTH_20_40 && channel == 11)
+			rtl_set_bbreg(hw, RRFMOD, 0xC00, 0x3);
+			/* 0x8AC[11:10] = 2'b11*/
+		else
+			rtl_set_bbreg(hw, RRFMOD, 0xC00, 0x2);
+			/* 0x8AC[11:10] = 2'b10*/
+
+		/* <20120914, Kordan> A workarould to resolve
+		 * 2480Mhz spur by setting ADC clock as 160M. (Asked by Binson)
+		 */
+		if (band_width == HT_CHANNEL_WIDTH_20 &&
+		    (channel == 13 || channel == 14)) {
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x3);
+			/*0x8AC[9:8] = 2'b11*/
+			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);
+			/* 0x8C4[30] = 1*/
+		} else if (band_width == HT_CHANNEL_WIDTH_20_40 &&
+			   channel == 11) {
+			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);
+			/*0x8C4[30] = 1*/
+		} else if (band_width != HT_CHANNEL_WIDTH_80) {
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x2);
+			/*0x8AC[9:8] = 2'b10*/
+			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
+			/*0x8C4[30] = 0*/
+		}
+	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		/* <20120914, Kordan> A workarould to resolve
+		 * 2480Mhz spur by setting ADC clock as 160M.
+		 */
+		if (band_width == HT_CHANNEL_WIDTH_20 &&
+		    (channel == 13 || channel == 14))
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x3);
+			/*0x8AC[9:8] = 11*/
+		else if (channel  <= 14) /*2.4G only*/
+			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x2);
+			/*0x8AC[9:8] = 10*/
+	}
+}
+
+u32 rtl8821ae_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr,
+			       u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 returnvalue, originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x)\n",
+		 regaddr, bitmask);
+	originalvalue = rtl_read_dword(rtlpriv, regaddr);
+	bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+	returnvalue = (originalvalue & bitmask) >> bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "BBR MASK=0x%x Addr[0x%x]=0x%x\n",
+		 bitmask, regaddr, originalvalue);
+	return returnvalue;
+}
+
+void rtl8821ae_phy_set_bb_reg(struct ieee80211_hw *hw,
+			      u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 originalvalue, bitshift;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		 regaddr, bitmask, data);
+
+	if (bitmask != MASKDWORD) {
+		originalvalue = rtl_read_dword(rtlpriv, regaddr);
+		bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+		data = ((originalvalue & (~bitmask)) |
+			((data << bitshift) & bitmask));
+	}
+
+	rtl_write_dword(rtlpriv, regaddr, data);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x)\n",
+		 regaddr, bitmask, data);
+}
+
+u32 rtl8821ae_phy_query_rf_reg(struct ieee80211_hw *hw,
+			       enum radio_path rfpath, u32 regaddr,
+			       u32 bitmask)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, readback_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), rfpath(%#x), bitmask(%#x)\n",
+		 regaddr, rfpath, bitmask);
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	original_value = _rtl8821ae_phy_rf_serial_read(hw, rfpath, regaddr);
+	bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+	readback_value = (original_value & bitmask) >> bitshift;
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), rfpath(%#x), bitmask(%#x), original_value(%#x)\n",
+		 regaddr, rfpath, bitmask, original_value);
+
+	return readback_value;
+}
+
+void rtl8821ae_phy_set_rf_reg(struct ieee80211_hw *hw,
+			   enum radio_path rfpath,
+			   u32 regaddr, u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 original_value, bitshift;
+	unsigned long flags;
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		  regaddr, bitmask, data, rfpath);
+
+	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
+
+	if (bitmask != RFREG_OFFSET_MASK) {
+		original_value =
+		   _rtl8821ae_phy_rf_serial_read(hw, rfpath, regaddr);
+		bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
+		data = ((original_value & (~bitmask)) | (data << bitshift));
+	}
+
+	_rtl8821ae_phy_rf_serial_write(hw, rfpath, regaddr, data);
+
+	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
+
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
+		 regaddr, bitmask, data, rfpath);
+}
+
+static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
+					 enum radio_path rfpath, u32 offset)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	bool is_pi_mode = false;
+	u32 retvalue = 0;
+
+	/* 2009/06/17 MH We can not execute IO for power
+	save or other accident mode.*/
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "return all one\n");
+		return 0xFFFFFFFF;
+	}
+	/* <20120809, Kordan> CCA OFF(when entering),
+		asked by James to avoid reading the wrong value.
+	    <20120828, Kordan> Toggling CCA would affect RF 0x0, skip it!*/
+	if (offset != 0x0 &&
+	    !((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) ||
+	    (IS_VENDOR_8812A_C_CUT(rtlhal->version))))
+		rtl_set_bbreg(hw, RCCAONSEC, 0x8, 1);
+	offset &= 0xff;
+
+	if (rfpath == RF90_PATH_A)
+		is_pi_mode = (bool)rtl_get_bbreg(hw, 0xC00, 0x4);
+	else if (rfpath == RF90_PATH_B)
+		is_pi_mode = (bool)rtl_get_bbreg(hw, 0xE00, 0x4);
+
+	rtl_set_bbreg(hw, RHSSIREAD_8821AE, 0xff, offset);
+
+	if ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) ||
+	    (IS_VENDOR_8812A_C_CUT(rtlhal->version)))
+		udelay(20);
+
+	if (is_pi_mode) {
+		if (rfpath == RF90_PATH_A)
+			retvalue =
+			  rtl_get_bbreg(hw, RA_PIREAD_8821A, BLSSIREADBACKDATA);
+		else if (rfpath == RF90_PATH_B)
+			retvalue =
+			  rtl_get_bbreg(hw, RB_PIREAD_8821A, BLSSIREADBACKDATA);
+	} else {
+		if (rfpath == RF90_PATH_A)
+			retvalue =
+			  rtl_get_bbreg(hw, RA_SIREAD_8821A, BLSSIREADBACKDATA);
+		else if (rfpath == RF90_PATH_B)
+			retvalue =
+			  rtl_get_bbreg(hw, RB_SIREAD_8821A, BLSSIREADBACKDATA);
+	}
+
+	/*<20120809, Kordan> CCA ON(when exiting),
+	 * asked by James to avoid reading the wrong value.
+	 *   <20120828, Kordan> Toggling CCA would affect RF 0x0, skip it!
+	 */
+	if (offset != 0x0 &&
+	    !((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) ||
+	    (IS_VENDOR_8812A_C_CUT(rtlhal->version))))
+		rtl_set_bbreg(hw, RCCAONSEC, 0x8, 0);
+	return retvalue;
+}
+
+static void _rtl8821ae_phy_rf_serial_write(struct ieee80211_hw *hw,
+					   enum radio_path rfpath, u32 offset,
+					   u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
+	u32 data_and_addr;
+	u32 newoffset;
+
+	if (RT_CANNOT_IO(hw)) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "stop\n");
+		return;
+	}
+	offset &= 0xff;
+	newoffset = offset;
+	data_and_addr = ((newoffset << 20) |
+			 (data & 0x000fffff)) & 0x0fffffff;
+	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
+	RT_TRACE(rtlpriv, COMP_RF, DBG_TRACE,
+		 "RFW-%d Addr[0x%x]=0x%x\n",
+		 rfpath, pphyreg->rf3wire_offset, data_and_addr);
+}
+
+static u32 _rtl8821ae_phy_calculate_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+
+bool rtl8821ae_phy_mac_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = 0;
+
+	rtstatus = _rtl8821ae_phy_config_mac_with_headerfile(hw);
+
+	return rtstatus;
+}
+
+bool rtl8821ae_phy_bb_config(struct ieee80211_hw *hw)
+{
+	bool rtstatus = true;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 regval;
+	u8 crystal_cap;
+
+	phy_init_bb_rf_register_definition(hw);
+
+	regval = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN);
+	regval |= FEN_PCIEA;
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, regval);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
+		       regval | FEN_BB_GLB_RSTN | FEN_BBRSTB);
+
+	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x7);
+	rtl_write_byte(rtlpriv, REG_OPT_CTRL + 2, 0x7);
+
+	rtstatus = _rtl8821ae_phy_bb8821a_config_parafile(hw);
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		crystal_cap = rtlefuse->crystalcap & 0x3F;
+		rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0x7FF80000,
+			      (crystal_cap | (crystal_cap << 6)));
+	} else {
+		crystal_cap = rtlefuse->crystalcap & 0x3F;
+		rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000,
+			      (crystal_cap | (crystal_cap << 6)));
+	}
+	rtlphy->reg_837 = rtl_read_byte(rtlpriv, 0x837);
+
+	return rtstatus;
+}
+
+bool rtl8821ae_phy_rf_config(struct ieee80211_hw *hw)
+{
+	return rtl8821ae_phy_rf6052_config(hw);
+}
+
+u32 phy_get_tx_swing_8812A(struct ieee80211_hw *hw, u8	band,
+			   u8 rf_path)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	char reg_swing_2g = -1;/* 0xff; */
+	char reg_swing_5g = -1;/* 0xff; */
+	char swing_2g = -1 * reg_swing_2g;
+	char swing_5g = -1 * reg_swing_5g;
+	u32  out = 0x200;
+	const char auto_temp = -1;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+		 "===> PHY_GetTxBBSwing_8812A, bbSwing_2G: %d, bbSwing_5G: %d,autoload_failflag=%d.\n",
+		 (int)swing_2g, (int)swing_5g,
+		 (int)rtlefuse->autoload_failflag);
+
+	if (rtlefuse->autoload_failflag) {
+		if (band == BAND_ON_2_4G) {
+			rtldm->swing_diff_2g = swing_2g;
+			if (swing_2g == 0) {
+				out = 0x200; /* 0 dB */
+			} else if (swing_2g == -3) {
+				out = 0x16A; /* -3 dB */
+			} else if (swing_2g == -6) {
+				out = 0x101; /* -6 dB */
+			} else if (swing_2g == -9) {
+				out = 0x0B6; /* -9 dB */
+			} else {
+				rtldm->swing_diff_2g = 0;
+				out = 0x200;
+			}
+		} else if (band == BAND_ON_5G) {
+			rtldm->swing_diff_5g = swing_5g;
+			if (swing_5g == 0) {
+				out = 0x200; /* 0 dB */
+			} else if (swing_5g == -3) {
+				out = 0x16A; /* -3 dB */
+			} else if (swing_5g == -6) {
+				out = 0x101; /* -6 dB */
+			} else if (swing_5g == -9) {
+				out = 0x0B6; /* -9 dB */
+			} else {
+				if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+					rtldm->swing_diff_5g = -3;
+					out = 0x16A;
+				} else {
+					rtldm->swing_diff_5g = 0;
+					out = 0x200;
+				}
+			}
+		} else {
+			rtldm->swing_diff_2g = -3;
+			rtldm->swing_diff_5g = -3;
+			out = 0x16A; /* -3 dB */
+		}
+	} else {
+	    u32 swing = 0, swing_a = 0, swing_b = 0;
+
+	    if (band == BAND_ON_2_4G) {
+			if (reg_swing_2g == auto_temp) {
+				efuse_shadow_read(hw, 1, 0xC6, (u32 *)&swing);
+				swing = (swing == 0xFF) ? 0x00 : swing;
+			} else if (swing_2g ==  0) {
+				swing = 0x00; /* 0 dB */
+			} else if (swing_2g == -3) {
+				swing = 0x05; /* -3 dB */
+			} else if (swing_2g == -6) {
+				swing = 0x0A; /* -6 dB */
+			} else if (swing_2g == -9) {
+				swing = 0xFF; /* -9 dB */
+			} else {
+				swing = 0x00;
+			}
+		} else {
+			if (reg_swing_5g == auto_temp) {
+				efuse_shadow_read(hw, 1, 0xC7, (u32 *)&swing);
+				swing = (swing == 0xFF) ? 0x00 : swing;
+			} else if (swing_5g ==  0) {
+				swing = 0x00; /* 0 dB */
+			} else if (swing_5g == -3) {
+				swing = 0x05; /* -3 dB */
+			} else if (swing_5g == -6) {
+				swing = 0x0A; /* -6 dB */
+			} else if (swing_5g == -9) {
+				swing = 0xFF; /* -9 dB */
+			} else {
+				swing = 0x00;
+			}
+		}
+
+		swing_a = (swing & 0x3) >> 0; /* 0xC6/C7[1:0] */
+		swing_b = (swing & 0xC) >> 2; /* 0xC6/C7[3:2] */
+		RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+			 "===> PHY_GetTxBBSwing_8812A, swingA: 0x%X, swingB: 0x%X\n",
+			 swing_a, swing_b);
+
+		/* 3 Path-A */
+		if (swing_a == 0x0) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = 0;
+			else
+				rtldm->swing_diff_5g = 0;
+			out = 0x200; /* 0 dB */
+		} else if (swing_a == 0x1) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = -3;
+			else
+				rtldm->swing_diff_5g = -3;
+			out = 0x16A; /* -3 dB */
+		} else if (swing_a == 0x2) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = -6;
+			else
+				rtldm->swing_diff_5g = -6;
+			out = 0x101; /* -6 dB */
+		} else if (swing_a == 0x3) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = -9;
+			else
+				rtldm->swing_diff_5g = -9;
+			out = 0x0B6; /* -9 dB */
+		}
+		/* 3 Path-B */
+		if (swing_b == 0x0) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = 0;
+			else
+				rtldm->swing_diff_5g = 0;
+			out = 0x200; /* 0 dB */
+		} else if (swing_b == 0x1) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = -3;
+			else
+				rtldm->swing_diff_5g = -3;
+			out = 0x16A; /* -3 dB */
+		} else if (swing_b == 0x2) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = -6;
+			else
+				rtldm->swing_diff_5g = -6;
+			out = 0x101; /* -6 dB */
+		} else if (swing_b == 0x3) {
+			if (band == BAND_ON_2_4G)
+				rtldm->swing_diff_2g = -9;
+			else
+				rtldm->swing_diff_5g = -9;
+			out = 0x0B6; /* -9 dB */
+		}
+	}
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+		 "<=== PHY_GetTxBBSwing_8812A, out = 0x%X\n", out);
+	 return out;
+}
+
+void rtl8821ae_phy_switch_wirelessband(struct ieee80211_hw *hw, u8 band)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
+	u8 current_band = rtlhal->current_bandtype;
+	u32 txpath, rxpath;
+	char bb_diff_between_band;
+
+	txpath = rtl8821ae_phy_query_bb_reg(hw, RTXPATH, 0xf0);
+	rxpath = rtl8821ae_phy_query_bb_reg(hw, RCCK_RX, 0x0f000000);
+	rtlhal->current_bandtype = (enum band_type) band;
+	/* reconfig BB/RF according to wireless mode */
+	if (rtlhal->current_bandtype == BAND_ON_2_4G) {
+		/* BB & RF Config */
+		rtl_set_bbreg(hw, ROFDMCCKEN, BOFDMEN|BCCKEN, 0x03);
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			/* 0xCB0[15:12] = 0x7 (LNA_On)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF000, 0x7);
+			/* 0xCB0[7:4] = 0x7 (PAPE_A)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF0, 0x7);
+		}
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			/*0x834[1:0] = 0x1*/
+			rtl_set_bbreg(hw, 0x834, 0x3, 0x1);
+		}
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			/* 0xC1C[11:8] = 0 */
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xF00, 0);
+		} else {
+			/* 0x82C[1:0] = 2b'00 */
+			rtl_set_bbreg(hw, 0x82c, 0x3, 0);
+		}
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, BMASKDWORD,
+				      0x77777777);
+			rtl_set_bbreg(hw, RB_RFE_PINMUX, BMASKDWORD,
+				      0x77777777);
+			rtl_set_bbreg(hw, RA_RFE_INV, 0x3ff00000, 0x000);
+			rtl_set_bbreg(hw, RB_RFE_INV, 0x3ff00000, 0x000);
+		}
+
+		rtl_set_bbreg(hw, RTXPATH, 0xf0, 0x1);
+		rtl_set_bbreg(hw, RCCK_RX, 0x0f000000, 0x1);
+
+		rtl_write_byte(rtlpriv, REG_CCK_CHECK, 0x0);
+	} else {/* 5G band */
+		u16 count, reg_41a;
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			/*0xCB0[15:12] = 0x5 (LNA_On)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF000, 0x5);
+			/*0xCB0[7:4] = 0x4 (PAPE_A)*/
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF0, 0x4);
+		}
+		/*CCK_CHECK_en*/
+		rtl_write_byte(rtlpriv, REG_CCK_CHECK, 0x80);
+
+		count = 0;
+		reg_41a = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
+		RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+			 "Reg41A value %d", reg_41a);
+		reg_41a &= 0x30;
+		while ((reg_41a != 0x30) && (count < 50)) {
+			udelay(50);
+			RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD, "Delay 50us\n");
+
+			reg_41a = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
+			reg_41a &= 0x30;
+			count++;
+			RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+				 "Reg41A value %d", reg_41a);
+		}
+		if (count != 0)
+			RT_TRACE(rtlpriv, COMP_MLME, DBG_LOUD,
+				 "PHY_SwitchWirelessBand8812(): Switch to 5G Band. Count = %d reg41A=0x%x\n",
+				 count, reg_41a);
+
+		/* 2012/02/01, Sinda add registry to switch workaround
+		without long-run verification for scan issue. */
+		rtl_set_bbreg(hw, ROFDMCCKEN, BOFDMEN|BCCKEN, 0x03);
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			/*0x834[1:0] = 0x2*/
+			rtl_set_bbreg(hw, 0x834, 0x3, 0x2);
+		}
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+			/* AGC table select */
+			/* 0xC1C[11:8] = 1*/
+			rtl_set_bbreg(hw, RA_TXSCALE, 0xF00, 1);
+		} else
+			/* 0x82C[1:0] = 2'b00 */
+			rtl_set_bbreg(hw, 0x82c, 0x3, 1);
+
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			rtl_set_bbreg(hw, RA_RFE_PINMUX, BMASKDWORD,
+				      0x77337777);
+			rtl_set_bbreg(hw, RB_RFE_PINMUX, BMASKDWORD,
+				      0x77337777);
+			rtl_set_bbreg(hw, RA_RFE_INV, 0x3ff00000, 0x010);
+			rtl_set_bbreg(hw, RB_RFE_INV, 0x3ff00000, 0x010);
+		}
+
+		rtl_set_bbreg(hw, RTXPATH, 0xf0, 0);
+		rtl_set_bbreg(hw, RCCK_RX, 0x0f000000, 0xf);
+
+		RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD,
+			 "==>PHY_SwitchWirelessBand8812() BAND_ON_5G settings OFDM index 0x%x\n",
+			 rtlpriv->dm.ofdm_index[RF90_PATH_A]);
+	}
+
+	if ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) ||
+	    (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)) {
+		/* 0xC1C[31:21] */
+		rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
+			      phy_get_tx_swing_8812A(hw, band, RF90_PATH_A));
+		/* 0xE1C[31:21] */
+		rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000,
+			      phy_get_tx_swing_8812A(hw, band, RF90_PATH_B));
+
+		/* <20121005, Kordan> When TxPowerTrack is ON,
+		 *	we should take care of the change of BB swing.
+		 *   That is, reset all info to trigger Tx power tracking.
+		 */
+		if (band != current_band) {
+			bb_diff_between_band =
+				(rtldm->swing_diff_2g - rtldm->swing_diff_5g);
+			bb_diff_between_band = (band == BAND_ON_2_4G) ?
+						bb_diff_between_band :
+						(-1 * bb_diff_between_band);
+			rtldm->default_ofdm_index += bb_diff_between_band * 2;
+		}
+		rtl8821ae_dm_clear_txpower_tracking_state(hw);
+	}
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "<==rtl8821ae_phy_switch_wirelessband():Switch Band OK.\n");
+	return;
+}
+
+static bool _rtl8821ae_check_condition(struct ieee80211_hw *hw,
+				       const u32 condition)
+{
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u32 _board = rtlefuse->board_type; /*need efuse define*/
+	u32 _interface = 0x01; /* ODM_ITRF_PCIE */
+	u32 _platform = 0x08;/* ODM_WIN */
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+
+	cond = condition & 0xFF;
+	if ((_board != cond) && cond != 0xFF)
+		return false;
+
+	cond = condition & 0xFF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = condition & 0xFF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+static void _rtl8821ae_config_rf_reg(struct ieee80211_hw *hw,
+				     u32 addr, u32 data,
+				     enum radio_path rfpath, u32 regaddr)
+{
+	if (addr == 0xfe || addr == 0xffe) {
+		/* In order not to disturb BT music when
+		 * wifi init.(1ant NIC only)
+		 */
+		mdelay(50);
+	} else {
+		rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
+		udelay(1);
+	}
+}
+
+static void _rtl8821ae_config_rf_radio_a(struct ieee80211_hw *hw,
+					 u32 addr, u32 data)
+{
+	u32 content = 0x1000; /*RF Content: radio_a_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl8821ae_config_rf_reg(hw, addr, data,
+				 RF90_PATH_A, addr | maskforphyset);
+}
+
+static void _rtl8821ae_config_rf_radio_b(struct ieee80211_hw *hw,
+					 u32 addr, u32 data)
+{
+	u32 content = 0x1001; /*RF Content: radio_b_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	_rtl8821ae_config_rf_reg(hw, addr, data,
+				 RF90_PATH_B, addr | maskforphyset);
+}
+
+static void _rtl8821ae_config_bb_reg(struct ieee80211_hw *hw,
+				     u32 addr, u32 data)
+{
+	if (addr == 0xfe)
+		mdelay(50);
+	else if (addr == 0xfd)
+		mdelay(5);
+	else if (addr == 0xfc)
+		mdelay(1);
+	else if (addr == 0xfb)
+		udelay(50);
+	else if (addr == 0xfa)
+		udelay(5);
+	else if (addr == 0xf9)
+		udelay(1);
+	else
+		rtl_set_bbreg(hw, addr, MASKDWORD, data);
+
+	udelay(1);
+}
+
+static void _rtl8821ae_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 band, rfpath, txnum, rate_section;
+
+	for (band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band)
+		for (rfpath = 0; rfpath < TX_PWR_BY_RATE_NUM_RF; ++rfpath)
+			for (txnum = 0; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum)
+				for (rate_section = 0;
+				     rate_section < TX_PWR_BY_RATE_NUM_SECTION;
+				     ++rate_section)
+					rtlphy->tx_power_by_rate_offset[band]
+					    [rfpath][txnum][rate_section] = 0;
+}
+
+static void _rtl8821ae_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
+					  u8 band, u8 path,
+					  u8 rate_section,
+					  u8 txnum, u8 value)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	if (path > RF90_PATH_D) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			"Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n", path);
+		return;
+	}
+
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][0] = value;
+			break;
+		case OFDM:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][1] = value;
+			break;
+		case HT_MCS0_MCS7:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][2] = value;
+			break;
+		case HT_MCS8_MCS15:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][4] = value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			rtlphy->txpwr_by_rate_base_24g[path][txnum][5] = value;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Invalid RateSection %d in Band 2.4G,Rf Path %d, %dTx in PHY_SetTxPowerByRateBase()\n",
+				 rate_section, path, txnum);
+			break;
+		};
+	} else if (band == BAND_ON_5G) {
+		switch (rate_section) {
+		case OFDM:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][0] = value;
+			break;
+		case HT_MCS0_MCS7:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][1] = value;
+			break;
+		case HT_MCS8_MCS15:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][2] = value;
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][3] = value;
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			rtlphy->txpwr_by_rate_base_5g[path][txnum][4] = value;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				"Invalid RateSection %d in Band 5G, Rf Path %d, %dTx in PHY_SetTxPowerByRateBase()\n",
+				rate_section, path, txnum);
+			break;
+		};
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			"Invalid Band %d in PHY_SetTxPowerByRateBase()\n", band);
+	}
+}
+
+static u8 _rtl8821ae_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw,
+						  u8 band, u8 path,
+						  u8 txnum, u8 rate_section)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 value = 0;
+
+	if (path > RF90_PATH_D) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n",
+			 path);
+		return 0;
+	}
+
+	if (band == BAND_ON_2_4G) {
+		switch (rate_section) {
+		case CCK:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][0];
+			break;
+		case OFDM:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][1];
+			break;
+		case HT_MCS0_MCS7:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][2];
+			break;
+		case HT_MCS8_MCS15:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][4];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][5];
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+				 rate_section, path, txnum);
+			break;
+		};
+	} else if (band == BAND_ON_5G) {
+		switch (rate_section) {
+		case OFDM:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][0];
+			break;
+		case HT_MCS0_MCS7:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][1];
+			break;
+		case HT_MCS8_MCS15:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][2];
+			break;
+		case VHT_1SSMCS0_1SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][3];
+			break;
+		case VHT_2SSMCS0_2SSMCS9:
+			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][4];
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				 "Invalid RateSection %d in Band 5G, Rf Path %d, %dTx in PHY_GetTxPowerByRateBase()\n",
+				 rate_section, path, txnum);
+			break;
+		};
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "Invalid Band %d in PHY_GetTxPowerByRateBase()\n", band);
+	}
+
+	return value;
+}
+
+static void _rtl8821ae_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u16 rawValue = 0;
+	u8 base = 0, path = 0;
+
+	for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][0] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, CCK, RF_1TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][2] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, OFDM, RF_1TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][4] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS0_MCS7, RF_1TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][6] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS8_MCS15, RF_2TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][8] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][11] >> 8) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][2] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, OFDM, RF_1TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][4] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, HT_MCS0_MCS7, RF_1TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_2TX][6] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, HT_MCS8_MCS15, RF_2TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][8] >> 24) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base);
+
+		rawValue = (u16)(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_2TX][11] >> 8) & 0xFF;
+		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
+		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base);
+	}
+}
+
+static void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
+						u8 end, u8 base_val)
+{
+	char i = 0;
+	u8 temp_value = 0;
+	u32 temp_data = 0;
+
+	for (i = 3; i >= 0; --i) {
+		if (i >= start && i <= end) {
+			/* Get the exact value */
+			temp_value = (u8)(*data >> (i * 8)) & 0xF;
+			temp_value += ((u8)((*data >> (i * 8 + 4)) & 0xF)) * 10;
+
+			/* Change the value to a relative value */
+			temp_value = (temp_value > base_val) ? temp_value -
+					base_val : base_val - temp_value;
+		} else {
+			temp_value = (u8)(*data >> (i * 8)) & 0xFF;
+		}
+		temp_data <<= 8;
+		temp_data |= temp_value;
+	}
+	*data = temp_data;
+}
+
+static void _rtl8812ae_phy_cross_reference_ht_and_vht_txpower_limit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 regulation, bw, channel, rate_section;
+	char temp_pwrlmt = 0;
+
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_5G_BANDWITH_NUM; ++bw) {
+			for (channel = 0; channel < CHANNEL_MAX_NUMBER_5G; ++channel) {
+				for (rate_section = 0; rate_section < MAX_RATE_SECTION_NUM; ++rate_section) {
+					temp_pwrlmt = rtlphy->txpwr_limit_5g[regulation]
+						[bw][rate_section][channel][RF90_PATH_A];
+					if (temp_pwrlmt == MAX_POWER_INDEX) {
+						if (bw == 0 || bw == 1) { /*5G 20M 40M VHT and HT can cross reference*/
+							RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+								"No power limit table of the specified band %d, bandwidth %d, ratesection %d, channel %d, rf path %d\n",
+								1, bw, rate_section, channel, RF90_PATH_A);
+							if (rate_section == 2) {
+								rtlphy->txpwr_limit_5g[regulation][bw][2][channel][RF90_PATH_A] =
+									rtlphy->txpwr_limit_5g[regulation][bw][4][channel][RF90_PATH_A];
+							} else if (rate_section == 4) {
+								rtlphy->txpwr_limit_5g[regulation][bw][4][channel][RF90_PATH_A] =
+									rtlphy->txpwr_limit_5g[regulation][bw][2][channel][RF90_PATH_A];
+							} else if (rate_section == 3) {
+								rtlphy->txpwr_limit_5g[regulation][bw][3][channel][RF90_PATH_A] =
+									rtlphy->txpwr_limit_5g[regulation][bw][5][channel][RF90_PATH_A];
+							} else if (rate_section == 5) {
+								rtlphy->txpwr_limit_5g[regulation][bw][5][channel][RF90_PATH_A] =
+									rtlphy->txpwr_limit_5g[regulation][bw][3][channel][RF90_PATH_A];
+							}
+
+							RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "use other value %d", temp_pwrlmt);
+						}
+					}
+				}
+			}
+		}
+	}
+}
+
+static u8 _rtl8812ae_phy_get_txpower_by_rate_base_index(struct ieee80211_hw *hw,
+						   enum band_type band, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 index = 0;
+	if (band == BAND_ON_2_4G) {
+		switch (rate) {
+		case MGN_1M:
+		case MGN_2M:
+		case MGN_5_5M:
+		case MGN_11M:
+			index = 0;
+			break;
+
+		case MGN_6M:
+		case MGN_9M:
+		case MGN_12M:
+		case MGN_18M:
+		case MGN_24M:
+		case MGN_36M:
+		case MGN_48M:
+		case MGN_54M:
+			index = 1;
+			break;
+
+		case MGN_MCS0:
+		case MGN_MCS1:
+		case MGN_MCS2:
+		case MGN_MCS3:
+		case MGN_MCS4:
+		case MGN_MCS5:
+		case MGN_MCS6:
+		case MGN_MCS7:
+			index = 2;
+			break;
+
+		case MGN_MCS8:
+		case MGN_MCS9:
+		case MGN_MCS10:
+		case MGN_MCS11:
+		case MGN_MCS12:
+		case MGN_MCS13:
+		case MGN_MCS14:
+		case MGN_MCS15:
+			index = 3;
+			break;
+
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				"Wrong rate 0x%x to obtain index in 2.4G in PHY_GetTxPowerByRateBaseIndex()\n",
+				rate);
+			break;
+		}
+	} else if (band == BAND_ON_5G) {
+		switch (rate) {
+		case MGN_6M:
+		case MGN_9M:
+		case MGN_12M:
+		case MGN_18M:
+		case MGN_24M:
+		case MGN_36M:
+		case MGN_48M:
+		case MGN_54M:
+			index = 0;
+			break;
+
+		case MGN_MCS0:
+		case MGN_MCS1:
+		case MGN_MCS2:
+		case MGN_MCS3:
+		case MGN_MCS4:
+		case MGN_MCS5:
+		case MGN_MCS6:
+		case MGN_MCS7:
+			index = 1;
+			break;
+
+		case MGN_MCS8:
+		case MGN_MCS9:
+		case MGN_MCS10:
+		case MGN_MCS11:
+		case MGN_MCS12:
+		case MGN_MCS13:
+		case MGN_MCS14:
+		case MGN_MCS15:
+			index = 2;
+			break;
+
+		case MGN_VHT1SS_MCS0:
+		case MGN_VHT1SS_MCS1:
+		case MGN_VHT1SS_MCS2:
+		case MGN_VHT1SS_MCS3:
+		case MGN_VHT1SS_MCS4:
+		case MGN_VHT1SS_MCS5:
+		case MGN_VHT1SS_MCS6:
+		case MGN_VHT1SS_MCS7:
+		case MGN_VHT1SS_MCS8:
+		case MGN_VHT1SS_MCS9:
+			index = 3;
+			break;
+
+		case MGN_VHT2SS_MCS0:
+		case MGN_VHT2SS_MCS1:
+		case MGN_VHT2SS_MCS2:
+		case MGN_VHT2SS_MCS3:
+		case MGN_VHT2SS_MCS4:
+		case MGN_VHT2SS_MCS5:
+		case MGN_VHT2SS_MCS6:
+		case MGN_VHT2SS_MCS7:
+		case MGN_VHT2SS_MCS8:
+		case MGN_VHT2SS_MCS9:
+			index = 4;
+			break;
+
+		default:
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+				"Wrong rate 0x%x to obtain index in 5G in PHY_GetTxPowerByRateBaseIndex()\n",
+				rate);
+			break;
+		}
+	}
+
+	return index;
+}
+
+static void _rtl8812ae_phy_convert_txpower_limit_to_power_index(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 bw40_pwr_base_dbm2_4G, bw40_pwr_base_dbm5G;
+	u8 regulation, bw, channel, rate_section;
+	u8 base_index2_4G = 0;
+	u8 base_index5G = 0;
+	char temp_value = 0, temp_pwrlmt = 0;
+	u8 rf_path = 0;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		"=====> _rtl8812ae_phy_convert_txpower_limit_to_power_index()\n");
+
+	_rtl8812ae_phy_cross_reference_ht_and_vht_txpower_limit(hw);
+
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_2_4G_BANDWITH_NUM; ++bw) {
+			for (channel = 0; channel < CHANNEL_MAX_NUMBER_2G; ++channel) {
+				for (rate_section = 0; rate_section < MAX_RATE_SECTION_NUM; ++rate_section) {
+					/* obtain the base dBm values in 2.4G band
+					 CCK => 11M, OFDM => 54M, HT 1T => MCS7, HT 2T => MCS15*/
+					if (rate_section == 0) { /*CCK*/
+						base_index2_4G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_2_4G, MGN_11M);
+					} else if (rate_section == 1) { /*OFDM*/
+						base_index2_4G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_2_4G, MGN_54M);
+					} else if (rate_section == 2) { /*HT IT*/
+						base_index2_4G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_2_4G, MGN_MCS7);
+					} else if (rate_section == 3) { /*HT 2T*/
+						base_index2_4G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_2_4G, MGN_MCS15);
+					}
+
+					temp_pwrlmt = rtlphy->txpwr_limit_2_4g[regulation]
+						[bw][rate_section][channel][RF90_PATH_A];
+
+					for (rf_path = RF90_PATH_A;
+						rf_path < MAX_RF_PATH_NUM;
+						++rf_path) {
+						if (rate_section == 3)
+							bw40_pwr_base_dbm2_4G =
+							rtlphy->txpwr_by_rate_base_24g[rf_path][RF_2TX][base_index2_4G];
+						else
+							bw40_pwr_base_dbm2_4G =
+							rtlphy->txpwr_by_rate_base_24g[rf_path][RF_1TX][base_index2_4G];
+
+						if (temp_pwrlmt != MAX_POWER_INDEX) {
+							temp_value = temp_pwrlmt - bw40_pwr_base_dbm2_4G;
+							rtlphy->txpwr_limit_2_4g[regulation]
+								[bw][rate_section][channel][rf_path] =
+								temp_value;
+						}
+
+						RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+							"TxPwrLimit_2_4G[regulation %d][bw %d][rateSection %d][channel %d] = %d\n(TxPwrLimit in dBm %d - BW40PwrLmt2_4G[channel %d][rfPath %d] %d)\n",
+							regulation, bw, rate_section, channel,
+							rtlphy->txpwr_limit_2_4g[regulation][bw]
+							[rate_section][channel][rf_path], (temp_pwrlmt == 63)
+							? 0 : temp_pwrlmt/2, channel, rf_path,
+							bw40_pwr_base_dbm2_4G);
+					}
+				}
+			}
+		}
+	}
+	for (regulation = 0; regulation < MAX_REGULATION_NUM; ++regulation) {
+		for (bw = 0; bw < MAX_5G_BANDWITH_NUM; ++bw) {
+			for (channel = 0; channel < CHANNEL_MAX_NUMBER_5G; ++channel) {
+				for (rate_section = 0; rate_section < MAX_RATE_SECTION_NUM; ++rate_section) {
+					/* obtain the base dBm values in 5G band
+					 OFDM => 54M, HT 1T => MCS7, HT 2T => MCS15,
+					VHT => 1SSMCS7, VHT 2T => 2SSMCS7*/
+					if (rate_section == 1) { /*OFDM*/
+						base_index5G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_5G, MGN_54M);
+					} else if (rate_section == 2) { /*HT 1T*/
+						base_index5G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_5G, MGN_MCS7);
+					} else if (rate_section == 3) { /*HT 2T*/
+						base_index5G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_5G, MGN_MCS15);
+					} else if (rate_section == 4) { /*VHT 1T*/
+						base_index5G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_5G, MGN_VHT1SS_MCS7);
+					} else if (rate_section == 5) { /*VHT 2T*/
+						base_index5G =
+							_rtl8812ae_phy_get_txpower_by_rate_base_index(hw,
+							BAND_ON_5G, MGN_VHT2SS_MCS7);
+					}
+
+					temp_pwrlmt = rtlphy->txpwr_limit_5g[regulation]
+						[bw][rate_section][channel]
+						[RF90_PATH_A];
+
+					for (rf_path = RF90_PATH_A;
+					     rf_path < MAX_RF_PATH_NUM;
+					     ++rf_path) {
+						if (rate_section == 3 || rate_section == 5)
+							bw40_pwr_base_dbm5G =
+							rtlphy->txpwr_by_rate_base_5g[rf_path]
+							[RF_2TX][base_index5G];
+						else
+							bw40_pwr_base_dbm5G =
+							rtlphy->txpwr_by_rate_base_5g[rf_path]
+							[RF_1TX][base_index5G];
+
+						if (temp_pwrlmt != MAX_POWER_INDEX) {
+							temp_value =
+								temp_pwrlmt - bw40_pwr_base_dbm5G;
+							rtlphy->txpwr_limit_5g[regulation]
+								[bw][rate_section][channel]
+								[rf_path] = temp_value;
+						}
+
+						RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+							"TxPwrLimit_5G[regulation %d][bw %d][rateSection %d][channel %d] =%d\n(TxPwrLimit in dBm %d - BW40PwrLmt5G[chnl group %d][rfPath %d] %d)\n",
+							regulation, bw, rate_section,
+							channel, rtlphy->txpwr_limit_5g[regulation]
+							[bw][rate_section][channel][rf_path],
+							temp_pwrlmt, channel, rf_path, bw40_pwr_base_dbm5G);
+					}
+				}
+			}
+		}
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "<===== _rtl8812ae_phy_convert_txpower_limit_to_power_index()\n");
+}
+
+static void _rtl8821ae_phy_init_txpower_limit(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 i, j, k, l, m;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "=====> _rtl8821ae_phy_init_txpower_limit()!\n");
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		for (j = 0; j < MAX_2_4G_BANDWITH_NUM; ++j)
+			for (k = 0; k < MAX_RATE_SECTION_NUM; ++k)
+				for (m = 0; m < CHANNEL_MAX_NUMBER_2G; ++m)
+					for (l = 0; l < MAX_RF_PATH_NUM; ++l)
+						rtlphy->txpwr_limit_2_4g
+								[i][j][k][m][l]
+							= MAX_POWER_INDEX;
+	}
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		for (j = 0; j < MAX_5G_BANDWITH_NUM; ++j)
+			for (k = 0; k < MAX_RATE_SECTION_NUM; ++k)
+				for (m = 0; m < CHANNEL_MAX_NUMBER_5G; ++m)
+					for (l = 0; l < MAX_RF_PATH_NUM; ++l)
+						rtlphy->txpwr_limit_5g
+								[i][j][k][m][l]
+							= MAX_POWER_INDEX;
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "<===== _rtl8821ae_phy_init_txpower_limit()!\n");
+}
+
+static void _rtl8821ae_phy_convert_txpower_dbm_to_relative_value(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 base = 0, rfPath = 0;
+
+	for (rfPath = RF90_PATH_A; rfPath <= RF90_PATH_B; ++rfPath) {
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, CCK);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][0],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, OFDM);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][1],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][2],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, HT_MCS0_MCS7);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][3],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][4],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_2TX, HT_MCS8_MCS15);
+
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][5],
+			0, 3, base);
+
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][6],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, VHT_1SSMCS0_1SSMCS9);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][7],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][8],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][9],
+			0, 1, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_2TX, VHT_2SSMCS0_2SSMCS9);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][9],
+			2, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][10],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][11],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, OFDM);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][1],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][2],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, HT_MCS0_MCS7);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][3],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][4],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_2TX, HT_MCS8_MCS15);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][5],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][6],
+			0, 3, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, VHT_1SSMCS0_1SSMCS9);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][7],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][8],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][9],
+			0, 1, base);
+
+		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_2TX, VHT_2SSMCS0_2SSMCS9);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][9],
+			2, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][10],
+			0, 3, base);
+		_phy_convert_txpower_dbm_to_relative_value(
+			&rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][11],
+			0, 3, base);
+	}
+
+	RT_TRACE(rtlpriv, COMP_POWER, DBG_TRACE,
+		"<===_rtl8821ae_phy_convert_txpower_dbm_to_relative_value()\n");
+}
+
+static void _rtl8821ae_phy_txpower_by_rate_configuration(struct ieee80211_hw *hw)
+{
+	_rtl8821ae_phy_store_txpower_by_rate_base(hw);
+	_rtl8821ae_phy_convert_txpower_dbm_to_relative_value(hw);
+}
+
+/* string is in decimal */
+static bool _rtl8812ae_get_integer_from_string(char *str, u8 *pint)
+{
+	u16 i = 0;
+	*pint = 0;
+
+	while (str[i] != '\0') {
+		if (str[i] >= '0' && str[i] <= '9') {
+			*pint *= 10;
+			*pint += (str[i] - '0');
+		} else {
+			return false;
+		}
+		++i;
+	}
+
+	return true;
+}
+
+static bool _rtl8812ae_eq_n_byte(u8 *str1, u8 *str2, u32 num)
+{
+	if (num == 0)
+		return false;
+	while (num > 0) {
+		num--;
+		if (str1[num] != str2[num])
+			return false;
+	}
+	return true;
+}
+
+static char _rtl8812ae_phy_get_chnl_idx_of_txpwr_lmt(struct ieee80211_hw *hw,
+					      u8 band, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	char channel_index = -1;
+	u8 channel_5g[CHANNEL_MAX_NUMBER_5G] = {
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64,
+		100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122,
+		124, 126, 128, 130, 132, 134, 136, 138, 140, 142, 144, 149,
+		151, 153, 155, 157, 159, 161, 163, 165, 167, 168, 169, 171,
+		173, 175, 177};
+	u8  i = 0;
+	if (band == BAND_ON_2_4G)
+		channel_index = channel - 1;
+	else if (band == BAND_ON_5G) {
+		for (i = 0; i < sizeof(channel_5g)/sizeof(u8); ++i) {
+			if (channel_5g[i] == channel)
+				channel_index = i;
+		}
+	} else
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD, "Invalid Band %d in %s",
+			 band,  __func__);
+
+	if (channel_index == -1)
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "Invalid Channel %d of Band %d in %s", channel,
+			 band, __func__);
+
+	return channel_index;
+}
+
+static void _rtl8812ae_phy_set_txpower_limit(struct ieee80211_hw *hw, u8 *pregulation,
+				      u8 *pband, u8 *pbandwidth,
+				      u8 *prate_section, u8 *prf_path,
+				      u8 *pchannel, u8 *ppower_limit)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 regulation = 0, bandwidth = 0, rate_section = 0, channel;
+	u8 channel_index;
+	char power_limit = 0, prev_power_limit, ret;
+
+	if (!_rtl8812ae_get_integer_from_string((char *)pchannel, &channel) ||
+	    !_rtl8812ae_get_integer_from_string((char *)ppower_limit,
+						&power_limit)) {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Illegal index of pwr_lmt table [chnl %d][val %d]\n",
+			  channel, power_limit);
+	}
+
+	power_limit = power_limit > MAX_POWER_INDEX ?
+		      MAX_POWER_INDEX : power_limit;
+
+	if (_rtl8812ae_eq_n_byte(pregulation, (u8 *)("FCC"), 3))
+		regulation = 0;
+	else if (_rtl8812ae_eq_n_byte(pregulation, (u8 *)("MKK"), 3))
+		regulation = 1;
+	else if (_rtl8812ae_eq_n_byte(pregulation, (u8 *)("ETSI"), 4))
+		regulation = 2;
+	else if (_rtl8812ae_eq_n_byte(pregulation, (u8 *)("WW13"), 4))
+		regulation = 3;
+
+	if (_rtl8812ae_eq_n_byte(prate_section, (u8 *)("CCK"), 3))
+		rate_section = 0;
+	else if (_rtl8812ae_eq_n_byte(prate_section, (u8 *)("OFDM"), 4))
+		rate_section = 1;
+	else if (_rtl8812ae_eq_n_byte(prate_section, (u8 *)("HT"), 2) &&
+		 _rtl8812ae_eq_n_byte(prf_path, (u8 *)("1T"), 2))
+		rate_section = 2;
+	else if (_rtl8812ae_eq_n_byte(prate_section, (u8 *)("HT"), 2) &&
+		 _rtl8812ae_eq_n_byte(prf_path, (u8 *)("2T"), 2))
+		rate_section = 3;
+	else if (_rtl8812ae_eq_n_byte(prate_section, (u8 *)("VHT"), 3) &&
+		 _rtl8812ae_eq_n_byte(prf_path, (u8 *)("1T"), 2))
+		rate_section = 4;
+	else if (_rtl8812ae_eq_n_byte(prate_section, (u8 *)("VHT"), 3) &&
+		 _rtl8812ae_eq_n_byte(prf_path, (u8 *)("2T"), 2))
+		rate_section = 5;
+
+	if (_rtl8812ae_eq_n_byte(pbandwidth, (u8 *)("20M"), 3))
+		bandwidth = 0;
+	else if (_rtl8812ae_eq_n_byte(pbandwidth, (u8 *)("40M"), 3))
+		bandwidth = 1;
+	else if (_rtl8812ae_eq_n_byte(pbandwidth, (u8 *)("80M"), 3))
+		bandwidth = 2;
+	else if (_rtl8812ae_eq_n_byte(pbandwidth, (u8 *)("160M"), 4))
+		bandwidth = 3;
+
+	if (_rtl8812ae_eq_n_byte(pband, (u8 *)("2.4G"), 4)) {
+		ret = _rtl8812ae_phy_get_chnl_idx_of_txpwr_lmt(hw,
+							       BAND_ON_2_4G,
+							       channel);
+
+		if (ret == -1)
+			return;
+
+		channel_index = ret;
+
+		prev_power_limit = rtlphy->txpwr_limit_2_4g[regulation]
+						[bandwidth][rate_section]
+						[channel_index][RF90_PATH_A];
+
+		if (power_limit < prev_power_limit)
+			rtlphy->txpwr_limit_2_4g[regulation][bandwidth]
+				[rate_section][channel_index][RF90_PATH_A] =
+								   power_limit;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "2.4G [regula %d][bw %d][sec %d][chnl %d][val %d]\n",
+			  regulation, bandwidth, rate_section, channel_index,
+			  rtlphy->txpwr_limit_2_4g[regulation][bandwidth]
+				[rate_section][channel_index][RF90_PATH_A]);
+	} else if (_rtl8812ae_eq_n_byte(pband, (u8 *)("5G"), 2)) {
+		ret = _rtl8812ae_phy_get_chnl_idx_of_txpwr_lmt(hw,
+							       BAND_ON_5G,
+							       channel);
+
+		if (ret == -1)
+			return;
+
+		channel_index = ret;
+
+		prev_power_limit = rtlphy->txpwr_limit_5g[regulation][bandwidth]
+						[rate_section][channel_index]
+						[RF90_PATH_A];
+
+		if (power_limit < prev_power_limit)
+			rtlphy->txpwr_limit_5g[regulation][bandwidth]
+			[rate_section][channel_index][RF90_PATH_A] = power_limit;
+
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "5G: [regul %d][bw %d][sec %d][chnl %d][val %d]\n",
+			  regulation, bandwidth, rate_section, channel,
+			  rtlphy->txpwr_limit_5g[regulation][bandwidth]
+				[rate_section][channel_index][RF90_PATH_A]);
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+			 "Cannot recognize the band info in %s\n", pband);
+		return;
+	}
+}
+
+static void _rtl8812ae_phy_config_bb_txpwr_lmt(struct ieee80211_hw *hw,
+					  u8 *regulation, u8 *band,
+					  u8 *bandwidth, u8 *rate_section,
+					  u8 *rf_path, u8 *channel,
+					  u8 *power_limit)
+{
+	_rtl8812ae_phy_set_txpower_limit(hw, regulation, band, bandwidth,
+					 rate_section, rf_path, channel,
+					 power_limit);
+}
+
+static void _rtl8821ae_phy_read_and_config_txpwr_lmt(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u32 i = 0;
+	u32 array_len;
+	u8 **array;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		array_len = RTL8812AE_TXPWR_LMT_ARRAY_LEN;
+		array = RTL8812AE_TXPWR_LMT;
+	} else {
+		array_len = RTL8821AE_TXPWR_LMT_ARRAY_LEN;
+		array = RTL8821AE_TXPWR_LMT;
+	}
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "\n");
+
+	for (i = 0; i < array_len; i += 7) {
+		u8 *regulation = array[i];
+		u8 *band = array[i+1];
+		u8 *bandwidth = array[i+2];
+		u8 *rate = array[i+3];
+		u8 *rf_path = array[i+4];
+		u8 *chnl = array[i+5];
+		u8 *val = array[i+6];
+
+		_rtl8812ae_phy_config_bb_txpwr_lmt(hw, regulation, band,
+						   bandwidth, rate, rf_path,
+						   chnl, val);
+	}
+}
+
+static bool _rtl8821ae_phy_bb8821a_config_parafile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	bool rtstatus;
+
+	_rtl8821ae_phy_init_txpower_limit(hw);
+
+	/* RegEnableTxPowerLimit == 1 for 8812a & 8821a */
+	if (rtlefuse->eeprom_regulatory != 2)
+		_rtl8821ae_phy_read_and_config_txpwr_lmt(hw);
+
+	rtstatus = _rtl8821ae_phy_config_bb_with_headerfile(hw,
+						       BASEBAND_CONFIG_PHY_REG);
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "Write BB Reg Fail!!");
+		return false;
+	}
+	_rtl8821ae_phy_init_tx_power_by_rate(hw);
+	if (rtlefuse->autoload_failflag == false) {
+		rtstatus = _rtl8821ae_phy_config_bb_with_pgheaderfile(hw,
+						    BASEBAND_CONFIG_PHY_REG);
+	}
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "BB_PG Reg Fail!!");
+		return false;
+	}
+
+	_rtl8821ae_phy_txpower_by_rate_configuration(hw);
+
+	/* RegEnableTxPowerLimit == 1 for 8812a & 8821a */
+	if (rtlefuse->eeprom_regulatory != 2)
+		_rtl8812ae_phy_convert_txpower_limit_to_power_index(hw);
+
+	rtstatus = _rtl8821ae_phy_config_bb_with_headerfile(hw,
+						BASEBAND_CONFIG_AGC_TAB);
+
+	if (rtstatus != true) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG, "AGC Table Fail\n");
+		return false;
+	}
+	rtlphy->cck_high_power = (bool)(rtl_get_bbreg(hw,
+			RFPGA0_XA_HSSIPARAMETER2, 0x200));
+	return true;
+}
+
+static bool _rtl8821ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u32 i, v1, v2;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "Read MAC_REG_Array\n");
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+		arraylength = RTL8821AEMAC_1T_ARRAYLEN;
+		ptrarray = RTL8821AE_MAC_REG_ARRAY;
+	} else {
+		arraylength = RTL8812AEMAC_1T_ARRAYLEN;
+		ptrarray = RTL8812AE_MAC_REG_ARRAY;
+	}
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Img: MAC_REG_ARRAY LEN %d\n", arraylength);
+	for (i = 0; i < arraylength; i += 2) {
+		v1 = ptrarray[i];
+		v2 = (u8)ptrarray[i + 1];
+		if (v1 < 0xCDCDCDCD) {
+			rtl_write_byte(rtlpriv, v1, (u8)v2);
+			continue;
+		} else {
+			if (!_rtl8821ae_check_condition(hw, v1)) {
+				/*Discard the following (offset, data) pairs*/
+				READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylength - 2) {
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				}
+				i -= 2; /* prevent from for-loop += 2*/
+			} else {/*Configure matched pairs and skip to end of if-else.*/
+				READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				while (v2 != 0xDEAD &&
+				       v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < arraylength - 2) {
+					rtl_write_byte(rtlpriv, v1, v2);
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < arraylength - 2)
+					READ_NEXT_PAIR(ptrarray, v1, v2, i);
+			}
+		}
+	}
+	return true;
+}
+
+static bool _rtl8821ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
+						     u8 configtype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	int i;
+	u32 *array_table;
+	u16 arraylen;
+	u32 v1 = 0, v2 = 0;
+
+	if (configtype == BASEBAND_CONFIG_PHY_REG) {
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			arraylen = RTL8812AEPHY_REG_1TARRAYLEN;
+			array_table = RTL8812AE_PHY_REG_ARRAY;
+		} else {
+			arraylen = RTL8821AEPHY_REG_1TARRAYLEN;
+			array_table = RTL8821AE_PHY_REG_ARRAY;
+		}
+
+		for (i = 0; i < arraylen; i += 2) {
+			v1 = array_table[i];
+			v2 = array_table[i + 1];
+			if (v1 < 0xCDCDCDCD) {
+				_rtl8821ae_config_bb_reg(hw, v1, v2);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				if (!_rtl8821ae_check_condition(hw, v1)) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_PAIR(array_table, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						READ_NEXT_PAIR(array_table, v1,
+								v2, i);
+					}
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_PAIR(array_table, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						_rtl8821ae_config_bb_reg(hw, v1,
+									 v2);
+						READ_NEXT_PAIR(array_table, v1,
+							       v2, i);
+					}
+
+					while (v2 != 0xDEAD &&
+					       i < arraylen - 2) {
+						READ_NEXT_PAIR(array_table, v1,
+							       v2, i);
+					}
+				}
+			}
+		}
+	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
+		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+			arraylen = RTL8812AEAGCTAB_1TARRAYLEN;
+			array_table = RTL8812AE_AGC_TAB_ARRAY;
+		} else {
+			arraylen = RTL8821AEAGCTAB_1TARRAYLEN;
+			array_table = RTL8821AE_AGC_TAB_ARRAY;
+		}
+
+		for (i = 0; i < arraylen; i = i + 2) {
+			v1 = array_table[i];
+			v2 = array_table[i+1];
+			if (v1 < 0xCDCDCDCD) {
+				rtl_set_bbreg(hw, v1, MASKDWORD, v2);
+				udelay(1);
+				continue;
+			} else {/*This line is the start line of branch.*/
+				if (!_rtl8821ae_check_condition(hw, v1)) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_PAIR(array_table, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						READ_NEXT_PAIR(array_table, v1,
+								v2, i);
+					}
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_PAIR(array_table, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD &&
+					       i < arraylen - 2) {
+						rtl_set_bbreg(hw, v1, MASKDWORD,
+							      v2);
+						udelay(1);
+						READ_NEXT_PAIR(array_table, v1,
+							       v2, i);
+					}
+
+					while (v2 != 0xDEAD &&
+						i < arraylen - 2) {
+						READ_NEXT_PAIR(array_table, v1,
+								v2, i);
+					}
+				}
+				RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+					 "The agctab_array_table[0] is %x Rtl818EEPHY_REGArray[1] is %x\n",
+					  array_table[i],  array_table[i + 1]);
+			}
+		}
+	}
+	return true;
+}
+
+static u8 _rtl8821ae_get_rate_section_index(u32 regaddr)
+{
+	u8 index = 0;
+	regaddr &= 0xFFF;
+	if (regaddr >= 0xC20 && regaddr <= 0xC4C)
+		index = (u8)((regaddr - 0xC20) / 4);
+	else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
+		index = (u8)((regaddr - 0xE20) / 4);
+	else
+		RT_ASSERT(!COMP_INIT,
+			  "Invalid RegAddr 0x%x\n", regaddr);
+	return index;
+}
+
+static void _rtl8821ae_store_tx_power_by_rate(struct ieee80211_hw *hw,
+					      u32 band, u32 rfpath,
+					      u32 txnum, u32 regaddr,
+					      u32 bitmask, u32 data)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 rate_section = _rtl8821ae_get_rate_section_index(regaddr);
+
+	if (band != BAND_ON_2_4G && band != BAND_ON_5G)
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_WARNING, "Invalid Band %d\n", band);
+
+	if (rfpath >= MAX_RF_PATH)
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_WARNING, "Invalid RfPath %d\n", rfpath);
+
+	if (txnum >= MAX_RF_PATH)
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_WARNING, "Invalid TxNum %d\n", txnum);
+
+	rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] = data;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "TxPwrByRateOffset[Band %d][RfPath %d][TxNum %d][RateSection %d] = 0x%x\n",
+		 band, rfpath, txnum, rate_section,
+		 rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section]);
+}
+
+static bool _rtl8821ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
+							u8 configtype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	int i;
+	u32 *array;
+	u16 arraylen;
+	u32 v1, v2, v3, v4, v5, v6;
+
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
+		arraylen = RTL8812AEPHY_REG_ARRAY_PGLEN;
+		array = RTL8812AE_PHY_REG_ARRAY_PG;
+	} else {
+		arraylen = RTL8821AEPHY_REG_ARRAY_PGLEN;
+		array = RTL8821AE_PHY_REG_ARRAY_PG;
+	}
+
+	if (configtype != BASEBAND_CONFIG_PHY_REG) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "configtype != BaseBand_Config_PHY_REG\n");
+		return true;
+	}
+	for (i = 0; i < arraylen; i += 6) {
+		v1 = array[i];
+		v2 = array[i+1];
+		v3 = array[i+2];
+		v4 = array[i+3];
+		v5 = array[i+4];
+		v6 = array[i+5];
+
+		if (v1 < 0xCDCDCDCD) {
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE &&
+				(v4 == 0xfe || v4 == 0xffe)) {
+				msleep(50);
+				continue;
+			}
+
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+				if (v4 == 0xfe)
+					msleep(50);
+				else if (v4 == 0xfd)
+					mdelay(5);
+				else if (v4 == 0xfc)
+					mdelay(1);
+				else if (v4 == 0xfb)
+					udelay(50);
+				else if (v4 == 0xfa)
+					udelay(5);
+				else if (v4 == 0xf9)
+					udelay(1);
+			}
+			_rtl8821ae_store_tx_power_by_rate(hw, v1, v2, v3,
+							  v4, v5, v6);
+			continue;
+		} else {
+			 /*don't need the hw_body*/
+			if (!_rtl8821ae_check_condition(hw, v1)) {
+				i += 2; /* skip the pair of expression*/
+				v1 = array[i];
+				v2 = array[i+1];
+				v3 = array[i+2];
+				while (v2 != 0xDEAD) {
+					i += 3;
+					v1 = array[i];
+					v2 = array[i+1];
+					v3 = array[i+2];
+				}
+			}
+		}
+	}
+
+	return true;
+}
+
+bool rtl8812ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+					     enum radio_path rfpath)
+{
+	int i;
+	bool rtstatus = true;
+	u32 *radioa_array_table_a, *radioa_array_table_b;
+	u16 radioa_arraylen_a, radioa_arraylen_b;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 v1 = 0, v2 = 0;
+
+	radioa_arraylen_a = RTL8812AE_RADIOA_1TARRAYLEN;
+	radioa_array_table_a = RTL8812AE_RADIOA_ARRAY;
+	radioa_arraylen_b = RTL8812AE_RADIOB_1TARRAYLEN;
+	radioa_array_table_b = RTL8812AE_RADIOB_ARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Radio_A:RTL8821AE_RADIOA_ARRAY %d\n", radioa_arraylen_a);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
+	rtstatus = true;
+	switch (rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < radioa_arraylen_a; i = i + 2) {
+			v1 = radioa_array_table_a[i];
+			v2 = radioa_array_table_a[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl8821ae_config_rf_radio_a(hw, v1, v2);
+				continue;
+			} else{/*This line is the start line of branch.*/
+				if (!_rtl8821ae_check_condition(hw, v1)) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_PAIR(radioa_array_table_a, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_a-2)
+						READ_NEXT_PAIR(radioa_array_table_a, v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_PAIR(radioa_array_table_a, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_a - 2) {
+						_rtl8821ae_config_rf_radio_a(hw, v1, v2);
+						READ_NEXT_PAIR(radioa_array_table_a, v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < radioa_arraylen_a-2)
+						READ_NEXT_PAIR(radioa_array_table_a, v1, v2, i);
+
+				}
+			}
+		}
+		break;
+	case RF90_PATH_B:
+		for (i = 0; i < radioa_arraylen_b; i = i + 2) {
+			v1 = radioa_array_table_b[i];
+			v2 = radioa_array_table_b[i+1];
+			if (v1 < 0xcdcdcdcd) {
+				_rtl8821ae_config_rf_radio_b(hw, v1, v2);
+				continue;
+			} else{/*This line is the start line of branch.*/
+				if (!_rtl8821ae_check_condition(hw, v1)) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_PAIR(radioa_array_table_b, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_b-2)
+						READ_NEXT_PAIR(radioa_array_table_b, v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_PAIR(radioa_array_table_b, v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen_b-2) {
+						_rtl8821ae_config_rf_radio_b(hw, v1, v2);
+						READ_NEXT_PAIR(radioa_array_table_b, v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < radioa_arraylen_b-2)
+						READ_NEXT_PAIR(radioa_array_table_b, v1, v2, i);
+				}
+			}
+		}
+		break;
+	case RF90_PATH_C:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	case RF90_PATH_D:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	}
+	return true;
+}
+
+bool rtl8821ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
+						enum radio_path rfpath)
+{
+	#define READ_NEXT_RF_PAIR(v1, v2, i) \
+	do { \
+		i += 2; \
+		v1 = radioa_array_table[i]; \
+		v2 = radioa_array_table[i+1]; \
+	} \
+	while (0)
+
+	int i;
+	bool rtstatus = true;
+	u32 *radioa_array_table;
+	u16 radioa_arraylen;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	/* struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw)); */
+	u32 v1 = 0, v2 = 0;
+
+	radioa_arraylen = RTL8821AE_RADIOA_1TARRAYLEN;
+	radioa_array_table = RTL8821AE_RADIOA_ARRAY;
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+		 "Radio_A:RTL8821AE_RADIOA_ARRAY %d\n", radioa_arraylen);
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "Radio No %x\n", rfpath);
+	rtstatus = true;
+	switch (rfpath) {
+	case RF90_PATH_A:
+		for (i = 0; i < radioa_arraylen; i = i + 2) {
+			v1 = radioa_array_table[i];
+			v2 = radioa_array_table[i+1];
+			if (v1 < 0xcdcdcdcd)
+				_rtl8821ae_config_rf_radio_a(hw, v1, v2);
+			else{/*This line is the start line of branch.*/
+				if (!_rtl8821ae_check_condition(hw, v1)) {
+					/*Discard the following (offset, data) pairs*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+						v2 != 0xCDEF &&
+						v2 != 0xCDCD && i < radioa_arraylen - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+
+					i -= 2; /* prevent from for-loop += 2*/
+				} else {/*Configure matched pairs and skip to end of if-else.*/
+					READ_NEXT_RF_PAIR(v1, v2, i);
+					while (v2 != 0xDEAD &&
+					       v2 != 0xCDEF &&
+					       v2 != 0xCDCD && i < radioa_arraylen - 2) {
+						_rtl8821ae_config_rf_radio_a(hw, v1, v2);
+						READ_NEXT_RF_PAIR(v1, v2, i);
+					}
+
+					while (v2 != 0xDEAD && i < radioa_arraylen - 2)
+						READ_NEXT_RF_PAIR(v1, v2, i);
+				}
+			}
+		}
+		break;
+
+	case RF90_PATH_B:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	case RF90_PATH_C:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	case RF90_PATH_D:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	}
+	return true;
+}
+
+void rtl8821ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	rtlphy->default_initialgain[0] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[1] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[2] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
+	rtlphy->default_initialgain[3] =
+	    (u8)rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default initial gain (c50=0x%x, c58=0x%x, c60=0x%x, c68=0x%x\n",
+		  rtlphy->default_initialgain[0],
+		  rtlphy->default_initialgain[1],
+		  rtlphy->default_initialgain[2],
+		  rtlphy->default_initialgain[3]);
+
+	rtlphy->framesync = (u8)rtl_get_bbreg(hw,
+					       ROFDM0_RXDETECTOR3, MASKBYTE0);
+	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
+					      ROFDM0_RXDETECTOR2, MASKDWORD);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+		 "Default framesync (0x%x) = 0x%x\n",
+		  ROFDM0_RXDETECTOR3, rtlphy->framesync);
+}
+
+static void phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset = RA_LSSIWRITE_8821A;
+	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset = RB_LSSIWRITE_8821A;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RHSSIREAD_8821AE;
+	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RHSSIREAD_8821AE;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rb = RA_SIREAD_8821A;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rb = RB_SIREAD_8821A;
+
+	rtlphy->phyreg_def[RF90_PATH_A].rf_rbpi = RA_PIREAD_8821A;
+	rtlphy->phyreg_def[RF90_PATH_B].rf_rbpi = RB_PIREAD_8821A;
+}
+
+void rtl8821ae_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 txpwr_level;
+	long txpwr_dbm;
+
+	txpwr_level = rtlphy->cur_cck_txpwridx;
+	txpwr_dbm = _rtl8821ae_phy_txpwr_idx_to_dbm(hw,
+						 WIRELESS_MODE_B, txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl8821ae_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl8821ae_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
+						 txpwr_level);
+	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
+	if (_rtl8821ae_phy_txpwr_idx_to_dbm(hw,
+					 WIRELESS_MODE_N_24G,
+					 txpwr_level) > txpwr_dbm)
+		txpwr_dbm =
+		    _rtl8821ae_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
+						 txpwr_level);
+	*powerlevel = txpwr_dbm;
+}
+
+static bool _rtl8821ae_phy_get_chnl_index(u8 channel, u8 *chnl_index)
+{
+	u8 channel_5g[CHANNEL_MAX_NUMBER_5G] = {
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62,
+		64, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118,
+		120, 122, 124, 126, 128, 130, 132, 134, 136, 138, 140,
+		142, 144, 149, 151, 153, 155, 157, 159, 161, 163, 165,
+		167, 168, 169, 171, 173, 175, 177
+	};
+	u8 i = 0;
+	bool in_24g = true;
+
+	if (channel <= 14) {
+		in_24g = true;
+		*chnl_index = channel - 1;
+	} else {
+		in_24g = false;
+
+		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; ++i) {
+			if (channel_5g[i] == channel) {
+				*chnl_index = i;
+				return in_24g;
+			}
+		}
+	}
+	return in_24g;
+}
+
+static char _rtl8821ae_phy_get_ratesection_intxpower_byrate(u8 path, u8 rate)
+{
+	char rate_section = 0;
+	switch (rate) {
+	case DESC_RATE1M:
+	case DESC_RATE2M:
+	case DESC_RATE5_5M:
+	case DESC_RATE11M:
+		rate_section = 0;
+		break;
+	case DESC_RATE6M:
+	case DESC_RATE9M:
+	case DESC_RATE12M:
+	case DESC_RATE18M:
+		rate_section = 1;
+		break;
+	case DESC_RATE24M:
+	case DESC_RATE36M:
+	case DESC_RATE48M:
+	case DESC_RATE54M:
+		rate_section = 2;
+		break;
+	case DESC_RATEMCS0:
+	case DESC_RATEMCS1:
+	case DESC_RATEMCS2:
+	case DESC_RATEMCS3:
+		rate_section = 3;
+		break;
+	case DESC_RATEMCS4:
+	case DESC_RATEMCS5:
+	case DESC_RATEMCS6:
+	case DESC_RATEMCS7:
+		rate_section = 4;
+		break;
+	case DESC_RATEMCS8:
+	case DESC_RATEMCS9:
+	case DESC_RATEMCS10:
+	case DESC_RATEMCS11:
+		rate_section = 5;
+		break;
+	case DESC_RATEMCS12:
+	case DESC_RATEMCS13:
+	case DESC_RATEMCS14:
+	case DESC_RATEMCS15:
+		rate_section = 6;
+		break;
+	case DESC_RATEVHT1SS_MCS0:
+	case DESC_RATEVHT1SS_MCS1:
+	case DESC_RATEVHT1SS_MCS2:
+	case DESC_RATEVHT1SS_MCS3:
+		rate_section = 7;
+		break;
+	case DESC_RATEVHT1SS_MCS4:
+	case DESC_RATEVHT1SS_MCS5:
+	case DESC_RATEVHT1SS_MCS6:
+	case DESC_RATEVHT1SS_MCS7:
+		rate_section = 8;
+		break;
+	case DESC_RATEVHT1SS_MCS8:
+	case DESC_RATEVHT1SS_MCS9:
+	case DESC_RATEVHT2SS_MCS0:
+	case DESC_RATEVHT2SS_MCS1:
+		rate_section = 9;
+		break;
+	case DESC_RATEVHT2SS_MCS2:
+	case DESC_RATEVHT2SS_MCS3:
+	case DESC_RATEVHT2SS_MCS4:
+	case DESC_RATEVHT2SS_MCS5:
+		rate_section = 10;
+		break;
+	case DESC_RATEVHT2SS_MCS6:
+	case DESC_RATEVHT2SS_MCS7:
+	case DESC_RATEVHT2SS_MCS8:
+	case DESC_RATEVHT2SS_MCS9:
+		rate_section = 11;
+		break;
+	default:
+		RT_ASSERT(true, "Rate_Section is Illegal\n");
+		break;
+	}
+
+	return rate_section;
+}
+
+static char _rtl8812ae_phy_get_world_wide_limit(char  *limit_table)
+{
+	char min = limit_table[0];
+	u8 i = 0;
+
+	for (i = 0; i < MAX_REGULATION_NUM; ++i) {
+		if (limit_table[i] < min)
+			min = limit_table[i];
+	}
+	return min;
+}
+
+static char _rtl8812ae_phy_get_txpower_limit(struct ieee80211_hw *hw,
+					     u8 band,
+					     enum ht_channel_width bandwidth,
+					     enum radio_path rf_path,
+					     u8 rate, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	short band_temp = -1, regulation = -1, bandwidth_temp = -1,
+		 rate_section = -1, channel_temp = -1;
+	u16 bd, regu, bdwidth, sec, chnl;
+	char power_limit = MAX_POWER_INDEX;
+
+	if (rtlefuse->eeprom_regulatory == 2)
+		return MAX_POWER_INDEX;
+
+	regulation = TXPWR_LMT_WW;
+
+	if (band == BAND_ON_2_4G)
+		band_temp = 0;
+	else if (band == BAND_ON_5G)
+		band_temp = 1;
+
+	if (bandwidth == HT_CHANNEL_WIDTH_20)
+		bandwidth_temp = 0;
+	else if (bandwidth == HT_CHANNEL_WIDTH_20_40)
+		bandwidth_temp = 1;
+	else if (bandwidth == HT_CHANNEL_WIDTH_80)
+		bandwidth_temp = 2;
+
+	switch (rate) {
+	case DESC_RATE1M:
+	case DESC_RATE2M:
+	case DESC_RATE5_5M:
+	case DESC_RATE11M:
+		rate_section = 0;
+		break;
+	case DESC_RATE6M:
+	case DESC_RATE9M:
+	case DESC_RATE12M:
+	case DESC_RATE18M:
+	case DESC_RATE24M:
+	case DESC_RATE36M:
+	case DESC_RATE48M:
+	case DESC_RATE54M:
+		rate_section = 1;
+		break;
+	case DESC_RATEMCS0:
+	case DESC_RATEMCS1:
+	case DESC_RATEMCS2:
+	case DESC_RATEMCS3:
+	case DESC_RATEMCS4:
+	case DESC_RATEMCS5:
+	case DESC_RATEMCS6:
+	case DESC_RATEMCS7:
+		rate_section = 2;
+		break;
+	case DESC_RATEMCS8:
+	case DESC_RATEMCS9:
+	case DESC_RATEMCS10:
+	case DESC_RATEMCS11:
+	case DESC_RATEMCS12:
+	case DESC_RATEMCS13:
+	case DESC_RATEMCS14:
+	case DESC_RATEMCS15:
+		rate_section = 3;
+		break;
+	case DESC_RATEVHT1SS_MCS0:
+	case DESC_RATEVHT1SS_MCS1:
+	case DESC_RATEVHT1SS_MCS2:
+	case DESC_RATEVHT1SS_MCS3:
+	case DESC_RATEVHT1SS_MCS4:
+	case DESC_RATEVHT1SS_MCS5:
+	case DESC_RATEVHT1SS_MCS6:
+	case DESC_RATEVHT1SS_MCS7:
+	case DESC_RATEVHT1SS_MCS8:
+	case DESC_RATEVHT1SS_MCS9:
+		rate_section = 4;
+		break;
+	case DESC_RATEVHT2SS_MCS0:
+	case DESC_RATEVHT2SS_MCS1:
+	case DESC_RATEVHT2SS_MCS2:
+	case DESC_RATEVHT2SS_MCS3:
+	case DESC_RATEVHT2SS_MCS4:
+	case DESC_RATEVHT2SS_MCS5:
+	case DESC_RATEVHT2SS_MCS6:
+	case DESC_RATEVHT2SS_MCS7:
+	case DESC_RATEVHT2SS_MCS8:
+	case DESC_RATEVHT2SS_MCS9:
+		rate_section = 5;
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			"Wrong rate 0x%x\n", rate);
+		break;
+	}
+
+	if (band_temp == BAND_ON_5G  && rate_section == 0)
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "Wrong rate 0x%x: No CCK in 5G Band\n", rate);
+
+	/*workaround for wrong index combination to obtain tx power limit,
+	  OFDM only exists in BW 20M*/
+	if (rate_section == 1)
+		bandwidth_temp = 0;
+
+	/*workaround for wrong index combination to obtain tx power limit,
+	 *HT on 80M will reference to HT on 40M
+	 */
+	if ((rate_section == 2 || rate_section == 3) && band == BAND_ON_5G &&
+	    bandwidth_temp == 2)
+		bandwidth_temp = 1;
+
+	if (band == BAND_ON_2_4G)
+		channel_temp = _rtl8812ae_phy_get_chnl_idx_of_txpwr_lmt(hw,
+		BAND_ON_2_4G, channel);
+	else if (band == BAND_ON_5G)
+		channel_temp = _rtl8812ae_phy_get_chnl_idx_of_txpwr_lmt(hw,
+		BAND_ON_5G, channel);
+	else if (band == BAND_ON_BOTH)
+		;/* BAND_ON_BOTH don't care temporarily */
+
+	if (band_temp == -1 || regulation == -1 || bandwidth_temp == -1 ||
+		rate_section == -1 || channel_temp == -1) {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "Wrong index value to access power limit table [band %d][regulation %d][bandwidth %d][rf_path %d][rate_section %d][chnl %d]\n",
+			 band_temp, regulation, bandwidth_temp, rf_path,
+			 rate_section, channel_temp);
+		return MAX_POWER_INDEX;
+	}
+
+	bd = band_temp;
+	regu = regulation;
+	bdwidth = bandwidth_temp;
+	sec = rate_section;
+	chnl = channel_temp;
+
+	if (band == BAND_ON_2_4G) {
+		char limits[10] = {0};
+		u8 i;
+
+		for (i = 0; i < 4; ++i)
+			limits[i] = rtlphy->txpwr_limit_2_4g[i][bdwidth]
+			[sec][chnl][rf_path];
+
+		power_limit = (regulation == TXPWR_LMT_WW) ?
+			_rtl8812ae_phy_get_world_wide_limit(limits) :
+			rtlphy->txpwr_limit_2_4g[regu][bdwidth]
+					[sec][chnl][rf_path];
+	} else if (band == BAND_ON_5G) {
+		char limits[10] = {0};
+		u8 i;
+
+		for (i = 0; i < MAX_REGULATION_NUM; ++i)
+			limits[i] = rtlphy->txpwr_limit_5g[i][bdwidth]
+			[sec][chnl][rf_path];
+
+		power_limit = (regulation == TXPWR_LMT_WW) ?
+			_rtl8812ae_phy_get_world_wide_limit(limits) :
+			rtlphy->txpwr_limit_5g[regu][chnl]
+			[sec][chnl][rf_path];
+	} else {
+		RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD,
+			 "No power limit table of the specified band\n");
+	}
+	return power_limit;
+}
+
+static char _rtl8821ae_phy_get_txpower_by_rate(struct ieee80211_hw *hw,
+					u8 band, u8 path, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 shift = 0, rate_section, tx_num;
+	char tx_pwr_diff = 0;
+	char limit = 0;
+
+	rate_section = _rtl8821ae_phy_get_ratesection_intxpower_byrate(path, rate);
+	tx_num = RF_TX_NUM_NONIMPLEMENT;
+
+	if (tx_num == RF_TX_NUM_NONIMPLEMENT) {
+		if ((rate >= DESC_RATEMCS8 && rate <= DESC_RATEMCS15) ||
+			(rate >= DESC_RATEVHT2SS_MCS2 && rate <= DESC_RATEVHT2SS_MCS9))
+			tx_num = RF_2TX;
+		else
+			tx_num = RF_1TX;
+	}
+
+	switch (rate) {
+	case DESC_RATE1M:
+	case DESC_RATE6M:
+	case DESC_RATE24M:
+	case DESC_RATEMCS0:
+	case DESC_RATEMCS4:
+	case DESC_RATEMCS8:
+	case DESC_RATEMCS12:
+	case DESC_RATEVHT1SS_MCS0:
+	case DESC_RATEVHT1SS_MCS4:
+	case DESC_RATEVHT1SS_MCS8:
+	case DESC_RATEVHT2SS_MCS2:
+	case DESC_RATEVHT2SS_MCS6:
+		shift = 0;
+		break;
+	case DESC_RATE2M:
+	case DESC_RATE9M:
+	case DESC_RATE36M:
+	case DESC_RATEMCS1:
+	case DESC_RATEMCS5:
+	case DESC_RATEMCS9:
+	case DESC_RATEMCS13:
+	case DESC_RATEVHT1SS_MCS1:
+	case DESC_RATEVHT1SS_MCS5:
+	case DESC_RATEVHT1SS_MCS9:
+	case DESC_RATEVHT2SS_MCS3:
+	case DESC_RATEVHT2SS_MCS7:
+		shift = 8;
+		break;
+	case DESC_RATE5_5M:
+	case DESC_RATE12M:
+	case DESC_RATE48M:
+	case DESC_RATEMCS2:
+	case DESC_RATEMCS6:
+	case DESC_RATEMCS10:
+	case DESC_RATEMCS14:
+	case DESC_RATEVHT1SS_MCS2:
+	case DESC_RATEVHT1SS_MCS6:
+	case DESC_RATEVHT2SS_MCS0:
+	case DESC_RATEVHT2SS_MCS4:
+	case DESC_RATEVHT2SS_MCS8:
+		shift = 16;
+		break;
+	case DESC_RATE11M:
+	case DESC_RATE18M:
+	case DESC_RATE54M:
+	case DESC_RATEMCS3:
+	case DESC_RATEMCS7:
+	case DESC_RATEMCS11:
+	case DESC_RATEMCS15:
+	case DESC_RATEVHT1SS_MCS3:
+	case DESC_RATEVHT1SS_MCS7:
+	case DESC_RATEVHT2SS_MCS1:
+	case DESC_RATEVHT2SS_MCS5:
+	case DESC_RATEVHT2SS_MCS9:
+		shift = 24;
+		break;
+	default:
+		RT_ASSERT(true, "Rate_Section is Illegal\n");
+		break;
+	}
+
+	tx_pwr_diff = (u8)(rtlphy->tx_power_by_rate_offset[band][path]
+		[tx_num][rate_section] >> shift) & 0xff;
+
+	/* RegEnableTxPowerLimit == 1 for 8812a & 8821a */
+	if (rtlpriv->efuse.eeprom_regulatory != 2) {
+		limit = _rtl8812ae_phy_get_txpower_limit(hw, band,
+			rtlphy->current_chan_bw, path, rate,
+			rtlphy->current_channel);
+
+		if (rate == DESC_RATEVHT1SS_MCS8 || rate == DESC_RATEVHT1SS_MCS9  ||
+			 rate == DESC_RATEVHT2SS_MCS8 || rate == DESC_RATEVHT2SS_MCS9) {
+			if (limit < 0) {
+				if (tx_pwr_diff < (-limit))
+					tx_pwr_diff = -limit;
+			}
+		} else {
+			if (limit < 0)
+				tx_pwr_diff = limit;
+			else
+				tx_pwr_diff = tx_pwr_diff > limit ? limit : tx_pwr_diff;
+		}
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			"Maximum power by rate %d, final power by rate %d\n",
+			limit, tx_pwr_diff);
+	}
+
+	return	tx_pwr_diff;
+}
+
+static u8 _rtl8821ae_get_txpower_index(struct ieee80211_hw *hw, u8 path,
+					u8 rate, u8 bandwidth, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	u8 index = (channel - 1);
+	u8 txpower = 0;
+	bool in_24g = false;
+	char powerdiff_byrate = 0;
+
+	if (((rtlhal->current_bandtype == BAND_ON_2_4G) &&
+	    (channel > 14 || channel < 1)) ||
+	    ((rtlhal->current_bandtype == BAND_ON_5G) && (channel <= 14))) {
+		index = 0;
+		RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD,
+			"Illegal channel!!\n");
+	}
+
+	in_24g = _rtl8821ae_phy_get_chnl_index(channel, &index);
+	if (in_24g) {
+		if (RTL8821AE_RX_HAL_IS_CCK_RATE(rate))
+			txpower = rtlefuse->txpwrlevel_cck[path][index];
+		else if (DESC_RATE6M <= rate)
+			txpower = rtlefuse->txpwrlevel_ht40_1s[path][index];
+		else
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_LOUD, "invalid rate\n");
+
+		if (DESC_RATE6M <= rate && rate <= DESC_RATE54M &&
+		    !RTL8821AE_RX_HAL_IS_CCK_RATE(rate))
+			txpower += rtlefuse->txpwr_legacyhtdiff[path][TX_1S];
+
+		if (bandwidth == HT_CHANNEL_WIDTH_20) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht20diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht20diff[path][TX_2S];
+		} else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_2S];
+		} else if (bandwidth == HT_CHANNEL_WIDTH_80) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT1SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT2SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_ht40diff[path][TX_2S];
+		}
+	} else {
+		if (DESC_RATE6M <= rate)
+			txpower = rtlefuse->txpwr_5g_bw40base[path][index];
+		else
+			RT_TRACE(rtlpriv, COMP_POWER_TRACKING, DBG_WARNING,
+				 "INVALID Rate.\n");
+
+		if (DESC_RATE6M <= rate && rate <= DESC_RATE54M &&
+		    !RTL8821AE_RX_HAL_IS_CCK_RATE(rate))
+			txpower += rtlefuse->txpwr_5g_ofdmdiff[path][TX_1S];
+
+		if (bandwidth == HT_CHANNEL_WIDTH_20) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT1SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_5g_bw20diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT2SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_5g_bw20diff[path][TX_2S];
+		} else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT1SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_5g_bw40diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT2SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower += rtlefuse->txpwr_5g_bw40diff[path][TX_2S];
+		} else if (bandwidth == HT_CHANNEL_WIDTH_80) {
+			u8 channel_5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {
+				42, 58, 106, 122, 138, 155, 171
+			};
+			u8 i;
+
+			for (i = 0; i < sizeof(channel_5g_80m) / sizeof(u8); ++i)
+				if (channel_5g_80m[i] == channel)
+					index = i;
+
+			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT1SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower = rtlefuse->txpwr_5g_bw80base[path][index]
+					+ rtlefuse->txpwr_5g_bw80diff[path][TX_1S];
+			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
+			    (DESC_RATEVHT2SS_MCS0 <= rate &&
+			     rate <= DESC_RATEVHT2SS_MCS9))
+				txpower = rtlefuse->txpwr_5g_bw80base[path][index]
+					+ rtlefuse->txpwr_5g_bw80diff[path][TX_1S]
+					+ rtlefuse->txpwr_5g_bw80diff[path][TX_2S];
+		    }
+	}
+	if (rtlefuse->eeprom_regulatory != 2)
+		powerdiff_byrate =
+		  _rtl8821ae_phy_get_txpower_by_rate(hw, (u8)(!in_24g),
+						     path, rate);
+
+	if (rate == DESC_RATEVHT1SS_MCS8 || rate == DESC_RATEVHT1SS_MCS9 ||
+	    rate == DESC_RATEVHT2SS_MCS8 || rate == DESC_RATEVHT2SS_MCS9)
+		txpower -= powerdiff_byrate;
+	else
+		txpower += powerdiff_byrate;
+
+	if (rate > DESC_RATE11M)
+		txpower += rtlpriv->dm.remnant_ofdm_swing_idx[path];
+	else
+		txpower += rtlpriv->dm.remnant_cck_idx;
+
+	if (txpower > MAX_POWER_INDEX)
+		txpower = MAX_POWER_INDEX;
+
+	return txpower;
+}
+
+static void _rtl8821ae_phy_set_txpower_index(struct ieee80211_hw *hw,
+					     u8 power_index, u8 path, u8 rate)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (path == RF90_PATH_A) {
+		switch (rate) {
+		case DESC_RATE1M:
+			rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATE2M:
+			rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATE5_5M:
+			rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATE11M:
+			rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATE6M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATE9M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATE12M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATE18M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATE24M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATE36M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATE48M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATE54M:
+			rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS0:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS1:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS2:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS3:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS4:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS5:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS6:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS7:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS8:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS9:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS10:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS11:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS12:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS13:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS14:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS15:
+			rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS0:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS1:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS2:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS3:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS4:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS5:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS6:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS7:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS8:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS9:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS0:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS1:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS2:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS3:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS4:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS5:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS6:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS7:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS8:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS9:
+			rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE3, power_index);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				"Invalid Rate!!\n");
+			break;
+		}
+	} else if (path == RF90_PATH_B) {
+		switch (rate) {
+		case DESC_RATE1M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATE2M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATE5_5M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATE11M:
+			rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATE6M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATE9M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATE12M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATE18M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATE24M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATE36M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATE48M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATE54M:
+			rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS0:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS1:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS2:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS3:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS4:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS5:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS6:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS7:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS8:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS9:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS10:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS11:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEMCS12:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEMCS13:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEMCS14:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEMCS15:
+			rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS0:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS1:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS2:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS3:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS4:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS5:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS6:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS7:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS8:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT1SS_MCS9:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS0:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS1:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS2:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS3:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS4:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS5:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2,
+				      MASKBYTE3, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS6:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE0, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS7:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE1, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS8:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE2, power_index);
+			break;
+		case DESC_RATEVHT2SS_MCS9:
+			rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6,
+				      MASKBYTE3, power_index);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+				 "Invalid Rate!!\n");
+			break;
+		}
+	} else {
+		RT_TRACE(rtlpriv, COMP_POWER, DBG_LOUD,
+			 "Invalid RFPath!!\n");
+	}
+}
+
+static void _rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw,
+						     u8 *array, u8 path,
+						     u8 channel, u8 size)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 i;
+	u8 power_index;
+
+	for (i = 0; i < size; i++) {
+		power_index =
+		  _rtl8821ae_get_txpower_index(hw, path, array[i],
+					       rtlphy->current_chan_bw,
+					       channel);
+		_rtl8821ae_phy_set_txpower_index(hw, power_index, path,
+						 array[i]);
+	}
+}
+
+static void _rtl8821ae_phy_txpower_training_by_path(struct ieee80211_hw *hw,
+						    u8 bw, u8 channel, u8 path)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	u8 i;
+	u32 power_level, data, offset;
+
+	if (path >= rtlphy->num_total_rfpath)
+		return;
+
+	data = 0;
+	if (path == RF90_PATH_A) {
+		power_level =
+			_rtl8821ae_get_txpower_index(hw, RF90_PATH_A,
+			DESC_RATEMCS7, bw, channel);
+		offset =  RA_TXPWRTRAING;
+	} else {
+		power_level =
+			_rtl8821ae_get_txpower_index(hw, RF90_PATH_B,
+			DESC_RATEMCS7, bw, channel);
+		offset =  RB_TXPWRTRAING;
+	}
+
+	for (i = 0; i < 3; i++) {
+		if (i == 0)
+			power_level = power_level - 10;
+		else if (i == 1)
+			power_level = power_level - 8;
+		else
+			power_level = power_level - 6;
+
+		data |= (((power_level > 2) ? (power_level) : 2) << (i * 8));
+	}
+	rtl_set_bbreg(hw, offset, 0xffffff, data);
+}
+
+void rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw,
+					     u8 channel, u8 path)
+{
+	/* struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw)); */
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 cck_rates[]  = {DESC_RATE1M, DESC_RATE2M, DESC_RATE5_5M,
+			      DESC_RATE11M};
+	u8 sizes_of_cck_retes = 4;
+	u8 ofdm_rates[]  = {DESC_RATE6M, DESC_RATE9M, DESC_RATE12M,
+				DESC_RATE18M, DESC_RATE24M, DESC_RATE36M,
+				DESC_RATE48M, DESC_RATE54M};
+	u8 sizes_of_ofdm_retes = 8;
+	u8 ht_rates_1t[]  = {DESC_RATEMCS0, DESC_RATEMCS1, DESC_RATEMCS2,
+				DESC_RATEMCS3, DESC_RATEMCS4, DESC_RATEMCS5,
+				DESC_RATEMCS6, DESC_RATEMCS7};
+	u8 sizes_of_ht_retes_1t = 8;
+	u8 ht_rates_2t[]  = {DESC_RATEMCS8, DESC_RATEMCS9,
+				DESC_RATEMCS10, DESC_RATEMCS11,
+				DESC_RATEMCS12, DESC_RATEMCS13,
+				DESC_RATEMCS14, DESC_RATEMCS15};
+	u8 sizes_of_ht_retes_2t = 8;
+	u8 vht_rates_1t[]  = {DESC_RATEVHT1SS_MCS0, DESC_RATEVHT1SS_MCS1,
+				DESC_RATEVHT1SS_MCS2, DESC_RATEVHT1SS_MCS3,
+				DESC_RATEVHT1SS_MCS4, DESC_RATEVHT1SS_MCS5,
+				DESC_RATEVHT1SS_MCS6, DESC_RATEVHT1SS_MCS7,
+			     DESC_RATEVHT1SS_MCS8, DESC_RATEVHT1SS_MCS9};
+	u8 vht_rates_2t[]  = {DESC_RATEVHT2SS_MCS0, DESC_RATEVHT2SS_MCS1,
+				DESC_RATEVHT2SS_MCS2, DESC_RATEVHT2SS_MCS3,
+				DESC_RATEVHT2SS_MCS4, DESC_RATEVHT2SS_MCS5,
+				DESC_RATEVHT2SS_MCS6, DESC_RATEVHT2SS_MCS7,
+				DESC_RATEVHT2SS_MCS8, DESC_RATEVHT2SS_MCS9};
+	u8 sizes_of_vht_retes = 10;
+
+	if (rtlhal->current_bandtype == BAND_ON_2_4G)
+		_rtl8821ae_phy_set_txpower_level_by_path(hw, cck_rates, path, channel,
+							 sizes_of_cck_retes);
+
+	_rtl8821ae_phy_set_txpower_level_by_path(hw, ofdm_rates, path, channel,
+						 sizes_of_ofdm_retes);
+	_rtl8821ae_phy_set_txpower_level_by_path(hw, ht_rates_1t, path, channel,
+						 sizes_of_ht_retes_1t);
+	_rtl8821ae_phy_set_txpower_level_by_path(hw, vht_rates_1t, path, channel,
+						 sizes_of_vht_retes);
+
+	if (rtlphy->num_total_rfpath >= 2) {
+		_rtl8821ae_phy_set_txpower_level_by_path(hw, ht_rates_2t, path,
+							 channel,
+							 sizes_of_ht_retes_2t);
+		_rtl8821ae_phy_set_txpower_level_by_path(hw, vht_rates_2t, path,
+							 channel,
+							 sizes_of_vht_retes);
+	}
+
+	_rtl8821ae_phy_txpower_training_by_path(hw, rtlphy->current_chan_bw,
+						channel, path);
+}
+
+/*just in case, write txpower in DW, to reduce time*/
+void rtl8821ae_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 path = 0;
+
+	for (path = RF90_PATH_A; path < rtlphy->num_total_rfpath; ++path)
+		rtl8821ae_phy_set_txpower_level_by_path(hw, channel, path);
+}
+
+static long _rtl8821ae_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
+					    enum wireless_mode wirelessmode,
+					    u8 txpwridx)
+{
+	long offset;
+	long pwrout_dbm;
+
+	switch (wirelessmode) {
+	case WIRELESS_MODE_B:
+		offset = -7;
+		break;
+	case WIRELESS_MODE_G:
+	case WIRELESS_MODE_N_24G:
+		offset = -8;
+		break;
+	default:
+		offset = -8;
+		break;
+	}
+	pwrout_dbm = txpwridx / 2 + offset;
+	return pwrout_dbm;
+}
+
+void rtl8821ae_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	enum io_type iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
+
+	if (!is_hal_stop(rtlhal)) {
+		switch (operation) {
+		case SCAN_OPT_BACKUP_BAND0:
+			iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+
+			break;
+		case SCAN_OPT_BACKUP_BAND1:
+			iotype = IO_CMD_PAUSE_BAND1_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+
+			break;
+		case SCAN_OPT_RESTORE:
+			iotype = IO_CMD_RESUME_DM_BY_SCAN;
+			rtlpriv->cfg->ops->set_hw_reg(hw,
+						      HW_VAR_IO_CMD,
+						      (u8 *)&iotype);
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "Unknown Scan Backup operation.\n");
+			break;
+		}
+	}
+}
+
+static void _rtl8821ae_phy_set_reg_bw(struct rtl_priv *rtlpriv, u8 bw)
+{
+	u16 reg_rf_mode_bw, tmp = 0;
+
+	reg_rf_mode_bw = rtl_read_word(rtlpriv, REG_TRXPTCL_CTL);
+	switch (bw) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, reg_rf_mode_bw & 0xFE7F);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		tmp = reg_rf_mode_bw | BIT(7);
+		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, tmp & 0xFEFF);
+		break;
+	case HT_CHANNEL_WIDTH_80:
+		tmp = reg_rf_mode_bw | BIT(8);
+		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, tmp & 0xFF7F);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING, "unknown Bandwidth: 0x%x\n", bw);
+		break;
+	}
+}
+
+static u8 _rtl8821ae_phy_get_secondary_chnl(struct rtl_priv *rtlpriv)
+{
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	u8 sc_set_40 = 0, sc_set_20 = 0;
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80) {
+		if (mac->cur_80_prime_sc == PRIME_CHNL_OFFSET_LOWER)
+			sc_set_40 = VHT_DATA_SC_40_LOWER_OF_80MHZ;
+		else if (mac->cur_80_prime_sc == PRIME_CHNL_OFFSET_UPPER)
+			sc_set_40 = VHT_DATA_SC_40_UPPER_OF_80MHZ;
+		else
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				"SCMapping: Not Correct Primary40MHz Setting\n");
+
+		if ((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_LOWER))
+			sc_set_20 = VHT_DATA_SC_20_LOWEST_OF_80MHZ;
+		else if ((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_LOWER))
+			sc_set_20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+		else if ((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_UPPER))
+			sc_set_20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+		else if ((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER) &&
+			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_UPPER))
+			sc_set_20 = VHT_DATA_SC_20_UPPERST_OF_80MHZ;
+		else
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				"SCMapping: Not Correct Primary40MHz Setting\n");
+	} else if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		if (mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER)
+			sc_set_20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
+		else if (mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER)
+			sc_set_20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
+		else
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "SCMapping: Not Correct Primary40MHz Setting\n");
+	}
+	return (sc_set_40 << 4) | sc_set_20;
+}
+
+void rtl8821ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 sub_chnl = 0;
+	u8 l1pk_val = 0;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "Switch to %s bandwidth\n",
+		  (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
+		  "20MHz" :
+		  (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40 ?
+		  "40MHz" : "80MHz")));
+
+	_rtl8821ae_phy_set_reg_bw(rtlpriv, rtlphy->current_chan_bw);
+	sub_chnl = _rtl8821ae_phy_get_secondary_chnl(rtlpriv);
+	rtl_write_byte(rtlpriv, 0x0483, sub_chnl);
+
+	switch (rtlphy->current_chan_bw) {
+	case HT_CHANNEL_WIDTH_20:
+		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300200);
+		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
+
+		if (rtlphy->rf_type == RF_2T2R)
+			rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, 7);
+		else
+			rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, 8);
+		break;
+	case HT_CHANNEL_WIDTH_20_40:
+		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300201);
+		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
+		rtl_set_bbreg(hw, RRFMOD, 0x3C, sub_chnl);
+		rtl_set_bbreg(hw, RCCAONSEC, 0xf0000000, sub_chnl);
+
+		if (rtlphy->reg_837 & BIT(2))
+			l1pk_val = 6;
+		else {
+			if (rtlphy->rf_type == RF_2T2R)
+				l1pk_val = 7;
+			else
+				l1pk_val = 8;
+		}
+		/* 0x848[25:22] = 0x6 */
+		rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, l1pk_val);
+
+		if (sub_chnl == VHT_DATA_SC_20_UPPER_OF_80MHZ)
+			rtl_set_bbreg(hw, RCCK_SYSTEM, BCCK_SYSTEM, 1);
+		else
+			rtl_set_bbreg(hw, RCCK_SYSTEM, BCCK_SYSTEM, 0);
+		break;
+
+	case HT_CHANNEL_WIDTH_80:
+		 /* 0x8ac[21,20,9:6,1,0]=8'b11100010 */
+		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300202);
+		/* 0x8c4[30] = 1 */
+		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);
+		rtl_set_bbreg(hw, RRFMOD, 0x3C, sub_chnl);
+		rtl_set_bbreg(hw, RCCAONSEC, 0xf0000000, sub_chnl);
+
+		if (rtlphy->reg_837 & BIT(2))
+			l1pk_val = 5;
+		else {
+			if (rtlphy->rf_type == RF_2T2R)
+				l1pk_val = 6;
+			else
+				l1pk_val = 7;
+		}
+		rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, l1pk_val);
+
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", rtlphy->current_chan_bw);
+		break;
+	}
+
+	rtl8812ae_fixspur(hw, rtlphy->current_chan_bw, rtlphy->current_channel);
+
+	rtl8821ae_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
+	rtlphy->set_bwmode_inprogress = false;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_LOUD, "\n");
+}
+
+void rtl8821ae_phy_set_bw_mode(struct ieee80211_hw *hw,
+			    enum nl80211_channel_type ch_type)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u8 tmp_bw = rtlphy->current_chan_bw;
+
+	if (rtlphy->set_bwmode_inprogress)
+		return;
+	rtlphy->set_bwmode_inprogress = true;
+	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw)))
+		rtl8821ae_phy_set_bw_mode_callback(hw);
+	else {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+			 "FALSE driver sleep or unload\n");
+		rtlphy->set_bwmode_inprogress = false;
+		rtlphy->current_chan_bw = tmp_bw;
+	}
+}
+
+void rtl8821ae_phy_sw_chnl_callback(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 channel = rtlphy->current_channel;
+	u8 path;
+	u32 data;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "switch to channel%d\n", rtlphy->current_channel);
+	if (is_hal_stop(rtlhal))
+		return;
+
+	if (36 <= channel && channel <= 48)
+		data = 0x494;
+	else if (50 <= channel && channel <= 64)
+		data = 0x453;
+	else if (100 <= channel && channel <= 116)
+		data = 0x452;
+	else if (118 <= channel)
+		data = 0x412;
+	else
+		data = 0x96a;
+	rtl_set_bbreg(hw, RFC_AREA, 0x1ffe0000, data);
+
+	for (path = RF90_PATH_A; path < rtlphy->num_total_rfpath; path++) {
+		if (36 <= channel && channel <= 64)
+			data = 0x101;
+		else if (100 <= channel && channel <= 140)
+			data = 0x301;
+		else if (140 < channel)
+			data = 0x501;
+		else
+			data = 0x000;
+		rtl8821ae_phy_set_rf_reg(hw, path, RF_CHNLBW,
+			BIT(18)|BIT(17)|BIT(16)|BIT(9)|BIT(8), data);
+
+		rtl8821ae_phy_set_rf_reg(hw, path, RF_CHNLBW,
+			BMASKBYTE0, channel);
+
+		if (channel > 14) {
+			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
+				if (36 <= channel && channel <= 64)
+					data = 0x114E9;
+				else if (100 <= channel && channel <= 140)
+					data = 0x110E9;
+				else
+					data = 0x110E9;
+				rtl8821ae_phy_set_rf_reg(hw, path, RF_APK,
+					BRFREGOFFSETMASK, data);
+			}
+		}
+	}
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
+}
+
+u8 rtl8821ae_phy_sw_chnl(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+	u32 timeout = 1000, timecount = 0;
+	u8 channel = rtlphy->current_channel;
+
+	if (rtlphy->sw_chnl_inprogress)
+		return 0;
+	if (rtlphy->set_bwmode_inprogress)
+		return 0;
+
+	if ((is_hal_stop(rtlhal)) || (RT_CANNOT_IO(hw))) {
+		RT_TRACE(rtlpriv, COMP_CHAN, DBG_LOUD,
+			 "sw_chnl_inprogress false driver sleep or unload\n");
+		return 0;
+	}
+	while (rtlphy->lck_inprogress && timecount < timeout) {
+		mdelay(50);
+		timecount += 50;
+	}
+
+	if (rtlphy->current_channel > 14 && rtlhal->current_bandtype != BAND_ON_5G)
+		rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_5G);
+	else if (rtlphy->current_channel <= 14 && rtlhal->current_bandtype != BAND_ON_2_4G)
+		rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_2_4G);
+
+	rtlphy->sw_chnl_inprogress = true;
+	if (channel == 0)
+		channel = 1;
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE,
+		 "switch to channel%d, band type is %d\n",
+		 rtlphy->current_channel, rtlhal->current_bandtype);
+
+	rtl8821ae_phy_sw_chnl_callback(hw);
+
+	rtl8821ae_dm_clear_txpower_tracking_state(hw);
+	rtl8821ae_phy_set_txpower_level(hw, rtlphy->current_channel);
+
+	RT_TRACE(rtlpriv, COMP_SCAN, DBG_TRACE, "\n");
+	rtlphy->sw_chnl_inprogress = false;
+	return 1;
+}
+
+u8 _rtl8812ae_get_right_chnl_place_for_iqk(u8 chnl)
+{
+	u8 channel_all[TARGET_CHNL_NUM_2G_5G_8812] = {
+		1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13,
+		14, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54,
+		56, 58, 60, 62, 64, 100, 102, 104, 106, 108,
+		110, 112, 114, 116, 118, 120, 122, 124, 126,
+		128, 130, 132, 134, 136, 138, 140, 149, 151,
+		153, 155, 157, 159, 161, 163, 165};
+	u8 place = chnl;
+
+	if (chnl > 14) {
+		for (place = 14; place < sizeof(channel_all); place++)
+			if (channel_all[place] == chnl)
+				return place-13;
+	}
+
+	return 0;
+}
+
+#define MACBB_REG_NUM 10
+#define AFE_REG_NUM 14
+#define RF_REG_NUM 3
+
+static void _rtl8821ae_iqk_backup_macbb(struct ieee80211_hw *hw,
+					u32 *macbb_backup,
+					u32 *backup_macbb_reg, u32 mac_bb_num)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*save MACBB default value*/
+	for (i = 0; i < mac_bb_num; i++)
+		macbb_backup[i] = rtl_read_dword(rtlpriv, backup_macbb_reg[i]);
+
+	RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "BackupMacBB Success!!!!\n");
+}
+
+static void _rtl8821ae_iqk_backup_afe(struct ieee80211_hw *hw, u32 *afe_backup,
+				      u32 *backup_afe_REG, u32 afe_num)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*Save AFE Parameters */
+	for (i = 0; i < afe_num; i++)
+		afe_backup[i] = rtl_read_dword(rtlpriv, backup_afe_REG[i]);
+	RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "BackupAFE Success!!!!\n");
+}
+
+static void _rtl8821ae_iqk_backup_rf(struct ieee80211_hw *hw, u32 *rfa_backup,
+				     u32 *rfb_backup, u32 *backup_rf_reg,
+				     u32 rf_num)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	/*Save RF Parameters*/
+	for (i = 0; i < rf_num; i++) {
+		rfa_backup[i] = rtl_get_rfreg(hw, RF90_PATH_A, backup_rf_reg[i],
+					      BMASKDWORD);
+		rfb_backup[i] = rtl_get_rfreg(hw, RF90_PATH_B, backup_rf_reg[i],
+					      BMASKDWORD);
+	}
+	RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "BackupRF Success!!!!\n");
+}
+
+static void _rtl8821ae_iqk_configure_mac(
+		struct ieee80211_hw *hw
+		)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	/* ========MAC register setting========*/
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+	rtl_write_byte(rtlpriv, 0x522, 0x3f);
+	rtl_set_bbreg(hw, 0x550, BIT(11) | BIT(3), 0x0);
+	rtl_write_byte(rtlpriv, 0x808, 0x00);		/*RX ante off*/
+	rtl_set_bbreg(hw, 0x838, 0xf, 0xc);		/*CCA off*/
+}
+
+static void _rtl8821ae_iqk_tx_fill_iqc(struct ieee80211_hw *hw,
+				       enum radio_path path, u32 tx_x, u32 tx_y)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	switch (path) {
+	case RF90_PATH_A:
+		/* [31] = 1 --> Page C1 */
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1);
+		rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
+		rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
+		rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
+		rtl_set_bbreg(hw, 0xccc, 0x000007ff, tx_y);
+		rtl_set_bbreg(hw, 0xcd4, 0x000007ff, tx_x);
+		RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+			 "TX_X = %x;;TX_Y = %x =====> fill to IQC\n",
+			 tx_x, tx_y);
+		RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+			 "0xcd4 = %x;;0xccc = %x ====>fill to IQC\n",
+			 rtl_get_bbreg(hw, 0xcd4, 0x000007ff),
+			 rtl_get_bbreg(hw, 0xccc, 0x000007ff));
+		break;
+	default:
+		break;
+	};
+}
+
+static void _rtl8821ae_iqk_rx_fill_iqc(struct ieee80211_hw *hw,
+				       enum radio_path path, u32 rx_x, u32 rx_y)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	switch (path) {
+	case RF90_PATH_A:
+		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+		rtl_set_bbreg(hw, 0xc10, 0x000003ff, rx_x>>1);
+		rtl_set_bbreg(hw, 0xc10, 0x03ff0000, rx_y>>1);
+		RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+			 "rx_x = %x;;rx_y = %x ====>fill to IQC\n",
+			 rx_x>>1, rx_y>>1);
+		RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+			 "0xc10 = %x ====>fill to IQC\n",
+			 rtl_read_dword(rtlpriv, 0xc10));
+		break;
+	default:
+		break;
+	};
+}
+
+#define cal_num 10
+
+static void _rtl8821ae_iqk_tx(struct ieee80211_hw *hw, enum radio_path path)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
+
+	u32	tx_fail, rx_fail, delay_count, iqk_ready, cal_retry, cal = 0, temp_reg65;
+	int	tx_x = 0, tx_y = 0, rx_x = 0, rx_y = 0, tx_average = 0, rx_average = 0;
+	int	tx_x0[cal_num], tx_y0[cal_num], tx_x0_rxk[cal_num],
+		tx_y0_rxk[cal_num], rx_x0[cal_num], rx_y0[cal_num];
+	bool	tx0iqkok = false, rx0iqkok = false;
+	bool	vdf_enable = false;
+	int	i, k, vdf_y[3], vdf_x[3], tx_dt[3], rx_dt[3],
+		ii, dx = 0, dy = 0, tx_finish = 0, rx_finish = 0;
+
+	RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+			"BandWidth = %d.\n",
+			 rtlphy->current_chan_bw);
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
+		vdf_enable = true;
+
+	while (cal < cal_num) {
+		switch (path) {
+		case RF90_PATH_A:
+			temp_reg65 = rtl_get_rfreg(hw, path, 0x65, 0xffffffff);
+			/* Path-A LOK */
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
+			/*========Path-A AFE all on========*/
+			/*Port 0 DAC/ADC on*/
+			rtl_write_dword(rtlpriv, 0xc60, 0x77777777);
+			rtl_write_dword(rtlpriv, 0xc64, 0x77777777);
+			rtl_write_dword(rtlpriv, 0xc68, 0x19791979);
+			rtl_write_dword(rtlpriv, 0xc6c, 0x19791979);
+			rtl_write_dword(rtlpriv, 0xc70, 0x19791979);
+			rtl_write_dword(rtlpriv, 0xc74, 0x19791979);
+			rtl_write_dword(rtlpriv, 0xc78, 0x19791979);
+			rtl_write_dword(rtlpriv, 0xc7c, 0x19791979);
+			rtl_write_dword(rtlpriv, 0xc80, 0x19791979);
+			rtl_write_dword(rtlpriv, 0xc84, 0x19791979);
+
+			rtl_set_bbreg(hw, 0xc00, 0xf, 0x4); /*hardware 3-wire off*/
+
+			/* LOK Setting */
+			/* ====== LOK ====== */
+			/*DAC/ADC sampling rate (160 MHz)*/
+			rtl_set_bbreg(hw, 0xc5c, BIT(26) | BIT(25) | BIT(24), 0x7);
+
+			/* 2. LoK RF Setting (at BW = 20M) */
+			rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80002);
+			rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x3);     /* BW 20M */
+			rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x20000);
+			rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0003f);
+			rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xf3fc3);
+			rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d5);
+			rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+			rtl_set_bbreg(hw, 0xcb8, 0xf, 0xd);
+			rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+			rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+			rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
+			rtl_write_dword(rtlpriv, 0x978, 0x29002000);/* TX (X,Y) */
+			rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);/* RX (X,Y) */
+			rtl_write_dword(rtlpriv, 0x984, 0x00462910);/* [0]:AGC_en, [15]:idac_K_Mask */
+
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+			rtl_write_dword(rtlpriv, 0xc88, 0x821403f4);
+
+			if (rtlhal->current_bandtype)
+				rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
+			else
+				rtl_write_dword(rtlpriv, 0xc8c, 0x28163e96);
+
+			rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+			rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+			rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] \B1N SI/PI \A8Ï¥\CE\C5v\A4\C1\B5\B9 iqk_dpk module */
+			rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+			rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+			mdelay(10); /* Delay 10ms */
+			rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+			rtl_set_rfreg(hw, path, 0x58, 0x7fe00, rtl_get_rfreg(hw, path, 0x8, 0xffc00)); /* Load LOK */
+
+			switch (rtlphy->current_chan_bw) {
+			case 1:
+				rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x1);
+				break;
+			case 2:
+				rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x0);
+				break;
+			default:
+				break;
+			}
+
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+
+			/* 3. TX RF Setting */
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+			rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+			rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x20000);
+			rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0003f);
+			rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xf3fc3);
+			rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d5);
+			rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+			rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+			/* ODM_SetBBReg(pDM_Odm, 0xcb8, 0xf, 0xd); */
+			rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+			rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+			rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
+			rtl_write_dword(rtlpriv, 0x978, 0x29002000);/* TX (X,Y) */
+			rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);/* RX (X,Y) */
+			rtl_write_dword(rtlpriv, 0x984, 0x0046a910);/* [0]:AGC_en, [15]:idac_K_Mask */
+
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+			rtl_write_dword(rtlpriv, 0xc88, 0x821403f1);
+			if (rtlhal->current_bandtype)
+				rtl_write_dword(rtlpriv, 0xc8c, 0x40163e96);
+			else
+				rtl_write_dword(rtlpriv, 0xc8c, 0x00163e96);
+
+			if (vdf_enable == 1) {
+				RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "VDF_enable\n");
+				for (k = 0; k <= 2; k++) {
+					switch (k) {
+					case 0:
+						rtl_write_dword(rtlpriv, 0xc80, 0x18008c38);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+						rtl_write_dword(rtlpriv, 0xc84, 0x38008c38);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+						rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);
+						break;
+					case 1:
+						rtl_set_bbreg(hw, 0xc80, BIT(28), 0x0);
+						rtl_set_bbreg(hw, 0xc84, BIT(28), 0x0);
+						rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);
+						break;
+					case 2:
+						RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+							"vdf_y[1] = %x;;;vdf_y[0] = %x\n", vdf_y[1]>>21 & 0x00007ff, vdf_y[0]>>21 & 0x00007ff);
+						RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+							"vdf_x[1] = %x;;;vdf_x[0] = %x\n", vdf_x[1]>>21 & 0x00007ff, vdf_x[0]>>21 & 0x00007ff);
+						tx_dt[cal] = (vdf_y[1]>>20)-(vdf_y[0]>>20);
+						tx_dt[cal] = ((16*tx_dt[cal])*10000/15708);
+						tx_dt[cal] = (tx_dt[cal] >> 1)+(tx_dt[cal] & BIT(0));
+						rtl_write_dword(rtlpriv, 0xc80, 0x18008c20);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+						rtl_write_dword(rtlpriv, 0xc84, 0x38008c20);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+						rtl_set_bbreg(hw, 0xce8, BIT(31), 0x1);
+						rtl_set_bbreg(hw, 0xce8, 0x3fff0000, tx_dt[cal] & 0x00003fff);
+						break;
+					default:
+						break;
+					}
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] \B1N SI/PI \A8Ï¥\CE\C5v\A4\C1\B5\B9 iqk_dpk module */
+					cal_retry = 0;
+					while (1) {
+						/* one shot */
+						rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+						rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+						mdelay(10); /* Delay 10ms */
+						rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+						delay_count = 0;
+						while (1) {
+							iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+							if ((~iqk_ready) || (delay_count > 20))
+								break;
+							else{
+								mdelay(1);
+								delay_count++;
+							}
+						}
+
+						if (delay_count < 20) {							/* If 20ms No Result, then cal_retry++ */
+							/* ============TXIQK Check============== */
+							tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+							if (~tx_fail) {
+								rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+								vdf_x[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+								rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+								vdf_y[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+								tx0iqkok = true;
+								break;
+							} else {
+								rtl_set_bbreg(hw, 0xccc, 0x000007ff, 0x0);
+								rtl_set_bbreg(hw, 0xcd4, 0x000007ff, 0x200);
+								tx0iqkok = false;
+								cal_retry++;
+								if (cal_retry == 10)
+									break;
+							}
+						} else {
+							tx0iqkok = false;
+							cal_retry++;
+							if (cal_retry == 10)
+								break;
+						}
+					}
+				}
+				if (k == 3) {
+					tx_x0[cal] = vdf_x[k-1];
+					tx_y0[cal] = vdf_y[k-1];
+				}
+			} else {
+				rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+				rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+				rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] \B1N SI/PI \A8Ï¥\CE\C5v\A4\C1\B5\B9 iqk_dpk module */
+				cal_retry = 0;
+				while (1) {
+					/* one shot */
+					rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+					rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+					mdelay(10); /* Delay 10ms */
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+					delay_count = 0;
+					while (1) {
+						iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+						if ((~iqk_ready) || (delay_count > 20))
+							break;
+						else{
+							mdelay(1);
+							delay_count++;
+						}
+					}
+
+					if (delay_count < 20) {							/* If 20ms No Result, then cal_retry++ */
+						/* ============TXIQK Check============== */
+						tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+						if (~tx_fail) {
+							rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+							tx_x0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+							rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+							tx_y0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+							tx0iqkok = true;
+							break;
+						} else {
+							rtl_set_bbreg(hw, 0xccc, 0x000007ff, 0x0);
+							rtl_set_bbreg(hw, 0xcd4, 0x000007ff, 0x200);
+							tx0iqkok = false;
+							cal_retry++;
+							if (cal_retry == 10)
+								break;
+						}
+					} else {
+						tx0iqkok = false;
+						cal_retry++;
+						if (cal_retry == 10)
+							break;
+					}
+				}
+			}
+
+			if (tx0iqkok == false)
+				break;				/* TXK fail, Don't do RXK */
+
+			if (vdf_enable == 1) {
+				rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);    /* TX VDF Disable */
+				RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "RXVDF Start\n");
+				for (k = 0; k <= 2; k++) {
+					/* ====== RX mode TXK (RXK Step 1) ====== */
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+					/* 1. TX RF Setting */
+					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+					rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+					rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x00029);
+					rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xd7ffb);
+					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
+					rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+
+					rtl_set_bbreg(hw, 0xcb8, 0xf, 0xd);
+					rtl_write_dword(rtlpriv, 0x978, 0x29002000);/* TX (X,Y) */
+					rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);/* RX (X,Y) */
+					rtl_write_dword(rtlpriv, 0x984, 0x0046a910);/* [0]:AGC_en, [15]:idac_K_Mask */
+					rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+					rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+					switch (k) {
+					case 0:
+						{
+							rtl_write_dword(rtlpriv, 0xc80, 0x18008c38);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+							rtl_write_dword(rtlpriv, 0xc84, 0x38008c38);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+							rtl_set_bbreg(hw, 0xce8, BIT(30), 0x0);
+						}
+						break;
+					case 1:
+						{
+							rtl_write_dword(rtlpriv, 0xc80, 0x08008c38);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+							rtl_write_dword(rtlpriv, 0xc84, 0x28008c38);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+							rtl_set_bbreg(hw, 0xce8, BIT(30), 0x0);
+						}
+						break;
+					case 2:
+						{
+							RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+							"VDF_Y[1] = %x;;;VDF_Y[0] = %x\n",
+							vdf_y[1]>>21 & 0x00007ff, vdf_y[0]>>21 & 0x00007ff);
+							RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+							"VDF_X[1] = %x;;;VDF_X[0] = %x\n",
+							vdf_x[1]>>21 & 0x00007ff, vdf_x[0]>>21 & 0x00007ff);
+							rx_dt[cal] = (vdf_y[1]>>20)-(vdf_y[0]>>20);
+							RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "Rx_dt = %d\n", rx_dt[cal]);
+							rx_dt[cal] = ((16*rx_dt[cal])*10000/13823);
+							rx_dt[cal] = (rx_dt[cal] >> 1)+(rx_dt[cal] & BIT(0));
+							rtl_write_dword(rtlpriv, 0xc80, 0x18008c20);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+							rtl_write_dword(rtlpriv, 0xc84, 0x38008c20);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+							rtl_set_bbreg(hw, 0xce8, 0x00003fff, rx_dt[cal] & 0x00003fff);
+						}
+						break;
+					default:
+						break;
+					}
+					rtl_write_dword(rtlpriv, 0xc88, 0x821603e0);
+					rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] \B1N SI/PI \A8Ï¥\CE\C5v\A4\C1\B5\B9 iqk_dpk module */
+					cal_retry = 0;
+					while (1) {
+						/* one shot */
+						rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+						rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+						mdelay(10); /* Delay 10ms */
+						rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+						delay_count = 0;
+						while (1) {
+							iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+							if ((~iqk_ready) || (delay_count > 20))
+								break;
+							else{
+								mdelay(1);
+								delay_count++;
+							}
+						}
+
+						if (delay_count < 20) {							/* If 20ms No Result, then cal_retry++ */
+							/* ============TXIQK Check============== */
+							tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+							if (~tx_fail) {
+								rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+								tx_x0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+								rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+								tx_y0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+								tx0iqkok = true;
+								break;
+							} else{
+								tx0iqkok = false;
+								cal_retry++;
+								if (cal_retry == 10)
+									break;
+							}
+						} else {
+							tx0iqkok = false;
+							cal_retry++;
+							if (cal_retry == 10)
+								break;
+						}
+					}
+
+					if (tx0iqkok == false) {   /* If RX mode TXK fail, then take TXK Result */
+						tx_x0_rxk[cal] = tx_x0[cal];
+						tx_y0_rxk[cal] = tx_y0[cal];
+						tx0iqkok = true;
+						RT_TRACE(rtlpriv,
+							 COMP_IQK,
+							 DBG_LOUD,
+							 "RXK Step 1 fail\n");
+					}
+
+					/* ====== RX IQK ====== */
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+					/* 1. RX RF Setting */
+					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+					rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+					rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0002f);
+					rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xfffbb);
+					rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x88001);
+					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d8);
+					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+
+					rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0_rxk[cal])>>21&0x000007ff);
+					rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0_rxk[cal])>>21&0x000007ff);
+					rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
+					rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
+					rtl_set_bbreg(hw, 0xcb8, 0xF, 0xe);
+					rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+					rtl_write_dword(rtlpriv, 0x984, 0x0046a911);
+
+					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+					rtl_set_bbreg(hw, 0xc80, BIT(29), 0x1);
+					rtl_set_bbreg(hw, 0xc84, BIT(29), 0x0);
+					rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
+
+					rtl_write_dword(rtlpriv, 0xc8c, 0x28160d00); /* pDM_Odm->SupportInterface == 1 */
+
+					if (k == 2)
+						rtl_set_bbreg(hw, 0xce8, BIT(30), 0x1);  /* RX VDF Enable */
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] \B1N SI/PI \A8Ï¥\CE\C5v\A4\C1\B5\B9 iqk_dpk module */
+
+					cal_retry = 0;
+					while (1) {
+						/* one shot */
+						rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+						rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+						mdelay(10); /* Delay 10ms */
+						rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+						delay_count = 0;
+						while (1) {
+							iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+							if ((~iqk_ready) || (delay_count > 20))
+								break;
+							else{
+								mdelay(1);
+								delay_count++;
+							}
+						}
+
+						if (delay_count < 20) {	/* If 20ms No Result, then cal_retry++ */
+							/* ============RXIQK Check============== */
+							rx_fail = rtl_get_bbreg(hw, 0xd00, BIT(11));
+							if (rx_fail == 0) {
+								rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
+								vdf_x[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+								rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
+								vdf_y[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+								rx0iqkok = true;
+								break;
+							} else {
+								rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x200>>1);
+								rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0x0>>1);
+								rx0iqkok = false;
+								cal_retry++;
+								if (cal_retry == 10)
+									break;
+
+							}
+						} else{
+							rx0iqkok = false;
+							cal_retry++;
+							if (cal_retry == 10)
+								break;
+						}
+					}
+
+				}
+				if (k == 3) {
+					rx_x0[cal] = vdf_x[k-1];
+					rx_y0[cal] = vdf_y[k-1];
+				}
+				rtl_set_bbreg(hw, 0xce8, BIT(31), 0x1);    /* TX VDF Enable */
+			}
+
+			else{
+				/* ====== RX mode TXK (RXK Step 1) ====== */
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+				/* 1. TX RF Setting */
+				rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+				rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+				rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x00029);
+				rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xd7ffb);
+				rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
+				rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
+				rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+				rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+				rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
+				rtl_write_dword(rtlpriv, 0x984, 0x0046a910);/* [0]:AGC_en, [15]:idac_K_Mask */
+
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+				rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+				rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+				rtl_write_dword(rtlpriv, 0xc88, 0x821603e0);
+				/* ODM_Write4Byte(pDM_Odm, 0xc8c, 0x68163e96); */
+				rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] \B1N SI/PI \A8Ï¥\CE\C5v\A4\C1\B5\B9 iqk_dpk module */
+				cal_retry = 0;
+				while (1) {
+					/* one shot */
+					rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+					rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+					mdelay(10); /* Delay 10ms */
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+					delay_count = 0;
+					while (1) {
+						iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+						if ((~iqk_ready) || (delay_count > 20))
+							break;
+						else{
+							mdelay(1);
+							delay_count++;
+						}
+					}
+
+					if (delay_count < 20) {							/* If 20ms No Result, then cal_retry++ */
+						/* ============TXIQK Check============== */
+						tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
+
+						if (~tx_fail) {
+							rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
+							tx_x0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+							rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
+							tx_y0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+							tx0iqkok = true;
+							break;
+						} else {
+							tx0iqkok = false;
+							cal_retry++;
+							if (cal_retry == 10)
+								break;
+						}
+					} else{
+						tx0iqkok = false;
+						cal_retry++;
+						if (cal_retry == 10)
+							break;
+					}
+				}
+
+				if (tx0iqkok == false) {   /* If RX mode TXK fail, then take TXK Result */
+					tx_x0_rxk[cal] = tx_x0[cal];
+					tx_y0_rxk[cal] = tx_y0[cal];
+					tx0iqkok = true;
+					RT_TRACE(rtlpriv, COMP_IQK,
+						 DBG_LOUD, "1");
+				}
+
+				/* ====== RX IQK ====== */
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+				/* 1. RX RF Setting */
+				rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
+				rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
+				rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0002f);
+				rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xfffbb);
+				rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x88001);
+				rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d8);
+				rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
+
+				rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0_rxk[cal])>>21&0x000007ff);
+				rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0_rxk[cal])>>21&0x000007ff);
+				rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
+				rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
+				/* ODM_SetBBReg(pDM_Odm, 0xcb8, 0xF, 0xe); */
+				rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
+				rtl_write_dword(rtlpriv, 0x984, 0x0046a911);
+
+				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+				rtl_write_dword(rtlpriv, 0xc80, 0x38008c10);/* TX_TONE_idx[9:0], TxK_Mask[29] TX_Tone = 16 */
+				rtl_write_dword(rtlpriv, 0xc84, 0x18008c10);/* RX_TONE_idx[9:0], RxK_Mask[29] */
+				rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
+
+				rtl_write_dword(rtlpriv, 0xc8c, 0x28160d00); /*pDM_Odm->SupportInterface == 1*/
+
+				rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] \B1N SI/PI \A8Ï¥\CE\C5v\A4\C1\B5\B9 iqk_dpk module */
+
+				cal_retry = 0;
+				while (1) {
+					/* one shot */
+					rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
+					rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
+
+					mdelay(10); /* Delay 10ms */
+					rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
+					delay_count = 0;
+					while (1) {
+						iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
+						if ((~iqk_ready) || (delay_count > 20))
+							break;
+						else{
+							mdelay(1);
+							delay_count++;
+						}
+					}
+
+					if (delay_count < 20) {	/* If 20ms No Result, then cal_retry++ */
+						/* ============RXIQK Check============== */
+						rx_fail = rtl_get_bbreg(hw, 0xd00, BIT(11));
+						if (rx_fail == 0) {
+							rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
+							rx_x0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+							rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
+							rx_y0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
+							rx0iqkok = true;
+							break;
+						} else{
+							rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x200>>1);
+							rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0x0>>1);
+							rx0iqkok = false;
+							cal_retry++;
+							if (cal_retry == 10)
+								break;
+
+						}
+					} else{
+						rx0iqkok = false;
+						cal_retry++;
+						if (cal_retry == 10)
+							break;
+					}
+				}
+			}
+
+			if (tx0iqkok)
+				tx_average++;
+			if (rx0iqkok)
+				rx_average++;
+			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+			rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
+			break;
+		default:
+			break;
+		}
+		cal++;
+	}
+
+	/* FillIQK Result */
+	switch (path) {
+	case RF90_PATH_A:
+		RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+			 "========Path_A =======\n");
+		if (tx_average == 0)
+			break;
+
+		for (i = 0; i < tx_average; i++) {
+			RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+				 "TX_X0_RXK[%d] = %x ;; TX_Y0_RXK[%d] = %x\n", i,
+				 (tx_x0_rxk[i])>>21&0x000007ff, i,
+				 (tx_y0_rxk[i])>>21&0x000007ff);
+			RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+				 "TX_X0[%d] = %x ;; TX_Y0[%d] = %x\n", i,
+				 (tx_x0[i])>>21&0x000007ff, i,
+				 (tx_y0[i])>>21&0x000007ff);
+		}
+		for (i = 0; i < tx_average; i++) {
+			for (ii = i+1; ii < tx_average; ii++) {
+				dx = (tx_x0[i]>>21) - (tx_x0[ii]>>21);
+				if (dx < 3 && dx > -3) {
+					dy = (tx_y0[i]>>21) - (tx_y0[ii]>>21);
+					if (dy < 3 && dy > -3) {
+						tx_x = ((tx_x0[i]>>21) + (tx_x0[ii]>>21))/2;
+						tx_y = ((tx_y0[i]>>21) + (tx_y0[ii]>>21))/2;
+						tx_finish = 1;
+						break;
+					}
+				}
+			}
+			if (tx_finish == 1)
+				break;
+		}
+
+		if (tx_finish == 1)
+			_rtl8821ae_iqk_tx_fill_iqc(hw, path, tx_x, tx_y); /* ? */
+		else
+			_rtl8821ae_iqk_tx_fill_iqc(hw, path, 0x200, 0x0);
+
+		if (rx_average == 0)
+			break;
+
+		for (i = 0; i < rx_average; i++)
+			RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+				"RX_X0[%d] = %x ;; RX_Y0[%d] = %x\n", i,
+				(rx_x0[i])>>21&0x000007ff, i,
+				(rx_y0[i])>>21&0x000007ff);
+		for (i = 0; i < rx_average; i++) {
+			for (ii = i+1; ii < rx_average; ii++) {
+				dx = (rx_x0[i]>>21) - (rx_x0[ii]>>21);
+				if (dx < 4 && dx > -4) {
+					dy = (rx_y0[i]>>21) - (rx_y0[ii]>>21);
+					if (dy < 4 && dy > -4) {
+						rx_x = ((rx_x0[i]>>21) + (rx_x0[ii]>>21))/2;
+						rx_y = ((rx_y0[i]>>21) + (rx_y0[ii]>>21))/2;
+						rx_finish = 1;
+						break;
+					}
+				}
+			}
+			if (rx_finish == 1)
+				break;
+		}
+
+		if (rx_finish == 1)
+			_rtl8821ae_iqk_rx_fill_iqc(hw, path, rx_x, rx_y);
+		else
+			_rtl8821ae_iqk_rx_fill_iqc(hw, path, 0x200, 0x0);
+		break;
+	default:
+		break;
+	}
+}
+
+static void _rtl8821ae_iqk_restore_rf(struct ieee80211_hw *hw,
+				      enum radio_path path,
+				      u32 *backup_rf_reg,
+				      u32 *rf_backup, u32 rf_reg_num)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u32 i;
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+	for (i = 0; i < RF_REG_NUM; i++)
+		rtl_set_rfreg(hw, path, backup_rf_reg[i], RFREG_OFFSET_MASK,
+			      rf_backup[i]);
+
+	switch (path) {
+	case RF90_PATH_A:
+		RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+			 "RestoreRF Path A Success!!!!\n");
+		break;
+	default:
+			break;
+	}
+}
+
+static void _rtl8821ae_iqk_restore_afe(struct ieee80211_hw *hw,
+				       u32 *afe_backup, u32 *backup_afe_reg,
+				       u32 afe_num)
+{
+	u32 i;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+	/* Reload AFE Parameters */
+	for (i = 0; i < afe_num; i++)
+		rtl_write_dword(rtlpriv, backup_afe_reg[i], afe_backup[i]);
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1 */
+	rtl_write_dword(rtlpriv, 0xc80, 0x0);
+	rtl_write_dword(rtlpriv, 0xc84, 0x0);
+	rtl_write_dword(rtlpriv, 0xc88, 0x0);
+	rtl_write_dword(rtlpriv, 0xc8c, 0x3c000000);
+	rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
+	rtl_write_dword(rtlpriv, 0xc94, 0x00000000);
+	rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
+	rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
+	rtl_write_dword(rtlpriv, 0xcb8, 0x0);
+	RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "RestoreAFE Success!!!!\n");
+}
+
+static void _rtl8821ae_iqk_restore_macbb(struct ieee80211_hw *hw,
+					 u32 *macbb_backup,
+					 u32 *backup_macbb_reg,
+					 u32 macbb_num)
+{
+	u32 i;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C */
+	/* Reload MacBB Parameters */
+	for (i = 0; i < macbb_num; i++)
+		rtl_write_dword(rtlpriv, backup_macbb_reg[i], macbb_backup[i]);
+	RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD, "RestoreMacBB Success!!!!\n");
+}
+
+#undef MACBB_REG_NUM
+#undef AFE_REG_NUM
+#undef RF_REG_NUM
+
+#define MACBB_REG_NUM 11
+#define AFE_REG_NUM 12
+#define RF_REG_NUM 3
+
+static void _rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw)
+{
+	u32	macbb_backup[MACBB_REG_NUM];
+	u32 afe_backup[AFE_REG_NUM];
+	u32 rfa_backup[RF_REG_NUM];
+	u32 rfb_backup[RF_REG_NUM];
+	u32 backup_macbb_reg[MACBB_REG_NUM] = {
+		0xb00, 0x520, 0x550, 0x808, 0x90c, 0xc00, 0xc50,
+		0xe00, 0xe50, 0x838, 0x82c
+	};
+	u32 backup_afe_reg[AFE_REG_NUM] = {
+		0xc5c, 0xc60, 0xc64, 0xc68, 0xc6c, 0xc70, 0xc74,
+		0xc78, 0xc7c, 0xc80, 0xc84, 0xcb8
+	};
+	u32	backup_rf_reg[RF_REG_NUM] = {0x65, 0x8f, 0x0};
+
+	_rtl8821ae_iqk_backup_macbb(hw, macbb_backup, backup_macbb_reg,
+				    MACBB_REG_NUM);
+	_rtl8821ae_iqk_backup_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
+	_rtl8821ae_iqk_backup_rf(hw, rfa_backup, rfb_backup, backup_rf_reg,
+				 RF_REG_NUM);
+
+	_rtl8821ae_iqk_configure_mac(hw);
+	_rtl8821ae_iqk_tx(hw, RF90_PATH_A);
+	_rtl8821ae_iqk_restore_rf(hw, RF90_PATH_A, backup_rf_reg, rfa_backup,
+				  RF_REG_NUM);
+
+	_rtl8821ae_iqk_restore_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
+	_rtl8821ae_iqk_restore_macbb(hw, macbb_backup, backup_macbb_reg,
+				     MACBB_REG_NUM);
+}
+
+static void _rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool main)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	/* struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw)); */
+	/* struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw)); */
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "\n");
+
+	if (main)
+		rtl_set_bbreg(hw, RA_RFE_PINMUX + 4, BIT(29) | BIT(28), 0x1);
+	else
+		rtl_set_bbreg(hw, RA_RFE_PINMUX + 4, BIT(29) | BIT(28), 0x2);
+}
+
+#undef IQK_ADDA_REG_NUM
+#undef IQK_DELAY_TIME
+
+void rtl8812ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
+{
+}
+
+void rtl8812ae_do_iqk(struct ieee80211_hw *hw, u8 delta_thermal_index,
+		      u8 thermal_value, u8 threshold)
+{
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+
+	rtldm->thermalvalue_iqk = thermal_value;
+	rtl8812ae_phy_iq_calibrate(hw, false);
+}
+
+void rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	if (!rtlphy->lck_inprogress) {
+		spin_lock(&rtlpriv->locks.iqk_lock);
+		rtlphy->lck_inprogress = true;
+		spin_unlock(&rtlpriv->locks.iqk_lock);
+
+		_rtl8821ae_phy_iq_calibrate(hw);
+
+		spin_lock(&rtlpriv->locks.iqk_lock);
+		rtlphy->lck_inprogress = false;
+		spin_unlock(&rtlpriv->locks.iqk_lock);
+	}
+}
+
+void rtl8821ae_reset_iqk_result(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 i;
+
+	RT_TRACE(rtlpriv, COMP_IQK, DBG_LOUD,
+		 "rtl8812ae_dm_reset_iqk_result:: settings regs %d default regs %d\n",
+		 (int)(sizeof(rtlphy->iqk_matrix) /
+		 sizeof(struct iqk_matrix_regs)),
+		 IQK_MATRIX_SETTINGS_NUM);
+
+	for (i = 0; i < IQK_MATRIX_SETTINGS_NUM; i++) {
+		rtlphy->iqk_matrix[i].value[0][0] = 0x100;
+		rtlphy->iqk_matrix[i].value[0][2] = 0x100;
+		rtlphy->iqk_matrix[i].value[0][4] = 0x100;
+		rtlphy->iqk_matrix[i].value[0][6] = 0x100;
+
+		rtlphy->iqk_matrix[i].value[0][1] = 0x0;
+		rtlphy->iqk_matrix[i].value[0][3] = 0x0;
+		rtlphy->iqk_matrix[i].value[0][5] = 0x0;
+		rtlphy->iqk_matrix[i].value[0][7] = 0x0;
+
+		rtlphy->iqk_matrix[i].iqk_done = false;
+	}
+}
+
+void rtl8821ae_do_iqk(struct ieee80211_hw *hw, u8 delta_thermal_index,
+		      u8 thermal_value, u8 threshold)
+{
+	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
+
+	rtl8821ae_reset_iqk_result(hw);
+
+	rtldm->thermalvalue_iqk = thermal_value;
+	rtl8821ae_phy_iq_calibrate(hw, false);
+}
+
+void rtl8821ae_phy_lc_calibrate(struct ieee80211_hw *hw)
+{
+}
+
+void rtl8821ae_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
+{
+}
+
+void rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
+{
+	_rtl8821ae_phy_set_rfpath_switch(hw, bmain);
+}
+
+bool rtl8821ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	bool postprocessing = false;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "-->IO Cmd(%#x), set_io_inprogress(%d)\n",
+		  iotype, rtlphy->set_io_inprogress);
+	do {
+		switch (iotype) {
+		case IO_CMD_RESUME_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Resume DM after scan.\n");
+			postprocessing = true;
+			break;
+		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		case IO_CMD_PAUSE_BAND1_DM_BY_SCAN:
+			RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+				 "[IO CMD] Pause DM before scan.\n");
+			postprocessing = true;
+			break;
+		default:
+			RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+				 "switch case not process\n");
+			break;
+		}
+	} while (false);
+	if (postprocessing && !rtlphy->set_io_inprogress) {
+		rtlphy->set_io_inprogress = true;
+		rtlphy->current_io_type = iotype;
+	} else {
+		return false;
+	}
+	rtl8821ae_phy_set_io(hw);
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE, "IO Type(%#x)\n", iotype);
+	return true;
+}
+
+static void rtl8821ae_phy_set_io(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct dig_t *dm_digtable = &rtlpriv->dm_digtable;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "--->Cmd(%#x), set_io_inprogress(%d)\n",
+		  rtlphy->current_io_type, rtlphy->set_io_inprogress);
+	switch (rtlphy->current_io_type) {
+	case IO_CMD_RESUME_DM_BY_SCAN:
+		if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC)
+			_rtl8821ae_resume_tx_beacon(hw);
+		rtl8821ae_dm_write_dig(hw, rtlphy->initgain_backup.xaagccore1);
+		rtl8821ae_dm_write_cck_cca_thres(hw,
+						 rtlphy->initgain_backup.cca);
+		break;
+	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
+		if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC)
+			_rtl8821ae_stop_tx_beacon(hw);
+		rtlphy->initgain_backup.xaagccore1 = dm_digtable->cur_igvalue;
+		rtl8821ae_dm_write_dig(hw, 0x17);
+		rtlphy->initgain_backup.cca = dm_digtable->cur_cck_cca_thres;
+		rtl8821ae_dm_write_cck_cca_thres(hw, 0x40);
+		break;
+	case IO_CMD_PAUSE_BAND1_DM_BY_SCAN:
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		break;
+	}
+	rtlphy->set_io_inprogress = false;
+	RT_TRACE(rtlpriv, COMP_CMD, DBG_TRACE,
+		 "(%#x)\n", rtlphy->current_io_type);
+}
+
+static void rtl8821ae_phy_set_rf_on(struct ieee80211_hw *hw)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
+	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
+	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
+}
+
+static bool _rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
+					      enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+	bool bresult = true;
+	u8 i, queue_id;
+	struct rtl8192_tx_ring *ring = NULL;
+
+	switch (rfpwr_state) {
+	case ERFON:
+		if ((ppsc->rfpwr_state == ERFOFF) &&
+		    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
+			bool rtstatus = false;
+			u32 initializecount = 0;
+
+			do {
+				initializecount++;
+				RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+					 "IPS Set eRf nic enable\n");
+				rtstatus = rtl_ps_enable_nic(hw);
+			} while (!rtstatus && (initializecount < 10));
+			RT_CLEAR_PS_LEVEL(ppsc,
+					  RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "Set ERFON sleeped:%d ms\n",
+				  jiffies_to_msecs(jiffies -
+						   ppsc->
+						   last_sleep_jiffies));
+			ppsc->last_awake_jiffies = jiffies;
+			rtl8821ae_phy_set_rf_on(hw);
+		}
+		if (mac->link_state == MAC80211_LINKED) {
+			rtlpriv->cfg->ops->led_control(hw,
+						       LED_CTL_LINK);
+		} else {
+			rtlpriv->cfg->ops->led_control(hw,
+						       LED_CTL_NO_LINK);
+		}
+		break;
+	case ERFOFF:
+		for (queue_id = 0, i = 0;
+		     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
+			ring = &pcipriv->dev.tx_ring[queue_id];
+			if (queue_id == BEACON_QUEUE ||
+			    skb_queue_len(&ring->queue) == 0) {
+				queue_id++;
+				continue;
+			} else {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "eRf Off/Sleep: %d times TcbBusyQueue[%d] =%d before doze!\n",
+					 (i + 1), queue_id,
+					 skb_queue_len(&ring->queue));
+
+				udelay(10);
+				i++;
+			}
+			if (i >= MAX_DOZE_WAITING_TIMES_9x) {
+				RT_TRACE(rtlpriv, COMP_ERR, DBG_WARNING,
+					 "\n ERFSLEEP: %d times TcbBusyQueue[%d] = %d !\n",
+					  MAX_DOZE_WAITING_TIMES_9x,
+					  queue_id,
+					  skb_queue_len(&ring->queue));
+				break;
+			}
+		}
+
+		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
+			RT_TRACE(rtlpriv, COMP_RF, DBG_DMESG,
+				 "IPS Set eRf nic disable\n");
+			rtl_ps_disable_nic(hw);
+			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
+		} else {
+			if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
+				rtlpriv->cfg->ops->led_control(hw,
+							       LED_CTL_NO_LINK);
+			} else {
+				rtlpriv->cfg->ops->led_control(hw,
+							       LED_CTL_POWER_OFF);
+			}
+		}
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "switch case not process\n");
+		bresult = false;
+		break;
+	}
+	if (bresult)
+		ppsc->rfpwr_state = rfpwr_state;
+	return bresult;
+}
+
+bool rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				      enum rf_pwrstate rfpwr_state)
+{
+	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
+
+	bool bresult = false;
+
+	if (rfpwr_state == ppsc->rfpwr_state)
+		return bresult;
+	bresult = _rtl8821ae_phy_set_rf_power_state(hw, rfpwr_state);
+	return bresult;
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/phy.h b/drivers/net/wireless/rtlwifi/rtl8821ae/phy.h
index a80bf739940a..c411f0a95cc4 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/phy.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/phy.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -30,39 +26,41 @@
 #ifndef __RTL8821AE_PHY_H__
 #define __RTL8821AE_PHY_H__
 
-/*It must always set to 4, otherwise read efuse table sequence will be wrong.*/
-#define MAX_TX_COUNT	4
-#define	TX_1S			0
-#define	TX_2S			1
-#define	TX_3S			2
-#define	TX_4S			3
+/* MAX_TX_COUNT must always be set to 4, otherwise read
+ * efuse table sequence will be wrong.
+ */
+#define MAX_TX_COUNT				4
+#define	TX_1S					0
+#define	TX_2S					1
+#define	TX_3S					2
+#define	TX_4S					3
 
-#define	MAX_POWER_INDEX	0x3F
+#define	MAX_POWER_INDEX				0x3F
 
-#define MAX_PRECMD_CNT 				16
-#define MAX_RFDEPENDCMD_CNT 		16
-#define MAX_POSTCMD_CNT 			16
+#define MAX_PRECMD_CNT				16
+#define MAX_RFDEPENDCMD_CNT			16
+#define MAX_POSTCMD_CNT				16
 
-#define MAX_DOZE_WAITING_TIMES_9x 	64
+#define MAX_DOZE_WAITING_TIMES_9x		64
 
 #define RT_CANNOT_IO(hw)			false
-#define HIGHPOWER_RADIOA_ARRAYLEN 	22
+#define HIGHPOWER_RADIOA_ARRAYLEN		22
 
 #define IQK_ADDA_REG_NUM			16
 #define IQK_BB_REG_NUM				9
 #define MAX_TOLERANCE				5
 #define	IQK_DELAY_TIME				10
-#define	index_mapping_NUM	15
+#define	index_mapping_NUM			15
 
 #define	APK_BB_REG_NUM				5
 #define	APK_AFE_REG_NUM				16
-#define	APK_CURVE_REG_NUM 			4
-#define	PATH_NUM					2
+#define	APK_CURVE_REG_NUM			4
+#define	PATH_NUM				2
 
-#define LOOP_LIMIT					5
+#define LOOP_LIMIT				5
 #define MAX_STALL_TIME				50
-#define AntennaDiversityValue		0x80
-#define MAX_TXPWR_IDX_NMODE_92S		63
+#define AntennaDiversityValue			0x80
+#define MAX_TXPWR_IDX_NMODE_92S			63
 #define Reset_Cnt_Limit				3
 
 #define IQK_ADDA_REG_NUM			16
@@ -70,10 +68,10 @@
 
 #define RF6052_MAX_PATH				2
 
-#define CT_OFFSET_MAC_ADDR 			0X16
+#define CT_OFFSET_MAC_ADDR			0X16
 
-#define CT_OFFSET_CCK_TX_PWR_IDX			0x5A
-#define CT_OFFSET_HT401S_TX_PWR_IDX			0x60
+#define CT_OFFSET_CCK_TX_PWR_IDX		0x5A
+#define CT_OFFSET_HT401S_TX_PWR_IDX		0x60
 #define CT_OFFSET_HT402S_TX_PWR_IDX_DIFF	0x66
 #define CT_OFFSET_HT20_TX_PWR_IDX_DIFF		0x69
 #define CT_OFFSET_OFDM_TX_PWR_IDX_DIFF		0x6C
@@ -81,15 +79,15 @@
 #define CT_OFFSET_HT40_MAX_PWR_OFFSET		0x6F
 #define CT_OFFSET_HT20_MAX_PWR_OFFSET		0x72
 
-#define CT_OFFSET_CHANNEL_PLAH				0x75
-#define CT_OFFSET_THERMAL_METER				0x78
-#define CT_OFFSET_RF_OPTION					0x79
-#define CT_OFFSET_VERSION					0x7E
-#define CT_OFFSET_CUSTOMER_ID				0x7F
+#define CT_OFFSET_CHANNEL_PLAH			0x75
+#define CT_OFFSET_THERMAL_METER			0x78
+#define CT_OFFSET_RF_OPTION			0x79
+#define CT_OFFSET_VERSION			0x7E
+#define CT_OFFSET_CUSTOMER_ID			0x7F
 
-#define RTL8821AE_MAX_PATH_NUM					2
+#define RTL8821AE_MAX_PATH_NUM			2
 
-#define TARGET_CHNL_NUM_2G_5G_8812	59
+#define TARGET_CHNL_NUM_2G_5G_8812		59
 
 enum swchnlcmd_id {
 	CMDID_END,
@@ -168,7 +166,6 @@ struct r_antenna_select_cck {
 	u8 r_ccktx_enable:4;
 };
 
-
 struct efuse_contents {
 	u8 mac_addr[ETH_ALEN];
 	u8 cck_tx_power_idx[6];
@@ -198,61 +195,65 @@ struct tx_power_struct {
 	u8 pwrgroup_cnt;
 	u32 mcs_original_offset[4][16];
 };
-enum _ANT_DIV_TYPE
-{
-	NO_ANTDIV				= 0xFF,
+enum _ANT_DIV_TYPE {
+	NO_ANTDIV			= 0xFF,
 	CG_TRX_HW_ANTDIV		= 0x01,
-	CGCS_RX_HW_ANTDIV 		= 0x02,
-	FIXED_HW_ANTDIV         = 0x03,
+	CGCS_RX_HW_ANTDIV		= 0x02,
+	FIXED_HW_ANTDIV     		= 0x03,
 	CG_TRX_SMART_ANTDIV		= 0x04,
 	CGCS_RX_SW_ANTDIV		= 0x05,
 
 };
 
-extern u32 rtl8821ae_phy_query_bb_reg(struct ieee80211_hw *hw,
-				   u32 regaddr, u32 bitmask);
-extern void rtl8821ae_phy_set_bb_reg(struct ieee80211_hw *hw,
-				  u32 regaddr, u32 bitmask, u32 data);
-extern u32 rtl8821ae_phy_query_rf_reg(struct ieee80211_hw *hw,
-				   enum radio_path rfpath, u32 regaddr,
-				   u32 bitmask);
-extern void rtl8821ae_phy_set_rf_reg(struct ieee80211_hw *hw,
-				  enum radio_path rfpath, u32 regaddr,
-				  u32 bitmask, u32 data);
-extern bool rtl8821ae_phy_mac_config(struct ieee80211_hw *hw);
-extern bool rtl8821ae_phy_bb_config(struct ieee80211_hw *hw);
-extern bool rtl8821ae_phy_rf_config(struct ieee80211_hw *hw);
-extern void rtl8821ae_phy_switch_wirelessband(struct ieee80211_hw *hw, u8 band);
-extern void rtl8821ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
-extern void rtl8821ae_phy_get_txpower_level(struct ieee80211_hw *hw,
-					 long *powerlevel);
-extern void rtl8821ae_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel);
-extern void rtl8821ae_phy_scan_operation_backup(struct ieee80211_hw *hw,
-					     u8 operation);
-extern void rtl8821ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
-extern void rtl8821ae_phy_set_bw_mode(struct ieee80211_hw *hw,
-				   enum nl80211_channel_type ch_type);
-extern void rtl8821ae_phy_sw_chnl_callback(struct ieee80211_hw *hw);
-extern u8 rtl8821ae_phy_sw_chnl(struct ieee80211_hw *hw);
-extern void rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
-extern void rtl8812ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery);
+u32 rtl8821ae_phy_query_bb_reg(struct ieee80211_hw *hw,
+			       u32 regaddr, u32 bitmask);
+void rtl8821ae_phy_set_bb_reg(struct ieee80211_hw *hw,
+			      u32 regaddr, u32 bitmask, u32 data);
+u32 rtl8821ae_phy_query_rf_reg(struct ieee80211_hw *hw,
+			       enum radio_path rfpath, u32 regaddr,
+			       u32 bitmask);
+void rtl8821ae_phy_set_rf_reg(struct ieee80211_hw *hw,
+			      enum radio_path rfpath, u32 regaddr,
+			      u32 bitmask, u32 data);
+bool rtl8821ae_phy_mac_config(struct ieee80211_hw *hw);
+bool rtl8821ae_phy_bb_config(struct ieee80211_hw *hw);
+bool rtl8821ae_phy_rf_config(struct ieee80211_hw *hw);
+void rtl8821ae_phy_switch_wirelessband(struct ieee80211_hw *hw,
+				       u8 band);
+void rtl8821ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw);
+void rtl8821ae_phy_get_txpower_level(struct ieee80211_hw *hw,
+				     long *powerlevel);
+void rtl8821ae_phy_set_txpower_level(struct ieee80211_hw *hw,
+				     u8 channel);
+void rtl8821ae_phy_scan_operation_backup(struct ieee80211_hw *hw,
+					 u8 operation);
+void rtl8821ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw);
+void rtl8821ae_phy_set_bw_mode(struct ieee80211_hw *hw,
+			       enum nl80211_channel_type ch_type);
+void rtl8821ae_phy_sw_chnl_callback(struct ieee80211_hw *hw);
+u8 rtl8821ae_phy_sw_chnl(struct ieee80211_hw *hw);
+void rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw,
+				bool b_recovery);
+void rtl8812ae_phy_iq_calibrate(struct ieee80211_hw *hw,
+				bool b_recovery);
 void rtl8821ae_phy_ap_calibrate(struct ieee80211_hw *hw, char delta);
 void rtl8821ae_phy_lc_calibrate(struct ieee80211_hw *hw);
 void rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain);
 bool rtl8812ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
-					  enum radio_path rfpath);
+					     enum radio_path rfpath);
 bool rtl8821ae_phy_config_rf_with_headerfile(struct ieee80211_hw *hw,
-					  enum radio_path rfpath);
+					     enum radio_path rfpath);
 bool rtl8821ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype);
-extern bool rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
-					  enum rf_pwrstate rfpwr_state);
+bool rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
+				      enum rf_pwrstate rfpwr_state);
 u8 _rtl8812ae_get_right_chnl_place_for_iqk(u8 chnl);
-void rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw, u8 channel, u8 path);
-void rtl8812ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
-	u8 thermal_value, u8 threshold);
-void rtl8821ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
+void rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw,
+					     u8 channel, u8 path);
+void rtl8812ae_do_iqk(struct ieee80211_hw *hw, u8 delta_thermal_index,
 	u8 thermal_value, u8 threshold);
+void rtl8821ae_do_iqk(struct ieee80211_hw *hw, u8 delta_thermal_index,
+		      u8 thermal_value, u8 threshold);
 void rtl8821ae_reset_iqk_result(struct ieee80211_hw *hw);
-
+u32 phy_get_tx_swing_8812A(struct ieee80211_hw *hw, u8 band, u8 rf_path);
 
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.c b/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.c
index a2e4a01b712b..9ddf78a187dd 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -27,123 +23,116 @@
  *
  *****************************************************************************/
 
-#include "pwrseqcmd.h"
+#include "../pwrseqcmd.h"
 #include "pwrseq.h"
 
-/*
-    drivers should parse below arrays and do the corresponding actions
-*/
-//3 Power on  Array
-struct wlan_pwr_cfg rtl8812_power_on_flow[RTL8812_TRANS_CARDEMU_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS]=
-{
+/* drivers should parse below arrays and do the corresponding actions */
+/* 3 Power on  Array */
+struct wlan_pwr_cfg rtl8812_power_on_flow[RTL8812_TRANS_CARDEMU_TO_ACT_STEPS +
+					RTL8812_TRANS_END_STEPS] = {
 	RTL8812_TRANS_CARDEMU_TO_ACT
 	RTL8812_TRANS_END
 };
 
-//3Radio off GPIO Array
-struct wlan_pwr_cfg rtl8812_radio_off_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_END_STEPS]=
-{
+/* 3Radio off GPIO Array */
+struct wlan_pwr_cfg rtl8812_radio_off_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+						RTL8812_TRANS_END_STEPS] = {
 	RTL8812_TRANS_ACT_TO_CARDEMU
 	RTL8812_TRANS_END
 };
 
-//3Card Disable Array
-struct wlan_pwr_cfg rtl8812_card_disable_flow[ RTL8812_TRANS_ACT_TO_CARDEMU_STEPS
+/* 3Card Disable Array */
+struct wlan_pwr_cfg rtl8812_card_disable_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS
 	+ RTL8812_TRANS_CARDEMU_TO_PDN_STEPS
-	+ RTL8812_TRANS_END_STEPS ] =
-{
+	+ RTL8812_TRANS_END_STEPS] = {
 	RTL8812_TRANS_ACT_TO_CARDEMU
 	RTL8812_TRANS_CARDEMU_TO_CARDDIS
 	RTL8812_TRANS_END
 };
 
-//3 Card Enable Array
-struct wlan_pwr_cfg rtl8812_card_enable_flow[ RTL8812_TRANS_ACT_TO_CARDEMU_STEPS
+/* 3 Card Enable Array */
+struct wlan_pwr_cfg rtl8812_card_enable_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS
 	+ RTL8812_TRANS_CARDEMU_TO_PDN_STEPS
-	+ RTL8812_TRANS_END_STEPS ] =
-{
+	+ RTL8812_TRANS_END_STEPS] = {
 	RTL8812_TRANS_CARDDIS_TO_CARDEMU
 	RTL8812_TRANS_CARDEMU_TO_ACT
 	RTL8812_TRANS_END
 };
 
-//3Suspend Array
-struct wlan_pwr_cfg rtl8812_suspend_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS]=
-{
+/* 3Suspend Array */
+struct wlan_pwr_cfg rtl8812_suspend_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+					RTL8812_TRANS_CARDEMU_TO_SUS_STEPS +
+					RTL8812_TRANS_END_STEPS] = {
 	RTL8812_TRANS_ACT_TO_CARDEMU
 	RTL8812_TRANS_CARDEMU_TO_SUS
 	RTL8812_TRANS_END
 };
 
-//3 Resume Array
-struct wlan_pwr_cfg rtl8812_resume_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS]=
-{
+/* 3 Resume Array */
+struct wlan_pwr_cfg rtl8812_resume_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+					RTL8812_TRANS_CARDEMU_TO_SUS_STEPS +
+					RTL8812_TRANS_END_STEPS] = {
 	RTL8812_TRANS_SUS_TO_CARDEMU
 	RTL8812_TRANS_CARDEMU_TO_ACT
 	RTL8812_TRANS_END
 };
 
-
-
-//3HWPDN Array
-struct wlan_pwr_cfg rtl8812_hwpdn_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS]=
-{
+/* 3HWPDN Array */
+struct wlan_pwr_cfg rtl8812_hwpdn_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+					RTL8812_TRANS_CARDEMU_TO_PDN_STEPS +
+					RTL8812_TRANS_END_STEPS] = {
 	RTL8812_TRANS_ACT_TO_CARDEMU
 	RTL8812_TRANS_CARDEMU_TO_PDN
 	RTL8812_TRANS_END
 };
 
-//3 Enter LPS
-struct wlan_pwr_cfg rtl8812_enter_lps_flow[RTL8812_TRANS_ACT_TO_LPS_STEPS+RTL8812_TRANS_END_STEPS]=
-{
-	//FW behavior
+/* 3 Enter LPS */
+struct wlan_pwr_cfg rtl8812_enter_lps_flow[RTL8812_TRANS_ACT_TO_LPS_STEPS +
+						RTL8812_TRANS_END_STEPS] = {
+	/* FW behavior */
 	RTL8812_TRANS_ACT_TO_LPS
 	RTL8812_TRANS_END
 };
 
-//3 Leave LPS
-struct wlan_pwr_cfg rtl8812_leave_lps_flow[RTL8812_TRANS_LPS_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS]=
-{
-	//FW behavior
+/* 3 Leave LPS */
+struct wlan_pwr_cfg rtl8812_leave_lps_flow[RTL8812_TRANS_LPS_TO_ACT_STEPS +
+						RTL8812_TRANS_END_STEPS] = {
+	/* FW behavior */
 	RTL8812_TRANS_LPS_TO_ACT
 	RTL8812_TRANS_END
 };
 
-
-/*
-    drivers should parse below arrays and do the corresponding actions
-*/
+/* drivers should parse below arrays and do the corresponding actions */
 /*3 Power on  Array*/
 struct wlan_pwr_cfg rtl8821A_power_on_flow[RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+					+ RTL8821A_TRANS_END_STEPS] = {
 	RTL8821A_TRANS_CARDEMU_TO_ACT
 	RTL8821A_TRANS_END
 };
 
 /*3Radio off GPIO Array */
 struct wlan_pwr_cfg rtl8821A_radio_off_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+					+ RTL8821A_TRANS_END_STEPS] = {
 	RTL8821A_TRANS_ACT_TO_CARDEMU
 	RTL8821A_TRANS_END
 };
 
 /*3Card Disable Array*/
-struct wlan_pwr_cfg rtl8821A_card_disable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+struct wlan_pwr_cfg rtl8821A_card_disable_flow
+					[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
 					+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+					+ RTL8821A_TRANS_END_STEPS] = {
 	RTL8821A_TRANS_ACT_TO_CARDEMU
 	RTL8821A_TRANS_CARDEMU_TO_CARDDIS
 	RTL8821A_TRANS_END
 };
 
 /*3 Card Enable Array*/
-struct wlan_pwr_cfg rtl8821A_card_enable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-					+ RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS /*RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS*/
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+/*RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS*/
+struct wlan_pwr_cfg rtl8821A_card_enable_flow
+					[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
+					+ RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS
+					+ RTL8821A_TRANS_END_STEPS] = {
 	RTL8821A_TRANS_CARDDIS_TO_CARDEMU
 	RTL8821A_TRANS_CARDEMU_TO_ACT
 	RTL8821A_TRANS_END
@@ -152,8 +141,7 @@ struct wlan_pwr_cfg rtl8821A_card_enable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEP
 /*3Suspend Array*/
 struct wlan_pwr_cfg rtl8821A_suspend_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
 					+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+					+ RTL8821A_TRANS_END_STEPS] = {
 	RTL8821A_TRANS_ACT_TO_CARDEMU
 	RTL8821A_TRANS_CARDEMU_TO_SUS
 	RTL8821A_TRANS_END
@@ -162,8 +150,7 @@ struct wlan_pwr_cfg rtl8821A_suspend_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
 /*3 Resume Array*/
 struct wlan_pwr_cfg rtl8821A_resume_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
 					+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+					+ RTL8821A_TRANS_END_STEPS] = {
 	RTL8821A_TRANS_SUS_TO_CARDEMU
 	RTL8821A_TRANS_CARDEMU_TO_ACT
 	RTL8821A_TRANS_END
@@ -172,8 +159,7 @@ struct wlan_pwr_cfg rtl8821A_resume_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
 /*3HWPDN Array*/
 struct wlan_pwr_cfg rtl8821A_hwpdn_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
 				+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
-				+ RTL8821A_TRANS_END_STEPS] =
-{
+				+ RTL8821A_TRANS_END_STEPS] = {
 	RTL8821A_TRANS_ACT_TO_CARDEMU
 	RTL8821A_TRANS_CARDEMU_TO_PDN
 	RTL8821A_TRANS_END
@@ -181,8 +167,7 @@ struct wlan_pwr_cfg rtl8821A_hwpdn_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
 
 /*3 Enter LPS */
 struct wlan_pwr_cfg rtl8821A_enter_lps_flow[RTL8821A_TRANS_ACT_TO_LPS_STEPS
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+					+ RTL8821A_TRANS_END_STEPS] = {
 	/*FW behavior*/
 	RTL8821A_TRANS_ACT_TO_LPS
 	RTL8821A_TRANS_END
@@ -190,10 +175,8 @@ struct wlan_pwr_cfg rtl8821A_enter_lps_flow[RTL8821A_TRANS_ACT_TO_LPS_STEPS
 
 /*3 Leave LPS */
 struct wlan_pwr_cfg rtl8821A_leave_lps_flow[RTL8821A_TRANS_LPS_TO_ACT_STEPS
-					+ RTL8821A_TRANS_END_STEPS] =
-{
+					+ RTL8821A_TRANS_END_STEPS] = {
 	/*FW behavior*/
 	RTL8821A_TRANS_LPS_TO_ACT
 	RTL8821A_TRANS_END
 };
-
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.h b/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.h
new file mode 100644
index 000000000000..bf0b0ce9519c
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/pwrseq.h
@@ -0,0 +1,738 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_PWRSEQ_H__
+#define __RTL8821AE_PWRSEQ_H__
+
+#include "../pwrseqcmd.h"
+#include "../btcoexist/halbt_precomp.h"
+
+#define	RTL8812_TRANS_CARDEMU_TO_ACT_STEPS	15
+#define	RTL8812_TRANS_ACT_TO_CARDEMU_STEPS	15
+#define	RTL8812_TRANS_CARDEMU_TO_SUS_STEPS	15
+#define	RTL8812_TRANS_SUS_TO_CARDEMU_STEPS	15
+#define	RTL8812_TRANS_CARDEMU_TO_PDN_STEPS	25
+#define	RTL8812_TRANS_PDN_TO_CARDEMU_STEPS	15
+#define	RTL8812_TRANS_ACT_TO_LPS_STEPS		15
+#define	RTL8812_TRANS_LPS_TO_ACT_STEPS		15
+#define	RTL8812_TRANS_END_STEPS			1
+
+/* The following macros have the following format:
+ * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value
+ *   comments },
+ */
+#define RTL8812_TRANS_CARDEMU_TO_ACT					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, 0 \
+	/* disable SW LPS 0x04[10]=0*/},	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, BIT1 \
+	/* wait till 0x04[17] = 1    power ready*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0 \
+	/* disable HWPDN 0x04[15]=0*/}, \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3, 0 \
+	/* disable WL suspend*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0 \
+	/* polling until return 0*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT0, 0},
+
+#define RTL8812_TRANS_ACT_TO_CARDEMU													\
+	{0x0c00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x04 \
+	 /* 0xc00[7:0] = 4	turn off 3-wire */},	\
+	{0x0e00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x04 \
+	 /* 0xe00[7:0] = 4	turn off 3-wire */},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0 \
+	 /* 0x2[0] = 0	 RESET BB, CLOSE RF */},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US \
+	/*Delay 1us*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	  /* Whole BB is reset*/},			\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x2A \
+	 /* 0x07[7:0] = 0x28 sps pwm mode 0x2a for BT coex*/},	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x02, 0 \
+	/*0x8[1] = 0 ANA clk =500k */},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1 \
+	 /*0x04[9] = 1 turn off MAC by HW state machine*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, 0 \
+	 /*wait till 0x04[9] = 0 polling until return 0 to disable*/},
+
+#define RTL8812_TRANS_CARDEMU_TO_SUS					\
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xcc}, \
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xEC}, \
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x07 \
+	/* gpio11 input mode, gpio10~8 output mode */},	\
+	{0x0045, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00 \
+	/* gpio 0~7 output same value as input ?? */},	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xff \
+	/* gpio0~7 output mode */},	\
+	{0x0047, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	/* 0x47[7:0] = 00 gpio mode */},	\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	/* suspend option all off */},	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x80, BIT7 \
+	/*0x14[7] = 1 turn on ZCD */},	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x01, BIT0 \
+	/* 0x15[0] =1 trun on ZCD */},	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x10, BIT4 \
+	/*0x23[4] = 1 hpon LDO sleep mode */},	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x02, 0 \
+	/*0x8[1] = 0 ANA clk =500k */},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3, BIT3 \
+	/*0x04[11] = 2b'11 enable WL suspend for PCIe*/},
+
+#define RTL8812_TRANS_SUS_TO_CARDEMU					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3, 0 \
+	/*0x04[11] = 2b'01enable WL suspend*/},   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x10, 0 \
+	/*0x23[4] = 0 hpon LDO sleep mode leave */},	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x01, 0 \
+	/* 0x15[0] =0 trun off ZCD */},	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x80, 0 \
+	/*0x14[7] = 0 turn off ZCD */},	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00 \
+	/* gpio0~7 input mode */},	\
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00 \
+	/* gpio11 input mode, gpio10~8 input mode */},
+
+#define RTL8812_TRANS_CARDEMU_TO_CARDDIS				\
+	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, 0 \
+	/*0x03[2] = 0, reset 8051*/},	\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x05 \
+	/*0x80=05h if reload fw, fill the default value of host_CPU handshake field*/},	\
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xcc}, \
+	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xF0, 0xEC}, \
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x07 \
+	/* gpio11 input mode, gpio10~8 output mode */},	\
+	{0x0045, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00 \
+	/* gpio 0~7 output same value as input ?? */},	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xff \
+	/* gpio0~7 output mode */},	\
+	{0x0047, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	/* 0x47[7:0] = 00 gpio mode */},	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x80, BIT7 \
+	/*0x14[7] = 1 turn on ZCD */},	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x01, BIT0 \
+	/* 0x15[0] =1 trun on ZCD */},	\
+	{0x0012, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x01, 0 \
+	/*0x12[0] = 0 force PFM mode */},	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x10, BIT4 \
+	/*0x23[4] = 1 hpon LDO sleep mode */},	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x02, 0 \
+	/*0x8[1] = 0 ANA clk =500k */},	\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20 \
+	 /*0x07=0x20 , SOP option to disable BG/MB*/},	\
+	{0x001f, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	 /*0x01f[1]=0 , disable RFC_0  control  REG_RF_CTRL_8812 */},	\
+	{0x0076, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	 /*0x076[1]=0 , disable RFC_1  control REG_OPT_CTRL_8812 +2 */},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3, BIT3 \
+	 /*0x04[11] = 2b'01 enable WL suspend*/},
+
+#define RTL8812_TRANS_CARDDIS_TO_CARDEMU				\
+	{0x0012, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0 \
+	/*0x12[0] = 1 force PWM mode */},	\
+	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x80, 0 \
+	/*0x14[7] = 0 turn off ZCD */},	\
+	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x01, 0 \
+	/* 0x15[0] =0 trun off ZCD */},	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0x10, 0 \
+	/*0x23[4] = 0 hpon LDO leave sleep mode */},	\
+	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00 \
+	/* gpio0~7 input mode */},	\
+	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00 \
+	/* gpio11 input mode, gpio10~8 input mode */}, \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, 0 \
+	 /*0x04[10] = 0, enable SW LPS PCIE only*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3, 0 \
+	 /*0x04[11] = 2b'01enable WL suspend*/},	\
+	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, BIT2 \
+	 /*0x03[2] = 1, enable 8051*/},	\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	/*PCIe DMA start*/},
+
+#define RTL8812_TRANS_CARDEMU_TO_PDN		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7 \
+	/* 0x04[15] = 1*/},
+
+#define RTL8812_TRANS_PDN_TO_CARDEMU			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0 \
+	/* 0x04[15] = 0*/},
+
+#define RTL8812_TRANS_ACT_TO_LPS		\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF \
+	/*PCIe DMA stop*/},	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x7F \
+	/*Tx Pause*/},		\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x0c00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x04 \
+	 /* 0xc00[7:0] = 4	turn off 3-wire */},	\
+	{0x0e00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x04 \
+	 /* 0xe00[7:0] = 4	turn off 3-wire */},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0 \
+	/*CCK and OFDM are disabled,and clock are gated,and RF closed*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US \
+	/*Delay 1us*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	  /* Whole BB is reset*/},			\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x03 \
+	/*Reset MAC TRX*/},			\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	/*check if removed later*/},		\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5 \
+	/*Respond TxOK to scheduler*/},
+
+#define RTL8812_TRANS_LPS_TO_ACT					\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84 \
+	 /*SDIO RPWM*/},	\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84 \
+	 /*USB RPWM*/},	\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84 \
+	 /*PCIe RPWM*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS \
+	 /*Delay*/},	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0 \
+	 /*.	0x08[4] = 0		 switch TSF to 40M*/},	\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT7, 0 \
+	 /*Polling 0x109[7]=0  TSF in 40M*/},			\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6|BIT7, 0 \
+	 /*.	0x29[7:6] = 2b'00	 enable BB clock*/},	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1 \
+	 /*.	0x101[1] = 1*/},					\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF \
+	 /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0 \
+	 /*.	0x02[1:0] = 2b'11	 enable BB macro*/},	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	 /*.	0x522 = 0*/},
+
+#define RTL8812_TRANS_END					\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, \
+	0, PWR_CMD_END, 0, 0},
+
+extern struct wlan_pwr_cfg  rtl8812_power_on_flow
+		[RTL8812_TRANS_CARDEMU_TO_ACT_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_radio_off_flow
+		[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_card_disable_flow
+		[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8812_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_card_enable_flow
+		[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8812_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_suspend_flow
+		[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8812_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_resume_flow
+		[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8812_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_hwpdn_flow
+		[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8812_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_enter_lps_flow
+		[RTL8812_TRANS_ACT_TO_LPS_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg  rtl8812_leave_lps_flow
+		[RTL8812_TRANS_LPS_TO_ACT_STEPS +
+		 RTL8812_TRANS_END_STEPS];
+
+/* Check document WM-20130516-JackieLau-RTL8821A_Power_Architecture-R10.vsd
+ *	There are 6 HW Power States:
+ *	0: POFF--Power Off
+ *	1: PDN--Power Down
+ *	2: CARDEMU--Card Emulation
+ *	3: ACT--Active Mode
+ *	4: LPS--Low Power State
+ *	5: SUS--Suspend
+ *
+ *	The transision from different states are defined below
+ *	TRANS_CARDEMU_TO_ACT
+ *	TRANS_ACT_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_SUS
+ *	TRANS_SUS_TO_CARDEMU
+ *	TRANS_CARDEMU_TO_PDN
+ *	TRANS_ACT_TO_LPS
+ *	TRANS_LPS_TO_ACT
+ *
+ *	TRANS_END
+ */
+#define	RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS	25
+#define	RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS	15
+#define	RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS	15
+#define	RTL8821A_TRANS_SUS_TO_CARDEMU_STEPS	15
+#define RTL8821A_TRANS_CARDDIS_TO_CARDEMU_STEPS	15
+#define	RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS	15
+#define	RTL8821A_TRANS_PDN_TO_CARDEMU_STEPS	15
+#define	RTL8821A_TRANS_ACT_TO_LPS_STEPS		15
+#define	RTL8821A_TRANS_LPS_TO_ACT_STEPS		15
+#define	RTL8821A_TRANS_END_STEPS		1
+
+#define RTL8821A_TRANS_CARDEMU_TO_ACT					\
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0 \
+	 /*0x20[0] = 1b'1 enable LDOA12 MACRO block for all interface*/},   \
+	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,		\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0 \
+	 /*0x67[0] = 0 to disable BT_GPS_SEL pins*/},	\
+	{0x0001, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 1, PWRSEQ_DELAY_MS \
+	/*Delay 1ms*/},   \
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, 0 \
+	 /*0x00[5] = 1b'0 release analog Ips to digital ,1:isolation*/},   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT4|BIT3|BIT2), 0 \
+	/* disable SW LPS 0x04[10]=0 and WLSUS_EN 0x04[12:11]=0*/},	\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0 , BIT0 \
+	/* Disable USB suspend */},	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, BIT1 \
+	/* wait till 0x04[17] = 1    power ready*/},	\
+	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0 , 0 \
+	/* Enable USB suspend */},	\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0 \
+	/* release WLON reset  0x04[16]=1*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0 \
+	/* disable HWPDN 0x04[15]=0*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT4|BIT3), 0 \
+	/* disable WL suspend*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0 \
+	/* polling until return 0*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT0, 0 \
+	/**/},	\
+	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0 \
+	/*0x4C[24] = 0x4F[0] = 1, switch DPDT_SEL_P output from WL BB */},\
+	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, (BIT5|BIT4), (BIT5|BIT4) \
+	/*0x66[13] = 0x67[5] = 1, switch for PAPE_G/PAPE_A 	\
+	 from WL BB ; 0x66[12] = 0x67[4] = 1, switch LNAON from WL BB */},\
+	{0x0025, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6, 0 \
+	/*anapar_mac<118> , 0x25[6]=0 by wlan single function*/},\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1 \
+	/*Enable falling edge triggering interrupt*/},\
+	{0x0063, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1 \
+	/*Enable GPIO9 interrupt mode*/},\
+	{0x0062, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	/*Enable GPIO9 input mode*/},\
+	{0x0058, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0 \
+	/*Enable HSISR GPIO[C:0] interrupt*/},\
+	{0x005A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1 \
+	/*Enable HSISR GPIO9 interrupt*/},\
+	{0x007A, PWR_CUT_TESTCHIP_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3A \
+	/*0x7A = 0x3A start BT*/},\
+	{0x002E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF , 0x82  \
+	/* 0x2C[23:12]=0x820 ; XTAL trim */}, \
+	{0x0010, PWR_CUT_A_MSK , PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6 , BIT6  \
+	/* 0x10[6]=1  */},
+
+#define RTL8821A_TRANS_ACT_TO_CARDEMU					\
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	/*0x1F[7:0] = 0 turn off RF*/},	\
+	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0 \
+	/*0x4C[24] = 0x4F[0] = 0, switch DPDT_SEL_P output from		\
+	 register 0x65[2] */},\
+	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	/*Enable rising edge triggering interrupt*/}, \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1 \
+	 /*0x04[9] = 1 turn off MAC by HW state machine*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, 0 \
+	 /*wait till 0x04[9] = 0 polling until return 0 to disable*/},	\
+	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5 \
+	 /*0x00[5] = 1b'1 analog Ips to digital ,1:isolation*/},   \
+	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,		\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0 \
+	 /*0x20[0] = 1b'0 disable LDOA12 MACRO block*/},
+
+#define RTL8821A_TRANS_CARDEMU_TO_SUS					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4|BIT3, (BIT4|BIT3) \
+	 /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,		\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3 \
+	 /*0x04[12:11] = 2b'01 enable WL suspend*/},	\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4 \
+	 /*0x23[4] = 1b'1 12H LDO enter sleep mode*/},   \
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20 \
+	 /*0x07[7:0] = 0x20 SDIO SOP option to disable BG/MB/ACK/SWR*/},   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3|BIT4 \
+	 /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/},	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0 \
+	 /*Set SDIO suspend local register*/},	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0 \
+	 /*wait power state to suspend*/},
+
+#define RTL8821A_TRANS_SUS_TO_CARDEMU					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3 | BIT7, 0 \
+	 /*clear suspend enable and power down enable*/},	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0 \
+	 /*Set SDIO suspend local register*/},	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1 \
+	 /*wait power state to suspend*/},\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0 \
+	 /*0x23[4] = 1b'0 12H LDO enter normal mode*/},   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0 \
+	 /*0x04[12:11] = 2b'01enable WL suspend*/},
+
+#define RTL8821A_TRANS_CARDEMU_TO_CARDDIS				\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20 \
+	 /*0x07=0x20 , SOP option to disable BG/MB*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,		\
+	 PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3 \
+	 /*0x04[12:11] = 2b'01 enable WL suspend*/},	\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT2, BIT2 \
+	 /*0x04[10] = 1, enable SW LPS*/},	\
+        {0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 1 \
+	 /*0x48[16] = 1 to enable GPIO9 as EXT WAKEUP*/},   \
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4 \
+	 /*0x23[4] = 1b'1 12H LDO enter sleep mode*/},   \
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0 \
+	 /*Set SDIO suspend local register*/},	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0 \
+	 /*wait power state to suspend*/},
+
+#define RTL8821A_TRANS_CARDDIS_TO_CARDEMU				\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3 | BIT7, 0 \
+	 /*clear suspend enable and power down enable*/},	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0 \
+	 /*Set SDIO suspend local register*/},	\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1 \
+	 /*wait power state to suspend*/},\
+	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0 \
+	 /*0x48[16] = 0 to disable GPIO9 as EXT WAKEUP*/},   \
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0 \
+	 /*0x04[12:11] = 2b'01enable WL suspend*/},\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0 \
+	 /*0x23[4] = 1b'0 12H LDO enter normal mode*/},   \
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	/*PCIe DMA start*/},
+
+#define RTL8821A_TRANS_CARDEMU_TO_PDN					\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4 \
+	 /*0x23[4] = 1b'1 12H LDO enter sleep mode*/},   \
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,		\
+	 PWR_INTF_SDIO_MSK|PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x20 \
+	 /*0x07[7:0] = 0x20 SOP option to disable BG/MB/ACK/SWR*/},   \
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0 \
+	/* 0x04[16] = 0*/},\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7 \
+	/* 0x04[15] = 1*/},
+
+#define RTL8821A_TRANS_PDN_TO_CARDEMU				\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0 \
+	/* 0x04[15] = 0*/},
+
+#define RTL8821A_TRANS_ACT_TO_LPS					\
+	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF \
+	/*PCIe DMA stop*/},	\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF \
+	/*Tx Pause*/},	\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0 \
+	/*Should be zero if no packet is transmitting*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0 \
+	/*CCK and OFDM are disabled,and clock are gated*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US \
+	/*Delay 1us*/},	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	/*Whole BB is reset*/},	\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x03 \
+	/*Reset MAC TRX*/},	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0 \
+	/*check if removed later*/},	\
+	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x00 \
+	/*When driver enter Sus/ Disable, enable LOP for BT*/},	\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5 \
+	/*Respond TxOK to scheduler*/},
+
+#define RTL8821A_TRANS_LPS_TO_ACT					\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84 \
+	 /*SDIO RPWM*/},\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84 \
+	 /*USB RPWM*/},\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84 \
+	 /*PCIe RPWM*/},\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS \
+	 /*Delay*/},\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0 \
+	 /*.	0x08[4] = 0		 switch TSF to 40M*/},\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT7, 0 \
+	 /*Polling 0x109[7]=0  TSF in 40M*/},\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6|BIT7, 0 \
+	 /*.	0x29[7:6] = 2b'00	 enable BB clock*/},\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1 \
+	 /*.	0x101[1] = 1*/},\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF \
+	 /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/},\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0 \
+	 /*.	0x02[1:0] = 2b'11	 enable BB macro*/},\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0 \
+	 /*.	0x522 = 0*/},
+
+#define RTL8821A_TRANS_END					\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,\
+	0, PWR_CMD_END, 0, 0},
+
+extern struct wlan_pwr_cfg rtl8821A_power_on_flow
+		[RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_radio_off_flow
+		[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_card_disable_flow
+		[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_card_enable_flow
+		[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_suspend_flow
+		[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_resume_flow
+		[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_hwpdn_flow
+		[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS +
+		 RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_enter_lps_flow
+		[RTL8821A_TRANS_ACT_TO_LPS_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+extern struct wlan_pwr_cfg rtl8821A_leave_lps_flow
+		[RTL8821A_TRANS_LPS_TO_ACT_STEPS +
+		 RTL8821A_TRANS_END_STEPS];
+
+/*RTL8812 Power Configuration CMDs for PCIe interface*/
+#define RTL8812_NIC_PWR_ON_FLOW			rtl8812_power_on_flow
+#define RTL8812_NIC_RF_OFF_FLOW			rtl8812_radio_off_flow
+#define RTL8812_NIC_DISABLE_FLOW		rtl8812_card_disable_flow
+#define RTL8812_NIC_ENABLE_FLOW			rtl8812_card_enable_flow
+#define RTL8812_NIC_SUSPEND_FLOW		rtl8812_suspend_flow
+#define RTL8812_NIC_RESUME_FLOW			rtl8812_resume_flow
+#define RTL8812_NIC_PDN_FLOW			rtl8812_hwpdn_flow
+#define RTL8812_NIC_LPS_ENTER_FLOW		rtl8812_enter_lps_flow
+#define RTL8812_NIC_LPS_LEAVE_FLOW		rtl8812_leave_lps_flow
+
+/* RTL8821 Power Configuration CMDs for PCIe interface */
+#define RTL8821A_NIC_PWR_ON_FLOW		rtl8821A_power_on_flow
+#define RTL8821A_NIC_RF_OFF_FLOW		rtl8821A_radio_off_flow
+#define RTL8821A_NIC_DISABLE_FLOW		rtl8821A_card_disable_flow
+#define RTL8821A_NIC_ENABLE_FLOW		rtl8821A_card_enable_flow
+#define RTL8821A_NIC_SUSPEND_FLOW		rtl8821A_suspend_flow
+#define RTL8821A_NIC_RESUME_FLOW		rtl8821A_resume_flow
+#define RTL8821A_NIC_PDN_FLOW			rtl8821A_hwpdn_flow
+#define RTL8821A_NIC_LPS_ENTER_FLOW		rtl8821A_enter_lps_flow
+#define RTL8821A_NIC_LPS_LEAVE_FLOW		rtl8821A_leave_lps_flow
+
+#endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/reg.h b/drivers/net/wireless/rtlwifi/rtl8821ae/reg.h
new file mode 100644
index 000000000000..53668fc8f23e
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/reg.h
@@ -0,0 +1,2464 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#ifndef __RTL8821AE_REG_H__
+#define __RTL8821AE_REG_H__
+
+#define TXPKT_BUF_SELECT			0x69
+#define RXPKT_BUF_SELECT			0xA5
+#define DISABLE_TRXPKT_BUF_ACCESS		0x0
+
+#define REG_SYS_ISO_CTRL			0x0000
+#define REG_SYS_FUNC_EN				0x0002
+#define REG_APS_FSMCO				0x0004
+#define REG_SYS_CLKR				0x0008
+#define REG_9346CR				0x000A
+#define REG_EE_VPD				0x000C
+#define REG_AFE_MISC				0x0010
+#define REG_SPS0_CTRL				0x0011
+#define REG_SPS_OCP_CFG				0x0018
+#define REG_RSV_CTRL				0x001C
+#define REG_RF_CTRL				0x001F
+#define REG_LDOA15_CTRL				0x0020
+#define REG_LDOV12D_CTRL			0x0021
+#define REG_LDOHCI12_CTRL			0x0022
+#define REG_LPLDO_CTRL				0x0023
+#define REG_AFE_XTAL_CTRL			0x0024
+ /* 1.5v for 8188EE test chip, 1.4v for MP chip */
+#define REG_AFE_LDO_CTRL			0x0027
+#define REG_AFE_PLL_CTRL			0x0028
+#define REG_MAC_PHY_CTRL			0x002c
+#define REG_EFUSE_CTRL				0x0030
+#define REG_EFUSE_TEST				0x0034
+#define REG_PWR_DATA				0x0038
+#define REG_CAL_TIMER				0x003C
+#define REG_ACLK_MON				0x003E
+#define REG_GPIO_MUXCFG				0x0040
+#define REG_GPIO_IO_SEL				0x0042
+#define REG_MAC_PINMUX_CFG			0x0043
+#define REG_GPIO_PIN_CTRL			0x0044
+#define REG_GPIO_INTM				0x0048
+#define REG_LEDCFG0				0x004C
+#define REG_LEDCFG1				0x004D
+#define REG_LEDCFG2				0x004E
+#define REG_LEDCFG3				0x004F
+#define REG_FSIMR				0x0050
+#define REG_FSISR				0x0054
+#define REG_HSIMR				0x0058
+#define REG_HSISR				0x005c
+#define REG_GPIO_PIN_CTRL_2			0x0060
+#define REG_GPIO_IO_SEL_2			0x0062
+#define REG_MULTI_FUNC_CTRL			0x0068
+#define REG_GPIO_OUTPUT				0x006c
+#define REG_OPT_CTRL				0x0074
+#define REG_AFE_XTAL_CTRL_EXT			0x0078
+#define REG_XCK_OUT_CTRL			0x007c
+#define REG_MCUFWDL				0x0080
+#define REG_WOL_EVENT				0x0081
+#define REG_MCUTSTCFG				0x0084
+
+#define REG_HIMR				0x00B0
+#define REG_HISR				0x00B4
+#define REG_HIMRE				0x00B8
+#define REG_HISRE				0x00BC
+
+#define REG_PMC_DBG_CTRL2			0x00CC
+
+#define REG_EFUSE_ACCESS			0x00CF
+
+#define REG_BIST_SCAN				0x00D0
+#define REG_BIST_RPT				0x00D4
+#define REG_BIST_ROM_RPT			0x00D8
+#define REG_USB_SIE_INTF			0x00E0
+#define REG_PCIE_MIO_INTF			0x00E4
+#define REG_PCIE_MIO_INTD			0x00E8
+#define REG_HPON_FSM				0x00EC
+#define REG_SYS_CFG				0x00F0
+#define REG_GPIO_OUTSTS				0x00F4
+#define REG_MAC_PHY_CTRL_NORMAL			0x00F8
+#define REG_SYS_CFG1				0x00FC
+#define REG_ROM_VERSION				0x00FD
+
+#define REG_CR					0x0100
+#define REG_PBP					0x0104
+#define REG_PKT_BUFF_ACCESS_CTRL		0x0106
+#define REG_TRXDMA_CTRL				0x010C
+#define REG_TRXFF_BNDY				0x0114
+#define REG_TRXFF_STATUS			0x0118
+#define REG_RXFF_PTR				0x011C
+
+#define REG_CPWM				0x012F
+#define REG_FWIMR				0x0130
+#define REG_FWISR				0x0134
+#define REG_FTISR				0x013C
+#define REG_PKTBUF_DBG_CTRL			0x0140
+#define REG_PKTBUF_DBG_DATA_L			0x0144
+#define REG_PKTBUF_DBG_DATA_H			0x0148
+#define REG_RXPKTBUF_CTRL			(REG_PKTBUF_DBG_CTRL+2)
+
+#define REG_TC0_CTRL				0x0150
+#define REG_TC1_CTRL				0x0154
+#define REG_TC2_CTRL				0x0158
+#define REG_TC3_CTRL				0x015C
+#define REG_TC4_CTRL				0x0160
+#define REG_TCUNIT_BASE				0x0164
+#define REG_MBIST_START				0x0174
+#define REG_MBIST_DONE				0x0178
+#define REG_MBIST_FAIL				0x017C
+#define REG_32K_CTRL				0x0194
+#define REG_C2HEVT_MSG_NORMAL			0x01A0
+#define REG_C2HEVT_CLEAR			0x01AF
+#define REG_C2HEVT_MSG_TEST			0x01B8
+#define REG_MCUTST_1				0x01c0
+#define REG_MCUTST_WOWLAN			0x01C7
+#define REG_FMETHR				0x01C8
+#define REG_HMETFR				0x01CC
+#define REG_HMEBOX_0				0x01D0
+#define REG_HMEBOX_1				0x01D4
+#define REG_HMEBOX_2				0x01D8
+#define REG_HMEBOX_3				0x01DC
+
+#define REG_LLT_INIT				0x01E0
+#define REG_BB_ACCEESS_CTRL			0x01E8
+#define REG_BB_ACCESS_DATA			0x01EC
+
+#define REG_HMEBOX_EXT_0			0x01F0
+#define REG_HMEBOX_EXT_1			0x01F4
+#define REG_HMEBOX_EXT_2			0x01F8
+#define REG_HMEBOX_EXT_3			0x01FC
+
+#define REG_RQPN				0x0200
+#define REG_FIFOPAGE				0x0204
+#define REG_TDECTRL				0x0208
+#define REG_TXDMA_OFFSET_CHK			0x020C
+#define REG_TXDMA_STATUS			0x0210
+#define REG_RQPN_NPQ				0x0214
+
+#define REG_RXDMA_AGG_PG_TH			0x0280
+ /* FW shall update this register before FW write RXPKT_RELEASE_POLL to 1 */
+#define REG_FW_UPD_RDPTR			0x0284
+ /* Control the RX DMA.*/
+#define REG_RXDMA_CONTROL			0x0286
+/* The number of packets in RXPKTBUF.	*/
+#define REG_RXPKT_NUM				0x0287
+
+#define	REG_PCIE_CTRL_REG			0x0300
+#define	REG_INT_MIG				0x0304
+#define	REG_BCNQ_DESA				0x0308
+#define	REG_HQ_DESA				0x0310
+#define	REG_MGQ_DESA				0x0318
+#define	REG_VOQ_DESA				0x0320
+#define	REG_VIQ_DESA				0x0328
+#define	REG_BEQ_DESA				0x0330
+#define	REG_BKQ_DESA				0x0338
+#define	REG_RX_DESA				0x0340
+
+#define	REG_DBI_WDATA				0x0348
+#define	REG_DBI_RDATA				0x034C
+#define	REG_DBI_CTRL				0x0350
+#define	REG_DBI_ADDR				0x0350
+#define	REG_DBI_FLAG				0x0352
+#define	REG_MDIO_WDATA				0x0354
+#define	REG_MDIO_RDATA				0x0356
+#define	REG_MDIO_CTL				0x0358
+#define	REG_DBG_SEL				0x0360
+#define	REG_PCIE_HRPWM				0x0361
+#define	REG_PCIE_HCPWM				0x0363
+#define	REG_UART_CTRL				0x0364
+#define	REG_WATCH_DOG				0x0368
+#define	REG_UART_TX_DESA			0x0370
+#define	REG_UART_RX_DESA			0x0378
+
+#define	REG_HDAQ_DESA_NODEF			0x0000
+#define	REG_CMDQ_DESA_NODEF			0x0000
+
+#define REG_VOQ_INFORMATION			0x0400
+#define REG_VIQ_INFORMATION			0x0404
+#define REG_BEQ_INFORMATION			0x0408
+#define REG_BKQ_INFORMATION			0x040C
+#define REG_MGQ_INFORMATION			0x0410
+#define REG_HGQ_INFORMATION			0x0414
+#define REG_BCNQ_INFORMATION			0x0418
+#define REG_TXPKT_EMPTY				0x041A
+
+#define REG_CPU_MGQ_INFORMATION			0x041C
+#define REG_FWHW_TXQ_CTRL			0x0420
+#define REG_HWSEQ_CTRL				0x0423
+#define REG_TXPKTBUF_BCNQ_BDNY			0x0424
+#define REG_TXPKTBUF_MGQ_BDNY			0x0425
+#define REG_MULTI_BCNQ_EN			0x0426
+#define REG_MULTI_BCNQ_OFFSET			0x0427
+#define REG_SPEC_SIFS				0x0428
+#define REG_RL					0x042A
+#define REG_DARFRC				0x0430
+#define REG_RARFRC				0x0438
+#define REG_RRSR				0x0440
+#define REG_ARFR0				0x0444
+#define REG_ARFR1				0x044C
+#define REG_CCK_CHECK				0x0454
+#define REG_AMPDU_MAX_TIME			0x0456
+#define REG_AGGLEN_LMT				0x0458
+#define REG_AMPDU_MIN_SPACE			0x045C
+#define REG_TXPKTBUF_WMAC_LBK_BF_HD		0x045D
+#define REG_FAST_EDCA_CTRL			0x0460
+#define REG_RD_RESP_PKT_TH			0x0463
+#define REG_INIRTS_RATE_SEL			0x0480
+#define REG_INIDATA_RATE_SEL			0x0484
+#define REG_ARFR2				0x048C
+#define REG_ARFR3				0x0494
+#define REG_POWER_STATUS			0x04A4
+#define REG_POWER_STAGE1			0x04B4
+#define REG_POWER_STAGE2			0x04B8
+#define REG_PKT_LIFE_TIME			0x04C0
+#define REG_STBC_SETTING			0x04C4
+#define REG_HT_SINGLE_AMPDU			0x04C7
+#define REG_PROT_MODE_CTRL			0x04C8
+#define REG_MAX_AGGR_NUM			0x04CA
+#define REG_BAR_MODE_CTRL			0x04CC
+#define REG_RA_TRY_RATE_AGG_LMT			0x04CF
+#define REG_EARLY_MODE_CONTROL			0x04D0
+#define REG_NQOS_SEQ				0x04DC
+#define REG_QOS_SEQ				0x04DE
+#define REG_NEED_CPU_HANDLE			0x04E0
+#define REG_PKT_LOSE_RPT			0x04E1
+#define REG_PTCL_ERR_STATUS			0x04E2
+#define REG_TX_RPT_CTRL				0x04EC
+#define REG_TX_RPT_TIME				0x04F0
+#define REG_DUMMY				0x04FC
+
+#define REG_EDCA_VO_PARAM			0x0500
+#define REG_EDCA_VI_PARAM			0x0504
+#define REG_EDCA_BE_PARAM			0x0508
+#define REG_EDCA_BK_PARAM			0x050C
+#define REG_BCNTCFG				0x0510
+#define REG_PIFS				0x0512
+#define REG_RDG_PIFS				0x0513
+#define REG_SIFS_CTX				0x0514
+#define REG_SIFS_TRX				0x0516
+#define REG_AGGR_BREAK_TIME			0x051A
+#define REG_SLOT				0x051B
+#define REG_TX_PTCL_CTRL			0x0520
+#define REG_TXPAUSE				0x0522
+#define REG_DIS_TXREQ_CLR			0x0523
+#define REG_RD_CTRL				0x0524
+#define REG_TBTT_PROHIBIT			0x0540
+#define REG_RD_NAV_NXT				0x0544
+#define REG_NAV_PROT_LEN			0x0546
+#define REG_BCN_CTRL				0x0550
+#define REG_USTIME_TSF				0x0551
+#define REG_MBID_NUM				0x0552
+#define REG_DUAL_TSF_RST			0x0553
+#define REG_BCN_INTERVAL			0x0554
+#define REG_MBSSID_BCN_SPACE			0x0554
+#define REG_DRVERLYINT				0x0558
+#define REG_BCNDMATIM				0x0559
+#define REG_ATIMWND				0x055A
+#define REG_BCN_MAX_ERR				0x055D
+#define REG_RXTSF_OFFSET_CCK			0x055E
+#define REG_RXTSF_OFFSET_OFDM			0x055F
+#define REG_TSFTR				0x0560
+#define REG_INIT_TSFTR				0x0564
+#define REG_SECONDARY_CCA_CTRL			0x0577
+#define REG_PSTIMER				0x0580
+#define REG_TIMER0				0x0584
+#define REG_TIMER1				0x0588
+#define REG_ACMHWCTRL				0x05C0
+#define REG_ACMRSTCTRL				0x05C1
+#define REG_ACMAVG				0x05C2
+#define REG_VO_ADMTIME				0x05C4
+#define REG_VI_ADMTIME				0x05C6
+#define REG_BE_ADMTIME				0x05C8
+#define REG_EDCA_RANDOM_GEN			0x05CC
+#define REG_NOA_DESC_SEL			0x05CF
+#define REG_NOA_DESC_DURATION			0x05E0
+#define REG_NOA_DESC_INTERVAL			0x05E4
+#define REG_NOA_DESC_START			0x05E8
+#define REG_NOA_DESC_COUNT			0x05EC
+#define REG_SCH_TXCMD				0x05F8
+
+#define REG_APSD_CTRL				0x0600
+#define REG_BWOPMODE				0x0603
+#define REG_TCR					0x0604
+#define REG_RCR					0x0608
+#define REG_RX_PKT_LIMIT			0x060C
+#define REG_RX_DLK_TIME				0x060D
+#define REG_RX_DRVINFO_SZ			0x060F
+
+#define REG_MACID				0x0610
+#define REG_BSSID				0x0618
+#define REG_MAR					0x0620
+#define REG_MBIDCAMCFG				0x0628
+
+#define REG_USTIME_EDCA				0x0638
+#define REG_MAC_SPEC_SIFS			0x063A
+#define REG_RESP_SIFS_CCK			0x063C
+#define REG_RESP_SIFS_OFDM			0x063E
+#define REG_ACKTO				0x0640
+#define REG_CTS2TO				0x0641
+#define REG_EIFS				0x0642
+
+#define REG_NAV_CTRL				0x0650
+#define REG_NAV_UPPER				0x0652
+#define REG_BACAMCMD				0x0654
+#define REG_BACAMCONTENT			0x0658
+#define REG_LBDLY				0x0660
+#define REG_FWDLY				0x0661
+#define REG_RXERR_RPT				0x0664
+#define REG_TRXPTCL_CTL				0x0668
+
+#define REG_CAMCMD				0x0670
+#define REG_CAMWRITE				0x0674
+#define REG_CAMREAD				0x0678
+#define REG_CAMDBG				0x067C
+#define REG_SECCFG				0x0680
+
+#define REG_WOW_CTRL				0x0690
+#define REG_PSSTATUS				0x0691
+#define REG_PS_RX_INFO				0x0692
+#define REG_UAPSD_TID				0x0693
+#define REG_LPNAV_CTRL				0x0694
+#define REG_WKFMCAM_NUM				0x0698
+#define REG_WKFMCAM_RWD				0x069C
+#define REG_RXFLTMAP0				0x06A0
+#define REG_RXFLTMAP1				0x06A2
+#define REG_RXFLTMAP2				0x06A4
+#define REG_BCN_PSR_RPT				0x06A8
+#define REG_CALB32K_CTRL			0x06AC
+#define REG_PKT_MON_CTRL			0x06B4
+#define REG_BT_COEX_TABLE			0x06C0
+#define REG_WMAC_RESP_TXINFO			0x06D8
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_TEST_USB_TXQS			0xFE48
+#define REG_TEST_SIE_VID			0xFE60
+#define REG_TEST_SIE_PID			0xFE62
+#define REG_TEST_SIE_OPTIONAL			0xFE64
+#define REG_TEST_SIE_CHIRP_K			0xFE65
+#define REG_TEST_SIE_PHY			0xFE66
+#define REG_TEST_SIE_MAC_ADDR			0xFE70
+#define REG_TEST_SIE_STRING			0xFE80
+
+#define REG_NORMAL_SIE_VID			0xFE60
+#define REG_NORMAL_SIE_PID			0xFE62
+#define REG_NORMAL_SIE_OPTIONAL			0xFE64
+#define REG_NORMAL_SIE_EP			0xFE65
+#define REG_NORMAL_SIE_PHY			0xFE68
+#define REG_NORMAL_SIE_MAC_ADDR			0xFE70
+#define REG_NORMAL_SIE_STRING			0xFE80
+
+#define	CR9346					REG_9346CR
+#define	MSR					(REG_CR + 2)
+#define	ISR					REG_HISR
+#define	TSFR					REG_TSFTR
+
+#define	MACIDR0					REG_MACID
+#define	MACIDR4					(REG_MACID + 4)
+
+#define PBP					REG_PBP
+
+#define	IDR0					MACIDR0
+#define	IDR4					MACIDR4
+
+#define	UNUSED_REGISTER				0x1BF
+#define	DCAM					UNUSED_REGISTER
+#define	PSR					UNUSED_REGISTER
+#define BBADDR					UNUSED_REGISTER
+#define	PHYDATAR				UNUSED_REGISTER
+
+#define	INVALID_BBRF_VALUE			0x12345678
+
+#define	MAX_MSS_DENSITY_2T			0x13
+#define	MAX_MSS_DENSITY_1T			0x0A
+
+#define	CMDEEPROM_EN				BIT(5)
+#define	CMDEEPROM_SEL				BIT(4)
+#define	CMD9346CR_9356SEL			BIT(4)
+#define	AUTOLOAD_EEPROM			(CMDEEPROM_EN|CMDEEPROM_SEL)
+#define	AUTOLOAD_EFUSE			CMDEEPROM_EN
+
+#define	GPIOSEL_GPIO				0
+#define	GPIOSEL_ENBT				BIT(5)
+
+#define	GPIO_IN				REG_GPIO_PIN_CTRL
+#define	GPIO_OUT			(REG_GPIO_PIN_CTRL+1)
+#define	GPIO_IO_SEL			(REG_GPIO_PIN_CTRL+2)
+#define	GPIO_MOD			(REG_GPIO_PIN_CTRL+3)
+
+/*    8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
+#define	HSIMR_GPIO12_0_INT_EN			BIT(0)
+#define	HSIMR_SPS_OCP_INT_EN			BIT(5)
+#define	HSIMR_RON_INT_EN			BIT(6)
+#define	HSIMR_PDN_INT_EN			BIT(7)
+#define	HSIMR_GPIO9_INT_EN			BIT(25)
+
+/* 8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte) */
+#define	HSISR_GPIO12_0_INT			BIT(0)
+#define	HSISR_SPS_OCP_INT			BIT(5)
+#define	HSISR_RON_INT_EN			BIT(6)
+#define	HSISR_PDNINT				BIT(7)
+#define	HSISR_GPIO9_INT				BIT(25)
+
+#define	MSR_NOLINK				0x00
+#define	MSR_ADHOC				0x01
+#define	MSR_INFRA				0x02
+#define	MSR_AP					0x03
+
+#define	RRSR_RSC_OFFSET				21
+#define	RRSR_SHORT_OFFSET			23
+#define	RRSR_RSC_BW_40M				0x600000
+#define	RRSR_RSC_UPSUBCHNL			0x400000
+#define	RRSR_RSC_LOWSUBCHNL			0x200000
+#define	RRSR_SHORT				0x800000
+#define	RRSR_1M					BIT(0)
+#define	RRSR_2M					BIT(1)
+#define	RRSR_5_5M				BIT(2)
+#define	RRSR_11M				BIT(3)
+#define	RRSR_6M					BIT(4)
+#define	RRSR_9M					BIT(5)
+#define	RRSR_12M				BIT(6)
+#define	RRSR_18M				BIT(7)
+#define	RRSR_24M				BIT(8)
+#define	RRSR_36M				BIT(9)
+#define	RRSR_48M				BIT(10)
+#define	RRSR_54M				BIT(11)
+#define	RRSR_MCS0				BIT(12)
+#define	RRSR_MCS1				BIT(13)
+#define	RRSR_MCS2				BIT(14)
+#define	RRSR_MCS3				BIT(15)
+#define	RRSR_MCS4				BIT(16)
+#define	RRSR_MCS5				BIT(17)
+#define	RRSR_MCS6				BIT(18)
+#define	RRSR_MCS7				BIT(19)
+#define	BRSR_ACKSHORTPMB			BIT(23)
+
+#define	RATR_1M					0x00000001
+#define	RATR_2M					0x00000002
+#define	RATR_55M				0x00000004
+#define	RATR_11M				0x00000008
+#define	RATR_6M					0x00000010
+#define	RATR_9M					0x00000020
+#define	RATR_12M				0x00000040
+#define	RATR_18M				0x00000080
+#define	RATR_24M				0x00000100
+#define	RATR_36M				0x00000200
+#define	RATR_48M				0x00000400
+#define	RATR_54M				0x00000800
+#define	RATR_MCS0				0x00001000
+#define	RATR_MCS1				0x00002000
+#define	RATR_MCS2				0x00004000
+#define	RATR_MCS3				0x00008000
+#define	RATR_MCS4				0x00010000
+#define	RATR_MCS5				0x00020000
+#define	RATR_MCS6				0x00040000
+#define	RATR_MCS7				0x00080000
+#define	RATR_MCS8				0x00100000
+#define	RATR_MCS9				0x00200000
+#define	RATR_MCS10				0x00400000
+#define	RATR_MCS11				0x00800000
+#define	RATR_MCS12				0x01000000
+#define	RATR_MCS13				0x02000000
+#define	RATR_MCS14				0x04000000
+#define	RATR_MCS15				0x08000000
+
+#define RATE_1M					BIT(0)
+#define RATE_2M					BIT(1)
+#define RATE_5_5M				BIT(2)
+#define RATE_11M				BIT(3)
+#define RATE_6M					BIT(4)
+#define RATE_9M					BIT(5)
+#define RATE_12M				BIT(6)
+#define RATE_18M				BIT(7)
+#define RATE_24M				BIT(8)
+#define RATE_36M				BIT(9)
+#define RATE_48M				BIT(10)
+#define RATE_54M				BIT(11)
+#define RATE_MCS0				BIT(12)
+#define RATE_MCS1				BIT(13)
+#define RATE_MCS2				BIT(14)
+#define RATE_MCS3				BIT(15)
+#define RATE_MCS4				BIT(16)
+#define RATE_MCS5				BIT(17)
+#define RATE_MCS6				BIT(18)
+#define RATE_MCS7				BIT(19)
+#define RATE_MCS8				BIT(20)
+#define RATE_MCS9				BIT(21)
+#define RATE_MCS10				BIT(22)
+#define RATE_MCS11				BIT(23)
+#define RATE_MCS12				BIT(24)
+#define RATE_MCS13				BIT(25)
+#define RATE_MCS14				BIT(26)
+#define RATE_MCS15				BIT(27)
+
+#define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
+#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
+				RATR_24M | RATR_36M | RATR_48M | RATR_54M)
+#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |\
+				RATR_MCS3 | RATR_MCS4 | RATR_MCS5 |\
+				RATR_MCS6 | RATR_MCS7)
+#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 |\
+				RATR_MCS11 | RATR_MCS12 | RATR_MCS13 |\
+				RATR_MCS14 | RATR_MCS15)
+
+#define	BW_OPMODE_20MHZ				BIT(2)
+#define	BW_OPMODE_5G				BIT(1)
+#define	BW_OPMODE_11J				BIT(0)
+
+#define	CAM_VALID				BIT(15)
+#define	CAM_NOTVALID				0x0000
+#define	CAM_USEDK				BIT(5)
+
+#define	CAM_NONE				0x0
+#define	CAM_WEP40				0x01
+#define	CAM_TKIP				0x02
+#define	CAM_AES					0x04
+#define	CAM_WEP104				0x05
+
+#define	TOTAL_CAM_ENTRY				32
+#define	HALF_CAM_ENTRY				16
+
+#define	CAM_WRITE				BIT(16)
+#define	CAM_READ				0x00000000
+#define	CAM_POLLINIG				BIT(31)
+
+#define	SCR_USEDK				0x01
+#define	SCR_TXSEC_ENABLE			0x02
+#define	SCR_RXSEC_ENABLE			0x04
+
+#define	WOW_PMEN				BIT(0)
+#define	WOW_WOMEN				BIT(1)
+#define	WOW_MAGIC				BIT(2)
+#define	WOW_UWF					BIT(3)
+
+/*********************************************
+*       8188 IMR/ISR bits
+**********************************************/
+#define	IMR_DISABLED				0x0
+/* IMR DW0(0x0060-0063) Bit 0-31 */
+/* TXRPT interrupt when CCX bit of the packet is set	*/
+#define	IMR_TXCCK				BIT(30)
+/* Power Save Time Out Interrupt */
+#define	IMR_PSTIMEOUT				BIT(29)
+/* When GTIMER4 expires, this bit is set to 1	*/
+#define	IMR_GTINT4				BIT(28)
+/* When GTIMER3 expires, this bit is set to 1	*/
+#define	IMR_GTINT3				BIT(27)
+/* Transmit Beacon0 Error			*/
+#define	IMR_TBDER				BIT(26)
+/* Transmit Beacon0 OK			*/
+#define	IMR_TBDOK				BIT(25)
+/* TSF Timer BIT32 toggle indication interrupt		*/
+#define	IMR_TSF_BIT32_TOGGLE			BIT(24)
+/* Beacon DMA Interrupt 0			*/
+#define	IMR_BCNDMAINT0				BIT(20)
+/* Beacon Queue DMA OK0			*/
+#define	IMR_BCNDOK0				BIT(16)
+/* HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1) */
+#define	IMR_HSISR_IND_ON_INT			BIT(15)
+/* Beacon DMA Interrupt Extension for Win7			*/
+#define	IMR_BCNDMAINT_E				BIT(14)
+/* CTWidnow End or ATIM Window End */
+#define	IMR_ATIMEND				BIT(12)
+/* HISR1 Indicator (HISR1 & HIMR1 is true, this bit is set to 1)*/
+#define	IMR_HISR1_IND_INT			BIT(11)
+/* CPU to Host Command INT Status, Write 1 clear	*/
+#define	IMR_C2HCMD				BIT(10)
+/* CPU power Mode exchange INT Status, Write 1 clear	*/
+#define	IMR_CPWM2				BIT(9)
+/* CPU power Mode exchange INT Status, Write 1 clear	*/
+#define	IMR_CPWM				BIT(8)
+/* High Queue DMA OK	*/
+#define	IMR_HIGHDOK				BIT(7)
+/* Management Queue DMA OK	*/
+#define	IMR_MGNTDOK				BIT(6)
+/* AC_BK DMA OK		*/
+#define	IMR_BKDOK				BIT(5)
+/* AC_BE DMA OK	*/
+#define	IMR_BEDOK				BIT(4)
+/* AC_VI DMA OK	*/
+#define	IMR_VIDOK				BIT(3)
+/* AC_VO DMA OK	*/
+#define	IMR_VODOK				BIT(2)
+/* Rx Descriptor Unavailable	*/
+#define	IMR_RDU					BIT(1)
+#define	IMR_ROK					BIT(0)	/* Receive DMA OK */
+
+/* IMR DW1(0x00B4-00B7) Bit 0-31 */
+/* Beacon DMA Interrupt 7	*/
+#define	IMR_BCNDMAINT7				BIT(27)
+/* Beacon DMA Interrupt 6		*/
+#define	IMR_BCNDMAINT6				BIT(26)
+/* Beacon DMA Interrupt 5		*/
+#define	IMR_BCNDMAINT5				BIT(25)
+/* Beacon DMA Interrupt 4		*/
+#define	IMR_BCNDMAINT4				BIT(24)
+/* Beacon DMA Interrupt 3		*/
+#define	IMR_BCNDMAINT3				BIT(23)
+/* Beacon DMA Interrupt 2		*/
+#define	IMR_BCNDMAINT2				BIT(22)
+/* Beacon DMA Interrupt 1		*/
+#define	IMR_BCNDMAINT1				BIT(21)
+/* Beacon Queue DMA OK Interrup 7 */
+#define	IMR_BCNDOK7				BIT(20)
+/* Beacon Queue DMA OK Interrup 6 */
+#define	IMR_BCNDOK6				BIT(19)
+/* Beacon Queue DMA OK Interrup 5 */
+#define	IMR_BCNDOK5				BIT(18)
+/* Beacon Queue DMA OK Interrup 4 */
+#define	IMR_BCNDOK4				BIT(17)
+/* Beacon Queue DMA OK Interrup 3 */
+#define	IMR_BCNDOK3				BIT(16)
+/* Beacon Queue DMA OK Interrup 2 */
+#define	IMR_BCNDOK2				BIT(15)
+/* Beacon Queue DMA OK Interrup 1 */
+#define	IMR_BCNDOK1				BIT(14)
+/* ATIM Window End Extension for Win7 */
+#define	IMR_ATIMEND_E				BIT(13)
+/* Tx Error Flag Interrupt Status, write 1 clear. */
+#define	IMR_TXERR				BIT(11)
+/* Rx Error Flag INT Status, Write 1 clear */
+#define	IMR_RXERR				BIT(10)
+/* Transmit FIFO Overflow */
+#define	IMR_TXFOVW				BIT(9)
+/* Receive FIFO Overflow */
+#define	IMR_RXFOVW				BIT(8)
+
+#define	HWSET_MAX_SIZE				512
+#define   EFUSE_MAX_SECTION			64
+#define   EFUSE_REAL_CONTENT_LEN		256
+/* PG data exclude header, dummy 7 bytes frome CP test and reserved 1byte.*/
+#define		EFUSE_OOB_PROTECT_BYTES		18
+
+#define	EEPROM_DEFAULT_TSSI			0x0
+#define EEPROM_DEFAULT_TXPOWERDIFF		0x0
+#define EEPROM_DEFAULT_CRYSTALCAP		0x5
+#define EEPROM_DEFAULT_BOARDTYPE		0x02
+#define EEPROM_DEFAULT_TXPOWER			0x1010
+#define	EEPROM_DEFAULT_HT2T_TXPWR		0x10
+
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define	EEPROM_DEFAULT_THERMALMETER		0x18
+#define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
+#define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
+#define	EEPROM_DEFAULT_TXPOWERLEVEL		0x22
+#define	EEPROM_DEFAULT_HT40_2SDIFF		0x0
+#define EEPROM_DEFAULT_HT20_DIFF		2
+#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
+#define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
+#define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
+
+#define RF_OPTION1				0x79
+#define RF_OPTION2				0x7A
+#define RF_OPTION3				0x7B
+#define RF_OPTION4				0xC3
+
+#define EEPROM_DEFAULT_PID			0x1234
+#define EEPROM_DEFAULT_VID			0x5678
+#define EEPROM_DEFAULT_CUSTOMERID		0xAB
+#define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
+#define EEPROM_DEFAULT_VERSION			0
+
+#define	EEPROM_CHANNEL_PLAN_FCC			0x0
+#define	EEPROM_CHANNEL_PLAN_IC			0x1
+#define	EEPROM_CHANNEL_PLAN_ETSI		0x2
+#define	EEPROM_CHANNEL_PLAN_SPAIN		0x3
+#define	EEPROM_CHANNEL_PLAN_FRANCE		0x4
+#define	EEPROM_CHANNEL_PLAN_MKK			0x5
+#define	EEPROM_CHANNEL_PLAN_MKK1		0x6
+#define	EEPROM_CHANNEL_PLAN_ISRAEL		0x7
+#define	EEPROM_CHANNEL_PLAN_TELEC		0x8
+#define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
+#define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
+#define	EEPROM_CHANNEL_PLAN_NCC			0xB
+#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
+
+#define EEPROM_CID_DEFAULT			0x0
+#define EEPROM_CID_TOSHIBA			0x4
+#define	EEPROM_CID_CCX				0x10
+#define	EEPROM_CID_QMI				0x0D
+#define EEPROM_CID_WHQL				0xFE
+
+#define	RTL_EEPROM_ID				0x8129
+
+#define EEPROM_HPON				0x02
+#define EEPROM_CLK				0x06
+#define EEPROM_TESTR				0x08
+
+#define EEPROM_TXPOWERCCK			0x10
+#define	EEPROM_TXPOWERHT40_1S			0x16
+#define EEPROM_TXPOWERHT20DIFF			0x1B
+#define EEPROM_TXPOWER_OFDMDIFF			0x1B
+
+#define	EEPROM_TX_PWR_INX			0x10
+
+#define	EEPROM_CHANNELPLAN			0xB8
+#define	EEPROM_XTAL_8821AE			0xB9
+#define	EEPROM_THERMAL_METER			0xBA
+#define	EEPROM_IQK_LCK_88E			0xBB
+
+#define	EEPROM_RF_BOARD_OPTION			0xC1
+#define	EEPROM_RF_FEATURE_OPTION_88E		0xC2
+#define	EEPROM_RF_BT_SETTING			0xC3
+#define	EEPROM_VERSION				0xC4
+#define	EEPROM_CUSTOMER_ID			0xC5
+#define	EEPROM_RF_ANTENNA_OPT_88E		0xC9
+#define	EEPROM_RFE_OPTION			0xCA
+
+#define	EEPROM_MAC_ADDR				0xD0
+#define EEPROM_VID				0xD6
+#define EEPROM_DID				0xD8
+#define EEPROM_SVID				0xDA
+#define EEPROM_SMID				0xDC
+
+#define	STOPBECON				BIT(6)
+#define	STOPHIGHT				BIT(5)
+#define	STOPMGT					BIT(4)
+#define	STOPVO					BIT(3)
+#define	STOPVI					BIT(2)
+#define	STOPBE					BIT(1)
+#define	STOPBK					BIT(0)
+
+#define	RCR_APPFCS				BIT(31)
+#define	RCR_APP_MIC				BIT(30)
+#define	RCR_APP_ICV				BIT(29)
+#define	RCR_APP_PHYST_RXFF			BIT(28)
+#define	RCR_APP_BA_SSN				BIT(27)
+#define	RCR_NONQOS_VHT				BIT(26)
+#define	RCR_ENMBID				BIT(24)
+#define	RCR_LSIGEN				BIT(23)
+#define	RCR_MFBEN				BIT(22)
+#define	RCR_HTC_LOC_CTRL			BIT(14)
+#define	RCR_AMF					BIT(13)
+#define	RCR_ACF					BIT(12)
+#define	RCR_ADF					BIT(11)
+#define	RCR_AICV				BIT(9)
+#define	RCR_ACRC32				BIT(8)
+#define	RCR_CBSSID_BCN				BIT(7)
+#define	RCR_CBSSID_DATA				BIT(6)
+#define	RCR_CBSSID				RCR_CBSSID_DATA
+#define	RCR_APWRMGT				BIT(5)
+#define	RCR_ADD3				BIT(4)
+#define	RCR_AB					BIT(3)
+#define	RCR_AM					BIT(2)
+#define	RCR_APM					BIT(1)
+#define	RCR_AAP					BIT(0)
+#define	RCR_MXDMA_OFFSET			8
+#define	RCR_FIFO_OFFSET				13
+
+#define RSV_CTRL				0x001C
+#define RD_CTRL					0x0524
+
+#define REG_USB_INFO				0xFE17
+#define REG_USB_SPECIAL_OPTION			0xFE55
+#define REG_USB_DMA_AGG_TO			0xFE5B
+#define REG_USB_AGG_TO				0xFE5C
+#define REG_USB_AGG_TH				0xFE5D
+
+#define REG_USB_VID				0xFE60
+#define REG_USB_PID				0xFE62
+#define REG_USB_OPTIONAL			0xFE64
+#define REG_USB_CHIRP_K				0xFE65
+#define REG_USB_PHY				0xFE66
+#define REG_USB_MAC_ADDR			0xFE70
+#define REG_USB_HRPWM				0xFE58
+#define REG_USB_HCPWM				0xFE57
+
+#define SW18_FPWM				BIT(3)
+
+#define ISO_MD2PP				BIT(0)
+#define ISO_UA2USB				BIT(1)
+#define ISO_UD2CORE				BIT(2)
+#define ISO_PA2PCIE				BIT(3)
+#define ISO_PD2CORE				BIT(4)
+#define ISO_IP2MAC				BIT(5)
+#define ISO_DIOP				BIT(6)
+#define ISO_DIOE				BIT(7)
+#define ISO_EB2CORE				BIT(8)
+#define ISO_DIOR				BIT(9)
+
+#define PWC_EV25V				BIT(14)
+#define PWC_EV12V				BIT(15)
+
+#define FEN_BBRSTB				BIT(0)
+#define FEN_BB_GLB_RSTN				BIT(1)
+#define FEN_USBA				BIT(2)
+#define FEN_UPLL				BIT(3)
+#define FEN_USBD				BIT(4)
+#define FEN_DIO_PCIE				BIT(5)
+#define FEN_PCIEA				BIT(6)
+#define FEN_PPLL				BIT(7)
+#define FEN_PCIED				BIT(8)
+#define FEN_DIOE				BIT(9)
+#define FEN_CPUEN				BIT(10)
+#define FEN_DCORE				BIT(11)
+#define FEN_ELDR				BIT(12)
+#define FEN_DIO_RF				BIT(13)
+#define FEN_HWPDN				BIT(14)
+#define FEN_MREGEN				BIT(15)
+
+#define PFM_LDALL				BIT(0)
+#define PFM_ALDN				BIT(1)
+#define PFM_LDKP				BIT(2)
+#define PFM_WOWL				BIT(3)
+#define ENPDN					BIT(4)
+#define PDN_PL					BIT(5)
+#define APFM_ONMAC				BIT(8)
+#define APFM_OFF				BIT(9)
+#define APFM_RSM				BIT(10)
+#define AFSM_HSUS				BIT(11)
+#define AFSM_PCIE				BIT(12)
+#define APDM_MAC				BIT(13)
+#define APDM_HOST				BIT(14)
+#define APDM_HPDN				BIT(15)
+#define RDY_MACON				BIT(16)
+#define SUS_HOST				BIT(17)
+#define ROP_ALD					BIT(20)
+#define ROP_PWR					BIT(21)
+#define ROP_SPS					BIT(22)
+#define SOP_MRST				BIT(25)
+#define SOP_FUSE				BIT(26)
+#define SOP_ABG					BIT(27)
+#define SOP_AMB					BIT(28)
+#define SOP_RCK					BIT(29)
+#define SOP_A8M					BIT(30)
+#define XOP_BTCK				BIT(31)
+
+#define ANAD16V_EN				BIT(0)
+#define ANA8M					BIT(1)
+#define MACSLP					BIT(4)
+#define LOADER_CLK_EN				BIT(5)
+#define _80M_SSC_DIS				BIT(7)
+#define _80M_SSC_EN_HO				BIT(8)
+#define PHY_SSC_RSTB				BIT(9)
+#define SEC_CLK_EN				BIT(10)
+#define MAC_CLK_EN				BIT(11)
+#define SYS_CLK_EN				BIT(12)
+#define RING_CLK_EN				BIT(13)
+
+#define	BOOT_FROM_EEPROM			BIT(4)
+#define	EEPROM_EN				BIT(5)
+
+#define AFE_BGEN				BIT(0)
+#define AFE_MBEN				BIT(1)
+#define MAC_ID_EN				BIT(7)
+
+#define WLOCK_ALL				BIT(0)
+#define WLOCK_00				BIT(1)
+#define WLOCK_04				BIT(2)
+#define WLOCK_08				BIT(3)
+#define WLOCK_40				BIT(4)
+#define R_DIS_PRST_0				BIT(5)
+#define R_DIS_PRST_1				BIT(6)
+#define LOCK_ALL_EN				BIT(7)
+
+#define RF_EN					BIT(0)
+#define RF_RSTB					BIT(1)
+#define RF_SDMRSTB				BIT(2)
+
+#define LDA15_EN				BIT(0)
+#define LDA15_STBY				BIT(1)
+#define LDA15_OBUF				BIT(2)
+#define LDA15_REG_VOS				BIT(3)
+#define _LDA15_VOADJ(x)				(((x) & 0x7) << 4)
+
+#define LDV12_EN				BIT(0)
+#define LDV12_SDBY				BIT(1)
+#define LPLDO_HSM				BIT(2)
+#define LPLDO_LSM_DIS				BIT(3)
+#define _LDV12_VADJ(x)				(((x) & 0xF) << 4)
+
+#define XTAL_EN					BIT(0)
+#define XTAL_BSEL				BIT(1)
+#define _XTAL_BOSC(x)				(((x) & 0x3) << 2)
+#define _XTAL_CADJ(x)				(((x) & 0xF) << 4)
+#define XTAL_GATE_USB				BIT(8)
+#define _XTAL_USB_DRV(x)			(((x) & 0x3) << 9)
+#define XTAL_GATE_AFE				BIT(11)
+#define _XTAL_AFE_DRV(x)			(((x) & 0x3) << 12)
+#define XTAL_RF_GATE				BIT(14)
+#define _XTAL_RF_DRV(x)				(((x) & 0x3) << 15)
+#define XTAL_GATE_DIG				BIT(17)
+#define _XTAL_DIG_DRV(x)			(((x) & 0x3) << 18)
+#define XTAL_BT_GATE				BIT(20)
+#define _XTAL_BT_DRV(x)				(((x) & 0x3) << 21)
+#define _XTAL_GPIO(x)				(((x) & 0x7) << 23)
+
+#define CKDLY_AFE				BIT(26)
+#define CKDLY_USB				BIT(27)
+#define CKDLY_DIG				BIT(28)
+#define CKDLY_BT				BIT(29)
+
+#define APLL_EN					BIT(0)
+#define APLL_320_EN				BIT(1)
+#define APLL_FREF_SEL				BIT(2)
+#define APLL_EDGE_SEL				BIT(3)
+#define APLL_WDOGB				BIT(4)
+#define APLL_LPFEN				BIT(5)
+
+#define APLL_REF_CLK_13MHZ			0x1
+#define APLL_REF_CLK_19_2MHZ			0x2
+#define APLL_REF_CLK_20MHZ			0x3
+#define APLL_REF_CLK_25MHZ			0x4
+#define APLL_REF_CLK_26MHZ			0x5
+#define APLL_REF_CLK_38_4MHZ			0x6
+#define APLL_REF_CLK_40MHZ			0x7
+
+#define APLL_320EN				BIT(14)
+#define APLL_80EN				BIT(15)
+#define APLL_1MEN				BIT(24)
+
+#define ALD_EN					BIT(18)
+#define EF_PD					BIT(19)
+#define EF_FLAG					BIT(31)
+
+#define EF_TRPT					BIT(7)
+#define LDOE25_EN				BIT(31)
+
+#define RSM_EN					BIT(0)
+#define TIMER_EN				BIT(4)
+
+#define TRSW0EN					BIT(2)
+#define TRSW1EN					BIT(3)
+#define EROM_EN					BIT(4)
+#define ENBT					BIT(5)
+#define ENUART					BIT(8)
+#define UART_910				BIT(9)
+#define ENPMAC					BIT(10)
+#define SIC_SWRST				BIT(11)
+#define ENSIC					BIT(12)
+#define SIC_23					BIT(13)
+#define ENHDP					BIT(14)
+#define SIC_LBK					BIT(15)
+
+#define LED0PL					BIT(4)
+#define LED1PL					BIT(12)
+#define LED0DIS					BIT(7)
+
+#define MCUFWDL_EN				BIT(0)
+#define MCUFWDL_RDY				BIT(1)
+#define FWDL_CHKSUM_RPT				BIT(2)
+#define MACINI_RDY				BIT(3)
+#define BBINI_RDY				BIT(4)
+#define RFINI_RDY				BIT(5)
+#define WINTINI_RDY				BIT(6)
+#define CPRST					BIT(23)
+
+#define XCLK_VLD				BIT(0)
+#define ACLK_VLD				BIT(1)
+#define UCLK_VLD				BIT(2)
+#define PCLK_VLD				BIT(3)
+#define PCIRSTB					BIT(4)
+#define V15_VLD					BIT(5)
+#define TRP_B15V_EN				BIT(7)
+#define SIC_IDLE				BIT(8)
+#define BD_MAC2					BIT(9)
+#define BD_MAC1					BIT(10)
+#define IC_MACPHY_MODE				BIT(11)
+#define VENDOR_ID				BIT(19)
+#define PAD_HWPD_IDN				BIT(22)
+#define TRP_VAUX_EN				BIT(23)
+#define TRP_BT_EN				BIT(24)
+#define BD_PKG_SEL				BIT(25)
+#define BD_HCI_SEL				BIT(26)
+#define TYPE_ID					BIT(27)
+
+#define CHIP_VER_RTL_MASK			0xF000
+#define CHIP_VER_RTL_SHIFT			12
+
+#define REG_LBMODE				(REG_CR + 3)
+
+#define HCI_TXDMA_EN				BIT(0)
+#define HCI_RXDMA_EN				BIT(1)
+#define TXDMA_EN				BIT(2)
+#define RXDMA_EN				BIT(3)
+#define PROTOCOL_EN				BIT(4)
+#define SCHEDULE_EN				BIT(5)
+#define MACTXEN					BIT(6)
+#define MACRXEN					BIT(7)
+#define ENSWBCN					BIT(8)
+#define ENSEC					BIT(9)
+
+#define _NETTYPE(x)				(((x) & 0x3) << 16)
+#define MASK_NETTYPE				0x30000
+#define NT_NO_LINK				0x0
+#define NT_LINK_AD_HOC				0x1
+#define NT_LINK_AP				0x2
+#define NT_AS_AP				0x3
+
+#define _LBMODE(x)				(((x) & 0xF) << 24)
+#define MASK_LBMODE				0xF000000
+#define LOOPBACK_NORMAL				0x0
+#define LOOPBACK_IMMEDIATELY			0xB
+#define LOOPBACK_MAC_DELAY			0x3
+#define LOOPBACK_PHY				0x1
+#define LOOPBACK_DMA				0x7
+
+#define GET_RX_PAGE_SIZE(value)		((value) & 0xF)
+#define GET_TX_PAGE_SIZE(value)		(((value) & 0xF0) >> 4)
+#define _PSRX_MASK				0xF
+#define _PSTX_MASK				0xF0
+#define _PSRX(x)				(x)
+#define _PSTX(x)				((x) << 4)
+
+#define PBP_64					0x0
+#define PBP_128					0x1
+#define PBP_256					0x2
+#define PBP_512					0x3
+#define PBP_1024				0x4
+
+#define RXDMA_ARBBW_EN				BIT(0)
+#define RXSHFT_EN				BIT(1)
+#define RXDMA_AGG_EN				BIT(2)
+#define QS_VO_QUEUE				BIT(8)
+#define QS_VI_QUEUE				BIT(9)
+#define QS_BE_QUEUE				BIT(10)
+#define QS_BK_QUEUE				BIT(11)
+#define QS_MANAGER_QUEUE			BIT(12)
+#define QS_HIGH_QUEUE				BIT(13)
+
+#define HQSEL_VOQ				BIT(0)
+#define HQSEL_VIQ				BIT(1)
+#define HQSEL_BEQ				BIT(2)
+#define HQSEL_BKQ				BIT(3)
+#define HQSEL_MGTQ				BIT(4)
+#define HQSEL_HIQ				BIT(5)
+
+#define _TXDMA_HIQ_MAP(x)			(((x)&0x3) << 14)
+#define _TXDMA_MGQ_MAP(x)			(((x)&0x3) << 12)
+#define _TXDMA_BKQ_MAP(x)			(((x)&0x3) << 10)
+#define _TXDMA_BEQ_MAP(x)			(((x)&0x3) << 8)
+#define _TXDMA_VIQ_MAP(x)			(((x)&0x3) << 6)
+#define _TXDMA_VOQ_MAP(x)			(((x)&0x3) << 4)
+
+#define QUEUE_LOW				1
+#define QUEUE_NORMAL				2
+#define QUEUE_HIGH				3
+
+#define _LLT_NO_ACTIVE				0x0
+#define _LLT_WRITE_ACCESS			0x1
+#define _LLT_READ_ACCESS			0x2
+
+#define _LLT_INIT_DATA(x)			((x) & 0xFF)
+#define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
+#define _LLT_OP(x)				(((x) & 0x3) << 30)
+#define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
+
+#define BB_WRITE_READ_MASK			(BIT(31) | BIT(30))
+#define BB_WRITE_EN				BIT(30)
+#define BB_READ_EN				BIT(31)
+
+#define _HPQ(x)				((x) & 0xFF)
+#define _LPQ(x)				(((x) & 0xFF) << 8)
+#define _PUBQ(x)			(((x) & 0xFF) << 16)
+#define _NPQ(x)				((x) & 0xFF)
+
+#define HPQ_PUBLIC_DIS				BIT(24)
+#define LPQ_PUBLIC_DIS				BIT(25)
+#define LD_RQPN					BIT(31)
+
+#define BCN_VALID				BIT(16)
+#define BCN_HEAD(x)			(((x) & 0xFF) << 8)
+#define	BCN_HEAD_MASK				0xFF00
+
+#define BLK_DESC_NUM_SHIFT			4
+#define BLK_DESC_NUM_MASK			0xF
+
+#define DROP_DATA_EN				BIT(9)
+
+#define EN_AMPDU_RTY_NEW			BIT(7)
+
+#define _INIRTSMCS_SEL(x)			((x) & 0x3F)
+
+#define _SPEC_SIFS_CCK(x)			((x) & 0xFF)
+#define _SPEC_SIFS_OFDM(x)			(((x) & 0xFF) << 8)
+
+#define RATE_REG_BITMAP_ALL			0xFFFFF
+
+#define _RRSC_BITMAP(x)				((x) & 0xFFFFF)
+
+#define _RRSR_RSC(x)				(((x) & 0x3) << 21)
+#define RRSR_RSC_RESERVED			0x0
+#define RRSR_RSC_UPPER_SUBCHANNEL		0x1
+#define RRSR_RSC_LOWER_SUBCHANNEL		0x2
+#define RRSR_RSC_DUPLICATE_MODE			0x3
+
+#define USE_SHORT_G1				BIT(20)
+
+#define _AGGLMT_MCS0(x)				((x) & 0xF)
+#define _AGGLMT_MCS1(x)				(((x) & 0xF) << 4)
+#define _AGGLMT_MCS2(x)				(((x) & 0xF) << 8)
+#define _AGGLMT_MCS3(x)				(((x) & 0xF) << 12)
+#define _AGGLMT_MCS4(x)				(((x) & 0xF) << 16)
+#define _AGGLMT_MCS5(x)				(((x) & 0xF) << 20)
+#define _AGGLMT_MCS6(x)				(((x) & 0xF) << 24)
+#define _AGGLMT_MCS7(x)				(((x) & 0xF) << 28)
+
+#define	RETRY_LIMIT_SHORT_SHIFT		8
+#define	RETRY_LIMIT_LONG_SHIFT		0
+
+#define _DARF_RC1(x)			((x) & 0x1F)
+#define _DARF_RC2(x)			(((x) & 0x1F) << 8)
+#define _DARF_RC3(x)			(((x) & 0x1F) << 16)
+#define _DARF_RC4(x)			(((x) & 0x1F) << 24)
+#define _DARF_RC5(x)			((x) & 0x1F)
+#define _DARF_RC6(x)			(((x) & 0x1F) << 8)
+#define _DARF_RC7(x)			(((x) & 0x1F) << 16)
+#define _DARF_RC8(x)			(((x) & 0x1F) << 24)
+
+#define _RARF_RC1(x)			((x) & 0x1F)
+#define _RARF_RC2(x)			(((x) & 0x1F) << 8)
+#define _RARF_RC3(x)			(((x) & 0x1F) << 16)
+#define _RARF_RC4(x)			(((x) & 0x1F) << 24)
+#define _RARF_RC5(x)			((x) & 0x1F)
+#define _RARF_RC6(x)			(((x) & 0x1F) << 8)
+#define _RARF_RC7(x)			(((x) & 0x1F) << 16)
+#define _RARF_RC8(x)			(((x) & 0x1F) << 24)
+
+#define AC_PARAM_TXOP_LIMIT_OFFSET	16
+#define AC_PARAM_ECW_MAX_OFFSET		12
+#define AC_PARAM_ECW_MIN_OFFSET		8
+#define AC_PARAM_AIFS_OFFSET		0
+
+#define _AIFS(x)			(x)
+#define _ECW_MAX_MIN(x)			((x) << 8)
+#define _TXOP_LIMIT(x)			((x) << 16)
+
+#define _BCNIFS(x)			((x) & 0xFF)
+#define _BCNECW(x)			((((x) & 0xF)) << 8)
+
+#define _LRL(x)				((x) & 0x3F)
+#define _SRL(x)				(((x) & 0x3F) << 8)
+
+#define _SIFS_CCK_CTX(x)		((x) & 0xFF)
+#define _SIFS_CCK_TRX(x)		(((x) & 0xFF) << 8)
+
+#define _SIFS_OFDM_CTX(x)		((x) & 0xFF)
+#define _SIFS_OFDM_TRX(x)		(((x) & 0xFF) << 8)
+
+#define _TBTT_PROHIBIT_HOLD(x)		(((x) & 0xFF) << 8)
+
+#define DIS_EDCA_CNT_DWN		BIT(11)
+
+#define EN_MBSSID			BIT(1)
+#define EN_TXBCN_RPT			BIT(2)
+#define	EN_BCN_FUNCTION			BIT(3)
+
+#define TSFTR_RST			BIT(0)
+#define TSFTR1_RST			BIT(1)
+
+#define STOP_BCNQ			BIT(6)
+
+#define	DIS_TSF_UDT0_NORMAL_CHIP	BIT(4)
+#define	DIS_TSF_UDT0_TEST_CHIP		BIT(5)
+
+#define	ACMHW_HWEN			BIT(0)
+#define	ACMHW_BEQEN			BIT(1)
+#define	ACMHW_VIQEN			BIT(2)
+#define	ACMHW_VOQEN			BIT(3)
+#define	ACMHW_BEQSTATUS			BIT(4)
+#define	ACMHW_VIQSTATUS			BIT(5)
+#define	ACMHW_VOQSTATUS			BIT(6)
+
+#define APSDOFF				BIT(6)
+#define APSDOFF_STATUS			BIT(7)
+
+#define BW_20MHZ			BIT(2)
+
+#define RATE_BITMAP_ALL			0xFFFFF
+
+#define RATE_RRSR_CCK_ONLY_1M		0xFFFF1
+
+#define TSFRST				BIT(0)
+#define DIS_GCLK			BIT(1)
+#define PAD_SEL				BIT(2)
+#define PWR_ST				BIT(6)
+#define PWRBIT_OW_EN			BIT(7)
+#define ACRC				BIT(8)
+#define CFENDFORM			BIT(9)
+#define ICV				BIT(10)
+
+#define AAP				BIT(0)
+#define APM				BIT(1)
+#define AM				BIT(2)
+#define AB				BIT(3)
+#define ADD3				BIT(4)
+#define APWRMGT				BIT(5)
+#define CBSSID				BIT(6)
+#define CBSSID_DATA			BIT(6)
+#define CBSSID_BCN			BIT(7)
+#define ACRC32				BIT(8)
+#define AICV				BIT(9)
+#define ADF				BIT(11)
+#define ACF				BIT(12)
+#define AMF				BIT(13)
+#define HTC_LOC_CTRL			BIT(14)
+#define UC_DATA_EN			BIT(16)
+#define BM_DATA_EN			BIT(17)
+#define MFBEN				BIT(22)
+#define LSIGEN				BIT(23)
+#define ENMBID				BIT(24)
+#define APP_BASSN			BIT(27)
+#define APP_PHYSTS			BIT(28)
+#define APP_ICV				BIT(29)
+#define APP_MIC				BIT(30)
+#define APP_FCS				BIT(31)
+
+#define _MIN_SPACE(x)			((x) & 0x7)
+#define _SHORT_GI_PADDING(x)		(((x) & 0x1F) << 3)
+
+#define RXERR_TYPE_OFDM_PPDU		0
+#define RXERR_TYPE_OFDM_FALSE_ALARM	1
+#define	RXERR_TYPE_OFDM_MPDU_OK		2
+#define RXERR_TYPE_OFDM_MPDU_FAIL	3
+#define RXERR_TYPE_CCK_PPDU		4
+#define RXERR_TYPE_CCK_FALSE_ALARM	5
+#define RXERR_TYPE_CCK_MPDU_OK		6
+#define RXERR_TYPE_CCK_MPDU_FAIL	7
+#define RXERR_TYPE_HT_PPDU		8
+#define RXERR_TYPE_HT_FALSE_ALARM	9
+#define RXERR_TYPE_HT_MPDU_TOTAL	10
+#define RXERR_TYPE_HT_MPDU_OK		11
+#define RXERR_TYPE_HT_MPDU_FAIL		12
+#define RXERR_TYPE_RX_FULL_DROP		15
+
+#define RXERR_COUNTER_MASK		0xFFFFF
+#define RXERR_RPT_RST			BIT(27)
+#define _RXERR_RPT_SEL(type)		((type) << 28)
+
+#define	SCR_TXUSEDK			BIT(0)
+#define	SCR_RXUSEDK			BIT(1)
+#define	SCR_TXENCENABLE			BIT(2)
+#define	SCR_RXDECENABLE			BIT(3)
+#define	SCR_SKBYA2			BIT(4)
+#define	SCR_NOSKMC			BIT(5)
+#define SCR_TXBCUSEDK			BIT(6)
+#define SCR_RXBCUSEDK			BIT(7)
+
+#define XCLK_VLD			BIT(0)
+#define ACLK_VLD			BIT(1)
+#define UCLK_VLD			BIT(2)
+#define PCLK_VLD			BIT(3)
+#define PCIRSTB				BIT(4)
+#define V15_VLD				BIT(5)
+#define TRP_B15V_EN			BIT(7)
+#define SIC_IDLE			BIT(8)
+#define BD_MAC2				BIT(9)
+#define BD_MAC1				BIT(10)
+#define IC_MACPHY_MODE			BIT(11)
+#define BT_FUNC				BIT(16)
+#define VENDOR_ID			BIT(19)
+#define PAD_HWPD_IDN			BIT(22)
+#define TRP_VAUX_EN			BIT(23)
+#define TRP_BT_EN			BIT(24)
+#define BD_PKG_SEL			BIT(25)
+#define BD_HCI_SEL			BIT(26)
+#define TYPE_ID				BIT(27)
+
+#define USB_IS_HIGH_SPEED		0
+#define USB_IS_FULL_SPEED		1
+#define USB_SPEED_MASK			BIT(5)
+
+#define USB_NORMAL_SIE_EP_MASK		0xF
+#define USB_NORMAL_SIE_EP_SHIFT		4
+
+#define USB_TEST_EP_MASK		0x30
+#define USB_TEST_EP_SHIFT		4
+
+#define USB_AGG_EN			BIT(3)
+
+#define MAC_ADDR_LEN			6
+#define LAST_ENTRY_OF_TX_PKT_BUFFER	255
+
+#define POLLING_LLT_THRESHOLD		20
+#define POLLING_READY_TIMEOUT_COUNT	3000
+
+#define	MAX_MSS_DENSITY_2T		0x13
+#define	MAX_MSS_DENSITY_1T		0x0A
+
+#define EPROM_CMD_OPERATING_MODE_MASK	((1<<7)|(1<<6))
+#define EPROM_CMD_CONFIG		0x3
+#define EPROM_CMD_LOAD			1
+
+#define	HWSET_MAX_SIZE_92S		HWSET_MAX_SIZE
+
+#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT(2)
+
+#define RA_LSSIWRITE_8821A		0xc90
+#define RB_LSSIWRITE_8821A		0xe90
+
+#define	RA_PIREAD_8821A			0xd04
+#define	RB_PIREAD_8821A			0xd44
+#define	RA_SIREAD_8821A			0xd08
+#define	RB_SIREAD_8821A			0xd48
+
+#define	RPMAC_RESET			0x100
+#define	RPMAC_TXSTART			0x104
+#define	RPMAC_TXLEGACYSIG		0x108
+#define	RPMAC_TXHTSIG1			0x10c
+#define	RPMAC_TXHTSIG2			0x110
+#define	RPMAC_PHYDEBUG			0x114
+#define	RPMAC_TXPACKETNUM		0x118
+#define	RPMAC_TXIDLE			0x11c
+#define	RPMAC_TXMACHEADER0		0x120
+#define	RPMAC_TXMACHEADER1		0x124
+#define	RPMAC_TXMACHEADER2		0x128
+#define	RPMAC_TXMACHEADER3		0x12c
+#define	RPMAC_TXMACHEADER4		0x130
+#define	RPMAC_TXMACHEADER5		0x134
+#define	RPMAC_TXDADATYPE		0x138
+#define	RPMAC_TXRANDOMSEED		0x13c
+#define	RPMAC_CCKPLCPPREAMBLE		0x140
+#define	RPMAC_CCKPLCPHEADER		0x144
+#define	RPMAC_CCKCRC16			0x148
+#define	RPMAC_OFDMRXCRC32OK		0x170
+#define	RPMAC_OFDMRXCRC32ER		0x174
+#define	RPMAC_OFDMRXPARITYER		0x178
+#define	RPMAC_OFDMRXCRC8ER		0x17c
+#define	RPMAC_CCKCRXRC16ER		0x180
+#define	RPMAC_CCKCRXRC32ER		0x184
+#define	RPMAC_CCKCRXRC32OK		0x188
+#define	RPMAC_TXSTATUS			0x18c
+
+#define	RFPGA0_RFMOD			0x800
+
+#define	RFPGA0_TXINFO			0x804
+#define	RFPGA0_PSDFUNCTION		0x808
+
+#define	RFPGA0_TXGAINSTAGE		0x80c
+
+#define	RFPGA0_RFTIMING1		0x810
+#define	RFPGA0_RFTIMING2		0x814
+
+#define	RFPGA0_XA_HSSIPARAMETER1	0x820
+#define	RFPGA0_XA_HSSIPARAMETER2	0x824
+#define	RFPGA0_XB_HSSIPARAMETER1	0x828
+#define	RFPGA0_XB_HSSIPARAMETER2	0x82c
+#define	RCCAONSEC			0x838
+
+#define	RFPGA0_XA_LSSIPARAMETER		0x840
+#define	RFPGA0_XB_LSSIPARAMETER		0x844
+#define	RL1PEAKTH			0x848
+
+#define	RFPGA0_RFWAKEUPPARAMETER	0x850
+#define	RFPGA0_RFSLEEPUPPARAMETER	0x854
+
+#define	RFPGA0_XAB_SWITCHCONTROL	0x858
+#define	RFPGA0_XCD_SWITCHCONTROL	0x85c
+
+#define	RFPGA0_XA_RFINTERFACEOE		0x860
+#define RFC_AREA			0x860
+#define	RFPGA0_XB_RFINTERFACEOE		0x864
+
+#define	RFPGA0_XAB_RFINTERFACESW	0x870
+#define	RFPGA0_XCD_RFINTERFACESW	0x874
+
+#define	RFPGA0_XAB_RFPARAMETER		0x878
+#define	RFPGA0_XCD_RFPARAMETER		0x87c
+
+#define	RFPGA0_ANALOGPARAMETER1		0x880
+#define	RFPGA0_ANALOGPARAMETER2		0x884
+#define	RFPGA0_ANALOGPARAMETER3		0x888
+#define	RFPGA0_ANALOGPARAMETER4		0x88c
+
+#define	RFPGA0_XA_LSSIREADBACK		0x8a0
+#define	RFPGA0_XB_LSSIREADBACK		0x8a4
+#define	RFPGA0_XC_LSSIREADBACK		0x8a8
+#define RRFMOD				0x8ac
+#define	RHSSIREAD_8821AE		0x8b0
+
+#define	RFPGA0_PSDREPORT		0x8b4
+#define	TRANSCEIVEA_HSPI_READBACK	0x8b8
+#define	TRANSCEIVEB_HSPI_READBACK	0x8bc
+#define RADC_BUF_CLK			0x8c4
+#define	RFPGA0_XAB_RFINTERFACERB	0x8e0
+#define	RFPGA0_XCD_RFINTERFACERB	0x8e4
+
+#define	RFPGA1_RFMOD			0x900
+
+#define	RFPGA1_TXBLOCK			0x904
+#define	RFPGA1_DEBUGSELECT		0x908
+#define	RFPGA1_TXINFO			0x90c
+
+#define	RCCK_SYSTEM			0xa00
+#define	BCCK_SYSTEM			0x10
+
+#define	RCCK0_AFESETTING		0xa04
+#define	RCCK0_CCA			0xa08
+
+#define	RCCK0_RXAGC1			0xa0c
+#define	RCCK0_RXAGC2			0xa10
+
+#define	RCCK0_RXHP			0xa14
+
+#define	RCCK0_DSPPARAMETER1		0xa18
+#define	RCCK0_DSPPARAMETER2		0xa1c
+
+#define	RCCK0_TXFILTER1			0xa20
+#define	RCCK0_TXFILTER2			0xa24
+#define	RCCK0_DEBUGPORT			0xa28
+#define	RCCK0_FALSEALARMREPORT		0xa2c
+#define	RCCK0_TRSSIREPORT		0xa50
+#define	RCCK0_RXREPORT			0xa54
+#define	RCCK0_FACOUNTERLOWER		0xa5c
+#define	RCCK0_FACOUNTERUPPER		0xa58
+#define	RCCK0_CCA_CNT			0xa60
+
+/* PageB(0xB00) */
+#define	RPDP_ANTA			0xb00
+#define	RPDP_ANTA_4			0xb04
+#define	RPDP_ANTA_8			0xb08
+#define	RPDP_ANTA_C			0xb0c
+#define	RPDP_ANTA_10			0xb10
+#define	RPDP_ANTA_14			0xb14
+#define	RPDP_ANTA_18			0xb18
+#define	RPDP_ANTA_1C			0xb1c
+#define	RPDP_ANTA_20			0xb20
+#define	RPDP_ANTA_24			0xb24
+
+#define	RCONFIG_PMPD_ANTA		0xb28
+#define	RCONFIG_RAM64x16		0xb2c
+
+#define	RBNDA				0xb30
+#define	RHSSIPAR			0xb34
+
+#define	RCONFIG_ANTA			0xb68
+#define	RCONFIG_ANTB			0xb6c
+
+#define	RPDP_ANTB			0xb70
+#define	RPDP_ANTB_4			0xb74
+#define	RPDP_ANTB_8			0xb78
+#define	RPDP_ANTB_C			0xb7c
+#define	RPDP_ANTB_10			0xb80
+#define	RPDP_ANTB_14			0xb84
+#define	RPDP_ANTB_18			0xb88
+#define	RPDP_ANTB_1C			0xb8c
+#define	RPDP_ANTB_20			0xb90
+#define	RPDP_ANTB_24			0xb94
+
+#define	RCONFIG_PMPD_ANTB		0xb98
+
+#define	RBNDB				0xba0
+
+#define	RAPK				0xbd8
+#define	RPM_RX0_ANTA			0xbdc
+#define	RPM_RX1_ANTA			0xbe0
+#define	RPM_RX2_ANTA			0xbe4
+#define	RPM_RX3_ANTA			0xbe8
+#define	RPM_RX0_ANTB			0xbec
+#define	RPM_RX1_ANTB			0xbf0
+#define	RPM_RX2_ANTB			0xbf4
+#define	RPM_RX3_ANTB			0xbf8
+
+/*RSSI Dump*/
+#define		RA_RSSI_DUMP		0xBF0
+#define		RB_RSSI_DUMP		0xBF1
+#define		RS1_RX_EVM_DUMP		0xBF4
+#define		RS2_RX_EVM_DUMP		0xBF5
+#define		RA_RX_SNR_DUMP		0xBF6
+#define		RB_RX_SNR_DUMP		0xBF7
+#define		RA_CFO_SHORT_DUMP	0xBF8
+#define		RB_CFO_SHORT_DUMP	0xBFA
+#define		RA_CFO_LONG_DUMP	0xBEC
+#define		RB_CFO_LONG_DUMP	0xBEE
+
+/*Page C*/
+#define	ROFDM0_LSTF			0xc00
+
+#define	ROFDM0_TRXPATHENABLE		0xc04
+#define	ROFDM0_TRMUXPAR			0xc08
+#define	ROFDM0_TRSWISOLATION		0xc0c
+
+#define	ROFDM0_XARXAFE			0xc10
+#define	ROFDM0_XARXIQIMBALANCE		0xc14
+#define	ROFDM0_XBRXAFE			0xc18
+#define	ROFDM0_XBRXIQIMBALANCE		0xc1c
+#define	ROFDM0_XCRXAFE			0xc20
+#define	ROFDM0_XCRXIQIMBANLANCE		0xc24
+#define	ROFDM0_XDRXAFE			0xc28
+#define	ROFDM0_XDRXIQIMBALANCE		0xc2c
+
+#define	ROFDM0_RXDETECTOR1		0xc30
+#define	ROFDM0_RXDETECTOR2		0xc34
+#define	ROFDM0_RXDETECTOR3		0xc38
+#define	ROFDM0_RXDETECTOR4		0xc3c
+
+#define	ROFDM0_RXDSP			0xc40
+#define	ROFDM0_CFOANDDAGC		0xc44
+#define	ROFDM0_CCADROPTHRESHOLD		0xc48
+#define	ROFDM0_ECCATHRESHOLD		0xc4c
+
+#define	ROFDM0_XAAGCCORE1		0xc50
+#define	ROFDM0_XAAGCCORE2		0xc54
+#define	ROFDM0_XBAGCCORE1		0xc58
+#define	ROFDM0_XBAGCCORE2		0xc5c
+#define	ROFDM0_XCAGCCORE1		0xc60
+#define	ROFDM0_XCAGCCORE2		0xc64
+#define	ROFDM0_XDAGCCORE1		0xc68
+#define	ROFDM0_XDAGCCORE2		0xc6c
+
+#define	ROFDM0_AGCPARAMETER1		0xc70
+#define	ROFDM0_AGCPARAMETER2		0xc74
+#define	ROFDM0_AGCRSSITABLE		0xc78
+#define	ROFDM0_HTSTFAGC			0xc7c
+
+#define	ROFDM0_XATXIQIMBALANCE		0xc80
+#define	ROFDM0_XATXAFE			0xc84
+#define	ROFDM0_XBTXIQIMBALANCE		0xc88
+#define	ROFDM0_XBTXAFE			0xc8c
+#define	ROFDM0_XCTXIQIMBALANCE		0xc90
+#define	ROFDM0_XCTXAFE			0xc94
+#define	ROFDM0_XDTXIQIMBALANCE		0xc98
+#define	ROFDM0_XDTXAFE			0xc9c
+
+#define ROFDM0_RXIQEXTANTA		0xca0
+#define	ROFDM0_TXCOEFF1			0xca4
+#define	ROFDM0_TXCOEFF2			0xca8
+#define	ROFDM0_TXCOEFF3			0xcac
+#define	ROFDM0_TXCOEFF4			0xcb0
+#define	ROFDM0_TXCOEFF5			0xcb4
+#define	ROFDM0_TXCOEFF6			0xcb8
+
+/*Path_A RFE cotrol */
+#define	RA_RFE_CTRL_8812		0xcb8
+/*Path_B RFE control*/
+#define	RB_RFE_CTRL_8812		0xeb8
+
+#define	ROFDM0_RXHPPARAMETER		0xce0
+#define	ROFDM0_TXPSEUDONOISEWGT		0xce4
+#define	ROFDM0_FRAMESYNC		0xcf0
+#define	ROFDM0_DFSREPORT		0xcf4
+
+#define	ROFDM1_LSTF			0xd00
+#define	ROFDM1_TRXPATHENABLE		0xd04
+
+#define	ROFDM1_CF0			0xd08
+#define	ROFDM1_CSI1			0xd10
+#define	ROFDM1_SBD			0xd14
+#define	ROFDM1_CSI2			0xd18
+#define	ROFDM1_CFOTRACKING		0xd2c
+#define	ROFDM1_TRXMESAURE1		0xd34
+#define	ROFDM1_INTFDET			0xd3c
+#define	ROFDM1_PSEUDONOISESTATEAB	0xd50
+#define	ROFDM1_PSEUDONOISESTATECD	0xd54
+#define	ROFDM1_RXPSEUDONOISEWGT		0xd58
+
+#define	ROFDM_PHYCOUNTER1		0xda0
+#define	ROFDM_PHYCOUNTER2		0xda4
+#define	ROFDM_PHYCOUNTER3		0xda8
+
+#define	ROFDM_SHORTCFOAB		0xdac
+#define	ROFDM_SHORTCFOCD		0xdb0
+#define	ROFDM_LONGCFOAB			0xdb4
+#define	ROFDM_LONGCFOCD			0xdb8
+#define	ROFDM_TAILCF0AB			0xdbc
+#define	ROFDM_TAILCF0CD			0xdc0
+#define	ROFDM_PWMEASURE1		0xdc4
+#define	ROFDM_PWMEASURE2		0xdc8
+#define	ROFDM_BWREPORT			0xdcc
+#define	ROFDM_AGCREPORT			0xdd0
+#define	ROFDM_RXSNR			0xdd4
+#define	ROFDM_RXEVMCSI			0xdd8
+#define	ROFDM_SIGREPORT			0xddc
+
+#define RTXAGC_A_CCK11_CCK1		0xc20
+#define RTXAGC_A_OFDM18_OFDM6		0xc24
+#define RTXAGC_A_OFDM54_OFDM24		0xc28
+#define RTXAGC_A_MCS03_MCS00		0xc2c
+#define RTXAGC_A_MCS07_MCS04		0xc30
+#define RTXAGC_A_MCS11_MCS08		0xc34
+#define RTXAGC_A_MCS15_MCS12		0xc38
+#define RTXAGC_A_NSS1INDEX3_NSS1INDEX0	0xc3c
+#define	RTXAGC_A_NSS1INDEX7_NSS1INDEX4	0xc40
+#define	RTXAGC_A_NSS2INDEX1_NSS1INDEX8	0xc44
+#define	RTXAGC_A_NSS2INDEX5_NSS2INDEX2	0xc48
+#define	RTXAGC_A_NSS2INDEX9_NSS2INDEX6	0xc4c
+#define	RTXAGC_B_CCK11_CCK1		0xe20
+#define	RTXAGC_B_OFDM18_OFDM6		0xe24
+#define	RTXAGC_B_OFDM54_OFDM24		0xe28
+#define	RTXAGC_B_MCS03_MCS00		0xe2c
+#define	RTXAGC_B_MCS07_MCS04		0xe30
+#define	RTXAGC_B_MCS11_MCS08		0xe34
+#define	RTXAGC_B_MCS15_MCS12		0xe38
+#define	RTXAGC_B_NSS1INDEX3_NSS1INDEX0	0xe3c
+#define	RTXAGC_B_NSS1INDEX7_NSS1INDEX4	0xe40
+#define	RTXAGC_B_NSS2INDEX1_NSS1INDEX8	0xe44
+#define	RTXAGC_B_NSS2INDEX5_NSS2INDEX2	0xe48
+#define	RTXAGC_B_NSS2INDEX9_NSS2INDEX6	0xe4c
+
+#define	RA_TXPWRTRAING			0xc54
+#define	RB_TXPWRTRAING			0xe54
+
+#define	RFPGA0_IQK			0xe28
+#define	RTX_IQK_TONE_A			0xe30
+#define	RRX_IQK_TONE_A			0xe34
+#define	RTX_IQK_PI_A			0xe38
+#define	RRX_IQK_PI_A			0xe3c
+
+#define	RTX_IQK				0xe40
+#define	RRX_IQK				0xe44
+#define	RIQK_AGC_PTS			0xe48
+#define	RIQK_AGC_RSP			0xe4c
+#define	RTX_IQK_TONE_B			0xe50
+#define	RRX_IQK_TONE_B			0xe54
+#define	RTX_IQK_PI_B			0xe58
+#define	RRX_IQK_PI_B			0xe5c
+#define	RIQK_AGC_CONT			0xe60
+
+#define	RBLUE_TOOTH			0xe6c
+#define	RRX_WAIT_CCA			0xe70
+#define	RTX_CCK_RFON			0xe74
+#define	RTX_CCK_BBON			0xe78
+#define	RTX_OFDM_RFON			0xe7c
+#define	RTX_OFDM_BBON			0xe80
+#define	RTX_TO_RX			0xe84
+#define	RTX_TO_TX			0xe88
+#define	RRX_CCK				0xe8c
+
+#define	RTX_POWER_BEFORE_IQK_A		0xe94
+#define	RTX_POWER_AFTER_IQK_A		0xe9c
+
+#define	RRX_POWER_BEFORE_IQK_A		0xea0
+#define	RRX_POWER_BEFORE_IQK_A_2	0xea4
+#define	RRX_POWER_AFTER_IQK_A		0xea8
+#define	RRX_POWER_AFTER_IQK_A_2		0xeac
+
+#define	RTX_POWER_BEFORE_IQK_B		0xeb4
+#define	RTX_POWER_AFTER_IQK_B		0xebc
+
+#define	RRX_POER_BEFORE_IQK_B		0xec0
+#define	RRX_POER_BEFORE_IQK_B_2		0xec4
+#define	RRX_POWER_AFTER_IQK_B		0xec8
+#define	RRX_POWER_AFTER_IQK_B_2		0xecc
+
+#define	RRX_OFDM			0xed0
+#define	RRX_WAIT_RIFS			0xed4
+#define	RRX_TO_RX			0xed8
+#define	RSTANDBY			0xedc
+#define	RSLEEP				0xee0
+#define	RPMPD_ANAEN			0xeec
+
+#define	RZEBRA1_HSSIENABLE		0x0
+#define	RZEBRA1_TRXENABLE1		0x1
+#define	RZEBRA1_TRXENABLE2		0x2
+#define	RZEBRA1_AGC			0x4
+#define	RZEBRA1_CHARGEPUMP		0x5
+#define	RZEBRA1_CHANNEL			0x7
+
+#define	RZEBRA1_TXGAIN			0x8
+#define	RZEBRA1_TXLPF			0x9
+#define	RZEBRA1_RXLPF			0xb
+#define	RZEBRA1_RXHPFCORNER		0xc
+
+#define	RGLOBALCTRL			0
+#define	RRTL8256_TXLPF			19
+#define	RRTL8256_RXLPF			11
+#define	RRTL8258_TXLPF			0x11
+#define	RRTL8258_RXLPF			0x13
+#define	RRTL8258_RSSILPF		0xa
+
+#define	RF_AC				0x00
+
+#define	RF_IQADJ_G1			0x01
+#define	RF_IQADJ_G2			0x02
+#define	RF_POW_TRSW			0x05
+
+#define	RF_GAIN_RX			0x06
+#define	RF_GAIN_TX			0x07
+
+#define	RF_TXM_IDAC			0x08
+#define	RF_BS_IQGEN			0x0F
+
+#define	RF_MODE1			0x10
+#define	RF_MODE2			0x11
+
+#define	RF_RX_AGC_HP			0x12
+#define	RF_TX_AGC			0x13
+#define	RF_BIAS				0x14
+#define	RF_IPA				0x15
+#define	RF_POW_ABILITY			0x17
+#define	RF_MODE_AG			0x18
+#define	RRFCHANNEL			0x18
+#define	RF_CHNLBW			0x18
+#define	RF_TOP				0x19
+
+#define	RF_RX_G1			0x1A
+#define	RF_RX_G2			0x1B
+
+#define	RF_RX_BB2			0x1C
+#define	RF_RX_BB1			0x1D
+
+#define	RF_RCK1				0x1E
+#define	RF_RCK2				0x1F
+
+#define	RF_TX_G1			0x20
+#define	RF_TX_G2			0x21
+#define	RF_TX_G3			0x22
+
+#define	RF_TX_BB1			0x23
+#define	RF_T_METER			0x24
+#define	RF_T_METER_88E			0x42
+#define  RF_T_METER_8812A		0x42
+
+#define	RF_SYN_G1			0x25
+#define	RF_SYN_G2			0x26
+#define	RF_SYN_G3			0x27
+#define	RF_SYN_G4			0x28
+#define	RF_SYN_G5			0x29
+#define	RF_SYN_G6			0x2A
+#define	RF_SYN_G7			0x2B
+#define	RF_SYN_G8			0x2C
+
+#define	RF_RCK_OS			0x30
+#define	RF_TXPA_G1			0x31
+#define	RF_TXPA_G2			0x32
+#define	RF_TXPA_G3			0x33
+
+#define	RF_TX_BIAS_A			0x35
+#define	RF_TX_BIAS_D			0x36
+#define	RF_LOBF_9			0x38
+#define	RF_RXRF_A3			0x3C
+#define	RF_TRSW				0x3F
+
+#define	RF_TXRF_A2			0x41
+#define	RF_TXPA_G4			0x46
+#define	RF_TXPA_A4			0x4B
+
+#define RF_APK				0x63
+
+#define	RF_WE_LUT			0xEF
+
+#define	BBBRESETB			0x100
+#define	BGLOBALRESETB			0x200
+#define	BOFDMTXSTART			0x4
+#define	BCCKTXSTART			0x8
+#define	BCRC32DEBUG			0x100
+#define	BPMACLOOPBACK			0x10
+#define	BTXLSIG				0xffffff
+#define	BOFDMTXRATE			0xf
+#define	BOFDMTXRESERVED			0x10
+#define	BOFDMTXLENGTH			0x1ffe0
+#define	BOFDMTXPARITY			0x20000
+#define	BTXHTSIG1			0xffffff
+#define	BTXHTMCSRATE			0x7f
+#define	BTXHTBW				0x80
+#define	BTXHTLENGTH			0xffff00
+#define	BTXHTSIG2			0xffffff
+#define	BTXHTSMOOTHING			0x1
+#define	BTXHTSOUNDING			0x2
+#define	BTXHTRESERVED			0x4
+#define	BTXHTAGGREATION			0x8
+#define	BTXHTSTBC			0x30
+#define	BTXHTADVANCECODING		0x40
+#define	BTXHTSHORTGI			0x80
+#define	BTXHTNUMBERHT_LTF		0x300
+#define	BTXHTCRC8			0x3fc00
+#define	BCOUNTERRESET			0x10000
+#define	BNUMOFOFDMTX			0xffff
+#define	BNUMOFCCKTX			0xffff0000
+#define	BTXIDLEINTERVAL			0xffff
+#define	BOFDMSERVICE			0xffff0000
+#define	BTXMACHEADER			0xffffffff
+#define	BTXDATAINIT			0xff
+#define	BTXHTMODE			0x100
+#define	BTXDATATYPE			0x30000
+#define	BTXRANDOMSEED			0xffffffff
+#define	BCCKTXPREAMBLE			0x1
+#define	BCCKTXSFD			0xffff0000
+#define	BCCKTXSIG			0xff
+#define	BCCKTXSERVICE			0xff00
+#define	BCCKLENGTHEXT			0x8000
+#define	BCCKTXLENGHT			0xffff0000
+#define	BCCKTXCRC16			0xffff
+#define	BCCKTXSTATUS			0x1
+#define	BOFDMTXSTATUS			0x2
+#define IS_BB_REG_OFFSET_92S(__offset)	\
+	((__offset >= 0x800) && (__offset <= 0xfff))
+
+#define	BRFMOD				0x1
+#define	BJAPANMODE			0x2
+#define	BCCKTXSC			0x30
+/* Block & Path enable*/
+#define ROFDMCCKEN			0x808
+#define	BCCKEN				0x10000000
+#define	BOFDMEN				0x20000000
+/* Rx antenna*/
+#define	RRXPATH				0x808
+#define	BRXPATH				0xff
+/* Tx antenna*/
+#define	RTXPATH				0x80c
+#define	BTXPATH				0x0fffffff
+/* for cck rx path selection*/
+#define	RCCK_RX				0xa04
+#define	BCCK_RX				0x0c000000
+/* Use LSIG for VHT length*/
+#define	RVHTLEN_USE_LSIG		0x8c3
+
+#define	BOFDMRXADCPHASE			0x10000
+#define	BOFDMTXDACPHASE			0x40000
+#define	BXATXAGC			0x3f
+
+#define	BXBTXAGC			0xf00
+#define	BXCTXAGC			0xf000
+#define	BXDTXAGC			0xf0000
+
+#define	BPASTART			0xf0000000
+#define	BTRSTART			0x00f00000
+#define	BRFSTART			0x0000f000
+#define	BBBSTART			0x000000f0
+#define	BBBCCKSTART			0x0000000f
+#define	BPAEND				0xf
+#define	BTREND				0x0f000000
+#define	BRFEND				0x000f0000
+#define	BCCAMASK			0x000000f0
+#define	BR2RCCAMASK			0x00000f00
+#define	BHSSI_R2TDELAY			0xf8000000
+#define	BHSSI_T2RDELAY			0xf80000
+#define	BCONTXHSSI			0x400
+#define	BIGFROMCCK			0x200
+#define	BAGCADDRESS			0x3f
+#define	BRXHPTX				0x7000
+#define	BRXHP2RX			0x38000
+#define	BRXHPCCKINI			0xc0000
+#define	BAGCTXCODE			0xc00000
+#define	BAGCRXCODE			0x300000
+
+#define	B3WIREDATALENGTH		0x800
+#define	B3WIREADDREAALENGTH		0x400
+
+#define	B3WIRERFPOWERDOWN		0x1
+#define	B5GPAPEPOLARITY			0x40000000
+#define	B2GPAPEPOLARITY			0x80000000
+#define	BRFSW_TXDEFAULTANT		0x3
+#define	BRFSW_TXOPTIONANT		0x30
+#define	BRFSW_RXDEFAULTANT		0x300
+#define	BRFSW_RXOPTIONANT		0x3000
+#define	BRFSI_3WIREDATA			0x1
+#define	BRFSI_3WIRECLOCK		0x2
+#define	BRFSI_3WIRELOAD			0x4
+#define	BRFSI_3WIRERW			0x8
+#define	BRFSI_3WIRE			0xf
+
+#define	BRFSI_RFENV			0x10
+
+#define	BRFSI_TRSW			0x20
+#define	BRFSI_TRSWB			0x40
+#define	BRFSI_ANTSW			0x100
+#define	BRFSI_ANTSWB			0x200
+#define	BRFSI_PAPE			0x400
+#define	BRFSI_PAPE5G			0x800
+#define	BBANDSELECT			0x1
+#define	BHTSIG2_GI			0x80
+#define	BHTSIG2_SMOOTHING		0x01
+#define	BHTSIG2_SOUNDING		0x02
+#define	BHTSIG2_AGGREATON		0x08
+#define	BHTSIG2_STBC			0x30
+#define	BHTSIG2_ADVCODING		0x40
+#define	BHTSIG2_NUMOFHTLTF		0x300
+#define	BHTSIG2_CRC8			0x3fc
+#define	BHTSIG1_MCS			0x7f
+#define	BHTSIG1_BANDWIDTH		0x80
+#define	BHTSIG1_HTLENGTH		0xffff
+#define	BLSIG_RATE			0xf
+#define	BLSIG_RESERVED			0x10
+#define	BLSIG_LENGTH			0x1fffe
+#define	BLSIG_PARITY			0x20
+#define	BCCKRXPHASE			0x4
+
+#define	BLSSIREADADDRESS		0x7f800000
+#define	BLSSIREADEDGE			0x80000000
+
+#define	BLSSIREADBACKDATA		0xfffff
+
+#define	BLSSIREADOKFLAG			0x1000
+#define	BCCKSAMPLERATE			0x8
+#define	BREGULATOR0STANDBY		0x1
+#define	BREGULATORPLLSTANDBY		0x2
+#define	BREGULATOR1STANDBY		0x4
+#define	BPLLPOWERUP			0x8
+#define	BDPLLPOWERUP			0x10
+#define	BDA10POWERUP			0x20
+#define	BAD7POWERUP			0x200
+#define	BDA6POWERUP			0x2000
+#define	BXTALPOWERUP			0x4000
+#define	B40MDCLKPOWERUP			0x8000
+#define	BDA6DEBUGMODE			0x20000
+#define	BDA6SWING			0x380000
+
+#define	BADCLKPHASE			0x4000000
+#define	B80MCLKDELAY			0x18000000
+#define	BAFEWATCHDOGENABLE		0x20000000
+
+#define	BXTALCAP01			0xc0000000
+#define	BXTALCAP23			0x3
+#define	BXTALCAP92X			0x0f000000
+#define BXTALCAP			0x0f000000
+
+#define	BINTDIFCLKENABLE		0x400
+#define	BEXTSIGCLKENABLE		0x800
+#define	BBANDGAP_MBIAS_POWERUP		0x10000
+#define	BAD11SH_GAIN			0xc0000
+#define	BAD11NPUT_RANGE			0x700000
+#define	BAD110P_CURRENT			0x3800000
+#define	BLPATH_LOOPBACK			0x4000000
+#define	BQPATH_LOOPBACK			0x8000000
+#define	BAFE_LOOPBACK			0x10000000
+#define	BDA10_SWING			0x7e0
+#define	BDA10_REVERSE			0x800
+#define	BDA_CLK_SOURCE			0x1000
+#define	BDA7INPUT_RANGE			0x6000
+#define	BDA7_GAIN			0x38000
+#define	BDA7OUTPUT_CM_MODE		0x40000
+#define	BDA7INPUT_CM_MODE		0x380000
+#define	BDA7CURRENT			0xc00000
+#define	BREGULATOR_ADJUST		0x7000000
+#define	BAD11POWERUP_ATTX		0x1
+#define	BDA10PS_ATTX			0x10
+#define	BAD11POWERUP_ATRX		0x100
+#define	BDA10PS_ATRX			0x1000
+#define	BCCKRX_AGC_FORMAT		0x200
+#define	BPSDFFT_SAMPLE_POINT		0xc000
+#define	BPSD_AVERAGE_NUM		0x3000
+#define	BIQPATH_CONTROL			0xc00
+#define	BPSD_FREQ			0x3ff
+#define	BPSD_ANTENNA_PATH		0x30
+#define	BPSD_IQ_SWITCH			0x40
+#define	BPSD_RX_TRIGGER			0x400000
+#define	BPSD_TX_TRIGGER			0x80000000
+#define	BPSD_SINE_TONE_SCALE		0x7f000000
+#define	BPSD_REPORT			0xffff
+
+#define	BOFDM_TXSC			0x30000000
+#define	BCCK_TXON			0x1
+#define	BOFDM_TXON			0x2
+#define	BDEBUG_PAGE			0xfff
+#define	BDEBUG_ITEM			0xff
+#define	BANTL				0x10
+#define	BANT_NONHT			0x100
+#define	BANT_HT1			0x1000
+#define	BANT_HT2			0x10000
+#define	BANT_HT1S1			0x100000
+#define	BANT_NONHTS1			0x1000000
+
+#define	BCCK_BBMODE			0x3
+#define	BCCK_TXPOWERSAVING		0x80
+#define	BCCK_RXPOWERSAVING		0x40
+
+#define	BCCK_SIDEBAND			0x10
+
+#define	BCCK_SCRAMBLE			0x8
+#define	BCCK_ANTDIVERSITY		0x8000
+#define	BCCK_CARRIER_RECOVERY		0x4000
+#define	BCCK_TXRATE			0x3000
+#define	BCCK_DCCANCEL			0x0800
+#define	BCCK_ISICANCEL			0x0400
+#define	BCCK_MATCH_FILTER		0x0200
+#define	BCCK_EQUALIZER			0x0100
+#define	BCCK_PREAMBLE_DETECT		0x800000
+#define	BCCK_FAST_FALSECCA		0x400000
+#define	BCCK_CH_ESTSTART		0x300000
+#define	BCCK_CCA_COUNT			0x080000
+#define	BCCK_CS_LIM			0x070000
+#define	BCCK_BIST_MODE			0x80000000
+#define	BCCK_CCAMASK			0x40000000
+#define	BCCK_TX_DAC_PHASE		0x4
+#define	BCCK_RX_ADC_PHASE		0x20000000
+#define	BCCKR_CP_MODE			0x0100
+#define	BCCK_TXDC_OFFSET		0xf0
+#define	BCCK_RXDC_OFFSET		0xf
+#define	BCCK_CCA_MODE			0xc000
+#define	BCCK_FALSECS_LIM		0x3f00
+#define	BCCK_CS_RATIO			0xc00000
+#define	BCCK_CORGBIT_SEL		0x300000
+#define	BCCK_PD_LIM			0x0f0000
+#define	BCCK_NEWCCA			0x80000000
+#define	BCCK_RXHP_OF_IG			0x8000
+#define	BCCK_RXIG			0x7f00
+#define	BCCK_LNA_POLARITY		0x800000
+#define	BCCK_RX1ST_BAIN			0x7f0000
+#define	BCCK_RF_EXTEND			0x20000000
+#define	BCCK_RXAGC_SATLEVEL		0x1f000000
+#define	BCCK_RXAGC_SATCOUNT		0xe0
+#define	BCCKRXRFSETTLE			0x1f
+#define	BCCK_FIXED_RXAGC		0x8000
+#define	BCCK_ANTENNA_POLARITY		0x2000
+#define	BCCK_TXFILTER_TYPE		0x0c00
+#define	BCCK_RXAGC_REPORTTYPE		0x0300
+#define	BCCK_RXDAGC_EN			0x80000000
+#define	BCCK_RXDAGC_PERIOD		0x20000000
+#define	BCCK_RXDAGC_SATLEVEL		0x1f000000
+#define	BCCK_TIMING_RECOVERY		0x800000
+#define	BCCK_TXC0			0x3f0000
+#define	BCCK_TXC1			0x3f000000
+#define	BCCK_TXC2			0x3f
+#define	BCCK_TXC3			0x3f00
+#define	BCCK_TXC4			0x3f0000
+#define	BCCK_TXC5			0x3f000000
+#define	BCCK_TXC6			0x3f
+#define	BCCK_TXC7			0x3f00
+#define	BCCK_DEBUGPORT			0xff0000
+#define	BCCK_DAC_DEBUG			0x0f000000
+#define	BCCK_FALSEALARM_ENABLE		0x8000
+#define	BCCK_FALSEALARM_READ		0x4000
+#define	BCCK_TRSSI			0x7f
+#define	BCCK_RXAGC_REPORT		0xfe
+#define	BCCK_RXREPORT_ANTSEL		0x80000000
+#define	BCCK_RXREPORT_MFOFF		0x40000000
+#define	BCCK_RXREPORT_SQLOSS		0x20000000
+#define	BCCK_RXREPORT_PKTLOSS		0x10000000
+#define	BCCK_RXREPORT_LOCKEDBIT		0x08000000
+#define	BCCK_RXREPORT_RATEERROR		0x04000000
+#define	BCCK_RXREPORT_RXRATE		0x03000000
+#define	BCCK_RXFA_COUNTER_LOWER		0xff
+#define	BCCK_RXFA_COUNTER_UPPER		0xff000000
+#define	BCCK_RXHPAGC_START		0xe000
+#define	BCCK_RXHPAGC_FINAL		0x1c00
+#define	BCCK_RXFALSEALARM_ENABLE	0x8000
+#define	BCCK_FACOUNTER_FREEZE		0x4000
+#define	BCCK_TXPATH_SEL			0x10000000
+#define	BCCK_DEFAULT_RXPATH		0xc000000
+#define	BCCK_OPTION_RXPATH		0x3000000
+
+#define	BNUM_OFSTF			0x3
+#define	BSHIFT_L			0xc0
+#define	BGI_TH				0xc
+#define	BRXPATH_A			0x1
+#define	BRXPATH_B			0x2
+#define	BRXPATH_C			0x4
+#define	BRXPATH_D			0x8
+#define	BTXPATH_A			0x1
+#define	BTXPATH_B			0x2
+#define	BTXPATH_C			0x4
+#define	BTXPATH_D			0x8
+#define	BTRSSI_FREQ			0x200
+#define	BADC_BACKOFF			0x3000
+#define	BDFIR_BACKOFF			0xc000
+#define	BTRSSI_LATCH_PHASE		0x10000
+#define	BRX_LDC_OFFSET			0xff
+#define	BRX_QDC_OFFSET			0xff00
+#define	BRX_DFIR_MODE			0x1800000
+#define	BRX_DCNF_TYPE			0xe000000
+#define	BRXIQIMB_A			0x3ff
+#define	BRXIQIMB_B			0xfc00
+#define	BRXIQIMB_C			0x3f0000
+#define	BRXIQIMB_D			0xffc00000
+#define	BDC_DC_NOTCH			0x60000
+#define	BRXNB_NOTCH			0x1f000000
+#define	BPD_TH				0xf
+#define	BPD_TH_OPT2			0xc000
+#define	BPWED_TH			0x700
+#define	BIFMF_WIN_L			0x800
+#define	BPD_OPTION			0x1000
+#define	BMF_WIN_L			0xe000
+#define	BBW_SEARCH_L			0x30000
+#define	BWIN_ENH_L			0xc0000
+#define	BBW_TH				0x700000
+#define	BED_TH2				0x3800000
+#define	BBW_OPTION			0x4000000
+#define	BRADIO_TH			0x18000000
+#define	BWINDOW_L			0xe0000000
+#define	BSBD_OPTION			0x1
+#define	BFRAME_TH			0x1c
+#define	BFS_OPTION			0x60
+#define	BDC_SLOPE_CHECK			0x80
+#define	BFGUARD_COUNTER_DC_L		0xe00
+#define	BFRAME_WEIGHT_SHORT		0x7000
+#define	BSUB_TUNE			0xe00000
+#define	BFRAME_DC_LENGTH		0xe000000
+#define	BSBD_START_OFFSET		0x30000000
+#define	BFRAME_TH_2			0x7
+#define	BFRAME_GI2_TH			0x38
+#define	BGI2_SYNC_EN			0x40
+#define	BSARCH_SHORT_EARLY		0x300
+#define	BSARCH_SHORT_LATE		0xc00
+#define	BSARCH_GI2_LATE			0x70000
+#define	BCFOANTSUM			0x1
+#define	BCFOACC				0x2
+#define	BCFOSTARTOFFSET			0xc
+#define	BCFOLOOPBACK			0x70
+#define	BCFOSUMWEIGHT			0x80
+#define	BDAGCENABLE			0x10000
+#define	BTXIQIMB_A			0x3ff
+#define	BTXIQIMB_b			0xfc00
+#define	BTXIQIMB_C			0x3f0000
+#define	BTXIQIMB_D			0xffc00000
+#define	BTXIDCOFFSET			0xff
+#define	BTXIQDCOFFSET			0xff00
+#define	BTXDFIRMODE			0x10000
+#define	BTXPESUDO_NOISEON		0x4000000
+#define	BTXPESUDO_NOISE_A		0xff
+#define	BTXPESUDO_NOISE_B		0xff00
+#define	BTXPESUDO_NOISE_C		0xff0000
+#define	BTXPESUDO_NOISE_D		0xff000000
+#define	BCCA_DROPOPTION			0x20000
+#define	BCCA_DROPTHRES			0xfff00000
+#define	BEDCCA_H			0xf
+#define	BEDCCA_L			0xf0
+#define	BLAMBDA_ED			0x300
+#define	BRX_INITIALGAIN			0x7f
+#define	BRX_ANTDIV_EN			0x80
+#define	BRX_AGC_ADDRESS_FOR_LNA		0x7f00
+#define	BRX_HIGHPOWER_FLOW		0x8000
+#define	BRX_AGC_FREEZE_THRES		0xc0000
+#define	BRX_FREEZESTEP_AGC1		0x300000
+#define	BRX_FREEZESTEP_AGC2		0xc00000
+#define	BRX_FREEZESTEP_AGC3		0x3000000
+#define	BRX_FREEZESTEP_AGC0		0xc000000
+#define	BRXRSSI_CMP_EN			0x10000000
+#define	BRXQUICK_AGCEN			0x20000000
+#define	BRXAGC_FREEZE_THRES_MODE	0x40000000
+#define	BRX_OVERFLOW_CHECKTYPE		0x80000000
+#define	BRX_AGCSHIFT			0x7f
+#define	BTRSW_TRI_ONLY			0x80
+#define	BPOWER_THRES			0x300
+#define	BRXAGC_EN			0x1
+#define	BRXAGC_TOGETHER_EN		0x2
+#define	BRXAGC_MIN			0x4
+#define	BRXHP_INI			0x7
+#define	BRXHP_TRLNA			0x70
+#define	BRXHP_RSSI			0x700
+#define	BRXHP_BBP1			0x7000
+#define	BRXHP_BBP2			0x70000
+#define	BRXHP_BBP3			0x700000
+#define	BRSSI_H				0x7f0000
+#define	BRSSI_GEN			0x7f000000
+#define	BRXSETTLE_TRSW			0x7
+#define	BRXSETTLE_LNA			0x38
+#define	BRXSETTLE_RSSI			0x1c0
+#define	BRXSETTLE_BBP			0xe00
+#define	BRXSETTLE_RXHP			0x7000
+#define	BRXSETTLE_ANTSW_RSSI		0x38000
+#define	BRXSETTLE_ANTSW			0xc0000
+#define	BRXPROCESS_TIME_DAGC		0x300000
+#define	BRXSETTLE_HSSI			0x400000
+#define	BRXPROCESS_TIME_BBPPW		0x800000
+#define	BRXANTENNA_POWER_SHIFT		0x3000000
+#define	BRSSI_TABLE_SELECT		0xc000000
+#define	BRXHP_FINAL			0x7000000
+#define	BRXHPSETTLE_BBP			0x7
+#define	BRXHTSETTLE_HSSI		0x8
+#define	BRXHTSETTLE_RXHP		0x70
+#define	BRXHTSETTLE_BBPPW		0x80
+#define	BRXHTSETTLE_IDLE		0x300
+#define	BRXHTSETTLE_RESERVED		0x1c00
+#define	BRXHT_RXHP_EN			0x8000
+#define	BRXAGC_FREEZE_THRES		0x30000
+#define	BRXAGC_TOGETHEREN		0x40000
+#define	BRXHTAGC_MIN			0x80000
+#define	BRXHTAGC_EN			0x100000
+#define	BRXHTDAGC_EN			0x200000
+#define	BRXHT_RXHP_BBP			0x1c00000
+#define	BRXHT_RXHP_FINAL		0xe0000000
+#define	BRXPW_RADIO_TH			0x3
+#define	BRXPW_RADIO_EN			0x4
+#define	BRXMF_HOLD			0x3800
+#define	BRXPD_DELAY_TH1			0x38
+#define	BRXPD_DELAY_TH2			0x1c0
+#define	BRXPD_DC_COUNT_MAX		0x600
+#define	BRXPD_DELAY_TH			0x8000
+#define	BRXPROCESS_DELAY		0xf0000
+#define	BRXSEARCHRANGE_GI2_EARLY	0x700000
+#define	BRXFRAME_FUARD_COUNTER_L	0x3800000
+#define	BRXSGI_GUARD_L			0xc000000
+#define	BRXSGI_SEARCH_L			0x30000000
+#define	BRXSGI_TH			0xc0000000
+#define	BDFSCNT0			0xff
+#define	BDFSCNT1			0xff00
+#define	BDFSFLAG			0xf0000
+#define	BMF_WEIGHT_SUM			0x300000
+#define	BMINIDX_TH			0x7f000000
+#define	BDAFORMAT			0x40000
+#define	BTXCH_EMU_ENABLE		0x01000000
+#define	BTRSW_ISOLATION_A		0x7f
+#define	BTRSW_ISOLATION_B		0x7f00
+#define	BTRSW_ISOLATION_C		0x7f0000
+#define	BTRSW_ISOLATION_D		0x7f000000
+#define	BEXT_LNA_GAIN			0x7c00
+
+#define	BSTBC_EN			0x4
+#define	BANTENNA_MAPPING		0x10
+#define	BNSS				0x20
+#define	BCFO_ANTSUM_ID			0x200
+#define	BPHY_COUNTER_RESET		0x8000000
+#define	BCFO_REPORT_GET			0x4000000
+#define	BOFDM_CONTINUE_TX		0x10000000
+#define	BOFDM_SINGLE_CARRIER		0x20000000
+#define	BOFDM_SINGLE_TONE		0x40000000
+#define	BHT_DETECT			0x100
+#define	BCFOEN				0x10000
+#define	BCFOVALUE			0xfff00000
+#define	BSIGTONE_RE			0x3f
+#define	BSIGTONE_IM			0x7f00
+#define	BCOUNTER_CCA			0xffff
+#define	BCOUNTER_PARITYFAIL		0xffff0000
+#define	BCOUNTER_RATEILLEGAL		0xffff
+#define	BCOUNTER_CRC8FAIL		0xffff0000
+#define	BCOUNTER_MCSNOSUPPORT		0xffff
+#define	BCOUNTER_FASTSYNC		0xffff
+#define	BSHORTCFO			0xfff
+#define	BSHORTCFOT_LENGTH		12
+#define	BSHORTCFOF_LENGTH		11
+#define	BLONGCFO			0x7ff
+#define	BLONGCFOT_LENGTH		11
+#define	BLONGCFOF_LENGTH		11
+#define	BTAILCFO			0x1fff
+#define	BTAILCFOT_LENGTH		13
+#define	BTAILCFOF_LENGTH		12
+#define	BNOISE_EN_PWDB			0xffff
+#define	BCC_POWER_DB			0xffff0000
+#define	BMOISE_PWDB			0xffff
+#define	BPOWERMEAST_LENGTH		10
+#define	BPOWERMEASF_LENGTH		3
+#define	BRX_HT_BW			0x1
+#define	BRXSC				0x6
+#define	BRX_HT				0x8
+#define	BNB_INTF_DET_ON			0x1
+#define	BINTF_WIN_LEN_CFG		0x30
+#define	BNB_INTF_TH_CFG			0x1c0
+#define	BRFGAIN				0x3f
+#define	BTABLESEL			0x40
+#define	BTRSW				0x80
+#define	BRXSNR_A			0xff
+#define	BRXSNR_B			0xff00
+#define	BRXSNR_C			0xff0000
+#define	BRXSNR_D			0xff000000
+#define	BSNR_EVMT_LENGTH		8
+#define	BSNR_EVMF_LENGTH		1
+#define	BCSI1ST				0xff
+#define	BCSI2ND				0xff00
+#define	BRXEVM1ST			0xff0000
+#define	BRXEVM2ND			0xff000000
+#define	BSIGEVM				0xff
+#define	BPWDB				0xff00
+#define	BSGIEN				0x10000
+
+#define	BSFACTOR_QMA1			0xf
+#define	BSFACTOR_QMA2			0xf0
+#define	BSFACTOR_QMA3			0xf00
+#define	BSFACTOR_QMA4			0xf000
+#define	BSFACTOR_QMA5			0xf0000
+#define	BSFACTOR_QMA6			0xf0000
+#define	BSFACTOR_QMA7			0xf00000
+#define	BSFACTOR_QMA8			0xf000000
+#define	BSFACTOR_QMA9			0xf0000000
+#define	BCSI_SCHEME			0x100000
+
+#define	BNOISE_LVL_TOP_SET		0x3
+#define	BCHSMOOTH			0x4
+#define	BCHSMOOTH_CFG1			0x38
+#define	BCHSMOOTH_CFG2			0x1c0
+#define	BCHSMOOTH_CFG3			0xe00
+#define	BCHSMOOTH_CFG4			0x7000
+#define	BMRCMODE			0x800000
+#define	BTHEVMCFG			0x7000000
+
+#define	BLOOP_FIT_TYPE			0x1
+#define	BUPD_CFO			0x40
+#define	BUPD_CFO_OFFDATA		0x80
+#define	BADV_UPD_CFO			0x100
+#define	BADV_TIME_CTRL			0x800
+#define	BUPD_CLKO			0x1000
+#define	BFC				0x6000
+#define	BTRACKING_MODE			0x8000
+#define	BPHCMP_ENABLE			0x10000
+#define	BUPD_CLKO_LTF			0x20000
+#define	BCOM_CH_CFO			0x40000
+#define	BCSI_ESTI_MODE			0x80000
+#define	BADV_UPD_EQZ			0x100000
+#define	BUCHCFG				0x7000000
+#define	BUPDEQZ				0x8000000
+
+#define	BRX_PESUDO_NOISE_ON		0x20000000
+#define	BRX_PESUDO_NOISE_A		0xff
+#define	BRX_PESUDO_NOISE_B		0xff00
+#define	BRX_PESUDO_NOISE_C		0xff0000
+#define	BRX_PESUDO_NOISE_D		0xff000000
+#define	BRX_PESUDO_NOISESTATE_A		0xffff
+#define	BRX_PESUDO_NOISESTATE_B		0xffff0000
+#define	BRX_PESUDO_NOISESTATE_C		0xffff
+#define	BRX_PESUDO_NOISESTATE_D		0xffff0000
+
+#define	BZEBRA1_HSSIENABLE		0x8
+#define	BZEBRA1_TRXCONTROL		0xc00
+#define	BZEBRA1_TRXGAINSETTING		0x07f
+#define	BZEBRA1_RXCOUNTER		0xc00
+#define	BZEBRA1_TXCHANGEPUMP		0x38
+#define	BZEBRA1_RXCHANGEPUMP		0x7
+#define	BZEBRA1_CHANNEL_NUM		0xf80
+#define	BZEBRA1_TXLPFBW			0x400
+#define	BZEBRA1_RXLPFBW			0x600
+
+#define	BRTL8256REG_MODE_CTRL1		0x100
+#define	BRTL8256REG_MODE_CTRL0		0x40
+#define	BRTL8256REG_TXLPFBW		0x18
+#define	BRTL8256REG_RXLPFBW		0x600
+
+#define	BRTL8258_TXLPFBW		0xc
+#define	BRTL8258_RXLPFBW		0xc00
+#define	BRTL8258_RSSILPFBW		0xc0
+
+#define	BBYTE0				0x1
+#define	BBYTE1				0x2
+#define	BBYTE2				0x4
+#define	BBYTE3				0x8
+#define	BWORD0				0x3
+#define	BWORD1				0xc
+#define	BWORD				0xf
+
+#define	MASKBYTE0			0xff
+#define	MASKBYTE1			0xff00
+#define	MASKBYTE2			0xff0000
+#define	MASKBYTE3			0xff000000
+#define	MASKHWORD			0xffff0000
+#define	MASKLWORD			0x0000ffff
+#define	MASKDWORD			0xffffffff
+#define	MASK12BITS			0xfff
+#define	MASKH4BITS			0xf0000000
+#define MASKOFDM_D			0xffc00000
+#define	MASKCCK				0x3f3f3f3f
+
+#define	MASK4BITS			0x0f
+#define	MASK20BITS			0xfffff
+#define RFREG_OFFSET_MASK		0xfffff
+
+#define	BENABLE				0x1
+#define	BDISABLE			0x0
+
+#define	LEFT_ANTENNA			0x0
+#define	RIGHT_ANTENNA			0x1
+
+#define	TCHECK_TXSTATUS			500
+#define	TUPDATE_RXCOUNTER		100
+
+#define	REG_UN_used_register		0x01bf
+
+/* Path_A RFE cotrol pinmux*/
+#define		RA_RFE_PINMUX		0xcb0
+/* Path_B RFE control pinmux*/
+#define		RB_RFE_PINMUX		0xeb0
+
+#define		RA_RFE_INV		0xcb4
+#define		RB_RFE_INV		0xeb4
+
+/* RXIQC */
+/*RxIQ imblance matrix coeff. A & B*/
+#define RA_RXIQC_AB			0xc10
+/*RxIQ imblance matrix coeff. C & D*/
+#define	RA_RXIQC_CD			0xc14
+/* Pah_A TX scaling factor*/
+#define	RA_TXSCALE			0xc1c
+/* Path_B TX scaling factor*/
+#define	RB_TXSCALE			0xe1c
+/*RxIQ imblance matrix coeff. A & B*/
+#define	RB_RXIQC_AB			0xe10
+/*RxIQ imblance matrix coeff. C & D*/
+#define	RB_RXIQC_CD			0xe14
+/*bit mask for IQC matrix element A & C*/
+#define	RXIQC_AC			0x02ff
+ /*bit mask for IQC matrix element A & C*/
+#define	RXIQC_BD			0x02ff0000
+
+/* 2 EFUSE_TEST (For RTL8723 partially) */
+#define EFUSE_SEL(x)			(((x) & 0x3) << 8)
+#define EFUSE_SEL_MASK			0x300
+#define EFUSE_WIFI_SEL_0		0x0
+
+/*REG_MULTI_FUNC_CTRL(For RTL8723 Only)*/
+/* Enable GPIO[9] as WiFi HW PDn source*/
+#define	WL_HWPDN_EN			BIT(0)
+/* WiFi HW PDn polarity control*/
+#define	WL_HWPDN_SL			BIT(1)
+/* WiFi function enable */
+#define	WL_FUNC_EN			BIT(2)
+/* Enable GPIO[9] as WiFi RF HW PDn source */
+#define	WL_HWROF_EN			BIT(3)
+/* Enable GPIO[11] as BT HW PDn source */
+#define	BT_HWPDN_EN			BIT(16)
+/* BT HW PDn polarity control */
+#define	BT_HWPDN_SL			BIT(17)
+/* BT function enable */
+#define	BT_FUNC_EN			BIT(18)
+/* Enable GPIO[11] as BT/GPS RF HW PDn source */
+#define	BT_HWROF_EN			BIT(19)
+/* Enable GPIO[10] as GPS HW PDn source */
+#define	GPS_HWPDN_EN			BIT(20)
+/* GPS HW PDn polarity control */
+#define	GPS_HWPDN_SL			BIT(21)
+/* GPS function enable */
+#define	GPS_FUNC_EN			BIT(22)
+
+#define	BMASKBYTE0			0xff
+#define	BMASKBYTE1			0xff00
+#define	BMASKBYTE2			0xff0000
+#define	BMASKBYTE3			0xff000000
+#define	BMASKHWORD			0xffff0000
+#define	BMASKLWORD			0x0000ffff
+#define	BMASKDWORD			0xffffffff
+#define	BMASK12BITS			0xfff
+#define	BMASKH4BITS			0xf0000000
+#define BMASKOFDM_D			0xffc00000
+#define	BMASKCCK			0x3f3f3f3f
+
+#define BRFREGOFFSETMASK		0xfffff
+
+#define	ODM_REG_CCK_RPT_FORMAT_11AC	0x804
+#define	ODM_REG_BB_RX_PATH_11AC		0x808
+/*PAGE 9*/
+#define	ODM_REG_OFDM_FA_RST_11AC	0x9A4
+/*PAGE A*/
+#define	ODM_REG_CCK_CCA_11AC		0xA0A
+#define	ODM_REG_CCK_FA_RST_11AC		0xA2C
+#define	ODM_REG_CCK_FA_11AC		0xA5C
+/*PAGE C*/
+#define	ODM_REG_IGI_A_11AC		0xC50
+/*PAGE E*/
+#define	ODM_REG_IGI_B_11AC		0xE50
+/*PAGE F*/
+#define	ODM_REG_OFDM_FA_11AC		0xF48
+
+/* 2 MAC REG LIST */
+
+/* DIG Related */
+#define	ODM_BIT_IGI_11AC		0xFFFFFFFF
+#define	ODM_BIT_CCK_RPT_FORMAT_11AC	BIT16
+#define	ODM_BIT_BB_RX_PATH_11AC		0xF
+
+enum AGGRE_SIZE {
+	HT_AGG_SIZE_8K = 0,
+	HT_AGG_SIZE_16K = 1,
+	HT_AGG_SIZE_32K = 2,
+	HT_AGG_SIZE_64K = 3,
+	VHT_AGG_SIZE_128K = 4,
+	VHT_AGG_SIZE_256K = 5,
+	VHT_AGG_SIZE_512K = 6,
+	VHT_AGG_SIZE_1024K = 7,
+};
+
+#define REG_AMPDU_MAX_LENGTH_8812	0x0458
+
+#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/rf.c b/drivers/net/wireless/rtlwifi/rtl8821ae/rf.c
index 87c1c9746c43..2922538160e5 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/rf.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/rf.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -54,17 +50,17 @@ void rtl8821ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw, u8 bandwidth)
 		rtl_set_rfreg(hw, RF90_PATH_B, RF_CHNLBW, BIT(11)|BIT(10), 0);
 		break;
 	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("unknown bandwidth: %#X\n", bandwidth));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "unknown bandwidth: %#X\n", bandwidth);
 		break;
 	}
 }
 
 void rtl8821ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
-				       u8 *ppowerlevel)
+					  u8 *ppowerlevel)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
 	u32 tx_agc[2] = {0, 0}, tmpval;
@@ -77,12 +73,14 @@ void rtl8821ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 	if (rtlefuse->eeprom_regulatory != 0)
 		turbo_scanoff = true;
 
-	if (mac->act_scanning == true) {
+	if (mac->act_scanning) {
 		tx_agc[RF90_PATH_A] = 0x3f3f3f3f;
 		tx_agc[RF90_PATH_B] = 0x3f3f3f3f;
 
 		if (turbo_scanoff) {
-			for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
+			for (idx1 = RF90_PATH_A;
+				idx1 <= RF90_PATH_B;
+				idx1++) {
 				tx_agc[idx1] = ppowerlevel[idx1] |
 				    (ppowerlevel[idx1] << 8) |
 				    (ppowerlevel[idx1] << 16) |
@@ -112,112 +110,113 @@ void rtl8821ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
 	}
 
 	for (idx1 = RF90_PATH_A; idx1 <= RF90_PATH_B; idx1++) {
-		ptr = (u8 *) (&(tx_agc[idx1]));
+		ptr = (u8 *)(&tx_agc[idx1]);
 		for (idx2 = 0; idx2 < 4; idx2++) {
 			if (*ptr > RF6052_MAX_TX_PWR)
 				*ptr = RF6052_MAX_TX_PWR;
 			ptr++;
 		}
 	}
-	rtl8821ae_dm_txpower_track_adjust(hw,1,&direction,&pwrtrac_value);
-	if (direction ==1){
+	rtl8821ae_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
+	if (direction == 1) {
 		tx_agc[0] += pwrtrac_value;
 		tx_agc[1] += pwrtrac_value;
-	} else if (direction == 2){
+	} else if (direction == 2) {
 		tx_agc[0] -= pwrtrac_value;
 		tx_agc[1] -= pwrtrac_value;
 	}
-	tmpval = tx_agc[RF90_PATH_A] ;
+	tmpval = tx_agc[RF90_PATH_A];
 	rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKDWORD, tmpval);
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-		("CCK PWR 1~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
-		 RTXAGC_A_CCK11_CCK1));
+		"CCK PWR 1~11M (rf-A) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_A_CCK11_CCK1);
 
-	tmpval = tx_agc[RF90_PATH_B] ;
+	tmpval = tx_agc[RF90_PATH_B];
 	rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKDWORD, tmpval);
 
 	RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-		("CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
-		 RTXAGC_B_CCK11_CCK1));
+		"CCK PWR 11M (rf-B) = 0x%x (reg 0x%x)\n", tmpval,
+		 RTXAGC_B_CCK11_CCK1);
 }
 
 static void rtl8821ae_phy_get_power_base(struct ieee80211_hw *hw,
-				      u8 *ppowerlevel_ofdm, u8 *ppowerlevel_bw20, u8 *ppowerlevel_bw40, u8 channel,
-				      u32 *ofdmbase, u32 *mcsbase)
+					 u8 *ppowerlevel_ofdm,
+					 u8 *ppowerlevel_bw20,
+					 u8 *ppowerlevel_bw40, u8 channel,
+					 u32 *ofdmbase, u32 *mcsbase)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 powerBase0, powerBase1;
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u32 powerbase0, powerbase1;
 	u8 i, powerlevel[2];
 
 	for (i = 0; i < 2; i++) {
-		powerBase0 = ppowerlevel_ofdm[i];
+		powerbase0 = ppowerlevel_ofdm[i];
 
-		powerBase0 = (powerBase0 << 24) | (powerBase0 << 16) |
-		    (powerBase0 << 8) | powerBase0;
-		*(ofdmbase + i) = powerBase0;
+		powerbase0 = (powerbase0 << 24) | (powerbase0 << 16) |
+		    (powerbase0 << 8) | powerbase0;
+		*(ofdmbase + i) = powerbase0;
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-			(" [OFDM power base index rf(%c) = 0x%x]\n",
-			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i)));
+			" [OFDM power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(ofdmbase + i));
 	}
 
 	for (i = 0; i < 2; i++) {
-		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20) {
+		if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20)
 			powerlevel[i] = ppowerlevel_bw20[i];
-		}else{
+		else
 			powerlevel[i] = ppowerlevel_bw40[i];
-		}
-		powerBase1 = powerlevel[i];
-		powerBase1 = (powerBase1 << 24) |
-		    (powerBase1 << 16) | (powerBase1 << 8) | powerBase1;
 
-		*(mcsbase + i) = powerBase1;
+		powerbase1 = powerlevel[i];
+		powerbase1 = (powerbase1 << 24) |
+		    (powerbase1 << 16) | (powerbase1 << 8) | powerbase1;
+
+		*(mcsbase + i) = powerbase1;
 
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-			(" [MCS power base index rf(%c) = 0x%x]\n",
-			 ((i == 0) ? 'A' : 'B'), *(mcsbase + i)));
+			" [MCS power base index rf(%c) = 0x%x]\n",
+			 ((i == 0) ? 'A' : 'B'), *(mcsbase + i));
 	}
 }
 
-static void _rtl8821ae_get_txpower_writeval_by_regulatory(struct ieee80211_hw *hw,
-						       u8 channel, u8 index,
-						       u32 *powerBase0,
-						       u32 *powerBase1,
-						       u32 *p_outwriteval)
+static void get_txpower_writeval_by_regulatory(struct ieee80211_hw *hw,
+					       u8 channel, u8 index,
+					       u32 *powerbase0,
+					       u32 *powerbase1,
+					       u32 *p_outwriteval)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 i, chnlgroup = 0, pwr_diff_limit[4],pwr_diff = 0,customer_pwr_diff;
-	u32 writeVal, customer_limit, rf;
+	u8 i, chnlgroup = 0, pwr_diff_limit[4], pwr_diff = 0, customer_pwr_diff;
+	u32 writeval, customer_limit, rf;
 
 	for (rf = 0; rf < 2; rf++) {
 		switch (rtlefuse->eeprom_regulatory) {
 		case 0:
 			chnlgroup = 0;
 
-			writeVal =
+			writeval =
 			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup][index +
-									 (rf ? 8 : 0)]
-			    + ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+							(rf ? 8 : 0)]
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				("RTK better performance, "
-				 "writeVal(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeVal));
+				"RTK better performance, writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 1:
-			if (rtlphy->pwrgroup_cnt == 1)
+			if (rtlphy->pwrgroup_cnt == 1) {
 				chnlgroup = 0;
-			else {
-				if(channel<3)
+			} else {
+				if (channel < 3)
 					chnlgroup = 0;
-				else if (channel <6)
+				else if (channel < 6)
 					chnlgroup = 1;
-				else if (channel <9)
+				else if (channel < 9)
 					chnlgroup = 2;
-				else if (channel <12)
+				else if (channel < 12)
 					chnlgroup = 3;
 				else if (channel < 14)
 					chnlgroup = 4;
@@ -225,55 +224,54 @@ static void _rtl8821ae_get_txpower_writeval_by_regulatory(struct ieee80211_hw *h
 					chnlgroup = 5;
 			}
 
-			writeVal =
+			writeval =
 			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
 			    [index + (rf ? 8 : 0)] + ((index < 2) ?
-						      powerBase0[rf] :
-						      powerBase1[rf]);
+						      powerbase0[rf] :
+						      powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				("Realtek regulatory, 20MHz, "
-				 "writeVal(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeVal));
+				"Realtek regulatory, 20MHz, writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 
 			break;
 		case 2:
-			writeVal =
-			    ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			writeval =
+			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				("Better regulatory, "
-				 "writeVal(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeVal));
+				"Better regulatory, writeval(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		case 3:
 			chnlgroup = 0;
 
 			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-					("customer's limit, 40MHz "
-					 "rf(%c) = 0x%x\n",
+					"customer's limit, 40MHz rf(%c) = 0x%x\n",
 					 ((rf == 0) ? 'A' : 'B'),
 					 rtlefuse->pwrgroup_ht40[rf][channel -
-								     1]));
+								     1]);
 			} else {
 				RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-					("customer's limit, 20MHz "
-					 "rf(%c) = 0x%x\n",
+					"customer's limit, 20MHz rf(%c) = 0x%x\n",
 					 ((rf == 0) ? 'A' : 'B'),
 					 rtlefuse->pwrgroup_ht20[rf][channel -
-								     1]));
+								     1]);
 			}
 
 			if (index < 2)
 				pwr_diff = rtlefuse->txpwr_legacyhtdiff[rf][channel-1];
 			else if (rtlphy->current_chan_bw ==  HT_CHANNEL_WIDTH_20)
-				pwr_diff = rtlefuse->txpwr_ht20diff[rf][channel-1];
+				pwr_diff =
+				  rtlefuse->txpwr_ht20diff[rf][channel-1];
 
 			if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
-				customer_pwr_diff = rtlefuse->pwrgroup_ht40[rf][channel-1];
+				customer_pwr_diff =
+				  rtlefuse->pwrgroup_ht40[rf][channel-1];
 			else
-				customer_pwr_diff = rtlefuse->pwrgroup_ht20[rf][channel-1];
+				customer_pwr_diff =
+				  rtlefuse->pwrgroup_ht20[rf][channel-1];
 
 			if (pwr_diff > customer_pwr_diff)
 				pwr_diff = 0;
@@ -282,12 +280,12 @@ static void _rtl8821ae_get_txpower_writeval_by_regulatory(struct ieee80211_hw *h
 
 			for (i = 0; i < 4; i++) {
 				pwr_diff_limit[i] =
-				    (u8) ((rtlphy->mcs_txpwrlevel_origoffset
-					   [chnlgroup][index + (rf ? 8 : 0)] & (0x7f <<
-									(i * 8))) >> (i * 8));
+				    (u8)((rtlphy->mcs_txpwrlevel_origoffset
+				    [chnlgroup][index + (rf ? 8 : 0)] &
+				    (0x7f << (i * 8))) >> (i * 8));
 
-					if(pwr_diff_limit[i] > pwr_diff)
-						pwr_diff_limit[i] = pwr_diff;
+				if (pwr_diff_limit[i] > pwr_diff)
+					pwr_diff_limit[i] = pwr_diff;
 			}
 
 			customer_limit = (pwr_diff_limit[3] << 24) |
@@ -295,41 +293,40 @@ static void _rtl8821ae_get_txpower_writeval_by_regulatory(struct ieee80211_hw *h
 			    (pwr_diff_limit[1] << 8) | (pwr_diff_limit[0]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				("Customer's limit rf(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), customer_limit));
+				"Customer's limit rf(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), customer_limit);
 
-			writeVal = customer_limit +
-			    ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			writeval = customer_limit +
+			    ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				("Customer, writeVal rf(%c)= 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeVal));
+				"Customer, writeval rf(%c)= 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		default:
 			chnlgroup = 0;
-			writeVal =
+			writeval =
 			    rtlphy->mcs_txpwrlevel_origoffset[chnlgroup]
 			    [index + (rf ? 8 : 0)]
-			    + ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
+			    + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
 
 			RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-				("RTK better performance, writeVal "
-				 "rf(%c) = 0x%x\n",
-				 ((rf == 0) ? 'A' : 'B'), writeVal));
+				"RTK better performance, writeval rf(%c) = 0x%x\n",
+				 ((rf == 0) ? 'A' : 'B'), writeval);
 			break;
 		}
 
 		if (rtlpriv->dm.dynamic_txhighpower_lvl == TXHIGHPWRLEVEL_BT1)
-			writeVal = writeVal - 0x06060606;
+			writeval = writeval - 0x06060606;
 		else if (rtlpriv->dm.dynamic_txhighpower_lvl ==
 			 TXHIGHPWRLEVEL_BT2)
-			writeVal = writeVal - 0x0c0c0c0c;
-		*(p_outwriteval + rf) = writeVal;
+			writeval = writeval - 0x0c0c0c0c;
+		*(p_outwriteval + rf) = writeval;
 	}
 }
 
 static void _rtl8821ae_write_ofdm_power_reg(struct ieee80211_hw *hw,
-					 u8 index, u32 *pValue)
+					    u8 index, u32 *pvalue)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	u16 regoffset_a[6] = {
@@ -343,66 +340,72 @@ static void _rtl8821ae_write_ofdm_power_reg(struct ieee80211_hw *hw,
 		RTXAGC_B_MCS11_MCS08, RTXAGC_B_MCS15_MCS12
 	};
 	u8 i, rf, pwr_val[4];
-	u32 writeVal;
+	u32 writeval;
 	u16 regoffset;
 
 	for (rf = 0; rf < 2; rf++) {
-		writeVal = pValue[rf];
+		writeval = pvalue[rf];
 		for (i = 0; i < 4; i++) {
-			pwr_val[i] = (u8) ((writeVal & (0x7f <<
+			pwr_val[i] = (u8)((writeval & (0x7f <<
 							(i * 8))) >> (i * 8));
 
 			if (pwr_val[i] > RF6052_MAX_TX_PWR)
 				pwr_val[i] = RF6052_MAX_TX_PWR;
 		}
-		writeVal = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
+		writeval = (pwr_val[3] << 24) | (pwr_val[2] << 16) |
 		    (pwr_val[1] << 8) | pwr_val[0];
 
 		if (rf == 0)
 			regoffset = regoffset_a[index];
 		else
 			regoffset = regoffset_b[index];
-		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeVal);
+		rtl_set_bbreg(hw, regoffset, MASKDWORD, writeval);
 
 		RTPRINT(rtlpriv, FPHY, PHY_TXPWR,
-			("Set 0x%x = %08x\n", regoffset, writeVal));
+			"Set 0x%x = %08x\n", regoffset, writeval);
 	}
 }
 
 void rtl8821ae_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
-					      u8 *ppowerlevel_ofdm, u8 *ppowerlevel_bw20, u8 *ppowerlevel_bw40, u8 channel)
+					   u8 *ppowerlevel_ofdm,
+					   u8 *ppowerlevel_bw20,
+					   u8 *ppowerlevel_bw40,
+					   u8 channel)
 {
-	u32 writeVal[2], powerBase0[2], powerBase1[2];
+	u32 writeval[2], powerbase0[2], powerbase1[2];
 	u8 index;
 	u8 direction;
 	u32 pwrtrac_value;
 
-	rtl8821ae_phy_get_power_base(hw, ppowerlevel_ofdm, ppowerlevel_bw20, ppowerlevel_bw40,
-				  channel, &powerBase0[0], &powerBase1[0]);
+	rtl8821ae_phy_get_power_base(hw, ppowerlevel_ofdm,
+				     ppowerlevel_bw20,
+				     ppowerlevel_bw40,
+				     channel,
+				     &powerbase0[0],
+				     &powerbase1[0]);
 
-	rtl8821ae_dm_txpower_track_adjust(hw,1,&direction,&pwrtrac_value);
+	rtl8821ae_dm_txpower_track_adjust(hw, 1, &direction, &pwrtrac_value);
 
 	for (index = 0; index < 6; index++) {
-		_rtl8821ae_get_txpower_writeval_by_regulatory(hw,
-							   channel, index,
-							   &powerBase0[0],
-							   &powerBase1[0],
-							   &writeVal[0]);
-		if (direction ==1){
-			writeVal[0] += pwrtrac_value;
-			writeVal[1] += pwrtrac_value;
-		} else if (direction == 2){
-			writeVal[0] -= pwrtrac_value;
-			writeVal[1] -= pwrtrac_value;
+		get_txpower_writeval_by_regulatory(hw, channel, index,
+						   &powerbase0[0],
+						   &powerbase1[0],
+						   &writeval[0]);
+		if (direction == 1) {
+			writeval[0] += pwrtrac_value;
+			writeval[1] += pwrtrac_value;
+		} else if (direction == 2) {
+			writeval[0] -= pwrtrac_value;
+			writeval[1] -= pwrtrac_value;
 		}
-		_rtl8821ae_write_ofdm_power_reg(hw, index, &writeVal[0]);
+		_rtl8821ae_write_ofdm_power_reg(hw, index, &writeval[0]);
 	}
 }
 
 bool rtl8821ae_phy_rf6052_config(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 
 	if (rtlphy->rf_type == RF_1T1R)
 		rtlphy->num_total_rfpath = 1;
@@ -410,55 +413,53 @@ bool rtl8821ae_phy_rf6052_config(struct ieee80211_hw *hw)
 		rtlphy->num_total_rfpath = 2;
 
 	return _rtl8821ae_phy_rf6052_config_parafile(hw);
-
 }
 
 static bool _rtl8821ae_phy_rf6052_config_parafile(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
 	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	//u32 u4_regvalue = 0;
 	u8 rfpath;
 	bool rtstatus = true;
-	//struct bb_reg_def *pphyreg;
 
 	for (rfpath = 0; rfpath < rtlphy->num_total_rfpath; rfpath++) {
 		switch (rfpath) {
 		case RF90_PATH_A: {
 			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-				rtstatus = rtl8812ae_phy_config_rf_with_headerfile(hw,
-									(enum radio_path)rfpath);
+				rtstatus =
+				  rtl8812ae_phy_config_rf_with_headerfile(hw,
+							(enum radio_path)rfpath);
 			else
-				rtstatus = rtl8821ae_phy_config_rf_with_headerfile(hw,
-									(enum radio_path)rfpath);
+				rtstatus =
+				  rtl8821ae_phy_config_rf_with_headerfile(hw,
+							(enum radio_path)rfpath);
 			break;
 			}
-		case RF90_PATH_B: {
+		case RF90_PATH_B:
 			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-				rtstatus = rtl8812ae_phy_config_rf_with_headerfile(hw,
-									(enum radio_path)rfpath);
+				rtstatus =
+				  rtl8812ae_phy_config_rf_with_headerfile(hw,
+							(enum radio_path)rfpath);
 			else
-				rtstatus = rtl8821ae_phy_config_rf_with_headerfile(hw,
-									(enum radio_path)rfpath);
+				rtstatus =
+				  rtl8821ae_phy_config_rf_with_headerfile(hw,
+							(enum radio_path)rfpath);
 			break;
-			}
 		case RF90_PATH_C:
 			break;
 		case RF90_PATH_D:
 			break;
 		}
 
-		if (rtstatus != true) {
-			RT_TRACE(COMP_INIT, DBG_TRACE,
-				 ("Radio[%d] Fail!!", rfpath));
+		if (!rtstatus) {
+			RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE,
+				 "Radio[%d] Fail!!", rfpath);
 			return false;
 		}
-
 	}
 
 	/*put arrays in dm.c*/
-	/*_rtl8821ae_config_rf_txpwr_track_headerfile(hw);*/
-	RT_TRACE(COMP_INIT, DBG_TRACE, ("\n"));
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_TRACE, "\n");
 	return rtstatus;
 }
diff --git a/drivers/staging/rtl8192ee/rc.h b/drivers/net/wireless/rtlwifi/rtl8821ae/rf.h
index 928f570b4b8e..d9582ee1c335 100644
--- a/drivers/staging/rtl8192ee/rc.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/rf.h
@@ -23,25 +23,21 @@
  *
  *****************************************************************************/
 
-#ifndef __RTL_RC_H__
-#define __RTL_RC_H__
+#ifndef __RTL8821AE_RF_H__
+#define __RTL8821AE_RF_H__
 
-#define B_MODE_MAX_RIX 3
-#define G_MODE_MAX_RIX 11
-#define A_MODE_MAX_RIX 7
+#define RF6052_MAX_TX_PWR		0x3F
+#define RF6052_MAX_REG			0x3F
 
-/* in mac80211 mcs0-mcs15 is idx0-idx15*/
-#define N_MODE_MCS7_RIX 7
-#define N_MODE_MCS15_RIX 15
+void rtl8821ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
+					u8 bandwidth);
+void rtl8821ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
+					  u8 *ppowerlevel);
+void rtl8821ae_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
+					   u8 *ppowerlevel_ofdm,
+					   u8 *ppowerlevel_bw20,
+					   u8 *ppowerlevel_bw40,
+					   u8 channel);
+bool rtl8821ae_phy_rf6052_config(struct ieee80211_hw *hw);
 
-#define AC_MODE_MCS7_RIX 7
-#define AC_MODE_MCS8_RIX 8
-#define AC_MODE_MCS9_RIX 9
-
-struct rtl_rate_priv {
-	u8 ht_cap;
-};
-
-int rtl92e_rate_control_register(void);
-void rtl92e_rate_control_unregister(void);
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/sw.c b/drivers/net/wireless/rtlwifi/rtl8821ae/sw.c
index 115002f98e2d..fc92dd6a0d07 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/sw.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/sw.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -27,9 +23,6 @@
  *
  *****************************************************************************/
 
-#include <linux/vmalloc.h>
-#include <linux/module.h>
-
 #include "../wifi.h"
 #include "../core.h"
 #include "../pci.h"
@@ -38,13 +31,16 @@
 #include "phy.h"
 #include "dm.h"
 #include "hw.h"
+#include "fw.h"
 #include "sw.h"
 #include "trx.h"
 #include "led.h"
 #include "table.h"
-#include "hal_btc.h"
 #include "../btcoexist/rtl_btc.h"
 
+#include <linux/vmalloc.h>
+#include <linux/module.h>
+
 static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
@@ -52,15 +48,15 @@ static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 	/*close ASPM for AMD defaultly */
 	rtlpci->const_amdpci_aspm = 0;
 
-	/*
+	/**
 	 * ASPM PS mode.
 	 * 0 - Disable ASPM,
 	 * 1 - Enable ASPM without Clock Req,
 	 * 2 - Enable ASPM with Clock Req,
-	 * 3 - Always Enable ASPM with Clock Req,
+	 * 3 - Alwyas Enable ASPM with Clock Req,
 	 * 4 - Always Enable ASPM without Clock Req.
-	 * set default to RTL8192CE:3 RTL8192E:2
-	 * */
+	 * set defult to RTL8192CE:3 RTL8192E:2
+	 */
 	rtlpci->const_pci_aspm = 3;
 
 	/*Setting for PCI-E device */
@@ -69,7 +65,7 @@ static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 	/*Setting for PCI-E bridge */
 	rtlpci->const_hostpci_aspm_setting = 0x02;
 
-	/*
+	/**
 	 * In Hw/Sw Radio Off situation.
 	 * 0 - Default,
 	 * 1 - From ASPM setting without low Mac Pwr,
@@ -79,7 +75,7 @@ static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 	 */
 	rtlpci->const_hwsw_rfoff_d3 = 0;
 
-	/*
+	/**
 	 * This setting works for those device with
 	 * backdoor ASPM setting such as EPHY setting.
 	 * 0 - Not support ASPM,
@@ -89,171 +85,195 @@ static void rtl8821ae_init_aspm_vars(struct ieee80211_hw *hw)
 	rtlpci->const_support_pciaspm = 1;
 }
 
+static void load_wowlan_fw(struct rtl_priv *rtlpriv)
+{
+	/* callback routine to load wowlan firmware after main fw has
+	 * been loaded
+	 */
+	const struct firmware *wowlan_firmware;
+	char *fw_name = NULL;
+	int err;
+
+	/* for wowlan firmware buf */
+	rtlpriv->rtlhal.wowlan_firmware = vzalloc(0x8000);
+	if (!rtlpriv->rtlhal.wowlan_firmware) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Can't alloc buffer for wowlan fw.\n");
+		return;
+	}
+
+	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8821AE)
+		fw_name = "rtlwifi/rtl8821aefw_wowlan.bin";
+	else
+		fw_name = "rtlwifi/rtl8812aefw_wowlan.bin";
+	err = request_firmware(&wowlan_firmware, fw_name, rtlpriv->io.dev);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Failed to request wowlan firmware!\n");
+		goto error;
+	}
+
+	if (wowlan_firmware->size > 0x8000) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Wowlan Firmware is too big!\n");
+		goto error;
+	}
+
+	memcpy(rtlpriv->rtlhal.wowlan_firmware, wowlan_firmware->data,
+	       wowlan_firmware->size);
+	rtlpriv->rtlhal.wowlan_fwsize = wowlan_firmware->size;
+	release_firmware(wowlan_firmware);
+
+	RT_TRACE(rtlpriv, COMP_INIT, DBG_LOUD, "WOWLAN FirmwareDownload OK\n");
+	return;
+error:
+	release_firmware(wowlan_firmware);
+	vfree(rtlpriv->rtlhal.wowlan_firmware);
+}
+
 /*InitializeVariables8812E*/
 int rtl8821ae_init_sw_vars(struct ieee80211_hw *hw)
 {
 	int err = 0;
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	const struct firmware *firmware;
 	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	char *fw_name = NULL;
+	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
 
 	rtl8821ae_bt_reg_init(hw);
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
 	rtlpriv->btcoexist.btc_ops = rtl_btc_get_ops_pointer();
 
-	rtlpriv->dm.b_dm_initialgain_enable = 1;
+	rtlpriv->dm.dm_initialgain_enable = 1;
 	rtlpriv->dm.dm_flag = 0;
-	rtlpriv->dm.b_disable_framebursting = 0;;
+	rtlpriv->dm.disable_framebursting = 0;
 	rtlpriv->dm.thermalvalue = 0;
 	rtlpci->transmit_config = CFENDFORM | BIT(15) | BIT(24) | BIT(25);
 
 	mac->ht_enable = true;
+	mac->ht_cur_stbc = 0;
+	mac->ht_stbc_cap = 0;
+	mac->vht_cur_ldpc = 0;
+	mac->vht_ldpc_cap = 0;
+	mac->vht_cur_stbc = 0;
+	mac->vht_stbc_cap = 0;
 
 	rtlpriv->rtlhal.current_bandtype = BAND_ON_2_4G;
 	/*following 2 is for register 5G band, refer to _rtl_init_mac80211()*/
 	rtlpriv->rtlhal.bandset = BAND_ON_BOTH;
 	rtlpriv->rtlhal.macphymode = SINGLEMAC_SINGLEPHY;
 
-	rtlpci->receive_config = (RCR_APPFCS			|
-							  RCR_APP_MIC 			|
-							  RCR_APP_ICV 			|
-							  RCR_APP_PHYST_RXFF	|
-							  RCR_NONQOS_VHT		|
-							  RCR_HTC_LOC_CTRL		|
-							  RCR_AMF 				|
-							  RCR_ACF 				|
-							  RCR_ADF 				|	/*This bit controls the PS-Poll packet filter.*/
-							  RCR_AICV				|
-							  RCR_ACRC32			|
-							  RCR_AB				|
-							  RCR_AM				|
-							  RCR_APM 				|
-							  0);
-
+	rtlpci->receive_config = (RCR_APPFCS	|
+				RCR_APP_MIC		|
+				RCR_APP_ICV		|
+				RCR_APP_PHYST_RXFF	|
+				RCR_NONQOS_VHT		|
+				RCR_HTC_LOC_CTRL	|
+				RCR_AMF			|
+				RCR_ACF			|
+			/*This bit controls the PS-Poll packet filter.*/
+				RCR_ADF			|
+				RCR_AICV		|
+				RCR_ACRC32		|
+				RCR_AB			|
+				RCR_AM			|
+				RCR_APM			|
+				0);
 
 	rtlpci->irq_mask[0] =
-	     (u32) (IMR_PSTIMEOUT			|
-				IMR_GTINT3 				|
-				/*IMR_TBDER				|
-				IMR_TBDOK				|
-				IMR_BCNDMAINT0 			|*/
+	     (u32)(IMR_PSTIMEOUT			|
+				IMR_GTINT3		|
 				IMR_HSISR_IND_ON_INT	|
-				IMR_C2HCMD 				|
-				IMR_HIGHDOK				|
-				IMR_MGNTDOK				|
-				IMR_BKDOK				|
-				IMR_BEDOK				|
-				IMR_VIDOK				|
-				IMR_VODOK				|
-				IMR_RDU					|
-				IMR_ROK					|
+				IMR_C2HCMD		|
+				IMR_HIGHDOK		|
+				IMR_MGNTDOK		|
+				IMR_BKDOK		|
+				IMR_BEDOK		|
+				IMR_VIDOK		|
+				IMR_VODOK		|
+				IMR_RDU			|
+				IMR_ROK			|
 				0);
 
 	rtlpci->irq_mask[1]	=
-		 (u32)(	IMR_RXFOVW |
-		 		IMR_TXFOVW |
+		 (u32)(IMR_RXFOVW |
+				IMR_TXFOVW |
 				0);
-
+	rtlpci->sys_irq_mask = (u32)(HSIMR_PDN_INT_EN	|
+				      HSIMR_RON_INT_EN	|
+				      0);
+	/* for WOWLAN */
+	rtlpriv->psc.wo_wlan_mode = WAKE_ON_MAGIC_PACKET |
+				    WAKE_ON_PATTERN_MATCH;
+
+	/* for debug level */
+	rtlpriv->dbg.global_debuglevel = rtlpriv->cfg->mod_params->debug;
 	/* for LPS & IPS */
-	rtlpriv->psc.b_inactiveps = rtlpriv->cfg->mod_params->b_inactiveps;
-	rtlpriv->psc.b_swctrl_lps = rtlpriv->cfg->mod_params->b_swctrl_lps;
-	rtlpriv->psc.b_fwctrl_lps = rtlpriv->cfg->mod_params->b_fwctrl_lps;
-	rtlpriv->psc.b_reg_fwctrl_lps = 3;
+	rtlpriv->psc.inactiveps = rtlpriv->cfg->mod_params->inactiveps;
+	rtlpriv->psc.swctrl_lps = rtlpriv->cfg->mod_params->swctrl_lps;
+	rtlpriv->psc.fwctrl_lps = rtlpriv->cfg->mod_params->fwctrl_lps;
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+	if (rtlpriv->cfg->mod_params->disable_watchdog)
+		pr_info("watchdog disabled\n");
+	rtlpriv->psc.reg_fwctrl_lps = 3;
 	rtlpriv->psc.reg_max_lps_awakeintvl = 5;
+	rtlpci->msi_support = rtlpriv->cfg->mod_params->msi_support;
+
 	/* for ASPM, you can close aspm through
-	 * set const_support_pciaspm = 0 */
+	 * set const_support_pciaspm = 0
+	 */
 	rtl8821ae_init_aspm_vars(hw);
 
-	if (rtlpriv->psc.b_reg_fwctrl_lps == 1)
+	if (rtlpriv->psc.reg_fwctrl_lps == 1)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_MIN_MODE;
-	else if (rtlpriv->psc.b_reg_fwctrl_lps == 2)
+	else if (rtlpriv->psc.reg_fwctrl_lps == 2)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_MAX_MODE;
-	else if (rtlpriv->psc.b_reg_fwctrl_lps == 3)
+	else if (rtlpriv->psc.reg_fwctrl_lps == 3)
 		rtlpriv->psc.fwctrl_psmode = FW_PS_DTIM_MODE;
 
+	rtlpriv->rtl_fw_second_cb = load_wowlan_fw;
 	/* for firmware buf */
-	rtlpriv->rtlhal.pfirmware = (u8 *) vmalloc(0x8000);
+	rtlpriv->rtlhal.pfirmware = vzalloc(0x8000);
 	if (!rtlpriv->rtlhal.pfirmware) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Can't alloc buffer for fw.\n"));
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Can't alloc buffer for fw.\n");
 		return 1;
 	}
 
-	fw_name = "rtlwifi/rtl8821aefw.bin";
-	err = request_firmware(&firmware, fw_name, rtlpriv->io.dev);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Failed to request firmware!\n"));
-		return 1;
-	}
+	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
+		rtlpriv->cfg->fw_name = "rtlwifi/rtl8812aefw.bin";
+	else
+		rtlpriv->cfg->fw_name = "rtlwifi/rtl8821aefw.bin";
 
-	if (firmware->size > 0x8000) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Firmware is too big!\n"));
-		release_firmware(firmware);
+	rtlpriv->max_fw_size = 0x8000;
+	pr_info("Using firmware %s\n", rtlpriv->cfg->fw_name);
+	err = request_firmware_nowait(THIS_MODULE, 1, rtlpriv->cfg->fw_name,
+				      rtlpriv->io.dev, GFP_KERNEL, hw,
+				      rtl_fw_cb);
+	if (err) {
+		RT_TRACE(rtlpriv, COMP_ERR, DBG_EMERG,
+			 "Failed to request firmware!\n");
 		return 1;
 	}
-
-	memcpy(rtlpriv->rtlhal.pfirmware, firmware->data, firmware->size);
-	rtlpriv->rtlhal.fwsize = firmware->size;
-	release_firmware(firmware);
-
-	if (rtlpriv->cfg->ops->get_btc_status()){
-		rtlpriv->btcoexist.btc_ops->btc_init_variables(rtlpriv);
-		rtlpriv->btcoexist.btc_ops->btc_init_hal_vars(rtlpriv);
-	}
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, (" FirmwareDownload OK\n"));
-	return err;
+	return 0;
 }
 
 void rtl8821ae_deinit_sw_vars(struct ieee80211_hw *hw)
 {
 	struct rtl_priv *rtlpriv = rtl_priv(hw);
 
-	//printk("=========>rtl8821ae_deinit_sw_vars().\n");
-	if (rtlpriv->cfg->ops->get_btc_status()){
-		//printk("=========>rtl8821ae_deinit_sw_vars().get_btc_status\n");
-		rtlpriv->btcoexist.btc_ops->btc_halt_notify();
-	}
 	if (rtlpriv->rtlhal.pfirmware) {
-		//printk("=========>rtl8821ae_deinit_sw_vars().rtlpriv->rtlhal.pfirmware\n");
 		vfree(rtlpriv->rtlhal.pfirmware);
 		rtlpriv->rtlhal.pfirmware = NULL;
 	}
-	//printk("<=========rtl8821ae_deinit_sw_vars().\n");
-}
-
-static u32 rtl8812ae_rx_command_packet_handler(
-	struct ieee80211_hw *hw,
-	const struct rtl_stats *status,
-	struct sk_buff *skb
-	)
-{
-	u32 result = 0;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	switch (status->packet_report_type) {
-		case NORMAL_RX:
-			result = 0;
-			break;
-		case C2H_PACKET:
-			rtl8812ae_c2h_packet_handler(hw, skb->data, (u8) skb->len);
-			result = 1;
-			RT_TRACE(COMP_RECV, DBG_LOUD,
-				("===>rtl8821ae_rx_command_packet_handler(): (u8) skb->len=%d\n\n", skb->len));
-			break;
-		default:
-			RT_TRACE(COMP_RECV, DBG_LOUD,
-				("===>rtl8821ae_rx_command_packet_handler(): No this packet type!!\n"));
-			break;
+#if (USE_SPECIFIC_FW_TO_SUPPORT_WOWLAN == 1)
+	if (rtlpriv->rtlhal.wowlan_firmware) {
+		vfree(rtlpriv->rtlhal.wowlan_firmware);
+		rtlpriv->rtlhal.wowlan_firmware = NULL;
 	}
-
-	return result;
+#endif
 }
 
-
 /* get bt coexist status */
 bool rtl8821ae_get_btc_status(void)
 {
@@ -298,24 +318,24 @@ static struct rtl_hal_ops rtl8821ae_hal_ops = {
 	.enable_hw_sec = rtl8821ae_enable_hw_security_config,
 	.set_key = rtl8821ae_set_key,
 	.init_sw_leds = rtl8821ae_init_sw_leds,
-	.allow_all_destaddr = rtl8821ae_allow_all_destaddr,
 	.get_bbreg = rtl8821ae_phy_query_bb_reg,
 	.set_bbreg = rtl8821ae_phy_set_bb_reg,
 	.get_rfreg = rtl8821ae_phy_query_rf_reg,
 	.set_rfreg = rtl8821ae_phy_set_rf_reg,
-	.c2h_command_handle = rtl_8821ae_c2h_command_handle,
-	.bt_wifi_media_status_notify = rtl_8821ae_bt_wifi_media_status_notify,
-	.bt_turn_off_bt_coexist_before_enter_lps = rtl8821ae_dm_bt_turn_off_bt_coexist_before_enter_lps,
 	.fill_h2c_cmd = rtl8821ae_fill_h2c_cmd,
 	.get_btc_status = rtl8821ae_get_btc_status,
-	.rx_command_packet_handler = rtl8812ae_rx_command_packet_handler,
+	.rx_command_packet = rtl8821ae_rx_command_packet,
+	.add_wowlan_pattern = rtl8821ae_add_wowlan_pattern,
 };
 
 static struct rtl_mod_params rtl8821ae_mod_params = {
 	.sw_crypto = false,
-	.b_inactiveps = false,//true,
-	.b_swctrl_lps = false,
-	.b_fwctrl_lps = false, //true,
+	.inactiveps = true,
+	.swctrl_lps = false,
+	.fwctrl_lps = true,
+	.msi_support = true,
+	.debug = DBG_EMERG,
+	.disable_watchdog = 0,
 };
 
 static struct rtl_hal_cfg rtl8821ae_hal_cfg = {
@@ -336,8 +356,7 @@ static struct rtl_hal_cfg rtl8821ae_hal_cfg = {
 	.maps[MAC_HIMR] = REG_HIMR,
 	.maps[MAC_HIMRE] = REG_HIMRE,
 
-
-       .maps[EFUSE_ACCESS] = REG_EFUSE_ACCESS,
+	.maps[EFUSE_ACCESS] = REG_EFUSE_ACCESS,
 
 	.maps[EFUSE_TEST] = REG_EFUSE_TEST,
 	.maps[EFUSE_CTRL] = REG_EFUSE_CTRL,
@@ -382,7 +401,7 @@ static struct rtl_hal_cfg rtl8821ae_hal_cfg = {
 
 	.maps[RTL_IMR_TXFOVW] = IMR_TXFOVW,
 	.maps[RTL_IMR_PSTIMEOUT] = IMR_PSTIMEOUT,
-	.maps[RTL_IMR_BcnInt] = IMR_BCNDMAINT0,
+	.maps[RTL_IMR_BCNINT] = IMR_BCNDMAINT0,
 	.maps[RTL_IMR_RXFOVW] = IMR_RXFOVW,
 	.maps[RTL_IMR_RDU] = IMR_RDU,
 	.maps[RTL_IMR_ATIMEND] = IMR_ATIMEND,
@@ -413,29 +432,44 @@ static struct rtl_hal_cfg rtl8821ae_hal_cfg = {
 
 	.maps[RTL_RC_HT_RATEMCS7] =  DESC_RATEMCS7,
 	.maps[RTL_RC_HT_RATEMCS15] =  DESC_RATEMCS15,
+
+	/*VHT hightest rate*/
+	.maps[RTL_RC_VHT_RATE_1SS_MCS7] = DESC_RATEVHT1SS_MCS7,
+	.maps[RTL_RC_VHT_RATE_1SS_MCS8] = DESC_RATEVHT1SS_MCS8,
+	.maps[RTL_RC_VHT_RATE_1SS_MCS9] = DESC_RATEVHT1SS_MCS9,
+	.maps[RTL_RC_VHT_RATE_2SS_MCS7] = DESC_RATEVHT2SS_MCS7,
+	.maps[RTL_RC_VHT_RATE_2SS_MCS8] = DESC_RATEVHT2SS_MCS8,
+	.maps[RTL_RC_VHT_RATE_2SS_MCS9] = DESC_RATEVHT2SS_MCS9,
 };
 
 static struct pci_device_id rtl8821ae_pci_ids[] = {
-        {RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8812, rtl8821ae_hal_cfg)},
-        {RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8821, rtl8821ae_hal_cfg)},
-        {},
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8812, rtl8821ae_hal_cfg)},
+	{RTL_PCI_DEVICE(PCI_VENDOR_ID_REALTEK, 0x8821, rtl8821ae_hal_cfg)},
+	{},
 };
 
 MODULE_DEVICE_TABLE(pci, rtl8821ae_pci_ids);
 
-MODULE_AUTHOR("Ping Yan<ping_yan@realsil.com.cn>");
 MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("Realtek 8821ae 802.11ac PCI wireless");
 MODULE_FIRMWARE("rtlwifi/rtl8821aefw.bin");
 
 module_param_named(swenc, rtl8821ae_mod_params.sw_crypto, bool, 0444);
-module_param_named(ips, rtl8821ae_mod_params.b_inactiveps, bool, 0444);
-module_param_named(swlps, rtl8821ae_mod_params.b_swctrl_lps, bool, 0444);
-module_param_named(fwlps, rtl8821ae_mod_params.b_fwctrl_lps, bool, 0444);
-MODULE_PARM_DESC(swenc, "using hardware crypto (default 0 [hardware])\n");
-MODULE_PARM_DESC(ips, "using no link power save (default 1 is open)\n");
-MODULE_PARM_DESC(fwlps, "using linked fw control power save (default 1 is open)\n");
+module_param_named(debug, rtl8821ae_mod_params.debug, int, 0444);
+module_param_named(ips, rtl8821ae_mod_params.inactiveps, bool, 0444);
+module_param_named(swlps, rtl8821ae_mod_params.swctrl_lps, bool, 0444);
+module_param_named(fwlps, rtl8821ae_mod_params.fwctrl_lps, bool, 0444);
+module_param_named(msi, rtl8821ae_mod_params.msi_support, bool, 0444);
+module_param_named(disable_watchdog, rtl8821ae_mod_params.disable_watchdog,
+		   bool, 0444);
+MODULE_PARM_DESC(swenc, "Set to 1 for software crypto (default 0)\n");
+MODULE_PARM_DESC(ips, "Set to 0 to not use link power save (default 1)\n");
+MODULE_PARM_DESC(swlps, "Set to 1 to use SW control power save (default 0)\n");
+MODULE_PARM_DESC(fwlps, "Set to 1 to use FW control power save (default 1)\n");
+MODULE_PARM_DESC(msi, "Set to 1 to use MSI interrupts mode (default 1)\n");
+MODULE_PARM_DESC(debug, "Set debug level (0-5) (default 0)");
+MODULE_PARM_DESC(disable_watchdog, "Set to 1 to disable the watchdog (default 0)\n");
 
 static SIMPLE_DEV_PM_OPS(rtlwifi_pm_ops, rtl_pci_suspend, rtl_pci_resume);
 
@@ -444,35 +478,7 @@ static struct pci_driver rtl8821ae_driver = {
 	.id_table = rtl8821ae_pci_ids,
 	.probe = rtl_pci_probe,
 	.remove = rtl_pci_disconnect,
-
 	.driver.pm = &rtlwifi_pm_ops,
 };
 
-
-extern int rtl_core_module_init(void);
-extern void rtl_core_module_exit(void);
-
-static int __init rtl8821ae_module_init(void)
-{
-	int ret;
-
-	ret = rtl_core_module_init();
-	if (ret)
-		return ret;
-
-	//printk("==========>rtl8821ae_module_init().\n");
-	ret = pci_register_driver(&rtl8821ae_driver);
-	if (ret)
-		RT_ASSERT(false, (": No device found\n"));
-
-	return ret;
-}
-
-static void __exit rtl8821ae_module_exit(void)
-{
-	pci_unregister_driver(&rtl8821ae_driver);
-	rtl_core_module_exit();
-}
-
-module_init(rtl8821ae_module_init);
-module_exit(rtl8821ae_module_exit);
+module_pci_driver(rtl8821ae_driver);
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/sw.h b/drivers/net/wireless/rtlwifi/rtl8821ae/sw.h
index 3d49b2f043da..d001e7ce3052 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/sw.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/sw.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -35,5 +31,4 @@ void rtl8821ae_deinit_sw_vars(struct ieee80211_hw *hw);
 void rtl8821ae_init_var_map(struct ieee80211_hw *hw);
 bool rtl8821ae_get_btc_status(void);
 
-
 #endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/table.c b/drivers/net/wireless/rtlwifi/rtl8821ae/table.c
index a6c4ca4fd9b2..62a0fb76f080 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/table.c
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/table.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -81,9 +77,9 @@ u32 RTL8812AE_PHY_REG_ARRAY[] = {
 		0x8F8, 0x400002C0,
 		0x8FC, 0x00000000,
 	0xFF0F07D8, 0xABCD,
-		0x900, 0x00000700,
+		0x900, 0x00000701,
 	0xFF0F07D0, 0xCDEF,
-		0x900, 0x00000700,
+		0x900, 0x00000701,
 	0xCDCDCDCD, 0xCDCD,
 		0x900, 0x00000700,
 	0xFF0F07D8, 0xDEAD,
@@ -520,574 +516,6 @@ u32 RTL8821AE_PHY_REG_ARRAY_PG[] = {
 	1, 0, 0, 0x00000c44, 0x0000ffff, 0x00002022
 };
 
-/* it seems not used
-u8 *RTL8821AE_TXPWR_LMT_ARRAY[] = {
-	"FCC", "2.4G", "20M", "CCK", "1T", "01", "32",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "01", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "01", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "02", "32",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "02", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "02", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "03", "32",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "03", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "03", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "04", "34",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "04", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "04", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "05", "34",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "05", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "05", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "06", "34",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "06", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "06", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "07", "34",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "07", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "07", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "08", "34",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "08", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "08", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "09", "34",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "09", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "09", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "10", "32",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "10", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "10", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "11", "32",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "11", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "11", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "12", "63",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "12", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "12", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "13", "63",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "13", "32",
-	"MKK", "2.4G", "20M", "CCK", "1T", "13", "32",
-	"FCC", "2.4G", "20M", "CCK", "1T", "14", "63",
-	"ETSI", "2.4G", "20M", "CCK", "1T", "14", "63",
-	"MKK", "2.4G", "20M", "CCK", "1T", "14", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "01", "30",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "01", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "01", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "02", "30",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "02", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "02", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "03", "30",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "03", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "03", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "04", "32",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "04", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "04", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "05", "32",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "05", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "05", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "06", "32",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "06", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "06", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "07", "32",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "07", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "07", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "08", "32",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "08", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "08", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "09", "32",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "09", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "09", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "10", "30",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "10", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "10", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "11", "30",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "11", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "11", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "12", "63",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "12", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "12", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "13", "63",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "13", "32",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "13", "32",
-	"FCC", "2.4G", "20M", "OFDM", "1T", "14", "63",
-	"ETSI", "2.4G", "20M", "OFDM", "1T", "14", "63",
-	"MKK", "2.4G", "20M", "OFDM", "1T", "14", "63",
-	"FCC", "2.4G", "20M", "HT", "1T", "01", "26",
-	"ETSI", "2.4G", "20M", "HT", "1T", "01", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "01", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "02", "26",
-	"ETSI", "2.4G", "20M", "HT", "1T", "02", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "02", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "03", "26",
-	"ETSI", "2.4G", "20M", "HT", "1T", "03", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "03", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "04", "32",
-	"ETSI", "2.4G", "20M", "HT", "1T", "04", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "04", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "05", "32",
-	"ETSI", "2.4G", "20M", "HT", "1T", "05", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "05", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "06", "32",
-	"ETSI", "2.4G", "20M", "HT", "1T", "06", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "06", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "07", "32",
-	"ETSI", "2.4G", "20M", "HT", "1T", "07", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "07", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "08", "32",
-	"ETSI", "2.4G", "20M", "HT", "1T", "08", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "08", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "09", "32",
-	"ETSI", "2.4G", "20M", "HT", "1T", "09", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "09", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "10", "26",
-	"ETSI", "2.4G", "20M", "HT", "1T", "10", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "10", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "11", "26",
-	"ETSI", "2.4G", "20M", "HT", "1T", "11", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "11", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "12", "63",
-	"ETSI", "2.4G", "20M", "HT", "1T", "12", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "12", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "13", "63",
-	"ETSI", "2.4G", "20M", "HT", "1T", "13", "32",
-	"MKK", "2.4G", "20M", "HT", "1T", "13", "32",
-	"FCC", "2.4G", "20M", "HT", "1T", "14", "63",
-	"ETSI", "2.4G", "20M", "HT", "1T", "14", "63",
-	"MKK", "2.4G", "20M", "HT", "1T", "14", "63",
-	"FCC", "2.4G", "20M", "HT", "2T", "01", "30",
-	"ETSI", "2.4G", "20M", "HT", "2T", "01", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "01", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "02", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "02", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "02", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "03", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "03", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "03", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "04", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "04", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "04", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "05", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "05", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "05", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "06", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "06", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "06", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "07", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "07", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "07", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "08", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "08", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "08", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "09", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "09", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "09", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "10", "32",
-	"ETSI", "2.4G", "20M", "HT", "2T", "10", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "10", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "11", "30",
-	"ETSI", "2.4G", "20M", "HT", "2T", "11", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "11", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "12", "63",
-	"ETSI", "2.4G", "20M", "HT", "2T", "12", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "12", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "13", "63",
-	"ETSI", "2.4G", "20M", "HT", "2T", "13", "32",
-	"MKK", "2.4G", "20M", "HT", "2T", "13", "32",
-	"FCC", "2.4G", "20M", "HT", "2T", "14", "63",
-	"ETSI", "2.4G", "20M", "HT", "2T", "14", "63",
-	"MKK", "2.4G", "20M", "HT", "2T", "14", "63",
-	"FCC", "2.4G", "40M", "HT", "1T", "01", "63",
-	"ETSI", "2.4G", "40M", "HT", "1T", "01", "63",
-	"MKK", "2.4G", "40M", "HT", "1T", "01", "63",
-	"FCC", "2.4G", "40M", "HT", "1T", "02", "63",
-	"ETSI", "2.4G", "40M", "HT", "1T", "02", "63",
-	"MKK", "2.4G", "40M", "HT", "1T", "02", "63",
-	"FCC", "2.4G", "40M", "HT", "1T", "03", "26",
-	"ETSI", "2.4G", "40M", "HT", "1T", "03", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "03", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "04", "26",
-	"ETSI", "2.4G", "40M", "HT", "1T", "04", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "04", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "05", "32",
-	"ETSI", "2.4G", "40M", "HT", "1T", "05", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "05", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "06", "32",
-	"ETSI", "2.4G", "40M", "HT", "1T", "06", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "06", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "07", "32",
-	"ETSI", "2.4G", "40M", "HT", "1T", "07", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "07", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "08", "26",
-	"ETSI", "2.4G", "40M", "HT", "1T", "08", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "08", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "09", "26",
-	"ETSI", "2.4G", "40M", "HT", "1T", "09", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "09", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "10", "26",
-	"ETSI", "2.4G", "40M", "HT", "1T", "10", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "10", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "11", "26",
-	"ETSI", "2.4G", "40M", "HT", "1T", "11", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "11", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "12", "63",
-	"ETSI", "2.4G", "40M", "HT", "1T", "12", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "12", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "13", "63",
-	"ETSI", "2.4G", "40M", "HT", "1T", "13", "32",
-	"MKK", "2.4G", "40M", "HT", "1T", "13", "32",
-	"FCC", "2.4G", "40M", "HT", "1T", "14", "63",
-	"ETSI", "2.4G", "40M", "HT", "1T", "14", "63",
-	"MKK", "2.4G", "40M", "HT", "1T", "14", "63",
-	"FCC", "2.4G", "40M", "HT", "2T", "01", "63",
-	"ETSI", "2.4G", "40M", "HT", "2T", "01", "63",
-	"MKK", "2.4G", "40M", "HT", "2T", "01", "63",
-	"FCC", "2.4G", "40M", "HT", "2T", "02", "63",
-	"ETSI", "2.4G", "40M", "HT", "2T", "02", "63",
-	"MKK", "2.4G", "40M", "HT", "2T", "02", "63",
-	"FCC", "2.4G", "40M", "HT", "2T", "03", "30",
-	"ETSI", "2.4G", "40M", "HT", "2T", "03", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "03", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "04", "32",
-	"ETSI", "2.4G", "40M", "HT", "2T", "04", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "04", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "05", "32",
-	"ETSI", "2.4G", "40M", "HT", "2T", "05", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "05", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "06", "32",
-	"ETSI", "2.4G", "40M", "HT", "2T", "06", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "06", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "07", "32",
-	"ETSI", "2.4G", "40M", "HT", "2T", "07", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "07", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "08", "32",
-	"ETSI", "2.4G", "40M", "HT", "2T", "08", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "08", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "09", "32",
-	"ETSI", "2.4G", "40M", "HT", "2T", "09", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "09", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "10", "32",
-	"ETSI", "2.4G", "40M", "HT", "2T", "10", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "10", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "11", "30",
-	"ETSI", "2.4G", "40M", "HT", "2T", "11", "30",
-	"MKK", "2.4G", "40M", "HT", "2T", "11", "30",
-	"FCC", "2.4G", "40M", "HT", "2T", "12", "63",
-	"ETSI", "2.4G", "40M", "HT", "2T", "12", "32",
-	"MKK", "2.4G", "40M", "HT", "2T", "12", "32",
-	"FCC", "2.4G", "40M", "HT", "2T", "13", "63",
-	"ETSI", "2.4G", "40M", "HT", "2T", "13", "32",
-	"MKK", "2.4G", "40M", "HT", "2T", "13", "32",
-	"FCC", "2.4G", "40M", "HT", "2T", "14", "63",
-	"ETSI", "2.4G", "40M", "HT", "2T", "14", "63",
-	"MKK", "2.4G", "40M", "HT", "2T", "14", "63",
-	"FCC", "5G", "20M", "OFDM", "1T", "36", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "36", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "36", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "40", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "40", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "40", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "44", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "44", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "44", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "48", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "48", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "48", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "52", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "52", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "52", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "56", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "56", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "56", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "60", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "60", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "60", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "64", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "64", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "64", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "100", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "100", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "100", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "114", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "114", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "114", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "108", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "108", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "108", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "112", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "112", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "112", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "116", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "116", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "116", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "120", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "120", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "120", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "124", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "124", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "124", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "128", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "128", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "128", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "132", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "132", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "132", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "136", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "136", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "136", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "140", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "140", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "140", "30",
-	"FCC", "5G", "20M", "OFDM", "1T", "149", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "149", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "149", "63",
-	"FCC", "5G", "20M", "OFDM", "1T", "153", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "153", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "153", "63",
-	"FCC", "5G", "20M", "OFDM", "1T", "157", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "157", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "157", "63",
-	"FCC", "5G", "20M", "OFDM", "1T", "161", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "161", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "161", "63",
-	"FCC", "5G", "20M", "OFDM", "1T", "165", "30",
-	"ETSI", "5G", "20M", "OFDM", "1T", "165", "30",
-	"MKK", "5G", "20M", "OFDM", "1T", "165", "63",
-	"FCC", "5G", "20M", "HT", "1T", "36", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "36", "30",
-	"MKK", "5G", "20M", "HT", "1T", "36", "30",
-	"FCC", "5G", "20M", "HT", "1T", "40", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "40", "30",
-	"MKK", "5G", "20M", "HT", "1T", "40", "30",
-	"FCC", "5G", "20M", "HT", "1T", "44", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "44", "30",
-	"MKK", "5G", "20M", "HT", "1T", "44", "30",
-	"FCC", "5G", "20M", "HT", "1T", "48", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "48", "30",
-	"MKK", "5G", "20M", "HT", "1T", "48", "30",
-	"FCC", "5G", "20M", "HT", "1T", "52", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "52", "30",
-	"MKK", "5G", "20M", "HT", "1T", "52", "30",
-	"FCC", "5G", "20M", "HT", "1T", "56", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "56", "30",
-	"MKK", "5G", "20M", "HT", "1T", "56", "30",
-	"FCC", "5G", "20M", "HT", "1T", "60", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "60", "30",
-	"MKK", "5G", "20M", "HT", "1T", "60", "30",
-	"FCC", "5G", "20M", "HT", "1T", "64", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "64", "30",
-	"MKK", "5G", "20M", "HT", "1T", "64", "30",
-	"FCC", "5G", "20M", "HT", "1T", "100", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "100", "30",
-	"MKK", "5G", "20M", "HT", "1T", "100", "30",
-	"FCC", "5G", "20M", "HT", "1T", "114", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "114", "30",
-	"MKK", "5G", "20M", "HT", "1T", "114", "30",
-	"FCC", "5G", "20M", "HT", "1T", "108", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "108", "30",
-	"MKK", "5G", "20M", "HT", "1T", "108", "30",
-	"FCC", "5G", "20M", "HT", "1T", "112", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "112", "30",
-	"MKK", "5G", "20M", "HT", "1T", "112", "30",
-	"FCC", "5G", "20M", "HT", "1T", "116", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "116", "30",
-	"MKK", "5G", "20M", "HT", "1T", "116", "30",
-	"FCC", "5G", "20M", "HT", "1T", "120", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "120", "30",
-	"MKK", "5G", "20M", "HT", "1T", "120", "30",
-	"FCC", "5G", "20M", "HT", "1T", "124", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "124", "30",
-	"MKK", "5G", "20M", "HT", "1T", "124", "30",
-	"FCC", "5G", "20M", "HT", "1T", "128", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "128", "30",
-	"MKK", "5G", "20M", "HT", "1T", "128", "30",
-	"FCC", "5G", "20M", "HT", "1T", "132", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "132", "30",
-	"MKK", "5G", "20M", "HT", "1T", "132", "30",
-	"FCC", "5G", "20M", "HT", "1T", "136", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "136", "30",
-	"MKK", "5G", "20M", "HT", "1T", "136", "30",
-	"FCC", "5G", "20M", "HT", "1T", "140", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "140", "30",
-	"MKK", "5G", "20M", "HT", "1T", "140", "30",
-	"FCC", "5G", "20M", "HT", "1T", "149", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "149", "30",
-	"MKK", "5G", "20M", "HT", "1T", "149", "63",
-	"FCC", "5G", "20M", "HT", "1T", "153", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "153", "30",
-	"MKK", "5G", "20M", "HT", "1T", "153", "63",
-	"FCC", "5G", "20M", "HT", "1T", "157", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "157", "30",
-	"MKK", "5G", "20M", "HT", "1T", "157", "63",
-	"FCC", "5G", "20M", "HT", "1T", "161", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "161", "30",
-	"MKK", "5G", "20M", "HT", "1T", "161", "63",
-	"FCC", "5G", "20M", "HT", "1T", "165", "30",
-	"ETSI", "5G", "20M", "HT", "1T", "165", "30",
-	"MKK", "5G", "20M", "HT", "1T", "165", "63",
-	"FCC", "5G", "20M", "HT", "2T", "36", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "36", "30",
-	"MKK", "5G", "20M", "HT", "2T", "36", "30",
-	"FCC", "5G", "20M", "HT", "2T", "40", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "40", "30",
-	"MKK", "5G", "20M", "HT", "2T", "40", "30",
-	"FCC", "5G", "20M", "HT", "2T", "44", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "44", "30",
-	"MKK", "5G", "20M", "HT", "2T", "44", "30",
-	"FCC", "5G", "20M", "HT", "2T", "48", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "48", "30",
-	"MKK", "5G", "20M", "HT", "2T", "48", "30",
-	"FCC", "5G", "20M", "HT", "2T", "52", "34",
-	"ETSI", "5G", "20M", "HT", "2T", "52", "30",
-	"MKK", "5G", "20M", "HT", "2T", "52", "30",
-	"FCC", "5G", "20M", "HT", "2T", "56", "32",
-	"ETSI", "5G", "20M", "HT", "2T", "56", "30",
-	"MKK", "5G", "20M", "HT", "2T", "56", "30",
-	"FCC", "5G", "20M", "HT", "2T", "60", "30",
-	"ETSI", "5G", "20M", "HT", "2T", "60", "30",
-	"MKK", "5G", "20M", "HT", "2T", "60", "30",
-	"FCC", "5G", "20M", "HT", "2T", "64", "26",
-	"ETSI", "5G", "20M", "HT", "2T", "64", "30",
-	"MKK", "5G", "20M", "HT", "2T", "64", "30",
-	"FCC", "5G", "20M", "HT", "2T", "100", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "100", "30",
-	"MKK", "5G", "20M", "HT", "2T", "100", "30",
-	"FCC", "5G", "20M", "HT", "2T", "114", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "114", "30",
-	"MKK", "5G", "20M", "HT", "2T", "114", "30",
-	"FCC", "5G", "20M", "HT", "2T", "108", "30",
-	"ETSI", "5G", "20M", "HT", "2T", "108", "30",
-	"MKK", "5G", "20M", "HT", "2T", "108", "30",
-	"FCC", "5G", "20M", "HT", "2T", "112", "32",
-	"ETSI", "5G", "20M", "HT", "2T", "112", "30",
-	"MKK", "5G", "20M", "HT", "2T", "112", "30",
-	"FCC", "5G", "20M", "HT", "2T", "116", "32",
-	"ETSI", "5G", "20M", "HT", "2T", "116", "30",
-	"MKK", "5G", "20M", "HT", "2T", "116", "30",
-	"FCC", "5G", "20M", "HT", "2T", "120", "34",
-	"ETSI", "5G", "20M", "HT", "2T", "120", "30",
-	"MKK", "5G", "20M", "HT", "2T", "120", "30",
-	"FCC", "5G", "20M", "HT", "2T", "124", "32",
-	"ETSI", "5G", "20M", "HT", "2T", "124", "30",
-	"MKK", "5G", "20M", "HT", "2T", "124", "30",
-	"FCC", "5G", "20M", "HT", "2T", "128", "30",
-	"ETSI", "5G", "20M", "HT", "2T", "128", "30",
-	"MKK", "5G", "20M", "HT", "2T", "128", "30",
-	"FCC", "5G", "20M", "HT", "2T", "132", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "132", "30",
-	"MKK", "5G", "20M", "HT", "2T", "132", "30",
-	"FCC", "5G", "20M", "HT", "2T", "136", "28",
-	"ETSI", "5G", "20M", "HT", "2T", "136", "30",
-	"MKK", "5G", "20M", "HT", "2T", "136", "30",
-	"FCC", "5G", "20M", "HT", "2T", "140", "26",
-	"ETSI", "5G", "20M", "HT", "2T", "140", "30",
-	"MKK", "5G", "20M", "HT", "2T", "140", "30",
-	"FCC", "5G", "20M", "HT", "2T", "149", "34",
-	"ETSI", "5G", "20M", "HT", "2T", "149", "30",
-	"MKK", "5G", "20M", "HT", "2T", "149", "63",
-	"FCC", "5G", "20M", "HT", "2T", "153", "34",
-	"ETSI", "5G", "20M", "HT", "2T", "153", "30",
-	"MKK", "5G", "20M", "HT", "2T", "153", "63",
-	"FCC", "5G", "20M", "HT", "2T", "157", "34",
-	"ETSI", "5G", "20M", "HT", "2T", "157", "30",
-	"MKK", "5G", "20M", "HT", "2T", "157", "63",
-	"FCC", "5G", "20M", "HT", "2T", "161", "34",
-	"ETSI", "5G", "20M", "HT", "2T", "161", "30",
-	"MKK", "5G", "20M", "HT", "2T", "161", "63",
-	"FCC", "5G", "20M", "HT", "2T", "165", "34",
-	"ETSI", "5G", "20M", "HT", "2T", "165", "30",
-	"MKK", "5G", "20M", "HT", "2T", "165", "63",
-	"FCC", "5G", "40M", "HT", "1T", "38", "26",
-	"ETSI", "5G", "40M", "HT", "1T", "38", "30",
-	"MKK", "5G", "40M", "HT", "1T", "38", "30",
-	"FCC", "5G", "40M", "HT", "1T", "46", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "46", "30",
-	"MKK", "5G", "40M", "HT", "1T", "46", "30",
-	"FCC", "5G", "40M", "HT", "1T", "54", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "54", "30",
-	"MKK", "5G", "40M", "HT", "1T", "54", "30",
-	"FCC", "5G", "40M", "HT", "1T", "62", "26",
-	"ETSI", "5G", "40M", "HT", "1T", "62", "30",
-	"MKK", "5G", "40M", "HT", "1T", "62", "30",
-	"FCC", "5G", "40M", "HT", "1T", "102", "24",
-	"ETSI", "5G", "40M", "HT", "1T", "102", "30",
-	"MKK", "5G", "40M", "HT", "1T", "102", "30",
-	"FCC", "5G", "40M", "HT", "1T", "110", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "110", "30",
-	"MKK", "5G", "40M", "HT", "1T", "110", "30",
-	"FCC", "5G", "40M", "HT", "1T", "118", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "118", "30",
-	"MKK", "5G", "40M", "HT", "1T", "118", "30",
-	"FCC", "5G", "40M", "HT", "1T", "126", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "126", "30",
-	"MKK", "5G", "40M", "HT", "1T", "126", "30",
-	"FCC", "5G", "40M", "HT", "1T", "134", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "134", "30",
-	"MKK", "5G", "40M", "HT", "1T", "134", "30",
-	"FCC", "5G", "40M", "HT", "1T", "151", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "151", "30",
-	"MKK", "5G", "40M", "HT", "1T", "151", "63",
-	"FCC", "5G", "40M", "HT", "1T", "159", "30",
-	"ETSI", "5G", "40M", "HT", "1T", "159", "30",
-	"MKK", "5G", "40M", "HT", "1T", "159", "63",
-	"FCC", "5G", "40M", "HT", "2T", "38", "28",
-	"ETSI", "5G", "40M", "HT", "2T", "38", "30",
-	"MKK", "5G", "40M", "HT", "2T", "38", "30",
-	"FCC", "5G", "40M", "HT", "2T", "46", "28",
-	"ETSI", "5G", "40M", "HT", "2T", "46", "30",
-	"MKK", "5G", "40M", "HT", "2T", "46", "30",
-	"FCC", "5G", "40M", "HT", "2T", "54", "30",
-	"ETSI", "5G", "40M", "HT", "2T", "54", "30",
-	"MKK", "5G", "40M", "HT", "2T", "54", "30",
-	"FCC", "5G", "40M", "HT", "2T", "62", "30",
-	"ETSI", "5G", "40M", "HT", "2T", "62", "30",
-	"MKK", "5G", "40M", "HT", "2T", "62", "30",
-	"FCC", "5G", "40M", "HT", "2T", "102", "26",
-	"ETSI", "5G", "40M", "HT", "2T", "102", "30",
-	"MKK", "5G", "40M", "HT", "2T", "102", "30",
-	"FCC", "5G", "40M", "HT", "2T", "110", "30",
-	"ETSI", "5G", "40M", "HT", "2T", "110", "30",
-	"MKK", "5G", "40M", "HT", "2T", "110", "30",
-	"FCC", "5G", "40M", "HT", "2T", "118", "34",
-	"ETSI", "5G", "40M", "HT", "2T", "118", "30",
-	"MKK", "5G", "40M", "HT", "2T", "118", "30",
-	"FCC", "5G", "40M", "HT", "2T", "126", "32",
-	"ETSI", "5G", "40M", "HT", "2T", "126", "30",
-	"MKK", "5G", "40M", "HT", "2T", "126", "30",
-	"FCC", "5G", "40M", "HT", "2T", "134", "30",
-	"ETSI", "5G", "40M", "HT", "2T", "134", "30",
-	"MKK", "5G", "40M", "HT", "2T", "134", "30",
-	"FCC", "5G", "40M", "HT", "2T", "151", "34",
-	"ETSI", "5G", "40M", "HT", "2T", "151", "30",
-	"MKK", "5G", "40M", "HT", "2T", "151", "63",
-	"FCC", "5G", "40M", "HT", "2T", "159", "34",
-	"ETSI", "5G", "40M", "HT", "2T", "159", "30",
-	"MKK", "5G", "40M", "HT", "2T", "159", "63",
-	"FCC", "5G", "80M", "VHT", "1T", "42", "22",
-	"ETSI", "5G", "80M", "VHT", "1T", "42", "30",
-	"MKK", "5G", "80M", "VHT", "1T", "42", "30",
-	"FCC", "5G", "80M", "VHT", "1T", "58", "20",
-	"ETSI", "5G", "80M", "VHT", "1T", "58", "30",
-	"MKK", "5G", "80M", "VHT", "1T", "58", "30",
-	"FCC", "5G", "80M", "VHT", "1T", "106", "20",
-	"ETSI", "5G", "80M", "VHT", "1T", "106", "30",
-	"MKK", "5G", "80M", "VHT", "1T", "106", "30",
-	"FCC", "5G", "80M", "VHT", "1T", "122", "28",
-	"ETSI", "5G", "80M", "VHT", "1T", "122", "30",
-	"MKK", "5G", "80M", "VHT", "1T", "122", "30",
-	"FCC", "5G", "80M", "VHT", "1T", "155", "30",
-	"ETSI", "5G", "80M", "VHT", "1T", "155", "30",
-	"MKK", "5G", "80M", "VHT", "1T", "155", "63",
-	"FCC", "5G", "80M", "VHT", "2T", "42", "28",
-	"ETSI", "5G", "80M", "VHT", "2T", "42", "30",
-	"MKK", "5G", "80M", "VHT", "2T", "42", "30",
-	"FCC", "5G", "80M", "VHT", "2T", "58", "26",
-	"ETSI", "5G", "80M", "VHT", "2T", "58", "30",
-	"MKK", "5G", "80M", "VHT", "2T", "58", "30",
-	"FCC", "5G", "80M", "VHT", "2T", "106", "28",
-	"ETSI", "5G", "80M", "VHT", "2T", "106", "30",
-	"MKK", "5G", "80M", "VHT", "2T", "106", "30",
-	"FCC", "5G", "80M", "VHT", "2T", "122", "32",
-	"ETSI", "5G", "80M", "VHT", "2T", "122", "30",
-	"MKK", "5G", "80M", "VHT", "2T", "122", "30",
-	"FCC", "5G", "80M", "VHT", "2T", "155", "34",
-	"ETSI", "5G", "80M", "VHT", "2T", "155", "30",
-	"MKK", "5G", "80M", "VHT", "2T", "155", "63"
-};*/
-
 u32 RTL8812AE_RADIOA_ARRAY[] = {
 		0x000, 0x00010000,
 		0x018, 0x0001712A,
@@ -1721,6 +1149,7 @@ u32 RTL8812AE_RADIOA_ARRAY[] = {
 		0x0FE, 0x00000000,
 		0x0B4, 0x0001A78D,
 		0x018, 0x0001712A,
+
 };
 
 u32 RTL8812AE_RADIOB_ARRAY[] = {
@@ -2344,6 +1773,7 @@ u32 RTL8812AE_RADIOB_ARRAY[] = {
 		0x065, 0x000931D7,
 	0xFF0F0740, 0xDEAD,
 		0x008, 0x00008400,
+
 };
 
 u32 RTL8821AE_RADIOA_ARRAY[] = {
@@ -3045,6 +2475,7 @@ u32 RTL8812AE_MAC_REG_ARRAY[] = {
 		0x70A, 0x00000065,
 		0x70B, 0x00000087,
 		0x718, 0x00000040,
+
 };
 
 u32 RTL8821AE_MAC_REG_ARRAY[] = {
@@ -3804,6 +3235,7 @@ u32 RTL8812AE_AGC_TAB_ARRAY[] = {
 		0xC50, 0x00000020,
 		0xE50, 0x00000022,
 		0xE50, 0x00000020,
+
 };
 
 u32 RTL8821AE_AGC_TAB_ARRAY[] = {
@@ -4000,3 +3432,1141 @@ u32 RTL8821AE_AGC_TAB_ARRAY[] = {
 		0xC50, 0x00000020,
 
 };
+
+/******************************************************************************
+*                           TXPWR_LMT.TXT
+******************************************************************************/
+
+u8 *RTL8812AE_TXPWR_LMT[] = {
+	"FCC", "2.4G", "20M", "CCK", "1T", "01", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "02", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "03", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "04", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "05", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "06", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "07", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "08", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "09", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "10", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "11", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "CCK", "1T", "14", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "01", "34",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "02", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "03", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "04", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "05", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "06", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "07", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "08", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "09", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "10", "36",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "1T", "01", "34",
+	"ETSI", "2.4G", "20M", "HT", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "02", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "03", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "04", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "05", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "06", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "07", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "08", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "09", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "10", "36",
+	"ETSI", "2.4G", "20M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "11", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "2T", "01", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "02", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "03", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "04", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "05", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "06", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "07", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "08", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "09", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "10", "34",
+	"ETSI", "2.4G", "20M", "HT", "2T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "2T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "03", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "04", "36",
+	"ETSI", "2.4G", "40M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "05", "36",
+	"ETSI", "2.4G", "40M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "06", "36",
+	"ETSI", "2.4G", "40M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "07", "36",
+	"ETSI", "2.4G", "40M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "08", "36",
+	"ETSI", "2.4G", "40M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "09", "36",
+	"ETSI", "2.4G", "40M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "10", "36",
+	"ETSI", "2.4G", "40M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "11", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "03", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "03", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "03", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "04", "34",
+	"ETSI", "2.4G", "40M", "HT", "2T", "04", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "04", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "05", "34",
+	"ETSI", "2.4G", "40M", "HT", "2T", "05", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "05", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "06", "34",
+	"ETSI", "2.4G", "40M", "HT", "2T", "06", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "06", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "07", "34",
+	"ETSI", "2.4G", "40M", "HT", "2T", "07", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "07", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "08", "34",
+	"ETSI", "2.4G", "40M", "HT", "2T", "08", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "08", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "09", "34",
+	"ETSI", "2.4G", "40M", "HT", "2T", "09", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "09", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "10", "34",
+	"ETSI", "2.4G", "40M", "HT", "2T", "10", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "10", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "11", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "11", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "14", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "36", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "36", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "36", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "40", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "40", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "40", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "44", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "44", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "44", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "48", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "48", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "48", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "52", "36",
+	"ETSI", "5G", "20M", "OFDM", "1T", "52", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "52", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "56", "34",
+	"ETSI", "5G", "20M", "OFDM", "1T", "56", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "56", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "60", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "60", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "60", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "64", "28",
+	"ETSI", "5G", "20M", "OFDM", "1T", "64", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "64", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "100", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "100", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "100", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "114", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "114", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "114", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "108", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "108", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "108", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "112", "34",
+	"ETSI", "5G", "20M", "OFDM", "1T", "112", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "112", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "116", "34",
+	"ETSI", "5G", "20M", "OFDM", "1T", "116", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "116", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "120", "36",
+	"ETSI", "5G", "20M", "OFDM", "1T", "120", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "120", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "124", "34",
+	"ETSI", "5G", "20M", "OFDM", "1T", "124", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "124", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "128", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "128", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "128", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "132", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "132", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "132", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "136", "30",
+	"ETSI", "5G", "20M", "OFDM", "1T", "136", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "136", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "140", "28",
+	"ETSI", "5G", "20M", "OFDM", "1T", "140", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "140", "32",
+	"FCC", "5G", "20M", "OFDM", "1T", "149", "36",
+	"ETSI", "5G", "20M", "OFDM", "1T", "149", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "149", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "153", "36",
+	"ETSI", "5G", "20M", "OFDM", "1T", "153", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "153", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "157", "36",
+	"ETSI", "5G", "20M", "OFDM", "1T", "157", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "157", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "161", "36",
+	"ETSI", "5G", "20M", "OFDM", "1T", "161", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "161", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "165", "36",
+	"ETSI", "5G", "20M", "OFDM", "1T", "165", "32",
+	"MKK", "5G", "20M", "OFDM", "1T", "165", "63",
+	"FCC", "5G", "20M", "HT", "1T", "36", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "36", "32",
+	"MKK", "5G", "20M", "HT", "1T", "36", "32",
+	"FCC", "5G", "20M", "HT", "1T", "40", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "40", "32",
+	"MKK", "5G", "20M", "HT", "1T", "40", "32",
+	"FCC", "5G", "20M", "HT", "1T", "44", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "44", "32",
+	"MKK", "5G", "20M", "HT", "1T", "44", "32",
+	"FCC", "5G", "20M", "HT", "1T", "48", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "48", "32",
+	"MKK", "5G", "20M", "HT", "1T", "48", "32",
+	"FCC", "5G", "20M", "HT", "1T", "52", "36",
+	"ETSI", "5G", "20M", "HT", "1T", "52", "32",
+	"MKK", "5G", "20M", "HT", "1T", "52", "32",
+	"FCC", "5G", "20M", "HT", "1T", "56", "34",
+	"ETSI", "5G", "20M", "HT", "1T", "56", "32",
+	"MKK", "5G", "20M", "HT", "1T", "56", "32",
+	"FCC", "5G", "20M", "HT", "1T", "60", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "60", "32",
+	"MKK", "5G", "20M", "HT", "1T", "60", "32",
+	"FCC", "5G", "20M", "HT", "1T", "64", "28",
+	"ETSI", "5G", "20M", "HT", "1T", "64", "32",
+	"MKK", "5G", "20M", "HT", "1T", "64", "32",
+	"FCC", "5G", "20M", "HT", "1T", "100", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "100", "32",
+	"MKK", "5G", "20M", "HT", "1T", "100", "32",
+	"FCC", "5G", "20M", "HT", "1T", "114", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "114", "32",
+	"MKK", "5G", "20M", "HT", "1T", "114", "32",
+	"FCC", "5G", "20M", "HT", "1T", "108", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "108", "32",
+	"MKK", "5G", "20M", "HT", "1T", "108", "32",
+	"FCC", "5G", "20M", "HT", "1T", "112", "34",
+	"ETSI", "5G", "20M", "HT", "1T", "112", "32",
+	"MKK", "5G", "20M", "HT", "1T", "112", "32",
+	"FCC", "5G", "20M", "HT", "1T", "116", "34",
+	"ETSI", "5G", "20M", "HT", "1T", "116", "32",
+	"MKK", "5G", "20M", "HT", "1T", "116", "32",
+	"FCC", "5G", "20M", "HT", "1T", "120", "36",
+	"ETSI", "5G", "20M", "HT", "1T", "120", "32",
+	"MKK", "5G", "20M", "HT", "1T", "120", "32",
+	"FCC", "5G", "20M", "HT", "1T", "124", "34",
+	"ETSI", "5G", "20M", "HT", "1T", "124", "32",
+	"MKK", "5G", "20M", "HT", "1T", "124", "32",
+	"FCC", "5G", "20M", "HT", "1T", "128", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "128", "32",
+	"MKK", "5G", "20M", "HT", "1T", "128", "32",
+	"FCC", "5G", "20M", "HT", "1T", "132", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "132", "32",
+	"MKK", "5G", "20M", "HT", "1T", "132", "32",
+	"FCC", "5G", "20M", "HT", "1T", "136", "30",
+	"ETSI", "5G", "20M", "HT", "1T", "136", "32",
+	"MKK", "5G", "20M", "HT", "1T", "136", "32",
+	"FCC", "5G", "20M", "HT", "1T", "140", "28",
+	"ETSI", "5G", "20M", "HT", "1T", "140", "32",
+	"MKK", "5G", "20M", "HT", "1T", "140", "32",
+	"FCC", "5G", "20M", "HT", "1T", "149", "36",
+	"ETSI", "5G", "20M", "HT", "1T", "149", "32",
+	"MKK", "5G", "20M", "HT", "1T", "149", "63",
+	"FCC", "5G", "20M", "HT", "1T", "153", "36",
+	"ETSI", "5G", "20M", "HT", "1T", "153", "32",
+	"MKK", "5G", "20M", "HT", "1T", "153", "63",
+	"FCC", "5G", "20M", "HT", "1T", "157", "36",
+	"ETSI", "5G", "20M", "HT", "1T", "157", "32",
+	"MKK", "5G", "20M", "HT", "1T", "157", "63",
+	"FCC", "5G", "20M", "HT", "1T", "161", "36",
+	"ETSI", "5G", "20M", "HT", "1T", "161", "32",
+	"MKK", "5G", "20M", "HT", "1T", "161", "63",
+	"FCC", "5G", "20M", "HT", "1T", "165", "36",
+	"ETSI", "5G", "20M", "HT", "1T", "165", "32",
+	"MKK", "5G", "20M", "HT", "1T", "165", "63",
+	"FCC", "5G", "20M", "HT", "2T", "36", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "36", "30",
+	"MKK", "5G", "20M", "HT", "2T", "36", "30",
+	"FCC", "5G", "20M", "HT", "2T", "40", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "40", "30",
+	"MKK", "5G", "20M", "HT", "2T", "40", "30",
+	"FCC", "5G", "20M", "HT", "2T", "44", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "44", "30",
+	"MKK", "5G", "20M", "HT", "2T", "44", "30",
+	"FCC", "5G", "20M", "HT", "2T", "48", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "48", "30",
+	"MKK", "5G", "20M", "HT", "2T", "48", "30",
+	"FCC", "5G", "20M", "HT", "2T", "52", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "52", "30",
+	"MKK", "5G", "20M", "HT", "2T", "52", "30",
+	"FCC", "5G", "20M", "HT", "2T", "56", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "56", "30",
+	"MKK", "5G", "20M", "HT", "2T", "56", "30",
+	"FCC", "5G", "20M", "HT", "2T", "60", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "60", "30",
+	"MKK", "5G", "20M", "HT", "2T", "60", "30",
+	"FCC", "5G", "20M", "HT", "2T", "64", "26",
+	"ETSI", "5G", "20M", "HT", "2T", "64", "30",
+	"MKK", "5G", "20M", "HT", "2T", "64", "30",
+	"FCC", "5G", "20M", "HT", "2T", "100", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "100", "30",
+	"MKK", "5G", "20M", "HT", "2T", "100", "30",
+	"FCC", "5G", "20M", "HT", "2T", "114", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "114", "30",
+	"MKK", "5G", "20M", "HT", "2T", "114", "30",
+	"FCC", "5G", "20M", "HT", "2T", "108", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "108", "30",
+	"MKK", "5G", "20M", "HT", "2T", "108", "30",
+	"FCC", "5G", "20M", "HT", "2T", "112", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "112", "30",
+	"MKK", "5G", "20M", "HT", "2T", "112", "30",
+	"FCC", "5G", "20M", "HT", "2T", "116", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "116", "30",
+	"MKK", "5G", "20M", "HT", "2T", "116", "30",
+	"FCC", "5G", "20M", "HT", "2T", "120", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "120", "30",
+	"MKK", "5G", "20M", "HT", "2T", "120", "30",
+	"FCC", "5G", "20M", "HT", "2T", "124", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "124", "30",
+	"MKK", "5G", "20M", "HT", "2T", "124", "30",
+	"FCC", "5G", "20M", "HT", "2T", "128", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "128", "30",
+	"MKK", "5G", "20M", "HT", "2T", "128", "30",
+	"FCC", "5G", "20M", "HT", "2T", "132", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "132", "30",
+	"MKK", "5G", "20M", "HT", "2T", "132", "30",
+	"FCC", "5G", "20M", "HT", "2T", "136", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "136", "30",
+	"MKK", "5G", "20M", "HT", "2T", "136", "30",
+	"FCC", "5G", "20M", "HT", "2T", "140", "26",
+	"ETSI", "5G", "20M", "HT", "2T", "140", "30",
+	"MKK", "5G", "20M", "HT", "2T", "140", "30",
+	"FCC", "5G", "20M", "HT", "2T", "149", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "149", "30",
+	"MKK", "5G", "20M", "HT", "2T", "149", "63",
+	"FCC", "5G", "20M", "HT", "2T", "153", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "153", "30",
+	"MKK", "5G", "20M", "HT", "2T", "153", "63",
+	"FCC", "5G", "20M", "HT", "2T", "157", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "157", "30",
+	"MKK", "5G", "20M", "HT", "2T", "157", "63",
+	"FCC", "5G", "20M", "HT", "2T", "161", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "161", "30",
+	"MKK", "5G", "20M", "HT", "2T", "161", "63",
+	"FCC", "5G", "20M", "HT", "2T", "165", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "165", "30",
+	"MKK", "5G", "20M", "HT", "2T", "165", "63",
+	"FCC", "5G", "40M", "HT", "1T", "38", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "38", "32",
+	"MKK", "5G", "40M", "HT", "1T", "38", "32",
+	"FCC", "5G", "40M", "HT", "1T", "46", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "46", "32",
+	"MKK", "5G", "40M", "HT", "1T", "46", "32",
+	"FCC", "5G", "40M", "HT", "1T", "54", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "54", "32",
+	"MKK", "5G", "40M", "HT", "1T", "54", "32",
+	"FCC", "5G", "40M", "HT", "1T", "62", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "62", "32",
+	"MKK", "5G", "40M", "HT", "1T", "62", "32",
+	"FCC", "5G", "40M", "HT", "1T", "102", "28",
+	"ETSI", "5G", "40M", "HT", "1T", "102", "32",
+	"MKK", "5G", "40M", "HT", "1T", "102", "32",
+	"FCC", "5G", "40M", "HT", "1T", "110", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "110", "32",
+	"MKK", "5G", "40M", "HT", "1T", "110", "32",
+	"FCC", "5G", "40M", "HT", "1T", "118", "36",
+	"ETSI", "5G", "40M", "HT", "1T", "118", "32",
+	"MKK", "5G", "40M", "HT", "1T", "118", "32",
+	"FCC", "5G", "40M", "HT", "1T", "126", "34",
+	"ETSI", "5G", "40M", "HT", "1T", "126", "32",
+	"MKK", "5G", "40M", "HT", "1T", "126", "32",
+	"FCC", "5G", "40M", "HT", "1T", "134", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "134", "32",
+	"MKK", "5G", "40M", "HT", "1T", "134", "32",
+	"FCC", "5G", "40M", "HT", "1T", "151", "36",
+	"ETSI", "5G", "40M", "HT", "1T", "151", "32",
+	"MKK", "5G", "40M", "HT", "1T", "151", "63",
+	"FCC", "5G", "40M", "HT", "1T", "159", "36",
+	"ETSI", "5G", "40M", "HT", "1T", "159", "32",
+	"MKK", "5G", "40M", "HT", "1T", "159", "63",
+	"FCC", "5G", "40M", "HT", "2T", "38", "28",
+	"ETSI", "5G", "40M", "HT", "2T", "38", "30",
+	"MKK", "5G", "40M", "HT", "2T", "38", "30",
+	"FCC", "5G", "40M", "HT", "2T", "46", "28",
+	"ETSI", "5G", "40M", "HT", "2T", "46", "30",
+	"MKK", "5G", "40M", "HT", "2T", "46", "30",
+	"FCC", "5G", "40M", "HT", "2T", "54", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "54", "30",
+	"MKK", "5G", "40M", "HT", "2T", "54", "30",
+	"FCC", "5G", "40M", "HT", "2T", "62", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "62", "30",
+	"MKK", "5G", "40M", "HT", "2T", "62", "30",
+	"FCC", "5G", "40M", "HT", "2T", "102", "26",
+	"ETSI", "5G", "40M", "HT", "2T", "102", "30",
+	"MKK", "5G", "40M", "HT", "2T", "102", "30",
+	"FCC", "5G", "40M", "HT", "2T", "110", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "110", "30",
+	"MKK", "5G", "40M", "HT", "2T", "110", "30",
+	"FCC", "5G", "40M", "HT", "2T", "118", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "118", "30",
+	"MKK", "5G", "40M", "HT", "2T", "118", "30",
+	"FCC", "5G", "40M", "HT", "2T", "126", "32",
+	"ETSI", "5G", "40M", "HT", "2T", "126", "30",
+	"MKK", "5G", "40M", "HT", "2T", "126", "30",
+	"FCC", "5G", "40M", "HT", "2T", "134", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "134", "30",
+	"MKK", "5G", "40M", "HT", "2T", "134", "30",
+	"FCC", "5G", "40M", "HT", "2T", "151", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "151", "30",
+	"MKK", "5G", "40M", "HT", "2T", "151", "63",
+	"FCC", "5G", "40M", "HT", "2T", "159", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "159", "30",
+	"MKK", "5G", "40M", "HT", "2T", "159", "63",
+	"FCC", "5G", "80M", "VHT", "1T", "42", "30",
+	"ETSI", "5G", "80M", "VHT", "1T", "42", "32",
+	"MKK", "5G", "80M", "VHT", "1T", "42", "32",
+	"FCC", "5G", "80M", "VHT", "1T", "58", "28",
+	"ETSI", "5G", "80M", "VHT", "1T", "58", "32",
+	"MKK", "5G", "80M", "VHT", "1T", "58", "32",
+	"FCC", "5G", "80M", "VHT", "1T", "106", "30",
+	"ETSI", "5G", "80M", "VHT", "1T", "106", "32",
+	"MKK", "5G", "80M", "VHT", "1T", "106", "32",
+	"FCC", "5G", "80M", "VHT", "1T", "122", "34",
+	"ETSI", "5G", "80M", "VHT", "1T", "122", "32",
+	"MKK", "5G", "80M", "VHT", "1T", "122", "32",
+	"FCC", "5G", "80M", "VHT", "1T", "155", "36",
+	"ETSI", "5G", "80M", "VHT", "1T", "155", "32",
+	"MKK", "5G", "80M", "VHT", "1T", "155", "63",
+	"FCC", "5G", "80M", "VHT", "2T", "42", "28",
+	"ETSI", "5G", "80M", "VHT", "2T", "42", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "42", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "58", "26",
+	"ETSI", "5G", "80M", "VHT", "2T", "58", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "58", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "106", "28",
+	"ETSI", "5G", "80M", "VHT", "2T", "106", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "106", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "122", "32",
+	"ETSI", "5G", "80M", "VHT", "2T", "122", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "122", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "155", "34",
+	"ETSI", "5G", "80M", "VHT", "2T", "155", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "155", "63"
+};
+
+u8 *RTL8821AE_TXPWR_LMT[] = {
+	"FCC", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "03", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "04", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "05", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "06", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "07", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "08", "36",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "CCK", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "CCK", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "CCK", "1T", "14", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "01", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "02", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "09", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "10", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "11", "30",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "OFDM", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "1T", "01", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "02", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "03", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "09", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "20M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "20M", "HT", "2T", "01", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "01", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "01", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "02", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "02", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "02", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "03", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "03", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "03", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "04", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "04", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "05", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "05", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "06", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "06", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "07", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "07", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "08", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "08", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "09", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "09", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "20M", "HT", "2T", "10", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "10", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "20M", "HT", "2T", "11", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "11", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "20M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "20M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "20M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "20M", "HT", "2T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "1T", "03", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "03", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "03", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "04", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "04", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "04", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "05", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "05", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "05", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "06", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "06", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "07", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "07", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "08", "32",
+	"ETSI", "2.4G", "40M", "HT", "1T", "08", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "08", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "09", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "09", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "09", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "10", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "10", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "10", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "11", "26",
+	"ETSI", "2.4G", "40M", "HT", "1T", "11", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "11", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "1T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "1T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "1T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "1T", "14", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "01", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "01", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "01", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "02", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "02", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "02", "63",
+	"FCC", "2.4G", "40M", "HT", "2T", "03", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "03", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "03", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "04", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "04", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "04", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "05", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "05", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "05", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "06", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "06", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "06", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "07", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "07", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "07", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "08", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "08", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "08", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "09", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "09", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "09", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "10", "32",
+	"ETSI", "2.4G", "40M", "HT", "2T", "10", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "10", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "11", "30",
+	"ETSI", "2.4G", "40M", "HT", "2T", "11", "30",
+	"MKK", "2.4G", "40M", "HT", "2T", "11", "30",
+	"FCC", "2.4G", "40M", "HT", "2T", "12", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "12", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "12", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "13", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "13", "32",
+	"MKK", "2.4G", "40M", "HT", "2T", "13", "32",
+	"FCC", "2.4G", "40M", "HT", "2T", "14", "63",
+	"ETSI", "2.4G", "40M", "HT", "2T", "14", "63",
+	"MKK", "2.4G", "40M", "HT", "2T", "14", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "36", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "36", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "36", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "40", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "40", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "40", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "44", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "44", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "44", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "48", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "48", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "48", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "52", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "52", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "52", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "56", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "56", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "56", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "60", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "60", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "60", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "64", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "64", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "64", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "100", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "100", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "100", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "114", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "114", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "114", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "108", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "108", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "108", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "112", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "112", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "112", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "116", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "116", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "116", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "120", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "120", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "120", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "124", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "124", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "124", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "128", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "128", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "128", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "132", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "132", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "132", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "136", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "136", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "136", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "140", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "140", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "140", "30",
+	"FCC", "5G", "20M", "OFDM", "1T", "149", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "149", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "149", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "153", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "153", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "153", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "157", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "157", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "157", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "161", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "161", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "161", "63",
+	"FCC", "5G", "20M", "OFDM", "1T", "165", "32",
+	"ETSI", "5G", "20M", "OFDM", "1T", "165", "30",
+	"MKK", "5G", "20M", "OFDM", "1T", "165", "63",
+	"FCC", "5G", "20M", "HT", "1T", "36", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "36", "30",
+	"MKK", "5G", "20M", "HT", "1T", "36", "30",
+	"FCC", "5G", "20M", "HT", "1T", "40", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "40", "30",
+	"MKK", "5G", "20M", "HT", "1T", "40", "30",
+	"FCC", "5G", "20M", "HT", "1T", "44", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "44", "30",
+	"MKK", "5G", "20M", "HT", "1T", "44", "30",
+	"FCC", "5G", "20M", "HT", "1T", "48", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "48", "30",
+	"MKK", "5G", "20M", "HT", "1T", "48", "30",
+	"FCC", "5G", "20M", "HT", "1T", "52", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "52", "30",
+	"MKK", "5G", "20M", "HT", "1T", "52", "30",
+	"FCC", "5G", "20M", "HT", "1T", "56", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "56", "30",
+	"MKK", "5G", "20M", "HT", "1T", "56", "30",
+	"FCC", "5G", "20M", "HT", "1T", "60", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "60", "30",
+	"MKK", "5G", "20M", "HT", "1T", "60", "30",
+	"FCC", "5G", "20M", "HT", "1T", "64", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "64", "30",
+	"MKK", "5G", "20M", "HT", "1T", "64", "30",
+	"FCC", "5G", "20M", "HT", "1T", "100", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "100", "30",
+	"MKK", "5G", "20M", "HT", "1T", "100", "30",
+	"FCC", "5G", "20M", "HT", "1T", "114", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "114", "30",
+	"MKK", "5G", "20M", "HT", "1T", "114", "30",
+	"FCC", "5G", "20M", "HT", "1T", "108", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "108", "30",
+	"MKK", "5G", "20M", "HT", "1T", "108", "30",
+	"FCC", "5G", "20M", "HT", "1T", "112", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "112", "30",
+	"MKK", "5G", "20M", "HT", "1T", "112", "30",
+	"FCC", "5G", "20M", "HT", "1T", "116", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "116", "30",
+	"MKK", "5G", "20M", "HT", "1T", "116", "30",
+	"FCC", "5G", "20M", "HT", "1T", "120", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "120", "30",
+	"MKK", "5G", "20M", "HT", "1T", "120", "30",
+	"FCC", "5G", "20M", "HT", "1T", "124", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "124", "30",
+	"MKK", "5G", "20M", "HT", "1T", "124", "30",
+	"FCC", "5G", "20M", "HT", "1T", "128", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "128", "30",
+	"MKK", "5G", "20M", "HT", "1T", "128", "30",
+	"FCC", "5G", "20M", "HT", "1T", "132", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "132", "30",
+	"MKK", "5G", "20M", "HT", "1T", "132", "30",
+	"FCC", "5G", "20M", "HT", "1T", "136", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "136", "30",
+	"MKK", "5G", "20M", "HT", "1T", "136", "30",
+	"FCC", "5G", "20M", "HT", "1T", "140", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "140", "30",
+	"MKK", "5G", "20M", "HT", "1T", "140", "30",
+	"FCC", "5G", "20M", "HT", "1T", "149", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "149", "30",
+	"MKK", "5G", "20M", "HT", "1T", "149", "63",
+	"FCC", "5G", "20M", "HT", "1T", "153", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "153", "30",
+	"MKK", "5G", "20M", "HT", "1T", "153", "63",
+	"FCC", "5G", "20M", "HT", "1T", "157", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "157", "30",
+	"MKK", "5G", "20M", "HT", "1T", "157", "63",
+	"FCC", "5G", "20M", "HT", "1T", "161", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "161", "30",
+	"MKK", "5G", "20M", "HT", "1T", "161", "63",
+	"FCC", "5G", "20M", "HT", "1T", "165", "32",
+	"ETSI", "5G", "20M", "HT", "1T", "165", "30",
+	"MKK", "5G", "20M", "HT", "1T", "165", "63",
+	"FCC", "5G", "20M", "HT", "2T", "36", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "36", "30",
+	"MKK", "5G", "20M", "HT", "2T", "36", "30",
+	"FCC", "5G", "20M", "HT", "2T", "40", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "40", "30",
+	"MKK", "5G", "20M", "HT", "2T", "40", "30",
+	"FCC", "5G", "20M", "HT", "2T", "44", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "44", "30",
+	"MKK", "5G", "20M", "HT", "2T", "44", "30",
+	"FCC", "5G", "20M", "HT", "2T", "48", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "48", "30",
+	"MKK", "5G", "20M", "HT", "2T", "48", "30",
+	"FCC", "5G", "20M", "HT", "2T", "52", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "52", "30",
+	"MKK", "5G", "20M", "HT", "2T", "52", "30",
+	"FCC", "5G", "20M", "HT", "2T", "56", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "56", "30",
+	"MKK", "5G", "20M", "HT", "2T", "56", "30",
+	"FCC", "5G", "20M", "HT", "2T", "60", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "60", "30",
+	"MKK", "5G", "20M", "HT", "2T", "60", "30",
+	"FCC", "5G", "20M", "HT", "2T", "64", "26",
+	"ETSI", "5G", "20M", "HT", "2T", "64", "30",
+	"MKK", "5G", "20M", "HT", "2T", "64", "30",
+	"FCC", "5G", "20M", "HT", "2T", "100", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "100", "30",
+	"MKK", "5G", "20M", "HT", "2T", "100", "30",
+	"FCC", "5G", "20M", "HT", "2T", "114", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "114", "30",
+	"MKK", "5G", "20M", "HT", "2T", "114", "30",
+	"FCC", "5G", "20M", "HT", "2T", "108", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "108", "30",
+	"MKK", "5G", "20M", "HT", "2T", "108", "30",
+	"FCC", "5G", "20M", "HT", "2T", "112", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "112", "30",
+	"MKK", "5G", "20M", "HT", "2T", "112", "30",
+	"FCC", "5G", "20M", "HT", "2T", "116", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "116", "30",
+	"MKK", "5G", "20M", "HT", "2T", "116", "30",
+	"FCC", "5G", "20M", "HT", "2T", "120", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "120", "30",
+	"MKK", "5G", "20M", "HT", "2T", "120", "30",
+	"FCC", "5G", "20M", "HT", "2T", "124", "32",
+	"ETSI", "5G", "20M", "HT", "2T", "124", "30",
+	"MKK", "5G", "20M", "HT", "2T", "124", "30",
+	"FCC", "5G", "20M", "HT", "2T", "128", "30",
+	"ETSI", "5G", "20M", "HT", "2T", "128", "30",
+	"MKK", "5G", "20M", "HT", "2T", "128", "30",
+	"FCC", "5G", "20M", "HT", "2T", "132", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "132", "30",
+	"MKK", "5G", "20M", "HT", "2T", "132", "30",
+	"FCC", "5G", "20M", "HT", "2T", "136", "28",
+	"ETSI", "5G", "20M", "HT", "2T", "136", "30",
+	"MKK", "5G", "20M", "HT", "2T", "136", "30",
+	"FCC", "5G", "20M", "HT", "2T", "140", "26",
+	"ETSI", "5G", "20M", "HT", "2T", "140", "30",
+	"MKK", "5G", "20M", "HT", "2T", "140", "30",
+	"FCC", "5G", "20M", "HT", "2T", "149", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "149", "30",
+	"MKK", "5G", "20M", "HT", "2T", "149", "63",
+	"FCC", "5G", "20M", "HT", "2T", "153", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "153", "30",
+	"MKK", "5G", "20M", "HT", "2T", "153", "63",
+	"FCC", "5G", "20M", "HT", "2T", "157", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "157", "30",
+	"MKK", "5G", "20M", "HT", "2T", "157", "63",
+	"FCC", "5G", "20M", "HT", "2T", "161", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "161", "30",
+	"MKK", "5G", "20M", "HT", "2T", "161", "63",
+	"FCC", "5G", "20M", "HT", "2T", "165", "34",
+	"ETSI", "5G", "20M", "HT", "2T", "165", "30",
+	"MKK", "5G", "20M", "HT", "2T", "165", "63",
+	"FCC", "5G", "40M", "HT", "1T", "38", "26",
+	"ETSI", "5G", "40M", "HT", "1T", "38", "30",
+	"MKK", "5G", "40M", "HT", "1T", "38", "30",
+	"FCC", "5G", "40M", "HT", "1T", "46", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "46", "30",
+	"MKK", "5G", "40M", "HT", "1T", "46", "30",
+	"FCC", "5G", "40M", "HT", "1T", "54", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "54", "30",
+	"MKK", "5G", "40M", "HT", "1T", "54", "30",
+	"FCC", "5G", "40M", "HT", "1T", "62", "24",
+	"ETSI", "5G", "40M", "HT", "1T", "62", "30",
+	"MKK", "5G", "40M", "HT", "1T", "62", "30",
+	"FCC", "5G", "40M", "HT", "1T", "102", "24",
+	"ETSI", "5G", "40M", "HT", "1T", "102", "30",
+	"MKK", "5G", "40M", "HT", "1T", "102", "30",
+	"FCC", "5G", "40M", "HT", "1T", "110", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "110", "30",
+	"MKK", "5G", "40M", "HT", "1T", "110", "30",
+	"FCC", "5G", "40M", "HT", "1T", "118", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "118", "30",
+	"MKK", "5G", "40M", "HT", "1T", "118", "30",
+	"FCC", "5G", "40M", "HT", "1T", "126", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "126", "30",
+	"MKK", "5G", "40M", "HT", "1T", "126", "30",
+	"FCC", "5G", "40M", "HT", "1T", "134", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "134", "30",
+	"MKK", "5G", "40M", "HT", "1T", "134", "30",
+	"FCC", "5G", "40M", "HT", "1T", "151", "30",
+	"ETSI", "5G", "40M", "HT", "1T", "151", "30",
+	"MKK", "5G", "40M", "HT", "1T", "151", "63",
+	"FCC", "5G", "40M", "HT", "1T", "159", "32",
+	"ETSI", "5G", "40M", "HT", "1T", "159", "30",
+	"MKK", "5G", "40M", "HT", "1T", "159", "63",
+	"FCC", "5G", "40M", "HT", "2T", "38", "28",
+	"ETSI", "5G", "40M", "HT", "2T", "38", "30",
+	"MKK", "5G", "40M", "HT", "2T", "38", "30",
+	"FCC", "5G", "40M", "HT", "2T", "46", "28",
+	"ETSI", "5G", "40M", "HT", "2T", "46", "30",
+	"MKK", "5G", "40M", "HT", "2T", "46", "30",
+	"FCC", "5G", "40M", "HT", "2T", "54", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "54", "30",
+	"MKK", "5G", "40M", "HT", "2T", "54", "30",
+	"FCC", "5G", "40M", "HT", "2T", "62", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "62", "30",
+	"MKK", "5G", "40M", "HT", "2T", "62", "30",
+	"FCC", "5G", "40M", "HT", "2T", "102", "26",
+	"ETSI", "5G", "40M", "HT", "2T", "102", "30",
+	"MKK", "5G", "40M", "HT", "2T", "102", "30",
+	"FCC", "5G", "40M", "HT", "2T", "110", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "110", "30",
+	"MKK", "5G", "40M", "HT", "2T", "110", "30",
+	"FCC", "5G", "40M", "HT", "2T", "118", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "118", "30",
+	"MKK", "5G", "40M", "HT", "2T", "118", "30",
+	"FCC", "5G", "40M", "HT", "2T", "126", "32",
+	"ETSI", "5G", "40M", "HT", "2T", "126", "30",
+	"MKK", "5G", "40M", "HT", "2T", "126", "30",
+	"FCC", "5G", "40M", "HT", "2T", "134", "30",
+	"ETSI", "5G", "40M", "HT", "2T", "134", "30",
+	"MKK", "5G", "40M", "HT", "2T", "134", "30",
+	"FCC", "5G", "40M", "HT", "2T", "151", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "151", "30",
+	"MKK", "5G", "40M", "HT", "2T", "151", "63",
+	"FCC", "5G", "40M", "HT", "2T", "159", "34",
+	"ETSI", "5G", "40M", "HT", "2T", "159", "30",
+	"MKK", "5G", "40M", "HT", "2T", "159", "63",
+	"FCC", "5G", "80M", "VHT", "1T", "42", "22",
+	"ETSI", "5G", "80M", "VHT", "1T", "42", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "42", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "58", "20",
+	"ETSI", "5G", "80M", "VHT", "1T", "58", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "58", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "106", "20",
+	"ETSI", "5G", "80M", "VHT", "1T", "106", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "106", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "122", "20",
+	"ETSI", "5G", "80M", "VHT", "1T", "122", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "122", "30",
+	"FCC", "5G", "80M", "VHT", "1T", "155", "28",
+	"ETSI", "5G", "80M", "VHT", "1T", "155", "30",
+	"MKK", "5G", "80M", "VHT", "1T", "155", "63",
+	"FCC", "5G", "80M", "VHT", "2T", "42", "28",
+	"ETSI", "5G", "80M", "VHT", "2T", "42", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "42", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "58", "26",
+	"ETSI", "5G", "80M", "VHT", "2T", "58", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "58", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "106", "28",
+	"ETSI", "5G", "80M", "VHT", "2T", "106", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "106", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "122", "32",
+	"ETSI", "5G", "80M", "VHT", "2T", "122", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "122", "30",
+	"FCC", "5G", "80M", "VHT", "2T", "155", "34",
+	"ETSI", "5G", "80M", "VHT", "2T", "155", "30",
+	"MKK", "5G", "80M", "VHT", "2T", "155", "63"
+};
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/table.h b/drivers/net/wireless/rtlwifi/rtl8821ae/table.h
index b9d7b266a33a..24bcff6bc507 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/table.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/table.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -37,26 +33,28 @@
 extern u32 RTL8821AE_PHY_REG_ARRAY[];
 #define  RTL8812AEPHY_REG_1TARRAYLEN	490
 extern u32 RTL8812AE_PHY_REG_ARRAY[];
-#define RTL8821AEPHY_REG_ARRAY_PGLEN 	90
+#define RTL8821AEPHY_REG_ARRAY_PGLEN	90
 extern u32 RTL8821AE_PHY_REG_ARRAY_PG[];
-#define RTL8812AEPHY_REG_ARRAY_PGLEN 	276
+#define RTL8812AEPHY_REG_ARRAY_PGLEN	276
 extern u32 RTL8812AE_PHY_REG_ARRAY_PG[];
-//#define	RTL8723BE_RADIOA_1TARRAYLEN 	206
-//extern u8 *RTL8821AE_TXPWR_LMT_ARRAY[];
-#define	RTL8812AE_RADIOA_1TARRAYLEN 	1264
+/* #define	RTL8723BE_RADIOA_1TARRAYLEN	206 */
+/* extern u8 *RTL8821AE_TXPWR_LMT_ARRAY[]; */
+#define	RTL8812AE_RADIOA_1TARRAYLEN	1264
 extern u32 RTL8812AE_RADIOA_ARRAY[];
-#define	RTL8812AE_RADIOB_1TARRAYLEN 	1240
+#define	RTL8812AE_RADIOB_1TARRAYLEN	1240
 extern u32 RTL8812AE_RADIOB_ARRAY[];
-#define	RTL8821AE_RADIOA_1TARRAYLEN 	1176
+#define	RTL8821AE_RADIOA_1TARRAYLEN	1176
 extern u32 RTL8821AE_RADIOA_ARRAY[];
-#define RTL8821AEMAC_1T_ARRAYLEN  		194
+#define RTL8821AEMAC_1T_ARRAYLEN		194
 extern u32 RTL8821AE_MAC_REG_ARRAY[];
-#define RTL8812AEMAC_1T_ARRAYLEN  		214
+#define RTL8812AEMAC_1T_ARRAYLEN		214
 extern u32 RTL8812AE_MAC_REG_ARRAY[];
-#define RTL8821AEAGCTAB_1TARRAYLEN 		382
+#define RTL8821AEAGCTAB_1TARRAYLEN		382
 extern u32 RTL8821AE_AGC_TAB_ARRAY[];
-#define RTL8812AEAGCTAB_1TARRAYLEN 		1312
+#define RTL8812AEAGCTAB_1TARRAYLEN		1312
 extern u32 RTL8812AE_AGC_TAB_ARRAY[];
-
-
+#define RTL8812AE_TXPWR_LMT_ARRAY_LEN		3948
+extern u8 *RTL8812AE_TXPWR_LMT[];
+#define RTL8821AE_TXPWR_LMT_ARRAY_LEN		3948
+extern u8 *RTL8821AE_TXPWR_LMT[];
 #endif
diff --git a/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
new file mode 100644
index 000000000000..383b86b05cba
--- /dev/null
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.c
@@ -0,0 +1,1236 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2010  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "../wifi.h"
+#include "../pci.h"
+#include "../base.h"
+#include "../stats.h"
+#include "reg.h"
+#include "def.h"
+#include "phy.h"
+#include "trx.h"
+#include "led.h"
+#include "dm.h"
+#include "phy.h"
+#include "fw.h"
+
+static u8 _rtl8821ae_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
+{
+	__le16 fc = rtl_get_fc(skb);
+
+	if (unlikely(ieee80211_is_beacon(fc)))
+		return QSLT_BEACON;
+	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
+		return QSLT_MGNT;
+
+	return skb->priority;
+}
+
+/* mac80211's rate_idx is like this:
+ *
+ * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
+ *
+ * B/G rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC_RATE1M-->DESC_RATE54M ==> idx is 0-->11,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
+ *
+ * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
+ * A rate:
+ * (rx_status->flag & RX_FLAG_HT) = 0,
+ * DESC_RATE6M-->DESC_RATE54M ==> idx is 0-->7,
+ *
+ * N rate:
+ * (rx_status->flag & RX_FLAG_HT) = 1,
+ * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
+ */
+static int _rtl8821ae_rate_mapping(struct ieee80211_hw *hw,
+				   bool isht, bool isvht, u8 desc_rate)
+{
+	int rate_idx;
+
+	if (!isht) {
+		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
+			switch (desc_rate) {
+			case DESC_RATE1M:
+				rate_idx = 0;
+				break;
+			case DESC_RATE2M:
+				rate_idx = 1;
+				break;
+			case DESC_RATE5_5M:
+				rate_idx = 2;
+				break;
+			case DESC_RATE11M:
+				rate_idx = 3;
+				break;
+			case DESC_RATE6M:
+				rate_idx = 4;
+				break;
+			case DESC_RATE9M:
+				rate_idx = 5;
+				break;
+			case DESC_RATE12M:
+				rate_idx = 6;
+				break;
+			case DESC_RATE18M:
+				rate_idx = 7;
+				break;
+			case DESC_RATE24M:
+				rate_idx = 8;
+				break;
+			case DESC_RATE36M:
+				rate_idx = 9;
+				break;
+			case DESC_RATE48M:
+				rate_idx = 10;
+				break;
+			case DESC_RATE54M:
+				rate_idx = 11;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		} else {
+			switch (desc_rate) {
+			case DESC_RATE6M:
+				rate_idx = 0;
+				break;
+			case DESC_RATE9M:
+				rate_idx = 1;
+				break;
+			case DESC_RATE12M:
+				rate_idx = 2;
+				break;
+			case DESC_RATE18M:
+				rate_idx = 3;
+				break;
+			case DESC_RATE24M:
+				rate_idx = 4;
+				break;
+			case DESC_RATE36M:
+				rate_idx = 5;
+				break;
+			case DESC_RATE48M:
+				rate_idx = 6;
+				break;
+			case DESC_RATE54M:
+				rate_idx = 7;
+				break;
+			default:
+				rate_idx = 0;
+				break;
+			}
+		}
+	} else {
+		switch (desc_rate) {
+		case DESC_RATEMCS0:
+			rate_idx = 0;
+			break;
+		case DESC_RATEMCS1:
+			rate_idx = 1;
+			break;
+		case DESC_RATEMCS2:
+			rate_idx = 2;
+			break;
+		case DESC_RATEMCS3:
+			rate_idx = 3;
+			break;
+		case DESC_RATEMCS4:
+			rate_idx = 4;
+			break;
+		case DESC_RATEMCS5:
+			rate_idx = 5;
+			break;
+		case DESC_RATEMCS6:
+			rate_idx = 6;
+			break;
+		case DESC_RATEMCS7:
+			rate_idx = 7;
+			break;
+		case DESC_RATEMCS8:
+			rate_idx = 8;
+			break;
+		case DESC_RATEMCS9:
+			rate_idx = 9;
+			break;
+		case DESC_RATEMCS10:
+			rate_idx = 10;
+			break;
+		case DESC_RATEMCS11:
+			rate_idx = 11;
+			break;
+		case DESC_RATEMCS12:
+			rate_idx = 12;
+			break;
+		case DESC_RATEMCS13:
+			rate_idx = 13;
+			break;
+		case DESC_RATEMCS14:
+			rate_idx = 14;
+			break;
+		case DESC_RATEMCS15:
+			rate_idx = 15;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+	}
+
+	if (isvht) {
+		switch (desc_rate) {
+		case DESC_RATEVHT1SS_MCS0:
+			rate_idx = 0;
+			break;
+		case DESC_RATEVHT1SS_MCS1:
+			rate_idx = 1;
+			break;
+		case DESC_RATEVHT1SS_MCS2:
+			rate_idx = 2;
+			break;
+		case DESC_RATEVHT1SS_MCS3:
+			rate_idx = 3;
+			break;
+		case DESC_RATEVHT1SS_MCS4:
+			rate_idx = 4;
+			break;
+		case DESC_RATEVHT1SS_MCS5:
+			rate_idx = 5;
+			break;
+		case DESC_RATEVHT1SS_MCS6:
+			rate_idx = 6;
+			break;
+		case DESC_RATEVHT1SS_MCS7:
+			rate_idx = 7;
+			break;
+		case DESC_RATEVHT1SS_MCS8:
+			rate_idx = 8;
+			break;
+		case DESC_RATEVHT1SS_MCS9:
+			rate_idx = 9;
+			break;
+		case DESC_RATEVHT2SS_MCS0:
+			rate_idx = 0;
+			break;
+		case DESC_RATEVHT2SS_MCS1:
+			rate_idx = 1;
+			break;
+		case DESC_RATEVHT2SS_MCS2:
+			rate_idx = 2;
+			break;
+		case DESC_RATEVHT2SS_MCS3:
+			rate_idx = 3;
+			break;
+		case DESC_RATEVHT2SS_MCS4:
+			rate_idx = 4;
+			break;
+		case DESC_RATEVHT2SS_MCS5:
+			rate_idx = 5;
+			break;
+		case DESC_RATEVHT2SS_MCS6:
+			rate_idx = 6;
+			break;
+		case DESC_RATEVHT2SS_MCS7:
+			rate_idx = 7;
+			break;
+		case DESC_RATEVHT2SS_MCS8:
+			rate_idx = 8;
+			break;
+		case DESC_RATEVHT2SS_MCS9:
+			rate_idx = 9;
+			break;
+		default:
+			rate_idx = 0;
+			break;
+		}
+	}
+	return rate_idx;
+}
+
+static u16 odm_cfo(char value)
+{
+	int ret_val;
+
+	if (value < 0) {
+		ret_val = 0 - value;
+		ret_val = (ret_val << 1) + (ret_val >> 1);
+		/* set bit12 as 1 for negative cfo */
+		ret_val = ret_val | BIT(12);
+	} else {
+		ret_val = value;
+		ret_val = (ret_val << 1) + (ret_val >> 1);
+	}
+	return ret_val;
+}
+
+static void query_rxphystatus(struct ieee80211_hw *hw,
+			      struct rtl_stats *pstatus, u8 *pdesc,
+			      struct rx_fwinfo_8821ae *p_drvinfo,
+			      bool bpacket_match_bssid,
+			      bool bpacket_toself, bool packet_beacon)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct phy_status_rpt *p_phystrpt = (struct phy_status_rpt *)p_drvinfo;
+	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	char rx_pwr_all = 0, rx_pwr[4];
+	u8 rf_rx_num = 0, evm, evmdbm, pwdb_all;
+	u8 i, max_spatial_stream;
+	u32 rssi, total_rssi = 0;
+	bool is_cck = pstatus->is_cck;
+	u8 lan_idx, vga_idx;
+
+	/* Record it for next packet processing */
+	pstatus->packet_matchbssid = bpacket_match_bssid;
+	pstatus->packet_toself = bpacket_toself;
+	pstatus->packet_beacon = packet_beacon;
+	pstatus->rx_mimo_signalquality[0] = -1;
+	pstatus->rx_mimo_signalquality[1] = -1;
+
+	if (is_cck) {
+		u8 cck_highpwr;
+		u8 cck_agc_rpt;
+
+		cck_agc_rpt = p_phystrpt->cfosho[0];
+
+		/* (1)Hardware does not provide RSSI for CCK
+		 * (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
+		cck_highpwr = (u8)rtlphy->cck_high_power;
+
+		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
+		vga_idx = (cck_agc_rpt & 0x1f);
+		if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE) {
+			switch (lan_idx) {
+			case 7:
+				if (vga_idx <= 27)
+					/*VGA_idx = 27~2*/
+					rx_pwr_all = -100 + 2*(27-vga_idx);
+				else
+					rx_pwr_all = -100;
+				break;
+			case 6:
+				/*VGA_idx = 2~0*/
+				rx_pwr_all = -48 + 2*(2-vga_idx);
+				break;
+			case 5:
+				/*VGA_idx = 7~5*/
+				rx_pwr_all = -42 + 2*(7-vga_idx);
+				break;
+			case 4:
+				/*VGA_idx = 7~4*/
+				rx_pwr_all = -36 + 2*(7-vga_idx);
+				break;
+			case 3:
+				/*VGA_idx = 7~0*/
+				rx_pwr_all = -24 + 2*(7-vga_idx);
+				break;
+			case 2:
+				if (cck_highpwr)
+					/*VGA_idx = 5~0*/
+					rx_pwr_all = -12 + 2*(5-vga_idx);
+				else
+					rx_pwr_all = -6 + 2*(5-vga_idx);
+				break;
+			case 1:
+				rx_pwr_all = 8-2*vga_idx;
+				break;
+			case 0:
+				rx_pwr_all = 14-2*vga_idx;
+				break;
+			default:
+				break;
+			}
+			rx_pwr_all += 6;
+			pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+			if (!cck_highpwr) {
+				if (pwdb_all >= 80)
+					pwdb_all =
+					  ((pwdb_all - 80)<<1) +
+					 ((pwdb_all - 80)>>1) + 80;
+				else if ((pwdb_all <= 78) && (pwdb_all >= 20))
+					pwdb_all += 3;
+				if (pwdb_all > 100)
+					pwdb_all = 100;
+			}
+		} else { /* 8821 */
+			char pout = -6;
+
+			switch (lan_idx) {
+			case 5:
+				rx_pwr_all = pout - 32 - (2*vga_idx);
+					break;
+			case 4:
+				rx_pwr_all = pout - 24 - (2*vga_idx);
+					break;
+			case 2:
+				rx_pwr_all = pout - 11 - (2*vga_idx);
+					break;
+			case 1:
+				rx_pwr_all = pout + 5 - (2*vga_idx);
+					break;
+			case 0:
+				rx_pwr_all = pout + 21 - (2*vga_idx);
+					break;
+			}
+			pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		}
+
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3) Get Signal Quality (EVM) */
+		if (bpacket_match_bssid) {
+			u8 sq;
+
+			if (pstatus->rx_pwdb_all > 40) {
+				sq = 100;
+			} else {
+				sq = p_phystrpt->pwdb_all;
+				if (sq > 64)
+					sq = 0;
+				else if (sq < 20)
+					sq = 100;
+				else
+					sq = ((64 - sq) * 100) / 44;
+			}
+
+			pstatus->signalquality = sq;
+			pstatus->rx_mimo_signalquality[0] = sq;
+			pstatus->rx_mimo_signalquality[1] = -1;
+		}
+	} else {
+		/* (1)Get RSSI for HT rate */
+		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
+			/* we will judge RF RX path now. */
+			if (rtlpriv->dm.rfpath_rxenable[i])
+				rf_rx_num++;
+
+			rx_pwr[i] = (p_phystrpt->gain_trsw[i] & 0x7f) - 110;
+
+			/* Translate DBM to percentage. */
+			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
+			total_rssi += rssi;
+
+			/* Get Rx snr value in DB */
+			pstatus->rx_snr[i] = p_phystrpt->rxsnr[i] / 2;
+			rtlpriv->stats.rx_snr_db[i] = p_phystrpt->rxsnr[i] / 2;
+
+			pstatus->cfo_short[i] = odm_cfo(p_phystrpt->cfosho[i]);
+			pstatus->cfo_tail[i] = odm_cfo(p_phystrpt->cfotail[i]);
+			/* Record Signal Strength for next packet */
+			pstatus->rx_mimo_signalstrength[i] = (u8)rssi;
+		}
+
+		/* (2)PWDB, Average PWDB cacluated by
+		 * hardware (for rate adaptive)
+		 */
+		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
+
+		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
+		pstatus->rx_pwdb_all = pwdb_all;
+		pstatus->rxpower = rx_pwr_all;
+		pstatus->recvsignalpower = rx_pwr_all;
+
+		/* (3)EVM of HT rate */
+		if ((pstatus->is_ht && pstatus->rate >= DESC_RATEMCS8 &&
+		     pstatus->rate <= DESC_RATEMCS15) ||
+		    (pstatus->is_vht &&
+		     pstatus->rate >= DESC_RATEVHT2SS_MCS0 &&
+		     pstatus->rate <= DESC_RATEVHT2SS_MCS9))
+			max_spatial_stream = 2;
+		else
+			max_spatial_stream = 1;
+
+		for (i = 0; i < max_spatial_stream; i++) {
+			evm = rtl_evm_db_to_percentage(p_phystrpt->rxevm[i]);
+			evmdbm = rtl_evm_dbm_jaguar(p_phystrpt->rxevm[i]);
+
+			if (bpacket_match_bssid) {
+				/* Fill value in RFD, Get the first
+				 * spatial stream only
+				 */
+				if (i == 0)
+					pstatus->signalquality = evm;
+				pstatus->rx_mimo_signalquality[i] = evm;
+				pstatus->rx_mimo_evm_dbm[i] = evmdbm;
+			}
+		}
+		if (bpacket_match_bssid) {
+			for (i = RF90_PATH_A; i <= RF90_PATH_B; i++)
+				rtl_priv(hw)->dm.cfo_tail[i] =
+					(char)p_phystrpt->cfotail[i];
+
+			rtl_priv(hw)->dm.packet_count++;
+		}
+	}
+
+	/* UI BSS List signal strength(in percentage),
+	 * make it good looking, from 0~100.
+	 */
+	if (is_cck)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+			pwdb_all));
+	else if (rf_rx_num != 0)
+		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
+			total_rssi /= rf_rx_num));
+	/*HW antenna diversity*/
+	rtldm->fat_table.antsel_rx_keep_0 = p_phystrpt->antidx_anta;
+	rtldm->fat_table.antsel_rx_keep_1 = p_phystrpt->antidx_antb;
+}
+
+static void translate_rx_signal_stuff(struct ieee80211_hw *hw,
+				      struct sk_buff *skb,
+				      struct rtl_stats *pstatus, u8 *pdesc,
+				      struct rx_fwinfo_8821ae *p_drvinfo)
+{
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
+	struct ieee80211_hdr *hdr;
+	u8 *tmp_buf;
+	u8 *praddr;
+	u8 *psaddr;
+	__le16 fc;
+	u16 type;
+	bool packet_matchbssid, packet_toself, packet_beacon;
+
+	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
+
+	hdr = (struct ieee80211_hdr *)tmp_buf;
+	fc = hdr->frame_control;
+	type = WLAN_FC_GET_TYPE(hdr->frame_control);
+	praddr = hdr->addr1;
+	psaddr = ieee80211_get_SA(hdr);
+	ether_addr_copy(pstatus->psaddr, psaddr);
+
+	packet_matchbssid = (!ieee80211_is_ctl(fc) &&
+			     (ether_addr_equal(mac->bssid,
+					       ieee80211_has_tods(fc) ?
+					       hdr->addr1 :
+					       ieee80211_has_fromds(fc) ?
+					       hdr->addr2 : hdr->addr3)) &&
+			      (!pstatus->hwerror) &&
+			      (!pstatus->crc) && (!pstatus->icv));
+
+	packet_toself = packet_matchbssid &&
+	    (ether_addr_equal(praddr, rtlefuse->dev_addr));
+
+	if (ieee80211_is_beacon(hdr->frame_control))
+		packet_beacon = true;
+	else
+		packet_beacon = false;
+
+	if (packet_beacon && packet_matchbssid)
+		rtl_priv(hw)->dm.dbginfo.num_qry_beacon_pkt++;
+
+	if (packet_matchbssid &&
+	    ieee80211_is_data_qos(hdr->frame_control) &&
+	    !is_multicast_ether_addr(ieee80211_get_DA(hdr))) {
+		struct ieee80211_qos_hdr *hdr_qos =
+			(struct ieee80211_qos_hdr *)tmp_buf;
+		u16 tid = le16_to_cpu(hdr_qos->qos_ctrl) & 0xf;
+
+		if (tid != 0 && tid != 3)
+			rtl_priv(hw)->dm.dbginfo.num_non_be_pkt++;
+	}
+
+	query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
+			  packet_matchbssid, packet_toself,
+			  packet_beacon);
+	/*_rtl8821ae_smart_antenna(hw, pstatus); */
+	rtl_process_phyinfo(hw, tmp_buf, pstatus);
+}
+
+static void _rtl8821ae_insert_emcontent(struct rtl_tcb_desc *ptcb_desc,
+					u8 *virtualaddress)
+{
+	u32 dwtmp = 0;
+
+	memset(virtualaddress, 0, 8);
+
+	SET_EARLYMODE_PKTNUM(virtualaddress, ptcb_desc->empkt_num);
+	if (ptcb_desc->empkt_num == 1) {
+		dwtmp = ptcb_desc->empkt_len[0];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[0];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0)+4;
+		dwtmp += ptcb_desc->empkt_len[1];
+	}
+	SET_EARLYMODE_LEN0(virtualaddress, dwtmp);
+
+	if (ptcb_desc->empkt_num <= 3) {
+		dwtmp = ptcb_desc->empkt_len[2];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[2];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0)+4;
+		dwtmp += ptcb_desc->empkt_len[3];
+	}
+	SET_EARLYMODE_LEN1(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 5) {
+		dwtmp = ptcb_desc->empkt_len[4];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[4];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0)+4;
+		dwtmp += ptcb_desc->empkt_len[5];
+	}
+	SET_EARLYMODE_LEN2_1(virtualaddress, dwtmp & 0xF);
+	SET_EARLYMODE_LEN2_2(virtualaddress, dwtmp >> 4);
+	if (ptcb_desc->empkt_num <= 7) {
+		dwtmp = ptcb_desc->empkt_len[6];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[6];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0)+4;
+		dwtmp += ptcb_desc->empkt_len[7];
+	}
+	SET_EARLYMODE_LEN3(virtualaddress, dwtmp);
+	if (ptcb_desc->empkt_num <= 9) {
+		dwtmp = ptcb_desc->empkt_len[8];
+	} else {
+		dwtmp = ptcb_desc->empkt_len[8];
+		dwtmp += ((dwtmp % 4) ? (4 - dwtmp % 4) : 0)+4;
+		dwtmp += ptcb_desc->empkt_len[9];
+	}
+	SET_EARLYMODE_LEN4(virtualaddress, dwtmp);
+}
+
+static bool rtl8821ae_get_rxdesc_is_ht(struct ieee80211_hw *hw, u8 *pdesc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 rx_rate = 0;
+
+	rx_rate = GET_RX_DESC_RXMCS(pdesc);
+
+	RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD, "rx_rate=0x%02x.\n", rx_rate);
+
+	if ((rx_rate >= DESC_RATEMCS0) && (rx_rate <= DESC_RATEMCS15))
+		return true;
+	return false;
+}
+
+static bool rtl8821ae_get_rxdesc_is_vht(struct ieee80211_hw *hw, u8 *pdesc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	u8 rx_rate = 0;
+
+	rx_rate = GET_RX_DESC_RXMCS(pdesc);
+
+	RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD, "rx_rate=0x%02x.\n", rx_rate);
+
+	if (rx_rate >= DESC_RATEVHT1SS_MCS0)
+		return true;
+	return false;
+}
+
+static u8 rtl8821ae_get_rx_vht_nss(struct ieee80211_hw *hw, u8 *pdesc)
+{
+	u8 rx_rate = 0;
+	u8 vht_nss = 0;
+
+	rx_rate = GET_RX_DESC_RXMCS(pdesc);
+	if ((rx_rate >= DESC_RATEVHT1SS_MCS0) &&
+	    (rx_rate <= DESC_RATEVHT1SS_MCS9))
+		vht_nss = 1;
+	else if ((rx_rate >= DESC_RATEVHT2SS_MCS0) &&
+		 (rx_rate <= DESC_RATEVHT2SS_MCS9))
+		vht_nss = 2;
+
+	return vht_nss;
+}
+
+bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
+			     struct rtl_stats *status,
+			     struct ieee80211_rx_status *rx_status,
+			     u8 *pdesc, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rx_fwinfo_8821ae *p_drvinfo;
+	struct ieee80211_hdr *hdr;
+
+	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
+
+	status->length = (u16)GET_RX_DESC_PKT_LEN(pdesc);
+	status->rx_drvinfo_size = (u8)GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
+	    RX_DRV_INFO_SIZE_UNIT;
+	status->rx_bufshift = (u8)(GET_RX_DESC_SHIFT(pdesc) & 0x03);
+	status->icv = (u16)GET_RX_DESC_ICV(pdesc);
+	status->crc = (u16)GET_RX_DESC_CRC32(pdesc);
+	status->hwerror = (status->crc | status->icv);
+	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
+	status->rate = (u8)GET_RX_DESC_RXMCS(pdesc);
+	status->shortpreamble = (u16)GET_RX_DESC_SPLCP(pdesc);
+	status->isampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->isfirst_ampdu = (bool)(GET_RX_DESC_PAGGR(pdesc) == 1);
+	status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
+	status->rx_packet_bw = GET_RX_DESC_BW(pdesc);
+	status->macid = GET_RX_DESC_MACID(pdesc);
+	status->is_short_gi = !(bool)GET_RX_DESC_SPLCP(pdesc);
+	status->is_ht = rtl8821ae_get_rxdesc_is_ht(hw, pdesc);
+	status->is_vht = rtl8821ae_get_rxdesc_is_vht(hw, pdesc);
+	status->vht_nss = rtl8821ae_get_rx_vht_nss(hw, pdesc);
+	status->is_cck = RTL8821AE_RX_HAL_IS_CCK_RATE(status->rate);
+
+	RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD,
+		 "rx_packet_bw=%s,is_ht %d, is_vht %d, vht_nss=%d,is_short_gi %d.\n",
+		 (status->rx_packet_bw == 2) ? "80M" :
+		 (status->rx_packet_bw == 1) ? "40M" : "20M",
+		 status->is_ht, status->is_vht, status->vht_nss,
+		 status->is_short_gi);
+
+	if (GET_RX_STATUS_DESC_RPT_SEL(pdesc))
+		status->packet_report_type = C2H_PACKET;
+	else
+		status->packet_report_type = NORMAL_RX;
+
+	if (GET_RX_STATUS_DESC_PATTERN_MATCH(pdesc))
+		status->wake_match = BIT(2);
+	else if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
+		status->wake_match = BIT(1);
+	else if (GET_RX_STATUS_DESC_UNICAST_MATCH(pdesc))
+		status->wake_match = BIT(0);
+	else
+		status->wake_match = 0;
+
+	if (status->wake_match)
+		RT_TRACE(rtlpriv, COMP_RXDESC, DBG_LOUD,
+			 "GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",
+			 status->wake_match);
+	rx_status->freq = hw->conf.chandef.chan->center_freq;
+	rx_status->band = hw->conf.chandef.chan->band;
+
+	hdr = (struct ieee80211_hdr *)(skb->data +
+	      status->rx_drvinfo_size + status->rx_bufshift);
+
+	if (status->crc)
+		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
+
+	if (status->rx_packet_bw == HT_CHANNEL_WIDTH_20_40)
+		rx_status->flag |= RX_FLAG_40MHZ;
+	else if (status->rx_packet_bw == HT_CHANNEL_WIDTH_80)
+		rx_status->vht_flag |= RX_VHT_FLAG_80MHZ;
+	if (status->is_ht)
+		rx_status->flag |= RX_FLAG_HT;
+	if (status->is_vht)
+		rx_status->flag |= RX_FLAG_VHT;
+
+	if (status->is_short_gi)
+		rx_status->flag |= RX_FLAG_SHORT_GI;
+
+	rx_status->vht_nss = status->vht_nss;
+	rx_status->flag |= RX_FLAG_MACTIME_START;
+
+	/* hw will set status->decrypted true, if it finds the
+	 * frame is open data frame or mgmt frame.
+	 * So hw will not decryption robust managment frame
+	 * for IEEE80211w but still set status->decrypted
+	 * true, so here we should set it back to undecrypted
+	 * for IEEE80211w frame, and mac80211 sw will help
+	 * to decrypt it
+	 */
+	if (status->decrypted) {
+		if ((!_ieee80211_is_robust_mgmt_frame(hdr)) &&
+		    (ieee80211_has_protected(hdr->frame_control)))
+			rx_status->flag |= RX_FLAG_DECRYPTED;
+		else
+			rx_status->flag &= ~RX_FLAG_DECRYPTED;
+	}
+
+	/* rate_idx: index of data rate into band's
+	 * supported rates or MCS index if HT rates
+	 * are use (RX_FLAG_HT)
+	 */
+	rx_status->rate_idx =
+	  _rtl8821ae_rate_mapping(hw, status->is_ht,
+				  status->is_vht, status->rate);
+
+	rx_status->mactime = status->timestamp_low;
+	if (phystatus) {
+		p_drvinfo = (struct rx_fwinfo_8821ae *)(skb->data +
+			    status->rx_bufshift);
+
+		translate_rx_signal_stuff(hw, skb, status, pdesc, p_drvinfo);
+	}
+	rx_status->signal = status->recvsignalpower + 10;
+	if (status->packet_report_type == TX_REPORT2) {
+		status->macid_valid_entry[0] =
+		  GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
+		status->macid_valid_entry[1] =
+		  GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
+	}
+	return true;
+}
+
+static u8 rtl8821ae_bw_mapping(struct ieee80211_hw *hw,
+			       struct rtl_tcb_desc *ptcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	u8 bw_setting_of_desc = 0;
+
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+		 "rtl8821ae_bw_mapping, current_chan_bw %d, packet_bw %d\n",
+		 rtlphy->current_chan_bw, ptcb_desc->packet_bw);
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80) {
+		if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_80)
+			bw_setting_of_desc = 2;
+		else if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40)
+			bw_setting_of_desc = 1;
+		else
+			bw_setting_of_desc = 0;
+	} else if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		if ((ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40) ||
+		    (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_80))
+			bw_setting_of_desc = 1;
+		else
+			bw_setting_of_desc = 0;
+	} else {
+		bw_setting_of_desc = 0;
+	}
+	return bw_setting_of_desc;
+}
+
+static u8 rtl8821ae_sc_mapping(struct ieee80211_hw *hw,
+			       struct rtl_tcb_desc *ptcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_phy *rtlphy = &rtlpriv->phy;
+	struct rtl_mac *mac = rtl_mac(rtlpriv);
+	u8 sc_setting_of_desc = 0;
+
+	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80) {
+		if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_80) {
+			sc_setting_of_desc = VHT_DATA_SC_DONOT_CARE;
+		} else if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40) {
+			if (mac->cur_80_prime_sc ==
+			    HAL_PRIME_CHNL_OFFSET_LOWER)
+				sc_setting_of_desc =
+					VHT_DATA_SC_40_LOWER_OF_80MHZ;
+			else if (mac->cur_80_prime_sc ==
+				 HAL_PRIME_CHNL_OFFSET_UPPER)
+				sc_setting_of_desc =
+					VHT_DATA_SC_40_UPPER_OF_80MHZ;
+			else
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_LOUD,
+					 "rtl8821ae_sc_mapping: Not Correct Primary40MHz Setting\n");
+		} else {
+			if ((mac->cur_40_prime_sc ==
+			     HAL_PRIME_CHNL_OFFSET_LOWER) &&
+			    (mac->cur_80_prime_sc  ==
+			     HAL_PRIME_CHNL_OFFSET_LOWER))
+				sc_setting_of_desc =
+					VHT_DATA_SC_20_LOWEST_OF_80MHZ;
+			else if ((mac->cur_40_prime_sc ==
+				  HAL_PRIME_CHNL_OFFSET_UPPER) &&
+				 (mac->cur_80_prime_sc ==
+				  HAL_PRIME_CHNL_OFFSET_LOWER))
+				sc_setting_of_desc =
+					VHT_DATA_SC_20_LOWER_OF_80MHZ;
+			else if ((mac->cur_40_prime_sc ==
+				  HAL_PRIME_CHNL_OFFSET_LOWER) &&
+				 (mac->cur_80_prime_sc ==
+				  HAL_PRIME_CHNL_OFFSET_UPPER))
+				sc_setting_of_desc =
+					VHT_DATA_SC_20_UPPER_OF_80MHZ;
+			else if ((mac->cur_40_prime_sc ==
+				  HAL_PRIME_CHNL_OFFSET_UPPER) &&
+				 (mac->cur_80_prime_sc ==
+				  HAL_PRIME_CHNL_OFFSET_UPPER))
+				sc_setting_of_desc =
+					VHT_DATA_SC_20_UPPERST_OF_80MHZ;
+			else
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_LOUD,
+					 "rtl8821ae_sc_mapping: Not Correct Primary40MHz Setting\n");
+		}
+	} else if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
+		if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20_40) {
+			sc_setting_of_desc = VHT_DATA_SC_DONOT_CARE;
+		} else if (ptcb_desc->packet_bw == HT_CHANNEL_WIDTH_20) {
+			if (mac->cur_40_prime_sc ==
+				HAL_PRIME_CHNL_OFFSET_UPPER) {
+				sc_setting_of_desc =
+					VHT_DATA_SC_20_UPPER_OF_80MHZ;
+			} else if (mac->cur_40_prime_sc ==
+				HAL_PRIME_CHNL_OFFSET_LOWER){
+				sc_setting_of_desc =
+					VHT_DATA_SC_20_LOWER_OF_80MHZ;
+			} else {
+				sc_setting_of_desc = VHT_DATA_SC_DONOT_CARE;
+			}
+		}
+	} else {
+		sc_setting_of_desc = VHT_DATA_SC_DONOT_CARE;
+	}
+
+	return sc_setting_of_desc;
+}
+
+void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
+			    struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
+			    struct ieee80211_tx_info *info,
+			    struct ieee80211_sta *sta,
+			    struct sk_buff *skb,
+			    u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
+	u8 *pdesc = (u8 *)pdesc_tx;
+	u16 seq_number;
+	__le16 fc = hdr->frame_control;
+	unsigned int buf_len = 0;
+	unsigned int skb_len = skb->len;
+	u8 fw_qsel = _rtl8821ae_map_hwqueue_to_fwqueue(skb, hw_queue);
+	bool firstseg = ((hdr->seq_ctrl &
+			  cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
+	bool lastseg = ((hdr->frame_control &
+			 cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
+	dma_addr_t mapping;
+	u8 short_gi = 0;
+
+	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
+	rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
+	/* reserve 8 byte for AMPDU early mode */
+	if (rtlhal->earlymode_enable) {
+		skb_push(skb, EM_HDR_LEN);
+		memset(skb->data, 0, EM_HDR_LEN);
+	}
+	buf_len = skb->len;
+	mapping = pci_map_single(rtlpci->pdev, skb->data, skb->len,
+				 PCI_DMA_TODEVICE);
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "DMA mapping error");
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, sizeof(struct tx_desc_8821ae));
+	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
+		firstseg = true;
+		lastseg = true;
+	}
+	if (firstseg) {
+		if (rtlhal->earlymode_enable) {
+			SET_TX_DESC_PKT_OFFSET(pdesc, 1);
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN +
+					   EM_HDR_LEN);
+			if (ptcb_desc->empkt_num) {
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Insert 8 byte.pTcb->EMPktNum:%d\n",
+					  ptcb_desc->empkt_num);
+				_rtl8821ae_insert_emcontent(ptcb_desc,
+					 (u8 *)(skb->data));
+			}
+		} else {
+			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+		}
+
+		/* ptcb_desc->use_driver_rate = true; */
+		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
+		if (ptcb_desc->hw_rate > DESC_RATEMCS0)
+			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
+		else
+			short_gi = (ptcb_desc->use_shortpreamble) ? 1 : 0;
+
+		SET_TX_DESC_DATA_SHORTGI(pdesc, short_gi);
+
+		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
+			SET_TX_DESC_AGG_ENABLE(pdesc, 1);
+			SET_TX_DESC_MAX_AGG_NUM(pdesc, 0x1f);
+		}
+		SET_TX_DESC_SEQ(pdesc, seq_number);
+		SET_TX_DESC_RTS_ENABLE(pdesc, ((ptcb_desc->rts_enable &&
+					!ptcb_desc->cts_enable) ? 1 : 0));
+		SET_TX_DESC_HW_RTS_ENABLE(pdesc, 0);
+		SET_TX_DESC_CTS2SELF(pdesc, ((ptcb_desc->cts_enable) ? 1 : 0));
+
+		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
+		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
+		SET_TX_DESC_RTS_SHORT(pdesc,
+			((ptcb_desc->rts_rate <= DESC_RATE54M) ?
+			(ptcb_desc->rts_use_shortpreamble ? 1 : 0) :
+			(ptcb_desc->rts_use_shortgi ? 1 : 0)));
+
+		if (ptcb_desc->tx_enable_sw_calc_duration)
+			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
+
+		SET_TX_DESC_DATA_BW(pdesc,
+			rtl8821ae_bw_mapping(hw, ptcb_desc));
+
+		SET_TX_DESC_TX_SUB_CARRIER(pdesc,
+			rtl8821ae_sc_mapping(hw, ptcb_desc));
+
+		SET_TX_DESC_LINIP(pdesc, 0);
+		SET_TX_DESC_PKT_SIZE(pdesc, (u16)skb_len);
+		if (sta) {
+			u8 ampdu_density = sta->ht_cap.ampdu_density;
+
+			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
+		}
+		if (info->control.hw_key) {
+			struct ieee80211_key_conf *keyconf =
+				info->control.hw_key;
+			switch (keyconf->cipher) {
+			case WLAN_CIPHER_SUITE_WEP40:
+			case WLAN_CIPHER_SUITE_WEP104:
+			case WLAN_CIPHER_SUITE_TKIP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x1);
+				break;
+			case WLAN_CIPHER_SUITE_CCMP:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x3);
+				break;
+			default:
+				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
+				break;
+			}
+		}
+
+		SET_TX_DESC_QUEUE_SEL(pdesc, fw_qsel);
+		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
+		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
+		SET_TX_DESC_DISABLE_FB(pdesc, ptcb_desc->disable_ratefallback ?
+				       1 : 0);
+		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
+
+		if (ieee80211_is_data_qos(fc)) {
+			if (mac->rdg_en) {
+				RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+					 "Enable RDG function.\n");
+				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
+				SET_TX_DESC_HTC(pdesc, 1);
+			}
+		}
+	}
+
+	SET_TX_DESC_FIRST_SEG(pdesc, (firstseg ? 1 : 0));
+	SET_TX_DESC_LAST_SEG(pdesc, (lastseg ? 1 : 0));
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16)buf_len);
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+	/* if (rtlpriv->dm.useramask) { */
+	if (1) {
+		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	} else {
+		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcb_desc->ratr_index);
+		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
+	}
+	if (!ieee80211_is_data_qos(fc))  {
+		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+		SET_TX_DESC_HWSEQ_SEL(pdesc, 0);
+	}
+	SET_TX_DESC_MORE_FRAG(pdesc, (lastseg ? 0 : 1));
+	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
+	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
+		SET_TX_DESC_BMC(pdesc, 1);
+	}
+
+	rtl8821ae_dm_set_tx_ant_by_tx_info(hw, pdesc, ptcb_desc->mac_id);
+	RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE, "\n");
+}
+
+void rtl8821ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
+			       u8 *pdesc, bool firstseg,
+			       bool lastseg, struct sk_buff *skb)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	u8 fw_queue = QSLT_BEACON;
+
+	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
+					    skb->data, skb->len,
+					    PCI_DMA_TODEVICE);
+
+	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
+		RT_TRACE(rtlpriv, COMP_SEND, DBG_TRACE,
+			 "DMA mapping error");
+		return;
+	}
+	CLEAR_PCI_TX_DESC_CONTENT(pdesc, TX_DESC_SIZE);
+
+	SET_TX_DESC_FIRST_SEG(pdesc, 1);
+	SET_TX_DESC_LAST_SEG(pdesc, 1);
+
+	SET_TX_DESC_PKT_SIZE((u8 *)pdesc, (u16)(skb->len));
+
+	SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
+
+	SET_TX_DESC_USE_RATE(pdesc, 1);
+	SET_TX_DESC_TX_RATE(pdesc, DESC_RATE1M);
+	SET_TX_DESC_DISABLE_FB(pdesc, 1);
+
+	SET_TX_DESC_DATA_BW(pdesc, 0);
+
+	SET_TX_DESC_HWSEQ_EN(pdesc, 1);
+
+	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
+
+	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16)(skb->len));
+
+	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, mapping);
+
+	SET_TX_DESC_MACID(pdesc, 0);
+
+	SET_TX_DESC_OWN(pdesc, 1);
+
+	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
+		      "H2C Tx Cmd Content\n",
+		      pdesc, TX_DESC_SIZE);
+}
+
+void rtl8821ae_set_desc(struct ieee80211_hw *hw, u8 *pdesc,
+			bool istx, u8 desc_name, u8 *val)
+{
+	if (istx) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			SET_TX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_TX_NEXTDESC_ADDR:
+			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *)val);
+			break;
+		default:
+			RT_ASSERT(false,
+				  "ERR txdesc :%d not process\n", desc_name);
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_RXOWN:
+			SET_RX_DESC_OWN(pdesc, 1);
+			break;
+		case HW_DESC_RXBUFF_ADDR:
+			SET_RX_DESC_BUFF_ADDR(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			SET_RX_DESC_PKT_LEN(pdesc, *(u32 *)val);
+			break;
+		case HW_DESC_RXERO:
+			SET_RX_DESC_EOR(pdesc, 1);
+			break;
+		default:
+			RT_ASSERT(false,
+				  "ERR rxdesc :%d not process\n", desc_name);
+			break;
+		}
+	}
+}
+
+u32 rtl8821ae_get_desc(u8 *pdesc, bool istx, u8 desc_name)
+{
+	u32 ret = 0;
+
+	if (istx) {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_TX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_TXBUFF_ADDR:
+			ret = GET_TX_DESC_TX_BUFFER_ADDRESS(pdesc);
+			break;
+		default:
+			RT_ASSERT(false,
+				  "ERR txdesc :%d not process\n", desc_name);
+			break;
+		}
+	} else {
+		switch (desc_name) {
+		case HW_DESC_OWN:
+			ret = GET_RX_DESC_OWN(pdesc);
+			break;
+		case HW_DESC_RXPKT_LEN:
+			ret = GET_RX_DESC_PKT_LEN(pdesc);
+			break;
+		case HW_DESC_RXBUFF_ADDR:
+			ret = GET_RX_DESC_BUFF_ADDR(pdesc);
+			break;
+		default:
+			RT_ASSERT(false,
+				  "ERR rxdesc :%d not process\n", desc_name);
+			break;
+		}
+	}
+	return ret;
+}
+
+bool rtl8821ae_is_tx_desc_closed(struct ieee80211_hw *hw,
+				 u8 hw_queue, u16 index)
+{
+	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
+	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
+	u8 *entry = (u8 *)(&ring->desc[ring->idx]);
+	u8 own = (u8)rtl8821ae_get_desc(entry, true, HW_DESC_OWN);
+
+	/**
+	 *beacon packet will only use the first
+	 *descriptor defautly,and the own may not
+	 *be cleared by the hardware
+	 */
+	if (own)
+		return false;
+	return true;
+}
+
+void rtl8821ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
+{
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	if (hw_queue == BEACON_QUEUE) {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, BIT(4));
+	} else {
+		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG,
+			       BIT(0) << (hw_queue));
+	}
+}
+
+u32 rtl8821ae_rx_command_packet(struct ieee80211_hw *hw,
+				struct rtl_stats status,
+				struct sk_buff *skb)
+{
+	u32 result = 0;
+	struct rtl_priv *rtlpriv = rtl_priv(hw);
+
+	switch (status.packet_report_type) {
+	case NORMAL_RX:
+		result = 0;
+		break;
+	case C2H_PACKET:
+		rtl8821ae_c2h_packet_handler(hw, skb->data, (u8)skb->len);
+		result = 1;
+		RT_TRACE(rtlpriv, COMP_RECV, DBG_LOUD,
+			 "skb->len=%d\n\n", skb->len);
+		break;
+	default:
+		RT_TRACE(rtlpriv, COMP_RECV, DBG_LOUD,
+			 "No this packet type!!\n");
+		break;
+	}
+
+	return result;
+}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.h b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.h
index af017844d82f..31409042d8dd 100644
--- a/drivers/staging/rtl8821ae/rtl8821ae/trx.h
+++ b/drivers/net/wireless/rtlwifi/rtl8821ae/trx.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -40,28 +36,28 @@
 #define USB_HWDESC_HEADER_LEN			40
 #define CRCLENGTH						4
 
-#define SET_TX_DESC_PKT_SIZE(__pdesc, __val) 		\
+#define SET_TX_DESC_PKT_SIZE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 16, __val)
-#define SET_TX_DESC_OFFSET(__pdesc, __val) 			\
+#define SET_TX_DESC_OFFSET(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 16, 8, __val)
-#define SET_TX_DESC_BMC(__pdesc, __val) 			\
+#define SET_TX_DESC_BMC(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 24, 1, __val)
-#define SET_TX_DESC_HTC(__pdesc, __val) 			\
+#define SET_TX_DESC_HTC(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 25, 1, __val)
-#define SET_TX_DESC_LAST_SEG(__pdesc, __val) 		\
+#define SET_TX_DESC_LAST_SEG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 26, 1, __val)
-#define SET_TX_DESC_FIRST_SEG(__pdesc, __val) 		\
+#define SET_TX_DESC_FIRST_SEG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 27, 1, __val)
-#define SET_TX_DESC_LINIP(__pdesc, __val) 			\
+#define SET_TX_DESC_LINIP(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 28, 1, __val)
-#define SET_TX_DESC_NO_ACM(__pdesc, __val) 			\
+#define SET_TX_DESC_NO_ACM(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 29, 1, __val)
-#define SET_TX_DESC_GF(__pdesc, __val) 				\
+#define SET_TX_DESC_GF(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
-#define SET_TX_DESC_OWN(__pdesc, __val) 			\
+#define SET_TX_DESC_OWN(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
 
-#define GET_TX_DESC_PKT_SIZE(__pdesc) 				\
+#define GET_TX_DESC_PKT_SIZE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 0, 16)
 #define GET_TX_DESC_OFFSET(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 16, 8)
@@ -69,11 +65,11 @@
 	LE_BITS_TO_4BYTE(__pdesc, 24, 1)
 #define GET_TX_DESC_HTC(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 25, 1)
-#define GET_TX_DESC_LAST_SEG(__pdesc) 				\
+#define GET_TX_DESC_LAST_SEG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
-#define GET_TX_DESC_FIRST_SEG(__pdesc) 				\
+#define GET_TX_DESC_FIRST_SEG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
-#define GET_TX_DESC_LINIP(__pdesc) 					\
+#define GET_TX_DESC_LINIP(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
 #define GET_TX_DESC_NO_ACM(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
@@ -82,350 +78,340 @@
 #define GET_TX_DESC_OWN(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
 
-#define SET_TX_DESC_MACID(__pdesc, __val) 			\
+#define SET_TX_DESC_MACID(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 0, 7, __val)
-#define SET_TX_DESC_QUEUE_SEL(__pdesc, __val) 		\
+#define SET_TX_DESC_QUEUE_SEL(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 8, 5, __val)
-#define SET_TX_DESC_RDG_NAV_EXT(__pdesc, __val) 	\
+#define SET_TX_DESC_RDG_NAV_EXT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 13, 1, __val)
-#define SET_TX_DESC_LSIG_TXOP_EN(__pdesc, __val) 	\
+#define SET_TX_DESC_LSIG_TXOP_EN(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 14, 1, __val)
-#define SET_TX_DESC_PIFS(__pdesc, __val) 			\
+#define SET_TX_DESC_PIFS(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 15, 1, __val)
-#define SET_TX_DESC_RATE_ID(__pdesc, __val) 		\
+#define SET_TX_DESC_RATE_ID(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 16, 5, __val)
-#define SET_TX_DESC_EN_DESC_ID(__pdesc, __val) 		\
+#define SET_TX_DESC_EN_DESC_ID(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 21, 1, __val)
-#define SET_TX_DESC_SEC_TYPE(__pdesc, __val) 		\
+#define SET_TX_DESC_SEC_TYPE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 22, 2, __val)
-#define SET_TX_DESC_PKT_OFFSET(__pdesc, __val) 		\
+#define SET_TX_DESC_PKT_OFFSET(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+4, 24, 5, __val)
 
-
-#define SET_TX_DESC_PAID(__pdesc, __val) 			\
+#define SET_TX_DESC_PAID(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 0, 9, __val)
-#define SET_TX_DESC_CCA_RTS(__pdesc, __val) 		\
+#define SET_TX_DESC_CCA_RTS(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 10, 2, __val)
 #define SET_TX_DESC_AGG_ENABLE(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 12, 1, __val)
 #define SET_TX_DESC_RDG_ENABLE(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 13, 1, __val)
-#define SET_TX_DESC_BAR_RTY_TH(__pdesc, __val) 		\
+#define SET_TX_DESC_BAR_RTY_TH(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 14, 2, __val)
-#define SET_TX_DESC_AGG_BREAK(__pdesc, __val) 		\
+#define SET_TX_DESC_AGG_BREAK(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 16, 1, __val)
-#define SET_TX_DESC_MORE_FRAG(__pdesc, __val) 		\
+#define SET_TX_DESC_MORE_FRAG(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 17, 1, __val)
-#define SET_TX_DESC_RAW(__pdesc, __val) 			\
+#define SET_TX_DESC_RAW(__pdesc, __val)				\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 18, 1, __val)
-#define SET_TX_DESC_SPE_RPT(__pdesc, __val) 			\
+#define SET_TX_DESC_SPE_RPT(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 19, 1, __val)
-#define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val) 	\
+#define SET_TX_DESC_AMPDU_DENSITY(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 20, 3, __val)
-#define SET_TX_DESC_BT_INT(__pdesc, __val) 	\
+#define SET_TX_DESC_BT_INT(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 23, 1, __val)
-#define SET_TX_DESC_GID(__pdesc, __val) 		\
+#define SET_TX_DESC_GID(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+8, 24, 6, __val)
 
-
-#define SET_TX_DESC_WHEADER_LEN(__pdesc, __val) 	\
+#define SET_TX_DESC_WHEADER_LEN(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 0, 4, __val)
-#define SET_TX_DESC_CHK_EN(__pdesc, __val) 		\
+#define SET_TX_DESC_CHK_EN(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 4, 1, __val)
-#define SET_TX_DESC_EARLY_MODE(__pdesc, __val) 		\
+#define SET_TX_DESC_EARLY_MODE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 5, 1, __val)
-#define SET_TX_DESC_HWSEQ_SEL(__pdesc, __val) 		\
+#define SET_TX_DESC_HWSEQ_SEL(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 6, 2, __val)
-#define SET_TX_DESC_USE_RATE(__pdesc, __val) 		\
+#define SET_TX_DESC_USE_RATE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 8, 1, __val)
-#define SET_TX_DESC_DISABLE_RTS_FB(__pdesc, __val) 		\
+#define SET_TX_DESC_DISABLE_RTS_FB(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 9, 1, __val)
-#define SET_TX_DESC_DISABLE_FB(__pdesc, __val) 		\
+#define SET_TX_DESC_DISABLE_FB(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 10, 1, __val)
-#define SET_TX_DESC_CTS2SELF(__pdesc, __val) 		\
+#define SET_TX_DESC_CTS2SELF(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 11, 1, __val)
-#define SET_TX_DESC_RTS_ENABLE(__pdesc, __val) 		\
+#define SET_TX_DESC_RTS_ENABLE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 12, 1, __val)
-#define SET_TX_DESC_HW_RTS_ENABLE(__pdesc, __val) 		\
+#define SET_TX_DESC_HW_RTS_ENABLE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 13, 1, __val)
-#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val) 		\
+#define SET_TX_DESC_NAV_USE_HDR(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 15, 1, __val)
-#define SET_TX_DESC_USE_MAX_LEN(__pdesc, __val) 		\
+#define SET_TX_DESC_USE_MAX_LEN(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 16, 1, __val)
-#define SET_TX_DESC_MAX_AGG_NUM(__pdesc, __val) 		\
+#define SET_TX_DESC_MAX_AGG_NUM(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 17, 5, __val)
-#define SET_TX_DESC_NDPA(__pdesc, __val) 		\
+#define SET_TX_DESC_NDPA(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 22, 2, __val)
-#define SET_TX_DESC_AMPDU_MAX_TIME(__pdesc, __val) 		\
+#define SET_TX_DESC_AMPDU_MAX_TIME(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+12, 24, 8, __val)
-#define SET_TX_DESC_TX_ANT(__pdesc, __val) 		\
+#define SET_TX_DESC_TX_ANT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 24, 4, __val)
 
-#define SET_TX_DESC_TX_RATE(__pdesc, __val) 		\
+#define SET_TX_DESC_TX_RATE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 0, 7, __val)
-#define SET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc, __val) 		\
+#define SET_TX_DESC_DATA_RATE_FB_LIMIT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 8, 5, __val)
-#define SET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc, __val) 		\
+#define SET_TX_DESC_RTS_RATE_FB_LIMIT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 13, 4, __val)
-#define SET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc, __val) 			\
+#define SET_TX_DESC_RETRY_LIMIT_ENABLE(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 17, 1, __val)
-#define SET_TX_DESC_DATA_RETRY_LIMIT(__pdesc, __val) 		\
+#define SET_TX_DESC_DATA_RETRY_LIMIT(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 18, 6, __val)
-#define SET_TX_DESC_RTS_RATE(__pdesc, __val) 		\
+#define SET_TX_DESC_RTS_RATE(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+16, 24, 5, __val)
 
-
-#define SET_TX_DESC_TX_SUB_CARRIER(__pdesc, __val) 		\
+#define SET_TX_DESC_TX_SUB_CARRIER(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 0, 4, __val)
-#define SET_TX_DESC_DATA_SHORTGI(__pdesc, __val) 	\
-	SET_BITS_TO_LE_1BYTE(__pdesc+20, 6, 1, __val)
-#define SET_TX_DESC_DATA_BW(__pdesc, __val) 		\
+#define SET_TX_DESC_DATA_SHORTGI(__pdesc, __val)	\
+	SET_BITS_TO_LE_1BYTE(__pdesc+20, 4, 1, __val)
+#define SET_TX_DESC_DATA_BW(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 5, 2, __val)
-#define SET_TX_DESC_DATA_LDPC(__pdesc, __val) 	\
+#define SET_TX_DESC_DATA_LDPC(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 7, 1, __val)
-#define SET_TX_DESC_DATA_STBC(__pdesc, __val) 	\
+#define SET_TX_DESC_DATA_STBC(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 8, 2, __val)
-#define SET_TX_DESC_CTROL_STBC(__pdesc, __val) 	\
+#define SET_TX_DESC_CTROL_STBC(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 10, 2, __val)
-#define SET_TX_DESC_RTS_SHORT(__pdesc, __val) 	\
+#define SET_TX_DESC_RTS_SHORT(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 12, 1, __val)
-#define SET_TX_DESC_RTS_SC(__pdesc, __val) 	\
+#define SET_TX_DESC_RTS_SC(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+20, 13, 4, __val)
 
-
-#define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val) 	\
+#define SET_TX_DESC_TX_BUFFER_SIZE(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 16, __val)
 
-#define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc) 		\
+#define GET_TX_DESC_TX_BUFFER_SIZE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+28, 0, 16)
 
-#define SET_TX_DESC_HWSEQ_EN(__pdesc, __val) 	\
+#define SET_TX_DESC_HWSEQ_EN(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+32, 15, 1, __val)
 
-#define SET_TX_DESC_SEQ(__pdesc, __val) 	\
+#define SET_TX_DESC_SEQ(__pdesc, __val)		\
 	SET_BITS_TO_LE_4BYTE(__pdesc+36, 12, 12, __val)
 
-#define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val) 	\
+#define SET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+40, 0, 32, __val)
 
-#define GET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc) 		\
+#define GET_TX_DESC_TX_BUFFER_ADDRESS(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+40, 0, 32)
 
-
-#define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val) 	\
+#define SET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+48, 0, 32, __val)
 
-#define GET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc) 		\
+#define GET_TX_DESC_NEXT_DESC_ADDRESS(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+48, 0, 32)
 
-#define GET_RX_DESC_PKT_LEN(__pdesc) 			\
+#define GET_RX_DESC_PKT_LEN(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc, 0, 14)
-#define GET_RX_DESC_CRC32(__pdesc) 				\
+#define GET_RX_DESC_CRC32(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 14, 1)
-#define GET_RX_DESC_ICV(__pdesc) 				\
+#define GET_RX_DESC_ICV(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 15, 1)
-#define GET_RX_DESC_DRV_INFO_SIZE(__pdesc) 		\
+#define GET_RX_DESC_DRV_INFO_SIZE(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc, 16, 4)
-#define GET_RX_DESC_SECURITY(__pdesc) 			\
+#define GET_RX_DESC_SECURITY(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc, 20, 3)
-#define GET_RX_DESC_QOS(__pdesc) 				\
+#define GET_RX_DESC_QOS(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 23, 1)
-#define GET_RX_DESC_SHIFT(__pdesc) 				\
+#define GET_RX_DESC_SHIFT(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 24, 2)
-#define GET_RX_DESC_PHYST(__pdesc) 				\
+#define GET_RX_DESC_PHYST(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 26, 1)
-#define GET_RX_DESC_SWDEC(__pdesc) 				\
+#define GET_RX_DESC_SWDEC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 27, 1)
-#define GET_RX_DESC_LS(__pdesc) 				\
+#define GET_RX_DESC_LS(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 28, 1)
-#define GET_RX_DESC_FS(__pdesc) 				\
+#define GET_RX_DESC_FS(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc, 29, 1)
-#define GET_RX_DESC_EOR(__pdesc) 				\
+#define GET_RX_DESC_EOR(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 30, 1)
-#define GET_RX_DESC_OWN(__pdesc) 				\
+#define GET_RX_DESC_OWN(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc, 31, 1)
 
-#define SET_RX_DESC_PKT_LEN(__pdesc, __val) 	\
+#define SET_RX_DESC_PKT_LEN(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 0, 14, __val)
-#define SET_RX_DESC_EOR(__pdesc, __val) 		\
+#define SET_RX_DESC_EOR(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 30, 1, __val)
-#define SET_RX_DESC_OWN(__pdesc, __val) 		\
+#define SET_RX_DESC_OWN(__pdesc, __val)			\
 	SET_BITS_TO_LE_4BYTE(__pdesc, 31, 1, __val)
 
-#define GET_RX_DESC_MACID(__pdesc) 				\
+#define GET_RX_DESC_MACID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 0, 7)
-#define GET_RX_DESC_TID(__pdesc) 				\
+#define GET_RX_DESC_TID(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 8, 4)
-#define GET_RX_DESC_AMSDU(__pdesc) 				\
+#define GET_RX_DESC_AMSDU(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 13, 1)
-#define GET_RX_STATUS_DESC_RXID_MATCH(__pdesc) 				\
+#define GET_RX_STATUS_DESC_RXID_MATCH(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 14, 1)
-#define GET_RX_DESC_PAGGR(__pdesc) 				\
+#define GET_RX_DESC_PAGGR(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 15, 1)
-#define GET_RX_DESC_A1_FIT(__pdesc) 			\
+#define GET_RX_DESC_A1_FIT(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 16, 4)
-#define GET_RX_DESC_CHKERR(__pdesc) 			\
+#define GET_RX_DESC_CHKERR(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 20, 1)
-#define GET_RX_DESC_IPVER(__pdesc) 				\
+#define GET_RX_DESC_IPVER(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 21, 1)
-#define GET_RX_STATUS_DESC_IS_TCPUDP(__pdesc) 				\
+#define GET_RX_STATUS_DESC_IS_TCPUDP(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 22, 1)
-#define GET_RX_STATUS_DESC_CHK_VLD(__pdesc) 				\
+#define GET_RX_STATUS_DESC_CHK_VLD(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+4, 23, 1)
 #define GET_RX_DESC_PAM(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 24, 1)
-#define GET_RX_DESC_PWR(__pdesc) 				\
+#define GET_RX_DESC_PWR(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 25, 1)
-#define GET_RX_DESC_MD(__pdesc) 				\
+#define GET_RX_DESC_MD(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 26, 1)
-#define GET_RX_DESC_MF(__pdesc) 				\
+#define GET_RX_DESC_MF(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 27, 1)
-#define GET_RX_DESC_TYPE(__pdesc) 				\
+#define GET_RX_DESC_TYPE(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+4, 28, 2)
-#define GET_RX_DESC_MC(__pdesc) 				\
+#define GET_RX_DESC_MC(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 30, 1)
-#define GET_RX_DESC_BC(__pdesc) 				\
+#define GET_RX_DESC_BC(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+4, 31, 1)
 
-
-#define GET_RX_DESC_SEQ(__pdesc) 				\
+#define GET_RX_DESC_SEQ(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 0, 12)
-#define GET_RX_DESC_FRAG(__pdesc) 				\
+#define GET_RX_DESC_FRAG(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+8, 12, 4)
-#define GET_RX_STATUS_DESC_RX_IS_QOS(__pdesc) 				\
+#define GET_RX_STATUS_DESC_RX_IS_QOS(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+8, 16, 1)
-#define GET_RX_STATUS_DESC_WLANHD_IV_LEN(__pdesc) 				\
+#define GET_RX_STATUS_DESC_WLANHD_IV_LEN(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+8, 18, 6)
-#define GET_RX_STATUS_DESC_RPT_SEL(__pdesc) 				\
+#define GET_RX_STATUS_DESC_RPT_SEL(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+8, 28, 1)
 
-
-#define GET_RX_DESC_RXMCS(__pdesc) 				\
+#define GET_RX_DESC_RXMCS(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 0, 7)
-#define GET_RX_DESC_RXHT(__pdesc) 				\
-	LE_BITS_TO_4BYTE(__pdesc+12, 6, 1)
-#define GET_RX_STATUS_DESC_RX_GF(__pdesc) 				\
-	LE_BITS_TO_4BYTE(__pdesc+12, 7, 1)
-#define GET_RX_DESC_HTC(__pdesc) 				\
+#define GET_RX_DESC_HTC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+12, 10, 1)
 #define GET_RX_STATUS_DESC_EOSP(__pdesc)		\
-	LE_BITS_TO_4BYTE( __pdesc+12, 11, 1)
+	LE_BITS_TO_4BYTE(__pdesc+12, 11, 1)
 #define GET_RX_STATUS_DESC_BSSID_FIT(__pdesc)		\
-	LE_BITS_TO_4BYTE( __pdesc+12, 12, 2)
+	LE_BITS_TO_4BYTE(__pdesc+12, 12, 2)
 
 #define GET_RX_STATUS_DESC_PATTERN_MATCH(__pdesc)	\
-	LE_BITS_TO_4BYTE( __pdesc+12, 29, 1)
+	LE_BITS_TO_4BYTE(__pdesc+12, 29, 1)
 #define GET_RX_STATUS_DESC_UNICAST_MATCH(__pdesc)	\
-	LE_BITS_TO_4BYTE( __pdesc+12, 30, 1)
+	LE_BITS_TO_4BYTE(__pdesc+12, 30, 1)
 #define GET_RX_STATUS_DESC_MAGIC_MATCH(__pdesc)	\
-	LE_BITS_TO_4BYTE( __pdesc+12, 31, 1)
+	LE_BITS_TO_4BYTE(__pdesc+12, 31, 1)
 
-#define GET_RX_DESC_SPLCP(__pdesc) 				\
+#define GET_RX_DESC_SPLCP(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 0, 1)
-#define GET_RX_STATUS_DESC_LDPC(__pdesc) 				\
+#define GET_RX_STATUS_DESC_LDPC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 1, 1)
-#define GET_RX_STATUS_DESC_STBC(__pdesc) 				\
+#define GET_RX_STATUS_DESC_STBC(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+16, 2, 1)
-#define GET_RX_DESC_BW(__pdesc) 				\
+#define GET_RX_DESC_BW(__pdesc)					\
 	LE_BITS_TO_4BYTE(__pdesc+16, 4, 2)
 
-#define GET_RX_DESC_TSFL(__pdesc) 				\
+#define GET_RX_DESC_TSFL(__pdesc)				\
 	LE_BITS_TO_4BYTE(__pdesc+20, 0, 32)
 
-#define GET_RX_DESC_BUFF_ADDR(__pdesc) 			\
+#define GET_RX_DESC_BUFF_ADDR(__pdesc)			\
 	LE_BITS_TO_4BYTE(__pdesc+24, 0, 32)
-#define GET_RX_DESC_BUFF_ADDR64(__pdesc) 		\
+#define GET_RX_DESC_BUFF_ADDR64(__pdesc)		\
 	LE_BITS_TO_4BYTE(__pdesc+28, 0, 32)
 
-#define SET_RX_DESC_BUFF_ADDR(__pdesc, __val) 	\
+#define SET_RX_DESC_BUFF_ADDR(__pdesc, __val)	\
 	SET_BITS_TO_LE_4BYTE(__pdesc+24, 0, 32, __val)
 #define SET_RX_DESC_BUFF_ADDR64(__pdesc, __val) \
 	SET_BITS_TO_LE_4BYTE(__pdesc+28, 0, 32, __val)
 
-
 /* TX report 2 format in Rx desc*/
 
-#define GET_RX_RPT2_DESC_PKT_LEN(__pRxStatusDesc)	\
-	LE_BITS_TO_4BYTE( __pRxStatusDesc, 0, 9)
-#define GET_RX_RPT2_DESC_MACID_VALID_1(__pRxStatusDesc)	\
-	LE_BITS_TO_4BYTE( __pRxStatusDesc+16, 0, 32)
-#define GET_RX_RPT2_DESC_MACID_VALID_2(__pRxStatusDesc)	\
-	LE_BITS_TO_4BYTE( __pRxStatusDesc+20, 0, 32)
+#define GET_RX_RPT2_DESC_PKT_LEN(__status)	\
+	LE_BITS_TO_4BYTE(__status, 0, 9)
+#define GET_RX_RPT2_DESC_MACID_VALID_1(__status)	\
+	LE_BITS_TO_4BYTE(__status+16, 0, 32)
+#define GET_RX_RPT2_DESC_MACID_VALID_2(__status)	\
+	LE_BITS_TO_4BYTE(__status+20, 0, 32)
 
-#define SET_EARLYMODE_PKTNUM(__paddr, __value) 	\
+#define SET_EARLYMODE_PKTNUM(__paddr, __value)	\
 	SET_BITS_TO_LE_4BYTE(__paddr, 0, 4, __value)
-#define SET_EARLYMODE_LEN0(__paddr, __value) 	\
+#define SET_EARLYMODE_LEN0(__paddr, __value)	\
 	SET_BITS_TO_LE_4BYTE(__paddr, 4, 12, __value)
-#define SET_EARLYMODE_LEN1(__paddr, __value) 	\
+#define SET_EARLYMODE_LEN1(__paddr, __value)	\
 	SET_BITS_TO_LE_4BYTE(__paddr, 16, 12, __value)
-#define SET_EARLYMODE_LEN2_1(__paddr, __value) 	\
+#define SET_EARLYMODE_LEN2_1(__paddr, __value)	\
 	SET_BITS_TO_LE_4BYTE(__paddr, 28, 4, __value)
-#define SET_EARLYMODE_LEN2_2(__paddr, __value) 	\
+#define SET_EARLYMODE_LEN2_2(__paddr, __value)	\
 	SET_BITS_TO_LE_4BYTE(__paddr+4, 0, 8, __value)
-#define SET_EARLYMODE_LEN3(__paddr, __value) 	\
+#define SET_EARLYMODE_LEN3(__paddr, __value)	\
 	SET_BITS_TO_LE_4BYTE(__paddr+4, 8, 12, __value)
-#define SET_EARLYMODE_LEN4(__paddr, __value) 	\
+#define SET_EARLYMODE_LEN4(__paddr, __value)	\
 	SET_BITS_TO_LE_4BYTE(__paddr+4, 20, 12, __value)
 
 #define CLEAR_PCI_TX_DESC_CONTENT(__pdesc, _size)		\
 do {								\
-	if(_size > TX_DESC_NEXT_DESC_OFFSET)			\
+	if (_size > TX_DESC_NEXT_DESC_OFFSET)			\
 		memset(__pdesc, 0, TX_DESC_NEXT_DESC_OFFSET);	\
 	else							\
 		memset(__pdesc, 0, _size);			\
-} while (0);
+} while (0)
 
-#define RX_HAL_IS_CCK_RATE(rxmcs)\
+#define RTL8821AE_RX_HAL_IS_CCK_RATE(rxmcs)\
 	(rxmcs == DESC_RATE1M ||\
 	 rxmcs == DESC_RATE2M ||\
 	 rxmcs == DESC_RATE5_5M ||\
 	 rxmcs == DESC_RATE11M)
 
-#define IS_LITTLE_ENDIAN	1
-
 struct phy_rx_agc_info_t {
-	#if IS_LITTLE_ENDIAN
-		u8	gain:7,trsw:1;
+	#ifdef __LITTLE_ENDIAN
+		u8	gain:7, trsw:1;
 	#else
-		u8	trsw:1,gain:7;
+		u8	trsw:1, gain:7;
 	#endif
 };
-struct phy_status_rpt{
-	struct phy_rx_agc_info_t path_agc[2];
-	u8	ch_corr[2];
-	u8	cck_sig_qual_ofdm_pwdb_all;
-	u8	cck_agc_rpt_ofdm_cfosho_a;
-	u8	cck_rpt_b_ofdm_cfosho_b;
-	u8	rsvd_1;//ch_corr_msb;
-	u8	noise_power_db_msb;
-	u8	path_cfotail[2];
-	u8	pcts_mask[2];
-	u8	stream_rxevm[2];
-	u8	path_rxsnr[2];
-	u8 	noise_power_db_lsb;
-	u8	rsvd_2[3];
-	u8 	stream_csi[2];
-	u8 	stream_target_csi[2];
-	u8 	sig_evm;
-	u8 	rsvd_3;
-#if IS_LITTLE_ENDIAN
-	u8 	antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
-	u8 	sgi_en:1;
-	u8 	rxsc:2;
-	u8 	idle_long:1;
-	u8 	r_ant_train_en:1;
-	u8 	ant_sel_b:1;
-	u8 	ant_sel:1;
-#else	/* _BIG_ENDIAN_	*/
-	u8 	ant_sel:1;
-	u8 	ant_sel_b:1;
-	u8 	r_ant_train_en:1;
-	u8 	idle_long:1;
-	u8 	rxsc:2;
-	u8 	sgi_en:1;
-	u8 	antsel_rx_keep_2:1;	/*ex_intf_flg:1;*/
+
+struct phy_status_rpt {
+	/* DWORD 0 */
+	u8 gain_trsw[2];
+#ifdef __LITTLE_ENDIAN
+	u16 chl_num:10;
+	u16 sub_chnl:4;
+	u16 r_rfmod:2;
+#else	/* _BIG_ENDIAN_ */
+	u16 r_rfmod:2;
+	u16 sub_chnl:4;
+	u16 chl_num:10;
 #endif
-}__packed;
+	/* DWORD 1 */
+	u8 pwdb_all;
+	u8 cfosho[4];	/* DW 1 byte 1 DW 2 byte 0 */
+
+	/* DWORD 2 */
+	char cfotail[4];	/* DW 2 byte 1 DW 3 byte 0 */
+
+	/* DWORD 3 */
+	char rxevm[2];	/* DW 3 byte 1 DW 3 byte 2 */
+	char rxsnr[2];	/* DW 3 byte 3 DW 4 byte 0 */
+
+	/* DWORD 4 */
+	u8 pcts_msk_rpt[2];
+	u8 pdsnr[2];	/* DW 4 byte 3 DW 5 Byte 0 */
+
+	/* DWORD 5 */
+	u8 csi_current[2];
+	u8 rx_gain_c;
+
+	/* DWORD 6 */
+	u8 rx_gain_d;
+	u8 sigevm;
+	u8 resvd_0;
+	u8 antidx_anta:3;
+	u8 antidx_antb:3;
+	u8 resvd_1:2;
+} __packed;
 
 struct rx_fwinfo_8821ae {
 	u8 gain_trsw[4];
@@ -610,21 +596,25 @@ struct rx_desc_8821ae {
 } __packed;
 
 void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
-			  struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb,
-			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
+			    struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
+			    struct ieee80211_tx_info *info,
+			    struct ieee80211_sta *sta,
+			    struct sk_buff *skb,
+			    u8 hw_queue, struct rtl_tcb_desc *ptcb_desc);
 bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
-			   struct rtl_stats *status,
-			   struct ieee80211_rx_status *rx_status,
-			   u8 *pdesc, struct sk_buff *skb);
-void rtl8821ae_set_desc(struct ieee80211_hw * hw, u8 *pdesc, bool istx, u8 desc_name, u8 *val);
+			     struct rtl_stats *status,
+			     struct ieee80211_rx_status *rx_status,
+			     u8 *pdesc, struct sk_buff *skb);
+void rtl8821ae_set_desc(struct ieee80211_hw *hw, u8 *pdesc,
+			bool istx, u8 desc_name, u8 *val);
 u32 rtl8821ae_get_desc(u8 *pdesc, bool istx, u8 desc_name);
 bool rtl8821ae_is_tx_desc_closed(struct ieee80211_hw *hw,
 				 u8 hw_queue, u16 index);
 void rtl8821ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue);
 void rtl8821ae_tx_fill_cmddesc(struct ieee80211_hw *hw, u8 *pdesc,
-			     bool b_firstseg, bool b_lastseg,
-			     struct sk_buff *skb);
+			       bool firstseg, bool lastseg,
+			       struct sk_buff *skb);
+u32 rtl8821ae_rx_command_packet(struct ieee80211_hw *hw,
+				struct rtl_stats status,
+				struct sk_buff *skb);
 #endif
diff --git a/drivers/net/wireless/rtlwifi/stats.c b/drivers/net/wireless/rtlwifi/stats.c
index 4f083fc1d360..2d0736a09fc0 100644
--- a/drivers/net/wireless/rtlwifi/stats.c
+++ b/drivers/net/wireless/rtlwifi/stats.c
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -59,8 +55,23 @@ u8 rtl_evm_db_to_percentage(char value)
 }
 EXPORT_SYMBOL(rtl_evm_db_to_percentage);
 
+u8 rtl_evm_dbm_jaguar(char value)
+{
+	char ret_val = value;
+
+	/* -33dB~0dB to 33dB ~ 0dB*/
+	if (ret_val == -128)
+		ret_val = 127;
+	else if (ret_val < 0)
+		ret_val = 0 - ret_val;
+
+	ret_val  = ret_val >> 1;
+	return ret_val;
+}
+EXPORT_SYMBOL(rtl_evm_dbm_jaguar);
+
 static long rtl_translate_todbm(struct ieee80211_hw *hw,
-				u8 signal_strength_index)
+			 u8 signal_strength_index)
 {
 	long signal_power;
 
@@ -106,6 +117,10 @@ static void rtl_process_ui_rssi(struct ieee80211_hw *hw,
 	u8 rfpath;
 	u32 last_rssi, tmpval;
 
+	if (!pstatus->packet_toself && !pstatus->packet_beacon)
+		return;
+
+	rtlpriv->stats.pwdb_all_cnt += pstatus->rx_pwdb_all;
 	rtlpriv->stats.rssi_calculate_cnt++;
 
 	if (rtlpriv->stats.ui_rssi.total_num++ >= PHY_RSSI_SLID_WIN_MAX) {
@@ -151,6 +166,12 @@ static void rtl_process_ui_rssi(struct ieee80211_hw *hw,
 			     (pstatus->rx_mimo_signalstrength[rfpath])) /
 			    (RX_SMOOTH_FACTOR);
 		}
+		rtlpriv->stats.rx_snr_db[rfpath] = pstatus->rx_snr[rfpath];
+		rtlpriv->stats.rx_evm_dbm[rfpath] =
+					pstatus->rx_mimo_evm_dbm[rfpath];
+		rtlpriv->stats.rx_cfo_short[rfpath] =
+					pstatus->cfo_short[rfpath];
+		rtlpriv->stats.rx_cfo_tail[rfpath] = pstatus->cfo_tail[rfpath];
 	}
 }
 
@@ -176,7 +197,6 @@ static void rtl_process_pwdb(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
 	struct rtl_sta_info *drv_priv = NULL;
 	struct ieee80211_sta *sta = NULL;
 	long undec_sm_pwdb;
-	long undec_sm_cck;
 
 	rcu_read_lock();
 	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
@@ -186,33 +206,21 @@ static void rtl_process_pwdb(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
 	if (sta) {
 		drv_priv = (struct rtl_sta_info *) sta->drv_priv;
 		undec_sm_pwdb = drv_priv->rssi_stat.undec_sm_pwdb;
-		undec_sm_cck = drv_priv->rssi_stat.undec_sm_cck;
 	} else {
 		undec_sm_pwdb = rtlpriv->dm.undec_sm_pwdb;
-		undec_sm_cck = rtlpriv->dm.undec_sm_cck;
 	}
 
 	if (undec_sm_pwdb < 0)
 		undec_sm_pwdb = pstatus->rx_pwdb_all;
-	if (undec_sm_cck < 0)
-		undec_sm_cck = pstatus->rx_pwdb_all;
 	if (pstatus->rx_pwdb_all > (u32) undec_sm_pwdb) {
 		undec_sm_pwdb = (((undec_sm_pwdb) *
 		      (RX_SMOOTH_FACTOR - 1)) +
 		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
 		undec_sm_pwdb = undec_sm_pwdb + 1;
 	} else {
-		undec_sm_pwdb = (((undec_sm_pwdb) * (RX_SMOOTH_FACTOR - 1)) +
-		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-	}
-	if (pstatus->rx_pwdb_all > (u32) undec_sm_cck) {
-		undec_sm_cck = (((undec_sm_pwdb) *
+		undec_sm_pwdb = (((undec_sm_pwdb) *
 		      (RX_SMOOTH_FACTOR - 1)) +
 		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-		undec_sm_cck = undec_sm_cck + 1;
-	} else {
-		undec_sm_pwdb = (((undec_sm_cck) * (RX_SMOOTH_FACTOR - 1)) +
-		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
 	}
 
 	if (sta) {
@@ -245,7 +253,7 @@ static void rtl_process_ui_link_quality(struct ieee80211_hw *hw,
 	rtlpriv->stats.ui_link_quality.total_val += pstatus->signalquality;
 	rtlpriv->stats.ui_link_quality.elements[
 		rtlpriv->stats.ui_link_quality.index++] =
-						 pstatus->signalquality;
+							pstatus->signalquality;
 	if (rtlpriv->stats.ui_link_quality.index >=
 	    PHY_LINKQUALITY_SLID_WIN_MAX)
 		rtlpriv->stats.ui_link_quality.index = 0;
@@ -269,7 +277,7 @@ static void rtl_process_ui_link_quality(struct ieee80211_hw *hw,
 }
 
 void rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
-	struct rtl_stats *pstatus)
+			 struct rtl_stats *pstatus)
 {
 
 	if (!pstatus->packet_matchbssid)
diff --git a/drivers/net/wireless/rtlwifi/stats.h b/drivers/net/wireless/rtlwifi/stats.h
index 0dbdc5203830..aa4eec80ccf7 100644
--- a/drivers/net/wireless/rtlwifi/stats.h
+++ b/drivers/net/wireless/rtlwifi/stats.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -39,8 +35,9 @@
 
 u8 rtl_query_rxpwrpercentage(char antpower);
 u8 rtl_evm_db_to_percentage(char value);
+u8 rtl_evm_dbm_jaguar(char value);
 long rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig);
 void rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
-	struct rtl_stats *pstatus);
+			 struct rtl_stats *pstatus);
 
 #endif
diff --git a/drivers/net/wireless/rtlwifi/usb.c b/drivers/net/wireless/rtlwifi/usb.c
index 0398d3ea15b0..10cf69c4bc42 100644
--- a/drivers/net/wireless/rtlwifi/usb.c
+++ b/drivers/net/wireless/rtlwifi/usb.c
@@ -75,11 +75,11 @@ static int _usbctrl_vendorreq_async_write(struct usb_device *udev, u8 request,
 	pipe = usb_sndctrlpipe(udev, 0); /* write_out */
 	reqtype =  REALTEK_USB_VENQT_WRITE;
 
-	dr = kmalloc(sizeof(*dr), GFP_ATOMIC);
+	dr = kzalloc(sizeof(*dr), GFP_ATOMIC);
 	if (!dr)
 		return -ENOMEM;
 
-	databuf = kmalloc(databuf_maxlen, GFP_ATOMIC);
+	databuf = kzalloc(databuf_maxlen, GFP_ATOMIC);
 	if (!databuf) {
 		kfree(dr);
 		return -ENOMEM;
diff --git a/drivers/net/wireless/rtlwifi/wifi.h b/drivers/net/wireless/rtlwifi/wifi.h
index 407a7936d364..6866dcf24340 100644
--- a/drivers/net/wireless/rtlwifi/wifi.h
+++ b/drivers/net/wireless/rtlwifi/wifi.h
@@ -11,10 +11,6 @@
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
  *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
  * The full GNU General Public License is included in this distribution in the
  * file called LICENSE.
  *
@@ -147,7 +143,27 @@
 #define FCS_LEN				4
 #define EM_HDR_LEN			8
 
+enum rtl8192c_h2c_cmd {
+	H2C_AP_OFFLOAD = 0,
+	H2C_SETPWRMODE = 1,
+	H2C_JOINBSSRPT = 2,
+	H2C_RSVDPAGE = 3,
+	H2C_RSSI_REPORT = 5,
+	H2C_RA_MASK = 6,
+	H2C_MACID_PS_MODE = 7,
+	H2C_P2P_PS_OFFLOAD = 8,
+	H2C_MAC_MODE_SEL = 9,
+	H2C_PWRM = 15,
+	H2C_P2P_PS_CTW_CMD = 24,
+	MAX_H2CCMD
+};
+
 #define MAX_TX_COUNT			4
+#define MAX_REGULATION_NUM		4
+#define MAX_RF_PATH_NUM			4
+#define MAX_RATE_SECTION_NUM		6
+#define MAX_2_4G_BANDWITH_NUM		4
+#define MAX_5G_BANDWITH_NUM		4
 #define	MAX_RF_PATH			4
 #define	MAX_CHNL_GROUP_24G		6
 #define	MAX_CHNL_GROUP_5G		14
@@ -163,6 +179,12 @@
 #define DEL_SW_IDX_SZ		30
 #define BAND_NUM			3
 
+/* For now, it's just for 8192ee
+ * but not OK yet, keep it 0
+ */
+#define DMA_IS_64BIT 0
+#define RTL8192EE_SEG_NUM		1 /* 0:2 seg, 1: 4 seg, 2: 8 seg */
+
 enum rf_tx_num {
 	RF_1TX = 0,
 	RF_2TX,
@@ -170,6 +192,36 @@ enum rf_tx_num {
 	RF_TX_NUM_NONIMPLEMENT,
 };
 
+#define PACKET_NORMAL			0
+#define PACKET_DHCP			1
+#define PACKET_ARP			2
+#define PACKET_EAPOL			3
+
+#define	MAX_SUPPORT_WOL_PATTERN_NUM	16
+#define	RSVD_WOL_PATTERN_NUM		1
+#define	WKFMCAM_ADDR_NUM		6
+#define	WKFMCAM_SIZE			24
+
+#define	MAX_WOL_BIT_MASK_SIZE		16
+/* MIN LEN keeps 13 here */
+#define	MIN_WOL_PATTERN_SIZE		13
+#define	MAX_WOL_PATTERN_SIZE		128
+
+#define	WAKE_ON_MAGIC_PACKET		BIT(0)
+#define	WAKE_ON_PATTERN_MATCH		BIT(1)
+
+#define	WOL_REASON_PTK_UPDATE		BIT(0)
+#define	WOL_REASON_GTK_UPDATE		BIT(1)
+#define	WOL_REASON_DISASSOC		BIT(2)
+#define	WOL_REASON_DEAUTH		BIT(3)
+#define	WOL_REASON_AP_LOST		BIT(4)
+#define	WOL_REASON_MAGIC_PKT		BIT(5)
+#define	WOL_REASON_UNICAST_PKT		BIT(6)
+#define	WOL_REASON_PATTERN_PKT		BIT(7)
+#define	WOL_REASON_RTD3_SSID_MATCH	BIT(8)
+#define	WOL_REASON_REALWOW_V2_WAKEUPPKT	BIT(9)
+#define	WOL_REASON_REALWOW_V2_ACKLOST	BIT(10)
+
 struct txpower_info_2g {
 	u8 index_cck_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
 	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
@@ -213,6 +265,15 @@ enum radio_path {
 	RF90_PATH_D = 3,
 };
 
+enum regulation_txpwr_lmt {
+	TXPWR_LMT_FCC = 0,
+	TXPWR_LMT_MKK = 1,
+	TXPWR_LMT_ETSI = 2,
+	TXPWR_LMT_WW = 3,
+
+	TXPWR_LMT_MAX_REGULATION_NUM = 4
+};
+
 enum rt_eeprom_type {
 	EEPROM_93C46,
 	EEPROM_93C56,
@@ -234,8 +295,9 @@ enum hardware_type {
 	HARDWARE_TYPE_RTL8192DU,
 	HARDWARE_TYPE_RTL8723AE,
 	HARDWARE_TYPE_RTL8723U,
-	HARDWARE_TYPE_RTL8723BE,
 	HARDWARE_TYPE_RTL8188EE,
+	HARDWARE_TYPE_RTL8723BE,
+	HARDWARE_TYPE_RTL8192EE,
 	HARDWARE_TYPE_RTL8821AE,
 	HARDWARE_TYPE_RTL8812AE,
 
@@ -268,13 +330,7 @@ enum hardware_type {
 #define	IS_HARDWARE_TYPE_8723(rtlhal)			\
 (IS_HARDWARE_TYPE_8723E(rtlhal) || IS_HARDWARE_TYPE_8723U(rtlhal))
 
-#define RX_HAL_IS_CCK_RATE(_pdesc)\
-	(_pdesc->rxmcs == DESC92_RATE1M ||		\
-	 _pdesc->rxmcs == DESC92_RATE2M ||		\
-	 _pdesc->rxmcs == DESC92_RATE5_5M ||		\
-	 _pdesc->rxmcs == DESC92_RATE11M)
-
-#define RTL8723E_RX_HAL_IS_CCK_RATE(rxmcs)		\
+#define RX_HAL_IS_CCK_RATE(rxmcs)			\
 	((rxmcs) == DESC92_RATE1M ||			\
 	 (rxmcs) == DESC92_RATE2M ||			\
 	 (rxmcs) == DESC92_RATE5_5M ||			\
@@ -339,6 +395,7 @@ enum hw_variables {
 	HW_VAR_DEFAULTKEY2,
 	HW_VAR_DEFAULTKEY3,
 	HW_VAR_SIFS,
+	HW_VAR_R2T_SIFS,
 	HW_VAR_DIFS,
 	HW_VAR_EIFS,
 	HW_VAR_SLOT_TIME,
@@ -390,6 +447,7 @@ enum hw_variables {
 	HW_VAR_H2C_FW_MEDIASTATUSRPT,
 	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
 	HW_VAR_FW_PSMODE_STATUS,
+	HW_VAR_INIT_RTS_RATE,
 	HW_VAR_RESUME_CLK_ON,
 	HW_VAR_FW_LPS_ACTION,
 	HW_VAR_1X1_RECV_COMBINE,
@@ -428,7 +486,7 @@ enum hw_variables {
 	HW_VAR_DATA_FILTER,
 };
 
-enum _RT_MEDIA_STATUS {
+enum rt_media_status {
 	RT_MEDIA_DISCONNECT = 0,
 	RT_MEDIA_CONNECT = 1
 };
@@ -630,6 +688,7 @@ enum rtl_var_map {
 	RTL_IMR_VIDOK,		/*AC_VI DMA OK Interrupt */
 	RTL_IMR_VODOK,		/*AC_VO DMA Interrupt */
 	RTL_IMR_ROK,		/*Receive DMA OK Interrupt */
+	RTL_IMR_HSISR_IND,	/*HSISR Interrupt*/
 	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BCNINT | RTL_IMR_TBDOK |
 				 * RTL_IMR_TBDER) */
 	RTL_IMR_C2HCMD,		/*fw interrupt*/
@@ -653,6 +712,13 @@ enum rtl_var_map {
 	RTL_RC_HT_RATEMCS7,
 	RTL_RC_HT_RATEMCS15,
 
+	RTL_RC_VHT_RATE_1SS_MCS7,
+	RTL_RC_VHT_RATE_1SS_MCS8,
+	RTL_RC_VHT_RATE_1SS_MCS9,
+	RTL_RC_VHT_RATE_2SS_MCS7,
+	RTL_RC_VHT_RATE_2SS_MCS8,
+	RTL_RC_VHT_RATE_2SS_MCS9,
+
 	/*keep it last */
 	RTL_VAR_MAP_MAX,
 };
@@ -744,7 +810,9 @@ enum wireless_mode {
 	WIRELESS_MODE_N_24G = 0x10,
 	WIRELESS_MODE_N_5G = 0x20,
 	WIRELESS_MODE_AC_5G = 0x40,
-	WIRELESS_MODE_AC_24G  = 0x80
+	WIRELESS_MODE_AC_24G  = 0x80,
+	WIRELESS_MODE_AC_ONLY = 0x100,
+	WIRELESS_MODE_MAX = 0x800
 };
 
 #define IS_WIRELESS_MODE_A(wirelessmode)	\
@@ -798,6 +866,30 @@ enum rt_polarity_ctl {
 	RT_POLARITY_HIGH_ACT = 1,
 };
 
+/* After 8188E, we use V2 reason define. 88C/8723A use V1 reason. */
+enum fw_wow_reason_v2 {
+	FW_WOW_V2_PTK_UPDATE_EVENT = 0x01,
+	FW_WOW_V2_GTK_UPDATE_EVENT = 0x02,
+	FW_WOW_V2_DISASSOC_EVENT = 0x04,
+	FW_WOW_V2_DEAUTH_EVENT = 0x08,
+	FW_WOW_V2_FW_DISCONNECT_EVENT = 0x10,
+	FW_WOW_V2_MAGIC_PKT_EVENT = 0x21,
+	FW_WOW_V2_UNICAST_PKT_EVENT = 0x22,
+	FW_WOW_V2_PATTERN_PKT_EVENT = 0x23,
+	FW_WOW_V2_RTD3_SSID_MATCH_EVENT = 0x24,
+	FW_WOW_V2_REALWOW_V2_WAKEUPPKT = 0x30,
+	FW_WOW_V2_REALWOW_V2_ACKLOST = 0x31,
+	FW_WOW_V2_REASON_MAX = 0xff,
+};
+
+enum wolpattern_type {
+	UNICAST_PATTERN = 0,
+	MULTICAST_PATTERN = 1,
+	BROADCAST_PATTERN = 2,
+	DONT_CARE_DA = 3,
+	UNKNOWN_TYPE = 4,
+};
+
 struct octet_string {
 	u8 *octet;
 	u16 length;
@@ -898,6 +990,7 @@ struct wireless_stats {
 	long last_sigstrength_inpercent;
 
 	u32 rssi_calculate_cnt;
+	u32 pwdb_all_cnt;
 
 	/*Transformed, in dbm. Beautified signal
 	   strength for UI, not correct. */
@@ -1106,6 +1199,8 @@ struct rtl_phy {
 
 	u8 pwrgroup_cnt;
 	u8 cck_high_power;
+	/* this is for 88E & 8723A */
+	u32 mcs_txpwrlevel_origoffset[MAX_PG_GROUP][16];
 	/* MAX_PG_GROUP groups of pwr diff by rates */
 	u32 mcs_offset[MAX_PG_GROUP][16];
 	u32 tx_power_by_rate_offset[TX_PWR_BY_RATE_NUM_BAND]
@@ -1126,6 +1221,17 @@ struct rtl_phy {
 	u8 cur_bw20_txpwridx;
 	u8 cur_bw40_txpwridx;
 
+	char txpwr_limit_2_4g[MAX_REGULATION_NUM]
+			     [MAX_2_4G_BANDWITH_NUM]
+			     [MAX_RATE_SECTION_NUM]
+			     [CHANNEL_MAX_NUMBER_2G]
+			     [MAX_RF_PATH_NUM];
+	char txpwr_limit_5g[MAX_REGULATION_NUM]
+			   [MAX_5G_BANDWITH_NUM]
+			   [MAX_RATE_SECTION_NUM]
+			   [CHANNEL_MAX_NUMBER_5G]
+			   [MAX_RF_PATH_NUM];
+
 	u32 rfreg_chnlval[2];
 	bool apk_done;
 	u32 reg_rf3c[2];	/* pathA / pathB  */
@@ -1249,11 +1355,22 @@ struct rtl_mac {
 	/* skb wait queue */
 	struct sk_buff_head skb_waitq[MAX_TID_COUNT];
 
+	u8 ht_stbc_cap;
+	u8 ht_cur_stbc;
+
+	/*vht support*/
+	u8 vht_enable;
+	u8 bw_80;
+	u8 vht_cur_ldpc;
+	u8 vht_cur_stbc;
+	u8 vht_stbc_cap;
+	u8 vht_ldpc_cap;
+
 	/*RDG*/
 	bool rdg_en;
 
 	/*AP*/
-	u8 bssid[6];
+	u8 bssid[ETH_ALEN] __aligned(2);
 	u32 vendor;
 	u8 mcs[16];	/* 16 bytes mcs for HT rates. */
 	u32 basic_rates; /* b/g rates */
@@ -1261,7 +1378,7 @@ struct rtl_mac {
 	u8 sgi_40;
 	u8 sgi_20;
 	u8 bw_40;
-	u8 mode;		/* wireless mode */
+	u16 mode;		/* wireless mode */
 	u8 slot_time;
 	u8 short_preamble;
 	u8 use_cts_protect;
@@ -1358,6 +1475,18 @@ struct rtl_hal {
 	u32 version;		/*version of chip */
 	u8 state;		/*stop 0, start 1 */
 	u8 board_type;
+	u8 external_pa;
+
+	u8 pa_mode;
+	u8 pa_type_2g;
+	u8 pa_type_5g;
+	u8 lna_type_2g;
+	u8 lna_type_5g;
+	u8 external_pa_2g;
+	u8 external_lna_2g;
+	u8 external_pa_5g;
+	u8 external_lna_5g;
+	u8 rfe_type;
 
 	/*firmware */
 	u32 fwsize;
@@ -1413,6 +1542,20 @@ struct rtl_hal {
 
 	u16 rx_tag;/*for 92ee*/
 	u8 rts_en;
+
+	/*for wowlan*/
+	bool wow_enable;
+	bool enter_pnp_sleep;
+	bool wake_from_pnp_sleep;
+	bool wow_enabled;
+	__kernel_time_t last_suspend_sec;
+	u32 wowlan_fwsize;
+	u8 *wowlan_firmware;
+
+	u8 hw_rof_enable; /*Enable GPIO[9] as WL RF HW PDn source*/
+
+	bool real_wow_v2_enable;
+	bool re_init_llt_table;
 };
 
 struct rtl_security {
@@ -1759,6 +1902,15 @@ struct rtl_ps_ctl {
 	struct rtl_p2p_ps_info p2p_ps_info;
 	u8 pwr_mode;
 	u8 smart_ps;
+
+	/* wake up on line */
+	u8 wo_wlan_mode;
+	u8 arp_offload_enable;
+	u8 gtk_offload_enable;
+	/* Used for WOL, indicates the reason for waking event.*/
+	u32 wakeup_reason;
+	/* Record the last waking time for comparison with setting key. */
+	u64 last_wakeup_time;
 };
 
 struct rtl_stats {
@@ -1794,17 +1946,26 @@ struct rtl_stats {
 	u16 wakeup:1;
 	u32 timestamp_low;
 	u32 timestamp_high;
+	bool shift;
 
 	u8 rx_drvinfo_size;
 	u8 rx_bufshift;
 	bool isampdu;
 	bool isfirst_ampdu;
 	bool rx_is40Mhzpacket;
+	u8 rx_packet_bw;
 	u32 rx_pwdb_all;
 	u8 rx_mimo_signalstrength[4];	/*in 0~100 index */
+	s8 rx_mimo_signalquality[4];
+	u8 rx_mimo_evm_dbm[4];
+	u16 cfo_short[4];		/* per-path's Cfo_short */
+	u16 cfo_tail[4];
+
 	s8 rx_mimo_sig_qual[4];
 	u8 rx_pwr[4]; /* per-path's pwdb */
 	u8 rx_snr[4]; /* per-path's SNR */
+	u8 bandwidth;
+	u8 bt_coex_pwr_adjust;
 	bool packet_matchbssid;
 	bool is_cck;
 	bool is_ht;
@@ -1812,6 +1973,10 @@ struct rtl_stats {
 	bool packet_beacon;	/*for rssi */
 	char cck_adc_pwdb[4];	/*for rx path selection */
 
+	bool is_vht;
+	bool is_short_gi;
+	u8 vht_nss;
+
 	u8 packet_report_type;
 
 	u32 macid;
@@ -1844,7 +2009,7 @@ struct rt_link_detect {
 };
 
 struct rtl_tcb_desc {
-	u8 packet_bw:1;
+	u8 packet_bw:2;
 	u8 multicast:1;
 	u8 broadcast:1;
 
@@ -1874,11 +2039,19 @@ struct rtl_tcb_desc {
 	u8 empkt_num;
 	/* The max value by HW */
 	u32 empkt_len[10];
-	bool btx_enable_sw_calc_duration;
+	bool tx_enable_sw_calc_duration;
 };
 
 struct rtl92c_firmware_header;
 
+struct rtl_wow_pattern {
+	u8 type;
+	u16 crc;
+	u32 mask[4];
+};
+
+struct rtl8723e_firmware_header;
+
 struct rtl_hal_ops {
 	int (*init_sw_vars) (struct ieee80211_hw *hw);
 	void (*deinit_sw_vars) (struct ieee80211_hw *hw);
@@ -1928,7 +2101,6 @@ struct rtl_hal_ops {
 	void (*fill_tx_cmddesc) (struct ieee80211_hw *hw, u8 *pdesc,
 				 bool firstseg, bool lastseg,
 				 struct sk_buff *skb);
-	bool (*cmd_send_packet)(struct ieee80211_hw *hw, struct sk_buff *skb);
 	bool (*query_rx_desc) (struct ieee80211_hw *hw,
 			       struct rtl_stats *stats,
 			       struct ieee80211_rx_status *rx_status,
@@ -1983,9 +2155,12 @@ struct rtl_hal_ops {
 	void (*fill_h2c_cmd) (struct ieee80211_hw *hw, u8 element_id,
 			      u32 cmd_len, u8 *p_cmdbuffer);
 	bool (*get_btc_status) (void);
-	bool (*is_fw_header) (struct rtl92c_firmware_header *hdr);
+	bool (*is_fw_header)(struct rtl8723e_firmware_header *hdr);
 	u32 (*rx_command_packet)(struct ieee80211_hw *hw,
 				 struct rtl_stats status, struct sk_buff *skb);
+	void (*add_wowlan_pattern)(struct ieee80211_hw *hw,
+				   struct rtl_wow_pattern *rtl_pattern,
+				   u8 index);
 };
 
 struct rtl_intf_ops {
@@ -2000,7 +2175,7 @@ struct rtl_intf_ops {
 			   struct ieee80211_sta *sta,
 			   struct sk_buff *skb,
 			   struct rtl_tcb_desc *ptcb_desc);
-	void (*flush)(struct ieee80211_hw *hw, bool drop);
+	void (*flush)(struct ieee80211_hw *hw, u32 queues, bool drop);
 	int (*reset_trx_ring) (struct ieee80211_hw *hw);
 	bool (*waitq_insert) (struct ieee80211_hw *hw,
 			      struct ieee80211_sta *sta,
@@ -2029,9 +2204,13 @@ struct rtl_mod_params {
 	/* default: 1 = using linked fw power save */
 	bool fwctrl_lps;
 
-	/* default: 0 = not using MSI interrupts mode */
-	/* submodules should set their own defalut value */
+	/* default: 0 = not using MSI interrupts mode
+	 * submodules should set their own default value
+	 */
 	bool msi_support;
+
+	/* default 0: 1 means disable */
+	bool disable_watchdog;
 };
 
 struct rtl_hal_usbint_cfg {
@@ -2312,10 +2491,11 @@ struct rtl_btc_ops {
 	void (*btc_init_hal_vars) (struct rtl_priv *rtlpriv);
 	void (*btc_init_hw_config) (struct rtl_priv *rtlpriv);
 	void (*btc_ips_notify) (struct rtl_priv *rtlpriv, u8 type);
+	void (*btc_lps_notify)(struct rtl_priv *rtlpriv, u8 type);
 	void (*btc_scan_notify) (struct rtl_priv *rtlpriv, u8 scantype);
 	void (*btc_connect_notify) (struct rtl_priv *rtlpriv, u8 action);
 	void (*btc_mediastatus_notify) (struct rtl_priv *rtlpriv,
-					enum _RT_MEDIA_STATUS mstatus);
+					enum rt_media_status mstatus);
 	void (*btc_periodical) (struct rtl_priv *rtlpriv);
 	void (*btc_halt_notify) (void);
 	void (*btc_btinfo_notify) (struct rtl_priv *rtlpriv,
@@ -2323,6 +2503,8 @@ struct rtl_btc_ops {
 	bool (*btc_is_limited_dig) (struct rtl_priv *rtlpriv);
 	bool (*btc_is_disable_edca_turbo) (struct rtl_priv *rtlpriv);
 	bool (*btc_is_bt_disabled) (struct rtl_priv *rtlpriv);
+	void (*btc_special_packet_notify)(struct rtl_priv *rtlpriv,
+					  u8 pkt_type);
 };
 
 struct proxim {
@@ -2336,6 +2518,8 @@ struct proxim {
 
 struct rtl_priv {
 	struct ieee80211_hw *hw;
+	/* Used to load a second firmware */
+	void (*rtl_fw_second_cb)(struct rtl_priv *rtlpriv);
 	struct completion firmware_loading_complete;
 	struct list_head list;
 	struct rtl_priv *buddy_priv;
@@ -2411,6 +2595,9 @@ struct rtl_priv {
 	 */
 	bool use_new_trx_flow;
 
+#ifdef CONFIG_PM
+	struct wiphy_wowlan_support wowlan;
+#endif
 	/*This must be the last item so
 	   that it points to the data allocated
 	   beyond  this structure like:
@@ -2659,6 +2846,26 @@ value to host byte ordering.*/
 	(des)[2] = (src)[2], (des)[3] = (src)[3],\
 	(des)[4] = (src)[4], (des)[5] = (src)[5])
 
+#define	LDPC_HT_ENABLE_RX			BIT(0)
+#define	LDPC_HT_ENABLE_TX			BIT(1)
+#define	LDPC_HT_TEST_TX_ENABLE			BIT(2)
+#define	LDPC_HT_CAP_TX				BIT(3)
+
+#define	STBC_HT_ENABLE_RX			BIT(0)
+#define	STBC_HT_ENABLE_TX			BIT(1)
+#define	STBC_HT_TEST_TX_ENABLE			BIT(2)
+#define	STBC_HT_CAP_TX				BIT(3)
+
+#define	LDPC_VHT_ENABLE_RX			BIT(0)
+#define	LDPC_VHT_ENABLE_TX			BIT(1)
+#define	LDPC_VHT_TEST_TX_ENABLE			BIT(2)
+#define	LDPC_VHT_CAP_TX				BIT(3)
+
+#define	STBC_VHT_ENABLE_RX			BIT(0)
+#define	STBC_VHT_ENABLE_TX			BIT(1)
+#define	STBC_VHT_TEST_TX_ENABLE			BIT(2)
+#define	STBC_VHT_CAP_TX				BIT(3)
+
 static inline u8 rtl_read_byte(struct rtl_priv *rtlpriv, u32 addr)
 {
 	return rtlpriv->io.read8_sync(rtlpriv, addr);
diff --git a/drivers/net/wireless/ti/wl1251/spi.c b/drivers/net/wireless/ti/wl1251/spi.c
index a0aa8fa72392..735be5352143 100644
--- a/drivers/net/wireless/ti/wl1251/spi.c
+++ b/drivers/net/wireless/ti/wl1251/spi.c
@@ -345,7 +345,6 @@ static int wl1251_spi_remove(struct spi_device *spi)
 {
 	struct wl1251 *wl = spi_get_drvdata(spi);
 
-	free_irq(wl->irq, wl);
 	wl1251_free_hw(wl);
 	regulator_disable(wl->vio);
 
diff --git a/drivers/net/wireless/ti/wlcore/debug.h b/drivers/net/wireless/ti/wlcore/debug.h
index 0420bd45e4ee..27bfb7c11e7b 100644
--- a/drivers/net/wireless/ti/wlcore/debug.h
+++ b/drivers/net/wireless/ti/wlcore/debug.h
@@ -65,7 +65,7 @@ extern u32 wl12xx_debug_level;
 	pr_err(DRIVER_PREFIX "ERROR " fmt "\n", ##arg)
 
 #define wl1271_warning(fmt, arg...) \
-	pr_warning(DRIVER_PREFIX "WARNING " fmt "\n", ##arg)
+	pr_warn(DRIVER_PREFIX "WARNING " fmt "\n", ##arg)
 
 #define wl1271_notice(fmt, arg...) \
 	pr_info(DRIVER_PREFIX fmt "\n", ##arg)
diff --git a/drivers/net/wireless/ti/wlcore/spi.c b/drivers/net/wireless/ti/wlcore/spi.c
index 392c882b28f0..69601f6741d9 100644
--- a/drivers/net/wireless/ti/wlcore/spi.c
+++ b/drivers/net/wireless/ti/wlcore/spi.c
@@ -327,23 +327,22 @@ static int wl1271_probe(struct spi_device *spi)
 	struct wl12xx_spi_glue *glue;
 	struct wlcore_platdev_data pdev_data;
 	struct resource res[1];
-	int ret = -ENOMEM;
+	int ret;
 
 	memset(&pdev_data, 0x00, sizeof(pdev_data));
 
 	pdev_data.pdata = dev_get_platdata(&spi->dev);
 	if (!pdev_data.pdata) {
 		dev_err(&spi->dev, "no platform data\n");
-		ret = -ENODEV;
-		goto out;
+		return -ENODEV;
 	}
 
 	pdev_data.if_ops = &spi_ops;
 
-	glue = kzalloc(sizeof(*glue), GFP_KERNEL);
+	glue = devm_kzalloc(&spi->dev, sizeof(*glue), GFP_KERNEL);
 	if (!glue) {
 		dev_err(&spi->dev, "can't allocate glue\n");
-		goto out;
+		return -ENOMEM;
 	}
 
 	glue->dev = &spi->dev;
@@ -357,14 +356,13 @@ static int wl1271_probe(struct spi_device *spi)
 	ret = spi_setup(spi);
 	if (ret < 0) {
 		dev_err(glue->dev, "spi_setup failed\n");
-		goto out_free_glue;
+		return ret;
 	}
 
 	glue->core = platform_device_alloc("wl12xx", PLATFORM_DEVID_AUTO);
 	if (!glue->core) {
 		dev_err(glue->dev, "can't allocate platform_device\n");
-		ret = -ENOMEM;
-		goto out_free_glue;
+		return -ENOMEM;
 	}
 
 	glue->core->dev.parent = &spi->dev;
@@ -398,11 +396,6 @@ static int wl1271_probe(struct spi_device *spi)
 
 out_dev_put:
 	platform_device_put(glue->core);
-
-out_free_glue:
-	kfree(glue);
-
-out:
 	return ret;
 }
 
@@ -411,7 +404,6 @@ static int wl1271_remove(struct spi_device *spi)
 	struct wl12xx_spi_glue *glue = spi_get_drvdata(spi);
 
 	platform_device_unregister(glue->core);
-	kfree(glue);
 
 	return 0;
 }
diff --git a/drivers/net/xen-netback/interface.c b/drivers/net/xen-netback/interface.c
index e29e15dca86e..f379689dde30 100644
--- a/drivers/net/xen-netback/interface.c
+++ b/drivers/net/xen-netback/interface.c
@@ -576,6 +576,9 @@ int xenvif_connect(struct xenvif_queue *queue, unsigned long tx_ring_ref,
 	init_waitqueue_head(&queue->dealloc_wq);
 	atomic_set(&queue->inflight_packets, 0);
 
+	netif_napi_add(queue->vif->dev, &queue->napi, xenvif_poll,
+			XENVIF_NAPI_WEIGHT);
+
 	if (tx_evtchn == rx_evtchn) {
 		/* feature-split-event-channels == 0 */
 		err = bind_interdomain_evtchn_to_irqhandler(
@@ -629,9 +632,6 @@ int xenvif_connect(struct xenvif_queue *queue, unsigned long tx_ring_ref,
 	wake_up_process(queue->task);
 	wake_up_process(queue->dealloc_task);
 
-	netif_napi_add(queue->vif->dev, &queue->napi, xenvif_poll,
-			XENVIF_NAPI_WEIGHT);
-
 	return 0;
 
 err_rx_unbind:
diff --git a/drivers/net/xen-netback/xenbus.c b/drivers/net/xen-netback/xenbus.c
index 9c47b897b6d2..8079c31ac5e6 100644
--- a/drivers/net/xen-netback/xenbus.c
+++ b/drivers/net/xen-netback/xenbus.c
@@ -937,22 +937,18 @@ static int read_xenbus_vif_flags(struct backend_info *be)
 	return 0;
 }
 
-
-/* ** Driver Registration ** */
-
-
 static const struct xenbus_device_id netback_ids[] = {
 	{ "vif" },
 	{ "" }
 };
 
-
-static DEFINE_XENBUS_DRIVER(netback, ,
+static struct xenbus_driver netback_driver = {
+	.ids = netback_ids,
 	.probe = netback_probe,
 	.remove = netback_remove,
 	.uevent = netback_uevent,
 	.otherend_changed = frontend_changed,
-);
+};
 
 int xenvif_xenbus_init(void)
 {
diff --git a/drivers/net/xen-netfront.c b/drivers/net/xen-netfront.c
index ca82f545ec2c..cca871346a0f 100644
--- a/drivers/net/xen-netfront.c
+++ b/drivers/net/xen-netfront.c
@@ -638,7 +638,7 @@ static int xennet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 	if (unlikely(!netif_carrier_ok(dev) ||
 		     (slots > 1 && !xennet_can_sg(dev)) ||
-		     netif_needs_gso(skb, netif_skb_features(skb)))) {
+		     netif_needs_gso(dev, skb, netif_skb_features(skb)))) {
 		spin_unlock_irqrestore(&queue->tx_lock, flags);
 		goto drop;
 	}
@@ -2300,12 +2300,6 @@ static void xennet_sysfs_delif(struct net_device *netdev)
 
 #endif /* CONFIG_SYSFS */
 
-static const struct xenbus_device_id netfront_ids[] = {
-	{ "vif" },
-	{ "" }
-};
-
-
 static int xennet_remove(struct xenbus_device *dev)
 {
 	struct netfront_info *info = dev_get_drvdata(&dev->dev);
@@ -2338,12 +2332,18 @@ static int xennet_remove(struct xenbus_device *dev)
 	return 0;
 }
 
-static DEFINE_XENBUS_DRIVER(netfront, ,
+static const struct xenbus_device_id netfront_ids[] = {
+	{ "vif" },
+	{ "" }
+};
+
+static struct xenbus_driver netfront_driver = {
+	.ids = netfront_ids,
 	.probe = netfront_probe,
 	.remove = xennet_remove,
 	.resume = netfront_resume,
 	.otherend_changed = netback_changed,
-);
+};
 
 static int __init netif_init(void)
 {
diff --git a/drivers/nfc/microread/microread.c b/drivers/nfc/microread/microread.c
index f868333271aa..963a4a5dc88e 100644
--- a/drivers/nfc/microread/microread.c
+++ b/drivers/nfc/microread/microread.c
@@ -501,9 +501,13 @@ static void microread_target_discovered(struct nfc_hci_dev *hdev, u8 gate,
 		targets->sens_res =
 			 be16_to_cpu(*(u16 *)&skb->data[MICROREAD_EMCF_A_ATQA]);
 		targets->sel_res = skb->data[MICROREAD_EMCF_A_SAK];
-		memcpy(targets->nfcid1, &skb->data[MICROREAD_EMCF_A_UID],
-		       skb->data[MICROREAD_EMCF_A_LEN]);
 		targets->nfcid1_len = skb->data[MICROREAD_EMCF_A_LEN];
+		if (targets->nfcid1_len > sizeof(targets->nfcid1)) {
+			r = -EINVAL;
+			goto exit_free;
+		}
+		memcpy(targets->nfcid1, &skb->data[MICROREAD_EMCF_A_UID],
+		       targets->nfcid1_len);
 		break;
 	case MICROREAD_GATE_ID_MREAD_ISO_A_3:
 		targets->supported_protocols =
@@ -511,9 +515,13 @@ static void microread_target_discovered(struct nfc_hci_dev *hdev, u8 gate,
 		targets->sens_res =
 			 be16_to_cpu(*(u16 *)&skb->data[MICROREAD_EMCF_A3_ATQA]);
 		targets->sel_res = skb->data[MICROREAD_EMCF_A3_SAK];
-		memcpy(targets->nfcid1, &skb->data[MICROREAD_EMCF_A3_UID],
-		       skb->data[MICROREAD_EMCF_A3_LEN]);
 		targets->nfcid1_len = skb->data[MICROREAD_EMCF_A3_LEN];
+		if (targets->nfcid1_len > sizeof(targets->nfcid1)) {
+			r = -EINVAL;
+			goto exit_free;
+		}
+		memcpy(targets->nfcid1, &skb->data[MICROREAD_EMCF_A3_UID],
+		       targets->nfcid1_len);
 		break;
 	case MICROREAD_GATE_ID_MREAD_ISO_B:
 		targets->supported_protocols = NFC_PROTO_ISO14443_B_MASK;
diff --git a/drivers/nfc/st21nfca/Makefile b/drivers/nfc/st21nfca/Makefile
index db7a38ae05f7..7d688f97aa27 100644
--- a/drivers/nfc/st21nfca/Makefile
+++ b/drivers/nfc/st21nfca/Makefile
@@ -2,7 +2,8 @@
 # Makefile for ST21NFCA HCI based NFC driver
 #
 
-st21nfca_i2c-objs  = i2c.o
+st21nfca_hci-objs = st21nfca.o st21nfca_dep.o
+obj-$(CONFIG_NFC_ST21NFCA)     += st21nfca_hci.o
 
-obj-$(CONFIG_NFC_ST21NFCA)     += st21nfca.o st21nfca_dep.o
+st21nfca_i2c-objs  = i2c.o
 obj-$(CONFIG_NFC_ST21NFCA_I2C) += st21nfca_i2c.o
diff --git a/drivers/nfc/st21nfca/i2c.c b/drivers/nfc/st21nfca/i2c.c
index ff31939978ae..0ea756b77519 100644
--- a/drivers/nfc/st21nfca/i2c.c
+++ b/drivers/nfc/st21nfca/i2c.c
@@ -271,6 +271,7 @@ static int st21nfca_hci_i2c_write(void *phy_id, struct sk_buff *skb)
 static int get_frame_size(u8 *buf, int buflen)
 {
 	int len = 0;
+
 	if (buf[len + 1] == ST21NFCA_SOF_EOF)
 		return 0;
 
@@ -311,6 +312,7 @@ static int check_crc(u8 *buf, int buflen)
 static int st21nfca_hci_i2c_repack(struct sk_buff *skb)
 {
 	int i, j, r, size;
+
 	if (skb->len < 1 || (skb->len > 1 && skb->data[1] != 0))
 		return -EBADMSG;
 
@@ -525,24 +527,19 @@ static int st21nfca_hci_i2c_of_request_resources(struct i2c_client *client)
 	}
 
 	/* GPIO request and configuration */
-	r = devm_gpio_request(&client->dev, gpio, "clf_enable");
+	r = devm_gpio_request_one(&client->dev, gpio, GPIOF_OUT_INIT_HIGH,
+				  "clf_enable");
 	if (r) {
 		nfc_err(&client->dev, "Failed to request enable pin\n");
 		return -ENODEV;
 	}
 
-	r = gpio_direction_output(gpio, 1);
-	if (r) {
-		nfc_err(&client->dev, "Failed to set enable pin direction as output\n");
-		return -ENODEV;
-	}
 	phy->gpio_ena = gpio;
 
 	/* IRQ */
 	r = irq_of_parse_and_map(pp, 0);
 	if (r < 0) {
-		nfc_err(&client->dev,
-				"Unable to get irq, error: %d\n", r);
+		nfc_err(&client->dev, "Unable to get irq, error: %d\n", r);
 		return r;
 	}
 
@@ -576,32 +573,20 @@ static int st21nfca_hci_i2c_request_resources(struct i2c_client *client)
 	phy->gpio_ena = pdata->gpio_ena;
 	phy->irq_polarity = pdata->irq_polarity;
 
-	r = devm_gpio_request(&client->dev, phy->gpio_irq, "wake_up");
+	r = devm_gpio_request_one(&client->dev, phy->gpio_irq, GPIOF_IN,
+				  "wake_up");
 	if (r) {
 		pr_err("%s : gpio_request failed\n", __FILE__);
 		return -ENODEV;
 	}
 
-	r = gpio_direction_input(phy->gpio_irq);
-	if (r) {
-		pr_err("%s : gpio_direction_input failed\n", __FILE__);
-		return -ENODEV;
-	}
-
 	if (phy->gpio_ena > 0) {
-		r = devm_gpio_request(&client->dev,
-					phy->gpio_ena, "clf_enable");
+		r = devm_gpio_request_one(&client->dev, phy->gpio_ena,
+					  GPIOF_OUT_INIT_HIGH, "clf_enable");
 		if (r) {
 			pr_err("%s : ena gpio_request failed\n", __FILE__);
 			return -ENODEV;
 		}
-		r = gpio_direction_output(phy->gpio_ena, 1);
-
-		if (r) {
-			pr_err("%s : ena gpio_direction_output failed\n",
-			       __FILE__);
-			return -ENODEV;
-		}
 	}
 
 	/* IRQ */
@@ -711,7 +696,6 @@ static struct i2c_driver st21nfca_hci_i2c_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = ST21NFCA_HCI_I2C_DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(of_st21nfca_i2c_match),
 	},
 	.probe = st21nfca_hci_i2c_probe,
diff --git a/drivers/nfc/st21nfca/st21nfca.c b/drivers/nfc/st21nfca/st21nfca.c
index a902b0551c86..a89e56c2c749 100644
--- a/drivers/nfc/st21nfca/st21nfca.c
+++ b/drivers/nfc/st21nfca/st21nfca.c
@@ -34,7 +34,7 @@
 #define ST21NFCA_RF_READER_CMD_PRESENCE_CHECK	0x30
 
 #define ST21NFCA_RF_READER_ISO15693_GATE	0x12
-#define ST21NFCA_RF_READER_ISO15693_INVENTORY 0x01
+#define ST21NFCA_RF_READER_ISO15693_INVENTORY	0x01
 
 /*
  * Reader gate for communication with contact-less cards using Type A
@@ -45,21 +45,42 @@
 #define ST21NFCA_RF_READER_14443_3_A_ATQA	0x03
 #define ST21NFCA_RF_READER_14443_3_A_SAK	0x04
 
+#define ST21NFCA_RF_READER_F_DATARATE		0x01
+#define ST21NFCA_RF_READER_F_DATARATE_106	0x01
+#define ST21NFCA_RF_READER_F_DATARATE_212	0x02
+#define ST21NFCA_RF_READER_F_DATARATE_424	0x04
+#define ST21NFCA_RF_READER_F_POL_REQ		0x02
+#define ST21NFCA_RF_READER_F_POL_REQ_DEFAULT	0xffff0000
+#define ST21NFCA_RF_READER_F_NFCID2		0x03
+#define ST21NFCA_RF_READER_F_NFCID1		0x04
+
+#define ST21NFCA_RF_CARD_F_MODE			0x01
+#define ST21NFCA_RF_CARD_F_NFCID2_LIST		0x04
+#define ST21NFCA_RF_CARD_F_NFCID1		0x05
+#define ST21NFCA_RF_CARD_F_SENS_RES		0x06
+#define ST21NFCA_RF_CARD_F_SEL_RES		0x07
+#define ST21NFCA_RF_CARD_F_DATARATE		0x08
+#define ST21NFCA_RF_CARD_F_DATARATE_212_424	0x01
+
 #define ST21NFCA_DEVICE_MGNT_GATE		0x01
 #define ST21NFCA_DEVICE_MGNT_PIPE		0x02
 
-#define ST21NFCA_DM_GETINFO         0x13
-#define ST21NFCA_DM_GETINFO_PIPE_LIST       0x02
-#define ST21NFCA_DM_GETINFO_PIPE_INFO       0x01
-#define ST21NFCA_DM_PIPE_CREATED        0x02
-#define ST21NFCA_DM_PIPE_OPEN           0x04
-#define ST21NFCA_DM_RF_ACTIVE           0x80
-#define ST21NFCA_DM_DISCONNECT		0x30
+#define ST21NFCA_DM_GETINFO			0x13
+#define ST21NFCA_DM_GETINFO_PIPE_LIST		0x02
+#define ST21NFCA_DM_GETINFO_PIPE_INFO		0x01
+#define ST21NFCA_DM_PIPE_CREATED		0x02
+#define ST21NFCA_DM_PIPE_OPEN			0x04
+#define ST21NFCA_DM_RF_ACTIVE			0x80
+#define ST21NFCA_DM_DISCONNECT			0x30
 
 #define ST21NFCA_DM_IS_PIPE_OPEN(p) \
 	((p & 0x0f) == (ST21NFCA_DM_PIPE_CREATED | ST21NFCA_DM_PIPE_OPEN))
 
-#define ST21NFCA_NFC_MODE	0x03	/* NFC_MODE parameter*/
+#define ST21NFCA_NFC_MODE			0x03	/* NFC_MODE parameter*/
+#define ST21NFCA_EVT_FIELD_ON			0x11
+#define ST21NFCA_EVT_CARD_DEACTIVATED		0x12
+#define ST21NFCA_EVT_CARD_ACTIVATED		0x13
+#define ST21NFCA_EVT_FIELD_OFF			0x14
 
 static DECLARE_BITMAP(dev_mask, ST21NFCA_NUM_DEVICES);
 
@@ -355,8 +376,8 @@ static int st21nfca_hci_start_poll(struct nfc_hci_dev *hdev,
 			if (r < 0)
 				return r;
 
-			pol_req =
-			    be32_to_cpu(ST21NFCA_RF_READER_F_POL_REQ_DEFAULT);
+			pol_req = be32_to_cpu((__force __be32)
+					ST21NFCA_RF_READER_F_POL_REQ_DEFAULT);
 			r = nfc_hci_set_param(hdev, ST21NFCA_RF_READER_F_GATE,
 					      ST21NFCA_RF_READER_F_POL_REQ,
 					      (u8 *) &pol_req, 4);
@@ -790,6 +811,7 @@ static int st21nfca_hci_check_presence(struct nfc_hci_dev *hdev,
 				       struct nfc_target *target)
 {
 	u8 fwi = 0x11;
+
 	switch (target->hci_reader_gate) {
 	case NFC_HCI_RF_READER_A_GATE:
 	case NFC_HCI_RF_READER_B_GATE:
@@ -839,20 +861,16 @@ static int st21nfca_hci_event_received(struct nfc_hci_dev *hdev, u8 gate,
 		if (gate == ST21NFCA_RF_CARD_F_GATE) {
 			r = st21nfca_tm_event_send_data(hdev, skb, gate);
 			if (r < 0)
-				goto exit;
+				return r;
 			return 0;
-		} else {
-			info->dep_info.curr_nfc_dep_pni = 0;
-			return 1;
 		}
-		break;
+		info->dep_info.curr_nfc_dep_pni = 0;
+		return 1;
 	default:
 		return 1;
 	}
 	kfree_skb(skb);
 	return 0;
-exit:
-	return r;
 }
 
 static struct nfc_hci_ops st21nfca_hci_ops = {
@@ -904,8 +922,11 @@ int st21nfca_hci_probe(void *phy_id, struct nfc_phy_ops *phy_ops,
 	 * persistent info to discriminate 2 identical chips
 	 */
 	dev_num = find_first_zero_bit(dev_mask, ST21NFCA_NUM_DEVICES);
+
 	if (dev_num >= ST21NFCA_NUM_DEVICES)
-		goto err_alloc_hdev;
+		return -ENODEV;
+
+	set_bit(dev_num, dev_mask);
 
 	scnprintf(init_data.session_id, sizeof(init_data.session_id), "%s%2x",
 		  "ST21AH", dev_num);
diff --git a/drivers/nfc/st21nfca/st21nfca.h b/drivers/nfc/st21nfca/st21nfca.h
index 96fe5a62dc0d..a0b77f1ba6d9 100644
--- a/drivers/nfc/st21nfca/st21nfca.h
+++ b/drivers/nfc/st21nfca/st21nfca.h
@@ -82,30 +82,9 @@ struct st21nfca_hci_info {
 #define ST21NFCA_WR_XCHG_DATA            0x10
 
 #define ST21NFCA_RF_READER_F_GATE               0x14
-#define ST21NFCA_RF_READER_F_DATARATE 0x01
-#define ST21NFCA_RF_READER_F_DATARATE_106 0x01
-#define ST21NFCA_RF_READER_F_DATARATE_212 0x02
-#define ST21NFCA_RF_READER_F_DATARATE_424 0x04
-#define ST21NFCA_RF_READER_F_POL_REQ    0x02
-#define ST21NFCA_RF_READER_F_POL_REQ_DEFAULT    0xffff0000
-#define ST21NFCA_RF_READER_F_NFCID2 0x03
-#define ST21NFCA_RF_READER_F_NFCID1 0x04
-#define ST21NFCA_RF_READER_F_SENS_RES 0x05
 
 #define ST21NFCA_RF_CARD_F_GATE 0x24
-#define ST21NFCA_RF_CARD_F_MODE 0x01
-#define ST21NFCA_RF_CARD_F_NFCID2_LIST 0x04
-#define ST21NFCA_RF_CARD_F_NFCID1 0x05
-#define ST21NFCA_RF_CARD_F_SENS_RES 0x06
-#define ST21NFCA_RF_CARD_F_SEL_RES 0x07
-#define ST21NFCA_RF_CARD_F_DATARATE 0x08
-#define ST21NFCA_RF_CARD_F_DATARATE_106 0x00
-#define ST21NFCA_RF_CARD_F_DATARATE_212_424 0x01
 
 #define ST21NFCA_EVT_SEND_DATA 0x10
-#define ST21NFCA_EVT_FIELD_ON 0x11
-#define ST21NFCA_EVT_CARD_DEACTIVATED 0x12
-#define ST21NFCA_EVT_CARD_ACTIVATED 0x13
-#define ST21NFCA_EVT_FIELD_OFF 0x14
 
 #endif /* __LOCAL_ST21NFCA_H_ */
diff --git a/drivers/nfc/st21nfca/st21nfca_dep.c b/drivers/nfc/st21nfca/st21nfca_dep.c
index b2d9957b57f8..bfb6df56c505 100644
--- a/drivers/nfc/st21nfca/st21nfca_dep.c
+++ b/drivers/nfc/st21nfca/st21nfca_dep.c
@@ -121,6 +121,7 @@ static void st21nfca_tx_work(struct work_struct *work)
 
 	struct nfc_dev *dev;
 	struct sk_buff *skb;
+
 	if (info) {
 		dev = info->hdev->ndev;
 		skb = info->dep_info.tx_pending;
@@ -128,9 +129,8 @@ static void st21nfca_tx_work(struct work_struct *work)
 		device_lock(&dev->dev);
 
 		nfc_hci_send_cmd_async(info->hdev, ST21NFCA_RF_READER_F_GATE,
-							ST21NFCA_WR_XCHG_DATA,
-							skb->data, skb->len,
-							info->async_cb, info);
+				ST21NFCA_WR_XCHG_DATA, skb->data, skb->len,
+				info->async_cb, info);
 		device_unlock(&dev->dev);
 		kfree_skb(skb);
 	}
@@ -185,8 +185,10 @@ static int st21nfca_tm_send_atr_res(struct nfc_hci_dev *hdev,
 
 	info->dep_info.curr_nfc_dep_pni = 0;
 
-	return nfc_hci_send_event(hdev, ST21NFCA_RF_CARD_F_GATE,
+	r = nfc_hci_send_event(hdev, ST21NFCA_RF_CARD_F_GATE,
 				ST21NFCA_EVT_SEND_DATA, skb->data, skb->len);
+	kfree_skb(skb);
+	return r;
 }
 
 static int st21nfca_tm_recv_atr_req(struct nfc_hci_dev *hdev,
@@ -197,10 +199,6 @@ static int st21nfca_tm_recv_atr_req(struct nfc_hci_dev *hdev,
 	int r;
 
 	skb_trim(skb, skb->len - 1);
-	if (IS_ERR(skb)) {
-		r = PTR_ERR(skb);
-		goto exit;
-	}
 
 	if (!skb->len) {
 		r = -EIO;
@@ -214,6 +212,11 @@ static int st21nfca_tm_recv_atr_req(struct nfc_hci_dev *hdev,
 
 	atr_req = (struct st21nfca_atr_req *)skb->data;
 
+	if (atr_req->length < sizeof(struct st21nfca_atr_req)) {
+		r = -EPROTO;
+		goto exit;
+	}
+
 	r = st21nfca_tm_send_atr_res(hdev, atr_req);
 	if (r)
 		goto exit;
@@ -237,7 +240,6 @@ static int st21nfca_tm_send_psl_res(struct nfc_hci_dev *hdev,
 	struct st21nfca_psl_res *psl_res;
 	struct sk_buff *skb;
 	u8 bitrate[2] = {0, 0};
-
 	int r;
 
 	skb = alloc_skb(sizeof(struct st21nfca_psl_res), GFP_KERNEL);
@@ -254,6 +256,8 @@ static int st21nfca_tm_send_psl_res(struct nfc_hci_dev *hdev,
 
 	r = nfc_hci_send_event(hdev, ST21NFCA_RF_CARD_F_GATE,
 				ST21NFCA_EVT_SEND_DATA, skb->data, skb->len);
+	if (r < 0)
+		goto error;
 
 	/*
 	 * ST21NFCA only support P2P passive.
@@ -269,8 +273,11 @@ static int st21nfca_tm_send_psl_res(struct nfc_hci_dev *hdev,
 	}
 
 	/* Send an event to change bitrate change event to card f */
-	return nfc_hci_send_event(hdev, ST21NFCA_RF_CARD_F_GATE,
+	r = nfc_hci_send_event(hdev, ST21NFCA_RF_CARD_F_GATE,
 			ST21NFCA_EVT_CARD_F_BITRATE, bitrate, 2);
+error:
+	kfree_skb(skb);
+	return r;
 }
 
 static int st21nfca_tm_recv_psl_req(struct nfc_hci_dev *hdev,
@@ -280,11 +287,6 @@ static int st21nfca_tm_recv_psl_req(struct nfc_hci_dev *hdev,
 	int r;
 
 	skb_trim(skb, skb->len - 1);
-	if (IS_ERR(skb)) {
-		r = PTR_ERR(skb);
-		skb = NULL;
-		goto exit;
-	}
 
 	if (!skb->len) {
 		r = -EIO;
@@ -314,7 +316,7 @@ int st21nfca_tm_send_dep_res(struct nfc_hci_dev *hdev, struct sk_buff *skb)
 	*skb_push(skb, 1) = skb->len;
 
 	r = nfc_hci_send_event(hdev, ST21NFCA_RF_CARD_F_GATE,
-				ST21NFCA_EVT_SEND_DATA, skb->data, skb->len);
+			ST21NFCA_EVT_SEND_DATA, skb->data, skb->len);
 	kfree_skb(skb);
 
 	return r;
@@ -330,11 +332,6 @@ static int st21nfca_tm_recv_dep_req(struct nfc_hci_dev *hdev,
 	struct st21nfca_hci_info *info = nfc_hci_get_clientdata(hdev);
 
 	skb_trim(skb, skb->len - 1);
-	if (IS_ERR(skb)) {
-		r = PTR_ERR(skb);
-		skb = NULL;
-		goto exit;
-	}
 
 	size = 4;
 
@@ -368,12 +365,6 @@ static int st21nfca_tm_recv_dep_req(struct nfc_hci_dev *hdev,
 		break;
 	}
 
-	if (IS_ERR(skb)) {
-		r = PTR_ERR(skb);
-		skb = NULL;
-		goto exit;
-	}
-
 	skb_pull(skb, size);
 
 	return nfc_tm_data_received(hdev->ndev, skb);
@@ -437,8 +428,6 @@ static void st21nfca_im_send_psl_req(struct nfc_hci_dev *hdev, u8 did, u8 bsi,
 	*skb_push(skb, 1) = info->dep_info.to | 0x10;
 
 	st21nfca_im_send_pdu(info, skb);
-
-	kfree_skb(skb);
 }
 
 #define ST21NFCA_CB_TYPE_READER_F 1
@@ -452,7 +441,7 @@ static void st21nfca_im_recv_atr_res_cb(void *context, struct sk_buff *skb,
 	if (err != 0)
 		return;
 
-	if (IS_ERR(skb))
+	if (!skb)
 		return;
 
 	switch (info->async_cb_type) {
@@ -484,8 +473,7 @@ static void st21nfca_im_recv_atr_res_cb(void *context, struct sk_buff *skb,
 						ST21NFCA_PP2LRI(atr_res->ppi));
 		break;
 	default:
-		if (err == 0)
-			kfree_skb(skb);
+		kfree_skb(skb);
 		break;
 	}
 }
@@ -522,7 +510,7 @@ int st21nfca_im_send_atr_req(struct nfc_hci_dev *hdev, u8 *gb, size_t gb_len)
 	memset(atr_req->nfcid3, 0, NFC_NFCID3_MAXSIZE);
 	target = hdev->ndev->targets;
 
-	if (target->sensf_res)
+	if (target->sensf_res_len > 0)
 		memcpy(atr_req->nfcid3, target->sensf_res,
 				target->sensf_res_len);
 	else
@@ -565,7 +553,7 @@ static void st21nfca_im_recv_dep_res_cb(void *context, struct sk_buff *skb,
 	if (err != 0)
 		return;
 
-	if (IS_ERR(skb))
+	if (!skb)
 		return;
 
 	switch (info->async_cb_type) {
@@ -615,8 +603,7 @@ static void st21nfca_im_recv_dep_res_cb(void *context, struct sk_buff *skb,
 	}
 
 exit:
-	if (err == 0)
-		kfree_skb(skb);
+	kfree_skb(skb);
 }
 
 int st21nfca_im_send_dep_req(struct nfc_hci_dev *hdev, struct sk_buff *skb)
diff --git a/drivers/nfc/st21nfcb/Makefile b/drivers/nfc/st21nfcb/Makefile
index 13d9f03b2fea..f4d835dd15f2 100644
--- a/drivers/nfc/st21nfcb/Makefile
+++ b/drivers/nfc/st21nfcb/Makefile
@@ -2,7 +2,8 @@
 # Makefile for ST21NFCB NCI based NFC driver
 #
 
-st21nfcb_i2c-objs  = i2c.o
+st21nfcb_nci-objs = ndlc.o st21nfcb.o
+obj-$(CONFIG_NFC_ST21NFCB)     += st21nfcb_nci.o
 
-obj-$(CONFIG_NFC_ST21NFCB)     += st21nfcb.o ndlc.o
+st21nfcb_i2c-objs = i2c.o
 obj-$(CONFIG_NFC_ST21NFCB_I2C) += st21nfcb_i2c.o
diff --git a/drivers/nfc/st21nfcb/i2c.c b/drivers/nfc/st21nfcb/i2c.c
index 8af880ead5db..c5d2427a3db2 100644
--- a/drivers/nfc/st21nfcb/i2c.c
+++ b/drivers/nfc/st21nfcb/i2c.c
@@ -17,24 +17,16 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
-#include <linux/crc-ccitt.h>
 #include <linux/module.h>
 #include <linux/i2c.h>
 #include <linux/gpio.h>
 #include <linux/of_irq.h>
 #include <linux/of_gpio.h>
-#include <linux/miscdevice.h>
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/nfc.h>
-#include <linux/firmware.h>
-#include <linux/unaligned/access_ok.h>
 #include <linux/platform_data/st21nfcb.h>
 
-#include <net/nfc/nci.h>
-#include <net/nfc/llc.h>
-#include <net/nfc/nfc.h>
-
 #include "ndlc.h"
 
 #define DRIVER_DESC "NCI NFC driver for ST21NFCB"
@@ -63,12 +55,6 @@ struct st21nfcb_i2c_phy {
 	unsigned int irq_polarity;
 
 	int powered;
-
-	/*
-	 * < 0 if hardware error occured (e.g. i2c err)
-	 * and prevents normal operation.
-	 */
-	int hard_fault;
 };
 
 #define I2C_DUMP_SKB(info, skb)					\
@@ -122,8 +108,8 @@ static int st21nfcb_nci_i2c_write(void *phy_id, struct sk_buff *skb)
 
 	I2C_DUMP_SKB("st21nfcb_nci_i2c_write", skb);
 
-	if (phy->hard_fault != 0)
-		return phy->hard_fault;
+	if (phy->ndlc->hard_fault != 0)
+		return phy->ndlc->hard_fault;
 
 	r = i2c_master_send(client, skb->data, skb->len);
 	if (r == -EREMOTEIO) {  /* Retry, chip was in standby */
@@ -168,11 +154,11 @@ static int st21nfcb_nci_i2c_read(struct st21nfcb_i2c_phy *phy,
 	if (r == -EREMOTEIO) {  /* Retry, chip was in standby */
 		usleep_range(1000, 4000);
 		r = i2c_master_recv(client, buf, ST21NFCB_NCI_I2C_MIN_SIZE);
-	} else if (r != ST21NFCB_NCI_I2C_MIN_SIZE) {
-		nfc_err(&client->dev, "cannot read ndlc & nci header\n");
-		return -EREMOTEIO;
 	}
 
+	if (r != ST21NFCB_NCI_I2C_MIN_SIZE)
+		return -EREMOTEIO;
+
 	len = be16_to_cpu(*(__be16 *) (buf + 2));
 	if (len > ST21NFCB_NCI_I2C_MAX_SIZE) {
 		nfc_err(&client->dev, "invalid frame len\n");
@@ -224,7 +210,7 @@ static irqreturn_t st21nfcb_nci_irq_thread_fn(int irq, void *phy_id)
 	client = phy->i2c_dev;
 	dev_dbg(&client->dev, "IRQ\n");
 
-	if (phy->hard_fault)
+	if (phy->ndlc->hard_fault)
 		return IRQ_HANDLED;
 
 	if (!phy->powered) {
@@ -233,13 +219,8 @@ static irqreturn_t st21nfcb_nci_irq_thread_fn(int irq, void *phy_id)
 	}
 
 	r = st21nfcb_nci_i2c_read(phy, &skb);
-	if (r == -EREMOTEIO) {
-		phy->hard_fault = r;
-                ndlc_recv(phy->ndlc, NULL);
-		return IRQ_HANDLED;
-	} else if (r == -ENOMEM || r == -EBADMSG) {
+	if (r == -EREMOTEIO || r == -ENOMEM || r == -EBADMSG)
 		return IRQ_HANDLED;
-	}
 
 	ndlc_recv(phy->ndlc, skb);
 
@@ -273,25 +254,18 @@ static int st21nfcb_nci_i2c_of_request_resources(struct i2c_client *client)
 	}
 
 	/* GPIO request and configuration */
-	r = devm_gpio_request(&client->dev, gpio, "clf_reset");
+	r = devm_gpio_request_one(&client->dev, gpio,
+				GPIOF_OUT_INIT_HIGH, "clf_reset");
 	if (r) {
 		nfc_err(&client->dev, "Failed to request reset pin\n");
 		return -ENODEV;
 	}
-
-	r = gpio_direction_output(gpio, 1);
-	if (r) {
-		nfc_err(&client->dev,
-			"Failed to set reset pin direction as output\n");
-		return -ENODEV;
-	}
 	phy->gpio_reset = gpio;
 
 	/* IRQ */
 	r = irq_of_parse_and_map(pp, 0);
 	if (r < 0) {
-		nfc_err(&client->dev,
-				"Unable to get irq, error: %d\n", r);
+		nfc_err(&client->dev, "Unable to get irq, error: %d\n", r);
 		return r;
 	}
 
@@ -325,32 +299,20 @@ static int st21nfcb_nci_i2c_request_resources(struct i2c_client *client)
 	phy->gpio_reset = pdata->gpio_reset;
 	phy->irq_polarity = pdata->irq_polarity;
 
-	r = devm_gpio_request(&client->dev, phy->gpio_irq, "wake_up");
+	r = devm_gpio_request_one(&client->dev, phy->gpio_irq,
+				GPIOF_IN, "clf_irq");
 	if (r) {
 		pr_err("%s : gpio_request failed\n", __FILE__);
 		return -ENODEV;
 	}
 
-	r = gpio_direction_input(phy->gpio_irq);
-	if (r) {
-		pr_err("%s : gpio_direction_input failed\n", __FILE__);
-		return -ENODEV;
-	}
-
-	r = devm_gpio_request(&client->dev,
-			      phy->gpio_reset, "clf_reset");
+	r = devm_gpio_request_one(&client->dev,
+			phy->gpio_reset, GPIOF_OUT_INIT_HIGH, "clf_reset");
 	if (r) {
 		pr_err("%s : reset gpio_request failed\n", __FILE__);
 		return -ENODEV;
 	}
 
-	r = gpio_direction_output(phy->gpio_reset, 1);
-	if (r) {
-		pr_err("%s : reset gpio_direction_output failed\n",
-			__FILE__);
-		return -ENODEV;
-	}
-
 	/* IRQ */
 	irq = gpio_to_irq(phy->gpio_irq);
 	if (irq < 0) {
@@ -448,7 +410,6 @@ static struct i2c_driver st21nfcb_nci_i2c_driver = {
 	.driver = {
 		.owner = THIS_MODULE,
 		.name = ST21NFCB_NCI_I2C_DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(of_st21nfcb_i2c_match),
 	},
 	.probe = st21nfcb_nci_i2c_probe,
diff --git a/drivers/nfc/st21nfcb/ndlc.c b/drivers/nfc/st21nfcb/ndlc.c
index 83c97c36112b..e7bff8921d11 100644
--- a/drivers/nfc/st21nfcb/ndlc.c
+++ b/drivers/nfc/st21nfcb/ndlc.c
@@ -112,6 +112,10 @@ static void llt_ndlc_send_queue(struct llt_ndlc *ndlc)
 		ndlc->t1_active = true;
 		mod_timer(&ndlc->t1_timer, time_sent +
 			msecs_to_jiffies(NDLC_TIMER_T1));
+		/* start timer t2 for chip availability */
+		ndlc->t2_active = true;
+		mod_timer(&ndlc->t2_timer, time_sent +
+			msecs_to_jiffies(NDLC_TIMER_T2));
 	}
 }
 
@@ -207,7 +211,7 @@ static void llt_ndlc_sm_work(struct work_struct *work)
 		ndlc->t2_active = false;
 		ndlc->t1_active = false;
 		del_timer_sync(&ndlc->t1_timer);
-
+		del_timer_sync(&ndlc->t2_timer);
 		ndlc_close(ndlc);
 		ndlc->hard_fault = -EREMOTEIO;
 	}
diff --git a/drivers/nfc/st21nfcb/ndlc.h b/drivers/nfc/st21nfcb/ndlc.h
index c30a2f0faa5f..b28140e0cd78 100644
--- a/drivers/nfc/st21nfcb/ndlc.h
+++ b/drivers/nfc/st21nfcb/ndlc.h
@@ -42,6 +42,10 @@ struct llt_ndlc {
 
 	struct device *dev;
 
+	/*
+	 * < 0 if hardware error occured
+	 * and prevents normal operation.
+	 */
 	int hard_fault;
 };
 
diff --git a/drivers/nfc/st21nfcb/st21nfcb.c b/drivers/nfc/st21nfcb/st21nfcb.c
index 4d95863e3063..ea63d5877831 100644
--- a/drivers/nfc/st21nfcb/st21nfcb.c
+++ b/drivers/nfc/st21nfcb/st21nfcb.c
@@ -22,10 +22,11 @@
 #include <net/nfc/nci_core.h>
 
 #include "st21nfcb.h"
-#include "ndlc.h"
 
 #define DRIVER_DESC "NCI NFC driver for ST21NFCB"
 
+#define ST21NFCB_NCI1_X_PROPRIETARY_ISO15693 0x83
+
 static int st21nfcb_nci_open(struct nci_dev *ndev)
 {
 	struct st21nfcb_nci_info *info = nci_get_drvdata(ndev);
@@ -65,10 +66,18 @@ static int st21nfcb_nci_send(struct nci_dev *ndev, struct sk_buff *skb)
 	return ndlc_send(info->ndlc, skb);
 }
 
+static __u32 st21nfcb_nci_get_rfprotocol(struct nci_dev *ndev,
+					 __u8 rf_protocol)
+{
+	return rf_protocol == ST21NFCB_NCI1_X_PROPRIETARY_ISO15693 ?
+		NFC_PROTO_ISO15693_MASK : 0;
+}
+
 static struct nci_ops st21nfcb_nci_ops = {
 	.open = st21nfcb_nci_open,
 	.close = st21nfcb_nci_close,
 	.send = st21nfcb_nci_send,
+	.get_rfprotocol = st21nfcb_nci_get_rfprotocol,
 };
 
 int st21nfcb_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
@@ -88,29 +97,25 @@ int st21nfcb_nci_probe(struct llt_ndlc *ndlc, int phy_headroom,
 		| NFC_PROTO_FELICA_MASK
 		| NFC_PROTO_ISO14443_MASK
 		| NFC_PROTO_ISO14443_B_MASK
+		| NFC_PROTO_ISO15693_MASK
 		| NFC_PROTO_NFC_DEP_MASK;
 
 	ndlc->ndev = nci_allocate_device(&st21nfcb_nci_ops, protocols,
 					phy_headroom, phy_tailroom);
 	if (!ndlc->ndev) {
 		pr_err("Cannot allocate nfc ndev\n");
-		r = -ENOMEM;
-		goto err_alloc_ndev;
+		return -ENOMEM;
 	}
 	info->ndlc = ndlc;
 
 	nci_set_drvdata(ndlc->ndev, info);
 
 	r = nci_register_device(ndlc->ndev);
-	if (r)
-		goto err_regdev;
-
-	return r;
-err_regdev:
-	nci_free_device(ndlc->ndev);
+	if (r) {
+		pr_err("Cannot register nfc device to nci core\n");
+		nci_free_device(ndlc->ndev);
+	}
 
-err_alloc_ndev:
-	kfree(info);
 	return r;
 }
 EXPORT_SYMBOL_GPL(st21nfcb_nci_probe);
diff --git a/drivers/nfc/st21nfcb/st21nfcb.h b/drivers/nfc/st21nfcb/st21nfcb.h
index 4bbbebb9f34d..ea58a56ad794 100644
--- a/drivers/nfc/st21nfcb/st21nfcb.h
+++ b/drivers/nfc/st21nfcb/st21nfcb.h
@@ -19,8 +19,6 @@
 #ifndef __LOCAL_ST21NFCB_H_
 #define __LOCAL_ST21NFCB_H_
 
-#include <net/nfc/nci_core.h>
-
 #include "ndlc.h"
 
 /* Define private flags: */
diff --git a/drivers/nfc/trf7970a.c b/drivers/nfc/trf7970a.c
index 3b78b031e617..d2ccd2890647 100644
--- a/drivers/nfc/trf7970a.c
+++ b/drivers/nfc/trf7970a.c
@@ -36,7 +36,13 @@
  * The trf7970a is very timing sensitive and the VIN, EN2, and EN
  * pins must asserted in that order and with specific delays in between.
  * The delays used in the driver were provided by TI and have been
- * confirmed to work with this driver.
+ * confirmed to work with this driver.  There is a bug with the current
+ * version of the trf7970a that requires that EN2 remain low no matter
+ * what.  If it goes high, it will generate an RF field even when in
+ * passive target mode.  TI has indicated that the chip will work okay
+ * when EN2 is left low.  The 'en2-rf-quirk' device tree property
+ * indicates that trf7970a currently being used has the erratum and
+ * that EN2 must be kept low.
  *
  * Timeouts are implemented using the delayed workqueue kernel facility.
  * Timeouts are required so things don't hang when there is no response
@@ -56,7 +62,7 @@
  * way to abort a command that's already been sent to the tag is so turn
  * off power to the tag.  If we do that, though, we'd have to go through
  * the entire anticollision procedure again but the digital layer doesn't
- * support that.  So, if an abort is received before trf7970a_in_send_cmd()
+ * support that.  So, if an abort is received before trf7970a_send_cmd()
  * has sent the command to the tag, it simply returns -ECANCELED.  If the
  * command has already been sent to the tag, then the driver continues
  * normally and recieves the response data (or error) but just before
@@ -77,6 +83,13 @@
  * been received and there isn't an error).  The delay is 20 ms since delays
  * of ~16 ms have been observed during testing.
  *
+ * When transmitting a frame larger than the FIFO size (127 bytes), the
+ * driver will wait 20 ms for the FIFO to drain past the low-watermark
+ * and generate an interrupt.  The low-watermark set to 32 bytes so the
+ * interrupt should fire after 127 - 32 = 95 bytes have been sent.  At
+ * the lowest possible bit rate (6.62 kbps for 15693), it will take up
+ * to ~14.35 ms so 20 ms is used for the timeout.
+ *
  * Type 2 write and sector select commands respond with a 4-bit ACK or NACK.
  * Having only 4 bits in the FIFO won't normally generate an interrupt so
  * driver enables the '4_bit_RX' bit of the Special Functions register 1
@@ -99,40 +112,43 @@
  * Note under Table 1-1 in section 1.6 of
  * http://www.ti.com/lit/ug/scbu011/scbu011.pdf, that wait should be at least
  * 10 ms for TI Tag-it HF-I tags; however testing has shown that is not long
- * enough.  For this reason, the driver waits 20 ms which seems to work
+ * enough so 20 ms is used.  So the timer is set to 40 ms - 20 ms to drain
+ * up to 127 bytes in the FIFO at the lowest bit rate plus another 20 ms to
+ * ensure the wait is long enough before sending the EOF.  This seems to work
  * reliably.
  */
 
 #define TRF7970A_SUPPORTED_PROTOCOLS \
 		(NFC_PROTO_MIFARE_MASK | NFC_PROTO_ISO14443_MASK |	\
 		 NFC_PROTO_ISO14443_B_MASK | NFC_PROTO_FELICA_MASK | \
-		 NFC_PROTO_ISO15693_MASK)
+		 NFC_PROTO_ISO15693_MASK | NFC_PROTO_NFC_DEP_MASK)
 
 #define TRF7970A_AUTOSUSPEND_DELAY		30000 /* 30 seconds */
 
-/* TX data must be prefixed with a FIFO reset cmd, a cmd that depends
- * on what the current framing is, the address of the TX length byte 1
- * register (0x1d), and the 2 byte length of the data to be transmitted.
- * That totals 5 bytes.
- */
-#define TRF7970A_TX_SKB_HEADROOM		5
-
 #define TRF7970A_RX_SKB_ALLOC_SIZE		256
 
-#define TRF7970A_FIFO_SIZE			128
+#define TRF7970A_FIFO_SIZE			127
 
 /* TX length is 3 nibbles long ==> 4KB - 1 bytes max */
 #define TRF7970A_TX_MAX				(4096 - 1)
 
+#define TRF7970A_WAIT_FOR_TX_IRQ		20
 #define TRF7970A_WAIT_FOR_RX_DATA_TIMEOUT	20
-#define TRF7970A_WAIT_FOR_FIFO_DRAIN_TIMEOUT	3
-#define TRF7970A_WAIT_TO_ISSUE_ISO15693_EOF	20
+#define TRF7970A_WAIT_FOR_FIFO_DRAIN_TIMEOUT	20
+#define TRF7970A_WAIT_TO_ISSUE_ISO15693_EOF	40
+
+/* Guard times for various RF technologies (in us) */
+#define TRF7970A_GUARD_TIME_NFCA		5000
+#define TRF7970A_GUARD_TIME_NFCB		5000
+#define TRF7970A_GUARD_TIME_NFCF		20000
+#define TRF7970A_GUARD_TIME_15693		1000
 
 /* Quirks */
 /* Erratum: When reading IRQ Status register on trf7970a, we must issue a
  * read continuous command for IRQ Status and Collision Position registers.
  */
-#define TRF7970A_QUIRK_IRQ_STATUS_READ_ERRATA	BIT(0)
+#define TRF7970A_QUIRK_IRQ_STATUS_READ		BIT(0)
+#define TRF7970A_QUIRK_EN2_MUST_STAY_LOW	BIT(1)
 
 /* Direct commands */
 #define TRF7970A_CMD_IDLE			0x00
@@ -149,8 +165,8 @@
 #define TRF7970A_CMD_CLOSE_SLOT			0x15
 #define TRF7970A_CMD_BLOCK_RX			0x16
 #define TRF7970A_CMD_ENABLE_RX			0x17
-#define TRF7970A_CMD_TEST_EXT_RF		0x18
-#define TRF7970A_CMD_TEST_INT_RF		0x19
+#define TRF7970A_CMD_TEST_INT_RF		0x18
+#define TRF7970A_CMD_TEST_EXT_RF		0x19
 #define TRF7970A_CMD_RX_GAIN_ADJUST		0x1a
 
 /* Bits determining whether its a direct command or register R/W,
@@ -224,6 +240,15 @@
 #define TRF7970A_ISO_CTRL_14443B_848		0x0f
 #define TRF7970A_ISO_CTRL_FELICA_212		0x1a
 #define TRF7970A_ISO_CTRL_FELICA_424		0x1b
+#define TRF7970A_ISO_CTRL_NFC_NFCA_106		0x01
+#define TRF7970A_ISO_CTRL_NFC_NFCF_212		0x02
+#define TRF7970A_ISO_CTRL_NFC_NFCF_424		0x03
+#define TRF7970A_ISO_CTRL_NFC_CE_14443A		0x00
+#define TRF7970A_ISO_CTRL_NFC_CE_14443B		0x01
+#define TRF7970A_ISO_CTRL_NFC_CE		BIT(2)
+#define TRF7970A_ISO_CTRL_NFC_ACTIVE		BIT(3)
+#define TRF7970A_ISO_CTRL_NFC_INITIATOR		BIT(4)
+#define TRF7970A_ISO_CTRL_NFC_NFC_CE_MODE	BIT(5)
 #define TRF7970A_ISO_CTRL_RFID			BIT(5)
 #define TRF7970A_ISO_CTRL_DIR_MODE		BIT(6)
 #define TRF7970A_ISO_CTRL_RX_CRC_N		BIT(7)	/* true == No CRC */
@@ -249,12 +274,32 @@
 #define TRF7970A_MODULATOR_EN_OOK		BIT(6)
 #define TRF7970A_MODULATOR_27MHZ		BIT(7)
 
+#define TRF7970A_RX_SPECIAL_SETTINGS_NO_LIM	BIT(0)
+#define TRF7970A_RX_SPECIAL_SETTINGS_AGCR	BIT(1)
+#define TRF7970A_RX_SPECIAL_SETTINGS_GD_0DB	(0x0 << 2)
+#define TRF7970A_RX_SPECIAL_SETTINGS_GD_5DB	(0x1 << 2)
+#define TRF7970A_RX_SPECIAL_SETTINGS_GD_10DB	(0x2 << 2)
+#define TRF7970A_RX_SPECIAL_SETTINGS_GD_15DB	(0x3 << 2)
+#define TRF7970A_RX_SPECIAL_SETTINGS_HBT	BIT(4)
+#define TRF7970A_RX_SPECIAL_SETTINGS_M848	BIT(5)
+#define TRF7970A_RX_SPECIAL_SETTINGS_C424	BIT(6)
+#define TRF7970A_RX_SPECIAL_SETTINGS_C212	BIT(7)
+
+#define TRF7970A_REG_IO_CTRL_VRS(v)		((v) & 0x07)
+#define TRF7970A_REG_IO_CTRL_IO_LOW		BIT(5)
+#define TRF7970A_REG_IO_CTRL_EN_EXT_PA		BIT(6)
+#define TRF7970A_REG_IO_CTRL_AUTO_REG		BIT(7)
+
 /* IRQ Status Register Bits */
 #define TRF7970A_IRQ_STATUS_NORESP		BIT(0) /* ISO15693 only */
+#define TRF7970A_IRQ_STATUS_NFC_COL_ERROR	BIT(0)
 #define TRF7970A_IRQ_STATUS_COL			BIT(1)
 #define TRF7970A_IRQ_STATUS_FRAMING_EOF_ERROR	BIT(2)
+#define TRF7970A_IRQ_STATUS_NFC_RF		BIT(2)
 #define TRF7970A_IRQ_STATUS_PARITY_ERROR	BIT(3)
+#define TRF7970A_IRQ_STATUS_NFC_SDD		BIT(3)
 #define TRF7970A_IRQ_STATUS_CRC_ERROR		BIT(4)
+#define TRF7970A_IRQ_STATUS_NFC_PROTO_ERROR	BIT(4)
 #define TRF7970A_IRQ_STATUS_FIFO		BIT(5)
 #define TRF7970A_IRQ_STATUS_SRX			BIT(6)
 #define TRF7970A_IRQ_STATUS_TX			BIT(7)
@@ -265,6 +310,10 @@
 		 TRF7970A_IRQ_STATUS_PARITY_ERROR |		\
 		 TRF7970A_IRQ_STATUS_CRC_ERROR)
 
+#define TRF7970A_RSSI_OSC_STATUS_RSSI_MASK	(BIT(2) | BIT(1) | BIT(0))
+#define TRF7970A_RSSI_OSC_STATUS_RSSI_X_MASK	(BIT(5) | BIT(4) | BIT(3))
+#define TRF7970A_RSSI_OSC_STATUS_RSSI_OSC_OK	BIT(6)
+
 #define TRF7970A_SPECIAL_FCN_REG1_COL_7_6		BIT(0)
 #define TRF7970A_SPECIAL_FCN_REG1_14_ANTICOLL		BIT(1)
 #define TRF7970A_SPECIAL_FCN_REG1_4_BIT_RX		BIT(2)
@@ -281,6 +330,49 @@
 #define TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLL_16	0x2
 #define TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLL_32	0x3
 
+#define TRF7970A_NFC_LOW_FIELD_LEVEL_RFDET(v)	((v) & 0x07)
+#define TRF7970A_NFC_LOW_FIELD_LEVEL_CLEX_DIS	BIT(7)
+
+#define TRF7970A_NFC_TARGET_LEVEL_RFDET(v)	((v) & 0x07)
+#define TRF7970A_NFC_TARGET_LEVEL_HI_RF		BIT(3)
+#define TRF7970A_NFC_TARGET_LEVEL_SDD_EN	BIT(3)
+#define TRF7970A_NFC_TARGET_LEVEL_LD_S_4BYTES	(0x0 << 6)
+#define TRF7970A_NFC_TARGET_LEVEL_LD_S_7BYTES	(0x1 << 6)
+#define TRF7970A_NFC_TARGET_LEVEL_LD_S_10BYTES	(0x2 << 6)
+
+#define TRF79070A_NFC_TARGET_PROTOCOL_NFCBR_106		BIT(0)
+#define TRF79070A_NFC_TARGET_PROTOCOL_NFCBR_212		BIT(1)
+#define TRF79070A_NFC_TARGET_PROTOCOL_NFCBR_424		(BIT(0) | BIT(1))
+#define TRF79070A_NFC_TARGET_PROTOCOL_PAS_14443B	BIT(2)
+#define TRF79070A_NFC_TARGET_PROTOCOL_PAS_106		BIT(3)
+#define TRF79070A_NFC_TARGET_PROTOCOL_FELICA		BIT(4)
+#define TRF79070A_NFC_TARGET_PROTOCOL_RF_L		BIT(6)
+#define TRF79070A_NFC_TARGET_PROTOCOL_RF_H		BIT(7)
+
+#define TRF79070A_NFC_TARGET_PROTOCOL_106A		\
+	 (TRF79070A_NFC_TARGET_PROTOCOL_RF_H |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_RF_L |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_PAS_106 |	\
+	  TRF79070A_NFC_TARGET_PROTOCOL_NFCBR_106)
+
+#define TRF79070A_NFC_TARGET_PROTOCOL_106B		\
+	 (TRF79070A_NFC_TARGET_PROTOCOL_RF_H |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_RF_L |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_PAS_14443B |	\
+	  TRF79070A_NFC_TARGET_PROTOCOL_NFCBR_106)
+
+#define TRF79070A_NFC_TARGET_PROTOCOL_212F		\
+	 (TRF79070A_NFC_TARGET_PROTOCOL_RF_H |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_RF_L |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_FELICA |	\
+	  TRF79070A_NFC_TARGET_PROTOCOL_NFCBR_212)
+
+#define TRF79070A_NFC_TARGET_PROTOCOL_424F		\
+	 (TRF79070A_NFC_TARGET_PROTOCOL_RF_H |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_RF_L |		\
+	  TRF79070A_NFC_TARGET_PROTOCOL_FELICA |	\
+	  TRF79070A_NFC_TARGET_PROTOCOL_NFCBR_424)
+
 #define TRF7970A_FIFO_STATUS_OVERFLOW		BIT(7)
 
 /* NFC (ISO/IEC 14443A) Type 2 Tag commands */
@@ -317,13 +409,16 @@
 		(ISO15693_REQ_FLAG_SUB_CARRIER | ISO15693_REQ_FLAG_DATA_RATE)
 
 enum trf7970a_state {
-	TRF7970A_ST_OFF,
+	TRF7970A_ST_PWR_OFF,
+	TRF7970A_ST_RF_OFF,
 	TRF7970A_ST_IDLE,
 	TRF7970A_ST_IDLE_RX_BLOCKED,
 	TRF7970A_ST_WAIT_FOR_TX_FIFO,
 	TRF7970A_ST_WAIT_FOR_RX_DATA,
 	TRF7970A_ST_WAIT_FOR_RX_DATA_CONT,
 	TRF7970A_ST_WAIT_TO_ISSUE_EOF,
+	TRF7970A_ST_LISTENING,
+	TRF7970A_ST_LISTENING_MD,
 	TRF7970A_ST_MAX
 };
 
@@ -334,6 +429,7 @@ struct trf7970a {
 	struct regulator		*regulator;
 	struct nfc_digital_dev		*ddev;
 	u32				quirks;
+	bool				is_initiator;
 	bool				aborting;
 	struct sk_buff			*tx_skb;
 	struct sk_buff			*rx_skb;
@@ -344,8 +440,10 @@ struct trf7970a {
 	u8				iso_ctrl_tech;
 	u8				modulator_sys_clk_ctrl;
 	u8				special_fcn_reg1;
+	unsigned int			guard_time;
 	int				technology;
 	int				framing;
+	u8				md_rf_tech;
 	u8				tx_cmd;
 	bool				issue_eof;
 	int				en2_gpio;
@@ -386,15 +484,28 @@ static int trf7970a_read(struct trf7970a *trf, u8 reg, u8 *val)
 	return ret;
 }
 
-static int trf7970a_read_cont(struct trf7970a *trf, u8 reg,
-		u8 *buf, size_t len)
+static int trf7970a_read_cont(struct trf7970a *trf, u8 reg, u8 *buf, size_t len)
 {
 	u8 addr = reg | TRF7970A_CMD_BIT_RW | TRF7970A_CMD_BIT_CONTINUOUS;
+	struct spi_transfer t[2];
+	struct spi_message m;
 	int ret;
 
 	dev_dbg(trf->dev, "read_cont(0x%x, %zd)\n", addr, len);
 
-	ret = spi_write_then_read(trf->spi, &addr, 1, buf, len);
+	spi_message_init(&m);
+
+	memset(&t, 0, sizeof(t));
+
+	t[0].tx_buf = &addr;
+	t[0].len = sizeof(addr);
+	spi_message_add_tail(&t[0], &m);
+
+	t[1].rx_buf = buf;
+	t[1].len = len;
+	spi_message_add_tail(&t[1], &m);
+
+	ret = spi_sync(trf->spi, &m);
 	if (ret)
 		dev_err(trf->dev, "%s - addr: 0x%x, ret: %d\n", __func__, addr,
 				ret);
@@ -424,7 +535,7 @@ static int trf7970a_read_irqstatus(struct trf7970a *trf, u8 *status)
 
 	addr = TRF7970A_IRQ_STATUS | TRF7970A_CMD_BIT_RW;
 
-	if (trf->quirks & TRF7970A_QUIRK_IRQ_STATUS_READ_ERRATA) {
+	if (trf->quirks & TRF7970A_QUIRK_IRQ_STATUS_READ) {
 		addr |= TRF7970A_CMD_BIT_CONTINUOUS;
 		ret = spi_write_then_read(trf->spi, &addr, 1, buf, 2);
 	} else {
@@ -440,10 +551,60 @@ static int trf7970a_read_irqstatus(struct trf7970a *trf, u8 *status)
 	return ret;
 }
 
-static void trf7970a_send_upstream(struct trf7970a *trf)
+static int trf7970a_read_target_proto(struct trf7970a *trf, u8 *target_proto)
 {
-	u8 rssi;
+	int ret;
+	u8 buf[2];
+	u8 addr;
 
+	addr = TRF79070A_NFC_TARGET_PROTOCOL | TRF7970A_CMD_BIT_RW |
+		TRF7970A_CMD_BIT_CONTINUOUS;
+
+	ret = spi_write_then_read(trf->spi, &addr, 1, buf, 2);
+	if (ret)
+		dev_err(trf->dev, "%s - target_proto: Read failed: %d\n",
+				__func__, ret);
+	else
+		*target_proto = buf[0];
+
+	return ret;
+}
+
+static int trf7970a_mode_detect(struct trf7970a *trf, u8 *rf_tech)
+{
+	int ret;
+	u8 target_proto, tech;
+
+	ret = trf7970a_read_target_proto(trf, &target_proto);
+	if (ret)
+		return ret;
+
+	switch (target_proto) {
+	case TRF79070A_NFC_TARGET_PROTOCOL_106A:
+		tech = NFC_DIGITAL_RF_TECH_106A;
+		break;
+	case TRF79070A_NFC_TARGET_PROTOCOL_106B:
+		tech = NFC_DIGITAL_RF_TECH_106B;
+		break;
+	case TRF79070A_NFC_TARGET_PROTOCOL_212F:
+		tech = NFC_DIGITAL_RF_TECH_212F;
+		break;
+	case TRF79070A_NFC_TARGET_PROTOCOL_424F:
+		tech = NFC_DIGITAL_RF_TECH_424F;
+		break;
+	default:
+		dev_dbg(trf->dev, "%s - mode_detect: target_proto: 0x%x\n",
+				__func__, target_proto);
+		return -EIO;
+	}
+
+	*rf_tech = tech;
+
+	return ret;
+}
+
+static void trf7970a_send_upstream(struct trf7970a *trf)
+{
 	dev_kfree_skb_any(trf->tx_skb);
 	trf->tx_skb = NULL;
 
@@ -452,13 +613,6 @@ static void trf7970a_send_upstream(struct trf7970a *trf)
 				16, 1, trf->rx_skb->data, trf->rx_skb->len,
 				false);
 
-	/* According to the manual it is "good form" to reset the fifo and
-	 * read the RSSI levels & oscillator status register here.  It doesn't
-	 * explain why.
-	 */
-	trf7970a_cmd(trf, TRF7970A_CMD_FIFO_RESET);
-	trf7970a_read(trf, TRF7970A_RSSI_OSC_STATUS, &rssi);
-
 	trf->state = TRF7970A_ST_IDLE;
 
 	if (trf->aborting) {
@@ -481,6 +635,8 @@ static void trf7970a_send_err_upstream(struct trf7970a *trf, int errno)
 {
 	dev_dbg(trf->dev, "Error - state: %d, errno: %d\n", trf->state, errno);
 
+	cancel_delayed_work(&trf->timeout_work);
+
 	kfree_skb(trf->rx_skb);
 	trf->rx_skb = ERR_PTR(errno);
 
@@ -488,15 +644,29 @@ static void trf7970a_send_err_upstream(struct trf7970a *trf, int errno)
 }
 
 static int trf7970a_transmit(struct trf7970a *trf, struct sk_buff *skb,
-		unsigned int len)
+		unsigned int len, u8 *prefix, unsigned int prefix_len)
 {
+	struct spi_transfer t[2];
+	struct spi_message m;
 	unsigned int timeout;
 	int ret;
 
 	print_hex_dump_debug("trf7970a tx data: ", DUMP_PREFIX_NONE,
 			16, 1, skb->data, len, false);
 
-	ret = spi_write(trf->spi, skb->data, len);
+	spi_message_init(&m);
+
+	memset(&t, 0, sizeof(t));
+
+	t[0].tx_buf = prefix;
+	t[0].len = prefix_len;
+	spi_message_add_tail(&t[0], &m);
+
+	t[1].tx_buf = skb->data;
+	t[1].len = len;
+	spi_message_add_tail(&t[1], &m);
+
+	ret = spi_sync(trf->spi, &m);
 	if (ret) {
 		dev_err(trf->dev, "%s - Can't send tx data: %d\n", __func__,
 				ret);
@@ -514,7 +684,11 @@ static int trf7970a_transmit(struct trf7970a *trf, struct sk_buff *skb,
 			timeout = TRF7970A_WAIT_TO_ISSUE_ISO15693_EOF;
 		} else {
 			trf->state = TRF7970A_ST_WAIT_FOR_RX_DATA;
-			timeout = trf->timeout;
+
+			if (!trf->timeout)
+				timeout = TRF7970A_WAIT_FOR_TX_IRQ;
+			else
+				timeout = trf->timeout;
 		}
 	}
 
@@ -532,6 +706,7 @@ static void trf7970a_fill_fifo(struct trf7970a *trf)
 	unsigned int len;
 	int ret;
 	u8 fifo_bytes;
+	u8 prefix;
 
 	ret = trf7970a_read(trf, TRF7970A_FIFO_STATUS, &fifo_bytes);
 	if (ret) {
@@ -541,18 +716,21 @@ static void trf7970a_fill_fifo(struct trf7970a *trf)
 
 	dev_dbg(trf->dev, "Filling FIFO - fifo_bytes: 0x%x\n", fifo_bytes);
 
-	if (fifo_bytes & TRF7970A_FIFO_STATUS_OVERFLOW) {
-		dev_err(trf->dev, "%s - fifo overflow: 0x%x\n", __func__,
-				fifo_bytes);
-		trf7970a_send_err_upstream(trf, -EIO);
-		return;
-	}
+	fifo_bytes &= ~TRF7970A_FIFO_STATUS_OVERFLOW;
 
 	/* Calculate how much more data can be written to the fifo */
 	len = TRF7970A_FIFO_SIZE - fifo_bytes;
+	if (!len) {
+		schedule_delayed_work(&trf->timeout_work,
+			msecs_to_jiffies(TRF7970A_WAIT_FOR_FIFO_DRAIN_TIMEOUT));
+		return;
+	}
+
 	len = min(skb->len, len);
 
-	ret = trf7970a_transmit(trf, skb, len);
+	prefix = TRF7970A_CMD_BIT_CONTINUOUS | TRF7970A_FIFO_IO_REGISTER;
+
+	ret = trf7970a_transmit(trf, skb, len, &prefix, sizeof(prefix));
 	if (ret)
 		trf7970a_send_err_upstream(trf, ret);
 }
@@ -576,16 +754,11 @@ static void trf7970a_drain_fifo(struct trf7970a *trf, u8 status)
 
 	dev_dbg(trf->dev, "Draining FIFO - fifo_bytes: 0x%x\n", fifo_bytes);
 
+	fifo_bytes &= ~TRF7970A_FIFO_STATUS_OVERFLOW;
+
 	if (!fifo_bytes)
 		goto no_rx_data;
 
-	if (fifo_bytes & TRF7970A_FIFO_STATUS_OVERFLOW) {
-		dev_err(trf->dev, "%s - fifo overflow: 0x%x\n", __func__,
-				fifo_bytes);
-		trf7970a_send_err_upstream(trf, -EIO);
-		return;
-	}
-
 	if (fifo_bytes > skb_tailroom(skb)) {
 		skb = skb_copy_expand(skb, skb_headroom(skb),
 				max_t(int, fifo_bytes,
@@ -615,6 +788,21 @@ static void trf7970a_drain_fifo(struct trf7970a *trf, u8 status)
 		status = TRF7970A_IRQ_STATUS_SRX;
 	} else {
 		trf->state = TRF7970A_ST_WAIT_FOR_RX_DATA_CONT;
+
+		ret = trf7970a_read(trf, TRF7970A_FIFO_STATUS, &fifo_bytes);
+		if (ret) {
+			trf7970a_send_err_upstream(trf, ret);
+			return;
+		}
+
+		fifo_bytes &= ~TRF7970A_FIFO_STATUS_OVERFLOW;
+
+		/* If there are bytes in the FIFO, set status to '0' so
+		 * the if stmt below doesn't fire and the driver will wait
+		 * for the trf7970a to generate another RX interrupt.
+		 */
+		if (fifo_bytes)
+			status = 0;
 	}
 
 no_rx_data:
@@ -634,11 +822,11 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 {
 	struct trf7970a *trf = dev_id;
 	int ret;
-	u8 status;
+	u8 status, fifo_bytes, iso_ctrl;
 
 	mutex_lock(&trf->lock);
 
-	if (trf->state == TRF7970A_ST_OFF) {
+	if (trf->state == TRF7970A_ST_RF_OFF) {
 		mutex_unlock(&trf->lock);
 		return IRQ_NONE;
 	}
@@ -660,12 +848,12 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 	switch (trf->state) {
 	case TRF7970A_ST_IDLE:
 	case TRF7970A_ST_IDLE_RX_BLOCKED:
-		/* If getting interrupts caused by RF noise, turn off the
-		 * receiver to avoid unnecessary interrupts.  It will be
-		 * turned back on in trf7970a_in_send_cmd() when the next
-		 * command is issued.
+		/* If initiator and getting interrupts caused by RF noise,
+		 * turn off the receiver to avoid unnecessary interrupts.
+		 * It will be turned back on in trf7970a_send_cmd() when
+		 * the next command is issued.
 		 */
-		if (status & TRF7970A_IRQ_STATUS_ERROR) {
+		if (trf->is_initiator && (status & TRF7970A_IRQ_STATUS_ERROR)) {
 			trf7970a_cmd(trf, TRF7970A_CMD_BLOCK_RX);
 			trf->state = TRF7970A_ST_IDLE_RX_BLOCKED;
 		}
@@ -687,8 +875,68 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 			trf->ignore_timeout =
 				!cancel_delayed_work(&trf->timeout_work);
 			trf7970a_drain_fifo(trf, status);
-		} else if (status == TRF7970A_IRQ_STATUS_TX) {
+		} else if (status & TRF7970A_IRQ_STATUS_FIFO) {
+			ret = trf7970a_read(trf, TRF7970A_FIFO_STATUS,
+					&fifo_bytes);
+
+			fifo_bytes &= ~TRF7970A_FIFO_STATUS_OVERFLOW;
+
+			if (ret)
+				trf7970a_send_err_upstream(trf, ret);
+			else if (!fifo_bytes)
+				trf7970a_cmd(trf, TRF7970A_CMD_FIFO_RESET);
+		} else if ((status == TRF7970A_IRQ_STATUS_TX) ||
+				(!trf->is_initiator &&
+				 (status == (TRF7970A_IRQ_STATUS_TX |
+					     TRF7970A_IRQ_STATUS_NFC_RF)))) {
 			trf7970a_cmd(trf, TRF7970A_CMD_FIFO_RESET);
+
+			if (!trf->timeout) {
+				trf->ignore_timeout = !cancel_delayed_work(
+						&trf->timeout_work);
+				trf->rx_skb = ERR_PTR(0);
+				trf7970a_send_upstream(trf);
+				break;
+			}
+
+			if (trf->is_initiator)
+				break;
+
+			iso_ctrl = trf->iso_ctrl;
+
+			switch (trf->framing) {
+			case NFC_DIGITAL_FRAMING_NFCA_STANDARD:
+				trf->tx_cmd = TRF7970A_CMD_TRANSMIT_NO_CRC;
+				iso_ctrl |= TRF7970A_ISO_CTRL_RX_CRC_N;
+				trf->iso_ctrl = 0xff; /* Force ISO_CTRL write */
+				break;
+			case NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A:
+				trf->tx_cmd = TRF7970A_CMD_TRANSMIT;
+				iso_ctrl &= ~TRF7970A_ISO_CTRL_RX_CRC_N;
+				trf->iso_ctrl = 0xff; /* Force ISO_CTRL write */
+				break;
+			case NFC_DIGITAL_FRAMING_NFCA_ANTICOL_COMPLETE:
+				ret = trf7970a_write(trf,
+					TRF7970A_SPECIAL_FCN_REG1,
+					TRF7970A_SPECIAL_FCN_REG1_14_ANTICOLL);
+				if (ret)
+					goto err_unlock_exit;
+
+				trf->special_fcn_reg1 =
+					TRF7970A_SPECIAL_FCN_REG1_14_ANTICOLL;
+				break;
+			default:
+				break;
+			}
+
+			if (iso_ctrl != trf->iso_ctrl) {
+				ret = trf7970a_write(trf, TRF7970A_ISO_CTRL,
+						iso_ctrl);
+				if (ret)
+					goto err_unlock_exit;
+
+				trf->iso_ctrl = iso_ctrl;
+			}
 		} else {
 			trf7970a_send_err_upstream(trf, -EIO);
 		}
@@ -697,11 +945,37 @@ static irqreturn_t trf7970a_irq(int irq, void *dev_id)
 		if (status != TRF7970A_IRQ_STATUS_TX)
 			trf7970a_send_err_upstream(trf, -EIO);
 		break;
+	case TRF7970A_ST_LISTENING:
+		if (status & TRF7970A_IRQ_STATUS_SRX) {
+			trf->ignore_timeout =
+				!cancel_delayed_work(&trf->timeout_work);
+			trf7970a_drain_fifo(trf, status);
+		} else if (!(status & TRF7970A_IRQ_STATUS_NFC_RF)) {
+			trf7970a_send_err_upstream(trf, -EIO);
+		}
+		break;
+	case TRF7970A_ST_LISTENING_MD:
+		if (status & TRF7970A_IRQ_STATUS_SRX) {
+			trf->ignore_timeout =
+				!cancel_delayed_work(&trf->timeout_work);
+
+			ret = trf7970a_mode_detect(trf, &trf->md_rf_tech);
+			if (ret) {
+				trf7970a_send_err_upstream(trf, ret);
+			} else {
+				trf->state = TRF7970A_ST_LISTENING;
+				trf7970a_drain_fifo(trf, status);
+			}
+		} else if (!(status & TRF7970A_IRQ_STATUS_NFC_RF)) {
+			trf7970a_send_err_upstream(trf, -EIO);
+		}
+		break;
 	default:
 		dev_err(trf->dev, "%s - Driver in invalid state: %d\n",
 				__func__, trf->state);
 	}
 
+err_unlock_exit:
 	mutex_unlock(&trf->lock);
 	return IRQ_HANDLED;
 }
@@ -742,7 +1016,7 @@ static void trf7970a_timeout_work_handler(struct work_struct *work)
 	if (trf->ignore_timeout)
 		trf->ignore_timeout = false;
 	else if (trf->state == TRF7970A_ST_WAIT_FOR_RX_DATA_CONT)
-		trf7970a_send_upstream(trf); /* No more rx data so send up */
+		trf7970a_drain_fifo(trf, TRF7970A_IRQ_STATUS_SRX);
 	else if (trf->state == TRF7970A_ST_WAIT_TO_ISSUE_EOF)
 		trf7970a_issue_eof(trf);
 	else
@@ -765,11 +1039,16 @@ static int trf7970a_init(struct trf7970a *trf)
 	if (ret)
 		goto err_out;
 
-	/* Must clear NFC Target Detection Level reg due to erratum */
-	ret = trf7970a_write(trf, TRF7970A_NFC_TARGET_LEVEL, 0);
+	usleep_range(1000, 2000);
+
+	trf->chip_status_ctrl &= ~TRF7970A_CHIP_STATUS_RF_ON;
+
+	ret = trf7970a_write(trf, TRF7970A_MODULATOR_SYS_CLK_CTRL, 0);
 	if (ret)
 		goto err_out;
 
+	trf->modulator_sys_clk_ctrl = 0;
+
 	ret = trf7970a_write(trf, TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS,
 			TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLH_96 |
 			TRF7970A_ADJUTABLE_FIFO_IRQ_LEVELS_WLL_32);
@@ -792,6 +1071,10 @@ err_out:
 
 static void trf7970a_switch_rf_off(struct trf7970a *trf)
 {
+	if ((trf->state == TRF7970A_ST_PWR_OFF) ||
+			(trf->state == TRF7970A_ST_RF_OFF))
+		return;
+
 	dev_dbg(trf->dev, "Switching rf off\n");
 
 	trf->chip_status_ctrl &= ~TRF7970A_CHIP_STATUS_RF_ON;
@@ -799,24 +1082,41 @@ static void trf7970a_switch_rf_off(struct trf7970a *trf)
 	trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL, trf->chip_status_ctrl);
 
 	trf->aborting = false;
-	trf->state = TRF7970A_ST_OFF;
+	trf->state = TRF7970A_ST_RF_OFF;
 
 	pm_runtime_mark_last_busy(trf->dev);
 	pm_runtime_put_autosuspend(trf->dev);
 }
 
-static void trf7970a_switch_rf_on(struct trf7970a *trf)
+static int trf7970a_switch_rf_on(struct trf7970a *trf)
 {
+	int ret;
+
 	dev_dbg(trf->dev, "Switching rf on\n");
 
 	pm_runtime_get_sync(trf->dev);
 
+	if (trf->state != TRF7970A_ST_RF_OFF) { /* Power on, RF off */
+		dev_err(trf->dev, "%s - Incorrect state: %d\n", __func__,
+				trf->state);
+		return -EINVAL;
+	}
+
+	ret = trf7970a_init(trf);
+	if (ret) {
+		dev_err(trf->dev, "%s - Can't initialize: %d\n", __func__, ret);
+		return ret;
+	}
+
 	trf->state = TRF7970A_ST_IDLE;
+
+	return 0;
 }
 
 static int trf7970a_switch_rf(struct nfc_digital_dev *ddev, bool on)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
+	int ret = 0;
 
 	dev_dbg(trf->dev, "Switching RF - state: %d, on: %d\n", trf->state, on);
 
@@ -824,8 +1124,9 @@ static int trf7970a_switch_rf(struct nfc_digital_dev *ddev, bool on)
 
 	if (on) {
 		switch (trf->state) {
-		case TRF7970A_ST_OFF:
-			trf7970a_switch_rf_on(trf);
+		case TRF7970A_ST_PWR_OFF:
+		case TRF7970A_ST_RF_OFF:
+			ret = trf7970a_switch_rf_on(trf);
 			break;
 		case TRF7970A_ST_IDLE:
 		case TRF7970A_ST_IDLE_RX_BLOCKED:
@@ -834,26 +1135,31 @@ static int trf7970a_switch_rf(struct nfc_digital_dev *ddev, bool on)
 			dev_err(trf->dev, "%s - Invalid request: %d %d\n",
 					__func__, trf->state, on);
 			trf7970a_switch_rf_off(trf);
+			ret = -EINVAL;
 		}
 	} else {
 		switch (trf->state) {
-		case TRF7970A_ST_OFF:
+		case TRF7970A_ST_PWR_OFF:
+		case TRF7970A_ST_RF_OFF:
 			break;
 		default:
 			dev_err(trf->dev, "%s - Invalid request: %d %d\n",
 					__func__, trf->state, on);
+			ret = -EINVAL;
 			/* FALLTHROUGH */
 		case TRF7970A_ST_IDLE:
 		case TRF7970A_ST_IDLE_RX_BLOCKED:
+		case TRF7970A_ST_WAIT_FOR_RX_DATA:
+		case TRF7970A_ST_WAIT_FOR_RX_DATA_CONT:
 			trf7970a_switch_rf_off(trf);
 		}
 	}
 
 	mutex_unlock(&trf->lock);
-	return 0;
+	return ret;
 }
 
-static int trf7970a_config_rf_tech(struct trf7970a *trf, int tech)
+static int trf7970a_in_config_rf_tech(struct trf7970a *trf, int tech)
 {
 	int ret = 0;
 
@@ -863,22 +1169,27 @@ static int trf7970a_config_rf_tech(struct trf7970a *trf, int tech)
 	case NFC_DIGITAL_RF_TECH_106A:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_14443A_106;
 		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_OOK;
+		trf->guard_time = TRF7970A_GUARD_TIME_NFCA;
 		break;
 	case NFC_DIGITAL_RF_TECH_106B:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_14443B_106;
 		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_ASK10;
+		trf->guard_time = TRF7970A_GUARD_TIME_NFCB;
 		break;
 	case NFC_DIGITAL_RF_TECH_212F:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_FELICA_212;
 		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_ASK10;
+		trf->guard_time = TRF7970A_GUARD_TIME_NFCF;
 		break;
 	case NFC_DIGITAL_RF_TECH_424F:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_FELICA_424;
 		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_ASK10;
+		trf->guard_time = TRF7970A_GUARD_TIME_NFCF;
 		break;
 	case NFC_DIGITAL_RF_TECH_ISO15693:
 		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_15693_SGL_1OF4_2648;
 		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_OOK;
+		trf->guard_time = TRF7970A_GUARD_TIME_15693;
 		break;
 	default:
 		dev_dbg(trf->dev, "Unsupported rf technology: %d\n", tech);
@@ -887,12 +1198,54 @@ static int trf7970a_config_rf_tech(struct trf7970a *trf, int tech)
 
 	trf->technology = tech;
 
+	/* If in initiator mode and not changing the RF tech due to a
+	 * PSL sequence (indicated by 'trf->iso_ctrl == 0xff' from
+	 * trf7970a_init()), clear the NFC Target Detection Level register
+	 * due to erratum.
+	 */
+	if (trf->iso_ctrl == 0xff)
+		ret = trf7970a_write(trf, TRF7970A_NFC_TARGET_LEVEL, 0);
+
 	return ret;
 }
 
-static int trf7970a_config_framing(struct trf7970a *trf, int framing)
+static int trf7970a_is_rf_field(struct trf7970a *trf, bool *is_rf_field)
+{
+	int ret;
+	u8 rssi;
+
+	ret = trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL,
+			trf->chip_status_ctrl | TRF7970A_CHIP_STATUS_REC_ON);
+	if (ret)
+		return ret;
+
+	ret = trf7970a_cmd(trf, TRF7970A_CMD_TEST_EXT_RF);
+	if (ret)
+		return ret;
+
+	usleep_range(50, 60);
+
+	ret = trf7970a_read(trf, TRF7970A_RSSI_OSC_STATUS, &rssi);
+	if (ret)
+		return ret;
+
+	ret = trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL,
+			trf->chip_status_ctrl);
+	if (ret)
+		return ret;
+
+	if (rssi & TRF7970A_RSSI_OSC_STATUS_RSSI_MASK)
+		*is_rf_field = true;
+	else
+		*is_rf_field = false;
+
+	return 0;
+}
+
+static int trf7970a_in_config_framing(struct trf7970a *trf, int framing)
 {
 	u8 iso_ctrl = trf->iso_ctrl_tech;
+	bool is_rf_field = false;
 	int ret;
 
 	dev_dbg(trf->dev, "framing: %d\n", framing);
@@ -911,6 +1264,8 @@ static int trf7970a_config_framing(struct trf7970a *trf, int framing)
 	case NFC_DIGITAL_FRAMING_NFCF_T3T:
 	case NFC_DIGITAL_FRAMING_ISO15693_INVENTORY:
 	case NFC_DIGITAL_FRAMING_ISO15693_T5T:
+	case NFC_DIGITAL_FRAMING_NFCA_NFC_DEP:
+	case NFC_DIGITAL_FRAMING_NFCF_NFC_DEP:
 		trf->tx_cmd = TRF7970A_CMD_TRANSMIT;
 		iso_ctrl &= ~TRF7970A_ISO_CTRL_RX_CRC_N;
 		break;
@@ -925,6 +1280,15 @@ static int trf7970a_config_framing(struct trf7970a *trf, int framing)
 
 	trf->framing = framing;
 
+	if (!(trf->chip_status_ctrl & TRF7970A_CHIP_STATUS_RF_ON)) {
+		ret = trf7970a_is_rf_field(trf, &is_rf_field);
+		if (ret)
+			return ret;
+
+		if (is_rf_field)
+			return -EBUSY;
+	}
+
 	if (iso_ctrl != trf->iso_ctrl) {
 		ret = trf7970a_write(trf, TRF7970A_ISO_CTRL, iso_ctrl);
 		if (ret)
@@ -947,7 +1311,7 @@ static int trf7970a_config_framing(struct trf7970a *trf, int framing)
 
 		trf->chip_status_ctrl |= TRF7970A_CHIP_STATUS_RF_ON;
 
-		usleep_range(5000, 6000);
+		usleep_range(trf->guard_time, trf->guard_time + 1000);
 	}
 
 	return 0;
@@ -963,21 +1327,28 @@ static int trf7970a_in_configure_hw(struct nfc_digital_dev *ddev, int type,
 
 	mutex_lock(&trf->lock);
 
-	if (trf->state == TRF7970A_ST_OFF)
-		trf7970a_switch_rf_on(trf);
+	trf->is_initiator = true;
+
+	if ((trf->state == TRF7970A_ST_PWR_OFF) ||
+			(trf->state == TRF7970A_ST_RF_OFF)) {
+		ret = trf7970a_switch_rf_on(trf);
+		if (ret)
+			goto err_unlock;
+	}
 
 	switch (type) {
 	case NFC_DIGITAL_CONFIG_RF_TECH:
-		ret = trf7970a_config_rf_tech(trf, param);
+		ret = trf7970a_in_config_rf_tech(trf, param);
 		break;
 	case NFC_DIGITAL_CONFIG_FRAMING:
-		ret = trf7970a_config_framing(trf, param);
+		ret = trf7970a_in_config_framing(trf, param);
 		break;
 	default:
 		dev_dbg(trf->dev, "Unknown type: %d\n", type);
 		ret = -EINVAL;
 	}
 
+err_unlock:
 	mutex_unlock(&trf->lock);
 	return ret;
 }
@@ -1067,14 +1438,15 @@ static int trf7970a_per_cmd_config(struct trf7970a *trf, struct sk_buff *skb)
 	return 0;
 }
 
-static int trf7970a_in_send_cmd(struct nfc_digital_dev *ddev,
+static int trf7970a_send_cmd(struct nfc_digital_dev *ddev,
 		struct sk_buff *skb, u16 timeout,
 		nfc_digital_cmd_complete_t cb, void *arg)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
-	char *prefix;
+	u8 prefix[5];
 	unsigned int len;
 	int ret;
+	u8 status;
 
 	dev_dbg(trf->dev, "New request - state: %d, timeout: %d ms, len: %d\n",
 			trf->state, timeout, skb->len);
@@ -1099,12 +1471,14 @@ static int trf7970a_in_send_cmd(struct nfc_digital_dev *ddev,
 		goto out_err;
 	}
 
-	trf->rx_skb = nfc_alloc_recv_skb(TRF7970A_RX_SKB_ALLOC_SIZE,
-			GFP_KERNEL);
-	if (!trf->rx_skb) {
-		dev_dbg(trf->dev, "Can't alloc rx_skb\n");
-		ret = -ENOMEM;
-		goto out_err;
+	if (timeout) {
+		trf->rx_skb = nfc_alloc_recv_skb(TRF7970A_RX_SKB_ALLOC_SIZE,
+				GFP_KERNEL);
+		if (!trf->rx_skb) {
+			dev_dbg(trf->dev, "Can't alloc rx_skb\n");
+			ret = -ENOMEM;
+			goto out_err;
+		}
 	}
 
 	if (trf->state == TRF7970A_ST_IDLE_RX_BLOCKED) {
@@ -1115,9 +1489,11 @@ static int trf7970a_in_send_cmd(struct nfc_digital_dev *ddev,
 		trf->state = TRF7970A_ST_IDLE;
 	}
 
-	ret = trf7970a_per_cmd_config(trf, skb);
-	if (ret)
-		goto out_err;
+	if (trf->is_initiator) {
+		ret = trf7970a_per_cmd_config(trf, skb);
+		if (ret)
+			goto out_err;
+	}
 
 	trf->ddev = ddev;
 	trf->tx_skb = skb;
@@ -1127,11 +1503,11 @@ static int trf7970a_in_send_cmd(struct nfc_digital_dev *ddev,
 	trf->ignore_timeout = false;
 
 	len = skb->len;
-	prefix = skb_push(skb, TRF7970A_TX_SKB_HEADROOM);
 
 	/* TX data must be prefixed with a FIFO reset cmd, a cmd that depends
 	 * on what the current framing is, the address of the TX length byte 1
 	 * register (0x1d), and the 2 byte length of the data to be transmitted.
+	 * That totals 5 bytes.
 	 */
 	prefix[0] = TRF7970A_CMD_BIT_CTRL |
 			TRF7970A_CMD_BIT_OPCODE(TRF7970A_CMD_FIFO_RESET);
@@ -1150,9 +1526,12 @@ static int trf7970a_in_send_cmd(struct nfc_digital_dev *ddev,
 
 	len = min_t(int, skb->len, TRF7970A_FIFO_SIZE);
 
-	usleep_range(1000, 2000);
+	/* Clear possible spurious interrupt */
+	ret = trf7970a_read_irqstatus(trf, &status);
+	if (ret)
+		goto out_err;
 
-	ret = trf7970a_transmit(trf, skb, len);
+	ret = trf7970a_transmit(trf, skb, len, prefix, sizeof(prefix));
 	if (ret) {
 		kfree_skb(trf->rx_skb);
 		trf->rx_skb = NULL;
@@ -1163,46 +1542,272 @@ out_err:
 	return ret;
 }
 
-static int trf7970a_tg_configure_hw(struct nfc_digital_dev *ddev,
-		int type, int param)
+static int trf7970a_tg_config_rf_tech(struct trf7970a *trf, int tech)
+{
+	int ret = 0;
+
+	dev_dbg(trf->dev, "rf technology: %d\n", tech);
+
+	switch (tech) {
+	case NFC_DIGITAL_RF_TECH_106A:
+		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_NFC_NFC_CE_MODE |
+			TRF7970A_ISO_CTRL_NFC_CE |
+			TRF7970A_ISO_CTRL_NFC_CE_14443A;
+		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_OOK;
+		break;
+	case NFC_DIGITAL_RF_TECH_212F:
+		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_NFC_NFC_CE_MODE |
+			TRF7970A_ISO_CTRL_NFC_NFCF_212;
+		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_ASK10;
+		break;
+	case NFC_DIGITAL_RF_TECH_424F:
+		trf->iso_ctrl_tech = TRF7970A_ISO_CTRL_NFC_NFC_CE_MODE |
+			TRF7970A_ISO_CTRL_NFC_NFCF_424;
+		trf->modulator_sys_clk_ctrl = TRF7970A_MODULATOR_DEPTH_ASK10;
+		break;
+	default:
+		dev_dbg(trf->dev, "Unsupported rf technology: %d\n", tech);
+		return -EINVAL;
+	}
+
+	trf->technology = tech;
+
+	/* Normally we write the ISO_CTRL register in
+	 * trf7970a_tg_config_framing() because the framing can change
+	 * the value written.  However, when sending a PSL RES,
+	 * digital_tg_send_psl_res_complete() doesn't call
+	 * trf7970a_tg_config_framing() so we must write the register
+	 * here.
+	 */
+	if ((trf->framing == NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED) &&
+			(trf->iso_ctrl_tech != trf->iso_ctrl)) {
+		ret = trf7970a_write(trf, TRF7970A_ISO_CTRL,
+				trf->iso_ctrl_tech);
+
+		trf->iso_ctrl = trf->iso_ctrl_tech;
+	}
+
+	return ret;
+}
+
+/* Since this is a target routine, several of the framing calls are
+ * made between receiving the request and sending the response so they
+ * should take effect until after the response is sent.  This is accomplished
+ * by skipping the ISO_CTRL register write here and doing it in the interrupt
+ * handler.
+ */
+static int trf7970a_tg_config_framing(struct trf7970a *trf, int framing)
+{
+	u8 iso_ctrl = trf->iso_ctrl_tech;
+	int ret;
+
+	dev_dbg(trf->dev, "framing: %d\n", framing);
+
+	switch (framing) {
+	case NFC_DIGITAL_FRAMING_NFCA_NFC_DEP:
+		trf->tx_cmd = TRF7970A_CMD_TRANSMIT_NO_CRC;
+		iso_ctrl |= TRF7970A_ISO_CTRL_RX_CRC_N;
+		break;
+	case NFC_DIGITAL_FRAMING_NFCA_STANDARD:
+	case NFC_DIGITAL_FRAMING_NFCA_STANDARD_WITH_CRC_A:
+	case NFC_DIGITAL_FRAMING_NFCA_ANTICOL_COMPLETE:
+		/* These ones are applied in the interrupt handler */
+		iso_ctrl = trf->iso_ctrl; /* Don't write to ISO_CTRL yet */
+		break;
+	case NFC_DIGITAL_FRAMING_NFCF_NFC_DEP:
+		trf->tx_cmd = TRF7970A_CMD_TRANSMIT;
+		iso_ctrl &= ~TRF7970A_ISO_CTRL_RX_CRC_N;
+		break;
+	case NFC_DIGITAL_FRAMING_NFC_DEP_ACTIVATED:
+		trf->tx_cmd = TRF7970A_CMD_TRANSMIT;
+		iso_ctrl &= ~TRF7970A_ISO_CTRL_RX_CRC_N;
+		break;
+	default:
+		dev_dbg(trf->dev, "Unsupported Framing: %d\n", framing);
+		return -EINVAL;
+	}
+
+	trf->framing = framing;
+
+	if (iso_ctrl != trf->iso_ctrl) {
+		ret = trf7970a_write(trf, TRF7970A_ISO_CTRL, iso_ctrl);
+		if (ret)
+			return ret;
+
+		trf->iso_ctrl = iso_ctrl;
+
+		ret = trf7970a_write(trf, TRF7970A_MODULATOR_SYS_CLK_CTRL,
+				trf->modulator_sys_clk_ctrl);
+		if (ret)
+			return ret;
+	}
+
+	if (!(trf->chip_status_ctrl & TRF7970A_CHIP_STATUS_RF_ON)) {
+		ret = trf7970a_write(trf, TRF7970A_CHIP_STATUS_CTRL,
+				trf->chip_status_ctrl |
+					TRF7970A_CHIP_STATUS_RF_ON);
+		if (ret)
+			return ret;
+
+		trf->chip_status_ctrl |= TRF7970A_CHIP_STATUS_RF_ON;
+	}
+
+	return 0;
+}
+
+static int trf7970a_tg_configure_hw(struct nfc_digital_dev *ddev, int type,
+		int param)
+{
+	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
+	int ret;
+
+	dev_dbg(trf->dev, "Configure hw - type: %d, param: %d\n", type, param);
+
+	mutex_lock(&trf->lock);
+
+	trf->is_initiator = false;
+
+	if ((trf->state == TRF7970A_ST_PWR_OFF) ||
+			(trf->state == TRF7970A_ST_RF_OFF)) {
+		ret = trf7970a_switch_rf_on(trf);
+		if (ret)
+			goto err_unlock;
+	}
+
+	switch (type) {
+	case NFC_DIGITAL_CONFIG_RF_TECH:
+		ret = trf7970a_tg_config_rf_tech(trf, param);
+		break;
+	case NFC_DIGITAL_CONFIG_FRAMING:
+		ret = trf7970a_tg_config_framing(trf, param);
+		break;
+	default:
+		dev_dbg(trf->dev, "Unknown type: %d\n", type);
+		ret = -EINVAL;
+	}
+
+err_unlock:
+	mutex_unlock(&trf->lock);
+	return ret;
+}
+
+static int _trf7970a_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
+		nfc_digital_cmd_complete_t cb, void *arg, bool mode_detect)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
+	int ret;
+
+	mutex_lock(&trf->lock);
 
-	dev_dbg(trf->dev, "Unsupported interface\n");
+	if ((trf->state != TRF7970A_ST_IDLE) &&
+			(trf->state != TRF7970A_ST_IDLE_RX_BLOCKED)) {
+		dev_err(trf->dev, "%s - Bogus state: %d\n", __func__,
+				trf->state);
+		ret = -EIO;
+		goto out_err;
+	}
 
-	return -EINVAL;
+	if (trf->aborting) {
+		dev_dbg(trf->dev, "Abort process complete\n");
+		trf->aborting = false;
+		ret = -ECANCELED;
+		goto out_err;
+	}
+
+	trf->rx_skb = nfc_alloc_recv_skb(TRF7970A_RX_SKB_ALLOC_SIZE,
+			GFP_KERNEL);
+	if (!trf->rx_skb) {
+		dev_dbg(trf->dev, "Can't alloc rx_skb\n");
+		ret = -ENOMEM;
+		goto out_err;
+	}
+
+	ret = trf7970a_write(trf, TRF7970A_RX_SPECIAL_SETTINGS,
+			TRF7970A_RX_SPECIAL_SETTINGS_HBT |
+			TRF7970A_RX_SPECIAL_SETTINGS_M848 |
+			TRF7970A_RX_SPECIAL_SETTINGS_C424 |
+			TRF7970A_RX_SPECIAL_SETTINGS_C212);
+	if (ret)
+		goto out_err;
+
+	ret = trf7970a_write(trf, TRF7970A_REG_IO_CTRL,
+			TRF7970A_REG_IO_CTRL_VRS(0x1));
+	if (ret)
+		goto out_err;
+
+	ret = trf7970a_write(trf, TRF7970A_NFC_LOW_FIELD_LEVEL,
+			TRF7970A_NFC_LOW_FIELD_LEVEL_RFDET(0x3));
+	if (ret)
+		goto out_err;
+
+	ret = trf7970a_write(trf, TRF7970A_NFC_TARGET_LEVEL,
+			TRF7970A_NFC_TARGET_LEVEL_RFDET(0x7));
+	if (ret)
+		goto out_err;
+
+	trf->ddev = ddev;
+	trf->cb = cb;
+	trf->cb_arg = arg;
+	trf->timeout = timeout;
+	trf->ignore_timeout = false;
+
+	ret = trf7970a_cmd(trf, TRF7970A_CMD_ENABLE_RX);
+	if (ret)
+		goto out_err;
+
+	trf->state = mode_detect ? TRF7970A_ST_LISTENING_MD :
+				   TRF7970A_ST_LISTENING;
+
+	schedule_delayed_work(&trf->timeout_work, msecs_to_jiffies(timeout));
+
+out_err:
+	mutex_unlock(&trf->lock);
+	return ret;
 }
 
-static int trf7970a_tg_send_cmd(struct nfc_digital_dev *ddev,
-		struct sk_buff *skb, u16 timeout,
+static int trf7970a_tg_listen(struct nfc_digital_dev *ddev, u16 timeout,
 		nfc_digital_cmd_complete_t cb, void *arg)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 
-	dev_dbg(trf->dev, "Unsupported interface\n");
+	dev_dbg(trf->dev, "Listen - state: %d, timeout: %d ms\n",
+			trf->state, timeout);
 
-	return -EINVAL;
+	return _trf7970a_tg_listen(ddev, timeout, cb, arg, false);
 }
 
-static int trf7970a_tg_listen(struct nfc_digital_dev *ddev,
+static int trf7970a_tg_listen_md(struct nfc_digital_dev *ddev,
 		u16 timeout, nfc_digital_cmd_complete_t cb, void *arg)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
+	int ret;
+
+	dev_dbg(trf->dev, "Listen MD - state: %d, timeout: %d ms\n",
+			trf->state, timeout);
 
-	dev_dbg(trf->dev, "Unsupported interface\n");
+	ret = trf7970a_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_RF_TECH,
+			NFC_DIGITAL_RF_TECH_106A);
+	if (ret)
+		return ret;
 
-	return -EINVAL;
+	ret = trf7970a_tg_configure_hw(ddev, NFC_DIGITAL_CONFIG_FRAMING,
+			NFC_DIGITAL_FRAMING_NFCA_NFC_DEP);
+	if (ret)
+		return ret;
+
+	return _trf7970a_tg_listen(ddev, timeout, cb, arg, true);
 }
 
-static int trf7970a_tg_listen_mdaa(struct nfc_digital_dev *ddev,
-		struct digital_tg_mdaa_params *mdaa_params,
-		u16 timeout, nfc_digital_cmd_complete_t cb, void *arg)
+static int trf7970a_tg_get_rf_tech(struct nfc_digital_dev *ddev, u8 *rf_tech)
 {
 	struct trf7970a *trf = nfc_digital_get_drvdata(ddev);
 
-	dev_dbg(trf->dev, "Unsupported interface\n");
+	dev_dbg(trf->dev, "Get RF Tech - state: %d, rf_tech: %d\n",
+			trf->state, trf->md_rf_tech);
 
-	return -EINVAL;
+	*rf_tech = trf->md_rf_tech;
+
+	return 0;
 }
 
 static void trf7970a_abort_cmd(struct nfc_digital_dev *ddev)
@@ -1220,6 +1825,11 @@ static void trf7970a_abort_cmd(struct nfc_digital_dev *ddev)
 	case TRF7970A_ST_WAIT_TO_ISSUE_EOF:
 		trf->aborting = true;
 		break;
+	case TRF7970A_ST_LISTENING:
+		trf->ignore_timeout = !cancel_delayed_work(&trf->timeout_work);
+		trf7970a_send_err_upstream(trf, -ECANCELED);
+		dev_dbg(trf->dev, "Abort process complete\n");
+		break;
 	default:
 		break;
 	}
@@ -1229,15 +1839,114 @@ static void trf7970a_abort_cmd(struct nfc_digital_dev *ddev)
 
 static struct nfc_digital_ops trf7970a_nfc_ops = {
 	.in_configure_hw	= trf7970a_in_configure_hw,
-	.in_send_cmd		= trf7970a_in_send_cmd,
+	.in_send_cmd		= trf7970a_send_cmd,
 	.tg_configure_hw	= trf7970a_tg_configure_hw,
-	.tg_send_cmd		= trf7970a_tg_send_cmd,
+	.tg_send_cmd		= trf7970a_send_cmd,
 	.tg_listen		= trf7970a_tg_listen,
-	.tg_listen_mdaa		= trf7970a_tg_listen_mdaa,
+	.tg_listen_md		= trf7970a_tg_listen_md,
+	.tg_get_rf_tech		= trf7970a_tg_get_rf_tech,
 	.switch_rf		= trf7970a_switch_rf,
 	.abort_cmd		= trf7970a_abort_cmd,
 };
 
+static int trf7970a_power_up(struct trf7970a *trf)
+{
+	int ret;
+
+	dev_dbg(trf->dev, "Powering up - state: %d\n", trf->state);
+
+	if (trf->state != TRF7970A_ST_PWR_OFF)
+		return 0;
+
+	ret = regulator_enable(trf->regulator);
+	if (ret) {
+		dev_err(trf->dev, "%s - Can't enable VIN: %d\n", __func__, ret);
+		return ret;
+	}
+
+	usleep_range(5000, 6000);
+
+	if (!(trf->quirks & TRF7970A_QUIRK_EN2_MUST_STAY_LOW)) {
+		gpio_set_value(trf->en2_gpio, 1);
+		usleep_range(1000, 2000);
+	}
+
+	gpio_set_value(trf->en_gpio, 1);
+
+	usleep_range(20000, 21000);
+
+	trf->state = TRF7970A_ST_RF_OFF;
+
+	return 0;
+}
+
+static int trf7970a_power_down(struct trf7970a *trf)
+{
+	int ret;
+
+	dev_dbg(trf->dev, "Powering down - state: %d\n", trf->state);
+
+	if (trf->state == TRF7970A_ST_PWR_OFF)
+		return 0;
+
+	if (trf->state != TRF7970A_ST_RF_OFF) {
+		dev_dbg(trf->dev, "Can't power down - not RF_OFF state (%d)\n",
+				trf->state);
+		return -EBUSY;
+	}
+
+	gpio_set_value(trf->en_gpio, 0);
+	gpio_set_value(trf->en2_gpio, 0);
+
+	ret = regulator_disable(trf->regulator);
+	if (ret)
+		dev_err(trf->dev, "%s - Can't disable VIN: %d\n", __func__,
+				ret);
+
+	trf->state = TRF7970A_ST_PWR_OFF;
+
+	return ret;
+}
+
+static int trf7970a_startup(struct trf7970a *trf)
+{
+	int ret;
+
+	ret = trf7970a_power_up(trf);
+	if (ret)
+		return ret;
+
+	pm_runtime_set_active(trf->dev);
+	pm_runtime_enable(trf->dev);
+	pm_runtime_mark_last_busy(trf->dev);
+
+	return 0;
+}
+
+static void trf7970a_shutdown(struct trf7970a *trf)
+{
+	switch (trf->state) {
+	case TRF7970A_ST_WAIT_FOR_TX_FIFO:
+	case TRF7970A_ST_WAIT_FOR_RX_DATA:
+	case TRF7970A_ST_WAIT_FOR_RX_DATA_CONT:
+	case TRF7970A_ST_WAIT_TO_ISSUE_EOF:
+	case TRF7970A_ST_LISTENING:
+		trf7970a_send_err_upstream(trf, -ECANCELED);
+		/* FALLTHROUGH */
+	case TRF7970A_ST_IDLE:
+	case TRF7970A_ST_IDLE_RX_BLOCKED:
+		trf7970a_switch_rf_off(trf);
+		break;
+	default:
+		break;
+	}
+
+	pm_runtime_disable(trf->dev);
+	pm_runtime_set_suspended(trf->dev);
+
+	trf7970a_power_down(trf);
+}
+
 static int trf7970a_get_autosuspend_delay(struct device_node *np)
 {
 	int autosuspend_delay, ret;
@@ -1246,15 +1955,18 @@ static int trf7970a_get_autosuspend_delay(struct device_node *np)
 	if (ret)
 		autosuspend_delay = TRF7970A_AUTOSUSPEND_DELAY;
 
-	of_node_put(np);
-
 	return autosuspend_delay;
 }
 
+static int trf7970a_get_vin_voltage_override(struct device_node *np,
+		u32 *vin_uvolts)
+{
+	return of_property_read_u32(np, "vin-voltage-override", vin_uvolts);
+}
+
 static int trf7970a_probe(struct spi_device *spi)
 {
 	struct device_node *np = spi->dev.of_node;
-	const struct spi_device_id *id = spi_get_device_id(spi);
 	struct trf7970a *trf;
 	int uvolts, autosuspend_delay, ret;
 
@@ -1267,14 +1979,22 @@ static int trf7970a_probe(struct spi_device *spi)
 	if (!trf)
 		return -ENOMEM;
 
-	trf->state = TRF7970A_ST_OFF;
+	trf->state = TRF7970A_ST_PWR_OFF;
 	trf->dev = &spi->dev;
 	trf->spi = spi;
-	trf->quirks = id->driver_data;
 
 	spi->mode = SPI_MODE_1;
 	spi->bits_per_word = 8;
 
+	ret = spi_setup(spi);
+	if (ret < 0) {
+		dev_err(trf->dev, "Can't set up SPI Communication\n");
+		return ret;
+	}
+
+	if (of_property_read_bool(np, "irq-status-read-quirk"))
+		trf->quirks |= TRF7970A_QUIRK_IRQ_STATUS_READ;
+
 	/* There are two enable pins - both must be present */
 	trf->en_gpio = of_get_named_gpio(np, "ti,enable-gpios", 0);
 	if (!gpio_is_valid(trf->en_gpio)) {
@@ -1283,7 +2003,7 @@ static int trf7970a_probe(struct spi_device *spi)
 	}
 
 	ret = devm_gpio_request_one(trf->dev, trf->en_gpio,
-			GPIOF_DIR_OUT | GPIOF_INIT_LOW, "EN");
+			GPIOF_DIR_OUT | GPIOF_INIT_LOW, "trf7970a EN");
 	if (ret) {
 		dev_err(trf->dev, "Can't request EN GPIO: %d\n", ret);
 		return ret;
@@ -1296,12 +2016,15 @@ static int trf7970a_probe(struct spi_device *spi)
 	}
 
 	ret = devm_gpio_request_one(trf->dev, trf->en2_gpio,
-			GPIOF_DIR_OUT | GPIOF_INIT_LOW, "EN2");
+			GPIOF_DIR_OUT | GPIOF_INIT_LOW, "trf7970a EN2");
 	if (ret) {
 		dev_err(trf->dev, "Can't request EN2 GPIO: %d\n", ret);
 		return ret;
 	}
 
+	if (of_property_read_bool(np, "en2-rf-quirk"))
+		trf->quirks |= TRF7970A_QUIRK_EN2_MUST_STAY_LOW;
+
 	ret = devm_request_threaded_irq(trf->dev, spi->irq, NULL,
 			trf7970a_irq, IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 			"trf7970a", trf);
@@ -1326,15 +2049,17 @@ static int trf7970a_probe(struct spi_device *spi)
 		goto err_destroy_lock;
 	}
 
-	uvolts = regulator_get_voltage(trf->regulator);
+	ret = trf7970a_get_vin_voltage_override(np, &uvolts);
+	if (ret)
+		uvolts = regulator_get_voltage(trf->regulator);
 
 	if (uvolts > 4000000)
 		trf->chip_status_ctrl = TRF7970A_CHIP_STATUS_VRS5_3;
 
 	trf->ddev = nfc_digital_allocate_device(&trf7970a_nfc_ops,
 			TRF7970A_SUPPORTED_PROTOCOLS,
-			NFC_DIGITAL_DRV_CAPS_IN_CRC, TRF7970A_TX_SKB_HEADROOM,
-			0);
+			NFC_DIGITAL_DRV_CAPS_IN_CRC |
+				NFC_DIGITAL_DRV_CAPS_TG_CRC, 0, 0);
 	if (!trf->ddev) {
 		dev_err(trf->dev, "Can't allocate NFC digital device\n");
 		ret = -ENOMEM;
@@ -1349,19 +2074,23 @@ static int trf7970a_probe(struct spi_device *spi)
 
 	pm_runtime_set_autosuspend_delay(trf->dev, autosuspend_delay);
 	pm_runtime_use_autosuspend(trf->dev);
-	pm_runtime_enable(trf->dev);
+
+	ret = trf7970a_startup(trf);
+	if (ret)
+		goto err_free_ddev;
 
 	ret = nfc_digital_register_device(trf->ddev);
 	if (ret) {
 		dev_err(trf->dev, "Can't register NFC digital device: %d\n",
 				ret);
-		goto err_free_ddev;
+		goto err_shutdown;
 	}
 
 	return 0;
 
+err_shutdown:
+	trf7970a_shutdown(trf);
 err_free_ddev:
-	pm_runtime_disable(trf->dev);
 	nfc_digital_free_device(trf->ddev);
 err_disable_regulator:
 	regulator_disable(trf->regulator);
@@ -1376,25 +2105,10 @@ static int trf7970a_remove(struct spi_device *spi)
 
 	mutex_lock(&trf->lock);
 
-	switch (trf->state) {
-	case TRF7970A_ST_WAIT_FOR_TX_FIFO:
-	case TRF7970A_ST_WAIT_FOR_RX_DATA:
-	case TRF7970A_ST_WAIT_FOR_RX_DATA_CONT:
-	case TRF7970A_ST_WAIT_TO_ISSUE_EOF:
-		trf7970a_send_err_upstream(trf, -ECANCELED);
-		/* FALLTHROUGH */
-	case TRF7970A_ST_IDLE:
-	case TRF7970A_ST_IDLE_RX_BLOCKED:
-		pm_runtime_put_sync(trf->dev);
-		break;
-	default:
-		break;
-	}
+	trf7970a_shutdown(trf);
 
 	mutex_unlock(&trf->lock);
 
-	pm_runtime_disable(trf->dev);
-
 	nfc_digital_unregister_device(trf->ddev);
 	nfc_digital_free_device(trf->ddev);
 
@@ -1405,72 +2119,83 @@ static int trf7970a_remove(struct spi_device *spi)
 	return 0;
 }
 
-#ifdef CONFIG_PM_RUNTIME
-static int trf7970a_pm_runtime_suspend(struct device *dev)
+#ifdef CONFIG_PM_SLEEP
+static int trf7970a_suspend(struct device *dev)
+{
+	struct spi_device *spi = container_of(dev, struct spi_device, dev);
+	struct trf7970a *trf = spi_get_drvdata(spi);
+
+	dev_dbg(dev, "Suspend\n");
+
+	mutex_lock(&trf->lock);
+
+	trf7970a_shutdown(trf);
+
+	mutex_unlock(&trf->lock);
+
+	return 0;
+}
+
+static int trf7970a_resume(struct device *dev)
 {
 	struct spi_device *spi = container_of(dev, struct spi_device, dev);
 	struct trf7970a *trf = spi_get_drvdata(spi);
 	int ret;
 
-	dev_dbg(dev, "Runtime suspend\n");
+	dev_dbg(dev, "Resume\n");
 
-	if (trf->state != TRF7970A_ST_OFF) {
-		dev_dbg(dev, "Can't suspend - not in OFF state (%d)\n",
-				trf->state);
-		return -EBUSY;
-	}
+	mutex_lock(&trf->lock);
 
-	gpio_set_value(trf->en_gpio, 0);
-	gpio_set_value(trf->en2_gpio, 0);
+	ret = trf7970a_startup(trf);
 
-	ret = regulator_disable(trf->regulator);
-	if (ret)
-		dev_err(dev, "%s - Can't disable VIN: %d\n", __func__, ret);
+	mutex_unlock(&trf->lock);
 
 	return ret;
 }
+#endif
 
-static int trf7970a_pm_runtime_resume(struct device *dev)
+#ifdef CONFIG_PM_RUNTIME
+static int trf7970a_pm_runtime_suspend(struct device *dev)
 {
 	struct spi_device *spi = container_of(dev, struct spi_device, dev);
 	struct trf7970a *trf = spi_get_drvdata(spi);
 	int ret;
 
-	dev_dbg(dev, "Runtime resume\n");
+	dev_dbg(dev, "Runtime suspend\n");
 
-	ret = regulator_enable(trf->regulator);
-	if (ret) {
-		dev_err(dev, "%s - Can't enable VIN: %d\n", __func__, ret);
-		return ret;
-	}
+	mutex_lock(&trf->lock);
 
-	usleep_range(5000, 6000);
+	ret = trf7970a_power_down(trf);
 
-	gpio_set_value(trf->en2_gpio, 1);
-	usleep_range(1000, 2000);
-	gpio_set_value(trf->en_gpio, 1);
+	mutex_unlock(&trf->lock);
 
-	usleep_range(20000, 21000);
+	return ret;
+}
 
-	ret = trf7970a_init(trf);
-	if (ret) {
-		dev_err(dev, "%s - Can't initialize: %d\n", __func__, ret);
-		return ret;
-	}
+static int trf7970a_pm_runtime_resume(struct device *dev)
+{
+	struct spi_device *spi = container_of(dev, struct spi_device, dev);
+	struct trf7970a *trf = spi_get_drvdata(spi);
+	int ret;
 
-	pm_runtime_mark_last_busy(dev);
+	dev_dbg(dev, "Runtime resume\n");
 
-	return 0;
+	ret = trf7970a_power_up(trf);
+	if (!ret)
+		pm_runtime_mark_last_busy(dev);
+
+	return ret;
 }
 #endif
 
 static const struct dev_pm_ops trf7970a_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(trf7970a_suspend, trf7970a_resume)
 	SET_RUNTIME_PM_OPS(trf7970a_pm_runtime_suspend,
 			trf7970a_pm_runtime_resume, NULL)
 };
 
 static const struct spi_device_id trf7970a_id_table[] = {
-	{ "trf7970a", TRF7970A_QUIRK_IRQ_STATUS_READ_ERRATA },
+	{ "trf7970a", 0 },
 	{ }
 };
 MODULE_DEVICE_TABLE(spi, trf7970a_id_table);
diff --git a/drivers/ntb/ntb_hw.c b/drivers/ntb/ntb_hw.c
index 372e08c4ffef..cd29b1038c5e 100644
--- a/drivers/ntb/ntb_hw.c
+++ b/drivers/ntb/ntb_hw.c
@@ -64,10 +64,6 @@ MODULE_VERSION(NTB_VER);
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_AUTHOR("Intel Corporation");
 
-static bool xeon_errata_workaround = true;
-module_param(xeon_errata_workaround, bool, 0644);
-MODULE_PARM_DESC(xeon_errata_workaround, "Workaround for the Xeon Errata");
-
 enum {
 	NTB_CONN_TRANSPARENT = 0,
 	NTB_CONN_B2B,
@@ -88,8 +84,8 @@ static struct dentry *debugfs_dir;
 
 #define BWD_LINK_RECOVERY_TIME	500
 
-/* Translate memory window 0,1 to BAR 2,4 */
-#define MW_TO_BAR(mw)	(mw * NTB_MAX_NUM_MW + 2)
+/* Translate memory window 0,1,2 to BAR 2,4,5 */
+#define MW_TO_BAR(mw)	(mw == 0 ? 2 : (mw == 1 ? 4 : 5))
 
 static const struct pci_device_id ntb_pci_tbl[] = {
 	{PCI_VDEVICE(INTEL, PCI_DEVICE_ID_INTEL_NTB_B2B_BWD)},
@@ -109,6 +105,65 @@ static const struct pci_device_id ntb_pci_tbl[] = {
 };
 MODULE_DEVICE_TABLE(pci, ntb_pci_tbl);
 
+static int is_ntb_xeon(struct ntb_device *ndev)
+{
+	switch (ndev->pdev->device) {
+	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
+		return 1;
+	default:
+		return 0;
+	}
+
+	return 0;
+}
+
+static int is_ntb_atom(struct ntb_device *ndev)
+{
+	switch (ndev->pdev->device) {
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_BWD:
+		return 1;
+	default:
+		return 0;
+	}
+
+	return 0;
+}
+
+static void ntb_set_errata_flags(struct ntb_device *ndev)
+{
+	switch (ndev->pdev->device) {
+	/*
+	 * this workaround applies to all platform up to IvyBridge
+	 * Haswell has splitbar support and use a different workaround
+	 */
+	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
+	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
+		ndev->wa_flags |= WA_SNB_ERR;
+		break;
+	}
+}
+
 /**
  * ntb_register_event_callback() - register event callback
  * @ndev: pointer to ntb_device instance
@@ -451,8 +506,14 @@ void ntb_set_mw_addr(struct ntb_device *ndev, unsigned int mw, u64 addr)
 	case NTB_BAR_23:
 		writeq(addr, ndev->reg_ofs.bar2_xlat);
 		break;
-	case NTB_BAR_45:
-		writeq(addr, ndev->reg_ofs.bar4_xlat);
+	case NTB_BAR_4:
+		if (ndev->split_bar)
+			writel(addr, ndev->reg_ofs.bar4_xlat);
+		else
+			writeq(addr, ndev->reg_ofs.bar4_xlat);
+		break;
+	case NTB_BAR_5:
+		writel(addr, ndev->reg_ofs.bar5_xlat);
 		break;
 	}
 }
@@ -535,7 +596,7 @@ static void ntb_link_event(struct ntb_device *ndev, int link_state)
 		ndev->link_status = NTB_LINK_UP;
 		event = NTB_EVENT_HW_LINK_UP;
 
-		if (ndev->hw_type == BWD_HW ||
+		if (is_ntb_atom(ndev) ||
 		    ndev->conn_type == NTB_CONN_TRANSPARENT)
 			status = readw(ndev->reg_ofs.lnk_stat);
 		else {
@@ -566,7 +627,7 @@ static int ntb_link_status(struct ntb_device *ndev)
 {
 	int link_state;
 
-	if (ndev->hw_type == BWD_HW) {
+	if (is_ntb_atom(ndev)) {
 		u32 ntb_cntl;
 
 		ntb_cntl = readl(ndev->reg_ofs.lnk_cntl);
@@ -667,29 +728,16 @@ static void bwd_link_poll(struct work_struct *work)
 
 static int ntb_xeon_setup(struct ntb_device *ndev)
 {
-	int rc;
-	u8 val;
-
-	ndev->hw_type = SNB_HW;
-
-	rc = pci_read_config_byte(ndev->pdev, NTB_PPD_OFFSET, &val);
-	if (rc)
-		return rc;
-
-	if (val & SNB_PPD_DEV_TYPE)
-		ndev->dev_type = NTB_DEV_USD;
-	else
-		ndev->dev_type = NTB_DEV_DSD;
-
-	switch (val & SNB_PPD_CONN_TYPE) {
+	switch (ndev->conn_type) {
 	case NTB_CONN_B2B:
-		dev_info(&ndev->pdev->dev, "Conn Type = B2B\n");
-		ndev->conn_type = NTB_CONN_B2B;
 		ndev->reg_ofs.ldb = ndev->reg_base + SNB_PDOORBELL_OFFSET;
 		ndev->reg_ofs.ldb_mask = ndev->reg_base + SNB_PDBMSK_OFFSET;
 		ndev->reg_ofs.spad_read = ndev->reg_base + SNB_SPAD_OFFSET;
 		ndev->reg_ofs.bar2_xlat = ndev->reg_base + SNB_SBAR2XLAT_OFFSET;
 		ndev->reg_ofs.bar4_xlat = ndev->reg_base + SNB_SBAR4XLAT_OFFSET;
+		if (ndev->split_bar)
+			ndev->reg_ofs.bar5_xlat =
+				ndev->reg_base + SNB_SBAR5XLAT_OFFSET;
 		ndev->limits.max_spads = SNB_MAX_B2B_SPADS;
 
 		/* There is a Xeon hardware errata related to writes to
@@ -698,16 +746,17 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 		 * this use the second memory window to access the interrupt and
 		 * scratch pad registers on the remote system.
 		 */
-		if (xeon_errata_workaround) {
-			if (!ndev->mw[1].bar_sz)
+		if (ndev->wa_flags & WA_SNB_ERR) {
+			if (!ndev->mw[ndev->limits.max_mw - 1].bar_sz)
 				return -EINVAL;
 
-			ndev->limits.max_mw = SNB_ERRATA_MAX_MW;
 			ndev->limits.max_db_bits = SNB_MAX_DB_BITS;
-			ndev->reg_ofs.spad_write = ndev->mw[1].vbase +
-						   SNB_SPAD_OFFSET;
-			ndev->reg_ofs.rdb = ndev->mw[1].vbase +
-					    SNB_PDOORBELL_OFFSET;
+			ndev->reg_ofs.spad_write =
+				ndev->mw[ndev->limits.max_mw - 1].vbase +
+				SNB_SPAD_OFFSET;
+			ndev->reg_ofs.rdb =
+				ndev->mw[ndev->limits.max_mw - 1].vbase +
+				SNB_PDOORBELL_OFFSET;
 
 			/* Set the Limit register to 4k, the minimum size, to
 			 * prevent an illegal access
@@ -720,9 +769,9 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 			 * the driver defaults, but write the Limit registers
 			 * first just in case.
 			 */
-		} else {
-			ndev->limits.max_mw = SNB_MAX_MW;
 
+			ndev->limits.max_mw = SNB_ERRATA_MAX_MW;
+		} else {
 			/* HW Errata on bit 14 of b2bdoorbell register.  Writes
 			 * will not be mirrored to the remote system.  Shrink
 			 * the number of bits by one, since bit 14 is the last
@@ -735,7 +784,8 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 					    SNB_B2B_DOORBELL_OFFSET;
 
 			/* Disable the Limit register, just incase it is set to
-			 * something silly
+			 * something silly. A 64bit write should handle it
+			 * regardless of whether it has a split BAR or not.
 			 */
 			writeq(0, ndev->reg_base + SNB_PBAR4LMT_OFFSET);
 			/* HW errata on the Limit registers.  They can only be
@@ -744,6 +794,10 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 			 * the driver defaults, but write the Limit registers
 			 * first just in case.
 			 */
+			if (ndev->split_bar)
+				ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
+			else
+				ndev->limits.max_mw = SNB_MAX_MW;
 		}
 
 		/* The Xeon errata workaround requires setting SBAR Base
@@ -753,12 +807,22 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 		if (ndev->dev_type == NTB_DEV_USD) {
 			writeq(SNB_MBAR23_DSD_ADDR, ndev->reg_base +
 			       SNB_PBAR2XLAT_OFFSET);
-			if (xeon_errata_workaround)
+			if (ndev->wa_flags & WA_SNB_ERR)
 				writeq(SNB_MBAR01_DSD_ADDR, ndev->reg_base +
 				       SNB_PBAR4XLAT_OFFSET);
 			else {
-				writeq(SNB_MBAR45_DSD_ADDR, ndev->reg_base +
-				       SNB_PBAR4XLAT_OFFSET);
+				if (ndev->split_bar) {
+					writel(SNB_MBAR4_DSD_ADDR,
+					       ndev->reg_base +
+					       SNB_PBAR4XLAT_OFFSET);
+					writel(SNB_MBAR5_DSD_ADDR,
+					       ndev->reg_base +
+					       SNB_PBAR5XLAT_OFFSET);
+				} else
+					writeq(SNB_MBAR4_DSD_ADDR,
+					       ndev->reg_base +
+					       SNB_PBAR4XLAT_OFFSET);
+
 				/* B2B_XLAT_OFFSET is a 64bit register, but can
 				 * only take 32bit writes
 				 */
@@ -772,18 +836,35 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 			       SNB_SBAR0BASE_OFFSET);
 			writeq(SNB_MBAR23_USD_ADDR, ndev->reg_base +
 			       SNB_SBAR2BASE_OFFSET);
-			writeq(SNB_MBAR45_USD_ADDR, ndev->reg_base +
-			       SNB_SBAR4BASE_OFFSET);
+			if (ndev->split_bar) {
+				writel(SNB_MBAR4_USD_ADDR, ndev->reg_base +
+				       SNB_SBAR4BASE_OFFSET);
+				writel(SNB_MBAR5_USD_ADDR, ndev->reg_base +
+				       SNB_SBAR5BASE_OFFSET);
+			} else
+				writeq(SNB_MBAR4_USD_ADDR, ndev->reg_base +
+				       SNB_SBAR4BASE_OFFSET);
 		} else {
 			writeq(SNB_MBAR23_USD_ADDR, ndev->reg_base +
 			       SNB_PBAR2XLAT_OFFSET);
-			if (xeon_errata_workaround)
+			if (ndev->wa_flags & WA_SNB_ERR)
 				writeq(SNB_MBAR01_USD_ADDR, ndev->reg_base +
 				       SNB_PBAR4XLAT_OFFSET);
 			else {
-				writeq(SNB_MBAR45_USD_ADDR, ndev->reg_base +
-				       SNB_PBAR4XLAT_OFFSET);
-				/* B2B_XLAT_OFFSET is a 64bit register, but can
+				if (ndev->split_bar) {
+					writel(SNB_MBAR4_USD_ADDR,
+					       ndev->reg_base +
+					       SNB_PBAR4XLAT_OFFSET);
+					writel(SNB_MBAR5_USD_ADDR,
+					       ndev->reg_base +
+					       SNB_PBAR5XLAT_OFFSET);
+				} else
+					writeq(SNB_MBAR4_USD_ADDR,
+					       ndev->reg_base +
+					       SNB_PBAR4XLAT_OFFSET);
+
+				/*
+				 * B2B_XLAT_OFFSET is a 64bit register, but can
 				 * only take 32bit writes
 				 */
 				writel(SNB_MBAR01_USD_ADDR & 0xffffffff,
@@ -795,17 +876,21 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 			       SNB_SBAR0BASE_OFFSET);
 			writeq(SNB_MBAR23_DSD_ADDR, ndev->reg_base +
 			       SNB_SBAR2BASE_OFFSET);
-			writeq(SNB_MBAR45_DSD_ADDR, ndev->reg_base +
-			       SNB_SBAR4BASE_OFFSET);
+			if (ndev->split_bar) {
+				writel(SNB_MBAR4_DSD_ADDR, ndev->reg_base +
+				       SNB_SBAR4BASE_OFFSET);
+				writel(SNB_MBAR5_DSD_ADDR, ndev->reg_base +
+				       SNB_SBAR5BASE_OFFSET);
+			} else
+				writeq(SNB_MBAR4_DSD_ADDR, ndev->reg_base +
+				       SNB_SBAR4BASE_OFFSET);
+
 		}
 		break;
 	case NTB_CONN_RP:
-		dev_info(&ndev->pdev->dev, "Conn Type = RP\n");
-		ndev->conn_type = NTB_CONN_RP;
-
-		if (xeon_errata_workaround) {
+		if (ndev->wa_flags & WA_SNB_ERR) {
 			dev_err(&ndev->pdev->dev,
-				"NTB-RP disabled due to hardware errata.  To disregard this warning and potentially lock-up the system, add the parameter 'xeon_errata_workaround=0'.\n");
+				"NTB-RP disabled due to hardware errata.\n");
 			return -EINVAL;
 		}
 
@@ -829,11 +914,20 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 		ndev->reg_ofs.spad_read = ndev->reg_base + SNB_SPAD_OFFSET;
 		ndev->reg_ofs.bar2_xlat = ndev->reg_base + SNB_SBAR2XLAT_OFFSET;
 		ndev->reg_ofs.bar4_xlat = ndev->reg_base + SNB_SBAR4XLAT_OFFSET;
-		ndev->limits.max_mw = SNB_MAX_MW;
+		if (ndev->split_bar) {
+			ndev->reg_ofs.bar5_xlat =
+				ndev->reg_base + SNB_SBAR5XLAT_OFFSET;
+			ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
+		} else
+			ndev->limits.max_mw = SNB_MAX_MW;
 		break;
 	case NTB_CONN_TRANSPARENT:
-		dev_info(&ndev->pdev->dev, "Conn Type = TRANSPARENT\n");
-		ndev->conn_type = NTB_CONN_TRANSPARENT;
+		if (ndev->wa_flags & WA_SNB_ERR) {
+			dev_err(&ndev->pdev->dev,
+				"NTB-TRANSPARENT disabled due to hardware errata.\n");
+			return -EINVAL;
+		}
+
 		/* Scratch pads need to have exclusive access from the primary
 		 * or secondary side.  Halve the num spads so that each side can
 		 * have an equal amount.
@@ -852,13 +946,18 @@ static int ntb_xeon_setup(struct ntb_device *ndev)
 		ndev->reg_ofs.bar2_xlat = ndev->reg_base + SNB_PBAR2XLAT_OFFSET;
 		ndev->reg_ofs.bar4_xlat = ndev->reg_base + SNB_PBAR4XLAT_OFFSET;
 
-		ndev->limits.max_mw = SNB_MAX_MW;
+		if (ndev->split_bar) {
+			ndev->reg_ofs.bar5_xlat =
+				ndev->reg_base + SNB_PBAR5XLAT_OFFSET;
+			ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
+		} else
+			ndev->limits.max_mw = SNB_MAX_MW;
 		break;
 	default:
-		/* Most likely caused by the remote NTB-RP device not being
-		 * configured
+		/*
+		 * we should never hit this. the detect function should've
+		 * take cared of everything.
 		 */
-		dev_err(&ndev->pdev->dev, "Unknown PPD %x\n", val);
 		return -EINVAL;
 	}
 
@@ -932,34 +1031,16 @@ static int ntb_device_setup(struct ntb_device *ndev)
 {
 	int rc;
 
-	switch (ndev->pdev->device) {
-	case PCI_DEVICE_ID_INTEL_NTB_SS_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_SS_HSX:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_PS_HSX:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_JSF:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_SNB:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_IVT:
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_HSX:
+	if (is_ntb_xeon(ndev))
 		rc = ntb_xeon_setup(ndev);
-		break;
-	case PCI_DEVICE_ID_INTEL_NTB_B2B_BWD:
+	else if (is_ntb_atom(ndev))
 		rc = ntb_bwd_setup(ndev);
-		break;
-	default:
+	else
 		rc = -ENODEV;
-	}
 
 	if (rc)
 		return rc;
 
-	dev_info(&ndev->pdev->dev, "Device Type = %s\n",
-		 ndev->dev_type == NTB_DEV_USD ? "USD/DSP" : "DSD/USP");
-
 	if (ndev->conn_type == NTB_CONN_B2B)
 		/* Enable Bus Master and Memory Space on the secondary side */
 		writew(PCI_COMMAND_MEMORY | PCI_COMMAND_MASTER,
@@ -970,7 +1051,7 @@ static int ntb_device_setup(struct ntb_device *ndev)
 
 static void ntb_device_free(struct ntb_device *ndev)
 {
-	if (ndev->hw_type == BWD_HW) {
+	if (is_ntb_atom(ndev)) {
 		cancel_delayed_work_sync(&ndev->hb_timer);
 		cancel_delayed_work_sync(&ndev->lr_timer);
 	}
@@ -1050,7 +1131,7 @@ static irqreturn_t ntb_interrupt(int irq, void *dev)
 	struct ntb_device *ndev = dev;
 	unsigned int i = 0;
 
-	if (ndev->hw_type == BWD_HW) {
+	if (is_ntb_atom(ndev)) {
 		u64 ldb = readq(ndev->reg_ofs.ldb);
 
 		dev_dbg(&ndev->pdev->dev, "irq %d - ldb = %Lx\n", irq, ldb);
@@ -1192,7 +1273,7 @@ static int ntb_setup_msix(struct ntb_device *ndev)
 	for (i = 0; i < msix_entries; i++)
 		ndev->msix_entries[i].entry = i;
 
-	if (ndev->hw_type == BWD_HW)
+	if (is_ntb_atom(ndev))
 		rc = ntb_setup_bwd_msix(ndev, msix_entries);
 	else
 		rc = ntb_setup_snb_msix(ndev, msix_entries);
@@ -1252,7 +1333,7 @@ static int ntb_setup_interrupts(struct ntb_device *ndev)
 	/* On BWD, disable all interrupts.  On SNB, disable all but Link
 	 * Interrupt.  The rest will be unmasked as callbacks are registered.
 	 */
-	if (ndev->hw_type == BWD_HW)
+	if (is_ntb_atom(ndev))
 		writeq(~0, ndev->reg_ofs.ldb_mask);
 	else {
 		u16 var = 1 << SNB_LINK_DB;
@@ -1285,7 +1366,7 @@ static void ntb_free_interrupts(struct ntb_device *ndev)
 	struct pci_dev *pdev = ndev->pdev;
 
 	/* mask interrupts */
-	if (ndev->hw_type == BWD_HW)
+	if (is_ntb_atom(ndev))
 		writeq(~0, ndev->reg_ofs.ldb_mask);
 	else
 		writew(~0, ndev->reg_ofs.ldb_mask);
@@ -1296,7 +1377,7 @@ static void ntb_free_interrupts(struct ntb_device *ndev)
 
 		for (i = 0; i < ndev->num_msix; i++) {
 			msix = &ndev->msix_entries[i];
-			if (ndev->hw_type != BWD_HW && i == ndev->num_msix - 1)
+			if (is_ntb_xeon(ndev) && i == ndev->num_msix - 1)
 				free_irq(msix->vector, ndev);
 			else
 				free_irq(msix->vector, &ndev->db_cb[i]);
@@ -1344,6 +1425,101 @@ static void ntb_free_callbacks(struct ntb_device *ndev)
 	kfree(ndev->db_cb);
 }
 
+static ssize_t ntb_debugfs_read(struct file *filp, char __user *ubuf,
+				size_t count, loff_t *offp)
+{
+	struct ntb_device *ndev;
+	char *buf;
+	ssize_t ret, offset, out_count;
+
+	out_count = 500;
+
+	buf = kmalloc(out_count, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	ndev = filp->private_data;
+	offset = 0;
+	offset += snprintf(buf + offset, out_count - offset,
+			   "NTB Device Information:\n");
+	offset += snprintf(buf + offset, out_count - offset,
+			   "Connection Type - \t\t%s\n",
+			   ndev->conn_type == NTB_CONN_TRANSPARENT ?
+			   "Transparent" : (ndev->conn_type == NTB_CONN_B2B) ?
+			   "Back to back" : "Root Port");
+	offset += snprintf(buf + offset, out_count - offset,
+			   "Device Type - \t\t\t%s\n",
+			   ndev->dev_type == NTB_DEV_USD ?
+			   "DSD/USP" : "USD/DSP");
+	offset += snprintf(buf + offset, out_count - offset,
+			   "Max Number of Callbacks - \t%u\n",
+			   ntb_max_cbs(ndev));
+	offset += snprintf(buf + offset, out_count - offset,
+			   "Link Status - \t\t\t%s\n",
+			   ntb_hw_link_status(ndev) ? "Up" : "Down");
+	if (ntb_hw_link_status(ndev)) {
+		offset += snprintf(buf + offset, out_count - offset,
+				   "Link Speed - \t\t\tPCI-E Gen %u\n",
+				   ndev->link_speed);
+		offset += snprintf(buf + offset, out_count - offset,
+				   "Link Width - \t\t\tx%u\n",
+				   ndev->link_width);
+	}
+
+	if (is_ntb_xeon(ndev)) {
+		u32 status32;
+		u16 status16;
+		int rc;
+
+		offset += snprintf(buf + offset, out_count - offset,
+				   "\nNTB Device Statistics:\n");
+		offset += snprintf(buf + offset, out_count - offset,
+				   "Upstream Memory Miss - \t%u\n",
+				   readw(ndev->reg_base +
+					 SNB_USMEMMISS_OFFSET));
+
+		offset += snprintf(buf + offset, out_count - offset,
+				   "\nNTB Hardware Errors:\n");
+
+		rc = pci_read_config_word(ndev->pdev, SNB_DEVSTS_OFFSET,
+					  &status16);
+		if (!rc)
+			offset += snprintf(buf + offset, out_count - offset,
+					   "DEVSTS - \t%#06x\n", status16);
+
+		rc = pci_read_config_word(ndev->pdev, SNB_LINK_STATUS_OFFSET,
+					  &status16);
+		if (!rc)
+			offset += snprintf(buf + offset, out_count - offset,
+					   "LNKSTS - \t%#06x\n", status16);
+
+		rc = pci_read_config_dword(ndev->pdev, SNB_UNCERRSTS_OFFSET,
+					   &status32);
+		if (!rc)
+			offset += snprintf(buf + offset, out_count - offset,
+					   "UNCERRSTS - \t%#010x\n", status32);
+
+		rc = pci_read_config_dword(ndev->pdev, SNB_CORERRSTS_OFFSET,
+					   &status32);
+		if (!rc)
+			offset += snprintf(buf + offset, out_count - offset,
+					   "CORERRSTS - \t%#010x\n", status32);
+	}
+
+	if (offset > out_count)
+		offset = out_count;
+
+	ret = simple_read_from_buffer(ubuf, count, offp, buf, offset);
+	kfree(buf);
+	return ret;
+}
+
+static const struct file_operations ntb_debugfs_info = {
+	.owner = THIS_MODULE,
+	.open = simple_open,
+	.read = ntb_debugfs_read,
+};
+
 static void ntb_setup_debugfs(struct ntb_device *ndev)
 {
 	if (!debugfs_initialized())
@@ -1354,6 +1530,11 @@ static void ntb_setup_debugfs(struct ntb_device *ndev)
 
 	ndev->debugfs_dir = debugfs_create_dir(pci_name(ndev->pdev),
 					       debugfs_dir);
+	if (ndev->debugfs_dir)
+		ndev->debugfs_info = debugfs_create_file("info", S_IRUSR,
+							 ndev->debugfs_dir,
+							 ndev,
+							 &ntb_debugfs_info);
 }
 
 static void ntb_free_debugfs(struct ntb_device *ndev)
@@ -1377,7 +1558,11 @@ static void ntb_hw_link_up(struct ntb_device *ndev)
 		ntb_cntl = readl(ndev->reg_ofs.lnk_cntl);
 		ntb_cntl &= ~(NTB_CNTL_LINK_DISABLE | NTB_CNTL_CFG_LOCK);
 		ntb_cntl |= NTB_CNTL_P2S_BAR23_SNOOP | NTB_CNTL_S2P_BAR23_SNOOP;
-		ntb_cntl |= NTB_CNTL_P2S_BAR45_SNOOP | NTB_CNTL_S2P_BAR45_SNOOP;
+		ntb_cntl |= NTB_CNTL_P2S_BAR4_SNOOP | NTB_CNTL_S2P_BAR4_SNOOP;
+		if (ndev->split_bar)
+			ntb_cntl |= NTB_CNTL_P2S_BAR5_SNOOP |
+				    NTB_CNTL_S2P_BAR5_SNOOP;
+
 		writel(ntb_cntl, ndev->reg_ofs.lnk_cntl);
 	}
 }
@@ -1394,11 +1579,128 @@ static void ntb_hw_link_down(struct ntb_device *ndev)
 	/* Bring NTB link down */
 	ntb_cntl = readl(ndev->reg_ofs.lnk_cntl);
 	ntb_cntl &= ~(NTB_CNTL_P2S_BAR23_SNOOP | NTB_CNTL_S2P_BAR23_SNOOP);
-	ntb_cntl &= ~(NTB_CNTL_P2S_BAR45_SNOOP | NTB_CNTL_S2P_BAR45_SNOOP);
+	ntb_cntl &= ~(NTB_CNTL_P2S_BAR4_SNOOP | NTB_CNTL_S2P_BAR4_SNOOP);
+	if (ndev->split_bar)
+		ntb_cntl &= ~(NTB_CNTL_P2S_BAR5_SNOOP |
+			      NTB_CNTL_S2P_BAR5_SNOOP);
 	ntb_cntl |= NTB_CNTL_LINK_DISABLE | NTB_CNTL_CFG_LOCK;
 	writel(ntb_cntl, ndev->reg_ofs.lnk_cntl);
 }
 
+static void ntb_max_mw_detect(struct ntb_device *ndev)
+{
+	if (ndev->split_bar)
+		ndev->limits.max_mw = HSX_SPLITBAR_MAX_MW;
+	else
+		ndev->limits.max_mw = SNB_MAX_MW;
+}
+
+static int ntb_xeon_detect(struct ntb_device *ndev)
+{
+	int rc, bars_mask;
+	u32 bars;
+	u8 ppd;
+
+	ndev->hw_type = SNB_HW;
+
+	rc = pci_read_config_byte(ndev->pdev, NTB_PPD_OFFSET, &ppd);
+	if (rc)
+		return -EIO;
+
+	if (ppd & SNB_PPD_DEV_TYPE)
+		ndev->dev_type = NTB_DEV_USD;
+	else
+		ndev->dev_type = NTB_DEV_DSD;
+
+	ndev->split_bar = (ppd & SNB_PPD_SPLIT_BAR) ? 1 : 0;
+
+	switch (ppd & SNB_PPD_CONN_TYPE) {
+	case NTB_CONN_B2B:
+		dev_info(&ndev->pdev->dev, "Conn Type = B2B\n");
+		ndev->conn_type = NTB_CONN_B2B;
+		break;
+	case NTB_CONN_RP:
+		dev_info(&ndev->pdev->dev, "Conn Type = RP\n");
+		ndev->conn_type = NTB_CONN_RP;
+		break;
+	case NTB_CONN_TRANSPARENT:
+		dev_info(&ndev->pdev->dev, "Conn Type = TRANSPARENT\n");
+		ndev->conn_type = NTB_CONN_TRANSPARENT;
+		/*
+		 * This mode is default to USD/DSP. HW does not report
+		 * properly in transparent mode as it has no knowledge of
+		 * NTB. We will just force correct here.
+		 */
+		ndev->dev_type = NTB_DEV_USD;
+
+		/*
+		 * This is a way for transparent BAR to figure out if we
+		 * are doing split BAR or not. There is no way for the hw
+		 * on the transparent side to know and set the PPD.
+		 */
+		bars_mask = pci_select_bars(ndev->pdev, IORESOURCE_MEM);
+		bars = hweight32(bars_mask);
+		if (bars == (HSX_SPLITBAR_MAX_MW + 1))
+			ndev->split_bar = 1;
+
+		break;
+	default:
+		dev_err(&ndev->pdev->dev, "Unknown PPD %x\n", ppd);
+		return -ENODEV;
+	}
+
+	ntb_max_mw_detect(ndev);
+
+	return 0;
+}
+
+static int ntb_atom_detect(struct ntb_device *ndev)
+{
+	int rc;
+	u32 ppd;
+
+	ndev->hw_type = BWD_HW;
+
+	rc = pci_read_config_dword(ndev->pdev, NTB_PPD_OFFSET, &ppd);
+	if (rc)
+		return rc;
+
+	switch ((ppd & BWD_PPD_CONN_TYPE) >> 8) {
+	case NTB_CONN_B2B:
+		dev_info(&ndev->pdev->dev, "Conn Type = B2B\n");
+		ndev->conn_type = NTB_CONN_B2B;
+		break;
+	case NTB_CONN_RP:
+	default:
+		dev_err(&ndev->pdev->dev, "Unsupported NTB configuration\n");
+		return -EINVAL;
+	}
+
+	if (ppd & BWD_PPD_DEV_TYPE)
+		ndev->dev_type = NTB_DEV_DSD;
+	else
+		ndev->dev_type = NTB_DEV_USD;
+
+	return 0;
+}
+
+static int ntb_device_detect(struct ntb_device *ndev)
+{
+	int rc;
+
+	if (is_ntb_xeon(ndev))
+		rc = ntb_xeon_detect(ndev);
+	else if (is_ntb_atom(ndev))
+		rc = ntb_atom_detect(ndev);
+	else
+		rc = -ENODEV;
+
+	dev_info(&ndev->pdev->dev, "Device Type = %s\n",
+		 ndev->dev_type == NTB_DEV_USD ? "USD/DSP" : "DSD/USP");
+
+	return 0;
+}
+
 static int ntb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct ntb_device *ndev;
@@ -1409,6 +1711,9 @@ static int ntb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 		return -ENOMEM;
 
 	ndev->pdev = pdev;
+
+	ntb_set_errata_flags(ndev);
+
 	ndev->link_status = NTB_LINK_DOWN;
 	pci_set_drvdata(pdev, ndev);
 	ntb_setup_debugfs(ndev);
@@ -1419,22 +1724,54 @@ static int ntb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	pci_set_master(ndev->pdev);
 
-	rc = pci_request_selected_regions(pdev, NTB_BAR_MASK, KBUILD_MODNAME);
+	rc = ntb_device_detect(ndev);
 	if (rc)
+		goto err;
+
+	ndev->mw = kcalloc(ndev->limits.max_mw, sizeof(struct ntb_mw),
+			   GFP_KERNEL);
+	if (!ndev->mw) {
+		rc = -ENOMEM;
 		goto err1;
+	}
+
+	if (ndev->split_bar)
+		rc = pci_request_selected_regions(pdev, NTB_SPLITBAR_MASK,
+						  KBUILD_MODNAME);
+	else
+		rc = pci_request_selected_regions(pdev, NTB_BAR_MASK,
+						  KBUILD_MODNAME);
+
+	if (rc)
+		goto err2;
 
 	ndev->reg_base = pci_ioremap_bar(pdev, NTB_BAR_MMIO);
 	if (!ndev->reg_base) {
 		dev_warn(&pdev->dev, "Cannot remap BAR 0\n");
 		rc = -EIO;
-		goto err2;
+		goto err3;
 	}
 
-	for (i = 0; i < NTB_MAX_NUM_MW; i++) {
+	for (i = 0; i < ndev->limits.max_mw; i++) {
 		ndev->mw[i].bar_sz = pci_resource_len(pdev, MW_TO_BAR(i));
-		ndev->mw[i].vbase =
-		    ioremap_wc(pci_resource_start(pdev, MW_TO_BAR(i)),
-			       ndev->mw[i].bar_sz);
+
+		/*
+		 * with the errata we need to steal last of the memory
+		 * windows for workarounds and they point to MMIO registers.
+		 */
+		if ((ndev->wa_flags & WA_SNB_ERR) &&
+		    (i == (ndev->limits.max_mw - 1))) {
+			ndev->mw[i].vbase =
+				ioremap_nocache(pci_resource_start(pdev,
+							MW_TO_BAR(i)),
+						ndev->mw[i].bar_sz);
+		} else {
+			ndev->mw[i].vbase =
+				ioremap_wc(pci_resource_start(pdev,
+							MW_TO_BAR(i)),
+					   ndev->mw[i].bar_sz);
+		}
+
 		dev_info(&pdev->dev, "MW %d size %llu\n", i,
 			 (unsigned long long) ndev->mw[i].bar_sz);
 		if (!ndev->mw[i].vbase) {
@@ -1449,7 +1786,7 @@ static int ntb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (rc) {
 		rc = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (rc)
-			goto err3;
+			goto err4;
 
 		dev_warn(&pdev->dev, "Cannot DMA highmem\n");
 	}
@@ -1458,22 +1795,22 @@ static int ntb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (rc) {
 		rc = pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32));
 		if (rc)
-			goto err3;
+			goto err4;
 
 		dev_warn(&pdev->dev, "Cannot DMA consistent highmem\n");
 	}
 
 	rc = ntb_device_setup(ndev);
 	if (rc)
-		goto err3;
+		goto err4;
 
 	rc = ntb_create_callbacks(ndev);
 	if (rc)
-		goto err4;
+		goto err5;
 
 	rc = ntb_setup_interrupts(ndev);
 	if (rc)
-		goto err5;
+		goto err6;
 
 	/* The scratchpad registers keep the values between rmmod/insmod,
 	 * blast them now
@@ -1485,24 +1822,29 @@ static int ntb_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	rc = ntb_transport_init(pdev);
 	if (rc)
-		goto err6;
+		goto err7;
 
 	ntb_hw_link_up(ndev);
 
 	return 0;
 
-err6:
+err7:
 	ntb_free_interrupts(ndev);
-err5:
+err6:
 	ntb_free_callbacks(ndev);
-err4:
+err5:
 	ntb_device_free(ndev);
-err3:
+err4:
 	for (i--; i >= 0; i--)
 		iounmap(ndev->mw[i].vbase);
 	iounmap(ndev->reg_base);
+err3:
+	if (ndev->split_bar)
+		pci_release_selected_regions(pdev, NTB_SPLITBAR_MASK);
+	else
+		pci_release_selected_regions(pdev, NTB_BAR_MASK);
 err2:
-	pci_release_selected_regions(pdev, NTB_BAR_MASK);
+	kfree(ndev->mw);
 err1:
 	pci_disable_device(pdev);
 err:
@@ -1526,11 +1868,19 @@ static void ntb_pci_remove(struct pci_dev *pdev)
 	ntb_free_callbacks(ndev);
 	ntb_device_free(ndev);
 
-	for (i = 0; i < NTB_MAX_NUM_MW; i++)
+	/* need to reset max_mw limits so we can unmap properly */
+	if (ndev->hw_type == SNB_HW)
+		ntb_max_mw_detect(ndev);
+
+	for (i = 0; i < ndev->limits.max_mw; i++)
 		iounmap(ndev->mw[i].vbase);
 
+	kfree(ndev->mw);
 	iounmap(ndev->reg_base);
-	pci_release_selected_regions(pdev, NTB_BAR_MASK);
+	if (ndev->split_bar)
+		pci_release_selected_regions(pdev, NTB_SPLITBAR_MASK);
+	else
+		pci_release_selected_regions(pdev, NTB_BAR_MASK);
 	pci_disable_device(pdev);
 	ntb_free_debugfs(ndev);
 	kfree(ndev);
@@ -1542,4 +1892,5 @@ static struct pci_driver ntb_pci_driver = {
 	.probe = ntb_pci_probe,
 	.remove = ntb_pci_remove,
 };
+
 module_pci_driver(ntb_pci_driver);
diff --git a/drivers/ntb/ntb_hw.h b/drivers/ntb/ntb_hw.h
index 465517b7393e..96de5fc95f90 100644
--- a/drivers/ntb/ntb_hw.h
+++ b/drivers/ntb/ntb_hw.h
@@ -78,14 +78,16 @@ static inline void writeq(u64 val, void __iomem *addr)
 
 #define NTB_BAR_MMIO		0
 #define NTB_BAR_23		2
-#define NTB_BAR_45		4
+#define NTB_BAR_4		4
+#define NTB_BAR_5		5
+
 #define NTB_BAR_MASK		((1 << NTB_BAR_MMIO) | (1 << NTB_BAR_23) |\
-				 (1 << NTB_BAR_45))
+				 (1 << NTB_BAR_4))
+#define NTB_SPLITBAR_MASK	((1 << NTB_BAR_MMIO) | (1 << NTB_BAR_23) |\
+				 (1 << NTB_BAR_4) | (1 << NTB_BAR_5))
 
 #define NTB_HB_TIMEOUT		msecs_to_jiffies(1000)
 
-#define NTB_MAX_NUM_MW		2
-
 enum ntb_hw_event {
 	NTB_EVENT_SW_EVENT0 = 0,
 	NTB_EVENT_SW_EVENT1,
@@ -109,11 +111,13 @@ struct ntb_db_cb {
 	struct tasklet_struct irq_work;
 };
 
+#define WA_SNB_ERR	0x00000001
+
 struct ntb_device {
 	struct pci_dev *pdev;
 	struct msix_entry *msix_entries;
 	void __iomem *reg_base;
-	struct ntb_mw mw[NTB_MAX_NUM_MW];
+	struct ntb_mw *mw;
 	struct {
 		unsigned char max_mw;
 		unsigned char max_spads;
@@ -126,6 +130,7 @@ struct ntb_device {
 		void __iomem *rdb;
 		void __iomem *bar2_xlat;
 		void __iomem *bar4_xlat;
+		void __iomem *bar5_xlat;
 		void __iomem *spad_write;
 		void __iomem *spad_read;
 		void __iomem *lnk_cntl;
@@ -145,6 +150,7 @@ struct ntb_device {
 	unsigned char link_width;
 	unsigned char link_speed;
 	unsigned char link_status;
+	unsigned char split_bar;
 
 	struct delayed_work hb_timer;
 	unsigned long last_ts;
@@ -152,6 +158,9 @@ struct ntb_device {
 	struct delayed_work lr_timer;
 
 	struct dentry *debugfs_dir;
+	struct dentry *debugfs_info;
+
+	unsigned int wa_flags;
 };
 
 /**
diff --git a/drivers/ntb/ntb_regs.h b/drivers/ntb/ntb_regs.h
index 9774506419d7..f028ff81fd77 100644
--- a/drivers/ntb/ntb_regs.h
+++ b/drivers/ntb/ntb_regs.h
@@ -57,34 +57,43 @@
 #define SNB_MAX_DB_BITS		15
 #define SNB_LINK_DB		15
 #define SNB_DB_BITS_PER_VEC	5
+#define HSX_SPLITBAR_MAX_MW	3
 #define SNB_MAX_MW		2
 #define SNB_ERRATA_MAX_MW	1
 
 #define SNB_DB_HW_LINK		0x8000
 
+#define SNB_UNCERRSTS_OFFSET	0x014C
+#define SNB_CORERRSTS_OFFSET	0x0158
+#define SNB_LINK_STATUS_OFFSET	0x01A2
 #define SNB_PCICMD_OFFSET	0x0504
 #define SNB_DEVCTRL_OFFSET	0x0598
+#define SNB_DEVSTS_OFFSET	0x059A
 #define SNB_SLINK_STATUS_OFFSET	0x05A2
-#define SNB_LINK_STATUS_OFFSET	0x01A2
 
 #define SNB_PBAR2LMT_OFFSET	0x0000
 #define SNB_PBAR4LMT_OFFSET	0x0008
+#define SNB_PBAR5LMT_OFFSET	0x000C
 #define SNB_PBAR2XLAT_OFFSET	0x0010
 #define SNB_PBAR4XLAT_OFFSET	0x0018
+#define SNB_PBAR5XLAT_OFFSET	0x001C
 #define SNB_SBAR2LMT_OFFSET	0x0020
 #define SNB_SBAR4LMT_OFFSET	0x0028
+#define SNB_SBAR5LMT_OFFSET	0x002C
 #define SNB_SBAR2XLAT_OFFSET	0x0030
 #define SNB_SBAR4XLAT_OFFSET	0x0038
+#define SNB_SBAR5XLAT_OFFSET	0x003C
 #define SNB_SBAR0BASE_OFFSET	0x0040
 #define SNB_SBAR2BASE_OFFSET	0x0048
 #define SNB_SBAR4BASE_OFFSET	0x0050
+#define SNB_SBAR5BASE_OFFSET	0x0054
 #define SNB_NTBCNTL_OFFSET	0x0058
 #define SNB_SBDF_OFFSET		0x005C
 #define SNB_PDOORBELL_OFFSET	0x0060
 #define SNB_PDBMSK_OFFSET	0x0062
 #define SNB_SDOORBELL_OFFSET	0x0064
 #define SNB_SDBMSK_OFFSET	0x0066
-#define SNB_USMEMMISS		0x0070
+#define SNB_USMEMMISS_OFFSET	0x0070
 #define SNB_SPAD_OFFSET		0x0080
 #define SNB_SPADSEMA4_OFFSET	0x00c0
 #define SNB_WCCNTRL_OFFSET	0x00e0
@@ -93,12 +102,18 @@
 #define SNB_B2B_XLAT_OFFSETL	0x0144
 #define SNB_B2B_XLAT_OFFSETU	0x0148
 
-#define SNB_MBAR01_USD_ADDR	0x000000210000000CULL
-#define SNB_MBAR23_USD_ADDR	0x000000410000000CULL
-#define SNB_MBAR45_USD_ADDR	0x000000810000000CULL
-#define SNB_MBAR01_DSD_ADDR	0x000000200000000CULL
-#define SNB_MBAR23_DSD_ADDR	0x000000400000000CULL
-#define SNB_MBAR45_DSD_ADDR	0x000000800000000CULL
+/*
+ * The addresses are setup so the 32bit BARs can function. Thus
+ * the addresses are all in 32bit space
+ */
+#define SNB_MBAR01_USD_ADDR	0x000000002100000CULL
+#define SNB_MBAR23_USD_ADDR	0x000000004100000CULL
+#define SNB_MBAR4_USD_ADDR	0x000000008100000CULL
+#define SNB_MBAR5_USD_ADDR	0x00000000A100000CULL
+#define SNB_MBAR01_DSD_ADDR	0x000000002000000CULL
+#define SNB_MBAR23_DSD_ADDR	0x000000004000000CULL
+#define SNB_MBAR4_DSD_ADDR	0x000000008000000CULL
+#define SNB_MBAR5_DSD_ADDR	0x00000000A000000CULL
 
 #define BWD_MSIX_CNT		34
 #define BWD_MAX_SPADS		16
@@ -147,13 +162,16 @@
 #define NTB_CNTL_LINK_DISABLE		(1 << 1)
 #define NTB_CNTL_S2P_BAR23_SNOOP	(1 << 2)
 #define NTB_CNTL_P2S_BAR23_SNOOP	(1 << 4)
-#define NTB_CNTL_S2P_BAR45_SNOOP	(1 << 6)
-#define NTB_CNTL_P2S_BAR45_SNOOP	(1 << 8)
+#define NTB_CNTL_S2P_BAR4_SNOOP	(1 << 6)
+#define NTB_CNTL_P2S_BAR4_SNOOP	(1 << 8)
+#define NTB_CNTL_S2P_BAR5_SNOOP	(1 << 12)
+#define NTB_CNTL_P2S_BAR5_SNOOP	(1 << 14)
 #define BWD_CNTL_LINK_DOWN		(1 << 16)
 
 #define NTB_PPD_OFFSET		0x00D4
 #define SNB_PPD_CONN_TYPE	0x0003
 #define SNB_PPD_DEV_TYPE	0x0010
+#define SNB_PPD_SPLIT_BAR	(1 << 6)
 #define BWD_PPD_INIT_LINK	0x0008
 #define BWD_PPD_CONN_TYPE	0x0300
 #define BWD_PPD_DEV_TYPE	0x1000
diff --git a/drivers/ntb/ntb_transport.c b/drivers/ntb/ntb_transport.c
index 9dd63b822025..e9bf2f47b61a 100644
--- a/drivers/ntb/ntb_transport.c
+++ b/drivers/ntb/ntb_transport.c
@@ -510,7 +510,7 @@ static void ntb_transport_setup_qp_mw(struct ntb_transport *nt,
 
 	WARN_ON(nt->mw[mw_num].virt_addr == NULL);
 
-	if (nt->max_qps % mw_max && mw_num < nt->max_qps % mw_max)
+	if (nt->max_qps % mw_max && mw_num + 1 < nt->max_qps / mw_max)
 		num_qps_mw = nt->max_qps / mw_max + 1;
 	else
 		num_qps_mw = nt->max_qps / mw_max;
@@ -576,6 +576,19 @@ static int ntb_set_mw(struct ntb_transport *nt, int num_mw, unsigned int size)
 		return -ENOMEM;
 	}
 
+	/*
+	 * we must ensure that the memory address allocated is BAR size
+	 * aligned in order for the XLAT register to take the value. This
+	 * is a requirement of the hardware. It is recommended to setup CMA
+	 * for BAR sizes equal or greater than 4MB.
+	 */
+	if (!IS_ALIGNED(mw->dma_addr, mw->size)) {
+		dev_err(&pdev->dev, "DMA memory %pad not aligned to BAR size\n",
+			&mw->dma_addr);
+		ntb_free_mw(nt, num_mw);
+		return -ENOMEM;
+	}
+
 	/* Notify HW the memory location of the receive buffer */
 	ntb_set_mw_addr(nt->ndev, num_mw, mw->dma_addr);
 
@@ -856,7 +869,7 @@ static int ntb_transport_init_queue(struct ntb_transport *nt,
 	qp->client_ready = NTB_LINK_DOWN;
 	qp->event_handler = NULL;
 
-	if (nt->max_qps % mw_max && mw_num < nt->max_qps % mw_max)
+	if (nt->max_qps % mw_max && mw_num + 1 < nt->max_qps / mw_max)
 		num_qps_mw = nt->max_qps / mw_max + 1;
 	else
 		num_qps_mw = nt->max_qps / mw_max;
diff --git a/drivers/of/Kconfig b/drivers/of/Kconfig
index 5160c4eb73c2..1a13f5b722c5 100644
--- a/drivers/of/Kconfig
+++ b/drivers/of/Kconfig
@@ -11,6 +11,7 @@ config OF_SELFTEST
 	bool "Device Tree Runtime self tests"
 	depends on OF_IRQ && OF_EARLY_FLATTREE
 	select OF_DYNAMIC
+	select OF_RESOLVE
 	help
 	  This option builds in test cases for the device tree infrastructure
 	  that are executed once at boot time, and the results dumped to the
@@ -79,4 +80,7 @@ config OF_RESERVED_MEM
 	help
 	  Helpers to allow for reservation of memory regions
 
+config OF_RESOLVE
+	bool
+
 endmenu # OF
diff --git a/drivers/of/Makefile b/drivers/of/Makefile
index 2b6a7b129d10..ca9209ce50cd 100644
--- a/drivers/of/Makefile
+++ b/drivers/of/Makefile
@@ -13,6 +13,7 @@ obj-$(CONFIG_OF_PCI)	+= of_pci.o
 obj-$(CONFIG_OF_PCI_IRQ)  += of_pci_irq.o
 obj-$(CONFIG_OF_MTD)	+= of_mtd.o
 obj-$(CONFIG_OF_RESERVED_MEM) += of_reserved_mem.o
+obj-$(CONFIG_OF_RESOLVE)  += resolver.o
 
 CFLAGS_fdt.o = -I$(src)/../../scripts/dtc/libfdt
 CFLAGS_fdt_address.o = -I$(src)/../../scripts/dtc/libfdt
diff --git a/drivers/of/address.c b/drivers/of/address.c
index e3718250d66e..afdb78299f61 100644
--- a/drivers/of/address.c
+++ b/drivers/of/address.c
@@ -5,6 +5,8 @@
 #include <linux/module.h>
 #include <linux/of_address.h>
 #include <linux/pci_regs.h>
+#include <linux/sizes.h>
+#include <linux/slab.h>
 #include <linux/string.h>
 
 /* Max address size we deal with */
@@ -293,6 +295,51 @@ struct of_pci_range *of_pci_range_parser_one(struct of_pci_range_parser *parser,
 }
 EXPORT_SYMBOL_GPL(of_pci_range_parser_one);
 
+/*
+ * of_pci_range_to_resource - Create a resource from an of_pci_range
+ * @range:	the PCI range that describes the resource
+ * @np:		device node where the range belongs to
+ * @res:	pointer to a valid resource that will be updated to
+ *              reflect the values contained in the range.
+ *
+ * Returns EINVAL if the range cannot be converted to resource.
+ *
+ * Note that if the range is an IO range, the resource will be converted
+ * using pci_address_to_pio() which can fail if it is called too early or
+ * if the range cannot be matched to any host bridge IO space (our case here).
+ * To guard against that we try to register the IO range first.
+ * If that fails we know that pci_address_to_pio() will do too.
+ */
+int of_pci_range_to_resource(struct of_pci_range *range,
+			     struct device_node *np, struct resource *res)
+{
+	int err;
+	res->flags = range->flags;
+	res->parent = res->child = res->sibling = NULL;
+	res->name = np->full_name;
+
+	if (res->flags & IORESOURCE_IO) {
+		unsigned long port;
+		err = pci_register_io_range(range->cpu_addr, range->size);
+		if (err)
+			goto invalid_range;
+		port = pci_address_to_pio(range->cpu_addr);
+		if (port == (unsigned long)-1) {
+			err = -EINVAL;
+			goto invalid_range;
+		}
+		res->start = port;
+	} else {
+		res->start = range->cpu_addr;
+	}
+	res->end = res->start + range->size - 1;
+	return 0;
+
+invalid_range:
+	res->start = (resource_size_t)OF_BAD_ADDR;
+	res->end = (resource_size_t)OF_BAD_ADDR;
+	return err;
+}
 #endif /* CONFIG_PCI */
 
 /*
@@ -601,12 +648,119 @@ const __be32 *of_get_address(struct device_node *dev, int index, u64 *size,
 }
 EXPORT_SYMBOL(of_get_address);
 
+#ifdef PCI_IOBASE
+struct io_range {
+	struct list_head list;
+	phys_addr_t start;
+	resource_size_t size;
+};
+
+static LIST_HEAD(io_range_list);
+static DEFINE_SPINLOCK(io_range_lock);
+#endif
+
+/*
+ * Record the PCI IO range (expressed as CPU physical address + size).
+ * Return a negative value if an error has occured, zero otherwise
+ */
+int __weak pci_register_io_range(phys_addr_t addr, resource_size_t size)
+{
+	int err = 0;
+
+#ifdef PCI_IOBASE
+	struct io_range *range;
+	resource_size_t allocated_size = 0;
+
+	/* check if the range hasn't been previously recorded */
+	spin_lock(&io_range_lock);
+	list_for_each_entry(range, &io_range_list, list) {
+		if (addr >= range->start && addr + size <= range->start + size) {
+			/* range already registered, bail out */
+			goto end_register;
+		}
+		allocated_size += range->size;
+	}
+
+	/* range not registed yet, check for available space */
+	if (allocated_size + size - 1 > IO_SPACE_LIMIT) {
+		/* if it's too big check if 64K space can be reserved */
+		if (allocated_size + SZ_64K - 1 > IO_SPACE_LIMIT) {
+			err = -E2BIG;
+			goto end_register;
+		}
+
+		size = SZ_64K;
+		pr_warn("Requested IO range too big, new size set to 64K\n");
+	}
+
+	/* add the range to the list */
+	range = kzalloc(sizeof(*range), GFP_KERNEL);
+	if (!range) {
+		err = -ENOMEM;
+		goto end_register;
+	}
+
+	range->start = addr;
+	range->size = size;
+
+	list_add_tail(&range->list, &io_range_list);
+
+end_register:
+	spin_unlock(&io_range_lock);
+#endif
+
+	return err;
+}
+
+phys_addr_t pci_pio_to_address(unsigned long pio)
+{
+	phys_addr_t address = (phys_addr_t)OF_BAD_ADDR;
+
+#ifdef PCI_IOBASE
+	struct io_range *range;
+	resource_size_t allocated_size = 0;
+
+	if (pio > IO_SPACE_LIMIT)
+		return address;
+
+	spin_lock(&io_range_lock);
+	list_for_each_entry(range, &io_range_list, list) {
+		if (pio >= allocated_size && pio < allocated_size + range->size) {
+			address = range->start + pio - allocated_size;
+			break;
+		}
+		allocated_size += range->size;
+	}
+	spin_unlock(&io_range_lock);
+#endif
+
+	return address;
+}
+
 unsigned long __weak pci_address_to_pio(phys_addr_t address)
 {
+#ifdef PCI_IOBASE
+	struct io_range *res;
+	resource_size_t offset = 0;
+	unsigned long addr = -1;
+
+	spin_lock(&io_range_lock);
+	list_for_each_entry(res, &io_range_list, list) {
+		if (address >= res->start && address < res->start + res->size) {
+			addr = res->start - address + offset;
+			break;
+		}
+		offset += res->size;
+	}
+	spin_unlock(&io_range_lock);
+
+	return addr;
+#else
 	if (address > IO_SPACE_LIMIT)
 		return (unsigned long)-1;
 
 	return (unsigned long) address;
+#endif
 }
 
 static int __of_address_to_resource(struct device_node *dev,
diff --git a/drivers/of/base.c b/drivers/of/base.c
index d8574adf0d62..2305dc0382bc 100644
--- a/drivers/of/base.c
+++ b/drivers/of/base.c
@@ -138,6 +138,9 @@ int __of_add_property_sysfs(struct device_node *np, struct property *pp)
 	/* Important: Don't leak passwords */
 	bool secure = strncmp(pp->name, "security-", 9) == 0;
 
+	if (!IS_ENABLED(CONFIG_SYSFS))
+		return 0;
+
 	if (!of_kset || !of_node_is_attached(np))
 		return 0;
 
@@ -158,6 +161,9 @@ int __of_attach_node_sysfs(struct device_node *np)
 	struct property *pp;
 	int rc;
 
+	if (!IS_ENABLED(CONFIG_SYSFS))
+		return 0;
+
 	if (!of_kset)
 		return 0;
 
@@ -1015,6 +1021,9 @@ struct device_node *of_find_node_by_phandle(phandle handle)
 	struct device_node *np;
 	unsigned long flags;
 
+	if (!handle)
+		return NULL;
+
 	raw_spin_lock_irqsave(&devtree_lock, flags);
 	for (np = of_allnodes; np; np = np->allnext)
 		if (np->phandle == handle)
@@ -1713,6 +1722,9 @@ int __of_remove_property(struct device_node *np, struct property *prop)
 
 void __of_remove_property_sysfs(struct device_node *np, struct property *prop)
 {
+	if (!IS_ENABLED(CONFIG_SYSFS))
+		return;
+
 	/* at early boot, bail here and defer setup to of_init() */
 	if (of_kset && of_node_is_attached(np))
 		sysfs_remove_bin_file(&np->kobj, &prop->attr);
@@ -1777,6 +1789,9 @@ int __of_update_property(struct device_node *np, struct property *newprop,
 void __of_update_property_sysfs(struct device_node *np, struct property *newprop,
 		struct property *oldprop)
 {
+	if (!IS_ENABLED(CONFIG_SYSFS))
+		return;
+
 	/* At early boot, bail out and defer setup to of_init() */
 	if (!of_kset)
 		return;
@@ -1847,6 +1862,7 @@ void of_alias_scan(void * (*dt_alloc)(u64 size, u64 align))
 {
 	struct property *pp;
 
+	of_aliases = of_find_node_by_path("/aliases");
 	of_chosen = of_find_node_by_path("/chosen");
 	if (of_chosen == NULL)
 		of_chosen = of_find_node_by_path("/chosen@0");
@@ -1862,7 +1878,6 @@ void of_alias_scan(void * (*dt_alloc)(u64 size, u64 align))
 			of_stdout = of_find_node_by_path(name);
 	}
 
-	of_aliases = of_find_node_by_path("/aliases");
 	if (!of_aliases)
 		return;
 
@@ -1986,7 +2001,7 @@ bool of_console_check(struct device_node *dn, char *name, int index)
 {
 	if (!dn || dn != of_stdout || console_set_on_cmdline)
 		return false;
-	return add_preferred_console(name, index, NULL);
+	return !add_preferred_console(name, index, NULL);
 }
 EXPORT_SYMBOL_GPL(of_console_check);
 
diff --git a/drivers/of/dynamic.c b/drivers/of/dynamic.c
index 54fecc49a1fe..f297891d8529 100644
--- a/drivers/of/dynamic.c
+++ b/drivers/of/dynamic.c
@@ -45,6 +45,9 @@ void __of_detach_node_sysfs(struct device_node *np)
 {
 	struct property *pp;
 
+	if (!IS_ENABLED(CONFIG_SYSFS))
+		return;
+
 	BUG_ON(!of_node_is_initialized(np));
 	if (!of_kset)
 		return;
diff --git a/drivers/of/fdt.c b/drivers/of/fdt.c
index f46a24ffa3fe..d1ffca8b34ea 100644
--- a/drivers/of/fdt.c
+++ b/drivers/of/fdt.c
@@ -453,7 +453,7 @@ static int __init __reserved_mem_reserve_reg(unsigned long node,
 		base = dt_mem_next_cell(dt_root_addr_cells, &prop);
 		size = dt_mem_next_cell(dt_root_size_cells, &prop);
 
-		if (base && size &&
+		if (size &&
 		    early_init_dt_reserve_memory_arch(base, size, nomap) == 0)
 			pr_debug("Reserved memory: reserved region for node '%s': base %pa, size %ld MiB\n",
 				uname, &base, (unsigned long)size / SZ_1M);
@@ -928,7 +928,11 @@ int __init early_init_dt_scan_chosen(unsigned long node, const char *uname,
 void __init __weak early_init_dt_add_memory_arch(u64 base, u64 size)
 {
 	const u64 phys_offset = __pa(PAGE_OFFSET);
-	base &= PAGE_MASK;
+
+	if (!PAGE_ALIGNED(base)) {
+		size -= PAGE_SIZE - (base & ~PAGE_MASK);
+		base = PAGE_ALIGN(base);
+	}
 	size &= PAGE_MASK;
 
 	if (base > MAX_PHYS_ADDR) {
@@ -937,10 +941,10 @@ void __init __weak early_init_dt_add_memory_arch(u64 base, u64 size)
 		return;
 	}
 
-	if (base + size > MAX_PHYS_ADDR) {
-		pr_warning("Ignoring memory range 0x%lx - 0x%llx\n",
-				ULONG_MAX, base + size);
-		size = MAX_PHYS_ADDR - base;
+	if (base + size - 1 > MAX_PHYS_ADDR) {
+		pr_warning("Ignoring memory range 0x%llx - 0x%llx\n",
+				((u64)MAX_PHYS_ADDR) + 1, base + size);
+		size = MAX_PHYS_ADDR - base + 1;
 	}
 
 	if (base + size < phys_offset) {
diff --git a/drivers/of/irq.c b/drivers/of/irq.c
index 3e06a699352d..1471e0a223a5 100644
--- a/drivers/of/irq.c
+++ b/drivers/of/irq.c
@@ -301,16 +301,17 @@ int of_irq_parse_one(struct device_node *device, int index, struct of_phandle_ar
 	/* Get the reg property (if any) */
 	addr = of_get_property(device, "reg", NULL);
 
+	/* Try the new-style interrupts-extended first */
+	res = of_parse_phandle_with_args(device, "interrupts-extended",
+					"#interrupt-cells", index, out_irq);
+	if (!res)
+		return of_irq_parse_raw(addr, out_irq);
+
 	/* Get the interrupts property */
 	intspec = of_get_property(device, "interrupts", &intlen);
-	if (intspec == NULL) {
-		/* Try the new-style interrupts-extended */
-		res = of_parse_phandle_with_args(device, "interrupts-extended",
-						"#interrupt-cells", index, out_irq);
-		if (res)
-			return -EINVAL;
-		return of_irq_parse_raw(addr, out_irq);
-	}
+	if (intspec == NULL)
+		return -EINVAL;
+
 	intlen /= sizeof(*intspec);
 
 	pr_debug(" intspec=%d intlen=%d\n", be32_to_cpup(intspec), intlen);
diff --git a/drivers/of/of_mdio.c b/drivers/of/of_mdio.c
index 401b2453da45..1bd43053b8c7 100644
--- a/drivers/of/of_mdio.c
+++ b/drivers/of/of_mdio.c
@@ -224,6 +224,8 @@ struct phy_device *of_phy_connect(struct net_device *dev,
 	if (!phy)
 		return NULL;
 
+	phy->dev_flags = flags;
+
 	return phy_connect_direct(dev, phy, hndlr, iface) ? NULL : phy;
 }
 EXPORT_SYMBOL(of_phy_connect);
@@ -284,6 +286,7 @@ int of_phy_register_fixed_link(struct device_node *np)
 	struct device_node *fixed_link_node;
 	const __be32 *fixed_link_prop;
 	int len;
+	struct phy_device *phy;
 
 	/* New binding */
 	fixed_link_node = of_get_child_by_name(np, "fixed-link");
@@ -297,7 +300,8 @@ int of_phy_register_fixed_link(struct device_node *np)
 		status.asym_pause = of_property_read_bool(fixed_link_node,
 							  "asym-pause");
 		of_node_put(fixed_link_node);
-		return fixed_phy_register(PHY_POLL, &status, np);
+		phy = fixed_phy_register(PHY_POLL, &status, np);
+		return IS_ERR(phy) ? PTR_ERR(phy) : 0;
 	}
 
 	/* Old binding */
@@ -308,7 +312,8 @@ int of_phy_register_fixed_link(struct device_node *np)
 		status.speed = be32_to_cpu(fixed_link_prop[2]);
 		status.pause = be32_to_cpu(fixed_link_prop[3]);
 		status.asym_pause = be32_to_cpu(fixed_link_prop[4]);
-		return fixed_phy_register(PHY_POLL, &status, np);
+		phy = fixed_phy_register(PHY_POLL, &status, np);
+		return IS_ERR(phy) ? PTR_ERR(phy) : 0;
 	}
 
 	return -ENODEV;
diff --git a/drivers/of/of_pci.c b/drivers/of/of_pci.c
index 848199633798..8882b467be95 100644
--- a/drivers/of/of_pci.c
+++ b/drivers/of/of_pci.c
@@ -1,7 +1,9 @@
 #include <linux/kernel.h>
 #include <linux/export.h>
 #include <linux/of.h>
+#include <linux/of_address.h>
 #include <linux/of_pci.h>
+#include <linux/slab.h>
 
 static inline int __of_pci_pci_compare(struct device_node *node,
 				       unsigned int data)
@@ -89,6 +91,146 @@ int of_pci_parse_bus_range(struct device_node *node, struct resource *res)
 }
 EXPORT_SYMBOL_GPL(of_pci_parse_bus_range);
 
+/**
+ * This function will try to obtain the host bridge domain number by
+ * finding a property called "linux,pci-domain" of the given device node.
+ *
+ * @node: device tree node with the domain information
+ *
+ * Returns the associated domain number from DT in the range [0-0xffff], or
+ * a negative value if the required property is not found.
+ */
+int of_get_pci_domain_nr(struct device_node *node)
+{
+	const __be32 *value;
+	int len;
+	u16 domain;
+
+	value = of_get_property(node, "linux,pci-domain", &len);
+	if (!value || len < sizeof(*value))
+		return -EINVAL;
+
+	domain = (u16)be32_to_cpup(value);
+
+	return domain;
+}
+EXPORT_SYMBOL_GPL(of_get_pci_domain_nr);
+
+#if defined(CONFIG_OF_ADDRESS)
+/**
+ * of_pci_get_host_bridge_resources - Parse PCI host bridge resources from DT
+ * @dev: device node of the host bridge having the range property
+ * @busno: bus number associated with the bridge root bus
+ * @bus_max: maximum number of buses for this bridge
+ * @resources: list where the range of resources will be added after DT parsing
+ * @io_base: pointer to a variable that will contain on return the physical
+ * address for the start of the I/O range. Can be NULL if the caller doesn't
+ * expect IO ranges to be present in the device tree.
+ *
+ * It is the caller's job to free the @resources list.
+ *
+ * This function will parse the "ranges" property of a PCI host bridge device
+ * node and setup the resource mapping based on its content. It is expected
+ * that the property conforms with the Power ePAPR document.
+ *
+ * It returns zero if the range parsing has been successful or a standard error
+ * value if it failed.
+ */
+int of_pci_get_host_bridge_resources(struct device_node *dev,
+			unsigned char busno, unsigned char bus_max,
+			struct list_head *resources, resource_size_t *io_base)
+{
+	struct resource *res;
+	struct resource *bus_range;
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	char range_type[4];
+	int err;
+
+	if (io_base)
+		*io_base = (resource_size_t)OF_BAD_ADDR;
+
+	bus_range = kzalloc(sizeof(*bus_range), GFP_KERNEL);
+	if (!bus_range)
+		return -ENOMEM;
+
+	pr_info("PCI host bridge %s ranges:\n", dev->full_name);
+
+	err = of_pci_parse_bus_range(dev, bus_range);
+	if (err) {
+		bus_range->start = busno;
+		bus_range->end = bus_max;
+		bus_range->flags = IORESOURCE_BUS;
+		pr_info("  No bus range found for %s, using %pR\n",
+			dev->full_name, bus_range);
+	} else {
+		if (bus_range->end > bus_range->start + bus_max)
+			bus_range->end = bus_range->start + bus_max;
+	}
+	pci_add_resource(resources, bus_range);
+
+	/* Check for ranges property */
+	err = of_pci_range_parser_init(&parser, dev);
+	if (err)
+		goto parse_failed;
+
+	pr_debug("Parsing ranges property...\n");
+	for_each_of_pci_range(&parser, &range) {
+		/* Read next ranges element */
+		if ((range.flags & IORESOURCE_TYPE_BITS) == IORESOURCE_IO)
+			snprintf(range_type, 4, " IO");
+		else if ((range.flags & IORESOURCE_TYPE_BITS) == IORESOURCE_MEM)
+			snprintf(range_type, 4, "MEM");
+		else
+			snprintf(range_type, 4, "err");
+		pr_info("  %s %#010llx..%#010llx -> %#010llx\n", range_type,
+			range.cpu_addr, range.cpu_addr + range.size - 1,
+			range.pci_addr);
+
+		/*
+		 * If we failed translation or got a zero-sized region
+		 * then skip this range
+		 */
+		if (range.cpu_addr == OF_BAD_ADDR || range.size == 0)
+			continue;
+
+		res = kzalloc(sizeof(struct resource), GFP_KERNEL);
+		if (!res) {
+			err = -ENOMEM;
+			goto parse_failed;
+		}
+
+		err = of_pci_range_to_resource(&range, dev, res);
+		if (err)
+			goto conversion_failed;
+
+		if (resource_type(res) == IORESOURCE_IO) {
+			if (!io_base) {
+				pr_err("I/O range found for %s. Please provide an io_base pointer to save CPU base address\n",
+					dev->full_name);
+				err = -EINVAL;
+				goto conversion_failed;
+			}
+			if (*io_base != (resource_size_t)OF_BAD_ADDR)
+				pr_warn("More than one I/O resource converted for %s. CPU base address for old range lost!\n",
+					dev->full_name);
+			*io_base = range.cpu_addr;
+		}
+
+		pci_add_resource_offset(resources, res,	res->start - range.pci_addr);
+	}
+
+	return 0;
+
+conversion_failed:
+	kfree(res);
+parse_failed:
+	pci_free_resource_list(resources);
+	return err;
+}
+EXPORT_SYMBOL_GPL(of_pci_get_host_bridge_resources);
+#endif /* CONFIG_OF_ADDRESS */
+
 #ifdef CONFIG_PCI_MSI
 
 static LIST_HEAD(of_pci_msi_chip_list);
diff --git a/drivers/of/platform.c b/drivers/of/platform.c
index 0197725e033a..3b64d0bf5bba 100644
--- a/drivers/of/platform.c
+++ b/drivers/of/platform.c
@@ -160,11 +160,10 @@ EXPORT_SYMBOL(of_device_alloc);
  * can use Platform bus notifier and handle BUS_NOTIFY_ADD_DEVICE event
  * to fix up DMA configuration.
  */
-static void of_dma_configure(struct platform_device *pdev)
+static void of_dma_configure(struct device *dev)
 {
 	u64 dma_addr, paddr, size;
 	int ret;
-	struct device *dev = &pdev->dev;
 
 	/*
 	 * Set default dma-mask to 32 bit. Drivers are expected to setup
@@ -229,7 +228,7 @@ static struct platform_device *of_platform_device_create_pdata(
 	if (!dev)
 		goto err_clear_flag;
 
-	of_dma_configure(dev);
+	of_dma_configure(&dev->dev);
 	dev->dev.bus = &platform_bus_type;
 	dev->dev.platform_data = platform_data;
 
@@ -291,7 +290,6 @@ static struct amba_device *of_amba_device_create(struct device_node *node,
 	}
 
 	/* setup generic device info */
-	dev->dev.coherent_dma_mask = ~0;
 	dev->dev.of_node = of_node_get(node);
 	dev->dev.parent = parent;
 	dev->dev.platform_data = platform_data;
@@ -299,6 +297,7 @@ static struct amba_device *of_amba_device_create(struct device_node *node,
 		dev_set_name(&dev->dev, "%s", bus_id);
 	else
 		of_device_make_bus_id(&dev->dev);
+	of_dma_configure(&dev->dev);
 
 	/* Allow the HW Peripheral ID to be overridden */
 	prop = of_get_property(node, "arm,primecell-periphid", NULL);
diff --git a/drivers/of/resolver.c b/drivers/of/resolver.c
new file mode 100644
index 000000000000..aed7959f800d
--- /dev/null
+++ b/drivers/of/resolver.c
@@ -0,0 +1,336 @@
+/*
+ * Functions for dealing with DT resolution
+ *
+ * Copyright (C) 2012 Pantelis Antoniou <panto@antoniou-consulting.com>
+ * Copyright (C) 2012 Texas Instruments Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/string.h>
+#include <linux/ctype.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/slab.h>
+
+/* illegal phandle value (set when unresolved) */
+#define OF_PHANDLE_ILLEGAL	0xdeadbeef
+
+/**
+ * Find a node with the give full name by recursively following any of
+ * the child node links.
+ */
+static struct device_node *__of_find_node_by_full_name(struct device_node *node,
+		const char *full_name)
+{
+	struct device_node *child, *found;
+
+	if (node == NULL)
+		return NULL;
+
+	/* check */
+	if (of_node_cmp(node->full_name, full_name) == 0)
+		return node;
+
+	for_each_child_of_node(node, child) {
+		found = __of_find_node_by_full_name(child, full_name);
+		if (found != NULL)
+			return found;
+	}
+
+	return NULL;
+}
+
+/*
+ * Find live tree's maximum phandle value.
+ */
+static phandle of_get_tree_max_phandle(void)
+{
+	struct device_node *node;
+	phandle phandle;
+	unsigned long flags;
+
+	/* now search recursively */
+	raw_spin_lock_irqsave(&devtree_lock, flags);
+	phandle = 0;
+	for_each_of_allnodes(node) {
+		if (node->phandle != OF_PHANDLE_ILLEGAL &&
+				node->phandle > phandle)
+			phandle = node->phandle;
+	}
+	raw_spin_unlock_irqrestore(&devtree_lock, flags);
+
+	return phandle;
+}
+
+/*
+ * Adjust a subtree's phandle values by a given delta.
+ * Makes sure not to just adjust the device node's phandle value,
+ * but modify the phandle properties values as well.
+ */
+static void __of_adjust_tree_phandles(struct device_node *node,
+		int phandle_delta)
+{
+	struct device_node *child;
+	struct property *prop;
+	phandle phandle;
+
+	/* first adjust the node's phandle direct value */
+	if (node->phandle != 0 && node->phandle != OF_PHANDLE_ILLEGAL)
+		node->phandle += phandle_delta;
+
+	/* now adjust phandle & linux,phandle values */
+	for_each_property_of_node(node, prop) {
+
+		/* only look for these two */
+		if (of_prop_cmp(prop->name, "phandle") != 0 &&
+		    of_prop_cmp(prop->name, "linux,phandle") != 0)
+			continue;
+
+		/* must be big enough */
+		if (prop->length < 4)
+			continue;
+
+		/* read phandle value */
+		phandle = be32_to_cpup(prop->value);
+		if (phandle == OF_PHANDLE_ILLEGAL)	/* unresolved */
+			continue;
+
+		/* adjust */
+		*(uint32_t *)prop->value = cpu_to_be32(node->phandle);
+	}
+
+	/* now do the children recursively */
+	for_each_child_of_node(node, child)
+		__of_adjust_tree_phandles(child, phandle_delta);
+}
+
+static int __of_adjust_phandle_ref(struct device_node *node, struct property *rprop, int value, bool is_delta)
+{
+	phandle phandle;
+	struct device_node *refnode;
+	struct property *sprop;
+	char *propval, *propcur, *propend, *nodestr, *propstr, *s;
+	int offset, propcurlen;
+	int err = 0;
+
+	/* make a copy */
+	propval = kmalloc(rprop->length, GFP_KERNEL);
+	if (!propval) {
+		pr_err("%s: Could not copy value of '%s'\n",
+				__func__, rprop->name);
+		return -ENOMEM;
+	}
+	memcpy(propval, rprop->value, rprop->length);
+
+	propend = propval + rprop->length;
+	for (propcur = propval; propcur < propend; propcur += propcurlen + 1) {
+		propcurlen = strlen(propcur);
+
+		nodestr = propcur;
+		s = strchr(propcur, ':');
+		if (!s) {
+			pr_err("%s: Illegal symbol entry '%s' (1)\n",
+				__func__, propcur);
+			err = -EINVAL;
+			goto err_fail;
+		}
+		*s++ = '\0';
+
+		propstr = s;
+		s = strchr(s, ':');
+		if (!s) {
+			pr_err("%s: Illegal symbol entry '%s' (2)\n",
+				__func__, (char *)rprop->value);
+			err = -EINVAL;
+			goto err_fail;
+		}
+
+		*s++ = '\0';
+		err = kstrtoint(s, 10, &offset);
+		if (err != 0) {
+			pr_err("%s: Could get offset '%s'\n",
+				__func__, (char *)rprop->value);
+			goto err_fail;
+		}
+
+		/* look into the resolve node for the full path */
+		refnode = __of_find_node_by_full_name(node, nodestr);
+		if (!refnode) {
+			pr_warn("%s: Could not find refnode '%s'\n",
+				__func__, (char *)rprop->value);
+			continue;
+		}
+
+		/* now find the property */
+		for_each_property_of_node(refnode, sprop) {
+			if (of_prop_cmp(sprop->name, propstr) == 0)
+				break;
+		}
+
+		if (!sprop) {
+			pr_err("%s: Could not find property '%s'\n",
+				__func__, (char *)rprop->value);
+			err = -ENOENT;
+			goto err_fail;
+		}
+
+		phandle = is_delta ? be32_to_cpup(sprop->value + offset) + value : value;
+		*(__be32 *)(sprop->value + offset) = cpu_to_be32(phandle);
+	}
+
+err_fail:
+	kfree(propval);
+	return err;
+}
+
+/*
+ * Adjust the local phandle references by the given phandle delta.
+ * Assumes the existances of a __local_fixups__ node at the root
+ * of the tree. Does not take any devtree locks so make sure you
+ * call this on a tree which is at the detached state.
+ */
+static int __of_adjust_tree_phandle_references(struct device_node *node,
+		int phandle_delta)
+{
+	struct device_node *child;
+	struct property *rprop;
+	int err;
+
+	/* locate the symbols & fixups nodes on resolve */
+	for_each_child_of_node(node, child)
+		if (of_node_cmp(child->name, "__local_fixups__") == 0)
+			break;
+
+	/* no local fixups */
+	if (!child)
+		return 0;
+
+	/* find the local fixups property */
+	for_each_property_of_node(child, rprop) {
+		/* skip properties added automatically */
+		if (of_prop_cmp(rprop->name, "name") == 0)
+			continue;
+
+		err = __of_adjust_phandle_ref(node, rprop, phandle_delta, true);
+		if (err)
+			return err;
+	}
+
+	return 0;
+}
+
+/**
+ * of_resolve	- Resolve the given node against the live tree.
+ *
+ * @resolve:	Node to resolve
+ *
+ * Perform dynamic Device Tree resolution against the live tree
+ * to the given node to resolve. This depends on the live tree
+ * having a __symbols__ node, and the resolve node the __fixups__ &
+ * __local_fixups__ nodes (if needed).
+ * The result of the operation is a resolve node that it's contents
+ * are fit to be inserted or operate upon the live tree.
+ * Returns 0 on success or a negative error value on error.
+ */
+int of_resolve_phandles(struct device_node *resolve)
+{
+	struct device_node *child, *refnode;
+	struct device_node *root_sym, *resolve_sym, *resolve_fix;
+	struct property *rprop;
+	const char *refpath;
+	phandle phandle, phandle_delta;
+	int err;
+
+	/* the resolve node must exist, and be detached */
+	if (!resolve || !of_node_check_flag(resolve, OF_DETACHED))
+		return -EINVAL;
+
+	/* first we need to adjust the phandles */
+	phandle_delta = of_get_tree_max_phandle() + 1;
+	__of_adjust_tree_phandles(resolve, phandle_delta);
+	err = __of_adjust_tree_phandle_references(resolve, phandle_delta);
+	if (err != 0)
+		return err;
+
+	root_sym = NULL;
+	resolve_sym = NULL;
+	resolve_fix = NULL;
+
+	/* this may fail (if no fixups are required) */
+	root_sym = of_find_node_by_path("/__symbols__");
+
+	/* locate the symbols & fixups nodes on resolve */
+	for_each_child_of_node(resolve, child) {
+
+		if (!resolve_sym &&
+				of_node_cmp(child->name, "__symbols__") == 0)
+			resolve_sym = child;
+
+		if (!resolve_fix &&
+				of_node_cmp(child->name, "__fixups__") == 0)
+			resolve_fix = child;
+
+		/* both found, don't bother anymore */
+		if (resolve_sym && resolve_fix)
+			break;
+	}
+
+	/* we do allow for the case where no fixups are needed */
+	if (!resolve_fix) {
+		err = 0;	/* no error */
+		goto out;
+	}
+
+	/* we need to fixup, but no root symbols... */
+	if (!root_sym) {
+		err = -EINVAL;
+		goto out;
+	}
+
+	for_each_property_of_node(resolve_fix, rprop) {
+
+		/* skip properties added automatically */
+		if (of_prop_cmp(rprop->name, "name") == 0)
+			continue;
+
+		err = of_property_read_string(root_sym,
+				rprop->name, &refpath);
+		if (err != 0) {
+			pr_err("%s: Could not find symbol '%s'\n",
+					__func__, rprop->name);
+			goto out;
+		}
+
+		refnode = of_find_node_by_path(refpath);
+		if (!refnode) {
+			pr_err("%s: Could not find node by path '%s'\n",
+					__func__, refpath);
+			err = -ENOENT;
+			goto out;
+		}
+
+		phandle = refnode->phandle;
+		of_node_put(refnode);
+
+		pr_debug("%s: %s phandle is 0x%08x\n",
+				__func__, rprop->name, phandle);
+
+		err = __of_adjust_phandle_ref(resolve, rprop, phandle, false);
+		if (err)
+			break;
+	}
+
+out:
+	/* NULL is handled by of_node_put as NOP */
+	of_node_put(root_sym);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(of_resolve_phandles);
diff --git a/drivers/of/selftest.c b/drivers/of/selftest.c
index d41002667833..78001270a598 100644
--- a/drivers/of/selftest.c
+++ b/drivers/of/selftest.c
@@ -7,6 +7,7 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/errno.h>
+#include <linux/hashtable.h>
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/of_fdt.h>
@@ -24,9 +25,10 @@ static struct selftest_results {
 	int failed;
 } selftest_results;
 
-#define NO_OF_NODES 2
+#define NO_OF_NODES 3
 static struct device_node *nodes[NO_OF_NODES];
 static int last_node_index;
+static bool selftest_live_tree;
 
 #define selftest(result, fmt, ...) { \
 	if (!(result)) { \
@@ -144,6 +146,97 @@ static void __init of_selftest_dynamic(void)
 			 "Adding a large property should have passed\n");
 }
 
+static int __init of_selftest_check_node_linkage(struct device_node *np)
+{
+	struct device_node *child, *allnext_index = np;
+	int count = 0, rc;
+
+	for_each_child_of_node(np, child) {
+		if (child->parent != np) {
+			pr_err("Child node %s links to wrong parent %s\n",
+				 child->name, np->name);
+			return -EINVAL;
+		}
+
+		while (allnext_index && allnext_index != child)
+			allnext_index = allnext_index->allnext;
+		if (allnext_index != child) {
+			pr_err("Node %s is ordered differently in sibling and allnode lists\n",
+				 child->name);
+			return -EINVAL;
+		}
+
+		rc = of_selftest_check_node_linkage(child);
+		if (rc < 0)
+			return rc;
+		count += rc;
+	}
+
+	return count + 1;
+}
+
+static void __init of_selftest_check_tree_linkage(void)
+{
+	struct device_node *np;
+	int allnode_count = 0, child_count;
+
+	if (!of_allnodes)
+		return;
+
+	for_each_of_allnodes(np)
+		allnode_count++;
+	child_count = of_selftest_check_node_linkage(of_allnodes);
+
+	selftest(child_count > 0, "Device node data structure is corrupted\n");
+	selftest(child_count == allnode_count, "allnodes list size (%i) doesn't match"
+		 "sibling lists size (%i)\n", allnode_count, child_count);
+	pr_debug("allnodes list size (%i); sibling lists size (%i)\n", allnode_count, child_count);
+}
+
+struct node_hash {
+	struct hlist_node node;
+	struct device_node *np;
+};
+
+static DEFINE_HASHTABLE(phandle_ht, 8);
+static void __init of_selftest_check_phandles(void)
+{
+	struct device_node *np;
+	struct node_hash *nh;
+	struct hlist_node *tmp;
+	int i, dup_count = 0, phandle_count = 0;
+
+	for_each_of_allnodes(np) {
+		if (!np->phandle)
+			continue;
+
+		hash_for_each_possible(phandle_ht, nh, node, np->phandle) {
+			if (nh->np->phandle == np->phandle) {
+				pr_info("Duplicate phandle! %i used by %s and %s\n",
+					np->phandle, nh->np->full_name, np->full_name);
+				dup_count++;
+				break;
+			}
+		}
+
+		nh = kzalloc(sizeof(*nh), GFP_KERNEL);
+		if (WARN_ON(!nh))
+			return;
+
+		nh->np = np;
+		hash_add(phandle_ht, &nh->node, np->phandle);
+		phandle_count++;
+	}
+	selftest(dup_count == 0, "Found %i duplicates in %i phandles\n",
+		 dup_count, phandle_count);
+
+	/* Clean up */
+	hash_for_each_safe(phandle_ht, i, tmp, nh, node) {
+		hash_del(&nh->node);
+		kfree(nh);
+	}
+}
+
 static void __init of_selftest_parse_phandle_with_args(void)
 {
 	struct device_node *np;
@@ -630,19 +723,14 @@ static int attach_node_and_children(struct device_node *np)
 {
 	struct device_node *next, *root = np, *dup;
 
-	if (!np) {
-		pr_warn("%s: No tree to attach; not running tests\n",
-			__func__);
-		return -ENODATA;
-	}
-
-
 	/* skip root node */
 	np = np->child;
 	/* storing a copy in temporary node */
 	dup = np;
 
 	while (dup) {
+		if (WARN_ON(last_node_index >= NO_OF_NODES))
+			return -EINVAL;
 		nodes[last_node_index++] = dup;
 		dup = dup->sibling;
 	}
@@ -672,12 +760,13 @@ static int attach_node_and_children(struct device_node *np)
 static int __init selftest_data_add(void)
 {
 	void *selftest_data;
-	struct device_node *selftest_data_node;
+	struct device_node *selftest_data_node, *np;
 	extern uint8_t __dtb_testcases_begin[];
 	extern uint8_t __dtb_testcases_end[];
 	const int size = __dtb_testcases_end - __dtb_testcases_begin;
+	int rc;
 
-	if (!size || !of_allnodes) {
+	if (!size) {
 		pr_warn("%s: No testcase data to attach; not running tests\n",
 			__func__);
 		return -ENODATA;
@@ -692,6 +781,28 @@ static int __init selftest_data_add(void)
 		return -ENOMEM;
 	}
 	of_fdt_unflatten_tree(selftest_data, &selftest_data_node);
+	if (!selftest_data_node) {
+		pr_warn("%s: No tree to attach; not running tests\n", __func__);
+		return -ENODATA;
+	}
+	of_node_set_flag(selftest_data_node, OF_DETACHED);
+	rc = of_resolve_phandles(selftest_data_node);
+	if (rc) {
+		pr_err("%s: Failed to resolve phandles (rc=%i)\n", __func__, rc);
+		return -EINVAL;
+	}
+
+	if (!of_allnodes) {
+		/* enabling flag for removing nodes */
+		selftest_live_tree = true;
+		of_allnodes = selftest_data_node;
+
+		for_each_of_allnodes(np)
+			__of_attach_node_sysfs(np);
+		of_aliases = of_find_node_by_path("/aliases");
+		of_chosen = of_find_node_by_path("/chosen");
+		return 0;
+	}
 
 	/* attach the sub-tree to live tree */
 	return attach_node_and_children(selftest_data_node);
@@ -707,10 +818,6 @@ static void detach_node_and_children(struct device_node *np)
 {
 	while (np->child)
 		detach_node_and_children(np->child);
-
-	while (np->sibling)
-		detach_node_and_children(np->sibling);
-
 	of_detach_node(np);
 }
 
@@ -723,12 +830,23 @@ static void selftest_data_remove(void)
 	struct device_node *np;
 	struct property *prop;
 
+	if (selftest_live_tree) {
+		of_node_put(of_aliases);
+		of_node_put(of_chosen);
+		of_aliases = NULL;
+		of_chosen = NULL;
+		for_each_child_of_node(of_allnodes, np)
+			detach_node_and_children(np);
+		__of_detach_node_sysfs(of_allnodes);
+		of_allnodes = NULL;
+		return;
+	}
+
 	while (last_node_index >= 0) {
 		if (nodes[last_node_index]) {
 			np = of_find_node_by_path(nodes[last_node_index]->full_name);
 			if (strcmp(np->full_name, "/aliases") != 0) {
-				detach_node_and_children(np->child);
-				of_detach_node(np);
+				detach_node_and_children(np);
 			} else {
 				for_each_property_of_node(np, prop) {
 					if (strcmp(prop->name, "testcase-alias") == 0)
@@ -758,6 +876,8 @@ static int __init of_selftest(void)
 	of_node_put(np);
 
 	pr_info("start of selftest - you will see error messages\n");
+	of_selftest_check_tree_linkage();
+	of_selftest_check_phandles();
 	of_selftest_find_node_by_name();
 	of_selftest_dynamic();
 	of_selftest_parse_phandle_with_args();
@@ -768,12 +888,16 @@ static int __init of_selftest(void)
 	of_selftest_parse_interrupts_extended();
 	of_selftest_match_node();
 	of_selftest_platform_populate();
-	pr_info("end of selftest - %i passed, %i failed\n",
-		selftest_results.passed, selftest_results.failed);
 
 	/* removing selftest data from live tree */
 	selftest_data_remove();
 
+	/* Double check linkage after removing testcase data */
+	of_selftest_check_tree_linkage();
+
+	pr_info("end of selftest - %i passed, %i failed\n",
+		selftest_results.passed, selftest_results.failed);
+
 	return 0;
 }
 late_initcall(of_selftest);
diff --git a/drivers/of/testcase-data/testcases.dts b/drivers/of/testcase-data/testcases.dts
index 219ef9324e9c..6994e15c24bf 100644
--- a/drivers/of/testcase-data/testcases.dts
+++ b/drivers/of/testcase-data/testcases.dts
@@ -13,3 +13,38 @@
 #include "tests-interrupts.dtsi"
 #include "tests-match.dtsi"
 #include "tests-platform.dtsi"
+
+/*
+ * phandle fixup data - generated by dtc patches that aren't upstream.
+ * This data must be regenerated whenever phandle references are modified in
+ * the testdata tree.
+ *
+ * The format of this data may be subject to change. For the time being consider
+ * this a kernel-internal data format.
+ */
+/ { __local_fixups__ {
+	fixup = "/testcase-data/testcase-device2:interrupt-parent:0",
+		"/testcase-data/testcase-device1:interrupt-parent:0",
+		"/testcase-data/interrupts/interrupts-extended0:interrupts-extended:60",
+		"/testcase-data/interrupts/interrupts-extended0:interrupts-extended:52",
+		"/testcase-data/interrupts/interrupts-extended0:interrupts-extended:44",
+		"/testcase-data/interrupts/interrupts-extended0:interrupts-extended:36",
+		"/testcase-data/interrupts/interrupts-extended0:interrupts-extended:24",
+		"/testcase-data/interrupts/interrupts-extended0:interrupts-extended:8",
+		"/testcase-data/interrupts/interrupts-extended0:interrupts-extended:0",
+		"/testcase-data/interrupts/interrupts1:interrupt-parent:0",
+		"/testcase-data/interrupts/interrupts0:interrupt-parent:0",
+		"/testcase-data/interrupts/intmap1:interrupt-map:12",
+		"/testcase-data/interrupts/intmap0:interrupt-map:52",
+		"/testcase-data/interrupts/intmap0:interrupt-map:36",
+		"/testcase-data/interrupts/intmap0:interrupt-map:16",
+		"/testcase-data/interrupts/intmap0:interrupt-map:4",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list-bad-args:12",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list-bad-args:0",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list:56",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list:52",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list:40",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list:24",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list:8",
+		"/testcase-data/phandle-tests/consumer-a:phandle-list:0";
+}; };
diff --git a/drivers/oprofile/cpu_buffer.c b/drivers/oprofile/cpu_buffer.c
index 8aa73fac6ad4..0581461c3a67 100644
--- a/drivers/oprofile/cpu_buffer.c
+++ b/drivers/oprofile/cpu_buffer.c
@@ -45,7 +45,7 @@ unsigned long oprofile_get_cpu_buffer_size(void)
 
 void oprofile_cpu_buffer_inc_smpl_lost(void)
 {
-	struct oprofile_cpu_buffer *cpu_buf = &__get_cpu_var(op_cpu_buffer);
+	struct oprofile_cpu_buffer *cpu_buf = this_cpu_ptr(&op_cpu_buffer);
 
 	cpu_buf->sample_lost_overflow++;
 }
@@ -297,7 +297,7 @@ __oprofile_add_ext_sample(unsigned long pc, struct pt_regs * const regs,
 			  unsigned long event, int is_kernel,
 			  struct task_struct *task)
 {
-	struct oprofile_cpu_buffer *cpu_buf = &__get_cpu_var(op_cpu_buffer);
+	struct oprofile_cpu_buffer *cpu_buf = this_cpu_ptr(&op_cpu_buffer);
 	unsigned long backtrace = oprofile_backtrace_depth;
 
 	/*
@@ -357,7 +357,7 @@ oprofile_write_reserve(struct op_entry *entry, struct pt_regs * const regs,
 {
 	struct op_sample *sample;
 	int is_kernel = !user_mode(regs);
-	struct oprofile_cpu_buffer *cpu_buf = &__get_cpu_var(op_cpu_buffer);
+	struct oprofile_cpu_buffer *cpu_buf = this_cpu_ptr(&op_cpu_buffer);
 
 	cpu_buf->sample_received++;
 
@@ -412,13 +412,13 @@ int oprofile_write_commit(struct op_entry *entry)
 
 void oprofile_add_pc(unsigned long pc, int is_kernel, unsigned long event)
 {
-	struct oprofile_cpu_buffer *cpu_buf = &__get_cpu_var(op_cpu_buffer);
+	struct oprofile_cpu_buffer *cpu_buf = this_cpu_ptr(&op_cpu_buffer);
 	log_sample(cpu_buf, pc, 0, is_kernel, event, NULL);
 }
 
 void oprofile_add_trace(unsigned long pc)
 {
-	struct oprofile_cpu_buffer *cpu_buf = &__get_cpu_var(op_cpu_buffer);
+	struct oprofile_cpu_buffer *cpu_buf = this_cpu_ptr(&op_cpu_buffer);
 
 	if (!cpu_buf->tracing)
 		return;
diff --git a/drivers/oprofile/timer_int.c b/drivers/oprofile/timer_int.c
index 61be1d9c16c8..bdef916e5dda 100644
--- a/drivers/oprofile/timer_int.c
+++ b/drivers/oprofile/timer_int.c
@@ -32,7 +32,7 @@ static enum hrtimer_restart oprofile_hrtimer_notify(struct hrtimer *hrtimer)
 
 static void __oprofile_hrtimer_start(void *unused)
 {
-	struct hrtimer *hrtimer = &__get_cpu_var(oprofile_hrtimer);
+	struct hrtimer *hrtimer = this_cpu_ptr(&oprofile_hrtimer);
 
 	if (!ctr_running)
 		return;
diff --git a/drivers/parisc/dino.c b/drivers/parisc/dino.c
index 9eae9834bcc7..a0580afe1713 100644
--- a/drivers/parisc/dino.c
+++ b/drivers/parisc/dino.c
@@ -913,7 +913,7 @@ static int __init dino_probe(struct parisc_device *dev)
 	printk("%s version %s found at 0x%lx\n", name, version, hpa);
 
 	if (!request_mem_region(hpa, PAGE_SIZE, name)) {
-		printk(KERN_ERR "DINO: Hey! Someone took my MMIO space (0x%ld)!\n",
+		printk(KERN_ERR "DINO: Hey! Someone took my MMIO space (0x%lx)!\n",
 			hpa);
 		return 1;
 	}
diff --git a/drivers/parisc/pdc_stable.c b/drivers/parisc/pdc_stable.c
index 0f54ab6260df..3651c3871d5b 100644
--- a/drivers/parisc/pdc_stable.c
+++ b/drivers/parisc/pdc_stable.c
@@ -278,7 +278,7 @@ pdcspath_hwpath_write(struct pdcspath_entry *entry, const char *buf, size_t coun
 {
 	struct hardware_path hwpath;
 	unsigned short i;
-	char in[count+1], *temp;
+	char in[64], *temp;
 	struct device *dev;
 	int ret;
 
@@ -286,8 +286,9 @@ pdcspath_hwpath_write(struct pdcspath_entry *entry, const char *buf, size_t coun
 		return -EINVAL;
 
 	/* We'll use a local copy of buf */
-	memset(in, 0, count+1);
+	count = min_t(size_t, count, sizeof(in)-1);
 	strncpy(in, buf, count);
+	in[count] = '\0';
 	
 	/* Let's clean up the target. 0xff is a blank pattern */
 	memset(&hwpath, 0xff, sizeof(hwpath));
@@ -393,14 +394,15 @@ pdcspath_layer_write(struct pdcspath_entry *entry, const char *buf, size_t count
 {
 	unsigned int layers[6]; /* device-specific info (ctlr#, unit#, ...) */
 	unsigned short i;
-	char in[count+1], *temp;
+	char in[64], *temp;
 
 	if (!entry || !buf || !count)
 		return -EINVAL;
 
 	/* We'll use a local copy of buf */
-	memset(in, 0, count+1);
+	count = min_t(size_t, count, sizeof(in)-1);
 	strncpy(in, buf, count);
+	in[count] = '\0';
 	
 	/* Let's clean up the target. 0 is a blank pattern */
 	memset(&layers, 0, sizeof(layers));
@@ -755,7 +757,7 @@ static ssize_t pdcs_auto_write(struct kobject *kobj,
 {
 	struct pdcspath_entry *pathentry;
 	unsigned char flags;
-	char in[count+1], *temp;
+	char in[8], *temp;
 	char c;
 
 	if (!capable(CAP_SYS_ADMIN))
@@ -765,8 +767,9 @@ static ssize_t pdcs_auto_write(struct kobject *kobj,
 		return -EINVAL;
 
 	/* We'll use a local copy of buf */
-	memset(in, 0, count+1);
+	count = min_t(size_t, count, sizeof(in)-1);
 	strncpy(in, buf, count);
+	in[count] = '\0';
 
 	/* Current flags are stored in primary boot path entry */
 	pathentry = &pdcspath_entry_primary;
diff --git a/drivers/parisc/power.c b/drivers/parisc/power.c
index 90cca5e3805f..ef31b77404ef 100644
--- a/drivers/parisc/power.c
+++ b/drivers/parisc/power.c
@@ -121,7 +121,6 @@ static int kpowerswd(void *param)
 		unsigned long soft_power_reg = (unsigned long) param;
 
 		schedule_timeout_interruptible(pwrsw_enabled ? HZ : HZ/POWERSWITCH_POLL_PER_SEC);
-		__set_current_state(TASK_RUNNING);
 
 		if (unlikely(!pwrsw_enabled))
 			continue;
diff --git a/drivers/parisc/superio.c b/drivers/parisc/superio.c
index a042d065a0c7..8be2096c8423 100644
--- a/drivers/parisc/superio.c
+++ b/drivers/parisc/superio.c
@@ -395,7 +395,8 @@ static void __init superio_serial_init(void)
 	serial_port.iotype	= UPIO_PORT;
 	serial_port.type	= PORT_16550A;
 	serial_port.uartclk	= 115200*16;
-	serial_port.fifosize	= 16;
+	serial_port.flags	= UPF_FIXED_PORT | UPF_FIXED_TYPE |
+				  UPF_BOOT_AUTOCONF;
 
 	/* serial port #1 */
 	serial_port.iobase	= sio_dev.sp1_base;
diff --git a/drivers/parport/parport_pc.c b/drivers/parport/parport_pc.c
index 76ee7750bc5e..f721299eb1ba 100644
--- a/drivers/parport/parport_pc.c
+++ b/drivers/parport/parport_pc.c
@@ -1702,6 +1702,46 @@ static int parport_ECP_supported(struct parport *pb)
 }
 #endif
 
+#ifdef CONFIG_X86_32
+static int intel_bug_present_check_epp(struct parport *pb)
+{
+	const struct parport_pc_private *priv = pb->private_data;
+	int bug_present = 0;
+
+	if (priv->ecr) {
+		/* store value of ECR */
+		unsigned char ecr = inb(ECONTROL(pb));
+		unsigned char i;
+		for (i = 0x00; i < 0x80; i += 0x20) {
+			ECR_WRITE(pb, i);
+			if (clear_epp_timeout(pb)) {
+				/* Phony EPP in ECP. */
+				bug_present = 1;
+				break;
+			}
+		}
+		/* return ECR into the inital state */
+		ECR_WRITE(pb, ecr);
+	}
+
+	return bug_present;
+}
+static int intel_bug_present(struct parport *pb)
+{
+/* Check whether the device is legacy, not PCI or PCMCIA. Only legacy is known to be affected. */
+	if (pb->dev != NULL) {
+		return 0;
+	}
+
+	return intel_bug_present_check_epp(pb);
+}
+#else
+static int intel_bug_present(struct parport *pb)
+{
+	return 0;
+}
+#endif /* CONFIG_X86_32 */
+
 static int parport_ECPPS2_supported(struct parport *pb)
 {
 	const struct parport_pc_private *priv = pb->private_data;
@@ -1722,8 +1762,6 @@ static int parport_ECPPS2_supported(struct parport *pb)
 
 static int parport_EPP_supported(struct parport *pb)
 {
-	const struct parport_pc_private *priv = pb->private_data;
-
 	/*
 	 * Theory:
 	 *	Bit 0 of STR is the EPP timeout bit, this bit is 0
@@ -1742,16 +1780,8 @@ static int parport_EPP_supported(struct parport *pb)
 		return 0;  /* No way to clear timeout */
 
 	/* Check for Intel bug. */
-	if (priv->ecr) {
-		unsigned char i;
-		for (i = 0x00; i < 0x80; i += 0x20) {
-			ECR_WRITE(pb, i);
-			if (clear_epp_timeout(pb)) {
-				/* Phony EPP in ECP. */
-				return 0;
-			}
-		}
-	}
+	if (intel_bug_present(pb))
+		return 0;
 
 	pb->modes |= PARPORT_MODE_EPP;
 
diff --git a/drivers/pci/host/Kconfig b/drivers/pci/host/Kconfig
index 2d8a4d05d78f..3dc25fad490c 100644
--- a/drivers/pci/host/Kconfig
+++ b/drivers/pci/host/Kconfig
@@ -1,9 +1,18 @@
 menu "PCI host controller drivers"
 	depends on PCI
 
+config PCI_DRA7XX
+	bool "TI DRA7xx PCIe controller"
+	select PCIE_DW
+	depends on OF && HAS_IOMEM && TI_PIPE3
+	help
+	 Enables support for the PCIe controller in the DRA7xx SoC.  There
+	 are two instances of PCIe controller in DRA7xx.  This controller can
+	 act both as EP and RC.  This reuses the Designware core.
+
 config PCI_MVEBU
 	bool "Marvell EBU PCIe controller"
-	depends on ARCH_MVEBU || ARCH_DOVE || ARCH_KIRKWOOD
+	depends on ARCH_MVEBU || ARCH_DOVE
 	depends on OF
 
 config PCIE_DW
@@ -47,11 +56,39 @@ config PCI_HOST_GENERIC
 	  controller, such as the one emulated by kvmtool.
 
 config PCIE_SPEAR13XX
-	tristate "STMicroelectronics SPEAr PCIe controller"
+	bool "STMicroelectronics SPEAr PCIe controller"
 	depends on ARCH_SPEAR13XX
 	select PCIEPORTBUS
 	select PCIE_DW
 	help
 	  Say Y here if you want PCIe support on SPEAr13XX SoCs.
 
+config PCI_KEYSTONE
+	bool "TI Keystone PCIe controller"
+	depends on ARCH_KEYSTONE
+	select PCIE_DW
+	select PCIEPORTBUS
+	help
+	  Say Y here if you want to enable PCI controller support on Keystone
+	  SoCs. The PCI controller on Keystone is based on Designware hardware
+	  and therefore the driver re-uses the Designware core functions to
+	  implement the driver.
+
+config PCIE_XILINX
+	bool "Xilinx AXI PCIe host bridge support"
+	depends on ARCH_ZYNQ
+	help
+	  Say 'Y' here if you want kernel to support the Xilinx AXI PCIe
+	  Host Bridge driver.
+
+config PCI_XGENE
+	bool "X-Gene PCIe controller"
+	depends on ARCH_XGENE
+	depends on OF
+	select PCIEPORTBUS
+	help
+	  Say Y here if you want internal PCI support on APM X-Gene SoC.
+	  There are 5 internal PCIe ports available. Each port is GEN3 capable
+	  and have varied lanes from x1 to x8.
+
 endmenu
diff --git a/drivers/pci/host/Makefile b/drivers/pci/host/Makefile
index 0daec7941aba..26b3461d68d7 100644
--- a/drivers/pci/host/Makefile
+++ b/drivers/pci/host/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_PCIE_DW) += pcie-designware.o
+obj-$(CONFIG_PCI_DRA7XX) += pci-dra7xx.o
 obj-$(CONFIG_PCI_EXYNOS) += pci-exynos.o
 obj-$(CONFIG_PCI_IMX6) += pci-imx6.o
 obj-$(CONFIG_PCI_MVEBU) += pci-mvebu.o
@@ -7,3 +8,6 @@ obj-$(CONFIG_PCI_RCAR_GEN2) += pci-rcar-gen2.o
 obj-$(CONFIG_PCI_RCAR_GEN2_PCIE) += pcie-rcar.o
 obj-$(CONFIG_PCI_HOST_GENERIC) += pci-host-generic.o
 obj-$(CONFIG_PCIE_SPEAR13XX) += pcie-spear13xx.o
+obj-$(CONFIG_PCI_KEYSTONE) += pci-keystone-dw.o pci-keystone.o
+obj-$(CONFIG_PCIE_XILINX) += pcie-xilinx.o
+obj-$(CONFIG_PCI_XGENE) += pci-xgene.o
diff --git a/drivers/pci/host/pci-dra7xx.c b/drivers/pci/host/pci-dra7xx.c
new file mode 100644
index 000000000000..52b34fee07fd
--- /dev/null
+++ b/drivers/pci/host/pci-dra7xx.c
@@ -0,0 +1,458 @@
+/*
+ * pcie-dra7xx - PCIe controller driver for TI DRA7xx SoCs
+ *
+ * Copyright (C) 2013-2014 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Authors: Kishon Vijay Abraham I <kishon@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/resource.h>
+#include <linux/types.h>
+
+#include "pcie-designware.h"
+
+/* PCIe controller wrapper DRA7XX configuration registers */
+
+#define	PCIECTRL_DRA7XX_CONF_IRQSTATUS_MAIN		0x0024
+#define	PCIECTRL_DRA7XX_CONF_IRQENABLE_SET_MAIN		0x0028
+#define	ERR_SYS						BIT(0)
+#define	ERR_FATAL					BIT(1)
+#define	ERR_NONFATAL					BIT(2)
+#define	ERR_COR						BIT(3)
+#define	ERR_AXI						BIT(4)
+#define	ERR_ECRC					BIT(5)
+#define	PME_TURN_OFF					BIT(8)
+#define	PME_TO_ACK					BIT(9)
+#define	PM_PME						BIT(10)
+#define	LINK_REQ_RST					BIT(11)
+#define	LINK_UP_EVT					BIT(12)
+#define	CFG_BME_EVT					BIT(13)
+#define	CFG_MSE_EVT					BIT(14)
+#define	INTERRUPTS (ERR_SYS | ERR_FATAL | ERR_NONFATAL | ERR_COR | ERR_AXI | \
+			ERR_ECRC | PME_TURN_OFF | PME_TO_ACK | PM_PME | \
+			LINK_REQ_RST | LINK_UP_EVT | CFG_BME_EVT | CFG_MSE_EVT)
+
+#define	PCIECTRL_DRA7XX_CONF_IRQSTATUS_MSI		0x0034
+#define	PCIECTRL_DRA7XX_CONF_IRQENABLE_SET_MSI		0x0038
+#define	INTA						BIT(0)
+#define	INTB						BIT(1)
+#define	INTC						BIT(2)
+#define	INTD						BIT(3)
+#define	MSI						BIT(4)
+#define	LEG_EP_INTERRUPTS (INTA | INTB | INTC | INTD)
+
+#define	PCIECTRL_DRA7XX_CONF_DEVICE_CMD			0x0104
+#define	LTSSM_EN					0x1
+
+#define	PCIECTRL_DRA7XX_CONF_PHY_CS			0x010C
+#define	LINK_UP						BIT(16)
+
+struct dra7xx_pcie {
+	void __iomem		*base;
+	struct phy		**phy;
+	int			phy_count;
+	struct device		*dev;
+	struct pcie_port	pp;
+};
+
+#define to_dra7xx_pcie(x)	container_of((x), struct dra7xx_pcie, pp)
+
+static inline u32 dra7xx_pcie_readl(struct dra7xx_pcie *pcie, u32 offset)
+{
+	return readl(pcie->base + offset);
+}
+
+static inline void dra7xx_pcie_writel(struct dra7xx_pcie *pcie, u32 offset,
+				      u32 value)
+{
+	writel(value, pcie->base + offset);
+}
+
+static int dra7xx_pcie_link_up(struct pcie_port *pp)
+{
+	struct dra7xx_pcie *dra7xx = to_dra7xx_pcie(pp);
+	u32 reg = dra7xx_pcie_readl(dra7xx, PCIECTRL_DRA7XX_CONF_PHY_CS);
+
+	return !!(reg & LINK_UP);
+}
+
+static int dra7xx_pcie_establish_link(struct pcie_port *pp)
+{
+	u32 reg;
+	unsigned int retries = 1000;
+	struct dra7xx_pcie *dra7xx = to_dra7xx_pcie(pp);
+
+	if (dw_pcie_link_up(pp)) {
+		dev_err(pp->dev, "link is already up\n");
+		return 0;
+	}
+
+	reg = dra7xx_pcie_readl(dra7xx, PCIECTRL_DRA7XX_CONF_DEVICE_CMD);
+	reg |= LTSSM_EN;
+	dra7xx_pcie_writel(dra7xx, PCIECTRL_DRA7XX_CONF_DEVICE_CMD, reg);
+
+	while (retries--) {
+		reg = dra7xx_pcie_readl(dra7xx,	PCIECTRL_DRA7XX_CONF_PHY_CS);
+		if (reg & LINK_UP)
+			break;
+		usleep_range(10, 20);
+	}
+
+	if (retries == 0) {
+		dev_err(pp->dev, "link is not up\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void dra7xx_pcie_enable_interrupts(struct pcie_port *pp)
+{
+	struct dra7xx_pcie *dra7xx = to_dra7xx_pcie(pp);
+
+	dra7xx_pcie_writel(dra7xx, PCIECTRL_DRA7XX_CONF_IRQSTATUS_MAIN,
+			   ~INTERRUPTS);
+	dra7xx_pcie_writel(dra7xx,
+			   PCIECTRL_DRA7XX_CONF_IRQENABLE_SET_MAIN, INTERRUPTS);
+	dra7xx_pcie_writel(dra7xx, PCIECTRL_DRA7XX_CONF_IRQSTATUS_MSI,
+			   ~LEG_EP_INTERRUPTS & ~MSI);
+
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		dra7xx_pcie_writel(dra7xx,
+				   PCIECTRL_DRA7XX_CONF_IRQENABLE_SET_MSI, MSI);
+	else
+		dra7xx_pcie_writel(dra7xx,
+				   PCIECTRL_DRA7XX_CONF_IRQENABLE_SET_MSI,
+				   LEG_EP_INTERRUPTS);
+}
+
+static void dra7xx_pcie_host_init(struct pcie_port *pp)
+{
+	dw_pcie_setup_rc(pp);
+	dra7xx_pcie_establish_link(pp);
+	if (IS_ENABLED(CONFIG_PCI_MSI))
+		dw_pcie_msi_init(pp);
+	dra7xx_pcie_enable_interrupts(pp);
+}
+
+static struct pcie_host_ops dra7xx_pcie_host_ops = {
+	.link_up = dra7xx_pcie_link_up,
+	.host_init = dra7xx_pcie_host_init,
+};
+
+static int dra7xx_pcie_intx_map(struct irq_domain *domain, unsigned int irq,
+				irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &dummy_irq_chip, handle_simple_irq);
+	irq_set_chip_data(irq, domain->host_data);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+static const struct irq_domain_ops intx_domain_ops = {
+	.map = dra7xx_pcie_intx_map,
+};
+
+static int dra7xx_pcie_init_irq_domain(struct pcie_port *pp)
+{
+	struct device *dev = pp->dev;
+	struct device_node *node = dev->of_node;
+	struct device_node *pcie_intc_node =  of_get_next_child(node, NULL);
+
+	if (!pcie_intc_node) {
+		dev_err(dev, "No PCIe Intc node found\n");
+		return PTR_ERR(pcie_intc_node);
+	}
+
+	pp->irq_domain = irq_domain_add_linear(pcie_intc_node, 4,
+					       &intx_domain_ops, pp);
+	if (!pp->irq_domain) {
+		dev_err(dev, "Failed to get a INTx IRQ domain\n");
+		return PTR_ERR(pp->irq_domain);
+	}
+
+	return 0;
+}
+
+static irqreturn_t dra7xx_pcie_msi_irq_handler(int irq, void *arg)
+{
+	struct pcie_port *pp = arg;
+	struct dra7xx_pcie *dra7xx = to_dra7xx_pcie(pp);
+	u32 reg;
+
+	reg = dra7xx_pcie_readl(dra7xx, PCIECTRL_DRA7XX_CONF_IRQSTATUS_MSI);
+
+	switch (reg) {
+	case MSI:
+		dw_handle_msi_irq(pp);
+		break;
+	case INTA:
+	case INTB:
+	case INTC:
+	case INTD:
+		generic_handle_irq(irq_find_mapping(pp->irq_domain, ffs(reg)));
+		break;
+	}
+
+	dra7xx_pcie_writel(dra7xx, PCIECTRL_DRA7XX_CONF_IRQSTATUS_MSI, reg);
+
+	return IRQ_HANDLED;
+}
+
+
+static irqreturn_t dra7xx_pcie_irq_handler(int irq, void *arg)
+{
+	struct dra7xx_pcie *dra7xx = arg;
+	u32 reg;
+
+	reg = dra7xx_pcie_readl(dra7xx, PCIECTRL_DRA7XX_CONF_IRQSTATUS_MAIN);
+
+	if (reg & ERR_SYS)
+		dev_dbg(dra7xx->dev, "System Error\n");
+
+	if (reg & ERR_FATAL)
+		dev_dbg(dra7xx->dev, "Fatal Error\n");
+
+	if (reg & ERR_NONFATAL)
+		dev_dbg(dra7xx->dev, "Non Fatal Error\n");
+
+	if (reg & ERR_COR)
+		dev_dbg(dra7xx->dev, "Correctable Error\n");
+
+	if (reg & ERR_AXI)
+		dev_dbg(dra7xx->dev, "AXI tag lookup fatal Error\n");
+
+	if (reg & ERR_ECRC)
+		dev_dbg(dra7xx->dev, "ECRC Error\n");
+
+	if (reg & PME_TURN_OFF)
+		dev_dbg(dra7xx->dev,
+			"Power Management Event Turn-Off message received\n");
+
+	if (reg & PME_TO_ACK)
+		dev_dbg(dra7xx->dev,
+			"Power Management Turn-Off Ack message received\n");
+
+	if (reg & PM_PME)
+		dev_dbg(dra7xx->dev,
+			"PM Power Management Event message received\n");
+
+	if (reg & LINK_REQ_RST)
+		dev_dbg(dra7xx->dev, "Link Request Reset\n");
+
+	if (reg & LINK_UP_EVT)
+		dev_dbg(dra7xx->dev, "Link-up state change\n");
+
+	if (reg & CFG_BME_EVT)
+		dev_dbg(dra7xx->dev, "CFG 'Bus Master Enable' change\n");
+
+	if (reg & CFG_MSE_EVT)
+		dev_dbg(dra7xx->dev, "CFG 'Memory Space Enable' change\n");
+
+	dra7xx_pcie_writel(dra7xx, PCIECTRL_DRA7XX_CONF_IRQSTATUS_MAIN, reg);
+
+	return IRQ_HANDLED;
+}
+
+static int add_pcie_port(struct dra7xx_pcie *dra7xx,
+			  struct platform_device *pdev)
+{
+	int ret;
+	struct pcie_port *pp;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+
+	pp = &dra7xx->pp;
+	pp->dev = dev;
+	pp->ops = &dra7xx_pcie_host_ops;
+
+	pp->irq = platform_get_irq(pdev, 1);
+	if (pp->irq < 0) {
+		dev_err(dev, "missing IRQ resource\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_irq(&pdev->dev, pp->irq,
+			       dra7xx_pcie_msi_irq_handler, IRQF_SHARED,
+			       "dra7-pcie-msi",	pp);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to request irq\n");
+		return ret;
+	}
+
+	if (!IS_ENABLED(CONFIG_PCI_MSI)) {
+		ret = dra7xx_pcie_init_irq_domain(pp);
+		if (ret < 0)
+			return ret;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "rc_dbics");
+	pp->dbi_base = devm_ioremap(dev, res->start, resource_size(res));
+	if (!pp->dbi_base)
+		return -ENOMEM;
+
+	ret = dw_pcie_host_init(pp);
+	if (ret) {
+		dev_err(dra7xx->dev, "failed to initialize host\n");
+		return ret;
+	}
+
+	return 0;
+}
+
+static int __init dra7xx_pcie_probe(struct platform_device *pdev)
+{
+	u32 reg;
+	int ret;
+	int irq;
+	int i;
+	int phy_count;
+	struct phy **phy;
+	void __iomem *base;
+	struct resource *res;
+	struct dra7xx_pcie *dra7xx;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	char name[10];
+
+	dra7xx = devm_kzalloc(dev, sizeof(*dra7xx), GFP_KERNEL);
+	if (!dra7xx)
+		return -ENOMEM;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(dev, "missing IRQ resource\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_irq(dev, irq, dra7xx_pcie_irq_handler,
+			       IRQF_SHARED, "dra7xx-pcie-main", dra7xx);
+	if (ret) {
+		dev_err(dev, "failed to request irq\n");
+		return ret;
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "ti_conf");
+	base = devm_ioremap_nocache(dev, res->start, resource_size(res));
+	if (!base)
+		return -ENOMEM;
+
+	phy_count = of_property_count_strings(np, "phy-names");
+	if (phy_count < 0) {
+		dev_err(dev, "unable to find the strings\n");
+		return phy_count;
+	}
+
+	phy = devm_kzalloc(dev, sizeof(*phy) * phy_count, GFP_KERNEL);
+	if (!phy)
+		return -ENOMEM;
+
+	for (i = 0; i < phy_count; i++) {
+		snprintf(name, sizeof(name), "pcie-phy%d", i);
+		phy[i] = devm_phy_get(dev, name);
+		if (IS_ERR(phy[i]))
+			return PTR_ERR(phy[i]);
+
+		ret = phy_init(phy[i]);
+		if (ret < 0)
+			goto err_phy;
+
+		ret = phy_power_on(phy[i]);
+		if (ret < 0) {
+			phy_exit(phy[i]);
+			goto err_phy;
+		}
+	}
+
+	dra7xx->base = base;
+	dra7xx->phy = phy;
+	dra7xx->dev = dev;
+	dra7xx->phy_count = phy_count;
+
+	pm_runtime_enable(dev);
+	ret = pm_runtime_get_sync(dev);
+	if (IS_ERR_VALUE(ret)) {
+		dev_err(dev, "pm_runtime_get_sync failed\n");
+		goto err_phy;
+	}
+
+	reg = dra7xx_pcie_readl(dra7xx, PCIECTRL_DRA7XX_CONF_DEVICE_CMD);
+	reg &= ~LTSSM_EN;
+	dra7xx_pcie_writel(dra7xx, PCIECTRL_DRA7XX_CONF_DEVICE_CMD, reg);
+
+	platform_set_drvdata(pdev, dra7xx);
+
+	ret = add_pcie_port(dra7xx, pdev);
+	if (ret < 0)
+		goto err_add_port;
+
+	return 0;
+
+err_add_port:
+	pm_runtime_put(dev);
+	pm_runtime_disable(dev);
+
+err_phy:
+	while (--i >= 0) {
+		phy_power_off(phy[i]);
+		phy_exit(phy[i]);
+	}
+
+	return ret;
+}
+
+static int __exit dra7xx_pcie_remove(struct platform_device *pdev)
+{
+	struct dra7xx_pcie *dra7xx = platform_get_drvdata(pdev);
+	struct pcie_port *pp = &dra7xx->pp;
+	struct device *dev = &pdev->dev;
+	int count = dra7xx->phy_count;
+
+	if (pp->irq_domain)
+		irq_domain_remove(pp->irq_domain);
+	pm_runtime_put(dev);
+	pm_runtime_disable(dev);
+	while (count--) {
+		phy_power_off(dra7xx->phy[count]);
+		phy_exit(dra7xx->phy[count]);
+	}
+
+	return 0;
+}
+
+static const struct of_device_id of_dra7xx_pcie_match[] = {
+	{ .compatible = "ti,dra7-pcie", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_dra7xx_pcie_match);
+
+static struct platform_driver dra7xx_pcie_driver = {
+	.remove		= __exit_p(dra7xx_pcie_remove),
+	.driver = {
+		.name	= "dra7-pcie",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_dra7xx_pcie_match,
+	},
+};
+
+module_platform_driver_probe(dra7xx_pcie_driver, dra7xx_pcie_probe);
+
+MODULE_AUTHOR("Kishon Vijay Abraham I <kishon@ti.com>");
+MODULE_DESCRIPTION("TI PCIe controller driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pci-imx6.c b/drivers/pci/host/pci-imx6.c
index a568efaa331c..233fe8a88264 100644
--- a/drivers/pci/host/pci-imx6.c
+++ b/drivers/pci/host/pci-imx6.c
@@ -49,6 +49,9 @@ struct imx6_pcie {
 
 /* PCIe Port Logic registers (memory-mapped) */
 #define PL_OFFSET 0x700
+#define PCIE_PL_PFLR (PL_OFFSET + 0x08)
+#define PCIE_PL_PFLR_LINK_STATE_MASK		(0x3f << 16)
+#define PCIE_PL_PFLR_FORCE_LINK			(1 << 15)
 #define PCIE_PHY_DEBUG_R0 (PL_OFFSET + 0x28)
 #define PCIE_PHY_DEBUG_R1 (PL_OFFSET + 0x2c)
 #define PCIE_PHY_DEBUG_R1_XMLH_LINK_IN_TRAINING	(1 << 29)
@@ -214,6 +217,32 @@ static int imx6q_pcie_abort_handler(unsigned long addr,
 static int imx6_pcie_assert_core_reset(struct pcie_port *pp)
 {
 	struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
+	u32 val, gpr1, gpr12;
+
+	/*
+	 * If the bootloader already enabled the link we need some special
+	 * handling to get the core back into a state where it is safe to
+	 * touch it for configuration.  As there is no dedicated reset signal
+	 * wired up for MX6QDL, we need to manually force LTSSM into "detect"
+	 * state before completely disabling LTSSM, which is a prerequisite
+	 * for core configuration.
+	 *
+	 * If both LTSSM_ENABLE and REF_SSP_ENABLE are active we have a strong
+	 * indication that the bootloader activated the link.
+	 */
+	regmap_read(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1, &gpr1);
+	regmap_read(imx6_pcie->iomuxc_gpr, IOMUXC_GPR12, &gpr12);
+
+	if ((gpr1 & IMX6Q_GPR1_PCIE_REF_CLK_EN) &&
+	    (gpr12 & IMX6Q_GPR12_PCIE_CTL_2)) {
+		val = readl(pp->dbi_base + PCIE_PL_PFLR);
+		val &= ~PCIE_PL_PFLR_LINK_STATE_MASK;
+		val |= PCIE_PL_PFLR_FORCE_LINK;
+		writel(val, pp->dbi_base + PCIE_PL_PFLR);
+
+		regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR12,
+				IMX6Q_GPR12_PCIE_CTL_2, 0 << 10);
+	}
 
 	regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1,
 			IMX6Q_GPR1_PCIE_TEST_PD, 1 << 18);
@@ -228,11 +257,6 @@ static int imx6_pcie_deassert_core_reset(struct pcie_port *pp)
 	struct imx6_pcie *imx6_pcie = to_imx6_pcie(pp);
 	int ret;
 
-	regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1,
-			IMX6Q_GPR1_PCIE_TEST_PD, 0 << 18);
-	regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1,
-			IMX6Q_GPR1_PCIE_REF_CLK_EN, 1 << 16);
-
 	ret = clk_prepare_enable(imx6_pcie->pcie_phy);
 	if (ret) {
 		dev_err(pp->dev, "unable to enable pcie_phy clock\n");
@@ -254,6 +278,12 @@ static int imx6_pcie_deassert_core_reset(struct pcie_port *pp)
 	/* allow the clocks to stabilize */
 	usleep_range(200, 500);
 
+	/* power up core phy and enable ref clock */
+	regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1,
+			IMX6Q_GPR1_PCIE_TEST_PD, 0 << 18);
+	regmap_update_bits(imx6_pcie->iomuxc_gpr, IOMUXC_GPR1,
+			IMX6Q_GPR1_PCIE_REF_CLK_EN, 1 << 16);
+
 	/* Some boards don't have PCIe reset GPIO. */
 	if (gpio_is_valid(imx6_pcie->reset_gpio)) {
 		gpio_set_value(imx6_pcie->reset_gpio, 0);
@@ -589,6 +619,14 @@ static int __init imx6_pcie_probe(struct platform_device *pdev)
 	return 0;
 }
 
+static void imx6_pcie_shutdown(struct platform_device *pdev)
+{
+	struct imx6_pcie *imx6_pcie = platform_get_drvdata(pdev);
+
+	/* bring down link, so bootloader gets clean state in case of reboot */
+	imx6_pcie_assert_core_reset(&imx6_pcie->pp);
+}
+
 static const struct of_device_id imx6_pcie_of_match[] = {
 	{ .compatible = "fsl,imx6q-pcie", },
 	{},
@@ -601,6 +639,7 @@ static struct platform_driver imx6_pcie_driver = {
 		.owner	= THIS_MODULE,
 		.of_match_table = imx6_pcie_of_match,
 	},
+	.shutdown = imx6_pcie_shutdown,
 };
 
 /* Freescale PCIe driver does not allow module unload */
@@ -609,7 +648,7 @@ static int __init imx6_pcie_init(void)
 {
 	return platform_driver_probe(&imx6_pcie_driver, imx6_pcie_probe);
 }
-fs_initcall(imx6_pcie_init);
+module_init(imx6_pcie_init);
 
 MODULE_AUTHOR("Sean Cross <xobs@kosagi.com>");
 MODULE_DESCRIPTION("Freescale i.MX6 PCIe host controller driver");
diff --git a/drivers/pci/host/pci-keystone-dw.c b/drivers/pci/host/pci-keystone-dw.c
new file mode 100644
index 000000000000..34086ce88e8e
--- /dev/null
+++ b/drivers/pci/host/pci-keystone-dw.c
@@ -0,0 +1,516 @@
+/*
+ * Designware application register space functions for Keystone PCI controller
+ *
+ * Copyright (C) 2013-2014 Texas Instruments., Ltd.
+ *		http://www.ti.com
+ *
+ * Author: Murali Karicheri <m-karicheri2@ti.com>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_pci.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+
+#include "pcie-designware.h"
+#include "pci-keystone.h"
+
+/* Application register defines */
+#define LTSSM_EN_VAL		        1
+#define LTSSM_STATE_MASK		0x1f
+#define LTSSM_STATE_L0			0x11
+#define DBI_CS2_EN_VAL			0x20
+#define OB_XLAT_EN_VAL		        2
+
+/* Application registers */
+#define CMD_STATUS			0x004
+#define CFG_SETUP			0x008
+#define OB_SIZE				0x030
+#define CFG_PCIM_WIN_SZ_IDX		3
+#define CFG_PCIM_WIN_CNT		32
+#define SPACE0_REMOTE_CFG_OFFSET	0x1000
+#define OB_OFFSET_INDEX(n)		(0x200 + (8 * n))
+#define OB_OFFSET_HI(n)			(0x204 + (8 * n))
+
+/* IRQ register defines */
+#define IRQ_EOI				0x050
+#define IRQ_STATUS			0x184
+#define IRQ_ENABLE_SET			0x188
+#define IRQ_ENABLE_CLR			0x18c
+
+#define MSI_IRQ				0x054
+#define MSI0_IRQ_STATUS			0x104
+#define MSI0_IRQ_ENABLE_SET		0x108
+#define MSI0_IRQ_ENABLE_CLR		0x10c
+#define IRQ_STATUS			0x184
+#define MSI_IRQ_OFFSET			4
+
+/* Config space registers */
+#define DEBUG0				0x728
+
+#define to_keystone_pcie(x)	container_of(x, struct keystone_pcie, pp)
+
+static inline struct pcie_port *sys_to_pcie(struct pci_sys_data *sys)
+{
+	return sys->private_data;
+}
+
+static inline void update_reg_offset_bit_pos(u32 offset, u32 *reg_offset,
+					     u32 *bit_pos)
+{
+	*reg_offset = offset % 8;
+	*bit_pos = offset >> 3;
+}
+
+u32 ks_dw_pcie_get_msi_addr(struct pcie_port *pp)
+{
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+
+	return ks_pcie->app.start + MSI_IRQ;
+}
+
+void ks_dw_pcie_handle_msi_irq(struct keystone_pcie *ks_pcie, int offset)
+{
+	struct pcie_port *pp = &ks_pcie->pp;
+	u32 pending, vector;
+	int src, virq;
+
+	pending = readl(ks_pcie->va_app_base + MSI0_IRQ_STATUS + (offset << 4));
+
+	/*
+	 * MSI0 status bit 0-3 shows vectors 0, 8, 16, 24, MSI1 status bit
+	 * shows 1, 9, 17, 25 and so forth
+	 */
+	for (src = 0; src < 4; src++) {
+		if (BIT(src) & pending) {
+			vector = offset + (src << 3);
+			virq = irq_linear_revmap(pp->irq_domain, vector);
+			dev_dbg(pp->dev, "irq: bit %d, vector %d, virq %d\n",
+				src, vector, virq);
+			generic_handle_irq(virq);
+		}
+	}
+}
+
+static void ks_dw_pcie_msi_irq_ack(struct irq_data *d)
+{
+	u32 offset, reg_offset, bit_pos;
+	struct keystone_pcie *ks_pcie;
+	unsigned int irq = d->irq;
+	struct msi_desc *msi;
+	struct pcie_port *pp;
+
+	msi = irq_get_msi_desc(irq);
+	pp = sys_to_pcie(msi->dev->bus->sysdata);
+	ks_pcie = to_keystone_pcie(pp);
+	offset = irq - irq_linear_revmap(pp->irq_domain, 0);
+	update_reg_offset_bit_pos(offset, &reg_offset, &bit_pos);
+
+	writel(BIT(bit_pos),
+	       ks_pcie->va_app_base + MSI0_IRQ_STATUS + (reg_offset << 4));
+	writel(reg_offset + MSI_IRQ_OFFSET, ks_pcie->va_app_base + IRQ_EOI);
+}
+
+void ks_dw_pcie_msi_set_irq(struct pcie_port *pp, int irq)
+{
+	u32 reg_offset, bit_pos;
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+
+	update_reg_offset_bit_pos(irq, &reg_offset, &bit_pos);
+	writel(BIT(bit_pos),
+	       ks_pcie->va_app_base + MSI0_IRQ_ENABLE_SET + (reg_offset << 4));
+}
+
+void ks_dw_pcie_msi_clear_irq(struct pcie_port *pp, int irq)
+{
+	u32 reg_offset, bit_pos;
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+
+	update_reg_offset_bit_pos(irq, &reg_offset, &bit_pos);
+	writel(BIT(bit_pos),
+	       ks_pcie->va_app_base + MSI0_IRQ_ENABLE_CLR + (reg_offset << 4));
+}
+
+static void ks_dw_pcie_msi_irq_mask(struct irq_data *d)
+{
+	struct keystone_pcie *ks_pcie;
+	unsigned int irq = d->irq;
+	struct msi_desc *msi;
+	struct pcie_port *pp;
+	u32 offset;
+
+	msi = irq_get_msi_desc(irq);
+	pp = sys_to_pcie(msi->dev->bus->sysdata);
+	ks_pcie = to_keystone_pcie(pp);
+	offset = irq - irq_linear_revmap(pp->irq_domain, 0);
+
+	/* Mask the end point if PVM implemented */
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		if (msi->msi_attrib.maskbit)
+			mask_msi_irq(d);
+	}
+
+	ks_dw_pcie_msi_clear_irq(pp, offset);
+}
+
+static void ks_dw_pcie_msi_irq_unmask(struct irq_data *d)
+{
+	struct keystone_pcie *ks_pcie;
+	unsigned int irq = d->irq;
+	struct msi_desc *msi;
+	struct pcie_port *pp;
+	u32 offset;
+
+	msi = irq_get_msi_desc(irq);
+	pp = sys_to_pcie(msi->dev->bus->sysdata);
+	ks_pcie = to_keystone_pcie(pp);
+	offset = irq - irq_linear_revmap(pp->irq_domain, 0);
+
+	/* Mask the end point if PVM implemented */
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		if (msi->msi_attrib.maskbit)
+			unmask_msi_irq(d);
+	}
+
+	ks_dw_pcie_msi_set_irq(pp, offset);
+}
+
+static struct irq_chip ks_dw_pcie_msi_irq_chip = {
+	.name = "Keystone-PCIe-MSI-IRQ",
+	.irq_ack = ks_dw_pcie_msi_irq_ack,
+	.irq_mask = ks_dw_pcie_msi_irq_mask,
+	.irq_unmask = ks_dw_pcie_msi_irq_unmask,
+};
+
+static int ks_dw_pcie_msi_map(struct irq_domain *domain, unsigned int irq,
+			      irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &ks_dw_pcie_msi_irq_chip,
+				 handle_level_irq);
+	irq_set_chip_data(irq, domain->host_data);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+const struct irq_domain_ops ks_dw_pcie_msi_domain_ops = {
+	.map = ks_dw_pcie_msi_map,
+};
+
+int ks_dw_pcie_msi_host_init(struct pcie_port *pp, struct msi_chip *chip)
+{
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+	int i;
+
+	pp->irq_domain = irq_domain_add_linear(ks_pcie->msi_intc_np,
+					MAX_MSI_IRQS,
+					&ks_dw_pcie_msi_domain_ops,
+					chip);
+	if (!pp->irq_domain) {
+		dev_err(pp->dev, "irq domain init failed\n");
+		return -ENXIO;
+	}
+
+	for (i = 0; i < MAX_MSI_IRQS; i++)
+		irq_create_mapping(pp->irq_domain, i);
+
+	return 0;
+}
+
+void ks_dw_pcie_enable_legacy_irqs(struct keystone_pcie *ks_pcie)
+{
+	int i;
+
+	for (i = 0; i < MAX_LEGACY_IRQS; i++)
+		writel(0x1, ks_pcie->va_app_base + IRQ_ENABLE_SET + (i << 4));
+}
+
+void ks_dw_pcie_handle_legacy_irq(struct keystone_pcie *ks_pcie, int offset)
+{
+	struct pcie_port *pp = &ks_pcie->pp;
+	u32 pending;
+	int virq;
+
+	pending = readl(ks_pcie->va_app_base + IRQ_STATUS + (offset << 4));
+
+	if (BIT(0) & pending) {
+		virq = irq_linear_revmap(ks_pcie->legacy_irq_domain, offset);
+		dev_dbg(pp->dev, ": irq: irq_offset %d, virq %d\n", offset,
+			virq);
+		generic_handle_irq(virq);
+	}
+
+	/* EOI the INTx interrupt */
+	writel(offset, ks_pcie->va_app_base + IRQ_EOI);
+}
+
+static void ks_dw_pcie_ack_legacy_irq(struct irq_data *d)
+{
+}
+
+static void ks_dw_pcie_mask_legacy_irq(struct irq_data *d)
+{
+}
+
+static void ks_dw_pcie_unmask_legacy_irq(struct irq_data *d)
+{
+}
+
+static struct irq_chip ks_dw_pcie_legacy_irq_chip = {
+	.name = "Keystone-PCI-Legacy-IRQ",
+	.irq_ack = ks_dw_pcie_ack_legacy_irq,
+	.irq_mask = ks_dw_pcie_mask_legacy_irq,
+	.irq_unmask = ks_dw_pcie_unmask_legacy_irq,
+};
+
+static int ks_dw_pcie_init_legacy_irq_map(struct irq_domain *d,
+				unsigned int irq, irq_hw_number_t hw_irq)
+{
+	irq_set_chip_and_handler(irq, &ks_dw_pcie_legacy_irq_chip,
+				 handle_level_irq);
+	irq_set_chip_data(irq, d->host_data);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+static const struct irq_domain_ops ks_dw_pcie_legacy_irq_domain_ops = {
+	.map = ks_dw_pcie_init_legacy_irq_map,
+	.xlate = irq_domain_xlate_onetwocell,
+};
+
+/**
+ * ks_dw_pcie_set_dbi_mode() - Set DBI mode to access overlaid BAR mask
+ * registers
+ *
+ * Since modification of dbi_cs2 involves different clock domain, read the
+ * status back to ensure the transition is complete.
+ */
+static void ks_dw_pcie_set_dbi_mode(void __iomem *reg_virt)
+{
+	u32 val;
+
+	writel(DBI_CS2_EN_VAL | readl(reg_virt + CMD_STATUS),
+	       reg_virt + CMD_STATUS);
+
+	do {
+		val = readl(reg_virt + CMD_STATUS);
+	} while (!(val & DBI_CS2_EN_VAL));
+}
+
+/**
+ * ks_dw_pcie_clear_dbi_mode() - Disable DBI mode
+ *
+ * Since modification of dbi_cs2 involves different clock domain, read the
+ * status back to ensure the transition is complete.
+ */
+static void ks_dw_pcie_clear_dbi_mode(void __iomem *reg_virt)
+{
+	u32 val;
+
+	writel(~DBI_CS2_EN_VAL & readl(reg_virt + CMD_STATUS),
+		     reg_virt + CMD_STATUS);
+
+	do {
+		val = readl(reg_virt + CMD_STATUS);
+	} while (val & DBI_CS2_EN_VAL);
+}
+
+void ks_dw_pcie_setup_rc_app_regs(struct keystone_pcie *ks_pcie)
+{
+	struct pcie_port *pp = &ks_pcie->pp;
+	u32 start = pp->mem.start, end = pp->mem.end;
+	int i, tr_size;
+
+	/* Disable BARs for inbound access */
+	ks_dw_pcie_set_dbi_mode(ks_pcie->va_app_base);
+	writel(0, pp->dbi_base + PCI_BASE_ADDRESS_0);
+	writel(0, pp->dbi_base + PCI_BASE_ADDRESS_1);
+	ks_dw_pcie_clear_dbi_mode(ks_pcie->va_app_base);
+
+	/* Set outbound translation size per window division */
+	writel(CFG_PCIM_WIN_SZ_IDX & 0x7, ks_pcie->va_app_base + OB_SIZE);
+
+	tr_size = (1 << (CFG_PCIM_WIN_SZ_IDX & 0x7)) * SZ_1M;
+
+	/* Using Direct 1:1 mapping of RC <-> PCI memory space */
+	for (i = 0; (i < CFG_PCIM_WIN_CNT) && (start < end); i++) {
+		writel(start | 1, ks_pcie->va_app_base + OB_OFFSET_INDEX(i));
+		writel(0, ks_pcie->va_app_base + OB_OFFSET_HI(i));
+		start += tr_size;
+	}
+
+	/* Enable OB translation */
+	writel(OB_XLAT_EN_VAL | readl(ks_pcie->va_app_base + CMD_STATUS),
+	       ks_pcie->va_app_base + CMD_STATUS);
+}
+
+/**
+ * ks_pcie_cfg_setup() - Set up configuration space address for a device
+ *
+ * @ks_pcie: ptr to keystone_pcie structure
+ * @bus: Bus number the device is residing on
+ * @devfn: device, function number info
+ *
+ * Forms and returns the address of configuration space mapped in PCIESS
+ * address space 0.  Also configures CFG_SETUP for remote configuration space
+ * access.
+ *
+ * The address space has two regions to access configuration - local and remote.
+ * We access local region for bus 0 (as RC is attached on bus 0) and remote
+ * region for others with TYPE 1 access when bus > 1.  As for device on bus = 1,
+ * we will do TYPE 0 access as it will be on our secondary bus (logical).
+ * CFG_SETUP is needed only for remote configuration access.
+ */
+static void __iomem *ks_pcie_cfg_setup(struct keystone_pcie *ks_pcie, u8 bus,
+				       unsigned int devfn)
+{
+	u8 device = PCI_SLOT(devfn), function = PCI_FUNC(devfn);
+	struct pcie_port *pp = &ks_pcie->pp;
+	u32 regval;
+
+	if (bus == 0)
+		return pp->dbi_base;
+
+	regval = (bus << 16) | (device << 8) | function;
+
+	/*
+	 * Since Bus#1 will be a virtual bus, we need to have TYPE0
+	 * access only.
+	 * TYPE 1
+	 */
+	if (bus != 1)
+		regval |= BIT(24);
+
+	writel(regval, ks_pcie->va_app_base + CFG_SETUP);
+	return pp->va_cfg0_base;
+}
+
+int ks_dw_pcie_rd_other_conf(struct pcie_port *pp, struct pci_bus *bus,
+			     unsigned int devfn, int where, int size, u32 *val)
+{
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+	u8 bus_num = bus->number;
+	void __iomem *addr;
+
+	addr = ks_pcie_cfg_setup(ks_pcie, bus_num, devfn);
+
+	return dw_pcie_cfg_read(addr + (where & ~0x3), where, size, val);
+}
+
+int ks_dw_pcie_wr_other_conf(struct pcie_port *pp, struct pci_bus *bus,
+			     unsigned int devfn, int where, int size, u32 val)
+{
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+	u8 bus_num = bus->number;
+	void __iomem *addr;
+
+	addr = ks_pcie_cfg_setup(ks_pcie, bus_num, devfn);
+
+	return dw_pcie_cfg_write(addr + (where & ~0x3), where, size, val);
+}
+
+/**
+ * ks_dw_pcie_v3_65_scan_bus() - keystone scan_bus post initialization
+ *
+ * This sets BAR0 to enable inbound access for MSI_IRQ register
+ */
+void ks_dw_pcie_v3_65_scan_bus(struct pcie_port *pp)
+{
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+
+	/* Configure and set up BAR0 */
+	ks_dw_pcie_set_dbi_mode(ks_pcie->va_app_base);
+
+	/* Enable BAR0 */
+	writel(1, pp->dbi_base + PCI_BASE_ADDRESS_0);
+	writel(SZ_4K - 1, pp->dbi_base + PCI_BASE_ADDRESS_0);
+
+	ks_dw_pcie_clear_dbi_mode(ks_pcie->va_app_base);
+
+	 /*
+	  * For BAR0, just setting bus address for inbound writes (MSI) should
+	  * be sufficient.  Use physical address to avoid any conflicts.
+	  */
+	writel(ks_pcie->app.start, pp->dbi_base + PCI_BASE_ADDRESS_0);
+}
+
+/**
+ * ks_dw_pcie_link_up() - Check if link up
+ */
+int ks_dw_pcie_link_up(struct pcie_port *pp)
+{
+	u32 val = readl(pp->dbi_base + DEBUG0);
+
+	return (val & LTSSM_STATE_MASK) == LTSSM_STATE_L0;
+}
+
+void ks_dw_pcie_initiate_link_train(struct keystone_pcie *ks_pcie)
+{
+	u32 val;
+
+	/* Disable Link training */
+	val = readl(ks_pcie->va_app_base + CMD_STATUS);
+	val &= ~LTSSM_EN_VAL;
+	writel(LTSSM_EN_VAL | val,  ks_pcie->va_app_base + CMD_STATUS);
+
+	/* Initiate Link Training */
+	val = readl(ks_pcie->va_app_base + CMD_STATUS);
+	writel(LTSSM_EN_VAL | val,  ks_pcie->va_app_base + CMD_STATUS);
+}
+
+/**
+ * ks_dw_pcie_host_init() - initialize host for v3_65 dw hardware
+ *
+ * Ioremap the register resources, initialize legacy irq domain
+ * and call dw_pcie_v3_65_host_init() API to initialize the Keystone
+ * PCI host controller.
+ */
+int __init ks_dw_pcie_host_init(struct keystone_pcie *ks_pcie,
+				struct device_node *msi_intc_np)
+{
+	struct pcie_port *pp = &ks_pcie->pp;
+	struct platform_device *pdev = to_platform_device(pp->dev);
+	struct resource *res;
+
+	/* Index 0 is the config reg. space address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	pp->dbi_base = devm_ioremap_resource(pp->dev, res);
+	if (IS_ERR(pp->dbi_base))
+		return PTR_ERR(pp->dbi_base);
+
+	/*
+	 * We set these same and is used in pcie rd/wr_other_conf
+	 * functions
+	 */
+	pp->va_cfg0_base = pp->dbi_base + SPACE0_REMOTE_CFG_OFFSET;
+	pp->va_cfg1_base = pp->va_cfg0_base;
+
+	/* Index 1 is the application reg. space address */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	ks_pcie->app = *res;
+	ks_pcie->va_app_base = devm_ioremap_resource(pp->dev, res);
+	if (IS_ERR(ks_pcie->va_app_base))
+		return PTR_ERR(ks_pcie->va_app_base);
+
+	/* Create legacy IRQ domain */
+	ks_pcie->legacy_irq_domain =
+			irq_domain_add_linear(ks_pcie->legacy_intc_np,
+					MAX_LEGACY_IRQS,
+					&ks_dw_pcie_legacy_irq_domain_ops,
+					NULL);
+	if (!ks_pcie->legacy_irq_domain) {
+		dev_err(pp->dev, "Failed to add irq domain for legacy irqs\n");
+		return -EINVAL;
+	}
+
+	return dw_pcie_host_init(pp);
+}
diff --git a/drivers/pci/host/pci-keystone.c b/drivers/pci/host/pci-keystone.c
new file mode 100644
index 000000000000..1b893bc8b842
--- /dev/null
+++ b/drivers/pci/host/pci-keystone.c
@@ -0,0 +1,415 @@
+/*
+ * PCIe host controller driver for Texas Instruments Keystone SoCs
+ *
+ * Copyright (C) 2013-2014 Texas Instruments., Ltd.
+ *		http://www.ti.com
+ *
+ * Author: Murali Karicheri <m-karicheri2@ti.com>
+ * Implementation based on pci-exynos.c and pcie-designware.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/irqchip/chained_irq.h>
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/irqdomain.h>
+#include <linux/module.h>
+#include <linux/msi.h>
+#include <linux/of_irq.h>
+#include <linux/of.h>
+#include <linux/of_pci.h>
+#include <linux/platform_device.h>
+#include <linux/phy/phy.h>
+#include <linux/resource.h>
+#include <linux/signal.h>
+
+#include "pcie-designware.h"
+#include "pci-keystone.h"
+
+#define DRIVER_NAME	"keystone-pcie"
+
+/* driver specific constants */
+#define MAX_MSI_HOST_IRQS		8
+#define MAX_LEGACY_HOST_IRQS		4
+
+/* DEV_STAT_CTRL */
+#define PCIE_CAP_BASE		0x70
+
+/* PCIE controller device IDs */
+#define PCIE_RC_K2HK		0xb008
+#define PCIE_RC_K2E		0xb009
+#define PCIE_RC_K2L		0xb00a
+
+#define to_keystone_pcie(x)	container_of(x, struct keystone_pcie, pp)
+
+static void quirk_limit_mrrs(struct pci_dev *dev)
+{
+	struct pci_bus *bus = dev->bus;
+	struct pci_dev *bridge = bus->self;
+	static const struct pci_device_id rc_pci_devids[] = {
+		{ PCI_DEVICE(PCI_VENDOR_ID_TI, PCIE_RC_K2HK),
+		 .class = PCI_CLASS_BRIDGE_PCI << 8, .class_mask = ~0, },
+		{ PCI_DEVICE(PCI_VENDOR_ID_TI, PCIE_RC_K2E),
+		 .class = PCI_CLASS_BRIDGE_PCI << 8, .class_mask = ~0, },
+		{ PCI_DEVICE(PCI_VENDOR_ID_TI, PCIE_RC_K2L),
+		 .class = PCI_CLASS_BRIDGE_PCI << 8, .class_mask = ~0, },
+		{ 0, },
+	};
+
+	if (pci_is_root_bus(bus))
+		return;
+
+	/* look for the host bridge */
+	while (!pci_is_root_bus(bus)) {
+		bridge = bus->self;
+		bus = bus->parent;
+	}
+
+	if (bridge) {
+		/*
+		 * Keystone PCI controller has a h/w limitation of
+		 * 256 bytes maximum read request size.  It can't handle
+		 * anything higher than this.  So force this limit on
+		 * all downstream devices.
+		 */
+		if (pci_match_id(rc_pci_devids, bridge)) {
+			if (pcie_get_readrq(dev) > 256) {
+				dev_info(&dev->dev, "limiting MRRS to 256\n");
+				pcie_set_readrq(dev, 256);
+			}
+		}
+	}
+}
+DECLARE_PCI_FIXUP_ENABLE(PCI_ANY_ID, PCI_ANY_ID, quirk_limit_mrrs);
+
+static int ks_pcie_establish_link(struct keystone_pcie *ks_pcie)
+{
+	struct pcie_port *pp = &ks_pcie->pp;
+	int count = 200;
+
+	dw_pcie_setup_rc(pp);
+
+	if (dw_pcie_link_up(pp)) {
+		dev_err(pp->dev, "Link already up\n");
+		return 0;
+	}
+
+	ks_dw_pcie_initiate_link_train(ks_pcie);
+	/* check if the link is up or not */
+	while (!dw_pcie_link_up(pp)) {
+		usleep_range(100, 1000);
+		if (--count) {
+			ks_dw_pcie_initiate_link_train(ks_pcie);
+			continue;
+		}
+		dev_err(pp->dev, "phy link never came up\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void ks_pcie_msi_irq_handler(unsigned int irq, struct irq_desc *desc)
+{
+	struct keystone_pcie *ks_pcie = irq_desc_get_handler_data(desc);
+	u32 offset = irq - ks_pcie->msi_host_irqs[0];
+	struct pcie_port *pp = &ks_pcie->pp;
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+
+	dev_dbg(pp->dev, "ks_pci_msi_irq_handler, irq %d\n", irq);
+
+	/*
+	 * The chained irq handler installation would have replaced normal
+	 * interrupt driver handler so we need to take care of mask/unmask and
+	 * ack operation.
+	 */
+	chained_irq_enter(chip, desc);
+	ks_dw_pcie_handle_msi_irq(ks_pcie, offset);
+	chained_irq_exit(chip, desc);
+}
+
+/**
+ * ks_pcie_legacy_irq_handler() - Handle legacy interrupt
+ * @irq: IRQ line for legacy interrupts
+ * @desc: Pointer to irq descriptor
+ *
+ * Traverse through pending legacy interrupts and invoke handler for each. Also
+ * takes care of interrupt controller level mask/ack operation.
+ */
+static void ks_pcie_legacy_irq_handler(unsigned int irq, struct irq_desc *desc)
+{
+	struct keystone_pcie *ks_pcie = irq_desc_get_handler_data(desc);
+	struct pcie_port *pp = &ks_pcie->pp;
+	u32 irq_offset = irq - ks_pcie->legacy_host_irqs[0];
+	struct irq_chip *chip = irq_desc_get_chip(desc);
+
+	dev_dbg(pp->dev, ": Handling legacy irq %d\n", irq);
+
+	/*
+	 * The chained irq handler installation would have replaced normal
+	 * interrupt driver handler so we need to take care of mask/unmask and
+	 * ack operation.
+	 */
+	chained_irq_enter(chip, desc);
+	ks_dw_pcie_handle_legacy_irq(ks_pcie, irq_offset);
+	chained_irq_exit(chip, desc);
+}
+
+static int ks_pcie_get_irq_controller_info(struct keystone_pcie *ks_pcie,
+					   char *controller, int *num_irqs)
+{
+	int temp, max_host_irqs, legacy = 1, *host_irqs, ret = -EINVAL;
+	struct device *dev = ks_pcie->pp.dev;
+	struct device_node *np_pcie = dev->of_node, **np_temp;
+
+	if (!strcmp(controller, "msi-interrupt-controller"))
+		legacy = 0;
+
+	if (legacy) {
+		np_temp = &ks_pcie->legacy_intc_np;
+		max_host_irqs = MAX_LEGACY_HOST_IRQS;
+		host_irqs = &ks_pcie->legacy_host_irqs[0];
+	} else {
+		np_temp = &ks_pcie->msi_intc_np;
+		max_host_irqs = MAX_MSI_HOST_IRQS;
+		host_irqs =  &ks_pcie->msi_host_irqs[0];
+	}
+
+	/* interrupt controller is in a child node */
+	*np_temp = of_find_node_by_name(np_pcie, controller);
+	if (!(*np_temp)) {
+		dev_err(dev, "Node for %s is absent\n", controller);
+		goto out;
+	}
+	temp = of_irq_count(*np_temp);
+	if (!temp)
+		goto out;
+	if (temp > max_host_irqs)
+		dev_warn(dev, "Too many %s interrupts defined %u\n",
+			(legacy ? "legacy" : "MSI"), temp);
+
+	/*
+	 * support upto max_host_irqs. In dt from index 0 to 3 (legacy) or 0 to
+	 * 7 (MSI)
+	 */
+	for (temp = 0; temp < max_host_irqs; temp++) {
+		host_irqs[temp] = irq_of_parse_and_map(*np_temp, temp);
+		if (host_irqs[temp] < 0)
+			break;
+	}
+	if (temp) {
+		*num_irqs = temp;
+		ret = 0;
+	}
+out:
+	return ret;
+}
+
+static void ks_pcie_setup_interrupts(struct keystone_pcie *ks_pcie)
+{
+	int i;
+
+	/* Legacy IRQ */
+	for (i = 0; i < ks_pcie->num_legacy_host_irqs; i++) {
+		irq_set_handler_data(ks_pcie->legacy_host_irqs[i], ks_pcie);
+		irq_set_chained_handler(ks_pcie->legacy_host_irqs[i],
+					ks_pcie_legacy_irq_handler);
+	}
+	ks_dw_pcie_enable_legacy_irqs(ks_pcie);
+
+	/* MSI IRQ */
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		for (i = 0; i < ks_pcie->num_msi_host_irqs; i++) {
+			irq_set_chained_handler(ks_pcie->msi_host_irqs[i],
+						ks_pcie_msi_irq_handler);
+			irq_set_handler_data(ks_pcie->msi_host_irqs[i],
+					     ks_pcie);
+		}
+	}
+}
+
+/*
+ * When a PCI device does not exist during config cycles, keystone host gets a
+ * bus error instead of returning 0xffffffff. This handler always returns 0
+ * for this kind of faults.
+ */
+static int keystone_pcie_fault(unsigned long addr, unsigned int fsr,
+				struct pt_regs *regs)
+{
+	unsigned long instr = *(unsigned long *) instruction_pointer(regs);
+
+	if ((instr & 0x0e100090) == 0x00100090) {
+		int reg = (instr >> 12) & 15;
+
+		regs->uregs[reg] = -1;
+		regs->ARM_pc += 4;
+	}
+
+	return 0;
+}
+
+static void __init ks_pcie_host_init(struct pcie_port *pp)
+{
+	struct keystone_pcie *ks_pcie = to_keystone_pcie(pp);
+	u32 val;
+
+	ks_pcie_establish_link(ks_pcie);
+	ks_dw_pcie_setup_rc_app_regs(ks_pcie);
+	ks_pcie_setup_interrupts(ks_pcie);
+	writew(PCI_IO_RANGE_TYPE_32 | (PCI_IO_RANGE_TYPE_32 << 8),
+			pp->dbi_base + PCI_IO_BASE);
+
+	/* update the Vendor ID */
+	writew(ks_pcie->device_id, pp->dbi_base + PCI_DEVICE_ID);
+
+	/* update the DEV_STAT_CTRL to publish right mrrs */
+	val = readl(pp->dbi_base + PCIE_CAP_BASE + PCI_EXP_DEVCTL);
+	val &= ~PCI_EXP_DEVCTL_READRQ;
+	/* set the mrrs to 256 bytes */
+	val |= BIT(12);
+	writel(val, pp->dbi_base + PCIE_CAP_BASE + PCI_EXP_DEVCTL);
+
+	/*
+	 * PCIe access errors that result into OCP errors are caught by ARM as
+	 * "External aborts"
+	 */
+	hook_fault_code(17, keystone_pcie_fault, SIGBUS, 0,
+			"Asynchronous external abort");
+}
+
+static struct pcie_host_ops keystone_pcie_host_ops = {
+	.rd_other_conf = ks_dw_pcie_rd_other_conf,
+	.wr_other_conf = ks_dw_pcie_wr_other_conf,
+	.link_up = ks_dw_pcie_link_up,
+	.host_init = ks_pcie_host_init,
+	.msi_set_irq = ks_dw_pcie_msi_set_irq,
+	.msi_clear_irq = ks_dw_pcie_msi_clear_irq,
+	.get_msi_addr = ks_dw_pcie_get_msi_addr,
+	.msi_host_init = ks_dw_pcie_msi_host_init,
+	.scan_bus = ks_dw_pcie_v3_65_scan_bus,
+};
+
+static int __init ks_add_pcie_port(struct keystone_pcie *ks_pcie,
+			 struct platform_device *pdev)
+{
+	struct pcie_port *pp = &ks_pcie->pp;
+	int ret;
+
+	ret = ks_pcie_get_irq_controller_info(ks_pcie,
+					"legacy-interrupt-controller",
+					&ks_pcie->num_legacy_host_irqs);
+	if (ret)
+		return ret;
+
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		ret = ks_pcie_get_irq_controller_info(ks_pcie,
+						"msi-interrupt-controller",
+						&ks_pcie->num_msi_host_irqs);
+		if (ret)
+			return ret;
+	}
+
+	pp->root_bus_nr = -1;
+	pp->ops = &keystone_pcie_host_ops;
+	ret = ks_dw_pcie_host_init(ks_pcie, ks_pcie->msi_intc_np);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to initialize host\n");
+		return ret;
+	}
+
+	return ret;
+}
+
+static const struct of_device_id ks_pcie_of_match[] = {
+	{
+		.type = "pci",
+		.compatible = "ti,keystone-pcie",
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, ks_pcie_of_match);
+
+static int __exit ks_pcie_remove(struct platform_device *pdev)
+{
+	struct keystone_pcie *ks_pcie = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(ks_pcie->clk);
+
+	return 0;
+}
+
+static int __init ks_pcie_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct keystone_pcie *ks_pcie;
+	struct pcie_port *pp;
+	struct resource *res;
+	void __iomem *reg_p;
+	struct phy *phy;
+	int ret = 0;
+
+	ks_pcie = devm_kzalloc(&pdev->dev, sizeof(*ks_pcie),
+				GFP_KERNEL);
+	if (!ks_pcie) {
+		dev_err(dev, "no memory for keystone pcie\n");
+		return -ENOMEM;
+	}
+	pp = &ks_pcie->pp;
+
+	/* initialize SerDes Phy if present */
+	phy = devm_phy_get(dev, "pcie-phy");
+	if (!IS_ERR_OR_NULL(phy)) {
+		ret = phy_init(phy);
+		if (ret < 0)
+			return ret;
+	}
+
+	/* index 2 is to read PCI DEVICE_ID */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 2);
+	reg_p = devm_ioremap_resource(dev, res);
+	if (IS_ERR(reg_p))
+		return PTR_ERR(reg_p);
+	ks_pcie->device_id = readl(reg_p) >> 16;
+	devm_iounmap(dev, reg_p);
+	devm_release_mem_region(dev, res->start, resource_size(res));
+
+	pp->dev = dev;
+	platform_set_drvdata(pdev, ks_pcie);
+	ks_pcie->clk = devm_clk_get(dev, "pcie");
+	if (IS_ERR(ks_pcie->clk)) {
+		dev_err(dev, "Failed to get pcie rc clock\n");
+		return PTR_ERR(ks_pcie->clk);
+	}
+	ret = clk_prepare_enable(ks_pcie->clk);
+	if (ret)
+		return ret;
+
+	ret = ks_add_pcie_port(ks_pcie, pdev);
+	if (ret < 0)
+		goto fail_clk;
+
+	return 0;
+fail_clk:
+	clk_disable_unprepare(ks_pcie->clk);
+
+	return ret;
+}
+
+static struct platform_driver ks_pcie_driver __refdata = {
+	.probe  = ks_pcie_probe,
+	.remove = __exit_p(ks_pcie_remove),
+	.driver = {
+		.name	= "keystone-pcie",
+		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(ks_pcie_of_match),
+	},
+};
+
+module_platform_driver(ks_pcie_driver);
+
+MODULE_AUTHOR("Murali Karicheri <m-karicheri2@ti.com>");
+MODULE_DESCRIPTION("Keystone PCIe host controller driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pci-keystone.h b/drivers/pci/host/pci-keystone.h
new file mode 100644
index 000000000000..1fc1fceede9e
--- /dev/null
+++ b/drivers/pci/host/pci-keystone.h
@@ -0,0 +1,58 @@
+/*
+ * Keystone PCI Controller's common includes
+ *
+ * Copyright (C) 2013-2014 Texas Instruments., Ltd.
+ *		http://www.ti.com
+ *
+ * Author: Murali Karicheri <m-karicheri2@ti.com>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#define MAX_LEGACY_IRQS			4
+#define MAX_MSI_HOST_IRQS		8
+#define MAX_LEGACY_HOST_IRQS		4
+
+struct keystone_pcie {
+	struct	clk		*clk;
+	struct	pcie_port	pp;
+	/* PCI Device ID */
+	u32			device_id;
+	int			num_legacy_host_irqs;
+	int			legacy_host_irqs[MAX_LEGACY_HOST_IRQS];
+	struct			device_node *legacy_intc_np;
+
+	int			num_msi_host_irqs;
+	int			msi_host_irqs[MAX_MSI_HOST_IRQS];
+	struct			device_node *msi_intc_np;
+	struct irq_domain	*legacy_irq_domain;
+
+	/* Application register space */
+	void __iomem		*va_app_base;
+	struct resource		app;
+};
+
+/* Keystone DW specific MSI controller APIs/definitions */
+void ks_dw_pcie_handle_msi_irq(struct keystone_pcie *ks_pcie, int offset);
+u32 ks_dw_pcie_get_msi_addr(struct pcie_port *pp);
+
+/* Keystone specific PCI controller APIs */
+void ks_dw_pcie_enable_legacy_irqs(struct keystone_pcie *ks_pcie);
+void ks_dw_pcie_handle_legacy_irq(struct keystone_pcie *ks_pcie, int offset);
+int  ks_dw_pcie_host_init(struct keystone_pcie *ks_pcie,
+			struct device_node *msi_intc_np);
+int ks_dw_pcie_wr_other_conf(struct pcie_port *pp, struct pci_bus *bus,
+		unsigned int devfn, int where, int size, u32 val);
+int ks_dw_pcie_rd_other_conf(struct pcie_port *pp, struct pci_bus *bus,
+		unsigned int devfn, int where, int size, u32 *val);
+void ks_dw_pcie_setup_rc_app_regs(struct keystone_pcie *ks_pcie);
+int ks_dw_pcie_link_up(struct pcie_port *pp);
+void ks_dw_pcie_initiate_link_train(struct keystone_pcie *ks_pcie);
+void ks_dw_pcie_msi_set_irq(struct pcie_port *pp, int irq);
+void ks_dw_pcie_msi_clear_irq(struct pcie_port *pp, int irq);
+void ks_dw_pcie_v3_65_scan_bus(struct pcie_port *pp);
+int ks_dw_pcie_msi_host_init(struct pcie_port *pp,
+		struct msi_chip *chip);
diff --git a/drivers/pci/host/pci-mvebu.c b/drivers/pci/host/pci-mvebu.c
index a8c6f1a92e0f..b1315e197ffb 100644
--- a/drivers/pci/host/pci-mvebu.c
+++ b/drivers/pci/host/pci-mvebu.c
@@ -873,7 +873,7 @@ static int mvebu_get_tgt_attr(struct device_node *np, int devfn,
 	rangesz = pna + na + ns;
 	nranges = rlen / sizeof(__be32) / rangesz;
 
-	for (i = 0; i < nranges; i++) {
+	for (i = 0; i < nranges; i++, range += rangesz) {
 		u32 flags = of_read_number(range, 1);
 		u32 slot = of_read_number(range + 1, 1);
 		u64 cpuaddr = of_read_number(range + na, pna);
@@ -883,14 +883,14 @@ static int mvebu_get_tgt_attr(struct device_node *np, int devfn,
 			rtype = IORESOURCE_IO;
 		else if (DT_FLAGS_TO_TYPE(flags) == DT_TYPE_MEM32)
 			rtype = IORESOURCE_MEM;
+		else
+			continue;
 
 		if (slot == PCI_SLOT(devfn) && type == rtype) {
 			*tgt = DT_CPUADDR_TO_TARGET(cpuaddr);
 			*attr = DT_CPUADDR_TO_ATTR(cpuaddr);
 			return 0;
 		}
-
-		range += rangesz;
 	}
 
 	return -ENOENT;
diff --git a/drivers/pci/host/pci-tegra.c b/drivers/pci/host/pci-tegra.c
index abd65784618d..3d43874319be 100644
--- a/drivers/pci/host/pci-tegra.c
+++ b/drivers/pci/host/pci-tegra.c
@@ -25,6 +25,7 @@
  */
 
 #include <linux/clk.h>
+#include <linux/debugfs.h>
 #include <linux/delay.h>
 #include <linux/export.h>
 #include <linux/interrupt.h>
@@ -37,6 +38,7 @@
 #include <linux/of_pci.h>
 #include <linux/of_platform.h>
 #include <linux/pci.h>
+#include <linux/phy/phy.h>
 #include <linux/platform_device.h>
 #include <linux/reset.h>
 #include <linux/sizes.h>
@@ -114,13 +116,20 @@
 
 #define AFI_INTR_CODE			0xb8
 #define  AFI_INTR_CODE_MASK		0xf
-#define  AFI_INTR_AXI_SLAVE_ERROR	1
-#define  AFI_INTR_AXI_DECODE_ERROR	2
+#define  AFI_INTR_INI_SLAVE_ERROR	1
+#define  AFI_INTR_INI_DECODE_ERROR	2
 #define  AFI_INTR_TARGET_ABORT		3
 #define  AFI_INTR_MASTER_ABORT		4
 #define  AFI_INTR_INVALID_WRITE		5
 #define  AFI_INTR_LEGACY		6
 #define  AFI_INTR_FPCI_DECODE_ERROR	7
+#define  AFI_INTR_AXI_DECODE_ERROR	8
+#define  AFI_INTR_FPCI_TIMEOUT		9
+#define  AFI_INTR_PE_PRSNT_SENSE	10
+#define  AFI_INTR_PE_CLKREQ_SENSE	11
+#define  AFI_INTR_CLKCLAMP_SENSE	12
+#define  AFI_INTR_RDY4PD_SENSE		13
+#define  AFI_INTR_P2P_ERROR		14
 
 #define AFI_INTR_SIGNATURE	0xbc
 #define AFI_UPPER_FPCI_ADDRESS	0xc0
@@ -151,8 +160,10 @@
 #define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_MASK	(0xf << 20)
 #define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_SINGLE	(0x0 << 20)
 #define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_420	(0x0 << 20)
+#define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X2_X1	(0x0 << 20)
 #define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_DUAL	(0x1 << 20)
 #define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_222	(0x1 << 20)
+#define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X4_X1	(0x1 << 20)
 #define  AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_411	(0x2 << 20)
 
 #define AFI_FUSE			0x104
@@ -164,12 +175,21 @@
 #define  AFI_PEX_CTRL_RST		(1 << 0)
 #define  AFI_PEX_CTRL_CLKREQ_EN		(1 << 1)
 #define  AFI_PEX_CTRL_REFCLK_EN		(1 << 3)
+#define  AFI_PEX_CTRL_OVERRIDE_EN	(1 << 4)
+
+#define AFI_PLLE_CONTROL		0x160
+#define  AFI_PLLE_CONTROL_BYPASS_PADS2PLLE_CONTROL (1 << 9)
+#define  AFI_PLLE_CONTROL_PADS2PLLE_CONTROL_EN (1 << 1)
 
 #define AFI_PEXBIAS_CTRL_0		0x168
 
 #define RP_VEND_XP	0x00000F00
 #define  RP_VEND_XP_DL_UP	(1 << 30)
 
+#define RP_PRIV_MISC	0x00000FE0
+#define  RP_PRIV_MISC_PRSNT_MAP_EP_PRSNT (0xE << 0)
+#define  RP_PRIV_MISC_PRSNT_MAP_EP_ABSNT (0xF << 0)
+
 #define RP_LINK_CONTROL_STATUS			0x00000090
 #define  RP_LINK_CONTROL_STATUS_DL_LINK_ACTIVE	0x20000000
 #define  RP_LINK_CONTROL_STATUS_LINKSTAT_MASK	0x3fff0000
@@ -196,6 +216,7 @@
 
 #define PADS_REFCLK_CFG0			0x000000C8
 #define PADS_REFCLK_CFG1			0x000000CC
+#define PADS_REFCLK_BIAS			0x000000D0
 
 /*
  * Fields in PADS_REFCLK_CFG*. Those registers form an array of 16-bit
@@ -235,6 +256,7 @@ struct tegra_pcie_soc_data {
 	bool has_pex_bias_ctrl;
 	bool has_intr_prsnt_sense;
 	bool has_cml_clk;
+	bool has_gen2;
 };
 
 static inline struct tegra_msi *to_tegra_msi(struct msi_chip *chip)
@@ -252,6 +274,7 @@ struct tegra_pcie {
 	struct list_head buses;
 	struct resource *cs;
 
+	struct resource all;
 	struct resource io;
 	struct resource mem;
 	struct resource prefetch;
@@ -266,6 +289,8 @@ struct tegra_pcie {
 	struct reset_control *afi_rst;
 	struct reset_control *pcie_xrst;
 
+	struct phy *phy;
+
 	struct tegra_msi msi;
 
 	struct list_head ports;
@@ -276,6 +301,7 @@ struct tegra_pcie {
 	unsigned int num_supplies;
 
 	const struct tegra_pcie_soc_data *soc_data;
+	struct dentry *debugfs;
 };
 
 struct tegra_pcie_port {
@@ -380,7 +406,7 @@ static struct tegra_pcie_bus *tegra_pcie_bus_alloc(struct tegra_pcie *pcie,
 	for (i = 0; i < 16; i++) {
 		unsigned long virt = (unsigned long)bus->area->addr +
 				     i * SZ_64K;
-		phys_addr_t phys = cs + i * SZ_1M + busnr * SZ_64K;
+		phys_addr_t phys = cs + i * SZ_16M + busnr * SZ_64K;
 
 		err = ioremap_page_range(virt, virt + SZ_64K, phys, prot);
 		if (err < 0) {
@@ -559,6 +585,8 @@ static void tegra_pcie_port_enable(struct tegra_pcie_port *port)
 	if (soc->has_pex_clkreq_en)
 		value |= AFI_PEX_CTRL_CLKREQ_EN;
 
+	value |= AFI_PEX_CTRL_OVERRIDE_EN;
+
 	afi_writel(port->pcie, value, ctrl);
 
 	tegra_pcie_port_reset(port);
@@ -566,6 +594,7 @@ static void tegra_pcie_port_enable(struct tegra_pcie_port *port)
 
 static void tegra_pcie_port_disable(struct tegra_pcie_port *port)
 {
+	const struct tegra_pcie_soc_data *soc = port->pcie->soc_data;
 	unsigned long ctrl = tegra_pcie_port_get_pex_ctrl(port);
 	unsigned long value;
 
@@ -576,6 +605,10 @@ static void tegra_pcie_port_disable(struct tegra_pcie_port *port)
 
 	/* disable reference clock */
 	value = afi_readl(port->pcie, ctrl);
+
+	if (soc->has_pex_clkreq_en)
+		value &= ~AFI_PEX_CTRL_CLKREQ_EN;
+
 	value &= ~AFI_PEX_CTRL_REFCLK_EN;
 	afi_writel(port->pcie, value, ctrl);
 }
@@ -624,13 +657,25 @@ DECLARE_PCI_FIXUP_FINAL(PCI_ANY_ID, PCI_ANY_ID, tegra_pcie_relax_enable);
 static int tegra_pcie_setup(int nr, struct pci_sys_data *sys)
 {
 	struct tegra_pcie *pcie = sys_to_pcie(sys);
+	int err;
+	phys_addr_t io_start;
+
+	err = devm_request_resource(pcie->dev, &pcie->all, &pcie->mem);
+	if (err < 0)
+		return err;
+
+	err = devm_request_resource(pcie->dev, &pcie->all, &pcie->prefetch);
+	if (err)
+		return err;
+
+	io_start = pci_pio_to_address(pcie->io.start);
 
 	pci_add_resource_offset(&sys->resources, &pcie->mem, sys->mem_offset);
 	pci_add_resource_offset(&sys->resources, &pcie->prefetch,
 				sys->mem_offset);
 	pci_add_resource(&sys->resources, &pcie->busn);
 
-	pci_ioremap_io(nr * SZ_64K, pcie->io.start);
+	pci_ioremap_io(nr * SZ_64K, io_start);
 
 	return 1;
 }
@@ -682,9 +727,15 @@ static irqreturn_t tegra_pcie_isr(int irq, void *arg)
 		"Target abort",
 		"Master abort",
 		"Invalid write",
+		"Legacy interrupt",
 		"Response decoding error",
 		"AXI response decoding error",
 		"Transaction timeout",
+		"Slot present pin change",
+		"Slot clock request change",
+		"TMS clock ramp change",
+		"TMS ready for power down",
+		"Peer2Peer error",
 	};
 	struct tegra_pcie *pcie = arg;
 	u32 code, signature;
@@ -735,6 +786,7 @@ static irqreturn_t tegra_pcie_isr(int irq, void *arg)
 static void tegra_pcie_setup_translations(struct tegra_pcie *pcie)
 {
 	u32 fpci_bar, size, axi_address;
+	phys_addr_t io_start = pci_pio_to_address(pcie->io.start);
 
 	/* Bar 0: type 1 extended configuration space */
 	fpci_bar = 0xfe100000;
@@ -747,7 +799,7 @@ static void tegra_pcie_setup_translations(struct tegra_pcie *pcie)
 	/* Bar 1: downstream IO bar */
 	fpci_bar = 0xfdfc0000;
 	size = resource_size(&pcie->io);
-	axi_address = pcie->io.start;
+	axi_address = io_start;
 	afi_writel(pcie, axi_address, AFI_AXI_BAR1_START);
 	afi_writel(pcie, size >> 12, AFI_AXI_BAR1_SZ);
 	afi_writel(pcie, fpci_bar, AFI_FPCI_BAR1);
@@ -790,30 +842,27 @@ static void tegra_pcie_setup_translations(struct tegra_pcie *pcie)
 	afi_writel(pcie, 0, AFI_MSI_BAR_SZ);
 }
 
-static int tegra_pcie_enable_controller(struct tegra_pcie *pcie)
+static int tegra_pcie_pll_wait(struct tegra_pcie *pcie, unsigned long timeout)
 {
 	const struct tegra_pcie_soc_data *soc = pcie->soc_data;
-	struct tegra_pcie_port *port;
-	unsigned int timeout;
-	unsigned long value;
-
-	/* power down PCIe slot clock bias pad */
-	if (soc->has_pex_bias_ctrl)
-		afi_writel(pcie, 0, AFI_PEXBIAS_CTRL_0);
+	u32 value;
 
-	/* configure mode and disable all ports */
-	value = afi_readl(pcie, AFI_PCIE_CONFIG);
-	value &= ~AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_MASK;
-	value |= AFI_PCIE_CONFIG_PCIE_DISABLE_ALL | pcie->xbar_config;
+	timeout = jiffies + msecs_to_jiffies(timeout);
 
-	list_for_each_entry(port, &pcie->ports, list)
-		value &= ~AFI_PCIE_CONFIG_PCIE_DISABLE(port->index);
+	while (time_before(jiffies, timeout)) {
+		value = pads_readl(pcie, soc->pads_pll_ctl);
+		if (value & PADS_PLL_CTL_LOCKDET)
+			return 0;
+	}
 
-	afi_writel(pcie, value, AFI_PCIE_CONFIG);
+	return -ETIMEDOUT;
+}
 
-	value = afi_readl(pcie, AFI_FUSE);
-	value |= AFI_FUSE_PCIE_T0_GEN2_DIS;
-	afi_writel(pcie, value, AFI_FUSE);
+static int tegra_pcie_phy_enable(struct tegra_pcie *pcie)
+{
+	const struct tegra_pcie_soc_data *soc = pcie->soc_data;
+	u32 value;
+	int err;
 
 	/* initialize internal PHY, enable up to 16 PCIE lanes */
 	pads_writel(pcie, 0x0, PADS_CTL_SEL);
@@ -832,6 +881,13 @@ static int tegra_pcie_enable_controller(struct tegra_pcie *pcie)
 	value |= PADS_PLL_CTL_REFCLK_INTERNAL_CML | soc->tx_ref_sel;
 	pads_writel(pcie, value, soc->pads_pll_ctl);
 
+	/* reset PLL */
+	value = pads_readl(pcie, soc->pads_pll_ctl);
+	value &= ~PADS_PLL_CTL_RST_B4SM;
+	pads_writel(pcie, value, soc->pads_pll_ctl);
+
+	usleep_range(20, 100);
+
 	/* take PLL out of reset  */
 	value = pads_readl(pcie, soc->pads_pll_ctl);
 	value |= PADS_PLL_CTL_RST_B4SM;
@@ -844,15 +900,11 @@ static int tegra_pcie_enable_controller(struct tegra_pcie *pcie)
 		pads_writel(pcie, PADS_REFCLK_CFG_VALUE, PADS_REFCLK_CFG1);
 
 	/* wait for the PLL to lock */
-	timeout = 300;
-	do {
-		value = pads_readl(pcie, soc->pads_pll_ctl);
-		usleep_range(1000, 2000);
-		if (--timeout == 0) {
-			pr_err("Tegra PCIe error: timeout waiting for PLL\n");
-			return -EBUSY;
-		}
-	} while (!(value & PADS_PLL_CTL_LOCKDET));
+	err = tegra_pcie_pll_wait(pcie, 500);
+	if (err < 0) {
+		dev_err(pcie->dev, "PLL failed to lock: %d\n", err);
+		return err;
+	}
 
 	/* turn off IDDQ override */
 	value = pads_readl(pcie, PADS_CTL);
@@ -864,6 +916,58 @@ static int tegra_pcie_enable_controller(struct tegra_pcie *pcie)
 	value |= PADS_CTL_TX_DATA_EN_1L | PADS_CTL_RX_DATA_EN_1L;
 	pads_writel(pcie, value, PADS_CTL);
 
+	return 0;
+}
+
+static int tegra_pcie_enable_controller(struct tegra_pcie *pcie)
+{
+	const struct tegra_pcie_soc_data *soc = pcie->soc_data;
+	struct tegra_pcie_port *port;
+	unsigned long value;
+	int err;
+
+	/* enable PLL power down */
+	if (pcie->phy) {
+		value = afi_readl(pcie, AFI_PLLE_CONTROL);
+		value &= ~AFI_PLLE_CONTROL_BYPASS_PADS2PLLE_CONTROL;
+		value |= AFI_PLLE_CONTROL_PADS2PLLE_CONTROL_EN;
+		afi_writel(pcie, value, AFI_PLLE_CONTROL);
+	}
+
+	/* power down PCIe slot clock bias pad */
+	if (soc->has_pex_bias_ctrl)
+		afi_writel(pcie, 0, AFI_PEXBIAS_CTRL_0);
+
+	/* configure mode and disable all ports */
+	value = afi_readl(pcie, AFI_PCIE_CONFIG);
+	value &= ~AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_MASK;
+	value |= AFI_PCIE_CONFIG_PCIE_DISABLE_ALL | pcie->xbar_config;
+
+	list_for_each_entry(port, &pcie->ports, list)
+		value &= ~AFI_PCIE_CONFIG_PCIE_DISABLE(port->index);
+
+	afi_writel(pcie, value, AFI_PCIE_CONFIG);
+
+	if (soc->has_gen2) {
+		value = afi_readl(pcie, AFI_FUSE);
+		value &= ~AFI_FUSE_PCIE_T0_GEN2_DIS;
+		afi_writel(pcie, value, AFI_FUSE);
+	} else {
+		value = afi_readl(pcie, AFI_FUSE);
+		value |= AFI_FUSE_PCIE_T0_GEN2_DIS;
+		afi_writel(pcie, value, AFI_FUSE);
+	}
+
+	if (!pcie->phy)
+		err = tegra_pcie_phy_enable(pcie);
+	else
+		err = phy_power_on(pcie->phy);
+
+	if (err < 0) {
+		dev_err(pcie->dev, "failed to power on PHY: %d\n", err);
+		return err;
+	}
+
 	/* take the PCIe interface module out of reset */
 	reset_control_deassert(pcie->pcie_xrst);
 
@@ -897,6 +1001,10 @@ static void tegra_pcie_power_off(struct tegra_pcie *pcie)
 
 	/* TODO: disable and unprepare clocks? */
 
+	err = phy_power_off(pcie->phy);
+	if (err < 0)
+		dev_warn(pcie->dev, "failed to power off PHY: %d\n", err);
+
 	reset_control_assert(pcie->pcie_xrst);
 	reset_control_assert(pcie->afi_rst);
 	reset_control_assert(pcie->pex_rst);
@@ -1018,6 +1126,19 @@ static int tegra_pcie_get_resources(struct tegra_pcie *pcie)
 		return err;
 	}
 
+	pcie->phy = devm_phy_optional_get(pcie->dev, "pcie");
+	if (IS_ERR(pcie->phy)) {
+		err = PTR_ERR(pcie->phy);
+		dev_err(&pdev->dev, "failed to get PHY: %d\n", err);
+		return err;
+	}
+
+	err = phy_init(pcie->phy);
+	if (err < 0) {
+		dev_err(&pdev->dev, "failed to initialize PHY: %d\n", err);
+		return err;
+	}
+
 	err = tegra_pcie_power_on(pcie);
 	if (err) {
 		dev_err(&pdev->dev, "failed to power up: %d\n", err);
@@ -1076,10 +1197,17 @@ poweroff:
 
 static int tegra_pcie_put_resources(struct tegra_pcie *pcie)
 {
+	int err;
+
 	if (pcie->irq > 0)
 		free_irq(pcie->irq, pcie);
 
 	tegra_pcie_power_off(pcie);
+
+	err = phy_exit(pcie->phy);
+	if (err < 0)
+		dev_err(pcie->dev, "failed to teardown PHY: %d\n", err);
+
 	return 0;
 }
 
@@ -1168,8 +1296,10 @@ static int tegra_msi_setup_irq(struct msi_chip *chip, struct pci_dev *pdev,
 		return hwirq;
 
 	irq = irq_create_mapping(msi->domain, hwirq);
-	if (!irq)
+	if (!irq) {
+		tegra_msi_free(msi, hwirq);
 		return -EINVAL;
+	}
 
 	irq_set_msi_desc(irq, desc);
 
@@ -1187,8 +1317,10 @@ static void tegra_msi_teardown_irq(struct msi_chip *chip, unsigned int irq)
 {
 	struct tegra_msi *msi = to_tegra_msi(chip);
 	struct irq_data *d = irq_get_irq_data(irq);
+	irq_hw_number_t hwirq = irqd_to_hwirq(d);
 
-	tegra_msi_free(msi, d->hwirq);
+	irq_dispose_mapping(irq);
+	tegra_msi_free(msi, hwirq);
 }
 
 static struct irq_chip tegra_msi_irq_chip = {
@@ -1325,7 +1457,19 @@ static int tegra_pcie_get_xbar_config(struct tegra_pcie *pcie, u32 lanes,
 {
 	struct device_node *np = pcie->dev->of_node;
 
-	if (of_device_is_compatible(np, "nvidia,tegra30-pcie")) {
+	if (of_device_is_compatible(np, "nvidia,tegra124-pcie")) {
+		switch (lanes) {
+		case 0x0000104:
+			dev_info(pcie->dev, "4x1, 1x1 configuration\n");
+			*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X4_X1;
+			return 0;
+
+		case 0x0000102:
+			dev_info(pcie->dev, "2x1, 1x1 configuration\n");
+			*xbar = AFI_PCIE_CONFIG_SM2TMS0_XBAR_CONFIG_X2_X1;
+			return 0;
+		}
+	} else if (of_device_is_compatible(np, "nvidia,tegra30-pcie")) {
 		switch (lanes) {
 		case 0x00000204:
 			dev_info(pcie->dev, "4x1, 2x1 configuration\n");
@@ -1433,7 +1577,23 @@ static int tegra_pcie_get_regulators(struct tegra_pcie *pcie, u32 lane_mask)
 	struct device_node *np = pcie->dev->of_node;
 	unsigned int i = 0;
 
-	if (of_device_is_compatible(np, "nvidia,tegra30-pcie")) {
+	if (of_device_is_compatible(np, "nvidia,tegra124-pcie")) {
+		pcie->num_supplies = 7;
+
+		pcie->supplies = devm_kcalloc(pcie->dev, pcie->num_supplies,
+					      sizeof(*pcie->supplies),
+					      GFP_KERNEL);
+		if (!pcie->supplies)
+			return -ENOMEM;
+
+		pcie->supplies[i++].supply = "avddio-pex";
+		pcie->supplies[i++].supply = "dvddio-pex";
+		pcie->supplies[i++].supply = "avdd-pex-pll";
+		pcie->supplies[i++].supply = "hvdd-pex";
+		pcie->supplies[i++].supply = "hvdd-pex-pll-e";
+		pcie->supplies[i++].supply = "vddio-pex-ctl";
+		pcie->supplies[i++].supply = "avdd-pll-erefe";
+	} else if (of_device_is_compatible(np, "nvidia,tegra30-pcie")) {
 		bool need_pexa = false, need_pexb = false;
 
 		/* VDD_PEXA and AVDD_PEXA supply lanes 0 to 3 */
@@ -1512,32 +1672,50 @@ static int tegra_pcie_parse_dt(struct tegra_pcie *pcie)
 	struct resource res;
 	int err;
 
+	memset(&pcie->all, 0, sizeof(pcie->all));
+	pcie->all.flags = IORESOURCE_MEM;
+	pcie->all.name = np->full_name;
+	pcie->all.start = ~0;
+	pcie->all.end = 0;
+
 	if (of_pci_range_parser_init(&parser, np)) {
 		dev_err(pcie->dev, "missing \"ranges\" property\n");
 		return -EINVAL;
 	}
 
 	for_each_of_pci_range(&parser, &range) {
-		of_pci_range_to_resource(&range, np, &res);
+		err = of_pci_range_to_resource(&range, np, &res);
+		if (err < 0)
+			return err;
 
 		switch (res.flags & IORESOURCE_TYPE_BITS) {
 		case IORESOURCE_IO:
 			memcpy(&pcie->io, &res, sizeof(res));
-			pcie->io.name = "I/O";
+			pcie->io.name = np->full_name;
 			break;
 
 		case IORESOURCE_MEM:
 			if (res.flags & IORESOURCE_PREFETCH) {
 				memcpy(&pcie->prefetch, &res, sizeof(res));
-				pcie->prefetch.name = "PREFETCH";
+				pcie->prefetch.name = "prefetchable";
 			} else {
 				memcpy(&pcie->mem, &res, sizeof(res));
-				pcie->mem.name = "MEM";
+				pcie->mem.name = "non-prefetchable";
 			}
 			break;
 		}
+
+		if (res.start <= pcie->all.start)
+			pcie->all.start = res.start;
+
+		if (res.end >= pcie->all.end)
+			pcie->all.end = res.end;
 	}
 
+	err = devm_request_resource(pcie->dev, &iomem_resource, &pcie->all);
+	if (err < 0)
+		return err;
+
 	err = of_pci_parse_bus_range(np, &pcie->busn);
 	if (err < 0) {
 		dev_err(pcie->dev, "failed to parse ranges property: %d\n",
@@ -1639,6 +1817,12 @@ static bool tegra_pcie_port_check_link(struct tegra_pcie_port *port)
 	unsigned int retries = 3;
 	unsigned long value;
 
+	/* override presence detection */
+	value = readl(port->base + RP_PRIV_MISC);
+	value &= ~RP_PRIV_MISC_PRSNT_MAP_EP_ABSNT;
+	value |= RP_PRIV_MISC_PRSNT_MAP_EP_PRSNT;
+	writel(value, port->base + RP_PRIV_MISC);
+
 	do {
 		unsigned int timeout = TEGRA_PCIE_LINKUP_TIMEOUT;
 
@@ -1719,6 +1903,7 @@ static const struct tegra_pcie_soc_data tegra20_pcie_data = {
 	.has_pex_bias_ctrl = false,
 	.has_intr_prsnt_sense = false,
 	.has_cml_clk = false,
+	.has_gen2 = false,
 };
 
 static const struct tegra_pcie_soc_data tegra30_pcie_data = {
@@ -1730,15 +1915,138 @@ static const struct tegra_pcie_soc_data tegra30_pcie_data = {
 	.has_pex_bias_ctrl = true,
 	.has_intr_prsnt_sense = true,
 	.has_cml_clk = true,
+	.has_gen2 = false,
+};
+
+static const struct tegra_pcie_soc_data tegra124_pcie_data = {
+	.num_ports = 2,
+	.msi_base_shift = 8,
+	.pads_pll_ctl = PADS_PLL_CTL_TEGRA30,
+	.tx_ref_sel = PADS_PLL_CTL_TXCLKREF_BUF_EN,
+	.has_pex_clkreq_en = true,
+	.has_pex_bias_ctrl = true,
+	.has_intr_prsnt_sense = true,
+	.has_cml_clk = true,
+	.has_gen2 = true,
 };
 
 static const struct of_device_id tegra_pcie_of_match[] = {
+	{ .compatible = "nvidia,tegra124-pcie", .data = &tegra124_pcie_data },
 	{ .compatible = "nvidia,tegra30-pcie", .data = &tegra30_pcie_data },
 	{ .compatible = "nvidia,tegra20-pcie", .data = &tegra20_pcie_data },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, tegra_pcie_of_match);
 
+static void *tegra_pcie_ports_seq_start(struct seq_file *s, loff_t *pos)
+{
+	struct tegra_pcie *pcie = s->private;
+
+	if (list_empty(&pcie->ports))
+		return NULL;
+
+	seq_printf(s, "Index  Status\n");
+
+	return seq_list_start(&pcie->ports, *pos);
+}
+
+static void *tegra_pcie_ports_seq_next(struct seq_file *s, void *v, loff_t *pos)
+{
+	struct tegra_pcie *pcie = s->private;
+
+	return seq_list_next(v, &pcie->ports, pos);
+}
+
+static void tegra_pcie_ports_seq_stop(struct seq_file *s, void *v)
+{
+}
+
+static int tegra_pcie_ports_seq_show(struct seq_file *s, void *v)
+{
+	bool up = false, active = false;
+	struct tegra_pcie_port *port;
+	unsigned int value;
+
+	port = list_entry(v, struct tegra_pcie_port, list);
+
+	value = readl(port->base + RP_VEND_XP);
+
+	if (value & RP_VEND_XP_DL_UP)
+		up = true;
+
+	value = readl(port->base + RP_LINK_CONTROL_STATUS);
+
+	if (value & RP_LINK_CONTROL_STATUS_DL_LINK_ACTIVE)
+		active = true;
+
+	seq_printf(s, "%2u     ", port->index);
+
+	if (up)
+		seq_printf(s, "up");
+
+	if (active) {
+		if (up)
+			seq_printf(s, ", ");
+
+		seq_printf(s, "active");
+	}
+
+	seq_printf(s, "\n");
+	return 0;
+}
+
+static const struct seq_operations tegra_pcie_ports_seq_ops = {
+	.start = tegra_pcie_ports_seq_start,
+	.next = tegra_pcie_ports_seq_next,
+	.stop = tegra_pcie_ports_seq_stop,
+	.show = tegra_pcie_ports_seq_show,
+};
+
+static int tegra_pcie_ports_open(struct inode *inode, struct file *file)
+{
+	struct tegra_pcie *pcie = inode->i_private;
+	struct seq_file *s;
+	int err;
+
+	err = seq_open(file, &tegra_pcie_ports_seq_ops);
+	if (err)
+		return err;
+
+	s = file->private_data;
+	s->private = pcie;
+
+	return 0;
+}
+
+static const struct file_operations tegra_pcie_ports_ops = {
+	.owner = THIS_MODULE,
+	.open = tegra_pcie_ports_open,
+	.read = seq_read,
+	.llseek = seq_lseek,
+	.release = seq_release,
+};
+
+static int tegra_pcie_debugfs_init(struct tegra_pcie *pcie)
+{
+	struct dentry *file;
+
+	pcie->debugfs = debugfs_create_dir("pcie", NULL);
+	if (!pcie->debugfs)
+		return -ENOMEM;
+
+	file = debugfs_create_file("ports", S_IFREG | S_IRUGO, pcie->debugfs,
+				   pcie, &tegra_pcie_ports_ops);
+	if (!file)
+		goto remove;
+
+	return 0;
+
+remove:
+	debugfs_remove_recursive(pcie->debugfs);
+	pcie->debugfs = NULL;
+	return -ENOMEM;
+}
+
 static int tegra_pcie_probe(struct platform_device *pdev)
 {
 	const struct of_device_id *match;
@@ -1793,6 +2101,13 @@ static int tegra_pcie_probe(struct platform_device *pdev)
 		goto disable_msi;
 	}
 
+	if (IS_ENABLED(CONFIG_DEBUG_FS)) {
+		err = tegra_pcie_debugfs_init(pcie);
+		if (err < 0)
+			dev_err(&pdev->dev, "failed to setup debugfs: %d\n",
+				err);
+	}
+
 	platform_set_drvdata(pdev, pcie);
 	return 0;
 
diff --git a/drivers/pci/host/pci-xgene.c b/drivers/pci/host/pci-xgene.c
new file mode 100644
index 000000000000..9ecabfa8c634
--- /dev/null
+++ b/drivers/pci/host/pci-xgene.c
@@ -0,0 +1,659 @@
+/**
+ * APM X-Gene PCIe Driver
+ *
+ * Copyright (c) 2014 Applied Micro Circuits Corporation.
+ *
+ * Author: Tanmay Inamdar <tinamdar@apm.com>.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 of the License, or (at your
+ * option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/clk-private.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/memblock.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_irq.h>
+#include <linux/of_pci.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+
+#define PCIECORE_CTLANDSTATUS		0x50
+#define PIM1_1L				0x80
+#define IBAR2				0x98
+#define IR2MSK				0x9c
+#define PIM2_1L				0xa0
+#define IBAR3L				0xb4
+#define IR3MSKL				0xbc
+#define PIM3_1L				0xc4
+#define OMR1BARL			0x100
+#define OMR2BARL			0x118
+#define OMR3BARL			0x130
+#define CFGBARL				0x154
+#define CFGBARH				0x158
+#define CFGCTL				0x15c
+#define RTDID				0x160
+#define BRIDGE_CFG_0			0x2000
+#define BRIDGE_CFG_4			0x2010
+#define BRIDGE_STATUS_0			0x2600
+
+#define LINK_UP_MASK			0x00000100
+#define AXI_EP_CFG_ACCESS		0x10000
+#define EN_COHERENCY			0xF0000000
+#define EN_REG				0x00000001
+#define OB_LO_IO			0x00000002
+#define XGENE_PCIE_VENDORID		0x10E8
+#define XGENE_PCIE_DEVICEID		0xE004
+#define SZ_1T				(SZ_1G*1024ULL)
+#define PIPE_PHY_RATE_RD(src)		((0xc000 & (u32)(src)) >> 0xe)
+
+struct xgene_pcie_port {
+	struct device_node	*node;
+	struct device		*dev;
+	struct clk		*clk;
+	void __iomem		*csr_base;
+	void __iomem		*cfg_base;
+	unsigned long		cfg_addr;
+	bool			link_up;
+};
+
+static inline u32 pcie_bar_low_val(u32 addr, u32 flags)
+{
+	return (addr & PCI_BASE_ADDRESS_MEM_MASK) | flags;
+}
+
+/* PCIe Configuration Out/In */
+static inline void xgene_pcie_cfg_out32(void __iomem *addr, int offset, u32 val)
+{
+	writel(val, addr + offset);
+}
+
+static inline void xgene_pcie_cfg_out16(void __iomem *addr, int offset, u16 val)
+{
+	u32 val32 = readl(addr + (offset & ~0x3));
+
+	switch (offset & 0x3) {
+	case 2:
+		val32 &= ~0xFFFF0000;
+		val32 |= (u32)val << 16;
+		break;
+	case 0:
+	default:
+		val32 &= ~0xFFFF;
+		val32 |= val;
+		break;
+	}
+	writel(val32, addr + (offset & ~0x3));
+}
+
+static inline void xgene_pcie_cfg_out8(void __iomem *addr, int offset, u8 val)
+{
+	u32 val32 = readl(addr + (offset & ~0x3));
+
+	switch (offset & 0x3) {
+	case 0:
+		val32 &= ~0xFF;
+		val32 |= val;
+		break;
+	case 1:
+		val32 &= ~0xFF00;
+		val32 |= (u32)val << 8;
+		break;
+	case 2:
+		val32 &= ~0xFF0000;
+		val32 |= (u32)val << 16;
+		break;
+	case 3:
+	default:
+		val32 &= ~0xFF000000;
+		val32 |= (u32)val << 24;
+		break;
+	}
+	writel(val32, addr + (offset & ~0x3));
+}
+
+static inline void xgene_pcie_cfg_in32(void __iomem *addr, int offset, u32 *val)
+{
+	*val = readl(addr + offset);
+}
+
+static inline void xgene_pcie_cfg_in16(void __iomem *addr, int offset, u32 *val)
+{
+	*val = readl(addr + (offset & ~0x3));
+
+	switch (offset & 0x3) {
+	case 2:
+		*val >>= 16;
+		break;
+	}
+
+	*val &= 0xFFFF;
+}
+
+static inline void xgene_pcie_cfg_in8(void __iomem *addr, int offset, u32 *val)
+{
+	*val = readl(addr + (offset & ~0x3));
+
+	switch (offset & 0x3) {
+	case 3:
+		*val = *val >> 24;
+		break;
+	case 2:
+		*val = *val >> 16;
+		break;
+	case 1:
+		*val = *val >> 8;
+		break;
+	}
+	*val &= 0xFF;
+}
+
+/*
+ * When the address bit [17:16] is 2'b01, the Configuration access will be
+ * treated as Type 1 and it will be forwarded to external PCIe device.
+ */
+static void __iomem *xgene_pcie_get_cfg_base(struct pci_bus *bus)
+{
+	struct xgene_pcie_port *port = bus->sysdata;
+
+	if (bus->number >= (bus->primary + 1))
+		return port->cfg_base + AXI_EP_CFG_ACCESS;
+
+	return port->cfg_base;
+}
+
+/*
+ * For Configuration request, RTDID register is used as Bus Number,
+ * Device Number and Function number of the header fields.
+ */
+static void xgene_pcie_set_rtdid_reg(struct pci_bus *bus, uint devfn)
+{
+	struct xgene_pcie_port *port = bus->sysdata;
+	unsigned int b, d, f;
+	u32 rtdid_val = 0;
+
+	b = bus->number;
+	d = PCI_SLOT(devfn);
+	f = PCI_FUNC(devfn);
+
+	if (!pci_is_root_bus(bus))
+		rtdid_val = (b << 8) | (d << 3) | f;
+
+	writel(rtdid_val, port->csr_base + RTDID);
+	/* read the register back to ensure flush */
+	readl(port->csr_base + RTDID);
+}
+
+/*
+ * X-Gene PCIe port uses BAR0-BAR1 of RC's configuration space as
+ * the translation from PCI bus to native BUS.  Entire DDR region
+ * is mapped into PCIe space using these registers, so it can be
+ * reached by DMA from EP devices.  The BAR0/1 of bridge should be
+ * hidden during enumeration to avoid the sizing and resource allocation
+ * by PCIe core.
+ */
+static bool xgene_pcie_hide_rc_bars(struct pci_bus *bus, int offset)
+{
+	if (pci_is_root_bus(bus) && ((offset == PCI_BASE_ADDRESS_0) ||
+				     (offset == PCI_BASE_ADDRESS_1)))
+		return true;
+
+	return false;
+}
+
+static int xgene_pcie_read_config(struct pci_bus *bus, unsigned int devfn,
+				  int offset, int len, u32 *val)
+{
+	struct xgene_pcie_port *port = bus->sysdata;
+	void __iomem *addr;
+
+	if ((pci_is_root_bus(bus) && devfn != 0) || !port->link_up)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	if (xgene_pcie_hide_rc_bars(bus, offset)) {
+		*val = 0;
+		return PCIBIOS_SUCCESSFUL;
+	}
+
+	xgene_pcie_set_rtdid_reg(bus, devfn);
+	addr = xgene_pcie_get_cfg_base(bus);
+	switch (len) {
+	case 1:
+		xgene_pcie_cfg_in8(addr, offset, val);
+		break;
+	case 2:
+		xgene_pcie_cfg_in16(addr, offset, val);
+		break;
+	default:
+		xgene_pcie_cfg_in32(addr, offset, val);
+		break;
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static int xgene_pcie_write_config(struct pci_bus *bus, unsigned int devfn,
+				   int offset, int len, u32 val)
+{
+	struct xgene_pcie_port *port = bus->sysdata;
+	void __iomem *addr;
+
+	if ((pci_is_root_bus(bus) && devfn != 0) || !port->link_up)
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	if (xgene_pcie_hide_rc_bars(bus, offset))
+		return PCIBIOS_SUCCESSFUL;
+
+	xgene_pcie_set_rtdid_reg(bus, devfn);
+	addr = xgene_pcie_get_cfg_base(bus);
+	switch (len) {
+	case 1:
+		xgene_pcie_cfg_out8(addr, offset, (u8)val);
+		break;
+	case 2:
+		xgene_pcie_cfg_out16(addr, offset, (u16)val);
+		break;
+	default:
+		xgene_pcie_cfg_out32(addr, offset, val);
+		break;
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+static struct pci_ops xgene_pcie_ops = {
+	.read = xgene_pcie_read_config,
+	.write = xgene_pcie_write_config
+};
+
+static u64 xgene_pcie_set_ib_mask(void __iomem *csr_base, u32 addr,
+				  u32 flags, u64 size)
+{
+	u64 mask = (~(size - 1) & PCI_BASE_ADDRESS_MEM_MASK) | flags;
+	u32 val32 = 0;
+	u32 val;
+
+	val32 = readl(csr_base + addr);
+	val = (val32 & 0x0000ffff) | (lower_32_bits(mask) << 16);
+	writel(val, csr_base + addr);
+
+	val32 = readl(csr_base + addr + 0x04);
+	val = (val32 & 0xffff0000) | (lower_32_bits(mask) >> 16);
+	writel(val, csr_base + addr + 0x04);
+
+	val32 = readl(csr_base + addr + 0x04);
+	val = (val32 & 0x0000ffff) | (upper_32_bits(mask) << 16);
+	writel(val, csr_base + addr + 0x04);
+
+	val32 = readl(csr_base + addr + 0x08);
+	val = (val32 & 0xffff0000) | (upper_32_bits(mask) >> 16);
+	writel(val, csr_base + addr + 0x08);
+
+	return mask;
+}
+
+static void xgene_pcie_linkup(struct xgene_pcie_port *port,
+				   u32 *lanes, u32 *speed)
+{
+	void __iomem *csr_base = port->csr_base;
+	u32 val32;
+
+	port->link_up = false;
+	val32 = readl(csr_base + PCIECORE_CTLANDSTATUS);
+	if (val32 & LINK_UP_MASK) {
+		port->link_up = true;
+		*speed = PIPE_PHY_RATE_RD(val32);
+		val32 = readl(csr_base + BRIDGE_STATUS_0);
+		*lanes = val32 >> 26;
+	}
+}
+
+static int xgene_pcie_init_port(struct xgene_pcie_port *port)
+{
+	int rc;
+
+	port->clk = clk_get(port->dev, NULL);
+	if (IS_ERR(port->clk)) {
+		dev_err(port->dev, "clock not available\n");
+		return -ENODEV;
+	}
+
+	rc = clk_prepare_enable(port->clk);
+	if (rc) {
+		dev_err(port->dev, "clock enable failed\n");
+		return rc;
+	}
+
+	return 0;
+}
+
+static int xgene_pcie_map_reg(struct xgene_pcie_port *port,
+			      struct platform_device *pdev)
+{
+	struct resource *res;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "csr");
+	port->csr_base = devm_ioremap_resource(port->dev, res);
+	if (IS_ERR(port->csr_base))
+		return PTR_ERR(port->csr_base);
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "cfg");
+	port->cfg_base = devm_ioremap_resource(port->dev, res);
+	if (IS_ERR(port->cfg_base))
+		return PTR_ERR(port->cfg_base);
+	port->cfg_addr = res->start;
+
+	return 0;
+}
+
+static void xgene_pcie_setup_ob_reg(struct xgene_pcie_port *port,
+				    struct resource *res, u32 offset,
+				    u64 cpu_addr, u64 pci_addr)
+{
+	void __iomem *base = port->csr_base + offset;
+	resource_size_t size = resource_size(res);
+	u64 restype = resource_type(res);
+	u64 mask = 0;
+	u32 min_size;
+	u32 flag = EN_REG;
+
+	if (restype == IORESOURCE_MEM) {
+		min_size = SZ_128M;
+	} else {
+		min_size = 128;
+		flag |= OB_LO_IO;
+	}
+
+	if (size >= min_size)
+		mask = ~(size - 1) | flag;
+	else
+		dev_warn(port->dev, "res size 0x%llx less than minimum 0x%x\n",
+			 (u64)size, min_size);
+
+	writel(lower_32_bits(cpu_addr), base);
+	writel(upper_32_bits(cpu_addr), base + 0x04);
+	writel(lower_32_bits(mask), base + 0x08);
+	writel(upper_32_bits(mask), base + 0x0c);
+	writel(lower_32_bits(pci_addr), base + 0x10);
+	writel(upper_32_bits(pci_addr), base + 0x14);
+}
+
+static void xgene_pcie_setup_cfg_reg(void __iomem *csr_base, u64 addr)
+{
+	writel(lower_32_bits(addr), csr_base + CFGBARL);
+	writel(upper_32_bits(addr), csr_base + CFGBARH);
+	writel(EN_REG, csr_base + CFGCTL);
+}
+
+static int xgene_pcie_map_ranges(struct xgene_pcie_port *port,
+				 struct list_head *res,
+				 resource_size_t io_base)
+{
+	struct pci_host_bridge_window *window;
+	struct device *dev = port->dev;
+	int ret;
+
+	list_for_each_entry(window, res, list) {
+		struct resource *res = window->res;
+		u64 restype = resource_type(res);
+
+		dev_dbg(port->dev, "%pR\n", res);
+
+		switch (restype) {
+		case IORESOURCE_IO:
+			xgene_pcie_setup_ob_reg(port, res, OMR3BARL, io_base,
+						res->start - window->offset);
+			ret = pci_remap_iospace(res, io_base);
+			if (ret < 0)
+				return ret;
+			break;
+		case IORESOURCE_MEM:
+			xgene_pcie_setup_ob_reg(port, res, OMR1BARL, res->start,
+						res->start - window->offset);
+			break;
+		case IORESOURCE_BUS:
+			break;
+		default:
+			dev_err(dev, "invalid resource %pR\n", res);
+			return -EINVAL;
+		}
+	}
+	xgene_pcie_setup_cfg_reg(port->csr_base, port->cfg_addr);
+
+	return 0;
+}
+
+static void xgene_pcie_setup_pims(void *addr, u64 pim, u64 size)
+{
+	writel(lower_32_bits(pim), addr);
+	writel(upper_32_bits(pim) | EN_COHERENCY, addr + 0x04);
+	writel(lower_32_bits(size), addr + 0x10);
+	writel(upper_32_bits(size), addr + 0x14);
+}
+
+/*
+ * X-Gene PCIe support maximum 3 inbound memory regions
+ * This function helps to select a region based on size of region
+ */
+static int xgene_pcie_select_ib_reg(u8 *ib_reg_mask, u64 size)
+{
+	if ((size > 4) && (size < SZ_16M) && !(*ib_reg_mask & (1 << 1))) {
+		*ib_reg_mask |= (1 << 1);
+		return 1;
+	}
+
+	if ((size > SZ_1K) && (size < SZ_1T) && !(*ib_reg_mask & (1 << 0))) {
+		*ib_reg_mask |= (1 << 0);
+		return 0;
+	}
+
+	if ((size > SZ_1M) && (size < SZ_1T) && !(*ib_reg_mask & (1 << 2))) {
+		*ib_reg_mask |= (1 << 2);
+		return 2;
+	}
+
+	return -EINVAL;
+}
+
+static void xgene_pcie_setup_ib_reg(struct xgene_pcie_port *port,
+				    struct of_pci_range *range, u8 *ib_reg_mask)
+{
+	void __iomem *csr_base = port->csr_base;
+	void __iomem *cfg_base = port->cfg_base;
+	void *bar_addr;
+	void *pim_addr;
+	u64 cpu_addr = range->cpu_addr;
+	u64 pci_addr = range->pci_addr;
+	u64 size = range->size;
+	u64 mask = ~(size - 1) | EN_REG;
+	u32 flags = PCI_BASE_ADDRESS_MEM_TYPE_64;
+	u32 bar_low;
+	int region;
+
+	region = xgene_pcie_select_ib_reg(ib_reg_mask, range->size);
+	if (region < 0) {
+		dev_warn(port->dev, "invalid pcie dma-range config\n");
+		return;
+	}
+
+	if (range->flags & IORESOURCE_PREFETCH)
+		flags |= PCI_BASE_ADDRESS_MEM_PREFETCH;
+
+	bar_low = pcie_bar_low_val((u32)cpu_addr, flags);
+	switch (region) {
+	case 0:
+		xgene_pcie_set_ib_mask(csr_base, BRIDGE_CFG_4, flags, size);
+		bar_addr = cfg_base + PCI_BASE_ADDRESS_0;
+		writel(bar_low, bar_addr);
+		writel(upper_32_bits(cpu_addr), bar_addr + 0x4);
+		pim_addr = csr_base + PIM1_1L;
+		break;
+	case 1:
+		bar_addr = csr_base + IBAR2;
+		writel(bar_low, bar_addr);
+		writel(lower_32_bits(mask), csr_base + IR2MSK);
+		pim_addr = csr_base + PIM2_1L;
+		break;
+	case 2:
+		bar_addr = csr_base + IBAR3L;
+		writel(bar_low, bar_addr);
+		writel(upper_32_bits(cpu_addr), bar_addr + 0x4);
+		writel(lower_32_bits(mask), csr_base + IR3MSKL);
+		writel(upper_32_bits(mask), csr_base + IR3MSKL + 0x4);
+		pim_addr = csr_base + PIM3_1L;
+		break;
+	}
+
+	xgene_pcie_setup_pims(pim_addr, pci_addr, ~(size - 1));
+}
+
+static int pci_dma_range_parser_init(struct of_pci_range_parser *parser,
+				     struct device_node *node)
+{
+	const int na = 3, ns = 2;
+	int rlen;
+
+	parser->node = node;
+	parser->pna = of_n_addr_cells(node);
+	parser->np = parser->pna + na + ns;
+
+	parser->range = of_get_property(node, "dma-ranges", &rlen);
+	if (!parser->range)
+		return -ENOENT;
+	parser->end = parser->range + rlen / sizeof(__be32);
+
+	return 0;
+}
+
+static int xgene_pcie_parse_map_dma_ranges(struct xgene_pcie_port *port)
+{
+	struct device_node *np = port->node;
+	struct of_pci_range range;
+	struct of_pci_range_parser parser;
+	struct device *dev = port->dev;
+	u8 ib_reg_mask = 0;
+
+	if (pci_dma_range_parser_init(&parser, np)) {
+		dev_err(dev, "missing dma-ranges property\n");
+		return -EINVAL;
+	}
+
+	/* Get the dma-ranges from DT */
+	for_each_of_pci_range(&parser, &range) {
+		u64 end = range.cpu_addr + range.size - 1;
+
+		dev_dbg(port->dev, "0x%08x 0x%016llx..0x%016llx -> 0x%016llx\n",
+			range.flags, range.cpu_addr, end, range.pci_addr);
+		xgene_pcie_setup_ib_reg(port, &range, &ib_reg_mask);
+	}
+	return 0;
+}
+
+/* clear BAR configuration which was done by firmware */
+static void xgene_pcie_clear_config(struct xgene_pcie_port *port)
+{
+	int i;
+
+	for (i = PIM1_1L; i <= CFGCTL; i += 4)
+		writel(0x0, port->csr_base + i);
+}
+
+static int xgene_pcie_setup(struct xgene_pcie_port *port,
+			    struct list_head *res,
+			    resource_size_t io_base)
+{
+	u32 val, lanes = 0, speed = 0;
+	int ret;
+
+	xgene_pcie_clear_config(port);
+
+	/* setup the vendor and device IDs correctly */
+	val = (XGENE_PCIE_DEVICEID << 16) | XGENE_PCIE_VENDORID;
+	writel(val, port->csr_base + BRIDGE_CFG_0);
+
+	ret = xgene_pcie_map_ranges(port, res, io_base);
+	if (ret)
+		return ret;
+
+	ret = xgene_pcie_parse_map_dma_ranges(port);
+	if (ret)
+		return ret;
+
+	xgene_pcie_linkup(port, &lanes, &speed);
+	if (!port->link_up)
+		dev_info(port->dev, "(rc) link down\n");
+	else
+		dev_info(port->dev, "(rc) x%d gen-%d link up\n",
+				lanes, speed + 1);
+	return 0;
+}
+
+static int xgene_pcie_probe_bridge(struct platform_device *pdev)
+{
+	struct device_node *dn = pdev->dev.of_node;
+	struct xgene_pcie_port *port;
+	resource_size_t iobase = 0;
+	struct pci_bus *bus;
+	int ret;
+	LIST_HEAD(res);
+
+	port = devm_kzalloc(&pdev->dev, sizeof(*port), GFP_KERNEL);
+	if (!port)
+		return -ENOMEM;
+	port->node = of_node_get(pdev->dev.of_node);
+	port->dev = &pdev->dev;
+
+	ret = xgene_pcie_map_reg(port, pdev);
+	if (ret)
+		return ret;
+
+	ret = xgene_pcie_init_port(port);
+	if (ret)
+		return ret;
+
+	ret = of_pci_get_host_bridge_resources(dn, 0, 0xff, &res, &iobase);
+	if (ret)
+		return ret;
+
+	ret = xgene_pcie_setup(port, &res, iobase);
+	if (ret)
+		return ret;
+
+	bus = pci_scan_root_bus(&pdev->dev, 0, &xgene_pcie_ops, port, &res);
+	if (!bus)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, port);
+	return 0;
+}
+
+static const struct of_device_id xgene_pcie_match_table[] = {
+	{.compatible = "apm,xgene-pcie",},
+	{},
+};
+
+static struct platform_driver xgene_pcie_driver = {
+	.driver = {
+		   .name = "xgene-pcie",
+		   .owner = THIS_MODULE,
+		   .of_match_table = of_match_ptr(xgene_pcie_match_table),
+	},
+	.probe = xgene_pcie_probe_bridge,
+};
+module_platform_driver(xgene_pcie_driver);
+
+MODULE_AUTHOR("Tanmay Inamdar <tinamdar@apm.com>");
+MODULE_DESCRIPTION("APM X-Gene PCIe driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/host/pcie-designware.c b/drivers/pci/host/pcie-designware.c
index 1eaf4df3618a..dfed00aa3ac0 100644
--- a/drivers/pci/host/pcie-designware.c
+++ b/drivers/pci/host/pcie-designware.c
@@ -20,6 +20,7 @@
 #include <linux/of_pci.h>
 #include <linux/pci.h>
 #include <linux/pci_regs.h>
+#include <linux/platform_device.h>
 #include <linux/types.h>
 
 #include "pcie-designware.h"
@@ -72,6 +73,8 @@ static unsigned long global_io_offset;
 
 static inline struct pcie_port *sys_to_pcie(struct pci_sys_data *sys)
 {
+	BUG_ON(!sys->private_data);
+
 	return sys->private_data;
 }
 
@@ -193,75 +196,54 @@ void dw_pcie_msi_init(struct pcie_port *pp)
 	dw_pcie_wr_own_conf(pp, PCIE_MSI_ADDR_HI, 4, 0);
 }
 
-static int find_valid_pos0(struct pcie_port *pp, int msgvec, int pos, int *pos0)
+static void dw_pcie_msi_clear_irq(struct pcie_port *pp, int irq)
 {
-	int flag = 1;
-
-	do {
-		pos = find_next_zero_bit(pp->msi_irq_in_use,
-				MAX_MSI_IRQS, pos);
-		/*if you have reached to the end then get out from here.*/
-		if (pos == MAX_MSI_IRQS)
-			return -ENOSPC;
-		/*
-		 * Check if this position is at correct offset.nvec is always a
-		 * power of two. pos0 must be nvec bit aligned.
-		 */
-		if (pos % msgvec)
-			pos += msgvec - (pos % msgvec);
-		else
-			flag = 0;
-	} while (flag);
+	unsigned int res, bit, val;
 
-	*pos0 = pos;
-	return 0;
+	res = (irq / 32) * 12;
+	bit = irq % 32;
+	dw_pcie_rd_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, &val);
+	val &= ~(1 << bit);
+	dw_pcie_wr_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, val);
 }
 
 static void clear_irq_range(struct pcie_port *pp, unsigned int irq_base,
 			    unsigned int nvec, unsigned int pos)
 {
-	unsigned int i, res, bit, val;
+	unsigned int i;
 
 	for (i = 0; i < nvec; i++) {
 		irq_set_msi_desc_off(irq_base, i, NULL);
-		clear_bit(pos + i, pp->msi_irq_in_use);
 		/* Disable corresponding interrupt on MSI controller */
-		res = ((pos + i) / 32) * 12;
-		bit = (pos + i) % 32;
-		dw_pcie_rd_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, &val);
-		val &= ~(1 << bit);
-		dw_pcie_wr_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, val);
+		if (pp->ops->msi_clear_irq)
+			pp->ops->msi_clear_irq(pp, pos + i);
+		else
+			dw_pcie_msi_clear_irq(pp, pos + i);
 	}
+
+	bitmap_release_region(pp->msi_irq_in_use, pos, order_base_2(nvec));
+}
+
+static void dw_pcie_msi_set_irq(struct pcie_port *pp, int irq)
+{
+	unsigned int res, bit, val;
+
+	res = (irq / 32) * 12;
+	bit = irq % 32;
+	dw_pcie_rd_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, &val);
+	val |= 1 << bit;
+	dw_pcie_wr_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, val);
 }
 
 static int assign_irq(int no_irqs, struct msi_desc *desc, int *pos)
 {
-	int res, bit, irq, pos0, pos1, i;
-	u32 val;
+	int irq, pos0, i;
 	struct pcie_port *pp = sys_to_pcie(desc->dev->bus->sysdata);
 
-	if (!pp) {
-		BUG();
-		return -EINVAL;
-	}
-
-	pos0 = find_first_zero_bit(pp->msi_irq_in_use,
-			MAX_MSI_IRQS);
-	if (pos0 % no_irqs) {
-		if (find_valid_pos0(pp, no_irqs, pos0, &pos0))
-			goto no_valid_irq;
-	}
-	if (no_irqs > 1) {
-		pos1 = find_next_bit(pp->msi_irq_in_use,
-				MAX_MSI_IRQS, pos0);
-		/* there must be nvec number of consecutive free bits */
-		while ((pos1 - pos0) < no_irqs) {
-			if (find_valid_pos0(pp, no_irqs, pos1, &pos0))
-				goto no_valid_irq;
-			pos1 = find_next_bit(pp->msi_irq_in_use,
-					MAX_MSI_IRQS, pos0);
-		}
-	}
+	pos0 = bitmap_find_free_region(pp->msi_irq_in_use, MAX_MSI_IRQS,
+				       order_base_2(no_irqs));
+	if (pos0 < 0)
+		goto no_valid_irq;
 
 	irq = irq_find_mapping(pp->irq_domain, pos0);
 	if (!irq)
@@ -279,13 +261,11 @@ static int assign_irq(int no_irqs, struct msi_desc *desc, int *pos)
 			clear_irq_range(pp, irq, i, pos0);
 			goto no_valid_irq;
 		}
-		set_bit(pos0 + i, pp->msi_irq_in_use);
 		/*Enable corresponding interrupt in MSI interrupt controller */
-		res = ((pos0 + i) / 32) * 12;
-		bit = (pos0 + i) % 32;
-		dw_pcie_rd_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, &val);
-		val |= 1 << bit;
-		dw_pcie_wr_own_conf(pp, PCIE_MSI_INTR0_ENABLE + res, 4, val);
+		if (pp->ops->msi_set_irq)
+			pp->ops->msi_set_irq(pp, pos0 + i);
+		else
+			dw_pcie_msi_set_irq(pp, pos0 + i);
 	}
 
 	*pos = pos0;
@@ -296,66 +276,28 @@ no_valid_irq:
 	return -ENOSPC;
 }
 
-static void clear_irq(unsigned int irq)
-{
-	unsigned int pos, nvec;
-	struct msi_desc *msi;
-	struct pcie_port *pp;
-	struct irq_data *data = irq_get_irq_data(irq);
-
-	/* get the port structure */
-	msi = irq_data_get_msi(data);
-	pp = sys_to_pcie(msi->dev->bus->sysdata);
-	if (!pp) {
-		BUG();
-		return;
-	}
-
-	/* undo what was done in assign_irq */
-	pos = data->hwirq;
-	nvec = 1 << msi->msi_attrib.multiple;
-
-	clear_irq_range(pp, irq, nvec, pos);
-
-	/* all irqs cleared; reset attributes */
-	msi->irq = 0;
-	msi->msi_attrib.multiple = 0;
-}
-
 static int dw_msi_setup_irq(struct msi_chip *chip, struct pci_dev *pdev,
 			struct msi_desc *desc)
 {
-	int irq, pos, msgvec;
-	u16 msg_ctr;
+	int irq, pos;
 	struct msi_msg msg;
 	struct pcie_port *pp = sys_to_pcie(pdev->bus->sysdata);
 
-	if (!pp) {
-		BUG();
-		return -EINVAL;
-	}
-
-	pci_read_config_word(pdev, desc->msi_attrib.pos+PCI_MSI_FLAGS,
-				&msg_ctr);
-	msgvec = (msg_ctr&PCI_MSI_FLAGS_QSIZE) >> 4;
-	if (msgvec == 0)
-		msgvec = (msg_ctr & PCI_MSI_FLAGS_QMASK) >> 1;
-	if (msgvec > 5)
-		msgvec = 0;
-
-	irq = assign_irq((1 << msgvec), desc, &pos);
+	irq = assign_irq(1, desc, &pos);
 	if (irq < 0)
 		return irq;
 
-	/*
-	 * write_msi_msg() will update PCI_MSI_FLAGS so there is
-	 * no need to explicitly call pci_write_config_word().
-	 */
-	desc->msi_attrib.multiple = msgvec;
-
-	msg.address_lo = virt_to_phys((void *)pp->msi_data);
+	if (pp->ops->get_msi_addr)
+		msg.address_lo = pp->ops->get_msi_addr(pp);
+	else
+		msg.address_lo = virt_to_phys((void *)pp->msi_data);
 	msg.address_hi = 0x0;
-	msg.data = pos;
+
+	if (pp->ops->get_msi_data)
+		msg.data = pp->ops->get_msi_data(pp, pos);
+	else
+		msg.data = pos;
+
 	write_msi_msg(irq, &msg);
 
 	return 0;
@@ -363,7 +305,11 @@ static int dw_msi_setup_irq(struct msi_chip *chip, struct pci_dev *pdev,
 
 static void dw_msi_teardown_irq(struct msi_chip *chip, unsigned int irq)
 {
-	clear_irq(irq);
+	struct irq_data *data = irq_get_irq_data(irq);
+	struct msi_desc *msi = irq_data_get_msi(data);
+	struct pcie_port *pp = sys_to_pcie(msi->dev->bus->sysdata);
+
+	clear_irq_range(pp, irq, 1, data->hwirq);
 }
 
 static struct msi_chip dw_pcie_msi_chip = {
@@ -396,10 +342,35 @@ static const struct irq_domain_ops msi_domain_ops = {
 int __init dw_pcie_host_init(struct pcie_port *pp)
 {
 	struct device_node *np = pp->dev->of_node;
+	struct platform_device *pdev = to_platform_device(pp->dev);
 	struct of_pci_range range;
 	struct of_pci_range_parser parser;
-	u32 val;
-	int i;
+	struct resource *cfg_res;
+	u32 val, na, ns;
+	const __be32 *addrp;
+	int i, index, ret;
+
+	/* Find the address cell size and the number of cells in order to get
+	 * the untranslated address.
+	 */
+	of_property_read_u32(np, "#address-cells", &na);
+	ns = of_n_size_cells(np);
+
+	cfg_res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "config");
+	if (cfg_res) {
+		pp->cfg0_size = resource_size(cfg_res)/2;
+		pp->cfg1_size = resource_size(cfg_res)/2;
+		pp->cfg0_base = cfg_res->start;
+		pp->cfg1_base = cfg_res->start + pp->cfg0_size;
+
+		/* Find the untranslated configuration space address */
+		index = of_property_match_string(np, "reg-names", "config");
+		addrp = of_get_address(np, index, NULL, NULL);
+		pp->cfg0_mod_base = of_read_number(addrp, ns);
+		pp->cfg1_mod_base = pp->cfg0_mod_base + pp->cfg0_size;
+	} else {
+		dev_err(pp->dev, "missing *config* reg space\n");
+	}
 
 	if (of_pci_range_parser_init(&parser, np)) {
 		dev_err(pp->dev, "missing ranges property\n");
@@ -418,24 +389,50 @@ int __init dw_pcie_host_init(struct pcie_port *pp)
 			pp->io.end = min_t(resource_size_t,
 					   IO_SPACE_LIMIT,
 					   range.pci_addr + range.size
-					   + global_io_offset);
-			pp->config.io_size = resource_size(&pp->io);
-			pp->config.io_bus_addr = range.pci_addr;
+					   + global_io_offset - 1);
+			pp->io_size = resource_size(&pp->io);
+			pp->io_bus_addr = range.pci_addr;
 			pp->io_base = range.cpu_addr;
+
+			/* Find the untranslated IO space address */
+			pp->io_mod_base = of_read_number(parser.range -
+							 parser.np + na, ns);
 		}
 		if (restype == IORESOURCE_MEM) {
 			of_pci_range_to_resource(&range, np, &pp->mem);
 			pp->mem.name = "MEM";
-			pp->config.mem_size = resource_size(&pp->mem);
-			pp->config.mem_bus_addr = range.pci_addr;
+			pp->mem_size = resource_size(&pp->mem);
+			pp->mem_bus_addr = range.pci_addr;
+
+			/* Find the untranslated MEM space address */
+			pp->mem_mod_base = of_read_number(parser.range -
+							  parser.np + na, ns);
 		}
 		if (restype == 0) {
 			of_pci_range_to_resource(&range, np, &pp->cfg);
-			pp->config.cfg0_size = resource_size(&pp->cfg)/2;
-			pp->config.cfg1_size = resource_size(&pp->cfg)/2;
+			pp->cfg0_size = resource_size(&pp->cfg)/2;
+			pp->cfg1_size = resource_size(&pp->cfg)/2;
+			pp->cfg0_base = pp->cfg.start;
+			pp->cfg1_base = pp->cfg.start + pp->cfg0_size;
+
+			/* Find the untranslated configuration space address */
+			pp->cfg0_mod_base = of_read_number(parser.range -
+							   parser.np + na, ns);
+			pp->cfg1_mod_base = pp->cfg0_mod_base +
+					    pp->cfg0_size;
 		}
 	}
 
+	ret = of_pci_parse_bus_range(np, &pp->busn);
+	if (ret < 0) {
+		pp->busn.name = np->name;
+		pp->busn.start = 0;
+		pp->busn.end = 0xff;
+		pp->busn.flags = IORESOURCE_BUS;
+		dev_dbg(pp->dev, "failed to parse bus-range property: %d, using default %pR\n",
+			ret, &pp->busn);
+	}
+
 	if (!pp->dbi_base) {
 		pp->dbi_base = devm_ioremap(pp->dev, pp->cfg.start,
 					resource_size(&pp->cfg));
@@ -445,21 +442,24 @@ int __init dw_pcie_host_init(struct pcie_port *pp)
 		}
 	}
 
-	pp->cfg0_base = pp->cfg.start;
-	pp->cfg1_base = pp->cfg.start + pp->config.cfg0_size;
 	pp->mem_base = pp->mem.start;
 
-	pp->va_cfg0_base = devm_ioremap(pp->dev, pp->cfg0_base,
-					pp->config.cfg0_size);
 	if (!pp->va_cfg0_base) {
-		dev_err(pp->dev, "error with ioremap in function\n");
-		return -ENOMEM;
+		pp->va_cfg0_base = devm_ioremap(pp->dev, pp->cfg0_base,
+						pp->cfg0_size);
+		if (!pp->va_cfg0_base) {
+			dev_err(pp->dev, "error with ioremap in function\n");
+			return -ENOMEM;
+		}
 	}
-	pp->va_cfg1_base = devm_ioremap(pp->dev, pp->cfg1_base,
-					pp->config.cfg1_size);
+
 	if (!pp->va_cfg1_base) {
-		dev_err(pp->dev, "error with ioremap\n");
-		return -ENOMEM;
+		pp->va_cfg1_base = devm_ioremap(pp->dev, pp->cfg1_base,
+						pp->cfg1_size);
+		if (!pp->va_cfg1_base) {
+			dev_err(pp->dev, "error with ioremap\n");
+			return -ENOMEM;
+		}
 	}
 
 	if (of_property_read_u32(np, "num-lanes", &pp->lanes)) {
@@ -468,16 +468,22 @@ int __init dw_pcie_host_init(struct pcie_port *pp)
 	}
 
 	if (IS_ENABLED(CONFIG_PCI_MSI)) {
-		pp->irq_domain = irq_domain_add_linear(pp->dev->of_node,
-					MAX_MSI_IRQS, &msi_domain_ops,
-					&dw_pcie_msi_chip);
-		if (!pp->irq_domain) {
-			dev_err(pp->dev, "irq domain init failed\n");
-			return -ENXIO;
-		}
+		if (!pp->ops->msi_host_init) {
+			pp->irq_domain = irq_domain_add_linear(pp->dev->of_node,
+						MAX_MSI_IRQS, &msi_domain_ops,
+						&dw_pcie_msi_chip);
+			if (!pp->irq_domain) {
+				dev_err(pp->dev, "irq domain init failed\n");
+				return -ENXIO;
+			}
 
-		for (i = 0; i < MAX_MSI_IRQS; i++)
-			irq_create_mapping(pp->irq_domain, i);
+			for (i = 0; i < MAX_MSI_IRQS; i++)
+				irq_create_mapping(pp->irq_domain, i);
+		} else {
+			ret = pp->ops->msi_host_init(pp, &dw_pcie_msi_chip);
+			if (ret < 0)
+				return ret;
+		}
 	}
 
 	if (pp->ops->host_init)
@@ -496,7 +502,6 @@ int __init dw_pcie_host_init(struct pcie_port *pp)
 	dw_pci.private_data = (void **)&pp;
 
 	pci_common_init_dev(pp->dev, &dw_pci);
-	pci_assign_unassigned_resources();
 #ifdef CONFIG_PCI_DOMAINS
 	dw_pci.domain++;
 #endif
@@ -509,9 +514,9 @@ static void dw_pcie_prog_viewport_cfg0(struct pcie_port *pp, u32 busdev)
 	/* Program viewport 0 : OUTBOUND : CFG0 */
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX0,
 			  PCIE_ATU_VIEWPORT);
-	dw_pcie_writel_rc(pp, pp->cfg0_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->cfg0_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->cfg0_base + pp->config.cfg0_size - 1,
+	dw_pcie_writel_rc(pp, pp->cfg0_mod_base, PCIE_ATU_LOWER_BASE);
+	dw_pcie_writel_rc(pp, (pp->cfg0_mod_base >> 32), PCIE_ATU_UPPER_BASE);
+	dw_pcie_writel_rc(pp, pp->cfg0_mod_base + pp->cfg0_size - 1,
 			  PCIE_ATU_LIMIT);
 	dw_pcie_writel_rc(pp, busdev, PCIE_ATU_LOWER_TARGET);
 	dw_pcie_writel_rc(pp, 0, PCIE_ATU_UPPER_TARGET);
@@ -525,9 +530,9 @@ static void dw_pcie_prog_viewport_cfg1(struct pcie_port *pp, u32 busdev)
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX1,
 			  PCIE_ATU_VIEWPORT);
 	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_CFG1, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, pp->cfg1_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->cfg1_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->cfg1_base + pp->config.cfg1_size - 1,
+	dw_pcie_writel_rc(pp, pp->cfg1_mod_base, PCIE_ATU_LOWER_BASE);
+	dw_pcie_writel_rc(pp, (pp->cfg1_mod_base >> 32), PCIE_ATU_UPPER_BASE);
+	dw_pcie_writel_rc(pp, pp->cfg1_mod_base + pp->cfg1_size - 1,
 			  PCIE_ATU_LIMIT);
 	dw_pcie_writel_rc(pp, busdev, PCIE_ATU_LOWER_TARGET);
 	dw_pcie_writel_rc(pp, 0, PCIE_ATU_UPPER_TARGET);
@@ -540,12 +545,12 @@ static void dw_pcie_prog_viewport_mem_outbound(struct pcie_port *pp)
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX0,
 			  PCIE_ATU_VIEWPORT);
 	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_MEM, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, pp->mem_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->mem_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->mem_base + pp->config.mem_size - 1,
+	dw_pcie_writel_rc(pp, pp->mem_mod_base, PCIE_ATU_LOWER_BASE);
+	dw_pcie_writel_rc(pp, (pp->mem_mod_base >> 32), PCIE_ATU_UPPER_BASE);
+	dw_pcie_writel_rc(pp, pp->mem_mod_base + pp->mem_size - 1,
 			  PCIE_ATU_LIMIT);
-	dw_pcie_writel_rc(pp, pp->config.mem_bus_addr, PCIE_ATU_LOWER_TARGET);
-	dw_pcie_writel_rc(pp, upper_32_bits(pp->config.mem_bus_addr),
+	dw_pcie_writel_rc(pp, pp->mem_bus_addr, PCIE_ATU_LOWER_TARGET);
+	dw_pcie_writel_rc(pp, upper_32_bits(pp->mem_bus_addr),
 			  PCIE_ATU_UPPER_TARGET);
 	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 }
@@ -556,12 +561,12 @@ static void dw_pcie_prog_viewport_io_outbound(struct pcie_port *pp)
 	dw_pcie_writel_rc(pp, PCIE_ATU_REGION_OUTBOUND | PCIE_ATU_REGION_INDEX1,
 			  PCIE_ATU_VIEWPORT);
 	dw_pcie_writel_rc(pp, PCIE_ATU_TYPE_IO, PCIE_ATU_CR1);
-	dw_pcie_writel_rc(pp, pp->io_base, PCIE_ATU_LOWER_BASE);
-	dw_pcie_writel_rc(pp, (pp->io_base >> 32), PCIE_ATU_UPPER_BASE);
-	dw_pcie_writel_rc(pp, pp->io_base + pp->config.io_size - 1,
+	dw_pcie_writel_rc(pp, pp->io_mod_base, PCIE_ATU_LOWER_BASE);
+	dw_pcie_writel_rc(pp, (pp->io_mod_base >> 32), PCIE_ATU_UPPER_BASE);
+	dw_pcie_writel_rc(pp, pp->io_mod_base + pp->io_size - 1,
 			  PCIE_ATU_LIMIT);
-	dw_pcie_writel_rc(pp, pp->config.io_bus_addr, PCIE_ATU_LOWER_TARGET);
-	dw_pcie_writel_rc(pp, upper_32_bits(pp->config.io_bus_addr),
+	dw_pcie_writel_rc(pp, pp->io_bus_addr, PCIE_ATU_LOWER_TARGET);
+	dw_pcie_writel_rc(pp, upper_32_bits(pp->io_bus_addr),
 			  PCIE_ATU_UPPER_TARGET);
 	dw_pcie_writel_rc(pp, PCIE_ATU_ENABLE, PCIE_ATU_CR2);
 }
@@ -645,18 +650,17 @@ static int dw_pcie_rd_conf(struct pci_bus *bus, u32 devfn, int where,
 	struct pcie_port *pp = sys_to_pcie(bus->sysdata);
 	int ret;
 
-	if (!pp) {
-		BUG();
-		return -EINVAL;
-	}
-
 	if (dw_pcie_valid_config(pp, bus, PCI_SLOT(devfn)) == 0) {
 		*val = 0xffffffff;
 		return PCIBIOS_DEVICE_NOT_FOUND;
 	}
 
 	if (bus->number != pp->root_bus_nr)
-		ret = dw_pcie_rd_other_conf(pp, bus, devfn,
+		if (pp->ops->rd_other_conf)
+			ret = pp->ops->rd_other_conf(pp, bus, devfn,
+						where, size, val);
+		else
+			ret = dw_pcie_rd_other_conf(pp, bus, devfn,
 						where, size, val);
 	else
 		ret = dw_pcie_rd_own_conf(pp, where, size, val);
@@ -670,16 +674,15 @@ static int dw_pcie_wr_conf(struct pci_bus *bus, u32 devfn,
 	struct pcie_port *pp = sys_to_pcie(bus->sysdata);
 	int ret;
 
-	if (!pp) {
-		BUG();
-		return -EINVAL;
-	}
-
 	if (dw_pcie_valid_config(pp, bus, PCI_SLOT(devfn)) == 0)
 		return PCIBIOS_DEVICE_NOT_FOUND;
 
 	if (bus->number != pp->root_bus_nr)
-		ret = dw_pcie_wr_other_conf(pp, bus, devfn,
+		if (pp->ops->wr_other_conf)
+			ret = pp->ops->wr_other_conf(pp, bus, devfn,
+						where, size, val);
+		else
+			ret = dw_pcie_wr_other_conf(pp, bus, devfn,
 						where, size, val);
 	else
 		ret = dw_pcie_wr_own_conf(pp, where, size, val);
@@ -698,19 +701,17 @@ static int dw_pcie_setup(int nr, struct pci_sys_data *sys)
 
 	pp = sys_to_pcie(sys);
 
-	if (!pp)
-		return 0;
-
-	if (global_io_offset < SZ_1M && pp->config.io_size > 0) {
-		sys->io_offset = global_io_offset - pp->config.io_bus_addr;
+	if (global_io_offset < SZ_1M && pp->io_size > 0) {
+		sys->io_offset = global_io_offset - pp->io_bus_addr;
 		pci_ioremap_io(global_io_offset, pp->io_base);
 		global_io_offset += SZ_64K;
 		pci_add_resource_offset(&sys->resources, &pp->io,
 					sys->io_offset);
 	}
 
-	sys->mem_offset = pp->mem.start - pp->config.mem_bus_addr;
+	sys->mem_offset = pp->mem.start - pp->mem_bus_addr;
 	pci_add_resource_offset(&sys->resources, &pp->mem, sys->mem_offset);
+	pci_add_resource(&sys->resources, &pp->busn);
 
 	return 1;
 }
@@ -720,14 +721,16 @@ static struct pci_bus *dw_pcie_scan_bus(int nr, struct pci_sys_data *sys)
 	struct pci_bus *bus;
 	struct pcie_port *pp = sys_to_pcie(sys);
 
-	if (pp) {
-		pp->root_bus_nr = sys->busnr;
-		bus = pci_scan_root_bus(pp->dev, sys->busnr, &dw_pcie_ops,
-					sys, &sys->resources);
-	} else {
-		bus = NULL;
-		BUG();
-	}
+	pp->root_bus_nr = sys->busnr;
+	bus = pci_create_root_bus(pp->dev, sys->busnr,
+				  &dw_pcie_ops, sys, &sys->resources);
+	if (!bus)
+		return NULL;
+
+	pci_scan_child_bus(bus);
+
+	if (bus && pp->ops->scan_bus)
+		pp->ops->scan_bus(pp);
 
 	return bus;
 }
@@ -763,7 +766,6 @@ static struct hw_pci dw_pci = {
 
 void dw_pcie_setup_rc(struct pcie_port *pp)
 {
-	struct pcie_port_info *config = &pp->config;
 	u32 val;
 	u32 membase;
 	u32 memlimit;
@@ -818,7 +820,7 @@ void dw_pcie_setup_rc(struct pcie_port *pp)
 
 	/* setup memory base, memory limit */
 	membase = ((u32)pp->mem_base & 0xfff00000) >> 16;
-	memlimit = (config->mem_size + (u32)pp->mem_base) & 0xfff00000;
+	memlimit = (pp->mem_size + (u32)pp->mem_base) & 0xfff00000;
 	val = memlimit | membase;
 	dw_pcie_writel_rc(pp, val, PCI_MEMORY_BASE);
 
diff --git a/drivers/pci/host/pcie-designware.h b/drivers/pci/host/pcie-designware.h
index 77f592faa7bf..c6256751daff 100644
--- a/drivers/pci/host/pcie-designware.h
+++ b/drivers/pci/host/pcie-designware.h
@@ -14,15 +14,6 @@
 #ifndef _PCIE_DESIGNWARE_H
 #define _PCIE_DESIGNWARE_H
 
-struct pcie_port_info {
-	u32		cfg0_size;
-	u32		cfg1_size;
-	u32		io_size;
-	u32		mem_size;
-	phys_addr_t	io_bus_addr;
-	phys_addr_t	mem_bus_addr;
-};
-
 /*
  * Maximum number of MSI IRQs can be 256 per controller. But keep
  * it 32 as of now. Probably we will never need more than 32. If needed,
@@ -36,15 +27,25 @@ struct pcie_port {
 	u8			root_bus_nr;
 	void __iomem		*dbi_base;
 	u64			cfg0_base;
+	u64			cfg0_mod_base;
 	void __iomem		*va_cfg0_base;
+	u32			cfg0_size;
 	u64			cfg1_base;
+	u64			cfg1_mod_base;
 	void __iomem		*va_cfg1_base;
+	u32			cfg1_size;
 	u64			io_base;
+	u64			io_mod_base;
+	phys_addr_t		io_bus_addr;
+	u32			io_size;
 	u64			mem_base;
+	u64			mem_mod_base;
+	phys_addr_t		mem_bus_addr;
+	u32			mem_size;
 	struct resource		cfg;
 	struct resource		io;
 	struct resource		mem;
-	struct pcie_port_info	config;
+	struct resource		busn;
 	int			irq;
 	u32			lanes;
 	struct pcie_host_ops	*ops;
@@ -61,8 +62,18 @@ struct pcie_host_ops {
 			u32 val, void __iomem *dbi_base);
 	int (*rd_own_conf)(struct pcie_port *pp, int where, int size, u32 *val);
 	int (*wr_own_conf)(struct pcie_port *pp, int where, int size, u32 val);
+	int (*rd_other_conf)(struct pcie_port *pp, struct pci_bus *bus,
+			unsigned int devfn, int where, int size, u32 *val);
+	int (*wr_other_conf)(struct pcie_port *pp, struct pci_bus *bus,
+			unsigned int devfn, int where, int size, u32 val);
 	int (*link_up)(struct pcie_port *pp);
 	void (*host_init)(struct pcie_port *pp);
+	void (*msi_set_irq)(struct pcie_port *pp, int irq);
+	void (*msi_clear_irq)(struct pcie_port *pp, int irq);
+	u32 (*get_msi_addr)(struct pcie_port *pp);
+	u32 (*get_msi_data)(struct pcie_port *pp, int pos);
+	void (*scan_bus)(struct pcie_port *pp);
+	int (*msi_host_init)(struct pcie_port *pp, struct msi_chip *chip);
 };
 
 int dw_pcie_cfg_read(void __iomem *addr, int where, int size, u32 *val);
diff --git a/drivers/pci/host/pcie-rcar.c b/drivers/pci/host/pcie-rcar.c
index 4884ee5e07d4..61158e03ab5f 100644
--- a/drivers/pci/host/pcie-rcar.c
+++ b/drivers/pci/host/pcie-rcar.c
@@ -323,6 +323,7 @@ static void rcar_pcie_setup_window(int win, struct rcar_pcie *pcie)
 
 	/* Setup PCIe address space mappings for each resource */
 	resource_size_t size;
+	resource_size_t res_start;
 	u32 mask;
 
 	rcar_pci_write_reg(pcie, 0x00000000, PCIEPTCTLR(win));
@@ -335,8 +336,13 @@ static void rcar_pcie_setup_window(int win, struct rcar_pcie *pcie)
 	mask = (roundup_pow_of_two(size) / SZ_128) - 1;
 	rcar_pci_write_reg(pcie, mask << 7, PCIEPAMR(win));
 
-	rcar_pci_write_reg(pcie, upper_32_bits(res->start), PCIEPARH(win));
-	rcar_pci_write_reg(pcie, lower_32_bits(res->start), PCIEPARL(win));
+	if (res->flags & IORESOURCE_IO)
+		res_start = pci_pio_to_address(res->start);
+	else
+		res_start = res->start;
+
+	rcar_pci_write_reg(pcie, upper_32_bits(res_start), PCIEPARH(win));
+	rcar_pci_write_reg(pcie, lower_32_bits(res_start), PCIEPARL(win));
 
 	/* First resource is for IO */
 	mask = PAR_ENABLE;
@@ -363,9 +369,10 @@ static int rcar_pcie_setup(int nr, struct pci_sys_data *sys)
 
 		rcar_pcie_setup_window(i, pcie);
 
-		if (res->flags & IORESOURCE_IO)
-			pci_ioremap_io(nr * SZ_64K, res->start);
-		else
+		if (res->flags & IORESOURCE_IO) {
+			phys_addr_t io_start = pci_pio_to_address(res->start);
+			pci_ioremap_io(nr * SZ_64K, io_start);
+		} else
 			pci_add_resource(&sys->resources, res);
 	}
 	pci_add_resource(&sys->resources, &pcie->busn);
@@ -935,8 +942,10 @@ static int rcar_pcie_probe(struct platform_device *pdev)
 	}
 
 	for_each_of_pci_range(&parser, &range) {
-		of_pci_range_to_resource(&range, pdev->dev.of_node,
+		err = of_pci_range_to_resource(&range, pdev->dev.of_node,
 						&pcie->res[win++]);
+		if (err < 0)
+			return err;
 
 		if (win > RCAR_PCI_MAX_RESOURCES)
 			break;
diff --git a/drivers/pci/host/pcie-spear13xx.c b/drivers/pci/host/pcie-spear13xx.c
index 6dea9e43a75c..85f594e1708f 100644
--- a/drivers/pci/host/pcie-spear13xx.c
+++ b/drivers/pci/host/pcie-spear13xx.c
@@ -340,7 +340,7 @@ static int __init spear13xx_pcie_probe(struct platform_device *pdev)
 
 	pp->dev = dev;
 
-	dbi_base = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	dbi_base = platform_get_resource_byname(pdev, IORESOURCE_MEM, "dbi");
 	pp->dbi_base = devm_ioremap_resource(dev, dbi_base);
 	if (IS_ERR(pp->dbi_base)) {
 		dev_err(dev, "couldn't remap dbi base %p\n", dbi_base);
diff --git a/drivers/pci/host/pcie-xilinx.c b/drivers/pci/host/pcie-xilinx.c
new file mode 100644
index 000000000000..ccc496b33a97
--- /dev/null
+++ b/drivers/pci/host/pcie-xilinx.c
@@ -0,0 +1,970 @@
+/*
+ * PCIe host controller driver for Xilinx AXI PCIe Bridge
+ *
+ * Copyright (c) 2012 - 2014 Xilinx, Inc.
+ *
+ * Based on the Tegra PCIe driver
+ *
+ * Bits taken from Synopsys Designware Host controller driver and
+ * ARM PCI Host generic driver.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/msi.h>
+#include <linux/of_address.h>
+#include <linux/of_pci.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/pci.h>
+#include <linux/platform_device.h>
+
+/* Register definitions */
+#define XILINX_PCIE_REG_BIR		0x00000130
+#define XILINX_PCIE_REG_IDR		0x00000138
+#define XILINX_PCIE_REG_IMR		0x0000013c
+#define XILINX_PCIE_REG_PSCR		0x00000144
+#define XILINX_PCIE_REG_RPSC		0x00000148
+#define XILINX_PCIE_REG_MSIBASE1	0x0000014c
+#define XILINX_PCIE_REG_MSIBASE2	0x00000150
+#define XILINX_PCIE_REG_RPEFR		0x00000154
+#define XILINX_PCIE_REG_RPIFR1		0x00000158
+#define XILINX_PCIE_REG_RPIFR2		0x0000015c
+
+/* Interrupt registers definitions */
+#define XILINX_PCIE_INTR_LINK_DOWN	BIT(0)
+#define XILINX_PCIE_INTR_ECRC_ERR	BIT(1)
+#define XILINX_PCIE_INTR_STR_ERR	BIT(2)
+#define XILINX_PCIE_INTR_HOT_RESET	BIT(3)
+#define XILINX_PCIE_INTR_CFG_TIMEOUT	BIT(8)
+#define XILINX_PCIE_INTR_CORRECTABLE	BIT(9)
+#define XILINX_PCIE_INTR_NONFATAL	BIT(10)
+#define XILINX_PCIE_INTR_FATAL		BIT(11)
+#define XILINX_PCIE_INTR_INTX		BIT(16)
+#define XILINX_PCIE_INTR_MSI		BIT(17)
+#define XILINX_PCIE_INTR_SLV_UNSUPP	BIT(20)
+#define XILINX_PCIE_INTR_SLV_UNEXP	BIT(21)
+#define XILINX_PCIE_INTR_SLV_COMPL	BIT(22)
+#define XILINX_PCIE_INTR_SLV_ERRP	BIT(23)
+#define XILINX_PCIE_INTR_SLV_CMPABT	BIT(24)
+#define XILINX_PCIE_INTR_SLV_ILLBUR	BIT(25)
+#define XILINX_PCIE_INTR_MST_DECERR	BIT(26)
+#define XILINX_PCIE_INTR_MST_SLVERR	BIT(27)
+#define XILINX_PCIE_INTR_MST_ERRP	BIT(28)
+#define XILINX_PCIE_IMR_ALL_MASK	0x1FF30FED
+#define XILINX_PCIE_IDR_ALL_MASK	0xFFFFFFFF
+
+/* Root Port Error FIFO Read Register definitions */
+#define XILINX_PCIE_RPEFR_ERR_VALID	BIT(18)
+#define XILINX_PCIE_RPEFR_REQ_ID	GENMASK(15, 0)
+#define XILINX_PCIE_RPEFR_ALL_MASK	0xFFFFFFFF
+
+/* Root Port Interrupt FIFO Read Register 1 definitions */
+#define XILINX_PCIE_RPIFR1_INTR_VALID	BIT(31)
+#define XILINX_PCIE_RPIFR1_MSI_INTR	BIT(30)
+#define XILINX_PCIE_RPIFR1_INTR_MASK	GENMASK(28, 27)
+#define XILINX_PCIE_RPIFR1_ALL_MASK	0xFFFFFFFF
+#define XILINX_PCIE_RPIFR1_INTR_SHIFT	27
+
+/* Bridge Info Register definitions */
+#define XILINX_PCIE_BIR_ECAM_SZ_MASK	GENMASK(18, 16)
+#define XILINX_PCIE_BIR_ECAM_SZ_SHIFT	16
+
+/* Root Port Interrupt FIFO Read Register 2 definitions */
+#define XILINX_PCIE_RPIFR2_MSG_DATA	GENMASK(15, 0)
+
+/* Root Port Status/control Register definitions */
+#define XILINX_PCIE_REG_RPSC_BEN	BIT(0)
+
+/* Phy Status/Control Register definitions */
+#define XILINX_PCIE_REG_PSCR_LNKUP	BIT(11)
+
+/* ECAM definitions */
+#define ECAM_BUS_NUM_SHIFT		20
+#define ECAM_DEV_NUM_SHIFT		12
+
+/* Number of MSI IRQs */
+#define XILINX_NUM_MSI_IRQS		128
+
+/* Number of Memory Resources */
+#define XILINX_MAX_NUM_RESOURCES	3
+
+/**
+ * struct xilinx_pcie_port - PCIe port information
+ * @reg_base: IO Mapped Register Base
+ * @irq: Interrupt number
+ * @msi_pages: MSI pages
+ * @root_busno: Root Bus number
+ * @dev: Device pointer
+ * @irq_domain: IRQ domain pointer
+ * @bus_range: Bus range
+ * @resources: Bus Resources
+ */
+struct xilinx_pcie_port {
+	void __iomem *reg_base;
+	u32 irq;
+	unsigned long msi_pages;
+	u8 root_busno;
+	struct device *dev;
+	struct irq_domain *irq_domain;
+	struct resource bus_range;
+	struct list_head resources;
+};
+
+static DECLARE_BITMAP(msi_irq_in_use, XILINX_NUM_MSI_IRQS);
+
+static inline struct xilinx_pcie_port *sys_to_pcie(struct pci_sys_data *sys)
+{
+	return sys->private_data;
+}
+
+static inline u32 pcie_read(struct xilinx_pcie_port *port, u32 reg)
+{
+	return readl(port->reg_base + reg);
+}
+
+static inline void pcie_write(struct xilinx_pcie_port *port, u32 val, u32 reg)
+{
+	writel(val, port->reg_base + reg);
+}
+
+static inline bool xilinx_pcie_link_is_up(struct xilinx_pcie_port *port)
+{
+	return (pcie_read(port, XILINX_PCIE_REG_PSCR) &
+		XILINX_PCIE_REG_PSCR_LNKUP) ? 1 : 0;
+}
+
+/**
+ * xilinx_pcie_clear_err_interrupts - Clear Error Interrupts
+ * @port: PCIe port information
+ */
+static void xilinx_pcie_clear_err_interrupts(struct xilinx_pcie_port *port)
+{
+	u32 val = pcie_read(port, XILINX_PCIE_REG_RPEFR);
+
+	if (val & XILINX_PCIE_RPEFR_ERR_VALID) {
+		dev_dbg(port->dev, "Requester ID %d\n",
+			val & XILINX_PCIE_RPEFR_REQ_ID);
+		pcie_write(port, XILINX_PCIE_RPEFR_ALL_MASK,
+			   XILINX_PCIE_REG_RPEFR);
+	}
+}
+
+/**
+ * xilinx_pcie_valid_device - Check if a valid device is present on bus
+ * @bus: PCI Bus structure
+ * @devfn: device/function
+ *
+ * Return: 'true' on success and 'false' if invalid device is found
+ */
+static bool xilinx_pcie_valid_device(struct pci_bus *bus, unsigned int devfn)
+{
+	struct xilinx_pcie_port *port = sys_to_pcie(bus->sysdata);
+
+	/* Check if link is up when trying to access downstream ports */
+	if (bus->number != port->root_busno)
+		if (!xilinx_pcie_link_is_up(port))
+			return false;
+
+	/* Only one device down on each root port */
+	if (bus->number == port->root_busno && devfn > 0)
+		return false;
+
+	/*
+	 * Do not read more than one device on the bus directly attached
+	 * to RC.
+	 */
+	if (bus->primary == port->root_busno && devfn > 0)
+		return false;
+
+	return true;
+}
+
+/**
+ * xilinx_pcie_config_base - Get configuration base
+ * @bus: PCI Bus structure
+ * @devfn: Device/function
+ * @where: Offset from base
+ *
+ * Return: Base address of the configuration space needed to be
+ *	   accessed.
+ */
+static void __iomem *xilinx_pcie_config_base(struct pci_bus *bus,
+					     unsigned int devfn, int where)
+{
+	struct xilinx_pcie_port *port = sys_to_pcie(bus->sysdata);
+	int relbus;
+
+	relbus = (bus->number << ECAM_BUS_NUM_SHIFT) |
+		 (devfn << ECAM_DEV_NUM_SHIFT);
+
+	return port->reg_base + relbus + where;
+}
+
+/**
+ * xilinx_pcie_read_config - Read configuration space
+ * @bus: PCI Bus structure
+ * @devfn: Device/function
+ * @where: Offset from base
+ * @size: Byte/word/dword
+ * @val: Value to be read
+ *
+ * Return: PCIBIOS_SUCCESSFUL on success
+ *	   PCIBIOS_DEVICE_NOT_FOUND on failure
+ */
+static int xilinx_pcie_read_config(struct pci_bus *bus, unsigned int devfn,
+				   int where, int size, u32 *val)
+{
+	void __iomem *addr;
+
+	if (!xilinx_pcie_valid_device(bus, devfn)) {
+		*val = 0xFFFFFFFF;
+		return PCIBIOS_DEVICE_NOT_FOUND;
+	}
+
+	addr = xilinx_pcie_config_base(bus, devfn, where);
+
+	switch (size) {
+	case 1:
+		*val = readb(addr);
+		break;
+	case 2:
+		*val = readw(addr);
+		break;
+	default:
+		*val = readl(addr);
+		break;
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+/**
+ * xilinx_pcie_write_config - Write configuration space
+ * @bus: PCI Bus structure
+ * @devfn: Device/function
+ * @where: Offset from base
+ * @size: Byte/word/dword
+ * @val: Value to be written to device
+ *
+ * Return: PCIBIOS_SUCCESSFUL on success
+ *	   PCIBIOS_DEVICE_NOT_FOUND on failure
+ */
+static int xilinx_pcie_write_config(struct pci_bus *bus, unsigned int devfn,
+				    int where, int size, u32 val)
+{
+	void __iomem *addr;
+
+	if (!xilinx_pcie_valid_device(bus, devfn))
+		return PCIBIOS_DEVICE_NOT_FOUND;
+
+	addr = xilinx_pcie_config_base(bus, devfn, where);
+
+	switch (size) {
+	case 1:
+		writeb(val, addr);
+		break;
+	case 2:
+		writew(val, addr);
+		break;
+	default:
+		writel(val, addr);
+		break;
+	}
+
+	return PCIBIOS_SUCCESSFUL;
+}
+
+/* PCIe operations */
+static struct pci_ops xilinx_pcie_ops = {
+	.read  = xilinx_pcie_read_config,
+	.write = xilinx_pcie_write_config,
+};
+
+/* MSI functions */
+
+/**
+ * xilinx_pcie_destroy_msi - Free MSI number
+ * @irq: IRQ to be freed
+ */
+static void xilinx_pcie_destroy_msi(unsigned int irq)
+{
+	struct irq_desc *desc;
+	struct msi_desc *msi;
+	struct xilinx_pcie_port *port;
+
+	desc = irq_to_desc(irq);
+	msi = irq_desc_get_msi_desc(desc);
+	port = sys_to_pcie(msi->dev->bus->sysdata);
+
+	if (!test_bit(irq, msi_irq_in_use))
+		dev_err(port->dev, "Trying to free unused MSI#%d\n", irq);
+	else
+		clear_bit(irq, msi_irq_in_use);
+}
+
+/**
+ * xilinx_pcie_assign_msi - Allocate MSI number
+ * @port: PCIe port structure
+ *
+ * Return: A valid IRQ on success and error value on failure.
+ */
+static int xilinx_pcie_assign_msi(struct xilinx_pcie_port *port)
+{
+	int pos;
+
+	pos = find_first_zero_bit(msi_irq_in_use, XILINX_NUM_MSI_IRQS);
+	if (pos < XILINX_NUM_MSI_IRQS)
+		set_bit(pos, msi_irq_in_use);
+	else
+		return -ENOSPC;
+
+	return pos;
+}
+
+/**
+ * xilinx_msi_teardown_irq - Destroy the MSI
+ * @chip: MSI Chip descriptor
+ * @irq: MSI IRQ to destroy
+ */
+static void xilinx_msi_teardown_irq(struct msi_chip *chip, unsigned int irq)
+{
+	xilinx_pcie_destroy_msi(irq);
+}
+
+/**
+ * xilinx_pcie_msi_setup_irq - Setup MSI request
+ * @chip: MSI chip pointer
+ * @pdev: PCIe device pointer
+ * @desc: MSI descriptor pointer
+ *
+ * Return: '0' on success and error value on failure
+ */
+static int xilinx_pcie_msi_setup_irq(struct msi_chip *chip,
+				     struct pci_dev *pdev,
+				     struct msi_desc *desc)
+{
+	struct xilinx_pcie_port *port = sys_to_pcie(pdev->bus->sysdata);
+	unsigned int irq;
+	int hwirq;
+	struct msi_msg msg;
+	phys_addr_t msg_addr;
+
+	hwirq = xilinx_pcie_assign_msi(port);
+	if (hwirq < 0)
+		return hwirq;
+
+	irq = irq_create_mapping(port->irq_domain, hwirq);
+	if (!irq)
+		return -EINVAL;
+
+	irq_set_msi_desc(irq, desc);
+
+	msg_addr = virt_to_phys((void *)port->msi_pages);
+
+	msg.address_hi = 0;
+	msg.address_lo = msg_addr;
+	msg.data = irq;
+
+	write_msi_msg(irq, &msg);
+
+	return 0;
+}
+
+/* MSI Chip Descriptor */
+static struct msi_chip xilinx_pcie_msi_chip = {
+	.setup_irq = xilinx_pcie_msi_setup_irq,
+	.teardown_irq = xilinx_msi_teardown_irq,
+};
+
+/* HW Interrupt Chip Descriptor */
+static struct irq_chip xilinx_msi_irq_chip = {
+	.name = "Xilinx PCIe MSI",
+	.irq_enable = unmask_msi_irq,
+	.irq_disable = mask_msi_irq,
+	.irq_mask = mask_msi_irq,
+	.irq_unmask = unmask_msi_irq,
+};
+
+/**
+ * xilinx_pcie_msi_map - Set the handler for the MSI and mark IRQ as valid
+ * @domain: IRQ domain
+ * @irq: Virtual IRQ number
+ * @hwirq: HW interrupt number
+ *
+ * Return: Always returns 0.
+ */
+static int xilinx_pcie_msi_map(struct irq_domain *domain, unsigned int irq,
+			       irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &xilinx_msi_irq_chip, handle_simple_irq);
+	irq_set_chip_data(irq, domain->host_data);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+/* IRQ Domain operations */
+static const struct irq_domain_ops msi_domain_ops = {
+	.map = xilinx_pcie_msi_map,
+};
+
+/**
+ * xilinx_pcie_enable_msi - Enable MSI support
+ * @port: PCIe port information
+ */
+static void xilinx_pcie_enable_msi(struct xilinx_pcie_port *port)
+{
+	phys_addr_t msg_addr;
+
+	port->msi_pages = __get_free_pages(GFP_KERNEL, 0);
+	msg_addr = virt_to_phys((void *)port->msi_pages);
+	pcie_write(port, 0x0, XILINX_PCIE_REG_MSIBASE1);
+	pcie_write(port, msg_addr, XILINX_PCIE_REG_MSIBASE2);
+}
+
+/**
+ * xilinx_pcie_add_bus - Add MSI chip info to PCIe bus
+ * @bus: PCIe bus
+ */
+static void xilinx_pcie_add_bus(struct pci_bus *bus)
+{
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		struct xilinx_pcie_port *port = sys_to_pcie(bus->sysdata);
+
+		xilinx_pcie_msi_chip.dev = port->dev;
+		bus->msi = &xilinx_pcie_msi_chip;
+	}
+}
+
+/* INTx Functions */
+
+/**
+ * xilinx_pcie_intx_map - Set the handler for the INTx and mark IRQ as valid
+ * @domain: IRQ domain
+ * @irq: Virtual IRQ number
+ * @hwirq: HW interrupt number
+ *
+ * Return: Always returns 0.
+ */
+static int xilinx_pcie_intx_map(struct irq_domain *domain, unsigned int irq,
+				irq_hw_number_t hwirq)
+{
+	irq_set_chip_and_handler(irq, &dummy_irq_chip, handle_simple_irq);
+	irq_set_chip_data(irq, domain->host_data);
+	set_irq_flags(irq, IRQF_VALID);
+
+	return 0;
+}
+
+/* INTx IRQ Domain operations */
+static const struct irq_domain_ops intx_domain_ops = {
+	.map = xilinx_pcie_intx_map,
+};
+
+/* PCIe HW Functions */
+
+/**
+ * xilinx_pcie_intr_handler - Interrupt Service Handler
+ * @irq: IRQ number
+ * @data: PCIe port information
+ *
+ * Return: IRQ_HANDLED on success and IRQ_NONE on failure
+ */
+static irqreturn_t xilinx_pcie_intr_handler(int irq, void *data)
+{
+	struct xilinx_pcie_port *port = (struct xilinx_pcie_port *)data;
+	u32 val, mask, status, msi_data;
+
+	/* Read interrupt decode and mask registers */
+	val = pcie_read(port, XILINX_PCIE_REG_IDR);
+	mask = pcie_read(port, XILINX_PCIE_REG_IMR);
+
+	status = val & mask;
+	if (!status)
+		return IRQ_NONE;
+
+	if (status & XILINX_PCIE_INTR_LINK_DOWN)
+		dev_warn(port->dev, "Link Down\n");
+
+	if (status & XILINX_PCIE_INTR_ECRC_ERR)
+		dev_warn(port->dev, "ECRC failed\n");
+
+	if (status & XILINX_PCIE_INTR_STR_ERR)
+		dev_warn(port->dev, "Streaming error\n");
+
+	if (status & XILINX_PCIE_INTR_HOT_RESET)
+		dev_info(port->dev, "Hot reset\n");
+
+	if (status & XILINX_PCIE_INTR_CFG_TIMEOUT)
+		dev_warn(port->dev, "ECAM access timeout\n");
+
+	if (status & XILINX_PCIE_INTR_CORRECTABLE) {
+		dev_warn(port->dev, "Correctable error message\n");
+		xilinx_pcie_clear_err_interrupts(port);
+	}
+
+	if (status & XILINX_PCIE_INTR_NONFATAL) {
+		dev_warn(port->dev, "Non fatal error message\n");
+		xilinx_pcie_clear_err_interrupts(port);
+	}
+
+	if (status & XILINX_PCIE_INTR_FATAL) {
+		dev_warn(port->dev, "Fatal error message\n");
+		xilinx_pcie_clear_err_interrupts(port);
+	}
+
+	if (status & XILINX_PCIE_INTR_INTX) {
+		/* INTx interrupt received */
+		val = pcie_read(port, XILINX_PCIE_REG_RPIFR1);
+
+		/* Check whether interrupt valid */
+		if (!(val & XILINX_PCIE_RPIFR1_INTR_VALID)) {
+			dev_warn(port->dev, "RP Intr FIFO1 read error\n");
+			return IRQ_HANDLED;
+		}
+
+		/* Clear interrupt FIFO register 1 */
+		pcie_write(port, XILINX_PCIE_RPIFR1_ALL_MASK,
+			   XILINX_PCIE_REG_RPIFR1);
+
+		/* Handle INTx Interrupt */
+		val = ((val & XILINX_PCIE_RPIFR1_INTR_MASK) >>
+			XILINX_PCIE_RPIFR1_INTR_SHIFT) + 1;
+		generic_handle_irq(irq_find_mapping(port->irq_domain, val));
+	}
+
+	if (status & XILINX_PCIE_INTR_MSI) {
+		/* MSI Interrupt */
+		val = pcie_read(port, XILINX_PCIE_REG_RPIFR1);
+
+		if (!(val & XILINX_PCIE_RPIFR1_INTR_VALID)) {
+			dev_warn(port->dev, "RP Intr FIFO1 read error\n");
+			return IRQ_HANDLED;
+		}
+
+		if (val & XILINX_PCIE_RPIFR1_MSI_INTR) {
+			msi_data = pcie_read(port, XILINX_PCIE_REG_RPIFR2) &
+				   XILINX_PCIE_RPIFR2_MSG_DATA;
+
+			/* Clear interrupt FIFO register 1 */
+			pcie_write(port, XILINX_PCIE_RPIFR1_ALL_MASK,
+				   XILINX_PCIE_REG_RPIFR1);
+
+			if (IS_ENABLED(CONFIG_PCI_MSI)) {
+				/* Handle MSI Interrupt */
+				generic_handle_irq(msi_data);
+			}
+		}
+	}
+
+	if (status & XILINX_PCIE_INTR_SLV_UNSUPP)
+		dev_warn(port->dev, "Slave unsupported request\n");
+
+	if (status & XILINX_PCIE_INTR_SLV_UNEXP)
+		dev_warn(port->dev, "Slave unexpected completion\n");
+
+	if (status & XILINX_PCIE_INTR_SLV_COMPL)
+		dev_warn(port->dev, "Slave completion timeout\n");
+
+	if (status & XILINX_PCIE_INTR_SLV_ERRP)
+		dev_warn(port->dev, "Slave Error Poison\n");
+
+	if (status & XILINX_PCIE_INTR_SLV_CMPABT)
+		dev_warn(port->dev, "Slave Completer Abort\n");
+
+	if (status & XILINX_PCIE_INTR_SLV_ILLBUR)
+		dev_warn(port->dev, "Slave Illegal Burst\n");
+
+	if (status & XILINX_PCIE_INTR_MST_DECERR)
+		dev_warn(port->dev, "Master decode error\n");
+
+	if (status & XILINX_PCIE_INTR_MST_SLVERR)
+		dev_warn(port->dev, "Master slave error\n");
+
+	if (status & XILINX_PCIE_INTR_MST_ERRP)
+		dev_warn(port->dev, "Master error poison\n");
+
+	/* Clear the Interrupt Decode register */
+	pcie_write(port, status, XILINX_PCIE_REG_IDR);
+
+	return IRQ_HANDLED;
+}
+
+/**
+ * xilinx_pcie_free_irq_domain - Free IRQ domain
+ * @port: PCIe port information
+ */
+static void xilinx_pcie_free_irq_domain(struct xilinx_pcie_port *port)
+{
+	int i;
+	u32 irq, num_irqs;
+
+	/* Free IRQ Domain */
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+
+		free_pages(port->msi_pages, 0);
+
+		num_irqs = XILINX_NUM_MSI_IRQS;
+	} else {
+		/* INTx */
+		num_irqs = 4;
+	}
+
+	for (i = 0; i < num_irqs; i++) {
+		irq = irq_find_mapping(port->irq_domain, i);
+		if (irq > 0)
+			irq_dispose_mapping(irq);
+	}
+
+	irq_domain_remove(port->irq_domain);
+}
+
+/**
+ * xilinx_pcie_init_irq_domain - Initialize IRQ domain
+ * @port: PCIe port information
+ *
+ * Return: '0' on success and error value on failure
+ */
+static int xilinx_pcie_init_irq_domain(struct xilinx_pcie_port *port)
+{
+	struct device *dev = port->dev;
+	struct device_node *node = dev->of_node;
+	struct device_node *pcie_intc_node;
+
+	/* Setup INTx */
+	pcie_intc_node = of_get_next_child(node, NULL);
+	if (!pcie_intc_node) {
+		dev_err(dev, "No PCIe Intc node found\n");
+		return PTR_ERR(pcie_intc_node);
+	}
+
+	port->irq_domain = irq_domain_add_linear(pcie_intc_node, 4,
+						 &intx_domain_ops,
+						 port);
+	if (!port->irq_domain) {
+		dev_err(dev, "Failed to get a INTx IRQ domain\n");
+		return PTR_ERR(port->irq_domain);
+	}
+
+	/* Setup MSI */
+	if (IS_ENABLED(CONFIG_PCI_MSI)) {
+		port->irq_domain = irq_domain_add_linear(node,
+							 XILINX_NUM_MSI_IRQS,
+							 &msi_domain_ops,
+							 &xilinx_pcie_msi_chip);
+		if (!port->irq_domain) {
+			dev_err(dev, "Failed to get a MSI IRQ domain\n");
+			return PTR_ERR(port->irq_domain);
+		}
+
+		xilinx_pcie_enable_msi(port);
+	}
+
+	return 0;
+}
+
+/**
+ * xilinx_pcie_init_port - Initialize hardware
+ * @port: PCIe port information
+ */
+static void xilinx_pcie_init_port(struct xilinx_pcie_port *port)
+{
+	if (xilinx_pcie_link_is_up(port))
+		dev_info(port->dev, "PCIe Link is UP\n");
+	else
+		dev_info(port->dev, "PCIe Link is DOWN\n");
+
+	/* Disable all interrupts */
+	pcie_write(port, ~XILINX_PCIE_IDR_ALL_MASK,
+		   XILINX_PCIE_REG_IMR);
+
+	/* Clear pending interrupts */
+	pcie_write(port, pcie_read(port, XILINX_PCIE_REG_IDR) &
+			 XILINX_PCIE_IMR_ALL_MASK,
+		   XILINX_PCIE_REG_IDR);
+
+	/* Enable all interrupts */
+	pcie_write(port, XILINX_PCIE_IMR_ALL_MASK, XILINX_PCIE_REG_IMR);
+
+	/* Enable the Bridge enable bit */
+	pcie_write(port, pcie_read(port, XILINX_PCIE_REG_RPSC) |
+			 XILINX_PCIE_REG_RPSC_BEN,
+		   XILINX_PCIE_REG_RPSC);
+}
+
+/**
+ * xilinx_pcie_setup - Setup memory resources
+ * @nr: Bus number
+ * @sys: Per controller structure
+ *
+ * Return: '1' on success and error value on failure
+ */
+static int xilinx_pcie_setup(int nr, struct pci_sys_data *sys)
+{
+	struct xilinx_pcie_port *port = sys_to_pcie(sys);
+
+	list_splice_init(&port->resources, &sys->resources);
+
+	return 1;
+}
+
+/**
+ * xilinx_pcie_scan_bus - Scan PCIe bus for devices
+ * @nr: Bus number
+ * @sys: Per controller structure
+ *
+ * Return: Valid Bus pointer on success and NULL on failure
+ */
+static struct pci_bus *xilinx_pcie_scan_bus(int nr, struct pci_sys_data *sys)
+{
+	struct xilinx_pcie_port *port = sys_to_pcie(sys);
+	struct pci_bus *bus;
+
+	port->root_busno = sys->busnr;
+	bus = pci_scan_root_bus(port->dev, sys->busnr, &xilinx_pcie_ops,
+				sys, &sys->resources);
+
+	return bus;
+}
+
+/**
+ * xilinx_pcie_parse_and_add_res - Add resources by parsing ranges
+ * @port: PCIe port information
+ *
+ * Return: '0' on success and error value on failure
+ */
+static int xilinx_pcie_parse_and_add_res(struct xilinx_pcie_port *port)
+{
+	struct device *dev = port->dev;
+	struct device_node *node = dev->of_node;
+	struct resource *mem;
+	resource_size_t offset;
+	struct of_pci_range_parser parser;
+	struct of_pci_range range;
+	struct pci_host_bridge_window *win;
+	int err = 0, mem_resno = 0;
+
+	/* Get the ranges */
+	if (of_pci_range_parser_init(&parser, node)) {
+		dev_err(dev, "missing \"ranges\" property\n");
+		return -EINVAL;
+	}
+
+	/* Parse the ranges and add the resources found to the list */
+	for_each_of_pci_range(&parser, &range) {
+
+		if (mem_resno >= XILINX_MAX_NUM_RESOURCES) {
+			dev_err(dev, "Maximum memory resources exceeded\n");
+			return -EINVAL;
+		}
+
+		mem = devm_kmalloc(dev, sizeof(*mem), GFP_KERNEL);
+		if (!mem) {
+			err = -ENOMEM;
+			goto free_resources;
+		}
+
+		of_pci_range_to_resource(&range, node, mem);
+
+		switch (mem->flags & IORESOURCE_TYPE_BITS) {
+		case IORESOURCE_MEM:
+			offset = range.cpu_addr - range.pci_addr;
+			mem_resno++;
+			break;
+		default:
+			err = -EINVAL;
+			break;
+		}
+
+		if (err < 0) {
+			dev_warn(dev, "Invalid resource found %pR\n", mem);
+			continue;
+		}
+
+		err = request_resource(&iomem_resource, mem);
+		if (err)
+			goto free_resources;
+
+		pci_add_resource_offset(&port->resources, mem, offset);
+	}
+
+	/* Get the bus range */
+	if (of_pci_parse_bus_range(node, &port->bus_range)) {
+		u32 val = pcie_read(port, XILINX_PCIE_REG_BIR);
+		u8 last;
+
+		last = (val & XILINX_PCIE_BIR_ECAM_SZ_MASK) >>
+			XILINX_PCIE_BIR_ECAM_SZ_SHIFT;
+
+		port->bus_range = (struct resource) {
+			.name	= node->name,
+			.start	= 0,
+			.end	= last,
+			.flags	= IORESOURCE_BUS,
+		};
+	}
+
+	/* Register bus resource */
+	pci_add_resource(&port->resources, &port->bus_range);
+
+	return 0;
+
+free_resources:
+	release_child_resources(&iomem_resource);
+	list_for_each_entry(win, &port->resources, list)
+		devm_kfree(dev, win->res);
+	pci_free_resource_list(&port->resources);
+
+	return err;
+}
+
+/**
+ * xilinx_pcie_parse_dt - Parse Device tree
+ * @port: PCIe port information
+ *
+ * Return: '0' on success and error value on failure
+ */
+static int xilinx_pcie_parse_dt(struct xilinx_pcie_port *port)
+{
+	struct device *dev = port->dev;
+	struct device_node *node = dev->of_node;
+	struct resource regs;
+	const char *type;
+	int err;
+
+	type = of_get_property(node, "device_type", NULL);
+	if (!type || strcmp(type, "pci")) {
+		dev_err(dev, "invalid \"device_type\" %s\n", type);
+		return -EINVAL;
+	}
+
+	err = of_address_to_resource(node, 0, &regs);
+	if (err) {
+		dev_err(dev, "missing \"reg\" property\n");
+		return err;
+	}
+
+	port->reg_base = devm_ioremap_resource(dev, &regs);
+	if (IS_ERR(port->reg_base))
+		return PTR_ERR(port->reg_base);
+
+	port->irq = irq_of_parse_and_map(node, 0);
+	err = devm_request_irq(dev, port->irq, xilinx_pcie_intr_handler,
+			       IRQF_SHARED, "xilinx-pcie", port);
+	if (err) {
+		dev_err(dev, "unable to request irq %d\n", port->irq);
+		return err;
+	}
+
+	return 0;
+}
+
+/**
+ * xilinx_pcie_probe - Probe function
+ * @pdev: Platform device pointer
+ *
+ * Return: '0' on success and error value on failure
+ */
+static int xilinx_pcie_probe(struct platform_device *pdev)
+{
+	struct xilinx_pcie_port *port;
+	struct hw_pci hw;
+	struct device *dev = &pdev->dev;
+	int err;
+
+	if (!dev->of_node)
+		return -ENODEV;
+
+	port = devm_kzalloc(dev, sizeof(*port), GFP_KERNEL);
+	if (!port)
+		return -ENOMEM;
+
+	port->dev = dev;
+
+	err = xilinx_pcie_parse_dt(port);
+	if (err) {
+		dev_err(dev, "Parsing DT failed\n");
+		return err;
+	}
+
+	xilinx_pcie_init_port(port);
+
+	err = xilinx_pcie_init_irq_domain(port);
+	if (err) {
+		dev_err(dev, "Failed creating IRQ Domain\n");
+		return err;
+	}
+
+	/*
+	 * Parse PCI ranges, configuration bus range and
+	 * request their resources
+	 */
+	INIT_LIST_HEAD(&port->resources);
+	err = xilinx_pcie_parse_and_add_res(port);
+	if (err) {
+		dev_err(dev, "Failed adding resources\n");
+		return err;
+	}
+
+	platform_set_drvdata(pdev, port);
+
+	/* Register the device */
+	memset(&hw, 0, sizeof(hw));
+	hw = (struct hw_pci) {
+		.nr_controllers	= 1,
+		.private_data	= (void **)&port,
+		.setup		= xilinx_pcie_setup,
+		.map_irq	= of_irq_parse_and_map_pci,
+		.add_bus	= xilinx_pcie_add_bus,
+		.scan		= xilinx_pcie_scan_bus,
+		.ops		= &xilinx_pcie_ops,
+	};
+	pci_common_init_dev(dev, &hw);
+
+	return 0;
+}
+
+/**
+ * xilinx_pcie_remove - Remove function
+ * @pdev: Platform device pointer
+ *
+ * Return: '0' always
+ */
+static int xilinx_pcie_remove(struct platform_device *pdev)
+{
+	struct xilinx_pcie_port *port = platform_get_drvdata(pdev);
+
+	xilinx_pcie_free_irq_domain(port);
+
+	return 0;
+}
+
+static struct of_device_id xilinx_pcie_of_match[] = {
+	{ .compatible = "xlnx,axi-pcie-host-1.00.a", },
+	{}
+};
+
+static struct platform_driver xilinx_pcie_driver = {
+	.driver = {
+		.name = "xilinx-pcie",
+		.owner = THIS_MODULE,
+		.of_match_table = xilinx_pcie_of_match,
+		.suppress_bind_attrs = true,
+	},
+	.probe = xilinx_pcie_probe,
+	.remove = xilinx_pcie_remove,
+};
+module_platform_driver(xilinx_pcie_driver);
+
+MODULE_AUTHOR("Xilinx Inc");
+MODULE_DESCRIPTION("Xilinx AXI PCIe driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pci/hotplug/Makefile b/drivers/pci/hotplug/Makefile
index 3e6532b945c1..4a9aa08b08f1 100644
--- a/drivers/pci/hotplug/Makefile
+++ b/drivers/pci/hotplug/Makefile
@@ -24,7 +24,7 @@ obj-$(CONFIG_HOTPLUG_PCI_S390)		+= s390_pci_hpc.o
 
 obj-$(CONFIG_HOTPLUG_PCI_ACPI_IBM)	+= acpiphp_ibm.o
 
-pci_hotplug-objs	:=	pci_hotplug_core.o pcihp_slot.o
+pci_hotplug-objs	:=	pci_hotplug_core.o
 
 ifdef CONFIG_HOTPLUG_PCI_CPCI
 pci_hotplug-objs	+=	cpci_hotplug_core.o	\
diff --git a/drivers/pci/hotplug/acpi_pcihp.c b/drivers/pci/hotplug/acpi_pcihp.c
index a94d850ae228..876ccc620440 100644
--- a/drivers/pci/hotplug/acpi_pcihp.c
+++ b/drivers/pci/hotplug/acpi_pcihp.c
@@ -46,215 +46,6 @@
 
 static bool debug_acpi;
 
-static acpi_status
-decode_type0_hpx_record(union acpi_object *record, struct hotplug_params *hpx)
-{
-	int i;
-	union acpi_object *fields = record->package.elements;
-	u32 revision = fields[1].integer.value;
-
-	switch (revision) {
-	case 1:
-		if (record->package.count != 6)
-			return AE_ERROR;
-		for (i = 2; i < 6; i++)
-			if (fields[i].type != ACPI_TYPE_INTEGER)
-				return AE_ERROR;
-		hpx->t0 = &hpx->type0_data;
-		hpx->t0->revision        = revision;
-		hpx->t0->cache_line_size = fields[2].integer.value;
-		hpx->t0->latency_timer   = fields[3].integer.value;
-		hpx->t0->enable_serr     = fields[4].integer.value;
-		hpx->t0->enable_perr     = fields[5].integer.value;
-		break;
-	default:
-		printk(KERN_WARNING
-		       "%s: Type 0 Revision %d record not supported\n",
-		       __func__, revision);
-		return AE_ERROR;
-	}
-	return AE_OK;
-}
-
-static acpi_status
-decode_type1_hpx_record(union acpi_object *record, struct hotplug_params *hpx)
-{
-	int i;
-	union acpi_object *fields = record->package.elements;
-	u32 revision = fields[1].integer.value;
-
-	switch (revision) {
-	case 1:
-		if (record->package.count != 5)
-			return AE_ERROR;
-		for (i = 2; i < 5; i++)
-			if (fields[i].type != ACPI_TYPE_INTEGER)
-				return AE_ERROR;
-		hpx->t1 = &hpx->type1_data;
-		hpx->t1->revision      = revision;
-		hpx->t1->max_mem_read  = fields[2].integer.value;
-		hpx->t1->avg_max_split = fields[3].integer.value;
-		hpx->t1->tot_max_split = fields[4].integer.value;
-		break;
-	default:
-		printk(KERN_WARNING
-		       "%s: Type 1 Revision %d record not supported\n",
-		       __func__, revision);
-		return AE_ERROR;
-	}
-	return AE_OK;
-}
-
-static acpi_status
-decode_type2_hpx_record(union acpi_object *record, struct hotplug_params *hpx)
-{
-	int i;
-	union acpi_object *fields = record->package.elements;
-	u32 revision = fields[1].integer.value;
-
-	switch (revision) {
-	case 1:
-		if (record->package.count != 18)
-			return AE_ERROR;
-		for (i = 2; i < 18; i++)
-			if (fields[i].type != ACPI_TYPE_INTEGER)
-				return AE_ERROR;
-		hpx->t2 = &hpx->type2_data;
-		hpx->t2->revision      = revision;
-		hpx->t2->unc_err_mask_and      = fields[2].integer.value;
-		hpx->t2->unc_err_mask_or       = fields[3].integer.value;
-		hpx->t2->unc_err_sever_and     = fields[4].integer.value;
-		hpx->t2->unc_err_sever_or      = fields[5].integer.value;
-		hpx->t2->cor_err_mask_and      = fields[6].integer.value;
-		hpx->t2->cor_err_mask_or       = fields[7].integer.value;
-		hpx->t2->adv_err_cap_and       = fields[8].integer.value;
-		hpx->t2->adv_err_cap_or        = fields[9].integer.value;
-		hpx->t2->pci_exp_devctl_and    = fields[10].integer.value;
-		hpx->t2->pci_exp_devctl_or     = fields[11].integer.value;
-		hpx->t2->pci_exp_lnkctl_and    = fields[12].integer.value;
-		hpx->t2->pci_exp_lnkctl_or     = fields[13].integer.value;
-		hpx->t2->sec_unc_err_sever_and = fields[14].integer.value;
-		hpx->t2->sec_unc_err_sever_or  = fields[15].integer.value;
-		hpx->t2->sec_unc_err_mask_and  = fields[16].integer.value;
-		hpx->t2->sec_unc_err_mask_or   = fields[17].integer.value;
-		break;
-	default:
-		printk(KERN_WARNING
-		       "%s: Type 2 Revision %d record not supported\n",
-		       __func__, revision);
-		return AE_ERROR;
-	}
-	return AE_OK;
-}
-
-static acpi_status
-acpi_run_hpx(acpi_handle handle, struct hotplug_params *hpx)
-{
-	acpi_status status;
-	struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER, NULL};
-	union acpi_object *package, *record, *fields;
-	u32 type;
-	int i;
-
-	/* Clear the return buffer with zeros */
-	memset(hpx, 0, sizeof(struct hotplug_params));
-
-	status = acpi_evaluate_object(handle, "_HPX", NULL, &buffer);
-	if (ACPI_FAILURE(status))
-		return status;
-
-	package = (union acpi_object *)buffer.pointer;
-	if (package->type != ACPI_TYPE_PACKAGE) {
-		status = AE_ERROR;
-		goto exit;
-	}
-
-	for (i = 0; i < package->package.count; i++) {
-		record = &package->package.elements[i];
-		if (record->type != ACPI_TYPE_PACKAGE) {
-			status = AE_ERROR;
-			goto exit;
-		}
-
-		fields = record->package.elements;
-		if (fields[0].type != ACPI_TYPE_INTEGER ||
-		    fields[1].type != ACPI_TYPE_INTEGER) {
-			status = AE_ERROR;
-			goto exit;
-		}
-
-		type = fields[0].integer.value;
-		switch (type) {
-		case 0:
-			status = decode_type0_hpx_record(record, hpx);
-			if (ACPI_FAILURE(status))
-				goto exit;
-			break;
-		case 1:
-			status = decode_type1_hpx_record(record, hpx);
-			if (ACPI_FAILURE(status))
-				goto exit;
-			break;
-		case 2:
-			status = decode_type2_hpx_record(record, hpx);
-			if (ACPI_FAILURE(status))
-				goto exit;
-			break;
-		default:
-			printk(KERN_ERR "%s: Type %d record not supported\n",
-			       __func__, type);
-			status = AE_ERROR;
-			goto exit;
-		}
-	}
- exit:
-	kfree(buffer.pointer);
-	return status;
-}
-
-static acpi_status
-acpi_run_hpp(acpi_handle handle, struct hotplug_params *hpp)
-{
-	acpi_status status;
-	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
-	union acpi_object *package, *fields;
-	int i;
-
-	memset(hpp, 0, sizeof(struct hotplug_params));
-
-	status = acpi_evaluate_object(handle, "_HPP", NULL, &buffer);
-	if (ACPI_FAILURE(status))
-		return status;
-
-	package = (union acpi_object *) buffer.pointer;
-	if (package->type != ACPI_TYPE_PACKAGE ||
-	    package->package.count != 4) {
-		status = AE_ERROR;
-		goto exit;
-	}
-
-	fields = package->package.elements;
-	for (i = 0; i < 4; i++) {
-		if (fields[i].type != ACPI_TYPE_INTEGER) {
-			status = AE_ERROR;
-			goto exit;
-		}
-	}
-
-	hpp->t0 = &hpp->type0_data;
-	hpp->t0->revision        = 1;
-	hpp->t0->cache_line_size = fields[0].integer.value;
-	hpp->t0->latency_timer   = fields[1].integer.value;
-	hpp->t0->enable_serr     = fields[2].integer.value;
-	hpp->t0->enable_perr     = fields[3].integer.value;
-
-exit:
-	kfree(buffer.pointer);
-	return status;
-}
-
-
-
 /* acpi_run_oshp - get control of hotplug from the firmware
  *
  * @handle - the handle of the hotplug controller.
@@ -283,48 +74,6 @@ static acpi_status acpi_run_oshp(acpi_handle handle)
 	return status;
 }
 
-/* pci_get_hp_params
- *
- * @dev - the pci_dev for which we want parameters
- * @hpp - allocated by the caller
- */
-int pci_get_hp_params(struct pci_dev *dev, struct hotplug_params *hpp)
-{
-	acpi_status status;
-	acpi_handle handle, phandle;
-	struct pci_bus *pbus;
-
-	handle = NULL;
-	for (pbus = dev->bus; pbus; pbus = pbus->parent) {
-		handle = acpi_pci_get_bridge_handle(pbus);
-		if (handle)
-			break;
-	}
-
-	/*
-	 * _HPP settings apply to all child buses, until another _HPP is
-	 * encountered. If we don't find an _HPP for the input pci dev,
-	 * look for it in the parent device scope since that would apply to
-	 * this pci dev.
-	 */
-	while (handle) {
-		status = acpi_run_hpx(handle, hpp);
-		if (ACPI_SUCCESS(status))
-			return 0;
-		status = acpi_run_hpp(handle, hpp);
-		if (ACPI_SUCCESS(status))
-			return 0;
-		if (acpi_is_root_bridge(handle))
-			break;
-		status = acpi_get_parent(handle, &phandle);
-		if (ACPI_FAILURE(status))
-			break;
-		handle = phandle;
-	}
-	return -ENODEV;
-}
-EXPORT_SYMBOL_GPL(pci_get_hp_params);
-
 /**
  * acpi_get_hp_hw_control_from_firmware
  * @dev: the pci_dev of the bridge that has a hotplug controller
@@ -433,7 +182,8 @@ int acpi_pci_check_ejectable(struct pci_bus *pbus, acpi_handle handle)
 {
 	acpi_handle bridge_handle, parent_handle;
 
-	if (!(bridge_handle = acpi_pci_get_bridge_handle(pbus)))
+	bridge_handle = acpi_pci_get_bridge_handle(pbus);
+	if (!bridge_handle)
 		return 0;
 	if ((ACPI_FAILURE(acpi_get_parent(handle, &parent_handle))))
 		return 0;
diff --git a/drivers/pci/hotplug/acpiphp_glue.c b/drivers/pci/hotplug/acpiphp_glue.c
index 70741c8c46a0..bcb90e4888dd 100644
--- a/drivers/pci/hotplug/acpiphp_glue.c
+++ b/drivers/pci/hotplug/acpiphp_glue.c
@@ -61,7 +61,6 @@ static DEFINE_MUTEX(bridge_mutex);
 static int acpiphp_hotplug_notify(struct acpi_device *adev, u32 type);
 static void acpiphp_post_dock_fixup(struct acpi_device *adev);
 static void acpiphp_sanitize_bus(struct pci_bus *bus);
-static void acpiphp_set_hpp_values(struct pci_bus *bus);
 static void hotplug_event(u32 type, struct acpiphp_context *context);
 static void free_bridge(struct kref *kref);
 
@@ -510,7 +509,7 @@ static void enable_slot(struct acpiphp_slot *slot)
 	__pci_bus_assign_resources(bus, &add_list, NULL);
 
 	acpiphp_sanitize_bus(bus);
-	acpiphp_set_hpp_values(bus);
+	pcie_bus_configure_settings(bus);
 	acpiphp_set_acpi_region(slot);
 
 	list_for_each_entry(dev, &bus->devices, bus_list) {
@@ -560,19 +559,15 @@ static void disable_slot(struct acpiphp_slot *slot)
 	slot->flags &= (~SLOT_ENABLED);
 }
 
-static bool acpiphp_no_hotplug(struct acpi_device *adev)
-{
-	return adev && adev->flags.no_hotplug;
-}
-
 static bool slot_no_hotplug(struct acpiphp_slot *slot)
 {
-	struct acpiphp_func *func;
+	struct pci_bus *bus = slot->bus;
+	struct pci_dev *dev;
 
-	list_for_each_entry(func, &slot->funcs, sibling)
-		if (acpiphp_no_hotplug(func_to_acpi_device(func)))
+	list_for_each_entry(dev, &bus->devices, bus_list) {
+		if (PCI_SLOT(dev->devfn) == slot->device && dev->ignore_hotplug)
 			return true;
-
+	}
 	return false;
 }
 
@@ -645,7 +640,7 @@ static void trim_stale_devices(struct pci_dev *dev)
 
 		status = acpi_evaluate_integer(adev->handle, "_STA", NULL, &sta);
 		alive = (ACPI_SUCCESS(status) && device_status_valid(sta))
-			|| acpiphp_no_hotplug(adev);
+			|| dev->ignore_hotplug;
 	}
 	if (!alive)
 		alive = pci_device_is_present(dev);
@@ -702,14 +697,6 @@ static void acpiphp_check_bridge(struct acpiphp_bridge *bridge)
 	}
 }
 
-static void acpiphp_set_hpp_values(struct pci_bus *bus)
-{
-	struct pci_dev *dev;
-
-	list_for_each_entry(dev, &bus->devices, bus_list)
-		pci_configure_slot(dev);
-}
-
 /*
  * Remove devices for which we could not assign resources, call
  * arch specific code to fix-up the bus
diff --git a/drivers/pci/hotplug/acpiphp_ibm.c b/drivers/pci/hotplug/acpiphp_ibm.c
index 8dcccffd6e21..6ca23998ee8f 100644
--- a/drivers/pci/hotplug/acpiphp_ibm.c
+++ b/drivers/pci/hotplug/acpiphp_ibm.c
@@ -302,7 +302,7 @@ static int ibm_get_table_from_acpi(char **bufp)
 		goto read_table_done;
 	}
 
-	for(size = 0, i = 0; i < package->package.count; i++) {
+	for (size = 0, i = 0; i < package->package.count; i++) {
 		if (package->package.elements[i].type != ACPI_TYPE_BUFFER) {
 			pr_err("%s:  Invalid APCI element %d\n", __func__, i);
 			goto read_table_done;
diff --git a/drivers/pci/hotplug/cpci_hotplug_core.c b/drivers/pci/hotplug/cpci_hotplug_core.c
index e09cf7827d68..a5a7fd8332ac 100644
--- a/drivers/pci/hotplug/cpci_hotplug_core.c
+++ b/drivers/pci/hotplug/cpci_hotplug_core.c
@@ -125,7 +125,8 @@ disable_slot(struct hotplug_slot *hotplug_slot)
 
 	/* Unconfigure device */
 	dbg("%s - unconfiguring slot %s", __func__, slot_name(slot));
-	if ((retval = cpci_unconfigure_slot(slot))) {
+	retval = cpci_unconfigure_slot(slot);
+	if (retval) {
 		err("%s - could not unconfigure slot %s",
 		    __func__, slot_name(slot));
 		goto disable_error;
@@ -141,9 +142,11 @@ disable_slot(struct hotplug_slot *hotplug_slot)
 	}
 	cpci_led_on(slot);
 
-	if (controller->ops->set_power)
-		if ((retval = controller->ops->set_power(slot, 0)))
+	if (controller->ops->set_power) {
+		retval = controller->ops->set_power(slot, 0);
+		if (retval)
 			goto disable_error;
+	}
 
 	if (update_adapter_status(slot->hotplug_slot, 0))
 		warn("failure to update adapter file");
@@ -467,9 +470,9 @@ check_slots(void)
 			    __func__, slot_name(slot), hs_csr);
 
 			if (!slot->extracting) {
-				if (update_latch_status(slot->hotplug_slot, 0)) {
+				if (update_latch_status(slot->hotplug_slot, 0))
 					warn("failure to update latch file");
-				}
+
 				slot->extracting = 1;
 				atomic_inc(&extracting);
 			}
diff --git a/drivers/pci/hotplug/cpcihp_generic.c b/drivers/pci/hotplug/cpcihp_generic.c
index 04fcd7811400..66b7bbebe493 100644
--- a/drivers/pci/hotplug/cpcihp_generic.c
+++ b/drivers/pci/hotplug/cpcihp_generic.c
@@ -56,7 +56,7 @@
 		if (debug)					\
 			printk (KERN_DEBUG "%s: " format "\n",	\
 				MY_NAME , ## arg);		\
-	} while(0)
+	} while (0)
 #define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME , ## arg)
 #define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME , ## arg)
 #define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME , ## arg)
@@ -82,28 +82,28 @@ static int __init validate_parameters(void)
 	char *p;
 	unsigned long tmp;
 
-	if(!bridge) {
+	if (!bridge) {
 		info("not configured, disabling.");
 		return -EINVAL;
 	}
 	str = bridge;
-	if(!*str)
+	if (!*str)
 		return -EINVAL;
 
 	tmp = simple_strtoul(str, &p, 16);
-	if(p == str || tmp > 0xff) {
+	if (p == str || tmp > 0xff) {
 		err("Invalid hotplug bus bridge device bus number");
 		return -EINVAL;
 	}
 	bridge_busnr = (u8) tmp;
 	dbg("bridge_busnr = 0x%02x", bridge_busnr);
-	if(*p != ':') {
+	if (*p != ':') {
 		err("Invalid hotplug bus bridge device");
 		return -EINVAL;
 	}
 	str = p + 1;
 	tmp = simple_strtoul(str, &p, 16);
-	if(p == str || tmp > 0x1f) {
+	if (p == str || tmp > 0x1f) {
 		err("Invalid hotplug bus bridge device slot number");
 		return -EINVAL;
 	}
@@ -112,18 +112,18 @@ static int __init validate_parameters(void)
 
 	dbg("first_slot = 0x%02x", first_slot);
 	dbg("last_slot = 0x%02x", last_slot);
-	if(!(first_slot && last_slot)) {
+	if (!(first_slot && last_slot)) {
 		err("Need to specify first_slot and last_slot");
 		return -EINVAL;
 	}
-	if(last_slot < first_slot) {
+	if (last_slot < first_slot) {
 		err("first_slot must be less than last_slot");
 		return -EINVAL;
 	}
 
 	dbg("port = 0x%04x", port);
 	dbg("enum_bit = 0x%02x", enum_bit);
-	if(enum_bit > 7) {
+	if (enum_bit > 7) {
 		err("Invalid #ENUM bit");
 		return -EINVAL;
 	}
@@ -151,12 +151,12 @@ static int __init cpcihp_generic_init(void)
 		return status;
 
 	r = request_region(port, 1, "#ENUM hotswap signal register");
-	if(!r)
+	if (!r)
 		return -EBUSY;
 
 	dev = pci_get_domain_bus_and_slot(0, bridge_busnr,
 					  PCI_DEVFN(bridge_slot, 0));
-	if(!dev || dev->hdr_type != PCI_HEADER_TYPE_BRIDGE) {
+	if (!dev || dev->hdr_type != PCI_HEADER_TYPE_BRIDGE) {
 		err("Invalid bridge device %s", bridge);
 		pci_dev_put(dev);
 		return -EINVAL;
@@ -169,21 +169,21 @@ static int __init cpcihp_generic_init(void)
 	generic_hpc.ops = &generic_hpc_ops;
 
 	status = cpci_hp_register_controller(&generic_hpc);
-	if(status != 0) {
+	if (status != 0) {
 		err("Could not register cPCI hotplug controller");
 		return -ENODEV;
 	}
 	dbg("registered controller");
 
 	status = cpci_hp_register_bus(bus, first_slot, last_slot);
-	if(status != 0) {
+	if (status != 0) {
 		err("Could not register cPCI hotplug bus");
 		goto init_bus_register_error;
 	}
 	dbg("registered bus");
 
 	status = cpci_hp_start();
-	if(status != 0) {
+	if (status != 0) {
 		err("Could not started cPCI hotplug system");
 		goto init_start_error;
 	}
diff --git a/drivers/pci/hotplug/cpcihp_zt5550.c b/drivers/pci/hotplug/cpcihp_zt5550.c
index 6757b3ef7e10..7ecf34e76a61 100644
--- a/drivers/pci/hotplug/cpcihp_zt5550.c
+++ b/drivers/pci/hotplug/cpcihp_zt5550.c
@@ -51,7 +51,7 @@
 		if (debug)					\
 			printk (KERN_DEBUG "%s: " format "\n",	\
 				MY_NAME , ## arg);		\
-	} while(0)
+	} while (0)
 #define err(format, arg...) printk(KERN_ERR "%s: " format "\n", MY_NAME , ## arg)
 #define info(format, arg...) printk(KERN_INFO "%s: " format "\n", MY_NAME , ## arg)
 #define warn(format, arg...) printk(KERN_WARNING "%s: " format "\n", MY_NAME , ## arg)
@@ -82,13 +82,13 @@ static int zt5550_hc_config(struct pci_dev *pdev)
 	int ret;
 
 	/* Since we know that no boards exist with two HC chips, treat it as an error */
-	if(hc_dev) {
+	if (hc_dev) {
 		err("too many host controller devices?");
 		return -EBUSY;
 	}
 
 	ret = pci_enable_device(pdev);
-	if(ret) {
+	if (ret) {
 		err("cannot enable %s\n", pci_name(pdev));
 		return ret;
 	}
@@ -98,7 +98,7 @@ static int zt5550_hc_config(struct pci_dev *pdev)
 	dbg("pci resource start %llx", (unsigned long long)pci_resource_start(hc_dev, 1));
 	dbg("pci resource len %llx", (unsigned long long)pci_resource_len(hc_dev, 1));
 
-	if(!request_mem_region(pci_resource_start(hc_dev, 1),
+	if (!request_mem_region(pci_resource_start(hc_dev, 1),
 				pci_resource_len(hc_dev, 1), MY_NAME)) {
 		err("cannot reserve MMIO region");
 		ret = -ENOMEM;
@@ -107,7 +107,7 @@ static int zt5550_hc_config(struct pci_dev *pdev)
 
 	hc_registers =
 	    ioremap(pci_resource_start(hc_dev, 1), pci_resource_len(hc_dev, 1));
-	if(!hc_registers) {
+	if (!hc_registers) {
 		err("cannot remap MMIO region %llx @ %llx",
 			(unsigned long long)pci_resource_len(hc_dev, 1),
 			(unsigned long long)pci_resource_start(hc_dev, 1));
@@ -146,7 +146,7 @@ exit_disable_device:
 
 static int zt5550_hc_cleanup(void)
 {
-	if(!hc_dev)
+	if (!hc_dev)
 		return -ENODEV;
 
 	iounmap(hc_registers);
@@ -170,9 +170,9 @@ static int zt5550_hc_check_irq(void *dev_id)
 	u8 reg;
 
 	ret = 0;
-	if(dev_id == zt5550_hpc.dev_id) {
+	if (dev_id == zt5550_hpc.dev_id) {
 		reg = readb(csr_int_status);
-		if(reg)
+		if (reg)
 			ret = 1;
 	}
 	return ret;
@@ -182,9 +182,9 @@ static int zt5550_hc_enable_irq(void)
 {
 	u8 reg;
 
-	if(hc_dev == NULL) {
+	if (hc_dev == NULL)
 		return -ENODEV;
-	}
+
 	reg = readb(csr_int_mask);
 	reg = reg & ~ENUM_INT_MASK;
 	writeb(reg, csr_int_mask);
@@ -195,9 +195,8 @@ static int zt5550_hc_disable_irq(void)
 {
 	u8 reg;
 
-	if(hc_dev == NULL) {
+	if (hc_dev == NULL)
 		return -ENODEV;
-	}
 
 	reg = readb(csr_int_mask);
 	reg = reg | ENUM_INT_MASK;
@@ -210,15 +209,15 @@ static int zt5550_hc_init_one (struct pci_dev *pdev, const struct pci_device_id
 	int status;
 
 	status = zt5550_hc_config(pdev);
-	if(status != 0) {
+	if (status != 0)
 		return status;
-	}
+
 	dbg("returned from zt5550_hc_config");
 
 	memset(&zt5550_hpc, 0, sizeof (struct cpci_hp_controller));
 	zt5550_hpc_ops.query_enum = zt5550_hc_query_enum;
 	zt5550_hpc.ops = &zt5550_hpc_ops;
-	if(!poll) {
+	if (!poll) {
 		zt5550_hpc.irq = hc_dev->irq;
 		zt5550_hpc.irq_flags = IRQF_SHARED;
 		zt5550_hpc.dev_id = hc_dev;
@@ -231,15 +230,16 @@ static int zt5550_hc_init_one (struct pci_dev *pdev, const struct pci_device_id
 	}
 
 	status = cpci_hp_register_controller(&zt5550_hpc);
-	if(status != 0) {
+	if (status != 0) {
 		err("could not register cPCI hotplug controller");
 		goto init_hc_error;
 	}
 	dbg("registered controller");
 
 	/* Look for first device matching cPCI bus's bridge vendor and device IDs */
-	if(!(bus0_dev = pci_get_device(PCI_VENDOR_ID_DEC,
-					 PCI_DEVICE_ID_DEC_21154, NULL))) {
+	bus0_dev = pci_get_device(PCI_VENDOR_ID_DEC,
+				  PCI_DEVICE_ID_DEC_21154, NULL);
+	if (!bus0_dev) {
 		status = -ENODEV;
 		goto init_register_error;
 	}
@@ -247,14 +247,14 @@ static int zt5550_hc_init_one (struct pci_dev *pdev, const struct pci_device_id
 	pci_dev_put(bus0_dev);
 
 	status = cpci_hp_register_bus(bus0, 0x0a, 0x0f);
-	if(status != 0) {
+	if (status != 0) {
 		err("could not register cPCI hotplug bus");
 		goto init_register_error;
 	}
 	dbg("registered bus");
 
 	status = cpci_hp_start();
-	if(status != 0) {
+	if (status != 0) {
 		err("could not started cPCI hotplug system");
 		cpci_hp_unregister_bus(bus0);
 		goto init_register_error;
@@ -300,11 +300,11 @@ static int __init zt5550_init(void)
 
 	info(DRIVER_DESC " version: " DRIVER_VERSION);
 	r = request_region(ENUM_PORT, 1, "#ENUM hotswap signal register");
-	if(!r)
+	if (!r)
 		return -EBUSY;
 
 	rc = pci_register_driver(&zt5550_hc_driver);
-	if(rc < 0)
+	if (rc < 0)
 		release_region(ENUM_PORT, 1);
 	return rc;
 }
diff --git a/drivers/pci/hotplug/cpqphp.h b/drivers/pci/hotplug/cpqphp.h
index 0450f405807d..b28b2d2184cd 100644
--- a/drivers/pci/hotplug/cpqphp.h
+++ b/drivers/pci/hotplug/cpqphp.h
@@ -690,7 +690,7 @@ static inline int cpq_get_latch_status(struct controller *ctrl,
 
 	status = (readl(ctrl->hpc_reg + INT_INPUT_CLEAR) & (0x01L << hp_slot));
 
-	return(status == 0) ? 1 : 0;
+	return (status == 0) ? 1 : 0;
 }
 
 
diff --git a/drivers/pci/hotplug/cpqphp_core.c b/drivers/pci/hotplug/cpqphp_core.c
index 4aaee746df88..a53084ddc118 100644
--- a/drivers/pci/hotplug/cpqphp_core.c
+++ b/drivers/pci/hotplug/cpqphp_core.c
@@ -1096,9 +1096,8 @@ static int cpqhpc_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 
 	/* initialize our threads if they haven't already been started up */
 	rc = one_time_init();
-	if (rc) {
+	if (rc)
 		goto err_free_bus;
-	}
 
 	dbg("pdev = %p\n", pdev);
 	dbg("pci resource start %llx\n", (unsigned long long)pci_resource_start(pdev, 0));
diff --git a/drivers/pci/hotplug/cpqphp_ctrl.c b/drivers/pci/hotplug/cpqphp_ctrl.c
index bde47fce3248..c5cbefee5236 100644
--- a/drivers/pci/hotplug/cpqphp_ctrl.c
+++ b/drivers/pci/hotplug/cpqphp_ctrl.c
@@ -705,9 +705,8 @@ static struct pci_resource *get_max_resource(struct pci_resource **head, u32 siz
 		if (temp == max) {
 			*head = max->next;
 		} else {
-			while (temp && temp->next != max) {
+			while (temp && temp->next != max)
 				temp = temp->next;
-			}
 
 			if (temp)
 				temp->next = max->next;
@@ -903,9 +902,8 @@ irqreturn_t cpqhp_ctrl_intr(int IRQ, void *data)
 	/*
 	 * Check to see if it was our interrupt
 	 */
-	if (!(misc & 0x000C)) {
+	if (!(misc & 0x000C))
 		return IRQ_NONE;
-	}
 
 	if (misc & 0x0004) {
 		/*
@@ -1143,7 +1141,7 @@ static u8 set_controller_speed(struct controller *ctrl, u8 adapter_speed, u8 hp_
 	/* We don't allow freq/mode changes if we find another adapter running
 	 * in another slot on this controller
 	 */
-	for(slot = ctrl->slot; slot; slot = slot->next) {
+	for (slot = ctrl->slot; slot; slot = slot->next) {
 		if (slot->device == (hp_slot + ctrl->slot_device_offset))
 			continue;
 		if (!slot->hotplug_slot || !slot->hotplug_slot->info)
@@ -1193,7 +1191,7 @@ static u8 set_controller_speed(struct controller *ctrl, u8 adapter_speed, u8 hp_
 
 	reg16 = readw(ctrl->hpc_reg + NEXT_CURR_FREQ);
 	reg16 &= ~0x000F;
-	switch(adapter_speed) {
+	switch (adapter_speed) {
 		case(PCI_SPEED_133MHz_PCIX):
 			reg = 0x75;
 			reg16 |= 0xB;
@@ -2006,9 +2004,8 @@ int cpqhp_process_SI(struct controller *ctrl, struct pci_func *func)
 	/* Check to see if the interlock is closed */
 	tempdword = readl(ctrl->hpc_reg + INT_INPUT_CLEAR);
 
-	if (tempdword & (0x01 << hp_slot)) {
+	if (tempdword & (0x01 << hp_slot))
 		return 1;
-	}
 
 	if (func->is_a_board) {
 		rc = board_replaced(func, ctrl);
@@ -2070,9 +2067,8 @@ int cpqhp_process_SI(struct controller *ctrl, struct pci_func *func)
 		}
 	}
 
-	if (rc) {
+	if (rc)
 		dbg("%s: rc = %d\n", __func__, rc);
-	}
 
 	if (p_slot)
 		update_slot_info(ctrl, p_slot);
@@ -2095,9 +2091,8 @@ int cpqhp_process_SS(struct controller *ctrl, struct pci_func *func)
 	device = func->device;
 	func = cpqhp_slot_find(ctrl->bus, device, index++);
 	p_slot = cpqhp_find_slot(ctrl, device);
-	if (p_slot) {
+	if (p_slot)
 		physical_slot = p_slot->number;
-	}
 
 	/* Make sure there are no video controllers here */
 	while (func && !rc) {
diff --git a/drivers/pci/hotplug/cpqphp_nvram.c b/drivers/pci/hotplug/cpqphp_nvram.c
index 0968a9bcb345..1e08ff8c229c 100644
--- a/drivers/pci/hotplug/cpqphp_nvram.c
+++ b/drivers/pci/hotplug/cpqphp_nvram.c
@@ -204,9 +204,8 @@ static int load_HRT (void __iomem *rom_start)
 	u8 temp_byte = 0xFF;
 	u32 rc;
 
-	if (!check_for_compaq_ROM(rom_start)) {
+	if (!check_for_compaq_ROM(rom_start))
 		return -ENODEV;
-	}
 
 	available = 1024;
 
@@ -250,9 +249,8 @@ static u32 store_HRT (void __iomem *rom_start)
 
 	available = 1024;
 
-	if (!check_for_compaq_ROM(rom_start)) {
+	if (!check_for_compaq_ROM(rom_start))
 		return(1);
-	}
 
 	buffer = (u32*) evbuffer;
 
@@ -427,9 +425,9 @@ static u32 store_HRT (void __iomem *rom_start)
 
 void compaq_nvram_init (void __iomem *rom_start)
 {
-	if (rom_start) {
+	if (rom_start)
 		compaq_int15_entry_point = (rom_start + ROM_INT15_PHY_ADDR - ROM_PHY_ADDR);
-	}
+
 	dbg("int15 entry  = %p\n", compaq_int15_entry_point);
 
 	/* initialize our int15 lock */
@@ -661,9 +659,8 @@ int compaq_nvram_store (void __iomem *rom_start)
 
 	if (evbuffer_init) {
 		rc = store_HRT(rom_start);
-		if (rc) {
+		if (rc)
 			err(msg_unable_to_save);
-		}
 	}
 	return rc;
 }
diff --git a/drivers/pci/hotplug/ibmphp_core.c b/drivers/pci/hotplug/ibmphp_core.c
index f7b8684a7739..3efaf4c38528 100644
--- a/drivers/pci/hotplug/ibmphp_core.c
+++ b/drivers/pci/hotplug/ibmphp_core.c
@@ -1023,7 +1023,8 @@ static int enable_slot(struct hotplug_slot *hs)
 	debug("ENABLING SLOT........\n");
 	slot_cur = hs->private;
 
-	if ((rc = validate(slot_cur, ENABLE))) {
+	rc = validate(slot_cur, ENABLE);
+	if (rc) {
 		err("validate function failed\n");
 		goto error_nopower;
 	}
@@ -1199,9 +1200,8 @@ int ibmphp_do_disable_slot(struct slot *slot_cur)
 
 	debug("DISABLING SLOT...\n");
 
-	if ((slot_cur == NULL) || (slot_cur->ctrl == NULL)) {
+	if ((slot_cur == NULL) || (slot_cur->ctrl == NULL))
 		return -ENODEV;
-	}
 
 	flag = slot_cur->flag;
 	slot_cur->flag = 1;
@@ -1336,17 +1336,20 @@ static int __init ibmphp_init(void)
 	for (i = 0; i < 16; i++)
 		irqs[i] = 0;
 
-	if ((rc = ibmphp_access_ebda()))
+	rc = ibmphp_access_ebda();
+	if (rc)
 		goto error;
 	debug("after ibmphp_access_ebda()\n");
 
-	if ((rc = ibmphp_rsrc_init()))
+	rc = ibmphp_rsrc_init();
+	if (rc)
 		goto error;
 	debug("AFTER Resource & EBDA INITIALIZATIONS\n");
 
 	max_slots = get_max_slots();
 
-	if ((rc = ibmphp_register_pci()))
+	rc = ibmphp_register_pci();
+	if (rc)
 		goto error;
 
 	if (init_ops()) {
@@ -1355,9 +1358,9 @@ static int __init ibmphp_init(void)
 	}
 
 	ibmphp_print_test();
-	if ((rc = ibmphp_hpc_start_poll_thread())) {
+	rc = ibmphp_hpc_start_poll_thread();
+	if (rc)
 		goto error;
-	}
 
 exit:
 	return rc;
diff --git a/drivers/pci/hotplug/ibmphp_ebda.c b/drivers/pci/hotplug/ibmphp_ebda.c
index 0f65ac555434..d9b197d5c6b4 100644
--- a/drivers/pci/hotplug/ibmphp_ebda.c
+++ b/drivers/pci/hotplug/ibmphp_ebda.c
@@ -215,9 +215,8 @@ static void __init print_ebda_hpc (void)
 			debug ("%s - cap of the slot: %x\n", __func__, hpc_ptr->slots[index].slot_cap);
 		}
 
-		for (index = 0; index < hpc_ptr->bus_count; index++) {
+		for (index = 0; index < hpc_ptr->bus_count; index++)
 			debug ("%s - bus# of each bus controlled by this ctlr: %x\n", __func__, hpc_ptr->buses[index].bus_num);
-		}
 
 		debug ("%s - type of hpc: %x\n", __func__, hpc_ptr->ctlr_type);
 		switch (hpc_ptr->ctlr_type) {
diff --git a/drivers/pci/hotplug/ibmphp_hpc.c b/drivers/pci/hotplug/ibmphp_hpc.c
index a936022956e6..220876715a08 100644
--- a/drivers/pci/hotplug/ibmphp_hpc.c
+++ b/drivers/pci/hotplug/ibmphp_hpc.c
@@ -997,9 +997,8 @@ static int process_changeinstatus (struct slot *pslot, struct slot *poldslot)
 		rc = ibmphp_do_disable_slot (pslot);
 	}
 
-	if (update || disable) {
+	if (update || disable)
 		ibmphp_update_slot_info (pslot);
-	}
 
 	debug ("%s - Exit rc[%d] disable[%x] update[%x]\n", __func__, rc, disable, update);
 
diff --git a/drivers/pci/hotplug/ibmphp_pci.c b/drivers/pci/hotplug/ibmphp_pci.c
index 2fd296706ce7..814cea22a9fa 100644
--- a/drivers/pci/hotplug/ibmphp_pci.c
+++ b/drivers/pci/hotplug/ibmphp_pci.c
@@ -145,7 +145,8 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 				case PCI_HEADER_TYPE_NORMAL:
 					debug ("single device case.... vendor id = %x, hdr_type = %x, class = %x\n", vendor_id, hdr_type, class);
 					assign_alt_irq (cur_func, class_code);
-					if ((rc = configure_device (cur_func)) < 0) {
+					rc = configure_device(cur_func);
+					if (rc < 0) {
 						/* We need to do this in case some other BARs were properly inserted */
 						err ("was not able to configure devfunc %x on bus %x.\n",
 						     cur_func->device, cur_func->busno);
@@ -157,7 +158,8 @@ int ibmphp_configure_card (struct pci_func *func, u8 slotno)
 					break;
 				case PCI_HEADER_TYPE_MULTIDEVICE:
 					assign_alt_irq (cur_func, class_code);
-					if ((rc = configure_device (cur_func)) < 0) {
+					rc = configure_device(cur_func);
+					if (rc < 0) {
 						/* We need to do this in case some other BARs were properly inserted */
 						err ("was not able to configure devfunc %x on bus %x...bailing out\n",
 						     cur_func->device, cur_func->busno);
diff --git a/drivers/pci/hotplug/ibmphp_res.c b/drivers/pci/hotplug/ibmphp_res.c
index f34745abd5b6..219ba8090a37 100644
--- a/drivers/pci/hotplug/ibmphp_res.c
+++ b/drivers/pci/hotplug/ibmphp_res.c
@@ -224,7 +224,8 @@ int __init ibmphp_rsrc_init (void)
 			if ((curr->rsrc_type & RESTYPE) == MMASK) {
 				/* no bus structure exists in place yet */
 				if (list_empty (&gbuses)) {
-					if ((rc = alloc_bus_range (&newbus, &newrange, curr, MEM, 1)))
+					rc = alloc_bus_range(&newbus, &newrange, curr, MEM, 1);
+					if (rc)
 						return rc;
 					list_add_tail (&newbus->bus_list, &gbuses);
 					debug ("gbuses = NULL, Memory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
@@ -237,7 +238,8 @@ int __init ibmphp_rsrc_init (void)
 							return rc;
 					} else {
 						/* went through all the buses and didn't find ours, need to create a new bus node */
-						if ((rc = alloc_bus_range (&newbus, &newrange, curr, MEM, 1)))
+						rc = alloc_bus_range(&newbus, &newrange, curr, MEM, 1);
+						if (rc)
 							return rc;
 
 						list_add_tail (&newbus->bus_list, &gbuses);
@@ -248,7 +250,8 @@ int __init ibmphp_rsrc_init (void)
 				/* prefetchable memory */
 				if (list_empty (&gbuses)) {
 					/* no bus structure exists in place yet */
-					if ((rc = alloc_bus_range (&newbus, &newrange, curr, PFMEM, 1)))
+					rc = alloc_bus_range(&newbus, &newrange, curr, PFMEM, 1);
+					if (rc)
 						return rc;
 					list_add_tail (&newbus->bus_list, &gbuses);
 					debug ("gbuses = NULL, PFMemory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
@@ -261,7 +264,8 @@ int __init ibmphp_rsrc_init (void)
 							return rc;
 					} else {
 						/* went through all the buses and didn't find ours, need to create a new bus node */
-						if ((rc = alloc_bus_range (&newbus, &newrange, curr, PFMEM, 1)))
+						rc = alloc_bus_range(&newbus, &newrange, curr, PFMEM, 1);
+						if (rc)
 							return rc;
 						list_add_tail (&newbus->bus_list, &gbuses);
 						debug ("1st Bus, PFMemory Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
@@ -271,7 +275,8 @@ int __init ibmphp_rsrc_init (void)
 				/* IO */
 				if (list_empty (&gbuses)) {
 					/* no bus structure exists in place yet */
-					if ((rc = alloc_bus_range (&newbus, &newrange, curr, IO, 1)))
+					rc = alloc_bus_range(&newbus, &newrange, curr, IO, 1);
+					if (rc)
 						return rc;
 					list_add_tail (&newbus->bus_list, &gbuses);
 					debug ("gbuses = NULL, IO Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
@@ -283,7 +288,8 @@ int __init ibmphp_rsrc_init (void)
 							return rc;
 					} else {
 						/* went through all the buses and didn't find ours, need to create a new bus node */
-						if ((rc = alloc_bus_range (&newbus, &newrange, curr, IO, 1)))
+						rc = alloc_bus_range(&newbus, &newrange, curr, IO, 1);
+						if (rc)
 							return rc;
 						list_add_tail (&newbus->bus_list, &gbuses);
 						debug ("1st Bus, IO Primary Bus %x [%x - %x]\n", newbus->busno, newrange->start, newrange->end);
@@ -1038,7 +1044,9 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 		/* found our range */
 		if (!res_prev) {
 			/* first time in the loop */
-			if ((res_cur->start != range->start) && ((len_tmp = res_cur->start - 1 - range->start) >= res->len)) {
+			len_tmp = res_cur->start - 1 - range->start;
+
+			if ((res_cur->start != range->start) && (len_tmp >= res->len)) {
 				debug ("len_tmp = %x\n", len_tmp);
 
 				if ((len_tmp < len_cur) || (len_cur == 0)) {
@@ -1078,7 +1086,9 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 		}
 		if (!res_cur->next) {
 			/* last device on the range */
-			if ((range->end != res_cur->end) && ((len_tmp = range->end - (res_cur->end + 1)) >= res->len)) {
+			len_tmp = range->end - (res_cur->end + 1);
+
+			if ((range->end != res_cur->end) && (len_tmp >= res->len)) {
 				debug ("len_tmp = %x\n", len_tmp);
 				if ((len_tmp < len_cur) || (len_cur == 0)) {
 
@@ -1117,8 +1127,9 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 		if (res_prev) {
 			if (res_prev->rangeno != res_cur->rangeno) {
 				/* 1st device on this range */
-				if ((res_cur->start != range->start) &&
-					((len_tmp = res_cur->start - 1 - range->start) >= res->len)) {
+				len_tmp = res_cur->start - 1 - range->start;
+
+				if ((res_cur->start != range->start) &&	(len_tmp >= res->len)) {
 					if ((len_tmp < len_cur) || (len_cur == 0)) {
 						if ((range->start % tmp_divide) == 0) {
 							/* just perfect, starting address is divisible by length */
@@ -1153,7 +1164,9 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 				}
 			} else {
 				/* in the same range */
-				if ((len_tmp = res_cur->start - 1 - res_prev->end - 1) >= res->len) {
+				len_tmp = res_cur->start - 1 - res_prev->end - 1;
+
+				if (len_tmp >= res->len) {
 					if ((len_tmp < len_cur) || (len_cur == 0)) {
 						if (((res_prev->end + 1) % tmp_divide) == 0) {
 							/* just perfect, starting address's divisible by length */
@@ -1212,7 +1225,9 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 				break;
 		}
 		while (range) {
-			if ((len_tmp = range->end - range->start) >= res->len) {
+			len_tmp = range->end - range->start;
+
+			if (len_tmp >= res->len) {
 				if ((len_tmp < len_cur) || (len_cur == 0)) {
 					if ((range->start % tmp_divide) == 0) {
 						/* just perfect, starting address's divisible by length */
@@ -1276,7 +1291,9 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 					break;
 			}
 			while (range) {
-				if ((len_tmp = range->end - range->start) >= res->len) {
+				len_tmp = range->end - range->start;
+
+				if (len_tmp >= res->len) {
 					if ((len_tmp < len_cur) || (len_cur == 0)) {
 						if ((range->start % tmp_divide) == 0) {
 							/* just perfect, starting address's divisible by length */
@@ -1335,7 +1352,7 @@ int ibmphp_check_resource (struct resource_node *res, u8 bridge)
 				return -EINVAL;
 			}
 		}
-	}	/* end if(!res_cur) */
+	}	/* end if (!res_cur) */
 	return -EINVAL;
 }
 
diff --git a/drivers/pci/hotplug/pciehp.h b/drivers/pci/hotplug/pciehp.h
index 9e5a9fbb93d7..b11521953485 100644
--- a/drivers/pci/hotplug/pciehp.h
+++ b/drivers/pci/hotplug/pciehp.h
@@ -92,7 +92,7 @@ struct controller {
 	struct slot *slot;
 	wait_queue_head_t queue;	/* sleep & wake process */
 	u32 slot_cap;
-	u32 slot_ctrl;
+	u16 slot_ctrl;
 	struct timer_list poll_timer;
 	unsigned long cmd_started;	/* jiffies */
 	unsigned int cmd_busy:1;
diff --git a/drivers/pci/hotplug/pciehp_core.c b/drivers/pci/hotplug/pciehp_core.c
index 07aa722bb12c..3a5e7e28b874 100644
--- a/drivers/pci/hotplug/pciehp_core.c
+++ b/drivers/pci/hotplug/pciehp_core.c
@@ -262,6 +262,13 @@ static int pciehp_probe(struct pcie_device *dev)
 		goto err_out_none;
 	}
 
+	if (!dev->port->subordinate) {
+		/* Can happen if we run out of bus numbers during probe */
+		dev_err(&dev->device,
+			"Hotplug bridge without secondary bus, ignoring\n");
+		goto err_out_none;
+	}
+
 	ctrl = pcie_init(dev);
 	if (!ctrl) {
 		dev_err(&dev->device, "Controller initialization failed\n");
diff --git a/drivers/pci/hotplug/pciehp_hpc.c b/drivers/pci/hotplug/pciehp_hpc.c
index 9da84b8b27d8..0ebf754fc177 100644
--- a/drivers/pci/hotplug/pciehp_hpc.c
+++ b/drivers/pci/hotplug/pciehp_hpc.c
@@ -160,7 +160,7 @@ static void pcie_wait_cmd(struct controller *ctrl)
 	    ctrl->slot_ctrl & PCI_EXP_SLTCTL_CCIE)
 		rc = wait_event_timeout(ctrl->queue, !ctrl->cmd_busy, timeout);
 	else
-		rc = pcie_poll_cmd(ctrl, timeout);
+		rc = pcie_poll_cmd(ctrl, jiffies_to_msecs(timeout));
 
 	/*
 	 * Controllers with errata like Intel CF118 don't generate
@@ -171,9 +171,9 @@ static void pcie_wait_cmd(struct controller *ctrl)
 	 * interrupts.
 	 */
 	if (!rc)
-		ctrl_info(ctrl, "Timeout on hotplug command %#010x (issued %u msec ago)\n",
+		ctrl_info(ctrl, "Timeout on hotplug command %#06x (issued %u msec ago)\n",
 			  ctrl->slot_ctrl,
-			  jiffies_to_msecs(now - ctrl->cmd_started));
+			  jiffies_to_msecs(jiffies - ctrl->cmd_started));
 }
 
 /**
@@ -422,9 +422,9 @@ void pciehp_set_attention_status(struct slot *slot, u8 value)
 	default:
 		return;
 	}
+	pcie_write_cmd(ctrl, slot_cmd, PCI_EXP_SLTCTL_AIC);
 	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
 		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, slot_cmd);
-	pcie_write_cmd(ctrl, slot_cmd, PCI_EXP_SLTCTL_AIC);
 }
 
 void pciehp_green_led_on(struct slot *slot)
@@ -506,6 +506,8 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 {
 	struct controller *ctrl = (struct controller *)dev_id;
 	struct pci_dev *pdev = ctrl_dev(ctrl);
+	struct pci_bus *subordinate = pdev->subordinate;
+	struct pci_dev *dev;
 	struct slot *slot = ctrl->slot;
 	u16 detected, intr_loc;
 
@@ -539,6 +541,16 @@ static irqreturn_t pcie_isr(int irq, void *dev_id)
 		wake_up(&ctrl->queue);
 	}
 
+	if (subordinate) {
+		list_for_each_entry(dev, &subordinate->devices, bus_list) {
+			if (dev->ignore_hotplug) {
+				ctrl_dbg(ctrl, "ignoring hotplug event %#06x (%s requested no hotplug)\n",
+					 intr_loc, pci_name(dev));
+				return IRQ_HANDLED;
+			}
+		}
+	}
+
 	if (!(intr_loc & ~PCI_EXP_SLTSTA_CC))
 		return IRQ_HANDLED;
 
@@ -602,6 +614,8 @@ void pcie_enable_notification(struct controller *ctrl)
 		PCI_EXP_SLTCTL_DLLSCE);
 
 	pcie_write_cmd(ctrl, cmd, mask);
+	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
+		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, cmd);
 }
 
 static void pcie_disable_notification(struct controller *ctrl)
@@ -613,6 +627,8 @@ static void pcie_disable_notification(struct controller *ctrl)
 		PCI_EXP_SLTCTL_HPIE | PCI_EXP_SLTCTL_CCIE |
 		PCI_EXP_SLTCTL_DLLSCE);
 	pcie_write_cmd(ctrl, 0, mask);
+	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
+		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, 0);
 }
 
 /*
@@ -640,6 +656,8 @@ int pciehp_reset_slot(struct slot *slot, int probe)
 	stat_mask |= PCI_EXP_SLTSTA_DLLSC;
 
 	pcie_write_cmd(ctrl, 0, ctrl_mask);
+	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
+		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, 0);
 	if (pciehp_poll_mode)
 		del_timer_sync(&ctrl->poll_timer);
 
@@ -647,6 +665,8 @@ int pciehp_reset_slot(struct slot *slot, int probe)
 
 	pcie_capability_write_word(pdev, PCI_EXP_SLTSTA, stat_mask);
 	pcie_write_cmd(ctrl, ctrl_mask, ctrl_mask);
+	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
+		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, ctrl_mask);
 	if (pciehp_poll_mode)
 		int_poll_timeout(ctrl->poll_timer.data);
 
@@ -785,9 +805,6 @@ struct controller *pcie_init(struct pcie_device *dev)
 		PCI_EXP_SLTSTA_MRLSC | PCI_EXP_SLTSTA_PDC |
 		PCI_EXP_SLTSTA_CC | PCI_EXP_SLTSTA_DLLSC);
 
-	/* Disable software notification */
-	pcie_disable_notification(ctrl);
-
 	ctrl_info(ctrl, "Slot #%d AttnBtn%c AttnInd%c PwrInd%c PwrCtrl%c MRL%c Interlock%c NoCompl%c LLActRep%c\n",
 		(slot_cap & PCI_EXP_SLTCAP_PSN) >> 19,
 		FLAG(slot_cap, PCI_EXP_SLTCAP_ABP),
diff --git a/drivers/pci/hotplug/pciehp_pci.c b/drivers/pci/hotplug/pciehp_pci.c
index 5f871f4c4af1..9e69403be632 100644
--- a/drivers/pci/hotplug/pciehp_pci.c
+++ b/drivers/pci/hotplug/pciehp_pci.c
@@ -65,14 +65,7 @@ int pciehp_configure_device(struct slot *p_slot)
 			pci_hp_add_bridge(dev);
 
 	pci_assign_unassigned_bridge_resources(bridge);
-
-	list_for_each_entry(dev, &parent->devices, bus_list) {
-		if ((dev->class >> 16) == PCI_BASE_CLASS_DISPLAY)
-			continue;
-
-		pci_configure_slot(dev);
-	}
-
+	pcie_bus_configure_settings(parent);
 	pci_bus_add_devices(parent);
 
  out:
diff --git a/drivers/pci/hotplug/pcihp_slot.c b/drivers/pci/hotplug/pcihp_slot.c
deleted file mode 100644
index e246a10a0d2c..000000000000
--- a/drivers/pci/hotplug/pcihp_slot.c
+++ /dev/null
@@ -1,180 +0,0 @@
-/*
- * Copyright (C) 1995,2001 Compaq Computer Corporation
- * Copyright (C) 2001 Greg Kroah-Hartman (greg@kroah.com)
- * Copyright (C) 2001 IBM Corp.
- * Copyright (C) 2003-2004 Intel Corporation
- * (c) Copyright 2009 Hewlett-Packard Development Company, L.P.
- *
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-
-#include <linux/pci.h>
-#include <linux/export.h>
-#include <linux/pci_hotplug.h>
-
-static struct hpp_type0 pci_default_type0 = {
-	.revision = 1,
-	.cache_line_size = 8,
-	.latency_timer = 0x40,
-	.enable_serr = 0,
-	.enable_perr = 0,
-};
-
-static void program_hpp_type0(struct pci_dev *dev, struct hpp_type0 *hpp)
-{
-	u16 pci_cmd, pci_bctl;
-
-	if (!hpp) {
-		/*
-		 * Perhaps we *should* use default settings for PCIe, but
-		 * pciehp didn't, so we won't either.
-		 */
-		if (pci_is_pcie(dev))
-			return;
-		dev_info(&dev->dev, "using default PCI settings\n");
-		hpp = &pci_default_type0;
-	}
-
-	if (hpp->revision > 1) {
-		dev_warn(&dev->dev,
-			 "PCI settings rev %d not supported; using defaults\n",
-			 hpp->revision);
-		hpp = &pci_default_type0;
-	}
-
-	pci_write_config_byte(dev, PCI_CACHE_LINE_SIZE, hpp->cache_line_size);
-	pci_write_config_byte(dev, PCI_LATENCY_TIMER, hpp->latency_timer);
-	pci_read_config_word(dev, PCI_COMMAND, &pci_cmd);
-	if (hpp->enable_serr)
-		pci_cmd |= PCI_COMMAND_SERR;
-	else
-		pci_cmd &= ~PCI_COMMAND_SERR;
-	if (hpp->enable_perr)
-		pci_cmd |= PCI_COMMAND_PARITY;
-	else
-		pci_cmd &= ~PCI_COMMAND_PARITY;
-	pci_write_config_word(dev, PCI_COMMAND, pci_cmd);
-
-	/* Program bridge control value */
-	if ((dev->class >> 8) == PCI_CLASS_BRIDGE_PCI) {
-		pci_write_config_byte(dev, PCI_SEC_LATENCY_TIMER,
-				      hpp->latency_timer);
-		pci_read_config_word(dev, PCI_BRIDGE_CONTROL, &pci_bctl);
-		if (hpp->enable_serr)
-			pci_bctl |= PCI_BRIDGE_CTL_SERR;
-		else
-			pci_bctl &= ~PCI_BRIDGE_CTL_SERR;
-		if (hpp->enable_perr)
-			pci_bctl |= PCI_BRIDGE_CTL_PARITY;
-		else
-			pci_bctl &= ~PCI_BRIDGE_CTL_PARITY;
-		pci_write_config_word(dev, PCI_BRIDGE_CONTROL, pci_bctl);
-	}
-}
-
-static void program_hpp_type1(struct pci_dev *dev, struct hpp_type1 *hpp)
-{
-	if (hpp)
-		dev_warn(&dev->dev, "PCI-X settings not supported\n");
-}
-
-static void program_hpp_type2(struct pci_dev *dev, struct hpp_type2 *hpp)
-{
-	int pos;
-	u32 reg32;
-
-	if (!hpp)
-		return;
-
-	if (hpp->revision > 1) {
-		dev_warn(&dev->dev, "PCIe settings rev %d not supported\n",
-			 hpp->revision);
-		return;
-	}
-
-	/* Initialize Device Control Register */
-	pcie_capability_clear_and_set_word(dev, PCI_EXP_DEVCTL,
-			~hpp->pci_exp_devctl_and, hpp->pci_exp_devctl_or);
-
-	/* Initialize Link Control Register */
-	if (dev->subordinate)
-		pcie_capability_clear_and_set_word(dev, PCI_EXP_LNKCTL,
-			~hpp->pci_exp_lnkctl_and, hpp->pci_exp_lnkctl_or);
-
-	/* Find Advanced Error Reporting Enhanced Capability */
-	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
-	if (!pos)
-		return;
-
-	/* Initialize Uncorrectable Error Mask Register */
-	pci_read_config_dword(dev, pos + PCI_ERR_UNCOR_MASK, &reg32);
-	reg32 = (reg32 & hpp->unc_err_mask_and) | hpp->unc_err_mask_or;
-	pci_write_config_dword(dev, pos + PCI_ERR_UNCOR_MASK, reg32);
-
-	/* Initialize Uncorrectable Error Severity Register */
-	pci_read_config_dword(dev, pos + PCI_ERR_UNCOR_SEVER, &reg32);
-	reg32 = (reg32 & hpp->unc_err_sever_and) | hpp->unc_err_sever_or;
-	pci_write_config_dword(dev, pos + PCI_ERR_UNCOR_SEVER, reg32);
-
-	/* Initialize Correctable Error Mask Register */
-	pci_read_config_dword(dev, pos + PCI_ERR_COR_MASK, &reg32);
-	reg32 = (reg32 & hpp->cor_err_mask_and) | hpp->cor_err_mask_or;
-	pci_write_config_dword(dev, pos + PCI_ERR_COR_MASK, reg32);
-
-	/* Initialize Advanced Error Capabilities and Control Register */
-	pci_read_config_dword(dev, pos + PCI_ERR_CAP, &reg32);
-	reg32 = (reg32 & hpp->adv_err_cap_and) | hpp->adv_err_cap_or;
-	pci_write_config_dword(dev, pos + PCI_ERR_CAP, reg32);
-
-	/*
-	 * FIXME: The following two registers are not supported yet.
-	 *
-	 *   o Secondary Uncorrectable Error Severity Register
-	 *   o Secondary Uncorrectable Error Mask Register
-	 */
-}
-
-void pci_configure_slot(struct pci_dev *dev)
-{
-	struct pci_dev *cdev;
-	struct hotplug_params hpp;
-	int ret;
-
-	if (!(dev->hdr_type == PCI_HEADER_TYPE_NORMAL ||
-			(dev->hdr_type == PCI_HEADER_TYPE_BRIDGE &&
-			(dev->class >> 8) == PCI_CLASS_BRIDGE_PCI)))
-		return;
-
-	pcie_bus_configure_settings(dev->bus);
-
-	memset(&hpp, 0, sizeof(hpp));
-	ret = pci_get_hp_params(dev, &hpp);
-	if (ret)
-		dev_warn(&dev->dev, "no hotplug settings from platform\n");
-
-	program_hpp_type2(dev, hpp.t2);
-	program_hpp_type1(dev, hpp.t1);
-	program_hpp_type0(dev, hpp.t0);
-
-	if (dev->subordinate) {
-		list_for_each_entry(cdev, &dev->subordinate->devices,
-				    bus_list)
-			pci_configure_slot(cdev);
-	}
-}
-EXPORT_SYMBOL_GPL(pci_configure_slot);
diff --git a/drivers/pci/hotplug/shpchp_ctrl.c b/drivers/pci/hotplug/shpchp_ctrl.c
index a81fb67ea9a1..10c7927599b3 100644
--- a/drivers/pci/hotplug/shpchp_ctrl.c
+++ b/drivers/pci/hotplug/shpchp_ctrl.c
@@ -195,7 +195,8 @@ static int change_bus_speed(struct controller *ctrl, struct slot *p_slot,
 	int rc = 0;
 
 	ctrl_dbg(ctrl, "Change speed to %d\n", speed);
-	if ((rc = p_slot->hpc_ops->set_bus_speed_mode(p_slot, speed))) {
+	rc = p_slot->hpc_ops->set_bus_speed_mode(p_slot, speed);
+	if (rc) {
 		ctrl_err(ctrl, "%s: Issue of set bus speed mode command failed\n",
 			 __func__);
 		return WRONG_BUS_FREQUENCY;
@@ -261,14 +262,16 @@ static int board_added(struct slot *p_slot)
 	}
 
 	if ((ctrl->pci_dev->vendor == 0x8086) && (ctrl->pci_dev->device == 0x0332)) {
-		if ((rc = p_slot->hpc_ops->set_bus_speed_mode(p_slot, PCI_SPEED_33MHz))) {
+		rc = p_slot->hpc_ops->set_bus_speed_mode(p_slot, PCI_SPEED_33MHz);
+		if (rc) {
 			ctrl_err(ctrl, "%s: Issue of set bus speed mode command failed\n",
 				 __func__);
 			return WRONG_BUS_FREQUENCY;
 		}
 
 		/* turn on board, blink green LED, turn off Amber LED */
-		if ((rc = p_slot->hpc_ops->slot_enable(p_slot))) {
+		rc = p_slot->hpc_ops->slot_enable(p_slot);
+		if (rc) {
 			ctrl_err(ctrl, "Issue of Slot Enable command failed\n");
 			return rc;
 		}
@@ -296,7 +299,8 @@ static int board_added(struct slot *p_slot)
 		return rc;
 
 	/* turn on board, blink green LED, turn off Amber LED */
-	if ((rc = p_slot->hpc_ops->slot_enable(p_slot))) {
+	rc = p_slot->hpc_ops->slot_enable(p_slot);
+	if (rc) {
 		ctrl_err(ctrl, "Issue of Slot Enable command failed\n");
 		return rc;
 	}
@@ -595,7 +599,7 @@ static int shpchp_enable_slot (struct slot *p_slot)
 	ctrl_dbg(ctrl, "%s: p_slot->pwr_save %x\n", __func__, p_slot->pwr_save);
 	p_slot->hpc_ops->get_latch_status(p_slot, &getstatus);
 
-	if(((p_slot->ctrl->pci_dev->vendor == PCI_VENDOR_ID_AMD) ||
+	if (((p_slot->ctrl->pci_dev->vendor == PCI_VENDOR_ID_AMD) ||
 	    (p_slot->ctrl->pci_dev->device == PCI_DEVICE_ID_AMD_POGO_7458))
 	     && p_slot->ctrl->num_slots == 1) {
 		/* handle amd pogo errata; this must be done before enable  */
diff --git a/drivers/pci/hotplug/shpchp_hpc.c b/drivers/pci/hotplug/shpchp_hpc.c
index 29e22352822c..7d223e9080ef 100644
--- a/drivers/pci/hotplug/shpchp_hpc.c
+++ b/drivers/pci/hotplug/shpchp_hpc.c
@@ -466,7 +466,8 @@ static int hpc_get_adapter_speed(struct slot *slot, enum pci_bus_speed *value)
 	u8 m66_cap  = !!(slot_reg & MHZ66_CAP);
 	u8 pi, pcix_cap;
 
-	if ((retval = hpc_get_prog_int(slot, &pi)))
+	retval = hpc_get_prog_int(slot, &pi);
+	if (retval)
 		return retval;
 
 	switch (pi) {
@@ -798,7 +799,7 @@ static irqreturn_t shpc_isr(int irq, void *dev_id)
 
 	ctrl_dbg(ctrl, "%s: intr_loc = %x\n", __func__, intr_loc);
 
-	if(!shpchp_poll_mode) {
+	if (!shpchp_poll_mode) {
 		/*
 		 * Mask Global Interrupt Mask - see implementation
 		 * note on p. 139 of SHPC spec rev 1.0
diff --git a/drivers/pci/hotplug/shpchp_pci.c b/drivers/pci/hotplug/shpchp_pci.c
index 469454e0cc48..f8cd3a27e351 100644
--- a/drivers/pci/hotplug/shpchp_pci.c
+++ b/drivers/pci/hotplug/shpchp_pci.c
@@ -69,13 +69,7 @@ int shpchp_configure_device(struct slot *p_slot)
 	}
 
 	pci_assign_unassigned_bridge_resources(bridge);
-
-	list_for_each_entry(dev, &parent->devices, bus_list) {
-		if (PCI_SLOT(dev->devfn) != p_slot->device)
-			continue;
-		pci_configure_slot(dev);
-	}
-
+	pcie_bus_configure_settings(parent);
 	pci_bus_add_devices(parent);
 
  out:
diff --git a/drivers/pci/iov.c b/drivers/pci/iov.c
index cb6f24740ee3..4d109c07294a 100644
--- a/drivers/pci/iov.c
+++ b/drivers/pci/iov.c
@@ -633,7 +633,7 @@ int pci_vfs_assigned(struct pci_dev *dev)
 		 * our dev as the physical function and the assigned bit is set
 		 */
 		if (vfdev->is_virtfn && (vfdev->physfn == dev) &&
-		    (vfdev->dev_flags & PCI_DEV_FLAGS_ASSIGNED))
+			pci_is_dev_assigned(vfdev))
 			vfs_assigned++;
 
 		vfdev = pci_get_device(dev->vendor, dev_id, vfdev);
diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 5a40516444f3..9fab30af0e75 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -56,16 +56,6 @@ void __weak arch_teardown_msi_irq(unsigned int irq)
 	chip->teardown_irq(chip, irq);
 }
 
-int __weak arch_msi_check_device(struct pci_dev *dev, int nvec, int type)
-{
-	struct msi_chip *chip = dev->bus->msi;
-
-	if (!chip || !chip->check_device)
-		return 0;
-
-	return chip->check_device(chip, dev, nvec, type);
-}
-
 int __weak arch_setup_msi_irqs(struct pci_dev *dev, int nvec, int type)
 {
 	struct msi_desc *entry;
@@ -130,7 +120,7 @@ static void default_restore_msi_irq(struct pci_dev *dev, int irq)
 	}
 
 	if (entry)
-		write_msi_msg(irq, &entry->msg);
+		__write_msi_msg(entry, &entry->msg);
 }
 
 void __weak arch_restore_msi_irqs(struct pci_dev *dev)
@@ -312,6 +302,7 @@ void get_cached_msi_msg(unsigned int irq, struct msi_msg *msg)
 
 	__get_cached_msi_msg(entry, msg);
 }
+EXPORT_SYMBOL_GPL(get_cached_msi_msg);
 
 void __write_msi_msg(struct msi_desc *entry, struct msi_msg *msg)
 {
@@ -356,6 +347,7 @@ void write_msi_msg(unsigned int irq, struct msi_msg *msg)
 
 	__write_msi_msg(entry, msg);
 }
+EXPORT_SYMBOL_GPL(write_msi_msg);
 
 static void free_msi_irqs(struct pci_dev *dev)
 {
@@ -384,17 +376,6 @@ static void free_msi_irqs(struct pci_dev *dev)
 				iounmap(entry->mask_base);
 		}
 
-		/*
-		 * Its possible that we get into this path
-		 * When populate_msi_sysfs fails, which means the entries
-		 * were not registered with sysfs.  In that case don't
-		 * unregister them.
-		 */
-		if (entry->kobj.parent) {
-			kobject_del(&entry->kobj);
-			kobject_put(&entry->kobj);
-		}
-
 		list_del(&entry->list);
 		kfree(entry);
 	}
@@ -595,7 +576,6 @@ static struct msi_desc *msi_setup_entry(struct pci_dev *dev)
 	entry->msi_attrib.entry_nr	= 0;
 	entry->msi_attrib.maskbit	= !!(control & PCI_MSI_FLAGS_MASKBIT);
 	entry->msi_attrib.default_irq	= dev->irq;	/* Save IOAPIC IRQ */
-	entry->msi_attrib.pos		= dev->msi_cap;
 	entry->msi_attrib.multi_cap	= (control & PCI_MSI_FLAGS_QMASK) >> 1;
 
 	if (control & PCI_MSI_FLAGS_64BIT)
@@ -699,7 +679,6 @@ static int msix_setup_entries(struct pci_dev *dev, void __iomem *base,
 		entry->msi_attrib.is_64		= 1;
 		entry->msi_attrib.entry_nr	= entries[i].entry;
 		entry->msi_attrib.default_irq	= dev->irq;
-		entry->msi_attrib.pos		= dev->msix_cap;
 		entry->mask_base		= base;
 
 		list_add_tail(&entry->list, &dev->msi_list);
@@ -806,23 +785,24 @@ out_free:
 }
 
 /**
- * pci_msi_check_device - check whether MSI may be enabled on a device
+ * pci_msi_supported - check whether MSI may be enabled on a device
  * @dev: pointer to the pci_dev data structure of MSI device function
  * @nvec: how many MSIs have been requested ?
- * @type: are we checking for MSI or MSI-X ?
  *
  * Look at global flags, the device itself, and its parent buses
  * to determine if MSI/-X are supported for the device. If MSI/-X is
- * supported return 0, else return an error code.
+ * supported return 1, else return 0.
  **/
-static int pci_msi_check_device(struct pci_dev *dev, int nvec, int type)
+static int pci_msi_supported(struct pci_dev *dev, int nvec)
 {
 	struct pci_bus *bus;
-	int ret;
 
 	/* MSI must be globally enabled and supported by the device */
-	if (!pci_msi_enable || !dev || dev->no_msi)
-		return -EINVAL;
+	if (!pci_msi_enable)
+		return 0;
+
+	if (!dev || dev->no_msi || dev->current_state != PCI_D0)
+		return 0;
 
 	/*
 	 * You can't ask to have 0 or less MSIs configured.
@@ -830,7 +810,7 @@ static int pci_msi_check_device(struct pci_dev *dev, int nvec, int type)
 	 *  b) the list manipulation code assumes nvec >= 1.
 	 */
 	if (nvec < 1)
-		return -ERANGE;
+		return 0;
 
 	/*
 	 * Any bridge which does NOT route MSI transactions from its
@@ -841,13 +821,9 @@ static int pci_msi_check_device(struct pci_dev *dev, int nvec, int type)
 	 */
 	for (bus = dev->bus; bus; bus = bus->parent)
 		if (bus->bus_flags & PCI_BUS_FLAGS_NO_MSI)
-			return -EINVAL;
-
-	ret = arch_msi_check_device(dev, nvec, type);
-	if (ret)
-		return ret;
+			return 0;
 
-	return 0;
+	return 1;
 }
 
 /**
@@ -946,15 +922,14 @@ EXPORT_SYMBOL(pci_msix_vec_count);
  **/
 int pci_enable_msix(struct pci_dev *dev, struct msix_entry *entries, int nvec)
 {
-	int status, nr_entries;
+	int nr_entries;
 	int i, j;
 
-	if (!entries || !dev->msix_cap || dev->current_state != PCI_D0)
+	if (!pci_msi_supported(dev, nvec))
 		return -EINVAL;
 
-	status = pci_msi_check_device(dev, nvec, PCI_CAP_ID_MSIX);
-	if (status)
-		return status;
+	if (!entries)
+		return -EINVAL;
 
 	nr_entries = pci_msix_vec_count(dev);
 	if (nr_entries < 0)
@@ -978,8 +953,7 @@ int pci_enable_msix(struct pci_dev *dev, struct msix_entry *entries, int nvec)
 		dev_info(&dev->dev, "can't enable MSI-X (MSI IRQ already assigned)\n");
 		return -EINVAL;
 	}
-	status = msix_capability_init(dev, entries, nvec);
-	return status;
+	return msix_capability_init(dev, entries, nvec);
 }
 EXPORT_SYMBOL(pci_enable_msix);
 
@@ -1062,7 +1036,7 @@ int pci_enable_msi_range(struct pci_dev *dev, int minvec, int maxvec)
 	int nvec;
 	int rc;
 
-	if (dev->current_state != PCI_D0)
+	if (!pci_msi_supported(dev, minvec))
 		return -EINVAL;
 
 	WARN_ON(!!dev->msi_enabled);
@@ -1086,17 +1060,6 @@ int pci_enable_msi_range(struct pci_dev *dev, int minvec, int maxvec)
 		nvec = maxvec;
 
 	do {
-		rc = pci_msi_check_device(dev, nvec, PCI_CAP_ID_MSI);
-		if (rc < 0) {
-			return rc;
-		} else if (rc > 0) {
-			if (rc < minvec)
-				return -ENOSPC;
-			nvec = rc;
-		}
-	} while (rc);
-
-	do {
 		rc = msi_capability_init(dev, nvec);
 		if (rc < 0) {
 			return rc;
diff --git a/drivers/pci/pci-acpi.c b/drivers/pci/pci-acpi.c
index 37263b0ebfe3..6ebf8edc5f3c 100644
--- a/drivers/pci/pci-acpi.c
+++ b/drivers/pci/pci-acpi.c
@@ -10,6 +10,7 @@
 #include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/pci.h>
+#include <linux/pci_hotplug.h>
 #include <linux/module.h>
 #include <linux/pci-aspm.h>
 #include <linux/pci-acpi.h>
@@ -17,6 +18,267 @@
 #include <linux/pm_qos.h>
 #include "pci.h"
 
+phys_addr_t acpi_pci_root_get_mcfg_addr(acpi_handle handle)
+{
+	acpi_status status = AE_NOT_EXIST;
+	unsigned long long mcfg_addr;
+
+	if (handle)
+		status = acpi_evaluate_integer(handle, METHOD_NAME__CBA,
+					       NULL, &mcfg_addr);
+	if (ACPI_FAILURE(status))
+		return 0;
+
+	return (phys_addr_t)mcfg_addr;
+}
+
+static acpi_status decode_type0_hpx_record(union acpi_object *record,
+					   struct hotplug_params *hpx)
+{
+	int i;
+	union acpi_object *fields = record->package.elements;
+	u32 revision = fields[1].integer.value;
+
+	switch (revision) {
+	case 1:
+		if (record->package.count != 6)
+			return AE_ERROR;
+		for (i = 2; i < 6; i++)
+			if (fields[i].type != ACPI_TYPE_INTEGER)
+				return AE_ERROR;
+		hpx->t0 = &hpx->type0_data;
+		hpx->t0->revision        = revision;
+		hpx->t0->cache_line_size = fields[2].integer.value;
+		hpx->t0->latency_timer   = fields[3].integer.value;
+		hpx->t0->enable_serr     = fields[4].integer.value;
+		hpx->t0->enable_perr     = fields[5].integer.value;
+		break;
+	default:
+		printk(KERN_WARNING
+		       "%s: Type 0 Revision %d record not supported\n",
+		       __func__, revision);
+		return AE_ERROR;
+	}
+	return AE_OK;
+}
+
+static acpi_status decode_type1_hpx_record(union acpi_object *record,
+					   struct hotplug_params *hpx)
+{
+	int i;
+	union acpi_object *fields = record->package.elements;
+	u32 revision = fields[1].integer.value;
+
+	switch (revision) {
+	case 1:
+		if (record->package.count != 5)
+			return AE_ERROR;
+		for (i = 2; i < 5; i++)
+			if (fields[i].type != ACPI_TYPE_INTEGER)
+				return AE_ERROR;
+		hpx->t1 = &hpx->type1_data;
+		hpx->t1->revision      = revision;
+		hpx->t1->max_mem_read  = fields[2].integer.value;
+		hpx->t1->avg_max_split = fields[3].integer.value;
+		hpx->t1->tot_max_split = fields[4].integer.value;
+		break;
+	default:
+		printk(KERN_WARNING
+		       "%s: Type 1 Revision %d record not supported\n",
+		       __func__, revision);
+		return AE_ERROR;
+	}
+	return AE_OK;
+}
+
+static acpi_status decode_type2_hpx_record(union acpi_object *record,
+					   struct hotplug_params *hpx)
+{
+	int i;
+	union acpi_object *fields = record->package.elements;
+	u32 revision = fields[1].integer.value;
+
+	switch (revision) {
+	case 1:
+		if (record->package.count != 18)
+			return AE_ERROR;
+		for (i = 2; i < 18; i++)
+			if (fields[i].type != ACPI_TYPE_INTEGER)
+				return AE_ERROR;
+		hpx->t2 = &hpx->type2_data;
+		hpx->t2->revision      = revision;
+		hpx->t2->unc_err_mask_and      = fields[2].integer.value;
+		hpx->t2->unc_err_mask_or       = fields[3].integer.value;
+		hpx->t2->unc_err_sever_and     = fields[4].integer.value;
+		hpx->t2->unc_err_sever_or      = fields[5].integer.value;
+		hpx->t2->cor_err_mask_and      = fields[6].integer.value;
+		hpx->t2->cor_err_mask_or       = fields[7].integer.value;
+		hpx->t2->adv_err_cap_and       = fields[8].integer.value;
+		hpx->t2->adv_err_cap_or        = fields[9].integer.value;
+		hpx->t2->pci_exp_devctl_and    = fields[10].integer.value;
+		hpx->t2->pci_exp_devctl_or     = fields[11].integer.value;
+		hpx->t2->pci_exp_lnkctl_and    = fields[12].integer.value;
+		hpx->t2->pci_exp_lnkctl_or     = fields[13].integer.value;
+		hpx->t2->sec_unc_err_sever_and = fields[14].integer.value;
+		hpx->t2->sec_unc_err_sever_or  = fields[15].integer.value;
+		hpx->t2->sec_unc_err_mask_and  = fields[16].integer.value;
+		hpx->t2->sec_unc_err_mask_or   = fields[17].integer.value;
+		break;
+	default:
+		printk(KERN_WARNING
+		       "%s: Type 2 Revision %d record not supported\n",
+		       __func__, revision);
+		return AE_ERROR;
+	}
+	return AE_OK;
+}
+
+static acpi_status acpi_run_hpx(acpi_handle handle, struct hotplug_params *hpx)
+{
+	acpi_status status;
+	struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER, NULL};
+	union acpi_object *package, *record, *fields;
+	u32 type;
+	int i;
+
+	/* Clear the return buffer with zeros */
+	memset(hpx, 0, sizeof(struct hotplug_params));
+
+	status = acpi_evaluate_object(handle, "_HPX", NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		return status;
+
+	package = (union acpi_object *)buffer.pointer;
+	if (package->type != ACPI_TYPE_PACKAGE) {
+		status = AE_ERROR;
+		goto exit;
+	}
+
+	for (i = 0; i < package->package.count; i++) {
+		record = &package->package.elements[i];
+		if (record->type != ACPI_TYPE_PACKAGE) {
+			status = AE_ERROR;
+			goto exit;
+		}
+
+		fields = record->package.elements;
+		if (fields[0].type != ACPI_TYPE_INTEGER ||
+		    fields[1].type != ACPI_TYPE_INTEGER) {
+			status = AE_ERROR;
+			goto exit;
+		}
+
+		type = fields[0].integer.value;
+		switch (type) {
+		case 0:
+			status = decode_type0_hpx_record(record, hpx);
+			if (ACPI_FAILURE(status))
+				goto exit;
+			break;
+		case 1:
+			status = decode_type1_hpx_record(record, hpx);
+			if (ACPI_FAILURE(status))
+				goto exit;
+			break;
+		case 2:
+			status = decode_type2_hpx_record(record, hpx);
+			if (ACPI_FAILURE(status))
+				goto exit;
+			break;
+		default:
+			printk(KERN_ERR "%s: Type %d record not supported\n",
+			       __func__, type);
+			status = AE_ERROR;
+			goto exit;
+		}
+	}
+ exit:
+	kfree(buffer.pointer);
+	return status;
+}
+
+static acpi_status acpi_run_hpp(acpi_handle handle, struct hotplug_params *hpp)
+{
+	acpi_status status;
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *package, *fields;
+	int i;
+
+	memset(hpp, 0, sizeof(struct hotplug_params));
+
+	status = acpi_evaluate_object(handle, "_HPP", NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		return status;
+
+	package = (union acpi_object *) buffer.pointer;
+	if (package->type != ACPI_TYPE_PACKAGE ||
+	    package->package.count != 4) {
+		status = AE_ERROR;
+		goto exit;
+	}
+
+	fields = package->package.elements;
+	for (i = 0; i < 4; i++) {
+		if (fields[i].type != ACPI_TYPE_INTEGER) {
+			status = AE_ERROR;
+			goto exit;
+		}
+	}
+
+	hpp->t0 = &hpp->type0_data;
+	hpp->t0->revision        = 1;
+	hpp->t0->cache_line_size = fields[0].integer.value;
+	hpp->t0->latency_timer   = fields[1].integer.value;
+	hpp->t0->enable_serr     = fields[2].integer.value;
+	hpp->t0->enable_perr     = fields[3].integer.value;
+
+exit:
+	kfree(buffer.pointer);
+	return status;
+}
+
+/* pci_get_hp_params
+ *
+ * @dev - the pci_dev for which we want parameters
+ * @hpp - allocated by the caller
+ */
+int pci_get_hp_params(struct pci_dev *dev, struct hotplug_params *hpp)
+{
+	acpi_status status;
+	acpi_handle handle, phandle;
+	struct pci_bus *pbus;
+
+	handle = NULL;
+	for (pbus = dev->bus; pbus; pbus = pbus->parent) {
+		handle = acpi_pci_get_bridge_handle(pbus);
+		if (handle)
+			break;
+	}
+
+	/*
+	 * _HPP settings apply to all child buses, until another _HPP is
+	 * encountered. If we don't find an _HPP for the input pci dev,
+	 * look for it in the parent device scope since that would apply to
+	 * this pci dev.
+	 */
+	while (handle) {
+		status = acpi_run_hpx(handle, hpp);
+		if (ACPI_SUCCESS(status))
+			return 0;
+		status = acpi_run_hpp(handle, hpp);
+		if (ACPI_SUCCESS(status))
+			return 0;
+		if (acpi_is_root_bridge(handle))
+			break;
+		status = acpi_get_parent(handle, &phandle);
+		if (ACPI_FAILURE(status))
+			break;
+		handle = phandle;
+	}
+	return -ENODEV;
+}
+EXPORT_SYMBOL_GPL(pci_get_hp_params);
+
 /**
  * pci_acpi_wake_bus - Root bus wakeup notification fork function.
  * @work: Work item to handle.
@@ -84,20 +346,6 @@ acpi_status pci_acpi_add_pm_notifier(struct acpi_device *dev,
 	return acpi_add_pm_notifier(dev, &pci_dev->dev, pci_acpi_wake_dev);
 }
 
-phys_addr_t acpi_pci_root_get_mcfg_addr(acpi_handle handle)
-{
-	acpi_status status = AE_NOT_EXIST;
-	unsigned long long mcfg_addr;
-
-	if (handle)
-		status = acpi_evaluate_integer(handle, METHOD_NAME__CBA,
-					       NULL, &mcfg_addr);
-	if (ACPI_FAILURE(status))
-		return 0;
-
-	return (phys_addr_t)mcfg_addr;
-}
-
 /*
  * _SxD returns the D-state with the highest power
  * (lowest D-state number) supported in the S-state "x".
diff --git a/drivers/pci/pci-driver.c b/drivers/pci/pci-driver.c
index d04c5adafc16..2b3c89425bb5 100644
--- a/drivers/pci/pci-driver.c
+++ b/drivers/pci/pci-driver.c
@@ -55,7 +55,6 @@ int pci_add_dynid(struct pci_driver *drv,
 		  unsigned long driver_data)
 {
 	struct pci_dynid *dynid;
-	int retval;
 
 	dynid = kzalloc(sizeof(*dynid), GFP_KERNEL);
 	if (!dynid)
@@ -73,9 +72,7 @@ int pci_add_dynid(struct pci_driver *drv,
 	list_add_tail(&dynid->node, &drv->dynids.list);
 	spin_unlock(&drv->dynids.lock);
 
-	retval = driver_attach(&drv->driver);
-
-	return retval;
+	return driver_attach(&drv->driver);
 }
 EXPORT_SYMBOL_GPL(pci_add_dynid);
 
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index 9ff0a901ecf7..92b6d9ab00e4 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -177,7 +177,7 @@ static ssize_t modalias_show(struct device *dev, struct device_attribute *attr,
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 
-	return sprintf(buf, "pci:v%08Xd%08Xsv%08Xsd%08Xbc%02Xsc%02Xi%02x\n",
+	return sprintf(buf, "pci:v%08Xd%08Xsv%08Xsd%08Xbc%02Xsc%02Xi%02X\n",
 		       pci_dev->vendor, pci_dev->device,
 		       pci_dev->subsystem_vendor, pci_dev->subsystem_device,
 		       (u8)(pci_dev->class >> 16), (u8)(pci_dev->class >> 8),
@@ -250,46 +250,45 @@ static ssize_t msi_bus_show(struct device *dev, struct device_attribute *attr,
 			    char *buf)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_bus *subordinate = pdev->subordinate;
 
-	if (!pdev->subordinate)
-		return 0;
-
-	return sprintf(buf, "%u\n",
-		       !(pdev->subordinate->bus_flags & PCI_BUS_FLAGS_NO_MSI));
+	return sprintf(buf, "%u\n", subordinate ?
+		       !(subordinate->bus_flags & PCI_BUS_FLAGS_NO_MSI)
+			   : !pdev->no_msi);
 }
 
 static ssize_t msi_bus_store(struct device *dev, struct device_attribute *attr,
 			     const char *buf, size_t count)
 {
 	struct pci_dev *pdev = to_pci_dev(dev);
+	struct pci_bus *subordinate = pdev->subordinate;
 	unsigned long val;
 
 	if (kstrtoul(buf, 0, &val) < 0)
 		return -EINVAL;
 
-	/*
-	 * Bad things may happen if the no_msi flag is changed
-	 * while drivers are loaded.
-	 */
 	if (!capable(CAP_SYS_ADMIN))
 		return -EPERM;
 
 	/*
-	 * Maybe devices without subordinate buses shouldn't have this
-	 * attribute in the first place?
+	 * "no_msi" and "bus_flags" only affect what happens when a driver
+	 * requests MSI or MSI-X.  They don't affect any drivers that have
+	 * already requested MSI or MSI-X.
 	 */
-	if (!pdev->subordinate)
+	if (!subordinate) {
+		pdev->no_msi = !val;
+		dev_info(&pdev->dev, "MSI/MSI-X %s for future drivers\n",
+			 val ? "allowed" : "disallowed");
 		return count;
-
-	/* Is the flag going to change, or keep the value it already had? */
-	if (!(pdev->subordinate->bus_flags & PCI_BUS_FLAGS_NO_MSI) ^
-	    !!val) {
-		pdev->subordinate->bus_flags ^= PCI_BUS_FLAGS_NO_MSI;
-
-		dev_warn(&pdev->dev, "forced subordinate bus to%s support MSI, bad things could happen\n",
-			 val ? "" : " not");
 	}
 
+	if (val)
+		subordinate->bus_flags &= ~PCI_BUS_FLAGS_NO_MSI;
+	else
+		subordinate->bus_flags |= PCI_BUS_FLAGS_NO_MSI;
+
+	dev_info(&subordinate->dev, "MSI/MSI-X %s for future drivers of devices on this bus\n",
+		 val ? "allowed" : "disallowed");
 	return count;
 }
 static DEVICE_ATTR_RW(msi_bus);
diff --git a/drivers/pci/pci.c b/drivers/pci/pci.c
index 2c9ac70254e2..625a4ace10b4 100644
--- a/drivers/pci/pci.c
+++ b/drivers/pci/pci.c
@@ -1003,12 +1003,19 @@ int pci_save_state(struct pci_dev *dev)
 	for (i = 0; i < 16; i++)
 		pci_read_config_dword(dev, i * 4, &dev->saved_config_space[i]);
 	dev->state_saved = true;
-	if ((i = pci_save_pcie_state(dev)) != 0)
+
+	i = pci_save_pcie_state(dev);
+	if (i != 0)
 		return i;
-	if ((i = pci_save_pcix_state(dev)) != 0)
+
+	i = pci_save_pcix_state(dev);
+	if (i != 0)
 		return i;
-	if ((i = pci_save_vc_state(dev)) != 0)
+
+	i = pci_save_vc_state(dev);
+	if (i != 0)
 		return i;
+
 	return 0;
 }
 EXPORT_SYMBOL(pci_save_state);
@@ -1907,10 +1914,6 @@ int pci_prepare_to_sleep(struct pci_dev *dev)
 	if (target_state == PCI_POWER_ERROR)
 		return -EIO;
 
-	/* D3cold during system suspend/hibernate is not supported */
-	if (target_state > PCI_D3hot)
-		target_state = PCI_D3hot;
-
 	pci_enable_wake(dev, target_state, device_may_wakeup(&dev->dev));
 
 	error = pci_set_power_state(dev, target_state);
@@ -2704,6 +2707,37 @@ int pci_request_regions_exclusive(struct pci_dev *pdev, const char *res_name)
 }
 EXPORT_SYMBOL(pci_request_regions_exclusive);
 
+/**
+ *	pci_remap_iospace - Remap the memory mapped I/O space
+ *	@res: Resource describing the I/O space
+ *	@phys_addr: physical address of range to be mapped
+ *
+ *	Remap the memory mapped I/O space described by the @res
+ *	and the CPU physical address @phys_addr into virtual address space.
+ *	Only architectures that have memory mapped IO functions defined
+ *	(and the PCI_IOBASE value defined) should call this function.
+ */
+int __weak pci_remap_iospace(const struct resource *res, phys_addr_t phys_addr)
+{
+#if defined(PCI_IOBASE) && defined(CONFIG_MMU)
+	unsigned long vaddr = (unsigned long)PCI_IOBASE + res->start;
+
+	if (!(res->flags & IORESOURCE_IO))
+		return -EINVAL;
+
+	if (res->end > IO_SPACE_LIMIT)
+		return -EINVAL;
+
+	return ioremap_page_range(vaddr, vaddr + resource_size(res), phys_addr,
+				  pgprot_device(PAGE_KERNEL));
+#else
+	/* this architecture does not have memory mapped I/O space,
+	   so this function should never be called */
+	WARN_ONCE(1, "This architecture does not support memory mapped I/O\n");
+	return -ENODEV;
+#endif
+}
+
 static void __pci_set_master(struct pci_dev *dev, bool enable)
 {
 	u16 old_cmd, cmd;
@@ -4406,6 +4440,15 @@ static void pci_no_domains(void)
 #endif
 }
 
+#ifdef CONFIG_PCI_DOMAINS
+static atomic_t __domain_nr = ATOMIC_INIT(-1);
+
+int pci_get_new_domain_nr(void)
+{
+	return atomic_inc_return(&__domain_nr);
+}
+#endif
+
 /**
  * pci_ext_cfg_avail - can we access extended PCI config space?
  *
diff --git a/drivers/pci/pcie/aer/aerdrv_errprint.c b/drivers/pci/pcie/aer/aerdrv_errprint.c
index 35d06e177917..c6849d9e86ce 100644
--- a/drivers/pci/pcie/aer/aerdrv_errprint.c
+++ b/drivers/pci/pcie/aer/aerdrv_errprint.c
@@ -89,15 +89,17 @@ static const char *aer_correctable_error_string[] = {
 	NULL,
 	"Replay Timer Timeout",		/* Bit Position 12	*/
 	"Advisory Non-Fatal",		/* Bit Position 13	*/
+	"Corrected Internal Error",	/* Bit Position 14	*/
+	"Header Log Overflow",		/* Bit Position 15	*/
 };
 
 static const char *aer_uncorrectable_error_string[] = {
-	NULL,
+	"Undefined",			/* Bit Position 0	*/
 	NULL,
 	NULL,
 	NULL,
 	"Data Link Protocol",		/* Bit Position 4	*/
-	NULL,
+	"Surprise Down Error",		/* Bit Position 5	*/
 	NULL,
 	NULL,
 	NULL,
@@ -113,6 +115,11 @@ static const char *aer_uncorrectable_error_string[] = {
 	"Malformed TLP",		/* Bit Position 18	*/
 	"ECRC",				/* Bit Position 19	*/
 	"Unsupported Request",		/* Bit Position 20	*/
+	"ACS Violation",		/* Bit Position 21	*/
+	"Uncorrectable Internal Error",	/* Bit Position 22	*/
+	"MC Blocked TLP",		/* Bit Position 23	*/
+	"AtomicOp Egress Blocked",	/* Bit Position 24	*/
+	"TLP Prefix Blocked Error",	/* Bit Position 25	*/
 };
 
 static const char *aer_agent_string[] = {
diff --git a/drivers/pci/pcie/pme.c b/drivers/pci/pcie/pme.c
index 82e06a86cd77..63fc63911295 100644
--- a/drivers/pci/pcie/pme.c
+++ b/drivers/pci/pcie/pme.c
@@ -41,11 +41,17 @@ static int __init pcie_pme_setup(char *str)
 }
 __setup("pcie_pme=", pcie_pme_setup);
 
+enum pme_suspend_level {
+	PME_SUSPEND_NONE = 0,
+	PME_SUSPEND_WAKEUP,
+	PME_SUSPEND_NOIRQ,
+};
+
 struct pcie_pme_service_data {
 	spinlock_t lock;
 	struct pcie_device *srv;
 	struct work_struct work;
-	bool noirq; /* Don't enable the PME interrupt used by this service. */
+	enum pme_suspend_level suspend_level;
 };
 
 /**
@@ -223,7 +229,7 @@ static void pcie_pme_work_fn(struct work_struct *work)
 	spin_lock_irq(&data->lock);
 
 	for (;;) {
-		if (data->noirq)
+		if (data->suspend_level != PME_SUSPEND_NONE)
 			break;
 
 		pcie_capability_read_dword(port, PCI_EXP_RTSTA, &rtsta);
@@ -250,7 +256,7 @@ static void pcie_pme_work_fn(struct work_struct *work)
 		spin_lock_irq(&data->lock);
 	}
 
-	if (!data->noirq)
+	if (data->suspend_level == PME_SUSPEND_NONE)
 		pcie_pme_interrupt_enable(port, true);
 
 	spin_unlock_irq(&data->lock);
@@ -367,6 +373,21 @@ static int pcie_pme_probe(struct pcie_device *srv)
 	return ret;
 }
 
+static bool pcie_pme_check_wakeup(struct pci_bus *bus)
+{
+	struct pci_dev *dev;
+
+	if (!bus)
+		return false;
+
+	list_for_each_entry(dev, &bus->devices, bus_list)
+		if (device_may_wakeup(&dev->dev)
+		    || pcie_pme_check_wakeup(dev->subordinate))
+			return true;
+
+	return false;
+}
+
 /**
  * pcie_pme_suspend - Suspend PCIe PME service device.
  * @srv: PCIe service device to suspend.
@@ -375,11 +396,28 @@ static int pcie_pme_suspend(struct pcie_device *srv)
 {
 	struct pcie_pme_service_data *data = get_service_data(srv);
 	struct pci_dev *port = srv->port;
+	bool wakeup;
+	int ret;
 
+	if (device_may_wakeup(&port->dev)) {
+		wakeup = true;
+	} else {
+		down_read(&pci_bus_sem);
+		wakeup = pcie_pme_check_wakeup(port->subordinate);
+		up_read(&pci_bus_sem);
+	}
 	spin_lock_irq(&data->lock);
-	pcie_pme_interrupt_enable(port, false);
-	pcie_clear_root_pme_status(port);
-	data->noirq = true;
+	if (wakeup) {
+		ret = enable_irq_wake(srv->irq);
+		data->suspend_level = PME_SUSPEND_WAKEUP;
+	}
+	if (!wakeup || ret) {
+		struct pci_dev *port = srv->port;
+
+		pcie_pme_interrupt_enable(port, false);
+		pcie_clear_root_pme_status(port);
+		data->suspend_level = PME_SUSPEND_NOIRQ;
+	}
 	spin_unlock_irq(&data->lock);
 
 	synchronize_irq(srv->irq);
@@ -394,12 +432,17 @@ static int pcie_pme_suspend(struct pcie_device *srv)
 static int pcie_pme_resume(struct pcie_device *srv)
 {
 	struct pcie_pme_service_data *data = get_service_data(srv);
-	struct pci_dev *port = srv->port;
 
 	spin_lock_irq(&data->lock);
-	data->noirq = false;
-	pcie_clear_root_pme_status(port);
-	pcie_pme_interrupt_enable(port, true);
+	if (data->suspend_level == PME_SUSPEND_NOIRQ) {
+		struct pci_dev *port = srv->port;
+
+		pcie_clear_root_pme_status(port);
+		pcie_pme_interrupt_enable(port, true);
+	} else {
+		disable_irq_wake(srv->irq);
+	}
+	data->suspend_level = PME_SUSPEND_NONE;
 	spin_unlock_irq(&data->lock);
 
 	return 0;
diff --git a/drivers/pci/pcie/portdrv_pci.c b/drivers/pci/pcie/portdrv_pci.c
index 2ccc9b926ea7..be35da2e105e 100644
--- a/drivers/pci/pcie/portdrv_pci.c
+++ b/drivers/pci/pcie/portdrv_pci.c
@@ -93,77 +93,6 @@ static int pcie_port_resume_noirq(struct device *dev)
 	return 0;
 }
 
-#ifdef CONFIG_PM_RUNTIME
-struct d3cold_info {
-	bool no_d3cold;
-	unsigned int d3cold_delay;
-};
-
-static int pci_dev_d3cold_info(struct pci_dev *pdev, void *data)
-{
-	struct d3cold_info *info = data;
-
-	info->d3cold_delay = max_t(unsigned int, pdev->d3cold_delay,
-				   info->d3cold_delay);
-	if (pdev->no_d3cold)
-		info->no_d3cold = true;
-	return 0;
-}
-
-static int pcie_port_runtime_suspend(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct d3cold_info d3cold_info = {
-		.no_d3cold	= false,
-		.d3cold_delay	= PCI_PM_D3_WAIT,
-	};
-
-	/*
-	 * If any subordinate device disable D3cold, we should not put
-	 * the port into D3cold.  The D3cold delay of port should be
-	 * the max of that of all subordinate devices.
-	 */
-	pci_walk_bus(pdev->subordinate, pci_dev_d3cold_info, &d3cold_info);
-	pdev->no_d3cold = d3cold_info.no_d3cold;
-	pdev->d3cold_delay = d3cold_info.d3cold_delay;
-	return 0;
-}
-
-static int pcie_port_runtime_resume(struct device *dev)
-{
-	return 0;
-}
-
-static int pci_dev_pme_poll(struct pci_dev *pdev, void *data)
-{
-	bool *pme_poll = data;
-
-	if (pdev->pme_poll)
-		*pme_poll = true;
-	return 0;
-}
-
-static int pcie_port_runtime_idle(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	bool pme_poll = false;
-
-	/*
-	 * If any subordinate device needs pme poll, we should keep
-	 * the port in D0, because we need port in D0 to poll it.
-	 */
-	pci_walk_bus(pdev->subordinate, pci_dev_pme_poll, &pme_poll);
-	/* Delay for a short while to prevent too frequent suspend/resume */
-	if (!pme_poll)
-		pm_schedule_suspend(dev, 10);
-	return -EBUSY;
-}
-#else
-#define pcie_port_runtime_suspend	NULL
-#define pcie_port_runtime_resume	NULL
-#define pcie_port_runtime_idle		NULL
-#endif
-
 static const struct dev_pm_ops pcie_portdrv_pm_ops = {
 	.suspend	= pcie_port_device_suspend,
 	.resume		= pcie_port_device_resume,
@@ -172,9 +101,6 @@ static const struct dev_pm_ops pcie_portdrv_pm_ops = {
 	.poweroff	= pcie_port_device_suspend,
 	.restore	= pcie_port_device_resume,
 	.resume_noirq	= pcie_port_resume_noirq,
-	.runtime_suspend = pcie_port_runtime_suspend,
-	.runtime_resume = pcie_port_runtime_resume,
-	.runtime_idle	= pcie_port_runtime_idle,
 };
 
 #define PCIE_PORTDRV_PM_OPS	(&pcie_portdrv_pm_ops)
diff --git a/drivers/pci/probe.c b/drivers/pci/probe.c
index e3cf8a2e6292..5ed99309c758 100644
--- a/drivers/pci/probe.c
+++ b/drivers/pci/probe.c
@@ -6,6 +6,7 @@
 #include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/pci.h>
+#include <linux/pci_hotplug.h>
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/cpumask.h>
@@ -485,7 +486,7 @@ void pci_read_bridge_bases(struct pci_bus *child)
 	}
 }
 
-static struct pci_bus *pci_alloc_bus(void)
+static struct pci_bus *pci_alloc_bus(struct pci_bus *parent)
 {
 	struct pci_bus *b;
 
@@ -500,6 +501,10 @@ static struct pci_bus *pci_alloc_bus(void)
 	INIT_LIST_HEAD(&b->resources);
 	b->max_bus_speed = PCI_SPEED_UNKNOWN;
 	b->cur_bus_speed = PCI_SPEED_UNKNOWN;
+#ifdef CONFIG_PCI_DOMAINS_GENERIC
+	if (parent)
+		b->domain_nr = parent->domain_nr;
+#endif
 	return b;
 }
 
@@ -671,7 +676,7 @@ static struct pci_bus *pci_alloc_child_bus(struct pci_bus *parent,
 	/*
 	 * Allocate a new bus, and inherit stuff from the parent..
 	 */
-	child = pci_alloc_bus();
+	child = pci_alloc_bus(parent);
 	if (!child)
 		return NULL;
 
@@ -740,6 +745,17 @@ struct pci_bus *pci_add_new_bus(struct pci_bus *parent, struct pci_dev *dev,
 }
 EXPORT_SYMBOL(pci_add_new_bus);
 
+static void pci_enable_crs(struct pci_dev *pdev)
+{
+	u16 root_cap = 0;
+
+	/* Enable CRS Software Visibility if supported */
+	pcie_capability_read_word(pdev, PCI_EXP_RTCAP, &root_cap);
+	if (root_cap & PCI_EXP_RTCAP_CRSVIS)
+		pcie_capability_set_word(pdev, PCI_EXP_RTCTL,
+					 PCI_EXP_RTCTL_CRSSVE);
+}
+
 /*
  * If it's a bridge, configure it and scan the bus behind it.
  * For CardBus bridges, we don't scan behind as the devices will
@@ -775,7 +791,7 @@ int pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max, int pass)
 	/* Check if setup is sensible at all */
 	if (!pass &&
 	    (primary != bus->number || secondary <= bus->number ||
-	     secondary > subordinate || subordinate > bus->busn_res.end)) {
+	     secondary > subordinate)) {
 		dev_info(&dev->dev, "bridge configuration invalid ([bus %02x-%02x]), reconfiguring\n",
 			 secondary, subordinate);
 		broken = 1;
@@ -787,6 +803,8 @@ int pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max, int pass)
 	pci_write_config_word(dev, PCI_BRIDGE_CONTROL,
 			      bctl & ~PCI_BRIDGE_CTL_MASTER_ABORT);
 
+	pci_enable_crs(dev);
+
 	if ((secondary || subordinate) && !pcibios_assign_all_busses() &&
 	    !is_cardbus && !broken) {
 		unsigned int cmax;
@@ -838,23 +856,18 @@ int pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max, int pass)
 			goto out;
 		}
 
-		if (max >= bus->busn_res.end) {
-			dev_warn(&dev->dev, "can't allocate child bus %02x from %pR\n",
-				 max, &bus->busn_res);
-			goto out;
-		}
-
 		/* Clear errors */
 		pci_write_config_word(dev, PCI_STATUS, 0xffff);
 
-		/* The bus will already exist if we are rescanning */
+		/* Prevent assigning a bus number that already exists.
+		 * This can happen when a bridge is hot-plugged, so in
+		 * this case we only re-scan this bus. */
 		child = pci_find_bus(pci_domain_nr(bus), max+1);
 		if (!child) {
 			child = pci_add_new_bus(bus, dev, max+1);
 			if (!child)
 				goto out;
-			pci_bus_insert_busn_res(child, max+1,
-						bus->busn_res.end);
+			pci_bus_insert_busn_res(child, max+1, 0xff);
 		}
 		max++;
 		buses = (buses & 0xff000000)
@@ -913,11 +926,6 @@ int pci_scan_bridge(struct pci_bus *bus, struct pci_dev *dev, int max, int pass)
 		/*
 		 * Set the subordinate bus number to its real value.
 		 */
-		if (max > bus->busn_res.end) {
-			dev_warn(&dev->dev, "max busn %02x is outside %pR\n",
-				 max, &bus->busn_res);
-			max = bus->busn_res.end;
-		}
 		pci_bus_update_busn_res_end(child, max);
 		pci_write_config_byte(dev, PCI_SUBORDINATE_BUS, max);
 	}
@@ -1236,6 +1244,137 @@ int pci_setup_device(struct pci_dev *dev)
 	return 0;
 }
 
+static struct hpp_type0 pci_default_type0 = {
+	.revision = 1,
+	.cache_line_size = 8,
+	.latency_timer = 0x40,
+	.enable_serr = 0,
+	.enable_perr = 0,
+};
+
+static void program_hpp_type0(struct pci_dev *dev, struct hpp_type0 *hpp)
+{
+	u16 pci_cmd, pci_bctl;
+
+	if (!hpp)
+		hpp = &pci_default_type0;
+
+	if (hpp->revision > 1) {
+		dev_warn(&dev->dev,
+			 "PCI settings rev %d not supported; using defaults\n",
+			 hpp->revision);
+		hpp = &pci_default_type0;
+	}
+
+	pci_write_config_byte(dev, PCI_CACHE_LINE_SIZE, hpp->cache_line_size);
+	pci_write_config_byte(dev, PCI_LATENCY_TIMER, hpp->latency_timer);
+	pci_read_config_word(dev, PCI_COMMAND, &pci_cmd);
+	if (hpp->enable_serr)
+		pci_cmd |= PCI_COMMAND_SERR;
+	if (hpp->enable_perr)
+		pci_cmd |= PCI_COMMAND_PARITY;
+	pci_write_config_word(dev, PCI_COMMAND, pci_cmd);
+
+	/* Program bridge control value */
+	if ((dev->class >> 8) == PCI_CLASS_BRIDGE_PCI) {
+		pci_write_config_byte(dev, PCI_SEC_LATENCY_TIMER,
+				      hpp->latency_timer);
+		pci_read_config_word(dev, PCI_BRIDGE_CONTROL, &pci_bctl);
+		if (hpp->enable_serr)
+			pci_bctl |= PCI_BRIDGE_CTL_SERR;
+		if (hpp->enable_perr)
+			pci_bctl |= PCI_BRIDGE_CTL_PARITY;
+		pci_write_config_word(dev, PCI_BRIDGE_CONTROL, pci_bctl);
+	}
+}
+
+static void program_hpp_type1(struct pci_dev *dev, struct hpp_type1 *hpp)
+{
+	if (hpp)
+		dev_warn(&dev->dev, "PCI-X settings not supported\n");
+}
+
+static void program_hpp_type2(struct pci_dev *dev, struct hpp_type2 *hpp)
+{
+	int pos;
+	u32 reg32;
+
+	if (!hpp)
+		return;
+
+	if (hpp->revision > 1) {
+		dev_warn(&dev->dev, "PCIe settings rev %d not supported\n",
+			 hpp->revision);
+		return;
+	}
+
+	/*
+	 * Don't allow _HPX to change MPS or MRRS settings.  We manage
+	 * those to make sure they're consistent with the rest of the
+	 * platform.
+	 */
+	hpp->pci_exp_devctl_and |= PCI_EXP_DEVCTL_PAYLOAD |
+				    PCI_EXP_DEVCTL_READRQ;
+	hpp->pci_exp_devctl_or &= ~(PCI_EXP_DEVCTL_PAYLOAD |
+				    PCI_EXP_DEVCTL_READRQ);
+
+	/* Initialize Device Control Register */
+	pcie_capability_clear_and_set_word(dev, PCI_EXP_DEVCTL,
+			~hpp->pci_exp_devctl_and, hpp->pci_exp_devctl_or);
+
+	/* Initialize Link Control Register */
+	if (dev->subordinate)
+		pcie_capability_clear_and_set_word(dev, PCI_EXP_LNKCTL,
+			~hpp->pci_exp_lnkctl_and, hpp->pci_exp_lnkctl_or);
+
+	/* Find Advanced Error Reporting Enhanced Capability */
+	pos = pci_find_ext_capability(dev, PCI_EXT_CAP_ID_ERR);
+	if (!pos)
+		return;
+
+	/* Initialize Uncorrectable Error Mask Register */
+	pci_read_config_dword(dev, pos + PCI_ERR_UNCOR_MASK, &reg32);
+	reg32 = (reg32 & hpp->unc_err_mask_and) | hpp->unc_err_mask_or;
+	pci_write_config_dword(dev, pos + PCI_ERR_UNCOR_MASK, reg32);
+
+	/* Initialize Uncorrectable Error Severity Register */
+	pci_read_config_dword(dev, pos + PCI_ERR_UNCOR_SEVER, &reg32);
+	reg32 = (reg32 & hpp->unc_err_sever_and) | hpp->unc_err_sever_or;
+	pci_write_config_dword(dev, pos + PCI_ERR_UNCOR_SEVER, reg32);
+
+	/* Initialize Correctable Error Mask Register */
+	pci_read_config_dword(dev, pos + PCI_ERR_COR_MASK, &reg32);
+	reg32 = (reg32 & hpp->cor_err_mask_and) | hpp->cor_err_mask_or;
+	pci_write_config_dword(dev, pos + PCI_ERR_COR_MASK, reg32);
+
+	/* Initialize Advanced Error Capabilities and Control Register */
+	pci_read_config_dword(dev, pos + PCI_ERR_CAP, &reg32);
+	reg32 = (reg32 & hpp->adv_err_cap_and) | hpp->adv_err_cap_or;
+	pci_write_config_dword(dev, pos + PCI_ERR_CAP, reg32);
+
+	/*
+	 * FIXME: The following two registers are not supported yet.
+	 *
+	 *   o Secondary Uncorrectable Error Severity Register
+	 *   o Secondary Uncorrectable Error Mask Register
+	 */
+}
+
+static void pci_configure_device(struct pci_dev *dev)
+{
+	struct hotplug_params hpp;
+	int ret;
+
+	memset(&hpp, 0, sizeof(hpp));
+	ret = pci_get_hp_params(dev, &hpp);
+	if (ret)
+		return;
+
+	program_hpp_type2(dev, hpp.t2);
+	program_hpp_type1(dev, hpp.t1);
+	program_hpp_type0(dev, hpp.t0);
+}
+
 static void pci_release_capabilities(struct pci_dev *dev)
 {
 	pci_vpd_release(dev);
@@ -1292,8 +1431,13 @@ bool pci_bus_read_dev_vendor_id(struct pci_bus *bus, int devfn, u32 *l,
 	    *l == 0x0000ffff || *l == 0xffff0000)
 		return false;
 
-	/* Configuration request Retry Status */
-	while (*l == 0xffff0001) {
+	/*
+	 * Configuration Request Retry Status.  Some root ports return the
+	 * actual device ID instead of the synthetic ID (0xFFFF) required
+	 * by the PCIe spec.  Ignore the device ID and only check for
+	 * (vendor id == 1).
+	 */
+	while ((*l & 0xffff) == 0x0001) {
 		if (!crs_timeout)
 			return false;
 
@@ -1373,6 +1517,8 @@ void pci_device_add(struct pci_dev *dev, struct pci_bus *bus)
 {
 	int ret;
 
+	pci_configure_device(dev);
+
 	device_initialize(&dev->dev);
 	dev->dev.release = pci_release_dev;
 
@@ -1761,13 +1907,14 @@ struct pci_bus *pci_create_root_bus(struct device *parent, int bus,
 	char bus_addr[64];
 	char *fmt;
 
-	b = pci_alloc_bus();
+	b = pci_alloc_bus(NULL);
 	if (!b)
 		return NULL;
 
 	b->sysdata = sysdata;
 	b->ops = ops;
 	b->number = b->busn_res.start = bus;
+	pci_bus_assign_domain_nr(b, parent);
 	b2 = pci_find_bus(pci_domain_nr(b), bus);
 	if (b2) {
 		/* If we already got to this bus through a different bridge, ignore it */
diff --git a/drivers/pci/quirks.c b/drivers/pci/quirks.c
index 80c2d014283d..90acb32c85b1 100644
--- a/drivers/pci/quirks.c
+++ b/drivers/pci/quirks.c
@@ -24,6 +24,7 @@
 #include <linux/ioport.h>
 #include <linux/sched.h>
 #include <linux/ktime.h>
+#include <linux/mm.h>
 #include <asm/dma.h>	/* isa_dma_bridge_buggy */
 #include "pci.h"
 
@@ -287,6 +288,25 @@ static void quirk_citrine(struct pci_dev *dev)
 }
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_IBM,	PCI_DEVICE_ID_IBM_CITRINE,	quirk_citrine);
 
+/*  On IBM Crocodile ipr SAS adapters, expand BAR to system page size */
+static void quirk_extend_bar_to_page(struct pci_dev *dev)
+{
+	int i;
+
+	for (i = 0; i < PCI_STD_RESOURCE_END; i++) {
+		struct resource *r = &dev->resource[i];
+
+		if (r->flags & IORESOURCE_MEM && resource_size(r) < PAGE_SIZE) {
+			r->end = PAGE_SIZE - 1;
+			r->start = 0;
+			r->flags |= IORESOURCE_UNSET;
+			dev_info(&dev->dev, "expanded BAR %d to page size: %pR\n",
+				 i, r);
+		}
+	}
+}
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_IBM, 0x034a, quirk_extend_bar_to_page);
+
 /*
  *  S3 868 and 968 chips report region size equal to 32M, but they decode 64M.
  *  If it's needed, re-allocate the region.
@@ -2985,6 +3005,8 @@ DECLARE_PCI_FIXUP_HEADER(0x1814, 0x0601, /* Ralink RT2800 802.11n PCI */
  */
 DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_REALTEK, 0x8169,
 			 quirk_broken_intx_masking);
+DECLARE_PCI_FIXUP_HEADER(PCI_VENDOR_ID_MELLANOX, PCI_ANY_ID,
+			 quirk_broken_intx_masking);
 
 #ifdef CONFIG_ACPI
 /*
@@ -3512,57 +3534,6 @@ DECLARE_PCI_FIXUP_HEADER(0x1283, 0x8892, quirk_use_pcie_bridge_dma_alias);
 /* Intel 82801, https://bugzilla.kernel.org/show_bug.cgi?id=44881#c49 */
 DECLARE_PCI_FIXUP_HEADER(0x8086, 0x244e, quirk_use_pcie_bridge_dma_alias);
 
-static struct pci_dev *pci_func_0_dma_source(struct pci_dev *dev)
-{
-	if (!PCI_FUNC(dev->devfn))
-		return pci_dev_get(dev);
-
-	return pci_get_slot(dev->bus, PCI_DEVFN(PCI_SLOT(dev->devfn), 0));
-}
-
-static const struct pci_dev_dma_source {
-	u16 vendor;
-	u16 device;
-	struct pci_dev *(*dma_source)(struct pci_dev *dev);
-} pci_dev_dma_source[] = {
-	/*
-	 * https://bugzilla.redhat.com/show_bug.cgi?id=605888
-	 *
-	 * Some Ricoh devices use the function 0 source ID for DMA on
-	 * other functions of a multifunction device.  The DMA devices
-	 * is therefore function 0, which will have implications of the
-	 * iommu grouping of these devices.
-	 */
-	{ PCI_VENDOR_ID_RICOH, 0xe822, pci_func_0_dma_source },
-	{ PCI_VENDOR_ID_RICOH, 0xe230, pci_func_0_dma_source },
-	{ PCI_VENDOR_ID_RICOH, 0xe832, pci_func_0_dma_source },
-	{ PCI_VENDOR_ID_RICOH, 0xe476, pci_func_0_dma_source },
-	{ 0 }
-};
-
-/*
- * IOMMUs with isolation capabilities need to be programmed with the
- * correct source ID of a device.  In most cases, the source ID matches
- * the device doing the DMA, but sometimes hardware is broken and will
- * tag the DMA as being sourced from a different device.  This function
- * allows that translation.  Note that the reference count of the
- * returned device is incremented on all paths.
- */
-struct pci_dev *pci_get_dma_source(struct pci_dev *dev)
-{
-	const struct pci_dev_dma_source *i;
-
-	for (i = pci_dev_dma_source; i->dma_source; i++) {
-		if ((i->vendor == dev->vendor ||
-		     i->vendor == (u16)PCI_ANY_ID) &&
-		    (i->device == dev->device ||
-		     i->device == (u16)PCI_ANY_ID))
-			return i->dma_source(dev);
-	}
-
-	return pci_dev_get(dev);
-}
-
 /*
  * AMD has indicated that the devices below do not support peer-to-peer
  * in any system where they are found in the southbridge with an AMD
@@ -3582,6 +3553,11 @@ struct pci_dev *pci_get_dma_source(struct pci_dev *dev)
  * 1002:439d SB7x0/SB8x0/SB9x0 LPC host controller
  * 1002:4384 SBx00 PCI to PCI Bridge
  * 1002:4399 SB7x0/SB8x0/SB9x0 USB OHCI2 Controller
+ *
+ * https://bugzilla.kernel.org/show_bug.cgi?id=81841#c15
+ *
+ * 1022:780f [AMD] FCH PCI Bridge
+ * 1022:7809 [AMD] FCH USB OHCI Controller
  */
 static int pci_quirk_amd_sb_acs(struct pci_dev *dev, u16 acs_flags)
 {
@@ -3664,6 +3640,23 @@ static int pci_quirk_intel_pch_acs(struct pci_dev *dev, u16 acs_flags)
 	return acs_flags & ~flags ? 0 : 1;
 }
 
+static int pci_quirk_mf_endpoint_acs(struct pci_dev *dev, u16 acs_flags)
+{
+	/*
+	 * SV, TB, and UF are not relevant to multifunction endpoints.
+	 *
+	 * Multifunction devices are only required to implement RR, CR, and DT
+	 * in their ACS capability if they support peer-to-peer transactions.
+	 * Devices matching this quirk have been verified by the vendor to not
+	 * perform peer-to-peer with other functions, allowing us to mask out
+	 * these bits as if they were unimplemented in the ACS capability.
+	 */
+	acs_flags &= ~(PCI_ACS_SV | PCI_ACS_TB | PCI_ACS_RR |
+		       PCI_ACS_CR | PCI_ACS_UF | PCI_ACS_DT);
+
+	return acs_flags ? 0 : 1;
+}
+
 static const struct pci_dev_acs_enabled {
 	u16 vendor;
 	u16 device;
@@ -3675,6 +3668,30 @@ static const struct pci_dev_acs_enabled {
 	{ PCI_VENDOR_ID_ATI, 0x439d, pci_quirk_amd_sb_acs },
 	{ PCI_VENDOR_ID_ATI, 0x4384, pci_quirk_amd_sb_acs },
 	{ PCI_VENDOR_ID_ATI, 0x4399, pci_quirk_amd_sb_acs },
+	{ PCI_VENDOR_ID_AMD, 0x780f, pci_quirk_amd_sb_acs },
+	{ PCI_VENDOR_ID_AMD, 0x7809, pci_quirk_amd_sb_acs },
+	{ PCI_VENDOR_ID_SOLARFLARE, 0x0903, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_SOLARFLARE, 0x0923, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10C6, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10DB, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10DD, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10E1, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10F1, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10F7, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10F8, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10F9, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10FA, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10FB, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x10FC, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1507, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1514, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x151C, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1529, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x152A, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x154D, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x154F, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1551, pci_quirk_mf_endpoint_acs },
+	{ PCI_VENDOR_ID_INTEL, 0x1558, pci_quirk_mf_endpoint_acs },
 	{ PCI_VENDOR_ID_INTEL, PCI_ANY_ID, pci_quirk_intel_pch_acs },
 	{ 0 }
 };
diff --git a/drivers/pci/search.c b/drivers/pci/search.c
index 827ad831f1dd..a81f413083e4 100644
--- a/drivers/pci/search.c
+++ b/drivers/pci/search.c
@@ -103,40 +103,6 @@ int pci_for_each_dma_alias(struct pci_dev *pdev,
 	return ret;
 }
 
-/*
- * find the upstream PCIe-to-PCI bridge of a PCI device
- * if the device is PCIE, return NULL
- * if the device isn't connected to a PCIe bridge (that is its parent is a
- * legacy PCI bridge and the bridge is directly connected to bus 0), return its
- * parent
- */
-struct pci_dev *pci_find_upstream_pcie_bridge(struct pci_dev *pdev)
-{
-	struct pci_dev *tmp = NULL;
-
-	if (pci_is_pcie(pdev))
-		return NULL;
-	while (1) {
-		if (pci_is_root_bus(pdev->bus))
-			break;
-		pdev = pdev->bus->self;
-		/* a p2p bridge */
-		if (!pci_is_pcie(pdev)) {
-			tmp = pdev;
-			continue;
-		}
-		/* PCI device should connect to a PCIe bridge */
-		if (pci_pcie_type(pdev) != PCI_EXP_TYPE_PCI_BRIDGE) {
-			/* Busted hardware? */
-			WARN_ON_ONCE(1);
-			return NULL;
-		}
-		return pdev;
-	}
-
-	return tmp;
-}
-
 static struct pci_bus *pci_do_find_bus(struct pci_bus *bus, unsigned char busnr)
 {
 	struct pci_bus *child;
diff --git a/drivers/pci/setup-bus.c b/drivers/pci/setup-bus.c
index 6373985ad3f7..0482235eee92 100644
--- a/drivers/pci/setup-bus.c
+++ b/drivers/pci/setup-bus.c
@@ -1652,7 +1652,7 @@ void pci_assign_unassigned_bridge_resources(struct pci_dev *bridge)
 	struct pci_dev_resource *fail_res;
 	int retval;
 	unsigned long type_mask = IORESOURCE_IO | IORESOURCE_MEM |
-				  IORESOURCE_PREFETCH;
+				  IORESOURCE_PREFETCH | IORESOURCE_MEM_64;
 
 again:
 	__pci_bus_size_bridges(parent, &add_list);
diff --git a/drivers/pci/xen-pcifront.c b/drivers/pci/xen-pcifront.c
index 53df39a22c8a..116ca3746adb 100644
--- a/drivers/pci/xen-pcifront.c
+++ b/drivers/pci/xen-pcifront.c
@@ -1136,11 +1136,13 @@ static const struct xenbus_device_id xenpci_ids[] = {
 	{""},
 };
 
-static DEFINE_XENBUS_DRIVER(xenpci, "pcifront",
+static struct xenbus_driver xenpci_driver = {
+	.name			= "pcifront",
+	.ids			= xenpci_ids,
 	.probe			= pcifront_xenbus_probe,
 	.remove			= pcifront_xenbus_remove,
 	.otherend_changed	= pcifront_backend_changed,
-);
+};
 
 static int __init pcifront_init(void)
 {
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index 0dd742719154..2a436e607f99 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -41,13 +41,20 @@ config PHY_MVEBU_SATA
 config PHY_MIPHY365X
 	tristate "STMicroelectronics MIPHY365X PHY driver for STiH41x series"
 	depends on ARCH_STI
-	depends on GENERIC_PHY
 	depends on HAS_IOMEM
 	depends on OF
+	select GENERIC_PHY
 	help
 	  Enable this to support the miphy transceiver (for SATA/PCIE)
 	  that is part of STMicroelectronics STiH41x SoC series.
 
+config PHY_RCAR_GEN2
+	tristate "Renesas R-Car generation 2 USB PHY driver"
+	depends on ARCH_SHMOBILE
+	depends on GENERIC_PHY
+	help
+	  Support for USB PHY found on Renesas R-Car generation 2 SoCs.
+
 config OMAP_CONTROL_PHY
 	tristate "OMAP CONTROL PHY Driver"
 	depends on ARCH_OMAP2PLUS || COMPILE_TEST
@@ -214,12 +221,14 @@ config PHY_QCOM_IPQ806X_SATA
 config PHY_ST_SPEAR1310_MIPHY
 	tristate "ST SPEAR1310-MIPHY driver"
 	select GENERIC_PHY
+	depends on MACH_SPEAR1310 || COMPILE_TEST
 	help
 	  Support for ST SPEAr1310 MIPHY which can be used for PCIe and SATA.
 
 config PHY_ST_SPEAR1340_MIPHY
 	tristate "ST SPEAR1340-MIPHY driver"
 	select GENERIC_PHY
+	depends on MACH_SPEAR1340 || COMPILE_TEST
 	help
 	  Support for ST SPEAr1340 MIPHY which can be used for PCIe and SATA.
 
@@ -230,4 +239,21 @@ config PHY_XGENE
 	help
 	  This option enables support for APM X-Gene SoC multi-purpose PHY.
 
+config PHY_STIH407_USB
+	tristate "STMicroelectronics USB2 picoPHY driver for STiH407 family"
+	depends on RESET_CONTROLLER
+	depends on ARCH_STI || COMPILE_TEST
+	select GENERIC_PHY
+	help
+	  Enable this support to enable the picoPHY device used by USB2
+	  and USB3 controllers on STMicroelectronics STiH407 SoC families.
+
+config PHY_STIH41X_USB
+	tristate "STMicroelectronics USB2 PHY driver for STiH41x series"
+	depends on ARCH_STI
+	select GENERIC_PHY
+	help
+	  Enable this to support the USB transceiver that is part of
+	  STMicroelectronics STiH41x SoC series.
+
 endmenu
diff --git a/drivers/phy/Makefile b/drivers/phy/Makefile
index 95c69ed5ed45..c4590fce082f 100644
--- a/drivers/phy/Makefile
+++ b/drivers/phy/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_PHY_EXYNOS_DP_VIDEO)	+= phy-exynos-dp-video.o
 obj-$(CONFIG_PHY_EXYNOS_MIPI_VIDEO)	+= phy-exynos-mipi-video.o
 obj-$(CONFIG_PHY_MVEBU_SATA)		+= phy-mvebu-sata.o
 obj-$(CONFIG_PHY_MIPHY365X)		+= phy-miphy365x.o
+obj-$(CONFIG_PHY_RCAR_GEN2)		+= phy-rcar-gen2.o
 obj-$(CONFIG_OMAP_CONTROL_PHY)		+= phy-omap-control.o
 obj-$(CONFIG_OMAP_USB2)			+= phy-omap-usb2.o
 obj-$(CONFIG_TI_PIPE3)			+= phy-ti-pipe3.o
@@ -28,3 +29,5 @@ obj-$(CONFIG_PHY_QCOM_IPQ806X_SATA)	+= phy-qcom-ipq806x-sata.o
 obj-$(CONFIG_PHY_ST_SPEAR1310_MIPHY)	+= phy-spear1310-miphy.o
 obj-$(CONFIG_PHY_ST_SPEAR1340_MIPHY)	+= phy-spear1340-miphy.o
 obj-$(CONFIG_PHY_XGENE)			+= phy-xgene.o
+obj-$(CONFIG_PHY_STIH407_USB)		+= phy-stih407-usb.o
+obj-$(CONFIG_PHY_STIH41X_USB)		+= phy-stih41x-usb.o
diff --git a/drivers/phy/phy-bcm-kona-usb2.c b/drivers/phy/phy-bcm-kona-usb2.c
index 894fe74c1e44..c1e0ca335c0e 100644
--- a/drivers/phy/phy-bcm-kona-usb2.c
+++ b/drivers/phy/phy-bcm-kona-usb2.c
@@ -143,7 +143,6 @@ static struct platform_driver bcm_kona_usb2_driver = {
 	.probe		= bcm_kona_usb2_probe,
 	.driver		= {
 		.name	= "bcm-kona-usb2",
-		.owner	= THIS_MODULE,
 		.of_match_table = bcm_kona_usb2_dt_ids,
 	},
 };
diff --git a/drivers/phy/phy-berlin-sata.c b/drivers/phy/phy-berlin-sata.c
index 5c3a0424aeb4..69ced52d72aa 100644
--- a/drivers/phy/phy-berlin-sata.c
+++ b/drivers/phy/phy-berlin-sata.c
@@ -273,7 +273,6 @@ static struct platform_driver phy_berlin_sata_driver = {
 	.probe	= phy_berlin_sata_probe,
 	.driver	= {
 		.name		= "phy-berlin-sata",
-		.owner		= THIS_MODULE,
 		.of_match_table	= phy_berlin_sata_of_match,
 	},
 };
diff --git a/drivers/phy/phy-exynos-dp-video.c b/drivers/phy/phy-exynos-dp-video.c
index 8b3026e2af7f..84f49e5a3f24 100644
--- a/drivers/phy/phy-exynos-dp-video.c
+++ b/drivers/phy/phy-exynos-dp-video.c
@@ -13,44 +13,55 @@
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/mfd/syscon.h>
+#include <linux/mfd/syscon/exynos5-pmu.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
+#include <linux/regmap.h>
 
-/* DPTX_PHY_CONTROL register */
-#define EXYNOS_DPTX_PHY_ENABLE		(1 << 0)
+struct exynos_dp_video_phy_drvdata {
+	u32 phy_ctrl_offset;
+};
 
 struct exynos_dp_video_phy {
-	void __iomem *regs;
+	struct regmap *regs;
+	const struct exynos_dp_video_phy_drvdata *drvdata;
 };
 
-static int __set_phy_state(struct exynos_dp_video_phy *state, unsigned int on)
+static void exynos_dp_video_phy_pwr_isol(struct exynos_dp_video_phy *state,
+							unsigned int on)
 {
-	u32 reg;
+	unsigned int val;
+
+	if (IS_ERR(state->regs))
+		return;
 
-	reg = readl(state->regs);
-	if (on)
-		reg |= EXYNOS_DPTX_PHY_ENABLE;
-	else
-		reg &= ~EXYNOS_DPTX_PHY_ENABLE;
-	writel(reg, state->regs);
+	val = on ? 0 : EXYNOS5_PHY_ENABLE;
 
-	return 0;
+	regmap_update_bits(state->regs, state->drvdata->phy_ctrl_offset,
+			   EXYNOS5_PHY_ENABLE, val);
 }
 
 static int exynos_dp_video_phy_power_on(struct phy *phy)
 {
 	struct exynos_dp_video_phy *state = phy_get_drvdata(phy);
 
-	return __set_phy_state(state, 1);
+	/* Disable power isolation on DP-PHY */
+	exynos_dp_video_phy_pwr_isol(state, 0);
+
+	return 0;
 }
 
 static int exynos_dp_video_phy_power_off(struct phy *phy)
 {
 	struct exynos_dp_video_phy *state = phy_get_drvdata(phy);
 
-	return __set_phy_state(state, 0);
+	/* Enable power isolation on DP-PHY */
+	exynos_dp_video_phy_pwr_isol(state, 1);
+
+	return 0;
 }
 
 static struct phy_ops exynos_dp_video_phy_ops = {
@@ -59,11 +70,31 @@ static struct phy_ops exynos_dp_video_phy_ops = {
 	.owner		= THIS_MODULE,
 };
 
+static const struct exynos_dp_video_phy_drvdata exynos5250_dp_video_phy = {
+	.phy_ctrl_offset	= EXYNOS5_DPTX_PHY_CONTROL,
+};
+
+static const struct exynos_dp_video_phy_drvdata exynos5420_dp_video_phy = {
+	.phy_ctrl_offset	= EXYNOS5420_DPTX_PHY_CONTROL,
+};
+
+static const struct of_device_id exynos_dp_video_phy_of_match[] = {
+	{
+		.compatible = "samsung,exynos5250-dp-video-phy",
+		.data = &exynos5250_dp_video_phy,
+	}, {
+		.compatible = "samsung,exynos5420-dp-video-phy",
+		.data = &exynos5420_dp_video_phy,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, exynos_dp_video_phy_of_match);
+
 static int exynos_dp_video_phy_probe(struct platform_device *pdev)
 {
 	struct exynos_dp_video_phy *state;
 	struct device *dev = &pdev->dev;
-	struct resource *res;
+	const struct of_device_id *match;
 	struct phy_provider *phy_provider;
 	struct phy *phy;
 
@@ -71,11 +102,15 @@ static int exynos_dp_video_phy_probe(struct platform_device *pdev)
 	if (!state)
 		return -ENOMEM;
 
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	state->regs = devm_ioremap_resource(dev, res);
-	if (IS_ERR(state->regs))
+	state->regs = syscon_regmap_lookup_by_phandle(dev->of_node,
+						      "samsung,pmu-syscon");
+	if (IS_ERR(state->regs)) {
+		dev_err(dev, "Failed to lookup PMU regmap\n");
 		return PTR_ERR(state->regs);
+	}
+
+	match = of_match_node(exynos_dp_video_phy_of_match, dev->of_node);
+	state->drvdata = match->data;
 
 	phy = devm_phy_create(dev, NULL, &exynos_dp_video_phy_ops, NULL);
 	if (IS_ERR(phy)) {
@@ -89,17 +124,10 @@ static int exynos_dp_video_phy_probe(struct platform_device *pdev)
 	return PTR_ERR_OR_ZERO(phy_provider);
 }
 
-static const struct of_device_id exynos_dp_video_phy_of_match[] = {
-	{ .compatible = "samsung,exynos5250-dp-video-phy" },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, exynos_dp_video_phy_of_match);
-
 static struct platform_driver exynos_dp_video_phy_driver = {
 	.probe	= exynos_dp_video_phy_probe,
 	.driver = {
 		.name	= "exynos-dp-video-phy",
-		.owner	= THIS_MODULE,
 		.of_match_table	= exynos_dp_video_phy_of_match,
 	}
 };
diff --git a/drivers/phy/phy-exynos-mipi-video.c b/drivers/phy/phy-exynos-mipi-video.c
index b55a92e12496..6a9bef138617 100644
--- a/drivers/phy/phy-exynos-mipi-video.c
+++ b/drivers/phy/phy-exynos-mipi-video.c
@@ -165,7 +165,6 @@ static struct platform_driver exynos_mipi_video_phy_driver = {
 	.driver = {
 		.of_match_table	= exynos_mipi_video_phy_of_match,
 		.name  = "exynos-mipi-video-phy",
-		.owner = THIS_MODULE,
 	}
 };
 module_platform_driver(exynos_mipi_video_phy_driver);
diff --git a/drivers/phy/phy-exynos5-usbdrd.c b/drivers/phy/phy-exynos5-usbdrd.c
index b05302b09c9f..f756aca871db 100644
--- a/drivers/phy/phy-exynos5-usbdrd.c
+++ b/drivers/phy/phy-exynos5-usbdrd.c
@@ -542,6 +542,7 @@ static const struct of_device_id exynos5_usbdrd_phy_of_match[] = {
 	},
 	{ },
 };
+MODULE_DEVICE_TABLE(of, exynos5_usbdrd_phy_of_match);
 
 static int exynos5_usbdrd_phy_probe(struct platform_device *pdev)
 {
@@ -666,7 +667,6 @@ static struct platform_driver exynos5_usb3drd_phy = {
 	.driver = {
 		.of_match_table	= exynos5_usbdrd_phy_of_match,
 		.name		= "exynos5_usb3drd_phy",
-		.owner		= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/phy/phy-exynos5250-sata.c b/drivers/phy/phy-exynos5250-sata.c
index 19a679aca4ac..54cf4ae60d29 100644
--- a/drivers/phy/phy-exynos5250-sata.c
+++ b/drivers/phy/phy-exynos5250-sata.c
@@ -240,7 +240,6 @@ static struct platform_driver exynos_sata_phy_driver = {
 	.driver = {
 		.of_match_table	= exynos_sata_phy_of_match,
 		.name  = "samsung,sata-phy",
-		.owner = THIS_MODULE,
 	}
 };
 module_platform_driver(exynos_sata_phy_driver);
diff --git a/drivers/phy/phy-hix5hd2-sata.c b/drivers/phy/phy-hix5hd2-sata.c
index 6a08fa5f81eb..d5d978085c6d 100644
--- a/drivers/phy/phy-hix5hd2-sata.c
+++ b/drivers/phy/phy-hix5hd2-sata.c
@@ -180,7 +180,6 @@ static struct platform_driver hix5hd2_sata_phy_driver = {
 	.probe	= hix5hd2_sata_phy_probe,
 	.driver = {
 		.name	= "hix5hd2-sata-phy",
-		.owner	= THIS_MODULE,
 		.of_match_table	= hix5hd2_sata_phy_of_match,
 	}
 };
diff --git a/drivers/phy/phy-miphy365x.c b/drivers/phy/phy-miphy365x.c
index e111baf187ce..801afaf2d449 100644
--- a/drivers/phy/phy-miphy365x.c
+++ b/drivers/phy/phy-miphy365x.c
@@ -163,6 +163,7 @@ enum miphy_sata_gen {
 };
 
 static u8 rx_tx_spd[] = {
+	0, /* GEN0 doesn't exist. */
 	TX_SPDSEL_GEN1_VAL | RX_SPDSEL_GEN1_VAL,
 	TX_SPDSEL_GEN2_VAL | RX_SPDSEL_GEN2_VAL,
 	TX_SPDSEL_GEN3_VAL | RX_SPDSEL_GEN3_VAL
@@ -625,7 +626,6 @@ static struct platform_driver miphy365x_driver = {
 	.probe	= miphy365x_probe,
 	.driver = {
 		.name	= "miphy365x-phy",
-		.owner	= THIS_MODULE,
 		.of_match_table	= miphy365x_of_match,
 	}
 };
diff --git a/drivers/phy/phy-mvebu-sata.c b/drivers/phy/phy-mvebu-sata.c
index cc3c0e166daf..d395558cb12e 100644
--- a/drivers/phy/phy-mvebu-sata.c
+++ b/drivers/phy/phy-mvebu-sata.c
@@ -89,6 +89,8 @@ static int phy_mvebu_sata_probe(struct platform_device *pdev)
 	struct phy *phy;
 
 	priv = devm_kzalloc(&pdev->dev, sizeof(*priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	priv->base = devm_ioremap_resource(&pdev->dev, res);
@@ -126,7 +128,6 @@ static struct platform_driver phy_mvebu_sata_driver = {
 	.probe	= phy_mvebu_sata_probe,
 	.driver = {
 		.name	= "phy-mvebu-sata",
-		.owner	= THIS_MODULE,
 		.of_match_table	= phy_mvebu_sata_of_match,
 	}
 };
diff --git a/drivers/phy/phy-omap-control.c b/drivers/phy/phy-omap-control.c
index 9487bf112267..c96e8183a8ff 100644
--- a/drivers/phy/phy-omap-control.c
+++ b/drivers/phy/phy-omap-control.c
@@ -295,10 +295,8 @@ static int omap_control_phy_probe(struct platform_device *pdev)
 
 	control_phy = devm_kzalloc(&pdev->dev, sizeof(*control_phy),
 		GFP_KERNEL);
-	if (!control_phy) {
-		dev_err(&pdev->dev, "unable to alloc memory for control phy\n");
+	if (!control_phy)
 		return -ENOMEM;
-	}
 
 	control_phy->dev = &pdev->dev;
 	control_phy->type = *(enum omap_control_phy_type *)of_id->data;
@@ -347,7 +345,6 @@ static struct platform_driver omap_control_phy_driver = {
 	.probe		= omap_control_phy_probe,
 	.driver		= {
 		.name	= "omap-control-phy",
-		.owner	= THIS_MODULE,
 		.of_match_table = of_match_ptr(omap_control_phy_id_table),
 	},
 };
diff --git a/drivers/phy/phy-omap-usb2.c b/drivers/phy/phy-omap-usb2.c
index 93d78359246c..8c842980834a 100644
--- a/drivers/phy/phy-omap-usb2.c
+++ b/drivers/phy/phy-omap-usb2.c
@@ -212,16 +212,12 @@ static int omap_usb2_probe(struct platform_device *pdev)
 	phy_data = (struct usb_phy_data *)of_id->data;
 
 	phy = devm_kzalloc(&pdev->dev, sizeof(*phy), GFP_KERNEL);
-	if (!phy) {
-		dev_err(&pdev->dev, "unable to allocate memory for USB2 PHY\n");
+	if (!phy)
 		return -ENOMEM;
-	}
 
 	otg = devm_kzalloc(&pdev->dev, sizeof(*otg), GFP_KERNEL);
-	if (!otg) {
-		dev_err(&pdev->dev, "unable to allocate memory for USB OTG\n");
+	if (!otg)
 		return -ENOMEM;
-	}
 
 	phy->dev		= &pdev->dev;
 
@@ -382,7 +378,6 @@ static struct platform_driver omap_usb2_driver = {
 	.remove		= omap_usb2_remove,
 	.driver		= {
 		.name	= "omap-usb2",
-		.owner	= THIS_MODULE,
 		.pm	= DEV_PM_OPS,
 		.of_match_table = of_match_ptr(omap_usb2_id_table),
 	},
diff --git a/drivers/phy/phy-qcom-apq8064-sata.c b/drivers/phy/phy-qcom-apq8064-sata.c
index b3ef7d805765..7b3ddfb65898 100644
--- a/drivers/phy/phy-qcom-apq8064-sata.c
+++ b/drivers/phy/phy-qcom-apq8064-sata.c
@@ -279,7 +279,6 @@ static struct platform_driver qcom_apq8064_sata_phy_driver = {
 	.remove	= qcom_apq8064_sata_phy_remove,
 	.driver = {
 		.name	= "qcom-apq8064-sata-phy",
-		.owner	= THIS_MODULE,
 		.of_match_table	= qcom_apq8064_sata_phy_of_match,
 	}
 };
diff --git a/drivers/phy/phy-qcom-ipq806x-sata.c b/drivers/phy/phy-qcom-ipq806x-sata.c
index 909b5a87fc6a..759b0bf5b6b3 100644
--- a/drivers/phy/phy-qcom-ipq806x-sata.c
+++ b/drivers/phy/phy-qcom-ipq806x-sata.c
@@ -201,7 +201,6 @@ static struct platform_driver qcom_ipq806x_sata_phy_driver = {
 	.remove	= qcom_ipq806x_sata_phy_remove,
 	.driver = {
 		.name	= "qcom-ipq806x-sata-phy",
-		.owner	= THIS_MODULE,
 		.of_match_table	= qcom_ipq806x_sata_phy_of_match,
 	}
 };
diff --git a/drivers/phy/phy-rcar-gen2.c b/drivers/phy/phy-rcar-gen2.c
new file mode 100644
index 000000000000..2793af17799f
--- /dev/null
+++ b/drivers/phy/phy-rcar-gen2.c
@@ -0,0 +1,341 @@
+/*
+ * Renesas R-Car Gen2 PHY driver
+ *
+ * Copyright (C) 2014 Renesas Solutions Corp.
+ * Copyright (C) 2014 Cogent Embedded, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/spinlock.h>
+
+#include <asm/cmpxchg.h>
+
+#define USBHS_LPSTS			0x02
+#define USBHS_UGCTRL			0x80
+#define USBHS_UGCTRL2			0x84
+#define USBHS_UGSTS			0x88	/* The manuals have 0x90 */
+
+/* Low Power Status register (LPSTS) */
+#define USBHS_LPSTS_SUSPM		0x4000
+
+/* USB General control register (UGCTRL) */
+#define USBHS_UGCTRL_CONNECT		0x00000004
+#define USBHS_UGCTRL_PLLRESET		0x00000001
+
+/* USB General control register 2 (UGCTRL2) */
+#define USBHS_UGCTRL2_USB2SEL		0x80000000
+#define USBHS_UGCTRL2_USB2SEL_PCI	0x00000000
+#define USBHS_UGCTRL2_USB2SEL_USB30	0x80000000
+#define USBHS_UGCTRL2_USB0SEL		0x00000030
+#define USBHS_UGCTRL2_USB0SEL_PCI	0x00000010
+#define USBHS_UGCTRL2_USB0SEL_HS_USB	0x00000030
+
+/* USB General status register (UGSTS) */
+#define USBHS_UGSTS_LOCK		0x00000300 /* The manuals have 0x3 */
+
+#define PHYS_PER_CHANNEL	2
+
+struct rcar_gen2_phy {
+	struct phy *phy;
+	struct rcar_gen2_channel *channel;
+	int number;
+	u32 select_value;
+};
+
+struct rcar_gen2_channel {
+	struct device_node *of_node;
+	struct rcar_gen2_phy_driver *drv;
+	struct rcar_gen2_phy phys[PHYS_PER_CHANNEL];
+	int selected_phy;
+	u32 select_mask;
+};
+
+struct rcar_gen2_phy_driver {
+	void __iomem *base;
+	struct clk *clk;
+	spinlock_t lock;
+	int num_channels;
+	struct rcar_gen2_channel *channels;
+};
+
+static int rcar_gen2_phy_init(struct phy *p)
+{
+	struct rcar_gen2_phy *phy = phy_get_drvdata(p);
+	struct rcar_gen2_channel *channel = phy->channel;
+	struct rcar_gen2_phy_driver *drv = channel->drv;
+	unsigned long flags;
+	u32 ugctrl2;
+
+	/*
+	 * Try to acquire exclusive access to PHY.  The first driver calling
+	 * phy_init()  on a given channel wins, and all attempts  to use another
+	 * PHY on this channel will fail until phy_exit() is called by the first
+	 * driver.   Achieving this with cmpxcgh() should be SMP-safe.
+	 */
+	if (cmpxchg(&channel->selected_phy, -1, phy->number) != -1)
+		return -EBUSY;
+
+	clk_prepare_enable(drv->clk);
+
+	spin_lock_irqsave(&drv->lock, flags);
+	ugctrl2 = readl(drv->base + USBHS_UGCTRL2);
+	ugctrl2 &= ~channel->select_mask;
+	ugctrl2 |= phy->select_value;
+	writel(ugctrl2, drv->base + USBHS_UGCTRL2);
+	spin_unlock_irqrestore(&drv->lock, flags);
+	return 0;
+}
+
+static int rcar_gen2_phy_exit(struct phy *p)
+{
+	struct rcar_gen2_phy *phy = phy_get_drvdata(p);
+	struct rcar_gen2_channel *channel = phy->channel;
+
+	clk_disable_unprepare(channel->drv->clk);
+
+	channel->selected_phy = -1;
+
+	return 0;
+}
+
+static int rcar_gen2_phy_power_on(struct phy *p)
+{
+	struct rcar_gen2_phy *phy = phy_get_drvdata(p);
+	struct rcar_gen2_phy_driver *drv = phy->channel->drv;
+	void __iomem *base = drv->base;
+	unsigned long flags;
+	u32 value;
+	int err = 0, i;
+
+	/* Skip if it's not USBHS */
+	if (phy->select_value != USBHS_UGCTRL2_USB0SEL_HS_USB)
+		return 0;
+
+	spin_lock_irqsave(&drv->lock, flags);
+
+	/* Power on USBHS PHY */
+	value = readl(base + USBHS_UGCTRL);
+	value &= ~USBHS_UGCTRL_PLLRESET;
+	writel(value, base + USBHS_UGCTRL);
+
+	value = readw(base + USBHS_LPSTS);
+	value |= USBHS_LPSTS_SUSPM;
+	writew(value, base + USBHS_LPSTS);
+
+	for (i = 0; i < 20; i++) {
+		value = readl(base + USBHS_UGSTS);
+		if ((value & USBHS_UGSTS_LOCK) == USBHS_UGSTS_LOCK) {
+			value = readl(base + USBHS_UGCTRL);
+			value |= USBHS_UGCTRL_CONNECT;
+			writel(value, base + USBHS_UGCTRL);
+			goto out;
+		}
+		udelay(1);
+	}
+
+	/* Timed out waiting for the PLL lock */
+	err = -ETIMEDOUT;
+
+out:
+	spin_unlock_irqrestore(&drv->lock, flags);
+
+	return err;
+}
+
+static int rcar_gen2_phy_power_off(struct phy *p)
+{
+	struct rcar_gen2_phy *phy = phy_get_drvdata(p);
+	struct rcar_gen2_phy_driver *drv = phy->channel->drv;
+	void __iomem *base = drv->base;
+	unsigned long flags;
+	u32 value;
+
+	/* Skip if it's not USBHS */
+	if (phy->select_value != USBHS_UGCTRL2_USB0SEL_HS_USB)
+		return 0;
+
+	spin_lock_irqsave(&drv->lock, flags);
+
+	/* Power off USBHS PHY */
+	value = readl(base + USBHS_UGCTRL);
+	value &= ~USBHS_UGCTRL_CONNECT;
+	writel(value, base + USBHS_UGCTRL);
+
+	value = readw(base + USBHS_LPSTS);
+	value &= ~USBHS_LPSTS_SUSPM;
+	writew(value, base + USBHS_LPSTS);
+
+	value = readl(base + USBHS_UGCTRL);
+	value |= USBHS_UGCTRL_PLLRESET;
+	writel(value, base + USBHS_UGCTRL);
+
+	spin_unlock_irqrestore(&drv->lock, flags);
+
+	return 0;
+}
+
+static struct phy_ops rcar_gen2_phy_ops = {
+	.init		= rcar_gen2_phy_init,
+	.exit		= rcar_gen2_phy_exit,
+	.power_on	= rcar_gen2_phy_power_on,
+	.power_off	= rcar_gen2_phy_power_off,
+	.owner		= THIS_MODULE,
+};
+
+static const struct of_device_id rcar_gen2_phy_match_table[] = {
+	{ .compatible = "renesas,usb-phy-r8a7790" },
+	{ .compatible = "renesas,usb-phy-r8a7791" },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rcar_gen2_phy_match_table);
+
+static struct phy *rcar_gen2_phy_xlate(struct device *dev,
+				       struct of_phandle_args *args)
+{
+	struct rcar_gen2_phy_driver *drv;
+	struct device_node *np = args->np;
+	int i;
+
+	if (!of_device_is_available(np)) {
+		dev_warn(dev, "Requested PHY is disabled\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	drv = dev_get_drvdata(dev);
+	if (!drv)
+		return ERR_PTR(-EINVAL);
+
+	for (i = 0; i < drv->num_channels; i++) {
+		if (np == drv->channels[i].of_node)
+			break;
+	}
+
+	if (i >= drv->num_channels || args->args[0] >= 2)
+		return ERR_PTR(-ENODEV);
+
+	return drv->channels[i].phys[args->args[0]].phy;
+}
+
+static const u32 select_mask[] = {
+	[0]	= USBHS_UGCTRL2_USB0SEL,
+	[2]	= USBHS_UGCTRL2_USB2SEL,
+};
+
+static const u32 select_value[][PHYS_PER_CHANNEL] = {
+	[0]	= { USBHS_UGCTRL2_USB0SEL_PCI, USBHS_UGCTRL2_USB0SEL_HS_USB },
+	[2]	= { USBHS_UGCTRL2_USB2SEL_PCI, USBHS_UGCTRL2_USB2SEL_USB30 },
+};
+
+static int rcar_gen2_phy_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct rcar_gen2_phy_driver *drv;
+	struct phy_provider *provider;
+	struct device_node *np;
+	struct resource *res;
+	void __iomem *base;
+	struct clk *clk;
+	int i = 0;
+
+	if (!dev->of_node) {
+		dev_err(dev,
+			"This driver is required to be instantiated from device tree\n");
+		return -EINVAL;
+	}
+
+	clk = devm_clk_get(dev, "usbhs");
+	if (IS_ERR(clk)) {
+		dev_err(dev, "Can't get USBHS clock\n");
+		return PTR_ERR(clk);
+	}
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	drv = devm_kzalloc(dev, sizeof(*drv), GFP_KERNEL);
+	if (!drv)
+		return -ENOMEM;
+
+	spin_lock_init(&drv->lock);
+
+	drv->clk = clk;
+	drv->base = base;
+
+	drv->num_channels = of_get_child_count(dev->of_node);
+	drv->channels = devm_kcalloc(dev, drv->num_channels,
+				     sizeof(struct rcar_gen2_channel),
+				     GFP_KERNEL);
+	if (!drv->channels)
+		return -ENOMEM;
+
+	for_each_child_of_node(dev->of_node, np) {
+		struct rcar_gen2_channel *channel = drv->channels + i;
+		u32 channel_num;
+		int error, n;
+
+		channel->of_node = np;
+		channel->drv = drv;
+		channel->selected_phy = -1;
+
+		error = of_property_read_u32(np, "reg", &channel_num);
+		if (error || channel_num > 2) {
+			dev_err(dev, "Invalid \"reg\" property\n");
+			return error;
+		}
+		channel->select_mask = select_mask[channel_num];
+
+		for (n = 0; n < PHYS_PER_CHANNEL; n++) {
+			struct rcar_gen2_phy *phy = &channel->phys[n];
+
+			phy->channel = channel;
+			phy->number = n;
+			phy->select_value = select_value[channel_num][n];
+
+			phy->phy = devm_phy_create(dev, NULL,
+						   &rcar_gen2_phy_ops, NULL);
+			if (IS_ERR(phy->phy)) {
+				dev_err(dev, "Failed to create PHY\n");
+				return PTR_ERR(phy->phy);
+			}
+			phy_set_drvdata(phy->phy, phy);
+		}
+
+		i++;
+	}
+
+	provider = devm_of_phy_provider_register(dev, rcar_gen2_phy_xlate);
+	if (IS_ERR(provider)) {
+		dev_err(dev, "Failed to register PHY provider\n");
+		return PTR_ERR(provider);
+	}
+
+	dev_set_drvdata(dev, drv);
+
+	return 0;
+}
+
+static struct platform_driver rcar_gen2_phy_driver = {
+	.driver = {
+		.name		= "phy_rcar_gen2",
+		.of_match_table	= rcar_gen2_phy_match_table,
+	},
+	.probe	= rcar_gen2_phy_probe,
+};
+
+module_platform_driver(rcar_gen2_phy_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Renesas R-Car Gen2 PHY");
+MODULE_AUTHOR("Sergei Shtylyov <sergei.shtylyov@cogentembedded.com>");
diff --git a/drivers/phy/phy-samsung-usb2.c b/drivers/phy/phy-samsung-usb2.c
index 3732ca25e09f..908949dfb4db 100644
--- a/drivers/phy/phy-samsung-usb2.c
+++ b/drivers/phy/phy-samsung-usb2.c
@@ -231,7 +231,6 @@ static struct platform_driver samsung_usb2_phy_driver = {
 	.driver = {
 		.of_match_table	= samsung_usb2_phy_of_match,
 		.name		= "samsung-usb2-phy",
-		.owner		= THIS_MODULE,
 	}
 };
 
diff --git a/drivers/phy/phy-spear1310-miphy.c b/drivers/phy/phy-spear1310-miphy.c
index 6dcbfcddb372..5f4c586ee951 100644
--- a/drivers/phy/phy-spear1310-miphy.c
+++ b/drivers/phy/phy-spear1310-miphy.c
@@ -212,10 +212,8 @@ static int spear1310_miphy_probe(struct platform_device *pdev)
 	struct phy_provider *phy_provider;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(dev, "can't alloc spear1310_miphy private date memory\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
 	priv->misc =
 		syscon_regmap_lookup_by_phandle(dev->of_node, "misc");
@@ -252,22 +250,11 @@ static struct platform_driver spear1310_miphy_driver = {
 	.probe		= spear1310_miphy_probe,
 	.driver = {
 		.name = "spear1310-miphy",
-		.owner = THIS_MODULE,
 		.of_match_table = of_match_ptr(spear1310_miphy_of_match),
 	},
 };
 
-static int __init spear1310_miphy_phy_init(void)
-{
-	return platform_driver_register(&spear1310_miphy_driver);
-}
-module_init(spear1310_miphy_phy_init);
-
-static void __exit spear1310_miphy_phy_exit(void)
-{
-	platform_driver_unregister(&spear1310_miphy_driver);
-}
-module_exit(spear1310_miphy_phy_exit);
+module_platform_driver(spear1310_miphy_driver);
 
 MODULE_DESCRIPTION("ST SPEAR1310-MIPHY driver");
 MODULE_AUTHOR("Pratyush Anand <pratyush.anand@st.com>");
diff --git a/drivers/phy/phy-spear1340-miphy.c b/drivers/phy/phy-spear1340-miphy.c
index 7135ba2603b6..1ecd0945bad3 100644
--- a/drivers/phy/phy-spear1340-miphy.c
+++ b/drivers/phy/phy-spear1340-miphy.c
@@ -249,10 +249,8 @@ static int spear1340_miphy_probe(struct platform_device *pdev)
 	struct phy_provider *phy_provider;
 
 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
-	if (!priv) {
-		dev_err(dev, "can't alloc spear1340_miphy private date memory\n");
+	if (!priv)
 		return -ENOMEM;
-	}
 
 	priv->misc =
 		syscon_regmap_lookup_by_phandle(dev->of_node, "misc");
@@ -284,23 +282,12 @@ static struct platform_driver spear1340_miphy_driver = {
 	.probe		= spear1340_miphy_probe,
 	.driver = {
 		.name = "spear1340-miphy",
-		.owner = THIS_MODULE,
 		.pm = &spear1340_miphy_pm_ops,
 		.of_match_table = of_match_ptr(spear1340_miphy_of_match),
 	},
 };
 
-static int __init spear1340_miphy_phy_init(void)
-{
-	return platform_driver_register(&spear1340_miphy_driver);
-}
-module_init(spear1340_miphy_phy_init);
-
-static void __exit spear1340_miphy_phy_exit(void)
-{
-	platform_driver_unregister(&spear1340_miphy_driver);
-}
-module_exit(spear1340_miphy_phy_exit);
+module_platform_driver(spear1340_miphy_driver);
 
 MODULE_DESCRIPTION("ST SPEAR1340-MIPHY driver");
 MODULE_AUTHOR("Pratyush Anand <pratyush.anand@st.com>");
diff --git a/drivers/phy/phy-stih407-usb.c b/drivers/phy/phy-stih407-usb.c
new file mode 100644
index 000000000000..42428d4181ea
--- /dev/null
+++ b/drivers/phy/phy-stih407-usb.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics
+ *
+ * STMicroelectronics Generic PHY driver for STiH407 USB2.
+ *
+ * Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/clk.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+#include <linux/mfd/syscon.h>
+#include <linux/phy/phy.h>
+
+/* Default PHY_SEL and REFCLKSEL configuration */
+#define STIH407_USB_PICOPHY_CTRL_PORT_CONF	0x6
+#define STIH407_USB_PICOPHY_CTRL_PORT_MASK	0x1f
+
+/* ports parameters overriding */
+#define STIH407_USB_PICOPHY_PARAM_DEF		0x39a4dc
+#define STIH407_USB_PICOPHY_PARAM_MASK		0xffffffff
+
+struct stih407_usb2_picophy {
+	struct phy *phy;
+	struct regmap *regmap;
+	struct device *dev;
+	struct reset_control *rstc;
+	struct reset_control *rstport;
+	int ctrl;
+	int param;
+};
+
+static int stih407_usb2_pico_ctrl(struct stih407_usb2_picophy *phy_dev)
+{
+	reset_control_deassert(phy_dev->rstc);
+
+	return regmap_update_bits(phy_dev->regmap, phy_dev->ctrl,
+				  STIH407_USB_PICOPHY_CTRL_PORT_MASK,
+				  STIH407_USB_PICOPHY_CTRL_PORT_CONF);
+}
+
+static int stih407_usb2_init_port(struct phy *phy)
+{
+	int ret;
+	struct stih407_usb2_picophy *phy_dev = phy_get_drvdata(phy);
+
+	stih407_usb2_pico_ctrl(phy_dev);
+
+	ret = regmap_update_bits(phy_dev->regmap,
+				 phy_dev->param,
+				 STIH407_USB_PICOPHY_PARAM_MASK,
+				 STIH407_USB_PICOPHY_PARAM_DEF);
+	if (ret)
+		return ret;
+
+	return reset_control_deassert(phy_dev->rstport);
+}
+
+static int stih407_usb2_exit_port(struct phy *phy)
+{
+	struct stih407_usb2_picophy *phy_dev = phy_get_drvdata(phy);
+
+	/*
+	 * Only port reset is asserted, phy global reset is kept untouched
+	 * as other ports may still be active. When all ports are in reset
+	 * state, assumption is made that power will be cut off on the phy, in
+	 * case of suspend for instance. Theoretically, asserting individual
+	 * reset (like here) or global reset should be equivalent.
+	 */
+	return reset_control_assert(phy_dev->rstport);
+}
+
+static const struct phy_ops stih407_usb2_picophy_data = {
+	.init = stih407_usb2_init_port,
+	.exit = stih407_usb2_exit_port,
+	.owner = THIS_MODULE,
+};
+
+static int stih407_usb2_picophy_probe(struct platform_device *pdev)
+{
+	struct stih407_usb2_picophy *phy_dev;
+	struct device *dev = &pdev->dev;
+	struct device_node *np = dev->of_node;
+	struct phy_provider *phy_provider;
+	struct phy *phy;
+	struct resource *res;
+
+	phy_dev = devm_kzalloc(dev, sizeof(*phy_dev), GFP_KERNEL);
+	if (!phy_dev)
+		return -ENOMEM;
+
+	phy_dev->dev = dev;
+	dev_set_drvdata(dev, phy_dev);
+
+	phy_dev->rstc = devm_reset_control_get(dev, "global");
+	if (IS_ERR(phy_dev->rstc)) {
+		dev_err(dev, "failed to ctrl picoPHY reset\n");
+		return PTR_ERR(phy_dev->rstc);
+	}
+
+	phy_dev->rstport = devm_reset_control_get(dev, "port");
+	if (IS_ERR(phy_dev->rstport)) {
+		dev_err(dev, "failed to ctrl picoPHY reset\n");
+		return PTR_ERR(phy_dev->rstport);
+	}
+
+	/* Reset port by default: only deassert it in phy init */
+	reset_control_assert(phy_dev->rstport);
+
+	phy_dev->regmap = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
+	if (IS_ERR(phy_dev->regmap)) {
+		dev_err(dev, "No syscfg phandle specified\n");
+		return PTR_ERR(phy_dev->regmap);
+	}
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "ctrl");
+	if (!res) {
+		dev_err(dev, "No ctrl reg found\n");
+		return -ENXIO;
+	}
+	phy_dev->ctrl = res->start;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "param");
+	if (!res) {
+		dev_err(dev, "No param reg found\n");
+		return -ENXIO;
+	}
+	phy_dev->param = res->start;
+
+	phy = devm_phy_create(dev, NULL, &stih407_usb2_picophy_data, NULL);
+	if (IS_ERR(phy)) {
+		dev_err(dev, "failed to create Display Port PHY\n");
+		return PTR_ERR(phy);
+	}
+
+	phy_dev->phy = phy;
+	phy_set_drvdata(phy, phy_dev);
+
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(phy_provider))
+		return PTR_ERR(phy_provider);
+
+	dev_info(dev, "STiH407 USB Generic picoPHY driver probed!");
+
+	return 0;
+}
+
+static const struct of_device_id stih407_usb2_picophy_of_match[] = {
+	{ .compatible = "st,stih407-usb2-phy" },
+	{ /*sentinel */ },
+};
+
+MODULE_DEVICE_TABLE(of, stih407_usb2_picophy_of_match);
+
+static struct platform_driver stih407_usb2_picophy_driver = {
+	.probe = stih407_usb2_picophy_probe,
+	.driver = {
+		   .name = "stih407-usb-genphy",
+		   .of_match_table = stih407_usb2_picophy_of_match,
+		   }
+};
+
+module_platform_driver(stih407_usb2_picophy_driver);
+
+MODULE_AUTHOR("Giuseppe Cavallaro <peppe.cavallaro@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics Generic picoPHY driver for STiH407");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-stih41x-usb.c b/drivers/phy/phy-stih41x-usb.c
new file mode 100644
index 000000000000..9f16cb8e01f4
--- /dev/null
+++ b/drivers/phy/phy-stih41x-usb.c
@@ -0,0 +1,187 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics
+ *
+ * STMicroelectronics PHY driver for STiH41x USB.
+ *
+ * Author: Maxime Coquelin <maxime.coquelin@st.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/clk.h>
+#include <linux/phy/phy.h>
+#include <linux/regmap.h>
+#include <linux/mfd/syscon.h>
+
+#define SYSCFG332  0x80
+#define SYSCFG2520 0x820
+
+/**
+ * struct stih41x_usb_cfg - SoC specific PHY register mapping
+ * @syscfg: Offset in syscfg registers bank
+ * @cfg_mask: Bits mask for PHY configuration
+ * @cfg: Static configuration value for PHY
+ * @oscok: Notify the PHY oscillator clock is ready
+ *	   Setting this bit enable the PHY
+ */
+struct stih41x_usb_cfg {
+	u32 syscfg;
+	u32 cfg_mask;
+	u32 cfg;
+	u32 oscok;
+};
+
+/**
+ * struct stih41x_usb_phy - Private data for the PHY
+ * @dev: device for this controller
+ * @regmap: Syscfg registers bank in which PHY is configured
+ * @cfg: SoC specific PHY register mapping
+ * @clk: Oscillator used by the PHY
+ */
+struct stih41x_usb_phy {
+	struct device *dev;
+	struct regmap *regmap;
+	const struct stih41x_usb_cfg *cfg;
+	struct clk *clk;
+};
+
+static struct stih41x_usb_cfg stih415_usb_phy_cfg = {
+	.syscfg = SYSCFG332,
+	.cfg_mask = 0x3f,
+	.cfg = 0x38,
+	.oscok = BIT(6),
+};
+
+static struct stih41x_usb_cfg stih416_usb_phy_cfg = {
+	.syscfg = SYSCFG2520,
+	.cfg_mask = 0x33f,
+	.cfg = 0x238,
+	.oscok = BIT(6),
+};
+
+static int stih41x_usb_phy_init(struct phy *phy)
+{
+	struct stih41x_usb_phy *phy_dev = phy_get_drvdata(phy);
+
+	return regmap_update_bits(phy_dev->regmap, phy_dev->cfg->syscfg,
+			   phy_dev->cfg->cfg_mask, phy_dev->cfg->cfg);
+}
+
+static int stih41x_usb_phy_power_on(struct phy *phy)
+{
+	struct stih41x_usb_phy *phy_dev = phy_get_drvdata(phy);
+	int ret;
+
+	ret = clk_prepare_enable(phy_dev->clk);
+	if (ret) {
+		dev_err(phy_dev->dev, "Failed to enable osc_phy clock\n");
+		return ret;
+	}
+
+	return regmap_update_bits(phy_dev->regmap, phy_dev->cfg->syscfg,
+			phy_dev->cfg->oscok, phy_dev->cfg->oscok);
+}
+
+static int stih41x_usb_phy_power_off(struct phy *phy)
+{
+	struct stih41x_usb_phy *phy_dev = phy_get_drvdata(phy);
+	int ret;
+
+	ret = regmap_update_bits(phy_dev->regmap, phy_dev->cfg->syscfg,
+			phy_dev->cfg->oscok, 0);
+	if (ret) {
+		dev_err(phy_dev->dev, "Failed to clear oscok bit\n");
+		return ret;
+	}
+
+	clk_disable_unprepare(phy_dev->clk);
+
+	return 0;
+}
+
+static struct phy_ops stih41x_usb_phy_ops = {
+	.init		= stih41x_usb_phy_init,
+	.power_on	= stih41x_usb_phy_power_on,
+	.power_off	= stih41x_usb_phy_power_off,
+	.owner		= THIS_MODULE,
+};
+
+static const struct of_device_id stih41x_usb_phy_of_match[];
+
+static int stih41x_usb_phy_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match;
+	struct stih41x_usb_phy *phy_dev;
+	struct device *dev = &pdev->dev;
+	struct phy_provider *phy_provider;
+	struct phy *phy;
+
+	phy_dev = devm_kzalloc(dev, sizeof(*phy_dev), GFP_KERNEL);
+	if (!phy_dev)
+		return -ENOMEM;
+
+	match = of_match_device(stih41x_usb_phy_of_match, &pdev->dev);
+	if (!match)
+		return -ENODEV;
+
+	phy_dev->cfg = match->data;
+
+	phy_dev->regmap = syscon_regmap_lookup_by_phandle(np, "st,syscfg");
+	if (IS_ERR(phy_dev->regmap)) {
+		dev_err(dev, "No syscfg phandle specified\n");
+		return PTR_ERR(phy_dev->regmap);
+	}
+
+	phy_dev->clk = devm_clk_get(dev, "osc_phy");
+	if (IS_ERR(phy_dev->clk)) {
+		dev_err(dev, "osc_phy clk not found\n");
+		return PTR_ERR(phy_dev->clk);
+	}
+
+	phy = devm_phy_create(dev, NULL, &stih41x_usb_phy_ops, NULL);
+
+	if (IS_ERR(phy)) {
+		dev_err(dev, "failed to create phy\n");
+		return PTR_ERR(phy);
+	}
+
+	phy_dev->dev = dev;
+
+	phy_set_drvdata(phy, phy_dev);
+
+	phy_provider = devm_of_phy_provider_register(dev, of_phy_simple_xlate);
+	if (IS_ERR(phy_provider))
+		return PTR_ERR(phy_provider);
+
+	return 0;
+}
+
+static const struct of_device_id stih41x_usb_phy_of_match[] = {
+	{ .compatible = "st,stih415-usb-phy", .data = &stih415_usb_phy_cfg },
+	{ .compatible = "st,stih416-usb-phy", .data = &stih416_usb_phy_cfg },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, stih41x_usb_phy_of_match);
+
+static struct platform_driver stih41x_usb_phy_driver = {
+	.probe	= stih41x_usb_phy_probe,
+	.driver = {
+		.name	= "stih41x-usb-phy",
+		.of_match_table	= stih41x_usb_phy_of_match,
+	}
+};
+module_platform_driver(stih41x_usb_phy_driver);
+
+MODULE_AUTHOR("Maxime Coquelin <maxime.coquelin@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics USB PHY driver for STiH41x series");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/phy/phy-sun4i-usb.c b/drivers/phy/phy-sun4i-usb.c
index 61ebea49709b..0baf5efc8a40 100644
--- a/drivers/phy/phy-sun4i-usb.c
+++ b/drivers/phy/phy-sun4i-usb.c
@@ -325,7 +325,6 @@ static struct platform_driver sun4i_usb_phy_driver = {
 	.driver = {
 		.of_match_table	= sun4i_usb_phy_of_match,
 		.name  = "sun4i-usb-phy",
-		.owner = THIS_MODULE,
 	}
 };
 module_platform_driver(sun4i_usb_phy_driver);
diff --git a/drivers/phy/phy-ti-pipe3.c b/drivers/phy/phy-ti-pipe3.c
index b964aa967b46..ab1e22d9a1e8 100644
--- a/drivers/phy/phy-ti-pipe3.c
+++ b/drivers/phy/phy-ti-pipe3.c
@@ -299,10 +299,9 @@ static int ti_pipe3_probe(struct platform_device *pdev)
 	struct clk *clk;
 
 	phy = devm_kzalloc(&pdev->dev, sizeof(*phy), GFP_KERNEL);
-	if (!phy) {
-		dev_err(&pdev->dev, "unable to alloc mem for TI PIPE3 PHY\n");
+	if (!phy)
 		return -ENOMEM;
-	}
+
 	phy->dev		= &pdev->dev;
 
 	if (!of_device_is_compatible(node, "ti,phy-pipe3-pcie")) {
@@ -519,7 +518,6 @@ static struct platform_driver ti_pipe3_driver = {
 	.remove		= ti_pipe3_remove,
 	.driver		= {
 		.name	= "ti-pipe3",
-		.owner	= THIS_MODULE,
 		.pm	= DEV_PM_OPS,
 		.of_match_table = of_match_ptr(ti_pipe3_id_table),
 	},
diff --git a/drivers/phy/phy-twl4030-usb.c b/drivers/phy/phy-twl4030-usb.c
index e1a6623d4696..7b04befd5271 100644
--- a/drivers/phy/phy-twl4030-usb.c
+++ b/drivers/phy/phy-twl4030-usb.c
@@ -28,12 +28,12 @@
 #include <linux/init.h>
 #include <linux/interrupt.h>
 #include <linux/platform_device.h>
-#include <linux/spinlock.h>
 #include <linux/workqueue.h>
 #include <linux/io.h>
 #include <linux/delay.h>
 #include <linux/usb/otg.h>
 #include <linux/phy/phy.h>
+#include <linux/pm_runtime.h>
 #include <linux/usb/musb-omap.h>
 #include <linux/usb/ulpi.h>
 #include <linux/i2c/twl.h>
@@ -154,7 +154,7 @@ struct twl4030_usb {
 	struct regulator	*usb3v1;
 
 	/* for vbus reporting with irqs disabled */
-	spinlock_t		lock;
+	struct mutex		lock;
 
 	/* pin configuration */
 	enum twl4030_usb_mode	usb_mode;
@@ -162,8 +162,6 @@ struct twl4030_usb {
 	int			irq;
 	enum omap_musb_vbus_id_status linkstat;
 	bool			vbus_supplied;
-	u8			asleep;
-	bool			irq_enabled;
 
 	struct delayed_work	id_workaround_work;
 };
@@ -383,86 +381,84 @@ static void __twl4030_phy_power(struct twl4030_usb *twl, int on)
 	WARN_ON(twl4030_usb_write_verify(twl, PHY_PWR_CTRL, pwr) < 0);
 }
 
-static void twl4030_phy_power(struct twl4030_usb *twl, int on)
+static int twl4030_usb_runtime_suspend(struct device *dev)
 {
-	int ret;
-
-	if (on) {
-		ret = regulator_enable(twl->usb3v1);
-		if (ret)
-			dev_err(twl->dev, "Failed to enable usb3v1\n");
-
-		ret = regulator_enable(twl->usb1v8);
-		if (ret)
-			dev_err(twl->dev, "Failed to enable usb1v8\n");
+	struct twl4030_usb *twl = dev_get_drvdata(dev);
 
-		/*
-		 * Disabling usb3v1 regulator (= writing 0 to VUSB3V1_DEV_GRP
-		 * in twl4030) resets the VUSB_DEDICATED2 register. This reset
-		 * enables VUSB3V1_SLEEP bit that remaps usb3v1 ACTIVE state to
-		 * SLEEP. We work around this by clearing the bit after usv3v1
-		 * is re-activated. This ensures that VUSB3V1 is really active.
-		 */
-		twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB_DEDICATED2);
+	dev_dbg(twl->dev, "%s\n", __func__);
+	if (pm_runtime_suspended(dev))
+		return 0;
 
-		ret = regulator_enable(twl->usb1v5);
-		if (ret)
-			dev_err(twl->dev, "Failed to enable usb1v5\n");
+	__twl4030_phy_power(twl, 0);
+	regulator_disable(twl->usb1v5);
+	regulator_disable(twl->usb1v8);
+	regulator_disable(twl->usb3v1);
 
-		__twl4030_phy_power(twl, 1);
-		twl4030_usb_write(twl, PHY_CLK_CTRL,
-				  twl4030_usb_read(twl, PHY_CLK_CTRL) |
-					(PHY_CLK_CTRL_CLOCKGATING_EN |
-						PHY_CLK_CTRL_CLK32K_EN));
-	} else {
-		__twl4030_phy_power(twl, 0);
-		regulator_disable(twl->usb1v5);
-		regulator_disable(twl->usb1v8);
-		regulator_disable(twl->usb3v1);
-	}
+	return 0;
 }
 
-static int twl4030_phy_power_off(struct phy *phy)
+static int twl4030_usb_runtime_resume(struct device *dev)
 {
-	struct twl4030_usb *twl = phy_get_drvdata(phy);
+	struct twl4030_usb *twl = dev_get_drvdata(dev);
+	int res;
 
-	if (twl->asleep)
+	dev_dbg(twl->dev, "%s\n", __func__);
+	if (pm_runtime_active(dev))
 		return 0;
 
-	twl4030_phy_power(twl, 0);
-	twl->asleep = 1;
-	dev_dbg(twl->dev, "%s\n", __func__);
+	res = regulator_enable(twl->usb3v1);
+	if (res)
+		dev_err(twl->dev, "Failed to enable usb3v1\n");
+
+	res = regulator_enable(twl->usb1v8);
+	if (res)
+		dev_err(twl->dev, "Failed to enable usb1v8\n");
+
+	/*
+	 * Disabling usb3v1 regulator (= writing 0 to VUSB3V1_DEV_GRP
+	 * in twl4030) resets the VUSB_DEDICATED2 register. This reset
+	 * enables VUSB3V1_SLEEP bit that remaps usb3v1 ACTIVE state to
+	 * SLEEP. We work around this by clearing the bit after usv3v1
+	 * is re-activated. This ensures that VUSB3V1 is really active.
+	 */
+	twl_i2c_write_u8(TWL_MODULE_PM_RECEIVER, 0, VUSB_DEDICATED2);
+
+	res = regulator_enable(twl->usb1v5);
+	if (res)
+		dev_err(twl->dev, "Failed to enable usb1v5\n");
+
+	__twl4030_phy_power(twl, 1);
+	twl4030_usb_write(twl, PHY_CLK_CTRL,
+			  twl4030_usb_read(twl, PHY_CLK_CTRL) |
+			  (PHY_CLK_CTRL_CLOCKGATING_EN |
+			   PHY_CLK_CTRL_CLK32K_EN));
+
 	return 0;
 }
 
-static void __twl4030_phy_power_on(struct twl4030_usb *twl)
+static int twl4030_phy_power_off(struct phy *phy)
 {
-	twl4030_phy_power(twl, 1);
-	twl4030_i2c_access(twl, 1);
-	twl4030_usb_set_mode(twl, twl->usb_mode);
-	if (twl->usb_mode == T2_USB_MODE_ULPI)
-		twl4030_i2c_access(twl, 0);
+	struct twl4030_usb *twl = phy_get_drvdata(phy);
+
+	dev_dbg(twl->dev, "%s\n", __func__);
+	pm_runtime_mark_last_busy(twl->dev);
+	pm_runtime_put_autosuspend(twl->dev);
+
+	return 0;
 }
 
 static int twl4030_phy_power_on(struct phy *phy)
 {
 	struct twl4030_usb *twl = phy_get_drvdata(phy);
 
-	if (!twl->asleep)
-		return 0;
-	__twl4030_phy_power_on(twl);
-	twl->asleep = 0;
 	dev_dbg(twl->dev, "%s\n", __func__);
+	pm_runtime_get_sync(twl->dev);
+	twl4030_i2c_access(twl, 1);
+	twl4030_usb_set_mode(twl, twl->usb_mode);
+	if (twl->usb_mode == T2_USB_MODE_ULPI)
+		twl4030_i2c_access(twl, 0);
+	schedule_delayed_work(&twl->id_workaround_work, 0);
 
-	/*
-	 * XXX When VBUS gets driven after musb goes to A mode,
-	 * ID_PRES related interrupts no longer arrive, why?
-	 * Register itself is updated fine though, so we must poll.
-	 */
-	if (twl->linkstat == OMAP_MUSB_ID_GROUND) {
-		cancel_delayed_work(&twl->id_workaround_work);
-		schedule_delayed_work(&twl->id_workaround_work, HZ);
-	}
 	return 0;
 }
 
@@ -519,13 +515,12 @@ static ssize_t twl4030_usb_vbus_show(struct device *dev,
 		struct device_attribute *attr, char *buf)
 {
 	struct twl4030_usb *twl = dev_get_drvdata(dev);
-	unsigned long flags;
 	int ret = -EINVAL;
 
-	spin_lock_irqsave(&twl->lock, flags);
+	mutex_lock(&twl->lock);
 	ret = sprintf(buf, "%s\n",
 			twl->vbus_supplied ? "on" : "off");
-	spin_unlock_irqrestore(&twl->lock, flags);
+	mutex_unlock(&twl->lock);
 
 	return ret;
 }
@@ -539,12 +534,12 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 
 	status = twl4030_usb_linkstat(twl);
 
-	spin_lock_irq(&twl->lock);
+	mutex_lock(&twl->lock);
 	if (status >= 0 && status != twl->linkstat) {
 		twl->linkstat = status;
 		status_changed = true;
 	}
-	spin_unlock_irq(&twl->lock);
+	mutex_unlock(&twl->lock);
 
 	if (status_changed) {
 		/* FIXME add a set_power() method so that B-devices can
@@ -558,9 +553,27 @@ static irqreturn_t twl4030_usb_irq(int irq, void *_twl)
 		 * USB_LINK_VBUS state.  musb_hdrc won't care until it
 		 * starts to handle softconnect right.
 		 */
+		if ((status == OMAP_MUSB_VBUS_VALID) ||
+		    (status == OMAP_MUSB_ID_GROUND)) {
+			if (pm_runtime_suspended(twl->dev))
+				pm_runtime_get_sync(twl->dev);
+		} else {
+			if (pm_runtime_active(twl->dev)) {
+				pm_runtime_mark_last_busy(twl->dev);
+				pm_runtime_put_autosuspend(twl->dev);
+			}
+		}
 		omap_musb_mailbox(status);
 	}
-	sysfs_notify(&twl->dev->kobj, NULL, "vbus");
+
+	/* don't schedule during sleep - irq works right then */
+	if (status == OMAP_MUSB_ID_GROUND && pm_runtime_active(twl->dev)) {
+		cancel_delayed_work(&twl->id_workaround_work);
+		schedule_delayed_work(&twl->id_workaround_work, HZ);
+	}
+
+	if (irq)
+		sysfs_notify(&twl->dev->kobj, NULL, "vbus");
 
 	return IRQ_HANDLED;
 }
@@ -569,52 +582,19 @@ static void twl4030_id_workaround_work(struct work_struct *work)
 {
 	struct twl4030_usb *twl = container_of(work, struct twl4030_usb,
 		id_workaround_work.work);
-	enum omap_musb_vbus_id_status status;
-	bool status_changed = false;
-
-	status = twl4030_usb_linkstat(twl);
-
-	spin_lock_irq(&twl->lock);
-	if (status >= 0 && status != twl->linkstat) {
-		twl->linkstat = status;
-		status_changed = true;
-	}
-	spin_unlock_irq(&twl->lock);
 
-	if (status_changed) {
-		dev_dbg(twl->dev, "handle missing status change to %d\n",
-				status);
-		omap_musb_mailbox(status);
-	}
-
-	/* don't schedule during sleep - irq works right then */
-	if (status == OMAP_MUSB_ID_GROUND && !twl->asleep) {
-		cancel_delayed_work(&twl->id_workaround_work);
-		schedule_delayed_work(&twl->id_workaround_work, HZ);
-	}
+	twl4030_usb_irq(0, twl);
 }
 
 static int twl4030_phy_init(struct phy *phy)
 {
 	struct twl4030_usb *twl = phy_get_drvdata(phy);
-	enum omap_musb_vbus_id_status status;
-
-	/*
-	 * Start in sleep state, we'll get called through set_suspend()
-	 * callback when musb is runtime resumed and it's time to start.
-	 */
-	__twl4030_phy_power(twl, 0);
-	twl->asleep = 1;
 
-	status = twl4030_usb_linkstat(twl);
-	twl->linkstat = status;
-
-	if (status == OMAP_MUSB_ID_GROUND || status == OMAP_MUSB_VBUS_VALID) {
-		omap_musb_mailbox(twl->linkstat);
-		twl4030_phy_power_on(phy);
-	}
+	pm_runtime_get_sync(twl->dev);
+	schedule_delayed_work(&twl->id_workaround_work, 0);
+	pm_runtime_mark_last_busy(twl->dev);
+	pm_runtime_put_autosuspend(twl->dev);
 
-	sysfs_notify(&twl->dev->kobj, NULL, "vbus");
 	return 0;
 }
 
@@ -650,6 +630,11 @@ static const struct phy_ops ops = {
 	.owner		= THIS_MODULE,
 };
 
+static const struct dev_pm_ops twl4030_usb_pm_ops = {
+	SET_RUNTIME_PM_OPS(twl4030_usb_runtime_suspend,
+			   twl4030_usb_runtime_resume, NULL)
+};
+
 static int twl4030_usb_probe(struct platform_device *pdev)
 {
 	struct twl4030_usb_data *pdata = dev_get_platdata(&pdev->dev);
@@ -683,7 +668,7 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	twl->dev		= &pdev->dev;
 	twl->irq		= platform_get_irq(pdev, 0);
 	twl->vbus_supplied	= false;
-	twl->asleep		= 1;
+	twl->linkstat		= -EINVAL;
 	twl->linkstat		= OMAP_MUSB_UNKNOWN;
 
 	twl->phy.dev		= twl->dev;
@@ -708,8 +693,8 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	if (IS_ERR(phy_provider))
 		return PTR_ERR(phy_provider);
 
-	/* init spinlock for workqueue */
-	spin_lock_init(&twl->lock);
+	/* init mutex for workqueue */
+	mutex_init(&twl->lock);
 
 	INIT_DELAYED_WORK(&twl->id_workaround_work, twl4030_id_workaround_work);
 
@@ -726,6 +711,11 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 
 	ATOMIC_INIT_NOTIFIER_HEAD(&twl->phy.notifier);
 
+	pm_runtime_use_autosuspend(&pdev->dev);
+	pm_runtime_set_autosuspend_delay(&pdev->dev, 2000);
+	pm_runtime_enable(&pdev->dev);
+	pm_runtime_get_sync(&pdev->dev);
+
 	/* Our job is to use irqs and status from the power module
 	 * to keep the transceiver disabled when nothing's connected.
 	 *
@@ -734,7 +724,6 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 	 * set_host() and/or set_peripheral() ... OTG_capable boards
 	 * need both handles, otherwise just one suffices.
 	 */
-	twl->irq_enabled = true;
 	status = devm_request_threaded_irq(twl->dev, twl->irq, NULL,
 			twl4030_usb_irq, IRQF_TRIGGER_FALLING |
 			IRQF_TRIGGER_RISING | IRQF_ONESHOT, "twl4030_usb", twl);
@@ -744,6 +733,9 @@ static int twl4030_usb_probe(struct platform_device *pdev)
 		return status;
 	}
 
+	pm_runtime_mark_last_busy(&pdev->dev);
+	pm_runtime_put_autosuspend(twl->dev);
+
 	dev_info(&pdev->dev, "Initialized TWL4030 USB module\n");
 	return 0;
 }
@@ -753,6 +745,7 @@ static int twl4030_usb_remove(struct platform_device *pdev)
 	struct twl4030_usb *twl = platform_get_drvdata(pdev);
 	int val;
 
+	pm_runtime_get_sync(twl->dev);
 	cancel_delayed_work(&twl->id_workaround_work);
 	device_remove_file(twl->dev, &dev_attr_vbus);
 
@@ -772,9 +765,8 @@ static int twl4030_usb_remove(struct platform_device *pdev)
 
 	/* disable complete OTG block */
 	twl4030_usb_clear_bits(twl, POWER_CTRL, POWER_CTRL_OTG_ENAB);
-
-	if (!twl->asleep)
-		twl4030_phy_power(twl, 0);
+	pm_runtime_mark_last_busy(twl->dev);
+	pm_runtime_put(twl->dev);
 
 	return 0;
 }
@@ -792,7 +784,7 @@ static struct platform_driver twl4030_usb_driver = {
 	.remove		= twl4030_usb_remove,
 	.driver		= {
 		.name	= "twl4030_usb",
-		.owner	= THIS_MODULE,
+		.pm	= &twl4030_usb_pm_ops,
 		.of_match_table = of_match_ptr(twl4030_usb_id_table),
 	},
 };
diff --git a/drivers/phy/phy-xgene.c b/drivers/phy/phy-xgene.c
index db809b97219e..f8a51b16ade8 100644
--- a/drivers/phy/phy-xgene.c
+++ b/drivers/phy/phy-xgene.c
@@ -1738,7 +1738,6 @@ static struct platform_driver xgene_phy_driver = {
 	.probe = xgene_phy_probe,
 	.driver = {
 		   .name = "xgene-phy",
-		   .owner = THIS_MODULE,
 		   .of_match_table = xgene_phy_of_match,
 	},
 };
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index bfd2c2e9f6cd..c6a66de6ed72 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -86,7 +86,7 @@ config PINCTRL_BCM2835
 
 config PINCTRL_BCM281XX
 	bool "Broadcom BCM281xx pinctrl driver"
-	depends on OF
+	depends on OF && (ARCH_BCM_MOBILE || COMPILE_TEST)
 	select PINMUX
 	select PINCONF
 	select GENERIC_PINCONF
@@ -97,95 +97,6 @@ config PINCTRL_BCM281XX
 	  BCM28145, and BCM28155 SoCs.  This driver requires the pinctrl
 	  framework.  GPIO is provided by a separate GPIO driver.
 
-config PINCTRL_IMX
-	bool
-	select PINMUX
-	select PINCONF
-
-config PINCTRL_IMX1_CORE
-	bool
-	select PINMUX
-	select PINCONF
-
-config PINCTRL_IMX1
-	bool "IMX1 pinctrl driver"
-	depends on SOC_IMX1
-	select PINCTRL_IMX1_CORE
-	help
-	  Say Y here to enable the imx1 pinctrl driver
-
-config PINCTRL_IMX27
-	bool "IMX27 pinctrl driver"
-	depends on SOC_IMX27
-	select PINCTRL_IMX1_CORE
-	help
-	  Say Y here to enable the imx27 pinctrl driver
-
-
-config PINCTRL_IMX25
-        bool "IMX25 pinctrl driver"
-        depends on OF
-        depends on SOC_IMX25
-        select PINCTRL_IMX
-        help
-          Say Y here to enable the imx25 pinctrl driver
-
-config PINCTRL_IMX35
-	bool "IMX35 pinctrl driver"
-	depends on SOC_IMX35
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the imx35 pinctrl driver
-
-config PINCTRL_IMX50
-	bool "IMX50 pinctrl driver"
-	depends on SOC_IMX50
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the imx50 pinctrl driver
-
-config PINCTRL_IMX51
-	bool "IMX51 pinctrl driver"
-	depends on SOC_IMX51
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the imx51 pinctrl driver
-
-config PINCTRL_IMX53
-	bool "IMX53 pinctrl driver"
-	depends on SOC_IMX53
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the imx53 pinctrl driver
-
-config PINCTRL_IMX6Q
-	bool "IMX6Q/DL pinctrl driver"
-	depends on SOC_IMX6Q
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the imx6q/dl pinctrl driver
-
-config PINCTRL_IMX6SL
-	bool "IMX6SL pinctrl driver"
-	depends on SOC_IMX6SL
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the imx6sl pinctrl driver
-
-config PINCTRL_IMX6SX
-	bool "IMX6SX pinctrl driver"
-	depends on SOC_IMX6SX
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the imx6sx pinctrl driver
-
-config PINCTRL_VF610
-	bool "Freescale Vybrid VF610 pinctrl driver"
-	depends on SOC_VF610
-	select PINCTRL_IMX
-	help
-	  Say Y here to enable the Freescale Vybrid VF610 pinctrl driver
-
 config PINCTRL_LANTIQ
 	bool
 	depends on LANTIQ
@@ -197,19 +108,6 @@ config PINCTRL_FALCON
 	depends on SOC_FALCON
 	depends on PINCTRL_LANTIQ
 
-config PINCTRL_MXS
-	bool
-	select PINMUX
-	select PINCONF
-
-config PINCTRL_IMX23
-	bool
-	select PINCTRL_MXS
-
-config PINCTRL_IMX28
-	bool
-	select PINCTRL_MXS
-
 config PINCTRL_ROCKCHIP
 	bool
 	select PINMUX
@@ -306,6 +204,7 @@ config PINCTRL_PALMAS
 	  TPS65913, TPS80036 etc.
 
 source "drivers/pinctrl/berlin/Kconfig"
+source "drivers/pinctrl/freescale/Kconfig"
 source "drivers/pinctrl/mvebu/Kconfig"
 source "drivers/pinctrl/nomadik/Kconfig"
 source "drivers/pinctrl/qcom/Kconfig"
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 05d227508c95..51f52d32859e 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -17,23 +17,7 @@ obj-$(CONFIG_PINCTRL_AT91)	+= pinctrl-at91.o
 obj-$(CONFIG_PINCTRL_BCM2835)	+= pinctrl-bcm2835.o
 obj-$(CONFIG_PINCTRL_BAYTRAIL)	+= pinctrl-baytrail.o
 obj-$(CONFIG_PINCTRL_BCM281XX)	+= pinctrl-bcm281xx.o
-obj-$(CONFIG_PINCTRL_IMX)	+= pinctrl-imx.o
-obj-$(CONFIG_PINCTRL_IMX1_CORE)	+= pinctrl-imx1-core.o
-obj-$(CONFIG_PINCTRL_IMX1)	+= pinctrl-imx1.o
-obj-$(CONFIG_PINCTRL_IMX27)	+= pinctrl-imx27.o
-obj-$(CONFIG_PINCTRL_IMX35)	+= pinctrl-imx35.o
-obj-$(CONFIG_PINCTRL_IMX50)	+= pinctrl-imx50.o
-obj-$(CONFIG_PINCTRL_IMX51)	+= pinctrl-imx51.o
-obj-$(CONFIG_PINCTRL_IMX53)	+= pinctrl-imx53.o
-obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6q.o
-obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6dl.o
-obj-$(CONFIG_PINCTRL_IMX6SL)	+= pinctrl-imx6sl.o
-obj-$(CONFIG_PINCTRL_IMX6SX)	+= pinctrl-imx6sx.o
 obj-$(CONFIG_PINCTRL_FALCON)	+= pinctrl-falcon.o
-obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
-obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
-obj-$(CONFIG_PINCTRL_IMX25)	+= pinctrl-imx25.o
-obj-$(CONFIG_PINCTRL_IMX28)	+= pinctrl-imx28.o
 obj-$(CONFIG_PINCTRL_PALMAS)	+= pinctrl-palmas.o
 obj-$(CONFIG_PINCTRL_ROCKCHIP)	+= pinctrl-rockchip.o
 obj-$(CONFIG_PINCTRL_SINGLE)	+= pinctrl-single.o
@@ -52,15 +36,14 @@ obj-$(CONFIG_PINCTRL_XWAY)	+= pinctrl-xway.o
 obj-$(CONFIG_PINCTRL_LANTIQ)	+= pinctrl-lantiq.o
 obj-$(CONFIG_PINCTRL_TB10X)	+= pinctrl-tb10x.o
 obj-$(CONFIG_PINCTRL_ST) 	+= pinctrl-st.o
-obj-$(CONFIG_PINCTRL_VF610)	+= pinctrl-vf610.o
 
 obj-$(CONFIG_ARCH_BERLIN)	+= berlin/
+obj-y				+= freescale/
 obj-$(CONFIG_PLAT_ORION)        += mvebu/
 obj-y				+= nomadik/
 obj-$(CONFIG_ARCH_QCOM)		+= qcom/
-obj-$(CONFIG_PLAT_SAMSUNG)	+= samsung/
-obj-$(CONFIG_ARCH_SHMOBILE)	+= sh-pfc/
-obj-$(CONFIG_SUPERH)		+= sh-pfc/
+obj-$(CONFIG_PINCTRL_SAMSUNG)	+= samsung/
+obj-$(CONFIG_PINCTRL_SH_PFC)	+= sh-pfc/
 obj-$(CONFIG_PLAT_SPEAR)	+= spear/
 obj-$(CONFIG_ARCH_SUNXI)	+= sunxi/
 obj-$(CONFIG_ARCH_VT8500)	+= vt8500/
diff --git a/drivers/pinctrl/berlin/berlin.c b/drivers/pinctrl/berlin/berlin.c
index 86db2235ab00..7f0b0f93242b 100644
--- a/drivers/pinctrl/berlin/berlin.c
+++ b/drivers/pinctrl/berlin/berlin.c
@@ -99,30 +99,11 @@ static int berlin_pinctrl_dt_node_to_map(struct pinctrl_dev *pctrl_dev,
 	return 0;
 }
 
-static void berlin_pinctrl_dt_free_map(struct pinctrl_dev *pctrl_dev,
-				       struct pinctrl_map *map,
-				       unsigned nmaps)
-{
-	int i;
-
-	for (i = 0; i < nmaps; i++) {
-		if (map[i].type == PIN_MAP_TYPE_MUX_GROUP) {
-			kfree(map[i].data.mux.group);
-
-			/* a function can be applied to multiple groups */
-			if (i == 0)
-				kfree(map[i].data.mux.function);
-		}
-	}
-
-	kfree(map);
-}
-
 static const struct pinctrl_ops berlin_pinctrl_ops = {
 	.get_groups_count	= &berlin_pinctrl_get_group_count,
 	.get_group_name		= &berlin_pinctrl_get_group_name,
 	.dt_node_to_map		= &berlin_pinctrl_dt_node_to_map,
-	.dt_free_map		= &berlin_pinctrl_dt_free_map,
+	.dt_free_map		= &pinctrl_utils_dt_free_map,
 };
 
 static int berlin_pinmux_get_functions_count(struct pinctrl_dev *pctrl_dev)
@@ -170,9 +151,9 @@ berlin_pinctrl_find_function_by_name(struct berlin_pinctrl *pctrl,
 	return NULL;
 }
 
-static int berlin_pinmux_enable(struct pinctrl_dev *pctrl_dev,
-				unsigned function,
-				unsigned group)
+static int berlin_pinmux_set(struct pinctrl_dev *pctrl_dev,
+			     unsigned function,
+			     unsigned group)
 {
 	struct berlin_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctrl_dev);
 	const struct berlin_desc_group *group_desc = pctrl->desc->groups + group;
@@ -197,7 +178,7 @@ static const struct pinmux_ops berlin_pinmux_ops = {
 	.get_functions_count	= &berlin_pinmux_get_functions_count,
 	.get_function_name	= &berlin_pinmux_get_function_name,
 	.get_function_groups	= &berlin_pinmux_get_function_groups,
-	.enable			= &berlin_pinmux_enable,
+	.set_mux		= &berlin_pinmux_set,
 };
 
 static int berlin_pinctrl_add_function(struct berlin_pinctrl *pctrl,
diff --git a/drivers/pinctrl/freescale/Kconfig b/drivers/pinctrl/freescale/Kconfig
new file mode 100644
index 000000000000..16aac38793fe
--- /dev/null
+++ b/drivers/pinctrl/freescale/Kconfig
@@ -0,0 +1,108 @@
+config PINCTRL_IMX
+	bool
+	select PINMUX
+	select PINCONF
+
+config PINCTRL_IMX1_CORE
+	bool
+	select PINMUX
+	select PINCONF
+
+config PINCTRL_IMX1
+	bool "IMX1 pinctrl driver"
+	depends on SOC_IMX1
+	select PINCTRL_IMX1_CORE
+	help
+	  Say Y here to enable the imx1 pinctrl driver
+
+config PINCTRL_IMX21
+	bool "i.MX21 pinctrl driver"
+	depends on SOC_IMX21
+	select PINCTRL_IMX1_CORE
+	help
+	  Say Y here to enable the i.MX21 pinctrl driver
+
+config PINCTRL_IMX27
+	bool "IMX27 pinctrl driver"
+	depends on SOC_IMX27
+	select PINCTRL_IMX1_CORE
+	help
+	  Say Y here to enable the imx27 pinctrl driver
+
+
+config PINCTRL_IMX25
+        bool "IMX25 pinctrl driver"
+        depends on OF
+        depends on SOC_IMX25
+        select PINCTRL_IMX
+        help
+          Say Y here to enable the imx25 pinctrl driver
+
+config PINCTRL_IMX35
+	bool "IMX35 pinctrl driver"
+	depends on SOC_IMX35
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx35 pinctrl driver
+
+config PINCTRL_IMX50
+	bool "IMX50 pinctrl driver"
+	depends on SOC_IMX50
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx50 pinctrl driver
+
+config PINCTRL_IMX51
+	bool "IMX51 pinctrl driver"
+	depends on SOC_IMX51
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx51 pinctrl driver
+
+config PINCTRL_IMX53
+	bool "IMX53 pinctrl driver"
+	depends on SOC_IMX53
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx53 pinctrl driver
+
+config PINCTRL_IMX6Q
+	bool "IMX6Q/DL pinctrl driver"
+	depends on SOC_IMX6Q
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx6q/dl pinctrl driver
+
+config PINCTRL_IMX6SL
+	bool "IMX6SL pinctrl driver"
+	depends on SOC_IMX6SL
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx6sl pinctrl driver
+
+config PINCTRL_IMX6SX
+	bool "IMX6SX pinctrl driver"
+	depends on SOC_IMX6SX
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the imx6sx pinctrl driver
+
+config PINCTRL_VF610
+	bool "Freescale Vybrid VF610 pinctrl driver"
+	depends on SOC_VF610
+	select PINCTRL_IMX
+	help
+	  Say Y here to enable the Freescale Vybrid VF610 pinctrl driver
+
+config PINCTRL_MXS
+	bool
+	select PINMUX
+	select PINCONF
+
+config PINCTRL_IMX23
+	bool
+	select PINCTRL_MXS
+
+config PINCTRL_IMX28
+	bool
+	select PINCTRL_MXS
diff --git a/drivers/pinctrl/freescale/Makefile b/drivers/pinctrl/freescale/Makefile
new file mode 100644
index 000000000000..bba73c22f043
--- /dev/null
+++ b/drivers/pinctrl/freescale/Makefile
@@ -0,0 +1,19 @@
+# Freescale pin control drivers
+obj-$(CONFIG_PINCTRL_IMX)	+= pinctrl-imx.o
+obj-$(CONFIG_PINCTRL_IMX1_CORE)	+= pinctrl-imx1-core.o
+obj-$(CONFIG_PINCTRL_IMX1)	+= pinctrl-imx1.o
+obj-$(CONFIG_PINCTRL_IMX21)	+= pinctrl-imx21.o
+obj-$(CONFIG_PINCTRL_IMX27)	+= pinctrl-imx27.o
+obj-$(CONFIG_PINCTRL_IMX35)	+= pinctrl-imx35.o
+obj-$(CONFIG_PINCTRL_IMX50)	+= pinctrl-imx50.o
+obj-$(CONFIG_PINCTRL_IMX51)	+= pinctrl-imx51.o
+obj-$(CONFIG_PINCTRL_IMX53)	+= pinctrl-imx53.o
+obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6q.o
+obj-$(CONFIG_PINCTRL_IMX6Q)	+= pinctrl-imx6dl.o
+obj-$(CONFIG_PINCTRL_IMX6SL)	+= pinctrl-imx6sl.o
+obj-$(CONFIG_PINCTRL_IMX6SX)	+= pinctrl-imx6sx.o
+obj-$(CONFIG_PINCTRL_VF610)	+= pinctrl-vf610.o
+obj-$(CONFIG_PINCTRL_MXS)	+= pinctrl-mxs.o
+obj-$(CONFIG_PINCTRL_IMX23)	+= pinctrl-imx23.o
+obj-$(CONFIG_PINCTRL_IMX25)	+= pinctrl-imx25.o
+obj-$(CONFIG_PINCTRL_IMX28)	+= pinctrl-imx28.o
diff --git a/drivers/pinctrl/pinctrl-imx.c b/drivers/pinctrl/freescale/pinctrl-imx.c
index 946d594a64dd..f2446769247f 100644
--- a/drivers/pinctrl/pinctrl-imx.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx.c
@@ -24,7 +24,7 @@
 #include <linux/pinctrl/pinmux.h>
 #include <linux/slab.h>
 
-#include "core.h"
+#include "../core.h"
 #include "pinctrl-imx.h"
 
 /* The bits in CONFIG cell defined in binding doc*/
@@ -179,8 +179,8 @@ static const struct pinctrl_ops imx_pctrl_ops = {
 
 };
 
-static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
-			   unsigned group)
+static int imx_pmx_set(struct pinctrl_dev *pctldev, unsigned selector,
+		       unsigned group)
 {
 	struct imx_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
@@ -204,7 +204,7 @@ static int imx_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
 		pin_id = pin->pin;
 		pin_reg = &info->pin_regs[pin_id];
 
-		if (!(info->flags & ZERO_OFFSET_VALID) && !pin_reg->mux_reg) {
+		if (pin_reg->mux_reg == -1) {
 			dev_err(ipctl->dev, "Pin(%s) does not support mux function\n",
 				info->pins[pin_id].name);
 			return -EINVAL;
@@ -298,7 +298,7 @@ static const struct pinmux_ops imx_pmx_ops = {
 	.get_functions_count = imx_pmx_get_funcs_count,
 	.get_function_name = imx_pmx_get_func_name,
 	.get_function_groups = imx_pmx_get_groups,
-	.enable = imx_pmx_enable,
+	.set_mux = imx_pmx_set,
 };
 
 static int imx_pinconf_get(struct pinctrl_dev *pctldev,
@@ -308,7 +308,7 @@ static int imx_pinconf_get(struct pinctrl_dev *pctldev,
 	const struct imx_pinctrl_soc_info *info = ipctl->info;
 	const struct imx_pin_reg *pin_reg = &info->pin_regs[pin_id];
 
-	if (!(info->flags & ZERO_OFFSET_VALID) && !pin_reg->conf_reg) {
+	if (pin_reg->conf_reg == -1) {
 		dev_err(info->dev, "Pin(%s) does not support config function\n",
 			info->pins[pin_id].name);
 		return -EINVAL;
@@ -331,7 +331,7 @@ static int imx_pinconf_set(struct pinctrl_dev *pctldev,
 	const struct imx_pin_reg *pin_reg = &info->pin_regs[pin_id];
 	int i;
 
-	if (!(info->flags & ZERO_OFFSET_VALID) && !pin_reg->conf_reg) {
+	if (pin_reg->conf_reg == -1) {
 		dev_err(info->dev, "Pin(%s) does not support config function\n",
 			info->pins[pin_id].name);
 		return -EINVAL;
@@ -586,10 +586,11 @@ int imx_pinctrl_probe(struct platform_device *pdev,
 	if (!ipctl)
 		return -ENOMEM;
 
-	info->pin_regs = devm_kzalloc(&pdev->dev, sizeof(*info->pin_regs) *
+	info->pin_regs = devm_kmalloc(&pdev->dev, sizeof(*info->pin_regs) *
 				      info->npins, GFP_KERNEL);
 	if (!info->pin_regs)
 		return -ENOMEM;
+	memset(info->pin_regs, 0xff, sizeof(*info->pin_regs) * info->npins);
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	ipctl->base = devm_ioremap_resource(&pdev->dev, res);
diff --git a/drivers/pinctrl/pinctrl-imx.h b/drivers/pinctrl/freescale/pinctrl-imx.h
index db408b057000..49e55d39f7c8 100644
--- a/drivers/pinctrl/pinctrl-imx.h
+++ b/drivers/pinctrl/freescale/pinctrl-imx.h
@@ -67,8 +67,8 @@ struct imx_pmx_func {
  * @conf_reg: config register offset
  */
 struct imx_pin_reg {
-	u16 mux_reg;
-	u16 conf_reg;
+	s16 mux_reg;
+	s16 conf_reg;
 };
 
 struct imx_pinctrl_soc_info {
@@ -83,8 +83,7 @@ struct imx_pinctrl_soc_info {
 	unsigned int flags;
 };
 
-#define ZERO_OFFSET_VALID	0x1
-#define SHARE_MUX_CONF_REG	0x2
+#define SHARE_MUX_CONF_REG	0x1
 
 #define NO_MUX		0x0
 #define NO_PAD		0x0
diff --git a/drivers/pinctrl/pinctrl-imx1-core.c b/drivers/pinctrl/freescale/pinctrl-imx1-core.c
index 483420757c9f..5ac59fbb2440 100644
--- a/drivers/pinctrl/pinctrl-imx1-core.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx1-core.c
@@ -28,7 +28,7 @@
 #include <linux/pinctrl/pinmux.h>
 #include <linux/slab.h>
 
-#include "core.h"
+#include "../core.h"
 #include "pinctrl-imx1.h"
 
 struct imx1_pinctrl {
@@ -298,8 +298,8 @@ static const struct pinctrl_ops imx1_pctrl_ops = {
 
 };
 
-static int imx1_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
-			   unsigned group)
+static int imx1_pmx_set(struct pinctrl_dev *pctldev, unsigned selector,
+			unsigned group)
 {
 	struct imx1_pinctrl *ipctl = pinctrl_dev_get_drvdata(pctldev);
 	const struct imx1_pinctrl_soc_info *info = ipctl->info;
@@ -385,7 +385,7 @@ static const struct pinmux_ops imx1_pmx_ops = {
 	.get_functions_count = imx1_pmx_get_funcs_count,
 	.get_function_name = imx1_pmx_get_func_name,
 	.get_function_groups = imx1_pmx_get_groups,
-	.enable = imx1_pmx_enable,
+	.set_mux = imx1_pmx_set,
 };
 
 static int imx1_pinconf_get(struct pinctrl_dev *pctldev,
diff --git a/drivers/pinctrl/pinctrl-imx1.c b/drivers/pinctrl/freescale/pinctrl-imx1.c
index 533a6e519648..533a6e519648 100644
--- a/drivers/pinctrl/pinctrl-imx1.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx1.c
diff --git a/drivers/pinctrl/pinctrl-imx1.h b/drivers/pinctrl/freescale/pinctrl-imx1.h
index 692a54c15cda..692a54c15cda 100644
--- a/drivers/pinctrl/pinctrl-imx1.h
+++ b/drivers/pinctrl/freescale/pinctrl-imx1.h
diff --git a/drivers/pinctrl/freescale/pinctrl-imx21.c b/drivers/pinctrl/freescale/pinctrl-imx21.c
new file mode 100644
index 000000000000..1b3b2311b033
--- /dev/null
+++ b/drivers/pinctrl/freescale/pinctrl-imx21.c
@@ -0,0 +1,342 @@
+/*
+ * i.MX21 pinctrl driver based on imx pinmux core
+ *
+ * Copyright (C) 2014 Alexander Shiyan <shc_work@mail.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-imx1.h"
+
+#define PAD_ID(port, pin)	((port) * 32 + (pin))
+#define PA	0
+#define PB	1
+#define PC	2
+#define PD	3
+#define PE	4
+#define PF	5
+
+enum imx21_pads {
+	MX21_PAD_LSCLK		= PAD_ID(PA, 5),
+	MX21_PAD_LD0		= PAD_ID(PA, 6),
+	MX21_PAD_LD1		= PAD_ID(PA, 7),
+	MX21_PAD_LD2		= PAD_ID(PA, 8),
+	MX21_PAD_LD3		= PAD_ID(PA, 9),
+	MX21_PAD_LD4		= PAD_ID(PA, 10),
+	MX21_PAD_LD5		= PAD_ID(PA, 11),
+	MX21_PAD_LD6		= PAD_ID(PA, 12),
+	MX21_PAD_LD7		= PAD_ID(PA, 13),
+	MX21_PAD_LD8		= PAD_ID(PA, 14),
+	MX21_PAD_LD9		= PAD_ID(PA, 15),
+	MX21_PAD_LD10		= PAD_ID(PA, 16),
+	MX21_PAD_LD11		= PAD_ID(PA, 17),
+	MX21_PAD_LD12		= PAD_ID(PA, 18),
+	MX21_PAD_LD13		= PAD_ID(PA, 19),
+	MX21_PAD_LD14		= PAD_ID(PA, 20),
+	MX21_PAD_LD15		= PAD_ID(PA, 21),
+	MX21_PAD_LD16		= PAD_ID(PA, 22),
+	MX21_PAD_LD17		= PAD_ID(PA, 23),
+	MX21_PAD_REV		= PAD_ID(PA, 24),
+	MX21_PAD_CLS		= PAD_ID(PA, 25),
+	MX21_PAD_PS		= PAD_ID(PA, 26),
+	MX21_PAD_SPL_SPR	= PAD_ID(PA, 27),
+	MX21_PAD_HSYNC		= PAD_ID(PA, 28),
+	MX21_PAD_VSYNC		= PAD_ID(PA, 29),
+	MX21_PAD_CONTRAST	= PAD_ID(PA, 30),
+	MX21_PAD_OE_ACD		= PAD_ID(PA, 31),
+	MX21_PAD_SD2_D0		= PAD_ID(PB, 4),
+	MX21_PAD_SD2_D1		= PAD_ID(PB, 5),
+	MX21_PAD_SD2_D2		= PAD_ID(PB, 6),
+	MX21_PAD_SD2_D3		= PAD_ID(PB, 7),
+	MX21_PAD_SD2_CMD	= PAD_ID(PB, 8),
+	MX21_PAD_SD2_CLK	= PAD_ID(PB, 9),
+	MX21_PAD_CSI_D0		= PAD_ID(PB, 10),
+	MX21_PAD_CSI_D1		= PAD_ID(PB, 11),
+	MX21_PAD_CSI_D2		= PAD_ID(PB, 12),
+	MX21_PAD_CSI_D3		= PAD_ID(PB, 13),
+	MX21_PAD_CSI_D4		= PAD_ID(PB, 14),
+	MX21_PAD_CSI_MCLK	= PAD_ID(PB, 15),
+	MX21_PAD_CSI_PIXCLK	= PAD_ID(PB, 16),
+	MX21_PAD_CSI_D5		= PAD_ID(PB, 17),
+	MX21_PAD_CSI_D6		= PAD_ID(PB, 18),
+	MX21_PAD_CSI_D7		= PAD_ID(PB, 19),
+	MX21_PAD_CSI_VSYNC	= PAD_ID(PB, 20),
+	MX21_PAD_CSI_HSYNC	= PAD_ID(PB, 21),
+	MX21_PAD_USB_BYP	= PAD_ID(PB, 22),
+	MX21_PAD_USB_PWR	= PAD_ID(PB, 23),
+	MX21_PAD_USB_OC		= PAD_ID(PB, 24),
+	MX21_PAD_USBH_ON	= PAD_ID(PB, 25),
+	MX21_PAD_USBH1_FS	= PAD_ID(PB, 26),
+	MX21_PAD_USBH1_OE	= PAD_ID(PB, 27),
+	MX21_PAD_USBH1_TXDM	= PAD_ID(PB, 28),
+	MX21_PAD_USBH1_TXDP	= PAD_ID(PB, 29),
+	MX21_PAD_USBH1_RXDM	= PAD_ID(PB, 30),
+	MX21_PAD_USBH1_RXDP	= PAD_ID(PB, 31),
+	MX21_PAD_USBG_SDA	= PAD_ID(PC, 5),
+	MX21_PAD_USBG_SCL	= PAD_ID(PC, 6),
+	MX21_PAD_USBG_ON	= PAD_ID(PC, 7),
+	MX21_PAD_USBG_FS	= PAD_ID(PC, 8),
+	MX21_PAD_USBG_OE	= PAD_ID(PC, 9),
+	MX21_PAD_USBG_TXDM	= PAD_ID(PC, 10),
+	MX21_PAD_USBG_TXDP	= PAD_ID(PC, 11),
+	MX21_PAD_USBG_RXDM	= PAD_ID(PC, 12),
+	MX21_PAD_USBG_RXDP	= PAD_ID(PC, 13),
+	MX21_PAD_TOUT		= PAD_ID(PC, 14),
+	MX21_PAD_TIN		= PAD_ID(PC, 15),
+	MX21_PAD_SAP_FS		= PAD_ID(PC, 16),
+	MX21_PAD_SAP_RXD	= PAD_ID(PC, 17),
+	MX21_PAD_SAP_TXD	= PAD_ID(PC, 18),
+	MX21_PAD_SAP_CLK	= PAD_ID(PC, 19),
+	MX21_PAD_SSI1_FS	= PAD_ID(PC, 20),
+	MX21_PAD_SSI1_RXD	= PAD_ID(PC, 21),
+	MX21_PAD_SSI1_TXD	= PAD_ID(PC, 22),
+	MX21_PAD_SSI1_CLK	= PAD_ID(PC, 23),
+	MX21_PAD_SSI2_FS	= PAD_ID(PC, 24),
+	MX21_PAD_SSI2_RXD	= PAD_ID(PC, 25),
+	MX21_PAD_SSI2_TXD	= PAD_ID(PC, 26),
+	MX21_PAD_SSI2_CLK	= PAD_ID(PC, 27),
+	MX21_PAD_SSI3_FS	= PAD_ID(PC, 28),
+	MX21_PAD_SSI3_RXD	= PAD_ID(PC, 29),
+	MX21_PAD_SSI3_TXD	= PAD_ID(PC, 30),
+	MX21_PAD_SSI3_CLK	= PAD_ID(PC, 31),
+	MX21_PAD_I2C_DATA	= PAD_ID(PD, 17),
+	MX21_PAD_I2C_CLK	= PAD_ID(PD, 18),
+	MX21_PAD_CSPI2_SS2	= PAD_ID(PD, 19),
+	MX21_PAD_CSPI2_SS1	= PAD_ID(PD, 20),
+	MX21_PAD_CSPI2_SS0	= PAD_ID(PD, 21),
+	MX21_PAD_CSPI2_SCLK	= PAD_ID(PD, 22),
+	MX21_PAD_CSPI2_MISO	= PAD_ID(PD, 23),
+	MX21_PAD_CSPI2_MOSI	= PAD_ID(PD, 24),
+	MX21_PAD_CSPI1_RDY	= PAD_ID(PD, 25),
+	MX21_PAD_CSPI1_SS2	= PAD_ID(PD, 26),
+	MX21_PAD_CSPI1_SS1	= PAD_ID(PD, 27),
+	MX21_PAD_CSPI1_SS0	= PAD_ID(PD, 28),
+	MX21_PAD_CSPI1_SCLK	= PAD_ID(PD, 29),
+	MX21_PAD_CSPI1_MISO	= PAD_ID(PD, 30),
+	MX21_PAD_CSPI1_MOSI	= PAD_ID(PD, 31),
+	MX21_PAD_TEST_WB2	= PAD_ID(PE, 0),
+	MX21_PAD_TEST_WB1	= PAD_ID(PE, 1),
+	MX21_PAD_TEST_WB0	= PAD_ID(PE, 2),
+	MX21_PAD_UART2_CTS	= PAD_ID(PE, 3),
+	MX21_PAD_UART2_RTS	= PAD_ID(PE, 4),
+	MX21_PAD_PWMO		= PAD_ID(PE, 5),
+	MX21_PAD_UART2_TXD	= PAD_ID(PE, 6),
+	MX21_PAD_UART2_RXD	= PAD_ID(PE, 7),
+	MX21_PAD_UART3_TXD	= PAD_ID(PE, 8),
+	MX21_PAD_UART3_RXD	= PAD_ID(PE, 9),
+	MX21_PAD_UART3_CTS	= PAD_ID(PE, 10),
+	MX21_PAD_UART3_RTS	= PAD_ID(PE, 11),
+	MX21_PAD_UART1_TXD	= PAD_ID(PE, 12),
+	MX21_PAD_UART1_RXD	= PAD_ID(PE, 13),
+	MX21_PAD_UART1_CTS	= PAD_ID(PE, 14),
+	MX21_PAD_UART1_RTS	= PAD_ID(PE, 15),
+	MX21_PAD_RTCK		= PAD_ID(PE, 16),
+	MX21_PAD_RESET_OUT	= PAD_ID(PE, 17),
+	MX21_PAD_SD1_D0		= PAD_ID(PE, 18),
+	MX21_PAD_SD1_D1		= PAD_ID(PE, 19),
+	MX21_PAD_SD1_D2		= PAD_ID(PE, 20),
+	MX21_PAD_SD1_D3		= PAD_ID(PE, 21),
+	MX21_PAD_SD1_CMD	= PAD_ID(PE, 22),
+	MX21_PAD_SD1_CLK	= PAD_ID(PE, 23),
+	MX21_PAD_NFRB		= PAD_ID(PF, 0),
+	MX21_PAD_NFCE		= PAD_ID(PF, 1),
+	MX21_PAD_NFWP		= PAD_ID(PF, 2),
+	MX21_PAD_NFCLE		= PAD_ID(PF, 3),
+	MX21_PAD_NFALE		= PAD_ID(PF, 4),
+	MX21_PAD_NFRE		= PAD_ID(PF, 5),
+	MX21_PAD_NFWE		= PAD_ID(PF, 6),
+	MX21_PAD_NFIO0		= PAD_ID(PF, 7),
+	MX21_PAD_NFIO1		= PAD_ID(PF, 8),
+	MX21_PAD_NFIO2		= PAD_ID(PF, 9),
+	MX21_PAD_NFIO3		= PAD_ID(PF, 10),
+	MX21_PAD_NFIO4		= PAD_ID(PF, 11),
+	MX21_PAD_NFIO5		= PAD_ID(PF, 12),
+	MX21_PAD_NFIO6		= PAD_ID(PF, 13),
+	MX21_PAD_NFIO7		= PAD_ID(PF, 14),
+	MX21_PAD_CLKO		= PAD_ID(PF, 15),
+	MX21_PAD_RESERVED	= PAD_ID(PF, 16),
+	MX21_PAD_CS4		= PAD_ID(PF, 21),
+	MX21_PAD_CS5		= PAD_ID(PF, 22),
+};
+
+/* Pad names for the pinmux subsystem */
+static const struct pinctrl_pin_desc imx21_pinctrl_pads[] = {
+	IMX_PINCTRL_PIN(MX21_PAD_LSCLK),
+	IMX_PINCTRL_PIN(MX21_PAD_LD0),
+	IMX_PINCTRL_PIN(MX21_PAD_LD1),
+	IMX_PINCTRL_PIN(MX21_PAD_LD2),
+	IMX_PINCTRL_PIN(MX21_PAD_LD3),
+	IMX_PINCTRL_PIN(MX21_PAD_LD4),
+	IMX_PINCTRL_PIN(MX21_PAD_LD5),
+	IMX_PINCTRL_PIN(MX21_PAD_LD6),
+	IMX_PINCTRL_PIN(MX21_PAD_LD7),
+	IMX_PINCTRL_PIN(MX21_PAD_LD8),
+	IMX_PINCTRL_PIN(MX21_PAD_LD9),
+	IMX_PINCTRL_PIN(MX21_PAD_LD10),
+	IMX_PINCTRL_PIN(MX21_PAD_LD11),
+	IMX_PINCTRL_PIN(MX21_PAD_LD12),
+	IMX_PINCTRL_PIN(MX21_PAD_LD13),
+	IMX_PINCTRL_PIN(MX21_PAD_LD14),
+	IMX_PINCTRL_PIN(MX21_PAD_LD15),
+	IMX_PINCTRL_PIN(MX21_PAD_LD16),
+	IMX_PINCTRL_PIN(MX21_PAD_LD17),
+	IMX_PINCTRL_PIN(MX21_PAD_REV),
+	IMX_PINCTRL_PIN(MX21_PAD_CLS),
+	IMX_PINCTRL_PIN(MX21_PAD_PS),
+	IMX_PINCTRL_PIN(MX21_PAD_SPL_SPR),
+	IMX_PINCTRL_PIN(MX21_PAD_HSYNC),
+	IMX_PINCTRL_PIN(MX21_PAD_VSYNC),
+	IMX_PINCTRL_PIN(MX21_PAD_CONTRAST),
+	IMX_PINCTRL_PIN(MX21_PAD_OE_ACD),
+	IMX_PINCTRL_PIN(MX21_PAD_SD2_D0),
+	IMX_PINCTRL_PIN(MX21_PAD_SD2_D1),
+	IMX_PINCTRL_PIN(MX21_PAD_SD2_D2),
+	IMX_PINCTRL_PIN(MX21_PAD_SD2_D3),
+	IMX_PINCTRL_PIN(MX21_PAD_SD2_CMD),
+	IMX_PINCTRL_PIN(MX21_PAD_SD2_CLK),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D0),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D1),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D2),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D3),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D4),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_MCLK),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_PIXCLK),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D5),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D6),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_D7),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_VSYNC),
+	IMX_PINCTRL_PIN(MX21_PAD_CSI_HSYNC),
+	IMX_PINCTRL_PIN(MX21_PAD_USB_BYP),
+	IMX_PINCTRL_PIN(MX21_PAD_USB_PWR),
+	IMX_PINCTRL_PIN(MX21_PAD_USB_OC),
+	IMX_PINCTRL_PIN(MX21_PAD_USBH_ON),
+	IMX_PINCTRL_PIN(MX21_PAD_USBH1_FS),
+	IMX_PINCTRL_PIN(MX21_PAD_USBH1_OE),
+	IMX_PINCTRL_PIN(MX21_PAD_USBH1_TXDM),
+	IMX_PINCTRL_PIN(MX21_PAD_USBH1_TXDP),
+	IMX_PINCTRL_PIN(MX21_PAD_USBH1_RXDM),
+	IMX_PINCTRL_PIN(MX21_PAD_USBH1_RXDP),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_SDA),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_SCL),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_ON),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_FS),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_OE),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_TXDM),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_TXDP),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_RXDM),
+	IMX_PINCTRL_PIN(MX21_PAD_USBG_RXDP),
+	IMX_PINCTRL_PIN(MX21_PAD_TOUT),
+	IMX_PINCTRL_PIN(MX21_PAD_TIN),
+	IMX_PINCTRL_PIN(MX21_PAD_SAP_FS),
+	IMX_PINCTRL_PIN(MX21_PAD_SAP_RXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SAP_TXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SAP_CLK),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI1_FS),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI1_RXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI1_TXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI1_CLK),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI2_FS),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI2_RXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI2_TXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI2_CLK),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI3_FS),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI3_RXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI3_TXD),
+	IMX_PINCTRL_PIN(MX21_PAD_SSI3_CLK),
+	IMX_PINCTRL_PIN(MX21_PAD_I2C_DATA),
+	IMX_PINCTRL_PIN(MX21_PAD_I2C_CLK),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI2_SS2),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI2_SS1),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI2_SS0),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI2_SCLK),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI2_MISO),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI2_MOSI),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI1_RDY),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI1_SS2),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI1_SS1),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI1_SS0),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI1_SCLK),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI1_MISO),
+	IMX_PINCTRL_PIN(MX21_PAD_CSPI1_MOSI),
+	IMX_PINCTRL_PIN(MX21_PAD_TEST_WB2),
+	IMX_PINCTRL_PIN(MX21_PAD_TEST_WB1),
+	IMX_PINCTRL_PIN(MX21_PAD_TEST_WB0),
+	IMX_PINCTRL_PIN(MX21_PAD_UART2_CTS),
+	IMX_PINCTRL_PIN(MX21_PAD_UART2_RTS),
+	IMX_PINCTRL_PIN(MX21_PAD_PWMO),
+	IMX_PINCTRL_PIN(MX21_PAD_UART2_TXD),
+	IMX_PINCTRL_PIN(MX21_PAD_UART2_RXD),
+	IMX_PINCTRL_PIN(MX21_PAD_UART3_TXD),
+	IMX_PINCTRL_PIN(MX21_PAD_UART3_RXD),
+	IMX_PINCTRL_PIN(MX21_PAD_UART3_CTS),
+	IMX_PINCTRL_PIN(MX21_PAD_UART3_RTS),
+	IMX_PINCTRL_PIN(MX21_PAD_UART1_TXD),
+	IMX_PINCTRL_PIN(MX21_PAD_UART1_RXD),
+	IMX_PINCTRL_PIN(MX21_PAD_UART1_CTS),
+	IMX_PINCTRL_PIN(MX21_PAD_UART1_RTS),
+	IMX_PINCTRL_PIN(MX21_PAD_RTCK),
+	IMX_PINCTRL_PIN(MX21_PAD_RESET_OUT),
+	IMX_PINCTRL_PIN(MX21_PAD_SD1_D0),
+	IMX_PINCTRL_PIN(MX21_PAD_SD1_D1),
+	IMX_PINCTRL_PIN(MX21_PAD_SD1_D2),
+	IMX_PINCTRL_PIN(MX21_PAD_SD1_D3),
+	IMX_PINCTRL_PIN(MX21_PAD_SD1_CMD),
+	IMX_PINCTRL_PIN(MX21_PAD_SD1_CLK),
+	IMX_PINCTRL_PIN(MX21_PAD_NFRB),
+	IMX_PINCTRL_PIN(MX21_PAD_NFCE),
+	IMX_PINCTRL_PIN(MX21_PAD_NFWP),
+	IMX_PINCTRL_PIN(MX21_PAD_NFCLE),
+	IMX_PINCTRL_PIN(MX21_PAD_NFALE),
+	IMX_PINCTRL_PIN(MX21_PAD_NFRE),
+	IMX_PINCTRL_PIN(MX21_PAD_NFWE),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO0),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO1),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO2),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO3),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO4),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO5),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO6),
+	IMX_PINCTRL_PIN(MX21_PAD_NFIO7),
+	IMX_PINCTRL_PIN(MX21_PAD_CLKO),
+	IMX_PINCTRL_PIN(MX21_PAD_RESERVED),
+	IMX_PINCTRL_PIN(MX21_PAD_CS4),
+	IMX_PINCTRL_PIN(MX21_PAD_CS5),
+};
+
+static struct imx1_pinctrl_soc_info imx21_pinctrl_info = {
+	.pins	= imx21_pinctrl_pads,
+	.npins	= ARRAY_SIZE(imx21_pinctrl_pads),
+};
+
+static int __init imx21_pinctrl_probe(struct platform_device *pdev)
+{
+	return imx1_pinctrl_core_probe(pdev, &imx21_pinctrl_info);
+}
+
+static const struct of_device_id imx21_pinctrl_of_match[] = {
+	{ .compatible = "fsl,imx21-iomuxc", },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, imx21_pinctrl_of_match);
+
+static struct platform_driver imx21_pinctrl_driver = {
+	.driver	= {
+		.name		= "imx21-pinctrl",
+		.owner		= THIS_MODULE,
+		.of_match_table	= imx21_pinctrl_of_match,
+	},
+	.remove	= imx1_pinctrl_core_remove,
+};
+module_platform_driver_probe(imx21_pinctrl_driver, imx21_pinctrl_probe);
+
+MODULE_AUTHOR("Alexander Shiyan <shc_work@mail.ru>");
+MODULE_DESCRIPTION("Freescale i.MX21 pinctrl driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/pinctrl-imx23.c b/drivers/pinctrl/freescale/pinctrl-imx23.c
index e76d75c9d1ba..df79096becb0 100644
--- a/drivers/pinctrl/pinctrl-imx23.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx23.c
@@ -272,7 +272,7 @@ static int imx23_pinctrl_probe(struct platform_device *pdev)
 	return mxs_pinctrl_probe(pdev, &imx23_pinctrl_data);
 }
 
-static struct of_device_id imx23_pinctrl_of_match[] = {
+static const struct of_device_id imx23_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx23-pinctrl", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx25.c b/drivers/pinctrl/freescale/pinctrl-imx25.c
index 1aae1b61c4dc..550e6d77ac2b 100644
--- a/drivers/pinctrl/pinctrl-imx25.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx25.c
@@ -315,7 +315,7 @@ static struct imx_pinctrl_soc_info imx25_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx25_pinctrl_pads),
 };
 
-static struct of_device_id imx25_pinctrl_of_match[] = {
+static const struct of_device_id imx25_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx25-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx27.c b/drivers/pinctrl/freescale/pinctrl-imx27.c
index f8dfefb69968..945eccadea74 100644
--- a/drivers/pinctrl/pinctrl-imx27.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx27.c
@@ -389,7 +389,7 @@ static struct imx1_pinctrl_soc_info imx27_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx27_pinctrl_pads),
 };
 
-static struct of_device_id imx27_pinctrl_of_match[] = {
+static const struct of_device_id imx27_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx27-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx28.c b/drivers/pinctrl/freescale/pinctrl-imx28.c
index 79c9c8d296af..3bd45da21229 100644
--- a/drivers/pinctrl/pinctrl-imx28.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx28.c
@@ -388,7 +388,7 @@ static int imx28_pinctrl_probe(struct platform_device *pdev)
 	return mxs_pinctrl_probe(pdev, &imx28_pinctrl_data);
 }
 
-static struct of_device_id imx28_pinctrl_of_match[] = {
+static const struct of_device_id imx28_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx28-pinctrl", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx35.c b/drivers/pinctrl/freescale/pinctrl-imx35.c
index 278a04ae8940..6bfbcd0112c1 100644
--- a/drivers/pinctrl/pinctrl-imx35.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx35.c
@@ -1005,7 +1005,7 @@ static struct imx_pinctrl_soc_info imx35_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx35_pinctrl_pads),
 };
 
-static struct of_device_id imx35_pinctrl_of_match[] = {
+static const struct of_device_id imx35_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx35-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx50.c b/drivers/pinctrl/freescale/pinctrl-imx50.c
index b06feed1b038..e8bd604ab147 100644
--- a/drivers/pinctrl/pinctrl-imx50.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx50.c
@@ -391,7 +391,7 @@ static struct imx_pinctrl_soc_info imx50_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx50_pinctrl_pads),
 };
 
-static struct of_device_id imx50_pinctrl_of_match[] = {
+static const struct of_device_id imx50_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx50-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx51.c b/drivers/pinctrl/freescale/pinctrl-imx51.c
index 19ab182bef61..b818051db7c9 100644
--- a/drivers/pinctrl/pinctrl-imx51.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx51.c
@@ -768,7 +768,7 @@ static struct imx_pinctrl_soc_info imx51_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx51_pinctrl_pads),
 };
 
-static struct of_device_id imx51_pinctrl_of_match[] = {
+static const struct of_device_id imx51_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx51-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx53.c b/drivers/pinctrl/freescale/pinctrl-imx53.c
index f8d45c4cfde7..1884d53cf750 100644
--- a/drivers/pinctrl/pinctrl-imx53.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx53.c
@@ -454,7 +454,7 @@ static struct imx_pinctrl_soc_info imx53_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx53_pinctrl_pads),
 };
 
-static struct of_device_id imx53_pinctrl_of_match[] = {
+static const struct of_device_id imx53_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx53-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx6dl.c b/drivers/pinctrl/freescale/pinctrl-imx6dl.c
index db2a1489bd99..656c4b08cc2e 100644
--- a/drivers/pinctrl/pinctrl-imx6dl.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6dl.c
@@ -460,7 +460,7 @@ static struct imx_pinctrl_soc_info imx6dl_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx6dl_pinctrl_pads),
 };
 
-static struct of_device_id imx6dl_pinctrl_of_match[] = {
+static const struct of_device_id imx6dl_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx6dl-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx6q.c b/drivers/pinctrl/freescale/pinctrl-imx6q.c
index 8eb5ac1bd5f6..59bb5b4ec0f6 100644
--- a/drivers/pinctrl/pinctrl-imx6q.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6q.c
@@ -466,7 +466,7 @@ static struct imx_pinctrl_soc_info imx6q_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx6q_pinctrl_pads),
 };
 
-static struct of_device_id imx6q_pinctrl_of_match[] = {
+static const struct of_device_id imx6q_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx6q-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-imx6sl.c b/drivers/pinctrl/freescale/pinctrl-imx6sl.c
index f21b7389df3c..e0924bd7b98c 100644
--- a/drivers/pinctrl/pinctrl-imx6sl.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6sl.c
@@ -366,10 +366,11 @@ static struct imx_pinctrl_soc_info imx6sl_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx6sl_pinctrl_pads),
 };
 
-static struct of_device_id imx6sl_pinctrl_of_match[] = {
+static const struct of_device_id imx6sl_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx6sl-iomuxc", },
 	{ /* sentinel */ }
 };
+MODULE_DEVICE_TABLE(of, imx6sl_pinctrl_of_match);
 
 static int imx6sl_pinctrl_probe(struct platform_device *pdev)
 {
diff --git a/drivers/pinctrl/pinctrl-imx6sx.c b/drivers/pinctrl/freescale/pinctrl-imx6sx.c
index 09758a56b9df..840344c8580d 100644
--- a/drivers/pinctrl/pinctrl-imx6sx.c
+++ b/drivers/pinctrl/freescale/pinctrl-imx6sx.c
@@ -370,7 +370,7 @@ static struct imx_pinctrl_soc_info imx6sx_pinctrl_info = {
 	.npins = ARRAY_SIZE(imx6sx_pinctrl_pads),
 };
 
-static struct of_device_id imx6sx_pinctrl_of_match[] = {
+static const struct of_device_id imx6sx_pinctrl_of_match[] = {
 	{ .compatible = "fsl,imx6sx-iomuxc", },
 	{ /* sentinel */ }
 };
diff --git a/drivers/pinctrl/pinctrl-mxs.c b/drivers/pinctrl/freescale/pinctrl-mxs.c
index 40c76f26998c..f98c6bb0f769 100644
--- a/drivers/pinctrl/pinctrl-mxs.c
+++ b/drivers/pinctrl/freescale/pinctrl-mxs.c
@@ -21,7 +21,7 @@
 #include <linux/pinctrl/pinmux.h>
 #include <linux/platform_device.h>
 #include <linux/slab.h>
-#include "core.h"
+#include "../core.h"
 #include "pinctrl-mxs.h"
 
 #define SUFFIX_LEN	4
@@ -195,8 +195,8 @@ static int mxs_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int mxs_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned selector,
-			      unsigned group)
+static int mxs_pinctrl_set_mux(struct pinctrl_dev *pctldev, unsigned selector,
+			       unsigned group)
 {
 	struct mxs_pinctrl_data *d = pinctrl_dev_get_drvdata(pctldev);
 	struct mxs_group *g = &d->soc->groups[group];
@@ -223,7 +223,7 @@ static const struct pinmux_ops mxs_pinmux_ops = {
 	.get_functions_count = mxs_pinctrl_get_funcs_count,
 	.get_function_name = mxs_pinctrl_get_func_name,
 	.get_function_groups = mxs_pinctrl_get_func_groups,
-	.enable = mxs_pinctrl_enable,
+	.set_mux = mxs_pinctrl_set_mux,
 };
 
 static int mxs_pinconf_get(struct pinctrl_dev *pctldev,
diff --git a/drivers/pinctrl/pinctrl-mxs.h b/drivers/pinctrl/freescale/pinctrl-mxs.h
index fdd88d0bae22..fdd88d0bae22 100644
--- a/drivers/pinctrl/pinctrl-mxs.h
+++ b/drivers/pinctrl/freescale/pinctrl-mxs.h
diff --git a/drivers/pinctrl/pinctrl-vf610.c b/drivers/pinctrl/freescale/pinctrl-vf610.c
index bddd913d28ba..b788e1578954 100644
--- a/drivers/pinctrl/pinctrl-vf610.c
+++ b/drivers/pinctrl/freescale/pinctrl-vf610.c
@@ -299,7 +299,7 @@ static const struct pinctrl_pin_desc vf610_pinctrl_pads[] = {
 static struct imx_pinctrl_soc_info vf610_pinctrl_info = {
 	.pins = vf610_pinctrl_pads,
 	.npins = ARRAY_SIZE(vf610_pinctrl_pads),
-	.flags = ZERO_OFFSET_VALID | SHARE_MUX_CONF_REG,
+	.flags = SHARE_MUX_CONF_REG,
 };
 
 static struct of_device_id vf610_pinctrl_of_match[] = {
diff --git a/drivers/pinctrl/mvebu/pinctrl-mvebu.c b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
index 9908374f8f92..f3b426cdaf8f 100644
--- a/drivers/pinctrl/mvebu/pinctrl-mvebu.c
+++ b/drivers/pinctrl/mvebu/pinctrl-mvebu.c
@@ -259,8 +259,8 @@ static int mvebu_pinmux_get_groups(struct pinctrl_dev *pctldev, unsigned fid,
 	return 0;
 }
 
-static int mvebu_pinmux_enable(struct pinctrl_dev *pctldev, unsigned fid,
-			unsigned gid)
+static int mvebu_pinmux_set(struct pinctrl_dev *pctldev, unsigned fid,
+			    unsigned gid)
 {
 	struct mvebu_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
 	struct mvebu_pinctrl_function *func = &pctl->functions[fid];
@@ -344,7 +344,7 @@ static const struct pinmux_ops mvebu_pinmux_ops = {
 	.get_function_groups = mvebu_pinmux_get_groups,
 	.gpio_request_enable = mvebu_pinmux_gpio_request_enable,
 	.gpio_set_direction = mvebu_pinmux_gpio_set_direction,
-	.enable = mvebu_pinmux_enable,
+	.set_mux = mvebu_pinmux_set,
 };
 
 static int mvebu_pinctrl_get_groups_count(struct pinctrl_dev *pctldev)
diff --git a/drivers/pinctrl/nomadik/pinctrl-abx500.c b/drivers/pinctrl/nomadik/pinctrl-abx500.c
index a53a689a2bfa..228972827132 100644
--- a/drivers/pinctrl/nomadik/pinctrl-abx500.c
+++ b/drivers/pinctrl/nomadik/pinctrl-abx500.c
@@ -34,6 +34,7 @@
 #include "pinctrl-abx500.h"
 #include "../core.h"
 #include "../pinconf.h"
+#include "../pinctrl-utils.h"
 
 /*
  * The AB9540 and AB8540 GPIO support are extended versions
@@ -620,8 +621,7 @@ static void abx500_gpio_dbg_show_one(struct seq_file *s,
 	} else
 		seq_printf(s, " %-9s", chip->get(chip, offset) ? "hi" : "lo");
 
-	if (pctldev)
-		mode = abx500_get_mode(pctldev, chip, offset);
+	mode = abx500_get_mode(pctldev, chip, offset);
 
 	seq_printf(s, " %s", (mode < 0) ? "unknown" : modes[mode]);
 
@@ -709,8 +709,8 @@ static int abx500_pmx_get_func_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int abx500_pmx_enable(struct pinctrl_dev *pctldev, unsigned function,
-			     unsigned group)
+static int abx500_pmx_set(struct pinctrl_dev *pctldev, unsigned function,
+			  unsigned group)
 {
 	struct abx500_pinctrl *pct = pinctrl_dev_get_drvdata(pctldev);
 	struct gpio_chip *chip = &pct->chip;
@@ -784,7 +784,7 @@ static const struct pinmux_ops abx500_pinmux_ops = {
 	.get_functions_count = abx500_pmx_get_funcs_cnt,
 	.get_function_name = abx500_pmx_get_func_name,
 	.get_function_groups = abx500_pmx_get_func_groups,
-	.enable = abx500_pmx_enable,
+	.set_mux = abx500_pmx_set,
 	.gpio_request_enable = abx500_gpio_request_enable,
 	.gpio_disable_free = abx500_gpio_disable_free,
 };
@@ -827,41 +827,6 @@ static void abx500_pin_dbg_show(struct pinctrl_dev *pctldev,
 				 chip->base + offset - 1);
 }
 
-static void abx500_dt_free_map(struct pinctrl_dev *pctldev,
-		struct pinctrl_map *map, unsigned num_maps)
-{
-	int i;
-
-	for (i = 0; i < num_maps; i++)
-		if (map[i].type == PIN_MAP_TYPE_CONFIGS_PIN)
-			kfree(map[i].data.configs.configs);
-	kfree(map);
-}
-
-static int abx500_dt_reserve_map(struct pinctrl_map **map,
-		unsigned *reserved_maps,
-		unsigned *num_maps,
-		unsigned reserve)
-{
-	unsigned old_num = *reserved_maps;
-	unsigned new_num = *num_maps + reserve;
-	struct pinctrl_map *new_map;
-
-	if (old_num >= new_num)
-		return 0;
-
-	new_map = krealloc(*map, sizeof(*new_map) * new_num, GFP_KERNEL);
-	if (!new_map)
-		return -ENOMEM;
-
-	memset(new_map + old_num, 0, (new_num - old_num) * sizeof(*new_map));
-
-	*map = new_map;
-	*reserved_maps = new_num;
-
-	return 0;
-}
-
 static int abx500_dt_add_map_mux(struct pinctrl_map **map,
 		unsigned *reserved_maps,
 		unsigned *num_maps, const char *group,
@@ -927,19 +892,32 @@ static int abx500_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 	unsigned long *configs;
 	unsigned int nconfigs = 0;
 	bool has_config = 0;
-	unsigned reserve = 0;
 	struct property *prop;
 	const char *group, *gpio_name;
 	struct device_node *np_config;
 
 	ret = of_property_read_string(np, "ste,function", &function);
-	if (ret >= 0)
-		reserve = 1;
+	if (ret >= 0) {
+		ret = of_property_count_strings(np, "ste,pins");
+		if (ret < 0)
+			goto exit;
+
+		ret = pinctrl_utils_reserve_map(pctldev, map, reserved_maps,
+						num_maps, ret);
+		if (ret < 0)
+			goto exit;
+
+		of_property_for_each_string(np, "ste,pins", prop, group) {
+			ret = abx500_dt_add_map_mux(map, reserved_maps,
+					num_maps, group, function);
+			if (ret < 0)
+				goto exit;
+		}
+	}
 
 	ret = pinconf_generic_parse_dt_config(np, &configs, &nconfigs);
 	if (nconfigs)
 		has_config = 1;
-
 	np_config = of_parse_phandle(np, "ste,config", 0);
 	if (np_config) {
 		ret = pinconf_generic_parse_dt_config(np_config, &configs,
@@ -948,28 +926,18 @@ static int abx500_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 			goto exit;
 		has_config |= nconfigs;
 	}
+	if (has_config) {
+		ret = of_property_count_strings(np, "ste,pins");
+		if (ret < 0)
+			goto exit;
 
-	ret = of_property_count_strings(np, "ste,pins");
-	if (ret < 0)
-		goto exit;
-
-	if (has_config)
-		reserve++;
-
-	reserve *= ret;
-
-	ret = abx500_dt_reserve_map(map, reserved_maps, num_maps, reserve);
-	if (ret < 0)
-		goto exit;
+		ret = pinctrl_utils_reserve_map(pctldev, map,
+						reserved_maps,
+						num_maps, ret);
+		if (ret < 0)
+			goto exit;
 
-	of_property_for_each_string(np, "ste,pins", prop, group) {
-		if (function) {
-			ret = abx500_dt_add_map_mux(map, reserved_maps,
-					num_maps, group, function);
-			if (ret < 0)
-				goto exit;
-		}
-		if (has_config) {
+		of_property_for_each_string(np, "ste,pins", prop, group) {
 			gpio_name = abx500_find_pin_name(pctldev, group);
 
 			ret = abx500_dt_add_map_configs(map, reserved_maps,
@@ -977,8 +945,8 @@ static int abx500_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 			if (ret < 0)
 				goto exit;
 		}
-
 	}
+
 exit:
 	return ret;
 }
@@ -999,7 +967,7 @@ static int abx500_dt_node_to_map(struct pinctrl_dev *pctldev,
 		ret = abx500_dt_subnode_to_map(pctldev, np, map,
 				&reserved_maps, num_maps);
 		if (ret < 0) {
-			abx500_dt_free_map(pctldev, *map, *num_maps);
+			pinctrl_utils_dt_free_map(pctldev, *map, *num_maps);
 			return ret;
 		}
 	}
@@ -1013,7 +981,7 @@ static const struct pinctrl_ops abx500_pinctrl_ops = {
 	.get_group_pins = abx500_get_group_pins,
 	.pin_dbg_show = abx500_pin_dbg_show,
 	.dt_node_to_map = abx500_dt_node_to_map,
-	.dt_free_map = abx500_dt_free_map,
+	.dt_free_map = pinctrl_utils_dt_free_map,
 };
 
 static int abx500_pin_config_get(struct pinctrl_dev *pctldev,
@@ -1206,7 +1174,7 @@ static int abx500_gpio_probe(struct platform_device *pdev)
 	const struct of_device_id *match;
 	struct abx500_pinctrl *pct;
 	unsigned int id = -1;
-	int ret, err;
+	int ret;
 	int i;
 
 	if (!np) {
@@ -1298,10 +1266,7 @@ static int abx500_gpio_probe(struct platform_device *pdev)
 	return 0;
 
 out_rem_chip:
-	err = gpiochip_remove(&pct->chip);
-	if (err)
-		dev_info(&pdev->dev, "failed to remove gpiochip\n");
-
+	gpiochip_remove(&pct->chip);
 	return ret;
 }
 
@@ -1312,15 +1277,8 @@ out_rem_chip:
 static int abx500_gpio_remove(struct platform_device *pdev)
 {
 	struct abx500_pinctrl *pct = platform_get_drvdata(pdev);
-	int ret;
-
-	ret = gpiochip_remove(&pct->chip);
-	if (ret < 0) {
-		dev_err(pct->dev, "unable to remove gpiochip: %d\n",
-			ret);
-		return ret;
-	}
 
+	gpiochip_remove(&pct->chip);
 	return 0;
 }
 
diff --git a/drivers/pinctrl/nomadik/pinctrl-nomadik.c b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
index e7cab07eef47..746db6acf648 100644
--- a/drivers/pinctrl/nomadik/pinctrl-nomadik.c
+++ b/drivers/pinctrl/nomadik/pinctrl-nomadik.c
@@ -32,6 +32,7 @@
 #include <linux/pinctrl/consumer.h>
 #include "pinctrl-nomadik.h"
 #include "../core.h"
+#include "../pinctrl-utils.h"
 
 /*
  * The GPIO module in the Nomadik family of Systems-on-Chip is an
@@ -985,6 +986,7 @@ static void nmk_gpio_dbg_show_one(struct seq_file *s,
 		container_of(chip, struct nmk_gpio_chip, chip);
 	int mode;
 	bool is_out;
+	bool data_out;
 	bool pull;
 	u32 bit = 1 << offset;
 	const char *modes[] = {
@@ -997,28 +999,41 @@ static void nmk_gpio_dbg_show_one(struct seq_file *s,
 		[NMK_GPIO_ALT_C+3]	= "altC3",
 		[NMK_GPIO_ALT_C+4]	= "altC4",
 	};
+	const char *pulls[] = {
+		"none     ",
+		"pull down",
+		"pull up  ",
+	};
 
 	clk_enable(nmk_chip->clk);
 	is_out = !!(readl(nmk_chip->addr + NMK_GPIO_DIR) & bit);
 	pull = !(readl(nmk_chip->addr + NMK_GPIO_PDIS) & bit);
+	data_out = !!(readl(nmk_chip->addr + NMK_GPIO_DAT) & bit);
 	mode = nmk_gpio_get_mode(gpio);
 	if ((mode == NMK_GPIO_ALT_C) && pctldev)
 		mode = nmk_prcm_gpiocr_get_mode(pctldev, gpio);
 
-	seq_printf(s, " gpio-%-3d (%-20.20s) %s %s %s %s",
-		   gpio, label ?: "(none)",
-		   is_out ? "out" : "in ",
-		   chip->get
-		   ? (chip->get(chip, offset) ? "hi" : "lo")
-		   : "?  ",
-		   (mode < 0) ? "unknown" : modes[mode],
-		   pull ? "pull" : "none");
-
-	if (!is_out) {
+	if (is_out) {
+		seq_printf(s, " gpio-%-3d (%-20.20s) out %s        %s",
+			   gpio,
+			   label ?: "(none)",
+			   data_out ? "hi" : "lo",
+			   (mode < 0) ? "unknown" : modes[mode]);
+	} else {
 		int irq = gpio_to_irq(gpio);
 		struct irq_desc	*desc = irq_to_desc(irq);
+		int pullidx = 0;
 
-		/* This races with request_irq(), set_irq_type(),
+		if (pull)
+			pullidx = data_out ? 1 : 2;
+
+		seq_printf(s, " gpio-%-3d (%-20.20s) in  %s %s",
+			   gpio,
+			   label ?: "(none)",
+			   pulls[pullidx],
+			   (mode < 0) ? "unknown" : modes[mode]);
+		/*
+		 * This races with request_irq(), set_irq_type(),
 		 * and set_irq_wake() ... but those are "rare".
 		 */
 		if (irq > 0 && desc && desc->action) {
@@ -1261,7 +1276,7 @@ static int nmk_gpio_probe(struct platform_device *dev)
 				   IRQ_TYPE_EDGE_FALLING);
 	if (ret) {
 		dev_err(&dev->dev, "could not add irqchip\n");
-		ret = gpiochip_remove(&nmk_chip->chip);
+		gpiochip_remove(&nmk_chip->chip);
 		return -ENODEV;
 	}
 	/* Then register the chain on the parent IRQ */
@@ -1338,39 +1353,6 @@ static void nmk_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
 	nmk_gpio_dbg_show_one(s, pctldev, chip, offset - chip->base, offset);
 }
 
-static void nmk_pinctrl_dt_free_map(struct pinctrl_dev *pctldev,
-		struct pinctrl_map *map, unsigned num_maps)
-{
-	int i;
-
-	for (i = 0; i < num_maps; i++)
-		if (map[i].type == PIN_MAP_TYPE_CONFIGS_PIN)
-			kfree(map[i].data.configs.configs);
-	kfree(map);
-}
-
-static int nmk_dt_reserve_map(struct pinctrl_map **map, unsigned *reserved_maps,
-		unsigned *num_maps, unsigned reserve)
-{
-	unsigned old_num = *reserved_maps;
-	unsigned new_num = *num_maps + reserve;
-	struct pinctrl_map *new_map;
-
-	if (old_num >= new_num)
-		return 0;
-
-	new_map = krealloc(*map, sizeof(*new_map) * new_num, GFP_KERNEL);
-	if (!new_map)
-		return -ENOMEM;
-
-	memset(new_map + old_num, 0, (new_num - old_num) * sizeof(*new_map));
-
-	*map = new_map;
-	*reserved_maps = new_num;
-
-	return 0;
-}
-
 static int nmk_dt_add_map_mux(struct pinctrl_map **map, unsigned *reserved_maps,
 		unsigned *num_maps, const char *group,
 		const char *function)
@@ -1537,51 +1519,55 @@ static int nmk_pinctrl_dt_subnode_to_map(struct pinctrl_dev *pctldev,
 	const char *function = NULL;
 	unsigned long configs = 0;
 	bool has_config = 0;
-	unsigned reserve = 0;
 	struct property *prop;
 	const char *group, *gpio_name;
 	struct device_node *np_config;
 
 	ret = of_property_read_string(np, "ste,function", &function);
-	if (ret >= 0)
-		reserve = 1;
-
-	has_config = nmk_pinctrl_dt_get_config(np, &configs);
-
-	np_config = of_parse_phandle(np, "ste,config", 0);
-	if (np_config)
-		has_config |= nmk_pinctrl_dt_get_config(np_config, &configs);
-
-	ret = of_property_count_strings(np, "ste,pins");
-	if (ret < 0)
-		goto exit;
-
-	if (has_config)
-		reserve++;
-
-	reserve *= ret;
-
-	ret = nmk_dt_reserve_map(map, reserved_maps, num_maps, reserve);
-	if (ret < 0)
-		goto exit;
-
-	of_property_for_each_string(np, "ste,pins", prop, group) {
-		if (function) {
+	if (ret >= 0) {
+		ret = of_property_count_strings(np, "ste,pins");
+		if (ret < 0)
+			goto exit;
+
+		ret = pinctrl_utils_reserve_map(pctldev, map,
+						reserved_maps,
+						num_maps, ret);
+		if (ret < 0)
+			goto exit;
+
+		of_property_for_each_string(np, "ste,pins", prop, group) {
 			ret = nmk_dt_add_map_mux(map, reserved_maps, num_maps,
 					  group, function);
 			if (ret < 0)
 				goto exit;
 		}
-		if (has_config) {
+	}
+
+	has_config = nmk_pinctrl_dt_get_config(np, &configs);
+	np_config = of_parse_phandle(np, "ste,config", 0);
+	if (np_config)
+		has_config |= nmk_pinctrl_dt_get_config(np_config, &configs);
+	if (has_config) {
+		ret = of_property_count_strings(np, "ste,pins");
+		if (ret < 0)
+			goto exit;
+		ret = pinctrl_utils_reserve_map(pctldev, map,
+						reserved_maps,
+						num_maps, ret);
+		if (ret < 0)
+			goto exit;
+
+		of_property_for_each_string(np, "ste,pins", prop, group) {
 			gpio_name = nmk_find_pin_name(pctldev, group);
 
-			ret = nmk_dt_add_map_configs(map, reserved_maps, num_maps,
-					      gpio_name, &configs, 1);
+			ret = nmk_dt_add_map_configs(map, reserved_maps,
+						     num_maps,
+						     gpio_name, &configs, 1);
 			if (ret < 0)
 				goto exit;
 		}
-
 	}
+
 exit:
 	return ret;
 }
@@ -1602,7 +1588,7 @@ static int nmk_pinctrl_dt_node_to_map(struct pinctrl_dev *pctldev,
 		ret = nmk_pinctrl_dt_subnode_to_map(pctldev, np, map,
 				&reserved_maps, num_maps);
 		if (ret < 0) {
-			nmk_pinctrl_dt_free_map(pctldev, *map, *num_maps);
+			pinctrl_utils_dt_free_map(pctldev, *map, *num_maps);
 			return ret;
 		}
 	}
@@ -1616,7 +1602,7 @@ static const struct pinctrl_ops nmk_pinctrl_ops = {
 	.get_group_pins = nmk_get_group_pins,
 	.pin_dbg_show = nmk_pin_dbg_show,
 	.dt_node_to_map = nmk_pinctrl_dt_node_to_map,
-	.dt_free_map = nmk_pinctrl_dt_free_map,
+	.dt_free_map = pinctrl_utils_dt_free_map,
 };
 
 static int nmk_pmx_get_funcs_cnt(struct pinctrl_dev *pctldev)
@@ -1647,8 +1633,8 @@ static int nmk_pmx_get_func_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int nmk_pmx_enable(struct pinctrl_dev *pctldev, unsigned function,
-			  unsigned group)
+static int nmk_pmx_set(struct pinctrl_dev *pctldev, unsigned function,
+		       unsigned group)
 {
 	struct nmk_pinctrl *npct = pinctrl_dev_get_drvdata(pctldev);
 	const struct nmk_pingroup *g;
@@ -1810,7 +1796,7 @@ static const struct pinmux_ops nmk_pinmux_ops = {
 	.get_functions_count = nmk_pmx_get_funcs_cnt,
 	.get_function_name = nmk_pmx_get_func_name,
 	.get_function_groups = nmk_pmx_get_func_groups,
-	.enable = nmk_pmx_enable,
+	.set_mux = nmk_pmx_set,
 	.gpio_request_enable = nmk_gpio_request_enable,
 	.gpio_disable_free = nmk_gpio_disable_free,
 };
diff --git a/drivers/pinctrl/pinctrl-adi2.c b/drivers/pinctrl/pinctrl-adi2.c
index b092b93c67a1..8434439c5017 100644
--- a/drivers/pinctrl/pinctrl-adi2.c
+++ b/drivers/pinctrl/pinctrl-adi2.c
@@ -619,8 +619,8 @@ static struct pinctrl_ops adi_pctrl_ops = {
 	.get_group_pins = adi_get_group_pins,
 };
 
-static int adi_pinmux_enable(struct pinctrl_dev *pctldev, unsigned func_id,
-	unsigned group_id)
+static int adi_pinmux_set(struct pinctrl_dev *pctldev, unsigned func_id,
+			  unsigned group_id)
 {
 	struct adi_pinctrl *pinctrl = pinctrl_dev_get_drvdata(pctldev);
 	struct gpio_port *port;
@@ -698,7 +698,7 @@ static int adi_pinmux_request_gpio(struct pinctrl_dev *pctldev,
 }
 
 static struct pinmux_ops adi_pinmux_ops = {
-	.enable = adi_pinmux_enable,
+	.set_mux = adi_pinmux_set,
 	.get_functions_count = adi_pinmux_get_funcs_count,
 	.get_function_name = adi_pinmux_get_func_name,
 	.get_function_groups = adi_pinmux_get_groups,
@@ -1041,7 +1041,6 @@ static int adi_gpio_remove(struct platform_device *pdev)
 	u8 offset;
 
 	list_del(&port->node);
-	gpiochip_remove_pin_ranges(&port->chip);
 	gpiochip_remove(&port->chip);
 	if (port->pint) {
 		for (offset = 0; offset < port->width; offset++)
diff --git a/drivers/pinctrl/pinctrl-as3722.c b/drivers/pinctrl/pinctrl-as3722.c
index 0e4ec91f4d49..1f790a4b83fe 100644
--- a/drivers/pinctrl/pinctrl-as3722.c
+++ b/drivers/pinctrl/pinctrl-as3722.c
@@ -230,7 +230,7 @@ static int as3722_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int as3722_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned function,
+static int as3722_pinctrl_set(struct pinctrl_dev *pctldev, unsigned function,
 		unsigned group)
 {
 	struct as3722_pctrl_info *as_pci = pinctrl_dev_get_drvdata(pctldev);
@@ -327,7 +327,7 @@ static const struct pinmux_ops as3722_pinmux_ops = {
 	.get_functions_count	= as3722_pinctrl_get_funcs_count,
 	.get_function_name	= as3722_pinctrl_get_func_name,
 	.get_function_groups	= as3722_pinctrl_get_func_groups,
-	.enable			= as3722_pinctrl_enable,
+	.set_mux		= as3722_pinctrl_set,
 	.gpio_request_enable	= as3722_pinctrl_gpio_request_enable,
 	.gpio_set_direction	= as3722_pinctrl_gpio_set_direction,
 };
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index af1ba4fc150d..354a81d40925 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -58,11 +58,28 @@ static int gpio_banks;
 #define DEGLITCH	(1 << 2)
 #define PULL_DOWN	(1 << 3)
 #define DIS_SCHMIT	(1 << 4)
+#define DRIVE_STRENGTH_SHIFT	5
+#define DRIVE_STRENGTH_MASK		0x3
+#define DRIVE_STRENGTH   (DRIVE_STRENGTH_MASK << DRIVE_STRENGTH_SHIFT)
 #define DEBOUNCE	(1 << 16)
 #define DEBOUNCE_VAL_SHIFT	17
 #define DEBOUNCE_VAL	(0x3fff << DEBOUNCE_VAL_SHIFT)
 
 /**
+ * These defines will translated the dt binding settings to our internal
+ * settings. They are not necessarily the same value as the register setting.
+ * The actual drive strength current of low, medium and high must be looked up
+ * from the corresponding device datasheet. This value is different for pins
+ * that are even in the same banks. It is also dependent on VCC.
+ * DRIVE_STRENGTH_DEFAULT is just a placeholder to avoid changing the drive
+ * strength when there is no dt config for it.
+ */
+#define DRIVE_STRENGTH_DEFAULT		(0 << DRIVE_STRENGTH_SHIFT)
+#define DRIVE_STRENGTH_LOW          (1 << DRIVE_STRENGTH_SHIFT)
+#define DRIVE_STRENGTH_MED          (2 << DRIVE_STRENGTH_SHIFT)
+#define DRIVE_STRENGTH_HI           (3 << DRIVE_STRENGTH_SHIFT)
+
+/**
  * struct at91_pmx_func - describes AT91 pinmux functions
  * @name: the name of this specific function
  * @groups: corresponding pin groups
@@ -148,6 +165,9 @@ struct at91_pinctrl_mux_ops {
 	void (*set_pulldown)(void __iomem *pio, unsigned mask, bool is_on);
 	bool (*get_schmitt_trig)(void __iomem *pio, unsigned pin);
 	void (*disable_schmitt_trig)(void __iomem *pio, unsigned mask);
+	unsigned (*get_drivestrength)(void __iomem *pio, unsigned pin);
+	void (*set_drivestrength)(void __iomem *pio, unsigned pin,
+					u32 strength);
 	/* irq */
 	int (*irq_type)(struct irq_data *d, unsigned type);
 };
@@ -315,6 +335,30 @@ static unsigned pin_to_mask(unsigned int pin)
 	return 1 << pin;
 }
 
+static unsigned two_bit_pin_value_shift_amount(unsigned int pin)
+{
+	/* return the shift value for a pin for "two bit" per pin registers,
+	 * i.e. drive strength */
+	return 2*((pin >= MAX_NB_GPIO_PER_BANK/2)
+			? pin - MAX_NB_GPIO_PER_BANK/2 : pin);
+}
+
+static unsigned sama5d3_get_drive_register(unsigned int pin)
+{
+	/* drive strength is split between two registers
+	 * with two bits per pin */
+	return (pin >= MAX_NB_GPIO_PER_BANK/2)
+			? SAMA5D3_PIO_DRIVER2 : SAMA5D3_PIO_DRIVER1;
+}
+
+static unsigned at91sam9x5_get_drive_register(unsigned int pin)
+{
+	/* drive strength is split between two registers
+	 * with two bits per pin */
+	return (pin >= MAX_NB_GPIO_PER_BANK/2)
+			? AT91SAM9X5_PIO_DRIVER2 : AT91SAM9X5_PIO_DRIVER1;
+}
+
 static void at91_mux_disable_interrupt(void __iomem *pio, unsigned mask)
 {
 	writel_relaxed(mask, pio + PIO_IDR);
@@ -327,6 +371,9 @@ static unsigned at91_mux_get_pullup(void __iomem *pio, unsigned pin)
 
 static void at91_mux_set_pullup(void __iomem *pio, unsigned mask, bool on)
 {
+	if (on)
+		writel_relaxed(mask, pio + PIO_PPDDR);
+
 	writel_relaxed(mask, pio + (on ? PIO_PUER : PIO_PUDR));
 }
 
@@ -455,6 +502,9 @@ static bool at91_mux_pio3_get_pulldown(void __iomem *pio, unsigned pin)
 
 static void at91_mux_pio3_set_pulldown(void __iomem *pio, unsigned mask, bool is_on)
 {
+	if (is_on)
+		__raw_writel(mask, pio + PIO_PUDR);
+
 	__raw_writel(mask, pio + (is_on ? PIO_PPDER : PIO_PPDDR));
 }
 
@@ -468,6 +518,79 @@ static bool at91_mux_pio3_get_schmitt_trig(void __iomem *pio, unsigned pin)
 	return (__raw_readl(pio + PIO_SCHMITT) >> pin) & 0x1;
 }
 
+static inline u32 read_drive_strength(void __iomem *reg, unsigned pin)
+{
+	unsigned tmp = __raw_readl(reg);
+
+	tmp = tmp >> two_bit_pin_value_shift_amount(pin);
+
+	return tmp & DRIVE_STRENGTH_MASK;
+}
+
+static unsigned at91_mux_sama5d3_get_drivestrength(void __iomem *pio,
+							unsigned pin)
+{
+	unsigned tmp = read_drive_strength(pio +
+					sama5d3_get_drive_register(pin), pin);
+
+	/* SAMA5 strength is 1:1 with our defines,
+	 * except 0 is equivalent to low per datasheet */
+	if (!tmp)
+		tmp = DRIVE_STRENGTH_LOW;
+
+	return tmp;
+}
+
+static unsigned at91_mux_sam9x5_get_drivestrength(void __iomem *pio,
+							unsigned pin)
+{
+	unsigned tmp = read_drive_strength(pio +
+				at91sam9x5_get_drive_register(pin), pin);
+
+	/* strength is inverse in SAM9x5s hardware with the pinctrl defines
+	 * hardware: 0 = hi, 1 = med, 2 = low, 3 = rsvd */
+	tmp = DRIVE_STRENGTH_HI - tmp;
+
+	return tmp;
+}
+
+static void set_drive_strength(void __iomem *reg, unsigned pin, u32 strength)
+{
+	unsigned tmp = __raw_readl(reg);
+	unsigned shift = two_bit_pin_value_shift_amount(pin);
+
+	tmp &= ~(DRIVE_STRENGTH_MASK  <<  shift);
+	tmp |= strength << shift;
+
+	__raw_writel(tmp, reg);
+}
+
+static void at91_mux_sama5d3_set_drivestrength(void __iomem *pio, unsigned pin,
+						u32 setting)
+{
+	/* do nothing if setting is zero */
+	if (!setting)
+		return;
+
+	/* strength is 1 to 1 with setting for SAMA5 */
+	set_drive_strength(pio + sama5d3_get_drive_register(pin), pin, setting);
+}
+
+static void at91_mux_sam9x5_set_drivestrength(void __iomem *pio, unsigned pin,
+						u32 setting)
+{
+	/* do nothing if setting is zero */
+	if (!setting)
+		return;
+
+	/* strength is inverse on SAM9x5s with our defines
+	 * 0 = hi, 1 = med, 2 = low, 3 = rsvd */
+	setting = DRIVE_STRENGTH_HI - setting;
+
+	set_drive_strength(pio + at91sam9x5_get_drive_register(pin), pin,
+				setting);
+}
+
 static struct at91_pinctrl_mux_ops at91rm9200_ops = {
 	.get_periph	= at91_mux_get_periph,
 	.mux_A_periph	= at91_mux_set_A_periph,
@@ -491,16 +614,37 @@ static struct at91_pinctrl_mux_ops at91sam9x5_ops = {
 	.set_pulldown	= at91_mux_pio3_set_pulldown,
 	.get_schmitt_trig = at91_mux_pio3_get_schmitt_trig,
 	.disable_schmitt_trig = at91_mux_pio3_disable_schmitt_trig,
+	.get_drivestrength = at91_mux_sam9x5_get_drivestrength,
+	.set_drivestrength = at91_mux_sam9x5_set_drivestrength,
+	.irq_type	= alt_gpio_irq_type,
+};
+
+static struct at91_pinctrl_mux_ops sama5d3_ops = {
+	.get_periph	= at91_mux_pio3_get_periph,
+	.mux_A_periph	= at91_mux_pio3_set_A_periph,
+	.mux_B_periph	= at91_mux_pio3_set_B_periph,
+	.mux_C_periph	= at91_mux_pio3_set_C_periph,
+	.mux_D_periph	= at91_mux_pio3_set_D_periph,
+	.get_deglitch	= at91_mux_pio3_get_deglitch,
+	.set_deglitch	= at91_mux_pio3_set_deglitch,
+	.get_debounce	= at91_mux_pio3_get_debounce,
+	.set_debounce	= at91_mux_pio3_set_debounce,
+	.get_pulldown	= at91_mux_pio3_get_pulldown,
+	.set_pulldown	= at91_mux_pio3_set_pulldown,
+	.get_schmitt_trig = at91_mux_pio3_get_schmitt_trig,
+	.disable_schmitt_trig = at91_mux_pio3_disable_schmitt_trig,
+	.get_drivestrength = at91_mux_sama5d3_get_drivestrength,
+	.set_drivestrength = at91_mux_sama5d3_set_drivestrength,
 	.irq_type	= alt_gpio_irq_type,
 };
 
 static void at91_pin_dbg(const struct device *dev, const struct at91_pmx_pin *pin)
 {
 	if (pin->mux) {
-		dev_dbg(dev, "pio%c%d configured as periph%c with conf = 0x%lu\n",
+		dev_dbg(dev, "pio%c%d configured as periph%c with conf = 0x%lx\n",
 			pin->bank + 'A', pin->pin, pin->mux - 1 + 'A', pin->conf);
 	} else {
-		dev_dbg(dev, "pio%c%d configured as gpio with conf = 0x%lu\n",
+		dev_dbg(dev, "pio%c%d configured as gpio with conf = 0x%lx\n",
 			pin->bank + 'A', pin->pin, pin->conf);
 	}
 }
@@ -554,8 +698,8 @@ static void at91_mux_gpio_enable(void __iomem *pio, unsigned mask, bool input)
 	writel_relaxed(mask, pio + (input ? PIO_ODR : PIO_OER));
 }
 
-static int at91_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
-			   unsigned group)
+static int at91_pmx_set(struct pinctrl_dev *pctldev, unsigned selector,
+			unsigned group)
 {
 	struct at91_pinctrl *info = pinctrl_dev_get_drvdata(pctldev);
 	const struct at91_pmx_pin *pins_conf = info->groups[group].pins_conf;
@@ -684,7 +828,7 @@ static const struct pinmux_ops at91_pmx_ops = {
 	.get_functions_count	= at91_pmx_get_funcs_count,
 	.get_function_name	= at91_pmx_get_func_name,
 	.get_function_groups	= at91_pmx_get_groups,
-	.enable			= at91_pmx_enable,
+	.set_mux		= at91_pmx_set,
 	.gpio_request_enable	= at91_gpio_request_enable,
 	.gpio_disable_free	= at91_gpio_disable_free,
 };
@@ -716,6 +860,9 @@ static int at91_pinconf_get(struct pinctrl_dev *pctldev,
 		*config |= PULL_DOWN;
 	if (info->ops->get_schmitt_trig && info->ops->get_schmitt_trig(pio, pin))
 		*config |= DIS_SCHMIT;
+	if (info->ops->get_drivestrength)
+		*config |= (info->ops->get_drivestrength(pio, pin)
+				<< DRIVE_STRENGTH_SHIFT);
 
 	return 0;
 }
@@ -729,6 +876,7 @@ static int at91_pinconf_set(struct pinctrl_dev *pctldev,
 	void __iomem *pio;
 	int i;
 	unsigned long config;
+	unsigned pin;
 
 	for (i = 0; i < num_configs; i++) {
 		config = configs[i];
@@ -737,7 +885,8 @@ static int at91_pinconf_set(struct pinctrl_dev *pctldev,
 			"%s:%d, pin_id=%d, config=0x%lx",
 			__func__, __LINE__, pin_id, config);
 		pio = pin_to_controller(info, pin_to_bank(pin_id));
-		mask = pin_to_mask(pin_id % MAX_NB_GPIO_PER_BANK);
+		pin = pin_id % MAX_NB_GPIO_PER_BANK;
+		mask = pin_to_mask(pin);
 
 		if (config & PULL_UP && config & PULL_DOWN)
 			return -EINVAL;
@@ -753,6 +902,10 @@ static int at91_pinconf_set(struct pinctrl_dev *pctldev,
 			info->ops->set_pulldown(pio, mask, config & PULL_DOWN);
 		if (info->ops->disable_schmitt_trig && config & DIS_SCHMIT)
 			info->ops->disable_schmitt_trig(pio, mask);
+		if (info->ops->set_drivestrength)
+			info->ops->set_drivestrength(pio, pin,
+				(config & DRIVE_STRENGTH)
+					>> DRIVE_STRENGTH_SHIFT);
 
 	} /* for each config */
 
@@ -768,6 +921,15 @@ static int at91_pinconf_set(struct pinctrl_dev *pctldev,
 	}					\
 } while (0)
 
+#define DBG_SHOW_FLAG_MASKED(mask,flag) do {	\
+	if ((config & mask) == flag) {		\
+		if (num_conf)			\
+			seq_puts(s, "|");	\
+		seq_puts(s, #flag);		\
+		num_conf++;			\
+	}					\
+} while (0)
+
 static void at91_pinconf_dbg_show(struct pinctrl_dev *pctldev,
 				   struct seq_file *s, unsigned pin_id)
 {
@@ -781,6 +943,9 @@ static void at91_pinconf_dbg_show(struct pinctrl_dev *pctldev,
 	DBG_SHOW_FLAG(PULL_DOWN);
 	DBG_SHOW_FLAG(DIS_SCHMIT);
 	DBG_SHOW_FLAG(DEGLITCH);
+	DBG_SHOW_FLAG_MASKED(DRIVE_STRENGTH, DRIVE_STRENGTH_LOW);
+	DBG_SHOW_FLAG_MASKED(DRIVE_STRENGTH, DRIVE_STRENGTH_MED);
+	DBG_SHOW_FLAG_MASKED(DRIVE_STRENGTH, DRIVE_STRENGTH_HI);
 	DBG_SHOW_FLAG(DEBOUNCE);
 	if (config & DEBOUNCE) {
 		val = config >> DEBOUNCE_VAL_SHIFT;
@@ -945,6 +1110,7 @@ static int at91_pinctrl_parse_functions(struct device_node *np,
 }
 
 static struct of_device_id at91_pinctrl_of_match[] = {
+	{ .compatible = "atmel,sama5d3-pinctrl", .data = &sama5d3_ops },
 	{ .compatible = "atmel,at91sam9x5-pinctrl", .data = &at91sam9x5_ops },
 	{ .compatible = "atmel,at91rm9200-pinctrl", .data = &at91rm9200_ops },
 	{ /* sentinel */ }
@@ -1445,7 +1611,7 @@ static void gpio_irq_handler(unsigned irq, struct irq_desc *desc)
 	/* now it may re-trigger */
 }
 
-static int at91_gpio_of_irq_setup(struct device_node *node,
+static int at91_gpio_of_irq_setup(struct platform_device *pdev,
 				  struct at91_gpio_chip *at91_gpio)
 {
 	struct at91_gpio_chip   *prev = NULL;
@@ -1470,9 +1636,11 @@ static int at91_gpio_of_irq_setup(struct device_node *node,
 				   0,
 				   handle_edge_irq,
 				   IRQ_TYPE_EDGE_BOTH);
-	if (ret)
-		panic("at91_gpio.%d: couldn't allocate irq domain (DT).\n",
+	if (ret) {
+		dev_err(&pdev->dev, "at91_gpio.%d: Couldn't add irqchip to gpiochip.\n",
 			at91_gpio->pioc_idx);
+		return ret;
+	}
 
 	/* Setup chained handler */
 	if (at91_gpio->pioc_idx)
@@ -1575,19 +1743,22 @@ static int at91_gpio_probe(struct platform_device *pdev)
 	at91_chip->pioc_virq = irq;
 	at91_chip->pioc_idx = alias_idx;
 
-	at91_chip->clock = clk_get(&pdev->dev, NULL);
+	at91_chip->clock = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(at91_chip->clock)) {
 		dev_err(&pdev->dev, "failed to get clock, ignoring.\n");
+		ret = PTR_ERR(at91_chip->clock);
 		goto err;
 	}
 
-	if (clk_prepare(at91_chip->clock))
-		goto clk_prep_err;
+	ret = clk_prepare(at91_chip->clock);
+	if (ret)
+		goto clk_prepare_err;
 
 	/* enable PIO controller's clock */
-	if (clk_enable(at91_chip->clock)) {
+	ret = clk_enable(at91_chip->clock);
+	if (ret) {
 		dev_err(&pdev->dev, "failed to enable clock, ignoring.\n");
-		goto clk_err;
+		goto clk_enable_err;
 	}
 
 	at91_chip->chip = at91_gpio_template;
@@ -1612,7 +1783,7 @@ static int at91_gpio_probe(struct platform_device *pdev)
 
 	if (!names) {
 		ret = -ENOMEM;
-		goto clk_err;
+		goto clk_enable_err;
 	}
 
 	for (i = 0; i < chip->ngpio; i++)
@@ -1630,23 +1801,28 @@ static int at91_gpio_probe(struct platform_device *pdev)
 
 	ret = gpiochip_add(chip);
 	if (ret)
-		goto clk_err;
+		goto gpiochip_add_err;
 
 	gpio_chips[alias_idx] = at91_chip;
 	gpio_banks = max(gpio_banks, alias_idx + 1);
 
 	at91_gpio_probe_fixup();
 
-	at91_gpio_of_irq_setup(np, at91_chip);
+	ret = at91_gpio_of_irq_setup(pdev, at91_chip);
+	if (ret)
+		goto irq_setup_err;
 
 	dev_info(&pdev->dev, "at address %p\n", at91_chip->regbase);
 
 	return 0;
 
-clk_err:
+irq_setup_err:
+	gpiochip_remove(chip);
+gpiochip_add_err:
+	clk_disable(at91_chip->clock);
+clk_enable_err:
 	clk_unprepare(at91_chip->clock);
-clk_prep_err:
-	clk_put(at91_chip->clock);
+clk_prepare_err:
 err:
 	dev_err(&pdev->dev, "Failure %i for GPIO %i\n", ret, alias_idx);
 
diff --git a/drivers/pinctrl/pinctrl-baytrail.c b/drivers/pinctrl/pinctrl-baytrail.c
index 9ca59a018743..e12e5b07f6d7 100644
--- a/drivers/pinctrl/pinctrl-baytrail.c
+++ b/drivers/pinctrl/pinctrl-baytrail.c
@@ -461,6 +461,7 @@ static struct irq_chip byt_irqchip = {
 	.irq_mask = byt_irq_mask,
 	.irq_unmask = byt_irq_unmask,
 	.irq_set_type = byt_irq_type,
+	.flags = IRQCHIP_SKIP_SET_WAKE,
 };
 
 static void byt_gpio_irq_init_hw(struct byt_gpio *vg)
diff --git a/drivers/pinctrl/pinctrl-bcm281xx.c b/drivers/pinctrl/pinctrl-bcm281xx.c
index c5ca9e633fff..a26e0c2ba33e 100644
--- a/drivers/pinctrl/pinctrl-bcm281xx.c
+++ b/drivers/pinctrl/pinctrl-bcm281xx.c
@@ -1055,9 +1055,9 @@ static int bcm281xx_pinctrl_get_fcn_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int bcm281xx_pinmux_enable(struct pinctrl_dev *pctldev,
-				  unsigned function,
-				  unsigned group)
+static int bcm281xx_pinmux_set(struct pinctrl_dev *pctldev,
+			       unsigned function,
+			       unsigned group)
 {
 	struct bcm281xx_pinctrl_data *pdata = pinctrl_dev_get_drvdata(pctldev);
 	const struct bcm281xx_pin_function *f = &pdata->functions[function];
@@ -1084,7 +1084,7 @@ static struct pinmux_ops bcm281xx_pinctrl_pinmux_ops = {
 	.get_functions_count = bcm281xx_pinctrl_get_fcns_count,
 	.get_function_name = bcm281xx_pinctrl_get_fcn_name,
 	.get_function_groups = bcm281xx_pinctrl_get_fcn_groups,
-	.enable = bcm281xx_pinmux_enable,
+	.set_mux = bcm281xx_pinmux_set,
 };
 
 static int bcm281xx_pinctrl_pin_config_get(struct pinctrl_dev *pctldev,
diff --git a/drivers/pinctrl/pinctrl-bcm2835.c b/drivers/pinctrl/pinctrl-bcm2835.c
index 5bcfd7ace0cd..eabba02f71f9 100644
--- a/drivers/pinctrl/pinctrl-bcm2835.c
+++ b/drivers/pinctrl/pinctrl-bcm2835.c
@@ -830,7 +830,7 @@ static int bcm2835_pmx_get_function_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int bcm2835_pmx_enable(struct pinctrl_dev *pctldev,
+static int bcm2835_pmx_set(struct pinctrl_dev *pctldev,
 		unsigned func_selector,
 		unsigned group_selector)
 {
@@ -869,7 +869,7 @@ static const struct pinmux_ops bcm2835_pmx_ops = {
 	.get_functions_count = bcm2835_pmx_get_functions_count,
 	.get_function_name = bcm2835_pmx_get_function_name,
 	.get_function_groups = bcm2835_pmx_get_function_groups,
-	.enable = bcm2835_pmx_enable,
+	.set_mux = bcm2835_pmx_set,
 	.gpio_disable_free = bcm2835_pmx_gpio_disable_free,
 	.gpio_set_direction = bcm2835_pmx_gpio_set_direction,
 };
diff --git a/drivers/pinctrl/pinctrl-lantiq.c b/drivers/pinctrl/pinctrl-lantiq.c
index d22ca252b80d..296e5b37f768 100644
--- a/drivers/pinctrl/pinctrl-lantiq.c
+++ b/drivers/pinctrl/pinctrl-lantiq.c
@@ -257,9 +257,9 @@ static int match_group_mux(const struct ltq_pin_group *grp,
 	return ret;
 }
 
-static int ltq_pmx_enable(struct pinctrl_dev *pctrldev,
-				unsigned func,
-				unsigned group)
+static int ltq_pmx_set(struct pinctrl_dev *pctrldev,
+		       unsigned func,
+		       unsigned group)
 {
 	struct ltq_pinmux_info *info = pinctrl_dev_get_drvdata(pctrldev);
 	const struct ltq_pin_group *pin_grp = &info->grps[group];
@@ -316,7 +316,7 @@ static const struct pinmux_ops ltq_pmx_ops = {
 	.get_functions_count	= ltq_pmx_func_count,
 	.get_function_name	= ltq_pmx_func_name,
 	.get_function_groups	= ltq_pmx_get_groups,
-	.enable			= ltq_pmx_enable,
+	.set_mux		= ltq_pmx_set,
 	.gpio_request_enable	= ltq_pmx_gpio_request_enable,
 };
 
diff --git a/drivers/pinctrl/pinctrl-palmas.c b/drivers/pinctrl/pinctrl-palmas.c
index f13d0e78a41c..e3079d3d19fe 100644
--- a/drivers/pinctrl/pinctrl-palmas.c
+++ b/drivers/pinctrl/pinctrl-palmas.c
@@ -685,7 +685,8 @@ static int palmas_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int palmas_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned function,
+static int palmas_pinctrl_set_mux(struct pinctrl_dev *pctldev,
+		unsigned function,
 		unsigned group)
 {
 	struct palmas_pctrl_chip_info *pci = pinctrl_dev_get_drvdata(pctldev);
@@ -742,7 +743,7 @@ static const struct pinmux_ops palmas_pinmux_ops = {
 	.get_functions_count = palmas_pinctrl_get_funcs_count,
 	.get_function_name = palmas_pinctrl_get_func_name,
 	.get_function_groups = palmas_pinctrl_get_func_groups,
-	.enable = palmas_pinctrl_enable,
+	.set_mux = palmas_pinctrl_set_mux,
 };
 
 static int palmas_pinconf_get(struct pinctrl_dev *pctldev,
diff --git a/drivers/pinctrl/pinctrl-rockchip.c b/drivers/pinctrl/pinctrl-rockchip.c
index 5e8b2e04cd7a..016f4578e494 100644
--- a/drivers/pinctrl/pinctrl-rockchip.c
+++ b/drivers/pinctrl/pinctrl-rockchip.c
@@ -438,7 +438,7 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 	int reg, ret, mask;
 	unsigned long flags;
 	u8 bit;
-	u32 data;
+	u32 data, rmask;
 
 	if (iomux_num > 3)
 		return -EINVAL;
@@ -478,8 +478,9 @@ static int rockchip_set_mux(struct rockchip_pin_bank *bank, int pin, int mux)
 	spin_lock_irqsave(&bank->slock, flags);
 
 	data = (mask << (bit + 16));
+	rmask = data | (data >> 16);
 	data |= (mux & mask) << bit;
-	ret = regmap_write(regmap, reg, data);
+	ret = regmap_update_bits(regmap, reg, rmask, data);
 
 	spin_unlock_irqrestore(&bank->slock, flags);
 
@@ -634,7 +635,7 @@ static int rk3288_set_drive(struct rockchip_pin_bank *bank, int pin_num,
 	struct regmap *regmap;
 	unsigned long flags;
 	int reg, ret, i;
-	u32 data;
+	u32 data, rmask;
 	u8 bit;
 
 	rk3288_calc_drv_reg_and_bit(bank, pin_num, &regmap, &reg, &bit);
@@ -657,9 +658,10 @@ static int rk3288_set_drive(struct rockchip_pin_bank *bank, int pin_num,
 
 	/* enable the write to the equivalent lower bits */
 	data = ((1 << RK3288_DRV_BITS_PER_PIN) - 1) << (bit + 16);
+	rmask = data | (data >> 16);
 	data |= (ret << bit);
 
-	ret = regmap_write(regmap, reg, data);
+	ret = regmap_update_bits(regmap, reg, rmask, data);
 	spin_unlock_irqrestore(&bank->slock, flags);
 
 	return ret;
@@ -722,7 +724,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 	int reg, ret;
 	unsigned long flags;
 	u8 bit;
-	u32 data;
+	u32 data, rmask;
 
 	dev_dbg(info->dev, "setting pull of GPIO%d-%d to %d\n",
 		 bank->bank_num, pin_num, pull);
@@ -750,6 +752,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 
 		/* enable the write to the equivalent lower bits */
 		data = ((1 << RK3188_PULL_BITS_PER_PIN) - 1) << (bit + 16);
+		rmask = data | (data >> 16);
 
 		switch (pull) {
 		case PIN_CONFIG_BIAS_DISABLE:
@@ -770,7 +773,7 @@ static int rockchip_set_pull(struct rockchip_pin_bank *bank,
 			return -EINVAL;
 		}
 
-		ret = regmap_write(regmap, reg, data);
+		ret = regmap_update_bits(regmap, reg, rmask, data);
 
 		spin_unlock_irqrestore(&bank->slock, flags);
 		break;
@@ -813,8 +816,8 @@ static int rockchip_pmx_get_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int rockchip_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
-							    unsigned group)
+static int rockchip_pmx_set(struct pinctrl_dev *pctldev, unsigned selector,
+			    unsigned group)
 {
 	struct rockchip_pinctrl *info = pinctrl_dev_get_drvdata(pctldev);
 	const unsigned int *pins = info->groups[group].pins;
@@ -889,7 +892,7 @@ static const struct pinmux_ops rockchip_pmx_ops = {
 	.get_functions_count	= rockchip_pmx_get_funcs_count,
 	.get_function_name	= rockchip_pmx_get_func_name,
 	.get_function_groups	= rockchip_pmx_get_groups,
-	.enable			= rockchip_pmx_enable,
+	.set_mux		= rockchip_pmx_set,
 	.gpio_set_direction	= rockchip_pmx_gpio_set_direction,
 };
 
diff --git a/drivers/pinctrl/pinctrl-single.c b/drivers/pinctrl/pinctrl-single.c
index 95dd9cf55cb3..fb94b772ad62 100644
--- a/drivers/pinctrl/pinctrl-single.c
+++ b/drivers/pinctrl/pinctrl-single.c
@@ -447,7 +447,7 @@ static int pcs_get_function(struct pinctrl_dev *pctldev, unsigned pin,
 	return 0;
 }
 
-static int pcs_enable(struct pinctrl_dev *pctldev, unsigned fselector,
+static int pcs_set_mux(struct pinctrl_dev *pctldev, unsigned fselector,
 	unsigned group)
 {
 	struct pcs_device *pcs;
@@ -519,7 +519,7 @@ static const struct pinmux_ops pcs_pinmux_ops = {
 	.get_functions_count = pcs_get_functions_count,
 	.get_function_name = pcs_get_function_name,
 	.get_function_groups = pcs_get_function_groups,
-	.enable = pcs_enable,
+	.set_mux = pcs_set_mux,
 	.gpio_request_enable = pcs_request_gpio,
 };
 
@@ -1981,6 +1981,18 @@ static const struct pcs_soc_data pinctrl_single_omap_wkup = {
 	.irq_status_mask = (1 << 15),	/* OMAP_WAKEUP_EVENT */
 };
 
+static const struct pcs_soc_data pinctrl_single_dra7 = {
+	.flags = PCS_QUIRK_SHARED_IRQ,
+	.irq_enable_mask = (1 << 24),	/* WAKEUPENABLE */
+	.irq_status_mask = (1 << 25),	/* WAKEUPEVENT */
+};
+
+static const struct pcs_soc_data pinctrl_single_am437x = {
+	.flags = PCS_QUIRK_SHARED_IRQ,
+	.irq_enable_mask = (1 << 29),   /* OMAP_WAKEUP_EN */
+	.irq_status_mask = (1 << 30),   /* OMAP_WAKEUP_EVENT */
+};
+
 static const struct pcs_soc_data pinctrl_single = {
 };
 
@@ -1992,6 +2004,8 @@ static struct of_device_id pcs_of_match[] = {
 	{ .compatible = "ti,omap3-padconf", .data = &pinctrl_single_omap_wkup },
 	{ .compatible = "ti,omap4-padconf", .data = &pinctrl_single_omap_wkup },
 	{ .compatible = "ti,omap5-padconf", .data = &pinctrl_single_omap_wkup },
+	{ .compatible = "ti,dra7-padconf", .data = &pinctrl_single_dra7 },
+	{ .compatible = "ti,am437-padconf", .data = &pinctrl_single_am437x },
 	{ .compatible = "pinctrl-single", .data = &pinctrl_single },
 	{ .compatible = "pinconf-single", .data = &pinconf_single },
 	{ },
diff --git a/drivers/pinctrl/pinctrl-st.c b/drivers/pinctrl/pinctrl-st.c
index 5475374d803f..4b1792aad3d8 100644
--- a/drivers/pinctrl/pinctrl-st.c
+++ b/drivers/pinctrl/pinctrl-st.c
@@ -914,8 +914,8 @@ static struct st_pio_control *st_get_pio_control(
 	return &bank->pc;
 }
 
-static int st_pmx_enable(struct pinctrl_dev *pctldev, unsigned fselector,
-		unsigned group)
+static int st_pmx_set_mux(struct pinctrl_dev *pctldev, unsigned fselector,
+			unsigned group)
 {
 	struct st_pinctrl *info = pinctrl_dev_get_drvdata(pctldev);
 	struct st_pinconf *conf = info->groups[group].pin_conf;
@@ -951,7 +951,7 @@ static struct pinmux_ops st_pmxops = {
 	.get_functions_count	= st_pmx_get_funcs_count,
 	.get_function_name	= st_pmx_get_fname,
 	.get_function_groups	= st_pmx_get_groups,
-	.enable			= st_pmx_enable,
+	.set_mux		= st_pmx_set_mux,
 	.gpio_set_direction	= st_pmx_set_gpio_direction,
 };
 
@@ -1517,6 +1517,7 @@ static int st_gpiolib_register_bank(struct st_pinctrl *info,
 					   0, handle_simple_irq,
 					   IRQ_TYPE_LEVEL_LOW);
 		if (err) {
+			gpiochip_remove(&bank->gpio_chip);
 			dev_info(dev, "could not add irqchip\n");
 			return err;
 		}
diff --git a/drivers/pinctrl/pinctrl-tb10x.c b/drivers/pinctrl/pinctrl-tb10x.c
index 71c5d4f0c538..3b9bfcf717ac 100644
--- a/drivers/pinctrl/pinctrl-tb10x.c
+++ b/drivers/pinctrl/pinctrl-tb10x.c
@@ -697,7 +697,7 @@ static void tb10x_gpio_disable_free(struct pinctrl_dev *pctl,
 	mutex_unlock(&state->mutex);
 }
 
-static int tb10x_pctl_enable(struct pinctrl_dev *pctl,
+static int tb10x_pctl_set_mux(struct pinctrl_dev *pctl,
 			unsigned func_selector, unsigned group_selector)
 {
 	struct tb10x_pinctrl *state = pinctrl_dev_get_drvdata(pctl);
@@ -744,7 +744,7 @@ static struct pinmux_ops tb10x_pinmux_ops = {
 	.get_function_groups = tb10x_get_function_groups,
 	.gpio_request_enable = tb10x_gpio_request_enable,
 	.gpio_disable_free = tb10x_gpio_disable_free,
-	.enable = tb10x_pctl_enable,
+	.set_mux = tb10x_pctl_set_mux,
 };
 
 static struct pinctrl_desc tb10x_pindesc = {
diff --git a/drivers/pinctrl/pinctrl-tegra-xusb.c b/drivers/pinctrl/pinctrl-tegra-xusb.c
index a06620474845..1631ec94fb02 100644
--- a/drivers/pinctrl/pinctrl-tegra-xusb.c
+++ b/drivers/pinctrl/pinctrl-tegra-xusb.c
@@ -281,9 +281,9 @@ static int tegra_xusb_padctl_get_function_groups(struct pinctrl_dev *pinctrl,
 	return 0;
 }
 
-static int tegra_xusb_padctl_pinmux_enable(struct pinctrl_dev *pinctrl,
-					   unsigned int function,
-					   unsigned int group)
+static int tegra_xusb_padctl_pinmux_set(struct pinctrl_dev *pinctrl,
+					unsigned int function,
+					unsigned int group)
 {
 	struct tegra_xusb_padctl *padctl = pinctrl_dev_get_drvdata(pinctrl);
 	const struct tegra_xusb_padctl_lane *lane;
@@ -311,7 +311,7 @@ static const struct pinmux_ops tegra_xusb_padctl_pinmux_ops = {
 	.get_functions_count = tegra_xusb_padctl_get_functions_count,
 	.get_function_name = tegra_xusb_padctl_get_function_name,
 	.get_function_groups = tegra_xusb_padctl_get_function_groups,
-	.enable = tegra_xusb_padctl_pinmux_enable,
+	.set_mux = tegra_xusb_padctl_pinmux_set,
 };
 
 static int tegra_xusb_padctl_pinconf_group_get(struct pinctrl_dev *pinctrl,
@@ -680,7 +680,7 @@ static struct phy *tegra_xusb_padctl_xlate(struct device *dev,
 	if (args->args_count <= 0)
 		return ERR_PTR(-EINVAL);
 
-	if (index > ARRAY_SIZE(padctl->phys))
+	if (index >= ARRAY_SIZE(padctl->phys))
 		return ERR_PTR(-EINVAL);
 
 	return padctl->phys[index];
@@ -930,7 +930,8 @@ static int tegra_xusb_padctl_probe(struct platform_device *pdev)
 
 	padctl->provider = devm_of_phy_provider_register(&pdev->dev,
 							 tegra_xusb_padctl_xlate);
-	if (err < 0) {
+	if (IS_ERR(padctl->provider)) {
+		err = PTR_ERR(padctl->provider);
 		dev_err(&pdev->dev, "failed to register PHYs: %d\n", err);
 		goto unregister;
 	}
diff --git a/drivers/pinctrl/pinctrl-tegra.c b/drivers/pinctrl/pinctrl-tegra.c
index 150af5503c09..e5949d51bc52 100644
--- a/drivers/pinctrl/pinctrl-tegra.c
+++ b/drivers/pinctrl/pinctrl-tegra.c
@@ -262,8 +262,9 @@ static int tegra_pinctrl_get_func_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int tegra_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned function,
-			       unsigned group)
+static int tegra_pinctrl_set_mux(struct pinctrl_dev *pctldev,
+				 unsigned function,
+				 unsigned group)
 {
 	struct tegra_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 	const struct tegra_pingroup *g;
@@ -294,7 +295,7 @@ static const struct pinmux_ops tegra_pinmux_ops = {
 	.get_functions_count = tegra_pinctrl_get_funcs_count,
 	.get_function_name = tegra_pinctrl_get_func_name,
 	.get_function_groups = tegra_pinctrl_get_func_groups,
-	.enable = tegra_pinctrl_enable,
+	.set_mux = tegra_pinctrl_set_mux,
 };
 
 static int tegra_pinconf_reg(struct tegra_pmx *pmx,
diff --git a/drivers/pinctrl/pinctrl-tegra114.c b/drivers/pinctrl/pinctrl-tegra114.c
index 33614baab4c0..a3db85b0b75f 100644
--- a/drivers/pinctrl/pinctrl-tegra114.c
+++ b/drivers/pinctrl/pinctrl-tegra114.c
@@ -1850,7 +1850,7 @@ static int tegra114_pinctrl_probe(struct platform_device *pdev)
 	return tegra_pinctrl_probe(pdev, &tegra114_pinctrl);
 }
 
-static struct of_device_id tegra114_pinctrl_of_match[] = {
+static const struct of_device_id tegra114_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra114-pinmux", },
 	{ },
 };
diff --git a/drivers/pinctrl/pinctrl-tegra124.c b/drivers/pinctrl/pinctrl-tegra124.c
index e80797e20017..2f9b75c14967 100644
--- a/drivers/pinctrl/pinctrl-tegra124.c
+++ b/drivers/pinctrl/pinctrl-tegra124.c
@@ -224,6 +224,16 @@
 #define TEGRA_PIN_OWR				_PIN(5)
 #define TEGRA_PIN_CLK_32K_IN			_PIN(6)
 #define TEGRA_PIN_JTAG_RTCK			_PIN(7)
+#define TEGRA_PIN_DSI_B_CLK_P			_PIN(8)
+#define TEGRA_PIN_DSI_B_CLK_N			_PIN(9)
+#define TEGRA_PIN_DSI_B_D0_P			_PIN(10)
+#define TEGRA_PIN_DSI_B_D0_N			_PIN(11)
+#define TEGRA_PIN_DSI_B_D1_P			_PIN(12)
+#define TEGRA_PIN_DSI_B_D1_N			_PIN(13)
+#define TEGRA_PIN_DSI_B_D2_P			_PIN(14)
+#define TEGRA_PIN_DSI_B_D2_N			_PIN(15)
+#define TEGRA_PIN_DSI_B_D3_P			_PIN(16)
+#define TEGRA_PIN_DSI_B_D3_N			_PIN(17)
 
 static const struct pinctrl_pin_desc tegra124_pins[] = {
 	PINCTRL_PIN(TEGRA_PIN_CLK_32K_OUT_PA0, "CLK_32K_OUT PA0"),
@@ -417,6 +427,16 @@ static const struct pinctrl_pin_desc tegra124_pins[] = {
 	PINCTRL_PIN(TEGRA_PIN_OWR, "OWR"),
 	PINCTRL_PIN(TEGRA_PIN_CLK_32K_IN, "CLK_32K_IN"),
 	PINCTRL_PIN(TEGRA_PIN_JTAG_RTCK, "JTAG_RTCK"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_CLK_P, "DSI_B_CLK_P"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_CLK_N, "DSI_B_CLK_N"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D0_P, "DSI_B_D0_P"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D0_N, "DSI_B_D0_N"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D1_P, "DSI_B_D1_P"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D1_N, "DSI_B_D1_N"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D2_P, "DSI_B_D2_P"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D2_N, "DSI_B_D2_N"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D3_P, "DSI_B_D3_P"),
+	PINCTRL_PIN(TEGRA_PIN_DSI_B_D3_N, "DSI_B_D3_N"),
 };
 
 static const unsigned clk_32k_out_pa0_pins[] = {
@@ -1495,6 +1515,19 @@ static const unsigned drive_ao4_pins[] = {
 	TEGRA_PIN_JTAG_RTCK,
 };
 
+static const unsigned mipi_pad_ctrl_dsi_b_pins[] = {
+	TEGRA_PIN_DSI_B_CLK_P,
+	TEGRA_PIN_DSI_B_CLK_N,
+	TEGRA_PIN_DSI_B_D0_P,
+	TEGRA_PIN_DSI_B_D0_N,
+	TEGRA_PIN_DSI_B_D1_P,
+	TEGRA_PIN_DSI_B_D1_N,
+	TEGRA_PIN_DSI_B_D2_P,
+	TEGRA_PIN_DSI_B_D2_N,
+	TEGRA_PIN_DSI_B_D3_P,
+	TEGRA_PIN_DSI_B_D3_N,
+};
+
 enum tegra_mux {
 	TEGRA_MUX_BLINK,
 	TEGRA_MUX_CCLA,
@@ -1580,6 +1613,8 @@ enum tegra_mux {
 	TEGRA_MUX_VI_ALT3,
 	TEGRA_MUX_VIMCLK2,
 	TEGRA_MUX_VIMCLK2_ALT,
+	TEGRA_MUX_CSI,
+	TEGRA_MUX_DSI_B,
 };
 
 #define FUNCTION(fname)					\
@@ -1672,10 +1707,13 @@ static struct tegra_function tegra124_functions[] = {
 	FUNCTION(vi_alt3),
 	FUNCTION(vimclk2),
 	FUNCTION(vimclk2_alt),
+	FUNCTION(csi),
+	FUNCTION(dsi_b),
 };
 
 #define DRV_PINGROUP_REG_A		0x868	/* bank 0 */
 #define PINGROUP_REG_A			0x3000	/* bank 1 */
+#define MIPI_PAD_CTRL_PINGROUP_REG_A	0x820	/* bank 2 */
 
 #define PINGROUP_REG(r)			((r) - PINGROUP_REG_A)
 
@@ -1744,6 +1782,32 @@ static struct tegra_function tegra124_functions[] = {
 		.drvtype_bit = PINGROUP_BIT_##drvtype(6),		\
 	}
 
+#define MIPI_PAD_CTRL_PINGROUP_REG_Y(r)	((r) - MIPI_PAD_CTRL_PINGROUP_REG_A)
+
+#define MIPI_PAD_CTRL_PINGROUP(pg_name, r, b, f0, f1)			\
+	{								\
+		.name = "mipi_pad_ctrl_" #pg_name,			\
+		.pins = mipi_pad_ctrl_##pg_name##_pins,			\
+		.npins = ARRAY_SIZE(mipi_pad_ctrl_##pg_name##_pins),	\
+		.funcs = {						\
+			TEGRA_MUX_ ## f0,				\
+			TEGRA_MUX_ ## f1,				\
+			TEGRA_MUX_RSVD3,				\
+			TEGRA_MUX_RSVD4,				\
+		},							\
+		.mux_reg = MIPI_PAD_CTRL_PINGROUP_REG_Y(r),		\
+		.mux_bank = 2,						\
+		.mux_bit = b,						\
+		.pupd_reg = -1,						\
+		.tri_reg = -1,						\
+		.einput_bit = -1,					\
+		.odrain_bit = -1,					\
+		.lock_bit = -1,						\
+		.ioreset_bit = -1,					\
+		.rcv_sel_bit = -1,					\
+		.drv_reg = -1,						\
+	}
+
 static const struct tegra_pingroup tegra124_groups[] = {
 	/*       pg_name,                f0,         f1,         f2,           f3,          r,      od, ior, rcv_sel */
 	PINGROUP(ulpi_data0_po1,         SPI3,       HSI,        UARTA,        ULPI,        0x3000, N,   N,  N),
@@ -1979,6 +2043,9 @@ static const struct tegra_pingroup tegra124_groups[] = {
 	DRV_PINGROUP(hv0,         0x9b4,  2,  3,  4,  12,  5,  -1, -1,  28,  2,  -1, -1,  N),
 	DRV_PINGROUP(sdio4,       0x9c4,  2,  3,  4,  12,  5,  20,  5,  28,  2,  30,  2,  N),
 	DRV_PINGROUP(ao4,         0x9c8,  2,  3,  4,  12,  7,  20,  7,  28,  2,  30,  2,  Y),
+
+	/*		       pg_name, r      b  f0,  f1 */
+	MIPI_PAD_CTRL_PINGROUP(dsi_b,   0x820, 1, CSI, DSI_B)
 };
 
 static const struct tegra_pinctrl_soc_data tegra124_pinctrl = {
@@ -1996,7 +2063,7 @@ static int tegra124_pinctrl_probe(struct platform_device *pdev)
 	return tegra_pinctrl_probe(pdev, &tegra124_pinctrl);
 }
 
-static struct of_device_id tegra124_pinctrl_of_match[] = {
+static const struct of_device_id tegra124_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra124-pinmux", },
 	{ },
 };
diff --git a/drivers/pinctrl/pinctrl-tegra20.c b/drivers/pinctrl/pinctrl-tegra20.c
index 7563ebc9c791..c9805d2e71b0 100644
--- a/drivers/pinctrl/pinctrl-tegra20.c
+++ b/drivers/pinctrl/pinctrl-tegra20.c
@@ -2228,7 +2228,7 @@ static int tegra20_pinctrl_probe(struct platform_device *pdev)
 	return tegra_pinctrl_probe(pdev, &tegra20_pinctrl);
 }
 
-static struct of_device_id tegra20_pinctrl_of_match[] = {
+static const struct of_device_id tegra20_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra20-pinmux", },
 	{ },
 };
diff --git a/drivers/pinctrl/pinctrl-tegra30.c b/drivers/pinctrl/pinctrl-tegra30.c
index fe2d2cf78ad9..e7b72e916558 100644
--- a/drivers/pinctrl/pinctrl-tegra30.c
+++ b/drivers/pinctrl/pinctrl-tegra30.c
@@ -2484,7 +2484,7 @@ static int tegra30_pinctrl_probe(struct platform_device *pdev)
 	return tegra_pinctrl_probe(pdev, &tegra30_pinctrl);
 }
 
-static struct of_device_id tegra30_pinctrl_of_match[] = {
+static const struct of_device_id tegra30_pinctrl_of_match[] = {
 	{ .compatible = "nvidia,tegra30-pinmux", },
 	{ },
 };
diff --git a/drivers/pinctrl/pinctrl-tz1090-pdc.c b/drivers/pinctrl/pinctrl-tz1090-pdc.c
index 41e81a35cabb..3bb6a3b78864 100644
--- a/drivers/pinctrl/pinctrl-tz1090-pdc.c
+++ b/drivers/pinctrl/pinctrl-tz1090-pdc.c
@@ -547,8 +547,9 @@ static void tz1090_pdc_pinctrl_mux(struct tz1090_pdc_pmx *pmx,
 	__global_unlock2(flags);
 }
 
-static int tz1090_pdc_pinctrl_enable(struct pinctrl_dev *pctldev,
-				     unsigned int function, unsigned int group)
+static int tz1090_pdc_pinctrl_set_mux(struct pinctrl_dev *pctldev,
+				      unsigned int function,
+				      unsigned int group)
 {
 	struct tz1090_pdc_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 	const struct tz1090_pdc_pingroup *grp = &tz1090_pdc_groups[group];
@@ -634,7 +635,7 @@ static struct pinmux_ops tz1090_pdc_pinmux_ops = {
 	.get_functions_count	= tz1090_pdc_pinctrl_get_funcs_count,
 	.get_function_name	= tz1090_pdc_pinctrl_get_func_name,
 	.get_function_groups	= tz1090_pdc_pinctrl_get_func_groups,
-	.enable			= tz1090_pdc_pinctrl_enable,
+	.set_mux		= tz1090_pdc_pinctrl_set_mux,
 	.gpio_request_enable	= tz1090_pdc_pinctrl_gpio_request_enable,
 	.gpio_disable_free	= tz1090_pdc_pinctrl_gpio_disable_free,
 };
diff --git a/drivers/pinctrl/pinctrl-tz1090.c b/drivers/pinctrl/pinctrl-tz1090.c
index 24082216842e..48d36413b99f 100644
--- a/drivers/pinctrl/pinctrl-tz1090.c
+++ b/drivers/pinctrl/pinctrl-tz1090.c
@@ -1415,8 +1415,8 @@ found_mux:
  * the effect is the same as enabling the function on each individual pin in the
  * group.
  */
-static int tz1090_pinctrl_enable(struct pinctrl_dev *pctldev,
-				 unsigned int function, unsigned int group)
+static int tz1090_pinctrl_set_mux(struct pinctrl_dev *pctldev,
+				  unsigned int function, unsigned int group)
 {
 	struct tz1090_pmx *pmx = pinctrl_dev_get_drvdata(pctldev);
 	struct tz1090_pingroup *grp;
@@ -1517,7 +1517,7 @@ static struct pinmux_ops tz1090_pinmux_ops = {
 	.get_functions_count	= tz1090_pinctrl_get_funcs_count,
 	.get_function_name	= tz1090_pinctrl_get_func_name,
 	.get_function_groups	= tz1090_pinctrl_get_func_groups,
-	.enable			= tz1090_pinctrl_enable,
+	.set_mux		= tz1090_pinctrl_set_mux,
 	.gpio_request_enable	= tz1090_pinctrl_gpio_request_enable,
 	.gpio_disable_free	= tz1090_pinctrl_gpio_disable_free,
 };
diff --git a/drivers/pinctrl/pinctrl-u300.c b/drivers/pinctrl/pinctrl-u300.c
index 0959bb36450f..e9c7113d81f2 100644
--- a/drivers/pinctrl/pinctrl-u300.c
+++ b/drivers/pinctrl/pinctrl-u300.c
@@ -955,8 +955,8 @@ static void u300_pmx_endisable(struct u300_pmx *upmx, unsigned selector,
 	}
 }
 
-static int u300_pmx_enable(struct pinctrl_dev *pctldev, unsigned selector,
-			   unsigned group)
+static int u300_pmx_set_mux(struct pinctrl_dev *pctldev, unsigned selector,
+			    unsigned group)
 {
 	struct u300_pmx *upmx;
 
@@ -994,7 +994,7 @@ static const struct pinmux_ops u300_pmx_ops = {
 	.get_functions_count = u300_pmx_get_funcs_count,
 	.get_function_name = u300_pmx_get_func_name,
 	.get_function_groups = u300_pmx_get_groups,
-	.enable = u300_pmx_enable,
+	.set_mux = u300_pmx_set_mux,
 };
 
 static int u300_pin_config_get(struct pinctrl_dev *pctldev, unsigned pin,
diff --git a/drivers/pinctrl/pinctrl-xway.c b/drivers/pinctrl/pinctrl-xway.c
index e66f4cae7633..37040ab42890 100644
--- a/drivers/pinctrl/pinctrl-xway.c
+++ b/drivers/pinctrl/pinctrl-xway.c
@@ -801,6 +801,7 @@ static int pinmux_xway_probe(struct platform_device *pdev)
 	of_gpiochip_add(&xway_chip);
 	ret = gpiochip_add(&xway_chip);
 	if (ret) {
+		of_gpiochip_remove(&xway_chip);
 		dev_err(&pdev->dev, "Failed to register gpio chip\n");
 		return ret;
 	}
@@ -822,6 +823,7 @@ static int pinmux_xway_probe(struct platform_device *pdev)
 	/* register with the generic lantiq layer */
 	ret = ltq_pinctrl_register(pdev, &xway_info);
 	if (ret) {
+		gpiochip_remove(&xway_chip);
 		dev_err(&pdev->dev, "Failed to register pinctrl driver\n");
 		return ret;
 	}
diff --git a/drivers/pinctrl/pinmux.c b/drivers/pinctrl/pinmux.c
index c055daf9a80f..b874458dcb88 100644
--- a/drivers/pinctrl/pinmux.c
+++ b/drivers/pinctrl/pinmux.c
@@ -41,7 +41,7 @@ int pinmux_check_ops(struct pinctrl_dev *pctldev)
 	    !ops->get_functions_count ||
 	    !ops->get_function_name ||
 	    !ops->get_function_groups ||
-	    !ops->enable) {
+	    !ops->set_mux) {
 		dev_err(pctldev->dev, "pinmux ops lacks necessary functions\n");
 		return -EINVAL;
 	}
@@ -445,15 +445,15 @@ int pinmux_enable_setting(struct pinctrl_setting const *setting)
 		desc->mux_setting = &(setting->data.mux);
 	}
 
-	ret = ops->enable(pctldev, setting->data.mux.func,
-			  setting->data.mux.group);
+	ret = ops->set_mux(pctldev, setting->data.mux.func,
+			   setting->data.mux.group);
 
 	if (ret)
-		goto err_enable;
+		goto err_set_mux;
 
 	return 0;
 
-err_enable:
+err_set_mux:
 	for (i = 0; i < num_pins; i++) {
 		desc = pin_desc_get(pctldev, pins[i]);
 		if (desc)
diff --git a/drivers/pinctrl/qcom/Kconfig b/drivers/pinctrl/qcom/Kconfig
index d160a710d704..81275af9638b 100644
--- a/drivers/pinctrl/qcom/Kconfig
+++ b/drivers/pinctrl/qcom/Kconfig
@@ -15,6 +15,14 @@ config PINCTRL_APQ8064
 	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
 	  Qualcomm TLMM block found in the Qualcomm APQ8064 platform.
 
+config PINCTRL_APQ8084
+	tristate "Qualcomm APQ8084 pin controller driver"
+	depends on GPIOLIB && OF
+	select PINCTRL_MSM
+	help
+	  This is the pinctrl, pinmux, pinconf and gpiolib driver for the
+	  Qualcomm TLMM block found in the Qualcomm APQ8084 platform.
+
 config PINCTRL_IPQ8064
 	tristate "Qualcomm IPQ8064 pin controller driver"
 	depends on GPIOLIB && OF
diff --git a/drivers/pinctrl/qcom/Makefile b/drivers/pinctrl/qcom/Makefile
index 2a02602d715c..ba8519fcd8d3 100644
--- a/drivers/pinctrl/qcom/Makefile
+++ b/drivers/pinctrl/qcom/Makefile
@@ -1,6 +1,7 @@
 # Qualcomm pin control drivers
 obj-$(CONFIG_PINCTRL_MSM)	+= pinctrl-msm.o
 obj-$(CONFIG_PINCTRL_APQ8064)	+= pinctrl-apq8064.o
+obj-$(CONFIG_PINCTRL_APQ8084)	+= pinctrl-apq8084.o
 obj-$(CONFIG_PINCTRL_IPQ8064)	+= pinctrl-ipq8064.o
 obj-$(CONFIG_PINCTRL_MSM8960)	+= pinctrl-msm8960.o
 obj-$(CONFIG_PINCTRL_MSM8X74)	+= pinctrl-msm8x74.o
diff --git a/drivers/pinctrl/qcom/pinctrl-apq8064.c b/drivers/pinctrl/qcom/pinctrl-apq8064.c
index feb6f152f9b7..c832d7d6b912 100644
--- a/drivers/pinctrl/qcom/pinctrl-apq8064.c
+++ b/drivers/pinctrl/qcom/pinctrl-apq8064.c
@@ -258,6 +258,7 @@ static const unsigned int sdc3_data_pins[] = { 95 };
 		.intr_status_bit = 0,			\
 		.intr_ack_high = 1,			\
 		.intr_target_bit = 0,			\
+		.intr_target_kpss_val = 4,		\
 		.intr_raw_status_bit = 3,		\
 		.intr_polarity_bit = 1,			\
 		.intr_detection_bit = 2,		\
@@ -283,6 +284,7 @@ static const unsigned int sdc3_data_pins[] = { 95 };
 		.intr_enable_bit = -1,			\
 		.intr_status_bit = -1,			\
 		.intr_target_bit = -1,			\
+		.intr_target_kpss_val = -1,		\
 		.intr_raw_status_bit = -1,		\
 		.intr_polarity_bit = -1,		\
 		.intr_detection_bit = -1,		\
@@ -324,6 +326,7 @@ enum apq8064_functions {
 	APQ_MUX_tsif1,
 	APQ_MUX_tsif2,
 	APQ_MUX_usb2_hsic,
+	APQ_MUX_ps_hold,
 	APQ_MUX_NA,
 };
 
@@ -351,6 +354,9 @@ static const char * const gpio_groups[] = {
 	"gpio78", "gpio79", "gpio80", "gpio81", "gpio82", "gpio83", "gpio84",
 	"gpio85", "gpio86", "gpio87", "gpio88", "gpio89"
 };
+static const char * const ps_hold_groups[] = {
+	"gpio78"
+};
 static const char * const gsbi1_groups[] = {
 	"gpio18", "gpio19", "gpio20", "gpio21"
 };
@@ -477,6 +483,7 @@ static const struct msm_function apq8064_functions[] = {
 	FUNCTION(tsif1),
 	FUNCTION(tsif2),
 	FUNCTION(usb2_hsic),
+	FUNCTION(ps_hold),
 };
 
 static const struct msm_pingroup apq8064_groups[] = {
@@ -558,7 +565,7 @@ static const struct msm_pingroup apq8064_groups[] = {
 	PINGROUP(75, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(76, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(77, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
-	PINGROUP(78, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(78, ps_hold, NA, NA, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(79, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(80, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
 	PINGROUP(81, NA, NA, NA, NA, NA, NA, NA, NA, NA, NA),
diff --git a/drivers/pinctrl/qcom/pinctrl-apq8084.c b/drivers/pinctrl/qcom/pinctrl-apq8084.c
new file mode 100644
index 000000000000..138cbf6134a5
--- /dev/null
+++ b/drivers/pinctrl/qcom/pinctrl-apq8084.c
@@ -0,0 +1,1245 @@
+/*
+ * Copyright (c) 2014, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/pinctrl/pinctrl.h>
+
+#include "pinctrl-msm.h"
+
+static const struct pinctrl_pin_desc apq8084_pins[] = {
+	PINCTRL_PIN(0, "GPIO_0"),
+	PINCTRL_PIN(1, "GPIO_1"),
+	PINCTRL_PIN(2, "GPIO_2"),
+	PINCTRL_PIN(3, "GPIO_3"),
+	PINCTRL_PIN(4, "GPIO_4"),
+	PINCTRL_PIN(5, "GPIO_5"),
+	PINCTRL_PIN(6, "GPIO_6"),
+	PINCTRL_PIN(7, "GPIO_7"),
+	PINCTRL_PIN(8, "GPIO_8"),
+	PINCTRL_PIN(9, "GPIO_9"),
+	PINCTRL_PIN(10, "GPIO_10"),
+	PINCTRL_PIN(11, "GPIO_11"),
+	PINCTRL_PIN(12, "GPIO_12"),
+	PINCTRL_PIN(13, "GPIO_13"),
+	PINCTRL_PIN(14, "GPIO_14"),
+	PINCTRL_PIN(15, "GPIO_15"),
+	PINCTRL_PIN(16, "GPIO_16"),
+	PINCTRL_PIN(17, "GPIO_17"),
+	PINCTRL_PIN(18, "GPIO_18"),
+	PINCTRL_PIN(19, "GPIO_19"),
+	PINCTRL_PIN(20, "GPIO_20"),
+	PINCTRL_PIN(21, "GPIO_21"),
+	PINCTRL_PIN(22, "GPIO_22"),
+	PINCTRL_PIN(23, "GPIO_23"),
+	PINCTRL_PIN(24, "GPIO_24"),
+	PINCTRL_PIN(25, "GPIO_25"),
+	PINCTRL_PIN(26, "GPIO_26"),
+	PINCTRL_PIN(27, "GPIO_27"),
+	PINCTRL_PIN(28, "GPIO_28"),
+	PINCTRL_PIN(29, "GPIO_29"),
+	PINCTRL_PIN(30, "GPIO_30"),
+	PINCTRL_PIN(31, "GPIO_31"),
+	PINCTRL_PIN(32, "GPIO_32"),
+	PINCTRL_PIN(33, "GPIO_33"),
+	PINCTRL_PIN(34, "GPIO_34"),
+	PINCTRL_PIN(35, "GPIO_35"),
+	PINCTRL_PIN(36, "GPIO_36"),
+	PINCTRL_PIN(37, "GPIO_37"),
+	PINCTRL_PIN(38, "GPIO_38"),
+	PINCTRL_PIN(39, "GPIO_39"),
+	PINCTRL_PIN(40, "GPIO_40"),
+	PINCTRL_PIN(41, "GPIO_41"),
+	PINCTRL_PIN(42, "GPIO_42"),
+	PINCTRL_PIN(43, "GPIO_43"),
+	PINCTRL_PIN(44, "GPIO_44"),
+	PINCTRL_PIN(45, "GPIO_45"),
+	PINCTRL_PIN(46, "GPIO_46"),
+	PINCTRL_PIN(47, "GPIO_47"),
+	PINCTRL_PIN(48, "GPIO_48"),
+	PINCTRL_PIN(49, "GPIO_49"),
+	PINCTRL_PIN(50, "GPIO_50"),
+	PINCTRL_PIN(51, "GPIO_51"),
+	PINCTRL_PIN(52, "GPIO_52"),
+	PINCTRL_PIN(53, "GPIO_53"),
+	PINCTRL_PIN(54, "GPIO_54"),
+	PINCTRL_PIN(55, "GPIO_55"),
+	PINCTRL_PIN(56, "GPIO_56"),
+	PINCTRL_PIN(57, "GPIO_57"),
+	PINCTRL_PIN(58, "GPIO_58"),
+	PINCTRL_PIN(59, "GPIO_59"),
+	PINCTRL_PIN(60, "GPIO_60"),
+	PINCTRL_PIN(61, "GPIO_61"),
+	PINCTRL_PIN(62, "GPIO_62"),
+	PINCTRL_PIN(63, "GPIO_63"),
+	PINCTRL_PIN(64, "GPIO_64"),
+	PINCTRL_PIN(65, "GPIO_65"),
+	PINCTRL_PIN(66, "GPIO_66"),
+	PINCTRL_PIN(67, "GPIO_67"),
+	PINCTRL_PIN(68, "GPIO_68"),
+	PINCTRL_PIN(69, "GPIO_69"),
+	PINCTRL_PIN(70, "GPIO_70"),
+	PINCTRL_PIN(71, "GPIO_71"),
+	PINCTRL_PIN(72, "GPIO_72"),
+	PINCTRL_PIN(73, "GPIO_73"),
+	PINCTRL_PIN(74, "GPIO_74"),
+	PINCTRL_PIN(75, "GPIO_75"),
+	PINCTRL_PIN(76, "GPIO_76"),
+	PINCTRL_PIN(77, "GPIO_77"),
+	PINCTRL_PIN(78, "GPIO_78"),
+	PINCTRL_PIN(79, "GPIO_79"),
+	PINCTRL_PIN(80, "GPIO_80"),
+	PINCTRL_PIN(81, "GPIO_81"),
+	PINCTRL_PIN(82, "GPIO_82"),
+	PINCTRL_PIN(83, "GPIO_83"),
+	PINCTRL_PIN(84, "GPIO_84"),
+	PINCTRL_PIN(85, "GPIO_85"),
+	PINCTRL_PIN(86, "GPIO_86"),
+	PINCTRL_PIN(87, "GPIO_87"),
+	PINCTRL_PIN(88, "GPIO_88"),
+	PINCTRL_PIN(89, "GPIO_89"),
+	PINCTRL_PIN(90, "GPIO_90"),
+	PINCTRL_PIN(91, "GPIO_91"),
+	PINCTRL_PIN(92, "GPIO_92"),
+	PINCTRL_PIN(93, "GPIO_93"),
+	PINCTRL_PIN(94, "GPIO_94"),
+	PINCTRL_PIN(95, "GPIO_95"),
+	PINCTRL_PIN(96, "GPIO_96"),
+	PINCTRL_PIN(97, "GPIO_97"),
+	PINCTRL_PIN(98, "GPIO_98"),
+	PINCTRL_PIN(99, "GPIO_99"),
+	PINCTRL_PIN(100, "GPIO_100"),
+	PINCTRL_PIN(101, "GPIO_101"),
+	PINCTRL_PIN(102, "GPIO_102"),
+	PINCTRL_PIN(103, "GPIO_103"),
+	PINCTRL_PIN(104, "GPIO_104"),
+	PINCTRL_PIN(105, "GPIO_105"),
+	PINCTRL_PIN(106, "GPIO_106"),
+	PINCTRL_PIN(107, "GPIO_107"),
+	PINCTRL_PIN(108, "GPIO_108"),
+	PINCTRL_PIN(109, "GPIO_109"),
+	PINCTRL_PIN(110, "GPIO_110"),
+	PINCTRL_PIN(111, "GPIO_111"),
+	PINCTRL_PIN(112, "GPIO_112"),
+	PINCTRL_PIN(113, "GPIO_113"),
+	PINCTRL_PIN(114, "GPIO_114"),
+	PINCTRL_PIN(115, "GPIO_115"),
+	PINCTRL_PIN(116, "GPIO_116"),
+	PINCTRL_PIN(117, "GPIO_117"),
+	PINCTRL_PIN(118, "GPIO_118"),
+	PINCTRL_PIN(119, "GPIO_119"),
+	PINCTRL_PIN(120, "GPIO_120"),
+	PINCTRL_PIN(121, "GPIO_121"),
+	PINCTRL_PIN(122, "GPIO_122"),
+	PINCTRL_PIN(123, "GPIO_123"),
+	PINCTRL_PIN(124, "GPIO_124"),
+	PINCTRL_PIN(125, "GPIO_125"),
+	PINCTRL_PIN(126, "GPIO_126"),
+	PINCTRL_PIN(127, "GPIO_127"),
+	PINCTRL_PIN(128, "GPIO_128"),
+	PINCTRL_PIN(129, "GPIO_129"),
+	PINCTRL_PIN(130, "GPIO_130"),
+	PINCTRL_PIN(131, "GPIO_131"),
+	PINCTRL_PIN(132, "GPIO_132"),
+	PINCTRL_PIN(133, "GPIO_133"),
+	PINCTRL_PIN(134, "GPIO_134"),
+	PINCTRL_PIN(135, "GPIO_135"),
+	PINCTRL_PIN(136, "GPIO_136"),
+	PINCTRL_PIN(137, "GPIO_137"),
+	PINCTRL_PIN(138, "GPIO_138"),
+	PINCTRL_PIN(139, "GPIO_139"),
+	PINCTRL_PIN(140, "GPIO_140"),
+	PINCTRL_PIN(141, "GPIO_141"),
+	PINCTRL_PIN(142, "GPIO_142"),
+	PINCTRL_PIN(143, "GPIO_143"),
+	PINCTRL_PIN(144, "GPIO_144"),
+	PINCTRL_PIN(145, "GPIO_145"),
+	PINCTRL_PIN(146, "GPIO_146"),
+
+	PINCTRL_PIN(147, "SDC1_CLK"),
+	PINCTRL_PIN(148, "SDC1_CMD"),
+	PINCTRL_PIN(149, "SDC1_DATA"),
+	PINCTRL_PIN(150, "SDC2_CLK"),
+	PINCTRL_PIN(151, "SDC2_CMD"),
+	PINCTRL_PIN(152, "SDC2_DATA"),
+};
+
+#define DECLARE_APQ_GPIO_PINS(pin) static const unsigned int gpio##pin##_pins[] = { pin }
+
+DECLARE_APQ_GPIO_PINS(0);
+DECLARE_APQ_GPIO_PINS(1);
+DECLARE_APQ_GPIO_PINS(2);
+DECLARE_APQ_GPIO_PINS(3);
+DECLARE_APQ_GPIO_PINS(4);
+DECLARE_APQ_GPIO_PINS(5);
+DECLARE_APQ_GPIO_PINS(6);
+DECLARE_APQ_GPIO_PINS(7);
+DECLARE_APQ_GPIO_PINS(8);
+DECLARE_APQ_GPIO_PINS(9);
+DECLARE_APQ_GPIO_PINS(10);
+DECLARE_APQ_GPIO_PINS(11);
+DECLARE_APQ_GPIO_PINS(12);
+DECLARE_APQ_GPIO_PINS(13);
+DECLARE_APQ_GPIO_PINS(14);
+DECLARE_APQ_GPIO_PINS(15);
+DECLARE_APQ_GPIO_PINS(16);
+DECLARE_APQ_GPIO_PINS(17);
+DECLARE_APQ_GPIO_PINS(18);
+DECLARE_APQ_GPIO_PINS(19);
+DECLARE_APQ_GPIO_PINS(20);
+DECLARE_APQ_GPIO_PINS(21);
+DECLARE_APQ_GPIO_PINS(22);
+DECLARE_APQ_GPIO_PINS(23);
+DECLARE_APQ_GPIO_PINS(24);
+DECLARE_APQ_GPIO_PINS(25);
+DECLARE_APQ_GPIO_PINS(26);
+DECLARE_APQ_GPIO_PINS(27);
+DECLARE_APQ_GPIO_PINS(28);
+DECLARE_APQ_GPIO_PINS(29);
+DECLARE_APQ_GPIO_PINS(30);
+DECLARE_APQ_GPIO_PINS(31);
+DECLARE_APQ_GPIO_PINS(32);
+DECLARE_APQ_GPIO_PINS(33);
+DECLARE_APQ_GPIO_PINS(34);
+DECLARE_APQ_GPIO_PINS(35);
+DECLARE_APQ_GPIO_PINS(36);
+DECLARE_APQ_GPIO_PINS(37);
+DECLARE_APQ_GPIO_PINS(38);
+DECLARE_APQ_GPIO_PINS(39);
+DECLARE_APQ_GPIO_PINS(40);
+DECLARE_APQ_GPIO_PINS(41);
+DECLARE_APQ_GPIO_PINS(42);
+DECLARE_APQ_GPIO_PINS(43);
+DECLARE_APQ_GPIO_PINS(44);
+DECLARE_APQ_GPIO_PINS(45);
+DECLARE_APQ_GPIO_PINS(46);
+DECLARE_APQ_GPIO_PINS(47);
+DECLARE_APQ_GPIO_PINS(48);
+DECLARE_APQ_GPIO_PINS(49);
+DECLARE_APQ_GPIO_PINS(50);
+DECLARE_APQ_GPIO_PINS(51);
+DECLARE_APQ_GPIO_PINS(52);
+DECLARE_APQ_GPIO_PINS(53);
+DECLARE_APQ_GPIO_PINS(54);
+DECLARE_APQ_GPIO_PINS(55);
+DECLARE_APQ_GPIO_PINS(56);
+DECLARE_APQ_GPIO_PINS(57);
+DECLARE_APQ_GPIO_PINS(58);
+DECLARE_APQ_GPIO_PINS(59);
+DECLARE_APQ_GPIO_PINS(60);
+DECLARE_APQ_GPIO_PINS(61);
+DECLARE_APQ_GPIO_PINS(62);
+DECLARE_APQ_GPIO_PINS(63);
+DECLARE_APQ_GPIO_PINS(64);
+DECLARE_APQ_GPIO_PINS(65);
+DECLARE_APQ_GPIO_PINS(66);
+DECLARE_APQ_GPIO_PINS(67);
+DECLARE_APQ_GPIO_PINS(68);
+DECLARE_APQ_GPIO_PINS(69);
+DECLARE_APQ_GPIO_PINS(70);
+DECLARE_APQ_GPIO_PINS(71);
+DECLARE_APQ_GPIO_PINS(72);
+DECLARE_APQ_GPIO_PINS(73);
+DECLARE_APQ_GPIO_PINS(74);
+DECLARE_APQ_GPIO_PINS(75);
+DECLARE_APQ_GPIO_PINS(76);
+DECLARE_APQ_GPIO_PINS(77);
+DECLARE_APQ_GPIO_PINS(78);
+DECLARE_APQ_GPIO_PINS(79);
+DECLARE_APQ_GPIO_PINS(80);
+DECLARE_APQ_GPIO_PINS(81);
+DECLARE_APQ_GPIO_PINS(82);
+DECLARE_APQ_GPIO_PINS(83);
+DECLARE_APQ_GPIO_PINS(84);
+DECLARE_APQ_GPIO_PINS(85);
+DECLARE_APQ_GPIO_PINS(86);
+DECLARE_APQ_GPIO_PINS(87);
+DECLARE_APQ_GPIO_PINS(88);
+DECLARE_APQ_GPIO_PINS(89);
+DECLARE_APQ_GPIO_PINS(90);
+DECLARE_APQ_GPIO_PINS(91);
+DECLARE_APQ_GPIO_PINS(92);
+DECLARE_APQ_GPIO_PINS(93);
+DECLARE_APQ_GPIO_PINS(94);
+DECLARE_APQ_GPIO_PINS(95);
+DECLARE_APQ_GPIO_PINS(96);
+DECLARE_APQ_GPIO_PINS(97);
+DECLARE_APQ_GPIO_PINS(98);
+DECLARE_APQ_GPIO_PINS(99);
+DECLARE_APQ_GPIO_PINS(100);
+DECLARE_APQ_GPIO_PINS(101);
+DECLARE_APQ_GPIO_PINS(102);
+DECLARE_APQ_GPIO_PINS(103);
+DECLARE_APQ_GPIO_PINS(104);
+DECLARE_APQ_GPIO_PINS(105);
+DECLARE_APQ_GPIO_PINS(106);
+DECLARE_APQ_GPIO_PINS(107);
+DECLARE_APQ_GPIO_PINS(108);
+DECLARE_APQ_GPIO_PINS(109);
+DECLARE_APQ_GPIO_PINS(110);
+DECLARE_APQ_GPIO_PINS(111);
+DECLARE_APQ_GPIO_PINS(112);
+DECLARE_APQ_GPIO_PINS(113);
+DECLARE_APQ_GPIO_PINS(114);
+DECLARE_APQ_GPIO_PINS(115);
+DECLARE_APQ_GPIO_PINS(116);
+DECLARE_APQ_GPIO_PINS(117);
+DECLARE_APQ_GPIO_PINS(118);
+DECLARE_APQ_GPIO_PINS(119);
+DECLARE_APQ_GPIO_PINS(120);
+DECLARE_APQ_GPIO_PINS(121);
+DECLARE_APQ_GPIO_PINS(122);
+DECLARE_APQ_GPIO_PINS(123);
+DECLARE_APQ_GPIO_PINS(124);
+DECLARE_APQ_GPIO_PINS(125);
+DECLARE_APQ_GPIO_PINS(126);
+DECLARE_APQ_GPIO_PINS(127);
+DECLARE_APQ_GPIO_PINS(128);
+DECLARE_APQ_GPIO_PINS(129);
+DECLARE_APQ_GPIO_PINS(130);
+DECLARE_APQ_GPIO_PINS(131);
+DECLARE_APQ_GPIO_PINS(132);
+DECLARE_APQ_GPIO_PINS(133);
+DECLARE_APQ_GPIO_PINS(134);
+DECLARE_APQ_GPIO_PINS(135);
+DECLARE_APQ_GPIO_PINS(136);
+DECLARE_APQ_GPIO_PINS(137);
+DECLARE_APQ_GPIO_PINS(138);
+DECLARE_APQ_GPIO_PINS(139);
+DECLARE_APQ_GPIO_PINS(140);
+DECLARE_APQ_GPIO_PINS(141);
+DECLARE_APQ_GPIO_PINS(142);
+DECLARE_APQ_GPIO_PINS(143);
+DECLARE_APQ_GPIO_PINS(144);
+DECLARE_APQ_GPIO_PINS(145);
+DECLARE_APQ_GPIO_PINS(146);
+
+static const unsigned int sdc1_clk_pins[] = { 147 };
+static const unsigned int sdc1_cmd_pins[] = { 148 };
+static const unsigned int sdc1_data_pins[] = { 149 };
+static const unsigned int sdc2_clk_pins[] = { 150 };
+static const unsigned int sdc2_cmd_pins[] = { 151 };
+static const unsigned int sdc2_data_pins[] = { 152 };
+
+#define FUNCTION(fname)					\
+	[APQ_MUX_##fname] = {				\
+		.name = #fname,				\
+		.groups = fname##_groups,		\
+		.ngroups = ARRAY_SIZE(fname##_groups),	\
+	}
+
+#define PINGROUP(id, f1, f2, f3, f4, f5, f6, f7)        \
+	{						\
+		.name = "gpio" #id,			\
+		.pins = gpio##id##_pins,		\
+		.npins = ARRAY_SIZE(gpio##id##_pins),	\
+		.funcs = (int[]){			\
+			APQ_MUX_gpio,			\
+			APQ_MUX_##f1,			\
+			APQ_MUX_##f2,			\
+			APQ_MUX_##f3,			\
+			APQ_MUX_##f4,			\
+			APQ_MUX_##f5,			\
+			APQ_MUX_##f6,			\
+			APQ_MUX_##f7			\
+		},					\
+		.nfuncs = 8,				\
+		.ctl_reg = 0x1000 + 0x10 * id,		\
+		.io_reg = 0x1004 + 0x10 * id,		\
+		.intr_cfg_reg = 0x1008 + 0x10 * id,	\
+		.intr_status_reg = 0x100c + 0x10 * id,	\
+		.intr_target_reg = 0x1008 + 0x10 * id,	\
+		.mux_bit = 2,				\
+		.pull_bit = 0,				\
+		.drv_bit = 6,				\
+		.oe_bit = 9,				\
+		.in_bit = 0,				\
+		.out_bit = 1,				\
+		.intr_enable_bit = 0,			\
+		.intr_status_bit = 0,			\
+		.intr_ack_high = 0,			\
+		.intr_target_bit = 5,			\
+		.intr_target_kpss_val = 3,		\
+		.intr_raw_status_bit = 4,		\
+		.intr_polarity_bit = 1,			\
+		.intr_detection_bit = 2,		\
+		.intr_detection_width = 2,		\
+	}
+
+#define SDC_PINGROUP(pg_name, ctl, pull, drv)		\
+	{						\
+		.name = #pg_name,	                \
+		.pins = pg_name##_pins,                 \
+		.npins = ARRAY_SIZE(pg_name##_pins),    \
+		.ctl_reg = ctl,                         \
+		.io_reg = 0,                            \
+		.intr_cfg_reg = 0,                      \
+		.intr_status_reg = 0,                   \
+		.intr_target_reg = 0,                   \
+		.mux_bit = -1,                          \
+		.pull_bit = pull,                       \
+		.drv_bit = drv,                         \
+		.oe_bit = -1,                           \
+		.in_bit = -1,                           \
+		.out_bit = -1,                          \
+		.intr_enable_bit = -1,                  \
+		.intr_status_bit = -1,                  \
+		.intr_target_bit = -1,                  \
+		.intr_target_kpss_val = -1,		\
+		.intr_raw_status_bit = -1,              \
+		.intr_polarity_bit = -1,                \
+		.intr_detection_bit = -1,               \
+		.intr_detection_width = -1,             \
+	}
+
+enum apq8084_functions {
+	APQ_MUX_adsp_ext,
+	APQ_MUX_audio_ref,
+	APQ_MUX_blsp_i2c1,
+	APQ_MUX_blsp_i2c2,
+	APQ_MUX_blsp_i2c3,
+	APQ_MUX_blsp_i2c4,
+	APQ_MUX_blsp_i2c5,
+	APQ_MUX_blsp_i2c6,
+	APQ_MUX_blsp_i2c7,
+	APQ_MUX_blsp_i2c8,
+	APQ_MUX_blsp_i2c9,
+	APQ_MUX_blsp_i2c10,
+	APQ_MUX_blsp_i2c11,
+	APQ_MUX_blsp_i2c12,
+	APQ_MUX_blsp_spi1,
+	APQ_MUX_blsp_spi1_cs1,
+	APQ_MUX_blsp_spi1_cs2,
+	APQ_MUX_blsp_spi1_cs3,
+	APQ_MUX_blsp_spi2,
+	APQ_MUX_blsp_spi3,
+	APQ_MUX_blsp_spi3_cs1,
+	APQ_MUX_blsp_spi3_cs2,
+	APQ_MUX_blsp_spi3_cs3,
+	APQ_MUX_blsp_spi4,
+	APQ_MUX_blsp_spi5,
+	APQ_MUX_blsp_spi6,
+	APQ_MUX_blsp_spi7,
+	APQ_MUX_blsp_spi8,
+	APQ_MUX_blsp_spi9,
+	APQ_MUX_blsp_spi10,
+	APQ_MUX_blsp_spi10_cs1,
+	APQ_MUX_blsp_spi10_cs2,
+	APQ_MUX_blsp_spi10_cs3,
+	APQ_MUX_blsp_spi11,
+	APQ_MUX_blsp_spi12,
+	APQ_MUX_blsp_uart1,
+	APQ_MUX_blsp_uart2,
+	APQ_MUX_blsp_uart3,
+	APQ_MUX_blsp_uart4,
+	APQ_MUX_blsp_uart5,
+	APQ_MUX_blsp_uart6,
+	APQ_MUX_blsp_uart7,
+	APQ_MUX_blsp_uart8,
+	APQ_MUX_blsp_uart9,
+	APQ_MUX_blsp_uart10,
+	APQ_MUX_blsp_uart11,
+	APQ_MUX_blsp_uart12,
+	APQ_MUX_blsp_uim1,
+	APQ_MUX_blsp_uim2,
+	APQ_MUX_blsp_uim3,
+	APQ_MUX_blsp_uim4,
+	APQ_MUX_blsp_uim5,
+	APQ_MUX_blsp_uim6,
+	APQ_MUX_blsp_uim7,
+	APQ_MUX_blsp_uim8,
+	APQ_MUX_blsp_uim9,
+	APQ_MUX_blsp_uim10,
+	APQ_MUX_blsp_uim11,
+	APQ_MUX_blsp_uim12,
+	APQ_MUX_cam_mclk0,
+	APQ_MUX_cam_mclk1,
+	APQ_MUX_cam_mclk2,
+	APQ_MUX_cam_mclk3,
+	APQ_MUX_cci_async,
+	APQ_MUX_cci_async_in0,
+	APQ_MUX_cci_i2c0,
+	APQ_MUX_cci_i2c1,
+	APQ_MUX_cci_timer0,
+	APQ_MUX_cci_timer1,
+	APQ_MUX_cci_timer2,
+	APQ_MUX_cci_timer3,
+	APQ_MUX_cci_timer4,
+	APQ_MUX_edp_hpd,
+	APQ_MUX_gcc_gp1,
+	APQ_MUX_gcc_gp2,
+	APQ_MUX_gcc_gp3,
+	APQ_MUX_gcc_obt,
+	APQ_MUX_gcc_vtt,
+	APQ_MUX_gp_mn,
+	APQ_MUX_gp_pdm0,
+	APQ_MUX_gp_pdm1,
+	APQ_MUX_gp_pdm2,
+	APQ_MUX_gp0_clk,
+	APQ_MUX_gp1_clk,
+	APQ_MUX_gpio,
+	APQ_MUX_hdmi_cec,
+	APQ_MUX_hdmi_ddc,
+	APQ_MUX_hdmi_dtest,
+	APQ_MUX_hdmi_hpd,
+	APQ_MUX_hdmi_rcv,
+	APQ_MUX_hsic,
+	APQ_MUX_ldo_en,
+	APQ_MUX_ldo_update,
+	APQ_MUX_mdp_vsync,
+	APQ_MUX_pci_e0,
+	APQ_MUX_pci_e0_n,
+	APQ_MUX_pci_e0_rst,
+	APQ_MUX_pci_e1,
+	APQ_MUX_pci_e1_rst,
+	APQ_MUX_pci_e1_rst_n,
+	APQ_MUX_pci_e1_clkreq_n,
+	APQ_MUX_pri_mi2s,
+	APQ_MUX_qua_mi2s,
+	APQ_MUX_sata_act,
+	APQ_MUX_sata_devsleep,
+	APQ_MUX_sata_devsleep_n,
+	APQ_MUX_sd_write,
+	APQ_MUX_sdc_emmc_mode,
+	APQ_MUX_sdc3,
+	APQ_MUX_sdc4,
+	APQ_MUX_sec_mi2s,
+	APQ_MUX_slimbus,
+	APQ_MUX_spdif_tx,
+	APQ_MUX_spkr_i2s,
+	APQ_MUX_spkr_i2s_ws,
+	APQ_MUX_spss_geni,
+	APQ_MUX_ter_mi2s,
+	APQ_MUX_tsif1,
+	APQ_MUX_tsif2,
+	APQ_MUX_uim,
+	APQ_MUX_uim_batt_alarm,
+	APQ_MUX_NA,
+};
+
+static const char * const gpio_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3", "gpio4", "gpio5", "gpio6", "gpio7",
+	"gpio8", "gpio9", "gpio10", "gpio11", "gpio12", "gpio13", "gpio14",
+	"gpio15", "gpio16", "gpio17", "gpio18", "gpio19", "gpio20", "gpio21",
+	"gpio22", "gpio23", "gpio24", "gpio25", "gpio26", "gpio27", "gpio28",
+	"gpio29", "gpio30", "gpio31", "gpio32", "gpio33", "gpio34", "gpio35",
+	"gpio36", "gpio37", "gpio38", "gpio39", "gpio40", "gpio41", "gpio42",
+	"gpio43", "gpio44", "gpio45", "gpio46", "gpio47", "gpio48", "gpio49",
+	"gpio50", "gpio51", "gpio52", "gpio53", "gpio54", "gpio55", "gpio56",
+	"gpio57", "gpio58", "gpio59", "gpio60", "gpio61", "gpio62", "gpio63",
+	"gpio64", "gpio65", "gpio66", "gpio67", "gpio68", "gpio69", "gpio70",
+	"gpio71", "gpio72", "gpio73", "gpio74", "gpio75", "gpio76", "gpio77",
+	"gpio78", "gpio79", "gpio80", "gpio81", "gpio82", "gpio83", "gpio84",
+	"gpio85", "gpio86", "gpio87", "gpio88", "gpio89", "gpio90", "gpio91",
+	"gpio92", "gpio93", "gpio94", "gpio95", "gpio96", "gpio97", "gpio98",
+	"gpio99", "gpio100", "gpio101", "gpio102", "gpio103", "gpio104",
+	"gpio105", "gpio106", "gpio107", "gpio108", "gpio109", "gpio110",
+	"gpio111", "gpio112", "gpio113", "gpio114", "gpio115", "gpio116",
+	"gpio117", "gpio118", "gpio119", "gpio120", "gpio121", "gpio122",
+	"gpio123", "gpio124", "gpio125", "gpio126", "gpio127", "gpio128",
+	"gpio129", "gpio130", "gpio131", "gpio132", "gpio133", "gpio134",
+	"gpio135", "gpio136", "gpio137", "gpio138", "gpio139", "gpio140",
+	"gpio141", "gpio142", "gpio143", "gpio144", "gpio145", "gpio146"
+};
+
+static const char * const adsp_ext_groups[] = {
+	"gpio34"
+};
+static const char * const audio_ref_groups[] = {
+	"gpio100"
+};
+static const char * const blsp_i2c1_groups[] = {
+	"gpio2", "gpio3"
+};
+static const char * const blsp_i2c2_groups[] = {
+	"gpio6", "gpio7"
+};
+static const char * const blsp_i2c3_groups[] = {
+	"gpio10", "gpio11"
+};
+static const char * const blsp_i2c4_groups[] = {
+	"gpio29", "gpio30"
+};
+static const char * const blsp_i2c5_groups[] = {
+	"gpio41", "gpio42"
+};
+static const char * const blsp_i2c6_groups[] = {
+	"gpio45", "gpio46"
+};
+static const char * const blsp_i2c7_groups[] = {
+	"gpio132", "gpio133"
+};
+static const char * const blsp_i2c8_groups[] = {
+	"gpio53", "gpio54"
+};
+static const char * const blsp_i2c9_groups[] = {
+	"gpio57", "gpio58"
+};
+static const char * const blsp_i2c10_groups[] = {
+	"gpio61", "gpio62"
+};
+static const char * const blsp_i2c11_groups[] = {
+	"gpio65", "gpio66"
+};
+static const char * const blsp_i2c12_groups[] = {
+	"gpio49", "gpio50"
+};
+static const char * const blsp_spi1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3"
+};
+static const char * const blsp_spi2_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7"
+};
+static const char * const blsp_spi3_groups[] = {
+	"gpio8", "gpio9", "gpio10", "gpio11"
+};
+static const char * const blsp_spi4_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30"
+};
+static const char * const blsp_spi5_groups[] = {
+	"gpio39", "gpio40", "gpio41", "gpio42"
+};
+static const char * const blsp_spi6_groups[] = {
+	"gpio43", "gpio44", "gpio45", "gpio46"
+};
+static const char * const blsp_spi7_groups[] = {
+	"gpio130", "gpio131", "gpio132", "gpio133"
+};
+static const char * const blsp_spi8_groups[] = {
+	"gpio51", "gpio52", "gpio53", "gpio54"
+};
+static const char * const blsp_spi9_groups[] = {
+	"gpio55", "gpio56", "gpio57", "gpio58"
+};
+static const char * const blsp_spi10_groups[] = {
+	"gpio59", "gpio60", "gpio61", "gpio62"
+};
+static const char * const blsp_spi11_groups[] = {
+	"gpio63", "gpio64", "gpio65", "gpio66"
+};
+static const char * const blsp_spi12_groups[] = {
+	"gpio47", "gpio48", "gpio49", "gpio50"
+};
+static const char * const blsp_uart1_groups[] = {
+	"gpio0", "gpio1", "gpio2", "gpio3"
+};
+static const char * const blsp_uart2_groups[] = {
+	"gpio4", "gpio5", "gpio6", "gpio7"
+};
+static const char * const blsp_uart3_groups[] = {
+	"gpio8"
+};
+static const char * const blsp_uart4_groups[] = {
+	"gpio27", "gpio28", "gpio29", "gpio30"
+};
+static const char * const blsp_uart5_groups[] = {
+	"gpio39", "gpio40", "gpio41", "gpio42"
+};
+static const char * const blsp_uart6_groups[] = {
+	"gpio43", "gpio44", "gpio45", "gpio46"
+};
+static const char * const blsp_uart7_groups[] = {
+	"gpio130", "gpio131", "gpio132", "gpio133"
+};
+static const char * const blsp_uart8_groups[] = {
+	"gpio51", "gpio52", "gpio53", "gpio54"
+};
+static const char * const blsp_uart9_groups[] = {
+	"gpio55", "gpio56", "gpio57", "gpio58"
+};
+static const char * const blsp_uart10_groups[] = {
+	"gpio59", "gpio60", "gpio61", "gpio62"
+};
+static const char * const blsp_uart11_groups[] = {
+	"gpio63", "gpio64", "gpio65", "gpio66"
+};
+static const char * const blsp_uart12_groups[] = {
+	"gpio47", "gpio48", "gpio49", "gpio50"
+};
+static const char * const blsp_uim1_groups[] = {
+	"gpio0", "gpio1"
+};
+static const char * const blsp_uim2_groups[] = {
+	"gpio4", "gpio5"
+};
+static const char * const blsp_uim3_groups[] = {
+	"gpio8", "gpio9"
+};
+static const char * const blsp_uim4_groups[] = {
+	"gpio27", "gpio28"
+};
+static const char * const blsp_uim5_groups[] = {
+	"gpio39", "gpio40"
+};
+static const char * const blsp_uim6_groups[] = {
+	"gpio43", "gpio44"
+};
+static const char * const blsp_uim7_groups[] = {
+	"gpio130", "gpio131"
+};
+static const char * const blsp_uim8_groups[] = {
+	"gpio51", "gpio52"
+};
+static const char * const blsp_uim9_groups[] = {
+	"gpio55", "gpio56"
+};
+static const char * const blsp_uim10_groups[] = {
+	"gpio59", "gpio60"
+};
+static const char * const blsp_uim11_groups[] = {
+	"gpio63", "gpio64"
+};
+static const char * const blsp_uim12_groups[] = {
+	"gpio47", "gpio48"
+};
+static const char * const blsp_spi1_cs1_groups[] = {
+	"gpio116"
+};
+static const char * const blsp_spi1_cs2_groups[] = {
+	"gpio117"
+};
+static const char * const blsp_spi1_cs3_groups[] = {
+	"gpio118"
+};
+static const char * const blsp_spi3_cs1_groups[] = {
+	"gpio67"
+};
+static const char * const blsp_spi3_cs2_groups[] = {
+	"gpio71"
+};
+static const char * const blsp_spi3_cs3_groups[] = {
+	"gpio72"
+};
+static const char * const blsp_spi10_cs1_groups[] = {
+	"gpio106"
+};
+static const char * const blsp_spi10_cs2_groups[] = {
+	"gpio111"
+};
+static const char * const blsp_spi10_cs3_groups[] = {
+	"gpio128"
+};
+static const char * const cam_mclk0_groups[] = {
+	"gpio15"
+};
+static const char * const cam_mclk1_groups[] = {
+	"gpio16"
+};
+static const char * const cam_mclk2_groups[] = {
+	"gpio17"
+};
+static const char * const cam_mclk3_groups[] = {
+	"gpio18"
+};
+static const char * const cci_async_groups[] = {
+	"gpio26", "gpio119"
+};
+static const char * const cci_async_in0_groups[] = {
+	"gpio120"
+};
+static const char * const cci_i2c0_groups[] = {
+	"gpio19", "gpio20"
+};
+static const char * const cci_i2c1_groups[] = {
+	"gpio21", "gpio22"
+};
+static const char * const cci_timer0_groups[] = {
+	"gpio23"
+};
+static const char * const cci_timer1_groups[] = {
+	"gpio24"
+};
+static const char * const cci_timer2_groups[] = {
+	"gpio25"
+};
+static const char * const cci_timer3_groups[] = {
+	"gpio26"
+};
+static const char * const cci_timer4_groups[] = {
+	"gpio119"
+};
+static const char * const edp_hpd_groups[] = {
+	"gpio103"
+};
+static const char * const gcc_gp1_groups[] = {
+	"gpio37"
+};
+static const char * const gcc_gp2_groups[] = {
+	"gpio38"
+};
+static const char * const gcc_gp3_groups[] = {
+	"gpio86"
+};
+static const char * const gcc_obt_groups[] = {
+	"gpio127"
+};
+static const char * const gcc_vtt_groups[] = {
+	"gpio126"
+};
+static const char * const gp_mn_groups[] = {
+	"gpio29"
+};
+static const char * const gp_pdm0_groups[] = {
+	"gpio48", "gpio83"
+};
+static const char * const gp_pdm1_groups[] = {
+	"gpio84", "gpio101"
+};
+static const char * const gp_pdm2_groups[] = {
+	"gpio85", "gpio110"
+};
+static const char * const gp0_clk_groups[] = {
+	"gpio25"
+};
+static const char * const gp1_clk_groups[] = {
+	"gpio26"
+};
+static const char * const hdmi_cec_groups[] = {
+	"gpio31"
+};
+static const char * const hdmi_ddc_groups[] = {
+	"gpio32", "gpio33"
+};
+static const char * const hdmi_dtest_groups[] = {
+	"gpio123"
+};
+static const char * const hdmi_hpd_groups[] = {
+	"gpio34"
+};
+static const char * const hdmi_rcv_groups[] = {
+	"gpio125"
+};
+static const char * const hsic_groups[] = {
+	"gpio134", "gpio135"
+};
+static const char * const ldo_en_groups[] = {
+	"gpio124"
+};
+static const char * const ldo_update_groups[] = {
+	"gpio125"
+};
+static const char * const mdp_vsync_groups[] = {
+	"gpio12", "gpio13", "gpio14"
+};
+static const char * const pci_e0_groups[] = {
+	"gpio68", "gpio70"
+};
+static const char * const pci_e0_n_groups[] = {
+	"gpio68", "gpio70"
+};
+static const char * const pci_e0_rst_groups[] = {
+	"gpio70"
+};
+static const char * const pci_e1_groups[] = {
+	"gpio140"
+};
+static const char * const pci_e1_rst_groups[] = {
+	"gpio140"
+};
+static const char * const pci_e1_rst_n_groups[] = {
+	"gpio140"
+};
+static const char * const pci_e1_clkreq_n_groups[] = {
+	"gpio141"
+};
+static const char * const pri_mi2s_groups[] = {
+	"gpio76", "gpio77", "gpio78", "gpio79", "gpio80"
+};
+static const char * const qua_mi2s_groups[] = {
+	"gpio91", "gpio92", "gpio93", "gpio94", "gpio95", "gpio96", "gpio97"
+};
+static const char * const sata_act_groups[] = {
+	"gpio129"
+};
+static const char * const sata_devsleep_groups[] = {
+	"gpio119"
+};
+static const char * const sata_devsleep_n_groups[] = {
+	"gpio119"
+};
+static const char * const sd_write_groups[] = {
+	"gpio75"
+};
+static const char * const sdc_emmc_mode_groups[] = {
+	"gpio146"
+};
+static const char * const sdc3_groups[] = {
+	"gpio67", "gpio68", "gpio69", "gpio70", "gpio71", "gpio72"
+};
+static const char * const sdc4_groups[] = {
+	"gpio82", "gpio83", "gpio84", "gpio85", "gpio86",
+	"gpio91", "gpio95", "gpio96", "gpio97", "gpio101"
+};
+static const char * const sec_mi2s_groups[] = {
+	"gpio81", "gpio82", "gpio83", "gpio84", "gpio85"
+};
+static const char * const slimbus_groups[] = {
+	"gpio98", "gpio99"
+};
+static const char * const spdif_tx_groups[] = {
+	"gpio124", "gpio136", "gpio142"
+};
+static const char * const spkr_i2s_groups[] = {
+	"gpio98", "gpio99", "gpio100"
+};
+static const char * const spkr_i2s_ws_groups[] = {
+	"gpio104"
+};
+static const char * const spss_geni_groups[] = {
+	"gpio8", "gpio9"
+};
+static const char * const ter_mi2s_groups[] = {
+	"gpio86", "gpio87", "gpio88", "gpio89", "gpio90"
+};
+static const char * const tsif1_groups[] = {
+	"gpio82", "gpio83", "gpio84", "gpio85", "gpio86"
+};
+static const char * const tsif2_groups[] = {
+	"gpio91", "gpio95", "gpio96", "gpio97", "gpio101"
+};
+static const char * const uim_groups[] = {
+	"gpio130", "gpio131", "gpio132", "gpio133"
+};
+static const char * const uim_batt_alarm_groups[] = {
+	"gpio102"
+};
+static const struct msm_function apq8084_functions[] = {
+	FUNCTION(adsp_ext),
+	FUNCTION(audio_ref),
+	FUNCTION(blsp_i2c1),
+	FUNCTION(blsp_i2c2),
+	FUNCTION(blsp_i2c3),
+	FUNCTION(blsp_i2c4),
+	FUNCTION(blsp_i2c5),
+	FUNCTION(blsp_i2c6),
+	FUNCTION(blsp_i2c7),
+	FUNCTION(blsp_i2c8),
+	FUNCTION(blsp_i2c9),
+	FUNCTION(blsp_i2c10),
+	FUNCTION(blsp_i2c11),
+	FUNCTION(blsp_i2c12),
+	FUNCTION(blsp_spi1),
+	FUNCTION(blsp_spi1_cs1),
+	FUNCTION(blsp_spi1_cs2),
+	FUNCTION(blsp_spi1_cs3),
+	FUNCTION(blsp_spi2),
+	FUNCTION(blsp_spi3),
+	FUNCTION(blsp_spi3_cs1),
+	FUNCTION(blsp_spi3_cs2),
+	FUNCTION(blsp_spi3_cs3),
+	FUNCTION(blsp_spi4),
+	FUNCTION(blsp_spi5),
+	FUNCTION(blsp_spi6),
+	FUNCTION(blsp_spi7),
+	FUNCTION(blsp_spi8),
+	FUNCTION(blsp_spi9),
+	FUNCTION(blsp_spi10),
+	FUNCTION(blsp_spi10_cs1),
+	FUNCTION(blsp_spi10_cs2),
+	FUNCTION(blsp_spi10_cs3),
+	FUNCTION(blsp_spi11),
+	FUNCTION(blsp_spi12),
+	FUNCTION(blsp_uart1),
+	FUNCTION(blsp_uart2),
+	FUNCTION(blsp_uart3),
+	FUNCTION(blsp_uart4),
+	FUNCTION(blsp_uart5),
+	FUNCTION(blsp_uart6),
+	FUNCTION(blsp_uart7),
+	FUNCTION(blsp_uart8),
+	FUNCTION(blsp_uart9),
+	FUNCTION(blsp_uart10),
+	FUNCTION(blsp_uart11),
+	FUNCTION(blsp_uart12),
+	FUNCTION(blsp_uim1),
+	FUNCTION(blsp_uim2),
+	FUNCTION(blsp_uim3),
+	FUNCTION(blsp_uim4),
+	FUNCTION(blsp_uim5),
+	FUNCTION(blsp_uim6),
+	FUNCTION(blsp_uim7),
+	FUNCTION(blsp_uim8),
+	FUNCTION(blsp_uim9),
+	FUNCTION(blsp_uim10),
+	FUNCTION(blsp_uim11),
+	FUNCTION(blsp_uim12),
+	FUNCTION(cam_mclk0),
+	FUNCTION(cam_mclk1),
+	FUNCTION(cam_mclk2),
+	FUNCTION(cam_mclk3),
+	FUNCTION(cci_async),
+	FUNCTION(cci_async_in0),
+	FUNCTION(cci_i2c0),
+	FUNCTION(cci_i2c1),
+	FUNCTION(cci_timer0),
+	FUNCTION(cci_timer1),
+	FUNCTION(cci_timer2),
+	FUNCTION(cci_timer3),
+	FUNCTION(cci_timer4),
+	FUNCTION(edp_hpd),
+	FUNCTION(gcc_gp1),
+	FUNCTION(gcc_gp2),
+	FUNCTION(gcc_gp3),
+	FUNCTION(gcc_obt),
+	FUNCTION(gcc_vtt),
+	FUNCTION(gp_mn),
+	FUNCTION(gp_pdm0),
+	FUNCTION(gp_pdm1),
+	FUNCTION(gp_pdm2),
+	FUNCTION(gp0_clk),
+	FUNCTION(gp1_clk),
+	FUNCTION(gpio),
+	FUNCTION(hdmi_cec),
+	FUNCTION(hdmi_ddc),
+	FUNCTION(hdmi_dtest),
+	FUNCTION(hdmi_hpd),
+	FUNCTION(hdmi_rcv),
+	FUNCTION(hsic),
+	FUNCTION(ldo_en),
+	FUNCTION(ldo_update),
+	FUNCTION(mdp_vsync),
+	FUNCTION(pci_e0),
+	FUNCTION(pci_e0_n),
+	FUNCTION(pci_e0_rst),
+	FUNCTION(pci_e1),
+	FUNCTION(pci_e1_rst),
+	FUNCTION(pci_e1_rst_n),
+	FUNCTION(pci_e1_clkreq_n),
+	FUNCTION(pri_mi2s),
+	FUNCTION(qua_mi2s),
+	FUNCTION(sata_act),
+	FUNCTION(sata_devsleep),
+	FUNCTION(sata_devsleep_n),
+	FUNCTION(sd_write),
+	FUNCTION(sdc_emmc_mode),
+	FUNCTION(sdc3),
+	FUNCTION(sdc4),
+	FUNCTION(sec_mi2s),
+	FUNCTION(slimbus),
+	FUNCTION(spdif_tx),
+	FUNCTION(spkr_i2s),
+	FUNCTION(spkr_i2s_ws),
+	FUNCTION(spss_geni),
+	FUNCTION(ter_mi2s),
+	FUNCTION(tsif1),
+	FUNCTION(tsif2),
+	FUNCTION(uim),
+	FUNCTION(uim_batt_alarm),
+};
+
+static const struct msm_pingroup apq8084_groups[] = {
+	PINGROUP(0,   blsp_spi1, blsp_uart1, blsp_uim1, NA, NA, NA, NA),
+	PINGROUP(1,   blsp_spi1, blsp_uart1, blsp_uim1, NA, NA, NA, NA),
+	PINGROUP(2,   blsp_spi1, blsp_uart1, blsp_i2c1, NA, NA, NA, NA),
+	PINGROUP(3,   blsp_spi1, blsp_uart1, blsp_i2c1, NA, NA, NA, NA),
+	PINGROUP(4,   blsp_spi2, blsp_uart2, blsp_uim2, NA, NA, NA, NA),
+	PINGROUP(5,   blsp_spi2, blsp_uart2, blsp_uim2, NA, NA, NA, NA),
+	PINGROUP(6,   blsp_spi2, blsp_uart2, blsp_i2c2, NA, NA, NA, NA),
+	PINGROUP(7,   blsp_spi2, blsp_uart2, blsp_i2c2, NA, NA, NA, NA),
+	PINGROUP(8,   blsp_spi3, blsp_uart3, blsp_uim3, spss_geni, NA, NA, NA),
+	PINGROUP(9,   blsp_spi3, blsp_uim3, blsp_uart3, spss_geni, NA, NA, NA),
+	PINGROUP(10,  blsp_spi3, blsp_uart3, blsp_i2c3, NA, NA, NA, NA),
+	PINGROUP(11,  blsp_spi3, blsp_uart3, blsp_i2c3, NA, NA, NA, NA),
+	PINGROUP(12,  mdp_vsync, NA, NA, NA, NA, NA, NA),
+	PINGROUP(13,  mdp_vsync, NA, NA, NA, NA, NA, NA),
+	PINGROUP(14,  mdp_vsync, NA, NA, NA, NA, NA, NA),
+	PINGROUP(15,  cam_mclk0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(16,  cam_mclk1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(17,  cam_mclk2, NA, NA, NA, NA, NA, NA),
+	PINGROUP(18,  cam_mclk3, NA, NA, NA, NA, NA, NA),
+	PINGROUP(19,  cci_i2c0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(20,  cci_i2c0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(21,  cci_i2c1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(22,  cci_i2c1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(23,  cci_timer0, NA, NA, NA, NA, NA, NA),
+	PINGROUP(24,  cci_timer1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(25,  cci_timer2, gp0_clk, NA, NA, NA, NA, NA),
+	PINGROUP(26,  cci_timer3, cci_async, gp1_clk, NA, NA, NA, NA),
+	PINGROUP(27,  blsp_spi4, blsp_uart4, blsp_uim4, NA, NA, NA, NA),
+	PINGROUP(28,  blsp_spi4, blsp_uart4, blsp_uim4, NA, NA, NA, NA),
+	PINGROUP(29,  blsp_spi4, blsp_uart4, blsp_i2c4, gp_mn, NA, NA, NA),
+	PINGROUP(30,  blsp_spi4, blsp_uart4, blsp_i2c4, NA, NA, NA, NA),
+	PINGROUP(31,  hdmi_cec, NA, NA, NA, NA, NA, NA),
+	PINGROUP(32,  hdmi_ddc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(33,  hdmi_ddc, NA, NA, NA, NA, NA, NA),
+	PINGROUP(34,  hdmi_hpd, NA, adsp_ext, NA, NA, NA, NA),
+	PINGROUP(35,  NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(36,  NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(37,  gcc_gp1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(38,  gcc_gp2, NA, NA, NA, NA, NA, NA),
+	PINGROUP(39,  blsp_spi5, blsp_uart5, blsp_uim5, NA, NA, NA, NA),
+	PINGROUP(40,  blsp_spi5, blsp_uart5, blsp_uim5, NA, NA, NA, NA),
+	PINGROUP(41,  blsp_spi5, blsp_uart5, blsp_i2c5, NA, NA, NA, NA),
+	PINGROUP(42,  blsp_spi5, blsp_uart5, blsp_i2c5, NA, NA, NA, NA),
+	PINGROUP(43,  blsp_spi6, blsp_uart6, blsp_uim6, NA, NA, NA, NA),
+	PINGROUP(44,  blsp_spi6, blsp_uart6, blsp_uim6, NA, NA, NA, NA),
+	PINGROUP(45,  blsp_spi6, blsp_uart6, blsp_i2c6, NA, NA, NA, NA),
+	PINGROUP(46,  blsp_spi6, blsp_uart6, blsp_i2c6, NA, NA, NA, NA),
+	PINGROUP(47,  blsp_spi12, blsp_uart12, blsp_uim12, NA, NA, NA, NA),
+	PINGROUP(48,  blsp_spi12, blsp_uart12, blsp_uim12, gp_pdm0, NA, NA, NA),
+	PINGROUP(49,  blsp_spi12, blsp_uart12, blsp_i2c12, NA, NA, NA, NA),
+	PINGROUP(50,  blsp_spi12, blsp_uart12, blsp_i2c12, NA, NA, NA, NA),
+	PINGROUP(51,  blsp_spi8, blsp_uart8, blsp_uim8, NA, NA, NA, NA),
+	PINGROUP(52,  blsp_spi8, blsp_uart8, blsp_uim8, NA, NA, NA, NA),
+	PINGROUP(53,  blsp_spi8, blsp_uart8, blsp_i2c8, NA, NA, NA, NA),
+	PINGROUP(54,  blsp_spi8, blsp_uart8, blsp_i2c8, NA, NA, NA, NA),
+	PINGROUP(55,  blsp_spi9, blsp_uart9, blsp_uim9, NA, NA, NA, NA),
+	PINGROUP(56,  blsp_spi9, blsp_uart9, blsp_uim9, NA, NA, NA, NA),
+	PINGROUP(57,  blsp_spi9, blsp_uart9, blsp_i2c9, NA, NA, NA, NA),
+	PINGROUP(58,  blsp_spi9, blsp_uart9, blsp_i2c9, NA, NA, NA, NA),
+	PINGROUP(59,  blsp_spi10, blsp_uart10, blsp_uim10, NA, NA, NA, NA),
+	PINGROUP(60,  blsp_spi10, blsp_uart10, blsp_uim10, NA, NA, NA, NA),
+	PINGROUP(61,  blsp_spi10, blsp_uart10, blsp_i2c10, NA, NA, NA, NA),
+	PINGROUP(62,  blsp_spi10, blsp_uart10, blsp_i2c10, NA, NA, NA, NA),
+	PINGROUP(63,  blsp_spi11, blsp_uart11, blsp_uim11, NA, NA, NA, NA),
+	PINGROUP(64,  blsp_spi11, blsp_uart11, blsp_uim11, NA, NA, NA, NA),
+	PINGROUP(65,  blsp_spi11, blsp_uart11, blsp_i2c11, NA, NA, NA, NA),
+	PINGROUP(66,  blsp_spi11, blsp_uart11, blsp_i2c11, NA, NA, NA, NA),
+	PINGROUP(67,  sdc3, blsp_spi3_cs1, NA, NA, NA, NA, NA),
+	PINGROUP(68,  sdc3, pci_e0, NA, NA, NA, NA, NA),
+	PINGROUP(69,  sdc3, NA, NA, NA, NA, NA, NA),
+	PINGROUP(70,  sdc3, pci_e0_n, pci_e0, NA, NA, NA, NA),
+	PINGROUP(71,  sdc3, blsp_spi3_cs2, NA, NA, NA, NA, NA),
+	PINGROUP(72,  sdc3, blsp_spi3_cs3, NA, NA, NA, NA, NA),
+	PINGROUP(73,  NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(74,  NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(75,  sd_write, NA, NA, NA, NA, NA, NA),
+	PINGROUP(76,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(77,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(78,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(79,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(80,  pri_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(81,  sec_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(82,  sec_mi2s, sdc4, tsif1, NA, NA, NA, NA),
+	PINGROUP(83,  sec_mi2s, sdc4, tsif1, NA, NA, NA, gp_pdm0),
+	PINGROUP(84,  sec_mi2s, sdc4, tsif1, NA, NA, NA, gp_pdm1),
+	PINGROUP(85,  sec_mi2s, sdc4, tsif1, NA, gp_pdm2, NA, NA),
+	PINGROUP(86,  ter_mi2s, sdc4, tsif1, NA, NA, NA, gcc_gp3),
+	PINGROUP(87,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(88,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(89,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(90,  ter_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(91,  qua_mi2s, sdc4, tsif2, NA, NA, NA, NA),
+	PINGROUP(92,  qua_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(93,  qua_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(94,  qua_mi2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(95,  qua_mi2s, sdc4, tsif2, NA, NA, NA, gcc_gp1),
+	PINGROUP(96,  qua_mi2s, sdc4, tsif2, NA, NA, NA, gcc_gp2),
+	PINGROUP(97,  qua_mi2s, sdc4, tsif2, NA, gcc_gp3, NA, NA),
+	PINGROUP(98,  slimbus, spkr_i2s, NA, NA, NA, NA, NA),
+	PINGROUP(99,  slimbus, spkr_i2s, NA, NA, NA, NA, NA),
+	PINGROUP(100, audio_ref, spkr_i2s, NA, NA, NA, NA, NA),
+	PINGROUP(101, sdc4, tsif2, gp_pdm1, NA, NA, NA, NA),
+	PINGROUP(102, uim_batt_alarm, NA, NA, NA, NA, NA, NA),
+	PINGROUP(103, edp_hpd, NA, NA, NA, NA, NA, NA),
+	PINGROUP(104, spkr_i2s, NA, NA, NA, NA, NA, NA),
+	PINGROUP(105, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(106, blsp_spi10_cs1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(107, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(108, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(109, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(110, gp_pdm2, NA, NA, NA, NA, NA, NA),
+	PINGROUP(111, blsp_spi10_cs2, NA, NA, NA, NA, NA, NA),
+	PINGROUP(112, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(113, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(114, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(115, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(116, blsp_spi1_cs1, NA, NA, NA, NA, NA, NA),
+	PINGROUP(117, blsp_spi1_cs2, NA, NA, NA, NA, NA, NA),
+	PINGROUP(118, blsp_spi1_cs3, NA, NA, NA, NA, NA, NA),
+	PINGROUP(119, cci_timer4, cci_async, sata_devsleep, sata_devsleep_n, NA, NA, NA),
+	PINGROUP(120, cci_async, NA, NA, NA, NA, NA, NA),
+	PINGROUP(121, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(122, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(123, hdmi_dtest, NA, NA, NA, NA, NA, NA),
+	PINGROUP(124, spdif_tx, ldo_en, NA, NA, NA, NA, NA),
+	PINGROUP(125, ldo_update, hdmi_rcv, NA, NA, NA, NA, NA),
+	PINGROUP(126, gcc_vtt, NA, NA, NA, NA, NA, NA),
+	PINGROUP(127, gcc_obt, NA, NA, NA, NA, NA, NA),
+	PINGROUP(128, blsp_spi10_cs3, NA, NA, NA, NA, NA, NA),
+	PINGROUP(129, sata_act, NA, NA, NA, NA, NA, NA),
+	PINGROUP(130, uim, blsp_spi7, blsp_uart7, blsp_uim7, NA, NA, NA),
+	PINGROUP(131, uim, blsp_spi7, blsp_uart7, blsp_uim7, NA, NA, NA),
+	PINGROUP(132, uim, blsp_spi7, blsp_uart7, blsp_i2c7, NA, NA, NA),
+	PINGROUP(133, uim, blsp_spi7, blsp_uart7, blsp_i2c7, NA, NA, NA),
+	PINGROUP(134, hsic, NA, NA, NA, NA, NA, NA),
+	PINGROUP(135, hsic, NA, NA, NA, NA, NA, NA),
+	PINGROUP(136, spdif_tx, NA, NA, NA, NA, NA, NA),
+	PINGROUP(137, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(138, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(139, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(140, pci_e1_rst_n, pci_e1_rst, NA, NA, NA, NA, NA),
+	PINGROUP(141, pci_e1_clkreq_n, NA, NA, NA, NA, NA, NA),
+	PINGROUP(142, spdif_tx, NA, NA, NA, NA, NA, NA),
+	PINGROUP(143, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(144, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(145, NA, NA, NA, NA, NA, NA, NA),
+	PINGROUP(146, sdc_emmc_mode, NA, NA, NA, NA, NA, NA),
+
+	SDC_PINGROUP(sdc1_clk, 0x2044, 13, 6),
+	SDC_PINGROUP(sdc1_cmd, 0x2044, 11, 3),
+	SDC_PINGROUP(sdc1_data, 0x2044, 9, 0),
+	SDC_PINGROUP(sdc2_clk, 0x2048, 14, 6),
+	SDC_PINGROUP(sdc2_cmd, 0x2048, 11, 3),
+	SDC_PINGROUP(sdc2_data, 0x2048, 9, 0),
+};
+
+#define NUM_GPIO_PINGROUPS 147
+
+static const struct msm_pinctrl_soc_data apq8084_pinctrl = {
+	.pins = apq8084_pins,
+	.npins = ARRAY_SIZE(apq8084_pins),
+	.functions = apq8084_functions,
+	.nfunctions = ARRAY_SIZE(apq8084_functions),
+	.groups = apq8084_groups,
+	.ngroups = ARRAY_SIZE(apq8084_groups),
+	.ngpios = NUM_GPIO_PINGROUPS,
+};
+
+static int apq8084_pinctrl_probe(struct platform_device *pdev)
+{
+	return msm_pinctrl_probe(pdev, &apq8084_pinctrl);
+}
+
+static const struct of_device_id apq8084_pinctrl_of_match[] = {
+	{ .compatible = "qcom,apq8084-pinctrl", },
+	{ },
+};
+
+static struct platform_driver apq8084_pinctrl_driver = {
+	.driver = {
+		.name = "apq8084-pinctrl",
+		.owner = THIS_MODULE,
+		.of_match_table = apq8084_pinctrl_of_match,
+	},
+	.probe = apq8084_pinctrl_probe,
+	.remove = msm_pinctrl_remove,
+};
+
+static int __init apq8084_pinctrl_init(void)
+{
+	return platform_driver_register(&apq8084_pinctrl_driver);
+}
+arch_initcall(apq8084_pinctrl_init);
+
+static void __exit apq8084_pinctrl_exit(void)
+{
+	platform_driver_unregister(&apq8084_pinctrl_driver);
+}
+module_exit(apq8084_pinctrl_exit);
+
+MODULE_DESCRIPTION("Qualcomm APQ8084 pinctrl driver");
+MODULE_LICENSE("GPL v2");
+MODULE_DEVICE_TABLE(of, apq8084_pinctrl_of_match);
diff --git a/drivers/pinctrl/qcom/pinctrl-ipq8064.c b/drivers/pinctrl/qcom/pinctrl-ipq8064.c
index 767cf1120b20..81f49a9b4dbe 100644
--- a/drivers/pinctrl/qcom/pinctrl-ipq8064.c
+++ b/drivers/pinctrl/qcom/pinctrl-ipq8064.c
@@ -211,6 +211,7 @@ static const unsigned int sdc3_data_pins[] = { 71 };
 		.intr_status_bit = 0,			\
 		.intr_ack_high = 1,			\
 		.intr_target_bit = 0,			\
+		.intr_target_kpss_val = 4,		\
 		.intr_raw_status_bit = 3,		\
 		.intr_polarity_bit = 1,			\
 		.intr_detection_bit = 2,		\
@@ -236,6 +237,7 @@ static const unsigned int sdc3_data_pins[] = { 71 };
 		.intr_enable_bit = -1,                  \
 		.intr_status_bit = -1,                  \
 		.intr_target_bit = -1,                  \
+		.intr_target_kpss_val = -1,		\
 		.intr_raw_status_bit = -1,              \
 		.intr_polarity_bit = -1,                \
 		.intr_detection_bit = -1,               \
diff --git a/drivers/pinctrl/qcom/pinctrl-msm.c b/drivers/pinctrl/qcom/pinctrl-msm.c
index 2738108caff2..e730935fa457 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm.c
@@ -12,6 +12,7 @@
  * GNU General Public License for more details.
  */
 
+#include <linux/delay.h>
 #include <linux/err.h>
 #include <linux/io.h>
 #include <linux/module.h>
@@ -26,6 +27,7 @@
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
 #include <linux/spinlock.h>
+#include <linux/reboot.h>
 
 #include "../core.h"
 #include "../pinconf.h"
@@ -33,12 +35,14 @@
 #include "../pinctrl-utils.h"
 
 #define MAX_NR_GPIO 300
+#define PS_HOLD_OFFSET 0x820
 
 /**
  * struct msm_pinctrl - state for a pinctrl-msm device
  * @dev:            device handle.
  * @pctrl:          pinctrl handle.
  * @chip:           gpiochip handle.
+ * @restart_nb:     restart notifier block.
  * @irq:            parent irq for the TLMM irq_chip.
  * @lock:           Spinlock to protect register resources as well
  *                  as msm_pinctrl data structures.
@@ -52,6 +56,7 @@ struct msm_pinctrl {
 	struct device *dev;
 	struct pinctrl_dev *pctrl;
 	struct gpio_chip chip;
+	struct notifier_block restart_nb;
 	int irq;
 
 	spinlock_t lock;
@@ -130,9 +135,9 @@ static int msm_get_function_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int msm_pinmux_enable(struct pinctrl_dev *pctldev,
-			     unsigned function,
-			     unsigned group)
+static int msm_pinmux_set_mux(struct pinctrl_dev *pctldev,
+			      unsigned function,
+			      unsigned group)
 {
 	struct msm_pinctrl *pctrl = pinctrl_dev_get_drvdata(pctldev);
 	const struct msm_pingroup *g;
@@ -166,7 +171,7 @@ static const struct pinmux_ops msm_pinmux_ops = {
 	.get_functions_count	= msm_get_functions_count,
 	.get_function_name	= msm_get_function_name,
 	.get_function_groups	= msm_get_function_groups,
-	.enable			= msm_pinmux_enable,
+	.set_mux		= msm_pinmux_set_mux,
 };
 
 static int msm_config_reg(struct msm_pinctrl *pctrl,
@@ -649,8 +654,6 @@ static void msm_gpio_irq_ack(struct irq_data *d)
 	spin_unlock_irqrestore(&pctrl->lock, flags);
 }
 
-#define INTR_TARGET_PROC_APPS    4
-
 static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 {
 	struct gpio_chip *gc = irq_data_get_irq_chip_data(d);
@@ -674,7 +677,7 @@ static int msm_gpio_irq_set_type(struct irq_data *d, unsigned int type)
 	/* Route interrupts to application cpu */
 	val = readl(pctrl->regs + g->intr_target_reg);
 	val &= ~(7 << g->intr_target_bit);
-	val |= INTR_TARGET_PROC_APPS << g->intr_target_bit;
+	val |= g->intr_target_kpss_val << g->intr_target_bit;
 	writel(val, pctrl->regs + g->intr_target_reg);
 
 	/* Update configuration for gpio.
@@ -829,6 +832,7 @@ static int msm_gpio_init(struct msm_pinctrl *pctrl)
 	ret = gpiochip_add_pin_range(&pctrl->chip, dev_name(pctrl->dev), 0, 0, chip->ngpio);
 	if (ret) {
 		dev_err(pctrl->dev, "Failed to add pin range\n");
+		gpiochip_remove(&pctrl->chip);
 		return ret;
 	}
 
@@ -839,6 +843,7 @@ static int msm_gpio_init(struct msm_pinctrl *pctrl)
 				   IRQ_TYPE_NONE);
 	if (ret) {
 		dev_err(pctrl->dev, "Failed to add irqchip to gpiochip\n");
+		gpiochip_remove(&pctrl->chip);
 		return -ENOSYS;
 	}
 
@@ -848,6 +853,32 @@ static int msm_gpio_init(struct msm_pinctrl *pctrl)
 	return 0;
 }
 
+static int msm_ps_hold_restart(struct notifier_block *nb, unsigned long action,
+			       void *data)
+{
+	struct msm_pinctrl *pctrl = container_of(nb, struct msm_pinctrl, restart_nb);
+
+	writel(0, pctrl->regs + PS_HOLD_OFFSET);
+	mdelay(1000);
+	return NOTIFY_DONE;
+}
+
+static void msm_pinctrl_setup_pm_reset(struct msm_pinctrl *pctrl)
+{
+	int i = 0;
+	const struct msm_function *func = pctrl->soc->functions;
+
+	for (; i <= pctrl->soc->nfunctions; i++)
+		if (!strcmp(func[i].name, "ps_hold")) {
+			pctrl->restart_nb.notifier_call = msm_ps_hold_restart;
+			pctrl->restart_nb.priority = 128;
+			if (register_restart_handler(&pctrl->restart_nb))
+				dev_err(pctrl->dev,
+					"failed to setup restart handler.\n");
+			break;
+		}
+}
+
 int msm_pinctrl_probe(struct platform_device *pdev,
 		      const struct msm_pinctrl_soc_data *soc_data)
 {
@@ -871,6 +902,8 @@ int msm_pinctrl_probe(struct platform_device *pdev,
 	if (IS_ERR(pctrl->regs))
 		return PTR_ERR(pctrl->regs);
 
+	msm_pinctrl_setup_pm_reset(pctrl);
+
 	pctrl->irq = platform_get_irq(pdev, 0);
 	if (pctrl->irq < 0) {
 		dev_err(&pdev->dev, "No interrupt defined for msmgpio\n");
@@ -903,16 +936,12 @@ EXPORT_SYMBOL(msm_pinctrl_probe);
 int msm_pinctrl_remove(struct platform_device *pdev)
 {
 	struct msm_pinctrl *pctrl = platform_get_drvdata(pdev);
-	int ret;
-
-	ret = gpiochip_remove(&pctrl->chip);
-	if (ret) {
-		dev_err(&pdev->dev, "Failed to remove gpiochip\n");
-		return ret;
-	}
 
+	gpiochip_remove(&pctrl->chip);
 	pinctrl_unregister(pctrl->pctrl);
 
+	unregister_restart_handler(&pctrl->restart_nb);
+
 	return 0;
 }
 EXPORT_SYMBOL(msm_pinctrl_remove);
diff --git a/drivers/pinctrl/qcom/pinctrl-msm.h b/drivers/pinctrl/qcom/pinctrl-msm.h
index 7b2a227a590a..b952c4b4a8e9 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm.h
+++ b/drivers/pinctrl/qcom/pinctrl-msm.h
@@ -53,6 +53,8 @@ struct msm_function {
  * @intr_status_bit:      Offset in @intr_status_reg for reading and acking the interrupt
  *                        status.
  * @intr_target_bit:      Offset in @intr_target_reg for configuring the interrupt routing.
+ * @intr_target_kpss_val: Value in @intr_target_bit for specifying that the interrupt from
+ *                        this gpio should get routed to the KPSS processor.
  * @intr_raw_status_bit:  Offset in @intr_cfg_reg for the raw status bit.
  * @intr_polarity_bit:    Offset in @intr_cfg_reg for specifying polarity of the interrupt.
  * @intr_detection_bit:   Offset in @intr_cfg_reg for specifying interrupt type.
@@ -88,6 +90,7 @@ struct msm_pingroup {
 	unsigned intr_ack_high:1;
 
 	unsigned intr_target_bit:5;
+	unsigned intr_target_kpss_val:5;
 	unsigned intr_raw_status_bit:5;
 	unsigned intr_polarity_bit:5;
 	unsigned intr_detection_bit:5;
diff --git a/drivers/pinctrl/qcom/pinctrl-msm8960.c b/drivers/pinctrl/qcom/pinctrl-msm8960.c
index 35047036a053..2ab21ce5575a 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm8960.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm8960.c
@@ -384,6 +384,7 @@ static const unsigned int sdc3_data_pins[] = { 157 };
 		.intr_status_bit = 0,			\
 		.intr_ack_high = 1,			\
 		.intr_target_bit = 0,			\
+		.intr_target_kpss_val = 4,		\
 		.intr_raw_status_bit = 3,		\
 		.intr_polarity_bit = 1,			\
 		.intr_detection_bit = 2,		\
@@ -409,6 +410,7 @@ static const unsigned int sdc3_data_pins[] = { 157 };
 		.intr_enable_bit = -1,			\
 		.intr_status_bit = -1,			\
 		.intr_target_bit = -1,			\
+		.intr_target_kpss_val = -1,		\
 		.intr_raw_status_bit = -1,		\
 		.intr_polarity_bit = -1,		\
 		.intr_detection_bit = -1,		\
diff --git a/drivers/pinctrl/qcom/pinctrl-msm8x74.c b/drivers/pinctrl/qcom/pinctrl-msm8x74.c
index 8c9720154d1e..3c858384d041 100644
--- a/drivers/pinctrl/qcom/pinctrl-msm8x74.c
+++ b/drivers/pinctrl/qcom/pinctrl-msm8x74.c
@@ -366,6 +366,7 @@ static const unsigned int sdc2_data_pins[] = { 151 };
 		.intr_enable_bit = 0,			\
 		.intr_status_bit = 0,			\
 		.intr_target_bit = 5,			\
+		.intr_target_kpss_val = 4,		\
 		.intr_raw_status_bit = 4,		\
 		.intr_polarity_bit = 1,			\
 		.intr_detection_bit = 2,		\
@@ -391,6 +392,7 @@ static const unsigned int sdc2_data_pins[] = { 151 };
 		.intr_enable_bit = -1,			\
 		.intr_status_bit = -1,			\
 		.intr_target_bit = -1,			\
+		.intr_target_kpss_val = -1,		\
 		.intr_raw_status_bit = -1,		\
 		.intr_polarity_bit = -1,		\
 		.intr_detection_bit = -1,		\
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos.c b/drivers/pinctrl/samsung/pinctrl-exynos.c
index 003bfd874a61..d7154ed0b0eb 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos.c
@@ -127,14 +127,10 @@ static int exynos_irq_set_type(struct irq_data *irqd, unsigned int type)
 	struct irq_chip *chip = irq_data_get_irq_chip(irqd);
 	struct exynos_irq_chip *our_chip = to_exynos_irq_chip(chip);
 	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
-	struct samsung_pin_bank_type *bank_type = bank->type;
 	struct samsung_pinctrl_drv_data *d = bank->drvdata;
-	unsigned int pin = irqd->hwirq;
-	unsigned int shift = EXYNOS_EINT_CON_LEN * pin;
+	unsigned int shift = EXYNOS_EINT_CON_LEN * irqd->hwirq;
 	unsigned int con, trig_type;
 	unsigned long reg_con = our_chip->eint_con + bank->eint_offset;
-	unsigned long flags;
-	unsigned int mask;
 
 	switch (type) {
 	case IRQ_TYPE_EDGE_RISING:
@@ -167,8 +163,32 @@ static int exynos_irq_set_type(struct irq_data *irqd, unsigned int type)
 	con |= trig_type << shift;
 	writel(con, d->virt_base + reg_con);
 
+	return 0;
+}
+
+static int exynos_irq_request_resources(struct irq_data *irqd)
+{
+	struct irq_chip *chip = irq_data_get_irq_chip(irqd);
+	struct exynos_irq_chip *our_chip = to_exynos_irq_chip(chip);
+	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
+	struct samsung_pin_bank_type *bank_type = bank->type;
+	struct samsung_pinctrl_drv_data *d = bank->drvdata;
+	unsigned int shift = EXYNOS_EINT_CON_LEN * irqd->hwirq;
+	unsigned long reg_con = our_chip->eint_con + bank->eint_offset;
+	unsigned long flags;
+	unsigned int mask;
+	unsigned int con;
+	int ret;
+
+	ret = gpio_lock_as_irq(&bank->gpio_chip, irqd->hwirq);
+	if (ret) {
+		dev_err(bank->gpio_chip.dev, "unable to lock pin %s-%lu IRQ\n",
+			bank->name, irqd->hwirq);
+		return ret;
+	}
+
 	reg_con = bank->pctl_offset + bank_type->reg_offset[PINCFG_TYPE_FUNC];
-	shift = pin * bank_type->fld_width[PINCFG_TYPE_FUNC];
+	shift = irqd->hwirq * bank_type->fld_width[PINCFG_TYPE_FUNC];
 	mask = (1 << bank_type->fld_width[PINCFG_TYPE_FUNC]) - 1;
 
 	spin_lock_irqsave(&bank->slock, flags);
@@ -180,9 +200,42 @@ static int exynos_irq_set_type(struct irq_data *irqd, unsigned int type)
 
 	spin_unlock_irqrestore(&bank->slock, flags);
 
+	exynos_irq_unmask(irqd);
+
 	return 0;
 }
 
+static void exynos_irq_release_resources(struct irq_data *irqd)
+{
+	struct irq_chip *chip = irq_data_get_irq_chip(irqd);
+	struct exynos_irq_chip *our_chip = to_exynos_irq_chip(chip);
+	struct samsung_pin_bank *bank = irq_data_get_irq_chip_data(irqd);
+	struct samsung_pin_bank_type *bank_type = bank->type;
+	struct samsung_pinctrl_drv_data *d = bank->drvdata;
+	unsigned int shift = EXYNOS_EINT_CON_LEN * irqd->hwirq;
+	unsigned long reg_con = our_chip->eint_con + bank->eint_offset;
+	unsigned long flags;
+	unsigned int mask;
+	unsigned int con;
+
+	reg_con = bank->pctl_offset + bank_type->reg_offset[PINCFG_TYPE_FUNC];
+	shift = irqd->hwirq * bank_type->fld_width[PINCFG_TYPE_FUNC];
+	mask = (1 << bank_type->fld_width[PINCFG_TYPE_FUNC]) - 1;
+
+	exynos_irq_mask(irqd);
+
+	spin_lock_irqsave(&bank->slock, flags);
+
+	con = readl(d->virt_base + reg_con);
+	con &= ~(mask << shift);
+	con |= FUNC_INPUT << shift;
+	writel(con, d->virt_base + reg_con);
+
+	spin_unlock_irqrestore(&bank->slock, flags);
+
+	gpio_unlock_as_irq(&bank->gpio_chip, irqd->hwirq);
+}
+
 /*
  * irq_chip for gpio interrupts.
  */
@@ -193,6 +246,8 @@ static struct exynos_irq_chip exynos_gpio_irq_chip = {
 		.irq_mask = exynos_irq_mask,
 		.irq_ack = exynos_irq_ack,
 		.irq_set_type = exynos_irq_set_type,
+		.irq_request_resources = exynos_irq_request_resources,
+		.irq_release_resources = exynos_irq_release_resources,
 	},
 	.eint_con = EXYNOS_GPIO_ECON_OFFSET,
 	.eint_mask = EXYNOS_GPIO_EMASK_OFFSET,
@@ -336,6 +391,8 @@ static struct exynos_irq_chip exynos_wkup_irq_chip = {
 		.irq_ack = exynos_irq_ack,
 		.irq_set_type = exynos_irq_set_type,
 		.irq_set_wake = exynos_wkup_irq_set_wake,
+		.irq_request_resources = exynos_irq_request_resources,
+		.irq_release_resources = exynos_irq_release_resources,
 	},
 	.eint_con = EXYNOS_WKUP_ECON_OFFSET,
 	.eint_mask = EXYNOS_WKUP_EMASK_OFFSET,
diff --git a/drivers/pinctrl/samsung/pinctrl-exynos5440.c b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
index 603da2f9dd95..88acfc0efd54 100644
--- a/drivers/pinctrl/samsung/pinctrl-exynos5440.c
+++ b/drivers/pinctrl/samsung/pinctrl-exynos5440.c
@@ -364,8 +364,9 @@ static void exynos5440_pinmux_setup(struct pinctrl_dev *pctldev, unsigned select
 }
 
 /* enable a specified pinmux by writing to registers */
-static int exynos5440_pinmux_enable(struct pinctrl_dev *pctldev, unsigned selector,
-					unsigned group)
+static int exynos5440_pinmux_set_mux(struct pinctrl_dev *pctldev,
+				     unsigned selector,
+				     unsigned group)
 {
 	exynos5440_pinmux_setup(pctldev, selector, group, true);
 	return 0;
@@ -387,7 +388,7 @@ static const struct pinmux_ops exynos5440_pinmux_ops = {
 	.get_functions_count	= exynos5440_get_functions_count,
 	.get_function_name	= exynos5440_pinmux_get_fname,
 	.get_function_groups	= exynos5440_pinmux_get_groups,
-	.enable			= exynos5440_pinmux_enable,
+	.set_mux		= exynos5440_pinmux_set_mux,
 	.gpio_set_direction	= exynos5440_pinmux_gpio_set_direction,
 };
 
@@ -873,11 +874,7 @@ static int exynos5440_gpiolib_register(struct platform_device *pdev,
 static int exynos5440_gpiolib_unregister(struct platform_device *pdev,
 				struct exynos5440_pinctrl_priv_data *priv)
 {
-	int ret = gpiochip_remove(priv->gc);
-	if (ret) {
-		dev_err(&pdev->dev, "gpio chip remove failed\n");
-		return ret;
-	}
+	gpiochip_remove(priv->gc);
 	return 0;
 }
 
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.c b/drivers/pinctrl/samsung/pinctrl-samsung.c
index b07406da333c..2d37c8f49f3c 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.c
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.c
@@ -401,8 +401,9 @@ static void samsung_pinmux_setup(struct pinctrl_dev *pctldev, unsigned selector,
 }
 
 /* enable a specified pinmux by writing to registers */
-static int samsung_pinmux_enable(struct pinctrl_dev *pctldev, unsigned selector,
-					unsigned group)
+static int samsung_pinmux_set_mux(struct pinctrl_dev *pctldev,
+				  unsigned selector,
+				  unsigned group)
 {
 	samsung_pinmux_setup(pctldev, selector, group, true);
 	return 0;
@@ -413,7 +414,7 @@ static const struct pinmux_ops samsung_pinmux_ops = {
 	.get_functions_count	= samsung_get_functions_count,
 	.get_function_name	= samsung_pinmux_get_fname,
 	.get_function_groups	= samsung_pinmux_get_groups,
-	.enable			= samsung_pinmux_enable,
+	.set_mux		= samsung_pinmux_set_mux,
 };
 
 /* set or get the pin config settings for a specified pin */
@@ -945,9 +946,7 @@ static int samsung_gpiolib_register(struct platform_device *pdev,
 
 fail:
 	for (--i, --bank; i >= 0; --i, --bank)
-		if (gpiochip_remove(&bank->gpio_chip))
-			dev_err(&pdev->dev, "gpio chip %s remove failed\n",
-							bank->gpio_chip.label);
+		gpiochip_remove(&bank->gpio_chip);
 	return ret;
 }
 
@@ -957,16 +956,11 @@ static int samsung_gpiolib_unregister(struct platform_device *pdev,
 {
 	struct samsung_pin_ctrl *ctrl = drvdata->ctrl;
 	struct samsung_pin_bank *bank = ctrl->pin_banks;
-	int ret = 0;
 	int i;
 
-	for (i = 0; !ret && i < ctrl->nr_banks; ++i, ++bank)
-		ret = gpiochip_remove(&bank->gpio_chip);
-
-	if (ret)
-		dev_err(&pdev->dev, "gpio chip remove failed\n");
-
-	return ret;
+	for (i = 0; i < ctrl->nr_banks; ++i, ++bank)
+		gpiochip_remove(&bank->gpio_chip);
+	return 0;
 }
 
 static const struct of_device_id samsung_pinctrl_dt_match[];
diff --git a/drivers/pinctrl/samsung/pinctrl-samsung.h b/drivers/pinctrl/samsung/pinctrl-samsung.h
index 2b882320e8e9..5cedc9d26390 100644
--- a/drivers/pinctrl/samsung/pinctrl-samsung.h
+++ b/drivers/pinctrl/samsung/pinctrl-samsung.h
@@ -26,6 +26,7 @@
 #include <linux/gpio.h>
 
 /* pinmux function number for pin as gpio output line */
+#define FUNC_INPUT	0x0
 #define FUNC_OUTPUT	0x1
 
 /**
diff --git a/drivers/pinctrl/sh-pfc/core.c b/drivers/pinctrl/sh-pfc/core.c
index b9b464d0578c..6572c233f73d 100644
--- a/drivers/pinctrl/sh-pfc/core.c
+++ b/drivers/pinctrl/sh-pfc/core.c
@@ -542,7 +542,7 @@ static int sh_pfc_probe(struct platform_device *pdev)
 	 */
 	ret = sh_pfc_register_pinctrl(pfc);
 	if (unlikely(ret != 0))
-		goto error;
+		return ret;
 
 #ifdef CONFIG_GPIO_SH_PFC
 	/*
@@ -564,11 +564,6 @@ static int sh_pfc_probe(struct platform_device *pdev)
 	dev_info(pfc->dev, "%s support registered\n", info->name);
 
 	return 0;
-
-error:
-	if (info->ops && info->ops->exit)
-		info->ops->exit(pfc);
-	return ret;
 }
 
 static int sh_pfc_remove(struct platform_device *pdev)
@@ -580,9 +575,6 @@ static int sh_pfc_remove(struct platform_device *pdev)
 #endif
 	sh_pfc_unregister_pinctrl(pfc);
 
-	if (pfc->info->ops && pfc->info->ops->exit)
-		pfc->info->ops->exit(pfc);
-
 	return 0;
 }
 
diff --git a/drivers/pinctrl/sh-pfc/core.h b/drivers/pinctrl/sh-pfc/core.h
index b7b0e6ccf305..3daaa5241c47 100644
--- a/drivers/pinctrl/sh-pfc/core.h
+++ b/drivers/pinctrl/sh-pfc/core.h
@@ -33,7 +33,6 @@ struct sh_pfc_pin_range {
 struct sh_pfc {
 	struct device *dev;
 	const struct sh_pfc_soc_info *info;
-	void *soc_data;
 	spinlock_t lock;
 
 	unsigned int num_windows;
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
index ce9fb7aa8ba3..280a56f97786 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a73a4.c
@@ -2717,14 +2717,14 @@ static void r8a73a4_pinmux_set_bias(struct sh_pfc *pfc, unsigned int pin,
 	iowrite8(value, addr);
 }
 
-static const struct sh_pfc_soc_operations r8a73a4_pinmux_ops = {
+static const struct sh_pfc_soc_operations r8a73a4_pfc_ops = {
 	.get_bias = r8a73a4_pinmux_get_bias,
 	.set_bias = r8a73a4_pinmux_set_bias,
 };
 
 const struct sh_pfc_soc_info r8a73a4_pinmux_info = {
 	.name		= "r8a73a4_pfc",
-	.ops		= &r8a73a4_pinmux_ops,
+	.ops		= &r8a73a4_pfc_ops,
 
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
index e4c1ef477053..b486e9d20cc2 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7740.c
@@ -3752,14 +3752,14 @@ static void r8a7740_pinmux_set_bias(struct sh_pfc *pfc, unsigned int pin,
 	iowrite8(value, addr);
 }
 
-static const struct sh_pfc_soc_operations r8a7740_pinmux_ops = {
+static const struct sh_pfc_soc_operations r8a7740_pfc_ops = {
 	.get_bias = r8a7740_pinmux_get_bias,
 	.set_bias = r8a7740_pinmux_set_bias,
 };
 
 const struct sh_pfc_soc_info r8a7740_pinmux_info = {
 	.name		= "r8a7740_pfc",
-	.ops		= &r8a7740_pinmux_ops,
+	.ops		= &r8a7740_pfc_ops,
 
 	.input		= { PINMUX_INPUT_BEGIN,
 			    PINMUX_INPUT_END },
diff --git a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
index 576d41b459e9..c6e5deba238e 100644
--- a/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
+++ b/drivers/pinctrl/sh-pfc/pfc-r8a7791.c
@@ -4509,24 +4509,24 @@ static const char * const audio_clk_groups[] = {
 };
 
 static const char * const can0_groups[] = {
-	"can0_data_a",
+	"can0_data",
 	"can0_data_b",
 	"can0_data_c",
 	"can0_data_d",
 	"can0_data_e",
 	"can0_data_f",
-	"can_clk_a",
+	"can_clk",
 	"can_clk_b",
 	"can_clk_c",
 	"can_clk_d",
 };
 
 static const char * const can1_groups[] = {
-	"can1_data_a",
+	"can1_data",
 	"can1_data_b",
 	"can1_data_c",
 	"can1_data_d",
-	"can_clk_a",
+	"can_clk",
 	"can_clk_b",
 	"can_clk_c",
 	"can_clk_d",
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh7372.c b/drivers/pinctrl/sh-pfc/pfc-sh7372.c
index d9158b3b2919..8211f66a2f68 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh7372.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh7372.c
@@ -2614,14 +2614,14 @@ static void sh7372_pinmux_set_bias(struct sh_pfc *pfc, unsigned int pin,
 	iowrite8(value, addr);
 }
 
-static const struct sh_pfc_soc_operations sh7372_pinmux_ops = {
+static const struct sh_pfc_soc_operations sh7372_pfc_ops = {
 	.get_bias = sh7372_pinmux_get_bias,
 	.set_bias = sh7372_pinmux_set_bias,
 };
 
 const struct sh_pfc_soc_info sh7372_pinmux_info = {
 	.name = "sh7372_pfc",
-	.ops = &sh7372_pinmux_ops,
+	.ops = &sh7372_pfc_ops,
 
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
diff --git a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
index 0bd8f4401b42..d2efbfb776ac 100644
--- a/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
+++ b/drivers/pinctrl/sh-pfc/pfc-sh73a0.c
@@ -3824,39 +3824,28 @@ static void sh73a0_pinmux_set_bias(struct sh_pfc *pfc, unsigned int pin,
  * SoC information
  */
 
-struct sh73a0_pinmux_data {
-	struct regulator_dev *vccq_mc0;
-};
-
 static int sh73a0_pinmux_soc_init(struct sh_pfc *pfc)
 {
-	struct sh73a0_pinmux_data *data;
 	struct regulator_config cfg = { };
+	struct regulator_dev *vccq;
 	int ret;
 
-	data = devm_kzalloc(pfc->dev, sizeof(*data), GFP_KERNEL);
-	if (data == NULL)
-		return -ENOMEM;
-
 	cfg.dev = pfc->dev;
 	cfg.init_data = &sh73a0_vccq_mc0_init_data;
 	cfg.driver_data = pfc;
 
-	data->vccq_mc0 = devm_regulator_register(pfc->dev,
-						 &sh73a0_vccq_mc0_desc, &cfg);
-	if (IS_ERR(data->vccq_mc0)) {
-		ret = PTR_ERR(data->vccq_mc0);
+	vccq = devm_regulator_register(pfc->dev, &sh73a0_vccq_mc0_desc, &cfg);
+	if (IS_ERR(vccq)) {
+		ret = PTR_ERR(vccq);
 		dev_err(pfc->dev, "Failed to register VCCQ MC0 regulator: %d\n",
 			ret);
 		return ret;
 	}
 
-	pfc->soc_data = data;
-
 	return 0;
 }
 
-static const struct sh_pfc_soc_operations sh73a0_pinmux_ops = {
+static const struct sh_pfc_soc_operations sh73a0_pfc_ops = {
 	.init = sh73a0_pinmux_soc_init,
 	.get_bias = sh73a0_pinmux_get_bias,
 	.set_bias = sh73a0_pinmux_set_bias,
@@ -3864,7 +3853,7 @@ static const struct sh_pfc_soc_operations sh73a0_pinmux_ops = {
 
 const struct sh_pfc_soc_info sh73a0_pinmux_info = {
 	.name = "sh73a0_pfc",
-	.ops = &sh73a0_pinmux_ops,
+	.ops = &sh73a0_pfc_ops,
 
 	.input = { PINMUX_INPUT_BEGIN, PINMUX_INPUT_END },
 	.output = { PINMUX_OUTPUT_BEGIN, PINMUX_OUTPUT_END },
diff --git a/drivers/pinctrl/sh-pfc/pinctrl.c b/drivers/pinctrl/sh-pfc/pinctrl.c
index 11db3ee39d40..910deaefa0ac 100644
--- a/drivers/pinctrl/sh-pfc/pinctrl.c
+++ b/drivers/pinctrl/sh-pfc/pinctrl.c
@@ -312,8 +312,8 @@ static int sh_pfc_get_function_groups(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int sh_pfc_func_enable(struct pinctrl_dev *pctldev, unsigned selector,
-			      unsigned group)
+static int sh_pfc_func_set_mux(struct pinctrl_dev *pctldev, unsigned selector,
+			       unsigned group)
 {
 	struct sh_pfc_pinctrl *pmx = pinctrl_dev_get_drvdata(pctldev);
 	struct sh_pfc *pfc = pmx->pfc;
@@ -442,7 +442,7 @@ static const struct pinmux_ops sh_pfc_pinmux_ops = {
 	.get_functions_count	= sh_pfc_get_functions_count,
 	.get_function_name	= sh_pfc_get_function_name,
 	.get_function_groups	= sh_pfc_get_function_groups,
-	.enable			= sh_pfc_func_enable,
+	.set_mux		= sh_pfc_func_set_mux,
 	.gpio_request_enable	= sh_pfc_gpio_request_enable,
 	.gpio_disable_free	= sh_pfc_gpio_disable_free,
 	.gpio_set_direction	= sh_pfc_gpio_set_direction,
diff --git a/drivers/pinctrl/sh-pfc/sh_pfc.h b/drivers/pinctrl/sh-pfc/sh_pfc.h
index d482c40b012a..5b7283182c1e 100644
--- a/drivers/pinctrl/sh-pfc/sh_pfc.h
+++ b/drivers/pinctrl/sh-pfc/sh_pfc.h
@@ -116,7 +116,6 @@ struct sh_pfc;
 
 struct sh_pfc_soc_operations {
 	int (*init)(struct sh_pfc *pfc);
-	void (*exit)(struct sh_pfc *pfc);
 	unsigned int (*get_bias)(struct sh_pfc *pfc, unsigned int pin);
 	void (*set_bias)(struct sh_pfc *pfc, unsigned int pin,
 			 unsigned int bias);
diff --git a/drivers/pinctrl/sirf/pinctrl-atlas6.c b/drivers/pinctrl/sirf/pinctrl-atlas6.c
index c4dd3d5cf9c3..45f8391ddb34 100644
--- a/drivers/pinctrl/sirf/pinctrl-atlas6.c
+++ b/drivers/pinctrl/sirf/pinctrl-atlas6.c
@@ -134,8 +134,9 @@ static const struct sirfsoc_muxmask lcd_16bits_sirfsoc_muxmask[] = {
 		.mask = BIT(30) | BIT(31),
 	}, {
 		.group = 2,
-		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
-			BIT(12) | BIT(13) | BIT(15) | BIT(16) | BIT(17) | BIT(18) | BIT(19) |
+		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) |
+			BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(15) |
+			BIT(16) | BIT(17) | BIT(18) | BIT(19) |
 			BIT(20) | BIT(21) | BIT(22) | BIT(31),
 	},
 };
@@ -148,14 +149,15 @@ static const struct sirfsoc_padmux lcd_16bits_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned lcd_16bits_pins[] = { 62, 63, 65, 70, 71, 72, 73, 74, 75, 76, 77, 79, 80, 81, 82, 83,
-	84, 85, 86, 95 };
+static const unsigned lcd_16bits_pins[] = { 62, 63, 65, 70, 71, 72, 73, 74, 75,
+	76, 77, 79, 80, 81, 82, 83, 84, 85, 86, 95 };
 
 static const struct sirfsoc_muxmask lcd_18bits_muxmask[] = {
 	{
 		.group = 2,
-		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
-			BIT(12) | BIT(13) | BIT(15) | BIT(16) | BIT(17) | BIT(18) | BIT(19) |
+		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) |
+			BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(15) |
+			BIT(16) | BIT(17) | BIT(18) | BIT(19) |
 			BIT(20) | BIT(21) | BIT(22) | BIT(31),
 	}, {
 		.group = 1,
@@ -174,21 +176,23 @@ static const struct sirfsoc_padmux lcd_18bits_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned lcd_18bits_pins[] = { 16, 17, 62, 63, 65, 70, 71, 72, 73, 74, 75, 76, 77, 79, 80, 81, 82, 83,
-	84, 85, 86, 95 };
+static const unsigned lcd_18bits_pins[] = { 16, 17, 62, 63, 65, 70, 71, 72, 73,
+	74, 75, 76, 77, 79, 80, 81, 82, 83, 84, 85, 86, 95 };
 
 static const struct sirfsoc_muxmask lcd_24bits_muxmask[] = {
 	{
 		.group = 2,
-		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
-			BIT(12) | BIT(13) | BIT(15) | BIT(16) | BIT(17) | BIT(18) | BIT(19) |
+		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) |
+			BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(15) |
+			BIT(16) | BIT(17) | BIT(18) | BIT(19) |
 			BIT(20) | BIT(21) | BIT(22) | BIT(31),
 	}, {
 		.group = 1,
 		.mask = BIT(30) | BIT(31),
 	}, {
 		.group = 0,
-		.mask = BIT(16) | BIT(17) | BIT(18) | BIT(19) | BIT(20) | BIT(21) | BIT(22) | BIT(23),
+		.mask = BIT(16) | BIT(17) | BIT(18) | BIT(19) | BIT(20) |
+			BIT(21) | BIT(22) | BIT(23),
 	},
 };
 
@@ -200,14 +204,16 @@ static const struct sirfsoc_padmux lcd_24bits_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned lcd_24bits_pins[] = { 16, 17, 18, 19, 20, 21, 22, 23, 62, 63, 65, 70, 71, 72, 73, 74, 75, 76, 77, 79,
-	80, 81, 82, 83, 84, 85, 86, 95};
+static const unsigned lcd_24bits_pins[] = { 16, 17, 18, 19, 20, 21, 22, 23, 62,
+	63, 65, 70, 71, 72, 73, 74, 75, 76, 77, 79, 80, 81, 82, 83, 84,
+	85, 86, 95};
 
 static const struct sirfsoc_muxmask lcdrom_muxmask[] = {
 	{
 		.group = 2,
-		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
-			BIT(12) | BIT(13) | BIT(15) | BIT(16) | BIT(17) | BIT(18) | BIT(19) |
+		.mask = BIT(1) | BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) |
+			BIT(11) | BIT(12) | BIT(13) | BIT(15) | BIT(16) |
+			BIT(17) | BIT(18) | BIT(19) |
 			BIT(20) | BIT(21) | BIT(22) | BIT(31),
 	}, {
 		.group = 1,
@@ -226,8 +232,8 @@ static const struct sirfsoc_padmux lcdrom_padmux = {
 	.funcval = BIT(4),
 };
 
-static const unsigned lcdrom_pins[] = { 8, 62, 63, 65, 70, 71, 72, 73, 74, 75, 76, 77, 79, 80, 81, 82, 83,
-	84, 85, 86, 95};
+static const unsigned lcdrom_pins[] = { 8, 62, 63, 65, 70, 71, 72, 73, 74, 75,
+	76, 77, 79, 80, 81, 82, 83, 84, 85, 86, 95};
 
 static const struct sirfsoc_muxmask uart0_muxmask[] = {
 	{
@@ -371,11 +377,42 @@ static const struct sirfsoc_padmux cko1_padmux = {
 
 static const unsigned cko1_pins[] = { 42 };
 
-static const struct sirfsoc_muxmask i2s_muxmask[] = {
+static const struct sirfsoc_muxmask i2s_mclk_muxmask[] = {
 	{
 		.group = 1,
 		.mask = BIT(10),
-	}, {
+	},
+};
+
+static const struct sirfsoc_padmux i2s_mclk_padmux = {
+	.muxmask_counts = ARRAY_SIZE(i2s_mclk_muxmask),
+	.muxmask = i2s_mclk_muxmask,
+	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
+	.funcmask = BIT(3),
+	.funcval = BIT(3),
+};
+
+static const unsigned i2s_mclk_pins[] = { 42 };
+
+static const struct sirfsoc_muxmask i2s_ext_clk_input_muxmask[] = {
+	{
+		.group = 1,
+		.mask = BIT(19),
+	},
+};
+
+static const struct sirfsoc_padmux i2s_ext_clk_input_padmux = {
+	.muxmask_counts = ARRAY_SIZE(i2s_ext_clk_input_muxmask),
+	.muxmask = i2s_ext_clk_input_muxmask,
+	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
+	.funcmask = BIT(2),
+	.funcval = BIT(2),
+};
+
+static const unsigned i2s_ext_clk_input_pins[] = { 51 };
+
+static const struct sirfsoc_muxmask i2s_muxmask[] = {
+	{
 		.group = 3,
 		.mask = BIT(2) | BIT(3) | BIT(4) | BIT(5),
 	},
@@ -385,17 +422,12 @@ static const struct sirfsoc_padmux i2s_padmux = {
 	.muxmask_counts = ARRAY_SIZE(i2s_muxmask),
 	.muxmask = i2s_muxmask,
 	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
-	.funcmask = BIT(3),
-	.funcval = BIT(3),
 };
 
-static const unsigned i2s_pins[] = { 42, 98, 99, 100, 101 };
+static const unsigned i2s_pins[] = { 98, 99, 100, 101 };
 
 static const struct sirfsoc_muxmask i2s_no_din_muxmask[] = {
 	{
-		.group = 1,
-		.mask = BIT(10),
-	}, {
 		.group = 3,
 		.mask = BIT(2) | BIT(3) | BIT(4),
 	},
@@ -405,17 +437,12 @@ static const struct sirfsoc_padmux i2s_no_din_padmux = {
 	.muxmask_counts = ARRAY_SIZE(i2s_no_din_muxmask),
 	.muxmask = i2s_no_din_muxmask,
 	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
-	.funcmask = BIT(3),
-	.funcval = BIT(3),
 };
 
-static const unsigned i2s_no_din_pins[] = { 42, 98, 99, 100 };
+static const unsigned i2s_no_din_pins[] = { 98, 99, 100 };
 
 static const struct sirfsoc_muxmask i2s_6chn_muxmask[] = {
 	{
-		.group = 1,
-		.mask = BIT(10) | BIT(20) | BIT(23),
-	}, {
 		.group = 3,
 		.mask = BIT(2) | BIT(3) | BIT(4) | BIT(5),
 	},
@@ -425,11 +452,11 @@ static const struct sirfsoc_padmux i2s_6chn_padmux = {
 	.muxmask_counts = ARRAY_SIZE(i2s_6chn_muxmask),
 	.muxmask = i2s_6chn_muxmask,
 	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
-	.funcmask = BIT(1) | BIT(3) | BIT(9),
-	.funcval = BIT(1) | BIT(3) | BIT(9),
+	.funcmask = BIT(1) | BIT(9),
+	.funcval = BIT(1) | BIT(9),
 };
 
-static const unsigned i2s_6chn_pins[] = { 42, 52, 55, 98, 99, 100, 101 };
+static const unsigned i2s_6chn_pins[] = { 52, 55, 98, 99, 100, 101 };
 
 static const struct sirfsoc_muxmask ac97_muxmask[] = {
 	{
@@ -716,7 +743,8 @@ static const struct sirfsoc_padmux vip_padmux = {
 	.funcval = BIT(18),
 };
 
-static const unsigned vip_pins[] = { 36, 37, 38, 40, 41, 56, 57, 58, 59, 60, 61 };
+static const unsigned vip_pins[] = { 36, 37, 38, 40, 41, 56, 57, 58, 59,
+	60, 61 };
 
 static const struct sirfsoc_muxmask vip_noupli_muxmask[] = {
 	{
@@ -737,7 +765,8 @@ static const struct sirfsoc_padmux vip_noupli_padmux = {
 	.funcval = BIT(15),
 };
 
-static const unsigned vip_noupli_pins[] = { 16, 17, 18, 19, 20, 21, 22, 23, 87, 88, 89 };
+static const unsigned vip_noupli_pins[] = { 16, 17, 18, 19, 20, 21, 22, 23,
+	87, 88, 89 };
 
 static const struct sirfsoc_muxmask i2c0_muxmask[] = {
 	{
@@ -876,7 +905,8 @@ static const struct sirfsoc_padmux usb0_upli_drvbus_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned usb0_upli_drvbus_pins[] = { 36, 37, 38, 39, 40, 41, 56, 57, 58, 59, 60, 61 };
+static const unsigned usb0_upli_drvbus_pins[] = { 36, 37, 38, 39, 40,
+	41, 56, 57, 58, 59, 60, 61 };
 
 static const struct sirfsoc_muxmask usb1_utmi_drvbus_muxmask[] = {
 	{
@@ -968,6 +998,8 @@ static const struct sirfsoc_pin_group sirfsoc_pin_groups[] = {
 	SIRFSOC_PIN_GROUP("usb1_dp_dngrp", usb1_dp_dn_pins),
 	SIRFSOC_PIN_GROUP("uart1_route_io_usb1grp", uart1_route_io_usb1_pins),
 	SIRFSOC_PIN_GROUP("pulse_countgrp", pulse_count_pins),
+	SIRFSOC_PIN_GROUP("i2smclkgrp", i2s_mclk_pins),
+	SIRFSOC_PIN_GROUP("i2s_ext_clk_inputgrp", i2s_ext_clk_input_pins),
 	SIRFSOC_PIN_GROUP("i2sgrp", i2s_pins),
 	SIRFSOC_PIN_GROUP("i2s_no_dingrp", i2s_no_din_pins),
 	SIRFSOC_PIN_GROUP("i2s_6chngrp", i2s_6chn_pins),
@@ -1017,8 +1049,11 @@ static const char * const sdmmc2_nowpgrp[] = { "sdmmc2_nowpgrp" };
 static const char * const usb0_upli_drvbusgrp[] = { "usb0_upli_drvbusgrp" };
 static const char * const usb1_utmi_drvbusgrp[] = { "usb1_utmi_drvbusgrp" };
 static const char * const usb1_dp_dngrp[] = { "usb1_dp_dngrp" };
-static const char * const uart1_route_io_usb1grp[] = { "uart1_route_io_usb1grp" };
+static const char * const
+	uart1_route_io_usb1grp[] = { "uart1_route_io_usb1grp" };
 static const char * const pulse_countgrp[] = { "pulse_countgrp" };
+static const char * const i2smclkgrp[] = { "i2smclkgrp" };
+static const char * const i2s_ext_clk_inputgrp[] = { "i2s_ext_clk_inputgrp" };
 static const char * const i2sgrp[] = { "i2sgrp" };
 static const char * const i2s_no_dingrp[] = { "i2s_no_dingrp" };
 static const char * const i2s_6chngrp[] = { "i2s_6chngrp" };
@@ -1038,7 +1073,8 @@ static const struct sirfsoc_pmx_func sirfsoc_pmx_functions[] = {
 						uart0_nostreamctrl_padmux),
 	SIRFSOC_PMX_FUNCTION("uart1", uart1grp, uart1_padmux),
 	SIRFSOC_PMX_FUNCTION("uart2", uart2grp, uart2_padmux),
-	SIRFSOC_PMX_FUNCTION("uart2_nostreamctrl", uart2_nostreamctrlgrp, uart2_nostreamctrl_padmux),
+	SIRFSOC_PMX_FUNCTION("uart2_nostreamctrl",
+		uart2_nostreamctrlgrp, uart2_nostreamctrl_padmux),
 	SIRFSOC_PMX_FUNCTION("usp0", usp0grp, usp0_padmux),
 	SIRFSOC_PMX_FUNCTION("usp0_uart_nostreamctrl",
 						usp0_uart_nostreamctrl_grp,
@@ -1068,12 +1104,19 @@ static const struct sirfsoc_pmx_func sirfsoc_pmx_functions[] = {
 	SIRFSOC_PMX_FUNCTION("sdmmc2", sdmmc2grp, sdmmc2_padmux),
 	SIRFSOC_PMX_FUNCTION("sdmmc3", sdmmc3grp, sdmmc3_padmux),
 	SIRFSOC_PMX_FUNCTION("sdmmc5", sdmmc5grp, sdmmc5_padmux),
-	SIRFSOC_PMX_FUNCTION("sdmmc2_nowp", sdmmc2_nowpgrp, sdmmc2_nowp_padmux),
-	SIRFSOC_PMX_FUNCTION("usb0_upli_drvbus", usb0_upli_drvbusgrp, usb0_upli_drvbus_padmux),
-	SIRFSOC_PMX_FUNCTION("usb1_utmi_drvbus", usb1_utmi_drvbusgrp, usb1_utmi_drvbus_padmux),
+	SIRFSOC_PMX_FUNCTION("sdmmc2_nowp",
+		sdmmc2_nowpgrp, sdmmc2_nowp_padmux),
+	SIRFSOC_PMX_FUNCTION("usb0_upli_drvbus",
+		usb0_upli_drvbusgrp, usb0_upli_drvbus_padmux),
+	SIRFSOC_PMX_FUNCTION("usb1_utmi_drvbus",
+		usb1_utmi_drvbusgrp, usb1_utmi_drvbus_padmux),
 	SIRFSOC_PMX_FUNCTION("usb1_dp_dn", usb1_dp_dngrp, usb1_dp_dn_padmux),
-	SIRFSOC_PMX_FUNCTION("uart1_route_io_usb1", uart1_route_io_usb1grp, uart1_route_io_usb1_padmux),
+	SIRFSOC_PMX_FUNCTION("uart1_route_io_usb1",
+		uart1_route_io_usb1grp, uart1_route_io_usb1_padmux),
 	SIRFSOC_PMX_FUNCTION("pulse_count", pulse_countgrp, pulse_count_padmux),
+	SIRFSOC_PMX_FUNCTION("i2s_mclk", i2smclkgrp, i2s_mclk_padmux),
+	SIRFSOC_PMX_FUNCTION("i2s_ext_clk_input", i2s_ext_clk_inputgrp,
+						i2s_ext_clk_input_padmux),
 	SIRFSOC_PMX_FUNCTION("i2s", i2sgrp, i2s_padmux),
 	SIRFSOC_PMX_FUNCTION("i2s_no_din", i2s_no_dingrp, i2s_no_din_padmux),
 	SIRFSOC_PMX_FUNCTION("i2s_6chn", i2s_6chngrp, i2s_6chn_padmux),
diff --git a/drivers/pinctrl/sirf/pinctrl-prima2.c b/drivers/pinctrl/sirf/pinctrl-prima2.c
index 8aa76f0776d7..357678ee28e3 100644
--- a/drivers/pinctrl/sirf/pinctrl-prima2.c
+++ b/drivers/pinctrl/sirf/pinctrl-prima2.c
@@ -135,8 +135,9 @@ static const struct pinctrl_pin_desc sirfsoc_pads[] = {
 static const struct sirfsoc_muxmask lcd_16bits_sirfsoc_muxmask[] = {
 	{
 		.group = 3,
-		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) |
-			BIT(9) | BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
+		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) |
+			BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
+			BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
 			BIT(17) | BIT(18),
 	}, {
 		.group = 2,
@@ -152,14 +153,15 @@ static const struct sirfsoc_padmux lcd_16bits_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned lcd_16bits_pins[] = { 95, 96, 97, 98, 99, 100, 101, 102, 103, 104,
-	105, 106, 107, 108, 109, 110, 111, 112, 113, 114 };
+static const unsigned lcd_16bits_pins[] = { 95, 96, 97, 98, 99, 100, 101, 102,
+	103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114 };
 
 static const struct sirfsoc_muxmask lcd_18bits_muxmask[] = {
 	{
 		.group = 3,
-		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) |
-			BIT(9) | BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
+		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) |
+			BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
+			BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
 			BIT(17) | BIT(18),
 	}, {
 		.group = 2,
@@ -178,21 +180,23 @@ static const struct sirfsoc_padmux lcd_18bits_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned lcd_18bits_pins[] = { 16, 17, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104,
-	105, 106, 107, 108, 109, 110, 111, 112, 113, 114};
+static const unsigned lcd_18bits_pins[] = { 16, 17, 95, 96, 97, 98, 99, 100,
+	101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114};
 
 static const struct sirfsoc_muxmask lcd_24bits_muxmask[] = {
 	{
 		.group = 3,
-		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) |
-			BIT(9) | BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
+		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) |
+			BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
+			BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
 			BIT(17) | BIT(18),
 	}, {
 		.group = 2,
 		.mask = BIT(31),
 	}, {
 		.group = 0,
-		.mask = BIT(16) | BIT(17) | BIT(18) | BIT(19) | BIT(20) | BIT(21) | BIT(22) | BIT(23),
+		.mask = BIT(16) | BIT(17) | BIT(18) | BIT(19) | BIT(20) |
+			BIT(21) | BIT(22) | BIT(23),
 	},
 };
 
@@ -204,14 +208,16 @@ static const struct sirfsoc_padmux lcd_24bits_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned lcd_24bits_pins[] = { 16, 17, 18, 19, 20, 21, 22, 23, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104,
-	105, 106, 107, 108, 109, 110, 111, 112, 113, 114 };
+static const unsigned lcd_24bits_pins[] = { 16, 17, 18, 19, 20, 21, 22, 23,
+	95, 96,	97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109,
+	110, 111, 112, 113, 114 };
 
 static const struct sirfsoc_muxmask lcdrom_muxmask[] = {
 	{
 		.group = 3,
-		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) | BIT(6) | BIT(7) | BIT(8) |
-			BIT(9) | BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
+		.mask = BIT(0) | BIT(1) | BIT(2) | BIT(3) | BIT(4) | BIT(5) |
+			BIT(6) | BIT(7) | BIT(8) | BIT(9) | BIT(10) | BIT(11) |
+			BIT(12) | BIT(13) | BIT(14) | BIT(15) | BIT(16) |
 			BIT(17) | BIT(18),
 	}, {
 		.group = 2,
@@ -230,8 +236,8 @@ static const struct sirfsoc_padmux lcdrom_padmux = {
 	.funcval = BIT(4),
 };
 
-static const unsigned lcdrom_pins[] = { 23, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104,
-	105, 106, 107, 108, 109, 110, 111, 112, 113, 114 };
+static const unsigned lcdrom_pins[] = { 23, 95, 96, 97, 98, 99, 100, 101, 102,
+	103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114 };
 
 static const struct sirfsoc_muxmask uart0_muxmask[] = {
 	{
@@ -380,12 +386,44 @@ static const struct sirfsoc_padmux cko1_padmux = {
 
 static const unsigned cko1_pins[] = { 42 };
 
+static const struct sirfsoc_muxmask i2s_mclk_muxmask[] = {
+	{
+		.group = 1,
+		.mask = BIT(10),
+	},
+};
+
+static const struct sirfsoc_padmux i2s_mclk_padmux = {
+	.muxmask_counts = ARRAY_SIZE(i2s_mclk_muxmask),
+	.muxmask = i2s_mclk_muxmask,
+	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
+	.funcmask = BIT(3),
+	.funcval = BIT(3),
+};
+
+static const unsigned i2s_mclk_pins[] = { 42 };
+
+static const struct sirfsoc_muxmask i2s_ext_clk_input_muxmask[] = {
+	{
+		.group = 1,
+		.mask = BIT(19),
+	},
+};
+
+static const struct sirfsoc_padmux i2s_ext_clk_input_padmux = {
+	.muxmask_counts = ARRAY_SIZE(i2s_ext_clk_input_muxmask),
+	.muxmask = i2s_ext_clk_input_muxmask,
+	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
+	.funcmask = BIT(2),
+	.funcval = BIT(2),
+};
+
+static const unsigned i2s_ext_clk_input_pins[] = { 51 };
+
 static const struct sirfsoc_muxmask i2s_muxmask[] = {
 	{
 		.group = 1,
-		.mask =
-			BIT(10) | BIT(11) | BIT(12) | BIT(13) | BIT(14) | BIT(19)
-				| BIT(23) | BIT(28),
+		.mask = BIT(11) | BIT(12) | BIT(13) | BIT(14),
 	},
 };
 
@@ -393,11 +431,42 @@ static const struct sirfsoc_padmux i2s_padmux = {
 	.muxmask_counts = ARRAY_SIZE(i2s_muxmask),
 	.muxmask = i2s_muxmask,
 	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
-	.funcmask = BIT(3) | BIT(9),
-	.funcval = BIT(3),
 };
 
-static const unsigned i2s_pins[] = { 42, 43, 44, 45, 46, 51, 55, 60 };
+static const unsigned i2s_pins[] = { 43, 44, 45, 46 };
+
+static const struct sirfsoc_muxmask i2s_no_din_muxmask[] = {
+	{
+		.group = 1,
+		.mask = BIT(11) | BIT(12) | BIT(14),
+	},
+};
+
+static const struct sirfsoc_padmux i2s_no_din_padmux = {
+	.muxmask_counts = ARRAY_SIZE(i2s_no_din_muxmask),
+	.muxmask = i2s_no_din_muxmask,
+	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
+};
+
+static const unsigned i2s_no_din_pins[] = { 43, 44, 46 };
+
+static const struct sirfsoc_muxmask i2s_6chn_muxmask[] = {
+	{
+		.group = 1,
+		.mask = BIT(11) | BIT(12) | BIT(13) | BIT(14)
+			| BIT(23) | BIT(28),
+	},
+};
+
+static const struct sirfsoc_padmux i2s_6chn_padmux = {
+	.muxmask_counts = ARRAY_SIZE(i2s_6chn_muxmask),
+	.muxmask = i2s_6chn_muxmask,
+	.ctrlreg = SIRFSOC_RSC_PIN_MUX,
+	.funcmask = BIT(1) | BIT(9),
+	.funcval = BIT(1) | BIT(9),
+};
+
+static const unsigned i2s_6chn_pins[] = { 43, 44, 45, 46, 55, 60  };
 
 static const struct sirfsoc_muxmask ac97_muxmask[] = {
 	{
@@ -685,7 +754,8 @@ static const struct sirfsoc_padmux vip_padmux = {
 	.funcval = 0,
 };
 
-static const unsigned vip_pins[] = { 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89 };
+static const unsigned vip_pins[] = { 79, 80, 81, 82, 83, 84, 85, 86, 87,
+	88, 89 };
 
 static const struct sirfsoc_muxmask i2c0_muxmask[] = {
 	{
@@ -735,7 +805,8 @@ static const struct sirfsoc_padmux viprom_padmux = {
 	.funcval = BIT(0),
 };
 
-static const unsigned viprom_pins[] = { 12, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89 };
+static const unsigned viprom_pins[] = { 12, 79, 80, 81, 82, 83, 84, 85, 86,
+	87, 88, 89 };
 
 static const struct sirfsoc_muxmask pwm0_muxmask[] = {
 	{
@@ -918,7 +989,11 @@ static const struct sirfsoc_pin_group sirfsoc_pin_groups[] = {
 	SIRFSOC_PIN_GROUP("usb1_dp_dngrp", usb1_dp_dn_pins),
 	SIRFSOC_PIN_GROUP("uart1_route_io_usb1grp", uart1_route_io_usb1_pins),
 	SIRFSOC_PIN_GROUP("pulse_countgrp", pulse_count_pins),
+	SIRFSOC_PIN_GROUP("i2smclkgrp", i2s_mclk_pins),
+	SIRFSOC_PIN_GROUP("i2s_ext_clk_inputgrp", i2s_ext_clk_input_pins),
 	SIRFSOC_PIN_GROUP("i2sgrp", i2s_pins),
+	SIRFSOC_PIN_GROUP("i2s_no_dingrp", i2s_no_din_pins),
+	SIRFSOC_PIN_GROUP("i2s_6chngrp", i2s_6chn_pins),
 	SIRFSOC_PIN_GROUP("ac97grp", ac97_pins),
 	SIRFSOC_PIN_GROUP("nandgrp", nand_pins),
 	SIRFSOC_PIN_GROUP("spi0grp", spi0_pins),
@@ -936,16 +1011,19 @@ static const char * const uart1grp[] = { "uart1grp" };
 static const char * const uart2grp[] = { "uart2grp" };
 static const char * const uart2_nostreamctrlgrp[] = { "uart2_nostreamctrlgrp" };
 static const char * const usp0grp[] = { "usp0grp" };
-static const char * const usp0_uart_nostreamctrl_grp[] =
-					{ "usp0_uart_nostreamctrl_grp" };
+static const char * const usp0_uart_nostreamctrl_grp[] = {
+	"usp0_uart_nostreamctrl_grp"
+};
 static const char * const usp0_only_utfs_grp[] = { "usp0_only_utfs_grp" };
 static const char * const usp0_only_urfs_grp[] = { "usp0_only_urfs_grp" };
 static const char * const usp1grp[] = { "usp1grp" };
-static const char * const usp1_uart_nostreamctrl_grp[] =
-					{ "usp1_uart_nostreamctrl_grp" };
+static const char * const usp1_uart_nostreamctrl_grp[] = {
+	"usp1_uart_nostreamctrl_grp"
+};
 static const char * const usp2grp[] = { "usp2grp" };
-static const char * const usp2_uart_nostreamctrl_grp[] =
-					{ "usp2_uart_nostreamctrl_grp" };
+static const char * const usp2_uart_nostreamctrl_grp[] = {
+	"usp2_uart_nostreamctrl_grp"
+};
 static const char * const i2c0grp[] = { "i2c0grp" };
 static const char * const i2c1grp[] = { "i2c1grp" };
 static const char * const pwm0grp[] = { "pwm0grp" };
@@ -966,9 +1044,14 @@ static const char * const sdmmc5grp[] = { "sdmmc5grp" };
 static const char * const usb0_utmi_drvbusgrp[] = { "usb0_utmi_drvbusgrp" };
 static const char * const usb1_utmi_drvbusgrp[] = { "usb1_utmi_drvbusgrp" };
 static const char * const usb1_dp_dngrp[] = { "usb1_dp_dngrp" };
-static const char * const uart1_route_io_usb1grp[] = { "uart1_route_io_usb1grp" };
+static const char * const
+	uart1_route_io_usb1grp[] = { "uart1_route_io_usb1grp" };
 static const char * const pulse_countgrp[] = { "pulse_countgrp" };
+static const char * const i2smclkgrp[] = { "i2smclkgrp" };
+static const char * const i2s_ext_clk_inputgrp[] = { "i2s_ext_clk_inputgrp" };
 static const char * const i2sgrp[] = { "i2sgrp" };
+static const char * const i2s_no_dingrp[] = { "i2s_no_dingrp" };
+static const char * const i2s_6chngrp[] = { "i2s_6chngrp" };
 static const char * const ac97grp[] = { "ac97grp" };
 static const char * const nandgrp[] = { "nandgrp" };
 static const char * const spi0grp[] = { "spi0grp" };
@@ -981,15 +1064,19 @@ static const struct sirfsoc_pmx_func sirfsoc_pmx_functions[] = {
 	SIRFSOC_PMX_FUNCTION("lcd_24bits", lcd_24bitsgrp, lcd_24bits_padmux),
 	SIRFSOC_PMX_FUNCTION("lcdrom", lcdromgrp, lcdrom_padmux),
 	SIRFSOC_PMX_FUNCTION("uart0", uart0grp, uart0_padmux),
-	SIRFSOC_PMX_FUNCTION("uart0_nostreamctrl", uart0_nostreamctrlgrp, uart0_nostreamctrl_padmux),
+	SIRFSOC_PMX_FUNCTION("uart0_nostreamctrl",
+		uart0_nostreamctrlgrp, uart0_nostreamctrl_padmux),
 	SIRFSOC_PMX_FUNCTION("uart1", uart1grp, uart1_padmux),
 	SIRFSOC_PMX_FUNCTION("uart2", uart2grp, uart2_padmux),
-	SIRFSOC_PMX_FUNCTION("uart2_nostreamctrl", uart2_nostreamctrlgrp, uart2_nostreamctrl_padmux),
+	SIRFSOC_PMX_FUNCTION("uart2_nostreamctrl",
+		uart2_nostreamctrlgrp, uart2_nostreamctrl_padmux),
 	SIRFSOC_PMX_FUNCTION("usp0", usp0grp, usp0_padmux),
 	SIRFSOC_PMX_FUNCTION("usp0_uart_nostreamctrl",
 		usp0_uart_nostreamctrl_grp, usp0_uart_nostreamctrl_padmux),
-	SIRFSOC_PMX_FUNCTION("usp0_only_utfs", usp0_only_utfs_grp, usp0_only_utfs_padmux),
-	SIRFSOC_PMX_FUNCTION("usp0_only_urfs", usp0_only_urfs_grp, usp0_only_urfs_padmux),
+	SIRFSOC_PMX_FUNCTION("usp0_only_utfs",
+		usp0_only_utfs_grp, usp0_only_utfs_padmux),
+	SIRFSOC_PMX_FUNCTION("usp0_only_urfs",
+		usp0_only_urfs_grp, usp0_only_urfs_padmux),
 	SIRFSOC_PMX_FUNCTION("usp1", usp1grp, usp1_padmux),
 	SIRFSOC_PMX_FUNCTION("usp1_uart_nostreamctrl",
 		usp1_uart_nostreamctrl_grp, usp1_uart_nostreamctrl_padmux),
@@ -1013,12 +1100,20 @@ static const struct sirfsoc_pmx_func sirfsoc_pmx_functions[] = {
 	SIRFSOC_PMX_FUNCTION("sdmmc3", sdmmc3grp, sdmmc3_padmux),
 	SIRFSOC_PMX_FUNCTION("sdmmc4", sdmmc4grp, sdmmc4_padmux),
 	SIRFSOC_PMX_FUNCTION("sdmmc5", sdmmc5grp, sdmmc5_padmux),
-	SIRFSOC_PMX_FUNCTION("usb0_utmi_drvbus", usb0_utmi_drvbusgrp, usb0_utmi_drvbus_padmux),
-	SIRFSOC_PMX_FUNCTION("usb1_utmi_drvbus", usb1_utmi_drvbusgrp, usb1_utmi_drvbus_padmux),
+	SIRFSOC_PMX_FUNCTION("usb0_utmi_drvbus",
+		usb0_utmi_drvbusgrp, usb0_utmi_drvbus_padmux),
+	SIRFSOC_PMX_FUNCTION("usb1_utmi_drvbus",
+		usb1_utmi_drvbusgrp, usb1_utmi_drvbus_padmux),
 	SIRFSOC_PMX_FUNCTION("usb1_dp_dn", usb1_dp_dngrp, usb1_dp_dn_padmux),
-	SIRFSOC_PMX_FUNCTION("uart1_route_io_usb1", uart1_route_io_usb1grp, uart1_route_io_usb1_padmux),
+	SIRFSOC_PMX_FUNCTION("uart1_route_io_usb1",
+		uart1_route_io_usb1grp, uart1_route_io_usb1_padmux),
 	SIRFSOC_PMX_FUNCTION("pulse_count", pulse_countgrp, pulse_count_padmux),
+	SIRFSOC_PMX_FUNCTION("i2s_mclk", i2smclkgrp, i2s_mclk_padmux),
+	SIRFSOC_PMX_FUNCTION("i2s_ext_clk_input", i2s_ext_clk_inputgrp,
+						i2s_ext_clk_input_padmux),
 	SIRFSOC_PMX_FUNCTION("i2s", i2sgrp, i2s_padmux),
+	SIRFSOC_PMX_FUNCTION("i2s_no_din", i2s_no_dingrp, i2s_no_din_padmux),
+	SIRFSOC_PMX_FUNCTION("i2s_6chn", i2s_6chngrp, i2s_6chn_padmux),
 	SIRFSOC_PMX_FUNCTION("ac97", ac97grp, ac97_padmux),
 	SIRFSOC_PMX_FUNCTION("nand", nandgrp, nand_padmux),
 	SIRFSOC_PMX_FUNCTION("spi0", spi0grp, spi0_padmux),
diff --git a/drivers/pinctrl/sirf/pinctrl-sirf.c b/drivers/pinctrl/sirf/pinctrl-sirf.c
index 4c1d7c68666d..4c831fdfcc2f 100644
--- a/drivers/pinctrl/sirf/pinctrl-sirf.c
+++ b/drivers/pinctrl/sirf/pinctrl-sirf.c
@@ -58,17 +58,18 @@ static const char *sirfsoc_get_group_name(struct pinctrl_dev *pctldev,
 	return sirfsoc_pin_groups[selector].name;
 }
 
-static int sirfsoc_get_group_pins(struct pinctrl_dev *pctldev, unsigned selector,
-			       const unsigned **pins,
-			       unsigned *num_pins)
+static int sirfsoc_get_group_pins(struct pinctrl_dev *pctldev,
+				unsigned selector,
+				const unsigned **pins,
+				unsigned *num_pins)
 {
 	*pins = sirfsoc_pin_groups[selector].pins;
 	*num_pins = sirfsoc_pin_groups[selector].num_pins;
 	return 0;
 }
 
-static void sirfsoc_pin_dbg_show(struct pinctrl_dev *pctldev, struct seq_file *s,
-		   unsigned offset)
+static void sirfsoc_pin_dbg_show(struct pinctrl_dev *pctldev,
+				struct seq_file *s, unsigned offset)
 {
 	seq_printf(s, " " DRIVER_NAME);
 }
@@ -138,22 +139,25 @@ static struct pinctrl_ops sirfsoc_pctrl_ops = {
 static struct sirfsoc_pmx_func *sirfsoc_pmx_functions;
 static int sirfsoc_pmxfunc_cnt;
 
-static void sirfsoc_pinmux_endisable(struct sirfsoc_pmx *spmx, unsigned selector,
-	bool enable)
+static void sirfsoc_pinmux_endisable(struct sirfsoc_pmx *spmx,
+					unsigned selector, bool enable)
 {
 	int i;
-	const struct sirfsoc_padmux *mux = sirfsoc_pmx_functions[selector].padmux;
+	const struct sirfsoc_padmux *mux =
+		sirfsoc_pmx_functions[selector].padmux;
 	const struct sirfsoc_muxmask *mask = mux->muxmask;
 
 	for (i = 0; i < mux->muxmask_counts; i++) {
 		u32 muxval;
 		if (!spmx->is_marco) {
-			muxval = readl(spmx->gpio_virtbase + SIRFSOC_GPIO_PAD_EN(mask[i].group));
+			muxval = readl(spmx->gpio_virtbase +
+				SIRFSOC_GPIO_PAD_EN(mask[i].group));
 			if (enable)
 				muxval = muxval & ~mask[i].mask;
 			else
 				muxval = muxval | mask[i].mask;
-			writel(muxval, spmx->gpio_virtbase + SIRFSOC_GPIO_PAD_EN(mask[i].group));
+			writel(muxval, spmx->gpio_virtbase +
+				SIRFSOC_GPIO_PAD_EN(mask[i].group));
 		} else {
 			if (enable)
 				writel(mask[i].mask, spmx->gpio_virtbase +
@@ -175,8 +179,9 @@ static void sirfsoc_pinmux_endisable(struct sirfsoc_pmx *spmx, unsigned selector
 	}
 }
 
-static int sirfsoc_pinmux_enable(struct pinctrl_dev *pmxdev, unsigned selector,
-	unsigned group)
+static int sirfsoc_pinmux_set_mux(struct pinctrl_dev *pmxdev,
+				unsigned selector,
+				unsigned group)
 {
 	struct sirfsoc_pmx *spmx;
 
@@ -197,9 +202,10 @@ static const char *sirfsoc_pinmux_get_func_name(struct pinctrl_dev *pctldev,
 	return sirfsoc_pmx_functions[selector].name;
 }
 
-static int sirfsoc_pinmux_get_groups(struct pinctrl_dev *pctldev, unsigned selector,
-			       const char * const **groups,
-			       unsigned * const num_groups)
+static int sirfsoc_pinmux_get_groups(struct pinctrl_dev *pctldev,
+				unsigned selector,
+				const char * const **groups,
+				unsigned * const num_groups)
 {
 	*groups = sirfsoc_pmx_functions[selector].groups;
 	*num_groups = sirfsoc_pmx_functions[selector].num_groups;
@@ -218,9 +224,11 @@ static int sirfsoc_pinmux_request_gpio(struct pinctrl_dev *pmxdev,
 	spmx = pinctrl_dev_get_drvdata(pmxdev);
 
 	if (!spmx->is_marco) {
-		muxval = readl(spmx->gpio_virtbase + SIRFSOC_GPIO_PAD_EN(group));
+		muxval = readl(spmx->gpio_virtbase +
+			SIRFSOC_GPIO_PAD_EN(group));
 		muxval = muxval | (1 << (offset - range->pin_base));
-		writel(muxval, spmx->gpio_virtbase + SIRFSOC_GPIO_PAD_EN(group));
+		writel(muxval, spmx->gpio_virtbase +
+			SIRFSOC_GPIO_PAD_EN(group));
 	} else {
 		writel(1 << (offset - range->pin_base), spmx->gpio_virtbase +
 			SIRFSOC_GPIO_PAD_EN(group));
@@ -230,7 +238,7 @@ static int sirfsoc_pinmux_request_gpio(struct pinctrl_dev *pmxdev,
 }
 
 static struct pinmux_ops sirfsoc_pinmux_ops = {
-	.enable = sirfsoc_pinmux_enable,
+	.set_mux = sirfsoc_pinmux_set_mux,
 	.get_functions_count = sirfsoc_pinmux_get_funcs_count,
 	.get_function_name = sirfsoc_pinmux_get_func_name,
 	.get_function_groups = sirfsoc_pinmux_get_groups,
@@ -518,24 +526,29 @@ static int sirfsoc_gpio_irq_type(struct irq_data *d, unsigned type)
 	case IRQ_TYPE_NONE:
 		break;
 	case IRQ_TYPE_EDGE_RISING:
-		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
+		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK |
+			SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
 		val &= ~SIRFSOC_GPIO_CTL_INTR_LOW_MASK;
 		break;
 	case IRQ_TYPE_EDGE_FALLING:
 		val &= ~SIRFSOC_GPIO_CTL_INTR_HIGH_MASK;
-		val |= SIRFSOC_GPIO_CTL_INTR_LOW_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
+		val |= SIRFSOC_GPIO_CTL_INTR_LOW_MASK |
+			SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
 		break;
 	case IRQ_TYPE_EDGE_BOTH:
-		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK | SIRFSOC_GPIO_CTL_INTR_LOW_MASK |
-			 SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
+		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK |
+			SIRFSOC_GPIO_CTL_INTR_LOW_MASK |
+			SIRFSOC_GPIO_CTL_INTR_TYPE_MASK;
 		break;
 	case IRQ_TYPE_LEVEL_LOW:
-		val &= ~(SIRFSOC_GPIO_CTL_INTR_HIGH_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK);
+		val &= ~(SIRFSOC_GPIO_CTL_INTR_HIGH_MASK |
+			SIRFSOC_GPIO_CTL_INTR_TYPE_MASK);
 		val |= SIRFSOC_GPIO_CTL_INTR_LOW_MASK;
 		break;
 	case IRQ_TYPE_LEVEL_HIGH:
 		val |= SIRFSOC_GPIO_CTL_INTR_HIGH_MASK;
-		val &= ~(SIRFSOC_GPIO_CTL_INTR_LOW_MASK | SIRFSOC_GPIO_CTL_INTR_TYPE_MASK);
+		val &= ~(SIRFSOC_GPIO_CTL_INTR_LOW_MASK |
+			SIRFSOC_GPIO_CTL_INTR_TYPE_MASK);
 		break;
 	}
 
@@ -694,7 +707,8 @@ static inline void sirfsoc_gpio_set_output(struct sirfsoc_gpio_chip *sgpio,
 	spin_unlock_irqrestore(&bank->lock, flags);
 }
 
-static int sirfsoc_gpio_direction_output(struct gpio_chip *chip, unsigned gpio, int value)
+static int sirfsoc_gpio_direction_output(struct gpio_chip *chip,
+	unsigned gpio, int value)
 {
 	struct sirfsoc_gpio_chip *sgpio = to_sirfsoc_gpio(chip);
 	struct sirfsoc_gpio_bank *bank = sirfsoc_gpio_to_bank(sgpio, gpio);
@@ -839,7 +853,7 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 	if (err) {
 		dev_err(&pdev->dev,
 			"could not connect irqchip to gpiochip\n");
-		goto out;
+		goto out_banks;
 	}
 
 	for (i = 0; i < SIRFSOC_GPIO_NO_OF_BANKS; i++) {
@@ -877,8 +891,7 @@ static int sirfsoc_gpio_probe(struct device_node *np)
 
 out_no_range:
 out_banks:
-	if (gpiochip_remove(&sgpio->chip.gc))
-		dev_err(&pdev->dev, "could not remove gpio chip\n");
+	gpiochip_remove(&sgpio->chip.gc);
 out:
 	iounmap(regs);
 	return err;
@@ -898,8 +911,8 @@ static int __init sirfsoc_gpio_init(void)
 }
 subsys_initcall(sirfsoc_gpio_init);
 
-MODULE_AUTHOR("Rongjun Ying <rongjun.ying@csr.com>, "
-	"Yuping Luo <yuping.luo@csr.com>, "
-	"Barry Song <baohua.song@csr.com>");
+MODULE_AUTHOR("Rongjun Ying <rongjun.ying@csr.com>");
+MODULE_AUTHOR("Yuping Luo <yuping.luo@csr.com>");
+MODULE_AUTHOR("Barry Song <baohua.song@csr.com>");
 MODULE_DESCRIPTION("SIRFSOC pin control driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/pinctrl/spear/pinctrl-spear.c b/drivers/pinctrl/spear/pinctrl-spear.c
index f72cc4e192bd..abdb05ac43dc 100644
--- a/drivers/pinctrl/spear/pinctrl-spear.c
+++ b/drivers/pinctrl/spear/pinctrl-spear.c
@@ -268,7 +268,7 @@ static int spear_pinctrl_endisable(struct pinctrl_dev *pctldev,
 	return 0;
 }
 
-static int spear_pinctrl_enable(struct pinctrl_dev *pctldev, unsigned function,
+static int spear_pinctrl_set_mux(struct pinctrl_dev *pctldev, unsigned function,
 		unsigned group)
 {
 	return spear_pinctrl_endisable(pctldev, function, group, true);
@@ -338,7 +338,7 @@ static const struct pinmux_ops spear_pinmux_ops = {
 	.get_functions_count = spear_pinctrl_get_funcs_count,
 	.get_function_name = spear_pinctrl_get_func_name,
 	.get_function_groups = spear_pinctrl_get_func_groups,
-	.enable = spear_pinctrl_enable,
+	.set_mux = spear_pinctrl_set_mux,
 	.gpio_request_enable = gpio_request_enable,
 	.gpio_disable_free = gpio_disable_free,
 };
diff --git a/drivers/pinctrl/spear/pinctrl-spear1310.c b/drivers/pinctrl/spear/pinctrl-spear1310.c
index 1a8bbfec60ca..6d57d43ab640 100644
--- a/drivers/pinctrl/spear/pinctrl-spear1310.c
+++ b/drivers/pinctrl/spear/pinctrl-spear1310.c
@@ -2692,7 +2692,7 @@ static struct spear_pinctrl_machdata spear1310_machdata = {
 	.modes_supported = false,
 };
 
-static struct of_device_id spear1310_pinctrl_of_match[] = {
+static const struct of_device_id spear1310_pinctrl_of_match[] = {
 	{
 		.compatible = "st,spear1310-pinmux",
 	},
diff --git a/drivers/pinctrl/spear/pinctrl-spear1340.c b/drivers/pinctrl/spear/pinctrl-spear1340.c
index 873966e2b99f..d243e43e7f6d 100644
--- a/drivers/pinctrl/spear/pinctrl-spear1340.c
+++ b/drivers/pinctrl/spear/pinctrl-spear1340.c
@@ -2008,7 +2008,7 @@ static struct spear_pinctrl_machdata spear1340_machdata = {
 	.modes_supported = false,
 };
 
-static struct of_device_id spear1340_pinctrl_of_match[] = {
+static const struct of_device_id spear1340_pinctrl_of_match[] = {
 	{
 		.compatible = "st,spear1340-pinmux",
 	},
diff --git a/drivers/pinctrl/spear/pinctrl-spear300.c b/drivers/pinctrl/spear/pinctrl-spear300.c
index 4777c0d0e730..9db83e9ee18c 100644
--- a/drivers/pinctrl/spear/pinctrl-spear300.c
+++ b/drivers/pinctrl/spear/pinctrl-spear300.c
@@ -646,7 +646,7 @@ static struct spear_function *spear300_functions[] = {
 	&gpio1_function,
 };
 
-static struct of_device_id spear300_pinctrl_of_match[] = {
+static const struct of_device_id spear300_pinctrl_of_match[] = {
 	{
 		.compatible = "st,spear300-pinmux",
 	},
diff --git a/drivers/pinctrl/spear/pinctrl-spear310.c b/drivers/pinctrl/spear/pinctrl-spear310.c
index ed1d3608f486..db775a414b7a 100644
--- a/drivers/pinctrl/spear/pinctrl-spear310.c
+++ b/drivers/pinctrl/spear/pinctrl-spear310.c
@@ -371,7 +371,7 @@ static struct spear_function *spear310_functions[] = {
 	&tdm_function,
 };
 
-static struct of_device_id spear310_pinctrl_of_match[] = {
+static const struct of_device_id spear310_pinctrl_of_match[] = {
 	{
 		.compatible = "st,spear310-pinmux",
 	},
diff --git a/drivers/pinctrl/spear/pinctrl-spear320.c b/drivers/pinctrl/spear/pinctrl-spear320.c
index b8e290a8c8c9..80fbd68e17bc 100644
--- a/drivers/pinctrl/spear/pinctrl-spear320.c
+++ b/drivers/pinctrl/spear/pinctrl-spear320.c
@@ -3410,7 +3410,7 @@ static struct spear_function *spear320_functions[] = {
 	&i2c2_function,
 };
 
-static struct of_device_id spear320_pinctrl_of_match[] = {
+static const struct of_device_id spear320_pinctrl_of_match[] = {
 	{
 		.compatible = "st,spear320-pinmux",
 	},
diff --git a/drivers/pinctrl/sunxi/pinctrl-sunxi.c b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
index 3df66e366c87..ef9d804e55de 100644
--- a/drivers/pinctrl/sunxi/pinctrl-sunxi.c
+++ b/drivers/pinctrl/sunxi/pinctrl-sunxi.c
@@ -393,9 +393,9 @@ static void sunxi_pmx_set(struct pinctrl_dev *pctldev,
 	spin_unlock_irqrestore(&pctl->lock, flags);
 }
 
-static int sunxi_pmx_enable(struct pinctrl_dev *pctldev,
-			    unsigned function,
-			    unsigned group)
+static int sunxi_pmx_set_mux(struct pinctrl_dev *pctldev,
+			     unsigned function,
+			     unsigned group)
 {
 	struct sunxi_pinctrl *pctl = pinctrl_dev_get_drvdata(pctldev);
 	struct sunxi_pinctrl_group *g = pctl->groups + group;
@@ -441,7 +441,7 @@ static const struct pinmux_ops sunxi_pmx_ops = {
 	.get_functions_count	= sunxi_pmx_get_funcs_cnt,
 	.get_function_name	= sunxi_pmx_get_func_name,
 	.get_function_groups	= sunxi_pmx_get_func_groups,
-	.enable			= sunxi_pmx_enable,
+	.set_mux		= sunxi_pmx_set_mux,
 	.gpio_set_direction	= sunxi_pmx_gpio_set_direction,
 };
 
diff --git a/drivers/pinctrl/vt8500/pinctrl-wmt.c b/drivers/pinctrl/vt8500/pinctrl-wmt.c
index 8cea355f9a81..d055d63309e4 100644
--- a/drivers/pinctrl/vt8500/pinctrl-wmt.c
+++ b/drivers/pinctrl/vt8500/pinctrl-wmt.c
@@ -131,9 +131,9 @@ static int wmt_set_pinmux(struct wmt_pinctrl_data *data, unsigned func,
 	return 0;
 }
 
-static int wmt_pmx_enable(struct pinctrl_dev *pctldev,
-			  unsigned func_selector,
-			  unsigned group_selector)
+static int wmt_pmx_set_mux(struct pinctrl_dev *pctldev,
+			   unsigned func_selector,
+			   unsigned group_selector)
 {
 	struct wmt_pinctrl_data *data = pinctrl_dev_get_drvdata(pctldev);
 	u32 pinnum = data->pins[group_selector].number;
@@ -168,7 +168,7 @@ static struct pinmux_ops wmt_pinmux_ops = {
 	.get_functions_count = wmt_pmx_get_functions_count,
 	.get_function_name = wmt_pmx_get_function_name,
 	.get_function_groups = wmt_pmx_get_function_groups,
-	.enable = wmt_pmx_enable,
+	.set_mux = wmt_pmx_set_mux,
 	.gpio_disable_free = wmt_pmx_gpio_disable_free,
 	.gpio_set_direction = wmt_pmx_gpio_set_direction,
 };
diff --git a/drivers/platform/x86/Kconfig b/drivers/platform/x86/Kconfig
index 3bbcbf12c1fb..4dcfb7116a04 100644
--- a/drivers/platform/x86/Kconfig
+++ b/drivers/platform/x86/Kconfig
@@ -540,7 +540,7 @@ config ASUS_NB_WMI
 	  This is a driver for newer Asus notebooks. It adds extra features
 	  like wireless radio and bluetooth control, leds, hotkeys, backlight...
 
-	  For more informations, see
+	  For more information, see
 	  <file:Documentation/ABI/testing/sysfs-platform-asus-wmi>
 
 	  If you have an ACPI-WMI compatible Asus Notebook, say Y or M
@@ -553,7 +553,7 @@ config EEEPC_WMI
 	  This is a driver for newer Eee PC laptops. It adds extra features
 	  like wireless radio and bluetooth control, leds, hotkeys, backlight...
 
-	  For more informations, see
+	  For more information, see
 	  <file:Documentation/ABI/testing/sysfs-platform-asus-wmi>
 
 	  If you have an ACPI-WMI compatible Eee PC laptop (>= 1000), say Y or M
diff --git a/drivers/platform/x86/dell-wmi.c b/drivers/platform/x86/dell-wmi.c
index 390e8e33d5e3..25721bf20092 100644
--- a/drivers/platform/x86/dell-wmi.c
+++ b/drivers/platform/x86/dell-wmi.c
@@ -163,18 +163,24 @@ static void dell_wmi_notify(u32 value, void *context)
 		const struct key_entry *key;
 		int reported_key;
 		u16 *buffer_entry = (u16 *)obj->buffer.pointer;
+		int buffer_size = obj->buffer.length/2;
 
-		if (dell_new_hk_type && (buffer_entry[1] != 0x10)) {
+		if (buffer_size >= 2 && dell_new_hk_type && buffer_entry[1] != 0x10) {
 			pr_info("Received unknown WMI event (0x%x)\n",
 				buffer_entry[1]);
 			kfree(obj);
 			return;
 		}
 
-		if (dell_new_hk_type || buffer_entry[1] == 0x0)
+		if (buffer_size >= 3 && (dell_new_hk_type || buffer_entry[1] == 0x0))
 			reported_key = (int)buffer_entry[2];
-		else
+		else if (buffer_size >= 2)
 			reported_key = (int)buffer_entry[1] & 0xffff;
+		else {
+			pr_info("Received unknown WMI event\n");
+			kfree(obj);
+			return;
+		}
 
 		key = sparse_keymap_entry_from_scancode(dell_wmi_input_dev,
 							reported_key);
diff --git a/drivers/platform/x86/eeepc-laptop.c b/drivers/platform/x86/eeepc-laptop.c
index bd533c22be57..db79902c4a8e 100644
--- a/drivers/platform/x86/eeepc-laptop.c
+++ b/drivers/platform/x86/eeepc-laptop.c
@@ -263,13 +263,11 @@ static int acpi_setter_handle(struct eeepc_laptop *eeepc, int cm,
 /*
  * Sys helpers
  */
-static int parse_arg(const char *buf, unsigned long count, int *val)
+static int parse_arg(const char *buf, int *val)
 {
-	if (!count)
-		return 0;
 	if (sscanf(buf, "%i", val) != 1)
 		return -EINVAL;
-	return count;
+	return 0;
 }
 
 static ssize_t store_sys_acpi(struct device *dev, int cm,
@@ -278,12 +276,13 @@ static ssize_t store_sys_acpi(struct device *dev, int cm,
 	struct eeepc_laptop *eeepc = dev_get_drvdata(dev);
 	int rv, value;
 
-	rv = parse_arg(buf, count, &value);
-	if (rv > 0)
-		value = set_acpi(eeepc, cm, value);
-	if (value < 0)
+	rv = parse_arg(buf, &value);
+	if (rv < 0)
+		return rv;
+	rv = set_acpi(eeepc, cm, value);
+	if (rv < 0)
 		return -EIO;
-	return rv;
+	return count;
 }
 
 static ssize_t show_sys_acpi(struct device *dev, int cm, char *buf)
@@ -296,30 +295,34 @@ static ssize_t show_sys_acpi(struct device *dev, int cm, char *buf)
 	return sprintf(buf, "%d\n", value);
 }
 
-#define EEEPC_CREATE_DEVICE_ATTR(_name, _mode, _cm)			\
-	static ssize_t show_##_name(struct device *dev,			\
+#define EEEPC_ACPI_SHOW_FUNC(_name, _cm)				\
+	static ssize_t _name##_show(struct device *dev,			\
 				    struct device_attribute *attr,	\
 				    char *buf)				\
 	{								\
 		return show_sys_acpi(dev, _cm, buf);			\
-	}								\
-	static ssize_t store_##_name(struct device *dev,		\
+	}
+
+#define EEEPC_ACPI_STORE_FUNC(_name, _cm)				\
+	static ssize_t _name##_store(struct device *dev,		\
 				     struct device_attribute *attr,	\
 				     const char *buf, size_t count)	\
 	{								\
 		return store_sys_acpi(dev, _cm, buf, count);		\
-	}								\
-	static struct device_attribute dev_attr_##_name = {		\
-		.attr = {						\
-			.name = __stringify(_name),			\
-			.mode = _mode },				\
-		.show   = show_##_name,					\
-		.store  = store_##_name,				\
 	}
 
-EEEPC_CREATE_DEVICE_ATTR(camera, 0644, CM_ASL_CAMERA);
-EEEPC_CREATE_DEVICE_ATTR(cardr, 0644, CM_ASL_CARDREADER);
-EEEPC_CREATE_DEVICE_ATTR(disp, 0200, CM_ASL_DISPLAYSWITCH);
+#define EEEPC_CREATE_DEVICE_ATTR_RW(_name, _cm)				\
+	EEEPC_ACPI_SHOW_FUNC(_name, _cm)				\
+	EEEPC_ACPI_STORE_FUNC(_name, _cm)				\
+	static DEVICE_ATTR_RW(_name)
+
+#define EEEPC_CREATE_DEVICE_ATTR_WO(_name, _cm)				\
+	EEEPC_ACPI_STORE_FUNC(_name, _cm)				\
+	static DEVICE_ATTR_WO(_name)
+
+EEEPC_CREATE_DEVICE_ATTR_RW(camera, CM_ASL_CAMERA);
+EEEPC_CREATE_DEVICE_ATTR_RW(cardr, CM_ASL_CARDREADER);
+EEEPC_CREATE_DEVICE_ATTR_WO(disp, CM_ASL_DISPLAYSWITCH);
 
 struct eeepc_cpufv {
 	int num;
@@ -329,14 +332,17 @@ struct eeepc_cpufv {
 static int get_cpufv(struct eeepc_laptop *eeepc, struct eeepc_cpufv *c)
 {
 	c->cur = get_acpi(eeepc, CM_ASL_CPUFV);
+	if (c->cur < 0)
+		return -ENODEV;
+
 	c->num = (c->cur >> 8) & 0xff;
 	c->cur &= 0xff;
-	if (c->cur < 0 || c->num <= 0 || c->num > 12)
+	if (c->num == 0 || c->num > 12)
 		return -ENODEV;
 	return 0;
 }
 
-static ssize_t show_available_cpufv(struct device *dev,
+static ssize_t available_cpufv_show(struct device *dev,
 				    struct device_attribute *attr,
 				    char *buf)
 {
@@ -353,7 +359,7 @@ static ssize_t show_available_cpufv(struct device *dev,
 	return len;
 }
 
-static ssize_t show_cpufv(struct device *dev,
+static ssize_t cpufv_show(struct device *dev,
 			  struct device_attribute *attr,
 			  char *buf)
 {
@@ -365,7 +371,7 @@ static ssize_t show_cpufv(struct device *dev,
 	return sprintf(buf, "%#x\n", (c.num << 8) | c.cur);
 }
 
-static ssize_t store_cpufv(struct device *dev,
+static ssize_t cpufv_store(struct device *dev,
 			   struct device_attribute *attr,
 			   const char *buf, size_t count)
 {
@@ -377,16 +383,18 @@ static ssize_t store_cpufv(struct device *dev,
 		return -EPERM;
 	if (get_cpufv(eeepc, &c))
 		return -ENODEV;
-	rv = parse_arg(buf, count, &value);
+	rv = parse_arg(buf, &value);
 	if (rv < 0)
 		return rv;
-	if (!rv || value < 0 || value >= c.num)
+	if (value < 0 || value >= c.num)
 		return -EINVAL;
-	set_acpi(eeepc, CM_ASL_CPUFV, value);
-	return rv;
+	rv = set_acpi(eeepc, CM_ASL_CPUFV, value);
+	if (rv)
+		return rv;
+	return count;
 }
 
-static ssize_t show_cpufv_disabled(struct device *dev,
+static ssize_t cpufv_disabled_show(struct device *dev,
 			  struct device_attribute *attr,
 			  char *buf)
 {
@@ -395,14 +403,14 @@ static ssize_t show_cpufv_disabled(struct device *dev,
 	return sprintf(buf, "%d\n", eeepc->cpufv_disabled);
 }
 
-static ssize_t store_cpufv_disabled(struct device *dev,
+static ssize_t cpufv_disabled_store(struct device *dev,
 			   struct device_attribute *attr,
 			   const char *buf, size_t count)
 {
 	struct eeepc_laptop *eeepc = dev_get_drvdata(dev);
 	int rv, value;
 
-	rv = parse_arg(buf, count, &value);
+	rv = parse_arg(buf, &value);
 	if (rv < 0)
 		return rv;
 
@@ -412,7 +420,7 @@ static ssize_t store_cpufv_disabled(struct device *dev,
 			pr_warn("cpufv enabled (not officially supported "
 				"on this model)\n");
 		eeepc->cpufv_disabled = false;
-		return rv;
+		return count;
 	case 1:
 		return -EPERM;
 	default:
@@ -421,29 +429,9 @@ static ssize_t store_cpufv_disabled(struct device *dev,
 }
 
 
-static struct device_attribute dev_attr_cpufv = {
-	.attr = {
-		.name = "cpufv",
-		.mode = 0644 },
-	.show   = show_cpufv,
-	.store  = store_cpufv
-};
-
-static struct device_attribute dev_attr_available_cpufv = {
-	.attr = {
-		.name = "available_cpufv",
-		.mode = 0444 },
-	.show   = show_available_cpufv
-};
-
-static struct device_attribute dev_attr_cpufv_disabled = {
-	.attr = {
-		.name = "cpufv_disabled",
-		.mode = 0644 },
-	.show   = show_cpufv_disabled,
-	.store  = store_cpufv_disabled
-};
-
+static DEVICE_ATTR_RW(cpufv);
+static DEVICE_ATTR_RO(available_cpufv);
+static DEVICE_ATTR_RW(cpufv_disabled);
 
 static struct attribute *platform_attributes[] = {
 	&dev_attr_camera.attr,
@@ -545,7 +533,7 @@ static int eeepc_led_init(struct eeepc_laptop *eeepc)
 	eeepc->tpd_led.name = "eeepc::touchpad";
 	eeepc->tpd_led.brightness_set = tpd_led_set;
 	if (get_acpi(eeepc, CM_ASL_TPD) >= 0) /* if method is available */
-	  eeepc->tpd_led.brightness_get = tpd_led_get;
+		eeepc->tpd_led.brightness_get = tpd_led_get;
 	eeepc->tpd_led.max_brightness = 1;
 
 	rv = led_classdev_register(&eeepc->platform_device->dev,
@@ -680,22 +668,21 @@ static int eeepc_register_rfkill_notifier(struct eeepc_laptop *eeepc,
 
 	status = acpi_get_handle(NULL, node, &handle);
 
-	if (ACPI_SUCCESS(status)) {
-		status = acpi_install_notify_handler(handle,
-						     ACPI_SYSTEM_NOTIFY,
-						     eeepc_rfkill_notify,
-						     eeepc);
-		if (ACPI_FAILURE(status))
-			pr_warn("Failed to register notify on %s\n", node);
-
-		/*
-		 * Refresh pci hotplug in case the rfkill state was
-		 * changed during setup.
-		 */
-		eeepc_rfkill_hotplug(eeepc, handle);
-	} else
+	if (ACPI_FAILURE(status))
 		return -ENODEV;
 
+	status = acpi_install_notify_handler(handle,
+					     ACPI_SYSTEM_NOTIFY,
+					     eeepc_rfkill_notify,
+					     eeepc);
+	if (ACPI_FAILURE(status))
+		pr_warn("Failed to register notify on %s\n", node);
+
+	/*
+	 * Refresh pci hotplug in case the rfkill state was
+	 * changed during setup.
+	 */
+	eeepc_rfkill_hotplug(eeepc, handle);
 	return 0;
 }
 
@@ -707,20 +694,21 @@ static void eeepc_unregister_rfkill_notifier(struct eeepc_laptop *eeepc,
 
 	status = acpi_get_handle(NULL, node, &handle);
 
-	if (ACPI_SUCCESS(status)) {
-		status = acpi_remove_notify_handler(handle,
-						     ACPI_SYSTEM_NOTIFY,
-						     eeepc_rfkill_notify);
-		if (ACPI_FAILURE(status))
-			pr_err("Error removing rfkill notify handler %s\n",
-				node);
-			/*
-			 * Refresh pci hotplug in case the rfkill
-			 * state was changed after
-			 * eeepc_unregister_rfkill_notifier()
-			 */
-		eeepc_rfkill_hotplug(eeepc, handle);
-	}
+	if (ACPI_FAILURE(status))
+		return;
+
+	status = acpi_remove_notify_handler(handle,
+					     ACPI_SYSTEM_NOTIFY,
+					     eeepc_rfkill_notify);
+	if (ACPI_FAILURE(status))
+		pr_err("Error removing rfkill notify handler %s\n",
+			node);
+		/*
+		 * Refresh pci hotplug in case the rfkill
+		 * state was changed after
+		 * eeepc_unregister_rfkill_notifier()
+		 */
+	eeepc_rfkill_hotplug(eeepc, handle);
 }
 
 static int eeepc_get_adapter_status(struct hotplug_slot *hotplug_slot,
@@ -1042,10 +1030,11 @@ static ssize_t store_sys_hwmon(void (*set)(int), const char *buf, size_t count)
 {
 	int rv, value;
 
-	rv = parse_arg(buf, count, &value);
-	if (rv > 0)
-		set(value);
-	return rv;
+	rv = parse_arg(buf, &value);
+	if (rv < 0)
+		return rv;
+	set(value);
+	return count;
 }
 
 static ssize_t show_sys_hwmon(int (*get)(void), char *buf)
@@ -1053,26 +1042,36 @@ static ssize_t show_sys_hwmon(int (*get)(void), char *buf)
 	return sprintf(buf, "%d\n", get());
 }
 
-#define EEEPC_CREATE_SENSOR_ATTR(_name, _mode, _get, _set)		\
-	static ssize_t show_##_name(struct device *dev,			\
+#define EEEPC_SENSOR_SHOW_FUNC(_name, _get)				\
+	static ssize_t _name##_show(struct device *dev,			\
 				    struct device_attribute *attr,	\
 				    char *buf)				\
 	{								\
 		return show_sys_hwmon(_get, buf);			\
-	}								\
-	static ssize_t store_##_name(struct device *dev,		\
+	}
+
+#define EEEPC_SENSOR_STORE_FUNC(_name, _set)				\
+	static ssize_t _name##_store(struct device *dev,		\
 				     struct device_attribute *attr,	\
 				     const char *buf, size_t count)	\
 	{								\
 		return store_sys_hwmon(_set, buf, count);		\
-	}								\
-	static DEVICE_ATTR(_name, _mode, show_##_name, store_##_name)
+	}
+
+#define EEEPC_CREATE_SENSOR_ATTR_RW(_name, _get, _set)			\
+	EEEPC_SENSOR_SHOW_FUNC(_name, _get)				\
+	EEEPC_SENSOR_STORE_FUNC(_name, _set)				\
+	static DEVICE_ATTR_RW(_name)
 
-EEEPC_CREATE_SENSOR_ATTR(fan1_input, S_IRUGO, eeepc_get_fan_rpm, NULL);
-EEEPC_CREATE_SENSOR_ATTR(pwm1, S_IRUGO | S_IWUSR,
-			 eeepc_get_fan_pwm, eeepc_set_fan_pwm);
-EEEPC_CREATE_SENSOR_ATTR(pwm1_enable, S_IRUGO | S_IWUSR,
-			 eeepc_get_fan_ctrl, eeepc_set_fan_ctrl);
+#define EEEPC_CREATE_SENSOR_ATTR_RO(_name, _get)			\
+	EEEPC_SENSOR_SHOW_FUNC(_name, _get)				\
+	static DEVICE_ATTR_RO(_name)
+
+EEEPC_CREATE_SENSOR_ATTR_RO(fan1_input, eeepc_get_fan_rpm);
+EEEPC_CREATE_SENSOR_ATTR_RW(pwm1, eeepc_get_fan_pwm,
+			    eeepc_set_fan_pwm);
+EEEPC_CREATE_SENSOR_ATTR_RW(pwm1_enable, eeepc_get_fan_ctrl,
+			    eeepc_set_fan_ctrl);
 
 static struct attribute *hwmon_attrs[] = {
 	&dev_attr_pwm1.attr,
@@ -1424,8 +1423,9 @@ static int eeepc_acpi_add(struct acpi_device *device)
 		result = eeepc_backlight_init(eeepc);
 		if (result)
 			goto fail_backlight;
-	} else
+	} else {
 		pr_info("Backlight controlled by ACPI video driver\n");
+	}
 
 	result = eeepc_input_init(eeepc);
 	if (result)
diff --git a/drivers/platform/x86/fujitsu-laptop.c b/drivers/platform/x86/fujitsu-laptop.c
index 87aa28c4280f..2655d4a988f3 100644
--- a/drivers/platform/x86/fujitsu-laptop.c
+++ b/drivers/platform/x86/fujitsu-laptop.c
@@ -1050,6 +1050,13 @@ static struct acpi_driver acpi_fujitsu_hotkey_driver = {
 		},
 };
 
+static const struct acpi_device_id fujitsu_ids[] __used = {
+	{ACPI_FUJITSU_HID, 0},
+	{ACPI_FUJITSU_HOTKEY_HID, 0},
+	{"", 0}
+};
+MODULE_DEVICE_TABLE(acpi, fujitsu_ids);
+
 static int __init fujitsu_init(void)
 {
 	int ret, result, max_brightness;
@@ -1208,12 +1215,3 @@ MODULE_LICENSE("GPL");
 MODULE_ALIAS("dmi:*:svnFUJITSUSIEMENS:*:pvr:rvnFUJITSU:rnFJNB1D3:*:cvrS6410:*");
 MODULE_ALIAS("dmi:*:svnFUJITSUSIEMENS:*:pvr:rvnFUJITSU:rnFJNB1E6:*:cvrS6420:*");
 MODULE_ALIAS("dmi:*:svnFUJITSU:*:pvr:rvnFUJITSU:rnFJNB19C:*:cvrS7020:*");
-
-static struct pnp_device_id pnp_ids[] __used = {
-	{.id = "FUJ02bf"},
-	{.id = "FUJ02B1"},
-	{.id = "FUJ02E3"},
-	{.id = ""}
-};
-
-MODULE_DEVICE_TABLE(pnp, pnp_ids);
diff --git a/drivers/platform/x86/ideapad-laptop.c b/drivers/platform/x86/ideapad-laptop.c
index fc468a3d95ce..02152de135b5 100644
--- a/drivers/platform/x86/ideapad-laptop.c
+++ b/drivers/platform/x86/ideapad-laptop.c
@@ -88,7 +88,6 @@ struct ideapad_private {
 	struct dentry *debug;
 	unsigned long cfg;
 	bool has_hw_rfkill_switch;
-	bool has_touchpad_control;
 };
 
 static bool no_bt_rfkill;
@@ -456,7 +455,7 @@ struct ideapad_rfk_data {
 	int type;
 };
 
-const const struct ideapad_rfk_data ideapad_rfk_data[] = {
+static const struct ideapad_rfk_data ideapad_rfk_data[] = {
 	{ "ideapad_wlan",    CFG_WIFI_BIT, VPCCMD_W_WIFI, RFKILL_TYPE_WLAN },
 	{ "ideapad_bluetooth", CFG_BT_BIT, VPCCMD_W_BT, RFKILL_TYPE_BLUETOOTH },
 	{ "ideapad_3g",        CFG_3G_BIT, VPCCMD_W_3G, RFKILL_TYPE_WWAN },
@@ -767,9 +766,6 @@ static void ideapad_sync_touchpad_state(struct ideapad_private *priv)
 {
 	unsigned long value;
 
-	if (!priv->has_touchpad_control)
-		return;
-
 	/* Without reading from EC touchpad LED doesn't switch state */
 	if (!read_ec_data(priv->adev->handle, VPCCMD_R_TOUCHPAD, &value)) {
 		/* Some IdeaPads don't really turn off touchpad - they only
@@ -833,29 +829,7 @@ static void ideapad_acpi_notify(acpi_handle handle, u32 event, void *data)
  * always results in 0 on these models, causing ideapad_laptop to wrongly
  * report all radios as hardware-blocked.
  */
-static struct dmi_system_id no_hw_rfkill_list[] = {
-	{
-		.ident = "Lenovo Yoga 2 11 / 13 / Pro",
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo Yoga 2"),
-		},
-	},
-	{}
-};
-
-/*
- * Some models don't offer touchpad ctrl through the ideapad interface, causing
- * ideapad_sync_touchpad_state to send wrong touchpad enable/disable events.
- */
-static struct dmi_system_id no_touchpad_ctrl_list[] = {
-	{
-		.ident = "Lenovo Yoga 1 series",
-		.matches = {
-			DMI_MATCH(DMI_SYS_VENDOR, "LENOVO"),
-			DMI_MATCH(DMI_PRODUCT_VERSION, "Lenovo IdeaPad Yoga"),
-		},
-	},
+static const struct dmi_system_id no_hw_rfkill_list[] = {
 	{
 		.ident = "Lenovo Yoga 2 11 / 13 / Pro",
 		.matches = {
@@ -889,7 +863,6 @@ static int ideapad_acpi_add(struct platform_device *pdev)
 	priv->adev = adev;
 	priv->platform_device = pdev;
 	priv->has_hw_rfkill_switch = !dmi_check_system(no_hw_rfkill_list);
-	priv->has_touchpad_control = !dmi_check_system(no_touchpad_ctrl_list);
 
 	ret = ideapad_sysfs_init(priv);
 	if (ret)
diff --git a/drivers/platform/x86/intel-rst.c b/drivers/platform/x86/intel-rst.c
index d45bca34bf1b..7344d841f4d9 100644
--- a/drivers/platform/x86/intel-rst.c
+++ b/drivers/platform/x86/intel-rst.c
@@ -35,7 +35,7 @@ static ssize_t irst_show_wakeup_events(struct device *dev,
 	acpi = to_acpi_device(dev);
 
 	status = acpi_evaluate_integer(acpi->handle, "GFFS", NULL, &value);
-	if (!ACPI_SUCCESS(status))
+	if (ACPI_FAILURE(status))
 		return -EINVAL;
 
 	return sprintf(buf, "%lld\n", value);
@@ -59,7 +59,7 @@ static ssize_t irst_store_wakeup_events(struct device *dev,
 
 	status = acpi_execute_simple_method(acpi->handle, "SFFS", value);
 
-	if (!ACPI_SUCCESS(status))
+	if (ACPI_FAILURE(status))
 		return -EINVAL;
 
 	return count;
@@ -81,7 +81,7 @@ static ssize_t irst_show_wakeup_time(struct device *dev,
 	acpi = to_acpi_device(dev);
 
 	status = acpi_evaluate_integer(acpi->handle, "GFTV", NULL, &value);
-	if (!ACPI_SUCCESS(status))
+	if (ACPI_FAILURE(status))
 		return -EINVAL;
 
 	return sprintf(buf, "%lld\n", value);
@@ -105,7 +105,7 @@ static ssize_t irst_store_wakeup_time(struct device *dev,
 
 	status = acpi_execute_simple_method(acpi->handle, "SFTV", value);
 
-	if (!ACPI_SUCCESS(status))
+	if (ACPI_FAILURE(status))
 		return -EINVAL;
 
 	return count;
@@ -119,21 +119,16 @@ static struct device_attribute irst_timeout_attr = {
 
 static int irst_add(struct acpi_device *acpi)
 {
-	int error = 0;
+	int error;
 
 	error = device_create_file(&acpi->dev, &irst_timeout_attr);
-	if (error)
-		goto out;
+	if (unlikely(error))
+		return error;
 
 	error = device_create_file(&acpi->dev, &irst_wakeup_attr);
-	if (error)
-		goto out_timeout;
+	if (unlikely(error))
+		device_remove_file(&acpi->dev, &irst_timeout_attr);
 
-	return 0;
-
-out_timeout:
-	device_remove_file(&acpi->dev, &irst_timeout_attr);
-out:
 	return error;
 }
 
diff --git a/drivers/platform/x86/intel_pmic_gpio.c b/drivers/platform/x86/intel_pmic_gpio.c
index 40929e4f7ad7..04fed00b88e9 100644
--- a/drivers/platform/x86/intel_pmic_gpio.c
+++ b/drivers/platform/x86/intel_pmic_gpio.c
@@ -301,8 +301,7 @@ static int platform_pmic_gpio_probe(struct platform_device *pdev)
 	return 0;
 
 fail_request_irq:
-	if (gpiochip_remove(&pg->chip))
-		pr_err("gpiochip_remove failed\n");
+	gpiochip_remove(&pg->chip);
 err:
 	iounmap(pg->gpiointr);
 err2:
diff --git a/drivers/platform/x86/thinkpad_acpi.c b/drivers/platform/x86/thinkpad_acpi.c
index 3bbc6eb60de5..cf0f89364d44 100644
--- a/drivers/platform/x86/thinkpad_acpi.c
+++ b/drivers/platform/x86/thinkpad_acpi.c
@@ -3440,7 +3440,7 @@ err_exit:
 	delete_attr_set(hotkey_dev_attributes, &tpacpi_pdev->dev.kobj);
 	hotkey_dev_attributes = NULL;
 
-	return (res < 0)? res : 1;
+	return (res < 0) ? res : 1;
 }
 
 /* Thinkpad X1 Carbon support 5 modes including Home mode, Web browser
@@ -4576,7 +4576,7 @@ static int __init video_init(struct ibm_init_struct *iibm)
 		str_supported(video_supported != TPACPI_VIDEO_NONE),
 		video_supported);
 
-	return (video_supported != TPACPI_VIDEO_NONE)? 0 : 1;
+	return (video_supported != TPACPI_VIDEO_NONE) ? 0 : 1;
 }
 
 static void video_exit(void)
@@ -4669,7 +4669,7 @@ static int video_outputsw_set(int status)
 		return -ENOSYS;
 	}
 
-	return (res)? 0 : -EIO;
+	return (res) ? 0 : -EIO;
 }
 
 static int video_autosw_get(void)
@@ -4695,7 +4695,7 @@ static int video_autosw_get(void)
 
 static int video_autosw_set(int enable)
 {
-	if (!acpi_evalf(vid_handle, NULL, "_DOS", "vd", (enable)? 1 : 0))
+	if (!acpi_evalf(vid_handle, NULL, "_DOS", "vd", (enable) ? 1 : 0))
 		return -EIO;
 	return 0;
 }
@@ -4730,20 +4730,20 @@ static int video_outputsw_cycle(void)
 		return -EIO;
 	}
 
-	return (res)? 0 : -EIO;
+	return (res) ? 0 : -EIO;
 }
 
 static int video_expand_toggle(void)
 {
 	switch (video_supported) {
 	case TPACPI_VIDEO_570:
-		return acpi_evalf(ec_handle, NULL, "_Q17", "v")?
+		return acpi_evalf(ec_handle, NULL, "_Q17", "v") ?
 			0 : -EIO;
 	case TPACPI_VIDEO_770:
-		return acpi_evalf(vid_handle, NULL, "VEXP", "v")?
+		return acpi_evalf(vid_handle, NULL, "VEXP", "v") ?
 			0 : -EIO;
 	case TPACPI_VIDEO_NEW:
-		return acpi_evalf(NULL, NULL, "\\VEXP", "v")?
+		return acpi_evalf(NULL, NULL, "\\VEXP", "v") ?
 			0 : -EIO;
 	default:
 		return -ENOSYS;
@@ -4887,14 +4887,14 @@ static int light_set_status(int status)
 	if (tp_features.light) {
 		if (cmos_handle) {
 			rc = acpi_evalf(cmos_handle, NULL, NULL, "vd",
-					(status)?
+					(status) ?
 						TP_CMOS_THINKLIGHT_ON :
 						TP_CMOS_THINKLIGHT_OFF);
 		} else {
 			rc = acpi_evalf(lght_handle, NULL, NULL, "vd",
-					(status)? 1 : 0);
+					(status) ? 1 : 0);
 		}
-		return (rc)? 0 : -EIO;
+		return (rc) ? 0 : -EIO;
 	}
 
 	return -ENXIO;
@@ -4923,7 +4923,7 @@ static void light_sysfs_set(struct led_classdev *led_cdev,
 
 static enum led_brightness light_sysfs_get(struct led_classdev *led_cdev)
 {
-	return (light_get_status() == 1)? LED_FULL : LED_OFF;
+	return (light_get_status() == 1) ? LED_FULL : LED_OFF;
 }
 
 static struct tpacpi_led_classdev tpacpi_led_thinklight = {
@@ -5045,7 +5045,7 @@ static ssize_t cmos_command_store(struct device *dev,
 		return -EINVAL;
 
 	res = issue_thinkpad_cmos_command(cmos_cmd);
-	return (res)? res : count;
+	return (res) ? res : count;
 }
 
 static struct device_attribute dev_attr_cmos_command =
@@ -5069,7 +5069,7 @@ static int __init cmos_init(struct ibm_init_struct *iibm)
 	if (res)
 		return res;
 
-	return (cmos_handle)? 0 : 1;
+	return (cmos_handle) ? 0 : 1;
 }
 
 static void cmos_exit(void)
@@ -5179,9 +5179,9 @@ static int led_get_status(const unsigned int led)
 		if (!acpi_evalf(ec_handle,
 				&status, "GLED", "dd", 1 << led))
 			return -EIO;
-		led_s = (status == 0)?
+		led_s = (status == 0) ?
 				TPACPI_LED_OFF :
-				((status == 1)?
+				((status == 1) ?
 					TPACPI_LED_ON :
 					TPACPI_LED_BLINK);
 		tpacpi_led_state_cache[led] = led_s;
@@ -5578,7 +5578,7 @@ static int __init beep_init(struct ibm_init_struct *iibm)
 
 	tp_features.beep_needs_two_args = !!(quirks & TPACPI_BEEP_Q1);
 
-	return (beep_handle)? 0 : 1;
+	return (beep_handle) ? 0 : 1;
 }
 
 static int beep_read(struct seq_file *m)
@@ -6527,7 +6527,7 @@ static int brightness_write(char *buf)
 	if (!rc && ibm_backlight_device)
 		backlight_force_update(ibm_backlight_device,
 					BACKLIGHT_UPDATE_SYSFS);
-	return (rc == -EINTR)? -ERESTARTSYS : rc;
+	return (rc == -EINTR) ? -ERESTARTSYS : rc;
 }
 
 static struct ibm_struct brightness_driver_data = {
@@ -7984,7 +7984,7 @@ static ssize_t fan_pwm1_store(struct device *dev,
 	}
 
 	mutex_unlock(&fan_mutex);
-	return (rc)? rc : count;
+	return (rc) ? rc : count;
 }
 
 static struct device_attribute dev_attr_fan_pwm1 =
@@ -8662,7 +8662,7 @@ static const char * __init str_supported(int is_supported)
 {
 	static char text_unsupported[] __initdata = "not supported";
 
-	return (is_supported)? &text_unsupported[4] : &text_unsupported[0];
+	return (is_supported) ? &text_unsupported[4] : &text_unsupported[0];
 }
 #endif /* CONFIG_THINKPAD_ACPI_DEBUG */
 
@@ -8783,7 +8783,7 @@ err_out:
 		ibm->name, ret);
 
 	ibm_exit(ibm);
-	return (ret < 0)? ret : 0;
+	return (ret < 0) ? ret : 0;
 }
 
 /* Probing */
@@ -8794,7 +8794,7 @@ static bool __pure __init tpacpi_is_fw_digit(const char c)
 }
 
 /* Most models: xxyTkkWW (#.##c); Ancient 570/600 and -SL lacks (#.##c) */
-static bool __pure __init tpacpi_is_valid_fw_id(const char* const s,
+static bool __pure __init tpacpi_is_valid_fw_id(const char * const s,
 						const char t)
 {
 	return s && strlen(s) >= 8 &&
@@ -8878,13 +8878,13 @@ static int __must_check __init get_thinkpad_model_data(
 	}
 
 	s = dmi_get_system_info(DMI_PRODUCT_VERSION);
-	if (s && !(strnicmp(s, "ThinkPad", 8) && strnicmp(s, "Lenovo", 6))) {
+	if (s && !(strncasecmp(s, "ThinkPad", 8) && strncasecmp(s, "Lenovo", 6))) {
 		tp->model_str = kstrdup(s, GFP_KERNEL);
 		if (!tp->model_str)
 			return -ENOMEM;
 	} else {
 		s = dmi_get_system_info(DMI_BIOS_VENDOR);
-		if (s && !(strnicmp(s, "Lenovo", 6))) {
+		if (s && !(strncasecmp(s, "Lenovo", 6))) {
 			tp->model_str = kstrdup(s, GFP_KERNEL);
 			if (!tp->model_str)
 				return -ENOMEM;
diff --git a/drivers/platform/x86/toshiba_acpi.c b/drivers/platform/x86/toshiba_acpi.c
index e4da61bcbf8b..ef3a1904e92f 100644
--- a/drivers/platform/x86/toshiba_acpi.c
+++ b/drivers/platform/x86/toshiba_acpi.c
@@ -71,7 +71,8 @@ MODULE_LICENSE("GPL");
 /* Toshiba ACPI method paths */
 #define METHOD_VIDEO_OUT	"\\_SB_.VALX.DSSX"
 
-/* Toshiba HCI interface definitions
+/* The Toshiba configuration interface is composed of the HCI and the SCI,
+ * which are defined as follows:
  *
  * HCI is Toshiba's "Hardware Control Interface" which is supposed to
  * be uniform across all their models.  Ideally we would just call
@@ -84,7 +85,7 @@ MODULE_LICENSE("GPL");
  * conceal differences in hardware between different models.
  */
 
-#define HCI_WORDS			6
+#define TCI_WORDS			6
 
 /* operations */
 #define HCI_SET				0xff00
@@ -95,17 +96,18 @@ MODULE_LICENSE("GPL");
 #define SCI_SET				0xf400
 
 /* return codes */
-#define HCI_SUCCESS			0x0000
-#define HCI_FAILURE			0x1000
-#define HCI_NOT_SUPPORTED		0x8000
-#define HCI_EMPTY			0x8c00
-#define HCI_DATA_NOT_AVAILABLE		0x8d20
-#define HCI_NOT_INITIALIZED		0x8d50
-#define SCI_OPEN_CLOSE_OK		0x0044
-#define SCI_ALREADY_OPEN		0x8100
-#define SCI_NOT_OPENED			0x8200
-#define SCI_INPUT_DATA_ERROR		0x8300
-#define SCI_NOT_PRESENT			0x8600
+#define TOS_SUCCESS			0x0000
+#define TOS_OPEN_CLOSE_OK		0x0044
+#define TOS_FAILURE			0x1000
+#define TOS_NOT_SUPPORTED		0x8000
+#define TOS_ALREADY_OPEN		0x8100
+#define TOS_NOT_OPENED			0x8200
+#define TOS_INPUT_DATA_ERROR		0x8300
+#define TOS_WRITE_PROTECTED		0x8400
+#define TOS_NOT_PRESENT			0x8600
+#define TOS_FIFO_EMPTY			0x8c00
+#define TOS_DATA_NOT_AVAILABLE		0x8d20
+#define TOS_NOT_INITIALIZED		0x8d50
 
 /* registers */
 #define HCI_FAN				0x0004
@@ -138,8 +140,12 @@ MODULE_LICENSE("GPL");
 #define HCI_WIRELESS_BT_PRESENT		0x0f
 #define HCI_WIRELESS_BT_ATTACH		0x40
 #define HCI_WIRELESS_BT_POWER		0x80
+#define SCI_KBD_MODE_MASK		0x1f
 #define SCI_KBD_MODE_FNZ		0x1
 #define SCI_KBD_MODE_AUTO		0x2
+#define SCI_KBD_MODE_ON			0x8
+#define SCI_KBD_MODE_OFF		0x10
+#define SCI_KBD_TIME_MAX		0x3c001a
 
 struct toshiba_acpi_dev {
 	struct acpi_device *acpi_dev;
@@ -155,6 +161,7 @@ struct toshiba_acpi_dev {
 	int force_fan;
 	int last_key_event;
 	int key_event_valid;
+	int kbd_type;
 	int kbd_mode;
 	int kbd_time;
 
@@ -190,6 +197,7 @@ static const struct key_entry toshiba_acpi_keymap[] = {
 	{ KE_KEY, 0x101, { KEY_MUTE } },
 	{ KE_KEY, 0x102, { KEY_ZOOMOUT } },
 	{ KE_KEY, 0x103, { KEY_ZOOMIN } },
+	{ KE_KEY, 0x10f, { KEY_TAB } },
 	{ KE_KEY, 0x12c, { KEY_KBDILLUMTOGGLE } },
 	{ KE_KEY, 0x139, { KEY_ZOOMRESET } },
 	{ KE_KEY, 0x13b, { KEY_COFFEE } },
@@ -210,7 +218,11 @@ static const struct key_entry toshiba_acpi_keymap[] = {
 	{ KE_KEY, 0xb32, { KEY_NEXTSONG } },
 	{ KE_KEY, 0xb33, { KEY_PLAYPAUSE } },
 	{ KE_KEY, 0xb5a, { KEY_MEDIA } },
-	{ KE_IGNORE, 0x1430, { KEY_RESERVED } },
+	{ KE_IGNORE, 0x1430, { KEY_RESERVED } }, /* Wake from sleep */
+	{ KE_IGNORE, 0x1501, { KEY_RESERVED } }, /* Output changed */
+	{ KE_IGNORE, 0x1502, { KEY_RESERVED } }, /* HDMI plugged/unplugged */
+	{ KE_IGNORE, 0x1ABE, { KEY_RESERVED } }, /* Protection level set */
+	{ KE_IGNORE, 0x1ABF, { KEY_RESERVED } }, /* Protection level off */
 	{ KE_END, 0 },
 };
 
@@ -264,22 +276,22 @@ static int write_acpi_int(const char *methodName, int val)
 	return (status == AE_OK) ? 0 : -EIO;
 }
 
-/* Perform a raw HCI call.  Here we don't care about input or output buffer
- * format.
+/* Perform a raw configuration call.  Here we don't care about input or output
+ * buffer format.
  */
-static acpi_status hci_raw(struct toshiba_acpi_dev *dev,
-			   const u32 in[HCI_WORDS], u32 out[HCI_WORDS])
+static acpi_status tci_raw(struct toshiba_acpi_dev *dev,
+			   const u32 in[TCI_WORDS], u32 out[TCI_WORDS])
 {
 	struct acpi_object_list params;
-	union acpi_object in_objs[HCI_WORDS];
+	union acpi_object in_objs[TCI_WORDS];
 	struct acpi_buffer results;
-	union acpi_object out_objs[HCI_WORDS + 1];
+	union acpi_object out_objs[TCI_WORDS + 1];
 	acpi_status status;
 	int i;
 
-	params.count = HCI_WORDS;
+	params.count = TCI_WORDS;
 	params.pointer = in_objs;
-	for (i = 0; i < HCI_WORDS; ++i) {
+	for (i = 0; i < TCI_WORDS; ++i) {
 		in_objs[i].type = ACPI_TYPE_INTEGER;
 		in_objs[i].integer.value = in[i];
 	}
@@ -290,7 +302,7 @@ static acpi_status hci_raw(struct toshiba_acpi_dev *dev,
 	status = acpi_evaluate_object(dev->acpi_dev->handle,
 				      (char *)dev->method_hci, &params,
 				      &results);
-	if ((status == AE_OK) && (out_objs->package.count <= HCI_WORDS)) {
+	if ((status == AE_OK) && (out_objs->package.count <= TCI_WORDS)) {
 		for (i = 0; i < out_objs->package.count; ++i) {
 			out[i] = out_objs->package.elements[i].integer.value;
 		}
@@ -305,47 +317,49 @@ static acpi_status hci_raw(struct toshiba_acpi_dev *dev,
  * may be useful (such as "not supported").
  */
 
-static acpi_status hci_write1(struct toshiba_acpi_dev *dev, u32 reg,
-			      u32 in1, u32 *result)
+static u32 hci_write1(struct toshiba_acpi_dev *dev, u32 reg, u32 in1)
 {
-	u32 in[HCI_WORDS] = { HCI_SET, reg, in1, 0, 0, 0 };
-	u32 out[HCI_WORDS];
-	acpi_status status = hci_raw(dev, in, out);
-	*result = (status == AE_OK) ? out[0] : HCI_FAILURE;
-	return status;
+	u32 in[TCI_WORDS] = { HCI_SET, reg, in1, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status = tci_raw(dev, in, out);
+
+	return ACPI_SUCCESS(status) ? out[0] : TOS_FAILURE;
 }
 
-static acpi_status hci_read1(struct toshiba_acpi_dev *dev, u32 reg,
-			     u32 *out1, u32 *result)
+static u32 hci_read1(struct toshiba_acpi_dev *dev, u32 reg, u32 *out1)
 {
-	u32 in[HCI_WORDS] = { HCI_GET, reg, 0, 0, 0, 0 };
-	u32 out[HCI_WORDS];
-	acpi_status status = hci_raw(dev, in, out);
+	u32 in[TCI_WORDS] = { HCI_GET, reg, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status))
+		return TOS_FAILURE;
+
 	*out1 = out[2];
-	*result = (status == AE_OK) ? out[0] : HCI_FAILURE;
-	return status;
+
+	return out[0];
 }
 
-static acpi_status hci_write2(struct toshiba_acpi_dev *dev, u32 reg,
-			      u32 in1, u32 in2, u32 *result)
+static u32 hci_write2(struct toshiba_acpi_dev *dev, u32 reg, u32 in1, u32 in2)
 {
-	u32 in[HCI_WORDS] = { HCI_SET, reg, in1, in2, 0, 0 };
-	u32 out[HCI_WORDS];
-	acpi_status status = hci_raw(dev, in, out);
-	*result = (status == AE_OK) ? out[0] : HCI_FAILURE;
-	return status;
+	u32 in[TCI_WORDS] = { HCI_SET, reg, in1, in2, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status = tci_raw(dev, in, out);
+
+	return ACPI_SUCCESS(status) ? out[0] : TOS_FAILURE;
 }
 
-static acpi_status hci_read2(struct toshiba_acpi_dev *dev, u32 reg,
-			     u32 *out1, u32 *out2, u32 *result)
+static u32 hci_read2(struct toshiba_acpi_dev *dev, u32 reg, u32 *out1, u32 *out2)
 {
-	u32 in[HCI_WORDS] = { HCI_GET, reg, *out1, *out2, 0, 0 };
-	u32 out[HCI_WORDS];
-	acpi_status status = hci_raw(dev, in, out);
+	u32 in[TCI_WORDS] = { HCI_GET, reg, *out1, *out2, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status))
+		return TOS_FAILURE;
+
 	*out1 = out[2];
 	*out2 = out[3];
-	*result = (status == AE_OK) ? out[0] : HCI_FAILURE;
-	return status;
+
+	return out[0];
 }
 
 /* common sci tasks
@@ -353,22 +367,22 @@ static acpi_status hci_read2(struct toshiba_acpi_dev *dev, u32 reg,
 
 static int sci_open(struct toshiba_acpi_dev *dev)
 {
-	u32 in[HCI_WORDS] = { SCI_OPEN, 0, 0, 0, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { SCI_OPEN, 0, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
-	status = hci_raw(dev, in, out);
-	if  (ACPI_FAILURE(status) || out[0] == HCI_FAILURE) {
+	status = tci_raw(dev, in, out);
+	if  (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
 		pr_err("ACPI call to open SCI failed\n");
 		return 0;
 	}
 
-	if (out[0] == SCI_OPEN_CLOSE_OK) {
+	if (out[0] == TOS_OPEN_CLOSE_OK) {
 		return 1;
-	} else if (out[0] == SCI_ALREADY_OPEN) {
+	} else if (out[0] == TOS_ALREADY_OPEN) {
 		pr_info("Toshiba SCI already opened\n");
 		return 1;
-	} else if (out[0] == SCI_NOT_PRESENT) {
+	} else if (out[0] == TOS_NOT_PRESENT) {
 		pr_info("Toshiba SCI is not present\n");
 	}
 
@@ -377,61 +391,62 @@ static int sci_open(struct toshiba_acpi_dev *dev)
 
 static void sci_close(struct toshiba_acpi_dev *dev)
 {
-	u32 in[HCI_WORDS] = { SCI_CLOSE, 0, 0, 0, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { SCI_CLOSE, 0, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
-	status = hci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == HCI_FAILURE) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
 		pr_err("ACPI call to close SCI failed\n");
 		return;
 	}
 
-	if (out[0] == SCI_OPEN_CLOSE_OK)
+	if (out[0] == TOS_OPEN_CLOSE_OK)
 		return;
-	else if (out[0] == SCI_NOT_OPENED)
+	else if (out[0] == TOS_NOT_OPENED)
 		pr_info("Toshiba SCI not opened\n");
-	else if (out[0] == SCI_NOT_PRESENT)
+	else if (out[0] == TOS_NOT_PRESENT)
 		pr_info("Toshiba SCI is not present\n");
 }
 
-static acpi_status sci_read(struct toshiba_acpi_dev *dev, u32 reg,
-			    u32 *out1, u32 *result)
+static u32 sci_read(struct toshiba_acpi_dev *dev, u32 reg, u32 *out1)
 {
-	u32 in[HCI_WORDS] = { SCI_GET, reg, 0, 0, 0, 0 };
-	u32 out[HCI_WORDS];
-	acpi_status status = hci_raw(dev, in, out);
+	u32 in[TCI_WORDS] = { SCI_GET, reg, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status))
+		return TOS_FAILURE;
+
 	*out1 = out[2];
-	*result = (ACPI_SUCCESS(status)) ? out[0] : HCI_FAILURE;
-	return status;
+
+	return out[0];
 }
 
-static acpi_status sci_write(struct toshiba_acpi_dev *dev, u32 reg,
-			     u32 in1, u32 *result)
+static u32 sci_write(struct toshiba_acpi_dev *dev, u32 reg, u32 in1)
 {
-	u32 in[HCI_WORDS] = { SCI_SET, reg, in1, 0, 0, 0 };
-	u32 out[HCI_WORDS];
-	acpi_status status = hci_raw(dev, in, out);
-	*result = (ACPI_SUCCESS(status)) ? out[0] : HCI_FAILURE;
-	return status;
+	u32 in[TCI_WORDS] = { SCI_SET, reg, in1, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status = tci_raw(dev, in, out);
+
+	return ACPI_SUCCESS(status) ? out[0] : TOS_FAILURE;
 }
 
 /* Illumination support */
 static int toshiba_illumination_available(struct toshiba_acpi_dev *dev)
 {
-	u32 in[HCI_WORDS] = { SCI_GET, SCI_ILLUMINATION, 0, 0, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { SCI_GET, SCI_ILLUMINATION, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
 	if (!sci_open(dev))
 		return 0;
 
-	status = hci_raw(dev, in, out);
+	status = tci_raw(dev, in, out);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || out[0] == HCI_FAILURE) {
+	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
 		pr_err("ACPI call to query Illumination support failed\n");
 		return 0;
-	} else if (out[0] == HCI_NOT_SUPPORTED || out[1] != 1) {
+	} else if (out[0] == TOS_NOT_SUPPORTED) {
 		pr_info("Illumination device not available\n");
 		return 0;
 	}
@@ -445,7 +460,6 @@ static void toshiba_illumination_set(struct led_classdev *cdev,
 	struct toshiba_acpi_dev *dev = container_of(cdev,
 			struct toshiba_acpi_dev, led_dev);
 	u32 state, result;
-	acpi_status status;
 
 	/* First request : initialize communication. */
 	if (!sci_open(dev))
@@ -453,12 +467,12 @@ static void toshiba_illumination_set(struct led_classdev *cdev,
 
 	/* Switch the illumination on/off */
 	state = brightness ? 1 : 0;
-	status = sci_write(dev, SCI_ILLUMINATION, state, &result);
+	result = sci_write(dev, SCI_ILLUMINATION, state);
 	sci_close(dev);
-	if (ACPI_FAILURE(status)) {
+	if (result == TOS_FAILURE) {
 		pr_err("ACPI call for illumination failed\n");
 		return;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		pr_info("Illumination not supported\n");
 		return;
 	}
@@ -469,19 +483,18 @@ static enum led_brightness toshiba_illumination_get(struct led_classdev *cdev)
 	struct toshiba_acpi_dev *dev = container_of(cdev,
 			struct toshiba_acpi_dev, led_dev);
 	u32 state, result;
-	acpi_status status;
 
 	/* First request : initialize communication. */
 	if (!sci_open(dev))
 		return LED_OFF;
 
 	/* Check the illumination */
-	status = sci_read(dev, SCI_ILLUMINATION, &state, &result);
+	result = sci_read(dev, SCI_ILLUMINATION, &state);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || result == SCI_INPUT_DATA_ERROR) {
+	if (result == TOS_FAILURE || result == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call for illumination failed\n");
 		return LED_OFF;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		pr_info("Illumination not supported\n");
 		return LED_OFF;
 	}
@@ -490,20 +503,55 @@ static enum led_brightness toshiba_illumination_get(struct led_classdev *cdev)
 }
 
 /* KBD Illumination */
+static int toshiba_kbd_illum_available(struct toshiba_acpi_dev *dev)
+{
+	u32 in[TCI_WORDS] = { SCI_GET, SCI_KBD_ILLUM_STATUS, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
+	acpi_status status;
+
+	if (!sci_open(dev))
+		return 0;
+
+	status = tci_raw(dev, in, out);
+	sci_close(dev);
+	if (ACPI_FAILURE(status) || out[0] == TOS_INPUT_DATA_ERROR) {
+		pr_err("ACPI call to query kbd illumination support failed\n");
+		return 0;
+	} else if (out[0] == TOS_NOT_SUPPORTED) {
+		pr_info("Keyboard illumination not available\n");
+		return 0;
+	}
+
+	/* Check for keyboard backlight timeout max value,
+	 * previous kbd backlight implementation set this to
+	 * 0x3c0003, and now the new implementation set this
+	 * to 0x3c001a, use this to distinguish between them
+	 */
+	if (out[3] == SCI_KBD_TIME_MAX)
+		dev->kbd_type = 2;
+	else
+		dev->kbd_type = 1;
+	/* Get the current keyboard backlight mode */
+	dev->kbd_mode = out[2] & SCI_KBD_MODE_MASK;
+	/* Get the current time (1-60 seconds) */
+	dev->kbd_time = out[2] >> HCI_MISC_SHIFT;
+
+	return 1;
+}
+
 static int toshiba_kbd_illum_status_set(struct toshiba_acpi_dev *dev, u32 time)
 {
 	u32 result;
-	acpi_status status;
 
 	if (!sci_open(dev))
 		return -EIO;
 
-	status = sci_write(dev, SCI_KBD_ILLUM_STATUS, time, &result);
+	result = sci_write(dev, SCI_KBD_ILLUM_STATUS, time);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || result == SCI_INPUT_DATA_ERROR) {
+	if (result == TOS_FAILURE || result == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to set KBD backlight status failed\n");
 		return -EIO;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		pr_info("Keyboard backlight status not supported\n");
 		return -ENODEV;
 	}
@@ -514,17 +562,16 @@ static int toshiba_kbd_illum_status_set(struct toshiba_acpi_dev *dev, u32 time)
 static int toshiba_kbd_illum_status_get(struct toshiba_acpi_dev *dev, u32 *time)
 {
 	u32 result;
-	acpi_status status;
 
 	if (!sci_open(dev))
 		return -EIO;
 
-	status = sci_read(dev, SCI_KBD_ILLUM_STATUS, time, &result);
+	result = sci_read(dev, SCI_KBD_ILLUM_STATUS, time);
 	sci_close(dev);
-	if (ACPI_FAILURE(status) || result == SCI_INPUT_DATA_ERROR) {
+	if (result == TOS_FAILURE || result == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to get KBD backlight status failed\n");
 		return -EIO;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		pr_info("Keyboard backlight status not supported\n");
 		return -ENODEV;
 	}
@@ -537,14 +584,13 @@ static enum led_brightness toshiba_kbd_backlight_get(struct led_classdev *cdev)
 	struct toshiba_acpi_dev *dev = container_of(cdev,
 			struct toshiba_acpi_dev, kbd_led);
 	u32 state, result;
-	acpi_status status;
 
 	/* Check the keyboard backlight state */
-	status = hci_read1(dev, HCI_KBD_ILLUMINATION, &state, &result);
-	if (ACPI_FAILURE(status) || result == SCI_INPUT_DATA_ERROR) {
+	result = hci_read1(dev, HCI_KBD_ILLUMINATION, &state);
+	if (result == TOS_FAILURE || result == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to get the keyboard backlight failed\n");
 		return LED_OFF;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		pr_info("Keyboard backlight not supported\n");
 		return LED_OFF;
 	}
@@ -558,15 +604,14 @@ static void toshiba_kbd_backlight_set(struct led_classdev *cdev,
 	struct toshiba_acpi_dev *dev = container_of(cdev,
 			struct toshiba_acpi_dev, kbd_led);
 	u32 state, result;
-	acpi_status status;
 
 	/* Set the keyboard backlight state */
 	state = brightness ? 1 : 0;
-	status = hci_write1(dev, HCI_KBD_ILLUMINATION, state, &result);
-	if (ACPI_FAILURE(status) || result == SCI_INPUT_DATA_ERROR) {
+	result = hci_write1(dev, HCI_KBD_ILLUMINATION, state);
+	if (result == TOS_FAILURE || result == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to set KBD Illumination mode failed\n");
 		return;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		pr_info("Keyboard backlight not supported\n");
 		return;
 	}
@@ -576,17 +621,16 @@ static void toshiba_kbd_backlight_set(struct led_classdev *cdev,
 static int toshiba_touchpad_set(struct toshiba_acpi_dev *dev, u32 state)
 {
 	u32 result;
-	acpi_status status;
 
 	if (!sci_open(dev))
 		return -EIO;
 
-	status = sci_write(dev, SCI_TOUCHPAD, state, &result);
+	result = sci_write(dev, SCI_TOUCHPAD, state);
 	sci_close(dev);
-	if (ACPI_FAILURE(status)) {
+	if (result == TOS_FAILURE) {
 		pr_err("ACPI call to set the touchpad failed\n");
 		return -EIO;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		return -ENODEV;
 	}
 
@@ -596,17 +640,16 @@ static int toshiba_touchpad_set(struct toshiba_acpi_dev *dev, u32 state)
 static int toshiba_touchpad_get(struct toshiba_acpi_dev *dev, u32 *state)
 {
 	u32 result;
-	acpi_status status;
 
 	if (!sci_open(dev))
 		return -EIO;
 
-	status = sci_read(dev, SCI_TOUCHPAD, state, &result);
+	result = sci_read(dev, SCI_TOUCHPAD, state);
 	sci_close(dev);
-	if (ACPI_FAILURE(status)) {
+	if (result == TOS_FAILURE) {
 		pr_err("ACPI call to query the touchpad failed\n");
 		return -EIO;
-	} else if (result == HCI_NOT_SUPPORTED) {
+	} else if (result == TOS_NOT_SUPPORTED) {
 		return -ENODEV;
 	}
 
@@ -617,11 +660,11 @@ static int toshiba_touchpad_get(struct toshiba_acpi_dev *dev, u32 *state)
 static int toshiba_eco_mode_available(struct toshiba_acpi_dev *dev)
 {
 	acpi_status status;
-	u32 in[HCI_WORDS] = { HCI_GET, HCI_ECO_MODE, 0, 1, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { HCI_GET, HCI_ECO_MODE, 0, 1, 0, 0 };
+	u32 out[TCI_WORDS];
 
-	status = hci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == SCI_INPUT_DATA_ERROR) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status) || out[0] == TOS_INPUT_DATA_ERROR) {
 		pr_info("ACPI call to get ECO led failed\n");
 		return 0;
 	}
@@ -633,12 +676,12 @@ static enum led_brightness toshiba_eco_mode_get_status(struct led_classdev *cdev
 {
 	struct toshiba_acpi_dev *dev = container_of(cdev,
 			struct toshiba_acpi_dev, eco_led);
-	u32 in[HCI_WORDS] = { HCI_GET, HCI_ECO_MODE, 0, 1, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { HCI_GET, HCI_ECO_MODE, 0, 1, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
-	status = hci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == SCI_INPUT_DATA_ERROR) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status) || out[0] == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to get ECO led failed\n");
 		return LED_OFF;
 	}
@@ -651,14 +694,14 @@ static void toshiba_eco_mode_set_status(struct led_classdev *cdev,
 {
 	struct toshiba_acpi_dev *dev = container_of(cdev,
 			struct toshiba_acpi_dev, eco_led);
-	u32 in[HCI_WORDS] = { HCI_SET, HCI_ECO_MODE, 0, 1, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { HCI_SET, HCI_ECO_MODE, 0, 1, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
 	/* Switch the Eco Mode led on/off */
 	in[2] = (brightness) ? 1 : 0;
-	status = hci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == SCI_INPUT_DATA_ERROR) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status) || out[0] == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to set ECO led failed\n");
 		return;
 	}
@@ -667,22 +710,22 @@ static void toshiba_eco_mode_set_status(struct led_classdev *cdev,
 /* Accelerometer support */
 static int toshiba_accelerometer_supported(struct toshiba_acpi_dev *dev)
 {
-	u32 in[HCI_WORDS] = { HCI_GET, HCI_ACCELEROMETER2, 0, 0, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { HCI_GET, HCI_ACCELEROMETER2, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
 	/* Check if the accelerometer call exists,
 	 * this call also serves as initialization
 	 */
-	status = hci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == SCI_INPUT_DATA_ERROR) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status) || out[0] == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to query the accelerometer failed\n");
 		return -EIO;
-	} else if (out[0] == HCI_DATA_NOT_AVAILABLE ||
-		   out[0] == HCI_NOT_INITIALIZED) {
+	} else if (out[0] == TOS_DATA_NOT_AVAILABLE ||
+		   out[0] == TOS_NOT_INITIALIZED) {
 		pr_err("Accelerometer not initialized\n");
 		return -EIO;
-	} else if (out[0] == HCI_NOT_SUPPORTED) {
+	} else if (out[0] == TOS_NOT_SUPPORTED) {
 		pr_info("Accelerometer not supported\n");
 		return -ENODEV;
 	}
@@ -693,13 +736,13 @@ static int toshiba_accelerometer_supported(struct toshiba_acpi_dev *dev)
 static int toshiba_accelerometer_get(struct toshiba_acpi_dev *dev,
 				      u32 *xy, u32 *z)
 {
-	u32 in[HCI_WORDS] = { HCI_GET, HCI_ACCELEROMETER, 0, 1, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { HCI_GET, HCI_ACCELEROMETER, 0, 1, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
 	/* Check the Accelerometer status */
-	status = hci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == SCI_INPUT_DATA_ERROR) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status) || out[0] == TOS_INPUT_DATA_ERROR) {
 		pr_err("ACPI call to query the accelerometer failed\n");
 		return -EIO;
 	}
@@ -719,8 +762,8 @@ static u32 hci_get_bt_present(struct toshiba_acpi_dev *dev, bool *present)
 
 	value = 0;
 	value2 = 0;
-	hci_read2(dev, HCI_WIRELESS, &value, &value2, &hci_result);
-	if (hci_result == HCI_SUCCESS)
+	hci_result = hci_read2(dev, HCI_WIRELESS, &value, &value2);
+	if (hci_result == TOS_SUCCESS)
 		*present = (value & HCI_WIRELESS_BT_PRESENT) ? true : false;
 
 	return hci_result;
@@ -733,7 +776,7 @@ static u32 hci_get_radio_state(struct toshiba_acpi_dev *dev, bool *radio_state)
 
 	value = 0;
 	value2 = 0x0001;
-	hci_read2(dev, HCI_WIRELESS, &value, &value2, &hci_result);
+	hci_result = hci_read2(dev, HCI_WIRELESS, &value, &value2);
 
 	*radio_state = value & HCI_WIRELESS_KILL_SWITCH;
 	return hci_result;
@@ -750,7 +793,7 @@ static int bt_rfkill_set_block(void *data, bool blocked)
 	value = (blocked == false);
 
 	mutex_lock(&dev->mutex);
-	if (hci_get_radio_state(dev, &radio_state) != HCI_SUCCESS) {
+	if (hci_get_radio_state(dev, &radio_state) != TOS_SUCCESS) {
 		err = -EIO;
 		goto out;
 	}
@@ -760,10 +803,10 @@ static int bt_rfkill_set_block(void *data, bool blocked)
 		goto out;
 	}
 
-	hci_write2(dev, HCI_WIRELESS, value, HCI_WIRELESS_BT_POWER, &result1);
-	hci_write2(dev, HCI_WIRELESS, value, HCI_WIRELESS_BT_ATTACH, &result2);
+	result1 = hci_write2(dev, HCI_WIRELESS, value, HCI_WIRELESS_BT_POWER);
+	result2 = hci_write2(dev, HCI_WIRELESS, value, HCI_WIRELESS_BT_ATTACH);
 
-	if (result1 != HCI_SUCCESS || result2 != HCI_SUCCESS)
+	if (result1 != TOS_SUCCESS || result2 != TOS_SUCCESS)
 		err = -EIO;
 	else
 		err = 0;
@@ -782,7 +825,7 @@ static void bt_rfkill_poll(struct rfkill *rfkill, void *data)
 	mutex_lock(&dev->mutex);
 
 	hci_result = hci_get_radio_state(dev, &value);
-	if (hci_result != HCI_SUCCESS) {
+	if (hci_result != TOS_SUCCESS) {
 		/* Can't do anything useful */
 		mutex_unlock(&dev->mutex);
 		return;
@@ -806,9 +849,9 @@ static int get_tr_backlight_status(struct toshiba_acpi_dev *dev, bool *enabled)
 	u32 hci_result;
 	u32 status;
 
-	hci_read1(dev, HCI_TR_BACKLIGHT, &status, &hci_result);
+	hci_result = hci_read1(dev, HCI_TR_BACKLIGHT, &status);
 	*enabled = !status;
-	return hci_result == HCI_SUCCESS ? 0 : -EIO;
+	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
 static int set_tr_backlight_status(struct toshiba_acpi_dev *dev, bool enable)
@@ -816,8 +859,8 @@ static int set_tr_backlight_status(struct toshiba_acpi_dev *dev, bool enable)
 	u32 hci_result;
 	u32 value = !enable;
 
-	hci_write1(dev, HCI_TR_BACKLIGHT, value, &hci_result);
-	return hci_result == HCI_SUCCESS ? 0 : -EIO;
+	hci_result = hci_write1(dev, HCI_TR_BACKLIGHT, value);
+	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
 static struct proc_dir_entry *toshiba_proc_dir /*= 0*/ ;
@@ -838,8 +881,8 @@ static int __get_lcd_brightness(struct toshiba_acpi_dev *dev)
 		brightness++;
 	}
 
-	hci_read1(dev, HCI_LCD_BRIGHTNESS, &value, &hci_result);
-	if (hci_result == HCI_SUCCESS)
+	hci_result = hci_read1(dev, HCI_LCD_BRIGHTNESS, &value);
+	if (hci_result == TOS_SUCCESS)
 		return brightness + (value >> HCI_LCD_BRIGHTNESS_SHIFT);
 
 	return -EIO;
@@ -879,8 +922,8 @@ static int lcd_proc_open(struct inode *inode, struct file *file)
 
 static int set_lcd_brightness(struct toshiba_acpi_dev *dev, int value)
 {
-	u32 in[HCI_WORDS] = { HCI_SET, HCI_LCD_BRIGHTNESS, 0, 0, 0, 0 };
-	u32 out[HCI_WORDS];
+	u32 in[TCI_WORDS] = { HCI_SET, HCI_LCD_BRIGHTNESS, 0, 0, 0, 0 };
+	u32 out[TCI_WORDS];
 	acpi_status status;
 
 	if (dev->tr_backlight_supported) {
@@ -893,19 +936,19 @@ static int set_lcd_brightness(struct toshiba_acpi_dev *dev, int value)
 	}
 
 	in[2] = value << HCI_LCD_BRIGHTNESS_SHIFT;
-	status = hci_raw(dev, in, out);
-	if (ACPI_FAILURE(status) || out[0] == HCI_FAILURE) {
+	status = tci_raw(dev, in, out);
+	if (ACPI_FAILURE(status) || out[0] == TOS_FAILURE) {
 		pr_err("ACPI call to set brightness failed");
 		return -EIO;
 	}
 	/* Extra check for "incomplete" backlight method, where the AML code
-	 * doesn't check for HCI_SET or HCI_GET and returns HCI_SUCCESS,
+	 * doesn't check for HCI_SET or HCI_GET and returns TOS_SUCCESS,
 	 * the actual brightness, and in some cases the max brightness.
 	 */
 	if (out[2] > 0  || out[3] == 0xE000)
 		return -ENODEV;
 
-	return out[0] == HCI_SUCCESS ? 0 : -EIO;
+	return out[0] == TOS_SUCCESS ? 0 : -EIO;
 }
 
 static int set_lcd_status(struct backlight_device *bd)
@@ -953,8 +996,8 @@ static int get_video_status(struct toshiba_acpi_dev *dev, u32 *status)
 {
 	u32 hci_result;
 
-	hci_read1(dev, HCI_VIDEO_OUT, status, &hci_result);
-	return hci_result == HCI_SUCCESS ? 0 : -EIO;
+	hci_result = hci_read1(dev, HCI_VIDEO_OUT, status);
+	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
 static int video_proc_show(struct seq_file *m, void *v)
@@ -1057,8 +1100,8 @@ static int get_fan_status(struct toshiba_acpi_dev *dev, u32 *status)
 {
 	u32 hci_result;
 
-	hci_read1(dev, HCI_FAN, status, &hci_result);
-	return hci_result == HCI_SUCCESS ? 0 : -EIO;
+	hci_result = hci_read1(dev, HCI_FAN, status);
+	return hci_result == TOS_SUCCESS ? 0 : -EIO;
 }
 
 static int fan_proc_show(struct seq_file *m, void *v)
@@ -1097,8 +1140,8 @@ static ssize_t fan_proc_write(struct file *file, const char __user *buf,
 
 	if (sscanf(cmd, " force_on : %i", &value) == 1 &&
 	    value >= 0 && value <= 1) {
-		hci_write1(dev, HCI_FAN, value, &hci_result);
-		if (hci_result != HCI_SUCCESS)
+		hci_result = hci_write1(dev, HCI_FAN, value);
+		if (hci_result != TOS_SUCCESS)
 			return -EIO;
 		else
 			dev->force_fan = value;
@@ -1125,17 +1168,17 @@ static int keys_proc_show(struct seq_file *m, void *v)
 	u32 value;
 
 	if (!dev->key_event_valid && dev->system_event_supported) {
-		hci_read1(dev, HCI_SYSTEM_EVENT, &value, &hci_result);
-		if (hci_result == HCI_SUCCESS) {
+		hci_result = hci_read1(dev, HCI_SYSTEM_EVENT, &value);
+		if (hci_result == TOS_SUCCESS) {
 			dev->key_event_valid = 1;
 			dev->last_key_event = value;
-		} else if (hci_result == HCI_EMPTY) {
+		} else if (hci_result == TOS_FIFO_EMPTY) {
 			/* better luck next time */
-		} else if (hci_result == HCI_NOT_SUPPORTED) {
+		} else if (hci_result == TOS_NOT_SUPPORTED) {
 			/* This is a workaround for an unresolved issue on
 			 * some machines where system events sporadically
 			 * become disabled. */
-			hci_write1(dev, HCI_SYSTEM_EVENT, 1, &hci_result);
+			hci_result = hci_write1(dev, HCI_SYSTEM_EVENT, 1);
 			pr_notice("Re-enabled hotkeys\n");
 		} else {
 			pr_err("Error reading hotkey status\n");
@@ -1249,28 +1292,120 @@ static const struct backlight_ops toshiba_backlight_data = {
 /*
  * Sysfs files
  */
+static ssize_t toshiba_kbd_bl_mode_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count);
+static ssize_t toshiba_kbd_bl_mode_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf);
+static ssize_t toshiba_kbd_type_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf);
+static ssize_t toshiba_available_kbd_modes_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf);
+static ssize_t toshiba_kbd_bl_timeout_store(struct device *dev,
+					    struct device_attribute *attr,
+					    const char *buf, size_t count);
+static ssize_t toshiba_kbd_bl_timeout_show(struct device *dev,
+					   struct device_attribute *attr,
+					   char *buf);
+static ssize_t toshiba_touchpad_store(struct device *dev,
+				      struct device_attribute *attr,
+				      const char *buf, size_t count);
+static ssize_t toshiba_touchpad_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf);
+static ssize_t toshiba_position_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf);
+
+static DEVICE_ATTR(kbd_backlight_mode, S_IRUGO | S_IWUSR,
+		   toshiba_kbd_bl_mode_show, toshiba_kbd_bl_mode_store);
+static DEVICE_ATTR(kbd_type, S_IRUGO, toshiba_kbd_type_show, NULL);
+static DEVICE_ATTR(available_kbd_modes, S_IRUGO,
+		   toshiba_available_kbd_modes_show, NULL);
+static DEVICE_ATTR(kbd_backlight_timeout, S_IRUGO | S_IWUSR,
+		   toshiba_kbd_bl_timeout_show, toshiba_kbd_bl_timeout_store);
+static DEVICE_ATTR(touchpad, S_IRUGO | S_IWUSR,
+		   toshiba_touchpad_show, toshiba_touchpad_store);
+static DEVICE_ATTR(position, S_IRUGO, toshiba_position_show, NULL);
+
+static struct attribute *toshiba_attributes[] = {
+	&dev_attr_kbd_backlight_mode.attr,
+	&dev_attr_kbd_type.attr,
+	&dev_attr_available_kbd_modes.attr,
+	&dev_attr_kbd_backlight_timeout.attr,
+	&dev_attr_touchpad.attr,
+	&dev_attr_position.attr,
+	NULL,
+};
+
+static umode_t toshiba_sysfs_is_visible(struct kobject *,
+					struct attribute *, int);
+
+static struct attribute_group toshiba_attr_group = {
+	.is_visible = toshiba_sysfs_is_visible,
+	.attrs = toshiba_attributes,
+};
 
 static ssize_t toshiba_kbd_bl_mode_store(struct device *dev,
 					 struct device_attribute *attr,
 					 const char *buf, size_t count)
 {
 	struct toshiba_acpi_dev *toshiba = dev_get_drvdata(dev);
-	int mode = -1;
-	int time = -1;
+	int mode;
+	int time;
+	int ret;
 
-	if (sscanf(buf, "%i", &mode) != 1  || (mode != 2 || mode != 1))
-		return -EINVAL;
+
+	ret = kstrtoint(buf, 0, &mode);
+	if (ret)
+		return ret;
+
+	/* Check for supported modes depending on keyboard backlight type */
+	if (toshiba->kbd_type == 1) {
+		/* Type 1 supports SCI_KBD_MODE_FNZ and SCI_KBD_MODE_AUTO */
+		if (mode != SCI_KBD_MODE_FNZ && mode != SCI_KBD_MODE_AUTO)
+			return -EINVAL;
+	} else if (toshiba->kbd_type == 2) {
+		/* Type 2 doesn't support SCI_KBD_MODE_FNZ */
+		if (mode != SCI_KBD_MODE_AUTO && mode != SCI_KBD_MODE_ON &&
+		    mode != SCI_KBD_MODE_OFF)
+			return -EINVAL;
+	}
 
 	/* Set the Keyboard Backlight Mode where:
-	 * Mode - Auto (2) | FN-Z (1)
 	 *	Auto - KBD backlight turns off automatically in given time
 	 *	FN-Z - KBD backlight "toggles" when hotkey pressed
+	 *	ON   - KBD backlight is always on
+	 *	OFF  - KBD backlight is always off
 	 */
-	if (mode != -1 && toshiba->kbd_mode != mode) {
+
+	/* Only make a change if the actual mode has changed */
+	if (toshiba->kbd_mode != mode) {
+		/* Shift the time to "base time" (0x3c0000 == 60 seconds) */
 		time = toshiba->kbd_time << HCI_MISC_SHIFT;
-		time = time + toshiba->kbd_mode;
-		if (toshiba_kbd_illum_status_set(toshiba, time) < 0)
-			return -EIO;
+
+		/* OR the "base time" to the actual method format */
+		if (toshiba->kbd_type == 1) {
+			/* Type 1 requires the current mode */
+			time |= toshiba->kbd_mode;
+		} else if (toshiba->kbd_type == 2) {
+			/* Type 2 requires the desired mode */
+			time |= mode;
+		}
+
+		ret = toshiba_kbd_illum_status_set(toshiba, time);
+		if (ret)
+			return ret;
+
+		/* Update sysfs entries on successful mode change*/
+		ret = sysfs_update_group(&toshiba->acpi_dev->dev.kobj,
+					 &toshiba_attr_group);
+		if (ret)
+			return ret;
+
 		toshiba->kbd_mode = mode;
 	}
 
@@ -1287,7 +1422,30 @@ static ssize_t toshiba_kbd_bl_mode_show(struct device *dev,
 	if (toshiba_kbd_illum_status_get(toshiba, &time) < 0)
 		return -EIO;
 
-	return sprintf(buf, "%i\n", time & 0x07);
+	return sprintf(buf, "%i\n", time & SCI_KBD_MODE_MASK);
+}
+
+static ssize_t toshiba_kbd_type_show(struct device *dev,
+				     struct device_attribute *attr,
+				     char *buf)
+{
+	struct toshiba_acpi_dev *toshiba = dev_get_drvdata(dev);
+
+	return sprintf(buf, "%d\n", toshiba->kbd_type);
+}
+
+static ssize_t toshiba_available_kbd_modes_show(struct device *dev,
+						struct device_attribute *attr,
+						char *buf)
+{
+	struct toshiba_acpi_dev *toshiba = dev_get_drvdata(dev);
+
+	if (toshiba->kbd_type == 1)
+		return sprintf(buf, "%x %x\n",
+			       SCI_KBD_MODE_FNZ, SCI_KBD_MODE_AUTO);
+
+	return sprintf(buf, "%x %x %x\n",
+		       SCI_KBD_MODE_AUTO, SCI_KBD_MODE_ON, SCI_KBD_MODE_OFF);
 }
 
 static ssize_t toshiba_kbd_bl_timeout_store(struct device *dev,
@@ -1295,18 +1453,38 @@ static ssize_t toshiba_kbd_bl_timeout_store(struct device *dev,
 					    const char *buf, size_t count)
 {
 	struct toshiba_acpi_dev *toshiba = dev_get_drvdata(dev);
-	int time = -1;
+	int time;
+	int ret;
 
-	if (sscanf(buf, "%i", &time) != 1 && (time < 0 || time > 60))
-		return -EINVAL;
+	ret = kstrtoint(buf, 0, &time);
+	if (ret)
+		return ret;
+
+	/* Check for supported values depending on kbd_type */
+	if (toshiba->kbd_type == 1) {
+		if (time < 0 || time > 60)
+			return -EINVAL;
+	} else if (toshiba->kbd_type == 2) {
+		if (time < 1 || time > 60)
+			return -EINVAL;
+	}
+
+	/* Set the Keyboard Backlight Timeout */
 
-	/* Set the Keyboard Backlight Timeout: 0-60 seconds */
-	if (time != -1 && toshiba->kbd_time != time) {
+	/* Only make a change if the actual timeout has changed */
+	if (toshiba->kbd_time != time) {
+		/* Shift the time to "base time" (0x3c0000 == 60 seconds) */
 		time = time << HCI_MISC_SHIFT;
-		time = (toshiba->kbd_mode == SCI_KBD_MODE_AUTO) ?
-							time + 1 : time + 2;
-		if (toshiba_kbd_illum_status_set(toshiba, time) < 0)
-			return -EIO;
+		/* OR the "base time" to the actual method format */
+		if (toshiba->kbd_type == 1)
+			time |= SCI_KBD_MODE_FNZ;
+		else if (toshiba->kbd_type == 2)
+			time |= SCI_KBD_MODE_AUTO;
+
+		ret = toshiba_kbd_illum_status_set(toshiba, time);
+		if (ret)
+			return ret;
+
 		toshiba->kbd_time = time >> HCI_MISC_SHIFT;
 	}
 
@@ -1332,12 +1510,18 @@ static ssize_t toshiba_touchpad_store(struct device *dev,
 {
 	struct toshiba_acpi_dev *toshiba = dev_get_drvdata(dev);
 	int state;
+	int ret;
 
 	/* Set the TouchPad on/off, 0 - Disable | 1 - Enable */
-	if (sscanf(buf, "%i", &state) == 1 && (state == 0 || state == 1)) {
-		if (toshiba_touchpad_set(toshiba, state) < 0)
-			return -EIO;
-	}
+	ret = kstrtoint(buf, 0, &state);
+	if (ret)
+		return ret;
+	if (state != 0 && state != 1)
+		return -EINVAL;
+
+	ret = toshiba_touchpad_set(toshiba, state);
+	if (ret)
+		return ret;
 
 	return count;
 }
@@ -1377,22 +1561,6 @@ static ssize_t toshiba_position_show(struct device *dev,
 	return sprintf(buf, "%d %d %d\n", x, y, z);
 }
 
-static DEVICE_ATTR(kbd_backlight_mode, S_IRUGO | S_IWUSR,
-		   toshiba_kbd_bl_mode_show, toshiba_kbd_bl_mode_store);
-static DEVICE_ATTR(kbd_backlight_timeout, S_IRUGO | S_IWUSR,
-		   toshiba_kbd_bl_timeout_show, toshiba_kbd_bl_timeout_store);
-static DEVICE_ATTR(touchpad, S_IRUGO | S_IWUSR,
-		   toshiba_touchpad_show, toshiba_touchpad_store);
-static DEVICE_ATTR(position, S_IRUGO, toshiba_position_show, NULL);
-
-static struct attribute *toshiba_attributes[] = {
-	&dev_attr_kbd_backlight_mode.attr,
-	&dev_attr_kbd_backlight_timeout.attr,
-	&dev_attr_touchpad.attr,
-	&dev_attr_position.attr,
-	NULL,
-};
-
 static umode_t toshiba_sysfs_is_visible(struct kobject *kobj,
 					struct attribute *attr, int idx)
 {
@@ -1412,11 +1580,6 @@ static umode_t toshiba_sysfs_is_visible(struct kobject *kobj,
 	return exists ? attr->mode : 0;
 }
 
-static struct attribute_group toshiba_attr_group = {
-	.is_visible = toshiba_sysfs_is_visible,
-	.attrs = toshiba_attributes,
-};
-
 static bool toshiba_acpi_i8042_filter(unsigned char data, unsigned char str,
 				      struct serio *port)
 {
@@ -1529,8 +1692,8 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 	if (acpi_has_method(dev->acpi_dev->handle, "INFO"))
 		dev->info_supported = 1;
 	else {
-		hci_write1(dev, HCI_SYSTEM_EVENT, 1, &hci_result);
-		if (hci_result == HCI_SUCCESS)
+		hci_result = hci_write1(dev, HCI_SYSTEM_EVENT, 1);
+		if (hci_result == TOS_SUCCESS)
 			dev->system_event_supported = 1;
 	}
 
@@ -1552,7 +1715,7 @@ static int toshiba_acpi_setup_keyboard(struct toshiba_acpi_dev *dev)
 		goto err_remove_filter;
 	}
 
-	hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE, &hci_result);
+	hci_result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE);
 	return 0;
 
  err_remove_filter:
@@ -1710,7 +1873,7 @@ static int toshiba_acpi_add(struct acpi_device *acpi_dev)
 		goto error;
 
 	/* Register rfkill switch for Bluetooth */
-	if (hci_get_bt_present(dev, &bt_present) == HCI_SUCCESS && bt_present) {
+	if (hci_get_bt_present(dev, &bt_present) == TOS_SUCCESS && bt_present) {
 		dev->bt_rfk = rfkill_alloc("Toshiba Bluetooth",
 					   &acpi_dev->dev,
 					   RFKILL_TYPE_BLUETOOTH,
@@ -1748,12 +1911,7 @@ static int toshiba_acpi_add(struct acpi_device *acpi_dev)
 			dev->eco_supported = 1;
 	}
 
-	ret = toshiba_kbd_illum_status_get(dev, &dummy);
-	if (!ret) {
-		dev->kbd_time = dummy >> HCI_MISC_SHIFT;
-		dev->kbd_mode = dummy & 0x07;
-	}
-	dev->kbd_illum_supported = !ret;
+	dev->kbd_illum_supported = toshiba_kbd_illum_available(dev);
 	/*
 	 * Only register the LED if KBD illumination is supported
 	 * and the keyboard backlight operation mode is set to FN-Z
@@ -1818,26 +1976,26 @@ static void toshiba_acpi_notify(struct acpi_device *acpi_dev, u32 event)
 			toshiba_acpi_report_hotkey(dev, scancode);
 	} else if (dev->system_event_supported) {
 		do {
-			hci_read1(dev, HCI_SYSTEM_EVENT, &value, &hci_result);
+			hci_result = hci_read1(dev, HCI_SYSTEM_EVENT, &value);
 			switch (hci_result) {
-			case HCI_SUCCESS:
+			case TOS_SUCCESS:
 				toshiba_acpi_report_hotkey(dev, (int)value);
 				break;
-			case HCI_NOT_SUPPORTED:
+			case TOS_NOT_SUPPORTED:
 				/*
 				 * This is a workaround for an unresolved
 				 * issue on some machines where system events
 				 * sporadically become disabled.
 				 */
-				hci_write1(dev, HCI_SYSTEM_EVENT, 1,
-					   &hci_result);
+				hci_result =
+					hci_write1(dev, HCI_SYSTEM_EVENT, 1);
 				pr_notice("Re-enabled hotkeys\n");
 				/* fall through */
 			default:
 				retries--;
 				break;
 			}
-		} while (retries && hci_result != HCI_EMPTY);
+		} while (retries && hci_result != TOS_FIFO_EMPTY);
 	}
 }
 
@@ -1848,7 +2006,7 @@ static int toshiba_acpi_suspend(struct device *device)
 	u32 result;
 
 	if (dev->hotkey_dev)
-		hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_DISABLE, &result);
+		result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_DISABLE);
 
 	return 0;
 }
@@ -1857,9 +2015,16 @@ static int toshiba_acpi_resume(struct device *device)
 {
 	struct toshiba_acpi_dev *dev = acpi_driver_data(to_acpi_device(device));
 	u32 result;
+	acpi_status status;
 
-	if (dev->hotkey_dev)
-		hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE, &result);
+	if (dev->hotkey_dev) {
+		status = acpi_evaluate_object(dev->acpi_dev->handle, "ENAB",
+				NULL, NULL);
+		if (ACPI_FAILURE(status))
+			pr_info("Unable to re-enable hotkeys\n");
+
+		result = hci_write1(dev, HCI_HOTKEY_EVENT, HCI_HOTKEY_ENABLE);
+	}
 
 	return 0;
 }
diff --git a/drivers/pnp/interface.c b/drivers/pnp/interface.c
index e6c403be09a9..4b6808ff0e5d 100644
--- a/drivers/pnp/interface.c
+++ b/drivers/pnp/interface.c
@@ -346,41 +346,41 @@ static ssize_t resources_store(struct device *dmdev,
 	}
 
 	buf = skip_spaces(buf);
-	if (!strnicmp(buf, "disable", 7)) {
+	if (!strncasecmp(buf, "disable", 7)) {
 		retval = pnp_disable_dev(dev);
 		goto done;
 	}
-	if (!strnicmp(buf, "activate", 8)) {
+	if (!strncasecmp(buf, "activate", 8)) {
 		retval = pnp_activate_dev(dev);
 		goto done;
 	}
-	if (!strnicmp(buf, "fill", 4)) {
+	if (!strncasecmp(buf, "fill", 4)) {
 		if (dev->active)
 			goto done;
 		retval = pnp_auto_config_dev(dev);
 		goto done;
 	}
-	if (!strnicmp(buf, "auto", 4)) {
+	if (!strncasecmp(buf, "auto", 4)) {
 		if (dev->active)
 			goto done;
 		pnp_init_resources(dev);
 		retval = pnp_auto_config_dev(dev);
 		goto done;
 	}
-	if (!strnicmp(buf, "clear", 5)) {
+	if (!strncasecmp(buf, "clear", 5)) {
 		if (dev->active)
 			goto done;
 		pnp_init_resources(dev);
 		goto done;
 	}
-	if (!strnicmp(buf, "get", 3)) {
+	if (!strncasecmp(buf, "get", 3)) {
 		mutex_lock(&pnp_res_mutex);
 		if (pnp_can_read(dev))
 			dev->protocol->get(dev);
 		mutex_unlock(&pnp_res_mutex);
 		goto done;
 	}
-	if (!strnicmp(buf, "set", 3)) {
+	if (!strncasecmp(buf, "set", 3)) {
 		resource_size_t start;
 		resource_size_t end;
 		unsigned long flags;
@@ -392,31 +392,31 @@ static ssize_t resources_store(struct device *dmdev,
 		mutex_lock(&pnp_res_mutex);
 		while (1) {
 			buf = skip_spaces(buf);
-			if (!strnicmp(buf, "io", 2)) {
+			if (!strncasecmp(buf, "io", 2)) {
 				buf = pnp_get_resource_value(buf + 2,
 							     IORESOURCE_IO,
 							     &start, &end,
 							     &flags);
 				pnp_add_io_resource(dev, start, end, flags);
-			} else if (!strnicmp(buf, "mem", 3)) {
+			} else if (!strncasecmp(buf, "mem", 3)) {
 				buf = pnp_get_resource_value(buf + 3,
 							     IORESOURCE_MEM,
 							     &start, &end,
 							     &flags);
 				pnp_add_mem_resource(dev, start, end, flags);
-			} else if (!strnicmp(buf, "irq", 3)) {
+			} else if (!strncasecmp(buf, "irq", 3)) {
 				buf = pnp_get_resource_value(buf + 3,
 							     IORESOURCE_IRQ,
 							     &start, NULL,
 							     &flags);
 				pnp_add_irq_resource(dev, start, flags);
-			} else if (!strnicmp(buf, "dma", 3)) {
+			} else if (!strncasecmp(buf, "dma", 3)) {
 				buf = pnp_get_resource_value(buf + 3,
 							     IORESOURCE_DMA,
 							     &start, NULL,
 							     &flags);
 				pnp_add_dma_resource(dev, start, flags);
-			} else if (!strnicmp(buf, "bus", 3)) {
+			} else if (!strncasecmp(buf, "bus", 3)) {
 				buf = pnp_get_resource_value(buf + 3,
 							     IORESOURCE_BUS,
 							     &start, &end,
diff --git a/drivers/power/Kconfig b/drivers/power/Kconfig
index 73cfcdf28a36..0108c2af005b 100644
--- a/drivers/power/Kconfig
+++ b/drivers/power/Kconfig
@@ -325,11 +325,12 @@ config CHARGER_MANAGER
           with help of suspend_again support.
 
 config CHARGER_MAX14577
-	tristate "Maxim MAX14577 MUIC battery charger driver"
+	tristate "Maxim MAX14577/77836 battery charger driver"
 	depends on MFD_MAX14577
+	depends on SYSFS
 	help
 	  Say Y to enable support for the battery charger control sysfs and
-	  platform data of MAX14577 MUICs.
+	  platform data of MAX14577/77836 MUICs.
 
 config CHARGER_MAX8997
 	tristate "Maxim MAX8997/MAX8966 PMIC battery charger driver"
diff --git a/drivers/power/ab8500_fg.c b/drivers/power/ab8500_fg.c
index 3cb4178e397c..217da4b2ca86 100644
--- a/drivers/power/ab8500_fg.c
+++ b/drivers/power/ab8500_fg.c
@@ -2969,7 +2969,7 @@ static struct device_attribute ab8505_fg_sysfs_psy_attrs[] = {
 
 static int ab8500_fg_sysfs_psy_create_attrs(struct device *dev)
 {
-	unsigned int i, j;
+	unsigned int i;
 	struct power_supply *psy = dev_get_drvdata(dev);
 	struct ab8500_fg *di;
 
@@ -2978,14 +2978,15 @@ static int ab8500_fg_sysfs_psy_create_attrs(struct device *dev)
 	if (((is_ab8505(di->parent) || is_ab9540(di->parent)) &&
 	     abx500_get_chip_id(dev->parent) >= AB8500_CUT2P0)
 	    || is_ab8540(di->parent)) {
-		for (j = 0; j < ARRAY_SIZE(ab8505_fg_sysfs_psy_attrs); j++)
-			if (device_create_file(dev, &ab8505_fg_sysfs_psy_attrs[j]))
+		for (i = 0; i < ARRAY_SIZE(ab8505_fg_sysfs_psy_attrs); i++)
+			if (device_create_file(dev,
+					       &ab8505_fg_sysfs_psy_attrs[i]))
 				goto sysfs_psy_create_attrs_failed_ab8505;
 	}
 	return 0;
 sysfs_psy_create_attrs_failed_ab8505:
 	dev_err(dev, "Failed creating sysfs psy attrs for ab8505.\n");
-	while (j--)
+	while (i--)
 		device_remove_file(dev, &ab8505_fg_sysfs_psy_attrs[i]);
 
 	return -EIO;
diff --git a/drivers/power/avs/Kconfig b/drivers/power/avs/Kconfig
index 2a1008b61121..7f3d389bd601 100644
--- a/drivers/power/avs/Kconfig
+++ b/drivers/power/avs/Kconfig
@@ -10,3 +10,11 @@ menuconfig POWER_AVS
 	  AVS is also called SmartReflex on OMAP devices.
 
 	  Say Y here to enable Adaptive Voltage Scaling class support.
+
+config ROCKCHIP_IODOMAIN
+        tristate "Rockchip IO domain support"
+        depends on ARCH_ROCKCHIP && OF
+        help
+          Say y here to enable support io domains on Rockchip SoCs. It is
+          necessary for the io domain setting of the SoC to match the
+          voltage supplied by the regulators.
diff --git a/drivers/power/avs/Makefile b/drivers/power/avs/Makefile
index 0843386a6c19..ba4c7bc69225 100644
--- a/drivers/power/avs/Makefile
+++ b/drivers/power/avs/Makefile
@@ -1 +1,2 @@
 obj-$(CONFIG_POWER_AVS_OMAP)		+= smartreflex.o
+obj-$(CONFIG_ROCKCHIP_IODOMAIN)		+= rockchip-io-domain.o
diff --git a/drivers/power/avs/rockchip-io-domain.c b/drivers/power/avs/rockchip-io-domain.c
new file mode 100644
index 000000000000..3ae35d0590d2
--- /dev/null
+++ b/drivers/power/avs/rockchip-io-domain.c
@@ -0,0 +1,351 @@
+/*
+ * Rockchip IO Voltage Domain driver
+ *
+ * Copyright 2014 MundoReader S.L.
+ * Copyright 2014 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/consumer.h>
+
+#define MAX_SUPPLIES		16
+
+/*
+ * The max voltage for 1.8V and 3.3V come from the Rockchip datasheet under
+ * "Recommended Operating Conditions" for "Digital GPIO".   When the typical
+ * is 3.3V the max is 3.6V.  When the typical is 1.8V the max is 1.98V.
+ *
+ * They are used like this:
+ * - If the voltage on a rail is above the "1.8" voltage (1.98V) we'll tell the
+ *   SoC we're at 3.3.
+ * - If the voltage on a rail is above the "3.3" voltage (3.6V) we'll consider
+ *   that to be an error.
+ */
+#define MAX_VOLTAGE_1_8		1980000
+#define MAX_VOLTAGE_3_3		3600000
+
+#define RK3288_SOC_CON2			0x24c
+#define RK3288_SOC_CON2_FLASH0		BIT(7)
+#define RK3288_SOC_FLASH_SUPPLY_NUM	2
+
+struct rockchip_iodomain;
+
+/**
+ * @supplies: voltage settings matching the register bits.
+ */
+struct rockchip_iodomain_soc_data {
+	int grf_offset;
+	const char *supply_names[MAX_SUPPLIES];
+	void (*init)(struct rockchip_iodomain *iod);
+};
+
+struct rockchip_iodomain_supply {
+	struct rockchip_iodomain *iod;
+	struct regulator *reg;
+	struct notifier_block nb;
+	int idx;
+};
+
+struct rockchip_iodomain {
+	struct device *dev;
+	struct regmap *grf;
+	struct rockchip_iodomain_soc_data *soc_data;
+	struct rockchip_iodomain_supply supplies[MAX_SUPPLIES];
+};
+
+static int rockchip_iodomain_write(struct rockchip_iodomain_supply *supply,
+				   int uV)
+{
+	struct rockchip_iodomain *iod = supply->iod;
+	u32 val;
+	int ret;
+
+	/* set value bit */
+	val = (uV > MAX_VOLTAGE_1_8) ? 0 : 1;
+	val <<= supply->idx;
+
+	/* apply hiword-mask */
+	val |= (BIT(supply->idx) << 16);
+
+	ret = regmap_write(iod->grf, iod->soc_data->grf_offset, val);
+	if (ret)
+		dev_err(iod->dev, "Couldn't write to GRF\n");
+
+	return ret;
+}
+
+static int rockchip_iodomain_notify(struct notifier_block *nb,
+				    unsigned long event,
+				    void *data)
+{
+	struct rockchip_iodomain_supply *supply =
+			container_of(nb, struct rockchip_iodomain_supply, nb);
+	int uV;
+	int ret;
+
+	/*
+	 * According to Rockchip it's important to keep the SoC IO domain
+	 * higher than (or equal to) the external voltage.  That means we need
+	 * to change it before external voltage changes happen in the case
+	 * of an increase.
+	 *
+	 * Note that in the "pre" change we pick the max possible voltage that
+	 * the regulator might end up at (the client requests a range and we
+	 * don't know for certain the exact voltage).  Right now we rely on the
+	 * slop in MAX_VOLTAGE_1_8 and MAX_VOLTAGE_3_3 to save us if clients
+	 * request something like a max of 3.6V when they really want 3.3V.
+	 * We could attempt to come up with better rules if this fails.
+	 */
+	if (event & REGULATOR_EVENT_PRE_VOLTAGE_CHANGE) {
+		struct pre_voltage_change_data *pvc_data = data;
+
+		uV = max_t(unsigned long, pvc_data->old_uV, pvc_data->max_uV);
+	} else if (event & (REGULATOR_EVENT_VOLTAGE_CHANGE |
+			    REGULATOR_EVENT_ABORT_VOLTAGE_CHANGE)) {
+		uV = (unsigned long)data;
+	} else {
+		return NOTIFY_OK;
+	}
+
+	dev_dbg(supply->iod->dev, "Setting to %d\n", uV);
+
+	if (uV > MAX_VOLTAGE_3_3) {
+		dev_err(supply->iod->dev, "Voltage too high: %d\n", uV);
+
+		if (event == REGULATOR_EVENT_PRE_VOLTAGE_CHANGE)
+			return NOTIFY_BAD;
+	}
+
+	ret = rockchip_iodomain_write(supply, uV);
+	if (ret && event == REGULATOR_EVENT_PRE_VOLTAGE_CHANGE)
+		return NOTIFY_BAD;
+
+	dev_info(supply->iod->dev, "Setting to %d done\n", uV);
+	return NOTIFY_OK;
+}
+
+static void rk3288_iodomain_init(struct rockchip_iodomain *iod)
+{
+	int ret;
+	u32 val;
+
+	/* if no flash supply we should leave things alone */
+	if (!iod->supplies[RK3288_SOC_FLASH_SUPPLY_NUM].reg)
+		return;
+
+	/*
+	 * set flash0 iodomain to also use this framework
+	 * instead of a special gpio.
+	 */
+	val = RK3288_SOC_CON2_FLASH0 | (RK3288_SOC_CON2_FLASH0 << 16);
+	ret = regmap_write(iod->grf, RK3288_SOC_CON2, val);
+	if (ret < 0)
+		dev_warn(iod->dev, "couldn't update flash0 ctrl\n");
+}
+
+/*
+ * On the rk3188 the io-domains are handled by a shared register with the
+ * lower 8 bits being still being continuing drive-strength settings.
+ */
+static const struct rockchip_iodomain_soc_data soc_data_rk3188 = {
+	.grf_offset = 0x104,
+	.supply_names = {
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+		NULL,
+		"ap0",
+		"ap1",
+		"cif",
+		"flash",
+		"vccio0",
+		"vccio1",
+		"lcdc0",
+		"lcdc1",
+	},
+};
+
+static const struct rockchip_iodomain_soc_data soc_data_rk3288 = {
+	.grf_offset = 0x380,
+	.supply_names = {
+		"lcdc",		/* LCDC_VDD */
+		"dvp",		/* DVPIO_VDD */
+		"flash0",	/* FLASH0_VDD (emmc) */
+		"flash1",	/* FLASH1_VDD (sdio1) */
+		"wifi",		/* APIO3_VDD  (sdio0) */
+		"bb",		/* APIO5_VDD */
+		"audio",	/* APIO4_VDD */
+		"sdcard",	/* SDMMC0_VDD (sdmmc) */
+		"gpio30",	/* APIO1_VDD */
+		"gpio1830",	/* APIO2_VDD */
+	},
+	.init = rk3288_iodomain_init,
+};
+
+static const struct of_device_id rockchip_iodomain_match[] = {
+	{
+		.compatible = "rockchip,rk3188-io-voltage-domain",
+		.data = (void *)&soc_data_rk3188
+	},
+	{
+		.compatible = "rockchip,rk3288-io-voltage-domain",
+		.data = (void *)&soc_data_rk3288
+	},
+	{ /* sentinel */ },
+};
+
+static int rockchip_iodomain_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match;
+	struct rockchip_iodomain *iod;
+	int i, ret = 0;
+
+	if (!np)
+		return -ENODEV;
+
+	iod = devm_kzalloc(&pdev->dev, sizeof(*iod), GFP_KERNEL);
+	if (!iod)
+		return -ENOMEM;
+
+	iod->dev = &pdev->dev;
+	platform_set_drvdata(pdev, iod);
+
+	match = of_match_node(rockchip_iodomain_match, np);
+	iod->soc_data = (struct rockchip_iodomain_soc_data *)match->data;
+
+	iod->grf = syscon_regmap_lookup_by_phandle(np, "rockchip,grf");
+	if (IS_ERR(iod->grf)) {
+		dev_err(&pdev->dev, "couldn't find grf regmap\n");
+		return PTR_ERR(iod->grf);
+	}
+
+	for (i = 0; i < MAX_SUPPLIES; i++) {
+		const char *supply_name = iod->soc_data->supply_names[i];
+		struct rockchip_iodomain_supply *supply = &iod->supplies[i];
+		struct regulator *reg;
+		int uV;
+
+		if (!supply_name)
+			continue;
+
+		reg = devm_regulator_get_optional(iod->dev, supply_name);
+		if (IS_ERR(reg)) {
+			ret = PTR_ERR(reg);
+
+			/* If a supply wasn't specified, that's OK */
+			if (ret == -ENODEV)
+				continue;
+			else if (ret != -EPROBE_DEFER)
+				dev_err(iod->dev, "couldn't get regulator %s\n",
+					supply_name);
+			goto unreg_notify;
+		}
+
+		/* set initial correct value */
+		uV = regulator_get_voltage(reg);
+
+		/* must be a regulator we can get the voltage of */
+		if (uV < 0) {
+			dev_err(iod->dev, "Can't determine voltage: %s\n",
+				supply_name);
+			goto unreg_notify;
+		}
+
+		if (uV > MAX_VOLTAGE_3_3) {
+			dev_crit(iod->dev,
+				 "%d uV is too high. May damage SoC!\n",
+				 uV);
+			ret = -EINVAL;
+			goto unreg_notify;
+		}
+
+		/* setup our supply */
+		supply->idx = i;
+		supply->iod = iod;
+		supply->reg = reg;
+		supply->nb.notifier_call = rockchip_iodomain_notify;
+
+		ret = rockchip_iodomain_write(supply, uV);
+		if (ret) {
+			supply->reg = NULL;
+			goto unreg_notify;
+		}
+
+		/* register regulator notifier */
+		ret = regulator_register_notifier(reg, &supply->nb);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"regulator notifier request failed\n");
+			supply->reg = NULL;
+			goto unreg_notify;
+		}
+	}
+
+	if (iod->soc_data->init)
+		iod->soc_data->init(iod);
+
+	return 0;
+
+unreg_notify:
+	for (i = MAX_SUPPLIES - 1; i >= 0; i--) {
+		struct rockchip_iodomain_supply *io_supply = &iod->supplies[i];
+
+		if (io_supply->reg)
+			regulator_unregister_notifier(io_supply->reg,
+						      &io_supply->nb);
+	}
+
+	return ret;
+}
+
+static int rockchip_iodomain_remove(struct platform_device *pdev)
+{
+	struct rockchip_iodomain *iod = platform_get_drvdata(pdev);
+	int i;
+
+	for (i = MAX_SUPPLIES - 1; i >= 0; i--) {
+		struct rockchip_iodomain_supply *io_supply = &iod->supplies[i];
+
+		if (io_supply->reg)
+			regulator_unregister_notifier(io_supply->reg,
+						      &io_supply->nb);
+	}
+
+	return 0;
+}
+
+static struct platform_driver rockchip_iodomain_driver = {
+	.probe   = rockchip_iodomain_probe,
+	.remove  = rockchip_iodomain_remove,
+	.driver  = {
+		.name  = "rockchip-iodomain",
+		.of_match_table = rockchip_iodomain_match,
+	},
+};
+
+module_platform_driver(rockchip_iodomain_driver);
+
+MODULE_DESCRIPTION("Rockchip IO-domain driver");
+MODULE_AUTHOR("Heiko Stuebner <heiko@sntech.de>");
+MODULE_AUTHOR("Doug Anderson <dianders@chromium.org>");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/power/bq27x00_battery.c b/drivers/power/bq27x00_battery.c
index e10763e3a1d5..e3bacfe3bcd0 100644
--- a/drivers/power/bq27x00_battery.c
+++ b/drivers/power/bq27x00_battery.c
@@ -23,6 +23,7 @@
  * http://focus.ti.com/docs/prod/folders/print/bq27000.html
  * http://focus.ti.com/docs/prod/folders/print/bq27500.html
  * http://www.ti.com/product/bq27425-g1
+ * http://www.ti.com/product/BQ27742-G1
  */
 
 #include <linux/device.h>
@@ -71,6 +72,8 @@
 #define BQ27500_FLAG_FC			BIT(9)
 #define BQ27500_FLAG_OTC		BIT(15)
 
+#define BQ27742_POWER_AVG		0x76
+
 /* bq27425 register addresses are same as bq27x00 addresses minus 4 */
 #define BQ27425_REG_OFFSET		0x04
 #define BQ27425_REG_SOC			0x18 /* Register address plus offset */
@@ -83,7 +86,7 @@ struct bq27x00_access_methods {
 	int (*read)(struct bq27x00_device_info *di, u8 reg, bool single);
 };
 
-enum bq27x00_chip { BQ27000, BQ27500, BQ27425};
+enum bq27x00_chip { BQ27000, BQ27500, BQ27425, BQ27742};
 
 struct bq27x00_reg_cache {
 	int temperature;
@@ -152,6 +155,24 @@ static enum power_supply_property bq27425_battery_props[] = {
 	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
 };
 
+static enum power_supply_property bq27742_battery_props[] = {
+	POWER_SUPPLY_PROP_STATUS,
+	POWER_SUPPLY_PROP_PRESENT,
+	POWER_SUPPLY_PROP_VOLTAGE_NOW,
+	POWER_SUPPLY_PROP_CURRENT_NOW,
+	POWER_SUPPLY_PROP_CAPACITY,
+	POWER_SUPPLY_PROP_CAPACITY_LEVEL,
+	POWER_SUPPLY_PROP_TEMP,
+	POWER_SUPPLY_PROP_TIME_TO_EMPTY_NOW,
+	POWER_SUPPLY_PROP_TECHNOLOGY,
+	POWER_SUPPLY_PROP_CHARGE_FULL,
+	POWER_SUPPLY_PROP_CHARGE_NOW,
+	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	POWER_SUPPLY_PROP_CYCLE_COUNT,
+	POWER_SUPPLY_PROP_POWER_AVG,
+	POWER_SUPPLY_PROP_HEALTH,
+};
+
 static unsigned int poll_interval = 360;
 module_param(poll_interval, uint, 0644);
 MODULE_PARM_DESC(poll_interval, "battery poll interval in seconds - " \
@@ -176,7 +197,7 @@ static inline int bq27x00_read(struct bq27x00_device_info *di, u8 reg,
  */
 static bool bq27xxx_is_chip_version_higher(struct bq27x00_device_info *di)
 {
-	if (di->chip == BQ27425 || di->chip == BQ27500)
+	if (di->chip == BQ27425 || di->chip == BQ27500 || di->chip == BQ27742)
 		return true;
 	return false;
 }
@@ -189,7 +210,7 @@ static int bq27x00_battery_read_rsoc(struct bq27x00_device_info *di)
 {
 	int rsoc;
 
-	if (di->chip == BQ27500)
+	if (di->chip == BQ27500 || di->chip == BQ27742)
 		rsoc = bq27x00_read(di, BQ27500_REG_SOC, false);
 	else if (di->chip == BQ27425)
 		rsoc = bq27x00_read(di, BQ27425_REG_SOC, false);
@@ -233,9 +254,11 @@ static inline int bq27x00_battery_read_nac(struct bq27x00_device_info *di)
 {
 	int flags;
 	bool is_bq27500 = di->chip == BQ27500;
+	bool is_bq27742 = di->chip == BQ27742;
 	bool is_higher = bq27xxx_is_chip_version_higher(di);
+	bool flags_1b = !(is_bq27500 || is_bq27742);
 
-	flags = bq27x00_read(di, BQ27x00_REG_FLAGS, !is_bq27500);
+	flags = bq27x00_read(di, BQ27x00_REG_FLAGS, flags_1b);
 	if (flags >= 0 && !is_higher && (flags & BQ27000_FLAG_CI))
 		return -ENODATA;
 
@@ -414,13 +437,15 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 	struct bq27x00_reg_cache cache = {0, };
 	bool is_bq27500 = di->chip == BQ27500;
 	bool is_bq27425 = di->chip == BQ27425;
+	bool is_bq27742 = di->chip == BQ27742;
+	bool flags_1b = !(is_bq27500 || is_bq27742);
 
-	cache.flags = bq27x00_read(di, BQ27x00_REG_FLAGS, !is_bq27500);
+	cache.flags = bq27x00_read(di, BQ27x00_REG_FLAGS, flags_1b);
 	if ((cache.flags & 0xff) == 0xff)
 		/* read error */
 		cache.flags = -1;
 	if (cache.flags >= 0) {
-		if (!is_bq27500 && !is_bq27425
+		if (!is_bq27500 && !is_bq27425 && !is_bq27742
 				&& (cache.flags & BQ27000_FLAG_CI)) {
 			dev_info(di->dev, "battery is not calibrated! ignoring capacity values\n");
 			cache.capacity = -ENODATA;
@@ -432,7 +457,11 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 			cache.health = -ENODATA;
 		} else {
 			cache.capacity = bq27x00_battery_read_rsoc(di);
-			if (!is_bq27425) {
+			if (is_bq27742)
+				cache.time_to_empty =
+					bq27x00_battery_read_time(di,
+							BQ27x00_REG_TTE);
+			else if (!is_bq27425) {
 				cache.energy = bq27x00_battery_read_energy(di);
 				cache.time_to_empty =
 					bq27x00_battery_read_time(di,
@@ -450,8 +479,14 @@ static void bq27x00_update(struct bq27x00_device_info *di)
 		cache.temperature = bq27x00_battery_read_temperature(di);
 		if (!is_bq27425)
 			cache.cycle_count = bq27x00_battery_read_cyct(di);
-		cache.power_avg =
-			bq27x00_battery_read_pwr_avg(di, BQ27x00_POWER_AVG);
+		if (is_bq27742)
+			cache.power_avg =
+				bq27x00_battery_read_pwr_avg(di,
+						BQ27742_POWER_AVG);
+		else
+			cache.power_avg =
+				bq27x00_battery_read_pwr_avg(di,
+						BQ27x00_POWER_AVG);
 
 		/* We only have to read charge design full once */
 		if (di->charge_design_full <= 0)
@@ -702,6 +737,9 @@ static int bq27x00_powersupply_init(struct bq27x00_device_info *di)
 	if (di->chip == BQ27425) {
 		di->bat.properties = bq27425_battery_props;
 		di->bat.num_properties = ARRAY_SIZE(bq27425_battery_props);
+	} else if (di->chip == BQ27742) {
+		di->bat.properties = bq27742_battery_props;
+		di->bat.num_properties = ARRAY_SIZE(bq27742_battery_props);
 	} else {
 		di->bat.properties = bq27x00_battery_props;
 		di->bat.num_properties = ARRAY_SIZE(bq27x00_battery_props);
@@ -858,6 +896,7 @@ static const struct i2c_device_id bq27x00_id[] = {
 	{ "bq27200", BQ27000 },	/* bq27200 is same as bq27000, but with i2c */
 	{ "bq27500", BQ27500 },
 	{ "bq27425", BQ27425 },
+	{ "bq27742", BQ27742 },
 	{},
 };
 MODULE_DEVICE_TABLE(i2c, bq27x00_id);
diff --git a/drivers/power/charger-manager.c b/drivers/power/charger-manager.c
index 9e4dab46eefd..7098a1ce2d3c 100644
--- a/drivers/power/charger-manager.c
+++ b/drivers/power/charger-manager.c
@@ -1656,7 +1656,7 @@ static inline struct charger_desc *cm_get_drv_data(struct platform_device *pdev)
 {
 	if (pdev->dev.of_node)
 		return of_cm_parse_desc(&pdev->dev);
-	return (struct charger_desc *)dev_get_platdata(&pdev->dev);
+	return dev_get_platdata(&pdev->dev);
 }
 
 static int charger_manager_probe(struct platform_device *pdev)
@@ -1677,7 +1677,7 @@ static int charger_manager_probe(struct platform_device *pdev)
 		}
 	}
 
-	if (!desc) {
+	if (IS_ERR(desc)) {
 		dev_err(&pdev->dev, "No platform data (desc) found\n");
 		return -ENODEV;
 	}
@@ -1720,6 +1720,11 @@ static int charger_manager_probe(struct platform_device *pdev)
 		return -EINVAL;
 	}
 
+	if (!desc->psy_fuel_gauge) {
+		dev_err(&pdev->dev, "No fuel gauge power supply defined\n");
+		return -EINVAL;
+	}
+
 	/* Counting index only */
 	while (desc->psy_charger_stat[i])
 		i++;
@@ -1839,6 +1844,13 @@ static int charger_manager_probe(struct platform_device *pdev)
 	device_init_wakeup(&pdev->dev, true);
 	device_set_wakeup_capable(&pdev->dev, false);
 
+	/*
+	 * Charger-manager have to check the charging state right after
+	 * tialization of charger-manager and then update current charging
+	 * state.
+	 */
+	cm_monitor();
+
 	schedule_work(&setup_polling);
 
 	return 0;
diff --git a/drivers/power/gpio-charger.c b/drivers/power/gpio-charger.c
index a0024b252197..7536933d0ab9 100644
--- a/drivers/power/gpio-charger.c
+++ b/drivers/power/gpio-charger.c
@@ -55,7 +55,7 @@ static int gpio_charger_get_property(struct power_supply *psy,
 
 	switch (psp) {
 	case POWER_SUPPLY_PROP_ONLINE:
-		val->intval = gpio_get_value_cansleep(pdata->gpio);
+		val->intval = !!gpio_get_value_cansleep(pdata->gpio);
 		val->intval ^= pdata->gpio_active_low;
 		break;
 	default:
diff --git a/drivers/power/max14577_charger.c b/drivers/power/max14577_charger.c
index fad2a75b3604..0a2bc7277026 100644
--- a/drivers/power/max14577_charger.c
+++ b/drivers/power/max14577_charger.c
@@ -1,7 +1,7 @@
 /*
- * Battery charger driver for the Maxim 14577
+ * max14577_charger.c - Battery charger driver for the Maxim 14577/77836
  *
- * Copyright (C) 2013 Samsung Electronics
+ * Copyright (C) 2013,2014 Samsung Electronics
  * Krzysztof Kozlowski <k.kozlowski@samsung.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -19,16 +19,44 @@
 #include <linux/platform_device.h>
 #include <linux/power_supply.h>
 #include <linux/mfd/max14577-private.h>
+#include <linux/mfd/max14577.h>
 
 struct max14577_charger {
 	struct device *dev;
 	struct max14577	*max14577;
 	struct power_supply	charger;
 
-	unsigned int	charging_state;
-	unsigned int	battery_state;
+	unsigned int		charging_state;
+	unsigned int		battery_state;
+
+	struct max14577_charger_platform_data	*pdata;
 };
 
+/*
+ * Helper function for mapping values of STATUS2/CHGTYP register on max14577
+ * and max77836 chipsets to enum maxim_muic_charger_type.
+ */
+static enum max14577_muic_charger_type maxim_get_charger_type(
+		enum maxim_device_type dev_type, u8 val) {
+	switch (val) {
+	case MAX14577_CHARGER_TYPE_NONE:
+	case MAX14577_CHARGER_TYPE_USB:
+	case MAX14577_CHARGER_TYPE_DOWNSTREAM_PORT:
+	case MAX14577_CHARGER_TYPE_DEDICATED_CHG:
+	case MAX14577_CHARGER_TYPE_SPECIAL_500MA:
+	case MAX14577_CHARGER_TYPE_SPECIAL_1A:
+		return val;
+	case MAX14577_CHARGER_TYPE_DEAD_BATTERY:
+	case MAX14577_CHARGER_TYPE_RESERVED:
+		if (dev_type == MAXIM_DEVICE_TYPE_MAX77836)
+			val |= 0x8;
+		return val;
+	default:
+		WARN_ONCE(1, "max14577: Unsupported chgtyp register value 0x%02x", val);
+		return val;
+	}
+}
+
 static int max14577_get_charger_state(struct max14577_charger *chg)
 {
 	struct regmap *rmap = chg->max14577->regmap;
@@ -89,19 +117,23 @@ static int max14577_get_online(struct max14577_charger *chg)
 {
 	struct regmap *rmap = chg->max14577->regmap;
 	u8 reg_data;
+	enum max14577_muic_charger_type chg_type;
 
 	max14577_read_reg(rmap, MAX14577_MUIC_REG_STATUS2, &reg_data);
 	reg_data = ((reg_data & STATUS2_CHGTYP_MASK) >> STATUS2_CHGTYP_SHIFT);
-	switch (reg_data) {
+	chg_type = maxim_get_charger_type(chg->max14577->dev_type, reg_data);
+	switch (chg_type) {
 	case MAX14577_CHARGER_TYPE_USB:
 	case MAX14577_CHARGER_TYPE_DEDICATED_CHG:
 	case MAX14577_CHARGER_TYPE_SPECIAL_500MA:
 	case MAX14577_CHARGER_TYPE_SPECIAL_1A:
 	case MAX14577_CHARGER_TYPE_DEAD_BATTERY:
+	case MAX77836_CHARGER_TYPE_SPECIAL_BIAS:
 		return 1;
 	case MAX14577_CHARGER_TYPE_NONE:
 	case MAX14577_CHARGER_TYPE_DOWNSTREAM_PORT:
 	case MAX14577_CHARGER_TYPE_RESERVED:
+	case MAX77836_CHARGER_TYPE_RESERVED:
 	default:
 		return 0;
 	}
@@ -118,10 +150,12 @@ static int max14577_get_battery_health(struct max14577_charger *chg)
 	struct regmap *rmap = chg->max14577->regmap;
 	int state = POWER_SUPPLY_HEALTH_GOOD;
 	u8 reg_data;
+	enum max14577_muic_charger_type chg_type;
 
 	max14577_read_reg(rmap, MAX14577_MUIC_REG_STATUS2, &reg_data);
 	reg_data = ((reg_data & STATUS2_CHGTYP_MASK) >> STATUS2_CHGTYP_SHIFT);
-	if (reg_data == MAX14577_CHARGER_TYPE_DEAD_BATTERY) {
+	chg_type = maxim_get_charger_type(chg->max14577->dev_type, reg_data);
+	if (chg_type == MAX14577_CHARGER_TYPE_DEAD_BATTERY) {
 		state = POWER_SUPPLY_HEALTH_DEAD;
 		goto state_set;
 	}
@@ -147,15 +181,131 @@ static int max14577_get_present(struct max14577_charger *chg)
 	return 1;
 }
 
+static int max14577_set_fast_charge_timer(struct max14577_charger *chg,
+		unsigned long hours)
+{
+	u8 reg_data;
+
+	switch (hours) {
+	case 5 ... 7:
+		reg_data = hours - 3;
+		break;
+	case 0:
+		/* Disable */
+		reg_data = 0x7;
+		break;
+	default:
+		dev_err(chg->dev, "Wrong value for Fast-Charge Timer: %lu\n",
+				hours);
+		return -EINVAL;
+	}
+	reg_data <<= CHGCTRL1_TCHW_SHIFT;
+
+	return max14577_update_reg(chg->max14577->regmap,
+			MAX14577_REG_CHGCTRL1, CHGCTRL1_TCHW_MASK, reg_data);
+}
+
+static int max14577_init_constant_voltage(struct max14577_charger *chg,
+		unsigned int uvolt)
+{
+	u8 reg_data;
+
+	if (uvolt < MAXIM_CHARGER_CONSTANT_VOLTAGE_MIN ||
+			uvolt > MAXIM_CHARGER_CONSTANT_VOLTAGE_MAX)
+		return -EINVAL;
+
+	if (uvolt == 4200000)
+		reg_data = 0x0;
+	else if (uvolt == MAXIM_CHARGER_CONSTANT_VOLTAGE_MAX)
+		reg_data = 0x1f;
+	else if (uvolt <= 4280000) {
+		unsigned int val = uvolt;
+
+		val -= MAXIM_CHARGER_CONSTANT_VOLTAGE_MIN;
+		val /= MAXIM_CHARGER_CONSTANT_VOLTAGE_STEP;
+		if (uvolt <= 4180000)
+			reg_data = 0x1 + val;
+		else
+			reg_data = val; /* Fix for gap between 4.18V and 4.22V */
+	} else
+		return -EINVAL;
+
+	reg_data <<= CHGCTRL3_MBCCVWRC_SHIFT;
+
+	return max14577_write_reg(chg->max14577->regmap,
+			MAX14577_CHG_REG_CHG_CTRL3, reg_data);
+}
+
+static int max14577_init_eoc(struct max14577_charger *chg,
+		unsigned int uamp)
+{
+	unsigned int current_bits = 0xf;
+	u8 reg_data;
+
+	switch (chg->max14577->dev_type) {
+	case MAXIM_DEVICE_TYPE_MAX77836:
+		if (uamp < 5000)
+			return -EINVAL; /* Requested current is too low */
+
+		if (uamp >= 7500 && uamp < 10000)
+			current_bits = 0x0;
+		else if (uamp <= 50000) {
+			/* <5000, 7499> and <10000, 50000> */
+			current_bits = uamp / 5000;
+		} else {
+			uamp = min(uamp, 100000U) - 50000U;
+			current_bits = 0xa + uamp / 10000;
+		}
+		break;
+
+	case MAXIM_DEVICE_TYPE_MAX14577:
+	default:
+		if (uamp < MAX14577_CHARGER_EOC_CURRENT_LIMIT_MIN)
+			return -EINVAL; /* Requested current is too low */
+
+		uamp = min(uamp, MAX14577_CHARGER_EOC_CURRENT_LIMIT_MAX);
+		uamp -= MAX14577_CHARGER_EOC_CURRENT_LIMIT_MIN;
+		current_bits = uamp / MAX14577_CHARGER_EOC_CURRENT_LIMIT_STEP;
+		break;
+	}
+
+	reg_data = current_bits << CHGCTRL5_EOCS_SHIFT;
+
+	return max14577_update_reg(chg->max14577->regmap,
+			MAX14577_CHG_REG_CHG_CTRL5, CHGCTRL5_EOCS_MASK,
+			reg_data);
+}
+
+static int max14577_init_fast_charge(struct max14577_charger *chg,
+		unsigned int uamp)
+{
+	u8 reg_data;
+	int ret;
+	const struct maxim_charger_current *limits =
+		&maxim_charger_currents[chg->max14577->dev_type];
+
+	ret = maxim_charger_calc_reg_current(limits, uamp, uamp, &reg_data);
+	if (ret) {
+		dev_err(chg->dev, "Wrong value for fast charge: %u\n", uamp);
+		return ret;
+	}
+
+	return max14577_update_reg(chg->max14577->regmap,
+			MAX14577_CHG_REG_CHG_CTRL4,
+			CHGCTRL4_MBCICHWRCL_MASK | CHGCTRL4_MBCICHWRCH_MASK,
+			reg_data);
+}
+
 /*
  * Sets charger registers to proper and safe default values.
  * Some of these values are equal to defaults in MAX14577E
  * data sheet but there are minor differences.
  */
-static void max14577_charger_reg_init(struct max14577_charger *chg)
+static int max14577_charger_reg_init(struct max14577_charger *chg)
 {
 	struct regmap *rmap = chg->max14577->regmap;
 	u8 reg_data;
+	int ret;
 
 	/*
 	 * Charger-Type Manual Detection, default off (set CHGTYPMAN to 0)
@@ -167,10 +317,6 @@ static void max14577_charger_reg_init(struct max14577_charger *chg)
 			CDETCTRL1_CHGDETEN_MASK | CDETCTRL1_CHGTYPMAN_MASK,
 			reg_data);
 
-	/* Battery Fast-Charge Timer, from SM-V700: 6hrs */
-	reg_data = 0x3 << CHGCTRL1_TCHW_SHIFT;
-	max14577_write_reg(rmap, MAX14577_REG_CHGCTRL1, reg_data);
-
 	/*
 	 * Wall-Adapter Rapid Charge, default on
 	 * Battery-Charger, default on
@@ -179,29 +325,46 @@ static void max14577_charger_reg_init(struct max14577_charger *chg)
 	reg_data |= 0x1 << CHGCTRL2_MBCHOSTEN_SHIFT;
 	max14577_write_reg(rmap, MAX14577_REG_CHGCTRL2, reg_data);
 
-	/* Battery-Charger Constant Voltage (CV) Mode, from SM-V700: 4.35V */
-	reg_data = 0xf << CHGCTRL3_MBCCVWRC_SHIFT;
-	max14577_write_reg(rmap, MAX14577_REG_CHGCTRL3, reg_data);
-
-	/*
-	 * Fast Battery-Charge Current Low, default 200-950mA
-	 * Fast Battery-Charge Current High, from SM-V700: 450mA
-	 */
-	reg_data = 0x1 << CHGCTRL4_MBCICHWRCL_SHIFT;
-	reg_data |= 0x5 << CHGCTRL4_MBCICHWRCH_SHIFT;
-	max14577_write_reg(rmap, MAX14577_REG_CHGCTRL4, reg_data);
-
-	/* End-of-Charge Current, from SM-V700: 50mA */
-	reg_data = 0x0 << CHGCTRL5_EOCS_SHIFT;
-	max14577_write_reg(rmap, MAX14577_REG_CHGCTRL5, reg_data);
-
 	/* Auto Charging Stop, default off */
 	reg_data = 0x0 << CHGCTRL6_AUTOSTOP_SHIFT;
 	max14577_write_reg(rmap, MAX14577_REG_CHGCTRL6, reg_data);
 
-	/* Overvoltage-Protection Threshold, from SM-V700: 6.5V */
-	reg_data = 0x2 << CHGCTRL7_OTPCGHCVS_SHIFT;
+	ret = max14577_init_constant_voltage(chg, chg->pdata->constant_uvolt);
+	if (ret)
+		return ret;
+
+	ret = max14577_init_eoc(chg, chg->pdata->eoc_uamp);
+	if (ret)
+		return ret;
+
+	ret = max14577_init_fast_charge(chg, chg->pdata->fast_charge_uamp);
+	if (ret)
+		return ret;
+
+	ret = max14577_set_fast_charge_timer(chg,
+			MAXIM_CHARGER_FAST_CHARGE_TIMER_DEFAULT);
+	if (ret)
+		return ret;
+
+	/* Initialize Overvoltage-Protection Threshold */
+	switch (chg->pdata->ovp_uvolt) {
+	case 7500000:
+		reg_data = 0x0;
+		break;
+	case 6000000:
+	case 6500000:
+	case 7000000:
+		reg_data = 0x1 + (chg->pdata->ovp_uvolt - 6000000) / 500000;
+		break;
+	default:
+		dev_err(chg->dev, "Wrong value for OVP: %u\n",
+				chg->pdata->ovp_uvolt);
+		return -EINVAL;
+	}
+	reg_data <<= CHGCTRL7_OTPCGHCVS_SHIFT;
 	max14577_write_reg(rmap, MAX14577_REG_CHGCTRL7, reg_data);
+
+	return 0;
 }
 
 /* Support property from charger */
@@ -215,7 +378,11 @@ static enum power_supply_property max14577_charger_props[] = {
 	POWER_SUPPLY_PROP_MANUFACTURER,
 };
 
-static const char *model_name = "MAX14577";
+static const char * const model_names[] = {
+	[MAXIM_DEVICE_TYPE_UNKNOWN]	= "MAX14577-like",
+	[MAXIM_DEVICE_TYPE_MAX14577]	= "MAX14577",
+	[MAXIM_DEVICE_TYPE_MAX77836]	= "MAX77836",
+};
 static const char *manufacturer = "Maxim Integrated";
 
 static int max14577_charger_get_property(struct power_supply *psy,
@@ -244,7 +411,8 @@ static int max14577_charger_get_property(struct power_supply *psy,
 		val->intval = max14577_get_online(chg);
 		break;
 	case POWER_SUPPLY_PROP_MODEL_NAME:
-		val->strval = model_name;
+		BUILD_BUG_ON(ARRAY_SIZE(model_names) != MAXIM_DEVICE_TYPE_NUM);
+		val->strval = model_names[chg->max14577->dev_type];
 		break;
 	case POWER_SUPPLY_PROP_MANUFACTURER:
 		val->strval = manufacturer;
@@ -256,6 +424,110 @@ static int max14577_charger_get_property(struct power_supply *psy,
 	return ret;
 }
 
+#ifdef CONFIG_OF
+static struct max14577_charger_platform_data *max14577_charger_dt_init(
+		struct platform_device *pdev)
+{
+	struct max14577_charger_platform_data *pdata;
+	struct device_node *np = pdev->dev.of_node;
+	int ret;
+
+	if (!np) {
+		dev_err(&pdev->dev, "No charger OF node\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	ret = of_property_read_u32(np, "maxim,constant-uvolt",
+			&pdata->constant_uvolt);
+	if (ret) {
+		dev_err(&pdev->dev, "Cannot parse maxim,constant-uvolt field from DT\n");
+		return ERR_PTR(ret);
+	}
+
+	ret = of_property_read_u32(np, "maxim,fast-charge-uamp",
+			&pdata->fast_charge_uamp);
+	if (ret) {
+		dev_err(&pdev->dev, "Cannot parse maxim,fast-charge-uamp field from DT\n");
+		return ERR_PTR(ret);
+	}
+
+	ret = of_property_read_u32(np, "maxim,eoc-uamp", &pdata->eoc_uamp);
+	if (ret) {
+		dev_err(&pdev->dev, "Cannot parse maxim,eoc-uamp field from DT\n");
+		return ERR_PTR(ret);
+	}
+
+	ret = of_property_read_u32(np, "maxim,ovp-uvolt", &pdata->ovp_uvolt);
+	if (ret) {
+		dev_err(&pdev->dev, "Cannot parse maxim,ovp-uvolt field from DT\n");
+		return ERR_PTR(ret);
+	}
+
+	return pdata;
+}
+#else /* CONFIG_OF */
+static struct max14577_charger_platform_data *max14577_charger_dt_init(
+		struct platform_device *pdev)
+{
+	return NULL;
+}
+#endif /* CONFIG_OF */
+
+static ssize_t show_fast_charge_timer(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct max14577_charger *chg = dev_get_drvdata(dev);
+	u8 reg_data;
+	int ret;
+	unsigned int val;
+
+	ret = max14577_read_reg(chg->max14577->regmap, MAX14577_REG_CHGCTRL1,
+			&reg_data);
+	if (ret)
+		return ret;
+
+	reg_data &= CHGCTRL1_TCHW_MASK;
+	reg_data >>= CHGCTRL1_TCHW_SHIFT;
+	switch (reg_data) {
+	case 0x2 ... 0x4:
+		val = reg_data + 3;
+		break;
+	case 0x7:
+		val = 0;
+		break;
+	default:
+		val = 5;
+		break;
+	}
+
+	return scnprintf(buf, PAGE_SIZE, "%u\n", val);
+}
+
+static ssize_t store_fast_charge_timer(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct max14577_charger *chg = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	ret = max14577_set_fast_charge_timer(chg, val);
+	if (ret)
+		return ret;
+
+	return count;
+}
+
+static DEVICE_ATTR(fast_charge_timer, S_IRUGO | S_IWUSR,
+		show_fast_charge_timer, store_fast_charge_timer);
+
 static int max14577_charger_probe(struct platform_device *pdev)
 {
 	struct max14577_charger *chg;
@@ -270,7 +542,13 @@ static int max14577_charger_probe(struct platform_device *pdev)
 	chg->dev = &pdev->dev;
 	chg->max14577 = max14577;
 
-	max14577_charger_reg_init(chg);
+	chg->pdata = max14577_charger_dt_init(pdev);
+	if (IS_ERR_OR_NULL(chg->pdata))
+		return PTR_ERR(chg->pdata);
+
+	ret = max14577_charger_reg_init(chg);
+	if (ret)
+		return ret;
 
 	chg->charger.name = "max14577-charger",
 	chg->charger.type = POWER_SUPPLY_TYPE_BATTERY,
@@ -278,24 +556,47 @@ static int max14577_charger_probe(struct platform_device *pdev)
 	chg->charger.num_properties = ARRAY_SIZE(max14577_charger_props),
 	chg->charger.get_property = max14577_charger_get_property,
 
+	ret = device_create_file(&pdev->dev, &dev_attr_fast_charge_timer);
+	if (ret) {
+		dev_err(&pdev->dev, "failed: create sysfs entry\n");
+		return ret;
+	}
+
 	ret = power_supply_register(&pdev->dev, &chg->charger);
 	if (ret) {
 		dev_err(&pdev->dev, "failed: power supply register\n");
-		return ret;
+		goto err;
 	}
 
+	/* Check for valid values for charger */
+	BUILD_BUG_ON(MAX14577_CHARGER_EOC_CURRENT_LIMIT_MIN +
+			MAX14577_CHARGER_EOC_CURRENT_LIMIT_STEP * 0xf !=
+			MAX14577_CHARGER_EOC_CURRENT_LIMIT_MAX);
 	return 0;
+
+err:
+	device_remove_file(&pdev->dev, &dev_attr_fast_charge_timer);
+
+	return ret;
 }
 
 static int max14577_charger_remove(struct platform_device *pdev)
 {
 	struct max14577_charger *chg = platform_get_drvdata(pdev);
 
+	device_remove_file(&pdev->dev, &dev_attr_fast_charge_timer);
 	power_supply_unregister(&chg->charger);
 
 	return 0;
 }
 
+static const struct platform_device_id max14577_charger_id[] = {
+	{ "max14577-charger", MAXIM_DEVICE_TYPE_MAX14577, },
+	{ "max77836-charger", MAXIM_DEVICE_TYPE_MAX77836, },
+	{ }
+};
+MODULE_DEVICE_TABLE(platform, max14577_charger_id);
+
 static struct platform_driver max14577_charger_driver = {
 	.driver = {
 		.owner	= THIS_MODULE,
@@ -303,9 +604,10 @@ static struct platform_driver max14577_charger_driver = {
 	},
 	.probe		= max14577_charger_probe,
 	.remove		= max14577_charger_remove,
+	.id_table	= max14577_charger_id,
 };
 module_platform_driver(max14577_charger_driver);
 
 MODULE_AUTHOR("Krzysztof Kozlowski <k.kozlowski@samsung.com>");
-MODULE_DESCRIPTION("MAXIM 14577 charger driver");
+MODULE_DESCRIPTION("Maxim 14577/77836 charger driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/power/max17040_battery.c b/drivers/power/max17040_battery.c
index 0fbac861080d..14d44706327b 100644
--- a/drivers/power/max17040_battery.c
+++ b/drivers/power/max17040_battery.c
@@ -277,7 +277,8 @@ static SIMPLE_DEV_PM_OPS(max17040_pm_ops, max17040_suspend, max17040_resume);
 #endif /* CONFIG_PM_SLEEP */
 
 static const struct i2c_device_id max17040_id[] = {
-	{ "max17040", 0 },
+	{ "max17040" },
+	{ "max77836-battery" },
 	{ }
 };
 MODULE_DEVICE_TABLE(i2c, max17040_id);
diff --git a/drivers/power/max8925_power.c b/drivers/power/max8925_power.c
index b4513f284bbc..a6d45eef64dd 100644
--- a/drivers/power/max8925_power.c
+++ b/drivers/power/max8925_power.c
@@ -443,7 +443,7 @@ max8925_power_dt_init(struct platform_device *pdev)
 	if (!nproot)
 		return pdev->dev.platform_data;
 
-	np = of_find_node_by_name(nproot, "charger");
+	np = of_get_child_by_name(nproot, "charger");
 	if (!np) {
 		dev_err(&pdev->dev, "failed to find charger node\n");
 		return NULL;
@@ -452,13 +452,14 @@ max8925_power_dt_init(struct platform_device *pdev)
 	pdata = devm_kzalloc(&pdev->dev,
 			sizeof(struct max8925_power_pdata),
 			GFP_KERNEL);
+	if (!pdata)
+		goto ret;
 
 	of_property_read_u32(np, "topoff-threshold", &topoff_threshold);
 	of_property_read_u32(np, "batt-detect", &batt_detect);
 	of_property_read_u32(np, "fast-charge", &fast_charge);
 	of_property_read_u32(np, "no-insert-detect", &no_insert_detect);
 	of_property_read_u32(np, "no-temp-support", &no_temp_support);
-	of_node_put(np);
 
 	pdata->batt_detect = batt_detect;
 	pdata->fast_charge = fast_charge;
@@ -466,6 +467,8 @@ max8925_power_dt_init(struct platform_device *pdev)
 	pdata->no_insert_detect = no_insert_detect;
 	pdata->no_temp_support = no_temp_support;
 
+ret:
+	of_node_put(np);
 	return pdata;
 }
 #else
diff --git a/drivers/power/power_supply_core.c b/drivers/power/power_supply_core.c
index 078afd61490d..6cb7fe5c022d 100644
--- a/drivers/power/power_supply_core.c
+++ b/drivers/power/power_supply_core.c
@@ -58,7 +58,7 @@ static bool __power_supply_is_supplied_by(struct power_supply *supplier,
 
 static int __power_supply_changed_work(struct device *dev, void *data)
 {
-	struct power_supply *psy = (struct power_supply *)data;
+	struct power_supply *psy = data;
 	struct power_supply *pst = dev_get_drvdata(dev);
 
 	if (__power_supply_is_supplied_by(psy, pst)) {
@@ -78,7 +78,14 @@ static void power_supply_changed_work(struct work_struct *work)
 	dev_dbg(psy->dev, "%s\n", __func__);
 
 	spin_lock_irqsave(&psy->changed_lock, flags);
-	if (psy->changed) {
+	/*
+	 * Check 'changed' here to avoid issues due to race between
+	 * power_supply_changed() and this routine. In worst case
+	 * power_supply_changed() can be called again just before we take above
+	 * lock. During the first call of this routine we will mark 'changed' as
+	 * false and it will stay false for the next call as well.
+	 */
+	if (likely(psy->changed)) {
 		psy->changed = false;
 		spin_unlock_irqrestore(&psy->changed_lock, flags);
 		class_for_each_device(power_supply_class, NULL, psy,
@@ -89,12 +96,13 @@ static void power_supply_changed_work(struct work_struct *work)
 		kobject_uevent(&psy->dev->kobj, KOBJ_CHANGE);
 		spin_lock_irqsave(&psy->changed_lock, flags);
 	}
+
 	/*
-	 * Dependent power supplies (e.g. battery) may have changed state
-	 * as a result of this event, so poll again and hold the
-	 * wakeup_source until all events are processed.
+	 * Hold the wakeup_source until all events are processed.
+	 * power_supply_changed() might have called again and have set 'changed'
+	 * to true.
 	 */
-	if (!psy->changed)
+	if (likely(!psy->changed))
 		pm_relax(psy->dev);
 	spin_unlock_irqrestore(&psy->changed_lock, flags);
 }
@@ -119,7 +127,7 @@ EXPORT_SYMBOL_GPL(power_supply_changed);
 static int __power_supply_populate_supplied_from(struct device *dev,
 						 void *data)
 {
-	struct power_supply *psy = (struct power_supply *)data;
+	struct power_supply *psy = data;
 	struct power_supply *epsy = dev_get_drvdata(dev);
 	struct device_node *np;
 	int i = 0;
@@ -127,7 +135,7 @@ static int __power_supply_populate_supplied_from(struct device *dev,
 	do {
 		np = of_parse_phandle(psy->of_node, "power-supplies", i++);
 		if (!np)
-			continue;
+			break;
 
 		if (np == epsy->of_node) {
 			dev_info(psy->dev, "%s: Found supply : %s\n",
@@ -158,12 +166,12 @@ static int power_supply_populate_supplied_from(struct power_supply *psy)
 static int  __power_supply_find_supply_from_node(struct device *dev,
 						 void *data)
 {
-	struct device_node *np = (struct device_node *)data;
+	struct device_node *np = data;
 	struct power_supply *epsy = dev_get_drvdata(dev);
 
-	/* return error breaks out of class_for_each_device loop */
+	/* returning non-zero breaks out of class_for_each_device loop */
 	if (epsy->of_node == np)
-		return -EINVAL;
+		return 1;
 
 	return 0;
 }
@@ -171,30 +179,21 @@ static int  __power_supply_find_supply_from_node(struct device *dev,
 static int power_supply_find_supply_from_node(struct device_node *supply_node)
 {
 	int error;
-	struct device *dev;
-	struct class_dev_iter iter;
 
 	/*
-	 * Use iterator to see if any other device is registered.
-	 * This is required since class_for_each_device returns 0
-	 * if there are no devices registered.
-	 */
-	class_dev_iter_init(&iter, power_supply_class, NULL, NULL);
-	dev = class_dev_iter_next(&iter);
-
-	if (!dev)
-		return -EPROBE_DEFER;
-
-	/*
-	 * We have to treat the return value as inverted, because if
-	 * we return error on not found, then it won't continue looking.
-	 * So we trick it by returning error on success to stop looking
-	 * once the matching device is found.
+	 * class_for_each_device() either returns its own errors or values
+	 * returned by __power_supply_find_supply_from_node().
+	 *
+	 * __power_supply_find_supply_from_node() will return 0 (no match)
+	 * or 1 (match).
+	 *
+	 * We return 0 if class_for_each_device() returned 1, -EPROBE_DEFER if
+	 * it returned 0, or error as returned by it.
 	 */
 	error = class_for_each_device(power_supply_class, NULL, supply_node,
 				       __power_supply_find_supply_from_node);
 
-	return error ? 0 : -EPROBE_DEFER;
+	return error ? (error == 1 ? 0 : error) : -EPROBE_DEFER;
 }
 
 static int power_supply_check_supplies(struct power_supply *psy)
@@ -215,17 +214,21 @@ static int power_supply_check_supplies(struct power_supply *psy)
 
 		np = of_parse_phandle(psy->of_node, "power-supplies", cnt++);
 		if (!np)
-			continue;
+			break;
 
 		ret = power_supply_find_supply_from_node(np);
+		of_node_put(np);
+
 		if (ret) {
-			dev_dbg(psy->dev, "Failed to find supply, defer!\n");
-			of_node_put(np);
-			return -EPROBE_DEFER;
+			dev_dbg(psy->dev, "Failed to find supply!\n");
+			return ret;
 		}
-		of_node_put(np);
 	} while (np);
 
+	/* Missing valid "power-supplies" entries */
+	if (cnt == 1)
+		return 0;
+
 	/* All supplies found, allocate char ** array for filling */
 	psy->supplied_from = devm_kzalloc(psy->dev, sizeof(psy->supplied_from),
 					  GFP_KERNEL);
@@ -234,7 +237,7 @@ static int power_supply_check_supplies(struct power_supply *psy)
 		return -ENOMEM;
 	}
 
-	*psy->supplied_from = devm_kzalloc(psy->dev, sizeof(char *) * cnt,
+	*psy->supplied_from = devm_kzalloc(psy->dev, sizeof(char *) * (cnt - 1),
 					   GFP_KERNEL);
 	if (!*psy->supplied_from) {
 		dev_err(psy->dev, "Couldn't allocate memory for supply list\n");
@@ -253,14 +256,12 @@ static inline int power_supply_check_supplies(struct power_supply *psy)
 static int __power_supply_am_i_supplied(struct device *dev, void *data)
 {
 	union power_supply_propval ret = {0,};
-	struct power_supply *psy = (struct power_supply *)data;
+	struct power_supply *psy = data;
 	struct power_supply *epsy = dev_get_drvdata(dev);
 
 	if (__power_supply_is_supplied_by(epsy, psy))
-		if (!epsy->get_property(epsy, POWER_SUPPLY_PROP_ONLINE, &ret)) {
-			if (ret.intval)
-				return ret.intval;
-		}
+		if (!epsy->get_property(epsy, POWER_SUPPLY_PROP_ONLINE, &ret))
+			return ret.intval;
 
 	return 0;
 }
@@ -285,12 +286,10 @@ static int __power_supply_is_system_supplied(struct device *dev, void *data)
 	unsigned int *count = data;
 
 	(*count)++;
-	if (psy->type != POWER_SUPPLY_TYPE_BATTERY) {
-		if (psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &ret))
-			return 0;
-		if (ret.intval)
+	if (psy->type != POWER_SUPPLY_TYPE_BATTERY)
+		if (!psy->get_property(psy, POWER_SUPPLY_PROP_ONLINE, &ret))
 			return ret.intval;
-	}
+
 	return 0;
 }
 
@@ -423,9 +422,7 @@ static int psy_register_thermal(struct power_supply *psy)
 		if (psy->properties[i] == POWER_SUPPLY_PROP_TEMP) {
 			psy->tzd = thermal_zone_device_register(psy->name, 0, 0,
 					psy, &psy_tzd_ops, NULL, 0, 0);
-			if (IS_ERR(psy->tzd))
-				return PTR_ERR(psy->tzd);
-			break;
+			return PTR_ERR_OR_ZERO(psy->tzd);
 		}
 	}
 	return 0;
@@ -503,9 +500,7 @@ static int psy_register_cooler(struct power_supply *psy)
 			psy->tcd = thermal_cooling_device_register(
 							(char *)psy->name,
 							psy, &psy_tcd_ops);
-			if (IS_ERR(psy->tcd))
-				return PTR_ERR(psy->tcd);
-			break;
+			return PTR_ERR_OR_ZERO(psy->tcd);
 		}
 	}
 	return 0;
@@ -591,7 +586,7 @@ static int __power_supply_register(struct device *parent,
 
 	power_supply_changed(psy);
 
-	goto success;
+	return 0;
 
 create_triggers_failed:
 	psy_unregister_cooler(psy);
@@ -604,7 +599,6 @@ wakeup_init_failed:
 check_supplies_failed:
 dev_set_name_failed:
 	put_device(dev);
-success:
 	return rc;
 }
 
diff --git a/drivers/power/power_supply_leds.c b/drivers/power/power_supply_leds.c
index 995f966ed5b7..effa093c37b0 100644
--- a/drivers/power/power_supply_leds.c
+++ b/drivers/power/power_supply_leds.c
@@ -57,8 +57,6 @@ static void power_supply_update_bat_leds(struct power_supply *psy)
 
 static int power_supply_create_bat_triggers(struct power_supply *psy)
 {
-	int rc = 0;
-
 	psy->charging_full_trig_name = kasprintf(GFP_KERNEL,
 					"%s-charging-or-full", psy->name);
 	if (!psy->charging_full_trig_name)
@@ -87,7 +85,7 @@ static int power_supply_create_bat_triggers(struct power_supply *psy)
 	led_trigger_register_simple(psy->charging_blink_full_solid_trig_name,
 				    &psy->charging_blink_full_solid_trig);
 
-	goto success;
+	return 0;
 
 charging_blink_full_solid_failed:
 	kfree(psy->full_trig_name);
@@ -96,9 +94,7 @@ full_failed:
 charging_failed:
 	kfree(psy->charging_full_trig_name);
 charging_full_failed:
-	rc = -ENOMEM;
-success:
-	return rc;
+	return -ENOMEM;
 }
 
 static void power_supply_remove_bat_triggers(struct power_supply *psy)
@@ -132,20 +128,13 @@ static void power_supply_update_gen_leds(struct power_supply *psy)
 
 static int power_supply_create_gen_triggers(struct power_supply *psy)
 {
-	int rc = 0;
-
 	psy->online_trig_name = kasprintf(GFP_KERNEL, "%s-online", psy->name);
 	if (!psy->online_trig_name)
-		goto online_failed;
+		return -ENOMEM;
 
 	led_trigger_register_simple(psy->online_trig_name, &psy->online_trig);
 
-	goto success;
-
-online_failed:
-	rc = -ENOMEM;
-success:
-	return rc;
+	return 0;
 }
 
 static void power_supply_remove_gen_triggers(struct power_supply *psy)
diff --git a/drivers/power/power_supply_sysfs.c b/drivers/power/power_supply_sysfs.c
index 750a20275664..62653f50a524 100644
--- a/drivers/power/power_supply_sysfs.c
+++ b/drivers/power/power_supply_sysfs.c
@@ -73,19 +73,20 @@ static ssize_t power_supply_show_property(struct device *dev,
 	const ptrdiff_t off = attr - power_supply_attrs;
 	union power_supply_propval value;
 
-	if (off == POWER_SUPPLY_PROP_TYPE)
+	if (off == POWER_SUPPLY_PROP_TYPE) {
 		value.intval = psy->type;
-	else
+	} else {
 		ret = psy->get_property(psy, off, &value);
 
-	if (ret < 0) {
-		if (ret == -ENODATA)
-			dev_dbg(dev, "driver has no data for `%s' property\n",
-				attr->attr.name);
-		else if (ret != -ENODEV)
-			dev_err(dev, "driver failed to report `%s' property: %zd\n",
-				attr->attr.name, ret);
-		return ret;
+		if (ret < 0) {
+			if (ret == -ENODATA)
+				dev_dbg(dev, "driver has no data for `%s' property\n",
+					attr->attr.name);
+			else if (ret != -ENODEV)
+				dev_err(dev, "driver failed to report `%s' property: %zd\n",
+					attr->attr.name, ret);
+			return ret;
+		}
 	}
 
 	if (off == POWER_SUPPLY_PROP_STATUS)
@@ -149,9 +150,11 @@ static struct device_attribute power_supply_attrs[] = {
 	POWER_SUPPLY_ATTR(voltage_now),
 	POWER_SUPPLY_ATTR(voltage_avg),
 	POWER_SUPPLY_ATTR(voltage_ocv),
+	POWER_SUPPLY_ATTR(voltage_boot),
 	POWER_SUPPLY_ATTR(current_max),
 	POWER_SUPPLY_ATTR(current_now),
 	POWER_SUPPLY_ATTR(current_avg),
+	POWER_SUPPLY_ATTR(current_boot),
 	POWER_SUPPLY_ATTR(power_now),
 	POWER_SUPPLY_ATTR(power_avg),
 	POWER_SUPPLY_ATTR(charge_full_design),
@@ -193,6 +196,7 @@ static struct device_attribute power_supply_attrs[] = {
 	POWER_SUPPLY_ATTR(type),
 	POWER_SUPPLY_ATTR(scope),
 	POWER_SUPPLY_ATTR(charge_term_current),
+	POWER_SUPPLY_ATTR(calibrate),
 	/* Properties of type `const char *' */
 	POWER_SUPPLY_ATTR(model_name),
 	POWER_SUPPLY_ATTR(manufacturer),
diff --git a/drivers/power/reset/Kconfig b/drivers/power/reset/Kconfig
index ca41523bbebf..f65ff49bb275 100644
--- a/drivers/power/reset/Kconfig
+++ b/drivers/power/reset/Kconfig
@@ -6,15 +6,33 @@ menuconfig POWER_RESET
 
 	  Say Y here to enable board reset and power off
 
+if POWER_RESET
+
 config POWER_RESET_AS3722
 	bool "ams AS3722 power-off driver"
-	depends on MFD_AS3722 && POWER_RESET
+	depends on MFD_AS3722
 	help
 	  This driver supports turning off board via a ams AS3722 power-off.
 
+config POWER_RESET_AT91_POWEROFF
+	bool "Atmel AT91 poweroff driver"
+	depends on ARCH_AT91
+	default SOC_AT91SAM9 || SOC_SAMA5
+	help
+	  This driver supports poweroff for Atmel AT91SAM9 and SAMA5
+	  SoCs
+
+config POWER_RESET_AT91_RESET
+	bool "Atmel AT91 reset driver"
+	depends on ARCH_AT91
+	default SOC_AT91SAM9 || SOC_SAMA5
+	help
+	  This driver supports restart for Atmel AT91SAM9 and SAMA5
+	  SoCs
+
 config POWER_RESET_AXXIA
 	bool "LSI Axxia reset driver"
-	depends on POWER_RESET && ARCH_AXXIA
+	depends on ARCH_AXXIA
 	help
 	  This driver supports restart for Axxia SoC.
 
@@ -22,7 +40,7 @@ config POWER_RESET_AXXIA
 
 config POWER_RESET_BRCMSTB
 	bool "Broadcom STB reset driver" if COMPILE_TEST
-	depends on POWER_RESET && ARM
+	depends on ARM
 	default ARCH_BRCMSTB
 	help
 	  This driver provides restart support for ARM-based Broadcom STB
@@ -33,27 +51,42 @@ config POWER_RESET_BRCMSTB
 
 config POWER_RESET_GPIO
 	bool "GPIO power-off driver"
-	depends on OF_GPIO && POWER_RESET
+	depends on OF_GPIO
 	help
 	  This driver supports turning off your board via a GPIO line.
 	  If your board needs a GPIO high/low to power down, say Y and
 	  create a binding in your devicetree.
 
+config POWER_RESET_GPIO_RESTART
+	bool "GPIO restart driver"
+	depends on OF_GPIO
+	help
+	  This driver supports restarting your board via a GPIO line.
+	  If your board needs a GPIO high/low to restart, say Y and
+	  create a binding in your devicetree.
+
 config POWER_RESET_HISI
 	bool "Hisilicon power-off driver"
-	depends on POWER_RESET && ARCH_HISI
+	depends on ARCH_HISI
 	help
 	  Reboot support for Hisilicon boards.
 
 config POWER_RESET_MSM
 	bool "Qualcomm MSM power-off driver"
-	depends on POWER_RESET && ARCH_QCOM
+	depends on ARCH_QCOM
 	help
 	  Power off and restart support for Qualcomm boards.
 
+config POWER_RESET_LTC2952
+	bool "LTC2952 PowerPath power-off driver"
+	depends on OF_GPIO
+	help
+	  This driver supports an external powerdown trigger and board power
+	  down via the LTC2952. Bindings are made in the device tree.
+
 config POWER_RESET_QNAP
 	bool "QNAP power-off driver"
-	depends on OF_GPIO && POWER_RESET && PLAT_ORION
+	depends on OF_GPIO && PLAT_ORION
 	help
 	  This driver supports turning off QNAP NAS devices by sending
 	  commands to the microcontroller which controls the main power.
@@ -71,14 +104,28 @@ config POWER_RESET_RESTART
 config POWER_RESET_SUN6I
 	bool "Allwinner A31 SoC reset driver"
 	depends on ARCH_SUNXI
-	depends on POWER_RESET
 	help
 	  Reboot support for the Allwinner A31 SoCs.
 
+config POWER_RESET_ST
+	bool "ST restart power-off driver"
+	depends on ARCH_STI
+	help
+	  Power off and reset support for STMicroelectronics boards.
+
+config POWER_RESET_VERSATILE
+	bool "ARM Versatile family reboot driver"
+	depends on ARM
+	depends on MFD_SYSCON
+	depends on OF
+	help
+	  Power off and restart support for ARM Versatile family of
+	  reference boards.
+
 config POWER_RESET_VEXPRESS
 	bool "ARM Versatile Express power-off and reset driver"
 	depends on ARM || ARM64
-	depends on POWER_RESET && VEXPRESS_CONFIG
+	depends on VEXPRESS_CONFIG
 	help
 	  Power off and reset support for the ARM Ltd. Versatile
 	  Express boards.
@@ -86,7 +133,6 @@ config POWER_RESET_VEXPRESS
 config POWER_RESET_XGENE
 	bool "APM SoC X-Gene reset driver"
 	depends on ARM64
-	depends on POWER_RESET
 	help
 	  Reboot support for the APM SoC X-Gene Eval boards.
 
@@ -97,3 +143,12 @@ config POWER_RESET_KEYSTONE
 	help
 	  Reboot support for the KEYSTONE SoCs.
 
+config POWER_RESET_SYSCON
+	bool "Generic SYSCON regmap reset driver"
+	depends on OF
+	select MFD_SYSCON
+	help
+	  Reboot support for generic SYSCON mapped register reset.
+
+endif
+
diff --git a/drivers/power/reset/Makefile b/drivers/power/reset/Makefile
index a42e70edd037..76ce1c59469b 100644
--- a/drivers/power/reset/Makefile
+++ b/drivers/power/reset/Makefile
@@ -1,12 +1,19 @@
 obj-$(CONFIG_POWER_RESET_AS3722) += as3722-poweroff.o
+obj-$(CONFIG_POWER_RESET_AT91_POWEROFF) += at91-poweroff.o
+obj-$(CONFIG_POWER_RESET_AT91_RESET) += at91-reset.o
 obj-$(CONFIG_POWER_RESET_AXXIA) += axxia-reset.o
 obj-$(CONFIG_POWER_RESET_BRCMSTB) += brcmstb-reboot.o
 obj-$(CONFIG_POWER_RESET_GPIO) += gpio-poweroff.o
+obj-$(CONFIG_POWER_RESET_GPIO_RESTART) += gpio-restart.o
 obj-$(CONFIG_POWER_RESET_HISI) += hisi-reboot.o
 obj-$(CONFIG_POWER_RESET_MSM) += msm-poweroff.o
+obj-$(CONFIG_POWER_RESET_LTC2952) += ltc2952-poweroff.o
 obj-$(CONFIG_POWER_RESET_QNAP) += qnap-poweroff.o
 obj-$(CONFIG_POWER_RESET_RESTART) += restart-poweroff.o
 obj-$(CONFIG_POWER_RESET_SUN6I) += sun6i-reboot.o
+obj-$(CONFIG_POWER_RESET_ST) += st-poweroff.o
+obj-$(CONFIG_POWER_RESET_VERSATILE) += arm-versatile-reboot.o
 obj-$(CONFIG_POWER_RESET_VEXPRESS) += vexpress-poweroff.o
 obj-$(CONFIG_POWER_RESET_XGENE) += xgene-reboot.o
 obj-$(CONFIG_POWER_RESET_KEYSTONE) += keystone-reset.o
+obj-$(CONFIG_POWER_RESET_SYSCON) += syscon-reboot.o
diff --git a/drivers/power/reset/arm-versatile-reboot.c b/drivers/power/reset/arm-versatile-reboot.c
new file mode 100644
index 000000000000..5b08bffcf1a8
--- /dev/null
+++ b/drivers/power/reset/arm-versatile-reboot.c
@@ -0,0 +1,111 @@
+/*
+ * Copyright (C) 2014 Linaro Ltd.
+ *
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+#include <linux/init.h>
+#include <linux/mfd/syscon.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+#include <asm/system_misc.h>
+
+#define REALVIEW_SYS_LOCK_OFFSET	0x20
+#define REALVIEW_SYS_LOCK_VAL		0xA05F
+#define REALVIEW_SYS_RESETCTL_OFFSET	0x40
+
+/*
+ * We detect the different syscon types from the compatible strings.
+ */
+enum versatile_reboot {
+	REALVIEW_REBOOT_EB,
+	REALVIEW_REBOOT_PB1176,
+	REALVIEW_REBOOT_PB11MP,
+	REALVIEW_REBOOT_PBA8,
+	REALVIEW_REBOOT_PBX,
+};
+
+/* Pointer to the system controller */
+static struct regmap *syscon_regmap;
+static enum versatile_reboot versatile_reboot_type;
+
+static const struct of_device_id versatile_reboot_of_match[] = {
+	{
+		.compatible = "arm,realview-eb-syscon",
+		.data = (void *)REALVIEW_REBOOT_EB,
+	},
+	{
+		.compatible = "arm,realview-pb1176-syscon",
+		.data = (void *)REALVIEW_REBOOT_PB1176,
+	},
+	{
+		.compatible = "arm,realview-pb11mp-syscon",
+		.data = (void *)REALVIEW_REBOOT_PB11MP,
+	},
+	{
+		.compatible = "arm,realview-pba8-syscon",
+		.data = (void *)REALVIEW_REBOOT_PBA8,
+	},
+	{
+		.compatible = "arm,realview-pbx-syscon",
+		.data = (void *)REALVIEW_REBOOT_PBX,
+	},
+};
+
+static void versatile_reboot(enum reboot_mode mode, const char *cmd)
+{
+	/* Unlock the reset register */
+	regmap_write(syscon_regmap, REALVIEW_SYS_LOCK_OFFSET,
+		     REALVIEW_SYS_LOCK_VAL);
+	/* Then hit reset on the different machines */
+	switch (versatile_reboot_type) {
+	case REALVIEW_REBOOT_EB:
+		regmap_write(syscon_regmap,
+			     REALVIEW_SYS_RESETCTL_OFFSET, 0x0008);
+		break;
+	case REALVIEW_REBOOT_PB1176:
+		regmap_write(syscon_regmap,
+			     REALVIEW_SYS_RESETCTL_OFFSET, 0x0100);
+		break;
+	case REALVIEW_REBOOT_PB11MP:
+	case REALVIEW_REBOOT_PBA8:
+		regmap_write(syscon_regmap, REALVIEW_SYS_RESETCTL_OFFSET,
+			     0x0000);
+		regmap_write(syscon_regmap, REALVIEW_SYS_RESETCTL_OFFSET,
+			     0x0004);
+		break;
+	case REALVIEW_REBOOT_PBX:
+		regmap_write(syscon_regmap, REALVIEW_SYS_RESETCTL_OFFSET,
+			     0x00f0);
+		regmap_write(syscon_regmap, REALVIEW_SYS_RESETCTL_OFFSET,
+			     0x00f4);
+		break;
+	}
+	dsb();
+}
+
+static int __init versatile_reboot_probe(void)
+{
+	const struct of_device_id *reboot_id;
+	struct device_node *np;
+
+	np = of_find_matching_node_and_match(NULL, versatile_reboot_of_match,
+						 &reboot_id);
+	if (!np)
+		return -ENODEV;
+	versatile_reboot_type = (enum versatile_reboot)reboot_id->data;
+
+	syscon_regmap = syscon_node_to_regmap(np);
+	if (IS_ERR(syscon_regmap))
+		return PTR_ERR(syscon_regmap);
+
+	arm_pm_restart = versatile_reboot;
+	pr_info("versatile reboot driver registered\n");
+	return 0;
+}
+device_initcall(versatile_reboot_probe);
diff --git a/drivers/power/reset/at91-poweroff.c b/drivers/power/reset/at91-poweroff.c
new file mode 100644
index 000000000000..c61000333bb9
--- /dev/null
+++ b/drivers/power/reset/at91-poweroff.c
@@ -0,0 +1,156 @@
+/*
+ * Atmel AT91 SAM9 SoCs reset code
+ *
+ * Copyright (C) 2007 Atmel Corporation.
+ * Copyright (C) 2011 Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>
+ * Copyright (C) 2014 Free Electrons
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/printk.h>
+
+#define AT91_SHDW_CR	0x00		/* Shut Down Control Register */
+#define AT91_SHDW_SHDW		BIT(0)			/* Shut Down command */
+#define AT91_SHDW_KEY		(0xa5 << 24)		/* KEY Password */
+
+#define AT91_SHDW_MR	0x04		/* Shut Down Mode Register */
+#define AT91_SHDW_WKMODE0	GENMASK(2, 0)		/* Wake-up 0 Mode Selection */
+#define AT91_SHDW_CPTWK0_MAX	0xf			/* Maximum Counter On Wake Up 0 */
+#define AT91_SHDW_CPTWK0	(AT91_SHDW_CPTWK0_MAX << 4) /* Counter On Wake Up 0 */
+#define AT91_SHDW_CPTWK0_(x)	((x) << 4)
+#define AT91_SHDW_RTTWKEN	BIT(16)			/* Real Time Timer Wake-up Enable */
+#define AT91_SHDW_RTCWKEN	BIT(17)			/* Real Time Clock Wake-up Enable */
+
+#define AT91_SHDW_SR	0x08		/* Shut Down Status Register */
+#define AT91_SHDW_WAKEUP0	BIT(0)			/* Wake-up 0 Status */
+#define AT91_SHDW_RTTWK		BIT(16)			/* Real-time Timer Wake-up */
+#define AT91_SHDW_RTCWK		BIT(17)			/* Real-time Clock Wake-up [SAM9RL] */
+
+enum wakeup_type {
+	AT91_SHDW_WKMODE0_NONE		= 0,
+	AT91_SHDW_WKMODE0_HIGH		= 1,
+	AT91_SHDW_WKMODE0_LOW		= 2,
+	AT91_SHDW_WKMODE0_ANYLEVEL	= 3,
+};
+
+static const char *shdwc_wakeup_modes[] = {
+	[AT91_SHDW_WKMODE0_NONE]	= "none",
+	[AT91_SHDW_WKMODE0_HIGH]	= "high",
+	[AT91_SHDW_WKMODE0_LOW]		= "low",
+	[AT91_SHDW_WKMODE0_ANYLEVEL]	= "any",
+};
+
+static void __iomem *at91_shdwc_base;
+
+static void __init at91_wakeup_status(void)
+{
+	u32 reg = readl(at91_shdwc_base + AT91_SHDW_SR);
+	char *reason = "unknown";
+
+	/* Simple power-on, just bail out */
+	if (!reg)
+		return;
+
+	if (reg & AT91_SHDW_RTTWK)
+		reason = "RTT";
+	else if (reg & AT91_SHDW_RTCWK)
+		reason = "RTC";
+
+	pr_info("AT91: Wake-Up source: %s\n", reason);
+}
+
+static void at91_poweroff(void)
+{
+	writel(AT91_SHDW_KEY | AT91_SHDW_SHDW, at91_shdwc_base + AT91_SHDW_CR);
+}
+
+const enum wakeup_type at91_poweroff_get_wakeup_mode(struct device_node *np)
+{
+	const char *pm;
+	int err, i;
+
+	err = of_property_read_string(np, "atmel,wakeup-mode", &pm);
+	if (err < 0)
+		return AT91_SHDW_WKMODE0_ANYLEVEL;
+
+	for (i = 0; i < ARRAY_SIZE(shdwc_wakeup_modes); i++)
+		if (!strcasecmp(pm, shdwc_wakeup_modes[i]))
+			return i;
+
+	return -ENODEV;
+}
+
+static void at91_poweroff_dt_set_wakeup_mode(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	enum wakeup_type wakeup_mode;
+	u32 mode = 0, tmp;
+
+	wakeup_mode = at91_poweroff_get_wakeup_mode(np);
+	if (wakeup_mode < 0) {
+		dev_warn(&pdev->dev, "shdwc unknown wakeup mode\n");
+		return;
+	}
+
+	if (!of_property_read_u32(np, "atmel,wakeup-counter", &tmp)) {
+		if (tmp > AT91_SHDW_CPTWK0_MAX) {
+			dev_warn(&pdev->dev,
+				 "shdwc wakeup counter 0x%x > 0x%x reduce it to 0x%x\n",
+				 tmp, AT91_SHDW_CPTWK0_MAX, AT91_SHDW_CPTWK0_MAX);
+			tmp = AT91_SHDW_CPTWK0_MAX;
+		}
+		mode |= AT91_SHDW_CPTWK0_(tmp);
+	}
+
+	if (of_property_read_bool(np, "atmel,wakeup-rtc-timer"))
+			mode |= AT91_SHDW_RTCWKEN;
+
+	if (of_property_read_bool(np, "atmel,wakeup-rtt-timer"))
+			mode |= AT91_SHDW_RTTWKEN;
+
+	writel(wakeup_mode | mode, at91_shdwc_base + AT91_SHDW_MR);
+}
+
+static int at91_poweroff_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	at91_shdwc_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(at91_shdwc_base)) {
+		dev_err(&pdev->dev, "Could not map reset controller address\n");
+		return PTR_ERR(at91_shdwc_base);
+	}
+
+	at91_wakeup_status();
+
+	if (pdev->dev.of_node)
+		at91_poweroff_dt_set_wakeup_mode(pdev);
+
+	pm_power_off = at91_poweroff;
+
+	return 0;
+}
+
+static struct of_device_id at91_poweroff_of_match[] = {
+	{ .compatible = "atmel,at91sam9260-shdwc", },
+	{ .compatible = "atmel,at91sam9rl-shdwc", },
+	{ .compatible = "atmel,at91sam9x5-shdwc", },
+	{ /*sentinel*/ }
+};
+
+static struct platform_driver at91_poweroff_driver = {
+	.probe = at91_poweroff_probe,
+	.driver = {
+		.name = "at91-poweroff",
+		.of_match_table = at91_poweroff_of_match,
+	},
+};
+module_platform_driver(at91_poweroff_driver);
diff --git a/drivers/power/reset/at91-reset.c b/drivers/power/reset/at91-reset.c
new file mode 100644
index 000000000000..3611806c9cfd
--- /dev/null
+++ b/drivers/power/reset/at91-reset.c
@@ -0,0 +1,252 @@
+/*
+ * Atmel AT91 SAM9 SoCs reset code
+ *
+ * Copyright (C) 2007 Atmel Corporation.
+ * Copyright (C) BitBox Ltd 2010
+ * Copyright (C) 2011 Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcosoft.com>
+ * Copyright (C) 2014 Free Electrons
+ *
+ * This file is licensed under the terms of the GNU General Public
+ * License version 2.  This program is licensed "as is" without any
+ * warranty of any kind, whether express or implied.
+ */
+
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+
+#include <asm/system_misc.h>
+
+#include <mach/at91sam9_ddrsdr.h>
+#include <mach/at91sam9_sdramc.h>
+
+#define AT91_RSTC_CR	0x00		/* Reset Controller Control Register */
+#define AT91_RSTC_PROCRST	BIT(0)		/* Processor Reset */
+#define AT91_RSTC_PERRST	BIT(2)		/* Peripheral Reset */
+#define AT91_RSTC_EXTRST	BIT(3)		/* External Reset */
+#define AT91_RSTC_KEY		(0xa5 << 24)	/* KEY Password */
+
+#define AT91_RSTC_SR	0x04		/* Reset Controller Status Register */
+#define AT91_RSTC_URSTS		BIT(0)		/* User Reset Status */
+#define AT91_RSTC_RSTTYP	GENMASK(10, 8)	/* Reset Type */
+#define AT91_RSTC_NRSTL		BIT(16)		/* NRST Pin Level */
+#define AT91_RSTC_SRCMP		BIT(17)		/* Software Reset Command in Progress */
+
+#define AT91_RSTC_MR	0x08		/* Reset Controller Mode Register */
+#define AT91_RSTC_URSTEN	BIT(0)		/* User Reset Enable */
+#define AT91_RSTC_URSTIEN	BIT(4)		/* User Reset Interrupt Enable */
+#define AT91_RSTC_ERSTL		GENMASK(11, 8)	/* External Reset Length */
+
+enum reset_type {
+	RESET_TYPE_GENERAL	= 0,
+	RESET_TYPE_WAKEUP	= 1,
+	RESET_TYPE_WATCHDOG	= 2,
+	RESET_TYPE_SOFTWARE	= 3,
+	RESET_TYPE_USER		= 4,
+};
+
+static void __iomem *at91_ramc_base[2], *at91_rstc_base;
+
+/*
+* unless the SDRAM is cleanly shutdown before we hit the
+* reset register it can be left driving the data bus and
+* killing the chance of a subsequent boot from NAND
+*/
+static void at91sam9260_restart(enum reboot_mode mode, const char *cmd)
+{
+	asm volatile(
+		/* Align to cache lines */
+		".balign 32\n\t"
+
+		/* Disable SDRAM accesses */
+		"str	%2, [%0, #" __stringify(AT91_SDRAMC_TR) "]\n\t"
+
+		/* Power down SDRAM */
+		"str	%3, [%0, #" __stringify(AT91_SDRAMC_LPR) "]\n\t"
+
+		/* Reset CPU */
+		"str	%4, [%1, #" __stringify(AT91_RSTC_CR) "]\n\t"
+
+		"b	.\n\t"
+		:
+		: "r" (at91_ramc_base[0]),
+		  "r" (at91_rstc_base),
+		  "r" (1),
+		  "r" (AT91_SDRAMC_LPCB_POWER_DOWN),
+		  "r" (AT91_RSTC_KEY | AT91_RSTC_PERRST | AT91_RSTC_PROCRST));
+}
+
+static void at91sam9g45_restart(enum reboot_mode mode, const char *cmd)
+{
+	asm volatile(
+		/*
+		 * Test wether we have a second RAM controller to care
+		 * about.
+		 *
+		 * First, test that we can dereference the virtual address.
+		 */
+		"cmp	%1, #0\n\t"
+		"beq	1f\n\t"
+
+		/* Then, test that the RAM controller is enabled */
+		"ldr	r0, [%1]\n\t"
+		"cmp	r0, #0\n\t"
+
+		/* Align to cache lines */
+		".balign 32\n\t"
+
+		/* Disable SDRAM0 accesses */
+		"1:	str	%3, [%0, #" __stringify(AT91_DDRSDRC_RTR) "]\n\t"
+		/* Power down SDRAM0 */
+		"	str	%4, [%0, #" __stringify(AT91_DDRSDRC_RTR) "]\n\t"
+		/* Disable SDRAM1 accesses */
+		"	strne	%3, [%1, #" __stringify(AT91_DDRSDRC_RTR) "]\n\t"
+		/* Power down SDRAM1 */
+		"	strne	%4, [%1, #" __stringify(AT91_DDRSDRC_RTR) "]\n\t"
+		/* Reset CPU */
+		"	str	%5, [%2, #" __stringify(AT91_RSTC_CR) "]\n\t"
+
+		"	b	.\n\t"
+		:
+		: "r" (at91_ramc_base[0]),
+		  "r" (at91_ramc_base[1]),
+		  "r" (at91_rstc_base),
+		  "r" (1),
+		  "r" (AT91_DDRSDRC_LPCB_POWER_DOWN),
+		  "r" (AT91_RSTC_KEY | AT91_RSTC_PERRST | AT91_RSTC_PROCRST)
+		: "r0");
+}
+
+static void __init at91_reset_status(struct platform_device *pdev)
+{
+	u32 reg = readl(at91_rstc_base + AT91_RSTC_SR);
+	char *reason;
+
+	switch ((reg & AT91_RSTC_RSTTYP) >> 8) {
+	case RESET_TYPE_GENERAL:
+		reason = "general reset";
+		break;
+	case RESET_TYPE_WAKEUP:
+		reason = "wakeup";
+		break;
+	case RESET_TYPE_WATCHDOG:
+		reason = "watchdog reset";
+		break;
+	case RESET_TYPE_SOFTWARE:
+		reason = "software reset";
+		break;
+	case RESET_TYPE_USER:
+		reason = "user reset";
+		break;
+	default:
+		reason = "unknown reset";
+		break;
+	}
+
+	pr_info("AT91: Starting after %s\n", reason);
+}
+
+static struct of_device_id at91_ramc_of_match[] = {
+	{ .compatible = "atmel,at91sam9260-sdramc", },
+	{ .compatible = "atmel,at91sam9g45-ddramc", },
+	{ .compatible = "atmel,sama5d3-ddramc", },
+	{ /* sentinel */ }
+};
+
+static struct of_device_id at91_reset_of_match[] = {
+	{ .compatible = "atmel,at91sam9260-rstc", .data = at91sam9260_restart },
+	{ .compatible = "atmel,at91sam9g45-rstc", .data = at91sam9g45_restart },
+	{ /* sentinel */ }
+};
+
+static int at91_reset_of_probe(struct platform_device *pdev)
+{
+	const struct of_device_id *match;
+	struct device_node *np;
+	int idx = 0;
+
+	at91_rstc_base = of_iomap(pdev->dev.of_node, 0);
+	if (!at91_rstc_base) {
+		dev_err(&pdev->dev, "Could not map reset controller address\n");
+		return -ENODEV;
+	}
+
+	for_each_matching_node(np, at91_ramc_of_match) {
+		at91_ramc_base[idx] = of_iomap(np, 0);
+		if (!at91_ramc_base[idx]) {
+			dev_err(&pdev->dev, "Could not map ram controller address\n");
+			return -ENODEV;
+		}
+		idx++;
+	}
+
+	match = of_match_node(at91_reset_of_match, pdev->dev.of_node);
+	arm_pm_restart = match->data;
+
+	return 0;
+}
+
+static int at91_reset_platform_probe(struct platform_device *pdev)
+{
+	const struct platform_device_id *match;
+	struct resource *res;
+	int idx = 0;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	at91_rstc_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(at91_rstc_base)) {
+		dev_err(&pdev->dev, "Could not map reset controller address\n");
+		return PTR_ERR(at91_rstc_base);
+	}
+
+	for (idx = 0; idx < 2; idx++) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, idx + 1 );
+		at91_ramc_base[idx] = devm_ioremap(&pdev->dev, res->start,
+						   resource_size(res));
+		if (IS_ERR(at91_ramc_base[idx])) {
+			dev_err(&pdev->dev, "Could not map ram controller address\n");
+			return PTR_ERR(at91_ramc_base[idx]);
+		}
+	}
+
+	match = platform_get_device_id(pdev);
+	arm_pm_restart = (void (*)(enum reboot_mode, const char*))
+		match->driver_data;
+
+	return 0;
+}
+
+static int at91_reset_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	if (pdev->dev.of_node)
+		ret = at91_reset_of_probe(pdev);
+	else
+		ret = at91_reset_platform_probe(pdev);
+
+	if (ret)
+		return ret;
+
+	at91_reset_status(pdev);
+
+	return 0;
+}
+
+static struct platform_device_id at91_reset_plat_match[] = {
+	{ "at91-sam9260-reset", (unsigned long)at91sam9260_restart },
+	{ "at91-sam9g45-reset", (unsigned long)at91sam9g45_restart },
+	{ /* sentinel */ }
+};
+
+static struct platform_driver at91_reset_driver = {
+	.probe = at91_reset_probe,
+	.driver = {
+		.name = "at91-reset",
+		.of_match_table = at91_reset_of_match,
+	},
+	.id_table = at91_reset_plat_match,
+};
+module_platform_driver(at91_reset_driver);
diff --git a/drivers/power/reset/gpio-restart.c b/drivers/power/reset/gpio-restart.c
new file mode 100644
index 000000000000..a76829b3f1cd
--- /dev/null
+++ b/drivers/power/reset/gpio-restart.c
@@ -0,0 +1,149 @@
+/*
+ * Toggles a GPIO pin to restart a device
+ *
+ * Copyright (C) 2014 Google, Inc.
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * Based on the gpio-poweroff driver.
+ */
+#include <linux/reboot.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/gpio/consumer.h>
+#include <linux/of_platform.h>
+#include <linux/module.h>
+
+struct gpio_restart {
+	struct gpio_desc *reset_gpio;
+	struct notifier_block restart_handler;
+	u32 active_delay_ms;
+	u32 inactive_delay_ms;
+	u32 wait_delay_ms;
+};
+
+static int gpio_restart_notify(struct notifier_block *this,
+				unsigned long mode, void *cmd)
+{
+	struct gpio_restart *gpio_restart =
+		container_of(this, struct gpio_restart, restart_handler);
+
+	/* drive it active, also inactive->active edge */
+	gpiod_direction_output(gpio_restart->reset_gpio, 1);
+	mdelay(gpio_restart->active_delay_ms);
+
+	/* drive inactive, also active->inactive edge */
+	gpiod_set_value(gpio_restart->reset_gpio, 0);
+	mdelay(gpio_restart->inactive_delay_ms);
+
+	/* drive it active, also inactive->active edge */
+	gpiod_set_value(gpio_restart->reset_gpio, 1);
+
+	/* give it some time */
+	mdelay(gpio_restart->wait_delay_ms);
+
+	WARN_ON(1);
+
+	return NOTIFY_DONE;
+}
+
+static int gpio_restart_probe(struct platform_device *pdev)
+{
+	struct gpio_restart *gpio_restart;
+	bool open_source = false;
+	u32 property;
+	int ret;
+
+	gpio_restart = devm_kzalloc(&pdev->dev, sizeof(*gpio_restart),
+			GFP_KERNEL);
+	if (!gpio_restart)
+		return -ENOMEM;
+
+	open_source = of_property_read_bool(pdev->dev.of_node, "open-source");
+
+	gpio_restart->reset_gpio = devm_gpiod_get(&pdev->dev, NULL,
+			open_source ? GPIOD_IN : GPIOD_OUT_LOW);
+	if (IS_ERR(gpio_restart->reset_gpio)) {
+		dev_err(&pdev->dev, "Could net get reset GPIO\n");
+		return PTR_ERR(gpio_restart->reset_gpio);
+	}
+
+	gpio_restart->restart_handler.notifier_call = gpio_restart_notify;
+	gpio_restart->restart_handler.priority = 128;
+	gpio_restart->active_delay_ms = 100;
+	gpio_restart->inactive_delay_ms = 100;
+	gpio_restart->wait_delay_ms = 3000;
+
+	ret = of_property_read_u32(pdev->dev.of_node, "priority", &property);
+	if (!ret) {
+		if (property > 255)
+			dev_err(&pdev->dev, "Invalid priority property: %u\n",
+					property);
+		else
+			gpio_restart->restart_handler.priority = property;
+	}
+
+	of_property_read_u32(pdev->dev.of_node, "active-delay",
+			&gpio_restart->active_delay_ms);
+	of_property_read_u32(pdev->dev.of_node, "inactive-delay",
+			&gpio_restart->inactive_delay_ms);
+	of_property_read_u32(pdev->dev.of_node, "wait-delay",
+			&gpio_restart->wait_delay_ms);
+
+	platform_set_drvdata(pdev, gpio_restart);
+
+	ret = register_restart_handler(&gpio_restart->restart_handler);
+	if (ret) {
+		dev_err(&pdev->dev, "%s: cannot register restart handler, %d\n",
+				__func__, ret);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static int gpio_restart_remove(struct platform_device *pdev)
+{
+	struct gpio_restart *gpio_restart = platform_get_drvdata(pdev);
+	int ret;
+
+	ret = unregister_restart_handler(&gpio_restart->restart_handler);
+	if (ret) {
+		dev_err(&pdev->dev,
+				"%s: cannot unregister restart handler, %d\n",
+				__func__, ret);
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id of_gpio_restart_match[] = {
+	{ .compatible = "gpio-restart", },
+	{},
+};
+
+static struct platform_driver gpio_restart_driver = {
+	.probe = gpio_restart_probe,
+	.remove = gpio_restart_remove,
+	.driver = {
+		.name = "restart-gpio",
+		.owner = THIS_MODULE,
+		.of_match_table = of_gpio_restart_match,
+	},
+};
+
+module_platform_driver(gpio_restart_driver);
+
+MODULE_AUTHOR("David Riley <davidriley@chromium.org>");
+MODULE_DESCRIPTION("GPIO restart driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/power/reset/ltc2952-poweroff.c b/drivers/power/reset/ltc2952-poweroff.c
new file mode 100644
index 000000000000..116a1cef8f7b
--- /dev/null
+++ b/drivers/power/reset/ltc2952-poweroff.c
@@ -0,0 +1,386 @@
+/*
+ * LTC2952 (PowerPath) driver
+ *
+ * Copyright (C) 2014, Xsens Technologies BV <info@xsens.com>
+ * Maintainer: René Moll <linux@r-moll.nl>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * ----------------------------------------
+ * - Description
+ * ----------------------------------------
+ *
+ * This driver is to be used with an external PowerPath Controller (LTC2952).
+ * Its function is to determine when a external shut down is triggered
+ * and react by properly shutting down the system.
+ *
+ * This driver expects a device tree with a ltc2952 entry for pin mapping.
+ *
+ * ----------------------------------------
+ * - GPIO
+ * ----------------------------------------
+ *
+ * The following GPIOs are used:
+ * - trigger (input)
+ *     A level change indicates the shut-down trigger. If it's state reverts
+ *     within the time-out defined by trigger_delay, the shut down is not
+ *     executed.
+ *
+ * - watchdog (output)
+ *     Once a shut down is triggered, the driver will toggle this signal,
+ *     with an internal (wde_interval) to stall the hardware shut down.
+ *
+ * - kill (output)
+ *     The last action during shut down is triggering this signalling, such
+ *     that the PowerPath Control will power down the hardware.
+ *
+ * ----------------------------------------
+ * - Interrupts
+ * ----------------------------------------
+ *
+ * The driver requires a non-shared, edge-triggered interrupt on the trigger
+ * GPIO.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/ktime.h>
+#include <linux/slab.h>
+#include <linux/kmod.h>
+#include <linux/module.h>
+#include <linux/gpio/consumer.h>
+#include <linux/reboot.h>
+
+struct ltc2952_poweroff_data {
+	struct hrtimer timer_trigger;
+	struct hrtimer timer_wde;
+
+	ktime_t trigger_delay;
+	ktime_t wde_interval;
+
+	struct device *dev;
+
+	unsigned int virq;
+
+	/**
+	 * 0: trigger
+	 * 1: watchdog
+	 * 2: kill
+	 */
+	struct gpio_desc *gpio[3];
+};
+
+static int ltc2952_poweroff_panic;
+static struct ltc2952_poweroff_data *ltc2952_data;
+
+#define POWERPATH_IO_TRIGGER	0
+#define POWERPATH_IO_WATCHDOG	1
+#define POWERPATH_IO_KILL	2
+
+/**
+ * ltc2952_poweroff_timer_wde - Timer callback
+ * Toggles the watchdog reset signal each wde_interval
+ *
+ * @timer: corresponding timer
+ *
+ * Returns HRTIMER_RESTART for an infinite loop which will only stop when the
+ * machine actually shuts down
+ */
+static enum hrtimer_restart ltc2952_poweroff_timer_wde(struct hrtimer *timer)
+{
+	ktime_t now;
+	int state;
+	unsigned long overruns;
+
+	if (ltc2952_poweroff_panic)
+		return HRTIMER_NORESTART;
+
+	state = gpiod_get_value(ltc2952_data->gpio[POWERPATH_IO_WATCHDOG]);
+	gpiod_set_value(ltc2952_data->gpio[POWERPATH_IO_WATCHDOG], !state);
+
+	now = hrtimer_cb_get_time(timer);
+	overruns = hrtimer_forward(timer, now, ltc2952_data->wde_interval);
+
+	return HRTIMER_RESTART;
+}
+
+static enum hrtimer_restart ltc2952_poweroff_timer_trigger(
+	struct hrtimer *timer)
+{
+	int ret;
+
+	ret = hrtimer_start(&ltc2952_data->timer_wde,
+		ltc2952_data->wde_interval, HRTIMER_MODE_REL);
+
+	if (ret) {
+		dev_err(ltc2952_data->dev, "unable to start the timer\n");
+		/*
+		 * The device will not toggle the watchdog reset,
+		 * thus shut down is only safe if the PowerPath controller
+		 * has a long enough time-off before triggering a hardware
+		 * power-off.
+		 *
+		 * Only sending a warning as the system will power-off anyway
+		 */
+	}
+
+	dev_info(ltc2952_data->dev, "executing shutdown\n");
+
+	orderly_poweroff(true);
+
+	return HRTIMER_NORESTART;
+}
+
+/**
+ * ltc2952_poweroff_handler - Interrupt handler
+ * Triggered each time the trigger signal changes state and (de)activates a
+ * time-out (timer_trigger). Once the time-out is actually reached the shut
+ * down is executed.
+ *
+ * @irq: IRQ number
+ * @dev_id: pointer to the main data structure
+ */
+static irqreturn_t ltc2952_poweroff_handler(int irq, void *dev_id)
+{
+	int ret;
+	struct ltc2952_poweroff_data *data = dev_id;
+
+	if (ltc2952_poweroff_panic)
+		goto irq_ok;
+
+	if (hrtimer_active(&data->timer_wde)) {
+		/* shutdown is already triggered, nothing to do any more */
+		goto irq_ok;
+	}
+
+	if (!hrtimer_active(&data->timer_trigger)) {
+		ret = hrtimer_start(&data->timer_trigger, data->trigger_delay,
+			HRTIMER_MODE_REL);
+
+		if (ret)
+			dev_err(data->dev, "unable to start the wait timer\n");
+	} else {
+		ret = hrtimer_cancel(&data->timer_trigger);
+		/* omitting return value check, timer should have been valid */
+	}
+
+irq_ok:
+	return IRQ_HANDLED;
+}
+
+static void ltc2952_poweroff_kill(void)
+{
+	gpiod_set_value(ltc2952_data->gpio[POWERPATH_IO_KILL], 1);
+}
+
+static int ltc2952_poweroff_suspend(struct platform_device *pdev,
+	pm_message_t state)
+{
+	return -ENOSYS;
+}
+
+static int ltc2952_poweroff_resume(struct platform_device *pdev)
+{
+	return -ENOSYS;
+}
+
+static void ltc2952_poweroff_default(struct ltc2952_poweroff_data *data)
+{
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(data->gpio); i++)
+		data->gpio[i] = NULL;
+
+	data->wde_interval = ktime_set(0, 300L*1E6L);
+	data->trigger_delay = ktime_set(2, 500L*1E6L);
+
+	hrtimer_init(&data->timer_trigger, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	data->timer_trigger.function = &ltc2952_poweroff_timer_trigger;
+
+	hrtimer_init(&data->timer_wde, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
+	data->timer_wde.function = &ltc2952_poweroff_timer_wde;
+}
+
+static int ltc2952_poweroff_init(struct platform_device *pdev)
+{
+	int ret, virq;
+	unsigned int i;
+	struct ltc2952_poweroff_data *data;
+
+	static char *name[] = {
+		"trigger",
+		"watchdog",
+		"kill",
+		NULL
+	};
+
+	data = ltc2952_data;
+	ltc2952_poweroff_default(ltc2952_data);
+
+	for (i = 0; i < ARRAY_SIZE(ltc2952_data->gpio); i++) {
+		ltc2952_data->gpio[i] = gpiod_get(&pdev->dev, name[i]);
+
+		if (IS_ERR(ltc2952_data->gpio[i])) {
+			ret = PTR_ERR(ltc2952_data->gpio[i]);
+			dev_err(&pdev->dev,
+				"unable to claim the following gpio: %s\n",
+				name[i]);
+			goto err_io;
+		}
+	}
+
+	ret = gpiod_direction_output(
+		ltc2952_data->gpio[POWERPATH_IO_WATCHDOG], 0);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to use watchdog-gpio as output\n");
+		goto err_io;
+	}
+
+	ret = gpiod_direction_output(ltc2952_data->gpio[POWERPATH_IO_KILL], 0);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to use kill-gpio as output\n");
+		goto err_io;
+	}
+
+	virq = gpiod_to_irq(ltc2952_data->gpio[POWERPATH_IO_TRIGGER]);
+	if (virq < 0) {
+		dev_err(&pdev->dev, "cannot map GPIO as interrupt");
+		goto err_io;
+	}
+
+	ltc2952_data->virq = virq;
+	ret = request_irq(virq,
+		ltc2952_poweroff_handler,
+		(IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING),
+		"ltc2952-poweroff",
+		ltc2952_data
+	);
+
+	if (ret) {
+		dev_err(&pdev->dev, "cannot configure an interrupt handler\n");
+		goto err_io;
+	}
+
+	return 0;
+
+err_io:
+	for (i = 0; i < ARRAY_SIZE(ltc2952_data->gpio); i++)
+		if (ltc2952_data->gpio[i])
+			gpiod_put(ltc2952_data->gpio[i]);
+
+	return ret;
+}
+
+static int ltc2952_poweroff_probe(struct platform_device *pdev)
+{
+	int ret;
+
+	if (pm_power_off) {
+		dev_err(&pdev->dev, "pm_power_off already registered");
+		return -EBUSY;
+	}
+
+	ltc2952_data = kzalloc(sizeof(*ltc2952_data), GFP_KERNEL);
+	if (!ltc2952_data)
+		return -ENOMEM;
+
+	ltc2952_data->dev = &pdev->dev;
+
+	ret = ltc2952_poweroff_init(pdev);
+	if (ret)
+		goto err;
+
+	pm_power_off = &ltc2952_poweroff_kill;
+
+	dev_info(&pdev->dev, "probe successful\n");
+
+	return 0;
+
+err:
+	kfree(ltc2952_data);
+	return ret;
+}
+
+static int ltc2952_poweroff_remove(struct platform_device *pdev)
+{
+	unsigned int i;
+
+	pm_power_off = NULL;
+
+	if (ltc2952_data) {
+		free_irq(ltc2952_data->virq, ltc2952_data);
+
+		for (i = 0; i < ARRAY_SIZE(ltc2952_data->gpio); i++)
+			gpiod_put(ltc2952_data->gpio[i]);
+
+		kfree(ltc2952_data);
+	}
+
+	return 0;
+}
+
+static const struct of_device_id of_ltc2952_poweroff_match[] = {
+	{ .compatible = "lltc,ltc2952"},
+	{},
+};
+MODULE_DEVICE_TABLE(of, of_ltc2952_poweroff_match);
+
+static struct platform_driver ltc2952_poweroff_driver = {
+	.probe = ltc2952_poweroff_probe,
+	.remove = ltc2952_poweroff_remove,
+	.driver = {
+		.name = "ltc2952-poweroff",
+		.owner = THIS_MODULE,
+		.of_match_table = of_ltc2952_poweroff_match,
+	},
+	.suspend = ltc2952_poweroff_suspend,
+	.resume = ltc2952_poweroff_resume,
+};
+
+static int ltc2952_poweroff_notify_panic(struct notifier_block *nb,
+	unsigned long code, void *unused)
+{
+	ltc2952_poweroff_panic = 1;
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block ltc2952_poweroff_panic_nb = {
+	.notifier_call = ltc2952_poweroff_notify_panic,
+};
+
+static int __init ltc2952_poweroff_platform_init(void)
+{
+	ltc2952_poweroff_panic = 0;
+
+	atomic_notifier_chain_register(&panic_notifier_list,
+		&ltc2952_poweroff_panic_nb);
+
+	return platform_driver_register(&ltc2952_poweroff_driver);
+}
+
+static void __exit ltc2952_poweroff_platform_exit(void)
+{
+	atomic_notifier_chain_unregister(&panic_notifier_list,
+		&ltc2952_poweroff_panic_nb);
+
+	platform_driver_unregister(&ltc2952_poweroff_driver);
+}
+
+module_init(ltc2952_poweroff_platform_init);
+module_exit(ltc2952_poweroff_platform_exit);
+
+MODULE_AUTHOR("René Moll <rene.moll@xsens.com>");
+MODULE_DESCRIPTION("LTC PowerPath power-off driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/power/reset/msm-poweroff.c b/drivers/power/reset/msm-poweroff.c
index 774f9a3b310d..4702efdfe466 100644
--- a/drivers/power/reset/msm-poweroff.c
+++ b/drivers/power/reset/msm-poweroff.c
@@ -20,21 +20,27 @@
 #include <linux/platform_device.h>
 #include <linux/module.h>
 #include <linux/reboot.h>
-
-#include <asm/system_misc.h>
+#include <linux/pm.h>
 
 static void __iomem *msm_ps_hold;
-
-static void do_msm_restart(enum reboot_mode reboot_mode, const char *cmd)
+static int do_msm_restart(struct notifier_block *nb, unsigned long action,
+			   void *data)
 {
 	writel(0, msm_ps_hold);
 	mdelay(10000);
+
+	return NOTIFY_DONE;
 }
 
+static struct notifier_block restart_nb = {
+	.notifier_call = do_msm_restart,
+	.priority = 128,
+};
+
 static void do_msm_poweroff(void)
 {
 	/* TODO: Add poweroff capability */
-	do_msm_restart(REBOOT_HARD, NULL);
+	do_msm_restart(&restart_nb, 0, NULL);
 }
 
 static int msm_restart_probe(struct platform_device *pdev)
@@ -47,8 +53,10 @@ static int msm_restart_probe(struct platform_device *pdev)
 	if (IS_ERR(msm_ps_hold))
 		return PTR_ERR(msm_ps_hold);
 
+	register_restart_handler(&restart_nb);
+
 	pm_power_off = do_msm_poweroff;
-	arm_pm_restart = do_msm_restart;
+
 	return 0;
 }
 
diff --git a/drivers/power/reset/restart-poweroff.c b/drivers/power/reset/restart-poweroff.c
index 3e51f8d29bfe..edd707ee7281 100644
--- a/drivers/power/reset/restart-poweroff.c
+++ b/drivers/power/reset/restart-poweroff.c
@@ -20,7 +20,8 @@
 
 static void restart_poweroff_do_poweroff(void)
 {
-	arm_pm_restart(REBOOT_HARD, NULL);
+	reboot_mode = REBOOT_HARD;
+	machine_restart(NULL);
 }
 
 static int restart_poweroff_probe(struct platform_device *pdev)
diff --git a/drivers/power/reset/st-poweroff.c b/drivers/power/reset/st-poweroff.c
new file mode 100644
index 000000000000..a0acf25ee2a2
--- /dev/null
+++ b/drivers/power/reset/st-poweroff.c
@@ -0,0 +1,151 @@
+/*
+ * Copyright (C) 2014 STMicroelectronics
+ *
+ * Power off Restart driver, used in STMicroelectronics devices.
+ *
+ * Author: Christophe Kerello <christophe.kerello@st.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+
+#include <asm/system_misc.h>
+
+struct reset_syscfg {
+	struct regmap *regmap;
+	/* syscfg used for reset */
+	unsigned int offset_rst;
+	unsigned int mask_rst;
+	/* syscfg used for unmask the reset */
+	unsigned int offset_rst_msk;
+	unsigned int mask_rst_msk;
+};
+
+/* STiH415 */
+#define STIH415_SYSCFG_11	0x2c
+#define STIH415_SYSCFG_15	0x3c
+
+static struct reset_syscfg stih415_reset = {
+	.offset_rst = STIH415_SYSCFG_11,
+	.mask_rst = BIT(0),
+	.offset_rst_msk = STIH415_SYSCFG_15,
+	.mask_rst_msk = BIT(0)
+};
+
+/* STiH416 */
+#define STIH416_SYSCFG_500	0x7d0
+#define STIH416_SYSCFG_504	0x7e0
+
+static struct reset_syscfg stih416_reset = {
+	.offset_rst = STIH416_SYSCFG_500,
+	.mask_rst = BIT(0),
+	.offset_rst_msk = STIH416_SYSCFG_504,
+	.mask_rst_msk = BIT(0)
+};
+
+/* STiH407 */
+#define STIH407_SYSCFG_4000	0x0
+#define STIH407_SYSCFG_4008	0x20
+
+static struct reset_syscfg stih407_reset = {
+	.offset_rst = STIH407_SYSCFG_4000,
+	.mask_rst = BIT(0),
+	.offset_rst_msk = STIH407_SYSCFG_4008,
+	.mask_rst_msk = BIT(0)
+};
+
+/* STiD127 */
+#define STID127_SYSCFG_700	0x0
+#define STID127_SYSCFG_773	0x124
+
+static struct reset_syscfg stid127_reset = {
+	.offset_rst = STID127_SYSCFG_773,
+	.mask_rst = BIT(0),
+	.offset_rst_msk = STID127_SYSCFG_700,
+	.mask_rst_msk = BIT(8)
+};
+
+static struct reset_syscfg *st_restart_syscfg;
+
+static void st_restart(enum reboot_mode reboot_mode, const char *cmd)
+{
+	/* reset syscfg updated */
+	regmap_update_bits(st_restart_syscfg->regmap,
+			   st_restart_syscfg->offset_rst,
+			   st_restart_syscfg->mask_rst,
+			   0);
+
+	/* unmask the reset */
+	regmap_update_bits(st_restart_syscfg->regmap,
+			   st_restart_syscfg->offset_rst_msk,
+			   st_restart_syscfg->mask_rst_msk,
+			   0);
+}
+
+static struct of_device_id st_reset_of_match[] = {
+	{
+		.compatible = "st,stih415-restart",
+		.data = (void *)&stih415_reset,
+	}, {
+		.compatible = "st,stih416-restart",
+		.data = (void *)&stih416_reset,
+	}, {
+		.compatible = "st,stih407-restart",
+		.data = (void *)&stih407_reset,
+	}, {
+		.compatible = "st,stid127-restart",
+		.data = (void *)&stid127_reset,
+	},
+	{}
+};
+
+static int st_reset_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match;
+	struct device *dev = &pdev->dev;
+
+	match = of_match_device(st_reset_of_match, dev);
+	if (!match)
+		return -ENODEV;
+
+	st_restart_syscfg = (struct reset_syscfg *)match->data;
+
+	st_restart_syscfg->regmap =
+		syscon_regmap_lookup_by_phandle(np, "st,syscfg");
+	if (IS_ERR(st_restart_syscfg->regmap)) {
+		dev_err(dev, "No syscfg phandle specified\n");
+		return PTR_ERR(st_restart_syscfg->regmap);
+	}
+
+	arm_pm_restart = st_restart;
+
+	return 0;
+}
+
+static struct platform_driver st_reset_driver = {
+	.probe = st_reset_probe,
+	.driver = {
+		.name = "st_reset",
+		.of_match_table = st_reset_of_match,
+	},
+};
+
+static int __init st_reset_init(void)
+{
+	return platform_driver_register(&st_reset_driver);
+}
+
+device_initcall(st_reset_init);
+
+MODULE_AUTHOR("Christophe Kerello <christophe.kerello@st.com>");
+MODULE_DESCRIPTION("STMicroelectronics Power off Restart driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/power/reset/syscon-reboot.c b/drivers/power/reset/syscon-reboot.c
new file mode 100644
index 000000000000..815b901822cf
--- /dev/null
+++ b/drivers/power/reset/syscon-reboot.c
@@ -0,0 +1,91 @@
+/*
+ * Generic Syscon Reboot Driver
+ *
+ * Copyright (c) 2013, Applied Micro Circuits Corporation
+ * Author: Feng Kan <fkan@apm.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/notifier.h>
+#include <linux/mfd/syscon.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/regmap.h>
+
+struct syscon_reboot_context {
+	struct regmap *map;
+	u32 offset;
+	u32 mask;
+	struct notifier_block restart_handler;
+};
+
+static int syscon_restart_handle(struct notifier_block *this,
+					unsigned long mode, void *cmd)
+{
+	struct syscon_reboot_context *ctx =
+			container_of(this, struct syscon_reboot_context,
+					restart_handler);
+
+	/* Issue the reboot */
+	regmap_write(ctx->map, ctx->offset, ctx->mask);
+
+	mdelay(1000);
+
+	pr_emerg("Unable to restart system\n");
+	return NOTIFY_DONE;
+}
+
+static int syscon_reboot_probe(struct platform_device *pdev)
+{
+	struct syscon_reboot_context *ctx;
+	struct device *dev = &pdev->dev;
+	int err;
+
+	ctx = devm_kzalloc(&pdev->dev, sizeof(*ctx), GFP_KERNEL);
+	if (!ctx)
+		return -ENOMEM;
+
+	ctx->map = syscon_regmap_lookup_by_phandle(dev->of_node, "regmap");
+	if (IS_ERR(ctx->map))
+		return PTR_ERR(ctx->map);
+
+	if (of_property_read_u32(pdev->dev.of_node, "offset", &ctx->offset))
+		return -EINVAL;
+
+	if (of_property_read_u32(pdev->dev.of_node, "mask", &ctx->mask))
+		return -EINVAL;
+
+	ctx->restart_handler.notifier_call = syscon_restart_handle;
+	ctx->restart_handler.priority = 128;
+	err = register_restart_handler(&ctx->restart_handler);
+	if (err)
+		dev_err(dev, "can't register restart notifier (err=%d)\n", err);
+
+	return err;
+}
+
+static struct of_device_id syscon_reboot_of_match[] = {
+	{ .compatible = "syscon-reboot" },
+	{}
+};
+
+static struct platform_driver syscon_reboot_driver = {
+	.probe = syscon_reboot_probe,
+	.driver = {
+		.name = "syscon-reboot",
+		.of_match_table = syscon_reboot_of_match,
+	},
+};
+module_platform_driver(syscon_reboot_driver);
diff --git a/drivers/power/reset/xgene-reboot.c b/drivers/power/reset/xgene-reboot.c
index ecd55f81b9d1..6b49be6867ab 100644
--- a/drivers/power/reset/xgene-reboot.c
+++ b/drivers/power/reset/xgene-reboot.c
@@ -40,7 +40,7 @@ struct xgene_reboot_context {
 
 static struct xgene_reboot_context *xgene_restart_ctx;
 
-static void xgene_restart(char str, const char *cmd)
+static void xgene_restart(enum reboot_mode mode, const char *cmd)
 {
 	struct xgene_reboot_context *ctx = xgene_restart_ctx;
 	unsigned long timeout;
diff --git a/drivers/power/sbs-battery.c b/drivers/power/sbs-battery.c
index b5f2a76b6cdf..c7b7b4018df3 100644
--- a/drivers/power/sbs-battery.c
+++ b/drivers/power/sbs-battery.c
@@ -48,7 +48,10 @@ enum {
 	REG_FULL_CHARGE_CAPACITY_CHARGE,
 	REG_DESIGN_CAPACITY,
 	REG_DESIGN_CAPACITY_CHARGE,
-	REG_DESIGN_VOLTAGE,
+	REG_DESIGN_VOLTAGE_MIN,
+	REG_DESIGN_VOLTAGE_MAX,
+	REG_MANUFACTURER,
+	REG_MODEL_NAME,
 };
 
 /* Battery Mode defines */
@@ -68,6 +71,7 @@ enum sbs_battery_mode {
 #define BATTERY_FULL_CHARGED		0x20
 #define BATTERY_FULL_DISCHARGED		0x10
 
+/* min_value and max_value are only valid for numerical data */
 #define SBS_DATA(_psp, _addr, _min_value, _max_value) { \
 	.psp = _psp, \
 	.addr = _addr, \
@@ -111,10 +115,17 @@ static const struct chip_data {
 		SBS_DATA(POWER_SUPPLY_PROP_ENERGY_FULL_DESIGN, 0x18, 0, 65535),
 	[REG_DESIGN_CAPACITY_CHARGE] =
 		SBS_DATA(POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN, 0x18, 0, 65535),
-	[REG_DESIGN_VOLTAGE] =
+	[REG_DESIGN_VOLTAGE_MIN] =
+		SBS_DATA(POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN, 0x19, 0, 65535),
+	[REG_DESIGN_VOLTAGE_MAX] =
 		SBS_DATA(POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN, 0x19, 0, 65535),
 	[REG_SERIAL_NUMBER] =
 		SBS_DATA(POWER_SUPPLY_PROP_SERIAL_NUMBER, 0x1C, 0, 65535),
+	/* Properties of type `const char *' */
+	[REG_MANUFACTURER] =
+		SBS_DATA(POWER_SUPPLY_PROP_MANUFACTURER, 0x20, 0, 65535),
+	[REG_MODEL_NAME] =
+		SBS_DATA(POWER_SUPPLY_PROP_MODEL_NAME, 0x21, 0, 65535)
 };
 
 static enum power_supply_property sbs_properties[] = {
@@ -130,6 +141,7 @@ static enum power_supply_property sbs_properties[] = {
 	POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG,
 	POWER_SUPPLY_PROP_TIME_TO_FULL_AVG,
 	POWER_SUPPLY_PROP_SERIAL_NUMBER,
+	POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN,
 	POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN,
 	POWER_SUPPLY_PROP_ENERGY_NOW,
 	POWER_SUPPLY_PROP_ENERGY_FULL,
@@ -137,6 +149,9 @@ static enum power_supply_property sbs_properties[] = {
 	POWER_SUPPLY_PROP_CHARGE_NOW,
 	POWER_SUPPLY_PROP_CHARGE_FULL,
 	POWER_SUPPLY_PROP_CHARGE_FULL_DESIGN,
+	/* Properties of type `const char *' */
+	POWER_SUPPLY_PROP_MANUFACTURER,
+	POWER_SUPPLY_PROP_MODEL_NAME
 };
 
 struct sbs_info {
@@ -153,6 +168,9 @@ struct sbs_info {
 	int				ignore_changes;
 };
 
+static char model_name[I2C_SMBUS_BLOCK_MAX + 1];
+static char manufacturer[I2C_SMBUS_BLOCK_MAX + 1];
+
 static int sbs_read_word_data(struct i2c_client *client, u8 address)
 {
 	struct sbs_info *chip = i2c_get_clientdata(client);
@@ -179,6 +197,74 @@ static int sbs_read_word_data(struct i2c_client *client, u8 address)
 	return le16_to_cpu(ret);
 }
 
+static int sbs_read_string_data(struct i2c_client *client, u8 address,
+				char *values)
+{
+	struct sbs_info *chip = i2c_get_clientdata(client);
+	s32 ret = 0, block_length = 0;
+	int retries_length = 1, retries_block = 1;
+	u8 block_buffer[I2C_SMBUS_BLOCK_MAX + 1];
+
+	if (chip->pdata) {
+		retries_length = max(chip->pdata->i2c_retry_count + 1, 1);
+		retries_block = max(chip->pdata->i2c_retry_count + 1, 1);
+	}
+
+	/* Adapter needs to support these two functions */
+	if (!i2c_check_functionality(client->adapter,
+				     I2C_FUNC_SMBUS_BYTE_DATA |
+				     I2C_FUNC_SMBUS_I2C_BLOCK)){
+		return -ENODEV;
+	}
+
+	/* Get the length of block data */
+	while (retries_length > 0) {
+		ret = i2c_smbus_read_byte_data(client, address);
+		if (ret >= 0)
+			break;
+		retries_length--;
+	}
+
+	if (ret < 0) {
+		dev_dbg(&client->dev,
+			"%s: i2c read at address 0x%x failed\n",
+			__func__, address);
+		return ret;
+	}
+
+	/* block_length does not include NULL terminator */
+	block_length = ret;
+	if (block_length > I2C_SMBUS_BLOCK_MAX) {
+		dev_err(&client->dev,
+			"%s: Returned block_length is longer than 0x%x\n",
+			__func__, I2C_SMBUS_BLOCK_MAX);
+		return -EINVAL;
+	}
+
+	/* Get the block data */
+	while (retries_block > 0) {
+		ret = i2c_smbus_read_i2c_block_data(
+				client, address,
+				block_length + 1, block_buffer);
+		if (ret >= 0)
+			break;
+		retries_block--;
+	}
+
+	if (ret < 0) {
+		dev_dbg(&client->dev,
+			"%s: i2c read at address 0x%x failed\n",
+			__func__, address);
+		return ret;
+	}
+
+	/* block_buffer[0] == block_length */
+	memcpy(values, block_buffer + 1, block_length);
+	values[block_length] = '\0';
+
+	return le16_to_cpu(ret);
+}
+
 static int sbs_write_word_data(struct i2c_client *client, u8 address,
 	u16 value)
 {
@@ -318,6 +404,19 @@ static int sbs_get_battery_property(struct i2c_client *client,
 	return 0;
 }
 
+static int sbs_get_battery_string_property(struct i2c_client *client,
+	int reg_offset, enum power_supply_property psp, char *val)
+{
+	s32 ret;
+
+	ret = sbs_read_string_data(client, sbs_data[reg_offset].addr, val);
+
+	if (ret < 0)
+		return ret;
+
+	return 0;
+}
+
 static void  sbs_unit_adjustment(struct i2c_client *client,
 	enum power_supply_property psp, union power_supply_propval *val)
 {
@@ -336,6 +435,7 @@ static void  sbs_unit_adjustment(struct i2c_client *client,
 		break;
 
 	case POWER_SUPPLY_PROP_VOLTAGE_NOW:
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
 	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
 	case POWER_SUPPLY_PROP_CURRENT_NOW:
 	case POWER_SUPPLY_PROP_CHARGE_NOW:
@@ -497,6 +597,7 @@ static int sbs_get_property(struct power_supply *psy,
 	case POWER_SUPPLY_PROP_TEMP:
 	case POWER_SUPPLY_PROP_TIME_TO_EMPTY_AVG:
 	case POWER_SUPPLY_PROP_TIME_TO_FULL_AVG:
+	case POWER_SUPPLY_PROP_VOLTAGE_MIN_DESIGN:
 	case POWER_SUPPLY_PROP_VOLTAGE_MAX_DESIGN:
 		ret = sbs_get_property_index(client, psp);
 		if (ret < 0)
@@ -505,6 +606,26 @@ static int sbs_get_property(struct power_supply *psy,
 		ret = sbs_get_battery_property(client, ret, psp, val);
 		break;
 
+	case POWER_SUPPLY_PROP_MODEL_NAME:
+		ret = sbs_get_property_index(client, psp);
+		if (ret < 0)
+			break;
+
+		ret = sbs_get_battery_string_property(client, ret, psp,
+						      model_name);
+		val->strval = model_name;
+		break;
+
+	case POWER_SUPPLY_PROP_MANUFACTURER:
+		ret = sbs_get_property_index(client, psp);
+		if (ret < 0)
+			break;
+
+		ret = sbs_get_battery_string_property(client, ret, psp,
+						      manufacturer);
+		val->strval = manufacturer;
+		break;
+
 	default:
 		dev_err(&client->dev,
 			"%s: INVALID property\n", __func__);
diff --git a/drivers/powercap/intel_rapl.c b/drivers/powercap/intel_rapl.c
index b1cda6ffdbcc..45e05b32f9b6 100644
--- a/drivers/powercap/intel_rapl.c
+++ b/drivers/powercap/intel_rapl.c
@@ -953,6 +953,7 @@ static const struct x86_cpu_id rapl_ids[] = {
 	{ X86_VENDOR_INTEL, 6, 0x3a},/* Ivy Bridge */
 	{ X86_VENDOR_INTEL, 6, 0x3c},/* Haswell */
 	{ X86_VENDOR_INTEL, 6, 0x3d},/* Broadwell */
+	{ X86_VENDOR_INTEL, 6, 0x3f},/* Haswell */
 	{ X86_VENDOR_INTEL, 6, 0x45},/* Haswell ULT */
 	/* TODO: Add more CPU IDs after testing */
 	{}
@@ -1166,11 +1167,10 @@ static int rapl_detect_domains(struct rapl_package *rp, int cpu)
 
 	for (i = 0; i < RAPL_DOMAIN_MAX; i++) {
 		/* use physical package id to read counters */
-		if (!rapl_check_domain(cpu, i))
+		if (!rapl_check_domain(cpu, i)) {
 			rp->domain_map |= 1 << i;
-		else
-			pr_warn("RAPL domain %s detection failed\n",
-				rapl_domain_names[i]);
+			pr_info("Found RAPL domain %s\n", rapl_domain_names[i]);
+		}
 	}
 	rp->nr_domains = bitmap_weight(&rp->domain_map,	RAPL_DOMAIN_MAX);
 	if (!rp->nr_domains) {
diff --git a/drivers/pwm/Kconfig b/drivers/pwm/Kconfig
index b800783800a3..ef2dd2e4754b 100644
--- a/drivers/pwm/Kconfig
+++ b/drivers/pwm/Kconfig
@@ -83,6 +83,7 @@ config PWM_BFIN
 config PWM_CLPS711X
 	tristate "CLPS711X PWM support"
 	depends on ARCH_CLPS711X || COMPILE_TEST
+	depends on HAS_IOMEM
 	help
 	  Generic PWM framework driver for Cirrus Logic CLPS711X.
 
@@ -101,6 +102,7 @@ config PWM_EP93XX
 config PWM_FSL_FTM
 	tristate "Freescale FlexTimer Module (FTM) PWM support"
 	depends on OF
+	select REGMAP_MMIO
 	help
 	  Generic FTM PWM framework driver for Freescale VF610 and
 	  Layerscape LS-1 SoCs.
@@ -149,7 +151,7 @@ config PWM_LPC32XX
 
 config PWM_LPSS
 	tristate "Intel LPSS PWM support"
-	depends on ACPI
+	depends on X86
 	help
 	  Generic PWM framework driver for Intel Low Power Subsystem PWM
 	  controller.
@@ -157,6 +159,24 @@ config PWM_LPSS
 	  To compile this driver as a module, choose M here: the module
 	  will be called pwm-lpss.
 
+config PWM_LPSS_PCI
+	tristate "Intel LPSS PWM PCI driver"
+	depends on PWM_LPSS && PCI
+	help
+	  The PCI driver for Intel Low Power Subsystem PWM controller.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-lpss-pci.
+
+config PWM_LPSS_PLATFORM
+	tristate "Intel LPSS PWM platform driver"
+	depends on PWM_LPSS && ACPI
+	help
+	  The platform driver for Intel Low Power Subsystem PWM controller.
+
+	  To compile this driver as a module, choose M here: the module
+	  will be called pwm-lpss-platform.
+
 config PWM_MXS
 	tristate "Freescale MXS PWM support"
 	depends on ARCH_MXS && OF
diff --git a/drivers/pwm/Makefile b/drivers/pwm/Makefile
index f8c577d41091..c458606c3755 100644
--- a/drivers/pwm/Makefile
+++ b/drivers/pwm/Makefile
@@ -13,6 +13,8 @@ obj-$(CONFIG_PWM_JZ4740)	+= pwm-jz4740.o
 obj-$(CONFIG_PWM_LP3943)	+= pwm-lp3943.o
 obj-$(CONFIG_PWM_LPC32XX)	+= pwm-lpc32xx.o
 obj-$(CONFIG_PWM_LPSS)		+= pwm-lpss.o
+obj-$(CONFIG_PWM_LPSS_PCI)	+= pwm-lpss-pci.o
+obj-$(CONFIG_PWM_LPSS_PLATFORM)	+= pwm-lpss-platform.o
 obj-$(CONFIG_PWM_MXS)		+= pwm-mxs.o
 obj-$(CONFIG_PWM_PCA9685)	+= pwm-pca9685.o
 obj-$(CONFIG_PWM_PUV3)		+= pwm-puv3.o
diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
index 4b66bf09ee55..966497d10c6e 100644
--- a/drivers/pwm/core.c
+++ b/drivers/pwm/core.c
@@ -236,7 +236,7 @@ int pwmchip_add(struct pwm_chip *chip)
 	int ret;
 
 	if (!chip || !chip->dev || !chip->ops || !chip->ops->config ||
-	    !chip->ops->enable || !chip->ops->disable)
+	    !chip->ops->enable || !chip->ops->disable || !chip->npwm)
 		return -EINVAL;
 
 	mutex_lock(&pwm_lock);
@@ -602,9 +602,8 @@ struct pwm_device *pwm_get(struct device *dev, const char *con_id)
 	struct pwm_device *pwm = ERR_PTR(-EPROBE_DEFER);
 	const char *dev_id = dev ? dev_name(dev) : NULL;
 	struct pwm_chip *chip = NULL;
-	unsigned int index = 0;
 	unsigned int best = 0;
-	struct pwm_lookup *p;
+	struct pwm_lookup *p, *chosen = NULL;
 	unsigned int match;
 
 	/* look up via DT first */
@@ -651,8 +650,7 @@ struct pwm_device *pwm_get(struct device *dev, const char *con_id)
 		}
 
 		if (match > best) {
-			chip = pwmchip_find_by_name(p->provider);
-			index = p->index;
+			chosen = p;
 
 			if (match != 3)
 				best = match;
@@ -661,17 +659,22 @@ struct pwm_device *pwm_get(struct device *dev, const char *con_id)
 		}
 	}
 
-	mutex_unlock(&pwm_lookup_lock);
+	if (!chosen)
+		goto out;
 
-	if (chip)
-		pwm = pwm_request_from_chip(chip, index, con_id ?: dev_id);
-	if (IS_ERR(pwm))
-		return pwm;
+	chip = pwmchip_find_by_name(chosen->provider);
+	if (!chip)
+		goto out;
 
-	pwm_set_period(pwm, p->period);
-	pwm_set_polarity(pwm, p->polarity);
+	pwm = pwm_request_from_chip(chip, chosen->index, con_id ?: dev_id);
+	if (IS_ERR(pwm))
+		goto out;
 
+	pwm_set_period(pwm, chosen->period);
+	pwm_set_polarity(pwm, chosen->polarity);
 
+out:
+	mutex_unlock(&pwm_lookup_lock);
 	return pwm;
 }
 EXPORT_SYMBOL_GPL(pwm_get);
diff --git a/drivers/pwm/pwm-atmel-tcb.c b/drivers/pwm/pwm-atmel-tcb.c
index f3dcd02390f1..d56e5b717431 100644
--- a/drivers/pwm/pwm-atmel-tcb.c
+++ b/drivers/pwm/pwm-atmel-tcb.c
@@ -379,7 +379,7 @@ static int atmel_tcb_pwm_probe(struct platform_device *pdev)
 		return err;
 	}
 
-	tc = atmel_tc_alloc(tcblock, "tcb-pwm");
+	tc = atmel_tc_alloc(tcblock);
 	if (tc == NULL) {
 		dev_err(&pdev->dev, "failed to allocate Timer Counter Block\n");
 		return -ENOMEM;
diff --git a/drivers/pwm/pwm-atmel.c b/drivers/pwm/pwm-atmel.c
index 6e700a541ca3..d3c22de9ee47 100644
--- a/drivers/pwm/pwm-atmel.c
+++ b/drivers/pwm/pwm-atmel.c
@@ -102,7 +102,7 @@ static int atmel_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 			    int duty_ns, int period_ns)
 {
 	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
-	unsigned long clk_rate, prd, dty;
+	unsigned long prd, dty;
 	unsigned long long div;
 	unsigned int pres = 0;
 	u32 val;
@@ -113,20 +113,18 @@ static int atmel_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 		return -EBUSY;
 	}
 
-	clk_rate = clk_get_rate(atmel_pwm->clk);
-	div = clk_rate;
+	/* Calculate the period cycles and prescale value */
+	div = (unsigned long long)clk_get_rate(atmel_pwm->clk) * period_ns;
+	do_div(div, NSEC_PER_SEC);
 
-	/* Calculate the period cycles */
 	while (div > PWM_MAX_PRD) {
-		div = clk_rate / (1 << pres);
-		div = div * period_ns;
-		/* 1/Hz = 100000000 ns */
-		do_div(div, 1000000000);
-
-		if (pres++ > PRD_MAX_PRES) {
-			dev_err(chip->dev, "pres exceeds the maximum value\n");
-			return -EINVAL;
-		}
+		div >>= 1;
+		pres++;
+	}
+
+	if (pres > PRD_MAX_PRES) {
+		dev_err(chip->dev, "pres exceeds the maximum value\n");
+		return -EINVAL;
 	}
 
 	/* Calculate the duty cycles */
diff --git a/drivers/pwm/pwm-fsl-ftm.c b/drivers/pwm/pwm-fsl-ftm.c
index a18bc8fea385..0f2cc7ef7784 100644
--- a/drivers/pwm/pwm-fsl-ftm.c
+++ b/drivers/pwm/pwm-fsl-ftm.c
@@ -18,14 +18,14 @@
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
 #include <linux/pwm.h>
+#include <linux/regmap.h>
 #include <linux/slab.h>
 
 #define FTM_SC		0x00
-#define FTM_SC_CLK_MASK	0x3
-#define FTM_SC_CLK_SHIFT	3
-#define FTM_SC_CLK(c)	(((c) + 1) << FTM_SC_CLK_SHIFT)
+#define FTM_SC_CLK_MASK_SHIFT	3
+#define FTM_SC_CLK_MASK	(3 << FTM_SC_CLK_MASK_SHIFT)
+#define FTM_SC_CLK(c)	(((c) + 1) << FTM_SC_CLK_MASK_SHIFT)
 #define FTM_SC_PS_MASK	0x7
-#define FTM_SC_PS_SHIFT	0
 
 #define FTM_CNT		0x04
 #define FTM_MOD		0x08
@@ -83,7 +83,7 @@ struct fsl_pwm_chip {
 	unsigned int cnt_select;
 	unsigned int clk_ps;
 
-	void __iomem *base;
+	struct regmap *regmap;
 
 	int period_ns;
 
@@ -219,10 +219,11 @@ static unsigned long fsl_pwm_calculate_duty(struct fsl_pwm_chip *fpc,
 					    unsigned long period_ns,
 					    unsigned long duty_ns)
 {
-	unsigned long long val, duty;
+	unsigned long long duty;
+	u32 val;
 
-	val = readl(fpc->base + FTM_MOD);
-	duty = duty_ns * (val + 1);
+	regmap_read(fpc->regmap, FTM_MOD, &val);
+	duty = (unsigned long long)duty_ns * (val + 1);
 	do_div(duty, period_ns);
 
 	return (unsigned long)duty;
@@ -232,7 +233,7 @@ static int fsl_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 			  int duty_ns, int period_ns)
 {
 	struct fsl_pwm_chip *fpc = to_fsl_chip(chip);
-	u32 val, period, duty;
+	u32 period, duty;
 
 	mutex_lock(&fpc->lock);
 
@@ -257,11 +258,9 @@ static int fsl_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 			return -EINVAL;
 		}
 
-		val = readl(fpc->base + FTM_SC);
-		val &= ~(FTM_SC_PS_MASK << FTM_SC_PS_SHIFT);
-		val |= fpc->clk_ps;
-		writel(val, fpc->base + FTM_SC);
-		writel(period - 1, fpc->base + FTM_MOD);
+		regmap_update_bits(fpc->regmap, FTM_SC, FTM_SC_PS_MASK,
+				   fpc->clk_ps);
+		regmap_write(fpc->regmap, FTM_MOD, period - 1);
 
 		fpc->period_ns = period_ns;
 	}
@@ -270,8 +269,9 @@ static int fsl_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 
 	duty = fsl_pwm_calculate_duty(fpc, period_ns, duty_ns);
 
-	writel(FTM_CSC_MSB | FTM_CSC_ELSB, fpc->base + FTM_CSC(pwm->hwpwm));
-	writel(duty, fpc->base + FTM_CV(pwm->hwpwm));
+	regmap_write(fpc->regmap, FTM_CSC(pwm->hwpwm),
+		     FTM_CSC_MSB | FTM_CSC_ELSB);
+	regmap_write(fpc->regmap, FTM_CV(pwm->hwpwm), duty);
 
 	return 0;
 }
@@ -283,31 +283,28 @@ static int fsl_pwm_set_polarity(struct pwm_chip *chip,
 	struct fsl_pwm_chip *fpc = to_fsl_chip(chip);
 	u32 val;
 
-	val = readl(fpc->base + FTM_POL);
+	regmap_read(fpc->regmap, FTM_POL, &val);
 
 	if (polarity == PWM_POLARITY_INVERSED)
 		val |= BIT(pwm->hwpwm);
 	else
 		val &= ~BIT(pwm->hwpwm);
 
-	writel(val, fpc->base + FTM_POL);
+	regmap_write(fpc->regmap, FTM_POL, val);
 
 	return 0;
 }
 
 static int fsl_counter_clock_enable(struct fsl_pwm_chip *fpc)
 {
-	u32 val;
 	int ret;
 
 	if (fpc->use_count != 0)
 		return 0;
 
 	/* select counter clock source */
-	val = readl(fpc->base + FTM_SC);
-	val &= ~(FTM_SC_CLK_MASK << FTM_SC_CLK_SHIFT);
-	val |= FTM_SC_CLK(fpc->cnt_select);
-	writel(val, fpc->base + FTM_SC);
+	regmap_update_bits(fpc->regmap, FTM_SC, FTM_SC_CLK_MASK,
+			   FTM_SC_CLK(fpc->cnt_select));
 
 	ret = clk_prepare_enable(fpc->clk[fpc->cnt_select]);
 	if (ret)
@@ -327,13 +324,10 @@ static int fsl_counter_clock_enable(struct fsl_pwm_chip *fpc)
 static int fsl_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct fsl_pwm_chip *fpc = to_fsl_chip(chip);
-	u32 val;
 	int ret;
 
 	mutex_lock(&fpc->lock);
-	val = readl(fpc->base + FTM_OUTMASK);
-	val &= ~BIT(pwm->hwpwm);
-	writel(val, fpc->base + FTM_OUTMASK);
+	regmap_update_bits(fpc->regmap, FTM_OUTMASK, BIT(pwm->hwpwm), 0);
 
 	ret = fsl_counter_clock_enable(fpc);
 	mutex_unlock(&fpc->lock);
@@ -343,8 +337,6 @@ static int fsl_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 
 static void fsl_counter_clock_disable(struct fsl_pwm_chip *fpc)
 {
-	u32 val;
-
 	/*
 	 * already disabled, do nothing
 	 */
@@ -356,9 +348,7 @@ static void fsl_counter_clock_disable(struct fsl_pwm_chip *fpc)
 		return;
 
 	/* no users left, disable PWM counter clock */
-	val = readl(fpc->base + FTM_SC);
-	val &= ~(FTM_SC_CLK_MASK << FTM_SC_CLK_SHIFT);
-	writel(val, fpc->base + FTM_SC);
+	regmap_update_bits(fpc->regmap, FTM_SC, FTM_SC_CLK_MASK, 0);
 
 	clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_CNTEN]);
 	clk_disable_unprepare(fpc->clk[fpc->cnt_select]);
@@ -370,14 +360,12 @@ static void fsl_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 	u32 val;
 
 	mutex_lock(&fpc->lock);
-	val = readl(fpc->base + FTM_OUTMASK);
-	val |= BIT(pwm->hwpwm);
-	writel(val, fpc->base + FTM_OUTMASK);
+	regmap_update_bits(fpc->regmap, FTM_OUTMASK, BIT(pwm->hwpwm),
+			   BIT(pwm->hwpwm));
 
 	fsl_counter_clock_disable(fpc);
 
-	val = readl(fpc->base + FTM_OUTMASK);
-
+	regmap_read(fpc->regmap, FTM_OUTMASK, &val);
 	if ((val & 0xFF) == 0xFF)
 		fpc->period_ns = 0;
 
@@ -402,19 +390,28 @@ static int fsl_pwm_init(struct fsl_pwm_chip *fpc)
 	if (ret)
 		return ret;
 
-	writel(0x00, fpc->base + FTM_CNTIN);
-	writel(0x00, fpc->base + FTM_OUTINIT);
-	writel(0xFF, fpc->base + FTM_OUTMASK);
+	regmap_write(fpc->regmap, FTM_CNTIN, 0x00);
+	regmap_write(fpc->regmap, FTM_OUTINIT, 0x00);
+	regmap_write(fpc->regmap, FTM_OUTMASK, 0xFF);
 
 	clk_disable_unprepare(fpc->clk[FSL_PWM_CLK_SYS]);
 
 	return 0;
 }
 
+static const struct regmap_config fsl_pwm_regmap_config = {
+	.reg_bits = 32,
+	.reg_stride = 4,
+	.val_bits = 32,
+
+	.max_register = FTM_PWMLOAD,
+};
+
 static int fsl_pwm_probe(struct platform_device *pdev)
 {
 	struct fsl_pwm_chip *fpc;
 	struct resource *res;
+	void __iomem *base;
 	int ret;
 
 	fpc = devm_kzalloc(&pdev->dev, sizeof(*fpc), GFP_KERNEL);
@@ -426,9 +423,16 @@ static int fsl_pwm_probe(struct platform_device *pdev)
 	fpc->chip.dev = &pdev->dev;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	fpc->base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(fpc->base))
-		return PTR_ERR(fpc->base);
+	base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(base))
+		return PTR_ERR(base);
+
+	fpc->regmap = devm_regmap_init_mmio_clk(&pdev->dev, NULL, base,
+						&fsl_pwm_regmap_config);
+	if (IS_ERR(fpc->regmap)) {
+		dev_err(&pdev->dev, "regmap init failed\n");
+		return PTR_ERR(fpc->regmap);
+	}
 
 	fpc->clk[FSL_PWM_CLK_SYS] = devm_clk_get(&pdev->dev, "ftm_sys");
 	if (IS_ERR(fpc->clk[FSL_PWM_CLK_SYS])) {
diff --git a/drivers/pwm/pwm-imx.c b/drivers/pwm/pwm-imx.c
index 5449d9150d40..f8b5f109c1ab 100644
--- a/drivers/pwm/pwm-imx.c
+++ b/drivers/pwm/pwm-imx.c
@@ -14,6 +14,7 @@
 #include <linux/slab.h>
 #include <linux/err.h>
 #include <linux/clk.h>
+#include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/pwm.h>
 #include <linux/of.h>
@@ -21,24 +22,30 @@
 
 /* i.MX1 and i.MX21 share the same PWM function block: */
 
-#define MX1_PWMC    0x00   /* PWM Control Register */
-#define MX1_PWMS    0x04   /* PWM Sample Register */
-#define MX1_PWMP    0x08   /* PWM Period Register */
+#define MX1_PWMC			0x00   /* PWM Control Register */
+#define MX1_PWMS			0x04   /* PWM Sample Register */
+#define MX1_PWMP			0x08   /* PWM Period Register */
 
-#define MX1_PWMC_EN		(1 << 4)
+#define MX1_PWMC_EN			(1 << 4)
 
 /* i.MX27, i.MX31, i.MX35 share the same PWM function block: */
 
-#define MX3_PWMCR                 0x00    /* PWM Control Register */
-#define MX3_PWMSAR                0x0C    /* PWM Sample Register */
-#define MX3_PWMPR                 0x10    /* PWM Period Register */
-#define MX3_PWMCR_PRESCALER(x)    (((x - 1) & 0xFFF) << 4)
-#define MX3_PWMCR_DOZEEN                (1 << 24)
-#define MX3_PWMCR_WAITEN                (1 << 23)
+#define MX3_PWMCR			0x00    /* PWM Control Register */
+#define MX3_PWMSR			0x04    /* PWM Status Register */
+#define MX3_PWMSAR			0x0C    /* PWM Sample Register */
+#define MX3_PWMPR			0x10    /* PWM Period Register */
+#define MX3_PWMCR_PRESCALER(x)		((((x) - 1) & 0xFFF) << 4)
+#define MX3_PWMCR_DOZEEN		(1 << 24)
+#define MX3_PWMCR_WAITEN		(1 << 23)
 #define MX3_PWMCR_DBGEN			(1 << 22)
-#define MX3_PWMCR_CLKSRC_IPG_HIGH (2 << 16)
-#define MX3_PWMCR_CLKSRC_IPG      (1 << 16)
-#define MX3_PWMCR_EN              (1 << 0)
+#define MX3_PWMCR_CLKSRC_IPG_HIGH	(2 << 16)
+#define MX3_PWMCR_CLKSRC_IPG		(1 << 16)
+#define MX3_PWMCR_SWR			(1 << 3)
+#define MX3_PWMCR_EN			(1 << 0)
+#define MX3_PWMSR_FIFOAV_4WORDS		0x4
+#define MX3_PWMSR_FIFOAV_MASK		0x7
+
+#define MX3_PWM_SWR_LOOP		5
 
 struct imx_chip {
 	struct clk	*clk_per;
@@ -103,9 +110,43 @@ static int imx_pwm_config_v2(struct pwm_chip *chip,
 		struct pwm_device *pwm, int duty_ns, int period_ns)
 {
 	struct imx_chip *imx = to_imx_chip(chip);
+	struct device *dev = chip->dev;
 	unsigned long long c;
 	unsigned long period_cycles, duty_cycles, prescale;
-	u32 cr;
+	unsigned int period_ms;
+	bool enable = test_bit(PWMF_ENABLED, &pwm->flags);
+	int wait_count = 0, fifoav;
+	u32 cr, sr;
+
+	/*
+	 * i.MX PWMv2 has a 4-word sample FIFO.
+	 * In order to avoid FIFO overflow issue, we do software reset
+	 * to clear all sample FIFO if the controller is disabled or
+	 * wait for a full PWM cycle to get a relinquished FIFO slot
+	 * when the controller is enabled and the FIFO is fully loaded.
+	 */
+	if (enable) {
+		sr = readl(imx->mmio_base + MX3_PWMSR);
+		fifoav = sr & MX3_PWMSR_FIFOAV_MASK;
+		if (fifoav == MX3_PWMSR_FIFOAV_4WORDS) {
+			period_ms = DIV_ROUND_UP(pwm->period, NSEC_PER_MSEC);
+			msleep(period_ms);
+
+			sr = readl(imx->mmio_base + MX3_PWMSR);
+			if (fifoav == (sr & MX3_PWMSR_FIFOAV_MASK))
+				dev_warn(dev, "there is no free FIFO slot\n");
+		}
+	} else {
+		writel(MX3_PWMCR_SWR, imx->mmio_base + MX3_PWMCR);
+		do {
+			usleep_range(200, 1000);
+			cr = readl(imx->mmio_base + MX3_PWMCR);
+		} while ((cr & MX3_PWMCR_SWR) &&
+			 (wait_count++ < MX3_PWM_SWR_LOOP));
+
+		if (cr & MX3_PWMCR_SWR)
+			dev_warn(dev, "software reset timeout\n");
+	}
 
 	c = clk_get_rate(imx->clk_per);
 	c = c * period_ns;
@@ -135,7 +176,7 @@ static int imx_pwm_config_v2(struct pwm_chip *chip,
 		MX3_PWMCR_DOZEEN | MX3_PWMCR_WAITEN |
 		MX3_PWMCR_DBGEN | MX3_PWMCR_CLKSRC_IPG_HIGH;
 
-	if (test_bit(PWMF_ENABLED, &pwm->flags))
+	if (enable)
 		cr |= MX3_PWMCR_EN;
 
 	writel(cr, imx->mmio_base + MX3_PWMCR);
diff --git a/drivers/pwm/pwm-lpss-pci.c b/drivers/pwm/pwm-lpss-pci.c
new file mode 100644
index 000000000000..cf20d2beacdd
--- /dev/null
+++ b/drivers/pwm/pwm-lpss-pci.c
@@ -0,0 +1,64 @@
+/*
+ * Intel Low Power Subsystem PWM controller PCI driver
+ *
+ * Copyright (C) 2014, Intel Corporation
+ *
+ * Derived from the original pwm-lpss.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+
+#include "pwm-lpss.h"
+
+static int pwm_lpss_probe_pci(struct pci_dev *pdev,
+			      const struct pci_device_id *id)
+{
+	const struct pwm_lpss_boardinfo *info;
+	struct pwm_lpss_chip *lpwm;
+	int err;
+
+	err = pcim_enable_device(pdev);
+	if (err < 0)
+		return err;
+
+	info = (struct pwm_lpss_boardinfo *)id->driver_data;
+	lpwm = pwm_lpss_probe(&pdev->dev, &pdev->resource[0], info);
+	if (IS_ERR(lpwm))
+		return PTR_ERR(lpwm);
+
+	pci_set_drvdata(pdev, lpwm);
+	return 0;
+}
+
+static void pwm_lpss_remove_pci(struct pci_dev *pdev)
+{
+	struct pwm_lpss_chip *lpwm = pci_get_drvdata(pdev);
+
+	pwm_lpss_remove(lpwm);
+}
+
+static const struct pci_device_id pwm_lpss_pci_ids[] = {
+	{ PCI_VDEVICE(INTEL, 0x0f08), (unsigned long)&pwm_lpss_byt_info},
+	{ PCI_VDEVICE(INTEL, 0x0f09), (unsigned long)&pwm_lpss_byt_info},
+	{ PCI_VDEVICE(INTEL, 0x2288), (unsigned long)&pwm_lpss_bsw_info},
+	{ PCI_VDEVICE(INTEL, 0x2289), (unsigned long)&pwm_lpss_bsw_info},
+	{ },
+};
+MODULE_DEVICE_TABLE(pci, pwm_lpss_pci_ids);
+
+static struct pci_driver pwm_lpss_driver_pci = {
+	.name = "pwm-lpss",
+	.id_table = pwm_lpss_pci_ids,
+	.probe = pwm_lpss_probe_pci,
+	.remove = pwm_lpss_remove_pci,
+};
+module_pci_driver(pwm_lpss_driver_pci);
+
+MODULE_DESCRIPTION("PWM PCI driver for Intel LPSS");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/pwm/pwm-lpss-platform.c b/drivers/pwm/pwm-lpss-platform.c
new file mode 100644
index 000000000000..18a9c880a76d
--- /dev/null
+++ b/drivers/pwm/pwm-lpss-platform.c
@@ -0,0 +1,68 @@
+/*
+ * Intel Low Power Subsystem PWM controller driver
+ *
+ * Copyright (C) 2014, Intel Corporation
+ *
+ * Derived from the original pwm-lpss.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/acpi.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+
+#include "pwm-lpss.h"
+
+static int pwm_lpss_probe_platform(struct platform_device *pdev)
+{
+	const struct pwm_lpss_boardinfo *info;
+	const struct acpi_device_id *id;
+	struct pwm_lpss_chip *lpwm;
+	struct resource *r;
+
+	id = acpi_match_device(pdev->dev.driver->acpi_match_table, &pdev->dev);
+	if (!id)
+		return -ENODEV;
+
+	info = (const struct pwm_lpss_boardinfo *)id->driver_data;
+	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+
+	lpwm = pwm_lpss_probe(&pdev->dev, r, info);
+	if (IS_ERR(lpwm))
+		return PTR_ERR(lpwm);
+
+	platform_set_drvdata(pdev, lpwm);
+	return 0;
+}
+
+static int pwm_lpss_remove_platform(struct platform_device *pdev)
+{
+	struct pwm_lpss_chip *lpwm = platform_get_drvdata(pdev);
+
+	return pwm_lpss_remove(lpwm);
+}
+
+static const struct acpi_device_id pwm_lpss_acpi_match[] = {
+	{ "80860F09", (unsigned long)&pwm_lpss_byt_info },
+	{ "80862288", (unsigned long)&pwm_lpss_bsw_info },
+	{ },
+};
+MODULE_DEVICE_TABLE(acpi, pwm_lpss_acpi_match);
+
+static struct platform_driver pwm_lpss_driver_platform = {
+	.driver = {
+		.name = "pwm-lpss",
+		.acpi_match_table = pwm_lpss_acpi_match,
+	},
+	.probe = pwm_lpss_probe_platform,
+	.remove = pwm_lpss_remove_platform,
+};
+module_platform_driver(pwm_lpss_driver_platform);
+
+MODULE_DESCRIPTION("PWM platform driver for Intel LPSS");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:pwm-lpss");
diff --git a/drivers/pwm/pwm-lpss.c b/drivers/pwm/pwm-lpss.c
index 4df994f72d96..e9798253a16f 100644
--- a/drivers/pwm/pwm-lpss.c
+++ b/drivers/pwm/pwm-lpss.c
@@ -13,15 +13,11 @@
  * published by the Free Software Foundation.
  */
 
-#include <linux/acpi.h>
-#include <linux/device.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/pwm.h>
-#include <linux/platform_device.h>
-#include <linux/pci.h>
 
-static int pci_drv, plat_drv;	/* So we know which drivers registered */
+#include "pwm-lpss.h"
 
 #define PWM				0x00000000
 #define PWM_ENABLE			BIT(31)
@@ -39,14 +35,17 @@ struct pwm_lpss_chip {
 	unsigned long clk_rate;
 };
 
-struct pwm_lpss_boardinfo {
-	unsigned long clk_rate;
+/* BayTrail */
+const struct pwm_lpss_boardinfo pwm_lpss_byt_info = {
+	.clk_rate = 25000000
 };
+EXPORT_SYMBOL_GPL(pwm_lpss_byt_info);
 
-/* BayTrail */
-static const struct pwm_lpss_boardinfo byt_info = {
-	25000000
+/* Braswell */
+const struct pwm_lpss_boardinfo pwm_lpss_bsw_info = {
+	.clk_rate = 19200000
 };
+EXPORT_SYMBOL_GPL(pwm_lpss_bsw_info);
 
 static inline struct pwm_lpss_chip *to_lpwm(struct pwm_chip *chip)
 {
@@ -118,9 +117,8 @@ static const struct pwm_ops pwm_lpss_ops = {
 	.owner = THIS_MODULE,
 };
 
-static struct pwm_lpss_chip *pwm_lpss_probe(struct device *dev,
-					    struct resource *r,
-					    const struct pwm_lpss_boardinfo *info)
+struct pwm_lpss_chip *pwm_lpss_probe(struct device *dev, struct resource *r,
+				     const struct pwm_lpss_boardinfo *info)
 {
 	struct pwm_lpss_chip *lpwm;
 	int ret;
@@ -147,8 +145,9 @@ static struct pwm_lpss_chip *pwm_lpss_probe(struct device *dev,
 
 	return lpwm;
 }
+EXPORT_SYMBOL_GPL(pwm_lpss_probe);
 
-static int pwm_lpss_remove(struct pwm_lpss_chip *lpwm)
+int pwm_lpss_remove(struct pwm_lpss_chip *lpwm)
 {
 	u32 ctrl;
 
@@ -157,114 +156,8 @@ static int pwm_lpss_remove(struct pwm_lpss_chip *lpwm)
 
 	return pwmchip_remove(&lpwm->chip);
 }
-
-static int pwm_lpss_probe_pci(struct pci_dev *pdev,
-			      const struct pci_device_id *id)
-{
-	const struct pwm_lpss_boardinfo *info;
-	struct pwm_lpss_chip *lpwm;
-	int err;
-
-	err = pci_enable_device(pdev);
-	if (err < 0)
-		return err;
-
-	info = (struct pwm_lpss_boardinfo *)id->driver_data;
-	lpwm = pwm_lpss_probe(&pdev->dev, &pdev->resource[0], info);
-	if (IS_ERR(lpwm))
-		return PTR_ERR(lpwm);
-
-	pci_set_drvdata(pdev, lpwm);
-	return 0;
-}
-
-static void pwm_lpss_remove_pci(struct pci_dev *pdev)
-{
-	struct pwm_lpss_chip *lpwm = pci_get_drvdata(pdev);
-
-	pwm_lpss_remove(lpwm);
-	pci_disable_device(pdev);
-}
-
-static struct pci_device_id pwm_lpss_pci_ids[] = {
-	{ PCI_VDEVICE(INTEL, 0x0f08), (unsigned long)&byt_info},
-	{ PCI_VDEVICE(INTEL, 0x0f09), (unsigned long)&byt_info},
-	{ },
-};
-MODULE_DEVICE_TABLE(pci, pwm_lpss_pci_ids);
-
-static struct pci_driver pwm_lpss_driver_pci = {
-	.name = "pwm-lpss",
-	.id_table = pwm_lpss_pci_ids,
-	.probe = pwm_lpss_probe_pci,
-	.remove = pwm_lpss_remove_pci,
-};
-
-static int pwm_lpss_probe_platform(struct platform_device *pdev)
-{
-	const struct pwm_lpss_boardinfo *info;
-	const struct acpi_device_id *id;
-	struct pwm_lpss_chip *lpwm;
-	struct resource *r;
-
-	id = acpi_match_device(pdev->dev.driver->acpi_match_table, &pdev->dev);
-	if (!id)
-		return -ENODEV;
-
-	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-
-	info = (struct pwm_lpss_boardinfo *)id->driver_data;
-	lpwm = pwm_lpss_probe(&pdev->dev, r, info);
-	if (IS_ERR(lpwm))
-		return PTR_ERR(lpwm);
-
-	platform_set_drvdata(pdev, lpwm);
-	return 0;
-}
-
-static int pwm_lpss_remove_platform(struct platform_device *pdev)
-{
-	struct pwm_lpss_chip *lpwm = platform_get_drvdata(pdev);
-
-	return pwm_lpss_remove(lpwm);
-}
-
-static const struct acpi_device_id pwm_lpss_acpi_match[] = {
-	{ "80860F09", (unsigned long)&byt_info },
-	{ },
-};
-MODULE_DEVICE_TABLE(acpi, pwm_lpss_acpi_match);
-
-static struct platform_driver pwm_lpss_driver_platform = {
-	.driver = {
-		.name = "pwm-lpss",
-		.acpi_match_table = pwm_lpss_acpi_match,
-	},
-	.probe = pwm_lpss_probe_platform,
-	.remove = pwm_lpss_remove_platform,
-};
-
-static int __init pwm_init(void)
-{
-	pci_drv = pci_register_driver(&pwm_lpss_driver_pci);
-	plat_drv = platform_driver_register(&pwm_lpss_driver_platform);
-	if (pci_drv && plat_drv)
-		return pci_drv;
-
-	return 0;
-}
-module_init(pwm_init);
-
-static void __exit pwm_exit(void)
-{
-	if (!pci_drv)
-		pci_unregister_driver(&pwm_lpss_driver_pci);
-	if (!plat_drv)
-		platform_driver_unregister(&pwm_lpss_driver_platform);
-}
-module_exit(pwm_exit);
+EXPORT_SYMBOL_GPL(pwm_lpss_remove);
 
 MODULE_DESCRIPTION("PWM driver for Intel LPSS");
 MODULE_AUTHOR("Mika Westerberg <mika.westerberg@linux.intel.com>");
 MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("platform:pwm-lpss");
diff --git a/drivers/pwm/pwm-lpss.h b/drivers/pwm/pwm-lpss.h
new file mode 100644
index 000000000000..aa041bb1b67d
--- /dev/null
+++ b/drivers/pwm/pwm-lpss.h
@@ -0,0 +1,32 @@
+/*
+ * Intel Low Power Subsystem PWM controller driver
+ *
+ * Copyright (C) 2014, Intel Corporation
+ *
+ * Derived from the original pwm-lpss.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __PWM_LPSS_H
+#define __PWM_LPSS_H
+
+#include <linux/device.h>
+#include <linux/pwm.h>
+
+struct pwm_lpss_chip;
+
+struct pwm_lpss_boardinfo {
+	unsigned long clk_rate;
+};
+
+extern const struct pwm_lpss_boardinfo pwm_lpss_byt_info;
+extern const struct pwm_lpss_boardinfo pwm_lpss_bsw_info;
+
+struct pwm_lpss_chip *pwm_lpss_probe(struct device *dev, struct resource *r,
+				     const struct pwm_lpss_boardinfo *info);
+int pwm_lpss_remove(struct pwm_lpss_chip *lpwm);
+
+#endif	/* __PWM_LPSS_H */
diff --git a/drivers/pwm/pwm-rockchip.c b/drivers/pwm/pwm-rockchip.c
index bdd8644c01cf..9442df244101 100644
--- a/drivers/pwm/pwm-rockchip.c
+++ b/drivers/pwm/pwm-rockchip.c
@@ -24,7 +24,9 @@
 #define PWM_ENABLE		(1 << 0)
 #define PWM_CONTINUOUS		(1 << 1)
 #define PWM_DUTY_POSITIVE	(1 << 3)
+#define PWM_DUTY_NEGATIVE	(0 << 3)
 #define PWM_INACTIVE_NEGATIVE	(0 << 4)
+#define PWM_INACTIVE_POSITIVE	(1 << 4)
 #define PWM_OUTPUT_LEFT		(0 << 5)
 #define PWM_LP_DISABLE		(0 << 8)
 
@@ -45,8 +47,10 @@ struct rockchip_pwm_regs {
 struct rockchip_pwm_data {
 	struct rockchip_pwm_regs regs;
 	unsigned int prescaler;
+	const struct pwm_ops *ops;
 
-	void (*set_enable)(struct pwm_chip *chip, bool enable);
+	void (*set_enable)(struct pwm_chip *chip,
+			   struct pwm_device *pwm, bool enable);
 };
 
 static inline struct rockchip_pwm_chip *to_rockchip_pwm_chip(struct pwm_chip *c)
@@ -54,7 +58,8 @@ static inline struct rockchip_pwm_chip *to_rockchip_pwm_chip(struct pwm_chip *c)
 	return container_of(c, struct rockchip_pwm_chip, chip);
 }
 
-static void rockchip_pwm_set_enable_v1(struct pwm_chip *chip, bool enable)
+static void rockchip_pwm_set_enable_v1(struct pwm_chip *chip,
+				       struct pwm_device *pwm, bool enable)
 {
 	struct rockchip_pwm_chip *pc = to_rockchip_pwm_chip(chip);
 	u32 enable_conf = PWM_CTRL_OUTPUT_EN | PWM_CTRL_TIMER_EN;
@@ -70,14 +75,19 @@ static void rockchip_pwm_set_enable_v1(struct pwm_chip *chip, bool enable)
 	writel_relaxed(val, pc->base + pc->data->regs.ctrl);
 }
 
-static void rockchip_pwm_set_enable_v2(struct pwm_chip *chip, bool enable)
+static void rockchip_pwm_set_enable_v2(struct pwm_chip *chip,
+				       struct pwm_device *pwm, bool enable)
 {
 	struct rockchip_pwm_chip *pc = to_rockchip_pwm_chip(chip);
 	u32 enable_conf = PWM_OUTPUT_LEFT | PWM_LP_DISABLE | PWM_ENABLE |
-			  PWM_CONTINUOUS | PWM_DUTY_POSITIVE |
-			  PWM_INACTIVE_NEGATIVE;
+			  PWM_CONTINUOUS;
 	u32 val;
 
+	if (pwm->polarity == PWM_POLARITY_INVERSED)
+		enable_conf |= PWM_DUTY_NEGATIVE | PWM_INACTIVE_POSITIVE;
+	else
+		enable_conf |= PWM_DUTY_POSITIVE | PWM_INACTIVE_NEGATIVE;
+
 	val = readl_relaxed(pc->base + pc->data->regs.ctrl);
 
 	if (enable)
@@ -124,6 +134,19 @@ static int rockchip_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
 	return 0;
 }
 
+static int rockchip_pwm_set_polarity(struct pwm_chip *chip,
+				     struct pwm_device *pwm,
+				     enum pwm_polarity polarity)
+{
+	/*
+	 * No action needed here because pwm->polarity will be set by the core
+	 * and the core will only change polarity when the PWM is not enabled.
+	 * We'll handle things in set_enable().
+	 */
+
+	return 0;
+}
+
 static int rockchip_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct rockchip_pwm_chip *pc = to_rockchip_pwm_chip(chip);
@@ -133,7 +156,7 @@ static int rockchip_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
 	if (ret)
 		return ret;
 
-	pc->data->set_enable(chip, true);
+	pc->data->set_enable(chip, pwm, true);
 
 	return 0;
 }
@@ -142,18 +165,26 @@ static void rockchip_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 {
 	struct rockchip_pwm_chip *pc = to_rockchip_pwm_chip(chip);
 
-	pc->data->set_enable(chip, false);
+	pc->data->set_enable(chip, pwm, false);
 
 	clk_disable(pc->clk);
 }
 
-static const struct pwm_ops rockchip_pwm_ops = {
+static const struct pwm_ops rockchip_pwm_ops_v1 = {
 	.config = rockchip_pwm_config,
 	.enable = rockchip_pwm_enable,
 	.disable = rockchip_pwm_disable,
 	.owner = THIS_MODULE,
 };
 
+static const struct pwm_ops rockchip_pwm_ops_v2 = {
+	.config = rockchip_pwm_config,
+	.set_polarity = rockchip_pwm_set_polarity,
+	.enable = rockchip_pwm_enable,
+	.disable = rockchip_pwm_disable,
+	.owner = THIS_MODULE,
+};
+
 static const struct rockchip_pwm_data pwm_data_v1 = {
 	.regs = {
 		.duty = 0x04,
@@ -162,6 +193,7 @@ static const struct rockchip_pwm_data pwm_data_v1 = {
 		.ctrl = 0x0c,
 	},
 	.prescaler = 2,
+	.ops = &rockchip_pwm_ops_v1,
 	.set_enable = rockchip_pwm_set_enable_v1,
 };
 
@@ -173,6 +205,7 @@ static const struct rockchip_pwm_data pwm_data_v2 = {
 		.ctrl = 0x0c,
 	},
 	.prescaler = 1,
+	.ops = &rockchip_pwm_ops_v2,
 	.set_enable = rockchip_pwm_set_enable_v2,
 };
 
@@ -184,6 +217,7 @@ static const struct rockchip_pwm_data pwm_data_vop = {
 		.ctrl = 0x00,
 	},
 	.prescaler = 1,
+	.ops = &rockchip_pwm_ops_v2,
 	.set_enable = rockchip_pwm_set_enable_v2,
 };
 
@@ -227,10 +261,15 @@ static int rockchip_pwm_probe(struct platform_device *pdev)
 
 	pc->data = id->data;
 	pc->chip.dev = &pdev->dev;
-	pc->chip.ops = &rockchip_pwm_ops;
+	pc->chip.ops = pc->data->ops;
 	pc->chip.base = -1;
 	pc->chip.npwm = 1;
 
+	if (pc->data->ops->set_polarity) {
+		pc->chip.of_xlate = of_pwm_xlate_with_flags;
+		pc->chip.of_pwm_n_cells = 3;
+	}
+
 	ret = pwmchip_add(&pc->chip);
 	if (ret < 0) {
 		clk_unprepare(pc->clk);
diff --git a/drivers/regulator/88pm8607.c b/drivers/regulator/88pm8607.c
index 337634ad0562..6d77dcd7dcf6 100644
--- a/drivers/regulator/88pm8607.c
+++ b/drivers/regulator/88pm8607.c
@@ -319,7 +319,7 @@ static int pm8607_regulator_dt_init(struct platform_device *pdev,
 				    struct regulator_config *config)
 {
 	struct device_node *nproot, *np;
-	nproot = of_node_get(pdev->dev.parent->of_node);
+	nproot = pdev->dev.parent->of_node;
 	if (!nproot)
 		return -ENODEV;
 	nproot = of_get_child_by_name(nproot, "regulators");
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index 2dc8289e5dba..55d7b7b0f2e0 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -199,13 +199,14 @@ config REGULATOR_DA9210
 	  interface.
 
 config REGULATOR_DA9211
-	tristate "Dialog Semiconductor DA9211/DA9212 regulator"
+	tristate "Dialog Semiconductor DA9211/DA9212/DA9213/DA9214 regulator"
 	depends on I2C
 	select REGMAP_I2C
 	help
-	  Say y here to support for the Dialog Semiconductor DA9211/DA9212.
-	  The DA9211/DA9212 is a multi-phase synchronous step down
-	  converter 12A DC-DC Buck controlled through an I2C
+	  Say y here to support for the Dialog Semiconductor DA9211/DA9212
+	  /DA9213/DA9214.
+	  The DA9211/DA9212/DA9213/DA9214 is a multi-phase synchronous
+	  step down converter 12A or 16A DC-DC Buck controlled through an I2C
 	  interface.
 
 config REGULATOR_DBX500_PRCMU
@@ -240,6 +241,23 @@ config REGULATOR_GPIO
 	  and the platform has to provide a mapping of GPIO-states
 	  to target volts/amps.
 
+config REGULATOR_HI6421
+	tristate "HiSilicon Hi6421 PMIC voltage regulator support"
+	depends on MFD_HI6421_PMIC && OF
+	help
+	  This driver provides support for the voltage regulators on the
+	  HiSilicon Hi6421 PMU / Codec IC.
+	  Hi6421 is a multi-function device which, on regulator part, provides
+	  21 general purpose LDOs, 3 dedicated LDOs, and 5 BUCKs. All
+	  of them come with support to either ECO (idle) or sleep mode.
+
+config REGULATOR_ISL9305
+	tristate "Intersil ISL9305 regulator"
+	depends on I2C
+	select REGMAP_I2C
+	help
+	  This driver supports ISL9305 voltage regulator chip.
+
 config REGULATOR_ISL6271A
 	tristate "Intersil ISL6271A Power regulator"
 	depends on I2C
@@ -387,6 +405,15 @@ config REGULATOR_MAX77693
 	  and one current regulator 'CHARGER'. This is suitable for
 	  Exynos-4x12 chips.
 
+config REGULATOR_MAX77802
+	tristate "Maxim 77802 regulator"
+	depends on MFD_MAX77686
+	help
+	  This driver controls a Maxim 77802 regulator
+	  via I2C bus. The provided regulator is suitable for
+	  Exynos5420/Exynos5800 SoCs to control various voltages.
+	  It includes support for control of voltage and ramp speed.
+
 config REGULATOR_MC13XXX_CORE
 	tristate
 
@@ -449,6 +476,25 @@ config REGULATOR_PFUZE100
 	  Say y here to support the regulators found on the Freescale
 	  PFUZE100/PFUZE200 PMIC.
 
+config REGULATOR_PWM
+	tristate "PWM voltage regulator"
+	depends on PWM
+	help
+	  This driver supports PWM controlled voltage regulators. PWM
+	  duty cycle can increase or decrease the voltage.
+
+config REGULATOR_QCOM_RPM
+	tristate "Qualcomm RPM regulator driver"
+	depends on MFD_QCOM_RPM
+	help
+	  If you say yes to this option, support will be included for the
+	  regulators exposed by the Resource Power Manager found in Qualcomm
+	  8660, 8960 and 8064 based devices.
+
+	  Say M here if you want to include support for the regulators on the
+	  Qualcomm RPM as a module. The module will be named
+	  "qcom_rpm-regulator".
+
 config REGULATOR_RC5T583
 	tristate "RICOH RC5T583 Power regulators"
 	depends on MFD_RC5T583
@@ -459,6 +505,22 @@ config REGULATOR_RC5T583
 	  through regulator interface. The device supports multiple DCDC/LDO
 	  outputs which can be controlled by i2c communication.
 
+config REGULATOR_RK808
+	tristate "Rockchip RK808 Power regulators"
+	depends on MFD_RK808
+	help
+	  Select this option to enable the power regulator of ROCKCHIP
+	  PMIC RK808.
+	  This driver supports the control of different power rails of device
+	  through regulator interface. The device supports multiple DCDC/LDO
+	  outputs which can be controlled by i2c communication.
+
+config REGULATOR_RN5T618
+	tristate "Ricoh RN5T618 voltage regulators"
+	depends on MFD_RN5T618
+	help
+	  Say y here to support the regulators found on Ricoh RN5T618 PMIC.
+
 config REGULATOR_S2MPA01
 	tristate "Samsung S2MPA01 voltage regulator"
 	depends on MFD_SEC_CORE
@@ -483,11 +545,16 @@ config REGULATOR_S5M8767
 	 via I2C bus. S5M8767A have 9 Bucks and 28 LDOs output and
 	 supports DVS mode with 8bits of output voltage control.
 
-config REGULATOR_ST_PWM
-	tristate "STMicroelectronics PWM voltage regulator"
-	depends on ARCH_STI
+config REGULATOR_SKY81452
+	tristate "Skyworks Solutions SKY81452 voltage regulator"
+	depends on SKY81452
 	help
-	 This driver supports ST's PWM controlled voltage regulators.
+	  This driver supports Skyworks SKY81452 voltage output regulator
+	  via I2C bus. SKY81452 has one voltage linear regulator can be
+	  programmed from 4.5V to 20V.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called sky81452-regulator.
 
 config REGULATOR_TI_ABB
 	tristate "TI Adaptive Body Bias on-chip LDO"
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index aa4a6aa7b558..1029ed39c512 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -32,7 +32,9 @@ obj-$(CONFIG_REGULATOR_DBX500_PRCMU) += dbx500-prcmu.o
 obj-$(CONFIG_REGULATOR_DB8500_PRCMU) += db8500-prcmu.o
 obj-$(CONFIG_REGULATOR_FAN53555) += fan53555.o
 obj-$(CONFIG_REGULATOR_GPIO) += gpio-regulator.o
+obj-$(CONFIG_REGULATOR_HI6421) += hi6421-regulator.o
 obj-$(CONFIG_REGULATOR_ISL6271A) += isl6271a-regulator.o
+obj-$(CONFIG_REGULATOR_ISL9305) += isl9305.o
 obj-$(CONFIG_REGULATOR_LP3971) += lp3971.o
 obj-$(CONFIG_REGULATOR_LP3972) += lp3972.o
 obj-$(CONFIG_REGULATOR_LP872X) += lp872x.o
@@ -52,20 +54,25 @@ obj-$(CONFIG_REGULATOR_MAX8997) += max8997.o
 obj-$(CONFIG_REGULATOR_MAX8998) += max8998.o
 obj-$(CONFIG_REGULATOR_MAX77686) += max77686.o
 obj-$(CONFIG_REGULATOR_MAX77693) += max77693.o
+obj-$(CONFIG_REGULATOR_MAX77802) += max77802.o
 obj-$(CONFIG_REGULATOR_MC13783) += mc13783-regulator.o
 obj-$(CONFIG_REGULATOR_MC13892) += mc13892-regulator.o
 obj-$(CONFIG_REGULATOR_MC13XXX_CORE) +=  mc13xxx-regulator-core.o
+obj-$(CONFIG_REGULATOR_QCOM_RPM) += qcom_rpm-regulator.o
 obj-$(CONFIG_REGULATOR_PALMAS) += palmas-regulator.o
 obj-$(CONFIG_REGULATOR_PFUZE100) += pfuze100-regulator.o
+obj-$(CONFIG_REGULATOR_PWM) += pwm-regulator.o
 obj-$(CONFIG_REGULATOR_TPS51632) += tps51632-regulator.o
 obj-$(CONFIG_REGULATOR_PBIAS) += pbias-regulator.o
 obj-$(CONFIG_REGULATOR_PCAP) += pcap-regulator.o
 obj-$(CONFIG_REGULATOR_PCF50633) += pcf50633-regulator.o
 obj-$(CONFIG_REGULATOR_RC5T583)  += rc5t583-regulator.o
+obj-$(CONFIG_REGULATOR_RK808)   += rk808-regulator.o
+obj-$(CONFIG_REGULATOR_RN5T618) += rn5t618-regulator.o
 obj-$(CONFIG_REGULATOR_S2MPA01) += s2mpa01.o
 obj-$(CONFIG_REGULATOR_S2MPS11) += s2mps11.o
 obj-$(CONFIG_REGULATOR_S5M8767) += s5m8767.o
-obj-$(CONFIG_REGULATOR_ST_PWM) += st-pwm.o
+obj-$(CONFIG_REGULATOR_SKY81452) += sky81452-regulator.o
 obj-$(CONFIG_REGULATOR_STW481X_VMMC) += stw481x-vmmc.o
 obj-$(CONFIG_REGULATOR_TI_ABB) += ti-abb-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6105X) += tps6105x-regulator.o
diff --git a/drivers/regulator/as3711-regulator.c b/drivers/regulator/as3711-regulator.c
index b47283f91e2d..8459b0b648cd 100644
--- a/drivers/regulator/as3711-regulator.c
+++ b/drivers/regulator/as3711-regulator.c
@@ -22,12 +22,10 @@
 
 struct as3711_regulator_info {
 	struct regulator_desc	desc;
-	unsigned int		max_uV;
 };
 
 struct as3711_regulator {
 	struct as3711_regulator_info *reg_info;
-	struct regulator_dev *rdev;
 };
 
 /*
@@ -132,39 +130,37 @@ static const struct regulator_linear_range as3711_dldo_ranges[] = {
 	REGULATOR_LINEAR_RANGE(1750000, 0x20, 0x3f, 50000),
 };
 
-#define AS3711_REG(_id, _en_reg, _en_bit, _vmask, _vshift, _min_uV, _max_uV, _sfx)	\
-	[AS3711_REGULATOR_ ## _id] = {							\
-	.desc = {									\
-		.name = "as3711-regulator-" # _id,					\
-		.id = AS3711_REGULATOR_ ## _id,						\
-		.n_voltages = (_vmask + 1),						\
-		.ops = &as3711_ ## _sfx ## _ops,					\
-		.type = REGULATOR_VOLTAGE,						\
-		.owner = THIS_MODULE,							\
-		.vsel_reg = AS3711_ ## _id ## _VOLTAGE,					\
-		.vsel_mask = _vmask << _vshift,						\
-		.enable_reg = AS3711_ ## _en_reg,					\
-		.enable_mask = BIT(_en_bit),						\
-		.min_uV	= _min_uV,							\
-		.linear_ranges = as3711_ ## _sfx ## _ranges,				\
-		.n_linear_ranges = ARRAY_SIZE(as3711_ ## _sfx ## _ranges),		\
-	},										\
-	.max_uV = _max_uV,								\
+#define AS3711_REG(_id, _en_reg, _en_bit, _vmask, _sfx)			   \
+	[AS3711_REGULATOR_ ## _id] = {					   \
+	.desc = {							   \
+		.name = "as3711-regulator-" # _id,			   \
+		.id = AS3711_REGULATOR_ ## _id,				   \
+		.n_voltages = (_vmask + 1),				   \
+		.ops = &as3711_ ## _sfx ## _ops,			   \
+		.type = REGULATOR_VOLTAGE,				   \
+		.owner = THIS_MODULE,					   \
+		.vsel_reg = AS3711_ ## _id ## _VOLTAGE,			   \
+		.vsel_mask = _vmask,					   \
+		.enable_reg = AS3711_ ## _en_reg,			   \
+		.enable_mask = BIT(_en_bit),				   \
+		.linear_ranges = as3711_ ## _sfx ## _ranges,		   \
+		.n_linear_ranges = ARRAY_SIZE(as3711_ ## _sfx ## _ranges), \
+	},								   \
 }
 
 static struct as3711_regulator_info as3711_reg_info[] = {
-	AS3711_REG(SD_1, SD_CONTROL, 0, 0x7f, 0, 612500, 3350000, sd),
-	AS3711_REG(SD_2, SD_CONTROL, 1, 0x7f, 0, 612500, 3350000, sd),
-	AS3711_REG(SD_3, SD_CONTROL, 2, 0x7f, 0, 612500, 3350000, sd),
-	AS3711_REG(SD_4, SD_CONTROL, 3, 0x7f, 0, 612500, 3350000, sd),
-	AS3711_REG(LDO_1, LDO_1_VOLTAGE, 7, 0x1f, 0, 1200000, 3300000, aldo),
-	AS3711_REG(LDO_2, LDO_2_VOLTAGE, 7, 0x1f, 0, 1200000, 3300000, aldo),
-	AS3711_REG(LDO_3, LDO_3_VOLTAGE, 7, 0x3f, 0, 900000, 3300000, dldo),
-	AS3711_REG(LDO_4, LDO_4_VOLTAGE, 7, 0x3f, 0, 900000, 3300000, dldo),
-	AS3711_REG(LDO_5, LDO_5_VOLTAGE, 7, 0x3f, 0, 900000, 3300000, dldo),
-	AS3711_REG(LDO_6, LDO_6_VOLTAGE, 7, 0x3f, 0, 900000, 3300000, dldo),
-	AS3711_REG(LDO_7, LDO_7_VOLTAGE, 7, 0x3f, 0, 900000, 3300000, dldo),
-	AS3711_REG(LDO_8, LDO_8_VOLTAGE, 7, 0x3f, 0, 900000, 3300000, dldo),
+	AS3711_REG(SD_1, SD_CONTROL, 0, 0x7f, sd),
+	AS3711_REG(SD_2, SD_CONTROL, 1, 0x7f, sd),
+	AS3711_REG(SD_3, SD_CONTROL, 2, 0x7f, sd),
+	AS3711_REG(SD_4, SD_CONTROL, 3, 0x7f, sd),
+	AS3711_REG(LDO_1, LDO_1_VOLTAGE, 7, 0x1f, aldo),
+	AS3711_REG(LDO_2, LDO_2_VOLTAGE, 7, 0x1f, aldo),
+	AS3711_REG(LDO_3, LDO_3_VOLTAGE, 7, 0x3f, dldo),
+	AS3711_REG(LDO_4, LDO_4_VOLTAGE, 7, 0x3f, dldo),
+	AS3711_REG(LDO_5, LDO_5_VOLTAGE, 7, 0x3f, dldo),
+	AS3711_REG(LDO_6, LDO_6_VOLTAGE, 7, 0x3f, dldo),
+	AS3711_REG(LDO_7, LDO_7_VOLTAGE, 7, 0x3f, dldo),
+	AS3711_REG(LDO_8, LDO_8_VOLTAGE, 7, 0x3f, dldo),
 	/* StepUp output voltage depends on supplying regulator */
 };
 
@@ -263,7 +259,6 @@ static int as3711_regulator_probe(struct platform_device *pdev)
 				ri->desc.name);
 			return PTR_ERR(rdev);
 		}
-		reg->rdev = rdev;
 	}
 	platform_set_drvdata(pdev, regs);
 	return 0;
diff --git a/drivers/regulator/axp20x-regulator.c b/drivers/regulator/axp20x-regulator.c
index 004aadb7bcc1..2e1010a34ddc 100644
--- a/drivers/regulator/axp20x-regulator.c
+++ b/drivers/regulator/axp20x-regulator.c
@@ -245,7 +245,7 @@ static int axp20x_regulator_probe(struct platform_device *pdev)
 	for (i = 0; i < AXP20X_REG_ID_MAX; i++) {
 		init_data = axp20x_matches[i].init_data;
 
-		config.dev = &pdev->dev;
+		config.dev = pdev->dev.parent;
 		config.init_data = init_data;
 		config.regmap = axp20x->regmap;
 		config.of_node = axp20x_matches[i].of_node;
diff --git a/drivers/regulator/bcm590xx-regulator.c b/drivers/regulator/bcm590xx-regulator.c
index 5d1fd6f3d10a..fe6ac69549a6 100644
--- a/drivers/regulator/bcm590xx-regulator.c
+++ b/drivers/regulator/bcm590xx-regulator.c
@@ -202,7 +202,6 @@ static struct bcm590xx_info bcm590xx_regs[] = {
 struct bcm590xx_reg {
 	struct regulator_desc *desc;
 	struct bcm590xx *mfd;
-	struct bcm590xx_info **info;
 };
 
 static int bcm590xx_get_vsel_register(int id)
@@ -389,11 +388,6 @@ static int bcm590xx_probe(struct platform_device *pdev)
 	if (!pmu->desc)
 		return -ENOMEM;
 
-	pmu->info = devm_kzalloc(&pdev->dev, BCM590XX_NUM_REGS *
-			sizeof(struct bcm590xx_info *), GFP_KERNEL);
-	if (!pmu->info)
-		return -ENOMEM;
-
 	info = bcm590xx_regs;
 
 	for (i = 0; i < BCM590XX_NUM_REGS; i++, info++) {
@@ -403,8 +397,6 @@ static int bcm590xx_probe(struct platform_device *pdev)
 			reg_data = NULL;
 
 		/* Register the regulators */
-		pmu->info[i] = info;
-
 		pmu->desc[i].name = info->name;
 		pmu->desc[i].supply_name = info->vin_name;
 		pmu->desc[i].id = i;
diff --git a/drivers/regulator/core.c b/drivers/regulator/core.c
index a3c3785901f5..cd87c0c37034 100644
--- a/drivers/regulator/core.c
+++ b/drivers/regulator/core.c
@@ -102,7 +102,7 @@ static int _regulator_disable(struct regulator_dev *rdev);
 static int _regulator_get_voltage(struct regulator_dev *rdev);
 static int _regulator_get_current_limit(struct regulator_dev *rdev);
 static unsigned int _regulator_get_mode(struct regulator_dev *rdev);
-static void _notifier_call_chain(struct regulator_dev *rdev,
+static int _notifier_call_chain(struct regulator_dev *rdev,
 				  unsigned long event, void *data);
 static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 				     int min_uV, int max_uV);
@@ -839,7 +839,7 @@ static void print_constraints(struct regulator_dev *rdev)
 static int machine_constraints_voltage(struct regulator_dev *rdev,
 	struct regulation_constraints *constraints)
 {
-	struct regulator_ops *ops = rdev->desc->ops;
+	const struct regulator_ops *ops = rdev->desc->ops;
 	int ret;
 
 	/* do we need to apply the constraint voltage */
@@ -938,7 +938,7 @@ static int machine_constraints_voltage(struct regulator_dev *rdev,
 static int machine_constraints_current(struct regulator_dev *rdev,
 	struct regulation_constraints *constraints)
 {
-	struct regulator_ops *ops = rdev->desc->ops;
+	const struct regulator_ops *ops = rdev->desc->ops;
 	int ret;
 
 	if (!constraints->min_uA && !constraints->max_uA)
@@ -982,7 +982,7 @@ static int set_machine_constraints(struct regulator_dev *rdev,
 	const struct regulation_constraints *constraints)
 {
 	int ret = 0;
-	struct regulator_ops *ops = rdev->desc->ops;
+	const struct regulator_ops *ops = rdev->desc->ops;
 
 	if (constraints)
 		rdev->constraints = kmemdup(constraints, sizeof(*constraints),
@@ -1759,6 +1759,45 @@ static int regulator_ena_gpio_ctrl(struct regulator_dev *rdev, bool enable)
 	return 0;
 }
 
+/**
+ * _regulator_enable_delay - a delay helper function
+ * @delay: time to delay in microseconds
+ *
+ * Delay for the requested amount of time as per the guidelines in:
+ *
+ *     Documentation/timers/timers-howto.txt
+ *
+ * The assumption here is that regulators will never be enabled in
+ * atomic context and therefore sleeping functions can be used.
+ */
+static void _regulator_enable_delay(unsigned int delay)
+{
+	unsigned int ms = delay / 1000;
+	unsigned int us = delay % 1000;
+
+	if (ms > 0) {
+		/*
+		 * For small enough values, handle super-millisecond
+		 * delays in the usleep_range() call below.
+		 */
+		if (ms < 20)
+			us += ms * 1000;
+		else
+			msleep(ms);
+	}
+
+	/*
+	 * Give the scheduler some room to coalesce with any other
+	 * wakeup sources. For delays shorter than 10 us, don't even
+	 * bother setting up high-resolution timers and just busy-
+	 * loop.
+	 */
+	if (us >= 10)
+		usleep_range(us, us + 100);
+	else
+		udelay(us);
+}
+
 static int _regulator_do_enable(struct regulator_dev *rdev)
 {
 	int ret, delay;
@@ -1774,6 +1813,31 @@ static int _regulator_do_enable(struct regulator_dev *rdev)
 
 	trace_regulator_enable(rdev_get_name(rdev));
 
+	if (rdev->desc->off_on_delay) {
+		/* if needed, keep a distance of off_on_delay from last time
+		 * this regulator was disabled.
+		 */
+		unsigned long start_jiffy = jiffies;
+		unsigned long intended, max_delay, remaining;
+
+		max_delay = usecs_to_jiffies(rdev->desc->off_on_delay);
+		intended = rdev->last_off_jiffy + max_delay;
+
+		if (time_before(start_jiffy, intended)) {
+			/* calc remaining jiffies to deal with one-time
+			 * timer wrapping.
+			 * in case of multiple timer wrapping, either it can be
+			 * detected by out-of-range remaining, or it cannot be
+			 * detected and we gets a panelty of
+			 * _regulator_enable_delay().
+			 */
+			remaining = intended - start_jiffy;
+			if (remaining <= max_delay)
+				_regulator_enable_delay(
+						jiffies_to_usecs(remaining));
+		}
+	}
+
 	if (rdev->ena_pin) {
 		ret = regulator_ena_gpio_ctrl(rdev, true);
 		if (ret < 0)
@@ -1792,40 +1856,7 @@ static int _regulator_do_enable(struct regulator_dev *rdev)
 	 * together.  */
 	trace_regulator_enable_delay(rdev_get_name(rdev));
 
-	/*
-	 * Delay for the requested amount of time as per the guidelines in:
-	 *
-	 *     Documentation/timers/timers-howto.txt
-	 *
-	 * The assumption here is that regulators will never be enabled in
-	 * atomic context and therefore sleeping functions can be used.
-	 */
-	if (delay) {
-		unsigned int ms = delay / 1000;
-		unsigned int us = delay % 1000;
-
-		if (ms > 0) {
-			/*
-			 * For small enough values, handle super-millisecond
-			 * delays in the usleep_range() call below.
-			 */
-			if (ms < 20)
-				us += ms * 1000;
-			else
-				msleep(ms);
-		}
-
-		/*
-		 * Give the scheduler some room to coalesce with any other
-		 * wakeup sources. For delays shorter than 10 us, don't even
-		 * bother setting up high-resolution timers and just busy-
-		 * loop.
-		 */
-		if (us >= 10)
-			usleep_range(us, us + 100);
-		else
-			udelay(us);
-	}
+	_regulator_enable_delay(delay);
 
 	trace_regulator_enable_complete(rdev_get_name(rdev));
 
@@ -1919,6 +1950,12 @@ static int _regulator_do_disable(struct regulator_dev *rdev)
 			return ret;
 	}
 
+	/* cares about last_off_jiffy only if off_on_delay is required by
+	 * device.
+	 */
+	if (rdev->desc->off_on_delay)
+		rdev->last_off_jiffy = jiffies;
+
 	trace_regulator_disable_complete(rdev_get_name(rdev));
 
 	return 0;
@@ -2208,9 +2245,9 @@ EXPORT_SYMBOL_GPL(regulator_count_voltages);
  */
 int regulator_list_voltage(struct regulator *regulator, unsigned selector)
 {
-	struct regulator_dev	*rdev = regulator->rdev;
-	struct regulator_ops	*ops = rdev->desc->ops;
-	int			ret;
+	struct regulator_dev *rdev = regulator->rdev;
+	const struct regulator_ops *ops = rdev->desc->ops;
+	int ret;
 
 	if (rdev->desc->fixed_uV && rdev->desc->n_voltages == 1 && !selector)
 		return rdev->desc->fixed_uV;
@@ -2270,8 +2307,8 @@ int regulator_get_hardware_vsel_register(struct regulator *regulator,
 					 unsigned *vsel_reg,
 					 unsigned *vsel_mask)
 {
-	struct regulator_dev	*rdev = regulator->rdev;
-	struct regulator_ops	*ops = rdev->desc->ops;
+	struct regulator_dev *rdev = regulator->rdev;
+	const struct regulator_ops *ops = rdev->desc->ops;
 
 	if (ops->set_voltage_sel != regulator_set_voltage_sel_regmap)
 		return -EOPNOTSUPP;
@@ -2297,8 +2334,8 @@ EXPORT_SYMBOL_GPL(regulator_get_hardware_vsel_register);
 int regulator_list_hardware_vsel(struct regulator *regulator,
 				 unsigned selector)
 {
-	struct regulator_dev	*rdev = regulator->rdev;
-	struct regulator_ops	*ops = rdev->desc->ops;
+	struct regulator_dev *rdev = regulator->rdev;
+	const struct regulator_ops *ops = rdev->desc->ops;
 
 	if (selector >= rdev->desc->n_voltages)
 		return -EINVAL;
@@ -2369,6 +2406,55 @@ int regulator_is_supported_voltage(struct regulator *regulator,
 }
 EXPORT_SYMBOL_GPL(regulator_is_supported_voltage);
 
+static int _regulator_call_set_voltage(struct regulator_dev *rdev,
+				       int min_uV, int max_uV,
+				       unsigned *selector)
+{
+	struct pre_voltage_change_data data;
+	int ret;
+
+	data.old_uV = _regulator_get_voltage(rdev);
+	data.min_uV = min_uV;
+	data.max_uV = max_uV;
+	ret = _notifier_call_chain(rdev, REGULATOR_EVENT_PRE_VOLTAGE_CHANGE,
+				   &data);
+	if (ret & NOTIFY_STOP_MASK)
+		return -EINVAL;
+
+	ret = rdev->desc->ops->set_voltage(rdev, min_uV, max_uV, selector);
+	if (ret >= 0)
+		return ret;
+
+	_notifier_call_chain(rdev, REGULATOR_EVENT_ABORT_VOLTAGE_CHANGE,
+			     (void *)data.old_uV);
+
+	return ret;
+}
+
+static int _regulator_call_set_voltage_sel(struct regulator_dev *rdev,
+					   int uV, unsigned selector)
+{
+	struct pre_voltage_change_data data;
+	int ret;
+
+	data.old_uV = _regulator_get_voltage(rdev);
+	data.min_uV = uV;
+	data.max_uV = uV;
+	ret = _notifier_call_chain(rdev, REGULATOR_EVENT_PRE_VOLTAGE_CHANGE,
+				   &data);
+	if (ret & NOTIFY_STOP_MASK)
+		return -EINVAL;
+
+	ret = rdev->desc->ops->set_voltage_sel(rdev, selector);
+	if (ret >= 0)
+		return ret;
+
+	_notifier_call_chain(rdev, REGULATOR_EVENT_ABORT_VOLTAGE_CHANGE,
+			     (void *)data.old_uV);
+
+	return ret;
+}
+
 static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 				     int min_uV, int max_uV)
 {
@@ -2396,8 +2482,8 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 	}
 
 	if (rdev->desc->ops->set_voltage) {
-		ret = rdev->desc->ops->set_voltage(rdev, min_uV, max_uV,
-						   &selector);
+		ret = _regulator_call_set_voltage(rdev, min_uV, max_uV,
+						  &selector);
 
 		if (ret >= 0) {
 			if (rdev->desc->ops->list_voltage)
@@ -2432,8 +2518,8 @@ static int _regulator_do_set_voltage(struct regulator_dev *rdev,
 				if (old_selector == selector)
 					ret = 0;
 				else
-					ret = rdev->desc->ops->set_voltage_sel(
-								rdev, ret);
+					ret = _regulator_call_set_voltage_sel(
+						rdev, best_val, selector);
 			} else {
 				ret = -EINVAL;
 			}
@@ -2572,8 +2658,8 @@ EXPORT_SYMBOL_GPL(regulator_set_voltage);
 int regulator_set_voltage_time(struct regulator *regulator,
 			       int old_uV, int new_uV)
 {
-	struct regulator_dev	*rdev = regulator->rdev;
-	struct regulator_ops	*ops = rdev->desc->ops;
+	struct regulator_dev *rdev = regulator->rdev;
+	const struct regulator_ops *ops = rdev->desc->ops;
 	int old_sel = -1;
 	int new_sel = -1;
 	int voltage;
@@ -3079,11 +3165,11 @@ EXPORT_SYMBOL_GPL(regulator_unregister_notifier);
 /* notify regulator consumers and downstream regulator consumers.
  * Note mutex must be held by caller.
  */
-static void _notifier_call_chain(struct regulator_dev *rdev,
+static int _notifier_call_chain(struct regulator_dev *rdev,
 				  unsigned long event, void *data)
 {
 	/* call rdev chain first */
-	blocking_notifier_call_chain(&rdev->notifier, event, data);
+	return blocking_notifier_call_chain(&rdev->notifier, event, data);
 }
 
 /**
@@ -3336,9 +3422,9 @@ EXPORT_SYMBOL_GPL(regulator_mode_to_status);
  */
 static int add_regulator_attributes(struct regulator_dev *rdev)
 {
-	struct device		*dev = &rdev->dev;
-	struct regulator_ops	*ops = rdev->desc->ops;
-	int			status = 0;
+	struct device *dev = &rdev->dev;
+	const struct regulator_ops *ops = rdev->desc->ops;
+	int status = 0;
 
 	/* some attributes need specific methods to be displayed */
 	if ((ops->get_voltage && ops->get_voltage(rdev) >= 0) ||
@@ -3516,12 +3602,17 @@ regulator_register(const struct regulator_desc *regulator_desc,
 		return ERR_PTR(-EINVAL);
 	}
 
-	init_data = config->init_data;
-
 	rdev = kzalloc(sizeof(struct regulator_dev), GFP_KERNEL);
 	if (rdev == NULL)
 		return ERR_PTR(-ENOMEM);
 
+	init_data = regulator_of_get_init_data(dev, regulator_desc,
+					       &rdev->dev.of_node);
+	if (!init_data) {
+		init_data = config->init_data;
+		rdev->dev.of_node = of_node_get(config->of_node);
+	}
+
 	mutex_lock(&regulator_list_mutex);
 
 	mutex_init(&rdev->mutex);
@@ -3548,7 +3639,6 @@ regulator_register(const struct regulator_desc *regulator_desc,
 
 	/* register with sysfs */
 	rdev->dev.class = &regulator_class;
-	rdev->dev.of_node = of_node_get(config->of_node);
 	rdev->dev.parent = dev;
 	dev_set_name(&rdev->dev, "regulator.%d",
 		     atomic_inc_return(&regulator_no) - 1);
@@ -3905,7 +3995,7 @@ core_initcall(regulator_init);
 static int __init regulator_init_complete(void)
 {
 	struct regulator_dev *rdev;
-	struct regulator_ops *ops;
+	const struct regulator_ops *ops;
 	struct regulation_constraints *c;
 	int enabled, ret;
 
diff --git a/drivers/regulator/da9052-regulator.c b/drivers/regulator/da9052-regulator.c
index fdb6ea8ae7e6..00033625a09c 100644
--- a/drivers/regulator/da9052-regulator.c
+++ b/drivers/regulator/da9052-regulator.c
@@ -422,9 +422,9 @@ static int da9052_regulator_probe(struct platform_device *pdev)
 		config.init_data = pdata->regulators[pdev->id];
 	} else {
 #ifdef CONFIG_OF
-		struct device_node *nproot, *np;
+		struct device_node *nproot = da9052->dev->of_node;
+		struct device_node *np;
 
-		nproot = of_node_get(da9052->dev->of_node);
 		if (!nproot)
 			return -ENODEV;
 
diff --git a/drivers/regulator/da9211-regulator.c b/drivers/regulator/da9211-regulator.c
index 1482adafa1ad..c78d2106d6cb 100644
--- a/drivers/regulator/da9211-regulator.c
+++ b/drivers/regulator/da9211-regulator.c
@@ -1,5 +1,5 @@
 /*
- * da9211-regulator.c - Regulator device driver for DA9211
+ * da9211-regulator.c - Regulator device driver for DA9211/DA9213
  * Copyright (C) 2014  Dialog Semiconductor Ltd.
  *
  * This library is free software; you can redistribute it and/or
@@ -24,9 +24,14 @@
 #include <linux/regmap.h>
 #include <linux/irq.h>
 #include <linux/interrupt.h>
+#include <linux/regulator/of_regulator.h>
 #include <linux/regulator/da9211.h>
 #include "da9211-regulator.h"
 
+/* DEVICE IDs */
+#define DA9211_DEVICE_ID	0x22
+#define DA9213_DEVICE_ID	0x23
+
 #define DA9211_BUCK_MODE_SLEEP	1
 #define DA9211_BUCK_MODE_SYNC	2
 #define DA9211_BUCK_MODE_AUTO	3
@@ -42,6 +47,7 @@ struct da9211 {
 	struct regulator_dev *rdev[DA9211_MAX_REGULATORS];
 	int num_regulator;
 	int chip_irq;
+	int chip_id;
 };
 
 static const struct regmap_range_cfg da9211_regmap_range[] = {
@@ -52,14 +58,14 @@ static const struct regmap_range_cfg da9211_regmap_range[] = {
 		.window_start = 0,
 		.window_len = 256,
 		.range_min = 0,
-		.range_max = 2*256,
+		.range_max = 5*128,
 	},
 };
 
 static const struct regmap_config da9211_regmap_config = {
 	.reg_bits = 8,
 	.val_bits = 8,
-	.max_register = 2 * 256,
+	.max_register = 5 * 128,
 	.ranges = da9211_regmap_range,
 	.num_ranges = ARRAY_SIZE(da9211_regmap_range),
 };
@@ -69,11 +75,20 @@ static const struct regmap_config da9211_regmap_config = {
 #define DA9211_MAX_MV		1570
 #define DA9211_STEP_MV		10
 
-/* Current limits for buck (uA) indices corresponds with register values */
+/* Current limits for DA9211 buck (uA) indices
+ * corresponds with register values
+ */
 static const int da9211_current_limits[] = {
 	2000000, 2200000, 2400000, 2600000, 2800000, 3000000, 3200000, 3400000,
 	3600000, 3800000, 4000000, 4200000, 4400000, 4600000, 4800000, 5000000
 };
+/* Current limits for DA9213 buck (uA) indices
+ * corresponds with register values
+ */
+static const int da9213_current_limits[] = {
+	3000000, 3200000, 3400000, 3600000, 3800000, 4000000, 4200000, 4400000,
+	4600000, 4800000, 5000000, 5200000, 5400000, 5600000, 5800000, 6000000
+};
 
 static unsigned int da9211_buck_get_mode(struct regulator_dev *rdev)
 {
@@ -129,12 +144,26 @@ static int da9211_set_current_limit(struct regulator_dev *rdev, int min,
 {
 	int id = rdev_get_id(rdev);
 	struct da9211 *chip = rdev_get_drvdata(rdev);
-	int i;
+	int i, max_size;
+	const int *current_limits;
+
+	switch (chip->chip_id) {
+	case DA9211:
+		current_limits = da9211_current_limits;
+		max_size = ARRAY_SIZE(da9211_current_limits)-1;
+		break;
+	case DA9213:
+		current_limits = da9213_current_limits;
+		max_size = ARRAY_SIZE(da9213_current_limits)-1;
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	/* search for closest to maximum */
-	for (i = ARRAY_SIZE(da9211_current_limits)-1; i >= 0; i--) {
-		if (min <= da9211_current_limits[i] &&
-		    max >= da9211_current_limits[i]) {
+	for (i = max_size; i >= 0; i--) {
+		if (min <= current_limits[i] &&
+		    max >= current_limits[i]) {
 				return regmap_update_bits(chip->regmap,
 					DA9211_REG_BUCK_ILIM,
 					(0x0F << id*4), (i << id*4));
@@ -150,14 +179,28 @@ static int da9211_get_current_limit(struct regulator_dev *rdev)
 	struct da9211 *chip = rdev_get_drvdata(rdev);
 	unsigned int data;
 	int ret;
+	const int *current_limits;
+
+	switch (chip->chip_id) {
+	case DA9211:
+		current_limits = da9211_current_limits;
+		break;
+	case DA9213:
+		current_limits = da9213_current_limits;
+		break;
+	default:
+		return -EINVAL;
+	}
 
 	ret = regmap_read(chip->regmap, DA9211_REG_BUCK_ILIM, &data);
 	if (ret < 0)
 		return ret;
 
-	/* select one of 16 values: 0000 (2000mA) to 1111 (5000mA) */
+	/* select one of 16 values: 0000 (2000mA or 3000mA)
+	 * to 1111 (5000mA or 6000mA).
+	 */
 	data = (data >> id*4) & 0x0F;
-	return da9211_current_limits[data];
+	return current_limits[data];
 }
 
 static struct regulator_ops da9211_buck_ops = {
@@ -194,6 +237,59 @@ static struct regulator_desc da9211_regulators[] = {
 	DA9211_BUCK(BUCKB),
 };
 
+#ifdef CONFIG_OF
+static struct of_regulator_match da9211_matches[] = {
+	[DA9211_ID_BUCKA] = { .name = "BUCKA" },
+	[DA9211_ID_BUCKB] = { .name = "BUCKB" },
+	};
+
+static struct da9211_pdata *da9211_parse_regulators_dt(
+		struct device *dev)
+{
+	struct da9211_pdata *pdata;
+	struct device_node *node;
+	int i, num, n;
+
+	node = of_get_child_by_name(dev->of_node, "regulators");
+	if (!node) {
+		dev_err(dev, "regulators node not found\n");
+		return ERR_PTR(-ENODEV);
+	}
+
+	num = of_regulator_match(dev, node, da9211_matches,
+				 ARRAY_SIZE(da9211_matches));
+	of_node_put(node);
+	if (num < 0) {
+		dev_err(dev, "Failed to match regulators\n");
+		return ERR_PTR(-EINVAL);
+	}
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return ERR_PTR(-ENOMEM);
+
+	pdata->num_buck = num;
+
+	n = 0;
+	for (i = 0; i < ARRAY_SIZE(da9211_matches); i++) {
+		if (!da9211_matches[i].init_data)
+			continue;
+
+		pdata->init_data[n] = da9211_matches[i].init_data;
+
+		n++;
+	}
+
+	return pdata;
+}
+#else
+static struct da9211_pdata *da9211_parse_regulators_dt(
+		struct device *dev)
+{
+	return ERR_PTR(-ENODEV);
+}
+#endif
+
 static irqreturn_t da9211_irq_handler(int irq, void *data)
 {
 	struct da9211 *chip = data;
@@ -264,13 +360,11 @@ static int da9211_regulator_init(struct da9211 *chip)
 	}
 
 	for (i = 0; i < chip->num_regulator; i++) {
-		if (chip->pdata)
-			config.init_data =
-				&(chip->pdata->init_data[i]);
-
+		config.init_data = chip->pdata->init_data[i];
 		config.dev = chip->dev;
 		config.driver_data = chip;
 		config.regmap = chip->regmap;
+		config.of_node = chip->dev->of_node;
 
 		chip->rdev[i] = devm_regulator_register(chip->dev,
 			&da9211_regulators[i], &config);
@@ -282,7 +376,7 @@ static int da9211_regulator_init(struct da9211 *chip)
 
 		if (chip->chip_irq != 0) {
 			ret = regmap_update_bits(chip->regmap,
-				DA9211_REG_MASK_B, DA9211_M_OV_CURR_A << i, 1);
+				DA9211_REG_MASK_B, DA9211_M_OV_CURR_A << i, 0);
 			if (ret < 0) {
 				dev_err(chip->dev,
 					"Failed to update mask reg: %d\n", ret);
@@ -293,6 +387,7 @@ static int da9211_regulator_init(struct da9211 *chip)
 
 	return 0;
 }
+
 /*
  * I2C driver interface functions
  */
@@ -301,14 +396,17 @@ static int da9211_i2c_probe(struct i2c_client *i2c,
 {
 	struct da9211 *chip;
 	int error, ret;
+	unsigned int data;
 
 	chip = devm_kzalloc(&i2c->dev, sizeof(struct da9211), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
 
 	chip->dev = &i2c->dev;
 	chip->regmap = devm_regmap_init_i2c(i2c, &da9211_regmap_config);
 	if (IS_ERR(chip->regmap)) {
 		error = PTR_ERR(chip->regmap);
-		dev_err(&i2c->dev, "Failed to allocate register map: %d\n",
+		dev_err(chip->dev, "Failed to allocate register map: %d\n",
 			error);
 		return error;
 	}
@@ -316,11 +414,33 @@ static int da9211_i2c_probe(struct i2c_client *i2c,
 	i2c_set_clientdata(i2c, chip);
 
 	chip->pdata = i2c->dev.platform_data;
-	if (!chip->pdata) {
-		dev_err(&i2c->dev, "No platform init data supplied\n");
+
+	ret = regmap_read(chip->regmap, DA9211_REG_DEVICE_ID, &data);
+	if (ret < 0) {
+		dev_err(chip->dev, "Failed to read DEVICE_ID reg: %d\n", ret);
+		return ret;
+	}
+
+	switch (data) {
+	case DA9211_DEVICE_ID:
+		chip->chip_id = DA9211;
+		break;
+	case DA9213_DEVICE_ID:
+		chip->chip_id = DA9213;
+		break;
+	default:
+		dev_err(chip->dev, "Unsupported device id = 0x%x.\n", data);
 		return -ENODEV;
 	}
 
+	if (!chip->pdata)
+		chip->pdata = da9211_parse_regulators_dt(chip->dev);
+
+	if (IS_ERR(chip->pdata)) {
+		dev_err(chip->dev, "No regulators defined for the platform\n");
+		return PTR_ERR(chip->pdata);
+	}
+
 	chip->chip_irq = i2c->irq;
 
 	if (chip->chip_irq != 0) {
@@ -340,22 +460,32 @@ static int da9211_i2c_probe(struct i2c_client *i2c,
 	ret = da9211_regulator_init(chip);
 
 	if (ret < 0)
-		dev_err(&i2c->dev, "Failed to initialize regulator: %d\n", ret);
+		dev_err(chip->dev, "Failed to initialize regulator: %d\n", ret);
 
 	return ret;
 }
 
 static const struct i2c_device_id da9211_i2c_id[] = {
-	{"da9211", 0},
+	{"da9211", DA9211},
+	{"da9213", DA9213},
 	{},
 };
-
 MODULE_DEVICE_TABLE(i2c, da9211_i2c_id);
 
+#ifdef CONFIG_OF
+static const struct of_device_id da9211_dt_ids[] = {
+	{ .compatible = "dlg,da9211", .data = &da9211_i2c_id[0] },
+	{ .compatible = "dlg,da9213", .data = &da9211_i2c_id[1] },
+	{},
+};
+MODULE_DEVICE_TABLE(of, da9211_dt_ids);
+#endif
+
 static struct i2c_driver da9211_regulator_driver = {
 	.driver = {
 		.name = "da9211",
 		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(da9211_dt_ids),
 	},
 	.probe = da9211_i2c_probe,
 	.id_table = da9211_i2c_id,
@@ -364,5 +494,5 @@ static struct i2c_driver da9211_regulator_driver = {
 module_i2c_driver(da9211_regulator_driver);
 
 MODULE_AUTHOR("James Ban <James.Ban.opensource@diasemi.com>");
-MODULE_DESCRIPTION("Regulator device driver for Dialog DA9211");
+MODULE_DESCRIPTION("Regulator device driver for Dialog DA9211/DA9213");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/da9211-regulator.h b/drivers/regulator/da9211-regulator.h
index 88b1769e8058..93fa9df2721c 100644
--- a/drivers/regulator/da9211-regulator.h
+++ b/drivers/regulator/da9211-regulator.h
@@ -1,5 +1,5 @@
 /*
- * da9211-regulator.h - Regulator definitions for DA9211
+ * da9211-regulator.h - Regulator definitions for DA9211/DA9213
  * Copyright (C) 2014  Dialog Semiconductor Ltd.
  *
  * This library is free software; you can redistribute it and/or
@@ -53,12 +53,15 @@
 /* BUCK Phase Selection*/
 #define DA9211_REG_CONFIG_E			0x147
 
+/* Device ID */
+#define	DA9211_REG_DEVICE_ID			0x201
+
 /*
  * Registers bits
  */
 /* DA9211_REG_PAGE_CON (addr=0x00) */
 #define	DA9211_REG_PAGE_SHIFT			1
-#define	DA9211_REG_PAGE_MASK			0x02
+#define	DA9211_REG_PAGE_MASK			0x06
 /* On I2C registers 0x00 - 0xFF */
 #define	DA9211_REG_PAGE0			0
 /* On I2C registers 0x100 - 0x1FF */
diff --git a/drivers/regulator/fan53555.c b/drivers/regulator/fan53555.c
index 714fd9a89aa1..f8e4257aef92 100644
--- a/drivers/regulator/fan53555.c
+++ b/drivers/regulator/fan53555.c
@@ -18,6 +18,8 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/of_device.h>
 #include <linux/i2c.h>
 #include <linux/slab.h>
 #include <linux/regmap.h>
@@ -50,6 +52,11 @@
 
 #define FAN53555_NVOLTAGES	64	/* Numbers of voltages */
 
+enum fan53555_vendor {
+	FAN53555_VENDOR_FAIRCHILD = 0,
+	FAN53555_VENDOR_SILERGY,
+};
+
 /* IC Type */
 enum {
 	FAN53555_CHIP_ID_00 = 0,
@@ -60,7 +67,12 @@ enum {
 	FAN53555_CHIP_ID_05,
 };
 
+enum {
+	SILERGY_SYR82X = 8,
+};
+
 struct fan53555_device_info {
+	enum fan53555_vendor vendor;
 	struct regmap *regmap;
 	struct device *dev;
 	struct regulator_desc desc;
@@ -135,6 +147,38 @@ static unsigned int fan53555_get_mode(struct regulator_dev *rdev)
 		return REGULATOR_MODE_NORMAL;
 }
 
+static int slew_rates[] = {
+	64000,
+	32000,
+	16000,
+	 8000,
+	 4000,
+	 2000,
+	 1000,
+	  500,
+};
+
+static int fan53555_set_ramp(struct regulator_dev *rdev, int ramp)
+{
+	struct fan53555_device_info *di = rdev_get_drvdata(rdev);
+	int regval = -1, i;
+
+	for (i = 0; i < ARRAY_SIZE(slew_rates); i++) {
+		if (ramp <= slew_rates[i])
+			regval = i;
+		else
+			break;
+	}
+
+	if (regval < 0) {
+		dev_err(di->dev, "unsupported ramp value %d\n", ramp);
+		return -EINVAL;
+	}
+
+	return regmap_update_bits(di->regmap, FAN53555_CONTROL,
+				  CTL_SLEW_MASK, regval << CTL_SLEW_SHIFT);
+}
+
 static struct regulator_ops fan53555_regulator_ops = {
 	.set_voltage_sel = regulator_set_voltage_sel_regmap,
 	.get_voltage_sel = regulator_get_voltage_sel_regmap,
@@ -146,8 +190,50 @@ static struct regulator_ops fan53555_regulator_ops = {
 	.is_enabled = regulator_is_enabled_regmap,
 	.set_mode = fan53555_set_mode,
 	.get_mode = fan53555_get_mode,
+	.set_ramp_delay = fan53555_set_ramp,
 };
 
+static int fan53555_voltages_setup_fairchild(struct fan53555_device_info *di)
+{
+	/* Init voltage range and step */
+	switch (di->chip_id) {
+	case FAN53555_CHIP_ID_00:
+	case FAN53555_CHIP_ID_01:
+	case FAN53555_CHIP_ID_03:
+	case FAN53555_CHIP_ID_05:
+		di->vsel_min = 600000;
+		di->vsel_step = 10000;
+		break;
+	case FAN53555_CHIP_ID_04:
+		di->vsel_min = 603000;
+		di->vsel_step = 12826;
+		break;
+	default:
+		dev_err(di->dev,
+			"Chip ID %d not supported!\n", di->chip_id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int fan53555_voltages_setup_silergy(struct fan53555_device_info *di)
+{
+	/* Init voltage range and step */
+	switch (di->chip_id) {
+	case SILERGY_SYR82X:
+		di->vsel_min = 712500;
+		di->vsel_step = 12500;
+		break;
+	default:
+		dev_err(di->dev,
+			"Chip ID %d not supported!\n", di->chip_id);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 /* For 00,01,03,05 options:
  * VOUT = 0.60V + NSELx * 10mV, from 0.60 to 1.23V.
  * For 04 option:
@@ -156,7 +242,7 @@ static struct regulator_ops fan53555_regulator_ops = {
 static int fan53555_device_setup(struct fan53555_device_info *di,
 				struct fan53555_platform_data *pdata)
 {
-	unsigned int reg, data, mask;
+	int ret = 0;
 
 	/* Setup voltage control register */
 	switch (pdata->sleep_vsel_id) {
@@ -172,33 +258,20 @@ static int fan53555_device_setup(struct fan53555_device_info *di,
 		dev_err(di->dev, "Invalid VSEL ID!\n");
 		return -EINVAL;
 	}
-	/* Init voltage range and step */
-	switch (di->chip_id) {
-	case FAN53555_CHIP_ID_00:
-	case FAN53555_CHIP_ID_01:
-	case FAN53555_CHIP_ID_03:
-	case FAN53555_CHIP_ID_05:
-		di->vsel_min = 600000;
-		di->vsel_step = 10000;
+
+	switch (di->vendor) {
+	case FAN53555_VENDOR_FAIRCHILD:
+		ret = fan53555_voltages_setup_fairchild(di);
 		break;
-	case FAN53555_CHIP_ID_04:
-		di->vsel_min = 603000;
-		di->vsel_step = 12826;
+	case FAN53555_VENDOR_SILERGY:
+		ret = fan53555_voltages_setup_silergy(di);
 		break;
 	default:
-		dev_err(di->dev,
-			"Chip ID[%d]\n not supported!\n", di->chip_id);
+		dev_err(di->dev, "vendor %d not supported!\n", di->vendor);
 		return -EINVAL;
 	}
-	/* Init slew rate */
-	if (pdata->slew_rate & 0x7)
-		di->slew_rate = pdata->slew_rate;
-	else
-		di->slew_rate = FAN53555_SLEW_RATE_64MV;
-	reg = FAN53555_CONTROL;
-	data = di->slew_rate << CTL_SLEW_SHIFT;
-	mask = CTL_SLEW_MASK;
-	return regmap_update_bits(di->regmap, reg, mask, data);
+
+	return ret;
 }
 
 static int fan53555_regulator_register(struct fan53555_device_info *di,
@@ -207,6 +280,7 @@ static int fan53555_regulator_register(struct fan53555_device_info *di,
 	struct regulator_desc *rdesc = &di->desc;
 
 	rdesc->name = "fan53555-reg";
+	rdesc->supply_name = "vin";
 	rdesc->ops = &fan53555_regulator_ops;
 	rdesc->type = REGULATOR_VOLTAGE;
 	rdesc->n_voltages = FAN53555_NVOLTAGES;
@@ -227,9 +301,46 @@ static struct regmap_config fan53555_regmap_config = {
 	.val_bits = 8,
 };
 
+static struct fan53555_platform_data *fan53555_parse_dt(struct device *dev,
+							struct device_node *np)
+{
+	struct fan53555_platform_data *pdata;
+	int ret;
+	u32 tmp;
+
+	pdata = devm_kzalloc(dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return NULL;
+
+	pdata->regulator = of_get_regulator_init_data(dev, np);
+
+	ret = of_property_read_u32(np, "fcs,suspend-voltage-selector",
+				   &tmp);
+	if (!ret)
+		pdata->sleep_vsel_id = tmp;
+
+	return pdata;
+}
+
+static const struct of_device_id fan53555_dt_ids[] = {
+	{
+		.compatible = "fcs,fan53555",
+		.data = (void *)FAN53555_VENDOR_FAIRCHILD
+	}, {
+		.compatible = "silergy,syr827",
+		.data = (void *)FAN53555_VENDOR_SILERGY,
+	}, {
+		.compatible = "silergy,syr828",
+		.data = (void *)FAN53555_VENDOR_SILERGY,
+	},
+	{ }
+};
+MODULE_DEVICE_TABLE(of, fan53555_dt_ids);
+
 static int fan53555_regulator_probe(struct i2c_client *client,
 				const struct i2c_device_id *id)
 {
+	struct device_node *np = client->dev.of_node;
 	struct fan53555_device_info *di;
 	struct fan53555_platform_data *pdata;
 	struct regulator_config config = { };
@@ -237,6 +348,9 @@ static int fan53555_regulator_probe(struct i2c_client *client,
 	int ret;
 
 	pdata = dev_get_platdata(&client->dev);
+	if (!pdata)
+		pdata = fan53555_parse_dt(&client->dev, np);
+
 	if (!pdata || !pdata->regulator) {
 		dev_err(&client->dev, "Platform data not found!\n");
 		return -ENODEV;
@@ -247,13 +361,35 @@ static int fan53555_regulator_probe(struct i2c_client *client,
 	if (!di)
 		return -ENOMEM;
 
+	di->regulator = pdata->regulator;
+	if (client->dev.of_node) {
+		const struct of_device_id *match;
+
+		match = of_match_device(of_match_ptr(fan53555_dt_ids),
+					&client->dev);
+		if (!match)
+			return -ENODEV;
+
+		di->vendor = (unsigned long) match->data;
+	} else {
+		/* if no ramp constraint set, get the pdata ramp_delay */
+		if (!di->regulator->constraints.ramp_delay) {
+			int slew_idx = (pdata->slew_rate & 0x7)
+						? pdata->slew_rate : 0;
+
+			di->regulator->constraints.ramp_delay
+						= slew_rates[slew_idx];
+		}
+
+		di->vendor = id->driver_data;
+	}
+
 	di->regmap = devm_regmap_init_i2c(client, &fan53555_regmap_config);
 	if (IS_ERR(di->regmap)) {
 		dev_err(&client->dev, "Failed to allocate regmap!\n");
 		return PTR_ERR(di->regmap);
 	}
 	di->dev = &client->dev;
-	di->regulator = pdata->regulator;
 	i2c_set_clientdata(client, di);
 	/* Get chip ID */
 	ret = regmap_read(di->regmap, FAN53555_ID1, &val);
@@ -282,6 +418,8 @@ static int fan53555_regulator_probe(struct i2c_client *client,
 	config.init_data = di->regulator;
 	config.regmap = di->regmap;
 	config.driver_data = di;
+	config.of_node = np;
+
 	ret = fan53555_regulator_register(di, &config);
 	if (ret < 0)
 		dev_err(&client->dev, "Failed to register regulator!\n");
@@ -290,13 +428,20 @@ static int fan53555_regulator_probe(struct i2c_client *client,
 }
 
 static const struct i2c_device_id fan53555_id[] = {
-	{"fan53555", -1},
+	{
+		.name = "fan53555",
+		.driver_data = FAN53555_VENDOR_FAIRCHILD
+	}, {
+		.name = "syr82x",
+		.driver_data = FAN53555_VENDOR_SILERGY
+	},
 	{ },
 };
 
 static struct i2c_driver fan53555_regulator_driver = {
 	.driver = {
 		.name = "fan53555-regulator",
+		.of_match_table = of_match_ptr(fan53555_dt_ids),
 	},
 	.probe = fan53555_regulator_probe,
 	.id_table = fan53555_id,
diff --git a/drivers/regulator/hi6421-regulator.c b/drivers/regulator/hi6421-regulator.c
new file mode 100644
index 000000000000..156d0d1a55f1
--- /dev/null
+++ b/drivers/regulator/hi6421-regulator.c
@@ -0,0 +1,634 @@
+/*
+ * Device driver for regulators in Hi6421 IC
+ *
+ * Copyright (c) <2011-2014> HiSilicon Technologies Co., Ltd.
+ *              http://www.hisilicon.com
+ * Copyright (c) <2013-2014> Linaro Ltd.
+ *              http://www.linaro.org
+ *
+ * Author: Guodong Xu <guodong.xu@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/mfd/hi6421-pmic.h>
+
+/*
+ * struct hi6421_regulator_pdata - Hi6421 regulator data of platform device
+ * @lock: mutex to serialize regulator enable
+ */
+struct hi6421_regulator_pdata {
+	struct mutex lock;
+};
+
+/*
+ * struct hi6421_regulator_info - hi6421 regulator information
+ * @desc: regulator description
+ * @mode_mask: ECO mode bitmask of LDOs; for BUCKs, this masks sleep
+ * @eco_microamp: eco mode load upper limit (in uA), valid for LDOs only
+ */
+struct hi6421_regulator_info {
+	struct regulator_desc	desc;
+	u8		mode_mask;
+	u32		eco_microamp;
+};
+
+/* HI6421 regulators */
+enum hi6421_regulator_id {
+	HI6421_LDO0,
+	HI6421_LDO1,
+	HI6421_LDO2,
+	HI6421_LDO3,
+	HI6421_LDO4,
+	HI6421_LDO5,
+	HI6421_LDO6,
+	HI6421_LDO7,
+	HI6421_LDO8,
+	HI6421_LDO9,
+	HI6421_LDO10,
+	HI6421_LDO11,
+	HI6421_LDO12,
+	HI6421_LDO13,
+	HI6421_LDO14,
+	HI6421_LDO15,
+	HI6421_LDO16,
+	HI6421_LDO17,
+	HI6421_LDO18,
+	HI6421_LDO19,
+	HI6421_LDO20,
+	HI6421_LDOAUDIO,
+	HI6421_BUCK0,
+	HI6421_BUCK1,
+	HI6421_BUCK2,
+	HI6421_BUCK3,
+	HI6421_BUCK4,
+	HI6421_BUCK5,
+	HI6421_NUM_REGULATORS,
+};
+
+#define HI6421_REGULATOR_OF_MATCH(_name, id)				\
+{									\
+	.name = #_name,							\
+	.driver_data = (void *) HI6421_##id,				\
+}
+
+static struct of_regulator_match hi6421_regulator_match[] = {
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout0, LDO0),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout1, LDO1),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout2, LDO2),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout3, LDO3),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout4, LDO4),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout5, LDO5),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout6, LDO6),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout7, LDO7),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout8, LDO8),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout9, LDO9),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout10, LDO10),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout11, LDO11),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout12, LDO12),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout13, LDO13),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout14, LDO14),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout15, LDO15),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout16, LDO16),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout17, LDO17),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout18, LDO18),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout19, LDO19),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout20, LDO20),
+	HI6421_REGULATOR_OF_MATCH(hi6421_vout_audio, LDOAUDIO),
+	HI6421_REGULATOR_OF_MATCH(hi6421_buck0, BUCK0),
+	HI6421_REGULATOR_OF_MATCH(hi6421_buck1, BUCK1),
+	HI6421_REGULATOR_OF_MATCH(hi6421_buck2, BUCK2),
+	HI6421_REGULATOR_OF_MATCH(hi6421_buck3, BUCK3),
+	HI6421_REGULATOR_OF_MATCH(hi6421_buck4, BUCK4),
+	HI6421_REGULATOR_OF_MATCH(hi6421_buck5, BUCK5),
+};
+
+/* LDO 0, 4~7, 9~14, 16~20 have same voltage table. */
+static const unsigned int ldo_0_voltages[] = {
+	1500000, 1800000, 2400000, 2500000,
+	2600000, 2700000, 2850000, 3000000,
+};
+
+/* LDO 8, 15 have same voltage table. */
+static const unsigned int ldo_8_voltages[] = {
+	1500000, 1800000, 2400000, 2600000,
+	2700000, 2850000, 3000000, 3300000,
+};
+
+/* Ranges are sorted in ascending order. */
+static const struct regulator_linear_range ldo_audio_volt_range[] = {
+	REGULATOR_LINEAR_RANGE(2800000, 0, 3, 50000),
+	REGULATOR_LINEAR_RANGE(3000000, 4, 7, 100000),
+};
+
+static const unsigned int buck_3_voltages[] = {
+	 950000, 1050000, 1100000, 1117000,
+	1134000, 1150000, 1167000, 1200000,
+};
+
+static const unsigned int buck_4_voltages[] = {
+	1150000, 1200000, 1250000, 1350000,
+	1700000, 1800000, 1900000, 2000000,
+};
+
+static const unsigned int buck_5_voltages[] = {
+	1150000, 1200000, 1250000, 1350000,
+	1600000, 1700000, 1800000, 1900000,
+};
+
+static const struct regulator_ops hi6421_ldo_ops;
+static const struct regulator_ops hi6421_ldo_linear_ops;
+static const struct regulator_ops hi6421_ldo_linear_range_ops;
+static const struct regulator_ops hi6421_buck012_ops;
+static const struct regulator_ops hi6421_buck345_ops;
+
+#define HI6421_LDO_ENABLE_TIME (350)
+/*
+ * _id - LDO id name string
+ * v_table - voltage table
+ * vreg - voltage select register
+ * vmask - voltage select mask
+ * ereg - enable register
+ * emask - enable mask
+ * odelay - off/on delay time in uS
+ * ecomask - eco mode mask
+ * ecoamp - eco mode load uppler limit in uA
+ */
+#define HI6421_LDO(_id, v_table, vreg, vmask, ereg, emask,		\
+		   odelay, ecomask, ecoamp)				\
+	[HI6421_##_id] = {						\
+		.desc = {						\
+			.name		= #_id,				\
+			.ops		= &hi6421_ldo_ops,		\
+			.type		= REGULATOR_VOLTAGE,		\
+			.id		= HI6421_##_id,			\
+			.owner		= THIS_MODULE,			\
+			.n_voltages	= ARRAY_SIZE(v_table),		\
+			.volt_table	= v_table,			\
+			.vsel_reg	= HI6421_REG_TO_BUS_ADDR(vreg),	\
+			.vsel_mask	= vmask,			\
+			.enable_reg	= HI6421_REG_TO_BUS_ADDR(ereg),	\
+			.enable_mask	= emask,			\
+			.enable_time	= HI6421_LDO_ENABLE_TIME,	\
+			.off_on_delay	= odelay,			\
+		},							\
+		.mode_mask		= ecomask,			\
+		.eco_microamp		= ecoamp,			\
+	}
+
+/* HI6421 LDO1~3 are linear voltage regulators at fixed uV_step
+ *
+ * _id - LDO id name string
+ * _min_uV - minimum voltage supported in uV
+ * n_volt - number of votages available
+ * vstep - voltage increase in each linear step in uV
+ * vreg - voltage select register
+ * vmask - voltage select mask
+ * ereg - enable register
+ * emask - enable mask
+ * odelay - off/on delay time in uS
+ * ecomask - eco mode mask
+ * ecoamp - eco mode load uppler limit in uA
+ */
+#define HI6421_LDO_LINEAR(_id, _min_uV, n_volt, vstep, vreg, vmask,	\
+			  ereg, emask, odelay, ecomask, ecoamp)		\
+	[HI6421_##_id] = {						\
+		.desc = {						\
+			.name		= #_id,				\
+			.ops		= &hi6421_ldo_linear_ops,	\
+			.type		= REGULATOR_VOLTAGE,		\
+			.id		= HI6421_##_id,			\
+			.owner		= THIS_MODULE,			\
+			.min_uV		= _min_uV,			\
+			.n_voltages	= n_volt,			\
+			.uV_step	= vstep,			\
+			.vsel_reg	= HI6421_REG_TO_BUS_ADDR(vreg),	\
+			.vsel_mask	= vmask,			\
+			.enable_reg	= HI6421_REG_TO_BUS_ADDR(ereg),	\
+			.enable_mask	= emask,			\
+			.enable_time	= HI6421_LDO_ENABLE_TIME,	\
+			.off_on_delay	= odelay,			\
+		},							\
+		.mode_mask		= ecomask,			\
+		.eco_microamp		= ecoamp,			\
+	}
+
+/* HI6421 LDOAUDIO is a linear voltage regulator with two 4-step ranges
+ *
+ * _id - LDO id name string
+ * n_volt - number of votages available
+ * volt_ranges - array of regulator_linear_range
+ * vstep - voltage increase in each linear step in uV
+ * vreg - voltage select register
+ * vmask - voltage select mask
+ * ereg - enable register
+ * emask - enable mask
+ * odelay - off/on delay time in uS
+ * ecomask - eco mode mask
+ * ecoamp - eco mode load uppler limit in uA
+ */
+#define HI6421_LDO_LINEAR_RANGE(_id, n_volt, volt_ranges, vreg, vmask,	\
+				ereg, emask, odelay, ecomask, ecoamp)	\
+	[HI6421_##_id] = {						\
+		.desc = {						\
+			.name		= #_id,				\
+			.ops		= &hi6421_ldo_linear_range_ops,	\
+			.type		= REGULATOR_VOLTAGE,		\
+			.id		= HI6421_##_id,			\
+			.owner		= THIS_MODULE,			\
+			.n_voltages	= n_volt,			\
+			.linear_ranges	= volt_ranges,			\
+			.n_linear_ranges = ARRAY_SIZE(volt_ranges),	\
+			.vsel_reg	= HI6421_REG_TO_BUS_ADDR(vreg),	\
+			.vsel_mask	= vmask,			\
+			.enable_reg	= HI6421_REG_TO_BUS_ADDR(ereg),	\
+			.enable_mask	= emask,			\
+			.enable_time	= HI6421_LDO_ENABLE_TIME,	\
+			.off_on_delay	= odelay,			\
+		},							\
+		.mode_mask		= ecomask,			\
+		.eco_microamp		= ecoamp,			\
+	}
+
+/* HI6421 BUCK0/1/2 are linear voltage regulators at fixed uV_step
+ *
+ * _id - BUCK0/1/2 id name string
+ * vreg - voltage select register
+ * vmask - voltage select mask
+ * ereg - enable register
+ * emask - enable mask
+ * sleepmask - mask of sleep mode
+ * etime - enable time
+ * odelay - off/on delay time in uS
+ */
+#define HI6421_BUCK012(_id, vreg, vmask, ereg, emask, sleepmask,	\
+			etime, odelay)					\
+	[HI6421_##_id] = {						\
+		.desc = {						\
+			.name		= #_id,				\
+			.ops		= &hi6421_buck012_ops,		\
+			.type		= REGULATOR_VOLTAGE,		\
+			.id		= HI6421_##_id,			\
+			.owner		= THIS_MODULE,			\
+			.min_uV		= 700000,			\
+			.n_voltages	= 128,				\
+			.uV_step	= 7086,				\
+			.vsel_reg	= HI6421_REG_TO_BUS_ADDR(vreg),	\
+			.vsel_mask	= vmask,			\
+			.enable_reg	= HI6421_REG_TO_BUS_ADDR(ereg),	\
+			.enable_mask	= emask,			\
+			.enable_time	= etime,			\
+			.off_on_delay	= odelay,			\
+		},							\
+		.mode_mask		= sleepmask,			\
+	}
+
+/* HI6421 BUCK3/4/5 share similar configurations as LDOs, with exception
+ *  that it supports SLEEP mode, so has different .ops.
+ *
+ * _id - LDO id name string
+ * v_table - voltage table
+ * vreg - voltage select register
+ * vmask - voltage select mask
+ * ereg - enable register
+ * emask - enable mask
+ * odelay - off/on delay time in uS
+ * sleepmask - mask of sleep mode
+ */
+#define HI6421_BUCK345(_id, v_table, vreg, vmask, ereg, emask,		\
+			odelay, sleepmask)				\
+	[HI6421_##_id] = {						\
+		.desc = {						\
+			.name		= #_id,				\
+			.ops		= &hi6421_buck345_ops,		\
+			.type		= REGULATOR_VOLTAGE,		\
+			.id		= HI6421_##_id,			\
+			.owner		= THIS_MODULE,			\
+			.n_voltages	= ARRAY_SIZE(v_table),		\
+			.volt_table	= v_table,			\
+			.vsel_reg	= HI6421_REG_TO_BUS_ADDR(vreg),	\
+			.vsel_mask	= vmask,			\
+			.enable_reg	= HI6421_REG_TO_BUS_ADDR(ereg),	\
+			.enable_mask	= emask,			\
+			.enable_time	= HI6421_LDO_ENABLE_TIME,	\
+			.off_on_delay	= odelay,			\
+		},							\
+		.mode_mask		= sleepmask,			\
+	}
+
+/* HI6421 regulator information */
+static struct hi6421_regulator_info
+		hi6421_regulator_info[HI6421_NUM_REGULATORS] = {
+	HI6421_LDO(LDO0, ldo_0_voltages, 0x20, 0x07, 0x20, 0x10,
+		   10000, 0x20, 8000),
+	HI6421_LDO_LINEAR(LDO1, 1700000, 4, 100000, 0x21, 0x03, 0x21, 0x10,
+			  10000, 0x20, 5000),
+	HI6421_LDO_LINEAR(LDO2, 1050000, 8, 50000, 0x22, 0x07, 0x22, 0x10,
+			  20000, 0x20, 8000),
+	HI6421_LDO_LINEAR(LDO3, 1050000, 8, 50000, 0x23, 0x07, 0x23, 0x10,
+			  20000, 0x20, 8000),
+	HI6421_LDO(LDO4, ldo_0_voltages, 0x24, 0x07, 0x24, 0x10,
+		   20000, 0x20, 8000),
+	HI6421_LDO(LDO5, ldo_0_voltages, 0x25, 0x07, 0x25, 0x10,
+		   20000, 0x20, 8000),
+	HI6421_LDO(LDO6, ldo_0_voltages, 0x26, 0x07, 0x26, 0x10,
+		   20000, 0x20, 8000),
+	HI6421_LDO(LDO7, ldo_0_voltages, 0x27, 0x07, 0x27, 0x10,
+		   20000, 0x20, 5000),
+	HI6421_LDO(LDO8, ldo_8_voltages, 0x28, 0x07, 0x28, 0x10,
+		   20000, 0x20, 8000),
+	HI6421_LDO(LDO9, ldo_0_voltages, 0x29, 0x07, 0x29, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO10, ldo_0_voltages, 0x2a, 0x07, 0x2a, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO11, ldo_0_voltages, 0x2b, 0x07, 0x2b, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO12, ldo_0_voltages, 0x2c, 0x07, 0x2c, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO13, ldo_0_voltages, 0x2d, 0x07, 0x2d, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO14, ldo_0_voltages, 0x2e, 0x07, 0x2e, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO15, ldo_8_voltages, 0x2f, 0x07, 0x2f, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO16, ldo_0_voltages, 0x30, 0x07, 0x30, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO17, ldo_0_voltages, 0x31, 0x07, 0x31, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO18, ldo_0_voltages, 0x32, 0x07, 0x32, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO19, ldo_0_voltages, 0x33, 0x07, 0x33, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO(LDO20, ldo_0_voltages, 0x34, 0x07, 0x34, 0x10,
+		   40000, 0x20, 8000),
+	HI6421_LDO_LINEAR_RANGE(LDOAUDIO, 8, ldo_audio_volt_range, 0x36,
+				0x70, 0x36, 0x01, 40000, 0x02, 5000),
+	HI6421_BUCK012(BUCK0, 0x0d, 0x7f, 0x0c, 0x01, 0x10, 400, 20000),
+	HI6421_BUCK012(BUCK1, 0x0f, 0x7f, 0x0e, 0x01, 0x10, 400, 20000),
+	HI6421_BUCK012(BUCK2, 0x11, 0x7f, 0x10, 0x01, 0x10, 350, 100),
+	HI6421_BUCK345(BUCK3, buck_3_voltages, 0x13, 0x07, 0x12, 0x01,
+		       20000, 0x10),
+	HI6421_BUCK345(BUCK4, buck_4_voltages, 0x15, 0x07, 0x14, 0x01,
+		       20000, 0x10),
+	HI6421_BUCK345(BUCK5, buck_5_voltages, 0x17, 0x07, 0x16, 0x01,
+		       20000, 0x10),
+};
+
+static int hi6421_regulator_enable(struct regulator_dev *rdev)
+{
+	struct hi6421_regulator_pdata *pdata;
+
+	pdata = dev_get_drvdata(rdev->dev.parent);
+	/* hi6421 spec requires regulator enablement must be serialized:
+	 *  - Because when BUCK, LDO switching from off to on, it will have
+	 *    a huge instantaneous current; so you can not turn on two or
+	 *    more LDO or BUCKs simultaneously, or it may burn the chip.
+	 */
+	mutex_lock(&pdata->lock);
+
+	/* call regulator regmap helper */
+	regulator_enable_regmap(rdev);
+
+	mutex_unlock(&pdata->lock);
+	return 0;
+}
+
+static unsigned int hi6421_regulator_ldo_get_mode(struct regulator_dev *rdev)
+{
+	struct hi6421_regulator_info *info = rdev_get_drvdata(rdev);
+	u32 reg_val;
+
+	regmap_read(rdev->regmap, rdev->desc->enable_reg, &reg_val);
+	if (reg_val & info->mode_mask)
+		return REGULATOR_MODE_IDLE;
+
+	return REGULATOR_MODE_NORMAL;
+}
+
+static unsigned int hi6421_regulator_buck_get_mode(struct regulator_dev *rdev)
+{
+	struct hi6421_regulator_info *info = rdev_get_drvdata(rdev);
+	u32 reg_val;
+
+	regmap_read(rdev->regmap, rdev->desc->enable_reg, &reg_val);
+	if (reg_val & info->mode_mask)
+		return REGULATOR_MODE_STANDBY;
+
+	return REGULATOR_MODE_NORMAL;
+}
+
+static int hi6421_regulator_ldo_set_mode(struct regulator_dev *rdev,
+						unsigned int mode)
+{
+	struct hi6421_regulator_info *info = rdev_get_drvdata(rdev);
+	u32 new_mode;
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		new_mode = 0;
+		break;
+	case REGULATOR_MODE_IDLE:
+		new_mode = info->mode_mask;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* set mode */
+	regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+			   info->mode_mask, new_mode);
+
+	return 0;
+}
+
+static int hi6421_regulator_buck_set_mode(struct regulator_dev *rdev,
+						unsigned int mode)
+{
+	struct hi6421_regulator_info *info = rdev_get_drvdata(rdev);
+	u32 new_mode;
+
+	switch (mode) {
+	case REGULATOR_MODE_NORMAL:
+		new_mode = 0;
+		break;
+	case REGULATOR_MODE_STANDBY:
+		new_mode = info->mode_mask;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	/* set mode */
+	regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+			   info->mode_mask, new_mode);
+
+	return 0;
+}
+
+unsigned int hi6421_regulator_ldo_get_optimum_mode(struct regulator_dev *rdev,
+			int input_uV, int output_uV, int load_uA)
+{
+	struct hi6421_regulator_info *info = rdev_get_drvdata(rdev);
+
+	if (load_uA > info->eco_microamp)
+		return REGULATOR_MODE_NORMAL;
+
+	return REGULATOR_MODE_IDLE;
+}
+
+static const struct regulator_ops hi6421_ldo_ops = {
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = hi6421_regulator_enable,
+	.disable = regulator_disable_regmap,
+	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_mode = hi6421_regulator_ldo_get_mode,
+	.set_mode = hi6421_regulator_ldo_set_mode,
+	.get_optimum_mode = hi6421_regulator_ldo_get_optimum_mode,
+};
+
+static const struct regulator_ops hi6421_ldo_linear_ops = {
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = hi6421_regulator_enable,
+	.disable = regulator_disable_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_mode = hi6421_regulator_ldo_get_mode,
+	.set_mode = hi6421_regulator_ldo_set_mode,
+	.get_optimum_mode = hi6421_regulator_ldo_get_optimum_mode,
+};
+
+static const struct regulator_ops hi6421_ldo_linear_range_ops = {
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = hi6421_regulator_enable,
+	.disable = regulator_disable_regmap,
+	.list_voltage = regulator_list_voltage_linear_range,
+	.map_voltage = regulator_map_voltage_linear_range,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_mode = hi6421_regulator_ldo_get_mode,
+	.set_mode = hi6421_regulator_ldo_set_mode,
+	.get_optimum_mode = hi6421_regulator_ldo_get_optimum_mode,
+};
+
+static const struct regulator_ops hi6421_buck012_ops = {
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = hi6421_regulator_enable,
+	.disable = regulator_disable_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.map_voltage = regulator_map_voltage_linear,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_mode = hi6421_regulator_buck_get_mode,
+	.set_mode = hi6421_regulator_buck_set_mode,
+};
+
+static const struct regulator_ops hi6421_buck345_ops = {
+	.is_enabled = regulator_is_enabled_regmap,
+	.enable = hi6421_regulator_enable,
+	.disable = regulator_disable_regmap,
+	.list_voltage = regulator_list_voltage_table,
+	.map_voltage = regulator_map_voltage_ascend,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.get_mode = hi6421_regulator_buck_get_mode,
+	.set_mode = hi6421_regulator_buck_set_mode,
+};
+
+static int hi6421_regulator_register(struct platform_device *pdev,
+				     struct regmap *rmap,
+				     struct regulator_init_data *init_data,
+				     int id, struct device_node *np)
+{
+	struct hi6421_regulator_info *info = NULL;
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+
+	/* assign per-regulator data */
+	info = &hi6421_regulator_info[id];
+
+	config.dev = &pdev->dev;
+	config.init_data = init_data;
+	config.driver_data = info;
+	config.regmap = rmap;
+	config.of_node = np;
+
+	/* register regulator with framework */
+	rdev = devm_regulator_register(&pdev->dev, &info->desc, &config);
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "failed to register regulator %s\n",
+			info->desc.name);
+		return PTR_ERR(rdev);
+	}
+
+	return 0;
+}
+
+static int hi6421_regulator_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *np;
+	struct hi6421_pmic *pmic;
+	struct hi6421_regulator_pdata *pdata;
+	int i, ret = 0;
+
+	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
+	if (!pdata)
+		return -ENOMEM;
+	mutex_init(&pdata->lock);
+	platform_set_drvdata(pdev, pdata);
+
+	np = of_get_child_by_name(dev->parent->of_node, "regulators");
+	if (!np)
+		return -ENODEV;
+
+	ret = of_regulator_match(dev, np,
+				 hi6421_regulator_match,
+				 ARRAY_SIZE(hi6421_regulator_match));
+	of_node_put(np);
+	if (ret < 0) {
+		dev_err(dev, "Error parsing regulator init data: %d\n", ret);
+		return ret;
+	}
+
+	pmic = dev_get_drvdata(dev->parent);
+
+	for (i = 0; i < ARRAY_SIZE(hi6421_regulator_info); i++) {
+		ret = hi6421_regulator_register(pdev, pmic->regmap,
+			hi6421_regulator_match[i].init_data, i,
+			hi6421_regulator_match[i].of_node);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static struct platform_driver hi6421_regulator_driver = {
+	.driver = {
+		.name	= "hi6421-regulator",
+		.owner  = THIS_MODULE,
+	},
+	.probe	= hi6421_regulator_probe,
+};
+module_platform_driver(hi6421_regulator_driver);
+
+MODULE_AUTHOR("Guodong Xu <guodong.xu@linaro.org>");
+MODULE_DESCRIPTION("Hi6421 regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/internal.h b/drivers/regulator/internal.h
index 84bbda10c396..80ba2a35a04b 100644
--- a/drivers/regulator/internal.h
+++ b/drivers/regulator/internal.h
@@ -35,4 +35,18 @@ struct regulator {
 	struct dentry *debugfs;
 };
 
+#ifdef CONFIG_OF
+struct regulator_init_data *regulator_of_get_init_data(struct device *dev,
+			         const struct regulator_desc *desc,
+				 struct device_node **node);
+#else
+static inline struct regulator_init_data *
+regulator_of_get_init_data(struct device *dev,
+			   const struct regulator_desc *desc,
+			   struct device_node **node)
+{
+	return NULL;
+}
+#endif
+
 #endif
diff --git a/drivers/regulator/isl9305.c b/drivers/regulator/isl9305.c
new file mode 100644
index 000000000000..92fefd98da58
--- /dev/null
+++ b/drivers/regulator/isl9305.c
@@ -0,0 +1,207 @@
+/*
+ * isl9305 - Intersil ISL9305 DCDC regulator
+ *
+ * Copyright 2014 Linaro Ltd
+ *
+ * Author: Mark Brown <broonie@kernel.org>
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ */
+
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/of.h>
+#include <linux/platform_data/isl9305.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/slab.h>
+
+/*
+ * Registers
+ */
+#define ISL9305_DCD1OUT          0x0
+#define ISL9305_DCD2OUT          0x1
+#define ISL9305_LDO1OUT          0x2
+#define ISL9305_LDO2OUT          0x3
+#define ISL9305_DCD_PARAMETER    0x4
+#define ISL9305_SYSTEM_PARAMETER 0x5
+#define ISL9305_DCD_SRCTL        0x6
+
+#define ISL9305_MAX_REG ISL9305_DCD_SRCTL
+
+/*
+ * DCD_PARAMETER
+ */
+#define ISL9305_DCD_PHASE   0x40
+#define ISL9305_DCD2_ULTRA  0x20
+#define ISL9305_DCD1_ULTRA  0x10
+#define ISL9305_DCD2_BLD    0x08
+#define ISL9305_DCD1_BLD    0x04
+#define ISL9305_DCD2_MODE   0x02
+#define ISL9305_DCD1_MODE   0x01
+
+/*
+ * SYSTEM_PARAMETER
+ */
+#define ISL9305_I2C_EN      0x40
+#define ISL9305_DCDPOR_MASK 0x30
+#define ISL9305_LDO2_EN     0x08
+#define ISL9305_LDO1_EN     0x04
+#define ISL9305_DCD2_EN     0x02
+#define ISL9305_DCD1_EN     0x01
+
+/*
+ * DCD_SRCTL
+ */
+#define ISL9305_DCD2SR_MASK 0xc0
+#define ISL9305_DCD1SR_MASK 0x07
+
+static const struct regulator_ops isl9305_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+	.list_voltage = regulator_list_voltage_linear,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+};
+
+static const struct regulator_desc isl9305_regulators[] = {
+	[ISL9305_DCD1] = {
+		.name =		"DCD1",
+		.of_match =	of_match_ptr("dcd1"),
+		.regulators_node = of_match_ptr("regulators"),
+		.n_voltages =	0x70,
+		.min_uV =	825000,
+		.uV_step =	25000,
+		.vsel_reg =	ISL9305_DCD1OUT,
+		.vsel_mask =	0x7f,
+		.enable_reg =	ISL9305_SYSTEM_PARAMETER,
+		.enable_mask =	ISL9305_DCD1_EN,
+		.supply_name =	"VINDCD1",
+		.ops =		&isl9305_ops,
+	},
+	[ISL9305_DCD2] = {
+		.name =		"DCD2",
+		.of_match =	of_match_ptr("dcd2"),
+		.regulators_node = of_match_ptr("regulators"),
+		.n_voltages =	0x70,
+		.min_uV =	825000,
+		.uV_step =	25000,
+		.vsel_reg =	ISL9305_DCD2OUT,
+		.vsel_mask =	0x7f,
+		.enable_reg =	ISL9305_SYSTEM_PARAMETER,
+		.enable_mask =	ISL9305_DCD2_EN,
+		.supply_name =	"VINDCD2",
+		.ops =		&isl9305_ops,
+	},
+	[ISL9305_LDO1] = {
+		.name =		"LDO1",
+		.of_match =	of_match_ptr("ldo1"),
+		.regulators_node = of_match_ptr("regulators"),
+		.n_voltages =	0x37,
+		.min_uV =	900000,
+		.uV_step =	50000,
+		.vsel_reg =	ISL9305_LDO1OUT,
+		.vsel_mask =	0x3f,
+		.enable_reg =	ISL9305_SYSTEM_PARAMETER,
+		.enable_mask =	ISL9305_LDO1_EN,
+		.supply_name =	"VINLDO1",
+		.ops =		&isl9305_ops,
+	},
+	[ISL9305_LDO2] = {
+		.name =		"LDO2",
+		.of_match =	of_match_ptr("ldo2"),
+		.regulators_node = of_match_ptr("regulators"),
+		.n_voltages =	0x37,
+		.min_uV =	900000,
+		.uV_step =	50000,
+		.vsel_reg =	ISL9305_LDO2OUT,
+		.vsel_mask =	0x3f,
+		.enable_reg =	ISL9305_SYSTEM_PARAMETER,
+		.enable_mask =	ISL9305_LDO2_EN,
+		.supply_name =	"VINLDO2",
+		.ops =		&isl9305_ops,
+	},
+};
+
+static const struct regmap_config isl9305_regmap = {
+	.reg_bits = 8,
+	.val_bits = 8,
+
+	.max_register = ISL9305_MAX_REG,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+static int isl9305_i2c_probe(struct i2c_client *i2c,
+			     const struct i2c_device_id *id)
+{
+	struct regulator_config config = { };
+	struct isl9305_pdata *pdata = i2c->dev.platform_data;
+	struct regulator_dev *rdev;
+	struct regmap *regmap;
+	int i, ret;
+
+	regmap = devm_regmap_init_i2c(i2c, &isl9305_regmap);
+	if (IS_ERR(regmap)) {
+		ret = PTR_ERR(regmap);
+		dev_err(&i2c->dev, "Failed to create regmap: %d\n", ret);
+		return ret;
+	}
+
+	config.dev = &i2c->dev;
+
+	for (i = 0; i < ARRAY_SIZE(isl9305_regulators); i++) {
+		if (pdata)
+			config.init_data = pdata->init_data[i];
+		else
+			config.init_data = NULL;
+
+		rdev = devm_regulator_register(&i2c->dev,
+					       &isl9305_regulators[i],
+					       &config);
+		if (IS_ERR(rdev)) {
+			ret = PTR_ERR(rdev);
+			dev_err(&i2c->dev, "Failed to register %s: %d\n",
+				isl9305_regulators[i].name, ret);
+			return ret;
+		}
+	}
+
+	return 0;
+}
+
+#ifdef CONFIG_OF
+static const struct of_device_id isl9305_dt_ids[] = {
+	{ .compatible = "isl,isl9305" },
+	{ .compatible = "isl,isl9305h" },
+	{},
+};
+#endif
+
+static const struct i2c_device_id isl9305_i2c_id[] = {
+	{ "isl9305", },
+	{ "isl9305h", },
+	{ }
+};
+MODULE_DEVICE_TABLE(i2c, isl9305_i2c_id);
+
+static struct i2c_driver isl9305_regulator_driver = {
+	.driver = {
+		.name = "isl9305",
+		.owner = THIS_MODULE,
+		.of_match_table	= of_match_ptr(isl9305_dt_ids),
+	},
+	.probe = isl9305_i2c_probe,
+	.id_table = isl9305_i2c_id,
+};
+
+module_i2c_driver(isl9305_regulator_driver);
+
+MODULE_AUTHOR("Mark Brown");
+MODULE_DESCRIPTION("Intersil ISL9305 DCDC regulator");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/ltc3589.c b/drivers/regulator/ltc3589.c
index c756955bfcc5..0ce8e4e0fa73 100644
--- a/drivers/regulator/ltc3589.c
+++ b/drivers/regulator/ltc3589.c
@@ -372,6 +372,7 @@ static bool ltc3589_volatile_reg(struct device *dev, unsigned int reg)
 	switch (reg) {
 	case LTC3589_IRQSTAT:
 	case LTC3589_PGSTAT:
+	case LTC3589_VCCR:
 		return true;
 	}
 	return false;
diff --git a/drivers/regulator/max14577.c b/drivers/regulator/max14577.c
index 5d9c605cf534..0ff5a20ac958 100644
--- a/drivers/regulator/max14577.c
+++ b/drivers/regulator/max14577.c
@@ -22,42 +22,6 @@
 #include <linux/mfd/max14577-private.h>
 #include <linux/regulator/of_regulator.h>
 
-/*
- * Valid limits of current for max14577 and max77836 chargers.
- * They must correspond to MBCICHWRCL and MBCICHWRCH fields in CHGCTRL4
- * register for given chipset.
- */
-struct maxim_charger_current {
-	/* Minimal current, set in CHGCTRL4/MBCICHWRCL, uA */
-	unsigned int min;
-	/*
-	 * Minimal current when high setting is active,
-	 * set in CHGCTRL4/MBCICHWRCH, uA
-	 */
-	unsigned int high_start;
-	/* Value of one step in high setting, uA */
-	unsigned int high_step;
-	/* Maximum current of high setting, uA */
-	unsigned int max;
-};
-
-/* Table of valid charger currents for different Maxim chipsets */
-static const struct maxim_charger_current maxim_charger_currents[] = {
-	[MAXIM_DEVICE_TYPE_UNKNOWN] = { 0, 0, 0, 0 },
-	[MAXIM_DEVICE_TYPE_MAX14577] = {
-		.min		= MAX14577_REGULATOR_CURRENT_LIMIT_MIN,
-		.high_start	= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START,
-		.high_step	= MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP,
-		.max		= MAX14577_REGULATOR_CURRENT_LIMIT_MAX,
-	},
-	[MAXIM_DEVICE_TYPE_MAX77836] = {
-		.min		= MAX77836_REGULATOR_CURRENT_LIMIT_MIN,
-		.high_start	= MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_START,
-		.high_step	= MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_STEP,
-		.max		= MAX77836_REGULATOR_CURRENT_LIMIT_MAX,
-	},
-};
-
 static int max14577_reg_is_enabled(struct regulator_dev *rdev)
 {
 	int rid = rdev_get_id(rdev);
@@ -103,8 +67,8 @@ static int max14577_reg_get_current_limit(struct regulator_dev *rdev)
 static int max14577_reg_set_current_limit(struct regulator_dev *rdev,
 		int min_uA, int max_uA)
 {
-	int i, current_bits = 0xf;
 	u8 reg_data;
+	int ret;
 	struct max14577 *max14577 = rdev_get_drvdata(rdev);
 	const struct maxim_charger_current *limits =
 		&maxim_charger_currents[max14577->dev_type];
@@ -112,35 +76,9 @@ static int max14577_reg_set_current_limit(struct regulator_dev *rdev,
 	if (rdev_get_id(rdev) != MAX14577_CHARGER)
 		return -EINVAL;
 
-	if (min_uA > limits->max || max_uA < limits->min)
-		return -EINVAL;
-
-	if (max_uA < limits->high_start) {
-		/*
-		 * Less than high_start,
-		 * so set the minimal current (turn only Low Bit off)
-		 */
-		u8 reg_data = 0x0 << CHGCTRL4_MBCICHWRCL_SHIFT;
-		return max14577_update_reg(rdev->regmap,
-				MAX14577_CHG_REG_CHG_CTRL4,
-				CHGCTRL4_MBCICHWRCL_MASK, reg_data);
-	}
-
-	/*
-	 * max_uA is in range: <high_start, inifinite>, so search for
-	 * valid current starting from maximum current.
-	 */
-	for (i = limits->max; i >= limits->high_start; i -= limits->high_step) {
-		if (i <= max_uA)
-			break;
-		current_bits--;
-	}
-	BUG_ON(current_bits < 0); /* Cannot happen */
-
-	/* Turn Low Bit on (use range high_start-max)... */
-	reg_data = 0x1 << CHGCTRL4_MBCICHWRCL_SHIFT;
-	/* and set proper High Bits */
-	reg_data |= current_bits << CHGCTRL4_MBCICHWRCH_SHIFT;
+	ret = maxim_charger_calc_reg_current(limits, min_uA, max_uA, &reg_data);
+	if (ret)
+		return ret;
 
 	return max14577_update_reg(rdev->regmap, MAX14577_CHG_REG_CHG_CTRL4,
 			CHGCTRL4_MBCICHWRCL_MASK | CHGCTRL4_MBCICHWRCH_MASK,
@@ -442,16 +380,6 @@ static struct platform_driver max14577_regulator_driver = {
 
 static int __init max14577_regulator_init(void)
 {
-	/* Check for valid values for charger */
-	BUILD_BUG_ON(MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_START +
-			MAX14577_REGULATOR_CURRENT_LIMIT_HIGH_STEP * 0xf !=
-			MAX14577_REGULATOR_CURRENT_LIMIT_MAX);
-	BUILD_BUG_ON(MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_START +
-			MAX77836_REGULATOR_CURRENT_LIMIT_HIGH_STEP * 0xf !=
-			MAX77836_REGULATOR_CURRENT_LIMIT_MAX);
-	/* Valid charger current values must be provided for each chipset */
-	BUILD_BUG_ON(ARRAY_SIZE(maxim_charger_currents) != MAXIM_DEVICE_TYPE_NUM);
-
 	BUILD_BUG_ON(ARRAY_SIZE(max14577_supported_regulators) != MAX14577_REGULATOR_NUM);
 	BUILD_BUG_ON(ARRAY_SIZE(max77836_supported_regulators) != MAX77836_REGULATOR_NUM);
 
diff --git a/drivers/regulator/max1586.c b/drivers/regulator/max1586.c
index d23d0577754b..86db310d5304 100644
--- a/drivers/regulator/max1586.c
+++ b/drivers/regulator/max1586.c
@@ -24,6 +24,8 @@
 #include <linux/regulator/driver.h>
 #include <linux/slab.h>
 #include <linux/regulator/max1586.h>
+#include <linux/of_device.h>
+#include <linux/regulator/of_regulator.h>
 
 #define MAX1586_V3_MAX_VSEL 31
 #define MAX1586_V6_MAX_VSEL 3
@@ -157,13 +159,87 @@ static struct regulator_desc max1586_reg[] = {
 	},
 };
 
+static int of_get_max1586_platform_data(struct device *dev,
+				 struct max1586_platform_data *pdata)
+{
+	struct max1586_subdev_data *sub;
+	struct of_regulator_match rmatch[ARRAY_SIZE(max1586_reg)];
+	struct device_node *np = dev->of_node;
+	int i, matched;
+
+	if (of_property_read_u32(np, "v3-gain",
+				 &pdata->v3_gain) < 0) {
+		dev_err(dev, "%s has no 'v3-gain' property\n", np->full_name);
+		return -EINVAL;
+	}
+
+	np = of_get_child_by_name(np, "regulators");
+	if (!np) {
+		dev_err(dev, "missing 'regulators' subnode in DT\n");
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(rmatch); i++)
+		rmatch[i].name = max1586_reg[i].name;
+
+	matched = of_regulator_match(dev, np, rmatch, ARRAY_SIZE(rmatch));
+	of_node_put(np);
+	/*
+	 * If matched is 0, ie. neither Output_V3 nor Output_V6 have been found,
+	 * return 0, which signals the normal situation where no subregulator is
+	 * available. This is normal because the max1586 doesn't provide any
+	 * readback support, so the subregulators can't report any status
+	 * anyway.  If matched < 0, return the error.
+	 */
+	if (matched <= 0)
+		return matched;
+
+	pdata->subdevs = devm_kzalloc(dev, sizeof(struct max1586_subdev_data) *
+						matched, GFP_KERNEL);
+	if (!pdata->subdevs)
+		return -ENOMEM;
+
+	pdata->num_subdevs = matched;
+	sub = pdata->subdevs;
+
+	for (i = 0; i < matched; i++) {
+		sub->id = i;
+		sub->name = rmatch[i].of_node->name;
+		sub->platform_data = rmatch[i].init_data;
+		sub++;
+	}
+
+	return 0;
+}
+
+static const struct of_device_id max1586_of_match[] = {
+	{ .compatible = "maxim,max1586", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, max1586_of_match);
+
 static int max1586_pmic_probe(struct i2c_client *client,
 					const struct i2c_device_id *i2c_id)
 {
-	struct max1586_platform_data *pdata = dev_get_platdata(&client->dev);
+	struct max1586_platform_data *pdata, pdata_of;
 	struct regulator_config config = { };
 	struct max1586_data *max1586;
-	int i, id;
+	int i, id, ret;
+	const struct of_device_id *match;
+
+	pdata = dev_get_platdata(&client->dev);
+	if (client->dev.of_node && !pdata) {
+		match = of_match_device(of_match_ptr(max1586_of_match),
+					&client->dev);
+		if (!match) {
+			dev_err(&client->dev, "Error: No device match found\n");
+			return -ENODEV;
+		}
+		ret = of_get_max1586_platform_data(&client->dev, &pdata_of);
+		if (ret < 0)
+			return ret;
+		pdata = &pdata_of;
+	}
 
 	max1586 = devm_kzalloc(&client->dev, sizeof(struct max1586_data),
 			GFP_KERNEL);
@@ -229,6 +305,7 @@ static struct i2c_driver max1586_pmic_driver = {
 	.driver		= {
 		.name	= "max1586",
 		.owner	= THIS_MODULE,
+		.of_match_table = of_match_ptr(max1586_of_match),
 	},
 	.id_table	= max1586_id,
 };
diff --git a/drivers/regulator/max77802.c b/drivers/regulator/max77802.c
new file mode 100644
index 000000000000..d89792b084e9
--- /dev/null
+++ b/drivers/regulator/max77802.c
@@ -0,0 +1,586 @@
+/*
+ * max77802.c - Regulator driver for the Maxim 77802
+ *
+ * Copyright (C) 2013-2014 Google, Inc
+ * Simon Glass <sjg@chromium.org>
+ *
+ * Copyright (C) 2012 Samsung Electronics
+ * Chiwoong Byun <woong.byun@smasung.com>
+ * Jonghwa Lee <jonghwa3.lee@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * This driver is based on max8997.c
+ */
+
+#include <linux/kernel.h>
+#include <linux/bug.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/gpio/consumer.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/mfd/max77686.h>
+#include <linux/mfd/max77686-private.h>
+
+/* Default ramp delay in case it is not manually set */
+#define MAX77802_RAMP_DELAY		100000		/* uV/us */
+
+#define MAX77802_OPMODE_SHIFT_LDO	6
+#define MAX77802_OPMODE_BUCK234_SHIFT	4
+#define MAX77802_OPMODE_MASK		0x3
+
+#define MAX77802_VSEL_MASK		0x3F
+#define MAX77802_DVS_VSEL_MASK		0xFF
+
+#define MAX77802_RAMP_RATE_MASK_2BIT	0xC0
+#define MAX77802_RAMP_RATE_SHIFT_2BIT	6
+#define MAX77802_RAMP_RATE_MASK_4BIT	0xF0
+#define MAX77802_RAMP_RATE_SHIFT_4BIT	4
+
+/* MAX77802 has two register formats: 2-bit and 4-bit */
+static const unsigned int ramp_table_77802_2bit[] = {
+	12500,
+	25000,
+	50000,
+	100000,
+};
+
+static unsigned int ramp_table_77802_4bit[] = {
+	1000,	2000,	3030,	4000,
+	5000,	5880,	7140,	8330,
+	9090,	10000,	11110,	12500,
+	16670,	25000,	50000,	100000,
+};
+
+struct max77802_regulator_prv {
+	unsigned int opmode[MAX77802_REG_MAX];
+};
+
+static int max77802_get_opmode_shift(int id)
+{
+	if (id == MAX77802_BUCK1 || (id >= MAX77802_BUCK5 &&
+				     id <= MAX77802_BUCK10))
+		return 0;
+
+	if (id >= MAX77802_BUCK2 && id <= MAX77802_BUCK4)
+		return MAX77802_OPMODE_BUCK234_SHIFT;
+
+	if (id >= MAX77802_LDO1 && id <= MAX77802_LDO35)
+		return MAX77802_OPMODE_SHIFT_LDO;
+
+	return -EINVAL;
+}
+
+/*
+ * Some BUCKS supports Normal[ON/OFF] mode during suspend
+ *
+ * BUCK 1, 6, 2-4, 5, 7-10 (all)
+ *
+ * The other mode (0x02) will make PWRREQ switch between normal
+ * and low power.
+ */
+static int max77802_buck_set_suspend_disable(struct regulator_dev *rdev)
+{
+	unsigned int val = MAX77802_OPMODE_STANDBY;
+	struct max77802_regulator_prv *max77802 = rdev_get_drvdata(rdev);
+	int id = rdev_get_id(rdev);
+	int shift = max77802_get_opmode_shift(id);
+
+	max77802->opmode[id] = val;
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  rdev->desc->enable_mask, val << shift);
+}
+
+/*
+ * Some LDOs supports LPM-ON/OFF/Normal-ON mode during suspend state
+ * (Enable Control Logic1 by PWRREQ)
+ *
+ * LDOs 2, 4-19, 22-35.
+ *
+ */
+static int max77802_ldo_set_suspend_mode_logic1(struct regulator_dev *rdev,
+						unsigned int mode)
+{
+	struct max77802_regulator_prv *max77802 = rdev_get_drvdata(rdev);
+	int id = rdev_get_id(rdev);
+	unsigned int val;
+	int shift = max77802_get_opmode_shift(id);
+
+	switch (mode) {
+	case REGULATOR_MODE_IDLE:			/* ON in LP Mode */
+		val = MAX77802_OPMODE_LP;
+		break;
+	case REGULATOR_MODE_NORMAL:			/* ON in Normal Mode */
+		val = MAX77802_OPMODE_NORMAL;
+		break;
+	case REGULATOR_MODE_STANDBY:			/* ON/OFF by PWRREQ */
+		val = MAX77802_OPMODE_STANDBY;
+		break;
+	default:
+		dev_warn(&rdev->dev, "%s: regulator mode: 0x%x not supported\n",
+			 rdev->desc->name, mode);
+		return -EINVAL;
+	}
+
+	max77802->opmode[id] = val;
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  rdev->desc->enable_mask, val << shift);
+}
+
+/*
+ * Mode 1 (Output[ON/OFF] by PWRREQ) is not supported on some LDOs
+ * (Enable Control Logic2 by PWRREQ)
+ *
+ * LDOs 1, 20, 21, and 3,
+ *
+ */
+static int max77802_ldo_set_suspend_mode_logic2(struct regulator_dev *rdev,
+						unsigned int mode)
+{
+	struct max77802_regulator_prv *max77802 = rdev_get_drvdata(rdev);
+	int id = rdev_get_id(rdev);
+	unsigned int val;
+	int shift = max77802_get_opmode_shift(id);
+
+	switch (mode) {
+	case REGULATOR_MODE_IDLE:			/* ON in LP Mode */
+		val = MAX77802_OPMODE_LP;
+		break;
+	case REGULATOR_MODE_NORMAL:			/* ON in Normal Mode */
+		val = MAX77802_OPMODE_NORMAL;
+		break;
+	default:
+		dev_warn(&rdev->dev, "%s: regulator mode: 0x%x not supported\n",
+			 rdev->desc->name, mode);
+		return -EINVAL;
+	}
+
+	max77802->opmode[id] = val;
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  rdev->desc->enable_mask, val << shift);
+}
+
+static int max77802_enable(struct regulator_dev *rdev)
+{
+	struct max77802_regulator_prv *max77802 = rdev_get_drvdata(rdev);
+	int id = rdev_get_id(rdev);
+	int shift = max77802_get_opmode_shift(id);
+
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  rdev->desc->enable_mask,
+				  max77802->opmode[id] << shift);
+}
+
+static int max77802_find_ramp_value(struct regulator_dev *rdev,
+				    const unsigned int limits[], int size,
+				    unsigned int ramp_delay)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		if (ramp_delay <= limits[i])
+			return i;
+	}
+
+	/* Use maximum value for no ramp control */
+	dev_warn(&rdev->dev, "%s: ramp_delay: %d not supported, setting 100000\n",
+		 rdev->desc->name, ramp_delay);
+	return size - 1;
+}
+
+/* Used for BUCKs 2-4 */
+static int max77802_set_ramp_delay_2bit(struct regulator_dev *rdev,
+					int ramp_delay)
+{
+	int id = rdev_get_id(rdev);
+	unsigned int ramp_value;
+
+	if (id > MAX77802_BUCK4) {
+			dev_warn(&rdev->dev,
+				 "%s: regulator: ramp delay not supported\n",
+				 rdev->desc->name);
+		return -EINVAL;
+	}
+	ramp_value = max77802_find_ramp_value(rdev, ramp_table_77802_2bit,
+				ARRAY_SIZE(ramp_table_77802_2bit), ramp_delay);
+
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  MAX77802_RAMP_RATE_MASK_2BIT,
+				  ramp_value << MAX77802_RAMP_RATE_SHIFT_2BIT);
+}
+
+/* For BUCK1, 6 */
+static int max77802_set_ramp_delay_4bit(struct regulator_dev *rdev,
+					    int ramp_delay)
+{
+	unsigned int ramp_value;
+
+	ramp_value = max77802_find_ramp_value(rdev, ramp_table_77802_4bit,
+				ARRAY_SIZE(ramp_table_77802_4bit), ramp_delay);
+
+	return regmap_update_bits(rdev->regmap, rdev->desc->enable_reg,
+				  MAX77802_RAMP_RATE_MASK_4BIT,
+				  ramp_value << MAX77802_RAMP_RATE_SHIFT_4BIT);
+}
+
+/*
+ * LDOs 2, 4-19, 22-35
+ */
+static struct regulator_ops max77802_ldo_ops_logic1 = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= max77802_enable,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_suspend_mode	= max77802_ldo_set_suspend_mode_logic1,
+};
+
+/*
+ * LDOs 1, 20, 21, 3
+ */
+static struct regulator_ops max77802_ldo_ops_logic2 = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= max77802_enable,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_suspend_mode	= max77802_ldo_set_suspend_mode_logic2,
+};
+
+/* BUCKS 1, 6 */
+static struct regulator_ops max77802_buck_16_dvs_ops = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= max77802_enable,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_ramp_delay		= max77802_set_ramp_delay_4bit,
+	.set_suspend_disable	= max77802_buck_set_suspend_disable,
+};
+
+/* BUCKs 2-4, 5, 7-10 */
+static struct regulator_ops max77802_buck_dvs_ops = {
+	.list_voltage		= regulator_list_voltage_linear,
+	.map_voltage		= regulator_map_voltage_linear,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.enable			= max77802_enable,
+	.disable		= regulator_disable_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.set_voltage_time_sel	= regulator_set_voltage_time_sel,
+	.set_ramp_delay		= max77802_set_ramp_delay_2bit,
+	.set_suspend_disable	= max77802_buck_set_suspend_disable,
+};
+
+/* LDOs 3-7, 9-14, 18-26, 28, 29, 32-34 */
+#define regulator_77802_desc_p_ldo(num, supply, log)	{		\
+	.name		= "LDO"#num,					\
+	.id		= MAX77802_LDO##num,				\
+	.supply_name	= "inl"#supply,					\
+	.ops		= &max77802_ldo_ops_logic##log,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= 800000,					\
+	.uV_step	= 50000,					\
+	.ramp_delay	= MAX77802_RAMP_DELAY,				\
+	.n_voltages	= 1 << 6,					\
+	.vsel_reg	= MAX77802_REG_LDO1CTRL1 + num - 1,		\
+	.vsel_mask	= MAX77802_VSEL_MASK,				\
+	.enable_reg	= MAX77802_REG_LDO1CTRL1 + num - 1,		\
+	.enable_mask	= MAX77802_OPMODE_MASK << MAX77802_OPMODE_SHIFT_LDO, \
+}
+
+/* LDOs 1, 2, 8, 15, 17, 27, 30, 35 */
+#define regulator_77802_desc_n_ldo(num, supply, log)   {		\
+	.name		= "LDO"#num,					\
+	.id		= MAX77802_LDO##num,				\
+	.supply_name	= "inl"#supply,					\
+	.ops		= &max77802_ldo_ops_logic##log,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= 800000,					\
+	.uV_step	= 25000,					\
+	.ramp_delay	= MAX77802_RAMP_DELAY,				\
+	.n_voltages	= 1 << 6,					\
+	.vsel_reg	= MAX77802_REG_LDO1CTRL1 + num - 1,		\
+	.vsel_mask	= MAX77802_VSEL_MASK,				\
+	.enable_reg	= MAX77802_REG_LDO1CTRL1 + num - 1,		\
+	.enable_mask	= MAX77802_OPMODE_MASK << MAX77802_OPMODE_SHIFT_LDO, \
+}
+
+/* BUCKs 1, 6 */
+#define regulator_77802_desc_16_buck(num)	{		\
+	.name		= "BUCK"#num,					\
+	.id		= MAX77802_BUCK##num,				\
+	.supply_name	= "inb"#num,					\
+	.ops		= &max77802_buck_16_dvs_ops,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= 612500,					\
+	.uV_step	= 6250,						\
+	.ramp_delay	= MAX77802_RAMP_DELAY,				\
+	.n_voltages	= 1 << 8,					\
+	.vsel_reg	= MAX77802_REG_BUCK ## num ## DVS1,		\
+	.vsel_mask	= MAX77802_DVS_VSEL_MASK,			\
+	.enable_reg	= MAX77802_REG_BUCK ## num ## CTRL,		\
+	.enable_mask	= MAX77802_OPMODE_MASK,				\
+}
+
+/* BUCKS 2-4 */
+#define regulator_77802_desc_234_buck(num)	{		\
+	.name		= "BUCK"#num,					\
+	.id		= MAX77802_BUCK##num,				\
+	.supply_name	= "inb"#num,					\
+	.ops		= &max77802_buck_dvs_ops,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= 600000,					\
+	.uV_step	= 6250,						\
+	.ramp_delay	= MAX77802_RAMP_DELAY,				\
+	.n_voltages	= 0x91,						\
+	.vsel_reg	= MAX77802_REG_BUCK ## num ## DVS1,		\
+	.vsel_mask	= MAX77802_DVS_VSEL_MASK,			\
+	.enable_reg	= MAX77802_REG_BUCK ## num ## CTRL1,		\
+	.enable_mask	= MAX77802_OPMODE_MASK <<			\
+				MAX77802_OPMODE_BUCK234_SHIFT,		\
+}
+
+/* BUCK 5 */
+#define regulator_77802_desc_buck5(num)		{		\
+	.name		= "BUCK"#num,					\
+	.id		= MAX77802_BUCK##num,				\
+	.supply_name	= "inb"#num,					\
+	.ops		= &max77802_buck_dvs_ops,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= 750000,					\
+	.uV_step	= 50000,					\
+	.ramp_delay	= MAX77802_RAMP_DELAY,				\
+	.n_voltages	= 1 << 6,					\
+	.vsel_reg	= MAX77802_REG_BUCK5OUT,			\
+	.vsel_mask	= MAX77802_VSEL_MASK,				\
+	.enable_reg	= MAX77802_REG_BUCK5CTRL,			\
+	.enable_mask	= MAX77802_OPMODE_MASK,				\
+}
+
+/* BUCKs 7-10 */
+#define regulator_77802_desc_buck7_10(num)	{		\
+	.name		= "BUCK"#num,					\
+	.id		= MAX77802_BUCK##num,				\
+	.supply_name	= "inb"#num,					\
+	.ops		= &max77802_buck_dvs_ops,			\
+	.type		= REGULATOR_VOLTAGE,				\
+	.owner		= THIS_MODULE,					\
+	.min_uV		= 750000,					\
+	.uV_step	= 50000,					\
+	.ramp_delay	= MAX77802_RAMP_DELAY,				\
+	.n_voltages	= 1 << 6,					\
+	.vsel_reg	= MAX77802_REG_BUCK7OUT + (num - 7) * 3,	\
+	.vsel_mask	= MAX77802_VSEL_MASK,				\
+	.enable_reg	= MAX77802_REG_BUCK7CTRL + (num - 7) * 3,	\
+	.enable_mask	= MAX77802_OPMODE_MASK,				\
+}
+
+static struct regulator_desc regulators[] = {
+	regulator_77802_desc_16_buck(1),
+	regulator_77802_desc_234_buck(2),
+	regulator_77802_desc_234_buck(3),
+	regulator_77802_desc_234_buck(4),
+	regulator_77802_desc_buck5(5),
+	regulator_77802_desc_16_buck(6),
+	regulator_77802_desc_buck7_10(7),
+	regulator_77802_desc_buck7_10(8),
+	regulator_77802_desc_buck7_10(9),
+	regulator_77802_desc_buck7_10(10),
+	regulator_77802_desc_n_ldo(1, 10, 2),
+	regulator_77802_desc_n_ldo(2, 10, 1),
+	regulator_77802_desc_p_ldo(3, 3, 2),
+	regulator_77802_desc_p_ldo(4, 6, 1),
+	regulator_77802_desc_p_ldo(5, 3, 1),
+	regulator_77802_desc_p_ldo(6, 3, 1),
+	regulator_77802_desc_p_ldo(7, 3, 1),
+	regulator_77802_desc_n_ldo(8, 1, 1),
+	regulator_77802_desc_p_ldo(9, 5, 1),
+	regulator_77802_desc_p_ldo(10, 4, 1),
+	regulator_77802_desc_p_ldo(11, 4, 1),
+	regulator_77802_desc_p_ldo(12, 9, 1),
+	regulator_77802_desc_p_ldo(13, 4, 1),
+	regulator_77802_desc_p_ldo(14, 4, 1),
+	regulator_77802_desc_n_ldo(15, 1, 1),
+	regulator_77802_desc_n_ldo(17, 2, 1),
+	regulator_77802_desc_p_ldo(18, 7, 1),
+	regulator_77802_desc_p_ldo(19, 5, 1),
+	regulator_77802_desc_p_ldo(20, 7, 2),
+	regulator_77802_desc_p_ldo(21, 6, 2),
+	regulator_77802_desc_p_ldo(23, 9, 1),
+	regulator_77802_desc_p_ldo(24, 6, 1),
+	regulator_77802_desc_p_ldo(25, 9, 1),
+	regulator_77802_desc_p_ldo(26, 9, 1),
+	regulator_77802_desc_n_ldo(27, 2, 1),
+	regulator_77802_desc_p_ldo(28, 7, 1),
+	regulator_77802_desc_p_ldo(29, 7, 1),
+	regulator_77802_desc_n_ldo(30, 2, 1),
+	regulator_77802_desc_p_ldo(32, 9, 1),
+	regulator_77802_desc_p_ldo(33, 6, 1),
+	regulator_77802_desc_p_ldo(34, 9, 1),
+	regulator_77802_desc_n_ldo(35, 2, 1),
+};
+
+#ifdef CONFIG_OF
+static int max77802_pmic_dt_parse_pdata(struct platform_device *pdev,
+					struct max77686_platform_data *pdata)
+{
+	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	struct device_node *pmic_np, *regulators_np;
+	struct max77686_regulator_data *rdata;
+	struct of_regulator_match rmatch;
+	unsigned int i;
+
+	pmic_np = iodev->dev->of_node;
+	regulators_np = of_get_child_by_name(pmic_np, "regulators");
+	if (!regulators_np) {
+		dev_err(&pdev->dev, "could not find regulators sub-node\n");
+		return -EINVAL;
+	}
+
+	pdata->num_regulators = ARRAY_SIZE(regulators);
+	rdata = devm_kzalloc(&pdev->dev, sizeof(*rdata) *
+			     pdata->num_regulators, GFP_KERNEL);
+	if (!rdata) {
+		of_node_put(regulators_np);
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < pdata->num_regulators; i++) {
+		rmatch.name = regulators[i].name;
+		rmatch.init_data = NULL;
+		rmatch.of_node = NULL;
+		if (of_regulator_match(&pdev->dev, regulators_np, &rmatch,
+				       1) != 1) {
+			dev_warn(&pdev->dev, "No matching regulator for '%s'\n",
+				 rmatch.name);
+			continue;
+		}
+		rdata[i].initdata = rmatch.init_data;
+		rdata[i].of_node = rmatch.of_node;
+		rdata[i].id = regulators[i].id;
+	}
+
+	pdata->regulators = rdata;
+	of_node_put(regulators_np);
+
+	return 0;
+}
+#else
+static int max77802_pmic_dt_parse_pdata(struct platform_device *pdev,
+					struct max77686_platform_data *pdata)
+{
+	return 0;
+}
+#endif /* CONFIG_OF */
+
+static int max77802_pmic_probe(struct platform_device *pdev)
+{
+	struct max77686_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+	struct max77686_platform_data *pdata = dev_get_platdata(iodev->dev);
+	struct max77802_regulator_prv *max77802;
+	int i, ret = 0, val;
+	struct regulator_config config = { };
+
+	/* This is allocated by the MFD driver */
+	if (!pdata) {
+		dev_err(&pdev->dev, "no platform data found for regulator\n");
+		return -ENODEV;
+	}
+
+	max77802 = devm_kzalloc(&pdev->dev,
+				sizeof(struct max77802_regulator_prv),
+				GFP_KERNEL);
+	if (!max77802)
+		return -ENOMEM;
+
+	if (iodev->dev->of_node) {
+		ret = max77802_pmic_dt_parse_pdata(pdev, pdata);
+		if (ret)
+			return ret;
+	}
+
+	config.dev = iodev->dev;
+	config.regmap = iodev->regmap;
+	config.driver_data = max77802;
+	platform_set_drvdata(pdev, max77802);
+
+	for (i = 0; i < MAX77802_REG_MAX; i++) {
+		struct regulator_dev *rdev;
+		int id = pdata->regulators[i].id;
+		int shift = max77802_get_opmode_shift(id);
+
+		config.init_data = pdata->regulators[i].initdata;
+		config.of_node = pdata->regulators[i].of_node;
+
+		ret = regmap_read(iodev->regmap, regulators[i].enable_reg, &val);
+		val = val >> shift & MAX77802_OPMODE_MASK;
+
+		/*
+		 * If the regulator is disabled and the system warm rebooted,
+		 * the hardware reports OFF as the regulator operating mode.
+		 * Default to operating mode NORMAL in that case.
+		 */
+		if (val == MAX77802_OPMODE_OFF)
+			max77802->opmode[id] = MAX77802_OPMODE_NORMAL;
+		else
+			max77802->opmode[id] = val;
+
+		rdev = devm_regulator_register(&pdev->dev,
+					       &regulators[i], &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev,
+				"regulator init failed for %d\n", i);
+			return PTR_ERR(rdev);
+		}
+	}
+
+	return 0;
+}
+
+static const struct platform_device_id max77802_pmic_id[] = {
+	{"max77802-pmic", 0},
+	{ },
+};
+MODULE_DEVICE_TABLE(platform, max77802_pmic_id);
+
+static struct platform_driver max77802_pmic_driver = {
+	.driver = {
+		.name = "max77802-pmic",
+		.owner = THIS_MODULE,
+	},
+	.probe = max77802_pmic_probe,
+	.id_table = max77802_pmic_id,
+};
+
+module_platform_driver(max77802_pmic_driver);
+
+MODULE_DESCRIPTION("MAXIM 77802 Regulator Driver");
+MODULE_AUTHOR("Simon Glass <sjg@chromium.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/max8907-regulator.c b/drivers/regulator/max8907-regulator.c
index 9623e9e290bf..3426be89c9f6 100644
--- a/drivers/regulator/max8907-regulator.c
+++ b/drivers/regulator/max8907-regulator.c
@@ -226,7 +226,7 @@ static int max8907_regulator_parse_dt(struct platform_device *pdev)
 	struct device_node *np, *regulators;
 	int ret;
 
-	np = of_node_get(pdev->dev.parent->of_node);
+	np = pdev->dev.parent->of_node;
 	if (!np)
 		return 0;
 
diff --git a/drivers/regulator/max8925-regulator.c b/drivers/regulator/max8925-regulator.c
index dad2bcd14e96..7770777befc4 100644
--- a/drivers/regulator/max8925-regulator.c
+++ b/drivers/regulator/max8925-regulator.c
@@ -250,7 +250,7 @@ static int max8925_regulator_dt_init(struct platform_device *pdev,
 	struct device_node *nproot, *np;
 	int rcount;
 
-	nproot = of_node_get(pdev->dev.parent->of_node);
+	nproot = pdev->dev.parent->of_node;
 	if (!nproot)
 		return -ENODEV;
 	np = of_get_child_by_name(nproot, "regulators");
diff --git a/drivers/regulator/max8997.c b/drivers/regulator/max8997.c
index 90b4c530dee5..9c31e215a521 100644
--- a/drivers/regulator/max8997.c
+++ b/drivers/regulator/max8997.c
@@ -917,7 +917,7 @@ static int max8997_pmic_dt_parse_pdata(struct platform_device *pdev,
 	struct max8997_regulator_data *rdata;
 	unsigned int i, dvs_voltage_nr = 1, ret;
 
-	pmic_np = of_node_get(iodev->dev->of_node);
+	pmic_np = iodev->dev->of_node;
 	if (!pmic_np) {
 		dev_err(&pdev->dev, "could not find pmic sub-node\n");
 		return -ENODEV;
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index f374fa57220f..793b662a1967 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -526,6 +526,7 @@ static unsigned int mc13892_vcam_get_mode(struct regulator_dev *rdev)
 	return REGULATOR_MODE_NORMAL;
 }
 
+static struct regulator_ops mc13892_vcam_ops;
 
 static int mc13892_regulator_probe(struct platform_device *pdev)
 {
@@ -582,10 +583,12 @@ static int mc13892_regulator_probe(struct platform_device *pdev)
 	}
 	mc13xxx_unlock(mc13892);
 
-	mc13892_regulators[MC13892_VCAM].desc.ops->set_mode
-		= mc13892_vcam_set_mode;
-	mc13892_regulators[MC13892_VCAM].desc.ops->get_mode
-		= mc13892_vcam_get_mode;
+	/* update mc13892_vcam ops */
+	memcpy(&mc13892_vcam_ops, mc13892_regulators[MC13892_VCAM].desc.ops,
+						sizeof(struct regulator_ops));
+	mc13892_vcam_ops.set_mode = mc13892_vcam_set_mode,
+	mc13892_vcam_ops.get_mode = mc13892_vcam_get_mode,
+	mc13892_regulators[MC13892_VCAM].desc.ops = &mc13892_vcam_ops;
 
 	mc13xxx_data = mc13xxx_parse_regulators_dt(pdev, mc13892_regulators,
 					ARRAY_SIZE(mc13892_regulators));
diff --git a/drivers/regulator/of_regulator.c b/drivers/regulator/of_regulator.c
index ee5e67bc8d5b..7a51814abdc5 100644
--- a/drivers/regulator/of_regulator.c
+++ b/drivers/regulator/of_regulator.c
@@ -14,8 +14,11 @@
 #include <linux/slab.h>
 #include <linux/of.h>
 #include <linux/regulator/machine.h>
+#include <linux/regulator/driver.h>
 #include <linux/regulator/of_regulator.h>
 
+#include "internal.h"
+
 static void of_get_regulation_constraints(struct device_node *np,
 					struct regulator_init_data **init_data)
 {
@@ -189,3 +192,51 @@ int of_regulator_match(struct device *dev, struct device_node *node,
 	return count;
 }
 EXPORT_SYMBOL_GPL(of_regulator_match);
+
+struct regulator_init_data *regulator_of_get_init_data(struct device *dev,
+					    const struct regulator_desc *desc,
+					    struct device_node **node)
+{
+	struct device_node *search, *child;
+	struct regulator_init_data *init_data = NULL;
+	const char *name;
+
+	if (!dev->of_node || !desc->of_match)
+		return NULL;
+
+	if (desc->regulators_node)
+		search = of_get_child_by_name(dev->of_node,
+					      desc->regulators_node);
+	else
+		search = dev->of_node;
+
+	if (!search) {
+		dev_err(dev, "Failed to find regulator container node\n");
+		return NULL;
+	}
+
+	for_each_child_of_node(search, child) {
+		name = of_get_property(child, "regulator-compatible", NULL);
+		if (!name)
+			name = child->name;
+
+		if (strcmp(desc->of_match, name))
+			continue;
+
+		init_data = of_get_regulator_init_data(dev, child);
+		if (!init_data) {
+			dev_err(dev,
+				"failed to parse DT for regulator %s\n",
+				child->name);
+			break;
+		}
+
+		of_node_get(child);
+		*node = child;
+		break;
+	}
+
+	of_node_put(search);
+
+	return init_data;
+}
diff --git a/drivers/regulator/palmas-regulator.c b/drivers/regulator/palmas-regulator.c
index a7ce34d1b5f2..1878e5b567ef 100644
--- a/drivers/regulator/palmas-regulator.c
+++ b/drivers/regulator/palmas-regulator.c
@@ -1427,7 +1427,6 @@ static void palmas_dt_to_pdata(struct device *dev,
 	u32 prop;
 	int idx, ret;
 
-	node = of_node_get(node);
 	regulators = of_get_child_by_name(node, "regulators");
 	if (!regulators) {
 		dev_info(dev, "regulator node not found\n");
diff --git a/drivers/regulator/pwm-regulator.c b/drivers/regulator/pwm-regulator.c
new file mode 100644
index 000000000000..d3f55eaea058
--- /dev/null
+++ b/drivers/regulator/pwm-regulator.c
@@ -0,0 +1,197 @@
+/*
+ * Regulator driver for PWM Regulators
+ *
+ * Copyright (C) 2014 - STMicroelectronics Inc.
+ *
+ * Author: Lee Jones <lee.jones@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/pwm.h>
+
+struct pwm_regulator_data {
+	struct regulator_desc desc;
+	struct pwm_voltages *duty_cycle_table;
+	struct pwm_device *pwm;
+	bool enabled;
+	int state;
+};
+
+struct pwm_voltages {
+	unsigned int uV;
+	unsigned int dutycycle;
+};
+
+static int pwm_regulator_get_voltage_sel(struct regulator_dev *dev)
+{
+	struct pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
+
+	return drvdata->state;
+}
+
+static int pwm_regulator_set_voltage_sel(struct regulator_dev *dev,
+					 unsigned selector)
+{
+	struct pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
+	unsigned int pwm_reg_period;
+	int dutycycle;
+	int ret;
+
+	pwm_reg_period = pwm_get_period(drvdata->pwm);
+
+	dutycycle = (pwm_reg_period *
+		    drvdata->duty_cycle_table[selector].dutycycle) / 100;
+
+	ret = pwm_config(drvdata->pwm, dutycycle, pwm_reg_period);
+	if (ret) {
+		dev_err(&dev->dev, "Failed to configure PWM\n");
+		return ret;
+	}
+
+	drvdata->state = selector;
+
+	if (!drvdata->enabled) {
+		ret = pwm_enable(drvdata->pwm);
+		if (ret) {
+			dev_err(&dev->dev, "Failed to enable PWM\n");
+			return ret;
+		}
+		drvdata->enabled = true;
+	}
+
+	return 0;
+}
+
+static int pwm_regulator_list_voltage(struct regulator_dev *dev,
+				      unsigned selector)
+{
+	struct pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
+
+	if (selector >= drvdata->desc.n_voltages)
+		return -EINVAL;
+
+	return drvdata->duty_cycle_table[selector].uV;
+}
+
+static struct regulator_ops pwm_regulator_voltage_ops = {
+	.set_voltage_sel = pwm_regulator_set_voltage_sel,
+	.get_voltage_sel = pwm_regulator_get_voltage_sel,
+	.list_voltage    = pwm_regulator_list_voltage,
+	.map_voltage     = regulator_map_voltage_iterate,
+};
+
+static const struct regulator_desc pwm_regulator_desc = {
+	.name		= "pwm-regulator",
+	.ops		= &pwm_regulator_voltage_ops,
+	.type		= REGULATOR_VOLTAGE,
+	.owner		= THIS_MODULE,
+	.supply_name    = "pwm",
+};
+
+static int pwm_regulator_probe(struct platform_device *pdev)
+{
+	struct pwm_regulator_data *drvdata;
+	struct property *prop;
+	struct regulator_dev *regulator;
+	struct regulator_config config = { };
+	struct device_node *np = pdev->dev.of_node;
+	int length, ret;
+
+	if (!np) {
+		dev_err(&pdev->dev, "Device Tree node missing\n");
+		return -EINVAL;
+	}
+
+	drvdata = devm_kzalloc(&pdev->dev, sizeof(*drvdata), GFP_KERNEL);
+	if (!drvdata)
+		return -ENOMEM;
+
+	memcpy(&drvdata->desc, &pwm_regulator_desc, sizeof(pwm_regulator_desc));
+
+	/* determine the number of voltage-table */
+	prop = of_find_property(np, "voltage-table", &length);
+	if (!prop) {
+		dev_err(&pdev->dev, "No voltage-table\n");
+		return -EINVAL;
+	}
+
+	if ((length < sizeof(*drvdata->duty_cycle_table)) ||
+	    (length % sizeof(*drvdata->duty_cycle_table))) {
+		dev_err(&pdev->dev, "voltage-table length(%d) is invalid\n",
+			length);
+		return -EINVAL;
+	}
+
+	drvdata->desc.n_voltages = length / sizeof(*drvdata->duty_cycle_table);
+
+	drvdata->duty_cycle_table = devm_kzalloc(&pdev->dev,
+						 length, GFP_KERNEL);
+	if (!drvdata->duty_cycle_table)
+		return -ENOMEM;
+
+	/* read voltage table from DT property */
+	ret = of_property_read_u32_array(np, "voltage-table",
+					 (u32 *)drvdata->duty_cycle_table,
+					 length / sizeof(u32));
+	if (ret < 0) {
+		dev_err(&pdev->dev, "read voltage-table failed\n");
+		return ret;
+	}
+
+	config.init_data = of_get_regulator_init_data(&pdev->dev, np);
+	if (!config.init_data)
+		return -ENOMEM;
+
+	config.of_node = np;
+	config.dev = &pdev->dev;
+	config.driver_data = drvdata;
+
+	drvdata->pwm = devm_pwm_get(&pdev->dev, NULL);
+	if (IS_ERR(drvdata->pwm)) {
+		dev_err(&pdev->dev, "Failed to get PWM\n");
+		return PTR_ERR(drvdata->pwm);
+	}
+
+	regulator = devm_regulator_register(&pdev->dev,
+					    &drvdata->desc, &config);
+	if (IS_ERR(regulator)) {
+		dev_err(&pdev->dev, "Failed to register regulator %s\n",
+			drvdata->desc.name);
+		return PTR_ERR(regulator);
+	}
+
+	return 0;
+}
+
+static const struct of_device_id pwm_of_match[] = {
+	{ .compatible = "pwm-regulator" },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, pwm_of_match);
+
+static struct platform_driver pwm_regulator_driver = {
+	.driver = {
+		.name		= "pwm-regulator",
+		.owner		= THIS_MODULE,
+		.of_match_table = of_match_ptr(pwm_of_match),
+	},
+	.probe = pwm_regulator_probe,
+};
+
+module_platform_driver(pwm_regulator_driver);
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Lee Jones <lee.jones@linaro.org>");
+MODULE_DESCRIPTION("PWM Regulator Driver");
+MODULE_ALIAS("platform:pwm-regulator");
diff --git a/drivers/regulator/qcom_rpm-regulator.c b/drivers/regulator/qcom_rpm-regulator.c
new file mode 100644
index 000000000000..b55cd5b50ebe
--- /dev/null
+++ b/drivers/regulator/qcom_rpm-regulator.c
@@ -0,0 +1,798 @@
+/*
+ * Copyright (c) 2014, Sony Mobile Communications AB.
+ * Copyright (c) 2012-2013, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/regulator/of_regulator.h>
+#include <linux/mfd/qcom_rpm.h>
+
+#include <dt-bindings/mfd/qcom-rpm.h>
+
+#define MAX_REQUEST_LEN 2
+
+struct request_member {
+	int		word;
+	unsigned int	mask;
+	int		shift;
+};
+
+struct rpm_reg_parts {
+	struct request_member mV;		/* used if voltage is in mV */
+	struct request_member uV;		/* used if voltage is in uV */
+	struct request_member ip;		/* peak current in mA */
+	struct request_member pd;		/* pull down enable */
+	struct request_member ia;		/* average current in mA */
+	struct request_member fm;		/* force mode */
+	struct request_member pm;		/* power mode */
+	struct request_member pc;		/* pin control */
+	struct request_member pf;		/* pin function */
+	struct request_member enable_state;	/* NCP and switch */
+	struct request_member comp_mode;	/* NCP */
+	struct request_member freq;		/* frequency: NCP and SMPS */
+	struct request_member freq_clk_src;	/* clock source: SMPS */
+	struct request_member hpm;		/* switch: control OCP and SS */
+	int request_len;
+};
+
+#define FORCE_MODE_IS_2_BITS(reg) \
+	(((reg)->parts->fm.mask >> (reg)->parts->fm.shift) == 3)
+
+struct qcom_rpm_reg {
+	struct qcom_rpm *rpm;
+
+	struct mutex lock;
+	struct device *dev;
+	struct regulator_desc desc;
+	const struct rpm_reg_parts *parts;
+
+	int resource;
+	u32 val[MAX_REQUEST_LEN];
+
+	int uV;
+	int is_enabled;
+
+	bool supports_force_mode_auto;
+	bool supports_force_mode_bypass;
+};
+
+static const struct rpm_reg_parts rpm8660_ldo_parts = {
+	.request_len    = 2,
+	.mV             = { 0, 0x00000FFF,  0 },
+	.ip             = { 0, 0x00FFF000, 12 },
+	.fm             = { 0, 0x03000000, 24 },
+	.pc             = { 0, 0x3C000000, 26 },
+	.pf             = { 0, 0xC0000000, 30 },
+	.pd             = { 1, 0x00000001,  0 },
+	.ia             = { 1, 0x00001FFE,  1 },
+};
+
+static const struct rpm_reg_parts rpm8660_smps_parts = {
+	.request_len    = 2,
+	.mV             = { 0, 0x00000FFF,  0 },
+	.ip             = { 0, 0x00FFF000, 12 },
+	.fm             = { 0, 0x03000000, 24 },
+	.pc             = { 0, 0x3C000000, 26 },
+	.pf             = { 0, 0xC0000000, 30 },
+	.pd             = { 1, 0x00000001,  0 },
+	.ia             = { 1, 0x00001FFE,  1 },
+	.freq           = { 1, 0x001FE000, 13 },
+	.freq_clk_src   = { 1, 0x00600000, 21 },
+};
+
+static const struct rpm_reg_parts rpm8660_switch_parts = {
+	.request_len    = 1,
+	.enable_state   = { 0, 0x00000001,  0 },
+	.pd             = { 0, 0x00000002,  1 },
+	.pc             = { 0, 0x0000003C,  2 },
+	.pf             = { 0, 0x000000C0,  6 },
+	.hpm            = { 0, 0x00000300,  8 },
+};
+
+static const struct rpm_reg_parts rpm8660_ncp_parts = {
+	.request_len    = 1,
+	.mV             = { 0, 0x00000FFF,  0 },
+	.enable_state   = { 0, 0x00001000, 12 },
+	.comp_mode      = { 0, 0x00002000, 13 },
+	.freq           = { 0, 0x003FC000, 14 },
+};
+
+static const struct rpm_reg_parts rpm8960_ldo_parts = {
+	.request_len    = 2,
+	.uV             = { 0, 0x007FFFFF,  0 },
+	.pd             = { 0, 0x00800000, 23 },
+	.pc             = { 0, 0x0F000000, 24 },
+	.pf             = { 0, 0xF0000000, 28 },
+	.ip             = { 1, 0x000003FF,  0 },
+	.ia             = { 1, 0x000FFC00, 10 },
+	.fm             = { 1, 0x00700000, 20 },
+};
+
+static const struct rpm_reg_parts rpm8960_smps_parts = {
+	.request_len    = 2,
+	.uV             = { 0, 0x007FFFFF,  0 },
+	.pd             = { 0, 0x00800000, 23 },
+	.pc             = { 0, 0x0F000000, 24 },
+	.pf             = { 0, 0xF0000000, 28 },
+	.ip             = { 1, 0x000003FF,  0 },
+	.ia             = { 1, 0x000FFC00, 10 },
+	.fm             = { 1, 0x00700000, 20 },
+	.pm             = { 1, 0x00800000, 23 },
+	.freq           = { 1, 0x1F000000, 24 },
+	.freq_clk_src   = { 1, 0x60000000, 29 },
+};
+
+static const struct rpm_reg_parts rpm8960_switch_parts = {
+	.request_len    = 1,
+	.enable_state   = { 0, 0x00000001,  0 },
+	.pd             = { 0, 0x00000002,  1 },
+	.pc             = { 0, 0x0000003C,  2 },
+	.pf             = { 0, 0x000003C0,  6 },
+	.hpm            = { 0, 0x00000C00, 10 },
+};
+
+static const struct rpm_reg_parts rpm8960_ncp_parts = {
+	.request_len    = 1,
+	.uV             = { 0, 0x007FFFFF,  0 },
+	.enable_state   = { 0, 0x00800000, 23 },
+	.comp_mode      = { 0, 0x01000000, 24 },
+	.freq           = { 0, 0x3E000000, 25 },
+};
+
+/*
+ * Physically available PMIC regulator voltage ranges
+ */
+static const struct regulator_linear_range pldo_ranges[] = {
+	REGULATOR_LINEAR_RANGE( 750000,   0,  59, 12500),
+	REGULATOR_LINEAR_RANGE(1500000,  60, 123, 25000),
+	REGULATOR_LINEAR_RANGE(3100000, 124, 160, 50000),
+};
+
+static const struct regulator_linear_range nldo_ranges[] = {
+	REGULATOR_LINEAR_RANGE( 750000,   0,  63, 12500),
+};
+
+static const struct regulator_linear_range nldo1200_ranges[] = {
+	REGULATOR_LINEAR_RANGE( 375000,   0,  59,  6250),
+	REGULATOR_LINEAR_RANGE( 750000,  60, 123, 12500),
+};
+
+static const struct regulator_linear_range smps_ranges[] = {
+	REGULATOR_LINEAR_RANGE( 375000,   0,  29, 12500),
+	REGULATOR_LINEAR_RANGE( 750000,  30,  89, 12500),
+	REGULATOR_LINEAR_RANGE(1500000,  90, 153, 25000),
+};
+
+static const struct regulator_linear_range ftsmps_ranges[] = {
+	REGULATOR_LINEAR_RANGE( 350000,   0,   6, 50000),
+	REGULATOR_LINEAR_RANGE( 700000,   7,  63, 12500),
+	REGULATOR_LINEAR_RANGE(1500000,  64, 100, 50000),
+};
+
+static const struct regulator_linear_range ncp_ranges[] = {
+	REGULATOR_LINEAR_RANGE(1500000,   0,  31, 50000),
+};
+
+static int rpm_reg_write(struct qcom_rpm_reg *vreg,
+			 const struct request_member *req,
+			 const int value)
+{
+	if (WARN_ON((value << req->shift) & ~req->mask))
+		return -EINVAL;
+
+	vreg->val[req->word] &= ~req->mask;
+	vreg->val[req->word] |= value << req->shift;
+
+	return qcom_rpm_write(vreg->rpm,
+			      vreg->resource,
+			      vreg->val,
+			      vreg->parts->request_len);
+}
+
+static int rpm_reg_set_mV_sel(struct regulator_dev *rdev,
+			      unsigned selector)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->mV;
+	int ret = 0;
+	int uV;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	uV = regulator_list_voltage_linear_range(rdev, selector);
+	if (uV < 0)
+		return uV;
+
+	mutex_lock(&vreg->lock);
+	vreg->uV = uV;
+	if (vreg->is_enabled)
+		ret = rpm_reg_write(vreg, req, vreg->uV / 1000);
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_set_uV_sel(struct regulator_dev *rdev,
+			      unsigned selector)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->uV;
+	int ret = 0;
+	int uV;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	uV = regulator_list_voltage_linear_range(rdev, selector);
+	if (uV < 0)
+		return uV;
+
+	mutex_lock(&vreg->lock);
+	vreg->uV = uV;
+	if (vreg->is_enabled)
+		ret = rpm_reg_write(vreg, req, vreg->uV);
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_get_voltage(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+
+	return vreg->uV;
+}
+
+static int rpm_reg_mV_enable(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->mV;
+	int ret;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	mutex_lock(&vreg->lock);
+	ret = rpm_reg_write(vreg, req, vreg->uV / 1000);
+	if (!ret)
+		vreg->is_enabled = 1;
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_uV_enable(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->uV;
+	int ret;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	mutex_lock(&vreg->lock);
+	ret = rpm_reg_write(vreg, req, vreg->uV);
+	if (!ret)
+		vreg->is_enabled = 1;
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_switch_enable(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->enable_state;
+	int ret;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	mutex_lock(&vreg->lock);
+	ret = rpm_reg_write(vreg, req, 1);
+	if (!ret)
+		vreg->is_enabled = 1;
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_mV_disable(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->mV;
+	int ret;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	mutex_lock(&vreg->lock);
+	ret = rpm_reg_write(vreg, req, 0);
+	if (!ret)
+		vreg->is_enabled = 0;
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_uV_disable(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->uV;
+	int ret;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	mutex_lock(&vreg->lock);
+	ret = rpm_reg_write(vreg, req, 0);
+	if (!ret)
+		vreg->is_enabled = 0;
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_switch_disable(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+	const struct rpm_reg_parts *parts = vreg->parts;
+	const struct request_member *req = &parts->enable_state;
+	int ret;
+
+	if (req->mask == 0)
+		return -EINVAL;
+
+	mutex_lock(&vreg->lock);
+	ret = rpm_reg_write(vreg, req, 0);
+	if (!ret)
+		vreg->is_enabled = 0;
+	mutex_unlock(&vreg->lock);
+
+	return ret;
+}
+
+static int rpm_reg_is_enabled(struct regulator_dev *rdev)
+{
+	struct qcom_rpm_reg *vreg = rdev_get_drvdata(rdev);
+
+	return vreg->is_enabled;
+}
+
+static struct regulator_ops uV_ops = {
+	.list_voltage = regulator_list_voltage_linear_range,
+
+	.set_voltage_sel = rpm_reg_set_uV_sel,
+	.get_voltage = rpm_reg_get_voltage,
+
+	.enable = rpm_reg_uV_enable,
+	.disable = rpm_reg_uV_disable,
+	.is_enabled = rpm_reg_is_enabled,
+};
+
+static struct regulator_ops mV_ops = {
+	.list_voltage = regulator_list_voltage_linear_range,
+
+	.set_voltage_sel = rpm_reg_set_mV_sel,
+	.get_voltage = rpm_reg_get_voltage,
+
+	.enable = rpm_reg_mV_enable,
+	.disable = rpm_reg_mV_disable,
+	.is_enabled = rpm_reg_is_enabled,
+};
+
+static struct regulator_ops switch_ops = {
+	.enable = rpm_reg_switch_enable,
+	.disable = rpm_reg_switch_disable,
+	.is_enabled = rpm_reg_is_enabled,
+};
+
+/*
+ * PM8058 regulators
+ */
+static const struct qcom_rpm_reg pm8058_pldo = {
+	.desc.linear_ranges = pldo_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(pldo_ranges),
+	.desc.n_voltages = 161,
+	.desc.ops = &mV_ops,
+	.parts = &rpm8660_ldo_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = false,
+};
+
+static const struct qcom_rpm_reg pm8058_nldo = {
+	.desc.linear_ranges = nldo_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(nldo_ranges),
+	.desc.n_voltages = 64,
+	.desc.ops = &mV_ops,
+	.parts = &rpm8660_ldo_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = false,
+};
+
+static const struct qcom_rpm_reg pm8058_smps = {
+	.desc.linear_ranges = smps_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(smps_ranges),
+	.desc.n_voltages = 154,
+	.desc.ops = &mV_ops,
+	.parts = &rpm8660_smps_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = false,
+};
+
+static const struct qcom_rpm_reg pm8058_ncp = {
+	.desc.linear_ranges = ncp_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(ncp_ranges),
+	.desc.n_voltages = 32,
+	.desc.ops = &mV_ops,
+	.parts = &rpm8660_ncp_parts,
+};
+
+static const struct qcom_rpm_reg pm8058_switch = {
+	.desc.ops = &switch_ops,
+	.parts = &rpm8660_switch_parts,
+};
+
+/*
+ * PM8901 regulators
+ */
+static const struct qcom_rpm_reg pm8901_pldo = {
+	.desc.linear_ranges = pldo_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(pldo_ranges),
+	.desc.n_voltages = 161,
+	.desc.ops = &mV_ops,
+	.parts = &rpm8660_ldo_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = true,
+};
+
+static const struct qcom_rpm_reg pm8901_nldo = {
+	.desc.linear_ranges = nldo_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(nldo_ranges),
+	.desc.n_voltages = 64,
+	.desc.ops = &mV_ops,
+	.parts = &rpm8660_ldo_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = true,
+};
+
+static const struct qcom_rpm_reg pm8901_ftsmps = {
+	.desc.linear_ranges = ftsmps_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(ftsmps_ranges),
+	.desc.n_voltages = 101,
+	.desc.ops = &mV_ops,
+	.parts = &rpm8660_smps_parts,
+	.supports_force_mode_auto = true,
+	.supports_force_mode_bypass = false,
+};
+
+static const struct qcom_rpm_reg pm8901_switch = {
+	.desc.ops = &switch_ops,
+	.parts = &rpm8660_switch_parts,
+};
+
+/*
+ * PM8921 regulators
+ */
+static const struct qcom_rpm_reg pm8921_pldo = {
+	.desc.linear_ranges = pldo_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(pldo_ranges),
+	.desc.n_voltages = 161,
+	.desc.ops = &uV_ops,
+	.parts = &rpm8960_ldo_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = true,
+};
+
+static const struct qcom_rpm_reg pm8921_nldo = {
+	.desc.linear_ranges = nldo_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(nldo_ranges),
+	.desc.n_voltages = 64,
+	.desc.ops = &uV_ops,
+	.parts = &rpm8960_ldo_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = true,
+};
+
+static const struct qcom_rpm_reg pm8921_nldo1200 = {
+	.desc.linear_ranges = nldo1200_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(nldo1200_ranges),
+	.desc.n_voltages = 124,
+	.desc.ops = &uV_ops,
+	.parts = &rpm8960_ldo_parts,
+	.supports_force_mode_auto = false,
+	.supports_force_mode_bypass = true,
+};
+
+static const struct qcom_rpm_reg pm8921_smps = {
+	.desc.linear_ranges = smps_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(smps_ranges),
+	.desc.n_voltages = 154,
+	.desc.ops = &uV_ops,
+	.parts = &rpm8960_smps_parts,
+	.supports_force_mode_auto = true,
+	.supports_force_mode_bypass = false,
+};
+
+static const struct qcom_rpm_reg pm8921_ftsmps = {
+	.desc.linear_ranges = ftsmps_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(ftsmps_ranges),
+	.desc.n_voltages = 101,
+	.desc.ops = &uV_ops,
+	.parts = &rpm8960_smps_parts,
+	.supports_force_mode_auto = true,
+	.supports_force_mode_bypass = false,
+};
+
+static const struct qcom_rpm_reg pm8921_ncp = {
+	.desc.linear_ranges = ncp_ranges,
+	.desc.n_linear_ranges = ARRAY_SIZE(ncp_ranges),
+	.desc.n_voltages = 32,
+	.desc.ops = &uV_ops,
+	.parts = &rpm8960_ncp_parts,
+};
+
+static const struct qcom_rpm_reg pm8921_switch = {
+	.desc.ops = &switch_ops,
+	.parts = &rpm8960_switch_parts,
+};
+
+static const struct of_device_id rpm_of_match[] = {
+	{ .compatible = "qcom,rpm-pm8058-pldo",     .data = &pm8058_pldo },
+	{ .compatible = "qcom,rpm-pm8058-nldo",     .data = &pm8058_nldo },
+	{ .compatible = "qcom,rpm-pm8058-smps",     .data = &pm8058_smps },
+	{ .compatible = "qcom,rpm-pm8058-ncp",      .data = &pm8058_ncp },
+	{ .compatible = "qcom,rpm-pm8058-switch",   .data = &pm8058_switch },
+
+	{ .compatible = "qcom,rpm-pm8901-pldo",     .data = &pm8901_pldo },
+	{ .compatible = "qcom,rpm-pm8901-nldo",     .data = &pm8901_nldo },
+	{ .compatible = "qcom,rpm-pm8901-ftsmps",   .data = &pm8901_ftsmps },
+	{ .compatible = "qcom,rpm-pm8901-switch",   .data = &pm8901_switch },
+
+	{ .compatible = "qcom,rpm-pm8921-pldo",     .data = &pm8921_pldo },
+	{ .compatible = "qcom,rpm-pm8921-nldo",     .data = &pm8921_nldo },
+	{ .compatible = "qcom,rpm-pm8921-nldo1200", .data = &pm8921_nldo1200 },
+	{ .compatible = "qcom,rpm-pm8921-smps",     .data = &pm8921_smps },
+	{ .compatible = "qcom,rpm-pm8921-ftsmps",   .data = &pm8921_ftsmps },
+	{ .compatible = "qcom,rpm-pm8921-ncp",      .data = &pm8921_ncp },
+	{ .compatible = "qcom,rpm-pm8921-switch",   .data = &pm8921_switch },
+	{ }
+};
+MODULE_DEVICE_TABLE(of, rpm_of_match);
+
+static int rpm_reg_set(struct qcom_rpm_reg *vreg,
+		       const struct request_member *req,
+		       const int value)
+{
+	if (req->mask == 0 || (value << req->shift) & ~req->mask)
+		return -EINVAL;
+
+	vreg->val[req->word] &= ~req->mask;
+	vreg->val[req->word] |= value << req->shift;
+
+	return 0;
+}
+
+static int rpm_reg_of_parse_freq(struct device *dev, struct qcom_rpm_reg *vreg)
+{
+	static const int freq_table[] = {
+		19200000, 9600000, 6400000, 4800000, 3840000, 3200000, 2740000,
+		2400000, 2130000, 1920000, 1750000, 1600000, 1480000, 1370000,
+		1280000, 1200000,
+
+	};
+	const char *key;
+	u32 freq;
+	int ret;
+	int i;
+
+	key = "qcom,switch-mode-frequency";
+	ret = of_property_read_u32(dev->of_node, key, &freq);
+	if (ret) {
+		dev_err(dev, "regulator requires %s property\n", key);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(freq_table); i++) {
+		if (freq == freq_table[i]) {
+			rpm_reg_set(vreg, &vreg->parts->freq, i + 1);
+			return 0;
+		}
+	}
+
+	dev_err(dev, "invalid frequency %d\n", freq);
+	return -EINVAL;
+}
+
+static int rpm_reg_probe(struct platform_device *pdev)
+{
+	struct regulator_init_data *initdata;
+	const struct qcom_rpm_reg *template;
+	const struct of_device_id *match;
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+	struct qcom_rpm_reg *vreg;
+	const char *key;
+	u32 force_mode;
+	bool pwm;
+	u32 val;
+	int ret;
+
+	match = of_match_device(rpm_of_match, &pdev->dev);
+	template = match->data;
+
+	initdata = of_get_regulator_init_data(&pdev->dev, pdev->dev.of_node);
+	if (!initdata)
+		return -EINVAL;
+
+	vreg = devm_kmalloc(&pdev->dev, sizeof(*vreg), GFP_KERNEL);
+	if (!vreg) {
+		dev_err(&pdev->dev, "failed to allocate vreg\n");
+		return -ENOMEM;
+	}
+	memcpy(vreg, template, sizeof(*vreg));
+	mutex_init(&vreg->lock);
+	vreg->dev = &pdev->dev;
+	vreg->desc.id = -1;
+	vreg->desc.owner = THIS_MODULE;
+	vreg->desc.type = REGULATOR_VOLTAGE;
+	vreg->desc.name = pdev->dev.of_node->name;
+
+	vreg->rpm = dev_get_drvdata(pdev->dev.parent);
+	if (!vreg->rpm) {
+		dev_err(&pdev->dev, "unable to retrieve handle to rpm\n");
+		return -ENODEV;
+	}
+
+	key = "reg";
+	ret = of_property_read_u32(pdev->dev.of_node, key, &val);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to read %s\n", key);
+		return ret;
+	}
+	vreg->resource = val;
+
+	if ((vreg->parts->uV.mask || vreg->parts->mV.mask) &&
+	    (!initdata->constraints.min_uV || !initdata->constraints.max_uV)) {
+		dev_err(&pdev->dev, "no voltage specified for regulator\n");
+		return -EINVAL;
+	}
+
+	key = "bias-pull-down";
+	if (of_property_read_bool(pdev->dev.of_node, key)) {
+		ret = rpm_reg_set(vreg, &vreg->parts->pd, 1);
+		if (ret) {
+			dev_err(&pdev->dev, "%s is invalid", key);
+			return ret;
+		}
+	}
+
+	if (vreg->parts->freq.mask) {
+		ret = rpm_reg_of_parse_freq(&pdev->dev, vreg);
+		if (ret < 0)
+			return ret;
+	}
+
+	if (vreg->parts->pm.mask) {
+		key = "qcom,power-mode-hysteretic";
+		pwm = !of_property_read_bool(pdev->dev.of_node, key);
+
+		ret = rpm_reg_set(vreg, &vreg->parts->pm, pwm);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to set power mode\n");
+			return ret;
+		}
+	}
+
+	if (vreg->parts->fm.mask) {
+		force_mode = -1;
+
+		key = "qcom,force-mode";
+		ret = of_property_read_u32(pdev->dev.of_node, key, &val);
+		if (ret == -EINVAL) {
+			val = QCOM_RPM_FORCE_MODE_NONE;
+		} else if (ret < 0) {
+			dev_err(&pdev->dev, "failed to read %s\n", key);
+			return ret;
+		}
+
+		/*
+		 * If force-mode is encoded as 2 bits then the
+		 * possible register values are:
+		 * NONE, LPM, HPM
+		 * otherwise:
+		 * NONE, LPM, AUTO, HPM, BYPASS
+		 */
+		switch (val) {
+		case QCOM_RPM_FORCE_MODE_NONE:
+			force_mode = 0;
+			break;
+		case QCOM_RPM_FORCE_MODE_LPM:
+			force_mode = 1;
+			break;
+		case QCOM_RPM_FORCE_MODE_HPM:
+			if (FORCE_MODE_IS_2_BITS(vreg))
+				force_mode = 2;
+			else
+				force_mode = 3;
+			break;
+		case QCOM_RPM_FORCE_MODE_AUTO:
+			if (vreg->supports_force_mode_auto)
+				force_mode = 2;
+			break;
+		case QCOM_RPM_FORCE_MODE_BYPASS:
+			if (vreg->supports_force_mode_bypass)
+				force_mode = 4;
+			break;
+		}
+
+		if (force_mode < 0) {
+			dev_err(&pdev->dev, "invalid force mode\n");
+			return -EINVAL;
+		}
+
+		ret = rpm_reg_set(vreg, &vreg->parts->fm, force_mode);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to set force mode\n");
+			return ret;
+		}
+	}
+
+	config.dev = &pdev->dev;
+	config.init_data = initdata;
+	config.driver_data = vreg;
+	config.of_node = pdev->dev.of_node;
+	rdev = devm_regulator_register(&pdev->dev, &vreg->desc, &config);
+	if (IS_ERR(rdev)) {
+		dev_err(&pdev->dev, "can't register regulator\n");
+		return PTR_ERR(rdev);
+	}
+
+	return 0;
+}
+
+static struct platform_driver rpm_reg_driver = {
+	.probe          = rpm_reg_probe,
+	.driver  = {
+		.name  = "qcom_rpm_reg",
+		.owner = THIS_MODULE,
+		.of_match_table = of_match_ptr(rpm_of_match),
+	},
+};
+
+static int __init rpm_reg_init(void)
+{
+	return platform_driver_register(&rpm_reg_driver);
+}
+subsys_initcall(rpm_reg_init);
+
+static void __exit rpm_reg_exit(void)
+{
+	platform_driver_unregister(&rpm_reg_driver);
+}
+module_exit(rpm_reg_exit)
+
+MODULE_DESCRIPTION("Qualcomm RPM regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/rk808-regulator.c b/drivers/regulator/rk808-regulator.c
new file mode 100644
index 000000000000..e305416d7697
--- /dev/null
+++ b/drivers/regulator/rk808-regulator.c
@@ -0,0 +1,381 @@
+/*
+ * Regulator driver for Rockchip RK808
+ *
+ * Copyright (c) 2014, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * Author: Chris Zhong <zyw@rock-chips.com>
+ * Author: Zhang Qing <zhangqing@rock-chips.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/module.h>
+#include <linux/i2c.h>
+#include <linux/mfd/rk808.h>
+#include <linux/of_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+
+/* Field Definitions */
+#define RK808_BUCK_VSEL_MASK	0x3f
+#define RK808_BUCK4_VSEL_MASK	0xf
+#define RK808_LDO_VSEL_MASK	0x1f
+
+/* Ramp rate definitions for buck1 / buck2 only */
+#define RK808_RAMP_RATE_OFFSET		3
+#define RK808_RAMP_RATE_MASK		(3 << RK808_RAMP_RATE_OFFSET)
+#define RK808_RAMP_RATE_2MV_PER_US	(0 << RK808_RAMP_RATE_OFFSET)
+#define RK808_RAMP_RATE_4MV_PER_US	(1 << RK808_RAMP_RATE_OFFSET)
+#define RK808_RAMP_RATE_6MV_PER_US	(2 << RK808_RAMP_RATE_OFFSET)
+#define RK808_RAMP_RATE_10MV_PER_US	(3 << RK808_RAMP_RATE_OFFSET)
+
+static const int rk808_buck_config_regs[] = {
+	RK808_BUCK1_CONFIG_REG,
+	RK808_BUCK2_CONFIG_REG,
+	RK808_BUCK3_CONFIG_REG,
+	RK808_BUCK4_CONFIG_REG,
+};
+
+static const struct regulator_linear_range rk808_buck_voltage_ranges[] = {
+	REGULATOR_LINEAR_RANGE(700000, 0, 63, 12500),
+};
+
+static const struct regulator_linear_range rk808_buck4_voltage_ranges[] = {
+	REGULATOR_LINEAR_RANGE(1800000, 0, 15, 100000),
+};
+
+static const struct regulator_linear_range rk808_ldo_voltage_ranges[] = {
+	REGULATOR_LINEAR_RANGE(1800000, 0, 16, 100000),
+};
+
+static const struct regulator_linear_range rk808_ldo3_voltage_ranges[] = {
+	REGULATOR_LINEAR_RANGE(800000, 0, 13, 100000),
+	REGULATOR_LINEAR_RANGE(2500000, 15, 15, 0),
+};
+
+static const struct regulator_linear_range rk808_ldo6_voltage_ranges[] = {
+	REGULATOR_LINEAR_RANGE(800000, 0, 17, 100000),
+};
+
+static int rk808_set_ramp_delay(struct regulator_dev *rdev, int ramp_delay)
+{
+	unsigned int ramp_value = RK808_RAMP_RATE_10MV_PER_US;
+	unsigned int reg = rk808_buck_config_regs[rdev->desc->id -
+						  RK808_ID_DCDC1];
+
+	switch (ramp_delay) {
+	case 1 ... 2000:
+		ramp_value = RK808_RAMP_RATE_2MV_PER_US;
+		break;
+	case 2001 ... 4000:
+		ramp_value = RK808_RAMP_RATE_4MV_PER_US;
+		break;
+	case 4001 ... 6000:
+		ramp_value = RK808_RAMP_RATE_6MV_PER_US;
+		break;
+	case 6001 ... 10000:
+		break;
+	default:
+		pr_warn("%s ramp_delay: %d not supported, setting 10000\n",
+			rdev->desc->name, ramp_delay);
+	}
+
+	return regmap_update_bits(rdev->regmap, reg,
+				  RK808_RAMP_RATE_MASK, ramp_value);
+}
+
+static struct regulator_ops rk808_buck1_2_ops = {
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.set_ramp_delay		= rk808_set_ramp_delay,
+};
+
+static struct regulator_ops rk808_reg_ops = {
+	.list_voltage		= regulator_list_voltage_linear_range,
+	.map_voltage		= regulator_map_voltage_linear_range,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+};
+
+static struct regulator_ops rk808_switch_ops = {
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+};
+
+static const struct regulator_desc rk808_reg[] = {
+	{
+		.name = "DCDC_REG1",
+		.supply_name = "vcc1",
+		.id = RK808_ID_DCDC1,
+		.ops = &rk808_buck1_2_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 64,
+		.linear_ranges = rk808_buck_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_buck_voltage_ranges),
+		.vsel_reg = RK808_BUCK1_ON_VSEL_REG,
+		.vsel_mask = RK808_BUCK_VSEL_MASK,
+		.enable_reg = RK808_DCDC_EN_REG,
+		.enable_mask = BIT(0),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "DCDC_REG2",
+		.supply_name = "vcc2",
+		.id = RK808_ID_DCDC2,
+		.ops = &rk808_buck1_2_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 64,
+		.linear_ranges = rk808_buck_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_buck_voltage_ranges),
+		.vsel_reg = RK808_BUCK2_ON_VSEL_REG,
+		.vsel_mask = RK808_BUCK_VSEL_MASK,
+		.enable_reg = RK808_DCDC_EN_REG,
+		.enable_mask = BIT(1),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "DCDC_REG3",
+		.supply_name = "vcc3",
+		.id = RK808_ID_DCDC3,
+		.ops = &rk808_switch_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 1,
+		.enable_reg = RK808_DCDC_EN_REG,
+		.enable_mask = BIT(2),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "DCDC_REG4",
+		.supply_name = "vcc4",
+		.id = RK808_ID_DCDC4,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 16,
+		.linear_ranges = rk808_buck4_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_buck4_voltage_ranges),
+		.vsel_reg = RK808_BUCK4_ON_VSEL_REG,
+		.vsel_mask = RK808_BUCK4_VSEL_MASK,
+		.enable_reg = RK808_DCDC_EN_REG,
+		.enable_mask = BIT(3),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG1",
+		.supply_name = "vcc6",
+		.id = RK808_ID_LDO1,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 17,
+		.linear_ranges = rk808_ldo_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo_voltage_ranges),
+		.vsel_reg = RK808_LDO1_ON_VSEL_REG,
+		.vsel_mask = RK808_LDO_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(0),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG2",
+		.supply_name = "vcc6",
+		.id = RK808_ID_LDO2,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 17,
+		.linear_ranges = rk808_ldo_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo_voltage_ranges),
+		.vsel_reg = RK808_LDO2_ON_VSEL_REG,
+		.vsel_mask = RK808_LDO_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(1),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG3",
+		.supply_name = "vcc7",
+		.id = RK808_ID_LDO3,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 16,
+		.linear_ranges = rk808_ldo3_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo3_voltage_ranges),
+		.vsel_reg = RK808_LDO3_ON_VSEL_REG,
+		.vsel_mask = RK808_BUCK4_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(2),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG4",
+		.supply_name = "vcc9",
+		.id = RK808_ID_LDO4,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 17,
+		.linear_ranges = rk808_ldo_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo_voltage_ranges),
+		.vsel_reg = RK808_LDO4_ON_VSEL_REG,
+		.vsel_mask = RK808_LDO_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(3),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG5",
+		.supply_name = "vcc9",
+		.id = RK808_ID_LDO5,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 17,
+		.linear_ranges = rk808_ldo_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo_voltage_ranges),
+		.vsel_reg = RK808_LDO5_ON_VSEL_REG,
+		.vsel_mask = RK808_LDO_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(4),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG6",
+		.supply_name = "vcc10",
+		.id = RK808_ID_LDO6,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 18,
+		.linear_ranges = rk808_ldo6_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo6_voltage_ranges),
+		.vsel_reg = RK808_LDO6_ON_VSEL_REG,
+		.vsel_mask = RK808_LDO_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(5),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG7",
+		.supply_name = "vcc7",
+		.id = RK808_ID_LDO7,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 18,
+		.linear_ranges = rk808_ldo6_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo6_voltage_ranges),
+		.vsel_reg = RK808_LDO7_ON_VSEL_REG,
+		.vsel_mask = RK808_LDO_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(6),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "LDO_REG8",
+		.supply_name = "vcc11",
+		.id = RK808_ID_LDO8,
+		.ops = &rk808_reg_ops,
+		.type = REGULATOR_VOLTAGE,
+		.n_voltages = 17,
+		.linear_ranges = rk808_ldo_voltage_ranges,
+		.n_linear_ranges = ARRAY_SIZE(rk808_ldo_voltage_ranges),
+		.vsel_reg = RK808_LDO8_ON_VSEL_REG,
+		.vsel_mask = RK808_LDO_VSEL_MASK,
+		.enable_reg = RK808_LDO_EN_REG,
+		.enable_mask = BIT(7),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "SWITCH_REG1",
+		.supply_name = "vcc8",
+		.id = RK808_ID_SWITCH1,
+		.ops = &rk808_switch_ops,
+		.type = REGULATOR_VOLTAGE,
+		.enable_reg = RK808_DCDC_EN_REG,
+		.enable_mask = BIT(5),
+		.owner = THIS_MODULE,
+	}, {
+		.name = "SWITCH_REG2",
+		.supply_name = "vcc12",
+		.id = RK808_ID_SWITCH2,
+		.ops = &rk808_switch_ops,
+		.type = REGULATOR_VOLTAGE,
+		.enable_reg = RK808_DCDC_EN_REG,
+		.enable_mask = BIT(6),
+		.owner = THIS_MODULE,
+	},
+};
+
+static struct of_regulator_match rk808_reg_matches[] = {
+	[RK808_ID_DCDC1]	= { .name = "DCDC_REG1" },
+	[RK808_ID_DCDC2]	= { .name = "DCDC_REG2" },
+	[RK808_ID_DCDC3]	= { .name = "DCDC_REG3" },
+	[RK808_ID_DCDC4]	= { .name = "DCDC_REG4" },
+	[RK808_ID_LDO1]		= { .name = "LDO_REG1" },
+	[RK808_ID_LDO2]		= { .name = "LDO_REG2" },
+	[RK808_ID_LDO3]		= { .name = "LDO_REG3" },
+	[RK808_ID_LDO4]		= { .name = "LDO_REG4" },
+	[RK808_ID_LDO5]		= { .name = "LDO_REG5" },
+	[RK808_ID_LDO6]		= { .name = "LDO_REG6" },
+	[RK808_ID_LDO7]		= { .name = "LDO_REG7" },
+	[RK808_ID_LDO8]		= { .name = "LDO_REG8" },
+	[RK808_ID_SWITCH1]	= { .name = "SWITCH_REG1" },
+	[RK808_ID_SWITCH2]	= { .name = "SWITCH_REG2" },
+};
+
+static int rk808_regulator_probe(struct platform_device *pdev)
+{
+	struct rk808 *rk808 = dev_get_drvdata(pdev->dev.parent);
+	struct i2c_client *client = rk808->i2c;
+	struct device_node *reg_np;
+	struct regulator_config config = {};
+	struct regulator_dev *rk808_rdev;
+	int ret, i;
+
+	reg_np = of_get_child_by_name(client->dev.of_node, "regulators");
+	if (!reg_np)
+		return -ENXIO;
+
+	ret = of_regulator_match(&pdev->dev, reg_np, rk808_reg_matches,
+				 RK808_NUM_REGULATORS);
+	of_node_put(reg_np);
+	if (ret < 0)
+		return ret;
+
+	/* Instantiate the regulators */
+	for (i = 0; i < RK808_NUM_REGULATORS; i++) {
+		if (!rk808_reg_matches[i].init_data ||
+		    !rk808_reg_matches[i].of_node)
+			continue;
+
+		config.dev = &client->dev;
+		config.driver_data = rk808;
+		config.regmap = rk808->regmap;
+		config.of_node = rk808_reg_matches[i].of_node;
+		config.init_data = rk808_reg_matches[i].init_data;
+
+		rk808_rdev = devm_regulator_register(&pdev->dev,
+						     &rk808_reg[i], &config);
+		if (IS_ERR(rk808_rdev)) {
+			dev_err(&client->dev,
+				"failed to register %d regulator\n", i);
+			return PTR_ERR(rk808_rdev);
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver rk808_regulator_driver = {
+	.probe = rk808_regulator_probe,
+	.driver = {
+		.name = "rk808-regulator",
+		.owner = THIS_MODULE,
+	},
+};
+
+module_platform_driver(rk808_regulator_driver);
+
+MODULE_DESCRIPTION("regulator driver for the rk808 series PMICs");
+MODULE_AUTHOR("Chris Zhong<zyw@rock-chips.com>");
+MODULE_AUTHOR("Zhang Qing<zhangqing@rock-chips.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rk808-regulator");
diff --git a/drivers/regulator/rn5t618-regulator.c b/drivers/regulator/rn5t618-regulator.c
new file mode 100644
index 000000000000..e58d79aeb393
--- /dev/null
+++ b/drivers/regulator/rn5t618-regulator.c
@@ -0,0 +1,143 @@
+/*
+ * Regulator driver for Ricoh RN5T618 PMIC
+ *
+ * Copyright (C) 2014 Beniamino Galvani <b.galvani@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/mfd/rn5t618.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/regmap.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+
+static struct regulator_ops rn5t618_reg_ops = {
+	.enable			= regulator_enable_regmap,
+	.disable		= regulator_disable_regmap,
+	.is_enabled		= regulator_is_enabled_regmap,
+	.set_voltage_sel	= regulator_set_voltage_sel_regmap,
+	.get_voltage_sel	= regulator_get_voltage_sel_regmap,
+	.list_voltage		= regulator_list_voltage_linear,
+};
+
+#define REG(rid, ereg, emask, vreg, vmask, min, max, step)		\
+	[RN5T618_##rid] = {						\
+		.name		= #rid,					\
+		.id		= RN5T618_##rid,			\
+		.type		= REGULATOR_VOLTAGE,			\
+		.owner		= THIS_MODULE,				\
+		.ops		= &rn5t618_reg_ops,			\
+		.n_voltages	= ((max) - (min)) / (step) + 1,		\
+		.min_uV		= (min),				\
+		.uV_step	= (step),				\
+		.enable_reg	= RN5T618_##ereg,			\
+		.enable_mask	= (emask),				\
+		.vsel_reg	= RN5T618_##vreg,			\
+		.vsel_mask	= (vmask),				\
+	}
+
+static struct regulator_desc rn5t618_regulators[] = {
+	/* DCDC */
+	REG(DCDC1, DC1CTL, BIT(0), DC1DAC, 0xff, 600000, 3500000, 12500),
+	REG(DCDC2, DC2CTL, BIT(0), DC2DAC, 0xff, 600000, 3500000, 12500),
+	REG(DCDC3, DC3CTL, BIT(0), DC3DAC, 0xff, 600000, 3500000, 12500),
+	/* LDO */
+	REG(LDO1, LDOEN1, BIT(0), LDO1DAC, 0x7f, 900000, 3500000, 25000),
+	REG(LDO2, LDOEN1, BIT(1), LDO2DAC, 0x7f, 900000, 3500000, 25000),
+	REG(LDO3, LDOEN1, BIT(2), LDO3DAC, 0x7f, 600000, 3500000, 25000),
+	REG(LDO4, LDOEN1, BIT(3), LDO4DAC, 0x7f, 900000, 3500000, 25000),
+	REG(LDO5, LDOEN1, BIT(4), LDO5DAC, 0x7f, 900000, 3500000, 25000),
+	/* LDO RTC */
+	REG(LDORTC1, LDOEN2, BIT(4), LDORTCDAC, 0x7f, 1700000, 3500000, 25000),
+	REG(LDORTC2, LDOEN2, BIT(5), LDORTC2DAC, 0x7f, 900000, 3500000, 25000),
+};
+
+static struct of_regulator_match rn5t618_matches[] = {
+	[RN5T618_DCDC1]		= { .name = "DCDC1" },
+	[RN5T618_DCDC2]		= { .name = "DCDC2" },
+	[RN5T618_DCDC3]		= { .name = "DCDC3" },
+	[RN5T618_LDO1]		= { .name = "LDO1" },
+	[RN5T618_LDO2]		= { .name = "LDO2" },
+	[RN5T618_LDO3]		= { .name = "LDO3" },
+	[RN5T618_LDO4]		= { .name = "LDO4" },
+	[RN5T618_LDO5]		= { .name = "LDO5" },
+	[RN5T618_LDORTC1]	= { .name = "LDORTC1" },
+	[RN5T618_LDORTC2]	= { .name = "LDORTC2" },
+};
+
+static int rn5t618_regulator_parse_dt(struct platform_device *pdev)
+{
+	struct device_node *np, *regulators;
+	int ret;
+
+	np = of_node_get(pdev->dev.parent->of_node);
+	if (!np)
+		return 0;
+
+	regulators = of_get_child_by_name(np, "regulators");
+	if (!regulators) {
+		dev_err(&pdev->dev, "regulators node not found\n");
+		return -EINVAL;
+	}
+
+	ret = of_regulator_match(&pdev->dev, regulators, rn5t618_matches,
+				 ARRAY_SIZE(rn5t618_matches));
+	of_node_put(regulators);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "error parsing regulator init data: %d\n",
+			ret);
+	}
+
+	return 0;
+}
+
+static int rn5t618_regulator_probe(struct platform_device *pdev)
+{
+	struct rn5t618 *rn5t618 = dev_get_drvdata(pdev->dev.parent);
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+	int ret, i;
+
+	ret = rn5t618_regulator_parse_dt(pdev);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < RN5T618_REG_NUM; i++) {
+		config.dev = &pdev->dev;
+		config.init_data = rn5t618_matches[i].init_data;
+		config.of_node = rn5t618_matches[i].of_node;
+		config.regmap = rn5t618->regmap;
+
+		rdev = devm_regulator_register(&pdev->dev,
+					       &rn5t618_regulators[i],
+					       &config);
+		if (IS_ERR(rdev)) {
+			dev_err(&pdev->dev, "failed to register %s regulator\n",
+				rn5t618_regulators[i].name);
+			return PTR_ERR(rdev);
+		}
+	}
+
+	return 0;
+}
+
+static struct platform_driver rn5t618_regulator_driver = {
+	.probe = rn5t618_regulator_probe,
+	.driver = {
+		.name	= "rn5t618-regulator",
+	},
+};
+
+module_platform_driver(rn5t618_regulator_driver);
+
+MODULE_AUTHOR("Beniamino Galvani <b.galvani@gmail.com>");
+MODULE_DESCRIPTION("RN5T618 regulator driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/regulator/s2mpa01.c b/drivers/regulator/s2mpa01.c
index ee83b4876420..4acefa6b462e 100644
--- a/drivers/regulator/s2mpa01.c
+++ b/drivers/regulator/s2mpa01.c
@@ -235,28 +235,14 @@ static struct regulator_ops s2mpa01_buck_ops = {
 	.set_ramp_delay		= s2mpa01_set_ramp_delay,
 };
 
-#define regulator_desc_ldo1(num)	{		\
+#define regulator_desc_ldo(num, step) {			\
 	.name		= "LDO"#num,			\
 	.id		= S2MPA01_LDO##num,		\
 	.ops		= &s2mpa01_ldo_ops,		\
 	.type		= REGULATOR_VOLTAGE,		\
 	.owner		= THIS_MODULE,			\
-	.min_uV		= S2MPA01_LDO_MIN,		\
-	.uV_step	= S2MPA01_LDO_STEP1,		\
-	.n_voltages	= S2MPA01_LDO_N_VOLTAGES,	\
-	.vsel_reg	= S2MPA01_REG_L1CTRL + num - 1,	\
-	.vsel_mask	= S2MPA01_LDO_VSEL_MASK,	\
-	.enable_reg	= S2MPA01_REG_L1CTRL + num - 1,	\
-	.enable_mask	= S2MPA01_ENABLE_MASK		\
-}
-#define regulator_desc_ldo2(num)	{		\
-	.name		= "LDO"#num,			\
-	.id		= S2MPA01_LDO##num,		\
-	.ops		= &s2mpa01_ldo_ops,		\
-	.type		= REGULATOR_VOLTAGE,		\
-	.owner		= THIS_MODULE,			\
-	.min_uV		= S2MPA01_LDO_MIN,		\
-	.uV_step	= S2MPA01_LDO_STEP2,		\
+	.min_uV		= MIN_800_MV,			\
+	.uV_step	= step,				\
 	.n_voltages	= S2MPA01_LDO_N_VOLTAGES,	\
 	.vsel_reg	= S2MPA01_REG_L1CTRL + num - 1,	\
 	.vsel_mask	= S2MPA01_LDO_VSEL_MASK,	\
@@ -270,8 +256,8 @@ static struct regulator_ops s2mpa01_buck_ops = {
 	.ops		= &s2mpa01_buck_ops,			\
 	.type		= REGULATOR_VOLTAGE,			\
 	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPA01_BUCK_MIN1,			\
-	.uV_step	= S2MPA01_BUCK_STEP1,			\
+	.min_uV		= MIN_600_MV,				\
+	.uV_step	= STEP_6_25_MV,				\
 	.n_voltages	= S2MPA01_BUCK_N_VOLTAGES,		\
 	.ramp_delay	= S2MPA01_RAMP_DELAY,			\
 	.vsel_reg	= S2MPA01_REG_B1CTRL2 + (num - 1) * 2,	\
@@ -286,8 +272,8 @@ static struct regulator_ops s2mpa01_buck_ops = {
 	.ops		= &s2mpa01_buck_ops,			\
 	.type		= REGULATOR_VOLTAGE,			\
 	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPA01_BUCK_MIN2,			\
-	.uV_step	= S2MPA01_BUCK_STEP1,			\
+	.min_uV		= MIN_800_MV,				\
+	.uV_step	= STEP_6_25_MV,				\
 	.n_voltages	= S2MPA01_BUCK_N_VOLTAGES,		\
 	.ramp_delay	= S2MPA01_RAMP_DELAY,			\
 	.vsel_reg	= S2MPA01_REG_B5CTRL2,			\
@@ -296,14 +282,14 @@ static struct regulator_ops s2mpa01_buck_ops = {
 	.enable_mask	= S2MPA01_ENABLE_MASK			\
 }
 
-#define regulator_desc_buck6_7(num)	{			\
+#define regulator_desc_buck6_10(num, min, step) {			\
 	.name		= "BUCK"#num,				\
 	.id		= S2MPA01_BUCK##num,			\
 	.ops		= &s2mpa01_buck_ops,			\
 	.type		= REGULATOR_VOLTAGE,			\
 	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPA01_BUCK_MIN1,			\
-	.uV_step	= S2MPA01_BUCK_STEP1,			\
+	.min_uV		= min,					\
+	.uV_step	= step,					\
 	.n_voltages	= S2MPA01_BUCK_N_VOLTAGES,		\
 	.ramp_delay	= S2MPA01_RAMP_DELAY,			\
 	.vsel_reg	= S2MPA01_REG_B6CTRL2 + (num - 6) * 2,	\
@@ -312,91 +298,43 @@ static struct regulator_ops s2mpa01_buck_ops = {
 	.enable_mask	= S2MPA01_ENABLE_MASK			\
 }
 
-#define regulator_desc_buck8	{				\
-	.name		= "BUCK8",				\
-	.id		= S2MPA01_BUCK8,			\
-	.ops		= &s2mpa01_buck_ops,			\
-	.type		= REGULATOR_VOLTAGE,			\
-	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPA01_BUCK_MIN2,			\
-	.uV_step	= S2MPA01_BUCK_STEP2,			\
-	.n_voltages	= S2MPA01_BUCK_N_VOLTAGES,		\
-	.ramp_delay	= S2MPA01_RAMP_DELAY,			\
-	.vsel_reg	= S2MPA01_REG_B8CTRL2,			\
-	.vsel_mask	= S2MPA01_BUCK_VSEL_MASK,		\
-	.enable_reg	= S2MPA01_REG_B8CTRL1,			\
-	.enable_mask	= S2MPA01_ENABLE_MASK			\
-}
-
-#define regulator_desc_buck9	{				\
-	.name		= "BUCK9",				\
-	.id		= S2MPA01_BUCK9,			\
-	.ops		= &s2mpa01_buck_ops,			\
-	.type		= REGULATOR_VOLTAGE,			\
-	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPA01_BUCK_MIN4,			\
-	.uV_step	= S2MPA01_BUCK_STEP2,			\
-	.n_voltages	= S2MPA01_BUCK_N_VOLTAGES,		\
-	.ramp_delay	= S2MPA01_RAMP_DELAY,			\
-	.vsel_reg	= S2MPA01_REG_B9CTRL2,			\
-	.vsel_mask	= S2MPA01_BUCK_VSEL_MASK,		\
-	.enable_reg	= S2MPA01_REG_B9CTRL1,			\
-	.enable_mask	= S2MPA01_ENABLE_MASK			\
-}
-
-#define regulator_desc_buck10	{				\
-	.name		= "BUCK10",				\
-	.id		= S2MPA01_BUCK10,			\
-	.ops		= &s2mpa01_buck_ops,			\
-	.type		= REGULATOR_VOLTAGE,			\
-	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPA01_BUCK_MIN3,			\
-	.uV_step	= S2MPA01_BUCK_STEP2,			\
-	.n_voltages	= S2MPA01_BUCK_N_VOLTAGES,		\
-	.ramp_delay	= S2MPA01_RAMP_DELAY,			\
-	.vsel_reg	= S2MPA01_REG_B10CTRL2,			\
-	.vsel_mask	= S2MPA01_BUCK_VSEL_MASK,		\
-	.enable_reg	= S2MPA01_REG_B10CTRL1,			\
-	.enable_mask	= S2MPA01_ENABLE_MASK			\
-}
-
 static struct regulator_desc regulators[] = {
-	regulator_desc_ldo2(1),
-	regulator_desc_ldo1(2),
-	regulator_desc_ldo1(3),
-	regulator_desc_ldo1(4),
-	regulator_desc_ldo1(5),
-	regulator_desc_ldo2(6),
-	regulator_desc_ldo1(7),
-	regulator_desc_ldo1(8),
-	regulator_desc_ldo1(9),
-	regulator_desc_ldo1(10),
-	regulator_desc_ldo2(11),
-	regulator_desc_ldo1(12),
-	regulator_desc_ldo1(13),
-	regulator_desc_ldo1(14),
-	regulator_desc_ldo1(15),
-	regulator_desc_ldo1(16),
-	regulator_desc_ldo1(17),
-	regulator_desc_ldo1(18),
-	regulator_desc_ldo1(19),
-	regulator_desc_ldo1(20),
-	regulator_desc_ldo1(21),
-	regulator_desc_ldo2(22),
-	regulator_desc_ldo2(23),
-	regulator_desc_ldo1(24),
-	regulator_desc_ldo1(25),
-	regulator_desc_ldo1(26),
+	regulator_desc_ldo(1, STEP_25_MV),
+	regulator_desc_ldo(2, STEP_50_MV),
+	regulator_desc_ldo(3, STEP_50_MV),
+	regulator_desc_ldo(4, STEP_50_MV),
+	regulator_desc_ldo(5, STEP_50_MV),
+	regulator_desc_ldo(6, STEP_25_MV),
+	regulator_desc_ldo(7, STEP_50_MV),
+	regulator_desc_ldo(8, STEP_50_MV),
+	regulator_desc_ldo(9, STEP_50_MV),
+	regulator_desc_ldo(10, STEP_50_MV),
+	regulator_desc_ldo(11, STEP_25_MV),
+	regulator_desc_ldo(12, STEP_50_MV),
+	regulator_desc_ldo(13, STEP_50_MV),
+	regulator_desc_ldo(14, STEP_50_MV),
+	regulator_desc_ldo(15, STEP_50_MV),
+	regulator_desc_ldo(16, STEP_50_MV),
+	regulator_desc_ldo(17, STEP_50_MV),
+	regulator_desc_ldo(18, STEP_50_MV),
+	regulator_desc_ldo(19, STEP_50_MV),
+	regulator_desc_ldo(20, STEP_50_MV),
+	regulator_desc_ldo(21, STEP_50_MV),
+	regulator_desc_ldo(22, STEP_25_MV),
+	regulator_desc_ldo(23, STEP_25_MV),
+	regulator_desc_ldo(24, STEP_50_MV),
+	regulator_desc_ldo(25, STEP_50_MV),
+	regulator_desc_ldo(26, STEP_50_MV),
 	regulator_desc_buck1_4(1),
 	regulator_desc_buck1_4(2),
 	regulator_desc_buck1_4(3),
 	regulator_desc_buck1_4(4),
 	regulator_desc_buck5,
-	regulator_desc_buck6_7(6),
-	regulator_desc_buck6_7(7),
-	regulator_desc_buck8,
-	regulator_desc_buck9,
-	regulator_desc_buck10,
+	regulator_desc_buck6_10(6, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_buck6_10(7, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_buck6_10(8, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_buck6_10(9, MIN_1500_MV, STEP_12_5_MV),
+	regulator_desc_buck6_10(10, MIN_1000_MV, STEP_12_5_MV),
 };
 
 static int s2mpa01_pmic_probe(struct platform_device *pdev)
diff --git a/drivers/regulator/s2mps11.c b/drivers/regulator/s2mps11.c
index b16c53a8272f..adab82d5279f 100644
--- a/drivers/regulator/s2mps11.c
+++ b/drivers/regulator/s2mps11.c
@@ -255,28 +255,14 @@ static struct regulator_ops s2mps11_buck_ops = {
 	.set_ramp_delay		= s2mps11_set_ramp_delay,
 };
 
-#define regulator_desc_s2mps11_ldo1(num)	{		\
+#define regulator_desc_s2mps11_ldo(num, step) {		\
 	.name		= "LDO"#num,			\
 	.id		= S2MPS11_LDO##num,		\
 	.ops		= &s2mps11_ldo_ops,		\
 	.type		= REGULATOR_VOLTAGE,		\
 	.owner		= THIS_MODULE,			\
-	.min_uV		= S2MPS11_LDO_MIN,		\
-	.uV_step	= S2MPS11_LDO_STEP1,		\
-	.n_voltages	= S2MPS11_LDO_N_VOLTAGES,	\
-	.vsel_reg	= S2MPS11_REG_L1CTRL + num - 1,	\
-	.vsel_mask	= S2MPS11_LDO_VSEL_MASK,	\
-	.enable_reg	= S2MPS11_REG_L1CTRL + num - 1,	\
-	.enable_mask	= S2MPS11_ENABLE_MASK		\
-}
-#define regulator_desc_s2mps11_ldo2(num) {		\
-	.name		= "LDO"#num,			\
-	.id		= S2MPS11_LDO##num,		\
-	.ops		= &s2mps11_ldo_ops,		\
-	.type		= REGULATOR_VOLTAGE,		\
-	.owner		= THIS_MODULE,			\
-	.min_uV		= S2MPS11_LDO_MIN,		\
-	.uV_step	= S2MPS11_LDO_STEP2,		\
+	.min_uV		= MIN_800_MV,			\
+	.uV_step	= step,				\
 	.n_voltages	= S2MPS11_LDO_N_VOLTAGES,	\
 	.vsel_reg	= S2MPS11_REG_L1CTRL + num - 1,	\
 	.vsel_mask	= S2MPS11_LDO_VSEL_MASK,	\
@@ -290,8 +276,8 @@ static struct regulator_ops s2mps11_buck_ops = {
 	.ops		= &s2mps11_buck_ops,			\
 	.type		= REGULATOR_VOLTAGE,			\
 	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPS11_BUCK_MIN1,			\
-	.uV_step	= S2MPS11_BUCK_STEP1,			\
+	.min_uV		= MIN_600_MV,				\
+	.uV_step	= STEP_6_25_MV,				\
 	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
 	.ramp_delay	= S2MPS11_RAMP_DELAY,			\
 	.vsel_reg	= S2MPS11_REG_B1CTRL2 + (num - 1) * 2,	\
@@ -306,8 +292,8 @@ static struct regulator_ops s2mps11_buck_ops = {
 	.ops		= &s2mps11_buck_ops,			\
 	.type		= REGULATOR_VOLTAGE,			\
 	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPS11_BUCK_MIN1,			\
-	.uV_step	= S2MPS11_BUCK_STEP1,			\
+	.min_uV		= MIN_600_MV,				\
+	.uV_step	= STEP_6_25_MV,				\
 	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
 	.ramp_delay	= S2MPS11_RAMP_DELAY,			\
 	.vsel_reg	= S2MPS11_REG_B5CTRL2,			\
@@ -316,14 +302,14 @@ static struct regulator_ops s2mps11_buck_ops = {
 	.enable_mask	= S2MPS11_ENABLE_MASK			\
 }
 
-#define regulator_desc_s2mps11_buck6_8(num) {			\
+#define regulator_desc_s2mps11_buck6_10(num, min, step) {	\
 	.name		= "BUCK"#num,				\
 	.id		= S2MPS11_BUCK##num,			\
 	.ops		= &s2mps11_buck_ops,			\
 	.type		= REGULATOR_VOLTAGE,			\
 	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPS11_BUCK_MIN1,			\
-	.uV_step	= S2MPS11_BUCK_STEP1,			\
+	.min_uV		= min,					\
+	.uV_step	= step,					\
 	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
 	.ramp_delay	= S2MPS11_RAMP_DELAY,			\
 	.vsel_reg	= S2MPS11_REG_B6CTRL2 + (num - 6) * 2,	\
@@ -332,87 +318,55 @@ static struct regulator_ops s2mps11_buck_ops = {
 	.enable_mask	= S2MPS11_ENABLE_MASK			\
 }
 
-#define regulator_desc_s2mps11_buck9 {				\
-	.name		= "BUCK9",				\
-	.id		= S2MPS11_BUCK9,			\
-	.ops		= &s2mps11_buck_ops,			\
-	.type		= REGULATOR_VOLTAGE,			\
-	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPS11_BUCK_MIN3,			\
-	.uV_step	= S2MPS11_BUCK_STEP3,			\
-	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
-	.ramp_delay	= S2MPS11_RAMP_DELAY,			\
-	.vsel_reg	= S2MPS11_REG_B9CTRL2,			\
-	.vsel_mask	= S2MPS11_BUCK_VSEL_MASK,		\
-	.enable_reg	= S2MPS11_REG_B9CTRL1,			\
-	.enable_mask	= S2MPS11_ENABLE_MASK			\
-}
-
-#define regulator_desc_s2mps11_buck10 {				\
-	.name		= "BUCK10",				\
-	.id		= S2MPS11_BUCK10,			\
-	.ops		= &s2mps11_buck_ops,			\
-	.type		= REGULATOR_VOLTAGE,			\
-	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPS11_BUCK_MIN2,			\
-	.uV_step	= S2MPS11_BUCK_STEP2,			\
-	.n_voltages	= S2MPS11_BUCK_N_VOLTAGES,		\
-	.ramp_delay	= S2MPS11_RAMP_DELAY,			\
-	.vsel_reg	= S2MPS11_REG_B10CTRL2,			\
-	.vsel_mask	= S2MPS11_BUCK_VSEL_MASK,		\
-	.enable_reg	= S2MPS11_REG_B10CTRL1,			\
-	.enable_mask	= S2MPS11_ENABLE_MASK			\
-}
-
 static const struct regulator_desc s2mps11_regulators[] = {
-	regulator_desc_s2mps11_ldo2(1),
-	regulator_desc_s2mps11_ldo1(2),
-	regulator_desc_s2mps11_ldo1(3),
-	regulator_desc_s2mps11_ldo1(4),
-	regulator_desc_s2mps11_ldo1(5),
-	regulator_desc_s2mps11_ldo2(6),
-	regulator_desc_s2mps11_ldo1(7),
-	regulator_desc_s2mps11_ldo1(8),
-	regulator_desc_s2mps11_ldo1(9),
-	regulator_desc_s2mps11_ldo1(10),
-	regulator_desc_s2mps11_ldo2(11),
-	regulator_desc_s2mps11_ldo1(12),
-	regulator_desc_s2mps11_ldo1(13),
-	regulator_desc_s2mps11_ldo1(14),
-	regulator_desc_s2mps11_ldo1(15),
-	regulator_desc_s2mps11_ldo1(16),
-	regulator_desc_s2mps11_ldo1(17),
-	regulator_desc_s2mps11_ldo1(18),
-	regulator_desc_s2mps11_ldo1(19),
-	regulator_desc_s2mps11_ldo1(20),
-	regulator_desc_s2mps11_ldo1(21),
-	regulator_desc_s2mps11_ldo2(22),
-	regulator_desc_s2mps11_ldo2(23),
-	regulator_desc_s2mps11_ldo1(24),
-	regulator_desc_s2mps11_ldo1(25),
-	regulator_desc_s2mps11_ldo1(26),
-	regulator_desc_s2mps11_ldo2(27),
-	regulator_desc_s2mps11_ldo1(28),
-	regulator_desc_s2mps11_ldo1(29),
-	regulator_desc_s2mps11_ldo1(30),
-	regulator_desc_s2mps11_ldo1(31),
-	regulator_desc_s2mps11_ldo1(32),
-	regulator_desc_s2mps11_ldo1(33),
-	regulator_desc_s2mps11_ldo1(34),
-	regulator_desc_s2mps11_ldo1(35),
-	regulator_desc_s2mps11_ldo1(36),
-	regulator_desc_s2mps11_ldo1(37),
-	regulator_desc_s2mps11_ldo1(38),
+	regulator_desc_s2mps11_ldo(1, STEP_25_MV),
+	regulator_desc_s2mps11_ldo(2, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(3, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(4, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(5, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(6, STEP_25_MV),
+	regulator_desc_s2mps11_ldo(7, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(8, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(9, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(10, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(11, STEP_25_MV),
+	regulator_desc_s2mps11_ldo(12, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(13, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(14, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(15, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(16, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(17, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(18, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(19, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(20, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(21, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(22, STEP_25_MV),
+	regulator_desc_s2mps11_ldo(23, STEP_25_MV),
+	regulator_desc_s2mps11_ldo(24, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(25, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(26, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(27, STEP_25_MV),
+	regulator_desc_s2mps11_ldo(28, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(29, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(30, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(31, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(32, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(33, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(34, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(35, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(36, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(37, STEP_50_MV),
+	regulator_desc_s2mps11_ldo(38, STEP_50_MV),
 	regulator_desc_s2mps11_buck1_4(1),
 	regulator_desc_s2mps11_buck1_4(2),
 	regulator_desc_s2mps11_buck1_4(3),
 	regulator_desc_s2mps11_buck1_4(4),
 	regulator_desc_s2mps11_buck5,
-	regulator_desc_s2mps11_buck6_8(6),
-	regulator_desc_s2mps11_buck6_8(7),
-	regulator_desc_s2mps11_buck6_8(8),
-	regulator_desc_s2mps11_buck9,
-	regulator_desc_s2mps11_buck10,
+	regulator_desc_s2mps11_buck6_10(6, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_s2mps11_buck6_10(7, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_s2mps11_buck6_10(8, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_s2mps11_buck6_10(9, MIN_3000_MV, STEP_25_MV),
+	regulator_desc_s2mps11_buck6_10(10, MIN_750_MV, STEP_12_5_MV),
 };
 
 static int s2mps14_regulator_enable(struct regulator_dev *rdev)
@@ -510,56 +464,29 @@ static struct regulator_ops s2mps14_reg_ops = {
 	.set_suspend_disable	= s2mps14_regulator_set_suspend_disable,
 };
 
-#define regulator_desc_s2mps14_ldo1(num) {		\
-	.name		= "LDO"#num,			\
-	.id		= S2MPS14_LDO##num,		\
-	.ops		= &s2mps14_reg_ops,		\
-	.type		= REGULATOR_VOLTAGE,		\
-	.owner		= THIS_MODULE,			\
-	.min_uV		= S2MPS14_LDO_MIN_800MV,	\
-	.uV_step	= S2MPS14_LDO_STEP_25MV,	\
-	.n_voltages	= S2MPS14_LDO_N_VOLTAGES,	\
-	.vsel_reg	= S2MPS14_REG_L1CTRL + num - 1,	\
-	.vsel_mask	= S2MPS14_LDO_VSEL_MASK,	\
-	.enable_reg	= S2MPS14_REG_L1CTRL + num - 1,	\
-	.enable_mask	= S2MPS14_ENABLE_MASK		\
-}
-#define regulator_desc_s2mps14_ldo2(num) {		\
-	.name		= "LDO"#num,			\
-	.id		= S2MPS14_LDO##num,		\
-	.ops		= &s2mps14_reg_ops,		\
-	.type		= REGULATOR_VOLTAGE,		\
-	.owner		= THIS_MODULE,			\
-	.min_uV		= S2MPS14_LDO_MIN_1800MV,	\
-	.uV_step	= S2MPS14_LDO_STEP_25MV,	\
-	.n_voltages	= S2MPS14_LDO_N_VOLTAGES,	\
-	.vsel_reg	= S2MPS14_REG_L1CTRL + num - 1,	\
-	.vsel_mask	= S2MPS14_LDO_VSEL_MASK,	\
-	.enable_reg	= S2MPS14_REG_L1CTRL + num - 1,	\
-	.enable_mask	= S2MPS14_ENABLE_MASK		\
-}
-#define regulator_desc_s2mps14_ldo3(num) {		\
+#define regulator_desc_s2mps14_ldo(num, min, step) {	\
 	.name		= "LDO"#num,			\
 	.id		= S2MPS14_LDO##num,		\
 	.ops		= &s2mps14_reg_ops,		\
 	.type		= REGULATOR_VOLTAGE,		\
 	.owner		= THIS_MODULE,			\
-	.min_uV		= S2MPS14_LDO_MIN_800MV,	\
-	.uV_step	= S2MPS14_LDO_STEP_12_5MV,	\
+	.min_uV		= min,				\
+	.uV_step	= step,				\
 	.n_voltages	= S2MPS14_LDO_N_VOLTAGES,	\
 	.vsel_reg	= S2MPS14_REG_L1CTRL + num - 1,	\
 	.vsel_mask	= S2MPS14_LDO_VSEL_MASK,	\
 	.enable_reg	= S2MPS14_REG_L1CTRL + num - 1,	\
 	.enable_mask	= S2MPS14_ENABLE_MASK		\
 }
-#define regulator_desc_s2mps14_buck1235(num) {			\
+
+#define regulator_desc_s2mps14_buck(num, min, step) {		\
 	.name		= "BUCK"#num,				\
 	.id		= S2MPS14_BUCK##num,			\
 	.ops		= &s2mps14_reg_ops,			\
 	.type		= REGULATOR_VOLTAGE,			\
 	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPS14_BUCK1235_MIN_600MV,		\
-	.uV_step	= S2MPS14_BUCK1235_STEP_6_25MV,		\
+	.min_uV		= min,					\
+	.uV_step	= step,					\
 	.n_voltages	= S2MPS14_BUCK_N_VOLTAGES,		\
 	.linear_min_sel = S2MPS14_BUCK1235_START_SEL,		\
 	.ramp_delay	= S2MPS14_BUCK_RAMP_DELAY,		\
@@ -568,54 +495,38 @@ static struct regulator_ops s2mps14_reg_ops = {
 	.enable_reg	= S2MPS14_REG_B1CTRL1 + (num - 1) * 2,	\
 	.enable_mask	= S2MPS14_ENABLE_MASK			\
 }
-#define regulator_desc_s2mps14_buck4(num) {			\
-	.name		= "BUCK"#num,				\
-	.id		= S2MPS14_BUCK##num,			\
-	.ops		= &s2mps14_reg_ops,			\
-	.type		= REGULATOR_VOLTAGE,			\
-	.owner		= THIS_MODULE,				\
-	.min_uV		= S2MPS14_BUCK4_MIN_1400MV,		\
-	.uV_step	= S2MPS14_BUCK4_STEP_12_5MV,		\
-	.n_voltages	= S2MPS14_BUCK_N_VOLTAGES,		\
-	.linear_min_sel = S2MPS14_BUCK4_START_SEL,		\
-	.ramp_delay	= S2MPS14_BUCK_RAMP_DELAY,		\
-	.vsel_reg	= S2MPS14_REG_B1CTRL2 + (num - 1) * 2,	\
-	.vsel_mask	= S2MPS14_BUCK_VSEL_MASK,		\
-	.enable_reg	= S2MPS14_REG_B1CTRL1 + (num - 1) * 2,	\
-	.enable_mask	= S2MPS14_ENABLE_MASK			\
-}
 
 static const struct regulator_desc s2mps14_regulators[] = {
-	regulator_desc_s2mps14_ldo3(1),
-	regulator_desc_s2mps14_ldo3(2),
-	regulator_desc_s2mps14_ldo1(3),
-	regulator_desc_s2mps14_ldo1(4),
-	regulator_desc_s2mps14_ldo3(5),
-	regulator_desc_s2mps14_ldo3(6),
-	regulator_desc_s2mps14_ldo1(7),
-	regulator_desc_s2mps14_ldo2(8),
-	regulator_desc_s2mps14_ldo3(9),
-	regulator_desc_s2mps14_ldo3(10),
-	regulator_desc_s2mps14_ldo1(11),
-	regulator_desc_s2mps14_ldo2(12),
-	regulator_desc_s2mps14_ldo2(13),
-	regulator_desc_s2mps14_ldo2(14),
-	regulator_desc_s2mps14_ldo2(15),
-	regulator_desc_s2mps14_ldo2(16),
-	regulator_desc_s2mps14_ldo2(17),
-	regulator_desc_s2mps14_ldo2(18),
-	regulator_desc_s2mps14_ldo1(19),
-	regulator_desc_s2mps14_ldo1(20),
-	regulator_desc_s2mps14_ldo1(21),
-	regulator_desc_s2mps14_ldo3(22),
-	regulator_desc_s2mps14_ldo1(23),
-	regulator_desc_s2mps14_ldo2(24),
-	regulator_desc_s2mps14_ldo2(25),
-	regulator_desc_s2mps14_buck1235(1),
-	regulator_desc_s2mps14_buck1235(2),
-	regulator_desc_s2mps14_buck1235(3),
-	regulator_desc_s2mps14_buck4(4),
-	regulator_desc_s2mps14_buck1235(5),
+	regulator_desc_s2mps14_ldo(1, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_ldo(2, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_ldo(3, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(4, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(5, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_ldo(6, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_ldo(7, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(8, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(9, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_ldo(10, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_ldo(11, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(12, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(13, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(14, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(15, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(16, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(17, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(18, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(19, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(20, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(21, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(22, MIN_800_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_ldo(23, MIN_800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(24, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_ldo(25, MIN_1800_MV, STEP_25_MV),
+	regulator_desc_s2mps14_buck(1, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_s2mps14_buck(2, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_s2mps14_buck(3, MIN_600_MV, STEP_6_25_MV),
+	regulator_desc_s2mps14_buck(4, MIN_1400_MV, STEP_12_5_MV),
+	regulator_desc_s2mps14_buck(5, MIN_600_MV, STEP_6_25_MV),
 };
 
 static int s2mps14_pmic_enable_ext_control(struct s2mps11_info *s2mps11,
diff --git a/drivers/regulator/sky81452-regulator.c b/drivers/regulator/sky81452-regulator.c
new file mode 100644
index 000000000000..97aff0ccd65f
--- /dev/null
+++ b/drivers/regulator/sky81452-regulator.c
@@ -0,0 +1,130 @@
+/*
+ * sky81452-regulator.c	SKY81452 regulator driver
+ *
+ * Copyright 2014 Skyworks Solutions Inc.
+ * Author : Gyungoh Yoo <jack.yoo@skyworksinc.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2, or (at your option) any
+ * later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/platform_device.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/of.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/of_regulator.h>
+
+/* registers */
+#define SKY81452_REG1	0x01
+#define SKY81452_REG3	0x03
+
+/* bit mask */
+#define SKY81452_LEN	0x40
+#define SKY81452_LOUT	0x1F
+
+static struct regulator_ops sky81452_reg_ops = {
+	.list_voltage = regulator_list_voltage_linear_range,
+	.map_voltage = regulator_map_voltage_linear_range,
+	.get_voltage_sel = regulator_get_voltage_sel_regmap,
+	.set_voltage_sel = regulator_set_voltage_sel_regmap,
+	.enable = regulator_enable_regmap,
+	.disable = regulator_disable_regmap,
+	.is_enabled = regulator_is_enabled_regmap,
+};
+
+static const struct regulator_linear_range sky81452_reg_ranges[] = {
+	REGULATOR_LINEAR_RANGE(4500000, 0, 14, 250000),
+	REGULATOR_LINEAR_RANGE(9000000, 15, 31, 1000000),
+};
+
+static const struct regulator_desc sky81452_reg = {
+	.name = "LOUT",
+	.ops = &sky81452_reg_ops,
+	.type = REGULATOR_VOLTAGE,
+	.owner = THIS_MODULE,
+	.n_voltages = SKY81452_LOUT + 1,
+	.linear_ranges = sky81452_reg_ranges,
+	.n_linear_ranges = ARRAY_SIZE(sky81452_reg_ranges),
+	.vsel_reg = SKY81452_REG3,
+	.vsel_mask = SKY81452_LOUT,
+	.enable_reg = SKY81452_REG1,
+	.enable_mask = SKY81452_LEN,
+};
+
+#ifdef CONFIG_OF
+static struct regulator_init_data *sky81452_reg_parse_dt(struct device *dev)
+{
+	struct regulator_init_data *init_data;
+	struct device_node *np;
+
+	np = of_get_child_by_name(dev->parent->of_node, "regulator");
+	if (unlikely(!np)) {
+		dev_err(dev, "regulator node not found");
+		return NULL;
+	}
+
+	init_data = of_get_regulator_init_data(dev, np);
+
+	of_node_put(np);
+	return init_data;
+}
+#else
+static struct regulator_init_data *sky81452_reg_parse_dt(struct device *dev)
+{
+	return ERR_PTR(-EINVAL);
+}
+#endif
+
+static int sky81452_reg_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	const struct regulator_init_data *init_data = dev_get_platdata(dev);
+	struct regulator_config config = { };
+	struct regulator_dev *rdev;
+
+	if (!init_data) {
+		init_data = sky81452_reg_parse_dt(dev);
+		if (IS_ERR(init_data))
+			return PTR_ERR(init_data);
+	}
+
+	config.dev = dev;
+	config.init_data = init_data;
+	config.of_node = dev->of_node;
+	config.regmap = dev_get_drvdata(dev->parent);
+
+	rdev = devm_regulator_register(dev, &sky81452_reg, &config);
+	if (IS_ERR(rdev))
+		return PTR_ERR(rdev);
+
+	platform_set_drvdata(pdev, rdev);
+
+	return 0;
+}
+
+static struct platform_driver sky81452_reg_driver = {
+	.driver = {
+		.name = "sky81452-regulator",
+	},
+	.probe = sky81452_reg_probe,
+};
+
+module_platform_driver(sky81452_reg_driver);
+
+MODULE_DESCRIPTION("Skyworks SKY81452 Regulator driver");
+MODULE_AUTHOR("Gyungoh Yoo <jack.yoo@skyworksinc.com>");
+MODULE_LICENSE("GPL");
+MODULE_VERSION("1.0");
diff --git a/drivers/regulator/st-pwm.c b/drivers/regulator/st-pwm.c
deleted file mode 100644
index 5ea78df449f8..000000000000
--- a/drivers/regulator/st-pwm.c
+++ /dev/null
@@ -1,190 +0,0 @@
-/*
- * Regulator driver for ST's PWM Regulators
- *
- * Copyright (C) 2014 - STMicroelectronics Inc.
- *
- * Author: Lee Jones <lee.jones@linaro.org>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- */
-
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/err.h>
-#include <linux/regulator/driver.h>
-#include <linux/regulator/machine.h>
-#include <linux/regulator/of_regulator.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
-#include <linux/pwm.h>
-
-#define ST_PWM_REG_PERIOD 8448
-
-struct st_pwm_regulator_pdata {
-	const struct regulator_desc *desc;
-	struct st_pwm_voltages *duty_cycle_table;
-};
-
-struct st_pwm_regulator_data {
-	const struct st_pwm_regulator_pdata *pdata;
-	struct pwm_device *pwm;
-	bool enabled;
-	int state;
-};
-
-struct st_pwm_voltages {
-	unsigned int uV;
-	unsigned int dutycycle;
-};
-
-static int st_pwm_regulator_get_voltage_sel(struct regulator_dev *dev)
-{
-	struct st_pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
-
-	return drvdata->state;
-}
-
-static int st_pwm_regulator_set_voltage_sel(struct regulator_dev *dev,
-					    unsigned selector)
-{
-	struct st_pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
-	int dutycycle;
-	int ret;
-
-	dutycycle = (ST_PWM_REG_PERIOD / 100) *
-		drvdata->pdata->duty_cycle_table[selector].dutycycle;
-
-	ret = pwm_config(drvdata->pwm, dutycycle, ST_PWM_REG_PERIOD);
-	if (ret) {
-		dev_err(&dev->dev, "Failed to configure PWM\n");
-		return ret;
-	}
-
-	drvdata->state = selector;
-
-	if (!drvdata->enabled) {
-		ret = pwm_enable(drvdata->pwm);
-		if (ret) {
-			dev_err(&dev->dev, "Failed to enable PWM\n");
-			return ret;
-		}
-		drvdata->enabled = true;
-	}
-
-	return 0;
-}
-
-static int st_pwm_regulator_list_voltage(struct regulator_dev *dev,
-					 unsigned selector)
-{
-	struct st_pwm_regulator_data *drvdata = rdev_get_drvdata(dev);
-
-	if (selector >= dev->desc->n_voltages)
-		return -EINVAL;
-
-	return drvdata->pdata->duty_cycle_table[selector].uV;
-}
-
-static struct regulator_ops st_pwm_regulator_voltage_ops = {
-	.set_voltage_sel = st_pwm_regulator_set_voltage_sel,
-	.get_voltage_sel = st_pwm_regulator_get_voltage_sel,
-	.list_voltage    = st_pwm_regulator_list_voltage,
-	.map_voltage     = regulator_map_voltage_iterate,
-};
-
-static struct st_pwm_voltages b2105_duty_cycle_table[] = {
-	{ .uV = 1114000, .dutycycle = 0,  },
-	{ .uV = 1095000, .dutycycle = 10, },
-	{ .uV = 1076000, .dutycycle = 20, },
-	{ .uV = 1056000, .dutycycle = 30, },
-	{ .uV = 1036000, .dutycycle = 40, },
-	{ .uV = 1016000, .dutycycle = 50, },
-	/* WARNING: Values above 50% duty-cycle cause boot failures. */
-};
-
-static const struct regulator_desc b2105_desc = {
-	.name		= "b2105-pwm-regulator",
-	.ops		= &st_pwm_regulator_voltage_ops,
-	.type		= REGULATOR_VOLTAGE,
-	.owner		= THIS_MODULE,
-	.n_voltages	= ARRAY_SIZE(b2105_duty_cycle_table),
-	.supply_name    = "pwm",
-};
-
-static const struct st_pwm_regulator_pdata b2105_info = {
-	.desc		  = &b2105_desc,
-	.duty_cycle_table = b2105_duty_cycle_table,
-};
-
-static const struct of_device_id st_pwm_of_match[] = {
-	{ .compatible = "st,b2105-pwm-regulator", .data = &b2105_info, },
-	{ },
-};
-MODULE_DEVICE_TABLE(of, st_pwm_of_match);
-
-static int st_pwm_regulator_probe(struct platform_device *pdev)
-{
-	struct st_pwm_regulator_data *drvdata;
-	struct regulator_dev *regulator;
-	struct regulator_config config = { };
-	struct device_node *np = pdev->dev.of_node;
-	const struct of_device_id *of_match;
-
-	if (!np) {
-		dev_err(&pdev->dev, "Device Tree node missing\n");
-		return -EINVAL;
-	}
-
-	drvdata = devm_kzalloc(&pdev->dev, sizeof(*drvdata), GFP_KERNEL);
-	if (!drvdata)
-		return -ENOMEM;
-
-	of_match = of_match_device(st_pwm_of_match, &pdev->dev);
-	if (!of_match) {
-		dev_err(&pdev->dev, "failed to match of device\n");
-		return -ENODEV;
-	}
-	drvdata->pdata = of_match->data;
-
-	config.init_data = of_get_regulator_init_data(&pdev->dev, np);
-	if (!config.init_data)
-		return -ENOMEM;
-
-	config.of_node = np;
-	config.dev = &pdev->dev;
-	config.driver_data = drvdata;
-
-	drvdata->pwm = devm_pwm_get(&pdev->dev, NULL);
-	if (IS_ERR(drvdata->pwm)) {
-		dev_err(&pdev->dev, "Failed to get PWM\n");
-		return PTR_ERR(drvdata->pwm);
-	}
-
-	regulator = devm_regulator_register(&pdev->dev,
-					    drvdata->pdata->desc, &config);
-	if (IS_ERR(regulator)) {
-		dev_err(&pdev->dev, "Failed to register regulator %s\n",
-			drvdata->pdata->desc->name);
-		return PTR_ERR(regulator);
-	}
-
-	return 0;
-}
-
-static struct platform_driver st_pwm_regulator_driver = {
-	.driver = {
-		.name		= "st-pwm-regulator",
-		.owner		= THIS_MODULE,
-		.of_match_table = of_match_ptr(st_pwm_of_match),
-	},
-	.probe = st_pwm_regulator_probe,
-};
-
-module_platform_driver(st_pwm_regulator_driver);
-
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Lee Jones <lee.jones@linaro.org>");
-MODULE_DESCRIPTION("ST PWM Regulator Driver");
-MODULE_ALIAS("platform:st_pwm-regulator");
diff --git a/drivers/regulator/tps65023-regulator.c b/drivers/regulator/tps65023-regulator.c
index 3ef67a86115c..7380af8bd50d 100644
--- a/drivers/regulator/tps65023-regulator.c
+++ b/drivers/regulator/tps65023-regulator.c
@@ -211,9 +211,6 @@ static int tps_65023_probe(struct i2c_client *client,
 	int i;
 	int error;
 
-	if (!i2c_check_functionality(client->adapter, I2C_FUNC_SMBUS_BYTE_DATA))
-		return -EIO;
-
 	/**
 	 * init_data points to array of regulator_init structures
 	 * coming from the board-evm file.
diff --git a/drivers/regulator/tps65217-regulator.c b/drivers/regulator/tps65217-regulator.c
index d58db72a63b0..adbe4fc5cf07 100644
--- a/drivers/regulator/tps65217-regulator.c
+++ b/drivers/regulator/tps65217-regulator.c
@@ -27,10 +27,13 @@
 #include <linux/regulator/machine.h>
 #include <linux/mfd/tps65217.h>
 
-#define TPS65217_REGULATOR(_name, _id, _ops, _n, _vr, _vm, _em, _t, _lr, _nlr) \
+#define TPS65217_REGULATOR(_name, _id, _of_match, _ops, _n, _vr, _vm, _em, \
+                           _t, _lr, _nlr) \
 	{						\
 		.name		= _name,		\
 		.id		= _id,			\
+		.of_match       = of_match_ptr(_of_match),    \
+		.regulators_node= of_match_ptr("regulators"), \
 		.ops		= &_ops,		\
 		.n_voltages	= _n,			\
 		.type		= REGULATOR_VOLTAGE,	\
@@ -138,87 +141,40 @@ static struct regulator_ops tps65217_pmic_ldo1_ops = {
 };
 
 static const struct regulator_desc regulators[] = {
-	TPS65217_REGULATOR("DCDC1", TPS65217_DCDC_1, tps65217_pmic_ops, 64,
-			   TPS65217_REG_DEFDCDC1, TPS65217_DEFDCDCX_DCDC_MASK,
-			   TPS65217_ENABLE_DC1_EN, NULL, tps65217_uv1_ranges,
-			   2),	/* DCDC1 voltage range: 900000 ~ 1800000 */
-	TPS65217_REGULATOR("DCDC2", TPS65217_DCDC_2, tps65217_pmic_ops, 64,
-			   TPS65217_REG_DEFDCDC2, TPS65217_DEFDCDCX_DCDC_MASK,
-			   TPS65217_ENABLE_DC2_EN, NULL, tps65217_uv1_ranges,
+	TPS65217_REGULATOR("DCDC1", TPS65217_DCDC_1, "dcdc1",
+			   tps65217_pmic_ops, 64, TPS65217_REG_DEFDCDC1,
+			   TPS65217_DEFDCDCX_DCDC_MASK, TPS65217_ENABLE_DC1_EN,
+			   NULL, tps65217_uv1_ranges, 2),
+	TPS65217_REGULATOR("DCDC2", TPS65217_DCDC_2, "dcdc2",
+			   tps65217_pmic_ops, 64, TPS65217_REG_DEFDCDC2,
+			   TPS65217_DEFDCDCX_DCDC_MASK, TPS65217_ENABLE_DC2_EN,
+			   NULL, tps65217_uv1_ranges,
 			   ARRAY_SIZE(tps65217_uv1_ranges)),
-	TPS65217_REGULATOR("DCDC3", TPS65217_DCDC_3, tps65217_pmic_ops, 64,
-			   TPS65217_REG_DEFDCDC3, TPS65217_DEFDCDCX_DCDC_MASK,
-			   TPS65217_ENABLE_DC3_EN, NULL, tps65217_uv1_ranges,
-			   1),	/* DCDC3 voltage range: 900000 ~ 1500000 */
-	TPS65217_REGULATOR("LDO1", TPS65217_LDO_1, tps65217_pmic_ldo1_ops, 16,
-			   TPS65217_REG_DEFLDO1, TPS65217_DEFLDO1_LDO1_MASK,
-			   TPS65217_ENABLE_LDO1_EN, LDO1_VSEL_table, NULL, 0),
-	TPS65217_REGULATOR("LDO2", TPS65217_LDO_2, tps65217_pmic_ops, 64,
-			   TPS65217_REG_DEFLDO2, TPS65217_DEFLDO2_LDO2_MASK,
-			   TPS65217_ENABLE_LDO2_EN, NULL, tps65217_uv1_ranges,
+	TPS65217_REGULATOR("DCDC3", TPS65217_DCDC_3, "dcdc3",
+			   tps65217_pmic_ops, 64, TPS65217_REG_DEFDCDC3,
+			   TPS65217_DEFDCDCX_DCDC_MASK, TPS65217_ENABLE_DC3_EN,
+			   NULL, tps65217_uv1_ranges, 1),
+	TPS65217_REGULATOR("LDO1", TPS65217_LDO_1, "ldo1",
+			   tps65217_pmic_ldo1_ops, 16, TPS65217_REG_DEFLDO1,
+			   TPS65217_DEFLDO1_LDO1_MASK, TPS65217_ENABLE_LDO1_EN,
+			   LDO1_VSEL_table, NULL, 0),
+	TPS65217_REGULATOR("LDO2", TPS65217_LDO_2, "ldo2", tps65217_pmic_ops,
+			   64, TPS65217_REG_DEFLDO2,
+			   TPS65217_DEFLDO2_LDO2_MASK, TPS65217_ENABLE_LDO2_EN,
+			   NULL, tps65217_uv1_ranges,
 			   ARRAY_SIZE(tps65217_uv1_ranges)),
-	TPS65217_REGULATOR("LDO3", TPS65217_LDO_3, tps65217_pmic_ops, 32,
-			   TPS65217_REG_DEFLS1, TPS65217_DEFLDO3_LDO3_MASK,
+	TPS65217_REGULATOR("LDO3", TPS65217_LDO_3, "ldo3", tps65217_pmic_ops,
+			   32, TPS65217_REG_DEFLS1, TPS65217_DEFLDO3_LDO3_MASK,
 			   TPS65217_ENABLE_LS1_EN | TPS65217_DEFLDO3_LDO3_EN,
 			   NULL, tps65217_uv2_ranges,
 			   ARRAY_SIZE(tps65217_uv2_ranges)),
-	TPS65217_REGULATOR("LDO4", TPS65217_LDO_4, tps65217_pmic_ops, 32,
-			   TPS65217_REG_DEFLS2, TPS65217_DEFLDO4_LDO4_MASK,
+	TPS65217_REGULATOR("LDO4", TPS65217_LDO_4, "ldo4", tps65217_pmic_ops,
+			   32, TPS65217_REG_DEFLS2, TPS65217_DEFLDO4_LDO4_MASK,
 			   TPS65217_ENABLE_LS2_EN | TPS65217_DEFLDO4_LDO4_EN,
 			   NULL, tps65217_uv2_ranges,
 			   ARRAY_SIZE(tps65217_uv2_ranges)),
 };
 
-#ifdef CONFIG_OF
-static struct of_regulator_match reg_matches[] = {
-	{ .name = "dcdc1", .driver_data = (void *)TPS65217_DCDC_1 },
-	{ .name = "dcdc2", .driver_data = (void *)TPS65217_DCDC_2 },
-	{ .name = "dcdc3", .driver_data = (void *)TPS65217_DCDC_3 },
-	{ .name = "ldo1", .driver_data = (void *)TPS65217_LDO_1 },
-	{ .name = "ldo2", .driver_data = (void *)TPS65217_LDO_2 },
-	{ .name = "ldo3", .driver_data = (void *)TPS65217_LDO_3 },
-	{ .name = "ldo4", .driver_data = (void *)TPS65217_LDO_4 },
-};
-
-static struct tps65217_board *tps65217_parse_dt(struct platform_device *pdev)
-{
-	struct tps65217 *tps = dev_get_drvdata(pdev->dev.parent);
-	struct device_node *node = tps->dev->of_node;
-	struct tps65217_board *pdata;
-	struct device_node *regs;
-	int i, count;
-
-	regs = of_get_child_by_name(node, "regulators");
-	if (!regs)
-		return NULL;
-
-	count = of_regulator_match(&pdev->dev, regs, reg_matches,
-				   TPS65217_NUM_REGULATOR);
-	of_node_put(regs);
-	if ((count < 0) || (count > TPS65217_NUM_REGULATOR))
-		return NULL;
-
-	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
-	if (!pdata)
-		return NULL;
-
-	for (i = 0; i < count; i++) {
-		if (!reg_matches[i].of_node)
-			continue;
-
-		pdata->tps65217_init_data[i] = reg_matches[i].init_data;
-		pdata->of_node[i] = reg_matches[i].of_node;
-	}
-
-	return pdata;
-}
-#else
-static struct tps65217_board *tps65217_parse_dt(struct platform_device *pdev)
-{
-	return NULL;
-}
-#endif
-
 static int tps65217_regulator_probe(struct platform_device *pdev)
 {
 	struct tps65217 *tps = dev_get_drvdata(pdev->dev.parent);
@@ -227,14 +183,6 @@ static int tps65217_regulator_probe(struct platform_device *pdev)
 	struct regulator_config config = { };
 	int i;
 
-	if (tps->dev->of_node)
-		pdata = tps65217_parse_dt(pdev);
-
-	if (!pdata) {
-		dev_err(&pdev->dev, "Platform data not found\n");
-		return -EINVAL;
-	}
-
 	if (tps65217_chip_id(tps) != TPS65217) {
 		dev_err(&pdev->dev, "Invalid tps chip version\n");
 		return -ENODEV;
@@ -245,11 +193,10 @@ static int tps65217_regulator_probe(struct platform_device *pdev)
 	for (i = 0; i < TPS65217_NUM_REGULATOR; i++) {
 		/* Register the regulators */
 		config.dev = tps->dev;
-		config.init_data = pdata->tps65217_init_data[i];
+		if (pdata)
+			config.init_data = pdata->tps65217_init_data[i];
 		config.driver_data = tps;
 		config.regmap = tps->regmap;
-		if (tps->dev->of_node)
-			config.of_node = pdata->of_node[i];
 
 		rdev = devm_regulator_register(&pdev->dev, &regulators[i],
 					       &config);
@@ -259,6 +206,7 @@ static int tps65217_regulator_probe(struct platform_device *pdev)
 			return PTR_ERR(rdev);
 		}
 	}
+
 	return 0;
 }
 
diff --git a/drivers/regulator/tps65910-regulator.c b/drivers/regulator/tps65910-regulator.c
index fa7db8847578..18fc991175bc 100644
--- a/drivers/regulator/tps65910-regulator.c
+++ b/drivers/regulator/tps65910-regulator.c
@@ -1014,7 +1014,7 @@ static struct tps65910_board *tps65910_parse_dt_reg_data(
 	if (!pmic_plat_data)
 		return NULL;
 
-	np = of_node_get(pdev->dev.parent->of_node);
+	np = pdev->dev.parent->of_node;
 	regulators = of_get_child_by_name(np, "regulators");
 	if (!regulators) {
 		dev_err(&pdev->dev, "regulator node not found\n");
@@ -1047,7 +1047,7 @@ static struct tps65910_board *tps65910_parse_dt_reg_data(
 	*tps65910_reg_matches = matches;
 
 	for (idx = 0; idx < count; idx++) {
-		if (!matches[idx].init_data || !matches[idx].of_node)
+		if (!matches[idx].of_node)
 			continue;
 
 		pmic_plat_data->tps65910_pmic_init_data[idx] =
@@ -1077,7 +1077,6 @@ static int tps65910_probe(struct platform_device *pdev)
 	struct tps65910 *tps65910 = dev_get_drvdata(pdev->dev.parent);
 	struct regulator_config config = { };
 	struct tps_info *info;
-	struct regulator_init_data *reg_data;
 	struct regulator_dev *rdev;
 	struct tps65910_reg *pmic;
 	struct tps65910_board *pmic_plat_data;
@@ -1140,14 +1139,6 @@ static int tps65910_probe(struct platform_device *pdev)
 
 	for (i = 0; i < pmic->num_regulators && i < TPS65910_NUM_REGS;
 			i++, info++) {
-
-		reg_data = pmic_plat_data->tps65910_pmic_init_data[i];
-
-		/* Regulator API handles empty constraints but not NULL
-		 * constraints */
-		if (!reg_data)
-			continue;
-
 		/* Register the regulators */
 		pmic->info[i] = info;
 
@@ -1199,7 +1190,7 @@ static int tps65910_probe(struct platform_device *pdev)
 		pmic->desc[i].enable_mask = TPS65910_SUPPLY_STATE_ENABLED;
 
 		config.dev = tps65910->dev;
-		config.init_data = reg_data;
+		config.init_data = pmic_plat_data->tps65910_pmic_init_data[i];
 		config.driver_data = pmic;
 		config.regmap = tps65910->regmap;
 
diff --git a/drivers/rtc/Kconfig b/drivers/rtc/Kconfig
index a168e96142b9..94ae1798d48a 100644
--- a/drivers/rtc/Kconfig
+++ b/drivers/rtc/Kconfig
@@ -288,6 +288,26 @@ config RTC_DRV_MAX77686
 	  This driver can also be built as a module. If so, the module
 	  will be called rtc-max77686.
 
+config RTC_DRV_RK808
+	tristate "Rockchip RK808 RTC"
+	depends on MFD_RK808
+	help
+	  If you say yes here you will get support for the
+	  RTC of RK808 PMIC.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rk808-rtc.
+
+config RTC_DRV_MAX77802
+	tristate "Maxim 77802 RTC"
+	depends on MFD_MAX77686
+	help
+	  If you say yes here you will get support for the
+	  RTC of Maxim MAX77802 PMIC.
+
+	  This driver can also be built as a module. If so, the module
+	  will be called rtc-max77802.
+
 config RTC_DRV_RS5C372
 	tristate "Ricoh R2025S/D, RS5C372A/B, RV5C386, RV5C387A"
 	help
@@ -732,6 +752,7 @@ config RTC_DRV_DS1216
 
 config RTC_DRV_DS1286
 	tristate "Dallas DS1286"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the Dallas DS1286 RTC chips.
 
@@ -743,6 +764,7 @@ config RTC_DRV_DS1302
 
 config RTC_DRV_DS1511
 	tristate "Dallas DS1511"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the
 	  Dallas DS1511 timekeeping/watchdog chip.
@@ -752,6 +774,7 @@ config RTC_DRV_DS1511
 
 config RTC_DRV_DS1553
 	tristate "Maxim/Dallas DS1553"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the
 	  Maxim/Dallas DS1553 timekeeping chip.
@@ -761,6 +784,7 @@ config RTC_DRV_DS1553
 
 config RTC_DRV_DS1742
 	tristate "Maxim/Dallas DS1742/1743"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the
 	  Maxim/Dallas DS1742/1743 timekeeping chip.
@@ -806,7 +830,7 @@ config RTC_DRV_DA9063
 
 config RTC_DRV_EFI
 	tristate "EFI RTC"
-	depends on EFI
+	depends on EFI && !X86
 	help
 	  If you say yes here you will get support for the EFI
 	  Real Time Clock.
@@ -816,6 +840,7 @@ config RTC_DRV_EFI
 
 config RTC_DRV_STK17TA8
 	tristate "Simtek STK17TA8"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the
 	  Simtek STK17TA8 timekeeping chip.
@@ -834,6 +859,7 @@ config RTC_DRV_M48T86
 
 config RTC_DRV_M48T35
 	tristate "ST M48T35"
+	depends on HAS_IOMEM
 	help
 	  If you say Y here you will get support for the
 	  ST M48T35 RTC chip.
@@ -843,6 +869,7 @@ config RTC_DRV_M48T35
 
 config RTC_DRV_M48T59
 	tristate "ST M48T59/M48T08/M48T02"
+	depends on HAS_IOMEM
 	help
 	  If you say Y here you will get support for the
 	  ST M48T59 RTC chip and compatible ST M48T08 and M48T02.
@@ -855,6 +882,7 @@ config RTC_DRV_M48T59
 
 config RTC_DRV_MSM6242
 	tristate "Oki MSM6242"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the Oki MSM6242
 	  timekeeping chip. It is used in some Amiga models (e.g. A2000).
@@ -864,6 +892,7 @@ config RTC_DRV_MSM6242
 
 config RTC_DRV_BQ4802
 	tristate "TI BQ4802"
+	depends on HAS_IOMEM
 	help
 	  If you say Y here you will get support for the TI
 	  BQ4802 RTC chip.
@@ -873,6 +902,7 @@ config RTC_DRV_BQ4802
 
 config RTC_DRV_RP5C01
 	tristate "Ricoh RP5C01"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the Ricoh RP5C01
 	  timekeeping chip. It is used in some Amiga models (e.g. A3000
@@ -1175,9 +1205,16 @@ config RTC_DRV_SUN4V
 	  If you say Y here you will get support for the Hypervisor
 	  based RTC on SUN4V systems.
 
+config RTC_DRV_SUN6I
+	tristate "Allwinner A31 RTC"
+	depends on MACH_SUN6I || MACH_SUN8I
+	help
+	  If you say Y here you will get support for the RTC found on
+	  Allwinner A31.
+
 config RTC_DRV_SUNXI
 	tristate "Allwinner sun4i/sun7i RTC"
-	depends on ARCH_SUNXI
+	depends on MACH_SUN4I || MACH_SUN7I
 	help
 	  If you say Y here you will get support for the RTC found on
 	  Allwinner A10/A20.
@@ -1198,7 +1235,7 @@ config RTC_DRV_TX4939
 
 config RTC_DRV_MV
 	tristate "Marvell SoC RTC"
-	depends on ARCH_KIRKWOOD || ARCH_DOVE || ARCH_MVEBU
+	depends on ARCH_DOVE || ARCH_MVEBU
 	help
 	  If you say yes here you will get support for the in-chip RTC
 	  that can be found in some of Marvell's SoC devices, such as
@@ -1367,6 +1404,7 @@ config RTC_DRV_MOXART
 
 config RTC_DRV_XGENE
 	tristate "APM X-Gene RTC"
+	depends on HAS_IOMEM
 	help
 	  If you say yes here you get support for the APM X-Gene SoC real time
 	  clock.
diff --git a/drivers/rtc/Makefile b/drivers/rtc/Makefile
index 56f061c7c815..b188323c096a 100644
--- a/drivers/rtc/Makefile
+++ b/drivers/rtc/Makefile
@@ -85,6 +85,7 @@ obj-$(CONFIG_RTC_DRV_MAX8998)	+= rtc-max8998.o
 obj-$(CONFIG_RTC_DRV_MAX8997)	+= rtc-max8997.o
 obj-$(CONFIG_RTC_DRV_MAX6902)	+= rtc-max6902.o
 obj-$(CONFIG_RTC_DRV_MAX77686)	+= rtc-max77686.o
+obj-$(CONFIG_RTC_DRV_MAX77802)  += rtc-max77802.o
 obj-$(CONFIG_RTC_DRV_MC13XXX)	+= rtc-mc13xxx.o
 obj-$(CONFIG_RTC_DRV_MCP795)	+= rtc-mcp795.o
 obj-$(CONFIG_RTC_DRV_MSM6242)	+= rtc-msm6242.o
@@ -109,6 +110,7 @@ obj-$(CONFIG_RTC_DRV_PUV3)	+= rtc-puv3.o
 obj-$(CONFIG_RTC_DRV_PXA)	+= rtc-pxa.o
 obj-$(CONFIG_RTC_DRV_R9701)	+= rtc-r9701.o
 obj-$(CONFIG_RTC_DRV_RC5T583)	+= rtc-rc5t583.o
+obj-$(CONFIG_RTC_DRV_RK808)	+= rtc-rk808.o
 obj-$(CONFIG_RTC_DRV_RP5C01)	+= rtc-rp5c01.o
 obj-$(CONFIG_RTC_DRV_RS5C313)	+= rtc-rs5c313.o
 obj-$(CONFIG_RTC_DRV_RS5C348)	+= rtc-rs5c348.o
@@ -128,6 +130,7 @@ obj-$(CONFIG_RTC_DRV_STARFIRE)	+= rtc-starfire.o
 obj-$(CONFIG_RTC_DRV_STK17TA8)	+= rtc-stk17ta8.o
 obj-$(CONFIG_RTC_DRV_STMP)	+= rtc-stmp3xxx.o
 obj-$(CONFIG_RTC_DRV_SUN4V)	+= rtc-sun4v.o
+obj-$(CONFIG_RTC_DRV_SUN6I)	+= rtc-sun6i.o
 obj-$(CONFIG_RTC_DRV_SUNXI)	+= rtc-sunxi.o
 obj-$(CONFIG_RTC_DRV_TEGRA)	+= rtc-tegra.o
 obj-$(CONFIG_RTC_DRV_TEST)	+= rtc-test.o
diff --git a/drivers/rtc/rtc-bq32k.c b/drivers/rtc/rtc-bq32k.c
index c74bf0dc52cc..314129e66d6e 100644
--- a/drivers/rtc/rtc-bq32k.c
+++ b/drivers/rtc/rtc-bq32k.c
@@ -2,10 +2,14 @@
  * Driver for TI BQ32000 RTC.
  *
  * Copyright (C) 2009 Semihalf.
+ * Copyright (C) 2014 Pavel Machek <pavel@denx.de>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
+ *
+ * You can get hardware description at
+ * http://www.ti.com/lit/ds/symlink/bq32000.pdf
  */
 
 #include <linux/module.h>
@@ -27,6 +31,10 @@
 #define BQ32K_CENT		0x40	/* Century flag */
 #define BQ32K_CENT_EN		0x80	/* Century flag enable bit */
 
+#define BQ32K_CALIBRATION	0x07	/* CAL_CFG1, calibration and control */
+#define BQ32K_TCH2		0x08	/* Trickle charge enable */
+#define BQ32K_CFG2		0x09	/* Trickle charger control */
+
 struct bq32k_regs {
 	uint8_t		seconds;
 	uint8_t		minutes;
@@ -122,6 +130,57 @@ static const struct rtc_class_ops bq32k_rtc_ops = {
 	.set_time	= bq32k_rtc_set_time,
 };
 
+static int trickle_charger_of_init(struct device *dev, struct device_node *node)
+{
+	unsigned char reg;
+	int error;
+	u32 ohms = 0;
+
+	if (of_property_read_u32(node, "trickle-resistor-ohms" , &ohms))
+		return 0;
+
+	switch (ohms) {
+	case 180+940:
+		/*
+		 * TCHE[3:0] == 0x05, TCH2 == 1, TCFE == 0 (charging
+		 * over diode and 940ohm resistor)
+		 */
+
+		if (of_property_read_bool(node, "trickle-diode-disable")) {
+			dev_err(dev, "diode and resistor mismatch\n");
+			return -EINVAL;
+		}
+		reg = 0x05;
+		break;
+
+	case 180+20000:
+		/* diode disabled */
+
+		if (!of_property_read_bool(node, "trickle-diode-disable")) {
+			dev_err(dev, "bq32k: diode and resistor mismatch\n");
+			return -EINVAL;
+		}
+		reg = 0x25;
+		break;
+
+	default:
+		dev_err(dev, "invalid resistor value (%d)\n", ohms);
+		return -EINVAL;
+	}
+
+	error = bq32k_write(dev, &reg, BQ32K_CFG2, 1);
+	if (error)
+		return error;
+
+	reg = 0x20;
+	error = bq32k_write(dev, &reg, BQ32K_TCH2, 1);
+	if (error)
+		return error;
+
+	dev_info(dev, "Enabled trickle RTC battery charge.\n");
+	return 0;
+}
+
 static int bq32k_probe(struct i2c_client *client,
 				const struct i2c_device_id *id)
 {
@@ -153,6 +212,9 @@ static int bq32k_probe(struct i2c_client *client,
 	if (error)
 		return error;
 
+	if (client && client->dev.of_node)
+		trickle_charger_of_init(dev, client->dev.of_node);
+
 	rtc = devm_rtc_device_register(&client->dev, bq32k_driver.driver.name,
 						&bq32k_rtc_ops, THIS_MODULE);
 	if (IS_ERR(rtc))
diff --git a/drivers/rtc/rtc-cmos.c b/drivers/rtc/rtc-cmos.c
index b0e4a3eb33c7..5b2e76159b41 100644
--- a/drivers/rtc/rtc-cmos.c
+++ b/drivers/rtc/rtc-cmos.c
@@ -856,7 +856,7 @@ static void __exit cmos_do_remove(struct device *dev)
 	cmos->dev = NULL;
 }
 
-#ifdef	CONFIG_PM_SLEEP
+#ifdef CONFIG_PM
 
 static int cmos_suspend(struct device *dev)
 {
@@ -907,6 +907,8 @@ static inline int cmos_poweroff(struct device *dev)
 	return cmos_suspend(dev);
 }
 
+#ifdef	CONFIG_PM_SLEEP
+
 static int cmos_resume(struct device *dev)
 {
 	struct cmos_rtc	*cmos = dev_get_drvdata(dev);
@@ -954,6 +956,7 @@ static int cmos_resume(struct device *dev)
 	return 0;
 }
 
+#endif
 #else
 
 static inline int cmos_poweroff(struct device *dev)
diff --git a/drivers/rtc/rtc-ds1307.c b/drivers/rtc/rtc-ds1307.c
index f03d5ba96db1..bb43cf703efc 100644
--- a/drivers/rtc/rtc-ds1307.c
+++ b/drivers/rtc/rtc-ds1307.c
@@ -126,9 +126,14 @@ struct chip_desc {
 	u16			nvram_offset;
 	u16			nvram_size;
 	u16			trickle_charger_reg;
+	u8			trickle_charger_setup;
+	u8			(*do_trickle_setup)(struct i2c_client *, uint32_t, bool);
 };
 
-static const struct chip_desc chips[last_ds_type] = {
+static u8 do_trickle_setup_ds1339(struct i2c_client *,
+				  uint32_t ohms, bool diode);
+
+static struct chip_desc chips[last_ds_type] = {
 	[ds_1307] = {
 		.nvram_offset	= 8,
 		.nvram_size	= 56,
@@ -143,6 +148,7 @@ static const struct chip_desc chips[last_ds_type] = {
 	[ds_1339] = {
 		.alarm		= 1,
 		.trickle_charger_reg = 0x10,
+		.do_trickle_setup = &do_trickle_setup_ds1339,
 	},
 	[ds_1340] = {
 		.trickle_charger_reg = 0x08,
@@ -833,15 +839,58 @@ ds1307_nvram_write(struct file *filp, struct kobject *kobj,
 	return count;
 }
 
+
 /*----------------------------------------------------------------------*/
 
+static u8 do_trickle_setup_ds1339(struct i2c_client *client,
+				  uint32_t ohms, bool diode)
+{
+	u8 setup = (diode) ? DS1307_TRICKLE_CHARGER_DIODE :
+		DS1307_TRICKLE_CHARGER_NO_DIODE;
+
+	switch (ohms) {
+	case 250:
+		setup |= DS1307_TRICKLE_CHARGER_250_OHM;
+		break;
+	case 2000:
+		setup |= DS1307_TRICKLE_CHARGER_2K_OHM;
+		break;
+	case 4000:
+		setup |= DS1307_TRICKLE_CHARGER_4K_OHM;
+		break;
+	default:
+		dev_warn(&client->dev,
+			 "Unsupported ohm value %u in dt\n", ohms);
+		return 0;
+	}
+	return setup;
+}
+
+static void ds1307_trickle_of_init(struct i2c_client *client,
+				   struct chip_desc *chip)
+{
+	uint32_t ohms = 0;
+	bool diode = true;
+
+	if (!chip->do_trickle_setup)
+		goto out;
+	if (of_property_read_u32(client->dev.of_node, "trickle-resistor-ohms" , &ohms))
+		goto out;
+	if (of_property_read_bool(client->dev.of_node, "trickle-diode-disable"))
+		diode = false;
+	chip->trickle_charger_setup = chip->do_trickle_setup(client,
+							     ohms, diode);
+out:
+	return;
+}
+
 static int ds1307_probe(struct i2c_client *client,
 			const struct i2c_device_id *id)
 {
 	struct ds1307		*ds1307;
 	int			err = -ENODEV;
 	int			tmp;
-	const struct chip_desc	*chip = &chips[id->driver_data];
+	struct chip_desc	*chip = &chips[id->driver_data];
 	struct i2c_adapter	*adapter = to_i2c_adapter(client->dev.parent);
 	bool			want_irq = false;
 	unsigned char		*buf;
@@ -866,9 +915,19 @@ static int ds1307_probe(struct i2c_client *client,
 	ds1307->client	= client;
 	ds1307->type	= id->driver_data;
 
-	if (pdata && pdata->trickle_charger_setup && chip->trickle_charger_reg)
+	if (!pdata && client->dev.of_node)
+		ds1307_trickle_of_init(client, chip);
+	else if (pdata && pdata->trickle_charger_setup)
+		chip->trickle_charger_setup = pdata->trickle_charger_setup;
+
+	if (chip->trickle_charger_setup && chip->trickle_charger_reg) {
+		dev_dbg(&client->dev, "writing trickle charger info 0x%x to 0x%x\n",
+		    DS13XX_TRICKLE_CHARGER_MAGIC | chip->trickle_charger_setup,
+		    chip->trickle_charger_reg);
 		i2c_smbus_write_byte_data(client, chip->trickle_charger_reg,
-			DS13XX_TRICKLE_CHARGER_MAGIC | pdata->trickle_charger_setup);
+		    DS13XX_TRICKLE_CHARGER_MAGIC |
+		    chip->trickle_charger_setup);
+	}
 
 	buf = ds1307->regs;
 	if (i2c_check_functionality(adapter, I2C_FUNC_SMBUS_I2C_BLOCK)) {
diff --git a/drivers/rtc/rtc-efi.c b/drivers/rtc/rtc-efi.c
index 8225b89de810..53b589dc34eb 100644
--- a/drivers/rtc/rtc-efi.c
+++ b/drivers/rtc/rtc-efi.c
@@ -232,6 +232,8 @@ static struct platform_driver efi_rtc_driver = {
 
 module_platform_driver_probe(efi_rtc_driver, efi_rtc_probe);
 
+MODULE_ALIAS("platform:rtc-efi");
 MODULE_AUTHOR("dann frazier <dannf@hp.com>");
 MODULE_LICENSE("GPL");
 MODULE_DESCRIPTION("EFI RTC driver");
+MODULE_ALIAS("platform:rtc-efi");
diff --git a/drivers/rtc/rtc-isl12022.c b/drivers/rtc/rtc-isl12022.c
index aa55f081c505..ee3ba7e6b45e 100644
--- a/drivers/rtc/rtc-isl12022.c
+++ b/drivers/rtc/rtc-isl12022.c
@@ -274,7 +274,7 @@ static int isl12022_probe(struct i2c_client *client,
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id isl12022_dt_match[] = {
+static const struct of_device_id isl12022_dt_match[] = {
 	{ .compatible = "isl,isl12022" },
 	{ },
 };
diff --git a/drivers/rtc/rtc-max77686.c b/drivers/rtc/rtc-max77686.c
index d20a7f0786eb..cf73e969c8cc 100644
--- a/drivers/rtc/rtc-max77686.c
+++ b/drivers/rtc/rtc-max77686.c
@@ -32,15 +32,6 @@
 #define RTC_UDR_MASK			(1 << RTC_UDR_SHIFT)
 #define RTC_RBUDR_SHIFT			4
 #define RTC_RBUDR_MASK			(1 << RTC_RBUDR_SHIFT)
-/* WTSR and SMPL Register */
-#define WTSRT_SHIFT			0
-#define SMPLT_SHIFT			2
-#define WTSR_EN_SHIFT			6
-#define SMPL_EN_SHIFT			7
-#define WTSRT_MASK			(3 << WTSRT_SHIFT)
-#define SMPLT_MASK			(3 << SMPLT_SHIFT)
-#define WTSR_EN_MASK			(1 << WTSR_EN_SHIFT)
-#define SMPL_EN_MASK			(1 << SMPL_EN_SHIFT)
 /* RTC Hour register */
 #define HOUR_PM_SHIFT			6
 #define HOUR_PM_MASK			(1 << HOUR_PM_SHIFT)
@@ -49,7 +40,6 @@
 #define ALARM_ENABLE_MASK		(1 << ALARM_ENABLE_SHIFT)
 
 #define MAX77686_RTC_UPDATE_DELAY	16
-#undef MAX77686_RTC_WTSR_SMPL
 
 enum {
 	RTC_SEC = 0,
@@ -80,16 +70,6 @@ enum MAX77686_RTC_OP {
 	MAX77686_RTC_READ,
 };
 
-static inline int max77686_rtc_calculate_wday(u8 shifted)
-{
-	int counter = -1;
-	while (shifted) {
-		shifted >>= 1;
-		counter++;
-	}
-	return counter;
-}
-
 static void max77686_rtc_data_to_tm(u8 *data, struct rtc_time *tm,
 				   int rtc_24hr_mode)
 {
@@ -103,7 +83,8 @@ static void max77686_rtc_data_to_tm(u8 *data, struct rtc_time *tm,
 			tm->tm_hour += 12;
 	}
 
-	tm->tm_wday = max77686_rtc_calculate_wday(data[RTC_WEEKDAY] & 0x7f);
+	/* Only a single bit is set in data[], so fls() would be equivalent */
+	tm->tm_wday = ffs(data[RTC_WEEKDAY] & 0x7f) - 1;
 	tm->tm_mday = data[RTC_DATE] & 0x1f;
 	tm->tm_mon = (data[RTC_MONTH] & 0x0f) - 1;
 	tm->tm_year = (data[RTC_YEAR] & 0x7f) + 100;
@@ -412,64 +393,6 @@ static const struct rtc_class_ops max77686_rtc_ops = {
 	.alarm_irq_enable = max77686_rtc_alarm_irq_enable,
 };
 
-#ifdef MAX77686_RTC_WTSR_SMPL
-static void max77686_rtc_enable_wtsr(struct max77686_rtc_info *info, bool enable)
-{
-	int ret;
-	unsigned int val, mask;
-
-	if (enable)
-		val = (1 << WTSR_EN_SHIFT) | (3 << WTSRT_SHIFT);
-	else
-		val = 0;
-
-	mask = WTSR_EN_MASK | WTSRT_MASK;
-
-	dev_info(info->dev, "%s: %s WTSR\n", __func__,
-			enable ? "enable" : "disable");
-
-	ret = regmap_update_bits(info->max77686->rtc_regmap,
-				 MAX77686_WTSR_SMPL_CNTL, mask, val);
-	if (ret < 0) {
-		dev_err(info->dev, "%s: fail to update WTSR reg(%d)\n",
-				__func__, ret);
-		return;
-	}
-
-	max77686_rtc_update(info, MAX77686_RTC_WRITE);
-}
-
-static void max77686_rtc_enable_smpl(struct max77686_rtc_info *info, bool enable)
-{
-	int ret;
-	unsigned int val, mask;
-
-	if (enable)
-		val = (1 << SMPL_EN_SHIFT) | (0 << SMPLT_SHIFT);
-	else
-		val = 0;
-
-	mask = SMPL_EN_MASK | SMPLT_MASK;
-
-	dev_info(info->dev, "%s: %s SMPL\n", __func__,
-			enable ? "enable" : "disable");
-
-	ret = regmap_update_bits(info->max77686->rtc_regmap,
-				 MAX77686_WTSR_SMPL_CNTL, mask, val);
-	if (ret < 0) {
-		dev_err(info->dev, "%s: fail to update SMPL reg(%d)\n",
-				__func__, ret);
-		return;
-	}
-
-	max77686_rtc_update(info, MAX77686_RTC_WRITE);
-
-	val = 0;
-	regmap_read(info->max77686->rtc_regmap, MAX77686_WTSR_SMPL_CNTL, &val);
-	dev_info(info->dev, "%s: WTSR_SMPL(0x%02x)\n", __func__, val);
-}
-#endif /* MAX77686_RTC_WTSR_SMPL */
-
 static int max77686_rtc_init_reg(struct max77686_rtc_info *info)
 {
 	u8 data[2];
@@ -519,19 +442,12 @@ static int max77686_rtc_probe(struct platform_device *pdev)
 		goto err_rtc;
 	}
 
-#ifdef MAX77686_RTC_WTSR_SMPL
-	max77686_rtc_enable_wtsr(info, true);
-	max77686_rtc_enable_smpl(info, true);
-#endif
-
 	device_init_wakeup(&pdev->dev, 1);
 
 	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "max77686-rtc",
 					&max77686_rtc_ops, THIS_MODULE);
 
 	if (IS_ERR(info->rtc_dev)) {
-		dev_info(&pdev->dev, "%s: fail\n", __func__);
-
 		ret = PTR_ERR(info->rtc_dev);
 		dev_err(&pdev->dev, "Failed to register RTC device: %d\n", ret);
 		if (ret == 0)
@@ -539,6 +455,12 @@ static int max77686_rtc_probe(struct platform_device *pdev)
 		goto err_rtc;
 	}
 
+	if (!max77686->rtc_irq_data) {
+		ret = -EINVAL;
+		dev_err(&pdev->dev, "%s: no RTC regmap IRQ chip\n", __func__);
+		goto err_rtc;
+	}
+
 	info->virq = regmap_irq_get_virq(max77686->rtc_irq_data,
 					 MAX77686_RTCIRQ_RTCA1);
 	if (!info->virq) {
@@ -556,33 +478,33 @@ err_rtc:
 	return ret;
 }
 
-static void max77686_rtc_shutdown(struct platform_device *pdev)
+#ifdef CONFIG_PM_SLEEP
+static int max77686_rtc_suspend(struct device *dev)
 {
-#ifdef MAX77686_RTC_WTSR_SMPL
-	struct max77686_rtc_info *info = platform_get_drvdata(pdev);
-	int i;
-	u8 val = 0;
-
-	for (i = 0; i < 3; i++) {
-		max77686_rtc_enable_wtsr(info, false);
-		regmap_read(info->max77686->rtc_regmap, MAX77686_WTSR_SMPL_CNTL, &val);
-		dev_info(info->dev, "%s: WTSR_SMPL reg(0x%02x)\n", __func__,
-				val);
-		if (val & WTSR_EN_MASK) {
-			dev_emerg(info->dev, "%s: fail to disable WTSR\n",
-					__func__);
-		} else {
-			dev_info(info->dev, "%s: success to disable WTSR\n",
-					__func__);
-			break;
-		}
+	if (device_may_wakeup(dev)) {
+		struct max77686_rtc_info *info = dev_get_drvdata(dev);
+
+		return enable_irq_wake(info->virq);
 	}
 
-	/* Disable SMPL when power off */
-	max77686_rtc_enable_smpl(info, false);
-#endif /* MAX77686_RTC_WTSR_SMPL */
+	return 0;
 }
 
+static int max77686_rtc_resume(struct device *dev)
+{
+	if (device_may_wakeup(dev)) {
+		struct max77686_rtc_info *info = dev_get_drvdata(dev);
+
+		return disable_irq_wake(info->virq);
+	}
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(max77686_rtc_pm_ops,
+			 max77686_rtc_suspend, max77686_rtc_resume);
+
 static const struct platform_device_id rtc_id[] = {
 	{ "max77686-rtc", 0 },
 	{},
@@ -592,9 +514,9 @@ static struct platform_driver max77686_rtc_driver = {
 	.driver		= {
 		.name	= "max77686-rtc",
 		.owner	= THIS_MODULE,
+		.pm	= &max77686_rtc_pm_ops,
 	},
 	.probe		= max77686_rtc_probe,
-	.shutdown	= max77686_rtc_shutdown,
 	.id_table	= rtc_id,
 };
 
diff --git a/drivers/rtc/rtc-max77802.c b/drivers/rtc/rtc-max77802.c
new file mode 100644
index 000000000000..566471335b33
--- /dev/null
+++ b/drivers/rtc/rtc-max77802.c
@@ -0,0 +1,502 @@
+/*
+ * RTC driver for Maxim MAX77802
+ *
+ * Copyright (C) 2013 Google, Inc
+ *
+ * Copyright (C) 2012 Samsung Electronics Co.Ltd
+ *
+ *  based on rtc-max8997.c
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ *
+ */
+
+#include <linux/slab.h>
+#include <linux/rtc.h>
+#include <linux/delay.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/max77686-private.h>
+#include <linux/irqdomain.h>
+#include <linux/regmap.h>
+
+/* RTC Control Register */
+#define BCD_EN_SHIFT			0
+#define BCD_EN_MASK			(1 << BCD_EN_SHIFT)
+#define MODEL24_SHIFT			1
+#define MODEL24_MASK			(1 << MODEL24_SHIFT)
+/* RTC Update Register1 */
+#define RTC_UDR_SHIFT			0
+#define RTC_UDR_MASK			(1 << RTC_UDR_SHIFT)
+#define RTC_RBUDR_SHIFT			4
+#define RTC_RBUDR_MASK			(1 << RTC_RBUDR_SHIFT)
+/* RTC Hour register */
+#define HOUR_PM_SHIFT			6
+#define HOUR_PM_MASK			(1 << HOUR_PM_SHIFT)
+/* RTC Alarm Enable */
+#define ALARM_ENABLE_SHIFT		7
+#define ALARM_ENABLE_MASK		(1 << ALARM_ENABLE_SHIFT)
+
+/* For the RTCAE1 register, we write this value to enable the alarm */
+#define ALARM_ENABLE_VALUE		0x77
+
+#define MAX77802_RTC_UPDATE_DELAY_US	200
+
+enum {
+	RTC_SEC = 0,
+	RTC_MIN,
+	RTC_HOUR,
+	RTC_WEEKDAY,
+	RTC_MONTH,
+	RTC_YEAR,
+	RTC_DATE,
+	RTC_NR_TIME
+};
+
+struct max77802_rtc_info {
+	struct device		*dev;
+	struct max77686_dev	*max77802;
+	struct i2c_client	*rtc;
+	struct rtc_device	*rtc_dev;
+	struct mutex		lock;
+
+	struct regmap		*regmap;
+
+	int virq;
+	int rtc_24hr_mode;
+};
+
+enum MAX77802_RTC_OP {
+	MAX77802_RTC_WRITE,
+	MAX77802_RTC_READ,
+};
+
+static void max77802_rtc_data_to_tm(u8 *data, struct rtc_time *tm,
+				   int rtc_24hr_mode)
+{
+	tm->tm_sec = data[RTC_SEC] & 0xff;
+	tm->tm_min = data[RTC_MIN] & 0xff;
+	if (rtc_24hr_mode)
+		tm->tm_hour = data[RTC_HOUR] & 0x1f;
+	else {
+		tm->tm_hour = data[RTC_HOUR] & 0x0f;
+		if (data[RTC_HOUR] & HOUR_PM_MASK)
+			tm->tm_hour += 12;
+	}
+
+	/* Only a single bit is set in data[], so fls() would be equivalent */
+	tm->tm_wday = ffs(data[RTC_WEEKDAY] & 0xff) - 1;
+	tm->tm_mday = data[RTC_DATE] & 0x1f;
+	tm->tm_mon = (data[RTC_MONTH] & 0x0f) - 1;
+
+	tm->tm_year = data[RTC_YEAR] & 0xff;
+	tm->tm_yday = 0;
+	tm->tm_isdst = 0;
+}
+
+static int max77802_rtc_tm_to_data(struct rtc_time *tm, u8 *data)
+{
+	data[RTC_SEC] = tm->tm_sec;
+	data[RTC_MIN] = tm->tm_min;
+	data[RTC_HOUR] = tm->tm_hour;
+	data[RTC_WEEKDAY] = 1 << tm->tm_wday;
+	data[RTC_DATE] = tm->tm_mday;
+	data[RTC_MONTH] = tm->tm_mon + 1;
+	data[RTC_YEAR] = tm->tm_year;
+
+	return 0;
+}
+
+static int max77802_rtc_update(struct max77802_rtc_info *info,
+	enum MAX77802_RTC_OP op)
+{
+	int ret;
+	unsigned int data;
+
+	if (op == MAX77802_RTC_WRITE)
+		data = 1 << RTC_UDR_SHIFT;
+	else
+		data = 1 << RTC_RBUDR_SHIFT;
+
+	ret = regmap_update_bits(info->max77802->regmap,
+				 MAX77802_RTC_UPDATE0, data, data);
+	if (ret < 0)
+		dev_err(info->dev, "%s: fail to write update reg(ret=%d, data=0x%x)\n",
+				__func__, ret, data);
+	else {
+		/* Minimum delay required before RTC update. */
+		usleep_range(MAX77802_RTC_UPDATE_DELAY_US,
+			     MAX77802_RTC_UPDATE_DELAY_US * 2);
+	}
+
+	return ret;
+}
+
+static int max77802_rtc_read_time(struct device *dev, struct rtc_time *tm)
+{
+	struct max77802_rtc_info *info = dev_get_drvdata(dev);
+	u8 data[RTC_NR_TIME];
+	int ret;
+
+	mutex_lock(&info->lock);
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_READ);
+	if (ret < 0)
+		goto out;
+
+	ret = regmap_bulk_read(info->max77802->regmap,
+				MAX77802_RTC_SEC, data, RTC_NR_TIME);
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to read time reg(%d)\n", __func__,
+			ret);
+		goto out;
+	}
+
+	max77802_rtc_data_to_tm(data, tm, info->rtc_24hr_mode);
+
+	ret = rtc_valid_tm(tm);
+
+out:
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int max77802_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct max77802_rtc_info *info = dev_get_drvdata(dev);
+	u8 data[RTC_NR_TIME];
+	int ret;
+
+	ret = max77802_rtc_tm_to_data(tm, data);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&info->lock);
+
+	ret = regmap_bulk_write(info->max77802->regmap,
+				 MAX77802_RTC_SEC, data, RTC_NR_TIME);
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to write time reg(%d)\n", __func__,
+			ret);
+		goto out;
+	}
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_WRITE);
+
+out:
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int max77802_rtc_read_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct max77802_rtc_info *info = dev_get_drvdata(dev);
+	u8 data[RTC_NR_TIME];
+	unsigned int val;
+	int ret;
+
+	mutex_lock(&info->lock);
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_READ);
+	if (ret < 0)
+		goto out;
+
+	ret = regmap_bulk_read(info->max77802->regmap,
+				 MAX77802_ALARM1_SEC, data, RTC_NR_TIME);
+	if (ret < 0) {
+		dev_err(info->dev, "%s:%d fail to read alarm reg(%d)\n",
+				__func__, __LINE__, ret);
+		goto out;
+	}
+
+	max77802_rtc_data_to_tm(data, &alrm->time, info->rtc_24hr_mode);
+
+	alrm->enabled = 0;
+	ret = regmap_read(info->max77802->regmap,
+			  MAX77802_RTC_AE1, &val);
+	if (ret < 0) {
+		dev_err(info->dev, "%s:%d fail to read alarm enable(%d)\n",
+			__func__, __LINE__, ret);
+		goto out;
+	}
+	if (val)
+		alrm->enabled = 1;
+
+	alrm->pending = 0;
+	ret = regmap_read(info->max77802->regmap, MAX77802_REG_STATUS2, &val);
+	if (ret < 0) {
+		dev_err(info->dev, "%s:%d fail to read status2 reg(%d)\n",
+				__func__, __LINE__, ret);
+		goto out;
+	}
+
+	if (val & (1 << 2)) /* RTCA1 */
+		alrm->pending = 1;
+
+out:
+	mutex_unlock(&info->lock);
+	return 0;
+}
+
+static int max77802_rtc_stop_alarm(struct max77802_rtc_info *info)
+{
+	int ret;
+
+	if (!mutex_is_locked(&info->lock))
+		dev_warn(info->dev, "%s: should have mutex locked\n", __func__);
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_READ);
+	if (ret < 0)
+		goto out;
+
+	ret = regmap_write(info->max77802->regmap,
+			   MAX77802_RTC_AE1, 0);
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to write alarm reg(%d)\n",
+			__func__, ret);
+		goto out;
+	}
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_WRITE);
+out:
+	return ret;
+}
+
+static int max77802_rtc_start_alarm(struct max77802_rtc_info *info)
+{
+	int ret;
+
+	if (!mutex_is_locked(&info->lock))
+		dev_warn(info->dev, "%s: should have mutex locked\n",
+			 __func__);
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_READ);
+	if (ret < 0)
+		goto out;
+
+	ret = regmap_write(info->max77802->regmap,
+				   MAX77802_RTC_AE1,
+				   ALARM_ENABLE_VALUE);
+
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to read alarm reg(%d)\n",
+				__func__, ret);
+		goto out;
+	}
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_WRITE);
+out:
+	return ret;
+}
+
+static int max77802_rtc_set_alarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct max77802_rtc_info *info = dev_get_drvdata(dev);
+	u8 data[RTC_NR_TIME];
+	int ret;
+
+	ret = max77802_rtc_tm_to_data(&alrm->time, data);
+	if (ret < 0)
+		return ret;
+
+	mutex_lock(&info->lock);
+
+	ret = max77802_rtc_stop_alarm(info);
+	if (ret < 0)
+		goto out;
+
+	ret = regmap_bulk_write(info->max77802->regmap,
+				 MAX77802_ALARM1_SEC, data, RTC_NR_TIME);
+
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to write alarm reg(%d)\n",
+				__func__, ret);
+		goto out;
+	}
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_WRITE);
+	if (ret < 0)
+		goto out;
+
+	if (alrm->enabled)
+		ret = max77802_rtc_start_alarm(info);
+out:
+	mutex_unlock(&info->lock);
+	return ret;
+}
+
+static int max77802_rtc_alarm_irq_enable(struct device *dev,
+					 unsigned int enabled)
+{
+	struct max77802_rtc_info *info = dev_get_drvdata(dev);
+	int ret;
+
+	mutex_lock(&info->lock);
+	if (enabled)
+		ret = max77802_rtc_start_alarm(info);
+	else
+		ret = max77802_rtc_stop_alarm(info);
+	mutex_unlock(&info->lock);
+
+	return ret;
+}
+
+static irqreturn_t max77802_rtc_alarm_irq(int irq, void *data)
+{
+	struct max77802_rtc_info *info = data;
+
+	dev_dbg(info->dev, "%s:irq(%d)\n", __func__, irq);
+
+	rtc_update_irq(info->rtc_dev, 1, RTC_IRQF | RTC_AF);
+
+	return IRQ_HANDLED;
+}
+
+static const struct rtc_class_ops max77802_rtc_ops = {
+	.read_time = max77802_rtc_read_time,
+	.set_time = max77802_rtc_set_time,
+	.read_alarm = max77802_rtc_read_alarm,
+	.set_alarm = max77802_rtc_set_alarm,
+	.alarm_irq_enable = max77802_rtc_alarm_irq_enable,
+};
+
+static int max77802_rtc_init_reg(struct max77802_rtc_info *info)
+{
+	u8 data[2];
+	int ret;
+
+	max77802_rtc_update(info, MAX77802_RTC_READ);
+
+	/* Set RTC control register : Binary mode, 24hour mdoe */
+	data[0] = (1 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
+	data[1] = (0 << BCD_EN_SHIFT) | (1 << MODEL24_SHIFT);
+
+	info->rtc_24hr_mode = 1;
+
+	ret = regmap_bulk_write(info->max77802->regmap,
+				MAX77802_RTC_CONTROLM, data, ARRAY_SIZE(data));
+	if (ret < 0) {
+		dev_err(info->dev, "%s: fail to write controlm reg(%d)\n",
+				__func__, ret);
+		return ret;
+	}
+
+	ret = max77802_rtc_update(info, MAX77802_RTC_WRITE);
+	return ret;
+}
+
+static int max77802_rtc_probe(struct platform_device *pdev)
+{
+	struct max77686_dev *max77802 = dev_get_drvdata(pdev->dev.parent);
+	struct max77802_rtc_info *info;
+	int ret;
+
+	dev_dbg(&pdev->dev, "%s\n", __func__);
+
+	info = devm_kzalloc(&pdev->dev, sizeof(struct max77802_rtc_info),
+			    GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
+
+	mutex_init(&info->lock);
+	info->dev = &pdev->dev;
+	info->max77802 = max77802;
+	info->rtc = max77802->i2c;
+
+	platform_set_drvdata(pdev, info);
+
+	ret = max77802_rtc_init_reg(info);
+
+	if (ret < 0) {
+		dev_err(&pdev->dev, "Failed to initialize RTC reg:%d\n", ret);
+		return ret;
+	}
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	info->rtc_dev = devm_rtc_device_register(&pdev->dev, "max77802-rtc",
+						 &max77802_rtc_ops, THIS_MODULE);
+
+	if (IS_ERR(info->rtc_dev)) {
+		ret = PTR_ERR(info->rtc_dev);
+		dev_err(&pdev->dev, "Failed to register RTC device: %d\n", ret);
+		if (ret == 0)
+			ret = -EINVAL;
+		return ret;
+	}
+
+	if (!max77802->rtc_irq_data) {
+		dev_err(&pdev->dev, "No RTC regmap IRQ chip\n");
+		return -EINVAL;
+	}
+
+	info->virq = regmap_irq_get_virq(max77802->rtc_irq_data,
+					 MAX77686_RTCIRQ_RTCA1);
+
+	if (info->virq <= 0) {
+		dev_err(&pdev->dev, "Failed to get virtual IRQ %d\n",
+			MAX77686_RTCIRQ_RTCA1);
+		return -EINVAL;
+	}
+
+	ret = devm_request_threaded_irq(&pdev->dev, info->virq, NULL,
+					max77802_rtc_alarm_irq, 0, "rtc-alarm1",
+					info);
+	if (ret < 0)
+		dev_err(&pdev->dev, "Failed to request alarm IRQ: %d: %d\n",
+			info->virq, ret);
+
+	return ret;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int max77802_rtc_suspend(struct device *dev)
+{
+	if (device_may_wakeup(dev)) {
+		struct max77802_rtc_info *info = dev_get_drvdata(dev);
+
+		return enable_irq_wake(info->virq);
+	}
+
+	return 0;
+}
+
+static int max77802_rtc_resume(struct device *dev)
+{
+	if (device_may_wakeup(dev)) {
+		struct max77802_rtc_info *info = dev_get_drvdata(dev);
+
+		return disable_irq_wake(info->virq);
+	}
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(max77802_rtc_pm_ops,
+			 max77802_rtc_suspend, max77802_rtc_resume);
+
+static const struct platform_device_id rtc_id[] = {
+	{ "max77802-rtc", 0 },
+	{},
+};
+
+static struct platform_driver max77802_rtc_driver = {
+	.driver		= {
+		.name	= "max77802-rtc",
+		.owner	= THIS_MODULE,
+		.pm	= &max77802_rtc_pm_ops,
+	},
+	.probe		= max77802_rtc_probe,
+	.id_table	= rtc_id,
+};
+
+module_platform_driver(max77802_rtc_driver);
+
+MODULE_DESCRIPTION("Maxim MAX77802 RTC driver");
+MODULE_AUTHOR("Simon Glass <sjg@chromium.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/rtc/rtc-mpc5121.c b/drivers/rtc/rtc-mpc5121.c
index dc4f14255cc3..3b965ad6f4d5 100644
--- a/drivers/rtc/rtc-mpc5121.c
+++ b/drivers/rtc/rtc-mpc5121.c
@@ -401,7 +401,7 @@ static int mpc5121_rtc_remove(struct platform_device *op)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id mpc5121_rtc_match[] = {
+static const struct of_device_id mpc5121_rtc_match[] = {
 	{ .compatible = "fsl,mpc5121-rtc", },
 	{ .compatible = "fsl,mpc5200-rtc", },
 	{},
diff --git a/drivers/rtc/rtc-pcf8563.c b/drivers/rtc/rtc-pcf8563.c
index 5a197d9dc7e7..c2ef0a22ee94 100644
--- a/drivers/rtc/rtc-pcf8563.c
+++ b/drivers/rtc/rtc-pcf8563.c
@@ -167,8 +167,8 @@ static irqreturn_t pcf8563_irq(int irq, void *dev_id)
 	char pending;
 
 	err = pcf8563_get_alarm_mode(pcf8563->client, NULL, &pending);
-	if (err < 0)
-		return err;
+	if (err)
+		return IRQ_NONE;
 
 	if (pending) {
 		rtc_update_irq(pcf8563->rtc, 1, RTC_IRQF | RTC_AF);
diff --git a/drivers/rtc/rtc-pcf8583.c b/drivers/rtc/rtc-pcf8583.c
index c2639845186b..5911a6dca291 100644
--- a/drivers/rtc/rtc-pcf8583.c
+++ b/drivers/rtc/rtc-pcf8583.c
@@ -176,7 +176,11 @@ static int pcf8583_rtc_read_time(struct device *dev, struct rtc_time *tm)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	unsigned char ctrl, year[2];
-	struct rtc_mem mem = { CMOS_YEAR, sizeof(year), year };
+	struct rtc_mem mem = {
+		.loc = CMOS_YEAR,
+		.nr = sizeof(year),
+		.data = year
+	};
 	int real_year, year_offset, err;
 
 	/*
@@ -222,8 +226,16 @@ static int pcf8583_rtc_set_time(struct device *dev, struct rtc_time *tm)
 {
 	struct i2c_client *client = to_i2c_client(dev);
 	unsigned char year[2], chk;
-	struct rtc_mem cmos_year  = { CMOS_YEAR, sizeof(year), year };
-	struct rtc_mem cmos_check = { CMOS_CHECKSUM, 1, &chk };
+	struct rtc_mem cmos_year  = {
+		.loc = CMOS_YEAR,
+		.nr = sizeof(year),
+		.data = year
+	};
+	struct rtc_mem cmos_check = {
+		.loc = CMOS_CHECKSUM,
+		.nr = 1,
+		.data = &chk
+	};
 	unsigned int proper_year = tm->tm_year + 1900;
 	int ret;
 
diff --git a/drivers/rtc/rtc-rk808.c b/drivers/rtc/rtc-rk808.c
new file mode 100644
index 000000000000..df42257668ac
--- /dev/null
+++ b/drivers/rtc/rtc-rk808.c
@@ -0,0 +1,414 @@
+/*
+ * RTC driver for Rockchip RK808
+ *
+ * Copyright (c) 2014, Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * Author: Chris Zhong <zyw@rock-chips.com>
+ * Author: Zhang Qing <zhangqing@rock-chips.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/rtc.h>
+#include <linux/bcd.h>
+#include <linux/mfd/rk808.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+
+/* RTC_CTRL_REG bitfields */
+#define BIT_RTC_CTRL_REG_STOP_RTC_M		BIT(0)
+
+/* RK808 has a shadowed register for saving a "frozen" RTC time.
+ * When user setting "GET_TIME" to 1, the time will save in this shadowed
+ * register. If set "READSEL" to 1, user read rtc time register, actually
+ * get the time of that moment. If we need the real time, clr this bit.
+ */
+#define BIT_RTC_CTRL_REG_RTC_GET_TIME		BIT(6)
+#define BIT_RTC_CTRL_REG_RTC_READSEL_M		BIT(7)
+#define BIT_RTC_INTERRUPTS_REG_IT_ALARM_M	BIT(3)
+#define RTC_STATUS_MASK		0xFE
+
+#define SECONDS_REG_MSK		0x7F
+#define MINUTES_REG_MAK		0x7F
+#define HOURS_REG_MSK		0x3F
+#define DAYS_REG_MSK		0x3F
+#define MONTHS_REG_MSK		0x1F
+#define YEARS_REG_MSK		0xFF
+#define WEEKS_REG_MSK		0x7
+
+/* REG_SECONDS_REG through REG_YEARS_REG is how many registers? */
+
+#define NUM_TIME_REGS	(RK808_WEEKS_REG - RK808_SECONDS_REG + 1)
+#define NUM_ALARM_REGS	(RK808_ALARM_YEARS_REG - RK808_ALARM_SECONDS_REG + 1)
+
+struct rk808_rtc {
+	struct rk808 *rk808;
+	struct rtc_device *rtc;
+	int irq;
+};
+
+/* Read current time and date in RTC */
+static int rk808_rtc_readtime(struct device *dev, struct rtc_time *tm)
+{
+	struct rk808_rtc *rk808_rtc = dev_get_drvdata(dev);
+	struct rk808 *rk808 = rk808_rtc->rk808;
+	u8 rtc_data[NUM_TIME_REGS];
+	int ret;
+
+	/* Force an update of the shadowed registers right now */
+	ret = regmap_update_bits(rk808->regmap, RK808_RTC_CTRL_REG,
+				 BIT_RTC_CTRL_REG_RTC_GET_TIME,
+				 0);
+	if (ret) {
+		dev_err(dev, "Failed to update bits rtc_ctrl: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_update_bits(rk808->regmap, RK808_RTC_CTRL_REG,
+				 BIT_RTC_CTRL_REG_RTC_GET_TIME,
+				 BIT_RTC_CTRL_REG_RTC_GET_TIME);
+	if (ret) {
+		dev_err(dev, "Failed to update bits rtc_ctrl: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_bulk_read(rk808->regmap, RK808_SECONDS_REG,
+			       rtc_data, NUM_TIME_REGS);
+	if (ret) {
+		dev_err(dev, "Failed to bulk read rtc_data: %d\n", ret);
+		return ret;
+	}
+
+	tm->tm_sec = bcd2bin(rtc_data[0] & SECONDS_REG_MSK);
+	tm->tm_min = bcd2bin(rtc_data[1] & MINUTES_REG_MAK);
+	tm->tm_hour = bcd2bin(rtc_data[2] & HOURS_REG_MSK);
+	tm->tm_mday = bcd2bin(rtc_data[3] & DAYS_REG_MSK);
+	tm->tm_mon = (bcd2bin(rtc_data[4] & MONTHS_REG_MSK)) - 1;
+	tm->tm_year = (bcd2bin(rtc_data[5] & YEARS_REG_MSK)) + 100;
+	tm->tm_wday = bcd2bin(rtc_data[6] & WEEKS_REG_MSK);
+	dev_dbg(dev, "RTC date/time %4d-%02d-%02d(%d) %02d:%02d:%02d\n",
+		1900 + tm->tm_year, tm->tm_mon + 1, tm->tm_mday,
+		tm->tm_wday, tm->tm_hour , tm->tm_min, tm->tm_sec);
+
+	return ret;
+}
+
+/* Set current time and date in RTC */
+static int rk808_rtc_set_time(struct device *dev, struct rtc_time *tm)
+{
+	struct rk808_rtc *rk808_rtc = dev_get_drvdata(dev);
+	struct rk808 *rk808 = rk808_rtc->rk808;
+	u8 rtc_data[NUM_TIME_REGS];
+	int ret;
+
+	rtc_data[0] = bin2bcd(tm->tm_sec);
+	rtc_data[1] = bin2bcd(tm->tm_min);
+	rtc_data[2] = bin2bcd(tm->tm_hour);
+	rtc_data[3] = bin2bcd(tm->tm_mday);
+	rtc_data[4] = bin2bcd(tm->tm_mon + 1);
+	rtc_data[5] = bin2bcd(tm->tm_year - 100);
+	rtc_data[6] = bin2bcd(tm->tm_wday);
+	dev_dbg(dev, "set RTC date/time %4d-%02d-%02d(%d) %02d:%02d:%02d\n",
+		1900 + tm->tm_year, tm->tm_mon + 1, tm->tm_mday,
+		tm->tm_wday, tm->tm_hour , tm->tm_min, tm->tm_sec);
+
+	/* Stop RTC while updating the RTC registers */
+	ret = regmap_update_bits(rk808->regmap, RK808_RTC_CTRL_REG,
+				 BIT_RTC_CTRL_REG_STOP_RTC_M,
+				 BIT_RTC_CTRL_REG_STOP_RTC_M);
+	if (ret) {
+		dev_err(dev, "Failed to update RTC control: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_bulk_write(rk808->regmap, RK808_SECONDS_REG,
+				rtc_data, NUM_TIME_REGS);
+	if (ret) {
+		dev_err(dev, "Failed to bull write rtc_data: %d\n", ret);
+		return ret;
+	}
+	/* Start RTC again */
+	ret = regmap_update_bits(rk808->regmap, RK808_RTC_CTRL_REG,
+				 BIT_RTC_CTRL_REG_STOP_RTC_M, 0);
+	if (ret) {
+		dev_err(dev, "Failed to update RTC control: %d\n", ret);
+		return ret;
+	}
+	return 0;
+}
+
+/* Read alarm time and date in RTC */
+static int rk808_rtc_readalarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct rk808_rtc *rk808_rtc = dev_get_drvdata(dev);
+	struct rk808 *rk808 = rk808_rtc->rk808;
+	u8 alrm_data[NUM_ALARM_REGS];
+	uint32_t int_reg;
+	int ret;
+
+	ret = regmap_bulk_read(rk808->regmap, RK808_ALARM_SECONDS_REG,
+			       alrm_data, NUM_ALARM_REGS);
+
+	alrm->time.tm_sec = bcd2bin(alrm_data[0] & SECONDS_REG_MSK);
+	alrm->time.tm_min = bcd2bin(alrm_data[1] & MINUTES_REG_MAK);
+	alrm->time.tm_hour = bcd2bin(alrm_data[2] & HOURS_REG_MSK);
+	alrm->time.tm_mday = bcd2bin(alrm_data[3] & DAYS_REG_MSK);
+	alrm->time.tm_mon = (bcd2bin(alrm_data[4] & MONTHS_REG_MSK)) - 1;
+	alrm->time.tm_year = (bcd2bin(alrm_data[5] & YEARS_REG_MSK)) + 100;
+
+	ret = regmap_read(rk808->regmap, RK808_RTC_INT_REG, &int_reg);
+	if (ret) {
+		dev_err(dev, "Failed to read RTC INT REG: %d\n", ret);
+		return ret;
+	}
+
+	dev_dbg(dev, "alrm read RTC date/time %4d-%02d-%02d(%d) %02d:%02d:%02d\n",
+		1900 + alrm->time.tm_year, alrm->time.tm_mon + 1,
+		alrm->time.tm_mday, alrm->time.tm_wday, alrm->time.tm_hour,
+		alrm->time.tm_min, alrm->time.tm_sec);
+
+	alrm->enabled = (int_reg & BIT_RTC_INTERRUPTS_REG_IT_ALARM_M) ? 1 : 0;
+
+	return 0;
+}
+
+static int rk808_rtc_stop_alarm(struct rk808_rtc *rk808_rtc)
+{
+	struct rk808 *rk808 = rk808_rtc->rk808;
+	int ret;
+
+	ret = regmap_update_bits(rk808->regmap, RK808_RTC_INT_REG,
+				 BIT_RTC_INTERRUPTS_REG_IT_ALARM_M, 0);
+
+	return ret;
+}
+
+static int rk808_rtc_start_alarm(struct rk808_rtc *rk808_rtc)
+{
+	struct rk808 *rk808 = rk808_rtc->rk808;
+	int ret;
+
+	ret = regmap_update_bits(rk808->regmap, RK808_RTC_INT_REG,
+				 BIT_RTC_INTERRUPTS_REG_IT_ALARM_M,
+				 BIT_RTC_INTERRUPTS_REG_IT_ALARM_M);
+
+	return ret;
+}
+
+static int rk808_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
+{
+	struct rk808_rtc *rk808_rtc = dev_get_drvdata(dev);
+	struct rk808 *rk808 = rk808_rtc->rk808;
+	u8 alrm_data[NUM_ALARM_REGS];
+	int ret;
+
+	ret = rk808_rtc_stop_alarm(rk808_rtc);
+	if (ret) {
+		dev_err(dev, "Failed to stop alarm: %d\n", ret);
+		return ret;
+	}
+	dev_dbg(dev, "alrm set RTC date/time %4d-%02d-%02d(%d) %02d:%02d:%02d\n",
+		1900 + alrm->time.tm_year, alrm->time.tm_mon + 1,
+		alrm->time.tm_mday, alrm->time.tm_wday, alrm->time.tm_hour,
+		alrm->time.tm_min, alrm->time.tm_sec);
+
+	alrm_data[0] = bin2bcd(alrm->time.tm_sec);
+	alrm_data[1] = bin2bcd(alrm->time.tm_min);
+	alrm_data[2] = bin2bcd(alrm->time.tm_hour);
+	alrm_data[3] = bin2bcd(alrm->time.tm_mday);
+	alrm_data[4] = bin2bcd(alrm->time.tm_mon + 1);
+	alrm_data[5] = bin2bcd(alrm->time.tm_year - 100);
+
+	ret = regmap_bulk_write(rk808->regmap, RK808_ALARM_SECONDS_REG,
+				alrm_data, NUM_ALARM_REGS);
+	if (ret) {
+		dev_err(dev, "Failed to bulk write: %d\n", ret);
+		return ret;
+	}
+	if (alrm->enabled) {
+		ret = rk808_rtc_start_alarm(rk808_rtc);
+		if (ret) {
+			dev_err(dev, "Failed to start alarm: %d\n", ret);
+			return ret;
+		}
+	}
+	return 0;
+}
+
+static int rk808_rtc_alarm_irq_enable(struct device *dev,
+				      unsigned int enabled)
+{
+	struct rk808_rtc *rk808_rtc = dev_get_drvdata(dev);
+
+	if (enabled)
+		return rk808_rtc_start_alarm(rk808_rtc);
+
+	return rk808_rtc_stop_alarm(rk808_rtc);
+}
+
+/*
+ * We will just handle setting the frequency and make use the framework for
+ * reading the periodic interupts.
+ *
+ * @freq: Current periodic IRQ freq:
+ * bit 0: every second
+ * bit 1: every minute
+ * bit 2: every hour
+ * bit 3: every day
+ */
+static irqreturn_t rk808_alarm_irq(int irq, void *data)
+{
+	struct rk808_rtc *rk808_rtc = data;
+	struct rk808 *rk808 = rk808_rtc->rk808;
+	struct i2c_client *client = rk808->i2c;
+	int ret;
+
+	ret = regmap_write(rk808->regmap, RK808_RTC_STATUS_REG,
+			   RTC_STATUS_MASK);
+	if (ret) {
+		dev_err(&client->dev,
+			"%s:Failed to update RTC status: %d\n", __func__, ret);
+		return ret;
+	}
+
+	rtc_update_irq(rk808_rtc->rtc, 1, RTC_IRQF | RTC_AF);
+	dev_dbg(&client->dev,
+		 "%s:irq=%d\n", __func__, irq);
+	return IRQ_HANDLED;
+}
+
+static const struct rtc_class_ops rk808_rtc_ops = {
+	.read_time = rk808_rtc_readtime,
+	.set_time = rk808_rtc_set_time,
+	.read_alarm = rk808_rtc_readalarm,
+	.set_alarm = rk808_rtc_setalarm,
+	.alarm_irq_enable = rk808_rtc_alarm_irq_enable,
+};
+
+#ifdef CONFIG_PM_SLEEP
+/* Turn off the alarm if it should not be a wake source. */
+static int rk808_rtc_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rk808_rtc *rk808_rtc = dev_get_drvdata(&pdev->dev);
+
+	if (device_may_wakeup(dev))
+		enable_irq_wake(rk808_rtc->irq);
+
+	return 0;
+}
+
+/* Enable the alarm if it should be enabled (in case it was disabled to
+ * prevent use as a wake source).
+ */
+static int rk808_rtc_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct rk808_rtc *rk808_rtc = dev_get_drvdata(&pdev->dev);
+
+	if (device_may_wakeup(dev))
+		disable_irq_wake(rk808_rtc->irq);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(rk808_rtc_pm_ops,
+	rk808_rtc_suspend, rk808_rtc_resume);
+
+static int rk808_rtc_probe(struct platform_device *pdev)
+{
+	struct rk808 *rk808 = dev_get_drvdata(pdev->dev.parent);
+	struct rk808_rtc *rk808_rtc;
+	struct rtc_time tm;
+	int ret;
+
+	rk808_rtc = devm_kzalloc(&pdev->dev, sizeof(*rk808_rtc), GFP_KERNEL);
+	if (rk808_rtc == NULL)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, rk808_rtc);
+	rk808_rtc->rk808 = rk808;
+
+	/* start rtc running by default, and use shadowed timer. */
+	ret = regmap_update_bits(rk808->regmap, RK808_RTC_CTRL_REG,
+				 BIT_RTC_CTRL_REG_STOP_RTC_M |
+				 BIT_RTC_CTRL_REG_RTC_READSEL_M,
+				 BIT_RTC_CTRL_REG_RTC_READSEL_M);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Failed to update RTC control: %d\n", ret);
+		return ret;
+	}
+
+	ret = regmap_write(rk808->regmap, RK808_RTC_STATUS_REG,
+			   RTC_STATUS_MASK);
+	if (ret) {
+		dev_err(&pdev->dev,
+			"Failed to write RTC status: %d\n", ret);
+			return ret;
+	}
+
+	/* set init time */
+	ret = rk808_rtc_readtime(&pdev->dev, &tm);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to read RTC time\n");
+		return ret;
+	}
+	ret = rtc_valid_tm(&tm);
+	if (ret)
+		dev_warn(&pdev->dev, "invalid date/time\n");
+
+	device_init_wakeup(&pdev->dev, 1);
+
+	rk808_rtc->rtc = devm_rtc_device_register(&pdev->dev, "rk808-rtc",
+						  &rk808_rtc_ops, THIS_MODULE);
+	if (IS_ERR(rk808_rtc->rtc)) {
+		ret = PTR_ERR(rk808_rtc->rtc);
+		return ret;
+	}
+
+	rk808_rtc->irq = platform_get_irq(pdev, 0);
+	if (rk808_rtc->irq < 0) {
+		if (rk808_rtc->irq != -EPROBE_DEFER)
+			dev_err(&pdev->dev, "Wake up is not possible as irq = %d\n",
+				rk808_rtc->irq);
+		return rk808_rtc->irq;
+	}
+
+	/* request alarm irq of rk808 */
+	ret = devm_request_threaded_irq(&pdev->dev, rk808_rtc->irq, NULL,
+					rk808_alarm_irq, 0,
+					"RTC alarm", rk808_rtc);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to request alarm IRQ %d: %d\n",
+			rk808_rtc->irq, ret);
+	}
+
+	return ret;
+}
+
+static struct platform_driver rk808_rtc_driver = {
+	.probe = rk808_rtc_probe,
+	.driver = {
+		.name = "rk808-rtc",
+		.pm = &rk808_rtc_pm_ops,
+	},
+};
+
+module_platform_driver(rk808_rtc_driver);
+
+MODULE_DESCRIPTION("RTC driver for the rk808 series PMICs");
+MODULE_AUTHOR("Chris Zhong <zyw@rock-chips.com>");
+MODULE_AUTHOR("Zhang Qing <zhangqing@rock-chips.com>");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:rk808-rtc");
diff --git a/drivers/rtc/rtc-rs5c372.c b/drivers/rtc/rtc-rs5c372.c
index ccf54f06396b..28871cd7e3b5 100644
--- a/drivers/rtc/rtc-rs5c372.c
+++ b/drivers/rtc/rtc-rs5c372.c
@@ -142,12 +142,11 @@ static int rs5c_get_regs(struct rs5c372 *rs5c)
 	}
 
 	dev_dbg(&client->dev,
-		"%02x %02x %02x (%02x) %02x %02x %02x (%02x), "
-		"%02x %02x %02x, %02x %02x %02x; %02x %02x\n",
-		rs5c->regs[0],  rs5c->regs[1],  rs5c->regs[2],  rs5c->regs[3],
-		rs5c->regs[4],  rs5c->regs[5],  rs5c->regs[6],  rs5c->regs[7],
-		rs5c->regs[8],  rs5c->regs[9],  rs5c->regs[10], rs5c->regs[11],
-		rs5c->regs[12], rs5c->regs[13], rs5c->regs[14], rs5c->regs[15]);
+		"%3ph (%02x) %3ph (%02x), %3ph, %3ph; %02x %02x\n",
+		rs5c->regs + 0, rs5c->regs[3],
+		rs5c->regs + 4, rs5c->regs[7],
+		rs5c->regs + 8, rs5c->regs + 11,
+		rs5c->regs[14], rs5c->regs[15]);
 
 	return 0;
 }
diff --git a/drivers/rtc/rtc-s3c.c b/drivers/rtc/rtc-s3c.c
index 4958a363b2c7..a6b1252c9941 100644
--- a/drivers/rtc/rtc-s3c.c
+++ b/drivers/rtc/rtc-s3c.c
@@ -32,155 +32,150 @@
 #include <asm/irq.h>
 #include "rtc-s3c.h"
 
-enum s3c_cpu_type {
-	TYPE_S3C2410,
-	TYPE_S3C2416,
-	TYPE_S3C2443,
-	TYPE_S3C64XX,
-};
+struct s3c_rtc {
+	struct device *dev;
+	struct rtc_device *rtc;
 
-struct s3c_rtc_drv_data {
-	int cpu_type;
-};
+	void __iomem *base;
+	struct clk *rtc_clk;
+	struct clk *rtc_src_clk;
+	bool enabled;
+
+	struct s3c_rtc_data *data;
 
-/* I have yet to find an S3C implementation with more than one
- * of these rtc blocks in */
+	int irq_alarm;
+	int irq_tick;
 
-static struct clk *rtc_clk;
-static void __iomem *s3c_rtc_base;
-static int s3c_rtc_alarmno;
-static int s3c_rtc_tickno;
-static enum s3c_cpu_type s3c_rtc_cpu_type;
+	spinlock_t pie_lock;
+	spinlock_t alarm_clk_lock;
 
-static DEFINE_SPINLOCK(s3c_rtc_pie_lock);
+	int ticnt_save, ticnt_en_save;
+	bool wake_en;
+};
+
+struct s3c_rtc_data {
+	int max_user_freq;
+	bool needs_src_clk;
+
+	void (*irq_handler) (struct s3c_rtc *info, int mask);
+	void (*set_freq) (struct s3c_rtc *info, int freq);
+	void (*enable_tick) (struct s3c_rtc *info, struct seq_file *seq);
+	void (*select_tick_clk) (struct s3c_rtc *info);
+	void (*save_tick_cnt) (struct s3c_rtc *info);
+	void (*restore_tick_cnt) (struct s3c_rtc *info);
+	void (*enable) (struct s3c_rtc *info);
+	void (*disable) (struct s3c_rtc *info);
+};
 
-static void s3c_rtc_alarm_clk_enable(bool enable)
+static void s3c_rtc_alarm_clk_enable(struct s3c_rtc *info, bool enable)
 {
-	static DEFINE_SPINLOCK(s3c_rtc_alarm_clk_lock);
-	static bool alarm_clk_enabled;
 	unsigned long irq_flags;
 
-	spin_lock_irqsave(&s3c_rtc_alarm_clk_lock, irq_flags);
+	spin_lock_irqsave(&info->alarm_clk_lock, irq_flags);
 	if (enable) {
-		if (!alarm_clk_enabled) {
-			clk_enable(rtc_clk);
-			alarm_clk_enabled = true;
+		if (!info->enabled) {
+			clk_enable(info->rtc_clk);
+			if (info->data->needs_src_clk)
+				clk_enable(info->rtc_src_clk);
+			info->enabled = true;
 		}
 	} else {
-		if (alarm_clk_enabled) {
-			clk_disable(rtc_clk);
-			alarm_clk_enabled = false;
+		if (info->enabled) {
+			if (info->data->needs_src_clk)
+				clk_disable(info->rtc_src_clk);
+			clk_disable(info->rtc_clk);
+			info->enabled = false;
 		}
 	}
-	spin_unlock_irqrestore(&s3c_rtc_alarm_clk_lock, irq_flags);
+	spin_unlock_irqrestore(&info->alarm_clk_lock, irq_flags);
 }
 
 /* IRQ Handlers */
-
-static irqreturn_t s3c_rtc_alarmirq(int irq, void *id)
+static irqreturn_t s3c_rtc_tickirq(int irq, void *id)
 {
-	struct rtc_device *rdev = id;
-
-	clk_enable(rtc_clk);
-	rtc_update_irq(rdev, 1, RTC_AF | RTC_IRQF);
-
-	if (s3c_rtc_cpu_type == TYPE_S3C64XX)
-		writeb(S3C2410_INTP_ALM, s3c_rtc_base + S3C2410_INTP);
-
-	clk_disable(rtc_clk);
+	struct s3c_rtc *info = (struct s3c_rtc *)id;
 
-	s3c_rtc_alarm_clk_enable(false);
+	if (info->data->irq_handler)
+		info->data->irq_handler(info, S3C2410_INTP_TIC);
 
 	return IRQ_HANDLED;
 }
 
-static irqreturn_t s3c_rtc_tickirq(int irq, void *id)
+static irqreturn_t s3c_rtc_alarmirq(int irq, void *id)
 {
-	struct rtc_device *rdev = id;
+	struct s3c_rtc *info = (struct s3c_rtc *)id;
 
-	clk_enable(rtc_clk);
-	rtc_update_irq(rdev, 1, RTC_PF | RTC_IRQF);
+	if (info->data->irq_handler)
+		info->data->irq_handler(info, S3C2410_INTP_ALM);
 
-	if (s3c_rtc_cpu_type == TYPE_S3C64XX)
-		writeb(S3C2410_INTP_TIC, s3c_rtc_base + S3C2410_INTP);
-
-	clk_disable(rtc_clk);
 	return IRQ_HANDLED;
 }
 
 /* Update control registers */
 static int s3c_rtc_setaie(struct device *dev, unsigned int enabled)
 {
+	struct s3c_rtc *info = dev_get_drvdata(dev);
 	unsigned int tmp;
 
-	dev_dbg(dev, "%s: aie=%d\n", __func__, enabled);
+	dev_dbg(info->dev, "%s: aie=%d\n", __func__, enabled);
 
-	clk_enable(rtc_clk);
-	tmp = readb(s3c_rtc_base + S3C2410_RTCALM) & ~S3C2410_RTCALM_ALMEN;
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+	tmp = readb(info->base + S3C2410_RTCALM) & ~S3C2410_RTCALM_ALMEN;
 
 	if (enabled)
 		tmp |= S3C2410_RTCALM_ALMEN;
 
-	writeb(tmp, s3c_rtc_base + S3C2410_RTCALM);
-	clk_disable(rtc_clk);
+	writeb(tmp, info->base + S3C2410_RTCALM);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 
-	s3c_rtc_alarm_clk_enable(enabled);
+	s3c_rtc_alarm_clk_enable(info, enabled);
 
 	return 0;
 }
 
-static int s3c_rtc_setfreq(struct device *dev, int freq)
+/* Set RTC frequency */
+static int s3c_rtc_setfreq(struct s3c_rtc *info, int freq)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct rtc_device *rtc_dev = platform_get_drvdata(pdev);
-	unsigned int tmp = 0;
-	int val;
-
 	if (!is_power_of_2(freq))
 		return -EINVAL;
 
-	clk_enable(rtc_clk);
-	spin_lock_irq(&s3c_rtc_pie_lock);
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+	spin_lock_irq(&info->pie_lock);
 
-	if (s3c_rtc_cpu_type != TYPE_S3C64XX) {
-		tmp = readb(s3c_rtc_base + S3C2410_TICNT);
-		tmp &= S3C2410_TICNT_ENABLE;
-	}
+	if (info->data->set_freq)
+		info->data->set_freq(info, freq);
 
-	val = (rtc_dev->max_user_freq / freq) - 1;
-
-	if (s3c_rtc_cpu_type == TYPE_S3C2416 || s3c_rtc_cpu_type == TYPE_S3C2443) {
-		tmp |= S3C2443_TICNT_PART(val);
-		writel(S3C2443_TICNT1_PART(val), s3c_rtc_base + S3C2443_TICNT1);
-
-		if (s3c_rtc_cpu_type == TYPE_S3C2416)
-			writel(S3C2416_TICNT2_PART(val), s3c_rtc_base + S3C2416_TICNT2);
-	} else {
-		tmp |= val;
-	}
-
-	writel(tmp, s3c_rtc_base + S3C2410_TICNT);
-	spin_unlock_irq(&s3c_rtc_pie_lock);
-	clk_disable(rtc_clk);
+	spin_unlock_irq(&info->pie_lock);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 
 	return 0;
 }
 
 /* Time read/write */
-
 static int s3c_rtc_gettime(struct device *dev, struct rtc_time *rtc_tm)
 {
+	struct s3c_rtc *info = dev_get_drvdata(dev);
 	unsigned int have_retried = 0;
-	void __iomem *base = s3c_rtc_base;
 
-	clk_enable(rtc_clk);
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+
  retry_get_time:
-	rtc_tm->tm_min  = readb(base + S3C2410_RTCMIN);
-	rtc_tm->tm_hour = readb(base + S3C2410_RTCHOUR);
-	rtc_tm->tm_mday = readb(base + S3C2410_RTCDATE);
-	rtc_tm->tm_mon  = readb(base + S3C2410_RTCMON);
-	rtc_tm->tm_year = readb(base + S3C2410_RTCYEAR);
-	rtc_tm->tm_sec  = readb(base + S3C2410_RTCSEC);
+	rtc_tm->tm_min  = readb(info->base + S3C2410_RTCMIN);
+	rtc_tm->tm_hour = readb(info->base + S3C2410_RTCHOUR);
+	rtc_tm->tm_mday = readb(info->base + S3C2410_RTCDATE);
+	rtc_tm->tm_mon  = readb(info->base + S3C2410_RTCMON);
+	rtc_tm->tm_year = readb(info->base + S3C2410_RTCYEAR);
+	rtc_tm->tm_sec  = readb(info->base + S3C2410_RTCSEC);
 
 	/* the only way to work out whether the system was mid-update
 	 * when we read it is to check the second counter, and if it
@@ -207,13 +202,16 @@ static int s3c_rtc_gettime(struct device *dev, struct rtc_time *rtc_tm)
 
 	rtc_tm->tm_mon -= 1;
 
-	clk_disable(rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
+
 	return rtc_valid_tm(rtc_tm);
 }
 
 static int s3c_rtc_settime(struct device *dev, struct rtc_time *tm)
 {
-	void __iomem *base = s3c_rtc_base;
+	struct s3c_rtc *info = dev_get_drvdata(dev);
 	int year = tm->tm_year - 100;
 
 	dev_dbg(dev, "set time %04d.%02d.%02d %02d:%02d:%02d\n",
@@ -227,33 +225,42 @@ static int s3c_rtc_settime(struct device *dev, struct rtc_time *tm)
 		return -EINVAL;
 	}
 
-	clk_enable(rtc_clk);
-	writeb(bin2bcd(tm->tm_sec),  base + S3C2410_RTCSEC);
-	writeb(bin2bcd(tm->tm_min),  base + S3C2410_RTCMIN);
-	writeb(bin2bcd(tm->tm_hour), base + S3C2410_RTCHOUR);
-	writeb(bin2bcd(tm->tm_mday), base + S3C2410_RTCDATE);
-	writeb(bin2bcd(tm->tm_mon + 1), base + S3C2410_RTCMON);
-	writeb(bin2bcd(year), base + S3C2410_RTCYEAR);
-	clk_disable(rtc_clk);
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+
+	writeb(bin2bcd(tm->tm_sec),  info->base + S3C2410_RTCSEC);
+	writeb(bin2bcd(tm->tm_min),  info->base + S3C2410_RTCMIN);
+	writeb(bin2bcd(tm->tm_hour), info->base + S3C2410_RTCHOUR);
+	writeb(bin2bcd(tm->tm_mday), info->base + S3C2410_RTCDATE);
+	writeb(bin2bcd(tm->tm_mon + 1), info->base + S3C2410_RTCMON);
+	writeb(bin2bcd(year), info->base + S3C2410_RTCYEAR);
+
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 
 	return 0;
 }
 
 static int s3c_rtc_getalarm(struct device *dev, struct rtc_wkalrm *alrm)
 {
+	struct s3c_rtc *info = dev_get_drvdata(dev);
 	struct rtc_time *alm_tm = &alrm->time;
-	void __iomem *base = s3c_rtc_base;
 	unsigned int alm_en;
 
-	clk_enable(rtc_clk);
-	alm_tm->tm_sec  = readb(base + S3C2410_ALMSEC);
-	alm_tm->tm_min  = readb(base + S3C2410_ALMMIN);
-	alm_tm->tm_hour = readb(base + S3C2410_ALMHOUR);
-	alm_tm->tm_mon  = readb(base + S3C2410_ALMMON);
-	alm_tm->tm_mday = readb(base + S3C2410_ALMDATE);
-	alm_tm->tm_year = readb(base + S3C2410_ALMYEAR);
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
 
-	alm_en = readb(base + S3C2410_RTCALM);
+	alm_tm->tm_sec  = readb(info->base + S3C2410_ALMSEC);
+	alm_tm->tm_min  = readb(info->base + S3C2410_ALMMIN);
+	alm_tm->tm_hour = readb(info->base + S3C2410_ALMHOUR);
+	alm_tm->tm_mon  = readb(info->base + S3C2410_ALMMON);
+	alm_tm->tm_mday = readb(info->base + S3C2410_ALMDATE);
+	alm_tm->tm_year = readb(info->base + S3C2410_ALMYEAR);
+
+	alm_en = readb(info->base + S3C2410_RTCALM);
 
 	alrm->enabled = (alm_en & S3C2410_RTCALM_ALMEN) ? 1 : 0;
 
@@ -297,65 +304,74 @@ static int s3c_rtc_getalarm(struct device *dev, struct rtc_wkalrm *alrm)
 	else
 		alm_tm->tm_year = -1;
 
-	clk_disable(rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
+
 	return 0;
 }
 
 static int s3c_rtc_setalarm(struct device *dev, struct rtc_wkalrm *alrm)
 {
+	struct s3c_rtc *info = dev_get_drvdata(dev);
 	struct rtc_time *tm = &alrm->time;
-	void __iomem *base = s3c_rtc_base;
 	unsigned int alrm_en;
 
-	clk_enable(rtc_clk);
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+
 	dev_dbg(dev, "s3c_rtc_setalarm: %d, %04d.%02d.%02d %02d:%02d:%02d\n",
 		 alrm->enabled,
 		 1900 + tm->tm_year, tm->tm_mon + 1, tm->tm_mday,
 		 tm->tm_hour, tm->tm_min, tm->tm_sec);
 
-	alrm_en = readb(base + S3C2410_RTCALM) & S3C2410_RTCALM_ALMEN;
-	writeb(0x00, base + S3C2410_RTCALM);
+	alrm_en = readb(info->base + S3C2410_RTCALM) & S3C2410_RTCALM_ALMEN;
+	writeb(0x00, info->base + S3C2410_RTCALM);
 
 	if (tm->tm_sec < 60 && tm->tm_sec >= 0) {
 		alrm_en |= S3C2410_RTCALM_SECEN;
-		writeb(bin2bcd(tm->tm_sec), base + S3C2410_ALMSEC);
+		writeb(bin2bcd(tm->tm_sec), info->base + S3C2410_ALMSEC);
 	}
 
 	if (tm->tm_min < 60 && tm->tm_min >= 0) {
 		alrm_en |= S3C2410_RTCALM_MINEN;
-		writeb(bin2bcd(tm->tm_min), base + S3C2410_ALMMIN);
+		writeb(bin2bcd(tm->tm_min), info->base + S3C2410_ALMMIN);
 	}
 
 	if (tm->tm_hour < 24 && tm->tm_hour >= 0) {
 		alrm_en |= S3C2410_RTCALM_HOUREN;
-		writeb(bin2bcd(tm->tm_hour), base + S3C2410_ALMHOUR);
+		writeb(bin2bcd(tm->tm_hour), info->base + S3C2410_ALMHOUR);
 	}
 
 	dev_dbg(dev, "setting S3C2410_RTCALM to %08x\n", alrm_en);
 
-	writeb(alrm_en, base + S3C2410_RTCALM);
+	writeb(alrm_en, info->base + S3C2410_RTCALM);
 
 	s3c_rtc_setaie(dev, alrm->enabled);
 
-	clk_disable(rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
+
 	return 0;
 }
 
 static int s3c_rtc_proc(struct device *dev, struct seq_file *seq)
 {
-	unsigned int ticnt;
+	struct s3c_rtc *info = dev_get_drvdata(dev);
 
-	clk_enable(rtc_clk);
-	if (s3c_rtc_cpu_type == TYPE_S3C64XX) {
-		ticnt = readw(s3c_rtc_base + S3C2410_RTCCON);
-		ticnt &= S3C64XX_RTCCON_TICEN;
-	} else {
-		ticnt = readb(s3c_rtc_base + S3C2410_TICNT);
-		ticnt &= S3C2410_TICNT_ENABLE;
-	}
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+
+	if (info->data->enable_tick)
+		info->data->enable_tick(info, seq);
+
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 
-	seq_printf(seq, "periodic_IRQ\t: %s\n", ticnt  ? "yes" : "no");
-	clk_disable(rtc_clk);
 	return 0;
 }
 
@@ -368,152 +384,199 @@ static const struct rtc_class_ops s3c_rtcops = {
 	.alarm_irq_enable = s3c_rtc_setaie,
 };
 
-static void s3c_rtc_enable(struct platform_device *pdev, int en)
+static void s3c24xx_rtc_enable(struct s3c_rtc *info)
 {
-	void __iomem *base = s3c_rtc_base;
-	unsigned int tmp;
+	unsigned int con, tmp;
 
-	if (s3c_rtc_base == NULL)
-		return;
-
-	clk_enable(rtc_clk);
-	if (!en) {
-		tmp = readw(base + S3C2410_RTCCON);
-		if (s3c_rtc_cpu_type == TYPE_S3C64XX)
-			tmp &= ~S3C64XX_RTCCON_TICEN;
-		tmp &= ~S3C2410_RTCCON_RTCEN;
-		writew(tmp, base + S3C2410_RTCCON);
-
-		if (s3c_rtc_cpu_type != TYPE_S3C64XX) {
-			tmp = readb(base + S3C2410_TICNT);
-			tmp &= ~S3C2410_TICNT_ENABLE;
-			writeb(tmp, base + S3C2410_TICNT);
-		}
-	} else {
-		/* re-enable the device, and check it is ok */
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
 
-		if ((readw(base+S3C2410_RTCCON) & S3C2410_RTCCON_RTCEN) == 0) {
-			dev_info(&pdev->dev, "rtc disabled, re-enabling\n");
+	con = readw(info->base + S3C2410_RTCCON);
+	/* re-enable the device, and check it is ok */
+	if ((con & S3C2410_RTCCON_RTCEN) == 0) {
+		dev_info(info->dev, "rtc disabled, re-enabling\n");
 
-			tmp = readw(base + S3C2410_RTCCON);
-			writew(tmp | S3C2410_RTCCON_RTCEN,
-				base + S3C2410_RTCCON);
-		}
+		tmp = readw(info->base + S3C2410_RTCCON);
+		writew(tmp | S3C2410_RTCCON_RTCEN,
+			info->base + S3C2410_RTCCON);
+	}
 
-		if ((readw(base + S3C2410_RTCCON) & S3C2410_RTCCON_CNTSEL)) {
-			dev_info(&pdev->dev, "removing RTCCON_CNTSEL\n");
+	if (con & S3C2410_RTCCON_CNTSEL) {
+		dev_info(info->dev, "removing RTCCON_CNTSEL\n");
 
-			tmp = readw(base + S3C2410_RTCCON);
-			writew(tmp & ~S3C2410_RTCCON_CNTSEL,
-				base + S3C2410_RTCCON);
-		}
+		tmp = readw(info->base + S3C2410_RTCCON);
+		writew(tmp & ~S3C2410_RTCCON_CNTSEL,
+			info->base + S3C2410_RTCCON);
+	}
 
-		if ((readw(base + S3C2410_RTCCON) & S3C2410_RTCCON_CLKRST)) {
-			dev_info(&pdev->dev, "removing RTCCON_CLKRST\n");
+	if (con & S3C2410_RTCCON_CLKRST) {
+		dev_info(info->dev, "removing RTCCON_CLKRST\n");
 
-			tmp = readw(base + S3C2410_RTCCON);
-			writew(tmp & ~S3C2410_RTCCON_CLKRST,
-				base + S3C2410_RTCCON);
-		}
+		tmp = readw(info->base + S3C2410_RTCCON);
+		writew(tmp & ~S3C2410_RTCCON_CLKRST,
+			info->base + S3C2410_RTCCON);
 	}
-	clk_disable(rtc_clk);
+
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 }
 
-static int s3c_rtc_remove(struct platform_device *dev)
+static void s3c24xx_rtc_disable(struct s3c_rtc *info)
 {
-	s3c_rtc_setaie(&dev->dev, 0);
+	unsigned int con;
+
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+
+	con = readw(info->base + S3C2410_RTCCON);
+	con &= ~S3C2410_RTCCON_RTCEN;
+	writew(con, info->base + S3C2410_RTCCON);
 
-	clk_unprepare(rtc_clk);
-	rtc_clk = NULL;
+	con = readb(info->base + S3C2410_TICNT);
+	con &= ~S3C2410_TICNT_ENABLE;
+	writeb(con, info->base + S3C2410_TICNT);
+
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
+}
+
+static void s3c6410_rtc_disable(struct s3c_rtc *info)
+{
+	unsigned int con;
+
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+
+	con = readw(info->base + S3C2410_RTCCON);
+	con &= ~S3C64XX_RTCCON_TICEN;
+	con &= ~S3C2410_RTCCON_RTCEN;
+	writew(con, info->base + S3C2410_RTCCON);
+
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
+}
+
+static int s3c_rtc_remove(struct platform_device *pdev)
+{
+	struct s3c_rtc *info = platform_get_drvdata(pdev);
+
+	s3c_rtc_setaie(info->dev, 0);
+
+	clk_unprepare(info->rtc_clk);
+	info->rtc_clk = NULL;
 
 	return 0;
 }
 
 static const struct of_device_id s3c_rtc_dt_match[];
 
-static inline int s3c_rtc_get_driver_data(struct platform_device *pdev)
+static struct s3c_rtc_data *s3c_rtc_get_data(struct platform_device *pdev)
 {
-#ifdef CONFIG_OF
-	struct s3c_rtc_drv_data *data;
-	if (pdev->dev.of_node) {
-		const struct of_device_id *match;
-		match = of_match_node(s3c_rtc_dt_match, pdev->dev.of_node);
-		data = (struct s3c_rtc_drv_data *) match->data;
-		return data->cpu_type;
-	}
-#endif
-	return platform_get_device_id(pdev)->driver_data;
+	const struct of_device_id *match;
+
+	match = of_match_node(s3c_rtc_dt_match, pdev->dev.of_node);
+	return (struct s3c_rtc_data *)match->data;
 }
 
 static int s3c_rtc_probe(struct platform_device *pdev)
 {
-	struct rtc_device *rtc;
+	struct s3c_rtc *info = NULL;
 	struct rtc_time rtc_tm;
 	struct resource *res;
 	int ret;
-	int tmp;
 
-	dev_dbg(&pdev->dev, "%s: probe=%p\n", __func__, pdev);
+	info = devm_kzalloc(&pdev->dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return -ENOMEM;
 
 	/* find the IRQs */
-
-	s3c_rtc_tickno = platform_get_irq(pdev, 1);
-	if (s3c_rtc_tickno < 0) {
+	info->irq_tick = platform_get_irq(pdev, 1);
+	if (info->irq_tick < 0) {
 		dev_err(&pdev->dev, "no irq for rtc tick\n");
-		return s3c_rtc_tickno;
+		return info->irq_tick;
+	}
+
+	info->dev = &pdev->dev;
+	info->data = s3c_rtc_get_data(pdev);
+	if (!info->data) {
+		dev_err(&pdev->dev, "failed getting s3c_rtc_data\n");
+		return -EINVAL;
 	}
+	spin_lock_init(&info->pie_lock);
+	spin_lock_init(&info->alarm_clk_lock);
+
+	platform_set_drvdata(pdev, info);
 
-	s3c_rtc_alarmno = platform_get_irq(pdev, 0);
-	if (s3c_rtc_alarmno < 0) {
+	info->irq_alarm = platform_get_irq(pdev, 0);
+	if (info->irq_alarm < 0) {
 		dev_err(&pdev->dev, "no irq for alarm\n");
-		return s3c_rtc_alarmno;
+		return info->irq_alarm;
 	}
 
 	dev_dbg(&pdev->dev, "s3c2410_rtc: tick irq %d, alarm irq %d\n",
-		 s3c_rtc_tickno, s3c_rtc_alarmno);
+		 info->irq_tick, info->irq_alarm);
 
 	/* get the memory region */
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	s3c_rtc_base = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(s3c_rtc_base))
-		return PTR_ERR(s3c_rtc_base);
-
-	rtc_clk = devm_clk_get(&pdev->dev, "rtc");
-	if (IS_ERR(rtc_clk)) {
-		dev_err(&pdev->dev, "failed to find rtc clock source\n");
-		ret = PTR_ERR(rtc_clk);
-		rtc_clk = NULL;
-		return ret;
+	info->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(info->base))
+		return PTR_ERR(info->base);
+
+	info->rtc_clk = devm_clk_get(&pdev->dev, "rtc");
+	if (IS_ERR(info->rtc_clk)) {
+		dev_err(&pdev->dev, "failed to find rtc clock\n");
+		return PTR_ERR(info->rtc_clk);
 	}
+	clk_prepare_enable(info->rtc_clk);
 
-	clk_prepare_enable(rtc_clk);
+	info->rtc_src_clk = devm_clk_get(&pdev->dev, "rtc_src");
+	if (IS_ERR(info->rtc_src_clk)) {
+		dev_err(&pdev->dev, "failed to find rtc source clock\n");
+		return PTR_ERR(info->rtc_src_clk);
+	}
+	clk_prepare_enable(info->rtc_src_clk);
 
-	/* check to see if everything is setup correctly */
 
-	s3c_rtc_enable(pdev, 1);
+	/* check to see if everything is setup correctly */
+	if (info->data->enable)
+		info->data->enable(info);
 
 	dev_dbg(&pdev->dev, "s3c2410_rtc: RTCCON=%02x\n",
-		 readw(s3c_rtc_base + S3C2410_RTCCON));
+		 readw(info->base + S3C2410_RTCCON));
 
 	device_init_wakeup(&pdev->dev, 1);
 
 	/* register RTC and exit */
-
-	rtc = devm_rtc_device_register(&pdev->dev, "s3c", &s3c_rtcops,
+	info->rtc = devm_rtc_device_register(&pdev->dev, "s3c", &s3c_rtcops,
 				  THIS_MODULE);
-
-	if (IS_ERR(rtc)) {
+	if (IS_ERR(info->rtc)) {
 		dev_err(&pdev->dev, "cannot attach rtc\n");
-		ret = PTR_ERR(rtc);
+		ret = PTR_ERR(info->rtc);
 		goto err_nortc;
 	}
 
-	s3c_rtc_cpu_type = s3c_rtc_get_driver_data(pdev);
+	ret = devm_request_irq(&pdev->dev, info->irq_alarm, s3c_rtc_alarmirq,
+			  0,  "s3c2410-rtc alarm", info);
+	if (ret) {
+		dev_err(&pdev->dev, "IRQ%d error %d\n", info->irq_alarm, ret);
+		goto err_nortc;
+	}
 
-	/* Check RTC Time */
+	ret = devm_request_irq(&pdev->dev, info->irq_tick, s3c_rtc_tickirq,
+			  0,  "s3c2410-rtc tick", info);
+	if (ret) {
+		dev_err(&pdev->dev, "IRQ%d error %d\n", info->irq_tick, ret);
+		goto err_nortc;
+	}
 
-	s3c_rtc_gettime(NULL, &rtc_tm);
+	/* Check RTC Time */
+	s3c_rtc_gettime(&pdev->dev, &rtc_tm);
 
 	if (rtc_valid_tm(&rtc_tm)) {
 		rtc_tm.tm_year	= 100;
@@ -523,163 +586,312 @@ static int s3c_rtc_probe(struct platform_device *pdev)
 		rtc_tm.tm_min	= 0;
 		rtc_tm.tm_sec	= 0;
 
-		s3c_rtc_settime(NULL, &rtc_tm);
+		s3c_rtc_settime(&pdev->dev, &rtc_tm);
 
 		dev_warn(&pdev->dev, "warning: invalid RTC value so initializing it\n");
 	}
 
-	if (s3c_rtc_cpu_type != TYPE_S3C2410)
-		rtc->max_user_freq = 32768;
-	else
-		rtc->max_user_freq = 128;
-
-	if (s3c_rtc_cpu_type == TYPE_S3C2416 || s3c_rtc_cpu_type == TYPE_S3C2443) {
-		tmp = readw(s3c_rtc_base + S3C2410_RTCCON);
-		tmp |= S3C2443_RTCCON_TICSEL;
-		writew(tmp, s3c_rtc_base + S3C2410_RTCCON);
-	}
+	if (info->data->select_tick_clk)
+		info->data->select_tick_clk(info);
 
-	platform_set_drvdata(pdev, rtc);
+	s3c_rtc_setfreq(info, 1);
 
-	s3c_rtc_setfreq(&pdev->dev, 1);
-
-	ret = devm_request_irq(&pdev->dev, s3c_rtc_alarmno, s3c_rtc_alarmirq,
-			  0,  "s3c2410-rtc alarm", rtc);
-	if (ret) {
-		dev_err(&pdev->dev, "IRQ%d error %d\n", s3c_rtc_alarmno, ret);
-		goto err_nortc;
-	}
-
-	ret = devm_request_irq(&pdev->dev, s3c_rtc_tickno, s3c_rtc_tickirq,
-			  0,  "s3c2410-rtc tick", rtc);
-	if (ret) {
-		dev_err(&pdev->dev, "IRQ%d error %d\n", s3c_rtc_tickno, ret);
-		goto err_nortc;
-	}
-
-	clk_disable(rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 
 	return 0;
 
  err_nortc:
-	s3c_rtc_enable(pdev, 0);
-	clk_disable_unprepare(rtc_clk);
+	if (info->data->disable)
+		info->data->disable(info);
+	clk_disable_unprepare(info->rtc_clk);
 
 	return ret;
 }
 
 #ifdef CONFIG_PM_SLEEP
-/* RTC Power management control */
-
-static int ticnt_save, ticnt_en_save;
-static bool wake_en;
 
 static int s3c_rtc_suspend(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
+	struct s3c_rtc *info = dev_get_drvdata(dev);
+
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
 
-	clk_enable(rtc_clk);
 	/* save TICNT for anyone using periodic interrupts */
-	if (s3c_rtc_cpu_type == TYPE_S3C64XX) {
-		ticnt_en_save = readw(s3c_rtc_base + S3C2410_RTCCON);
-		ticnt_en_save &= S3C64XX_RTCCON_TICEN;
-		ticnt_save = readl(s3c_rtc_base + S3C2410_TICNT);
-	} else {
-		ticnt_save = readb(s3c_rtc_base + S3C2410_TICNT);
-	}
-	s3c_rtc_enable(pdev, 0);
+	if (info->data->save_tick_cnt)
+		info->data->save_tick_cnt(info);
+
+	if (info->data->disable)
+		info->data->disable(info);
 
-	if (device_may_wakeup(dev) && !wake_en) {
-		if (enable_irq_wake(s3c_rtc_alarmno) == 0)
-			wake_en = true;
+	if (device_may_wakeup(dev) && !info->wake_en) {
+		if (enable_irq_wake(info->irq_alarm) == 0)
+			info->wake_en = true;
 		else
 			dev_err(dev, "enable_irq_wake failed\n");
 	}
-	clk_disable(rtc_clk);
+
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 
 	return 0;
 }
 
 static int s3c_rtc_resume(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	unsigned int tmp;
+	struct s3c_rtc *info = dev_get_drvdata(dev);
 
-	clk_enable(rtc_clk);
-	s3c_rtc_enable(pdev, 1);
-	if (s3c_rtc_cpu_type == TYPE_S3C64XX) {
-		writel(ticnt_save, s3c_rtc_base + S3C2410_TICNT);
-		if (ticnt_en_save) {
-			tmp = readw(s3c_rtc_base + S3C2410_RTCCON);
-			writew(tmp | ticnt_en_save,
-					s3c_rtc_base + S3C2410_RTCCON);
-		}
-	} else {
-		writeb(ticnt_save, s3c_rtc_base + S3C2410_TICNT);
-	}
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
 
-	if (device_may_wakeup(dev) && wake_en) {
-		disable_irq_wake(s3c_rtc_alarmno);
-		wake_en = false;
+	if (info->data->enable)
+		info->data->enable(info);
+
+	if (info->data->restore_tick_cnt)
+		info->data->restore_tick_cnt(info);
+
+	if (device_may_wakeup(dev) && info->wake_en) {
+		disable_irq_wake(info->irq_alarm);
+		info->wake_en = false;
 	}
-	clk_disable(rtc_clk);
+
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
 
 	return 0;
 }
 #endif
-
 static SIMPLE_DEV_PM_OPS(s3c_rtc_pm_ops, s3c_rtc_suspend, s3c_rtc_resume);
 
-#ifdef CONFIG_OF
-static struct s3c_rtc_drv_data s3c_rtc_drv_data_array[] = {
-	[TYPE_S3C2410] = { TYPE_S3C2410 },
-	[TYPE_S3C2416] = { TYPE_S3C2416 },
-	[TYPE_S3C2443] = { TYPE_S3C2443 },
-	[TYPE_S3C64XX] = { TYPE_S3C64XX },
+static void s3c24xx_rtc_irq(struct s3c_rtc *info, int mask)
+{
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+	rtc_update_irq(info->rtc, 1, RTC_AF | RTC_IRQF);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
+
+	s3c_rtc_alarm_clk_enable(info, false);
+}
+
+static void s3c6410_rtc_irq(struct s3c_rtc *info, int mask)
+{
+	clk_enable(info->rtc_clk);
+	if (info->data->needs_src_clk)
+		clk_enable(info->rtc_src_clk);
+	rtc_update_irq(info->rtc, 1, RTC_AF | RTC_IRQF);
+	writeb(mask, info->base + S3C2410_INTP);
+	if (info->data->needs_src_clk)
+		clk_disable(info->rtc_src_clk);
+	clk_disable(info->rtc_clk);
+
+	s3c_rtc_alarm_clk_enable(info, false);
+}
+
+static void s3c2410_rtc_setfreq(struct s3c_rtc *info, int freq)
+{
+	unsigned int tmp = 0;
+	int val;
+
+	tmp = readb(info->base + S3C2410_TICNT);
+	tmp &= S3C2410_TICNT_ENABLE;
+
+	val = (info->rtc->max_user_freq / freq) - 1;
+	tmp |= val;
+
+	writel(tmp, info->base + S3C2410_TICNT);
+}
+
+static void s3c2416_rtc_setfreq(struct s3c_rtc *info, int freq)
+{
+	unsigned int tmp = 0;
+	int val;
+
+	tmp = readb(info->base + S3C2410_TICNT);
+	tmp &= S3C2410_TICNT_ENABLE;
+
+	val = (info->rtc->max_user_freq / freq) - 1;
+
+	tmp |= S3C2443_TICNT_PART(val);
+	writel(S3C2443_TICNT1_PART(val), info->base + S3C2443_TICNT1);
+
+	writel(S3C2416_TICNT2_PART(val), info->base + S3C2416_TICNT2);
+
+	writel(tmp, info->base + S3C2410_TICNT);
+}
+
+static void s3c2443_rtc_setfreq(struct s3c_rtc *info, int freq)
+{
+	unsigned int tmp = 0;
+	int val;
+
+	tmp = readb(info->base + S3C2410_TICNT);
+	tmp &= S3C2410_TICNT_ENABLE;
+
+	val = (info->rtc->max_user_freq / freq) - 1;
+
+	tmp |= S3C2443_TICNT_PART(val);
+	writel(S3C2443_TICNT1_PART(val), info->base + S3C2443_TICNT1);
+
+	writel(tmp, info->base + S3C2410_TICNT);
+}
+
+static void s3c6410_rtc_setfreq(struct s3c_rtc *info, int freq)
+{
+	int val;
+
+	val = (info->rtc->max_user_freq / freq) - 1;
+	writel(val, info->base + S3C2410_TICNT);
+}
+
+static void s3c24xx_rtc_enable_tick(struct s3c_rtc *info, struct seq_file *seq)
+{
+	unsigned int ticnt;
+
+	ticnt = readb(info->base + S3C2410_TICNT);
+	ticnt &= S3C2410_TICNT_ENABLE;
+
+	seq_printf(seq, "periodic_IRQ\t: %s\n", ticnt  ? "yes" : "no");
+}
+
+static void s3c2416_rtc_select_tick_clk(struct s3c_rtc *info)
+{
+	unsigned int con;
+
+	con = readw(info->base + S3C2410_RTCCON);
+	con |= S3C2443_RTCCON_TICSEL;
+	writew(con, info->base + S3C2410_RTCCON);
+}
+
+static void s3c6410_rtc_enable_tick(struct s3c_rtc *info, struct seq_file *seq)
+{
+	unsigned int ticnt;
+
+	ticnt = readw(info->base + S3C2410_RTCCON);
+	ticnt &= S3C64XX_RTCCON_TICEN;
+
+	seq_printf(seq, "periodic_IRQ\t: %s\n", ticnt  ? "yes" : "no");
+}
+
+static void s3c24xx_rtc_save_tick_cnt(struct s3c_rtc *info)
+{
+	info->ticnt_save = readb(info->base + S3C2410_TICNT);
+}
+
+static void s3c24xx_rtc_restore_tick_cnt(struct s3c_rtc *info)
+{
+	writeb(info->ticnt_save, info->base + S3C2410_TICNT);
+}
+
+static void s3c6410_rtc_save_tick_cnt(struct s3c_rtc *info)
+{
+	info->ticnt_en_save = readw(info->base + S3C2410_RTCCON);
+	info->ticnt_en_save &= S3C64XX_RTCCON_TICEN;
+	info->ticnt_save = readl(info->base + S3C2410_TICNT);
+}
+
+static void s3c6410_rtc_restore_tick_cnt(struct s3c_rtc *info)
+{
+	unsigned int con;
+
+	writel(info->ticnt_save, info->base + S3C2410_TICNT);
+	if (info->ticnt_en_save) {
+		con = readw(info->base + S3C2410_RTCCON);
+		writew(con | info->ticnt_en_save,
+				info->base + S3C2410_RTCCON);
+	}
+}
+
+static struct s3c_rtc_data const s3c2410_rtc_data = {
+	.max_user_freq		= 128,
+	.irq_handler		= s3c24xx_rtc_irq,
+	.set_freq		= s3c2410_rtc_setfreq,
+	.enable_tick		= s3c24xx_rtc_enable_tick,
+	.save_tick_cnt		= s3c24xx_rtc_save_tick_cnt,
+	.restore_tick_cnt	= s3c24xx_rtc_restore_tick_cnt,
+	.enable			= s3c24xx_rtc_enable,
+	.disable		= s3c24xx_rtc_disable,
+};
+
+static struct s3c_rtc_data const s3c2416_rtc_data = {
+	.max_user_freq		= 32768,
+	.irq_handler		= s3c24xx_rtc_irq,
+	.set_freq		= s3c2416_rtc_setfreq,
+	.enable_tick		= s3c24xx_rtc_enable_tick,
+	.select_tick_clk	= s3c2416_rtc_select_tick_clk,
+	.save_tick_cnt		= s3c24xx_rtc_save_tick_cnt,
+	.restore_tick_cnt	= s3c24xx_rtc_restore_tick_cnt,
+	.enable			= s3c24xx_rtc_enable,
+	.disable		= s3c24xx_rtc_disable,
+};
+
+static struct s3c_rtc_data const s3c2443_rtc_data = {
+	.max_user_freq		= 32768,
+	.irq_handler		= s3c24xx_rtc_irq,
+	.set_freq		= s3c2443_rtc_setfreq,
+	.enable_tick		= s3c24xx_rtc_enable_tick,
+	.select_tick_clk	= s3c2416_rtc_select_tick_clk,
+	.save_tick_cnt		= s3c24xx_rtc_save_tick_cnt,
+	.restore_tick_cnt	= s3c24xx_rtc_restore_tick_cnt,
+	.enable			= s3c24xx_rtc_enable,
+	.disable		= s3c24xx_rtc_disable,
+};
+
+static struct s3c_rtc_data const s3c6410_rtc_data = {
+	.max_user_freq		= 32768,
+	.irq_handler		= s3c6410_rtc_irq,
+	.set_freq		= s3c6410_rtc_setfreq,
+	.enable_tick		= s3c6410_rtc_enable_tick,
+	.save_tick_cnt		= s3c6410_rtc_save_tick_cnt,
+	.restore_tick_cnt	= s3c6410_rtc_restore_tick_cnt,
+	.enable			= s3c24xx_rtc_enable,
+	.disable		= s3c6410_rtc_disable,
+};
+
+static struct s3c_rtc_data const exynos3250_rtc_data = {
+	.max_user_freq		= 32768,
+	.needs_src_clk		= true,
+	.irq_handler		= s3c6410_rtc_irq,
+	.set_freq		= s3c6410_rtc_setfreq,
+	.enable_tick		= s3c6410_rtc_enable_tick,
+	.save_tick_cnt		= s3c6410_rtc_save_tick_cnt,
+	.restore_tick_cnt	= s3c6410_rtc_restore_tick_cnt,
+	.enable			= s3c24xx_rtc_enable,
+	.disable		= s3c6410_rtc_disable,
 };
 
 static const struct of_device_id s3c_rtc_dt_match[] = {
 	{
 		.compatible = "samsung,s3c2410-rtc",
-		.data = &s3c_rtc_drv_data_array[TYPE_S3C2410],
+		.data = (void *)&s3c2410_rtc_data,
 	}, {
 		.compatible = "samsung,s3c2416-rtc",
-		.data = &s3c_rtc_drv_data_array[TYPE_S3C2416],
+		.data = (void *)&s3c2416_rtc_data,
 	}, {
 		.compatible = "samsung,s3c2443-rtc",
-		.data = &s3c_rtc_drv_data_array[TYPE_S3C2443],
+		.data = (void *)&s3c2443_rtc_data,
 	}, {
 		.compatible = "samsung,s3c6410-rtc",
-		.data = &s3c_rtc_drv_data_array[TYPE_S3C64XX],
-	},
-	{},
-};
-MODULE_DEVICE_TABLE(of, s3c_rtc_dt_match);
-#endif
-
-static struct platform_device_id s3c_rtc_driver_ids[] = {
-	{
-		.name		= "s3c2410-rtc",
-		.driver_data	= TYPE_S3C2410,
-	}, {
-		.name		= "s3c2416-rtc",
-		.driver_data	= TYPE_S3C2416,
-	}, {
-		.name		= "s3c2443-rtc",
-		.driver_data	= TYPE_S3C2443,
+		.data = (void *)&s3c6410_rtc_data,
 	}, {
-		.name		= "s3c64xx-rtc",
-		.driver_data	= TYPE_S3C64XX,
+		.compatible = "samsung,exynos3250-rtc",
+		.data = (void *)&exynos3250_rtc_data,
 	},
-	{ }
+	{ /* sentinel */ },
 };
-
-MODULE_DEVICE_TABLE(platform, s3c_rtc_driver_ids);
+MODULE_DEVICE_TABLE(of, s3c_rtc_dt_match);
 
 static struct platform_driver s3c_rtc_driver = {
 	.probe		= s3c_rtc_probe,
 	.remove		= s3c_rtc_remove,
-	.id_table	= s3c_rtc_driver_ids,
 	.driver		= {
 		.name	= "s3c-rtc",
 		.owner	= THIS_MODULE,
@@ -687,7 +899,6 @@ static struct platform_driver s3c_rtc_driver = {
 		.of_match_table	= of_match_ptr(s3c_rtc_dt_match),
 	},
 };
-
 module_platform_driver(s3c_rtc_driver);
 
 MODULE_DESCRIPTION("Samsung S3C RTC Driver");
diff --git a/drivers/rtc/rtc-s5m.c b/drivers/rtc/rtc-s5m.c
index 8f06250a0389..8754c33361e8 100644
--- a/drivers/rtc/rtc-s5m.c
+++ b/drivers/rtc/rtc-s5m.c
@@ -717,12 +717,14 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 	info->device_type = s5m87xx->device_type;
 	info->wtsr_smpl = s5m87xx->wtsr_smpl;
 
-	info->irq = regmap_irq_get_virq(s5m87xx->irq_data, alarm_irq);
-	if (info->irq <= 0) {
-		ret = -EINVAL;
-		dev_err(&pdev->dev, "Failed to get virtual IRQ %d\n",
+	if (s5m87xx->irq_data) {
+		info->irq = regmap_irq_get_virq(s5m87xx->irq_data, alarm_irq);
+		if (info->irq <= 0) {
+			ret = -EINVAL;
+			dev_err(&pdev->dev, "Failed to get virtual IRQ %d\n",
 				alarm_irq);
-		goto err;
+			goto err;
+		}
 	}
 
 	platform_set_drvdata(pdev, info);
@@ -744,6 +746,11 @@ static int s5m_rtc_probe(struct platform_device *pdev)
 		goto err;
 	}
 
+	if (!info->irq) {
+		dev_info(&pdev->dev, "Alarm IRQ not available\n");
+		return 0;
+	}
+
 	ret = devm_request_threaded_irq(&pdev->dev, info->irq, NULL,
 					s5m_rtc_alarm_irq, 0, "rtc-alarm0",
 					info);
@@ -802,7 +809,7 @@ static int s5m_rtc_resume(struct device *dev)
 	struct s5m_rtc_info *info = dev_get_drvdata(dev);
 	int ret = 0;
 
-	if (device_may_wakeup(dev))
+	if (info->irq && device_may_wakeup(dev))
 		ret = disable_irq_wake(info->irq);
 
 	return ret;
@@ -813,7 +820,7 @@ static int s5m_rtc_suspend(struct device *dev)
 	struct s5m_rtc_info *info = dev_get_drvdata(dev);
 	int ret = 0;
 
-	if (device_may_wakeup(dev))
+	if (info->irq && device_may_wakeup(dev))
 		ret = enable_irq_wake(info->irq);
 
 	return ret;
diff --git a/drivers/rtc/rtc-sun6i.c b/drivers/rtc/rtc-sun6i.c
new file mode 100644
index 000000000000..c169a2cd4727
--- /dev/null
+++ b/drivers/rtc/rtc-sun6i.c
@@ -0,0 +1,447 @@
+/*
+ * An RTC driver for Allwinner A31/A23
+ *
+ * Copyright (c) 2014, Chen-Yu Tsai <wens@csie.org>
+ *
+ * based on rtc-sunxi.c
+ *
+ * An RTC driver for Allwinner A10/A20
+ *
+ * Copyright (c) 2013, Carlo Caione <carlo.caione@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/fs.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/platform_device.h>
+#include <linux/rtc.h>
+#include <linux/types.h>
+
+/* Control register */
+#define SUN6I_LOSC_CTRL				0x0000
+#define SUN6I_LOSC_CTRL_ALM_DHMS_ACC		BIT(9)
+#define SUN6I_LOSC_CTRL_RTC_HMS_ACC		BIT(8)
+#define SUN6I_LOSC_CTRL_RTC_YMD_ACC		BIT(7)
+#define SUN6I_LOSC_CTRL_ACC_MASK		GENMASK(9, 7)
+
+/* RTC */
+#define SUN6I_RTC_YMD				0x0010
+#define SUN6I_RTC_HMS				0x0014
+
+/* Alarm 0 (counter) */
+#define SUN6I_ALRM_COUNTER			0x0020
+#define SUN6I_ALRM_CUR_VAL			0x0024
+#define SUN6I_ALRM_EN				0x0028
+#define SUN6I_ALRM_EN_CNT_EN			BIT(0)
+#define SUN6I_ALRM_IRQ_EN			0x002c
+#define SUN6I_ALRM_IRQ_EN_CNT_IRQ_EN		BIT(0)
+#define SUN6I_ALRM_IRQ_STA			0x0030
+#define SUN6I_ALRM_IRQ_STA_CNT_IRQ_PEND		BIT(0)
+
+/* Alarm 1 (wall clock) */
+#define SUN6I_ALRM1_EN				0x0044
+#define SUN6I_ALRM1_IRQ_EN			0x0048
+#define SUN6I_ALRM1_IRQ_STA			0x004c
+#define SUN6I_ALRM1_IRQ_STA_WEEK_IRQ_PEND	BIT(0)
+
+/* Alarm config */
+#define SUN6I_ALARM_CONFIG			0x0050
+#define SUN6I_ALARM_CONFIG_WAKEUP		BIT(0)
+
+/*
+ * Get date values
+ */
+#define SUN6I_DATE_GET_DAY_VALUE(x)		((x)  & 0x0000001f)
+#define SUN6I_DATE_GET_MON_VALUE(x)		(((x) & 0x00000f00) >> 8)
+#define SUN6I_DATE_GET_YEAR_VALUE(x)		(((x) & 0x003f0000) >> 16)
+#define SUN6I_LEAP_GET_VALUE(x)			(((x) & 0x00400000) >> 22)
+
+/*
+ * Get time values
+ */
+#define SUN6I_TIME_GET_SEC_VALUE(x)		((x)  & 0x0000003f)
+#define SUN6I_TIME_GET_MIN_VALUE(x)		(((x) & 0x00003f00) >> 8)
+#define SUN6I_TIME_GET_HOUR_VALUE(x)		(((x) & 0x001f0000) >> 16)
+
+/*
+ * Set date values
+ */
+#define SUN6I_DATE_SET_DAY_VALUE(x)		((x)       & 0x0000001f)
+#define SUN6I_DATE_SET_MON_VALUE(x)		((x) <<  8 & 0x00000f00)
+#define SUN6I_DATE_SET_YEAR_VALUE(x)		((x) << 16 & 0x003f0000)
+#define SUN6I_LEAP_SET_VALUE(x)			((x) << 22 & 0x00400000)
+
+/*
+ * Set time values
+ */
+#define SUN6I_TIME_SET_SEC_VALUE(x)		((x)       & 0x0000003f)
+#define SUN6I_TIME_SET_MIN_VALUE(x)		((x) <<  8 & 0x00003f00)
+#define SUN6I_TIME_SET_HOUR_VALUE(x)		((x) << 16 & 0x001f0000)
+
+/*
+ * The year parameter passed to the driver is usually an offset relative to
+ * the year 1900. This macro is used to convert this offset to another one
+ * relative to the minimum year allowed by the hardware.
+ *
+ * The year range is 1970 - 2033. This range is selected to match Allwinner's
+ * driver, even though it is somewhat limited.
+ */
+#define SUN6I_YEAR_MIN				1970
+#define SUN6I_YEAR_MAX				2033
+#define SUN6I_YEAR_OFF				(SUN6I_YEAR_MIN - 1900)
+
+struct sun6i_rtc_dev {
+	struct rtc_device *rtc;
+	struct device *dev;
+	void __iomem *base;
+	int irq;
+	unsigned long alarm;
+};
+
+static irqreturn_t sun6i_rtc_alarmirq(int irq, void *id)
+{
+	struct sun6i_rtc_dev *chip = (struct sun6i_rtc_dev *) id;
+	u32 val;
+
+	val = readl(chip->base + SUN6I_ALRM_IRQ_STA);
+
+	if (val & SUN6I_ALRM_IRQ_STA_CNT_IRQ_PEND) {
+		val |= SUN6I_ALRM_IRQ_STA_CNT_IRQ_PEND;
+		writel(val, chip->base + SUN6I_ALRM_IRQ_STA);
+
+		rtc_update_irq(chip->rtc, 1, RTC_AF | RTC_IRQF);
+
+		return IRQ_HANDLED;
+	}
+
+	return IRQ_NONE;
+}
+
+static void sun6i_rtc_setaie(int to, struct sun6i_rtc_dev *chip)
+{
+	u32 alrm_val = 0;
+	u32 alrm_irq_val = 0;
+	u32 alrm_wake_val = 0;
+
+	if (to) {
+		alrm_val = SUN6I_ALRM_EN_CNT_EN;
+		alrm_irq_val = SUN6I_ALRM_IRQ_EN_CNT_IRQ_EN;
+		alrm_wake_val = SUN6I_ALARM_CONFIG_WAKEUP;
+	} else {
+		writel(SUN6I_ALRM_IRQ_STA_CNT_IRQ_PEND,
+		       chip->base + SUN6I_ALRM_IRQ_STA);
+	}
+
+	writel(alrm_val, chip->base + SUN6I_ALRM_EN);
+	writel(alrm_irq_val, chip->base + SUN6I_ALRM_IRQ_EN);
+	writel(alrm_wake_val, chip->base + SUN6I_ALARM_CONFIG);
+}
+
+static int sun6i_rtc_gettime(struct device *dev, struct rtc_time *rtc_tm)
+{
+	struct sun6i_rtc_dev *chip = dev_get_drvdata(dev);
+	u32 date, time;
+
+	/*
+	 * read again in case it changes
+	 */
+	do {
+		date = readl(chip->base + SUN6I_RTC_YMD);
+		time = readl(chip->base + SUN6I_RTC_HMS);
+	} while ((date != readl(chip->base + SUN6I_RTC_YMD)) ||
+		 (time != readl(chip->base + SUN6I_RTC_HMS)));
+
+	rtc_tm->tm_sec  = SUN6I_TIME_GET_SEC_VALUE(time);
+	rtc_tm->tm_min  = SUN6I_TIME_GET_MIN_VALUE(time);
+	rtc_tm->tm_hour = SUN6I_TIME_GET_HOUR_VALUE(time);
+
+	rtc_tm->tm_mday = SUN6I_DATE_GET_DAY_VALUE(date);
+	rtc_tm->tm_mon  = SUN6I_DATE_GET_MON_VALUE(date);
+	rtc_tm->tm_year = SUN6I_DATE_GET_YEAR_VALUE(date);
+
+	rtc_tm->tm_mon  -= 1;
+
+	/*
+	 * switch from (data_year->min)-relative offset to
+	 * a (1900)-relative one
+	 */
+	rtc_tm->tm_year += SUN6I_YEAR_OFF;
+
+	return rtc_valid_tm(rtc_tm);
+}
+
+static int sun6i_rtc_getalarm(struct device *dev, struct rtc_wkalrm *wkalrm)
+{
+	struct sun6i_rtc_dev *chip = dev_get_drvdata(dev);
+	u32 alrm_st;
+	u32 alrm_en;
+
+	alrm_en = readl(chip->base + SUN6I_ALRM_IRQ_EN);
+	alrm_st = readl(chip->base + SUN6I_ALRM_IRQ_STA);
+	wkalrm->enabled = !!(alrm_en & SUN6I_ALRM_EN_CNT_EN);
+	wkalrm->pending = !!(alrm_st & SUN6I_ALRM_EN_CNT_EN);
+	rtc_time_to_tm(chip->alarm, &wkalrm->time);
+
+	return 0;
+}
+
+static int sun6i_rtc_setalarm(struct device *dev, struct rtc_wkalrm *wkalrm)
+{
+	struct sun6i_rtc_dev *chip = dev_get_drvdata(dev);
+	struct rtc_time *alrm_tm = &wkalrm->time;
+	struct rtc_time tm_now;
+	unsigned long time_now = 0;
+	unsigned long time_set = 0;
+	unsigned long time_gap = 0;
+	int ret = 0;
+
+	ret = sun6i_rtc_gettime(dev, &tm_now);
+	if (ret < 0) {
+		dev_err(dev, "Error in getting time\n");
+		return -EINVAL;
+	}
+
+	rtc_tm_to_time(alrm_tm, &time_set);
+	rtc_tm_to_time(&tm_now, &time_now);
+	if (time_set <= time_now) {
+		dev_err(dev, "Date to set in the past\n");
+		return -EINVAL;
+	}
+
+	time_gap = time_set - time_now;
+
+	if (time_gap > U32_MAX) {
+		dev_err(dev, "Date too far in the future\n");
+		return -EINVAL;
+	}
+
+	sun6i_rtc_setaie(0, chip);
+	writel(0, chip->base + SUN6I_ALRM_COUNTER);
+	usleep_range(100, 300);
+
+	writel(time_gap, chip->base + SUN6I_ALRM_COUNTER);
+	chip->alarm = time_set;
+
+	sun6i_rtc_setaie(wkalrm->enabled, chip);
+
+	return 0;
+}
+
+static int sun6i_rtc_wait(struct sun6i_rtc_dev *chip, int offset,
+			  unsigned int mask, unsigned int ms_timeout)
+{
+	const unsigned long timeout = jiffies + msecs_to_jiffies(ms_timeout);
+	u32 reg;
+
+	do {
+		reg = readl(chip->base + offset);
+		reg &= mask;
+
+		if (!reg)
+			return 0;
+
+	} while (time_before(jiffies, timeout));
+
+	return -ETIMEDOUT;
+}
+
+static int sun6i_rtc_settime(struct device *dev, struct rtc_time *rtc_tm)
+{
+	struct sun6i_rtc_dev *chip = dev_get_drvdata(dev);
+	u32 date = 0;
+	u32 time = 0;
+	int year;
+
+	year = rtc_tm->tm_year + 1900;
+	if (year < SUN6I_YEAR_MIN || year > SUN6I_YEAR_MAX) {
+		dev_err(dev, "rtc only supports year in range %d - %d\n",
+			SUN6I_YEAR_MIN, SUN6I_YEAR_MAX);
+		return -EINVAL;
+	}
+
+	rtc_tm->tm_year -= SUN6I_YEAR_OFF;
+	rtc_tm->tm_mon += 1;
+
+	date = SUN6I_DATE_SET_DAY_VALUE(rtc_tm->tm_mday) |
+		SUN6I_DATE_SET_MON_VALUE(rtc_tm->tm_mon)  |
+		SUN6I_DATE_SET_YEAR_VALUE(rtc_tm->tm_year);
+
+	if (is_leap_year(year))
+		date |= SUN6I_LEAP_SET_VALUE(1);
+
+	time = SUN6I_TIME_SET_SEC_VALUE(rtc_tm->tm_sec)  |
+		SUN6I_TIME_SET_MIN_VALUE(rtc_tm->tm_min)  |
+		SUN6I_TIME_SET_HOUR_VALUE(rtc_tm->tm_hour);
+
+	/* Check whether registers are writable */
+	if (sun6i_rtc_wait(chip, SUN6I_LOSC_CTRL,
+			   SUN6I_LOSC_CTRL_ACC_MASK, 50)) {
+		dev_err(dev, "rtc is still busy.\n");
+		return -EBUSY;
+	}
+
+	writel(time, chip->base + SUN6I_RTC_HMS);
+
+	/*
+	 * After writing the RTC HH-MM-SS register, the
+	 * SUN6I_LOSC_CTRL_RTC_HMS_ACC bit is set and it will not
+	 * be cleared until the real writing operation is finished
+	 */
+
+	if (sun6i_rtc_wait(chip, SUN6I_LOSC_CTRL,
+			   SUN6I_LOSC_CTRL_RTC_HMS_ACC, 50)) {
+		dev_err(dev, "Failed to set rtc time.\n");
+		return -ETIMEDOUT;
+	}
+
+	writel(date, chip->base + SUN6I_RTC_YMD);
+
+	/*
+	 * After writing the RTC YY-MM-DD register, the
+	 * SUN6I_LOSC_CTRL_RTC_YMD_ACC bit is set and it will not
+	 * be cleared until the real writing operation is finished
+	 */
+
+	if (sun6i_rtc_wait(chip, SUN6I_LOSC_CTRL,
+			   SUN6I_LOSC_CTRL_RTC_YMD_ACC, 50)) {
+		dev_err(dev, "Failed to set rtc time.\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int sun6i_rtc_alarm_irq_enable(struct device *dev, unsigned int enabled)
+{
+	struct sun6i_rtc_dev *chip = dev_get_drvdata(dev);
+
+	if (!enabled)
+		sun6i_rtc_setaie(enabled, chip);
+
+	return 0;
+}
+
+static const struct rtc_class_ops sun6i_rtc_ops = {
+	.read_time		= sun6i_rtc_gettime,
+	.set_time		= sun6i_rtc_settime,
+	.read_alarm		= sun6i_rtc_getalarm,
+	.set_alarm		= sun6i_rtc_setalarm,
+	.alarm_irq_enable	= sun6i_rtc_alarm_irq_enable
+};
+
+static int sun6i_rtc_probe(struct platform_device *pdev)
+{
+	struct sun6i_rtc_dev *chip;
+	struct resource *res;
+	int ret;
+
+	chip = devm_kzalloc(&pdev->dev, sizeof(*chip), GFP_KERNEL);
+	if (!chip)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, chip);
+	chip->dev = &pdev->dev;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	chip->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(chip->base))
+		return PTR_ERR(chip->base);
+
+	chip->irq = platform_get_irq(pdev, 0);
+	if (chip->irq < 0) {
+		dev_err(&pdev->dev, "No IRQ resource\n");
+		return chip->irq;
+	}
+
+	ret = devm_request_irq(&pdev->dev, chip->irq, sun6i_rtc_alarmirq,
+			       0, dev_name(&pdev->dev), chip);
+	if (ret) {
+		dev_err(&pdev->dev, "Could not request IRQ\n");
+		return ret;
+	}
+
+	/* clear the alarm counter value */
+	writel(0, chip->base + SUN6I_ALRM_COUNTER);
+
+	/* disable counter alarm */
+	writel(0, chip->base + SUN6I_ALRM_EN);
+
+	/* disable counter alarm interrupt */
+	writel(0, chip->base + SUN6I_ALRM_IRQ_EN);
+
+	/* disable week alarm */
+	writel(0, chip->base + SUN6I_ALRM1_EN);
+
+	/* disable week alarm interrupt */
+	writel(0, chip->base + SUN6I_ALRM1_IRQ_EN);
+
+	/* clear counter alarm pending interrupts */
+	writel(SUN6I_ALRM_IRQ_STA_CNT_IRQ_PEND,
+	       chip->base + SUN6I_ALRM_IRQ_STA);
+
+	/* clear week alarm pending interrupts */
+	writel(SUN6I_ALRM1_IRQ_STA_WEEK_IRQ_PEND,
+	       chip->base + SUN6I_ALRM1_IRQ_STA);
+
+	/* disable alarm wakeup */
+	writel(0, chip->base + SUN6I_ALARM_CONFIG);
+
+	chip->rtc = rtc_device_register("rtc-sun6i", &pdev->dev,
+					&sun6i_rtc_ops, THIS_MODULE);
+	if (IS_ERR(chip->rtc)) {
+		dev_err(&pdev->dev, "unable to register device\n");
+		return PTR_ERR(chip->rtc);
+	}
+
+	dev_info(&pdev->dev, "RTC enabled\n");
+
+	return 0;
+}
+
+static int sun6i_rtc_remove(struct platform_device *pdev)
+{
+	struct sun6i_rtc_dev *chip = platform_get_drvdata(pdev);
+
+	rtc_device_unregister(chip->rtc);
+
+	return 0;
+}
+
+static const struct of_device_id sun6i_rtc_dt_ids[] = {
+	{ .compatible = "allwinner,sun6i-a31-rtc" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, sun6i_rtc_dt_ids);
+
+static struct platform_driver sun6i_rtc_driver = {
+	.probe		= sun6i_rtc_probe,
+	.remove		= sun6i_rtc_remove,
+	.driver		= {
+		.name		= "sun6i-rtc",
+		.of_match_table = sun6i_rtc_dt_ids,
+	},
+};
+
+module_platform_driver(sun6i_rtc_driver);
+
+MODULE_DESCRIPTION("sun6i RTC driver");
+MODULE_AUTHOR("Chen-Yu Tsai <wens@csie.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/s390/block/dasd.c b/drivers/s390/block/dasd.c
index 5df05f26b7d9..329db997ee66 100644
--- a/drivers/s390/block/dasd.c
+++ b/drivers/s390/block/dasd.c
@@ -1660,6 +1660,14 @@ void dasd_int_handler(struct ccw_device *cdev, unsigned long intparm,
 		device->discipline->check_for_device_change(device, cqr, irb);
 		dasd_put_device(device);
 	}
+
+	/* check for for attention message */
+	if (scsw_dstat(&irb->scsw) & DEV_STAT_ATTENTION) {
+		device = dasd_device_from_cdev_locked(cdev);
+		device->discipline->check_attention(device, irb->esw.esw1.lpum);
+		dasd_put_device(device);
+	}
+
 	if (!cqr)
 		return;
 
@@ -2261,8 +2269,8 @@ static inline int _wait_for_wakeup_queue(struct list_head *ccw_queue)
 static int _dasd_sleep_on_queue(struct list_head *ccw_queue, int interruptible)
 {
 	struct dasd_device *device;
-	int rc;
 	struct dasd_ccw_req *cqr, *n;
+	int rc;
 
 retry:
 	list_for_each_entry_safe(cqr, n, ccw_queue, blocklist) {
@@ -2310,21 +2318,26 @@ retry:
 		/*
 		 * for alias devices simplify error recovery and
 		 * return to upper layer
+		 * do not skip ERP requests
 		 */
-		if (cqr->startdev != cqr->basedev &&
+		if (cqr->startdev != cqr->basedev && !cqr->refers &&
 		    (cqr->status == DASD_CQR_TERMINATED ||
 		     cqr->status == DASD_CQR_NEED_ERP))
 			return -EAGAIN;
-		else {
-			/* normal recovery for basedev IO */
-			if (__dasd_sleep_on_erp(cqr)) {
-				if (!cqr->status == DASD_CQR_TERMINATED &&
-				    !cqr->status == DASD_CQR_NEED_ERP)
-					break;
-				rc = 1;
-			}
+
+		/* normal recovery for basedev IO */
+		if (__dasd_sleep_on_erp(cqr)) {
+			goto retry;
+			/* remember that ERP was needed */
+			rc = 1;
+			/* skip processing for active cqr */
+			if (cqr->status != DASD_CQR_TERMINATED &&
+			    cqr->status != DASD_CQR_NEED_ERP)
+				break;
 		}
 	}
+
+	/* start ERP requests in upper loop */
 	if (rc)
 		goto retry;
 
diff --git a/drivers/s390/block/dasd_devmap.c b/drivers/s390/block/dasd_devmap.c
index 2ead7e78c456..8286f742436b 100644
--- a/drivers/s390/block/dasd_devmap.c
+++ b/drivers/s390/block/dasd_devmap.c
@@ -77,7 +77,7 @@ EXPORT_SYMBOL_GPL(dasd_nofcx);
  * strings when running as a module.
  */
 static char *dasd[256];
-module_param_array(dasd, charp, NULL, 0);
+module_param_array(dasd, charp, NULL, S_IRUGO);
 
 /*
  * Single spinlock to protect devmap and servermap structures and lists.
@@ -1432,6 +1432,29 @@ static ssize_t dasd_reservation_state_store(struct device *dev,
 static DEVICE_ATTR(last_known_reservation_state, 0644,
 		   dasd_reservation_state_show, dasd_reservation_state_store);
 
+static ssize_t dasd_pm_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct dasd_device *device;
+	u8 opm, nppm, cablepm, cuirpm, hpfpm;
+
+	device = dasd_device_from_cdev(to_ccwdev(dev));
+	if (IS_ERR(device))
+		return sprintf(buf, "0\n");
+
+	opm = device->path_data.opm;
+	nppm = device->path_data.npm;
+	cablepm = device->path_data.cablepm;
+	cuirpm = device->path_data.cuirpm;
+	hpfpm = device->path_data.hpfpm;
+	dasd_put_device(device);
+
+	return sprintf(buf, "%02x %02x %02x %02x %02x\n", opm, nppm,
+		       cablepm, cuirpm, hpfpm);
+}
+
+static DEVICE_ATTR(path_masks, 0444, dasd_pm_show, NULL);
+
 static struct attribute * dasd_attrs[] = {
 	&dev_attr_readonly.attr,
 	&dev_attr_discipline.attr,
@@ -1450,6 +1473,7 @@ static struct attribute * dasd_attrs[] = {
 	&dev_attr_reservation_policy.attr,
 	&dev_attr_last_known_reservation_state.attr,
 	&dev_attr_safe_offline.attr,
+	&dev_attr_path_masks.attr,
 	NULL,
 };
 
diff --git a/drivers/s390/block/dasd_eckd.c b/drivers/s390/block/dasd_eckd.c
index 51dea7baf02c..d47f5b99623a 100644
--- a/drivers/s390/block/dasd_eckd.c
+++ b/drivers/s390/block/dasd_eckd.c
@@ -29,6 +29,8 @@
 #include <asm/cio.h>
 #include <asm/ccwdev.h>
 #include <asm/itcw.h>
+#include <asm/schid.h>
+#include <asm/chpid.h>
 
 #include "dasd_int.h"
 #include "dasd_eckd.h"
@@ -112,6 +114,12 @@ struct path_verification_work_data {
 static struct path_verification_work_data *path_verification_worker;
 static DEFINE_MUTEX(dasd_path_verification_mutex);
 
+struct check_attention_work_data {
+	struct work_struct worker;
+	struct dasd_device *device;
+	__u8 lpum;
+};
+
 /* initial attempt at a probe function. this can be simplified once
  * the other detection code is gone */
 static int
@@ -1126,6 +1134,7 @@ static int dasd_eckd_read_conf(struct dasd_device *device)
 					"device %s instead of %s\n", lpm,
 					print_path_uid, print_device_uid);
 				path_err = -EINVAL;
+				path_data->cablepm |= lpm;
 				continue;
 			}
 
@@ -1141,6 +1150,13 @@ static int dasd_eckd_read_conf(struct dasd_device *device)
 			break;
 		}
 		path_data->opm |= lpm;
+		/*
+		 * if the path is used
+		 * it should not be in one of the negative lists
+		 */
+		path_data->cablepm &= ~lpm;
+		path_data->hpfpm &= ~lpm;
+		path_data->cuirpm &= ~lpm;
 
 		if (conf_data != private->conf_data)
 			kfree(conf_data);
@@ -1230,7 +1246,7 @@ static void do_path_verification_work(struct work_struct *work)
 	struct dasd_eckd_private path_private;
 	struct dasd_uid *uid;
 	__u8 path_rcd_buf[DASD_ECKD_RCD_DATA_SIZE];
-	__u8 lpm, opm, npm, ppm, epm;
+	__u8 lpm, opm, npm, ppm, epm, hpfpm, cablepm;
 	unsigned long flags;
 	char print_uid[60];
 	int rc;
@@ -1248,6 +1264,9 @@ static void do_path_verification_work(struct work_struct *work)
 	npm = 0;
 	ppm = 0;
 	epm = 0;
+	hpfpm = 0;
+	cablepm = 0;
+
 	for (lpm = 0x80; lpm; lpm >>= 1) {
 		if (!(lpm & data->tbvpm))
 			continue;
@@ -1289,6 +1308,7 @@ static void do_path_verification_work(struct work_struct *work)
 			opm &= ~lpm;
 			npm &= ~lpm;
 			ppm &= ~lpm;
+			hpfpm |= lpm;
 			continue;
 		}
 
@@ -1350,6 +1370,7 @@ static void do_path_verification_work(struct work_struct *work)
 				opm &= ~lpm;
 				npm &= ~lpm;
 				ppm &= ~lpm;
+				cablepm |= lpm;
 				continue;
 			}
 		}
@@ -1364,12 +1385,21 @@ static void do_path_verification_work(struct work_struct *work)
 		spin_lock_irqsave(get_ccwdev_lock(device->cdev), flags);
 		if (!device->path_data.opm && opm) {
 			device->path_data.opm = opm;
+			device->path_data.cablepm &= ~opm;
+			device->path_data.cuirpm &= ~opm;
+			device->path_data.hpfpm &= ~opm;
 			dasd_generic_path_operational(device);
-		} else
+		} else {
 			device->path_data.opm |= opm;
+			device->path_data.cablepm &= ~opm;
+			device->path_data.cuirpm &= ~opm;
+			device->path_data.hpfpm &= ~opm;
+		}
 		device->path_data.npm |= npm;
 		device->path_data.ppm |= ppm;
 		device->path_data.tbvpm |= epm;
+		device->path_data.cablepm |= cablepm;
+		device->path_data.hpfpm |= hpfpm;
 		spin_unlock_irqrestore(get_ccwdev_lock(device->cdev), flags);
 	}
 
@@ -4475,6 +4505,343 @@ out_err:
 	return -1;
 }
 
+static int dasd_eckd_read_message_buffer(struct dasd_device *device,
+					 struct dasd_rssd_messages *messages,
+					 __u8 lpum)
+{
+	struct dasd_rssd_messages *message_buf;
+	struct dasd_psf_prssd_data *prssdp;
+	struct dasd_eckd_private *private;
+	struct dasd_ccw_req *cqr;
+	struct ccw1 *ccw;
+	int rc;
+
+	private = (struct dasd_eckd_private *) device->private;
+	cqr = dasd_smalloc_request(DASD_ECKD_MAGIC, 1 /* PSF */	+ 1 /* RSSD */,
+				   (sizeof(struct dasd_psf_prssd_data) +
+				    sizeof(struct dasd_rssd_messages)),
+				   device);
+	if (IS_ERR(cqr)) {
+		DBF_EVENT_DEVID(DBF_WARNING, device->cdev, "%s",
+				"Could not allocate read message buffer request");
+		return PTR_ERR(cqr);
+	}
+
+	cqr->startdev = device;
+	cqr->memdev = device;
+	cqr->block = NULL;
+	cqr->retries = 256;
+	cqr->expires = 10 * HZ;
+
+	/* we need to check for messages on exactly this path */
+	set_bit(DASD_CQR_VERIFY_PATH, &cqr->flags);
+	cqr->lpm = lpum;
+
+	/* Prepare for Read Subsystem Data */
+	prssdp = (struct dasd_psf_prssd_data *) cqr->data;
+	memset(prssdp, 0, sizeof(struct dasd_psf_prssd_data));
+	prssdp->order = PSF_ORDER_PRSSD;
+	prssdp->suborder = 0x03;	/* Message Buffer */
+	/* all other bytes of prssdp must be zero */
+
+	ccw = cqr->cpaddr;
+	ccw->cmd_code = DASD_ECKD_CCW_PSF;
+	ccw->count = sizeof(struct dasd_psf_prssd_data);
+	ccw->flags |= CCW_FLAG_CC;
+	ccw->flags |= CCW_FLAG_SLI;
+	ccw->cda = (__u32)(addr_t) prssdp;
+
+	/* Read Subsystem Data - message buffer */
+	message_buf = (struct dasd_rssd_messages *) (prssdp + 1);
+	memset(message_buf, 0, sizeof(struct dasd_rssd_messages));
+
+	ccw++;
+	ccw->cmd_code = DASD_ECKD_CCW_RSSD;
+	ccw->count = sizeof(struct dasd_rssd_messages);
+	ccw->flags |= CCW_FLAG_SLI;
+	ccw->cda = (__u32)(addr_t) message_buf;
+
+	cqr->buildclk = get_tod_clock();
+	cqr->status = DASD_CQR_FILLED;
+	rc = dasd_sleep_on_immediatly(cqr);
+	if (rc == 0) {
+		prssdp = (struct dasd_psf_prssd_data *) cqr->data;
+		message_buf = (struct dasd_rssd_messages *)
+			(prssdp + 1);
+		memcpy(messages, message_buf,
+		       sizeof(struct dasd_rssd_messages));
+	} else
+		DBF_EVENT_DEVID(DBF_WARNING, device->cdev,
+				"Reading messages failed with rc=%d\n"
+				, rc);
+	dasd_sfree_request(cqr, cqr->memdev);
+	return rc;
+}
+
+/*
+ * Perform Subsystem Function - CUIR response
+ */
+static int
+dasd_eckd_psf_cuir_response(struct dasd_device *device, int response,
+			    __u32 message_id,
+			    struct channel_path_desc *desc,
+			    struct subchannel_id sch_id)
+{
+	struct dasd_psf_cuir_response *psf_cuir;
+	struct dasd_ccw_req *cqr;
+	struct ccw1 *ccw;
+	int rc;
+
+	cqr = dasd_smalloc_request(DASD_ECKD_MAGIC, 1 /* PSF */ ,
+				  sizeof(struct dasd_psf_cuir_response),
+				  device);
+
+	if (IS_ERR(cqr)) {
+		DBF_DEV_EVENT(DBF_WARNING, device, "%s",
+			   "Could not allocate PSF-CUIR request");
+		return PTR_ERR(cqr);
+	}
+
+	psf_cuir = (struct dasd_psf_cuir_response *)cqr->data;
+	psf_cuir->order = PSF_ORDER_CUIR_RESPONSE;
+	psf_cuir->cc = response;
+	if (desc)
+		psf_cuir->chpid = desc->chpid;
+	psf_cuir->message_id = message_id;
+	psf_cuir->cssid = sch_id.cssid;
+	psf_cuir->ssid = sch_id.ssid;
+
+	ccw = cqr->cpaddr;
+	ccw->cmd_code = DASD_ECKD_CCW_PSF;
+	ccw->cda = (__u32)(addr_t)psf_cuir;
+	ccw->count = sizeof(struct dasd_psf_cuir_response);
+
+	cqr->startdev = device;
+	cqr->memdev = device;
+	cqr->block = NULL;
+	cqr->retries = 256;
+	cqr->expires = 10*HZ;
+	cqr->buildclk = get_tod_clock();
+	cqr->status = DASD_CQR_FILLED;
+
+	rc = dasd_sleep_on(cqr);
+
+	dasd_sfree_request(cqr, cqr->memdev);
+	return rc;
+}
+
+static int dasd_eckd_cuir_change_state(struct dasd_device *device, __u8 lpum)
+{
+	unsigned long flags;
+	__u8 tbcpm;
+
+	spin_lock_irqsave(get_ccwdev_lock(device->cdev), flags);
+	tbcpm = device->path_data.opm & ~lpum;
+	if (tbcpm) {
+		device->path_data.opm = tbcpm;
+		device->path_data.cuirpm |= lpum;
+	}
+	spin_unlock_irqrestore(get_ccwdev_lock(device->cdev), flags);
+	return tbcpm ? 0 : PSF_CUIR_LAST_PATH;
+}
+
+/*
+ * walk through all devices and quiesce them
+ * if it is the last path return error
+ *
+ * if only part of the devices are quiesced and an error
+ * occurs no onlining necessary, the storage server will
+ * notify the already set offline devices again
+ */
+static int dasd_eckd_cuir_quiesce(struct dasd_device *device, __u8 lpum,
+				 struct channel_path_desc *desc,
+				 struct subchannel_id sch_id)
+{
+	struct alias_pav_group *pavgroup, *tempgroup;
+	struct dasd_eckd_private *private;
+	struct dasd_device *dev, *n;
+	int rc;
+
+	private = (struct dasd_eckd_private *) device->private;
+	rc = 0;
+
+	/* active devices */
+	list_for_each_entry_safe(dev, n,
+				 &private->lcu->active_devices,
+				 alias_list) {
+		rc = dasd_eckd_cuir_change_state(dev, lpum);
+		if (rc)
+			goto out;
+	}
+
+	/* inactive devices */
+	list_for_each_entry_safe(dev, n,
+				 &private->lcu->inactive_devices,
+				 alias_list) {
+		rc = dasd_eckd_cuir_change_state(dev, lpum);
+		if (rc)
+			goto out;
+	}
+
+	/* devices in PAV groups */
+	list_for_each_entry_safe(pavgroup, tempgroup,
+				 &private->lcu->grouplist, group) {
+		list_for_each_entry_safe(dev, n, &pavgroup->baselist,
+					 alias_list) {
+			rc = dasd_eckd_cuir_change_state(dev, lpum);
+			if (rc)
+				goto out;
+		}
+		list_for_each_entry_safe(dev, n, &pavgroup->aliaslist,
+					 alias_list) {
+			rc = dasd_eckd_cuir_change_state(dev, lpum);
+			if (rc)
+				goto out;
+		}
+	}
+
+	pr_warn("Service on the storage server caused path %x.%02x to go offline",
+		sch_id.cssid, desc ? desc->chpid : 0);
+	rc = PSF_CUIR_COMPLETED;
+out:
+	return rc;
+}
+
+static int dasd_eckd_cuir_resume(struct dasd_device *device, __u8 lpum,
+				 struct channel_path_desc *desc,
+				 struct subchannel_id sch_id)
+{
+	struct alias_pav_group *pavgroup, *tempgroup;
+	struct dasd_eckd_private *private;
+	struct dasd_device *dev, *n;
+
+	pr_info("Path %x.%02x is back online after service on the storage server",
+		sch_id.cssid, desc ? desc->chpid : 0);
+	private = (struct dasd_eckd_private *) device->private;
+
+	/*
+	 * the path may have been added through a generic path event before
+	 * only trigger path verification if the path is not already in use
+	 */
+
+	list_for_each_entry_safe(dev, n,
+				 &private->lcu->active_devices,
+				 alias_list) {
+		if (!(dev->path_data.opm & lpum)) {
+			dev->path_data.tbvpm |= lpum;
+			dasd_schedule_device_bh(dev);
+		}
+	}
+
+	list_for_each_entry_safe(dev, n,
+				 &private->lcu->inactive_devices,
+				 alias_list) {
+		if (!(dev->path_data.opm & lpum)) {
+			dev->path_data.tbvpm |= lpum;
+			dasd_schedule_device_bh(dev);
+		}
+	}
+
+	/* devices in PAV groups */
+	list_for_each_entry_safe(pavgroup, tempgroup,
+				 &private->lcu->grouplist,
+				 group) {
+		list_for_each_entry_safe(dev, n,
+					 &pavgroup->baselist,
+					 alias_list) {
+			if (!(dev->path_data.opm & lpum)) {
+				dev->path_data.tbvpm |= lpum;
+				dasd_schedule_device_bh(dev);
+			}
+		}
+		list_for_each_entry_safe(dev, n,
+					 &pavgroup->aliaslist,
+					 alias_list) {
+			if (!(dev->path_data.opm & lpum)) {
+				dev->path_data.tbvpm |= lpum;
+				dasd_schedule_device_bh(dev);
+			}
+		}
+	}
+	return PSF_CUIR_COMPLETED;
+}
+
+static void dasd_eckd_handle_cuir(struct dasd_device *device, void *messages,
+				 __u8 lpum)
+{
+	struct dasd_cuir_message *cuir = messages;
+	struct channel_path_desc *desc;
+	struct subchannel_id sch_id;
+	int pos, response;
+	ccw_device_get_schid(device->cdev, &sch_id);
+
+	/* get position of path in mask */
+	pos = 8 - ffs(lpum);
+	/* get channel path descriptor from this position */
+	desc = ccw_device_get_chp_desc(device->cdev, pos);
+
+	if (cuir->code == CUIR_QUIESCE) {
+		/* quiesce */
+		response = dasd_eckd_cuir_quiesce(device, lpum, desc, sch_id);
+	} else if (cuir->code == CUIR_RESUME) {
+		/* resume */
+		response = dasd_eckd_cuir_resume(device, lpum, desc, sch_id);
+	} else
+		response = PSF_CUIR_NOT_SUPPORTED;
+
+	dasd_eckd_psf_cuir_response(device, response, cuir->message_id,
+				    desc, sch_id);
+
+	/* free descriptor copy */
+	kfree(desc);
+}
+
+static void dasd_eckd_check_attention_work(struct work_struct *work)
+{
+	struct check_attention_work_data *data;
+	struct dasd_rssd_messages *messages;
+	struct dasd_device *device;
+	int rc;
+
+	data = container_of(work, struct check_attention_work_data, worker);
+	device = data->device;
+
+	messages = kzalloc(sizeof(*messages), GFP_KERNEL);
+	if (!messages) {
+		DBF_DEV_EVENT(DBF_WARNING, device, "%s",
+			      "Could not allocate attention message buffer");
+		goto out;
+	}
+
+	rc = dasd_eckd_read_message_buffer(device, messages, data->lpum);
+	if (rc)
+		goto out;
+
+	if (messages->length == ATTENTION_LENGTH_CUIR &&
+	    messages->format == ATTENTION_FORMAT_CUIR)
+		dasd_eckd_handle_cuir(device, messages, data->lpum);
+
+out:
+	dasd_put_device(device);
+	kfree(messages);
+	kfree(data);
+}
+
+static int dasd_eckd_check_attention(struct dasd_device *device, __u8 lpum)
+{
+	struct check_attention_work_data *data;
+
+	data = kzalloc(sizeof(*data), GFP_ATOMIC);
+	if (!data)
+		return -ENOMEM;
+	INIT_WORK(&data->worker, dasd_eckd_check_attention_work);
+	dasd_get_device(device);
+	data->device = device;
+	data->lpum = lpum;
+	schedule_work(&data->worker);
+	return 0;
+}
+
 static struct ccw_driver dasd_eckd_driver = {
 	.driver = {
 		.name	= "dasd-eckd",
@@ -4539,6 +4906,7 @@ static struct dasd_discipline dasd_eckd_discipline = {
 	.reload = dasd_eckd_reload_device,
 	.get_uid = dasd_eckd_get_uid,
 	.kick_validate = dasd_eckd_kick_validate_server,
+	.check_attention = dasd_eckd_check_attention,
 };
 
 static int __init
diff --git a/drivers/s390/block/dasd_eckd.h b/drivers/s390/block/dasd_eckd.h
index 2555e494591f..ddab7df36e25 100644
--- a/drivers/s390/block/dasd_eckd.h
+++ b/drivers/s390/block/dasd_eckd.h
@@ -51,8 +51,35 @@
 /*
  * Perform Subsystem Function / Sub-Orders
  */
-#define PSF_ORDER_PRSSD 0x18
-#define PSF_ORDER_SSC	0x1D
+#define PSF_ORDER_PRSSD			 0x18
+#define PSF_ORDER_CUIR_RESPONSE		 0x1A
+#define PSF_ORDER_SSC			 0x1D
+
+/*
+ * CUIR response condition codes
+ */
+#define PSF_CUIR_INVALID		 0x00
+#define PSF_CUIR_COMPLETED		 0x01
+#define PSF_CUIR_NOT_SUPPORTED		 0x02
+#define PSF_CUIR_ERROR_IN_REQ		 0x03
+#define PSF_CUIR_DENIED			 0x04
+#define PSF_CUIR_LAST_PATH		 0x05
+#define PSF_CUIR_DEVICE_ONLINE		 0x06
+#define PSF_CUIR_VARY_FAILURE		 0x07
+#define PSF_CUIR_SOFTWARE_FAILURE	 0x08
+#define PSF_CUIR_NOT_RECOGNIZED		 0x09
+
+/*
+ * CUIR codes
+ */
+#define CUIR_QUIESCE			 0x01
+#define CUIR_RESUME			 0x02
+
+/*
+ * attention message definitions
+ */
+#define ATTENTION_LENGTH_CUIR		 0x0e
+#define ATTENTION_FORMAT_CUIR		 0x01
 
 /*
  * Size that is reportet for large volumes in the old 16-bit no_cyl field
@@ -342,6 +369,38 @@ struct dasd_rssd_features {
 	char feature[256];
 } __attribute__((packed));
 
+struct dasd_rssd_messages {
+	__u16 length;
+	__u8 format;
+	__u8 code;
+	__u32 message_id;
+	__u8 flags;
+	char messages[4087];
+} __packed;
+
+struct dasd_cuir_message {
+	__u16 length;
+	__u8 format;
+	__u8 code;
+	__u32 message_id;
+	__u8 flags;
+	__u8 neq_map[3];
+	__u8 ned_map;
+	__u8 record_selector;
+} __packed;
+
+struct dasd_psf_cuir_response {
+	__u8 order;
+	__u8 flags;
+	__u8 cc;
+	__u8 chpid;
+	__u16 device_nr;
+	__u16 reserved;
+	__u32 message_id;
+	__u64 system_id;
+	__u8 cssid;
+	__u8 ssid;
+} __packed;
 
 /*
  * Perform Subsystem Function - Prepare for Read Subsystem Data
diff --git a/drivers/s390/block/dasd_int.h b/drivers/s390/block/dasd_int.h
index c20170166909..8b5d4100abf7 100644
--- a/drivers/s390/block/dasd_int.h
+++ b/drivers/s390/block/dasd_int.h
@@ -357,6 +357,7 @@ struct dasd_discipline {
 
 	int (*get_uid) (struct dasd_device *, struct dasd_uid *);
 	void (*kick_validate) (struct dasd_device *);
+	int (*check_attention)(struct dasd_device *, __u8);
 };
 
 extern struct dasd_discipline *dasd_diag_discipline_pointer;
@@ -382,6 +383,10 @@ struct dasd_path {
 	__u8 tbvpm;
 	__u8 ppm;
 	__u8 npm;
+	/* paths that are not used because of a special condition */
+	__u8 cablepm; /* miss-cabled */
+	__u8 hpfpm;   /* the HPF requirements of the other paths are not met */
+	__u8 cuirpm;  /* CUIR varied offline */
 };
 
 struct dasd_profile_info {
@@ -501,7 +506,10 @@ struct dasd_block {
 	struct dasd_profile profile;
 };
 
-
+struct dasd_attention_data {
+	struct dasd_device *device;
+	__u8 lpum;
+};
 
 /* reasons why device (ccw_device_start) was stopped */
 #define DASD_STOPPED_NOT_ACC 1         /* not accessible */
diff --git a/drivers/s390/block/scm_blk.c b/drivers/s390/block/scm_blk.c
index 76bed1743db1..56046ab39629 100644
--- a/drivers/s390/block/scm_blk.c
+++ b/drivers/s390/block/scm_blk.c
@@ -386,6 +386,7 @@ int scm_blk_dev_setup(struct scm_blk_dev *bdev, struct scm_device *scmdev)
 	blk_queue_max_hw_sectors(rq, nr_max_blk << 3); /* 8 * 512 = blk_size */
 	blk_queue_max_segments(rq, nr_max_blk);
 	queue_flag_set_unlocked(QUEUE_FLAG_NONROT, rq);
+	queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, rq);
 	scm_blk_dev_cluster_setup(bdev);
 
 	bdev->gendisk = alloc_disk(SCM_NR_PARTS);
diff --git a/drivers/s390/block/xpram.c b/drivers/s390/block/xpram.c
index 6969d39f1e2e..9e0de9c9a6fc 100644
--- a/drivers/s390/block/xpram.c
+++ b/drivers/s390/block/xpram.c
@@ -346,6 +346,7 @@ static int __init xpram_setup_blkdev(void)
 			goto out;
 		}
 		queue_flag_set_unlocked(QUEUE_FLAG_NONROT, xpram_queues[i]);
+		queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, xpram_queues[i]);
 		blk_queue_make_request(xpram_queues[i], xpram_make_request);
 		blk_queue_logical_block_size(xpram_queues[i], 4096);
 	}
diff --git a/drivers/s390/char/Kconfig b/drivers/s390/char/Kconfig
index 71bf959732fe..db2cb1f8a1b5 100644
--- a/drivers/s390/char/Kconfig
+++ b/drivers/s390/char/Kconfig
@@ -102,6 +102,19 @@ config SCLP_ASYNC
 	  want for inform other people about your kernel panics,
 	  need this feature and intend to run your kernel in LPAR.
 
+config HMC_DRV
+	def_tristate m
+	prompt "Support for file transfers from HMC drive CD/DVD-ROM"
+	depends on S390 && 64BIT
+	select CRC16
+	help
+	  This option enables support for file transfers from a Hardware
+	  Management Console (HMC) drive CD/DVD-ROM. It is available as a
+	  module, called 'hmcdrv', and also as kernel built-in. There is one
+	  optional parameter for this module: cachesize=N, which modifies the
+	  transfer cache size from it's default value 0.5MB to N bytes. If N
+	  is zero, then no caching is performed.
+
 config S390_TAPE
 	def_tristate m
 	prompt "S/390 tape device support"
diff --git a/drivers/s390/char/Makefile b/drivers/s390/char/Makefile
index 78b6ace7edcb..6fa9364d1c07 100644
--- a/drivers/s390/char/Makefile
+++ b/drivers/s390/char/Makefile
@@ -33,3 +33,6 @@ obj-$(CONFIG_S390_VMUR) += vmur.o
 
 zcore_mod-objs := sclp_sdias.o zcore.o
 obj-$(CONFIG_CRASH_DUMP) += zcore_mod.o
+
+hmcdrv-objs := hmcdrv_mod.o hmcdrv_dev.o hmcdrv_ftp.o hmcdrv_cache.o diag_ftp.o sclp_ftp.o
+obj-$(CONFIG_HMC_DRV) += hmcdrv.o
diff --git a/drivers/s390/char/con3215.c b/drivers/s390/char/con3215.c
index a6d47e5eee9e..c43aca69fb30 100644
--- a/drivers/s390/char/con3215.c
+++ b/drivers/s390/char/con3215.c
@@ -1035,12 +1035,26 @@ static int tty3215_write(struct tty_struct * tty,
 			 const unsigned char *buf, int count)
 {
 	struct raw3215_info *raw;
+	int i, written;
 
 	if (!tty)
 		return 0;
 	raw = (struct raw3215_info *) tty->driver_data;
-	raw3215_write(raw, buf, count);
-	return count;
+	written = count;
+	while (count > 0) {
+		for (i = 0; i < count; i++)
+			if (buf[i] == '\t' || buf[i] == '\n')
+				break;
+		raw3215_write(raw, buf, i);
+		count -= i;
+		buf += i;
+		if (count > 0) {
+			raw3215_putchar(raw, *buf);
+			count--;
+			buf++;
+		}
+	}
+	return written;
 }
 
 /*
@@ -1188,7 +1202,7 @@ static int __init tty3215_init(void)
 	driver->subtype = SYSTEM_TYPE_TTY;
 	driver->init_termios = tty_std_termios;
 	driver->init_termios.c_iflag = IGNBRK | IGNPAR;
-	driver->init_termios.c_oflag = ONLCR | XTABS;
+	driver->init_termios.c_oflag = ONLCR;
 	driver->init_termios.c_lflag = ISIG;
 	driver->flags = TTY_DRIVER_REAL_RAW;
 	tty_set_operations(driver, &tty3215_ops);
diff --git a/drivers/s390/char/diag_ftp.c b/drivers/s390/char/diag_ftp.c
new file mode 100644
index 000000000000..93889632fdf9
--- /dev/null
+++ b/drivers/s390/char/diag_ftp.c
@@ -0,0 +1,237 @@
+/*
+ *    DIAGNOSE X'2C4' instruction based HMC FTP services, useable on z/VM
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ *
+ */
+
+#define KMSG_COMPONENT "hmcdrv"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/irq.h>
+#include <linux/wait.h>
+#include <linux/string.h>
+#include <asm/ctl_reg.h>
+
+#include "hmcdrv_ftp.h"
+#include "diag_ftp.h"
+
+/* DIAGNOSE X'2C4' return codes in Ry */
+#define DIAG_FTP_RET_OK	0 /* HMC FTP started successfully */
+#define DIAG_FTP_RET_EBUSY	4 /* HMC FTP service currently busy */
+#define DIAG_FTP_RET_EIO	8 /* HMC FTP service I/O error */
+/* and an artificial extension */
+#define DIAG_FTP_RET_EPERM	2 /* HMC FTP service privilege error */
+
+/* FTP service status codes (after INTR at guest real location 133) */
+#define DIAG_FTP_STAT_OK	0U /* request completed successfully */
+#define DIAG_FTP_STAT_PGCC	4U /* program check condition */
+#define DIAG_FTP_STAT_PGIOE	8U /* paging I/O error */
+#define DIAG_FTP_STAT_TIMEOUT	12U /* timeout */
+#define DIAG_FTP_STAT_EBASE	16U /* base of error codes from SCLP */
+#define DIAG_FTP_STAT_LDFAIL	(DIAG_FTP_STAT_EBASE + 1U) /* failed */
+#define DIAG_FTP_STAT_LDNPERM	(DIAG_FTP_STAT_EBASE + 2U) /* not allowed */
+#define DIAG_FTP_STAT_LDRUNS	(DIAG_FTP_STAT_EBASE + 3U) /* runs */
+#define DIAG_FTP_STAT_LDNRUNS	(DIAG_FTP_STAT_EBASE + 4U) /* not runs */
+
+/**
+ * struct diag_ftp_ldfpl - load file FTP parameter list (LDFPL)
+ * @bufaddr: real buffer address (at 4k boundary)
+ * @buflen: length of buffer
+ * @offset: dir/file offset
+ * @intparm: interruption parameter (unused)
+ * @transferred: bytes transferred
+ * @fsize: file size, filled on GET
+ * @failaddr: failing address
+ * @spare: padding
+ * @fident: file name - ASCII
+ */
+struct diag_ftp_ldfpl {
+	u64 bufaddr;
+	u64 buflen;
+	u64 offset;
+	u64 intparm;
+	u64 transferred;
+	u64 fsize;
+	u64 failaddr;
+	u64 spare;
+	u8 fident[HMCDRV_FTP_FIDENT_MAX];
+} __packed;
+
+static DECLARE_COMPLETION(diag_ftp_rx_complete);
+static int diag_ftp_subcode;
+
+/**
+ * diag_ftp_handler() - FTP services IRQ handler
+ * @extirq: external interrupt (sub-) code
+ * @param32: 32-bit interruption parameter from &struct diag_ftp_ldfpl
+ * @param64: unused (for 64-bit interrupt parameters)
+ */
+static void diag_ftp_handler(struct ext_code extirq,
+			     unsigned int param32,
+			     unsigned long param64)
+{
+	if ((extirq.subcode >> 8) != 8)
+		return; /* not a FTP services sub-code */
+
+	inc_irq_stat(IRQEXT_FTP);
+	diag_ftp_subcode = extirq.subcode & 0xffU;
+	complete(&diag_ftp_rx_complete);
+}
+
+/**
+ * diag_ftp_2c4() - DIAGNOSE X'2C4' service call
+ * @fpl: pointer to prepared LDFPL
+ * @cmd: FTP command to be executed
+ *
+ * Performs a DIAGNOSE X'2C4' call with (input/output) FTP parameter list
+ * @fpl and FTP function code @cmd. In case of an error the function does
+ * nothing and returns an (negative) error code.
+ *
+ * Notes:
+ * 1. This function only initiates a transfer, so the caller must wait
+ *    for completion (asynchronous execution).
+ * 2. The FTP parameter list @fpl must be aligned to a double-word boundary.
+ * 3. fpl->bufaddr must be a real address, 4k aligned
+ */
+static int diag_ftp_2c4(struct diag_ftp_ldfpl *fpl,
+			enum hmcdrv_ftp_cmdid cmd)
+{
+	int rc;
+
+	asm volatile(
+		"	diag	%[addr],%[cmd],0x2c4\n"
+		"0:	j	2f\n"
+		"1:	la	%[rc],%[err]\n"
+		"2:\n"
+		EX_TABLE(0b, 1b)
+		: [rc] "=d" (rc), "+m" (*fpl)
+		: [cmd] "0" (cmd), [addr] "d" (virt_to_phys(fpl)),
+		  [err] "i" (DIAG_FTP_RET_EPERM)
+		: "cc");
+
+	switch (rc) {
+	case DIAG_FTP_RET_OK:
+		return 0;
+	case DIAG_FTP_RET_EBUSY:
+		return -EBUSY;
+	case DIAG_FTP_RET_EPERM:
+		return -EPERM;
+	case DIAG_FTP_RET_EIO:
+	default:
+		return -EIO;
+	}
+}
+
+/**
+ * diag_ftp_cmd() - executes a DIAG X'2C4' FTP command, targeting a HMC
+ * @ftp: pointer to FTP command specification
+ * @fsize: return of file size (or NULL if undesirable)
+ *
+ * Attention: Notice that this function is not reentrant - so the caller
+ * must ensure locking.
+ *
+ * Return: number of bytes read/written or a (negative) error code
+ */
+ssize_t diag_ftp_cmd(const struct hmcdrv_ftp_cmdspec *ftp, size_t *fsize)
+{
+	struct diag_ftp_ldfpl *ldfpl;
+	ssize_t len;
+#ifdef DEBUG
+	unsigned long start_jiffies;
+
+	pr_debug("starting DIAG X'2C4' on '%s', requesting %zd bytes\n",
+		 ftp->fname, ftp->len);
+	start_jiffies = jiffies;
+#endif
+	init_completion(&diag_ftp_rx_complete);
+
+	ldfpl = (void *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
+	if (!ldfpl) {
+		len = -ENOMEM;
+		goto out;
+	}
+
+	len = strlcpy(ldfpl->fident, ftp->fname, sizeof(ldfpl->fident));
+	if (len >= HMCDRV_FTP_FIDENT_MAX) {
+		len = -EINVAL;
+		goto out_free;
+	}
+
+	ldfpl->transferred = 0;
+	ldfpl->fsize = 0;
+	ldfpl->offset = ftp->ofs;
+	ldfpl->buflen = ftp->len;
+	ldfpl->bufaddr = virt_to_phys(ftp->buf);
+
+	len = diag_ftp_2c4(ldfpl, ftp->id);
+	if (len)
+		goto out_free;
+
+	/*
+	 * There is no way to cancel the running diag X'2C4', the code
+	 * needs to wait unconditionally until the transfer is complete.
+	 */
+	wait_for_completion(&diag_ftp_rx_complete);
+
+#ifdef DEBUG
+	pr_debug("completed DIAG X'2C4' after %lu ms\n",
+		 (jiffies - start_jiffies) * 1000 / HZ);
+	pr_debug("status of DIAG X'2C4' is %u, with %lld/%lld bytes\n",
+		 diag_ftp_subcode, ldfpl->transferred, ldfpl->fsize);
+#endif
+
+	switch (diag_ftp_subcode) {
+	case DIAG_FTP_STAT_OK: /* success */
+		len = ldfpl->transferred;
+		if (fsize)
+			*fsize = ldfpl->fsize;
+		break;
+	case DIAG_FTP_STAT_LDNPERM:
+		len = -EPERM;
+		break;
+	case DIAG_FTP_STAT_LDRUNS:
+		len = -EBUSY;
+		break;
+	case DIAG_FTP_STAT_LDFAIL:
+		len = -ENOENT; /* no such file or media */
+		break;
+	default:
+		len = -EIO;
+		break;
+	}
+
+out_free:
+	free_page((unsigned long) ldfpl);
+out:
+	return len;
+}
+
+/**
+ * diag_ftp_startup() - startup of FTP services, when running on z/VM
+ *
+ * Return: 0 on success, else an (negative) error code
+ */
+int diag_ftp_startup(void)
+{
+	int rc;
+
+	rc = register_external_irq(EXT_IRQ_CP_SERVICE, diag_ftp_handler);
+	if (rc)
+		return rc;
+
+	ctl_set_bit(0, 63 - 22);
+	return 0;
+}
+
+/**
+ * diag_ftp_shutdown() - shutdown of FTP services, when running on z/VM
+ */
+void diag_ftp_shutdown(void)
+{
+	ctl_clear_bit(0, 63 - 22);
+	unregister_external_irq(EXT_IRQ_CP_SERVICE, diag_ftp_handler);
+}
diff --git a/drivers/s390/char/diag_ftp.h b/drivers/s390/char/diag_ftp.h
new file mode 100644
index 000000000000..3abd2614053a
--- /dev/null
+++ b/drivers/s390/char/diag_ftp.h
@@ -0,0 +1,21 @@
+/*
+ *    DIAGNOSE X'2C4' instruction based SE/HMC FTP Services, useable on z/VM
+ *
+ *    Notice that all functions exported here are not reentrant.
+ *    So usage should be exclusive, ensured by the caller (e.g. using a
+ *    mutex).
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#ifndef __DIAG_FTP_H__
+#define __DIAG_FTP_H__
+
+#include "hmcdrv_ftp.h"
+
+int diag_ftp_startup(void);
+void diag_ftp_shutdown(void);
+ssize_t diag_ftp_cmd(const struct hmcdrv_ftp_cmdspec *ftp, size_t *fsize);
+
+#endif	 /* __DIAG_FTP_H__ */
diff --git a/drivers/s390/char/hmcdrv_cache.c b/drivers/s390/char/hmcdrv_cache.c
new file mode 100644
index 000000000000..4cda5ada143a
--- /dev/null
+++ b/drivers/s390/char/hmcdrv_cache.c
@@ -0,0 +1,252 @@
+/*
+ *    SE/HMC Drive (Read) Cache Functions
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ *
+ */
+
+#define KMSG_COMPONENT "hmcdrv"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/jiffies.h>
+
+#include "hmcdrv_ftp.h"
+#include "hmcdrv_cache.h"
+
+#define HMCDRV_CACHE_TIMEOUT		30 /* aging timeout in seconds */
+
+/**
+ * struct hmcdrv_cache_entry - file cache (only used on read/dir)
+ * @id: FTP command ID
+ * @content: kernel-space buffer, 4k aligned
+ * @len: size of @content cache (0 if caching disabled)
+ * @ofs: start of content within file (-1 if no cached content)
+ * @fname: file name
+ * @fsize: file size
+ * @timeout: cache timeout in jiffies
+ *
+ * Notice that the first three members (id, fname, fsize) are cached on all
+ * read/dir requests. But content is cached only under some preconditions.
+ * Uncached content is signalled by a negative value of @ofs.
+ */
+struct hmcdrv_cache_entry {
+	enum hmcdrv_ftp_cmdid id;
+	char fname[HMCDRV_FTP_FIDENT_MAX];
+	size_t fsize;
+	loff_t ofs;
+	unsigned long timeout;
+	void *content;
+	size_t len;
+};
+
+static int hmcdrv_cache_order; /* cache allocated page order */
+
+static struct hmcdrv_cache_entry hmcdrv_cache_file = {
+	.fsize = SIZE_MAX,
+	.ofs = -1,
+	.len = 0,
+	.fname = {'\0'}
+};
+
+/**
+ * hmcdrv_cache_get() - looks for file data/content in read cache
+ * @ftp: pointer to FTP command specification
+ *
+ * Return: number of bytes read from cache or a negative number if nothing
+ * in content cache (for the file/cmd specified in @ftp)
+ */
+static ssize_t hmcdrv_cache_get(const struct hmcdrv_ftp_cmdspec *ftp)
+{
+	loff_t pos; /* position in cache (signed) */
+	ssize_t len;
+
+	if ((ftp->id != hmcdrv_cache_file.id) ||
+	    strcmp(hmcdrv_cache_file.fname, ftp->fname))
+		return -1;
+
+	if (ftp->ofs >= hmcdrv_cache_file.fsize) /* EOF ? */
+		return 0;
+
+	if ((hmcdrv_cache_file.ofs < 0) || /* has content? */
+	    time_after(jiffies, hmcdrv_cache_file.timeout))
+		return -1;
+
+	/* there seems to be cached content - calculate the maximum number
+	 * of bytes that can be returned (regarding file size and offset)
+	 */
+	len = hmcdrv_cache_file.fsize - ftp->ofs;
+
+	if (len > ftp->len)
+		len = ftp->len;
+
+	/* check if the requested chunk falls into our cache (which starts
+	 * at offset 'hmcdrv_cache_file.ofs' in the file of interest)
+	 */
+	pos = ftp->ofs - hmcdrv_cache_file.ofs;
+
+	if ((pos >= 0) &&
+	    ((pos + len) <= hmcdrv_cache_file.len)) {
+
+		memcpy(ftp->buf,
+		       hmcdrv_cache_file.content + pos,
+		       len);
+		pr_debug("using cached content of '%s', returning %zd/%zd bytes\n",
+			 hmcdrv_cache_file.fname, len,
+			 hmcdrv_cache_file.fsize);
+
+		return len;
+	}
+
+	return -1;
+}
+
+/**
+ * hmcdrv_cache_do() - do a HMC drive CD/DVD transfer with cache update
+ * @ftp: pointer to FTP command specification
+ * @func: FTP transfer function to be used
+ *
+ * Return: number of bytes read/written or a (negative) error code
+ */
+static ssize_t hmcdrv_cache_do(const struct hmcdrv_ftp_cmdspec *ftp,
+			       hmcdrv_cache_ftpfunc func)
+{
+	ssize_t len;
+
+	/* only cache content if the read/dir cache really exists
+	 * (hmcdrv_cache_file.len > 0), is large enough to handle the
+	 * request (hmcdrv_cache_file.len >= ftp->len) and there is a need
+	 * to do so (ftp->len > 0)
+	 */
+	if ((ftp->len > 0) && (hmcdrv_cache_file.len >= ftp->len)) {
+
+		/* because the cache is not located at ftp->buf, we have to
+		 * assemble a new HMC drive FTP cmd specification (pointing
+		 * to our cache, and using the increased size)
+		 */
+		struct hmcdrv_ftp_cmdspec cftp = *ftp; /* make a copy */
+		cftp.buf = hmcdrv_cache_file.content;  /* and update */
+		cftp.len = hmcdrv_cache_file.len;      /* buffer data */
+
+		len = func(&cftp, &hmcdrv_cache_file.fsize); /* now do */
+
+		if (len > 0) {
+			pr_debug("caching %zd bytes content for '%s'\n",
+				 len, ftp->fname);
+
+			if (len > ftp->len)
+				len = ftp->len;
+
+			hmcdrv_cache_file.ofs = ftp->ofs;
+			hmcdrv_cache_file.timeout = jiffies +
+				HMCDRV_CACHE_TIMEOUT * HZ;
+			memcpy(ftp->buf, hmcdrv_cache_file.content, len);
+		}
+	} else {
+		len = func(ftp, &hmcdrv_cache_file.fsize);
+		hmcdrv_cache_file.ofs = -1; /* invalidate content */
+	}
+
+	if (len > 0) {
+		/* cache some file info (FTP command, file name and file
+		 * size) unconditionally
+		 */
+		strlcpy(hmcdrv_cache_file.fname, ftp->fname,
+			HMCDRV_FTP_FIDENT_MAX);
+		hmcdrv_cache_file.id = ftp->id;
+		pr_debug("caching cmd %d, file size %zu for '%s'\n",
+			 ftp->id, hmcdrv_cache_file.fsize, ftp->fname);
+	}
+
+	return len;
+}
+
+/**
+ * hmcdrv_cache_cmd() - perform a cached HMC drive CD/DVD transfer
+ * @ftp: pointer to FTP command specification
+ * @func: FTP transfer function to be used
+ *
+ * Attention: Notice that this function is not reentrant - so the caller
+ * must ensure exclusive execution.
+ *
+ * Return: number of bytes read/written or a (negative) error code
+ */
+ssize_t hmcdrv_cache_cmd(const struct hmcdrv_ftp_cmdspec *ftp,
+			 hmcdrv_cache_ftpfunc func)
+{
+	ssize_t len;
+
+	if ((ftp->id == HMCDRV_FTP_DIR) || /* read cache */
+	    (ftp->id == HMCDRV_FTP_NLIST) ||
+	    (ftp->id == HMCDRV_FTP_GET)) {
+
+		len = hmcdrv_cache_get(ftp);
+
+		if (len >= 0) /* got it from cache ? */
+			return len; /* yes */
+
+		len = hmcdrv_cache_do(ftp, func);
+
+		if (len >= 0)
+			return len;
+
+	} else {
+		len = func(ftp, NULL); /* simply do original command */
+	}
+
+	/* invalidate the (read) cache in case there was a write operation
+	 * or an error on read/dir
+	 */
+	hmcdrv_cache_file.id = HMCDRV_FTP_NOOP;
+	hmcdrv_cache_file.fsize = LLONG_MAX;
+	hmcdrv_cache_file.ofs = -1;
+
+	return len;
+}
+
+/**
+ * hmcdrv_cache_startup() - startup of HMC drive cache
+ * @cachesize: cache size
+ *
+ * Return: 0 on success, else a (negative) error code
+ */
+int hmcdrv_cache_startup(size_t cachesize)
+{
+	if (cachesize > 0) { /* perform caching ? */
+		hmcdrv_cache_order = get_order(cachesize);
+		hmcdrv_cache_file.content =
+			(void *) __get_free_pages(GFP_KERNEL | GFP_DMA,
+						  hmcdrv_cache_order);
+
+		if (!hmcdrv_cache_file.content) {
+			pr_err("Allocating the requested cache size of %zu bytes failed\n",
+			       cachesize);
+			return -ENOMEM;
+		}
+
+		pr_debug("content cache enabled, size is %zu bytes\n",
+			 cachesize);
+	}
+
+	hmcdrv_cache_file.len = cachesize;
+	return 0;
+}
+
+/**
+ * hmcdrv_cache_shutdown() - shutdown of HMC drive cache
+ */
+void hmcdrv_cache_shutdown(void)
+{
+	if (hmcdrv_cache_file.content) {
+		free_pages((unsigned long) hmcdrv_cache_file.content,
+			   hmcdrv_cache_order);
+		hmcdrv_cache_file.content = NULL;
+	}
+
+	hmcdrv_cache_file.id = HMCDRV_FTP_NOOP;
+	hmcdrv_cache_file.fsize = LLONG_MAX;
+	hmcdrv_cache_file.ofs = -1;
+	hmcdrv_cache_file.len = 0; /* no cache */
+}
diff --git a/drivers/s390/char/hmcdrv_cache.h b/drivers/s390/char/hmcdrv_cache.h
new file mode 100644
index 000000000000..a14b57526781
--- /dev/null
+++ b/drivers/s390/char/hmcdrv_cache.h
@@ -0,0 +1,24 @@
+/*
+ *    SE/HMC Drive (Read) Cache Functions
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#ifndef __HMCDRV_CACHE_H__
+#define __HMCDRV_CACHE_H__
+
+#include <linux/mmzone.h>
+#include "hmcdrv_ftp.h"
+
+#define HMCDRV_CACHE_SIZE_DFLT	(MAX_ORDER_NR_PAGES * PAGE_SIZE / 2UL)
+
+typedef ssize_t (*hmcdrv_cache_ftpfunc)(const struct hmcdrv_ftp_cmdspec *ftp,
+					size_t *fsize);
+
+ssize_t hmcdrv_cache_cmd(const struct hmcdrv_ftp_cmdspec *ftp,
+			 hmcdrv_cache_ftpfunc func);
+int hmcdrv_cache_startup(size_t cachesize);
+void hmcdrv_cache_shutdown(void);
+
+#endif	 /* __HMCDRV_CACHE_H__ */
diff --git a/drivers/s390/char/hmcdrv_dev.c b/drivers/s390/char/hmcdrv_dev.c
new file mode 100644
index 000000000000..0c5176179c17
--- /dev/null
+++ b/drivers/s390/char/hmcdrv_dev.c
@@ -0,0 +1,370 @@
+/*
+ *    HMC Drive CD/DVD Device
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ *
+ *    This file provides a Linux "misc" character device for access to an
+ *    assigned HMC drive CD/DVD-ROM. It works as follows: First create the
+ *    device by calling hmcdrv_dev_init(). After open() a lseek(fd, 0,
+ *    SEEK_END) indicates that a new FTP command follows (not needed on the
+ *    first command after open). Then write() the FTP command ASCII string
+ *    to it, e.g. "dir /" or "nls <directory>" or "get <filename>". At the
+ *    end read() the response.
+ */
+
+#define KMSG_COMPONENT "hmcdrv"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/fs.h>
+#include <linux/cdev.h>
+#include <linux/miscdevice.h>
+#include <linux/device.h>
+#include <linux/capability.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+
+#include "hmcdrv_dev.h"
+#include "hmcdrv_ftp.h"
+
+/* If the following macro is defined, then the HMC device creates it's own
+ * separated device class (and dynamically assigns a major number). If not
+ * defined then the HMC device is assigned to the "misc" class devices.
+ *
+#define HMCDRV_DEV_CLASS "hmcftp"
+ */
+
+#define HMCDRV_DEV_NAME  "hmcdrv"
+#define HMCDRV_DEV_BUSY_DELAY	 500 /* delay between -EBUSY trials in ms */
+#define HMCDRV_DEV_BUSY_RETRIES  3   /* number of retries on -EBUSY */
+
+struct hmcdrv_dev_node {
+
+#ifdef HMCDRV_DEV_CLASS
+	struct cdev dev; /* character device structure */
+	umode_t mode;	 /* mode of device node (unused, zero) */
+#else
+	struct miscdevice dev; /* "misc" device structure */
+#endif
+
+};
+
+static int hmcdrv_dev_open(struct inode *inode, struct file *fp);
+static int hmcdrv_dev_release(struct inode *inode, struct file *fp);
+static loff_t hmcdrv_dev_seek(struct file *fp, loff_t pos, int whence);
+static ssize_t hmcdrv_dev_read(struct file *fp, char __user *ubuf,
+			       size_t len, loff_t *pos);
+static ssize_t hmcdrv_dev_write(struct file *fp, const char __user *ubuf,
+				size_t len, loff_t *pos);
+static ssize_t hmcdrv_dev_transfer(char __kernel *cmd, loff_t offset,
+				   char __user *buf, size_t len);
+
+/*
+ * device operations
+ */
+static const struct file_operations hmcdrv_dev_fops = {
+	.open = hmcdrv_dev_open,
+	.llseek = hmcdrv_dev_seek,
+	.release = hmcdrv_dev_release,
+	.read = hmcdrv_dev_read,
+	.write = hmcdrv_dev_write,
+};
+
+static struct hmcdrv_dev_node hmcdrv_dev; /* HMC device struct (static) */
+
+#ifdef HMCDRV_DEV_CLASS
+
+static struct class *hmcdrv_dev_class; /* device class pointer */
+static dev_t hmcdrv_dev_no; /* device number (major/minor) */
+
+/**
+ * hmcdrv_dev_name() - provides a naming hint for a device node in /dev
+ * @dev: device for which the naming/mode hint is
+ * @mode: file mode for device node created in /dev
+ *
+ * See: devtmpfs.c, function devtmpfs_create_node()
+ *
+ * Return: recommended device file name in /dev
+ */
+static char *hmcdrv_dev_name(struct device *dev, umode_t *mode)
+{
+	char *nodename = NULL;
+	const char *devname = dev_name(dev); /* kernel device name */
+
+	if (devname)
+		nodename = kasprintf(GFP_KERNEL, "%s", devname);
+
+	/* on device destroy (rmmod) the mode pointer may be NULL
+	 */
+	if (mode)
+		*mode = hmcdrv_dev.mode;
+
+	return nodename;
+}
+
+#endif	/* HMCDRV_DEV_CLASS */
+
+/*
+ * open()
+ */
+static int hmcdrv_dev_open(struct inode *inode, struct file *fp)
+{
+	int rc;
+
+	/* check for non-blocking access, which is really unsupported
+	 */
+	if (fp->f_flags & O_NONBLOCK)
+		return -EINVAL;
+
+	/* Because it makes no sense to open this device read-only (then a
+	 * FTP command cannot be emitted), we respond with an error.
+	 */
+	if ((fp->f_flags & O_ACCMODE) == O_RDONLY)
+		return -EINVAL;
+
+	/* prevent unloading this module as long as anyone holds the
+	 * device file open - so increment the reference count here
+	 */
+	if (!try_module_get(THIS_MODULE))
+		return -ENODEV;
+
+	fp->private_data = NULL; /* no command yet */
+	rc = hmcdrv_ftp_startup();
+	if (rc)
+		module_put(THIS_MODULE);
+
+	pr_debug("open file '/dev/%s' with return code %d\n",
+		 fp->f_dentry->d_name.name, rc);
+	return rc;
+}
+
+/*
+ * release()
+ */
+static int hmcdrv_dev_release(struct inode *inode, struct file *fp)
+{
+	pr_debug("closing file '/dev/%s'\n", fp->f_dentry->d_name.name);
+	kfree(fp->private_data);
+	fp->private_data = NULL;
+	hmcdrv_ftp_shutdown();
+	module_put(THIS_MODULE);
+	return 0;
+}
+
+/*
+ * lseek()
+ */
+static loff_t hmcdrv_dev_seek(struct file *fp, loff_t pos, int whence)
+{
+	switch (whence) {
+	case SEEK_CUR: /* relative to current file position */
+		pos += fp->f_pos; /* new position stored in 'pos' */
+		break;
+
+	case SEEK_SET: /* absolute (relative to beginning of file) */
+		break; /* SEEK_SET */
+
+		/* We use SEEK_END as a special indicator for a SEEK_SET
+		 * (set absolute position), combined with a FTP command
+		 * clear.
+		 */
+	case SEEK_END:
+		if (fp->private_data) {
+			kfree(fp->private_data);
+			fp->private_data = NULL;
+		}
+
+		break; /* SEEK_END */
+
+	default: /* SEEK_DATA, SEEK_HOLE: unsupported */
+		return -EINVAL;
+	}
+
+	if (pos < 0)
+		return -EINVAL;
+
+	if (fp->f_pos != pos)
+		++fp->f_version;
+
+	fp->f_pos = pos;
+	return pos;
+}
+
+/*
+ * transfer (helper function)
+ */
+static ssize_t hmcdrv_dev_transfer(char __kernel *cmd, loff_t offset,
+				   char __user *buf, size_t len)
+{
+	ssize_t retlen;
+	unsigned trials = HMCDRV_DEV_BUSY_RETRIES;
+
+	do {
+		retlen = hmcdrv_ftp_cmd(cmd, offset, buf, len);
+
+		if (retlen != -EBUSY)
+			break;
+
+		msleep(HMCDRV_DEV_BUSY_DELAY);
+
+	} while (--trials > 0);
+
+	return retlen;
+}
+
+/*
+ * read()
+ */
+static ssize_t hmcdrv_dev_read(struct file *fp, char __user *ubuf,
+			       size_t len, loff_t *pos)
+{
+	ssize_t retlen;
+
+	if (((fp->f_flags & O_ACCMODE) == O_WRONLY) ||
+	    (fp->private_data == NULL)) { /* no FTP cmd defined ? */
+		return -EBADF;
+	}
+
+	retlen = hmcdrv_dev_transfer((char *) fp->private_data,
+				     *pos, ubuf, len);
+
+	pr_debug("read from file '/dev/%s' at %lld returns %zd/%zu\n",
+		 fp->f_dentry->d_name.name, (long long) *pos, retlen, len);
+
+	if (retlen > 0)
+		*pos += retlen;
+
+	return retlen;
+}
+
+/*
+ * write()
+ */
+static ssize_t hmcdrv_dev_write(struct file *fp, const char __user *ubuf,
+				size_t len, loff_t *pos)
+{
+	ssize_t retlen;
+
+	pr_debug("writing file '/dev/%s' at pos. %lld with length %zd\n",
+		 fp->f_dentry->d_name.name, (long long) *pos, len);
+
+	if (!fp->private_data) { /* first expect a cmd write */
+		fp->private_data = kmalloc(len + 1, GFP_KERNEL);
+
+		if (!fp->private_data)
+			return -ENOMEM;
+
+		if (!copy_from_user(fp->private_data, ubuf, len)) {
+			((char *)fp->private_data)[len] = '\0';
+			return len;
+		}
+
+		kfree(fp->private_data);
+		fp->private_data = NULL;
+		return -EFAULT;
+	}
+
+	retlen = hmcdrv_dev_transfer((char *) fp->private_data,
+				     *pos, (char __user *) ubuf, len);
+	if (retlen > 0)
+		*pos += retlen;
+
+	pr_debug("write to file '/dev/%s' returned %zd\n",
+		 fp->f_dentry->d_name.name, retlen);
+
+	return retlen;
+}
+
+/**
+ * hmcdrv_dev_init() - creates a HMC drive CD/DVD device
+ *
+ * This function creates a HMC drive CD/DVD kernel device and an associated
+ * device under /dev, using a dynamically allocated major number.
+ *
+ * Return: 0 on success, else an error code.
+ */
+int hmcdrv_dev_init(void)
+{
+	int rc;
+
+#ifdef HMCDRV_DEV_CLASS
+	struct device *dev;
+
+	rc = alloc_chrdev_region(&hmcdrv_dev_no, 0, 1, HMCDRV_DEV_NAME);
+
+	if (rc)
+		goto out_err;
+
+	cdev_init(&hmcdrv_dev.dev, &hmcdrv_dev_fops);
+	hmcdrv_dev.dev.owner = THIS_MODULE;
+	rc = cdev_add(&hmcdrv_dev.dev, hmcdrv_dev_no, 1);
+
+	if (rc)
+		goto out_unreg;
+
+	/* At this point the character device exists in the kernel (see
+	 * /proc/devices), but not under /dev nor /sys/devices/virtual. So
+	 * we have to create an associated class (see /sys/class).
+	 */
+	hmcdrv_dev_class = class_create(THIS_MODULE, HMCDRV_DEV_CLASS);
+
+	if (IS_ERR(hmcdrv_dev_class)) {
+		rc = PTR_ERR(hmcdrv_dev_class);
+		goto out_devdel;
+	}
+
+	/* Finally a device node in /dev has to be established (as 'mkdev'
+	 * does from the command line). Notice that assignment of a device
+	 * node name/mode function is optional (only for mode != 0600).
+	 */
+	hmcdrv_dev.mode = 0; /* "unset" */
+	hmcdrv_dev_class->devnode = hmcdrv_dev_name;
+
+	dev = device_create(hmcdrv_dev_class, NULL, hmcdrv_dev_no, NULL,
+			    "%s", HMCDRV_DEV_NAME);
+	if (!IS_ERR(dev))
+		return 0;
+
+	rc = PTR_ERR(dev);
+	class_destroy(hmcdrv_dev_class);
+	hmcdrv_dev_class = NULL;
+
+out_devdel:
+	cdev_del(&hmcdrv_dev.dev);
+
+out_unreg:
+	unregister_chrdev_region(hmcdrv_dev_no, 1);
+
+out_err:
+
+#else  /* !HMCDRV_DEV_CLASS */
+	hmcdrv_dev.dev.minor = MISC_DYNAMIC_MINOR;
+	hmcdrv_dev.dev.name = HMCDRV_DEV_NAME;
+	hmcdrv_dev.dev.fops = &hmcdrv_dev_fops;
+	hmcdrv_dev.dev.mode = 0; /* finally produces 0600 */
+	rc = misc_register(&hmcdrv_dev.dev);
+#endif	/* HMCDRV_DEV_CLASS */
+
+	return rc;
+}
+
+/**
+ * hmcdrv_dev_exit() - destroys a HMC drive CD/DVD device
+ */
+void hmcdrv_dev_exit(void)
+{
+#ifdef HMCDRV_DEV_CLASS
+	if (!IS_ERR_OR_NULL(hmcdrv_dev_class)) {
+		device_destroy(hmcdrv_dev_class, hmcdrv_dev_no);
+		class_destroy(hmcdrv_dev_class);
+	}
+
+	cdev_del(&hmcdrv_dev.dev);
+	unregister_chrdev_region(hmcdrv_dev_no, 1);
+#else  /* !HMCDRV_DEV_CLASS */
+	misc_deregister(&hmcdrv_dev.dev);
+#endif	/* HMCDRV_DEV_CLASS */
+}
diff --git a/drivers/s390/char/hmcdrv_dev.h b/drivers/s390/char/hmcdrv_dev.h
new file mode 100644
index 000000000000..cb17f07e02de
--- /dev/null
+++ b/drivers/s390/char/hmcdrv_dev.h
@@ -0,0 +1,14 @@
+/*
+ *    SE/HMC Drive FTP Device
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#ifndef __HMCDRV_DEV_H__
+#define __HMCDRV_DEV_H__
+
+int hmcdrv_dev_init(void);
+void hmcdrv_dev_exit(void);
+
+#endif	 /* __HMCDRV_DEV_H__ */
diff --git a/drivers/s390/char/hmcdrv_ftp.c b/drivers/s390/char/hmcdrv_ftp.c
new file mode 100644
index 000000000000..4bd63322fc29
--- /dev/null
+++ b/drivers/s390/char/hmcdrv_ftp.c
@@ -0,0 +1,343 @@
+/*
+ *    HMC Drive FTP Services
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#define KMSG_COMPONENT "hmcdrv"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/export.h>
+
+#include <linux/ctype.h>
+#include <linux/crc16.h>
+
+#include "hmcdrv_ftp.h"
+#include "hmcdrv_cache.h"
+#include "sclp_ftp.h"
+#include "diag_ftp.h"
+
+/**
+ * struct hmcdrv_ftp_ops - HMC drive FTP operations
+ * @startup: startup function
+ * @shutdown: shutdown function
+ * @cmd: FTP transfer function
+ */
+struct hmcdrv_ftp_ops {
+	int (*startup)(void);
+	void (*shutdown)(void);
+	ssize_t (*transfer)(const struct hmcdrv_ftp_cmdspec *ftp,
+			    size_t *fsize);
+};
+
+static enum hmcdrv_ftp_cmdid hmcdrv_ftp_cmd_getid(const char *cmd, int len);
+static int hmcdrv_ftp_parse(char *cmd, struct hmcdrv_ftp_cmdspec *ftp);
+
+static struct hmcdrv_ftp_ops *hmcdrv_ftp_funcs; /* current operations */
+static DEFINE_MUTEX(hmcdrv_ftp_mutex); /* mutex for hmcdrv_ftp_funcs */
+static unsigned hmcdrv_ftp_refcnt; /* start/shutdown reference counter */
+
+/**
+ * hmcdrv_ftp_cmd_getid() - determine FTP command ID from a command string
+ * @cmd: FTP command string (NOT zero-terminated)
+ * @len: length of FTP command string in @cmd
+ */
+static enum hmcdrv_ftp_cmdid hmcdrv_ftp_cmd_getid(const char *cmd, int len)
+{
+	/* HMC FTP command descriptor */
+	struct hmcdrv_ftp_cmd_desc {
+		const char *str;	   /* command string */
+		enum hmcdrv_ftp_cmdid cmd; /* associated command as enum */
+	};
+
+	/* Description of all HMC drive FTP commands
+	 *
+	 * Notes:
+	 * 1. Array size should be a prime number.
+	 * 2. Do not change the order of commands in table (because the
+	 *    index is determined by CRC % ARRAY_SIZE).
+	 * 3. Original command 'nlist' was renamed, else the CRC would
+	 *    collide with 'append' (see point 2).
+	 */
+	static const struct hmcdrv_ftp_cmd_desc ftpcmds[7] = {
+
+		{.str = "get", /* [0] get (CRC = 0x68eb) */
+		 .cmd = HMCDRV_FTP_GET},
+		{.str = "dir", /* [1] dir (CRC = 0x6a9e) */
+		 .cmd = HMCDRV_FTP_DIR},
+		{.str = "delete", /* [2] delete (CRC = 0x53ae) */
+		 .cmd = HMCDRV_FTP_DELETE},
+		{.str = "nls", /* [3] nls (CRC = 0xf87c) */
+		 .cmd = HMCDRV_FTP_NLIST},
+		{.str = "put", /* [4] put (CRC = 0xac56) */
+		 .cmd = HMCDRV_FTP_PUT},
+		{.str = "append", /* [5] append (CRC = 0xf56e) */
+		 .cmd = HMCDRV_FTP_APPEND},
+		{.str = NULL} /* [6] unused */
+	};
+
+	const struct hmcdrv_ftp_cmd_desc *pdesc;
+
+	u16 crc = 0xffffU;
+
+	if (len == 0)
+		return HMCDRV_FTP_NOOP; /* error indiactor */
+
+	crc = crc16(crc, cmd, len);
+	pdesc = ftpcmds + (crc % ARRAY_SIZE(ftpcmds));
+	pr_debug("FTP command '%s' has CRC 0x%04x, at table pos. %lu\n",
+		 cmd, crc, (crc % ARRAY_SIZE(ftpcmds)));
+
+	if (!pdesc->str || strncmp(pdesc->str, cmd, len))
+		return HMCDRV_FTP_NOOP;
+
+	pr_debug("FTP command '%s' found, with ID %d\n",
+		 pdesc->str, pdesc->cmd);
+
+	return pdesc->cmd;
+}
+
+/**
+ * hmcdrv_ftp_parse() - HMC drive FTP command parser
+ * @cmd: FTP command string "<cmd> <filename>"
+ * @ftp: Pointer to FTP command specification buffer (output)
+ *
+ * Return: 0 on success, else a (negative) error code
+ */
+static int hmcdrv_ftp_parse(char *cmd, struct hmcdrv_ftp_cmdspec *ftp)
+{
+	char *start;
+	int argc = 0;
+
+	ftp->id = HMCDRV_FTP_NOOP;
+	ftp->fname = NULL;
+
+	while (*cmd != '\0') {
+
+		while (isspace(*cmd))
+			++cmd;
+
+		if (*cmd == '\0')
+			break;
+
+		start = cmd;
+
+		switch (argc) {
+		case 0: /* 1st argument (FTP command) */
+			while ((*cmd != '\0') && !isspace(*cmd))
+				++cmd;
+			ftp->id = hmcdrv_ftp_cmd_getid(start, cmd - start);
+			break;
+		case 1: /* 2nd / last argument (rest of line) */
+			while ((*cmd != '\0') && !iscntrl(*cmd))
+				++cmd;
+			ftp->fname = start;
+			/* fall through */
+		default:
+			*cmd = '\0';
+			break;
+		} /* switch */
+
+		++argc;
+	} /* while */
+
+	if (!ftp->fname || (ftp->id == HMCDRV_FTP_NOOP))
+		return -EINVAL;
+
+	return 0;
+}
+
+/**
+ * hmcdrv_ftp_do() - perform a HMC drive FTP, with data from kernel-space
+ * @ftp: pointer to FTP command specification
+ *
+ * Return: number of bytes read/written or a negative error code
+ */
+ssize_t hmcdrv_ftp_do(const struct hmcdrv_ftp_cmdspec *ftp)
+{
+	ssize_t len;
+
+	mutex_lock(&hmcdrv_ftp_mutex);
+
+	if (hmcdrv_ftp_funcs && hmcdrv_ftp_refcnt) {
+		pr_debug("starting transfer, cmd %d for '%s' at %lld with %zd bytes\n",
+			 ftp->id, ftp->fname, (long long) ftp->ofs, ftp->len);
+		len = hmcdrv_cache_cmd(ftp, hmcdrv_ftp_funcs->transfer);
+	} else {
+		len = -ENXIO;
+	}
+
+	mutex_unlock(&hmcdrv_ftp_mutex);
+	return len;
+}
+EXPORT_SYMBOL(hmcdrv_ftp_do);
+
+/**
+ * hmcdrv_ftp_probe() - probe for the HMC drive FTP service
+ *
+ * Return: 0 if service is available, else an (negative) error code
+ */
+int hmcdrv_ftp_probe(void)
+{
+	int rc;
+
+	struct hmcdrv_ftp_cmdspec ftp = {
+		.id = HMCDRV_FTP_NOOP,
+		.ofs = 0,
+		.fname = "",
+		.len = PAGE_SIZE
+	};
+
+	ftp.buf = (void *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
+
+	if (!ftp.buf)
+		return -ENOMEM;
+
+	rc = hmcdrv_ftp_startup();
+
+	if (rc)
+		return rc;
+
+	rc = hmcdrv_ftp_do(&ftp);
+	free_page((unsigned long) ftp.buf);
+	hmcdrv_ftp_shutdown();
+
+	switch (rc) {
+	case -ENOENT: /* no such file/media or currently busy, */
+	case -EBUSY:  /* but service seems to be available */
+		rc = 0;
+		break;
+	default: /* leave 'rc' as it is for [0, -EPERM, -E...] */
+		if (rc > 0)
+			rc = 0; /* clear length (success) */
+		break;
+	} /* switch */
+
+	return rc;
+}
+EXPORT_SYMBOL(hmcdrv_ftp_probe);
+
+/**
+ * hmcdrv_ftp_cmd() - Perform a HMC drive FTP, with data from user-space
+ *
+ * @cmd: FTP command string "<cmd> <filename>"
+ * @offset: file position to read/write
+ * @buf: user-space buffer for read/written directory/file
+ * @len: size of @buf (read/dir) or number of bytes to write
+ *
+ * This function must not be called before hmcdrv_ftp_startup() was called.
+ *
+ * Return: number of bytes read/written or a negative error code
+ */
+ssize_t hmcdrv_ftp_cmd(char __kernel *cmd, loff_t offset,
+		       char __user *buf, size_t len)
+{
+	int order;
+
+	struct hmcdrv_ftp_cmdspec ftp = {.len = len, .ofs = offset};
+	ssize_t retlen = hmcdrv_ftp_parse(cmd, &ftp);
+
+	if (retlen)
+		return retlen;
+
+	order = get_order(ftp.len);
+	ftp.buf = (void *) __get_free_pages(GFP_KERNEL | GFP_DMA, order);
+
+	if (!ftp.buf)
+		return -ENOMEM;
+
+	switch (ftp.id) {
+	case HMCDRV_FTP_DIR:
+	case HMCDRV_FTP_NLIST:
+	case HMCDRV_FTP_GET:
+		retlen = hmcdrv_ftp_do(&ftp);
+
+		if ((retlen >= 0) &&
+		    copy_to_user(buf, ftp.buf, retlen))
+			retlen = -EFAULT;
+		break;
+
+	case HMCDRV_FTP_PUT:
+	case HMCDRV_FTP_APPEND:
+		if (!copy_from_user(ftp.buf, buf, ftp.len))
+			retlen = hmcdrv_ftp_do(&ftp);
+		else
+			retlen = -EFAULT;
+		break;
+
+	case HMCDRV_FTP_DELETE:
+		retlen = hmcdrv_ftp_do(&ftp);
+		break;
+
+	default:
+		retlen = -EOPNOTSUPP;
+		break;
+	}
+
+	free_pages((unsigned long) ftp.buf, order);
+	return retlen;
+}
+
+/**
+ * hmcdrv_ftp_startup() - startup of HMC drive FTP functionality for a
+ * dedicated (owner) instance
+ *
+ * Return: 0 on success, else an (negative) error code
+ */
+int hmcdrv_ftp_startup(void)
+{
+	static struct hmcdrv_ftp_ops hmcdrv_ftp_zvm = {
+		.startup = diag_ftp_startup,
+		.shutdown = diag_ftp_shutdown,
+		.transfer = diag_ftp_cmd
+	};
+
+	static struct hmcdrv_ftp_ops hmcdrv_ftp_lpar = {
+		.startup = sclp_ftp_startup,
+		.shutdown = sclp_ftp_shutdown,
+		.transfer = sclp_ftp_cmd
+	};
+
+	int rc = 0;
+
+	mutex_lock(&hmcdrv_ftp_mutex); /* block transfers while start-up */
+
+	if (hmcdrv_ftp_refcnt == 0) {
+		if (MACHINE_IS_VM)
+			hmcdrv_ftp_funcs = &hmcdrv_ftp_zvm;
+		else if (MACHINE_IS_LPAR || MACHINE_IS_KVM)
+			hmcdrv_ftp_funcs = &hmcdrv_ftp_lpar;
+		else
+			rc = -EOPNOTSUPP;
+
+		if (hmcdrv_ftp_funcs)
+			rc = hmcdrv_ftp_funcs->startup();
+	}
+
+	if (!rc)
+		++hmcdrv_ftp_refcnt;
+
+	mutex_unlock(&hmcdrv_ftp_mutex);
+	return rc;
+}
+EXPORT_SYMBOL(hmcdrv_ftp_startup);
+
+/**
+ * hmcdrv_ftp_shutdown() - shutdown of HMC drive FTP functionality for a
+ * dedicated (owner) instance
+ */
+void hmcdrv_ftp_shutdown(void)
+{
+	mutex_lock(&hmcdrv_ftp_mutex);
+	--hmcdrv_ftp_refcnt;
+
+	if ((hmcdrv_ftp_refcnt == 0) && hmcdrv_ftp_funcs)
+		hmcdrv_ftp_funcs->shutdown();
+
+	mutex_unlock(&hmcdrv_ftp_mutex);
+}
+EXPORT_SYMBOL(hmcdrv_ftp_shutdown);
diff --git a/drivers/s390/char/hmcdrv_ftp.h b/drivers/s390/char/hmcdrv_ftp.h
new file mode 100644
index 000000000000..f3643a7b3676
--- /dev/null
+++ b/drivers/s390/char/hmcdrv_ftp.h
@@ -0,0 +1,63 @@
+/*
+ *    SE/HMC Drive FTP Services
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#ifndef __HMCDRV_FTP_H__
+#define __HMCDRV_FTP_H__
+
+#include <linux/types.h> /* size_t, loff_t */
+
+/*
+ * HMC drive FTP Service max. length of path (w/ EOS)
+ */
+#define HMCDRV_FTP_FIDENT_MAX 192
+
+/**
+ * enum hmcdrv_ftp_cmdid - HMC drive FTP commands
+ * @HMCDRV_FTP_NOOP: do nothing (only for probing)
+ * @HMCDRV_FTP_GET: read a file
+ * @HMCDRV_FTP_PUT: (over-) write a file
+ * @HMCDRV_FTP_APPEND: append to a file
+ * @HMCDRV_FTP_DIR: list directory long (ls -l)
+ * @HMCDRV_FTP_NLIST: list files, no directories (name list)
+ * @HMCDRV_FTP_DELETE: delete a file
+ * @HMCDRV_FTP_CANCEL: cancel operation (SCLP/LPAR only)
+ */
+enum hmcdrv_ftp_cmdid {
+	HMCDRV_FTP_NOOP = 0,
+	HMCDRV_FTP_GET = 1,
+	HMCDRV_FTP_PUT = 2,
+	HMCDRV_FTP_APPEND = 3,
+	HMCDRV_FTP_DIR = 4,
+	HMCDRV_FTP_NLIST = 5,
+	HMCDRV_FTP_DELETE = 6,
+	HMCDRV_FTP_CANCEL = 7
+};
+
+/**
+ * struct hmcdrv_ftp_cmdspec - FTP command specification
+ * @id: FTP command ID
+ * @ofs: offset in file
+ * @fname: filename (ASCII), null-terminated
+ * @buf: kernel-space transfer data buffer, 4k aligned
+ * @len: (max) number of bytes to transfer from/to @buf
+ */
+struct hmcdrv_ftp_cmdspec {
+	enum hmcdrv_ftp_cmdid id;
+	loff_t ofs;
+	const char *fname;
+	void __kernel *buf;
+	size_t len;
+};
+
+int hmcdrv_ftp_startup(void);
+void hmcdrv_ftp_shutdown(void);
+int hmcdrv_ftp_probe(void);
+ssize_t hmcdrv_ftp_do(const struct hmcdrv_ftp_cmdspec *ftp);
+ssize_t hmcdrv_ftp_cmd(char __kernel *cmd, loff_t offset,
+		       char __user *buf, size_t len);
+
+#endif	 /* __HMCDRV_FTP_H__ */
diff --git a/drivers/s390/char/hmcdrv_mod.c b/drivers/s390/char/hmcdrv_mod.c
new file mode 100644
index 000000000000..505c6a78ee1a
--- /dev/null
+++ b/drivers/s390/char/hmcdrv_mod.c
@@ -0,0 +1,64 @@
+/*
+ *    HMC Drive DVD Module
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#define KMSG_COMPONENT "hmcdrv"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/version.h>
+#include <linux/stat.h>
+
+#include "hmcdrv_ftp.h"
+#include "hmcdrv_dev.h"
+#include "hmcdrv_cache.h"
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Copyright 2013 IBM Corporation");
+MODULE_DESCRIPTION("HMC drive DVD access");
+
+/*
+ * module parameter 'cachesize'
+ */
+static size_t hmcdrv_mod_cachesize = HMCDRV_CACHE_SIZE_DFLT;
+module_param_named(cachesize, hmcdrv_mod_cachesize, ulong, S_IRUGO);
+
+/**
+ * hmcdrv_mod_init() - module init function
+ */
+static int __init hmcdrv_mod_init(void)
+{
+	int rc = hmcdrv_ftp_probe(); /* perform w/o cache */
+
+	if (rc)
+		return rc;
+
+	rc = hmcdrv_cache_startup(hmcdrv_mod_cachesize);
+
+	if (rc)
+		return rc;
+
+	rc = hmcdrv_dev_init();
+
+	if (rc)
+		hmcdrv_cache_shutdown();
+
+	return rc;
+}
+
+/**
+ * hmcdrv_mod_exit() - module exit function
+ */
+static void __exit hmcdrv_mod_exit(void)
+{
+	hmcdrv_dev_exit();
+	hmcdrv_cache_shutdown();
+}
+
+module_init(hmcdrv_mod_init);
+module_exit(hmcdrv_mod_exit);
diff --git a/drivers/s390/char/sclp.h b/drivers/s390/char/sclp.h
index a68b5ec7d042..a88069f8c677 100644
--- a/drivers/s390/char/sclp.h
+++ b/drivers/s390/char/sclp.h
@@ -19,6 +19,7 @@
 
 #define EVTYP_OPCMD		0x01
 #define EVTYP_MSG		0x02
+#define EVTYP_DIAG_TEST		0x07
 #define EVTYP_STATECHANGE	0x08
 #define EVTYP_PMSGCMD		0x09
 #define EVTYP_CNTLPROGOPCMD	0x20
@@ -32,6 +33,7 @@
 
 #define EVTYP_OPCMD_MASK	0x80000000
 #define EVTYP_MSG_MASK		0x40000000
+#define EVTYP_DIAG_TEST_MASK	0x02000000
 #define EVTYP_STATECHANGE_MASK	0x01000000
 #define EVTYP_PMSGCMD_MASK	0x00800000
 #define EVTYP_CTLPROGOPCMD_MASK	0x00000001
diff --git a/drivers/s390/char/sclp_diag.h b/drivers/s390/char/sclp_diag.h
new file mode 100644
index 000000000000..59c4afa5e670
--- /dev/null
+++ b/drivers/s390/char/sclp_diag.h
@@ -0,0 +1,89 @@
+/*
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#ifndef _SCLP_DIAG_H
+#define _SCLP_DIAG_H
+
+#include <linux/types.h>
+
+/* return codes for Diagnostic Test FTP Service, as indicated in member
+ * sclp_diag_ftp::ldflg
+ */
+#define SCLP_DIAG_FTP_OK	0x80U /* success */
+#define SCLP_DIAG_FTP_LDFAIL	0x01U /* load failed */
+#define SCLP_DIAG_FTP_LDNPERM	0x02U /* not allowed */
+#define SCLP_DIAG_FTP_LDRUNS	0x03U /* LD runs */
+#define SCLP_DIAG_FTP_LDNRUNS	0x04U /* LD does not run */
+
+#define SCLP_DIAG_FTP_XPCX	0x80 /* PCX communication code */
+#define SCLP_DIAG_FTP_ROUTE	4 /* routing code for new FTP service */
+
+/*
+ * length of Diagnostic Test FTP Service event buffer
+ */
+#define SCLP_DIAG_FTP_EVBUF_LEN				\
+	(offsetof(struct sclp_diag_evbuf, mdd) +	\
+	 sizeof(struct sclp_diag_ftp))
+
+/**
+ * struct sclp_diag_ftp - Diagnostic Test FTP Service model-dependent data
+ * @pcx: code for PCX communication (should be 0x80)
+ * @ldflg: load flag (see defines above)
+ * @cmd: FTP command
+ * @pgsize: page size (0 = 4kB, 1 = large page size)
+ * @srcflg: source flag
+ * @spare: reserved (zeroes)
+ * @offset: file offset
+ * @fsize: file size
+ * @length: buffer size resp. bytes transferred
+ * @failaddr: failing address
+ * @bufaddr: buffer address, virtual
+ * @asce: region or segment table designation
+ * @fident: file name (ASCII, zero-terminated)
+ */
+struct sclp_diag_ftp {
+	u8 pcx;
+	u8 ldflg;
+	u8 cmd;
+	u8 pgsize;
+	u8 srcflg;
+	u8 spare;
+	u64 offset;
+	u64 fsize;
+	u64 length;
+	u64 failaddr;
+	u64 bufaddr;
+	u64 asce;
+
+	u8 fident[256];
+} __packed;
+
+/**
+ * struct sclp_diag_evbuf - Diagnostic Test (ET7) Event Buffer
+ * @hdr: event buffer header
+ * @route: diagnostic route
+ * @mdd: model-dependent data (@route dependent)
+ */
+struct sclp_diag_evbuf {
+	struct evbuf_header hdr;
+	u16 route;
+
+	union {
+		struct sclp_diag_ftp ftp;
+	} mdd;
+} __packed;
+
+/**
+ * struct sclp_diag_sccb - Diagnostic Test (ET7) SCCB
+ * @hdr: SCCB header
+ * @evbuf: event buffer
+ */
+struct sclp_diag_sccb {
+
+	struct sccb_header hdr;
+	struct sclp_diag_evbuf evbuf;
+} __packed;
+
+#endif /* _SCLP_DIAG_H */
diff --git a/drivers/s390/char/sclp_early.c b/drivers/s390/char/sclp_early.c
index 1918d9dff45d..5bd6cb145a87 100644
--- a/drivers/s390/char/sclp_early.c
+++ b/drivers/s390/char/sclp_early.c
@@ -281,7 +281,7 @@ out:
 
 static unsigned int __init sclp_con_check_linemode(struct init_sccb *sccb)
 {
-	if (!(sccb->sclp_send_mask & (EVTYP_OPCMD_MASK | EVTYP_PMSGCMD_MASK)))
+	if (!(sccb->sclp_send_mask & EVTYP_OPCMD_MASK))
 		return 0;
 	if (!(sccb->sclp_receive_mask & (EVTYP_MSG_MASK | EVTYP_PMSGCMD_MASK)))
 		return 0;
diff --git a/drivers/s390/char/sclp_ftp.c b/drivers/s390/char/sclp_ftp.c
new file mode 100644
index 000000000000..6561cc5b2d5d
--- /dev/null
+++ b/drivers/s390/char/sclp_ftp.c
@@ -0,0 +1,275 @@
+/*
+ *    SCLP Event Type (ET) 7 - Diagnostic Test FTP Services, useable on LPAR
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ *
+ */
+
+#define KMSG_COMPONENT "hmcdrv"
+#define pr_fmt(fmt) KMSG_COMPONENT ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/io.h>
+#include <linux/wait.h>
+#include <linux/string.h>
+#include <linux/jiffies.h>
+#include <asm/sysinfo.h>
+#include <asm/ebcdic.h>
+
+#include "sclp.h"
+#include "sclp_diag.h"
+#include "sclp_ftp.h"
+
+static DECLARE_COMPLETION(sclp_ftp_rx_complete);
+static u8 sclp_ftp_ldflg;
+static u64 sclp_ftp_fsize;
+static u64 sclp_ftp_length;
+
+/**
+ * sclp_ftp_txcb() - Diagnostic Test FTP services SCLP command callback
+ */
+static void sclp_ftp_txcb(struct sclp_req *req, void *data)
+{
+	struct completion *completion = data;
+
+#ifdef DEBUG
+	pr_debug("SCLP (ET7) TX-IRQ, SCCB @ 0x%p: %*phN\n",
+		 req->sccb, 24, req->sccb);
+#endif
+	complete(completion);
+}
+
+/**
+ * sclp_ftp_rxcb() - Diagnostic Test FTP services receiver event callback
+ */
+static void sclp_ftp_rxcb(struct evbuf_header *evbuf)
+{
+	struct sclp_diag_evbuf *diag = (struct sclp_diag_evbuf *) evbuf;
+
+	/*
+	 * Check for Diagnostic Test FTP Service
+	 */
+	if (evbuf->type != EVTYP_DIAG_TEST ||
+	    diag->route != SCLP_DIAG_FTP_ROUTE ||
+	    diag->mdd.ftp.pcx != SCLP_DIAG_FTP_XPCX ||
+	    evbuf->length < SCLP_DIAG_FTP_EVBUF_LEN)
+		return;
+
+#ifdef DEBUG
+	pr_debug("SCLP (ET7) RX-IRQ, Event @ 0x%p: %*phN\n",
+		 evbuf, 24, evbuf);
+#endif
+
+	/*
+	 * Because the event buffer is located in a page which is owned
+	 * by the SCLP core, all data of interest must be copied. The
+	 * error indication is in 'sclp_ftp_ldflg'
+	 */
+	sclp_ftp_ldflg = diag->mdd.ftp.ldflg;
+	sclp_ftp_fsize = diag->mdd.ftp.fsize;
+	sclp_ftp_length = diag->mdd.ftp.length;
+
+	complete(&sclp_ftp_rx_complete);
+}
+
+/**
+ * sclp_ftp_et7() - start a Diagnostic Test FTP Service SCLP request
+ * @ftp: pointer to FTP descriptor
+ *
+ * Return: 0 on success, else a (negative) error code
+ */
+static int sclp_ftp_et7(const struct hmcdrv_ftp_cmdspec *ftp)
+{
+	struct completion completion;
+	struct sclp_diag_sccb *sccb;
+	struct sclp_req *req;
+	size_t len;
+	int rc;
+
+	req = kzalloc(sizeof(*req), GFP_KERNEL);
+	sccb = (void *) get_zeroed_page(GFP_KERNEL | GFP_DMA);
+	if (!req || !sccb) {
+		rc = -ENOMEM;
+		goto out_free;
+	}
+
+	sccb->hdr.length = SCLP_DIAG_FTP_EVBUF_LEN +
+		sizeof(struct sccb_header);
+	sccb->evbuf.hdr.type = EVTYP_DIAG_TEST;
+	sccb->evbuf.hdr.length = SCLP_DIAG_FTP_EVBUF_LEN;
+	sccb->evbuf.hdr.flags = 0; /* clear processed-buffer */
+	sccb->evbuf.route = SCLP_DIAG_FTP_ROUTE;
+	sccb->evbuf.mdd.ftp.pcx = SCLP_DIAG_FTP_XPCX;
+	sccb->evbuf.mdd.ftp.srcflg = 0;
+	sccb->evbuf.mdd.ftp.pgsize = 0;
+	sccb->evbuf.mdd.ftp.asce = _ASCE_REAL_SPACE;
+	sccb->evbuf.mdd.ftp.ldflg = SCLP_DIAG_FTP_LDFAIL;
+	sccb->evbuf.mdd.ftp.fsize = 0;
+	sccb->evbuf.mdd.ftp.cmd = ftp->id;
+	sccb->evbuf.mdd.ftp.offset = ftp->ofs;
+	sccb->evbuf.mdd.ftp.length = ftp->len;
+	sccb->evbuf.mdd.ftp.bufaddr = virt_to_phys(ftp->buf);
+
+	len = strlcpy(sccb->evbuf.mdd.ftp.fident, ftp->fname,
+		      HMCDRV_FTP_FIDENT_MAX);
+	if (len >= HMCDRV_FTP_FIDENT_MAX) {
+		rc = -EINVAL;
+		goto out_free;
+	}
+
+	req->command = SCLP_CMDW_WRITE_EVENT_DATA;
+	req->sccb = sccb;
+	req->status = SCLP_REQ_FILLED;
+	req->callback = sclp_ftp_txcb;
+	req->callback_data = &completion;
+
+	init_completion(&completion);
+
+	rc = sclp_add_request(req);
+	if (rc)
+		goto out_free;
+
+	/* Wait for end of ftp sclp command. */
+	wait_for_completion(&completion);
+
+#ifdef DEBUG
+	pr_debug("status of SCLP (ET7) request is 0x%04x (0x%02x)\n",
+		 sccb->hdr.response_code, sccb->evbuf.hdr.flags);
+#endif
+
+	/*
+	 * Check if sclp accepted the request. The data transfer runs
+	 * asynchronously and the completion is indicated with an
+	 * sclp ET7 event.
+	 */
+	if (req->status != SCLP_REQ_DONE ||
+	    (sccb->evbuf.hdr.flags & 0x80) == 0 || /* processed-buffer */
+	    (sccb->hdr.response_code & 0xffU) != 0x20U) {
+		rc = -EIO;
+	}
+
+out_free:
+	free_page((unsigned long) sccb);
+	kfree(req);
+	return rc;
+}
+
+/**
+ * sclp_ftp_cmd() - executes a HMC related SCLP Diagnose (ET7) FTP command
+ * @ftp: pointer to FTP command specification
+ * @fsize: return of file size (or NULL if undesirable)
+ *
+ * Attention: Notice that this function is not reentrant - so the caller
+ * must ensure locking.
+ *
+ * Return: number of bytes read/written or a (negative) error code
+ */
+ssize_t sclp_ftp_cmd(const struct hmcdrv_ftp_cmdspec *ftp, size_t *fsize)
+{
+	ssize_t len;
+#ifdef DEBUG
+	unsigned long start_jiffies;
+
+	pr_debug("starting SCLP (ET7), cmd %d for '%s' at %lld with %zd bytes\n",
+		 ftp->id, ftp->fname, (long long) ftp->ofs, ftp->len);
+	start_jiffies = jiffies;
+#endif
+
+	init_completion(&sclp_ftp_rx_complete);
+
+	/* Start ftp sclp command. */
+	len = sclp_ftp_et7(ftp);
+	if (len)
+		goto out_unlock;
+
+	/*
+	 * There is no way to cancel the sclp ET7 request, the code
+	 * needs to wait unconditionally until the transfer is complete.
+	 */
+	wait_for_completion(&sclp_ftp_rx_complete);
+
+#ifdef DEBUG
+	pr_debug("completed SCLP (ET7) request after %lu ms (all)\n",
+		 (jiffies - start_jiffies) * 1000 / HZ);
+	pr_debug("return code of SCLP (ET7) FTP Service is 0x%02x, with %lld/%lld bytes\n",
+		 sclp_ftp_ldflg, sclp_ftp_length, sclp_ftp_fsize);
+#endif
+
+	switch (sclp_ftp_ldflg) {
+	case SCLP_DIAG_FTP_OK:
+		len = sclp_ftp_length;
+		if (fsize)
+			*fsize = sclp_ftp_fsize;
+		break;
+	case SCLP_DIAG_FTP_LDNPERM:
+		len = -EPERM;
+		break;
+	case SCLP_DIAG_FTP_LDRUNS:
+		len = -EBUSY;
+		break;
+	case SCLP_DIAG_FTP_LDFAIL:
+		len = -ENOENT;
+		break;
+	default:
+		len = -EIO;
+		break;
+	}
+
+out_unlock:
+	return len;
+}
+
+/*
+ * ET7 event listener
+ */
+static struct sclp_register sclp_ftp_event = {
+	.send_mask = EVTYP_DIAG_TEST_MASK,    /* want tx events */
+	.receive_mask = EVTYP_DIAG_TEST_MASK, /* want rx events */
+	.receiver_fn = sclp_ftp_rxcb,	      /* async callback (rx) */
+	.state_change_fn = NULL,
+	.pm_event_fn = NULL,
+};
+
+/**
+ * sclp_ftp_startup() - startup of FTP services, when running on LPAR
+ */
+int sclp_ftp_startup(void)
+{
+#ifdef DEBUG
+	unsigned long info;
+#endif
+	int rc;
+
+	rc = sclp_register(&sclp_ftp_event);
+	if (rc)
+		return rc;
+
+#ifdef DEBUG
+	info = get_zeroed_page(GFP_KERNEL);
+
+	if (info != 0) {
+		struct sysinfo_2_2_2 *info222 = (struct sysinfo_2_2_2 *)info;
+
+		if (!stsi(info222, 2, 2, 2)) { /* get SYSIB 2.2.2 */
+			info222->name[sizeof(info222->name) - 1] = '\0';
+			EBCASC_500(info222->name, sizeof(info222->name) - 1);
+			pr_debug("SCLP (ET7) FTP Service working on LPAR %u (%s)\n",
+				 info222->lpar_number, info222->name);
+		}
+
+		free_page(info);
+	}
+#endif	/* DEBUG */
+	return 0;
+}
+
+/**
+ * sclp_ftp_shutdown() - shutdown of FTP services, when running on LPAR
+ */
+void sclp_ftp_shutdown(void)
+{
+	sclp_unregister(&sclp_ftp_event);
+}
diff --git a/drivers/s390/char/sclp_ftp.h b/drivers/s390/char/sclp_ftp.h
new file mode 100644
index 000000000000..98ba3183e7d9
--- /dev/null
+++ b/drivers/s390/char/sclp_ftp.h
@@ -0,0 +1,21 @@
+/*
+ *    SCLP Event Type (ET) 7 - Diagnostic Test FTP Services, useable on LPAR
+ *
+ *    Notice that all functions exported here are not reentrant.
+ *    So usage should be exclusive, ensured by the caller (e.g. using a
+ *    mutex).
+ *
+ *    Copyright IBM Corp. 2013
+ *    Author(s): Ralf Hoppe (rhoppe@de.ibm.com)
+ */
+
+#ifndef __SCLP_FTP_H__
+#define __SCLP_FTP_H__
+
+#include "hmcdrv_ftp.h"
+
+int sclp_ftp_startup(void);
+void sclp_ftp_shutdown(void);
+ssize_t sclp_ftp_cmd(const struct hmcdrv_ftp_cmdspec *ftp, size_t *fsize);
+
+#endif	 /* __SCLP_FTP_H__ */
diff --git a/drivers/s390/char/sclp_rw.c b/drivers/s390/char/sclp_rw.c
index 3b13d58fe87b..35a84af875ee 100644
--- a/drivers/s390/char/sclp_rw.c
+++ b/drivers/s390/char/sclp_rw.c
@@ -33,7 +33,7 @@ static void sclp_rw_pm_event(struct sclp_register *reg,
 
 /* Event type structure for write message and write priority message */
 static struct sclp_register sclp_rw_event = {
-	.send_mask = EVTYP_MSG_MASK | EVTYP_PMSGCMD_MASK,
+	.send_mask = EVTYP_MSG_MASK,
 	.pm_event_fn = sclp_rw_pm_event,
 };
 
@@ -456,14 +456,9 @@ sclp_emit_buffer(struct sclp_buffer *buffer,
 		return -EIO;
 
 	sccb = buffer->sccb;
-	if (sclp_rw_event.sclp_receive_mask & EVTYP_MSG_MASK)
-		/* Use normal write message */
-		sccb->msg_buf.header.type = EVTYP_MSG;
-	else if (sclp_rw_event.sclp_receive_mask & EVTYP_PMSGCMD_MASK)
-		/* Use write priority message */
-		sccb->msg_buf.header.type = EVTYP_PMSGCMD;
-	else
-		return -EOPNOTSUPP;
+	/* Use normal write message */
+	sccb->msg_buf.header.type = EVTYP_MSG;
+
 	buffer->request.command = SCLP_CMDW_WRITE_EVENT_DATA;
 	buffer->request.status = SCLP_REQ_FILLED;
 	buffer->request.callback = sclp_writedata_callback;
diff --git a/drivers/s390/char/sclp_tty.c b/drivers/s390/char/sclp_tty.c
index 7ed7a5987816..003663288e29 100644
--- a/drivers/s390/char/sclp_tty.c
+++ b/drivers/s390/char/sclp_tty.c
@@ -559,7 +559,7 @@ sclp_tty_init(void)
 	driver->subtype = SYSTEM_TYPE_TTY;
 	driver->init_termios = tty_std_termios;
 	driver->init_termios.c_iflag = IGNBRK | IGNPAR;
-	driver->init_termios.c_oflag = ONLCR | XTABS;
+	driver->init_termios.c_oflag = ONLCR;
 	driver->init_termios.c_lflag = ISIG | ECHO;
 	driver->flags = TTY_DRIVER_REAL_RAW;
 	tty_set_operations(driver, &sclp_ops);
diff --git a/drivers/s390/char/sclp_vt220.c b/drivers/s390/char/sclp_vt220.c
index b9a9f721716d..ae67386c03d3 100644
--- a/drivers/s390/char/sclp_vt220.c
+++ b/drivers/s390/char/sclp_vt220.c
@@ -206,10 +206,6 @@ sclp_vt220_callback(struct sclp_req *request, void *data)
 static int
 __sclp_vt220_emit(struct sclp_vt220_request *request)
 {
-	if (!(sclp_vt220_register.sclp_receive_mask & EVTYP_VT220MSG_MASK)) {
-		request->sclp_req.status = SCLP_REQ_FAILED;
-		return -EIO;
-	}
 	request->sclp_req.command = SCLP_CMDW_WRITE_EVENT_DATA;
 	request->sclp_req.status = SCLP_REQ_FILLED;
 	request->sclp_req.callback = sclp_vt220_callback;
diff --git a/drivers/s390/char/tape_char.c b/drivers/s390/char/tape_char.c
index 6dc60725de92..77f9b9c2f701 100644
--- a/drivers/s390/char/tape_char.c
+++ b/drivers/s390/char/tape_char.c
@@ -402,7 +402,9 @@ __tapechar_ioctl(struct tape_device *device,
 		memset(&get, 0, sizeof(get));
 		get.mt_type = MT_ISUNKNOWN;
 		get.mt_resid = 0 /* device->devstat.rescnt */;
-		get.mt_dsreg = device->tape_state;
+		get.mt_dsreg =
+			((device->char_data.block_size << MT_ST_BLKSIZE_SHIFT)
+			 & MT_ST_BLKSIZE_MASK);
 		/* FIXME: mt_gstat, mt_erreg, mt_fileno */
 		get.mt_gstat = 0;
 		get.mt_erreg = 0;
diff --git a/drivers/s390/char/vmlogrdr.c b/drivers/s390/char/vmlogrdr.c
index a8848db7b09d..9bb48d70957c 100644
--- a/drivers/s390/char/vmlogrdr.c
+++ b/drivers/s390/char/vmlogrdr.c
@@ -338,7 +338,6 @@ static int vmlogrdr_open (struct inode *inode, struct file *filp)
 
 	/* set the file options */
 	filp->private_data = logptr;
-	filp->f_op = &vmlogrdr_fops;
 
 	/* start recording for this service*/
 	if (logptr->autorecording) {
diff --git a/drivers/s390/char/zcore.c b/drivers/s390/char/zcore.c
index 1884653e4472..efcf48481c5f 100644
--- a/drivers/s390/char/zcore.c
+++ b/drivers/s390/char/zcore.c
@@ -28,6 +28,7 @@
 #include <asm/processor.h>
 #include <asm/irqflags.h>
 #include <asm/checksum.h>
+#include <asm/switch_to.h>
 #include "sclp.h"
 
 #define TRACE(x...) debug_sprintf_event(zcore_dbf, 1, x)
@@ -149,18 +150,21 @@ static int memcpy_hsa_kernel(void *dest, unsigned long src, size_t count)
 
 static int __init init_cpu_info(enum arch_id arch)
 {
-	struct save_area *sa;
+	struct save_area_ext *sa_ext;
 
 	/* get info for boot cpu from lowcore, stored in the HSA */
 
-	sa = dump_save_area_create(0);
-	if (!sa)
+	sa_ext = dump_save_area_create(0);
+	if (!sa_ext)
 		return -ENOMEM;
-	if (memcpy_hsa_kernel(sa, sys_info.sa_base, sys_info.sa_size) < 0) {
+	if (memcpy_hsa_kernel(&sa_ext->sa, sys_info.sa_base,
+			      sys_info.sa_size) < 0) {
 		TRACE("could not copy from HSA\n");
-		kfree(sa);
+		kfree(sa_ext);
 		return -EIO;
 	}
+	if (MACHINE_HAS_VX)
+		save_vx_regs_safe(sa_ext->vx_regs);
 	return 0;
 }
 
@@ -258,7 +262,7 @@ static int zcore_add_lc(char __user *buf, unsigned long start, size_t count)
 		unsigned long sa_start, sa_end; /* save area range */
 		unsigned long prefix;
 		unsigned long sa_off, len, buf_off;
-		struct save_area *save_area = dump_save_areas.areas[i];
+		struct save_area *save_area = &dump_save_areas.areas[i]->sa;
 
 		prefix = save_area->pref_reg;
 		sa_start = prefix + sys_info.sa_base;
@@ -612,7 +616,7 @@ static void __init zcore_header_init(int arch, struct zcore_header *hdr,
 	hdr->tod = get_tod_clock();
 	get_cpu_id(&hdr->cpu_id);
 	for (i = 0; i < dump_save_areas.count; i++) {
-		prefix = dump_save_areas.areas[i]->pref_reg;
+		prefix = dump_save_areas.areas[i]->sa.pref_reg;
 		hdr->real_cpu_cnt++;
 		if (!prefix)
 			continue;
diff --git a/drivers/s390/cio/airq.c b/drivers/s390/cio/airq.c
index 00bfbee0af9e..56eb4ee4deba 100644
--- a/drivers/s390/cio/airq.c
+++ b/drivers/s390/cio/airq.c
@@ -87,7 +87,7 @@ static irqreturn_t do_airq_interrupt(int irq, void *dummy)
 	struct airq_struct *airq;
 	struct hlist_head *head;
 
-	__this_cpu_write(s390_idle.nohz_delay, 1);
+	set_cpu_flag(CIF_NOHZ_DELAY);
 	tpi_info = (struct tpi_info *) &get_irq_regs()->int_code;
 	head = &airq_lists[tpi_info->isc];
 	rcu_read_lock();
diff --git a/drivers/s390/cio/ccwreq.c b/drivers/s390/cio/ccwreq.c
index 07676c22d514..79f59915f71b 100644
--- a/drivers/s390/cio/ccwreq.c
+++ b/drivers/s390/cio/ccwreq.c
@@ -252,7 +252,7 @@ static void ccwreq_log_status(struct ccw_device *cdev, enum io_status status)
  */
 void ccw_request_handler(struct ccw_device *cdev)
 {
-	struct irb *irb = &__get_cpu_var(cio_irb);
+	struct irb *irb = this_cpu_ptr(&cio_irb);
 	struct ccw_request *req = &cdev->private->req;
 	enum io_status status;
 	int rc = -EOPNOTSUPP;
diff --git a/drivers/s390/cio/chp.c b/drivers/s390/cio/chp.c
index d497aa05a72f..c692dfebd0ba 100644
--- a/drivers/s390/cio/chp.c
+++ b/drivers/s390/cio/chp.c
@@ -257,11 +257,11 @@ static ssize_t chp_status_write(struct device *dev,
 	if (!num_args)
 		return count;
 
-	if (!strnicmp(cmd, "on", 2) || !strcmp(cmd, "1")) {
+	if (!strncasecmp(cmd, "on", 2) || !strcmp(cmd, "1")) {
 		mutex_lock(&cp->lock);
 		error = s390_vary_chpid(cp->chpid, 1);
 		mutex_unlock(&cp->lock);
-	} else if (!strnicmp(cmd, "off", 3) || !strcmp(cmd, "0")) {
+	} else if (!strncasecmp(cmd, "off", 3) || !strcmp(cmd, "0")) {
 		mutex_lock(&cp->lock);
 		error = s390_vary_chpid(cp->chpid, 0);
 		mutex_unlock(&cp->lock);
diff --git a/drivers/s390/cio/chsc_sch.c b/drivers/s390/cio/chsc_sch.c
index 3d22d2a4ce14..213159dec89e 100644
--- a/drivers/s390/cio/chsc_sch.c
+++ b/drivers/s390/cio/chsc_sch.c
@@ -58,7 +58,7 @@ static void chsc_subchannel_irq(struct subchannel *sch)
 {
 	struct chsc_private *private = dev_get_drvdata(&sch->dev);
 	struct chsc_request *request = private->request;
-	struct irb *irb = &__get_cpu_var(cio_irb);
+	struct irb *irb = this_cpu_ptr(&cio_irb);
 
 	CHSC_LOG(4, "irb");
 	CHSC_LOG_HEX(4, irb, sizeof(*irb));
diff --git a/drivers/s390/cio/cio.c b/drivers/s390/cio/cio.c
index 2905d8b0ec95..10eb738fc81a 100644
--- a/drivers/s390/cio/cio.c
+++ b/drivers/s390/cio/cio.c
@@ -561,9 +561,9 @@ static irqreturn_t do_cio_interrupt(int irq, void *dummy)
 	struct subchannel *sch;
 	struct irb *irb;
 
-	__this_cpu_write(s390_idle.nohz_delay, 1);
+	set_cpu_flag(CIF_NOHZ_DELAY);
 	tpi_info = (struct tpi_info *) &get_irq_regs()->int_code;
-	irb = &__get_cpu_var(cio_irb);
+	irb = this_cpu_ptr(&cio_irb);
 	sch = (struct subchannel *)(unsigned long) tpi_info->intparm;
 	if (!sch) {
 		/* Clear pending interrupt condition. */
@@ -613,7 +613,7 @@ void cio_tsch(struct subchannel *sch)
 	struct irb *irb;
 	int irq_context;
 
-	irb = &__get_cpu_var(cio_irb);
+	irb = this_cpu_ptr(&cio_irb);
 	/* Store interrupt response block to lowcore. */
 	if (tsch(sch->schid, irb) != 0)
 		/* Not status pending or not operational. */
@@ -751,7 +751,7 @@ __clear_io_subchannel_easy(struct subchannel_id schid)
 		struct tpi_info ti;
 
 		if (tpi(&ti)) {
-			tsch(ti.schid, &__get_cpu_var(cio_irb));
+			tsch(ti.schid, this_cpu_ptr(&cio_irb));
 			if (schid_equal(&ti.schid, &schid))
 				return 0;
 		}
diff --git a/drivers/s390/cio/device_fsm.c b/drivers/s390/cio/device_fsm.c
index 0bc902b3cd84..83da53c8e54c 100644
--- a/drivers/s390/cio/device_fsm.c
+++ b/drivers/s390/cio/device_fsm.c
@@ -739,7 +739,7 @@ ccw_device_irq(struct ccw_device *cdev, enum dev_event dev_event)
 	struct irb *irb;
 	int is_cmd;
 
-	irb = &__get_cpu_var(cio_irb);
+	irb = this_cpu_ptr(&cio_irb);
 	is_cmd = !scsw_is_tm(&irb->scsw);
 	/* Check for unsolicited interrupt. */
 	if (!scsw_is_solicited(&irb->scsw)) {
@@ -805,7 +805,7 @@ ccw_device_w4sense(struct ccw_device *cdev, enum dev_event dev_event)
 {
 	struct irb *irb;
 
-	irb = &__get_cpu_var(cio_irb);
+	irb = this_cpu_ptr(&cio_irb);
 	/* Check for unsolicited interrupt. */
 	if (scsw_stctl(&irb->scsw) ==
 	    (SCSW_STCTL_STATUS_PEND | SCSW_STCTL_ALERT_STATUS)) {
diff --git a/drivers/s390/cio/eadm_sch.c b/drivers/s390/cio/eadm_sch.c
index c4f7bf3e24c2..37f0834300ea 100644
--- a/drivers/s390/cio/eadm_sch.c
+++ b/drivers/s390/cio/eadm_sch.c
@@ -134,7 +134,7 @@ static void eadm_subchannel_irq(struct subchannel *sch)
 {
 	struct eadm_private *private = get_eadm_private(sch);
 	struct eadm_scsw *scsw = &sch->schib.scsw.eadm;
-	struct irb *irb = &__get_cpu_var(cio_irb);
+	struct irb *irb = this_cpu_ptr(&cio_irb);
 	int error = 0;
 
 	EADM_LOG(6, "irq");
diff --git a/drivers/s390/crypto/ap_bus.c b/drivers/s390/crypto/ap_bus.c
index 4038437ff033..99485415dcc2 100644
--- a/drivers/s390/crypto/ap_bus.c
+++ b/drivers/s390/crypto/ap_bus.c
@@ -664,6 +664,17 @@ static ssize_t ap_hwtype_show(struct device *dev,
 }
 
 static DEVICE_ATTR(hwtype, 0444, ap_hwtype_show, NULL);
+
+static ssize_t ap_raw_hwtype_show(struct device *dev,
+			      struct device_attribute *attr, char *buf)
+{
+	struct ap_device *ap_dev = to_ap_dev(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%d\n", ap_dev->raw_hwtype);
+}
+
+static DEVICE_ATTR(raw_hwtype, 0444, ap_raw_hwtype_show, NULL);
+
 static ssize_t ap_depth_show(struct device *dev, struct device_attribute *attr,
 			     char *buf)
 {
@@ -734,6 +745,7 @@ static DEVICE_ATTR(ap_functions, 0444, ap_functions_show, NULL);
 
 static struct attribute *ap_dev_attrs[] = {
 	&dev_attr_hwtype.attr,
+	&dev_attr_raw_hwtype.attr,
 	&dev_attr_depth.attr,
 	&dev_attr_request_count.attr,
 	&dev_attr_requestq_count.attr,
@@ -1188,6 +1200,10 @@ static int ap_select_domain(void)
 	ap_qid_t qid;
 	int rc, i, j;
 
+	/* IF APXA isn't installed, only 16 domains could be defined */
+	if (!ap_configuration->ap_extended && (ap_domain_index > 15))
+		return -EINVAL;
+
 	/*
 	 * We want to use a single domain. Either the one specified with
 	 * the "domain=" parameter or the domain with the maximum number
@@ -1413,9 +1429,13 @@ static void ap_scan_bus(struct work_struct *unused)
 				continue;
 			}
 			break;
+		case 11:
+			ap_dev->device_type = 10;
+			break;
 		default:
 			ap_dev->device_type = device_type;
 		}
+		ap_dev->raw_hwtype = device_type;
 
 		rc = ap_query_functions(qid, &device_functions);
 		if (!rc)
@@ -1900,9 +1920,15 @@ static void ap_reset_all(void)
 {
 	int i, j;
 
-	for (i = 0; i < AP_DOMAINS; i++)
-		for (j = 0; j < AP_DEVICES; j++)
+	for (i = 0; i < AP_DOMAINS; i++) {
+		if (!ap_test_config_domain(i))
+			continue;
+		for (j = 0; j < AP_DEVICES; j++) {
+			if (!ap_test_config_card_id(j))
+				continue;
 			ap_reset_queue(AP_MKQID(j, i));
+		}
+	}
 }
 
 static struct reset_call ap_reset_call = {
diff --git a/drivers/s390/crypto/ap_bus.h b/drivers/s390/crypto/ap_bus.h
index 6405ae24a7a6..055a0f956d17 100644
--- a/drivers/s390/crypto/ap_bus.h
+++ b/drivers/s390/crypto/ap_bus.h
@@ -31,7 +31,7 @@
 #include <linux/types.h>
 
 #define AP_DEVICES 64		/* Number of AP devices. */
-#define AP_DOMAINS 16		/* Number of AP domains. */
+#define AP_DOMAINS 256		/* Number of AP domains. */
 #define AP_MAX_RESET 90		/* Maximum number of resets. */
 #define AP_RESET_TIMEOUT (HZ*0.7)	/* Time in ticks for reset timeouts. */
 #define AP_CONFIG_TIME 30	/* Time in seconds between AP bus rescans. */
@@ -45,9 +45,9 @@ extern int ap_domain_index;
  */
 typedef unsigned int ap_qid_t;
 
-#define AP_MKQID(_device,_queue) (((_device) & 63) << 8 | ((_queue) & 15))
+#define AP_MKQID(_device, _queue) (((_device) & 63) << 8 | ((_queue) & 255))
 #define AP_QID_DEVICE(_qid) (((_qid) >> 8) & 63)
-#define AP_QID_QUEUE(_qid) ((_qid) & 15)
+#define AP_QID_QUEUE(_qid) ((_qid) & 255)
 
 /**
  * structy ap_queue_status - Holds the AP queue status.
@@ -161,6 +161,7 @@ struct ap_device {
 	ap_qid_t qid;			/* AP queue id. */
 	int queue_depth;		/* AP queue depth.*/
 	int device_type;		/* AP device type. */
+	int raw_hwtype;			/* AP raw hardware type. */
 	unsigned int functions;		/* AP device function bitfield. */
 	int unregistered;		/* marks AP device as unregistered */
 	struct timer_list timeout;	/* Timer for request timeouts. */
diff --git a/drivers/s390/crypto/zcrypt_api.c b/drivers/s390/crypto/zcrypt_api.c
index 0e18c5dcd91f..08f1830cbfc4 100644
--- a/drivers/s390/crypto/zcrypt_api.c
+++ b/drivers/s390/crypto/zcrypt_api.c
@@ -343,10 +343,11 @@ struct zcrypt_ops *__ops_lookup(unsigned char *name, int variant)
 			break;
 		}
 	}
+	if (!found || !try_module_get(zops->owner))
+		zops = NULL;
+
 	spin_unlock_bh(&zcrypt_ops_list_lock);
 
-	if (!found)
-		return NULL;
 	return zops;
 }
 
@@ -359,8 +360,6 @@ struct zcrypt_ops *zcrypt_msgtype_request(unsigned char *name, int variant)
 		request_module("%s", name);
 		zops = __ops_lookup(name, variant);
 	}
-	if ((!zops) || (!try_module_get(zops->owner)))
-		return NULL;
 	return zops;
 }
 EXPORT_SYMBOL(zcrypt_msgtype_request);
diff --git a/drivers/s390/kvm/kvm_virtio.c b/drivers/s390/kvm/kvm_virtio.c
index a1349653c6d9..643129070c51 100644
--- a/drivers/s390/kvm/kvm_virtio.c
+++ b/drivers/s390/kvm/kvm_virtio.c
@@ -406,15 +406,8 @@ static void kvm_extint_handler(struct ext_code ext_code,
 
 	switch (param) {
 	case VIRTIO_PARAM_CONFIG_CHANGED:
-	{
-		struct virtio_driver *drv;
-		drv = container_of(vq->vdev->dev.driver,
-				   struct virtio_driver, driver);
-		if (drv->config_changed)
-			drv->config_changed(vq->vdev);
-
+		virtio_config_changed(vq->vdev);
 		break;
-	}
 	case VIRTIO_PARAM_DEV_ADD:
 		schedule_work(&hotplug_work);
 		break;
diff --git a/drivers/s390/kvm/virtio_ccw.c b/drivers/s390/kvm/virtio_ccw.c
index d2c0b442bce5..6cbe6ef3c889 100644
--- a/drivers/s390/kvm/virtio_ccw.c
+++ b/drivers/s390/kvm/virtio_ccw.c
@@ -940,11 +940,7 @@ static void virtio_ccw_int_handler(struct ccw_device *cdev,
 		vring_interrupt(0, vq);
 	}
 	if (test_bit(0, &vcdev->indicators2)) {
-		drv = container_of(vcdev->vdev.dev.driver,
-				   struct virtio_driver, driver);
-
-		if (drv && drv->config_changed)
-			drv->config_changed(&vcdev->vdev);
+		virtio_config_changed(&vcdev->vdev);
 		clear_bit(0, &vcdev->indicators2);
 	}
 }
diff --git a/drivers/s390/net/claw.c b/drivers/s390/net/claw.c
index fbc6701bef30..213e54ee8a66 100644
--- a/drivers/s390/net/claw.c
+++ b/drivers/s390/net/claw.c
@@ -481,7 +481,6 @@ claw_open(struct net_device *dev)
                 spin_unlock_irqrestore(
 			get_ccwdev_lock(privptr->channel[i].cdev), saveflags);
                 schedule();
-		set_current_state(TASK_RUNNING);
                 remove_wait_queue(&privptr->channel[i].wait, &wait);
                 if(rc != 0)
                         ccw_check_return_code(privptr->channel[i].cdev, rc);
@@ -828,7 +827,6 @@ claw_release(struct net_device *dev)
 	        spin_unlock_irqrestore(
 			get_ccwdev_lock(privptr->channel[i].cdev), saveflags);
 	        schedule();
-		set_current_state(TASK_RUNNING);
 	        remove_wait_queue(&privptr->channel[i].wait, &wait);
 	        if (rc != 0) {
                         ccw_check_return_code(privptr->channel[i].cdev, rc);
diff --git a/drivers/s390/net/qeth_core.h b/drivers/s390/net/qeth_core.h
index 97ef37b51068..e7646ce3d659 100644
--- a/drivers/s390/net/qeth_core.h
+++ b/drivers/s390/net/qeth_core.h
@@ -889,6 +889,7 @@ extern const struct attribute_group *qeth_generic_attr_groups[];
 extern const struct attribute_group *qeth_osn_attr_groups[];
 extern struct workqueue_struct *qeth_wq;
 
+int qeth_card_hw_is_reachable(struct qeth_card *);
 const char *qeth_get_cardname_short(struct qeth_card *);
 int qeth_realloc_buffer_pool(struct qeth_card *, int);
 int qeth_core_load_discipline(struct qeth_card *, enum qeth_discipline_id);
diff --git a/drivers/s390/net/qeth_core_main.c b/drivers/s390/net/qeth_core_main.c
index c0d6ba8655c7..fd22c811cbe1 100644
--- a/drivers/s390/net/qeth_core_main.c
+++ b/drivers/s390/net/qeth_core_main.c
@@ -73,6 +73,13 @@ static int qeth_init_qdio_out_buf(struct qeth_qdio_out_q *, int);
 struct workqueue_struct *qeth_wq;
 EXPORT_SYMBOL_GPL(qeth_wq);
 
+int qeth_card_hw_is_reachable(struct qeth_card *card)
+{
+	return (card->state == CARD_STATE_SOFTSETUP) ||
+		(card->state == CARD_STATE_UP);
+}
+EXPORT_SYMBOL_GPL(qeth_card_hw_is_reachable);
+
 static void qeth_close_dev_handler(struct work_struct *work)
 {
 	struct qeth_card *card;
@@ -5790,6 +5797,7 @@ int qeth_core_ethtool_get_settings(struct net_device *netdev,
 	struct qeth_card *card = netdev->ml_priv;
 	enum qeth_link_types link_type;
 	struct carrier_info carrier_info;
+	int rc;
 	u32 speed;
 
 	if ((card->info.type == QETH_CARD_TYPE_IQD) || (card->info.guestlan))
@@ -5832,8 +5840,14 @@ int qeth_core_ethtool_get_settings(struct net_device *netdev,
 	/* Check if we can obtain more accurate information.	 */
 	/* If QUERY_CARD_INFO command is not supported or fails, */
 	/* just return the heuristics that was filled above.	 */
-	if (qeth_query_card_info(card, &carrier_info) != 0)
+	if (!qeth_card_hw_is_reachable(card))
+		return -ENODEV;
+	rc = qeth_query_card_info(card, &carrier_info);
+	if (rc == -EOPNOTSUPP) /* for old hardware, return heuristic */
 		return 0;
+	if (rc) /* report error from the hardware operation */
+		return rc;
+	/* on success, fill in the information got from the hardware */
 
 	netdev_dbg(netdev,
 	"card info: card_type=0x%02x, port_mode=0x%04x, port_speed=0x%08x\n",
diff --git a/drivers/s390/net/qeth_l2_sys.c b/drivers/s390/net/qeth_l2_sys.c
index ae1bc04b8653..59e3aa538b4d 100644
--- a/drivers/s390/net/qeth_l2_sys.c
+++ b/drivers/s390/net/qeth_l2_sys.c
@@ -5,17 +5,12 @@
 
 #include <linux/slab.h>
 #include <asm/ebcdic.h>
+#include "qeth_core.h"
 #include "qeth_l2.h"
 
 #define QETH_DEVICE_ATTR(_id, _name, _mode, _show, _store) \
 struct device_attribute dev_attr_##_id = __ATTR(_name, _mode, _show, _store)
 
-static int qeth_card_hw_is_reachable(struct qeth_card *card)
-{
-	return (card->state == CARD_STATE_SOFTSETUP) ||
-		(card->state == CARD_STATE_UP);
-}
-
 static ssize_t qeth_bridge_port_role_state_show(struct device *dev,
 				struct device_attribute *attr, char *buf,
 				int show_state)
diff --git a/drivers/s390/net/qeth_l3_main.c b/drivers/s390/net/qeth_l3_main.c
index f8427a2c4840..afebb9709763 100644
--- a/drivers/s390/net/qeth_l3_main.c
+++ b/drivers/s390/net/qeth_l3_main.c
@@ -3306,7 +3306,7 @@ static int qeth_l3_setup_netdev(struct qeth_card *card)
 	card->dev->features |=	NETIF_F_HW_VLAN_CTAG_TX |
 				NETIF_F_HW_VLAN_CTAG_RX |
 				NETIF_F_HW_VLAN_CTAG_FILTER;
-	card->dev->priv_flags &= ~IFF_XMIT_DST_RELEASE;
+	netif_keep_dst(card->dev);
 	card->dev->gso_max_size = 15 * PAGE_SIZE;
 
 	SET_NETDEV_DEV(card->dev, &card->gdev->dev);
diff --git a/drivers/scsi/Kconfig b/drivers/scsi/Kconfig
index 18a3358eb1d4..3a820f61ce65 100644
--- a/drivers/scsi/Kconfig
+++ b/drivers/scsi/Kconfig
@@ -43,7 +43,18 @@ config SCSI_DMA
 config SCSI_NETLINK
 	bool
 	default	n
-	select NET
+	depends on NET
+
+config SCSI_MQ_DEFAULT
+	bool "SCSI: use blk-mq I/O path by default"
+	depends on SCSI
+	---help---
+	  This option enables the new blk-mq based I/O path for SCSI
+	  devices by default.  With the option the scsi_mod.use_blk_mq
+	  module/boot option defaults to Y, without it to N, but it can
+	  still be overriden either way.
+
+	  If unsure say N.
 
 config SCSI_PROC_FS
 	bool "legacy /proc/scsi/ support"
@@ -62,7 +73,6 @@ comment "SCSI support type (disk, tape, CD-ROM)"
 config BLK_DEV_SD
 	tristate "SCSI disk support"
 	depends on SCSI
-	select CRC_T10DIF if BLK_DEV_INTEGRITY
 	---help---
 	  If you want to use SCSI hard disks, Fibre Channel disks,
 	  Serial ATA (SATA) or Parallel ATA (PATA) hard disks,
@@ -257,7 +267,7 @@ config SCSI_SPI_ATTRS
 
 config SCSI_FC_ATTRS
 	tristate "FiberChannel Transport Attributes"
-	depends on SCSI
+	depends on SCSI && NET
 	select SCSI_NETLINK
 	help
 	  If you wish to export transport-specific information about
@@ -576,6 +586,16 @@ config VMWARE_PVSCSI
 	  To compile this driver as a module, choose M here: the
 	  module will be called vmw_pvscsi.
 
+config XEN_SCSI_FRONTEND
+	tristate "XEN SCSI frontend driver"
+	depends on SCSI && XEN
+	select XEN_XENBUS_FRONTEND
+	help
+	  The XEN SCSI frontend driver allows the kernel to access SCSI Devices
+	  within another guest OS (usually Dom0).
+	  Only needed if the kernel is running in a XEN guest and generic
+	  SCSI access to a device is needed.
+
 config HYPERV_STORAGE
 	tristate "Microsoft Hyper-V virtual storage driver"
 	depends on SCSI && HYPERV
@@ -585,28 +605,28 @@ config HYPERV_STORAGE
 
 config LIBFC
 	tristate "LibFC module"
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	select CRC32
 	---help---
 	  Fibre Channel library module
 
 config LIBFCOE
 	tristate "LibFCoE module"
-	select LIBFC
+	depends on LIBFC
 	---help---
 	  Library for Fibre Channel over Ethernet module
 
 config FCOE
 	tristate "FCoE module"
 	depends on PCI
-	select LIBFCOE
+	depends on LIBFCOE
 	---help---
 	  Fibre Channel over Ethernet module
 
 config FCOE_FNIC
 	tristate "Cisco FNIC Driver"
 	depends on PCI && X86
-	select LIBFCOE
+	depends on LIBFCOE
 	help
 	  This is support for the Cisco PCI-Express FCoE HBA.
 
@@ -816,7 +836,7 @@ config SCSI_IBMVSCSI
 config SCSI_IBMVFC
 	tristate "IBM Virtual FC support"
 	depends on PPC_PSERIES && SCSI
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	help
 	  This is the IBM POWER Virtual FC Client
 
@@ -1266,7 +1286,7 @@ source "drivers/scsi/qla4xxx/Kconfig"
 config SCSI_LPFC
 	tristate "Emulex LightPulse Fibre Channel Support"
 	depends on PCI && SCSI
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	select CRC_T10DIF
 	help
           This lpfc driver supports the Emulex LightPulse
@@ -1676,7 +1696,7 @@ config SCSI_SUNESP
 config ZFCP
 	tristate "FCP host bus adapter driver for IBM eServer zSeries"
 	depends on S390 && QDIO && SCSI
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	help
           If you want to access SCSI devices attached to your IBM eServer
           zSeries by means of Fibre Channel interfaces say Y.
@@ -1704,7 +1724,7 @@ config SCSI_PM8001
 config SCSI_BFA_FC
 	tristate "Brocade BFA Fibre Channel Support"
 	depends on PCI && SCSI
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	help
 	  This bfa driver supports all Brocade PCIe FC/FCOE host adapters.
 
diff --git a/drivers/scsi/Makefile b/drivers/scsi/Makefile
index 5f0d299b0093..59f1ce6df2d6 100644
--- a/drivers/scsi/Makefile
+++ b/drivers/scsi/Makefile
@@ -141,6 +141,7 @@ obj-$(CONFIG_SCSI_ESAS2R)	+= esas2r/
 obj-$(CONFIG_SCSI_PMCRAID)	+= pmcraid.o
 obj-$(CONFIG_SCSI_VIRTIO)	+= virtio_scsi.o
 obj-$(CONFIG_VMWARE_PVSCSI)	+= vmw_pvscsi.o
+obj-$(CONFIG_XEN_SCSI_FRONTEND)	+= xen-scsifront.o
 obj-$(CONFIG_HYPERV_STORAGE)	+= hv_storvsc.o
 
 obj-$(CONFIG_ARM)		+= arm/
diff --git a/drivers/scsi/aacraid/linit.c b/drivers/scsi/aacraid/linit.c
index 63f576c9300a..a759cb2d4b15 100644
--- a/drivers/scsi/aacraid/linit.c
+++ b/drivers/scsi/aacraid/linit.c
@@ -1152,6 +1152,7 @@ static int aac_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	shost->irq = pdev->irq;
 	shost->unique_id = unique_id;
 	shost->max_cmd_len = 16;
+	shost->use_cmd_list = 1;
 
 	aac = (struct aac_dev *)shost->hostdata;
 	aac->base_start = pci_resource_start(pdev, 0);
diff --git a/drivers/scsi/aic7xxx/Kconfig.aic79xx b/drivers/scsi/aic7xxx/Kconfig.aic79xx
index 6739069477de..3b3d599103f8 100644
--- a/drivers/scsi/aic7xxx/Kconfig.aic79xx
+++ b/drivers/scsi/aic7xxx/Kconfig.aic79xx
@@ -70,7 +70,7 @@ config AIC79XX_DEBUG_MASK
 	default "0"
 	help
 	Bit mask of debug options that is only valid if the
-	CONFIG_AIC79XX_DEBUG_ENBLE option is enabled.  The bits in this mask
+	CONFIG_AIC79XX_DEBUG_ENABLE option is enabled.  The bits in this mask
 	are defined in the drivers/scsi/aic7xxx/aic79xx.h - search for the
 	variable ahd_debug in that file to find them.
 
diff --git a/drivers/scsi/arcmsr/arcmsr.h b/drivers/scsi/arcmsr/arcmsr.h
index 77b26f5b9c33..3bcaaac0ae4b 100644
--- a/drivers/scsi/arcmsr/arcmsr.h
+++ b/drivers/scsi/arcmsr/arcmsr.h
@@ -45,13 +45,14 @@
 #include <linux/interrupt.h>
 struct device_attribute;
 /*The limit of outstanding scsi command that firmware can handle*/
-#define ARCMSR_MAX_OUTSTANDING_CMD						256
 #ifdef CONFIG_XEN
 	#define ARCMSR_MAX_FREECCB_NUM	160
+#define ARCMSR_MAX_OUTSTANDING_CMD	155
 #else
 	#define ARCMSR_MAX_FREECCB_NUM	320
+#define ARCMSR_MAX_OUTSTANDING_CMD	255
 #endif
-#define ARCMSR_DRIVER_VERSION		     "Driver Version 1.20.00.15 2010/08/05"
+#define ARCMSR_DRIVER_VERSION		"v1.30.00.04-20140919"
 #define ARCMSR_SCSI_INITIATOR_ID						255
 #define ARCMSR_MAX_XFER_SECTORS							512
 #define ARCMSR_MAX_XFER_SECTORS_B						4096
@@ -62,11 +63,17 @@ struct device_attribute;
 #define ARCMSR_MAX_QBUFFER							4096
 #define ARCMSR_DEFAULT_SG_ENTRIES						38
 #define ARCMSR_MAX_HBB_POSTQUEUE						264
+#define ARCMSR_MAX_ARC1214_POSTQUEUE	256
+#define ARCMSR_MAX_ARC1214_DONEQUEUE	257
 #define ARCMSR_MAX_XFER_LEN							0x26000 /* 152K */
 #define ARCMSR_CDB_SG_PAGE_LENGTH						256 
+#define ARCMST_NUM_MSIX_VECTORS		4
 #ifndef PCI_DEVICE_ID_ARECA_1880
 #define PCI_DEVICE_ID_ARECA_1880 0x1880
  #endif
+#ifndef PCI_DEVICE_ID_ARECA_1214
+	#define PCI_DEVICE_ID_ARECA_1214	0x1214
+#endif
 /*
 **********************************************************************************
 **
@@ -100,10 +107,11 @@ struct CMD_MESSAGE
 **        IOP Message Transfer Data for user space
 *******************************************************************************
 */
+#define	ARCMSR_API_DATA_BUFLEN	1032
 struct CMD_MESSAGE_FIELD
 {
     struct CMD_MESSAGE			cmdmessage;
-    uint8_t				messagedatabuffer[1032];
+    uint8_t				messagedatabuffer[ARCMSR_API_DATA_BUFLEN];
 };
 /* IOP message transfer */
 #define ARCMSR_MESSAGE_FAIL			0x0001
@@ -337,6 +345,56 @@ struct FIRMWARE_INFO
 #define ARCMSR_HBCMU_MESSAGE_FIRMWARE_OK			0x80000000
 /*
 *******************************************************************************
+**                SPEC. for Areca Type D adapter
+*******************************************************************************
+*/
+#define ARCMSR_ARC1214_CHIP_ID				0x00004
+#define ARCMSR_ARC1214_CPU_MEMORY_CONFIGURATION		0x00008
+#define ARCMSR_ARC1214_I2_HOST_INTERRUPT_MASK		0x00034
+#define ARCMSR_ARC1214_SAMPLE_RESET			0x00100
+#define ARCMSR_ARC1214_RESET_REQUEST			0x00108
+#define ARCMSR_ARC1214_MAIN_INTERRUPT_STATUS		0x00200
+#define ARCMSR_ARC1214_PCIE_F0_INTERRUPT_ENABLE		0x0020C
+#define ARCMSR_ARC1214_INBOUND_MESSAGE0			0x00400
+#define ARCMSR_ARC1214_INBOUND_MESSAGE1			0x00404
+#define ARCMSR_ARC1214_OUTBOUND_MESSAGE0		0x00420
+#define ARCMSR_ARC1214_OUTBOUND_MESSAGE1		0x00424
+#define ARCMSR_ARC1214_INBOUND_DOORBELL			0x00460
+#define ARCMSR_ARC1214_OUTBOUND_DOORBELL		0x00480
+#define ARCMSR_ARC1214_OUTBOUND_DOORBELL_ENABLE		0x00484
+#define ARCMSR_ARC1214_INBOUND_LIST_BASE_LOW		0x01000
+#define ARCMSR_ARC1214_INBOUND_LIST_BASE_HIGH		0x01004
+#define ARCMSR_ARC1214_INBOUND_LIST_WRITE_POINTER	0x01018
+#define ARCMSR_ARC1214_OUTBOUND_LIST_BASE_LOW		0x01060
+#define ARCMSR_ARC1214_OUTBOUND_LIST_BASE_HIGH		0x01064
+#define ARCMSR_ARC1214_OUTBOUND_LIST_COPY_POINTER	0x0106C
+#define ARCMSR_ARC1214_OUTBOUND_LIST_READ_POINTER	0x01070
+#define ARCMSR_ARC1214_OUTBOUND_INTERRUPT_CAUSE		0x01088
+#define ARCMSR_ARC1214_OUTBOUND_INTERRUPT_ENABLE	0x0108C
+#define ARCMSR_ARC1214_MESSAGE_WBUFFER			0x02000
+#define ARCMSR_ARC1214_MESSAGE_RBUFFER			0x02100
+#define ARCMSR_ARC1214_MESSAGE_RWBUFFER			0x02200
+/* Host Interrupt Mask */
+#define ARCMSR_ARC1214_ALL_INT_ENABLE			0x00001010
+#define ARCMSR_ARC1214_ALL_INT_DISABLE			0x00000000
+/* Host Interrupt Status */
+#define ARCMSR_ARC1214_OUTBOUND_DOORBELL_ISR		0x00001000
+#define ARCMSR_ARC1214_OUTBOUND_POSTQUEUE_ISR		0x00000010
+/* DoorBell*/
+#define ARCMSR_ARC1214_DRV2IOP_DATA_IN_READY		0x00000001
+#define ARCMSR_ARC1214_DRV2IOP_DATA_OUT_READ		0x00000002
+/*inbound message 0 ready*/
+#define ARCMSR_ARC1214_IOP2DRV_DATA_WRITE_OK		0x00000001
+/*outbound DATA WRITE isr door bell clear*/
+#define ARCMSR_ARC1214_IOP2DRV_DATA_READ_OK		0x00000002
+/*outbound message 0 ready*/
+#define ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE		0x02000000
+/*outbound message cmd isr door bell clear*/
+/*ARCMSR_HBAMU_MESSAGE_FIRMWARE_OK*/
+#define ARCMSR_ARC1214_MESSAGE_FIRMWARE_OK		0x80000000
+#define ARCMSR_ARC1214_OUTBOUND_LIST_INTERRUPT_CLEAR	0x00000001
+/*
+*******************************************************************************
 **    ARECA SCSI COMMAND DESCRIPTOR BLOCK size 0x1F8 (504)
 *******************************************************************************
 */
@@ -357,7 +415,7 @@ struct ARCMSR_CDB
 #define ARCMSR_CDB_FLAG_ORDEREDQ           0x10
 
 	uint8_t							msgPages;
-	uint32_t						Context;
+	uint32_t						msgContext;
 	uint32_t						DataLength;
 	uint8_t							Cdb[16];
 	uint8_t							DeviceStatus;
@@ -494,6 +552,56 @@ struct MessageUnit_C{
 	uint32_t	msgcode_rwbuffer[256];			/*2200 23FF*/
 };
 /*
+*********************************************************************
+**     Messaging Unit (MU) of Type D processor
+*********************************************************************
+*/
+struct InBound_SRB {
+	uint32_t addressLow; /* pointer to SRB block */
+	uint32_t addressHigh;
+	uint32_t length; /* in DWORDs */
+	uint32_t reserved0;
+};
+
+struct OutBound_SRB {
+	uint32_t addressLow; /* pointer to SRB block */
+	uint32_t addressHigh;
+};
+
+struct MessageUnit_D {
+	struct InBound_SRB	post_qbuffer[ARCMSR_MAX_ARC1214_POSTQUEUE];
+	volatile struct OutBound_SRB
+				done_qbuffer[ARCMSR_MAX_ARC1214_DONEQUEUE];
+	u16 postq_index;
+	volatile u16 doneq_index;
+	u32 __iomem *chip_id;			/* 0x00004 */
+	u32 __iomem *cpu_mem_config;		/* 0x00008 */
+	u32 __iomem *i2o_host_interrupt_mask;	/* 0x00034 */
+	u32 __iomem *sample_at_reset;		/* 0x00100 */
+	u32 __iomem *reset_request;		/* 0x00108 */
+	u32 __iomem *host_int_status;		/* 0x00200 */
+	u32 __iomem *pcief0_int_enable;		/* 0x0020C */
+	u32 __iomem *inbound_msgaddr0;		/* 0x00400 */
+	u32 __iomem *inbound_msgaddr1;		/* 0x00404 */
+	u32 __iomem *outbound_msgaddr0;		/* 0x00420 */
+	u32 __iomem *outbound_msgaddr1;		/* 0x00424 */
+	u32 __iomem *inbound_doorbell;		/* 0x00460 */
+	u32 __iomem *outbound_doorbell;		/* 0x00480 */
+	u32 __iomem *outbound_doorbell_enable;	/* 0x00484 */
+	u32 __iomem *inboundlist_base_low;	/* 0x01000 */
+	u32 __iomem *inboundlist_base_high;	/* 0x01004 */
+	u32 __iomem *inboundlist_write_pointer;	/* 0x01018 */
+	u32 __iomem *outboundlist_base_low;	/* 0x01060 */
+	u32 __iomem *outboundlist_base_high;	/* 0x01064 */
+	u32 __iomem *outboundlist_copy_pointer;	/* 0x0106C */
+	u32 __iomem *outboundlist_read_pointer;	/* 0x01070 0x01072 */
+	u32 __iomem *outboundlist_interrupt_cause;	/* 0x1088 */
+	u32 __iomem *outboundlist_interrupt_enable;	/* 0x108C */
+	u32 __iomem *message_wbuffer;		/* 0x2000 */
+	u32 __iomem *message_rbuffer;		/* 0x2100 */
+	u32 __iomem *msgcode_rwbuffer;		/* 0x2200 */
+};
+/*
 *******************************************************************************
 **                 Adapter Control Block
 *******************************************************************************
@@ -505,19 +613,26 @@ struct AdapterControlBlock
 	#define ACB_ADAPTER_TYPE_B            0x00000002	/* hbb M IOP */
 	#define ACB_ADAPTER_TYPE_C            0x00000004	/* hbc P IOP */
 	#define ACB_ADAPTER_TYPE_D            0x00000008	/* hbd A IOP */
+	u32				roundup_ccbsize;
 	struct pci_dev *		pdev;
 	struct Scsi_Host *		host;
 	unsigned long			vir2phy_offset;
+	struct msix_entry	entries[ARCMST_NUM_MSIX_VECTORS];
 	/* Offset is used in making arc cdb physical to virtual calculations */
 	uint32_t			outbound_int_enable;
 	uint32_t			cdb_phyaddr_hi32;
 	uint32_t			reg_mu_acc_handle0;
 	spinlock_t                      			eh_lock;
 	spinlock_t                      			ccblist_lock;
+	spinlock_t			postq_lock;
+	spinlock_t			doneq_lock;
+	spinlock_t			rqbuffer_lock;
+	spinlock_t			wqbuffer_lock;
 	union {
 		struct MessageUnit_A __iomem *pmuA;
 		struct MessageUnit_B 	*pmuB;
 		struct MessageUnit_C __iomem *pmuC;
+		struct MessageUnit_D 	*pmuD;
 	};
 	/* message unit ATU inbound base address0 */
 	void __iomem *mem_base0;
@@ -544,6 +659,8 @@ struct AdapterControlBlock
 	/* iop init */
 	#define ACB_F_ABORT				0x0200
 	#define ACB_F_FIRMWARE_TRAP           		0x0400
+	#define ACB_F_MSI_ENABLED		0x1000
+	#define ACB_F_MSIX_ENABLED		0x2000
 	struct CommandControlBlock *			pccb_pool[ARCMSR_MAX_FREECCB_NUM];
 	/* used for memory free */
 	struct list_head		ccb_free_list;
@@ -557,19 +674,20 @@ struct AdapterControlBlock
 	/* dma_coherent used for memory free */
 	dma_addr_t			dma_coherent_handle;
 	/* dma_coherent_handle used for memory free */
-	dma_addr_t				dma_coherent_handle_hbb_mu;
+	dma_addr_t				dma_coherent_handle2;
+	void				*dma_coherent2;
 	unsigned int				uncache_size;
 	uint8_t				rqbuffer[ARCMSR_MAX_QBUFFER];
 	/* data collection buffer for read from 80331 */
-	int32_t				rqbuf_firstindex;
+	int32_t				rqbuf_getIndex;
 	/* first of read buffer  */
-	int32_t				rqbuf_lastindex;
+	int32_t				rqbuf_putIndex;
 	/* last of read buffer   */
 	uint8_t				wqbuffer[ARCMSR_MAX_QBUFFER];
 	/* data collection buffer for write to 80331  */
-	int32_t				wqbuf_firstindex;
+	int32_t				wqbuf_getIndex;
 	/* first of write buffer */
-	int32_t				wqbuf_lastindex;
+	int32_t				wqbuf_putIndex;
 	/* last of write buffer  */
 	uint8_t				devstate[ARCMSR_MAX_TARGETID][ARCMSR_MAX_TARGETLUN];
 	/* id0 ..... id15, lun0...lun7 */
@@ -594,6 +712,8 @@ struct AdapterControlBlock
 				#define	FW_DEADLOCK	0x0010
 	atomic_t 			rq_map_token;
 	atomic_t			ante_token_value;
+	uint32_t	maxOutstanding;
+	int		msix_vector_count;
 };/* HW_DEVICE_EXTENSION */
 /*
 *******************************************************************************
@@ -606,7 +726,7 @@ struct CommandControlBlock{
 	struct list_head		list;				/*x32: 8byte, x64: 16byte*/
 	struct scsi_cmnd		*pcmd;				/*8 bytes pointer of linux scsi command */
 	struct AdapterControlBlock	*acb;				/*x32: 4byte, x64: 8byte*/
-	uint32_t			cdb_phyaddr_pattern;		/*x32: 4byte, x64: 4byte*/
+	uint32_t			cdb_phyaddr;			/*x32: 4byte, x64: 4byte*/
 	uint32_t			arc_cdb_size;			/*x32:4byte,x64:4byte*/
 	uint16_t			ccb_flags;			/*x32: 2byte, x64: 2byte*/
 	#define			CCB_FLAG_READ			0x0000
@@ -684,8 +804,10 @@ struct SENSE_DATA
 #define     ARCMSR_MU_OUTBOUND_MESSAGE0_INTMASKENABLE               0x01
 #define     ARCMSR_MU_OUTBOUND_ALL_INTMASKENABLE                    0x1F
 
-extern void arcmsr_post_ioctldata2iop(struct AdapterControlBlock *);
-extern void arcmsr_iop_message_read(struct AdapterControlBlock *);
+extern void arcmsr_write_ioctldata2iop(struct AdapterControlBlock *);
+extern uint32_t arcmsr_Read_iop_rqbuffer_data(struct AdapterControlBlock *,
+	struct QBUFFER __iomem *);
+extern void arcmsr_clear_iop2drv_rqueue_buffer(struct AdapterControlBlock *);
 extern struct QBUFFER __iomem *arcmsr_get_iop_rqbuffer(struct AdapterControlBlock *);
 extern struct device_attribute *arcmsr_host_attrs[];
 extern int arcmsr_alloc_sysfs_attr(struct AdapterControlBlock *);
diff --git a/drivers/scsi/arcmsr/arcmsr_attr.c b/drivers/scsi/arcmsr/arcmsr_attr.c
index acdae33de521..9c86481f779f 100644
--- a/drivers/scsi/arcmsr/arcmsr_attr.c
+++ b/drivers/scsi/arcmsr/arcmsr_attr.c
@@ -50,6 +50,7 @@
 #include <linux/errno.h>
 #include <linux/delay.h>
 #include <linux/pci.h>
+#include <linux/circ_buf.h>
 
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_device.h>
@@ -68,42 +69,42 @@ static ssize_t arcmsr_sysfs_iop_message_read(struct file *filp,
 	struct device *dev = container_of(kobj,struct device,kobj);
 	struct Scsi_Host *host = class_to_shost(dev);
 	struct AdapterControlBlock *acb = (struct AdapterControlBlock *) host->hostdata;
-	uint8_t *pQbuffer,*ptmpQbuffer;
+	uint8_t *ptmpQbuffer;
 	int32_t allxfer_len = 0;
+	unsigned long flags;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
 
 	/* do message unit read. */
 	ptmpQbuffer = (uint8_t *)buf;
-	while ((acb->rqbuf_firstindex != acb->rqbuf_lastindex)
-		&& (allxfer_len < 1031)) {
-		pQbuffer = &acb->rqbuffer[acb->rqbuf_firstindex];
-		memcpy(ptmpQbuffer, pQbuffer, 1);
-		acb->rqbuf_firstindex++;
-		acb->rqbuf_firstindex %= ARCMSR_MAX_QBUFFER;
-		ptmpQbuffer++;
-		allxfer_len++;
+	spin_lock_irqsave(&acb->rqbuffer_lock, flags);
+	if (acb->rqbuf_getIndex != acb->rqbuf_putIndex) {
+		unsigned int tail = acb->rqbuf_getIndex;
+		unsigned int head = acb->rqbuf_putIndex;
+		unsigned int cnt_to_end = CIRC_CNT_TO_END(head, tail, ARCMSR_MAX_QBUFFER);
+
+		allxfer_len = CIRC_CNT(head, tail, ARCMSR_MAX_QBUFFER);
+		if (allxfer_len > ARCMSR_API_DATA_BUFLEN)
+			allxfer_len = ARCMSR_API_DATA_BUFLEN;
+
+		if (allxfer_len <= cnt_to_end)
+			memcpy(ptmpQbuffer, acb->rqbuffer + tail, allxfer_len);
+		else {
+			memcpy(ptmpQbuffer, acb->rqbuffer + tail, cnt_to_end);
+			memcpy(ptmpQbuffer + cnt_to_end, acb->rqbuffer, allxfer_len - cnt_to_end);
+		}
+		acb->rqbuf_getIndex = (acb->rqbuf_getIndex + allxfer_len) % ARCMSR_MAX_QBUFFER;
 	}
 	if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
 		struct QBUFFER __iomem *prbuffer;
-		uint8_t __iomem *iop_data;
-		int32_t iop_len;
-
 		acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
 		prbuffer = arcmsr_get_iop_rqbuffer(acb);
-		iop_data = prbuffer->data;
-		iop_len = readl(&prbuffer->data_len);
-		while (iop_len > 0) {
-			acb->rqbuffer[acb->rqbuf_lastindex] = readb(iop_data);
-			acb->rqbuf_lastindex++;
-			acb->rqbuf_lastindex %= ARCMSR_MAX_QBUFFER;
-			iop_data++;
-			iop_len--;
-		}
-		arcmsr_iop_message_read(acb);
+		if (arcmsr_Read_iop_rqbuffer_data(acb, prbuffer) == 0)
+			acb->acb_flags |= ACB_F_IOPDATA_OVERFLOW;
 	}
-	return (allxfer_len);
+	spin_unlock_irqrestore(&acb->rqbuffer_lock, flags);
+	return allxfer_len;
 }
 
 static ssize_t arcmsr_sysfs_iop_message_write(struct file *filp,
@@ -115,43 +116,42 @@ static ssize_t arcmsr_sysfs_iop_message_write(struct file *filp,
 	struct device *dev = container_of(kobj,struct device,kobj);
 	struct Scsi_Host *host = class_to_shost(dev);
 	struct AdapterControlBlock *acb = (struct AdapterControlBlock *) host->hostdata;
-	int32_t my_empty_len, user_len, wqbuf_firstindex, wqbuf_lastindex;
+	int32_t user_len, cnt2end;
 	uint8_t *pQbuffer, *ptmpuserbuffer;
+	unsigned long flags;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
-	if (count > 1032)
+	if (count > ARCMSR_API_DATA_BUFLEN)
 		return -EINVAL;
 	/* do message unit write. */
 	ptmpuserbuffer = (uint8_t *)buf;
 	user_len = (int32_t)count;
-	wqbuf_lastindex = acb->wqbuf_lastindex;
-	wqbuf_firstindex = acb->wqbuf_firstindex;
-	if (wqbuf_lastindex != wqbuf_firstindex) {
-		arcmsr_post_ioctldata2iop(acb);
+	spin_lock_irqsave(&acb->wqbuffer_lock, flags);
+	if (acb->wqbuf_putIndex != acb->wqbuf_getIndex) {
+		arcmsr_write_ioctldata2iop(acb);
+		spin_unlock_irqrestore(&acb->wqbuffer_lock, flags);
 		return 0;	/*need retry*/
 	} else {
-		my_empty_len = (wqbuf_firstindex-wqbuf_lastindex - 1)
-				&(ARCMSR_MAX_QBUFFER - 1);
-		if (my_empty_len >= user_len) {
-			while (user_len > 0) {
-				pQbuffer =
-				&acb->wqbuffer[acb->wqbuf_lastindex];
-				memcpy(pQbuffer, ptmpuserbuffer, 1);
-				acb->wqbuf_lastindex++;
-				acb->wqbuf_lastindex %= ARCMSR_MAX_QBUFFER;
-				ptmpuserbuffer++;
-				user_len--;
-			}
-			if (acb->acb_flags & ACB_F_MESSAGE_WQBUFFER_CLEARED) {
-				acb->acb_flags &=
-					~ACB_F_MESSAGE_WQBUFFER_CLEARED;
-				arcmsr_post_ioctldata2iop(acb);
-			}
-			return count;
-		} else {
-			return 0;	/*need retry*/
+		pQbuffer = &acb->wqbuffer[acb->wqbuf_putIndex];
+		cnt2end = ARCMSR_MAX_QBUFFER - acb->wqbuf_putIndex;
+		if (user_len > cnt2end) {
+			memcpy(pQbuffer, ptmpuserbuffer, cnt2end);
+			ptmpuserbuffer += cnt2end;
+			user_len -= cnt2end;
+			acb->wqbuf_putIndex = 0;
+			pQbuffer = acb->wqbuffer;
+		}
+		memcpy(pQbuffer, ptmpuserbuffer, user_len);
+		acb->wqbuf_putIndex += user_len;
+		acb->wqbuf_putIndex %= ARCMSR_MAX_QBUFFER;
+		if (acb->acb_flags & ACB_F_MESSAGE_WQBUFFER_CLEARED) {
+			acb->acb_flags &=
+				~ACB_F_MESSAGE_WQBUFFER_CLEARED;
+			arcmsr_write_ioctldata2iop(acb);
 		}
+		spin_unlock_irqrestore(&acb->wqbuffer_lock, flags);
+		return count;
 	}
 }
 
@@ -165,22 +165,24 @@ static ssize_t arcmsr_sysfs_iop_message_clear(struct file *filp,
 	struct Scsi_Host *host = class_to_shost(dev);
 	struct AdapterControlBlock *acb = (struct AdapterControlBlock *) host->hostdata;
 	uint8_t *pQbuffer;
+	unsigned long flags;
 
 	if (!capable(CAP_SYS_ADMIN))
 		return -EACCES;
 
-	if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
-		acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
-		arcmsr_iop_message_read(acb);
-	}
+	arcmsr_clear_iop2drv_rqueue_buffer(acb);
 	acb->acb_flags |=
 		(ACB_F_MESSAGE_WQBUFFER_CLEARED
 		| ACB_F_MESSAGE_RQBUFFER_CLEARED
 		| ACB_F_MESSAGE_WQBUFFER_READED);
-	acb->rqbuf_firstindex = 0;
-	acb->rqbuf_lastindex = 0;
-	acb->wqbuf_firstindex = 0;
-	acb->wqbuf_lastindex = 0;
+	spin_lock_irqsave(&acb->rqbuffer_lock, flags);
+	acb->rqbuf_getIndex = 0;
+	acb->rqbuf_putIndex = 0;
+	spin_unlock_irqrestore(&acb->rqbuffer_lock, flags);
+	spin_lock_irqsave(&acb->wqbuffer_lock, flags);
+	acb->wqbuf_getIndex = 0;
+	acb->wqbuf_putIndex = 0;
+	spin_unlock_irqrestore(&acb->wqbuffer_lock, flags);
 	pQbuffer = acb->rqbuffer;
 	memset(pQbuffer, 0, sizeof (struct QBUFFER));
 	pQbuffer = acb->wqbuffer;
@@ -193,7 +195,7 @@ static struct bin_attribute arcmsr_sysfs_message_read_attr = {
 		.name = "mu_read",
 		.mode = S_IRUSR ,
 	},
-	.size = 1032,
+	.size = ARCMSR_API_DATA_BUFLEN,
 	.read = arcmsr_sysfs_iop_message_read,
 };
 
@@ -202,7 +204,7 @@ static struct bin_attribute arcmsr_sysfs_message_write_attr = {
 		.name = "mu_write",
 		.mode = S_IWUSR,
 	},
-	.size = 1032,
+	.size = ARCMSR_API_DATA_BUFLEN,
 	.write = arcmsr_sysfs_iop_message_write,
 };
 
diff --git a/drivers/scsi/arcmsr/arcmsr_hba.c b/drivers/scsi/arcmsr/arcmsr_hba.c
index b13764ca23fd..0b44fb5ee485 100644
--- a/drivers/scsi/arcmsr/arcmsr_hba.c
+++ b/drivers/scsi/arcmsr/arcmsr_hba.c
@@ -2,11 +2,10 @@
 *******************************************************************************
 **        O.S   : Linux
 **   FILE NAME  : arcmsr_hba.c
-**        BY    : Nick Cheng
-**   Description: SCSI RAID Device Driver for
-**                ARECA RAID Host adapter
+**        BY    : Nick Cheng, C.L. Huang
+**   Description: SCSI RAID Device Driver for Areca RAID Controller
 *******************************************************************************
-** Copyright (C) 2002 - 2005, Areca Technology Corporation All rights reserved
+** Copyright (C) 2002 - 2014, Areca Technology Corporation All rights reserved
 **
 **     Web site: www.areca.com.tw
 **       E-mail: support@areca.com.tw
@@ -59,6 +58,7 @@
 #include <linux/slab.h>
 #include <linux/pci.h>
 #include <linux/aer.h>
+#include <linux/circ_buf.h>
 #include <asm/dma.h>
 #include <asm/io.h>
 #include <asm/uaccess.h>
@@ -70,15 +70,15 @@
 #include <scsi/scsi_transport.h>
 #include <scsi/scsicam.h>
 #include "arcmsr.h"
-MODULE_AUTHOR("Nick Cheng <support@areca.com.tw>");
-MODULE_DESCRIPTION("ARECA (ARC11xx/12xx/16xx/1880) SATA/SAS RAID Host Bus Adapter");
+MODULE_AUTHOR("Nick Cheng, C.L. Huang <support@areca.com.tw>");
+MODULE_DESCRIPTION("Areca ARC11xx/12xx/16xx/188x SAS/SATA RAID Controller Driver");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_VERSION(ARCMSR_DRIVER_VERSION);
 
 #define	ARCMSR_SLEEPTIME	10
 #define	ARCMSR_RETRYCOUNT	12
 
-wait_queue_head_t wait_q;
+static wait_queue_head_t wait_q;
 static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb,
 					struct scsi_cmnd *cmd);
 static int arcmsr_iop_confirm(struct AdapterControlBlock *acb);
@@ -89,25 +89,31 @@ static int arcmsr_bios_param(struct scsi_device *sdev,
 static int arcmsr_queue_command(struct Scsi_Host *h, struct scsi_cmnd *cmd);
 static int arcmsr_probe(struct pci_dev *pdev,
 				const struct pci_device_id *id);
+static int arcmsr_suspend(struct pci_dev *pdev, pm_message_t state);
+static int arcmsr_resume(struct pci_dev *pdev);
 static void arcmsr_remove(struct pci_dev *pdev);
 static void arcmsr_shutdown(struct pci_dev *pdev);
 static void arcmsr_iop_init(struct AdapterControlBlock *acb);
 static void arcmsr_free_ccb_pool(struct AdapterControlBlock *acb);
 static u32 arcmsr_disable_outbound_ints(struct AdapterControlBlock *acb);
+static void arcmsr_enable_outbound_ints(struct AdapterControlBlock *acb,
+	u32 intmask_org);
 static void arcmsr_stop_adapter_bgrb(struct AdapterControlBlock *acb);
-static void arcmsr_flush_hba_cache(struct AdapterControlBlock *acb);
-static void arcmsr_flush_hbb_cache(struct AdapterControlBlock *acb);
+static void arcmsr_hbaA_flush_cache(struct AdapterControlBlock *acb);
+static void arcmsr_hbaB_flush_cache(struct AdapterControlBlock *acb);
 static void arcmsr_request_device_map(unsigned long pacb);
-static void arcmsr_request_hba_device_map(struct AdapterControlBlock *acb);
-static void arcmsr_request_hbb_device_map(struct AdapterControlBlock *acb);
-static void arcmsr_request_hbc_device_map(struct AdapterControlBlock *acb);
+static void arcmsr_hbaA_request_device_map(struct AdapterControlBlock *acb);
+static void arcmsr_hbaB_request_device_map(struct AdapterControlBlock *acb);
+static void arcmsr_hbaC_request_device_map(struct AdapterControlBlock *acb);
 static void arcmsr_message_isr_bh_fn(struct work_struct *work);
 static bool arcmsr_get_firmware_spec(struct AdapterControlBlock *acb);
 static void arcmsr_start_adapter_bgrb(struct AdapterControlBlock *acb);
-static void arcmsr_hbc_message_isr(struct AdapterControlBlock *pACB);
+static void arcmsr_hbaC_message_isr(struct AdapterControlBlock *pACB);
+static void arcmsr_hbaD_message_isr(struct AdapterControlBlock *acb);
 static void arcmsr_hardware_reset(struct AdapterControlBlock *acb);
 static const char *arcmsr_info(struct Scsi_Host *);
 static irqreturn_t arcmsr_interrupt(struct AdapterControlBlock *acb);
+static void arcmsr_free_irq(struct pci_dev *, struct AdapterControlBlock *);
 static int arcmsr_adjust_disk_queue_depth(struct scsi_device *sdev,
 					  int queue_depth, int reason)
 {
@@ -122,15 +128,14 @@ static int arcmsr_adjust_disk_queue_depth(struct scsi_device *sdev,
 
 static struct scsi_host_template arcmsr_scsi_host_template = {
 	.module			= THIS_MODULE,
-	.name			= "ARCMSR ARECA SATA/SAS RAID Controller"
-				ARCMSR_DRIVER_VERSION,
+	.name			= "Areca SAS/SATA RAID driver",
 	.info			= arcmsr_info,
 	.queuecommand		= arcmsr_queue_command,
 	.eh_abort_handler		= arcmsr_abort,
 	.eh_bus_reset_handler	= arcmsr_bus_reset,
 	.bios_param		= arcmsr_bios_param,
 	.change_queue_depth	= arcmsr_adjust_disk_queue_depth,
-	.can_queue		= ARCMSR_MAX_FREECCB_NUM,
+	.can_queue		= ARCMSR_MAX_OUTSTANDING_CMD,
 	.this_id			= ARCMSR_SCSI_INITIATOR_ID,
 	.sg_tablesize	        	= ARCMSR_DEFAULT_SG_ENTRIES, 
 	.max_sectors    	    	= ARCMSR_MAX_XFER_SECTORS_C, 
@@ -139,34 +144,59 @@ static struct scsi_host_template arcmsr_scsi_host_template = {
 	.shost_attrs		= arcmsr_host_attrs,
 	.no_write_same		= 1,
 };
+
 static struct pci_device_id arcmsr_device_id_table[] = {
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1110)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1120)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1130)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1160)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1170)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1200)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1201)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1202)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1210)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1220)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1230)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1260)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1270)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1280)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1380)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1381)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1680)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1681)},
-	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1880)},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1110),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1120),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1130),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1160),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1170),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1200),
+		.driver_data = ACB_ADAPTER_TYPE_B},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1201),
+		.driver_data = ACB_ADAPTER_TYPE_B},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1202),
+		.driver_data = ACB_ADAPTER_TYPE_B},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1210),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1214),
+		.driver_data = ACB_ADAPTER_TYPE_D},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1220),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1230),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1260),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1270),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1280),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1380),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1381),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1680),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1681),
+		.driver_data = ACB_ADAPTER_TYPE_A},
+	{PCI_DEVICE(PCI_VENDOR_ID_ARECA, PCI_DEVICE_ID_ARECA_1880),
+		.driver_data = ACB_ADAPTER_TYPE_C},
 	{0, 0}, /* Terminating entry */
 };
 MODULE_DEVICE_TABLE(pci, arcmsr_device_id_table);
+
 static struct pci_driver arcmsr_pci_driver = {
 	.name			= "arcmsr",
 	.id_table			= arcmsr_device_id_table,
 	.probe			= arcmsr_probe,
 	.remove			= arcmsr_remove,
+	.suspend		= arcmsr_suspend,
+	.resume			= arcmsr_resume,
 	.shutdown		= arcmsr_shutdown,
 };
 /*
@@ -174,16 +204,14 @@ static struct pci_driver arcmsr_pci_driver = {
 ****************************************************************************
 */
 
-static void arcmsr_free_hbb_mu(struct AdapterControlBlock *acb)
+static void arcmsr_free_mu(struct AdapterControlBlock *acb)
 {
 	switch (acb->adapter_type) {
-	case ACB_ADAPTER_TYPE_A:
-	case ACB_ADAPTER_TYPE_C:
+	case ACB_ADAPTER_TYPE_B:
+	case ACB_ADAPTER_TYPE_D: {
+		dma_free_coherent(&acb->pdev->dev, acb->roundup_ccbsize,
+			acb->dma_coherent2, acb->dma_coherent_handle2);
 		break;
-	case ACB_ADAPTER_TYPE_B:{
-		dma_free_coherent(&acb->pdev->dev,
-			sizeof(struct MessageUnit_B),
-			acb->pmuB, acb->dma_coherent_handle_hbb_mu);
 	}
 	}
 }
@@ -229,6 +257,25 @@ static bool arcmsr_remap_pciregion(struct AdapterControlBlock *acb)
 		}
 		break;
 	}
+	case ACB_ADAPTER_TYPE_D: {
+		void __iomem *mem_base0;
+		unsigned long addr, range, flags;
+
+		addr = (unsigned long)pci_resource_start(pdev, 0);
+		range = pci_resource_len(pdev, 0);
+		flags = pci_resource_flags(pdev, 0);
+		if (flags & IORESOURCE_CACHEABLE)
+			mem_base0 = ioremap(addr, range);
+		else
+			mem_base0 = ioremap_nocache(addr, range);
+		if (!mem_base0) {
+			pr_notice("arcmsr%d: memory mapping region fail\n",
+				acb->host->host_no);
+			return false;
+		}
+		acb->mem_base0 = mem_base0;
+		break;
+		}
 	}
 	return true;
 }
@@ -249,6 +296,10 @@ static void arcmsr_unmap_pciregion(struct AdapterControlBlock *acb)
 	case ACB_ADAPTER_TYPE_C:{
 		iounmap(acb->pmuC);
 	}
+	break;
+	case ACB_ADAPTER_TYPE_D:
+		iounmap(acb->mem_base0);
+		break;
 	}
 }
 
@@ -289,27 +340,7 @@ static int arcmsr_bios_param(struct scsi_device *sdev,
 	return 0;
 }
 
-static void arcmsr_define_adapter_type(struct AdapterControlBlock *acb)
-{
-	struct pci_dev *pdev = acb->pdev;
-	u16 dev_id;
-	pci_read_config_word(pdev, PCI_DEVICE_ID, &dev_id);
-	acb->dev_id = dev_id;
-	switch (dev_id) {
-	case 0x1880: {
-		acb->adapter_type = ACB_ADAPTER_TYPE_C;
-		}
-		break;
-	case 0x1201: {
-		acb->adapter_type = ACB_ADAPTER_TYPE_B;
-		}
-		break;
-
-	default: acb->adapter_type = ACB_ADAPTER_TYPE_A;
-	}
-}
-
-static uint8_t arcmsr_hba_wait_msgint_ready(struct AdapterControlBlock *acb)
+static uint8_t arcmsr_hbaA_wait_msgint_ready(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	int i;
@@ -327,7 +358,7 @@ static uint8_t arcmsr_hba_wait_msgint_ready(struct AdapterControlBlock *acb)
 	return false;
 }
 
-static uint8_t arcmsr_hbb_wait_msgint_ready(struct AdapterControlBlock *acb)
+static uint8_t arcmsr_hbaB_wait_msgint_ready(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
 	int i;
@@ -347,9 +378,9 @@ static uint8_t arcmsr_hbb_wait_msgint_ready(struct AdapterControlBlock *acb)
 	return false;
 }
 
-static uint8_t arcmsr_hbc_wait_msgint_ready(struct AdapterControlBlock *pACB)
+static uint8_t arcmsr_hbaC_wait_msgint_ready(struct AdapterControlBlock *pACB)
 {
-	struct MessageUnit_C *phbcmu = (struct MessageUnit_C *)pACB->pmuC;
+	struct MessageUnit_C __iomem *phbcmu = pACB->pmuC;
 	int i;
 
 	for (i = 0; i < 2000; i++) {
@@ -365,13 +396,30 @@ static uint8_t arcmsr_hbc_wait_msgint_ready(struct AdapterControlBlock *pACB)
 	return false;
 }
 
-static void arcmsr_flush_hba_cache(struct AdapterControlBlock *acb)
+static bool arcmsr_hbaD_wait_msgint_ready(struct AdapterControlBlock *pACB)
+{
+	struct MessageUnit_D *reg = pACB->pmuD;
+	int i;
+
+	for (i = 0; i < 2000; i++) {
+		if (readl(reg->outbound_doorbell)
+			& ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE) {
+			writel(ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE,
+				reg->outbound_doorbell);
+			return true;
+		}
+		msleep(10);
+	} /* max 20 seconds */
+	return false;
+}
+
+static void arcmsr_hbaA_flush_cache(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	int retry_count = 30;
 	writel(ARCMSR_INBOUND_MESG0_FLUSH_CACHE, &reg->inbound_msgaddr0);
 	do {
-		if (arcmsr_hba_wait_msgint_ready(acb))
+		if (arcmsr_hbaA_wait_msgint_ready(acb))
 			break;
 		else {
 			retry_count--;
@@ -381,13 +429,13 @@ static void arcmsr_flush_hba_cache(struct AdapterControlBlock *acb)
 	} while (retry_count != 0);
 }
 
-static void arcmsr_flush_hbb_cache(struct AdapterControlBlock *acb)
+static void arcmsr_hbaB_flush_cache(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
 	int retry_count = 30;
 	writel(ARCMSR_MESSAGE_FLUSH_CACHE, reg->drv2iop_doorbell);
 	do {
-		if (arcmsr_hbb_wait_msgint_ready(acb))
+		if (arcmsr_hbaB_wait_msgint_ready(acb))
 			break;
 		else {
 			retry_count--;
@@ -397,14 +445,14 @@ static void arcmsr_flush_hbb_cache(struct AdapterControlBlock *acb)
 	} while (retry_count != 0);
 }
 
-static void arcmsr_flush_hbc_cache(struct AdapterControlBlock *pACB)
+static void arcmsr_hbaC_flush_cache(struct AdapterControlBlock *pACB)
 {
-	struct MessageUnit_C *reg = (struct MessageUnit_C *)pACB->pmuC;
+	struct MessageUnit_C __iomem *reg = pACB->pmuC;
 	int retry_count = 30;/* enlarge wait flush adapter cache time: 10 minute */
 	writel(ARCMSR_INBOUND_MESG0_FLUSH_CACHE, &reg->inbound_msgaddr0);
 	writel(ARCMSR_HBCMU_DRV2IOP_MESSAGE_CMD_DONE, &reg->inbound_doorbell);
 	do {
-		if (arcmsr_hbc_wait_msgint_ready(pACB)) {
+		if (arcmsr_hbaC_wait_msgint_ready(pACB)) {
 			break;
 		} else {
 			retry_count--;
@@ -414,22 +462,44 @@ static void arcmsr_flush_hbc_cache(struct AdapterControlBlock *pACB)
 	} while (retry_count != 0);
 	return;
 }
+
+static void arcmsr_hbaD_flush_cache(struct AdapterControlBlock *pACB)
+{
+	int retry_count = 15;
+	struct MessageUnit_D *reg = pACB->pmuD;
+
+	writel(ARCMSR_INBOUND_MESG0_FLUSH_CACHE, reg->inbound_msgaddr0);
+	do {
+		if (arcmsr_hbaD_wait_msgint_ready(pACB))
+			break;
+
+		retry_count--;
+		pr_notice("arcmsr%d: wait 'flush adapter "
+			"cache' timeout, retry count down = %d\n",
+			pACB->host->host_no, retry_count);
+	} while (retry_count != 0);
+}
+
 static void arcmsr_flush_adapter_cache(struct AdapterControlBlock *acb)
 {
 	switch (acb->adapter_type) {
 
 	case ACB_ADAPTER_TYPE_A: {
-		arcmsr_flush_hba_cache(acb);
+		arcmsr_hbaA_flush_cache(acb);
 		}
 		break;
 
 	case ACB_ADAPTER_TYPE_B: {
-		arcmsr_flush_hbb_cache(acb);
+		arcmsr_hbaB_flush_cache(acb);
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		arcmsr_flush_hbc_cache(acb);
+		arcmsr_hbaC_flush_cache(acb);
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D:
+		arcmsr_hbaD_flush_cache(acb);
+		break;
 	}
 }
 
@@ -473,7 +543,16 @@ static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
 	acb->vir2phy_offset = (unsigned long)dma_coherent - (unsigned long)dma_coherent_handle;
 	for(i = 0; i < ARCMSR_MAX_FREECCB_NUM; i++){
 		cdb_phyaddr = dma_coherent_handle + offsetof(struct CommandControlBlock, arcmsr_cdb);
-		ccb_tmp->cdb_phyaddr_pattern = ((acb->adapter_type == ACB_ADAPTER_TYPE_C) ? cdb_phyaddr : (cdb_phyaddr >> 5));
+		switch (acb->adapter_type) {
+		case ACB_ADAPTER_TYPE_A:
+		case ACB_ADAPTER_TYPE_B:
+			ccb_tmp->cdb_phyaddr = cdb_phyaddr >> 5;
+			break;
+		case ACB_ADAPTER_TYPE_C:
+		case ACB_ADAPTER_TYPE_D:
+			ccb_tmp->cdb_phyaddr = cdb_phyaddr;
+			break;
+		}
 		acb->pccb_pool[i] = ccb_tmp;
 		ccb_tmp->acb = acb;
 		INIT_LIST_HEAD(&ccb_tmp->list);
@@ -486,121 +565,126 @@ static int arcmsr_alloc_ccb_pool(struct AdapterControlBlock *acb)
 
 static void arcmsr_message_isr_bh_fn(struct work_struct *work) 
 {
-	struct AdapterControlBlock *acb = container_of(work,struct AdapterControlBlock, arcmsr_do_message_isr_bh);
+	struct AdapterControlBlock *acb = container_of(work,
+		struct AdapterControlBlock, arcmsr_do_message_isr_bh);
+	char *acb_dev_map = (char *)acb->device_map;
+	uint32_t __iomem *signature = NULL;
+	char __iomem *devicemap = NULL;
+	int target, lun;
+	struct scsi_device *psdev;
+	char diff, temp;
+
 	switch (acb->adapter_type) {
-		case ACB_ADAPTER_TYPE_A: {
+	case ACB_ADAPTER_TYPE_A: {
+		struct MessageUnit_A __iomem *reg  = acb->pmuA;
 
-			struct MessageUnit_A __iomem *reg  = acb->pmuA;
-			char *acb_dev_map = (char *)acb->device_map;
-			uint32_t __iomem *signature = (uint32_t __iomem*) (&reg->message_rwbuffer[0]);
-			char __iomem *devicemap = (char __iomem*) (&reg->message_rwbuffer[21]);
-			int target, lun;
-			struct scsi_device *psdev;
-			char diff;
-
-			atomic_inc(&acb->rq_map_token);
-			if (readl(signature) == ARCMSR_SIGNATURE_GET_CONFIG) {
-				for(target = 0; target < ARCMSR_MAX_TARGETID -1; target++) {
-					diff = (*acb_dev_map)^readb(devicemap);
-					if (diff != 0) {
-						char temp;
-						*acb_dev_map = readb(devicemap);
-						temp =*acb_dev_map;
-						for(lun = 0; lun < ARCMSR_MAX_TARGETLUN; lun++) {
-							if((temp & 0x01)==1 && (diff & 0x01) == 1) {	
-								scsi_add_device(acb->host, 0, target, lun);
-							}else if((temp & 0x01) == 0 && (diff & 0x01) == 1) {
-								psdev = scsi_device_lookup(acb->host, 0, target, lun);
-								if (psdev != NULL ) {
-									scsi_remove_device(psdev);
-									scsi_device_put(psdev);
-								}
-							}
-							temp >>= 1;
-							diff >>= 1;
-						}
-					}
-					devicemap++;
-					acb_dev_map++;
-				}
-			}
-			break;
-		}
+		signature = (uint32_t __iomem *)(&reg->message_rwbuffer[0]);
+		devicemap = (char __iomem *)(&reg->message_rwbuffer[21]);
+		break;
+	}
+	case ACB_ADAPTER_TYPE_B: {
+		struct MessageUnit_B *reg  = acb->pmuB;
 
-		case ACB_ADAPTER_TYPE_B: {
-			struct MessageUnit_B *reg  = acb->pmuB;
-			char *acb_dev_map = (char *)acb->device_map;
-			uint32_t __iomem *signature = (uint32_t __iomem*)(&reg->message_rwbuffer[0]);
-			char __iomem *devicemap = (char __iomem*)(&reg->message_rwbuffer[21]);
-			int target, lun;
-			struct scsi_device *psdev;
-			char diff;
-
-			atomic_inc(&acb->rq_map_token);
-			if (readl(signature) == ARCMSR_SIGNATURE_GET_CONFIG) {
-				for(target = 0; target < ARCMSR_MAX_TARGETID -1; target++) {
-					diff = (*acb_dev_map)^readb(devicemap);
-					if (diff != 0) {
-						char temp;
-						*acb_dev_map = readb(devicemap);
-						temp =*acb_dev_map;
-						for(lun = 0; lun < ARCMSR_MAX_TARGETLUN; lun++) {
-							if((temp & 0x01)==1 && (diff & 0x01) == 1) {	
-								scsi_add_device(acb->host, 0, target, lun);
-							}else if((temp & 0x01) == 0 && (diff & 0x01) == 1) {
-								psdev = scsi_device_lookup(acb->host, 0, target, lun);
-								if (psdev != NULL ) {
-									scsi_remove_device(psdev);
-									scsi_device_put(psdev);
-								}
-							}
-							temp >>= 1;
-							diff >>= 1;
-						}
-					}
-					devicemap++;
-					acb_dev_map++;
-				}
-			}
-		}
+		signature = (uint32_t __iomem *)(&reg->message_rwbuffer[0]);
+		devicemap = (char __iomem *)(&reg->message_rwbuffer[21]);
 		break;
-		case ACB_ADAPTER_TYPE_C: {
-			struct MessageUnit_C *reg  = acb->pmuC;
-			char *acb_dev_map = (char *)acb->device_map;
-			uint32_t __iomem *signature = (uint32_t __iomem *)(&reg->msgcode_rwbuffer[0]);
-			char __iomem *devicemap = (char __iomem *)(&reg->msgcode_rwbuffer[21]);
-			int target, lun;
-			struct scsi_device *psdev;
-			char diff;
-
-			atomic_inc(&acb->rq_map_token);
-			if (readl(signature) == ARCMSR_SIGNATURE_GET_CONFIG) {
-				for (target = 0; target < ARCMSR_MAX_TARGETID - 1; target++) {
-					diff = (*acb_dev_map)^readb(devicemap);
-					if (diff != 0) {
-						char temp;
-						*acb_dev_map = readb(devicemap);
-						temp = *acb_dev_map;
-						for (lun = 0; lun < ARCMSR_MAX_TARGETLUN; lun++) {
-							if ((temp & 0x01) == 1 && (diff & 0x01) == 1) {
-								scsi_add_device(acb->host, 0, target, lun);
-							} else if ((temp & 0x01) == 0 && (diff & 0x01) == 1) {
-								psdev = scsi_device_lookup(acb->host, 0, target, lun);
-								if (psdev != NULL) {
-									scsi_remove_device(psdev);
-									scsi_device_put(psdev);
-								}
-							}
-							temp >>= 1;
-							diff >>= 1;
-						}
+	}
+	case ACB_ADAPTER_TYPE_C: {
+		struct MessageUnit_C __iomem *reg  = acb->pmuC;
+
+		signature = (uint32_t __iomem *)(&reg->msgcode_rwbuffer[0]);
+		devicemap = (char __iomem *)(&reg->msgcode_rwbuffer[21]);
+		break;
+	}
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg  = acb->pmuD;
+
+		signature = (uint32_t __iomem *)(&reg->msgcode_rwbuffer[0]);
+		devicemap = (char __iomem *)(&reg->msgcode_rwbuffer[21]);
+		break;
+	}
+	}
+	atomic_inc(&acb->rq_map_token);
+	if (readl(signature) != ARCMSR_SIGNATURE_GET_CONFIG)
+		return;
+	for (target = 0; target < ARCMSR_MAX_TARGETID - 1;
+		target++) {
+		temp = readb(devicemap);
+		diff = (*acb_dev_map) ^ temp;
+		if (diff != 0) {
+			*acb_dev_map = temp;
+			for (lun = 0; lun < ARCMSR_MAX_TARGETLUN;
+				lun++) {
+				if ((diff & 0x01) == 1 &&
+					(temp & 0x01) == 1) {
+					scsi_add_device(acb->host,
+						0, target, lun);
+				} else if ((diff & 0x01) == 1
+					&& (temp & 0x01) == 0) {
+					psdev = scsi_device_lookup(acb->host,
+						0, target, lun);
+					if (psdev != NULL) {
+						scsi_remove_device(psdev);
+						scsi_device_put(psdev);
 					}
-					devicemap++;
-					acb_dev_map++;
 				}
+				temp >>= 1;
+				diff >>= 1;
 			}
 		}
+		devicemap++;
+		acb_dev_map++;
+	}
+}
+
+static int
+arcmsr_request_irq(struct pci_dev *pdev, struct AdapterControlBlock *acb)
+{
+	int	i, j, r;
+	struct msix_entry entries[ARCMST_NUM_MSIX_VECTORS];
+
+	for (i = 0; i < ARCMST_NUM_MSIX_VECTORS; i++)
+		entries[i].entry = i;
+	r = pci_enable_msix_range(pdev, entries, 1, ARCMST_NUM_MSIX_VECTORS);
+	if (r < 0)
+		goto msi_int;
+	acb->msix_vector_count = r;
+	for (i = 0; i < r; i++) {
+		if (request_irq(entries[i].vector,
+			arcmsr_do_interrupt, 0, "arcmsr", acb)) {
+			pr_warn("arcmsr%d: request_irq =%d failed!\n",
+				acb->host->host_no, entries[i].vector);
+			for (j = 0 ; j < i ; j++)
+				free_irq(entries[j].vector, acb);
+			pci_disable_msix(pdev);
+			goto msi_int;
+		}
+		acb->entries[i] = entries[i];
+	}
+	acb->acb_flags |= ACB_F_MSIX_ENABLED;
+	pr_info("arcmsr%d: msi-x enabled\n", acb->host->host_no);
+	return SUCCESS;
+msi_int:
+	if (pci_enable_msi_exact(pdev, 1) < 0)
+		goto legacy_int;
+	if (request_irq(pdev->irq, arcmsr_do_interrupt,
+		IRQF_SHARED, "arcmsr", acb)) {
+		pr_warn("arcmsr%d: request_irq =%d failed!\n",
+			acb->host->host_no, pdev->irq);
+		pci_disable_msi(pdev);
+		goto legacy_int;
+	}
+	acb->acb_flags |= ACB_F_MSI_ENABLED;
+	pr_info("arcmsr%d: msi enabled\n", acb->host->host_no);
+	return SUCCESS;
+legacy_int:
+	if (request_irq(pdev->irq, arcmsr_do_interrupt,
+		IRQF_SHARED, "arcmsr", acb)) {
+		pr_warn("arcmsr%d: request_irq = %d failed!\n",
+			acb->host->host_no, pdev->irq);
+		return FAILED;
 	}
+	return SUCCESS;
 }
 
 static int arcmsr_probe(struct pci_dev *pdev, const struct pci_device_id *id)
@@ -637,7 +721,7 @@ static int arcmsr_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	host->max_lun = ARCMSR_MAX_TARGETLUN;
 	host->max_id = ARCMSR_MAX_TARGETID;		/*16:8*/
 	host->max_cmd_len = 16;	 			/*this is issue of 64bit LBA ,over 2T byte*/
-	host->can_queue = ARCMSR_MAX_FREECCB_NUM;	/* max simultaneous cmds */		
+	host->can_queue = ARCMSR_MAX_OUTSTANDING_CMD;
 	host->cmd_per_lun = ARCMSR_MAX_CMD_PERLUN;	    
 	host->this_id = ARCMSR_SCSI_INITIATOR_ID;
 	host->unique_id = (bus << 8) | dev_fun;
@@ -649,12 +733,16 @@ static int arcmsr_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	}
 	spin_lock_init(&acb->eh_lock);
 	spin_lock_init(&acb->ccblist_lock);
+	spin_lock_init(&acb->postq_lock);
+	spin_lock_init(&acb->doneq_lock);
+	spin_lock_init(&acb->rqbuffer_lock);
+	spin_lock_init(&acb->wqbuffer_lock);
 	acb->acb_flags |= (ACB_F_MESSAGE_WQBUFFER_CLEARED |
 			ACB_F_MESSAGE_RQBUFFER_CLEARED |
 			ACB_F_MESSAGE_WQBUFFER_READED);
 	acb->acb_flags &= ~ACB_F_SCSISTOPADAPTER;
 	INIT_LIST_HEAD(&acb->ccb_free_list);
-	arcmsr_define_adapter_type(acb);
+	acb->adapter_type = id->driver_data;
 	error = arcmsr_remap_pciregion(acb);
 	if(!error){
 		goto pci_release_regs;
@@ -667,17 +755,13 @@ static int arcmsr_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if(error){
 		goto free_hbb_mu;
 	}
-	arcmsr_iop_init(acb);
 	error = scsi_add_host(host, &pdev->dev);
 	if(error){
-		goto RAID_controller_stop;
+		goto free_ccb_pool;
 	}
-	error = request_irq(pdev->irq, arcmsr_do_interrupt, IRQF_SHARED, "arcmsr", acb);
-	if(error){
+	if (arcmsr_request_irq(pdev, acb) == FAILED)
 		goto scsi_host_remove;
-	}
-	host->irq = pdev->irq;
-    	scsi_scan_host(host);
+	arcmsr_iop_init(acb);
 	INIT_WORK(&acb->arcmsr_do_message_isr_bh, arcmsr_message_isr_bh_fn);
 	atomic_set(&acb->rq_map_token, 16);
 	atomic_set(&acb->ante_token_value, 16);
@@ -689,16 +773,20 @@ static int arcmsr_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	add_timer(&acb->eternal_timer);
 	if(arcmsr_alloc_sysfs_attr(acb))
 		goto out_free_sysfs;
+	scsi_scan_host(host);
 	return 0;
 out_free_sysfs:
-scsi_host_remove:
-	scsi_remove_host(host);
-RAID_controller_stop:
+	del_timer_sync(&acb->eternal_timer);
+	flush_work(&acb->arcmsr_do_message_isr_bh);
 	arcmsr_stop_adapter_bgrb(acb);
 	arcmsr_flush_adapter_cache(acb);
+	arcmsr_free_irq(pdev, acb);
+scsi_host_remove:
+	scsi_remove_host(host);
+free_ccb_pool:
 	arcmsr_free_ccb_pool(acb);
 free_hbb_mu:
-	arcmsr_free_hbb_mu(acb);
+	arcmsr_free_mu(acb);
 unmap_pci_region:
 	arcmsr_unmap_pciregion(acb);
 pci_release_regs:
@@ -710,75 +798,169 @@ pci_disable_dev:
 	return -ENODEV;
 }
 
-static uint8_t arcmsr_abort_hba_allcmd(struct AdapterControlBlock *acb)
+static void arcmsr_free_irq(struct pci_dev *pdev,
+		struct AdapterControlBlock *acb)
+{
+	int i;
+
+	if (acb->acb_flags & ACB_F_MSI_ENABLED) {
+		free_irq(pdev->irq, acb);
+		pci_disable_msi(pdev);
+	} else if (acb->acb_flags & ACB_F_MSIX_ENABLED) {
+		for (i = 0; i < acb->msix_vector_count; i++)
+			free_irq(acb->entries[i].vector, acb);
+		pci_disable_msix(pdev);
+	} else
+		free_irq(pdev->irq, acb);
+}
+
+static int arcmsr_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	uint32_t intmask_org;
+	struct Scsi_Host *host = pci_get_drvdata(pdev);
+	struct AdapterControlBlock *acb =
+		(struct AdapterControlBlock *)host->hostdata;
+
+	intmask_org = arcmsr_disable_outbound_ints(acb);
+	arcmsr_free_irq(pdev, acb);
+	del_timer_sync(&acb->eternal_timer);
+	flush_work(&acb->arcmsr_do_message_isr_bh);
+	arcmsr_stop_adapter_bgrb(acb);
+	arcmsr_flush_adapter_cache(acb);
+	pci_set_drvdata(pdev, host);
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	return 0;
+}
+
+static int arcmsr_resume(struct pci_dev *pdev)
+{
+	int error;
+	struct Scsi_Host *host = pci_get_drvdata(pdev);
+	struct AdapterControlBlock *acb =
+		(struct AdapterControlBlock *)host->hostdata;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_enable_wake(pdev, PCI_D0, 0);
+	pci_restore_state(pdev);
+	if (pci_enable_device(pdev)) {
+		pr_warn("%s: pci_enable_device error\n", __func__);
+		return -ENODEV;
+	}
+	error = pci_set_dma_mask(pdev, DMA_BIT_MASK(64));
+	if (error) {
+		error = pci_set_dma_mask(pdev, DMA_BIT_MASK(32));
+		if (error) {
+			pr_warn("scsi%d: No suitable DMA mask available\n",
+			       host->host_no);
+			goto controller_unregister;
+		}
+	}
+	pci_set_master(pdev);
+	if (arcmsr_request_irq(pdev, acb) == FAILED)
+		goto controller_stop;
+	arcmsr_iop_init(acb);
+	INIT_WORK(&acb->arcmsr_do_message_isr_bh, arcmsr_message_isr_bh_fn);
+	atomic_set(&acb->rq_map_token, 16);
+	atomic_set(&acb->ante_token_value, 16);
+	acb->fw_flag = FW_NORMAL;
+	init_timer(&acb->eternal_timer);
+	acb->eternal_timer.expires = jiffies + msecs_to_jiffies(6 * HZ);
+	acb->eternal_timer.data = (unsigned long) acb;
+	acb->eternal_timer.function = &arcmsr_request_device_map;
+	add_timer(&acb->eternal_timer);
+	return 0;
+controller_stop:
+	arcmsr_stop_adapter_bgrb(acb);
+	arcmsr_flush_adapter_cache(acb);
+controller_unregister:
+	scsi_remove_host(host);
+	arcmsr_free_ccb_pool(acb);
+	arcmsr_unmap_pciregion(acb);
+	pci_release_regions(pdev);
+	scsi_host_put(host);
+	pci_disable_device(pdev);
+	return -ENODEV;
+}
+
+static uint8_t arcmsr_hbaA_abort_allcmd(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	writel(ARCMSR_INBOUND_MESG0_ABORT_CMD, &reg->inbound_msgaddr0);
-	if (!arcmsr_hba_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaA_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE
-			"arcmsr%d: wait 'abort all outstanding command' timeout \n"
+			"arcmsr%d: wait 'abort all outstanding command' timeout\n"
 			, acb->host->host_no);
 		return false;
 	}
 	return true;
 }
 
-static uint8_t arcmsr_abort_hbb_allcmd(struct AdapterControlBlock *acb)
+static uint8_t arcmsr_hbaB_abort_allcmd(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
 
 	writel(ARCMSR_MESSAGE_ABORT_CMD, reg->drv2iop_doorbell);
-	if (!arcmsr_hbb_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE
-			"arcmsr%d: wait 'abort all outstanding command' timeout \n"
+			"arcmsr%d: wait 'abort all outstanding command' timeout\n"
 			, acb->host->host_no);
 		return false;
 	}
 	return true;
 }
-static uint8_t arcmsr_abort_hbc_allcmd(struct AdapterControlBlock *pACB)
+static uint8_t arcmsr_hbaC_abort_allcmd(struct AdapterControlBlock *pACB)
 {
-	struct MessageUnit_C *reg = (struct MessageUnit_C *)pACB->pmuC;
+	struct MessageUnit_C __iomem *reg = pACB->pmuC;
 	writel(ARCMSR_INBOUND_MESG0_ABORT_CMD, &reg->inbound_msgaddr0);
 	writel(ARCMSR_HBCMU_DRV2IOP_MESSAGE_CMD_DONE, &reg->inbound_doorbell);
-	if (!arcmsr_hbc_wait_msgint_ready(pACB)) {
+	if (!arcmsr_hbaC_wait_msgint_ready(pACB)) {
 		printk(KERN_NOTICE
-			"arcmsr%d: wait 'abort all outstanding command' timeout \n"
+			"arcmsr%d: wait 'abort all outstanding command' timeout\n"
 			, pACB->host->host_no);
 		return false;
 	}
 	return true;
 }
+
+static uint8_t arcmsr_hbaD_abort_allcmd(struct AdapterControlBlock *pACB)
+{
+	struct MessageUnit_D *reg = pACB->pmuD;
+
+	writel(ARCMSR_INBOUND_MESG0_ABORT_CMD, reg->inbound_msgaddr0);
+	if (!arcmsr_hbaD_wait_msgint_ready(pACB)) {
+		pr_notice("arcmsr%d: wait 'abort all outstanding "
+			"command' timeout\n", pACB->host->host_no);
+		return false;
+	}
+	return true;
+}
+
 static uint8_t arcmsr_abort_allcmd(struct AdapterControlBlock *acb)
 {
 	uint8_t rtnval = 0;
 	switch (acb->adapter_type) {
 	case ACB_ADAPTER_TYPE_A: {
-		rtnval = arcmsr_abort_hba_allcmd(acb);
+		rtnval = arcmsr_hbaA_abort_allcmd(acb);
 		}
 		break;
 
 	case ACB_ADAPTER_TYPE_B: {
-		rtnval = arcmsr_abort_hbb_allcmd(acb);
+		rtnval = arcmsr_hbaB_abort_allcmd(acb);
 		}
 		break;
 
 	case ACB_ADAPTER_TYPE_C: {
-		rtnval = arcmsr_abort_hbc_allcmd(acb);
+		rtnval = arcmsr_hbaC_abort_allcmd(acb);
 		}
-	}
-	return rtnval;
-}
+		break;
 
-static bool arcmsr_hbb_enable_driver_mode(struct AdapterControlBlock *pacb)
-{
-	struct MessageUnit_B *reg = pacb->pmuB;
-	writel(ARCMSR_MESSAGE_START_DRIVER_MODE, reg->drv2iop_doorbell);
-	if (!arcmsr_hbb_wait_msgint_ready(pacb)) {
-		printk(KERN_ERR "arcmsr%d: can't set driver mode. \n", pacb->host->host_no);
-		return false;
+	case ACB_ADAPTER_TYPE_D:
+		rtnval = arcmsr_hbaD_abort_allcmd(acb);
+		break;
 	}
-    	return true;
+	return rtnval;
 }
 
 static void arcmsr_pci_unmap_dma(struct CommandControlBlock *ccb)
@@ -837,12 +1019,18 @@ static u32 arcmsr_disable_outbound_ints(struct AdapterControlBlock *acb)
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C:{
-		struct MessageUnit_C *reg = (struct MessageUnit_C *)acb->pmuC;
+		struct MessageUnit_C __iomem *reg = acb->pmuC;
 		/* disable all outbound interrupt */
 		orig_mask = readl(&reg->host_int_mask); /* disable outbound message0 int */
 		writel(orig_mask|ARCMSR_HBCMU_ALL_INTMASKENABLE, &reg->host_int_mask);
 		}
 		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+		/* disable all outbound interrupt */
+		writel(ARCMSR_ARC1214_ALL_INT_DISABLE, reg->pcief0_int_enable);
+		}
+		break;
 	}
 	return orig_mask;
 }
@@ -933,7 +1121,7 @@ static void arcmsr_drain_donequeue(struct AdapterControlBlock *acb, struct Comma
 static void arcmsr_done4abort_postqueue(struct AdapterControlBlock *acb)
 {
 	int i = 0;
-	uint32_t flag_ccb;
+	uint32_t flag_ccb, ccb_cdb_phy;
 	struct ARCMSR_CDB *pARCMSR_CDB;
 	bool error;
 	struct CommandControlBlock *pCCB;
@@ -961,8 +1149,9 @@ static void arcmsr_done4abort_postqueue(struct AdapterControlBlock *acb)
 		/*clear all outbound posted Q*/
 		writel(ARCMSR_DOORBELL_INT_CLEAR_PATTERN, reg->iop2drv_doorbell); /* clear doorbell interrupt */
 		for (i = 0; i < ARCMSR_MAX_HBB_POSTQUEUE; i++) {
-			if ((flag_ccb = readl(&reg->done_qbuffer[i])) != 0) {
-				writel(0, &reg->done_qbuffer[i]);
+			flag_ccb = reg->done_qbuffer[i];
+			if (flag_ccb != 0) {
+				reg->done_qbuffer[i] = 0;
 				pARCMSR_CDB = (struct ARCMSR_CDB *)(acb->vir2phy_offset+(flag_ccb << 5));/*frame must be 32 bytes aligned*/
 				pCCB = container_of(pARCMSR_CDB, struct CommandControlBlock, arcmsr_cdb);
 				error = (flag_ccb & ARCMSR_CCBREPLY_FLAG_ERROR_MODE0) ? true : false;
@@ -975,11 +1164,7 @@ static void arcmsr_done4abort_postqueue(struct AdapterControlBlock *acb)
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		struct MessageUnit_C *reg = acb->pmuC;
-		struct  ARCMSR_CDB *pARCMSR_CDB;
-		uint32_t flag_ccb, ccb_cdb_phy;
-		bool error;
-		struct CommandControlBlock *pCCB;
+		struct MessageUnit_C __iomem *reg = acb->pmuC;
 		while ((readl(&reg->host_int_status) & ARCMSR_HBCMU_OUTBOUND_POSTQUEUE_ISR) && (i++ < ARCMSR_MAX_OUTSTANDING_CMD)) {
 			/*need to do*/
 			flag_ccb = readl(&reg->outbound_queueport_low);
@@ -989,9 +1174,54 @@ static void arcmsr_done4abort_postqueue(struct AdapterControlBlock *acb)
 			error = (flag_ccb & ARCMSR_CCBREPLY_FLAG_ERROR_MODE1) ? true : false;
 			arcmsr_drain_donequeue(acb, pCCB, error);
 		}
-	}
+		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D  *pmu = acb->pmuD;
+		uint32_t outbound_write_pointer;
+		uint32_t doneq_index, index_stripped, addressLow, residual, toggle;
+		unsigned long flags;
+
+		residual = atomic_read(&acb->ccboutstandingcount);
+		for (i = 0; i < residual; i++) {
+			spin_lock_irqsave(&acb->doneq_lock, flags);
+			outbound_write_pointer =
+				pmu->done_qbuffer[0].addressLow + 1;
+			doneq_index = pmu->doneq_index;
+			if ((doneq_index & 0xFFF) !=
+				(outbound_write_pointer & 0xFFF)) {
+				toggle = doneq_index & 0x4000;
+				index_stripped = (doneq_index & 0xFFF) + 1;
+				index_stripped %= ARCMSR_MAX_ARC1214_DONEQUEUE;
+				pmu->doneq_index = index_stripped ? (index_stripped | toggle) :
+					((toggle ^ 0x4000) + 1);
+				doneq_index = pmu->doneq_index;
+				spin_unlock_irqrestore(&acb->doneq_lock, flags);
+				addressLow = pmu->done_qbuffer[doneq_index &
+					0xFFF].addressLow;
+				ccb_cdb_phy = (addressLow & 0xFFFFFFF0);
+				pARCMSR_CDB = (struct  ARCMSR_CDB *)
+					(acb->vir2phy_offset + ccb_cdb_phy);
+				pCCB = container_of(pARCMSR_CDB,
+					struct CommandControlBlock, arcmsr_cdb);
+				error = (addressLow &
+					ARCMSR_CCBREPLY_FLAG_ERROR_MODE1) ?
+					true : false;
+				arcmsr_drain_donequeue(acb, pCCB, error);
+				writel(doneq_index,
+					pmu->outboundlist_read_pointer);
+			} else {
+				spin_unlock_irqrestore(&acb->doneq_lock, flags);
+				mdelay(10);
+			}
+		}
+		pmu->postq_index = 0;
+		pmu->doneq_index = 0x40FF;
+		}
+		break;
 	}
 }
+
 static void arcmsr_remove(struct pci_dev *pdev)
 {
 	struct Scsi_Host *host = pci_get_drvdata(pdev);
@@ -1029,9 +1259,9 @@ static void arcmsr_remove(struct pci_dev *pdev)
 			}
 		}
 	}
-	free_irq(pdev->irq, acb);
+	arcmsr_free_irq(pdev, acb);
 	arcmsr_free_ccb_pool(acb);
-	arcmsr_free_hbb_mu(acb);
+	arcmsr_free_mu(acb);
 	arcmsr_unmap_pciregion(acb);
 	pci_release_regions(pdev);
 	scsi_host_put(host);
@@ -1045,6 +1275,7 @@ static void arcmsr_shutdown(struct pci_dev *pdev)
 		(struct AdapterControlBlock *)host->hostdata;
 	del_timer_sync(&acb->eternal_timer);
 	arcmsr_disable_outbound_ints(acb);
+	arcmsr_free_irq(pdev, acb);
 	flush_work(&acb->arcmsr_do_message_isr_bh);
 	arcmsr_stop_adapter_bgrb(acb);
 	arcmsr_flush_adapter_cache(acb);
@@ -1091,11 +1322,19 @@ static void arcmsr_enable_outbound_ints(struct AdapterControlBlock *acb,
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		struct MessageUnit_C *reg = acb->pmuC;
+		struct MessageUnit_C __iomem *reg = acb->pmuC;
 		mask = ~(ARCMSR_HBCMU_UTILITY_A_ISR_MASK | ARCMSR_HBCMU_OUTBOUND_DOORBELL_ISR_MASK|ARCMSR_HBCMU_OUTBOUND_POSTQUEUE_ISR_MASK);
 		writel(intmask_org & mask, &reg->host_int_mask);
 		acb->outbound_int_enable = ~(intmask_org & mask) & 0x0000000f;
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+
+		mask = ARCMSR_ARC1214_ALL_INT_ENABLE;
+		writel(intmask_org | mask, reg->pcief0_int_enable);
+		break;
+		}
 	}
 }
 
@@ -1115,7 +1354,7 @@ static int arcmsr_build_ccb(struct AdapterControlBlock *acb,
 	arcmsr_cdb->TargetID = pcmd->device->id;
 	arcmsr_cdb->LUN = pcmd->device->lun;
 	arcmsr_cdb->Function = 1;
-	arcmsr_cdb->Context = 0;
+	arcmsr_cdb->msgContext = 0;
 	memcpy(arcmsr_cdb->Cdb, pcmd->cmnd, pcmd->cmd_len);
 
 	nseg = scsi_dma_map(pcmd);
@@ -1156,7 +1395,7 @@ static int arcmsr_build_ccb(struct AdapterControlBlock *acb,
 
 static void arcmsr_post_ccb(struct AdapterControlBlock *acb, struct CommandControlBlock *ccb)
 {
-	uint32_t cdb_phyaddr_pattern = ccb->cdb_phyaddr_pattern;
+	uint32_t cdb_phyaddr = ccb->cdb_phyaddr;
 	struct ARCMSR_CDB *arcmsr_cdb = (struct ARCMSR_CDB *)&ccb->arcmsr_cdb;
 	atomic_inc(&acb->ccboutstandingcount);
 	ccb->startdone = ARCMSR_CCB_START;
@@ -1165,25 +1404,24 @@ static void arcmsr_post_ccb(struct AdapterControlBlock *acb, struct CommandContr
 		struct MessageUnit_A __iomem *reg = acb->pmuA;
 
 		if (arcmsr_cdb->Flags & ARCMSR_CDB_FLAG_SGL_BSIZE)
-			writel(cdb_phyaddr_pattern | ARCMSR_CCBPOST_FLAG_SGL_BSIZE,
+			writel(cdb_phyaddr | ARCMSR_CCBPOST_FLAG_SGL_BSIZE,
 			&reg->inbound_queueport);
-		else {
-				writel(cdb_phyaddr_pattern, &reg->inbound_queueport);
-		}
-		}
+		else
+			writel(cdb_phyaddr, &reg->inbound_queueport);
 		break;
+	}
 
 	case ACB_ADAPTER_TYPE_B: {
 		struct MessageUnit_B *reg = acb->pmuB;
 		uint32_t ending_index, index = reg->postq_index;
 
 		ending_index = ((index + 1) % ARCMSR_MAX_HBB_POSTQUEUE);
-		writel(0, &reg->post_qbuffer[ending_index]);
+		reg->post_qbuffer[ending_index] = 0;
 		if (arcmsr_cdb->Flags & ARCMSR_CDB_FLAG_SGL_BSIZE) {
-			writel(cdb_phyaddr_pattern | ARCMSR_CCBPOST_FLAG_SGL_BSIZE,\
-						 &reg->post_qbuffer[index]);
+			reg->post_qbuffer[index] =
+				cdb_phyaddr | ARCMSR_CCBPOST_FLAG_SGL_BSIZE;
 		} else {
-			writel(cdb_phyaddr_pattern, &reg->post_qbuffer[index]);
+			reg->post_qbuffer[index] = cdb_phyaddr;
 		}
 		index++;
 		index %= ARCMSR_MAX_HBB_POSTQUEUE;/*if last index number set it to 0 */
@@ -1192,11 +1430,11 @@ static void arcmsr_post_ccb(struct AdapterControlBlock *acb, struct CommandContr
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		struct MessageUnit_C *phbcmu = (struct MessageUnit_C *)acb->pmuC;
+		struct MessageUnit_C __iomem *phbcmu = acb->pmuC;
 		uint32_t ccb_post_stamp, arc_cdb_size;
 
 		arc_cdb_size = (ccb->arc_cdb_size > 0x300) ? 0x300 : ccb->arc_cdb_size;
-		ccb_post_stamp = (cdb_phyaddr_pattern | ((arc_cdb_size - 1) >> 6) | 1);
+		ccb_post_stamp = (cdb_phyaddr | ((arc_cdb_size - 1) >> 6) | 1);
 		if (acb->cdb_phyaddr_hi32) {
 			writel(acb->cdb_phyaddr_hi32, &phbcmu->inbound_queueport_high);
 			writel(ccb_post_stamp, &phbcmu->inbound_queueport_low);
@@ -1204,62 +1442,102 @@ static void arcmsr_post_ccb(struct AdapterControlBlock *acb, struct CommandContr
 			writel(ccb_post_stamp, &phbcmu->inbound_queueport_low);
 		}
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D  *pmu = acb->pmuD;
+		u16 index_stripped;
+		u16 postq_index, toggle;
+		unsigned long flags;
+		struct InBound_SRB *pinbound_srb;
+
+		spin_lock_irqsave(&acb->postq_lock, flags);
+		postq_index = pmu->postq_index;
+		pinbound_srb = (struct InBound_SRB *)&(pmu->post_qbuffer[postq_index & 0xFF]);
+		pinbound_srb->addressHigh = dma_addr_hi32(cdb_phyaddr);
+		pinbound_srb->addressLow = dma_addr_lo32(cdb_phyaddr);
+		pinbound_srb->length = ccb->arc_cdb_size >> 2;
+		arcmsr_cdb->msgContext = dma_addr_lo32(cdb_phyaddr);
+		toggle = postq_index & 0x4000;
+		index_stripped = postq_index + 1;
+		index_stripped &= (ARCMSR_MAX_ARC1214_POSTQUEUE - 1);
+		pmu->postq_index = index_stripped ? (index_stripped | toggle) :
+			(toggle ^ 0x4000);
+		writel(postq_index, pmu->inboundlist_write_pointer);
+		spin_unlock_irqrestore(&acb->postq_lock, flags);
+		break;
+		}
 	}
 }
 
-static void arcmsr_stop_hba_bgrb(struct AdapterControlBlock *acb)
+static void arcmsr_hbaA_stop_bgrb(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	acb->acb_flags &= ~ACB_F_MSG_START_BGRB;
 	writel(ARCMSR_INBOUND_MESG0_STOP_BGRB, &reg->inbound_msgaddr0);
-	if (!arcmsr_hba_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaA_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE
-			"arcmsr%d: wait 'stop adapter background rebulid' timeout \n"
+			"arcmsr%d: wait 'stop adapter background rebulid' timeout\n"
 			, acb->host->host_no);
 	}
 }
 
-static void arcmsr_stop_hbb_bgrb(struct AdapterControlBlock *acb)
+static void arcmsr_hbaB_stop_bgrb(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
 	acb->acb_flags &= ~ACB_F_MSG_START_BGRB;
 	writel(ARCMSR_MESSAGE_STOP_BGRB, reg->drv2iop_doorbell);
 
-	if (!arcmsr_hbb_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE
-			"arcmsr%d: wait 'stop adapter background rebulid' timeout \n"
+			"arcmsr%d: wait 'stop adapter background rebulid' timeout\n"
 			, acb->host->host_no);
 	}
 }
 
-static void arcmsr_stop_hbc_bgrb(struct AdapterControlBlock *pACB)
+static void arcmsr_hbaC_stop_bgrb(struct AdapterControlBlock *pACB)
 {
-	struct MessageUnit_C *reg = (struct MessageUnit_C *)pACB->pmuC;
+	struct MessageUnit_C __iomem *reg = pACB->pmuC;
 	pACB->acb_flags &= ~ACB_F_MSG_START_BGRB;
 	writel(ARCMSR_INBOUND_MESG0_STOP_BGRB, &reg->inbound_msgaddr0);
 	writel(ARCMSR_HBCMU_DRV2IOP_MESSAGE_CMD_DONE, &reg->inbound_doorbell);
-	if (!arcmsr_hbc_wait_msgint_ready(pACB)) {
+	if (!arcmsr_hbaC_wait_msgint_ready(pACB)) {
 		printk(KERN_NOTICE
-			"arcmsr%d: wait 'stop adapter background rebulid' timeout \n"
+			"arcmsr%d: wait 'stop adapter background rebulid' timeout\n"
 			, pACB->host->host_no);
 	}
 	return;
 }
+
+static void arcmsr_hbaD_stop_bgrb(struct AdapterControlBlock *pACB)
+{
+	struct MessageUnit_D *reg = pACB->pmuD;
+
+	pACB->acb_flags &= ~ACB_F_MSG_START_BGRB;
+	writel(ARCMSR_INBOUND_MESG0_STOP_BGRB, reg->inbound_msgaddr0);
+	if (!arcmsr_hbaD_wait_msgint_ready(pACB))
+		pr_notice("arcmsr%d: wait 'stop adapter background rebulid' "
+			"timeout\n", pACB->host->host_no);
+}
+
 static void arcmsr_stop_adapter_bgrb(struct AdapterControlBlock *acb)
 {
 	switch (acb->adapter_type) {
 	case ACB_ADAPTER_TYPE_A: {
-		arcmsr_stop_hba_bgrb(acb);
+		arcmsr_hbaA_stop_bgrb(acb);
 		}
 		break;
 
 	case ACB_ADAPTER_TYPE_B: {
-		arcmsr_stop_hbb_bgrb(acb);
+		arcmsr_hbaB_stop_bgrb(acb);
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		arcmsr_stop_hbc_bgrb(acb);
+		arcmsr_hbaC_stop_bgrb(acb);
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D:
+		arcmsr_hbaD_stop_bgrb(acb);
+		break;
 	}
 }
 
@@ -1268,7 +1546,7 @@ static void arcmsr_free_ccb_pool(struct AdapterControlBlock *acb)
 	dma_free_coherent(&acb->pdev->dev, acb->uncache_size, acb->dma_coherent, acb->dma_coherent_handle);
 }
 
-void arcmsr_iop_message_read(struct AdapterControlBlock *acb)
+static void arcmsr_iop_message_read(struct AdapterControlBlock *acb)
 {
 	switch (acb->adapter_type) {
 	case ACB_ADAPTER_TYPE_A: {
@@ -1284,8 +1562,16 @@ void arcmsr_iop_message_read(struct AdapterControlBlock *acb)
 		break;
 	case ACB_ADAPTER_TYPE_C: {
 		struct MessageUnit_C __iomem *reg = acb->pmuC;
+
 		writel(ARCMSR_HBCMU_DRV2IOP_DATA_READ_OK, &reg->inbound_doorbell);
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+		writel(ARCMSR_ARC1214_DRV2IOP_DATA_OUT_READ,
+			reg->inbound_doorbell);
+		}
+		break;
 	}
 }
 
@@ -1320,6 +1606,12 @@ static void arcmsr_iop_message_wrote(struct AdapterControlBlock *acb)
 		writel(ARCMSR_HBCMU_DRV2IOP_DATA_WRITE_OK, &reg->inbound_doorbell);
 		}
 		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+		writel(ARCMSR_ARC1214_DRV2IOP_DATA_IN_READY,
+			reg->inbound_doorbell);
+		}
+		break;
 	}
 }
 
@@ -1340,9 +1632,15 @@ struct QBUFFER __iomem *arcmsr_get_iop_rqbuffer(struct AdapterControlBlock *acb)
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		struct MessageUnit_C *phbcmu = (struct MessageUnit_C *)acb->pmuC;
+		struct MessageUnit_C __iomem *phbcmu = acb->pmuC;
 		qbuffer = (struct QBUFFER __iomem *)&phbcmu->message_rbuffer;
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+		qbuffer = (struct QBUFFER __iomem *)reg->message_rbuffer;
+		}
+		break;
 	}
 	return qbuffer;
 }
@@ -1364,96 +1662,208 @@ static struct QBUFFER __iomem *arcmsr_get_iop_wqbuffer(struct AdapterControlBloc
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		struct MessageUnit_C *reg = (struct MessageUnit_C *)acb->pmuC;
+		struct MessageUnit_C __iomem *reg = acb->pmuC;
 		pqbuffer = (struct QBUFFER __iomem *)&reg->message_wbuffer;
-	}
-
+		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+		pqbuffer = (struct QBUFFER __iomem *)reg->message_wbuffer;
+		}
+		break;
 	}
 	return pqbuffer;
 }
 
-static void arcmsr_iop2drv_data_wrote_handle(struct AdapterControlBlock *acb)
+static uint32_t
+arcmsr_Read_iop_rqbuffer_in_DWORD(struct AdapterControlBlock *acb,
+		struct QBUFFER __iomem *prbuffer)
 {
-	struct QBUFFER __iomem *prbuffer;
-	struct QBUFFER *pQbuffer;
-	uint8_t __iomem *iop_data;
-	int32_t my_empty_len, iop_len, rqbuf_firstindex, rqbuf_lastindex;
-	rqbuf_lastindex = acb->rqbuf_lastindex;
-	rqbuf_firstindex = acb->rqbuf_firstindex;
-	prbuffer = arcmsr_get_iop_rqbuffer(acb);
-	iop_data = (uint8_t __iomem *)prbuffer->data;
-	iop_len = prbuffer->data_len;
-	my_empty_len = (rqbuf_firstindex - rqbuf_lastindex - 1) & (ARCMSR_MAX_QBUFFER - 1);
-
-	if (my_empty_len >= iop_len)
-	{
-		while (iop_len > 0) {
-			pQbuffer = (struct QBUFFER *)&acb->rqbuffer[rqbuf_lastindex];
-			memcpy(pQbuffer, iop_data, 1);
-			rqbuf_lastindex++;
-			rqbuf_lastindex %= ARCMSR_MAX_QBUFFER;
+	uint8_t *pQbuffer;
+	uint8_t *buf1 = NULL;
+	uint32_t __iomem *iop_data;
+	uint32_t iop_len, data_len, *buf2 = NULL;
+
+	iop_data = (uint32_t __iomem *)prbuffer->data;
+	iop_len = readl(&prbuffer->data_len);
+	if (iop_len > 0) {
+		buf1 = kmalloc(128, GFP_ATOMIC);
+		buf2 = (uint32_t *)buf1;
+		if (buf1 == NULL)
+			return 0;
+		data_len = iop_len;
+		while (data_len >= 4) {
+			*buf2++ = readl(iop_data);
 			iop_data++;
-			iop_len--;
+			data_len -= 4;
 		}
-		acb->rqbuf_lastindex = rqbuf_lastindex;
-		arcmsr_iop_message_read(acb);
+		if (data_len)
+			*buf2 = readl(iop_data);
+		buf2 = (uint32_t *)buf1;
+	}
+	while (iop_len > 0) {
+		pQbuffer = &acb->rqbuffer[acb->rqbuf_putIndex];
+		*pQbuffer = *buf1;
+		acb->rqbuf_putIndex++;
+		/* if last, index number set it to 0 */
+		acb->rqbuf_putIndex %= ARCMSR_MAX_QBUFFER;
+		buf1++;
+		iop_len--;
 	}
+	kfree(buf2);
+	/* let IOP know data has been read */
+	arcmsr_iop_message_read(acb);
+	return 1;
+}
+
+uint32_t
+arcmsr_Read_iop_rqbuffer_data(struct AdapterControlBlock *acb,
+	struct QBUFFER __iomem *prbuffer) {
 
-	else {
+	uint8_t *pQbuffer;
+	uint8_t __iomem *iop_data;
+	uint32_t iop_len;
+
+	if (acb->adapter_type & (ACB_ADAPTER_TYPE_C | ACB_ADAPTER_TYPE_D))
+		return arcmsr_Read_iop_rqbuffer_in_DWORD(acb, prbuffer);
+	iop_data = (uint8_t __iomem *)prbuffer->data;
+	iop_len = readl(&prbuffer->data_len);
+	while (iop_len > 0) {
+		pQbuffer = &acb->rqbuffer[acb->rqbuf_putIndex];
+		*pQbuffer = readb(iop_data);
+		acb->rqbuf_putIndex++;
+		acb->rqbuf_putIndex %= ARCMSR_MAX_QBUFFER;
+		iop_data++;
+		iop_len--;
+	}
+	arcmsr_iop_message_read(acb);
+	return 1;
+}
+
+static void arcmsr_iop2drv_data_wrote_handle(struct AdapterControlBlock *acb)
+{
+	unsigned long flags;
+	struct QBUFFER __iomem  *prbuffer;
+	int32_t buf_empty_len;
+
+	spin_lock_irqsave(&acb->rqbuffer_lock, flags);
+	prbuffer = arcmsr_get_iop_rqbuffer(acb);
+	buf_empty_len = (acb->rqbuf_putIndex - acb->rqbuf_getIndex - 1) &
+		(ARCMSR_MAX_QBUFFER - 1);
+	if (buf_empty_len >= readl(&prbuffer->data_len)) {
+		if (arcmsr_Read_iop_rqbuffer_data(acb, prbuffer) == 0)
+			acb->acb_flags |= ACB_F_IOPDATA_OVERFLOW;
+	} else
 		acb->acb_flags |= ACB_F_IOPDATA_OVERFLOW;
+	spin_unlock_irqrestore(&acb->rqbuffer_lock, flags);
+}
+
+static void arcmsr_write_ioctldata2iop_in_DWORD(struct AdapterControlBlock *acb)
+{
+	uint8_t *pQbuffer;
+	struct QBUFFER __iomem *pwbuffer;
+	uint8_t *buf1 = NULL;
+	uint32_t __iomem *iop_data;
+	uint32_t allxfer_len = 0, data_len, *buf2 = NULL, data;
+
+	if (acb->acb_flags & ACB_F_MESSAGE_WQBUFFER_READED) {
+		buf1 = kmalloc(128, GFP_ATOMIC);
+		buf2 = (uint32_t *)buf1;
+		if (buf1 == NULL)
+			return;
+
+		acb->acb_flags &= (~ACB_F_MESSAGE_WQBUFFER_READED);
+		pwbuffer = arcmsr_get_iop_wqbuffer(acb);
+		iop_data = (uint32_t __iomem *)pwbuffer->data;
+		while ((acb->wqbuf_getIndex != acb->wqbuf_putIndex)
+			&& (allxfer_len < 124)) {
+			pQbuffer = &acb->wqbuffer[acb->wqbuf_getIndex];
+			*buf1 = *pQbuffer;
+			acb->wqbuf_getIndex++;
+			acb->wqbuf_getIndex %= ARCMSR_MAX_QBUFFER;
+			buf1++;
+			allxfer_len++;
+		}
+		data_len = allxfer_len;
+		buf1 = (uint8_t *)buf2;
+		while (data_len >= 4) {
+			data = *buf2++;
+			writel(data, iop_data);
+			iop_data++;
+			data_len -= 4;
+		}
+		if (data_len) {
+			data = *buf2;
+			writel(data, iop_data);
+		}
+		writel(allxfer_len, &pwbuffer->data_len);
+		kfree(buf1);
+		arcmsr_iop_message_wrote(acb);
 	}
 }
 
-static void arcmsr_iop2drv_data_read_handle(struct AdapterControlBlock *acb)
+void
+arcmsr_write_ioctldata2iop(struct AdapterControlBlock *acb)
 {
-	acb->acb_flags |= ACB_F_MESSAGE_WQBUFFER_READED;
-	if (acb->wqbuf_firstindex != acb->wqbuf_lastindex) {
-		uint8_t *pQbuffer;
-		struct QBUFFER __iomem *pwbuffer;
-		uint8_t __iomem *iop_data;
-		int32_t allxfer_len = 0;
+	uint8_t *pQbuffer;
+	struct QBUFFER __iomem *pwbuffer;
+	uint8_t __iomem *iop_data;
+	int32_t allxfer_len = 0;
 
+	if (acb->adapter_type & (ACB_ADAPTER_TYPE_C | ACB_ADAPTER_TYPE_D)) {
+		arcmsr_write_ioctldata2iop_in_DWORD(acb);
+		return;
+	}
+	if (acb->acb_flags & ACB_F_MESSAGE_WQBUFFER_READED) {
 		acb->acb_flags &= (~ACB_F_MESSAGE_WQBUFFER_READED);
 		pwbuffer = arcmsr_get_iop_wqbuffer(acb);
 		iop_data = (uint8_t __iomem *)pwbuffer->data;
-
-		while ((acb->wqbuf_firstindex != acb->wqbuf_lastindex) && \
-							(allxfer_len < 124)) {
-			pQbuffer = &acb->wqbuffer[acb->wqbuf_firstindex];
-			memcpy(iop_data, pQbuffer, 1);
-			acb->wqbuf_firstindex++;
-			acb->wqbuf_firstindex %= ARCMSR_MAX_QBUFFER;
+		while ((acb->wqbuf_getIndex != acb->wqbuf_putIndex)
+			&& (allxfer_len < 124)) {
+			pQbuffer = &acb->wqbuffer[acb->wqbuf_getIndex];
+			writeb(*pQbuffer, iop_data);
+			acb->wqbuf_getIndex++;
+			acb->wqbuf_getIndex %= ARCMSR_MAX_QBUFFER;
 			iop_data++;
 			allxfer_len++;
 		}
-		pwbuffer->data_len = allxfer_len;
-
+		writel(allxfer_len, &pwbuffer->data_len);
 		arcmsr_iop_message_wrote(acb);
 	}
+}
 
-	if (acb->wqbuf_firstindex == acb->wqbuf_lastindex) {
+static void arcmsr_iop2drv_data_read_handle(struct AdapterControlBlock *acb)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&acb->wqbuffer_lock, flags);
+	acb->acb_flags |= ACB_F_MESSAGE_WQBUFFER_READED;
+	if (acb->wqbuf_getIndex != acb->wqbuf_putIndex)
+		arcmsr_write_ioctldata2iop(acb);
+	if (acb->wqbuf_getIndex == acb->wqbuf_putIndex)
 		acb->acb_flags |= ACB_F_MESSAGE_WQBUFFER_CLEARED;
-	}
+	spin_unlock_irqrestore(&acb->wqbuffer_lock, flags);
 }
 
-static void arcmsr_hba_doorbell_isr(struct AdapterControlBlock *acb)
+static void arcmsr_hbaA_doorbell_isr(struct AdapterControlBlock *acb)
 {
 	uint32_t outbound_doorbell;
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	outbound_doorbell = readl(&reg->outbound_doorbell);
-	writel(outbound_doorbell, &reg->outbound_doorbell);
-	if (outbound_doorbell & ARCMSR_OUTBOUND_IOP331_DATA_WRITE_OK) {
-		arcmsr_iop2drv_data_wrote_handle(acb);
-	}
-
-	if (outbound_doorbell & ARCMSR_OUTBOUND_IOP331_DATA_READ_OK) {
-		arcmsr_iop2drv_data_read_handle(acb);
-	}
+	do {
+		writel(outbound_doorbell, &reg->outbound_doorbell);
+		if (outbound_doorbell & ARCMSR_OUTBOUND_IOP331_DATA_WRITE_OK)
+			arcmsr_iop2drv_data_wrote_handle(acb);
+		if (outbound_doorbell & ARCMSR_OUTBOUND_IOP331_DATA_READ_OK)
+			arcmsr_iop2drv_data_read_handle(acb);
+		outbound_doorbell = readl(&reg->outbound_doorbell);
+	} while (outbound_doorbell & (ARCMSR_OUTBOUND_IOP331_DATA_WRITE_OK
+		| ARCMSR_OUTBOUND_IOP331_DATA_READ_OK));
 }
-static void arcmsr_hbc_doorbell_isr(struct AdapterControlBlock *pACB)
+static void arcmsr_hbaC_doorbell_isr(struct AdapterControlBlock *pACB)
 {
 	uint32_t outbound_doorbell;
-	struct MessageUnit_C *reg = (struct MessageUnit_C *)pACB->pmuC;
+	struct MessageUnit_C __iomem *reg = pACB->pmuC;
 	/*
 	*******************************************************************
 	**  Maybe here we need to check wrqbuffer_lock is lock or not
@@ -1462,19 +1872,42 @@ static void arcmsr_hbc_doorbell_isr(struct AdapterControlBlock *pACB)
 	*******************************************************************
 	*/
 	outbound_doorbell = readl(&reg->outbound_doorbell);
-	writel(outbound_doorbell, &reg->outbound_doorbell_clear);/*clear interrupt*/
-	if (outbound_doorbell & ARCMSR_HBCMU_IOP2DRV_DATA_WRITE_OK) {
-		arcmsr_iop2drv_data_wrote_handle(pACB);
-	}
-	if (outbound_doorbell & ARCMSR_HBCMU_IOP2DRV_DATA_READ_OK) {
-		arcmsr_iop2drv_data_read_handle(pACB);
-	}
-	if (outbound_doorbell & ARCMSR_HBCMU_IOP2DRV_MESSAGE_CMD_DONE) {
-		arcmsr_hbc_message_isr(pACB);    /* messenger of "driver to iop commands" */
-	}
-	return;
+	do {
+		writel(outbound_doorbell, &reg->outbound_doorbell_clear);
+		readl(&reg->outbound_doorbell_clear);
+		if (outbound_doorbell & ARCMSR_HBCMU_IOP2DRV_DATA_WRITE_OK)
+			arcmsr_iop2drv_data_wrote_handle(pACB);
+		if (outbound_doorbell & ARCMSR_HBCMU_IOP2DRV_DATA_READ_OK)
+			arcmsr_iop2drv_data_read_handle(pACB);
+		if (outbound_doorbell & ARCMSR_HBCMU_IOP2DRV_MESSAGE_CMD_DONE)
+			arcmsr_hbaC_message_isr(pACB);
+		outbound_doorbell = readl(&reg->outbound_doorbell);
+	} while (outbound_doorbell & (ARCMSR_HBCMU_IOP2DRV_DATA_WRITE_OK
+		| ARCMSR_HBCMU_IOP2DRV_DATA_READ_OK
+		| ARCMSR_HBCMU_IOP2DRV_MESSAGE_CMD_DONE));
 }
-static void arcmsr_hba_postqueue_isr(struct AdapterControlBlock *acb)
+
+static void arcmsr_hbaD_doorbell_isr(struct AdapterControlBlock *pACB)
+{
+	uint32_t outbound_doorbell;
+	struct MessageUnit_D  *pmu = pACB->pmuD;
+
+	outbound_doorbell = readl(pmu->outbound_doorbell);
+	do {
+		writel(outbound_doorbell, pmu->outbound_doorbell);
+		if (outbound_doorbell & ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE)
+			arcmsr_hbaD_message_isr(pACB);
+		if (outbound_doorbell & ARCMSR_ARC1214_IOP2DRV_DATA_WRITE_OK)
+			arcmsr_iop2drv_data_wrote_handle(pACB);
+		if (outbound_doorbell & ARCMSR_ARC1214_IOP2DRV_DATA_READ_OK)
+			arcmsr_iop2drv_data_read_handle(pACB);
+		outbound_doorbell = readl(pmu->outbound_doorbell);
+	} while (outbound_doorbell & (ARCMSR_ARC1214_IOP2DRV_DATA_WRITE_OK
+		| ARCMSR_ARC1214_IOP2DRV_DATA_READ_OK
+		| ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE));
+}
+
+static void arcmsr_hbaA_postqueue_isr(struct AdapterControlBlock *acb)
 {
 	uint32_t flag_ccb;
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
@@ -1488,7 +1921,7 @@ static void arcmsr_hba_postqueue_isr(struct AdapterControlBlock *acb)
 		arcmsr_drain_donequeue(acb, pCCB, error);
 	}
 }
-static void arcmsr_hbb_postqueue_isr(struct AdapterControlBlock *acb)
+static void arcmsr_hbaB_postqueue_isr(struct AdapterControlBlock *acb)
 {
 	uint32_t index;
 	uint32_t flag_ccb;
@@ -1497,8 +1930,8 @@ static void arcmsr_hbb_postqueue_isr(struct AdapterControlBlock *acb)
 	struct CommandControlBlock *pCCB;
 	bool error;
 	index = reg->doneq_index;
-	while ((flag_ccb = readl(&reg->done_qbuffer[index])) != 0) {
-		writel(0, &reg->done_qbuffer[index]);
+	while ((flag_ccb = reg->done_qbuffer[index]) != 0) {
+		reg->done_qbuffer[index] = 0;
 		pARCMSR_CDB = (struct ARCMSR_CDB *)(acb->vir2phy_offset+(flag_ccb << 5));/*frame must be 32 bytes aligned*/
 		pCCB = container_of(pARCMSR_CDB, struct CommandControlBlock, arcmsr_cdb);
 		error = (flag_ccb & ARCMSR_CCBREPLY_FLAG_ERROR_MODE0) ? true : false;
@@ -1509,35 +1942,80 @@ static void arcmsr_hbb_postqueue_isr(struct AdapterControlBlock *acb)
 	}
 }
 
-static void arcmsr_hbc_postqueue_isr(struct AdapterControlBlock *acb)
+static void arcmsr_hbaC_postqueue_isr(struct AdapterControlBlock *acb)
 {
-	struct MessageUnit_C *phbcmu;
+	struct MessageUnit_C __iomem *phbcmu;
 	struct ARCMSR_CDB *arcmsr_cdb;
 	struct CommandControlBlock *ccb;
 	uint32_t flag_ccb, ccb_cdb_phy, throttling = 0;
 	int error;
 
-	phbcmu = (struct MessageUnit_C *)acb->pmuC;
+	phbcmu = acb->pmuC;
 	/* areca cdb command done */
 	/* Use correct offset and size for syncing */
 
-	while (readl(&phbcmu->host_int_status) &
-	ARCMSR_HBCMU_OUTBOUND_POSTQUEUE_ISR){
-	/* check if command done with no error*/
-	flag_ccb = readl(&phbcmu->outbound_queueport_low);
-	ccb_cdb_phy = (flag_ccb & 0xFFFFFFF0);/*frame must be 32 bytes aligned*/
-	arcmsr_cdb = (struct ARCMSR_CDB *)(acb->vir2phy_offset + ccb_cdb_phy);
-	ccb = container_of(arcmsr_cdb, struct CommandControlBlock, arcmsr_cdb);
-	error = (flag_ccb & ARCMSR_CCBREPLY_FLAG_ERROR_MODE1) ? true : false;
-	/* check if command done with no error */
-	arcmsr_drain_donequeue(acb, ccb, error);
-	if (throttling == ARCMSR_HBC_ISR_THROTTLING_LEVEL) {
-		writel(ARCMSR_HBCMU_DRV2IOP_POSTQUEUE_THROTTLING, &phbcmu->inbound_doorbell);
-		break;
-	}
-	throttling++;
+	while ((flag_ccb = readl(&phbcmu->outbound_queueport_low)) !=
+			0xFFFFFFFF) {
+		ccb_cdb_phy = (flag_ccb & 0xFFFFFFF0);
+		arcmsr_cdb = (struct ARCMSR_CDB *)(acb->vir2phy_offset
+			+ ccb_cdb_phy);
+		ccb = container_of(arcmsr_cdb, struct CommandControlBlock,
+			arcmsr_cdb);
+		error = (flag_ccb & ARCMSR_CCBREPLY_FLAG_ERROR_MODE1)
+			? true : false;
+		/* check if command done with no error */
+		arcmsr_drain_donequeue(acb, ccb, error);
+		throttling++;
+		if (throttling == ARCMSR_HBC_ISR_THROTTLING_LEVEL) {
+			writel(ARCMSR_HBCMU_DRV2IOP_POSTQUEUE_THROTTLING,
+				&phbcmu->inbound_doorbell);
+			throttling = 0;
+		}
 	}
 }
+
+static void arcmsr_hbaD_postqueue_isr(struct AdapterControlBlock *acb)
+{
+	u32 outbound_write_pointer, doneq_index, index_stripped, toggle;
+	uint32_t addressLow, ccb_cdb_phy;
+	int error;
+	struct MessageUnit_D  *pmu;
+	struct ARCMSR_CDB *arcmsr_cdb;
+	struct CommandControlBlock *ccb;
+	unsigned long flags;
+
+	spin_lock_irqsave(&acb->doneq_lock, flags);
+	pmu = acb->pmuD;
+	outbound_write_pointer = pmu->done_qbuffer[0].addressLow + 1;
+	doneq_index = pmu->doneq_index;
+	if ((doneq_index & 0xFFF) != (outbound_write_pointer & 0xFFF)) {
+		do {
+			toggle = doneq_index & 0x4000;
+			index_stripped = (doneq_index & 0xFFF) + 1;
+			index_stripped %= ARCMSR_MAX_ARC1214_DONEQUEUE;
+			pmu->doneq_index = index_stripped ? (index_stripped | toggle) :
+				((toggle ^ 0x4000) + 1);
+			doneq_index = pmu->doneq_index;
+			addressLow = pmu->done_qbuffer[doneq_index &
+				0xFFF].addressLow;
+			ccb_cdb_phy = (addressLow & 0xFFFFFFF0);
+			arcmsr_cdb = (struct ARCMSR_CDB *)(acb->vir2phy_offset
+				+ ccb_cdb_phy);
+			ccb = container_of(arcmsr_cdb,
+				struct CommandControlBlock, arcmsr_cdb);
+			error = (addressLow & ARCMSR_CCBREPLY_FLAG_ERROR_MODE1)
+				? true : false;
+			arcmsr_drain_donequeue(acb, ccb, error);
+			writel(doneq_index, pmu->outboundlist_read_pointer);
+		} while ((doneq_index & 0xFFF) !=
+			(outbound_write_pointer & 0xFFF));
+	}
+	writel(ARCMSR_ARC1214_OUTBOUND_LIST_INTERRUPT_CLEAR,
+		pmu->outboundlist_interrupt_cause);
+	readl(pmu->outboundlist_interrupt_cause);
+	spin_unlock_irqrestore(&acb->doneq_lock, flags);
+}
+
 /*
 **********************************************************************************
 ** Handle a message interrupt
@@ -1546,14 +2024,14 @@ static void arcmsr_hbc_postqueue_isr(struct AdapterControlBlock *acb)
 ** We want this in order to compare the drivemap so that we can detect newly-attached drives.
 **********************************************************************************
 */
-static void arcmsr_hba_message_isr(struct AdapterControlBlock *acb)
+static void arcmsr_hbaA_message_isr(struct AdapterControlBlock *acb)
 {
-	struct MessageUnit_A *reg  = acb->pmuA;
+	struct MessageUnit_A __iomem *reg  = acb->pmuA;
 	/*clear interrupt and message state*/
 	writel(ARCMSR_MU_OUTBOUND_MESSAGE0_INT, &reg->outbound_intstatus);
 	schedule_work(&acb->arcmsr_do_message_isr_bh);
 }
-static void arcmsr_hbb_message_isr(struct AdapterControlBlock *acb)
+static void arcmsr_hbaB_message_isr(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg  = acb->pmuB;
 
@@ -1570,114 +2048,142 @@ static void arcmsr_hbb_message_isr(struct AdapterControlBlock *acb)
 ** We want this in order to compare the drivemap so that we can detect newly-attached drives.
 **********************************************************************************
 */
-static void arcmsr_hbc_message_isr(struct AdapterControlBlock *acb)
+static void arcmsr_hbaC_message_isr(struct AdapterControlBlock *acb)
 {
-	struct MessageUnit_C *reg  = acb->pmuC;
+	struct MessageUnit_C __iomem *reg  = acb->pmuC;
 	/*clear interrupt and message state*/
 	writel(ARCMSR_HBCMU_IOP2DRV_MESSAGE_CMD_DONE_DOORBELL_CLEAR, &reg->outbound_doorbell_clear);
 	schedule_work(&acb->arcmsr_do_message_isr_bh);
 }
 
-static int arcmsr_handle_hba_isr(struct AdapterControlBlock *acb)
+static void arcmsr_hbaD_message_isr(struct AdapterControlBlock *acb)
+{
+	struct MessageUnit_D *reg  = acb->pmuD;
+
+	writel(ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE, reg->outbound_doorbell);
+	readl(reg->outbound_doorbell);
+	schedule_work(&acb->arcmsr_do_message_isr_bh);
+}
+
+static int arcmsr_hbaA_handle_isr(struct AdapterControlBlock *acb)
 {
 	uint32_t outbound_intstatus;
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	outbound_intstatus = readl(&reg->outbound_intstatus) &
 		acb->outbound_int_enable;
-	if (!(outbound_intstatus & ARCMSR_MU_OUTBOUND_HANDLE_INT))	{
-		return 1;
-	}
-	writel(outbound_intstatus, &reg->outbound_intstatus);
-	if (outbound_intstatus & ARCMSR_MU_OUTBOUND_DOORBELL_INT)	{
-		arcmsr_hba_doorbell_isr(acb);
-	}
-	if (outbound_intstatus & ARCMSR_MU_OUTBOUND_POSTQUEUE_INT) {
-		arcmsr_hba_postqueue_isr(acb);
-	}
-	if(outbound_intstatus & ARCMSR_MU_OUTBOUND_MESSAGE0_INT) 	{
-		/* messenger of "driver to iop commands" */
-		arcmsr_hba_message_isr(acb);
-	}
-	return 0;
+	if (!(outbound_intstatus & ARCMSR_MU_OUTBOUND_HANDLE_INT))
+		return IRQ_NONE;
+	do {
+		writel(outbound_intstatus, &reg->outbound_intstatus);
+		if (outbound_intstatus & ARCMSR_MU_OUTBOUND_DOORBELL_INT)
+			arcmsr_hbaA_doorbell_isr(acb);
+		if (outbound_intstatus & ARCMSR_MU_OUTBOUND_POSTQUEUE_INT)
+			arcmsr_hbaA_postqueue_isr(acb);
+		if (outbound_intstatus & ARCMSR_MU_OUTBOUND_MESSAGE0_INT)
+			arcmsr_hbaA_message_isr(acb);
+		outbound_intstatus = readl(&reg->outbound_intstatus) &
+			acb->outbound_int_enable;
+	} while (outbound_intstatus & (ARCMSR_MU_OUTBOUND_DOORBELL_INT
+		| ARCMSR_MU_OUTBOUND_POSTQUEUE_INT
+		| ARCMSR_MU_OUTBOUND_MESSAGE0_INT));
+	return IRQ_HANDLED;
 }
 
-static int arcmsr_handle_hbb_isr(struct AdapterControlBlock *acb)
+static int arcmsr_hbaB_handle_isr(struct AdapterControlBlock *acb)
 {
 	uint32_t outbound_doorbell;
 	struct MessageUnit_B *reg = acb->pmuB;
 	outbound_doorbell = readl(reg->iop2drv_doorbell) &
 				acb->outbound_int_enable;
 	if (!outbound_doorbell)
-		return 1;
-
-	writel(~outbound_doorbell, reg->iop2drv_doorbell);
-	/*in case the last action of doorbell interrupt clearance is cached,
-	this action can push HW to write down the clear bit*/
-	readl(reg->iop2drv_doorbell);
-	writel(ARCMSR_DRV2IOP_END_OF_INTERRUPT, reg->drv2iop_doorbell);
-	if (outbound_doorbell & ARCMSR_IOP2DRV_DATA_WRITE_OK) {
-		arcmsr_iop2drv_data_wrote_handle(acb);
-	}
-	if (outbound_doorbell & ARCMSR_IOP2DRV_DATA_READ_OK) {
-		arcmsr_iop2drv_data_read_handle(acb);
-	}
-	if (outbound_doorbell & ARCMSR_IOP2DRV_CDB_DONE) {
-		arcmsr_hbb_postqueue_isr(acb);
-	}
-	if(outbound_doorbell & ARCMSR_IOP2DRV_MESSAGE_CMD_DONE) {
-		/* messenger of "driver to iop commands" */
-		arcmsr_hbb_message_isr(acb);
-	}
-	return 0;
+		return IRQ_NONE;
+	do {
+		writel(~outbound_doorbell, reg->iop2drv_doorbell);
+		writel(ARCMSR_DRV2IOP_END_OF_INTERRUPT, reg->drv2iop_doorbell);
+		if (outbound_doorbell & ARCMSR_IOP2DRV_DATA_WRITE_OK)
+			arcmsr_iop2drv_data_wrote_handle(acb);
+		if (outbound_doorbell & ARCMSR_IOP2DRV_DATA_READ_OK)
+			arcmsr_iop2drv_data_read_handle(acb);
+		if (outbound_doorbell & ARCMSR_IOP2DRV_CDB_DONE)
+			arcmsr_hbaB_postqueue_isr(acb);
+		if (outbound_doorbell & ARCMSR_IOP2DRV_MESSAGE_CMD_DONE)
+			arcmsr_hbaB_message_isr(acb);
+		outbound_doorbell = readl(reg->iop2drv_doorbell) &
+			acb->outbound_int_enable;
+	} while (outbound_doorbell & (ARCMSR_IOP2DRV_DATA_WRITE_OK
+		| ARCMSR_IOP2DRV_DATA_READ_OK
+		| ARCMSR_IOP2DRV_CDB_DONE
+		| ARCMSR_IOP2DRV_MESSAGE_CMD_DONE));
+	return IRQ_HANDLED;
 }
 
-static int arcmsr_handle_hbc_isr(struct AdapterControlBlock *pACB)
+static int arcmsr_hbaC_handle_isr(struct AdapterControlBlock *pACB)
 {
 	uint32_t host_interrupt_status;
-	struct MessageUnit_C *phbcmu = (struct MessageUnit_C *)pACB->pmuC;
+	struct MessageUnit_C __iomem *phbcmu = pACB->pmuC;
 	/*
 	*********************************************
 	**   check outbound intstatus
 	*********************************************
 	*/
-	host_interrupt_status = readl(&phbcmu->host_int_status);
-	if (!host_interrupt_status) {
-		/*it must be share irq*/
-		return 1;
-	}
-	/* MU ioctl transfer doorbell interrupts*/
-	if (host_interrupt_status & ARCMSR_HBCMU_OUTBOUND_DOORBELL_ISR) {
-		arcmsr_hbc_doorbell_isr(pACB);   /* messenger of "ioctl message read write" */
-	}
-	/* MU post queue interrupts*/
-	if (host_interrupt_status & ARCMSR_HBCMU_OUTBOUND_POSTQUEUE_ISR) {
-		arcmsr_hbc_postqueue_isr(pACB);  /* messenger of "scsi commands" */
-	}
-	return 0;
+	host_interrupt_status = readl(&phbcmu->host_int_status) &
+		(ARCMSR_HBCMU_OUTBOUND_POSTQUEUE_ISR |
+		ARCMSR_HBCMU_OUTBOUND_DOORBELL_ISR);
+	if (!host_interrupt_status)
+		return IRQ_NONE;
+	do {
+		if (host_interrupt_status & ARCMSR_HBCMU_OUTBOUND_DOORBELL_ISR)
+			arcmsr_hbaC_doorbell_isr(pACB);
+		/* MU post queue interrupts*/
+		if (host_interrupt_status & ARCMSR_HBCMU_OUTBOUND_POSTQUEUE_ISR)
+			arcmsr_hbaC_postqueue_isr(pACB);
+		host_interrupt_status = readl(&phbcmu->host_int_status);
+	} while (host_interrupt_status & (ARCMSR_HBCMU_OUTBOUND_POSTQUEUE_ISR |
+		ARCMSR_HBCMU_OUTBOUND_DOORBELL_ISR));
+	return IRQ_HANDLED;
 }
+
+static irqreturn_t arcmsr_hbaD_handle_isr(struct AdapterControlBlock *pACB)
+{
+	u32 host_interrupt_status;
+	struct MessageUnit_D  *pmu = pACB->pmuD;
+
+	host_interrupt_status = readl(pmu->host_int_status) &
+		(ARCMSR_ARC1214_OUTBOUND_POSTQUEUE_ISR |
+		ARCMSR_ARC1214_OUTBOUND_DOORBELL_ISR);
+	if (!host_interrupt_status)
+		return IRQ_NONE;
+	do {
+		/* MU post queue interrupts*/
+		if (host_interrupt_status &
+			ARCMSR_ARC1214_OUTBOUND_POSTQUEUE_ISR)
+			arcmsr_hbaD_postqueue_isr(pACB);
+		if (host_interrupt_status &
+			ARCMSR_ARC1214_OUTBOUND_DOORBELL_ISR)
+			arcmsr_hbaD_doorbell_isr(pACB);
+		host_interrupt_status = readl(pmu->host_int_status);
+	} while (host_interrupt_status &
+		(ARCMSR_ARC1214_OUTBOUND_POSTQUEUE_ISR |
+		ARCMSR_ARC1214_OUTBOUND_DOORBELL_ISR));
+	return IRQ_HANDLED;
+}
+
 static irqreturn_t arcmsr_interrupt(struct AdapterControlBlock *acb)
 {
 	switch (acb->adapter_type) {
-	case ACB_ADAPTER_TYPE_A: {
-		if (arcmsr_handle_hba_isr(acb)) {
-			return IRQ_NONE;
-		}
-		}
+	case ACB_ADAPTER_TYPE_A:
+		return arcmsr_hbaA_handle_isr(acb);
 		break;
-
-	case ACB_ADAPTER_TYPE_B: {
-		if (arcmsr_handle_hbb_isr(acb)) {
-			return IRQ_NONE;
-		}
-		}
+	case ACB_ADAPTER_TYPE_B:
+		return arcmsr_hbaB_handle_isr(acb);
 		break;
-	 case ACB_ADAPTER_TYPE_C: {
-		if (arcmsr_handle_hbc_isr(acb)) {
-			return IRQ_NONE;
-		}
-		}
+	case ACB_ADAPTER_TYPE_C:
+		return arcmsr_hbaC_handle_isr(acb);
+	case ACB_ADAPTER_TYPE_D:
+		return arcmsr_hbaD_handle_isr(acb);
+	default:
+		return IRQ_NONE;
 	}
-	return IRQ_HANDLED;
 }
 
 static void arcmsr_iop_parking(struct AdapterControlBlock *acb)
@@ -1695,296 +2201,273 @@ static void arcmsr_iop_parking(struct AdapterControlBlock *acb)
 	}
 }
 
-void arcmsr_post_ioctldata2iop(struct AdapterControlBlock *acb)
+
+void arcmsr_clear_iop2drv_rqueue_buffer(struct AdapterControlBlock *acb)
 {
-	int32_t wqbuf_firstindex, wqbuf_lastindex;
-	uint8_t *pQbuffer;
-	struct QBUFFER __iomem *pwbuffer;
-	uint8_t __iomem *iop_data;
-	int32_t allxfer_len = 0;
-	pwbuffer = arcmsr_get_iop_wqbuffer(acb);
-	iop_data = (uint8_t __iomem *)pwbuffer->data;
-	if (acb->acb_flags & ACB_F_MESSAGE_WQBUFFER_READED) {
-		acb->acb_flags &= (~ACB_F_MESSAGE_WQBUFFER_READED);
-		wqbuf_firstindex = acb->wqbuf_firstindex;
-		wqbuf_lastindex = acb->wqbuf_lastindex;
-		while ((wqbuf_firstindex != wqbuf_lastindex) && (allxfer_len < 124)) {
-			pQbuffer = &acb->wqbuffer[wqbuf_firstindex];
-			memcpy(iop_data, pQbuffer, 1);
-			wqbuf_firstindex++;
-			wqbuf_firstindex %= ARCMSR_MAX_QBUFFER;
-			iop_data++;
-			allxfer_len++;
+	uint32_t	i;
+
+	if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
+		for (i = 0; i < 15; i++) {
+			if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
+				acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
+				acb->rqbuf_getIndex = 0;
+				acb->rqbuf_putIndex = 0;
+				arcmsr_iop_message_read(acb);
+				mdelay(30);
+			} else if (acb->rqbuf_getIndex !=
+				   acb->rqbuf_putIndex) {
+				acb->rqbuf_getIndex = 0;
+				acb->rqbuf_putIndex = 0;
+				mdelay(30);
+			} else
+				break;
 		}
-		acb->wqbuf_firstindex = wqbuf_firstindex;
-		pwbuffer->data_len = allxfer_len;
-		arcmsr_iop_message_wrote(acb);
 	}
 }
 
 static int arcmsr_iop_message_xfer(struct AdapterControlBlock *acb,
-					struct scsi_cmnd *cmd)
+		struct scsi_cmnd *cmd)
 {
-	struct CMD_MESSAGE_FIELD *pcmdmessagefld;
-	int retvalue = 0, transfer_len = 0;
 	char *buffer;
+	unsigned short use_sg;
+	int retvalue = 0, transfer_len = 0;
+	unsigned long flags;
+	struct CMD_MESSAGE_FIELD *pcmdmessagefld;
+	uint32_t controlcode = (uint32_t)cmd->cmnd[5] << 24 |
+		(uint32_t)cmd->cmnd[6] << 16 |
+		(uint32_t)cmd->cmnd[7] << 8 |
+		(uint32_t)cmd->cmnd[8];
 	struct scatterlist *sg;
-	uint32_t controlcode = (uint32_t ) cmd->cmnd[5] << 24 |
-						(uint32_t ) cmd->cmnd[6] << 16 |
-						(uint32_t ) cmd->cmnd[7] << 8  |
-						(uint32_t ) cmd->cmnd[8];
-						/* 4 bytes: Areca io control code */
+
+	use_sg = scsi_sg_count(cmd);
 	sg = scsi_sglist(cmd);
 	buffer = kmap_atomic(sg_page(sg)) + sg->offset;
-	if (scsi_sg_count(cmd) > 1) {
+	if (use_sg > 1) {
 		retvalue = ARCMSR_MESSAGE_FAIL;
 		goto message_out;
 	}
 	transfer_len += sg->length;
-
 	if (transfer_len > sizeof(struct CMD_MESSAGE_FIELD)) {
 		retvalue = ARCMSR_MESSAGE_FAIL;
+		pr_info("%s: ARCMSR_MESSAGE_FAIL!\n", __func__);
 		goto message_out;
 	}
-	pcmdmessagefld = (struct CMD_MESSAGE_FIELD *) buffer;
-	switch(controlcode) {
-
+	pcmdmessagefld = (struct CMD_MESSAGE_FIELD *)buffer;
+	switch (controlcode) {
 	case ARCMSR_MESSAGE_READ_RQBUFFER: {
 		unsigned char *ver_addr;
-		uint8_t *pQbuffer, *ptmpQbuffer;
-		int32_t allxfer_len = 0;
-
-		ver_addr = kmalloc(1032, GFP_ATOMIC);
+		uint8_t *ptmpQbuffer;
+		uint32_t allxfer_len = 0;
+		ver_addr = kmalloc(ARCMSR_API_DATA_BUFLEN, GFP_ATOMIC);
 		if (!ver_addr) {
 			retvalue = ARCMSR_MESSAGE_FAIL;
+			pr_info("%s: memory not enough!\n", __func__);
 			goto message_out;
 		}
-				
 		ptmpQbuffer = ver_addr;
-		while ((acb->rqbuf_firstindex != acb->rqbuf_lastindex)
-			&& (allxfer_len < 1031)) {
-			pQbuffer = &acb->rqbuffer[acb->rqbuf_firstindex];
-			memcpy(ptmpQbuffer, pQbuffer, 1);
-			acb->rqbuf_firstindex++;
-			acb->rqbuf_firstindex %= ARCMSR_MAX_QBUFFER;
-			ptmpQbuffer++;
-			allxfer_len++;
+		spin_lock_irqsave(&acb->rqbuffer_lock, flags);
+		if (acb->rqbuf_getIndex != acb->rqbuf_putIndex) {
+			unsigned int tail = acb->rqbuf_getIndex;
+			unsigned int head = acb->rqbuf_putIndex;
+			unsigned int cnt_to_end = CIRC_CNT_TO_END(head, tail, ARCMSR_MAX_QBUFFER);
+
+			allxfer_len = CIRC_CNT(head, tail, ARCMSR_MAX_QBUFFER);
+			if (allxfer_len > ARCMSR_API_DATA_BUFLEN)
+				allxfer_len = ARCMSR_API_DATA_BUFLEN;
+
+			if (allxfer_len <= cnt_to_end)
+				memcpy(ptmpQbuffer, acb->rqbuffer + tail, allxfer_len);
+			else {
+				memcpy(ptmpQbuffer, acb->rqbuffer + tail, cnt_to_end);
+				memcpy(ptmpQbuffer + cnt_to_end, acb->rqbuffer, allxfer_len - cnt_to_end);
+			}
+			acb->rqbuf_getIndex = (acb->rqbuf_getIndex + allxfer_len) % ARCMSR_MAX_QBUFFER;
 		}
+		memcpy(pcmdmessagefld->messagedatabuffer, ver_addr,
+			allxfer_len);
 		if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
-
 			struct QBUFFER __iomem *prbuffer;
-			uint8_t __iomem *iop_data;
-			int32_t iop_len;
-
 			acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
 			prbuffer = arcmsr_get_iop_rqbuffer(acb);
-			iop_data = prbuffer->data;
-			iop_len = readl(&prbuffer->data_len);
-			while (iop_len > 0) {
-				acb->rqbuffer[acb->rqbuf_lastindex] = readb(iop_data);
-				acb->rqbuf_lastindex++;
-				acb->rqbuf_lastindex %= ARCMSR_MAX_QBUFFER;
-				iop_data++;
-				iop_len--;
-			}
-			arcmsr_iop_message_read(acb);
-		}
-		memcpy(pcmdmessagefld->messagedatabuffer, ver_addr, allxfer_len);
-		pcmdmessagefld->cmdmessage.Length = allxfer_len;
-		if(acb->fw_flag == FW_DEADLOCK) {
-			pcmdmessagefld->cmdmessage.ReturnCode = ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}else{
-			pcmdmessagefld->cmdmessage.ReturnCode = ARCMSR_MESSAGE_RETURNCODE_OK;
+			if (arcmsr_Read_iop_rqbuffer_data(acb, prbuffer) == 0)
+				acb->acb_flags |= ACB_F_IOPDATA_OVERFLOW;
 		}
+		spin_unlock_irqrestore(&acb->rqbuffer_lock, flags);
 		kfree(ver_addr);
-		}
+		pcmdmessagefld->cmdmessage.Length = allxfer_len;
+		if (acb->fw_flag == FW_DEADLOCK)
+			pcmdmessagefld->cmdmessage.ReturnCode =
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
+			pcmdmessagefld->cmdmessage.ReturnCode =
+				ARCMSR_MESSAGE_RETURNCODE_OK;
 		break;
-
+	}
 	case ARCMSR_MESSAGE_WRITE_WQBUFFER: {
 		unsigned char *ver_addr;
-		int32_t my_empty_len, user_len, wqbuf_firstindex, wqbuf_lastindex;
+		int32_t user_len, cnt2end;
 		uint8_t *pQbuffer, *ptmpuserbuffer;
-
-		ver_addr = kmalloc(1032, GFP_ATOMIC);
+		ver_addr = kmalloc(ARCMSR_API_DATA_BUFLEN, GFP_ATOMIC);
 		if (!ver_addr) {
 			retvalue = ARCMSR_MESSAGE_FAIL;
 			goto message_out;
 		}
-		if(acb->fw_flag == FW_DEADLOCK) {
-			pcmdmessagefld->cmdmessage.ReturnCode = 
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}else{
-			pcmdmessagefld->cmdmessage.ReturnCode = 
-			ARCMSR_MESSAGE_RETURNCODE_OK;
-		}
 		ptmpuserbuffer = ver_addr;
 		user_len = pcmdmessagefld->cmdmessage.Length;
-		memcpy(ptmpuserbuffer, pcmdmessagefld->messagedatabuffer, user_len);
-		wqbuf_lastindex = acb->wqbuf_lastindex;
-		wqbuf_firstindex = acb->wqbuf_firstindex;
-		if (wqbuf_lastindex != wqbuf_firstindex) {
+		memcpy(ptmpuserbuffer,
+			pcmdmessagefld->messagedatabuffer, user_len);
+		spin_lock_irqsave(&acb->wqbuffer_lock, flags);
+		if (acb->wqbuf_putIndex != acb->wqbuf_getIndex) {
 			struct SENSE_DATA *sensebuffer =
 				(struct SENSE_DATA *)cmd->sense_buffer;
-			arcmsr_post_ioctldata2iop(acb);
+			arcmsr_write_ioctldata2iop(acb);
 			/* has error report sensedata */
-			sensebuffer->ErrorCode = 0x70;
+			sensebuffer->ErrorCode = SCSI_SENSE_CURRENT_ERRORS;
 			sensebuffer->SenseKey = ILLEGAL_REQUEST;
 			sensebuffer->AdditionalSenseLength = 0x0A;
 			sensebuffer->AdditionalSenseCode = 0x20;
 			sensebuffer->Valid = 1;
 			retvalue = ARCMSR_MESSAGE_FAIL;
 		} else {
-			my_empty_len = (wqbuf_firstindex-wqbuf_lastindex - 1)
-				&(ARCMSR_MAX_QBUFFER - 1);
-			if (my_empty_len >= user_len) {
-				while (user_len > 0) {
-					pQbuffer =
-					&acb->wqbuffer[acb->wqbuf_lastindex];
-					memcpy(pQbuffer, ptmpuserbuffer, 1);
-					acb->wqbuf_lastindex++;
-					acb->wqbuf_lastindex %= ARCMSR_MAX_QBUFFER;
-					ptmpuserbuffer++;
-					user_len--;
-				}
-				if (acb->acb_flags & ACB_F_MESSAGE_WQBUFFER_CLEARED) {
-					acb->acb_flags &=
-						~ACB_F_MESSAGE_WQBUFFER_CLEARED;
-					arcmsr_post_ioctldata2iop(acb);
-				}
-			} else {
-				/* has error report sensedata */
-				struct SENSE_DATA *sensebuffer =
-					(struct SENSE_DATA *)cmd->sense_buffer;
-				sensebuffer->ErrorCode = 0x70;
-				sensebuffer->SenseKey = ILLEGAL_REQUEST;
-				sensebuffer->AdditionalSenseLength = 0x0A;
-				sensebuffer->AdditionalSenseCode = 0x20;
-				sensebuffer->Valid = 1;
-				retvalue = ARCMSR_MESSAGE_FAIL;
+			pQbuffer = &acb->wqbuffer[acb->wqbuf_putIndex];
+			cnt2end = ARCMSR_MAX_QBUFFER - acb->wqbuf_putIndex;
+			if (user_len > cnt2end) {
+				memcpy(pQbuffer, ptmpuserbuffer, cnt2end);
+				ptmpuserbuffer += cnt2end;
+				user_len -= cnt2end;
+				acb->wqbuf_putIndex = 0;
+				pQbuffer = acb->wqbuffer;
 			}
+			memcpy(pQbuffer, ptmpuserbuffer, user_len);
+			acb->wqbuf_putIndex += user_len;
+			acb->wqbuf_putIndex %= ARCMSR_MAX_QBUFFER;
+			if (acb->acb_flags & ACB_F_MESSAGE_WQBUFFER_CLEARED) {
+				acb->acb_flags &=
+						~ACB_F_MESSAGE_WQBUFFER_CLEARED;
+				arcmsr_write_ioctldata2iop(acb);
 			}
-			kfree(ver_addr);
 		}
+		spin_unlock_irqrestore(&acb->wqbuffer_lock, flags);
+		kfree(ver_addr);
+		if (acb->fw_flag == FW_DEADLOCK)
+			pcmdmessagefld->cmdmessage.ReturnCode =
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
+			pcmdmessagefld->cmdmessage.ReturnCode =
+				ARCMSR_MESSAGE_RETURNCODE_OK;
 		break;
-
+	}
 	case ARCMSR_MESSAGE_CLEAR_RQBUFFER: {
 		uint8_t *pQbuffer = acb->rqbuffer;
-		if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
-			acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
-			arcmsr_iop_message_read(acb);
-		}
+
+		arcmsr_clear_iop2drv_rqueue_buffer(acb);
+		spin_lock_irqsave(&acb->rqbuffer_lock, flags);
 		acb->acb_flags |= ACB_F_MESSAGE_RQBUFFER_CLEARED;
-		acb->rqbuf_firstindex = 0;
-		acb->rqbuf_lastindex = 0;
+		acb->rqbuf_getIndex = 0;
+		acb->rqbuf_putIndex = 0;
 		memset(pQbuffer, 0, ARCMSR_MAX_QBUFFER);
-		if(acb->fw_flag == FW_DEADLOCK) {
+		spin_unlock_irqrestore(&acb->rqbuffer_lock, flags);
+		if (acb->fw_flag == FW_DEADLOCK)
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}else{
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_OK;
-		}
-		}
+				ARCMSR_MESSAGE_RETURNCODE_OK;
 		break;
-
+	}
 	case ARCMSR_MESSAGE_CLEAR_WQBUFFER: {
 		uint8_t *pQbuffer = acb->wqbuffer;
-		if(acb->fw_flag == FW_DEADLOCK) {
+		spin_lock_irqsave(&acb->wqbuffer_lock, flags);
+		acb->acb_flags |= (ACB_F_MESSAGE_WQBUFFER_CLEARED |
+			ACB_F_MESSAGE_WQBUFFER_READED);
+		acb->wqbuf_getIndex = 0;
+		acb->wqbuf_putIndex = 0;
+		memset(pQbuffer, 0, ARCMSR_MAX_QBUFFER);
+		spin_unlock_irqrestore(&acb->wqbuffer_lock, flags);
+		if (acb->fw_flag == FW_DEADLOCK)
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}else{
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_OK;
-		}
-
-		if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
-			acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
-			arcmsr_iop_message_read(acb);
-		}
-		acb->acb_flags |=
-			(ACB_F_MESSAGE_WQBUFFER_CLEARED |
-				ACB_F_MESSAGE_WQBUFFER_READED);
-		acb->wqbuf_firstindex = 0;
-		acb->wqbuf_lastindex = 0;
-		memset(pQbuffer, 0, ARCMSR_MAX_QBUFFER);
-		}
+				ARCMSR_MESSAGE_RETURNCODE_OK;
 		break;
-
+	}
 	case ARCMSR_MESSAGE_CLEAR_ALLQBUFFER: {
 		uint8_t *pQbuffer;
-
-		if (acb->acb_flags & ACB_F_IOPDATA_OVERFLOW) {
-			acb->acb_flags &= ~ACB_F_IOPDATA_OVERFLOW;
-			arcmsr_iop_message_read(acb);
-		}
-		acb->acb_flags |=
-			(ACB_F_MESSAGE_WQBUFFER_CLEARED
-			| ACB_F_MESSAGE_RQBUFFER_CLEARED
-			| ACB_F_MESSAGE_WQBUFFER_READED);
-		acb->rqbuf_firstindex = 0;
-		acb->rqbuf_lastindex = 0;
-		acb->wqbuf_firstindex = 0;
-		acb->wqbuf_lastindex = 0;
+		arcmsr_clear_iop2drv_rqueue_buffer(acb);
+		spin_lock_irqsave(&acb->rqbuffer_lock, flags);
+		acb->acb_flags |= ACB_F_MESSAGE_RQBUFFER_CLEARED;
+		acb->rqbuf_getIndex = 0;
+		acb->rqbuf_putIndex = 0;
 		pQbuffer = acb->rqbuffer;
 		memset(pQbuffer, 0, sizeof(struct QBUFFER));
+		spin_unlock_irqrestore(&acb->rqbuffer_lock, flags);
+		spin_lock_irqsave(&acb->wqbuffer_lock, flags);
+		acb->acb_flags |= (ACB_F_MESSAGE_WQBUFFER_CLEARED |
+			ACB_F_MESSAGE_WQBUFFER_READED);
+		acb->wqbuf_getIndex = 0;
+		acb->wqbuf_putIndex = 0;
 		pQbuffer = acb->wqbuffer;
 		memset(pQbuffer, 0, sizeof(struct QBUFFER));
-		if(acb->fw_flag == FW_DEADLOCK) {
+		spin_unlock_irqrestore(&acb->wqbuffer_lock, flags);
+		if (acb->fw_flag == FW_DEADLOCK)
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}else{
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_OK;
-		}
-		}
+				ARCMSR_MESSAGE_RETURNCODE_OK;
 		break;
-
+	}
 	case ARCMSR_MESSAGE_RETURN_CODE_3F: {
-		if(acb->fw_flag == FW_DEADLOCK) {
+		if (acb->fw_flag == FW_DEADLOCK)
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}else{
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_3F;
-		}
+				ARCMSR_MESSAGE_RETURNCODE_3F;
 		break;
-		}
+	}
 	case ARCMSR_MESSAGE_SAY_HELLO: {
 		int8_t *hello_string = "Hello! I am ARCMSR";
-		if(acb->fw_flag == FW_DEADLOCK) {
+		if (acb->fw_flag == FW_DEADLOCK)
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}else{
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_OK;
-		}
-		memcpy(pcmdmessagefld->messagedatabuffer, hello_string
-			, (int16_t)strlen(hello_string));
-		}
+				ARCMSR_MESSAGE_RETURNCODE_OK;
+		memcpy(pcmdmessagefld->messagedatabuffer,
+			hello_string, (int16_t)strlen(hello_string));
 		break;
-
-	case ARCMSR_MESSAGE_SAY_GOODBYE:
-		if(acb->fw_flag == FW_DEADLOCK) {
+	}
+	case ARCMSR_MESSAGE_SAY_GOODBYE: {
+		if (acb->fw_flag == FW_DEADLOCK)
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
+			pcmdmessagefld->cmdmessage.ReturnCode =
+				ARCMSR_MESSAGE_RETURNCODE_OK;
 		arcmsr_iop_parking(acb);
 		break;
-
-	case ARCMSR_MESSAGE_FLUSH_ADAPTER_CACHE:
-		if(acb->fw_flag == FW_DEADLOCK) {
+	}
+	case ARCMSR_MESSAGE_FLUSH_ADAPTER_CACHE: {
+		if (acb->fw_flag == FW_DEADLOCK)
 			pcmdmessagefld->cmdmessage.ReturnCode =
-			ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
-		}
+				ARCMSR_MESSAGE_RETURNCODE_BUS_HANG_ON;
+		else
+			pcmdmessagefld->cmdmessage.ReturnCode =
+				ARCMSR_MESSAGE_RETURNCODE_OK;
 		arcmsr_flush_adapter_cache(acb);
 		break;
-
+	}
 	default:
 		retvalue = ARCMSR_MESSAGE_FAIL;
+		pr_info("%s: unknown controlcode!\n", __func__);
+	}
+message_out:
+	if (use_sg) {
+		struct scatterlist *sg = scsi_sglist(cmd);
+		kunmap_atomic(buffer - sg->offset);
 	}
-	message_out:
-	sg = scsi_sglist(cmd);
-	kunmap_atomic(buffer - sg->offset);
 	return retvalue;
 }
 
@@ -1999,7 +2482,7 @@ static struct CommandControlBlock *arcmsr_get_freeccb(struct AdapterControlBlock
 		list_del_init(&ccb->list);
 	}else{
 		spin_unlock_irqrestore(&acb->ccblist_lock, flags);
-		return 0;
+		return NULL;
 	}
 	spin_unlock_irqrestore(&acb->ccblist_lock, flags);
 	return ccb;
@@ -2079,9 +2562,6 @@ static int arcmsr_queue_command_lck(struct scsi_cmnd *cmd,
 		arcmsr_handle_virtual_command(acb, cmd);
 		return 0;
 	}
-	if (atomic_read(&acb->ccboutstandingcount) >=
-			ARCMSR_MAX_OUTSTANDING_CMD)
-		return SCSI_MLQUEUE_HOST_BUSY;
 	ccb = arcmsr_get_freeccb(acb);
 	if (!ccb)
 		return SCSI_MLQUEUE_HOST_BUSY;
@@ -2096,7 +2576,7 @@ static int arcmsr_queue_command_lck(struct scsi_cmnd *cmd,
 
 static DEF_SCSI_QCMD(arcmsr_queue_command)
 
-static bool arcmsr_get_hba_config(struct AdapterControlBlock *acb)
+static bool arcmsr_hbaA_get_config(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	char *acb_firm_model = acb->firm_model;
@@ -2107,7 +2587,7 @@ static bool arcmsr_get_hba_config(struct AdapterControlBlock *acb)
 	char __iomem *iop_device_map = (char __iomem *)(&reg->message_rwbuffer[21]);
 	int count;
 	writel(ARCMSR_INBOUND_MESG0_GET_CONFIG, &reg->inbound_msgaddr0);
-	if (!arcmsr_hba_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaA_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE "arcmsr%d: wait 'get adapter firmware \
 			miscellaneous data' timeout \n", acb->host->host_no);
 		return false;
@@ -2135,10 +2615,10 @@ static bool arcmsr_get_hba_config(struct AdapterControlBlock *acb)
 		iop_device_map++;
 		count--;
 	}
-	printk(KERN_NOTICE "Areca RAID Controller%d: F/W %s & Model %s\n", 
+	pr_notice("Areca RAID Controller%d: Model %s, F/W %s\n",
 		acb->host->host_no,
-		acb->firm_version,
-		acb->firm_model);
+		acb->firm_model,
+		acb->firm_version);
 	acb->signature = readl(&reg->message_rwbuffer[0]);
 	acb->firm_request_len = readl(&reg->message_rwbuffer[1]);
 	acb->firm_numbers_queue = readl(&reg->message_rwbuffer[2]);
@@ -2147,7 +2627,7 @@ static bool arcmsr_get_hba_config(struct AdapterControlBlock *acb)
 	acb->firm_cfg_version = readl(&reg->message_rwbuffer[25]);  /*firm_cfg_version,25,100-103*/
 	return true;
 }
-static bool arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
+static bool arcmsr_hbaB_get_config(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
 	struct pci_dev *pdev = acb->pdev;
@@ -2163,12 +2643,18 @@ static bool arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
 	char __iomem *iop_device_map;
 	/*firm_version,21,84-99*/
 	int count;
-	dma_coherent = dma_alloc_coherent(&pdev->dev, sizeof(struct MessageUnit_B), &dma_coherent_handle, GFP_KERNEL);
+
+	acb->roundup_ccbsize = roundup(sizeof(struct MessageUnit_B), 32);
+	dma_coherent = dma_alloc_coherent(&pdev->dev, acb->roundup_ccbsize,
+			&dma_coherent_handle, GFP_KERNEL);
 	if (!dma_coherent){
-		printk(KERN_NOTICE "arcmsr%d: dma_alloc_coherent got error for hbb mu\n", acb->host->host_no);
+		printk(KERN_NOTICE
+			"arcmsr%d: dma_alloc_coherent got error for hbb mu\n",
+			acb->host->host_no);
 		return false;
 	}
-	acb->dma_coherent_handle_hbb_mu = dma_coherent_handle;
+	acb->dma_coherent_handle2 = dma_coherent_handle;
+	acb->dma_coherent2 = dma_coherent;
 	reg = (struct MessageUnit_B *)dma_coherent;
 	acb->pmuB = reg;
 	reg->drv2iop_doorbell= (uint32_t __iomem *)((unsigned long)acb->mem_base0 + ARCMSR_DRV2IOP_DOORBELL);
@@ -2183,7 +2669,7 @@ static bool arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
 	iop_device_map = (char __iomem *)(&reg->message_rwbuffer[21]);	/*firm_version,21,84-99*/
 
 	writel(ARCMSR_MESSAGE_GET_CONFIG, reg->drv2iop_doorbell);
-	if (!arcmsr_hbb_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE "arcmsr%d: wait 'get adapter firmware \
 			miscellaneous data' timeout \n", acb->host->host_no);
 		return false;
@@ -2211,10 +2697,10 @@ static bool arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
 		count--;
 	}
 	
-	printk(KERN_NOTICE "Areca RAID Controller%d: F/W %s & Model %s\n",
+	pr_notice("Areca RAID Controller%d: Model %s, F/W %s\n",
 		acb->host->host_no,
-		acb->firm_version,
-		acb->firm_model);
+		acb->firm_model,
+		acb->firm_version);
 
 	acb->signature = readl(&reg->message_rwbuffer[1]);
 	/*firm_signature,1,00-03*/
@@ -2231,14 +2717,14 @@ static bool arcmsr_get_hbb_config(struct AdapterControlBlock *acb)
 	return true;
 }
 
-static bool arcmsr_get_hbc_config(struct AdapterControlBlock *pACB)
+static bool arcmsr_hbaC_get_config(struct AdapterControlBlock *pACB)
 {
 	uint32_t intmask_org, Index, firmware_state = 0;
-	struct MessageUnit_C *reg = pACB->pmuC;
+	struct MessageUnit_C __iomem *reg = pACB->pmuC;
 	char *acb_firm_model = pACB->firm_model;
 	char *acb_firm_version = pACB->firm_version;
-	char *iop_firm_model = (char *)(&reg->msgcode_rwbuffer[15]);    /*firm_model,15,60-67*/
-	char *iop_firm_version = (char *)(&reg->msgcode_rwbuffer[17]);  /*firm_version,17,68-83*/
+	char __iomem *iop_firm_model = (char __iomem *)(&reg->msgcode_rwbuffer[15]);    /*firm_model,15,60-67*/
+	char __iomem *iop_firm_version = (char __iomem *)(&reg->msgcode_rwbuffer[17]);  /*firm_version,17,68-83*/
 	int count;
 	/* disable all outbound interrupt */
 	intmask_org = readl(&reg->host_int_mask); /* disable outbound message0 int */
@@ -2277,10 +2763,10 @@ static bool arcmsr_get_hbc_config(struct AdapterControlBlock *pACB)
 		iop_firm_version++;
 		count--;
 	}
-	printk(KERN_NOTICE "Areca RAID Controller%d: F/W %s & Model %s\n",
+	pr_notice("Areca RAID Controller%d: Model %s, F/W %s\n",
 		pACB->host->host_no,
-		pACB->firm_version,
-		pACB->firm_model);
+		pACB->firm_model,
+		pACB->firm_version);
 	pACB->firm_request_len = readl(&reg->msgcode_rwbuffer[1]);   /*firm_request_len,1,04-07*/
 	pACB->firm_numbers_queue = readl(&reg->msgcode_rwbuffer[2]); /*firm_numbers_queue,2,08-11*/
 	pACB->firm_sdram_size = readl(&reg->msgcode_rwbuffer[3]);    /*firm_sdram_size,3,12-15*/
@@ -2289,17 +2775,166 @@ static bool arcmsr_get_hbc_config(struct AdapterControlBlock *pACB)
 	/*all interrupt service will be enable at arcmsr_iop_init*/
 	return true;
 }
+
+static bool arcmsr_hbaD_get_config(struct AdapterControlBlock *acb)
+{
+	char *acb_firm_model = acb->firm_model;
+	char *acb_firm_version = acb->firm_version;
+	char *acb_device_map = acb->device_map;
+	char __iomem *iop_firm_model;
+	char __iomem *iop_firm_version;
+	char __iomem *iop_device_map;
+	u32 count;
+	struct MessageUnit_D *reg;
+	void *dma_coherent2;
+	dma_addr_t dma_coherent_handle2;
+	struct pci_dev *pdev = acb->pdev;
+
+	acb->roundup_ccbsize = roundup(sizeof(struct MessageUnit_D), 32);
+	dma_coherent2 = dma_alloc_coherent(&pdev->dev, acb->roundup_ccbsize,
+		&dma_coherent_handle2, GFP_KERNEL);
+	if (!dma_coherent2) {
+		pr_notice("DMA allocation failed...\n");
+		return false;
+	}
+	memset(dma_coherent2, 0, acb->roundup_ccbsize);
+	acb->dma_coherent_handle2 = dma_coherent_handle2;
+	acb->dma_coherent2 = dma_coherent2;
+	reg = (struct MessageUnit_D *)dma_coherent2;
+	acb->pmuD = reg;
+	reg->chip_id = acb->mem_base0 + ARCMSR_ARC1214_CHIP_ID;
+	reg->cpu_mem_config = acb->mem_base0 +
+		ARCMSR_ARC1214_CPU_MEMORY_CONFIGURATION;
+	reg->i2o_host_interrupt_mask = acb->mem_base0 +
+		ARCMSR_ARC1214_I2_HOST_INTERRUPT_MASK;
+	reg->sample_at_reset = acb->mem_base0 + ARCMSR_ARC1214_SAMPLE_RESET;
+	reg->reset_request = acb->mem_base0 + ARCMSR_ARC1214_RESET_REQUEST;
+	reg->host_int_status = acb->mem_base0 +
+		ARCMSR_ARC1214_MAIN_INTERRUPT_STATUS;
+	reg->pcief0_int_enable = acb->mem_base0 +
+		ARCMSR_ARC1214_PCIE_F0_INTERRUPT_ENABLE;
+	reg->inbound_msgaddr0 = acb->mem_base0 +
+		ARCMSR_ARC1214_INBOUND_MESSAGE0;
+	reg->inbound_msgaddr1 = acb->mem_base0 +
+		ARCMSR_ARC1214_INBOUND_MESSAGE1;
+	reg->outbound_msgaddr0 = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_MESSAGE0;
+	reg->outbound_msgaddr1 = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_MESSAGE1;
+	reg->inbound_doorbell = acb->mem_base0 +
+		ARCMSR_ARC1214_INBOUND_DOORBELL;
+	reg->outbound_doorbell = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_DOORBELL;
+	reg->outbound_doorbell_enable = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_DOORBELL_ENABLE;
+	reg->inboundlist_base_low = acb->mem_base0 +
+		ARCMSR_ARC1214_INBOUND_LIST_BASE_LOW;
+	reg->inboundlist_base_high = acb->mem_base0 +
+		ARCMSR_ARC1214_INBOUND_LIST_BASE_HIGH;
+	reg->inboundlist_write_pointer = acb->mem_base0 +
+		ARCMSR_ARC1214_INBOUND_LIST_WRITE_POINTER;
+	reg->outboundlist_base_low = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_LIST_BASE_LOW;
+	reg->outboundlist_base_high = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_LIST_BASE_HIGH;
+	reg->outboundlist_copy_pointer = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_LIST_COPY_POINTER;
+	reg->outboundlist_read_pointer = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_LIST_READ_POINTER;
+	reg->outboundlist_interrupt_cause = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_INTERRUPT_CAUSE;
+	reg->outboundlist_interrupt_enable = acb->mem_base0 +
+		ARCMSR_ARC1214_OUTBOUND_INTERRUPT_ENABLE;
+	reg->message_wbuffer = acb->mem_base0 + ARCMSR_ARC1214_MESSAGE_WBUFFER;
+	reg->message_rbuffer = acb->mem_base0 + ARCMSR_ARC1214_MESSAGE_RBUFFER;
+	reg->msgcode_rwbuffer = acb->mem_base0 +
+		ARCMSR_ARC1214_MESSAGE_RWBUFFER;
+	iop_firm_model = (char __iomem *)(&reg->msgcode_rwbuffer[15]);
+	iop_firm_version = (char __iomem *)(&reg->msgcode_rwbuffer[17]);
+	iop_device_map = (char __iomem *)(&reg->msgcode_rwbuffer[21]);
+	if (readl(acb->pmuD->outbound_doorbell) &
+		ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE) {
+		writel(ARCMSR_ARC1214_IOP2DRV_MESSAGE_CMD_DONE,
+			acb->pmuD->outbound_doorbell);/*clear interrupt*/
+	}
+	/* post "get config" instruction */
+	writel(ARCMSR_INBOUND_MESG0_GET_CONFIG, reg->inbound_msgaddr0);
+	/* wait message ready */
+	if (!arcmsr_hbaD_wait_msgint_ready(acb)) {
+		pr_notice("arcmsr%d: wait get adapter firmware "
+			"miscellaneous data timeout\n", acb->host->host_no);
+		dma_free_coherent(&acb->pdev->dev, acb->roundup_ccbsize,
+			acb->dma_coherent2, acb->dma_coherent_handle2);
+		return false;
+	}
+	count = 8;
+	while (count) {
+		*acb_firm_model = readb(iop_firm_model);
+		acb_firm_model++;
+		iop_firm_model++;
+		count--;
+	}
+	count = 16;
+	while (count) {
+		*acb_firm_version = readb(iop_firm_version);
+		acb_firm_version++;
+		iop_firm_version++;
+		count--;
+	}
+	count = 16;
+	while (count) {
+		*acb_device_map = readb(iop_device_map);
+		acb_device_map++;
+		iop_device_map++;
+		count--;
+	}
+	acb->signature = readl(&reg->msgcode_rwbuffer[1]);
+	/*firm_signature,1,00-03*/
+	acb->firm_request_len = readl(&reg->msgcode_rwbuffer[2]);
+	/*firm_request_len,1,04-07*/
+	acb->firm_numbers_queue = readl(&reg->msgcode_rwbuffer[3]);
+	/*firm_numbers_queue,2,08-11*/
+	acb->firm_sdram_size = readl(&reg->msgcode_rwbuffer[4]);
+	/*firm_sdram_size,3,12-15*/
+	acb->firm_hd_channels = readl(&reg->msgcode_rwbuffer[5]);
+	/*firm_hd_channels,4,16-19*/
+	acb->firm_cfg_version = readl(&reg->msgcode_rwbuffer[25]);
+	pr_notice("Areca RAID Controller%d: Model %s, F/W %s\n",
+		acb->host->host_no,
+		acb->firm_model,
+		acb->firm_version);
+	return true;
+}
+
 static bool arcmsr_get_firmware_spec(struct AdapterControlBlock *acb)
 {
-	if (acb->adapter_type == ACB_ADAPTER_TYPE_A)
-		return arcmsr_get_hba_config(acb);
-	else if (acb->adapter_type == ACB_ADAPTER_TYPE_B)
-		return arcmsr_get_hbb_config(acb);
+	bool rtn = false;
+
+	switch (acb->adapter_type) {
+	case ACB_ADAPTER_TYPE_A:
+		rtn = arcmsr_hbaA_get_config(acb);
+		break;
+	case ACB_ADAPTER_TYPE_B:
+		rtn = arcmsr_hbaB_get_config(acb);
+		break;
+	case ACB_ADAPTER_TYPE_C:
+		rtn = arcmsr_hbaC_get_config(acb);
+		break;
+	case ACB_ADAPTER_TYPE_D:
+		rtn = arcmsr_hbaD_get_config(acb);
+		break;
+	default:
+		break;
+	}
+	if (acb->firm_numbers_queue > ARCMSR_MAX_OUTSTANDING_CMD)
+		acb->maxOutstanding = ARCMSR_MAX_OUTSTANDING_CMD;
 	else
-		return arcmsr_get_hbc_config(acb);
+		acb->maxOutstanding = acb->firm_numbers_queue - 1;
+	acb->host->can_queue = acb->maxOutstanding;
+	return rtn;
 }
 
-static int arcmsr_polling_hba_ccbdone(struct AdapterControlBlock *acb,
+static int arcmsr_hbaA_polling_ccbdone(struct AdapterControlBlock *acb,
 	struct CommandControlBlock *poll_ccb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
@@ -2328,7 +2963,7 @@ static int arcmsr_polling_hba_ccbdone(struct AdapterControlBlock *acb,
 		}
 		arcmsr_cdb = (struct ARCMSR_CDB *)(acb->vir2phy_offset + (flag_ccb << 5));
 		ccb = container_of(arcmsr_cdb, struct CommandControlBlock, arcmsr_cdb);
-		poll_ccb_done = (ccb == poll_ccb) ? 1:0;
+		poll_ccb_done |= (ccb == poll_ccb) ? 1 : 0;
 		if ((ccb->acb != acb) || (ccb->startdone != ARCMSR_CCB_START)) {
 			if ((ccb->startdone == ARCMSR_CCB_ABORTED) || (ccb == poll_ccb)) {
 				printk(KERN_NOTICE "arcmsr%d: scsi id = %d lun = %d ccb = '0x%p'"
@@ -2355,7 +2990,7 @@ static int arcmsr_polling_hba_ccbdone(struct AdapterControlBlock *acb,
 	return rtn;
 }
 
-static int arcmsr_polling_hbb_ccbdone(struct AdapterControlBlock *acb,
+static int arcmsr_hbaB_polling_ccbdone(struct AdapterControlBlock *acb,
 					struct CommandControlBlock *poll_ccb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
@@ -2371,7 +3006,8 @@ static int arcmsr_polling_hbb_ccbdone(struct AdapterControlBlock *acb,
 	writel(ARCMSR_DOORBELL_INT_CLEAR_PATTERN, reg->iop2drv_doorbell);
 	while(1){
 		index = reg->doneq_index;
-		if ((flag_ccb = readl(&reg->done_qbuffer[index])) == 0) {
+		flag_ccb = reg->done_qbuffer[index];
+		if (flag_ccb == 0) {
 			if (poll_ccb_done){
 				rtn = SUCCESS;
 				break;
@@ -2384,7 +3020,7 @@ static int arcmsr_polling_hbb_ccbdone(struct AdapterControlBlock *acb,
 				goto polling_hbb_ccb_retry;
 			}
 		}
-		writel(0, &reg->done_qbuffer[index]);
+		reg->done_qbuffer[index] = 0;
 		index++;
 		/*if last index number set it to 0 */
 		index %= ARCMSR_MAX_HBB_POSTQUEUE;
@@ -2392,7 +3028,7 @@ static int arcmsr_polling_hbb_ccbdone(struct AdapterControlBlock *acb,
 		/* check if command done with no error*/
 		arcmsr_cdb = (struct ARCMSR_CDB *)(acb->vir2phy_offset + (flag_ccb << 5));
 		ccb = container_of(arcmsr_cdb, struct CommandControlBlock, arcmsr_cdb);
-		poll_ccb_done = (ccb == poll_ccb) ? 1:0;
+		poll_ccb_done |= (ccb == poll_ccb) ? 1 : 0;
 		if ((ccb->acb != acb) || (ccb->startdone != ARCMSR_CCB_START)) {
 			if ((ccb->startdone == ARCMSR_CCB_ABORTED) || (ccb == poll_ccb)) {
 				printk(KERN_NOTICE "arcmsr%d: scsi id = %d lun = %d ccb = '0x%p'"
@@ -2419,9 +3055,10 @@ static int arcmsr_polling_hbb_ccbdone(struct AdapterControlBlock *acb,
 	return rtn;
 }
 
-static int arcmsr_polling_hbc_ccbdone(struct AdapterControlBlock *acb, struct CommandControlBlock *poll_ccb)
+static int arcmsr_hbaC_polling_ccbdone(struct AdapterControlBlock *acb,
+		struct CommandControlBlock *poll_ccb)
 {
-	struct MessageUnit_C *reg = (struct MessageUnit_C *)acb->pmuC;
+	struct MessageUnit_C __iomem *reg = acb->pmuC;
 	uint32_t flag_ccb, ccb_cdb_phy;
 	struct ARCMSR_CDB *arcmsr_cdb;
 	bool error;
@@ -2448,7 +3085,7 @@ polling_hbc_ccb_retry:
 		ccb_cdb_phy = (flag_ccb & 0xFFFFFFF0);
 		arcmsr_cdb = (struct ARCMSR_CDB *)(acb->vir2phy_offset + ccb_cdb_phy);/*frame must be 32 bytes aligned*/
 		pCCB = container_of(arcmsr_cdb, struct CommandControlBlock, arcmsr_cdb);
-		poll_ccb_done = (pCCB == poll_ccb) ? 1 : 0;
+		poll_ccb_done |= (pCCB == poll_ccb) ? 1 : 0;
 		/* check ifcommand done with no error*/
 		if ((pCCB->acb != acb) || (pCCB->startdone != ARCMSR_CCB_START)) {
 			if (pCCB->startdone == ARCMSR_CCB_ABORTED) {
@@ -2475,6 +3112,81 @@ polling_hbc_ccb_retry:
 	}
 	return rtn;
 }
+
+static int arcmsr_hbaD_polling_ccbdone(struct AdapterControlBlock *acb,
+				struct CommandControlBlock *poll_ccb)
+{
+	bool error;
+	uint32_t poll_ccb_done = 0, poll_count = 0, flag_ccb, ccb_cdb_phy;
+	int rtn, doneq_index, index_stripped, outbound_write_pointer, toggle;
+	unsigned long flags;
+	struct ARCMSR_CDB *arcmsr_cdb;
+	struct CommandControlBlock *pCCB;
+	struct MessageUnit_D *pmu = acb->pmuD;
+
+polling_hbaD_ccb_retry:
+	poll_count++;
+	while (1) {
+		spin_lock_irqsave(&acb->doneq_lock, flags);
+		outbound_write_pointer = pmu->done_qbuffer[0].addressLow + 1;
+		doneq_index = pmu->doneq_index;
+		if ((outbound_write_pointer & 0xFFF) == (doneq_index & 0xFFF)) {
+			spin_unlock_irqrestore(&acb->doneq_lock, flags);
+			if (poll_ccb_done) {
+				rtn = SUCCESS;
+				break;
+			} else {
+				msleep(25);
+				if (poll_count > 40) {
+					rtn = FAILED;
+					break;
+				}
+				goto polling_hbaD_ccb_retry;
+			}
+		}
+		toggle = doneq_index & 0x4000;
+		index_stripped = (doneq_index & 0xFFF) + 1;
+		index_stripped %= ARCMSR_MAX_ARC1214_DONEQUEUE;
+		pmu->doneq_index = index_stripped ? (index_stripped | toggle) :
+				((toggle ^ 0x4000) + 1);
+		doneq_index = pmu->doneq_index;
+		spin_unlock_irqrestore(&acb->doneq_lock, flags);
+		flag_ccb = pmu->done_qbuffer[doneq_index & 0xFFF].addressLow;
+		ccb_cdb_phy = (flag_ccb & 0xFFFFFFF0);
+		arcmsr_cdb = (struct ARCMSR_CDB *)(acb->vir2phy_offset +
+			ccb_cdb_phy);
+		pCCB = container_of(arcmsr_cdb, struct CommandControlBlock,
+			arcmsr_cdb);
+		poll_ccb_done |= (pCCB == poll_ccb) ? 1 : 0;
+		if ((pCCB->acb != acb) ||
+			(pCCB->startdone != ARCMSR_CCB_START)) {
+			if (pCCB->startdone == ARCMSR_CCB_ABORTED) {
+				pr_notice("arcmsr%d: scsi id = %d "
+					"lun = %d ccb = '0x%p' poll command "
+					"abort successfully\n"
+					, acb->host->host_no
+					, pCCB->pcmd->device->id
+					, (u32)pCCB->pcmd->device->lun
+					, pCCB);
+				pCCB->pcmd->result = DID_ABORT << 16;
+				arcmsr_ccb_complete(pCCB);
+				continue;
+			}
+			pr_notice("arcmsr%d: polling an illegal "
+				"ccb command done ccb = '0x%p' "
+				"ccboutstandingcount = %d\n"
+				, acb->host->host_no
+				, pCCB
+				, atomic_read(&acb->ccboutstandingcount));
+			continue;
+		}
+		error = (flag_ccb & ARCMSR_CCBREPLY_FLAG_ERROR_MODE1)
+			? true : false;
+		arcmsr_report_ccb_state(acb, pCCB, error);
+	}
+	return rtn;
+}
+
 static int arcmsr_polling_ccbdone(struct AdapterControlBlock *acb,
 					struct CommandControlBlock *poll_ccb)
 {
@@ -2482,17 +3194,21 @@ static int arcmsr_polling_ccbdone(struct AdapterControlBlock *acb,
 	switch (acb->adapter_type) {
 
 	case ACB_ADAPTER_TYPE_A: {
-		rtn = arcmsr_polling_hba_ccbdone(acb, poll_ccb);
+		rtn = arcmsr_hbaA_polling_ccbdone(acb, poll_ccb);
 		}
 		break;
 
 	case ACB_ADAPTER_TYPE_B: {
-		rtn = arcmsr_polling_hbb_ccbdone(acb, poll_ccb);
+		rtn = arcmsr_hbaB_polling_ccbdone(acb, poll_ccb);
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		rtn = arcmsr_polling_hbc_ccbdone(acb, poll_ccb);
+		rtn = arcmsr_hbaC_polling_ccbdone(acb, poll_ccb);
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D:
+		rtn = arcmsr_hbaD_polling_ccbdone(acb, poll_ccb);
+		break;
 	}
 	return rtn;
 }
@@ -2500,6 +3216,7 @@ static int arcmsr_polling_ccbdone(struct AdapterControlBlock *acb,
 static int arcmsr_iop_confirm(struct AdapterControlBlock *acb)
 {
 	uint32_t cdb_phyaddr, cdb_phyaddr_hi32;
+	dma_addr_t dma_coherent_handle;
 
 	/*
 	********************************************************************
@@ -2507,8 +3224,17 @@ static int arcmsr_iop_confirm(struct AdapterControlBlock *acb)
 	** if freeccb.HighPart is not zero
 	********************************************************************
 	*/
-	cdb_phyaddr = lower_32_bits(acb->dma_coherent_handle);
-	cdb_phyaddr_hi32 = upper_32_bits(acb->dma_coherent_handle);
+	switch (acb->adapter_type) {
+	case ACB_ADAPTER_TYPE_B:
+	case ACB_ADAPTER_TYPE_D:
+		dma_coherent_handle = acb->dma_coherent_handle2;
+		break;
+	default:
+		dma_coherent_handle = acb->dma_coherent_handle;
+		break;
+	}
+	cdb_phyaddr = lower_32_bits(dma_coherent_handle);
+	cdb_phyaddr_hi32 = upper_32_bits(dma_coherent_handle);
 	acb->cdb_phyaddr_hi32 = cdb_phyaddr_hi32;
 	/*
 	***********************************************************************
@@ -2520,65 +3246,62 @@ static int arcmsr_iop_confirm(struct AdapterControlBlock *acb)
 	case ACB_ADAPTER_TYPE_A: {
 		if (cdb_phyaddr_hi32 != 0) {
 			struct MessageUnit_A __iomem *reg = acb->pmuA;
-			uint32_t intmask_org;
-			intmask_org = arcmsr_disable_outbound_ints(acb);
 			writel(ARCMSR_SIGNATURE_SET_CONFIG, \
 						&reg->message_rwbuffer[0]);
 			writel(cdb_phyaddr_hi32, &reg->message_rwbuffer[1]);
 			writel(ARCMSR_INBOUND_MESG0_SET_CONFIG, \
 							&reg->inbound_msgaddr0);
-			if (!arcmsr_hba_wait_msgint_ready(acb)) {
+			if (!arcmsr_hbaA_wait_msgint_ready(acb)) {
 				printk(KERN_NOTICE "arcmsr%d: ""set ccb high \
 				part physical address timeout\n",
 				acb->host->host_no);
 				return 1;
 			}
-			arcmsr_enable_outbound_ints(acb, intmask_org);
 		}
 		}
 		break;
 
 	case ACB_ADAPTER_TYPE_B: {
-		unsigned long post_queue_phyaddr;
 		uint32_t __iomem *rwbuffer;
 
 		struct MessageUnit_B *reg = acb->pmuB;
-		uint32_t intmask_org;
-		intmask_org = arcmsr_disable_outbound_ints(acb);
 		reg->postq_index = 0;
 		reg->doneq_index = 0;
 		writel(ARCMSR_MESSAGE_SET_POST_WINDOW, reg->drv2iop_doorbell);
-		if (!arcmsr_hbb_wait_msgint_ready(acb)) {
+		if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 			printk(KERN_NOTICE "arcmsr%d:can not set diver mode\n", \
 				acb->host->host_no);
 			return 1;
 		}
-		post_queue_phyaddr = acb->dma_coherent_handle_hbb_mu;
 		rwbuffer = reg->message_rwbuffer;
 		/* driver "set config" signature */
 		writel(ARCMSR_SIGNATURE_SET_CONFIG, rwbuffer++);
 		/* normal should be zero */
 		writel(cdb_phyaddr_hi32, rwbuffer++);
 		/* postQ size (256 + 8)*4	 */
-		writel(post_queue_phyaddr, rwbuffer++);
+		writel(cdb_phyaddr, rwbuffer++);
 		/* doneQ size (256 + 8)*4	 */
-		writel(post_queue_phyaddr + 1056, rwbuffer++);
+		writel(cdb_phyaddr + 1056, rwbuffer++);
 		/* ccb maxQ size must be --> [(256 + 8)*4]*/
 		writel(1056, rwbuffer);
 
 		writel(ARCMSR_MESSAGE_SET_CONFIG, reg->drv2iop_doorbell);
-		if (!arcmsr_hbb_wait_msgint_ready(acb)) {
+		if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 			printk(KERN_NOTICE "arcmsr%d: 'set command Q window' \
 			timeout \n",acb->host->host_no);
 			return 1;
 		}
-		arcmsr_hbb_enable_driver_mode(acb);
-		arcmsr_enable_outbound_ints(acb, intmask_org);
+		writel(ARCMSR_MESSAGE_START_DRIVER_MODE, reg->drv2iop_doorbell);
+		if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
+			pr_err("arcmsr%d: can't set driver mode.\n",
+				acb->host->host_no);
+			return 1;
+		}
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
 		if (cdb_phyaddr_hi32 != 0) {
-			struct MessageUnit_C *reg = (struct MessageUnit_C *)acb->pmuC;
+			struct MessageUnit_C __iomem *reg = acb->pmuC;
 
 			printk(KERN_NOTICE "arcmsr%d: cdb_phyaddr_hi32=0x%x\n",
 					acb->adapter_index, cdb_phyaddr_hi32);
@@ -2586,13 +3309,34 @@ static int arcmsr_iop_confirm(struct AdapterControlBlock *acb)
 			writel(cdb_phyaddr_hi32, &reg->msgcode_rwbuffer[1]);
 			writel(ARCMSR_INBOUND_MESG0_SET_CONFIG, &reg->inbound_msgaddr0);
 			writel(ARCMSR_HBCMU_DRV2IOP_MESSAGE_CMD_DONE, &reg->inbound_doorbell);
-			if (!arcmsr_hbc_wait_msgint_ready(acb)) {
+			if (!arcmsr_hbaC_wait_msgint_ready(acb)) {
 				printk(KERN_NOTICE "arcmsr%d: 'set command Q window' \
 				timeout \n", acb->host->host_no);
 				return 1;
 			}
 		}
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		uint32_t __iomem *rwbuffer;
+		struct MessageUnit_D *reg = acb->pmuD;
+		reg->postq_index = 0;
+		reg->doneq_index = 0;
+		rwbuffer = reg->msgcode_rwbuffer;
+		writel(ARCMSR_SIGNATURE_SET_CONFIG, rwbuffer++);
+		writel(cdb_phyaddr_hi32, rwbuffer++);
+		writel(cdb_phyaddr, rwbuffer++);
+		writel(cdb_phyaddr + (ARCMSR_MAX_ARC1214_POSTQUEUE *
+			sizeof(struct InBound_SRB)), rwbuffer++);
+		writel(0x100, rwbuffer);
+		writel(ARCMSR_INBOUND_MESG0_SET_CONFIG, reg->inbound_msgaddr0);
+		if (!arcmsr_hbaD_wait_msgint_ready(acb)) {
+			pr_notice("arcmsr%d: 'set command Q window' timeout\n",
+				acb->host->host_no);
+			return 1;
+		}
+		}
+		break;
 	}
 	return 0;
 }
@@ -2619,15 +3363,24 @@ static void arcmsr_wait_firmware_ready(struct AdapterControlBlock *acb)
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		struct MessageUnit_C *reg = (struct MessageUnit_C *)acb->pmuC;
+		struct MessageUnit_C __iomem *reg = acb->pmuC;
 		do {
 			firmware_state = readl(&reg->outbound_msgaddr1);
 		} while ((firmware_state & ARCMSR_HBCMU_MESSAGE_FIRMWARE_OK) == 0);
 		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+		do {
+			firmware_state = readl(reg->outbound_msgaddr1);
+		} while ((firmware_state &
+			ARCMSR_ARC1214_MESSAGE_FIRMWARE_OK) == 0);
+		}
+		break;
 	}
 }
 
-static void arcmsr_request_hba_device_map(struct AdapterControlBlock *acb)
+static void arcmsr_hbaA_request_device_map(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	if (unlikely(atomic_read(&acb->rq_map_token) == 0) || ((acb->acb_flags & ACB_F_BUS_RESET) != 0 ) || ((acb->acb_flags & ACB_F_ABORT) != 0 )){
@@ -2649,9 +3402,9 @@ static void arcmsr_request_hba_device_map(struct AdapterControlBlock *acb)
 	return;
 }
 
-static void arcmsr_request_hbb_device_map(struct AdapterControlBlock *acb)
+static void arcmsr_hbaB_request_device_map(struct AdapterControlBlock *acb)
 {
-	struct MessageUnit_B __iomem *reg = acb->pmuB;
+	struct MessageUnit_B *reg = acb->pmuB;
 	if (unlikely(atomic_read(&acb->rq_map_token) == 0) || ((acb->acb_flags & ACB_F_BUS_RESET) != 0 ) || ((acb->acb_flags & ACB_F_ABORT) != 0 )){
 		mod_timer(&acb->eternal_timer, jiffies + msecs_to_jiffies(6 * HZ));
 		return;
@@ -2671,7 +3424,7 @@ static void arcmsr_request_hbb_device_map(struct AdapterControlBlock *acb)
 	return;
 }
 
-static void arcmsr_request_hbc_device_map(struct AdapterControlBlock *acb)
+static void arcmsr_hbaC_request_device_map(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_C __iomem *reg = acb->pmuC;
 	if (unlikely(atomic_read(&acb->rq_map_token) == 0) || ((acb->acb_flags & ACB_F_BUS_RESET) != 0) || ((acb->acb_flags & ACB_F_ABORT) != 0)) {
@@ -2694,69 +3447,119 @@ static void arcmsr_request_hbc_device_map(struct AdapterControlBlock *acb)
 	return;
 }
 
+static void arcmsr_hbaD_request_device_map(struct AdapterControlBlock *acb)
+{
+	struct MessageUnit_D *reg = acb->pmuD;
+
+	if (unlikely(atomic_read(&acb->rq_map_token) == 0) ||
+		((acb->acb_flags & ACB_F_BUS_RESET) != 0) ||
+		((acb->acb_flags & ACB_F_ABORT) != 0)) {
+		mod_timer(&acb->eternal_timer,
+			jiffies + msecs_to_jiffies(6 * HZ));
+	} else {
+		acb->fw_flag = FW_NORMAL;
+		if (atomic_read(&acb->ante_token_value) ==
+			atomic_read(&acb->rq_map_token)) {
+			atomic_set(&acb->rq_map_token, 16);
+		}
+		atomic_set(&acb->ante_token_value,
+			atomic_read(&acb->rq_map_token));
+		if (atomic_dec_and_test(&acb->rq_map_token)) {
+			mod_timer(&acb->eternal_timer, jiffies +
+				msecs_to_jiffies(6 * HZ));
+			return;
+		}
+		writel(ARCMSR_INBOUND_MESG0_GET_CONFIG,
+			reg->inbound_msgaddr0);
+		mod_timer(&acb->eternal_timer, jiffies +
+			msecs_to_jiffies(6 * HZ));
+	}
+}
+
 static void arcmsr_request_device_map(unsigned long pacb)
 {
 	struct AdapterControlBlock *acb = (struct AdapterControlBlock *)pacb;
 	switch (acb->adapter_type) {
 		case ACB_ADAPTER_TYPE_A: {
-			arcmsr_request_hba_device_map(acb);
+			arcmsr_hbaA_request_device_map(acb);
 		}
 		break;
 		case ACB_ADAPTER_TYPE_B: {
-			arcmsr_request_hbb_device_map(acb);
+			arcmsr_hbaB_request_device_map(acb);
 		}
 		break;
 		case ACB_ADAPTER_TYPE_C: {
-			arcmsr_request_hbc_device_map(acb);
+			arcmsr_hbaC_request_device_map(acb);
 		}
+		break;
+		case ACB_ADAPTER_TYPE_D:
+			arcmsr_hbaD_request_device_map(acb);
+		break;
 	}
 }
 
-static void arcmsr_start_hba_bgrb(struct AdapterControlBlock *acb)
+static void arcmsr_hbaA_start_bgrb(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_A __iomem *reg = acb->pmuA;
 	acb->acb_flags |= ACB_F_MSG_START_BGRB;
 	writel(ARCMSR_INBOUND_MESG0_START_BGRB, &reg->inbound_msgaddr0);
-	if (!arcmsr_hba_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaA_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE "arcmsr%d: wait 'start adapter background \
 				rebulid' timeout \n", acb->host->host_no);
 	}
 }
 
-static void arcmsr_start_hbb_bgrb(struct AdapterControlBlock *acb)
+static void arcmsr_hbaB_start_bgrb(struct AdapterControlBlock *acb)
 {
 	struct MessageUnit_B *reg = acb->pmuB;
 	acb->acb_flags |= ACB_F_MSG_START_BGRB;
 	writel(ARCMSR_MESSAGE_START_BGRB, reg->drv2iop_doorbell);
-	if (!arcmsr_hbb_wait_msgint_ready(acb)) {
+	if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 		printk(KERN_NOTICE "arcmsr%d: wait 'start adapter background \
 				rebulid' timeout \n",acb->host->host_no);
 	}
 }
 
-static void arcmsr_start_hbc_bgrb(struct AdapterControlBlock *pACB)
+static void arcmsr_hbaC_start_bgrb(struct AdapterControlBlock *pACB)
 {
-	struct MessageUnit_C *phbcmu = (struct MessageUnit_C *)pACB->pmuC;
+	struct MessageUnit_C __iomem *phbcmu = pACB->pmuC;
 	pACB->acb_flags |= ACB_F_MSG_START_BGRB;
 	writel(ARCMSR_INBOUND_MESG0_START_BGRB, &phbcmu->inbound_msgaddr0);
 	writel(ARCMSR_HBCMU_DRV2IOP_MESSAGE_CMD_DONE, &phbcmu->inbound_doorbell);
-	if (!arcmsr_hbc_wait_msgint_ready(pACB)) {
+	if (!arcmsr_hbaC_wait_msgint_ready(pACB)) {
 		printk(KERN_NOTICE "arcmsr%d: wait 'start adapter background \
 				rebulid' timeout \n", pACB->host->host_no);
 	}
 	return;
 }
+
+static void arcmsr_hbaD_start_bgrb(struct AdapterControlBlock *pACB)
+{
+	struct MessageUnit_D *pmu = pACB->pmuD;
+
+	pACB->acb_flags |= ACB_F_MSG_START_BGRB;
+	writel(ARCMSR_INBOUND_MESG0_START_BGRB, pmu->inbound_msgaddr0);
+	if (!arcmsr_hbaD_wait_msgint_ready(pACB)) {
+		pr_notice("arcmsr%d: wait 'start adapter "
+			"background rebulid' timeout\n", pACB->host->host_no);
+	}
+}
+
 static void arcmsr_start_adapter_bgrb(struct AdapterControlBlock *acb)
 {
 	switch (acb->adapter_type) {
 	case ACB_ADAPTER_TYPE_A:
-		arcmsr_start_hba_bgrb(acb);
+		arcmsr_hbaA_start_bgrb(acb);
 		break;
 	case ACB_ADAPTER_TYPE_B:
-		arcmsr_start_hbb_bgrb(acb);
+		arcmsr_hbaB_start_bgrb(acb);
 		break;
 	case ACB_ADAPTER_TYPE_C:
-		arcmsr_start_hbc_bgrb(acb);
+		arcmsr_hbaC_start_bgrb(acb);
+		break;
+	case ACB_ADAPTER_TYPE_D:
+		arcmsr_hbaD_start_bgrb(acb);
+		break;
 	}
 }
 
@@ -2783,13 +3586,48 @@ static void arcmsr_clear_doorbell_queue_buffer(struct AdapterControlBlock *acb)
 		}
 		break;
 	case ACB_ADAPTER_TYPE_C: {
-		struct MessageUnit_C *reg = (struct MessageUnit_C *)acb->pmuC;
-		uint32_t outbound_doorbell;
+		struct MessageUnit_C __iomem *reg = acb->pmuC;
+		uint32_t outbound_doorbell, i;
 		/* empty doorbell Qbuffer if door bell ringed */
 		outbound_doorbell = readl(&reg->outbound_doorbell);
 		writel(outbound_doorbell, &reg->outbound_doorbell_clear);
 		writel(ARCMSR_HBCMU_DRV2IOP_DATA_READ_OK, &reg->inbound_doorbell);
+		for (i = 0; i < 200; i++) {
+			msleep(20);
+			outbound_doorbell = readl(&reg->outbound_doorbell);
+			if (outbound_doorbell &
+				ARCMSR_HBCMU_IOP2DRV_DATA_WRITE_OK) {
+				writel(outbound_doorbell,
+					&reg->outbound_doorbell_clear);
+				writel(ARCMSR_HBCMU_DRV2IOP_DATA_READ_OK,
+					&reg->inbound_doorbell);
+			} else
+				break;
+		}
+		}
+		break;
+	case ACB_ADAPTER_TYPE_D: {
+		struct MessageUnit_D *reg = acb->pmuD;
+		uint32_t outbound_doorbell, i;
+		/* empty doorbell Qbuffer if door bell ringed */
+		outbound_doorbell = readl(reg->outbound_doorbell);
+		writel(outbound_doorbell, reg->outbound_doorbell);
+		writel(ARCMSR_ARC1214_DRV2IOP_DATA_OUT_READ,
+			reg->inbound_doorbell);
+		for (i = 0; i < 200; i++) {
+			msleep(20);
+			outbound_doorbell = readl(reg->outbound_doorbell);
+			if (outbound_doorbell &
+				ARCMSR_ARC1214_IOP2DRV_DATA_WRITE_OK) {
+				writel(outbound_doorbell,
+					reg->outbound_doorbell);
+				writel(ARCMSR_ARC1214_DRV2IOP_DATA_OUT_READ,
+					reg->inbound_doorbell);
+			} else
+				break;
 		}
+		}
+		break;
 	}
 }
 
@@ -2802,7 +3640,7 @@ static void arcmsr_enable_eoi_mode(struct AdapterControlBlock *acb)
 		{
 			struct MessageUnit_B *reg = acb->pmuB;
 			writel(ARCMSR_MESSAGE_ACTIVE_EOI_MODE, reg->drv2iop_doorbell);
-			if (!arcmsr_hbb_wait_msgint_ready(acb)) {
+			if (!arcmsr_hbaB_wait_msgint_ready(acb)) {
 				printk(KERN_NOTICE "ARCMSR IOP enables EOI_MODE TIMEOUT");
 				return;
 			}
@@ -2820,6 +3658,7 @@ static void arcmsr_hardware_reset(struct AdapterControlBlock *acb)
 	int i, count = 0;
 	struct MessageUnit_A __iomem *pmuA = acb->pmuA;
 	struct MessageUnit_C __iomem *pmuC = acb->pmuC;
+	struct MessageUnit_D *pmuD = acb->pmuD;
 
 	/* backup pci config data */
 	printk(KERN_NOTICE "arcmsr%d: executing hw bus reset .....\n", acb->host->host_no);
@@ -2840,6 +3679,8 @@ static void arcmsr_hardware_reset(struct AdapterControlBlock *acb)
 			writel(0xD, &pmuC->write_sequence);
 		} while (((readl(&pmuC->host_diagnostic) & ARCMSR_ARC1880_DiagWrite_ENABLE) == 0) && (count < 5));
 		writel(ARCMSR_ARC1880_RESET_ADAPTER, &pmuC->host_diagnostic);
+	} else if ((acb->dev_id == 0x1214)) {
+		writel(0x20, pmuD->reset_request);
 	} else {
 		pci_write_config_byte(acb->pdev, 0x84, 0x20);
 	}
@@ -3016,9 +3857,7 @@ sleep:
 				arcmsr_get_firmware_spec(acb);
 				arcmsr_start_adapter_bgrb(acb);
 				/* clear Qbuffer if door bell ringed */
-				outbound_doorbell = readl(&reg->outbound_doorbell);
-				writel(outbound_doorbell, &reg->outbound_doorbell_clear); /*clear interrupt */
-				writel(ARCMSR_HBCMU_DRV2IOP_DATA_READ_OK, &reg->inbound_doorbell);
+				arcmsr_clear_doorbell_queue_buffer(acb);
 				/* enable outbound Post Queue,outbound doorbell Interrupt */
 				arcmsr_enable_outbound_ints(acb, intmask_org);
 				atomic_set(&acb->rq_map_token, 16);
@@ -3038,6 +3877,66 @@ sleep:
 			}
 			break;
 		}
+		case ACB_ADAPTER_TYPE_D: {
+			if (acb->acb_flags & ACB_F_BUS_RESET) {
+				long timeout;
+				pr_notice("arcmsr: there is an bus reset"
+					" eh proceeding.......\n");
+				timeout = wait_event_timeout(wait_q, (acb->acb_flags
+					& ACB_F_BUS_RESET) == 0, 220 * HZ);
+				if (timeout)
+					return SUCCESS;
+			}
+			acb->acb_flags |= ACB_F_BUS_RESET;
+			if (!arcmsr_iop_reset(acb)) {
+				struct MessageUnit_D *reg;
+				reg = acb->pmuD;
+				arcmsr_hardware_reset(acb);
+				acb->acb_flags &= ~ACB_F_IOP_INITED;
+			nap:
+				ssleep(ARCMSR_SLEEPTIME);
+				if ((readl(reg->sample_at_reset) & 0x80) != 0) {
+					pr_err("arcmsr%d: waiting for "
+						"hw bus reset return, retry=%d\n",
+						acb->host->host_no, retry_count);
+					if (retry_count > ARCMSR_RETRYCOUNT) {
+						acb->fw_flag = FW_DEADLOCK;
+						pr_err("arcmsr%d: waiting for hw bus"
+							" reset return, "
+							"RETRY TERMINATED!!\n",
+							acb->host->host_no);
+						return FAILED;
+					}
+					retry_count++;
+					goto nap;
+				}
+				acb->acb_flags |= ACB_F_IOP_INITED;
+				/* disable all outbound interrupt */
+				intmask_org = arcmsr_disable_outbound_ints(acb);
+				arcmsr_get_firmware_spec(acb);
+				arcmsr_start_adapter_bgrb(acb);
+				arcmsr_clear_doorbell_queue_buffer(acb);
+				arcmsr_enable_outbound_ints(acb, intmask_org);
+				atomic_set(&acb->rq_map_token, 16);
+				atomic_set(&acb->ante_token_value, 16);
+				acb->fw_flag = FW_NORMAL;
+				mod_timer(&acb->eternal_timer,
+					jiffies + msecs_to_jiffies(6 * HZ));
+				acb->acb_flags &= ~ACB_F_BUS_RESET;
+				rtn = SUCCESS;
+				pr_err("arcmsr: scsi bus reset "
+					"eh returns with success\n");
+			} else {
+				acb->acb_flags &= ~ACB_F_BUS_RESET;
+				atomic_set(&acb->rq_map_token, 16);
+				atomic_set(&acb->ante_token_value, 16);
+				acb->fw_flag = FW_NORMAL;
+				mod_timer(&acb->eternal_timer,
+					jiffies + msecs_to_jiffies(6 * HZ));
+				rtn = SUCCESS;
+			}
+			break;
+		}
 	}
 	return rtn;
 }
@@ -3056,8 +3955,10 @@ static int arcmsr_abort(struct scsi_cmnd *cmd)
 		(struct AdapterControlBlock *)cmd->device->host->hostdata;
 	int i = 0;
 	int rtn = FAILED;
+	uint32_t intmask_org;
+
 	printk(KERN_NOTICE
-		"arcmsr%d: abort device command of scsi id = %d lun = %d \n",
+		"arcmsr%d: abort device command of scsi id = %d lun = %d\n",
 		acb->host->host_no, cmd->device->id, (u32)cmd->device->lun);
 	acb->acb_flags |= ACB_F_ABORT;
 	acb->num_aborts++;
@@ -3067,9 +3968,12 @@ static int arcmsr_abort(struct scsi_cmnd *cmd)
 	** we need to handle it as soon as possible and exit
 	************************************************
 	*/
-	if (!atomic_read(&acb->ccboutstandingcount))
+	if (!atomic_read(&acb->ccboutstandingcount)) {
+		acb->acb_flags &= ~ACB_F_ABORT;
 		return rtn;
+	}
 
+	intmask_org = arcmsr_disable_outbound_ints(acb);
 	for (i = 0; i < ARCMSR_MAX_FREECCB_NUM; i++) {
 		struct CommandControlBlock *ccb = acb->pccb_pool[i];
 		if (ccb->startdone == ARCMSR_CCB_START && ccb->pcmd == cmd) {
@@ -3079,6 +3983,7 @@ static int arcmsr_abort(struct scsi_cmnd *cmd)
 		}
 	}
 	acb->acb_flags &= ~ACB_F_ABORT;
+	arcmsr_enable_outbound_ints(acb, intmask_org);
 	return rtn;
 }
 
@@ -3108,19 +4013,20 @@ static const char *arcmsr_info(struct Scsi_Host *host)
 	case PCI_DEVICE_ID_ARECA_1280:
 		type = "SATA";
 		break;
+	case PCI_DEVICE_ID_ARECA_1214:
 	case PCI_DEVICE_ID_ARECA_1380:
 	case PCI_DEVICE_ID_ARECA_1381:
 	case PCI_DEVICE_ID_ARECA_1680:
 	case PCI_DEVICE_ID_ARECA_1681:
 	case PCI_DEVICE_ID_ARECA_1880:
-		type = "SAS";
+		type = "SAS/SATA";
 		break;
 	default:
-		type = "X-TYPE";
+		type = "unknown";
+		raid6 =	0;
 		break;
 	}
-	sprintf(buf, "Areca %s Host Adapter RAID Controller%s\n %s",
-			type, raid6 ? "( RAID6 capable)" : "",
-			ARCMSR_DRIVER_VERSION);
+	sprintf(buf, "Areca %s RAID Controller %s\narcmsr version %s\n",
+		type, raid6 ? "(RAID6 capable)" : "", ARCMSR_DRIVER_VERSION);
 	return buf;
 }
diff --git a/drivers/scsi/be2iscsi/be.h b/drivers/scsi/be2iscsi/be.h
index 860f527d8f26..81e83a65a193 100644
--- a/drivers/scsi/be2iscsi/be.h
+++ b/drivers/scsi/be2iscsi/be.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/be2iscsi/be_cmds.c b/drivers/scsi/be2iscsi/be_cmds.c
index 1432ed5e9fc6..80d97f3d2ed9 100644
--- a/drivers/scsi/be2iscsi/be_cmds.c
+++ b/drivers/scsi/be2iscsi/be_cmds.c
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -275,6 +275,19 @@ bool is_link_state_evt(u32 trailer)
 		  ASYNC_EVENT_CODE_LINK_STATE);
 }
 
+static bool is_iscsi_evt(u32 trailer)
+{
+	return ((trailer >> ASYNC_TRAILER_EVENT_CODE_SHIFT) &
+		  ASYNC_TRAILER_EVENT_CODE_MASK) ==
+		  ASYNC_EVENT_CODE_ISCSI;
+}
+
+static int iscsi_evt_type(u32 trailer)
+{
+	return (trailer >> ASYNC_TRAILER_EVENT_TYPE_SHIFT) &
+		 ASYNC_TRAILER_EVENT_TYPE_MASK;
+}
+
 static inline bool be_mcc_compl_is_new(struct be_mcc_compl *compl)
 {
 	if (compl->flags != 0) {
@@ -438,7 +451,7 @@ void beiscsi_async_link_state_process(struct beiscsi_hba *phba,
 	} else if ((evt->port_link_status & ASYNC_EVENT_LINK_UP) ||
 		    ((evt->port_link_status & ASYNC_EVENT_LOGICAL) &&
 		     (evt->port_fault == BEISCSI_PHY_LINK_FAULT_NONE))) {
-		phba->state = BE_ADAPTER_LINK_UP;
+		phba->state = BE_ADAPTER_LINK_UP | BE_ADAPTER_CHECK_BOOT;
 
 		beiscsi_log(phba, KERN_ERR,
 			    BEISCSI_LOG_CONFIG | BEISCSI_LOG_INIT,
@@ -461,7 +474,28 @@ int beiscsi_process_mcc(struct beiscsi_hba *phba)
 				/* Interpret compl as a async link evt */
 				beiscsi_async_link_state_process(phba,
 				   (struct be_async_event_link_state *) compl);
-			else
+			else if (is_iscsi_evt(compl->flags)) {
+				switch (iscsi_evt_type(compl->flags)) {
+				case ASYNC_EVENT_NEW_ISCSI_TGT_DISC:
+				case ASYNC_EVENT_NEW_ISCSI_CONN:
+				case ASYNC_EVENT_NEW_TCP_CONN:
+					phba->state |= BE_ADAPTER_CHECK_BOOT;
+					beiscsi_log(phba, KERN_ERR,
+						    BEISCSI_LOG_CONFIG |
+						    BEISCSI_LOG_MBOX,
+						    "BC_%d : Async iscsi Event,"
+						    " flags handled = 0x%08x\n",
+						    compl->flags);
+					break;
+				default:
+					beiscsi_log(phba, KERN_ERR,
+						    BEISCSI_LOG_CONFIG |
+						    BEISCSI_LOG_MBOX,
+						    "BC_%d : Unsupported Async"
+						    " Event, flags = 0x%08x\n",
+						    compl->flags);
+				}
+			} else
 				beiscsi_log(phba, KERN_ERR,
 					    BEISCSI_LOG_CONFIG |
 					    BEISCSI_LOG_MBOX,
diff --git a/drivers/scsi/be2iscsi/be_cmds.h b/drivers/scsi/be2iscsi/be_cmds.h
index cc7405c0eca0..98897434bcb4 100644
--- a/drivers/scsi/be2iscsi/be_cmds.h
+++ b/drivers/scsi/be2iscsi/be_cmds.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -26,9 +26,9 @@
  * The commands are serviced by the ARM processor in the OneConnect's MPU.
  */
 struct be_sge {
-	u32 pa_lo;
-	u32 pa_hi;
-	u32 len;
+	__le32 pa_lo;
+	__le32 pa_hi;
+	__le32 len;
 };
 
 #define MCC_WRB_SGE_CNT_SHIFT 3	/* bits 3 - 7 of dword 0 */
@@ -118,6 +118,14 @@ struct be_mcc_compl {
 #define ASYNC_TRAILER_EVENT_CODE_SHIFT	8	/* bits 8 - 15 */
 #define ASYNC_TRAILER_EVENT_CODE_MASK	0xFF
 #define ASYNC_EVENT_CODE_LINK_STATE	0x1
+#define ASYNC_EVENT_CODE_ISCSI		0x4
+
+#define ASYNC_TRAILER_EVENT_TYPE_SHIFT	16	/* bits 16 - 23 */
+#define ASYNC_TRAILER_EVENT_TYPE_MASK	0xF
+#define ASYNC_EVENT_NEW_ISCSI_TGT_DISC	0x4
+#define ASYNC_EVENT_NEW_ISCSI_CONN	0x5
+#define ASYNC_EVENT_NEW_TCP_CONN	0x7
+
 struct be_async_event_trailer {
 	u32 code;
 };
@@ -624,11 +632,11 @@ static inline struct be_sge *nonembedded_sgl(struct be_mcc_wrb *wrb)
 /******************** Modify EQ Delay *******************/
 struct be_cmd_req_modify_eq_delay {
 	struct be_cmd_req_hdr hdr;
-	u32 num_eq;
+	__le32 num_eq;
 	struct {
-		u32 eq_id;
-		u32 phase;
-		u32 delay_multiplier;
+		__le32 eq_id;
+		__le32 phase;
+		__le32 delay_multiplier;
 	} delay[MAX_CPUS];
 } __packed;
 
diff --git a/drivers/scsi/be2iscsi/be_iscsi.c b/drivers/scsi/be2iscsi/be_iscsi.c
index 86162811812d..b7391a3f9f0b 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.c
+++ b/drivers/scsi/be2iscsi/be_iscsi.c
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -1274,6 +1274,31 @@ int beiscsi_ep_poll(struct iscsi_endpoint *ep, int timeout_ms)
 }
 
 /**
+ * beiscsi_flush_cq()- Flush the CQ created.
+ * @phba: ptr device priv structure.
+ *
+ * Before the connection resource are freed flush
+ * all the CQ enteries
+ **/
+static void beiscsi_flush_cq(struct beiscsi_hba *phba)
+{
+	uint16_t i;
+	struct be_eq_obj *pbe_eq;
+	struct hwi_controller *phwi_ctrlr;
+	struct hwi_context_memory *phwi_context;
+
+	phwi_ctrlr = phba->phwi_ctrlr;
+	phwi_context = phwi_ctrlr->phwi_ctxt;
+
+	for (i = 0; i < phba->num_cpus; i++) {
+		pbe_eq = &phwi_context->be_eq[i];
+		blk_iopoll_disable(&pbe_eq->iopoll);
+		beiscsi_process_cq(pbe_eq);
+		blk_iopoll_enable(&pbe_eq->iopoll);
+	}
+}
+
+/**
  * beiscsi_close_conn - Upload the  connection
  * @ep: The iscsi endpoint
  * @flag: The type of connection closure
@@ -1294,6 +1319,10 @@ static int beiscsi_close_conn(struct  beiscsi_endpoint *beiscsi_ep, int flag)
 	}
 
 	ret = beiscsi_mccq_compl(phba, tag, NULL, NULL);
+
+	/* Flush the CQ entries */
+	beiscsi_flush_cq(phba);
+
 	return ret;
 }
 
diff --git a/drivers/scsi/be2iscsi/be_iscsi.h b/drivers/scsi/be2iscsi/be_iscsi.h
index 31ddc8494398..e0b3b2d1f27a 100644
--- a/drivers/scsi/be2iscsi/be_iscsi.h
+++ b/drivers/scsi/be2iscsi/be_iscsi.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/be2iscsi/be_main.c b/drivers/scsi/be2iscsi/be_main.c
index 915c26b23ab6..30d74a06b993 100644
--- a/drivers/scsi/be2iscsi/be_main.c
+++ b/drivers/scsi/be2iscsi/be_main.c
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -2068,7 +2068,7 @@ static void  beiscsi_process_mcc_isr(struct beiscsi_hba *phba)
  * return
  *     Number of Completion Entries processed.
  **/
-static unsigned int beiscsi_process_cq(struct be_eq_obj *pbe_eq)
+unsigned int beiscsi_process_cq(struct be_eq_obj *pbe_eq)
 {
 	struct be_queue_info *cq;
 	struct sol_cqe *sol;
@@ -2110,6 +2110,18 @@ static unsigned int beiscsi_process_cq(struct be_eq_obj *pbe_eq)
 
 		cri_index = BE_GET_CRI_FROM_CID(cid);
 		ep = phba->ep_array[cri_index];
+
+		if (ep == NULL) {
+			/* connection has already been freed
+			 * just move on to next one
+			 */
+			beiscsi_log(phba, KERN_WARNING,
+				    BEISCSI_LOG_INIT,
+				    "BM_%d : proc cqe of disconn ep: cid %d\n",
+				    cid);
+			goto proc_next_cqe;
+		}
+
 		beiscsi_ep = ep->dd_data;
 		beiscsi_conn = beiscsi_ep->conn;
 
@@ -2219,6 +2231,7 @@ static unsigned int beiscsi_process_cq(struct be_eq_obj *pbe_eq)
 			break;
 		}
 
+proc_next_cqe:
 		AMAP_SET_BITS(struct amap_sol_cqe, valid, sol, 0);
 		queue_tail_inc(cq);
 		sol = queue_tail_node(cq);
@@ -4377,6 +4390,10 @@ static int beiscsi_setup_boot_info(struct beiscsi_hba *phba)
 {
 	struct iscsi_boot_kobj *boot_kobj;
 
+	/* it has been created previously */
+	if (phba->boot_kset)
+		return 0;
+
 	/* get boot info using mgmt cmd */
 	if (beiscsi_get_boot_info(phba))
 		/* Try to see if we can carry on without this */
@@ -5206,6 +5223,7 @@ static void beiscsi_quiesce(struct beiscsi_hba *phba,
 			free_irq(phba->pcidev->irq, phba);
 		}
 	pci_disable_msix(phba->pcidev);
+	cancel_delayed_work_sync(&phba->beiscsi_hw_check_task);
 
 	for (i = 0; i < phba->num_cpus; i++) {
 		pbe_eq = &phwi_context->be_eq[i];
@@ -5227,7 +5245,6 @@ static void beiscsi_quiesce(struct beiscsi_hba *phba,
 		hwi_cleanup(phba);
 	}
 
-	cancel_delayed_work_sync(&phba->beiscsi_hw_check_task);
 }
 
 static void beiscsi_remove(struct pci_dev *pcidev)
@@ -5276,9 +5293,9 @@ static void beiscsi_msix_enable(struct beiscsi_hba *phba)
 	for (i = 0; i <= phba->num_cpus; i++)
 		phba->msix_entries[i].entry = i;
 
-	status = pci_enable_msix(phba->pcidev, phba->msix_entries,
-				 (phba->num_cpus + 1));
-	if (!status)
+	status = pci_enable_msix_range(phba->pcidev, phba->msix_entries,
+				       phba->num_cpus + 1, phba->num_cpus + 1);
+	if (status > 0)
 		phba->msix_enabled = true;
 
 	return;
@@ -5335,6 +5352,14 @@ static void be_eqd_update(struct beiscsi_hba *phba)
 	}
 }
 
+static void be_check_boot_session(struct beiscsi_hba *phba)
+{
+	if (beiscsi_setup_boot_info(phba))
+		beiscsi_log(phba, KERN_ERR, BEISCSI_LOG_INIT,
+			    "BM_%d : Could not set up "
+			    "iSCSI boot info on async event.\n");
+}
+
 /*
  * beiscsi_hw_health_check()- Check adapter health
  * @work: work item to check HW health
@@ -5350,6 +5375,11 @@ beiscsi_hw_health_check(struct work_struct *work)
 
 	be_eqd_update(phba);
 
+	if (phba->state & BE_ADAPTER_CHECK_BOOT) {
+		phba->state &= ~BE_ADAPTER_CHECK_BOOT;
+		be_check_boot_session(phba);
+	}
+
 	beiscsi_ue_detect(phba);
 
 	schedule_delayed_work(&phba->beiscsi_hw_check_task,
diff --git a/drivers/scsi/be2iscsi/be_main.h b/drivers/scsi/be2iscsi/be_main.h
index 9ceab426eec9..7ee0ffc38514 100644
--- a/drivers/scsi/be2iscsi/be_main.h
+++ b/drivers/scsi/be2iscsi/be_main.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -36,7 +36,7 @@
 #include <scsi/scsi_transport_iscsi.h>
 
 #define DRV_NAME		"be2iscsi"
-#define BUILD_STR		"10.2.273.0"
+#define BUILD_STR		"10.4.114.0"
 #define BE_NAME			"Emulex OneConnect" \
 				"Open-iSCSI Driver version" BUILD_STR
 #define DRV_DESC		BE_NAME " " "Driver"
@@ -104,6 +104,7 @@
 #define BE_ADAPTER_LINK_DOWN	0x002
 #define BE_ADAPTER_PCI_ERR	0x004
 #define BE_ADAPTER_STATE_SHUTDOWN	0x008
+#define BE_ADAPTER_CHECK_BOOT	0x010
 
 
 #define BEISCSI_CLEAN_UNLOAD	0x01
@@ -839,6 +840,9 @@ void beiscsi_free_mgmt_task_handles(struct beiscsi_conn *beiscsi_conn,
 void hwi_ring_cq_db(struct beiscsi_hba *phba,
 		     unsigned int id, unsigned int num_processed,
 		     unsigned char rearm, unsigned char event);
+
+unsigned int beiscsi_process_cq(struct be_eq_obj *pbe_eq);
+
 static inline bool beiscsi_error(struct beiscsi_hba *phba)
 {
 	return phba->ue_detected || phba->fw_timeout;
diff --git a/drivers/scsi/be2iscsi/be_mgmt.c b/drivers/scsi/be2iscsi/be_mgmt.c
index 665afcb74a56..681d4e8f003a 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.c
+++ b/drivers/scsi/be2iscsi/be_mgmt.c
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
@@ -943,17 +943,20 @@ mgmt_static_ip_modify(struct beiscsi_hba *phba,
 
 	if (ip_action == IP_ACTION_ADD) {
 		memcpy(req->ip_params.ip_record.ip_addr.addr, ip_param->value,
-		       ip_param->len);
+		       sizeof(req->ip_params.ip_record.ip_addr.addr));
 
 		if (subnet_param)
 			memcpy(req->ip_params.ip_record.ip_addr.subnet_mask,
-			       subnet_param->value, subnet_param->len);
+			       subnet_param->value,
+			       sizeof(req->ip_params.ip_record.ip_addr.subnet_mask));
 	} else {
 		memcpy(req->ip_params.ip_record.ip_addr.addr,
-		       if_info->ip_addr.addr, ip_param->len);
+		       if_info->ip_addr.addr,
+		       sizeof(req->ip_params.ip_record.ip_addr.addr));
 
 		memcpy(req->ip_params.ip_record.ip_addr.subnet_mask,
-		       if_info->ip_addr.subnet_mask, ip_param->len);
+		       if_info->ip_addr.subnet_mask,
+		       sizeof(req->ip_params.ip_record.ip_addr.subnet_mask));
 	}
 
 	rc = mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, NULL, 0);
@@ -981,7 +984,7 @@ static int mgmt_modify_gateway(struct beiscsi_hba *phba, uint8_t *gt_addr,
 	req->action = gtway_action;
 	req->ip_addr.ip_type = BE2_IPV4;
 
-	memcpy(req->ip_addr.addr, gt_addr, param_len);
+	memcpy(req->ip_addr.addr, gt_addr, sizeof(req->ip_addr.addr));
 
 	return mgmt_exec_nonemb_cmd(phba, &nonemb_cmd, NULL, 0);
 }
diff --git a/drivers/scsi/be2iscsi/be_mgmt.h b/drivers/scsi/be2iscsi/be_mgmt.h
index 24a8fc577477..bd81446936fc 100644
--- a/drivers/scsi/be2iscsi/be_mgmt.h
+++ b/drivers/scsi/be2iscsi/be_mgmt.h
@@ -1,5 +1,5 @@
 /**
- * Copyright (C) 2005 - 2013 Emulex
+ * Copyright (C) 2005 - 2014 Emulex
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/bnx2fc/Kconfig b/drivers/scsi/bnx2fc/Kconfig
index f245d543d7b1..097882882649 100644
--- a/drivers/scsi/bnx2fc/Kconfig
+++ b/drivers/scsi/bnx2fc/Kconfig
@@ -1,11 +1,12 @@
 config SCSI_BNX2X_FCOE
 	tristate "QLogic NetXtreme II FCoE support"
 	depends on PCI
+	depends on (IPV6 || IPV6=n)
+	depends on LIBFC
+	depends on LIBFCOE
 	select NETDEVICES
 	select ETHERNET
 	select NET_VENDOR_BROADCOM
-	select LIBFC
-	select LIBFCOE
 	select CNIC
 	---help---
 	This driver supports FCoE offload for the QLogic NetXtreme II
diff --git a/drivers/scsi/bnx2fc/bnx2fc_io.c b/drivers/scsi/bnx2fc/bnx2fc_io.c
index 4c5891e66038..0679782d9d15 100644
--- a/drivers/scsi/bnx2fc/bnx2fc_io.c
+++ b/drivers/scsi/bnx2fc/bnx2fc_io.c
@@ -1654,6 +1654,10 @@ static int bnx2fc_map_sg(struct bnx2fc_cmd *io_req)
 	u64 addr;
 	int i;
 
+	/*
+	 * Use dma_map_sg directly to ensure we're using the correct
+	 * dev struct off of pcidev.
+	 */
 	sg_count = dma_map_sg(&hba->pcidev->dev, scsi_sglist(sc),
 			      scsi_sg_count(sc), sc->sc_data_direction);
 	scsi_for_each_sg(sc, sg, sg_count, i) {
@@ -1703,9 +1707,16 @@ static int bnx2fc_build_bd_list_from_sg(struct bnx2fc_cmd *io_req)
 static void bnx2fc_unmap_sg_list(struct bnx2fc_cmd *io_req)
 {
 	struct scsi_cmnd *sc = io_req->sc_cmd;
+	struct bnx2fc_interface *interface = io_req->port->priv;
+	struct bnx2fc_hba *hba = interface->hba;
 
-	if (io_req->bd_tbl->bd_valid && sc) {
-		scsi_dma_unmap(sc);
+	/*
+	 * Use dma_unmap_sg directly to ensure we're using the correct
+	 * dev struct off of pcidev.
+	 */
+	if (io_req->bd_tbl->bd_valid && sc && scsi_sg_count(sc)) {
+		dma_unmap_sg(&hba->pcidev->dev, scsi_sglist(sc),
+		    scsi_sg_count(sc), sc->sc_data_direction);
 		io_req->bd_tbl->bd_valid = 0;
 	}
 }
diff --git a/drivers/scsi/bnx2i/Kconfig b/drivers/scsi/bnx2i/Kconfig
index 44ce54e536e5..ba30ff86d581 100644
--- a/drivers/scsi/bnx2i/Kconfig
+++ b/drivers/scsi/bnx2i/Kconfig
@@ -2,6 +2,7 @@ config SCSI_BNX2_ISCSI
 	tristate "QLogic NetXtreme II iSCSI support"
 	depends on NET
 	depends on PCI
+	depends on (IPV6 || IPV6=n)
 	select SCSI_ISCSI_ATTRS
 	select NETDEVICES
 	select ETHERNET
diff --git a/drivers/scsi/bnx2i/bnx2i_iscsi.c b/drivers/scsi/bnx2i/bnx2i_iscsi.c
index 40e22497d249..7a36388822aa 100644
--- a/drivers/scsi/bnx2i/bnx2i_iscsi.c
+++ b/drivers/scsi/bnx2i/bnx2i_iscsi.c
@@ -2235,6 +2235,9 @@ static umode_t bnx2i_attr_is_visible(int param_type, int param)
 		case ISCSI_PARAM_TGT_RESET_TMO:
 		case ISCSI_PARAM_IFACE_NAME:
 		case ISCSI_PARAM_INITIATOR_NAME:
+		case ISCSI_PARAM_BOOT_ROOT:
+		case ISCSI_PARAM_BOOT_NIC:
+		case ISCSI_PARAM_BOOT_TARGET:
 			return S_IRUGO;
 		default:
 			return 0;
diff --git a/drivers/scsi/csiostor/Kconfig b/drivers/scsi/csiostor/Kconfig
index 4d03b032aa10..7c7e5085968b 100644
--- a/drivers/scsi/csiostor/Kconfig
+++ b/drivers/scsi/csiostor/Kconfig
@@ -1,7 +1,7 @@
 config SCSI_CHELSIO_FCOE
 	tristate "Chelsio Communications FCoE support"
 	depends on PCI && SCSI
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	select FW_LOADER
 	help
 	  This driver supports FCoE Offload functionality over
diff --git a/drivers/scsi/csiostor/csio_hw.h b/drivers/scsi/csiostor/csio_hw.h
index 49b1daa4476e..5db2d85195b1 100644
--- a/drivers/scsi/csiostor/csio_hw.h
+++ b/drivers/scsi/csiostor/csio_hw.h
@@ -94,7 +94,7 @@ enum {
 };
 
 struct csio_msix_entries {
-	unsigned short	vector;		/* Vector assigned by pci_enable_msix */
+	unsigned short	vector;		/* Assigned MSI-X vector */
 	void		*dev_id;	/* Priv object associated w/ this msix*/
 	char		desc[24];	/* Description of this vector */
 };
diff --git a/drivers/scsi/csiostor/csio_isr.c b/drivers/scsi/csiostor/csio_isr.c
index 7ee9777ae2c5..a8c748a35f9c 100644
--- a/drivers/scsi/csiostor/csio_isr.c
+++ b/drivers/scsi/csiostor/csio_isr.c
@@ -499,7 +499,7 @@ csio_reduce_sqsets(struct csio_hw *hw, int cnt)
 static int
 csio_enable_msix(struct csio_hw *hw)
 {
-	int rv, i, j, k, n, min, cnt;
+	int i, j, k, n, min, cnt;
 	struct csio_msix_entries *entryp;
 	struct msix_entry *entries;
 	int extra = CSIO_EXTRA_VECS;
@@ -521,21 +521,15 @@ csio_enable_msix(struct csio_hw *hw)
 
 	csio_dbg(hw, "FW supp #niq:%d, trying %d msix's\n", hw->cfg_niq, cnt);
 
-	while ((rv = pci_enable_msix(hw->pdev, entries, cnt)) >= min)
-		cnt = rv;
-	if (!rv) {
-		if (cnt < (hw->num_sqsets + extra)) {
-			csio_dbg(hw, "Reducing sqsets to %d\n", cnt - extra);
-			csio_reduce_sqsets(hw, cnt - extra);
-		}
-	} else {
-		if (rv > 0) {
-			pci_disable_msix(hw->pdev);
-			csio_info(hw, "Not using MSI-X, remainder:%d\n", rv);
-		}
-
+	cnt = pci_enable_msix_range(hw->pdev, entries, min, cnt);
+	if (cnt < 0) {
 		kfree(entries);
-		return -ENOMEM;
+		return cnt;
+	}
+
+	if (cnt < (hw->num_sqsets + extra)) {
+		csio_dbg(hw, "Reducing sqsets to %d\n", cnt - extra);
+		csio_reduce_sqsets(hw, cnt - extra);
 	}
 
 	/* Save off vectors */
diff --git a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
index 79788a12712d..3e0a0d315f72 100644
--- a/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
+++ b/drivers/scsi/cxgbi/cxgb4i/cxgb4i.c
@@ -259,6 +259,7 @@ static void send_act_open_req(struct cxgbi_sock *csk, struct sk_buff *skb,
 	cxgb4_l2t_send(csk->cdev->ports[csk->port_id], skb, csk->l2t);
 }
 
+#if IS_ENABLED(CONFIG_IPV6)
 static void send_act_open_req6(struct cxgbi_sock *csk, struct sk_buff *skb,
 			       struct l2t_entry *e)
 {
@@ -344,6 +345,7 @@ static void send_act_open_req6(struct cxgbi_sock *csk, struct sk_buff *skb,
 
 	cxgb4_l2t_send(csk->cdev->ports[csk->port_id], skb, csk->l2t);
 }
+#endif
 
 static void send_close_req(struct cxgbi_sock *csk)
 {
@@ -756,7 +758,7 @@ static int act_open_rpl_status_to_errno(int status)
 
 static void csk_act_open_retry_timer(unsigned long data)
 {
-	struct sk_buff *skb;
+	struct sk_buff *skb = NULL;
 	struct cxgbi_sock *csk = (struct cxgbi_sock *)data;
 	struct cxgb4_lld_info *lldi = cxgbi_cdev_priv(csk->cdev);
 	void (*send_act_open_func)(struct cxgbi_sock *, struct sk_buff *,
@@ -781,9 +783,11 @@ static void csk_act_open_retry_timer(unsigned long data)
 	if (csk->csk_family == AF_INET) {
 		send_act_open_func = send_act_open_req;
 		skb = alloc_wr(size, 0, GFP_ATOMIC);
+#if IS_ENABLED(CONFIG_IPV6)
 	} else {
 		send_act_open_func = send_act_open_req6;
 		skb = alloc_wr(size6, 0, GFP_ATOMIC);
+#endif
 	}
 
 	if (!skb)
@@ -1313,11 +1317,6 @@ static int init_act_open(struct cxgbi_sock *csk)
 	cxgbi_sock_set_flag(csk, CTPF_HAS_ATID);
 	cxgbi_sock_get(csk);
 
-	n = dst_neigh_lookup(csk->dst, &csk->daddr.sin_addr.s_addr);
-	if (!n) {
-		pr_err("%s, can't get neighbour of csk->dst.\n", ndev->name);
-		goto rel_resource;
-	}
 	csk->l2t = cxgb4_l2t_get(lldi->l2t, n, ndev, 0);
 	if (!csk->l2t) {
 		pr_err("%s, cannot alloc l2t.\n", ndev->name);
@@ -1335,8 +1334,10 @@ static int init_act_open(struct cxgbi_sock *csk)
 
 	if (csk->csk_family == AF_INET)
 		skb = alloc_wr(size, 0, GFP_NOIO);
+#if IS_ENABLED(CONFIG_IPV6)
 	else
 		skb = alloc_wr(size6, 0, GFP_NOIO);
+#endif
 
 	if (!skb)
 		goto rel_resource;
@@ -1370,8 +1371,10 @@ static int init_act_open(struct cxgbi_sock *csk)
 	cxgbi_sock_set_state(csk, CTP_ACTIVE_OPEN);
 	if (csk->csk_family == AF_INET)
 		send_act_open_req(csk, skb, csk->l2t);
+#if IS_ENABLED(CONFIG_IPV6)
 	else
 		send_act_open_req6(csk, skb, csk->l2t);
+#endif
 	neigh_release(n);
 
 	return 0;
@@ -1635,129 +1638,6 @@ static int cxgb4i_ddp_init(struct cxgbi_device *cdev)
 	return 0;
 }
 
-#if IS_ENABLED(CONFIG_IPV6)
-static int cxgbi_inet6addr_handler(struct notifier_block *this,
-				   unsigned long event, void *data)
-{
-	struct inet6_ifaddr *ifa = data;
-	struct net_device *event_dev = ifa->idev->dev;
-	struct cxgbi_device *cdev;
-	int ret = NOTIFY_DONE;
-
-	if (event_dev->priv_flags & IFF_802_1Q_VLAN)
-		event_dev = vlan_dev_real_dev(event_dev);
-
-	cdev = cxgbi_device_find_by_netdev(event_dev, NULL);
-
-	if (!cdev)
-		return ret;
-
-	switch (event) {
-	case NETDEV_UP:
-		ret = cxgb4_clip_get(event_dev,
-				     (const struct in6_addr *)
-				     ((ifa)->addr.s6_addr));
-		if (ret < 0)
-			return ret;
-
-		ret = NOTIFY_OK;
-		break;
-
-	case NETDEV_DOWN:
-		cxgb4_clip_release(event_dev,
-				   (const struct in6_addr *)
-				   ((ifa)->addr.s6_addr));
-		ret = NOTIFY_OK;
-		break;
-
-	default:
-		break;
-	}
-
-	return ret;
-}
-
-static struct notifier_block cxgbi_inet6addr_notifier = {
-	.notifier_call = cxgbi_inet6addr_handler
-};
-
-/* Retrieve IPv6 addresses from a root device (bond, vlan) associated with
- * a physical device.
- * The physical device reference is needed to send the actual CLIP command.
- */
-static int update_dev_clip(struct net_device *root_dev, struct net_device *dev)
-{
-	struct inet6_dev *idev = NULL;
-	struct inet6_ifaddr *ifa;
-	int ret = 0;
-
-	idev = __in6_dev_get(root_dev);
-	if (!idev)
-		return ret;
-
-	read_lock_bh(&idev->lock);
-	list_for_each_entry(ifa, &idev->addr_list, if_list) {
-		pr_info("updating the clip for addr %pI6\n",
-			ifa->addr.s6_addr);
-		ret = cxgb4_clip_get(dev, (const struct in6_addr *)
-				     ifa->addr.s6_addr);
-		if (ret < 0)
-			break;
-	}
-
-	read_unlock_bh(&idev->lock);
-	return ret;
-}
-
-static int update_root_dev_clip(struct net_device *dev)
-{
-	struct net_device *root_dev = NULL;
-	int i, ret = 0;
-
-	/* First populate the real net device's IPv6 address */
-	ret = update_dev_clip(dev, dev);
-	if (ret)
-		return ret;
-
-	/* Parse all bond and vlan devices layered on top of the physical dev */
-	root_dev = netdev_master_upper_dev_get(dev);
-	if (root_dev) {
-		ret = update_dev_clip(root_dev, dev);
-		if (ret)
-			return ret;
-	}
-
-	for (i = 0; i < VLAN_N_VID; i++) {
-		root_dev = __vlan_find_dev_deep_rcu(dev, htons(ETH_P_8021Q), i);
-		if (!root_dev)
-			continue;
-
-		ret = update_dev_clip(root_dev, dev);
-		if (ret)
-			break;
-	}
-	return ret;
-}
-
-static void cxgbi_update_clip(struct cxgbi_device *cdev)
-{
-	int i;
-
-	rcu_read_lock();
-
-	for (i = 0; i < cdev->nports; i++) {
-		struct net_device *dev = cdev->ports[i];
-		int ret = 0;
-
-		if (dev)
-			ret = update_root_dev_clip(dev);
-		if (ret < 0)
-			break;
-	}
-	rcu_read_unlock();
-}
-#endif /* IS_ENABLED(CONFIG_IPV6) */
-
 static void *t4_uld_add(const struct cxgb4_lld_info *lldi)
 {
 	struct cxgbi_device *cdev;
@@ -1876,10 +1756,6 @@ static int t4_uld_state_change(void *handle, enum cxgb4_state state)
 	switch (state) {
 	case CXGB4_STATE_UP:
 		pr_info("cdev 0x%p, UP.\n", cdev);
-#if IS_ENABLED(CONFIG_IPV6)
-		cxgbi_update_clip(cdev);
-#endif
-		/* re-initialize */
 		break;
 	case CXGB4_STATE_START_RECOVERY:
 		pr_info("cdev 0x%p, RECOVERY.\n", cdev);
@@ -1910,17 +1786,11 @@ static int __init cxgb4i_init_module(void)
 		return rc;
 	cxgb4_register_uld(CXGB4_ULD_ISCSI, &cxgb4i_uld_info);
 
-#if IS_ENABLED(CONFIG_IPV6)
-	register_inet6addr_notifier(&cxgbi_inet6addr_notifier);
-#endif
 	return 0;
 }
 
 static void __exit cxgb4i_exit_module(void)
 {
-#if IS_ENABLED(CONFIG_IPV6)
-	unregister_inet6addr_notifier(&cxgbi_inet6addr_notifier);
-#endif
 	cxgb4_unregister_uld(CXGB4_ULD_ISCSI);
 	cxgbi_device_unregister_all(CXGBI_FLAG_DEV_T4);
 	cxgbi_iscsi_cleanup(&cxgb4i_iscsi_transport, &cxgb4i_stt);
diff --git a/drivers/scsi/cxgbi/libcxgbi.c b/drivers/scsi/cxgbi/libcxgbi.c
index d65df6dc106f..54fa6e0bc1bb 100644
--- a/drivers/scsi/cxgbi/libcxgbi.c
+++ b/drivers/scsi/cxgbi/libcxgbi.c
@@ -57,6 +57,9 @@ MODULE_PARM_DESC(dbg_level, "libiscsi debug level (default=0)");
 static LIST_HEAD(cdev_list);
 static DEFINE_MUTEX(cdev_mutex);
 
+static LIST_HEAD(cdev_rcu_list);
+static DEFINE_SPINLOCK(cdev_rcu_lock);
+
 int cxgbi_device_portmap_create(struct cxgbi_device *cdev, unsigned int base,
 				unsigned int max_conn)
 {
@@ -142,6 +145,10 @@ struct cxgbi_device *cxgbi_device_register(unsigned int extra,
 	list_add_tail(&cdev->list_head, &cdev_list);
 	mutex_unlock(&cdev_mutex);
 
+	spin_lock(&cdev_rcu_lock);
+	list_add_tail_rcu(&cdev->rcu_node, &cdev_rcu_list);
+	spin_unlock(&cdev_rcu_lock);
+
 	log_debug(1 << CXGBI_DBG_DEV,
 		"cdev 0x%p, p# %u.\n", cdev, nports);
 	return cdev;
@@ -153,9 +160,16 @@ void cxgbi_device_unregister(struct cxgbi_device *cdev)
 	log_debug(1 << CXGBI_DBG_DEV,
 		"cdev 0x%p, p# %u,%s.\n",
 		cdev, cdev->nports, cdev->nports ? cdev->ports[0]->name : "");
+
 	mutex_lock(&cdev_mutex);
 	list_del(&cdev->list_head);
 	mutex_unlock(&cdev_mutex);
+
+	spin_lock(&cdev_rcu_lock);
+	list_del_rcu(&cdev->rcu_node);
+	spin_unlock(&cdev_rcu_lock);
+	synchronize_rcu();
+
 	cxgbi_device_destroy(cdev);
 }
 EXPORT_SYMBOL_GPL(cxgbi_device_unregister);
@@ -167,12 +181,9 @@ void cxgbi_device_unregister_all(unsigned int flag)
 	mutex_lock(&cdev_mutex);
 	list_for_each_entry_safe(cdev, tmp, &cdev_list, list_head) {
 		if ((cdev->flags & flag) == flag) {
-			log_debug(1 << CXGBI_DBG_DEV,
-				"cdev 0x%p, p# %u,%s.\n",
-				cdev, cdev->nports, cdev->nports ?
-				 cdev->ports[0]->name : "");
-			list_del(&cdev->list_head);
-			cxgbi_device_destroy(cdev);
+			mutex_unlock(&cdev_mutex);
+			cxgbi_device_unregister(cdev);
+			mutex_lock(&cdev_mutex);
 		}
 	}
 	mutex_unlock(&cdev_mutex);
@@ -191,6 +202,7 @@ struct cxgbi_device *cxgbi_device_find_by_lldev(void *lldev)
 		}
 	}
 	mutex_unlock(&cdev_mutex);
+
 	log_debug(1 << CXGBI_DBG_DEV,
 		"lldev 0x%p, NO match found.\n", lldev);
 	return NULL;
@@ -230,6 +242,40 @@ struct cxgbi_device *cxgbi_device_find_by_netdev(struct net_device *ndev,
 }
 EXPORT_SYMBOL_GPL(cxgbi_device_find_by_netdev);
 
+struct cxgbi_device *cxgbi_device_find_by_netdev_rcu(struct net_device *ndev,
+						     int *port)
+{
+	struct net_device *vdev = NULL;
+	struct cxgbi_device *cdev;
+	int i;
+
+	if (ndev->priv_flags & IFF_802_1Q_VLAN) {
+		vdev = ndev;
+		ndev = vlan_dev_real_dev(ndev);
+		pr_info("vlan dev %s -> %s.\n", vdev->name, ndev->name);
+	}
+
+	rcu_read_lock();
+	list_for_each_entry_rcu(cdev, &cdev_rcu_list, rcu_node) {
+		for (i = 0; i < cdev->nports; i++) {
+			if (ndev == cdev->ports[i]) {
+				cdev->hbas[i]->vdev = vdev;
+				rcu_read_unlock();
+				if (port)
+					*port = i;
+				return cdev;
+			}
+		}
+	}
+	rcu_read_unlock();
+
+	log_debug(1 << CXGBI_DBG_DEV,
+		  "ndev 0x%p, %s, NO match found.\n", ndev, ndev->name);
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(cxgbi_device_find_by_netdev_rcu);
+
+#if IS_ENABLED(CONFIG_IPV6)
 static struct cxgbi_device *cxgbi_device_find_by_mac(struct net_device *ndev,
 						     int *port)
 {
@@ -262,6 +308,7 @@ static struct cxgbi_device *cxgbi_device_find_by_mac(struct net_device *ndev,
 		  ndev, ndev->name);
 	return NULL;
 }
+#endif
 
 void cxgbi_hbas_remove(struct cxgbi_device *cdev)
 {
@@ -1807,7 +1854,7 @@ static void csk_return_rx_credits(struct cxgbi_sock *csk, int copied)
 	u32 credits;
 
 	log_debug(1 << CXGBI_DBG_PDU_RX,
-		"csk 0x%p,%u,0x%lu,%u, seq %u, wup %u, thre %u, %u.\n",
+		"csk 0x%p,%u,0x%lx,%u, seq %u, wup %u, thre %u, %u.\n",
 		csk, csk->state, csk->flags, csk->tid, csk->copied_seq,
 		csk->rcv_wup, cdev->rx_credit_thres,
 		cdev->rcv_win);
diff --git a/drivers/scsi/cxgbi/libcxgbi.h b/drivers/scsi/cxgbi/libcxgbi.h
index b3e6e7541cc5..1d98fad6a0ab 100644
--- a/drivers/scsi/cxgbi/libcxgbi.h
+++ b/drivers/scsi/cxgbi/libcxgbi.h
@@ -527,6 +527,7 @@ struct cxgbi_ports_map {
 #define CXGBI_FLAG_IPV4_SET		0x10
 struct cxgbi_device {
 	struct list_head list_head;
+	struct list_head rcu_node;
 	unsigned int flags;
 	struct net_device **ports;
 	void *lldev;
@@ -709,6 +710,8 @@ void cxgbi_device_unregister(struct cxgbi_device *);
 void cxgbi_device_unregister_all(unsigned int flag);
 struct cxgbi_device *cxgbi_device_find_by_lldev(void *);
 struct cxgbi_device *cxgbi_device_find_by_netdev(struct net_device *, int *);
+struct cxgbi_device *cxgbi_device_find_by_netdev_rcu(struct net_device *,
+						     int *);
 int cxgbi_hbas_add(struct cxgbi_device *, u64, unsigned int,
 			struct scsi_host_template *,
 			struct scsi_transport_template *);
diff --git a/drivers/scsi/device_handler/scsi_dh_alua.c b/drivers/scsi/device_handler/scsi_dh_alua.c
index 7bcf67eec921..e99507ed0e3c 100644
--- a/drivers/scsi/device_handler/scsi_dh_alua.c
+++ b/drivers/scsi/device_handler/scsi_dh_alua.c
@@ -115,7 +115,7 @@ static struct request *get_alua_req(struct scsi_device *sdev,
 
 	rq = blk_get_request(q, rw, GFP_NOIO);
 
-	if (!rq) {
+	if (IS_ERR(rq)) {
 		sdev_printk(KERN_INFO, sdev,
 			    "%s: blk_get_request failed\n", __func__);
 		return NULL;
diff --git a/drivers/scsi/device_handler/scsi_dh_emc.c b/drivers/scsi/device_handler/scsi_dh_emc.c
index 6f07f7fe3aa1..84765384c47c 100644
--- a/drivers/scsi/device_handler/scsi_dh_emc.c
+++ b/drivers/scsi/device_handler/scsi_dh_emc.c
@@ -275,7 +275,7 @@ static struct request *get_req(struct scsi_device *sdev, int cmd,
 
 	rq = blk_get_request(sdev->request_queue,
 			(cmd != INQUIRY) ? WRITE : READ, GFP_NOIO);
-	if (!rq) {
+	if (IS_ERR(rq)) {
 		sdev_printk(KERN_INFO, sdev, "get_req: blk_get_request failed");
 		return NULL;
 	}
diff --git a/drivers/scsi/device_handler/scsi_dh_hp_sw.c b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
index e9d9fea9e272..4ee2759f5299 100644
--- a/drivers/scsi/device_handler/scsi_dh_hp_sw.c
+++ b/drivers/scsi/device_handler/scsi_dh_hp_sw.c
@@ -117,7 +117,7 @@ static int hp_sw_tur(struct scsi_device *sdev, struct hp_sw_dh_data *h)
 
 retry:
 	req = blk_get_request(sdev->request_queue, WRITE, GFP_NOIO);
-	if (!req)
+	if (IS_ERR(req))
 		return SCSI_DH_RES_TEMP_UNAVAIL;
 
 	blk_rq_set_block_pc(req);
@@ -247,7 +247,7 @@ static int hp_sw_start_stop(struct hp_sw_dh_data *h)
 	struct request *req;
 
 	req = blk_get_request(h->sdev->request_queue, WRITE, GFP_ATOMIC);
-	if (!req)
+	if (IS_ERR(req))
 		return SCSI_DH_RES_TEMP_UNAVAIL;
 
 	blk_rq_set_block_pc(req);
diff --git a/drivers/scsi/device_handler/scsi_dh_rdac.c b/drivers/scsi/device_handler/scsi_dh_rdac.c
index 826069db9848..1b5bc9293e37 100644
--- a/drivers/scsi/device_handler/scsi_dh_rdac.c
+++ b/drivers/scsi/device_handler/scsi_dh_rdac.c
@@ -274,7 +274,7 @@ static struct request *get_rdac_req(struct scsi_device *sdev,
 
 	rq = blk_get_request(q, rw, GFP_NOIO);
 
-	if (!rq) {
+	if (IS_ERR(rq)) {
 		sdev_printk(KERN_INFO, sdev,
 				"get_rdac_req: blk_get_request failed.\n");
 		return NULL;
diff --git a/drivers/scsi/dpt_i2o.c b/drivers/scsi/dpt_i2o.c
index 67283ef418ac..072f0ec2851e 100644
--- a/drivers/scsi/dpt_i2o.c
+++ b/drivers/scsi/dpt_i2o.c
@@ -2363,6 +2363,7 @@ static s32 adpt_scsi_host_alloc(adpt_hba* pHba, struct scsi_host_template *sht)
 	host->unique_id = (u32)sys_tbl_pa + pHba->unit;
 	host->sg_tablesize = pHba->sg_tablesize;
 	host->can_queue = pHba->post_fifo_size;
+	host->use_cmd_list = 1;
 
 	return 0;
 }
diff --git a/drivers/scsi/eata.c b/drivers/scsi/eata.c
index 813dd5c998e4..943ad3a19661 100644
--- a/drivers/scsi/eata.c
+++ b/drivers/scsi/eata.c
@@ -837,7 +837,6 @@ struct hostdata {
 static struct Scsi_Host *sh[MAX_BOARDS];
 static const char *driver_name = "EATA";
 static char sha[MAX_BOARDS];
-static DEFINE_SPINLOCK(driver_lock);
 
 /* Initialize num_boards so that ihdlr can work while detect is in progress */
 static unsigned int num_boards = MAX_BOARDS;
@@ -1097,8 +1096,6 @@ static int port_detect(unsigned long port_base, unsigned int j,
 		goto fail;
 	}
 
-	spin_lock_irq(&driver_lock);
-
 	if (do_dma(port_base, 0, READ_CONFIG_PIO)) {
 #if defined(DEBUG_DETECT)
 		printk("%s: detect, do_dma failed at 0x%03lx.\n", name,
@@ -1264,10 +1261,7 @@ static int port_detect(unsigned long port_base, unsigned int j,
 	}
 #endif
 
-	spin_unlock_irq(&driver_lock);
 	sh[j] = shost = scsi_register(tpnt, sizeof(struct hostdata));
-	spin_lock_irq(&driver_lock);
-
 	if (shost == NULL) {
 		printk("%s: unable to register host, detaching.\n", name);
 		goto freedma;
@@ -1344,8 +1338,6 @@ static int port_detect(unsigned long port_base, unsigned int j,
 	else
 		sprintf(dma_name, "DMA %u", dma_channel);
 
-	spin_unlock_irq(&driver_lock);
-
 	for (i = 0; i < shost->can_queue; i++)
 		ha->cp[i].cp_dma_addr = pci_map_single(ha->pdev,
 							  &ha->cp[i],
@@ -1438,7 +1430,6 @@ static int port_detect(unsigned long port_base, unsigned int j,
       freeirq:
 	free_irq(irq, &sha[j]);
       freelock:
-	spin_unlock_irq(&driver_lock);
 	release_region(port_base, REGION_SIZE);
       fail:
 	return 0;
diff --git a/drivers/scsi/fcoe/fcoe.c b/drivers/scsi/fcoe/fcoe.c
index 00ee0ed642aa..4a8ac7d8c76b 100644
--- a/drivers/scsi/fcoe/fcoe.c
+++ b/drivers/scsi/fcoe/fcoe.c
@@ -1884,7 +1884,6 @@ retry:
 			set_current_state(TASK_INTERRUPTIBLE);
 			spin_unlock_bh(&p->fcoe_rx_list.lock);
 			schedule();
-			set_current_state(TASK_RUNNING);
 			goto retry;
 		}
 
diff --git a/drivers/scsi/fcoe/fcoe_transport.c b/drivers/scsi/fcoe/fcoe_transport.c
index 74277c20f6a5..bdc89899561a 100644
--- a/drivers/scsi/fcoe/fcoe_transport.c
+++ b/drivers/scsi/fcoe/fcoe_transport.c
@@ -96,14 +96,32 @@ int fcoe_link_speed_update(struct fc_lport *lport)
 	struct ethtool_cmd ecmd;
 
 	if (!__ethtool_get_settings(netdev, &ecmd)) {
-		lport->link_supported_speeds &=
-			~(FC_PORTSPEED_1GBIT | FC_PORTSPEED_10GBIT);
+		lport->link_supported_speeds &= ~(FC_PORTSPEED_1GBIT  |
+		                                  FC_PORTSPEED_10GBIT |
+		                                  FC_PORTSPEED_20GBIT |
+		                                  FC_PORTSPEED_40GBIT);
+
 		if (ecmd.supported & (SUPPORTED_1000baseT_Half |
-				      SUPPORTED_1000baseT_Full))
+		                      SUPPORTED_1000baseT_Full |
+		                      SUPPORTED_1000baseKX_Full))
 			lport->link_supported_speeds |= FC_PORTSPEED_1GBIT;
-		if (ecmd.supported & SUPPORTED_10000baseT_Full)
-			lport->link_supported_speeds |=
-				FC_PORTSPEED_10GBIT;
+
+		if (ecmd.supported & (SUPPORTED_10000baseT_Full   |
+		                      SUPPORTED_10000baseKX4_Full |
+		                      SUPPORTED_10000baseKR_Full  |
+		                      SUPPORTED_10000baseR_FEC))
+			lport->link_supported_speeds |= FC_PORTSPEED_10GBIT;
+
+		if (ecmd.supported & (SUPPORTED_20000baseMLD2_Full |
+		                      SUPPORTED_20000baseKR2_Full))
+			lport->link_supported_speeds |= FC_PORTSPEED_20GBIT;
+
+		if (ecmd.supported & (SUPPORTED_40000baseKR4_Full |
+		                      SUPPORTED_40000baseCR4_Full |
+		                      SUPPORTED_40000baseSR4_Full |
+		                      SUPPORTED_40000baseLR4_Full))
+			lport->link_supported_speeds |= FC_PORTSPEED_40GBIT;
+
 		switch (ethtool_cmd_speed(&ecmd)) {
 		case SPEED_1000:
 			lport->link_speed = FC_PORTSPEED_1GBIT;
@@ -111,6 +129,15 @@ int fcoe_link_speed_update(struct fc_lport *lport)
 		case SPEED_10000:
 			lport->link_speed = FC_PORTSPEED_10GBIT;
 			break;
+		case 20000:
+			lport->link_speed = FC_PORTSPEED_20GBIT;
+			break;
+		case 40000:
+			lport->link_speed = FC_PORTSPEED_40GBIT;
+			break;
+		default:
+			lport->link_speed = FC_PORTSPEED_UNKNOWN;
+			break;
 		}
 		return 0;
 	}
diff --git a/drivers/scsi/fnic/fnic.h b/drivers/scsi/fnic/fnic.h
index 1d3521e13d77..bf8d34c26f13 100644
--- a/drivers/scsi/fnic/fnic.h
+++ b/drivers/scsi/fnic/fnic.h
@@ -39,7 +39,7 @@
 
 #define DRV_NAME		"fnic"
 #define DRV_DESCRIPTION		"Cisco FCoE HBA Driver"
-#define DRV_VERSION		"1.6.0.10"
+#define DRV_VERSION		"1.6.0.11"
 #define PFX			DRV_NAME ": "
 #define DFX                     DRV_NAME "%d: "
 
diff --git a/drivers/scsi/fnic/fnic_debugfs.c b/drivers/scsi/fnic/fnic_debugfs.c
index 2c613bdea78f..5980c10c734d 100644
--- a/drivers/scsi/fnic/fnic_debugfs.c
+++ b/drivers/scsi/fnic/fnic_debugfs.c
@@ -210,7 +210,7 @@ static ssize_t fnic_trace_ctrl_write(struct file *filp,
 	else if (*trace_type == fc_trc_flag->fc_clear)
 		fnic_fc_trace_cleared = val;
 	else
-		pr_err("fnic: cannot write to any debufs file\n");
+		pr_err("fnic: cannot write to any debugfs file\n");
 
 	(*ppos)++;
 
diff --git a/drivers/scsi/fnic/fnic_fcs.c b/drivers/scsi/fnic/fnic_fcs.c
index 1b948f633fc5..f3984b48f8e9 100644
--- a/drivers/scsi/fnic/fnic_fcs.c
+++ b/drivers/scsi/fnic/fnic_fcs.c
@@ -35,7 +35,7 @@
 #include "cq_enet_desc.h"
 #include "cq_exch_desc.h"
 
-static u8 fcoe_all_fcfs[ETH_ALEN];
+static u8 fcoe_all_fcfs[ETH_ALEN] = FIP_ALL_FCF_MACS;
 struct workqueue_struct *fnic_fip_queue;
 struct workqueue_struct *fnic_event_queue;
 
@@ -101,13 +101,14 @@ void fnic_handle_link(struct work_struct *work)
 				FNIC_FCS_DBG(KERN_DEBUG, fnic->lport->host,
 					     "link up\n");
 				fcoe_ctlr_link_up(&fnic->ctlr);
-			} else
+			} else {
 				/* UP -> UP */
 				spin_unlock_irqrestore(&fnic->fnic_lock, flags);
 				fnic_fc_trace_set_data(
 					fnic->lport->host->host_no, FNIC_FC_LE,
 					"Link Status: UP_UP",
 					strlen("Link Status: UP_UP"));
+			}
 		}
 	} else if (fnic->link_status) {
 		/* DOWN -> UP */
diff --git a/drivers/scsi/fnic/fnic_trace.c b/drivers/scsi/fnic/fnic_trace.c
index c77285926827..acf1f95cb5c5 100644
--- a/drivers/scsi/fnic/fnic_trace.c
+++ b/drivers/scsi/fnic/fnic_trace.c
@@ -592,7 +592,7 @@ int fnic_fc_trace_set_data(u32 host_no, u8 frame_type,
 
 	if (fnic_fc_trace_cleared == 1) {
 		fc_trace_entries.rd_idx = fc_trace_entries.wr_idx = 0;
-		pr_info("fnic: Reseting the read idx\n");
+		pr_info("fnic: Resetting the read idx\n");
 		memset((void *)fnic_fc_ctlr_trace_buf_p, 0,
 				fnic_fc_trace_max_pages * PAGE_SIZE);
 		fnic_fc_trace_cleared = 0;
@@ -743,7 +743,7 @@ void copy_and_format_trace_data(struct fc_trace_hdr *tdata,
 
 	fmt = "%02d:%02d:%04ld %02d:%02d:%02d.%09lu ns%8x       %c%8x\t";
 	len += snprintf(fnic_dbgfs_prt->buffer + len,
-		(fnic_fc_trace_max_pages * PAGE_SIZE * 3) - len,
+		max_size - len,
 		fmt,
 		tm.tm_mon + 1, tm.tm_mday, tm.tm_year + 1900,
 		tm.tm_hour, tm.tm_min, tm.tm_sec,
@@ -767,8 +767,7 @@ void copy_and_format_trace_data(struct fc_trace_hdr *tdata,
 				j == ethhdr_len + fcoehdr_len + fchdr_len ||
 				(i > 3 && j%fchdr_len == 0)) {
 				len += snprintf(fnic_dbgfs_prt->buffer
-					+ len, (fnic_fc_trace_max_pages
-					* PAGE_SIZE * 3) - len,
+					+ len, max_size - len,
 					"\n\t\t\t\t\t\t\t\t");
 				i++;
 			}
diff --git a/drivers/scsi/hpsa.c b/drivers/scsi/hpsa.c
index 6b35d0dfe64c..cef5d49b59cd 100644
--- a/drivers/scsi/hpsa.c
+++ b/drivers/scsi/hpsa.c
@@ -5971,10 +5971,6 @@ static int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 
 	/* Save the PCI command register */
 	pci_read_config_word(pdev, 4, &command_register);
-	/* Turn the board off.  This is so that later pci_restore_state()
-	 * won't turn the board on before the rest of config space is ready.
-	 */
-	pci_disable_device(pdev);
 	pci_save_state(pdev);
 
 	/* find the first memory BAR, so we can find the cfg table */
@@ -6022,11 +6018,6 @@ static int hpsa_kdump_hard_reset_controller(struct pci_dev *pdev)
 		goto unmap_cfgtable;
 
 	pci_restore_state(pdev);
-	rc = pci_enable_device(pdev);
-	if (rc) {
-		dev_warn(&pdev->dev, "failed to enable device.\n");
-		goto unmap_cfgtable;
-	}
 	pci_write_config_word(pdev, 4, command_register);
 
 	/* Some devices (notably the HP Smart Array 5i Controller)
@@ -6159,26 +6150,22 @@ static void hpsa_interrupt_mode(struct ctlr_info *h)
 		h->msix_vector = MAX_REPLY_QUEUES;
 		if (h->msix_vector > num_online_cpus())
 			h->msix_vector = num_online_cpus();
-		err = pci_enable_msix(h->pdev, hpsa_msix_entries,
-				      h->msix_vector);
-		if (err > 0) {
+		err = pci_enable_msix_range(h->pdev, hpsa_msix_entries,
+					    1, h->msix_vector);
+		if (err < 0) {
+			dev_warn(&h->pdev->dev, "MSI-X init failed %d\n", err);
+			h->msix_vector = 0;
+			goto single_msi_mode;
+		} else if (err < h->msix_vector) {
 			dev_warn(&h->pdev->dev, "only %d MSI-X vectors "
 			       "available\n", err);
-			h->msix_vector = err;
-			err = pci_enable_msix(h->pdev, hpsa_msix_entries,
-					      h->msix_vector);
-		}
-		if (!err) {
-			for (i = 0; i < h->msix_vector; i++)
-				h->intr[i] = hpsa_msix_entries[i].vector;
-			return;
-		} else {
-			dev_warn(&h->pdev->dev, "MSI-X init failed %d\n",
-			       err);
-			h->msix_vector = 0;
-			goto default_int_mode;
 		}
+		h->msix_vector = err;
+		for (i = 0; i < h->msix_vector; i++)
+			h->intr[i] = hpsa_msix_entries[i].vector;
+		return;
 	}
+single_msi_mode:
 	if (pci_find_capability(h->pdev, PCI_CAP_ID_MSI)) {
 		dev_info(&h->pdev->dev, "MSI\n");
 		if (!pci_enable_msi(h->pdev))
@@ -6541,6 +6528,23 @@ static int hpsa_init_reset_devices(struct pci_dev *pdev)
 	if (!reset_devices)
 		return 0;
 
+	/* kdump kernel is loading, we don't know in which state is
+	 * the pci interface. The dev->enable_cnt is equal zero
+	 * so we call enable+disable, wait a while and switch it on.
+	 */
+	rc = pci_enable_device(pdev);
+	if (rc) {
+		dev_warn(&pdev->dev, "Failed to enable PCI device\n");
+		return -ENODEV;
+	}
+	pci_disable_device(pdev);
+	msleep(260);			/* a randomly chosen number */
+	rc = pci_enable_device(pdev);
+	if (rc) {
+		dev_warn(&pdev->dev, "failed to enable device.\n");
+		return -ENODEV;
+	}
+	pci_set_master(pdev);
 	/* Reset the controller with a PCI power-cycle or via doorbell */
 	rc = hpsa_kdump_hard_reset_controller(pdev);
 
@@ -6549,10 +6553,11 @@ static int hpsa_init_reset_devices(struct pci_dev *pdev)
 	 * "performant mode".  Or, it might be 640x, which can't reset
 	 * due to concerns about shared bbwc between 6402/6404 pair.
 	 */
-	if (rc == -ENOTSUPP)
-		return rc; /* just try to do the kdump anyhow. */
-	if (rc)
-		return -ENODEV;
+	if (rc) {
+		if (rc != -ENOTSUPP) /* just try to do the kdump anyhow. */
+			rc = -ENODEV;
+		goto out_disable;
+	}
 
 	/* Now try to get the controller to respond to a no-op */
 	dev_warn(&pdev->dev, "Waiting for controller to respond to no-op\n");
@@ -6563,7 +6568,11 @@ static int hpsa_init_reset_devices(struct pci_dev *pdev)
 			dev_warn(&pdev->dev, "no-op failed%s\n",
 					(i < 11 ? "; re-trying" : ""));
 	}
-	return 0;
+
+out_disable:
+
+	pci_disable_device(pdev);
+	return rc;
 }
 
 static int hpsa_allocate_cmd_pool(struct ctlr_info *h)
@@ -6743,6 +6752,7 @@ static void hpsa_undo_allocations_after_kdump_soft_reset(struct ctlr_info *h)
 		iounmap(h->transtable);
 	if (h->cfgtable)
 		iounmap(h->cfgtable);
+	pci_disable_device(h->pdev);
 	pci_release_regions(h->pdev);
 	kfree(h);
 }
diff --git a/drivers/scsi/ipr.c b/drivers/scsi/ipr.c
index 924b0ba74dfe..2a9578c116b7 100644
--- a/drivers/scsi/ipr.c
+++ b/drivers/scsi/ipr.c
@@ -2440,6 +2440,7 @@ static void ipr_handle_log_data(struct ipr_ioa_cfg *ioa_cfg,
 {
 	u32 ioasc;
 	int error_index;
+	struct ipr_hostrcb_type_21_error *error;
 
 	if (hostrcb->hcam.notify_type != IPR_HOST_RCB_NOTIF_TYPE_ERROR_LOG_ENTRY)
 		return;
@@ -2464,6 +2465,15 @@ static void ipr_handle_log_data(struct ipr_ioa_cfg *ioa_cfg,
 	if (!ipr_error_table[error_index].log_hcam)
 		return;
 
+	if (ioasc == IPR_IOASC_HW_CMD_FAILED &&
+	    hostrcb->hcam.overlay_id == IPR_HOST_RCB_OVERLAY_ID_21) {
+		error = &hostrcb->hcam.u.error64.u.type_21_error;
+
+		if (((be32_to_cpu(error->sense_data[0]) & 0x0000ff00) >> 8) == ILLEGAL_REQUEST &&
+			ioa_cfg->log_level <= IPR_DEFAULT_LOG_LEVEL)
+				return;
+	}
+
 	ipr_hcam_err(hostrcb, "%s\n", ipr_error_table[error_index].error);
 
 	/* Set indication we have logged an error */
diff --git a/drivers/scsi/ipr.h b/drivers/scsi/ipr.h
index 31ed126f7143..d0201ceb4aac 100644
--- a/drivers/scsi/ipr.h
+++ b/drivers/scsi/ipr.h
@@ -130,6 +130,7 @@
 #define IPR_IOASC_HW_DEV_BUS_STATUS			0x04448500
 #define	IPR_IOASC_IOASC_MASK			0xFFFFFF00
 #define	IPR_IOASC_SCSI_STATUS_MASK		0x000000FF
+#define IPR_IOASC_HW_CMD_FAILED			0x046E0000
 #define IPR_IOASC_IR_INVALID_REQ_TYPE_OR_PKT	0x05240000
 #define IPR_IOASC_IR_RESOURCE_HANDLE		0x05250000
 #define IPR_IOASC_IR_NO_CMDS_TO_2ND_IOA		0x05258100
diff --git a/drivers/scsi/ips.c b/drivers/scsi/ips.c
index 52a216f21ae5..e5afc3884d74 100644
--- a/drivers/scsi/ips.c
+++ b/drivers/scsi/ips.c
@@ -528,7 +528,7 @@ ips_setup(char *ips_str)
 		 * Update the variables
 		 */
 		for (i = 0; i < ARRAY_SIZE(options); i++) {
-			if (strnicmp
+			if (strncasecmp
 			    (key, options[i].option_name,
 			     strlen(options[i].option_name)) == 0) {
 				if (value)
diff --git a/drivers/scsi/iscsi_tcp.c b/drivers/scsi/iscsi_tcp.c
index a669f2d11c31..427af0f24b0f 100644
--- a/drivers/scsi/iscsi_tcp.c
+++ b/drivers/scsi/iscsi_tcp.c
@@ -726,13 +726,18 @@ static int iscsi_sw_tcp_conn_get_param(struct iscsi_cls_conn *cls_conn,
 	switch(param) {
 	case ISCSI_PARAM_CONN_PORT:
 	case ISCSI_PARAM_CONN_ADDRESS:
+	case ISCSI_PARAM_LOCAL_PORT:
 		spin_lock_bh(&conn->session->frwd_lock);
 		if (!tcp_sw_conn || !tcp_sw_conn->sock) {
 			spin_unlock_bh(&conn->session->frwd_lock);
 			return -ENOTCONN;
 		}
-		rc = kernel_getpeername(tcp_sw_conn->sock,
-					(struct sockaddr *)&addr, &len);
+		if (param == ISCSI_PARAM_LOCAL_PORT)
+			rc = kernel_getsockname(tcp_sw_conn->sock,
+						(struct sockaddr *)&addr, &len);
+		else
+			rc = kernel_getpeername(tcp_sw_conn->sock,
+						(struct sockaddr *)&addr, &len);
 		spin_unlock_bh(&conn->session->frwd_lock);
 		if (rc)
 			return rc;
@@ -895,6 +900,7 @@ static umode_t iscsi_sw_tcp_attr_is_visible(int param_type, int param)
 		case ISCSI_PARAM_DATADGST_EN:
 		case ISCSI_PARAM_CONN_ADDRESS:
 		case ISCSI_PARAM_CONN_PORT:
+		case ISCSI_PARAM_LOCAL_PORT:
 		case ISCSI_PARAM_EXP_STATSN:
 		case ISCSI_PARAM_PERSISTENT_ADDRESS:
 		case ISCSI_PARAM_PERSISTENT_PORT:
diff --git a/drivers/scsi/libfc/fc_libfc.c b/drivers/scsi/libfc/fc_libfc.c
index 8d65a51a7598..c11a638f32e6 100644
--- a/drivers/scsi/libfc/fc_libfc.c
+++ b/drivers/scsi/libfc/fc_libfc.c
@@ -296,9 +296,9 @@ void fc_fc4_deregister_provider(enum fc_fh_type type, struct fc4_prov *prov)
 	BUG_ON(type >= FC_FC4_PROV_SIZE);
 	mutex_lock(&fc_prov_mutex);
 	if (prov->recv)
-		rcu_assign_pointer(fc_passive_prov[type], NULL);
+		RCU_INIT_POINTER(fc_passive_prov[type], NULL);
 	else
-		rcu_assign_pointer(fc_active_prov[type], NULL);
+		RCU_INIT_POINTER(fc_active_prov[type], NULL);
 	mutex_unlock(&fc_prov_mutex);
 	synchronize_rcu();
 }
diff --git a/drivers/scsi/libiscsi.c b/drivers/scsi/libiscsi.c
index ea025e4806b6..0d8bc6c66650 100644
--- a/drivers/scsi/libiscsi.c
+++ b/drivers/scsi/libiscsi.c
@@ -717,11 +717,21 @@ __iscsi_conn_send_pdu(struct iscsi_conn *conn, struct iscsi_hdr *hdr,
 			return NULL;
 		}
 
+		if (data_size > ISCSI_DEF_MAX_RECV_SEG_LEN) {
+			iscsi_conn_printk(KERN_ERR, conn, "Invalid buffer len of %u for login task. Max len is %u\n", data_size, ISCSI_DEF_MAX_RECV_SEG_LEN);
+			return NULL;
+		}
+
 		task = conn->login_task;
 	} else {
 		if (session->state != ISCSI_STATE_LOGGED_IN)
 			return NULL;
 
+		if (data_size != 0) {
+			iscsi_conn_printk(KERN_ERR, conn, "Can not send data buffer of len %u for op 0x%x\n", data_size, opcode);
+			return NULL;
+		}
+
 		BUG_ON(conn->c_stage == ISCSI_CONN_INITIAL_STAGE);
 		BUG_ON(conn->c_stage == ISCSI_CONN_STOPPED);
 
@@ -3495,6 +3505,7 @@ int iscsi_conn_get_addr_param(struct sockaddr_storage *addr,
 			len = sprintf(buf, "%pI6\n", &sin6->sin6_addr);
 		break;
 	case ISCSI_PARAM_CONN_PORT:
+	case ISCSI_PARAM_LOCAL_PORT:
 		if (sin)
 			len = sprintf(buf, "%hu\n", be16_to_cpu(sin->sin_port));
 		else
diff --git a/drivers/scsi/lpfc/lpfc_attr.c b/drivers/scsi/lpfc/lpfc_attr.c
index 6eed9e76a166..2f9b96826ac0 100644
--- a/drivers/scsi/lpfc/lpfc_attr.c
+++ b/drivers/scsi/lpfc/lpfc_attr.c
@@ -3385,7 +3385,7 @@ lpfc_stat_data_ctrl_store(struct device *dev, struct device_attribute *attr,
 		if (strlen(buf) > (LPFC_MAX_DATA_CTRL_LEN - 1))
 			return -EINVAL;
 
-		strcpy(bucket_data, buf);
+		strncpy(bucket_data, buf, LPFC_MAX_DATA_CTRL_LEN);
 		str_ptr = &bucket_data[0];
 		/* Ignore this token - this is command token */
 		token = strsep(&str_ptr, "\t ");
diff --git a/drivers/scsi/lpfc/lpfc_bsg.c b/drivers/scsi/lpfc/lpfc_bsg.c
index 5b5c825d9576..a7bf359aa0c6 100644
--- a/drivers/scsi/lpfc/lpfc_bsg.c
+++ b/drivers/scsi/lpfc/lpfc_bsg.c
@@ -656,7 +656,6 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 	struct lpfc_nodelist *ndlp = rdata->pnode;
 	uint32_t elscmd;
 	uint32_t cmdsize;
-	uint32_t rspsize;
 	struct lpfc_iocbq *cmdiocbq;
 	uint16_t rpi = 0;
 	struct bsg_job_data *dd_data;
@@ -687,7 +686,6 @@ lpfc_bsg_rport_els(struct fc_bsg_job *job)
 
 	elscmd = job->request->rqst_data.r_els.els_code;
 	cmdsize = job->request_payload.payload_len;
-	rspsize = job->reply_payload.payload_len;
 
 	if (!lpfc_nlp_get(ndlp)) {
 		rc = -ENODEV;
@@ -2251,7 +2249,6 @@ lpfc_sli4_bsg_diag_mode_end(struct fc_bsg_job *job)
 	i = 0;
 	while (phba->link_state != LPFC_LINK_DOWN) {
 		if (i++ > timeout) {
-			rc = -ETIMEDOUT;
 			lpfc_printf_log(phba, KERN_INFO, LOG_LIBDFC,
 					"3140 Timeout waiting for link to "
 					"diagnostic mode_end, timeout:%d ms\n",
@@ -2291,7 +2288,6 @@ lpfc_sli4_bsg_link_diag_test(struct fc_bsg_job *job)
 	LPFC_MBOXQ_t *pmboxq;
 	struct sli4_link_diag *link_diag_test_cmd;
 	uint32_t req_len, alloc_len;
-	uint32_t timeout;
 	struct lpfc_mbx_run_link_diag_test *run_link_diag_test;
 	union lpfc_sli4_cfg_shdr *shdr;
 	uint32_t shdr_status, shdr_add_status;
@@ -2342,7 +2338,6 @@ lpfc_sli4_bsg_link_diag_test(struct fc_bsg_job *job)
 
 	link_diag_test_cmd = (struct sli4_link_diag *)
 			 job->request->rqst_data.h_vendor.vendor_cmd;
-	timeout = link_diag_test_cmd->timeout * 100;
 
 	rc = lpfc_sli4_bsg_set_link_diag_state(phba, 1);
 
@@ -2693,14 +2688,13 @@ lpfc_bsg_dma_page_alloc(struct lpfc_hba *phba)
 	INIT_LIST_HEAD(&dmabuf->list);
 
 	/* now, allocate dma buffer */
-	dmabuf->virt = dma_alloc_coherent(&pcidev->dev, BSG_MBOX_SIZE,
-					  &(dmabuf->phys), GFP_KERNEL);
+	dmabuf->virt = dma_zalloc_coherent(&pcidev->dev, BSG_MBOX_SIZE,
+					   &(dmabuf->phys), GFP_KERNEL);
 
 	if (!dmabuf->virt) {
 		kfree(dmabuf);
 		return NULL;
 	}
-	memset((uint8_t *)dmabuf->virt, 0, BSG_MBOX_SIZE);
 
 	return dmabuf;
 }
@@ -2828,8 +2822,10 @@ diag_cmd_data_alloc(struct lpfc_hba *phba,
 		size -= cnt;
 	}
 
-	mlist->flag = i;
-	return mlist;
+	if (mlist) {
+		mlist->flag = i;
+		return mlist;
+	}
 out:
 	diag_cmd_data_free(phba, mlist);
 	return NULL;
@@ -3344,7 +3340,7 @@ job_error:
  * will wake up thread waiting on the wait queue pointed by context1
  * of the mailbox.
  **/
-void
+static void
 lpfc_bsg_issue_mbox_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 {
 	struct bsg_job_data *dd_data;
@@ -4593,7 +4589,7 @@ sli_cfg_ext_error:
  * being reset) and com-plete the job, otherwise issue the mailbox command and
  * let our completion handler finish the command.
  **/
-static uint32_t
+static int
 lpfc_bsg_issue_mbox(struct lpfc_hba *phba, struct fc_bsg_job *job,
 	struct lpfc_vport *vport)
 {
diff --git a/drivers/scsi/lpfc/lpfc_crtn.h b/drivers/scsi/lpfc/lpfc_crtn.h
index db5604f01a1a..00665a5d92fd 100644
--- a/drivers/scsi/lpfc/lpfc_crtn.h
+++ b/drivers/scsi/lpfc/lpfc_crtn.h
@@ -451,7 +451,6 @@ int lpfc_send_rrq(struct lpfc_hba *, struct lpfc_node_rrq *);
 int lpfc_set_rrq_active(struct lpfc_hba *, struct lpfc_nodelist *,
 	uint16_t, uint16_t, uint16_t);
 uint16_t lpfc_sli4_xri_inrange(struct lpfc_hba *, uint16_t);
-void lpfc_cleanup_wt_rrqs(struct lpfc_hba *);
 void lpfc_cleanup_vports_rrqs(struct lpfc_vport *, struct lpfc_nodelist *);
 struct lpfc_node_rrq *lpfc_get_active_rrq(struct lpfc_vport *, uint16_t,
 	uint32_t);
diff --git a/drivers/scsi/lpfc/lpfc_ct.c b/drivers/scsi/lpfc/lpfc_ct.c
index da61d8dc0449..61a32cd23f79 100644
--- a/drivers/scsi/lpfc/lpfc_ct.c
+++ b/drivers/scsi/lpfc/lpfc_ct.c
@@ -1439,7 +1439,7 @@ lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
 			/* #2 HBA attribute entry */
 			ae = (ATTRIBUTE_ENTRY *) ((uint8_t *) rh + size);
 			ae->ad.bits.AttrType = be16_to_cpu(MANUFACTURER);
-			strcpy(ae->un.Manufacturer, "Emulex Corporation");
+			strncpy(ae->un.Manufacturer, "Emulex Corporation", 64);
 			len = strlen(ae->un.Manufacturer);
 			len += (len & 3) ? (4 - (len & 3)) : 4;
 			ae->ad.bits.AttrLen = be16_to_cpu(FOURBYTES + len);
@@ -1449,7 +1449,7 @@ lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
 			/* #3 HBA attribute entry */
 			ae = (ATTRIBUTE_ENTRY *) ((uint8_t *) rh + size);
 			ae->ad.bits.AttrType = be16_to_cpu(SERIAL_NUMBER);
-			strcpy(ae->un.SerialNumber, phba->SerialNumber);
+			strncpy(ae->un.SerialNumber, phba->SerialNumber, 64);
 			len = strlen(ae->un.SerialNumber);
 			len += (len & 3) ? (4 - (len & 3)) : 4;
 			ae->ad.bits.AttrLen = be16_to_cpu(FOURBYTES + len);
@@ -1459,7 +1459,7 @@ lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
 			/* #4 HBA attribute entry */
 			ae = (ATTRIBUTE_ENTRY *) ((uint8_t *) rh + size);
 			ae->ad.bits.AttrType = be16_to_cpu(MODEL);
-			strcpy(ae->un.Model, phba->ModelName);
+			strncpy(ae->un.Model, phba->ModelName, 256);
 			len = strlen(ae->un.Model);
 			len += (len & 3) ? (4 - (len & 3)) : 4;
 			ae->ad.bits.AttrLen = be16_to_cpu(FOURBYTES + len);
@@ -1469,7 +1469,7 @@ lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
 			/* #5 HBA attribute entry */
 			ae = (ATTRIBUTE_ENTRY *) ((uint8_t *) rh + size);
 			ae->ad.bits.AttrType = be16_to_cpu(MODEL_DESCRIPTION);
-			strcpy(ae->un.ModelDescription, phba->ModelDesc);
+			strncpy(ae->un.ModelDescription, phba->ModelDesc, 256);
 			len = strlen(ae->un.ModelDescription);
 			len += (len & 3) ? (4 - (len & 3)) : 4;
 			ae->ad.bits.AttrLen = be16_to_cpu(FOURBYTES + len);
@@ -1500,7 +1500,8 @@ lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
 			/* #7 HBA attribute entry */
 			ae = (ATTRIBUTE_ENTRY *) ((uint8_t *) rh + size);
 			ae->ad.bits.AttrType = be16_to_cpu(DRIVER_VERSION);
-			strcpy(ae->un.DriverVersion, lpfc_release_version);
+			strncpy(ae->un.DriverVersion,
+				lpfc_release_version, 256);
 			len = strlen(ae->un.DriverVersion);
 			len += (len & 3) ? (4 - (len & 3)) : 4;
 			ae->ad.bits.AttrLen = be16_to_cpu(FOURBYTES + len);
@@ -1510,7 +1511,8 @@ lpfc_fdmi_cmd(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp, int cmdcode)
 			/* #8 HBA attribute entry */
 			ae = (ATTRIBUTE_ENTRY *) ((uint8_t *) rh + size);
 			ae->ad.bits.AttrType = be16_to_cpu(OPTION_ROM_VERSION);
-			strcpy(ae->un.OptionROMVersion, phba->OptionROMVersion);
+			strncpy(ae->un.OptionROMVersion,
+				phba->OptionROMVersion, 256);
 			len = strlen(ae->un.OptionROMVersion);
 			len += (len & 3) ? (4 - (len & 3)) : 4;
 			ae->ad.bits.AttrLen = be16_to_cpu(FOURBYTES + len);
diff --git a/drivers/scsi/lpfc/lpfc_debugfs.c b/drivers/scsi/lpfc/lpfc_debugfs.c
index b0aedce3f54b..786a2aff7b59 100644
--- a/drivers/scsi/lpfc/lpfc_debugfs.c
+++ b/drivers/scsi/lpfc/lpfc_debugfs.c
@@ -269,7 +269,7 @@ static int
 lpfc_debugfs_hbqinfo_data(struct lpfc_hba *phba, char *buf, int size)
 {
 	int len = 0;
-	int cnt, i, j, found, posted, low;
+	int i, j, found, posted, low;
 	uint32_t phys, raw_index, getidx;
 	struct lpfc_hbq_init *hip;
 	struct hbq_s *hbqs;
@@ -279,7 +279,7 @@ lpfc_debugfs_hbqinfo_data(struct lpfc_hba *phba, char *buf, int size)
 
 	if (phba->sli_rev != 3)
 		return 0;
-	cnt = LPFC_HBQINFO_SIZE;
+
 	spin_lock_irq(&phba->hbalock);
 
 	/* toggle between multiple hbqs, if any */
diff --git a/drivers/scsi/lpfc/lpfc_disc.h b/drivers/scsi/lpfc/lpfc_disc.h
index 1a6fe524940d..6977027979be 100644
--- a/drivers/scsi/lpfc/lpfc_disc.h
+++ b/drivers/scsi/lpfc/lpfc_disc.h
@@ -78,7 +78,8 @@ struct lpfc_nodelist {
 	struct list_head nlp_listp;
 	struct lpfc_name nlp_portname;
 	struct lpfc_name nlp_nodename;
-	uint32_t         nlp_flag;		/* entry  flags */
+	uint32_t         nlp_flag;		/* entry flags */
+	uint32_t         nlp_add_flag;		/* additional flags */
 	uint32_t         nlp_DID;		/* FC D_ID of entry */
 	uint32_t         nlp_last_elscmd;	/* Last ELS cmd sent */
 	uint16_t         nlp_type;
@@ -157,6 +158,9 @@ struct lpfc_node_rrq {
 #define NLP_FIRSTBURST     0x40000000	/* Target supports FirstBurst */
 #define NLP_RPI_REGISTERED 0x80000000	/* nlp_rpi is valid */
 
+/* Defines for nlp_add_flag (uint32) */
+#define NLP_IN_DEV_LOSS  0x00000001	/* Dev Loss processing in progress */
+
 /* ndlp usage management macros */
 #define NLP_CHK_NODE_ACT(ndlp)		(((ndlp)->nlp_usg_map \
 						& NLP_USG_NODE_ACT_BIT) \
diff --git a/drivers/scsi/lpfc/lpfc_els.c b/drivers/scsi/lpfc/lpfc_els.c
index 7a5d81a65be8..4c25485aa934 100644
--- a/drivers/scsi/lpfc/lpfc_els.c
+++ b/drivers/scsi/lpfc/lpfc_els.c
@@ -1084,7 +1084,8 @@ stop_rr_fcf_flogi:
 	 * accessing it.
 	 */
 	prsp = list_get_first(&pcmd->list, struct lpfc_dmabuf, list);
-
+	if (!prsp)
+		goto out;
 	sp = prsp->virt + sizeof(uint32_t);
 
 	/* FLOGI completes successfully */
@@ -1828,7 +1829,7 @@ lpfc_cmpl_els_plogi(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 	IOCB_t *irsp;
 	struct lpfc_nodelist *ndlp;
 	struct lpfc_dmabuf *prsp;
-	int disc, rc, did, type;
+	int disc, rc;
 
 	/* we pass cmdiocb to state machine which needs rspiocb as well */
 	cmdiocb->context_un.rsp_iocb = rspiocb;
@@ -1873,10 +1874,6 @@ lpfc_cmpl_els_plogi(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 		goto out;
 	}
 
-	/* ndlp could be freed in DSM, save these values now */
-	type = ndlp->nlp_type;
-	did = ndlp->nlp_DID;
-
 	if (irsp->ulpStatus) {
 		/* Check for retry */
 		if (lpfc_els_retry(phba, cmdiocb, rspiocb)) {
@@ -2269,8 +2266,6 @@ lpfc_adisc_done(struct lpfc_vport *vport)
 void
 lpfc_more_adisc(struct lpfc_vport *vport)
 {
-	int sentadisc;
-
 	if (vport->num_disc_nodes)
 		vport->num_disc_nodes--;
 	/* Continue discovery with <num_disc_nodes> ADISCs to go */
@@ -2283,7 +2278,7 @@ lpfc_more_adisc(struct lpfc_vport *vport)
 	if (vport->fc_flag & FC_NLP_MORE) {
 		lpfc_set_disctmo(vport);
 		/* go thru NPR nodes and issue any remaining ELS ADISCs */
-		sentadisc = lpfc_els_disc_adisc(vport);
+		lpfc_els_disc_adisc(vport);
 	}
 	if (!vport->num_disc_nodes)
 		lpfc_adisc_done(vport);
@@ -3027,10 +3022,9 @@ lpfc_els_retry_delay_handler(struct lpfc_nodelist *ndlp)
 {
 	struct lpfc_vport *vport = ndlp->vport;
 	struct Scsi_Host  *shost = lpfc_shost_from_vport(vport);
-	uint32_t cmd, did, retry;
+	uint32_t cmd, retry;
 
 	spin_lock_irq(shost->host_lock);
-	did = ndlp->nlp_DID;
 	cmd = ndlp->nlp_last_elscmd;
 	ndlp->nlp_last_elscmd = 0;
 
@@ -5288,10 +5282,9 @@ lpfc_els_rcv_rnid(struct lpfc_vport *vport, struct lpfc_iocbq *cmdiocb,
 	IOCB_t *icmd;
 	RNID *rn;
 	struct ls_rjt stat;
-	uint32_t cmd, did;
+	uint32_t cmd;
 
 	icmd = &cmdiocb->iocb;
-	did = icmd->un.elsreq64.remoteID;
 	pcmd = (struct lpfc_dmabuf *) cmdiocb->context2;
 	lp = (uint32_t *) pcmd->virt;
 
@@ -6693,6 +6686,13 @@ lpfc_els_unsol_buffer(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 
 	phba->fc_stat.elsRcvFrame++;
 
+	/*
+	 * Do not process any unsolicited ELS commands
+	 * if the ndlp is in DEV_LOSS
+	 */
+	if (ndlp->nlp_add_flag & NLP_IN_DEV_LOSS)
+		goto dropit;
+
 	elsiocb->context1 = lpfc_nlp_get(ndlp);
 	elsiocb->vport = vport;
 
@@ -7514,6 +7514,8 @@ lpfc_cmpl_els_fdisc(struct lpfc_hba *phba, struct lpfc_iocbq *cmdiocb,
 	vport->fc_myDID = irsp->un.ulpWord[4] & Mask_DID;
 	lpfc_vport_set_state(vport, FC_VPORT_ACTIVE);
 	prsp = list_get_first(&pcmd->list, struct lpfc_dmabuf, list);
+	if (!prsp)
+		goto out;
 	sp = prsp->virt + sizeof(uint32_t);
 	fabric_param_changed = lpfc_check_clean_addr_bit(vport, sp);
 	memcpy(&vport->fabric_portname, &sp->portName,
@@ -8187,9 +8189,11 @@ lpfc_sli4_els_xri_aborted(struct lpfc_hba *phba,
 			list_del(&sglq_entry->list);
 			ndlp = sglq_entry->ndlp;
 			sglq_entry->ndlp = NULL;
+			spin_lock(&pring->ring_lock);
 			list_add_tail(&sglq_entry->list,
 				&phba->sli4_hba.lpfc_sgl_list);
 			sglq_entry->state = SGL_FREED;
+			spin_unlock(&pring->ring_lock);
 			spin_unlock(&phba->sli4_hba.abts_sgl_list_lock);
 			spin_unlock_irqrestore(&phba->hbalock, iflag);
 			lpfc_set_rrq_active(phba, ndlp,
@@ -8208,12 +8212,15 @@ lpfc_sli4_els_xri_aborted(struct lpfc_hba *phba,
 		spin_unlock_irqrestore(&phba->hbalock, iflag);
 		return;
 	}
+	spin_lock(&pring->ring_lock);
 	sglq_entry = __lpfc_get_active_sglq(phba, lxri);
 	if (!sglq_entry || (sglq_entry->sli4_xritag != xri)) {
+		spin_unlock(&pring->ring_lock);
 		spin_unlock_irqrestore(&phba->hbalock, iflag);
 		return;
 	}
 	sglq_entry->state = SGL_XRI_ABORTED;
+	spin_unlock(&pring->ring_lock);
 	spin_unlock_irqrestore(&phba->hbalock, iflag);
 	return;
 }
diff --git a/drivers/scsi/lpfc/lpfc_hbadisc.c b/drivers/scsi/lpfc/lpfc_hbadisc.c
index 2a17e31265b8..5452f1f4220e 100644
--- a/drivers/scsi/lpfc/lpfc_hbadisc.c
+++ b/drivers/scsi/lpfc/lpfc_hbadisc.c
@@ -150,9 +150,30 @@ lpfc_dev_loss_tmo_callbk(struct fc_rport *rport)
 
 		/* If the WWPN of the rport and ndlp don't match, ignore it */
 		if (rport->port_name != wwn_to_u64(ndlp->nlp_portname.u.wwn)) {
+			lpfc_printf_vlog(vport, KERN_ERR, LOG_NODE,
+				"6789 rport name %lx != node port name %lx",
+				(unsigned long)rport->port_name,
+				(unsigned long)wwn_to_u64(
+						ndlp->nlp_portname.u.wwn));
+			put_node = rdata->pnode != NULL;
+			put_rport = ndlp->rport != NULL;
+			rdata->pnode = NULL;
+			ndlp->rport = NULL;
+			if (put_node)
+				lpfc_nlp_put(ndlp);
 			put_device(&rport->dev);
 			return;
 		}
+
+		put_node = rdata->pnode != NULL;
+		put_rport = ndlp->rport != NULL;
+		rdata->pnode = NULL;
+		ndlp->rport = NULL;
+		if (put_node)
+			lpfc_nlp_put(ndlp);
+		if (put_rport)
+			put_device(&rport->dev);
+		return;
 	}
 
 	evtp = &ndlp->dev_loss_evt;
@@ -161,6 +182,7 @@ lpfc_dev_loss_tmo_callbk(struct fc_rport *rport)
 		return;
 
 	evtp->evt_arg1  = lpfc_nlp_get(ndlp);
+	ndlp->nlp_add_flag |= NLP_IN_DEV_LOSS;
 
 	spin_lock_irq(&phba->hbalock);
 	/* We need to hold the node by incrementing the reference
@@ -201,8 +223,10 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 
 	rport = ndlp->rport;
 
-	if (!rport)
+	if (!rport) {
+		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		return fcf_inuse;
+	}
 
 	rdata = rport->dd_data;
 	name = (uint8_t *) &ndlp->nlp_portname;
@@ -235,6 +259,7 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 		put_rport = ndlp->rport != NULL;
 		rdata->pnode = NULL;
 		ndlp->rport = NULL;
+		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		if (put_node)
 			lpfc_nlp_put(ndlp);
 		if (put_rport)
@@ -250,6 +275,7 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 				 *name, *(name+1), *(name+2), *(name+3),
 				 *(name+4), *(name+5), *(name+6), *(name+7),
 				 ndlp->nlp_DID);
+		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		return fcf_inuse;
 	}
 
@@ -259,6 +285,7 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 		put_rport = ndlp->rport != NULL;
 		rdata->pnode = NULL;
 		ndlp->rport = NULL;
+		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		if (put_node)
 			lpfc_nlp_put(ndlp);
 		if (put_rport)
@@ -269,6 +296,7 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 	if (ndlp->nlp_sid != NLP_NO_SID) {
 		warn_on = 1;
 		/* flush the target */
+		ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 		lpfc_sli_abort_iocb(vport, &phba->sli.ring[phba->sli.fcp_ring],
 				    ndlp->nlp_sid, 0, LPFC_CTX_TGT);
 	}
@@ -297,6 +325,7 @@ lpfc_dev_loss_tmo_handler(struct lpfc_nodelist *ndlp)
 	put_rport = ndlp->rport != NULL;
 	rdata->pnode = NULL;
 	ndlp->rport = NULL;
+	ndlp->nlp_add_flag &= ~NLP_IN_DEV_LOSS;
 	if (put_node)
 		lpfc_nlp_put(ndlp);
 	if (put_rport)
@@ -995,7 +1024,6 @@ lpfc_linkup(struct lpfc_hba *phba)
 	struct lpfc_vport **vports;
 	int i;
 
-	lpfc_cleanup_wt_rrqs(phba);
 	phba->link_state = LPFC_LINK_UP;
 
 	/* Unblock fabric iocbs if they are blocked */
@@ -2042,7 +2070,8 @@ lpfc_sli4_set_fcf_flogi_fail(struct lpfc_hba *phba, uint16_t fcf_index)
  * returns:
  * 0=success 1=failure
  **/
-int lpfc_sli4_fcf_pri_list_add(struct lpfc_hba *phba, uint16_t fcf_index,
+static int lpfc_sli4_fcf_pri_list_add(struct lpfc_hba *phba,
+	uint16_t fcf_index,
 	struct fcf_record *new_fcf_record)
 {
 	uint16_t current_fcf_pri;
@@ -2146,7 +2175,6 @@ lpfc_mbx_cmpl_fcf_scan_read_fcf_rec(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 	uint16_t fcf_index, next_fcf_index;
 	struct lpfc_fcf_rec *fcf_rec = NULL;
 	uint16_t vlan_id;
-	uint32_t seed;
 	bool select_new_fcf;
 	int rc;
 
@@ -2383,9 +2411,6 @@ lpfc_mbx_cmpl_fcf_scan_read_fcf_rec(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 		phba->fcf.fcf_flag |= FCF_AVAILABLE;
 		/* Setup initial running random FCF selection count */
 		phba->fcf.eligible_fcf_cnt = 1;
-		/* Seeding the random number generator for random selection */
-		seed = (uint32_t)(0xFFFFFFFF & jiffies);
-		prandom_seed(seed);
 	}
 	spin_unlock_irq(&phba->hbalock);
 	goto read_next_fcf;
@@ -2678,7 +2703,7 @@ out:
  *
  * This function handles completion of init vfi mailbox command.
  */
-void
+static void
 lpfc_init_vfi_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq)
 {
 	struct lpfc_vport *vport = mboxq->vport;
@@ -4438,7 +4463,7 @@ lpfc_no_rpi(struct lpfc_hba *phba, struct lpfc_nodelist *ndlp)
  * This function will issue an ELS LOGO command after completing
  * the UNREG_RPI.
  **/
-void
+static void
 lpfc_nlp_logo_unreg(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmb)
 {
 	struct lpfc_vport  *vport = pmb->vport;
@@ -5006,7 +5031,6 @@ lpfc_disc_start(struct lpfc_vport *vport)
 	struct lpfc_hba  *phba = vport->phba;
 	uint32_t num_sent;
 	uint32_t clear_la_pending;
-	int did_changed;
 
 	if (!lpfc_is_link_up(phba)) {
 		lpfc_printf_vlog(vport, KERN_INFO, LOG_SLI,
@@ -5025,11 +5049,6 @@ lpfc_disc_start(struct lpfc_vport *vport)
 
 	lpfc_set_disctmo(vport);
 
-	if (vport->fc_prevDID == vport->fc_myDID)
-		did_changed = 0;
-	else
-		did_changed = 1;
-
 	vport->fc_prevDID = vport->fc_myDID;
 	vport->num_disc_nodes = 0;
 
@@ -6318,7 +6337,7 @@ lpfc_parse_fcoe_conf(struct lpfc_hba *phba,
 		uint8_t *buff,
 		uint32_t size)
 {
-	uint32_t offset = 0, rec_length;
+	uint32_t offset = 0;
 	uint8_t *rec_ptr;
 
 	/*
@@ -6345,8 +6364,6 @@ lpfc_parse_fcoe_conf(struct lpfc_hba *phba,
 	}
 	offset += 4;
 
-	rec_length = buff[offset + 1];
-
 	/* Read FCoE param record */
 	rec_ptr = lpfc_get_rec_conf23(&buff[offset],
 			size - offset, FCOE_PARAM_TYPE);
diff --git a/drivers/scsi/lpfc/lpfc_init.c b/drivers/scsi/lpfc/lpfc_init.c
index a5769a9960ac..0b2c53af85c7 100644
--- a/drivers/scsi/lpfc/lpfc_init.c
+++ b/drivers/scsi/lpfc/lpfc_init.c
@@ -306,10 +306,10 @@ lpfc_dump_wakeup_param_cmpl(struct lpfc_hba *phba, LPFC_MBOXQ_t *pmboxq)
 		dist = dist_char[prg->dist];
 
 	if ((prg->dist == 3) && (prg->num == 0))
-		sprintf(phba->OptionROMVersion, "%d.%d%d",
+		snprintf(phba->OptionROMVersion, 32, "%d.%d%d",
 			prg->ver, prg->rev, prg->lev);
 	else
-		sprintf(phba->OptionROMVersion, "%d.%d%d%c%d",
+		snprintf(phba->OptionROMVersion, 32, "%d.%d%d%c%d",
 			prg->ver, prg->rev, prg->lev,
 			dist, prg->num);
 	mempool_free(pmboxq, phba->mbox_mem_pool);
@@ -649,7 +649,7 @@ lpfc_config_port_post(struct lpfc_hba *phba)
  *		0 - success
  *		Any other value - error
  **/
-int
+static int
 lpfc_hba_init_link(struct lpfc_hba *phba, uint32_t flag)
 {
 	return lpfc_hba_init_link_fc_topology(phba, phba->cfg_topology, flag);
@@ -750,7 +750,7 @@ lpfc_hba_init_link_fc_topology(struct lpfc_hba *phba, uint32_t fc_topology,
  *		0 - success
  *		Any other value - error
  **/
-int
+static int
 lpfc_hba_down_link(struct lpfc_hba *phba, uint32_t flag)
 {
 	LPFC_MBOXQ_t *pmb;
@@ -988,9 +988,12 @@ lpfc_hba_down_post_s4(struct lpfc_hba *phba)
 	LIST_HEAD(aborts);
 	unsigned long iflag = 0;
 	struct lpfc_sglq *sglq_entry = NULL;
+	struct lpfc_sli *psli = &phba->sli;
+	struct lpfc_sli_ring *pring;
 
 	lpfc_hba_free_post_buf(phba);
 	lpfc_hba_clean_txcmplq(phba);
+	pring = &psli->ring[LPFC_ELS_RING];
 
 	/* At this point in time the HBA is either reset or DOA. Either
 	 * way, nothing should be on lpfc_abts_els_sgl_list, it needs to be
@@ -1008,8 +1011,10 @@ lpfc_hba_down_post_s4(struct lpfc_hba *phba)
 		&phba->sli4_hba.lpfc_abts_els_sgl_list, list)
 		sglq_entry->state = SGL_FREED;
 
+	spin_lock(&pring->ring_lock);
 	list_splice_init(&phba->sli4_hba.lpfc_abts_els_sgl_list,
 			&phba->sli4_hba.lpfc_sgl_list);
+	spin_unlock(&pring->ring_lock);
 	spin_unlock(&phba->sli4_hba.abts_sgl_list_lock);
 	/* abts_scsi_buf_list_lock required because worker thread uses this
 	 * list.
@@ -3047,6 +3052,7 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 	LIST_HEAD(els_sgl_list);
 	LIST_HEAD(scsi_sgl_list);
 	int rc;
+	struct lpfc_sli_ring *pring = &phba->sli.ring[LPFC_ELS_RING];
 
 	/*
 	 * update on pci function's els xri-sgl list
@@ -3087,7 +3093,9 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 			list_add_tail(&sglq_entry->list, &els_sgl_list);
 		}
 		spin_lock_irq(&phba->hbalock);
+		spin_lock(&pring->ring_lock);
 		list_splice_init(&els_sgl_list, &phba->sli4_hba.lpfc_sgl_list);
+		spin_unlock(&pring->ring_lock);
 		spin_unlock_irq(&phba->hbalock);
 	} else if (els_xri_cnt < phba->sli4_hba.els_xri_cnt) {
 		/* els xri-sgl shrinked */
@@ -3097,7 +3105,9 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 				"%d to %d\n", phba->sli4_hba.els_xri_cnt,
 				els_xri_cnt);
 		spin_lock_irq(&phba->hbalock);
+		spin_lock(&pring->ring_lock);
 		list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &els_sgl_list);
+		spin_unlock(&pring->ring_lock);
 		spin_unlock_irq(&phba->hbalock);
 		/* release extra els sgls from list */
 		for (i = 0; i < xri_cnt; i++) {
@@ -3110,7 +3120,9 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 			}
 		}
 		spin_lock_irq(&phba->hbalock);
+		spin_lock(&pring->ring_lock);
 		list_splice_init(&els_sgl_list, &phba->sli4_hba.lpfc_sgl_list);
+		spin_unlock(&pring->ring_lock);
 		spin_unlock_irq(&phba->hbalock);
 	} else
 		lpfc_printf_log(phba, KERN_INFO, LOG_SLI,
@@ -3165,9 +3177,11 @@ lpfc_sli4_xri_sgl_update(struct lpfc_hba *phba)
 		for (i = 0; i < scsi_xri_cnt; i++) {
 			list_remove_head(&scsi_sgl_list, psb,
 					 struct lpfc_scsi_buf, list);
-			pci_pool_free(phba->lpfc_scsi_dma_buf_pool, psb->data,
-				      psb->dma_handle);
-			kfree(psb);
+			if (psb) {
+				pci_pool_free(phba->lpfc_scsi_dma_buf_pool,
+					      psb->data, psb->dma_handle);
+				kfree(psb);
+			}
 		}
 		spin_lock_irq(&phba->scsi_buf_list_get_lock);
 		phba->sli4_hba.scsi_xri_cnt -= scsi_xri_cnt;
@@ -3550,7 +3564,7 @@ lpfc_fcf_redisc_wait_start_timer(struct lpfc_hba *phba)
  * list, and then worker thread shall be waked up for processing from the
  * worker thread context.
  **/
-void
+static void
 lpfc_sli4_fcf_redisc_wait_tmo(unsigned long ptr)
 {
 	struct lpfc_hba *phba = (struct lpfc_hba *)ptr;
@@ -5680,10 +5694,13 @@ static void
 lpfc_free_els_sgl_list(struct lpfc_hba *phba)
 {
 	LIST_HEAD(sglq_list);
+	struct lpfc_sli_ring *pring = &phba->sli.ring[LPFC_ELS_RING];
 
 	/* Retrieve all els sgls from driver list */
 	spin_lock_irq(&phba->hbalock);
+	spin_lock(&pring->ring_lock);
 	list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &sglq_list);
+	spin_unlock(&pring->ring_lock);
 	spin_unlock_irq(&phba->hbalock);
 
 	/* Now free the sgl list */
@@ -5848,16 +5865,14 @@ lpfc_sli4_create_rpi_hdr(struct lpfc_hba *phba)
 	if (!dmabuf)
 		return NULL;
 
-	dmabuf->virt = dma_alloc_coherent(&phba->pcidev->dev,
-					  LPFC_HDR_TEMPLATE_SIZE,
-					  &dmabuf->phys,
-					  GFP_KERNEL);
+	dmabuf->virt = dma_zalloc_coherent(&phba->pcidev->dev,
+					   LPFC_HDR_TEMPLATE_SIZE,
+					   &dmabuf->phys, GFP_KERNEL);
 	if (!dmabuf->virt) {
 		rpi_hdr = NULL;
 		goto err_free_dmabuf;
 	}
 
-	memset(dmabuf->virt, 0, LPFC_HDR_TEMPLATE_SIZE);
 	if (!IS_ALIGNED(dmabuf->phys, LPFC_HDR_TEMPLATE_SIZE)) {
 		rpi_hdr = NULL;
 		goto err_free_coherent;
@@ -6246,14 +6261,11 @@ lpfc_sli_pci_mem_setup(struct lpfc_hba *phba)
 	}
 
 	/* Allocate memory for SLI-2 structures */
-	phba->slim2p.virt = dma_alloc_coherent(&pdev->dev,
-					       SLI2_SLIM_SIZE,
-					       &phba->slim2p.phys,
-					       GFP_KERNEL);
+	phba->slim2p.virt = dma_zalloc_coherent(&pdev->dev, SLI2_SLIM_SIZE,
+						&phba->slim2p.phys, GFP_KERNEL);
 	if (!phba->slim2p.virt)
 		goto out_iounmap;
 
-	memset(phba->slim2p.virt, 0, SLI2_SLIM_SIZE);
 	phba->mbox = phba->slim2p.virt + offsetof(struct lpfc_sli2_slim, mbx);
 	phba->mbox_ext = (phba->slim2p.virt +
 		offsetof(struct lpfc_sli2_slim, mbx_ext_words));
@@ -6618,15 +6630,12 @@ lpfc_create_bootstrap_mbox(struct lpfc_hba *phba)
 	 * plus an alignment restriction of 16 bytes.
 	 */
 	bmbx_size = sizeof(struct lpfc_bmbx_create) + (LPFC_ALIGN_16_BYTE - 1);
-	dmabuf->virt = dma_alloc_coherent(&phba->pcidev->dev,
-					  bmbx_size,
-					  &dmabuf->phys,
-					  GFP_KERNEL);
+	dmabuf->virt = dma_zalloc_coherent(&phba->pcidev->dev, bmbx_size,
+					   &dmabuf->phys, GFP_KERNEL);
 	if (!dmabuf->virt) {
 		kfree(dmabuf);
 		return -ENOMEM;
 	}
-	memset(dmabuf->virt, 0, bmbx_size);
 
 	/*
 	 * Initialize the bootstrap mailbox pointers now so that the register
@@ -6710,7 +6719,6 @@ lpfc_sli4_read_config(struct lpfc_hba *phba)
 	struct lpfc_mbx_get_func_cfg *get_func_cfg;
 	struct lpfc_rsrc_desc_fcfcoe *desc;
 	char *pdesc_0;
-	uint32_t desc_count;
 	int length, i, rc = 0, rc2;
 
 	pmb = (LPFC_MBOXQ_t *) mempool_alloc(phba->mbox_mem_pool, GFP_KERNEL);
@@ -6841,7 +6849,6 @@ lpfc_sli4_read_config(struct lpfc_hba *phba)
 
 	/* search for fc_fcoe resrouce descriptor */
 	get_func_cfg = &pmb->u.mqe.un.get_func_cfg;
-	desc_count = get_func_cfg->func_cfg.rsrc_desc_count;
 
 	pdesc_0 = (char *)&get_func_cfg->func_cfg.desc[0];
 	desc = (struct lpfc_rsrc_desc_fcfcoe *)pdesc_0;
@@ -7417,7 +7424,8 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 		if (rc) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"0523 Failed setup of fast-path EQ "
-					"(%d), rc = 0x%x\n", fcp_eqidx, rc);
+					"(%d), rc = 0x%x\n", fcp_eqidx,
+					(uint32_t)rc);
 			goto out_destroy_hba_eq;
 		}
 		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
@@ -7448,7 +7456,8 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 		if (rc) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"0527 Failed setup of fast-path FCP "
-					"CQ (%d), rc = 0x%x\n", fcp_cqidx, rc);
+					"CQ (%d), rc = 0x%x\n", fcp_cqidx,
+					(uint32_t)rc);
 			goto out_destroy_fcp_cq;
 		}
 
@@ -7488,7 +7497,8 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 		if (rc) {
 			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 					"0535 Failed setup of fast-path FCP "
-					"WQ (%d), rc = 0x%x\n", fcp_wqidx, rc);
+					"WQ (%d), rc = 0x%x\n", fcp_wqidx,
+					(uint32_t)rc);
 			goto out_destroy_fcp_wq;
 		}
 
@@ -7521,7 +7531,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (rc) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0529 Failed setup of slow-path mailbox CQ: "
-				"rc = 0x%x\n", rc);
+				"rc = 0x%x\n", (uint32_t)rc);
 		goto out_destroy_fcp_wq;
 	}
 	lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
@@ -7541,7 +7551,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (rc) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0531 Failed setup of slow-path ELS CQ: "
-				"rc = 0x%x\n", rc);
+				"rc = 0x%x\n", (uint32_t)rc);
 		goto out_destroy_mbx_cq;
 	}
 	lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
@@ -7585,7 +7595,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (rc) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0537 Failed setup of slow-path ELS WQ: "
-				"rc = 0x%x\n", rc);
+				"rc = 0x%x\n", (uint32_t)rc);
 		goto out_destroy_mbx_wq;
 	}
 
@@ -7617,7 +7627,7 @@ lpfc_sli4_queue_setup(struct lpfc_hba *phba)
 	if (rc) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"0541 Failed setup of Receive Queue: "
-				"rc = 0x%x\n", rc);
+				"rc = 0x%x\n", (uint32_t)rc);
 		goto out_destroy_fcp_wq;
 	}
 
@@ -7896,7 +7906,8 @@ lpfc_pci_function_reset(struct lpfc_hba *phba)
 	LPFC_MBOXQ_t *mboxq;
 	uint32_t rc = 0, if_type;
 	uint32_t shdr_status, shdr_add_status;
-	uint32_t rdy_chk, num_resets = 0, reset_again = 0;
+	uint32_t rdy_chk;
+	uint32_t port_reset = 0;
 	union lpfc_sli4_cfg_shdr *shdr;
 	struct lpfc_register reg_data;
 	uint16_t devid;
@@ -7936,9 +7947,42 @@ lpfc_pci_function_reset(struct lpfc_hba *phba)
 		}
 		break;
 	case LPFC_SLI_INTF_IF_TYPE_2:
-		for (num_resets = 0;
-		     num_resets < MAX_IF_TYPE_2_RESETS;
-		     num_resets++) {
+wait:
+		/*
+		 * Poll the Port Status Register and wait for RDY for
+		 * up to 30 seconds. If the port doesn't respond, treat
+		 * it as an error.
+		 */
+		for (rdy_chk = 0; rdy_chk < 3000; rdy_chk++) {
+			if (lpfc_readl(phba->sli4_hba.u.if_type2.
+				STATUSregaddr, &reg_data.word0)) {
+				rc = -ENODEV;
+				goto out;
+			}
+			if (bf_get(lpfc_sliport_status_rdy, &reg_data))
+				break;
+			msleep(20);
+		}
+
+		if (!bf_get(lpfc_sliport_status_rdy, &reg_data)) {
+			phba->work_status[0] = readl(
+				phba->sli4_hba.u.if_type2.ERR1regaddr);
+			phba->work_status[1] = readl(
+				phba->sli4_hba.u.if_type2.ERR2regaddr);
+			lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
+					"2890 Port not ready, port status reg "
+					"0x%x error 1=0x%x, error 2=0x%x\n",
+					reg_data.word0,
+					phba->work_status[0],
+					phba->work_status[1]);
+			rc = -ENODEV;
+			goto out;
+		}
+
+		if (!port_reset) {
+			/*
+			 * Reset the port now
+			 */
 			reg_data.word0 = 0;
 			bf_set(lpfc_sliport_ctrl_end, &reg_data,
 			       LPFC_SLIPORT_LITTLE_ENDIAN);
@@ -7949,64 +7993,16 @@ lpfc_pci_function_reset(struct lpfc_hba *phba)
 			/* flush */
 			pci_read_config_word(phba->pcidev,
 					     PCI_DEVICE_ID, &devid);
-			/*
-			 * Poll the Port Status Register and wait for RDY for
-			 * up to 10 seconds.  If the port doesn't respond, treat
-			 * it as an error.  If the port responds with RN, start
-			 * the loop again.
-			 */
-			for (rdy_chk = 0; rdy_chk < 1000; rdy_chk++) {
-				msleep(10);
-				if (lpfc_readl(phba->sli4_hba.u.if_type2.
-					      STATUSregaddr, &reg_data.word0)) {
-					rc = -ENODEV;
-					goto out;
-				}
-				if (bf_get(lpfc_sliport_status_rn, &reg_data))
-					reset_again++;
-				if (bf_get(lpfc_sliport_status_rdy, &reg_data))
-					break;
-			}
-
-			/*
-			 * If the port responds to the init request with
-			 * reset needed, delay for a bit and restart the loop.
-			 */
-			if (reset_again && (rdy_chk < 1000)) {
-				msleep(10);
-				reset_again = 0;
-				continue;
-			}
 
-			/* Detect any port errors. */
-			if ((bf_get(lpfc_sliport_status_err, &reg_data)) ||
-			    (rdy_chk >= 1000)) {
-				phba->work_status[0] = readl(
-					phba->sli4_hba.u.if_type2.ERR1regaddr);
-				phba->work_status[1] = readl(
-					phba->sli4_hba.u.if_type2.ERR2regaddr);
-				lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
-					"2890 Port error detected during port "
-					"reset(%d): wait_tmo:%d ms, "
-					"port status reg 0x%x, "
-					"error 1=0x%x, error 2=0x%x\n",
-					num_resets, rdy_chk*10,
-					reg_data.word0,
-					phba->work_status[0],
-					phba->work_status[1]);
-				rc = -ENODEV;
-			}
-
-			/*
-			 * Terminate the outer loop provided the Port indicated
-			 * ready within 10 seconds.
-			 */
-			if (rdy_chk < 1000)
-				break;
+			port_reset = 1;
+			msleep(20);
+			goto wait;
+		} else if (bf_get(lpfc_sliport_status_rn, &reg_data)) {
+			rc = -ENODEV;
+			goto out;
 		}
-		/* delay driver action following IF_TYPE_2 function reset */
-		msleep(100);
 		break;
+
 	case LPFC_SLI_INTF_IF_TYPE_1:
 	default:
 		break;
@@ -8014,11 +8010,10 @@ lpfc_pci_function_reset(struct lpfc_hba *phba)
 
 out:
 	/* Catch the not-ready port failure after a port reset. */
-	if (num_resets >= MAX_IF_TYPE_2_RESETS) {
+	if (rc) {
 		lpfc_printf_log(phba, KERN_ERR, LOG_INIT,
 				"3317 HBA not functional: IP Reset Failed "
-				"after (%d) retries, try: "
-				"echo fw_reset > board_mode\n", num_resets);
+				"try: echo fw_reset > board_mode\n");
 		rc = -ENODEV;
 	}
 
@@ -8211,9 +8206,9 @@ lpfc_sli4_pci_mem_unset(struct lpfc_hba *phba)
  * @phba: pointer to lpfc hba data structure.
  *
  * This routine is invoked to enable the MSI-X interrupt vectors to device
- * with SLI-3 interface specs. The kernel function pci_enable_msix() is
- * called to enable the MSI-X vectors. Note that pci_enable_msix(), once
- * invoked, enables either all or nothing, depending on the current
+ * with SLI-3 interface specs. The kernel function pci_enable_msix_exact()
+ * is called to enable the MSI-X vectors. Note that pci_enable_msix_exact(),
+ * once invoked, enables either all or nothing, depending on the current
  * availability of PCI vector resources. The device driver is responsible
  * for calling the individual request_irq() to register each MSI-X vector
  * with a interrupt handler, which is done in this function. Note that
@@ -8237,8 +8232,8 @@ lpfc_sli_enable_msix(struct lpfc_hba *phba)
 		phba->msix_entries[i].entry = i;
 
 	/* Configure MSI-X capability structure */
-	rc = pci_enable_msix(phba->pcidev, phba->msix_entries,
-				ARRAY_SIZE(phba->msix_entries));
+	rc = pci_enable_msix_exact(phba->pcidev, phba->msix_entries,
+				   LPFC_MSIX_VECTORS);
 	if (rc) {
 		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
 				"0420 PCI enable MSI-X failed (%d)\n", rc);
@@ -8775,16 +8770,14 @@ out:
  * @phba: pointer to lpfc hba data structure.
  *
  * This routine is invoked to enable the MSI-X interrupt vectors to device
- * with SLI-4 interface spec. The kernel function pci_enable_msix() is called
- * to enable the MSI-X vectors. Note that pci_enable_msix(), once invoked,
- * enables either all or nothing, depending on the current availability of
- * PCI vector resources. The device driver is responsible for calling the
- * individual request_irq() to register each MSI-X vector with a interrupt
- * handler, which is done in this function. Note that later when device is
- * unloading, the driver should always call free_irq() on all MSI-X vectors
- * it has done request_irq() on before calling pci_disable_msix(). Failure
- * to do so results in a BUG_ON() and a device will be left with MSI-X
- * enabled and leaks its vectors.
+ * with SLI-4 interface spec. The kernel function pci_enable_msix_range()
+ * is called to enable the MSI-X vectors. The device driver is responsible
+ * for calling the individual request_irq() to register each MSI-X vector
+ * with a interrupt handler, which is done in this function. Note that
+ * later when device is unloading, the driver should always call free_irq()
+ * on all MSI-X vectors it has done request_irq() on before calling
+ * pci_disable_msix(). Failure to do so results in a BUG_ON() and a device
+ * will be left with MSI-X enabled and leaks its vectors.
  *
  * Return codes
  * 0 - successful
@@ -8805,17 +8798,14 @@ lpfc_sli4_enable_msix(struct lpfc_hba *phba)
 		phba->sli4_hba.msix_entries[index].entry = index;
 		vectors++;
 	}
-enable_msix_vectors:
-	rc = pci_enable_msix(phba->pcidev, phba->sli4_hba.msix_entries,
-			     vectors);
-	if (rc > 1) {
-		vectors = rc;
-		goto enable_msix_vectors;
-	} else if (rc) {
+	rc = pci_enable_msix_range(phba->pcidev, phba->sli4_hba.msix_entries,
+				   2, vectors);
+	if (rc < 0) {
 		lpfc_printf_log(phba, KERN_INFO, LOG_INIT,
 				"0484 PCI enable MSI-X failed (%d)\n", rc);
 		goto vec_fail_out;
 	}
+	vectors = rc;
 
 	/* Log MSI-X vector assignment */
 	for (index = 0; index < vectors; index++)
@@ -8828,7 +8818,8 @@ enable_msix_vectors:
 	/* Assign MSI-X vectors to interrupt handlers */
 	for (index = 0; index < vectors; index++) {
 		memset(&phba->sli4_hba.handler_name[index], 0, 16);
-		sprintf((char *)&phba->sli4_hba.handler_name[index],
+		snprintf((char *)&phba->sli4_hba.handler_name[index],
+			 LPFC_SLI4_HANDLER_NAME_SZ,
 			 LPFC_DRIVER_HANDLER_NAME"%d", index);
 
 		phba->sli4_hba.fcp_eq_hdl[index].idx = index;
diff --git a/drivers/scsi/lpfc/lpfc_mbox.c b/drivers/scsi/lpfc/lpfc_mbox.c
index 1f292e29d566..06241f590c1e 100644
--- a/drivers/scsi/lpfc/lpfc_mbox.c
+++ b/drivers/scsi/lpfc/lpfc_mbox.c
@@ -1811,12 +1811,12 @@ lpfc_sli4_config(struct lpfc_hba *phba, struct lpfcMboxq *mbox,
 		 * page, this is used as a priori size of SLI4_PAGE_SIZE for
 		 * the later DMA memory free.
 		 */
-		viraddr = dma_alloc_coherent(&phba->pcidev->dev, SLI4_PAGE_SIZE,
-					     &phyaddr, GFP_KERNEL);
+		viraddr = dma_zalloc_coherent(&phba->pcidev->dev,
+					      SLI4_PAGE_SIZE, &phyaddr,
+					      GFP_KERNEL);
 		/* In case of malloc fails, proceed with whatever we have */
 		if (!viraddr)
 			break;
-		memset(viraddr, 0, SLI4_PAGE_SIZE);
 		mbox->sge_array->addr[pagen] = viraddr;
 		/* Keep the first page for later sub-header construction */
 		if (pagen == 0)
diff --git a/drivers/scsi/lpfc/lpfc_nportdisc.c b/drivers/scsi/lpfc/lpfc_nportdisc.c
index c342f6afd747..5cc1103d811e 100644
--- a/drivers/scsi/lpfc/lpfc_nportdisc.c
+++ b/drivers/scsi/lpfc/lpfc_nportdisc.c
@@ -1031,6 +1031,8 @@ lpfc_cmpl_plogi_plogi_issue(struct lpfc_vport *vport,
 	pcmd = (struct lpfc_dmabuf *) cmdiocb->context2;
 
 	prsp = list_get_first(&pcmd->list, struct lpfc_dmabuf, list);
+	if (!prsp)
+		goto out;
 
 	lp = (uint32_t *) prsp->virt;
 	sp = (struct serv_parm *) ((uint8_t *) lp + sizeof (uint32_t));
diff --git a/drivers/scsi/lpfc/lpfc_scsi.c b/drivers/scsi/lpfc/lpfc_scsi.c
index 7862c5540861..b99399fe2548 100644
--- a/drivers/scsi/lpfc/lpfc_scsi.c
+++ b/drivers/scsi/lpfc/lpfc_scsi.c
@@ -306,7 +306,7 @@ lpfc_send_sdev_queuedepth_change_event(struct lpfc_hba *phba,
  * depth for a scsi device. This function sets the queue depth to the new
  * value and sends an event out to log the queue depth change.
  **/
-int
+static int
 lpfc_change_queue_depth(struct scsi_device *sdev, int qdepth, int reason)
 {
 	struct lpfc_vport *vport = (struct lpfc_vport *) sdev->host->hostdata;
@@ -380,12 +380,14 @@ lpfc_rampdown_queue_depth(struct lpfc_hba *phba)
 {
 	unsigned long flags;
 	uint32_t evt_posted;
+	unsigned long expires;
 
 	spin_lock_irqsave(&phba->hbalock, flags);
 	atomic_inc(&phba->num_rsrc_err);
 	phba->last_rsrc_error_time = jiffies;
 
-	if ((phba->last_ramp_down_time + QUEUE_RAMP_DOWN_INTERVAL) > jiffies) {
+	expires = phba->last_ramp_down_time + QUEUE_RAMP_DOWN_INTERVAL;
+	if (time_after(expires, jiffies)) {
 		spin_unlock_irqrestore(&phba->hbalock, flags);
 		return;
 	}
@@ -741,7 +743,7 @@ lpfc_sli4_fcp_xri_aborted(struct lpfc_hba *phba,
  *
  * Returns: 0 = failure, non-zero number of successfully posted buffers.
  **/
-int
+static int
 lpfc_sli4_post_scsi_sgl_list(struct lpfc_hba *phba,
 			     struct list_head *post_sblist, int sb_count)
 {
@@ -2965,7 +2967,7 @@ err:
  * on the specified data using a CRC algorithmn
  * using crc_t10dif.
  */
-uint16_t
+static uint16_t
 lpfc_bg_crc(uint8_t *data, int count)
 {
 	uint16_t crc = 0;
@@ -2981,7 +2983,7 @@ lpfc_bg_crc(uint8_t *data, int count)
  * on the specified data using a CSUM algorithmn
  * using ip_compute_csum.
  */
-uint16_t
+static uint16_t
 lpfc_bg_csum(uint8_t *data, int count)
 {
 	uint16_t ret;
@@ -2994,7 +2996,7 @@ lpfc_bg_csum(uint8_t *data, int count)
  * This function examines the protection data to try to determine
  * what type of T10-DIF error occurred.
  */
-void
+static void
 lpfc_calc_bg_err(struct lpfc_hba *phba, struct lpfc_scsi_buf *lpfc_cmd)
 {
 	struct scatterlist *sgpe; /* s/g prot entry */
@@ -3464,7 +3466,7 @@ lpfc_scsi_prep_dma_buf_s4(struct lpfc_hba *phba, struct lpfc_scsi_buf *lpfc_cmd)
 	 */
 	if ((phba->cfg_fof) && ((struct lpfc_device_data *)
 		scsi_cmnd->device->hostdata)->oas_enabled)
-		lpfc_cmd->cur_iocbq.iocb_flag |= LPFC_IO_OAS;
+		lpfc_cmd->cur_iocbq.iocb_flag |= (LPFC_IO_OAS | LPFC_IO_FOF);
 	return 0;
 }
 
@@ -3604,6 +3606,14 @@ lpfc_bg_scsi_prep_dma_buf_s4(struct lpfc_hba *phba,
 	 */
 	iocb_cmd->un.fcpi.fcpi_parm = fcpdl;
 
+	/*
+	 * If the OAS driver feature is enabled and the lun is enabled for
+	 * OAS, set the oas iocb related flags.
+	 */
+	if ((phba->cfg_fof) && ((struct lpfc_device_data *)
+		scsi_cmnd->device->hostdata)->oas_enabled)
+		lpfc_cmd->cur_iocbq.iocb_flag |= (LPFC_IO_OAS | LPFC_IO_FOF);
+
 	return 0;
 err:
 	if (lpfc_cmd->seg_cnt)
@@ -4874,6 +4884,8 @@ lpfc_abort_handler(struct scsi_cmnd *cmnd)
 	/* ABTS WQE must go to the same WQ as the WQE to be aborted */
 	abtsiocb->fcp_wqidx = iocb->fcp_wqidx;
 	abtsiocb->iocb_flag |= LPFC_USE_FCPWQIDX;
+	if (iocb->iocb_flag & LPFC_IO_FOF)
+		abtsiocb->iocb_flag |= LPFC_IO_FOF;
 
 	if (lpfc_is_link_up(phba))
 		icmd->ulpCommand = CMD_ABORT_XRI_CN;
@@ -5327,7 +5339,13 @@ lpfc_target_reset_handler(struct scsi_cmnd *cmnd)
 	if (status == FAILED) {
 		lpfc_printf_vlog(vport, KERN_ERR, LOG_FCP,
 			"0722 Target Reset rport failure: rdata x%p\n", rdata);
-		return FAILED;
+		spin_lock_irq(shost->host_lock);
+		pnode->nlp_flag &= ~NLP_NPR_ADISC;
+		pnode->nlp_fcp_info &= ~NLP_FCP_2_DEVICE;
+		spin_unlock_irq(shost->host_lock);
+		lpfc_reset_flush_io_context(vport, tgt_id, lun_id,
+					  LPFC_CTX_TGT);
+		return FAST_IO_FAIL;
 	}
 
 	scsi_event.event_type = FC_REG_SCSI_EVENT;
diff --git a/drivers/scsi/lpfc/lpfc_sli.c b/drivers/scsi/lpfc/lpfc_sli.c
index 32ada0505576..207a43d952fa 100644
--- a/drivers/scsi/lpfc/lpfc_sli.c
+++ b/drivers/scsi/lpfc/lpfc_sli.c
@@ -187,7 +187,6 @@ lpfc_sli4_mq_put(struct lpfc_queue *q, struct lpfc_mqe *mqe)
 {
 	struct lpfc_mqe *temp_mqe;
 	struct lpfc_register doorbell;
-	uint32_t host_index;
 
 	/* sanity check on queue memory */
 	if (unlikely(!q))
@@ -202,7 +201,6 @@ lpfc_sli4_mq_put(struct lpfc_queue *q, struct lpfc_mqe *mqe)
 	q->phba->mbox = (MAILBOX_t *)temp_mqe;
 
 	/* Update the host index before invoking device */
-	host_index = q->host_index;
 	q->host_index = ((q->host_index + 1) % q->entry_count);
 
 	/* Ring Doorbell */
@@ -786,42 +784,6 @@ lpfc_cleanup_vports_rrqs(struct lpfc_vport *vport, struct lpfc_nodelist *ndlp)
 }
 
 /**
- * lpfc_cleanup_wt_rrqs - Remove all rrq's from the active list.
- * @phba: Pointer to HBA context object.
- *
- * Remove all rrqs from the phba->active_rrq_list and free them by
- * calling __lpfc_clr_active_rrq
- *
- **/
-void
-lpfc_cleanup_wt_rrqs(struct lpfc_hba *phba)
-{
-	struct lpfc_node_rrq *rrq;
-	struct lpfc_node_rrq *nextrrq;
-	unsigned long next_time;
-	unsigned long iflags;
-	LIST_HEAD(rrq_list);
-
-	if (phba->sli_rev != LPFC_SLI_REV4)
-		return;
-	spin_lock_irqsave(&phba->hbalock, iflags);
-	phba->hba_flag &= ~HBA_RRQ_ACTIVE;
-	next_time = jiffies + msecs_to_jiffies(1000 * (phba->fc_ratov * 2));
-	list_splice_init(&phba->active_rrq_list, &rrq_list);
-	spin_unlock_irqrestore(&phba->hbalock, iflags);
-
-	list_for_each_entry_safe(rrq, nextrrq, &rrq_list, list) {
-		list_del(&rrq->list);
-		lpfc_clr_rrq_active(phba, rrq->xritag, rrq);
-	}
-	if ((!list_empty(&phba->active_rrq_list)) &&
-	    (!(phba->pport->load_flag & FC_UNLOADING)))
-
-		mod_timer(&phba->rrq_tmr, next_time);
-}
-
-
-/**
  * lpfc_test_rrq_active - Test RRQ bit in xri_bitmap.
  * @phba: Pointer to HBA context object.
  * @ndlp: Targets nodelist pointer for this exchange.
@@ -937,7 +899,7 @@ out:
  * @phba: Pointer to HBA context object.
  * @piocb: Pointer to the iocbq.
  *
- * This function is called with hbalock held. This function
+ * This function is called with the ring lock held. This function
  * gets a new driver sglq object from the sglq list. If the
  * list is not empty then it is successful, it returns pointer to the newly
  * allocated sglq object else it returns NULL.
@@ -1053,10 +1015,12 @@ __lpfc_sli_release_iocbq_s4(struct lpfc_hba *phba, struct lpfc_iocbq *iocbq)
 			spin_unlock_irqrestore(
 				&phba->sli4_hba.abts_sgl_list_lock, iflag);
 		} else {
+			spin_lock_irqsave(&pring->ring_lock, iflag);
 			sglq->state = SGL_FREED;
 			sglq->ndlp = NULL;
 			list_add_tail(&sglq->list,
 				&phba->sli4_hba.lpfc_sgl_list);
+			spin_unlock_irqrestore(&pring->ring_lock, iflag);
 
 			/* Check if TXQ queue needs to be serviced */
 			if (!list_empty(&pring->txq))
@@ -2469,11 +2433,9 @@ lpfc_sli_process_unsol_iocb(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	IOCB_t           * irsp;
 	WORD5            * w5p;
 	uint32_t           Rctl, Type;
-	uint32_t           match;
 	struct lpfc_iocbq *iocbq;
 	struct lpfc_dmabuf *dmzbuf;
 
-	match = 0;
 	irsp = &(saveq->iocb);
 
 	if (irsp->ulpCommand == CMD_ASYNC_STATUS) {
@@ -2899,7 +2861,7 @@ lpfc_sli_rsp_pointers_error(struct lpfc_hba *phba, struct lpfc_sli_ring *pring)
 void lpfc_poll_eratt(unsigned long ptr)
 {
 	struct lpfc_hba *phba;
-	uint32_t eratt = 0, rem;
+	uint32_t eratt = 0;
 	uint64_t sli_intr, cnt;
 
 	phba = (struct lpfc_hba *)ptr;
@@ -2914,7 +2876,7 @@ void lpfc_poll_eratt(unsigned long ptr)
 		cnt = (sli_intr - phba->sli.slistat.sli_prev_intr);
 
 	/* 64-bit integer division not supporte on 32-bit x86 - use do_div */
-	rem = do_div(cnt, LPFC_ERATT_POLL_INTERVAL);
+	do_div(cnt, LPFC_ERATT_POLL_INTERVAL);
 	phba->sli.slistat.sli_ips = cnt;
 
 	phba->sli.slistat.sli_prev_intr = sli_intr;
@@ -4864,15 +4826,12 @@ lpfc_sli4_read_rev(struct lpfc_hba *phba, LPFC_MBOXQ_t *mboxq,
 	 * mailbox command.
 	 */
 	dma_size = *vpd_size;
-	dmabuf->virt = dma_alloc_coherent(&phba->pcidev->dev,
-					  dma_size,
-					  &dmabuf->phys,
-					  GFP_KERNEL);
+	dmabuf->virt = dma_zalloc_coherent(&phba->pcidev->dev, dma_size,
+					   &dmabuf->phys, GFP_KERNEL);
 	if (!dmabuf->virt) {
 		kfree(dmabuf);
 		return -ENOMEM;
 	}
-	memset(dmabuf->virt, 0, dma_size);
 
 	/*
 	 * The SLI4 implementation of READ_REV conflicts at word1,
@@ -5990,9 +5949,6 @@ lpfc_sli4_get_allocated_extnts(struct lpfc_hba *phba, uint16_t type,
 		curr_blks++;
 	}
 
-	/* Calculate the total requested length of the dma memory. */
-	req_len = curr_blks * sizeof(uint16_t);
-
 	/*
 	 * Calculate the size of an embedded mailbox.  The uint32_t
 	 * accounts for extents-specific word.
@@ -6101,14 +6057,18 @@ lpfc_sli4_repost_els_sgl_list(struct lpfc_hba *phba)
 	struct lpfc_sglq *sglq_entry_first = NULL;
 	int status, total_cnt, post_cnt = 0, num_posted = 0, block_cnt = 0;
 	int last_xritag = NO_XRI;
+	struct lpfc_sli_ring *pring;
 	LIST_HEAD(prep_sgl_list);
 	LIST_HEAD(blck_sgl_list);
 	LIST_HEAD(allc_sgl_list);
 	LIST_HEAD(post_sgl_list);
 	LIST_HEAD(free_sgl_list);
 
+	pring = &phba->sli.ring[LPFC_ELS_RING];
 	spin_lock_irq(&phba->hbalock);
+	spin_lock(&pring->ring_lock);
 	list_splice_init(&phba->sli4_hba.lpfc_sgl_list, &allc_sgl_list);
+	spin_unlock(&pring->ring_lock);
 	spin_unlock_irq(&phba->hbalock);
 
 	total_cnt = phba->sli4_hba.els_xri_cnt;
@@ -6210,8 +6170,10 @@ lpfc_sli4_repost_els_sgl_list(struct lpfc_hba *phba)
 	/* push els sgls posted to the availble list */
 	if (!list_empty(&post_sgl_list)) {
 		spin_lock_irq(&phba->hbalock);
+		spin_lock(&pring->ring_lock);
 		list_splice_init(&post_sgl_list,
 				 &phba->sli4_hba.lpfc_sgl_list);
+		spin_unlock(&pring->ring_lock);
 		spin_unlock_irq(&phba->hbalock);
 	} else {
 		lpfc_printf_log(phba, KERN_ERR, LOG_SLI,
@@ -6797,13 +6759,16 @@ void
 lpfc_mbox_timeout_handler(struct lpfc_hba *phba)
 {
 	LPFC_MBOXQ_t *pmbox = phba->sli.mbox_active;
-	MAILBOX_t *mb = &pmbox->u.mb;
+	MAILBOX_t *mb = NULL;
+
 	struct lpfc_sli *psli = &phba->sli;
 
 	/* If the mailbox completed, process the completion and return */
 	if (lpfc_sli4_process_missed_mbox_completions(phba))
 		return;
 
+	if (pmbox != NULL)
+		mb = &pmbox->u.mb;
 	/* Check the pmbox pointer first.  There is a race condition
 	 * between the mbox timeout handler getting executed in the
 	 * worklist and the mailbox actually completing. When this
@@ -8138,7 +8103,7 @@ lpfc_sli4_bpl2sgl(struct lpfc_hba *phba, struct lpfc_iocbq *piocbq,
  *
  * Return: index into SLI4 fast-path FCP queue index.
  **/
-static inline uint32_t
+static inline int
 lpfc_sli4_scmd_to_wqidx_distr(struct lpfc_hba *phba)
 {
 	struct lpfc_vector_map_info *cpup;
@@ -8152,7 +8117,6 @@ lpfc_sli4_scmd_to_wqidx_distr(struct lpfc_hba *phba)
 			cpup += cpu;
 			return cpup->channel_id;
 		}
-		chann = cpu;
 	}
 	chann = atomic_add_return(1, &phba->fcp_qidx);
 	chann = (chann % phba->cfg_fcp_io_channel);
@@ -8784,6 +8748,37 @@ lpfc_sli_api_table_setup(struct lpfc_hba *phba, uint8_t dev_grp)
 	return 0;
 }
 
+int
+lpfc_sli_calc_ring(struct lpfc_hba *phba, uint32_t ring_number,
+		    struct lpfc_iocbq *piocb)
+{
+	uint32_t idx;
+
+	if (phba->sli_rev == LPFC_SLI_REV4) {
+		if (piocb->iocb_flag &  (LPFC_IO_FCP | LPFC_USE_FCPWQIDX)) {
+			/*
+			 * fcp_wqidx should already be setup based on what
+			 * completion queue we want to use.
+			 */
+			if (!(phba->cfg_fof) ||
+			    (!(piocb->iocb_flag & LPFC_IO_FOF))) {
+				if (unlikely(!phba->sli4_hba.fcp_wq))
+					return LPFC_HBA_ERROR;
+				idx = lpfc_sli4_scmd_to_wqidx_distr(phba);
+				piocb->fcp_wqidx = idx;
+				ring_number = MAX_SLI3_CONFIGURED_RINGS + idx;
+			} else {
+				if (unlikely(!phba->sli4_hba.oas_wq))
+					return LPFC_HBA_ERROR;
+				idx = 0;
+				piocb->fcp_wqidx = idx;
+				ring_number =  LPFC_FCP_OAS_RING;
+			}
+		}
+	}
+	return ring_number;
+}
+
 /**
  * lpfc_sli_issue_iocb - Wrapper function for __lpfc_sli_issue_iocb
  * @phba: Pointer to HBA context object.
@@ -8809,61 +8804,42 @@ lpfc_sli_issue_iocb(struct lpfc_hba *phba, uint32_t ring_number,
 	int rc, idx;
 
 	if (phba->sli_rev == LPFC_SLI_REV4) {
-		if (piocb->iocb_flag &  LPFC_IO_FCP) {
-			if (!phba->cfg_fof || (!(piocb->iocb_flag &
-				LPFC_IO_OAS))) {
-				if (unlikely(!phba->sli4_hba.fcp_wq))
-					return IOCB_ERROR;
-				idx = lpfc_sli4_scmd_to_wqidx_distr(phba);
-				piocb->fcp_wqidx = idx;
-				ring_number = MAX_SLI3_CONFIGURED_RINGS + idx;
-			} else {
-				if (unlikely(!phba->sli4_hba.oas_wq))
-					return IOCB_ERROR;
-				idx = 0;
-				piocb->fcp_wqidx = 0;
-				ring_number =  LPFC_FCP_OAS_RING;
-			}
-			pring = &phba->sli.ring[ring_number];
-			spin_lock_irqsave(&pring->ring_lock, iflags);
-			rc = __lpfc_sli_issue_iocb(phba, ring_number, piocb,
-				flag);
-			spin_unlock_irqrestore(&pring->ring_lock, iflags);
+		ring_number = lpfc_sli_calc_ring(phba, ring_number, piocb);
+		if (unlikely(ring_number == LPFC_HBA_ERROR))
+			return IOCB_ERROR;
+		idx = piocb->fcp_wqidx;
 
-			if (lpfc_fcp_look_ahead) {
-				fcp_eq_hdl = &phba->sli4_hba.fcp_eq_hdl[idx];
+		pring = &phba->sli.ring[ring_number];
+		spin_lock_irqsave(&pring->ring_lock, iflags);
+		rc = __lpfc_sli_issue_iocb(phba, ring_number, piocb, flag);
+		spin_unlock_irqrestore(&pring->ring_lock, iflags);
 
-				if (atomic_dec_and_test(&fcp_eq_hdl->
-					fcp_eq_in_use)) {
+		if (lpfc_fcp_look_ahead && (piocb->iocb_flag &  LPFC_IO_FCP)) {
+			fcp_eq_hdl = &phba->sli4_hba.fcp_eq_hdl[idx];
 
-					/* Get associated EQ with this index */
-					fpeq = phba->sli4_hba.hba_eq[idx];
+			if (atomic_dec_and_test(&fcp_eq_hdl->
+				fcp_eq_in_use)) {
 
-					/* Turn off interrupts from this EQ */
-					lpfc_sli4_eq_clr_intr(fpeq);
+				/* Get associated EQ with this index */
+				fpeq = phba->sli4_hba.hba_eq[idx];
 
-					/*
-					 * Process all the events on FCP EQ
-					 */
-					while ((eqe = lpfc_sli4_eq_get(fpeq))) {
-						lpfc_sli4_hba_handle_eqe(phba,
-							eqe, idx);
-						fpeq->EQ_processed++;
-					}
+				/* Turn off interrupts from this EQ */
+				lpfc_sli4_eq_clr_intr(fpeq);
 
-					/* Always clear and re-arm the EQ */
-					lpfc_sli4_eq_release(fpeq,
-						LPFC_QUEUE_REARM);
+				/*
+				 * Process all the events on FCP EQ
+				 */
+				while ((eqe = lpfc_sli4_eq_get(fpeq))) {
+					lpfc_sli4_hba_handle_eqe(phba,
+						eqe, idx);
+					fpeq->EQ_processed++;
 				}
-				atomic_inc(&fcp_eq_hdl->fcp_eq_in_use);
-			}
-		} else {
-			pring = &phba->sli.ring[ring_number];
-			spin_lock_irqsave(&pring->ring_lock, iflags);
-			rc = __lpfc_sli_issue_iocb(phba, ring_number, piocb,
-				flag);
-			spin_unlock_irqrestore(&pring->ring_lock, iflags);
 
+				/* Always clear and re-arm the EQ */
+				lpfc_sli4_eq_release(fpeq,
+					LPFC_QUEUE_REARM);
+			}
+			atomic_inc(&fcp_eq_hdl->fcp_eq_in_use);
 		}
 	} else {
 		/* For now, SLI2/3 will still use hbalock */
@@ -9746,6 +9722,7 @@ lpfc_sli_abort_iotag_issue(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	struct lpfc_iocbq *abtsiocbp;
 	IOCB_t *icmd = NULL;
 	IOCB_t *iabt = NULL;
+	int ring_number;
 	int retval;
 	unsigned long iflags;
 
@@ -9786,6 +9763,8 @@ lpfc_sli_abort_iotag_issue(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 	abtsiocbp->fcp_wqidx = cmdiocb->fcp_wqidx;
 	if (cmdiocb->iocb_flag & LPFC_IO_FCP)
 		abtsiocbp->iocb_flag |= LPFC_USE_FCPWQIDX;
+	if (cmdiocb->iocb_flag & LPFC_IO_FOF)
+		abtsiocbp->iocb_flag |= LPFC_IO_FOF;
 
 	if (phba->link_state >= LPFC_LINK_UP)
 		iabt->ulpCommand = CMD_ABORT_XRI_CN;
@@ -9802,6 +9781,11 @@ lpfc_sli_abort_iotag_issue(struct lpfc_hba *phba, struct lpfc_sli_ring *pring,
 			 abtsiocbp->iotag);
 
 	if (phba->sli_rev == LPFC_SLI_REV4) {
+		ring_number =
+			lpfc_sli_calc_ring(phba, pring->ringno, abtsiocbp);
+		if (unlikely(ring_number == LPFC_HBA_ERROR))
+			return 0;
+		pring = &phba->sli.ring[ring_number];
 		/* Note: both hbalock and ring_lock need to be set here */
 		spin_lock_irqsave(&pring->ring_lock, iflags);
 		retval = __lpfc_sli_issue_iocb(phba, pring->ringno,
@@ -10099,6 +10083,8 @@ lpfc_sli_abort_iocb(struct lpfc_vport *vport, struct lpfc_sli_ring *pring,
 		abtsiocb->fcp_wqidx = iocbq->fcp_wqidx;
 		if (iocbq->iocb_flag & LPFC_IO_FCP)
 			abtsiocb->iocb_flag |= LPFC_USE_FCPWQIDX;
+		if (iocbq->iocb_flag & LPFC_IO_FOF)
+			abtsiocb->iocb_flag |= LPFC_IO_FOF;
 
 		if (lpfc_is_link_up(phba))
 			abtsiocb->iocb.ulpCommand = CMD_ABORT_XRI_CN;
@@ -10146,7 +10132,9 @@ lpfc_sli_abort_taskmgmt(struct lpfc_vport *vport, struct lpfc_sli_ring *pring,
 			uint16_t tgt_id, uint64_t lun_id, lpfc_ctx_cmd cmd)
 {
 	struct lpfc_hba *phba = vport->phba;
+	struct lpfc_scsi_buf *lpfc_cmd;
 	struct lpfc_iocbq *abtsiocbq;
+	struct lpfc_nodelist *ndlp;
 	struct lpfc_iocbq *iocbq;
 	IOCB_t *icmd;
 	int sum, i, ret_val;
@@ -10198,8 +10186,14 @@ lpfc_sli_abort_taskmgmt(struct lpfc_vport *vport, struct lpfc_sli_ring *pring,
 		abtsiocbq->fcp_wqidx = iocbq->fcp_wqidx;
 		if (iocbq->iocb_flag & LPFC_IO_FCP)
 			abtsiocbq->iocb_flag |= LPFC_USE_FCPWQIDX;
+		if (iocbq->iocb_flag & LPFC_IO_FOF)
+			abtsiocbq->iocb_flag |= LPFC_IO_FOF;
 
-		if (lpfc_is_link_up(phba))
+		lpfc_cmd = container_of(iocbq, struct lpfc_scsi_buf, cur_iocbq);
+		ndlp = lpfc_cmd->rdata->pnode;
+
+		if (lpfc_is_link_up(phba) &&
+		    (ndlp && ndlp->nlp_state == NLP_STE_MAPPED_NODE))
 			abtsiocbq->iocb.ulpCommand = CMD_ABORT_XRI_CN;
 		else
 			abtsiocbq->iocb.ulpCommand = CMD_CLOSE_XRI_CN;
@@ -12611,6 +12605,9 @@ lpfc_sli4_hba_intr_handler(int irq, void *dev_id)
 	 * Process all the event on FCP fast-path EQ
 	 */
 	while ((eqe = lpfc_sli4_eq_get(fpeq))) {
+		if (eqe == NULL)
+			break;
+
 		lpfc_sli4_hba_handle_eqe(phba, eqe, fcp_eqidx);
 		if (!(++ecount % fpeq->entry_repost))
 			lpfc_sli4_eq_release(fpeq, LPFC_QUEUE_NOARM);
@@ -12760,14 +12757,13 @@ lpfc_sli4_queue_alloc(struct lpfc_hba *phba, uint32_t entry_size,
 		dmabuf = kzalloc(sizeof(struct lpfc_dmabuf), GFP_KERNEL);
 		if (!dmabuf)
 			goto out_fail;
-		dmabuf->virt = dma_alloc_coherent(&phba->pcidev->dev,
-						  hw_page_size, &dmabuf->phys,
-						  GFP_KERNEL);
+		dmabuf->virt = dma_zalloc_coherent(&phba->pcidev->dev,
+						   hw_page_size, &dmabuf->phys,
+						   GFP_KERNEL);
 		if (!dmabuf->virt) {
 			kfree(dmabuf);
 			goto out_fail;
 		}
-		memset(dmabuf->virt, 0, hw_page_size);
 		dmabuf->buffer_tag = x;
 		list_add_tail(&dmabuf->list, &queue->page_list);
 		/* initialize queue's entry array */
@@ -12845,7 +12841,7 @@ lpfc_dual_chute_pci_bar_map(struct lpfc_hba *phba, uint16_t pci_barset)
  * memory this function will return -ENOMEM. If the queue create mailbox command
  * fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
 {
 	struct lpfc_mbx_modify_eq_delay *eq_delay;
@@ -12931,7 +12927,7 @@ lpfc_modify_fcp_eq_delay(struct lpfc_hba *phba, uint16_t startq)
  * memory this function will return -ENOMEM. If the queue create mailbox command
  * fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint32_t imax)
 {
 	struct lpfc_mbx_eq_create *eq_create;
@@ -13053,7 +13049,7 @@ lpfc_eq_create(struct lpfc_hba *phba, struct lpfc_queue *eq, uint32_t imax)
  * memory this function will return -ENOMEM. If the queue create mailbox command
  * fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_cq_create(struct lpfc_hba *phba, struct lpfc_queue *cq,
 	       struct lpfc_queue *eq, uint32_t type, uint32_t subtype)
 {
@@ -13394,7 +13390,7 @@ out:
  * memory this function will return -ENOMEM. If the queue create mailbox command
  * fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_wq_create(struct lpfc_hba *phba, struct lpfc_queue *wq,
 	       struct lpfc_queue *cq, uint32_t subtype)
 {
@@ -13630,7 +13626,7 @@ lpfc_rq_adjust_repost(struct lpfc_hba *phba, struct lpfc_queue *rq, int qno)
  * memory this function will return -ENOMEM. If the queue create mailbox command
  * fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_rq_create(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 	       struct lpfc_queue *drq, struct lpfc_queue *cq, uint32_t subtype)
 {
@@ -13895,7 +13891,7 @@ out:
  * On success this function will return a zero. If the queue destroy mailbox
  * command fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_eq_destroy(struct lpfc_hba *phba, struct lpfc_queue *eq)
 {
 	LPFC_MBOXQ_t *mbox;
@@ -13951,7 +13947,7 @@ lpfc_eq_destroy(struct lpfc_hba *phba, struct lpfc_queue *eq)
  * On success this function will return a zero. If the queue destroy mailbox
  * command fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_cq_destroy(struct lpfc_hba *phba, struct lpfc_queue *cq)
 {
 	LPFC_MBOXQ_t *mbox;
@@ -14005,7 +14001,7 @@ lpfc_cq_destroy(struct lpfc_hba *phba, struct lpfc_queue *cq)
  * On success this function will return a zero. If the queue destroy mailbox
  * command fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_mq_destroy(struct lpfc_hba *phba, struct lpfc_queue *mq)
 {
 	LPFC_MBOXQ_t *mbox;
@@ -14059,7 +14055,7 @@ lpfc_mq_destroy(struct lpfc_hba *phba, struct lpfc_queue *mq)
  * On success this function will return a zero. If the queue destroy mailbox
  * command fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_wq_destroy(struct lpfc_hba *phba, struct lpfc_queue *wq)
 {
 	LPFC_MBOXQ_t *mbox;
@@ -14112,7 +14108,7 @@ lpfc_wq_destroy(struct lpfc_hba *phba, struct lpfc_queue *wq)
  * On success this function will return a zero. If the queue destroy mailbox
  * command fails this function will return -ENXIO.
  **/
-uint32_t
+int
 lpfc_rq_destroy(struct lpfc_hba *phba, struct lpfc_queue *hrq,
 		struct lpfc_queue *drq)
 {
@@ -14252,7 +14248,6 @@ lpfc_sli4_post_sgl(struct lpfc_hba *phba,
 				"2511 POST_SGL mailbox failed with "
 				"status x%x add_status x%x, mbx status x%x\n",
 				shdr_status, shdr_add_status, rc);
-		rc = -ENXIO;
 	}
 	return 0;
 }
@@ -14270,7 +14265,7 @@ lpfc_sli4_post_sgl(struct lpfc_hba *phba,
  *	A nonzero rpi defined as rpi_base <= rpi < max_rpi if successful
  *	LPFC_RPI_ALLOC_ERROR if no rpis are available.
  **/
-uint16_t
+static uint16_t
 lpfc_sli4_alloc_xri(struct lpfc_hba *phba)
 {
 	unsigned long xri;
@@ -14300,7 +14295,7 @@ lpfc_sli4_alloc_xri(struct lpfc_hba *phba)
  * This routine is invoked to release an xri to the pool of
  * available rpis maintained by the driver.
  **/
-void
+static void
 __lpfc_sli4_free_xri(struct lpfc_hba *phba, int xri)
 {
 	if (test_and_clear_bit(xri, phba->sli4_hba.xri_bmask)) {
@@ -14720,7 +14715,7 @@ lpfc_fc_frame_to_vport(struct lpfc_hba *phba, struct fc_frame_header *fc_hdr,
  * the driver uses this time stamp to indicate if any received sequences have
  * timed out.
  **/
-void
+static void
 lpfc_update_rcv_time_stamp(struct lpfc_vport *vport)
 {
 	struct lpfc_dmabuf *h_buf;
@@ -15019,7 +15014,7 @@ uint16_t
 lpfc_sli4_xri_inrange(struct lpfc_hba *phba,
 		      uint16_t xri)
 {
-	int i;
+	uint16_t i;
 
 	for (i = 0; i < phba->sli4_hba.max_cfg_param.max_xri; i++) {
 		if (xri == phba->sli4_hba.xri_ids[i])
@@ -15189,7 +15184,7 @@ lpfc_sli4_seq_abort_rsp(struct lpfc_vport *vport,
  * unsolicited sequence has been aborted. After that, it will issue a basic
  * accept to accept the abort.
  **/
-void
+static void
 lpfc_sli4_handle_unsol_abort(struct lpfc_vport *vport,
 			     struct hbq_dmabuf *dmabuf)
 {
@@ -15734,7 +15729,7 @@ lpfc_sli4_alloc_rpi(struct lpfc_hba *phba)
  * This routine is invoked to release an rpi to the pool of
  * available rpis maintained by the driver.
  **/
-void
+static void
 __lpfc_sli4_free_rpi(struct lpfc_hba *phba, int rpi)
 {
 	if (test_and_clear_bit(rpi, phba->sli4_hba.rpi_bmask)) {
@@ -16172,7 +16167,7 @@ fail_fcf_read:
  * returns:
  * 1=success 0=failure
  **/
-int
+static int
 lpfc_check_next_fcf_pri_level(struct lpfc_hba *phba)
 {
 	uint16_t next_fcf_pri;
@@ -16403,7 +16398,7 @@ lpfc_sli4_fcf_rr_index_clear(struct lpfc_hba *phba, uint16_t fcf_index)
  * command. If the mailbox command returned failure, it will try to stop the
  * FCF rediscover wait timer.
  **/
-void
+static void
 lpfc_mbx_cmpl_redisc_fcf_table(struct lpfc_hba *phba, LPFC_MBOXQ_t *mbox)
 {
 	struct lpfc_mbx_redisc_fcf_tbl *redisc_fcf;
@@ -16956,7 +16951,7 @@ lpfc_drain_txq(struct lpfc_hba *phba)
 	char *fail_msg = NULL;
 	struct lpfc_sglq *sglq;
 	union lpfc_wqe wqe;
-	int txq_cnt = 0;
+	uint32_t txq_cnt = 0;
 
 	spin_lock_irqsave(&pring->ring_lock, iflags);
 	list_for_each_entry(piocbq, &pring->txq, list) {
diff --git a/drivers/scsi/lpfc/lpfc_sli.h b/drivers/scsi/lpfc/lpfc_sli.h
index edb48832c39b..4a01452415cf 100644
--- a/drivers/scsi/lpfc/lpfc_sli.h
+++ b/drivers/scsi/lpfc/lpfc_sli.h
@@ -79,6 +79,7 @@ struct lpfc_iocbq {
 #define LPFC_FIP_ELS_ID_SHIFT	14
 
 #define LPFC_IO_OAS		0x10000 /* OAS FCP IO */
+#define LPFC_IO_FOF		0x20000 /* FOF FCP IO */
 
 	uint32_t drvrTimeout;	/* driver timeout in seconds */
 	uint32_t fcp_wqidx;	/* index to FCP work queue */
diff --git a/drivers/scsi/lpfc/lpfc_sli4.h b/drivers/scsi/lpfc/lpfc_sli4.h
index 7f50aa04d66a..22ceb2b05ba1 100644
--- a/drivers/scsi/lpfc/lpfc_sli4.h
+++ b/drivers/scsi/lpfc/lpfc_sli4.h
@@ -670,22 +670,22 @@ void lpfc_sli4_hba_reset(struct lpfc_hba *);
 struct lpfc_queue *lpfc_sli4_queue_alloc(struct lpfc_hba *, uint32_t,
 			uint32_t);
 void lpfc_sli4_queue_free(struct lpfc_queue *);
-uint32_t lpfc_eq_create(struct lpfc_hba *, struct lpfc_queue *, uint32_t);
-uint32_t lpfc_modify_fcp_eq_delay(struct lpfc_hba *, uint16_t);
-uint32_t lpfc_cq_create(struct lpfc_hba *, struct lpfc_queue *,
+int lpfc_eq_create(struct lpfc_hba *, struct lpfc_queue *, uint32_t);
+int lpfc_modify_fcp_eq_delay(struct lpfc_hba *, uint16_t);
+int lpfc_cq_create(struct lpfc_hba *, struct lpfc_queue *,
 			struct lpfc_queue *, uint32_t, uint32_t);
 int32_t lpfc_mq_create(struct lpfc_hba *, struct lpfc_queue *,
 		       struct lpfc_queue *, uint32_t);
-uint32_t lpfc_wq_create(struct lpfc_hba *, struct lpfc_queue *,
+int lpfc_wq_create(struct lpfc_hba *, struct lpfc_queue *,
 			struct lpfc_queue *, uint32_t);
-uint32_t lpfc_rq_create(struct lpfc_hba *, struct lpfc_queue *,
+int lpfc_rq_create(struct lpfc_hba *, struct lpfc_queue *,
 			struct lpfc_queue *, struct lpfc_queue *, uint32_t);
 void lpfc_rq_adjust_repost(struct lpfc_hba *, struct lpfc_queue *, int);
-uint32_t lpfc_eq_destroy(struct lpfc_hba *, struct lpfc_queue *);
-uint32_t lpfc_cq_destroy(struct lpfc_hba *, struct lpfc_queue *);
-uint32_t lpfc_mq_destroy(struct lpfc_hba *, struct lpfc_queue *);
-uint32_t lpfc_wq_destroy(struct lpfc_hba *, struct lpfc_queue *);
-uint32_t lpfc_rq_destroy(struct lpfc_hba *, struct lpfc_queue *,
+int lpfc_eq_destroy(struct lpfc_hba *, struct lpfc_queue *);
+int lpfc_cq_destroy(struct lpfc_hba *, struct lpfc_queue *);
+int lpfc_mq_destroy(struct lpfc_hba *, struct lpfc_queue *);
+int lpfc_wq_destroy(struct lpfc_hba *, struct lpfc_queue *);
+int lpfc_rq_destroy(struct lpfc_hba *, struct lpfc_queue *,
 			 struct lpfc_queue *);
 int lpfc_sli4_queue_setup(struct lpfc_hba *);
 void lpfc_sli4_queue_unset(struct lpfc_hba *);
diff --git a/drivers/scsi/lpfc/lpfc_version.h b/drivers/scsi/lpfc/lpfc_version.h
index 41675c1193e7..89413add2252 100644
--- a/drivers/scsi/lpfc/lpfc_version.h
+++ b/drivers/scsi/lpfc/lpfc_version.h
@@ -18,7 +18,7 @@
  * included with this package.                                     *
  *******************************************************************/
 
-#define LPFC_DRIVER_VERSION "10.2.8001.0."
+#define LPFC_DRIVER_VERSION "10.4.8000.0."
 #define LPFC_DRIVER_NAME		"lpfc"
 
 /* Used for SLI 2/3 */
diff --git a/drivers/scsi/megaraid/megaraid_sas.h b/drivers/scsi/megaraid/megaraid_sas.h
index 32166c2c7854..a49914de4b95 100644
--- a/drivers/scsi/megaraid/megaraid_sas.h
+++ b/drivers/scsi/megaraid/megaraid_sas.h
@@ -33,9 +33,9 @@
 /*
  * MegaRAID SAS Driver meta data
  */
-#define MEGASAS_VERSION				"06.803.01.00-rc1"
-#define MEGASAS_RELDATE				"Mar. 10, 2014"
-#define MEGASAS_EXT_VERSION			"Mon. Mar. 10 17:00:00 PDT 2014"
+#define MEGASAS_VERSION				"06.805.06.00-rc1"
+#define MEGASAS_RELDATE				"Sep. 4, 2014"
+#define MEGASAS_EXT_VERSION			"Thu. Sep. 4 17:00:00 PDT 2014"
 
 /*
  * Device IDs
@@ -105,6 +105,9 @@
 #define MFI_STATE_READY				0xB0000000
 #define MFI_STATE_OPERATIONAL			0xC0000000
 #define MFI_STATE_FAULT				0xF0000000
+#define MFI_STATE_FORCE_OCR			0x00000080
+#define MFI_STATE_DMADONE			0x00000008
+#define MFI_STATE_CRASH_DUMP_DONE		0x00000004
 #define MFI_RESET_REQUIRED			0x00000001
 #define MFI_RESET_ADAPTER			0x00000002
 #define MEGAMFI_FRAME_SIZE			64
@@ -191,6 +194,9 @@
 #define MR_DCMD_CLUSTER_RESET_LD		0x08010200
 #define MR_DCMD_PD_LIST_QUERY                   0x02010100
 
+#define MR_DCMD_CTRL_SET_CRASH_DUMP_PARAMS	0x01190100
+#define MR_DRIVER_SET_APP_CRASHDUMP_MODE	(0xF0010000 | 0x0600)
+
 /*
  * Global functions
  */
@@ -264,6 +270,25 @@ enum MFI_STAT {
 };
 
 /*
+ * Crash dump related defines
+ */
+#define MAX_CRASH_DUMP_SIZE 512
+#define CRASH_DMA_BUF_SIZE  (1024 * 1024)
+
+enum MR_FW_CRASH_DUMP_STATE {
+	UNAVAILABLE = 0,
+	AVAILABLE = 1,
+	COPYING = 2,
+	COPIED = 3,
+	COPY_ERROR = 4,
+};
+
+enum _MR_CRASH_BUF_STATUS {
+	MR_CRASH_BUF_TURN_OFF = 0,
+	MR_CRASH_BUF_TURN_ON = 1,
+};
+
+/*
  * Number of mailbox bytes in DCMD message frame
  */
 #define MFI_MBOX_SIZE				12
@@ -365,7 +390,6 @@ enum MR_LD_QUERY_TYPE {
 #define MR_EVT_FOREIGN_CFG_IMPORTED                     0x00db
 #define MR_EVT_LD_OFFLINE                               0x00fc
 #define MR_EVT_CTRL_HOST_BUS_SCAN_REQUESTED             0x0152
-#define MAX_LOGICAL_DRIVES				64
 
 enum MR_PD_STATE {
 	MR_PD_STATE_UNCONFIGURED_GOOD   = 0x00,
@@ -443,14 +467,14 @@ struct MR_LD_LIST {
 		u8          state;
 		u8          reserved[3];
 		u64         size;
-	} ldList[MAX_LOGICAL_DRIVES];
+	} ldList[MAX_LOGICAL_DRIVES_EXT];
 } __packed;
 
 struct MR_LD_TARGETID_LIST {
 	u32	size;
 	u32	count;
 	u8	pad[3];
-	u8	targetId[MAX_LOGICAL_DRIVES];
+	u8	targetId[MAX_LOGICAL_DRIVES_EXT];
 };
 
 
@@ -916,6 +940,15 @@ struct megasas_ctrl_info {
 	* HA cluster information
 	*/
 	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u32     reserved:26;
+		u32     premiumFeatureMismatch:1;
+		u32     ctrlPropIncompatible:1;
+		u32     fwVersionMismatch:1;
+		u32     hwIncompatible:1;
+		u32     peerIsIncompatible:1;
+		u32     peerIsPresent:1;
+#else
 		u32     peerIsPresent:1;
 		u32     peerIsIncompatible:1;
 		u32     hwIncompatible:1;
@@ -923,6 +956,7 @@ struct megasas_ctrl_info {
 		u32     ctrlPropIncompatible:1;
 		u32     premiumFeatureMismatch:1;
 		u32     reserved:26;
+#endif
 	} cluster;
 
 	char clusterId[16];                     /*7D4h */
@@ -933,7 +967,27 @@ struct megasas_ctrl_info {
 		u8  reserved;                   /*0x7E7*/
 	} iov;
 
-	u8          pad[0x800-0x7E8];           /*0x7E8 pad to 2k */
+	struct {
+#if defined(__BIG_ENDIAN_BITFIELD)
+		u32     reserved:25;
+		u32     supportCrashDump:1;
+		u32     supportMaxExtLDs:1;
+		u32     supportT10RebuildAssist:1;
+		u32     supportDisableImmediateIO:1;
+		u32     supportThermalPollInterval:1;
+		u32     supportPersonalityChange:2;
+#else
+		u32     supportPersonalityChange:2;
+		u32     supportThermalPollInterval:1;
+		u32     supportDisableImmediateIO:1;
+		u32     supportT10RebuildAssist:1;
+		u32     supportMaxExtLDs:1;
+		u32     supportCrashDump:1;
+		u32     reserved:25;
+#endif
+	} adapterOperations3;
+
+	u8          pad[0x800-0x7EC];
 } __packed;
 
 /*
@@ -942,13 +996,12 @@ struct megasas_ctrl_info {
  * ===============================
  */
 #define MEGASAS_MAX_PD_CHANNELS			2
-#define MEGASAS_MAX_LD_CHANNELS			1
+#define MEGASAS_MAX_LD_CHANNELS			2
 #define MEGASAS_MAX_CHANNELS			(MEGASAS_MAX_PD_CHANNELS + \
 						MEGASAS_MAX_LD_CHANNELS)
 #define MEGASAS_MAX_DEV_PER_CHANNEL		128
 #define MEGASAS_DEFAULT_INIT_ID			-1
 #define MEGASAS_MAX_LUN				8
-#define MEGASAS_MAX_LD				64
 #define MEGASAS_DEFAULT_CMD_PER_LUN		256
 #define MEGASAS_MAX_PD                          (MEGASAS_MAX_PD_CHANNELS * \
 						MEGASAS_MAX_DEV_PER_CHANNEL)
@@ -961,6 +1014,14 @@ struct megasas_ctrl_info {
 
 #define MEGASAS_FW_BUSY				1
 
+#define VD_EXT_DEBUG 0
+
+enum MR_MFI_MPT_PTHR_FLAGS {
+	MFI_MPT_DETACHED = 0,
+	MFI_LIST_ADDED = 1,
+	MFI_MPT_ATTACHED = 2,
+};
+
 /* Frame Type */
 #define IO_FRAME				0
 #define PTHRU_FRAME				1
@@ -978,7 +1039,7 @@ struct megasas_ctrl_info {
 #define MEGASAS_IOCTL_CMD			0
 #define MEGASAS_DEFAULT_CMD_TIMEOUT		90
 #define MEGASAS_THROTTLE_QUEUE_DEPTH		16
-
+#define MEGASAS_BLOCKED_CMD_TIMEOUT		60
 /*
  * FW reports the maximum of number of commands that it can accept (maximum
  * commands that can be outstanding) at any time. The driver must report a
@@ -1133,13 +1194,19 @@ union megasas_sgl_frame {
 typedef union _MFI_CAPABILITIES {
 	struct {
 #if   defined(__BIG_ENDIAN_BITFIELD)
-		u32     reserved:30;
+		u32     reserved:27;
+		u32     support_ndrive_r1_lb:1;
+		u32	support_max_255lds:1;
+		u32	reserved1:1;
 		u32     support_additional_msix:1;
 		u32     support_fp_remote_lun:1;
 #else
 		u32     support_fp_remote_lun:1;
 		u32     support_additional_msix:1;
-		u32     reserved:30;
+		u32	reserved1:1;
+		u32	support_max_255lds:1;
+		u32     support_ndrive_r1_lb:1;
+		u32     reserved:27;
 #endif
 	} mfi_capabilities;
 	u32     reg;
@@ -1559,6 +1626,20 @@ struct megasas_instance {
 	u32 *reply_queue;
 	dma_addr_t reply_queue_h;
 
+	u32 *crash_dump_buf;
+	dma_addr_t crash_dump_h;
+	void *crash_buf[MAX_CRASH_DUMP_SIZE];
+	u32 crash_buf_pages;
+	unsigned int    fw_crash_buffer_size;
+	unsigned int    fw_crash_state;
+	unsigned int    fw_crash_buffer_offset;
+	u32 drv_buf_index;
+	u32 drv_buf_alloc;
+	u32 crash_dump_fw_support;
+	u32 crash_dump_drv_support;
+	u32 crash_dump_app_support;
+	spinlock_t crashdump_lock;
+
 	struct megasas_register_set __iomem *reg_set;
 	u32 *reply_post_host_index_addr[MR_MAX_MSIX_REG_ARRAY];
 	struct megasas_pd_list          pd_list[MEGASAS_MAX_PD];
@@ -1577,7 +1658,7 @@ struct megasas_instance {
 	struct megasas_cmd **cmd_list;
 	struct list_head cmd_pool;
 	/* used to sync fire the cmd to fw */
-	spinlock_t cmd_pool_lock;
+	spinlock_t mfi_pool_lock;
 	/* used to sync fire the cmd to fw */
 	spinlock_t hba_lock;
 	/* used to synch producer, consumer ptrs in dpc */
@@ -1606,6 +1687,7 @@ struct megasas_instance {
 	struct megasas_instance_template *instancet;
 	struct tasklet_struct isr_tasklet;
 	struct work_struct work_init;
+	struct work_struct crash_init;
 
 	u8 flag;
 	u8 unload;
@@ -1613,6 +1695,14 @@ struct megasas_instance {
 	u8 issuepend_done;
 	u8 disableOnlineCtrlReset;
 	u8 UnevenSpanSupport;
+
+	u8 supportmax256vd;
+	u16 fw_supported_vd_count;
+	u16 fw_supported_pd_count;
+
+	u16 drv_supported_vd_count;
+	u16 drv_supported_pd_count;
+
 	u8 adprecovery;
 	unsigned long last_time;
 	u32 mfiStatus;
@@ -1622,6 +1712,8 @@ struct megasas_instance {
 
 	/* Ptr to hba specific information */
 	void *ctrl_context;
+	u32 ctrl_context_pages;
+	struct megasas_ctrl_info *ctrl_info;
 	unsigned int msix_vectors;
 	struct msix_entry msixentry[MEGASAS_MAX_MSIX_QUEUES];
 	struct megasas_irq_context irq_context[MEGASAS_MAX_MSIX_QUEUES];
@@ -1633,8 +1725,6 @@ struct megasas_instance {
 	struct timer_list sriov_heartbeat_timer;
 	char skip_heartbeat_timer_del;
 	u8 requestorId;
-	u64 initiator_sas_address;
-	u64 ld_sas_address[64];
 	char PlasmaFW111;
 	char mpio;
 	int throttlequeuedepth;
@@ -1661,6 +1751,7 @@ struct MR_LD_VF_AFFILIATION {
 /* Plasma 1.11 FW backward compatibility structures */
 #define IOV_111_OFFSET 0x7CE
 #define MAX_VIRTUAL_FUNCTIONS 8
+#define MR_LD_ACCESS_HIDDEN 15
 
 struct IOV_111 {
 	u8 maxVFsSupported;
@@ -1754,6 +1845,11 @@ struct megasas_cmd {
 
 	struct list_head list;
 	struct scsi_cmnd *scmd;
+
+	void *mpt_pthr_cmd_blocked;
+	atomic_t mfi_mpt_pthr;
+	u8 is_wait_event;
+
 	struct megasas_instance *instance;
 	union {
 		struct {
@@ -1823,12 +1919,33 @@ u8
 MR_BuildRaidContext(struct megasas_instance *instance,
 		    struct IO_REQUEST_INFO *io_info,
 		    struct RAID_CONTEXT *pRAID_Context,
-		    struct MR_FW_RAID_MAP_ALL *map, u8 **raidLUN);
-u8 MR_TargetIdToLdGet(u32 ldTgtId, struct MR_FW_RAID_MAP_ALL *map);
-struct MR_LD_RAID *MR_LdRaidGet(u32 ld, struct MR_FW_RAID_MAP_ALL *map);
-u16 MR_ArPdGet(u32 ar, u32 arm, struct MR_FW_RAID_MAP_ALL *map);
-u16 MR_LdSpanArrayGet(u32 ld, u32 span, struct MR_FW_RAID_MAP_ALL *map);
-u16 MR_PdDevHandleGet(u32 pd, struct MR_FW_RAID_MAP_ALL *map);
-u16 MR_GetLDTgtId(u32 ld, struct MR_FW_RAID_MAP_ALL *map);
+		    struct MR_DRV_RAID_MAP_ALL *map, u8 **raidLUN);
+u8 MR_TargetIdToLdGet(u32 ldTgtId, struct MR_DRV_RAID_MAP_ALL *map);
+struct MR_LD_RAID *MR_LdRaidGet(u32 ld, struct MR_DRV_RAID_MAP_ALL *map);
+u16 MR_ArPdGet(u32 ar, u32 arm, struct MR_DRV_RAID_MAP_ALL *map);
+u16 MR_LdSpanArrayGet(u32 ld, u32 span, struct MR_DRV_RAID_MAP_ALL *map);
+u16 MR_PdDevHandleGet(u32 pd, struct MR_DRV_RAID_MAP_ALL *map);
+u16 MR_GetLDTgtId(u32 ld, struct MR_DRV_RAID_MAP_ALL *map);
+
+u16 get_updated_dev_handle(struct megasas_instance *instance,
+	struct LD_LOAD_BALANCE_INFO *lbInfo, struct IO_REQUEST_INFO *in_info);
+void mr_update_load_balance_params(struct MR_DRV_RAID_MAP_ALL *map,
+	struct LD_LOAD_BALANCE_INFO *lbInfo);
+int megasas_get_ctrl_info(struct megasas_instance *instance,
+	struct megasas_ctrl_info *ctrl_info);
+int megasas_set_crash_dump_params(struct megasas_instance *instance,
+	u8 crash_buf_state);
+void megasas_free_host_crash_buffer(struct megasas_instance *instance);
+void megasas_fusion_crash_dump_wq(struct work_struct *work);
+
+void megasas_return_cmd_fusion(struct megasas_instance *instance,
+	struct megasas_cmd_fusion *cmd);
+int megasas_issue_blocked_cmd(struct megasas_instance *instance,
+	struct megasas_cmd *cmd, int timeout);
+void __megasas_return_cmd(struct megasas_instance *instance,
+	struct megasas_cmd *cmd);
+
+void megasas_return_mfi_mpt_pthr(struct megasas_instance *instance,
+	struct megasas_cmd *cmd_mfi, struct megasas_cmd_fusion *cmd_fusion);
 
 #endif				/*LSI_MEGARAID_SAS_H */
diff --git a/drivers/scsi/megaraid/megaraid_sas_base.c b/drivers/scsi/megaraid/megaraid_sas_base.c
index 22a04e37b70a..f6a69a3b1b3f 100644
--- a/drivers/scsi/megaraid/megaraid_sas_base.c
+++ b/drivers/scsi/megaraid/megaraid_sas_base.c
@@ -18,7 +18,7 @@
  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  *
  *  FILE: megaraid_sas_base.c
- *  Version : 06.803.01.00-rc1
+ *  Version : 06.805.06.00-rc1
  *
  *  Authors: LSI Corporation
  *           Sreenivas Bagalkote
@@ -89,6 +89,10 @@ module_param(resetwaittime, int, S_IRUGO);
 MODULE_PARM_DESC(resetwaittime, "Wait time in seconds after I/O timeout "
 		 "before resetting adapter. Default: 180");
 
+int smp_affinity_enable = 1;
+module_param(smp_affinity_enable, int, S_IRUGO);
+MODULE_PARM_DESC(smp_affinity_enable, "SMP affinity feature enable/disbale Default: enable(1)");
+
 MODULE_LICENSE("GPL");
 MODULE_VERSION(MEGASAS_VERSION);
 MODULE_AUTHOR("megaraidlinux@lsi.com");
@@ -206,43 +210,66 @@ struct megasas_cmd *megasas_get_cmd(struct megasas_instance
 	unsigned long flags;
 	struct megasas_cmd *cmd = NULL;
 
-	spin_lock_irqsave(&instance->cmd_pool_lock, flags);
+	spin_lock_irqsave(&instance->mfi_pool_lock, flags);
 
 	if (!list_empty(&instance->cmd_pool)) {
 		cmd = list_entry((&instance->cmd_pool)->next,
 				 struct megasas_cmd, list);
 		list_del_init(&cmd->list);
+		atomic_set(&cmd->mfi_mpt_pthr, MFI_MPT_DETACHED);
 	} else {
 		printk(KERN_ERR "megasas: Command pool empty!\n");
 	}
 
-	spin_unlock_irqrestore(&instance->cmd_pool_lock, flags);
+	spin_unlock_irqrestore(&instance->mfi_pool_lock, flags);
 	return cmd;
 }
 
 /**
- * megasas_return_cmd -	Return a cmd to free command pool
+ * __megasas_return_cmd -	Return a cmd to free command pool
  * @instance:		Adapter soft state
  * @cmd:		Command packet to be returned to free command pool
  */
 inline void
-megasas_return_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd)
+__megasas_return_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd)
 {
-	unsigned long flags;
-
-	spin_lock_irqsave(&instance->cmd_pool_lock, flags);
+	/*
+	 * Don't go ahead and free the MFI frame, if corresponding
+	 * MPT frame is not freed(valid for only fusion adapters).
+	 * In case of MFI adapters, anyways for any allocated MFI
+	 * frame will have cmd->mfi_mpt_mpthr set to MFI_MPT_DETACHED
+	 */
+	if (atomic_read(&cmd->mfi_mpt_pthr) != MFI_MPT_DETACHED)
+		return;
 
 	cmd->scmd = NULL;
 	cmd->frame_count = 0;
+	cmd->is_wait_event = 0;
+	cmd->mpt_pthr_cmd_blocked = NULL;
+
 	if ((instance->pdev->device != PCI_DEVICE_ID_LSI_FUSION) &&
-	    (instance->pdev->device != PCI_DEVICE_ID_LSI_PLASMA) &&
 	    (instance->pdev->device != PCI_DEVICE_ID_LSI_INVADER) &&
 	    (instance->pdev->device != PCI_DEVICE_ID_LSI_FURY) &&
 	    (reset_devices))
 		cmd->frame->hdr.cmd = MFI_CMD_INVALID;
-	list_add_tail(&cmd->list, &instance->cmd_pool);
 
-	spin_unlock_irqrestore(&instance->cmd_pool_lock, flags);
+	atomic_set(&cmd->mfi_mpt_pthr, MFI_LIST_ADDED);
+	list_add(&cmd->list, (&instance->cmd_pool)->next);
+}
+
+/**
+ * megasas_return_cmd -	Return a cmd to free command pool
+ * @instance:		Adapter soft state
+ * @cmd:		Command packet to be returned to free command pool
+ */
+inline void
+megasas_return_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&instance->mfi_pool_lock, flags);
+	__megasas_return_cmd(instance, cmd);
+	spin_unlock_irqrestore(&instance->mfi_pool_lock, flags);
 }
 
 
@@ -921,13 +948,14 @@ megasas_issue_polled(struct megasas_instance *instance, struct megasas_cmd *cmd)
  * Max wait time is MEGASAS_INTERNAL_CMD_WAIT_TIME secs
  * Used to issue ioctl commands.
  */
-static int
+int
 megasas_issue_blocked_cmd(struct megasas_instance *instance,
 			  struct megasas_cmd *cmd, int timeout)
 {
 	int ret = 0;
 	cmd->cmd_status = ENODATA;
 
+	cmd->is_wait_event = 1;
 	instance->instancet->issue_dcmd(instance, cmd);
 	if (timeout) {
 		ret = wait_event_timeout(instance->int_cmd_wait_q,
@@ -1536,7 +1564,7 @@ out_return_cmd:
  * @done:			Callback entry point
  */
 static int
-megasas_queue_command_lck(struct scsi_cmnd *scmd, void (*done) (struct scsi_cmnd *))
+megasas_queue_command(struct Scsi_Host *shost, struct scsi_cmnd *scmd)
 {
 	struct megasas_instance *instance;
 	unsigned long flags;
@@ -1558,7 +1586,7 @@ megasas_queue_command_lck(struct scsi_cmnd *scmd, void (*done) (struct scsi_cmnd
 		} else {
 			spin_unlock_irqrestore(&instance->hba_lock, flags);
 			scmd->result = DID_NO_CONNECT << 16;
-			done(scmd);
+			scmd->scsi_done(scmd);
 			return 0;
 		}
 	}
@@ -1566,7 +1594,7 @@ megasas_queue_command_lck(struct scsi_cmnd *scmd, void (*done) (struct scsi_cmnd
 	if (instance->adprecovery == MEGASAS_HW_CRITICAL_ERROR) {
 		spin_unlock_irqrestore(&instance->hba_lock, flags);
 		scmd->result = DID_NO_CONNECT << 16;
-		done(scmd);
+		scmd->scsi_done(scmd);
 		return 0;
 	}
 
@@ -1577,11 +1605,11 @@ megasas_queue_command_lck(struct scsi_cmnd *scmd, void (*done) (struct scsi_cmnd
 
 	spin_unlock_irqrestore(&instance->hba_lock, flags);
 
-	scmd->scsi_done = done;
 	scmd->result = 0;
 
 	if (MEGASAS_IS_LOGICAL(scmd) &&
-	    (scmd->device->id >= MEGASAS_MAX_LD || scmd->device->lun)) {
+	    (scmd->device->id >= instance->fw_supported_vd_count ||
+		scmd->device->lun)) {
 		scmd->result = DID_BAD_TARGET << 16;
 		goto out_done;
 	}
@@ -1606,12 +1634,10 @@ megasas_queue_command_lck(struct scsi_cmnd *scmd, void (*done) (struct scsi_cmnd
 	return 0;
 
  out_done:
-	done(scmd);
+	scmd->scsi_done(scmd);
 	return 0;
 }
 
-static DEF_SCSI_QCMD(megasas_queue_command)
-
 static struct megasas_instance *megasas_lookup_instance(u16 host_no)
 {
 	int i;
@@ -1628,36 +1654,12 @@ static struct megasas_instance *megasas_lookup_instance(u16 host_no)
 
 static int megasas_slave_configure(struct scsi_device *sdev)
 {
-	u16             pd_index = 0;
-	struct  megasas_instance *instance ;
-
-	instance = megasas_lookup_instance(sdev->host->host_no);
-
-	/*
-	* Don't export physical disk devices to the disk driver.
-	*
-	* FIXME: Currently we don't export them to the midlayer at all.
-	*        That will be fixed once LSI engineers have audited the
-	*        firmware for possible issues.
-	*/
-	if (sdev->channel < MEGASAS_MAX_PD_CHANNELS &&
-				sdev->type == TYPE_DISK) {
-		pd_index = (sdev->channel * MEGASAS_MAX_DEV_PER_CHANNEL) +
-								sdev->id;
-		if (instance->pd_list[pd_index].driveState ==
-						MR_PD_STATE_SYSTEM) {
-			blk_queue_rq_timeout(sdev->request_queue,
-				MEGASAS_DEFAULT_CMD_TIMEOUT * HZ);
-			return 0;
-		}
-		return -ENXIO;
-	}
-
 	/*
 	* The RAID firmware may require extended timeouts.
 	*/
 	blk_queue_rq_timeout(sdev->request_queue,
 		MEGASAS_DEFAULT_CMD_TIMEOUT * HZ);
+
 	return 0;
 }
 
@@ -1666,18 +1668,15 @@ static int megasas_slave_alloc(struct scsi_device *sdev)
 	u16             pd_index = 0;
 	struct megasas_instance *instance ;
 	instance = megasas_lookup_instance(sdev->host->host_no);
-	if ((sdev->channel < MEGASAS_MAX_PD_CHANNELS) &&
-				(sdev->type == TYPE_DISK)) {
+	if (sdev->channel < MEGASAS_MAX_PD_CHANNELS) {
 		/*
 		 * Open the OS scan to the SYSTEM PD
 		 */
 		pd_index =
 			(sdev->channel * MEGASAS_MAX_DEV_PER_CHANNEL) +
 			sdev->id;
-		if ((instance->pd_list[pd_index].driveState ==
-					MR_PD_STATE_SYSTEM) &&
-			(instance->pd_list[pd_index].driveType ==
-						TYPE_DISK)) {
+		if (instance->pd_list[pd_index].driveState ==
+					MR_PD_STATE_SYSTEM) {
 			return 0;
 		}
 		return -ENXIO;
@@ -1825,16 +1824,12 @@ void megasas_do_ocr(struct megasas_instance *instance)
 	process_fw_state_change_wq(&instance->work_init);
 }
 
-/* This function will get the current SR-IOV LD/VF affiliation */
-static int megasas_get_ld_vf_affiliation(struct megasas_instance *instance,
-	int initial)
+static int megasas_get_ld_vf_affiliation_111(struct megasas_instance *instance,
+					    int initial)
 {
 	struct megasas_cmd *cmd;
 	struct megasas_dcmd_frame *dcmd;
-	struct MR_LD_VF_AFFILIATION *new_affiliation = NULL;
 	struct MR_LD_VF_AFFILIATION_111 *new_affiliation_111 = NULL;
-	struct MR_LD_VF_MAP *newmap = NULL, *savedmap = NULL;
-	dma_addr_t new_affiliation_h;
 	dma_addr_t new_affiliation_111_h;
 	int ld, retval = 0;
 	u8 thisVf;
@@ -1842,15 +1837,15 @@ static int megasas_get_ld_vf_affiliation(struct megasas_instance *instance,
 	cmd = megasas_get_cmd(instance);
 
 	if (!cmd) {
-		printk(KERN_DEBUG "megasas: megasas_get_ld_vf_"
-		       "affiliation: Failed to get cmd for scsi%d.\n",
+		printk(KERN_DEBUG "megasas: megasas_get_ld_vf_affiliation_111:"
+		       "Failed to get cmd for scsi%d.\n",
 			instance->host->host_no);
 		return -ENOMEM;
 	}
 
 	dcmd = &cmd->frame->dcmd;
 
-	if (!instance->vf_affiliation && !instance->vf_affiliation_111) {
+	if (!instance->vf_affiliation_111) {
 		printk(KERN_WARNING "megasas: SR-IOV: Couldn't get LD/VF "
 		       "affiliation for scsi%d.\n", instance->host->host_no);
 		megasas_return_cmd(instance, cmd);
@@ -1858,38 +1853,22 @@ static int megasas_get_ld_vf_affiliation(struct megasas_instance *instance,
 	}
 
 	if (initial)
-		if (instance->PlasmaFW111)
 			memset(instance->vf_affiliation_111, 0,
 			       sizeof(struct MR_LD_VF_AFFILIATION_111));
-		else
-			memset(instance->vf_affiliation, 0,
-			       (MAX_LOGICAL_DRIVES + 1) *
-			       sizeof(struct MR_LD_VF_AFFILIATION));
 	else {
-		if (instance->PlasmaFW111)
-			new_affiliation_111 =
-				pci_alloc_consistent(instance->pdev,
-						     sizeof(struct MR_LD_VF_AFFILIATION_111),
-						     &new_affiliation_111_h);
-		else
-			new_affiliation =
-				pci_alloc_consistent(instance->pdev,
-						     (MAX_LOGICAL_DRIVES + 1) *
-						     sizeof(struct MR_LD_VF_AFFILIATION),
-						     &new_affiliation_h);
-		if (!new_affiliation && !new_affiliation_111) {
+		new_affiliation_111 =
+			pci_alloc_consistent(instance->pdev,
+					     sizeof(struct MR_LD_VF_AFFILIATION_111),
+					     &new_affiliation_111_h);
+		if (!new_affiliation_111) {
 			printk(KERN_DEBUG "megasas: SR-IOV: Couldn't allocate "
 			       "memory for new affiliation for scsi%d.\n",
-				instance->host->host_no);
+			       instance->host->host_no);
 			megasas_return_cmd(instance, cmd);
 			return -ENOMEM;
 		}
-		if (instance->PlasmaFW111)
-			memset(new_affiliation_111, 0,
-			       sizeof(struct MR_LD_VF_AFFILIATION_111));
-		else
-			memset(new_affiliation, 0, (MAX_LOGICAL_DRIVES + 1) *
-			       sizeof(struct MR_LD_VF_AFFILIATION));
+		memset(new_affiliation_111, 0,
+		       sizeof(struct MR_LD_VF_AFFILIATION_111));
 	}
 
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
@@ -1900,34 +1879,17 @@ static int megasas_get_ld_vf_affiliation(struct megasas_instance *instance,
 	dcmd->flags = MFI_FRAME_DIR_BOTH;
 	dcmd->timeout = 0;
 	dcmd->pad_0 = 0;
-	if (instance->PlasmaFW111) {
-		dcmd->data_xfer_len = sizeof(struct MR_LD_VF_AFFILIATION_111);
-		dcmd->opcode = MR_DCMD_LD_VF_MAP_GET_ALL_LDS_111;
-	} else {
-		dcmd->data_xfer_len = (MAX_LOGICAL_DRIVES + 1) *
-			sizeof(struct MR_LD_VF_AFFILIATION);
-		dcmd->opcode = MR_DCMD_LD_VF_MAP_GET_ALL_LDS;
-	}
+	dcmd->data_xfer_len = sizeof(struct MR_LD_VF_AFFILIATION_111);
+	dcmd->opcode = MR_DCMD_LD_VF_MAP_GET_ALL_LDS_111;
 
-	if (initial) {
-		if (instance->PlasmaFW111)
-			dcmd->sgl.sge32[0].phys_addr =
-			  instance->vf_affiliation_111_h;
-		else
-			dcmd->sgl.sge32[0].phys_addr =
-			  instance->vf_affiliation_h;
-	} else {
-		if (instance->PlasmaFW111)
-			dcmd->sgl.sge32[0].phys_addr = new_affiliation_111_h;
-		else
-			dcmd->sgl.sge32[0].phys_addr = new_affiliation_h;
-	}
-	if (instance->PlasmaFW111)
-		dcmd->sgl.sge32[0].length =
-		  sizeof(struct MR_LD_VF_AFFILIATION_111);
+	if (initial)
+		dcmd->sgl.sge32[0].phys_addr =
+			instance->vf_affiliation_111_h;
 	else
-		dcmd->sgl.sge32[0].length = (MAX_LOGICAL_DRIVES + 1) *
-			sizeof(struct MR_LD_VF_AFFILIATION);
+		dcmd->sgl.sge32[0].phys_addr = new_affiliation_111_h;
+
+	dcmd->sgl.sge32[0].length =
+		sizeof(struct MR_LD_VF_AFFILIATION_111);
 
 	printk(KERN_WARNING "megasas: SR-IOV: Getting LD/VF affiliation for "
 	       "scsi%d\n", instance->host->host_no);
@@ -1943,80 +1905,222 @@ static int megasas_get_ld_vf_affiliation(struct megasas_instance *instance,
 	}
 
 	if (!initial) {
-		if (instance->PlasmaFW111) {
-			if (!new_affiliation_111->vdCount) {
-				printk(KERN_WARNING "megasas: SR-IOV: Got new "
-				       "LD/VF affiliation for passive path "
+		thisVf = new_affiliation_111->thisVf;
+		for (ld = 0 ; ld < new_affiliation_111->vdCount; ld++)
+			if (instance->vf_affiliation_111->map[ld].policy[thisVf] !=
+			    new_affiliation_111->map[ld].policy[thisVf]) {
+				printk(KERN_WARNING "megasas: SR-IOV: "
+				       "Got new LD/VF affiliation "
 				       "for scsi%d.\n",
-					instance->host->host_no);
-				retval = 1;
-				goto out;
-			}
-			thisVf = new_affiliation_111->thisVf;
-			for (ld = 0 ; ld < new_affiliation_111->vdCount; ld++)
-				if (instance->vf_affiliation_111->map[ld].policy[thisVf] != new_affiliation_111->map[ld].policy[thisVf]) {
-					printk(KERN_WARNING "megasas: SR-IOV: "
-					       "Got new LD/VF affiliation "
-					       "for scsi%d.\n",
-						instance->host->host_no);
-					memcpy(instance->vf_affiliation_111,
-					       new_affiliation_111,
-					       sizeof(struct MR_LD_VF_AFFILIATION_111));
-					retval = 1;
-					goto out;
-				}
-		} else {
-			if (!new_affiliation->ldCount) {
-				printk(KERN_WARNING "megasas: SR-IOV: Got new "
-				       "LD/VF affiliation for passive "
-				       "path for scsi%d.\n",
 				       instance->host->host_no);
+				memcpy(instance->vf_affiliation_111,
+				       new_affiliation_111,
+				       sizeof(struct MR_LD_VF_AFFILIATION_111));
 				retval = 1;
 				goto out;
 			}
-			newmap = new_affiliation->map;
-			savedmap = instance->vf_affiliation->map;
-			thisVf = new_affiliation->thisVf;
-			for (ld = 0 ; ld < new_affiliation->ldCount; ld++) {
-				if (savedmap->policy[thisVf] !=
-				    newmap->policy[thisVf]) {
-					printk(KERN_WARNING "megasas: SR-IOV: "
-					       "Got new LD/VF affiliation "
-					       "for scsi%d.\n",
-						instance->host->host_no);
-					memcpy(instance->vf_affiliation,
-					       new_affiliation,
-					       new_affiliation->size);
-					retval = 1;
-					goto out;
+	}
+out:
+	if (new_affiliation_111) {
+		pci_free_consistent(instance->pdev,
+				    sizeof(struct MR_LD_VF_AFFILIATION_111),
+				    new_affiliation_111,
+				    new_affiliation_111_h);
+	}
+
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
+
+	return retval;
+}
+
+static int megasas_get_ld_vf_affiliation_12(struct megasas_instance *instance,
+					    int initial)
+{
+	struct megasas_cmd *cmd;
+	struct megasas_dcmd_frame *dcmd;
+	struct MR_LD_VF_AFFILIATION *new_affiliation = NULL;
+	struct MR_LD_VF_MAP *newmap = NULL, *savedmap = NULL;
+	dma_addr_t new_affiliation_h;
+	int i, j, retval = 0, found = 0, doscan = 0;
+	u8 thisVf;
+
+	cmd = megasas_get_cmd(instance);
+
+	if (!cmd) {
+		printk(KERN_DEBUG "megasas: megasas_get_ld_vf_affiliation12: "
+		       "Failed to get cmd for scsi%d.\n",
+		       instance->host->host_no);
+		return -ENOMEM;
+	}
+
+	dcmd = &cmd->frame->dcmd;
+
+	if (!instance->vf_affiliation) {
+		printk(KERN_WARNING "megasas: SR-IOV: Couldn't get LD/VF "
+		       "affiliation for scsi%d.\n", instance->host->host_no);
+		megasas_return_cmd(instance, cmd);
+		return -ENOMEM;
+	}
+
+	if (initial)
+		memset(instance->vf_affiliation, 0, (MAX_LOGICAL_DRIVES + 1) *
+		       sizeof(struct MR_LD_VF_AFFILIATION));
+	else {
+		new_affiliation =
+			pci_alloc_consistent(instance->pdev,
+					     (MAX_LOGICAL_DRIVES + 1) *
+					     sizeof(struct MR_LD_VF_AFFILIATION),
+					     &new_affiliation_h);
+		if (!new_affiliation) {
+			printk(KERN_DEBUG "megasas: SR-IOV: Couldn't allocate "
+			       "memory for new affiliation for scsi%d.\n",
+			       instance->host->host_no);
+			megasas_return_cmd(instance, cmd);
+			return -ENOMEM;
+		}
+		memset(new_affiliation, 0, (MAX_LOGICAL_DRIVES + 1) *
+		       sizeof(struct MR_LD_VF_AFFILIATION));
+	}
+
+	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
+
+	dcmd->cmd = MFI_CMD_DCMD;
+	dcmd->cmd_status = 0xFF;
+	dcmd->sge_count = 1;
+	dcmd->flags = MFI_FRAME_DIR_BOTH;
+	dcmd->timeout = 0;
+	dcmd->pad_0 = 0;
+	dcmd->data_xfer_len = (MAX_LOGICAL_DRIVES + 1) *
+		sizeof(struct MR_LD_VF_AFFILIATION);
+	dcmd->opcode = MR_DCMD_LD_VF_MAP_GET_ALL_LDS;
+
+	if (initial)
+		dcmd->sgl.sge32[0].phys_addr = instance->vf_affiliation_h;
+	else
+		dcmd->sgl.sge32[0].phys_addr = new_affiliation_h;
+
+	dcmd->sgl.sge32[0].length = (MAX_LOGICAL_DRIVES + 1) *
+		sizeof(struct MR_LD_VF_AFFILIATION);
+
+	printk(KERN_WARNING "megasas: SR-IOV: Getting LD/VF affiliation for "
+	       "scsi%d\n", instance->host->host_no);
+
+	megasas_issue_blocked_cmd(instance, cmd, 0);
+
+	if (dcmd->cmd_status) {
+		printk(KERN_WARNING "megasas: SR-IOV: LD/VF affiliation DCMD"
+		       " failed with status 0x%x for scsi%d.\n",
+		       dcmd->cmd_status, instance->host->host_no);
+		retval = 1; /* Do a scan if we couldn't get affiliation */
+		goto out;
+	}
+
+	if (!initial) {
+		if (!new_affiliation->ldCount) {
+			printk(KERN_WARNING "megasas: SR-IOV: Got new LD/VF "
+			       "affiliation for passive path for scsi%d.\n",
+			       instance->host->host_no);
+			retval = 1;
+			goto out;
+		}
+		newmap = new_affiliation->map;
+		savedmap = instance->vf_affiliation->map;
+		thisVf = new_affiliation->thisVf;
+		for (i = 0 ; i < new_affiliation->ldCount; i++) {
+			found = 0;
+			for (j = 0; j < instance->vf_affiliation->ldCount;
+			     j++) {
+				if (newmap->ref.targetId ==
+				    savedmap->ref.targetId) {
+					found = 1;
+					if (newmap->policy[thisVf] !=
+					    savedmap->policy[thisVf]) {
+						doscan = 1;
+						goto out;
+					}
 				}
 				savedmap = (struct MR_LD_VF_MAP *)
 					((unsigned char *)savedmap +
 					 savedmap->size);
+			}
+			if (!found && newmap->policy[thisVf] !=
+			    MR_LD_ACCESS_HIDDEN) {
+				doscan = 1;
+				goto out;
+			}
+			newmap = (struct MR_LD_VF_MAP *)
+				((unsigned char *)newmap + newmap->size);
+		}
+
+		newmap = new_affiliation->map;
+		savedmap = instance->vf_affiliation->map;
+
+		for (i = 0 ; i < instance->vf_affiliation->ldCount; i++) {
+			found = 0;
+			for (j = 0 ; j < new_affiliation->ldCount; j++) {
+				if (savedmap->ref.targetId ==
+				    newmap->ref.targetId) {
+					found = 1;
+					if (savedmap->policy[thisVf] !=
+					    newmap->policy[thisVf]) {
+						doscan = 1;
+						goto out;
+					}
+				}
 				newmap = (struct MR_LD_VF_MAP *)
 					((unsigned char *)newmap +
 					 newmap->size);
 			}
+			if (!found && savedmap->policy[thisVf] !=
+			    MR_LD_ACCESS_HIDDEN) {
+				doscan = 1;
+				goto out;
+			}
+			savedmap = (struct MR_LD_VF_MAP *)
+				((unsigned char *)savedmap +
+				 savedmap->size);
 		}
 	}
 out:
-	if (new_affiliation) {
-		if (instance->PlasmaFW111)
-			pci_free_consistent(instance->pdev,
-					    sizeof(struct MR_LD_VF_AFFILIATION_111),
-					    new_affiliation_111,
-					    new_affiliation_111_h);
-		else
-			pci_free_consistent(instance->pdev,
-					    (MAX_LOGICAL_DRIVES + 1) *
-					    sizeof(struct MR_LD_VF_AFFILIATION),
-					    new_affiliation, new_affiliation_h);
+	if (doscan) {
+		printk(KERN_WARNING "megasas: SR-IOV: Got new LD/VF "
+		       "affiliation for scsi%d.\n", instance->host->host_no);
+		memcpy(instance->vf_affiliation, new_affiliation,
+		       new_affiliation->size);
+		retval = 1;
 	}
-	megasas_return_cmd(instance, cmd);
+
+	if (new_affiliation)
+		pci_free_consistent(instance->pdev,
+				    (MAX_LOGICAL_DRIVES + 1) *
+				    sizeof(struct MR_LD_VF_AFFILIATION),
+				    new_affiliation, new_affiliation_h);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	return retval;
 }
 
+/* This function will get the current SR-IOV LD/VF affiliation */
+static int megasas_get_ld_vf_affiliation(struct megasas_instance *instance,
+	int initial)
+{
+	int retval;
+
+	if (instance->PlasmaFW111)
+		retval = megasas_get_ld_vf_affiliation_111(instance, initial);
+	else
+		retval = megasas_get_ld_vf_affiliation_12(instance, initial);
+	return retval;
+}
+
 /* This function will tell FW to start the SR-IOV heartbeat */
 int megasas_sriov_start_heartbeat(struct megasas_instance *instance,
 					 int initial)
@@ -2459,7 +2563,12 @@ megasas_service_aen(struct megasas_instance *instance, struct megasas_cmd *cmd)
 		cmd->abort_aen = 0;
 
 	instance->aen_cmd = NULL;
-	megasas_return_cmd(instance, cmd);
+
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	if ((instance->unload == 0) &&
 		((instance->issuepend_done == 1))) {
@@ -2491,6 +2600,152 @@ static int megasas_change_queue_depth(struct scsi_device *sdev,
 	return queue_depth;
 }
 
+static ssize_t
+megasas_fw_crash_buffer_store(struct device *cdev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct Scsi_Host *shost = class_to_shost(cdev);
+	struct megasas_instance *instance =
+		(struct megasas_instance *) shost->hostdata;
+	int val = 0;
+	unsigned long flags;
+
+	if (kstrtoint(buf, 0, &val) != 0)
+		return -EINVAL;
+
+	spin_lock_irqsave(&instance->crashdump_lock, flags);
+	instance->fw_crash_buffer_offset = val;
+	spin_unlock_irqrestore(&instance->crashdump_lock, flags);
+	return strlen(buf);
+}
+
+static ssize_t
+megasas_fw_crash_buffer_show(struct device *cdev,
+	struct device_attribute *attr, char *buf)
+{
+	struct Scsi_Host *shost = class_to_shost(cdev);
+	struct megasas_instance *instance =
+		(struct megasas_instance *) shost->hostdata;
+	u32 size;
+	unsigned long buff_addr;
+	unsigned long dmachunk = CRASH_DMA_BUF_SIZE;
+	unsigned long src_addr;
+	unsigned long flags;
+	u32 buff_offset;
+
+	spin_lock_irqsave(&instance->crashdump_lock, flags);
+	buff_offset = instance->fw_crash_buffer_offset;
+	if (!instance->crash_dump_buf &&
+		!((instance->fw_crash_state == AVAILABLE) ||
+		(instance->fw_crash_state == COPYING))) {
+		dev_err(&instance->pdev->dev,
+			"Firmware crash dump is not available\n");
+		spin_unlock_irqrestore(&instance->crashdump_lock, flags);
+		return -EINVAL;
+	}
+
+	buff_addr = (unsigned long) buf;
+
+	if (buff_offset >
+		(instance->fw_crash_buffer_size * dmachunk)) {
+		dev_err(&instance->pdev->dev,
+			"Firmware crash dump offset is out of range\n");
+		spin_unlock_irqrestore(&instance->crashdump_lock, flags);
+		return 0;
+	}
+
+	size = (instance->fw_crash_buffer_size * dmachunk) - buff_offset;
+	size = (size >= PAGE_SIZE) ? (PAGE_SIZE - 1) : size;
+
+	src_addr = (unsigned long)instance->crash_buf[buff_offset / dmachunk] +
+		(buff_offset % dmachunk);
+	memcpy(buf, (void *)src_addr,  size);
+	spin_unlock_irqrestore(&instance->crashdump_lock, flags);
+
+	return size;
+}
+
+static ssize_t
+megasas_fw_crash_buffer_size_show(struct device *cdev,
+	struct device_attribute *attr, char *buf)
+{
+	struct Scsi_Host *shost = class_to_shost(cdev);
+	struct megasas_instance *instance =
+		(struct megasas_instance *) shost->hostdata;
+
+	return snprintf(buf, PAGE_SIZE, "%ld\n", (unsigned long)
+		((instance->fw_crash_buffer_size) * 1024 * 1024)/PAGE_SIZE);
+}
+
+static ssize_t
+megasas_fw_crash_state_store(struct device *cdev,
+	struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct Scsi_Host *shost = class_to_shost(cdev);
+	struct megasas_instance *instance =
+		(struct megasas_instance *) shost->hostdata;
+	int val = 0;
+	unsigned long flags;
+
+	if (kstrtoint(buf, 0, &val) != 0)
+		return -EINVAL;
+
+	if ((val <= AVAILABLE || val > COPY_ERROR)) {
+		dev_err(&instance->pdev->dev, "application updates invalid "
+			"firmware crash state\n");
+		return -EINVAL;
+	}
+
+	instance->fw_crash_state = val;
+
+	if ((val == COPIED) || (val == COPY_ERROR)) {
+		spin_lock_irqsave(&instance->crashdump_lock, flags);
+		megasas_free_host_crash_buffer(instance);
+		spin_unlock_irqrestore(&instance->crashdump_lock, flags);
+		if (val == COPY_ERROR)
+			dev_info(&instance->pdev->dev, "application failed to "
+				"copy Firmware crash dump\n");
+		else
+			dev_info(&instance->pdev->dev, "Firmware crash dump "
+				"copied successfully\n");
+	}
+	return strlen(buf);
+}
+
+static ssize_t
+megasas_fw_crash_state_show(struct device *cdev,
+	struct device_attribute *attr, char *buf)
+{
+	struct Scsi_Host *shost = class_to_shost(cdev);
+	struct megasas_instance *instance =
+		(struct megasas_instance *) shost->hostdata;
+	return snprintf(buf, PAGE_SIZE, "%d\n", instance->fw_crash_state);
+}
+
+static ssize_t
+megasas_page_size_show(struct device *cdev,
+	struct device_attribute *attr, char *buf)
+{
+	return snprintf(buf, PAGE_SIZE, "%ld\n", (unsigned long)PAGE_SIZE - 1);
+}
+
+static DEVICE_ATTR(fw_crash_buffer, S_IRUGO | S_IWUSR,
+	megasas_fw_crash_buffer_show, megasas_fw_crash_buffer_store);
+static DEVICE_ATTR(fw_crash_buffer_size, S_IRUGO,
+	megasas_fw_crash_buffer_size_show, NULL);
+static DEVICE_ATTR(fw_crash_state, S_IRUGO | S_IWUSR,
+	megasas_fw_crash_state_show, megasas_fw_crash_state_store);
+static DEVICE_ATTR(page_size, S_IRUGO,
+	megasas_page_size_show, NULL);
+
+struct device_attribute *megaraid_host_attrs[] = {
+	&dev_attr_fw_crash_buffer_size,
+	&dev_attr_fw_crash_buffer,
+	&dev_attr_fw_crash_state,
+	&dev_attr_page_size,
+	NULL,
+};
+
 /*
  * Scsi host template for megaraid_sas driver
  */
@@ -2506,6 +2761,7 @@ static struct scsi_host_template megasas_template = {
 	.eh_bus_reset_handler = megasas_reset_bus_host,
 	.eh_host_reset_handler = megasas_reset_bus_host,
 	.eh_timed_out = megasas_reset_timer,
+	.shost_attrs = megaraid_host_attrs,
 	.bios_param = megasas_bios_param,
 	.use_clustering = ENABLE_CLUSTERING,
 	.change_queue_depth = megasas_change_queue_depth,
@@ -2688,7 +2944,8 @@ megasas_complete_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd,
 					       "failed, status = 0x%x.\n",
 					       cmd->frame->hdr.cmd_status);
 				else {
-					megasas_return_cmd(instance, cmd);
+					megasas_return_mfi_mpt_pthr(instance,
+						cmd, cmd->mpt_pthr_cmd_blocked);
 					spin_unlock_irqrestore(
 						instance->host->host_lock,
 						flags);
@@ -2696,7 +2953,8 @@ megasas_complete_cmd(struct megasas_instance *instance, struct megasas_cmd *cmd,
 				}
 			} else
 				instance->map_id++;
-			megasas_return_cmd(instance, cmd);
+			megasas_return_mfi_mpt_pthr(instance, cmd,
+				cmd->mpt_pthr_cmd_blocked);
 
 			/*
 			 * Set fast path IO to ZERO.
@@ -2852,7 +3110,7 @@ megasas_internal_reset_defer_cmds(struct megasas_instance *instance)
 	unsigned long flags;
 
 	defer_index     = 0;
-	spin_lock_irqsave(&instance->cmd_pool_lock, flags);
+	spin_lock_irqsave(&instance->mfi_pool_lock, flags);
 	for (i = 0; i < max_cmd; i++) {
 		cmd = instance->cmd_list[i];
 		if (cmd->sync_cmd == 1 || cmd->scmd) {
@@ -2873,7 +3131,7 @@ megasas_internal_reset_defer_cmds(struct megasas_instance *instance)
 				&instance->internal_reset_pending_q);
 		}
 	}
-	spin_unlock_irqrestore(&instance->cmd_pool_lock, flags);
+	spin_unlock_irqrestore(&instance->mfi_pool_lock, flags);
 }
 
 
@@ -3438,7 +3696,9 @@ int megasas_alloc_cmds(struct megasas_instance *instance)
 	int j;
 	u32 max_cmd;
 	struct megasas_cmd *cmd;
+	struct fusion_context *fusion;
 
+	fusion = instance->ctrl_context;
 	max_cmd = instance->max_mfi_cmds;
 
 	/*
@@ -3471,13 +3731,11 @@ int megasas_alloc_cmds(struct megasas_instance *instance)
 		}
 	}
 
-	/*
-	 * Add all the commands to command pool (instance->cmd_pool)
-	 */
 	for (i = 0; i < max_cmd; i++) {
 		cmd = instance->cmd_list[i];
 		memset(cmd, 0, sizeof(struct megasas_cmd));
 		cmd->index = i;
+		atomic_set(&cmd->mfi_mpt_pthr, MFI_LIST_ADDED);
 		cmd->scmd = NULL;
 		cmd->instance = instance;
 
@@ -3548,11 +3806,11 @@ megasas_get_pd_list(struct megasas_instance *instance)
 	dcmd->sgl.sge32[0].phys_addr = cpu_to_le32(ci_h);
 	dcmd->sgl.sge32[0].length = cpu_to_le32(MEGASAS_MAX_PD * sizeof(struct MR_PD_LIST));
 
-	if (!megasas_issue_polled(instance, cmd)) {
-		ret = 0;
-	} else {
-		ret = -1;
-	}
+	if (instance->ctrl_context && !instance->mask_interrupts)
+		ret = megasas_issue_blocked_cmd(instance, cmd,
+			MEGASAS_BLOCKED_CMD_TIMEOUT);
+	else
+		ret = megasas_issue_polled(instance, cmd);
 
 	/*
 	* the following function will get the instance PD LIST.
@@ -3584,7 +3842,12 @@ megasas_get_pd_list(struct megasas_instance *instance)
 	pci_free_consistent(instance->pdev,
 				MEGASAS_MAX_PD * sizeof(struct MR_PD_LIST),
 				ci, ci_h);
-	megasas_return_cmd(instance, cmd);
+
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	return ret;
 }
@@ -3630,6 +3893,8 @@ megasas_get_ld_list(struct megasas_instance *instance)
 	memset(ci, 0, sizeof(*ci));
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
 
+	if (instance->supportmax256vd)
+		dcmd->mbox.b[0] = 1;
 	dcmd->cmd = MFI_CMD_DCMD;
 	dcmd->cmd_status = 0xFF;
 	dcmd->sge_count = 1;
@@ -3641,18 +3906,19 @@ megasas_get_ld_list(struct megasas_instance *instance)
 	dcmd->sgl.sge32[0].length = cpu_to_le32(sizeof(struct MR_LD_LIST));
 	dcmd->pad_0  = 0;
 
-	if (!megasas_issue_polled(instance, cmd)) {
-		ret = 0;
-	} else {
-		ret = -1;
-	}
+	if (instance->ctrl_context && !instance->mask_interrupts)
+		ret = megasas_issue_blocked_cmd(instance, cmd,
+			MEGASAS_BLOCKED_CMD_TIMEOUT);
+	else
+		ret = megasas_issue_polled(instance, cmd);
+
 
 	ld_count = le32_to_cpu(ci->ldCount);
 
 	/* the following function will get the instance PD LIST */
 
-	if ((ret == 0) && (ld_count <= MAX_LOGICAL_DRIVES)) {
-		memset(instance->ld_ids, 0xff, MEGASAS_MAX_LD_IDS);
+	if ((ret == 0) && (ld_count <= instance->fw_supported_vd_count)) {
+		memset(instance->ld_ids, 0xff, MAX_LOGICAL_DRIVES_EXT);
 
 		for (ld_index = 0; ld_index < ld_count; ld_index++) {
 			if (ci->ldList[ld_index].state != 0) {
@@ -3668,7 +3934,11 @@ megasas_get_ld_list(struct megasas_instance *instance)
 				ci,
 				ci_h);
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 	return ret;
 }
 
@@ -3715,6 +3985,8 @@ megasas_ld_list_query(struct megasas_instance *instance, u8 query_type)
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
 
 	dcmd->mbox.b[0] = query_type;
+	if (instance->supportmax256vd)
+		dcmd->mbox.b[2] = 1;
 
 	dcmd->cmd = MFI_CMD_DCMD;
 	dcmd->cmd_status = 0xFF;
@@ -3727,16 +3999,15 @@ megasas_ld_list_query(struct megasas_instance *instance, u8 query_type)
 	dcmd->sgl.sge32[0].length = cpu_to_le32(sizeof(struct MR_LD_TARGETID_LIST));
 	dcmd->pad_0  = 0;
 
-	if (!megasas_issue_polled(instance, cmd) && !dcmd->cmd_status) {
-		ret = 0;
-	} else {
-		/* On failure, call older LD list DCMD */
-		ret = 1;
-	}
+	if (instance->ctrl_context && !instance->mask_interrupts)
+		ret = megasas_issue_blocked_cmd(instance, cmd,
+			MEGASAS_BLOCKED_CMD_TIMEOUT);
+	else
+		ret = megasas_issue_polled(instance, cmd);
 
 	tgtid_count = le32_to_cpu(ci->count);
 
-	if ((ret == 0) && (tgtid_count <= (MAX_LOGICAL_DRIVES))) {
+	if ((ret == 0) && (tgtid_count <= (instance->fw_supported_vd_count))) {
 		memset(instance->ld_ids, 0xff, MEGASAS_MAX_LD_IDS);
 		for (ld_index = 0; ld_index < tgtid_count; ld_index++) {
 			ids = ci->targetId[ld_index];
@@ -3748,7 +4019,11 @@ megasas_ld_list_query(struct megasas_instance *instance, u8 query_type)
 	pci_free_consistent(instance->pdev, sizeof(struct MR_LD_TARGETID_LIST),
 			    ci, ci_h);
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	return ret;
 }
@@ -3762,7 +4037,7 @@ megasas_ld_list_query(struct megasas_instance *instance, u8 query_type)
  * This information is mainly used to find out the maximum IO transfer per
  * command supported by the FW.
  */
-static int
+int
 megasas_get_ctrl_info(struct megasas_instance *instance,
 		      struct megasas_ctrl_info *ctrl_info)
 {
@@ -3803,18 +4078,84 @@ megasas_get_ctrl_info(struct megasas_instance *instance,
 	dcmd->opcode = cpu_to_le32(MR_DCMD_CTRL_GET_INFO);
 	dcmd->sgl.sge32[0].phys_addr = cpu_to_le32(ci_h);
 	dcmd->sgl.sge32[0].length = cpu_to_le32(sizeof(struct megasas_ctrl_info));
+	dcmd->mbox.b[0] = 1;
 
-	if (!megasas_issue_polled(instance, cmd)) {
-		ret = 0;
+	if (instance->ctrl_context && !instance->mask_interrupts)
+		ret = megasas_issue_blocked_cmd(instance, cmd,
+			MEGASAS_BLOCKED_CMD_TIMEOUT);
+	else
+		ret = megasas_issue_polled(instance, cmd);
+
+	if (!ret)
 		memcpy(ctrl_info, ci, sizeof(struct megasas_ctrl_info));
-	} else {
-		ret = -1;
-	}
 
 	pci_free_consistent(instance->pdev, sizeof(struct megasas_ctrl_info),
 			    ci, ci_h);
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
+	return ret;
+}
+
+/*
+ * megasas_set_crash_dump_params -	Sends address of crash dump DMA buffer
+ *					to firmware
+ *
+ * @instance:				Adapter soft state
+ * @crash_buf_state		-	tell FW to turn ON/OFF crash dump feature
+					MR_CRASH_BUF_TURN_OFF = 0
+					MR_CRASH_BUF_TURN_ON = 1
+ * @return 0 on success non-zero on failure.
+ * Issues an internal command (DCMD) to set parameters for crash dump feature.
+ * Driver will send address of crash dump DMA buffer and set mbox to tell FW
+ * that driver supports crash dump feature. This DCMD will be sent only if
+ * crash dump feature is supported by the FW.
+ *
+ */
+int megasas_set_crash_dump_params(struct megasas_instance *instance,
+	u8 crash_buf_state)
+{
+	int ret = 0;
+	struct megasas_cmd *cmd;
+	struct megasas_dcmd_frame *dcmd;
+
+	cmd = megasas_get_cmd(instance);
+
+	if (!cmd) {
+		dev_err(&instance->pdev->dev, "Failed to get a free cmd\n");
+		return -ENOMEM;
+	}
+
+
+	dcmd = &cmd->frame->dcmd;
+
+	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
+	dcmd->mbox.b[0] = crash_buf_state;
+	dcmd->cmd = MFI_CMD_DCMD;
+	dcmd->cmd_status = 0xFF;
+	dcmd->sge_count = 1;
+	dcmd->flags = cpu_to_le16(MFI_FRAME_DIR_NONE);
+	dcmd->timeout = 0;
+	dcmd->pad_0 = 0;
+	dcmd->data_xfer_len = cpu_to_le32(CRASH_DMA_BUF_SIZE);
+	dcmd->opcode = cpu_to_le32(MR_DCMD_CTRL_SET_CRASH_DUMP_PARAMS);
+	dcmd->sgl.sge32[0].phys_addr = cpu_to_le32(instance->crash_dump_h);
+	dcmd->sgl.sge32[0].length = cpu_to_le32(CRASH_DMA_BUF_SIZE);
+
+	if (instance->ctrl_context && !instance->mask_interrupts)
+		ret = megasas_issue_blocked_cmd(instance, cmd,
+			MEGASAS_BLOCKED_CMD_TIMEOUT);
+	else
+		ret = megasas_issue_polled(instance, cmd);
+
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 	return ret;
 }
 
@@ -3948,6 +4289,13 @@ megasas_init_adapter_mfi(struct megasas_instance *instance)
 	if (megasas_issue_init_mfi(instance))
 		goto fail_fw_init;
 
+	if (megasas_get_ctrl_info(instance, instance->ctrl_info)) {
+		dev_err(&instance->pdev->dev, "(%d): Could get controller info "
+			"Fail from %s %d\n", instance->unique_id,
+			__func__, __LINE__);
+		goto fail_fw_init;
+	}
+
 	instance->fw_support_ieee = 0;
 	instance->fw_support_ieee =
 		(instance->instancet->read_fw_status_reg(reg_set) &
@@ -3986,7 +4334,7 @@ static int megasas_init_fw(struct megasas_instance *instance)
 	u32 tmp_sectors, msix_enable, scratch_pad_2;
 	resource_size_t base_addr;
 	struct megasas_register_set __iomem *reg_set;
-	struct megasas_ctrl_info *ctrl_info;
+	struct megasas_ctrl_info *ctrl_info = NULL;
 	unsigned long bar_list;
 	int i, loop, fw_msix_count = 0;
 	struct IOV_111 *iovPtr;
@@ -4103,17 +4451,11 @@ static int megasas_init_fw(struct megasas_instance *instance)
 					     (unsigned int)num_online_cpus());
 		for (i = 0; i < instance->msix_vectors; i++)
 			instance->msixentry[i].entry = i;
-		i = pci_enable_msix(instance->pdev, instance->msixentry,
-				    instance->msix_vectors);
-		if (i >= 0) {
-			if (i) {
-				if (!pci_enable_msix(instance->pdev,
-						     instance->msixentry, i))
-					instance->msix_vectors = i;
-				else
-					instance->msix_vectors = 0;
-			}
-		} else
+		i = pci_enable_msix_range(instance->pdev, instance->msixentry,
+					  1, instance->msix_vectors);
+		if (i)
+			instance->msix_vectors = i;
+		else
 			instance->msix_vectors = 0;
 
 		dev_info(&instance->pdev->dev, "[scsi%d]: FW supports"
@@ -4123,6 +4465,17 @@ static int megasas_init_fw(struct megasas_instance *instance)
 			instance->msix_vectors);
 	}
 
+	instance->ctrl_info = kzalloc(sizeof(struct megasas_ctrl_info),
+				GFP_KERNEL);
+	if (instance->ctrl_info == NULL)
+		goto fail_init_adapter;
+
+	/*
+	 * Below are default value for legacy Firmware.
+	 * non-fusion based controllers
+	 */
+	instance->fw_supported_vd_count = MAX_LOGICAL_DRIVES;
+	instance->fw_supported_pd_count = MAX_PHYSICAL_DEVICES;
 	/* Get operational params, sge flags, send init cmd to controller */
 	if (instance->instancet->init_adapter(instance))
 		goto fail_init_adapter;
@@ -4145,8 +4498,6 @@ static int megasas_init_fw(struct megasas_instance *instance)
 				  MR_LD_QUERY_TYPE_EXPOSED_TO_HOST))
 		megasas_get_ld_list(instance);
 
-	ctrl_info = kmalloc(sizeof(struct megasas_ctrl_info), GFP_KERNEL);
-
 	/*
 	 * Compute the max allowed sectors per IO: The controller info has two
 	 * limits on max sectors. Driver should use the minimum of these two.
@@ -4157,58 +4508,79 @@ static int megasas_init_fw(struct megasas_instance *instance)
 	 * to calculate max_sectors_1. So the number ended up as zero always.
 	 */
 	tmp_sectors = 0;
-	if (ctrl_info && !megasas_get_ctrl_info(instance, ctrl_info)) {
+	ctrl_info = instance->ctrl_info;
 
-		max_sectors_1 = (1 << ctrl_info->stripe_sz_ops.min) *
-			le16_to_cpu(ctrl_info->max_strips_per_io);
-		max_sectors_2 = le32_to_cpu(ctrl_info->max_request_size);
+	max_sectors_1 = (1 << ctrl_info->stripe_sz_ops.min) *
+		le16_to_cpu(ctrl_info->max_strips_per_io);
+	max_sectors_2 = le32_to_cpu(ctrl_info->max_request_size);
 
-		tmp_sectors = min_t(u32, max_sectors_1 , max_sectors_2);
+	tmp_sectors = min_t(u32, max_sectors_1 , max_sectors_2);
 
-		/*Check whether controller is iMR or MR */
-		if (ctrl_info->memory_size) {
-			instance->is_imr = 0;
-			dev_info(&instance->pdev->dev, "Controller type: MR,"
-				"Memory size is: %dMB\n",
-				le16_to_cpu(ctrl_info->memory_size));
-		} else {
-			instance->is_imr = 1;
-			dev_info(&instance->pdev->dev,
-				"Controller type: iMR\n");
-		}
-		/* OnOffProperties are converted into CPU arch*/
-		le32_to_cpus((u32 *)&ctrl_info->properties.OnOffProperties);
-		instance->disableOnlineCtrlReset =
-		ctrl_info->properties.OnOffProperties.disableOnlineCtrlReset;
-		/* adapterOperations2 are converted into CPU arch*/
-		le32_to_cpus((u32 *)&ctrl_info->adapterOperations2);
-		instance->mpio = ctrl_info->adapterOperations2.mpio;
-		instance->UnevenSpanSupport =
-			ctrl_info->adapterOperations2.supportUnevenSpans;
-		if (instance->UnevenSpanSupport) {
-			struct fusion_context *fusion = instance->ctrl_context;
-			dev_info(&instance->pdev->dev, "FW supports: "
-			"UnevenSpanSupport=%x\n", instance->UnevenSpanSupport);
-			if (MR_ValidateMapInfo(instance))
-				fusion->fast_path_io = 1;
-			else
-				fusion->fast_path_io = 0;
+	/*Check whether controller is iMR or MR */
+	if (ctrl_info->memory_size) {
+		instance->is_imr = 0;
+		dev_info(&instance->pdev->dev, "Controller type: MR,"
+			"Memory size is: %dMB\n",
+			le16_to_cpu(ctrl_info->memory_size));
+	} else {
+		instance->is_imr = 1;
+		dev_info(&instance->pdev->dev,
+			"Controller type: iMR\n");
+	}
+	/* OnOffProperties are converted into CPU arch*/
+	le32_to_cpus((u32 *)&ctrl_info->properties.OnOffProperties);
+	instance->disableOnlineCtrlReset =
+	ctrl_info->properties.OnOffProperties.disableOnlineCtrlReset;
+	/* adapterOperations2 are converted into CPU arch*/
+	le32_to_cpus((u32 *)&ctrl_info->adapterOperations2);
+	instance->mpio = ctrl_info->adapterOperations2.mpio;
+	instance->UnevenSpanSupport =
+		ctrl_info->adapterOperations2.supportUnevenSpans;
+	if (instance->UnevenSpanSupport) {
+		struct fusion_context *fusion = instance->ctrl_context;
+
+		dev_info(&instance->pdev->dev, "FW supports: "
+		"UnevenSpanSupport=%x\n", instance->UnevenSpanSupport);
+		if (MR_ValidateMapInfo(instance))
+			fusion->fast_path_io = 1;
+		else
+			fusion->fast_path_io = 0;
 
+	}
+	if (ctrl_info->host_interface.SRIOV) {
+		if (!ctrl_info->adapterOperations2.activePassive)
+			instance->PlasmaFW111 = 1;
+
+		if (!instance->PlasmaFW111)
+			instance->requestorId =
+				ctrl_info->iov.requestorId;
+		else {
+			iovPtr = (struct IOV_111 *)((unsigned char *)ctrl_info + IOV_111_OFFSET);
+			instance->requestorId = iovPtr->requestorId;
 		}
-		if (ctrl_info->host_interface.SRIOV) {
-			if (!ctrl_info->adapterOperations2.activePassive)
-				instance->PlasmaFW111 = 1;
-
-			if (!instance->PlasmaFW111)
-				instance->requestorId =
-					ctrl_info->iov.requestorId;
-			else {
-				iovPtr = (struct IOV_111 *)((unsigned char *)ctrl_info + IOV_111_OFFSET);
-				instance->requestorId = iovPtr->requestorId;
-			}
-			printk(KERN_WARNING "megaraid_sas: I am VF "
-			       "requestorId %d\n", instance->requestorId);
-		}
+		dev_warn(&instance->pdev->dev, "I am VF "
+		       "requestorId %d\n", instance->requestorId);
+	}
+
+	le32_to_cpus((u32 *)&ctrl_info->adapterOperations3);
+	instance->crash_dump_fw_support =
+		ctrl_info->adapterOperations3.supportCrashDump;
+	instance->crash_dump_drv_support =
+		(instance->crash_dump_fw_support &&
+		instance->crash_dump_buf);
+	if (instance->crash_dump_drv_support) {
+		dev_info(&instance->pdev->dev, "Firmware Crash dump "
+			"feature is supported\n");
+		megasas_set_crash_dump_params(instance,
+			MR_CRASH_BUF_TURN_OFF);
+
+	} else {
+		if (instance->crash_dump_buf)
+			pci_free_consistent(instance->pdev,
+				CRASH_DMA_BUF_SIZE,
+				instance->crash_dump_buf,
+				instance->crash_dump_h);
+		instance->crash_dump_buf = NULL;
 	}
 	instance->max_sectors_per_req = instance->max_num_sge *
 						PAGE_SIZE / 512;
@@ -4256,6 +4628,8 @@ static int megasas_init_fw(struct megasas_instance *instance)
 
 fail_init_adapter:
 fail_ready_state:
+	kfree(instance->ctrl_info);
+	instance->ctrl_info = NULL;
 	iounmap(instance->reg_set);
 
       fail_ioremap:
@@ -4351,7 +4725,11 @@ megasas_get_seq_num(struct megasas_instance *instance,
 	pci_free_consistent(instance->pdev, sizeof(struct megasas_evt_log_info),
 			    el_info, el_info_h);
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	return 0;
 }
@@ -4634,6 +5012,7 @@ static int megasas_probe_one(struct pci_dev *pdev,
 	struct Scsi_Host *host;
 	struct megasas_instance *instance;
 	u16 control = 0;
+	struct fusion_context *fusion = NULL;
 
 	/* Reset MSI-X in the kdump kernel */
 	if (reset_devices) {
@@ -4694,10 +5073,10 @@ static int megasas_probe_one(struct pci_dev *pdev,
 	case PCI_DEVICE_ID_LSI_INVADER:
 	case PCI_DEVICE_ID_LSI_FURY:
 	{
-		struct fusion_context *fusion;
-
-		instance->ctrl_context =
-			kzalloc(sizeof(struct fusion_context), GFP_KERNEL);
+		instance->ctrl_context_pages =
+			get_order(sizeof(struct fusion_context));
+		instance->ctrl_context = (void *)__get_free_pages(GFP_KERNEL,
+				instance->ctrl_context_pages);
 		if (!instance->ctrl_context) {
 			printk(KERN_DEBUG "megasas: Failed to allocate "
 			       "memory for Fusion context info\n");
@@ -4705,7 +5084,9 @@ static int megasas_probe_one(struct pci_dev *pdev,
 		}
 		fusion = instance->ctrl_context;
 		INIT_LIST_HEAD(&fusion->cmd_pool);
-		spin_lock_init(&fusion->cmd_pool_lock);
+		spin_lock_init(&fusion->mpt_pool_lock);
+		memset(fusion->load_balance_info, 0,
+			sizeof(struct LD_LOAD_BALANCE_INFO) * MAX_LOGICAL_DRIVES_EXT);
 	}
 	break;
 	default: /* For all other supported controllers */
@@ -4728,13 +5109,29 @@ static int megasas_probe_one(struct pci_dev *pdev,
 		break;
 	}
 
+	/* Crash dump feature related initialisation*/
+	instance->drv_buf_index = 0;
+	instance->drv_buf_alloc = 0;
+	instance->crash_dump_fw_support = 0;
+	instance->crash_dump_app_support = 0;
+	instance->fw_crash_state = UNAVAILABLE;
+	spin_lock_init(&instance->crashdump_lock);
+	instance->crash_dump_buf = NULL;
+
+	if (!reset_devices)
+		instance->crash_dump_buf = pci_alloc_consistent(pdev,
+						CRASH_DMA_BUF_SIZE,
+						&instance->crash_dump_h);
+	if (!instance->crash_dump_buf)
+		dev_err(&instance->pdev->dev, "Can't allocate Firmware "
+			"crash dump DMA buffer\n");
+
 	megasas_poll_wait_aen = 0;
 	instance->flag_ieee = 0;
 	instance->ev = NULL;
 	instance->issuepend_done = 1;
 	instance->adprecovery = MEGASAS_HBA_OPERATIONAL;
 	instance->is_imr = 0;
-	megasas_poll_wait_aen = 0;
 
 	instance->evt_detail = pci_alloc_consistent(pdev,
 						    sizeof(struct
@@ -4758,7 +5155,7 @@ static int megasas_probe_one(struct pci_dev *pdev,
 	init_waitqueue_head(&instance->int_cmd_wait_q);
 	init_waitqueue_head(&instance->abort_cmd_wait_q);
 
-	spin_lock_init(&instance->cmd_pool_lock);
+	spin_lock_init(&instance->mfi_pool_lock);
 	spin_lock_init(&instance->hba_lock);
 	spin_lock_init(&instance->completion_lock);
 
@@ -4771,13 +5168,14 @@ static int megasas_probe_one(struct pci_dev *pdev,
 	instance->host = host;
 	instance->unique_id = pdev->bus->number << 8 | pdev->devfn;
 	instance->init_id = MEGASAS_DEFAULT_INIT_ID;
+	instance->ctrl_info = NULL;
 
 	if ((instance->pdev->device == PCI_DEVICE_ID_LSI_SAS0073SKINNY) ||
 		(instance->pdev->device == PCI_DEVICE_ID_LSI_SAS0071SKINNY)) {
 		instance->flag_ieee = 1;
 		sema_init(&instance->ioctl_sem, MEGASAS_SKINNY_INT_CMDS);
 	} else
-		sema_init(&instance->ioctl_sem, MEGASAS_INT_CMDS);
+		sema_init(&instance->ioctl_sem, (MEGASAS_INT_CMDS - 5));
 
 	megasas_dbg_lvl = 0;
 	instance->flag = 0;
@@ -4789,9 +5187,10 @@ static int megasas_probe_one(struct pci_dev *pdev,
 	if ((instance->pdev->device == PCI_DEVICE_ID_LSI_FUSION) ||
 	    (instance->pdev->device == PCI_DEVICE_ID_LSI_PLASMA) ||
 	    (instance->pdev->device == PCI_DEVICE_ID_LSI_INVADER) ||
-	    (instance->pdev->device == PCI_DEVICE_ID_LSI_FURY))
+	    (instance->pdev->device == PCI_DEVICE_ID_LSI_FURY)) {
 		INIT_WORK(&instance->work_init, megasas_fusion_ocr_wq);
-	else
+		INIT_WORK(&instance->crash_init, megasas_fusion_crash_dump_wq);
+	} else
 		INIT_WORK(&instance->work_init, process_fw_state_change_wq);
 
 	/*
@@ -4836,8 +5235,9 @@ retry_irq_register:
 				printk(KERN_DEBUG "megasas: Failed to "
 				       "register IRQ for vector %d.\n", i);
 				for (j = 0; j < i; j++) {
-					irq_set_affinity_hint(
-						instance->msixentry[j].vector, NULL);
+					if (smp_affinity_enable)
+						irq_set_affinity_hint(
+							instance->msixentry[j].vector, NULL);
 					free_irq(
 						instance->msixentry[j].vector,
 						&instance->irq_context[j]);
@@ -4846,11 +5246,14 @@ retry_irq_register:
 				instance->msix_vectors = 0;
 				goto retry_irq_register;
 			}
-			if (irq_set_affinity_hint(instance->msixentry[i].vector,
-				get_cpu_mask(cpu)))
-				dev_err(&instance->pdev->dev, "Error setting"
-					"affinity hint for cpu %d\n", cpu);
-			cpu = cpumask_next(cpu, cpu_online_mask);
+			if (smp_affinity_enable) {
+				if (irq_set_affinity_hint(instance->msixentry[i].vector,
+					get_cpu_mask(cpu)))
+					dev_err(&instance->pdev->dev,
+						"Error setting affinity hint "
+						"for cpu %d\n", cpu);
+				cpu = cpumask_next(cpu, cpu_online_mask);
+			}
 		}
 	} else {
 		instance->irq_context[0].instance = instance;
@@ -4894,6 +5297,10 @@ retry_irq_register:
 		goto fail_start_aen;
 	}
 
+	/* Get current SR-IOV LD/VF affiliation */
+	if (instance->requestorId)
+		megasas_get_ld_vf_affiliation(instance, 1);
+
 	return 0;
 
       fail_start_aen:
@@ -4905,8 +5312,9 @@ retry_irq_register:
 	instance->instancet->disable_intr(instance);
 	if (instance->msix_vectors)
 		for (i = 0; i < instance->msix_vectors; i++) {
-			irq_set_affinity_hint(
-				instance->msixentry[i].vector, NULL);
+			if (smp_affinity_enable)
+				irq_set_affinity_hint(
+					instance->msixentry[i].vector, NULL);
 			free_irq(instance->msixentry[i].vector,
 				 &instance->irq_context[i]);
 		}
@@ -4979,7 +5387,11 @@ static void megasas_flush_cache(struct megasas_instance *instance)
 		dev_err(&instance->pdev->dev, "Command timedout"
 			" from %s\n", __func__);
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	return;
 }
@@ -5026,7 +5438,11 @@ static void megasas_shutdown_controller(struct megasas_instance *instance,
 		dev_err(&instance->pdev->dev, "Command timedout"
 			"from %s\n", __func__);
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	return;
 }
@@ -5069,8 +5485,9 @@ megasas_suspend(struct pci_dev *pdev, pm_message_t state)
 
 	if (instance->msix_vectors)
 		for (i = 0; i < instance->msix_vectors; i++) {
-			irq_set_affinity_hint(
-				instance->msixentry[i].vector, NULL);
+			if (smp_affinity_enable)
+				irq_set_affinity_hint(
+					instance->msixentry[i].vector, NULL);
 			free_irq(instance->msixentry[i].vector,
 				 &instance->irq_context[i]);
 		}
@@ -5132,9 +5549,10 @@ megasas_resume(struct pci_dev *pdev)
 		goto fail_ready_state;
 
 	/* Now re-enable MSI-X */
-	if (instance->msix_vectors)
-		pci_enable_msix(instance->pdev, instance->msixentry,
-				instance->msix_vectors);
+	if (instance->msix_vectors &&
+	    pci_enable_msix_exact(instance->pdev, instance->msixentry,
+				  instance->msix_vectors))
+		goto fail_reenable_msix;
 
 	switch (instance->pdev->device) {
 	case PCI_DEVICE_ID_LSI_FUSION:
@@ -5178,8 +5596,9 @@ megasas_resume(struct pci_dev *pdev)
 				printk(KERN_DEBUG "megasas: Failed to "
 				       "register IRQ for vector %d.\n", i);
 				for (j = 0; j < i; j++) {
-					irq_set_affinity_hint(
-						instance->msixentry[j].vector, NULL);
+					if (smp_affinity_enable)
+						irq_set_affinity_hint(
+							instance->msixentry[j].vector, NULL);
 					free_irq(
 						instance->msixentry[j].vector,
 						&instance->irq_context[j]);
@@ -5187,11 +5606,14 @@ megasas_resume(struct pci_dev *pdev)
 				goto fail_irq;
 			}
 
-			if (irq_set_affinity_hint(instance->msixentry[i].vector,
-				get_cpu_mask(cpu)))
-				dev_err(&instance->pdev->dev, "Error setting"
-					"affinity hint for cpu %d\n", cpu);
-			cpu = cpumask_next(cpu, cpu_online_mask);
+			if (smp_affinity_enable) {
+				if (irq_set_affinity_hint(instance->msixentry[i].vector,
+					get_cpu_mask(cpu)))
+					dev_err(&instance->pdev->dev, "Error "
+						"setting affinity hint for cpu "
+						"%d\n", cpu);
+				cpu = cpumask_next(cpu, cpu_online_mask);
+			}
 		}
 	} else {
 		instance->irq_context[0].instance = instance;
@@ -5243,6 +5665,7 @@ fail_init_mfi:
 
 fail_set_dma_mask:
 fail_ready_state:
+fail_reenable_msix:
 
 	pci_disable_device(pdev);
 
@@ -5273,6 +5696,8 @@ static void megasas_detach_one(struct pci_dev *pdev)
 	if (instance->requestorId && !instance->skip_heartbeat_timer_del)
 		del_timer_sync(&instance->sriov_heartbeat_timer);
 
+	if (instance->fw_crash_state != UNAVAILABLE)
+		megasas_free_host_crash_buffer(instance);
 	scsi_remove_host(instance->host);
 	megasas_flush_cache(instance);
 	megasas_shutdown_controller(instance, MR_DCMD_CTRL_SHUTDOWN);
@@ -5306,8 +5731,9 @@ static void megasas_detach_one(struct pci_dev *pdev)
 
 	if (instance->msix_vectors)
 		for (i = 0; i < instance->msix_vectors; i++) {
-			irq_set_affinity_hint(
-				instance->msixentry[i].vector, NULL);
+			if (smp_affinity_enable)
+				irq_set_affinity_hint(
+					instance->msixentry[i].vector, NULL);
 			free_irq(instance->msixentry[i].vector,
 				 &instance->irq_context[i]);
 		}
@@ -5322,14 +5748,18 @@ static void megasas_detach_one(struct pci_dev *pdev)
 	case PCI_DEVICE_ID_LSI_INVADER:
 	case PCI_DEVICE_ID_LSI_FURY:
 		megasas_release_fusion(instance);
-		for (i = 0; i < 2 ; i++)
+		for (i = 0; i < 2 ; i++) {
 			if (fusion->ld_map[i])
 				dma_free_coherent(&instance->pdev->dev,
-						  fusion->map_sz,
+						  fusion->max_map_sz,
 						  fusion->ld_map[i],
-						  fusion->
-						  ld_map_phys[i]);
-		kfree(instance->ctrl_context);
+						  fusion->ld_map_phys[i]);
+			if (fusion->ld_drv_map[i])
+				free_pages((ulong)fusion->ld_drv_map[i],
+					fusion->drv_map_pages);
+		}
+		free_pages((ulong)instance->ctrl_context,
+			instance->ctrl_context_pages);
 		break;
 	default:
 		megasas_release_mfi(instance);
@@ -5342,6 +5772,8 @@ static void megasas_detach_one(struct pci_dev *pdev)
 		break;
 	}
 
+	kfree(instance->ctrl_info);
+
 	if (instance->evt_detail)
 		pci_free_consistent(pdev, sizeof(struct megasas_evt_detail),
 				instance->evt_detail, instance->evt_detail_h);
@@ -5363,6 +5795,10 @@ static void megasas_detach_one(struct pci_dev *pdev)
 				    instance->hb_host_mem,
 				    instance->hb_host_mem_h);
 
+	if (instance->crash_dump_buf)
+		pci_free_consistent(pdev, CRASH_DMA_BUF_SIZE,
+			    instance->crash_dump_buf, instance->crash_dump_h);
+
 	scsi_host_put(host);
 
 	pci_disable_device(pdev);
@@ -5385,8 +5821,9 @@ static void megasas_shutdown(struct pci_dev *pdev)
 	instance->instancet->disable_intr(instance);
 	if (instance->msix_vectors)
 		for (i = 0; i < instance->msix_vectors; i++) {
-			irq_set_affinity_hint(
-				instance->msixentry[i].vector, NULL);
+			if (smp_affinity_enable)
+				irq_set_affinity_hint(
+					instance->msixentry[i].vector, NULL);
 			free_irq(instance->msixentry[i].vector,
 				 &instance->irq_context[i]);
 		}
@@ -5448,12 +5885,53 @@ static unsigned int megasas_mgmt_poll(struct file *file, poll_table *wait)
 	spin_lock_irqsave(&poll_aen_lock, flags);
 	if (megasas_poll_wait_aen)
 		mask =   (POLLIN | POLLRDNORM);
+
 	else
 		mask = 0;
+	megasas_poll_wait_aen = 0;
 	spin_unlock_irqrestore(&poll_aen_lock, flags);
 	return mask;
 }
 
+/*
+ * megasas_set_crash_dump_params_ioctl:
+ *		Send CRASH_DUMP_MODE DCMD to all controllers
+ * @cmd:	MFI command frame
+ */
+
+static int megasas_set_crash_dump_params_ioctl(
+	struct megasas_cmd *cmd)
+{
+	struct megasas_instance *local_instance;
+	int i, error = 0;
+	int crash_support;
+
+	crash_support = cmd->frame->dcmd.mbox.w[0];
+
+	for (i = 0; i < megasas_mgmt_info.max_index; i++) {
+		local_instance = megasas_mgmt_info.instance[i];
+		if (local_instance && local_instance->crash_dump_drv_support) {
+			if ((local_instance->adprecovery ==
+				MEGASAS_HBA_OPERATIONAL) &&
+				!megasas_set_crash_dump_params(local_instance,
+					crash_support)) {
+				local_instance->crash_dump_app_support =
+					crash_support;
+				dev_info(&local_instance->pdev->dev,
+					"Application firmware crash "
+					"dump mode set success\n");
+				error = 0;
+			} else {
+				dev_info(&local_instance->pdev->dev,
+					"Application firmware crash "
+					"dump mode set failed\n");
+				error = -1;
+			}
+		}
+	}
+	return error;
+}
+
 /**
  * megasas_mgmt_fw_ioctl -	Issues management ioctls to FW
  * @instance:			Adapter soft state
@@ -5500,6 +5978,12 @@ megasas_mgmt_fw_ioctl(struct megasas_instance *instance,
 					       MFI_FRAME_SGL64 |
 					       MFI_FRAME_SENSE64));
 
+	if (cmd->frame->dcmd.opcode == MR_DRIVER_SET_APP_CRASHDUMP_MODE) {
+		error = megasas_set_crash_dump_params_ioctl(cmd);
+		megasas_return_cmd(instance, cmd);
+		return error;
+	}
+
 	/*
 	 * The management interface between applications and the fw uses
 	 * MFI frames. E.g, RAID configuration changes, LD property changes
@@ -5619,9 +6103,14 @@ megasas_mgmt_fw_ioctl(struct megasas_instance *instance,
 					  le32_to_cpu(kern_sge32[i].length),
 					  kbuff_arr[i],
 					  le32_to_cpu(kern_sge32[i].phys_addr));
+			kbuff_arr[i] = NULL;
 	}
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 	return error;
 }
 
diff --git a/drivers/scsi/megaraid/megaraid_sas_fp.c b/drivers/scsi/megaraid/megaraid_sas_fp.c
index 081bfff12d00..685e6f391fe4 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fp.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fp.c
@@ -55,6 +55,13 @@
 #include "megaraid_sas.h"
 #include <asm/div64.h>
 
+#define LB_PENDING_CMDS_DEFAULT 4
+static unsigned int lb_pending_cmds = LB_PENDING_CMDS_DEFAULT;
+module_param(lb_pending_cmds, int, S_IRUGO);
+MODULE_PARM_DESC(lb_pending_cmds, "Change raid-1 load balancing outstanding "
+	"threshold. Valid Values are 1-128. Default: 4");
+
+
 #define ABS_DIFF(a, b)   (((a) > (b)) ? ((a) - (b)) : ((b) - (a)))
 #define MR_LD_STATE_OPTIMAL 3
 #define FALSE 0
@@ -66,16 +73,13 @@
 #define SPAN_INVALID  0xff
 
 /* Prototypes */
-void mr_update_load_balance_params(struct MR_FW_RAID_MAP_ALL *map,
-	struct LD_LOAD_BALANCE_INFO *lbInfo);
-
-static void mr_update_span_set(struct MR_FW_RAID_MAP_ALL *map,
+static void mr_update_span_set(struct MR_DRV_RAID_MAP_ALL *map,
 	PLD_SPAN_INFO ldSpanInfo);
 static u8 mr_spanset_get_phy_params(struct megasas_instance *instance, u32 ld,
 	u64 stripRow, u16 stripRef, struct IO_REQUEST_INFO *io_info,
-	struct RAID_CONTEXT *pRAID_Context, struct MR_FW_RAID_MAP_ALL *map);
+	struct RAID_CONTEXT *pRAID_Context, struct MR_DRV_RAID_MAP_ALL *map);
 static u64 get_row_from_strip(struct megasas_instance *instance, u32 ld,
-	u64 strip, struct MR_FW_RAID_MAP_ALL *map);
+	u64 strip, struct MR_DRV_RAID_MAP_ALL *map);
 
 u32 mega_mod64(u64 dividend, u32 divisor)
 {
@@ -109,94 +113,183 @@ u64 mega_div64_32(uint64_t dividend, uint32_t divisor)
 	return d;
 }
 
-struct MR_LD_RAID *MR_LdRaidGet(u32 ld, struct MR_FW_RAID_MAP_ALL *map)
+struct MR_LD_RAID *MR_LdRaidGet(u32 ld, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return &map->raidMap.ldSpanMap[ld].ldRaid;
 }
 
 static struct MR_SPAN_BLOCK_INFO *MR_LdSpanInfoGet(u32 ld,
-						   struct MR_FW_RAID_MAP_ALL
+						   struct MR_DRV_RAID_MAP_ALL
 						   *map)
 {
 	return &map->raidMap.ldSpanMap[ld].spanBlock[0];
 }
 
-static u8 MR_LdDataArmGet(u32 ld, u32 armIdx, struct MR_FW_RAID_MAP_ALL *map)
+static u8 MR_LdDataArmGet(u32 ld, u32 armIdx, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return map->raidMap.ldSpanMap[ld].dataArmMap[armIdx];
 }
 
-u16 MR_ArPdGet(u32 ar, u32 arm, struct MR_FW_RAID_MAP_ALL *map)
+u16 MR_ArPdGet(u32 ar, u32 arm, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return le16_to_cpu(map->raidMap.arMapInfo[ar].pd[arm]);
 }
 
-u16 MR_LdSpanArrayGet(u32 ld, u32 span, struct MR_FW_RAID_MAP_ALL *map)
+u16 MR_LdSpanArrayGet(u32 ld, u32 span, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return le16_to_cpu(map->raidMap.ldSpanMap[ld].spanBlock[span].span.arrayRef);
 }
 
-u16 MR_PdDevHandleGet(u32 pd, struct MR_FW_RAID_MAP_ALL *map)
+u16 MR_PdDevHandleGet(u32 pd, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return map->raidMap.devHndlInfo[pd].curDevHdl;
 }
 
-u16 MR_GetLDTgtId(u32 ld, struct MR_FW_RAID_MAP_ALL *map)
+u16 MR_GetLDTgtId(u32 ld, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return le16_to_cpu(map->raidMap.ldSpanMap[ld].ldRaid.targetId);
 }
 
-u8 MR_TargetIdToLdGet(u32 ldTgtId, struct MR_FW_RAID_MAP_ALL *map)
+u8 MR_TargetIdToLdGet(u32 ldTgtId, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return map->raidMap.ldTgtIdToLd[ldTgtId];
 }
 
 static struct MR_LD_SPAN *MR_LdSpanPtrGet(u32 ld, u32 span,
-					  struct MR_FW_RAID_MAP_ALL *map)
+					  struct MR_DRV_RAID_MAP_ALL *map)
 {
 	return &map->raidMap.ldSpanMap[ld].spanBlock[span].span;
 }
 
 /*
+ * This function will Populate Driver Map using firmware raid map
+ */
+void MR_PopulateDrvRaidMap(struct megasas_instance *instance)
+{
+	struct fusion_context *fusion = instance->ctrl_context;
+	struct MR_FW_RAID_MAP_ALL     *fw_map_old    = NULL;
+	struct MR_FW_RAID_MAP         *pFwRaidMap    = NULL;
+	int i;
+
+
+	struct MR_DRV_RAID_MAP_ALL *drv_map =
+			fusion->ld_drv_map[(instance->map_id & 1)];
+	struct MR_DRV_RAID_MAP *pDrvRaidMap = &drv_map->raidMap;
+
+	if (instance->supportmax256vd) {
+		memcpy(fusion->ld_drv_map[instance->map_id & 1],
+			fusion->ld_map[instance->map_id & 1],
+			fusion->current_map_sz);
+		/* New Raid map will not set totalSize, so keep expected value
+		 * for legacy code in ValidateMapInfo
+		 */
+		pDrvRaidMap->totalSize = sizeof(struct MR_FW_RAID_MAP_EXT);
+	} else {
+		fw_map_old = (struct MR_FW_RAID_MAP_ALL *)
+			fusion->ld_map[(instance->map_id & 1)];
+		pFwRaidMap = &fw_map_old->raidMap;
+
+#if VD_EXT_DEBUG
+		for (i = 0; i < pFwRaidMap->ldCount; i++) {
+			dev_dbg(&instance->pdev->dev, "(%d) :Index 0x%x "
+				"Target Id 0x%x Seq Num 0x%x Size 0/%llx\n",
+				instance->unique_id, i,
+				fw_map_old->raidMap.ldSpanMap[i].ldRaid.targetId,
+				fw_map_old->raidMap.ldSpanMap[i].ldRaid.seqNum,
+				fw_map_old->raidMap.ldSpanMap[i].ldRaid.size);
+		}
+#endif
+
+		memset(drv_map, 0, fusion->drv_map_sz);
+		pDrvRaidMap->totalSize = pFwRaidMap->totalSize;
+		pDrvRaidMap->ldCount = pFwRaidMap->ldCount;
+		pDrvRaidMap->fpPdIoTimeoutSec = pFwRaidMap->fpPdIoTimeoutSec;
+		for (i = 0; i < MAX_RAIDMAP_LOGICAL_DRIVES + MAX_RAIDMAP_VIEWS; i++)
+			pDrvRaidMap->ldTgtIdToLd[i] =
+				(u8)pFwRaidMap->ldTgtIdToLd[i];
+		for (i = 0; i < pDrvRaidMap->ldCount; i++) {
+			pDrvRaidMap->ldSpanMap[i] = pFwRaidMap->ldSpanMap[i];
+#if VD_EXT_DEBUG
+			dev_dbg(&instance->pdev->dev,
+				"pFwRaidMap->ldSpanMap[%d].ldRaid.targetId 0x%x "
+				"pFwRaidMap->ldSpanMap[%d].ldRaid.seqNum 0x%x "
+				"size 0x%x\n", i, i,
+				pFwRaidMap->ldSpanMap[i].ldRaid.targetId,
+				pFwRaidMap->ldSpanMap[i].ldRaid.seqNum,
+				(u32)pFwRaidMap->ldSpanMap[i].ldRaid.rowSize);
+			dev_dbg(&instance->pdev->dev,
+				"pDrvRaidMap->ldSpanMap[%d].ldRaid.targetId 0x%x "
+				"pDrvRaidMap->ldSpanMap[%d].ldRaid.seqNum 0x%x "
+				"size 0x%x\n", i, i,
+				pDrvRaidMap->ldSpanMap[i].ldRaid.targetId,
+				pDrvRaidMap->ldSpanMap[i].ldRaid.seqNum,
+				(u32)pDrvRaidMap->ldSpanMap[i].ldRaid.rowSize);
+			dev_dbg(&instance->pdev->dev, "Driver raid map all %p "
+				"raid map %p LD RAID MAP %p/%p\n", drv_map,
+				pDrvRaidMap, &pFwRaidMap->ldSpanMap[i].ldRaid,
+				&pDrvRaidMap->ldSpanMap[i].ldRaid);
+#endif
+		}
+		memcpy(pDrvRaidMap->arMapInfo, pFwRaidMap->arMapInfo,
+			sizeof(struct MR_ARRAY_INFO) * MAX_RAIDMAP_ARRAYS);
+		memcpy(pDrvRaidMap->devHndlInfo, pFwRaidMap->devHndlInfo,
+			sizeof(struct MR_DEV_HANDLE_INFO) *
+			MAX_RAIDMAP_PHYSICAL_DEVICES);
+	}
+}
+
+/*
  * This function will validate Map info data provided by FW
  */
 u8 MR_ValidateMapInfo(struct megasas_instance *instance)
 {
-	struct fusion_context *fusion = instance->ctrl_context;
-	struct MR_FW_RAID_MAP_ALL *map = fusion->ld_map[(instance->map_id & 1)];
-	struct LD_LOAD_BALANCE_INFO *lbInfo = fusion->load_balance_info;
-	PLD_SPAN_INFO ldSpanInfo = fusion->log_to_span;
-	struct MR_FW_RAID_MAP *pFwRaidMap = &map->raidMap;
+	struct fusion_context *fusion;
+	struct MR_DRV_RAID_MAP_ALL *drv_map;
+	struct MR_DRV_RAID_MAP *pDrvRaidMap;
+	struct LD_LOAD_BALANCE_INFO *lbInfo;
+	PLD_SPAN_INFO ldSpanInfo;
 	struct MR_LD_RAID         *raid;
 	int ldCount, num_lds;
 	u16 ld;
+	u32 expected_size;
+
+
+	MR_PopulateDrvRaidMap(instance);
+
+	fusion = instance->ctrl_context;
+	drv_map = fusion->ld_drv_map[(instance->map_id & 1)];
+	pDrvRaidMap = &drv_map->raidMap;
 
+	lbInfo = fusion->load_balance_info;
+	ldSpanInfo = fusion->log_to_span;
 
-	if (le32_to_cpu(pFwRaidMap->totalSize) !=
-	    (sizeof(struct MR_FW_RAID_MAP) -sizeof(struct MR_LD_SPAN_MAP) +
-	     (sizeof(struct MR_LD_SPAN_MAP) * le32_to_cpu(pFwRaidMap->ldCount)))) {
-		printk(KERN_ERR "megasas: map info structure size 0x%x is not matching with ld count\n",
-		       (unsigned int)((sizeof(struct MR_FW_RAID_MAP) -
-				       sizeof(struct MR_LD_SPAN_MAP)) +
-				      (sizeof(struct MR_LD_SPAN_MAP) *
-					le32_to_cpu(pFwRaidMap->ldCount))));
-		printk(KERN_ERR "megasas: span map %x, pFwRaidMap->totalSize "
-		       ": %x\n", (unsigned int)sizeof(struct MR_LD_SPAN_MAP),
-			le32_to_cpu(pFwRaidMap->totalSize));
+	if (instance->supportmax256vd)
+		expected_size = sizeof(struct MR_FW_RAID_MAP_EXT);
+	else
+		expected_size =
+			(sizeof(struct MR_FW_RAID_MAP) - sizeof(struct MR_LD_SPAN_MAP) +
+			(sizeof(struct MR_LD_SPAN_MAP) * le32_to_cpu(pDrvRaidMap->ldCount)));
+
+	if (le32_to_cpu(pDrvRaidMap->totalSize) != expected_size) {
+		dev_err(&instance->pdev->dev, "map info structure size 0x%x is not matching with ld count\n",
+		       (unsigned int) expected_size);
+		dev_err(&instance->pdev->dev, "megasas: span map %x, pDrvRaidMap->totalSize : %x\n",
+			(unsigned int)sizeof(struct MR_LD_SPAN_MAP),
+			le32_to_cpu(pDrvRaidMap->totalSize));
 		return 0;
 	}
 
 	if (instance->UnevenSpanSupport)
-		mr_update_span_set(map, ldSpanInfo);
+		mr_update_span_set(drv_map, ldSpanInfo);
 
-	mr_update_load_balance_params(map, lbInfo);
+	mr_update_load_balance_params(drv_map, lbInfo);
 
-	num_lds = le32_to_cpu(map->raidMap.ldCount);
+	num_lds = le32_to_cpu(drv_map->raidMap.ldCount);
 
 	/*Convert Raid capability values to CPU arch */
 	for (ldCount = 0; ldCount < num_lds; ldCount++) {
-		ld = MR_TargetIdToLdGet(ldCount, map);
-		raid = MR_LdRaidGet(ld, map);
+		ld = MR_TargetIdToLdGet(ldCount, drv_map);
+		raid = MR_LdRaidGet(ld, drv_map);
 		le32_to_cpus((u32 *)&raid->capability);
 	}
 
@@ -204,7 +297,7 @@ u8 MR_ValidateMapInfo(struct megasas_instance *instance)
 }
 
 u32 MR_GetSpanBlock(u32 ld, u64 row, u64 *span_blk,
-		    struct MR_FW_RAID_MAP_ALL *map)
+		    struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct MR_SPAN_BLOCK_INFO *pSpanBlock = MR_LdSpanInfoGet(ld, map);
 	struct MR_QUAD_ELEMENT    *quad;
@@ -246,7 +339,8 @@ u32 MR_GetSpanBlock(u32 ld, u64 row, u64 *span_blk,
 * ldSpanInfo - ldSpanInfo per HBA instance
 */
 #if SPAN_DEBUG
-static int getSpanInfo(struct MR_FW_RAID_MAP_ALL *map, PLD_SPAN_INFO ldSpanInfo)
+static int getSpanInfo(struct MR_DRV_RAID_MAP_ALL *map,
+	PLD_SPAN_INFO ldSpanInfo)
 {
 
 	u8   span;
@@ -257,9 +351,9 @@ static int getSpanInfo(struct MR_FW_RAID_MAP_ALL *map, PLD_SPAN_INFO ldSpanInfo)
 	int ldCount;
 	u16 ld;
 
-	for (ldCount = 0; ldCount < MAX_LOGICAL_DRIVES; ldCount++) {
+	for (ldCount = 0; ldCount < MAX_LOGICAL_DRIVES_EXT; ldCount++) {
 		ld = MR_TargetIdToLdGet(ldCount, map);
-			if (ld >= MAX_LOGICAL_DRIVES)
+			if (ld >= MAX_LOGICAL_DRIVES_EXT)
 				continue;
 		raid = MR_LdRaidGet(ld, map);
 		dev_dbg(&instance->pdev->dev, "LD %x: span_depth=%x\n",
@@ -339,7 +433,7 @@ static int getSpanInfo(struct MR_FW_RAID_MAP_ALL *map, PLD_SPAN_INFO ldSpanInfo)
 */
 
 u32 mr_spanset_get_span_block(struct megasas_instance *instance,
-		u32 ld, u64 row, u64 *span_blk, struct MR_FW_RAID_MAP_ALL *map)
+		u32 ld, u64 row, u64 *span_blk, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct fusion_context *fusion = instance->ctrl_context;
 	struct MR_LD_RAID         *raid = MR_LdRaidGet(ld, map);
@@ -402,7 +496,7 @@ u32 mr_spanset_get_span_block(struct megasas_instance *instance,
 */
 
 static u64  get_row_from_strip(struct megasas_instance *instance,
-	u32 ld, u64 strip, struct MR_FW_RAID_MAP_ALL *map)
+	u32 ld, u64 strip, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct fusion_context *fusion = instance->ctrl_context;
 	struct MR_LD_RAID	*raid = MR_LdRaidGet(ld, map);
@@ -471,7 +565,7 @@ static u64  get_row_from_strip(struct megasas_instance *instance,
 */
 
 static u64 get_strip_from_row(struct megasas_instance *instance,
-		u32 ld, u64 row, struct MR_FW_RAID_MAP_ALL *map)
+		u32 ld, u64 row, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct fusion_context *fusion = instance->ctrl_context;
 	struct MR_LD_RAID         *raid = MR_LdRaidGet(ld, map);
@@ -532,7 +626,7 @@ static u64 get_strip_from_row(struct megasas_instance *instance,
 */
 
 static u32 get_arm_from_strip(struct megasas_instance *instance,
-	u32 ld, u64 strip, struct MR_FW_RAID_MAP_ALL *map)
+	u32 ld, u64 strip, struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct fusion_context *fusion = instance->ctrl_context;
 	struct MR_LD_RAID         *raid = MR_LdRaidGet(ld, map);
@@ -580,7 +674,7 @@ static u32 get_arm_from_strip(struct megasas_instance *instance,
 
 /* This Function will return Phys arm */
 u8 get_arm(struct megasas_instance *instance, u32 ld, u8 span, u64 stripe,
-		struct MR_FW_RAID_MAP_ALL *map)
+		struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct MR_LD_RAID  *raid = MR_LdRaidGet(ld, map);
 	/* Need to check correct default value */
@@ -624,7 +718,7 @@ u8 get_arm(struct megasas_instance *instance, u32 ld, u8 span, u64 stripe,
 static u8 mr_spanset_get_phy_params(struct megasas_instance *instance, u32 ld,
 		u64 stripRow, u16 stripRef, struct IO_REQUEST_INFO *io_info,
 		struct RAID_CONTEXT *pRAID_Context,
-		struct MR_FW_RAID_MAP_ALL *map)
+		struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct MR_LD_RAID  *raid = MR_LdRaidGet(ld, map);
 	u32     pd, arRef;
@@ -682,6 +776,7 @@ static u8 mr_spanset_get_phy_params(struct megasas_instance *instance, u32 ld,
 	*pdBlock += stripRef + le64_to_cpu(MR_LdSpanPtrGet(ld, span, map)->startBlk);
 	pRAID_Context->spanArm = (span << RAID_CTX_SPANARM_SPAN_SHIFT) |
 					physArm;
+	io_info->span_arm = pRAID_Context->spanArm;
 	return retval;
 }
 
@@ -705,7 +800,7 @@ static u8 mr_spanset_get_phy_params(struct megasas_instance *instance, u32 ld,
 u8 MR_GetPhyParams(struct megasas_instance *instance, u32 ld, u64 stripRow,
 		u16 stripRef, struct IO_REQUEST_INFO *io_info,
 		struct RAID_CONTEXT *pRAID_Context,
-		struct MR_FW_RAID_MAP_ALL *map)
+		struct MR_DRV_RAID_MAP_ALL *map)
 {
 	struct MR_LD_RAID  *raid = MR_LdRaidGet(ld, map);
 	u32         pd, arRef;
@@ -778,6 +873,7 @@ u8 MR_GetPhyParams(struct megasas_instance *instance, u32 ld, u64 stripRow,
 	*pdBlock += stripRef + le64_to_cpu(MR_LdSpanPtrGet(ld, span, map)->startBlk);
 	pRAID_Context->spanArm = (span << RAID_CTX_SPANARM_SPAN_SHIFT) |
 		physArm;
+	io_info->span_arm = pRAID_Context->spanArm;
 	return retval;
 }
 
@@ -794,7 +890,7 @@ u8
 MR_BuildRaidContext(struct megasas_instance *instance,
 		    struct IO_REQUEST_INFO *io_info,
 		    struct RAID_CONTEXT *pRAID_Context,
-		    struct MR_FW_RAID_MAP_ALL *map, u8 **raidLUN)
+		    struct MR_DRV_RAID_MAP_ALL *map, u8 **raidLUN)
 {
 	struct MR_LD_RAID  *raid;
 	u32         ld, stripSize, stripe_mask;
@@ -1043,8 +1139,8 @@ MR_BuildRaidContext(struct megasas_instance *instance,
 * ldSpanInfo - ldSpanInfo per HBA instance
 *
 */
-void mr_update_span_set(struct MR_FW_RAID_MAP_ALL *map,
-			PLD_SPAN_INFO ldSpanInfo)
+void mr_update_span_set(struct MR_DRV_RAID_MAP_ALL *map,
+	PLD_SPAN_INFO ldSpanInfo)
 {
 	u8   span, count;
 	u32  element, span_row_width;
@@ -1056,9 +1152,9 @@ void mr_update_span_set(struct MR_FW_RAID_MAP_ALL *map,
 	u16 ld;
 
 
-	for (ldCount = 0; ldCount < MAX_LOGICAL_DRIVES; ldCount++) {
+	for (ldCount = 0; ldCount < MAX_LOGICAL_DRIVES_EXT; ldCount++) {
 		ld = MR_TargetIdToLdGet(ldCount, map);
-		if (ld >= MAX_LOGICAL_DRIVES)
+		if (ld >= MAX_LOGICAL_DRIVES_EXT)
 			continue;
 		raid = MR_LdRaidGet(ld, map);
 		for (element = 0; element < MAX_QUAD_DEPTH; element++) {
@@ -1152,90 +1248,105 @@ void mr_update_span_set(struct MR_FW_RAID_MAP_ALL *map,
 
 }
 
-void
-mr_update_load_balance_params(struct MR_FW_RAID_MAP_ALL *map,
-			      struct LD_LOAD_BALANCE_INFO *lbInfo)
+void mr_update_load_balance_params(struct MR_DRV_RAID_MAP_ALL *drv_map,
+	struct LD_LOAD_BALANCE_INFO *lbInfo)
 {
 	int ldCount;
 	u16 ld;
 	struct MR_LD_RAID *raid;
 
-	for (ldCount = 0; ldCount < MAX_LOGICAL_DRIVES; ldCount++) {
-		ld = MR_TargetIdToLdGet(ldCount, map);
-		if (ld >= MAX_LOGICAL_DRIVES) {
+	if (lb_pending_cmds > 128 || lb_pending_cmds < 1)
+		lb_pending_cmds = LB_PENDING_CMDS_DEFAULT;
+
+	for (ldCount = 0; ldCount < MAX_LOGICAL_DRIVES_EXT; ldCount++) {
+		ld = MR_TargetIdToLdGet(ldCount, drv_map);
+		if (ld >= MAX_LOGICAL_DRIVES_EXT) {
 			lbInfo[ldCount].loadBalanceFlag = 0;
 			continue;
 		}
 
-		raid = MR_LdRaidGet(ld, map);
-
-		/* Two drive Optimal RAID 1 */
-		if ((raid->level == 1)  &&  (raid->rowSize == 2) &&
-		    (raid->spanDepth == 1) && raid->ldState ==
-		    MR_LD_STATE_OPTIMAL) {
-			u32 pd, arRef;
-
-			lbInfo[ldCount].loadBalanceFlag = 1;
-
-			/* Get the array on which this span is present */
-			arRef = MR_LdSpanArrayGet(ld, 0, map);
-
-			/* Get the Pd */
-			pd = MR_ArPdGet(arRef, 0, map);
-			/* Get dev handle from Pd */
-			lbInfo[ldCount].raid1DevHandle[0] =
-				MR_PdDevHandleGet(pd, map);
-			/* Get the Pd */
-			pd = MR_ArPdGet(arRef, 1, map);
-
-			/* Get the dev handle from Pd */
-			lbInfo[ldCount].raid1DevHandle[1] =
-				MR_PdDevHandleGet(pd, map);
-		} else
+		raid = MR_LdRaidGet(ld, drv_map);
+		if ((raid->level != 1) ||
+			(raid->ldState != MR_LD_STATE_OPTIMAL)) {
 			lbInfo[ldCount].loadBalanceFlag = 0;
+			continue;
+		}
+		lbInfo[ldCount].loadBalanceFlag = 1;
 	}
 }
 
-u8 megasas_get_best_arm(struct LD_LOAD_BALANCE_INFO *lbInfo, u8 arm, u64 block,
-			u32 count)
+u8 megasas_get_best_arm_pd(struct megasas_instance *instance,
+	struct LD_LOAD_BALANCE_INFO *lbInfo, struct IO_REQUEST_INFO *io_info)
 {
-	u16     pend0, pend1;
+	struct fusion_context *fusion;
+	struct MR_LD_RAID  *raid;
+	struct MR_DRV_RAID_MAP_ALL *drv_map;
+	u16     pend0, pend1, ld;
 	u64     diff0, diff1;
-	u8      bestArm;
+	u8      bestArm, pd0, pd1, span, arm;
+	u32     arRef, span_row_size;
+
+	u64 block = io_info->ldStartBlock;
+	u32 count = io_info->numBlocks;
+
+	span = ((io_info->span_arm & RAID_CTX_SPANARM_SPAN_MASK)
+			>> RAID_CTX_SPANARM_SPAN_SHIFT);
+	arm = (io_info->span_arm & RAID_CTX_SPANARM_ARM_MASK);
+
+
+	fusion = instance->ctrl_context;
+	drv_map = fusion->ld_drv_map[(instance->map_id & 1)];
+	ld = MR_TargetIdToLdGet(io_info->ldTgtId, drv_map);
+	raid = MR_LdRaidGet(ld, drv_map);
+	span_row_size = instance->UnevenSpanSupport ?
+			SPAN_ROW_SIZE(drv_map, ld, span) : raid->rowSize;
+
+	arRef = MR_LdSpanArrayGet(ld, span, drv_map);
+	pd0 = MR_ArPdGet(arRef, arm, drv_map);
+	pd1 = MR_ArPdGet(arRef, (arm + 1) >= span_row_size ?
+		(arm + 1 - span_row_size) : arm + 1, drv_map);
 
 	/* get the pending cmds for the data and mirror arms */
-	pend0 = atomic_read(&lbInfo->scsi_pending_cmds[0]);
-	pend1 = atomic_read(&lbInfo->scsi_pending_cmds[1]);
+	pend0 = atomic_read(&lbInfo->scsi_pending_cmds[pd0]);
+	pend1 = atomic_read(&lbInfo->scsi_pending_cmds[pd1]);
 
 	/* Determine the disk whose head is nearer to the req. block */
-	diff0 = ABS_DIFF(block, lbInfo->last_accessed_block[0]);
-	diff1 = ABS_DIFF(block, lbInfo->last_accessed_block[1]);
-	bestArm = (diff0 <= diff1 ? 0 : 1);
+	diff0 = ABS_DIFF(block, lbInfo->last_accessed_block[pd0]);
+	diff1 = ABS_DIFF(block, lbInfo->last_accessed_block[pd1]);
+	bestArm = (diff0 <= diff1 ? arm : arm ^ 1);
 
-	/*Make balance count from 16 to 4 to keep driver in sync with Firmware*/
-	if ((bestArm == arm && pend0 > pend1 + 4)  ||
-	    (bestArm != arm && pend1 > pend0 + 4))
+	if ((bestArm == arm && pend0 > pend1 + lb_pending_cmds)  ||
+			(bestArm != arm && pend1 > pend0 + lb_pending_cmds))
 		bestArm ^= 1;
 
 	/* Update the last accessed block on the correct pd */
-	lbInfo->last_accessed_block[bestArm] = block + count - 1;
-
-	return bestArm;
+	io_info->pd_after_lb = (bestArm == arm) ? pd0 : pd1;
+	lbInfo->last_accessed_block[io_info->pd_after_lb] = block + count - 1;
+	io_info->span_arm = (span << RAID_CTX_SPANARM_SPAN_SHIFT) | bestArm;
+#if SPAN_DEBUG
+	if (arm != bestArm)
+		dev_dbg(&instance->pdev->dev, "LSI Debug R1 Load balance "
+			"occur - span 0x%x arm 0x%x bestArm 0x%x "
+			"io_info->span_arm 0x%x\n",
+			span, arm, bestArm, io_info->span_arm);
+#endif
+	return io_info->pd_after_lb;
 }
 
-u16 get_updated_dev_handle(struct LD_LOAD_BALANCE_INFO *lbInfo,
-			   struct IO_REQUEST_INFO *io_info)
+u16 get_updated_dev_handle(struct megasas_instance *instance,
+	struct LD_LOAD_BALANCE_INFO *lbInfo, struct IO_REQUEST_INFO *io_info)
 {
-	u8 arm, old_arm;
+	u8 arm_pd;
 	u16 devHandle;
+	struct fusion_context *fusion;
+	struct MR_DRV_RAID_MAP_ALL *drv_map;
 
-	old_arm = lbInfo->raid1DevHandle[0] == io_info->devHandle ? 0 : 1;
-
-	/* get best new arm */
-	arm  = megasas_get_best_arm(lbInfo, old_arm, io_info->ldStartBlock,
-				    io_info->numBlocks);
-	devHandle = lbInfo->raid1DevHandle[arm];
-	atomic_inc(&lbInfo->scsi_pending_cmds[arm]);
+	fusion = instance->ctrl_context;
+	drv_map = fusion->ld_drv_map[(instance->map_id & 1)];
 
+	/* get best new arm (PD ID) */
+	arm_pd  = megasas_get_best_arm_pd(instance, lbInfo, io_info);
+	devHandle = MR_PdDevHandleGet(arm_pd, drv_map);
+	atomic_inc(&lbInfo->scsi_pending_cmds[arm_pd]);
 	return devHandle;
 }
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.c b/drivers/scsi/megaraid/megaraid_sas_fusion.c
index 3ed03dfab76c..f37eed682c75 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.c
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.c
@@ -50,6 +50,7 @@
 #include <scsi/scsi_cmnd.h>
 #include <scsi/scsi_device.h>
 #include <scsi/scsi_host.h>
+#include <scsi/scsi_dbg.h>
 
 #include "megaraid_sas_fusion.h"
 #include "megaraid_sas.h"
@@ -76,8 +77,6 @@ megasas_issue_polled(struct megasas_instance *instance,
 void
 megasas_check_and_restore_queue_depth(struct megasas_instance *instance);
 
-u16 get_updated_dev_handle(struct LD_LOAD_BALANCE_INFO *lbInfo,
-			   struct IO_REQUEST_INFO *in_info);
 int megasas_transition_to_ready(struct megasas_instance *instance, int ocr);
 void megaraid_sas_kill_hba(struct megasas_instance *instance);
 
@@ -91,6 +90,8 @@ void megasas_start_timer(struct megasas_instance *instance,
 extern struct megasas_mgmt_info megasas_mgmt_info;
 extern int resetwaittime;
 
+
+
 /**
  * megasas_enable_intr_fusion -	Enables interrupts
  * @regs:			MFI register set
@@ -163,7 +164,7 @@ struct megasas_cmd_fusion *megasas_get_cmd_fusion(struct megasas_instance
 		(struct fusion_context *)instance->ctrl_context;
 	struct megasas_cmd_fusion *cmd = NULL;
 
-	spin_lock_irqsave(&fusion->cmd_pool_lock, flags);
+	spin_lock_irqsave(&fusion->mpt_pool_lock, flags);
 
 	if (!list_empty(&fusion->cmd_pool)) {
 		cmd = list_entry((&fusion->cmd_pool)->next,
@@ -173,7 +174,7 @@ struct megasas_cmd_fusion *megasas_get_cmd_fusion(struct megasas_instance
 		printk(KERN_ERR "megasas: Command pool (fusion) empty!\n");
 	}
 
-	spin_unlock_irqrestore(&fusion->cmd_pool_lock, flags);
+	spin_unlock_irqrestore(&fusion->mpt_pool_lock, flags);
 	return cmd;
 }
 
@@ -182,21 +183,47 @@ struct megasas_cmd_fusion *megasas_get_cmd_fusion(struct megasas_instance
  * @instance:		Adapter soft state
  * @cmd:		Command packet to be returned to free command pool
  */
-static inline void
-megasas_return_cmd_fusion(struct megasas_instance *instance,
-			  struct megasas_cmd_fusion *cmd)
+inline void megasas_return_cmd_fusion(struct megasas_instance *instance,
+	struct megasas_cmd_fusion *cmd)
 {
 	unsigned long flags;
 	struct fusion_context *fusion =
 		(struct fusion_context *)instance->ctrl_context;
 
-	spin_lock_irqsave(&fusion->cmd_pool_lock, flags);
+	spin_lock_irqsave(&fusion->mpt_pool_lock, flags);
 
 	cmd->scmd = NULL;
 	cmd->sync_cmd_idx = (u32)ULONG_MAX;
-	list_add_tail(&cmd->list, &fusion->cmd_pool);
+	list_add(&cmd->list, (&fusion->cmd_pool)->next);
 
-	spin_unlock_irqrestore(&fusion->cmd_pool_lock, flags);
+	spin_unlock_irqrestore(&fusion->mpt_pool_lock, flags);
+}
+
+/**
+ * megasas_return_mfi_mpt_pthr - Return a mfi and mpt to free command pool
+ * @instance:		Adapter soft state
+ * @cmd_mfi:		MFI Command packet to be returned to free command pool
+ * @cmd_mpt:		MPT Command packet to be returned to free command pool
+ */
+inline void megasas_return_mfi_mpt_pthr(struct megasas_instance *instance,
+		struct megasas_cmd *cmd_mfi,
+		struct megasas_cmd_fusion *cmd_fusion)
+{
+	unsigned long flags;
+
+	/*
+	 * TO DO: optimize this code and use only one lock instead of two
+	 * locks being used currently- mpt_pool_lock is acquired
+	 * inside mfi_pool_lock
+	 */
+	spin_lock_irqsave(&instance->mfi_pool_lock, flags);
+	megasas_return_cmd_fusion(instance, cmd_fusion);
+	if (atomic_read(&cmd_mfi->mfi_mpt_pthr) != MFI_MPT_ATTACHED)
+		dev_err(&instance->pdev->dev, "Possible bug from %s %d\n",
+			__func__, __LINE__);
+	atomic_set(&cmd_mfi->mfi_mpt_pthr, MFI_MPT_DETACHED);
+	__megasas_return_cmd(instance, cmd_mfi);
+	spin_unlock_irqrestore(&instance->mfi_pool_lock, flags);
 }
 
 /**
@@ -562,9 +589,11 @@ wait_and_poll(struct megasas_instance *instance, struct megasas_cmd *cmd,
 {
 	int i;
 	struct megasas_header *frame_hdr = &cmd->frame->hdr;
+	struct fusion_context *fusion;
 
 	u32 msecs = seconds * 1000;
 
+	fusion = instance->ctrl_context;
 	/*
 	 * Wait for cmd_status to change
 	 */
@@ -573,8 +602,12 @@ wait_and_poll(struct megasas_instance *instance, struct megasas_cmd *cmd,
 		msleep(20);
 	}
 
-	if (frame_hdr->cmd_status == 0xff)
+	if (frame_hdr->cmd_status == 0xff) {
+		if (fusion)
+			megasas_return_mfi_mpt_pthr(instance, cmd,
+				cmd->mpt_pthr_cmd_blocked);
 		return -ETIME;
+	}
 
 	return 0;
 }
@@ -650,6 +683,10 @@ megasas_ioc_init_fusion(struct megasas_instance *instance)
 	/* driver supports HA / Remote LUN over Fast Path interface */
 	init_frame->driver_operations.mfi_capabilities.support_fp_remote_lun
 		= 1;
+	init_frame->driver_operations.mfi_capabilities.support_max_255lds
+		= 1;
+	init_frame->driver_operations.mfi_capabilities.support_ndrive_r1_lb
+		= 1;
 	/* Convert capability to LE32 */
 	cpu_to_le32s((u32 *)&init_frame->driver_operations.mfi_capabilities);
 
@@ -709,6 +746,13 @@ fail_get_cmd:
  * Issues an internal command (DCMD) to get the FW's controller PD
  * list structure.  This information is mainly used to find out SYSTEM
  * supported by the FW.
+ * dcmd.mbox value setting for MR_DCMD_LD_MAP_GET_INFO
+ * dcmd.mbox.b[0]	- number of LDs being sync'd
+ * dcmd.mbox.b[1]	- 0 - complete command immediately.
+ *			- 1 - pend till config change
+ * dcmd.mbox.b[2]	- 0 - supports max 64 lds and uses legacy MR_FW_RAID_MAP
+ *			- 1 - supports max MAX_LOGICAL_DRIVES_EXT lds and
+ *				uses extended struct MR_FW_RAID_MAP_EXT
  */
 static int
 megasas_get_ld_map_info(struct megasas_instance *instance)
@@ -716,7 +760,7 @@ megasas_get_ld_map_info(struct megasas_instance *instance)
 	int ret = 0;
 	struct megasas_cmd *cmd;
 	struct megasas_dcmd_frame *dcmd;
-	struct MR_FW_RAID_MAP_ALL *ci;
+	void *ci;
 	dma_addr_t ci_h = 0;
 	u32 size_map_info;
 	struct fusion_context *fusion;
@@ -737,10 +781,9 @@ megasas_get_ld_map_info(struct megasas_instance *instance)
 
 	dcmd = &cmd->frame->dcmd;
 
-	size_map_info = sizeof(struct MR_FW_RAID_MAP) +
-		(sizeof(struct MR_LD_SPAN_MAP) *(MAX_LOGICAL_DRIVES - 1));
+	size_map_info = fusion->current_map_sz;
 
-	ci = fusion->ld_map[(instance->map_id & 1)];
+	ci = (void *) fusion->ld_map[(instance->map_id & 1)];
 	ci_h = fusion->ld_map_phys[(instance->map_id & 1)];
 
 	if (!ci) {
@@ -749,9 +792,13 @@ megasas_get_ld_map_info(struct megasas_instance *instance)
 		return -ENOMEM;
 	}
 
-	memset(ci, 0, sizeof(*ci));
+	memset(ci, 0, fusion->max_map_sz);
 	memset(dcmd->mbox.b, 0, MFI_MBOX_SIZE);
-
+#if VD_EXT_DEBUG
+	dev_dbg(&instance->pdev->dev,
+		"%s sending MR_DCMD_LD_MAP_GET_INFO with size %d\n",
+		__func__, cpu_to_le32(size_map_info));
+#endif
 	dcmd->cmd = MFI_CMD_DCMD;
 	dcmd->cmd_status = 0xFF;
 	dcmd->sge_count = 1;
@@ -763,14 +810,17 @@ megasas_get_ld_map_info(struct megasas_instance *instance)
 	dcmd->sgl.sge32[0].phys_addr = cpu_to_le32(ci_h);
 	dcmd->sgl.sge32[0].length = cpu_to_le32(size_map_info);
 
-	if (!megasas_issue_polled(instance, cmd))
-		ret = 0;
-	else {
-		printk(KERN_ERR "megasas: Get LD Map Info Failed\n");
-		ret = -1;
-	}
+	if (instance->ctrl_context && !instance->mask_interrupts)
+		ret = megasas_issue_blocked_cmd(instance, cmd,
+			MEGASAS_BLOCKED_CMD_TIMEOUT);
+	else
+		ret = megasas_issue_polled(instance, cmd);
 
-	megasas_return_cmd(instance, cmd);
+	if (instance->ctrl_context && cmd->mpt_pthr_cmd_blocked)
+		megasas_return_mfi_mpt_pthr(instance, cmd,
+			cmd->mpt_pthr_cmd_blocked);
+	else
+		megasas_return_cmd(instance, cmd);
 
 	return ret;
 }
@@ -807,7 +857,7 @@ megasas_sync_map_info(struct megasas_instance *instance)
 	u32 size_sync_info, num_lds;
 	struct fusion_context *fusion;
 	struct MR_LD_TARGET_SYNC *ci = NULL;
-	struct MR_FW_RAID_MAP_ALL *map;
+	struct MR_DRV_RAID_MAP_ALL *map;
 	struct MR_LD_RAID  *raid;
 	struct MR_LD_TARGET_SYNC *ld_sync;
 	dma_addr_t ci_h = 0;
@@ -828,7 +878,7 @@ megasas_sync_map_info(struct megasas_instance *instance)
 		return 1;
 	}
 
-	map = fusion->ld_map[instance->map_id & 1];
+	map = fusion->ld_drv_map[instance->map_id & 1];
 
 	num_lds = le32_to_cpu(map->raidMap.ldCount);
 
@@ -840,7 +890,7 @@ megasas_sync_map_info(struct megasas_instance *instance)
 
 	ci = (struct MR_LD_TARGET_SYNC *)
 	  fusion->ld_map[(instance->map_id - 1) & 1];
-	memset(ci, 0, sizeof(struct MR_FW_RAID_MAP_ALL));
+	memset(ci, 0, fusion->max_map_sz);
 
 	ci_h = fusion->ld_map_phys[(instance->map_id - 1) & 1];
 
@@ -852,8 +902,7 @@ megasas_sync_map_info(struct megasas_instance *instance)
 		ld_sync->seqNum = raid->seqNum;
 	}
 
-	size_map_info = sizeof(struct MR_FW_RAID_MAP) +
-		(sizeof(struct MR_LD_SPAN_MAP) *(MAX_LOGICAL_DRIVES - 1));
+	size_map_info = fusion->current_map_sz;
 
 	dcmd->cmd = MFI_CMD_DCMD;
 	dcmd->cmd_status = 0xFF;
@@ -971,7 +1020,7 @@ megasas_init_adapter_fusion(struct megasas_instance *instance)
 
 	max_cmd = instance->max_fw_cmds;
 
-	fusion->reply_q_depth = ((max_cmd + 1 + 15)/16)*16;
+	fusion->reply_q_depth = 2 * (((max_cmd + 1 + 15)/16)*16);
 
 	fusion->request_alloc_sz =
 		sizeof(union MEGASAS_REQUEST_DESCRIPTOR_UNION) *max_cmd;
@@ -988,8 +1037,8 @@ megasas_init_adapter_fusion(struct megasas_instance *instance)
 	fusion->max_sge_in_chain =
 		MEGASAS_MAX_SZ_CHAIN_FRAME / sizeof(union MPI2_SGE_IO_UNION);
 
-	instance->max_num_sge = fusion->max_sge_in_main_msg +
-		fusion->max_sge_in_chain - 2;
+	instance->max_num_sge = rounddown_pow_of_two(
+		fusion->max_sge_in_main_msg + fusion->max_sge_in_chain - 2);
 
 	/* Used for pass thru MFI frame (DCMD) */
 	fusion->chain_offset_mfi_pthru =
@@ -1016,17 +1065,75 @@ megasas_init_adapter_fusion(struct megasas_instance *instance)
 		goto fail_ioc_init;
 
 	megasas_display_intel_branding(instance);
+	if (megasas_get_ctrl_info(instance, instance->ctrl_info)) {
+		dev_err(&instance->pdev->dev,
+			"Could not get controller info. Fail from %s %d\n",
+			__func__, __LINE__);
+		goto fail_ioc_init;
+	}
+
+	instance->supportmax256vd =
+		instance->ctrl_info->adapterOperations3.supportMaxExtLDs;
+	/* Below is additional check to address future FW enhancement */
+	if (instance->ctrl_info->max_lds > 64)
+		instance->supportmax256vd = 1;
+	instance->drv_supported_vd_count = MEGASAS_MAX_LD_CHANNELS
+					* MEGASAS_MAX_DEV_PER_CHANNEL;
+	instance->drv_supported_pd_count = MEGASAS_MAX_PD_CHANNELS
+					* MEGASAS_MAX_DEV_PER_CHANNEL;
+	if (instance->supportmax256vd) {
+		instance->fw_supported_vd_count = MAX_LOGICAL_DRIVES_EXT;
+		instance->fw_supported_pd_count = MAX_PHYSICAL_DEVICES;
+	} else {
+		instance->fw_supported_vd_count = MAX_LOGICAL_DRIVES;
+		instance->fw_supported_pd_count = MAX_PHYSICAL_DEVICES;
+	}
+	dev_info(&instance->pdev->dev, "Firmware supports %d VDs %d PDs\n"
+		"Driver supports %d VDs  %d PDs\n",
+		instance->fw_supported_vd_count,
+		instance->fw_supported_pd_count,
+		instance->drv_supported_vd_count,
+		instance->drv_supported_pd_count);
 
 	instance->flag_ieee = 1;
+	fusion->fast_path_io = 0;
 
-	fusion->map_sz =  sizeof(struct MR_FW_RAID_MAP) +
-	  (sizeof(struct MR_LD_SPAN_MAP) *(MAX_LOGICAL_DRIVES - 1));
+	fusion->old_map_sz =
+		sizeof(struct MR_FW_RAID_MAP) + (sizeof(struct MR_LD_SPAN_MAP) *
+		(instance->fw_supported_vd_count - 1));
+	fusion->new_map_sz =
+		sizeof(struct MR_FW_RAID_MAP_EXT);
+	fusion->drv_map_sz =
+		sizeof(struct MR_DRV_RAID_MAP) + (sizeof(struct MR_LD_SPAN_MAP) *
+		(instance->drv_supported_vd_count - 1));
+
+	fusion->drv_map_pages = get_order(fusion->drv_map_sz);
+	for (i = 0; i < 2; i++) {
+		fusion->ld_map[i] = NULL;
+		fusion->ld_drv_map[i] = (void *)__get_free_pages(GFP_KERNEL,
+			fusion->drv_map_pages);
+		if (!fusion->ld_drv_map[i]) {
+			dev_err(&instance->pdev->dev, "Could not allocate "
+				"memory for local map info for %d pages\n",
+				fusion->drv_map_pages);
+			if (i == 1)
+				free_pages((ulong)fusion->ld_drv_map[0],
+					fusion->drv_map_pages);
+			goto fail_ioc_init;
+		}
+	}
+
+	fusion->max_map_sz = max(fusion->old_map_sz, fusion->new_map_sz);
+
+	if (instance->supportmax256vd)
+		fusion->current_map_sz = fusion->new_map_sz;
+	else
+		fusion->current_map_sz = fusion->old_map_sz;
 
-	fusion->fast_path_io = 0;
 
 	for (i = 0; i < 2; i++) {
 		fusion->ld_map[i] = dma_alloc_coherent(&instance->pdev->dev,
-						       fusion->map_sz,
+						       fusion->max_map_sz,
 						       &fusion->ld_map_phys[i],
 						       GFP_KERNEL);
 		if (!fusion->ld_map[i]) {
@@ -1043,7 +1150,7 @@ megasas_init_adapter_fusion(struct megasas_instance *instance)
 
 fail_map_info:
 	if (i == 1)
-		dma_free_coherent(&instance->pdev->dev, fusion->map_sz,
+		dma_free_coherent(&instance->pdev->dev, fusion->max_map_sz,
 				  fusion->ld_map[0], fusion->ld_map_phys[0]);
 fail_ioc_init:
 	megasas_free_cmds_fusion(instance);
@@ -1065,6 +1172,11 @@ megasas_fire_cmd_fusion(struct megasas_instance *instance,
 			u32 req_desc_hi,
 			struct megasas_register_set __iomem *regs)
 {
+#if defined(writeq) && defined(CONFIG_64BIT)
+	u64 req_data = (((u64)req_desc_hi << 32) | (u32)req_desc_lo);
+
+	writeq(le64_to_cpu(req_data), &(regs)->inbound_low_queue_port);
+#else
 	unsigned long flags;
 
 	spin_lock_irqsave(&instance->hba_lock, flags);
@@ -1072,6 +1184,7 @@ megasas_fire_cmd_fusion(struct megasas_instance *instance,
 	writel(le32_to_cpu(req_desc_lo), &(regs)->inbound_low_queue_port);
 	writel(le32_to_cpu(req_desc_hi), &(regs)->inbound_high_queue_port);
 	spin_unlock_irqrestore(&instance->hba_lock, flags);
+#endif
 }
 
 /**
@@ -1224,7 +1337,7 @@ megasas_make_sgl_fusion(struct megasas_instance *instance,
 void
 megasas_set_pd_lba(struct MPI2_RAID_SCSI_IO_REQUEST *io_request, u8 cdb_len,
 		   struct IO_REQUEST_INFO *io_info, struct scsi_cmnd *scp,
-		   struct MR_FW_RAID_MAP_ALL *local_map_ptr, u32 ref_tag)
+		   struct MR_DRV_RAID_MAP_ALL *local_map_ptr, u32 ref_tag)
 {
 	struct MR_LD_RAID *raid;
 	u32 ld;
@@ -1409,7 +1522,7 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	union MEGASAS_REQUEST_DESCRIPTOR_UNION *req_desc;
 	struct IO_REQUEST_INFO io_info;
 	struct fusion_context *fusion;
-	struct MR_FW_RAID_MAP_ALL *local_map_ptr;
+	struct MR_DRV_RAID_MAP_ALL *local_map_ptr;
 	u8 *raidLUN;
 
 	device_id = MEGASAS_DEV_INDEX(instance, scp);
@@ -1486,10 +1599,10 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 	if (scp->sc_data_direction == PCI_DMA_FROMDEVICE)
 		io_info.isRead = 1;
 
-	local_map_ptr = fusion->ld_map[(instance->map_id & 1)];
+	local_map_ptr = fusion->ld_drv_map[(instance->map_id & 1)];
 
 	if ((MR_TargetIdToLdGet(device_id, local_map_ptr) >=
-	     MAX_LOGICAL_DRIVES) || (!fusion->fast_path_io)) {
+		instance->fw_supported_vd_count) || (!fusion->fast_path_io)) {
 		io_request->RaidContext.regLockFlags  = 0;
 		fp_possible = 0;
 	} else {
@@ -1529,10 +1642,11 @@ megasas_build_ldio_fusion(struct megasas_instance *instance,
 		if ((fusion->load_balance_info[device_id].loadBalanceFlag) &&
 		    (io_info.isRead)) {
 			io_info.devHandle =
-				get_updated_dev_handle(
+				get_updated_dev_handle(instance,
 					&fusion->load_balance_info[device_id],
 					&io_info);
 			scp->SCp.Status |= MEGASAS_LOAD_BALANCE_FLAG;
+			cmd->pd_r1_lb = io_info.pd_after_lb;
 		} else
 			scp->SCp.Status &= ~MEGASAS_LOAD_BALANCE_FLAG;
 		cmd->request_desc->SCSIIO.DevHandle = io_info.devHandle;
@@ -1579,7 +1693,7 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 	u32 device_id;
 	struct MPI2_RAID_SCSI_IO_REQUEST *io_request;
 	u16 pd_index = 0;
-	struct MR_FW_RAID_MAP_ALL *local_map_ptr;
+	struct MR_DRV_RAID_MAP_ALL *local_map_ptr;
 	struct fusion_context *fusion = instance->ctrl_context;
 	u8                          span, physArm;
 	u16                         devHandle;
@@ -1591,7 +1705,7 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 	device_id = MEGASAS_DEV_INDEX(instance, scmd);
 	pd_index = (scmd->device->channel * MEGASAS_MAX_DEV_PER_CHANNEL)
 		+scmd->device->id;
-	local_map_ptr = fusion->ld_map[(instance->map_id & 1)];
+	local_map_ptr = fusion->ld_drv_map[(instance->map_id & 1)];
 
 	io_request->DataLength = cpu_to_le32(scsi_bufflen(scmd));
 
@@ -1639,7 +1753,8 @@ megasas_build_dcdb_fusion(struct megasas_instance *instance,
 			goto NonFastPath;
 
 		ld = MR_TargetIdToLdGet(device_id, local_map_ptr);
-		if ((ld >= MAX_LOGICAL_DRIVES) || (!fusion->fast_path_io))
+		if ((ld >= instance->fw_supported_vd_count) ||
+			(!fusion->fast_path_io))
 			goto NonFastPath;
 
 		raid = MR_LdRaidGet(ld, local_map_ptr);
@@ -1864,10 +1979,11 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 	struct megasas_cmd *cmd_mfi;
 	struct megasas_cmd_fusion *cmd_fusion;
 	u16 smid, num_completed;
-	u8 reply_descript_type, arm;
+	u8 reply_descript_type;
 	u32 status, extStatus, device_id;
 	union desc_value d_val;
 	struct LD_LOAD_BALANCE_INFO *lbinfo;
+	int threshold_reply_count = 0;
 
 	fusion = instance->ctrl_context;
 
@@ -1914,10 +2030,7 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 			lbinfo = &fusion->load_balance_info[device_id];
 			if (cmd_fusion->scmd->SCp.Status &
 			    MEGASAS_LOAD_BALANCE_FLAG) {
-				arm = lbinfo->raid1DevHandle[0] ==
-					cmd_fusion->io_request->DevHandle ? 0 :
-					1;
-				atomic_dec(&lbinfo->scsi_pending_cmds[arm]);
+				atomic_dec(&lbinfo->scsi_pending_cmds[cmd_fusion->pd_r1_lb]);
 				cmd_fusion->scmd->SCp.Status &=
 					~MEGASAS_LOAD_BALANCE_FLAG;
 			}
@@ -1941,10 +2054,19 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 			break;
 		case MEGASAS_MPI2_FUNCTION_PASSTHRU_IO_REQUEST: /*MFI command */
 			cmd_mfi = instance->cmd_list[cmd_fusion->sync_cmd_idx];
+
+			if (!cmd_mfi->mpt_pthr_cmd_blocked) {
+				if (megasas_dbg_lvl == 5)
+					dev_info(&instance->pdev->dev,
+						"freeing mfi/mpt pass-through "
+						"from %s %d\n",
+						 __func__, __LINE__);
+				megasas_return_mfi_mpt_pthr(instance, cmd_mfi,
+					cmd_fusion);
+			}
+
 			megasas_complete_cmd(instance, cmd_mfi, DID_OK);
 			cmd_fusion->flags = 0;
-			megasas_return_cmd_fusion(instance, cmd_fusion);
-
 			break;
 		}
 
@@ -1955,6 +2077,7 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 
 		desc->Words = ULLONG_MAX;
 		num_completed++;
+		threshold_reply_count++;
 
 		/* Get the next reply descriptor */
 		if (!fusion->last_reply_idx[MSIxIndex])
@@ -1974,6 +2097,25 @@ complete_cmd_fusion(struct megasas_instance *instance, u32 MSIxIndex)
 
 		if (reply_descript_type == MPI2_RPY_DESCRIPT_FLAGS_UNUSED)
 			break;
+		/*
+		 * Write to reply post host index register after completing threshold
+		 * number of reply counts and still there are more replies in reply queue
+		 * pending to be completed
+		 */
+		if (threshold_reply_count >= THRESHOLD_REPLY_COUNT) {
+			if ((instance->pdev->device ==
+				PCI_DEVICE_ID_LSI_INVADER) ||
+				(instance->pdev->device ==
+				PCI_DEVICE_ID_LSI_FURY))
+				writel(((MSIxIndex & 0x7) << 24) |
+					fusion->last_reply_idx[MSIxIndex],
+					instance->reply_post_host_index_addr[MSIxIndex/8]);
+			else
+				writel((MSIxIndex << 24) |
+					fusion->last_reply_idx[MSIxIndex],
+					instance->reply_post_host_index_addr[0]);
+			threshold_reply_count = 0;
+		}
 	}
 
 	if (!num_completed)
@@ -2028,7 +2170,7 @@ irqreturn_t megasas_isr_fusion(int irq, void *devp)
 {
 	struct megasas_irq_context *irq_context = devp;
 	struct megasas_instance *instance = irq_context->instance;
-	u32 mfiStatus, fw_state;
+	u32 mfiStatus, fw_state, dma_state;
 
 	if (instance->mask_interrupts)
 		return IRQ_NONE;
@@ -2050,7 +2192,16 @@ irqreturn_t megasas_isr_fusion(int irq, void *devp)
 		/* If we didn't complete any commands, check for FW fault */
 		fw_state = instance->instancet->read_fw_status_reg(
 			instance->reg_set) & MFI_STATE_MASK;
-		if (fw_state == MFI_STATE_FAULT) {
+		dma_state = instance->instancet->read_fw_status_reg
+			(instance->reg_set) & MFI_STATE_DMADONE;
+		if (instance->crash_dump_drv_support &&
+			instance->crash_dump_app_support) {
+			/* Start collecting crash, if DMA bit is done */
+			if ((fw_state == MFI_STATE_FAULT) && dma_state)
+				schedule_work(&instance->crash_init);
+			else if (fw_state == MFI_STATE_FAULT)
+				schedule_work(&instance->work_init);
+		} else if (fw_state == MFI_STATE_FAULT) {
 			printk(KERN_WARNING "megaraid_sas: Iop2SysDoorbellInt"
 			       "for scsi%d\n", instance->host->host_no);
 			schedule_work(&instance->work_init);
@@ -2075,6 +2226,7 @@ build_mpt_mfi_pass_thru(struct megasas_instance *instance,
 	struct megasas_cmd_fusion *cmd;
 	struct fusion_context *fusion;
 	struct megasas_header *frame_hdr = &mfi_cmd->frame->hdr;
+	u32 opcode;
 
 	cmd = megasas_get_cmd_fusion(instance);
 	if (!cmd)
@@ -2082,9 +2234,20 @@ build_mpt_mfi_pass_thru(struct megasas_instance *instance,
 
 	/*  Save the smid. To be used for returning the cmd */
 	mfi_cmd->context.smid = cmd->index;
-
 	cmd->sync_cmd_idx = mfi_cmd->index;
 
+	/* Set this only for Blocked commands */
+	opcode = le32_to_cpu(mfi_cmd->frame->dcmd.opcode);
+	if ((opcode == MR_DCMD_LD_MAP_GET_INFO)
+		&& (mfi_cmd->frame->dcmd.mbox.b[1] == 1))
+		mfi_cmd->is_wait_event = 1;
+
+	if (opcode == MR_DCMD_CTRL_EVENT_WAIT)
+		mfi_cmd->is_wait_event = 1;
+
+	if (mfi_cmd->is_wait_event)
+		mfi_cmd->mpt_pthr_cmd_blocked = cmd;
+
 	/*
 	 * For cmds where the flag is set, store the flag and check
 	 * on completion. For cmds with this flag, don't call
@@ -2173,6 +2336,7 @@ megasas_issue_dcmd_fusion(struct megasas_instance *instance,
 		printk(KERN_ERR "Couldn't issue MFI pass thru cmd\n");
 		return;
 	}
+	atomic_set(&cmd->mfi_mpt_pthr, MFI_MPT_ATTACHED);
 	instance->instancet->fire_cmd(instance, req_desc->u.low,
 				      req_desc->u.high, instance->reg_set);
 }
@@ -2203,6 +2367,49 @@ megasas_read_fw_status_reg_fusion(struct megasas_register_set __iomem *regs)
 }
 
 /**
+ * megasas_alloc_host_crash_buffer -	Host buffers for Crash dump collection from Firmware
+ * @instance:				Controller's soft instance
+ * return:			        Number of allocated host crash buffers
+ */
+static void
+megasas_alloc_host_crash_buffer(struct megasas_instance *instance)
+{
+	unsigned int i;
+
+	instance->crash_buf_pages = get_order(CRASH_DMA_BUF_SIZE);
+	for (i = 0; i < MAX_CRASH_DUMP_SIZE; i++) {
+		instance->crash_buf[i] = (void	*)__get_free_pages(GFP_KERNEL,
+				instance->crash_buf_pages);
+		if (!instance->crash_buf[i]) {
+			dev_info(&instance->pdev->dev, "Firmware crash dump "
+				"memory allocation failed at index %d\n", i);
+			break;
+		}
+	}
+	instance->drv_buf_alloc = i;
+}
+
+/**
+ * megasas_free_host_crash_buffer -	Host buffers for Crash dump collection from Firmware
+ * @instance:				Controller's soft instance
+ */
+void
+megasas_free_host_crash_buffer(struct megasas_instance *instance)
+{
+	unsigned int i
+;
+	for (i = 0; i < instance->drv_buf_alloc; i++) {
+		if (instance->crash_buf[i])
+			free_pages((ulong)instance->crash_buf[i],
+					instance->crash_buf_pages);
+	}
+	instance->drv_buf_index = 0;
+	instance->drv_buf_alloc = 0;
+	instance->fw_crash_state = UNAVAILABLE;
+	instance->fw_crash_buffer_size = 0;
+}
+
+/**
  * megasas_adp_reset_fusion -	For controller reset
  * @regs:				MFI register set
  */
@@ -2345,6 +2552,7 @@ int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 	struct megasas_cmd *cmd_mfi;
 	union MEGASAS_REQUEST_DESCRIPTOR_UNION *req_desc;
 	u32 host_diag, abs_state, status_reg, reset_adapter;
+	u32 io_timeout_in_crash_mode = 0;
 
 	instance = (struct megasas_instance *)shost->hostdata;
 	fusion = instance->ctrl_context;
@@ -2355,8 +2563,45 @@ int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 		printk(KERN_WARNING "megaraid_sas: Hardware critical error, "
 		       "returning FAILED for scsi%d.\n",
 			instance->host->host_no);
+		mutex_unlock(&instance->reset_mutex);
 		return FAILED;
 	}
+	status_reg = instance->instancet->read_fw_status_reg(instance->reg_set);
+	abs_state = status_reg & MFI_STATE_MASK;
+
+	/* IO timeout detected, forcibly put FW in FAULT state */
+	if (abs_state != MFI_STATE_FAULT && instance->crash_dump_buf &&
+		instance->crash_dump_app_support && iotimeout) {
+		dev_info(&instance->pdev->dev, "IO timeout is detected, "
+			"forcibly FAULT Firmware\n");
+		instance->adprecovery = MEGASAS_ADPRESET_SM_INFAULT;
+		status_reg = readl(&instance->reg_set->doorbell);
+		writel(status_reg | MFI_STATE_FORCE_OCR,
+			&instance->reg_set->doorbell);
+		readl(&instance->reg_set->doorbell);
+		mutex_unlock(&instance->reset_mutex);
+		do {
+			ssleep(3);
+			io_timeout_in_crash_mode++;
+			dev_dbg(&instance->pdev->dev, "waiting for [%d] "
+				"seconds for crash dump collection and OCR "
+				"to be done\n", (io_timeout_in_crash_mode * 3));
+		} while ((instance->adprecovery != MEGASAS_HBA_OPERATIONAL) &&
+			(io_timeout_in_crash_mode < 80));
+
+		if (instance->adprecovery == MEGASAS_HBA_OPERATIONAL) {
+			dev_info(&instance->pdev->dev, "OCR done for IO "
+				"timeout case\n");
+			retval = SUCCESS;
+		} else {
+			dev_info(&instance->pdev->dev, "Controller is not "
+				"operational after 240 seconds wait for IO "
+				"timeout case in FW crash dump mode\n do "
+				"OCR/kill adapter\n");
+			retval = megasas_reset_fusion(shost, 0);
+		}
+		return retval;
+	}
 
 	if (instance->requestorId && !instance->skip_heartbeat_timer_del)
 		del_timer_sync(&instance->sriov_heartbeat_timer);
@@ -2563,10 +2808,7 @@ int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 					cmd_list[cmd_fusion->sync_cmd_idx];
 					if (cmd_mfi->frame->dcmd.opcode ==
 					    cpu_to_le32(MR_DCMD_LD_MAP_GET_INFO)) {
-						megasas_return_cmd(instance,
-								   cmd_mfi);
-						megasas_return_cmd_fusion(
-							instance, cmd_fusion);
+						megasas_return_mfi_mpt_pthr(instance, cmd_mfi, cmd_fusion);
 					} else  {
 						req_desc =
 						megasas_get_request_descriptor(
@@ -2603,7 +2845,7 @@ int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 			/* Reset load balance info */
 			memset(fusion->load_balance_info, 0,
 			       sizeof(struct LD_LOAD_BALANCE_INFO)
-			       *MAX_LOGICAL_DRIVES);
+			       *MAX_LOGICAL_DRIVES_EXT);
 
 			if (!megasas_get_map_info(instance))
 				megasas_sync_map_info(instance);
@@ -2623,6 +2865,15 @@ int megasas_reset_fusion(struct Scsi_Host *shost, int iotimeout)
 			printk(KERN_WARNING "megaraid_sas: Reset "
 			       "successful for scsi%d.\n",
 				instance->host->host_no);
+
+			if (instance->crash_dump_drv_support) {
+				if (instance->crash_dump_app_support)
+					megasas_set_crash_dump_params(instance,
+						MR_CRASH_BUF_TURN_ON);
+				else
+					megasas_set_crash_dump_params(instance,
+						MR_CRASH_BUF_TURN_OFF);
+			}
 			retval = SUCCESS;
 			goto out;
 		}
@@ -2651,6 +2902,74 @@ out:
 	return retval;
 }
 
+/* Fusion Crash dump collection work queue */
+void  megasas_fusion_crash_dump_wq(struct work_struct *work)
+{
+	struct megasas_instance *instance =
+		container_of(work, struct megasas_instance, crash_init);
+	u32 status_reg;
+	u8 partial_copy = 0;
+
+
+	status_reg = instance->instancet->read_fw_status_reg(instance->reg_set);
+
+	/*
+	 * Allocate host crash buffers to copy data from 1 MB DMA crash buffer
+	 * to host crash buffers
+	 */
+	if (instance->drv_buf_index == 0) {
+		/* Buffer is already allocated for old Crash dump.
+		 * Do OCR and do not wait for crash dump collection
+		 */
+		if (instance->drv_buf_alloc) {
+			dev_info(&instance->pdev->dev, "earlier crash dump is "
+				"not yet copied by application, ignoring this "
+				"crash dump and initiating OCR\n");
+			status_reg |= MFI_STATE_CRASH_DUMP_DONE;
+			writel(status_reg,
+				&instance->reg_set->outbound_scratch_pad);
+			readl(&instance->reg_set->outbound_scratch_pad);
+			return;
+		}
+		megasas_alloc_host_crash_buffer(instance);
+		dev_info(&instance->pdev->dev, "Number of host crash buffers "
+			"allocated: %d\n", instance->drv_buf_alloc);
+	}
+
+	/*
+	 * Driver has allocated max buffers, which can be allocated
+	 * and FW has more crash dump data, then driver will
+	 * ignore the data.
+	 */
+	if (instance->drv_buf_index >= (instance->drv_buf_alloc)) {
+		dev_info(&instance->pdev->dev, "Driver is done copying "
+			"the buffer: %d\n", instance->drv_buf_alloc);
+		status_reg |= MFI_STATE_CRASH_DUMP_DONE;
+		partial_copy = 1;
+	} else {
+		memcpy(instance->crash_buf[instance->drv_buf_index],
+			instance->crash_dump_buf, CRASH_DMA_BUF_SIZE);
+		instance->drv_buf_index++;
+		status_reg &= ~MFI_STATE_DMADONE;
+	}
+
+	if (status_reg & MFI_STATE_CRASH_DUMP_DONE) {
+		dev_info(&instance->pdev->dev, "Crash Dump is available,number "
+			"of copied buffers: %d\n", instance->drv_buf_index);
+		instance->fw_crash_buffer_size =  instance->drv_buf_index;
+		instance->fw_crash_state = AVAILABLE;
+		instance->drv_buf_index = 0;
+		writel(status_reg, &instance->reg_set->outbound_scratch_pad);
+		readl(&instance->reg_set->outbound_scratch_pad);
+		if (!partial_copy)
+			megasas_reset_fusion(instance->host, 0);
+	} else {
+		writel(status_reg, &instance->reg_set->outbound_scratch_pad);
+		readl(&instance->reg_set->outbound_scratch_pad);
+	}
+}
+
+
 /* Fusion OCR work queue */
 void megasas_fusion_ocr_wq(struct work_struct *work)
 {
diff --git a/drivers/scsi/megaraid/megaraid_sas_fusion.h b/drivers/scsi/megaraid/megaraid_sas_fusion.h
index e76af5459a09..0d183d521bdd 100644
--- a/drivers/scsi/megaraid/megaraid_sas_fusion.h
+++ b/drivers/scsi/megaraid/megaraid_sas_fusion.h
@@ -86,6 +86,7 @@ enum MR_RAID_FLAGS_IO_SUB_TYPE {
 
 #define MEGASAS_FP_CMD_LEN	16
 #define MEGASAS_FUSION_IN_RESET 0
+#define THRESHOLD_REPLY_COUNT 50
 
 /*
  * Raid Context structure which describes MegaRAID specific IO Parameters
@@ -478,10 +479,13 @@ struct MPI2_IOC_INIT_REQUEST {
 #define MAX_ROW_SIZE 32
 #define MAX_RAIDMAP_ROW_SIZE (MAX_ROW_SIZE)
 #define MAX_LOGICAL_DRIVES 64
+#define MAX_LOGICAL_DRIVES_EXT 256
 #define MAX_RAIDMAP_LOGICAL_DRIVES (MAX_LOGICAL_DRIVES)
 #define MAX_RAIDMAP_VIEWS (MAX_LOGICAL_DRIVES)
 #define MAX_ARRAYS 128
 #define MAX_RAIDMAP_ARRAYS (MAX_ARRAYS)
+#define MAX_ARRAYS_EXT	256
+#define MAX_API_ARRAYS_EXT (MAX_ARRAYS_EXT)
 #define MAX_PHYSICAL_DEVICES 256
 #define MAX_RAIDMAP_PHYSICAL_DEVICES (MAX_PHYSICAL_DEVICES)
 #define MR_DCMD_LD_MAP_GET_INFO             0x0300e101
@@ -601,7 +605,6 @@ struct MR_FW_RAID_MAP {
 			u32         maxArrays;
 		} validationInfo;
 		u32             version[5];
-		u32             reserved1[5];
 	};
 
 	u32                 ldCount;
@@ -627,6 +630,8 @@ struct IO_REQUEST_INFO {
 	u8 start_span;
 	u8 reserved;
 	u64 start_row;
+	u8  span_arm;	/* span[7:5], arm[4:0] */
+	u8  pd_after_lb;
 };
 
 struct MR_LD_TARGET_SYNC {
@@ -678,14 +683,14 @@ struct megasas_cmd_fusion {
 	u32 sync_cmd_idx;
 	u32 index;
 	u8 flags;
+	u8 pd_r1_lb;
 };
 
 struct LD_LOAD_BALANCE_INFO {
 	u8	loadBalanceFlag;
 	u8	reserved1;
-	u16     raid1DevHandle[2];
-	atomic_t     scsi_pending_cmds[2];
-	u64     last_accessed_block[2];
+	atomic_t     scsi_pending_cmds[MAX_PHYSICAL_DEVICES];
+	u64     last_accessed_block[MAX_PHYSICAL_DEVICES];
 };
 
 /* SPAN_SET is info caclulated from span info from Raid map per LD */
@@ -713,11 +718,86 @@ struct MR_FW_RAID_MAP_ALL {
 	struct MR_LD_SPAN_MAP ldSpanMap[MAX_LOGICAL_DRIVES - 1];
 } __attribute__ ((packed));
 
+struct MR_DRV_RAID_MAP {
+	/* total size of this structure, including this field.
+	 * This feild will be manupulated by driver for ext raid map,
+	 * else pick the value from firmware raid map.
+	 */
+	u32                 totalSize;
+
+	union {
+	struct {
+		u32         maxLd;
+		u32         maxSpanDepth;
+		u32         maxRowSize;
+		u32         maxPdCount;
+		u32         maxArrays;
+	} validationInfo;
+	u32             version[5];
+	};
+
+	/* timeout value used by driver in FP IOs*/
+	u8                  fpPdIoTimeoutSec;
+	u8                  reserved2[7];
+
+	u16                 ldCount;
+	u16                 arCount;
+	u16                 spanCount;
+	u16                 reserve3;
+
+	struct MR_DEV_HANDLE_INFO  devHndlInfo[MAX_RAIDMAP_PHYSICAL_DEVICES];
+	u8                  ldTgtIdToLd[MAX_LOGICAL_DRIVES_EXT];
+	struct MR_ARRAY_INFO       arMapInfo[MAX_API_ARRAYS_EXT];
+	struct MR_LD_SPAN_MAP      ldSpanMap[1];
+
+};
+
+/* Driver raid map size is same as raid map ext
+ * MR_DRV_RAID_MAP_ALL is created to sync with old raid.
+ * And it is mainly for code re-use purpose.
+ */
+struct MR_DRV_RAID_MAP_ALL {
+
+	struct MR_DRV_RAID_MAP raidMap;
+	struct MR_LD_SPAN_MAP      ldSpanMap[MAX_LOGICAL_DRIVES_EXT - 1];
+} __packed;
+
+
+
+struct MR_FW_RAID_MAP_EXT {
+	/* Not usred in new map */
+	u32                 reserved;
+
+	union {
+	struct {
+		u32         maxLd;
+		u32         maxSpanDepth;
+		u32         maxRowSize;
+		u32         maxPdCount;
+		u32         maxArrays;
+	} validationInfo;
+	u32             version[5];
+	};
+
+	u8                  fpPdIoTimeoutSec;
+	u8                  reserved2[7];
+
+	u16                 ldCount;
+	u16                 arCount;
+	u16                 spanCount;
+	u16                 reserve3;
+
+	struct MR_DEV_HANDLE_INFO  devHndlInfo[MAX_RAIDMAP_PHYSICAL_DEVICES];
+	u8                  ldTgtIdToLd[MAX_LOGICAL_DRIVES_EXT];
+	struct MR_ARRAY_INFO       arMapInfo[MAX_API_ARRAYS_EXT];
+	struct MR_LD_SPAN_MAP      ldSpanMap[MAX_LOGICAL_DRIVES_EXT];
+};
+
 struct fusion_context {
 	struct megasas_cmd_fusion **cmd_list;
 	struct list_head cmd_pool;
 
-	spinlock_t cmd_pool_lock;
+	spinlock_t mpt_pool_lock;
 
 	dma_addr_t req_frames_desc_phys;
 	u8 *req_frames_desc;
@@ -749,10 +829,18 @@ struct fusion_context {
 	struct MR_FW_RAID_MAP_ALL *ld_map[2];
 	dma_addr_t ld_map_phys[2];
 
-	u32 map_sz;
+	/*Non dma-able memory. Driver local copy.*/
+	struct MR_DRV_RAID_MAP_ALL *ld_drv_map[2];
+
+	u32 max_map_sz;
+	u32 current_map_sz;
+	u32 old_map_sz;
+	u32 new_map_sz;
+	u32 drv_map_sz;
+	u32 drv_map_pages;
 	u8 fast_path_io;
-	struct LD_LOAD_BALANCE_INFO load_balance_info[MAX_LOGICAL_DRIVES];
-	LD_SPAN_INFO log_to_span[MAX_LOGICAL_DRIVES];
+	struct LD_LOAD_BALANCE_INFO load_balance_info[MAX_LOGICAL_DRIVES_EXT];
+	LD_SPAN_INFO log_to_span[MAX_LOGICAL_DRIVES_EXT];
 };
 
 union desc_value {
@@ -763,4 +851,5 @@ union desc_value {
 	} u;
 };
 
+
 #endif /* _MEGARAID_SAS_FUSION_H_ */
diff --git a/drivers/scsi/mpt2sas/Kconfig b/drivers/scsi/mpt2sas/Kconfig
index 39f08dd20556..657b45ca04c5 100644
--- a/drivers/scsi/mpt2sas/Kconfig
+++ b/drivers/scsi/mpt2sas/Kconfig
@@ -2,7 +2,7 @@
 # Kernel configuration file for the MPT2SAS
 #
 # This code is based on drivers/scsi/mpt2sas/Kconfig
-# Copyright (C) 2007-2012  LSI Corporation
+# Copyright (C) 2007-2014  LSI Corporation
 #  (mailto:DL-MPTFusionLinux@lsi.com)
 
 # This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2.h b/drivers/scsi/mpt2sas/mpi/mpi2.h
index 7b14a015c903..088eefa67da8 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2.h
@@ -1,5 +1,5 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2.h
@@ -8,7 +8,7 @@
  *                  scatter/gather formats.
  *  Creation Date:  June 21, 2006
  *
- *  mpi2.h Version:  02.00.28
+ *  mpi2.h Version:  02.00.32
  *
  *  Version History
  *  ---------------
@@ -78,6 +78,11 @@
  *  07-10-12  02.00.26  Bumped MPI2_HEADER_VERSION_UNIT.
  *  07-26-12  02.00.27  Bumped MPI2_HEADER_VERSION_UNIT.
  *  11-27-12  02.00.28  Bumped MPI2_HEADER_VERSION_UNIT.
+ *  12-20-12  02.00.29  Bumped MPI2_HEADER_VERSION_UNIT.
+ *			Added MPI25_SUP_REPLY_POST_HOST_INDEX_OFFSET.
+ *  04-09-13  02.00.30  Bumped MPI2_HEADER_VERSION_UNIT.
+ *  04-17-13  02.00.31  Bumped MPI2_HEADER_VERSION_UNIT.
+ *  08-19-13  02.00.32  Bumped MPI2_HEADER_VERSION_UNIT.
  *  --------------------------------------------------------------------------
  */
 
@@ -103,7 +108,7 @@
 #define MPI2_VERSION_02_00                  (0x0200)
 
 /* versioning for this MPI header set */
-#define MPI2_HEADER_VERSION_UNIT            (0x1C)
+#define MPI2_HEADER_VERSION_UNIT            (0x20)
 #define MPI2_HEADER_VERSION_DEV             (0x00)
 #define MPI2_HEADER_VERSION_UNIT_MASK       (0xFF00)
 #define MPI2_HEADER_VERSION_UNIT_SHIFT      (8)
@@ -263,6 +268,7 @@ typedef volatile struct _MPI2_SYSTEM_INTERFACE_REGS
 #define MPI2_REPLY_POST_HOST_INDEX_MASK         (0x00FFFFFF)
 #define MPI2_RPHI_MSIX_INDEX_MASK               (0xFF000000)
 #define MPI2_RPHI_MSIX_INDEX_SHIFT              (24)
+#define MPI25_SUP_REPLY_POST_HOST_INDEX_OFFSET  (0x0000030C) /* MPI v2.5 only */
 
 /*
  * Defines for the HCBSize and address
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
index 88cb7f828bbd..510ef0dc8d7b 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_cnfg.h
@@ -1,12 +1,12 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_cnfg.h
  *          Title:  MPI Configuration messages and pages
  *  Creation Date:  November 10, 2006
  *
- *    mpi2_cnfg.h Version:  02.00.23
+ *    mpi2_cnfg.h Version:  02.00.26
  *
  *  Version History
  *  ---------------
@@ -150,7 +150,13 @@
  *                      Added UEFIVersion field to BIOS Page 1 and defined new
  *                      BiosOptions bits.
  *  11-27-12  02.00.23  Added MPI2_MANPAGE7_FLAG_EVENTREPLAY_SLOT_ORDER.
- *			 Added MPI2_BIOSPAGE1_OPTIONS_MASK_OEM_ID.
+ *			Added MPI2_BIOSPAGE1_OPTIONS_MASK_OEM_ID.
+ *  12-20-12  02.00.24  Marked MPI2_SASIOUNIT1_CONTROL_CLEAR_AFFILIATION as
+ *			obsolete for MPI v2.5 and later.
+ *			Added some defines for 12G SAS speeds.
+ *  04-09-13  02.00.25  Added MPI2_IOUNITPAGE1_ATA_SECURITY_FREEZE_LOCK.
+ *			Fixed MPI2_IOUNITPAGE5_DMA_CAP_MASK_MAX_REQUESTS to
+ *			match the specification.
  *  --------------------------------------------------------------------------
  */
 
@@ -773,6 +779,7 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_1
 #define MPI2_IOUNITPAGE1_PAGEVERSION                    (0x04)
 
 /* IO Unit Page 1 Flags defines */
+#define MPI2_IOUNITPAGE1_ATA_SECURITY_FREEZE_LOCK       (0x00004000)
 #define MPI2_IOUNITPAGE1_ENABLE_HOST_BASED_DISCOVERY    (0x00000800)
 #define MPI2_IOUNITPAGE1_MASK_SATA_WRITE_CACHE          (0x00000600)
 #define MPI2_IOUNITPAGE1_SATA_WRITE_CACHE_SHIFT         (9)
@@ -844,7 +851,7 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_5 {
 #define MPI2_IOUNITPAGE5_PAGEVERSION                    (0x00)
 
 /* defines for IO Unit Page 5 DmaEngineCapabilities field */
-#define MPI2_IOUNITPAGE5_DMA_CAP_MASK_MAX_REQUESTS      (0xFF00)
+#define MPI2_IOUNITPAGE5_DMA_CAP_MASK_MAX_REQUESTS      (0xFFFF0000)
 #define MPI2_IOUNITPAGE5_DMA_CAP_SHIFT_MAX_REQUESTS     (16)
 
 #define MPI2_IOUNITPAGE5_DMA_CAP_EEDP                   (0x0008)
@@ -885,13 +892,17 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_7 {
     U16                     IOCTemperature;                         /* 0x10 */
     U8                      IOCTemperatureUnits;                    /* 0x12 */
     U8                      IOCSpeed;                               /* 0x13 */
-	U16                     BoardTemperature;              /* 0x14 */
-	U8                      BoardTemperatureUnits;         /* 0x16 */
-	U8                      Reserved3;                     /* 0x17 */
+	U16                     BoardTemperature;		    /* 0x14 */
+	U8                      BoardTemperatureUnits;		    /* 0x16 */
+	U8                      Reserved3;			    /* 0x17 */
+	U32                     Reserved4;                          /* 0x18 */
+	U32                     Reserved5;                          /* 0x1C */
+	U32                     Reserved6;                          /* 0x20 */
+	U32                     Reserved7;                          /* 0x24 */
 } MPI2_CONFIG_PAGE_IO_UNIT_7, MPI2_POINTER PTR_MPI2_CONFIG_PAGE_IO_UNIT_7,
   Mpi2IOUnitPage7_t, MPI2_POINTER pMpi2IOUnitPage7_t;
 
-#define MPI2_IOUNITPAGE7_PAGEVERSION                    (0x02)
+#define MPI2_IOUNITPAGE7_PAGEVERSION                    (0x04)
 
 /* defines for IO Unit Page 7 PCIeWidth field */
 #define MPI2_IOUNITPAGE7_PCIE_WIDTH_X1              (0x01)
@@ -1801,6 +1812,7 @@ typedef struct _MPI2_CONFIG_PAGE_RD_PDISK_1
 #define MPI2_SAS_PRATE_MAX_RATE_1_5                     (0x80)
 #define MPI2_SAS_PRATE_MAX_RATE_3_0                     (0x90)
 #define MPI2_SAS_PRATE_MAX_RATE_6_0                     (0xA0)
+#define MPI25_SAS_PRATE_MAX_RATE_12_0                   (0xB0)
 #define MPI2_SAS_PRATE_MIN_RATE_MASK                    (0x0F)
 #define MPI2_SAS_PRATE_MIN_RATE_NOT_PROGRAMMABLE        (0x00)
 #define MPI2_SAS_PRATE_MIN_RATE_1_5                     (0x08)
@@ -1813,6 +1825,7 @@ typedef struct _MPI2_CONFIG_PAGE_RD_PDISK_1
 #define MPI2_SAS_HWRATE_MAX_RATE_1_5                    (0x80)
 #define MPI2_SAS_HWRATE_MAX_RATE_3_0                    (0x90)
 #define MPI2_SAS_HWRATE_MAX_RATE_6_0                    (0xA0)
+#define MPI25_SAS_HWRATE_MAX_RATE_12_0                  (0xB0)
 #define MPI2_SAS_HWRATE_MIN_RATE_MASK                   (0x0F)
 #define MPI2_SAS_HWRATE_MIN_RATE_1_5                    (0x08)
 #define MPI2_SAS_HWRATE_MIN_RATE_3_0                    (0x09)
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_init.h b/drivers/scsi/mpt2sas/mpi/mpi2_init.h
index 9d284dae6553..eea1a16b13ec 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_init.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_init.h
@@ -1,12 +1,12 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_init.h
  *          Title:  MPI SCSI initiator mode messages and structures
  *  Creation Date:  June 23, 2006
  *
- *    mpi2_init.h Version:  02.00.14
+ *    mpi2_init.h Version:  02.00.15
  *
  *  Version History
  *  ---------------
@@ -37,6 +37,8 @@
  *  02-06-12  02.00.13  Added alternate defines for Task Priority / Command
  *                      Priority to match SAM-4.
  *  07-10-12  02.00.14  Added MPI2_SCSIIO_CONTROL_SHIFT_DATADIRECTION.
+ *  04-09-13  02.00.15  Added SCSIStatusQualifier field to MPI2_SCSI_IO_REPLY,
+ *			replacing the Reserved4 field.
  *  --------------------------------------------------------------------------
  */
 
@@ -234,7 +236,7 @@ typedef struct _MPI2_SCSI_IO_REPLY
     U32                     SenseCount;                     /* 0x18 */
     U32                     ResponseInfo;                   /* 0x1C */
     U16                     TaskTag;                        /* 0x20 */
-    U16                     Reserved4;                      /* 0x22 */
+	U16                     SCSIStatusQualifier;	     /* 0x22 */
     U32                     BidirectionalTransferCount;     /* 0x24 */
     U32                     Reserved5;                      /* 0x28 */
     U32                     Reserved6;                      /* 0x2C */
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h b/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h
index d159c5f24aab..2c3b0f28576b 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_ioc.h
@@ -1,12 +1,12 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_ioc.h
  *          Title:  MPI IOC, Port, Event, FW Download, and FW Upload messages
  *  Creation Date:  October 11, 2006
  *
- *  mpi2_ioc.h Version:  02.00.22
+ *  mpi2_ioc.h Version:  02.00.23
  *
  *  Version History
  *  ---------------
@@ -121,6 +121,11 @@
  *  07-26-12  02.00.22  Added MPI2_IOCFACTS_EXCEPT_PARTIAL_MEMORY_FAILURE.
  *                      Added ElapsedSeconds field to
  *                      MPI2_EVENT_DATA_IR_OPERATION_STATUS.
+ *  08-19-13  02.00.23  For IOCInit, added MPI2_IOCINIT_MSGFLAG_RDPQ_ARRAY_MODE
+ *                      and MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY.
+ *                      Added MPI2_IOCFACTS_CAPABILITY_RDPQ_ARRAY_CAPABLE.
+ *                      Added MPI2_FW_DOWNLOAD_ITYPE_PUBLIC_KEY.
+ *                      Added Encrypted Hash Extended Image.
  *  --------------------------------------------------------------------------
  */
 
@@ -177,6 +182,9 @@ typedef struct _MPI2_IOC_INIT_REQUEST
 #define MPI2_WHOINIT_HOST_DRIVER                (0x04)
 #define MPI2_WHOINIT_MANUFACTURER               (0x05)
 
+/* MsgFlags */
+#define MPI2_IOCINIT_MSGFLAG_RDPQ_ARRAY_MODE    (0x01)
+
 /* MsgVersion */
 #define MPI2_IOCINIT_MSGVERSION_MAJOR_MASK      (0xFF00)
 #define MPI2_IOCINIT_MSGVERSION_MAJOR_SHIFT     (8)
@@ -189,9 +197,17 @@ typedef struct _MPI2_IOC_INIT_REQUEST
 #define MPI2_IOCINIT_HDRVERSION_DEV_MASK        (0x00FF)
 #define MPI2_IOCINIT_HDRVERSION_DEV_SHIFT       (0)
 
-/* minimum depth for the Reply Descriptor Post Queue */
+/* minimum depth for a Reply Descriptor Post Queue */
 #define MPI2_RDPQ_DEPTH_MIN                     (16)
 
+/* Reply Descriptor Post Queue Array Entry */
+typedef struct _MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY {
+	U64                 RDPQBaseAddress;                    /* 0x00 */
+	U32                 Reserved1;                          /* 0x08 */
+	U32                 Reserved2;                          /* 0x0C */
+} MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY,
+MPI2_POINTER PTR_MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY,
+Mpi2IOCInitRDPQArrayEntry, MPI2_POINTER pMpi2IOCInitRDPQArrayEntry;
 
 /* IOCInit Reply message */
 typedef struct _MPI2_IOC_INIT_REPLY
@@ -307,6 +323,7 @@ typedef struct _MPI2_IOC_FACTS_REPLY
 /* ProductID field uses MPI2_FW_HEADER_PID_ */
 
 /* IOCCapabilities */
+#define MPI2_IOCFACTS_CAPABILITY_RDPQ_ARRAY_CAPABLE     (0x00040000)
 #define MPI2_IOCFACTS_CAPABILITY_HOST_BASED_DISCOVERY   (0x00010000)
 #define MPI2_IOCFACTS_CAPABILITY_MSI_X_INDEX            (0x00008000)
 #define MPI2_IOCFACTS_CAPABILITY_RAID_ACCELERATOR       (0x00004000)
@@ -1153,6 +1170,7 @@ typedef struct _MPI2_FW_DOWNLOAD_REQUEST
 #define MPI2_FW_DOWNLOAD_ITYPE_MEGARAID             (0x09)
 #define MPI2_FW_DOWNLOAD_ITYPE_COMPLETE             (0x0A)
 #define MPI2_FW_DOWNLOAD_ITYPE_COMMON_BOOT_BLOCK    (0x0B)
+#define MPI2_FW_DOWNLOAD_ITYPE_PUBLIC_KEY           (0x0C)
 #define MPI2_FW_DOWNLOAD_ITYPE_MIN_PRODUCT_SPECIFIC (0xF0)
 
 /* FWDownload TransactionContext Element */
@@ -1379,14 +1397,15 @@ typedef struct _MPI2_EXT_IMAGE_HEADER
 #define MPI2_EXT_IMAGE_HEADER_SIZE              (0x40)
 
 /* defines for the ImageType field */
-#define MPI2_EXT_IMAGE_TYPE_UNSPECIFIED				(0x00)
-#define MPI2_EXT_IMAGE_TYPE_FW						(0x01)
-#define MPI2_EXT_IMAGE_TYPE_NVDATA					(0x03)
-#define MPI2_EXT_IMAGE_TYPE_BOOTLOADER				(0x04)
-#define MPI2_EXT_IMAGE_TYPE_INITIALIZATION			(0x05)
-#define MPI2_EXT_IMAGE_TYPE_FLASH_LAYOUT			(0x06)
-#define MPI2_EXT_IMAGE_TYPE_SUPPORTED_DEVICES		(0x07)
-#define MPI2_EXT_IMAGE_TYPE_MEGARAID				(0x08)
+#define MPI2_EXT_IMAGE_TYPE_UNSPECIFIED             (0x00)
+#define MPI2_EXT_IMAGE_TYPE_FW                      (0x01)
+#define MPI2_EXT_IMAGE_TYPE_NVDATA                  (0x03)
+#define MPI2_EXT_IMAGE_TYPE_BOOTLOADER              (0x04)
+#define MPI2_EXT_IMAGE_TYPE_INITIALIZATION          (0x05)
+#define MPI2_EXT_IMAGE_TYPE_FLASH_LAYOUT            (0x06)
+#define MPI2_EXT_IMAGE_TYPE_SUPPORTED_DEVICES       (0x07)
+#define MPI2_EXT_IMAGE_TYPE_MEGARAID                (0x08)
+#define MPI2_EXT_IMAGE_TYPE_ENCRYPTED_HASH          (0x09)
 #define MPI2_EXT_IMAGE_TYPE_MIN_PRODUCT_SPECIFIC    (0x80)
 #define MPI2_EXT_IMAGE_TYPE_MAX_PRODUCT_SPECIFIC    (0xFF)
 #define MPI2_EXT_IMAGE_TYPE_MAX                   \
@@ -1555,6 +1574,39 @@ typedef struct _MPI2_INIT_IMAGE_FOOTER
 #define MPI2_INIT_IMAGE_RESETVECTOR_OFFSET      (0x14)
 
 
+/* Encrypted Hash Extended Image Data */
+
+typedef struct _MPI25_ENCRYPTED_HASH_ENTRY {
+	U8                  HashImageType;          /* 0x00 */
+	U8                  HashAlgorithm;          /* 0x01 */
+	U8                  EncryptionAlgorithm;    /* 0x02 */
+	U8                  Reserved1;              /* 0x03 */
+	U32                 Reserved2;              /* 0x04 */
+	U32                 EncryptedHash[1];       /* 0x08 */
+} MPI25_ENCRYPTED_HASH_ENTRY, MPI2_POINTER PTR_MPI25_ENCRYPTED_HASH_ENTRY,
+Mpi25EncryptedHashEntry_t, MPI2_POINTER pMpi25EncryptedHashEntry_t;
+
+/* values for HashImageType */
+#define MPI25_HASH_IMAGE_TYPE_UNUSED            (0x00)
+#define MPI25_HASH_IMAGE_TYPE_FIRMWARE          (0x01)
+
+/* values for HashAlgorithm */
+#define MPI25_HASH_ALGORITHM_UNUSED             (0x00)
+#define MPI25_HASH_ALGORITHM_SHA256             (0x01)
+
+/* values for EncryptionAlgorithm */
+#define MPI25_ENCRYPTION_ALG_UNUSED             (0x00)
+#define MPI25_ENCRYPTION_ALG_RSA256             (0x01)
+
+typedef struct _MPI25_ENCRYPTED_HASH_DATA {
+	U8                              ImageVersion;           /* 0x00 */
+	U8                              NumHash;                /* 0x01 */
+	U16                             Reserved1;              /* 0x02 */
+	U32                             Reserved2;              /* 0x04 */
+	MPI25_ENCRYPTED_HASH_ENTRY      EncryptedHashEntry[1];  /* 0x08 */
+} MPI25_ENCRYPTED_HASH_DATA, MPI2_POINTER PTR_MPI25_ENCRYPTED_HASH_DATA,
+Mpi25EncryptedHashData_t, MPI2_POINTER pMpi25EncryptedHashData_t;
+
 /****************************************************************************
 *  PowerManagementControl message
 ****************************************************************************/
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_raid.h b/drivers/scsi/mpt2sas/mpi/mpi2_raid.h
index 0d202a2c6db7..7efa58ff0d34 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_raid.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_raid.h
@@ -1,12 +1,12 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_raid.h
  *          Title:  MPI Integrated RAID messages and structures
  *  Creation Date:  April 26, 2007
  *
- *    mpi2_raid.h Version:  02.00.09
+ *    mpi2_raid.h Version:  02.00.10
  *
  *  Version History
  *  ---------------
@@ -29,6 +29,7 @@
  *  02-06-12  02.00.08  Added MPI2_RAID_ACTION_PHYSDISK_HIDDEN.
  *  07-26-12  02.00.09  Added ElapsedSeconds field to MPI2_RAID_VOL_INDICATOR.
  *                      Added MPI2_RAID_VOL_FLAGS_ELAPSED_SECONDS_VALID define.
+ *  04-17-13  02.00.10  Added MPI25_RAID_ACTION_ADATA_ALLOW_PI.
  *  --------------------------------------------------------------------------
  */
 
@@ -45,6 +46,9 @@
 *  RAID Action messages
 ****************************************************************************/
 
+/* ActionDataWord defines for use with MPI2_RAID_ACTION_CREATE_VOLUME action */
+#define MPI25_RAID_ACTION_ADATA_ALLOW_PI            (0x80000000)
+
 /* ActionDataWord defines for use with MPI2_RAID_ACTION_DELETE_VOLUME action */
 #define MPI2_RAID_ACTION_ADATA_KEEP_LBA0            (0x00000000)
 #define MPI2_RAID_ACTION_ADATA_ZERO_LBA0            (0x00000001)
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_sas.h b/drivers/scsi/mpt2sas/mpi/mpi2_sas.h
index 50b39ccd526a..45b6fa10b803 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_sas.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_sas.h
@@ -1,5 +1,5 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_sas.h
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_tool.h b/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
index 11b2ac4e7c6e..9be03ed46180 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_tool.h
@@ -1,12 +1,12 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_tool.h
  *          Title:  MPI diagnostic tool structures and definitions
  *  Creation Date:  March 26, 2007
  *
- *    mpi2_tool.h Version:  02.00.10
+ *    mpi2_tool.h Version:  02.00.11
  *
  *  Version History
  *  ---------------
@@ -29,6 +29,7 @@
  *                      MPI2_TOOLBOX_ISTWI_READ_WRITE_REQUEST.
  *  07-26-12  02.00.10  Modified MPI2_TOOLBOX_DIAGNOSTIC_CLI_REQUEST so that
  *			it uses MPI Chain SGE as well as MPI Simple SGE.
+ * 08-19-13  02.00.11  Added MPI2_TOOLBOX_TEXT_DISPLAY_TOOL and related info.
  *  --------------------------------------------------------------------------
  */
 
@@ -48,6 +49,7 @@
 #define MPI2_TOOLBOX_ISTWI_READ_WRITE_TOOL          (0x03)
 #define MPI2_TOOLBOX_BEACON_TOOL                    (0x05)
 #define MPI2_TOOLBOX_DIAGNOSTIC_CLI_TOOL            (0x06)
+#define MPI2_TOOLBOX_TEXT_DISPLAY_TOOL              (0x07)
 
 
 /****************************************************************************
@@ -321,6 +323,44 @@ typedef struct _MPI2_TOOLBOX_DIAGNOSTIC_CLI_REPLY {
   MPI2_POINTER pMpi2ToolboxDiagnosticCliReply_t;
 
 
+/****************************************************************************
+*  Toolbox Console Text Display Tool
+****************************************************************************/
+
+/* Toolbox Console Text Display Tool request message */
+typedef struct _MPI2_TOOLBOX_TEXT_DISPLAY_REQUEST {
+	U8                      Tool;               /* 0x00 */
+	U8                      Reserved1;          /* 0x01 */
+	U8                      ChainOffset;        /* 0x02 */
+	U8                      Function;           /* 0x03 */
+	U16                     Reserved2;          /* 0x04 */
+	U8                      Reserved3;          /* 0x06 */
+	U8                      MsgFlags;           /* 0x07 */
+	U8                      VP_ID;              /* 0x08 */
+	U8                      VF_ID;              /* 0x09 */
+	U16                     Reserved4;          /* 0x0A */
+	U8                      Console;            /* 0x0C */
+	U8                      Flags;              /* 0x0D */
+	U16                     Reserved6;          /* 0x0E */
+	U8                      TextToDisplay[4];   /* 0x10 */
+} MPI2_TOOLBOX_TEXT_DISPLAY_REQUEST,
+MPI2_POINTER PTR_MPI2_TOOLBOX_TEXT_DISPLAY_REQUEST,
+Mpi2ToolboxTextDisplayRequest_t,
+MPI2_POINTER pMpi2ToolboxTextDisplayRequest_t;
+
+/* defines for the Console field */
+#define MPI2_TOOLBOX_CONSOLE_TYPE_MASK          (0xF0)
+#define MPI2_TOOLBOX_CONSOLE_TYPE_DEFAULT       (0x00)
+#define MPI2_TOOLBOX_CONSOLE_TYPE_UART          (0x10)
+#define MPI2_TOOLBOX_CONSOLE_TYPE_ETHERNET      (0x20)
+
+#define MPI2_TOOLBOX_CONSOLE_NUMBER_MASK        (0x0F)
+
+/* defines for the Flags field */
+#define MPI2_TOOLBOX_CONSOLE_FLAG_TIMESTAMP     (0x01)
+
+
+
 /*****************************************************************************
 *
 *       Diagnostic Buffer Messages
diff --git a/drivers/scsi/mpt2sas/mpi/mpi2_type.h b/drivers/scsi/mpt2sas/mpi/mpi2_type.h
index 0b128b68a5ea..6b0dcdd02f68 100644
--- a/drivers/scsi/mpt2sas/mpi/mpi2_type.h
+++ b/drivers/scsi/mpt2sas/mpi/mpi2_type.h
@@ -1,5 +1,5 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_type.h
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.c b/drivers/scsi/mpt2sas/mpt2sas_base.c
index 2f262be890c5..58e45216d1ec 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.c
@@ -3,7 +3,7 @@
  * for access to MPT (Message Passing Technology) firmware.
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_base.c
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
@@ -80,6 +80,10 @@ static int msix_disable = -1;
 module_param(msix_disable, int, 0);
 MODULE_PARM_DESC(msix_disable, " disable msix routed interrupts (default=0)");
 
+static int max_msix_vectors = -1;
+module_param(max_msix_vectors, int, 0);
+MODULE_PARM_DESC(max_msix_vectors, " max msix vectors ");
+
 static int mpt2sas_fwfault_debug;
 MODULE_PARM_DESC(mpt2sas_fwfault_debug, " enable detection of firmware fault "
 	"and halt firmware - (default=0)");
@@ -88,6 +92,12 @@ static int disable_discovery = -1;
 module_param(disable_discovery, int, 0);
 MODULE_PARM_DESC(disable_discovery, " disable discovery ");
 
+static int
+_base_get_ioc_facts(struct MPT2SAS_ADAPTER *ioc, int sleep_flag);
+
+static int
+_base_diag_reset(struct MPT2SAS_ADAPTER *ioc, int sleep_flag);
+
 /**
  * _scsih_set_fwfault_debug - global setting of ioc->fwfault_debug.
  *
@@ -1175,17 +1185,22 @@ static int
 _base_config_dma_addressing(struct MPT2SAS_ADAPTER *ioc, struct pci_dev *pdev)
 {
 	struct sysinfo s;
-	char *desc = NULL;
+	u64 consistent_dma_mask;
+
+	if (ioc->dma_mask)
+		consistent_dma_mask = DMA_BIT_MASK(64);
+	else
+		consistent_dma_mask = DMA_BIT_MASK(32);
 
 	if (sizeof(dma_addr_t) > 4) {
 		const uint64_t required_mask =
 		    dma_get_required_mask(&pdev->dev);
-		if ((required_mask > DMA_BIT_MASK(32)) && !pci_set_dma_mask(pdev,
-		    DMA_BIT_MASK(64)) && !pci_set_consistent_dma_mask(pdev,
-		    DMA_BIT_MASK(64))) {
+		if ((required_mask > DMA_BIT_MASK(32)) &&
+		    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64)) &&
+		    !pci_set_consistent_dma_mask(pdev, consistent_dma_mask)) {
 			ioc->base_add_sg_single = &_base_add_sg_single_64;
 			ioc->sge_size = sizeof(Mpi2SGESimple64_t);
-			desc = "64";
+			ioc->dma_mask = 64;
 			goto out;
 		}
 	}
@@ -1194,18 +1209,29 @@ _base_config_dma_addressing(struct MPT2SAS_ADAPTER *ioc, struct pci_dev *pdev)
 	    && !pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
 		ioc->base_add_sg_single = &_base_add_sg_single_32;
 		ioc->sge_size = sizeof(Mpi2SGESimple32_t);
-		desc = "32";
+		ioc->dma_mask = 32;
 	} else
 		return -ENODEV;
 
  out:
 	si_meminfo(&s);
-	printk(MPT2SAS_INFO_FMT "%s BIT PCI BUS DMA ADDRESSING SUPPORTED, "
-	    "total mem (%ld kB)\n", ioc->name, desc, convert_to_kb(s.totalram));
+	printk(MPT2SAS_INFO_FMT
+	    "%d BIT PCI BUS DMA ADDRESSING SUPPORTED, total mem (%ld kB)\n",
+	    ioc->name, ioc->dma_mask, convert_to_kb(s.totalram));
 
 	return 0;
 }
 
+static int
+_base_change_consistent_dma_mask(struct MPT2SAS_ADAPTER *ioc,
+				  struct pci_dev *pdev)
+{
+	if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64))) {
+		if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32)))
+			return -ENODEV;
+	}
+	return 0;
+}
 /**
  * _base_check_enable_msix - checks MSIX capabable.
  * @ioc: per adapter object
@@ -1402,6 +1428,20 @@ _base_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 	ioc->reply_queue_count = min_t(int, ioc->cpu_count,
 	    ioc->msix_vector_count);
 
+	if (!ioc->rdpq_array_enable && max_msix_vectors == -1)
+		max_msix_vectors = 8;
+
+	if (max_msix_vectors > 0) {
+		ioc->reply_queue_count = min_t(int, max_msix_vectors,
+		    ioc->reply_queue_count);
+		ioc->msix_vector_count = ioc->reply_queue_count;
+	} else if (max_msix_vectors == 0)
+		goto try_ioapic;
+
+	printk(MPT2SAS_INFO_FMT
+	"MSI-X vectors supported: %d, no of cores: %d, max_msix_vectors: %d\n",
+	 ioc->name, ioc->msix_vector_count, ioc->cpu_count, max_msix_vectors);
+
 	entries = kcalloc(ioc->reply_queue_count, sizeof(struct msix_entry),
 	    GFP_KERNEL);
 	if (!entries) {
@@ -1414,10 +1454,10 @@ _base_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 	for (i = 0, a = entries; i < ioc->reply_queue_count; i++, a++)
 		a->entry = i;
 
-	r = pci_enable_msix(ioc->pdev, entries, ioc->reply_queue_count);
+	r = pci_enable_msix_exact(ioc->pdev, entries, ioc->reply_queue_count);
 	if (r) {
-		dfailprintk(ioc, printk(MPT2SAS_INFO_FMT "pci_enable_msix "
-		    "failed (r=%d) !!!\n", ioc->name, r));
+		dfailprintk(ioc, printk(MPT2SAS_INFO_FMT
+		    "pci_enable_msix_exact failed (r=%d) !!!\n", ioc->name, r));
 		kfree(entries);
 		goto try_ioapic;
 	}
@@ -1439,6 +1479,7 @@ _base_enable_msix(struct MPT2SAS_ADAPTER *ioc)
 /* failback to io_apic interrupt routing */
  try_ioapic:
 
+	ioc->reply_queue_count = 1;
 	r = _base_request_irq(ioc, 0, ioc->pdev->irq);
 
 	return r;
@@ -1520,6 +1561,16 @@ mpt2sas_base_map_resources(struct MPT2SAS_ADAPTER *ioc)
 	}
 
 	_base_mask_interrupts(ioc);
+
+	r = _base_get_ioc_facts(ioc, CAN_SLEEP);
+	if (r)
+		goto out_fail;
+
+	if (!ioc->rdpq_array_enable_assigned) {
+		ioc->rdpq_array_enable = ioc->rdpq_array_capable;
+		ioc->rdpq_array_enable_assigned = 1;
+	}
+
 	r = _base_enable_msix(ioc);
 	if (r)
 		goto out_fail;
@@ -2317,7 +2368,8 @@ _base_static_config_pages(struct MPT2SAS_ADAPTER *ioc)
 static void
 _base_release_memory_pools(struct MPT2SAS_ADAPTER *ioc)
 {
-	int i;
+	int i = 0;
+	struct reply_post_struct *rps;
 
 	dexitprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -2358,15 +2410,25 @@ _base_release_memory_pools(struct MPT2SAS_ADAPTER *ioc)
 		ioc->reply_free = NULL;
 	}
 
-	if (ioc->reply_post_free) {
-		pci_pool_free(ioc->reply_post_free_dma_pool,
-		    ioc->reply_post_free, ioc->reply_post_free_dma);
+	if (ioc->reply_post) {
+		do {
+			rps = &ioc->reply_post[i];
+			if (rps->reply_post_free) {
+				pci_pool_free(
+				    ioc->reply_post_free_dma_pool,
+				    rps->reply_post_free,
+				    rps->reply_post_free_dma);
+				dexitprintk(ioc, printk(MPT2SAS_INFO_FMT
+				    "reply_post_free_pool(0x%p): free\n",
+				    ioc->name, rps->reply_post_free));
+				rps->reply_post_free = NULL;
+			}
+		} while (ioc->rdpq_array_enable &&
+			   (++i < ioc->reply_queue_count));
+
 		if (ioc->reply_post_free_dma_pool)
 			pci_pool_destroy(ioc->reply_post_free_dma_pool);
-		dexitprintk(ioc, printk(MPT2SAS_INFO_FMT
-		    "reply_post_free_pool(0x%p): free\n", ioc->name,
-		    ioc->reply_post_free));
-		ioc->reply_post_free = NULL;
+		kfree(ioc->reply_post);
 	}
 
 	if (ioc->config_page) {
@@ -2509,6 +2571,65 @@ _base_allocate_memory_pools(struct MPT2SAS_ADAPTER *ioc,  int sleep_flag)
 	    ioc->max_sges_in_chain_message, ioc->shost->sg_tablesize,
 	    ioc->chains_needed_per_io));
 
+	/* reply post queue, 16 byte align */
+	reply_post_free_sz = ioc->reply_post_queue_depth *
+	    sizeof(Mpi2DefaultReplyDescriptor_t);
+
+	sz = reply_post_free_sz;
+	if (_base_is_controller_msix_enabled(ioc) && !ioc->rdpq_array_enable)
+		sz *= ioc->reply_queue_count;
+
+	ioc->reply_post = kcalloc((ioc->rdpq_array_enable) ?
+	    (ioc->reply_queue_count):1,
+	    sizeof(struct reply_post_struct), GFP_KERNEL);
+
+	if (!ioc->reply_post) {
+		printk(MPT2SAS_ERR_FMT "reply_post_free pool: kcalloc failed\n",
+			ioc->name);
+		goto out;
+	}
+	ioc->reply_post_free_dma_pool = pci_pool_create("reply_post_free pool",
+	    ioc->pdev, sz, 16, 0);
+	if (!ioc->reply_post_free_dma_pool) {
+		printk(MPT2SAS_ERR_FMT
+		 "reply_post_free pool: pci_pool_create failed\n",
+		 ioc->name);
+		goto out;
+	}
+	i = 0;
+	do {
+		ioc->reply_post[i].reply_post_free =
+		    pci_pool_alloc(ioc->reply_post_free_dma_pool,
+		    GFP_KERNEL,
+		    &ioc->reply_post[i].reply_post_free_dma);
+		if (!ioc->reply_post[i].reply_post_free) {
+			printk(MPT2SAS_ERR_FMT
+			"reply_post_free pool: pci_pool_alloc failed\n",
+			ioc->name);
+			goto out;
+		}
+		memset(ioc->reply_post[i].reply_post_free, 0, sz);
+		dinitprintk(ioc, printk(MPT2SAS_INFO_FMT
+		    "reply post free pool (0x%p): depth(%d),"
+		    "element_size(%d), pool_size(%d kB)\n", ioc->name,
+		    ioc->reply_post[i].reply_post_free,
+		    ioc->reply_post_queue_depth, 8, sz/1024));
+		dinitprintk(ioc, printk(MPT2SAS_INFO_FMT
+		    "reply_post_free_dma = (0x%llx)\n", ioc->name,
+		    (unsigned long long)
+		    ioc->reply_post[i].reply_post_free_dma));
+		total_sz += sz;
+	} while (ioc->rdpq_array_enable && (++i < ioc->reply_queue_count));
+
+	if (ioc->dma_mask == 64) {
+		if (_base_change_consistent_dma_mask(ioc, ioc->pdev) != 0) {
+			printk(MPT2SAS_WARN_FMT
+			    "no suitable consistent DMA mask for %s\n",
+			    ioc->name, pci_name(ioc->pdev));
+			goto out;
+		}
+	}
+
 	ioc->scsiio_depth = ioc->hba_queue_depth -
 	    ioc->hi_priority_depth - ioc->internal_depth;
 
@@ -2720,37 +2841,6 @@ chain_done:
 	    "(0x%llx)\n", ioc->name, (unsigned long long)ioc->reply_free_dma));
 	total_sz += sz;
 
-	/* reply post queue, 16 byte align */
-	reply_post_free_sz = ioc->reply_post_queue_depth *
-	    sizeof(Mpi2DefaultReplyDescriptor_t);
-	if (_base_is_controller_msix_enabled(ioc))
-		sz = reply_post_free_sz * ioc->reply_queue_count;
-	else
-		sz = reply_post_free_sz;
-	ioc->reply_post_free_dma_pool = pci_pool_create("reply_post_free pool",
-	    ioc->pdev, sz, 16, 0);
-	if (!ioc->reply_post_free_dma_pool) {
-		printk(MPT2SAS_ERR_FMT "reply_post_free pool: pci_pool_create "
-		    "failed\n", ioc->name);
-		goto out;
-	}
-	ioc->reply_post_free = pci_pool_alloc(ioc->reply_post_free_dma_pool ,
-	    GFP_KERNEL, &ioc->reply_post_free_dma);
-	if (!ioc->reply_post_free) {
-		printk(MPT2SAS_ERR_FMT "reply_post_free pool: pci_pool_alloc "
-		    "failed\n", ioc->name);
-		goto out;
-	}
-	memset(ioc->reply_post_free, 0, sz);
-	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "reply post free pool"
-	    "(0x%p): depth(%d), element_size(%d), pool_size(%d kB)\n",
-	    ioc->name, ioc->reply_post_free, ioc->reply_post_queue_depth, 8,
-	    sz/1024));
-	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "reply_post_free_dma = "
-	    "(0x%llx)\n", ioc->name, (unsigned long long)
-	    ioc->reply_post_free_dma));
-	total_sz += sz;
-
 	ioc->config_page_sz = 512;
 	ioc->config_page = pci_alloc_consistent(ioc->pdev,
 	    ioc->config_page_sz, &ioc->config_page_dma);
@@ -3374,6 +3464,64 @@ _base_get_port_facts(struct MPT2SAS_ADAPTER *ioc, int port, int sleep_flag)
 }
 
 /**
+ * _base_wait_for_iocstate - Wait until the card is in READY or OPERATIONAL
+ * @ioc: per adapter object
+ * @timeout:
+ * @sleep_flag: CAN_SLEEP or NO_SLEEP
+ *
+ * Returns 0 for success, non-zero for failure.
+ */
+static int
+_base_wait_for_iocstate(struct MPT2SAS_ADAPTER *ioc, int timeout,
+	int sleep_flag)
+{
+	u32 ioc_state, doorbell;
+	int rc;
+
+	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
+	    __func__));
+
+	if (ioc->pci_error_recovery)
+		return 0;
+
+	doorbell = mpt2sas_base_get_iocstate(ioc, 0);
+	ioc_state = doorbell & MPI2_IOC_STATE_MASK;
+	dhsprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: ioc_state(0x%08x)\n",
+	    ioc->name, __func__, ioc_state));
+
+	switch (ioc_state) {
+	case MPI2_IOC_STATE_READY:
+	case MPI2_IOC_STATE_OPERATIONAL:
+		return 0;
+	}
+
+	if (doorbell & MPI2_DOORBELL_USED) {
+		dhsprintk(ioc, printk(MPT2SAS_INFO_FMT
+		    "unexpected doorbell activ!e\n", ioc->name));
+		goto issue_diag_reset;
+	}
+
+	if (ioc_state == MPI2_IOC_STATE_FAULT) {
+		mpt2sas_base_fault_info(ioc, doorbell &
+		    MPI2_DOORBELL_DATA_MASK);
+		goto issue_diag_reset;
+	}
+
+	ioc_state = _base_wait_on_iocstate(ioc, MPI2_IOC_STATE_READY,
+	    timeout, sleep_flag);
+	if (ioc_state) {
+		printk(MPT2SAS_ERR_FMT
+		    "%s: failed going to ready state (ioc_state=0x%x)\n",
+		    ioc->name, __func__, ioc_state);
+		return -EFAULT;
+	}
+
+ issue_diag_reset:
+	rc = _base_diag_reset(ioc, sleep_flag);
+	return rc;
+}
+
+/**
  * _base_get_ioc_facts - obtain ioc facts reply and save in ioc
  * @ioc: per adapter object
  * @sleep_flag: CAN_SLEEP or NO_SLEEP
@@ -3391,6 +3539,13 @@ _base_get_ioc_facts(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
 
+	r = _base_wait_for_iocstate(ioc, 10, sleep_flag);
+	if (r) {
+		printk(MPT2SAS_ERR_FMT "%s: failed getting to correct state\n",
+			ioc->name, __func__);
+		return r;
+	}
+
 	mpi_reply_sz = sizeof(Mpi2IOCFactsReply_t);
 	mpi_request_sz = sizeof(Mpi2IOCFactsRequest_t);
 	memset(&mpi_request, 0, mpi_request_sz);
@@ -3422,6 +3577,9 @@ _base_get_ioc_facts(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	facts->IOCCapabilities = le32_to_cpu(mpi_reply.IOCCapabilities);
 	if ((facts->IOCCapabilities & MPI2_IOCFACTS_CAPABILITY_INTEGRATED_RAID))
 		ioc->ir_firmware = 1;
+	if ((facts->IOCCapabilities &
+	      MPI2_IOCFACTS_CAPABILITY_RDPQ_ARRAY_CAPABLE))
+		ioc->rdpq_array_capable = 1;
 	facts->FWVersion.Word = le32_to_cpu(mpi_reply.FWVersion.Word);
 	facts->IOCRequestFrameSize =
 	    le16_to_cpu(mpi_reply.IOCRequestFrameSize);
@@ -3457,9 +3615,12 @@ _base_send_ioc_init(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 {
 	Mpi2IOCInitRequest_t mpi_request;
 	Mpi2IOCInitReply_t mpi_reply;
-	int r;
+	int i, r = 0;
 	struct timeval current_time;
 	u16 ioc_status;
+	u32 reply_post_free_array_sz = 0;
+	Mpi2IOCInitRDPQArrayEntry *reply_post_free_array = NULL;
+	dma_addr_t reply_post_free_array_dma;
 
 	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -3488,9 +3649,31 @@ _base_send_ioc_init(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	    cpu_to_le64((u64)ioc->request_dma);
 	mpi_request.ReplyFreeQueueAddress =
 	    cpu_to_le64((u64)ioc->reply_free_dma);
-	mpi_request.ReplyDescriptorPostQueueAddress =
-	    cpu_to_le64((u64)ioc->reply_post_free_dma);
 
+	if (ioc->rdpq_array_enable) {
+		reply_post_free_array_sz = ioc->reply_queue_count *
+		    sizeof(Mpi2IOCInitRDPQArrayEntry);
+		reply_post_free_array = pci_alloc_consistent(ioc->pdev,
+			reply_post_free_array_sz, &reply_post_free_array_dma);
+		if (!reply_post_free_array) {
+			printk(MPT2SAS_ERR_FMT
+			"reply_post_free_array: pci_alloc_consistent failed\n",
+			ioc->name);
+			r = -ENOMEM;
+			goto out;
+		}
+		memset(reply_post_free_array, 0, reply_post_free_array_sz);
+		for (i = 0; i < ioc->reply_queue_count; i++)
+			reply_post_free_array[i].RDPQBaseAddress =
+			    cpu_to_le64(
+				(u64)ioc->reply_post[i].reply_post_free_dma);
+		mpi_request.MsgFlags = MPI2_IOCINIT_MSGFLAG_RDPQ_ARRAY_MODE;
+		mpi_request.ReplyDescriptorPostQueueAddress =
+		    cpu_to_le64((u64)reply_post_free_array_dma);
+	} else {
+		mpi_request.ReplyDescriptorPostQueueAddress =
+		    cpu_to_le64((u64)ioc->reply_post[0].reply_post_free_dma);
+	}
 
 	/* This time stamp specifies number of milliseconds
 	 * since epoch ~ midnight January 1, 1970.
@@ -3518,7 +3701,7 @@ _base_send_ioc_init(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	if (r != 0) {
 		printk(MPT2SAS_ERR_FMT "%s: handshake failed (r=%d)\n",
 		    ioc->name, __func__, r);
-		return r;
+		goto out;
 	}
 
 	ioc_status = le16_to_cpu(mpi_reply.IOCStatus) & MPI2_IOCSTATUS_MASK;
@@ -3528,7 +3711,12 @@ _base_send_ioc_init(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 		r = -EIO;
 	}
 
-	return 0;
+out:
+	if (reply_post_free_array)
+		pci_free_consistent(ioc->pdev, reply_post_free_array_sz,
+				    reply_post_free_array,
+				    reply_post_free_array_dma);
+	return r;
 }
 
 /**
@@ -4061,7 +4249,7 @@ _base_make_ioc_operational(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 	u8 hide_flag;
 	struct adapter_reply_queue *reply_q;
 	long reply_post_free;
-	u32 reply_post_free_sz;
+	u32 reply_post_free_sz, index = 0;
 
 	dinitprintk(ioc, printk(MPT2SAS_INFO_FMT "%s\n", ioc->name,
 	    __func__));
@@ -4132,19 +4320,27 @@ _base_make_ioc_operational(struct MPT2SAS_ADAPTER *ioc, int sleep_flag)
 		_base_assign_reply_queues(ioc);
 
 	/* initialize Reply Post Free Queue */
-	reply_post_free = (long)ioc->reply_post_free;
 	reply_post_free_sz = ioc->reply_post_queue_depth *
 	    sizeof(Mpi2DefaultReplyDescriptor_t);
+	reply_post_free = (long)ioc->reply_post[index].reply_post_free;
 	list_for_each_entry(reply_q, &ioc->reply_queue_list, list) {
 		reply_q->reply_post_host_index = 0;
 		reply_q->reply_post_free = (Mpi2ReplyDescriptorsUnion_t *)
 		    reply_post_free;
 		for (i = 0; i < ioc->reply_post_queue_depth; i++)
 			reply_q->reply_post_free[i].Words =
-							cpu_to_le64(ULLONG_MAX);
+						     cpu_to_le64(ULLONG_MAX);
 		if (!_base_is_controller_msix_enabled(ioc))
 			goto skip_init_reply_post_free_queue;
-		reply_post_free += reply_post_free_sz;
+		/*
+		 * If RDPQ is enabled, switch to the next allocation.
+		 * Otherwise advance within the contiguous region.
+		 */
+		if (ioc->rdpq_array_enable)
+			reply_post_free = (long)
+			    ioc->reply_post[++index].reply_post_free;
+		else
+			reply_post_free += reply_post_free_sz;
 	}
  skip_init_reply_post_free_queue:
 
@@ -4272,6 +4468,8 @@ mpt2sas_base_attach(struct MPT2SAS_ADAPTER *ioc)
 		}
 	}
 
+	ioc->rdpq_array_enable_assigned = 0;
+	ioc->dma_mask = 0;
 	r = mpt2sas_base_map_resources(ioc);
 	if (r)
 		goto out_free_resources;
@@ -4633,6 +4831,16 @@ mpt2sas_base_hard_reset_handler(struct MPT2SAS_ADAPTER *ioc, int sleep_flag,
 		r = -EFAULT;
 		goto out;
 	}
+
+	r = _base_get_ioc_facts(ioc, CAN_SLEEP);
+	if (r)
+		goto out;
+
+	if (ioc->rdpq_array_enable && !ioc->rdpq_array_capable)
+		panic("%s: Issue occurred with flashing controller firmware."
+		      "Please reboot the system and ensure that the correct"
+		      " firmware version is running\n", ioc->name);
+
 	r = _base_make_ioc_operational(ioc, sleep_flag);
 	if (!r)
 		_base_reset_handler(ioc, MPT2_IOC_DONE_RESET);
diff --git a/drivers/scsi/mpt2sas/mpt2sas_base.h b/drivers/scsi/mpt2sas/mpt2sas_base.h
index 0ac5815a7f91..239f169b0673 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_base.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_base.h
@@ -3,7 +3,7 @@
  * for access to MPT (Message Passing Technology) firmware.
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_base.h
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
@@ -69,8 +69,8 @@
 #define MPT2SAS_DRIVER_NAME		"mpt2sas"
 #define MPT2SAS_AUTHOR	"LSI Corporation <DL-MPTFusionLinux@lsi.com>"
 #define MPT2SAS_DESCRIPTION	"LSI MPT Fusion SAS 2.0 Device Driver"
-#define MPT2SAS_DRIVER_VERSION		"16.100.00.00"
-#define MPT2SAS_MAJOR_VERSION		16
+#define MPT2SAS_DRIVER_VERSION		"18.100.00.00"
+#define MPT2SAS_MAJOR_VERSION		18
 #define MPT2SAS_MINOR_VERSION		100
 #define MPT2SAS_BUILD_VERSION		00
 #define MPT2SAS_RELEASE_VERSION		00
@@ -355,6 +355,7 @@ struct _internal_cmd {
  * @slot: number number
  * @phy: phy identifier provided in sas device page 0
  * @responding: used in _scsih_sas_device_mark_responding
+ * @pfa_led_on: flag for PFA LED status
  */
 struct _sas_device {
 	struct list_head list;
@@ -373,6 +374,7 @@ struct _sas_device {
 	u16	slot;
 	u8	phy;
 	u8	responding;
+	u8	pfa_led_on;
 };
 
 /**
@@ -634,6 +636,11 @@ struct mpt2sas_port_facts {
 	u16			MaxPostedCmdBuffers;
 };
 
+struct reply_post_struct {
+	Mpi2ReplyDescriptorsUnion_t	*reply_post_free;
+	dma_addr_t			reply_post_free_dma;
+};
+
 /**
  * enum mutex_type - task management mutex type
  * @TM_MUTEX_OFF: mutex is not required becuase calling function is acquiring it
@@ -661,6 +668,7 @@ typedef void (*MPT2SAS_FLUSH_RUNNING_CMDS)(struct MPT2SAS_ADAPTER *ioc);
  * @ir_firmware: IR firmware present
  * @bars: bitmask of BAR's that must be configured
  * @mask_interrupts: ignore interrupt
+ * @dma_mask: used to set the consistent dma mask
  * @fault_reset_work_q_name: fw fault work queue
  * @fault_reset_work_q: ""
  * @fault_reset_work: ""
@@ -777,8 +785,11 @@ typedef void (*MPT2SAS_FLUSH_RUNNING_CMDS)(struct MPT2SAS_ADAPTER *ioc);
  * @reply_free_dma_pool:
  * @reply_free_host_index: tail index in pool to insert free replys
  * @reply_post_queue_depth: reply post queue depth
- * @reply_post_free: pool for reply post (64bit descriptor)
- * @reply_post_free_dma:
+ * @reply_post_struct: struct for reply_post_free physical & virt address
+ * @rdpq_array_capable: FW supports multiple reply queue addresses in ioc_init
+ * @rdpq_array_enable: rdpq_array support is enabled in the driver
+ * @rdpq_array_enable_assigned: this ensures that rdpq_array_enable flag
+ *				is assigned only ones
  * @reply_queue_count: number of reply queue's
  * @reply_queue_list: link list contaning the reply queue info
  * @reply_post_host_index: head index in the pool where FW completes IO
@@ -800,6 +811,7 @@ struct MPT2SAS_ADAPTER {
 	u8		ir_firmware;
 	int		bars;
 	u8		mask_interrupts;
+	int		dma_mask;
 
 	/* fw fault handler */
 	char		fault_reset_work_q_name[20];
@@ -970,8 +982,10 @@ struct MPT2SAS_ADAPTER {
 
 	/* reply post queue */
 	u16 		reply_post_queue_depth;
-	Mpi2ReplyDescriptorsUnion_t *reply_post_free;
-	dma_addr_t	reply_post_free_dma;
+	struct reply_post_struct *reply_post;
+	u8		rdpq_array_capable;
+	u8		rdpq_array_enable;
+	u8		rdpq_array_enable_assigned;
 	struct dma_pool *reply_post_free_dma_pool;
 	u8		reply_queue_count;
 	struct list_head reply_queue_list;
diff --git a/drivers/scsi/mpt2sas/mpt2sas_config.c b/drivers/scsi/mpt2sas/mpt2sas_config.c
index 0c47425c73f2..c72a2fff5dbb 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_config.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_config.c
@@ -2,7 +2,7 @@
  * This module provides common API for accessing firmware configuration pages
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_base.c
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt2sas/mpt2sas_ctl.c b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
index 62df8f9d4271..ca4e563c01dd 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_ctl.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_ctl.c
@@ -3,7 +3,7 @@
  * controllers
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_ctl.c
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt2sas/mpt2sas_ctl.h b/drivers/scsi/mpt2sas/mpt2sas_ctl.h
index 8b2ac1869dcc..fa0567c96050 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_ctl.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_ctl.h
@@ -3,7 +3,7 @@
  * controllers
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_ctl.h
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt2sas/mpt2sas_debug.h b/drivers/scsi/mpt2sas/mpt2sas_debug.h
index a9021cbd6628..cc57ef31d0fe 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_debug.h
+++ b/drivers/scsi/mpt2sas/mpt2sas_debug.h
@@ -2,7 +2,7 @@
  * Logging Support for MPT (Message Passing Technology) based controllers
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_debug.c
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt2sas/mpt2sas_scsih.c b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
index dd461015813f..c80ed0482649 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_scsih.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_scsih.c
@@ -2,7 +2,7 @@
  * Scsi Host Layer for MPT (Message Passing Technology) based controllers
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_scsih.c
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
@@ -55,6 +55,8 @@
 #include <linux/raid_class.h>
 #include <linux/slab.h>
 
+#include <asm/unaligned.h>
+
 #include "mpt2sas_base.h"
 
 MODULE_AUTHOR(MPT2SAS_AUTHOR);
@@ -145,7 +147,7 @@ struct sense_info {
 };
 
 
-#define MPT2SAS_TURN_ON_FAULT_LED (0xFFFC)
+#define MPT2SAS_TURN_ON_PFA_LED (0xFFFC)
 #define MPT2SAS_PORT_ENABLE_COMPLETE (0xFFFD)
 #define MPT2SAS_REMOVE_UNRESPONDING_DEVICES (0xFFFF)
 /**
@@ -3858,85 +3860,46 @@ _scsih_setup_direct_io(struct MPT2SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
 	struct _raid_device *raid_device, Mpi2SCSIIORequest_t *mpi_request,
 	u16 smid)
 {
-	u32 v_lba, p_lba, stripe_off, stripe_unit, column, io_size;
+	sector_t v_lba, p_lba, stripe_off, column, io_size;
 	u32 stripe_sz, stripe_exp;
-	u8 num_pds, *cdb_ptr, i;
-	u8 cdb0 = scmd->cmnd[0];
-	u64 v_llba;
+	u8 num_pds, cmd = scmd->cmnd[0];
 
-	/*
-	 * Try Direct I/O to RAID memeber disks
-	 */
-	if (cdb0 == READ_16 || cdb0 == READ_10 ||
-	    cdb0 == WRITE_16 || cdb0 == WRITE_10) {
-		cdb_ptr = mpi_request->CDB.CDB32;
-
-		if ((cdb0 < READ_16) || !(cdb_ptr[2] | cdb_ptr[3] | cdb_ptr[4]
-			| cdb_ptr[5])) {
-			io_size = scsi_bufflen(scmd) >>
-			    raid_device->block_exponent;
-			i = (cdb0 < READ_16) ? 2 : 6;
-			/* get virtual lba */
-			v_lba = be32_to_cpu(*(__be32 *)(&cdb_ptr[i]));
-
-			if (((u64)v_lba + (u64)io_size - 1) <=
-			    (u32)raid_device->max_lba) {
-				stripe_sz = raid_device->stripe_sz;
-				stripe_exp = raid_device->stripe_exponent;
-				stripe_off = v_lba & (stripe_sz - 1);
-
-				/* Check whether IO falls within a stripe */
-				if ((stripe_off + io_size) <= stripe_sz) {
-					num_pds = raid_device->num_pds;
-					p_lba = v_lba >> stripe_exp;
-					stripe_unit = p_lba / num_pds;
-					column = p_lba % num_pds;
-					p_lba = (stripe_unit << stripe_exp) +
-					    stripe_off;
-					mpi_request->DevHandle =
-						cpu_to_le16(raid_device->
-						    pd_handle[column]);
-					(*(__be32 *)(&cdb_ptr[i])) =
-						cpu_to_be32(p_lba);
-					/*
-					* WD: To indicate this I/O is directI/O
-					*/
-					_scsih_scsi_direct_io_set(ioc, smid, 1);
-				}
-			}
-		} else {
-			io_size = scsi_bufflen(scmd) >>
-			    raid_device->block_exponent;
-			/* get virtual lba */
-			v_llba = be64_to_cpu(*(__be64 *)(&cdb_ptr[2]));
-
-			if ((v_llba + (u64)io_size - 1) <=
-			    raid_device->max_lba) {
-				stripe_sz = raid_device->stripe_sz;
-				stripe_exp = raid_device->stripe_exponent;
-				stripe_off = (u32) (v_llba & (stripe_sz - 1));
-
-				/* Check whether IO falls within a stripe */
-				if ((stripe_off + io_size) <= stripe_sz) {
-					num_pds = raid_device->num_pds;
-					p_lba = (u32)(v_llba >> stripe_exp);
-					stripe_unit = p_lba / num_pds;
-					column = p_lba % num_pds;
-					p_lba = (stripe_unit << stripe_exp) +
-					    stripe_off;
-					mpi_request->DevHandle =
-						cpu_to_le16(raid_device->
-						    pd_handle[column]);
-					(*(__be64 *)(&cdb_ptr[2])) =
-					    cpu_to_be64((u64)p_lba);
-					/*
-					* WD: To indicate this I/O is directI/O
-					*/
-					_scsih_scsi_direct_io_set(ioc, smid, 1);
-				}
-			}
-		}
-	}
+	if (cmd != READ_10 && cmd != WRITE_10 &&
+	    cmd != READ_16 && cmd != WRITE_16)
+		return;
+
+	if (cmd == READ_10 || cmd == WRITE_10)
+		v_lba = get_unaligned_be32(&mpi_request->CDB.CDB32[2]);
+	else
+		v_lba = get_unaligned_be64(&mpi_request->CDB.CDB32[2]);
+
+	io_size = scsi_bufflen(scmd) >> raid_device->block_exponent;
+
+	if (v_lba + io_size - 1 > raid_device->max_lba)
+		return;
+
+	stripe_sz = raid_device->stripe_sz;
+	stripe_exp = raid_device->stripe_exponent;
+	stripe_off = v_lba & (stripe_sz - 1);
+
+	/* Return unless IO falls within a stripe */
+	if (stripe_off + io_size > stripe_sz)
+		return;
+
+	num_pds = raid_device->num_pds;
+	p_lba = v_lba >> stripe_exp;
+	column = sector_div(p_lba, num_pds);
+	p_lba = (p_lba << stripe_exp) + stripe_off;
+
+	mpi_request->DevHandle = cpu_to_le16(raid_device->pd_handle[column]);
+
+	if (cmd == READ_10 || cmd == WRITE_10)
+		put_unaligned_be32(lower_32_bits(p_lba),
+				   &mpi_request->CDB.CDB32[2]);
+	else
+		put_unaligned_be64(p_lba, &mpi_request->CDB.CDB32[2]);
+
+	_scsih_scsi_direct_io_set(ioc, smid, 1);
 }
 
 /**
@@ -4308,7 +4271,7 @@ _scsih_scsi_ioc_info(struct MPT2SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
 #endif
 
 /**
- * _scsih_turn_on_fault_led - illuminate Fault LED
+ * _scsih_turn_on_pfa_led - illuminate PFA LED
  * @ioc: per adapter object
  * @handle: device handle
  * Context: process
@@ -4316,10 +4279,15 @@ _scsih_scsi_ioc_info(struct MPT2SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
  * Return nothing.
  */
 static void
-_scsih_turn_on_fault_led(struct MPT2SAS_ADAPTER *ioc, u16 handle)
+_scsih_turn_on_pfa_led(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 {
 	Mpi2SepReply_t mpi_reply;
 	Mpi2SepRequest_t mpi_request;
+	struct _sas_device *sas_device;
+
+	sas_device = _scsih_sas_device_find_by_handle(ioc, handle);
+	if (!sas_device)
+		return;
 
 	memset(&mpi_request, 0, sizeof(Mpi2SepRequest_t));
 	mpi_request.Function = MPI2_FUNCTION_SCSI_ENCLOSURE_PROCESSOR;
@@ -4334,6 +4302,47 @@ _scsih_turn_on_fault_led(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 		__FILE__, __LINE__, __func__);
 		return;
 	}
+	sas_device->pfa_led_on = 1;
+
+
+	if (mpi_reply.IOCStatus || mpi_reply.IOCLogInfo) {
+		dewtprintk(ioc, printk(MPT2SAS_INFO_FMT
+		 "enclosure_processor: ioc_status (0x%04x), loginfo(0x%08x)\n",
+		 ioc->name, le16_to_cpu(mpi_reply.IOCStatus),
+		 le32_to_cpu(mpi_reply.IOCLogInfo)));
+		return;
+	}
+}
+
+/**
+ * _scsih_turn_off_pfa_led - turn off PFA LED
+ * @ioc: per adapter object
+ * @sas_device: sas device whose PFA LED has to turned off
+ * Context: process
+ *
+ * Return nothing.
+ */
+static void
+_scsih_turn_off_pfa_led(struct MPT2SAS_ADAPTER *ioc,
+	struct _sas_device *sas_device)
+{
+	Mpi2SepReply_t mpi_reply;
+	Mpi2SepRequest_t mpi_request;
+
+	memset(&mpi_request, 0, sizeof(Mpi2SepRequest_t));
+	mpi_request.Function = MPI2_FUNCTION_SCSI_ENCLOSURE_PROCESSOR;
+	mpi_request.Action = MPI2_SEP_REQ_ACTION_WRITE_STATUS;
+	mpi_request.SlotStatus = 0;
+	mpi_request.Slot = cpu_to_le16(sas_device->slot);
+	mpi_request.DevHandle = 0;
+	mpi_request.EnclosureHandle = cpu_to_le16(sas_device->enclosure_handle);
+	mpi_request.Flags = MPI2_SEP_REQ_FLAGS_ENCLOSURE_SLOT_ADDRESS;
+	if ((mpt2sas_base_scsi_enclosure_processor(ioc, &mpi_reply,
+	    &mpi_request)) != 0) {
+		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n", ioc->name,
+		__FILE__, __LINE__, __func__);
+		return;
+	}
 
 	if (mpi_reply.IOCStatus || mpi_reply.IOCLogInfo) {
 		dewtprintk(ioc, printk(MPT2SAS_INFO_FMT "enclosure_processor: "
@@ -4345,7 +4354,7 @@ _scsih_turn_on_fault_led(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 }
 
 /**
- * _scsih_send_event_to_turn_on_fault_led - fire delayed event
+ * _scsih_send_event_to_turn_on_pfa_led - fire delayed event
  * @ioc: per adapter object
  * @handle: device handle
  * Context: interrupt.
@@ -4353,14 +4362,14 @@ _scsih_turn_on_fault_led(struct MPT2SAS_ADAPTER *ioc, u16 handle)
  * Return nothing.
  */
 static void
-_scsih_send_event_to_turn_on_fault_led(struct MPT2SAS_ADAPTER *ioc, u16 handle)
+_scsih_send_event_to_turn_on_pfa_led(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 {
 	struct fw_event_work *fw_event;
 
 	fw_event = kzalloc(sizeof(struct fw_event_work), GFP_ATOMIC);
 	if (!fw_event)
 		return;
-	fw_event->event = MPT2SAS_TURN_ON_FAULT_LED;
+	fw_event->event = MPT2SAS_TURN_ON_PFA_LED;
 	fw_event->device_handle = handle;
 	fw_event->ioc = ioc;
 	_scsih_fw_event_add(ioc, fw_event);
@@ -4404,7 +4413,7 @@ _scsih_smart_predicted_fault(struct MPT2SAS_ADAPTER *ioc, u16 handle)
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 
 	if (ioc->pdev->subsystem_vendor == PCI_VENDOR_ID_IBM)
-		_scsih_send_event_to_turn_on_fault_led(ioc, handle);
+		_scsih_send_event_to_turn_on_pfa_led(ioc, handle);
 
 	/* insert into event log */
 	sz = offsetof(Mpi2EventNotificationReply_t, EventData) +
@@ -5325,6 +5334,12 @@ _scsih_remove_device(struct MPT2SAS_ADAPTER *ioc,
 {
 	struct MPT2SAS_TARGET *sas_target_priv_data;
 
+	if ((ioc->pdev->subsystem_vendor == PCI_VENDOR_ID_IBM) &&
+		(sas_device->pfa_led_on)) {
+		_scsih_turn_off_pfa_led(ioc, sas_device);
+		sas_device->pfa_led_on = 0;
+	}
+
 	dewtprintk(ioc, printk(MPT2SAS_INFO_FMT "%s: enter: "
 	    "handle(0x%04x), sas_addr(0x%016llx)\n", ioc->name, __func__,
 		sas_device->handle, (unsigned long long)
@@ -7441,8 +7456,8 @@ _firmware_event_work(struct work_struct *work)
 		dewtprintk(ioc, printk(MPT2SAS_INFO_FMT "port enable: complete "
 		    "from worker thread\n", ioc->name));
 		break;
-	case MPT2SAS_TURN_ON_FAULT_LED:
-		_scsih_turn_on_fault_led(ioc, fw_event->device_handle);
+	case MPT2SAS_TURN_ON_PFA_LED:
+		_scsih_turn_on_pfa_led(ioc, fw_event->device_handle);
 		break;
 	case MPI2_EVENT_SAS_TOPOLOGY_CHANGE_LIST:
 		_scsih_sas_topology_change_event(ioc, fw_event);
@@ -8132,6 +8147,7 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct MPT2SAS_ADAPTER *ioc;
 	struct Scsi_Host *shost;
+	int rv;
 
 	shost = scsi_host_alloc(&scsih_driver_template,
 	    sizeof(struct MPT2SAS_ADAPTER));
@@ -8227,6 +8243,7 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (!ioc->firmware_event_thread) {
 		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
+		rv = -ENODEV;
 		goto out_thread_fail;
 	}
 
@@ -8234,6 +8251,7 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if ((mpt2sas_base_attach(ioc))) {
 		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
+		rv = -ENODEV;
 		goto out_attach_fail;
 	}
 
@@ -8251,7 +8269,8 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	} else
 		ioc->hide_drives = 0;
 
-	if ((scsi_add_host(shost, &pdev->dev))) {
+	rv = scsi_add_host(shost, &pdev->dev);
+	if (rv) {
 		printk(MPT2SAS_ERR_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
 		goto out_add_shost_fail;
@@ -8268,7 +8287,7 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
  out_thread_fail:
 	list_del(&ioc->list);
 	scsi_host_put(shost);
-	return -ENODEV;
+	return rv;
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/scsi/mpt2sas/mpt2sas_transport.c b/drivers/scsi/mpt2sas/mpt2sas_transport.c
index 410f4a3e8888..0d1d06488a28 100644
--- a/drivers/scsi/mpt2sas/mpt2sas_transport.c
+++ b/drivers/scsi/mpt2sas/mpt2sas_transport.c
@@ -2,7 +2,7 @@
  * SAS Transport Layer for MPT (Message Passing Technology) based controllers
  *
  * This code is based on drivers/scsi/mpt2sas/mpt2_transport.c
- * Copyright (C) 2007-2013  LSI Corporation
+ * Copyright (C) 2007-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/Kconfig b/drivers/scsi/mpt3sas/Kconfig
index d53e1b02e893..4d235dd741bf 100644
--- a/drivers/scsi/mpt3sas/Kconfig
+++ b/drivers/scsi/mpt3sas/Kconfig
@@ -2,7 +2,7 @@
 # Kernel configuration file for the MPT3SAS
 #
 # This code is based on drivers/scsi/mpt3sas/Kconfig
-# Copyright (C) 2012-2013  LSI Corporation
+# Copyright (C) 2012-2014  LSI Corporation
 #  (mailto:DL-MPTFusionLinux@lsi.com)
 
 # This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2.h b/drivers/scsi/mpt3sas/mpi/mpi2.h
index 20da8f907c00..c34c1157907b 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2000-2013 LSI Corporation.
+ * Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *          Name:  mpi2.h
@@ -8,7 +8,7 @@
  *                 scatter/gather formats.
  * Creation Date:  June 21, 2006
  *
- * mpi2.h Version:  02.00.29
+ * mpi2.h Version:  02.00.31
  *
  * NOTE: Names (typedefs, defines, etc.) beginning with an MPI25 or Mpi25
  *       prefix are for use only on MPI v2.5 products, and must not be used
@@ -86,6 +86,8 @@
  * 11-27-12  02.00.28  Bumped MPI2_HEADER_VERSION_UNIT.
  * 12-20-12  02.00.29  Bumped MPI2_HEADER_VERSION_UNIT.
  *                     Added MPI25_SUP_REPLY_POST_HOST_INDEX_OFFSET.
+ * 04-09-13  02.00.30  Bumped MPI2_HEADER_VERSION_UNIT.
+ * 04-17-13  02.00.31  Bumped MPI2_HEADER_VERSION_UNIT.
  * --------------------------------------------------------------------------
  */
 
@@ -119,7 +121,7 @@
 #define MPI2_VERSION_02_05                  (0x0205)
 
 /*Unit and Dev versioning for this MPI header set */
-#define MPI2_HEADER_VERSION_UNIT            (0x1D)
+#define MPI2_HEADER_VERSION_UNIT            (0x1F)
 #define MPI2_HEADER_VERSION_DEV             (0x00)
 #define MPI2_HEADER_VERSION_UNIT_MASK       (0xFF00)
 #define MPI2_HEADER_VERSION_UNIT_SHIFT      (8)
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2_cnfg.h b/drivers/scsi/mpt3sas/mpi/mpi2_cnfg.h
index 889aa7067899..e261a3153bb3 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2_cnfg.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2_cnfg.h
@@ -1,12 +1,12 @@
 /*
- * Copyright (c) 2000-2013 LSI Corporation.
+ * Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *          Name:  mpi2_cnfg.h
  *         Title:  MPI Configuration messages and pages
  * Creation Date:  November 10, 2006
  *
- *   mpi2_cnfg.h Version:  02.00.24
+ *   mpi2_cnfg.h Version:  02.00.26
  *
  * NOTE: Names (typedefs, defines, etc.) beginning with an MPI25 or Mpi25
  *       prefix are for use only on MPI v2.5 products, and must not be used
@@ -160,6 +160,11 @@
  * 12-20-12  02.00.24  Marked MPI2_SASIOUNIT1_CONTROL_CLEAR_AFFILIATION as
  *                     obsolete for MPI v2.5 and later.
  *                     Added some defines for 12G SAS speeds.
+ * 04-09-13  02.00.25  Added MPI2_IOUNITPAGE1_ATA_SECURITY_FREEZE_LOCK.
+ *                     Fixed MPI2_IOUNITPAGE5_DMA_CAP_MASK_MAX_REQUESTS to
+ *                     match the specification.
+ * 08-19-13  02.00.26  Added reserved words to MPI2_CONFIG_PAGE_IO_UNIT_7 for
+ *			future use.
  * --------------------------------------------------------------------------
  */
 
@@ -792,6 +797,7 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_1 {
 #define MPI2_IOUNITPAGE1_PAGEVERSION                    (0x04)
 
 /*IO Unit Page 1 Flags defines */
+#define MPI2_IOUNITPAGE1_ATA_SECURITY_FREEZE_LOCK       (0x00004000)
 #define MPI25_IOUNITPAGE1_NEW_DEVICE_FAST_PATH_DISABLE  (0x00002000)
 #define MPI25_IOUNITPAGE1_DISABLE_FAST_PATH             (0x00001000)
 #define MPI2_IOUNITPAGE1_ENABLE_HOST_BASED_DISCOVERY    (0x00000800)
@@ -870,7 +876,7 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_5 {
 #define MPI2_IOUNITPAGE5_PAGEVERSION                    (0x00)
 
 /*defines for IO Unit Page 5 DmaEngineCapabilities field */
-#define MPI2_IOUNITPAGE5_DMA_CAP_MASK_MAX_REQUESTS      (0xFF00)
+#define MPI2_IOUNITPAGE5_DMA_CAP_MASK_MAX_REQUESTS      (0xFFFF0000)
 #define MPI2_IOUNITPAGE5_DMA_CAP_SHIFT_MAX_REQUESTS     (16)
 
 #define MPI2_IOUNITPAGE5_DMA_CAP_EEDP                   (0x0008)
@@ -920,11 +926,15 @@ typedef struct _MPI2_CONFIG_PAGE_IO_UNIT_7 {
 	U8
 		BoardTemperatureUnits;                  /*0x16 */
 	U8                      Reserved3;              /*0x17 */
+	U32			Reserved4;		/* 0x18 */
+	U32			Reserved5;		/* 0x1C */
+	U32			Reserved6;		/* 0x20 */
+	U32			Reserved7;		/* 0x24 */
 } MPI2_CONFIG_PAGE_IO_UNIT_7,
 	*PTR_MPI2_CONFIG_PAGE_IO_UNIT_7,
 	Mpi2IOUnitPage7_t, *pMpi2IOUnitPage7_t;
 
-#define MPI2_IOUNITPAGE7_PAGEVERSION                    (0x02)
+#define MPI2_IOUNITPAGE7_PAGEVERSION			(0x04)
 
 /*defines for IO Unit Page 7 CurrentPowerMode and PreviousPowerMode fields */
 #define MPI25_IOUNITPAGE7_PM_INIT_MASK              (0xC0)
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2_init.h b/drivers/scsi/mpt3sas/mpi/mpi2_init.h
index f7928bf66478..068c98efd742 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2_init.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2_init.h
@@ -1,12 +1,12 @@
 /*
- * Copyright (c) 2000-2013 LSI Corporation.
+ * Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *          Name:  mpi2_init.h
  *         Title:  MPI SCSI initiator mode messages and structures
  * Creation Date:  June 23, 2006
  *
- * mpi2_init.h Version:  02.00.14
+ * mpi2_init.h Version:  02.00.15
  *
  * NOTE: Names (typedefs, defines, etc.) beginning with an MPI25 or Mpi25
  *       prefix are for use only on MPI v2.5 products, and must not be used
@@ -44,6 +44,8 @@
  *                     Priority to match SAM-4.
  *                     Added EEDPErrorOffset to MPI2_SCSI_IO_REPLY.
  * 07-10-12  02.00.14  Added MPI2_SCSIIO_CONTROL_SHIFT_DATADIRECTION.
+ * 04-09-13  02.00.15  Added SCSIStatusQualifier field to MPI2_SCSI_IO_REPLY,
+ *                     replacing the Reserved4 field.
  * --------------------------------------------------------------------------
  */
 
@@ -347,7 +349,7 @@ typedef struct _MPI2_SCSI_IO_REPLY {
 	U32 SenseCount;		/*0x18 */
 	U32 ResponseInfo;	/*0x1C */
 	U16 TaskTag;		/*0x20 */
-	U16 Reserved4;		/*0x22 */
+	U16 SCSIStatusQualifier; /* 0x22 */
 	U32 BidirectionalTransferCount;	/*0x24 */
 	U32 EEDPErrorOffset;	/*0x28 *//*MPI 2.5 only; Reserved in MPI 2.0*/
 	U32 Reserved6;		/*0x2C */
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2_ioc.h b/drivers/scsi/mpt3sas/mpi/mpi2_ioc.h
index e2bb82143720..490830957806 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2_ioc.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2_ioc.h
@@ -1,12 +1,12 @@
 /*
- * Copyright (c) 2000-2013 LSI Corporation.
+ * Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *          Name:  mpi2_ioc.h
  *         Title:  MPI IOC, Port, Event, FW Download, and FW Upload messages
  * Creation Date:  October 11, 2006
  *
- * mpi2_ioc.h Version:  02.00.22
+ * mpi2_ioc.h Version:  02.00.23
  *
  * NOTE: Names (typedefs, defines, etc.) beginning with an MPI25 or Mpi25
  *       prefix are for use only on MPI v2.5 products, and must not be used
@@ -127,6 +127,11 @@
  * 07-26-12  02.00.22  Added MPI2_IOCFACTS_EXCEPT_PARTIAL_MEMORY_FAILURE.
  *                     Added ElapsedSeconds field to
  *                     MPI2_EVENT_DATA_IR_OPERATION_STATUS.
+ * 08-19-13  02.00.23  For IOCInit, added MPI2_IOCINIT_MSGFLAG_RDPQ_ARRAY_MODE
+ *			and MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY.
+ *			Added MPI2_IOCFACTS_CAPABILITY_RDPQ_ARRAY_CAPABLE.
+ *			Added MPI2_FW_DOWNLOAD_ITYPE_PUBLIC_KEY.
+ *			Added Encrypted Hash Extended Image.
  * --------------------------------------------------------------------------
  */
 
@@ -182,6 +187,10 @@ typedef struct _MPI2_IOC_INIT_REQUEST {
 #define MPI2_WHOINIT_HOST_DRIVER                (0x04)
 #define MPI2_WHOINIT_MANUFACTURER               (0x05)
 
+/* MsgFlags */
+#define MPI2_IOCINIT_MSGFLAG_RDPQ_ARRAY_MODE    (0x01)
+
+
 /*MsgVersion */
 #define MPI2_IOCINIT_MSGVERSION_MAJOR_MASK      (0xFF00)
 #define MPI2_IOCINIT_MSGVERSION_MAJOR_SHIFT     (8)
@@ -194,9 +203,19 @@ typedef struct _MPI2_IOC_INIT_REQUEST {
 #define MPI2_IOCINIT_HDRVERSION_DEV_MASK        (0x00FF)
 #define MPI2_IOCINIT_HDRVERSION_DEV_SHIFT       (0)
 
-/*minimum depth for the Reply Descriptor Post Queue */
+/*minimum depth for a Reply Descriptor Post Queue */
 #define MPI2_RDPQ_DEPTH_MIN                     (16)
 
+/* Reply Descriptor Post Queue Array Entry */
+typedef struct _MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY {
+	U64                 RDPQBaseAddress;                    /* 0x00 */
+	U32                 Reserved1;                          /* 0x08 */
+	U32                 Reserved2;                          /* 0x0C */
+} MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY,
+*PTR_MPI2_IOC_INIT_RDPQ_ARRAY_ENTRY,
+Mpi2IOCInitRDPQArrayEntry, *pMpi2IOCInitRDPQArrayEntry;
+
+
 /*IOCInit Reply message */
 typedef struct _MPI2_IOC_INIT_REPLY {
 	U8 WhoInit;		/*0x00 */
@@ -306,6 +325,7 @@ typedef struct _MPI2_IOC_FACTS_REPLY {
 /*ProductID field uses MPI2_FW_HEADER_PID_ */
 
 /*IOCCapabilities */
+#define MPI2_IOCFACTS_CAPABILITY_RDPQ_ARRAY_CAPABLE     (0x00040000)
 #define MPI25_IOCFACTS_CAPABILITY_FAST_PATH_CAPABLE     (0x00020000)
 #define MPI2_IOCFACTS_CAPABILITY_HOST_BASED_DISCOVERY   (0x00010000)
 #define MPI2_IOCFACTS_CAPABILITY_MSI_X_INDEX            (0x00008000)
@@ -1140,6 +1160,7 @@ typedef struct _MPI2_FW_DOWNLOAD_REQUEST {
 #define MPI2_FW_DOWNLOAD_ITYPE_MEGARAID             (0x09)
 #define MPI2_FW_DOWNLOAD_ITYPE_COMPLETE             (0x0A)
 #define MPI2_FW_DOWNLOAD_ITYPE_COMMON_BOOT_BLOCK    (0x0B)
+#define MPI2_FW_DOWNLOAD_ITYPE_PUBLIC_KEY           (0x0C)
 #define MPI2_FW_DOWNLOAD_ITYPE_MIN_PRODUCT_SPECIFIC (0xF0)
 
 /*MPI v2.0 FWDownload TransactionContext Element */
@@ -1404,6 +1425,7 @@ typedef struct _MPI2_EXT_IMAGE_HEADER {
 #define MPI2_EXT_IMAGE_TYPE_FLASH_LAYOUT            (0x06)
 #define MPI2_EXT_IMAGE_TYPE_SUPPORTED_DEVICES       (0x07)
 #define MPI2_EXT_IMAGE_TYPE_MEGARAID                (0x08)
+#define MPI2_EXT_IMAGE_TYPE_ENCRYPTED_HASH          (0x09)
 #define MPI2_EXT_IMAGE_TYPE_MIN_PRODUCT_SPECIFIC    (0x80)
 #define MPI2_EXT_IMAGE_TYPE_MAX_PRODUCT_SPECIFIC    (0xFF)
 
@@ -1560,6 +1582,42 @@ typedef struct _MPI2_INIT_IMAGE_FOOTER {
 /*defines for the ResetVector field */
 #define MPI2_INIT_IMAGE_RESETVECTOR_OFFSET      (0x14)
 
+
+/* Encrypted Hash Extended Image Data */
+
+typedef struct _MPI25_ENCRYPTED_HASH_ENTRY {
+	U8		HashImageType;		/* 0x00 */
+	U8		HashAlgorithm;		/* 0x01 */
+	U8		EncryptionAlgorithm;	/* 0x02 */
+	U8		Reserved1;		/* 0x03 */
+	U32		Reserved2;		/* 0x04 */
+	U32		EncryptedHash[1];	/* 0x08 */ /* variable length */
+} MPI25_ENCRYPTED_HASH_ENTRY, *PTR_MPI25_ENCRYPTED_HASH_ENTRY,
+Mpi25EncryptedHashEntry_t, *pMpi25EncryptedHashEntry_t;
+
+/* values for HashImageType */
+#define MPI25_HASH_IMAGE_TYPE_UNUSED		(0x00)
+#define MPI25_HASH_IMAGE_TYPE_FIRMWARE		(0x01)
+
+/* values for HashAlgorithm */
+#define MPI25_HASH_ALGORITHM_UNUSED		(0x00)
+#define MPI25_HASH_ALGORITHM_SHA256		(0x01)
+
+/* values for EncryptionAlgorithm */
+#define MPI25_ENCRYPTION_ALG_UNUSED		(0x00)
+#define MPI25_ENCRYPTION_ALG_RSA256		(0x01)
+
+typedef struct _MPI25_ENCRYPTED_HASH_DATA {
+	U8				ImageVersion;		/* 0x00 */
+	U8				NumHash;		/* 0x01 */
+	U16				Reserved1;		/* 0x02 */
+	U32				Reserved2;		/* 0x04 */
+	MPI25_ENCRYPTED_HASH_ENTRY	EncryptedHashEntry[1];  /* 0x08 */
+} MPI25_ENCRYPTED_HASH_DATA, *PTR_MPI25_ENCRYPTED_HASH_DATA,
+Mpi25EncryptedHashData_t, *pMpi25EncryptedHashData_t;
+
+
+
 /****************************************************************************
 * PowerManagementControl message
 ****************************************************************************/
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2_raid.h b/drivers/scsi/mpt3sas/mpi/mpi2_raid.h
index 71765236afef..13d93ca029d5 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2_raid.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2_raid.h
@@ -1,12 +1,12 @@
 /*
- * Copyright (c) 2000-2013 LSI Corporation.
+ * Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *          Name:  mpi2_raid.h
  *         Title:  MPI Integrated RAID messages and structures
  * Creation Date:  April 26, 2007
  *
- *   mpi2_raid.h Version:  02.00.09
+ *   mpi2_raid.h Version:  02.00.10
  *
  * Version History
  * ---------------
@@ -30,6 +30,7 @@
  * 02-06-12  02.00.08  Added MPI2_RAID_ACTION_PHYSDISK_HIDDEN.
  * 07-26-12  02.00.09  Added ElapsedSeconds field to MPI2_RAID_VOL_INDICATOR.
  *                     Added MPI2_RAID_VOL_FLAGS_ELAPSED_SECONDS_VALID define.
+ * 04-17-13  02.00.10  Added MPI25_RAID_ACTION_ADATA_ALLOW_PI.
  * --------------------------------------------------------------------------
  */
 
@@ -46,6 +47,9 @@
 * RAID Action messages
 ****************************************************************************/
 
+/* ActionDataWord defines for use with MPI2_RAID_ACTION_CREATE_VOLUME action */
+#define MPI25_RAID_ACTION_ADATA_ALLOW_PI            (0x80000000)
+
 /*ActionDataWord defines for use with MPI2_RAID_ACTION_DELETE_VOLUME action */
 #define MPI2_RAID_ACTION_ADATA_KEEP_LBA0            (0x00000000)
 #define MPI2_RAID_ACTION_ADATA_ZERO_LBA0            (0x00000001)
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2_sas.h b/drivers/scsi/mpt3sas/mpi/mpi2_sas.h
index cba046f6a4b4..156e30543a2f 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2_sas.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2_sas.h
@@ -1,12 +1,12 @@
 /*
- * Copyright (c) 2000-2013 LSI Corporation.
+ * Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *          Name:  mpi2_sas.h
  *         Title:  MPI Serial Attached SCSI structures and definitions
  * Creation Date:  February 9, 2007
  *
- * mpi2_sas.h Version:  02.00.07
+ * mpi2_sas.h Version:  02.00.08
  *
  * NOTE: Names (typedefs, defines, etc.) beginning with an MPI25 or Mpi25
  *       prefix are for use only on MPI v2.5 products, and must not be used
@@ -30,6 +30,8 @@
  * 11-18-11  02.00.06  Incorporating additions for MPI v2.5.
  * 07-10-12  02.00.07  Added MPI2_SATA_PT_SGE_UNION for use in the SATA
  *                     Passthrough Request message.
+ * 08-19-13  02.00.08  Made MPI2_SAS_OP_TRANSMIT_PORT_SELECT_SIGNAL obsolete
+ *			for anything newer than MPI v2.0.
  * --------------------------------------------------------------------------
  */
 
@@ -251,7 +253,7 @@ typedef struct _MPI2_SAS_IOUNIT_CONTROL_REQUEST {
 #define MPI2_SAS_OP_PHY_CLEAR_ERROR_LOG         (0x08)
 #define MPI2_SAS_OP_SEND_PRIMITIVE              (0x0A)
 #define MPI2_SAS_OP_FORCE_FULL_DISCOVERY        (0x0B)
-#define MPI2_SAS_OP_TRANSMIT_PORT_SELECT_SIGNAL (0x0C)
+#define MPI2_SAS_OP_TRANSMIT_PORT_SELECT_SIGNAL (0x0C) /* MPI v2.0 only */
 #define MPI2_SAS_OP_REMOVE_DEVICE               (0x0D)
 #define MPI2_SAS_OP_LOOKUP_MAPPING              (0x0E)
 #define MPI2_SAS_OP_SET_IOC_PARAMETER           (0x0F)
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2_tool.h b/drivers/scsi/mpt3sas/mpi/mpi2_tool.h
index 34e9a7ba76b0..904910d8a737 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2_tool.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2_tool.h
@@ -1,12 +1,12 @@
 /*
- * Copyright (c) 2000-2013 LSI Corporation.
+ * Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *          Name:  mpi2_tool.h
  *         Title:  MPI diagnostic tool structures and definitions
  * Creation Date:  March 26, 2007
  *
- *   mpi2_tool.h Version:  02.00.10
+ *   mpi2_tool.h Version:  02.00.11
  *
  * Version History
  * ---------------
@@ -32,6 +32,7 @@
  *                     message.
  * 07-26-12  02.00.10  Modified MPI2_TOOLBOX_DIAGNOSTIC_CLI_REQUEST so that
  *                     it uses MPI Chain SGE as well as MPI Simple SGE.
+ * 08-19-13  02.00.11  Added MPI2_TOOLBOX_TEXT_DISPLAY_TOOL and related info.
  * --------------------------------------------------------------------------
  */
 
@@ -51,6 +52,7 @@
 #define MPI2_TOOLBOX_ISTWI_READ_WRITE_TOOL          (0x03)
 #define MPI2_TOOLBOX_BEACON_TOOL                    (0x05)
 #define MPI2_TOOLBOX_DIAGNOSTIC_CLI_TOOL            (0x06)
+#define MPI2_TOOLBOX_TEXT_DISPLAY_TOOL              (0x07)
 
 /****************************************************************************
 * Toolbox reply
@@ -331,6 +333,45 @@ typedef struct _MPI2_TOOLBOX_DIAGNOSTIC_CLI_REPLY {
 	Mpi2ToolboxDiagnosticCliReply_t,
 	*pMpi2ToolboxDiagnosticCliReply_t;
 
+
+/****************************************************************************
+*  Toolbox Console Text Display Tool
+****************************************************************************/
+
+/* Toolbox Console Text Display Tool request message */
+typedef struct _MPI2_TOOLBOX_TEXT_DISPLAY_REQUEST {
+	U8			Tool;			/* 0x00 */
+	U8			Reserved1;		/* 0x01 */
+	U8			ChainOffset;		/* 0x02 */
+	U8			Function;		/* 0x03 */
+	U16			Reserved2;		/* 0x04 */
+	U8			Reserved3;		/* 0x06 */
+	U8			MsgFlags;		/* 0x07 */
+	U8			VP_ID;			/* 0x08 */
+	U8			VF_ID;			/* 0x09 */
+	U16			Reserved4;		/* 0x0A */
+	U8			Console;		/* 0x0C */
+	U8			Flags;			/* 0x0D */
+	U16			Reserved6;		/* 0x0E */
+	U8			TextToDisplay[4];	/* 0x10 */
+} MPI2_TOOLBOX_TEXT_DISPLAY_REQUEST,
+*PTR_MPI2_TOOLBOX_TEXT_DISPLAY_REQUEST,
+Mpi2ToolboxTextDisplayRequest_t,
+*pMpi2ToolboxTextDisplayRequest_t;
+
+/* defines for the Console field */
+#define MPI2_TOOLBOX_CONSOLE_TYPE_MASK          (0xF0)
+#define MPI2_TOOLBOX_CONSOLE_TYPE_DEFAULT       (0x00)
+#define MPI2_TOOLBOX_CONSOLE_TYPE_UART          (0x10)
+#define MPI2_TOOLBOX_CONSOLE_TYPE_ETHERNET      (0x20)
+
+#define MPI2_TOOLBOX_CONSOLE_NUMBER_MASK        (0x0F)
+
+/* defines for the Flags field */
+#define MPI2_TOOLBOX_CONSOLE_FLAG_TIMESTAMP     (0x01)
+
+
+
 /*****************************************************************************
 *
 *      Diagnostic Buffer Messages
diff --git a/drivers/scsi/mpt3sas/mpi/mpi2_type.h b/drivers/scsi/mpt3sas/mpi/mpi2_type.h
index ba1fed50966e..99ab093602e8 100644
--- a/drivers/scsi/mpt3sas/mpi/mpi2_type.h
+++ b/drivers/scsi/mpt3sas/mpi/mpi2_type.h
@@ -1,5 +1,5 @@
 /*
- *  Copyright (c) 2000-2013 LSI Corporation.
+ *  Copyright (c) 2000-2014 LSI Corporation.
  *
  *
  *           Name:  mpi2_type.h
diff --git a/drivers/scsi/mpt3sas/mpt3sas_base.c b/drivers/scsi/mpt3sas/mpt3sas_base.c
index 93ce2b2baa41..1560115079c7 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_base.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_base.c
@@ -3,7 +3,7 @@
  * for access to MPT (Message Passing Technology) firmware.
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_base.c
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
@@ -91,6 +91,8 @@ static int mpt3sas_fwfault_debug;
 MODULE_PARM_DESC(mpt3sas_fwfault_debug,
 	" enable detection of firmware fault and halt firmware - (default=0)");
 
+static int
+_base_get_ioc_facts(struct MPT3SAS_ADAPTER *ioc, int sleep_flag);
 
 /**
  * _scsih_set_fwfault_debug - global setting of ioc->fwfault_debug.
@@ -1482,17 +1484,22 @@ static int
 _base_config_dma_addressing(struct MPT3SAS_ADAPTER *ioc, struct pci_dev *pdev)
 {
 	struct sysinfo s;
-	char *desc = NULL;
+	u64 consistent_dma_mask;
+
+	if (ioc->dma_mask)
+		consistent_dma_mask = DMA_BIT_MASK(64);
+	else
+		consistent_dma_mask = DMA_BIT_MASK(32);
 
 	if (sizeof(dma_addr_t) > 4) {
 		const uint64_t required_mask =
 		    dma_get_required_mask(&pdev->dev);
 		if ((required_mask > DMA_BIT_MASK(32)) &&
 		    !pci_set_dma_mask(pdev, DMA_BIT_MASK(64)) &&
-		    !pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64))) {
+		    !pci_set_consistent_dma_mask(pdev, consistent_dma_mask)) {
 			ioc->base_add_sg_single = &_base_add_sg_single_64;
 			ioc->sge_size = sizeof(Mpi2SGESimple64_t);
-			desc = "64";
+			ioc->dma_mask = 64;
 			goto out;
 		}
 	}
@@ -1501,19 +1508,30 @@ _base_config_dma_addressing(struct MPT3SAS_ADAPTER *ioc, struct pci_dev *pdev)
 	    && !pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
 		ioc->base_add_sg_single = &_base_add_sg_single_32;
 		ioc->sge_size = sizeof(Mpi2SGESimple32_t);
-		desc = "32";
+		ioc->dma_mask = 32;
 	} else
 		return -ENODEV;
 
  out:
 	si_meminfo(&s);
 	pr_info(MPT3SAS_FMT
-		"%s BIT PCI BUS DMA ADDRESSING SUPPORTED, total mem (%ld kB)\n",
-		ioc->name, desc, convert_to_kb(s.totalram));
+		"%d BIT PCI BUS DMA ADDRESSING SUPPORTED, total mem (%ld kB)\n",
+		ioc->name, ioc->dma_mask, convert_to_kb(s.totalram));
 
 	return 0;
 }
 
+static int
+_base_change_consistent_dma_mask(struct MPT3SAS_ADAPTER *ioc,
+				      struct pci_dev *pdev)
+{
+	if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(64))) {
+		if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32)))
+			return -ENODEV;
+	}
+	return 0;
+}
+
 /**
  * _base_check_enable_msix - checks MSIX capabable.
  * @ioc: per adapter object
@@ -1698,11 +1716,15 @@ _base_enable_msix(struct MPT3SAS_ADAPTER *ioc)
 	  ": %d, max_msix_vectors: %d\n", ioc->name, ioc->msix_vector_count,
 	  ioc->cpu_count, max_msix_vectors);
 
+	if (!ioc->rdpq_array_enable && max_msix_vectors == -1)
+		max_msix_vectors = 8;
+
 	if (max_msix_vectors > 0) {
 		ioc->reply_queue_count = min_t(int, max_msix_vectors,
 			ioc->reply_queue_count);
 		ioc->msix_vector_count = ioc->reply_queue_count;
-	}
+	} else if (max_msix_vectors == 0)
+		goto try_ioapic;
 
 	entries = kcalloc(ioc->reply_queue_count, sizeof(struct msix_entry),
 	    GFP_KERNEL);
@@ -1716,10 +1738,10 @@ _base_enable_msix(struct MPT3SAS_ADAPTER *ioc)
 	for (i = 0, a = entries; i < ioc->reply_queue_count; i++, a++)
 		a->entry = i;
 
-	r = pci_enable_msix(ioc->pdev, entries, ioc->reply_queue_count);
+	r = pci_enable_msix_exact(ioc->pdev, entries, ioc->reply_queue_count);
 	if (r) {
 		dfailprintk(ioc, pr_info(MPT3SAS_FMT
-			"pci_enable_msix failed (r=%d) !!!\n",
+			"pci_enable_msix_exact failed (r=%d) !!!\n",
 			ioc->name, r));
 		kfree(entries);
 		goto try_ioapic;
@@ -1742,6 +1764,7 @@ _base_enable_msix(struct MPT3SAS_ADAPTER *ioc)
 /* failback to io_apic interrupt routing */
  try_ioapic:
 
+	ioc->reply_queue_count = 1;
 	r = _base_request_irq(ioc, 0, ioc->pdev->irq);
 
 	return r;
@@ -1821,6 +1844,16 @@ mpt3sas_base_map_resources(struct MPT3SAS_ADAPTER *ioc)
 	}
 
 	_base_mask_interrupts(ioc);
+
+	r = _base_get_ioc_facts(ioc, CAN_SLEEP);
+	if (r)
+		goto out_fail;
+
+	if (!ioc->rdpq_array_enable_assigned) {
+		ioc->rdpq_array_enable = ioc->rdpq_array_capable;
+		ioc->rdpq_array_enable_assigned = 1;
+	}
+
 	r = _base_enable_msix(ioc);
 	if (r)
 		goto out_fail;
@@ -2185,6 +2218,53 @@ mpt3sas_base_put_smid_default(struct MPT3SAS_ADAPTER *ioc, u16 smid)
 	    &ioc->scsi_lookup_lock);
 }
 
+/**
+ * _base_display_intel_branding - Display branding string
+ * @ioc: per adapter object
+ *
+ * Return nothing.
+ */
+static void
+_base_display_intel_branding(struct MPT3SAS_ADAPTER *ioc)
+{
+	if (ioc->pdev->subsystem_vendor != PCI_VENDOR_ID_INTEL)
+		return;
+
+	switch (ioc->pdev->device) {
+	case MPI25_MFGPAGE_DEVID_SAS3008:
+		switch (ioc->pdev->subsystem_device) {
+		case MPT3SAS_INTEL_RMS3JC080_SSDID:
+			pr_info(MPT3SAS_FMT "%s\n", ioc->name,
+				MPT3SAS_INTEL_RMS3JC080_BRANDING);
+			break;
+
+		case MPT3SAS_INTEL_RS3GC008_SSDID:
+			pr_info(MPT3SAS_FMT "%s\n", ioc->name,
+				MPT3SAS_INTEL_RS3GC008_BRANDING);
+			break;
+		case MPT3SAS_INTEL_RS3FC044_SSDID:
+			pr_info(MPT3SAS_FMT "%s\n", ioc->name,
+				MPT3SAS_INTEL_RS3FC044_BRANDING);
+			break;
+		case MPT3SAS_INTEL_RS3UC080_SSDID:
+			pr_info(MPT3SAS_FMT "%s\n", ioc->name,
+				MPT3SAS_INTEL_RS3UC080_BRANDING);
+			break;
+		default:
+			pr_info(MPT3SAS_FMT
+				"Intel(R) Controller: Subsystem ID: 0x%X\n",
+				ioc->name, ioc->pdev->subsystem_device);
+			break;
+		}
+		break;
+	default:
+		pr_info(MPT3SAS_FMT
+			"Intel(R) Controller: Subsystem ID: 0x%X\n",
+			ioc->name, ioc->pdev->subsystem_device);
+		break;
+	}
+}
+
 
 
 /**
@@ -2216,6 +2296,8 @@ _base_display_ioc_capabilities(struct MPT3SAS_ADAPTER *ioc)
 	   (bios_version & 0x0000FF00) >> 8,
 	    bios_version & 0x000000FF);
 
+	_base_display_intel_branding(ioc);
+
 	pr_info(MPT3SAS_FMT "Protocol=(", ioc->name);
 
 	if (ioc->facts.ProtocolFlags & MPI2_IOCFACTS_PROTOCOL_SCSI_INITIATOR) {
@@ -2447,7 +2529,8 @@ _base_static_config_pages(struct MPT3SAS_ADAPTER *ioc)
 static void
 _base_release_memory_pools(struct MPT3SAS_ADAPTER *ioc)
 {
-	int i;
+	int i = 0;
+	struct reply_post_struct *rps;
 
 	dexitprintk(ioc, pr_info(MPT3SAS_FMT "%s\n", ioc->name,
 	    __func__));
@@ -2492,15 +2575,25 @@ _base_release_memory_pools(struct MPT3SAS_ADAPTER *ioc)
 		ioc->reply_free = NULL;
 	}
 
-	if (ioc->reply_post_free) {
-		pci_pool_free(ioc->reply_post_free_dma_pool,
-		    ioc->reply_post_free, ioc->reply_post_free_dma);
+	if (ioc->reply_post) {
+		do {
+			rps = &ioc->reply_post[i];
+			if (rps->reply_post_free) {
+				pci_pool_free(
+				    ioc->reply_post_free_dma_pool,
+				    rps->reply_post_free,
+				    rps->reply_post_free_dma);
+				dexitprintk(ioc, pr_info(MPT3SAS_FMT
+				    "reply_post_free_pool(0x%p): free\n",
+				    ioc->name, rps->reply_post_free));
+				rps->reply_post_free = NULL;
+			}
+		} while (ioc->rdpq_array_enable &&
+			   (++i < ioc->reply_queue_count));
+
 		if (ioc->reply_post_free_dma_pool)
 			pci_pool_destroy(ioc->reply_post_free_dma_pool);
-		dexitprintk(ioc, pr_info(MPT3SAS_FMT
-		    "reply_post_free_pool(0x%p): free\n", ioc->name,
-		    ioc->reply_post_free));
-		ioc->reply_post_free = NULL;
+		kfree(ioc->reply_post);
 	}
 
 	if (ioc->config_page) {
@@ -2647,6 +2740,65 @@ _base_allocate_memory_pools(struct MPT3SAS_ADAPTER *ioc,  int sleep_flag)
 	    ioc->max_sges_in_chain_message, ioc->shost->sg_tablesize,
 	    ioc->chains_needed_per_io));
 
+	/* reply post queue, 16 byte align */
+	reply_post_free_sz = ioc->reply_post_queue_depth *
+	    sizeof(Mpi2DefaultReplyDescriptor_t);
+
+	sz = reply_post_free_sz;
+	if (_base_is_controller_msix_enabled(ioc) && !ioc->rdpq_array_enable)
+		sz *= ioc->reply_queue_count;
+
+	ioc->reply_post = kcalloc((ioc->rdpq_array_enable) ?
+	    (ioc->reply_queue_count):1,
+	    sizeof(struct reply_post_struct), GFP_KERNEL);
+
+	if (!ioc->reply_post) {
+		pr_err(MPT3SAS_FMT "reply_post_free pool: kcalloc failed\n",
+			ioc->name);
+		goto out;
+	}
+	ioc->reply_post_free_dma_pool = pci_pool_create("reply_post_free pool",
+	    ioc->pdev, sz, 16, 0);
+	if (!ioc->reply_post_free_dma_pool) {
+		pr_err(MPT3SAS_FMT
+		 "reply_post_free pool: pci_pool_create failed\n",
+		 ioc->name);
+		goto out;
+	}
+	i = 0;
+	do {
+		ioc->reply_post[i].reply_post_free =
+		    pci_pool_alloc(ioc->reply_post_free_dma_pool,
+		    GFP_KERNEL,
+		    &ioc->reply_post[i].reply_post_free_dma);
+		if (!ioc->reply_post[i].reply_post_free) {
+			pr_err(MPT3SAS_FMT
+			"reply_post_free pool: pci_pool_alloc failed\n",
+			ioc->name);
+			goto out;
+		}
+		memset(ioc->reply_post[i].reply_post_free, 0, sz);
+		dinitprintk(ioc, pr_info(MPT3SAS_FMT
+		    "reply post free pool (0x%p): depth(%d),"
+		    "element_size(%d), pool_size(%d kB)\n", ioc->name,
+		    ioc->reply_post[i].reply_post_free,
+		    ioc->reply_post_queue_depth, 8, sz/1024));
+		dinitprintk(ioc, pr_info(MPT3SAS_FMT
+		    "reply_post_free_dma = (0x%llx)\n", ioc->name,
+		    (unsigned long long)
+		    ioc->reply_post[i].reply_post_free_dma));
+		total_sz += sz;
+	} while (ioc->rdpq_array_enable && (++i < ioc->reply_queue_count));
+
+	if (ioc->dma_mask == 64) {
+		if (_base_change_consistent_dma_mask(ioc, ioc->pdev) != 0) {
+			pr_warn(MPT3SAS_FMT
+			    "no suitable consistent DMA mask for %s\n",
+			    ioc->name, pci_name(ioc->pdev));
+			goto out;
+		}
+	}
+
 	ioc->scsiio_depth = ioc->hba_queue_depth -
 	    ioc->hi_priority_depth - ioc->internal_depth;
 
@@ -2861,40 +3013,6 @@ _base_allocate_memory_pools(struct MPT3SAS_ADAPTER *ioc,  int sleep_flag)
 		ioc->name, (unsigned long long)ioc->reply_free_dma));
 	total_sz += sz;
 
-	/* reply post queue, 16 byte align */
-	reply_post_free_sz = ioc->reply_post_queue_depth *
-	    sizeof(Mpi2DefaultReplyDescriptor_t);
-	if (_base_is_controller_msix_enabled(ioc))
-		sz = reply_post_free_sz * ioc->reply_queue_count;
-	else
-		sz = reply_post_free_sz;
-	ioc->reply_post_free_dma_pool = pci_pool_create("reply_post_free pool",
-	    ioc->pdev, sz, 16, 0);
-	if (!ioc->reply_post_free_dma_pool) {
-		pr_err(MPT3SAS_FMT
-			"reply_post_free pool: pci_pool_create failed\n",
-			ioc->name);
-		goto out;
-	}
-	ioc->reply_post_free = pci_pool_alloc(ioc->reply_post_free_dma_pool ,
-	    GFP_KERNEL, &ioc->reply_post_free_dma);
-	if (!ioc->reply_post_free) {
-		pr_err(MPT3SAS_FMT
-			"reply_post_free pool: pci_pool_alloc failed\n",
-			ioc->name);
-		goto out;
-	}
-	memset(ioc->reply_post_free, 0, sz);
-	dinitprintk(ioc, pr_info(MPT3SAS_FMT "reply post free pool" \
-	    "(0x%p): depth(%d), element_size(%d), pool_size(%d kB)\n",
-	    ioc->name, ioc->reply_post_free, ioc->reply_post_queue_depth, 8,
-	    sz/1024));
-	dinitprintk(ioc, pr_info(MPT3SAS_FMT
-		"reply_post_free_dma = (0x%llx)\n",
-		ioc->name, (unsigned long long)
-	    ioc->reply_post_free_dma));
-	total_sz += sz;
-
 	ioc->config_page_sz = 512;
 	ioc->config_page = pci_alloc_consistent(ioc->pdev,
 	    ioc->config_page_sz, &ioc->config_page_dma);
@@ -3577,6 +3695,9 @@ _base_get_ioc_facts(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 	facts->IOCCapabilities = le32_to_cpu(mpi_reply.IOCCapabilities);
 	if ((facts->IOCCapabilities & MPI2_IOCFACTS_CAPABILITY_INTEGRATED_RAID))
 		ioc->ir_firmware = 1;
+	if ((facts->IOCCapabilities &
+	      MPI2_IOCFACTS_CAPABILITY_RDPQ_ARRAY_CAPABLE))
+		ioc->rdpq_array_capable = 1;
 	facts->FWVersion.Word = le32_to_cpu(mpi_reply.FWVersion.Word);
 	facts->IOCRequestFrameSize =
 	    le16_to_cpu(mpi_reply.IOCRequestFrameSize);
@@ -3613,9 +3734,12 @@ _base_send_ioc_init(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 {
 	Mpi2IOCInitRequest_t mpi_request;
 	Mpi2IOCInitReply_t mpi_reply;
-	int r;
+	int i, r = 0;
 	struct timeval current_time;
 	u16 ioc_status;
+	u32 reply_post_free_array_sz = 0;
+	Mpi2IOCInitRDPQArrayEntry *reply_post_free_array = NULL;
+	dma_addr_t reply_post_free_array_dma;
 
 	dinitprintk(ioc, pr_info(MPT3SAS_FMT "%s\n", ioc->name,
 	    __func__));
@@ -3644,9 +3768,31 @@ _base_send_ioc_init(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 	    cpu_to_le64((u64)ioc->request_dma);
 	mpi_request.ReplyFreeQueueAddress =
 	    cpu_to_le64((u64)ioc->reply_free_dma);
-	mpi_request.ReplyDescriptorPostQueueAddress =
-	    cpu_to_le64((u64)ioc->reply_post_free_dma);
 
+	if (ioc->rdpq_array_enable) {
+		reply_post_free_array_sz = ioc->reply_queue_count *
+		    sizeof(Mpi2IOCInitRDPQArrayEntry);
+		reply_post_free_array = pci_alloc_consistent(ioc->pdev,
+			reply_post_free_array_sz, &reply_post_free_array_dma);
+		if (!reply_post_free_array) {
+			pr_err(MPT3SAS_FMT
+			"reply_post_free_array: pci_alloc_consistent failed\n",
+			ioc->name);
+			r = -ENOMEM;
+			goto out;
+		}
+		memset(reply_post_free_array, 0, reply_post_free_array_sz);
+		for (i = 0; i < ioc->reply_queue_count; i++)
+			reply_post_free_array[i].RDPQBaseAddress =
+			    cpu_to_le64(
+				(u64)ioc->reply_post[i].reply_post_free_dma);
+		mpi_request.MsgFlags = MPI2_IOCINIT_MSGFLAG_RDPQ_ARRAY_MODE;
+		mpi_request.ReplyDescriptorPostQueueAddress =
+		    cpu_to_le64((u64)reply_post_free_array_dma);
+	} else {
+		mpi_request.ReplyDescriptorPostQueueAddress =
+		    cpu_to_le64((u64)ioc->reply_post[0].reply_post_free_dma);
+	}
 
 	/* This time stamp specifies number of milliseconds
 	 * since epoch ~ midnight January 1, 1970.
@@ -3674,7 +3820,7 @@ _base_send_ioc_init(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 	if (r != 0) {
 		pr_err(MPT3SAS_FMT "%s: handshake failed (r=%d)\n",
 		    ioc->name, __func__, r);
-		return r;
+		goto out;
 	}
 
 	ioc_status = le16_to_cpu(mpi_reply.IOCStatus) & MPI2_IOCSTATUS_MASK;
@@ -3684,7 +3830,12 @@ _base_send_ioc_init(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 		r = -EIO;
 	}
 
-	return 0;
+out:
+	if (reply_post_free_array)
+		pci_free_consistent(ioc->pdev, reply_post_free_array_sz,
+				    reply_post_free_array,
+				    reply_post_free_array_dma);
+	return r;
 }
 
 /**
@@ -4234,7 +4385,7 @@ _base_make_ioc_operational(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 	struct _tr_list *delayed_tr, *delayed_tr_next;
 	struct adapter_reply_queue *reply_q;
 	long reply_post_free;
-	u32 reply_post_free_sz;
+	u32 reply_post_free_sz, index = 0;
 
 	dinitprintk(ioc, pr_info(MPT3SAS_FMT "%s\n", ioc->name,
 	    __func__));
@@ -4305,9 +4456,9 @@ _base_make_ioc_operational(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 		_base_assign_reply_queues(ioc);
 
 	/* initialize Reply Post Free Queue */
-	reply_post_free = (long)ioc->reply_post_free;
 	reply_post_free_sz = ioc->reply_post_queue_depth *
 	    sizeof(Mpi2DefaultReplyDescriptor_t);
+	reply_post_free = (long)ioc->reply_post[index].reply_post_free;
 	list_for_each_entry(reply_q, &ioc->reply_queue_list, list) {
 		reply_q->reply_post_host_index = 0;
 		reply_q->reply_post_free = (Mpi2ReplyDescriptorsUnion_t *)
@@ -4317,7 +4468,15 @@ _base_make_ioc_operational(struct MPT3SAS_ADAPTER *ioc, int sleep_flag)
 			    cpu_to_le64(ULLONG_MAX);
 		if (!_base_is_controller_msix_enabled(ioc))
 			goto skip_init_reply_post_free_queue;
-		reply_post_free += reply_post_free_sz;
+		/*
+		 * If RDPQ is enabled, switch to the next allocation.
+		 * Otherwise advance within the contiguous region.
+		 */
+		if (ioc->rdpq_array_enable)
+			reply_post_free = (long)
+			    ioc->reply_post[++index].reply_post_free;
+		else
+			reply_post_free += reply_post_free_sz;
 	}
  skip_init_reply_post_free_queue:
 
@@ -4428,6 +4587,8 @@ mpt3sas_base_attach(struct MPT3SAS_ADAPTER *ioc)
 		goto out_free_resources;
 	}
 
+	ioc->rdpq_array_enable_assigned = 0;
+	ioc->dma_mask = 0;
 	r = mpt3sas_base_map_resources(ioc);
 	if (r)
 		goto out_free_resources;
@@ -4804,6 +4965,12 @@ mpt3sas_base_hard_reset_handler(struct MPT3SAS_ADAPTER *ioc, int sleep_flag,
 	r = _base_get_ioc_facts(ioc, CAN_SLEEP);
 	if (r)
 		goto out;
+
+	if (ioc->rdpq_array_enable && !ioc->rdpq_array_capable)
+		panic("%s: Issue occurred with flashing controller firmware."
+		      "Please reboot the system and ensure that the correct"
+		      " firmware version is running\n", ioc->name);
+
 	r = _base_make_ioc_operational(ioc, sleep_flag);
 	if (!r)
 		_base_reset_handler(ioc, MPT3_IOC_DONE_RESET);
diff --git a/drivers/scsi/mpt3sas/mpt3sas_base.h b/drivers/scsi/mpt3sas/mpt3sas_base.h
index 9b90a6fef706..40926aa9b24d 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_base.h
+++ b/drivers/scsi/mpt3sas/mpt3sas_base.h
@@ -3,7 +3,7 @@
  * for access to MPT (Message Passing Technology) firmware.
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_base.h
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
@@ -70,8 +70,8 @@
 #define MPT3SAS_DRIVER_NAME		"mpt3sas"
 #define MPT3SAS_AUTHOR	"LSI Corporation <DL-MPTFusionLinux@lsi.com>"
 #define MPT3SAS_DESCRIPTION	"LSI MPT Fusion SAS 3.0 Device Driver"
-#define MPT3SAS_DRIVER_VERSION		"02.100.00.00"
-#define MPT3SAS_MAJOR_VERSION		2
+#define MPT3SAS_DRIVER_VERSION		"04.100.00.00"
+#define MPT3SAS_MAJOR_VERSION		4
 #define MPT3SAS_MINOR_VERSION		100
 #define MPT3SAS_BUILD_VERSION		0
 #define MPT3SAS_RELEASE_VERSION	00
@@ -130,7 +130,25 @@
 #define MPT_TARGET_FLAGS_DELETED	0x04
 #define MPT_TARGET_FASTPATH_IO		0x08
 
+/*
+ * Intel HBA branding
+ */
+#define MPT3SAS_INTEL_RMS3JC080_BRANDING       \
+	"Intel(R) Integrated RAID Module RMS3JC080"
+#define MPT3SAS_INTEL_RS3GC008_BRANDING       \
+	"Intel(R) RAID Controller RS3GC008"
+#define MPT3SAS_INTEL_RS3FC044_BRANDING       \
+	"Intel(R) RAID Controller RS3FC044"
+#define MPT3SAS_INTEL_RS3UC080_BRANDING       \
+	"Intel(R) RAID Controller RS3UC080"
 
+/*
+ * Intel HBA SSDIDs
+ */
+#define MPT3SAS_INTEL_RMS3JC080_SSDID	0x3521
+#define MPT3SAS_INTEL_RS3GC008_SSDID	0x3522
+#define MPT3SAS_INTEL_RS3FC044_SSDID	0x3523
+#define MPT3SAS_INTEL_RS3UC080_SSDID    0x3524
 
 /*
  * status bits for ioc->diag_buffer_status
@@ -272,8 +290,10 @@ struct _internal_cmd {
  * @channel: target channel
  * @slot: number number
  * @phy: phy identifier provided in sas device page 0
- * @fast_path: fast path feature enable bit
  * @responding: used in _scsih_sas_device_mark_responding
+ * @fast_path: fast path feature enable bit
+ * @pfa_led_on: flag for PFA LED status
+ *
  */
 struct _sas_device {
 	struct list_head list;
@@ -293,6 +313,7 @@ struct _sas_device {
 	u8	phy;
 	u8	responding;
 	u8	fast_path;
+	u8	pfa_led_on;
 };
 
 /**
@@ -548,6 +569,11 @@ struct mpt3sas_port_facts {
 	u16			MaxPostedCmdBuffers;
 };
 
+struct reply_post_struct {
+	Mpi2ReplyDescriptorsUnion_t	*reply_post_free;
+	dma_addr_t			reply_post_free_dma;
+};
+
 /**
  * enum mutex_type - task management mutex type
  * @TM_MUTEX_OFF: mutex is not required becuase calling function is acquiring it
@@ -576,6 +602,7 @@ typedef void (*MPT3SAS_FLUSH_RUNNING_CMDS)(struct MPT3SAS_ADAPTER *ioc);
  * @ir_firmware: IR firmware present
  * @bars: bitmask of BAR's that must be configured
  * @mask_interrupts: ignore interrupt
+ * @dma_mask: used to set the consistent dma mask
  * @fault_reset_work_q_name: fw fault work queue
  * @fault_reset_work_q: ""
  * @fault_reset_work: ""
@@ -691,8 +718,11 @@ typedef void (*MPT3SAS_FLUSH_RUNNING_CMDS)(struct MPT3SAS_ADAPTER *ioc);
  * @reply_free_dma_pool:
  * @reply_free_host_index: tail index in pool to insert free replys
  * @reply_post_queue_depth: reply post queue depth
- * @reply_post_free: pool for reply post (64bit descriptor)
- * @reply_post_free_dma:
+ * @reply_post_struct: struct for reply_post_free physical & virt address
+ * @rdpq_array_capable: FW supports multiple reply queue addresses in ioc_init
+ * @rdpq_array_enable: rdpq_array support is enabled in the driver
+ * @rdpq_array_enable_assigned: this ensures that rdpq_array_enable flag
+ *				is assigned only ones
  * @reply_queue_count: number of reply queue's
  * @reply_queue_list: link list contaning the reply queue info
  * @reply_post_host_index: head index in the pool where FW completes IO
@@ -714,6 +744,7 @@ struct MPT3SAS_ADAPTER {
 	u8		ir_firmware;
 	int		bars;
 	u8		mask_interrupts;
+	int		dma_mask;
 
 	/* fw fault handler */
 	char		fault_reset_work_q_name[20];
@@ -893,8 +924,10 @@ struct MPT3SAS_ADAPTER {
 
 	/* reply post queue */
 	u16		reply_post_queue_depth;
-	Mpi2ReplyDescriptorsUnion_t *reply_post_free;
-	dma_addr_t	reply_post_free_dma;
+	struct reply_post_struct *reply_post;
+	u8		rdpq_array_capable;
+	u8		rdpq_array_enable;
+	u8		rdpq_array_enable_assigned;
 	struct dma_pool *reply_post_free_dma_pool;
 	u8		reply_queue_count;
 	struct list_head reply_queue_list;
diff --git a/drivers/scsi/mpt3sas/mpt3sas_config.c b/drivers/scsi/mpt3sas/mpt3sas_config.c
index 936ec0391990..4472c2af9255 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_config.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_config.c
@@ -2,7 +2,7 @@
  * This module provides common API for accessing firmware configuration pages
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_base.c
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/mpt3sas_ctl.c b/drivers/scsi/mpt3sas/mpt3sas_ctl.c
index ba9cbe598a91..dca14877d5ab 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_ctl.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_ctl.c
@@ -3,7 +3,7 @@
  * controllers
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_ctl.c
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/mpt3sas_ctl.h b/drivers/scsi/mpt3sas/mpt3sas_ctl.h
index 53b0c480d98f..5f3d7fd7c2f8 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_ctl.h
+++ b/drivers/scsi/mpt3sas/mpt3sas_ctl.h
@@ -3,7 +3,7 @@
  * controllers
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_ctl.h
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/mpt3sas_debug.h b/drivers/scsi/mpt3sas/mpt3sas_debug.h
index 545b22d2cbdf..4778e7dd98bd 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_debug.h
+++ b/drivers/scsi/mpt3sas/mpt3sas_debug.h
@@ -2,7 +2,7 @@
  * Logging Support for MPT (Message Passing Technology) based controllers
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_debug.c
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/mpt3sas_scsih.c b/drivers/scsi/mpt3sas/mpt3sas_scsih.c
index 135f12c20ecf..857276b8880f 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_scsih.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_scsih.c
@@ -2,7 +2,7 @@
  * Scsi Host Layer for MPT (Message Passing Technology) based controllers
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_scsih.c
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
@@ -159,7 +159,7 @@ struct sense_info {
 };
 
 #define MPT3SAS_PROCESS_TRIGGER_DIAG (0xFFFB)
-#define MPT3SAS_TURN_ON_FAULT_LED (0xFFFC)
+#define MPT3SAS_TURN_ON_PFA_LED (0xFFFC)
 #define MPT3SAS_PORT_ENABLE_COMPLETE (0xFFFD)
 #define MPT3SAS_ABRT_TASK_SET (0xFFFE)
 #define MPT3SAS_REMOVE_UNRESPONDING_DEVICES (0xFFFF)
@@ -3885,7 +3885,7 @@ _scsih_scsi_ioc_info(struct MPT3SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
 #endif
 
 /**
- * _scsih_turn_on_fault_led - illuminate Fault LED
+ * _scsih_turn_on_pfa_led - illuminate PFA LED
  * @ioc: per adapter object
  * @handle: device handle
  * Context: process
@@ -3893,10 +3893,15 @@ _scsih_scsi_ioc_info(struct MPT3SAS_ADAPTER *ioc, struct scsi_cmnd *scmd,
  * Return nothing.
  */
 static void
-_scsih_turn_on_fault_led(struct MPT3SAS_ADAPTER *ioc, u16 handle)
+_scsih_turn_on_pfa_led(struct MPT3SAS_ADAPTER *ioc, u16 handle)
 {
 	Mpi2SepReply_t mpi_reply;
 	Mpi2SepRequest_t mpi_request;
+	struct _sas_device *sas_device;
+
+	sas_device = _scsih_sas_device_find_by_handle(ioc, handle);
+	if (!sas_device)
+		return;
 
 	memset(&mpi_request, 0, sizeof(Mpi2SepRequest_t));
 	mpi_request.Function = MPI2_FUNCTION_SCSI_ENCLOSURE_PROCESSOR;
@@ -3911,6 +3916,7 @@ _scsih_turn_on_fault_led(struct MPT3SAS_ADAPTER *ioc, u16 handle)
 		__FILE__, __LINE__, __func__);
 		return;
 	}
+	sas_device->pfa_led_on = 1;
 
 	if (mpi_reply.IOCStatus || mpi_reply.IOCLogInfo) {
 		dewtprintk(ioc, pr_info(MPT3SAS_FMT
@@ -3920,9 +3926,46 @@ _scsih_turn_on_fault_led(struct MPT3SAS_ADAPTER *ioc, u16 handle)
 		return;
 	}
 }
+/**
+ * _scsih_turn_off_pfa_led - turn off Fault LED
+ * @ioc: per adapter object
+ * @sas_device: sas device whose PFA LED has to turned off
+ * Context: process
+ *
+ * Return nothing.
+ */
+static void
+_scsih_turn_off_pfa_led(struct MPT3SAS_ADAPTER *ioc,
+	struct _sas_device *sas_device)
+{
+	Mpi2SepReply_t mpi_reply;
+	Mpi2SepRequest_t mpi_request;
 
+	memset(&mpi_request, 0, sizeof(Mpi2SepRequest_t));
+	mpi_request.Function = MPI2_FUNCTION_SCSI_ENCLOSURE_PROCESSOR;
+	mpi_request.Action = MPI2_SEP_REQ_ACTION_WRITE_STATUS;
+	mpi_request.SlotStatus = 0;
+	mpi_request.Slot = cpu_to_le16(sas_device->slot);
+	mpi_request.DevHandle = 0;
+	mpi_request.EnclosureHandle = cpu_to_le16(sas_device->enclosure_handle);
+	mpi_request.Flags = MPI2_SEP_REQ_FLAGS_ENCLOSURE_SLOT_ADDRESS;
+	if ((mpt3sas_base_scsi_enclosure_processor(ioc, &mpi_reply,
+		&mpi_request)) != 0) {
+		printk(MPT3SAS_FMT "failure at %s:%d/%s()!\n", ioc->name,
+		__FILE__, __LINE__, __func__);
+		return;
+	}
+
+	if (mpi_reply.IOCStatus || mpi_reply.IOCLogInfo) {
+		dewtprintk(ioc, printk(MPT3SAS_FMT
+		 "enclosure_processor: ioc_status (0x%04x), loginfo(0x%08x)\n",
+		 ioc->name, le16_to_cpu(mpi_reply.IOCStatus),
+		 le32_to_cpu(mpi_reply.IOCLogInfo)));
+		return;
+	}
+}
 /**
- * _scsih_send_event_to_turn_on_fault_led - fire delayed event
+ * _scsih_send_event_to_turn_on_pfa_led - fire delayed event
  * @ioc: per adapter object
  * @handle: device handle
  * Context: interrupt.
@@ -3930,14 +3973,14 @@ _scsih_turn_on_fault_led(struct MPT3SAS_ADAPTER *ioc, u16 handle)
  * Return nothing.
  */
 static void
-_scsih_send_event_to_turn_on_fault_led(struct MPT3SAS_ADAPTER *ioc, u16 handle)
+_scsih_send_event_to_turn_on_pfa_led(struct MPT3SAS_ADAPTER *ioc, u16 handle)
 {
 	struct fw_event_work *fw_event;
 
 	fw_event = kzalloc(sizeof(struct fw_event_work), GFP_ATOMIC);
 	if (!fw_event)
 		return;
-	fw_event->event = MPT3SAS_TURN_ON_FAULT_LED;
+	fw_event->event = MPT3SAS_TURN_ON_PFA_LED;
 	fw_event->device_handle = handle;
 	fw_event->ioc = ioc;
 	_scsih_fw_event_add(ioc, fw_event);
@@ -3981,7 +4024,7 @@ _scsih_smart_predicted_fault(struct MPT3SAS_ADAPTER *ioc, u16 handle)
 	spin_unlock_irqrestore(&ioc->sas_device_lock, flags);
 
 	if (ioc->pdev->subsystem_vendor == PCI_VENDOR_ID_IBM)
-		_scsih_send_event_to_turn_on_fault_led(ioc, handle);
+		_scsih_send_event_to_turn_on_pfa_led(ioc, handle);
 
 	/* insert into event log */
 	sz = offsetof(Mpi2EventNotificationReply_t, EventData) +
@@ -4911,7 +4954,11 @@ _scsih_remove_device(struct MPT3SAS_ADAPTER *ioc,
 {
 	struct MPT3SAS_TARGET *sas_target_priv_data;
 
-
+	if ((ioc->pdev->subsystem_vendor == PCI_VENDOR_ID_IBM) &&
+	     (sas_device->pfa_led_on)) {
+		_scsih_turn_off_pfa_led(ioc, sas_device);
+		sas_device->pfa_led_on = 0;
+	}
 	dewtprintk(ioc, pr_info(MPT3SAS_FMT
 		"%s: enter: handle(0x%04x), sas_addr(0x%016llx)\n",
 		ioc->name, __func__,
@@ -7065,8 +7112,8 @@ _mpt3sas_fw_work(struct MPT3SAS_ADAPTER *ioc, struct fw_event_work *fw_event)
 			"port enable: complete from worker thread\n",
 			ioc->name));
 		break;
-	case MPT3SAS_TURN_ON_FAULT_LED:
-		_scsih_turn_on_fault_led(ioc, fw_event->device_handle);
+	case MPT3SAS_TURN_ON_PFA_LED:
+		_scsih_turn_on_pfa_led(ioc, fw_event->device_handle);
 		break;
 	case MPI2_EVENT_SAS_TOPOLOGY_CHANGE_LIST:
 		_scsih_sas_topology_change_event(ioc, fw_event);
@@ -7734,6 +7781,7 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 {
 	struct MPT3SAS_ADAPTER *ioc;
 	struct Scsi_Host *shost;
+	int rv;
 
 	shost = scsi_host_alloc(&scsih_driver_template,
 	    sizeof(struct MPT3SAS_ADAPTER));
@@ -7826,6 +7874,7 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if (!ioc->firmware_event_thread) {
 		pr_err(MPT3SAS_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
+		rv = -ENODEV;
 		goto out_thread_fail;
 	}
 
@@ -7833,12 +7882,13 @@ _scsih_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 	if ((mpt3sas_base_attach(ioc))) {
 		pr_err(MPT3SAS_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
+		rv = -ENODEV;
 		goto out_attach_fail;
 	}
-	if ((scsi_add_host(shost, &pdev->dev))) {
+	rv = scsi_add_host(shost, &pdev->dev);
+	if (rv) {
 		pr_err(MPT3SAS_FMT "failure at %s:%d/%s()!\n",
 		    ioc->name, __FILE__, __LINE__, __func__);
-		list_del(&ioc->list);
 		goto out_add_shost_fail;
 	}
 
@@ -7851,7 +7901,7 @@ out_add_shost_fail:
  out_thread_fail:
 	list_del(&ioc->list);
 	scsi_host_put(shost);
-	return -ENODEV;
+	return rv;
 }
 
 #ifdef CONFIG_PM
diff --git a/drivers/scsi/mpt3sas/mpt3sas_transport.c b/drivers/scsi/mpt3sas/mpt3sas_transport.c
index 65170cb1a00f..d4bafaaebea9 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_transport.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_transport.c
@@ -2,7 +2,7 @@
  * SAS Transport Layer for MPT (Message Passing Technology) based controllers
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_transport.c
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.c b/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.c
index f6533ab20364..8a2dd113f401 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.c
+++ b/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.c
@@ -3,7 +3,7 @@
  * (Message Passing Technology) based controllers
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_trigger_diag.c
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.h b/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.h
index bb693923bef1..f681db56c53b 100644
--- a/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.h
+++ b/drivers/scsi/mpt3sas/mpt3sas_trigger_diag.h
@@ -4,7 +4,7 @@
  * controllers
  *
  * This code is based on drivers/scsi/mpt3sas/mpt3sas_base.h
- * Copyright (C) 2012-2013  LSI Corporation
+ * Copyright (C) 2012-2014  LSI Corporation
  *  (mailto:DL-MPTFusionLinux@lsi.com)
  *
  * This program is free software; you can redistribute it and/or
diff --git a/drivers/scsi/nsp32.c b/drivers/scsi/nsp32.c
index 50b086aef178..90abb03c9074 100644
--- a/drivers/scsi/nsp32.c
+++ b/drivers/scsi/nsp32.c
@@ -915,7 +915,7 @@ static int nsp32_queuecommand_lck(struct scsi_cmnd *SCpnt, void (*done)(struct s
 	int ret;
 
 	nsp32_dbg(NSP32_DEBUG_QUEUECOMMAND,
-		  "enter. target: 0x%x LUN: 0x%llu cmnd: 0x%x cmndlen: 0x%x "
+		  "enter. target: 0x%x LUN: 0x%llx cmnd: 0x%x cmndlen: 0x%x "
 		  "use_sg: 0x%x reqbuf: 0x%lx reqlen: 0x%x",
 		  SCpnt->device->id, SCpnt->device->lun, SCpnt->cmnd[0], SCpnt->cmd_len,
 		  scsi_sg_count(SCpnt), scsi_sglist(SCpnt), scsi_bufflen(SCpnt));
@@ -930,7 +930,7 @@ static int nsp32_queuecommand_lck(struct scsi_cmnd *SCpnt, void (*done)(struct s
 
 	/* check target ID is not same as this initiator ID */
 	if (scmd_id(SCpnt) == SCpnt->device->host->this_id) {
-		nsp32_dbg(NSP32_DEBUG_QUEUECOMMAND, "terget==host???");
+		nsp32_dbg(NSP32_DEBUG_QUEUECOMMAND, "target==host???");
 		SCpnt->result = DID_BAD_TARGET << 16;
 		done(SCpnt);
 		return 0;
diff --git a/drivers/scsi/osd/Kbuild b/drivers/scsi/osd/Kbuild
index 5fd73d77c3af..58cecd45b0f5 100644
--- a/drivers/scsi/osd/Kbuild
+++ b/drivers/scsi/osd/Kbuild
@@ -4,7 +4,7 @@
 # Copyright (C) 2008 Panasas Inc.  All rights reserved.
 #
 # Authors:
-#   Boaz Harrosh <bharrosh@panasas.com>
+#   Boaz Harrosh <ooo@electrozaur.com>
 #   Benny Halevy <bhalevy@panasas.com>
 #
 # This program is free software; you can redistribute it and/or modify
diff --git a/drivers/scsi/osd/Kconfig b/drivers/scsi/osd/Kconfig
index a0703514eb0f..347cc5e33749 100644
--- a/drivers/scsi/osd/Kconfig
+++ b/drivers/scsi/osd/Kconfig
@@ -4,7 +4,7 @@
 # Copyright (C) 2008 Panasas Inc.  All rights reserved.
 #
 # Authors:
-#   Boaz Harrosh <bharrosh@panasas.com>
+#   Boaz Harrosh <ooo@electrozaur.com>
 #   Benny Halevy <bhalevy@panasas.com>
 #
 # This program is free software; you can redistribute it and/or modify
diff --git a/drivers/scsi/osd/osd_debug.h b/drivers/scsi/osd/osd_debug.h
index 579e491f11df..26341261bb5c 100644
--- a/drivers/scsi/osd/osd_debug.h
+++ b/drivers/scsi/osd/osd_debug.h
@@ -4,7 +4,7 @@
  * Copyright (C) 2008 Panasas Inc.  All rights reserved.
  *
  * Authors:
- *   Boaz Harrosh <bharrosh@panasas.com>
+ *   Boaz Harrosh <ooo@electrozaur.com>
  *   Benny Halevy <bhalevy@panasas.com>
  *
  * This program is free software; you can redistribute it and/or modify
diff --git a/drivers/scsi/osd/osd_initiator.c b/drivers/scsi/osd/osd_initiator.c
index 5f4cbf0c4759..488c3929f19a 100644
--- a/drivers/scsi/osd/osd_initiator.c
+++ b/drivers/scsi/osd/osd_initiator.c
@@ -7,7 +7,7 @@
  * Copyright (C) 2008 Panasas Inc.  All rights reserved.
  *
  * Authors:
- *   Boaz Harrosh <bharrosh@panasas.com>
+ *   Boaz Harrosh <ooo@electrozaur.com>
  *   Benny Halevy <bhalevy@panasas.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -57,7 +57,7 @@
 
 enum { OSD_REQ_RETRIES = 1 };
 
-MODULE_AUTHOR("Boaz Harrosh <bharrosh@panasas.com>");
+MODULE_AUTHOR("Boaz Harrosh <ooo@electrozaur.com>");
 MODULE_DESCRIPTION("open-osd initiator library libosd.ko");
 MODULE_LICENSE("GPL");
 
@@ -1567,8 +1567,8 @@ static struct request *_make_request(struct request_queue *q, bool has_write,
 		struct request *req;
 
 		req = blk_get_request(q, has_write ? WRITE : READ, flags);
-		if (unlikely(!req))
-			return ERR_PTR(-ENOMEM);
+		if (IS_ERR(req))
+			return req;
 
 		blk_rq_set_block_pc(req);
 		return req;
diff --git a/drivers/scsi/osd/osd_uld.c b/drivers/scsi/osd/osd_uld.c
index e1d9a4c4c4b3..92cdd4b06526 100644
--- a/drivers/scsi/osd/osd_uld.c
+++ b/drivers/scsi/osd/osd_uld.c
@@ -10,7 +10,7 @@
  * Copyright (C) 2008 Panasas Inc.  All rights reserved.
  *
  * Authors:
- *   Boaz Harrosh <bharrosh@panasas.com>
+ *   Boaz Harrosh <ooo@electrozaur.com>
  *   Benny Halevy <bhalevy@panasas.com>
  *
  * This program is free software; you can redistribute it and/or modify
@@ -74,7 +74,7 @@
 static const char osd_name[] = "osd";
 static const char *osd_version_string = "open-osd 0.2.1";
 
-MODULE_AUTHOR("Boaz Harrosh <bharrosh@panasas.com>");
+MODULE_AUTHOR("Boaz Harrosh <ooo@electrozaur.com>");
 MODULE_DESCRIPTION("open-osd Upper-Layer-Driver osd.ko");
 MODULE_LICENSE("GPL");
 MODULE_ALIAS_CHARDEV_MAJOR(SCSI_OSD_MAJOR);
diff --git a/drivers/scsi/osst.c b/drivers/scsi/osst.c
index 0727ea7cc387..dff37a250d79 100644
--- a/drivers/scsi/osst.c
+++ b/drivers/scsi/osst.c
@@ -362,7 +362,7 @@ static int osst_execute(struct osst_request *SRpnt, const unsigned char *cmd,
 	int write = (data_direction == DMA_TO_DEVICE);
 
 	req = blk_get_request(SRpnt->stp->device->request_queue, write, GFP_KERNEL);
-	if (!req)
+	if (IS_ERR(req))
 		return DRIVER_ERROR << 24;
 
 	blk_rq_set_block_pc(req);
diff --git a/drivers/scsi/pcmcia/nsp_cs.c b/drivers/scsi/pcmcia/nsp_cs.c
index 340ceff03823..34aad32829f5 100644
--- a/drivers/scsi/pcmcia/nsp_cs.c
+++ b/drivers/scsi/pcmcia/nsp_cs.c
@@ -533,7 +533,7 @@ static int nsp_negate_signal(struct scsi_cmnd *SCpnt, unsigned char mask,
 	} while ((--time_out != 0) && (reg & mask) != 0);
 
 	if (time_out == 0) {
-		nsp_msg(KERN_DEBUG, " %s signal off timeut", str);
+		nsp_msg(KERN_DEBUG, " %s signal off timeout", str);
 	}
 
 	return 0;
diff --git a/drivers/scsi/pm8001/pm8001_ctl.c b/drivers/scsi/pm8001/pm8001_ctl.c
index 7abbf284da1a..be8269c8d127 100644
--- a/drivers/scsi/pm8001/pm8001_ctl.c
+++ b/drivers/scsi/pm8001/pm8001_ctl.c
@@ -385,7 +385,6 @@ static ssize_t pm8001_ctl_bios_version_show(struct device *cdev,
 	struct sas_ha_struct *sha = SHOST_TO_SAS_HA(shost);
 	struct pm8001_hba_info *pm8001_ha = sha->lldd_ha;
 	char *str = buf;
-	void *virt_addr;
 	int bios_index;
 	DECLARE_COMPLETION_ONSTACK(completion);
 	struct pm8001_ioctl_payload payload;
@@ -402,11 +401,10 @@ static ssize_t pm8001_ctl_bios_version_show(struct device *cdev,
 		return -ENOMEM;
 	}
 	wait_for_completion(&completion);
-	virt_addr = pm8001_ha->memoryMap.region[NVMD].virt_ptr;
 	for (bios_index = BIOSOFFSET; bios_index < BIOS_OFFSET_LIMIT;
 		bios_index++)
 		str += sprintf(str, "%c",
-			*((u8 *)((u8 *)virt_addr+bios_index)));
+			*(payload.func_specific+bios_index));
 	kfree(payload.func_specific);
 	return str - buf;
 }
diff --git a/drivers/scsi/pm8001/pm8001_hwi.c b/drivers/scsi/pm8001/pm8001_hwi.c
index dd12c6fe57a6..933f21471951 100644
--- a/drivers/scsi/pm8001/pm8001_hwi.c
+++ b/drivers/scsi/pm8001/pm8001_hwi.c
@@ -3132,6 +3132,7 @@ void pm8001_mpi_set_nvmd_resp(struct pm8001_hba_info *pm8001_ha, void *piomb)
 void
 pm8001_mpi_get_nvmd_resp(struct pm8001_hba_info *pm8001_ha, void *piomb)
 {
+	struct fw_control_ex    *fw_control_context;
 	struct get_nvm_data_resp *pPayload =
 		(struct get_nvm_data_resp *)(piomb + 4);
 	u32 tag = le32_to_cpu(pPayload->tag);
@@ -3140,6 +3141,7 @@ pm8001_mpi_get_nvmd_resp(struct pm8001_hba_info *pm8001_ha, void *piomb)
 	u32 ir_tds_bn_dps_das_nvm =
 		le32_to_cpu(pPayload->ir_tda_bn_dps_das_nvm);
 	void *virt_addr = pm8001_ha->memoryMap.region[NVMD].virt_ptr;
+	fw_control_context = ccb->fw_control_context;
 
 	PM8001_MSG_DBG(pm8001_ha, pm8001_printk("Get nvm data complete!\n"));
 	if ((dlen_status & NVMD_STAT) != 0) {
@@ -3180,6 +3182,12 @@ pm8001_mpi_get_nvmd_resp(struct pm8001_hba_info *pm8001_ha, void *piomb)
 			pm8001_printk("Get NVMD success, IR=0, dataLen=%d\n",
 			(dlen_status & NVMD_LEN) >> 24));
 	}
+	/* Though fw_control_context is freed below, usrAddr still needs
+	 * to be updated as this holds the response to the request function
+	 */
+	memcpy(fw_control_context->usrAddr,
+		pm8001_ha->memoryMap.region[NVMD].virt_ptr,
+		fw_control_context->len);
 	kfree(ccb->fw_control_context);
 	ccb->task = NULL;
 	ccb->ccb_tag = 0xFFFFFFFF;
diff --git a/drivers/scsi/pmcraid.c b/drivers/scsi/pmcraid.c
index 6f3275d020a0..bcb64eb1387f 100644
--- a/drivers/scsi/pmcraid.c
+++ b/drivers/scsi/pmcraid.c
@@ -4698,19 +4698,10 @@ pmcraid_register_interrupt_handler(struct pmcraid_instance *pinstance)
 		for (i = 0; i < PMCRAID_NUM_MSIX_VECTORS; i++)
 			entries[i].entry = i;
 
-		rc = pci_enable_msix(pdev, entries, num_hrrq);
-		if (rc < 0)
+		num_hrrq = pci_enable_msix_range(pdev, entries, 1, num_hrrq);
+		if (num_hrrq < 0)
 			goto pmcraid_isr_legacy;
 
-		/* Check how many MSIX vectors are allocated and register
-		 * msi-x handlers for each of them giving appropriate buffer
-		 */
-		if (rc > 0) {
-			num_hrrq = rc;
-			if (pci_enable_msix(pdev, entries, num_hrrq))
-				goto pmcraid_isr_legacy;
-		}
-
 		for (i = 0; i < num_hrrq; i++) {
 			pinstance->hrrq_vector[i].hrrq_id = i;
 			pinstance->hrrq_vector[i].drv_inst = pinstance;
@@ -4746,7 +4737,6 @@ pmcraid_isr_legacy:
 	pinstance->hrrq_vector[0].drv_inst = pinstance;
 	pinstance->hrrq_vector[0].vector = pdev->irq;
 	pinstance->num_hrrq = 1;
-	rc = 0;
 
 	rc = request_irq(pdev->irq, pmcraid_isr, IRQF_SHARED,
 			 PMCRAID_DRIVER_NAME, &pinstance->hrrq_vector[0]);
diff --git a/drivers/scsi/qla2xxx/Kconfig b/drivers/scsi/qla2xxx/Kconfig
index 23d607218ae8..113e6c9826a1 100644
--- a/drivers/scsi/qla2xxx/Kconfig
+++ b/drivers/scsi/qla2xxx/Kconfig
@@ -1,7 +1,7 @@
 config SCSI_QLA_FC
 	tristate "QLogic QLA2XXX Fibre Channel Support"
 	depends on PCI && SCSI
-	select SCSI_FC_ATTRS
+	depends on SCSI_FC_ATTRS
 	select FW_LOADER
 	---help---
 	This qla2xxx driver supports all QLogic Fibre Channel
@@ -31,7 +31,7 @@ config SCSI_QLA_FC
 config TCM_QLA2XXX
 	tristate "TCM_QLA2XXX fabric module for Qlogic 2xxx series target mode HBAs"
 	depends on SCSI_QLA_FC && TARGET_CORE
-	select LIBFC
+	depends on LIBFC
 	select BTREE
 	default n
 	---help---
diff --git a/drivers/scsi/qla2xxx/qla_attr.c b/drivers/scsi/qla2xxx/qla_attr.c
index 16fe5196e6d9..82b92c414a9c 100644
--- a/drivers/scsi/qla2xxx/qla_attr.c
+++ b/drivers/scsi/qla2xxx/qla_attr.c
@@ -484,7 +484,8 @@ qla2x00_sysfs_write_optrom_ctl(struct file *filp, struct kobject *kobj,
 		    start == (ha->flt_region_fw * 4))
 			valid = 1;
 		else if (IS_QLA24XX_TYPE(ha) || IS_QLA25XX(ha)
-			|| IS_CNA_CAPABLE(ha) || IS_QLA2031(ha))
+			|| IS_CNA_CAPABLE(ha) || IS_QLA2031(ha)
+			|| IS_QLA27XX(ha))
 			valid = 1;
 		if (!valid) {
 			ql_log(ql_log_warn, vha, 0x7065,
@@ -987,6 +988,8 @@ qla2x00_free_sysfs_attr(scsi_qla_host_t *vha, bool stop_beacon)
 			continue;
 		if (iter->is4GBp_only == 3 && !(IS_CNA_CAPABLE(vha->hw)))
 			continue;
+		if (iter->is4GBp_only == 0x27 && !IS_QLA27XX(vha->hw))
+			continue;
 
 		sysfs_remove_bin_file(&host->shost_gendev.kobj,
 		    iter->attr);
@@ -1014,7 +1017,7 @@ qla2x00_fw_version_show(struct device *dev,
 	char fw_str[128];
 
 	return scnprintf(buf, PAGE_SIZE, "%s\n",
-	    ha->isp_ops->fw_version_str(vha, fw_str));
+	    ha->isp_ops->fw_version_str(vha, fw_str, sizeof(fw_str)));
 }
 
 static ssize_t
@@ -1440,7 +1443,7 @@ qla2x00_fw_state_show(struct device *dev, struct device_attribute *attr,
 {
 	scsi_qla_host_t *vha = shost_priv(class_to_shost(dev));
 	int rval = QLA_FUNCTION_FAILED;
-	uint16_t state[5];
+	uint16_t state[6];
 	uint32_t pstate;
 
 	if (IS_QLAFX00(vha->hw)) {
@@ -1456,8 +1459,8 @@ qla2x00_fw_state_show(struct device *dev, struct device_attribute *attr,
 	if (rval != QLA_SUCCESS)
 		memset(state, -1, sizeof(state));
 
-	return scnprintf(buf, PAGE_SIZE, "0x%x 0x%x 0x%x 0x%x 0x%x\n", state[0],
-	    state[1], state[2], state[3], state[4]);
+	return scnprintf(buf, PAGE_SIZE, "0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+	    state[0], state[1], state[2], state[3], state[4], state[5]);
 }
 
 static ssize_t
@@ -1924,7 +1927,8 @@ qla2x00_get_host_symbolic_name(struct Scsi_Host *shost)
 {
 	scsi_qla_host_t *vha = shost_priv(shost);
 
-	qla2x00_get_sym_node_name(vha, fc_host_symbolic_name(shost));
+	qla2x00_get_sym_node_name(vha, fc_host_symbolic_name(shost),
+	    sizeof(fc_host_symbolic_name(shost)));
 }
 
 static void
diff --git a/drivers/scsi/qla2xxx/qla_bsg.c b/drivers/scsi/qla2xxx/qla_bsg.c
index 524f9eb7fcd1..2e2bb6f45ce6 100644
--- a/drivers/scsi/qla2xxx/qla_bsg.c
+++ b/drivers/scsi/qla2xxx/qla_bsg.c
@@ -1390,7 +1390,7 @@ qla2x00_optrom_setup(struct fc_bsg_job *bsg_job, scsi_qla_host_t *vha,
 		    start == (ha->flt_region_fw * 4))
 			valid = 1;
 		else if (IS_QLA24XX_TYPE(ha) || IS_QLA25XX(ha) ||
-		    IS_CNA_CAPABLE(ha) || IS_QLA2031(ha))
+		    IS_CNA_CAPABLE(ha) || IS_QLA2031(ha) || IS_QLA27XX(ha))
 			valid = 1;
 		if (!valid) {
 			ql_log(ql_log_warn, vha, 0x7058,
diff --git a/drivers/scsi/qla2xxx/qla_dbg.c b/drivers/scsi/qla2xxx/qla_dbg.c
index c72ee97bf3f7..d77fe43793b6 100644
--- a/drivers/scsi/qla2xxx/qla_dbg.c
+++ b/drivers/scsi/qla2xxx/qla_dbg.c
@@ -11,19 +11,15 @@
  * ----------------------------------------------------------------------
  * |             Level            |   Last Value Used  |     Holes	|
  * ----------------------------------------------------------------------
- * | Module Init and Probe        |       0x017d       | 0x004b,0x0141	|
- * |                              |                    | 0x0144,0x0146	|
+ * | Module Init and Probe        |       0x017d       | 0x0144,0x0146	|
  * |                              |                    | 0x015b-0x0160	|
  * |                              |                    | 0x016e-0x0170	|
- * | Mailbox commands             |       0x118d       | 0x1018-0x1019	|
- * |                              |                    | 0x10ca         |
- * |                              |                    | 0x1115-0x1116  |
- * |                              |                    | 0x111a-0x111b	|
- * |                              |                    | 0x1155-0x1158  |
- * | Device Discovery             |       0x2095       | 0x2020-0x2022, |
+ * | Mailbox commands             |       0x118d       | 0x1115-0x1116	|
+ * |                              |                    | 0x111a-0x111b  |
+ * | Device Discovery             |       0x2016       | 0x2020-0x2022, |
  * |                              |                    | 0x2011-0x2012, |
- * |                              |                    | 0x2016         |
- * | Queue Command and IO tracing |       0x3059       | 0x3006-0x300b  |
+ * |                              |                    | 0x2099-0x20a4  |
+ * | Queue Command and IO tracing |       0x3059       | 0x300b         |
  * |                              |                    | 0x3027-0x3028  |
  * |                              |                    | 0x303d-0x3041  |
  * |                              |                    | 0x302d,0x3033  |
@@ -31,10 +27,10 @@
  * |                              |                    | 0x303a		|
  * | DPC Thread                   |       0x4023       | 0x4002,0x4013  |
  * | Async Events                 |       0x5087       | 0x502b-0x502f  |
- * |                              |                    | 0x5047,0x5052  |
+ * |                              |                    | 0x5047		|
  * |                              |                    | 0x5084,0x5075	|
  * |                              |                    | 0x503d,0x5044  |
- * |                              |                    | 0x507b		|
+ * |                              |                    | 0x507b,0x505f	|
  * | Timer Routines               |       0x6012       |                |
  * | User Space Interactions      |       0x70e2       | 0x7018,0x702e  |
  * |				  |		       | 0x7020,0x7024  |
@@ -64,13 +60,15 @@
  * |                              |                    | 0xb13c-0xb140  |
  * |                              |                    | 0xb149		|
  * | MultiQ                       |       0xc00c       |		|
- * | Misc                         |       0xd212       | 0xd017-0xd019	|
- * |                              |                    | 0xd020		|
- * |                              |                    | 0xd030-0xd0ff	|
+ * | Misc                         |       0xd213       | 0xd011-0xd017	|
+ * |                              |                    | 0xd021,0xd024	|
+ * |                              |                    | 0xd025,0xd029	|
+ * |                              |                    | 0xd02a,0xd02e	|
+ * |                              |                    | 0xd031-0xd0ff	|
  * |                              |                    | 0xd101-0xd1fe	|
- * |                              |                    | 0xd213-0xd2fe	|
- * | Target Mode		  |	  0xe078       |		|
- * | Target Mode Management	  |	  0xf072       | 0xf002-0xf003	|
+ * |                              |                    | 0xd214-0xd2fe	|
+ * | Target Mode		  |	  0xe079       |		|
+ * | Target Mode Management	  |	  0xf072       | 0xf002		|
  * |                              |                    | 0xf046-0xf049  |
  * | Target Mode Task Management  |	  0x1000b      |		|
  * ----------------------------------------------------------------------
diff --git a/drivers/scsi/qla2xxx/qla_def.h b/drivers/scsi/qla2xxx/qla_def.h
index b64399153135..5f6b2960cccb 100644
--- a/drivers/scsi/qla2xxx/qla_def.h
+++ b/drivers/scsi/qla2xxx/qla_def.h
@@ -191,6 +191,11 @@
 					     * reset-recovery completion is
 					     * second
 					     */
+/* ISP2031: Values for laser on/off */
+#define PORT_0_2031	0x00201340
+#define PORT_1_2031	0x00201350
+#define LASER_ON_2031	0x01800100
+#define LASER_OFF_2031	0x01800180
 
 /*
  * The ISP2312 v2 chip cannot access the FLASH/GPIO registers via MMIO in an
@@ -261,6 +266,7 @@
 #define REQUEST_ENTRY_CNT_2100		128	/* Number of request entries. */
 #define REQUEST_ENTRY_CNT_2200		2048	/* Number of request entries. */
 #define REQUEST_ENTRY_CNT_24XX		2048	/* Number of request entries. */
+#define REQUEST_ENTRY_CNT_83XX		8192	/* Number of request entries. */
 #define RESPONSE_ENTRY_CNT_2100		64	/* Number of response entries.*/
 #define RESPONSE_ENTRY_CNT_2300		512	/* Number of response entries.*/
 #define RESPONSE_ENTRY_CNT_MQ		128	/* Number of response entries.*/
@@ -803,6 +809,7 @@ struct mbx_cmd_32 {
 #define MBA_FW_RESTART_CMPLT	0x8060	/* Firmware restart complete */
 #define MBA_INIT_REQUIRED	0x8061	/* Initialization required */
 #define MBA_SHUTDOWN_REQUESTED	0x8062	/* Shutdown Requested */
+#define MBA_DPORT_DIAGNOSTICS	0x8080	/* D-port Diagnostics */
 #define MBA_FW_INIT_FAILURE	0x8401	/* Firmware initialization failure */
 #define MBA_MIRROR_LUN_CHANGE	0x8402	/* Mirror LUN State Change
 					   Notification */
@@ -948,6 +955,7 @@ struct mbx_cmd_32 {
 #define MBC_WRITE_SFP			0x30	/* Write SFP Data. */
 #define MBC_READ_SFP			0x31	/* Read SFP Data. */
 #define MBC_SET_TIMEOUT_PARAMS		0x32	/* Set FW timeouts. */
+#define MBC_DPORT_DIAGNOSTICS		0x47	/* D-Port Diagnostics */
 #define MBC_MID_INITIALIZE_FIRMWARE	0x48	/* MID Initialize firmware. */
 #define MBC_MID_GET_VP_DATABASE		0x49	/* MID Get VP Database. */
 #define MBC_MID_GET_VP_ENTRY		0x4a	/* MID Get VP Entry. */
@@ -2016,6 +2024,8 @@ typedef struct fc_port {
 	unsigned long last_ramp_up;
 
 	uint16_t port_id;
+
+	unsigned long retry_delay_timestamp;
 } fc_port_t;
 
 #include "qla_mr.h"
@@ -2056,10 +2066,21 @@ static const char * const port_state_str[] = {
 
 #define CT_REJECT_RESPONSE	0x8001
 #define CT_ACCEPT_RESPONSE	0x8002
-#define CT_REASON_INVALID_COMMAND_CODE	0x01
-#define CT_REASON_CANNOT_PERFORM	0x09
-#define CT_REASON_COMMAND_UNSUPPORTED	0x0b
-#define CT_EXPL_ALREADY_REGISTERED	0x10
+#define CT_REASON_INVALID_COMMAND_CODE		0x01
+#define CT_REASON_CANNOT_PERFORM		0x09
+#define CT_REASON_COMMAND_UNSUPPORTED		0x0b
+#define CT_EXPL_ALREADY_REGISTERED		0x10
+#define CT_EXPL_HBA_ATTR_NOT_REGISTERED		0x11
+#define CT_EXPL_MULTIPLE_HBA_ATTR		0x12
+#define CT_EXPL_INVALID_HBA_BLOCK_LENGTH	0x13
+#define CT_EXPL_MISSING_REQ_HBA_ATTR		0x14
+#define CT_EXPL_PORT_NOT_REGISTERED_		0x15
+#define CT_EXPL_MISSING_HBA_ID_PORT_LIST	0x16
+#define CT_EXPL_HBA_NOT_REGISTERED		0x17
+#define CT_EXPL_PORT_ATTR_NOT_REGISTERED	0x20
+#define CT_EXPL_PORT_NOT_REGISTERED		0x21
+#define CT_EXPL_MULTIPLE_PORT_ATTR		0x22
+#define CT_EXPL_INVALID_PORT_BLOCK_LENGTH	0x23
 
 #define NS_N_PORT_TYPE	0x01
 #define NS_NL_PORT_TYPE	0x02
@@ -2116,33 +2137,40 @@ static const char * const port_state_str[] = {
  * HBA attribute types.
  */
 #define FDMI_HBA_ATTR_COUNT			9
-#define FDMI_HBA_NODE_NAME			1
-#define FDMI_HBA_MANUFACTURER			2
-#define FDMI_HBA_SERIAL_NUMBER			3
-#define FDMI_HBA_MODEL				4
-#define FDMI_HBA_MODEL_DESCRIPTION		5
-#define FDMI_HBA_HARDWARE_VERSION		6
-#define FDMI_HBA_DRIVER_VERSION			7
-#define FDMI_HBA_OPTION_ROM_VERSION		8
-#define FDMI_HBA_FIRMWARE_VERSION		9
+#define FDMIV2_HBA_ATTR_COUNT			17
+#define FDMI_HBA_NODE_NAME			0x1
+#define FDMI_HBA_MANUFACTURER			0x2
+#define FDMI_HBA_SERIAL_NUMBER			0x3
+#define FDMI_HBA_MODEL				0x4
+#define FDMI_HBA_MODEL_DESCRIPTION		0x5
+#define FDMI_HBA_HARDWARE_VERSION		0x6
+#define FDMI_HBA_DRIVER_VERSION			0x7
+#define FDMI_HBA_OPTION_ROM_VERSION		0x8
+#define FDMI_HBA_FIRMWARE_VERSION		0x9
 #define FDMI_HBA_OS_NAME_AND_VERSION		0xa
 #define FDMI_HBA_MAXIMUM_CT_PAYLOAD_LENGTH	0xb
+#define FDMI_HBA_NODE_SYMBOLIC_NAME		0xc
+#define FDMI_HBA_VENDOR_ID			0xd
+#define FDMI_HBA_NUM_PORTS			0xe
+#define FDMI_HBA_FABRIC_NAME			0xf
+#define FDMI_HBA_BOOT_BIOS_NAME			0x10
+#define FDMI_HBA_TYPE_VENDOR_IDENTIFIER		0xe0
 
 struct ct_fdmi_hba_attr {
 	uint16_t type;
 	uint16_t len;
 	union {
 		uint8_t node_name[WWN_SIZE];
-		uint8_t manufacturer[32];
-		uint8_t serial_num[8];
+		uint8_t manufacturer[64];
+		uint8_t serial_num[32];
 		uint8_t model[16];
 		uint8_t model_desc[80];
-		uint8_t hw_version[16];
+		uint8_t hw_version[32];
 		uint8_t driver_version[32];
 		uint8_t orom_version[16];
-		uint8_t fw_version[16];
+		uint8_t fw_version[32];
 		uint8_t os_version[128];
-		uint8_t max_ct_len[4];
+		uint32_t max_ct_len;
 	} a;
 };
 
@@ -2151,16 +2179,56 @@ struct ct_fdmi_hba_attributes {
 	struct ct_fdmi_hba_attr entry[FDMI_HBA_ATTR_COUNT];
 };
 
+struct ct_fdmiv2_hba_attr {
+	uint16_t type;
+	uint16_t len;
+	union {
+		uint8_t node_name[WWN_SIZE];
+		uint8_t manufacturer[32];
+		uint8_t serial_num[32];
+		uint8_t model[16];
+		uint8_t model_desc[80];
+		uint8_t hw_version[16];
+		uint8_t driver_version[32];
+		uint8_t orom_version[16];
+		uint8_t fw_version[32];
+		uint8_t os_version[128];
+		uint32_t max_ct_len;
+		uint8_t sym_name[256];
+		uint32_t vendor_id;
+		uint32_t num_ports;
+		uint8_t fabric_name[WWN_SIZE];
+		uint8_t bios_name[32];
+		uint8_t vendor_indentifer[8];
+	} a;
+};
+
+struct ct_fdmiv2_hba_attributes {
+	uint32_t count;
+	struct ct_fdmiv2_hba_attr entry[FDMIV2_HBA_ATTR_COUNT];
+};
+
 /*
  * Port attribute types.
  */
 #define FDMI_PORT_ATTR_COUNT		6
-#define FDMI_PORT_FC4_TYPES		1
-#define FDMI_PORT_SUPPORT_SPEED		2
-#define FDMI_PORT_CURRENT_SPEED		3
-#define FDMI_PORT_MAX_FRAME_SIZE	4
-#define FDMI_PORT_OS_DEVICE_NAME	5
-#define FDMI_PORT_HOST_NAME		6
+#define FDMIV2_PORT_ATTR_COUNT		16
+#define FDMI_PORT_FC4_TYPES		0x1
+#define FDMI_PORT_SUPPORT_SPEED		0x2
+#define FDMI_PORT_CURRENT_SPEED		0x3
+#define FDMI_PORT_MAX_FRAME_SIZE	0x4
+#define FDMI_PORT_OS_DEVICE_NAME	0x5
+#define FDMI_PORT_HOST_NAME		0x6
+#define FDMI_PORT_NODE_NAME		0x7
+#define FDMI_PORT_NAME			0x8
+#define FDMI_PORT_SYM_NAME		0x9
+#define FDMI_PORT_TYPE			0xa
+#define FDMI_PORT_SUPP_COS		0xb
+#define FDMI_PORT_FABRIC_NAME		0xc
+#define FDMI_PORT_FC4_TYPE		0xd
+#define FDMI_PORT_STATE			0x101
+#define FDMI_PORT_COUNT			0x102
+#define FDMI_PORT_ID			0x103
 
 #define FDMI_PORT_SPEED_1GB		0x1
 #define FDMI_PORT_SPEED_2GB		0x2
@@ -2171,7 +2239,11 @@ struct ct_fdmi_hba_attributes {
 #define FDMI_PORT_SPEED_32GB		0x40
 #define FDMI_PORT_SPEED_UNKNOWN		0x8000
 
-struct ct_fdmi_port_attr {
+#define FC_CLASS_2	0x04
+#define FC_CLASS_3	0x08
+#define FC_CLASS_2_3	0x0C
+
+struct ct_fdmiv2_port_attr {
 	uint16_t type;
 	uint16_t len;
 	union {
@@ -2181,12 +2253,40 @@ struct ct_fdmi_port_attr {
 		uint32_t max_frame_size;
 		uint8_t os_dev_name[32];
 		uint8_t host_name[32];
+		uint8_t node_name[WWN_SIZE];
+		uint8_t port_name[WWN_SIZE];
+		uint8_t port_sym_name[128];
+		uint32_t port_type;
+		uint32_t port_supported_cos;
+		uint8_t fabric_name[WWN_SIZE];
+		uint8_t port_fc4_type[32];
+		uint32_t port_state;
+		uint32_t num_ports;
+		uint32_t port_id;
 	} a;
 };
 
 /*
  * Port Attribute Block.
  */
+struct ct_fdmiv2_port_attributes {
+	uint32_t count;
+	struct ct_fdmiv2_port_attr entry[FDMIV2_PORT_ATTR_COUNT];
+};
+
+struct ct_fdmi_port_attr {
+	uint16_t type;
+	uint16_t len;
+	union {
+		uint8_t fc4_types[32];
+		uint32_t sup_speed;
+		uint32_t cur_speed;
+		uint32_t max_frame_size;
+		uint8_t os_dev_name[32];
+		uint8_t host_name[32];
+	} a;
+};
+
 struct ct_fdmi_port_attributes {
 	uint32_t count;
 	struct ct_fdmi_port_attr entry[FDMI_PORT_ATTR_COUNT];
@@ -2286,6 +2386,13 @@ struct ct_sns_req {
 
 		struct {
 			uint8_t hba_identifier[8];
+			uint32_t entry_count;
+			uint8_t port_name[8];
+			struct ct_fdmiv2_hba_attributes attrs;
+		} rhba2;
+
+		struct {
+			uint8_t hba_identifier[8];
 			struct ct_fdmi_hba_attributes attrs;
 		} rhat;
 
@@ -2296,6 +2403,11 @@ struct ct_sns_req {
 
 		struct {
 			uint8_t port_name[8];
+			struct ct_fdmiv2_port_attributes attrs;
+		} rpa2;
+
+		struct {
+			uint8_t port_name[8];
 		} dhba;
 
 		struct {
@@ -2522,7 +2634,7 @@ struct isp_operations {
 	int (*load_risc) (struct scsi_qla_host *, uint32_t *);
 
 	char * (*pci_info_str) (struct scsi_qla_host *, char *);
-	char * (*fw_version_str) (struct scsi_qla_host *, char *);
+	char * (*fw_version_str)(struct scsi_qla_host *, char *, size_t);
 
 	irq_handler_t intr_handler;
 	void (*enable_intrs) (struct qla_hw_data *);
@@ -2664,6 +2776,9 @@ struct qla_statistics {
 	uint32_t control_requests;
 
 	uint64_t jiffies_at_last_reset;
+	uint32_t stat_max_pend_cmds;
+	uint32_t stat_max_qfull_cmds_alloc;
+	uint32_t stat_max_qfull_cmds_dropped;
 };
 
 struct bidi_statistics {
@@ -2786,8 +2901,22 @@ struct qlt_hw_data {
 	uint8_t saved_add_firmware_options[2];
 
 	uint8_t tgt_node_name[WWN_SIZE];
+
+	struct list_head q_full_list;
+	uint32_t num_pend_cmds;
+	uint32_t num_qfull_cmds_alloc;
+	uint32_t num_qfull_cmds_dropped;
+	spinlock_t q_full_lock;
+	uint32_t leak_exchg_thresh_hold;
 };
 
+#define MAX_QFULL_CMDS_ALLOC	8192
+#define Q_FULL_THRESH_HOLD_PERCENT 90
+#define Q_FULL_THRESH_HOLD(ha) \
+	((ha->fw_xcb_count/100) * Q_FULL_THRESH_HOLD_PERCENT)
+
+#define LEAK_EXCHG_THRESH_HOLD_PERCENT 75	/* 75 percent */
+
 /*
  * Qlogic host adapter specific data structure.
 */
@@ -2834,7 +2963,8 @@ struct qla_hw_data {
 
 		uint32_t        mr_reset_hdlr_active:1;
 		uint32_t        mr_intr_valid:1;
-		/* 34 bits */
+		uint32_t	fawwpn_enabled:1;
+		/* 35 bits */
 	} flags;
 
 	/* This spinlock is used to protect "io transactions", you must
@@ -3032,6 +3162,7 @@ struct qla_hw_data {
 #define IS_ATIO_MSIX_CAPABLE(ha) (IS_QLA83XX(ha))
 #define IS_TGT_MODE_CAPABLE(ha)	(ha->tgt.atio_q_length)
 #define IS_SHADOW_REG_CAPABLE(ha)  (IS_QLA27XX(ha))
+#define IS_DPORT_CAPABLE(ha)  (IS_QLA83XX(ha) || IS_QLA27XX(ha))
 
 	/* HBA serial number */
 	uint8_t		serial0;
@@ -3333,6 +3464,7 @@ struct qla_hw_data {
 	struct work_struct board_disable;
 
 	struct mr_data_fx00 mr;
+	uint32_t chip_reset;
 
 	struct qlt_hw_data tgt;
 	int	allow_cna_fw_dump;
@@ -3402,6 +3534,11 @@ typedef struct scsi_qla_host {
 #define FX00_CRITEMP_RECOVERY	25
 #define FX00_HOST_INFO_RESEND	26
 
+	unsigned long	pci_flags;
+#define PFLG_DISCONNECTED	0	/* PCI device removed */
+#define PFLG_DRIVER_REMOVING	1	/* PCI driver .remove */
+#define PFLG_DRIVER_PROBING	2	/* PCI driver .probe */
+
 	uint32_t	device_flags;
 #define SWITCH_FOUND		BIT_0
 #define DFLG_NO_CABLE		BIT_1
diff --git a/drivers/scsi/qla2xxx/qla_fw.h b/drivers/scsi/qla2xxx/qla_fw.h
index eb8f57249f1d..42bb357bf56b 100644
--- a/drivers/scsi/qla2xxx/qla_fw.h
+++ b/drivers/scsi/qla2xxx/qla_fw.h
@@ -91,7 +91,7 @@ struct nvram_24xx {
 	/* Firmware Initialization Control Block. */
 	uint16_t version;
 	uint16_t reserved_1;
-	uint16_t frame_payload_size;
+	__le16 frame_payload_size;
 	uint16_t execution_throttle;
 	uint16_t exchange_count;
 	uint16_t hard_address;
@@ -317,8 +317,8 @@ struct init_cb_24xx {
 	 * BIT 3  = Reserved
 	 * BIT 4  = Enable Target Mode
 	 * BIT 5  = Disable Initiator Mode
-	 * BIT 6  = Reserved
-	 * BIT 7  = Reserved
+	 * BIT 6  = Acquire FA-WWN
+	 * BIT 7  = Enable D-port Diagnostics
 	 *
 	 * BIT 8  = Reserved
 	 * BIT 9  = Non Participating LIP
@@ -567,7 +567,7 @@ struct sts_entry_24xx {
 #define SF_TRANSFERRED_DATA	BIT_11
 #define SF_FCP_RSP_DMA		BIT_0
 
-	uint16_t reserved_2;
+	uint16_t retry_delay;
 	uint16_t scsi_status;		/* SCSI status. */
 #define SS_CONFIRMATION_REQ		BIT_12
 
diff --git a/drivers/scsi/qla2xxx/qla_gbl.h b/drivers/scsi/qla2xxx/qla_gbl.h
index d646540db3ac..b1865a72ce59 100644
--- a/drivers/scsi/qla2xxx/qla_gbl.h
+++ b/drivers/scsi/qla2xxx/qla_gbl.h
@@ -72,6 +72,7 @@ extern void qla2x00_async_logout_done(struct scsi_qla_host *, fc_port_t *,
 extern void qla2x00_async_adisc_done(struct scsi_qla_host *, fc_port_t *,
     uint16_t *);
 extern void *qla2x00_alloc_iocbs(struct scsi_qla_host *, srb_t *);
+extern void *qla2x00_alloc_iocbs_ready(struct scsi_qla_host *, srb_t *);
 extern int qla24xx_update_fcport_fcp_prio(scsi_qla_host_t *, fc_port_t *);
 
 extern fc_port_t *
@@ -475,7 +476,8 @@ extern uint8_t *qla25xx_read_nvram_data(scsi_qla_host_t *, uint8_t *, uint32_t,
 extern int qla25xx_write_nvram_data(scsi_qla_host_t *, uint8_t *, uint32_t,
 				    uint32_t);
 extern int qla2x00_is_a_vp_did(scsi_qla_host_t *, uint32_t);
-bool qla2x00_check_reg_for_disconnect(scsi_qla_host_t *, uint32_t);
+bool qla2x00_check_reg32_for_disconnect(scsi_qla_host_t *, uint32_t);
+bool qla2x00_check_reg16_for_disconnect(scsi_qla_host_t *, uint16_t);
 
 extern int qla2x00_beacon_on(struct scsi_qla_host *);
 extern int qla2x00_beacon_off(struct scsi_qla_host *);
@@ -561,7 +563,7 @@ extern void *qla24xx_prep_ms_fdmi_iocb(scsi_qla_host_t *, uint32_t, uint32_t);
 extern int qla2x00_fdmi_register(scsi_qla_host_t *);
 extern int qla2x00_gfpn_id(scsi_qla_host_t *, sw_info_t *);
 extern int qla2x00_gpsc(scsi_qla_host_t *, sw_info_t *);
-extern void qla2x00_get_sym_node_name(scsi_qla_host_t *, uint8_t *);
+extern void qla2x00_get_sym_node_name(scsi_qla_host_t *, uint8_t *, size_t);
 
 /*
  * Global Function Prototypes in qla_attr.c source file.
@@ -613,7 +615,7 @@ extern void qlafx00_soft_reset(scsi_qla_host_t *);
 extern int qlafx00_chip_diag(scsi_qla_host_t *);
 extern void qlafx00_config_rings(struct scsi_qla_host *);
 extern char *qlafx00_pci_info_str(struct scsi_qla_host *, char *);
-extern char *qlafx00_fw_version_str(struct scsi_qla_host *, char *);
+extern char *qlafx00_fw_version_str(struct scsi_qla_host *, char *, size_t);
 extern irqreturn_t qlafx00_intr_handler(int, void *);
 extern void qlafx00_enable_intrs(struct qla_hw_data *);
 extern void qlafx00_disable_intrs(struct qla_hw_data *);
@@ -765,4 +767,5 @@ extern void qla82xx_mbx_completion(scsi_qla_host_t *, uint16_t);
 extern int qla8044_abort_isp(scsi_qla_host_t *);
 extern int qla8044_check_fw_alive(struct scsi_qla_host *);
 
+extern void qlt_host_reset_handler(struct qla_hw_data *ha);
 #endif /* _QLA_GBL_H */
diff --git a/drivers/scsi/qla2xxx/qla_gs.c b/drivers/scsi/qla2xxx/qla_gs.c
index a0df3b1b3823..dccc4dcc39c8 100644
--- a/drivers/scsi/qla2xxx/qla_gs.c
+++ b/drivers/scsi/qla2xxx/qla_gs.c
@@ -6,6 +6,7 @@
  */
 #include "qla_def.h"
 #include "qla_target.h"
+#include <linux/utsname.h>
 
 static int qla2x00_sns_ga_nxt(scsi_qla_host_t *, fc_port_t *);
 static int qla2x00_sns_gid_pt(scsi_qla_host_t *, sw_info_t *);
@@ -143,10 +144,10 @@ qla2x00_chk_ms_status(scsi_qla_host_t *vha, ms_iocb_entry_t *ms_pkt,
 			if (ct_rsp->header.response !=
 			    __constant_cpu_to_be16(CT_ACCEPT_RESPONSE)) {
 				ql_dbg(ql_dbg_disc + ql_dbg_buffer, vha, 0x2077,
-				    "%s failed rejected request on port_id: "
-				    "%02x%02x%02x.\n", routine,
-				    vha->d_id.b.domain, vha->d_id.b.area,
-				    vha->d_id.b.al_pa);
+				    "%s failed rejected request on port_id: %02x%02x%02x Compeltion status 0x%x, response 0x%x\n",
+				    routine, vha->d_id.b.domain,
+				    vha->d_id.b.area, vha->d_id.b.al_pa,
+				    comp_status, ct_rsp->header.response);
 				ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha,
 				    0x2078, (uint8_t *)&ct_rsp->header,
 				    sizeof(struct ct_rsp_hdr));
@@ -622,15 +623,16 @@ qla2x00_rnn_id(scsi_qla_host_t *vha)
 }
 
 void
-qla2x00_get_sym_node_name(scsi_qla_host_t *vha, uint8_t *snn)
+qla2x00_get_sym_node_name(scsi_qla_host_t *vha, uint8_t *snn, size_t size)
 {
 	struct qla_hw_data *ha = vha->hw;
 
 	if (IS_QLAFX00(ha))
-		sprintf(snn, "%s FW:v%s DVR:v%s", ha->model_number,
+		snprintf(snn, size, "%s FW:v%s DVR:v%s", ha->model_number,
 		    ha->mr.fw_version, qla2x00_version_str);
 	else
-		sprintf(snn, "%s FW:v%d.%02d.%02d DVR:v%s", ha->model_number,
+		snprintf(snn, size,
+		    "%s FW:v%d.%02d.%02d DVR:v%s", ha->model_number,
 		    ha->fw_major_version, ha->fw_minor_version,
 		    ha->fw_subminor_version, qla2x00_version_str);
 }
@@ -670,7 +672,8 @@ qla2x00_rsnn_nn(scsi_qla_host_t *vha)
 	memcpy(ct_req->req.rsnn_nn.node_name, vha->node_name, WWN_SIZE);
 
 	/* Prepare the Symbolic Node Name */
-	qla2x00_get_sym_node_name(vha, ct_req->req.rsnn_nn.sym_node_name);
+	qla2x00_get_sym_node_name(vha, ct_req->req.rsnn_nn.sym_node_name,
+	    sizeof(ct_req->req.rsnn_nn.sym_node_name));
 
 	/* Calculate SNN length */
 	ct_req->req.rsnn_nn.name_len =
@@ -1263,7 +1266,7 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	ms_iocb_entry_t *ms_pkt;
 	struct ct_sns_req *ct_req;
 	struct ct_sns_rsp *ct_rsp;
-	uint8_t *entries;
+	void *entries;
 	struct ct_fdmi_hba_attr *eiter;
 	struct qla_hw_data *ha = vha->hw;
 
@@ -1288,7 +1291,7 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	entries = ct_req->req.rhba.hba_identifier;
 
 	/* Nodename. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_NODE_NAME);
 	eiter->len = __constant_cpu_to_be16(4 + WWN_SIZE);
 	memcpy(eiter->a.node_name, vha->node_name, WWN_SIZE);
@@ -1298,11 +1301,12 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "NodeName = %8phN.\n", eiter->a.node_name);
 
 	/* Manufacturer. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_MANUFACTURER);
 	alen = strlen(QLA2XXX_MANUFACTURER);
-	strncpy(eiter->a.manufacturer, QLA2XXX_MANUFACTURER, alen + 1);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	snprintf(eiter->a.manufacturer, sizeof(eiter->a.manufacturer),
+	    "%s", "QLogic Corporation");
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1310,12 +1314,19 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "Manufacturer = %s.\n", eiter->a.manufacturer);
 
 	/* Serial number. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_SERIAL_NUMBER);
-	sn = ((ha->serial0 & 0x1f) << 16) | (ha->serial2 << 8) | ha->serial1;
-	sprintf(eiter->a.serial_num, "%c%05d", 'A' + sn / 100000, sn % 100000);
+	if (IS_FWI2_CAPABLE(ha))
+		qla2xxx_get_vpd_field(vha, "SN", eiter->a.serial_num,
+		    sizeof(eiter->a.serial_num));
+	else {
+		sn = ((ha->serial0 & 0x1f) << 16) |
+			(ha->serial2 << 8) | ha->serial1;
+		snprintf(eiter->a.serial_num, sizeof(eiter->a.serial_num),
+		    "%c%05d", 'A' + sn / 100000, sn % 100000);
+	}
 	alen = strlen(eiter->a.serial_num);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1323,11 +1334,12 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "Serial no. = %s.\n", eiter->a.serial_num);
 
 	/* Model name. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_MODEL);
-	strcpy(eiter->a.model, ha->model_number);
+	snprintf(eiter->a.model, sizeof(eiter->a.model),
+	    "%s", ha->model_number);
 	alen = strlen(eiter->a.model);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1335,11 +1347,12 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "Model Name = %s.\n", eiter->a.model);
 
 	/* Model description. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_MODEL_DESCRIPTION);
-	strncpy(eiter->a.model_desc, ha->model_desc, 80);
+	snprintf(eiter->a.model_desc, sizeof(eiter->a.model_desc),
+	    "%s", ha->model_desc);
 	alen = strlen(eiter->a.model_desc);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1347,11 +1360,23 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "Model Desc = %s.\n", eiter->a.model_desc);
 
 	/* Hardware version. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_HARDWARE_VERSION);
-	strcpy(eiter->a.hw_version, ha->adapter_id);
+	if (!IS_FWI2_CAPABLE(ha)) {
+		snprintf(eiter->a.hw_version, sizeof(eiter->a.hw_version),
+		    "HW:%s", ha->adapter_id);
+	} else if (qla2xxx_get_vpd_field(vha, "MN", eiter->a.hw_version,
+		    sizeof(eiter->a.hw_version))) {
+		;
+	} else if (qla2xxx_get_vpd_field(vha, "EC", eiter->a.hw_version,
+		    sizeof(eiter->a.hw_version))) {
+		;
+	} else {
+		snprintf(eiter->a.hw_version, sizeof(eiter->a.hw_version),
+		    "HW:%s", ha->adapter_id);
+	}
 	alen = strlen(eiter->a.hw_version);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1359,11 +1384,12 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "Hardware ver = %s.\n", eiter->a.hw_version);
 
 	/* Driver version. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_DRIVER_VERSION);
-	strcpy(eiter->a.driver_version, qla2x00_version_str);
+	snprintf(eiter->a.driver_version, sizeof(eiter->a.driver_version),
+	    "%s", qla2x00_version_str);
 	alen = strlen(eiter->a.driver_version);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1371,11 +1397,12 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "Driver ver = %s.\n", eiter->a.driver_version);
 
 	/* Option ROM version. */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_OPTION_ROM_VERSION);
-	strcpy(eiter->a.orom_version, "0.00");
+	snprintf(eiter->a.orom_version, sizeof(eiter->a.orom_version),
+	    "%d.%02d", ha->bios_revision[1], ha->bios_revision[0]);
 	alen = strlen(eiter->a.orom_version);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1383,11 +1410,12 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 	    "Optrom vers = %s.\n", eiter->a.orom_version);
 
 	/* Firmware version */
-	eiter = (struct ct_fdmi_hba_attr *) (entries + size);
+	eiter = entries + size;
 	eiter->type = __constant_cpu_to_be16(FDMI_HBA_FIRMWARE_VERSION);
-	ha->isp_ops->fw_version_str(vha, eiter->a.fw_version);
+	ha->isp_ops->fw_version_str(vha, eiter->a.fw_version,
+	    sizeof(eiter->a.fw_version));
 	alen = strlen(eiter->a.fw_version);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
@@ -1419,6 +1447,11 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 			ql_dbg(ql_dbg_disc, vha, 0x2034,
 			    "HBA already registered.\n");
 			rval = QLA_ALREADY_REGISTERED;
+		} else {
+			ql_dbg(ql_dbg_disc, vha, 0x20ad,
+			    "RHBA FDMI registration failed, CT Reason code: 0x%x, CT Explanation 0x%x\n",
+			    ct_rsp->header.reason_code,
+			    ct_rsp->header.explanation_code);
 		}
 	} else {
 		ql_dbg(ql_dbg_disc, vha, 0x2035,
@@ -1429,6 +1462,534 @@ qla2x00_fdmi_rhba(scsi_qla_host_t *vha)
 }
 
 /**
+ * qla2x00_fdmi_rpa() -
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+static int
+qla2x00_fdmi_rpa(scsi_qla_host_t *vha)
+{
+	int rval, alen;
+	uint32_t size;
+	struct qla_hw_data *ha = vha->hw;
+	ms_iocb_entry_t *ms_pkt;
+	struct ct_sns_req *ct_req;
+	struct ct_sns_rsp *ct_rsp;
+	void *entries;
+	struct ct_fdmi_port_attr *eiter;
+	struct init_cb_24xx *icb24 = (struct init_cb_24xx *)ha->init_cb;
+	struct new_utsname *p_sysid = NULL;
+
+	/* Issue RPA */
+	/* Prepare common MS IOCB */
+	/*   Request size adjusted after CT preparation */
+	ms_pkt = ha->isp_ops->prep_ms_fdmi_iocb(vha, 0, RPA_RSP_SIZE);
+
+	/* Prepare CT request */
+	ct_req = qla2x00_prep_ct_fdmi_req(ha->ct_sns, RPA_CMD,
+	    RPA_RSP_SIZE);
+	ct_rsp = &ha->ct_sns->p.rsp;
+
+	/* Prepare FDMI command arguments -- attribute block, attributes. */
+	memcpy(ct_req->req.rpa.port_name, vha->port_name, WWN_SIZE);
+	size = WWN_SIZE + 4;
+
+	/* Attributes */
+	ct_req->req.rpa.attrs.count = cpu_to_be32(FDMI_PORT_ATTR_COUNT);
+	entries = ct_req->req.rpa.port_name;
+
+	/* FC4 types. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_FC4_TYPES);
+	eiter->len = cpu_to_be16(4 + 32);
+	eiter->a.fc4_types[2] = 0x01;
+	size += 4 + 32;
+
+	ql_dbg(ql_dbg_disc, vha, 0x2039,
+	    "FC4_TYPES=%02x %02x.\n",
+	    eiter->a.fc4_types[2],
+	    eiter->a.fc4_types[1]);
+
+	/* Supported speed. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_SUPPORT_SPEED);
+	eiter->len = cpu_to_be16(4 + 4);
+	if (IS_CNA_CAPABLE(ha))
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_10GB);
+	else if (IS_QLA27XX(ha))
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_32GB|
+		    FDMI_PORT_SPEED_16GB|
+		    FDMI_PORT_SPEED_8GB);
+	else if (IS_QLA2031(ha))
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_16GB|
+		    FDMI_PORT_SPEED_8GB|
+		    FDMI_PORT_SPEED_4GB);
+	else if (IS_QLA25XX(ha))
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_8GB|
+		    FDMI_PORT_SPEED_4GB|
+		    FDMI_PORT_SPEED_2GB|
+		    FDMI_PORT_SPEED_1GB);
+	else if (IS_QLA24XX_TYPE(ha))
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_4GB|
+		    FDMI_PORT_SPEED_2GB|
+		    FDMI_PORT_SPEED_1GB);
+	else if (IS_QLA23XX(ha))
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_2GB|
+		    FDMI_PORT_SPEED_1GB);
+	else
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_1GB);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x203a,
+	    "Supported_Speed=%x.\n", eiter->a.sup_speed);
+
+	/* Current speed. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_CURRENT_SPEED);
+	eiter->len = cpu_to_be16(4 + 4);
+	switch (ha->link_data_rate) {
+	case PORT_SPEED_1GB:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_1GB);
+		break;
+	case PORT_SPEED_2GB:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_2GB);
+		break;
+	case PORT_SPEED_4GB:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_4GB);
+		break;
+	case PORT_SPEED_8GB:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_8GB);
+		break;
+	case PORT_SPEED_10GB:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_10GB);
+		break;
+	case PORT_SPEED_16GB:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_16GB);
+		break;
+	case PORT_SPEED_32GB:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_32GB);
+		break;
+	default:
+		eiter->a.cur_speed =
+		    cpu_to_be32(FDMI_PORT_SPEED_UNKNOWN);
+		break;
+	}
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x203b,
+	    "Current_Speed=%x.\n", eiter->a.cur_speed);
+
+	/* Max frame size. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_MAX_FRAME_SIZE);
+	eiter->len = cpu_to_be16(4 + 4);
+	eiter->a.max_frame_size = IS_FWI2_CAPABLE(ha) ?
+	    le16_to_cpu(icb24->frame_payload_size) :
+	    le16_to_cpu(ha->init_cb->frame_payload_size);
+	eiter->a.max_frame_size = cpu_to_be32(eiter->a.max_frame_size);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x203c,
+	    "Max_Frame_Size=%x.\n", eiter->a.max_frame_size);
+
+	/* OS device name. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_OS_DEVICE_NAME);
+	snprintf(eiter->a.os_dev_name, sizeof(eiter->a.os_dev_name),
+	    "%s:host%lu", QLA2XXX_DRIVER_NAME, vha->host_no);
+	alen = strlen(eiter->a.os_dev_name);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x204b,
+	    "OS_Device_Name=%s.\n", eiter->a.os_dev_name);
+
+	/* Hostname. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_HOST_NAME);
+	p_sysid = utsname();
+	if (p_sysid) {
+		snprintf(eiter->a.host_name, sizeof(eiter->a.host_name),
+		    "%s", p_sysid->nodename);
+	} else {
+		snprintf(eiter->a.host_name, sizeof(eiter->a.host_name),
+		    "%s", fc_host_system_hostname(vha->host));
+	}
+	alen = strlen(eiter->a.host_name);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x203d, "HostName=%s.\n", eiter->a.host_name);
+
+	/* Update MS request size. */
+	qla2x00_update_ms_fdmi_iocb(vha, size + 16);
+
+	ql_dbg(ql_dbg_disc, vha, 0x203e,
+	    "RPA portname  %016llx, size = %d.\n",
+	    wwn_to_u64(ct_req->req.rpa.port_name), size);
+	ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha, 0x2079,
+	    entries, size);
+
+	/* Execute MS IOCB */
+	rval = qla2x00_issue_iocb(vha, ha->ms_iocb, ha->ms_iocb_dma,
+	    sizeof(ms_iocb_entry_t));
+	if (rval != QLA_SUCCESS) {
+		/*EMPTY*/
+		ql_dbg(ql_dbg_disc, vha, 0x2040,
+		    "RPA issue IOCB failed (%d).\n", rval);
+	} else if (qla2x00_chk_ms_status(vha, ms_pkt, ct_rsp, "RPA") !=
+	    QLA_SUCCESS) {
+		rval = QLA_FUNCTION_FAILED;
+		if (ct_rsp->header.reason_code == CT_REASON_CANNOT_PERFORM &&
+		    ct_rsp->header.explanation_code ==
+		    CT_EXPL_ALREADY_REGISTERED) {
+			ql_dbg(ql_dbg_disc, vha, 0x20cd,
+			    "RPA already registered.\n");
+			rval = QLA_ALREADY_REGISTERED;
+		}
+
+	} else {
+		ql_dbg(ql_dbg_disc, vha, 0x2041,
+		    "RPA exiting normally.\n");
+	}
+
+	return rval;
+}
+
+/**
+ * qla2x00_fdmiv2_rhba() -
+ * @ha: HA context
+ *
+ * Returns 0 on success.
+ */
+static int
+qla2x00_fdmiv2_rhba(scsi_qla_host_t *vha)
+{
+	int rval, alen;
+	uint32_t size, sn;
+	ms_iocb_entry_t *ms_pkt;
+	struct ct_sns_req *ct_req;
+	struct ct_sns_rsp *ct_rsp;
+	void *entries;
+	struct ct_fdmiv2_hba_attr *eiter;
+	struct qla_hw_data *ha = vha->hw;
+	struct init_cb_24xx *icb24 = (struct init_cb_24xx *)ha->init_cb;
+	struct new_utsname *p_sysid = NULL;
+
+	/* Issue RHBA */
+	/* Prepare common MS IOCB */
+	/*   Request size adjusted after CT preparation */
+	ms_pkt = ha->isp_ops->prep_ms_fdmi_iocb(vha, 0, RHBA_RSP_SIZE);
+
+	/* Prepare CT request */
+	ct_req = qla2x00_prep_ct_fdmi_req(ha->ct_sns, RHBA_CMD,
+	    RHBA_RSP_SIZE);
+	ct_rsp = &ha->ct_sns->p.rsp;
+
+	/* Prepare FDMI command arguments -- attribute block, attributes. */
+	memcpy(ct_req->req.rhba2.hba_identifier, vha->port_name, WWN_SIZE);
+	ct_req->req.rhba2.entry_count = cpu_to_be32(1);
+	memcpy(ct_req->req.rhba2.port_name, vha->port_name, WWN_SIZE);
+	size = 2 * WWN_SIZE + 4 + 4;
+
+	/* Attributes */
+	ct_req->req.rhba2.attrs.count = cpu_to_be32(FDMIV2_HBA_ATTR_COUNT);
+	entries = ct_req->req.rhba2.hba_identifier;
+
+	/* Nodename. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_NODE_NAME);
+	eiter->len = cpu_to_be16(4 + WWN_SIZE);
+	memcpy(eiter->a.node_name, vha->node_name, WWN_SIZE);
+	size += 4 + WWN_SIZE;
+
+	ql_dbg(ql_dbg_disc, vha, 0x207d,
+	    "NodeName = %016llx.\n", wwn_to_u64(eiter->a.node_name));
+
+	/* Manufacturer. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_MANUFACTURER);
+	snprintf(eiter->a.manufacturer, sizeof(eiter->a.manufacturer),
+	    "%s", "QLogic Corporation");
+	eiter->a.manufacturer[strlen("QLogic Corporation")] = '\0';
+	alen = strlen(eiter->a.manufacturer);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20a5,
+	    "Manufacturer = %s.\n", eiter->a.manufacturer);
+
+	/* Serial number. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_SERIAL_NUMBER);
+	if (IS_FWI2_CAPABLE(ha))
+		qla2xxx_get_vpd_field(vha, "SN", eiter->a.serial_num,
+		    sizeof(eiter->a.serial_num));
+	else {
+		sn = ((ha->serial0 & 0x1f) << 16) |
+			(ha->serial2 << 8) | ha->serial1;
+		snprintf(eiter->a.serial_num, sizeof(eiter->a.serial_num),
+		    "%c%05d", 'A' + sn / 100000, sn % 100000);
+	}
+	alen = strlen(eiter->a.serial_num);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20a6,
+	    "Serial no. = %s.\n", eiter->a.serial_num);
+
+	/* Model name. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_MODEL);
+	snprintf(eiter->a.model, sizeof(eiter->a.model),
+	    "%s", ha->model_number);
+	alen = strlen(eiter->a.model);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20a7,
+	    "Model Name = %s.\n", eiter->a.model);
+
+	/* Model description. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_MODEL_DESCRIPTION);
+	snprintf(eiter->a.model_desc, sizeof(eiter->a.model_desc),
+	    "%s", ha->model_desc);
+	alen = strlen(eiter->a.model_desc);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20a8,
+	    "Model Desc = %s.\n", eiter->a.model_desc);
+
+	/* Hardware version. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_HARDWARE_VERSION);
+	if (!IS_FWI2_CAPABLE(ha)) {
+		snprintf(eiter->a.hw_version, sizeof(eiter->a.hw_version),
+		    "HW:%s", ha->adapter_id);
+	} else if (qla2xxx_get_vpd_field(vha, "MN", eiter->a.hw_version,
+		    sizeof(eiter->a.hw_version))) {
+		;
+	} else if (qla2xxx_get_vpd_field(vha, "EC", eiter->a.hw_version,
+		    sizeof(eiter->a.hw_version))) {
+		;
+	} else {
+		snprintf(eiter->a.hw_version, sizeof(eiter->a.hw_version),
+		    "HW:%s", ha->adapter_id);
+	}
+	alen = strlen(eiter->a.hw_version);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20a9,
+	    "Hardware ver = %s.\n", eiter->a.hw_version);
+
+	/* Driver version. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_DRIVER_VERSION);
+	snprintf(eiter->a.driver_version, sizeof(eiter->a.driver_version),
+	    "%s", qla2x00_version_str);
+	alen = strlen(eiter->a.driver_version);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20aa,
+	    "Driver ver = %s.\n", eiter->a.driver_version);
+
+	/* Option ROM version. */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_OPTION_ROM_VERSION);
+	snprintf(eiter->a.orom_version, sizeof(eiter->a.orom_version),
+	    "%d.%02d", ha->bios_revision[1], ha->bios_revision[0]);
+	alen = strlen(eiter->a.orom_version);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha , 0x20ab,
+	    "Optrom version = %d.%02d.\n", eiter->a.orom_version[1],
+	    eiter->a.orom_version[0]);
+
+	/* Firmware version */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_FIRMWARE_VERSION);
+	ha->isp_ops->fw_version_str(vha, eiter->a.fw_version,
+	    sizeof(eiter->a.fw_version));
+	alen = strlen(eiter->a.fw_version);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20ac,
+	    "Firmware vers = %s.\n", eiter->a.fw_version);
+
+	/* OS Name and Version */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_OS_NAME_AND_VERSION);
+	p_sysid = utsname();
+	if (p_sysid) {
+		snprintf(eiter->a.os_version, sizeof(eiter->a.os_version),
+		    "%s %s %s",
+		    p_sysid->sysname, p_sysid->release, p_sysid->version);
+	} else {
+		snprintf(eiter->a.os_version, sizeof(eiter->a.os_version),
+		    "%s %s", "Linux", fc_host_system_hostname(vha->host));
+	}
+	alen = strlen(eiter->a.os_version);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20ae,
+	    "OS Name and Version = %s.\n", eiter->a.os_version);
+
+	/* MAX CT Payload Length */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_MAXIMUM_CT_PAYLOAD_LENGTH);
+	eiter->a.max_ct_len = IS_FWI2_CAPABLE(ha) ?
+	    le16_to_cpu(icb24->frame_payload_size) :
+	    le16_to_cpu(ha->init_cb->frame_payload_size);
+	eiter->a.max_ct_len = cpu_to_be32(eiter->a.max_ct_len);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20af,
+	    "CT Payload Length = 0x%x.\n", eiter->a.max_ct_len);
+
+	/* Node Sybolic Name */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_NODE_SYMBOLIC_NAME);
+	qla2x00_get_sym_node_name(vha, eiter->a.sym_name,
+	    sizeof(eiter->a.sym_name));
+	alen = strlen(eiter->a.sym_name);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20b0,
+	    "Symbolic Name = %s.\n", eiter->a.sym_name);
+
+	/* Vendor Id */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_VENDOR_ID);
+	eiter->a.vendor_id = cpu_to_be32(0x1077);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20b1,
+	    "Vendor Id = %x.\n", eiter->a.vendor_id);
+
+	/* Num Ports */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_NUM_PORTS);
+	eiter->a.num_ports = cpu_to_be32(1);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20b2,
+	    "Port Num = %x.\n", eiter->a.num_ports);
+
+	/* Fabric Name */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_FABRIC_NAME);
+	memcpy(eiter->a.fabric_name, vha->fabric_node_name, WWN_SIZE);
+	eiter->len = cpu_to_be16(4 + WWN_SIZE);
+	size += 4 + WWN_SIZE;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20b3,
+	    "Fabric Name = %016llx.\n", wwn_to_u64(eiter->a.fabric_name));
+
+	/* BIOS Version */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_BOOT_BIOS_NAME);
+	snprintf(eiter->a.bios_name, sizeof(eiter->a.bios_name),
+	    "BIOS %d.%02d", ha->bios_revision[1], ha->bios_revision[0]);
+	alen = strlen(eiter->a.bios_name);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20b4,
+	    "BIOS Name = %s\n", eiter->a.bios_name);
+
+	/* Vendor Identifier */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_HBA_TYPE_VENDOR_IDENTIFIER);
+	snprintf(eiter->a.vendor_indentifer, sizeof(eiter->a.vendor_indentifer),
+	    "%s", "QLGC");
+	alen = strlen(eiter->a.vendor_indentifer);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20b1,
+	    "Vendor Identifier = %s.\n", eiter->a.vendor_indentifer);
+
+	/* Update MS request size. */
+	qla2x00_update_ms_fdmi_iocb(vha, size + 16);
+
+	ql_dbg(ql_dbg_disc, vha, 0x20b5,
+	    "RHBA identifier = %016llx.\n",
+	    wwn_to_u64(ct_req->req.rhba2.hba_identifier));
+	ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha, 0x20b6,
+	    entries, size);
+
+	/* Execute MS IOCB */
+	rval = qla2x00_issue_iocb(vha, ha->ms_iocb, ha->ms_iocb_dma,
+	    sizeof(ms_iocb_entry_t));
+	if (rval != QLA_SUCCESS) {
+		/*EMPTY*/
+		ql_dbg(ql_dbg_disc, vha, 0x20b7,
+		    "RHBA issue IOCB failed (%d).\n", rval);
+	} else if (qla2x00_chk_ms_status(vha, ms_pkt, ct_rsp, "RHBA") !=
+	    QLA_SUCCESS) {
+		rval = QLA_FUNCTION_FAILED;
+
+		if (ct_rsp->header.reason_code == CT_REASON_CANNOT_PERFORM &&
+		    ct_rsp->header.explanation_code ==
+		    CT_EXPL_ALREADY_REGISTERED) {
+			ql_dbg(ql_dbg_disc, vha, 0x20b8,
+			    "HBA already registered.\n");
+			rval = QLA_ALREADY_REGISTERED;
+		} else {
+			ql_dbg(ql_dbg_disc, vha, 0x2016,
+			    "RHBA FDMI v2 failed, CT Reason code: 0x%x, CT Explanation 0x%x\n",
+			    ct_rsp->header.reason_code,
+			    ct_rsp->header.explanation_code);
+		}
+	} else {
+		ql_dbg(ql_dbg_disc, vha, 0x20b9,
+		    "RHBA FDMI V2 exiting normally.\n");
+	}
+
+	return rval;
+}
+
+/**
  * qla2x00_fdmi_dhba() -
  * @ha: HA context
  *
@@ -1477,23 +2038,24 @@ qla2x00_fdmi_dhba(scsi_qla_host_t *vha)
 }
 
 /**
- * qla2x00_fdmi_rpa() -
+ * qla2x00_fdmiv2_rpa() -
  * @ha: HA context
  *
  * Returns 0 on success.
  */
 static int
-qla2x00_fdmi_rpa(scsi_qla_host_t *vha)
+qla2x00_fdmiv2_rpa(scsi_qla_host_t *vha)
 {
 	int rval, alen;
-	uint32_t size, max_frame_size;
+	uint32_t size;
 	struct qla_hw_data *ha = vha->hw;
 	ms_iocb_entry_t *ms_pkt;
 	struct ct_sns_req *ct_req;
 	struct ct_sns_rsp *ct_rsp;
-	uint8_t *entries;
-	struct ct_fdmi_port_attr *eiter;
+	void *entries;
+	struct ct_fdmiv2_port_attr *eiter;
 	struct init_cb_24xx *icb24 = (struct init_cb_24xx *)ha->init_cb;
+	struct new_utsname *p_sysid = NULL;
 
 	/* Issue RPA */
 	/* Prepare common MS IOCB */
@@ -1505,147 +2067,258 @@ qla2x00_fdmi_rpa(scsi_qla_host_t *vha)
 	ct_rsp = &ha->ct_sns->p.rsp;
 
 	/* Prepare FDMI command arguments -- attribute block, attributes. */
-	memcpy(ct_req->req.rpa.port_name, vha->port_name, WWN_SIZE);
+	memcpy(ct_req->req.rpa2.port_name, vha->port_name, WWN_SIZE);
 	size = WWN_SIZE + 4;
 
 	/* Attributes */
-	ct_req->req.rpa.attrs.count =
-	    __constant_cpu_to_be32(FDMI_PORT_ATTR_COUNT - 1);
-	entries = ct_req->req.rpa.port_name;
+	ct_req->req.rpa2.attrs.count = cpu_to_be32(FDMIV2_PORT_ATTR_COUNT);
+	entries = ct_req->req.rpa2.port_name;
 
 	/* FC4 types. */
-	eiter = (struct ct_fdmi_port_attr *) (entries + size);
-	eiter->type = __constant_cpu_to_be16(FDMI_PORT_FC4_TYPES);
-	eiter->len = __constant_cpu_to_be16(4 + 32);
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_FC4_TYPES);
+	eiter->len = cpu_to_be16(4 + 32);
 	eiter->a.fc4_types[2] = 0x01;
 	size += 4 + 32;
 
-	ql_dbg(ql_dbg_disc, vha, 0x2039,
+	ql_dbg(ql_dbg_disc, vha, 0x20ba,
 	    "FC4_TYPES=%02x %02x.\n",
 	    eiter->a.fc4_types[2],
 	    eiter->a.fc4_types[1]);
 
 	/* Supported speed. */
-	eiter = (struct ct_fdmi_port_attr *) (entries + size);
-	eiter->type = __constant_cpu_to_be16(FDMI_PORT_SUPPORT_SPEED);
-	eiter->len = __constant_cpu_to_be16(4 + 4);
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_SUPPORT_SPEED);
+	eiter->len = cpu_to_be16(4 + 4);
 	if (IS_CNA_CAPABLE(ha))
-		eiter->a.sup_speed = __constant_cpu_to_be32(
+		eiter->a.sup_speed = cpu_to_be32(
 		    FDMI_PORT_SPEED_10GB);
 	else if (IS_QLA27XX(ha))
-		eiter->a.sup_speed = __constant_cpu_to_be32(
-		    FDMI_PORT_SPEED_32GB|FDMI_PORT_SPEED_16GB|
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_32GB|
+		    FDMI_PORT_SPEED_16GB|
 		    FDMI_PORT_SPEED_8GB);
+	else if (IS_QLA2031(ha))
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_16GB|
+		    FDMI_PORT_SPEED_8GB|
+		    FDMI_PORT_SPEED_4GB);
 	else if (IS_QLA25XX(ha))
-		eiter->a.sup_speed = __constant_cpu_to_be32(
-		    FDMI_PORT_SPEED_1GB|FDMI_PORT_SPEED_2GB|
-		    FDMI_PORT_SPEED_4GB|FDMI_PORT_SPEED_8GB);
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_8GB|
+		    FDMI_PORT_SPEED_4GB|
+		    FDMI_PORT_SPEED_2GB|
+		    FDMI_PORT_SPEED_1GB);
 	else if (IS_QLA24XX_TYPE(ha))
-		eiter->a.sup_speed = __constant_cpu_to_be32(
-		    FDMI_PORT_SPEED_1GB|FDMI_PORT_SPEED_2GB|
-		    FDMI_PORT_SPEED_4GB);
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_4GB|
+		    FDMI_PORT_SPEED_2GB|
+		    FDMI_PORT_SPEED_1GB);
 	else if (IS_QLA23XX(ha))
-		eiter->a.sup_speed =__constant_cpu_to_be32(
-		    FDMI_PORT_SPEED_1GB|FDMI_PORT_SPEED_2GB);
+		eiter->a.sup_speed = cpu_to_be32(
+		    FDMI_PORT_SPEED_2GB|
+		    FDMI_PORT_SPEED_1GB);
 	else
-		eiter->a.sup_speed = __constant_cpu_to_be32(
+		eiter->a.sup_speed = cpu_to_be32(
 		    FDMI_PORT_SPEED_1GB);
 	size += 4 + 4;
 
-	ql_dbg(ql_dbg_disc, vha, 0x203a,
-	    "Supported_Speed=%x.\n", eiter->a.sup_speed);
+	ql_dbg(ql_dbg_disc, vha, 0x20bb,
+	    "Supported Port Speed = %x.\n", eiter->a.sup_speed);
 
 	/* Current speed. */
-	eiter = (struct ct_fdmi_port_attr *) (entries + size);
-	eiter->type = __constant_cpu_to_be16(FDMI_PORT_CURRENT_SPEED);
-	eiter->len = __constant_cpu_to_be16(4 + 4);
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_CURRENT_SPEED);
+	eiter->len = cpu_to_be16(4 + 4);
 	switch (ha->link_data_rate) {
 	case PORT_SPEED_1GB:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_1GB);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_1GB);
 		break;
 	case PORT_SPEED_2GB:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_2GB);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_2GB);
 		break;
 	case PORT_SPEED_4GB:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_4GB);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_4GB);
 		break;
 	case PORT_SPEED_8GB:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_8GB);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_8GB);
 		break;
 	case PORT_SPEED_10GB:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_10GB);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_10GB);
 		break;
 	case PORT_SPEED_16GB:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_16GB);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_16GB);
 		break;
 	case PORT_SPEED_32GB:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_32GB);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_32GB);
 		break;
 	default:
-		eiter->a.cur_speed =
-		    __constant_cpu_to_be32(FDMI_PORT_SPEED_UNKNOWN);
+		eiter->a.cur_speed = cpu_to_be32(FDMI_PORT_SPEED_UNKNOWN);
 		break;
 	}
 	size += 4 + 4;
 
-	ql_dbg(ql_dbg_disc, vha, 0x203b,
-	    "Current_Speed=%x.\n", eiter->a.cur_speed);
+	ql_dbg(ql_dbg_disc, vha, 0x20bc,
+	    "Current_Speed = %x.\n", eiter->a.cur_speed);
 
 	/* Max frame size. */
-	eiter = (struct ct_fdmi_port_attr *) (entries + size);
-	eiter->type = __constant_cpu_to_be16(FDMI_PORT_MAX_FRAME_SIZE);
-	eiter->len = __constant_cpu_to_be16(4 + 4);
-	max_frame_size = IS_FWI2_CAPABLE(ha) ?
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_MAX_FRAME_SIZE);
+	eiter->len = cpu_to_be16(4 + 4);
+	eiter->a.max_frame_size = IS_FWI2_CAPABLE(ha) ?
 	    le16_to_cpu(icb24->frame_payload_size):
 	    le16_to_cpu(ha->init_cb->frame_payload_size);
-	eiter->a.max_frame_size = cpu_to_be32(max_frame_size);
+	eiter->a.max_frame_size = cpu_to_be32(eiter->a.max_frame_size);
 	size += 4 + 4;
 
-	ql_dbg(ql_dbg_disc, vha, 0x203c,
-	    "Max_Frame_Size=%x.\n", eiter->a.max_frame_size);
+	ql_dbg(ql_dbg_disc, vha, 0x20bc,
+	    "Max_Frame_Size = %x.\n", eiter->a.max_frame_size);
 
 	/* OS device name. */
-	eiter = (struct ct_fdmi_port_attr *) (entries + size);
-	eiter->type = __constant_cpu_to_be16(FDMI_PORT_OS_DEVICE_NAME);
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_OS_DEVICE_NAME);
 	alen = strlen(QLA2XXX_DRIVER_NAME);
-	strncpy(eiter->a.os_dev_name, QLA2XXX_DRIVER_NAME, alen + 1);
-	alen += (alen & 3) ? (4 - (alen & 3)) : 4;
+	snprintf(eiter->a.os_dev_name, sizeof(eiter->a.os_dev_name),
+	    "%s:host%lu", QLA2XXX_DRIVER_NAME, vha->host_no);
+	alen += 4 - (alen & 3);
 	eiter->len = cpu_to_be16(4 + alen);
 	size += 4 + alen;
 
-	ql_dbg(ql_dbg_disc, vha, 0x204b,
-	    "OS_Device_Name=%s.\n", eiter->a.os_dev_name);
+	ql_dbg(ql_dbg_disc, vha, 0x20be,
+	    "OS_Device_Name = %s.\n", eiter->a.os_dev_name);
 
 	/* Hostname. */
-	if (strlen(fc_host_system_hostname(vha->host))) {
-		ct_req->req.rpa.attrs.count =
-		    __constant_cpu_to_be32(FDMI_PORT_ATTR_COUNT);
-		eiter = (struct ct_fdmi_port_attr *) (entries + size);
-		eiter->type = __constant_cpu_to_be16(FDMI_PORT_HOST_NAME);
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_HOST_NAME);
+	p_sysid = utsname();
+	if (p_sysid) {
+		snprintf(eiter->a.host_name, sizeof(eiter->a.host_name),
+		    "%s", p_sysid->nodename);
+	} else {
 		snprintf(eiter->a.host_name, sizeof(eiter->a.host_name),
 		    "%s", fc_host_system_hostname(vha->host));
-		alen = strlen(eiter->a.host_name);
-		alen += (alen & 3) ? (4 - (alen & 3)) : 4;
-		eiter->len = cpu_to_be16(4 + alen);
-		size += 4 + alen;
-
-		ql_dbg(ql_dbg_disc, vha, 0x203d,
-		    "HostName=%s.\n", eiter->a.host_name);
 	}
+	alen = strlen(eiter->a.host_name);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x203d,
+	    "HostName=%s.\n", eiter->a.host_name);
+
+	/* Node Name */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_NODE_NAME);
+	memcpy(eiter->a.node_name, vha->node_name, WWN_SIZE);
+	eiter->len = cpu_to_be16(4 + WWN_SIZE);
+	size += 4 + WWN_SIZE;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c0,
+	    "Node Name = %016llx.\n", wwn_to_u64(eiter->a.node_name));
+
+	/* Port Name */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_NAME);
+	memcpy(eiter->a.port_name, vha->port_name, WWN_SIZE);
+	eiter->len = cpu_to_be16(4 + WWN_SIZE);
+	size += 4 + WWN_SIZE;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c1,
+	    "Port Name = %016llx.\n", wwn_to_u64(eiter->a.port_name));
+
+	/* Port Symbolic Name */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_SYM_NAME);
+	qla2x00_get_sym_node_name(vha, eiter->a.port_sym_name,
+	    sizeof(eiter->a.port_sym_name));
+	alen = strlen(eiter->a.port_sym_name);
+	alen += 4 - (alen & 3);
+	eiter->len = cpu_to_be16(4 + alen);
+	size += 4 + alen;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c2,
+	    "port symbolic name = %s\n", eiter->a.port_sym_name);
+
+	/* Port Type */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_TYPE);
+	eiter->a.port_type = cpu_to_be32(NS_NX_PORT_TYPE);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c3,
+	    "Port Type = %x.\n", eiter->a.port_type);
+
+	/* Class of Service  */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_SUPP_COS);
+	eiter->a.port_supported_cos = cpu_to_be32(FC_CLASS_3);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c4,
+	    "Supported COS = %08x\n", eiter->a.port_supported_cos);
+
+	/* Port Fabric Name */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_FABRIC_NAME);
+	memcpy(eiter->a.fabric_name, vha->fabric_node_name, WWN_SIZE);
+	eiter->len = cpu_to_be16(4 + WWN_SIZE);
+	size += 4 + WWN_SIZE;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c5,
+	    "Fabric Name = %016llx.\n", wwn_to_u64(eiter->a.fabric_name));
+
+	/* FC4_type */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_FC4_TYPE);
+	eiter->a.port_fc4_type[0] = 0;
+	eiter->a.port_fc4_type[1] = 0;
+	eiter->a.port_fc4_type[2] = 1;
+	eiter->a.port_fc4_type[3] = 0;
+	eiter->len = cpu_to_be16(4 + 32);
+	size += 4 + 32;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c6,
+	    "Port Active FC4 Type = %02x %02x.\n",
+	    eiter->a.port_fc4_type[2], eiter->a.port_fc4_type[1]);
+
+	/* Port State */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_STATE);
+	eiter->a.port_state = cpu_to_be32(1);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c7,
+	    "Port State = %x.\n", eiter->a.port_state);
+
+	/* Number of Ports */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_COUNT);
+	eiter->a.num_ports = cpu_to_be32(1);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c8,
+	    "Number of ports = %x.\n", eiter->a.num_ports);
+
+	/* Port Id */
+	eiter = entries + size;
+	eiter->type = cpu_to_be16(FDMI_PORT_ID);
+	eiter->a.port_id = cpu_to_be32(vha->d_id.b24);
+	eiter->len = cpu_to_be16(4 + 4);
+	size += 4 + 4;
+
+	ql_dbg(ql_dbg_disc, vha, 0x20c8,
+	    "Port Id = %x.\n", eiter->a.port_id);
 
 	/* Update MS request size. */
 	qla2x00_update_ms_fdmi_iocb(vha, size + 16);
 
 	ql_dbg(ql_dbg_disc, vha, 0x203e,
 	    "RPA portname= %8phN size=%d.\n", ct_req->req.rpa.port_name, size);
-	ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha, 0x2079,
+	ql_dump_buffer(ql_dbg_disc + ql_dbg_buffer, vha, 0x20ca,
 	    entries, size);
 
 	/* Execute MS IOCB */
@@ -1653,14 +2326,26 @@ qla2x00_fdmi_rpa(scsi_qla_host_t *vha)
 	    sizeof(ms_iocb_entry_t));
 	if (rval != QLA_SUCCESS) {
 		/*EMPTY*/
-		ql_dbg(ql_dbg_disc, vha, 0x2040,
-		    "RPA issue IOCB failed (%d).\n", rval);
+		ql_dbg(ql_dbg_disc, vha, 0x20cb,
+		    "RPA FDMI v2 issue IOCB failed (%d).\n", rval);
 	} else if (qla2x00_chk_ms_status(vha, ms_pkt, ct_rsp, "RPA") !=
 	    QLA_SUCCESS) {
 		rval = QLA_FUNCTION_FAILED;
+		if (ct_rsp->header.reason_code == CT_REASON_CANNOT_PERFORM &&
+		    ct_rsp->header.explanation_code ==
+		    CT_EXPL_ALREADY_REGISTERED) {
+			ql_dbg(ql_dbg_disc, vha, 0x20ce,
+			    "RPA FDMI v2 already registered\n");
+			rval = QLA_ALREADY_REGISTERED;
+		} else {
+			ql_dbg(ql_dbg_disc, vha, 0x2020,
+			    "RPA FDMI v2 failed, CT Reason code: 0x%x, CT Explanation 0x%x\n",
+			    ct_rsp->header.reason_code,
+			    ct_rsp->header.explanation_code);
+		}
 	} else {
-		ql_dbg(ql_dbg_disc, vha, 0x2041,
-		    "RPA exiting nornally.\n");
+		ql_dbg(ql_dbg_disc, vha, 0x20cc,
+		    "RPA FDMI V2 exiting normally.\n");
 	}
 
 	return rval;
@@ -1675,8 +2360,8 @@ qla2x00_fdmi_rpa(scsi_qla_host_t *vha)
 int
 qla2x00_fdmi_register(scsi_qla_host_t *vha)
 {
-	int rval;
-       struct qla_hw_data *ha = vha->hw;
+	int rval = QLA_FUNCTION_FAILED;
+	struct qla_hw_data *ha = vha->hw;
 
 	if (IS_QLA2100(ha) || IS_QLA2200(ha) ||
 	    IS_QLAFX00(ha))
@@ -1686,6 +2371,26 @@ qla2x00_fdmi_register(scsi_qla_host_t *vha)
 	if (rval)
 		return rval;
 
+	rval = qla2x00_fdmiv2_rhba(vha);
+	if (rval) {
+		if (rval != QLA_ALREADY_REGISTERED)
+			goto try_fdmi;
+
+		rval = qla2x00_fdmi_dhba(vha);
+		if (rval)
+			goto try_fdmi;
+
+		rval = qla2x00_fdmiv2_rhba(vha);
+		if (rval)
+			goto try_fdmi;
+	}
+	rval = qla2x00_fdmiv2_rpa(vha);
+	if (rval)
+		goto try_fdmi;
+
+	goto out;
+
+try_fdmi:
 	rval = qla2x00_fdmi_rhba(vha);
 	if (rval) {
 		if (rval != QLA_ALREADY_REGISTERED)
@@ -1700,7 +2405,7 @@ qla2x00_fdmi_register(scsi_qla_host_t *vha)
 			return rval;
 	}
 	rval = qla2x00_fdmi_rpa(vha);
-
+out:
 	return rval;
 }
 
diff --git a/drivers/scsi/qla2xxx/qla_init.c b/drivers/scsi/qla2xxx/qla_init.c
index 46990f4ceb40..a4dde7e80dbd 100644
--- a/drivers/scsi/qla2xxx/qla_init.c
+++ b/drivers/scsi/qla2xxx/qla_init.c
@@ -1848,7 +1848,9 @@ enable_82xx_npiv:
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 	}
 
-	if (rval == QLA_SUCCESS && IS_FAC_REQUIRED(ha)) {
+	if (IS_QLA27XX(ha))
+		ha->flags.fac_supported = 1;
+	else if (rval == QLA_SUCCESS && IS_FAC_REQUIRED(ha)) {
 		uint32_t size;
 
 		rval = qla81xx_fac_get_sector_size(vha, &size);
@@ -2196,6 +2198,15 @@ qla2x00_init_rings(scsi_qla_host_t *vha)
 		mid_init_cb->options = __constant_cpu_to_le16(BIT_1);
 		mid_init_cb->init_cb.execution_throttle =
 		    cpu_to_le16(ha->fw_xcb_count);
+		/* D-Port Status */
+		if (IS_DPORT_CAPABLE(ha))
+			mid_init_cb->init_cb.firmware_options_1 |=
+			    cpu_to_le16(BIT_7);
+		/* Enable FA-WWPN */
+		ha->flags.fawwpn_enabled =
+		    (mid_init_cb->init_cb.firmware_options_1 & BIT_6) ? 1 : 0;
+		ql_dbg(ql_dbg_init, vha, 0x0141, "FA-WWPN Support: %s.\n",
+		    (ha->flags.fawwpn_enabled) ? "enabled" : "disabled");
 	}
 
 	rval = qla2x00_init_firmware(vha, ha->init_cb_size);
@@ -2224,7 +2235,7 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 	unsigned long	wtime, mtime, cs84xx_time;
 	uint16_t	min_wait;	/* Minimum wait time if loop is down */
 	uint16_t	wait_time;	/* Wait time if loop is coming ready */
-	uint16_t	state[5];
+	uint16_t	state[6];
 	struct qla_hw_data *ha = vha->hw;
 
 	if (IS_QLAFX00(vha->hw))
@@ -2329,8 +2340,8 @@ qla2x00_fw_ready(scsi_qla_host_t *vha)
 	} while (1);
 
 	ql_dbg(ql_dbg_taskm, vha, 0x803a,
-	    "fw_state=%x (%x, %x, %x, %x) " "curr time=%lx.\n", state[0],
-	    state[1], state[2], state[3], state[4], jiffies);
+	    "fw_state=%x (%x, %x, %x, %x %x) curr time=%lx.\n", state[0],
+	    state[1], state[2], state[3], state[4], state[5], jiffies);
 
 	if (rval && !(vha->device_flags & DFLG_NO_CABLE)) {
 		ql_log(ql_log_warn, vha, 0x803b,
@@ -2596,18 +2607,18 @@ qla2x00_nvram_config(scsi_qla_host_t *vha)
 			nv->firmware_options[1] = BIT_7 | BIT_5;
 			nv->add_firmware_options[0] = BIT_5;
 			nv->add_firmware_options[1] = BIT_5 | BIT_4;
-			nv->frame_payload_size = __constant_cpu_to_le16(2048);
+			nv->frame_payload_size = 2048;
 			nv->special_options[1] = BIT_7;
 		} else if (IS_QLA2200(ha)) {
 			nv->firmware_options[0] = BIT_2 | BIT_1;
 			nv->firmware_options[1] = BIT_7 | BIT_5;
 			nv->add_firmware_options[0] = BIT_5;
 			nv->add_firmware_options[1] = BIT_5 | BIT_4;
-			nv->frame_payload_size = __constant_cpu_to_le16(1024);
+			nv->frame_payload_size = 1024;
 		} else if (IS_QLA2100(ha)) {
 			nv->firmware_options[0] = BIT_3 | BIT_1;
 			nv->firmware_options[1] = BIT_5;
-			nv->frame_payload_size = __constant_cpu_to_le16(1024);
+			nv->frame_payload_size = 1024;
 		}
 
 		nv->max_iocb_allocation = __constant_cpu_to_le16(256);
@@ -2643,7 +2654,7 @@ qla2x00_nvram_config(scsi_qla_host_t *vha)
 	 * are valid.
 	 */
 	if (ia64_platform_is("sn2")) {
-		nv->frame_payload_size = __constant_cpu_to_le16(2048);
+		nv->frame_payload_size = 2048;
 		if (IS_QLA23XX(ha))
 			nv->special_options[1] = BIT_7;
 	}
@@ -3192,7 +3203,7 @@ static void
 qla2x00_iidma_fcport(scsi_qla_host_t *vha, fc_port_t *fcport)
 {
 	int rval;
-	uint16_t mb[4];
+	uint16_t mb[MAILBOX_REGISTER_COUNT];
 	struct qla_hw_data *ha = vha->hw;
 
 	if (!IS_IIDMA_CAPABLE(ha))
@@ -4564,6 +4575,10 @@ qla2x00_abort_isp_cleanup(scsi_qla_host_t *vha)
 		/* Requeue all commands in outstanding command list. */
 		qla2x00_abort_all_cmds(vha, DID_RESET << 16);
 	}
+
+	ha->chip_reset++;
+	/* memory barrier */
+	wmb();
 }
 
 /*
@@ -4958,7 +4973,7 @@ qla24xx_nvram_config(scsi_qla_host_t *vha)
 		memset(nv, 0, ha->nvram_size);
 		nv->nvram_version = __constant_cpu_to_le16(ICB_VERSION);
 		nv->version = __constant_cpu_to_le16(ICB_VERSION);
-		nv->frame_payload_size = __constant_cpu_to_le16(2048);
+		nv->frame_payload_size = 2048;
 		nv->execution_throttle = __constant_cpu_to_le16(0xFFFF);
 		nv->exchange_count = __constant_cpu_to_le16(0);
 		nv->hard_address = __constant_cpu_to_le16(124);
@@ -5225,7 +5240,7 @@ qla24xx_load_risc_flash(scsi_qla_host_t *vha, uint32_t *srisc_addr,
 				ql_log(ql_log_fatal, vha, 0x008f,
 				    "Failed to load segment %d of firmware.\n",
 				    fragment);
-				break;
+				return QLA_FUNCTION_FAILED;
 			}
 
 			faddr += dlen;
@@ -5528,7 +5543,7 @@ qla24xx_load_risc_blob(scsi_qla_host_t *vha, uint32_t *srisc_addr)
 				ql_log(ql_log_fatal, vha, 0x0098,
 				    "Failed to load segment %d of firmware.\n",
 				    fragment);
-				break;
+				return QLA_FUNCTION_FAILED;
 			}
 
 			fwcode += dlen;
@@ -5905,7 +5920,7 @@ qla81xx_nvram_config(scsi_qla_host_t *vha)
 		memset(nv, 0, ha->nvram_size);
 		nv->nvram_version = __constant_cpu_to_le16(ICB_VERSION);
 		nv->version = __constant_cpu_to_le16(ICB_VERSION);
-		nv->frame_payload_size = __constant_cpu_to_le16(2048);
+		nv->frame_payload_size = 2048;
 		nv->execution_throttle = __constant_cpu_to_le16(0xFFFF);
 		nv->exchange_count = __constant_cpu_to_le16(0);
 		nv->port_name[0] = 0x21;
diff --git a/drivers/scsi/qla2xxx/qla_inline.h b/drivers/scsi/qla2xxx/qla_inline.h
index b3b1d6fc2d6c..fee9eb7c8a60 100644
--- a/drivers/scsi/qla2xxx/qla_inline.h
+++ b/drivers/scsi/qla2xxx/qla_inline.h
@@ -279,3 +279,11 @@ qla2x00_handle_mbx_completion(struct qla_hw_data *ha, int status)
 		complete(&ha->mbx_intr_comp);
 	}
 }
+
+static inline void
+qla2x00_set_retry_delay_timestamp(fc_port_t *fcport, uint16_t retry_delay)
+{
+	if (retry_delay)
+		fcport->retry_delay_timestamp = jiffies +
+		    (retry_delay * HZ / 10);
+}
diff --git a/drivers/scsi/qla2xxx/qla_iocb.c b/drivers/scsi/qla2xxx/qla_iocb.c
index 150529d98db4..f0edb07f3198 100644
--- a/drivers/scsi/qla2xxx/qla_iocb.c
+++ b/drivers/scsi/qla2xxx/qla_iocb.c
@@ -1858,6 +1858,17 @@ static void qla25xx_set_que(srb_t *sp, struct rsp_que **rsp)
 }
 
 /* Generic Control-SRB manipulation functions. */
+
+/* hardware_lock assumed to be held. */
+void *
+qla2x00_alloc_iocbs_ready(scsi_qla_host_t *vha, srb_t *sp)
+{
+	if (qla2x00_reset_active(vha))
+		return NULL;
+
+	return qla2x00_alloc_iocbs(vha, sp);
+}
+
 void *
 qla2x00_alloc_iocbs(scsi_qla_host_t *vha, srb_t *sp)
 {
@@ -1901,7 +1912,7 @@ qla2x00_alloc_iocbs(scsi_qla_host_t *vha, srb_t *sp)
 
 skip_cmd_array:
 	/* Check for room on request queue. */
-	if (req->cnt < req_cnt) {
+	if (req->cnt < req_cnt + 2) {
 		if (ha->mqenable || IS_QLA83XX(ha) || IS_QLA27XX(ha))
 			cnt = RD_REG_DWORD(&reg->isp25mq.req_q_out);
 		else if (IS_P3P_TYPE(ha))
@@ -1920,7 +1931,7 @@ skip_cmd_array:
 			req->cnt = req->length -
 			    (req->ring_index - cnt);
 	}
-	if (req->cnt < req_cnt)
+	if (req->cnt < req_cnt + 2)
 		goto queuing_error;
 
 	/* Prep packet */
@@ -2648,7 +2659,7 @@ queuing_error:
 	return QLA_FUNCTION_FAILED;
 }
 
-void
+static void
 qla24xx_abort_iocb(srb_t *sp, struct abort_entry_24xx *abt_iocb)
 {
 	struct srb_iocb *aio = &sp->u.iocb_cmd;
diff --git a/drivers/scsi/qla2xxx/qla_isr.c b/drivers/scsi/qla2xxx/qla_isr.c
index 550a4a31f51a..a04a1b1f7f32 100644
--- a/drivers/scsi/qla2xxx/qla_isr.c
+++ b/drivers/scsi/qla2xxx/qla_isr.c
@@ -56,16 +56,8 @@ qla2100_intr_handler(int irq, void *dev_id)
 	vha = pci_get_drvdata(ha->pdev);
 	for (iter = 50; iter--; ) {
 		hccr = RD_REG_WORD(&reg->hccr);
-		/* Check for PCI disconnection */
-		if (hccr == 0xffff) {
-			/*
-			 * Schedule this on the default system workqueue so that
-			 * all the adapter workqueues and the DPC thread can be
-			 * shutdown cleanly.
-			 */
-			schedule_work(&ha->board_disable);
+		if (qla2x00_check_reg16_for_disconnect(vha, hccr))
 			break;
-		}
 		if (hccr & HCCR_RISC_PAUSE) {
 			if (pci_channel_offline(ha->pdev))
 				break;
@@ -121,21 +113,31 @@ qla2100_intr_handler(int irq, void *dev_id)
 }
 
 bool
-qla2x00_check_reg_for_disconnect(scsi_qla_host_t *vha, uint32_t reg)
+qla2x00_check_reg32_for_disconnect(scsi_qla_host_t *vha, uint32_t reg)
 {
 	/* Check for PCI disconnection */
 	if (reg == 0xffffffff) {
-		/*
-		 * Schedule this on the default system workqueue so that all the
-		 * adapter workqueues and the DPC thread can be shutdown
-		 * cleanly.
-		 */
-		schedule_work(&vha->hw->board_disable);
+		if (!test_and_set_bit(PFLG_DISCONNECTED, &vha->pci_flags) &&
+		    !test_bit(PFLG_DRIVER_REMOVING, &vha->pci_flags) &&
+		    !test_bit(PFLG_DRIVER_PROBING, &vha->pci_flags)) {
+			/*
+			 * Schedule this (only once) on the default system
+			 * workqueue so that all the adapter workqueues and the
+			 * DPC thread can be shutdown cleanly.
+			 */
+			schedule_work(&vha->hw->board_disable);
+		}
 		return true;
 	} else
 		return false;
 }
 
+bool
+qla2x00_check_reg16_for_disconnect(scsi_qla_host_t *vha, uint16_t reg)
+{
+	return qla2x00_check_reg32_for_disconnect(vha, 0xffff0000 | reg);
+}
+
 /**
  * qla2300_intr_handler() - Process interrupts for the ISP23xx and ISP63xx.
  * @irq:
@@ -174,7 +176,7 @@ qla2300_intr_handler(int irq, void *dev_id)
 	vha = pci_get_drvdata(ha->pdev);
 	for (iter = 50; iter--; ) {
 		stat = RD_REG_DWORD(&reg->u.isp2300.host_status);
-		if (qla2x00_check_reg_for_disconnect(vha, stat))
+		if (qla2x00_check_reg32_for_disconnect(vha, stat))
 			break;
 		if (stat & HSR_RISC_PAUSED) {
 			if (unlikely(pci_channel_offline(ha->pdev)))
@@ -573,8 +575,9 @@ qla2x00_async_event(scsi_qla_host_t *vha, struct rsp_que *rsp, uint16_t *mb)
 	struct device_reg_2xxx __iomem *reg = &ha->iobase->isp;
 	struct device_reg_24xx __iomem *reg24 = &ha->iobase->isp24;
 	struct device_reg_82xx __iomem *reg82 = &ha->iobase->isp82;
-	uint32_t	rscn_entry, host_pid;
+	uint32_t	rscn_entry, host_pid, tmp_pid;
 	unsigned long	flags;
+	fc_port_t	*fcport = NULL;
 
 	/* Setup to process RIO completion. */
 	handle_cnt = 0;
@@ -730,7 +733,7 @@ skip_rio:
 		else
 			ha->link_data_rate = mb[1];
 
-		ql_dbg(ql_dbg_async, vha, 0x500a,
+		ql_log(ql_log_info, vha, 0x500a,
 		    "LOOP UP detected (%s Gbps).\n",
 		    qla2x00_get_link_speed_str(ha, ha->link_data_rate));
 
@@ -743,13 +746,23 @@ skip_rio:
 			? RD_REG_WORD(&reg24->mailbox4) : 0;
 		mbx = (IS_P3P_TYPE(ha)) ? RD_REG_WORD(&reg82->mailbox_out[4])
 			: mbx;
-		ql_dbg(ql_dbg_async, vha, 0x500b,
+		ql_log(ql_log_info, vha, 0x500b,
 		    "LOOP DOWN detected (%x %x %x %x).\n",
 		    mb[1], mb[2], mb[3], mbx);
 
 		if (atomic_read(&vha->loop_state) != LOOP_DOWN) {
 			atomic_set(&vha->loop_state, LOOP_DOWN);
 			atomic_set(&vha->loop_down_timer, LOOP_DOWN_TIME);
+			/*
+			 * In case of loop down, restore WWPN from
+			 * NVRAM in case of FA-WWPN capable ISP
+			 */
+			if (ha->flags.fawwpn_enabled) {
+				void *wwpn = ha->init_cb->port_name;
+
+				memcpy(vha->port_name, wwpn, WWN_SIZE);
+			}
+
 			vha->device_flags |= DFLG_NO_CABLE;
 			qla2x00_mark_all_devices_lost(vha, 1);
 		}
@@ -908,7 +921,8 @@ skip_rio:
 		 * it.  Otherwise ignore it and Wait for RSCN to come in.
 		 */
 		atomic_set(&vha->loop_down_timer, 0);
-		if (mb[1] != 0xffff || (mb[2] != 0x6 && mb[2] != 0x4)) {
+		if (atomic_read(&vha->loop_state) != LOOP_DOWN &&
+		    atomic_read(&vha->loop_state) != LOOP_DEAD) {
 			ql_dbg(ql_dbg_async, vha, 0x5011,
 			    "Asynchronous PORT UPDATE ignored %04x/%04x/%04x.\n",
 			    mb[1], mb[2], mb[3]);
@@ -920,9 +934,6 @@ skip_rio:
 		ql_dbg(ql_dbg_async, vha, 0x5012,
 		    "Port database changed %04x %04x %04x.\n",
 		    mb[1], mb[2], mb[3]);
-		ql_log(ql_log_warn, vha, 0x505f,
-		    "Link is operational (%s Gbps).\n",
-		    qla2x00_get_link_speed_str(ha, ha->link_data_rate));
 
 		/*
 		 * Mark all devices as missing so we will login again.
@@ -969,6 +980,20 @@ skip_rio:
 		if (qla2x00_is_a_vp_did(vha, rscn_entry))
 			break;
 
+		/*
+		 * Search for the rport related to this RSCN entry and mark it
+		 * as lost.
+		 */
+		list_for_each_entry(fcport, &vha->vp_fcports, list) {
+			if (atomic_read(&fcport->state) != FCS_ONLINE)
+				continue;
+			tmp_pid = fcport->d_id.b24;
+			if (fcport->d_id.b24 == rscn_entry) {
+				qla2x00_mark_device_lost(vha, fcport, 0, 0);
+				break;
+			}
+		}
+
 		atomic_set(&vha->loop_down_timer, 0);
 		vha->flags.management_server_logged_in = 0;
 
@@ -1086,6 +1111,14 @@ skip_rio:
 		qla83xx_handle_8200_aen(vha, mb);
 		break;
 
+	case MBA_DPORT_DIAGNOSTICS:
+		ql_dbg(ql_dbg_async, vha, 0x5052,
+		    "D-Port Diagnostics: %04x %04x=%s\n", mb[0], mb[1],
+		    mb[1] == 0 ? "start" :
+		    mb[1] == 1 ? "done (ok)" :
+		    mb[1] == 2 ? "done (error)" : "other");
+		break;
+
 	default:
 		ql_dbg(ql_dbg_async, vha, 0x5057,
 		    "Unknown AEN:%04x %04x %04x %04x\n",
@@ -1975,6 +2008,7 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 	int logit = 1;
 	int res = 0;
 	uint16_t state_flags = 0;
+	uint16_t retry_delay = 0;
 
 	sts = (sts_entry_t *) pkt;
 	sts24 = (struct sts_entry_24xx *) pkt;
@@ -2068,6 +2102,9 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		host_to_fcp_swap(sts24->data, sizeof(sts24->data));
 		ox_id = le16_to_cpu(sts24->ox_id);
 		par_sense_len = sizeof(sts24->data);
+		/* Valid values of the retry delay timer are 0x1-0xffef */
+		if (sts24->retry_delay > 0 && sts24->retry_delay < 0xfff1)
+			retry_delay = sts24->retry_delay;
 	} else {
 		if (scsi_status & SS_SENSE_LEN_VALID)
 			sense_len = le16_to_cpu(sts->req_sense_length);
@@ -2102,6 +2139,14 @@ qla2x00_status_entry(scsi_qla_host_t *vha, struct rsp_que *rsp, void *pkt)
 		comp_status = CS_DATA_OVERRUN;
 
 	/*
+	 * Check retry_delay_timer value if we receive a busy or
+	 * queue full.
+	 */
+	if (lscsi_status == SAM_STAT_TASK_SET_FULL ||
+	    lscsi_status == SAM_STAT_BUSY)
+		qla2x00_set_retry_delay_timestamp(fcport, retry_delay);
+
+	/*
 	 * Based on Host and scsi status generate status code for Linux
 	 */
 	switch (comp_status) {
@@ -2633,7 +2678,7 @@ qla24xx_intr_handler(int irq, void *dev_id)
 	vha = pci_get_drvdata(ha->pdev);
 	for (iter = 50; iter--; ) {
 		stat = RD_REG_DWORD(&reg->host_status);
-		if (qla2x00_check_reg_for_disconnect(vha, stat))
+		if (qla2x00_check_reg32_for_disconnect(vha, stat))
 			break;
 		if (stat & HSRX_RISC_PAUSED) {
 			if (unlikely(pci_channel_offline(ha->pdev)))
@@ -2723,7 +2768,7 @@ qla24xx_msix_rsp_q(int irq, void *dev_id)
 	 * we process the response queue.
 	 */
 	stat = RD_REG_DWORD(&reg->host_status);
-	if (qla2x00_check_reg_for_disconnect(vha, stat))
+	if (qla2x00_check_reg32_for_disconnect(vha, stat))
 		goto out;
 	qla24xx_process_response_queue(vha, rsp);
 	if (!ha->flags.disable_msix_handshake) {
@@ -2763,7 +2808,7 @@ qla25xx_msix_rsp_q(int irq, void *dev_id)
 		hccr = RD_REG_DWORD_RELAXED(&reg->hccr);
 		spin_unlock_irqrestore(&ha->hardware_lock, flags);
 	}
-	if (qla2x00_check_reg_for_disconnect(vha, hccr))
+	if (qla2x00_check_reg32_for_disconnect(vha, hccr))
 		goto out;
 	queue_work_on((int) (rsp->id - 1), ha->wq, &rsp->q_work);
 
@@ -2798,7 +2843,7 @@ qla24xx_msix_default(int irq, void *dev_id)
 	vha = pci_get_drvdata(ha->pdev);
 	do {
 		stat = RD_REG_DWORD(&reg->host_status);
-		if (qla2x00_check_reg_for_disconnect(vha, stat))
+		if (qla2x00_check_reg32_for_disconnect(vha, stat))
 			break;
 		if (stat & HSRX_RISC_PAUSED) {
 			if (unlikely(pci_channel_offline(ha->pdev)))
@@ -2923,27 +2968,22 @@ qla24xx_enable_msix(struct qla_hw_data *ha, struct rsp_que *rsp)
 	for (i = 0; i < ha->msix_count; i++)
 		entries[i].entry = i;
 
-	ret = pci_enable_msix(ha->pdev, entries, ha->msix_count);
-	if (ret) {
-		if (ret < MIN_MSIX_COUNT)
-			goto msix_failed;
-
+	ret = pci_enable_msix_range(ha->pdev,
+				    entries, MIN_MSIX_COUNT, ha->msix_count);
+	if (ret < 0) {
+		ql_log(ql_log_fatal, vha, 0x00c7,
+		    "MSI-X: Failed to enable support, "
+		    "giving   up -- %d/%d.\n",
+		    ha->msix_count, ret);
+		goto msix_out;
+	} else if (ret < ha->msix_count) {
 		ql_log(ql_log_warn, vha, 0x00c6,
 		    "MSI-X: Failed to enable support "
 		    "-- %d/%d\n Retry with %d vectors.\n",
 		    ha->msix_count, ret, ret);
-		ha->msix_count = ret;
-		ret = pci_enable_msix(ha->pdev, entries, ha->msix_count);
-		if (ret) {
-msix_failed:
-			ql_log(ql_log_fatal, vha, 0x00c7,
-			    "MSI-X: Failed to enable support, "
-			    "giving   up -- %d/%d.\n",
-			    ha->msix_count, ret);
-			goto msix_out;
-		}
-		ha->max_rsp_queues = ha->msix_count - 1;
 	}
+	ha->msix_count = ret;
+	ha->max_rsp_queues = ha->msix_count - 1;
 	ha->msix_entries = kzalloc(sizeof(struct qla_msix_entry) *
 				ha->msix_count, GFP_KERNEL);
 	if (!ha->msix_entries) {
@@ -3103,10 +3143,11 @@ skip_msi:
 	}
 
 clear_risc_ints:
+	if (IS_FWI2_CAPABLE(ha) || IS_QLAFX00(ha))
+		goto fail;
 
 	spin_lock_irq(&ha->hardware_lock);
-	if (!IS_FWI2_CAPABLE(ha))
-		WRT_REG_WORD(&reg->isp.semaphore, 0);
+	WRT_REG_WORD(&reg->isp.semaphore, 0);
 	spin_unlock_irq(&ha->hardware_lock);
 
 fail:
diff --git a/drivers/scsi/qla2xxx/qla_mbx.c b/drivers/scsi/qla2xxx/qla_mbx.c
index d9aafc003be2..72971daa2552 100644
--- a/drivers/scsi/qla2xxx/qla_mbx.c
+++ b/drivers/scsi/qla2xxx/qla_mbx.c
@@ -117,7 +117,7 @@ qla2x00_mailbox_command(scsi_qla_host_t *vha, mbx_cmd_t *mcp)
 	command = mcp->mb[0];
 	mboxes = mcp->out_mb;
 
-	ql_dbg(ql_dbg_mbx + ql_dbg_buffer, vha, 0x1111,
+	ql_dbg(ql_dbg_mbx, vha, 0x1111,
 	    "Mailbox registers (OUT):\n");
 	for (cnt = 0; cnt < ha->mbx_count; cnt++) {
 		if (IS_QLA2200(ha) && cnt == 8)
@@ -373,7 +373,7 @@ premature_exit:
 
 mbx_done:
 	if (rval) {
-		ql_log(ql_log_warn, base_vha, 0x1020,
+		ql_dbg(ql_dbg_disc, base_vha, 0x1020,
 		    "**** Failed mbx[0]=%x, mb[1]=%x, mb[2]=%x, mb[3]=%x, cmd=%x ****.\n",
 		    mcp->mb[0], mcp->mb[1], mcp->mb[2], mcp->mb[3], command);
 	} else {
@@ -1085,6 +1085,8 @@ qla2x00_get_adapter_id(scsi_qla_host_t *vha, uint16_t *id, uint8_t *al_pa,
 	mcp->in_mb = MBX_9|MBX_7|MBX_6|MBX_3|MBX_2|MBX_1|MBX_0;
 	if (IS_CNA_CAPABLE(vha->hw))
 		mcp->in_mb |= MBX_13|MBX_12|MBX_11|MBX_10;
+	if (IS_FWI2_CAPABLE(vha->hw))
+		mcp->in_mb |= MBX_19|MBX_18|MBX_17|MBX_16;
 	mcp->tov = MBX_TOV_SECONDS;
 	mcp->flags = 0;
 	rval = qla2x00_mailbox_command(vha, mcp);
@@ -1118,6 +1120,22 @@ qla2x00_get_adapter_id(scsi_qla_host_t *vha, uint16_t *id, uint8_t *al_pa,
 			vha->fcoe_vn_port_mac[1] = mcp->mb[13] >> 8;
 			vha->fcoe_vn_port_mac[0] = mcp->mb[13] & 0xff;
 		}
+		/* If FA-WWN supported */
+		if (mcp->mb[7] & BIT_14) {
+			vha->port_name[0] = MSB(mcp->mb[16]);
+			vha->port_name[1] = LSB(mcp->mb[16]);
+			vha->port_name[2] = MSB(mcp->mb[17]);
+			vha->port_name[3] = LSB(mcp->mb[17]);
+			vha->port_name[4] = MSB(mcp->mb[18]);
+			vha->port_name[5] = LSB(mcp->mb[18]);
+			vha->port_name[6] = MSB(mcp->mb[19]);
+			vha->port_name[7] = LSB(mcp->mb[19]);
+			fc_host_port_name(vha->host) =
+			    wwn_to_u64(vha->port_name);
+			ql_dbg(ql_dbg_mbx, vha, 0x10ca,
+			    "FA-WWN acquired %016llx\n",
+			    wwn_to_u64(vha->port_name));
+		}
 	}
 
 	return rval;
@@ -1546,7 +1564,7 @@ qla2x00_get_firmware_state(scsi_qla_host_t *vha, uint16_t *states)
 	mcp->mb[0] = MBC_GET_FIRMWARE_STATE;
 	mcp->out_mb = MBX_0;
 	if (IS_FWI2_CAPABLE(vha->hw))
-		mcp->in_mb = MBX_5|MBX_4|MBX_3|MBX_2|MBX_1|MBX_0;
+		mcp->in_mb = MBX_6|MBX_5|MBX_4|MBX_3|MBX_2|MBX_1|MBX_0;
 	else
 		mcp->in_mb = MBX_1|MBX_0;
 	mcp->tov = MBX_TOV_SECONDS;
@@ -1560,6 +1578,7 @@ qla2x00_get_firmware_state(scsi_qla_host_t *vha, uint16_t *states)
 		states[2] = mcp->mb[3];
 		states[3] = mcp->mb[4];
 		states[4] = mcp->mb[5];
+		states[5] = mcp->mb[6];  /* DPORT status */
 	}
 
 	if (rval != QLA_SUCCESS) {
@@ -3328,8 +3347,24 @@ qla24xx_report_id_acquisition(scsi_qla_host_t *vha,
 		    rptid_entry->port_id[2], rptid_entry->port_id[1],
 		    rptid_entry->port_id[0]);
 
+		/* FA-WWN is only for physical port */
+		if (!vp_idx) {
+			void *wwpn = ha->init_cb->port_name;
+
+			if (!MSB(stat)) {
+				if (rptid_entry->vp_idx_map[1] & BIT_6)
+					wwpn = rptid_entry->reserved_4 + 8;
+			}
+			memcpy(vha->port_name, wwpn, WWN_SIZE);
+			fc_host_port_name(vha->host) =
+			    wwn_to_u64(vha->port_name);
+			ql_dbg(ql_dbg_mbx, vha, 0x1018,
+			    "FA-WWN portname %016llx (%x)\n",
+			    fc_host_port_name(vha->host), MSB(stat));
+		}
+
 		vp = vha;
-		if (vp_idx == 0 && (MSB(stat) != 1))
+		if (vp_idx == 0)
 			goto reg_needed;
 
 		if (MSB(stat) != 0 && MSB(stat) != 2) {
diff --git a/drivers/scsi/qla2xxx/qla_mid.c b/drivers/scsi/qla2xxx/qla_mid.c
index 89998244f48d..5c2e0317f1c0 100644
--- a/drivers/scsi/qla2xxx/qla_mid.c
+++ b/drivers/scsi/qla2xxx/qla_mid.c
@@ -702,6 +702,7 @@ qla25xx_create_req_que(struct qla_hw_data *ha, uint16_t options,
 	req->req_q_in = &reg->isp25mq.req_q_in;
 	req->req_q_out = &reg->isp25mq.req_q_out;
 	req->max_q_depth = ha->req_q_map[0]->max_q_depth;
+	req->out_ptr = (void *)(req->ring + req->length);
 	mutex_unlock(&ha->vport_lock);
 	ql_dbg(ql_dbg_multiq, base_vha, 0xc004,
 	    "ring_ptr=%p ring_index=%d, "
@@ -811,6 +812,7 @@ qla25xx_create_rsp_que(struct qla_hw_data *ha, uint16_t options,
 	reg = ISP_QUE_REG(ha, que_id);
 	rsp->rsp_q_in = &reg->isp25mq.rsp_q_in;
 	rsp->rsp_q_out = &reg->isp25mq.rsp_q_out;
+	rsp->in_ptr = (void *)(rsp->ring + rsp->length);
 	mutex_unlock(&ha->vport_lock);
 	ql_dbg(ql_dbg_multiq, base_vha, 0xc00b,
 	    "options=%x id=%d rsp_q_in=%p rsp_q_out=%p",
diff --git a/drivers/scsi/qla2xxx/qla_mr.c b/drivers/scsi/qla2xxx/qla_mr.c
index 4775baa8b6a0..80867599527d 100644
--- a/drivers/scsi/qla2xxx/qla_mr.c
+++ b/drivers/scsi/qla2xxx/qla_mr.c
@@ -695,11 +695,11 @@ qlafx00_pci_info_str(struct scsi_qla_host *vha, char *str)
 }
 
 char *
-qlafx00_fw_version_str(struct scsi_qla_host *vha, char *str)
+qlafx00_fw_version_str(struct scsi_qla_host *vha, char *str, size_t size)
 {
 	struct qla_hw_data *ha = vha->hw;
 
-	sprintf(str, "%s", ha->mr.fw_version);
+	snprintf(str, size, "%s", ha->mr.fw_version);
 	return str;
 }
 
@@ -1551,7 +1551,10 @@ qlafx00_timer_routine(scsi_qla_host_t *vha)
 			ha->mr.fw_reset_timer_tick =
 			    QLAFX00_MAX_RESET_INTERVAL;
 		}
-		ha->mr.old_aenmbx0_state = aenmbx0;
+		if (ha->mr.old_aenmbx0_state != aenmbx0) {
+			ha->mr.old_aenmbx0_state = aenmbx0;
+			ha->mr.fw_reset_timer_tick = QLAFX00_RESET_INTERVAL;
+		}
 		ha->mr.fw_reset_timer_tick--;
 	}
 	if (test_bit(FX00_CRITEMP_RECOVERY, &vha->dpc_flags)) {
@@ -1675,17 +1678,16 @@ qlafx00_get_fcport(struct scsi_qla_host *vha, int tgt_id)
 	fc_port_t	*fcport;
 
 	/* Check for matching device in remote port list. */
-	fcport = NULL;
 	list_for_each_entry(fcport, &vha->vp_fcports, list) {
 		if (fcport->tgt_id == tgt_id) {
 			ql_dbg(ql_dbg_async, vha, 0x5072,
 			    "Matching fcport(%p) found with TGT-ID: 0x%x "
 			    "and Remote TGT_ID: 0x%x\n",
 			    fcport, fcport->tgt_id, tgt_id);
-			break;
+			return fcport;
 		}
 	}
-	return fcport;
+	return NULL;
 }
 
 static void
@@ -2924,7 +2926,7 @@ qlafx00_intr_handler(int irq, void *dev_id)
 	vha = pci_get_drvdata(ha->pdev);
 	for (iter = 50; iter--; clr_intr = 0) {
 		stat = QLAFX00_RD_INTR_REG(ha);
-		if (qla2x00_check_reg_for_disconnect(vha, stat))
+		if (qla2x00_check_reg32_for_disconnect(vha, stat))
 			break;
 		intr_stat = stat & QLAFX00_HST_INT_STS_BITS;
 		if (!intr_stat)
diff --git a/drivers/scsi/qla2xxx/qla_nx.c b/drivers/scsi/qla2xxx/qla_nx.c
index 58f3c912d96e..54cb2ac9339b 100644
--- a/drivers/scsi/qla2xxx/qla_nx.c
+++ b/drivers/scsi/qla2xxx/qla_nx.c
@@ -857,7 +857,7 @@ qla82xx_rom_lock(struct qla_hw_data *ha)
 			break;
 		if (timeout >= qla82xx_rom_lock_timeout) {
 			lock_owner = qla82xx_rd_32(ha, QLA82XX_ROM_LOCK_ID);
-			ql_log(ql_log_warn, vha, 0xb157,
+			ql_dbg(ql_dbg_p3p, vha, 0xb157,
 			    "%s: Simultaneous flash access by following ports, active port = %d: accessing port = %d",
 			    __func__, ha->portnum, lock_owner);
 			return -1;
@@ -2123,7 +2123,7 @@ qla82xx_msix_default(int irq, void *dev_id)
 	vha = pci_get_drvdata(ha->pdev);
 	do {
 		host_int = RD_REG_DWORD(&reg->host_int);
-		if (qla2x00_check_reg_for_disconnect(vha, host_int))
+		if (qla2x00_check_reg32_for_disconnect(vha, host_int))
 			break;
 		if (host_int) {
 			stat = RD_REG_DWORD(&reg->host_status);
@@ -2184,7 +2184,7 @@ qla82xx_msix_rsp_q(int irq, void *dev_id)
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	vha = pci_get_drvdata(ha->pdev);
 	host_int = RD_REG_DWORD(&reg->host_int);
-	if (qla2x00_check_reg_for_disconnect(vha, host_int))
+	if (qla2x00_check_reg32_for_disconnect(vha, host_int))
 		goto out;
 	qla24xx_process_response_queue(vha, rsp);
 	WRT_REG_DWORD(&reg->host_int, 0);
@@ -2219,7 +2219,7 @@ qla82xx_poll(int irq, void *dev_id)
 	vha = pci_get_drvdata(ha->pdev);
 
 	host_int = RD_REG_DWORD(&reg->host_int);
-	if (qla2x00_check_reg_for_disconnect(vha, host_int))
+	if (qla2x00_check_reg32_for_disconnect(vha, host_int))
 		goto out;
 	if (host_int) {
 		stat = RD_REG_DWORD(&reg->host_status);
diff --git a/drivers/scsi/qla2xxx/qla_nx2.c b/drivers/scsi/qla2xxx/qla_nx2.c
index da9e3902f219..24a852828b5d 100644
--- a/drivers/scsi/qla2xxx/qla_nx2.c
+++ b/drivers/scsi/qla2xxx/qla_nx2.c
@@ -399,7 +399,7 @@ qla8044_idc_lock(struct qla_hw_data *ha)
 				 */
 				 ql_dbg(ql_dbg_p3p, vha, 0xb08a,
 				     "%s: IDC lock Recovery by %d "
-				     "failed, Retrying timout\n", __func__,
+				     "failed, Retrying timeout\n", __func__,
 				     ha->portnum);
 				 timeout = 0;
 			}
diff --git a/drivers/scsi/qla2xxx/qla_os.c b/drivers/scsi/qla2xxx/qla_os.c
index be9698d920c2..db3dbd999cb6 100644
--- a/drivers/scsi/qla2xxx/qla_os.c
+++ b/drivers/scsi/qla2xxx/qla_os.c
@@ -105,7 +105,7 @@ MODULE_PARM_DESC(ql2xshiftctondsd,
 		"based on total number of SG elements.");
 
 int ql2xfdmienable=1;
-module_param(ql2xfdmienable, int, S_IRUGO);
+module_param(ql2xfdmienable, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(ql2xfdmienable,
 		"Enables FDMI registrations. "
 		"0 - no FDMI. Default is 1 - perform FDMI.");
@@ -238,7 +238,9 @@ static int qla2xxx_eh_host_reset(struct scsi_cmnd *);
 
 static int qla2x00_change_queue_depth(struct scsi_device *, int, int);
 static int qla2x00_change_queue_type(struct scsi_device *, int);
+static void qla2x00_clear_drv_active(struct qla_hw_data *);
 static void qla2x00_free_device(scsi_qla_host_t *);
+static void qla83xx_disable_laser(scsi_qla_host_t *vha);
 
 struct scsi_host_template qla2xxx_driver_template = {
 	.module			= THIS_MODULE,
@@ -547,14 +549,13 @@ qla24xx_pci_info_str(struct scsi_qla_host *vha, char *str)
 }
 
 static char *
-qla2x00_fw_version_str(struct scsi_qla_host *vha, char *str)
+qla2x00_fw_version_str(struct scsi_qla_host *vha, char *str, size_t size)
 {
 	char un_str[10];
 	struct qla_hw_data *ha = vha->hw;
 
-	sprintf(str, "%d.%02d.%02d ", ha->fw_major_version,
-	    ha->fw_minor_version,
-	    ha->fw_subminor_version);
+	snprintf(str, size, "%d.%02d.%02d ", ha->fw_major_version,
+	    ha->fw_minor_version, ha->fw_subminor_version);
 
 	if (ha->fw_attributes & BIT_9) {
 		strcat(str, "FLX");
@@ -586,11 +587,11 @@ qla2x00_fw_version_str(struct scsi_qla_host *vha, char *str)
 }
 
 static char *
-qla24xx_fw_version_str(struct scsi_qla_host *vha, char *str)
+qla24xx_fw_version_str(struct scsi_qla_host *vha, char *str, size_t size)
 {
 	struct qla_hw_data *ha = vha->hw;
 
-	sprintf(str, "%d.%02d.%02d (%x)", ha->fw_major_version,
+	snprintf(str, size, "%d.%02d.%02d (%x)", ha->fw_major_version,
 	    ha->fw_minor_version, ha->fw_subminor_version, ha->fw_attributes);
 	return str;
 }
@@ -730,6 +731,15 @@ qla2xxx_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 		goto qc24_target_busy;
 	}
 
+	/*
+	 * Return target busy if we've received a non-zero retry_delay_timer
+	 * in a FCP_RSP.
+	 */
+	if (time_after(jiffies, fcport->retry_delay_timestamp))
+		fcport->retry_delay_timestamp = 0;
+	else
+		goto qc24_target_busy;
+
 	sp = qla2x00_get_sp(vha, fcport, GFP_ATOMIC);
 	if (!sp)
 		goto qc24_host_busy;
@@ -860,8 +870,10 @@ qla2x00_wait_for_hba_ready(scsi_qla_host_t *vha)
 {
 	struct qla_hw_data *ha = vha->hw;
 
-	while ((!(vha->flags.online) || ha->dpc_active ||
-	    ha->flags.mbox_busy))
+	while (((qla2x00_reset_active(vha)) || ha->dpc_active ||
+	    ha->flags.mbox_busy) ||
+		test_bit(FX00_RESET_RECOVERY, &vha->dpc_flags) ||
+		test_bit(FX00_TARGET_SCAN, &vha->dpc_flags))
 		msleep(1000);
 }
 
@@ -1351,6 +1363,8 @@ qla2x00_abort_all_cmds(scsi_qla_host_t *vha, int res)
 	struct qla_hw_data *ha = vha->hw;
 	struct req_que *req;
 
+	qlt_host_reset_handler(ha);
+
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	for (que = 0; que < ha->max_req_queues; que++) {
 		req = ha->req_q_map[que];
@@ -2384,6 +2398,8 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	    "Memory allocated for ha=%p.\n", ha);
 	ha->pdev = pdev;
 	ha->tgt.enable_class_2 = ql2xenableclass2;
+	INIT_LIST_HEAD(&ha->tgt.q_full_list);
+	spin_lock_init(&ha->tgt.q_full_lock);
 
 	/* Clear our data area */
 	ha->bars = bars;
@@ -2527,7 +2543,7 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 		ha->portnum = PCI_FUNC(ha->pdev->devfn);
 		ha->max_fibre_devices = MAX_FIBRE_DEVICES_2400;
 		ha->mbx_count = MAILBOX_REGISTER_COUNT;
-		req_length = REQUEST_ENTRY_CNT_24XX;
+		req_length = REQUEST_ENTRY_CNT_83XX;
 		rsp_length = RESPONSE_ENTRY_CNT_2300;
 		ha->tgt.atio_q_length = ATIO_ENTRY_CNT_24XX;
 		ha->max_loop_id = SNS_LAST_LOOP_ID_2300;
@@ -2631,6 +2647,7 @@ qla2x00_probe_one(struct pci_dev *pdev, const struct pci_device_id *id)
 	}
 
 	pci_set_drvdata(pdev, base_vha);
+	set_bit(PFLG_DRIVER_PROBING, &base_vha->pci_flags);
 
 	host = base_vha->host;
 	base_vha->req = req;
@@ -2923,10 +2940,11 @@ skip_dpc:
 	    pdev->device, ha->isp_ops->pci_info_str(base_vha, pci_info),
 	    pci_name(pdev), ha->flags.enable_64bit_addressing ? '+' : '-',
 	    base_vha->host_no,
-	    ha->isp_ops->fw_version_str(base_vha, fw_str));
+	    ha->isp_ops->fw_version_str(base_vha, fw_str, sizeof(fw_str)));
 
 	qlt_add_target(ha, base_vha);
 
+	clear_bit(PFLG_DRIVER_PROBING, &base_vha->pci_flags);
 	return 0;
 
 probe_init_failed:
@@ -2954,16 +2972,8 @@ probe_failed:
 	scsi_host_put(base_vha->host);
 
 probe_hw_failed:
-	if (IS_QLA82XX(ha)) {
-		qla82xx_idc_lock(ha);
-		qla82xx_clear_drv_active(ha);
-		qla82xx_idc_unlock(ha);
-	}
-	if (IS_QLA8044(ha)) {
-		qla8044_idc_lock(ha);
-		qla8044_clear_drv_active(ha);
-		qla8044_idc_unlock(ha);
-	}
+	qla2x00_clear_drv_active(ha);
+
 iospace_config_failed:
 	if (IS_P3P_TYPE(ha)) {
 		if (!ha->nx_pcibase)
@@ -3026,6 +3036,9 @@ qla2x00_shutdown(struct pci_dev *pdev)
 	qla2x00_free_irqs(vha);
 
 	qla2x00_free_fw_dump(ha);
+
+	pci_disable_pcie_error_reporting(pdev);
+	pci_disable_device(pdev);
 }
 
 /* Deletes all the virtual ports for a given ha */
@@ -3119,10 +3132,8 @@ qla2x00_unmap_iobases(struct qla_hw_data *ha)
 }
 
 static void
-qla2x00_clear_drv_active(scsi_qla_host_t *vha)
+qla2x00_clear_drv_active(struct qla_hw_data *ha)
 {
-	struct qla_hw_data *ha = vha->hw;
-
 	if (IS_QLA8044(ha)) {
 		qla8044_idc_lock(ha);
 		qla8044_clear_drv_active(ha);
@@ -3140,15 +3151,25 @@ qla2x00_remove_one(struct pci_dev *pdev)
 	scsi_qla_host_t *base_vha;
 	struct qla_hw_data  *ha;
 
+	base_vha = pci_get_drvdata(pdev);
+	ha = base_vha->hw;
+
+	/* Indicate device removal to prevent future board_disable and wait
+	 * until any pending board_disable has completed. */
+	set_bit(PFLG_DRIVER_REMOVING, &base_vha->pci_flags);
+	cancel_work_sync(&ha->board_disable);
+
 	/*
-	 * If the PCI device is disabled that means that probe failed and any
-	 * resources should be have cleaned up on probe exit.
+	 * If the PCI device is disabled then there was a PCI-disconnect and
+	 * qla2x00_disable_board_on_pci_error has taken care of most of the
+	 * resources.
 	 */
-	if (!atomic_read(&pdev->enable_cnt))
+	if (!atomic_read(&pdev->enable_cnt)) {
+		scsi_host_put(base_vha->host);
+		kfree(ha);
+		pci_set_drvdata(pdev, NULL);
 		return;
-
-	base_vha = pci_get_drvdata(pdev);
-	ha = base_vha->hw;
+	}
 
 	qla2x00_wait_for_hba_ready(base_vha);
 
@@ -3173,6 +3194,10 @@ qla2x00_remove_one(struct pci_dev *pdev)
 
 	qla84xx_put_chip(base_vha);
 
+	/* Laser should be disabled only for ISP2031 */
+	if (IS_QLA2031(ha))
+		qla83xx_disable_laser(base_vha);
+
 	/* Disable timer */
 	if (base_vha->timer_active)
 		qla2x00_stop_timer(base_vha);
@@ -3191,9 +3216,9 @@ qla2x00_remove_one(struct pci_dev *pdev)
 
 	qla2x00_free_device(base_vha);
 
-	scsi_host_put(base_vha->host);
+	qla2x00_clear_drv_active(ha);
 
-	qla2x00_clear_drv_active(base_vha);
+	scsi_host_put(base_vha->host);
 
 	qla2x00_unmap_iobases(ha);
 
@@ -4808,18 +4833,15 @@ qla2x00_disable_board_on_pci_error(struct work_struct *work)
 	qla82xx_md_free(base_vha);
 	qla2x00_free_queues(ha);
 
-	scsi_host_put(base_vha->host);
-
 	qla2x00_unmap_iobases(ha);
 
 	pci_release_selected_regions(ha->pdev, ha->bars);
-	kfree(ha);
-	ha = NULL;
-
 	pci_disable_pcie_error_reporting(pdev);
 	pci_disable_device(pdev);
-	pci_set_drvdata(pdev, NULL);
 
+	/*
+	 * Let qla2x00_remove_one cleanup qla_hw_data on device removal.
+	 */
 }
 
 /**************************************************************************
@@ -4853,7 +4875,6 @@ qla2x00_do_dpc(void *data)
 		    "DPC handler sleeping.\n");
 
 		schedule();
-		__set_current_state(TASK_RUNNING);
 
 		if (!base_vha->flags.init_done || ha->flags.mbox_busy)
 			goto end_loop;
@@ -5192,13 +5213,7 @@ qla2x00_timer(scsi_qla_host_t *vha)
 	 */
 	if (!pci_channel_offline(ha->pdev)) {
 		pci_read_config_word(ha->pdev, PCI_VENDOR_ID, &w);
-		if (w == 0xffff)
-			/*
-			 * Schedule this on the default system workqueue so that
-			 * all the adapter workqueues and the DPC thread can be
-			 * shutdown cleanly.
-			 */
-			schedule_work(&ha->board_disable);
+		qla2x00_check_reg16_for_disconnect(vha, w);
 	}
 
 	/* Make sure qla82xx_watchdog is run only for physical port */
@@ -5706,6 +5721,32 @@ qla2xxx_pci_resume(struct pci_dev *pdev)
 	ha->flags.eeh_busy = 0;
 }
 
+static void
+qla83xx_disable_laser(scsi_qla_host_t *vha)
+{
+	uint32_t reg, data, fn;
+	struct qla_hw_data *ha = vha->hw;
+	struct device_reg_24xx __iomem *isp_reg = &ha->iobase->isp24;
+
+	/* pci func #/port # */
+	ql_dbg(ql_dbg_init, vha, 0x004b,
+	    "Disabling Laser for hba: %p\n", vha);
+
+	fn = (RD_REG_DWORD(&isp_reg->ctrl_status) &
+		(BIT_15|BIT_14|BIT_13|BIT_12));
+
+	fn = (fn >> 12);
+
+	if (fn & 1)
+		reg = PORT_1_2031;
+	else
+		reg = PORT_0_2031;
+
+	data = LASER_OFF_2031;
+
+	qla83xx_wr_reg(vha, reg, data);
+}
+
 static const struct pci_error_handlers qla2xxx_err_handler = {
 	.error_detected = qla2xxx_pci_error_detected,
 	.mmio_enabled = qla2xxx_pci_mmio_enabled,
diff --git a/drivers/scsi/qla2xxx/qla_sup.c b/drivers/scsi/qla2xxx/qla_sup.c
index bca173e56f16..b656a05613e8 100644
--- a/drivers/scsi/qla2xxx/qla_sup.c
+++ b/drivers/scsi/qla2xxx/qla_sup.c
@@ -2580,7 +2580,8 @@ qla25xx_read_optrom_data(struct scsi_qla_host *vha, uint8_t *buf,
 	uint32_t faddr, left, burst;
 	struct qla_hw_data *ha = vha->hw;
 
-	if (IS_QLA25XX(ha) || IS_QLA81XX(ha) || IS_QLA27XX(ha))
+	if (IS_QLA25XX(ha) || IS_QLA81XX(ha) || IS_QLA83XX(ha) ||
+	    IS_QLA27XX(ha))
 		goto try_fast;
 	if (offset & 0xfff)
 		goto slow_read;
@@ -3091,7 +3092,7 @@ qla24xx_get_flash_version(scsi_qla_host_t *vha, void *mbuf)
 		ha->fw_revision[2] = dcode[2];
 		ha->fw_revision[3] = dcode[3];
 		ql_dbg(ql_dbg_init, vha, 0x0060,
-		    "Firmware revision %d.%d.%d.%d.\n",
+		    "Firmware revision %d.%d.%d (%x).\n",
 		    ha->fw_revision[0], ha->fw_revision[1],
 		    ha->fw_revision[2], ha->fw_revision[3]);
 	}
@@ -3162,7 +3163,7 @@ qla2xxx_get_vpd_field(scsi_qla_host_t *vha, char *key, char *str, size_t size)
 	}
 
 	if (pos < end - len && *pos != 0x78)
-		return snprintf(str, size, "%.*s", len, pos + 3);
+		return scnprintf(str, size, "%.*s", len, pos + 3);
 
 	return 0;
 }
diff --git a/drivers/scsi/qla2xxx/qla_target.c b/drivers/scsi/qla2xxx/qla_target.c
index e632e14180cf..a902fa1db7af 100644
--- a/drivers/scsi/qla2xxx/qla_target.c
+++ b/drivers/scsi/qla2xxx/qla_target.c
@@ -42,6 +42,11 @@
 #include "qla_def.h"
 #include "qla_target.h"
 
+static int ql2xtgt_tape_enable;
+module_param(ql2xtgt_tape_enable, int, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(ql2xtgt_tape_enable,
+		"Enables Sequence level error recovery (aka FC Tape). Default is 0 - no SLER. 1 - Enable SLER.");
+
 static char *qlini_mode = QLA2XXX_INI_MODE_STR_ENABLED;
 module_param(qlini_mode, charp, S_IRUGO);
 MODULE_PARM_DESC(qlini_mode,
@@ -54,6 +59,8 @@ MODULE_PARM_DESC(qlini_mode,
 
 int ql2x_ini_mode = QLA2XXX_INI_MODE_EXCLUSIVE;
 
+static int temp_sam_status = SAM_STAT_BUSY;
+
 /*
  * From scsi/fc/fc_fcp.h
  */
@@ -101,6 +108,11 @@ static void qlt_send_term_exchange(struct scsi_qla_host *ha, struct qla_tgt_cmd
 	*cmd, struct atio_from_isp *atio, int ha_locked);
 static void qlt_reject_free_srr_imm(struct scsi_qla_host *ha,
 	struct qla_tgt_srr_imm *imm, int ha_lock);
+static void qlt_abort_cmd_on_host_reset(struct scsi_qla_host *vha,
+	struct qla_tgt_cmd *cmd);
+static void qlt_alloc_qfull_cmd(struct scsi_qla_host *vha,
+	struct atio_from_isp *atio, uint16_t status, int qfull);
+static void qlt_disable_vha(struct scsi_qla_host *vha);
 /*
  * Global Variables
  */
@@ -178,7 +190,28 @@ struct scsi_qla_host *qlt_find_host_by_vp_idx(struct scsi_qla_host *vha,
 	return NULL;
 }
 
-void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
+static inline void qlt_incr_num_pend_cmds(struct scsi_qla_host *vha)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&vha->hw->tgt.q_full_lock, flags);
+
+	vha->hw->tgt.num_pend_cmds++;
+	if (vha->hw->tgt.num_pend_cmds > vha->hw->qla_stats.stat_max_pend_cmds)
+		vha->hw->qla_stats.stat_max_pend_cmds =
+			vha->hw->tgt.num_pend_cmds;
+	spin_unlock_irqrestore(&vha->hw->tgt.q_full_lock, flags);
+}
+static inline void qlt_decr_num_pend_cmds(struct scsi_qla_host *vha)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&vha->hw->tgt.q_full_lock, flags);
+	vha->hw->tgt.num_pend_cmds--;
+	spin_unlock_irqrestore(&vha->hw->tgt.q_full_lock, flags);
+}
+
+static void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *vha,
 	struct atio_from_isp *atio)
 {
 	ql_dbg(ql_dbg_tgt, vha, 0xe072,
@@ -401,7 +434,7 @@ static int qlt_reset(struct scsi_qla_host *vha, void *iocb, int mcmd)
 #if 0 /* FIXME: Re-enable Global event handling.. */
 		/* Global event */
 		atomic_inc(&ha->tgt.qla_tgt->tgt_global_resets_count);
-		qlt_clear_tgt_db(ha->tgt.qla_tgt, 1);
+		qlt_clear_tgt_db(ha->tgt.qla_tgt);
 		if (!list_empty(&ha->tgt.qla_tgt->sess_list)) {
 			sess = list_entry(ha->tgt.qla_tgt->sess_list.next,
 			    typeof(*sess), sess_list_entry);
@@ -483,7 +516,7 @@ static void qlt_schedule_sess_for_deletion(struct qla_tgt_sess *sess,
 }
 
 /* ha->hardware_lock supposed to be held on entry */
-static void qlt_clear_tgt_db(struct qla_tgt *tgt, bool local_only)
+static void qlt_clear_tgt_db(struct qla_tgt *tgt)
 {
 	struct qla_tgt_sess *sess;
 
@@ -835,7 +868,7 @@ int qlt_stop_phase1(struct qla_tgt *tgt)
 	mutex_lock(&vha->vha_tgt.tgt_mutex);
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	tgt->tgt_stop = 1;
-	qlt_clear_tgt_db(tgt, true);
+	qlt_clear_tgt_db(tgt);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 	mutex_unlock(&vha->vha_tgt.tgt_mutex);
 	mutex_unlock(&qla_tgt_mutex);
@@ -1008,6 +1041,8 @@ static void qlt_send_notify_ack(struct scsi_qla_host *vha,
 	    "qla_target(%d): Sending 24xx Notify Ack %d\n",
 	    vha->vp_idx, nack->u.isp24.status);
 
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(vha, vha->req);
 }
 
@@ -1031,7 +1066,7 @@ static void qlt_24xx_send_abts_resp(struct scsi_qla_host *vha,
 	if (qlt_issue_marker(vha, 1) != QLA_SUCCESS)
 		return;
 
-	resp = (struct abts_resp_to_24xx *)qla2x00_alloc_iocbs(vha, NULL);
+	resp = (struct abts_resp_to_24xx *)qla2x00_alloc_iocbs_ready(vha, NULL);
 	if (!resp) {
 		ql_dbg(ql_dbg_tgt, vha, 0xe04a,
 		    "qla_target(%d): %s failed: unable to allocate "
@@ -1085,6 +1120,8 @@ static void qlt_24xx_send_abts_resp(struct scsi_qla_host *vha,
 
 	vha->vha_tgt.qla_tgt->abts_resp_expected++;
 
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(vha, vha->req);
 }
 
@@ -1102,7 +1139,7 @@ static void qlt_24xx_retry_term_exchange(struct scsi_qla_host *vha,
 	if (qlt_issue_marker(vha, 1) != QLA_SUCCESS)
 		return;
 
-	ctio = (struct ctio7_to_24xx *)qla2x00_alloc_iocbs(vha, NULL);
+	ctio = (struct ctio7_to_24xx *)qla2x00_alloc_iocbs_ready(vha, NULL);
 	if (ctio == NULL) {
 		ql_dbg(ql_dbg_tgt, vha, 0xe04b,
 		    "qla_target(%d): %s failed: unable to allocate "
@@ -1130,6 +1167,8 @@ static void qlt_24xx_retry_term_exchange(struct scsi_qla_host *vha,
 		CTIO7_FLAGS_TERMINATE);
 	ctio->u.status1.ox_id = cpu_to_le16(entry->fcp_hdr_le.ox_id);
 
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(vha, vha->req);
 
 	qlt_24xx_send_abts_resp(vha, (struct abts_recv_from_24xx *)entry,
@@ -1178,6 +1217,7 @@ static int __qlt_24xx_handle_abts(struct scsi_qla_host *vha,
 
 	mcmd->sess = sess;
 	memcpy(&mcmd->orig_iocb.abts, abts, sizeof(mcmd->orig_iocb.abts));
+	mcmd->reset_count = vha->hw->chip_reset;
 
 	rc = ha->tgt.tgt_ops->handle_tmr(mcmd, lun, TMR_ABORT_TASK,
 	    abts->exchange_addr_to_abort);
@@ -1300,6 +1340,8 @@ static void qlt_24xx_send_task_mgmt_ctio(struct scsi_qla_host *ha,
 	ctio->u.status1.response_len = __constant_cpu_to_le16(8);
 	ctio->u.status1.sense_data[0] = resp_code;
 
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(ha, ha->req);
 }
 
@@ -1321,6 +1363,21 @@ void qlt_xmit_tm_rsp(struct qla_tgt_mgmt_cmd *mcmd)
 	    mcmd, mcmd->fc_tm_rsp, mcmd->flags);
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
+
+	if (qla2x00_reset_active(vha) || mcmd->reset_count != ha->chip_reset) {
+		/*
+		 * Either a chip reset is active or this request was from
+		 * previous life, just abort the processing.
+		 */
+		ql_dbg(ql_dbg_async, vha, 0xe100,
+			"RESET-TMR active/old-count/new-count = %d/%d/%d.\n",
+			qla2x00_reset_active(vha), mcmd->reset_count,
+			ha->chip_reset);
+		ha->tgt.tgt_ops->free_mcmd(mcmd);
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		return;
+	}
+
 	if (mcmd->flags == QLA24XX_MGMT_SEND_NACK)
 		qlt_send_notify_ack(vha, &mcmd->orig_iocb.imm_ntfy,
 		    0, 0, 0, 0, 0, 0);
@@ -1397,8 +1454,6 @@ static int qlt_pci_map_calc_cnt(struct qla_tgt_prm *prm)
 		}
 	}
 
-	ql_dbg(ql_dbg_tgt, prm->cmd->vha, 0xe009, "seg_cnt=%d, req_cnt=%d\n",
-	    prm->seg_cnt, prm->req_cnt);
 	return 0;
 
 out_err:
@@ -1408,12 +1463,13 @@ out_err:
 	return -1;
 }
 
-static inline void qlt_unmap_sg(struct scsi_qla_host *vha,
-	struct qla_tgt_cmd *cmd)
+static void qlt_unmap_sg(struct scsi_qla_host *vha, struct qla_tgt_cmd *cmd)
 {
 	struct qla_hw_data *ha = vha->hw;
 
-	BUG_ON(!cmd->sg_mapped);
+	if (!cmd->sg_mapped)
+		return;
+
 	pci_unmap_sg(ha->pdev, cmd->sg, cmd->sg_cnt, cmd->dma_data_direction);
 	cmd->sg_mapped = 0;
 
@@ -1431,17 +1487,12 @@ static inline void qlt_unmap_sg(struct scsi_qla_host *vha,
 static int qlt_check_reserve_free_req(struct scsi_qla_host *vha,
 	uint32_t req_cnt)
 {
-	struct qla_hw_data *ha = vha->hw;
-	device_reg_t __iomem *reg = ha->iobase;
-	uint32_t cnt;
+	uint32_t cnt, cnt_in;
 
 	if (vha->req->cnt < (req_cnt + 2)) {
-		cnt = (uint16_t)RD_REG_DWORD(&reg->isp24.req_q_out);
+		cnt = (uint16_t)RD_REG_DWORD(vha->req->req_q_out);
+		cnt_in = (uint16_t)RD_REG_DWORD(vha->req->req_q_in);
 
-		ql_dbg(ql_dbg_tgt, vha, 0xe00a,
-		    "Request ring circled: cnt=%d, vha->->ring_index=%d, "
-		    "vha->req->cnt=%d, req_cnt=%d\n", cnt,
-		    vha->req->ring_index, vha->req->cnt, req_cnt);
 		if  (vha->req->ring_index < cnt)
 			vha->req->cnt = cnt - vha->req->ring_index;
 		else
@@ -1450,11 +1501,10 @@ static int qlt_check_reserve_free_req(struct scsi_qla_host *vha,
 	}
 
 	if (unlikely(vha->req->cnt < (req_cnt + 2))) {
-		ql_dbg(ql_dbg_tgt, vha, 0xe00b,
-		    "qla_target(%d): There is no room in the "
-		    "request ring: vha->req->ring_index=%d, vha->req->cnt=%d, "
-		    "req_cnt=%d\n", vha->vp_idx, vha->req->ring_index,
-		    vha->req->cnt, req_cnt);
+		ql_dbg(ql_dbg_io, vha, 0x305a,
+		    "qla_target(%d): There is no room in the request ring: vha->req->ring_index=%d, vha->req->cnt=%d, req_cnt=%d Req-out=%d Req-in=%d Req-Length=%d\n",
+		    vha->vp_idx, vha->req->ring_index,
+		    vha->req->cnt, req_cnt, cnt, cnt_in, vha->req->length);
 		return -EAGAIN;
 	}
 	vha->req->cnt -= req_cnt;
@@ -1491,7 +1541,7 @@ static inline uint32_t qlt_make_handle(struct scsi_qla_host *vha)
 		if (h > DEFAULT_OUTSTANDING_COMMANDS)
 			h = 1; /* 0 is QLA_TGT_NULL_HANDLE */
 		if (h == ha->tgt.current_handle) {
-			ql_dbg(ql_dbg_tgt, vha, 0xe04e,
+			ql_dbg(ql_dbg_io, vha, 0x305b,
 			    "qla_target(%d): Ran out of "
 			    "empty cmd slots in ha %p\n", vha->vp_idx, ha);
 			h = QLA_TGT_NULL_HANDLE;
@@ -1548,9 +1598,6 @@ static int qlt_24xx_build_ctio_pkt(struct qla_tgt_prm *prm,
 	pkt->u.status0.ox_id = cpu_to_le16(temp);
 	pkt->u.status0.relative_offset = cpu_to_le32(prm->cmd->offset);
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe00c,
-	    "qla_target(%d): handle(cmd) -> %08x, timeout %d, ox_id %#x\n",
-	    vha->vp_idx, pkt->handle, QLA_TGT_TIMEOUT, temp);
 	return 0;
 }
 
@@ -1608,14 +1655,6 @@ static void qlt_load_cont_data_segments(struct qla_tgt_prm *prm,
 			}
 			*dword_ptr++ = cpu_to_le32(sg_dma_len(prm->sg));
 
-			ql_dbg(ql_dbg_tgt, vha, 0xe00d,
-			    "S/G Segment Cont. phys_addr=%llx:%llx, len=%d\n",
-			    (long long unsigned int)
-			    pci_dma_hi32(sg_dma_address(prm->sg)),
-			    (long long unsigned int)
-			    pci_dma_lo32(sg_dma_address(prm->sg)),
-			    (int)sg_dma_len(prm->sg));
-
 			prm->sg = sg_next(prm->sg);
 		}
 	}
@@ -1633,11 +1672,6 @@ static void qlt_load_data_segments(struct qla_tgt_prm *prm,
 	int enable_64bit_addressing = prm->tgt->tgt_enable_64bit_addr;
 	struct ctio7_to_24xx *pkt24 = (struct ctio7_to_24xx *)prm->pkt;
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe00e,
-	    "iocb->scsi_status=%x, iocb->flags=%x\n",
-	    le16_to_cpu(pkt24->u.status0.scsi_status),
-	    le16_to_cpu(pkt24->u.status0.flags));
-
 	pkt24->u.status0.transfer_length = cpu_to_le32(prm->cmd->bufflen);
 
 	/* Setup packet address segment pointer */
@@ -1655,7 +1689,6 @@ static void qlt_load_data_segments(struct qla_tgt_prm *prm,
 	}
 
 	/* If scatter gather */
-	ql_dbg(ql_dbg_tgt, vha, 0xe00f, "%s", "Building S/G data segments...");
 
 	/* Load command entry data segments */
 	for (cnt = 0;
@@ -1670,14 +1703,6 @@ static void qlt_load_data_segments(struct qla_tgt_prm *prm,
 		}
 		*dword_ptr++ = cpu_to_le32(sg_dma_len(prm->sg));
 
-		ql_dbg(ql_dbg_tgt, vha, 0xe010,
-		    "S/G Segment phys_addr=%llx:%llx, len=%d\n",
-		    (long long unsigned int)pci_dma_hi32(sg_dma_address(
-		    prm->sg)),
-		    (long long unsigned int)pci_dma_lo32(sg_dma_address(
-		    prm->sg)),
-		    (int)sg_dma_len(prm->sg));
-
 		prm->sg = sg_next(prm->sg);
 	}
 
@@ -1708,6 +1733,7 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 		    se_cmd, cmd->tag);
 
 		cmd->state = QLA_TGT_STATE_ABORTED;
+		cmd->cmd_flags |= BIT_6;
 
 		qlt_send_term_exchange(vha, cmd, &cmd->atio, 0);
 
@@ -1715,10 +1741,6 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 		return QLA_TGT_PRE_XMIT_RESP_CMD_ABORTED;
 	}
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe011, "qla_target(%d): tag=%u ox_id %04x\n",
-		vha->vp_idx, cmd->tag,
-		be16_to_cpu(cmd->atio.u.isp24.fcp_hdr.ox_id));
-
 	prm->cmd = cmd;
 	prm->tgt = tgt;
 	prm->rq_result = scsi_status;
@@ -1729,15 +1751,10 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 	prm->req_cnt = 1;
 	prm->add_status_pkt = 0;
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe012, "rq_result=%x, xmit_type=%x\n",
-	    prm->rq_result, xmit_type);
-
 	/* Send marker if required */
 	if (qlt_issue_marker(vha, 0) != QLA_SUCCESS)
 		return -EFAULT;
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe013, "CTIO start: vha(%d)\n", vha->vp_idx);
-
 	if ((xmit_type & QLA_TGT_XMIT_DATA) && qlt_has_data(cmd)) {
 		if  (qlt_pci_map_calc_cnt(prm) != 0)
 			return -EAGAIN;
@@ -1747,7 +1764,7 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 
 	if (se_cmd->se_cmd_flags & SCF_UNDERFLOW_BIT) {
 		prm->residual = se_cmd->residual_count;
-		ql_dbg(ql_dbg_tgt, vha, 0xe014,
+		ql_dbg(ql_dbg_io + ql_dbg_verbose, vha, 0x305c,
 		    "Residual underflow: %d (tag %d, "
 		    "op %x, bufflen %d, rq_result %x)\n", prm->residual,
 		    cmd->tag, se_cmd->t_task_cdb ? se_cmd->t_task_cdb[0] : 0,
@@ -1755,7 +1772,7 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 		prm->rq_result |= SS_RESIDUAL_UNDER;
 	} else if (se_cmd->se_cmd_flags & SCF_OVERFLOW_BIT) {
 		prm->residual = se_cmd->residual_count;
-		ql_dbg(ql_dbg_tgt, vha, 0xe015,
+		ql_dbg(ql_dbg_io, vha, 0x305d,
 		    "Residual overflow: %d (tag %d, "
 		    "op %x, bufflen %d, rq_result %x)\n", prm->residual,
 		    cmd->tag, se_cmd->t_task_cdb ? se_cmd->t_task_cdb[0] : 0,
@@ -1778,10 +1795,6 @@ static int qlt_pre_xmit_response(struct qla_tgt_cmd *cmd,
 		}
 	}
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe016,
-	    "req_cnt=%d, full_req_cnt=%d, add_status_pkt=%d\n",
-	    prm->req_cnt, *full_req_cnt, prm->add_status_pkt);
-
 	return 0;
 }
 
@@ -2310,6 +2323,21 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 
+	if (qla2x00_reset_active(vha) || cmd->reset_count != ha->chip_reset) {
+		/*
+		 * Either a chip reset is active or this request was from
+		 * previous life, just abort the processing.
+		 */
+		cmd->state = QLA_TGT_STATE_PROCESSED;
+		qlt_abort_cmd_on_host_reset(cmd->vha, cmd);
+		ql_dbg(ql_dbg_async, vha, 0xe101,
+			"RESET-RSP active/old-count/new-count = %d/%d/%d.\n",
+			qla2x00_reset_active(vha), cmd->reset_count,
+			ha->chip_reset);
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		return 0;
+	}
+
 	/* Does F/W have an IOCBs for this request */
 	res = qlt_check_reserve_free_req(vha, full_req_cnt);
 	if (unlikely(res))
@@ -2358,8 +2386,9 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 			struct ctio7_to_24xx *ctio =
 				(struct ctio7_to_24xx *)qlt_get_req_pkt(vha);
 
-			ql_dbg(ql_dbg_tgt, vha, 0xe019,
-			    "Building additional status packet\n");
+			ql_dbg(ql_dbg_io, vha, 0x305e,
+			    "Building additional status packet 0x%p.\n",
+			    ctio);
 
 			/*
 			 * T10Dif: ctio_crc2_to_fw overlay ontop of
@@ -2391,19 +2420,17 @@ int qlt_xmit_response(struct qla_tgt_cmd *cmd, int xmit_type,
 
 
 	cmd->state = QLA_TGT_STATE_PROCESSED; /* Mid-level is done processing */
+	cmd->cmd_sent_to_fw = 1;
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe01a,
-	    "Xmitting CTIO7 response pkt for 24xx: %p scsi_status: 0x%02x\n",
-	    pkt, scsi_status);
-
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(vha, vha->req);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
 	return 0;
 
 out_unmap_unlock:
-	if (cmd->sg_mapped)
-		qlt_unmap_sg(vha, cmd);
+	qlt_unmap_sg(vha, cmd);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
 	return res;
@@ -2430,17 +2457,27 @@ int qlt_rdy_to_xfer(struct qla_tgt_cmd *cmd)
 	if (qlt_issue_marker(vha, 0) != QLA_SUCCESS)
 		return -EIO;
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe01b,
-		"%s: CTIO_start: vha(%d) se_cmd %p ox_id %04x\n",
-		__func__, (int)vha->vp_idx, &cmd->se_cmd,
-		be16_to_cpu(cmd->atio.u.isp24.fcp_hdr.ox_id));
-
 	/* Calculate number of entries and segments required */
 	if (qlt_pci_map_calc_cnt(&prm) != 0)
 		return -EAGAIN;
 
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 
+	if (qla2x00_reset_active(vha) || cmd->reset_count != ha->chip_reset) {
+		/*
+		 * Either a chip reset is active or this request was from
+		 * previous life, just abort the processing.
+		 */
+		cmd->state = QLA_TGT_STATE_NEED_DATA;
+		qlt_abort_cmd_on_host_reset(cmd->vha, cmd);
+		ql_dbg(ql_dbg_async, vha, 0xe102,
+			"RESET-XFR active/old-count/new-count = %d/%d/%d.\n",
+			qla2x00_reset_active(vha), cmd->reset_count,
+			ha->chip_reset);
+		spin_unlock_irqrestore(&ha->hardware_lock, flags);
+		return 0;
+	}
+
 	/* Does F/W have an IOCBs for this request */
 	res = qlt_check_reserve_free_req(vha, prm.req_cnt);
 	if (res != 0)
@@ -2460,15 +2497,17 @@ int qlt_rdy_to_xfer(struct qla_tgt_cmd *cmd)
 		qlt_load_data_segments(&prm, vha);
 
 	cmd->state = QLA_TGT_STATE_NEED_DATA;
+	cmd->cmd_sent_to_fw = 1;
 
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(vha, vha->req);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
 	return res;
 
 out_unlock_free_unmap:
-	if (cmd->sg_mapped)
-		qlt_unmap_sg(vha, cmd);
+	qlt_unmap_sg(vha, cmd);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 
 	return res;
@@ -2503,7 +2542,7 @@ qlt_handle_dif_error(struct scsi_qla_host *vha, struct qla_tgt_cmd *cmd,
 	    "iocb(s) %p Returned STATUS.\n", sts);
 
 	ql_dbg(ql_dbg_tgt, vha, 0xf075,
-	    "dif check TGT cdb 0x%x lba 0x%llu: [Actual|Expected] Ref Tag[0x%x|0x%x], App Tag [0x%x|0x%x], Guard [0x%x|0x%x]\n",
+	    "dif check TGT cdb 0x%x lba 0x%llx: [Actual|Expected] Ref Tag[0x%x|0x%x], App Tag [0x%x|0x%x], Guard [0x%x|0x%x]\n",
 	    cmd->atio.u.isp24.fcp_cmnd.cdb[0], lba,
 	    a_ref_tag, e_ref_tag, a_app_tag, e_app_tag, a_guard, e_guard);
 
@@ -2626,7 +2665,7 @@ static int __qlt_send_term_exchange(struct scsi_qla_host *vha,
 
 	ql_dbg(ql_dbg_tgt, vha, 0xe01c, "Sending TERM EXCH CTIO (ha=%p)\n", ha);
 
-	pkt = (request_t *)qla2x00_alloc_iocbs(vha, NULL);
+	pkt = (request_t *)qla2x00_alloc_iocbs_ready(vha, NULL);
 	if (pkt == NULL) {
 		ql_dbg(ql_dbg_tgt, vha, 0xe050,
 		    "qla_target(%d): %s failed: unable to allocate "
@@ -2669,6 +2708,8 @@ static int __qlt_send_term_exchange(struct scsi_qla_host *vha,
 	if (ctio24->u.status1.residual != 0)
 		ctio24->u.status1.scsi_status |= SS_RESIDUAL_UNDER;
 
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(vha, vha->req);
 	return ret;
 }
@@ -2684,34 +2725,75 @@ static void qlt_send_term_exchange(struct scsi_qla_host *vha,
 
 	if (ha_locked) {
 		rc = __qlt_send_term_exchange(vha, cmd, atio);
+		if (rc == -ENOMEM)
+			qlt_alloc_qfull_cmd(vha, atio, 0, 0);
 		goto done;
 	}
 	spin_lock_irqsave(&vha->hw->hardware_lock, flags);
 	rc = __qlt_send_term_exchange(vha, cmd, atio);
+	if (rc == -ENOMEM)
+		qlt_alloc_qfull_cmd(vha, atio, 0, 0);
 	spin_unlock_irqrestore(&vha->hw->hardware_lock, flags);
+
 done:
-	/*
-	 * Terminate exchange will tell fw to release any active CTIO
-	 * that's in FW posession and cleanup the exchange.
-	 *
-	 * "cmd->state == QLA_TGT_STATE_ABORTED" means CTIO is still
-	 * down at FW.  Free the cmd later when CTIO comes back later
-	 * w/aborted(0x2) status.
-	 *
-	 * "cmd->state != QLA_TGT_STATE_ABORTED" means CTIO is already
-	 * back w/some err.  Free the cmd now.
-	 */
-	if ((rc == 1) && (cmd->state != QLA_TGT_STATE_ABORTED)) {
+	if (cmd && ((cmd->state != QLA_TGT_STATE_ABORTED) ||
+	    !cmd->cmd_sent_to_fw)) {
 		if (!ha_locked && !in_interrupt())
 			msleep(250); /* just in case */
 
-		if (cmd->sg_mapped)
-			qlt_unmap_sg(vha, cmd);
+		qlt_unmap_sg(vha, cmd);
 		vha->hw->tgt.tgt_ops->free_cmd(cmd);
 	}
 	return;
 }
 
+static void qlt_init_term_exchange(struct scsi_qla_host *vha)
+{
+	struct list_head free_list;
+	struct qla_tgt_cmd *cmd, *tcmd;
+
+	vha->hw->tgt.leak_exchg_thresh_hold =
+	    (vha->hw->fw_xcb_count/100) * LEAK_EXCHG_THRESH_HOLD_PERCENT;
+
+	cmd = tcmd = NULL;
+	if (!list_empty(&vha->hw->tgt.q_full_list)) {
+		INIT_LIST_HEAD(&free_list);
+		list_splice_init(&vha->hw->tgt.q_full_list, &free_list);
+
+		list_for_each_entry_safe(cmd, tcmd, &free_list, cmd_list) {
+			list_del(&cmd->cmd_list);
+			/* This cmd was never sent to TCM.  There is no need
+			 * to schedule free or call free_cmd
+			 */
+			qlt_free_cmd(cmd);
+			vha->hw->tgt.num_qfull_cmds_alloc--;
+		}
+	}
+	vha->hw->tgt.num_qfull_cmds_dropped = 0;
+}
+
+static void qlt_chk_exch_leak_thresh_hold(struct scsi_qla_host *vha)
+{
+	uint32_t total_leaked;
+
+	total_leaked = vha->hw->tgt.num_qfull_cmds_dropped;
+
+	if (vha->hw->tgt.leak_exchg_thresh_hold &&
+	    (total_leaked > vha->hw->tgt.leak_exchg_thresh_hold)) {
+
+		ql_dbg(ql_dbg_tgt, vha, 0xe079,
+		    "Chip reset due to exchange starvation: %d/%d.\n",
+		    total_leaked, vha->hw->fw_xcb_count);
+
+		if (IS_P3P_TYPE(vha->hw))
+			set_bit(FCOE_CTX_RESET_NEEDED, &vha->dpc_flags);
+		else
+			set_bit(ISP_ABORT_NEEDED, &vha->dpc_flags);
+		qla2xxx_wake_dpc(vha);
+	}
+
+}
+
 void qlt_free_cmd(struct qla_tgt_cmd *cmd)
 {
 	struct qla_tgt_sess *sess = cmd->sess;
@@ -2721,7 +2803,13 @@ void qlt_free_cmd(struct qla_tgt_cmd *cmd)
 	    __func__, &cmd->se_cmd,
 	    be16_to_cpu(cmd->atio.u.isp24.fcp_hdr.ox_id));
 
+	BUG_ON(cmd->cmd_in_wq);
+
+	if (!cmd->q_full)
+		qlt_decr_num_pend_cmds(cmd->vha);
+
 	BUG_ON(cmd->sg_mapped);
+	cmd->jiffies_at_free = get_jiffies_64();
 	if (unlikely(cmd->free_sg))
 		kfree(cmd->sg);
 
@@ -2729,6 +2817,7 @@ void qlt_free_cmd(struct qla_tgt_cmd *cmd)
 		WARN_ON(1);
 		return;
 	}
+	cmd->jiffies_at_free = get_jiffies_64();
 	percpu_ida_free(&sess->se_sess->sess_tag_pool, cmd->se_cmd.map_tag);
 }
 EXPORT_SYMBOL(qlt_free_cmd);
@@ -2742,6 +2831,7 @@ static int qlt_prepare_srr_ctio(struct scsi_qla_host *vha,
 	struct qla_tgt_srr_imm *imm;
 
 	tgt->ctio_srr_id++;
+	cmd->cmd_flags |= BIT_15;
 
 	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf019,
 	    "qla_target(%d): CTIO with SRR status received\n", vha->vp_idx);
@@ -2863,11 +2953,9 @@ static struct qla_tgt_cmd *qlt_ctio_to_cmd(struct scsi_qla_host *vha,
 	    CTIO_INTERMEDIATE_HANDLE_MARK);
 
 	if (handle != QLA_TGT_NULL_HANDLE) {
-		if (unlikely(handle == QLA_TGT_SKIP_HANDLE)) {
-			ql_dbg(ql_dbg_tgt, vha, 0xe01d, "%s",
-			    "SKIP_HANDLE CTIO\n");
+		if (unlikely(handle == QLA_TGT_SKIP_HANDLE))
 			return NULL;
-		}
+
 		/* handle-1 is actually used */
 		if (unlikely(handle > DEFAULT_OUTSTANDING_COMMANDS)) {
 			ql_dbg(ql_dbg_tgt, vha, 0xe052,
@@ -2894,6 +2982,81 @@ static struct qla_tgt_cmd *qlt_ctio_to_cmd(struct scsi_qla_host *vha,
 	return cmd;
 }
 
+/* hardware_lock should be held by caller. */
+static void
+qlt_abort_cmd_on_host_reset(struct scsi_qla_host *vha, struct qla_tgt_cmd *cmd)
+{
+	struct qla_hw_data *ha = vha->hw;
+	uint32_t handle;
+
+	if (cmd->sg_mapped)
+		qlt_unmap_sg(vha, cmd);
+
+	handle = qlt_make_handle(vha);
+
+	/* TODO: fix debug message type and ids. */
+	if (cmd->state == QLA_TGT_STATE_PROCESSED) {
+		ql_dbg(ql_dbg_io, vha, 0xff00,
+		    "HOST-ABORT: handle=%d, state=PROCESSED.\n", handle);
+	} else if (cmd->state == QLA_TGT_STATE_NEED_DATA) {
+		cmd->write_data_transferred = 0;
+		cmd->state = QLA_TGT_STATE_DATA_IN;
+
+		ql_dbg(ql_dbg_io, vha, 0xff01,
+		    "HOST-ABORT: handle=%d, state=DATA_IN.\n", handle);
+
+		ha->tgt.tgt_ops->handle_data(cmd);
+		return;
+	} else if (cmd->state == QLA_TGT_STATE_ABORTED) {
+		ql_dbg(ql_dbg_io, vha, 0xff02,
+		    "HOST-ABORT: handle=%d, state=ABORTED.\n", handle);
+	} else {
+		ql_dbg(ql_dbg_io, vha, 0xff03,
+		    "HOST-ABORT: handle=%d, state=BAD(%d).\n", handle,
+		    cmd->state);
+		dump_stack();
+	}
+
+	cmd->cmd_flags |= BIT_12;
+	ha->tgt.tgt_ops->free_cmd(cmd);
+}
+
+void
+qlt_host_reset_handler(struct qla_hw_data *ha)
+{
+	struct qla_tgt_cmd *cmd;
+	unsigned long flags;
+	scsi_qla_host_t *base_vha = pci_get_drvdata(ha->pdev);
+	scsi_qla_host_t *vha = NULL;
+	struct qla_tgt *tgt = base_vha->vha_tgt.qla_tgt;
+	uint32_t i;
+
+	if (!base_vha->hw->tgt.tgt_ops)
+		return;
+
+	if (!tgt || qla_ini_mode_enabled(base_vha)) {
+		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf003,
+			"Target mode disabled\n");
+		return;
+	}
+
+	ql_dbg(ql_dbg_tgt_mgt, vha, 0xff10,
+	    "HOST-ABORT-HNDLR: base_vha->dpc_flags=%lx.\n",
+	    base_vha->dpc_flags);
+
+	spin_lock_irqsave(&ha->hardware_lock, flags);
+	for (i = 1; i < DEFAULT_OUTSTANDING_COMMANDS + 1; i++) {
+		cmd = qlt_get_cmd(base_vha, i);
+		if (!cmd)
+			continue;
+		/* ha->tgt.cmds entry is cleared by qlt_get_cmd. */
+		vha = cmd->vha;
+		qlt_abort_cmd_on_host_reset(vha, cmd);
+	}
+	spin_unlock_irqrestore(&ha->hardware_lock, flags);
+}
+
+
 /*
  * ha->hardware_lock supposed to be held on entry. Might drop it, then reaquire
  */
@@ -2905,10 +3068,6 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 	struct target_core_fabric_ops *tfo;
 	struct qla_tgt_cmd *cmd;
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe01e,
-	    "qla_target(%d): handle(ctio %p status %#x) <- %08x\n",
-	    vha->vp_idx, ctio, status, handle);
-
 	if (handle & CTIO_INTERMEDIATE_HANDLE_MARK) {
 		/* That could happen only in case of an error/reset/abort */
 		if (status != CTIO_SUCCESS) {
@@ -2925,9 +3084,9 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 
 	se_cmd = &cmd->se_cmd;
 	tfo = se_cmd->se_tfo;
+	cmd->cmd_sent_to_fw = 0;
 
-	if (cmd->sg_mapped)
-		qlt_unmap_sg(vha, cmd);
+	qlt_unmap_sg(vha, cmd);
 
 	if (unlikely(status != CTIO_SUCCESS)) {
 		switch (status & 0xFFFF) {
@@ -3011,7 +3170,8 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 		 * level.
 		 */
 		if ((cmd->state != QLA_TGT_STATE_NEED_DATA) &&
-			(cmd->state != QLA_TGT_STATE_ABORTED)) {
+		    (cmd->state != QLA_TGT_STATE_ABORTED)) {
+			cmd->cmd_flags |= BIT_13;
 			if (qlt_term_ctio_exchange(vha, ctio, cmd, status))
 				return;
 		}
@@ -3019,7 +3179,7 @@ static void qlt_do_ctio_completion(struct scsi_qla_host *vha, uint32_t handle,
 skip_term:
 
 	if (cmd->state == QLA_TGT_STATE_PROCESSED) {
-		ql_dbg(ql_dbg_tgt, vha, 0xe01f, "Command %p finished\n", cmd);
+		;
 	} else if (cmd->state == QLA_TGT_STATE_NEED_DATA) {
 		int rx_status = 0;
 
@@ -3030,10 +3190,6 @@ skip_term:
 		else
 			cmd->write_data_transferred = 1;
 
-		ql_dbg(ql_dbg_tgt, vha, 0xe020,
-		    "Data received, context %x, rx_status %d\n",
-		    0x0, rx_status);
-
 		ha->tgt.tgt_ops->handle_data(cmd);
 		return;
 	} else if (cmd->state == QLA_TGT_STATE_ABORTED) {
@@ -3051,6 +3207,7 @@ skip_term:
 		dump_stack();
 	}
 
+
 	ha->tgt.tgt_ops->free_cmd(cmd);
 }
 
@@ -3103,6 +3260,8 @@ static void __qlt_do_work(struct qla_tgt_cmd *cmd)
 	uint32_t data_length;
 	int ret, fcp_task_attr, data_dir, bidi = 0;
 
+	cmd->cmd_in_wq = 0;
+	cmd->cmd_flags |= BIT_1;
 	if (tgt->tgt_stop)
 		goto out_term;
 
@@ -3128,11 +3287,6 @@ static void __qlt_do_work(struct qla_tgt_cmd *cmd)
 	    &atio->u.isp24.fcp_cmnd.add_cdb[
 	    atio->u.isp24.fcp_cmnd.add_cdb_len]));
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe022,
-		"qla_target: START qla cmd: %p se_cmd %p lun: 0x%04x (tag %d) len(%d) ox_id %x\n",
-		cmd, &cmd->se_cmd, cmd->unpacked_lun, cmd->tag, data_length,
-		cmd->atio.u.isp24.fcp_hdr.ox_id);
-
 	ret = ha->tgt.tgt_ops->handle_cmd(vha, cmd, cdb, data_length,
 				          fcp_task_attr, data_dir, bidi);
 	if (ret != 0)
@@ -3146,13 +3300,16 @@ static void __qlt_do_work(struct qla_tgt_cmd *cmd)
 	return;
 
 out_term:
-	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf020, "Terminating work cmd %p", cmd);
+	ql_dbg(ql_dbg_io, vha, 0x3060, "Terminating work cmd %p", cmd);
 	/*
 	 * cmd has not sent to target yet, so pass NULL as the second
 	 * argument to qlt_send_term_exchange() and free the memory here.
 	 */
+	cmd->cmd_flags |= BIT_2;
 	spin_lock_irqsave(&ha->hardware_lock, flags);
 	qlt_send_term_exchange(vha, NULL, &cmd->atio, 1);
+
+	qlt_decr_num_pend_cmds(vha);
 	percpu_ida_free(&sess->se_sess->sess_tag_pool, cmd->se_cmd.map_tag);
 	ha->tgt.tgt_ops->put_sess(sess);
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
@@ -3183,6 +3340,7 @@ static struct qla_tgt_cmd *qlt_get_tag(scsi_qla_host_t *vha,
 	memcpy(&cmd->atio, atio, sizeof(*atio));
 	cmd->state = QLA_TGT_STATE_NEW;
 	cmd->tgt = vha->vha_tgt.qla_tgt;
+	qlt_incr_num_pend_cmds(vha);
 	cmd->vha = vha;
 	cmd->se_cmd.map_tag = tag;
 	cmd->sess = sess;
@@ -3264,7 +3422,7 @@ static int qlt_handle_cmd_for_atio(struct scsi_qla_host *vha,
 	struct qla_tgt_cmd *cmd;
 
 	if (unlikely(tgt->tgt_stop)) {
-		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf021,
+		ql_dbg(ql_dbg_io, vha, 0x3061,
 		    "New command while device %p is shutting down\n", tgt);
 		return -EFAULT;
 	}
@@ -3277,6 +3435,7 @@ static int qlt_handle_cmd_for_atio(struct scsi_qla_host *vha,
 			return -ENOMEM;
 
 		memcpy(&op->atio, atio, sizeof(*atio));
+		op->vha = vha;
 		INIT_WORK(&op->work, qlt_create_sess_from_atio);
 		queue_work(qla_tgt_wq, &op->work);
 		return 0;
@@ -3288,12 +3447,19 @@ static int qlt_handle_cmd_for_atio(struct scsi_qla_host *vha,
 
 	cmd = qlt_get_tag(vha, sess, atio);
 	if (!cmd) {
-		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf05e,
+		ql_dbg(ql_dbg_io, vha, 0x3062,
 		    "qla_target(%d): Allocation of cmd failed\n", vha->vp_idx);
 		ha->tgt.tgt_ops->put_sess(sess);
 		return -ENOMEM;
 	}
 
+	cmd->cmd_flags = 0;
+	cmd->jiffies_at_alloc = get_jiffies_64();
+
+	cmd->reset_count = vha->hw->chip_reset;
+
+	cmd->cmd_in_wq = 1;
+	cmd->cmd_flags |= BIT_0;
 	INIT_WORK(&cmd->work, qlt_do_work);
 	queue_work(qla_tgt_wq, &cmd->work);
 	return 0;
@@ -3327,6 +3493,7 @@ static int qlt_issue_task_mgmt(struct qla_tgt_sess *sess, uint32_t lun,
 	}
 	mcmd->tmr_func = fn;
 	mcmd->flags = flags;
+	mcmd->reset_count = vha->hw->chip_reset;
 
 	switch (fn) {
 	case QLA_TGT_CLEAR_ACA:
@@ -3462,6 +3629,7 @@ static int __qlt_abort_task(struct scsi_qla_host *vha,
 
 	lun = a->u.isp24.fcp_cmnd.lun;
 	unpacked_lun = scsilun_to_int((struct scsi_lun *)&lun);
+	mcmd->reset_count = vha->hw->chip_reset;
 
 	rc = ha->tgt.tgt_ops->handle_tmr(mcmd, unpacked_lun, TMR_ABORT_TASK,
 	    le16_to_cpu(iocb->u.isp2x.seq_id));
@@ -3753,8 +3921,10 @@ static void qlt_handle_srr(struct scsi_qla_host *vha,
 			qlt_send_notify_ack(vha, ntfy,
 			    0, 0, 0, NOTIFY_ACK_SRR_FLAGS_ACCEPT, 0, 0);
 			spin_unlock_irqrestore(&ha->hardware_lock, flags);
-			if (xmit_type & QLA_TGT_XMIT_DATA)
+			if (xmit_type & QLA_TGT_XMIT_DATA) {
+				cmd->cmd_flags |= BIT_8;
 				qlt_rdy_to_xfer(cmd);
+			}
 		} else {
 			ql_dbg(ql_dbg_tgt_mgt, vha, 0xf066,
 			    "qla_target(%d): SRR for out data for cmd "
@@ -3772,8 +3942,10 @@ static void qlt_handle_srr(struct scsi_qla_host *vha,
 	}
 
 	/* Transmit response in case of status and data-in cases */
-	if (resp)
+	if (resp) {
+		cmd->cmd_flags |= BIT_7;
 		qlt_xmit_response(cmd, xmit_type, se_cmd->scsi_status);
+	}
 
 	return;
 
@@ -3786,8 +3958,10 @@ out_reject:
 	if (cmd->state == QLA_TGT_STATE_NEED_DATA) {
 		cmd->state = QLA_TGT_STATE_DATA_IN;
 		dump_stack();
-	} else
+	} else {
+		cmd->cmd_flags |= BIT_9;
 		qlt_send_term_exchange(vha, cmd, &cmd->atio, 1);
+	}
 	spin_unlock_irqrestore(&ha->hardware_lock, flags);
 }
 
@@ -3901,7 +4075,7 @@ static void qlt_prepare_srr_imm(struct scsi_qla_host *vha,
 
 	tgt->imm_srr_id++;
 
-	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf02d, "qla_target(%d): SRR received\n",
+	ql_log(ql_log_warn, vha, 0xf02d, "qla_target(%d): SRR received\n",
 	    vha->vp_idx);
 
 	imm = kzalloc(sizeof(*imm), GFP_ATOMIC);
@@ -4121,7 +4295,7 @@ static void qlt_handle_imm_notify(struct scsi_qla_host *vha,
  * ha->hardware_lock supposed to be held on entry. Might drop it, then reaquire
  * This function sends busy to ISP 2xxx or 24xx.
  */
-static void qlt_send_busy(struct scsi_qla_host *vha,
+static int __qlt_send_busy(struct scsi_qla_host *vha,
 	struct atio_from_isp *atio, uint16_t status)
 {
 	struct ctio7_to_24xx *ctio24;
@@ -4133,16 +4307,16 @@ static void qlt_send_busy(struct scsi_qla_host *vha,
 	    atio->u.isp24.fcp_hdr.s_id);
 	if (!sess) {
 		qlt_send_term_exchange(vha, NULL, atio, 1);
-		return;
+		return 0;
 	}
 	/* Sending marker isn't necessary, since we called from ISR */
 
 	pkt = (request_t *)qla2x00_alloc_iocbs(vha, NULL);
 	if (!pkt) {
-		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf06e,
+		ql_dbg(ql_dbg_io, vha, 0x3063,
 		    "qla_target(%d): %s failed: unable to allocate "
 		    "request packet", vha->vp_idx, __func__);
-		return;
+		return -ENOMEM;
 	}
 
 	pkt->entry_count = 1;
@@ -4167,13 +4341,192 @@ static void qlt_send_busy(struct scsi_qla_host *vha,
 	 */
 	ctio24->u.status1.ox_id = swab16(atio->u.isp24.fcp_hdr.ox_id);
 	ctio24->u.status1.scsi_status = cpu_to_le16(status);
-	ctio24->u.status1.residual = get_unaligned((uint32_t *)
-	    &atio->u.isp24.fcp_cmnd.add_cdb[
-	    atio->u.isp24.fcp_cmnd.add_cdb_len]);
-	if (ctio24->u.status1.residual != 0)
-		ctio24->u.status1.scsi_status |= SS_RESIDUAL_UNDER;
-
+	/* Memory Barrier */
+	wmb();
 	qla2x00_start_iocbs(vha, vha->req);
+	return 0;
+}
+
+/*
+ * This routine is used to allocate a command for either a QFull condition
+ * (ie reply SAM_STAT_BUSY) or to terminate an exchange that did not go
+ * out previously.
+ */
+static void
+qlt_alloc_qfull_cmd(struct scsi_qla_host *vha,
+	struct atio_from_isp *atio, uint16_t status, int qfull)
+{
+	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
+	struct qla_hw_data *ha = vha->hw;
+	struct qla_tgt_sess *sess;
+	struct se_session *se_sess;
+	struct qla_tgt_cmd *cmd;
+	int tag;
+
+	if (unlikely(tgt->tgt_stop)) {
+		ql_dbg(ql_dbg_io, vha, 0x300a,
+			"New command while device %p is shutting down\n", tgt);
+		return;
+	}
+
+	if ((vha->hw->tgt.num_qfull_cmds_alloc + 1) > MAX_QFULL_CMDS_ALLOC) {
+		vha->hw->tgt.num_qfull_cmds_dropped++;
+		if (vha->hw->tgt.num_qfull_cmds_dropped >
+			vha->hw->qla_stats.stat_max_qfull_cmds_dropped)
+			vha->hw->qla_stats.stat_max_qfull_cmds_dropped =
+				vha->hw->tgt.num_qfull_cmds_dropped;
+
+		ql_dbg(ql_dbg_io, vha, 0x3068,
+			"qla_target(%d): %s: QFull CMD dropped[%d]\n",
+			vha->vp_idx, __func__,
+			vha->hw->tgt.num_qfull_cmds_dropped);
+
+		qlt_chk_exch_leak_thresh_hold(vha);
+		return;
+	}
+
+	sess = ha->tgt.tgt_ops->find_sess_by_s_id
+		(vha, atio->u.isp24.fcp_hdr.s_id);
+	if (!sess)
+		return;
+
+	se_sess = sess->se_sess;
+
+	tag = percpu_ida_alloc(&se_sess->sess_tag_pool, TASK_RUNNING);
+	if (tag < 0)
+		return;
+
+	cmd = &((struct qla_tgt_cmd *)se_sess->sess_cmd_map)[tag];
+	if (!cmd) {
+		ql_dbg(ql_dbg_io, vha, 0x3009,
+			"qla_target(%d): %s: Allocation of cmd failed\n",
+			vha->vp_idx, __func__);
+
+		vha->hw->tgt.num_qfull_cmds_dropped++;
+		if (vha->hw->tgt.num_qfull_cmds_dropped >
+			vha->hw->qla_stats.stat_max_qfull_cmds_dropped)
+			vha->hw->qla_stats.stat_max_qfull_cmds_dropped =
+				vha->hw->tgt.num_qfull_cmds_dropped;
+
+		qlt_chk_exch_leak_thresh_hold(vha);
+		return;
+	}
+
+	memset(cmd, 0, sizeof(struct qla_tgt_cmd));
+
+	qlt_incr_num_pend_cmds(vha);
+	INIT_LIST_HEAD(&cmd->cmd_list);
+	memcpy(&cmd->atio, atio, sizeof(*atio));
+
+	cmd->tgt = vha->vha_tgt.qla_tgt;
+	cmd->vha = vha;
+	cmd->reset_count = vha->hw->chip_reset;
+	cmd->q_full = 1;
+
+	if (qfull) {
+		cmd->q_full = 1;
+		/* NOTE: borrowing the state field to carry the status */
+		cmd->state = status;
+	} else
+		cmd->term_exchg = 1;
+
+	list_add_tail(&cmd->cmd_list, &vha->hw->tgt.q_full_list);
+
+	vha->hw->tgt.num_qfull_cmds_alloc++;
+	if (vha->hw->tgt.num_qfull_cmds_alloc >
+		vha->hw->qla_stats.stat_max_qfull_cmds_alloc)
+		vha->hw->qla_stats.stat_max_qfull_cmds_alloc =
+			vha->hw->tgt.num_qfull_cmds_alloc;
+}
+
+int
+qlt_free_qfull_cmds(struct scsi_qla_host *vha)
+{
+	struct qla_hw_data *ha = vha->hw;
+	unsigned long flags;
+	struct qla_tgt_cmd *cmd, *tcmd;
+	struct list_head free_list;
+	int rc = 0;
+
+	if (list_empty(&ha->tgt.q_full_list))
+		return 0;
+
+	INIT_LIST_HEAD(&free_list);
+
+	spin_lock_irqsave(&vha->hw->hardware_lock, flags);
+
+	if (list_empty(&ha->tgt.q_full_list)) {
+		spin_unlock_irqrestore(&vha->hw->hardware_lock, flags);
+		return 0;
+	}
+
+	list_for_each_entry_safe(cmd, tcmd, &ha->tgt.q_full_list, cmd_list) {
+		if (cmd->q_full)
+			/* cmd->state is a borrowed field to hold status */
+			rc = __qlt_send_busy(vha, &cmd->atio, cmd->state);
+		else if (cmd->term_exchg)
+			rc = __qlt_send_term_exchange(vha, NULL, &cmd->atio);
+
+		if (rc == -ENOMEM)
+			break;
+
+		if (cmd->q_full)
+			ql_dbg(ql_dbg_io, vha, 0x3006,
+			    "%s: busy sent for ox_id[%04x]\n", __func__,
+			    be16_to_cpu(cmd->atio.u.isp24.fcp_hdr.ox_id));
+		else if (cmd->term_exchg)
+			ql_dbg(ql_dbg_io, vha, 0x3007,
+			    "%s: Term exchg sent for ox_id[%04x]\n", __func__,
+			    be16_to_cpu(cmd->atio.u.isp24.fcp_hdr.ox_id));
+		else
+			ql_dbg(ql_dbg_io, vha, 0x3008,
+			    "%s: Unexpected cmd in QFull list %p\n", __func__,
+			    cmd);
+
+		list_del(&cmd->cmd_list);
+		list_add_tail(&cmd->cmd_list, &free_list);
+
+		/* piggy back on hardware_lock for protection */
+		vha->hw->tgt.num_qfull_cmds_alloc--;
+	}
+	spin_unlock_irqrestore(&vha->hw->hardware_lock, flags);
+
+	cmd = NULL;
+
+	list_for_each_entry_safe(cmd, tcmd, &free_list, cmd_list) {
+		list_del(&cmd->cmd_list);
+		/* This cmd was never sent to TCM.  There is no need
+		 * to schedule free or call free_cmd
+		 */
+		qlt_free_cmd(cmd);
+	}
+	return rc;
+}
+
+static void
+qlt_send_busy(struct scsi_qla_host *vha,
+	struct atio_from_isp *atio, uint16_t status)
+{
+	int rc = 0;
+
+	rc = __qlt_send_busy(vha, atio, status);
+	if (rc == -ENOMEM)
+		qlt_alloc_qfull_cmd(vha, atio, status, 1);
+}
+
+static int
+qlt_chk_qfull_thresh_hold(struct scsi_qla_host *vha,
+	struct atio_from_isp *atio)
+{
+	struct qla_hw_data *ha = vha->hw;
+	uint16_t status;
+
+	if (ha->tgt.num_pend_cmds < Q_FULL_THRESH_HOLD(ha))
+		return 0;
+
+	status = temp_sam_status;
+	qlt_send_busy(vha, atio, status);
+	return 1;
 }
 
 /* ha->hardware_lock supposed to be held on entry */
@@ -4186,14 +4539,10 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 	int rc;
 
 	if (unlikely(tgt == NULL)) {
-		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf039,
+		ql_dbg(ql_dbg_io, vha, 0x3064,
 		    "ATIO pkt, but no tgt (ha %p)", ha);
 		return;
 	}
-	ql_dbg(ql_dbg_tgt, vha, 0xe02c,
-	    "qla_target(%d): ATIO pkt %p: type %02x count %02x",
-	    vha->vp_idx, atio, atio->u.raw.entry_type,
-	    atio->u.raw.entry_count);
 	/*
 	 * In tgt_stop mode we also should allow all requests to pass.
 	 * Otherwise, some commands can stuck.
@@ -4203,33 +4552,28 @@ static void qlt_24xx_atio_pkt(struct scsi_qla_host *vha,
 
 	switch (atio->u.raw.entry_type) {
 	case ATIO_TYPE7:
-		ql_dbg(ql_dbg_tgt, vha, 0xe02d,
-		    "ATIO_TYPE7 instance %d, lun %Lx, read/write %d/%d, cdb %x, add_cdb_len %x, data_length %04x, s_id %02x%02x%02x\n",
-		    vha->vp_idx, atio->u.isp24.fcp_cmnd.lun,
-		    atio->u.isp24.fcp_cmnd.rddata,
-		    atio->u.isp24.fcp_cmnd.wrdata,
-		    atio->u.isp24.fcp_cmnd.cdb[0],
-		    atio->u.isp24.fcp_cmnd.add_cdb_len,
-		    be32_to_cpu(get_unaligned((uint32_t *)
-			&atio->u.isp24.fcp_cmnd.add_cdb[
-			atio->u.isp24.fcp_cmnd.add_cdb_len])),
-		    atio->u.isp24.fcp_hdr.s_id[0],
-		    atio->u.isp24.fcp_hdr.s_id[1],
-		    atio->u.isp24.fcp_hdr.s_id[2]);
-
 		if (unlikely(atio->u.isp24.exchange_addr ==
 		    ATIO_EXCHANGE_ADDRESS_UNKNOWN)) {
-			ql_dbg(ql_dbg_tgt, vha, 0xe058,
+			ql_dbg(ql_dbg_io, vha, 0x3065,
 			    "qla_target(%d): ATIO_TYPE7 "
 			    "received with UNKNOWN exchange address, "
 			    "sending QUEUE_FULL\n", vha->vp_idx);
 			qlt_send_busy(vha, atio, SAM_STAT_TASK_SET_FULL);
 			break;
 		}
-		if (likely(atio->u.isp24.fcp_cmnd.task_mgmt_flags == 0))
+
+
+
+		if (likely(atio->u.isp24.fcp_cmnd.task_mgmt_flags == 0)) {
+			rc = qlt_chk_qfull_thresh_hold(vha, atio);
+			if (rc != 0) {
+				tgt->irq_cmd_count--;
+				return;
+			}
 			rc = qlt_handle_cmd_for_atio(vha, atio);
-		else
+		} else {
 			rc = qlt_handle_task_mgmt(vha, atio);
+		}
 		if (unlikely(rc != 0)) {
 			if (rc == -ESRCH) {
 #if 1 /* With TERM EXCHANGE some FC cards refuse to boot */
@@ -4293,11 +4637,6 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 		return;
 	}
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe02f,
-	    "qla_target(%d): response pkt %p: T %02x C %02x S %02x "
-	    "handle %#x\n", vha->vp_idx, pkt, pkt->entry_type,
-	    pkt->entry_count, pkt->entry_status, pkt->handle);
-
 	/*
 	 * In tgt_stop mode we also should allow all requests to pass.
 	 * Otherwise, some commands can stuck.
@@ -4310,9 +4649,6 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 	case CTIO_TYPE7:
 	{
 		struct ctio7_from_24xx *entry = (struct ctio7_from_24xx *)pkt;
-		ql_dbg(ql_dbg_tgt, vha, 0xe030,
-			"CTIO[0x%x] 12/CTIO7 7A/CRC2: instance %d\n",
-			entry->entry_type, vha->vp_idx);
 		qlt_do_ctio_completion(vha, entry->handle,
 		    le16_to_cpu(entry->status)|(pkt->entry_status << 16),
 		    entry);
@@ -4323,15 +4659,6 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 	{
 		struct atio_from_isp *atio = (struct atio_from_isp *)pkt;
 		int rc;
-		ql_dbg(ql_dbg_tgt, vha, 0xe031,
-		    "ACCEPT_TGT_IO instance %d status %04x "
-		    "lun %04x read/write %d data_length %04x "
-		    "target_id %02x rx_id %04x\n ", vha->vp_idx,
-		    le16_to_cpu(atio->u.isp2x.status),
-		    le16_to_cpu(atio->u.isp2x.lun),
-		    atio->u.isp2x.execution_codes,
-		    le32_to_cpu(atio->u.isp2x.data_length), GET_TARGET_ID(ha,
-		    atio), atio->u.isp2x.rx_id);
 		if (atio->u.isp2x.status !=
 		    __constant_cpu_to_le16(ATIO_CDB_VALID)) {
 			ql_dbg(ql_dbg_tgt, vha, 0xe05e,
@@ -4340,10 +4667,12 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 			    le16_to_cpu(atio->u.isp2x.status));
 			break;
 		}
-		ql_dbg(ql_dbg_tgt, vha, 0xe032,
-		    "FCP CDB: 0x%02x, sizeof(cdb): %lu",
-		    atio->u.isp2x.cdb[0], (unsigned long
-		    int)sizeof(atio->u.isp2x.cdb));
+
+		rc = qlt_chk_qfull_thresh_hold(vha, atio);
+		if (rc != 0) {
+			tgt->irq_cmd_count--;
+			return;
+		}
 
 		rc = qlt_handle_cmd_for_atio(vha, atio);
 		if (unlikely(rc != 0)) {
@@ -4376,8 +4705,6 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 	case CONTINUE_TGT_IO_TYPE:
 	{
 		struct ctio_to_2xxx *entry = (struct ctio_to_2xxx *)pkt;
-		ql_dbg(ql_dbg_tgt, vha, 0xe033,
-		    "CONTINUE_TGT_IO: instance %d\n", vha->vp_idx);
 		qlt_do_ctio_completion(vha, entry->handle,
 		    le16_to_cpu(entry->status)|(pkt->entry_status << 16),
 		    entry);
@@ -4387,8 +4714,6 @@ static void qlt_response_pkt(struct scsi_qla_host *vha, response_t *pkt)
 	case CTIO_A64_TYPE:
 	{
 		struct ctio_to_2xxx *entry = (struct ctio_to_2xxx *)pkt;
-		ql_dbg(ql_dbg_tgt, vha, 0xe034, "CTIO_A64: instance %d\n",
-		    vha->vp_idx);
 		qlt_do_ctio_completion(vha, entry->handle,
 		    le16_to_cpu(entry->status)|(pkt->entry_status << 16),
 		    entry);
@@ -4492,11 +4817,6 @@ void qlt_async_event(uint16_t code, struct scsi_qla_host *vha,
 	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
 	int login_code;
 
-	ql_dbg(ql_dbg_tgt, vha, 0xe039,
-	    "scsi(%ld): ha state %d init_done %d oper_mode %d topo %d\n",
-	    vha->host_no, atomic_read(&vha->loop_state), vha->flags.init_done,
-	    ha->operating_mode, ha->current_topology);
-
 	if (!ha->tgt.tgt_ops)
 		return;
 
@@ -4573,11 +4893,6 @@ void qlt_async_event(uint16_t code, struct scsi_qla_host *vha,
 		break;
 
 	default:
-		ql_dbg(ql_dbg_tgt_mgt, vha, 0xf040,
-		    "qla_target(%d): Async event %#x occurred: "
-		    "ignore (m[0]=%x, m[1]=%x, m[2]=%x, m[3]=%x)", vha->vp_idx,
-		    code, le16_to_cpu(mailbox[0]), le16_to_cpu(mailbox[1]),
-		    le16_to_cpu(mailbox[2]), le16_to_cpu(mailbox[3]));
 		break;
 	}
 
@@ -4598,8 +4913,6 @@ static fc_port_t *qlt_get_port_database(struct scsi_qla_host *vha,
 		return NULL;
 	}
 
-	ql_dbg(ql_dbg_tgt_mgt, vha, 0xf041, "loop_id %d", loop_id);
-
 	fcport->loop_id = loop_id;
 
 	rc = qla2x00_get_port_database(vha, fcport, 0);
@@ -4898,6 +5211,10 @@ int qlt_remove_target(struct qla_hw_data *ha, struct scsi_qla_host *vha)
 		qlt_release(vha->vha_tgt.qla_tgt);
 		return 0;
 	}
+
+	/* free left over qfull cmds */
+	qlt_init_term_exchange(vha);
+
 	mutex_lock(&qla_tgt_mutex);
 	list_del(&vha->vha_tgt.qla_tgt->tgt_list_entry);
 	mutex_unlock(&qla_tgt_mutex);
@@ -5024,7 +5341,7 @@ void qlt_lport_deregister(struct scsi_qla_host *vha)
 EXPORT_SYMBOL(qlt_lport_deregister);
 
 /* Must be called under HW lock */
-void qlt_set_mode(struct scsi_qla_host *vha)
+static void qlt_set_mode(struct scsi_qla_host *vha)
 {
 	struct qla_hw_data *ha = vha->hw;
 
@@ -5045,7 +5362,7 @@ void qlt_set_mode(struct scsi_qla_host *vha)
 }
 
 /* Must be called under HW lock */
-void qlt_clear_mode(struct scsi_qla_host *vha)
+static void qlt_clear_mode(struct scsi_qla_host *vha)
 {
 	struct qla_hw_data *ha = vha->hw;
 
@@ -5109,8 +5426,7 @@ EXPORT_SYMBOL(qlt_enable_vha);
  *
  * Disable Target Mode and reset the adapter
  */
-void
-qlt_disable_vha(struct scsi_qla_host *vha)
+static void qlt_disable_vha(struct scsi_qla_host *vha)
 {
 	struct qla_hw_data *ha = vha->hw;
 	struct qla_tgt *tgt = vha->vha_tgt.qla_tgt;
@@ -5295,8 +5611,13 @@ qlt_24xx_config_nvram_stage1(struct scsi_qla_host *vha, struct nvram_24xx *nv)
 		nv->firmware_options_1 &= __constant_cpu_to_le32(~BIT_13);
 		/* Enable initial LIP */
 		nv->firmware_options_1 &= __constant_cpu_to_le32(~BIT_9);
-		/* Enable FC tapes support */
-		nv->firmware_options_2 |= __constant_cpu_to_le32(BIT_12);
+		if (ql2xtgt_tape_enable)
+			/* Enable FC Tape support */
+			nv->firmware_options_2 |= cpu_to_le32(BIT_12);
+		else
+			/* Disable FC Tape support */
+			nv->firmware_options_2 &= cpu_to_le32(~BIT_12);
+
 		/* Disable Full Login after LIP */
 		nv->host_p &= __constant_cpu_to_le32(~BIT_10);
 		/* Enable target PRLI control */
@@ -5378,8 +5699,13 @@ qlt_81xx_config_nvram_stage1(struct scsi_qla_host *vha, struct nvram_81xx *nv)
 		nv->firmware_options_1 &= __constant_cpu_to_le32(~BIT_13);
 		/* Enable initial LIP */
 		nv->firmware_options_1 &= __constant_cpu_to_le32(~BIT_9);
-		/* Enable FC tapes support */
-		nv->firmware_options_2 |= __constant_cpu_to_le32(BIT_12);
+		if (ql2xtgt_tape_enable)
+			/* Enable FC tape support */
+			nv->firmware_options_2 |= cpu_to_le32(BIT_12);
+		else
+			/* Disable FC tape support */
+			nv->firmware_options_2 &= cpu_to_le32(~BIT_12);
+
 		/* Disable Full Login after LIP */
 		nv->host_p &= __constant_cpu_to_le32(~BIT_10);
 		/* Enable target PRLI control */
diff --git a/drivers/scsi/qla2xxx/qla_target.h b/drivers/scsi/qla2xxx/qla_target.h
index d1d24fb0160a..332086776dfe 100644
--- a/drivers/scsi/qla2xxx/qla_target.h
+++ b/drivers/scsi/qla2xxx/qla_target.h
@@ -915,6 +915,10 @@ struct qla_tgt_cmd {
 	unsigned int aborted:1; /* Needed in case of SRR */
 	unsigned int write_data_transferred:1;
 	unsigned int ctx_dsd_alloced:1;
+	unsigned int q_full:1;
+	unsigned int term_exchg:1;
+	unsigned int cmd_sent_to_fw:1;
+	unsigned int cmd_in_wq:1;
 
 	struct scatterlist *sg;	/* cmd data buffer SG vector */
 	int sg_cnt;		/* SG segments count */
@@ -923,10 +927,12 @@ struct qla_tgt_cmd {
 	uint32_t tag;
 	uint32_t unpacked_lun;
 	enum dma_data_direction dma_data_direction;
+	uint32_t reset_count;
 
 	uint16_t loop_id;	/* to save extra sess dereferences */
 	struct qla_tgt *tgt;	/* to save extra sess dereferences */
 	struct scsi_qla_host *vha;
+	struct list_head cmd_list;
 
 	struct atio_from_isp atio;
 	/* t10dif */
@@ -935,6 +941,29 @@ struct qla_tgt_cmd {
 	uint32_t blk_sz;
 	struct crc_context *ctx;
 
+	uint64_t jiffies_at_alloc;
+	uint64_t jiffies_at_free;
+	/* BIT_0 - Atio Arrival / schedule to work
+	 * BIT_1 - qlt_do_work
+	 * BIT_2 - qlt_do work failed
+	 * BIT_3 - xfer rdy/tcm_qla2xxx_write_pending
+	 * BIT_4 - read respond/tcm_qla2xx_queue_data_in
+	 * BIT_5 - status respond / tcm_qla2xx_queue_status
+	 * BIT_6 - tcm request to abort/Term exchange.
+	 *	pre_xmit_response->qlt_send_term_exchange
+	 * BIT_7 - SRR received (qlt_handle_srr->qlt_xmit_response)
+	 * BIT_8 - SRR received (qlt_handle_srr->qlt_rdy_to_xfer)
+	 * BIT_9 - SRR received (qla_handle_srr->qlt_send_term_exchange)
+	 * BIT_10 - Data in - hanlde_data->tcm_qla2xxx_handle_data
+	 * BIT_11 - Data actually going to TCM : tcm_qla2xx_handle_data_work
+	 * BIT_12 - good completion - qlt_ctio_do_completion -->free_cmd
+	 * BIT_13 - Bad completion -
+	 *	qlt_ctio_do_completion --> qlt_term_ctio_exchange
+	 * BIT_14 - Back end data received/sent.
+	 * BIT_15 - SRR prepare ctio
+	 * BIT_16 - complete free
+	 */
+	uint32_t cmd_flags;
 };
 
 struct qla_tgt_sess_work_param {
@@ -958,6 +987,7 @@ struct qla_tgt_mgmt_cmd {
 	struct se_cmd se_cmd;
 	struct work_struct free_work;
 	unsigned int flags;
+	uint32_t reset_count;
 #define QLA24XX_MGMT_SEND_NACK	1
 	union {
 		struct atio_from_isp atio;
@@ -971,11 +1001,11 @@ struct qla_tgt_prm {
 	struct qla_tgt *tgt;
 	void *pkt;
 	struct scatterlist *sg;	/* cmd data buffer SG vector */
+	unsigned char *sense_buffer;
 	int seg_cnt;
 	int req_cnt;
 	uint16_t rq_result;
 	uint16_t scsi_status;
-	unsigned char *sense_buffer;
 	int sense_buffer_len;
 	int residual;
 	int add_status_pkt;
@@ -1003,10 +1033,6 @@ struct qla_tgt_srr_ctio {
 
 
 extern struct qla_tgt_data qla_target;
-/*
- * Internal function prototypes
- */
-void qlt_disable_vha(struct scsi_qla_host *);
 
 /*
  * Function prototypes for qla_target.c logic used by qla2xxx LLD code.
@@ -1019,8 +1045,6 @@ extern void qlt_lport_deregister(struct scsi_qla_host *);
 extern void qlt_unreg_sess(struct qla_tgt_sess *);
 extern void qlt_fc_port_added(struct scsi_qla_host *, fc_port_t *);
 extern void qlt_fc_port_deleted(struct scsi_qla_host *, fc_port_t *);
-extern void qlt_set_mode(struct scsi_qla_host *ha);
-extern void qlt_clear_mode(struct scsi_qla_host *ha);
 extern int __init qlt_init(void);
 extern void qlt_exit(void);
 extern void qlt_update_vp_map(struct scsi_qla_host *, int);
@@ -1053,13 +1077,9 @@ static inline void qla_reverse_ini_mode(struct scsi_qla_host *ha)
 /*
  * Exported symbols from qla_target.c LLD logic used by qla2xxx code..
  */
-extern void qlt_24xx_atio_pkt_all_vps(struct scsi_qla_host *,
-	struct atio_from_isp *);
 extern void qlt_response_pkt_all_vps(struct scsi_qla_host *, response_t *);
 extern int qlt_rdy_to_xfer(struct qla_tgt_cmd *);
 extern int qlt_xmit_response(struct qla_tgt_cmd *, int, uint8_t);
-extern int qlt_rdy_to_xfer_dif(struct qla_tgt_cmd *);
-extern int qlt_xmit_response_dif(struct qla_tgt_cmd *, int, uint8_t);
 extern void qlt_xmit_tm_rsp(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_mcmd(struct qla_tgt_mgmt_cmd *);
 extern void qlt_free_cmd(struct qla_tgt_cmd *cmd);
@@ -1089,5 +1109,6 @@ extern int qlt_stop_phase1(struct qla_tgt *);
 extern void qlt_stop_phase2(struct qla_tgt *);
 extern irqreturn_t qla83xx_msix_atio_q(int, void *);
 extern void qlt_83xx_iospace_config(struct qla_hw_data *);
+extern int qlt_free_qfull_cmds(struct scsi_qla_host *);
 
 #endif /* __QLA_TARGET_H */
diff --git a/drivers/scsi/qla2xxx/qla_tmpl.c b/drivers/scsi/qla2xxx/qla_tmpl.c
index cb9a0c4bc419..a8c0c7362e48 100644
--- a/drivers/scsi/qla2xxx/qla_tmpl.c
+++ b/drivers/scsi/qla2xxx/qla_tmpl.c
@@ -128,18 +128,10 @@ qla27xx_insert32(uint32_t value, void *buf, ulong *len)
 static inline void
 qla27xx_insertbuf(void *mem, ulong size, void *buf, ulong *len)
 {
-	ulong cnt = size;
 
-	if (buf && mem) {
+	if (buf && mem && size) {
 		buf += *len;
-		while (cnt >= sizeof(uint32_t)) {
-			*(__le32 *)buf = cpu_to_le32p(mem);
-			buf += sizeof(uint32_t);
-			mem += sizeof(uint32_t);
-			cnt -= sizeof(uint32_t);
-		}
-		if (cnt)
-			memcpy(buf, mem, cnt);
+		memcpy(buf, mem, size);
 	}
 	*len += size;
 }
@@ -151,8 +143,6 @@ qla27xx_read8(void *window, void *buf, ulong *len)
 
 	if (buf) {
 		value = RD_REG_BYTE((__iomem void *)window);
-		ql_dbg(ql_dbg_misc, NULL, 0xd011,
-		    "%s: -> %x\n", __func__, value);
 	}
 	qla27xx_insert32(value, buf, len);
 }
@@ -164,8 +154,6 @@ qla27xx_read16(void *window, void *buf, ulong *len)
 
 	if (buf) {
 		value = RD_REG_WORD((__iomem void *)window);
-		ql_dbg(ql_dbg_misc, NULL, 0xd012,
-		    "%s: -> %x\n", __func__, value);
 	}
 	qla27xx_insert32(value, buf, len);
 }
@@ -177,8 +165,6 @@ qla27xx_read32(void *window, void *buf, ulong *len)
 
 	if (buf) {
 		value = RD_REG_DWORD((__iomem void *)window);
-		ql_dbg(ql_dbg_misc, NULL, 0xd013,
-		    "%s: -> %x\n", __func__, value);
 	}
 	qla27xx_insert32(value, buf, len);
 }
@@ -197,10 +183,6 @@ qla27xx_read_reg(__iomem struct device_reg_24xx *reg,
 {
 	void *window = (void *)reg + offset;
 
-	if (buf) {
-		ql_dbg(ql_dbg_misc, NULL, 0xd014,
-		    "%s: @%x\n", __func__, offset);
-	}
 	qla27xx_read32(window, buf, len);
 }
 
@@ -211,8 +193,6 @@ qla27xx_write_reg(__iomem struct device_reg_24xx *reg,
 	__iomem void *window = reg + offset;
 
 	if (buf) {
-		ql_dbg(ql_dbg_misc, NULL, 0xd015,
-		    "%s: @%x <- %x\n", __func__, offset, data);
 		WRT_REG_DWORD(window, data);
 	}
 }
@@ -225,11 +205,6 @@ qla27xx_read_window(__iomem struct device_reg_24xx *reg,
 	void *window = (void *)reg + offset;
 	void (*readn)(void *, void *, ulong *) = qla27xx_read_vector(width);
 
-	if (buf) {
-		ql_dbg(ql_dbg_misc, NULL, 0xd016,
-		    "%s: base=%x offset=%x count=%x width=%x\n",
-		    __func__, addr, offset, count, width);
-	}
 	qla27xx_write_reg(reg, IOBASE_ADDR, addr, buf);
 	while (count--) {
 		qla27xx_insert32(addr, buf, len);
@@ -380,14 +355,9 @@ qla27xx_fwdt_entry_t262(struct scsi_qla_host *vha,
 			ent->t262.start_addr = start;
 			ent->t262.end_addr = end;
 		}
-	} else if (ent->t262.ram_area == T262_RAM_AREA_DDR_RAM) {
-		ql_dbg(ql_dbg_misc, vha, 0xd021,
-		    "%s: unsupported ddr ram\n", __func__);
-		qla27xx_skip_entry(ent, buf);
-		goto done;
 	} else {
 		ql_dbg(ql_dbg_misc, vha, 0xd022,
-		    "%s: unknown area %u\n", __func__, ent->t262.ram_area);
+		    "%s: unknown area %x\n", __func__, ent->t262.ram_area);
 		qla27xx_skip_entry(ent, buf);
 		goto done;
 	}
@@ -402,8 +372,6 @@ qla27xx_fwdt_entry_t262(struct scsi_qla_host *vha,
 
 	dwords = end - start + 1;
 	if (buf) {
-		ql_dbg(ql_dbg_misc, vha, 0xd024,
-		    "%s: @%lx -> (%lx dwords)\n", __func__, start, dwords);
 		buf += *len;
 		qla24xx_dump_ram(vha->hw, start, buf, dwords, &buf);
 	}
@@ -448,13 +416,9 @@ qla27xx_fwdt_entry_t263(struct scsi_qla_host *vha,
 				count++;
 			}
 		}
-	} else if (ent->t263.queue_type == T263_QUEUE_TYPE_ATIO) {
-		ql_dbg(ql_dbg_misc, vha, 0xd025,
-		    "%s: unsupported atio queue\n", __func__);
-		qla27xx_skip_entry(ent, buf);
 	} else {
 		ql_dbg(ql_dbg_misc, vha, 0xd026,
-		    "%s: unknown queue %u\n", __func__, ent->t263.queue_type);
+		    "%s: unknown queue %x\n", __func__, ent->t263.queue_type);
 		qla27xx_skip_entry(ent, buf);
 	}
 
@@ -549,17 +513,9 @@ qla27xx_fwdt_entry_t268(struct scsi_qla_host *vha,
 			    "%s: missing eft\n", __func__);
 			qla27xx_skip_entry(ent, buf);
 		}
-	} else if (ent->t268.buf_type == T268_BUF_TYPE_EXCH_BUFOFF) {
-		ql_dbg(ql_dbg_misc, vha, 0xd029,
-		    "%s: unsupported exchange offload buffer\n", __func__);
-		qla27xx_skip_entry(ent, buf);
-	} else if (ent->t268.buf_type == T268_BUF_TYPE_EXTD_LOGIN) {
-		ql_dbg(ql_dbg_misc, vha, 0xd02a,
-		    "%s: unsupported extended login buffer\n", __func__);
-		qla27xx_skip_entry(ent, buf);
 	} else {
 		ql_dbg(ql_dbg_misc, vha, 0xd02b,
-		    "%s: unknown buf %x\n", __func__, ent->t268.buf_type);
+		    "%s: unknown buffer %x\n", __func__, ent->t268.buf_type);
 		qla27xx_skip_entry(ent, buf);
 	}
 
@@ -695,13 +651,9 @@ qla27xx_fwdt_entry_t274(struct scsi_qla_host *vha,
 				count++;
 			}
 		}
-	} else if (ent->t274.queue_type == T274_QUEUE_TYPE_ATIO_SHAD) {
-		ql_dbg(ql_dbg_misc, vha, 0xd02e,
-		    "%s: unsupported atio queue\n", __func__);
-		qla27xx_skip_entry(ent, buf);
 	} else {
 		ql_dbg(ql_dbg_misc, vha, 0xd02f,
-		    "%s: unknown queue %u\n", __func__, ent->t274.queue_type);
+		    "%s: unknown queue %x\n", __func__, ent->t274.queue_type);
 		qla27xx_skip_entry(ent, buf);
 	}
 
@@ -715,6 +667,32 @@ qla27xx_fwdt_entry_t274(struct scsi_qla_host *vha,
 }
 
 static int
+qla27xx_fwdt_entry_t275(struct scsi_qla_host *vha,
+	struct qla27xx_fwdt_entry *ent, void *buf, ulong *len)
+{
+	ulong offset = offsetof(typeof(*ent), t275.buffer);
+
+	ql_dbg(ql_dbg_misc, vha, 0xd213,
+	    "%s: buffer(%x) [%lx]\n", __func__, ent->t275.length, *len);
+	if (!ent->t275.length) {
+		ql_dbg(ql_dbg_misc, vha, 0xd020,
+		    "%s: buffer zero length\n", __func__);
+		qla27xx_skip_entry(ent, buf);
+		goto done;
+	}
+	if (offset + ent->t275.length > ent->hdr.entry_size) {
+		ql_dbg(ql_dbg_misc, vha, 0xd030,
+		    "%s: buffer overflow\n", __func__);
+		qla27xx_skip_entry(ent, buf);
+		goto done;
+	}
+
+	qla27xx_insertbuf(ent->t275.buffer, ent->t275.length, buf, len);
+done:
+	return false;
+}
+
+static int
 qla27xx_fwdt_entry_other(struct scsi_qla_host *vha,
 	struct qla27xx_fwdt_entry *ent, void *buf, ulong *len)
 {
@@ -726,7 +704,7 @@ qla27xx_fwdt_entry_other(struct scsi_qla_host *vha,
 }
 
 struct qla27xx_fwdt_entry_call {
-	int type;
+	uint type;
 	int (*call)(
 	    struct scsi_qla_host *,
 	    struct qla27xx_fwdt_entry *,
@@ -756,18 +734,21 @@ static struct qla27xx_fwdt_entry_call ql27xx_fwdt_entry_call_list[] = {
 	{ ENTRY_TYPE_RDREMRAM		, qla27xx_fwdt_entry_t272  } ,
 	{ ENTRY_TYPE_PCICFG		, qla27xx_fwdt_entry_t273  } ,
 	{ ENTRY_TYPE_GET_SHADOW		, qla27xx_fwdt_entry_t274  } ,
+	{ ENTRY_TYPE_WRITE_BUF		, qla27xx_fwdt_entry_t275  } ,
 	{ -1				, qla27xx_fwdt_entry_other }
 };
 
-static inline int (*qla27xx_find_entry(int type))
+static inline int (*qla27xx_find_entry(uint type))
 	(struct scsi_qla_host *, struct qla27xx_fwdt_entry *, void *, ulong *)
 {
 	struct qla27xx_fwdt_entry_call *list = ql27xx_fwdt_entry_call_list;
 
-	while (list->type != -1 && list->type != type)
+	while (list->type < type)
 		list++;
 
-	return list->call;
+	if (list->type == type)
+		return list->call;
+	return qla27xx_fwdt_entry_other;
 }
 
 static inline void *
@@ -792,6 +773,15 @@ qla27xx_walk_template(struct scsi_qla_host *vha,
 			break;
 		ent = qla27xx_next_entry(ent);
 	}
+
+	if (count)
+		ql_dbg(ql_dbg_misc, vha, 0xd018,
+		    "%s: residual count (%lx)\n", __func__, count);
+
+	if (ent->hdr.entry_type != ENTRY_TYPE_TMP_END)
+		ql_dbg(ql_dbg_misc, vha, 0xd019,
+		    "%s: missing end (%lx)\n", __func__, count);
+
 	ql_dbg(ql_dbg_misc, vha, 0xd01b,
 	    "%s: len=%lx\n", __func__, *len);
 }
diff --git a/drivers/scsi/qla2xxx/qla_tmpl.h b/drivers/scsi/qla2xxx/qla_tmpl.h
index 1967424c8e64..141c1c5e73f4 100644
--- a/drivers/scsi/qla2xxx/qla_tmpl.h
+++ b/drivers/scsi/qla2xxx/qla_tmpl.h
@@ -53,6 +53,7 @@ struct __packed qla27xx_fwdt_template {
 #define ENTRY_TYPE_RDREMRAM		272
 #define ENTRY_TYPE_PCICFG		273
 #define ENTRY_TYPE_GET_SHADOW		274
+#define ENTRY_TYPE_WRITE_BUF		275
 
 #define CAPTURE_FLAG_PHYS_ONLY		BIT_0
 #define CAPTURE_FLAG_PHYS_VIRT		BIT_1
@@ -193,6 +194,11 @@ struct __packed qla27xx_fwdt_entry {
 			uint8_t  queue_type;
 			uint8_t  reserved[3];
 		} t274;
+
+		struct __packed {
+			uint32_t length;
+			uint8_t  buffer[];
+		} t275;
 	};
 };
 
@@ -208,6 +214,8 @@ struct __packed qla27xx_fwdt_entry {
 #define T268_BUF_TYPE_EXTD_TRACE	1
 #define T268_BUF_TYPE_EXCH_BUFOFF	2
 #define T268_BUF_TYPE_EXTD_LOGIN	3
+#define T268_BUF_TYPE_REQ_MIRROR	4
+#define T268_BUF_TYPE_RSP_MIRROR	5
 
 #define T274_QUEUE_TYPE_REQ_SHAD	1
 #define T274_QUEUE_TYPE_RSP_SHAD	2
diff --git a/drivers/scsi/qla2xxx/qla_version.h b/drivers/scsi/qla2xxx/qla_version.h
index 4d2c98cbec4f..d88b86214ec5 100644
--- a/drivers/scsi/qla2xxx/qla_version.h
+++ b/drivers/scsi/qla2xxx/qla_version.h
@@ -7,7 +7,7 @@
 /*
  * Driver version
  */
-#define QLA2XXX_VERSION      "8.07.00.08-k"
+#define QLA2XXX_VERSION      "8.07.00.16-k"
 
 #define QLA_DRIVER_MAJOR_VER	8
 #define QLA_DRIVER_MINOR_VER	7
diff --git a/drivers/scsi/qla2xxx/tcm_qla2xxx.c b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
index e2beab962096..73f9feecda72 100644
--- a/drivers/scsi/qla2xxx/tcm_qla2xxx.c
+++ b/drivers/scsi/qla2xxx/tcm_qla2xxx.c
@@ -50,8 +50,12 @@
 #include "qla_target.h"
 #include "tcm_qla2xxx.h"
 
-struct workqueue_struct *tcm_qla2xxx_free_wq;
-struct workqueue_struct *tcm_qla2xxx_cmd_wq;
+static struct workqueue_struct *tcm_qla2xxx_free_wq;
+static struct workqueue_struct *tcm_qla2xxx_cmd_wq;
+
+/* Local pointer to allocated TCM configfs fabric module */
+static struct target_fabric_configfs *tcm_qla2xxx_fabric_configfs;
+static struct target_fabric_configfs *tcm_qla2xxx_npiv_fabric_configfs;
 
 /*
  * Parse WWN.
@@ -386,6 +390,11 @@ static void tcm_qla2xxx_complete_free(struct work_struct *work)
 {
 	struct qla_tgt_cmd *cmd = container_of(work, struct qla_tgt_cmd, work);
 
+	cmd->cmd_in_wq = 0;
+
+	WARN_ON(cmd->cmd_flags &  BIT_16);
+
+	cmd->cmd_flags |= BIT_16;
 	transport_generic_free_cmd(&cmd->se_cmd, 0);
 }
 
@@ -396,6 +405,7 @@ static void tcm_qla2xxx_complete_free(struct work_struct *work)
  */
 static void tcm_qla2xxx_free_cmd(struct qla_tgt_cmd *cmd)
 {
+	cmd->cmd_in_wq = 1;
 	INIT_WORK(&cmd->work, tcm_qla2xxx_complete_free);
 	queue_work(tcm_qla2xxx_free_wq, &cmd->work);
 }
@@ -405,6 +415,13 @@ static void tcm_qla2xxx_free_cmd(struct qla_tgt_cmd *cmd)
  */
 static int tcm_qla2xxx_check_stop_free(struct se_cmd *se_cmd)
 {
+	struct qla_tgt_cmd *cmd;
+
+	if ((se_cmd->se_cmd_flags & SCF_SCSI_TMR_CDB) == 0) {
+		cmd = container_of(se_cmd, struct qla_tgt_cmd, se_cmd);
+		cmd->cmd_flags |= BIT_14;
+	}
+
 	return target_put_sess_cmd(se_cmd->se_sess, se_cmd);
 }
 
@@ -511,8 +528,13 @@ static void tcm_qla2xxx_set_default_node_attrs(struct se_node_acl *nacl)
 
 static u32 tcm_qla2xxx_get_task_tag(struct se_cmd *se_cmd)
 {
-	struct qla_tgt_cmd *cmd = container_of(se_cmd,
-				struct qla_tgt_cmd, se_cmd);
+	struct qla_tgt_cmd *cmd;
+
+	/* check for task mgmt cmd */
+	if (se_cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)
+		return 0xffffffff;
+
+	cmd = container_of(se_cmd, struct qla_tgt_cmd, se_cmd);
 
 	return cmd->tag;
 }
@@ -562,6 +584,8 @@ static void tcm_qla2xxx_handle_data_work(struct work_struct *work)
 	 * Ensure that the complete FCP WRITE payload has been received.
 	 * Otherwise return an exception via CHECK_CONDITION status.
 	 */
+	cmd->cmd_in_wq = 0;
+	cmd->cmd_flags |= BIT_11;
 	if (!cmd->write_data_transferred) {
 		/*
 		 * Check if se_cmd has already been aborted via LUN_RESET, and
@@ -590,6 +614,8 @@ static void tcm_qla2xxx_handle_data_work(struct work_struct *work)
  */
 static void tcm_qla2xxx_handle_data(struct qla_tgt_cmd *cmd)
 {
+	cmd->cmd_flags |= BIT_10;
+	cmd->cmd_in_wq = 1;
 	INIT_WORK(&cmd->work, tcm_qla2xxx_handle_data_work);
 	queue_work(tcm_qla2xxx_free_wq, &cmd->work);
 }
@@ -633,6 +659,7 @@ static int tcm_qla2xxx_queue_data_in(struct se_cmd *se_cmd)
 	struct qla_tgt_cmd *cmd = container_of(se_cmd,
 				struct qla_tgt_cmd, se_cmd);
 
+	cmd->cmd_flags |= BIT_4;
 	cmd->bufflen = se_cmd->data_length;
 	cmd->dma_data_direction = target_reverse_dma_direction(se_cmd);
 	cmd->aborted = (se_cmd->transport_state & CMD_T_ABORTED);
@@ -640,6 +667,7 @@ static int tcm_qla2xxx_queue_data_in(struct se_cmd *se_cmd)
 	cmd->sg_cnt = se_cmd->t_data_nents;
 	cmd->sg = se_cmd->t_data_sg;
 	cmd->offset = 0;
+	cmd->cmd_flags |= BIT_3;
 
 	cmd->prot_sg_cnt = se_cmd->t_prot_nents;
 	cmd->prot_sg = se_cmd->t_prot_sg;
@@ -665,6 +693,11 @@ static int tcm_qla2xxx_queue_status(struct se_cmd *se_cmd)
 	cmd->offset = 0;
 	cmd->dma_data_direction = target_reverse_dma_direction(se_cmd);
 	cmd->aborted = (se_cmd->transport_state & CMD_T_ABORTED);
+	if (cmd->cmd_flags &  BIT_5) {
+		pr_crit("Bit_5 already set for cmd = %p.\n", cmd);
+		dump_stack();
+	}
+	cmd->cmd_flags |= BIT_5;
 
 	if (se_cmd->data_direction == DMA_FROM_DEVICE) {
 		/*
@@ -734,10 +767,6 @@ static void tcm_qla2xxx_aborted_task(struct se_cmd *se_cmd)
 	cmd->sg_mapped = 0;
 }
 
-/* Local pointer to allocated TCM configfs fabric module */
-struct target_fabric_configfs *tcm_qla2xxx_fabric_configfs;
-struct target_fabric_configfs *tcm_qla2xxx_npiv_fabric_configfs;
-
 static void tcm_qla2xxx_clear_sess_lookup(struct tcm_qla2xxx_lport *,
 			struct tcm_qla2xxx_nacl *, struct qla_tgt_sess *);
 /*
@@ -757,7 +786,16 @@ static void tcm_qla2xxx_clear_nacl_from_fcport_map(struct qla_tgt_sess *sess)
 	pr_debug("fc_rport domain: port_id 0x%06x\n", nacl->nport_id);
 
 	node = btree_remove32(&lport->lport_fcport_map, nacl->nport_id);
-	WARN_ON(node && (node != se_nacl));
+	if (WARN_ON(node && (node != se_nacl))) {
+		/*
+		 * The nacl no longer matches what we think it should be.
+		 * Most likely a new dynamic acl has been added while
+		 * someone dropped the hardware lock.  It clearly is a
+		 * bug elsewhere, but this bit can't make things worse.
+		 */
+		btree_insert32(&lport->lport_fcport_map, nacl->nport_id,
+			       node, GFP_ATOMIC);
+	}
 
 	pr_debug("Removed from fcport_map: %p for WWNN: 0x%016LX, port_id: 0x%06x\n",
 	    se_nacl, nacl->nport_wwnn, nacl->nport_id);
diff --git a/drivers/scsi/scsi.c b/drivers/scsi/scsi.c
index df3306019a7e..79c77b485a67 100644
--- a/drivers/scsi/scsi.c
+++ b/drivers/scsi/scsi.c
@@ -377,6 +377,10 @@ scsi_alloc_host_cmd_pool(struct Scsi_Host *shost)
 		pool->slab_flags |= SLAB_CACHE_DMA;
 		pool->gfp_mask = __GFP_DMA;
 	}
+
+	if (hostt->cmd_size)
+		hostt->cmd_pool = pool;
+
 	return pool;
 }
 
@@ -421,8 +425,10 @@ out:
 out_free_slab:
 	kmem_cache_destroy(pool->cmd_slab);
 out_free_pool:
-	if (hostt->cmd_size)
+	if (hostt->cmd_size) {
 		scsi_free_host_cmd_pool(pool);
+		hostt->cmd_pool = NULL;
+	}
 	goto out;
 }
 
@@ -444,8 +450,10 @@ static void scsi_put_host_cmd_pool(struct Scsi_Host *shost)
 	if (!--pool->users) {
 		kmem_cache_destroy(pool->cmd_slab);
 		kmem_cache_destroy(pool->sense_slab);
-		if (hostt->cmd_size)
+		if (hostt->cmd_size) {
 			scsi_free_host_cmd_pool(pool);
+			hostt->cmd_pool = NULL;
+		}
 	}
 	mutex_unlock(&host_cmd_pool_mutex);
 }
@@ -662,14 +670,10 @@ int scsi_dispatch_cmd(struct scsi_cmnd *cmd)
 		return SCSI_MLQUEUE_DEVICE_BUSY;
 	}
 
-	/*
-	 * If SCSI-2 or lower, store the LUN value in cmnd.
-	 */
-	if (cmd->device->scsi_level <= SCSI_2 &&
-	    cmd->device->scsi_level != SCSI_UNKNOWN) {
+	/* Store the LUN value in cmnd, if needed. */
+	if (cmd->device->lun_in_cdb)
 		cmd->cmnd[1] = (cmd->cmnd[1] & 0x1f) |
 			       (cmd->device->lun << 5 & 0xe0);
-	}
 
 	scsi_log_send(cmd);
 
@@ -1363,7 +1367,11 @@ MODULE_LICENSE("GPL");
 module_param(scsi_logging_level, int, S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(scsi_logging_level, "a bit mask of logging levels");
 
+#ifdef CONFIG_SCSI_MQ_DEFAULT
+bool scsi_use_blk_mq = true;
+#else
 bool scsi_use_blk_mq = false;
+#endif
 module_param_named(use_blk_mq, scsi_use_blk_mq, bool, S_IWUSR | S_IRUGO);
 
 static int __init init_scsi(void)
diff --git a/drivers/scsi/scsi_debug.c b/drivers/scsi/scsi_debug.c
index d19c0e3c7f48..238e06f13b8a 100644
--- a/drivers/scsi/scsi_debug.c
+++ b/drivers/scsi/scsi_debug.c
@@ -123,7 +123,7 @@ static const char *scsi_debug_version_date = "20140706";
 #define DEF_PHYSBLK_EXP 0
 #define DEF_PTYPE   0
 #define DEF_REMOVABLE false
-#define DEF_SCSI_LEVEL   5    /* INQUIRY, byte2 [5->SPC-3] */
+#define DEF_SCSI_LEVEL   6    /* INQUIRY, byte2 [6->SPC-4] */
 #define DEF_SECTOR_SIZE 512
 #define DEF_TAGGED_QUEUING 0 /* 0 | MSG_SIMPLE_TAG | MSG_ORDERED_TAG */
 #define DEF_UNMAP_ALIGNMENT 0
@@ -929,7 +929,7 @@ static int resp_inquiry(struct scsi_cmnd *scp, int target,
 {
 	unsigned char pq_pdt;
 	unsigned char * arr;
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 	int alloc_len, n, ret;
 
 	alloc_len = (cmd[3] << 8) + cmd[4];
@@ -1056,15 +1056,15 @@ static int resp_inquiry(struct scsi_cmnd *scp, int target,
 	memcpy(&arr[16], inq_product_id, 16);
 	memcpy(&arr[32], inq_product_rev, 4);
 	/* version descriptors (2 bytes each) follow */
-	arr[58] = 0x0; arr[59] = 0x77; /* SAM-3 ANSI */
-	arr[60] = 0x3; arr[61] = 0x14;  /* SPC-3 ANSI */
+	arr[58] = 0x0; arr[59] = 0xa2;  /* SAM-5 rev 4 */
+	arr[60] = 0x4; arr[61] = 0x68;  /* SPC-4 rev 37 */
 	n = 62;
 	if (scsi_debug_ptype == 0) {
-		arr[n++] = 0x3; arr[n++] = 0x3d; /* SBC-2 ANSI */
+		arr[n++] = 0x4; arr[n++] = 0xc5; /* SBC-4 rev 36 */
 	} else if (scsi_debug_ptype == 1) {
-		arr[n++] = 0x3; arr[n++] = 0x60; /* SSC-2 no version */
+		arr[n++] = 0x5; arr[n++] = 0x25; /* SSC-4 rev 3 */
 	}
-	arr[n++] = 0xc; arr[n++] = 0xf;  /* SAS-1.1 rev 10 */
+	arr[n++] = 0x20; arr[n++] = 0xe6;  /* SPL-3 rev 7 */
 	ret = fill_from_dev_buffer(scp, arr,
 			    min(alloc_len, SDEBUG_LONG_INQ_SZ));
 	kfree(arr);
@@ -1075,7 +1075,7 @@ static int resp_requests(struct scsi_cmnd * scp,
 			 struct sdebug_dev_info * devip)
 {
 	unsigned char * sbuff;
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 	unsigned char arr[SCSI_SENSE_BUFFERSIZE];
 	int want_dsense;
 	int len = 18;
@@ -1115,7 +1115,7 @@ static int resp_requests(struct scsi_cmnd * scp,
 static int resp_start_stop(struct scsi_cmnd * scp,
 			   struct sdebug_dev_info * devip)
 {
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 	int power_cond, errsts, start;
 
 	errsts = check_readiness(scp, UAS_ONLY, devip);
@@ -1177,7 +1177,7 @@ static int resp_readcap(struct scsi_cmnd * scp,
 static int resp_readcap16(struct scsi_cmnd * scp,
 			  struct sdebug_dev_info * devip)
 {
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 	unsigned char arr[SDEBUG_READCAP16_ARR_SZ];
 	unsigned long long capac;
 	int errsts, k, alloc_len;
@@ -1222,7 +1222,7 @@ static int resp_readcap16(struct scsi_cmnd * scp,
 static int resp_report_tgtpgs(struct scsi_cmnd * scp,
 			      struct sdebug_dev_info * devip)
 {
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 	unsigned char * arr;
 	int host_no = devip->sdbg_host->shost->host_no;
 	int n, ret, alen, rlen;
@@ -1468,7 +1468,7 @@ static int resp_mode_sense(struct scsi_cmnd * scp, int target,
 	int k, alloc_len, msense_6, offset, len, errsts, target_dev_id;
 	unsigned char * ap;
 	unsigned char arr[SDEBUG_MAX_MSENSE_SZ];
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 
 	errsts = check_readiness(scp, UAS_ONLY, devip);
 	if (errsts)
@@ -1630,7 +1630,7 @@ static int resp_mode_select(struct scsi_cmnd * scp, int mselect6,
 	int pf, sp, ps, md_len, bd_len, off, spf, pg_len;
 	int param_len, res, errsts, mpage;
 	unsigned char arr[SDEBUG_MAX_MSELECT_SZ];
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 
 	errsts = check_readiness(scp, UAS_ONLY, devip);
 	if (errsts)
@@ -1739,7 +1739,7 @@ static int resp_log_sense(struct scsi_cmnd * scp,
 {
 	int ppc, sp, pcontrol, pcode, subpcode, alloc_len, errsts, len, n;
 	unsigned char arr[SDEBUG_MAX_LSENSE_SZ];
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 
 	errsts = check_readiness(scp, UAS_ONLY, devip);
 	if (errsts)
@@ -2414,7 +2414,7 @@ static int resp_report_luns(struct scsi_cmnd * scp,
 	unsigned int alloc_len;
 	int lun_cnt, i, upper, num, n;
 	u64 wlun, lun;
-	unsigned char *cmd = (unsigned char *)scp->cmnd;
+	unsigned char *cmd = scp->cmnd;
 	int select_report = (int)cmd[2];
 	struct scsi_lun *one_lun;
 	unsigned char arr[SDEBUG_RLUN_ARR_SZ];
@@ -2743,6 +2743,13 @@ static int stop_queued_cmnd(struct scsi_cmnd *cmnd)
 		if (test_bit(k, queued_in_use_bm)) {
 			sqcp = &queued_arr[k];
 			if (cmnd == sqcp->a_cmnd) {
+				devip = (struct sdebug_dev_info *)
+					cmnd->device->hostdata;
+				if (devip)
+					atomic_dec(&devip->num_in_q);
+				sqcp->a_cmnd = NULL;
+				spin_unlock_irqrestore(&queued_arr_lock,
+						       iflags);
 				if (scsi_debug_ndelay > 0) {
 					if (sqcp->sd_hrtp)
 						hrtimer_cancel(
@@ -2755,18 +2762,13 @@ static int stop_queued_cmnd(struct scsi_cmnd *cmnd)
 					if (sqcp->tletp)
 						tasklet_kill(sqcp->tletp);
 				}
-				__clear_bit(k, queued_in_use_bm);
-				devip = (struct sdebug_dev_info *)
-					cmnd->device->hostdata;
-				if (devip)
-					atomic_dec(&devip->num_in_q);
-				sqcp->a_cmnd = NULL;
-				break;
+				clear_bit(k, queued_in_use_bm);
+				return 1;
 			}
 		}
 	}
 	spin_unlock_irqrestore(&queued_arr_lock, iflags);
-	return (k < qmax) ? 1 : 0;
+	return 0;
 }
 
 /* Deletes (stops) timers or tasklets of all queued commands */
@@ -2782,6 +2784,13 @@ static void stop_all_queued(void)
 		if (test_bit(k, queued_in_use_bm)) {
 			sqcp = &queued_arr[k];
 			if (sqcp->a_cmnd) {
+				devip = (struct sdebug_dev_info *)
+					sqcp->a_cmnd->device->hostdata;
+				if (devip)
+					atomic_dec(&devip->num_in_q);
+				sqcp->a_cmnd = NULL;
+				spin_unlock_irqrestore(&queued_arr_lock,
+						       iflags);
 				if (scsi_debug_ndelay > 0) {
 					if (sqcp->sd_hrtp)
 						hrtimer_cancel(
@@ -2794,12 +2803,8 @@ static void stop_all_queued(void)
 					if (sqcp->tletp)
 						tasklet_kill(sqcp->tletp);
 				}
-				__clear_bit(k, queued_in_use_bm);
-				devip = (struct sdebug_dev_info *)
-					sqcp->a_cmnd->device->hostdata;
-				if (devip)
-					atomic_dec(&devip->num_in_q);
-				sqcp->a_cmnd = NULL;
+				clear_bit(k, queued_in_use_bm);
+				spin_lock_irqsave(&queued_arr_lock, iflags);
 			}
 		}
 	}
@@ -3006,7 +3011,7 @@ schedule_resp(struct scsi_cmnd *cmnd, struct sdebug_dev_info *devip,
 	      int scsi_result, int delta_jiff)
 {
 	unsigned long iflags;
-	int k, num_in_q, tsf, qdepth, inject;
+	int k, num_in_q, qdepth, inject;
 	struct sdebug_queued_cmd *sqcp = NULL;
 	struct scsi_device *sdp = cmnd->device;
 
@@ -3019,55 +3024,48 @@ schedule_resp(struct scsi_cmnd *cmnd, struct sdebug_dev_info *devip,
 	if ((scsi_result) && (SCSI_DEBUG_OPT_NOISE & scsi_debug_opts))
 		sdev_printk(KERN_INFO, sdp, "%s: non-zero result=0x%x\n",
 			    __func__, scsi_result);
-	if (delta_jiff == 0) {
-		/* using same thread to call back mid-layer */
-		cmnd->result = scsi_result;
-		cmnd->scsi_done(cmnd);
-		return 0;
-	}
+	if (delta_jiff == 0)
+		goto respond_in_thread;
 
-	/* deferred response cases */
+	/* schedule the response at a later time if resources permit */
 	spin_lock_irqsave(&queued_arr_lock, iflags);
 	num_in_q = atomic_read(&devip->num_in_q);
 	qdepth = cmnd->device->queue_depth;
-	k = find_first_zero_bit(queued_in_use_bm, scsi_debug_max_queue);
-	tsf = 0;
 	inject = 0;
-	if ((qdepth > 0) && (num_in_q >= qdepth))
-		tsf = 1;
-	else if ((scsi_debug_every_nth != 0) &&
-		 (SCSI_DEBUG_OPT_RARE_TSF & scsi_debug_opts)) {
+	if ((qdepth > 0) && (num_in_q >= qdepth)) {
+		if (scsi_result) {
+			spin_unlock_irqrestore(&queued_arr_lock, iflags);
+			goto respond_in_thread;
+		} else
+			scsi_result = device_qfull_result;
+	} else if ((scsi_debug_every_nth != 0) &&
+		   (SCSI_DEBUG_OPT_RARE_TSF & scsi_debug_opts) &&
+		   (scsi_result == 0)) {
 		if ((num_in_q == (qdepth - 1)) &&
 		    (atomic_inc_return(&sdebug_a_tsf) >=
 		     abs(scsi_debug_every_nth))) {
 			atomic_set(&sdebug_a_tsf, 0);
 			inject = 1;
-			tsf = 1;
+			scsi_result = device_qfull_result;
 		}
 	}
 
-	/* if (tsf) simulate device reporting SCSI status of TASK SET FULL.
-	 * Might override existing CHECK CONDITION. */
-	if (tsf)
-		scsi_result = device_qfull_result;
+	k = find_first_zero_bit(queued_in_use_bm, scsi_debug_max_queue);
 	if (k >= scsi_debug_max_queue) {
-		if (SCSI_DEBUG_OPT_ALL_TSF & scsi_debug_opts)
-			tsf = 1;
 		spin_unlock_irqrestore(&queued_arr_lock, iflags);
+		if (scsi_result)
+			goto respond_in_thread;
+		else if (SCSI_DEBUG_OPT_ALL_TSF & scsi_debug_opts)
+			scsi_result = device_qfull_result;
 		if (SCSI_DEBUG_OPT_Q_NOISE & scsi_debug_opts)
 			sdev_printk(KERN_INFO, sdp,
-				    "%s: num_in_q=%d, bypass q, %s%s\n",
-				    __func__, num_in_q,
-				    (inject ? "<inject> " : ""),
-				    (tsf ?  "status: TASK SET FULL" :
-					    "report: host busy"));
-		if (tsf) {
-			/* queued_arr full so respond in same thread */
-			cmnd->result = scsi_result;
-			cmnd->scsi_done(cmnd);
-			/* As scsi_done() is called "inline" must return 0 */
-			return 0;
-		} else
+				    "%s: max_queue=%d exceeded, %s\n",
+				    __func__, scsi_debug_max_queue,
+				    (scsi_result ?  "status: TASK SET FULL" :
+						    "report: host busy"));
+		if (scsi_result)
+			goto respond_in_thread;
+		else
 			return SCSI_MLQUEUE_HOST_BUSY;
 	}
 	__set_bit(k, queued_in_use_bm);
@@ -3117,12 +3115,18 @@ schedule_resp(struct scsi_cmnd *cmnd, struct sdebug_dev_info *devip,
 		else
 			tasklet_schedule(sqcp->tletp);
 	}
-	if (tsf && (SCSI_DEBUG_OPT_Q_NOISE & scsi_debug_opts))
+	if ((SCSI_DEBUG_OPT_Q_NOISE & scsi_debug_opts) &&
+	    (scsi_result == device_qfull_result))
 		sdev_printk(KERN_INFO, sdp,
 			    "%s: num_in_q=%d +1, %s%s\n", __func__,
 			    num_in_q, (inject ? "<inject> " : ""),
 			    "status: TASK SET FULL");
 	return 0;
+
+respond_in_thread:	/* call back to mid-layer using invocation thread */
+	cmnd->result = scsi_result;
+	cmnd->scsi_done(cmnd);
+	return 0;
 }
 
 /* Note: The following macros create attribute files in the
@@ -3206,7 +3210,7 @@ MODULE_PARM_DESC(opts, "1->noise, 2->medium_err, 4->timeout, 8->recovered_err...
 MODULE_PARM_DESC(physblk_exp, "physical block exponent (def=0)");
 MODULE_PARM_DESC(ptype, "SCSI peripheral type(def=0[disk])");
 MODULE_PARM_DESC(removable, "claim to have removable media (def=0)");
-MODULE_PARM_DESC(scsi_level, "SCSI level to simulate(def=5[SPC-3])");
+MODULE_PARM_DESC(scsi_level, "SCSI level to simulate(def=6[SPC-4])");
 MODULE_PARM_DESC(sector_size, "logical block size in bytes (def=512)");
 MODULE_PARM_DESC(unmap_alignment, "lowest aligned thin provisioning lba (def=0)");
 MODULE_PARM_DESC(unmap_granularity, "thin provisioning granularity in blocks (def=1)");
@@ -3367,7 +3371,7 @@ static ssize_t opts_store(struct device_driver *ddp, const char *buf,
 	char work[20];
 
         if (1 == sscanf(buf, "%10s", work)) {
-		if (0 == strnicmp(work,"0x", 2)) {
+		if (0 == strncasecmp(work,"0x", 2)) {
 			if (1 == sscanf(&work[2], "%x", &opts))
 				goto opts_done;
 		} else {
@@ -4085,7 +4089,7 @@ static void sdebug_remove_adapter(void)
 static int
 scsi_debug_queuecommand(struct scsi_cmnd *SCpnt)
 {
-	unsigned char *cmd = (unsigned char *) SCpnt->cmnd;
+	unsigned char *cmd = SCpnt->cmnd;
 	int len, k;
 	unsigned int num;
 	unsigned long long lba;
@@ -4103,7 +4107,7 @@ scsi_debug_queuecommand(struct scsi_cmnd *SCpnt)
 
 	scsi_set_resid(SCpnt, 0);
 	if ((SCSI_DEBUG_OPT_NOISE & scsi_debug_opts) &&
-	    !(SCSI_DEBUG_OPT_NO_CDB_NOISE & scsi_debug_opts) && cmd) {
+	    !(SCSI_DEBUG_OPT_NO_CDB_NOISE & scsi_debug_opts)) {
 		char b[120];
 		int n;
 
diff --git a/drivers/scsi/scsi_error.c b/drivers/scsi/scsi_error.c
index 5db8454474ee..9a6f8468225f 100644
--- a/drivers/scsi/scsi_error.c
+++ b/drivers/scsi/scsi_error.c
@@ -1238,9 +1238,9 @@ retry_tur:
 /**
  * scsi_eh_test_devices - check if devices are responding from error recovery.
  * @cmd_list:	scsi commands in error recovery.
- * @work_q:     queue for commands which still need more error recovery
- * @done_q:     queue for commands which are finished
- * @try_stu:    boolean on if a STU command should be tried in addition to TUR.
+ * @work_q:	queue for commands which still need more error recovery
+ * @done_q:	queue for commands which are finished
+ * @try_stu:	boolean on if a STU command should be tried in addition to TUR.
  *
  * Decription:
  *    Tests if devices are in a working state.  Commands to devices now in
@@ -1373,7 +1373,7 @@ static int scsi_eh_try_stu(struct scsi_cmnd *scmd)
  /**
  * scsi_eh_stu - send START_UNIT if needed
  * @shost:	&scsi host being recovered.
- * @work_q:     &list_head for pending commands.
+ * @work_q:	&list_head for pending commands.
  * @done_q:	&list_head for processed commands.
  *
  * Notes:
@@ -1436,7 +1436,7 @@ static int scsi_eh_stu(struct Scsi_Host *shost,
 /**
  * scsi_eh_bus_device_reset - send bdr if needed
  * @shost:	scsi host being recovered.
- * @work_q:     &list_head for pending commands.
+ * @work_q:	&list_head for pending commands.
  * @done_q:	&list_head for processed commands.
  *
  * Notes:
@@ -1502,7 +1502,7 @@ static int scsi_eh_bus_device_reset(struct Scsi_Host *shost,
 /**
  * scsi_eh_target_reset - send target reset if needed
  * @shost:	scsi host being recovered.
- * @work_q:     &list_head for pending commands.
+ * @work_q:	&list_head for pending commands.
  * @done_q:	&list_head for processed commands.
  *
  * Notes:
@@ -1567,7 +1567,7 @@ static int scsi_eh_target_reset(struct Scsi_Host *shost,
 /**
  * scsi_eh_bus_reset - send a bus reset
  * @shost:	&scsi host being recovered.
- * @work_q:     &list_head for pending commands.
+ * @work_q:	&list_head for pending commands.
  * @done_q:	&list_head for processed commands.
  */
 static int scsi_eh_bus_reset(struct Scsi_Host *shost,
@@ -1638,8 +1638,9 @@ static int scsi_eh_bus_reset(struct Scsi_Host *shost,
 
 /**
  * scsi_eh_host_reset - send a host reset
- * @work_q:	list_head for processed commands.
- * @done_q:	list_head for processed commands.
+ * @shost:	host to be reset.
+ * @work_q:	&list_head for pending commands.
+ * @done_q:	&list_head for processed commands.
  */
 static int scsi_eh_host_reset(struct Scsi_Host *shost,
 			      struct list_head *work_q,
@@ -1677,8 +1678,8 @@ static int scsi_eh_host_reset(struct Scsi_Host *shost,
 
 /**
  * scsi_eh_offline_sdevs - offline scsi devices that fail to recover
- * @work_q:	list_head for processed commands.
- * @done_q:	list_head for processed commands.
+ * @work_q:	&list_head for pending commands.
+ * @done_q:	&list_head for processed commands.
  */
 static void scsi_eh_offline_sdevs(struct list_head *work_q,
 				  struct list_head *done_q)
@@ -1960,6 +1961,8 @@ static void scsi_eh_lock_door(struct scsi_device *sdev)
 	 * request becomes available
 	 */
 	req = blk_get_request(sdev->request_queue, READ, GFP_KERNEL);
+	if (IS_ERR(req))
+		return;
 
 	blk_rq_set_block_pc(req);
 
@@ -2043,8 +2046,8 @@ static void scsi_restart_operations(struct Scsi_Host *shost)
 
 /**
  * scsi_eh_ready_devs - check device ready state and recover if not.
- * @shost: 	host to be recovered.
- * @work_q:     &list_head for pending commands.
+ * @shost:	host to be recovered.
+ * @work_q:	&list_head for pending commands.
  * @done_q:	&list_head for processed commands.
  */
 void scsi_eh_ready_devs(struct Scsi_Host *shost,
diff --git a/drivers/scsi/scsi_lib.c b/drivers/scsi/scsi_lib.c
index 9c44392b748f..9eff8a375132 100644
--- a/drivers/scsi/scsi_lib.c
+++ b/drivers/scsi/scsi_lib.c
@@ -221,7 +221,7 @@ int scsi_execute(struct scsi_device *sdev, const unsigned char *cmd,
 	int ret = DRIVER_ERROR << 24;
 
 	req = blk_get_request(sdev->request_queue, write, __GFP_WAIT);
-	if (!req)
+	if (IS_ERR(req))
 		return ret;
 	blk_rq_set_block_pc(req);
 
@@ -645,16 +645,18 @@ static void scsi_mq_free_sgtables(struct scsi_cmnd *cmd)
 static void scsi_mq_uninit_cmd(struct scsi_cmnd *cmd)
 {
 	struct scsi_device *sdev = cmd->device;
+	struct Scsi_Host *shost = sdev->host;
 	unsigned long flags;
 
-	BUG_ON(list_empty(&cmd->list));
-
 	scsi_mq_free_sgtables(cmd);
 	scsi_uninit_cmd(cmd);
 
-	spin_lock_irqsave(&sdev->list_lock, flags);
-	list_del_init(&cmd->list);
-	spin_unlock_irqrestore(&sdev->list_lock, flags);
+	if (shost->use_cmd_list) {
+		BUG_ON(list_empty(&cmd->list));
+		spin_lock_irqsave(&sdev->list_lock, flags);
+		list_del_init(&cmd->list);
+		spin_unlock_irqrestore(&sdev->list_lock, flags);
+	}
 }
 
 /*
@@ -713,7 +715,7 @@ static bool scsi_end_request(struct request *req, int error,
 
 	if (req->mq_ctx) {
 		/*
-		 * In the MQ case the command gets freed by __blk_mq_end_io,
+		 * In the MQ case the command gets freed by __blk_mq_end_request,
 		 * so we have to do all cleanup that depends on it earlier.
 		 *
 		 * We also can't kick the queues from irq context, so we
@@ -721,7 +723,7 @@ static bool scsi_end_request(struct request *req, int error,
 		 */
 		scsi_mq_uninit_cmd(cmd);
 
-		__blk_mq_end_io(req, error);
+		__blk_mq_end_request(req, error);
 
 		if (scsi_target(sdev)->single_lun ||
 		    !list_empty(&sdev->host->starved_list))
@@ -733,12 +735,13 @@ static bool scsi_end_request(struct request *req, int error,
 	} else {
 		unsigned long flags;
 
+		if (bidi_bytes)
+			scsi_release_bidi_buffers(cmd);
+
 		spin_lock_irqsave(q->queue_lock, flags);
 		blk_finish_request(req, error);
 		spin_unlock_irqrestore(q->queue_lock, flags);
 
-		if (bidi_bytes)
-			scsi_release_bidi_buffers(cmd);
 		scsi_release_buffers(cmd);
 		scsi_next_command(cmd);
 	}
@@ -1774,7 +1777,7 @@ static void scsi_request_fn(struct request_queue *q)
 	blk_requeue_request(q, req);
 	atomic_dec(&sdev->device_busy);
 out_delay:
-	if (atomic_read(&sdev->device_busy) && !scsi_device_blocked(sdev))
+	if (!atomic_read(&sdev->device_busy) && !scsi_device_blocked(sdev))
 		blk_delay_queue(q, SCSI_QUEUE_DELAY);
 }
 
@@ -1808,7 +1811,6 @@ static int scsi_mq_prep_fn(struct request *req)
 
 	cmd->tag = req->tag;
 
-	req->cmd = req->__cmd;
 	cmd->cmnd = req->cmd;
 	cmd->prot_op = SCSI_PROT_NORMAL;
 
@@ -1816,13 +1818,11 @@ static int scsi_mq_prep_fn(struct request *req)
 	INIT_DELAYED_WORK(&cmd->abort_work, scmd_eh_abort_handler);
 	cmd->jiffies_at_alloc = jiffies;
 
-	/*
-	 * XXX: cmd_list lookups are only used by two drivers, try to get
-	 * rid of this list in common code.
-	 */
-	spin_lock_irq(&sdev->list_lock);
-	list_add_tail(&cmd->list, &sdev->cmd_list);
-	spin_unlock_irq(&sdev->list_lock);
+	if (shost->use_cmd_list) {
+		spin_lock_irq(&sdev->list_lock);
+		list_add_tail(&cmd->list, &sdev->cmd_list);
+		spin_unlock_irq(&sdev->list_lock);
+	}
 
 	sg = (void *)cmd + sizeof(struct scsi_cmnd) + shost->hostt->cmd_size;
 	cmd->sdb.table.sgl = sg;
@@ -1847,6 +1847,8 @@ static int scsi_mq_prep_fn(struct request *req)
 		next_rq->special = bidi_sdb;
 	}
 
+	blk_mq_start_request(req);
+
 	return scsi_setup_cmnd(sdev, req);
 }
 
@@ -1856,7 +1858,8 @@ static void scsi_mq_done(struct scsi_cmnd *cmd)
 	blk_mq_complete_request(cmd->request);
 }
 
-static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
+static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req,
+		bool last)
 {
 	struct request_queue *q = req->q;
 	struct scsi_device *sdev = q->queuedata;
@@ -1880,11 +1883,14 @@ static int scsi_queue_rq(struct blk_mq_hw_ctx *hctx, struct request *req)
 	if (!scsi_host_queue_ready(q, shost, sdev))
 		goto out_dec_target_busy;
 
+
 	if (!(req->cmd_flags & REQ_DONTPREP)) {
 		ret = prep_to_mq(scsi_mq_prep_fn(req));
 		if (ret)
 			goto out_dec_host_busy;
 		req->cmd_flags |= REQ_DONTPREP;
+	} else {
+		blk_mq_start_request(req);
 	}
 
 	scsi_init_cmd_errh(cmd);
@@ -1931,6 +1937,14 @@ out:
 	return ret;
 }
 
+static enum blk_eh_timer_return scsi_timeout(struct request *req,
+		bool reserved)
+{
+	if (reserved)
+		return BLK_EH_RESET_TIMER;
+	return scsi_times_out(req);
+}
+
 static int scsi_init_request(void *data, struct request *rq,
 		unsigned int hctx_idx, unsigned int request_idx,
 		unsigned int numa_node)
@@ -2042,7 +2056,7 @@ static struct blk_mq_ops scsi_mq_ops = {
 	.map_queue	= blk_mq_map_queue,
 	.queue_rq	= scsi_queue_rq,
 	.complete	= scsi_softirq_done,
-	.timeout	= scsi_times_out,
+	.timeout	= scsi_timeout,
 	.init_request	= scsi_init_request,
 	.exit_request	= scsi_exit_request,
 };
diff --git a/drivers/scsi/scsi_scan.c b/drivers/scsi/scsi_scan.c
index 56675dbbf681..ba3f1e8d0d57 100644
--- a/drivers/scsi/scsi_scan.c
+++ b/drivers/scsi/scsi_scan.c
@@ -736,6 +736,16 @@ static int scsi_probe_lun(struct scsi_device *sdev, unsigned char *inq_result,
 		sdev->scsi_level++;
 	sdev->sdev_target->scsi_level = sdev->scsi_level;
 
+	/*
+	 * If SCSI-2 or lower, and if the transport requires it,
+	 * store the LUN value in CDB[1].
+	 */
+	sdev->lun_in_cdb = 0;
+	if (sdev->scsi_level <= SCSI_2 &&
+	    sdev->scsi_level != SCSI_UNKNOWN &&
+	    !sdev->host->no_scsi2_lun_in_cdb)
+		sdev->lun_in_cdb = 1;
+
 	return 0;
 }
 
@@ -805,6 +815,19 @@ static int scsi_add_lun(struct scsi_device *sdev, unsigned char *inq_result,
 	} else {
 		sdev->type = (inq_result[0] & 0x1f);
 		sdev->removable = (inq_result[1] & 0x80) >> 7;
+
+		/*
+		 * some devices may respond with wrong type for
+		 * well-known logical units. Force well-known type
+		 * to enumerate them correctly.
+		 */
+		if (scsi_is_wlun(sdev->lun) && sdev->type != TYPE_WLUN) {
+			sdev_printk(KERN_WARNING, sdev,
+				"%s: correcting incorrect peripheral device type 0x%x for W-LUN 0x%16xhN\n",
+				__func__, sdev->type, (unsigned int)sdev->lun);
+			sdev->type = TYPE_WLUN;
+		}
+
 	}
 
 	if (sdev->type == TYPE_RBC || sdev->type == TYPE_ROM) {
@@ -1733,6 +1756,9 @@ static void scsi_sysfs_add_devices(struct Scsi_Host *shost)
 		/* target removed before the device could be added */
 		if (sdev->sdev_state == SDEV_DEL)
 			continue;
+		/* If device is already visible, skip adding it to sysfs */
+		if (sdev->is_visible)
+			continue;
 		if (!scsi_host_scan_allowed(shost) ||
 		    scsi_sysfs_add_sdev(sdev) != 0)
 			__scsi_remove_device(sdev);
diff --git a/drivers/scsi/scsi_sysfs.c b/drivers/scsi/scsi_sysfs.c
index 8b4105a22ac2..f4cb7b3e9e23 100644
--- a/drivers/scsi/scsi_sysfs.c
+++ b/drivers/scsi/scsi_sysfs.c
@@ -1044,10 +1044,6 @@ int scsi_sysfs_add_sdev(struct scsi_device *sdev)
 	pm_runtime_enable(&sdev->sdev_gendev);
 	scsi_autopm_put_target(starget);
 
-	/* The following call will keep sdev active indefinitely, until
-	 * its driver does a corresponding scsi_autopm_pm_device().  Only
-	 * drivers supporting autosuspend will do this.
-	 */
 	scsi_autopm_get_device(sdev);
 
 	error = device_add(&sdev->sdev_gendev);
@@ -1085,6 +1081,7 @@ int scsi_sysfs_add_sdev(struct scsi_device *sdev)
 		}
 	}
 
+	scsi_autopm_put_device(sdev);
 	return error;
 }
 
@@ -1263,7 +1260,19 @@ void scsi_sysfs_device_initialize(struct scsi_device *sdev)
 	sdev->sdev_dev.class = &sdev_class;
 	dev_set_name(&sdev->sdev_dev, "%d:%d:%d:%llu",
 		     sdev->host->host_no, sdev->channel, sdev->id, sdev->lun);
+	/*
+	 * Get a default scsi_level from the target (derived from sibling
+	 * devices).  This is the best we can do for guessing how to set
+	 * sdev->lun_in_cdb for the initial INQUIRY command.  For LUN 0 the
+	 * setting doesn't matter, because all the bits are zero anyway.
+	 * But it does matter for higher LUNs.
+	 */
 	sdev->scsi_level = starget->scsi_level;
+	if (sdev->scsi_level <= SCSI_2 &&
+			sdev->scsi_level != SCSI_UNKNOWN &&
+			!shost->no_scsi2_lun_in_cdb)
+		sdev->lun_in_cdb = 1;
+
 	transport_setup_device(&sdev->sdev_gendev);
 	spin_lock_irqsave(shost->host_lock, flags);
 	list_add_tail(&sdev->same_target_siblings, &starget->devices);
diff --git a/drivers/scsi/sd.c b/drivers/scsi/sd.c
index 2c2041ca4b70..cfba74cd8e8b 100644
--- a/drivers/scsi/sd.c
+++ b/drivers/scsi/sd.c
@@ -185,7 +185,7 @@ cache_type_store(struct device *dev, struct device_attribute *attr,
 	if (ct < 0)
 		return -EINVAL;
 	rcd = ct & 0x01 ? 1 : 0;
-	wce = ct & 0x02 ? 1 : 0;
+	wce = (ct & 0x02) && !sdkp->write_prot ? 1 : 0;
 
 	if (sdkp->cache_override) {
 		sdkp->WCE = wce;
@@ -610,29 +610,44 @@ static void scsi_disk_put(struct scsi_disk *sdkp)
 	mutex_unlock(&sd_ref_mutex);
 }
 
-static void sd_prot_op(struct scsi_cmnd *scmd, unsigned int dif)
+
+
+static unsigned char sd_setup_protect_cmnd(struct scsi_cmnd *scmd,
+					   unsigned int dix, unsigned int dif)
 {
-	unsigned int prot_op = SCSI_PROT_NORMAL;
-	unsigned int dix = scsi_prot_sg_count(scmd);
+	struct bio *bio = scmd->request->bio;
+	unsigned int prot_op = sd_prot_op(rq_data_dir(scmd->request), dix, dif);
+	unsigned int protect = 0;
 
-	if (scmd->sc_data_direction == DMA_FROM_DEVICE) {
-		if (dif && dix)
-			prot_op = SCSI_PROT_READ_PASS;
-		else if (dif && !dix)
-			prot_op = SCSI_PROT_READ_STRIP;
-		else if (!dif && dix)
-			prot_op = SCSI_PROT_READ_INSERT;
-	} else {
-		if (dif && dix)
-			prot_op = SCSI_PROT_WRITE_PASS;
-		else if (dif && !dix)
-			prot_op = SCSI_PROT_WRITE_INSERT;
-		else if (!dif && dix)
-			prot_op = SCSI_PROT_WRITE_STRIP;
+	if (dix) {				/* DIX Type 0, 1, 2, 3 */
+		if (bio_integrity_flagged(bio, BIP_IP_CHECKSUM))
+			scmd->prot_flags |= SCSI_PROT_IP_CHECKSUM;
+
+		if (bio_integrity_flagged(bio, BIP_CTRL_NOCHECK) == false)
+			scmd->prot_flags |= SCSI_PROT_GUARD_CHECK;
+	}
+
+	if (dif != SD_DIF_TYPE3_PROTECTION) {	/* DIX/DIF Type 0, 1, 2 */
+		scmd->prot_flags |= SCSI_PROT_REF_INCREMENT;
+
+		if (bio_integrity_flagged(bio, BIP_CTRL_NOCHECK) == false)
+			scmd->prot_flags |= SCSI_PROT_REF_CHECK;
+	}
+
+	if (dif) {				/* DIX/DIF Type 1, 2, 3 */
+		scmd->prot_flags |= SCSI_PROT_TRANSFER_PI;
+
+		if (bio_integrity_flagged(bio, BIP_DISK_NOCHECK))
+			protect = 3 << 5;	/* Disable target PI checking */
+		else
+			protect = 1 << 5;	/* Enable target PI checking */
 	}
 
 	scsi_set_prot_op(scmd, prot_op);
 	scsi_set_prot_type(scmd, dif);
+	scmd->prot_flags &= sd_prot_flag_mask(prot_op);
+
+	return protect;
 }
 
 static void sd_config_discard(struct scsi_disk *sdkp, unsigned int mode)
@@ -893,7 +908,8 @@ static int sd_setup_read_write_cmnd(struct scsi_cmnd *SCpnt)
 	sector_t block = blk_rq_pos(rq);
 	sector_t threshold;
 	unsigned int this_count = blk_rq_sectors(rq);
-	int ret, host_dif;
+	unsigned int dif, dix;
+	int ret;
 	unsigned char protect;
 
 	ret = scsi_init_io(SCpnt, GFP_ATOMIC);
@@ -995,7 +1011,7 @@ static int sd_setup_read_write_cmnd(struct scsi_cmnd *SCpnt)
 		SCpnt->cmnd[0] = WRITE_6;
 
 		if (blk_integrity_rq(rq))
-			sd_dif_prepare(rq, block, sdp->sector_size);
+			sd_dif_prepare(SCpnt);
 
 	} else if (rq_data_dir(rq) == READ) {
 		SCpnt->cmnd[0] = READ_6;
@@ -1010,14 +1026,15 @@ static int sd_setup_read_write_cmnd(struct scsi_cmnd *SCpnt)
 					"writing" : "reading", this_count,
 					blk_rq_sectors(rq)));
 
-	/* Set RDPROTECT/WRPROTECT if disk is formatted with DIF */
-	host_dif = scsi_host_dif_capable(sdp->host, sdkp->protection_type);
-	if (host_dif)
-		protect = 1 << 5;
+	dix = scsi_prot_sg_count(SCpnt);
+	dif = scsi_host_dif_capable(SCpnt->device->host, sdkp->protection_type);
+
+	if (dif || dix)
+		protect = sd_setup_protect_cmnd(SCpnt, dix, dif);
 	else
 		protect = 0;
 
-	if (host_dif == SD_DIF_TYPE2_PROTECTION) {
+	if (protect && sdkp->protection_type == SD_DIF_TYPE2_PROTECTION) {
 		SCpnt->cmnd = mempool_alloc(sd_cdb_pool, GFP_ATOMIC);
 
 		if (unlikely(SCpnt->cmnd == NULL)) {
@@ -1102,10 +1119,6 @@ static int sd_setup_read_write_cmnd(struct scsi_cmnd *SCpnt)
 	}
 	SCpnt->sdb.length = this_count * sdp->sector_size;
 
-	/* If DIF or DIX is enabled, tell HBA how to handle request */
-	if (host_dif || scsi_prot_sg_count(SCpnt))
-		sd_prot_op(SCpnt, host_dif);
-
 	/*
 	 * We shouldn't disconnect in the middle of a sector, so with a dumb
 	 * host adapter, it's safe to assume that we can at least transfer
@@ -2490,6 +2503,10 @@ sd_read_cache_type(struct scsi_disk *sdkp, unsigned char *buffer)
 			sdkp->DPOFUA = 0;
 		}
 
+		/* No cache flush allowed for write protected devices */
+		if (sdkp->WCE && sdkp->write_prot)
+			sdkp->WCE = 0;
+
 		if (sdkp->first_scan || old_wce != sdkp->WCE ||
 		    old_rcd != sdkp->RCD || old_dpofua != sdkp->DPOFUA)
 			sd_printk(KERN_NOTICE, sdkp,
@@ -2660,8 +2677,10 @@ static void sd_read_block_characteristics(struct scsi_disk *sdkp)
 
 	rot = get_unaligned_be16(&buffer[4]);
 
-	if (rot == 1)
+	if (rot == 1) {
 		queue_flag_set_unlocked(QUEUE_FLAG_NONROT, sdkp->disk->queue);
+		queue_flag_clear_unlocked(QUEUE_FLAG_ADD_RANDOM, sdkp->disk->queue);
+	}
 
  out:
 	kfree(buffer);
@@ -2961,6 +2980,7 @@ static int sd_probe(struct device *dev)
 	int index;
 	int error;
 
+	scsi_autopm_get_device(sdp);
 	error = -ENODEV;
 	if (sdp->type != TYPE_DISK && sdp->type != TYPE_MOD && sdp->type != TYPE_RBC)
 		goto out;
@@ -3037,6 +3057,7 @@ static int sd_probe(struct device *dev)
  out_free:
 	kfree(sdkp);
  out:
+	scsi_autopm_put_device(sdp);
 	return error;
 }
 
diff --git a/drivers/scsi/sd.h b/drivers/scsi/sd.h
index 4c3ab8377fd3..467377884b63 100644
--- a/drivers/scsi/sd.h
+++ b/drivers/scsi/sd.h
@@ -167,6 +167,68 @@ enum sd_dif_target_protection_types {
 };
 
 /*
+ * Look up the DIX operation based on whether the command is read or
+ * write and whether dix and dif are enabled.
+ */
+static inline unsigned int sd_prot_op(bool write, bool dix, bool dif)
+{
+	/* Lookup table: bit 2 (write), bit 1 (dix), bit 0 (dif) */
+	const unsigned int ops[] = {	/* wrt dix dif */
+		SCSI_PROT_NORMAL,	/*  0	0   0  */
+		SCSI_PROT_READ_STRIP,	/*  0	0   1  */
+		SCSI_PROT_READ_INSERT,	/*  0	1   0  */
+		SCSI_PROT_READ_PASS,	/*  0	1   1  */
+		SCSI_PROT_NORMAL,	/*  1	0   0  */
+		SCSI_PROT_WRITE_INSERT, /*  1	0   1  */
+		SCSI_PROT_WRITE_STRIP,	/*  1	1   0  */
+		SCSI_PROT_WRITE_PASS,	/*  1	1   1  */
+	};
+
+	return ops[write << 2 | dix << 1 | dif];
+}
+
+/*
+ * Returns a mask of the protection flags that are valid for a given DIX
+ * operation.
+ */
+static inline unsigned int sd_prot_flag_mask(unsigned int prot_op)
+{
+	const unsigned int flag_mask[] = {
+		[SCSI_PROT_NORMAL]		= 0,
+
+		[SCSI_PROT_READ_STRIP]		= SCSI_PROT_TRANSFER_PI |
+						  SCSI_PROT_GUARD_CHECK |
+						  SCSI_PROT_REF_CHECK |
+						  SCSI_PROT_REF_INCREMENT,
+
+		[SCSI_PROT_READ_INSERT]		= SCSI_PROT_REF_INCREMENT |
+						  SCSI_PROT_IP_CHECKSUM,
+
+		[SCSI_PROT_READ_PASS]		= SCSI_PROT_TRANSFER_PI |
+						  SCSI_PROT_GUARD_CHECK |
+						  SCSI_PROT_REF_CHECK |
+						  SCSI_PROT_REF_INCREMENT |
+						  SCSI_PROT_IP_CHECKSUM,
+
+		[SCSI_PROT_WRITE_INSERT]	= SCSI_PROT_TRANSFER_PI |
+						  SCSI_PROT_REF_INCREMENT,
+
+		[SCSI_PROT_WRITE_STRIP]		= SCSI_PROT_GUARD_CHECK |
+						  SCSI_PROT_REF_CHECK |
+						  SCSI_PROT_REF_INCREMENT |
+						  SCSI_PROT_IP_CHECKSUM,
+
+		[SCSI_PROT_WRITE_PASS]		= SCSI_PROT_TRANSFER_PI |
+						  SCSI_PROT_GUARD_CHECK |
+						  SCSI_PROT_REF_CHECK |
+						  SCSI_PROT_REF_INCREMENT |
+						  SCSI_PROT_IP_CHECKSUM,
+	};
+
+	return flag_mask[prot_op];
+}
+
+/*
  * Data Integrity Field tuple.
  */
 struct sd_dif_tuple {
@@ -178,7 +240,7 @@ struct sd_dif_tuple {
 #ifdef CONFIG_BLK_DEV_INTEGRITY
 
 extern void sd_dif_config_host(struct scsi_disk *);
-extern void sd_dif_prepare(struct request *rq, sector_t, unsigned int);
+extern void sd_dif_prepare(struct scsi_cmnd *scmd);
 extern void sd_dif_complete(struct scsi_cmnd *, unsigned int);
 
 #else /* CONFIG_BLK_DEV_INTEGRITY */
@@ -186,7 +248,7 @@ extern void sd_dif_complete(struct scsi_cmnd *, unsigned int);
 static inline void sd_dif_config_host(struct scsi_disk *disk)
 {
 }
-static inline int sd_dif_prepare(struct request *rq, sector_t s, unsigned int a)
+static inline int sd_dif_prepare(struct scsi_cmnd *scmd)
 {
 	return 0;
 }
diff --git a/drivers/scsi/sd_dif.c b/drivers/scsi/sd_dif.c
index a7a691d0af7d..14c7d42a11c2 100644
--- a/drivers/scsi/sd_dif.c
+++ b/drivers/scsi/sd_dif.c
@@ -21,7 +21,7 @@
  */
 
 #include <linux/blkdev.h>
-#include <linux/crc-t10dif.h>
+#include <linux/t10-pi.h>
 
 #include <scsi/scsi.h>
 #include <scsi/scsi_cmnd.h>
@@ -33,268 +33,8 @@
 #include <scsi/scsi_ioctl.h>
 #include <scsi/scsicam.h>
 
-#include <net/checksum.h>
-
 #include "sd.h"
 
-typedef __u16 (csum_fn) (void *, unsigned int);
-
-static __u16 sd_dif_crc_fn(void *data, unsigned int len)
-{
-	return cpu_to_be16(crc_t10dif(data, len));
-}
-
-static __u16 sd_dif_ip_fn(void *data, unsigned int len)
-{
-	return ip_compute_csum(data, len);
-}
-
-/*
- * Type 1 and Type 2 protection use the same format: 16 bit guard tag,
- * 16 bit app tag, 32 bit reference tag.
- */
-static void sd_dif_type1_generate(struct blk_integrity_exchg *bix, csum_fn *fn)
-{
-	void *buf = bix->data_buf;
-	struct sd_dif_tuple *sdt = bix->prot_buf;
-	sector_t sector = bix->sector;
-	unsigned int i;
-
-	for (i = 0 ; i < bix->data_size ; i += bix->sector_size, sdt++) {
-		sdt->guard_tag = fn(buf, bix->sector_size);
-		sdt->ref_tag = cpu_to_be32(sector & 0xffffffff);
-		sdt->app_tag = 0;
-
-		buf += bix->sector_size;
-		sector++;
-	}
-}
-
-static void sd_dif_type1_generate_crc(struct blk_integrity_exchg *bix)
-{
-	sd_dif_type1_generate(bix, sd_dif_crc_fn);
-}
-
-static void sd_dif_type1_generate_ip(struct blk_integrity_exchg *bix)
-{
-	sd_dif_type1_generate(bix, sd_dif_ip_fn);
-}
-
-static int sd_dif_type1_verify(struct blk_integrity_exchg *bix, csum_fn *fn)
-{
-	void *buf = bix->data_buf;
-	struct sd_dif_tuple *sdt = bix->prot_buf;
-	sector_t sector = bix->sector;
-	unsigned int i;
-	__u16 csum;
-
-	for (i = 0 ; i < bix->data_size ; i += bix->sector_size, sdt++) {
-		/* Unwritten sectors */
-		if (sdt->app_tag == 0xffff)
-			return 0;
-
-		if (be32_to_cpu(sdt->ref_tag) != (sector & 0xffffffff)) {
-			printk(KERN_ERR
-			       "%s: ref tag error on sector %lu (rcvd %u)\n",
-			       bix->disk_name, (unsigned long)sector,
-			       be32_to_cpu(sdt->ref_tag));
-			return -EIO;
-		}
-
-		csum = fn(buf, bix->sector_size);
-
-		if (sdt->guard_tag != csum) {
-			printk(KERN_ERR "%s: guard tag error on sector %lu " \
-			       "(rcvd %04x, data %04x)\n", bix->disk_name,
-			       (unsigned long)sector,
-			       be16_to_cpu(sdt->guard_tag), be16_to_cpu(csum));
-			return -EIO;
-		}
-
-		buf += bix->sector_size;
-		sector++;
-	}
-
-	return 0;
-}
-
-static int sd_dif_type1_verify_crc(struct blk_integrity_exchg *bix)
-{
-	return sd_dif_type1_verify(bix, sd_dif_crc_fn);
-}
-
-static int sd_dif_type1_verify_ip(struct blk_integrity_exchg *bix)
-{
-	return sd_dif_type1_verify(bix, sd_dif_ip_fn);
-}
-
-/*
- * Functions for interleaving and deinterleaving application tags
- */
-static void sd_dif_type1_set_tag(void *prot, void *tag_buf, unsigned int sectors)
-{
-	struct sd_dif_tuple *sdt = prot;
-	u8 *tag = tag_buf;
-	unsigned int i, j;
-
-	for (i = 0, j = 0 ; i < sectors ; i++, j += 2, sdt++) {
-		sdt->app_tag = tag[j] << 8 | tag[j+1];
-		BUG_ON(sdt->app_tag == 0xffff);
-	}
-}
-
-static void sd_dif_type1_get_tag(void *prot, void *tag_buf, unsigned int sectors)
-{
-	struct sd_dif_tuple *sdt = prot;
-	u8 *tag = tag_buf;
-	unsigned int i, j;
-
-	for (i = 0, j = 0 ; i < sectors ; i++, j += 2, sdt++) {
-		tag[j] = (sdt->app_tag & 0xff00) >> 8;
-		tag[j+1] = sdt->app_tag & 0xff;
-	}
-}
-
-static struct blk_integrity dif_type1_integrity_crc = {
-	.name			= "T10-DIF-TYPE1-CRC",
-	.generate_fn		= sd_dif_type1_generate_crc,
-	.verify_fn		= sd_dif_type1_verify_crc,
-	.get_tag_fn		= sd_dif_type1_get_tag,
-	.set_tag_fn		= sd_dif_type1_set_tag,
-	.tuple_size		= sizeof(struct sd_dif_tuple),
-	.tag_size		= 0,
-};
-
-static struct blk_integrity dif_type1_integrity_ip = {
-	.name			= "T10-DIF-TYPE1-IP",
-	.generate_fn		= sd_dif_type1_generate_ip,
-	.verify_fn		= sd_dif_type1_verify_ip,
-	.get_tag_fn		= sd_dif_type1_get_tag,
-	.set_tag_fn		= sd_dif_type1_set_tag,
-	.tuple_size		= sizeof(struct sd_dif_tuple),
-	.tag_size		= 0,
-};
-
-
-/*
- * Type 3 protection has a 16-bit guard tag and 16 + 32 bits of opaque
- * tag space.
- */
-static void sd_dif_type3_generate(struct blk_integrity_exchg *bix, csum_fn *fn)
-{
-	void *buf = bix->data_buf;
-	struct sd_dif_tuple *sdt = bix->prot_buf;
-	unsigned int i;
-
-	for (i = 0 ; i < bix->data_size ; i += bix->sector_size, sdt++) {
-		sdt->guard_tag = fn(buf, bix->sector_size);
-		sdt->ref_tag = 0;
-		sdt->app_tag = 0;
-
-		buf += bix->sector_size;
-	}
-}
-
-static void sd_dif_type3_generate_crc(struct blk_integrity_exchg *bix)
-{
-	sd_dif_type3_generate(bix, sd_dif_crc_fn);
-}
-
-static void sd_dif_type3_generate_ip(struct blk_integrity_exchg *bix)
-{
-	sd_dif_type3_generate(bix, sd_dif_ip_fn);
-}
-
-static int sd_dif_type3_verify(struct blk_integrity_exchg *bix, csum_fn *fn)
-{
-	void *buf = bix->data_buf;
-	struct sd_dif_tuple *sdt = bix->prot_buf;
-	sector_t sector = bix->sector;
-	unsigned int i;
-	__u16 csum;
-
-	for (i = 0 ; i < bix->data_size ; i += bix->sector_size, sdt++) {
-		/* Unwritten sectors */
-		if (sdt->app_tag == 0xffff && sdt->ref_tag == 0xffffffff)
-			return 0;
-
-		csum = fn(buf, bix->sector_size);
-
-		if (sdt->guard_tag != csum) {
-			printk(KERN_ERR "%s: guard tag error on sector %lu " \
-			       "(rcvd %04x, data %04x)\n", bix->disk_name,
-			       (unsigned long)sector,
-			       be16_to_cpu(sdt->guard_tag), be16_to_cpu(csum));
-			return -EIO;
-		}
-
-		buf += bix->sector_size;
-		sector++;
-	}
-
-	return 0;
-}
-
-static int sd_dif_type3_verify_crc(struct blk_integrity_exchg *bix)
-{
-	return sd_dif_type3_verify(bix, sd_dif_crc_fn);
-}
-
-static int sd_dif_type3_verify_ip(struct blk_integrity_exchg *bix)
-{
-	return sd_dif_type3_verify(bix, sd_dif_ip_fn);
-}
-
-static void sd_dif_type3_set_tag(void *prot, void *tag_buf, unsigned int sectors)
-{
-	struct sd_dif_tuple *sdt = prot;
-	u8 *tag = tag_buf;
-	unsigned int i, j;
-
-	for (i = 0, j = 0 ; i < sectors ; i++, j += 6, sdt++) {
-		sdt->app_tag = tag[j] << 8 | tag[j+1];
-		sdt->ref_tag = tag[j+2] << 24 | tag[j+3] << 16 |
-			tag[j+4] << 8 | tag[j+5];
-	}
-}
-
-static void sd_dif_type3_get_tag(void *prot, void *tag_buf, unsigned int sectors)
-{
-	struct sd_dif_tuple *sdt = prot;
-	u8 *tag = tag_buf;
-	unsigned int i, j;
-
-	for (i = 0, j = 0 ; i < sectors ; i++, j += 2, sdt++) {
-		tag[j] = (sdt->app_tag & 0xff00) >> 8;
-		tag[j+1] = sdt->app_tag & 0xff;
-		tag[j+2] = (sdt->ref_tag & 0xff000000) >> 24;
-		tag[j+3] = (sdt->ref_tag & 0xff0000) >> 16;
-		tag[j+4] = (sdt->ref_tag & 0xff00) >> 8;
-		tag[j+5] = sdt->ref_tag & 0xff;
-		BUG_ON(sdt->app_tag == 0xffff || sdt->ref_tag == 0xffffffff);
-	}
-}
-
-static struct blk_integrity dif_type3_integrity_crc = {
-	.name			= "T10-DIF-TYPE3-CRC",
-	.generate_fn		= sd_dif_type3_generate_crc,
-	.verify_fn		= sd_dif_type3_verify_crc,
-	.get_tag_fn		= sd_dif_type3_get_tag,
-	.set_tag_fn		= sd_dif_type3_set_tag,
-	.tuple_size		= sizeof(struct sd_dif_tuple),
-	.tag_size		= 0,
-};
-
-static struct blk_integrity dif_type3_integrity_ip = {
-	.name			= "T10-DIF-TYPE3-IP",
-	.generate_fn		= sd_dif_type3_generate_ip,
-	.verify_fn		= sd_dif_type3_verify_ip,
-	.get_tag_fn		= sd_dif_type3_get_tag,
-	.set_tag_fn		= sd_dif_type3_set_tag,
-	.tuple_size		= sizeof(struct sd_dif_tuple),
-	.tag_size		= 0,
-};
-
 /*
  * Configure exchange of protection information between OS and HBA.
  */
@@ -316,22 +56,30 @@ void sd_dif_config_host(struct scsi_disk *sdkp)
 		return;
 
 	/* Enable DMA of protection information */
-	if (scsi_host_get_guard(sdkp->device->host) & SHOST_DIX_GUARD_IP)
+	if (scsi_host_get_guard(sdkp->device->host) & SHOST_DIX_GUARD_IP) {
 		if (type == SD_DIF_TYPE3_PROTECTION)
-			blk_integrity_register(disk, &dif_type3_integrity_ip);
+			blk_integrity_register(disk, &t10_pi_type3_ip);
 		else
-			blk_integrity_register(disk, &dif_type1_integrity_ip);
-	else
+			blk_integrity_register(disk, &t10_pi_type1_ip);
+
+		disk->integrity->flags |= BLK_INTEGRITY_IP_CHECKSUM;
+	} else
 		if (type == SD_DIF_TYPE3_PROTECTION)
-			blk_integrity_register(disk, &dif_type3_integrity_crc);
+			blk_integrity_register(disk, &t10_pi_type3_crc);
 		else
-			blk_integrity_register(disk, &dif_type1_integrity_crc);
+			blk_integrity_register(disk, &t10_pi_type1_crc);
 
 	sd_printk(KERN_NOTICE, sdkp,
 		  "Enabling DIX %s protection\n", disk->integrity->name);
 
 	/* Signal to block layer that we support sector tagging */
-	if (dif && type && sdkp->ATO) {
+	if (dif && type) {
+
+		disk->integrity->flags |= BLK_INTEGRITY_DEVICE_CAPABLE;
+
+		if (!sdkp)
+			return;
+
 		if (type == SD_DIF_TYPE3_PROTECTION)
 			disk->integrity->tag_size = sizeof(u16) + sizeof(u32);
 		else
@@ -358,50 +106,49 @@ void sd_dif_config_host(struct scsi_disk *sdkp)
  *
  * Type 3 does not have a reference tag so no remapping is required.
  */
-void sd_dif_prepare(struct request *rq, sector_t hw_sector,
-		    unsigned int sector_sz)
+void sd_dif_prepare(struct scsi_cmnd *scmd)
 {
-	const int tuple_sz = sizeof(struct sd_dif_tuple);
+	const int tuple_sz = sizeof(struct t10_pi_tuple);
 	struct bio *bio;
 	struct scsi_disk *sdkp;
-	struct sd_dif_tuple *sdt;
+	struct t10_pi_tuple *pi;
 	u32 phys, virt;
 
-	sdkp = rq->bio->bi_bdev->bd_disk->private_data;
+	sdkp = scsi_disk(scmd->request->rq_disk);
 
 	if (sdkp->protection_type == SD_DIF_TYPE3_PROTECTION)
 		return;
 
-	phys = hw_sector & 0xffffffff;
+	phys = scsi_prot_ref_tag(scmd);
 
-	__rq_for_each_bio(bio, rq) {
+	__rq_for_each_bio(bio, scmd->request) {
+		struct bio_integrity_payload *bip = bio_integrity(bio);
 		struct bio_vec iv;
 		struct bvec_iter iter;
 		unsigned int j;
 
 		/* Already remapped? */
-		if (bio_flagged(bio, BIO_MAPPED_INTEGRITY))
+		if (bip->bip_flags & BIP_MAPPED_INTEGRITY)
 			break;
 
-		virt = bio->bi_integrity->bip_iter.bi_sector & 0xffffffff;
+		virt = bip_get_seed(bip) & 0xffffffff;
 
-		bip_for_each_vec(iv, bio->bi_integrity, iter) {
-			sdt = kmap_atomic(iv.bv_page)
-				+ iv.bv_offset;
+		bip_for_each_vec(iv, bip, iter) {
+			pi = kmap_atomic(iv.bv_page) + iv.bv_offset;
 
-			for (j = 0; j < iv.bv_len; j += tuple_sz, sdt++) {
+			for (j = 0; j < iv.bv_len; j += tuple_sz, pi++) {
 
-				if (be32_to_cpu(sdt->ref_tag) == virt)
-					sdt->ref_tag = cpu_to_be32(phys);
+				if (be32_to_cpu(pi->ref_tag) == virt)
+					pi->ref_tag = cpu_to_be32(phys);
 
 				virt++;
 				phys++;
 			}
 
-			kunmap_atomic(sdt);
+			kunmap_atomic(pi);
 		}
 
-		bio->bi_flags |= (1 << BIO_MAPPED_INTEGRITY);
+		bip->bip_flags |= BIP_MAPPED_INTEGRITY;
 	}
 }
 
@@ -411,11 +158,11 @@ void sd_dif_prepare(struct request *rq, sector_t hw_sector,
  */
 void sd_dif_complete(struct scsi_cmnd *scmd, unsigned int good_bytes)
 {
-	const int tuple_sz = sizeof(struct sd_dif_tuple);
+	const int tuple_sz = sizeof(struct t10_pi_tuple);
 	struct scsi_disk *sdkp;
 	struct bio *bio;
-	struct sd_dif_tuple *sdt;
-	unsigned int j, sectors, sector_sz;
+	struct t10_pi_tuple *pi;
+	unsigned int j, intervals;
 	u32 phys, virt;
 
 	sdkp = scsi_disk(scmd->request->rq_disk);
@@ -423,39 +170,35 @@ void sd_dif_complete(struct scsi_cmnd *scmd, unsigned int good_bytes)
 	if (sdkp->protection_type == SD_DIF_TYPE3_PROTECTION || good_bytes == 0)
 		return;
 
-	sector_sz = scmd->device->sector_size;
-	sectors = good_bytes / sector_sz;
-
-	phys = blk_rq_pos(scmd->request) & 0xffffffff;
-	if (sector_sz == 4096)
-		phys >>= 3;
+	intervals = good_bytes / scsi_prot_interval(scmd);
+	phys = scsi_prot_ref_tag(scmd);
 
 	__rq_for_each_bio(bio, scmd->request) {
+		struct bio_integrity_payload *bip = bio_integrity(bio);
 		struct bio_vec iv;
 		struct bvec_iter iter;
 
-		virt = bio->bi_integrity->bip_iter.bi_sector & 0xffffffff;
+		virt = bip_get_seed(bip) & 0xffffffff;
 
-		bip_for_each_vec(iv, bio->bi_integrity, iter) {
-			sdt = kmap_atomic(iv.bv_page)
-				+ iv.bv_offset;
+		bip_for_each_vec(iv, bip, iter) {
+			pi = kmap_atomic(iv.bv_page) + iv.bv_offset;
 
-			for (j = 0; j < iv.bv_len; j += tuple_sz, sdt++) {
+			for (j = 0; j < iv.bv_len; j += tuple_sz, pi++) {
 
-				if (sectors == 0) {
-					kunmap_atomic(sdt);
+				if (intervals == 0) {
+					kunmap_atomic(pi);
 					return;
 				}
 
-				if (be32_to_cpu(sdt->ref_tag) == phys)
-					sdt->ref_tag = cpu_to_be32(virt);
+				if (be32_to_cpu(pi->ref_tag) == phys)
+					pi->ref_tag = cpu_to_be32(virt);
 
 				virt++;
 				phys++;
-				sectors--;
+				intervals--;
 			}
 
-			kunmap_atomic(sdt);
+			kunmap_atomic(pi);
 		}
 	}
 }
diff --git a/drivers/scsi/sg.c b/drivers/scsi/sg.c
index 01cf88888797..60354449d9ed 100644
--- a/drivers/scsi/sg.c
+++ b/drivers/scsi/sg.c
@@ -1711,9 +1711,9 @@ sg_start_req(Sg_request *srp, unsigned char *cmd)
 	}
 
 	rq = blk_get_request(q, rw, GFP_ATOMIC);
-	if (!rq) {
+	if (IS_ERR(rq)) {
 		kfree(long_cmdp);
-		return -ENOMEM;
+		return PTR_ERR(rq);
 	}
 
 	blk_rq_set_block_pc(rq);
diff --git a/drivers/scsi/sr.c b/drivers/scsi/sr.c
index 7eeb93627beb..2de44cc58b1a 100644
--- a/drivers/scsi/sr.c
+++ b/drivers/scsi/sr.c
@@ -657,6 +657,7 @@ static int sr_probe(struct device *dev)
 	struct scsi_cd *cd;
 	int minor, error;
 
+	scsi_autopm_get_device(sdev);
 	error = -ENODEV;
 	if (sdev->type != TYPE_ROM && sdev->type != TYPE_WORM)
 		goto fail;
@@ -744,6 +745,7 @@ fail_put:
 fail_free:
 	kfree(cd);
 fail:
+	scsi_autopm_put_device(sdev);
 	return error;
 }
 
diff --git a/drivers/scsi/st.c b/drivers/scsi/st.c
index aff9689de0f7..4daa372ed381 100644
--- a/drivers/scsi/st.c
+++ b/drivers/scsi/st.c
@@ -490,7 +490,7 @@ static int st_scsi_execute(struct st_request *SRpnt, const unsigned char *cmd,
 
 	req = blk_get_request(SRpnt->stp->device->request_queue, write,
 			      GFP_KERNEL);
-	if (!req)
+	if (IS_ERR(req))
 		return DRIVER_ERROR << 24;
 
 	blk_rq_set_block_pc(req);
@@ -4105,6 +4105,7 @@ static int st_probe(struct device *dev)
 		return -ENODEV;
 	}
 
+	scsi_autopm_get_device(SDp);
 	i = queue_max_segments(SDp->request_queue);
 	if (st_max_sg_segs < i)
 		i = st_max_sg_segs;
@@ -4244,6 +4245,7 @@ out_put_disk:
 out_buffer_free:
 	kfree(buffer);
 out:
+	scsi_autopm_put_device(SDp);
 	return -ENODEV;
 };
 
diff --git a/drivers/scsi/storvsc_drv.c b/drivers/scsi/storvsc_drv.c
index fecac5d03fdd..733e5f759518 100644
--- a/drivers/scsi/storvsc_drv.c
+++ b/drivers/scsi/storvsc_drv.c
@@ -1152,24 +1152,12 @@ static void storvsc_on_io_completion(struct hv_device *device,
 	stor_pkt->vm_srb.sense_info_length =
 	vstor_packet->vm_srb.sense_info_length;
 
-	if (vstor_packet->vm_srb.scsi_status != 0 ||
-		vstor_packet->vm_srb.srb_status != SRB_STATUS_SUCCESS){
-		dev_warn(&device->device,
-			 "cmd 0x%x scsi status 0x%x srb status 0x%x\n",
-			 stor_pkt->vm_srb.cdb[0],
-			 vstor_packet->vm_srb.scsi_status,
-			 vstor_packet->vm_srb.srb_status);
-	}
 
 	if ((vstor_packet->vm_srb.scsi_status & 0xFF) == 0x02) {
 		/* CHECK_CONDITION */
 		if (vstor_packet->vm_srb.srb_status &
 			SRB_STATUS_AUTOSENSE_VALID) {
 			/* autosense data available */
-			dev_warn(&device->device,
-				 "stor pkt %p autosense data valid - len %d\n",
-				 request,
-				 vstor_packet->vm_srb.sense_info_length);
 
 			memcpy(request->sense_buffer,
 			       vstor_packet->vm_srb.sense_data,
diff --git a/drivers/scsi/ufs/Kconfig b/drivers/scsi/ufs/Kconfig
index f07f90179bbc..6e07b2afddeb 100644
--- a/drivers/scsi/ufs/Kconfig
+++ b/drivers/scsi/ufs/Kconfig
@@ -35,6 +35,8 @@
 config SCSI_UFSHCD
 	tristate "Universal Flash Storage Controller Driver Core"
 	depends on SCSI && SCSI_DMA
+	select PM_DEVFREQ
+	select DEVFREQ_GOV_SIMPLE_ONDEMAND
 	---help---
 	This selects the support for UFS devices in Linux, say Y and make
 	  sure that you know the name of your UFS host adapter (the card
diff --git a/drivers/scsi/ufs/ufs.h b/drivers/scsi/ufs/ufs.h
index fafcf5e354c6..42c459a9d3fe 100644
--- a/drivers/scsi/ufs/ufs.h
+++ b/drivers/scsi/ufs/ufs.h
@@ -49,6 +49,27 @@
 #define UPIU_HEADER_DWORD(byte3, byte2, byte1, byte0)\
 			cpu_to_be32((byte3 << 24) | (byte2 << 16) |\
 			 (byte1 << 8) | (byte0))
+/*
+ * UFS device may have standard LUs and LUN id could be from 0x00 to
+ * 0x7F. Standard LUs use "Peripheral Device Addressing Format".
+ * UFS device may also have the Well Known LUs (also referred as W-LU)
+ * which again could be from 0x00 to 0x7F. For W-LUs, device only use
+ * the "Extended Addressing Format" which means the W-LUNs would be
+ * from 0xc100 (SCSI_W_LUN_BASE) onwards.
+ * This means max. LUN number reported from UFS device could be 0xC17F.
+ */
+#define UFS_UPIU_MAX_UNIT_NUM_ID	0x7F
+#define UFS_MAX_LUNS		(SCSI_W_LUN_BASE + UFS_UPIU_MAX_UNIT_NUM_ID)
+#define UFS_UPIU_WLUN_ID	(1 << 7)
+#define UFS_UPIU_MAX_GENERAL_LUN	8
+
+/* Well known logical unit id in LUN field of UPIU */
+enum {
+	UFS_UPIU_REPORT_LUNS_WLUN	= 0x81,
+	UFS_UPIU_UFS_DEVICE_WLUN	= 0xD0,
+	UFS_UPIU_BOOT_WLUN		= 0xB0,
+	UFS_UPIU_RPMB_WLUN		= 0xC4,
+};
 
 /*
  * UFS Protocol Information Unit related definitions
@@ -108,11 +129,13 @@ enum {
 /* Flag idn for Query Requests*/
 enum flag_idn {
 	QUERY_FLAG_IDN_FDEVICEINIT      = 0x01,
+	QUERY_FLAG_IDN_PWR_ON_WPE	= 0x03,
 	QUERY_FLAG_IDN_BKOPS_EN         = 0x04,
 };
 
 /* Attribute idn for Query requests */
 enum attr_idn {
+	QUERY_ATTR_IDN_ACTIVE_ICC_LVL	= 0x03,
 	QUERY_ATTR_IDN_BKOPS_STATUS	= 0x05,
 	QUERY_ATTR_IDN_EE_CONTROL	= 0x0D,
 	QUERY_ATTR_IDN_EE_STATUS	= 0x0E,
@@ -129,10 +152,29 @@ enum desc_idn {
 	QUERY_DESC_IDN_RFU_1		= 0x6,
 	QUERY_DESC_IDN_GEOMETRY		= 0x7,
 	QUERY_DESC_IDN_POWER		= 0x8,
-	QUERY_DESC_IDN_RFU_2		= 0x9,
+	QUERY_DESC_IDN_MAX,
+};
+
+enum desc_header_offset {
+	QUERY_DESC_LENGTH_OFFSET	= 0x00,
+	QUERY_DESC_DESC_TYPE_OFFSET	= 0x01,
+};
+
+enum ufs_desc_max_size {
+	QUERY_DESC_DEVICE_MAX_SIZE		= 0x1F,
+	QUERY_DESC_CONFIGURAION_MAX_SIZE	= 0x90,
+	QUERY_DESC_UNIT_MAX_SIZE		= 0x23,
+	QUERY_DESC_INTERCONNECT_MAX_SIZE	= 0x06,
+	/*
+	 * Max. 126 UNICODE characters (2 bytes per character) plus 2 bytes
+	 * of descriptor header.
+	 */
+	QUERY_DESC_STRING_MAX_SIZE		= 0xFE,
+	QUERY_DESC_GEOMETRY_MAZ_SIZE		= 0x44,
+	QUERY_DESC_POWER_MAX_SIZE		= 0x62,
+	QUERY_DESC_RFU_MAX_SIZE			= 0x00,
 };
 
-#define UNIT_DESC_MAX_SIZE       0x22
 /* Unit descriptor parameters offsets in bytes*/
 enum unit_desc_param {
 	UNIT_DESC_PARAM_LEN			= 0x0,
@@ -153,6 +195,43 @@ enum unit_desc_param {
 	UNIT_DESC_PARAM_LARGE_UNIT_SIZE_M1	= 0x22,
 };
 
+/*
+ * Logical Unit Write Protect
+ * 00h: LU not write protected
+ * 01h: LU write protected when fPowerOnWPEn =1
+ * 02h: LU permanently write protected when fPermanentWPEn =1
+ */
+enum ufs_lu_wp_type {
+	UFS_LU_NO_WP		= 0x00,
+	UFS_LU_POWER_ON_WP	= 0x01,
+	UFS_LU_PERM_WP		= 0x02,
+};
+
+/* bActiveICCLevel parameter current units */
+enum {
+	UFSHCD_NANO_AMP		= 0,
+	UFSHCD_MICRO_AMP	= 1,
+	UFSHCD_MILI_AMP		= 2,
+	UFSHCD_AMP		= 3,
+};
+
+#define POWER_DESC_MAX_SIZE			0x62
+#define POWER_DESC_MAX_ACTV_ICC_LVLS		16
+
+/* Attribute  bActiveICCLevel parameter bit masks definitions */
+#define ATTR_ICC_LVL_UNIT_OFFSET	14
+#define ATTR_ICC_LVL_UNIT_MASK		(0x3 << ATTR_ICC_LVL_UNIT_OFFSET)
+#define ATTR_ICC_LVL_VALUE_MASK		0x3FF
+
+/* Power descriptor parameters offsets in bytes */
+enum power_desc_param_offset {
+	PWR_DESC_LEN			= 0x0,
+	PWR_DESC_TYPE			= 0x1,
+	PWR_DESC_ACTIVE_LVLS_VCC_0	= 0x2,
+	PWR_DESC_ACTIVE_LVLS_VCCQ_0	= 0x22,
+	PWR_DESC_ACTIVE_LVLS_VCCQ2_0	= 0x42,
+};
+
 /* Exception event mask values */
 enum {
 	MASK_EE_STATUS		= 0xFFFF,
@@ -160,11 +239,12 @@ enum {
 };
 
 /* Background operation status */
-enum {
+enum bkops_status {
 	BKOPS_STATUS_NO_OP               = 0x0,
 	BKOPS_STATUS_NON_CRITICAL        = 0x1,
 	BKOPS_STATUS_PERF_IMPACT         = 0x2,
 	BKOPS_STATUS_CRITICAL            = 0x3,
+	BKOPS_STATUS_MAX		 = BKOPS_STATUS_CRITICAL,
 };
 
 /* UTP QUERY Transaction Specific Fields OpCode */
@@ -225,6 +305,14 @@ enum {
 	UPIU_TASK_MANAGEMENT_FUNC_FAILED	= 0x05,
 	UPIU_INCORRECT_LOGICAL_UNIT_NO		= 0x09,
 };
+
+/* UFS device power modes */
+enum ufs_dev_pwr_mode {
+	UFS_ACTIVE_PWR_MODE	= 1,
+	UFS_SLEEP_PWR_MODE	= 2,
+	UFS_POWERDOWN_PWR_MODE	= 3,
+};
+
 /**
  * struct utp_upiu_header - UPIU header structure
  * @dword_0: UPIU header DW-0
@@ -362,4 +450,42 @@ struct ufs_query_res {
 	struct utp_upiu_query upiu_res;
 };
 
+#define UFS_VREG_VCC_MIN_UV	   2700000 /* uV */
+#define UFS_VREG_VCC_MAX_UV	   3600000 /* uV */
+#define UFS_VREG_VCC_1P8_MIN_UV    1700000 /* uV */
+#define UFS_VREG_VCC_1P8_MAX_UV    1950000 /* uV */
+#define UFS_VREG_VCCQ_MIN_UV	   1100000 /* uV */
+#define UFS_VREG_VCCQ_MAX_UV	   1300000 /* uV */
+#define UFS_VREG_VCCQ2_MIN_UV	   1650000 /* uV */
+#define UFS_VREG_VCCQ2_MAX_UV	   1950000 /* uV */
+
+/*
+ * VCCQ & VCCQ2 current requirement when UFS device is in sleep state
+ * and link is in Hibern8 state.
+ */
+#define UFS_VREG_LPM_LOAD_UA	1000 /* uA */
+
+struct ufs_vreg {
+	struct regulator *reg;
+	const char *name;
+	bool enabled;
+	int min_uV;
+	int max_uV;
+	int min_uA;
+	int max_uA;
+};
+
+struct ufs_vreg_info {
+	struct ufs_vreg *vcc;
+	struct ufs_vreg *vccq;
+	struct ufs_vreg *vccq2;
+	struct ufs_vreg *vdd_hba;
+};
+
+struct ufs_dev_info {
+	bool f_power_on_wp_en;
+	/* Keeps information if any of the LU is power on write protected */
+	bool is_lu_power_on_wp;
+};
+
 #endif /* End of Header */
diff --git a/drivers/scsi/ufs/ufshcd-pci.c b/drivers/scsi/ufs/ufshcd-pci.c
index afaabe2aeac8..955ed5587011 100644
--- a/drivers/scsi/ufs/ufshcd-pci.c
+++ b/drivers/scsi/ufs/ufshcd-pci.c
@@ -43,34 +43,24 @@
  * @pdev: pointer to PCI device handle
  * @state: power state
  *
- * Returns -ENOSYS
+ * Returns 0 if successful
+ * Returns non-zero otherwise
  */
 static int ufshcd_pci_suspend(struct device *dev)
 {
-	/*
-	 * TODO:
-	 * 1. Call ufshcd_suspend
-	 * 2. Do bus specific power management
-	 */
-
-	return -ENOSYS;
+	return ufshcd_system_suspend(dev_get_drvdata(dev));
 }
 
 /**
  * ufshcd_pci_resume - resume power management function
  * @pdev: pointer to PCI device handle
  *
- * Returns -ENOSYS
+ * Returns 0 if successful
+ * Returns non-zero otherwise
  */
 static int ufshcd_pci_resume(struct device *dev)
 {
-	/*
-	 * TODO:
-	 * 1. Call ufshcd_resume.
-	 * 2. Do bus specific wake up
-	 */
-
-	return -ENOSYS;
+	return ufshcd_system_resume(dev_get_drvdata(dev));
 }
 #else
 #define ufshcd_pci_suspend	NULL
@@ -80,30 +70,15 @@ static int ufshcd_pci_resume(struct device *dev)
 #ifdef CONFIG_PM_RUNTIME
 static int ufshcd_pci_runtime_suspend(struct device *dev)
 {
-	struct ufs_hba *hba = dev_get_drvdata(dev);
-
-	if (!hba)
-		return 0;
-
-	return ufshcd_runtime_suspend(hba);
+	return ufshcd_runtime_suspend(dev_get_drvdata(dev));
 }
 static int ufshcd_pci_runtime_resume(struct device *dev)
 {
-	struct ufs_hba *hba = dev_get_drvdata(dev);
-
-	if (!hba)
-		return 0;
-
-	return ufshcd_runtime_resume(hba);
+	return ufshcd_runtime_resume(dev_get_drvdata(dev));
 }
 static int ufshcd_pci_runtime_idle(struct device *dev)
 {
-	struct ufs_hba *hba = dev_get_drvdata(dev);
-
-	if (!hba)
-		return 0;
-
-	return ufshcd_runtime_idle(hba);
+	return ufshcd_runtime_idle(dev_get_drvdata(dev));
 }
 #else /* !CONFIG_PM_RUNTIME */
 #define ufshcd_pci_runtime_suspend	NULL
@@ -117,7 +92,7 @@ static int ufshcd_pci_runtime_idle(struct device *dev)
  */
 static void ufshcd_pci_shutdown(struct pci_dev *pdev)
 {
-	ufshcd_hba_stop((struct ufs_hba *)pci_get_drvdata(pdev));
+	ufshcd_shutdown((struct ufs_hba *)pci_get_drvdata(pdev));
 }
 
 /**
@@ -164,7 +139,15 @@ ufshcd_pci_probe(struct pci_dev *pdev, const struct pci_device_id *id)
 
 	mmio_base = pcim_iomap_table(pdev)[0];
 
-	err = ufshcd_init(&pdev->dev, &hba, mmio_base, pdev->irq);
+	err = ufshcd_alloc_host(&pdev->dev, &hba);
+	if (err) {
+		dev_err(&pdev->dev, "Allocation failed\n");
+		return err;
+	}
+
+	INIT_LIST_HEAD(&hba->clk_list_head);
+
+	err = ufshcd_init(hba, mmio_base, pdev->irq);
 	if (err) {
 		dev_err(&pdev->dev, "Initialization failed\n");
 		return err;
diff --git a/drivers/scsi/ufs/ufshcd-pltfrm.c b/drivers/scsi/ufs/ufshcd-pltfrm.c
index 5e4623225422..8adf067ff019 100644
--- a/drivers/scsi/ufs/ufshcd-pltfrm.c
+++ b/drivers/scsi/ufs/ufshcd-pltfrm.c
@@ -35,53 +35,236 @@
 
 #include <linux/platform_device.h>
 #include <linux/pm_runtime.h>
+#include <linux/of.h>
 
 #include "ufshcd.h"
 
+static const struct of_device_id ufs_of_match[];
+static struct ufs_hba_variant_ops *get_variant_ops(struct device *dev)
+{
+	if (dev->of_node) {
+		const struct of_device_id *match;
+
+		match = of_match_node(ufs_of_match, dev->of_node);
+		if (match)
+			return (struct ufs_hba_variant_ops *)match->data;
+	}
+
+	return NULL;
+}
+
+static int ufshcd_parse_clock_info(struct ufs_hba *hba)
+{
+	int ret = 0;
+	int cnt;
+	int i;
+	struct device *dev = hba->dev;
+	struct device_node *np = dev->of_node;
+	char *name;
+	u32 *clkfreq = NULL;
+	struct ufs_clk_info *clki;
+	int len = 0;
+	size_t sz = 0;
+
+	if (!np)
+		goto out;
+
+	INIT_LIST_HEAD(&hba->clk_list_head);
+
+	cnt = of_property_count_strings(np, "clock-names");
+	if (!cnt || (cnt == -EINVAL)) {
+		dev_info(dev, "%s: Unable to find clocks, assuming enabled\n",
+				__func__);
+	} else if (cnt < 0) {
+		dev_err(dev, "%s: count clock strings failed, err %d\n",
+				__func__, cnt);
+		ret = cnt;
+	}
+
+	if (cnt <= 0)
+		goto out;
+
+	if (!of_get_property(np, "freq-table-hz", &len)) {
+		dev_info(dev, "freq-table-hz property not specified\n");
+		goto out;
+	}
+
+	if (len <= 0)
+		goto out;
+
+	sz = len / sizeof(*clkfreq);
+	if (sz != 2 * cnt) {
+		dev_err(dev, "%s len mismatch\n", "freq-table-hz");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	clkfreq = devm_kzalloc(dev, sz * sizeof(*clkfreq),
+			GFP_KERNEL);
+	if (!clkfreq) {
+		dev_err(dev, "%s: no memory\n", "freq-table-hz");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = of_property_read_u32_array(np, "freq-table-hz",
+			clkfreq, sz);
+	if (ret && (ret != -EINVAL)) {
+		dev_err(dev, "%s: error reading array %d\n",
+				"freq-table-hz", ret);
+		goto free_clkfreq;
+	}
+
+	for (i = 0; i < sz; i += 2) {
+		ret = of_property_read_string_index(np,
+				"clock-names", i/2, (const char **)&name);
+		if (ret)
+			goto free_clkfreq;
+
+		clki = devm_kzalloc(dev, sizeof(*clki), GFP_KERNEL);
+		if (!clki) {
+			ret = -ENOMEM;
+			goto free_clkfreq;
+		}
+
+		clki->min_freq = clkfreq[i];
+		clki->max_freq = clkfreq[i+1];
+		clki->name = kstrdup(name, GFP_KERNEL);
+		dev_dbg(dev, "%s: min %u max %u name %s\n", "freq-table-hz",
+				clki->min_freq, clki->max_freq, clki->name);
+		list_add_tail(&clki->list, &hba->clk_list_head);
+	}
+free_clkfreq:
+	kfree(clkfreq);
+out:
+	return ret;
+}
+
+#define MAX_PROP_SIZE 32
+static int ufshcd_populate_vreg(struct device *dev, const char *name,
+		struct ufs_vreg **out_vreg)
+{
+	int ret = 0;
+	char prop_name[MAX_PROP_SIZE];
+	struct ufs_vreg *vreg = NULL;
+	struct device_node *np = dev->of_node;
+
+	if (!np) {
+		dev_err(dev, "%s: non DT initialization\n", __func__);
+		goto out;
+	}
+
+	snprintf(prop_name, MAX_PROP_SIZE, "%s-supply", name);
+	if (!of_parse_phandle(np, prop_name, 0)) {
+		dev_info(dev, "%s: Unable to find %s regulator, assuming enabled\n",
+				__func__, prop_name);
+		goto out;
+	}
+
+	vreg = devm_kzalloc(dev, sizeof(*vreg), GFP_KERNEL);
+	if (!vreg) {
+		dev_err(dev, "No memory for %s regulator\n", name);
+		goto out;
+	}
+
+	vreg->name = kstrdup(name, GFP_KERNEL);
+
+	/* if fixed regulator no need further initialization */
+	snprintf(prop_name, MAX_PROP_SIZE, "%s-fixed-regulator", name);
+	if (of_property_read_bool(np, prop_name))
+		goto out;
+
+	snprintf(prop_name, MAX_PROP_SIZE, "%s-max-microamp", name);
+	ret = of_property_read_u32(np, prop_name, &vreg->max_uA);
+	if (ret) {
+		dev_err(dev, "%s: unable to find %s err %d\n",
+				__func__, prop_name, ret);
+		goto out_free;
+	}
+
+	vreg->min_uA = 0;
+	if (!strcmp(name, "vcc")) {
+		if (of_property_read_bool(np, "vcc-supply-1p8")) {
+			vreg->min_uV = UFS_VREG_VCC_1P8_MIN_UV;
+			vreg->max_uV = UFS_VREG_VCC_1P8_MAX_UV;
+		} else {
+			vreg->min_uV = UFS_VREG_VCC_MIN_UV;
+			vreg->max_uV = UFS_VREG_VCC_MAX_UV;
+		}
+	} else if (!strcmp(name, "vccq")) {
+		vreg->min_uV = UFS_VREG_VCCQ_MIN_UV;
+		vreg->max_uV = UFS_VREG_VCCQ_MAX_UV;
+	} else if (!strcmp(name, "vccq2")) {
+		vreg->min_uV = UFS_VREG_VCCQ2_MIN_UV;
+		vreg->max_uV = UFS_VREG_VCCQ2_MAX_UV;
+	}
+
+	goto out;
+
+out_free:
+	devm_kfree(dev, vreg);
+	vreg = NULL;
+out:
+	if (!ret)
+		*out_vreg = vreg;
+	return ret;
+}
+
+/**
+ * ufshcd_parse_regulator_info - get regulator info from device tree
+ * @hba: per adapter instance
+ *
+ * Get regulator info from device tree for vcc, vccq, vccq2 power supplies.
+ * If any of the supplies are not defined it is assumed that they are always-on
+ * and hence return zero. If the property is defined but parsing is failed
+ * then return corresponding error.
+ */
+static int ufshcd_parse_regulator_info(struct ufs_hba *hba)
+{
+	int err;
+	struct device *dev = hba->dev;
+	struct ufs_vreg_info *info = &hba->vreg_info;
+
+	err = ufshcd_populate_vreg(dev, "vdd-hba", &info->vdd_hba);
+	if (err)
+		goto out;
+
+	err = ufshcd_populate_vreg(dev, "vcc", &info->vcc);
+	if (err)
+		goto out;
+
+	err = ufshcd_populate_vreg(dev, "vccq", &info->vccq);
+	if (err)
+		goto out;
+
+	err = ufshcd_populate_vreg(dev, "vccq2", &info->vccq2);
+out:
+	return err;
+}
+
 #ifdef CONFIG_PM
 /**
  * ufshcd_pltfrm_suspend - suspend power management function
  * @dev: pointer to device handle
  *
- *
- * Returns 0
+ * Returns 0 if successful
+ * Returns non-zero otherwise
  */
 static int ufshcd_pltfrm_suspend(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct ufs_hba *hba =  platform_get_drvdata(pdev);
-
-	/*
-	 * TODO:
-	 * 1. Call ufshcd_suspend
-	 * 2. Do bus specific power management
-	 */
-
-	disable_irq(hba->irq);
-
-	return 0;
+	return ufshcd_system_suspend(dev_get_drvdata(dev));
 }
 
 /**
  * ufshcd_pltfrm_resume - resume power management function
  * @dev: pointer to device handle
  *
- * Returns 0
+ * Returns 0 if successful
+ * Returns non-zero otherwise
  */
 static int ufshcd_pltfrm_resume(struct device *dev)
 {
-	struct platform_device *pdev = to_platform_device(dev);
-	struct ufs_hba *hba =  platform_get_drvdata(pdev);
-
-	/*
-	 * TODO:
-	 * 1. Call ufshcd_resume.
-	 * 2. Do bus specific wake up
-	 */
-
-	enable_irq(hba->irq);
-
-	return 0;
+	return ufshcd_system_resume(dev_get_drvdata(dev));
 }
 #else
 #define ufshcd_pltfrm_suspend	NULL
@@ -91,30 +274,15 @@ static int ufshcd_pltfrm_resume(struct device *dev)
 #ifdef CONFIG_PM_RUNTIME
 static int ufshcd_pltfrm_runtime_suspend(struct device *dev)
 {
-	struct ufs_hba *hba =  dev_get_drvdata(dev);
-
-	if (!hba)
-		return 0;
-
-	return ufshcd_runtime_suspend(hba);
+	return ufshcd_runtime_suspend(dev_get_drvdata(dev));
 }
 static int ufshcd_pltfrm_runtime_resume(struct device *dev)
 {
-	struct ufs_hba *hba =  dev_get_drvdata(dev);
-
-	if (!hba)
-		return 0;
-
-	return ufshcd_runtime_resume(hba);
+	return ufshcd_runtime_resume(dev_get_drvdata(dev));
 }
 static int ufshcd_pltfrm_runtime_idle(struct device *dev)
 {
-	struct ufs_hba *hba =  dev_get_drvdata(dev);
-
-	if (!hba)
-		return 0;
-
-	return ufshcd_runtime_idle(hba);
+	return ufshcd_runtime_idle(dev_get_drvdata(dev));
 }
 #else /* !CONFIG_PM_RUNTIME */
 #define ufshcd_pltfrm_runtime_suspend	NULL
@@ -122,6 +290,11 @@ static int ufshcd_pltfrm_runtime_idle(struct device *dev)
 #define ufshcd_pltfrm_runtime_idle	NULL
 #endif /* CONFIG_PM_RUNTIME */
 
+static void ufshcd_pltfrm_shutdown(struct platform_device *pdev)
+{
+	ufshcd_shutdown((struct ufs_hba *)platform_get_drvdata(pdev));
+}
+
 /**
  * ufshcd_pltfrm_probe - probe routine of the driver
  * @pdev: pointer to Platform device handle
@@ -138,8 +311,8 @@ static int ufshcd_pltfrm_probe(struct platform_device *pdev)
 
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	mmio_base = devm_ioremap_resource(dev, mem_res);
-	if (IS_ERR(mmio_base)) {
-		err = PTR_ERR(mmio_base);
+	if (IS_ERR(*(void **)&mmio_base)) {
+		err = PTR_ERR(*(void **)&mmio_base);
 		goto out;
 	}
 
@@ -150,10 +323,31 @@ static int ufshcd_pltfrm_probe(struct platform_device *pdev)
 		goto out;
 	}
 
+	err = ufshcd_alloc_host(dev, &hba);
+	if (err) {
+		dev_err(&pdev->dev, "Allocation failed\n");
+		goto out;
+	}
+
+	hba->vops = get_variant_ops(&pdev->dev);
+
+	err = ufshcd_parse_clock_info(hba);
+	if (err) {
+		dev_err(&pdev->dev, "%s: clock parse failed %d\n",
+				__func__, err);
+		goto out;
+	}
+	err = ufshcd_parse_regulator_info(hba);
+	if (err) {
+		dev_err(&pdev->dev, "%s: regulator init failed %d\n",
+				__func__, err);
+		goto out;
+	}
+
 	pm_runtime_set_active(&pdev->dev);
 	pm_runtime_enable(&pdev->dev);
 
-	err = ufshcd_init(dev, &hba, mmio_base, irq);
+	err = ufshcd_init(hba, mmio_base, irq);
 	if (err) {
 		dev_err(dev, "Intialization failed\n");
 		goto out_disable_rpm;
@@ -201,6 +395,7 @@ static const struct dev_pm_ops ufshcd_dev_pm_ops = {
 static struct platform_driver ufshcd_pltfrm_driver = {
 	.probe	= ufshcd_pltfrm_probe,
 	.remove	= ufshcd_pltfrm_remove,
+	.shutdown = ufshcd_pltfrm_shutdown,
 	.driver	= {
 		.name	= "ufshcd",
 		.owner	= THIS_MODULE,
diff --git a/drivers/scsi/ufs/ufshcd.c b/drivers/scsi/ufs/ufshcd.c
index ba27215b8034..497c38a4a866 100644
--- a/drivers/scsi/ufs/ufshcd.c
+++ b/drivers/scsi/ufs/ufshcd.c
@@ -3,6 +3,7 @@
  *
  * This code is based on drivers/scsi/ufs/ufshcd.c
  * Copyright (C) 2011-2013 Samsung India Software Operations
+ * Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
  *
  * Authors:
  *	Santosh Yaraganavi <santosh.sy@samsung.com>
@@ -31,16 +32,19 @@
  * circumstances will the contributor of this Program be liable for
  * any damages of any kind arising from your use or distribution of
  * this program.
+ *
+ * The Linux Foundation chooses to take subject only to the GPLv2
+ * license terms, and distributes only under these terms.
  */
 
 #include <linux/async.h>
+#include <linux/devfreq.h>
 
 #include "ufshcd.h"
 #include "unipro.h"
 
 #define UFSHCD_ENABLE_INTRS	(UTP_TRANSFER_REQ_COMPL |\
 				 UTP_TASK_REQ_COMPL |\
-				 UIC_POWER_MODE |\
 				 UFSHCD_ERROR_MASK)
 /* UIC command timeout, unit: ms */
 #define UIC_CMD_TIMEOUT	500
@@ -58,16 +62,44 @@
 /* Task management command timeout */
 #define TM_CMD_TIMEOUT	100 /* msecs */
 
+/* maximum number of link-startup retries */
+#define DME_LINKSTARTUP_RETRIES 3
+
+/* maximum number of reset retries before giving up */
+#define MAX_HOST_RESET_RETRIES 5
+
 /* Expose the flag value from utp_upiu_query.value */
 #define MASK_QUERY_UPIU_FLAG_LOC 0xFF
 
 /* Interrupt aggregation default timeout, unit: 40us */
 #define INT_AGGR_DEF_TO	0x02
 
+#define ufshcd_toggle_vreg(_dev, _vreg, _on)				\
+	({                                                              \
+		int _ret;                                               \
+		if (_on)                                                \
+			_ret = ufshcd_enable_vreg(_dev, _vreg);         \
+		else                                                    \
+			_ret = ufshcd_disable_vreg(_dev, _vreg);        \
+		_ret;                                                   \
+	})
+
+static u32 ufs_query_desc_max_size[] = {
+	QUERY_DESC_DEVICE_MAX_SIZE,
+	QUERY_DESC_CONFIGURAION_MAX_SIZE,
+	QUERY_DESC_UNIT_MAX_SIZE,
+	QUERY_DESC_RFU_MAX_SIZE,
+	QUERY_DESC_INTERCONNECT_MAX_SIZE,
+	QUERY_DESC_STRING_MAX_SIZE,
+	QUERY_DESC_RFU_MAX_SIZE,
+	QUERY_DESC_GEOMETRY_MAZ_SIZE,
+	QUERY_DESC_POWER_MAX_SIZE,
+	QUERY_DESC_RFU_MAX_SIZE,
+};
+
 enum {
 	UFSHCD_MAX_CHANNEL	= 0,
 	UFSHCD_MAX_ID		= 1,
-	UFSHCD_MAX_LUNS		= 8,
 	UFSHCD_CMD_PER_LUN	= 32,
 	UFSHCD_CAN_QUEUE	= 32,
 };
@@ -106,12 +138,79 @@ enum {
 #define ufshcd_clear_eh_in_progress(h) \
 	(h->eh_flags &= ~UFSHCD_EH_IN_PROGRESS)
 
+#define ufshcd_set_ufs_dev_active(h) \
+	((h)->curr_dev_pwr_mode = UFS_ACTIVE_PWR_MODE)
+#define ufshcd_set_ufs_dev_sleep(h) \
+	((h)->curr_dev_pwr_mode = UFS_SLEEP_PWR_MODE)
+#define ufshcd_set_ufs_dev_poweroff(h) \
+	((h)->curr_dev_pwr_mode = UFS_POWERDOWN_PWR_MODE)
+#define ufshcd_is_ufs_dev_active(h) \
+	((h)->curr_dev_pwr_mode == UFS_ACTIVE_PWR_MODE)
+#define ufshcd_is_ufs_dev_sleep(h) \
+	((h)->curr_dev_pwr_mode == UFS_SLEEP_PWR_MODE)
+#define ufshcd_is_ufs_dev_poweroff(h) \
+	((h)->curr_dev_pwr_mode == UFS_POWERDOWN_PWR_MODE)
+
+static struct ufs_pm_lvl_states ufs_pm_lvl_states[] = {
+	{UFS_ACTIVE_PWR_MODE, UIC_LINK_ACTIVE_STATE},
+	{UFS_ACTIVE_PWR_MODE, UIC_LINK_HIBERN8_STATE},
+	{UFS_SLEEP_PWR_MODE, UIC_LINK_ACTIVE_STATE},
+	{UFS_SLEEP_PWR_MODE, UIC_LINK_HIBERN8_STATE},
+	{UFS_POWERDOWN_PWR_MODE, UIC_LINK_HIBERN8_STATE},
+	{UFS_POWERDOWN_PWR_MODE, UIC_LINK_OFF_STATE},
+};
+
+static inline enum ufs_dev_pwr_mode
+ufs_get_pm_lvl_to_dev_pwr_mode(enum ufs_pm_level lvl)
+{
+	return ufs_pm_lvl_states[lvl].dev_state;
+}
+
+static inline enum uic_link_state
+ufs_get_pm_lvl_to_link_pwr_state(enum ufs_pm_level lvl)
+{
+	return ufs_pm_lvl_states[lvl].link_state;
+}
+
 static void ufshcd_tmc_handler(struct ufs_hba *hba);
 static void ufshcd_async_scan(void *data, async_cookie_t cookie);
 static int ufshcd_reset_and_restore(struct ufs_hba *hba);
 static int ufshcd_clear_tm_cmd(struct ufs_hba *hba, int tag);
-static int ufshcd_read_sdev_qdepth(struct ufs_hba *hba,
-					struct scsi_device *sdev);
+static void ufshcd_hba_exit(struct ufs_hba *hba);
+static int ufshcd_probe_hba(struct ufs_hba *hba);
+static int __ufshcd_setup_clocks(struct ufs_hba *hba, bool on,
+				 bool skip_ref_clk);
+static int ufshcd_setup_clocks(struct ufs_hba *hba, bool on);
+static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba);
+static int ufshcd_uic_hibern8_enter(struct ufs_hba *hba);
+static int ufshcd_host_reset_and_restore(struct ufs_hba *hba);
+static irqreturn_t ufshcd_intr(int irq, void *__hba);
+static int ufshcd_config_pwr_mode(struct ufs_hba *hba,
+		struct ufs_pa_layer_attr *desired_pwr_mode);
+
+static inline int ufshcd_enable_irq(struct ufs_hba *hba)
+{
+	int ret = 0;
+
+	if (!hba->is_irq_enabled) {
+		ret = request_irq(hba->irq, ufshcd_intr, IRQF_SHARED, UFSHCD,
+				hba);
+		if (ret)
+			dev_err(hba->dev, "%s: request_irq failed, ret=%d\n",
+				__func__, ret);
+		hba->is_irq_enabled = true;
+	}
+
+	return ret;
+}
+
+static inline void ufshcd_disable_irq(struct ufs_hba *hba)
+{
+	if (hba->is_irq_enabled) {
+		free_irq(hba->irq, hba);
+		hba->is_irq_enabled = false;
+	}
+}
 
 /*
  * ufshcd_wait_for_register - wait for register value to change
@@ -175,13 +274,14 @@ static inline u32 ufshcd_get_ufs_version(struct ufs_hba *hba)
 /**
  * ufshcd_is_device_present - Check if any device connected to
  *			      the host controller
- * @reg_hcs - host controller status register value
+ * @hba: pointer to adapter instance
  *
  * Returns 1 if device present, 0 if no device detected
  */
-static inline int ufshcd_is_device_present(u32 reg_hcs)
+static inline int ufshcd_is_device_present(struct ufs_hba *hba)
 {
-	return (DEVICE_PRESENT & reg_hcs) ? 1 : 0;
+	return (ufshcd_readl(hba, REG_CONTROLLER_STATUS) &
+						DEVICE_PRESENT) ? 1 : 0;
 }
 
 /**
@@ -413,6 +513,265 @@ static inline int ufshcd_is_hba_active(struct ufs_hba *hba)
 	return (ufshcd_readl(hba, REG_CONTROLLER_ENABLE) & 0x1) ? 0 : 1;
 }
 
+static void ufshcd_ungate_work(struct work_struct *work)
+{
+	int ret;
+	unsigned long flags;
+	struct ufs_hba *hba = container_of(work, struct ufs_hba,
+			clk_gating.ungate_work);
+
+	cancel_delayed_work_sync(&hba->clk_gating.gate_work);
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	if (hba->clk_gating.state == CLKS_ON) {
+		spin_unlock_irqrestore(hba->host->host_lock, flags);
+		goto unblock_reqs;
+	}
+
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+	ufshcd_setup_clocks(hba, true);
+
+	/* Exit from hibern8 */
+	if (ufshcd_can_hibern8_during_gating(hba)) {
+		/* Prevent gating in this path */
+		hba->clk_gating.is_suspended = true;
+		if (ufshcd_is_link_hibern8(hba)) {
+			ret = ufshcd_uic_hibern8_exit(hba);
+			if (ret)
+				dev_err(hba->dev, "%s: hibern8 exit failed %d\n",
+					__func__, ret);
+			else
+				ufshcd_set_link_active(hba);
+		}
+		hba->clk_gating.is_suspended = false;
+	}
+unblock_reqs:
+	if (ufshcd_is_clkscaling_enabled(hba))
+		devfreq_resume_device(hba->devfreq);
+	scsi_unblock_requests(hba->host);
+}
+
+/**
+ * ufshcd_hold - Enable clocks that were gated earlier due to ufshcd_release.
+ * Also, exit from hibern8 mode and set the link as active.
+ * @hba: per adapter instance
+ * @async: This indicates whether caller should ungate clocks asynchronously.
+ */
+int ufshcd_hold(struct ufs_hba *hba, bool async)
+{
+	int rc = 0;
+	unsigned long flags;
+
+	if (!ufshcd_is_clkgating_allowed(hba))
+		goto out;
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	hba->clk_gating.active_reqs++;
+
+start:
+	switch (hba->clk_gating.state) {
+	case CLKS_ON:
+		break;
+	case REQ_CLKS_OFF:
+		if (cancel_delayed_work(&hba->clk_gating.gate_work)) {
+			hba->clk_gating.state = CLKS_ON;
+			break;
+		}
+		/*
+		 * If we here, it means gating work is either done or
+		 * currently running. Hence, fall through to cancel gating
+		 * work and to enable clocks.
+		 */
+	case CLKS_OFF:
+		scsi_block_requests(hba->host);
+		hba->clk_gating.state = REQ_CLKS_ON;
+		schedule_work(&hba->clk_gating.ungate_work);
+		/*
+		 * fall through to check if we should wait for this
+		 * work to be done or not.
+		 */
+	case REQ_CLKS_ON:
+		if (async) {
+			rc = -EAGAIN;
+			hba->clk_gating.active_reqs--;
+			break;
+		}
+
+		spin_unlock_irqrestore(hba->host->host_lock, flags);
+		flush_work(&hba->clk_gating.ungate_work);
+		/* Make sure state is CLKS_ON before returning */
+		spin_lock_irqsave(hba->host->host_lock, flags);
+		goto start;
+	default:
+		dev_err(hba->dev, "%s: clk gating is in invalid state %d\n",
+				__func__, hba->clk_gating.state);
+		break;
+	}
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+out:
+	return rc;
+}
+
+static void ufshcd_gate_work(struct work_struct *work)
+{
+	struct ufs_hba *hba = container_of(work, struct ufs_hba,
+			clk_gating.gate_work.work);
+	unsigned long flags;
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	if (hba->clk_gating.is_suspended) {
+		hba->clk_gating.state = CLKS_ON;
+		goto rel_lock;
+	}
+
+	if (hba->clk_gating.active_reqs
+		|| hba->ufshcd_state != UFSHCD_STATE_OPERATIONAL
+		|| hba->lrb_in_use || hba->outstanding_tasks
+		|| hba->active_uic_cmd || hba->uic_async_done)
+		goto rel_lock;
+
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+
+	/* put the link into hibern8 mode before turning off clocks */
+	if (ufshcd_can_hibern8_during_gating(hba)) {
+		if (ufshcd_uic_hibern8_enter(hba)) {
+			hba->clk_gating.state = CLKS_ON;
+			goto out;
+		}
+		ufshcd_set_link_hibern8(hba);
+	}
+
+	if (ufshcd_is_clkscaling_enabled(hba)) {
+		devfreq_suspend_device(hba->devfreq);
+		hba->clk_scaling.window_start_t = 0;
+	}
+
+	if (!ufshcd_is_link_active(hba))
+		ufshcd_setup_clocks(hba, false);
+	else
+		/* If link is active, device ref_clk can't be switched off */
+		__ufshcd_setup_clocks(hba, false, true);
+
+	/*
+	 * In case you are here to cancel this work the gating state
+	 * would be marked as REQ_CLKS_ON. In this case keep the state
+	 * as REQ_CLKS_ON which would anyway imply that clocks are off
+	 * and a request to turn them on is pending. By doing this way,
+	 * we keep the state machine in tact and this would ultimately
+	 * prevent from doing cancel work multiple times when there are
+	 * new requests arriving before the current cancel work is done.
+	 */
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	if (hba->clk_gating.state == REQ_CLKS_OFF)
+		hba->clk_gating.state = CLKS_OFF;
+
+rel_lock:
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+out:
+	return;
+}
+
+/* host lock must be held before calling this variant */
+static void __ufshcd_release(struct ufs_hba *hba)
+{
+	if (!ufshcd_is_clkgating_allowed(hba))
+		return;
+
+	hba->clk_gating.active_reqs--;
+
+	if (hba->clk_gating.active_reqs || hba->clk_gating.is_suspended
+		|| hba->ufshcd_state != UFSHCD_STATE_OPERATIONAL
+		|| hba->lrb_in_use || hba->outstanding_tasks
+		|| hba->active_uic_cmd || hba->uic_async_done)
+		return;
+
+	hba->clk_gating.state = REQ_CLKS_OFF;
+	schedule_delayed_work(&hba->clk_gating.gate_work,
+			msecs_to_jiffies(hba->clk_gating.delay_ms));
+}
+
+void ufshcd_release(struct ufs_hba *hba)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	__ufshcd_release(hba);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+}
+
+static ssize_t ufshcd_clkgate_delay_show(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct ufs_hba *hba = dev_get_drvdata(dev);
+
+	return snprintf(buf, PAGE_SIZE, "%lu\n", hba->clk_gating.delay_ms);
+}
+
+static ssize_t ufshcd_clkgate_delay_store(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct ufs_hba *hba = dev_get_drvdata(dev);
+	unsigned long flags, value;
+
+	if (kstrtoul(buf, 0, &value))
+		return -EINVAL;
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	hba->clk_gating.delay_ms = value;
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+	return count;
+}
+
+static void ufshcd_init_clk_gating(struct ufs_hba *hba)
+{
+	if (!ufshcd_is_clkgating_allowed(hba))
+		return;
+
+	hba->clk_gating.delay_ms = 150;
+	INIT_DELAYED_WORK(&hba->clk_gating.gate_work, ufshcd_gate_work);
+	INIT_WORK(&hba->clk_gating.ungate_work, ufshcd_ungate_work);
+
+	hba->clk_gating.delay_attr.show = ufshcd_clkgate_delay_show;
+	hba->clk_gating.delay_attr.store = ufshcd_clkgate_delay_store;
+	sysfs_attr_init(&hba->clk_gating.delay_attr.attr);
+	hba->clk_gating.delay_attr.attr.name = "clkgate_delay_ms";
+	hba->clk_gating.delay_attr.attr.mode = S_IRUGO | S_IWUSR;
+	if (device_create_file(hba->dev, &hba->clk_gating.delay_attr))
+		dev_err(hba->dev, "Failed to create sysfs for clkgate_delay\n");
+}
+
+static void ufshcd_exit_clk_gating(struct ufs_hba *hba)
+{
+	if (!ufshcd_is_clkgating_allowed(hba))
+		return;
+	device_remove_file(hba->dev, &hba->clk_gating.delay_attr);
+}
+
+/* Must be called with host lock acquired */
+static void ufshcd_clk_scaling_start_busy(struct ufs_hba *hba)
+{
+	if (!ufshcd_is_clkscaling_enabled(hba))
+		return;
+
+	if (!hba->clk_scaling.is_busy_started) {
+		hba->clk_scaling.busy_start_t = ktime_get();
+		hba->clk_scaling.is_busy_started = true;
+	}
+}
+
+static void ufshcd_clk_scaling_update_busy(struct ufs_hba *hba)
+{
+	struct ufs_clk_scaling *scaling = &hba->clk_scaling;
+
+	if (!ufshcd_is_clkscaling_enabled(hba))
+		return;
+
+	if (!hba->outstanding_reqs && scaling->is_busy_started) {
+		scaling->tot_busy_t += ktime_to_us(ktime_sub(ktime_get(),
+					scaling->busy_start_t));
+		scaling->busy_start_t = ktime_set(0, 0);
+		scaling->is_busy_started = false;
+	}
+}
 /**
  * ufshcd_send_command - Send SCSI or device management commands
  * @hba: per adapter instance
@@ -421,6 +780,7 @@ static inline int ufshcd_is_hba_active(struct ufs_hba *hba)
 static inline
 void ufshcd_send_command(struct ufs_hba *hba, unsigned int task_tag)
 {
+	ufshcd_clk_scaling_start_busy(hba);
 	__set_bit(task_tag, &hba->outstanding_reqs);
 	ufshcd_writel(hba, 1 << task_tag, REG_UTP_TRANSFER_REQ_DOOR_BELL);
 }
@@ -576,15 +936,12 @@ ufshcd_wait_for_uic_cmd(struct ufs_hba *hba, struct uic_command *uic_cmd)
  * @uic_cmd: UIC command
  *
  * Identical to ufshcd_send_uic_cmd() expect mutex. Must be called
- * with mutex held.
+ * with mutex held and host_lock locked.
  * Returns 0 only if success.
  */
 static int
 __ufshcd_send_uic_cmd(struct ufs_hba *hba, struct uic_command *uic_cmd)
 {
-	int ret;
-	unsigned long flags;
-
 	if (!ufshcd_ready_for_uic_cmd(hba)) {
 		dev_err(hba->dev,
 			"Controller not ready to accept UIC commands\n");
@@ -593,13 +950,9 @@ __ufshcd_send_uic_cmd(struct ufs_hba *hba, struct uic_command *uic_cmd)
 
 	init_completion(&uic_cmd->done);
 
-	spin_lock_irqsave(hba->host->host_lock, flags);
 	ufshcd_dispatch_uic_cmd(hba, uic_cmd);
-	spin_unlock_irqrestore(hba->host->host_lock, flags);
-
-	ret = ufshcd_wait_for_uic_cmd(hba, uic_cmd);
 
-	return ret;
+	return 0;
 }
 
 /**
@@ -613,11 +966,19 @@ static int
 ufshcd_send_uic_cmd(struct ufs_hba *hba, struct uic_command *uic_cmd)
 {
 	int ret;
+	unsigned long flags;
 
+	ufshcd_hold(hba, false);
 	mutex_lock(&hba->uic_cmd_mutex);
+	spin_lock_irqsave(hba->host->host_lock, flags);
 	ret = __ufshcd_send_uic_cmd(hba, uic_cmd);
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+	if (!ret)
+		ret = ufshcd_wait_for_uic_cmd(hba, uic_cmd);
+
 	mutex_unlock(&hba->uic_cmd_mutex);
 
+	ufshcd_release(hba);
 	return ret;
 }
 
@@ -867,6 +1228,32 @@ static int ufshcd_compose_upiu(struct ufs_hba *hba, struct ufshcd_lrb *lrbp)
 	return ret;
 }
 
+/*
+ * ufshcd_scsi_to_upiu_lun - maps scsi LUN to UPIU LUN
+ * @scsi_lun: scsi LUN id
+ *
+ * Returns UPIU LUN id
+ */
+static inline u8 ufshcd_scsi_to_upiu_lun(unsigned int scsi_lun)
+{
+	if (scsi_is_wlun(scsi_lun))
+		return (scsi_lun & UFS_UPIU_MAX_UNIT_NUM_ID)
+			| UFS_UPIU_WLUN_ID;
+	else
+		return scsi_lun & UFS_UPIU_MAX_UNIT_NUM_ID;
+}
+
+/**
+ * ufshcd_upiu_wlun_to_scsi_wlun - maps UPIU W-LUN id to SCSI W-LUN ID
+ * @scsi_lun: UPIU W-LUN id
+ *
+ * Returns SCSI W-LUN id
+ */
+static inline u16 ufshcd_upiu_wlun_to_scsi_wlun(u8 upiu_wlun_id)
+{
+	return (upiu_wlun_id & ~UFS_UPIU_WLUN_ID) | SCSI_W_LUN_BASE;
+}
+
 /**
  * ufshcd_queuecommand - main entry point for SCSI requests
  * @cmd: command from SCSI Midlayer
@@ -918,6 +1305,14 @@ static int ufshcd_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 		goto out;
 	}
 
+	err = ufshcd_hold(hba, true);
+	if (err) {
+		err = SCSI_MLQUEUE_HOST_BUSY;
+		clear_bit_unlock(tag, &hba->lrb_in_use);
+		goto out;
+	}
+	WARN_ON(hba->clk_gating.state != CLKS_ON);
+
 	lrbp = &hba->lrb[tag];
 
 	WARN_ON(lrbp->cmd);
@@ -925,7 +1320,7 @@ static int ufshcd_queuecommand(struct Scsi_Host *host, struct scsi_cmnd *cmd)
 	lrbp->sense_bufflen = SCSI_SENSE_BUFFERSIZE;
 	lrbp->sense_buffer = cmd->sense_buffer;
 	lrbp->task_tag = tag;
-	lrbp->lun = cmd->device->lun;
+	lrbp->lun = ufshcd_scsi_to_upiu_lun(cmd->device->lun);
 	lrbp->intr_cmd = false;
 	lrbp->command_type = UTP_CMD_TYPE_SCSI;
 
@@ -1193,6 +1588,7 @@ static int ufshcd_query_flag(struct ufs_hba *hba, enum query_opcode opcode,
 
 	BUG_ON(!hba);
 
+	ufshcd_hold(hba, false);
 	mutex_lock(&hba->dev_cmd.lock);
 	ufshcd_init_query(hba, &request, &response, opcode, idn, index,
 			selector);
@@ -1236,6 +1632,7 @@ static int ufshcd_query_flag(struct ufs_hba *hba, enum query_opcode opcode,
 
 out_unlock:
 	mutex_unlock(&hba->dev_cmd.lock);
+	ufshcd_release(hba);
 	return err;
 }
 
@@ -1259,6 +1656,7 @@ static int ufshcd_query_attr(struct ufs_hba *hba, enum query_opcode opcode,
 
 	BUG_ON(!hba);
 
+	ufshcd_hold(hba, false);
 	if (!attr_val) {
 		dev_err(hba->dev, "%s: attribute value required for opcode 0x%x\n",
 				__func__, opcode);
@@ -1298,6 +1696,7 @@ static int ufshcd_query_attr(struct ufs_hba *hba, enum query_opcode opcode,
 out_unlock:
 	mutex_unlock(&hba->dev_cmd.lock);
 out:
+	ufshcd_release(hba);
 	return err;
 }
 
@@ -1325,6 +1724,7 @@ static int ufshcd_query_descriptor(struct ufs_hba *hba,
 
 	BUG_ON(!hba);
 
+	ufshcd_hold(hba, false);
 	if (!desc_buf) {
 		dev_err(hba->dev, "%s: descriptor buffer required for opcode 0x%x\n",
 				__func__, opcode);
@@ -1374,10 +1774,120 @@ static int ufshcd_query_descriptor(struct ufs_hba *hba,
 out_unlock:
 	mutex_unlock(&hba->dev_cmd.lock);
 out:
+	ufshcd_release(hba);
 	return err;
 }
 
 /**
+ * ufshcd_read_desc_param - read the specified descriptor parameter
+ * @hba: Pointer to adapter instance
+ * @desc_id: descriptor idn value
+ * @desc_index: descriptor index
+ * @param_offset: offset of the parameter to read
+ * @param_read_buf: pointer to buffer where parameter would be read
+ * @param_size: sizeof(param_read_buf)
+ *
+ * Return 0 in case of success, non-zero otherwise
+ */
+static int ufshcd_read_desc_param(struct ufs_hba *hba,
+				  enum desc_idn desc_id,
+				  int desc_index,
+				  u32 param_offset,
+				  u8 *param_read_buf,
+				  u32 param_size)
+{
+	int ret;
+	u8 *desc_buf;
+	u32 buff_len;
+	bool is_kmalloc = true;
+
+	/* safety checks */
+	if (desc_id >= QUERY_DESC_IDN_MAX)
+		return -EINVAL;
+
+	buff_len = ufs_query_desc_max_size[desc_id];
+	if ((param_offset + param_size) > buff_len)
+		return -EINVAL;
+
+	if (!param_offset && (param_size == buff_len)) {
+		/* memory space already available to hold full descriptor */
+		desc_buf = param_read_buf;
+		is_kmalloc = false;
+	} else {
+		/* allocate memory to hold full descriptor */
+		desc_buf = kmalloc(buff_len, GFP_KERNEL);
+		if (!desc_buf)
+			return -ENOMEM;
+	}
+
+	ret = ufshcd_query_descriptor(hba, UPIU_QUERY_OPCODE_READ_DESC,
+				      desc_id, desc_index, 0, desc_buf,
+				      &buff_len);
+
+	if (ret || (buff_len < ufs_query_desc_max_size[desc_id]) ||
+	    (desc_buf[QUERY_DESC_LENGTH_OFFSET] !=
+	     ufs_query_desc_max_size[desc_id])
+	    || (desc_buf[QUERY_DESC_DESC_TYPE_OFFSET] != desc_id)) {
+		dev_err(hba->dev, "%s: Failed reading descriptor. desc_id %d param_offset %d buff_len %d ret %d",
+			__func__, desc_id, param_offset, buff_len, ret);
+		if (!ret)
+			ret = -EINVAL;
+
+		goto out;
+	}
+
+	if (is_kmalloc)
+		memcpy(param_read_buf, &desc_buf[param_offset], param_size);
+out:
+	if (is_kmalloc)
+		kfree(desc_buf);
+	return ret;
+}
+
+static inline int ufshcd_read_desc(struct ufs_hba *hba,
+				   enum desc_idn desc_id,
+				   int desc_index,
+				   u8 *buf,
+				   u32 size)
+{
+	return ufshcd_read_desc_param(hba, desc_id, desc_index, 0, buf, size);
+}
+
+static inline int ufshcd_read_power_desc(struct ufs_hba *hba,
+					 u8 *buf,
+					 u32 size)
+{
+	return ufshcd_read_desc(hba, QUERY_DESC_IDN_POWER, 0, buf, size);
+}
+
+/**
+ * ufshcd_read_unit_desc_param - read the specified unit descriptor parameter
+ * @hba: Pointer to adapter instance
+ * @lun: lun id
+ * @param_offset: offset of the parameter to read
+ * @param_read_buf: pointer to buffer where parameter would be read
+ * @param_size: sizeof(param_read_buf)
+ *
+ * Return 0 in case of success, non-zero otherwise
+ */
+static inline int ufshcd_read_unit_desc_param(struct ufs_hba *hba,
+					      int lun,
+					      enum unit_desc_param param_offset,
+					      u8 *param_read_buf,
+					      u32 param_size)
+{
+	/*
+	 * Unit descriptors are only available for general purpose LUs (LUN id
+	 * from 0 to 7) and RPMB Well known LU.
+	 */
+	if (lun != UFS_UPIU_RPMB_WLUN && (lun >= UFS_UPIU_MAX_GENERAL_LUN))
+		return -EOPNOTSUPP;
+
+	return ufshcd_read_desc_param(hba, QUERY_DESC_IDN_UNIT, lun,
+				      param_offset, param_read_buf, param_size);
+}
+
+/**
  * ufshcd_memory_alloc - allocate memory for host memory space data structures
  * @hba: per adapter instance
  *
@@ -1621,44 +2131,54 @@ out:
 EXPORT_SYMBOL_GPL(ufshcd_dme_get_attr);
 
 /**
- * ufshcd_uic_change_pwr_mode - Perform the UIC power mode chage
- *				using DME_SET primitives.
+ * ufshcd_uic_pwr_ctrl - executes UIC commands (which affects the link power
+ * state) and waits for it to take effect.
+ *
  * @hba: per adapter instance
- * @mode: powr mode value
+ * @cmd: UIC command to execute
+ *
+ * DME operations like DME_SET(PA_PWRMODE), DME_HIBERNATE_ENTER &
+ * DME_HIBERNATE_EXIT commands take some time to take its effect on both host
+ * and device UniPro link and hence it's final completion would be indicated by
+ * dedicated status bits in Interrupt Status register (UPMS, UHES, UHXS) in
+ * addition to normal UIC command completion Status (UCCS). This function only
+ * returns after the relevant status bits indicate the completion.
  *
  * Returns 0 on success, non-zero value on failure
  */
-static int ufshcd_uic_change_pwr_mode(struct ufs_hba *hba, u8 mode)
+static int ufshcd_uic_pwr_ctrl(struct ufs_hba *hba, struct uic_command *cmd)
 {
-	struct uic_command uic_cmd = {0};
-	struct completion pwr_done;
+	struct completion uic_async_done;
 	unsigned long flags;
 	u8 status;
 	int ret;
 
-	uic_cmd.command = UIC_CMD_DME_SET;
-	uic_cmd.argument1 = UIC_ARG_MIB(PA_PWRMODE);
-	uic_cmd.argument3 = mode;
-	init_completion(&pwr_done);
-
 	mutex_lock(&hba->uic_cmd_mutex);
+	init_completion(&uic_async_done);
 
 	spin_lock_irqsave(hba->host->host_lock, flags);
-	hba->pwr_done = &pwr_done;
+	hba->uic_async_done = &uic_async_done;
+	ret = __ufshcd_send_uic_cmd(hba, cmd);
 	spin_unlock_irqrestore(hba->host->host_lock, flags);
-	ret = __ufshcd_send_uic_cmd(hba, &uic_cmd);
 	if (ret) {
 		dev_err(hba->dev,
-			"pwr mode change with mode 0x%x uic error %d\n",
-			mode, ret);
+			"pwr ctrl cmd 0x%x with mode 0x%x uic error %d\n",
+			cmd->command, cmd->argument3, ret);
+		goto out;
+	}
+	ret = ufshcd_wait_for_uic_cmd(hba, cmd);
+	if (ret) {
+		dev_err(hba->dev,
+			"pwr ctrl cmd 0x%x with mode 0x%x uic error %d\n",
+			cmd->command, cmd->argument3, ret);
 		goto out;
 	}
 
-	if (!wait_for_completion_timeout(hba->pwr_done,
+	if (!wait_for_completion_timeout(hba->uic_async_done,
 					 msecs_to_jiffies(UIC_CMD_TIMEOUT))) {
 		dev_err(hba->dev,
-			"pwr mode change with mode 0x%x completion timeout\n",
-			mode);
+			"pwr ctrl cmd 0x%x with mode 0x%x completion timeout\n",
+			cmd->command, cmd->argument3);
 		ret = -ETIMEDOUT;
 		goto out;
 	}
@@ -1666,53 +2186,144 @@ static int ufshcd_uic_change_pwr_mode(struct ufs_hba *hba, u8 mode)
 	status = ufshcd_get_upmcrs(hba);
 	if (status != PWR_LOCAL) {
 		dev_err(hba->dev,
-			"pwr mode change failed, host umpcrs:0x%x\n",
-			status);
+			"pwr ctrl cmd 0x%0x failed, host umpcrs:0x%x\n",
+			cmd->command, status);
 		ret = (status != PWR_OK) ? status : -1;
 	}
 out:
 	spin_lock_irqsave(hba->host->host_lock, flags);
-	hba->pwr_done = NULL;
+	hba->uic_async_done = NULL;
 	spin_unlock_irqrestore(hba->host->host_lock, flags);
 	mutex_unlock(&hba->uic_cmd_mutex);
+
 	return ret;
 }
 
 /**
- * ufshcd_config_max_pwr_mode - Set & Change power mode with
- *	maximum capability attribute information.
+ * ufshcd_uic_change_pwr_mode - Perform the UIC power mode chage
+ *				using DME_SET primitives.
  * @hba: per adapter instance
+ * @mode: powr mode value
  *
  * Returns 0 on success, non-zero value on failure
  */
-static int ufshcd_config_max_pwr_mode(struct ufs_hba *hba)
+static int ufshcd_uic_change_pwr_mode(struct ufs_hba *hba, u8 mode)
 {
-	enum {RX = 0, TX = 1};
-	u32 lanes[] = {1, 1};
-	u32 gear[] = {1, 1};
-	u8 pwr[] = {FASTAUTO_MODE, FASTAUTO_MODE};
+	struct uic_command uic_cmd = {0};
 	int ret;
 
+	uic_cmd.command = UIC_CMD_DME_SET;
+	uic_cmd.argument1 = UIC_ARG_MIB(PA_PWRMODE);
+	uic_cmd.argument3 = mode;
+	ufshcd_hold(hba, false);
+	ret = ufshcd_uic_pwr_ctrl(hba, &uic_cmd);
+	ufshcd_release(hba);
+
+	return ret;
+}
+
+static int ufshcd_uic_hibern8_enter(struct ufs_hba *hba)
+{
+	struct uic_command uic_cmd = {0};
+
+	uic_cmd.command = UIC_CMD_DME_HIBER_ENTER;
+
+	return ufshcd_uic_pwr_ctrl(hba, &uic_cmd);
+}
+
+static int ufshcd_uic_hibern8_exit(struct ufs_hba *hba)
+{
+	struct uic_command uic_cmd = {0};
+	int ret;
+
+	uic_cmd.command = UIC_CMD_DME_HIBER_EXIT;
+	ret = ufshcd_uic_pwr_ctrl(hba, &uic_cmd);
+	if (ret) {
+		ufshcd_set_link_off(hba);
+		ret = ufshcd_host_reset_and_restore(hba);
+	}
+
+	return ret;
+}
+
+/**
+ * ufshcd_get_max_pwr_mode - reads the max power mode negotiated with device
+ * @hba: per-adapter instance
+ */
+static int ufshcd_get_max_pwr_mode(struct ufs_hba *hba)
+{
+	struct ufs_pa_layer_attr *pwr_info = &hba->max_pwr_info.info;
+
+	if (hba->max_pwr_info.is_valid)
+		return 0;
+
+	pwr_info->pwr_tx = FASTAUTO_MODE;
+	pwr_info->pwr_rx = FASTAUTO_MODE;
+	pwr_info->hs_rate = PA_HS_MODE_B;
+
 	/* Get the connected lane count */
-	ufshcd_dme_get(hba, UIC_ARG_MIB(PA_CONNECTEDRXDATALANES), &lanes[RX]);
-	ufshcd_dme_get(hba, UIC_ARG_MIB(PA_CONNECTEDTXDATALANES), &lanes[TX]);
+	ufshcd_dme_get(hba, UIC_ARG_MIB(PA_CONNECTEDRXDATALANES),
+			&pwr_info->lane_rx);
+	ufshcd_dme_get(hba, UIC_ARG_MIB(PA_CONNECTEDTXDATALANES),
+			&pwr_info->lane_tx);
+
+	if (!pwr_info->lane_rx || !pwr_info->lane_tx) {
+		dev_err(hba->dev, "%s: invalid connected lanes value. rx=%d, tx=%d\n",
+				__func__,
+				pwr_info->lane_rx,
+				pwr_info->lane_tx);
+		return -EINVAL;
+	}
 
 	/*
 	 * First, get the maximum gears of HS speed.
 	 * If a zero value, it means there is no HSGEAR capability.
 	 * Then, get the maximum gears of PWM speed.
 	 */
-	ufshcd_dme_get(hba, UIC_ARG_MIB(PA_MAXRXHSGEAR), &gear[RX]);
-	if (!gear[RX]) {
-		ufshcd_dme_get(hba, UIC_ARG_MIB(PA_MAXRXPWMGEAR), &gear[RX]);
-		pwr[RX] = SLOWAUTO_MODE;
+	ufshcd_dme_get(hba, UIC_ARG_MIB(PA_MAXRXHSGEAR), &pwr_info->gear_rx);
+	if (!pwr_info->gear_rx) {
+		ufshcd_dme_get(hba, UIC_ARG_MIB(PA_MAXRXPWMGEAR),
+				&pwr_info->gear_rx);
+		if (!pwr_info->gear_rx) {
+			dev_err(hba->dev, "%s: invalid max pwm rx gear read = %d\n",
+				__func__, pwr_info->gear_rx);
+			return -EINVAL;
+		}
+		pwr_info->pwr_rx = SLOWAUTO_MODE;
 	}
 
-	ufshcd_dme_peer_get(hba, UIC_ARG_MIB(PA_MAXRXHSGEAR), &gear[TX]);
-	if (!gear[TX]) {
+	ufshcd_dme_peer_get(hba, UIC_ARG_MIB(PA_MAXRXHSGEAR),
+			&pwr_info->gear_tx);
+	if (!pwr_info->gear_tx) {
 		ufshcd_dme_peer_get(hba, UIC_ARG_MIB(PA_MAXRXPWMGEAR),
-				    &gear[TX]);
-		pwr[TX] = SLOWAUTO_MODE;
+				&pwr_info->gear_tx);
+		if (!pwr_info->gear_tx) {
+			dev_err(hba->dev, "%s: invalid max pwm tx gear read = %d\n",
+				__func__, pwr_info->gear_tx);
+			return -EINVAL;
+		}
+		pwr_info->pwr_tx = SLOWAUTO_MODE;
+	}
+
+	hba->max_pwr_info.is_valid = true;
+	return 0;
+}
+
+static int ufshcd_change_power_mode(struct ufs_hba *hba,
+			     struct ufs_pa_layer_attr *pwr_mode)
+{
+	int ret;
+
+	/* if already configured to the requested pwr_mode */
+	if (pwr_mode->gear_rx == hba->pwr_info.gear_rx &&
+	    pwr_mode->gear_tx == hba->pwr_info.gear_tx &&
+	    pwr_mode->lane_rx == hba->pwr_info.lane_rx &&
+	    pwr_mode->lane_tx == hba->pwr_info.lane_tx &&
+	    pwr_mode->pwr_rx == hba->pwr_info.pwr_rx &&
+	    pwr_mode->pwr_tx == hba->pwr_info.pwr_tx &&
+	    pwr_mode->hs_rate == hba->pwr_info.hs_rate) {
+		dev_dbg(hba->dev, "%s: power already configured\n", __func__);
+		return 0;
 	}
 
 	/*
@@ -1721,23 +2332,67 @@ static int ufshcd_config_max_pwr_mode(struct ufs_hba *hba)
 	 * - PA_TXGEAR, PA_ACTIVETXDATALANES, PA_TXTERMINATION,
 	 * - PA_HSSERIES
 	 */
-	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_RXGEAR), gear[RX]);
-	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_ACTIVERXDATALANES), lanes[RX]);
-	if (pwr[RX] == FASTAUTO_MODE)
+	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_RXGEAR), pwr_mode->gear_rx);
+	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_ACTIVERXDATALANES),
+			pwr_mode->lane_rx);
+	if (pwr_mode->pwr_rx == FASTAUTO_MODE ||
+			pwr_mode->pwr_rx == FAST_MODE)
 		ufshcd_dme_set(hba, UIC_ARG_MIB(PA_RXTERMINATION), TRUE);
+	else
+		ufshcd_dme_set(hba, UIC_ARG_MIB(PA_RXTERMINATION), FALSE);
 
-	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_TXGEAR), gear[TX]);
-	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_ACTIVETXDATALANES), lanes[TX]);
-	if (pwr[TX] == FASTAUTO_MODE)
+	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_TXGEAR), pwr_mode->gear_tx);
+	ufshcd_dme_set(hba, UIC_ARG_MIB(PA_ACTIVETXDATALANES),
+			pwr_mode->lane_tx);
+	if (pwr_mode->pwr_tx == FASTAUTO_MODE ||
+			pwr_mode->pwr_tx == FAST_MODE)
 		ufshcd_dme_set(hba, UIC_ARG_MIB(PA_TXTERMINATION), TRUE);
+	else
+		ufshcd_dme_set(hba, UIC_ARG_MIB(PA_TXTERMINATION), FALSE);
 
-	if (pwr[RX] == FASTAUTO_MODE || pwr[TX] == FASTAUTO_MODE)
-		ufshcd_dme_set(hba, UIC_ARG_MIB(PA_HSSERIES), PA_HS_MODE_B);
+	if (pwr_mode->pwr_rx == FASTAUTO_MODE ||
+	    pwr_mode->pwr_tx == FASTAUTO_MODE ||
+	    pwr_mode->pwr_rx == FAST_MODE ||
+	    pwr_mode->pwr_tx == FAST_MODE)
+		ufshcd_dme_set(hba, UIC_ARG_MIB(PA_HSSERIES),
+						pwr_mode->hs_rate);
 
-	ret = ufshcd_uic_change_pwr_mode(hba, pwr[RX] << 4 | pwr[TX]);
-	if (ret)
+	ret = ufshcd_uic_change_pwr_mode(hba, pwr_mode->pwr_rx << 4
+			| pwr_mode->pwr_tx);
+
+	if (ret) {
 		dev_err(hba->dev,
-			"pwr_mode: power mode change failed %d\n", ret);
+			"%s: power mode change failed %d\n", __func__, ret);
+	} else {
+		if (hba->vops && hba->vops->pwr_change_notify)
+			hba->vops->pwr_change_notify(hba,
+				POST_CHANGE, NULL, pwr_mode);
+
+		memcpy(&hba->pwr_info, pwr_mode,
+			sizeof(struct ufs_pa_layer_attr));
+	}
+
+	return ret;
+}
+
+/**
+ * ufshcd_config_pwr_mode - configure a new power mode
+ * @hba: per-adapter instance
+ * @desired_pwr_mode: desired power configuration
+ */
+static int ufshcd_config_pwr_mode(struct ufs_hba *hba,
+		struct ufs_pa_layer_attr *desired_pwr_mode)
+{
+	struct ufs_pa_layer_attr final_params = { 0 };
+	int ret;
+
+	if (hba->vops && hba->vops->pwr_change_notify)
+		hba->vops->pwr_change_notify(hba,
+		     PRE_CHANGE, desired_pwr_mode, &final_params);
+	else
+		memcpy(&final_params, desired_pwr_mode, sizeof(final_params));
+
+	ret = ufshcd_change_power_mode(hba, &final_params);
 
 	return ret;
 }
@@ -1798,11 +2453,10 @@ out:
  * @hba: per adapter instance
  *
  * To bring UFS host controller to operational state,
- * 1. Check if device is present
- * 2. Enable required interrupts
- * 3. Configure interrupt aggregation
- * 4. Program UTRL and UTMRL base addres
- * 5. Configure run-stop-registers
+ * 1. Enable required interrupts
+ * 2. Configure interrupt aggregation
+ * 3. Program UTRL and UTMRL base addres
+ * 4. Configure run-stop-registers
  *
  * Returns 0 on success, non-zero value on failure
  */
@@ -1811,14 +2465,6 @@ static int ufshcd_make_hba_operational(struct ufs_hba *hba)
 	int err = 0;
 	u32 reg;
 
-	/* check if device present */
-	reg = ufshcd_readl(hba, REG_CONTROLLER_STATUS);
-	if (!ufshcd_is_device_present(reg)) {
-		dev_err(hba->dev, "cc: Device not present\n");
-		err = -ENXIO;
-		goto out;
-	}
-
 	/* Enable required interrupts */
 	ufshcd_enable_intr(hba, UFSHCD_ENABLE_INTRS);
 
@@ -1839,6 +2485,7 @@ static int ufshcd_make_hba_operational(struct ufs_hba *hba)
 	 * UCRDY, UTMRLDY and UTRLRDY bits must be 1
 	 * DEI, HEI bits must be 0
 	 */
+	reg = ufshcd_readl(hba, REG_CONTROLLER_STATUS);
 	if (!(ufshcd_get_lists_status(reg))) {
 		ufshcd_enable_run_stop_reg(hba);
 	} else {
@@ -1885,6 +2532,12 @@ static int ufshcd_hba_enable(struct ufs_hba *hba)
 		msleep(5);
 	}
 
+	/* UniPro link is disabled at this point */
+	ufshcd_set_link_off(hba);
+
+	if (hba->vops && hba->vops->hce_enable_notify)
+		hba->vops->hce_enable_notify(hba, PRE_CHANGE);
+
 	/* start controller initialization sequence */
 	ufshcd_hba_start(hba);
 
@@ -1912,6 +2565,13 @@ static int ufshcd_hba_enable(struct ufs_hba *hba)
 		}
 		msleep(5);
 	}
+
+	/* enable UIC related interrupts */
+	ufshcd_enable_intr(hba, UFSHCD_UIC_MASK);
+
+	if (hba->vops && hba->vops->hce_enable_notify)
+		hba->vops->hce_enable_notify(hba, POST_CHANGE);
+
 	return 0;
 }
 
@@ -1924,16 +2584,42 @@ static int ufshcd_hba_enable(struct ufs_hba *hba)
 static int ufshcd_link_startup(struct ufs_hba *hba)
 {
 	int ret;
+	int retries = DME_LINKSTARTUP_RETRIES;
 
-	/* enable UIC related interrupts */
-	ufshcd_enable_intr(hba, UIC_COMMAND_COMPL);
+	do {
+		if (hba->vops && hba->vops->link_startup_notify)
+			hba->vops->link_startup_notify(hba, PRE_CHANGE);
+
+		ret = ufshcd_dme_link_startup(hba);
+
+		/* check if device is detected by inter-connect layer */
+		if (!ret && !ufshcd_is_device_present(hba)) {
+			dev_err(hba->dev, "%s: Device not present\n", __func__);
+			ret = -ENXIO;
+			goto out;
+		}
+
+		/*
+		 * DME link lost indication is only received when link is up,
+		 * but we can't be sure if the link is up until link startup
+		 * succeeds. So reset the local Uni-Pro and try again.
+		 */
+		if (ret && ufshcd_hba_enable(hba))
+			goto out;
+	} while (ret && retries--);
 
-	ret = ufshcd_dme_link_startup(hba);
 	if (ret)
+		/* failed to get the link up... retire */
 		goto out;
 
-	ret = ufshcd_make_hba_operational(hba);
+	/* Include any host controller configuration via UIC commands */
+	if (hba->vops && hba->vops->link_startup_notify) {
+		ret = hba->vops->link_startup_notify(hba, POST_CHANGE);
+		if (ret)
+			goto out;
+	}
 
+	ret = ufshcd_make_hba_operational(hba);
 out:
 	if (ret)
 		dev_err(hba->dev, "link startup failed %d\n", ret);
@@ -1955,6 +2641,7 @@ static int ufshcd_verify_dev_init(struct ufs_hba *hba)
 	int err = 0;
 	int retries;
 
+	ufshcd_hold(hba, false);
 	mutex_lock(&hba->dev_cmd.lock);
 	for (retries = NOP_OUT_RETRIES; retries > 0; retries--) {
 		err = ufshcd_exec_dev_cmd(hba, DEV_CMD_TYPE_NOP,
@@ -1966,6 +2653,7 @@ static int ufshcd_verify_dev_init(struct ufs_hba *hba)
 		dev_dbg(hba->dev, "%s: error %d retrying\n", __func__, err);
 	}
 	mutex_unlock(&hba->dev_cmd.lock);
+	ufshcd_release(hba);
 
 	if (err)
 		dev_err(hba->dev, "%s: NOP OUT failed %d\n", __func__, err);
@@ -1973,6 +2661,100 @@ static int ufshcd_verify_dev_init(struct ufs_hba *hba)
 }
 
 /**
+ * ufshcd_set_queue_depth - set lun queue depth
+ * @sdev: pointer to SCSI device
+ *
+ * Read bLUQueueDepth value and activate scsi tagged command
+ * queueing. For WLUN, queue depth is set to 1. For best-effort
+ * cases (bLUQueueDepth = 0) the queue depth is set to a maximum
+ * value that host can queue.
+ */
+static void ufshcd_set_queue_depth(struct scsi_device *sdev)
+{
+	int ret = 0;
+	u8 lun_qdepth;
+	struct ufs_hba *hba;
+
+	hba = shost_priv(sdev->host);
+
+	lun_qdepth = hba->nutrs;
+	ret = ufshcd_read_unit_desc_param(hba,
+					  ufshcd_scsi_to_upiu_lun(sdev->lun),
+					  UNIT_DESC_PARAM_LU_Q_DEPTH,
+					  &lun_qdepth,
+					  sizeof(lun_qdepth));
+
+	/* Some WLUN doesn't support unit descriptor */
+	if (ret == -EOPNOTSUPP)
+		lun_qdepth = 1;
+	else if (!lun_qdepth)
+		/* eventually, we can figure out the real queue depth */
+		lun_qdepth = hba->nutrs;
+	else
+		lun_qdepth = min_t(int, lun_qdepth, hba->nutrs);
+
+	dev_dbg(hba->dev, "%s: activate tcq with queue depth %d\n",
+			__func__, lun_qdepth);
+	scsi_activate_tcq(sdev, lun_qdepth);
+}
+
+/*
+ * ufshcd_get_lu_wp - returns the "b_lu_write_protect" from UNIT DESCRIPTOR
+ * @hba: per-adapter instance
+ * @lun: UFS device lun id
+ * @b_lu_write_protect: pointer to buffer to hold the LU's write protect info
+ *
+ * Returns 0 in case of success and b_lu_write_protect status would be returned
+ * @b_lu_write_protect parameter.
+ * Returns -ENOTSUPP if reading b_lu_write_protect is not supported.
+ * Returns -EINVAL in case of invalid parameters passed to this function.
+ */
+static int ufshcd_get_lu_wp(struct ufs_hba *hba,
+			    u8 lun,
+			    u8 *b_lu_write_protect)
+{
+	int ret;
+
+	if (!b_lu_write_protect)
+		ret = -EINVAL;
+	/*
+	 * According to UFS device spec, RPMB LU can't be write
+	 * protected so skip reading bLUWriteProtect parameter for
+	 * it. For other W-LUs, UNIT DESCRIPTOR is not available.
+	 */
+	else if (lun >= UFS_UPIU_MAX_GENERAL_LUN)
+		ret = -ENOTSUPP;
+	else
+		ret = ufshcd_read_unit_desc_param(hba,
+					  lun,
+					  UNIT_DESC_PARAM_LU_WR_PROTECT,
+					  b_lu_write_protect,
+					  sizeof(*b_lu_write_protect));
+	return ret;
+}
+
+/**
+ * ufshcd_get_lu_power_on_wp_status - get LU's power on write protect
+ * status
+ * @hba: per-adapter instance
+ * @sdev: pointer to SCSI device
+ *
+ */
+static inline void ufshcd_get_lu_power_on_wp_status(struct ufs_hba *hba,
+						    struct scsi_device *sdev)
+{
+	if (hba->dev_info.f_power_on_wp_en &&
+	    !hba->dev_info.is_lu_power_on_wp) {
+		u8 b_lu_write_protect;
+
+		if (!ufshcd_get_lu_wp(hba, ufshcd_scsi_to_upiu_lun(sdev->lun),
+				      &b_lu_write_protect) &&
+		    (b_lu_write_protect == UFS_LU_POWER_ON_WP))
+			hba->dev_info.is_lu_power_on_wp = true;
+	}
+}
+
+/**
  * ufshcd_slave_alloc - handle initial SCSI device configurations
  * @sdev: pointer to SCSI device
  *
@@ -1981,7 +2763,6 @@ static int ufshcd_verify_dev_init(struct ufs_hba *hba)
 static int ufshcd_slave_alloc(struct scsi_device *sdev)
 {
 	struct ufs_hba *hba;
-	int lun_qdepth;
 
 	hba = shost_priv(sdev->host);
 	sdev->tagged_supported = 1;
@@ -1996,16 +2777,10 @@ static int ufshcd_slave_alloc(struct scsi_device *sdev)
 	/* REPORT SUPPORTED OPERATION CODES is not supported */
 	sdev->no_report_opcodes = 1;
 
-	lun_qdepth = ufshcd_read_sdev_qdepth(hba, sdev);
-	if (lun_qdepth <= 0)
-		/* eventually, we can figure out the real queue depth */
-		lun_qdepth = hba->nutrs;
-	else
-		lun_qdepth = min_t(int, lun_qdepth, hba->nutrs);
 
-	dev_dbg(hba->dev, "%s: activate tcq with queue depth %d\n",
-			__func__, lun_qdepth);
-	scsi_activate_tcq(sdev, lun_qdepth);
+	ufshcd_set_queue_depth(sdev);
+
+	ufshcd_get_lu_power_on_wp_status(hba, sdev);
 
 	return 0;
 }
@@ -2068,6 +2843,9 @@ static void ufshcd_slave_destroy(struct scsi_device *sdev)
 
 	hba = shost_priv(sdev->host);
 	scsi_deactivate_tcq(sdev, hba->nutrs);
+	/* Drop the reference as it won't be needed anymore */
+	if (ufshcd_scsi_to_upiu_lun(sdev->lun) == UFS_UPIU_UFS_DEVICE_WLUN)
+		hba->sdev_ufs_device = NULL;
 }
 
 /**
@@ -2234,8 +3012,8 @@ static void ufshcd_uic_cmd_compl(struct ufs_hba *hba, u32 intr_status)
 		complete(&hba->active_uic_cmd->done);
 	}
 
-	if ((intr_status & UIC_POWER_MODE) && hba->pwr_done)
-		complete(hba->pwr_done);
+	if ((intr_status & UFSHCD_UIC_PWR_MASK) && hba->uic_async_done)
+		complete(hba->uic_async_done);
 }
 
 /**
@@ -2275,6 +3053,7 @@ static void ufshcd_transfer_req_compl(struct ufs_hba *hba)
 			clear_bit_unlock(index, &hba->lrb_in_use);
 			/* Do not touch lrbp after scsi done */
 			cmd->scsi_done(cmd);
+			__ufshcd_release(hba);
 		} else if (lrbp->command_type == UTP_CMD_TYPE_DEV_MANAGE) {
 			if (hba->dev_cmd.complete)
 				complete(hba->dev_cmd.complete);
@@ -2284,6 +3063,8 @@ static void ufshcd_transfer_req_compl(struct ufs_hba *hba)
 	/* clear corresponding bits of completed commands */
 	hba->outstanding_reqs ^= completed_reqs;
 
+	ufshcd_clk_scaling_update_busy(hba);
+
 	/* we might have free'd some tags above */
 	wake_up(&hba->dev_cmd.tag_wq);
 }
@@ -2447,33 +3228,62 @@ static inline int ufshcd_get_bkops_status(struct ufs_hba *hba, u32 *status)
 }
 
 /**
- * ufshcd_urgent_bkops - handle urgent bkops exception event
+ * ufshcd_bkops_ctrl - control the auto bkops based on current bkops status
  * @hba: per-adapter instance
+ * @status: bkops_status value
  *
- * Enable fBackgroundOpsEn flag in the device to permit background
- * operations.
+ * Read the bkops_status from the UFS device and Enable fBackgroundOpsEn
+ * flag in the device to permit background operations if the device
+ * bkops_status is greater than or equal to "status" argument passed to
+ * this function, disable otherwise.
+ *
+ * Returns 0 for success, non-zero in case of failure.
+ *
+ * NOTE: Caller of this function can check the "hba->auto_bkops_enabled" flag
+ * to know whether auto bkops is enabled or disabled after this function
+ * returns control to it.
  */
-static int ufshcd_urgent_bkops(struct ufs_hba *hba)
+static int ufshcd_bkops_ctrl(struct ufs_hba *hba,
+			     enum bkops_status status)
 {
 	int err;
-	u32 status = 0;
+	u32 curr_status = 0;
 
-	err = ufshcd_get_bkops_status(hba, &status);
+	err = ufshcd_get_bkops_status(hba, &curr_status);
 	if (err) {
 		dev_err(hba->dev, "%s: failed to get BKOPS status %d\n",
 				__func__, err);
 		goto out;
+	} else if (curr_status > BKOPS_STATUS_MAX) {
+		dev_err(hba->dev, "%s: invalid BKOPS status %d\n",
+				__func__, curr_status);
+		err = -EINVAL;
+		goto out;
 	}
 
-	status = status & 0xF;
-
-	/* handle only if status indicates performance impact or critical */
-	if (status >= BKOPS_STATUS_PERF_IMPACT)
+	if (curr_status >= status)
 		err = ufshcd_enable_auto_bkops(hba);
+	else
+		err = ufshcd_disable_auto_bkops(hba);
 out:
 	return err;
 }
 
+/**
+ * ufshcd_urgent_bkops - handle urgent bkops exception event
+ * @hba: per-adapter instance
+ *
+ * Enable fBackgroundOpsEn flag in the device to permit background
+ * operations.
+ *
+ * If BKOPs is enabled, this function returns 0, 1 if the bkops in not enabled
+ * and negative error value for any other failure.
+ */
+static int ufshcd_urgent_bkops(struct ufs_hba *hba)
+{
+	return ufshcd_bkops_ctrl(hba, BKOPS_STATUS_PERF_IMPACT);
+}
+
 static inline int ufshcd_get_ee_status(struct ufs_hba *hba, u32 *status)
 {
 	return ufshcd_query_attr(hba, UPIU_QUERY_OPCODE_READ_ATTR,
@@ -2505,7 +3315,7 @@ static void ufshcd_exception_event_handler(struct work_struct *work)
 	status &= hba->ee_ctrl_mask;
 	if (status & MASK_EE_URGENT_BKOPS) {
 		err = ufshcd_urgent_bkops(hba);
-		if (err)
+		if (err < 0)
 			dev_err(hba->dev, "%s: failed to handle urgent bkops %d\n",
 					__func__, err);
 	}
@@ -2530,6 +3340,7 @@ static void ufshcd_err_handler(struct work_struct *work)
 	hba = container_of(work, struct ufs_hba, eh_work);
 
 	pm_runtime_get_sync(hba->dev);
+	ufshcd_hold(hba, false);
 
 	spin_lock_irqsave(hba->host->host_lock, flags);
 	if (hba->ufshcd_state == UFSHCD_STATE_RESET) {
@@ -2583,6 +3394,7 @@ static void ufshcd_err_handler(struct work_struct *work)
 
 out:
 	scsi_unblock_requests(hba->host);
+	ufshcd_release(hba);
 	pm_runtime_put_sync(hba->dev);
 }
 
@@ -2766,6 +3578,7 @@ static int ufshcd_issue_tm_cmd(struct ufs_hba *hba, int lun_id, int task_id,
 	 * the maximum wait time is bounded by %TM_CMD_TIMEOUT.
 	 */
 	wait_event(hba->tm_tag_wq, ufshcd_get_tm_free_slot(hba, &free_slot));
+	ufshcd_hold(hba, false);
 
 	spin_lock_irqsave(host->host_lock, flags);
 	task_req_descp = hba->utmrdl_base_addr;
@@ -2785,7 +3598,10 @@ static int ufshcd_issue_tm_cmd(struct ufs_hba *hba, int lun_id, int task_id,
 					      lun_id, task_tag);
 	task_req_upiup->header.dword_1 =
 		UPIU_HEADER_DWORD(0, tm_function, 0, 0);
-
+	/*
+	 * The host shall provide the same value for LUN field in the basic
+	 * header and for Input Parameter.
+	 */
 	task_req_upiup->input_param1 = cpu_to_be32(lun_id);
 	task_req_upiup->input_param2 = cpu_to_be32(task_id);
 
@@ -2814,6 +3630,7 @@ static int ufshcd_issue_tm_cmd(struct ufs_hba *hba, int lun_id, int task_id,
 	ufshcd_put_tm_slot(hba, free_slot);
 	wake_up(&hba->tm_tag_wq);
 
+	ufshcd_release(hba);
 	return err;
 }
 
@@ -2896,6 +3713,7 @@ static int ufshcd_abort(struct scsi_cmnd *cmd)
 	hba = shost_priv(host);
 	tag = cmd->request->tag;
 
+	ufshcd_hold(hba, false);
 	/* If command is already aborted/completed, return SUCCESS */
 	if (!(test_bit(tag, &hba->outstanding_reqs)))
 		goto out;
@@ -2960,6 +3778,7 @@ static int ufshcd_abort(struct scsi_cmnd *cmd)
 
 	clear_bit_unlock(tag, &hba->lrb_in_use);
 	wake_up(&hba->dev_cmd.tag_wq);
+
 out:
 	if (!err) {
 		err = SUCCESS;
@@ -2968,6 +3787,11 @@ out:
 		err = FAILED;
 	}
 
+	/*
+	 * This ufshcd_release() corresponds to the original scsi cmd that got
+	 * aborted here (as we won't get any IRQ for it).
+	 */
+	ufshcd_release(hba);
 	return err;
 }
 
@@ -2984,7 +3808,6 @@ out:
 static int ufshcd_host_reset_and_restore(struct ufs_hba *hba)
 {
 	int err;
-	async_cookie_t cookie;
 	unsigned long flags;
 
 	/* Reset the host controller */
@@ -2997,10 +3820,9 @@ static int ufshcd_host_reset_and_restore(struct ufs_hba *hba)
 		goto out;
 
 	/* Establish the link again and restore the device */
-	cookie = async_schedule(ufshcd_async_scan, hba);
-	/* wait for async scan to be completed */
-	async_synchronize_cookie(++cookie);
-	if (hba->ufshcd_state != UFSHCD_STATE_OPERATIONAL)
+	err = ufshcd_probe_hba(hba);
+
+	if (!err && (hba->ufshcd_state != UFSHCD_STATE_OPERATIONAL))
 		err = -EIO;
 out:
 	if (err)
@@ -3022,8 +3844,11 @@ static int ufshcd_reset_and_restore(struct ufs_hba *hba)
 {
 	int err = 0;
 	unsigned long flags;
+	int retries = MAX_HOST_RESET_RETRIES;
 
-	err = ufshcd_host_reset_and_restore(hba);
+	do {
+		err = ufshcd_host_reset_and_restore(hba);
+	} while (err && --retries);
 
 	/*
 	 * After reset the door-bell might be cleared, complete
@@ -3051,6 +3876,7 @@ static int ufshcd_eh_host_reset_handler(struct scsi_cmnd *cmd)
 
 	hba = shost_priv(cmd->device->host);
 
+	ufshcd_hold(hba, false);
 	/*
 	 * Check if there is any race with fatal error handling.
 	 * If so, wait for it to complete. Even though fatal error
@@ -3084,56 +3910,232 @@ static int ufshcd_eh_host_reset_handler(struct scsi_cmnd *cmd)
 	ufshcd_clear_eh_in_progress(hba);
 	spin_unlock_irqrestore(hba->host->host_lock, flags);
 
+	ufshcd_release(hba);
 	return err;
 }
 
 /**
- * ufshcd_read_sdev_qdepth - read the lun command queue depth
- * @hba: Pointer to adapter instance
- * @sdev: pointer to SCSI device
+ * ufshcd_get_max_icc_level - calculate the ICC level
+ * @sup_curr_uA: max. current supported by the regulator
+ * @start_scan: row at the desc table to start scan from
+ * @buff: power descriptor buffer
  *
- * Return in case of success the lun's queue depth else error.
+ * Returns calculated max ICC level for specific regulator
  */
-static int ufshcd_read_sdev_qdepth(struct ufs_hba *hba,
-				struct scsi_device *sdev)
+static u32 ufshcd_get_max_icc_level(int sup_curr_uA, u32 start_scan, char *buff)
+{
+	int i;
+	int curr_uA;
+	u16 data;
+	u16 unit;
+
+	for (i = start_scan; i >= 0; i--) {
+		data = be16_to_cpu(*((u16 *)(buff + 2*i)));
+		unit = (data & ATTR_ICC_LVL_UNIT_MASK) >>
+						ATTR_ICC_LVL_UNIT_OFFSET;
+		curr_uA = data & ATTR_ICC_LVL_VALUE_MASK;
+		switch (unit) {
+		case UFSHCD_NANO_AMP:
+			curr_uA = curr_uA / 1000;
+			break;
+		case UFSHCD_MILI_AMP:
+			curr_uA = curr_uA * 1000;
+			break;
+		case UFSHCD_AMP:
+			curr_uA = curr_uA * 1000 * 1000;
+			break;
+		case UFSHCD_MICRO_AMP:
+		default:
+			break;
+		}
+		if (sup_curr_uA >= curr_uA)
+			break;
+	}
+	if (i < 0) {
+		i = 0;
+		pr_err("%s: Couldn't find valid icc_level = %d", __func__, i);
+	}
+
+	return (u32)i;
+}
+
+/**
+ * ufshcd_calc_icc_level - calculate the max ICC level
+ * In case regulators are not initialized we'll return 0
+ * @hba: per-adapter instance
+ * @desc_buf: power descriptor buffer to extract ICC levels from.
+ * @len: length of desc_buff
+ *
+ * Returns calculated ICC level
+ */
+static u32 ufshcd_find_max_sup_active_icc_level(struct ufs_hba *hba,
+							u8 *desc_buf, int len)
+{
+	u32 icc_level = 0;
+
+	if (!hba->vreg_info.vcc || !hba->vreg_info.vccq ||
+						!hba->vreg_info.vccq2) {
+		dev_err(hba->dev,
+			"%s: Regulator capability was not set, actvIccLevel=%d",
+							__func__, icc_level);
+		goto out;
+	}
+
+	if (hba->vreg_info.vcc)
+		icc_level = ufshcd_get_max_icc_level(
+				hba->vreg_info.vcc->max_uA,
+				POWER_DESC_MAX_ACTV_ICC_LVLS - 1,
+				&desc_buf[PWR_DESC_ACTIVE_LVLS_VCC_0]);
+
+	if (hba->vreg_info.vccq)
+		icc_level = ufshcd_get_max_icc_level(
+				hba->vreg_info.vccq->max_uA,
+				icc_level,
+				&desc_buf[PWR_DESC_ACTIVE_LVLS_VCCQ_0]);
+
+	if (hba->vreg_info.vccq2)
+		icc_level = ufshcd_get_max_icc_level(
+				hba->vreg_info.vccq2->max_uA,
+				icc_level,
+				&desc_buf[PWR_DESC_ACTIVE_LVLS_VCCQ2_0]);
+out:
+	return icc_level;
+}
+
+static void ufshcd_init_icc_levels(struct ufs_hba *hba)
 {
 	int ret;
-	int buff_len = UNIT_DESC_MAX_SIZE;
-	u8 desc_buf[UNIT_DESC_MAX_SIZE];
+	int buff_len = QUERY_DESC_POWER_MAX_SIZE;
+	u8 desc_buf[QUERY_DESC_POWER_MAX_SIZE];
 
-	ret = ufshcd_query_descriptor(hba, UPIU_QUERY_OPCODE_READ_DESC,
-			QUERY_DESC_IDN_UNIT, sdev->lun, 0, desc_buf, &buff_len);
+	ret = ufshcd_read_power_desc(hba, desc_buf, buff_len);
+	if (ret) {
+		dev_err(hba->dev,
+			"%s: Failed reading power descriptor.len = %d ret = %d",
+			__func__, buff_len, ret);
+		return;
+	}
+
+	hba->init_prefetch_data.icc_level =
+			ufshcd_find_max_sup_active_icc_level(hba,
+			desc_buf, buff_len);
+	dev_dbg(hba->dev, "%s: setting icc_level 0x%x",
+			__func__, hba->init_prefetch_data.icc_level);
 
-	if (ret || (buff_len < UNIT_DESC_PARAM_LU_Q_DEPTH)) {
+	ret = ufshcd_query_attr(hba, UPIU_QUERY_OPCODE_WRITE_ATTR,
+			QUERY_ATTR_IDN_ACTIVE_ICC_LVL, 0, 0,
+			&hba->init_prefetch_data.icc_level);
+
+	if (ret)
 		dev_err(hba->dev,
-			"%s:Failed reading unit descriptor. len = %d ret = %d"
-			, __func__, buff_len, ret);
-		if (!ret)
-			ret = -EINVAL;
+			"%s: Failed configuring bActiveICCLevel = %d ret = %d",
+			__func__, hba->init_prefetch_data.icc_level , ret);
 
+}
+
+/**
+ * ufshcd_scsi_add_wlus - Adds required W-LUs
+ * @hba: per-adapter instance
+ *
+ * UFS device specification requires the UFS devices to support 4 well known
+ * logical units:
+ *	"REPORT_LUNS" (address: 01h)
+ *	"UFS Device" (address: 50h)
+ *	"RPMB" (address: 44h)
+ *	"BOOT" (address: 30h)
+ * UFS device's power management needs to be controlled by "POWER CONDITION"
+ * field of SSU (START STOP UNIT) command. But this "power condition" field
+ * will take effect only when its sent to "UFS device" well known logical unit
+ * hence we require the scsi_device instance to represent this logical unit in
+ * order for the UFS host driver to send the SSU command for power management.
+
+ * We also require the scsi_device instance for "RPMB" (Replay Protected Memory
+ * Block) LU so user space process can control this LU. User space may also
+ * want to have access to BOOT LU.
+
+ * This function adds scsi device instances for each of all well known LUs
+ * (except "REPORT LUNS" LU).
+ *
+ * Returns zero on success (all required W-LUs are added successfully),
+ * non-zero error value on failure (if failed to add any of the required W-LU).
+ */
+static int ufshcd_scsi_add_wlus(struct ufs_hba *hba)
+{
+	int ret = 0;
+
+	hba->sdev_ufs_device = __scsi_add_device(hba->host, 0, 0,
+		ufshcd_upiu_wlun_to_scsi_wlun(UFS_UPIU_UFS_DEVICE_WLUN), NULL);
+	if (IS_ERR(hba->sdev_ufs_device)) {
+		ret = PTR_ERR(hba->sdev_ufs_device);
+		hba->sdev_ufs_device = NULL;
 		goto out;
 	}
 
-	ret = desc_buf[UNIT_DESC_PARAM_LU_Q_DEPTH] & 0xFF;
+	hba->sdev_boot = __scsi_add_device(hba->host, 0, 0,
+		ufshcd_upiu_wlun_to_scsi_wlun(UFS_UPIU_BOOT_WLUN), NULL);
+	if (IS_ERR(hba->sdev_boot)) {
+		ret = PTR_ERR(hba->sdev_boot);
+		hba->sdev_boot = NULL;
+		goto remove_sdev_ufs_device;
+	}
+
+	hba->sdev_rpmb = __scsi_add_device(hba->host, 0, 0,
+		ufshcd_upiu_wlun_to_scsi_wlun(UFS_UPIU_RPMB_WLUN), NULL);
+	if (IS_ERR(hba->sdev_rpmb)) {
+		ret = PTR_ERR(hba->sdev_rpmb);
+		hba->sdev_rpmb = NULL;
+		goto remove_sdev_boot;
+	}
+	goto out;
+
+remove_sdev_boot:
+	scsi_remove_device(hba->sdev_boot);
+remove_sdev_ufs_device:
+	scsi_remove_device(hba->sdev_ufs_device);
 out:
 	return ret;
 }
 
 /**
- * ufshcd_async_scan - asynchronous execution for link startup
- * @data: data pointer to pass to this function
- * @cookie: cookie data
+ * ufshcd_scsi_remove_wlus - Removes the W-LUs which were added by
+ *			     ufshcd_scsi_add_wlus()
+ * @hba: per-adapter instance
+ *
  */
-static void ufshcd_async_scan(void *data, async_cookie_t cookie)
+static void ufshcd_scsi_remove_wlus(struct ufs_hba *hba)
+{
+	if (hba->sdev_ufs_device) {
+		scsi_remove_device(hba->sdev_ufs_device);
+		hba->sdev_ufs_device = NULL;
+	}
+
+	if (hba->sdev_boot) {
+		scsi_remove_device(hba->sdev_boot);
+		hba->sdev_boot = NULL;
+	}
+
+	if (hba->sdev_rpmb) {
+		scsi_remove_device(hba->sdev_rpmb);
+		hba->sdev_rpmb = NULL;
+	}
+}
+
+/**
+ * ufshcd_probe_hba - probe hba to detect device and initialize
+ * @hba: per-adapter instance
+ *
+ * Execute link-startup and verify device initialization
+ */
+static int ufshcd_probe_hba(struct ufs_hba *hba)
 {
-	struct ufs_hba *hba = (struct ufs_hba *)data;
 	int ret;
 
 	ret = ufshcd_link_startup(hba);
 	if (ret)
 		goto out;
 
-	ufshcd_config_max_pwr_mode(hba);
+	/* UniPro link is active now */
+	ufshcd_set_link_active(hba);
 
 	ret = ufshcd_verify_dev_init(hba);
 	if (ret)
@@ -3143,16 +4145,77 @@ static void ufshcd_async_scan(void *data, async_cookie_t cookie)
 	if (ret)
 		goto out;
 
+	/* UFS device is also active now */
+	ufshcd_set_ufs_dev_active(hba);
 	ufshcd_force_reset_auto_bkops(hba);
 	hba->ufshcd_state = UFSHCD_STATE_OPERATIONAL;
+	hba->wlun_dev_clr_ua = true;
+
+	if (ufshcd_get_max_pwr_mode(hba)) {
+		dev_err(hba->dev,
+			"%s: Failed getting max supported power mode\n",
+			__func__);
+	} else {
+		ret = ufshcd_config_pwr_mode(hba, &hba->max_pwr_info.info);
+		if (ret)
+			dev_err(hba->dev, "%s: Failed setting power mode, err = %d\n",
+					__func__, ret);
+	}
+
+	/*
+	 * If we are in error handling context or in power management callbacks
+	 * context, no need to scan the host
+	 */
+	if (!ufshcd_eh_in_progress(hba) && !hba->pm_op_in_progress) {
+		bool flag;
+
+		/* clear any previous UFS device information */
+		memset(&hba->dev_info, 0, sizeof(hba->dev_info));
+		if (!ufshcd_query_flag(hba, UPIU_QUERY_OPCODE_READ_FLAG,
+				       QUERY_FLAG_IDN_PWR_ON_WPE, &flag))
+			hba->dev_info.f_power_on_wp_en = flag;
+
+		if (!hba->is_init_prefetch)
+			ufshcd_init_icc_levels(hba);
+
+		/* Add required well known logical units to scsi mid layer */
+		if (ufshcd_scsi_add_wlus(hba))
+			goto out;
 
-	/* If we are in error handling context no need to scan the host */
-	if (!ufshcd_eh_in_progress(hba)) {
 		scsi_scan_host(hba->host);
 		pm_runtime_put_sync(hba->dev);
 	}
+
+	if (!hba->is_init_prefetch)
+		hba->is_init_prefetch = true;
+
+	/* Resume devfreq after UFS device is detected */
+	if (ufshcd_is_clkscaling_enabled(hba))
+		devfreq_resume_device(hba->devfreq);
+
 out:
-	return;
+	/*
+	 * If we failed to initialize the device or the device is not
+	 * present, turn off the power/clocks etc.
+	 */
+	if (ret && !ufshcd_eh_in_progress(hba) && !hba->pm_op_in_progress) {
+		pm_runtime_put_sync(hba->dev);
+		ufshcd_hba_exit(hba);
+	}
+
+	return ret;
+}
+
+/**
+ * ufshcd_async_scan - asynchronous execution for probing hba
+ * @data: data pointer to pass to this function
+ * @cookie: cookie data
+ */
+static void ufshcd_async_scan(void *data, async_cookie_t cookie)
+{
+	struct ufs_hba *hba = (struct ufs_hba *)data;
+
+	ufshcd_probe_hba(hba);
 }
 
 static struct scsi_host_template ufshcd_driver_template = {
@@ -3171,70 +4234,956 @@ static struct scsi_host_template ufshcd_driver_template = {
 	.sg_tablesize		= SG_ALL,
 	.cmd_per_lun		= UFSHCD_CMD_PER_LUN,
 	.can_queue		= UFSHCD_CAN_QUEUE,
+	.max_host_blocked	= 1,
 };
 
+static int ufshcd_config_vreg_load(struct device *dev, struct ufs_vreg *vreg,
+				   int ua)
+{
+	int ret = 0;
+	struct regulator *reg = vreg->reg;
+	const char *name = vreg->name;
+
+	BUG_ON(!vreg);
+
+	ret = regulator_set_optimum_mode(reg, ua);
+	if (ret >= 0) {
+		/*
+		 * regulator_set_optimum_mode() returns new regulator
+		 * mode upon success.
+		 */
+		ret = 0;
+	} else {
+		dev_err(dev, "%s: %s set optimum mode(ua=%d) failed, err=%d\n",
+				__func__, name, ua, ret);
+	}
+
+	return ret;
+}
+
+static inline int ufshcd_config_vreg_lpm(struct ufs_hba *hba,
+					 struct ufs_vreg *vreg)
+{
+	return ufshcd_config_vreg_load(hba->dev, vreg, UFS_VREG_LPM_LOAD_UA);
+}
+
+static inline int ufshcd_config_vreg_hpm(struct ufs_hba *hba,
+					 struct ufs_vreg *vreg)
+{
+	return ufshcd_config_vreg_load(hba->dev, vreg, vreg->max_uA);
+}
+
+static int ufshcd_config_vreg(struct device *dev,
+		struct ufs_vreg *vreg, bool on)
+{
+	int ret = 0;
+	struct regulator *reg = vreg->reg;
+	const char *name = vreg->name;
+	int min_uV, uA_load;
+
+	BUG_ON(!vreg);
+
+	if (regulator_count_voltages(reg) > 0) {
+		min_uV = on ? vreg->min_uV : 0;
+		ret = regulator_set_voltage(reg, min_uV, vreg->max_uV);
+		if (ret) {
+			dev_err(dev, "%s: %s set voltage failed, err=%d\n",
+					__func__, name, ret);
+			goto out;
+		}
+
+		uA_load = on ? vreg->max_uA : 0;
+		ret = ufshcd_config_vreg_load(dev, vreg, uA_load);
+		if (ret)
+			goto out;
+	}
+out:
+	return ret;
+}
+
+static int ufshcd_enable_vreg(struct device *dev, struct ufs_vreg *vreg)
+{
+	int ret = 0;
+
+	if (!vreg || vreg->enabled)
+		goto out;
+
+	ret = ufshcd_config_vreg(dev, vreg, true);
+	if (!ret)
+		ret = regulator_enable(vreg->reg);
+
+	if (!ret)
+		vreg->enabled = true;
+	else
+		dev_err(dev, "%s: %s enable failed, err=%d\n",
+				__func__, vreg->name, ret);
+out:
+	return ret;
+}
+
+static int ufshcd_disable_vreg(struct device *dev, struct ufs_vreg *vreg)
+{
+	int ret = 0;
+
+	if (!vreg || !vreg->enabled)
+		goto out;
+
+	ret = regulator_disable(vreg->reg);
+
+	if (!ret) {
+		/* ignore errors on applying disable config */
+		ufshcd_config_vreg(dev, vreg, false);
+		vreg->enabled = false;
+	} else {
+		dev_err(dev, "%s: %s disable failed, err=%d\n",
+				__func__, vreg->name, ret);
+	}
+out:
+	return ret;
+}
+
+static int ufshcd_setup_vreg(struct ufs_hba *hba, bool on)
+{
+	int ret = 0;
+	struct device *dev = hba->dev;
+	struct ufs_vreg_info *info = &hba->vreg_info;
+
+	if (!info)
+		goto out;
+
+	ret = ufshcd_toggle_vreg(dev, info->vcc, on);
+	if (ret)
+		goto out;
+
+	ret = ufshcd_toggle_vreg(dev, info->vccq, on);
+	if (ret)
+		goto out;
+
+	ret = ufshcd_toggle_vreg(dev, info->vccq2, on);
+	if (ret)
+		goto out;
+
+out:
+	if (ret) {
+		ufshcd_toggle_vreg(dev, info->vccq2, false);
+		ufshcd_toggle_vreg(dev, info->vccq, false);
+		ufshcd_toggle_vreg(dev, info->vcc, false);
+	}
+	return ret;
+}
+
+static int ufshcd_setup_hba_vreg(struct ufs_hba *hba, bool on)
+{
+	struct ufs_vreg_info *info = &hba->vreg_info;
+
+	if (info)
+		return ufshcd_toggle_vreg(hba->dev, info->vdd_hba, on);
+
+	return 0;
+}
+
+static int ufshcd_get_vreg(struct device *dev, struct ufs_vreg *vreg)
+{
+	int ret = 0;
+
+	if (!vreg)
+		goto out;
+
+	vreg->reg = devm_regulator_get(dev, vreg->name);
+	if (IS_ERR(vreg->reg)) {
+		ret = PTR_ERR(vreg->reg);
+		dev_err(dev, "%s: %s get failed, err=%d\n",
+				__func__, vreg->name, ret);
+	}
+out:
+	return ret;
+}
+
+static int ufshcd_init_vreg(struct ufs_hba *hba)
+{
+	int ret = 0;
+	struct device *dev = hba->dev;
+	struct ufs_vreg_info *info = &hba->vreg_info;
+
+	if (!info)
+		goto out;
+
+	ret = ufshcd_get_vreg(dev, info->vcc);
+	if (ret)
+		goto out;
+
+	ret = ufshcd_get_vreg(dev, info->vccq);
+	if (ret)
+		goto out;
+
+	ret = ufshcd_get_vreg(dev, info->vccq2);
+out:
+	return ret;
+}
+
+static int ufshcd_init_hba_vreg(struct ufs_hba *hba)
+{
+	struct ufs_vreg_info *info = &hba->vreg_info;
+
+	if (info)
+		return ufshcd_get_vreg(hba->dev, info->vdd_hba);
+
+	return 0;
+}
+
+static int __ufshcd_setup_clocks(struct ufs_hba *hba, bool on,
+					bool skip_ref_clk)
+{
+	int ret = 0;
+	struct ufs_clk_info *clki;
+	struct list_head *head = &hba->clk_list_head;
+	unsigned long flags;
+
+	if (!head || list_empty(head))
+		goto out;
+
+	list_for_each_entry(clki, head, list) {
+		if (!IS_ERR_OR_NULL(clki->clk)) {
+			if (skip_ref_clk && !strcmp(clki->name, "ref_clk"))
+				continue;
+
+			if (on && !clki->enabled) {
+				ret = clk_prepare_enable(clki->clk);
+				if (ret) {
+					dev_err(hba->dev, "%s: %s prepare enable failed, %d\n",
+						__func__, clki->name, ret);
+					goto out;
+				}
+			} else if (!on && clki->enabled) {
+				clk_disable_unprepare(clki->clk);
+			}
+			clki->enabled = on;
+			dev_dbg(hba->dev, "%s: clk: %s %sabled\n", __func__,
+					clki->name, on ? "en" : "dis");
+		}
+	}
+
+	if (hba->vops && hba->vops->setup_clocks)
+		ret = hba->vops->setup_clocks(hba, on);
+out:
+	if (ret) {
+		list_for_each_entry(clki, head, list) {
+			if (!IS_ERR_OR_NULL(clki->clk) && clki->enabled)
+				clk_disable_unprepare(clki->clk);
+		}
+	} else if (!ret && on) {
+		spin_lock_irqsave(hba->host->host_lock, flags);
+		hba->clk_gating.state = CLKS_ON;
+		spin_unlock_irqrestore(hba->host->host_lock, flags);
+	}
+	return ret;
+}
+
+static int ufshcd_setup_clocks(struct ufs_hba *hba, bool on)
+{
+	return  __ufshcd_setup_clocks(hba, on, false);
+}
+
+static int ufshcd_init_clocks(struct ufs_hba *hba)
+{
+	int ret = 0;
+	struct ufs_clk_info *clki;
+	struct device *dev = hba->dev;
+	struct list_head *head = &hba->clk_list_head;
+
+	if (!head || list_empty(head))
+		goto out;
+
+	list_for_each_entry(clki, head, list) {
+		if (!clki->name)
+			continue;
+
+		clki->clk = devm_clk_get(dev, clki->name);
+		if (IS_ERR(clki->clk)) {
+			ret = PTR_ERR(clki->clk);
+			dev_err(dev, "%s: %s clk get failed, %d\n",
+					__func__, clki->name, ret);
+			goto out;
+		}
+
+		if (clki->max_freq) {
+			ret = clk_set_rate(clki->clk, clki->max_freq);
+			if (ret) {
+				dev_err(hba->dev, "%s: %s clk set rate(%dHz) failed, %d\n",
+					__func__, clki->name,
+					clki->max_freq, ret);
+				goto out;
+			}
+			clki->curr_freq = clki->max_freq;
+		}
+		dev_dbg(dev, "%s: clk: %s, rate: %lu\n", __func__,
+				clki->name, clk_get_rate(clki->clk));
+	}
+out:
+	return ret;
+}
+
+static int ufshcd_variant_hba_init(struct ufs_hba *hba)
+{
+	int err = 0;
+
+	if (!hba->vops)
+		goto out;
+
+	if (hba->vops->init) {
+		err = hba->vops->init(hba);
+		if (err)
+			goto out;
+	}
+
+	if (hba->vops->setup_regulators) {
+		err = hba->vops->setup_regulators(hba, true);
+		if (err)
+			goto out_exit;
+	}
+
+	goto out;
+
+out_exit:
+	if (hba->vops->exit)
+		hba->vops->exit(hba);
+out:
+	if (err)
+		dev_err(hba->dev, "%s: variant %s init failed err %d\n",
+			__func__, hba->vops ? hba->vops->name : "", err);
+	return err;
+}
+
+static void ufshcd_variant_hba_exit(struct ufs_hba *hba)
+{
+	if (!hba->vops)
+		return;
+
+	if (hba->vops->setup_clocks)
+		hba->vops->setup_clocks(hba, false);
+
+	if (hba->vops->setup_regulators)
+		hba->vops->setup_regulators(hba, false);
+
+	if (hba->vops->exit)
+		hba->vops->exit(hba);
+}
+
+static int ufshcd_hba_init(struct ufs_hba *hba)
+{
+	int err;
+
+	/*
+	 * Handle host controller power separately from the UFS device power
+	 * rails as it will help controlling the UFS host controller power
+	 * collapse easily which is different than UFS device power collapse.
+	 * Also, enable the host controller power before we go ahead with rest
+	 * of the initialization here.
+	 */
+	err = ufshcd_init_hba_vreg(hba);
+	if (err)
+		goto out;
+
+	err = ufshcd_setup_hba_vreg(hba, true);
+	if (err)
+		goto out;
+
+	err = ufshcd_init_clocks(hba);
+	if (err)
+		goto out_disable_hba_vreg;
+
+	err = ufshcd_setup_clocks(hba, true);
+	if (err)
+		goto out_disable_hba_vreg;
+
+	err = ufshcd_init_vreg(hba);
+	if (err)
+		goto out_disable_clks;
+
+	err = ufshcd_setup_vreg(hba, true);
+	if (err)
+		goto out_disable_clks;
+
+	err = ufshcd_variant_hba_init(hba);
+	if (err)
+		goto out_disable_vreg;
+
+	hba->is_powered = true;
+	goto out;
+
+out_disable_vreg:
+	ufshcd_setup_vreg(hba, false);
+out_disable_clks:
+	ufshcd_setup_clocks(hba, false);
+out_disable_hba_vreg:
+	ufshcd_setup_hba_vreg(hba, false);
+out:
+	return err;
+}
+
+static void ufshcd_hba_exit(struct ufs_hba *hba)
+{
+	if (hba->is_powered) {
+		ufshcd_variant_hba_exit(hba);
+		ufshcd_setup_vreg(hba, false);
+		ufshcd_setup_clocks(hba, false);
+		ufshcd_setup_hba_vreg(hba, false);
+		hba->is_powered = false;
+	}
+}
+
+static int
+ufshcd_send_request_sense(struct ufs_hba *hba, struct scsi_device *sdp)
+{
+	unsigned char cmd[6] = {REQUEST_SENSE,
+				0,
+				0,
+				0,
+				SCSI_SENSE_BUFFERSIZE,
+				0};
+	char *buffer;
+	int ret;
+
+	buffer = kzalloc(SCSI_SENSE_BUFFERSIZE, GFP_KERNEL);
+	if (!buffer) {
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	ret = scsi_execute_req_flags(sdp, cmd, DMA_FROM_DEVICE, buffer,
+				SCSI_SENSE_BUFFERSIZE, NULL,
+				msecs_to_jiffies(1000), 3, NULL, REQ_PM);
+	if (ret)
+		pr_err("%s: failed with err %d\n", __func__, ret);
+
+	kfree(buffer);
+out:
+	return ret;
+}
+
+/**
+ * ufshcd_set_dev_pwr_mode - sends START STOP UNIT command to set device
+ *			     power mode
+ * @hba: per adapter instance
+ * @pwr_mode: device power mode to set
+ *
+ * Returns 0 if requested power mode is set successfully
+ * Returns non-zero if failed to set the requested power mode
+ */
+static int ufshcd_set_dev_pwr_mode(struct ufs_hba *hba,
+				     enum ufs_dev_pwr_mode pwr_mode)
+{
+	unsigned char cmd[6] = { START_STOP };
+	struct scsi_sense_hdr sshdr;
+	struct scsi_device *sdp = hba->sdev_ufs_device;
+	int ret;
+
+	if (!sdp || !scsi_device_online(sdp))
+		return -ENODEV;
+
+	/*
+	 * If scsi commands fail, the scsi mid-layer schedules scsi error-
+	 * handling, which would wait for host to be resumed. Since we know
+	 * we are functional while we are here, skip host resume in error
+	 * handling context.
+	 */
+	hba->host->eh_noresume = 1;
+	if (hba->wlun_dev_clr_ua) {
+		ret = ufshcd_send_request_sense(hba, sdp);
+		if (ret)
+			goto out;
+		/* Unit attention condition is cleared now */
+		hba->wlun_dev_clr_ua = false;
+	}
+
+	cmd[4] = pwr_mode << 4;
+
+	/*
+	 * Current function would be generally called from the power management
+	 * callbacks hence set the REQ_PM flag so that it doesn't resume the
+	 * already suspended childs.
+	 */
+	ret = scsi_execute_req_flags(sdp, cmd, DMA_NONE, NULL, 0, &sshdr,
+				     START_STOP_TIMEOUT, 0, NULL, REQ_PM);
+	if (ret) {
+		sdev_printk(KERN_WARNING, sdp,
+			  "START_STOP failed for power mode: %d\n", pwr_mode);
+		scsi_show_result(ret);
+		if (driver_byte(ret) & DRIVER_SENSE) {
+			scsi_show_sense_hdr(&sshdr);
+			scsi_show_extd_sense(sshdr.asc, sshdr.ascq);
+		}
+	}
+
+	if (!ret)
+		hba->curr_dev_pwr_mode = pwr_mode;
+out:
+	hba->host->eh_noresume = 0;
+	return ret;
+}
+
+static int ufshcd_link_state_transition(struct ufs_hba *hba,
+					enum uic_link_state req_link_state,
+					int check_for_bkops)
+{
+	int ret = 0;
+
+	if (req_link_state == hba->uic_link_state)
+		return 0;
+
+	if (req_link_state == UIC_LINK_HIBERN8_STATE) {
+		ret = ufshcd_uic_hibern8_enter(hba);
+		if (!ret)
+			ufshcd_set_link_hibern8(hba);
+		else
+			goto out;
+	}
+	/*
+	 * If autobkops is enabled, link can't be turned off because
+	 * turning off the link would also turn off the device.
+	 */
+	else if ((req_link_state == UIC_LINK_OFF_STATE) &&
+		   (!check_for_bkops || (check_for_bkops &&
+		    !hba->auto_bkops_enabled))) {
+		/*
+		 * Change controller state to "reset state" which
+		 * should also put the link in off/reset state
+		 */
+		ufshcd_hba_stop(hba);
+		/*
+		 * TODO: Check if we need any delay to make sure that
+		 * controller is reset
+		 */
+		ufshcd_set_link_off(hba);
+	}
+
+out:
+	return ret;
+}
+
+static void ufshcd_vreg_set_lpm(struct ufs_hba *hba)
+{
+	/*
+	 * If UFS device is either in UFS_Sleep turn off VCC rail to save some
+	 * power.
+	 *
+	 * If UFS device and link is in OFF state, all power supplies (VCC,
+	 * VCCQ, VCCQ2) can be turned off if power on write protect is not
+	 * required. If UFS link is inactive (Hibern8 or OFF state) and device
+	 * is in sleep state, put VCCQ & VCCQ2 rails in LPM mode.
+	 *
+	 * Ignore the error returned by ufshcd_toggle_vreg() as device is anyway
+	 * in low power state which would save some power.
+	 */
+	if (ufshcd_is_ufs_dev_poweroff(hba) && ufshcd_is_link_off(hba) &&
+	    !hba->dev_info.is_lu_power_on_wp) {
+		ufshcd_setup_vreg(hba, false);
+	} else if (!ufshcd_is_ufs_dev_active(hba)) {
+		ufshcd_toggle_vreg(hba->dev, hba->vreg_info.vcc, false);
+		if (!ufshcd_is_link_active(hba)) {
+			ufshcd_config_vreg_lpm(hba, hba->vreg_info.vccq);
+			ufshcd_config_vreg_lpm(hba, hba->vreg_info.vccq2);
+		}
+	}
+}
+
+static int ufshcd_vreg_set_hpm(struct ufs_hba *hba)
+{
+	int ret = 0;
+
+	if (ufshcd_is_ufs_dev_poweroff(hba) && ufshcd_is_link_off(hba) &&
+	    !hba->dev_info.is_lu_power_on_wp) {
+		ret = ufshcd_setup_vreg(hba, true);
+	} else if (!ufshcd_is_ufs_dev_active(hba)) {
+		ret = ufshcd_toggle_vreg(hba->dev, hba->vreg_info.vcc, true);
+		if (!ret && !ufshcd_is_link_active(hba)) {
+			ret = ufshcd_config_vreg_hpm(hba, hba->vreg_info.vccq);
+			if (ret)
+				goto vcc_disable;
+			ret = ufshcd_config_vreg_hpm(hba, hba->vreg_info.vccq2);
+			if (ret)
+				goto vccq_lpm;
+		}
+	}
+	goto out;
+
+vccq_lpm:
+	ufshcd_config_vreg_lpm(hba, hba->vreg_info.vccq);
+vcc_disable:
+	ufshcd_toggle_vreg(hba->dev, hba->vreg_info.vcc, false);
+out:
+	return ret;
+}
+
+static void ufshcd_hba_vreg_set_lpm(struct ufs_hba *hba)
+{
+	if (ufshcd_is_link_off(hba))
+		ufshcd_setup_hba_vreg(hba, false);
+}
+
+static void ufshcd_hba_vreg_set_hpm(struct ufs_hba *hba)
+{
+	if (ufshcd_is_link_off(hba))
+		ufshcd_setup_hba_vreg(hba, true);
+}
+
 /**
- * ufshcd_suspend - suspend power management function
+ * ufshcd_suspend - helper function for suspend operations
  * @hba: per adapter instance
- * @state: power state
+ * @pm_op: desired low power operation type
+ *
+ * This function will try to put the UFS device and link into low power
+ * mode based on the "rpm_lvl" (Runtime PM level) or "spm_lvl"
+ * (System PM level).
+ *
+ * If this function is called during shutdown, it will make sure that
+ * both UFS device and UFS link is powered off.
  *
- * Returns -ENOSYS
+ * NOTE: UFS device & link must be active before we enter in this function.
+ *
+ * Returns 0 for success and non-zero for failure
  */
-int ufshcd_suspend(struct ufs_hba *hba, pm_message_t state)
+static int ufshcd_suspend(struct ufs_hba *hba, enum ufs_pm_op pm_op)
 {
+	int ret = 0;
+	enum ufs_pm_level pm_lvl;
+	enum ufs_dev_pwr_mode req_dev_pwr_mode;
+	enum uic_link_state req_link_state;
+
+	hba->pm_op_in_progress = 1;
+	if (!ufshcd_is_shutdown_pm(pm_op)) {
+		pm_lvl = ufshcd_is_runtime_pm(pm_op) ?
+			 hba->rpm_lvl : hba->spm_lvl;
+		req_dev_pwr_mode = ufs_get_pm_lvl_to_dev_pwr_mode(pm_lvl);
+		req_link_state = ufs_get_pm_lvl_to_link_pwr_state(pm_lvl);
+	} else {
+		req_dev_pwr_mode = UFS_POWERDOWN_PWR_MODE;
+		req_link_state = UIC_LINK_OFF_STATE;
+	}
+
 	/*
-	 * TODO:
-	 * 1. Block SCSI requests from SCSI midlayer
-	 * 2. Change the internal driver state to non operational
-	 * 3. Set UTRLRSR and UTMRLRSR bits to zero
-	 * 4. Wait until outstanding commands are completed
-	 * 5. Set HCE to zero to send the UFS host controller to reset state
+	 * If we can't transition into any of the low power modes
+	 * just gate the clocks.
 	 */
+	ufshcd_hold(hba, false);
+	hba->clk_gating.is_suspended = true;
+
+	if (req_dev_pwr_mode == UFS_ACTIVE_PWR_MODE &&
+			req_link_state == UIC_LINK_ACTIVE_STATE) {
+		goto disable_clks;
+	}
+
+	if ((req_dev_pwr_mode == hba->curr_dev_pwr_mode) &&
+	    (req_link_state == hba->uic_link_state))
+		goto out;
+
+	/* UFS device & link must be active before we enter in this function */
+	if (!ufshcd_is_ufs_dev_active(hba) || !ufshcd_is_link_active(hba)) {
+		ret = -EINVAL;
+		goto out;
+	}
 
-	return -ENOSYS;
+	if (ufshcd_is_runtime_pm(pm_op)) {
+		if (ufshcd_can_autobkops_during_suspend(hba)) {
+			/*
+			 * The device is idle with no requests in the queue,
+			 * allow background operations if bkops status shows
+			 * that performance might be impacted.
+			 */
+			ret = ufshcd_urgent_bkops(hba);
+			if (ret)
+				goto enable_gating;
+		} else {
+			/* make sure that auto bkops is disabled */
+			ufshcd_disable_auto_bkops(hba);
+		}
+	}
+
+	if ((req_dev_pwr_mode != hba->curr_dev_pwr_mode) &&
+	     ((ufshcd_is_runtime_pm(pm_op) && !hba->auto_bkops_enabled) ||
+	       !ufshcd_is_runtime_pm(pm_op))) {
+		/* ensure that bkops is disabled */
+		ufshcd_disable_auto_bkops(hba);
+		ret = ufshcd_set_dev_pwr_mode(hba, req_dev_pwr_mode);
+		if (ret)
+			goto enable_gating;
+	}
+
+	ret = ufshcd_link_state_transition(hba, req_link_state, 1);
+	if (ret)
+		goto set_dev_active;
+
+	ufshcd_vreg_set_lpm(hba);
+
+disable_clks:
+	/*
+	 * The clock scaling needs access to controller registers. Hence, Wait
+	 * for pending clock scaling work to be done before clocks are
+	 * turned off.
+	 */
+	if (ufshcd_is_clkscaling_enabled(hba)) {
+		devfreq_suspend_device(hba->devfreq);
+		hba->clk_scaling.window_start_t = 0;
+	}
+	/*
+	 * Call vendor specific suspend callback. As these callbacks may access
+	 * vendor specific host controller register space call them before the
+	 * host clocks are ON.
+	 */
+	if (hba->vops && hba->vops->suspend) {
+		ret = hba->vops->suspend(hba, pm_op);
+		if (ret)
+			goto set_link_active;
+	}
+
+	if (hba->vops && hba->vops->setup_clocks) {
+		ret = hba->vops->setup_clocks(hba, false);
+		if (ret)
+			goto vops_resume;
+	}
+
+	if (!ufshcd_is_link_active(hba))
+		ufshcd_setup_clocks(hba, false);
+	else
+		/* If link is active, device ref_clk can't be switched off */
+		__ufshcd_setup_clocks(hba, false, true);
+
+	hba->clk_gating.state = CLKS_OFF;
+	/*
+	 * Disable the host irq as host controller as there won't be any
+	 * host controller trasanction expected till resume.
+	 */
+	ufshcd_disable_irq(hba);
+	/* Put the host controller in low power mode if possible */
+	ufshcd_hba_vreg_set_lpm(hba);
+	goto out;
+
+vops_resume:
+	if (hba->vops && hba->vops->resume)
+		hba->vops->resume(hba, pm_op);
+set_link_active:
+	ufshcd_vreg_set_hpm(hba);
+	if (ufshcd_is_link_hibern8(hba) && !ufshcd_uic_hibern8_exit(hba))
+		ufshcd_set_link_active(hba);
+	else if (ufshcd_is_link_off(hba))
+		ufshcd_host_reset_and_restore(hba);
+set_dev_active:
+	if (!ufshcd_set_dev_pwr_mode(hba, UFS_ACTIVE_PWR_MODE))
+		ufshcd_disable_auto_bkops(hba);
+enable_gating:
+	hba->clk_gating.is_suspended = false;
+	ufshcd_release(hba);
+out:
+	hba->pm_op_in_progress = 0;
+	return ret;
 }
-EXPORT_SYMBOL_GPL(ufshcd_suspend);
 
 /**
- * ufshcd_resume - resume power management function
+ * ufshcd_resume - helper function for resume operations
  * @hba: per adapter instance
+ * @pm_op: runtime PM or system PM
+ *
+ * This function basically brings the UFS device, UniPro link and controller
+ * to active state.
  *
- * Returns -ENOSYS
+ * Returns 0 for success and non-zero for failure
  */
-int ufshcd_resume(struct ufs_hba *hba)
+static int ufshcd_resume(struct ufs_hba *hba, enum ufs_pm_op pm_op)
 {
+	int ret;
+	enum uic_link_state old_link_state;
+
+	hba->pm_op_in_progress = 1;
+	old_link_state = hba->uic_link_state;
+
+	ufshcd_hba_vreg_set_hpm(hba);
+	/* Make sure clocks are enabled before accessing controller */
+	ret = ufshcd_setup_clocks(hba, true);
+	if (ret)
+		goto out;
+
+	/* enable the host irq as host controller would be active soon */
+	ret = ufshcd_enable_irq(hba);
+	if (ret)
+		goto disable_irq_and_vops_clks;
+
+	ret = ufshcd_vreg_set_hpm(hba);
+	if (ret)
+		goto disable_irq_and_vops_clks;
+
+	/*
+	 * Call vendor specific resume callback. As these callbacks may access
+	 * vendor specific host controller register space call them when the
+	 * host clocks are ON.
+	 */
+	if (hba->vops && hba->vops->resume) {
+		ret = hba->vops->resume(hba, pm_op);
+		if (ret)
+			goto disable_vreg;
+	}
+
+	if (ufshcd_is_link_hibern8(hba)) {
+		ret = ufshcd_uic_hibern8_exit(hba);
+		if (!ret)
+			ufshcd_set_link_active(hba);
+		else
+			goto vendor_suspend;
+	} else if (ufshcd_is_link_off(hba)) {
+		ret = ufshcd_host_reset_and_restore(hba);
+		/*
+		 * ufshcd_host_reset_and_restore() should have already
+		 * set the link state as active
+		 */
+		if (ret || !ufshcd_is_link_active(hba))
+			goto vendor_suspend;
+	}
+
+	if (!ufshcd_is_ufs_dev_active(hba)) {
+		ret = ufshcd_set_dev_pwr_mode(hba, UFS_ACTIVE_PWR_MODE);
+		if (ret)
+			goto set_old_link_state;
+	}
+
 	/*
-	 * TODO:
-	 * 1. Set HCE to 1, to start the UFS host controller
-	 * initialization process
-	 * 2. Set UTRLRSR and UTMRLRSR bits to 1
-	 * 3. Change the internal driver state to operational
-	 * 4. Unblock SCSI requests from SCSI midlayer
+	 * If BKOPs operations are urgently needed at this moment then
+	 * keep auto-bkops enabled or else disable it.
 	 */
+	ufshcd_urgent_bkops(hba);
+	hba->clk_gating.is_suspended = false;
+
+	if (ufshcd_is_clkscaling_enabled(hba))
+		devfreq_resume_device(hba->devfreq);
+
+	/* Schedule clock gating in case of no access to UFS device yet */
+	ufshcd_release(hba);
+	goto out;
+
+set_old_link_state:
+	ufshcd_link_state_transition(hba, old_link_state, 0);
+vendor_suspend:
+	if (hba->vops && hba->vops->suspend)
+		hba->vops->suspend(hba, pm_op);
+disable_vreg:
+	ufshcd_vreg_set_lpm(hba);
+disable_irq_and_vops_clks:
+	ufshcd_disable_irq(hba);
+	ufshcd_setup_clocks(hba, false);
+out:
+	hba->pm_op_in_progress = 0;
+	return ret;
+}
+
+/**
+ * ufshcd_system_suspend - system suspend routine
+ * @hba: per adapter instance
+ * @pm_op: runtime PM or system PM
+ *
+ * Check the description of ufshcd_suspend() function for more details.
+ *
+ * Returns 0 for success and non-zero for failure
+ */
+int ufshcd_system_suspend(struct ufs_hba *hba)
+{
+	int ret = 0;
+
+	if (!hba || !hba->is_powered)
+		goto out;
+
+	if (pm_runtime_suspended(hba->dev)) {
+		if (hba->rpm_lvl == hba->spm_lvl)
+			/*
+			 * There is possibility that device may still be in
+			 * active state during the runtime suspend.
+			 */
+			if ((ufs_get_pm_lvl_to_dev_pwr_mode(hba->spm_lvl) ==
+			    hba->curr_dev_pwr_mode) && !hba->auto_bkops_enabled)
+				goto out;
+
+		/*
+		 * UFS device and/or UFS link low power states during runtime
+		 * suspend seems to be different than what is expected during
+		 * system suspend. Hence runtime resume the devic & link and
+		 * let the system suspend low power states to take effect.
+		 * TODO: If resume takes longer time, we might have optimize
+		 * it in future by not resuming everything if possible.
+		 */
+		ret = ufshcd_runtime_resume(hba);
+		if (ret)
+			goto out;
+	}
+
+	ret = ufshcd_suspend(hba, UFS_SYSTEM_PM);
+out:
+	if (!ret)
+		hba->is_sys_suspended = true;
+	return ret;
+}
+EXPORT_SYMBOL(ufshcd_system_suspend);
+
+/**
+ * ufshcd_system_resume - system resume routine
+ * @hba: per adapter instance
+ *
+ * Returns 0 for success and non-zero for failure
+ */
+
+int ufshcd_system_resume(struct ufs_hba *hba)
+{
+	if (!hba || !hba->is_powered || pm_runtime_suspended(hba->dev))
+		/*
+		 * Let the runtime resume take care of resuming
+		 * if runtime suspended.
+		 */
+		return 0;
 
-	return -ENOSYS;
+	return ufshcd_resume(hba, UFS_SYSTEM_PM);
 }
-EXPORT_SYMBOL_GPL(ufshcd_resume);
+EXPORT_SYMBOL(ufshcd_system_resume);
 
+/**
+ * ufshcd_runtime_suspend - runtime suspend routine
+ * @hba: per adapter instance
+ *
+ * Check the description of ufshcd_suspend() function for more details.
+ *
+ * Returns 0 for success and non-zero for failure
+ */
 int ufshcd_runtime_suspend(struct ufs_hba *hba)
 {
-	if (!hba)
+	if (!hba || !hba->is_powered)
 		return 0;
 
-	/*
-	 * The device is idle with no requests in the queue,
-	 * allow background operations.
-	 */
-	return ufshcd_enable_auto_bkops(hba);
+	return ufshcd_suspend(hba, UFS_RUNTIME_PM);
 }
 EXPORT_SYMBOL(ufshcd_runtime_suspend);
 
+/**
+ * ufshcd_runtime_resume - runtime resume routine
+ * @hba: per adapter instance
+ *
+ * This function basically brings the UFS device, UniPro link and controller
+ * to active state. Following operations are done in this function:
+ *
+ * 1. Turn on all the controller related clocks
+ * 2. Bring the UniPro link out of Hibernate state
+ * 3. If UFS device is in sleep state, turn ON VCC rail and bring the UFS device
+ *    to active state.
+ * 4. If auto-bkops is enabled on the device, disable it.
+ *
+ * So following would be the possible power state after this function return
+ * successfully:
+ *	S1: UFS device in Active state with VCC rail ON
+ *	    UniPro link in Active state
+ *	    All the UFS/UniPro controller clocks are ON
+ *
+ * Returns 0 for success and non-zero for failure
+ */
 int ufshcd_runtime_resume(struct ufs_hba *hba)
 {
-	if (!hba)
+	if (!hba || !hba->is_powered)
 		return 0;
-
-	return ufshcd_disable_auto_bkops(hba);
+	else
+		return ufshcd_resume(hba, UFS_RUNTIME_PM);
 }
 EXPORT_SYMBOL(ufshcd_runtime_resume);
 
@@ -3245,6 +5194,36 @@ int ufshcd_runtime_idle(struct ufs_hba *hba)
 EXPORT_SYMBOL(ufshcd_runtime_idle);
 
 /**
+ * ufshcd_shutdown - shutdown routine
+ * @hba: per adapter instance
+ *
+ * This function would power off both UFS device and UFS link.
+ *
+ * Returns 0 always to allow force shutdown even in case of errors.
+ */
+int ufshcd_shutdown(struct ufs_hba *hba)
+{
+	int ret = 0;
+
+	if (ufshcd_is_ufs_dev_poweroff(hba) && ufshcd_is_link_off(hba))
+		goto out;
+
+	if (pm_runtime_suspended(hba->dev)) {
+		ret = ufshcd_runtime_resume(hba);
+		if (ret)
+			goto out;
+	}
+
+	ret = ufshcd_suspend(hba, UFS_SHUTDOWN_PM);
+out:
+	if (ret)
+		dev_err(hba->dev, "%s failed, err %d\n", __func__, ret);
+	/* allow force shutdown even in case of errors */
+	return 0;
+}
+EXPORT_SYMBOL(ufshcd_shutdown);
+
+/**
  * ufshcd_remove - de-allocate SCSI host and host memory space
  *		data structure memory
  * @hba - per adapter instance
@@ -3252,11 +5231,17 @@ EXPORT_SYMBOL(ufshcd_runtime_idle);
 void ufshcd_remove(struct ufs_hba *hba)
 {
 	scsi_remove_host(hba->host);
+	ufshcd_scsi_remove_wlus(hba);
 	/* disable interrupts */
 	ufshcd_disable_intr(hba, hba->intr_mask);
 	ufshcd_hba_stop(hba);
 
 	scsi_host_put(hba->host);
+
+	ufshcd_exit_clk_gating(hba);
+	if (ufshcd_is_clkscaling_enabled(hba))
+		devfreq_remove_device(hba->devfreq);
+	ufshcd_hba_exit(hba);
 }
 EXPORT_SYMBOL_GPL(ufshcd_remove);
 
@@ -3277,19 +5262,16 @@ static int ufshcd_set_dma_mask(struct ufs_hba *hba)
 }
 
 /**
- * ufshcd_init - Driver initialization routine
+ * ufshcd_alloc_host - allocate Host Bus Adapter (HBA)
  * @dev: pointer to device handle
  * @hba_handle: driver private handle
- * @mmio_base: base register address
- * @irq: Interrupt line of device
  * Returns 0 on success, non-zero value on failure
  */
-int ufshcd_init(struct device *dev, struct ufs_hba **hba_handle,
-		 void __iomem *mmio_base, unsigned int irq)
+int ufshcd_alloc_host(struct device *dev, struct ufs_hba **hba_handle)
 {
 	struct Scsi_Host *host;
 	struct ufs_hba *hba;
-	int err;
+	int err = 0;
 
 	if (!dev) {
 		dev_err(dev,
@@ -3298,13 +5280,6 @@ int ufshcd_init(struct device *dev, struct ufs_hba **hba_handle,
 		goto out_error;
 	}
 
-	if (!mmio_base) {
-		dev_err(dev,
-		"Invalid memory reference for mmio_base is NULL\n");
-		err = -ENODEV;
-		goto out_error;
-	}
-
 	host = scsi_host_alloc(&ufshcd_driver_template,
 				sizeof(struct ufs_hba));
 	if (!host) {
@@ -3315,9 +5290,146 @@ int ufshcd_init(struct device *dev, struct ufs_hba **hba_handle,
 	hba = shost_priv(host);
 	hba->host = host;
 	hba->dev = dev;
+	*hba_handle = hba;
+
+out_error:
+	return err;
+}
+EXPORT_SYMBOL(ufshcd_alloc_host);
+
+static int ufshcd_scale_clks(struct ufs_hba *hba, bool scale_up)
+{
+	int ret = 0;
+	struct ufs_clk_info *clki;
+	struct list_head *head = &hba->clk_list_head;
+
+	if (!head || list_empty(head))
+		goto out;
+
+	list_for_each_entry(clki, head, list) {
+		if (!IS_ERR_OR_NULL(clki->clk)) {
+			if (scale_up && clki->max_freq) {
+				if (clki->curr_freq == clki->max_freq)
+					continue;
+				ret = clk_set_rate(clki->clk, clki->max_freq);
+				if (ret) {
+					dev_err(hba->dev, "%s: %s clk set rate(%dHz) failed, %d\n",
+						__func__, clki->name,
+						clki->max_freq, ret);
+					break;
+				}
+				clki->curr_freq = clki->max_freq;
+
+			} else if (!scale_up && clki->min_freq) {
+				if (clki->curr_freq == clki->min_freq)
+					continue;
+				ret = clk_set_rate(clki->clk, clki->min_freq);
+				if (ret) {
+					dev_err(hba->dev, "%s: %s clk set rate(%dHz) failed, %d\n",
+						__func__, clki->name,
+						clki->min_freq, ret);
+					break;
+				}
+				clki->curr_freq = clki->min_freq;
+			}
+		}
+		dev_dbg(hba->dev, "%s: clk: %s, rate: %lu\n", __func__,
+				clki->name, clk_get_rate(clki->clk));
+	}
+	if (hba->vops->clk_scale_notify)
+		hba->vops->clk_scale_notify(hba);
+out:
+	return ret;
+}
+
+static int ufshcd_devfreq_target(struct device *dev,
+				unsigned long *freq, u32 flags)
+{
+	int err = 0;
+	struct ufs_hba *hba = dev_get_drvdata(dev);
+
+	if (!ufshcd_is_clkscaling_enabled(hba))
+		return -EINVAL;
+
+	if (*freq == UINT_MAX)
+		err = ufshcd_scale_clks(hba, true);
+	else if (*freq == 0)
+		err = ufshcd_scale_clks(hba, false);
+
+	return err;
+}
+
+static int ufshcd_devfreq_get_dev_status(struct device *dev,
+		struct devfreq_dev_status *stat)
+{
+	struct ufs_hba *hba = dev_get_drvdata(dev);
+	struct ufs_clk_scaling *scaling = &hba->clk_scaling;
+	unsigned long flags;
+
+	if (!ufshcd_is_clkscaling_enabled(hba))
+		return -EINVAL;
+
+	memset(stat, 0, sizeof(*stat));
+
+	spin_lock_irqsave(hba->host->host_lock, flags);
+	if (!scaling->window_start_t)
+		goto start_window;
+
+	if (scaling->is_busy_started)
+		scaling->tot_busy_t += ktime_to_us(ktime_sub(ktime_get(),
+					scaling->busy_start_t));
+
+	stat->total_time = jiffies_to_usecs((long)jiffies -
+				(long)scaling->window_start_t);
+	stat->busy_time = scaling->tot_busy_t;
+start_window:
+	scaling->window_start_t = jiffies;
+	scaling->tot_busy_t = 0;
+
+	if (hba->outstanding_reqs) {
+		scaling->busy_start_t = ktime_get();
+		scaling->is_busy_started = true;
+	} else {
+		scaling->busy_start_t = ktime_set(0, 0);
+		scaling->is_busy_started = false;
+	}
+	spin_unlock_irqrestore(hba->host->host_lock, flags);
+	return 0;
+}
+
+static struct devfreq_dev_profile ufs_devfreq_profile = {
+	.polling_ms	= 100,
+	.target		= ufshcd_devfreq_target,
+	.get_dev_status	= ufshcd_devfreq_get_dev_status,
+};
+
+/**
+ * ufshcd_init - Driver initialization routine
+ * @hba: per-adapter instance
+ * @mmio_base: base register address
+ * @irq: Interrupt line of device
+ * Returns 0 on success, non-zero value on failure
+ */
+int ufshcd_init(struct ufs_hba *hba, void __iomem *mmio_base, unsigned int irq)
+{
+	int err;
+	struct Scsi_Host *host = hba->host;
+	struct device *dev = hba->dev;
+
+	if (!mmio_base) {
+		dev_err(hba->dev,
+		"Invalid memory reference for mmio_base is NULL\n");
+		err = -ENODEV;
+		goto out_error;
+	}
+
 	hba->mmio_base = mmio_base;
 	hba->irq = irq;
 
+	err = ufshcd_hba_init(hba);
+	if (err)
+		goto out_error;
+
 	/* Read capabilities registers */
 	ufshcd_hba_capabilities(hba);
 
@@ -3346,11 +5458,13 @@ int ufshcd_init(struct device *dev, struct ufs_hba **hba_handle,
 	host->can_queue = hba->nutrs;
 	host->cmd_per_lun = hba->nutrs;
 	host->max_id = UFSHCD_MAX_ID;
-	host->max_lun = UFSHCD_MAX_LUNS;
+	host->max_lun = UFS_MAX_LUNS;
 	host->max_channel = UFSHCD_MAX_CHANNEL;
 	host->unique_id = host->host_no;
 	host->max_cmd_len = MAX_CDB_SIZE;
 
+	hba->max_pwr_info.is_valid = false;
+
 	/* Initailize wait queue for task management */
 	init_waitqueue_head(&hba->tm_wq);
 	init_waitqueue_head(&hba->tm_tag_wq);
@@ -3368,24 +5482,27 @@ int ufshcd_init(struct device *dev, struct ufs_hba **hba_handle,
 	/* Initialize device management tag acquire wait queue */
 	init_waitqueue_head(&hba->dev_cmd.tag_wq);
 
+	ufshcd_init_clk_gating(hba);
 	/* IRQ registration */
 	err = devm_request_irq(dev, irq, ufshcd_intr, IRQF_SHARED, UFSHCD, hba);
 	if (err) {
 		dev_err(hba->dev, "request irq failed\n");
-		goto out_disable;
+		goto exit_gating;
+	} else {
+		hba->is_irq_enabled = true;
 	}
 
 	/* Enable SCSI tag mapping */
 	err = scsi_init_shared_tag_map(host, host->can_queue);
 	if (err) {
 		dev_err(hba->dev, "init shared queue failed\n");
-		goto out_disable;
+		goto exit_gating;
 	}
 
 	err = scsi_add_host(host, hba->dev);
 	if (err) {
 		dev_err(hba->dev, "scsi_add_host failed\n");
-		goto out_disable;
+		goto exit_gating;
 	}
 
 	/* Host controller enable */
@@ -3395,19 +5512,40 @@ int ufshcd_init(struct device *dev, struct ufs_hba **hba_handle,
 		goto out_remove_scsi_host;
 	}
 
-	*hba_handle = hba;
+	if (ufshcd_is_clkscaling_enabled(hba)) {
+		hba->devfreq = devfreq_add_device(dev, &ufs_devfreq_profile,
+						   "simple_ondemand", NULL);
+		if (IS_ERR(hba->devfreq)) {
+			dev_err(hba->dev, "Unable to register with devfreq %ld\n",
+					PTR_ERR(hba->devfreq));
+			goto out_remove_scsi_host;
+		}
+		/* Suspend devfreq until the UFS device is detected */
+		devfreq_suspend_device(hba->devfreq);
+		hba->clk_scaling.window_start_t = 0;
+	}
 
 	/* Hold auto suspend until async scan completes */
 	pm_runtime_get_sync(dev);
 
+	/*
+	 * The device-initialize-sequence hasn't been invoked yet.
+	 * Set the device to power-off state
+	 */
+	ufshcd_set_ufs_dev_poweroff(hba);
+
 	async_schedule(ufshcd_async_scan, hba);
 
 	return 0;
 
 out_remove_scsi_host:
 	scsi_remove_host(hba->host);
+exit_gating:
+	ufshcd_exit_clk_gating(hba);
 out_disable:
+	hba->is_irq_enabled = false;
 	scsi_host_put(host);
+	ufshcd_hba_exit(hba);
 out_error:
 	return err;
 }
diff --git a/drivers/scsi/ufs/ufshcd.h b/drivers/scsi/ufs/ufshcd.h
index acf318e338ed..58ecdff5065c 100644
--- a/drivers/scsi/ufs/ufshcd.h
+++ b/drivers/scsi/ufs/ufshcd.h
@@ -52,6 +52,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/clk.h>
 #include <linux/completion.h>
+#include <linux/regulator/consumer.h>
 
 #include <asm/irq.h>
 #include <asm/byteorder.h>
@@ -68,6 +69,8 @@
 #define UFSHCD "ufshcd"
 #define UFSHCD_DRIVER_VERSION "0.2"
 
+struct ufs_hba;
+
 enum dev_cmd_type {
 	DEV_CMD_TYPE_NOP		= 0x0,
 	DEV_CMD_TYPE_QUERY		= 0x1,
@@ -93,6 +96,54 @@ struct uic_command {
 	struct completion done;
 };
 
+/* Used to differentiate the power management options */
+enum ufs_pm_op {
+	UFS_RUNTIME_PM,
+	UFS_SYSTEM_PM,
+	UFS_SHUTDOWN_PM,
+};
+
+#define ufshcd_is_runtime_pm(op) ((op) == UFS_RUNTIME_PM)
+#define ufshcd_is_system_pm(op) ((op) == UFS_SYSTEM_PM)
+#define ufshcd_is_shutdown_pm(op) ((op) == UFS_SHUTDOWN_PM)
+
+/* Host <-> Device UniPro Link state */
+enum uic_link_state {
+	UIC_LINK_OFF_STATE	= 0, /* Link powered down or disabled */
+	UIC_LINK_ACTIVE_STATE	= 1, /* Link is in Fast/Slow/Sleep state */
+	UIC_LINK_HIBERN8_STATE	= 2, /* Link is in Hibernate state */
+};
+
+#define ufshcd_is_link_off(hba) ((hba)->uic_link_state == UIC_LINK_OFF_STATE)
+#define ufshcd_is_link_active(hba) ((hba)->uic_link_state == \
+				    UIC_LINK_ACTIVE_STATE)
+#define ufshcd_is_link_hibern8(hba) ((hba)->uic_link_state == \
+				    UIC_LINK_HIBERN8_STATE)
+#define ufshcd_set_link_off(hba) ((hba)->uic_link_state = UIC_LINK_OFF_STATE)
+#define ufshcd_set_link_active(hba) ((hba)->uic_link_state = \
+				    UIC_LINK_ACTIVE_STATE)
+#define ufshcd_set_link_hibern8(hba) ((hba)->uic_link_state = \
+				    UIC_LINK_HIBERN8_STATE)
+
+/*
+ * UFS Power management levels.
+ * Each level is in increasing order of power savings.
+ */
+enum ufs_pm_level {
+	UFS_PM_LVL_0, /* UFS_ACTIVE_PWR_MODE, UIC_LINK_ACTIVE_STATE */
+	UFS_PM_LVL_1, /* UFS_ACTIVE_PWR_MODE, UIC_LINK_HIBERN8_STATE */
+	UFS_PM_LVL_2, /* UFS_SLEEP_PWR_MODE, UIC_LINK_ACTIVE_STATE */
+	UFS_PM_LVL_3, /* UFS_SLEEP_PWR_MODE, UIC_LINK_HIBERN8_STATE */
+	UFS_PM_LVL_4, /* UFS_POWERDOWN_PWR_MODE, UIC_LINK_HIBERN8_STATE */
+	UFS_PM_LVL_5, /* UFS_POWERDOWN_PWR_MODE, UIC_LINK_OFF_STATE */
+	UFS_PM_LVL_MAX
+};
+
+struct ufs_pm_lvl_states {
+	enum ufs_dev_pwr_mode dev_state;
+	enum uic_link_state link_state;
+};
+
 /**
  * struct ufshcd_lrb - local reference block
  * @utr_descriptor_ptr: UTRD address of the command
@@ -121,7 +172,7 @@ struct ufshcd_lrb {
 
 	int command_type;
 	int task_tag;
-	unsigned int lun;
+	u8 lun; /* UPIU LUN id field is only 8-bit wide */
 	bool intr_cmd;
 };
 
@@ -153,6 +204,126 @@ struct ufs_dev_cmd {
 };
 
 /**
+ * struct ufs_clk_info - UFS clock related info
+ * @list: list headed by hba->clk_list_head
+ * @clk: clock node
+ * @name: clock name
+ * @max_freq: maximum frequency supported by the clock
+ * @min_freq: min frequency that can be used for clock scaling
+ * @curr_freq: indicates the current frequency that it is set to
+ * @enabled: variable to check against multiple enable/disable
+ */
+struct ufs_clk_info {
+	struct list_head list;
+	struct clk *clk;
+	const char *name;
+	u32 max_freq;
+	u32 min_freq;
+	u32 curr_freq;
+	bool enabled;
+};
+
+#define PRE_CHANGE      0
+#define POST_CHANGE     1
+
+struct ufs_pa_layer_attr {
+	u32 gear_rx;
+	u32 gear_tx;
+	u32 lane_rx;
+	u32 lane_tx;
+	u32 pwr_rx;
+	u32 pwr_tx;
+	u32 hs_rate;
+};
+
+struct ufs_pwr_mode_info {
+	bool is_valid;
+	struct ufs_pa_layer_attr info;
+};
+
+/**
+ * struct ufs_hba_variant_ops - variant specific callbacks
+ * @name: variant name
+ * @init: called when the driver is initialized
+ * @exit: called to cleanup everything done in init
+ * @clk_scale_notify: notifies that clks are scaled up/down
+ * @setup_clocks: called before touching any of the controller registers
+ * @setup_regulators: called before accessing the host controller
+ * @hce_enable_notify: called before and after HCE enable bit is set to allow
+ *                     variant specific Uni-Pro initialization.
+ * @link_startup_notify: called before and after Link startup is carried out
+ *                       to allow variant specific Uni-Pro initialization.
+ * @pwr_change_notify: called before and after a power mode change
+ *			is carried out to allow vendor spesific capabilities
+ *			to be set.
+ * @suspend: called during host controller PM callback
+ * @resume: called during host controller PM callback
+ */
+struct ufs_hba_variant_ops {
+	const char *name;
+	int	(*init)(struct ufs_hba *);
+	void    (*exit)(struct ufs_hba *);
+	void    (*clk_scale_notify)(struct ufs_hba *);
+	int     (*setup_clocks)(struct ufs_hba *, bool);
+	int     (*setup_regulators)(struct ufs_hba *, bool);
+	int     (*hce_enable_notify)(struct ufs_hba *, bool);
+	int     (*link_startup_notify)(struct ufs_hba *, bool);
+	int	(*pwr_change_notify)(struct ufs_hba *,
+					bool, struct ufs_pa_layer_attr *,
+					struct ufs_pa_layer_attr *);
+	int     (*suspend)(struct ufs_hba *, enum ufs_pm_op);
+	int     (*resume)(struct ufs_hba *, enum ufs_pm_op);
+};
+
+/* clock gating state  */
+enum clk_gating_state {
+	CLKS_OFF,
+	CLKS_ON,
+	REQ_CLKS_OFF,
+	REQ_CLKS_ON,
+};
+
+/**
+ * struct ufs_clk_gating - UFS clock gating related info
+ * @gate_work: worker to turn off clocks after some delay as specified in
+ * delay_ms
+ * @ungate_work: worker to turn on clocks that will be used in case of
+ * interrupt context
+ * @state: the current clocks state
+ * @delay_ms: gating delay in ms
+ * @is_suspended: clk gating is suspended when set to 1 which can be used
+ * during suspend/resume
+ * @delay_attr: sysfs attribute to control delay_attr
+ * @active_reqs: number of requests that are pending and should be waited for
+ * completion before gating clocks.
+ */
+struct ufs_clk_gating {
+	struct delayed_work gate_work;
+	struct work_struct ungate_work;
+	enum clk_gating_state state;
+	unsigned long delay_ms;
+	bool is_suspended;
+	struct device_attribute delay_attr;
+	int active_reqs;
+};
+
+struct ufs_clk_scaling {
+	ktime_t  busy_start_t;
+	bool is_busy_started;
+	unsigned long  tot_busy_t;
+	unsigned long window_start_t;
+};
+
+/**
+ * struct ufs_init_prefetch - contains data that is pre-fetched once during
+ * initialization
+ * @icc_level: icc level which was read during initialization
+ */
+struct ufs_init_prefetch {
+	u32 icc_level;
+};
+
+/**
  * struct ufs_hba - per adapter private structure
  * @mmio_base: UFSHCI base register address
  * @ucdl_base_addr: UFS Command Descriptor base address
@@ -171,6 +342,8 @@ struct ufs_dev_cmd {
  * @nutrs: Transfer Request Queue depth supported by controller
  * @nutmrs: Task Management Queue depth supported by controller
  * @ufs_version: UFS Version to which controller complies
+ * @vops: pointer to variant specific operations
+ * @priv: pointer to variant specific private data
  * @irq: Irq number of the controller
  * @active_uic_cmd: handle of active UIC command
  * @uic_cmd_mutex: mutex for uic command
@@ -183,6 +356,9 @@ struct ufs_dev_cmd {
  * @eh_flags: Error handling flags
  * @intr_mask: Interrupt Mask Bits
  * @ee_ctrl_mask: Exception event control mask
+ * @is_powered: flag to check if HBA is powered
+ * @is_init_prefetch: flag to check if data was pre-fetched in initialization
+ * @init_prefetch_data: data pre-fetched during initialization
  * @eh_work: Worker to handle UFS errors that require s/w attention
  * @eeh_work: Worker to handle exception events
  * @errors: HBA errors
@@ -191,6 +367,10 @@ struct ufs_dev_cmd {
  * @saved_uic_err: sticky UIC error mask
  * @dev_cmd: ufs device management command information
  * @auto_bkops_enabled: to track whether bkops is enabled in device
+ * @vreg_info: UFS device voltage regulator information
+ * @clk_list_head: UFS host controller clocks list node head
+ * @pwr_info: holds current power mode
+ * @max_pwr_info: keeps the device max valid pwm
  */
 struct ufs_hba {
 	void __iomem *mmio_base;
@@ -207,6 +387,21 @@ struct ufs_hba {
 
 	struct Scsi_Host *host;
 	struct device *dev;
+	/*
+	 * This field is to keep a reference to "scsi_device" corresponding to
+	 * "UFS device" W-LU.
+	 */
+	struct scsi_device *sdev_ufs_device;
+	struct scsi_device *sdev_rpmb;
+	struct scsi_device *sdev_boot;
+
+	enum ufs_dev_pwr_mode curr_dev_pwr_mode;
+	enum uic_link_state uic_link_state;
+	/* Desired UFS power management level during runtime PM */
+	enum ufs_pm_level rpm_lvl;
+	/* Desired UFS power management level during system PM */
+	enum ufs_pm_level spm_lvl;
+	int pm_op_in_progress;
 
 	struct ufshcd_lrb *lrb;
 	unsigned long lrb_in_use;
@@ -218,22 +413,28 @@ struct ufs_hba {
 	int nutrs;
 	int nutmrs;
 	u32 ufs_version;
+	struct ufs_hba_variant_ops *vops;
+	void *priv;
 	unsigned int irq;
+	bool is_irq_enabled;
 
-	struct uic_command *active_uic_cmd;
-	struct mutex uic_cmd_mutex;
 
 	wait_queue_head_t tm_wq;
 	wait_queue_head_t tm_tag_wq;
 	unsigned long tm_condition;
 	unsigned long tm_slots_in_use;
 
-	struct completion *pwr_done;
+	struct uic_command *active_uic_cmd;
+	struct mutex uic_cmd_mutex;
+	struct completion *uic_async_done;
 
 	u32 ufshcd_state;
 	u32 eh_flags;
 	u32 intr_mask;
 	u16 ee_ctrl_mask;
+	bool is_powered;
+	bool is_init_prefetch;
+	struct ufs_init_prefetch init_prefetch_data;
 
 	/* Work Queues */
 	struct work_struct eh_work;
@@ -248,16 +449,76 @@ struct ufs_hba {
 	/* Device management request data */
 	struct ufs_dev_cmd dev_cmd;
 
+	/* Keeps information of the UFS device connected to this host */
+	struct ufs_dev_info dev_info;
 	bool auto_bkops_enabled;
+	struct ufs_vreg_info vreg_info;
+	struct list_head clk_list_head;
+
+	bool wlun_dev_clr_ua;
+
+	struct ufs_pa_layer_attr pwr_info;
+	struct ufs_pwr_mode_info max_pwr_info;
+
+	struct ufs_clk_gating clk_gating;
+	/* Control to enable/disable host capabilities */
+	u32 caps;
+	/* Allow dynamic clk gating */
+#define UFSHCD_CAP_CLK_GATING	(1 << 0)
+	/* Allow hiberb8 with clk gating */
+#define UFSHCD_CAP_HIBERN8_WITH_CLK_GATING (1 << 1)
+	/* Allow dynamic clk scaling */
+#define UFSHCD_CAP_CLK_SCALING	(1 << 2)
+	/* Allow auto bkops to enabled during runtime suspend */
+#define UFSHCD_CAP_AUTO_BKOPS_SUSPEND (1 << 3)
+
+	struct devfreq *devfreq;
+	struct ufs_clk_scaling clk_scaling;
+	bool is_sys_suspended;
 };
 
+/* Returns true if clocks can be gated. Otherwise false */
+static inline bool ufshcd_is_clkgating_allowed(struct ufs_hba *hba)
+{
+	return hba->caps & UFSHCD_CAP_CLK_GATING;
+}
+static inline bool ufshcd_can_hibern8_during_gating(struct ufs_hba *hba)
+{
+	return hba->caps & UFSHCD_CAP_HIBERN8_WITH_CLK_GATING;
+}
+static inline int ufshcd_is_clkscaling_enabled(struct ufs_hba *hba)
+{
+	return hba->caps & UFSHCD_CAP_CLK_SCALING;
+}
+static inline bool ufshcd_can_autobkops_during_suspend(struct ufs_hba *hba)
+{
+	return hba->caps & UFSHCD_CAP_AUTO_BKOPS_SUSPEND;
+}
+
 #define ufshcd_writel(hba, val, reg)	\
 	writel((val), (hba)->mmio_base + (reg))
 #define ufshcd_readl(hba, reg)	\
 	readl((hba)->mmio_base + (reg))
 
-int ufshcd_init(struct device *, struct ufs_hba ** , void __iomem * ,
-			unsigned int);
+/**
+ * ufshcd_rmwl - read modify write into a register
+ * @hba - per adapter instance
+ * @mask - mask to apply on read value
+ * @val - actual value to write
+ * @reg - register address
+ */
+static inline void ufshcd_rmwl(struct ufs_hba *hba, u32 mask, u32 val, u32 reg)
+{
+	u32 tmp;
+
+	tmp = ufshcd_readl(hba, reg);
+	tmp &= ~mask;
+	tmp |= (val & mask);
+	ufshcd_writel(hba, tmp, reg);
+}
+
+int ufshcd_alloc_host(struct device *, struct ufs_hba **);
+int ufshcd_init(struct ufs_hba * , void __iomem * , unsigned int);
 void ufshcd_remove(struct ufs_hba *);
 
 /**
@@ -275,11 +536,12 @@ static inline void check_upiu_size(void)
 		GENERAL_UPIU_REQUEST_SIZE + QUERY_DESC_MAX_SIZE);
 }
 
-extern int ufshcd_suspend(struct ufs_hba *hba, pm_message_t state);
-extern int ufshcd_resume(struct ufs_hba *hba);
 extern int ufshcd_runtime_suspend(struct ufs_hba *hba);
 extern int ufshcd_runtime_resume(struct ufs_hba *hba);
 extern int ufshcd_runtime_idle(struct ufs_hba *hba);
+extern int ufshcd_system_suspend(struct ufs_hba *hba);
+extern int ufshcd_system_resume(struct ufs_hba *hba);
+extern int ufshcd_shutdown(struct ufs_hba *hba);
 extern int ufshcd_dme_set_attr(struct ufs_hba *hba, u32 attr_sel,
 			       u8 attr_set, u32 mib_val, u8 peer);
 extern int ufshcd_dme_get_attr(struct ufs_hba *hba, u32 attr_sel,
@@ -331,4 +593,6 @@ static inline int ufshcd_dme_peer_get(struct ufs_hba *hba,
 	return ufshcd_dme_get_attr(hba, attr_sel, mib_val, DME_PEER);
 }
 
+int ufshcd_hold(struct ufs_hba *hba, bool async);
+void ufshcd_release(struct ufs_hba *hba);
 #endif /* End of Header */
diff --git a/drivers/scsi/ufs/ufshci.h b/drivers/scsi/ufs/ufshci.h
index e1b844bc9460..d5721199e9cc 100644
--- a/drivers/scsi/ufs/ufshci.h
+++ b/drivers/scsi/ufs/ufshci.h
@@ -124,8 +124,11 @@ enum {
 #define CONTROLLER_FATAL_ERROR			UFS_BIT(16)
 #define SYSTEM_BUS_FATAL_ERROR			UFS_BIT(17)
 
-#define UFSHCD_UIC_MASK		(UIC_COMMAND_COMPL |\
-				 UIC_POWER_MODE)
+#define UFSHCD_UIC_PWR_MASK	(UIC_HIBERNATE_ENTER |\
+				UIC_HIBERNATE_EXIT |\
+				UIC_POWER_MODE)
+
+#define UFSHCD_UIC_MASK		(UIC_COMMAND_COMPL | UFSHCD_UIC_PWR_MASK)
 
 #define UFSHCD_ERROR_MASK	(UIC_ERROR |\
 				DEVICE_FATAL_ERROR |\
@@ -210,7 +213,7 @@ enum {
 #define UIC_GET_ATTR_ID(v)		(((v) >> 16) & 0xFFFF)
 
 /* UIC Commands */
-enum {
+enum uic_cmd_dme {
 	UIC_CMD_DME_GET			= 0x01,
 	UIC_CMD_DME_SET			= 0x02,
 	UIC_CMD_DME_PEER_GET		= 0x03,
diff --git a/drivers/scsi/ufs/unipro.h b/drivers/scsi/ufs/unipro.h
index 0bb8041c047a..3fc3e21b746b 100644
--- a/drivers/scsi/ufs/unipro.h
+++ b/drivers/scsi/ufs/unipro.h
@@ -13,6 +13,44 @@
 #define _UNIPRO_H_
 
 /*
+ * M-TX Configuration Attributes
+ */
+#define TX_MODE					0x0021
+#define TX_HSRATE_SERIES			0x0022
+#define TX_HSGEAR				0x0023
+#define TX_PWMGEAR				0x0024
+#define TX_AMPLITUDE				0x0025
+#define TX_HS_SLEWRATE				0x0026
+#define TX_SYNC_SOURCE				0x0027
+#define TX_HS_SYNC_LENGTH			0x0028
+#define TX_HS_PREPARE_LENGTH			0x0029
+#define TX_LS_PREPARE_LENGTH			0x002A
+#define TX_HIBERN8_CONTROL			0x002B
+#define TX_LCC_ENABLE				0x002C
+#define TX_PWM_BURST_CLOSURE_EXTENSION		0x002D
+#define TX_BYPASS_8B10B_ENABLE			0x002E
+#define TX_DRIVER_POLARITY			0x002F
+#define TX_HS_UNTERMINATED_LINE_DRIVE_ENABLE	0x0030
+#define TX_LS_TERMINATED_LINE_DRIVE_ENABLE	0x0031
+#define TX_LCC_SEQUENCER			0x0032
+#define TX_MIN_ACTIVATETIME			0x0033
+#define TX_PWM_G6_G7_SYNC_LENGTH		0x0034
+
+/*
+ * M-RX Configuration Attributes
+ */
+#define RX_MODE					0x00A1
+#define RX_HSRATE_SERIES			0x00A2
+#define RX_HSGEAR				0x00A3
+#define RX_PWMGEAR				0x00A4
+#define RX_LS_TERMINATED_ENABLE			0x00A5
+#define RX_HS_UNTERMINATED_ENABLE		0x00A6
+#define RX_ENTER_HIBERN8			0x00A7
+#define RX_BYPASS_8B10B_ENABLE			0x00A8
+#define RX_TERMINATION_FORCE_ENABLE		0x0089
+
+#define is_mphy_tx_attr(attr)			(attr < RX_MODE)
+/*
  * PHY Adpater attributes
  */
 #define PA_ACTIVETXDATALANES	0x1560
@@ -87,6 +125,24 @@ enum {
 	PA_HS_MODE_B	= 2,
 };
 
+enum ufs_pwm_gear_tag {
+	UFS_PWM_DONT_CHANGE,	/* Don't change Gear */
+	UFS_PWM_G1,		/* PWM Gear 1 (default for reset) */
+	UFS_PWM_G2,		/* PWM Gear 2 */
+	UFS_PWM_G3,		/* PWM Gear 3 */
+	UFS_PWM_G4,		/* PWM Gear 4 */
+	UFS_PWM_G5,		/* PWM Gear 5 */
+	UFS_PWM_G6,		/* PWM Gear 6 */
+	UFS_PWM_G7,		/* PWM Gear 7 */
+};
+
+enum ufs_hs_gear_tag {
+	UFS_HS_DONT_CHANGE,	/* Don't change Gear */
+	UFS_HS_G1,		/* HS Gear 1 (default for reset) */
+	UFS_HS_G2,		/* HS Gear 2 */
+	UFS_HS_G3,		/* HS Gear 3 */
+};
+
 /*
  * Data Link Layer Attributes
  */
diff --git a/drivers/scsi/virtio_scsi.c b/drivers/scsi/virtio_scsi.c
index eee1bc0b506e..b83846fc7859 100644
--- a/drivers/scsi/virtio_scsi.c
+++ b/drivers/scsi/virtio_scsi.c
@@ -110,6 +110,9 @@ struct virtio_scsi {
 	/* CPU hotplug notifier */
 	struct notifier_block nb;
 
+	/* Protected by event_vq lock */
+	bool stop_events;
+
 	struct virtio_scsi_vq ctrl_vq;
 	struct virtio_scsi_vq event_vq;
 	struct virtio_scsi_vq req_vqs[];
@@ -303,6 +306,11 @@ static void virtscsi_cancel_event_work(struct virtio_scsi *vscsi)
 {
 	int i;
 
+	/* Stop scheduling work before calling cancel_work_sync.  */
+	spin_lock_irq(&vscsi->event_vq.vq_lock);
+	vscsi->stop_events = true;
+	spin_unlock_irq(&vscsi->event_vq.vq_lock);
+
 	for (i = 0; i < VIRTIO_SCSI_EVENT_LEN; i++)
 		cancel_work_sync(&vscsi->event_list[i].work);
 }
@@ -390,7 +398,8 @@ static void virtscsi_complete_event(struct virtio_scsi *vscsi, void *buf)
 {
 	struct virtio_scsi_event_node *event_node = buf;
 
-	schedule_work(&event_node->work);
+	if (!vscsi->stop_events)
+		queue_work(system_freezable_wq, &event_node->work);
 }
 
 static void virtscsi_event_done(struct virtqueue *vq)
@@ -851,13 +860,6 @@ static void virtscsi_init_vq(struct virtio_scsi_vq *virtscsi_vq,
 	virtscsi_vq->vq = vq;
 }
 
-static void virtscsi_scan(struct virtio_device *vdev)
-{
-	struct Scsi_Host *shost = (struct Scsi_Host *)vdev->priv;
-
-	scsi_scan_host(shost);
-}
-
 static void virtscsi_remove_vqs(struct virtio_device *vdev)
 {
 	struct Scsi_Host *sh = virtio_scsi_host(vdev);
@@ -916,9 +918,6 @@ static int virtscsi_init(struct virtio_device *vdev,
 	virtscsi_config_set(vdev, cdb_size, VIRTIO_SCSI_CDB_SIZE);
 	virtscsi_config_set(vdev, sense_size, VIRTIO_SCSI_SENSE_SIZE);
 
-	if (virtio_has_feature(vdev, VIRTIO_SCSI_F_HOTPLUG))
-		virtscsi_kick_event_all(vscsi);
-
 	err = 0;
 
 out:
@@ -997,10 +996,13 @@ static int virtscsi_probe(struct virtio_device *vdev)
 	err = scsi_add_host(shost, &vdev->dev);
 	if (err)
 		goto scsi_add_host_failed;
-	/*
-	 * scsi_scan_host() happens in virtscsi_scan() via virtio_driver->scan()
-	 * after VIRTIO_CONFIG_S_DRIVER_OK has been set..
-	 */
+
+	virtio_device_ready(vdev);
+
+	if (virtio_has_feature(vdev, VIRTIO_SCSI_F_HOTPLUG))
+		virtscsi_kick_event_all(vscsi);
+
+	scsi_scan_host(shost);
 	return 0;
 
 scsi_add_host_failed:
@@ -1048,8 +1050,15 @@ static int virtscsi_restore(struct virtio_device *vdev)
 		return err;
 
 	err = register_hotcpu_notifier(&vscsi->nb);
-	if (err)
+	if (err) {
 		vdev->config->del_vqs(vdev);
+		return err;
+	}
+
+	virtio_device_ready(vdev);
+
+	if (virtio_has_feature(vdev, VIRTIO_SCSI_F_HOTPLUG))
+		virtscsi_kick_event_all(vscsi);
 
 	return err;
 }
@@ -1073,7 +1082,6 @@ static struct virtio_driver virtio_scsi_driver = {
 	.driver.owner = THIS_MODULE,
 	.id_table = id_table,
 	.probe = virtscsi_probe,
-	.scan = virtscsi_scan,
 #ifdef CONFIG_PM_SLEEP
 	.freeze = virtscsi_freeze,
 	.restore = virtscsi_restore,
diff --git a/drivers/scsi/vmw_pvscsi.h b/drivers/scsi/vmw_pvscsi.h
index ce4588851274..ee16f0c5c47d 100644
--- a/drivers/scsi/vmw_pvscsi.h
+++ b/drivers/scsi/vmw_pvscsi.h
@@ -32,7 +32,6 @@
 
 #define MASK(n)        ((1 << (n)) - 1)        /* make an n-bit mask */
 
-#define PCI_VENDOR_ID_VMWARE		0x15AD
 #define PCI_DEVICE_ID_VMWARE_PVSCSI	0x07C0
 
 /*
diff --git a/drivers/scsi/xen-scsifront.c b/drivers/scsi/xen-scsifront.c
new file mode 100644
index 000000000000..34199d206ba6
--- /dev/null
+++ b/drivers/scsi/xen-scsifront.c
@@ -0,0 +1,1026 @@
+/*
+ * Xen SCSI frontend driver
+ *
+ * Copyright (c) 2008, FUJITSU Limited
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation; or, when distributed
+ * separately from the Linux kernel or incorporated into other
+ * software packages, subject to the following license:
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this source file (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy, modify,
+ * merge, publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/wait.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/spinlock.h>
+#include <linux/sched.h>
+#include <linux/blkdev.h>
+#include <linux/pfn.h>
+#include <linux/slab.h>
+#include <linux/bitops.h>
+
+#include <scsi/scsi_cmnd.h>
+#include <scsi/scsi_device.h>
+#include <scsi/scsi.h>
+#include <scsi/scsi_host.h>
+
+#include <xen/xen.h>
+#include <xen/xenbus.h>
+#include <xen/grant_table.h>
+#include <xen/events.h>
+#include <xen/page.h>
+
+#include <xen/interface/grant_table.h>
+#include <xen/interface/io/vscsiif.h>
+#include <xen/interface/io/protocols.h>
+
+#include <asm/xen/hypervisor.h>
+
+
+#define GRANT_INVALID_REF	0
+
+#define VSCSIFRONT_OP_ADD_LUN	1
+#define VSCSIFRONT_OP_DEL_LUN	2
+
+/* Tuning point. */
+#define VSCSIIF_DEFAULT_CMD_PER_LUN 10
+#define VSCSIIF_MAX_TARGET          64
+#define VSCSIIF_MAX_LUN             255
+
+#define VSCSIIF_RING_SIZE	__CONST_RING_SIZE(vscsiif, PAGE_SIZE)
+#define VSCSIIF_MAX_REQS	VSCSIIF_RING_SIZE
+
+#define vscsiif_grants_sg(_sg)	(PFN_UP((_sg) *		\
+				sizeof(struct scsiif_request_segment)))
+
+struct vscsifrnt_shadow {
+	/* command between backend and frontend */
+	unsigned char act;
+	uint16_t rqid;
+
+	unsigned int nr_grants;		/* number of grants in gref[] */
+	struct scsiif_request_segment *sg;	/* scatter/gather elements */
+
+	/* Do reset or abort function. */
+	wait_queue_head_t wq_reset;	/* reset work queue           */
+	int wait_reset;			/* reset work queue condition */
+	int32_t rslt_reset;		/* reset response status:     */
+					/* SUCCESS or FAILED or:      */
+#define RSLT_RESET_WAITING	0
+#define RSLT_RESET_ERR		-1
+
+	/* Requested struct scsi_cmnd is stored from kernel. */
+	struct scsi_cmnd *sc;
+	int gref[vscsiif_grants_sg(SG_ALL) + SG_ALL];
+};
+
+struct vscsifrnt_info {
+	struct xenbus_device *dev;
+
+	struct Scsi_Host *host;
+	int host_active;
+
+	unsigned int evtchn;
+	unsigned int irq;
+
+	grant_ref_t ring_ref;
+	struct vscsiif_front_ring ring;
+	struct vscsiif_response	ring_rsp;
+
+	spinlock_t shadow_lock;
+	DECLARE_BITMAP(shadow_free_bitmap, VSCSIIF_MAX_REQS);
+	struct vscsifrnt_shadow *shadow[VSCSIIF_MAX_REQS];
+
+	wait_queue_head_t wq_sync;
+	unsigned int wait_ring_available:1;
+
+	char dev_state_path[64];
+	struct task_struct *curr;
+};
+
+static DEFINE_MUTEX(scsifront_mutex);
+
+static void scsifront_wake_up(struct vscsifrnt_info *info)
+{
+	info->wait_ring_available = 0;
+	wake_up(&info->wq_sync);
+}
+
+static int scsifront_get_rqid(struct vscsifrnt_info *info)
+{
+	unsigned long flags;
+	int free;
+
+	spin_lock_irqsave(&info->shadow_lock, flags);
+
+	free = find_first_bit(info->shadow_free_bitmap, VSCSIIF_MAX_REQS);
+	__clear_bit(free, info->shadow_free_bitmap);
+
+	spin_unlock_irqrestore(&info->shadow_lock, flags);
+
+	return free;
+}
+
+static int _scsifront_put_rqid(struct vscsifrnt_info *info, uint32_t id)
+{
+	int empty = bitmap_empty(info->shadow_free_bitmap, VSCSIIF_MAX_REQS);
+
+	__set_bit(id, info->shadow_free_bitmap);
+	info->shadow[id] = NULL;
+
+	return empty || info->wait_ring_available;
+}
+
+static void scsifront_put_rqid(struct vscsifrnt_info *info, uint32_t id)
+{
+	unsigned long flags;
+	int kick;
+
+	spin_lock_irqsave(&info->shadow_lock, flags);
+	kick = _scsifront_put_rqid(info, id);
+	spin_unlock_irqrestore(&info->shadow_lock, flags);
+
+	if (kick)
+		scsifront_wake_up(info);
+}
+
+static struct vscsiif_request *scsifront_pre_req(struct vscsifrnt_info *info)
+{
+	struct vscsiif_front_ring *ring = &(info->ring);
+	struct vscsiif_request *ring_req;
+	uint32_t id;
+
+	id = scsifront_get_rqid(info);	/* use id in response */
+	if (id >= VSCSIIF_MAX_REQS)
+		return NULL;
+
+	ring_req = RING_GET_REQUEST(&(info->ring), ring->req_prod_pvt);
+
+	ring->req_prod_pvt++;
+
+	ring_req->rqid = (uint16_t)id;
+
+	return ring_req;
+}
+
+static void scsifront_do_request(struct vscsifrnt_info *info)
+{
+	struct vscsiif_front_ring *ring = &(info->ring);
+	int notify;
+
+	RING_PUSH_REQUESTS_AND_CHECK_NOTIFY(ring, notify);
+	if (notify)
+		notify_remote_via_irq(info->irq);
+}
+
+static void scsifront_gnttab_done(struct vscsifrnt_info *info, uint32_t id)
+{
+	struct vscsifrnt_shadow *s = info->shadow[id];
+	int i;
+
+	if (s->sc->sc_data_direction == DMA_NONE)
+		return;
+
+	for (i = 0; i < s->nr_grants; i++) {
+		if (unlikely(gnttab_query_foreign_access(s->gref[i]) != 0)) {
+			shost_printk(KERN_ALERT, info->host, KBUILD_MODNAME
+				     "grant still in use by backend\n");
+			BUG();
+		}
+		gnttab_end_foreign_access(s->gref[i], 0, 0UL);
+	}
+
+	kfree(s->sg);
+}
+
+static void scsifront_cdb_cmd_done(struct vscsifrnt_info *info,
+				   struct vscsiif_response *ring_rsp)
+{
+	struct scsi_cmnd *sc;
+	uint32_t id;
+	uint8_t sense_len;
+
+	id = ring_rsp->rqid;
+	sc = info->shadow[id]->sc;
+
+	BUG_ON(sc == NULL);
+
+	scsifront_gnttab_done(info, id);
+	scsifront_put_rqid(info, id);
+
+	sc->result = ring_rsp->rslt;
+	scsi_set_resid(sc, ring_rsp->residual_len);
+
+	sense_len = min_t(uint8_t, VSCSIIF_SENSE_BUFFERSIZE,
+			  ring_rsp->sense_len);
+
+	if (sense_len)
+		memcpy(sc->sense_buffer, ring_rsp->sense_buffer, sense_len);
+
+	sc->scsi_done(sc);
+}
+
+static void scsifront_sync_cmd_done(struct vscsifrnt_info *info,
+				    struct vscsiif_response *ring_rsp)
+{
+	uint16_t id = ring_rsp->rqid;
+	unsigned long flags;
+	struct vscsifrnt_shadow *shadow = info->shadow[id];
+	int kick;
+
+	spin_lock_irqsave(&info->shadow_lock, flags);
+	shadow->wait_reset = 1;
+	switch (shadow->rslt_reset) {
+	case RSLT_RESET_WAITING:
+		shadow->rslt_reset = ring_rsp->rslt;
+		break;
+	case RSLT_RESET_ERR:
+		kick = _scsifront_put_rqid(info, id);
+		spin_unlock_irqrestore(&info->shadow_lock, flags);
+		kfree(shadow);
+		if (kick)
+			scsifront_wake_up(info);
+		return;
+	default:
+		shost_printk(KERN_ERR, info->host, KBUILD_MODNAME
+			     "bad reset state %d, possibly leaking %u\n",
+			     shadow->rslt_reset, id);
+		break;
+	}
+	spin_unlock_irqrestore(&info->shadow_lock, flags);
+
+	wake_up(&shadow->wq_reset);
+}
+
+static int scsifront_cmd_done(struct vscsifrnt_info *info)
+{
+	struct vscsiif_response *ring_rsp;
+	RING_IDX i, rp;
+	int more_to_do = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(info->host->host_lock, flags);
+
+	rp = info->ring.sring->rsp_prod;
+	rmb();	/* ordering required respective to dom0 */
+	for (i = info->ring.rsp_cons; i != rp; i++) {
+
+		ring_rsp = RING_GET_RESPONSE(&info->ring, i);
+
+		if (WARN(ring_rsp->rqid >= VSCSIIF_MAX_REQS ||
+			 test_bit(ring_rsp->rqid, info->shadow_free_bitmap),
+			 "illegal rqid %u returned by backend!\n",
+			 ring_rsp->rqid))
+			continue;
+
+		if (info->shadow[ring_rsp->rqid]->act == VSCSIIF_ACT_SCSI_CDB)
+			scsifront_cdb_cmd_done(info, ring_rsp);
+		else
+			scsifront_sync_cmd_done(info, ring_rsp);
+	}
+
+	info->ring.rsp_cons = i;
+
+	if (i != info->ring.req_prod_pvt)
+		RING_FINAL_CHECK_FOR_RESPONSES(&info->ring, more_to_do);
+	else
+		info->ring.sring->rsp_event = i + 1;
+
+	info->wait_ring_available = 0;
+
+	spin_unlock_irqrestore(info->host->host_lock, flags);
+
+	wake_up(&info->wq_sync);
+
+	return more_to_do;
+}
+
+static irqreturn_t scsifront_irq_fn(int irq, void *dev_id)
+{
+	struct vscsifrnt_info *info = dev_id;
+
+	while (scsifront_cmd_done(info))
+		/* Yield point for this unbounded loop. */
+		cond_resched();
+
+	return IRQ_HANDLED;
+}
+
+static int map_data_for_request(struct vscsifrnt_info *info,
+				struct scsi_cmnd *sc,
+				struct vscsiif_request *ring_req,
+				struct vscsifrnt_shadow *shadow)
+{
+	grant_ref_t gref_head;
+	struct page *page;
+	int err, ref, ref_cnt = 0;
+	int grant_ro = (sc->sc_data_direction == DMA_TO_DEVICE);
+	unsigned int i, off, len, bytes;
+	unsigned int data_len = scsi_bufflen(sc);
+	unsigned int data_grants = 0, seg_grants = 0;
+	struct scatterlist *sg;
+	unsigned long mfn;
+	struct scsiif_request_segment *seg;
+
+	ring_req->nr_segments = 0;
+	if (sc->sc_data_direction == DMA_NONE || !data_len)
+		return 0;
+
+	scsi_for_each_sg(sc, sg, scsi_sg_count(sc), i)
+		data_grants += PFN_UP(sg->offset + sg->length);
+
+	if (data_grants > VSCSIIF_SG_TABLESIZE) {
+		if (data_grants > info->host->sg_tablesize) {
+			shost_printk(KERN_ERR, info->host, KBUILD_MODNAME
+			     "Unable to map request_buffer for command!\n");
+			return -E2BIG;
+		}
+		seg_grants = vscsiif_grants_sg(data_grants);
+		shadow->sg = kcalloc(data_grants,
+			sizeof(struct scsiif_request_segment), GFP_ATOMIC);
+		if (!shadow->sg)
+			return -ENOMEM;
+	}
+	seg = shadow->sg ? : ring_req->seg;
+
+	err = gnttab_alloc_grant_references(seg_grants + data_grants,
+					    &gref_head);
+	if (err) {
+		kfree(shadow->sg);
+		shost_printk(KERN_ERR, info->host, KBUILD_MODNAME
+			     "gnttab_alloc_grant_references() error\n");
+		return -ENOMEM;
+	}
+
+	if (seg_grants) {
+		page = virt_to_page(seg);
+		off = (unsigned long)seg & ~PAGE_MASK;
+		len = sizeof(struct scsiif_request_segment) * data_grants;
+		while (len > 0) {
+			bytes = min_t(unsigned int, len, PAGE_SIZE - off);
+
+			ref = gnttab_claim_grant_reference(&gref_head);
+			BUG_ON(ref == -ENOSPC);
+
+			mfn = pfn_to_mfn(page_to_pfn(page));
+			gnttab_grant_foreign_access_ref(ref,
+				info->dev->otherend_id, mfn, 1);
+			shadow->gref[ref_cnt] = ref;
+			ring_req->seg[ref_cnt].gref   = ref;
+			ring_req->seg[ref_cnt].offset = (uint16_t)off;
+			ring_req->seg[ref_cnt].length = (uint16_t)bytes;
+
+			page++;
+			len -= bytes;
+			off = 0;
+			ref_cnt++;
+		}
+		BUG_ON(seg_grants < ref_cnt);
+		seg_grants = ref_cnt;
+	}
+
+	scsi_for_each_sg(sc, sg, scsi_sg_count(sc), i) {
+		page = sg_page(sg);
+		off = sg->offset;
+		len = sg->length;
+
+		while (len > 0 && data_len > 0) {
+			/*
+			 * sg sends a scatterlist that is larger than
+			 * the data_len it wants transferred for certain
+			 * IO sizes.
+			 */
+			bytes = min_t(unsigned int, len, PAGE_SIZE - off);
+			bytes = min(bytes, data_len);
+
+			ref = gnttab_claim_grant_reference(&gref_head);
+			BUG_ON(ref == -ENOSPC);
+
+			mfn = pfn_to_mfn(page_to_pfn(page));
+			gnttab_grant_foreign_access_ref(ref,
+				info->dev->otherend_id, mfn, grant_ro);
+
+			shadow->gref[ref_cnt] = ref;
+			seg->gref   = ref;
+			seg->offset = (uint16_t)off;
+			seg->length = (uint16_t)bytes;
+
+			page++;
+			seg++;
+			len -= bytes;
+			data_len -= bytes;
+			off = 0;
+			ref_cnt++;
+		}
+	}
+
+	if (seg_grants)
+		ring_req->nr_segments = VSCSIIF_SG_GRANT | seg_grants;
+	else
+		ring_req->nr_segments = (uint8_t)ref_cnt;
+	shadow->nr_grants = ref_cnt;
+
+	return 0;
+}
+
+static struct vscsiif_request *scsifront_command2ring(
+		struct vscsifrnt_info *info, struct scsi_cmnd *sc,
+		struct vscsifrnt_shadow *shadow)
+{
+	struct vscsiif_request *ring_req;
+
+	memset(shadow, 0, sizeof(*shadow));
+
+	ring_req = scsifront_pre_req(info);
+	if (!ring_req)
+		return NULL;
+
+	info->shadow[ring_req->rqid] = shadow;
+	shadow->rqid = ring_req->rqid;
+
+	ring_req->id      = sc->device->id;
+	ring_req->lun     = sc->device->lun;
+	ring_req->channel = sc->device->channel;
+	ring_req->cmd_len = sc->cmd_len;
+
+	BUG_ON(sc->cmd_len > VSCSIIF_MAX_COMMAND_SIZE);
+
+	memcpy(ring_req->cmnd, sc->cmnd, sc->cmd_len);
+
+	ring_req->sc_data_direction   = (uint8_t)sc->sc_data_direction;
+	ring_req->timeout_per_command = sc->request->timeout / HZ;
+
+	return ring_req;
+}
+
+static int scsifront_queuecommand(struct Scsi_Host *shost,
+				  struct scsi_cmnd *sc)
+{
+	struct vscsifrnt_info *info = shost_priv(shost);
+	struct vscsiif_request *ring_req;
+	struct vscsifrnt_shadow *shadow = scsi_cmd_priv(sc);
+	unsigned long flags;
+	int err;
+	uint16_t rqid;
+
+	spin_lock_irqsave(shost->host_lock, flags);
+	if (RING_FULL(&info->ring))
+		goto busy;
+
+	ring_req = scsifront_command2ring(info, sc, shadow);
+	if (!ring_req)
+		goto busy;
+
+	sc->result = 0;
+
+	rqid = ring_req->rqid;
+	ring_req->act = VSCSIIF_ACT_SCSI_CDB;
+
+	shadow->sc  = sc;
+	shadow->act = VSCSIIF_ACT_SCSI_CDB;
+
+	err = map_data_for_request(info, sc, ring_req, shadow);
+	if (err < 0) {
+		pr_debug("%s: err %d\n", __func__, err);
+		scsifront_put_rqid(info, rqid);
+		spin_unlock_irqrestore(shost->host_lock, flags);
+		if (err == -ENOMEM)
+			return SCSI_MLQUEUE_HOST_BUSY;
+		sc->result = DID_ERROR << 16;
+		sc->scsi_done(sc);
+		return 0;
+	}
+
+	scsifront_do_request(info);
+	spin_unlock_irqrestore(shost->host_lock, flags);
+
+	return 0;
+
+busy:
+	spin_unlock_irqrestore(shost->host_lock, flags);
+	pr_debug("%s: busy\n", __func__);
+	return SCSI_MLQUEUE_HOST_BUSY;
+}
+
+/*
+ * Any exception handling (reset or abort) must be forwarded to the backend.
+ * We have to wait until an answer is returned. This answer contains the
+ * result to be returned to the requestor.
+ */
+static int scsifront_action_handler(struct scsi_cmnd *sc, uint8_t act)
+{
+	struct Scsi_Host *host = sc->device->host;
+	struct vscsifrnt_info *info = shost_priv(host);
+	struct vscsifrnt_shadow *shadow, *s = scsi_cmd_priv(sc);
+	struct vscsiif_request *ring_req;
+	int err = 0;
+
+	shadow = kmalloc(sizeof(*shadow), GFP_NOIO);
+	if (!shadow)
+		return FAILED;
+
+	spin_lock_irq(host->host_lock);
+
+	for (;;) {
+		if (!RING_FULL(&info->ring)) {
+			ring_req = scsifront_command2ring(info, sc, shadow);
+			if (ring_req)
+				break;
+		}
+		if (err) {
+			spin_unlock_irq(host->host_lock);
+			kfree(shadow);
+			return FAILED;
+		}
+		info->wait_ring_available = 1;
+		spin_unlock_irq(host->host_lock);
+		err = wait_event_interruptible(info->wq_sync,
+					       !info->wait_ring_available);
+		spin_lock_irq(host->host_lock);
+	}
+
+	ring_req->act = act;
+	ring_req->ref_rqid = s->rqid;
+
+	shadow->act = act;
+	shadow->rslt_reset = RSLT_RESET_WAITING;
+	init_waitqueue_head(&shadow->wq_reset);
+
+	ring_req->nr_segments = 0;
+
+	scsifront_do_request(info);
+
+	spin_unlock_irq(host->host_lock);
+	err = wait_event_interruptible(shadow->wq_reset, shadow->wait_reset);
+	spin_lock_irq(host->host_lock);
+
+	if (!err) {
+		err = shadow->rslt_reset;
+		scsifront_put_rqid(info, shadow->rqid);
+		kfree(shadow);
+	} else {
+		spin_lock(&info->shadow_lock);
+		shadow->rslt_reset = RSLT_RESET_ERR;
+		spin_unlock(&info->shadow_lock);
+		err = FAILED;
+	}
+
+	spin_unlock_irq(host->host_lock);
+	return err;
+}
+
+static int scsifront_eh_abort_handler(struct scsi_cmnd *sc)
+{
+	pr_debug("%s\n", __func__);
+	return scsifront_action_handler(sc, VSCSIIF_ACT_SCSI_ABORT);
+}
+
+static int scsifront_dev_reset_handler(struct scsi_cmnd *sc)
+{
+	pr_debug("%s\n", __func__);
+	return scsifront_action_handler(sc, VSCSIIF_ACT_SCSI_RESET);
+}
+
+static int scsifront_sdev_configure(struct scsi_device *sdev)
+{
+	struct vscsifrnt_info *info = shost_priv(sdev->host);
+
+	if (info && current == info->curr)
+		xenbus_printf(XBT_NIL, info->dev->nodename,
+			      info->dev_state_path, "%d", XenbusStateConnected);
+
+	return 0;
+}
+
+static void scsifront_sdev_destroy(struct scsi_device *sdev)
+{
+	struct vscsifrnt_info *info = shost_priv(sdev->host);
+
+	if (info && current == info->curr)
+		xenbus_printf(XBT_NIL, info->dev->nodename,
+			      info->dev_state_path, "%d", XenbusStateClosed);
+}
+
+static struct scsi_host_template scsifront_sht = {
+	.module			= THIS_MODULE,
+	.name			= "Xen SCSI frontend driver",
+	.queuecommand		= scsifront_queuecommand,
+	.eh_abort_handler	= scsifront_eh_abort_handler,
+	.eh_device_reset_handler = scsifront_dev_reset_handler,
+	.slave_configure	= scsifront_sdev_configure,
+	.slave_destroy		= scsifront_sdev_destroy,
+	.cmd_per_lun		= VSCSIIF_DEFAULT_CMD_PER_LUN,
+	.can_queue		= VSCSIIF_MAX_REQS,
+	.this_id		= -1,
+	.cmd_size		= sizeof(struct vscsifrnt_shadow),
+	.sg_tablesize		= VSCSIIF_SG_TABLESIZE,
+	.use_clustering		= DISABLE_CLUSTERING,
+	.proc_name		= "scsifront",
+};
+
+static int scsifront_alloc_ring(struct vscsifrnt_info *info)
+{
+	struct xenbus_device *dev = info->dev;
+	struct vscsiif_sring *sring;
+	int err = -ENOMEM;
+
+	/***** Frontend to Backend ring start *****/
+	sring = (struct vscsiif_sring *)__get_free_page(GFP_KERNEL);
+	if (!sring) {
+		xenbus_dev_fatal(dev, err,
+			"fail to allocate shared ring (Front to Back)");
+		return err;
+	}
+	SHARED_RING_INIT(sring);
+	FRONT_RING_INIT(&info->ring, sring, PAGE_SIZE);
+
+	err = xenbus_grant_ring(dev, virt_to_mfn(sring));
+	if (err < 0) {
+		free_page((unsigned long)sring);
+		xenbus_dev_fatal(dev, err,
+			"fail to grant shared ring (Front to Back)");
+		return err;
+	}
+	info->ring_ref = err;
+
+	err = xenbus_alloc_evtchn(dev, &info->evtchn);
+	if (err) {
+		xenbus_dev_fatal(dev, err, "xenbus_alloc_evtchn");
+		goto free_gnttab;
+	}
+
+	err = bind_evtchn_to_irq(info->evtchn);
+	if (err <= 0) {
+		xenbus_dev_fatal(dev, err, "bind_evtchn_to_irq");
+		goto free_gnttab;
+	}
+
+	info->irq = err;
+
+	err = request_threaded_irq(info->irq, NULL, scsifront_irq_fn,
+				   IRQF_ONESHOT, "scsifront", info);
+	if (err) {
+		xenbus_dev_fatal(dev, err, "request_threaded_irq");
+		goto free_irq;
+	}
+
+	return 0;
+
+/* free resource */
+free_irq:
+	unbind_from_irqhandler(info->irq, info);
+free_gnttab:
+	gnttab_end_foreign_access(info->ring_ref, 0,
+				  (unsigned long)info->ring.sring);
+
+	return err;
+}
+
+static int scsifront_init_ring(struct vscsifrnt_info *info)
+{
+	struct xenbus_device *dev = info->dev;
+	struct xenbus_transaction xbt;
+	int err;
+
+	pr_debug("%s\n", __func__);
+
+	err = scsifront_alloc_ring(info);
+	if (err)
+		return err;
+	pr_debug("%s: %u %u\n", __func__, info->ring_ref, info->evtchn);
+
+again:
+	err = xenbus_transaction_start(&xbt);
+	if (err)
+		xenbus_dev_fatal(dev, err, "starting transaction");
+
+	err = xenbus_printf(xbt, dev->nodename, "ring-ref", "%u",
+			    info->ring_ref);
+	if (err) {
+		xenbus_dev_fatal(dev, err, "%s", "writing ring-ref");
+		goto fail;
+	}
+
+	err = xenbus_printf(xbt, dev->nodename, "event-channel", "%u",
+			    info->evtchn);
+
+	if (err) {
+		xenbus_dev_fatal(dev, err, "%s", "writing event-channel");
+		goto fail;
+	}
+
+	err = xenbus_transaction_end(xbt, 0);
+	if (err) {
+		if (err == -EAGAIN)
+			goto again;
+		xenbus_dev_fatal(dev, err, "completing transaction");
+		goto free_sring;
+	}
+
+	return 0;
+
+fail:
+	xenbus_transaction_end(xbt, 1);
+free_sring:
+	unbind_from_irqhandler(info->irq, info);
+	gnttab_end_foreign_access(info->ring_ref, 0,
+				  (unsigned long)info->ring.sring);
+
+	return err;
+}
+
+
+static int scsifront_probe(struct xenbus_device *dev,
+			   const struct xenbus_device_id *id)
+{
+	struct vscsifrnt_info *info;
+	struct Scsi_Host *host;
+	int err = -ENOMEM;
+	char name[TASK_COMM_LEN];
+
+	host = scsi_host_alloc(&scsifront_sht, sizeof(*info));
+	if (!host) {
+		xenbus_dev_fatal(dev, err, "fail to allocate scsi host");
+		return err;
+	}
+	info = (struct vscsifrnt_info *)host->hostdata;
+
+	dev_set_drvdata(&dev->dev, info);
+	info->dev = dev;
+
+	bitmap_fill(info->shadow_free_bitmap, VSCSIIF_MAX_REQS);
+
+	err = scsifront_init_ring(info);
+	if (err) {
+		scsi_host_put(host);
+		return err;
+	}
+
+	init_waitqueue_head(&info->wq_sync);
+	spin_lock_init(&info->shadow_lock);
+
+	snprintf(name, TASK_COMM_LEN, "vscsiif.%d", host->host_no);
+
+	host->max_id      = VSCSIIF_MAX_TARGET;
+	host->max_channel = 0;
+	host->max_lun     = VSCSIIF_MAX_LUN;
+	host->max_sectors = (host->sg_tablesize - 1) * PAGE_SIZE / 512;
+	host->max_cmd_len = VSCSIIF_MAX_COMMAND_SIZE;
+
+	err = scsi_add_host(host, &dev->dev);
+	if (err) {
+		dev_err(&dev->dev, "fail to add scsi host %d\n", err);
+		goto free_sring;
+	}
+	info->host = host;
+	info->host_active = 1;
+
+	xenbus_switch_state(dev, XenbusStateInitialised);
+
+	return 0;
+
+free_sring:
+	unbind_from_irqhandler(info->irq, info);
+	gnttab_end_foreign_access(info->ring_ref, 0,
+				  (unsigned long)info->ring.sring);
+	scsi_host_put(host);
+	return err;
+}
+
+static int scsifront_remove(struct xenbus_device *dev)
+{
+	struct vscsifrnt_info *info = dev_get_drvdata(&dev->dev);
+
+	pr_debug("%s: %s removed\n", __func__, dev->nodename);
+
+	mutex_lock(&scsifront_mutex);
+	if (info->host_active) {
+		/* Scsi_host not yet removed */
+		scsi_remove_host(info->host);
+		info->host_active = 0;
+	}
+	mutex_unlock(&scsifront_mutex);
+
+	gnttab_end_foreign_access(info->ring_ref, 0,
+				  (unsigned long)info->ring.sring);
+	unbind_from_irqhandler(info->irq, info);
+
+	scsi_host_put(info->host);
+
+	return 0;
+}
+
+static void scsifront_disconnect(struct vscsifrnt_info *info)
+{
+	struct xenbus_device *dev = info->dev;
+	struct Scsi_Host *host = info->host;
+
+	pr_debug("%s: %s disconnect\n", __func__, dev->nodename);
+
+	/*
+	 * When this function is executed, all devices of
+	 * Frontend have been deleted.
+	 * Therefore, it need not block I/O before remove_host.
+	 */
+
+	mutex_lock(&scsifront_mutex);
+	if (info->host_active) {
+		scsi_remove_host(host);
+		info->host_active = 0;
+	}
+	mutex_unlock(&scsifront_mutex);
+
+	xenbus_frontend_closed(dev);
+}
+
+static void scsifront_do_lun_hotplug(struct vscsifrnt_info *info, int op)
+{
+	struct xenbus_device *dev = info->dev;
+	int i, err = 0;
+	char str[64];
+	char **dir;
+	unsigned int dir_n = 0;
+	unsigned int device_state;
+	unsigned int hst, chn, tgt, lun;
+	struct scsi_device *sdev;
+
+	dir = xenbus_directory(XBT_NIL, dev->otherend, "vscsi-devs", &dir_n);
+	if (IS_ERR(dir))
+		return;
+
+	/* mark current task as the one allowed to modify device states */
+	BUG_ON(info->curr);
+	info->curr = current;
+
+	for (i = 0; i < dir_n; i++) {
+		/* read status */
+		snprintf(str, sizeof(str), "vscsi-devs/%s/state", dir[i]);
+		err = xenbus_scanf(XBT_NIL, dev->otherend, str, "%u",
+				   &device_state);
+		if (XENBUS_EXIST_ERR(err))
+			continue;
+
+		/* virtual SCSI device */
+		snprintf(str, sizeof(str), "vscsi-devs/%s/v-dev", dir[i]);
+		err = xenbus_scanf(XBT_NIL, dev->otherend, str,
+				   "%u:%u:%u:%u", &hst, &chn, &tgt, &lun);
+		if (XENBUS_EXIST_ERR(err))
+			continue;
+
+		/*
+		 * Front device state path, used in slave_configure called
+		 * on successfull scsi_add_device, and in slave_destroy called
+		 * on remove of a device.
+		 */
+		snprintf(info->dev_state_path, sizeof(info->dev_state_path),
+			 "vscsi-devs/%s/state", dir[i]);
+
+		switch (op) {
+		case VSCSIFRONT_OP_ADD_LUN:
+			if (device_state != XenbusStateInitialised)
+				break;
+
+			if (scsi_add_device(info->host, chn, tgt, lun)) {
+				dev_err(&dev->dev, "scsi_add_device\n");
+				xenbus_printf(XBT_NIL, dev->nodename,
+					      info->dev_state_path,
+					      "%d", XenbusStateClosed);
+			}
+			break;
+		case VSCSIFRONT_OP_DEL_LUN:
+			if (device_state != XenbusStateClosing)
+				break;
+
+			sdev = scsi_device_lookup(info->host, chn, tgt, lun);
+			if (sdev) {
+				scsi_remove_device(sdev);
+				scsi_device_put(sdev);
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	info->curr = NULL;
+
+	kfree(dir);
+}
+
+static void scsifront_read_backend_params(struct xenbus_device *dev,
+					  struct vscsifrnt_info *info)
+{
+	unsigned int sg_grant;
+	int ret;
+	struct Scsi_Host *host = info->host;
+
+	ret = xenbus_scanf(XBT_NIL, dev->otherend, "feature-sg-grant", "%u",
+			   &sg_grant);
+	if (ret == 1 && sg_grant) {
+		sg_grant = min_t(unsigned int, sg_grant, SG_ALL);
+		sg_grant = max_t(unsigned int, sg_grant, VSCSIIF_SG_TABLESIZE);
+		host->sg_tablesize = min_t(unsigned int, sg_grant,
+			VSCSIIF_SG_TABLESIZE * PAGE_SIZE /
+			sizeof(struct scsiif_request_segment));
+		host->max_sectors = (host->sg_tablesize - 1) * PAGE_SIZE / 512;
+	}
+	dev_info(&dev->dev, "using up to %d SG entries\n", host->sg_tablesize);
+}
+
+static void scsifront_backend_changed(struct xenbus_device *dev,
+				      enum xenbus_state backend_state)
+{
+	struct vscsifrnt_info *info = dev_get_drvdata(&dev->dev);
+
+	pr_debug("%s: %p %u %u\n", __func__, dev, dev->state, backend_state);
+
+	switch (backend_state) {
+	case XenbusStateUnknown:
+	case XenbusStateInitialising:
+	case XenbusStateInitWait:
+	case XenbusStateInitialised:
+		break;
+
+	case XenbusStateConnected:
+		scsifront_read_backend_params(dev, info);
+		if (xenbus_read_driver_state(dev->nodename) ==
+		    XenbusStateInitialised)
+			scsifront_do_lun_hotplug(info, VSCSIFRONT_OP_ADD_LUN);
+
+		if (dev->state != XenbusStateConnected)
+			xenbus_switch_state(dev, XenbusStateConnected);
+		break;
+
+	case XenbusStateClosed:
+		if (dev->state == XenbusStateClosed)
+			break;
+		/* Missed the backend's Closing state -- fallthrough */
+	case XenbusStateClosing:
+		scsifront_disconnect(info);
+		break;
+
+	case XenbusStateReconfiguring:
+		scsifront_do_lun_hotplug(info, VSCSIFRONT_OP_DEL_LUN);
+		xenbus_switch_state(dev, XenbusStateReconfiguring);
+		break;
+
+	case XenbusStateReconfigured:
+		scsifront_do_lun_hotplug(info, VSCSIFRONT_OP_ADD_LUN);
+		xenbus_switch_state(dev, XenbusStateConnected);
+		break;
+	}
+}
+
+static const struct xenbus_device_id scsifront_ids[] = {
+	{ "vscsi" },
+	{ "" }
+};
+
+static struct xenbus_driver scsifront_driver = {
+	.ids			= scsifront_ids,
+	.probe			= scsifront_probe,
+	.remove			= scsifront_remove,
+	.otherend_changed	= scsifront_backend_changed,
+};
+
+static int __init scsifront_init(void)
+{
+	if (!xen_domain())
+		return -ENODEV;
+
+	return xenbus_register_frontend(&scsifront_driver);
+}
+module_init(scsifront_init);
+
+static void __exit scsifront_exit(void)
+{
+	xenbus_unregister_driver(&scsifront_driver);
+}
+module_exit(scsifront_exit);
+
+MODULE_DESCRIPTION("Xen SCSI frontend driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("xen:vscsi");
+MODULE_AUTHOR("Juergen Gross <jgross@suse.com>");
diff --git a/drivers/sh/Makefile b/drivers/sh/Makefile
index 788ed9b59b4e..114203f32843 100644
--- a/drivers/sh/Makefile
+++ b/drivers/sh/Makefile
@@ -1,8 +1,7 @@
 #
 # Makefile for the SuperH specific drivers.
 #
-obj-$(CONFIG_SUPERH)			+= intc/
-obj-$(CONFIG_ARCH_SHMOBILE_LEGACY)	+= intc/
+obj-$(CONFIG_SH_INTC)			+= intc/
 ifneq ($(CONFIG_COMMON_CLK),y)
 obj-$(CONFIG_HAVE_CLK)			+= clk/
 endif
diff --git a/drivers/sh/intc/Kconfig b/drivers/sh/intc/Kconfig
index 60228fae943f..6a1b05ddc8c9 100644
--- a/drivers/sh/intc/Kconfig
+++ b/drivers/sh/intc/Kconfig
@@ -1,7 +1,9 @@
 config SH_INTC
-	def_bool y
+	bool
 	select IRQ_DOMAIN
 
+if SH_INTC
+
 comment "Interrupt controller options"
 
 config INTC_USERIMASK
@@ -37,3 +39,5 @@ config INTC_MAPPING_DEBUG
 	  between system IRQs and the per-controller id tables.
 
 	  If in doubt, say N.
+
+endif
diff --git a/drivers/sh/pm_runtime.c b/drivers/sh/pm_runtime.c
index 72f63817a1a0..fe2c2d595f59 100644
--- a/drivers/sh/pm_runtime.c
+++ b/drivers/sh/pm_runtime.c
@@ -75,8 +75,6 @@ static struct pm_clk_notifier_block platform_bus_notifier = {
 	.con_ids = { NULL, },
 };
 
-static bool default_pm_on;
-
 static int __init sh_pm_runtime_init(void)
 {
 	if (IS_ENABLED(CONFIG_ARCH_SHMOBILE_MULTI)) {
@@ -96,16 +94,7 @@ static int __init sh_pm_runtime_init(void)
 			return 0;
 	}
 
-	default_pm_on = true;
 	pm_clk_add_notifier(&platform_bus_type, &platform_bus_notifier);
 	return 0;
 }
 core_initcall(sh_pm_runtime_init);
-
-static int __init sh_pm_runtime_late_init(void)
-{
-	if (default_pm_on)
-		pm_genpd_poweroff_unused();
-	return 0;
-}
-late_initcall(sh_pm_runtime_late_init);
diff --git a/drivers/soc/Kconfig b/drivers/soc/Kconfig
index c8543855aa82..76d6bd4da138 100644
--- a/drivers/soc/Kconfig
+++ b/drivers/soc/Kconfig
@@ -1,5 +1,7 @@
 menu "SOC (System On Chip) specific Drivers"
 
 source "drivers/soc/qcom/Kconfig"
+source "drivers/soc/ti/Kconfig"
+source "drivers/soc/versatile/Kconfig"
 
 endmenu
diff --git a/drivers/soc/Makefile b/drivers/soc/Makefile
index 3b1b95d932d1..063113d0bd38 100644
--- a/drivers/soc/Makefile
+++ b/drivers/soc/Makefile
@@ -4,3 +4,5 @@
 
 obj-$(CONFIG_ARCH_QCOM)		+= qcom/
 obj-$(CONFIG_ARCH_TEGRA)	+= tegra/
+obj-$(CONFIG_SOC_TI)		+= ti/
+obj-$(CONFIG_PLAT_VERSATILE)	+= versatile/
diff --git a/drivers/soc/qcom/qcom_gsbi.c b/drivers/soc/qcom/qcom_gsbi.c
index 447458e696a9..7e1f120f2b32 100644
--- a/drivers/soc/qcom/qcom_gsbi.c
+++ b/drivers/soc/qcom/qcom_gsbi.c
@@ -22,44 +22,63 @@
 #define GSBI_CTRL_REG		0x0000
 #define GSBI_PROTOCOL_SHIFT	4
 
+struct gsbi_info {
+	struct clk *hclk;
+	u32 mode;
+	u32 crci;
+};
+
 static int gsbi_probe(struct platform_device *pdev)
 {
 	struct device_node *node = pdev->dev.of_node;
 	struct resource *res;
 	void __iomem *base;
-	struct clk *hclk;
-	u32 mode, crci = 0;
+	struct gsbi_info *gsbi;
+
+	gsbi = devm_kzalloc(&pdev->dev, sizeof(*gsbi), GFP_KERNEL);
+
+	if (!gsbi)
+		return -ENOMEM;
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(base))
 		return PTR_ERR(base);
 
-	if (of_property_read_u32(node, "qcom,mode", &mode)) {
+	if (of_property_read_u32(node, "qcom,mode", &gsbi->mode)) {
 		dev_err(&pdev->dev, "missing mode configuration\n");
 		return -EINVAL;
 	}
 
 	/* not required, so default to 0 if not present */
-	of_property_read_u32(node, "qcom,crci", &crci);
+	of_property_read_u32(node, "qcom,crci", &gsbi->crci);
 
-	dev_info(&pdev->dev, "GSBI port protocol: %d crci: %d\n", mode, crci);
+	dev_info(&pdev->dev, "GSBI port protocol: %d crci: %d\n",
+		 gsbi->mode, gsbi->crci);
+	gsbi->hclk = devm_clk_get(&pdev->dev, "iface");
+	if (IS_ERR(gsbi->hclk))
+		return PTR_ERR(gsbi->hclk);
 
-	hclk = devm_clk_get(&pdev->dev, "iface");
-	if (IS_ERR(hclk))
-		return PTR_ERR(hclk);
+	clk_prepare_enable(gsbi->hclk);
 
-	clk_prepare_enable(hclk);
-
-	writel_relaxed((mode << GSBI_PROTOCOL_SHIFT) | crci,
+	writel_relaxed((gsbi->mode << GSBI_PROTOCOL_SHIFT) | gsbi->crci,
 				base + GSBI_CTRL_REG);
 
 	/* make sure the gsbi control write is not reordered */
 	wmb();
 
-	clk_disable_unprepare(hclk);
+	platform_set_drvdata(pdev, gsbi);
+
+	return of_platform_populate(node, NULL, NULL, &pdev->dev);
+}
+
+static int gsbi_remove(struct platform_device *pdev)
+{
+	struct gsbi_info *gsbi = platform_get_drvdata(pdev);
+
+	clk_disable_unprepare(gsbi->hclk);
 
-	return of_platform_populate(pdev->dev.of_node, NULL, NULL, &pdev->dev);
+	return 0;
 }
 
 static const struct of_device_id gsbi_dt_match[] = {
@@ -76,6 +95,7 @@ static struct platform_driver gsbi_driver = {
 		.of_match_table	= gsbi_dt_match,
 	},
 	.probe = gsbi_probe,
+	.remove	= gsbi_remove,
 };
 
 module_platform_driver(gsbi_driver);
diff --git a/drivers/soc/ti/Kconfig b/drivers/soc/ti/Kconfig
new file mode 100644
index 000000000000..7266b2165183
--- /dev/null
+++ b/drivers/soc/ti/Kconfig
@@ -0,0 +1,31 @@
+#
+# TI SOC drivers
+#
+menuconfig SOC_TI
+	bool "TI SOC drivers support"
+
+if SOC_TI
+
+config KEYSTONE_NAVIGATOR_QMSS
+	tristate "Keystone Queue Manager Sub System"
+	depends on ARCH_KEYSTONE
+	help
+	  Say y here to support the Keystone multicore Navigator Queue
+	  Manager support. The Queue Manager is a hardware module that
+	  is responsible for accelerating management of the packet queues.
+	  Packets are queued/de-queued by writing/reading descriptor address
+	  to a particular memory mapped location in the Queue Manager module.
+
+	  If unsure, say N.
+
+config KEYSTONE_NAVIGATOR_DMA
+	tristate "TI Keystone Navigator Packet DMA support"
+	depends on ARCH_KEYSTONE
+	help
+	  Say y tp enable support for the Keystone Navigator Packet DMA on
+	  on Keystone family of devices. It sets up the dma channels for the
+	  Queue Manager Sub System.
+
+	  If unsure, say N.
+
+endif # SOC_TI
diff --git a/drivers/soc/ti/Makefile b/drivers/soc/ti/Makefile
new file mode 100644
index 000000000000..6bed611e1934
--- /dev/null
+++ b/drivers/soc/ti/Makefile
@@ -0,0 +1,5 @@
+#
+# TI Keystone SOC drivers
+#
+obj-$(CONFIG_KEYSTONE_NAVIGATOR_QMSS)	+= knav_qmss_queue.o knav_qmss_acc.o
+obj-$(CONFIG_KEYSTONE_NAVIGATOR_DMA)	+= knav_dma.o
diff --git a/drivers/soc/ti/knav_dma.c b/drivers/soc/ti/knav_dma.c
new file mode 100644
index 000000000000..17264275f32b
--- /dev/null
+++ b/drivers/soc/ti/knav_dma.c
@@ -0,0 +1,815 @@
+/*
+ * Copyright (C) 2014 Texas Instruments Incorporated
+ * Authors:	Santosh Shilimkar <santosh.shilimkar@ti.com>
+ *		Sandeep Nair <sandeep_n@ti.com>
+ *		Cyril Chemparathy <cyril@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation version 2.
+ *
+ * This program is distributed "as is" WITHOUT ANY WARRANTY of any
+ * kind, whether express or implied; without even the implied warranty
+ * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/io.h>
+#include <linux/sched.h>
+#include <linux/module.h>
+#include <linux/dma-direction.h>
+#include <linux/interrupt.h>
+#include <linux/pm_runtime.h>
+#include <linux/of_dma.h>
+#include <linux/of_address.h>
+#include <linux/platform_device.h>
+#include <linux/soc/ti/knav_dma.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+
+#define REG_MASK		0xffffffff
+
+#define DMA_LOOPBACK		BIT(31)
+#define DMA_ENABLE		BIT(31)
+#define DMA_TEARDOWN		BIT(30)
+
+#define DMA_TX_FILT_PSWORDS	BIT(29)
+#define DMA_TX_FILT_EINFO	BIT(30)
+#define DMA_TX_PRIO_SHIFT	0
+#define DMA_RX_PRIO_SHIFT	16
+#define DMA_PRIO_MASK		GENMASK(3, 0)
+#define DMA_PRIO_DEFAULT	0
+#define DMA_RX_TIMEOUT_DEFAULT	17500 /* cycles */
+#define DMA_RX_TIMEOUT_MASK	GENMASK(16, 0)
+#define DMA_RX_TIMEOUT_SHIFT	0
+
+#define CHAN_HAS_EPIB		BIT(30)
+#define CHAN_HAS_PSINFO		BIT(29)
+#define CHAN_ERR_RETRY		BIT(28)
+#define CHAN_PSINFO_AT_SOP	BIT(25)
+#define CHAN_SOP_OFF_SHIFT	16
+#define CHAN_SOP_OFF_MASK	GENMASK(9, 0)
+#define DESC_TYPE_SHIFT		26
+#define DESC_TYPE_MASK		GENMASK(2, 0)
+
+/*
+ * QMGR & QNUM together make up 14 bits with QMGR as the 2 MSb's in the logical
+ * navigator cloud mapping scheme.
+ * using the 14bit physical queue numbers directly maps into this scheme.
+ */
+#define CHAN_QNUM_MASK		GENMASK(14, 0)
+#define DMA_MAX_QMS		4
+#define DMA_TIMEOUT		1	/* msecs */
+#define DMA_INVALID_ID		0xffff
+
+struct reg_global {
+	u32	revision;
+	u32	perf_control;
+	u32	emulation_control;
+	u32	priority_control;
+	u32	qm_base_address[DMA_MAX_QMS];
+};
+
+struct reg_chan {
+	u32	control;
+	u32	mode;
+	u32	__rsvd[6];
+};
+
+struct reg_tx_sched {
+	u32	prio;
+};
+
+struct reg_rx_flow {
+	u32	control;
+	u32	tags;
+	u32	tag_sel;
+	u32	fdq_sel[2];
+	u32	thresh[3];
+};
+
+struct knav_dma_pool_device {
+	struct device			*dev;
+	struct list_head		list;
+};
+
+struct knav_dma_device {
+	bool				loopback, enable_all;
+	unsigned			tx_priority, rx_priority, rx_timeout;
+	unsigned			logical_queue_managers;
+	unsigned			qm_base_address[DMA_MAX_QMS];
+	struct reg_global __iomem	*reg_global;
+	struct reg_chan __iomem		*reg_tx_chan;
+	struct reg_rx_flow __iomem	*reg_rx_flow;
+	struct reg_chan __iomem		*reg_rx_chan;
+	struct reg_tx_sched __iomem	*reg_tx_sched;
+	unsigned			max_rx_chan, max_tx_chan;
+	unsigned			max_rx_flow;
+	char				name[32];
+	atomic_t			ref_count;
+	struct list_head		list;
+	struct list_head		chan_list;
+	spinlock_t			lock;
+};
+
+struct knav_dma_chan {
+	enum dma_transfer_direction	direction;
+	struct knav_dma_device		*dma;
+	atomic_t			ref_count;
+
+	/* registers */
+	struct reg_chan __iomem		*reg_chan;
+	struct reg_tx_sched __iomem	*reg_tx_sched;
+	struct reg_rx_flow __iomem	*reg_rx_flow;
+
+	/* configuration stuff */
+	unsigned			channel, flow;
+	struct knav_dma_cfg		cfg;
+	struct list_head		list;
+	spinlock_t			lock;
+};
+
+#define chan_number(ch)	((ch->direction == DMA_MEM_TO_DEV) ? \
+			ch->channel : ch->flow)
+
+static struct knav_dma_pool_device *kdev;
+
+static bool check_config(struct knav_dma_chan *chan, struct knav_dma_cfg *cfg)
+{
+	if (!memcmp(&chan->cfg, cfg, sizeof(*cfg)))
+		return true;
+	else
+		return false;
+}
+
+static int chan_start(struct knav_dma_chan *chan,
+			struct knav_dma_cfg *cfg)
+{
+	u32 v = 0;
+
+	spin_lock(&chan->lock);
+	if ((chan->direction == DMA_MEM_TO_DEV) && chan->reg_chan) {
+		if (cfg->u.tx.filt_pswords)
+			v |= DMA_TX_FILT_PSWORDS;
+		if (cfg->u.tx.filt_einfo)
+			v |= DMA_TX_FILT_EINFO;
+		writel_relaxed(v, &chan->reg_chan->mode);
+		writel_relaxed(DMA_ENABLE, &chan->reg_chan->control);
+	}
+
+	if (chan->reg_tx_sched)
+		writel_relaxed(cfg->u.tx.priority, &chan->reg_tx_sched->prio);
+
+	if (chan->reg_rx_flow) {
+		v = 0;
+
+		if (cfg->u.rx.einfo_present)
+			v |= CHAN_HAS_EPIB;
+		if (cfg->u.rx.psinfo_present)
+			v |= CHAN_HAS_PSINFO;
+		if (cfg->u.rx.err_mode == DMA_RETRY)
+			v |= CHAN_ERR_RETRY;
+		v |= (cfg->u.rx.desc_type & DESC_TYPE_MASK) << DESC_TYPE_SHIFT;
+		if (cfg->u.rx.psinfo_at_sop)
+			v |= CHAN_PSINFO_AT_SOP;
+		v |= (cfg->u.rx.sop_offset & CHAN_SOP_OFF_MASK)
+			<< CHAN_SOP_OFF_SHIFT;
+		v |= cfg->u.rx.dst_q & CHAN_QNUM_MASK;
+
+		writel_relaxed(v, &chan->reg_rx_flow->control);
+		writel_relaxed(0, &chan->reg_rx_flow->tags);
+		writel_relaxed(0, &chan->reg_rx_flow->tag_sel);
+
+		v =  cfg->u.rx.fdq[0] << 16;
+		v |=  cfg->u.rx.fdq[1] & CHAN_QNUM_MASK;
+		writel_relaxed(v, &chan->reg_rx_flow->fdq_sel[0]);
+
+		v =  cfg->u.rx.fdq[2] << 16;
+		v |=  cfg->u.rx.fdq[3] & CHAN_QNUM_MASK;
+		writel_relaxed(v, &chan->reg_rx_flow->fdq_sel[1]);
+
+		writel_relaxed(0, &chan->reg_rx_flow->thresh[0]);
+		writel_relaxed(0, &chan->reg_rx_flow->thresh[1]);
+		writel_relaxed(0, &chan->reg_rx_flow->thresh[2]);
+	}
+
+	/* Keep a copy of the cfg */
+	memcpy(&chan->cfg, cfg, sizeof(*cfg));
+	spin_unlock(&chan->lock);
+
+	return 0;
+}
+
+static int chan_teardown(struct knav_dma_chan *chan)
+{
+	unsigned long end, value;
+
+	if (!chan->reg_chan)
+		return 0;
+
+	/* indicate teardown */
+	writel_relaxed(DMA_TEARDOWN, &chan->reg_chan->control);
+
+	/* wait for the dma to shut itself down */
+	end = jiffies + msecs_to_jiffies(DMA_TIMEOUT);
+	do {
+		value = readl_relaxed(&chan->reg_chan->control);
+		if ((value & DMA_ENABLE) == 0)
+			break;
+	} while (time_after(end, jiffies));
+
+	if (readl_relaxed(&chan->reg_chan->control) & DMA_ENABLE) {
+		dev_err(kdev->dev, "timeout waiting for teardown\n");
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static void chan_stop(struct knav_dma_chan *chan)
+{
+	spin_lock(&chan->lock);
+	if (chan->reg_rx_flow) {
+		/* first detach fdqs, starve out the flow */
+		writel_relaxed(0, &chan->reg_rx_flow->fdq_sel[0]);
+		writel_relaxed(0, &chan->reg_rx_flow->fdq_sel[1]);
+		writel_relaxed(0, &chan->reg_rx_flow->thresh[0]);
+		writel_relaxed(0, &chan->reg_rx_flow->thresh[1]);
+		writel_relaxed(0, &chan->reg_rx_flow->thresh[2]);
+	}
+
+	/* teardown the dma channel */
+	chan_teardown(chan);
+
+	/* then disconnect the completion side */
+	if (chan->reg_rx_flow) {
+		writel_relaxed(0, &chan->reg_rx_flow->control);
+		writel_relaxed(0, &chan->reg_rx_flow->tags);
+		writel_relaxed(0, &chan->reg_rx_flow->tag_sel);
+	}
+
+	memset(&chan->cfg, 0, sizeof(struct knav_dma_cfg));
+	spin_unlock(&chan->lock);
+
+	dev_dbg(kdev->dev, "channel stopped\n");
+}
+
+static void dma_hw_enable_all(struct knav_dma_device *dma)
+{
+	int i;
+
+	for (i = 0; i < dma->max_tx_chan; i++) {
+		writel_relaxed(0, &dma->reg_tx_chan[i].mode);
+		writel_relaxed(DMA_ENABLE, &dma->reg_tx_chan[i].control);
+	}
+}
+
+
+static void knav_dma_hw_init(struct knav_dma_device *dma)
+{
+	unsigned v;
+	int i;
+
+	spin_lock(&dma->lock);
+	v  = dma->loopback ? DMA_LOOPBACK : 0;
+	writel_relaxed(v, &dma->reg_global->emulation_control);
+
+	v = readl_relaxed(&dma->reg_global->perf_control);
+	v |= ((dma->rx_timeout & DMA_RX_TIMEOUT_MASK) << DMA_RX_TIMEOUT_SHIFT);
+	writel_relaxed(v, &dma->reg_global->perf_control);
+
+	v = ((dma->tx_priority << DMA_TX_PRIO_SHIFT) |
+	     (dma->rx_priority << DMA_RX_PRIO_SHIFT));
+
+	writel_relaxed(v, &dma->reg_global->priority_control);
+
+	/* Always enable all Rx channels. Rx paths are managed using flows */
+	for (i = 0; i < dma->max_rx_chan; i++)
+		writel_relaxed(DMA_ENABLE, &dma->reg_rx_chan[i].control);
+
+	for (i = 0; i < dma->logical_queue_managers; i++)
+		writel_relaxed(dma->qm_base_address[i],
+			       &dma->reg_global->qm_base_address[i]);
+	spin_unlock(&dma->lock);
+}
+
+static void knav_dma_hw_destroy(struct knav_dma_device *dma)
+{
+	int i;
+	unsigned v;
+
+	spin_lock(&dma->lock);
+	v = ~DMA_ENABLE & REG_MASK;
+
+	for (i = 0; i < dma->max_rx_chan; i++)
+		writel_relaxed(v, &dma->reg_rx_chan[i].control);
+
+	for (i = 0; i < dma->max_tx_chan; i++)
+		writel_relaxed(v, &dma->reg_tx_chan[i].control);
+	spin_unlock(&dma->lock);
+}
+
+static void dma_debug_show_channels(struct seq_file *s,
+					struct knav_dma_chan *chan)
+{
+	int i;
+
+	seq_printf(s, "\t%s %d:\t",
+		((chan->direction == DMA_MEM_TO_DEV) ? "tx chan" : "rx flow"),
+		chan_number(chan));
+
+	if (chan->direction == DMA_MEM_TO_DEV) {
+		seq_printf(s, "einfo - %d, pswords - %d, priority - %d\n",
+			chan->cfg.u.tx.filt_einfo,
+			chan->cfg.u.tx.filt_pswords,
+			chan->cfg.u.tx.priority);
+	} else {
+		seq_printf(s, "einfo - %d, psinfo - %d, desc_type - %d\n",
+			chan->cfg.u.rx.einfo_present,
+			chan->cfg.u.rx.psinfo_present,
+			chan->cfg.u.rx.desc_type);
+		seq_printf(s, "\t\t\tdst_q: [%d], thresh: %d fdq: ",
+			chan->cfg.u.rx.dst_q,
+			chan->cfg.u.rx.thresh);
+		for (i = 0; i < KNAV_DMA_FDQ_PER_CHAN; i++)
+			seq_printf(s, "[%d]", chan->cfg.u.rx.fdq[i]);
+		seq_printf(s, "\n");
+	}
+}
+
+static void dma_debug_show_devices(struct seq_file *s,
+					struct knav_dma_device *dma)
+{
+	struct knav_dma_chan *chan;
+
+	list_for_each_entry(chan, &dma->chan_list, list) {
+		if (atomic_read(&chan->ref_count))
+			dma_debug_show_channels(s, chan);
+	}
+}
+
+static int dma_debug_show(struct seq_file *s, void *v)
+{
+	struct knav_dma_device *dma;
+
+	list_for_each_entry(dma, &kdev->list, list) {
+		if (atomic_read(&dma->ref_count)) {
+			seq_printf(s, "%s : max_tx_chan: (%d), max_rx_flows: (%d)\n",
+			dma->name, dma->max_tx_chan, dma->max_rx_flow);
+			dma_debug_show_devices(s, dma);
+		}
+	}
+
+	return 0;
+}
+
+static int knav_dma_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, dma_debug_show, NULL);
+}
+
+static const struct file_operations knav_dma_debug_ops = {
+	.open		= knav_dma_debug_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static int of_channel_match_helper(struct device_node *np, const char *name,
+					const char **dma_instance)
+{
+	struct of_phandle_args args;
+	struct device_node *dma_node;
+	int index;
+
+	dma_node = of_parse_phandle(np, "ti,navigator-dmas", 0);
+	if (!dma_node)
+		return -ENODEV;
+
+	*dma_instance = dma_node->name;
+	index = of_property_match_string(np, "ti,navigator-dma-names", name);
+	if (index < 0) {
+		dev_err(kdev->dev, "No 'ti,navigator-dma-names' propery\n");
+		return -ENODEV;
+	}
+
+	if (of_parse_phandle_with_fixed_args(np, "ti,navigator-dmas",
+					1, index, &args)) {
+		dev_err(kdev->dev, "Missing the pahndle args name %s\n", name);
+		return -ENODEV;
+	}
+
+	if (args.args[0] < 0) {
+		dev_err(kdev->dev, "Missing args for %s\n", name);
+		return -ENODEV;
+	}
+
+	return args.args[0];
+}
+
+/**
+ * knav_dma_open_channel() - try to setup an exclusive slave channel
+ * @dev:	pointer to client device structure
+ * @name:	slave channel name
+ * @config:	dma configuration parameters
+ *
+ * Returns pointer to appropriate DMA channel on success or NULL.
+ */
+void *knav_dma_open_channel(struct device *dev, const char *name,
+					struct knav_dma_cfg *config)
+{
+	struct knav_dma_chan *chan;
+	struct knav_dma_device *dma;
+	bool found = false;
+	int chan_num = -1;
+	const char *instance;
+
+	if (!kdev) {
+		pr_err("keystone-navigator-dma driver not registered\n");
+		return (void *)-EINVAL;
+	}
+
+	chan_num = of_channel_match_helper(dev->of_node, name, &instance);
+	if (chan_num < 0) {
+		dev_err(kdev->dev, "No DMA instace with name %s\n", name);
+		return (void *)-EINVAL;
+	}
+
+	dev_dbg(kdev->dev, "initializing %s channel %d from DMA %s\n",
+		  config->direction == DMA_MEM_TO_DEV   ? "transmit" :
+		  config->direction == DMA_DEV_TO_MEM ? "receive"  :
+		  "unknown", chan_num, instance);
+
+	if (config->direction != DMA_MEM_TO_DEV &&
+	    config->direction != DMA_DEV_TO_MEM) {
+		dev_err(kdev->dev, "bad direction\n");
+		return (void *)-EINVAL;
+	}
+
+	/* Look for correct dma instance */
+	list_for_each_entry(dma, &kdev->list, list) {
+		if (!strcmp(dma->name, instance)) {
+			found = true;
+			break;
+		}
+	}
+	if (!found) {
+		dev_err(kdev->dev, "No DMA instace with name %s\n", instance);
+		return (void *)-EINVAL;
+	}
+
+	/* Look for correct dma channel from dma instance */
+	found = false;
+	list_for_each_entry(chan, &dma->chan_list, list) {
+		if (config->direction == DMA_MEM_TO_DEV) {
+			if (chan->channel == chan_num) {
+				found = true;
+				break;
+			}
+		} else {
+			if (chan->flow == chan_num) {
+				found = true;
+				break;
+			}
+		}
+	}
+	if (!found) {
+		dev_err(kdev->dev, "channel %d is not in DMA %s\n",
+				chan_num, instance);
+		return (void *)-EINVAL;
+	}
+
+	if (atomic_read(&chan->ref_count) >= 1) {
+		if (!check_config(chan, config)) {
+			dev_err(kdev->dev, "channel %d config miss-match\n",
+				chan_num);
+			return (void *)-EINVAL;
+		}
+	}
+
+	if (atomic_inc_return(&chan->dma->ref_count) <= 1)
+		knav_dma_hw_init(chan->dma);
+
+	if (atomic_inc_return(&chan->ref_count) <= 1)
+		chan_start(chan, config);
+
+	dev_dbg(kdev->dev, "channel %d opened from DMA %s\n",
+				chan_num, instance);
+
+	return chan;
+}
+EXPORT_SYMBOL_GPL(knav_dma_open_channel);
+
+/**
+ * knav_dma_close_channel()	- Destroy a dma channel
+ *
+ * channel:	dma channel handle
+ *
+ */
+void knav_dma_close_channel(void *channel)
+{
+	struct knav_dma_chan *chan = channel;
+
+	if (!kdev) {
+		pr_err("keystone-navigator-dma driver not registered\n");
+		return;
+	}
+
+	if (atomic_dec_return(&chan->ref_count) <= 0)
+		chan_stop(chan);
+
+	if (atomic_dec_return(&chan->dma->ref_count) <= 0)
+		knav_dma_hw_destroy(chan->dma);
+
+	dev_dbg(kdev->dev, "channel %d or flow %d closed from DMA %s\n",
+			chan->channel, chan->flow, chan->dma->name);
+}
+EXPORT_SYMBOL_GPL(knav_dma_close_channel);
+
+static void __iomem *pktdma_get_regs(struct knav_dma_device *dma,
+				struct device_node *node,
+				unsigned index, resource_size_t *_size)
+{
+	struct device *dev = kdev->dev;
+	struct resource res;
+	void __iomem *regs;
+	int ret;
+
+	ret = of_address_to_resource(node, index, &res);
+	if (ret) {
+		dev_err(dev, "Can't translate of node(%s) address for index(%d)\n",
+			node->name, index);
+		return ERR_PTR(ret);
+	}
+
+	regs = devm_ioremap_resource(kdev->dev, &res);
+	if (IS_ERR(regs))
+		dev_err(dev, "Failed to map register base for index(%d) node(%s)\n",
+			index, node->name);
+	if (_size)
+		*_size = resource_size(&res);
+
+	return regs;
+}
+
+static int pktdma_init_rx_chan(struct knav_dma_chan *chan, u32 flow)
+{
+	struct knav_dma_device *dma = chan->dma;
+
+	chan->flow = flow;
+	chan->reg_rx_flow = dma->reg_rx_flow + flow;
+	chan->channel = DMA_INVALID_ID;
+	dev_dbg(kdev->dev, "rx flow(%d) (%p)\n", chan->flow, chan->reg_rx_flow);
+
+	return 0;
+}
+
+static int pktdma_init_tx_chan(struct knav_dma_chan *chan, u32 channel)
+{
+	struct knav_dma_device *dma = chan->dma;
+
+	chan->channel = channel;
+	chan->reg_chan = dma->reg_tx_chan + channel;
+	chan->reg_tx_sched = dma->reg_tx_sched + channel;
+	chan->flow = DMA_INVALID_ID;
+	dev_dbg(kdev->dev, "tx channel(%d) (%p)\n", chan->channel, chan->reg_chan);
+
+	return 0;
+}
+
+static int pktdma_init_chan(struct knav_dma_device *dma,
+				enum dma_transfer_direction dir,
+				unsigned chan_num)
+{
+	struct device *dev = kdev->dev;
+	struct knav_dma_chan *chan;
+	int ret = -EINVAL;
+
+	chan = devm_kzalloc(dev, sizeof(*chan), GFP_KERNEL);
+	if (!chan)
+		return -ENOMEM;
+
+	INIT_LIST_HEAD(&chan->list);
+	chan->dma	= dma;
+	chan->direction	= DMA_NONE;
+	atomic_set(&chan->ref_count, 0);
+	spin_lock_init(&chan->lock);
+
+	if (dir == DMA_MEM_TO_DEV) {
+		chan->direction = dir;
+		ret = pktdma_init_tx_chan(chan, chan_num);
+	} else if (dir == DMA_DEV_TO_MEM) {
+		chan->direction = dir;
+		ret = pktdma_init_rx_chan(chan, chan_num);
+	} else {
+		dev_err(dev, "channel(%d) direction unknown\n", chan_num);
+	}
+
+	list_add_tail(&chan->list, &dma->chan_list);
+
+	return ret;
+}
+
+static int dma_init(struct device_node *cloud, struct device_node *dma_node)
+{
+	unsigned max_tx_chan, max_rx_chan, max_rx_flow, max_tx_sched;
+	struct device_node *node = dma_node;
+	struct knav_dma_device *dma;
+	int ret, len, num_chan = 0;
+	resource_size_t size;
+	u32 timeout;
+	u32 i;
+
+	dma = devm_kzalloc(kdev->dev, sizeof(*dma), GFP_KERNEL);
+	if (!dma) {
+		dev_err(kdev->dev, "could not allocate driver mem\n");
+		return -ENOMEM;
+	}
+	INIT_LIST_HEAD(&dma->list);
+	INIT_LIST_HEAD(&dma->chan_list);
+
+	if (!of_find_property(cloud, "ti,navigator-cloud-address", &len)) {
+		dev_err(kdev->dev, "unspecified navigator cloud addresses\n");
+		return -ENODEV;
+	}
+
+	dma->logical_queue_managers = len / sizeof(u32);
+	if (dma->logical_queue_managers > DMA_MAX_QMS) {
+		dev_warn(kdev->dev, "too many queue mgrs(>%d) rest ignored\n",
+			 dma->logical_queue_managers);
+		dma->logical_queue_managers = DMA_MAX_QMS;
+	}
+
+	ret = of_property_read_u32_array(cloud, "ti,navigator-cloud-address",
+					dma->qm_base_address,
+					dma->logical_queue_managers);
+	if (ret) {
+		dev_err(kdev->dev, "invalid navigator cloud addresses\n");
+		return -ENODEV;
+	}
+
+	dma->reg_global	 = pktdma_get_regs(dma, node, 0, &size);
+	if (!dma->reg_global)
+		return -ENODEV;
+	if (size < sizeof(struct reg_global)) {
+		dev_err(kdev->dev, "bad size %pa for global regs\n", &size);
+		return -ENODEV;
+	}
+
+	dma->reg_tx_chan = pktdma_get_regs(dma, node, 1, &size);
+	if (!dma->reg_tx_chan)
+		return -ENODEV;
+
+	max_tx_chan = size / sizeof(struct reg_chan);
+	dma->reg_rx_chan = pktdma_get_regs(dma, node, 2, &size);
+	if (!dma->reg_rx_chan)
+		return -ENODEV;
+
+	max_rx_chan = size / sizeof(struct reg_chan);
+	dma->reg_tx_sched = pktdma_get_regs(dma, node, 3, &size);
+	if (!dma->reg_tx_sched)
+		return -ENODEV;
+
+	max_tx_sched = size / sizeof(struct reg_tx_sched);
+	dma->reg_rx_flow = pktdma_get_regs(dma, node, 4, &size);
+	if (!dma->reg_rx_flow)
+		return -ENODEV;
+
+	max_rx_flow = size / sizeof(struct reg_rx_flow);
+	dma->rx_priority = DMA_PRIO_DEFAULT;
+	dma->tx_priority = DMA_PRIO_DEFAULT;
+
+	dma->enable_all	= (of_get_property(node, "ti,enable-all", NULL) != NULL);
+	dma->loopback	= (of_get_property(node, "ti,loop-back",  NULL) != NULL);
+
+	ret = of_property_read_u32(node, "ti,rx-retry-timeout", &timeout);
+	if (ret < 0) {
+		dev_dbg(kdev->dev, "unspecified rx timeout using value %d\n",
+			DMA_RX_TIMEOUT_DEFAULT);
+		timeout = DMA_RX_TIMEOUT_DEFAULT;
+	}
+
+	dma->rx_timeout = timeout;
+	dma->max_rx_chan = max_rx_chan;
+	dma->max_rx_flow = max_rx_flow;
+	dma->max_tx_chan = min(max_tx_chan, max_tx_sched);
+	atomic_set(&dma->ref_count, 0);
+	strcpy(dma->name, node->name);
+	spin_lock_init(&dma->lock);
+
+	for (i = 0; i < dma->max_tx_chan; i++) {
+		if (pktdma_init_chan(dma, DMA_MEM_TO_DEV, i) >= 0)
+			num_chan++;
+	}
+
+	for (i = 0; i < dma->max_rx_flow; i++) {
+		if (pktdma_init_chan(dma, DMA_DEV_TO_MEM, i) >= 0)
+			num_chan++;
+	}
+
+	list_add_tail(&dma->list, &kdev->list);
+
+	/*
+	 * For DSP software usecases or userpace transport software, setup all
+	 * the DMA hardware resources.
+	 */
+	if (dma->enable_all) {
+		atomic_inc(&dma->ref_count);
+		knav_dma_hw_init(dma);
+		dma_hw_enable_all(dma);
+	}
+
+	dev_info(kdev->dev, "DMA %s registered %d logical channels, flows %d, tx chans: %d, rx chans: %d%s\n",
+		dma->name, num_chan, dma->max_rx_flow,
+		dma->max_tx_chan, dma->max_rx_chan,
+		dma->loopback ? ", loopback" : "");
+
+	return 0;
+}
+
+static int knav_dma_probe(struct platform_device *pdev)
+{
+	struct device *dev = &pdev->dev;
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *child;
+	int ret = 0;
+
+	if (!node) {
+		dev_err(&pdev->dev, "could not find device info\n");
+		return -EINVAL;
+	}
+
+	kdev = devm_kzalloc(dev,
+			sizeof(struct knav_dma_pool_device), GFP_KERNEL);
+	if (!kdev) {
+		dev_err(dev, "could not allocate driver mem\n");
+		return -ENOMEM;
+	}
+
+	kdev->dev = dev;
+	INIT_LIST_HEAD(&kdev->list);
+
+	pm_runtime_enable(kdev->dev);
+	ret = pm_runtime_get_sync(kdev->dev);
+	if (ret < 0) {
+		dev_err(kdev->dev, "unable to enable pktdma, err %d\n", ret);
+		return ret;
+	}
+
+	/* Initialise all packet dmas */
+	for_each_child_of_node(node, child) {
+		ret = dma_init(node, child);
+		if (ret) {
+			dev_err(&pdev->dev, "init failed with %d\n", ret);
+			break;
+		}
+	}
+
+	if (list_empty(&kdev->list)) {
+		dev_err(dev, "no valid dma instance\n");
+		return -ENODEV;
+	}
+
+	debugfs_create_file("knav_dma", S_IFREG | S_IRUGO, NULL, NULL,
+			    &knav_dma_debug_ops);
+
+	return ret;
+}
+
+static int knav_dma_remove(struct platform_device *pdev)
+{
+	struct knav_dma_device *dma;
+
+	list_for_each_entry(dma, &kdev->list, list) {
+		if (atomic_dec_return(&dma->ref_count) == 0)
+			knav_dma_hw_destroy(dma);
+	}
+
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+
+	return 0;
+}
+
+static struct of_device_id of_match[] = {
+	{ .compatible = "ti,keystone-navigator-dma", },
+	{},
+};
+
+MODULE_DEVICE_TABLE(of, of_match);
+
+static struct platform_driver knav_dma_driver = {
+	.probe	= knav_dma_probe,
+	.remove	= knav_dma_remove,
+	.driver = {
+		.name		= "keystone-navigator-dma",
+		.owner		= THIS_MODULE,
+		.of_match_table	= of_match,
+	},
+};
+module_platform_driver(knav_dma_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TI Keystone Navigator Packet DMA driver");
+MODULE_AUTHOR("Sandeep Nair <sandeep_n@ti.com>");
+MODULE_AUTHOR("Santosh Shilimkar <santosh.shilimkar@ti.com>");
diff --git a/drivers/soc/ti/knav_qmss.h b/drivers/soc/ti/knav_qmss.h
new file mode 100644
index 000000000000..bc9dcc8cc3ce
--- /dev/null
+++ b/drivers/soc/ti/knav_qmss.h
@@ -0,0 +1,386 @@
+/*
+ * Keystone Navigator QMSS driver internal header
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com
+ * Author:	Sandeep Nair <sandeep_n@ti.com>
+ *		Cyril Chemparathy <cyril@ti.com>
+ *		Santosh Shilimkar <santosh.shilimkar@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+
+#ifndef __KNAV_QMSS_H__
+#define __KNAV_QMSS_H__
+
+#define THRESH_GTE	BIT(7)
+#define THRESH_LT	0
+
+#define PDSP_CTRL_PC_MASK	0xffff0000
+#define PDSP_CTRL_SOFT_RESET	BIT(0)
+#define PDSP_CTRL_ENABLE	BIT(1)
+#define PDSP_CTRL_RUNNING	BIT(15)
+
+#define ACC_MAX_CHANNEL		48
+#define ACC_DEFAULT_PERIOD	25 /* usecs */
+
+#define ACC_CHANNEL_INT_BASE		2
+
+#define ACC_LIST_ENTRY_TYPE		1
+#define ACC_LIST_ENTRY_WORDS		(1 << ACC_LIST_ENTRY_TYPE)
+#define ACC_LIST_ENTRY_QUEUE_IDX	0
+#define ACC_LIST_ENTRY_DESC_IDX	(ACC_LIST_ENTRY_WORDS - 1)
+
+#define ACC_CMD_DISABLE_CHANNEL	0x80
+#define ACC_CMD_ENABLE_CHANNEL	0x81
+#define ACC_CFG_MULTI_QUEUE		BIT(21)
+
+#define ACC_INTD_OFFSET_EOI		(0x0010)
+#define ACC_INTD_OFFSET_COUNT(ch)	(0x0300 + 4 * (ch))
+#define ACC_INTD_OFFSET_STATUS(ch)	(0x0200 + 4 * ((ch) / 32))
+
+#define RANGE_MAX_IRQS			64
+
+#define ACC_DESCS_MAX		SZ_1K
+#define ACC_DESCS_MASK		(ACC_DESCS_MAX - 1)
+#define DESC_SIZE_MASK		0xful
+#define DESC_PTR_MASK		(~DESC_SIZE_MASK)
+
+#define KNAV_NAME_SIZE			32
+
+enum knav_acc_result {
+	ACC_RET_IDLE,
+	ACC_RET_SUCCESS,
+	ACC_RET_INVALID_COMMAND,
+	ACC_RET_INVALID_CHANNEL,
+	ACC_RET_INACTIVE_CHANNEL,
+	ACC_RET_ACTIVE_CHANNEL,
+	ACC_RET_INVALID_QUEUE,
+	ACC_RET_INVALID_RET,
+};
+
+struct knav_reg_config {
+	u32		revision;
+	u32		__pad1;
+	u32		divert;
+	u32		link_ram_base0;
+	u32		link_ram_size0;
+	u32		link_ram_base1;
+	u32		__pad2[2];
+	u32		starvation[0];
+};
+
+struct knav_reg_region {
+	u32		base;
+	u32		start_index;
+	u32		size_count;
+	u32		__pad;
+};
+
+struct knav_reg_pdsp_regs {
+	u32		control;
+	u32		status;
+	u32		cycle_count;
+	u32		stall_count;
+};
+
+struct knav_reg_acc_command {
+	u32		command;
+	u32		queue_mask;
+	u32		list_phys;
+	u32		queue_num;
+	u32		timer_config;
+};
+
+struct knav_link_ram_block {
+	dma_addr_t	 phys;
+	void		*virt;
+	size_t		 size;
+};
+
+struct knav_acc_info {
+	u32			 pdsp_id;
+	u32			 start_channel;
+	u32			 list_entries;
+	u32			 pacing_mode;
+	u32			 timer_count;
+	int			 mem_size;
+	int			 list_size;
+	struct knav_pdsp_info	*pdsp;
+};
+
+struct knav_acc_channel {
+	u32			channel;
+	u32			list_index;
+	u32			open_mask;
+	u32			*list_cpu[2];
+	dma_addr_t		list_dma[2];
+	char			name[KNAV_NAME_SIZE];
+	atomic_t		retrigger_count;
+};
+
+struct knav_pdsp_info {
+	const char					*name;
+	struct knav_reg_pdsp_regs  __iomem		*regs;
+	union {
+		void __iomem				*command;
+		struct knav_reg_acc_command __iomem	*acc_command;
+		u32 __iomem				*qos_command;
+	};
+	void __iomem					*intd;
+	u32 __iomem					*iram;
+	const char					*firmware;
+	u32						id;
+	struct list_head				list;
+};
+
+struct knav_qmgr_info {
+	unsigned			start_queue;
+	unsigned			num_queues;
+	struct knav_reg_config __iomem	*reg_config;
+	struct knav_reg_region __iomem	*reg_region;
+	struct knav_reg_queue __iomem	*reg_push, *reg_pop, *reg_peek;
+	void __iomem			*reg_status;
+	struct list_head		list;
+};
+
+#define KNAV_NUM_LINKRAM	2
+
+/**
+ * struct knav_queue_stats:	queue statistics
+ * pushes:			number of push operations
+ * pops:			number of pop operations
+ * push_errors:			number of push errors
+ * pop_errors:			number of pop errors
+ * notifies:			notifier counts
+ */
+struct knav_queue_stats {
+	atomic_t	 pushes;
+	atomic_t	 pops;
+	atomic_t	 push_errors;
+	atomic_t	 pop_errors;
+	atomic_t	 notifies;
+};
+
+/**
+ * struct knav_reg_queue:	queue registers
+ * @entry_count:		valid entries in the queue
+ * @byte_count:			total byte count in thhe queue
+ * @packet_size:		packet size for the queue
+ * @ptr_size_thresh:		packet pointer size threshold
+ */
+struct knav_reg_queue {
+	u32		entry_count;
+	u32		byte_count;
+	u32		packet_size;
+	u32		ptr_size_thresh;
+};
+
+/**
+ * struct knav_region:		qmss region info
+ * @dma_start, dma_end:		start and end dma address
+ * @virt_start, virt_end:	start and end virtual address
+ * @desc_size:			descriptor size
+ * @used_desc:			consumed descriptors
+ * @id:				region number
+ * @num_desc:			total descriptors
+ * @link_index:			index of the first descriptor
+ * @name:			region name
+ * @list:			instance in the device's region list
+ * @pools:			list of descriptor pools in the region
+ */
+struct knav_region {
+	dma_addr_t		dma_start, dma_end;
+	void			*virt_start, *virt_end;
+	unsigned		desc_size;
+	unsigned		used_desc;
+	unsigned		id;
+	unsigned		num_desc;
+	unsigned		link_index;
+	const char		*name;
+	struct list_head	list;
+	struct list_head	pools;
+};
+
+/**
+ * struct knav_pool:		qmss pools
+ * @dev:			device pointer
+ * @region:			qmss region info
+ * @queue:			queue registers
+ * @kdev:			qmss device pointer
+ * @region_offset:		offset from the base
+ * @num_desc:			total descriptors
+ * @desc_size:			descriptor size
+ * @region_id:			region number
+ * @name:			pool name
+ * @list:			list head
+ * @region_inst:		instance in the region's pool list
+ */
+struct knav_pool {
+	struct device			*dev;
+	struct knav_region		*region;
+	struct knav_queue		*queue;
+	struct knav_device		*kdev;
+	int				region_offset;
+	int				num_desc;
+	int				desc_size;
+	int				region_id;
+	const char			*name;
+	struct list_head		list;
+	struct list_head		region_inst;
+};
+
+/**
+ * struct knav_queue_inst:		qmss queue instace properties
+ * @descs:				descriptor pointer
+ * @desc_head, desc_tail, desc_count:	descriptor counters
+ * @acc:				accumulator channel pointer
+ * @kdev:				qmss device pointer
+ * @range:				range info
+ * @qmgr:				queue manager info
+ * @id:					queue instace id
+ * @irq_num:				irq line number
+ * @notify_needed:			notifier needed based on queue type
+ * @num_notifiers:			total notifiers
+ * @handles:				list head
+ * @name:				queue instance name
+ * @irq_name:				irq line name
+ */
+struct knav_queue_inst {
+	u32				*descs;
+	atomic_t			desc_head, desc_tail, desc_count;
+	struct knav_acc_channel	*acc;
+	struct knav_device		*kdev;
+	struct knav_range_info		*range;
+	struct knav_qmgr_info		*qmgr;
+	u32				id;
+	int				irq_num;
+	int				notify_needed;
+	atomic_t			num_notifiers;
+	struct list_head		handles;
+	const char			*name;
+	const char			*irq_name;
+};
+
+/**
+ * struct knav_queue:			qmss queue properties
+ * @reg_push, reg_pop, reg_peek:	push, pop queue registers
+ * @inst:				qmss queue instace properties
+ * @notifier_fn:			notifier function
+ * @notifier_fn_arg:			notifier function argument
+ * @notifier_enabled:			notier enabled for a give queue
+ * @rcu:				rcu head
+ * @flags:				queue flags
+ * @list:				list head
+ */
+struct knav_queue {
+	struct knav_reg_queue __iomem	*reg_push, *reg_pop, *reg_peek;
+	struct knav_queue_inst		*inst;
+	struct knav_queue_stats	stats;
+	knav_queue_notify_fn		notifier_fn;
+	void				*notifier_fn_arg;
+	atomic_t			notifier_enabled;
+	struct rcu_head			rcu;
+	unsigned			flags;
+	struct list_head		list;
+};
+
+struct knav_device {
+	struct device				*dev;
+	unsigned				base_id;
+	unsigned				num_queues;
+	unsigned				num_queues_in_use;
+	unsigned				inst_shift;
+	struct knav_link_ram_block		link_rams[KNAV_NUM_LINKRAM];
+	void					*instances;
+	struct list_head			regions;
+	struct list_head			queue_ranges;
+	struct list_head			pools;
+	struct list_head			pdsps;
+	struct list_head			qmgrs;
+};
+
+struct knav_range_ops {
+	int	(*init_range)(struct knav_range_info *range);
+	int	(*free_range)(struct knav_range_info *range);
+	int	(*init_queue)(struct knav_range_info *range,
+			      struct knav_queue_inst *inst);
+	int	(*open_queue)(struct knav_range_info *range,
+			      struct knav_queue_inst *inst, unsigned flags);
+	int	(*close_queue)(struct knav_range_info *range,
+			       struct knav_queue_inst *inst);
+	int	(*set_notify)(struct knav_range_info *range,
+			      struct knav_queue_inst *inst, bool enabled);
+};
+
+struct knav_irq_info {
+	int	irq;
+	u32	cpu_map;
+};
+
+struct knav_range_info {
+	const char			*name;
+	struct knav_device		*kdev;
+	unsigned			queue_base;
+	unsigned			num_queues;
+	void				*queue_base_inst;
+	unsigned			flags;
+	struct list_head		list;
+	struct knav_range_ops		*ops;
+	struct knav_acc_info		acc_info;
+	struct knav_acc_channel	*acc;
+	unsigned			num_irqs;
+	struct knav_irq_info		irqs[RANGE_MAX_IRQS];
+};
+
+#define RANGE_RESERVED		BIT(0)
+#define RANGE_HAS_IRQ		BIT(1)
+#define RANGE_HAS_ACCUMULATOR	BIT(2)
+#define RANGE_MULTI_QUEUE	BIT(3)
+
+#define for_each_region(kdev, region)				\
+	list_for_each_entry(region, &kdev->regions, list)
+
+#define first_region(kdev)					\
+	list_first_entry(&kdev->regions, \
+			struct knav_region, list)
+
+#define for_each_queue_range(kdev, range)			\
+	list_for_each_entry(range, &kdev->queue_ranges, list)
+
+#define first_queue_range(kdev)					\
+	list_first_entry(&kdev->queue_ranges, \
+			struct knav_range_info, list)
+
+#define for_each_pool(kdev, pool)				\
+	list_for_each_entry(pool, &kdev->pools, list)
+
+#define for_each_pdsp(kdev, pdsp)				\
+	list_for_each_entry(pdsp, &kdev->pdsps, list)
+
+#define for_each_qmgr(kdev, qmgr)				\
+	list_for_each_entry(qmgr, &kdev->qmgrs, list)
+
+static inline struct knav_pdsp_info *
+knav_find_pdsp(struct knav_device *kdev, unsigned pdsp_id)
+{
+	struct knav_pdsp_info *pdsp;
+
+	for_each_pdsp(kdev, pdsp)
+		if (pdsp_id == pdsp->id)
+			return pdsp;
+	return NULL;
+}
+
+extern int knav_init_acc_range(struct knav_device *kdev,
+					struct device_node *node,
+					struct knav_range_info *range);
+extern void knav_queue_notify(struct knav_queue_inst *inst);
+
+#endif /* __KNAV_QMSS_H__ */
diff --git a/drivers/soc/ti/knav_qmss_acc.c b/drivers/soc/ti/knav_qmss_acc.c
new file mode 100644
index 000000000000..6fbfde6e748f
--- /dev/null
+++ b/drivers/soc/ti/knav_qmss_acc.c
@@ -0,0 +1,591 @@
+/*
+ * Keystone accumulator queue manager
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com
+ * Author:	Sandeep Nair <sandeep_n@ti.com>
+ *		Cyril Chemparathy <cyril@ti.com>
+ *		Santosh Shilimkar <santosh.shilimkar@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/soc/ti/knav_qmss.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/firmware.h>
+
+#include "knav_qmss.h"
+
+#define knav_range_offset_to_inst(kdev, range, q)	\
+	(range->queue_base_inst + (q << kdev->inst_shift))
+
+static void __knav_acc_notify(struct knav_range_info *range,
+				struct knav_acc_channel *acc)
+{
+	struct knav_device *kdev = range->kdev;
+	struct knav_queue_inst *inst;
+	int range_base, queue;
+
+	range_base = kdev->base_id + range->queue_base;
+
+	if (range->flags & RANGE_MULTI_QUEUE) {
+		for (queue = 0; queue < range->num_queues; queue++) {
+			inst = knav_range_offset_to_inst(kdev, range,
+								queue);
+			if (inst->notify_needed) {
+				inst->notify_needed = 0;
+				dev_dbg(kdev->dev, "acc-irq: notifying %d\n",
+					range_base + queue);
+				knav_queue_notify(inst);
+			}
+		}
+	} else {
+		queue = acc->channel - range->acc_info.start_channel;
+		inst = knav_range_offset_to_inst(kdev, range, queue);
+		dev_dbg(kdev->dev, "acc-irq: notifying %d\n",
+			range_base + queue);
+		knav_queue_notify(inst);
+	}
+}
+
+static int knav_acc_set_notify(struct knav_range_info *range,
+				struct knav_queue_inst *kq,
+				bool enabled)
+{
+	struct knav_pdsp_info *pdsp = range->acc_info.pdsp;
+	struct knav_device *kdev = range->kdev;
+	u32 mask, offset;
+
+	/*
+	 * when enabling, we need to re-trigger an interrupt if we
+	 * have descriptors pending
+	 */
+	if (!enabled || atomic_read(&kq->desc_count) <= 0)
+		return 0;
+
+	kq->notify_needed = 1;
+	atomic_inc(&kq->acc->retrigger_count);
+	mask = BIT(kq->acc->channel % 32);
+	offset = ACC_INTD_OFFSET_STATUS(kq->acc->channel);
+	dev_dbg(kdev->dev, "setup-notify: re-triggering irq for %s\n",
+		kq->acc->name);
+	writel_relaxed(mask, pdsp->intd + offset);
+	return 0;
+}
+
+static irqreturn_t knav_acc_int_handler(int irq, void *_instdata)
+{
+	struct knav_acc_channel *acc;
+	struct knav_queue_inst *kq = NULL;
+	struct knav_range_info *range;
+	struct knav_pdsp_info *pdsp;
+	struct knav_acc_info *info;
+	struct knav_device *kdev;
+
+	u32 *list, *list_cpu, val, idx, notifies;
+	int range_base, channel, queue = 0;
+	dma_addr_t list_dma;
+
+	range = _instdata;
+	info  = &range->acc_info;
+	kdev  = range->kdev;
+	pdsp  = range->acc_info.pdsp;
+	acc   = range->acc;
+
+	range_base = kdev->base_id + range->queue_base;
+	if ((range->flags & RANGE_MULTI_QUEUE) == 0) {
+		for (queue = 0; queue < range->num_irqs; queue++)
+			if (range->irqs[queue].irq == irq)
+				break;
+		kq = knav_range_offset_to_inst(kdev, range, queue);
+		acc += queue;
+	}
+
+	channel = acc->channel;
+	list_dma = acc->list_dma[acc->list_index];
+	list_cpu = acc->list_cpu[acc->list_index];
+	dev_dbg(kdev->dev, "acc-irq: channel %d, list %d, virt %p, phys %x\n",
+		channel, acc->list_index, list_cpu, list_dma);
+	if (atomic_read(&acc->retrigger_count)) {
+		atomic_dec(&acc->retrigger_count);
+		__knav_acc_notify(range, acc);
+		writel_relaxed(1, pdsp->intd + ACC_INTD_OFFSET_COUNT(channel));
+		/* ack the interrupt */
+		writel_relaxed(ACC_CHANNEL_INT_BASE + channel,
+			       pdsp->intd + ACC_INTD_OFFSET_EOI);
+
+		return IRQ_HANDLED;
+	}
+
+	notifies = readl_relaxed(pdsp->intd + ACC_INTD_OFFSET_COUNT(channel));
+	WARN_ON(!notifies);
+	dma_sync_single_for_cpu(kdev->dev, list_dma, info->list_size,
+				DMA_FROM_DEVICE);
+
+	for (list = list_cpu; list < list_cpu + (info->list_size / sizeof(u32));
+	     list += ACC_LIST_ENTRY_WORDS) {
+		if (ACC_LIST_ENTRY_WORDS == 1) {
+			dev_dbg(kdev->dev,
+				"acc-irq: list %d, entry @%p, %08x\n",
+				acc->list_index, list, list[0]);
+		} else if (ACC_LIST_ENTRY_WORDS == 2) {
+			dev_dbg(kdev->dev,
+				"acc-irq: list %d, entry @%p, %08x %08x\n",
+				acc->list_index, list, list[0], list[1]);
+		} else if (ACC_LIST_ENTRY_WORDS == 4) {
+			dev_dbg(kdev->dev,
+				"acc-irq: list %d, entry @%p, %08x %08x %08x %08x\n",
+				acc->list_index, list, list[0], list[1],
+				list[2], list[3]);
+		}
+
+		val = list[ACC_LIST_ENTRY_DESC_IDX];
+		if (!val)
+			break;
+
+		if (range->flags & RANGE_MULTI_QUEUE) {
+			queue = list[ACC_LIST_ENTRY_QUEUE_IDX] >> 16;
+			if (queue < range_base ||
+			    queue >= range_base + range->num_queues) {
+				dev_err(kdev->dev,
+					"bad queue %d, expecting %d-%d\n",
+					queue, range_base,
+					range_base + range->num_queues);
+				break;
+			}
+			queue -= range_base;
+			kq = knav_range_offset_to_inst(kdev, range,
+								queue);
+		}
+
+		if (atomic_inc_return(&kq->desc_count) >= ACC_DESCS_MAX) {
+			atomic_dec(&kq->desc_count);
+			dev_err(kdev->dev,
+				"acc-irq: queue %d full, entry dropped\n",
+				queue + range_base);
+			continue;
+		}
+
+		idx = atomic_inc_return(&kq->desc_tail) & ACC_DESCS_MASK;
+		kq->descs[idx] = val;
+		kq->notify_needed = 1;
+		dev_dbg(kdev->dev, "acc-irq: enqueue %08x at %d, queue %d\n",
+			val, idx, queue + range_base);
+	}
+
+	__knav_acc_notify(range, acc);
+	memset(list_cpu, 0, info->list_size);
+	dma_sync_single_for_device(kdev->dev, list_dma, info->list_size,
+				   DMA_TO_DEVICE);
+
+	/* flip to the other list */
+	acc->list_index ^= 1;
+
+	/* reset the interrupt counter */
+	writel_relaxed(1, pdsp->intd + ACC_INTD_OFFSET_COUNT(channel));
+
+	/* ack the interrupt */
+	writel_relaxed(ACC_CHANNEL_INT_BASE + channel,
+		       pdsp->intd + ACC_INTD_OFFSET_EOI);
+
+	return IRQ_HANDLED;
+}
+
+int knav_range_setup_acc_irq(struct knav_range_info *range,
+				int queue, bool enabled)
+{
+	struct knav_device *kdev = range->kdev;
+	struct knav_acc_channel *acc;
+	unsigned long cpu_map;
+	int ret = 0, irq;
+	u32 old, new;
+
+	if (range->flags & RANGE_MULTI_QUEUE) {
+		acc = range->acc;
+		irq = range->irqs[0].irq;
+		cpu_map = range->irqs[0].cpu_map;
+	} else {
+		acc = range->acc + queue;
+		irq = range->irqs[queue].irq;
+		cpu_map = range->irqs[queue].cpu_map;
+	}
+
+	old = acc->open_mask;
+	if (enabled)
+		new = old | BIT(queue);
+	else
+		new = old & ~BIT(queue);
+	acc->open_mask = new;
+
+	dev_dbg(kdev->dev,
+		"setup-acc-irq: open mask old %08x, new %08x, channel %s\n",
+		old, new, acc->name);
+
+	if (likely(new == old))
+		return 0;
+
+	if (new && !old) {
+		dev_dbg(kdev->dev,
+			"setup-acc-irq: requesting %s for channel %s\n",
+			acc->name, acc->name);
+		ret = request_irq(irq, knav_acc_int_handler, 0, acc->name,
+				  range);
+		if (!ret && cpu_map) {
+			ret = irq_set_affinity_hint(irq, to_cpumask(&cpu_map));
+			if (ret) {
+				dev_warn(range->kdev->dev,
+					 "Failed to set IRQ affinity\n");
+				return ret;
+			}
+		}
+	}
+
+	if (old && !new) {
+		dev_dbg(kdev->dev, "setup-acc-irq: freeing %s for channel %s\n",
+			acc->name, acc->name);
+		free_irq(irq, range);
+	}
+
+	return ret;
+}
+
+static const char *knav_acc_result_str(enum knav_acc_result result)
+{
+	static const char * const result_str[] = {
+		[ACC_RET_IDLE]			= "idle",
+		[ACC_RET_SUCCESS]		= "success",
+		[ACC_RET_INVALID_COMMAND]	= "invalid command",
+		[ACC_RET_INVALID_CHANNEL]	= "invalid channel",
+		[ACC_RET_INACTIVE_CHANNEL]	= "inactive channel",
+		[ACC_RET_ACTIVE_CHANNEL]	= "active channel",
+		[ACC_RET_INVALID_QUEUE]		= "invalid queue",
+		[ACC_RET_INVALID_RET]		= "invalid return code",
+	};
+
+	if (result >= ARRAY_SIZE(result_str))
+		return result_str[ACC_RET_INVALID_RET];
+	else
+		return result_str[result];
+}
+
+static enum knav_acc_result
+knav_acc_write(struct knav_device *kdev, struct knav_pdsp_info *pdsp,
+		struct knav_reg_acc_command *cmd)
+{
+	u32 result;
+
+	dev_dbg(kdev->dev, "acc command %08x %08x %08x %08x %08x\n",
+		cmd->command, cmd->queue_mask, cmd->list_phys,
+		cmd->queue_num, cmd->timer_config);
+
+	writel_relaxed(cmd->timer_config, &pdsp->acc_command->timer_config);
+	writel_relaxed(cmd->queue_num, &pdsp->acc_command->queue_num);
+	writel_relaxed(cmd->list_phys, &pdsp->acc_command->list_phys);
+	writel_relaxed(cmd->queue_mask, &pdsp->acc_command->queue_mask);
+	writel_relaxed(cmd->command, &pdsp->acc_command->command);
+
+	/* wait for the command to clear */
+	do {
+		result = readl_relaxed(&pdsp->acc_command->command);
+	} while ((result >> 8) & 0xff);
+
+	return (result >> 24) & 0xff;
+}
+
+static void knav_acc_setup_cmd(struct knav_device *kdev,
+				struct knav_range_info *range,
+				struct knav_reg_acc_command *cmd,
+				int queue)
+{
+	struct knav_acc_info *info = &range->acc_info;
+	struct knav_acc_channel *acc;
+	int queue_base;
+	u32 queue_mask;
+
+	if (range->flags & RANGE_MULTI_QUEUE) {
+		acc = range->acc;
+		queue_base = range->queue_base;
+		queue_mask = BIT(range->num_queues) - 1;
+	} else {
+		acc = range->acc + queue;
+		queue_base = range->queue_base + queue;
+		queue_mask = 0;
+	}
+
+	memset(cmd, 0, sizeof(*cmd));
+	cmd->command    = acc->channel;
+	cmd->queue_mask = queue_mask;
+	cmd->list_phys  = acc->list_dma[0];
+	cmd->queue_num  = info->list_entries << 16;
+	cmd->queue_num |= queue_base;
+
+	cmd->timer_config = ACC_LIST_ENTRY_TYPE << 18;
+	if (range->flags & RANGE_MULTI_QUEUE)
+		cmd->timer_config |= ACC_CFG_MULTI_QUEUE;
+	cmd->timer_config |= info->pacing_mode << 16;
+	cmd->timer_config |= info->timer_count;
+}
+
+static void knav_acc_stop(struct knav_device *kdev,
+				struct knav_range_info *range,
+				int queue)
+{
+	struct knav_reg_acc_command cmd;
+	struct knav_acc_channel *acc;
+	enum knav_acc_result result;
+
+	acc = range->acc + queue;
+
+	knav_acc_setup_cmd(kdev, range, &cmd, queue);
+	cmd.command |= ACC_CMD_DISABLE_CHANNEL << 8;
+	result = knav_acc_write(kdev, range->acc_info.pdsp, &cmd);
+
+	dev_dbg(kdev->dev, "stopped acc channel %s, result %s\n",
+		acc->name, knav_acc_result_str(result));
+}
+
+static enum knav_acc_result knav_acc_start(struct knav_device *kdev,
+						struct knav_range_info *range,
+						int queue)
+{
+	struct knav_reg_acc_command cmd;
+	struct knav_acc_channel *acc;
+	enum knav_acc_result result;
+
+	acc = range->acc + queue;
+
+	knav_acc_setup_cmd(kdev, range, &cmd, queue);
+	cmd.command |= ACC_CMD_ENABLE_CHANNEL << 8;
+	result = knav_acc_write(kdev, range->acc_info.pdsp, &cmd);
+
+	dev_dbg(kdev->dev, "started acc channel %s, result %s\n",
+		acc->name, knav_acc_result_str(result));
+
+	return result;
+}
+
+static int knav_acc_init_range(struct knav_range_info *range)
+{
+	struct knav_device *kdev = range->kdev;
+	struct knav_acc_channel *acc;
+	enum knav_acc_result result;
+	int queue;
+
+	for (queue = 0; queue < range->num_queues; queue++) {
+		acc = range->acc + queue;
+
+		knav_acc_stop(kdev, range, queue);
+		acc->list_index = 0;
+		result = knav_acc_start(kdev, range, queue);
+
+		if (result != ACC_RET_SUCCESS)
+			return -EIO;
+
+		if (range->flags & RANGE_MULTI_QUEUE)
+			return 0;
+	}
+	return 0;
+}
+
+static int knav_acc_init_queue(struct knav_range_info *range,
+				struct knav_queue_inst *kq)
+{
+	unsigned id = kq->id - range->queue_base;
+
+	kq->descs = devm_kzalloc(range->kdev->dev,
+				 ACC_DESCS_MAX * sizeof(u32), GFP_KERNEL);
+	if (!kq->descs)
+		return -ENOMEM;
+
+	kq->acc = range->acc;
+	if ((range->flags & RANGE_MULTI_QUEUE) == 0)
+		kq->acc += id;
+	return 0;
+}
+
+static int knav_acc_open_queue(struct knav_range_info *range,
+				struct knav_queue_inst *inst, unsigned flags)
+{
+	unsigned id = inst->id - range->queue_base;
+
+	return knav_range_setup_acc_irq(range, id, true);
+}
+
+static int knav_acc_close_queue(struct knav_range_info *range,
+					struct knav_queue_inst *inst)
+{
+	unsigned id = inst->id - range->queue_base;
+
+	return knav_range_setup_acc_irq(range, id, false);
+}
+
+static int knav_acc_free_range(struct knav_range_info *range)
+{
+	struct knav_device *kdev = range->kdev;
+	struct knav_acc_channel *acc;
+	struct knav_acc_info *info;
+	int channel, channels;
+
+	info = &range->acc_info;
+
+	if (range->flags & RANGE_MULTI_QUEUE)
+		channels = 1;
+	else
+		channels = range->num_queues;
+
+	for (channel = 0; channel < channels; channel++) {
+		acc = range->acc + channel;
+		if (!acc->list_cpu[0])
+			continue;
+		dma_unmap_single(kdev->dev, acc->list_dma[0],
+				 info->mem_size, DMA_BIDIRECTIONAL);
+		free_pages_exact(acc->list_cpu[0], info->mem_size);
+	}
+	devm_kfree(range->kdev->dev, range->acc);
+	return 0;
+}
+
+struct knav_range_ops knav_acc_range_ops = {
+	.set_notify	= knav_acc_set_notify,
+	.init_queue	= knav_acc_init_queue,
+	.open_queue	= knav_acc_open_queue,
+	.close_queue	= knav_acc_close_queue,
+	.init_range	= knav_acc_init_range,
+	.free_range	= knav_acc_free_range,
+};
+
+/**
+ * knav_init_acc_range: Initialise accumulator ranges
+ *
+ * @kdev:		qmss device
+ * @node:		device node
+ * @range:		qmms range information
+ *
+ * Return 0 on success or error
+ */
+int knav_init_acc_range(struct knav_device *kdev,
+				struct device_node *node,
+				struct knav_range_info *range)
+{
+	struct knav_acc_channel *acc;
+	struct knav_pdsp_info *pdsp;
+	struct knav_acc_info *info;
+	int ret, channel, channels;
+	int list_size, mem_size;
+	dma_addr_t list_dma;
+	void *list_mem;
+	u32 config[5];
+
+	range->flags |= RANGE_HAS_ACCUMULATOR;
+	info = &range->acc_info;
+
+	ret = of_property_read_u32_array(node, "accumulator", config, 5);
+	if (ret)
+		return ret;
+
+	info->pdsp_id		= config[0];
+	info->start_channel	= config[1];
+	info->list_entries	= config[2];
+	info->pacing_mode	= config[3];
+	info->timer_count	= config[4] / ACC_DEFAULT_PERIOD;
+
+	if (info->start_channel > ACC_MAX_CHANNEL) {
+		dev_err(kdev->dev, "channel %d invalid for range %s\n",
+			info->start_channel, range->name);
+		return -EINVAL;
+	}
+
+	if (info->pacing_mode > 3) {
+		dev_err(kdev->dev, "pacing mode %d invalid for range %s\n",
+			info->pacing_mode, range->name);
+		return -EINVAL;
+	}
+
+	pdsp = knav_find_pdsp(kdev, info->pdsp_id);
+	if (!pdsp) {
+		dev_err(kdev->dev, "pdsp id %d not found for range %s\n",
+			info->pdsp_id, range->name);
+		return -EINVAL;
+	}
+
+	info->pdsp = pdsp;
+	channels = range->num_queues;
+	if (of_get_property(node, "multi-queue", NULL)) {
+		range->flags |= RANGE_MULTI_QUEUE;
+		channels = 1;
+		if (range->queue_base & (32 - 1)) {
+			dev_err(kdev->dev,
+				"misaligned multi-queue accumulator range %s\n",
+				range->name);
+			return -EINVAL;
+		}
+		if (range->num_queues > 32) {
+			dev_err(kdev->dev,
+				"too many queues in accumulator range %s\n",
+				range->name);
+			return -EINVAL;
+		}
+	}
+
+	/* figure out list size */
+	list_size  = info->list_entries;
+	list_size *= ACC_LIST_ENTRY_WORDS * sizeof(u32);
+	info->list_size = list_size;
+	mem_size   = PAGE_ALIGN(list_size * 2);
+	info->mem_size  = mem_size;
+	range->acc = devm_kzalloc(kdev->dev, channels * sizeof(*range->acc),
+				  GFP_KERNEL);
+	if (!range->acc)
+		return -ENOMEM;
+
+	for (channel = 0; channel < channels; channel++) {
+		acc = range->acc + channel;
+		acc->channel = info->start_channel + channel;
+
+		/* allocate memory for the two lists */
+		list_mem = alloc_pages_exact(mem_size, GFP_KERNEL | GFP_DMA);
+		if (!list_mem)
+			return -ENOMEM;
+
+		list_dma = dma_map_single(kdev->dev, list_mem, mem_size,
+					  DMA_BIDIRECTIONAL);
+		if (dma_mapping_error(kdev->dev, list_dma)) {
+			free_pages_exact(list_mem, mem_size);
+			return -ENOMEM;
+		}
+
+		memset(list_mem, 0, mem_size);
+		dma_sync_single_for_device(kdev->dev, list_dma, mem_size,
+					   DMA_TO_DEVICE);
+		scnprintf(acc->name, sizeof(acc->name), "hwqueue-acc-%d",
+			  acc->channel);
+		acc->list_cpu[0] = list_mem;
+		acc->list_cpu[1] = list_mem + list_size;
+		acc->list_dma[0] = list_dma;
+		acc->list_dma[1] = list_dma + list_size;
+		dev_dbg(kdev->dev, "%s: channel %d, phys %08x, virt %8p\n",
+			acc->name, acc->channel, list_dma, list_mem);
+	}
+
+	range->ops = &knav_acc_range_ops;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(knav_init_acc_range);
diff --git a/drivers/soc/ti/knav_qmss_queue.c b/drivers/soc/ti/knav_qmss_queue.c
new file mode 100644
index 000000000000..0a2c8634c48b
--- /dev/null
+++ b/drivers/soc/ti/knav_qmss_queue.c
@@ -0,0 +1,1816 @@
+/*
+ * Keystone Queue Manager subsystem driver
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com
+ * Authors:	Sandeep Nair <sandeep_n@ti.com>
+ *		Cyril Chemparathy <cyril@ti.com>
+ *		Santosh Shilimkar <santosh.shilimkar@ti.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/bitops.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/of_device.h>
+#include <linux/of_address.h>
+#include <linux/pm_runtime.h>
+#include <linux/firmware.h>
+#include <linux/debugfs.h>
+#include <linux/seq_file.h>
+#include <linux/string.h>
+#include <linux/soc/ti/knav_qmss.h>
+
+#include "knav_qmss.h"
+
+static struct knav_device *kdev;
+static DEFINE_MUTEX(knav_dev_lock);
+
+/* Queue manager register indices in DTS */
+#define KNAV_QUEUE_PEEK_REG_INDEX	0
+#define KNAV_QUEUE_STATUS_REG_INDEX	1
+#define KNAV_QUEUE_CONFIG_REG_INDEX	2
+#define KNAV_QUEUE_REGION_REG_INDEX	3
+#define KNAV_QUEUE_PUSH_REG_INDEX	4
+#define KNAV_QUEUE_POP_REG_INDEX	5
+
+/* PDSP register indices in DTS */
+#define KNAV_QUEUE_PDSP_IRAM_REG_INDEX	0
+#define KNAV_QUEUE_PDSP_REGS_REG_INDEX	1
+#define KNAV_QUEUE_PDSP_INTD_REG_INDEX	2
+#define KNAV_QUEUE_PDSP_CMD_REG_INDEX	3
+
+#define knav_queue_idx_to_inst(kdev, idx)			\
+	(kdev->instances + (idx << kdev->inst_shift))
+
+#define for_each_handle_rcu(qh, inst)			\
+	list_for_each_entry_rcu(qh, &inst->handles, list)
+
+#define for_each_instance(idx, inst, kdev)		\
+	for (idx = 0, inst = kdev->instances;		\
+	     idx < (kdev)->num_queues_in_use;			\
+	     idx++, inst = knav_queue_idx_to_inst(kdev, idx))
+
+/**
+ * knav_queue_notify: qmss queue notfier call
+ *
+ * @inst:		qmss queue instance like accumulator
+ */
+void knav_queue_notify(struct knav_queue_inst *inst)
+{
+	struct knav_queue *qh;
+
+	if (!inst)
+		return;
+
+	rcu_read_lock();
+	for_each_handle_rcu(qh, inst) {
+		if (atomic_read(&qh->notifier_enabled) <= 0)
+			continue;
+		if (WARN_ON(!qh->notifier_fn))
+			continue;
+		atomic_inc(&qh->stats.notifies);
+		qh->notifier_fn(qh->notifier_fn_arg);
+	}
+	rcu_read_unlock();
+}
+EXPORT_SYMBOL_GPL(knav_queue_notify);
+
+static irqreturn_t knav_queue_int_handler(int irq, void *_instdata)
+{
+	struct knav_queue_inst *inst = _instdata;
+
+	knav_queue_notify(inst);
+	return IRQ_HANDLED;
+}
+
+static int knav_queue_setup_irq(struct knav_range_info *range,
+			  struct knav_queue_inst *inst)
+{
+	unsigned queue = inst->id - range->queue_base;
+	unsigned long cpu_map;
+	int ret = 0, irq;
+
+	if (range->flags & RANGE_HAS_IRQ) {
+		irq = range->irqs[queue].irq;
+		cpu_map = range->irqs[queue].cpu_map;
+		ret = request_irq(irq, knav_queue_int_handler, 0,
+					inst->irq_name, inst);
+		if (ret)
+			return ret;
+		disable_irq(irq);
+		if (cpu_map) {
+			ret = irq_set_affinity_hint(irq, to_cpumask(&cpu_map));
+			if (ret) {
+				dev_warn(range->kdev->dev,
+					 "Failed to set IRQ affinity\n");
+				return ret;
+			}
+		}
+	}
+	return ret;
+}
+
+static void knav_queue_free_irq(struct knav_queue_inst *inst)
+{
+	struct knav_range_info *range = inst->range;
+	unsigned queue = inst->id - inst->range->queue_base;
+	int irq;
+
+	if (range->flags & RANGE_HAS_IRQ) {
+		irq = range->irqs[queue].irq;
+		irq_set_affinity_hint(irq, NULL);
+		free_irq(irq, inst);
+	}
+}
+
+static inline bool knav_queue_is_busy(struct knav_queue_inst *inst)
+{
+	return !list_empty(&inst->handles);
+}
+
+static inline bool knav_queue_is_reserved(struct knav_queue_inst *inst)
+{
+	return inst->range->flags & RANGE_RESERVED;
+}
+
+static inline bool knav_queue_is_shared(struct knav_queue_inst *inst)
+{
+	struct knav_queue *tmp;
+
+	rcu_read_lock();
+	for_each_handle_rcu(tmp, inst) {
+		if (tmp->flags & KNAV_QUEUE_SHARED) {
+			rcu_read_unlock();
+			return true;
+		}
+	}
+	rcu_read_unlock();
+	return false;
+}
+
+static inline bool knav_queue_match_type(struct knav_queue_inst *inst,
+						unsigned type)
+{
+	if ((type == KNAV_QUEUE_QPEND) &&
+	    (inst->range->flags & RANGE_HAS_IRQ)) {
+		return true;
+	} else if ((type == KNAV_QUEUE_ACC) &&
+		(inst->range->flags & RANGE_HAS_ACCUMULATOR)) {
+		return true;
+	} else if ((type == KNAV_QUEUE_GP) &&
+		!(inst->range->flags &
+			(RANGE_HAS_ACCUMULATOR | RANGE_HAS_IRQ))) {
+		return true;
+	}
+	return false;
+}
+
+static inline struct knav_queue_inst *
+knav_queue_match_id_to_inst(struct knav_device *kdev, unsigned id)
+{
+	struct knav_queue_inst *inst;
+	int idx;
+
+	for_each_instance(idx, inst, kdev) {
+		if (inst->id == id)
+			return inst;
+	}
+	return NULL;
+}
+
+static inline struct knav_queue_inst *knav_queue_find_by_id(int id)
+{
+	if (kdev->base_id <= id &&
+	    kdev->base_id + kdev->num_queues > id) {
+		id -= kdev->base_id;
+		return knav_queue_match_id_to_inst(kdev, id);
+	}
+	return NULL;
+}
+
+static struct knav_queue *__knav_queue_open(struct knav_queue_inst *inst,
+				      const char *name, unsigned flags)
+{
+	struct knav_queue *qh;
+	unsigned id;
+	int ret = 0;
+
+	qh = devm_kzalloc(inst->kdev->dev, sizeof(*qh), GFP_KERNEL);
+	if (!qh)
+		return ERR_PTR(-ENOMEM);
+
+	qh->flags = flags;
+	qh->inst = inst;
+	id = inst->id - inst->qmgr->start_queue;
+	qh->reg_push = &inst->qmgr->reg_push[id];
+	qh->reg_pop = &inst->qmgr->reg_pop[id];
+	qh->reg_peek = &inst->qmgr->reg_peek[id];
+
+	/* first opener? */
+	if (!knav_queue_is_busy(inst)) {
+		struct knav_range_info *range = inst->range;
+
+		inst->name = kstrndup(name, KNAV_NAME_SIZE, GFP_KERNEL);
+		if (range->ops && range->ops->open_queue)
+			ret = range->ops->open_queue(range, inst, flags);
+
+		if (ret) {
+			devm_kfree(inst->kdev->dev, qh);
+			return ERR_PTR(ret);
+		}
+	}
+	list_add_tail_rcu(&qh->list, &inst->handles);
+	return qh;
+}
+
+static struct knav_queue *
+knav_queue_open_by_id(const char *name, unsigned id, unsigned flags)
+{
+	struct knav_queue_inst *inst;
+	struct knav_queue *qh;
+
+	mutex_lock(&knav_dev_lock);
+
+	qh = ERR_PTR(-ENODEV);
+	inst = knav_queue_find_by_id(id);
+	if (!inst)
+		goto unlock_ret;
+
+	qh = ERR_PTR(-EEXIST);
+	if (!(flags & KNAV_QUEUE_SHARED) && knav_queue_is_busy(inst))
+		goto unlock_ret;
+
+	qh = ERR_PTR(-EBUSY);
+	if ((flags & KNAV_QUEUE_SHARED) &&
+	    (knav_queue_is_busy(inst) && !knav_queue_is_shared(inst)))
+		goto unlock_ret;
+
+	qh = __knav_queue_open(inst, name, flags);
+
+unlock_ret:
+	mutex_unlock(&knav_dev_lock);
+
+	return qh;
+}
+
+static struct knav_queue *knav_queue_open_by_type(const char *name,
+						unsigned type, unsigned flags)
+{
+	struct knav_queue_inst *inst;
+	struct knav_queue *qh = ERR_PTR(-EINVAL);
+	int idx;
+
+	mutex_lock(&knav_dev_lock);
+
+	for_each_instance(idx, inst, kdev) {
+		if (knav_queue_is_reserved(inst))
+			continue;
+		if (!knav_queue_match_type(inst, type))
+			continue;
+		if (knav_queue_is_busy(inst))
+			continue;
+		qh = __knav_queue_open(inst, name, flags);
+		goto unlock_ret;
+	}
+
+unlock_ret:
+	mutex_unlock(&knav_dev_lock);
+	return qh;
+}
+
+static void knav_queue_set_notify(struct knav_queue_inst *inst, bool enabled)
+{
+	struct knav_range_info *range = inst->range;
+
+	if (range->ops && range->ops->set_notify)
+		range->ops->set_notify(range, inst, enabled);
+}
+
+static int knav_queue_enable_notifier(struct knav_queue *qh)
+{
+	struct knav_queue_inst *inst = qh->inst;
+	bool first;
+
+	if (WARN_ON(!qh->notifier_fn))
+		return -EINVAL;
+
+	/* Adjust the per handle notifier count */
+	first = (atomic_inc_return(&qh->notifier_enabled) == 1);
+	if (!first)
+		return 0; /* nothing to do */
+
+	/* Now adjust the per instance notifier count */
+	first = (atomic_inc_return(&inst->num_notifiers) == 1);
+	if (first)
+		knav_queue_set_notify(inst, true);
+
+	return 0;
+}
+
+static int knav_queue_disable_notifier(struct knav_queue *qh)
+{
+	struct knav_queue_inst *inst = qh->inst;
+	bool last;
+
+	last = (atomic_dec_return(&qh->notifier_enabled) == 0);
+	if (!last)
+		return 0; /* nothing to do */
+
+	last = (atomic_dec_return(&inst->num_notifiers) == 0);
+	if (last)
+		knav_queue_set_notify(inst, false);
+
+	return 0;
+}
+
+static int knav_queue_set_notifier(struct knav_queue *qh,
+				struct knav_queue_notify_config *cfg)
+{
+	knav_queue_notify_fn old_fn = qh->notifier_fn;
+
+	if (!cfg)
+		return -EINVAL;
+
+	if (!(qh->inst->range->flags & (RANGE_HAS_ACCUMULATOR | RANGE_HAS_IRQ)))
+		return -ENOTSUPP;
+
+	if (!cfg->fn && old_fn)
+		knav_queue_disable_notifier(qh);
+
+	qh->notifier_fn = cfg->fn;
+	qh->notifier_fn_arg = cfg->fn_arg;
+
+	if (cfg->fn && !old_fn)
+		knav_queue_enable_notifier(qh);
+
+	return 0;
+}
+
+static int knav_gp_set_notify(struct knav_range_info *range,
+			       struct knav_queue_inst *inst,
+			       bool enabled)
+{
+	unsigned queue;
+
+	if (range->flags & RANGE_HAS_IRQ) {
+		queue = inst->id - range->queue_base;
+		if (enabled)
+			enable_irq(range->irqs[queue].irq);
+		else
+			disable_irq_nosync(range->irqs[queue].irq);
+	}
+	return 0;
+}
+
+static int knav_gp_open_queue(struct knav_range_info *range,
+				struct knav_queue_inst *inst, unsigned flags)
+{
+	return knav_queue_setup_irq(range, inst);
+}
+
+static int knav_gp_close_queue(struct knav_range_info *range,
+				struct knav_queue_inst *inst)
+{
+	knav_queue_free_irq(inst);
+	return 0;
+}
+
+struct knav_range_ops knav_gp_range_ops = {
+	.set_notify	= knav_gp_set_notify,
+	.open_queue	= knav_gp_open_queue,
+	.close_queue	= knav_gp_close_queue,
+};
+
+
+static int knav_queue_get_count(void *qhandle)
+{
+	struct knav_queue *qh = qhandle;
+	struct knav_queue_inst *inst = qh->inst;
+
+	return readl_relaxed(&qh->reg_peek[0].entry_count) +
+		atomic_read(&inst->desc_count);
+}
+
+static void knav_queue_debug_show_instance(struct seq_file *s,
+					struct knav_queue_inst *inst)
+{
+	struct knav_device *kdev = inst->kdev;
+	struct knav_queue *qh;
+
+	if (!knav_queue_is_busy(inst))
+		return;
+
+	seq_printf(s, "\tqueue id %d (%s)\n",
+		   kdev->base_id + inst->id, inst->name);
+	for_each_handle_rcu(qh, inst) {
+		seq_printf(s, "\t\thandle %p: ", qh);
+		seq_printf(s, "pushes %8d, ",
+			   atomic_read(&qh->stats.pushes));
+		seq_printf(s, "pops %8d, ",
+			   atomic_read(&qh->stats.pops));
+		seq_printf(s, "count %8d, ",
+			   knav_queue_get_count(qh));
+		seq_printf(s, "notifies %8d, ",
+			   atomic_read(&qh->stats.notifies));
+		seq_printf(s, "push errors %8d, ",
+			   atomic_read(&qh->stats.push_errors));
+		seq_printf(s, "pop errors %8d\n",
+			   atomic_read(&qh->stats.pop_errors));
+	}
+}
+
+static int knav_queue_debug_show(struct seq_file *s, void *v)
+{
+	struct knav_queue_inst *inst;
+	int idx;
+
+	mutex_lock(&knav_dev_lock);
+	seq_printf(s, "%s: %u-%u\n",
+		   dev_name(kdev->dev), kdev->base_id,
+		   kdev->base_id + kdev->num_queues - 1);
+	for_each_instance(idx, inst, kdev)
+		knav_queue_debug_show_instance(s, inst);
+	mutex_unlock(&knav_dev_lock);
+
+	return 0;
+}
+
+static int knav_queue_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open(file, knav_queue_debug_show, NULL);
+}
+
+static const struct file_operations knav_queue_debug_ops = {
+	.open		= knav_queue_debug_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.release	= single_release,
+};
+
+static inline int knav_queue_pdsp_wait(u32 * __iomem addr, unsigned timeout,
+					u32 flags)
+{
+	unsigned long end;
+	u32 val = 0;
+
+	end = jiffies + msecs_to_jiffies(timeout);
+	while (time_after(end, jiffies)) {
+		val = readl_relaxed(addr);
+		if (flags)
+			val &= flags;
+		if (!val)
+			break;
+		cpu_relax();
+	}
+	return val ? -ETIMEDOUT : 0;
+}
+
+
+static int knav_queue_flush(struct knav_queue *qh)
+{
+	struct knav_queue_inst *inst = qh->inst;
+	unsigned id = inst->id - inst->qmgr->start_queue;
+
+	atomic_set(&inst->desc_count, 0);
+	writel_relaxed(0, &inst->qmgr->reg_push[id].ptr_size_thresh);
+	return 0;
+}
+
+/**
+ * knav_queue_open()	- open a hardware queue
+ * @name		- name to give the queue handle
+ * @id			- desired queue number if any or specifes the type
+ *			  of queue
+ * @flags		- the following flags are applicable to queues:
+ *	KNAV_QUEUE_SHARED - allow the queue to be shared. Queues are
+ *			     exclusive by default.
+ *			     Subsequent attempts to open a shared queue should
+ *			     also have this flag.
+ *
+ * Returns a handle to the open hardware queue if successful. Use IS_ERR()
+ * to check the returned value for error codes.
+ */
+void *knav_queue_open(const char *name, unsigned id,
+					unsigned flags)
+{
+	struct knav_queue *qh = ERR_PTR(-EINVAL);
+
+	switch (id) {
+	case KNAV_QUEUE_QPEND:
+	case KNAV_QUEUE_ACC:
+	case KNAV_QUEUE_GP:
+		qh = knav_queue_open_by_type(name, id, flags);
+		break;
+
+	default:
+		qh = knav_queue_open_by_id(name, id, flags);
+		break;
+	}
+	return qh;
+}
+EXPORT_SYMBOL_GPL(knav_queue_open);
+
+/**
+ * knav_queue_close()	- close a hardware queue handle
+ * @qh			- handle to close
+ */
+void knav_queue_close(void *qhandle)
+{
+	struct knav_queue *qh = qhandle;
+	struct knav_queue_inst *inst = qh->inst;
+
+	while (atomic_read(&qh->notifier_enabled) > 0)
+		knav_queue_disable_notifier(qh);
+
+	mutex_lock(&knav_dev_lock);
+	list_del_rcu(&qh->list);
+	mutex_unlock(&knav_dev_lock);
+	synchronize_rcu();
+	if (!knav_queue_is_busy(inst)) {
+		struct knav_range_info *range = inst->range;
+
+		if (range->ops && range->ops->close_queue)
+			range->ops->close_queue(range, inst);
+	}
+	devm_kfree(inst->kdev->dev, qh);
+}
+EXPORT_SYMBOL_GPL(knav_queue_close);
+
+/**
+ * knav_queue_device_control()	- Perform control operations on a queue
+ * @qh				- queue handle
+ * @cmd				- control commands
+ * @arg				- command argument
+ *
+ * Returns 0 on success, errno otherwise.
+ */
+int knav_queue_device_control(void *qhandle, enum knav_queue_ctrl_cmd cmd,
+				unsigned long arg)
+{
+	struct knav_queue *qh = qhandle;
+	struct knav_queue_notify_config *cfg;
+	int ret;
+
+	switch ((int)cmd) {
+	case KNAV_QUEUE_GET_ID:
+		ret = qh->inst->kdev->base_id + qh->inst->id;
+		break;
+
+	case KNAV_QUEUE_FLUSH:
+		ret = knav_queue_flush(qh);
+		break;
+
+	case KNAV_QUEUE_SET_NOTIFIER:
+		cfg = (void *)arg;
+		ret = knav_queue_set_notifier(qh, cfg);
+		break;
+
+	case KNAV_QUEUE_ENABLE_NOTIFY:
+		ret = knav_queue_enable_notifier(qh);
+		break;
+
+	case KNAV_QUEUE_DISABLE_NOTIFY:
+		ret = knav_queue_disable_notifier(qh);
+		break;
+
+	case KNAV_QUEUE_GET_COUNT:
+		ret = knav_queue_get_count(qh);
+		break;
+
+	default:
+		ret = -ENOTSUPP;
+		break;
+	}
+	return ret;
+}
+EXPORT_SYMBOL_GPL(knav_queue_device_control);
+
+
+
+/**
+ * knav_queue_push()	- push data (or descriptor) to the tail of a queue
+ * @qh			- hardware queue handle
+ * @data		- data to push
+ * @size		- size of data to push
+ * @flags		- can be used to pass additional information
+ *
+ * Returns 0 on success, errno otherwise.
+ */
+int knav_queue_push(void *qhandle, dma_addr_t dma,
+					unsigned size, unsigned flags)
+{
+	struct knav_queue *qh = qhandle;
+	u32 val;
+
+	val = (u32)dma | ((size / 16) - 1);
+	writel_relaxed(val, &qh->reg_push[0].ptr_size_thresh);
+
+	atomic_inc(&qh->stats.pushes);
+	return 0;
+}
+
+/**
+ * knav_queue_pop()	- pop data (or descriptor) from the head of a queue
+ * @qh			- hardware queue handle
+ * @size		- (optional) size of the data pop'ed.
+ *
+ * Returns a DMA address on success, 0 on failure.
+ */
+dma_addr_t knav_queue_pop(void *qhandle, unsigned *size)
+{
+	struct knav_queue *qh = qhandle;
+	struct knav_queue_inst *inst = qh->inst;
+	dma_addr_t dma;
+	u32 val, idx;
+
+	/* are we accumulated? */
+	if (inst->descs) {
+		if (unlikely(atomic_dec_return(&inst->desc_count) < 0)) {
+			atomic_inc(&inst->desc_count);
+			return 0;
+		}
+		idx  = atomic_inc_return(&inst->desc_head);
+		idx &= ACC_DESCS_MASK;
+		val = inst->descs[idx];
+	} else {
+		val = readl_relaxed(&qh->reg_pop[0].ptr_size_thresh);
+		if (unlikely(!val))
+			return 0;
+	}
+
+	dma = val & DESC_PTR_MASK;
+	if (size)
+		*size = ((val & DESC_SIZE_MASK) + 1) * 16;
+
+	atomic_inc(&qh->stats.pops);
+	return dma;
+}
+
+/* carve out descriptors and push into queue */
+static void kdesc_fill_pool(struct knav_pool *pool)
+{
+	struct knav_region *region;
+	int i;
+
+	region = pool->region;
+	pool->desc_size = region->desc_size;
+	for (i = 0; i < pool->num_desc; i++) {
+		int index = pool->region_offset + i;
+		dma_addr_t dma_addr;
+		unsigned dma_size;
+		dma_addr = region->dma_start + (region->desc_size * index);
+		dma_size = ALIGN(pool->desc_size, SMP_CACHE_BYTES);
+		dma_sync_single_for_device(pool->dev, dma_addr, dma_size,
+					   DMA_TO_DEVICE);
+		knav_queue_push(pool->queue, dma_addr, dma_size, 0);
+	}
+}
+
+/* pop out descriptors and close the queue */
+static void kdesc_empty_pool(struct knav_pool *pool)
+{
+	dma_addr_t dma;
+	unsigned size;
+	void *desc;
+	int i;
+
+	if (!pool->queue)
+		return;
+
+	for (i = 0;; i++) {
+		dma = knav_queue_pop(pool->queue, &size);
+		if (!dma)
+			break;
+		desc = knav_pool_desc_dma_to_virt(pool, dma);
+		if (!desc) {
+			dev_dbg(pool->kdev->dev,
+				"couldn't unmap desc, continuing\n");
+			continue;
+		}
+	}
+	WARN_ON(i != pool->num_desc);
+	knav_queue_close(pool->queue);
+}
+
+
+/* Get the DMA address of a descriptor */
+dma_addr_t knav_pool_desc_virt_to_dma(void *ph, void *virt)
+{
+	struct knav_pool *pool = ph;
+	return pool->region->dma_start + (virt - pool->region->virt_start);
+}
+
+void *knav_pool_desc_dma_to_virt(void *ph, dma_addr_t dma)
+{
+	struct knav_pool *pool = ph;
+	return pool->region->virt_start + (dma - pool->region->dma_start);
+}
+
+/**
+ * knav_pool_create()	- Create a pool of descriptors
+ * @name		- name to give the pool handle
+ * @num_desc		- numbers of descriptors in the pool
+ * @region_id		- QMSS region id from which the descriptors are to be
+ *			  allocated.
+ *
+ * Returns a pool handle on success.
+ * Use IS_ERR_OR_NULL() to identify error values on return.
+ */
+void *knav_pool_create(const char *name,
+					int num_desc, int region_id)
+{
+	struct knav_region *reg_itr, *region = NULL;
+	struct knav_pool *pool, *pi;
+	struct list_head *node;
+	unsigned last_offset;
+	bool slot_found;
+	int ret;
+
+	if (!kdev->dev)
+		return ERR_PTR(-ENODEV);
+
+	pool = devm_kzalloc(kdev->dev, sizeof(*pool), GFP_KERNEL);
+	if (!pool) {
+		dev_err(kdev->dev, "out of memory allocating pool\n");
+		return ERR_PTR(-ENOMEM);
+	}
+
+	for_each_region(kdev, reg_itr) {
+		if (reg_itr->id != region_id)
+			continue;
+		region = reg_itr;
+		break;
+	}
+
+	if (!region) {
+		dev_err(kdev->dev, "region-id(%d) not found\n", region_id);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	pool->queue = knav_queue_open(name, KNAV_QUEUE_GP, 0);
+	if (IS_ERR_OR_NULL(pool->queue)) {
+		dev_err(kdev->dev,
+			"failed to open queue for pool(%s), error %ld\n",
+			name, PTR_ERR(pool->queue));
+		ret = PTR_ERR(pool->queue);
+		goto err;
+	}
+
+	pool->name = kstrndup(name, KNAV_NAME_SIZE, GFP_KERNEL);
+	pool->kdev = kdev;
+	pool->dev = kdev->dev;
+
+	mutex_lock(&knav_dev_lock);
+
+	if (num_desc > (region->num_desc - region->used_desc)) {
+		dev_err(kdev->dev, "out of descs in region(%d) for pool(%s)\n",
+			region_id, name);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	/* Region maintains a sorted (by region offset) list of pools
+	 * use the first free slot which is large enough to accomodate
+	 * the request
+	 */
+	last_offset = 0;
+	slot_found = false;
+	node = &region->pools;
+	list_for_each_entry(pi, &region->pools, region_inst) {
+		if ((pi->region_offset - last_offset) >= num_desc) {
+			slot_found = true;
+			break;
+		}
+		last_offset = pi->region_offset + pi->num_desc;
+	}
+	node = &pi->region_inst;
+
+	if (slot_found) {
+		pool->region = region;
+		pool->num_desc = num_desc;
+		pool->region_offset = last_offset;
+		region->used_desc += num_desc;
+		list_add_tail(&pool->list, &kdev->pools);
+		list_add_tail(&pool->region_inst, node);
+	} else {
+		dev_err(kdev->dev, "pool(%s) create failed: fragmented desc pool in region(%d)\n",
+			name, region_id);
+		ret = -ENOMEM;
+		goto err;
+	}
+
+	mutex_unlock(&knav_dev_lock);
+	kdesc_fill_pool(pool);
+	return pool;
+
+err:
+	mutex_unlock(&knav_dev_lock);
+	kfree(pool->name);
+	devm_kfree(kdev->dev, pool);
+	return ERR_PTR(ret);
+}
+EXPORT_SYMBOL_GPL(knav_pool_create);
+
+/**
+ * knav_pool_destroy()	- Free a pool of descriptors
+ * @pool		- pool handle
+ */
+void knav_pool_destroy(void *ph)
+{
+	struct knav_pool *pool = ph;
+
+	if (!pool)
+		return;
+
+	if (!pool->region)
+		return;
+
+	kdesc_empty_pool(pool);
+	mutex_lock(&knav_dev_lock);
+
+	pool->region->used_desc -= pool->num_desc;
+	list_del(&pool->region_inst);
+	list_del(&pool->list);
+
+	mutex_unlock(&knav_dev_lock);
+	kfree(pool->name);
+	devm_kfree(kdev->dev, pool);
+}
+EXPORT_SYMBOL_GPL(knav_pool_destroy);
+
+
+/**
+ * knav_pool_desc_get()	- Get a descriptor from the pool
+ * @pool			- pool handle
+ *
+ * Returns descriptor from the pool.
+ */
+void *knav_pool_desc_get(void *ph)
+{
+	struct knav_pool *pool = ph;
+	dma_addr_t dma;
+	unsigned size;
+	void *data;
+
+	dma = knav_queue_pop(pool->queue, &size);
+	if (unlikely(!dma))
+		return ERR_PTR(-ENOMEM);
+	data = knav_pool_desc_dma_to_virt(pool, dma);
+	return data;
+}
+
+/**
+ * knav_pool_desc_put()	- return a descriptor to the pool
+ * @pool			- pool handle
+ */
+void knav_pool_desc_put(void *ph, void *desc)
+{
+	struct knav_pool *pool = ph;
+	dma_addr_t dma;
+	dma = knav_pool_desc_virt_to_dma(pool, desc);
+	knav_queue_push(pool->queue, dma, pool->region->desc_size, 0);
+}
+
+/**
+ * knav_pool_desc_map()	- Map descriptor for DMA transfer
+ * @pool			- pool handle
+ * @desc			- address of descriptor to map
+ * @size			- size of descriptor to map
+ * @dma				- DMA address return pointer
+ * @dma_sz			- adjusted return pointer
+ *
+ * Returns 0 on success, errno otherwise.
+ */
+int knav_pool_desc_map(void *ph, void *desc, unsigned size,
+					dma_addr_t *dma, unsigned *dma_sz)
+{
+	struct knav_pool *pool = ph;
+	*dma = knav_pool_desc_virt_to_dma(pool, desc);
+	size = min(size, pool->region->desc_size);
+	size = ALIGN(size, SMP_CACHE_BYTES);
+	*dma_sz = size;
+	dma_sync_single_for_device(pool->dev, *dma, size, DMA_TO_DEVICE);
+
+	/* Ensure the descriptor reaches to the memory */
+	__iowmb();
+
+	return 0;
+}
+
+/**
+ * knav_pool_desc_unmap()	- Unmap descriptor after DMA transfer
+ * @pool			- pool handle
+ * @dma				- DMA address of descriptor to unmap
+ * @dma_sz			- size of descriptor to unmap
+ *
+ * Returns descriptor address on success, Use IS_ERR_OR_NULL() to identify
+ * error values on return.
+ */
+void *knav_pool_desc_unmap(void *ph, dma_addr_t dma, unsigned dma_sz)
+{
+	struct knav_pool *pool = ph;
+	unsigned desc_sz;
+	void *desc;
+
+	desc_sz = min(dma_sz, pool->region->desc_size);
+	desc = knav_pool_desc_dma_to_virt(pool, dma);
+	dma_sync_single_for_cpu(pool->dev, dma, desc_sz, DMA_FROM_DEVICE);
+	prefetch(desc);
+	return desc;
+}
+
+/**
+ * knav_pool_count()	- Get the number of descriptors in pool.
+ * @pool		- pool handle
+ * Returns number of elements in the pool.
+ */
+int knav_pool_count(void *ph)
+{
+	struct knav_pool *pool = ph;
+	return knav_queue_get_count(pool->queue);
+}
+
+static void knav_queue_setup_region(struct knav_device *kdev,
+					struct knav_region *region)
+{
+	unsigned hw_num_desc, hw_desc_size, size;
+	struct knav_reg_region __iomem  *regs;
+	struct knav_qmgr_info *qmgr;
+	struct knav_pool *pool;
+	int id = region->id;
+	struct page *page;
+
+	/* unused region? */
+	if (!region->num_desc) {
+		dev_warn(kdev->dev, "unused region %s\n", region->name);
+		return;
+	}
+
+	/* get hardware descriptor value */
+	hw_num_desc = ilog2(region->num_desc - 1) + 1;
+
+	/* did we force fit ourselves into nothingness? */
+	if (region->num_desc < 32) {
+		region->num_desc = 0;
+		dev_warn(kdev->dev, "too few descriptors in region %s\n",
+			 region->name);
+		return;
+	}
+
+	size = region->num_desc * region->desc_size;
+	region->virt_start = alloc_pages_exact(size, GFP_KERNEL | GFP_DMA |
+						GFP_DMA32);
+	if (!region->virt_start) {
+		region->num_desc = 0;
+		dev_err(kdev->dev, "memory alloc failed for region %s\n",
+			region->name);
+		return;
+	}
+	region->virt_end = region->virt_start + size;
+	page = virt_to_page(region->virt_start);
+
+	region->dma_start = dma_map_page(kdev->dev, page, 0, size,
+					 DMA_BIDIRECTIONAL);
+	if (dma_mapping_error(kdev->dev, region->dma_start)) {
+		dev_err(kdev->dev, "dma map failed for region %s\n",
+			region->name);
+		goto fail;
+	}
+	region->dma_end = region->dma_start + size;
+
+	pool = devm_kzalloc(kdev->dev, sizeof(*pool), GFP_KERNEL);
+	if (!pool) {
+		dev_err(kdev->dev, "out of memory allocating dummy pool\n");
+		goto fail;
+	}
+	pool->num_desc = 0;
+	pool->region_offset = region->num_desc;
+	list_add(&pool->region_inst, &region->pools);
+
+	dev_dbg(kdev->dev,
+		"region %s (%d): size:%d, link:%d@%d, phys:%08x-%08x, virt:%p-%p\n",
+		region->name, id, region->desc_size, region->num_desc,
+		region->link_index, region->dma_start, region->dma_end,
+		region->virt_start, region->virt_end);
+
+	hw_desc_size = (region->desc_size / 16) - 1;
+	hw_num_desc -= 5;
+
+	for_each_qmgr(kdev, qmgr) {
+		regs = qmgr->reg_region + id;
+		writel_relaxed(region->dma_start, &regs->base);
+		writel_relaxed(region->link_index, &regs->start_index);
+		writel_relaxed(hw_desc_size << 16 | hw_num_desc,
+			       &regs->size_count);
+	}
+	return;
+
+fail:
+	if (region->dma_start)
+		dma_unmap_page(kdev->dev, region->dma_start, size,
+				DMA_BIDIRECTIONAL);
+	if (region->virt_start)
+		free_pages_exact(region->virt_start, size);
+	region->num_desc = 0;
+	return;
+}
+
+static const char *knav_queue_find_name(struct device_node *node)
+{
+	const char *name;
+
+	if (of_property_read_string(node, "label", &name) < 0)
+		name = node->name;
+	if (!name)
+		name = "unknown";
+	return name;
+}
+
+static int knav_queue_setup_regions(struct knav_device *kdev,
+					struct device_node *regions)
+{
+	struct device *dev = kdev->dev;
+	struct knav_region *region;
+	struct device_node *child;
+	u32 temp[2];
+	int ret;
+
+	for_each_child_of_node(regions, child) {
+		region = devm_kzalloc(dev, sizeof(*region), GFP_KERNEL);
+		if (!region) {
+			dev_err(dev, "out of memory allocating region\n");
+			return -ENOMEM;
+		}
+
+		region->name = knav_queue_find_name(child);
+		of_property_read_u32(child, "id", &region->id);
+		ret = of_property_read_u32_array(child, "region-spec", temp, 2);
+		if (!ret) {
+			region->num_desc  = temp[0];
+			region->desc_size = temp[1];
+		} else {
+			dev_err(dev, "invalid region info %s\n", region->name);
+			devm_kfree(dev, region);
+			continue;
+		}
+
+		if (!of_get_property(child, "link-index", NULL)) {
+			dev_err(dev, "No link info for %s\n", region->name);
+			devm_kfree(dev, region);
+			continue;
+		}
+		ret = of_property_read_u32(child, "link-index",
+					   &region->link_index);
+		if (ret) {
+			dev_err(dev, "link index not found for %s\n",
+				region->name);
+			devm_kfree(dev, region);
+			continue;
+		}
+
+		INIT_LIST_HEAD(&region->pools);
+		list_add_tail(&region->list, &kdev->regions);
+	}
+	if (list_empty(&kdev->regions)) {
+		dev_err(dev, "no valid region information found\n");
+		return -ENODEV;
+	}
+
+	/* Next, we run through the regions and set things up */
+	for_each_region(kdev, region)
+		knav_queue_setup_region(kdev, region);
+
+	return 0;
+}
+
+static int knav_get_link_ram(struct knav_device *kdev,
+				       const char *name,
+				       struct knav_link_ram_block *block)
+{
+	struct platform_device *pdev = to_platform_device(kdev->dev);
+	struct device_node *node = pdev->dev.of_node;
+	u32 temp[2];
+
+	/*
+	 * Note: link ram resources are specified in "entry" sized units. In
+	 * reality, although entries are ~40bits in hardware, we treat them as
+	 * 64-bit entities here.
+	 *
+	 * For example, to specify the internal link ram for Keystone-I class
+	 * devices, we would set the linkram0 resource to 0x80000-0x83fff.
+	 *
+	 * This gets a bit weird when other link rams are used.  For example,
+	 * if the range specified is 0x0c000000-0x0c003fff (i.e., 16K entries
+	 * in MSMC SRAM), the actual memory used is 0x0c000000-0x0c020000,
+	 * which accounts for 64-bits per entry, for 16K entries.
+	 */
+	if (!of_property_read_u32_array(node, name , temp, 2)) {
+		if (temp[0]) {
+			/*
+			 * queue_base specified => using internal or onchip
+			 * link ram WARNING - we do not "reserve" this block
+			 */
+			block->phys = (dma_addr_t)temp[0];
+			block->virt = NULL;
+			block->size = temp[1];
+		} else {
+			block->size = temp[1];
+			/* queue_base not specific => allocate requested size */
+			block->virt = dmam_alloc_coherent(kdev->dev,
+						  8 * block->size, &block->phys,
+						  GFP_KERNEL);
+			if (!block->virt) {
+				dev_err(kdev->dev, "failed to alloc linkram\n");
+				return -ENOMEM;
+			}
+		}
+	} else {
+		return -ENODEV;
+	}
+	return 0;
+}
+
+static int knav_queue_setup_link_ram(struct knav_device *kdev)
+{
+	struct knav_link_ram_block *block;
+	struct knav_qmgr_info *qmgr;
+
+	for_each_qmgr(kdev, qmgr) {
+		block = &kdev->link_rams[0];
+		dev_dbg(kdev->dev, "linkram0: phys:%x, virt:%p, size:%x\n",
+			block->phys, block->virt, block->size);
+		writel_relaxed(block->phys, &qmgr->reg_config->link_ram_base0);
+		writel_relaxed(block->size, &qmgr->reg_config->link_ram_size0);
+
+		block++;
+		if (!block->size)
+			return 0;
+
+		dev_dbg(kdev->dev, "linkram1: phys:%x, virt:%p, size:%x\n",
+			block->phys, block->virt, block->size);
+		writel_relaxed(block->phys, &qmgr->reg_config->link_ram_base1);
+	}
+
+	return 0;
+}
+
+static int knav_setup_queue_range(struct knav_device *kdev,
+					struct device_node *node)
+{
+	struct device *dev = kdev->dev;
+	struct knav_range_info *range;
+	struct knav_qmgr_info *qmgr;
+	u32 temp[2], start, end, id, index;
+	int ret, i;
+
+	range = devm_kzalloc(dev, sizeof(*range), GFP_KERNEL);
+	if (!range) {
+		dev_err(dev, "out of memory allocating range\n");
+		return -ENOMEM;
+	}
+
+	range->kdev = kdev;
+	range->name = knav_queue_find_name(node);
+	ret = of_property_read_u32_array(node, "qrange", temp, 2);
+	if (!ret) {
+		range->queue_base = temp[0] - kdev->base_id;
+		range->num_queues = temp[1];
+	} else {
+		dev_err(dev, "invalid queue range %s\n", range->name);
+		devm_kfree(dev, range);
+		return -EINVAL;
+	}
+
+	for (i = 0; i < RANGE_MAX_IRQS; i++) {
+		struct of_phandle_args oirq;
+
+		if (of_irq_parse_one(node, i, &oirq))
+			break;
+
+		range->irqs[i].irq = irq_create_of_mapping(&oirq);
+		if (range->irqs[i].irq == IRQ_NONE)
+			break;
+
+		range->num_irqs++;
+
+		if (oirq.args_count == 3)
+			range->irqs[i].cpu_map =
+				(oirq.args[2] & 0x0000ff00) >> 8;
+	}
+
+	range->num_irqs = min(range->num_irqs, range->num_queues);
+	if (range->num_irqs)
+		range->flags |= RANGE_HAS_IRQ;
+
+	if (of_get_property(node, "qalloc-by-id", NULL))
+		range->flags |= RANGE_RESERVED;
+
+	if (of_get_property(node, "accumulator", NULL)) {
+		ret = knav_init_acc_range(kdev, node, range);
+		if (ret < 0) {
+			devm_kfree(dev, range);
+			return ret;
+		}
+	} else {
+		range->ops = &knav_gp_range_ops;
+	}
+
+	/* set threshold to 1, and flush out the queues */
+	for_each_qmgr(kdev, qmgr) {
+		start = max(qmgr->start_queue, range->queue_base);
+		end   = min(qmgr->start_queue + qmgr->num_queues,
+			    range->queue_base + range->num_queues);
+		for (id = start; id < end; id++) {
+			index = id - qmgr->start_queue;
+			writel_relaxed(THRESH_GTE | 1,
+				       &qmgr->reg_peek[index].ptr_size_thresh);
+			writel_relaxed(0,
+				       &qmgr->reg_push[index].ptr_size_thresh);
+		}
+	}
+
+	list_add_tail(&range->list, &kdev->queue_ranges);
+	dev_dbg(dev, "added range %s: %d-%d, %d irqs%s%s%s\n",
+		range->name, range->queue_base,
+		range->queue_base + range->num_queues - 1,
+		range->num_irqs,
+		(range->flags & RANGE_HAS_IRQ) ? ", has irq" : "",
+		(range->flags & RANGE_RESERVED) ? ", reserved" : "",
+		(range->flags & RANGE_HAS_ACCUMULATOR) ? ", acc" : "");
+	kdev->num_queues_in_use += range->num_queues;
+	return 0;
+}
+
+static int knav_setup_queue_pools(struct knav_device *kdev,
+				   struct device_node *queue_pools)
+{
+	struct device_node *type, *range;
+	int ret;
+
+	for_each_child_of_node(queue_pools, type) {
+		for_each_child_of_node(type, range) {
+			ret = knav_setup_queue_range(kdev, range);
+			/* return value ignored, we init the rest... */
+		}
+	}
+
+	/* ... and barf if they all failed! */
+	if (list_empty(&kdev->queue_ranges)) {
+		dev_err(kdev->dev, "no valid queue range found\n");
+		return -ENODEV;
+	}
+	return 0;
+}
+
+static void knav_free_queue_range(struct knav_device *kdev,
+				  struct knav_range_info *range)
+{
+	if (range->ops && range->ops->free_range)
+		range->ops->free_range(range);
+	list_del(&range->list);
+	devm_kfree(kdev->dev, range);
+}
+
+static void knav_free_queue_ranges(struct knav_device *kdev)
+{
+	struct knav_range_info *range;
+
+	for (;;) {
+		range = first_queue_range(kdev);
+		if (!range)
+			break;
+		knav_free_queue_range(kdev, range);
+	}
+}
+
+static void knav_queue_free_regions(struct knav_device *kdev)
+{
+	struct knav_region *region;
+	struct knav_pool *pool;
+	unsigned size;
+
+	for (;;) {
+		region = first_region(kdev);
+		if (!region)
+			break;
+		list_for_each_entry(pool, &region->pools, region_inst)
+			knav_pool_destroy(pool);
+
+		size = region->virt_end - region->virt_start;
+		if (size)
+			free_pages_exact(region->virt_start, size);
+		list_del(&region->list);
+		devm_kfree(kdev->dev, region);
+	}
+}
+
+static void __iomem *knav_queue_map_reg(struct knav_device *kdev,
+					struct device_node *node, int index)
+{
+	struct resource res;
+	void __iomem *regs;
+	int ret;
+
+	ret = of_address_to_resource(node, index, &res);
+	if (ret) {
+		dev_err(kdev->dev, "Can't translate of node(%s) address for index(%d)\n",
+			node->name, index);
+		return ERR_PTR(ret);
+	}
+
+	regs = devm_ioremap_resource(kdev->dev, &res);
+	if (IS_ERR(regs))
+		dev_err(kdev->dev, "Failed to map register base for index(%d) node(%s)\n",
+			index, node->name);
+	return regs;
+}
+
+static int knav_queue_init_qmgrs(struct knav_device *kdev,
+					struct device_node *qmgrs)
+{
+	struct device *dev = kdev->dev;
+	struct knav_qmgr_info *qmgr;
+	struct device_node *child;
+	u32 temp[2];
+	int ret;
+
+	for_each_child_of_node(qmgrs, child) {
+		qmgr = devm_kzalloc(dev, sizeof(*qmgr), GFP_KERNEL);
+		if (!qmgr) {
+			dev_err(dev, "out of memory allocating qmgr\n");
+			return -ENOMEM;
+		}
+
+		ret = of_property_read_u32_array(child, "managed-queues",
+						 temp, 2);
+		if (!ret) {
+			qmgr->start_queue = temp[0];
+			qmgr->num_queues = temp[1];
+		} else {
+			dev_err(dev, "invalid qmgr queue range\n");
+			devm_kfree(dev, qmgr);
+			continue;
+		}
+
+		dev_info(dev, "qmgr start queue %d, number of queues %d\n",
+			 qmgr->start_queue, qmgr->num_queues);
+
+		qmgr->reg_peek =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_PEEK_REG_INDEX);
+		qmgr->reg_status =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_STATUS_REG_INDEX);
+		qmgr->reg_config =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_CONFIG_REG_INDEX);
+		qmgr->reg_region =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_REGION_REG_INDEX);
+		qmgr->reg_push =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_PUSH_REG_INDEX);
+		qmgr->reg_pop =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_POP_REG_INDEX);
+
+		if (IS_ERR(qmgr->reg_peek) || IS_ERR(qmgr->reg_status) ||
+		    IS_ERR(qmgr->reg_config) || IS_ERR(qmgr->reg_region) ||
+		    IS_ERR(qmgr->reg_push) || IS_ERR(qmgr->reg_pop)) {
+			dev_err(dev, "failed to map qmgr regs\n");
+			if (!IS_ERR(qmgr->reg_peek))
+				devm_iounmap(dev, qmgr->reg_peek);
+			if (!IS_ERR(qmgr->reg_status))
+				devm_iounmap(dev, qmgr->reg_status);
+			if (!IS_ERR(qmgr->reg_config))
+				devm_iounmap(dev, qmgr->reg_config);
+			if (!IS_ERR(qmgr->reg_region))
+				devm_iounmap(dev, qmgr->reg_region);
+			if (!IS_ERR(qmgr->reg_push))
+				devm_iounmap(dev, qmgr->reg_push);
+			if (!IS_ERR(qmgr->reg_pop))
+				devm_iounmap(dev, qmgr->reg_pop);
+			devm_kfree(dev, qmgr);
+			continue;
+		}
+
+		list_add_tail(&qmgr->list, &kdev->qmgrs);
+		dev_info(dev, "added qmgr start queue %d, num of queues %d, reg_peek %p, reg_status %p, reg_config %p, reg_region %p, reg_push %p, reg_pop %p\n",
+			 qmgr->start_queue, qmgr->num_queues,
+			 qmgr->reg_peek, qmgr->reg_status,
+			 qmgr->reg_config, qmgr->reg_region,
+			 qmgr->reg_push, qmgr->reg_pop);
+	}
+	return 0;
+}
+
+static int knav_queue_init_pdsps(struct knav_device *kdev,
+					struct device_node *pdsps)
+{
+	struct device *dev = kdev->dev;
+	struct knav_pdsp_info *pdsp;
+	struct device_node *child;
+	int ret;
+
+	for_each_child_of_node(pdsps, child) {
+		pdsp = devm_kzalloc(dev, sizeof(*pdsp), GFP_KERNEL);
+		if (!pdsp) {
+			dev_err(dev, "out of memory allocating pdsp\n");
+			return -ENOMEM;
+		}
+		pdsp->name = knav_queue_find_name(child);
+		ret = of_property_read_string(child, "firmware",
+					      &pdsp->firmware);
+		if (ret < 0 || !pdsp->firmware) {
+			dev_err(dev, "unknown firmware for pdsp %s\n",
+				pdsp->name);
+			devm_kfree(dev, pdsp);
+			continue;
+		}
+		dev_dbg(dev, "pdsp name %s fw name :%s\n", pdsp->name,
+			pdsp->firmware);
+
+		pdsp->iram =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_PDSP_IRAM_REG_INDEX);
+		pdsp->regs =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_PDSP_REGS_REG_INDEX);
+		pdsp->intd =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_PDSP_INTD_REG_INDEX);
+		pdsp->command =
+			knav_queue_map_reg(kdev, child,
+					   KNAV_QUEUE_PDSP_CMD_REG_INDEX);
+
+		if (IS_ERR(pdsp->command) || IS_ERR(pdsp->iram) ||
+		    IS_ERR(pdsp->regs) || IS_ERR(pdsp->intd)) {
+			dev_err(dev, "failed to map pdsp %s regs\n",
+				pdsp->name);
+			if (!IS_ERR(pdsp->command))
+				devm_iounmap(dev, pdsp->command);
+			if (!IS_ERR(pdsp->iram))
+				devm_iounmap(dev, pdsp->iram);
+			if (!IS_ERR(pdsp->regs))
+				devm_iounmap(dev, pdsp->regs);
+			if (!IS_ERR(pdsp->intd))
+				devm_iounmap(dev, pdsp->intd);
+			devm_kfree(dev, pdsp);
+			continue;
+		}
+		of_property_read_u32(child, "id", &pdsp->id);
+		list_add_tail(&pdsp->list, &kdev->pdsps);
+		dev_dbg(dev, "added pdsp %s: command %p, iram %p, regs %p, intd %p, firmware %s\n",
+			pdsp->name, pdsp->command, pdsp->iram, pdsp->regs,
+			pdsp->intd, pdsp->firmware);
+	}
+	return 0;
+}
+
+static int knav_queue_stop_pdsp(struct knav_device *kdev,
+			  struct knav_pdsp_info *pdsp)
+{
+	u32 val, timeout = 1000;
+	int ret;
+
+	val = readl_relaxed(&pdsp->regs->control) & ~PDSP_CTRL_ENABLE;
+	writel_relaxed(val, &pdsp->regs->control);
+	ret = knav_queue_pdsp_wait(&pdsp->regs->control, timeout,
+					PDSP_CTRL_RUNNING);
+	if (ret < 0) {
+		dev_err(kdev->dev, "timed out on pdsp %s stop\n", pdsp->name);
+		return ret;
+	}
+	return 0;
+}
+
+static int knav_queue_load_pdsp(struct knav_device *kdev,
+			  struct knav_pdsp_info *pdsp)
+{
+	int i, ret, fwlen;
+	const struct firmware *fw;
+	u32 *fwdata;
+
+	ret = request_firmware(&fw, pdsp->firmware, kdev->dev);
+	if (ret) {
+		dev_err(kdev->dev, "failed to get firmware %s for pdsp %s\n",
+			pdsp->firmware, pdsp->name);
+		return ret;
+	}
+	writel_relaxed(pdsp->id + 1, pdsp->command + 0x18);
+	/* download the firmware */
+	fwdata = (u32 *)fw->data;
+	fwlen = (fw->size + sizeof(u32) - 1) / sizeof(u32);
+	for (i = 0; i < fwlen; i++)
+		writel_relaxed(be32_to_cpu(fwdata[i]), pdsp->iram + i);
+
+	release_firmware(fw);
+	return 0;
+}
+
+static int knav_queue_start_pdsp(struct knav_device *kdev,
+			   struct knav_pdsp_info *pdsp)
+{
+	u32 val, timeout = 1000;
+	int ret;
+
+	/* write a command for sync */
+	writel_relaxed(0xffffffff, pdsp->command);
+	while (readl_relaxed(pdsp->command) != 0xffffffff)
+		cpu_relax();
+
+	/* soft reset the PDSP */
+	val  = readl_relaxed(&pdsp->regs->control);
+	val &= ~(PDSP_CTRL_PC_MASK | PDSP_CTRL_SOFT_RESET);
+	writel_relaxed(val, &pdsp->regs->control);
+
+	/* enable pdsp */
+	val = readl_relaxed(&pdsp->regs->control) | PDSP_CTRL_ENABLE;
+	writel_relaxed(val, &pdsp->regs->control);
+
+	/* wait for command register to clear */
+	ret = knav_queue_pdsp_wait(pdsp->command, timeout, 0);
+	if (ret < 0) {
+		dev_err(kdev->dev,
+			"timed out on pdsp %s command register wait\n",
+			pdsp->name);
+		return ret;
+	}
+	return 0;
+}
+
+static void knav_queue_stop_pdsps(struct knav_device *kdev)
+{
+	struct knav_pdsp_info *pdsp;
+
+	/* disable all pdsps */
+	for_each_pdsp(kdev, pdsp)
+		knav_queue_stop_pdsp(kdev, pdsp);
+}
+
+static int knav_queue_start_pdsps(struct knav_device *kdev)
+{
+	struct knav_pdsp_info *pdsp;
+	int ret;
+
+	knav_queue_stop_pdsps(kdev);
+	/* now load them all */
+	for_each_pdsp(kdev, pdsp) {
+		ret = knav_queue_load_pdsp(kdev, pdsp);
+		if (ret < 0)
+			return ret;
+	}
+
+	for_each_pdsp(kdev, pdsp) {
+		ret = knav_queue_start_pdsp(kdev, pdsp);
+		WARN_ON(ret);
+	}
+	return 0;
+}
+
+static inline struct knav_qmgr_info *knav_find_qmgr(unsigned id)
+{
+	struct knav_qmgr_info *qmgr;
+
+	for_each_qmgr(kdev, qmgr) {
+		if ((id >= qmgr->start_queue) &&
+		    (id < qmgr->start_queue + qmgr->num_queues))
+			return qmgr;
+	}
+	return NULL;
+}
+
+static int knav_queue_init_queue(struct knav_device *kdev,
+					struct knav_range_info *range,
+					struct knav_queue_inst *inst,
+					unsigned id)
+{
+	char irq_name[KNAV_NAME_SIZE];
+	inst->qmgr = knav_find_qmgr(id);
+	if (!inst->qmgr)
+		return -1;
+
+	INIT_LIST_HEAD(&inst->handles);
+	inst->kdev = kdev;
+	inst->range = range;
+	inst->irq_num = -1;
+	inst->id = id;
+	scnprintf(irq_name, sizeof(irq_name), "hwqueue-%d", id);
+	inst->irq_name = kstrndup(irq_name, sizeof(irq_name), GFP_KERNEL);
+
+	if (range->ops && range->ops->init_queue)
+		return range->ops->init_queue(range, inst);
+	else
+		return 0;
+}
+
+static int knav_queue_init_queues(struct knav_device *kdev)
+{
+	struct knav_range_info *range;
+	int size, id, base_idx;
+	int idx = 0, ret = 0;
+
+	/* how much do we need for instance data? */
+	size = sizeof(struct knav_queue_inst);
+
+	/* round this up to a power of 2, keep the index to instance
+	 * arithmetic fast.
+	 * */
+	kdev->inst_shift = order_base_2(size);
+	size = (1 << kdev->inst_shift) * kdev->num_queues_in_use;
+	kdev->instances = devm_kzalloc(kdev->dev, size, GFP_KERNEL);
+	if (!kdev->instances)
+		return -1;
+
+	for_each_queue_range(kdev, range) {
+		if (range->ops && range->ops->init_range)
+			range->ops->init_range(range);
+		base_idx = idx;
+		for (id = range->queue_base;
+		     id < range->queue_base + range->num_queues; id++, idx++) {
+			ret = knav_queue_init_queue(kdev, range,
+					knav_queue_idx_to_inst(kdev, idx), id);
+			if (ret < 0)
+				return ret;
+		}
+		range->queue_base_inst =
+			knav_queue_idx_to_inst(kdev, base_idx);
+	}
+	return 0;
+}
+
+static int knav_queue_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct device_node *qmgrs, *queue_pools, *regions, *pdsps;
+	struct device *dev = &pdev->dev;
+	u32 temp[2];
+	int ret;
+
+	if (!node) {
+		dev_err(dev, "device tree info unavailable\n");
+		return -ENODEV;
+	}
+
+	kdev = devm_kzalloc(dev, sizeof(struct knav_device), GFP_KERNEL);
+	if (!kdev) {
+		dev_err(dev, "memory allocation failed\n");
+		return -ENOMEM;
+	}
+
+	platform_set_drvdata(pdev, kdev);
+	kdev->dev = dev;
+	INIT_LIST_HEAD(&kdev->queue_ranges);
+	INIT_LIST_HEAD(&kdev->qmgrs);
+	INIT_LIST_HEAD(&kdev->pools);
+	INIT_LIST_HEAD(&kdev->regions);
+	INIT_LIST_HEAD(&kdev->pdsps);
+
+	pm_runtime_enable(&pdev->dev);
+	ret = pm_runtime_get_sync(&pdev->dev);
+	if (ret < 0) {
+		dev_err(dev, "Failed to enable QMSS\n");
+		return ret;
+	}
+
+	if (of_property_read_u32_array(node, "queue-range", temp, 2)) {
+		dev_err(dev, "queue-range not specified\n");
+		ret = -ENODEV;
+		goto err;
+	}
+	kdev->base_id    = temp[0];
+	kdev->num_queues = temp[1];
+
+	/* Initialize queue managers using device tree configuration */
+	qmgrs =  of_get_child_by_name(node, "qmgrs");
+	if (!qmgrs) {
+		dev_err(dev, "queue manager info not specified\n");
+		ret = -ENODEV;
+		goto err;
+	}
+	ret = knav_queue_init_qmgrs(kdev, qmgrs);
+	of_node_put(qmgrs);
+	if (ret)
+		goto err;
+
+	/* get pdsp configuration values from device tree */
+	pdsps =  of_get_child_by_name(node, "pdsps");
+	if (pdsps) {
+		ret = knav_queue_init_pdsps(kdev, pdsps);
+		if (ret)
+			goto err;
+
+		ret = knav_queue_start_pdsps(kdev);
+		if (ret)
+			goto err;
+	}
+	of_node_put(pdsps);
+
+	/* get usable queue range values from device tree */
+	queue_pools = of_get_child_by_name(node, "queue-pools");
+	if (!queue_pools) {
+		dev_err(dev, "queue-pools not specified\n");
+		ret = -ENODEV;
+		goto err;
+	}
+	ret = knav_setup_queue_pools(kdev, queue_pools);
+	of_node_put(queue_pools);
+	if (ret)
+		goto err;
+
+	ret = knav_get_link_ram(kdev, "linkram0", &kdev->link_rams[0]);
+	if (ret) {
+		dev_err(kdev->dev, "could not setup linking ram\n");
+		goto err;
+	}
+
+	ret = knav_get_link_ram(kdev, "linkram1", &kdev->link_rams[1]);
+	if (ret) {
+		/*
+		 * nothing really, we have one linking ram already, so we just
+		 * live within our means
+		 */
+	}
+
+	ret = knav_queue_setup_link_ram(kdev);
+	if (ret)
+		goto err;
+
+	regions =  of_get_child_by_name(node, "descriptor-regions");
+	if (!regions) {
+		dev_err(dev, "descriptor-regions not specified\n");
+		goto err;
+	}
+	ret = knav_queue_setup_regions(kdev, regions);
+	of_node_put(regions);
+	if (ret)
+		goto err;
+
+	ret = knav_queue_init_queues(kdev);
+	if (ret < 0) {
+		dev_err(dev, "hwqueue initialization failed\n");
+		goto err;
+	}
+
+	debugfs_create_file("qmss", S_IFREG | S_IRUGO, NULL, NULL,
+			    &knav_queue_debug_ops);
+	return 0;
+
+err:
+	knav_queue_stop_pdsps(kdev);
+	knav_queue_free_regions(kdev);
+	knav_free_queue_ranges(kdev);
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	return ret;
+}
+
+static int knav_queue_remove(struct platform_device *pdev)
+{
+	/* TODO: Free resources */
+	pm_runtime_put_sync(&pdev->dev);
+	pm_runtime_disable(&pdev->dev);
+	return 0;
+}
+
+/* Match table for of_platform binding */
+static struct of_device_id keystone_qmss_of_match[] = {
+	{ .compatible = "ti,keystone-navigator-qmss", },
+	{},
+};
+MODULE_DEVICE_TABLE(of, keystone_qmss_of_match);
+
+static struct platform_driver keystone_qmss_driver = {
+	.probe		= knav_queue_probe,
+	.remove		= knav_queue_remove,
+	.driver		= {
+		.name	= "keystone-navigator-qmss",
+		.owner	= THIS_MODULE,
+		.of_match_table = keystone_qmss_of_match,
+	},
+};
+module_platform_driver(keystone_qmss_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("TI QMSS driver for Keystone SOCs");
+MODULE_AUTHOR("Sandeep Nair <sandeep_n@ti.com>");
+MODULE_AUTHOR("Santosh Shilimkar <santosh.shilimkar@ti.com>");
diff --git a/drivers/soc/versatile/Kconfig b/drivers/soc/versatile/Kconfig
new file mode 100644
index 000000000000..bf5ee9c85330
--- /dev/null
+++ b/drivers/soc/versatile/Kconfig
@@ -0,0 +1,10 @@
+#
+# ARM Versatile SoC drivers
+#
+config SOC_REALVIEW
+	bool "SoC bus device for the ARM RealView platforms"
+	depends on ARCH_REALVIEW
+	select SOC_BUS
+	help
+	  Include support for the SoC bus on the ARM RealView platforms
+	  providing some sysfs information about the ASIC variant.
diff --git a/drivers/soc/versatile/Makefile b/drivers/soc/versatile/Makefile
new file mode 100644
index 000000000000..ad547435648e
--- /dev/null
+++ b/drivers/soc/versatile/Makefile
@@ -0,0 +1 @@
+obj-$(CONFIG_SOC_REALVIEW)	+= soc-realview.o
diff --git a/drivers/soc/versatile/soc-realview.c b/drivers/soc/versatile/soc-realview.c
new file mode 100644
index 000000000000..cea8ea3491d2
--- /dev/null
+++ b/drivers/soc/versatile/soc-realview.c
@@ -0,0 +1,144 @@
+/*
+ * Copyright (C) 2014 Linaro Ltd.
+ *
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2, as
+ * published by the Free Software Foundation.
+ *
+ */
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/syscon.h>
+#include <linux/regmap.h>
+#include <linux/of.h>
+
+/* System ID in syscon */
+#define REALVIEW_SYS_ID_OFFSET 0x00
+
+static const struct of_device_id realview_soc_of_match[] = {
+	{ .compatible = "arm,realview-eb-soc",	},
+	{ .compatible = "arm,realview-pb1176-soc", },
+	{ .compatible = "arm,realview-pb11mp-soc", },
+	{ .compatible = "arm,realview-pba8-soc", },
+	{ .compatible = "arm,realview-pbx-soc", },
+};
+
+static u32 realview_coreid;
+
+static const char *realview_board_str(u32 id)
+{
+	switch ((id >> 16) & 0xfff) {
+	case 0x0147:
+		return "HBI-0147";
+	default:
+		return "Unknown";
+	}
+}
+
+static const char *realview_arch_str(u32 id)
+{
+	switch ((id >> 8) & 0xf) {
+	case 0x05:
+		return "Multi-layer AXI";
+	default:
+		return "Unknown";
+	}
+}
+
+static ssize_t realview_get_manf(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	return sprintf(buf, "%02x\n", realview_coreid >> 24);
+}
+
+static struct device_attribute realview_manf_attr =
+	__ATTR(manufacturer,  S_IRUGO, realview_get_manf,  NULL);
+
+static ssize_t realview_get_board(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	return sprintf(buf, "%s\n", realview_board_str(realview_coreid));
+}
+
+static struct device_attribute realview_board_attr =
+	__ATTR(board,  S_IRUGO, realview_get_board,  NULL);
+
+static ssize_t realview_get_arch(struct device *dev,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	return sprintf(buf, "%s\n", realview_arch_str(realview_coreid));
+}
+
+static struct device_attribute realview_arch_attr =
+	__ATTR(fpga,  S_IRUGO, realview_get_arch,  NULL);
+
+static ssize_t realview_get_build(struct device *dev,
+			       struct device_attribute *attr,
+			       char *buf)
+{
+	return sprintf(buf, "%02x\n", (realview_coreid & 0xFF));
+}
+
+static struct device_attribute realview_build_attr =
+	__ATTR(build,  S_IRUGO, realview_get_build,  NULL);
+
+static int realview_soc_probe(struct platform_device *pdev)
+{
+	static struct regmap *syscon_regmap;
+	struct soc_device *soc_dev;
+	struct soc_device_attribute *soc_dev_attr;
+	struct device_node *np = pdev->dev.of_node;
+	int ret;
+
+	syscon_regmap = syscon_regmap_lookup_by_phandle(np, "regmap");
+	if (IS_ERR(syscon_regmap))
+		return PTR_ERR(syscon_regmap);
+
+	soc_dev_attr = kzalloc(sizeof(*soc_dev_attr), GFP_KERNEL);
+	if (!soc_dev_attr)
+		return -ENOMEM;
+
+	ret = of_property_read_string(np, "compatible",
+				      &soc_dev_attr->soc_id);
+	if (ret)
+		return -EINVAL;
+
+	soc_dev_attr->machine = "RealView";
+	soc_dev_attr->family = "Versatile";
+	soc_dev = soc_device_register(soc_dev_attr);
+	if (IS_ERR(soc_dev)) {
+		kfree(soc_dev_attr);
+		return -ENODEV;
+	}
+	ret = regmap_read(syscon_regmap, REALVIEW_SYS_ID_OFFSET,
+			  &realview_coreid);
+	if (ret)
+		return -ENODEV;
+
+	device_create_file(soc_device_to_device(soc_dev), &realview_manf_attr);
+	device_create_file(soc_device_to_device(soc_dev), &realview_board_attr);
+	device_create_file(soc_device_to_device(soc_dev), &realview_arch_attr);
+	device_create_file(soc_device_to_device(soc_dev), &realview_build_attr);
+
+	dev_info(&pdev->dev, "RealView Syscon Core ID: 0x%08x\n",
+		 realview_coreid);
+	/* FIXME: add attributes for SoC to sysfs */
+	return 0;
+}
+
+static struct platform_driver realview_soc_driver = {
+	.probe = realview_soc_probe,
+	.driver = {
+		.name = "realview-soc",
+		.of_match_table = realview_soc_of_match,
+	},
+};
+module_platform_driver(realview_soc_driver);
diff --git a/drivers/spi/Kconfig b/drivers/spi/Kconfig
index 62e2242ad7e0..84e7c9e6ccef 100644
--- a/drivers/spi/Kconfig
+++ b/drivers/spi/Kconfig
@@ -69,6 +69,7 @@ config SPI_ATH79
 
 config SPI_ATMEL
 	tristate "Atmel SPI Controller"
+	depends on HAS_DMA
 	depends on (ARCH_AT91 || AVR32 || COMPILE_TEST)
 	help
 	  This selects a driver for the Atmel SPI Controller, present on
@@ -112,6 +113,14 @@ config SPI_AU1550
 	  If you say yes to this option, support will be included for the
 	  PSC SPI controller found on Au1550, Au1200 and Au1300 series.
 
+config SPI_BCM53XX
+	tristate "Broadcom BCM53xx SPI controller"
+	depends on ARCH_BCM_5301X
+	depends on BCMA_POSSIBLE
+	select BCMA
+	help
+          Enable support for the SPI controller on Broadcom BCM53xx ARM SoCs.
+
 config SPI_BCM63XX
 	tristate "Broadcom BCM63xx SPI controller"
 	depends on BCM63XX
@@ -185,6 +194,7 @@ config SPI_EFM32
 
 config SPI_EP93XX
 	tristate "Cirrus Logic EP93xx SPI controller"
+	depends on HAS_DMA
 	depends on ARCH_EP93XX || COMPILE_TEST
 	help
 	  This enables using the Cirrus EP93xx SPI controller in master
@@ -314,6 +324,7 @@ config SPI_OMAP_UWIRE
 
 config SPI_OMAP24XX
 	tristate "McSPI driver for OMAP"
+	depends on HAS_DMA
 	depends on ARM || ARM64 || AVR32 || HEXAGON || MIPS || SUPERH
 	depends on ARCH_OMAP2PLUS || COMPILE_TEST
 	help
@@ -380,7 +391,7 @@ config SPI_PXA2XX
 	  additional documentation can be found a Documentation/spi/pxa2xx.
 
 config SPI_PXA2XX_PCI
-	def_tristate SPI_PXA2XX && PCI
+	def_tristate SPI_PXA2XX && PCI && COMMON_CLK
 
 config SPI_ROCKCHIP
 	tristate "Rockchip SPI controller driver"
@@ -500,7 +511,7 @@ config SPI_MXS
 config SPI_TEGRA114
 	tristate "NVIDIA Tegra114 SPI Controller"
 	depends on (ARCH_TEGRA && TEGRA20_APB_DMA) || COMPILE_TEST
-	depends on RESET_CONTROLLER
+	depends on RESET_CONTROLLER && HAS_DMA
 	help
 	  SPI driver for NVIDIA Tegra114 SPI Controller interface. This controller
 	  is different than the older SoCs SPI controller and also register interface
@@ -518,7 +529,7 @@ config SPI_TEGRA20_SFLASH
 config SPI_TEGRA20_SLINK
 	tristate "Nvidia Tegra20/Tegra30 SLINK Controller"
 	depends on (ARCH_TEGRA && TEGRA20_APB_DMA) || COMPILE_TEST
-	depends on RESET_CONTROLLER
+	depends on RESET_CONTROLLER && HAS_DMA
 	help
 	  SPI driver for Nvidia Tegra20/Tegra30 SLINK Controller interface.
 
@@ -591,7 +602,7 @@ config SPI_DW_PCI
 	depends on SPI_DESIGNWARE && PCI
 
 config SPI_DW_MID_DMA
-	bool "DMA support for DW SPI controller on Intel Moorestown platform"
+	bool "DMA support for DW SPI controller on Intel MID platform"
 	depends on SPI_DW_PCI && INTEL_MID_DMAC
 
 config SPI_DW_MMIO
diff --git a/drivers/spi/Makefile b/drivers/spi/Makefile
index 762da0741148..78f24ca36fcf 100644
--- a/drivers/spi/Makefile
+++ b/drivers/spi/Makefile
@@ -15,6 +15,7 @@ obj-$(CONFIG_SPI_ATMEL)			+= spi-atmel.o
 obj-$(CONFIG_SPI_ATH79)			+= spi-ath79.o
 obj-$(CONFIG_SPI_AU1550)		+= spi-au1550.o
 obj-$(CONFIG_SPI_BCM2835)		+= spi-bcm2835.o
+obj-$(CONFIG_SPI_BCM53XX)		+= spi-bcm53xx.o
 obj-$(CONFIG_SPI_BCM63XX)		+= spi-bcm63xx.o
 obj-$(CONFIG_SPI_BCM63XX_HSSPI)		+= spi-bcm63xx-hsspi.o
 obj-$(CONFIG_SPI_BFIN5XX)		+= spi-bfin5xx.o
diff --git a/drivers/spi/spi-au1550.c b/drivers/spi/spi-au1550.c
index 40c3d43c9292..f40b34cdf2fc 100644
--- a/drivers/spi/spi-au1550.c
+++ b/drivers/spi/spi-au1550.c
@@ -945,7 +945,7 @@ static int au1550_spi_remove(struct platform_device *pdev)
 	spi_bitbang_stop(&hw->bitbang);
 	free_irq(hw->irq, hw);
 	iounmap((void __iomem *)hw->regs);
-	release_mem_region(r->start, sizeof(psc_spi_t));
+	release_mem_region(hw->ioarea->start, sizeof(psc_spi_t));
 
 	if (hw->usedma) {
 		au1550_spi_dma_rxtmp_free(hw);
diff --git a/drivers/spi/spi-bcm53xx.c b/drivers/spi/spi-bcm53xx.c
new file mode 100644
index 000000000000..17b34cbadc03
--- /dev/null
+++ b/drivers/spi/spi-bcm53xx.c
@@ -0,0 +1,299 @@
+#define pr_fmt(fmt)		KBUILD_MODNAME ": " fmt
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/bcma/bcma.h>
+#include <linux/spi/spi.h>
+
+#include "spi-bcm53xx.h"
+
+#define BCM53XXSPI_MAX_SPI_BAUD	13500000	/* 216 MHz? */
+
+/* The longest observed required wait was 19 ms */
+#define BCM53XXSPI_SPE_TIMEOUT_MS	80
+
+struct bcm53xxspi {
+	struct bcma_device *core;
+	struct spi_master *master;
+
+	size_t read_offset;
+};
+
+static inline u32 bcm53xxspi_read(struct bcm53xxspi *b53spi, u16 offset)
+{
+	return bcma_read32(b53spi->core, offset);
+}
+
+static inline void bcm53xxspi_write(struct bcm53xxspi *b53spi, u16 offset,
+				    u32 value)
+{
+	bcma_write32(b53spi->core, offset, value);
+}
+
+static inline unsigned int bcm53xxspi_calc_timeout(size_t len)
+{
+	/* Do some magic calculation based on length and buad. Add 10% and 1. */
+	return (len * 9000 / BCM53XXSPI_MAX_SPI_BAUD * 110 / 100) + 1;
+}
+
+static int bcm53xxspi_wait(struct bcm53xxspi *b53spi, unsigned int timeout_ms)
+{
+	unsigned long deadline;
+	u32 tmp;
+
+	/* SPE bit has to be 0 before we read MSPI STATUS */
+	deadline = jiffies + BCM53XXSPI_SPE_TIMEOUT_MS * HZ / 1000;
+	do {
+		tmp = bcm53xxspi_read(b53spi, B53SPI_MSPI_SPCR2);
+		if (!(tmp & B53SPI_MSPI_SPCR2_SPE))
+			break;
+		udelay(5);
+	} while (!time_after_eq(jiffies, deadline));
+
+	if (tmp & B53SPI_MSPI_SPCR2_SPE)
+		goto spi_timeout;
+
+	/* Check status */
+	deadline = jiffies + timeout_ms * HZ / 1000;
+	do {
+		tmp = bcm53xxspi_read(b53spi, B53SPI_MSPI_MSPI_STATUS);
+		if (tmp & B53SPI_MSPI_MSPI_STATUS_SPIF) {
+			bcm53xxspi_write(b53spi, B53SPI_MSPI_MSPI_STATUS, 0);
+			return 0;
+		}
+
+		cpu_relax();
+		udelay(100);
+	} while (!time_after_eq(jiffies, deadline));
+
+spi_timeout:
+	bcm53xxspi_write(b53spi, B53SPI_MSPI_MSPI_STATUS, 0);
+
+	pr_err("Timeout waiting for SPI to be ready!\n");
+
+	return -EBUSY;
+}
+
+static void bcm53xxspi_buf_write(struct bcm53xxspi *b53spi, u8 *w_buf,
+				 size_t len, bool cont)
+{
+	u32 tmp;
+	int i;
+
+	for (i = 0; i < len; i++) {
+		/* Transmit Register File MSB */
+		bcm53xxspi_write(b53spi, B53SPI_MSPI_TXRAM + 4 * (i * 2),
+				 (unsigned int)w_buf[i]);
+	}
+
+	for (i = 0; i < len; i++) {
+		tmp = B53SPI_CDRAM_CONT | B53SPI_CDRAM_PCS_DISABLE_ALL |
+		      B53SPI_CDRAM_PCS_DSCK;
+		if (!cont && i == len - 1)
+			tmp &= ~B53SPI_CDRAM_CONT;
+		tmp &= ~0x1;
+		/* Command Register File */
+		bcm53xxspi_write(b53spi, B53SPI_MSPI_CDRAM + 4 * i, tmp);
+	}
+
+	/* Set queue pointers */
+	bcm53xxspi_write(b53spi, B53SPI_MSPI_NEWQP, 0);
+	bcm53xxspi_write(b53spi, B53SPI_MSPI_ENDQP, len - 1);
+
+	if (cont)
+		bcm53xxspi_write(b53spi, B53SPI_MSPI_WRITE_LOCK, 1);
+
+	/* Start SPI transfer */
+	tmp = bcm53xxspi_read(b53spi, B53SPI_MSPI_SPCR2);
+	tmp |= B53SPI_MSPI_SPCR2_SPE;
+	if (cont)
+		tmp |= B53SPI_MSPI_SPCR2_CONT_AFTER_CMD;
+	bcm53xxspi_write(b53spi, B53SPI_MSPI_SPCR2, tmp);
+
+	/* Wait for SPI to finish */
+	bcm53xxspi_wait(b53spi, bcm53xxspi_calc_timeout(len));
+
+	if (!cont)
+		bcm53xxspi_write(b53spi, B53SPI_MSPI_WRITE_LOCK, 0);
+
+	b53spi->read_offset = len;
+}
+
+static void bcm53xxspi_buf_read(struct bcm53xxspi *b53spi, u8 *r_buf,
+				size_t len, bool cont)
+{
+	u32 tmp;
+	int i;
+
+	for (i = 0; i < b53spi->read_offset + len; i++) {
+		tmp = B53SPI_CDRAM_CONT | B53SPI_CDRAM_PCS_DISABLE_ALL |
+		      B53SPI_CDRAM_PCS_DSCK;
+		if (!cont && i == b53spi->read_offset + len - 1)
+			tmp &= ~B53SPI_CDRAM_CONT;
+		tmp &= ~0x1;
+		/* Command Register File */
+		bcm53xxspi_write(b53spi, B53SPI_MSPI_CDRAM + 4 * i, tmp);
+	}
+
+	/* Set queue pointers */
+	bcm53xxspi_write(b53spi, B53SPI_MSPI_NEWQP, 0);
+	bcm53xxspi_write(b53spi, B53SPI_MSPI_ENDQP,
+			 b53spi->read_offset + len - 1);
+
+	if (cont)
+		bcm53xxspi_write(b53spi, B53SPI_MSPI_WRITE_LOCK, 1);
+
+	/* Start SPI transfer */
+	tmp = bcm53xxspi_read(b53spi, B53SPI_MSPI_SPCR2);
+	tmp |= B53SPI_MSPI_SPCR2_SPE;
+	if (cont)
+		tmp |= B53SPI_MSPI_SPCR2_CONT_AFTER_CMD;
+	bcm53xxspi_write(b53spi, B53SPI_MSPI_SPCR2, tmp);
+
+	/* Wait for SPI to finish */
+	bcm53xxspi_wait(b53spi, bcm53xxspi_calc_timeout(len));
+
+	if (!cont)
+		bcm53xxspi_write(b53spi, B53SPI_MSPI_WRITE_LOCK, 0);
+
+	for (i = 0; i < len; ++i) {
+		int offset = b53spi->read_offset + i;
+
+		/* Data stored in the transmit register file LSB */
+		r_buf[i] = (u8)bcm53xxspi_read(b53spi, B53SPI_MSPI_RXRAM + 4 * (1 + offset * 2));
+	}
+
+	b53spi->read_offset = 0;
+}
+
+static int bcm53xxspi_transfer_one(struct spi_master *master,
+				   struct spi_device *spi,
+				   struct spi_transfer *t)
+{
+	struct bcm53xxspi *b53spi = spi_master_get_devdata(master);
+	u8 *buf;
+	size_t left;
+
+	if (t->tx_buf) {
+		buf = (u8 *)t->tx_buf;
+		left = t->len;
+		while (left) {
+			size_t to_write = min_t(size_t, 16, left);
+			bool cont = left - to_write > 0;
+
+			bcm53xxspi_buf_write(b53spi, buf, to_write, cont);
+			left -= to_write;
+			buf += to_write;
+		}
+	}
+
+	if (t->rx_buf) {
+		buf = (u8 *)t->rx_buf;
+		left = t->len;
+		while (left) {
+			size_t to_read = min_t(size_t, 16 - b53spi->read_offset,
+					       left);
+			bool cont = left - to_read > 0;
+
+			bcm53xxspi_buf_read(b53spi, buf, to_read, cont);
+			left -= to_read;
+			buf += to_read;
+		}
+	}
+
+	return 0;
+}
+
+/**************************************************
+ * BCMA
+ **************************************************/
+
+static struct spi_board_info bcm53xx_info = {
+	.modalias	= "bcm53xxspiflash",
+};
+
+static const struct bcma_device_id bcm53xxspi_bcma_tbl[] = {
+	BCMA_CORE(BCMA_MANUF_BCM, BCMA_CORE_NS_QSPI, BCMA_ANY_REV, BCMA_ANY_CLASS),
+	BCMA_CORETABLE_END
+};
+MODULE_DEVICE_TABLE(bcma, bcm53xxspi_bcma_tbl);
+
+static int bcm53xxspi_bcma_probe(struct bcma_device *core)
+{
+	struct bcm53xxspi *b53spi;
+	struct spi_master *master;
+	int err;
+
+	if (core->bus->drv_cc.core->id.rev != 42) {
+		pr_err("SPI on SoC with unsupported ChipCommon rev\n");
+		return -ENOTSUPP;
+	}
+
+	master = spi_alloc_master(&core->dev, sizeof(*b53spi));
+	if (!master)
+		return -ENOMEM;
+
+	b53spi = spi_master_get_devdata(master);
+	b53spi->master = master;
+	b53spi->core = core;
+
+	master->transfer_one = bcm53xxspi_transfer_one;
+
+	bcma_set_drvdata(core, b53spi);
+
+	err = devm_spi_register_master(&core->dev, master);
+	if (err) {
+		spi_master_put(master);
+		bcma_set_drvdata(core, NULL);
+		goto out;
+	}
+
+	/* Broadcom SoCs (at least with the CC rev 42) use SPI for flash only */
+	spi_new_device(master, &bcm53xx_info);
+
+out:
+	return err;
+}
+
+static void bcm53xxspi_bcma_remove(struct bcma_device *core)
+{
+	struct bcm53xxspi *b53spi = bcma_get_drvdata(core);
+
+	spi_unregister_master(b53spi->master);
+}
+
+static struct bcma_driver bcm53xxspi_bcma_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= bcm53xxspi_bcma_tbl,
+	.probe		= bcm53xxspi_bcma_probe,
+	.remove		= bcm53xxspi_bcma_remove,
+};
+
+/**************************************************
+ * Init & exit
+ **************************************************/
+
+static int __init bcm53xxspi_module_init(void)
+{
+	int err = 0;
+
+	err = bcma_driver_register(&bcm53xxspi_bcma_driver);
+	if (err)
+		pr_err("Failed to register bcma driver: %d\n", err);
+
+	return err;
+}
+
+static void __exit bcm53xxspi_module_exit(void)
+{
+	bcma_driver_unregister(&bcm53xxspi_bcma_driver);
+}
+
+module_init(bcm53xxspi_module_init);
+module_exit(bcm53xxspi_module_exit);
+
+MODULE_DESCRIPTION("Broadcom BCM53xx SPI Controller driver");
+MODULE_AUTHOR("Rafał Miłecki <zajec5@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/spi/spi-bcm53xx.h b/drivers/spi/spi-bcm53xx.h
new file mode 100644
index 000000000000..73575dfe6916
--- /dev/null
+++ b/drivers/spi/spi-bcm53xx.h
@@ -0,0 +1,72 @@
+#ifndef SPI_BCM53XX_H
+#define SPI_BCM53XX_H
+
+#define B53SPI_BSPI_REVISION_ID			0x000
+#define B53SPI_BSPI_SCRATCH			0x004
+#define B53SPI_BSPI_MAST_N_BOOT_CTRL		0x008
+#define B53SPI_BSPI_BUSY_STATUS			0x00c
+#define B53SPI_BSPI_INTR_STATUS			0x010
+#define B53SPI_BSPI_B0_STATUS			0x014
+#define B53SPI_BSPI_B0_CTRL			0x018
+#define B53SPI_BSPI_B1_STATUS			0x01c
+#define B53SPI_BSPI_B1_CTRL			0x020
+#define B53SPI_BSPI_STRAP_OVERRIDE_CTRL		0x024
+#define B53SPI_BSPI_FLEX_MODE_ENABLE		0x028
+#define B53SPI_BSPI_BITS_PER_CYCLE		0x02c
+#define B53SPI_BSPI_BITS_PER_PHASE		0x030
+#define B53SPI_BSPI_CMD_AND_MODE_BYTE		0x034
+#define B53SPI_BSPI_BSPI_FLASH_UPPER_ADDR_BYTE	0x038
+#define B53SPI_BSPI_BSPI_XOR_VALUE		0x03c
+#define B53SPI_BSPI_BSPI_XOR_ENABLE		0x040
+#define B53SPI_BSPI_BSPI_PIO_MODE_ENABLE	0x044
+#define B53SPI_BSPI_BSPI_PIO_IODIR		0x048
+#define B53SPI_BSPI_BSPI_PIO_DATA		0x04c
+
+/* RAF */
+#define B53SPI_RAF_START_ADDR			0x100
+#define B53SPI_RAF_NUM_WORDS			0x104
+#define B53SPI_RAF_CTRL				0x108
+#define B53SPI_RAF_FULLNESS			0x10c
+#define B53SPI_RAF_WATERMARK			0x110
+#define B53SPI_RAF_STATUS			0x114
+#define B53SPI_RAF_READ_DATA			0x118
+#define B53SPI_RAF_WORD_CNT			0x11c
+#define B53SPI_RAF_CURR_ADDR			0x120
+
+/* MSPI */
+#define B53SPI_MSPI_SPCR0_LSB			0x200
+#define B53SPI_MSPI_SPCR0_MSB			0x204
+#define B53SPI_MSPI_SPCR1_LSB			0x208
+#define B53SPI_MSPI_SPCR1_MSB			0x20c
+#define B53SPI_MSPI_NEWQP			0x210
+#define B53SPI_MSPI_ENDQP			0x214
+#define B53SPI_MSPI_SPCR2			0x218
+#define  B53SPI_MSPI_SPCR2_SPE			0x00000040
+#define  B53SPI_MSPI_SPCR2_CONT_AFTER_CMD	0x00000080
+#define B53SPI_MSPI_MSPI_STATUS			0x220
+#define  B53SPI_MSPI_MSPI_STATUS_SPIF		0x00000001
+#define B53SPI_MSPI_CPTQP			0x224
+#define B53SPI_MSPI_TXRAM			0x240 /* 32 registers, up to 0x2b8 */
+#define B53SPI_MSPI_RXRAM			0x2c0 /* 32 registers, up to 0x33c */
+#define B53SPI_MSPI_CDRAM			0x340 /* 16 registers, up to 0x37c */
+#define  B53SPI_CDRAM_PCS_PCS0			0x00000001
+#define  B53SPI_CDRAM_PCS_PCS1			0x00000002
+#define  B53SPI_CDRAM_PCS_PCS2			0x00000004
+#define  B53SPI_CDRAM_PCS_PCS3			0x00000008
+#define  B53SPI_CDRAM_PCS_DISABLE_ALL		0x0000000f
+#define  B53SPI_CDRAM_PCS_DSCK			0x00000010
+#define  B53SPI_CDRAM_BITSE			0x00000040
+#define  B53SPI_CDRAM_CONT			0x00000080
+#define B53SPI_MSPI_WRITE_LOCK			0x380
+#define B53SPI_MSPI_DISABLE_FLUSH_GEN		0x384
+
+/* Interrupt */
+#define B53SPI_INTR_RAF_LR_FULLNESS_REACHED	0x3a0
+#define B53SPI_INTR_RAF_LR_TRUNCATED		0x3a4
+#define B53SPI_INTR_RAF_LR_IMPATIENT		0x3a8
+#define B53SPI_INTR_RAF_LR_SESSION_DONE		0x3ac
+#define B53SPI_INTR_RAF_LR_OVERREAD		0x3b0
+#define B53SPI_INTR_MSPI_DONE			0x3b4
+#define B53SPI_INTR_MSPI_HALT_SET_TRANSACTION_DONE	0x3b8
+
+#endif /* SPI_BCM53XX_H */
diff --git a/drivers/spi/spi-cadence.c b/drivers/spi/spi-cadence.c
index 562ff83debd9..7b811e38c7ad 100644
--- a/drivers/spi/spi-cadence.c
+++ b/drivers/spi/spi-cadence.c
@@ -677,7 +677,6 @@ static struct platform_driver cdns_spi_driver = {
 	.remove	= cdns_spi_remove,
 	.driver = {
 		.name = CDNS_SPI_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = cdns_spi_of_match,
 		.pm = &cdns_spi_dev_pm_ops,
 	},
diff --git a/drivers/spi/spi-clps711x.c b/drivers/spi/spi-clps711x.c
index ce538dad526b..181cf2262006 100644
--- a/drivers/spi/spi-clps711x.c
+++ b/drivers/spi/spi-clps711x.c
@@ -30,7 +30,6 @@
 struct spi_clps711x_data {
 	void __iomem		*syncio;
 	struct regmap		*syscon;
-	struct regmap		*syscon1;
 	struct clk		*spi_clk;
 
 	u8			*tx_buf;
@@ -47,27 +46,6 @@ static int spi_clps711x_setup(struct spi_device *spi)
 	return 0;
 }
 
-static void spi_clps711x_setup_xfer(struct spi_device *spi,
-				    struct spi_transfer *xfer)
-{
-	struct spi_master *master = spi->master;
-	struct spi_clps711x_data *hw = spi_master_get_devdata(master);
-
-	/* Setup SPI frequency divider */
-	if (xfer->speed_hz >= master->max_speed_hz)
-		regmap_update_bits(hw->syscon1, SYSCON_OFFSET,
-				   SYSCON1_ADCKSEL_MASK, SYSCON1_ADCKSEL(3));
-	else if (xfer->speed_hz >= (master->max_speed_hz / 2))
-		regmap_update_bits(hw->syscon1, SYSCON_OFFSET,
-				   SYSCON1_ADCKSEL_MASK, SYSCON1_ADCKSEL(2));
-	else if (xfer->speed_hz >= (master->max_speed_hz / 8))
-		regmap_update_bits(hw->syscon1, SYSCON_OFFSET,
-				   SYSCON1_ADCKSEL_MASK, SYSCON1_ADCKSEL(1));
-	else
-		regmap_update_bits(hw->syscon1, SYSCON_OFFSET,
-				   SYSCON1_ADCKSEL_MASK, SYSCON1_ADCKSEL(0));
-}
-
 static int spi_clps711x_prepare_message(struct spi_master *master,
 					struct spi_message *msg)
 {
@@ -87,7 +65,7 @@ static int spi_clps711x_transfer_one(struct spi_master *master,
 	struct spi_clps711x_data *hw = spi_master_get_devdata(master);
 	u8 data;
 
-	spi_clps711x_setup_xfer(spi, xfer);
+	clk_set_rate(hw->spi_clk, xfer->speed_hz ? : spi->max_speed_hz);
 
 	hw->len = xfer->len;
 	hw->bpw = xfer->bits_per_word;
@@ -176,13 +154,11 @@ static int spi_clps711x_probe(struct platform_device *pdev)
 		}
 	}
 
-	hw->spi_clk = devm_clk_get(&pdev->dev, "spi");
+	hw->spi_clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(hw->spi_clk)) {
-		dev_err(&pdev->dev, "Can't get clocks\n");
 		ret = PTR_ERR(hw->spi_clk);
 		goto err_out;
 	}
-	master->max_speed_hz = clk_get_rate(hw->spi_clk);
 
 	hw->syscon = syscon_regmap_lookup_by_pdevname("syscon.3");
 	if (IS_ERR(hw->syscon)) {
@@ -190,12 +166,6 @@ static int spi_clps711x_probe(struct platform_device *pdev)
 		goto err_out;
 	}
 
-	hw->syscon1 = syscon_regmap_lookup_by_pdevname("syscon.1");
-	if (IS_ERR(hw->syscon1)) {
-		ret = PTR_ERR(hw->syscon1);
-		goto err_out;
-	}
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	hw->syncio = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(hw->syncio)) {
diff --git a/drivers/spi/spi-davinci.c b/drivers/spi/spi-davinci.c
index 276a3884fb3c..63700ab7bd9f 100644
--- a/drivers/spi/spi-davinci.c
+++ b/drivers/spi/spi-davinci.c
@@ -65,6 +65,7 @@
 
 /* SPIDAT1 (upper 16 bit defines) */
 #define SPIDAT1_CSHOLD_MASK	BIT(12)
+#define SPIDAT1_WDEL		BIT(10)
 
 /* SPIGCR1 */
 #define SPIGCR1_CLKMOD_MASK	BIT(1)
@@ -167,8 +168,10 @@ static void davinci_spi_rx_buf_u16(u32 data, struct davinci_spi *dspi)
 static u32 davinci_spi_tx_buf_u8(struct davinci_spi *dspi)
 {
 	u32 data = 0;
+
 	if (dspi->tx) {
 		const u8 *tx = dspi->tx;
+
 		data = *tx++;
 		dspi->tx = tx;
 	}
@@ -178,8 +181,10 @@ static u32 davinci_spi_tx_buf_u8(struct davinci_spi *dspi)
 static u32 davinci_spi_tx_buf_u16(struct davinci_spi *dspi)
 {
 	u32 data = 0;
+
 	if (dspi->tx) {
 		const u16 *tx = dspi->tx;
+
 		data = *tx++;
 		dspi->tx = tx;
 	}
@@ -209,6 +214,7 @@ static void davinci_spi_chipselect(struct spi_device *spi, int value)
 {
 	struct davinci_spi *dspi;
 	struct davinci_spi_platform_data *pdata;
+	struct davinci_spi_config *spicfg = spi->controller_data;
 	u8 chip_sel = spi->chip_select;
 	u16 spidat1 = CS_DEFAULT;
 	bool gpio_chipsel = false;
@@ -223,6 +229,10 @@ static void davinci_spi_chipselect(struct spi_device *spi, int value)
 		gpio = spi->cs_gpio;
 	}
 
+	/* program delay transfers if tx_delay is non zero */
+	if (spicfg->wdelay)
+		spidat1 |= SPIDAT1_WDEL;
+
 	/*
 	 * Board specific chip select logic decides the polarity and cs
 	 * line for the controller
@@ -237,9 +247,9 @@ static void davinci_spi_chipselect(struct spi_device *spi, int value)
 			spidat1 |= SPIDAT1_CSHOLD_MASK;
 			spidat1 &= ~(0x1 << chip_sel);
 		}
-
-		iowrite16(spidat1, dspi->base + SPIDAT1 + 2);
 	}
+
+	iowrite16(spidat1, dspi->base + SPIDAT1 + 2);
 }
 
 /**
@@ -285,7 +295,7 @@ static int davinci_spi_setup_transfer(struct spi_device *spi,
 	int prescale;
 
 	dspi = spi_master_get_devdata(spi->master);
-	spicfg = (struct davinci_spi_config *)spi->controller_data;
+	spicfg = spi->controller_data;
 	if (!spicfg)
 		spicfg = &davinci_spi_default_cfg;
 
@@ -333,6 +343,14 @@ static int davinci_spi_setup_transfer(struct spi_device *spi,
 		spifmt |= SPIFMT_PHASE_MASK;
 
 	/*
+	* Assume wdelay is used only on SPI peripherals that has this field
+	* in SPIFMTn register and when it's configured from board file or DT.
+	*/
+	if (spicfg->wdelay)
+		spifmt |= ((spicfg->wdelay << SPIFMT_WDELAY_SHIFT)
+				& SPIFMT_WDELAY_MASK);
+
+	/*
 	 * Version 1 hardware supports two basic SPI modes:
 	 *  - Standard SPI mode uses 4 pins, with chipselect
 	 *  - 3 pin SPI is a 4 pin variant without CS (SPI_NO_CS)
@@ -349,9 +367,6 @@ static int davinci_spi_setup_transfer(struct spi_device *spi,
 
 		u32 delay = 0;
 
-		spifmt |= ((spicfg->wdelay << SPIFMT_WDELAY_SHIFT)
-							& SPIFMT_WDELAY_MASK);
-
 		if (spicfg->odd_parity)
 			spifmt |= SPIFMT_ODD_PARITY_MASK;
 
@@ -383,6 +398,26 @@ static int davinci_spi_setup_transfer(struct spi_device *spi,
 	return 0;
 }
 
+static int davinci_spi_of_setup(struct spi_device *spi)
+{
+	struct davinci_spi_config *spicfg = spi->controller_data;
+	struct device_node *np = spi->dev.of_node;
+	u32 prop;
+
+	if (spicfg == NULL && np) {
+		spicfg = kzalloc(sizeof(*spicfg), GFP_KERNEL);
+		if (!spicfg)
+			return -ENOMEM;
+		*spicfg = davinci_spi_default_cfg;
+		/* override with dt configured values */
+		if (!of_property_read_u32(np, "ti,spi-wdelay", &prop))
+			spicfg->wdelay = (u8)prop;
+		spi->controller_data = spicfg;
+	}
+
+	return 0;
+}
+
 /**
  * davinci_spi_setup - This functions will set default transfer method
  * @spi: spi device on which data transfer to be done
@@ -397,36 +432,33 @@ static int davinci_spi_setup(struct spi_device *spi)
 	struct spi_master *master = spi->master;
 	struct device_node *np = spi->dev.of_node;
 	bool internal_cs = true;
-	unsigned long flags = GPIOF_DIR_OUT;
 
 	dspi = spi_master_get_devdata(spi->master);
 	pdata = &dspi->pdata;
 
-	flags |= (spi->mode & SPI_CS_HIGH) ? GPIOF_INIT_LOW : GPIOF_INIT_HIGH;
-
 	if (!(spi->mode & SPI_NO_CS)) {
 		if (np && (master->cs_gpios != NULL) && (spi->cs_gpio >= 0)) {
-			retval = gpio_request_one(spi->cs_gpio,
-						  flags, dev_name(&spi->dev));
+			retval = gpio_direction_output(
+				      spi->cs_gpio, !(spi->mode & SPI_CS_HIGH));
 			internal_cs = false;
 		} else if (pdata->chip_sel &&
 			   spi->chip_select < pdata->num_chipselect &&
 			   pdata->chip_sel[spi->chip_select] != SPI_INTERN_CS) {
 			spi->cs_gpio = pdata->chip_sel[spi->chip_select];
-			retval = gpio_request_one(spi->cs_gpio,
-						  flags, dev_name(&spi->dev));
+			retval = gpio_direction_output(
+				      spi->cs_gpio, !(spi->mode & SPI_CS_HIGH));
 			internal_cs = false;
 		}
-	}
 
-	if (retval) {
-		dev_err(&spi->dev, "GPIO %d setup failed (%d)\n",
-			spi->cs_gpio, retval);
-		return retval;
-	}
+		if (retval) {
+			dev_err(&spi->dev, "GPIO %d setup failed (%d)\n",
+				spi->cs_gpio, retval);
+			return retval;
+		}
 
-	if (internal_cs)
-		set_io_bits(dspi->base + SPIPC0, 1 << spi->chip_select);
+		if (internal_cs)
+			set_io_bits(dspi->base + SPIPC0, 1 << spi->chip_select);
+	}
 
 	if (spi->mode & SPI_READY)
 		set_io_bits(dspi->base + SPIPC0, SPIPC0_SPIENA_MASK);
@@ -436,13 +468,16 @@ static int davinci_spi_setup(struct spi_device *spi)
 	else
 		clear_io_bits(dspi->base + SPIGCR1, SPIGCR1_LOOPBACK_MASK);
 
-	return retval;
+	return davinci_spi_of_setup(spi);
 }
 
 static void davinci_spi_cleanup(struct spi_device *spi)
 {
-	if (spi->cs_gpio >= 0)
-		gpio_free(spi->cs_gpio);
+	struct davinci_spi_config *spicfg = spi->controller_data;
+
+	spi->controller_data = NULL;
+	if (spi->dev.of_node)
+		kfree(spicfg);
 }
 
 static int davinci_spi_check_error(struct davinci_spi *dspi, int int_status)
@@ -967,6 +1002,27 @@ static int davinci_spi_probe(struct platform_device *pdev)
 	if (dspi->version == SPI_VERSION_2)
 		dspi->bitbang.flags |= SPI_READY;
 
+	if (pdev->dev.of_node) {
+		int i;
+
+		for (i = 0; i < pdata->num_chipselect; i++) {
+			int cs_gpio = of_get_named_gpio(pdev->dev.of_node,
+							"cs-gpios", i);
+
+			if (cs_gpio == -EPROBE_DEFER) {
+				ret = cs_gpio;
+				goto free_clk;
+			}
+
+			if (gpio_is_valid(cs_gpio)) {
+				ret = devm_gpio_request(&pdev->dev, cs_gpio,
+							dev_name(&pdev->dev));
+				if (ret)
+					goto free_clk;
+			}
+		}
+	}
+
 	r = platform_get_resource(pdev, IORESOURCE_DMA, 0);
 	if (r)
 		dma_rx_chan = r->start;
@@ -985,8 +1041,8 @@ static int davinci_spi_probe(struct platform_device *pdev)
 			goto free_clk;
 
 		dev_info(&pdev->dev, "DMA: supported\n");
-		dev_info(&pdev->dev, "DMA: RX channel: %pa, TX channel: %pa, "
-				"event queue: %d\n", &dma_rx_chan, &dma_tx_chan,
+		dev_info(&pdev->dev, "DMA: RX channel: %pa, TX channel: %pa, event queue: %d\n",
+				&dma_rx_chan, &dma_tx_chan,
 				pdata->dma_event_q);
 	}
 
diff --git a/drivers/spi/spi-dw-mid.c b/drivers/spi/spi-dw-mid.c
index 6d207afec8cb..46c6d58e1fda 100644
--- a/drivers/spi/spi-dw-mid.c
+++ b/drivers/spi/spi-dw-mid.c
@@ -1,7 +1,7 @@
 /*
  * Special handling for DW core on Intel MID platform
  *
- * Copyright (c) 2009, Intel Corporation.
+ * Copyright (c) 2009, 2014 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -11,10 +11,6 @@
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation,
- * Inc., 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
 #include <linux/dma-mapping.h>
@@ -39,22 +35,25 @@ static bool mid_spi_dma_chan_filter(struct dma_chan *chan, void *param)
 {
 	struct dw_spi *dws = param;
 
-	return dws->dmac && (&dws->dmac->dev == chan->device->dev);
+	return dws->dma_dev == chan->device->dev;
 }
 
 static int mid_spi_dma_init(struct dw_spi *dws)
 {
 	struct mid_dma *dw_dma = dws->dma_priv;
+	struct pci_dev *dma_dev;
 	struct intel_mid_dma_slave *rxs, *txs;
 	dma_cap_mask_t mask;
 
 	/*
 	 * Get pci device for DMA controller, currently it could only
-	 * be the DMA controller of either Moorestown or Medfield
+	 * be the DMA controller of Medfield
 	 */
-	dws->dmac = pci_get_device(PCI_VENDOR_ID_INTEL, 0x0813, NULL);
-	if (!dws->dmac)
-		dws->dmac = pci_get_device(PCI_VENDOR_ID_INTEL, 0x0827, NULL);
+	dma_dev = pci_get_device(PCI_VENDOR_ID_INTEL, 0x0827, NULL);
+	if (!dma_dev)
+		return -ENODEV;
+
+	dws->dma_dev = &dma_dev->dev;
 
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
@@ -83,13 +82,18 @@ static int mid_spi_dma_init(struct dw_spi *dws)
 free_rxchan:
 	dma_release_channel(dws->rxchan);
 err_exit:
-	return -1;
-
+	return -EBUSY;
 }
 
 static void mid_spi_dma_exit(struct dw_spi *dws)
 {
+	if (!dws->dma_inited)
+		return;
+
+	dmaengine_terminate_all(dws->txchan);
 	dma_release_channel(dws->txchan);
+
+	dmaengine_terminate_all(dws->rxchan);
 	dma_release_channel(dws->rxchan);
 }
 
@@ -109,8 +113,7 @@ static void dw_spi_dma_done(void *arg)
 
 static int mid_spi_dma_transfer(struct dw_spi *dws, int cs_change)
 {
-	struct dma_async_tx_descriptor *txdesc = NULL, *rxdesc = NULL;
-	struct dma_chan *txchan, *rxchan;
+	struct dma_async_tx_descriptor *txdesc, *rxdesc;
 	struct dma_slave_config txconf, rxconf;
 	u16 dma_ctrl = 0;
 
@@ -120,37 +123,34 @@ static int mid_spi_dma_transfer(struct dw_spi *dws, int cs_change)
 		dw_writew(dws, DW_SPI_DMARDLR, 0xf);
 		dw_writew(dws, DW_SPI_DMATDLR, 0x10);
 		if (dws->tx_dma)
-			dma_ctrl |= 0x2;
+			dma_ctrl |= SPI_DMA_TDMAE;
 		if (dws->rx_dma)
-			dma_ctrl |= 0x1;
+			dma_ctrl |= SPI_DMA_RDMAE;
 		dw_writew(dws, DW_SPI_DMACR, dma_ctrl);
 		spi_enable_chip(dws, 1);
 	}
 
 	dws->dma_chan_done = 0;
-	txchan = dws->txchan;
-	rxchan = dws->rxchan;
 
 	/* 2. Prepare the TX dma transfer */
 	txconf.direction = DMA_MEM_TO_DEV;
 	txconf.dst_addr = dws->dma_addr;
 	txconf.dst_maxburst = LNW_DMA_MSIZE_16;
 	txconf.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
-	txconf.dst_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+	txconf.dst_addr_width = dws->dma_width;
 	txconf.device_fc = false;
 
-	txchan->device->device_control(txchan, DMA_SLAVE_CONFIG,
-				       (unsigned long) &txconf);
+	dmaengine_slave_config(dws->txchan, &txconf);
 
 	memset(&dws->tx_sgl, 0, sizeof(dws->tx_sgl));
 	dws->tx_sgl.dma_address = dws->tx_dma;
 	dws->tx_sgl.length = dws->len;
 
-	txdesc = dmaengine_prep_slave_sg(txchan,
+	txdesc = dmaengine_prep_slave_sg(dws->txchan,
 				&dws->tx_sgl,
 				1,
 				DMA_MEM_TO_DEV,
-				DMA_PREP_INTERRUPT);
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	txdesc->callback = dw_spi_dma_done;
 	txdesc->callback_param = dws;
 
@@ -159,27 +159,30 @@ static int mid_spi_dma_transfer(struct dw_spi *dws, int cs_change)
 	rxconf.src_addr = dws->dma_addr;
 	rxconf.src_maxburst = LNW_DMA_MSIZE_16;
 	rxconf.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
-	rxconf.src_addr_width = DMA_SLAVE_BUSWIDTH_2_BYTES;
+	rxconf.src_addr_width = dws->dma_width;
 	rxconf.device_fc = false;
 
-	rxchan->device->device_control(rxchan, DMA_SLAVE_CONFIG,
-				       (unsigned long) &rxconf);
+	dmaengine_slave_config(dws->rxchan, &rxconf);
 
 	memset(&dws->rx_sgl, 0, sizeof(dws->rx_sgl));
 	dws->rx_sgl.dma_address = dws->rx_dma;
 	dws->rx_sgl.length = dws->len;
 
-	rxdesc = dmaengine_prep_slave_sg(rxchan,
+	rxdesc = dmaengine_prep_slave_sg(dws->rxchan,
 				&dws->rx_sgl,
 				1,
 				DMA_DEV_TO_MEM,
-				DMA_PREP_INTERRUPT);
+				DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 	rxdesc->callback = dw_spi_dma_done;
 	rxdesc->callback_param = dws;
 
 	/* rx must be started before tx due to spi instinct */
-	rxdesc->tx_submit(rxdesc);
-	txdesc->tx_submit(txdesc);
+	dmaengine_submit(rxdesc);
+	dma_async_issue_pending(dws->rxchan);
+
+	dmaengine_submit(txdesc);
+	dma_async_issue_pending(dws->txchan);
+
 	return 0;
 }
 
@@ -190,7 +193,7 @@ static struct dw_spi_dma_ops mid_dma_ops = {
 };
 #endif
 
-/* Some specific info for SPI0 controller on Moorestown */
+/* Some specific info for SPI0 controller on Intel MID */
 
 /* HW info for MRST CLk Control Unit, one 32b reg */
 #define MRST_SPI_CLK_BASE	100000000	/* 100m */
diff --git a/drivers/spi/spi-dw-pci.c b/drivers/spi/spi-dw-pci.c
index 3f3dc1226edf..ba68da12cdf0 100644
--- a/drivers/spi/spi-dw-pci.c
+++ b/drivers/spi/spi-dw-pci.c
@@ -1,7 +1,7 @@
 /*
  * PCI interface driver for DW SPI Core
  *
- * Copyright (c) 2009, Intel Corporation.
+ * Copyright (c) 2009, 2014 Intel Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -11,10 +11,6 @@
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
- *
- * You should have received a copy of the GNU General Public License along
- * with this program; if not, write to the Free Software Foundation,
- * Inc., 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
 #include <linux/interrupt.h>
@@ -32,17 +28,22 @@ struct dw_spi_pci {
 	struct dw_spi	dws;
 };
 
-static int spi_pci_probe(struct pci_dev *pdev,
-	const struct pci_device_id *ent)
+struct spi_pci_desc {
+	int	(*setup)(struct dw_spi *);
+};
+
+static struct spi_pci_desc spi_pci_mid_desc = {
+	.setup = dw_spi_mid_init,
+};
+
+static int spi_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
 	struct dw_spi_pci *dwpci;
 	struct dw_spi *dws;
+	struct spi_pci_desc *desc = (struct spi_pci_desc *)ent->driver_data;
 	int pci_bar = 0;
 	int ret;
 
-	dev_info(&pdev->dev, "found PCI SPI controller(ID: %04x:%04x)\n",
-		pdev->vendor, pdev->device);
-
 	ret = pcim_enable_device(pdev);
 	if (ret)
 		return ret;
@@ -58,20 +59,22 @@ static int spi_pci_probe(struct pci_dev *pdev,
 	/* Get basic io resource and map it */
 	dws->paddr = pci_resource_start(pdev, pci_bar);
 
-	ret = pcim_iomap_regions(pdev, 1, dev_name(&pdev->dev));
+	ret = pcim_iomap_regions(pdev, 1 << pci_bar, pci_name(pdev));
 	if (ret)
 		return ret;
 
+	dws->regs = pcim_iomap_table(pdev)[pci_bar];
+
 	dws->bus_num = 0;
 	dws->num_cs = 4;
 	dws->irq = pdev->irq;
 
 	/*
-	 * Specific handling for Intel MID paltforms, like dma setup,
+	 * Specific handling for paltforms, like dma setup,
 	 * clock rate, FIFO depth.
 	 */
-	if (pdev->device == 0x0800) {
-		ret = dw_spi_mid_init(dws);
+	if (desc && desc->setup) {
+		ret = desc->setup(dws);
 		if (ret)
 			return ret;
 	}
@@ -83,6 +86,9 @@ static int spi_pci_probe(struct pci_dev *pdev,
 	/* PCI hook and SPI hook use the same drv data */
 	pci_set_drvdata(pdev, dwpci);
 
+	dev_info(&pdev->dev, "found PCI SPI controller(ID: %04x:%04x)\n",
+		pdev->vendor, pdev->device);
+
 	return 0;
 }
 
@@ -93,41 +99,29 @@ static void spi_pci_remove(struct pci_dev *pdev)
 	dw_spi_remove_host(&dwpci->dws);
 }
 
-#ifdef CONFIG_PM
-static int spi_suspend(struct pci_dev *pdev, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int spi_suspend(struct device *dev)
 {
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct dw_spi_pci *dwpci = pci_get_drvdata(pdev);
-	int ret;
 
-	ret = dw_spi_suspend_host(&dwpci->dws);
-	if (ret)
-		return ret;
-	pci_save_state(pdev);
-	pci_disable_device(pdev);
-	pci_set_power_state(pdev, pci_choose_state(pdev, state));
-	return ret;
+	return dw_spi_suspend_host(&dwpci->dws);
 }
 
-static int spi_resume(struct pci_dev *pdev)
+static int spi_resume(struct device *dev)
 {
+	struct pci_dev *pdev = to_pci_dev(dev);
 	struct dw_spi_pci *dwpci = pci_get_drvdata(pdev);
-	int ret;
 
-	pci_set_power_state(pdev, PCI_D0);
-	pci_restore_state(pdev);
-	ret = pci_enable_device(pdev);
-	if (ret)
-		return ret;
 	return dw_spi_resume_host(&dwpci->dws);
 }
-#else
-#define spi_suspend	NULL
-#define spi_resume	NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(dw_spi_pm_ops, spi_suspend, spi_resume);
+
 static const struct pci_device_id pci_ids[] = {
 	/* Intel MID platform SPI controller 0 */
-	{ PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x0800) },
+	{ PCI_VDEVICE(INTEL, 0x0800), (kernel_ulong_t)&spi_pci_mid_desc},
 	{},
 };
 
@@ -136,8 +130,9 @@ static struct pci_driver dw_spi_driver = {
 	.id_table =	pci_ids,
 	.probe =	spi_pci_probe,
 	.remove =	spi_pci_remove,
-	.suspend =	spi_suspend,
-	.resume	=	spi_resume,
+	.driver         = {
+		.pm     = &dw_spi_pm_ops,
+	},
 };
 
 module_pci_driver(dw_spi_driver);
diff --git a/drivers/spi/spi-dw.c b/drivers/spi/spi-dw.c
index 29f33143b795..729215885250 100644
--- a/drivers/spi/spi-dw.c
+++ b/drivers/spi/spi-dw.c
@@ -11,10 +11,6 @@
  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
  */
 
 #include <linux/dma-mapping.h>
@@ -59,22 +55,20 @@ struct chip_data {
 
 #ifdef CONFIG_DEBUG_FS
 #define SPI_REGS_BUFSIZE	1024
-static ssize_t  spi_show_regs(struct file *file, char __user *user_buf,
-				size_t count, loff_t *ppos)
+static ssize_t dw_spi_show_regs(struct file *file, char __user *user_buf,
+		size_t count, loff_t *ppos)
 {
-	struct dw_spi *dws;
+	struct dw_spi *dws = file->private_data;
 	char *buf;
 	u32 len = 0;
 	ssize_t ret;
 
-	dws = file->private_data;
-
 	buf = kzalloc(SPI_REGS_BUFSIZE, GFP_KERNEL);
 	if (!buf)
 		return 0;
 
 	len += snprintf(buf + len, SPI_REGS_BUFSIZE - len,
-			"MRST SPI0 registers:\n");
+			"%s registers:\n", dev_name(&dws->master->dev));
 	len += snprintf(buf + len, SPI_REGS_BUFSIZE - len,
 			"=================================\n");
 	len += snprintf(buf + len, SPI_REGS_BUFSIZE - len,
@@ -110,42 +104,41 @@ static ssize_t  spi_show_regs(struct file *file, char __user *user_buf,
 	len += snprintf(buf + len, SPI_REGS_BUFSIZE - len,
 			"=================================\n");
 
-	ret =  simple_read_from_buffer(user_buf, count, ppos, buf, len);
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, len);
 	kfree(buf);
 	return ret;
 }
 
-static const struct file_operations mrst_spi_regs_ops = {
+static const struct file_operations dw_spi_regs_ops = {
 	.owner		= THIS_MODULE,
 	.open		= simple_open,
-	.read		= spi_show_regs,
+	.read		= dw_spi_show_regs,
 	.llseek		= default_llseek,
 };
 
-static int mrst_spi_debugfs_init(struct dw_spi *dws)
+static int dw_spi_debugfs_init(struct dw_spi *dws)
 {
-	dws->debugfs = debugfs_create_dir("mrst_spi", NULL);
+	dws->debugfs = debugfs_create_dir("dw_spi", NULL);
 	if (!dws->debugfs)
 		return -ENOMEM;
 
 	debugfs_create_file("registers", S_IFREG | S_IRUGO,
-		dws->debugfs, (void *)dws, &mrst_spi_regs_ops);
+		dws->debugfs, (void *)dws, &dw_spi_regs_ops);
 	return 0;
 }
 
-static void mrst_spi_debugfs_remove(struct dw_spi *dws)
+static void dw_spi_debugfs_remove(struct dw_spi *dws)
 {
-	if (dws->debugfs)
-		debugfs_remove_recursive(dws->debugfs);
+	debugfs_remove_recursive(dws->debugfs);
 }
 
 #else
-static inline int mrst_spi_debugfs_init(struct dw_spi *dws)
+static inline int dw_spi_debugfs_init(struct dw_spi *dws)
 {
 	return 0;
 }
 
-static inline void mrst_spi_debugfs_remove(struct dw_spi *dws)
+static inline void dw_spi_debugfs_remove(struct dw_spi *dws)
 {
 }
 #endif /* CONFIG_DEBUG_FS */
@@ -177,7 +170,7 @@ static inline u32 rx_max(struct dw_spi *dws)
 {
 	u32 rx_left = (dws->rx_end - dws->rx) / dws->n_bytes;
 
-	return min(rx_left, (u32)dw_readw(dws, DW_SPI_RXFLR));
+	return min_t(u32, rx_left, dw_readw(dws, DW_SPI_RXFLR));
 }
 
 static void dw_writer(struct dw_spi *dws)
@@ -228,8 +221,9 @@ static void *next_transfer(struct dw_spi *dws)
 					struct spi_transfer,
 					transfer_list);
 		return RUNNING_STATE;
-	} else
-		return DONE_STATE;
+	}
+
+	return DONE_STATE;
 }
 
 /*
@@ -271,7 +265,7 @@ static void giveback(struct dw_spi *dws)
 					transfer_list);
 
 	if (!last_transfer->cs_change)
-		spi_chip_sel(dws, dws->cur_msg->spi, 0);
+		spi_chip_sel(dws, msg->spi, 0);
 
 	spi_finalize_current_message(dws->master);
 }
@@ -396,7 +390,7 @@ static void pump_transfers(unsigned long data)
 		goto early_exit;
 	}
 
-	/* Delay if requested at end of transfer*/
+	/* Delay if requested at end of transfer */
 	if (message->state == RUNNING_STATE) {
 		previous = list_entry(transfer->transfer_list.prev,
 					struct spi_transfer,
@@ -471,10 +465,12 @@ static void pump_transfers(unsigned long data)
 	 */
 	if (!dws->dma_mapped && !chip->poll_mode) {
 		int templen = dws->len / dws->n_bytes;
+
 		txint_level = dws->fifo_len / 2;
 		txint_level = (templen > txint_level) ? txint_level : templen;
 
-		imask |= SPI_INT_TXEI | SPI_INT_TXOI | SPI_INT_RXUI | SPI_INT_RXOI;
+		imask |= SPI_INT_TXEI | SPI_INT_TXOI |
+			 SPI_INT_RXUI | SPI_INT_RXOI;
 		dws->transfer_handler = interrupt_transfer;
 	}
 
@@ -515,7 +511,6 @@ static void pump_transfers(unsigned long data)
 
 early_exit:
 	giveback(dws);
-	return;
 }
 
 static int dw_spi_transfer_one_message(struct spi_master *master,
@@ -524,7 +519,7 @@ static int dw_spi_transfer_one_message(struct spi_master *master,
 	struct dw_spi *dws = spi_master_get_devdata(master);
 
 	dws->cur_msg = msg;
-	/* Initial message state*/
+	/* Initial message state */
 	dws->cur_msg->state = START_STATE;
 	dws->cur_transfer = list_entry(dws->cur_msg->transfers.next,
 						struct spi_transfer,
@@ -547,8 +542,7 @@ static int dw_spi_setup(struct spi_device *spi)
 	/* Only alloc on first setup */
 	chip = spi_get_ctldata(spi);
 	if (!chip) {
-		chip = devm_kzalloc(&spi->dev, sizeof(struct chip_data),
-				GFP_KERNEL);
+		chip = kzalloc(sizeof(struct chip_data), GFP_KERNEL);
 		if (!chip)
 			return -ENOMEM;
 		spi_set_ctldata(spi, chip);
@@ -596,6 +590,9 @@ static int dw_spi_setup(struct spi_device *spi)
 			| (spi->mode  << SPI_MODE_OFFSET)
 			| (chip->tmode << SPI_TMOD_OFFSET);
 
+	if (spi->mode & SPI_LOOP)
+		chip->cr0 |= 1 << SPI_SRL_OFFSET;
+
 	if (gpio_is_valid(spi->cs_gpio)) {
 		ret = gpio_direction_output(spi->cs_gpio,
 				!(spi->mode & SPI_CS_HIGH));
@@ -606,6 +603,14 @@ static int dw_spi_setup(struct spi_device *spi)
 	return 0;
 }
 
+static void dw_spi_cleanup(struct spi_device *spi)
+{
+	struct chip_data *chip = spi_get_ctldata(spi);
+
+	kfree(chip);
+	spi_set_ctldata(spi, NULL);
+}
+
 /* Restart the controller, disable all interrupts, clean rx fifo */
 static void spi_hw_init(struct dw_spi *dws)
 {
@@ -619,6 +624,7 @@ static void spi_hw_init(struct dw_spi *dws)
 	 */
 	if (!dws->fifo_len) {
 		u32 fifo;
+
 		for (fifo = 2; fifo <= 257; fifo++) {
 			dw_writew(dws, DW_SPI_TXFLTR, fifo);
 			if (fifo != dw_readw(dws, DW_SPI_TXFLTR))
@@ -646,8 +652,7 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 	dws->prev_chip = NULL;
 	dws->dma_inited = 0;
 	dws->dma_addr = (dma_addr_t)(dws->paddr + 0x60);
-	snprintf(dws->name, sizeof(dws->name), "dw_spi%d",
-			dws->bus_num);
+	snprintf(dws->name, sizeof(dws->name), "dw_spi%d", dws->bus_num);
 
 	ret = devm_request_irq(dev, dws->irq, dw_spi_irq, IRQF_SHARED,
 			dws->name, dws);
@@ -656,11 +661,12 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 		goto err_free_master;
 	}
 
-	master->mode_bits = SPI_CPOL | SPI_CPHA;
+	master->mode_bits = SPI_CPOL | SPI_CPHA | SPI_LOOP;
 	master->bits_per_word_mask = SPI_BPW_MASK(8) | SPI_BPW_MASK(16);
 	master->bus_num = dws->bus_num;
 	master->num_chipselect = dws->num_cs;
 	master->setup = dw_spi_setup;
+	master->cleanup = dw_spi_cleanup;
 	master->transfer_one_message = dw_spi_transfer_one_message;
 	master->max_speed_hz = dws->max_freq;
 
@@ -684,7 +690,7 @@ int dw_spi_add_host(struct device *dev, struct dw_spi *dws)
 		goto err_dma_exit;
 	}
 
-	mrst_spi_debugfs_init(dws);
+	dw_spi_debugfs_init(dws);
 	return 0;
 
 err_dma_exit:
@@ -701,7 +707,7 @@ void dw_spi_remove_host(struct dw_spi *dws)
 {
 	if (!dws)
 		return;
-	mrst_spi_debugfs_remove(dws);
+	dw_spi_debugfs_remove(dws);
 
 	if (dws->dma_ops && dws->dma_ops->dma_exit)
 		dws->dma_ops->dma_exit(dws);
diff --git a/drivers/spi/spi-dw.h b/drivers/spi/spi-dw.h
index 6d2acad34f64..83a103a76481 100644
--- a/drivers/spi/spi-dw.h
+++ b/drivers/spi/spi-dw.h
@@ -74,6 +74,10 @@
 #define SPI_INT_RXFI			(1 << 4)
 #define SPI_INT_MSTI			(1 << 5)
 
+/* Bit fields in DMACR */
+#define SPI_DMA_RDMAE			(1 << 0)
+#define SPI_DMA_TDMAE			(1 << 1)
+
 /* TX RX interrupt level threshold, max can be 256 */
 #define SPI_INT_THRESHOLD		32
 
@@ -140,7 +144,6 @@ struct dw_spi {
 	dma_addr_t		dma_addr; /* phy address of the Data register */
 	struct dw_spi_dma_ops	*dma_ops;
 	void			*dma_priv; /* platform relate info */
-	struct pci_dev		*dmac;
 
 	/* Bus interface info */
 	void			*priv;
@@ -217,11 +220,11 @@ static inline void spi_umask_intr(struct dw_spi *dws, u32 mask)
  * Each SPI slave device to work with dw_api controller should
  * has such a structure claiming its working mode (PIO/DMA etc),
  * which can be save in the "controller_data" member of the
- * struct spi_device
+ * struct spi_device.
  */
 struct dw_spi_chip {
-	u8 poll_mode;	/* 0 for contoller polling mode */
-	u8 type;	/* SPI/SSP/Micrwire */
+	u8 poll_mode;	/* 1 for controller polling mode */
+	u8 type;	/* SPI/SSP/MicroWire */
 	u8 enable_dma;
 	void (*cs_control)(u32 command);
 };
diff --git a/drivers/spi/spi-ep93xx.c b/drivers/spi/spi-ep93xx.c
index 2f675d32df0e..bf9728773247 100644
--- a/drivers/spi/spi-ep93xx.c
+++ b/drivers/spi/spi-ep93xx.c
@@ -266,6 +266,7 @@ static int ep93xx_spi_setup(struct spi_device *spi)
 
 		if (chip->ops && chip->ops->setup) {
 			int ret = chip->ops->setup(spi);
+
 			if (ret) {
 				kfree(chip);
 				return ret;
diff --git a/drivers/spi/spi-fsl-cpm.c b/drivers/spi/spi-fsl-cpm.c
index 54b06376f03c..c5dd20beee22 100644
--- a/drivers/spi/spi-fsl-cpm.c
+++ b/drivers/spi/spi-fsl-cpm.c
@@ -15,17 +15,17 @@
  * Free Software Foundation;  either version 2 of the  License, or (at your
  * option) any later version.
  */
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/spi/spi.h>
-#include <linux/fsl_devices.h>
-#include <linux/dma-mapping.h>
-#include <linux/of_address.h>
 #include <asm/cpm.h>
 #include <asm/qe.h>
+#include <linux/dma-mapping.h>
+#include <linux/fsl_devices.h>
+#include <linux/kernel.h>
+#include <linux/of_address.h>
+#include <linux/spi/spi.h>
+#include <linux/types.h>
 
-#include "spi-fsl-lib.h"
 #include "spi-fsl-cpm.h"
+#include "spi-fsl-lib.h"
 #include "spi-fsl-spi.h"
 
 /* CPM1 and CPM2 are mutually exclusive. */
diff --git a/drivers/spi/spi-fsl-dspi.c b/drivers/spi/spi-fsl-dspi.c
index 5021ddf03f60..448216025ce8 100644
--- a/drivers/spi/spi-fsl-dspi.c
+++ b/drivers/spi/spi-fsl-dspi.c
@@ -13,22 +13,22 @@
  *
  */
 
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/errno.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/errno.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
 #include <linux/regmap.h>
 #include <linux/sched.h>
-#include <linux/delay.h>
-#include <linux/io.h>
-#include <linux/clk.h>
-#include <linux/err.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
-#include <linux/pm_runtime.h>
-#include <linux/of.h>
-#include <linux/of_device.h>
 
 #define DRIVER_NAME "fsl-dspi"
 
@@ -493,9 +493,6 @@ static int dspi_probe(struct platform_device *pdev)
 	}
 
 	dspi_regmap_config.lock_arg = dspi;
-	dspi_regmap_config.val_format_endian =
-		of_property_read_bool(np, "big-endian")
-			? REGMAP_ENDIAN_BIG : REGMAP_ENDIAN_DEFAULT;
 	dspi->regmap = devm_regmap_init_mmio_clk(&pdev->dev, "dspi", base,
 						&dspi_regmap_config);
 	if (IS_ERR(dspi->regmap)) {
@@ -535,7 +532,6 @@ static int dspi_probe(struct platform_device *pdev)
 		goto out_clk_put;
 	}
 
-	pr_info(KERN_INFO "Freescale DSPI master initialized\n");
 	return ret;
 
 out_clk_put:
diff --git a/drivers/spi/spi-fsl-espi.c b/drivers/spi/spi-fsl-espi.c
index 8ebd724e4c59..a7f94b6a9e70 100644
--- a/drivers/spi/spi-fsl-espi.c
+++ b/drivers/spi/spi-fsl-espi.c
@@ -8,19 +8,19 @@
  * Free Software Foundation;  either version 2 of the  License, or (at your
  * option) any later version.
  */
-#include <linux/module.h>
 #include <linux/delay.h>
-#include <linux/irq.h>
-#include <linux/spi/spi.h>
-#include <linux/platform_device.h>
+#include <linux/err.h>
 #include <linux/fsl_devices.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/module.h>
 #include <linux/mm.h>
 #include <linux/of.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/of_platform.h>
-#include <linux/interrupt.h>
-#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
 #include <sysdev/fsl_soc.h>
 
 #include "spi-fsl-lib.h"
@@ -452,16 +452,16 @@ static int fsl_espi_setup(struct spi_device *spi)
 	int retval;
 	u32 hw_mode;
 	u32 loop_mode;
-	struct spi_mpc8xxx_cs *cs = spi->controller_state;
+	struct spi_mpc8xxx_cs *cs = spi_get_ctldata(spi);
 
 	if (!spi->max_speed_hz)
 		return -EINVAL;
 
 	if (!cs) {
-		cs = devm_kzalloc(&spi->dev, sizeof(*cs), GFP_KERNEL);
+		cs = kzalloc(sizeof(*cs), GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
-		spi->controller_state = cs;
+		spi_set_ctldata(spi, cs);
 	}
 
 	mpc8xxx_spi = spi_master_get_devdata(spi->master);
@@ -496,6 +496,14 @@ static int fsl_espi_setup(struct spi_device *spi)
 	return 0;
 }
 
+static void fsl_espi_cleanup(struct spi_device *spi)
+{
+	struct spi_mpc8xxx_cs *cs = spi_get_ctldata(spi);
+
+	kfree(cs);
+	spi_set_ctldata(spi, NULL);
+}
+
 void fsl_espi_cpu_irq(struct mpc8xxx_spi *mspi, u32 events)
 {
 	struct fsl_espi_reg *reg_base = mspi->reg_base;
@@ -605,6 +613,7 @@ static struct spi_master * fsl_espi_probe(struct device *dev,
 
 	master->bits_per_word_mask = SPI_BPW_RANGE_MASK(4, 16);
 	master->setup = fsl_espi_setup;
+	master->cleanup = fsl_espi_cleanup;
 
 	mpc8xxx_spi = spi_master_get_devdata(master);
 	mpc8xxx_spi->spi_do_one_msg = fsl_espi_do_one_msg;
diff --git a/drivers/spi/spi-fsl-lib.c b/drivers/spi/spi-fsl-lib.c
index e0b773fc29cb..5ddb5b098e4e 100644
--- a/drivers/spi/spi-fsl-lib.c
+++ b/drivers/spi/spi-fsl-lib.c
@@ -16,10 +16,10 @@
  * Free Software Foundation;  either version 2 of the  License, or (at your
  * option) any later version.
  */
-#include <linux/kernel.h>
-#include <linux/interrupt.h>
-#include <linux/fsl_devices.h>
 #include <linux/dma-mapping.h>
+#include <linux/fsl_devices.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
 #include <linux/mm.h>
 #include <linux/of_platform.h>
 #include <linux/spi/spi.h>
diff --git a/drivers/spi/spi-fsl-spi.c b/drivers/spi/spi-fsl-spi.c
index 9452f6740997..ed792880c9d6 100644
--- a/drivers/spi/spi-fsl-spi.c
+++ b/drivers/spi/spi-fsl-spi.c
@@ -19,25 +19,25 @@
  * Free Software Foundation;  either version 2 of the  License, or (at your
  * option) any later version.
  */
-#include <linux/module.h>
-#include <linux/types.h>
-#include <linux/kernel.h>
-#include <linux/interrupt.h>
 #include <linux/delay.h>
-#include <linux/irq.h>
-#include <linux/spi/spi.h>
-#include <linux/spi/spi_bitbang.h>
-#include <linux/platform_device.h>
-#include <linux/fsl_devices.h>
 #include <linux/dma-mapping.h>
+#include <linux/fsl_devices.h>
+#include <linux/gpio.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
 #include <linux/mm.h>
+#include <linux/module.h>
 #include <linux/mutex.h>
 #include <linux/of.h>
-#include <linux/of_platform.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
-#include <linux/gpio.h>
 #include <linux/of_gpio.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_bitbang.h>
+#include <linux/types.h>
 
 #include "spi-fsl-lib.h"
 #include "spi-fsl-cpm.h"
@@ -425,16 +425,16 @@ static int fsl_spi_setup(struct spi_device *spi)
 	struct fsl_spi_reg *reg_base;
 	int retval;
 	u32 hw_mode;
-	struct spi_mpc8xxx_cs	*cs = spi->controller_state;
+	struct spi_mpc8xxx_cs *cs = spi_get_ctldata(spi);
 
 	if (!spi->max_speed_hz)
 		return -EINVAL;
 
 	if (!cs) {
-		cs = devm_kzalloc(&spi->dev, sizeof(*cs), GFP_KERNEL);
+		cs = kzalloc(sizeof(*cs), GFP_KERNEL);
 		if (!cs)
 			return -ENOMEM;
-		spi->controller_state = cs;
+		spi_set_ctldata(spi, cs);
 	}
 	mpc8xxx_spi = spi_master_get_devdata(spi->master);
 
@@ -496,9 +496,13 @@ static int fsl_spi_setup(struct spi_device *spi)
 static void fsl_spi_cleanup(struct spi_device *spi)
 {
 	struct mpc8xxx_spi *mpc8xxx_spi = spi_master_get_devdata(spi->master);
+	struct spi_mpc8xxx_cs *cs = spi_get_ctldata(spi);
 
 	if (mpc8xxx_spi->type == TYPE_GRLIB && gpio_is_valid(spi->cs_gpio))
 		gpio_free(spi->cs_gpio);
+
+	kfree(cs);
+	spi_set_ctldata(spi, NULL);
 }
 
 static void fsl_spi_cpu_irq(struct mpc8xxx_spi *mspi, u32 events)
diff --git a/drivers/spi/spi-imx.c b/drivers/spi/spi-imx.c
index 5daff2054ae4..3637847b5370 100644
--- a/drivers/spi/spi-imx.c
+++ b/drivers/spi/spi-imx.c
@@ -21,6 +21,8 @@
 #include <linux/clk.h>
 #include <linux/completion.h>
 #include <linux/delay.h>
+#include <linux/dmaengine.h>
+#include <linux/dma-mapping.h>
 #include <linux/err.h>
 #include <linux/gpio.h>
 #include <linux/interrupt.h>
@@ -37,6 +39,7 @@
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
 
+#include <linux/platform_data/dma-imx.h>
 #include <linux/platform_data/spi-imx.h>
 
 #define DRIVER_NAME "spi_imx"
@@ -51,6 +54,9 @@
 #define MXC_INT_RR	(1 << 0) /* Receive data ready interrupt */
 #define MXC_INT_TE	(1 << 1) /* Transmit FIFO empty interrupt */
 
+/* The maximum  bytes that a sdma BD can transfer.*/
+#define MAX_SDMA_BD_BYTES  (1 << 15)
+#define IMX_DMA_TIMEOUT (msecs_to_jiffies(3000))
 struct spi_imx_config {
 	unsigned int speed_hz;
 	unsigned int bpw;
@@ -95,6 +101,16 @@ struct spi_imx_data {
 	const void *tx_buf;
 	unsigned int txfifo; /* number of words pushed in tx FIFO */
 
+	/* DMA */
+	unsigned int dma_is_inited;
+	unsigned int dma_finished;
+	bool usedma;
+	u32 rx_wml;
+	u32 tx_wml;
+	u32 rxt_wml;
+	struct completion dma_rx_completion;
+	struct completion dma_tx_completion;
+
 	const struct spi_imx_devtype_data *devtype_data;
 	int chipselect[0];
 };
@@ -181,9 +197,21 @@ static unsigned int spi_imx_clkdiv_2(unsigned int fin,
 	return 7;
 }
 
+static bool spi_imx_can_dma(struct spi_master *master, struct spi_device *spi,
+			 struct spi_transfer *transfer)
+{
+	struct spi_imx_data *spi_imx = spi_master_get_devdata(master);
+
+	if (spi_imx->dma_is_inited && (transfer->len > spi_imx->rx_wml)
+	    && (transfer->len > spi_imx->tx_wml))
+		return true;
+	return false;
+}
+
 #define MX51_ECSPI_CTRL		0x08
 #define MX51_ECSPI_CTRL_ENABLE		(1 <<  0)
 #define MX51_ECSPI_CTRL_XCH		(1 <<  2)
+#define MX51_ECSPI_CTRL_SMC		(1 << 3)
 #define MX51_ECSPI_CTRL_MODE_MASK	(0xf << 4)
 #define MX51_ECSPI_CTRL_POSTDIV_OFFSET	8
 #define MX51_ECSPI_CTRL_PREDIV_OFFSET	12
@@ -201,6 +229,18 @@ static unsigned int spi_imx_clkdiv_2(unsigned int fin,
 #define MX51_ECSPI_INT_TEEN		(1 <<  0)
 #define MX51_ECSPI_INT_RREN		(1 <<  3)
 
+#define MX51_ECSPI_DMA      0x14
+#define MX51_ECSPI_DMA_TX_WML_OFFSET	0
+#define MX51_ECSPI_DMA_TX_WML_MASK	0x3F
+#define MX51_ECSPI_DMA_RX_WML_OFFSET	16
+#define MX51_ECSPI_DMA_RX_WML_MASK	(0x3F << 16)
+#define MX51_ECSPI_DMA_RXT_WML_OFFSET	24
+#define MX51_ECSPI_DMA_RXT_WML_MASK	(0x3F << 24)
+
+#define MX51_ECSPI_DMA_TEDEN_OFFSET	7
+#define MX51_ECSPI_DMA_RXDEN_OFFSET	23
+#define MX51_ECSPI_DMA_RXTDEN_OFFSET	31
+
 #define MX51_ECSPI_STAT		0x18
 #define MX51_ECSPI_STAT_RR		(1 <<  3)
 
@@ -257,17 +297,22 @@ static void __maybe_unused mx51_ecspi_intctrl(struct spi_imx_data *spi_imx, int
 
 static void __maybe_unused mx51_ecspi_trigger(struct spi_imx_data *spi_imx)
 {
-	u32 reg;
-
-	reg = readl(spi_imx->base + MX51_ECSPI_CTRL);
-	reg |= MX51_ECSPI_CTRL_XCH;
+	u32 reg = readl(spi_imx->base + MX51_ECSPI_CTRL);
+
+	if (!spi_imx->usedma)
+		reg |= MX51_ECSPI_CTRL_XCH;
+	else if (!spi_imx->dma_finished)
+		reg |= MX51_ECSPI_CTRL_SMC;
+	else
+		reg &= ~MX51_ECSPI_CTRL_SMC;
 	writel(reg, spi_imx->base + MX51_ECSPI_CTRL);
 }
 
 static int __maybe_unused mx51_ecspi_config(struct spi_imx_data *spi_imx,
 		struct spi_imx_config *config)
 {
-	u32 ctrl = MX51_ECSPI_CTRL_ENABLE, cfg = 0;
+	u32 ctrl = MX51_ECSPI_CTRL_ENABLE, cfg = 0, dma = 0;
+	u32 tx_wml_cfg, rx_wml_cfg, rxt_wml_cfg;
 	u32 clk = config->speed_hz, delay;
 
 	/*
@@ -319,6 +364,30 @@ static int __maybe_unused mx51_ecspi_config(struct spi_imx_data *spi_imx,
 	else			/* SCLK is _very_ slow */
 		usleep_range(delay, delay + 10);
 
+	/*
+	 * Configure the DMA register: setup the watermark
+	 * and enable DMA request.
+	 */
+	if (spi_imx->dma_is_inited) {
+		dma = readl(spi_imx->base + MX51_ECSPI_DMA);
+
+		spi_imx->tx_wml = spi_imx_get_fifosize(spi_imx) / 2;
+		spi_imx->rx_wml = spi_imx_get_fifosize(spi_imx) / 2;
+		spi_imx->rxt_wml = spi_imx_get_fifosize(spi_imx) / 2;
+		rx_wml_cfg = spi_imx->rx_wml << MX51_ECSPI_DMA_RX_WML_OFFSET;
+		tx_wml_cfg = spi_imx->tx_wml << MX51_ECSPI_DMA_TX_WML_OFFSET;
+		rxt_wml_cfg = spi_imx->rxt_wml << MX51_ECSPI_DMA_RXT_WML_OFFSET;
+		dma = (dma & ~MX51_ECSPI_DMA_TX_WML_MASK
+			   & ~MX51_ECSPI_DMA_RX_WML_MASK
+			   & ~MX51_ECSPI_DMA_RXT_WML_MASK)
+			   | rx_wml_cfg | tx_wml_cfg | rxt_wml_cfg
+			   |(1 << MX51_ECSPI_DMA_TEDEN_OFFSET)
+			   |(1 << MX51_ECSPI_DMA_RXDEN_OFFSET)
+			   |(1 << MX51_ECSPI_DMA_RXTDEN_OFFSET);
+
+		writel(dma, spi_imx->base + MX51_ECSPI_DMA);
+	}
+
 	return 0;
 }
 
@@ -730,7 +799,186 @@ static int spi_imx_setupxfer(struct spi_device *spi,
 	return 0;
 }
 
-static int spi_imx_transfer(struct spi_device *spi,
+static void spi_imx_sdma_exit(struct spi_imx_data *spi_imx)
+{
+	struct spi_master *master = spi_imx->bitbang.master;
+
+	if (master->dma_rx) {
+		dma_release_channel(master->dma_rx);
+		master->dma_rx = NULL;
+	}
+
+	if (master->dma_tx) {
+		dma_release_channel(master->dma_tx);
+		master->dma_tx = NULL;
+	}
+
+	spi_imx->dma_is_inited = 0;
+}
+
+static int spi_imx_sdma_init(struct device *dev, struct spi_imx_data *spi_imx,
+			     struct spi_master *master,
+			     const struct resource *res)
+{
+	struct dma_slave_config slave_config = {};
+	int ret;
+
+	/* Prepare for TX DMA: */
+	master->dma_tx = dma_request_slave_channel(dev, "tx");
+	if (!master->dma_tx) {
+		dev_err(dev, "cannot get the TX DMA channel!\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	slave_config.direction = DMA_MEM_TO_DEV;
+	slave_config.dst_addr = res->start + MXC_CSPITXDATA;
+	slave_config.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	slave_config.dst_maxburst = spi_imx_get_fifosize(spi_imx) / 2;
+	ret = dmaengine_slave_config(master->dma_tx, &slave_config);
+	if (ret) {
+		dev_err(dev, "error in TX dma configuration.\n");
+		goto err;
+	}
+
+	/* Prepare for RX : */
+	master->dma_rx = dma_request_slave_channel(dev, "rx");
+	if (!master->dma_rx) {
+		dev_dbg(dev, "cannot get the DMA channel.\n");
+		ret = -EINVAL;
+		goto err;
+	}
+
+	slave_config.direction = DMA_DEV_TO_MEM;
+	slave_config.src_addr = res->start + MXC_CSPIRXDATA;
+	slave_config.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	slave_config.src_maxburst = spi_imx_get_fifosize(spi_imx) / 2;
+	ret = dmaengine_slave_config(master->dma_rx, &slave_config);
+	if (ret) {
+		dev_err(dev, "error in RX dma configuration.\n");
+		goto err;
+	}
+
+	init_completion(&spi_imx->dma_rx_completion);
+	init_completion(&spi_imx->dma_tx_completion);
+	master->can_dma = spi_imx_can_dma;
+	master->max_dma_len = MAX_SDMA_BD_BYTES;
+	spi_imx->bitbang.master->flags = SPI_MASTER_MUST_RX |
+					 SPI_MASTER_MUST_TX;
+	spi_imx->dma_is_inited = 1;
+
+	return 0;
+err:
+	spi_imx_sdma_exit(spi_imx);
+	return ret;
+}
+
+static void spi_imx_dma_rx_callback(void *cookie)
+{
+	struct spi_imx_data *spi_imx = (struct spi_imx_data *)cookie;
+
+	complete(&spi_imx->dma_rx_completion);
+}
+
+static void spi_imx_dma_tx_callback(void *cookie)
+{
+	struct spi_imx_data *spi_imx = (struct spi_imx_data *)cookie;
+
+	complete(&spi_imx->dma_tx_completion);
+}
+
+static int spi_imx_dma_transfer(struct spi_imx_data *spi_imx,
+				struct spi_transfer *transfer)
+{
+	struct dma_async_tx_descriptor *desc_tx = NULL, *desc_rx = NULL;
+	int ret;
+	u32 dma;
+	int left;
+	struct spi_master *master = spi_imx->bitbang.master;
+	struct sg_table *tx = &transfer->tx_sg, *rx = &transfer->rx_sg;
+
+	if (tx) {
+		desc_tx = dmaengine_prep_slave_sg(master->dma_tx,
+					tx->sgl, tx->nents, DMA_TO_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc_tx)
+			goto no_dma;
+
+		desc_tx->callback = spi_imx_dma_tx_callback;
+		desc_tx->callback_param = (void *)spi_imx;
+		dmaengine_submit(desc_tx);
+	}
+
+	if (rx) {
+		desc_rx = dmaengine_prep_slave_sg(master->dma_rx,
+					rx->sgl, rx->nents, DMA_FROM_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc_rx)
+			goto no_dma;
+
+		desc_rx->callback = spi_imx_dma_rx_callback;
+		desc_rx->callback_param = (void *)spi_imx;
+		dmaengine_submit(desc_rx);
+	}
+
+	reinit_completion(&spi_imx->dma_rx_completion);
+	reinit_completion(&spi_imx->dma_tx_completion);
+
+	/* Trigger the cspi module. */
+	spi_imx->dma_finished = 0;
+
+	dma = readl(spi_imx->base + MX51_ECSPI_DMA);
+	dma = dma & (~MX51_ECSPI_DMA_RXT_WML_MASK);
+	/* Change RX_DMA_LENGTH trigger dma fetch tail data */
+	left = transfer->len % spi_imx->rxt_wml;
+	if (left)
+		writel(dma | (left << MX51_ECSPI_DMA_RXT_WML_OFFSET),
+				spi_imx->base + MX51_ECSPI_DMA);
+	spi_imx->devtype_data->trigger(spi_imx);
+
+	dma_async_issue_pending(master->dma_tx);
+	dma_async_issue_pending(master->dma_rx);
+	/* Wait SDMA to finish the data transfer.*/
+	ret = wait_for_completion_timeout(&spi_imx->dma_tx_completion,
+						IMX_DMA_TIMEOUT);
+	if (!ret) {
+		pr_warn("%s %s: I/O Error in DMA TX\n",
+			dev_driver_string(&master->dev),
+			dev_name(&master->dev));
+		dmaengine_terminate_all(master->dma_tx);
+	} else {
+		ret = wait_for_completion_timeout(&spi_imx->dma_rx_completion,
+				IMX_DMA_TIMEOUT);
+		if (!ret) {
+			pr_warn("%s %s: I/O Error in DMA RX\n",
+				dev_driver_string(&master->dev),
+				dev_name(&master->dev));
+			spi_imx->devtype_data->reset(spi_imx);
+			dmaengine_terminate_all(master->dma_rx);
+		}
+		writel(dma |
+		       spi_imx->rxt_wml << MX51_ECSPI_DMA_RXT_WML_OFFSET,
+		       spi_imx->base + MX51_ECSPI_DMA);
+	}
+
+	spi_imx->dma_finished = 1;
+	spi_imx->devtype_data->trigger(spi_imx);
+
+	if (!ret)
+		ret = -ETIMEDOUT;
+	else if (ret > 0)
+		ret = transfer->len;
+
+	return ret;
+
+no_dma:
+	pr_warn_once("%s %s: DMA not available, falling back to PIO\n",
+		     dev_driver_string(&master->dev),
+		     dev_name(&master->dev));
+	return -EAGAIN;
+}
+
+static int spi_imx_pio_transfer(struct spi_device *spi,
 				struct spi_transfer *transfer)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
@@ -751,6 +999,24 @@ static int spi_imx_transfer(struct spi_device *spi,
 	return transfer->len;
 }
 
+static int spi_imx_transfer(struct spi_device *spi,
+				struct spi_transfer *transfer)
+{
+	int ret;
+	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
+
+	if (spi_imx->bitbang.master->can_dma &&
+	    spi_imx_can_dma(spi_imx->bitbang.master, spi, transfer)) {
+		spi_imx->usedma = true;
+		ret = spi_imx_dma_transfer(spi_imx, transfer);
+		if (ret != -EAGAIN)
+			return ret;
+	}
+	spi_imx->usedma = false;
+
+	return spi_imx_pio_transfer(spi, transfer);
+}
+
 static int spi_imx_setup(struct spi_device *spi)
 {
 	struct spi_imx_data *spi_imx = spi_master_get_devdata(spi->master);
@@ -911,6 +1177,13 @@ static int spi_imx_probe(struct platform_device *pdev)
 		goto out_put_per;
 
 	spi_imx->spi_clk = clk_get_rate(spi_imx->clk_per);
+	/*
+	 * Only validated on i.mx6 now, can remove the constrain if validated on
+	 * other chips.
+	 */
+	if (spi_imx->devtype_data == &imx51_ecspi_devtype_data
+	    && spi_imx_sdma_init(&pdev->dev, spi_imx, master, res))
+		dev_err(&pdev->dev, "dma setup error,use pio instead\n");
 
 	spi_imx->devtype_data->reset(spi_imx);
 
@@ -949,6 +1222,7 @@ static int spi_imx_remove(struct platform_device *pdev)
 	writel(0, spi_imx->base + MXC_CSPICTRL);
 	clk_unprepare(spi_imx->clk_ipg);
 	clk_unprepare(spi_imx->clk_per);
+	spi_imx_sdma_exit(spi_imx);
 	spi_master_put(master);
 
 	return 0;
diff --git a/drivers/spi/spi-mxs.c b/drivers/spi/spi-mxs.c
index 2884f0c2f5f0..51460878af04 100644
--- a/drivers/spi/spi-mxs.c
+++ b/drivers/spi/spi-mxs.c
@@ -85,7 +85,7 @@ static int mxs_spi_setup_transfer(struct spi_device *dev,
 		mxs_ssp_set_clk_rate(ssp, hz);
 		/*
 		 * Save requested rate, hz, rather than the actual rate,
-		 * ssp->clk_rate.  Otherwise we would set the rate every trasfer
+		 * ssp->clk_rate.  Otherwise we would set the rate every transfer
 		 * when the actual rate is not quite the same as requested rate.
 		 */
 		spi->sck = hz;
@@ -154,12 +154,14 @@ static int mxs_ssp_wait(struct mxs_spi *spi, int offset, int mask, bool set)
 static void mxs_ssp_dma_irq_callback(void *param)
 {
 	struct mxs_spi *spi = param;
+
 	complete(&spi->c);
 }
 
 static irqreturn_t mxs_ssp_irq_handler(int irq, void *dev_id)
 {
 	struct mxs_ssp *ssp = dev_id;
+
 	dev_err(ssp->dev, "%s[%i] CTRL1=%08x STATUS=%08x\n",
 		__func__, __LINE__,
 		readl(ssp->base + HW_SSP_CTRL1(ssp)),
@@ -189,7 +191,7 @@ static int mxs_spi_txrx_dma(struct mxs_spi *spi,
 	if (!len)
 		return -EINVAL;
 
-	dma_xfer = kzalloc(sizeof(*dma_xfer) * sgs, GFP_KERNEL);
+	dma_xfer = kcalloc(sgs, sizeof(*dma_xfer), GFP_KERNEL);
 	if (!dma_xfer)
 		return -ENOMEM;
 
diff --git a/drivers/spi/spi-omap-100k.c b/drivers/spi/spi-omap-100k.c
index 5e91858f6f01..fb522765ce5a 100644
--- a/drivers/spi/spi-omap-100k.c
+++ b/drivers/spi/spi-omap-100k.c
@@ -70,10 +70,6 @@
 #define SPI_STATUS_WE                   (1UL << 1)
 #define SPI_STATUS_RD                   (1UL << 0)
 
-#define WRITE 0
-#define READ  1
-
-
 /* use PIO for small transfers, avoiding DMA setup/teardown overhead and
  * cache operations; better heuristics consider wordsize and bitrate.
  */
diff --git a/drivers/spi/spi-omap2-mcspi.c b/drivers/spi/spi-omap2-mcspi.c
index 68441fa448de..352eed7463ac 100644
--- a/drivers/spi/spi-omap2-mcspi.c
+++ b/drivers/spi/spi-omap2-mcspi.c
@@ -329,7 +329,8 @@ static void omap2_mcspi_set_fifo(const struct spi_device *spi,
 disable_fifo:
 	if (t->rx_buf != NULL)
 		chconf &= ~OMAP2_MCSPI_CHCONF_FFER;
-	else
+
+	if (t->tx_buf != NULL)
 		chconf &= ~OMAP2_MCSPI_CHCONF_FFET;
 
 	mcspi_write_chconf0(spi, chconf);
diff --git a/drivers/spi/spi-orion.c b/drivers/spi/spi-orion.c
index c4675fa8b645..835cdda6f4f5 100644
--- a/drivers/spi/spi-orion.c
+++ b/drivers/spi/spi-orion.c
@@ -18,6 +18,7 @@
 #include <linux/module.h>
 #include <linux/pm_runtime.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/clk.h>
 #include <linux/sizes.h>
 #include <asm/unaligned.h>
@@ -40,13 +41,27 @@
 #define ORION_SPI_MODE_CPHA		(1 << 12)
 #define ORION_SPI_IF_8_16_BIT_MODE	(1 << 5)
 #define ORION_SPI_CLK_PRESCALE_MASK	0x1F
+#define ARMADA_SPI_CLK_PRESCALE_MASK	0xDF
 #define ORION_SPI_MODE_MASK		(ORION_SPI_MODE_CPOL | \
 					 ORION_SPI_MODE_CPHA)
 
+enum orion_spi_type {
+	ORION_SPI,
+	ARMADA_SPI,
+};
+
+struct orion_spi_dev {
+	enum orion_spi_type	typ;
+	unsigned int		min_divisor;
+	unsigned int		max_divisor;
+	u32			prescale_mask;
+};
+
 struct orion_spi {
 	struct spi_master	*master;
 	void __iomem		*base;
 	struct clk              *clk;
+	const struct orion_spi_dev *devdata;
 };
 
 static inline void __iomem *spi_reg(struct orion_spi *orion_spi, u32 reg)
@@ -83,30 +98,66 @@ static int orion_spi_baudrate_set(struct spi_device *spi, unsigned int speed)
 	u32 prescale;
 	u32 reg;
 	struct orion_spi *orion_spi;
+	const struct orion_spi_dev *devdata;
 
 	orion_spi = spi_master_get_devdata(spi->master);
+	devdata = orion_spi->devdata;
 
 	tclk_hz = clk_get_rate(orion_spi->clk);
 
-	/*
-	 * the supported rates are: 4,6,8...30
-	 * round up as we look for equal or less speed
-	 */
-	rate = DIV_ROUND_UP(tclk_hz, speed);
-	rate = roundup(rate, 2);
+	if (devdata->typ == ARMADA_SPI) {
+		unsigned int clk, spr, sppr, sppr2, err;
+		unsigned int best_spr, best_sppr, best_err;
 
-	/* check if requested speed is too small */
-	if (rate > 30)
-		return -EINVAL;
+		best_err = speed;
+		best_spr = 0;
+		best_sppr = 0;
 
-	if (rate < 4)
-		rate = 4;
+		/* Iterate over the valid range looking for best fit */
+		for (sppr = 0; sppr < 8; sppr++) {
+			sppr2 = 0x1 << sppr;
+
+			spr = tclk_hz / sppr2;
+			spr = DIV_ROUND_UP(spr, speed);
+			if ((spr == 0) || (spr > 15))
+				continue;
+
+			clk = tclk_hz / (spr * sppr2);
+			err = speed - clk;
+
+			if (err < best_err) {
+				best_spr = spr;
+				best_sppr = sppr;
+				best_err = err;
+			}
+		}
 
-	/* Convert the rate to SPI clock divisor value.	*/
-	prescale = 0x10 + rate/2;
+		if ((best_sppr == 0) && (best_spr == 0))
+			return -EINVAL;
+
+		prescale = ((best_sppr & 0x6) << 5) |
+			((best_sppr & 0x1) << 4) | best_spr;
+	} else {
+		/*
+		 * the supported rates are: 4,6,8...30
+		 * round up as we look for equal or less speed
+		 */
+		rate = DIV_ROUND_UP(tclk_hz, speed);
+		rate = roundup(rate, 2);
+
+		/* check if requested speed is too small */
+		if (rate > 30)
+			return -EINVAL;
+
+		if (rate < 4)
+			rate = 4;
+
+		/* Convert the rate to SPI clock divisor value.	*/
+		prescale = 0x10 + rate/2;
+	}
 
 	reg = readl(spi_reg(orion_spi, ORION_SPI_IF_CONFIG_REG));
-	reg = ((reg & ~ORION_SPI_CLK_PRESCALE_MASK) | prescale);
+	reg = ((reg & ~devdata->prescale_mask) | prescale);
 	writel(reg, spi_reg(orion_spi, ORION_SPI_IF_CONFIG_REG));
 
 	return 0;
@@ -179,8 +230,8 @@ static inline int orion_spi_wait_till_ready(struct orion_spi *orion_spi)
 	for (i = 0; i < ORION_SPI_WAIT_RDY_MAX_LOOP; i++) {
 		if (readl(spi_reg(orion_spi, ORION_SPI_INT_CAUSE_REG)))
 			return 1;
-		else
-			udelay(1);
+
+		udelay(1);
 	}
 
 	return -1;
@@ -342,8 +393,31 @@ static int orion_spi_reset(struct orion_spi *orion_spi)
 	return 0;
 }
 
+static const struct orion_spi_dev orion_spi_dev_data = {
+	.typ = ORION_SPI,
+	.min_divisor = 4,
+	.max_divisor = 30,
+	.prescale_mask = ORION_SPI_CLK_PRESCALE_MASK,
+};
+
+static const struct orion_spi_dev armada_spi_dev_data = {
+	.typ = ARMADA_SPI,
+	.min_divisor = 1,
+	.max_divisor = 1920,
+	.prescale_mask = ARMADA_SPI_CLK_PRESCALE_MASK,
+};
+
+static const struct of_device_id orion_spi_of_match_table[] = {
+	{ .compatible = "marvell,orion-spi", .data = &orion_spi_dev_data, },
+	{ .compatible = "marvell,armada-370-spi", .data = &armada_spi_dev_data, },
+	{}
+};
+MODULE_DEVICE_TABLE(of, orion_spi_of_match_table);
+
 static int orion_spi_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id;
+	const struct orion_spi_dev *devdata;
 	struct spi_master *master;
 	struct orion_spi *spi;
 	struct resource *r;
@@ -360,6 +434,7 @@ static int orion_spi_probe(struct platform_device *pdev)
 		master->bus_num = pdev->id;
 	if (pdev->dev.of_node) {
 		u32 cell_index;
+
 		if (!of_property_read_u32(pdev->dev.of_node, "cell-index",
 					  &cell_index))
 			master->bus_num = cell_index;
@@ -378,6 +453,10 @@ static int orion_spi_probe(struct platform_device *pdev)
 	spi = spi_master_get_devdata(master);
 	spi->master = master;
 
+	of_id = of_match_device(orion_spi_of_match_table, &pdev->dev);
+	devdata = of_id->data;
+	spi->devdata = devdata;
+
 	spi->clk = devm_clk_get(&pdev->dev, NULL);
 	if (IS_ERR(spi->clk)) {
 		status = PTR_ERR(spi->clk);
@@ -389,8 +468,8 @@ static int orion_spi_probe(struct platform_device *pdev)
 		goto out;
 
 	tclk_hz = clk_get_rate(spi->clk);
-	master->max_speed_hz = DIV_ROUND_UP(tclk_hz, 4);
-	master->min_speed_hz = DIV_ROUND_UP(tclk_hz, 30);
+	master->max_speed_hz = DIV_ROUND_UP(tclk_hz, devdata->min_divisor);
+	master->min_speed_hz = DIV_ROUND_UP(tclk_hz, devdata->max_divisor);
 
 	r = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	spi->base = devm_ioremap_resource(&pdev->dev, r);
@@ -469,12 +548,6 @@ static const struct dev_pm_ops orion_spi_pm_ops = {
 			   NULL)
 };
 
-static const struct of_device_id orion_spi_of_match_table[] = {
-	{ .compatible = "marvell,orion-spi", },
-	{}
-};
-MODULE_DEVICE_TABLE(of, orion_spi_of_match_table);
-
 static struct platform_driver orion_spi_driver = {
 	.driver = {
 		.name	= DRIVER_NAME,
diff --git a/drivers/spi/spi-pl022.c b/drivers/spi/spi-pl022.c
index 1189cfd96477..f35f723816ea 100644
--- a/drivers/spi/spi-pl022.c
+++ b/drivers/spi/spi-pl022.c
@@ -82,6 +82,7 @@
 #define SSP_MIS(r)	(r + 0x01C)
 #define SSP_ICR(r)	(r + 0x020)
 #define SSP_DMACR(r)	(r + 0x024)
+#define SSP_CSR(r)	(r + 0x030) /* vendor extension */
 #define SSP_ITCR(r)	(r + 0x080)
 #define SSP_ITIP(r)	(r + 0x084)
 #define SSP_ITOP(r)	(r + 0x088)
@@ -198,6 +199,12 @@
 #define SSP_DMACR_MASK_TXDMAE		(0x1UL << 1)
 
 /*
+ * SSP Chip Select Control Register - SSP_CSR
+ * (vendor extension)
+ */
+#define SSP_CSR_CSVALUE_MASK		(0x1FUL << 0)
+
+/*
  * SSP Integration Test control Register - SSP_ITCR
  */
 #define SSP_ITCR_MASK_ITEN		(0x1UL << 0)
@@ -313,6 +320,7 @@ enum ssp_writing {
  * @extended_cr: 32 bit wide control register 0 with extra
  * features and extra features in CR1 as found in the ST variants
  * @pl023: supports a subset of the ST extensions called "PL023"
+ * @internal_cs_ctrl: supports chip select control register
  */
 struct vendor_data {
 	int fifodepth;
@@ -321,6 +329,7 @@ struct vendor_data {
 	bool extended_cr;
 	bool pl023;
 	bool loopback;
+	bool internal_cs_ctrl;
 };
 
 /**
@@ -440,9 +449,32 @@ static void null_cs_control(u32 command)
 	pr_debug("pl022: dummy chip select control, CS=0x%x\n", command);
 }
 
+/**
+ * internal_cs_control - Control chip select signals via SSP_CSR.
+ * @pl022: SSP driver private data structure
+ * @command: select/delect the chip
+ *
+ * Used on controller with internal chip select control via SSP_CSR register
+ * (vendor extension). Each of the 5 LSB in the register controls one chip
+ * select signal.
+ */
+static void internal_cs_control(struct pl022 *pl022, u32 command)
+{
+	u32 tmp;
+
+	tmp = readw(SSP_CSR(pl022->virtbase));
+	if (command == SSP_CHIP_SELECT)
+		tmp &= ~BIT(pl022->cur_cs);
+	else
+		tmp |= BIT(pl022->cur_cs);
+	writew(tmp, SSP_CSR(pl022->virtbase));
+}
+
 static void pl022_cs_control(struct pl022 *pl022, u32 command)
 {
-	if (gpio_is_valid(pl022->cur_cs))
+	if (pl022->vendor->internal_cs_ctrl)
+		internal_cs_control(pl022, command);
+	else if (gpio_is_valid(pl022->cur_cs))
 		gpio_set_value(pl022->cur_cs, command);
 	else
 		pl022->cur_chip->cs_control(command);
@@ -2100,6 +2132,10 @@ static int pl022_probe(struct amba_device *adev, const struct amba_id *id)
 	pl022->vendor = id->data;
 	pl022->chipselects = devm_kzalloc(dev, num_cs * sizeof(int),
 					  GFP_KERNEL);
+	if (!pl022->chipselects) {
+		status = -ENOMEM;
+		goto err_no_mem;
+	}
 
 	/*
 	 * Bus Number Which has been Assigned to this SSP controller
@@ -2118,6 +2154,9 @@ static int pl022_probe(struct amba_device *adev, const struct amba_id *id)
 	if (platform_info->num_chipselect && platform_info->chipselects) {
 		for (i = 0; i < num_cs; i++)
 			pl022->chipselects[i] = platform_info->chipselects[i];
+	} else if (pl022->vendor->internal_cs_ctrl) {
+		for (i = 0; i < num_cs; i++)
+			pl022->chipselects[i] = i;
 	} else if (IS_ENABLED(CONFIG_OF)) {
 		for (i = 0; i < num_cs; i++) {
 			int cs_gpio = of_get_named_gpio(np, "cs-gpios", i);
@@ -2136,7 +2175,7 @@ static int pl022_probe(struct amba_device *adev, const struct amba_id *id)
 						cs_gpio);
 				else if (gpio_direction_output(cs_gpio, 1))
 					dev_err(&adev->dev,
-						"could set gpio %d as output\n",
+						"could not set gpio %d as output\n",
 						cs_gpio);
 			}
 		}
@@ -2241,6 +2280,7 @@ static int pl022_probe(struct amba_device *adev, const struct amba_id *id)
 	amba_release_regions(adev);
  err_no_ioregion:
  err_no_gpio:
+ err_no_mem:
 	spi_master_put(master);
 	return status;
 }
@@ -2347,6 +2387,7 @@ static struct vendor_data vendor_arm = {
 	.extended_cr = false,
 	.pl023 = false,
 	.loopback = true,
+	.internal_cs_ctrl = false,
 };
 
 static struct vendor_data vendor_st = {
@@ -2356,6 +2397,7 @@ static struct vendor_data vendor_st = {
 	.extended_cr = true,
 	.pl023 = false,
 	.loopback = true,
+	.internal_cs_ctrl = false,
 };
 
 static struct vendor_data vendor_st_pl023 = {
@@ -2365,6 +2407,17 @@ static struct vendor_data vendor_st_pl023 = {
 	.extended_cr = true,
 	.pl023 = true,
 	.loopback = false,
+	.internal_cs_ctrl = false,
+};
+
+static struct vendor_data vendor_lsi = {
+	.fifodepth = 8,
+	.max_bpw = 16,
+	.unidir = false,
+	.extended_cr = false,
+	.pl023 = false,
+	.loopback = true,
+	.internal_cs_ctrl = true,
 };
 
 static struct amba_id pl022_ids[] = {
@@ -2398,6 +2451,15 @@ static struct amba_id pl022_ids[] = {
 		.mask	= 0xffffffff,
 		.data	= &vendor_st_pl023,
 	},
+	{
+		/*
+		 * PL022 variant that has a chip select control register whih
+		 * allows control of 5 output signals nCS[0:4].
+		 */
+		.id	= 0x000b6022,
+		.mask	= 0x000fffff,
+		.data	= &vendor_lsi,
+	},
 	{ 0, 0 },
 };
 
diff --git a/drivers/spi/spi-pxa2xx-dma.c b/drivers/spi/spi-pxa2xx-dma.c
index c41ff148a2b4..62a9297e96ac 100644
--- a/drivers/spi/spi-pxa2xx-dma.c
+++ b/drivers/spi/spi-pxa2xx-dma.c
@@ -157,7 +157,6 @@ static struct dma_async_tx_descriptor *
 pxa2xx_spi_dma_prepare_one(struct driver_data *drv_data,
 			   enum dma_transfer_direction dir)
 {
-	struct pxa2xx_spi_master *pdata = drv_data->master_info;
 	struct chip_data *chip = drv_data->cur_chip;
 	enum dma_slave_buswidth width;
 	struct dma_slave_config cfg;
@@ -184,7 +183,6 @@ pxa2xx_spi_dma_prepare_one(struct driver_data *drv_data,
 		cfg.dst_addr = drv_data->ssdr_physical;
 		cfg.dst_addr_width = width;
 		cfg.dst_maxburst = chip->dma_burst_size;
-		cfg.slave_id = pdata->tx_slave_id;
 
 		sgt = &drv_data->tx_sgt;
 		nents = drv_data->tx_nents;
@@ -193,7 +191,6 @@ pxa2xx_spi_dma_prepare_one(struct driver_data *drv_data,
 		cfg.src_addr = drv_data->ssdr_physical;
 		cfg.src_addr_width = width;
 		cfg.src_maxburst = chip->dma_burst_size;
-		cfg.slave_id = pdata->rx_slave_id;
 
 		sgt = &drv_data->rx_sgt;
 		nents = drv_data->rx_nents;
@@ -210,14 +207,6 @@ pxa2xx_spi_dma_prepare_one(struct driver_data *drv_data,
 				       DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 }
 
-static bool pxa2xx_spi_dma_filter(struct dma_chan *chan, void *param)
-{
-	const struct pxa2xx_spi_master *pdata = param;
-
-	return chan->chan_id == pdata->tx_chan_id ||
-	       chan->chan_id == pdata->rx_chan_id;
-}
-
 bool pxa2xx_spi_dma_is_possible(size_t len)
 {
 	return len <= MAX_DMA_LEN;
@@ -321,12 +310,12 @@ int pxa2xx_spi_dma_setup(struct driver_data *drv_data)
 		return -ENOMEM;
 
 	drv_data->tx_chan = dma_request_slave_channel_compat(mask,
-				pxa2xx_spi_dma_filter, pdata, dev, "tx");
+				pdata->dma_filter, pdata->tx_param, dev, "tx");
 	if (!drv_data->tx_chan)
 		return -ENODEV;
 
 	drv_data->rx_chan = dma_request_slave_channel_compat(mask,
-				pxa2xx_spi_dma_filter, pdata, dev, "rx");
+				pdata->dma_filter, pdata->rx_param, dev, "rx");
 	if (!drv_data->rx_chan) {
 		dma_release_channel(drv_data->tx_chan);
 		drv_data->tx_chan = NULL;
diff --git a/drivers/spi/spi-pxa2xx-pci.c b/drivers/spi/spi-pxa2xx-pci.c
index c1865c92ccb9..6beee8ce2d68 100644
--- a/drivers/spi/spi-pxa2xx-pci.c
+++ b/drivers/spi/spi-pxa2xx-pci.c
@@ -7,40 +7,90 @@
 #include <linux/of_device.h>
 #include <linux/module.h>
 #include <linux/spi/pxa2xx_spi.h>
+#include <linux/clk.h>
+#include <linux/clk-provider.h>
+
+#include <linux/dmaengine.h>
+#include <linux/platform_data/dma-dw.h>
 
 enum {
 	PORT_CE4100,
 	PORT_BYT,
+	PORT_BSW0,
+	PORT_BSW1,
+	PORT_BSW2,
 };
 
 struct pxa_spi_info {
 	enum pxa_ssp_type type;
 	int port_id;
 	int num_chipselect;
-	int tx_slave_id;
-	int tx_chan_id;
-	int rx_slave_id;
-	int rx_chan_id;
+	unsigned long max_clk_rate;
+
+	/* DMA channel request parameters */
+	void *tx_param;
+	void *rx_param;
 };
 
+static struct dw_dma_slave byt_tx_param = { .dst_id = 0 };
+static struct dw_dma_slave byt_rx_param = { .src_id = 1 };
+
+static struct dw_dma_slave bsw0_tx_param = { .dst_id = 0 };
+static struct dw_dma_slave bsw0_rx_param = { .src_id = 1 };
+static struct dw_dma_slave bsw1_tx_param = { .dst_id = 6 };
+static struct dw_dma_slave bsw1_rx_param = { .src_id = 7 };
+static struct dw_dma_slave bsw2_tx_param = { .dst_id = 8 };
+static struct dw_dma_slave bsw2_rx_param = { .src_id = 9 };
+
+static bool lpss_dma_filter(struct dma_chan *chan, void *param)
+{
+	struct dw_dma_slave *dws = param;
+
+	if (dws->dma_dev != chan->device->dev)
+		return false;
+
+	chan->private = dws;
+	return true;
+}
+
 static struct pxa_spi_info spi_info_configs[] = {
 	[PORT_CE4100] = {
 		.type = PXA25x_SSP,
 		.port_id =  -1,
 		.num_chipselect = -1,
-		.tx_slave_id = -1,
-		.tx_chan_id = -1,
-		.rx_slave_id = -1,
-		.rx_chan_id = -1,
+		.max_clk_rate = 3686400,
 	},
 	[PORT_BYT] = {
 		.type = LPSS_SSP,
 		.port_id = 0,
 		.num_chipselect = 1,
-		.tx_slave_id = 0,
-		.tx_chan_id = 0,
-		.rx_slave_id = 1,
-		.rx_chan_id = 1,
+		.max_clk_rate = 50000000,
+		.tx_param = &byt_tx_param,
+		.rx_param = &byt_rx_param,
+	},
+	[PORT_BSW0] = {
+		.type = LPSS_SSP,
+		.port_id = 0,
+		.num_chipselect = 1,
+		.max_clk_rate = 50000000,
+		.tx_param = &bsw0_tx_param,
+		.rx_param = &bsw0_rx_param,
+	},
+	[PORT_BSW1] = {
+		.type = LPSS_SSP,
+		.port_id = 1,
+		.num_chipselect = 1,
+		.max_clk_rate = 50000000,
+		.tx_param = &bsw1_tx_param,
+		.rx_param = &bsw1_rx_param,
+	},
+	[PORT_BSW2] = {
+		.type = LPSS_SSP,
+		.port_id = 2,
+		.num_chipselect = 1,
+		.max_clk_rate = 50000000,
+		.tx_param = &bsw2_tx_param,
+		.rx_param = &bsw2_rx_param,
 	},
 };
 
@@ -53,6 +103,8 @@ static int pxa2xx_spi_pci_probe(struct pci_dev *dev,
 	struct pxa2xx_spi_master spi_pdata;
 	struct ssp_device *ssp;
 	struct pxa_spi_info *c;
+	char buf[40];
+	struct pci_dev *dma_dev;
 
 	ret = pcim_enable_device(dev);
 	if (ret)
@@ -67,11 +119,29 @@ static int pxa2xx_spi_pci_probe(struct pci_dev *dev,
 	memset(&spi_pdata, 0, sizeof(spi_pdata));
 	spi_pdata.num_chipselect = (c->num_chipselect > 0) ?
 					c->num_chipselect : dev->devfn;
-	spi_pdata.tx_slave_id = c->tx_slave_id;
-	spi_pdata.tx_chan_id = c->tx_chan_id;
-	spi_pdata.rx_slave_id = c->rx_slave_id;
-	spi_pdata.rx_chan_id = c->rx_chan_id;
-	spi_pdata.enable_dma = c->rx_slave_id >= 0 && c->tx_slave_id >= 0;
+
+	dma_dev = pci_get_slot(dev->bus, PCI_DEVFN(PCI_SLOT(dev->devfn), 0));
+
+	if (c->tx_param) {
+		struct dw_dma_slave *slave = c->tx_param;
+
+		slave->dma_dev = &dma_dev->dev;
+		slave->src_master = 1;
+		slave->dst_master = 0;
+	}
+
+	if (c->rx_param) {
+		struct dw_dma_slave *slave = c->rx_param;
+
+		slave->dma_dev = &dma_dev->dev;
+		slave->src_master = 1;
+		slave->dst_master = 0;
+	}
+
+	spi_pdata.dma_filter = lpss_dma_filter;
+	spi_pdata.tx_param = c->tx_param;
+	spi_pdata.rx_param = c->rx_param;
+	spi_pdata.enable_dma = c->rx_param && c->tx_param;
 
 	ssp = &spi_pdata.ssp;
 	ssp->phys_base = pci_resource_start(dev, 0);
@@ -84,6 +154,12 @@ static int pxa2xx_spi_pci_probe(struct pci_dev *dev,
 	ssp->port_id = (c->port_id >= 0) ? c->port_id : dev->devfn;
 	ssp->type = c->type;
 
+	snprintf(buf, sizeof(buf), "pxa2xx-spi.%d", ssp->port_id);
+	ssp->clk = clk_register_fixed_rate(&dev->dev, buf , NULL,
+					CLK_IS_ROOT, c->max_clk_rate);
+	 if (IS_ERR(ssp->clk))
+		return PTR_ERR(ssp->clk);
+
 	memset(&pi, 0, sizeof(pi));
 	pi.parent = &dev->dev;
 	pi.name = "pxa2xx-spi";
@@ -92,8 +168,10 @@ static int pxa2xx_spi_pci_probe(struct pci_dev *dev,
 	pi.size_data = sizeof(spi_pdata);
 
 	pdev = platform_device_register_full(&pi);
-	if (IS_ERR(pdev))
+	if (IS_ERR(pdev)) {
+		clk_unregister(ssp->clk);
 		return PTR_ERR(pdev);
+	}
 
 	pci_set_drvdata(dev, pdev);
 
@@ -103,13 +181,20 @@ static int pxa2xx_spi_pci_probe(struct pci_dev *dev,
 static void pxa2xx_spi_pci_remove(struct pci_dev *dev)
 {
 	struct platform_device *pdev = pci_get_drvdata(dev);
+	struct pxa2xx_spi_master *spi_pdata;
+
+	spi_pdata = dev_get_platdata(&pdev->dev);
 
 	platform_device_unregister(pdev);
+	clk_unregister(spi_pdata->ssp.clk);
 }
 
 static const struct pci_device_id pxa2xx_spi_pci_devices[] = {
 	{ PCI_VDEVICE(INTEL, 0x2e6a), PORT_CE4100 },
 	{ PCI_VDEVICE(INTEL, 0x0f0e), PORT_BYT },
+	{ PCI_VDEVICE(INTEL, 0x228e), PORT_BSW0 },
+	{ PCI_VDEVICE(INTEL, 0x2290), PORT_BSW1 },
+	{ PCI_VDEVICE(INTEL, 0x22ac), PORT_BSW2 },
 	{ },
 };
 MODULE_DEVICE_TABLE(pci, pxa2xx_spi_pci_devices);
diff --git a/drivers/spi/spi-pxa2xx.c b/drivers/spi/spi-pxa2xx.c
index fe792106bdc5..d8a105f76837 100644
--- a/drivers/spi/spi-pxa2xx.c
+++ b/drivers/spi/spi-pxa2xx.c
@@ -1062,8 +1062,6 @@ pxa2xx_spi_acpi_get_pdata(struct platform_device *pdev)
 
 	pdata->num_chipselect = 1;
 	pdata->enable_dma = true;
-	pdata->tx_chan_id = -1;
-	pdata->rx_chan_id = -1;
 
 	return pdata;
 }
@@ -1074,6 +1072,7 @@ static struct acpi_device_id pxa2xx_spi_acpi_match[] = {
 	{ "INT3430", 0 },
 	{ "INT3431", 0 },
 	{ "80860F0E", 0 },
+	{ "8086228E", 0 },
 	{ },
 };
 MODULE_DEVICE_TABLE(acpi, pxa2xx_spi_acpi_match);
diff --git a/drivers/spi/spi-rockchip.c b/drivers/spi/spi-rockchip.c
index c0743604b906..f96ea8a38d64 100644
--- a/drivers/spi/spi-rockchip.c
+++ b/drivers/spi/spi-rockchip.c
@@ -220,7 +220,7 @@ static inline void wait_for_idle(struct rockchip_spi *rs)
 	do {
 		if (!(readl_relaxed(rs->regs + ROCKCHIP_SPI_SR) & SR_BUSY))
 			return;
-	} while (time_before(jiffies, timeout));
+	} while (!time_after(jiffies, timeout));
 
 	dev_warn(rs->dev, "spi controller is in busy state!\n");
 }
@@ -415,7 +415,7 @@ static void rockchip_spi_dma_txcb(void *data)
 	spin_unlock_irqrestore(&rs->lock, flags);
 }
 
-static int rockchip_spi_dma_transfer(struct rockchip_spi *rs)
+static void rockchip_spi_prepare_dma(struct rockchip_spi *rs)
 {
 	unsigned long flags;
 	struct dma_slave_config rxconf, txconf;
@@ -474,8 +474,6 @@ static int rockchip_spi_dma_transfer(struct rockchip_spi *rs)
 		dmaengine_submit(txdesc);
 		dma_async_issue_pending(rs->dma_tx.ch);
 	}
-
-	return 1;
 }
 
 static void rockchip_spi_config(struct rockchip_spi *rs)
@@ -499,7 +497,7 @@ static void rockchip_spi_config(struct rockchip_spi *rs)
 	}
 
 	/* div doesn't support odd number */
-	div = rs->max_freq / rs->speed;
+	div = max_t(u32, rs->max_freq / rs->speed, 1);
 	div = (div + 1) & 0xfffe;
 
 	spi_enable_chip(rs, 0);
@@ -529,7 +527,8 @@ static int rockchip_spi_transfer_one(
 	int ret = 0;
 	struct rockchip_spi *rs = spi_master_get_devdata(master);
 
-	WARN_ON((readl_relaxed(rs->regs + ROCKCHIP_SPI_SR) & SR_BUSY));
+	WARN_ON(readl_relaxed(rs->regs + ROCKCHIP_SPI_SSIENR) &&
+		(readl_relaxed(rs->regs + ROCKCHIP_SPI_SR) & SR_BUSY));
 
 	if (!xfer->tx_buf && !xfer->rx_buf) {
 		dev_err(rs->dev, "No buffer for transfer\n");
@@ -556,16 +555,17 @@ static int rockchip_spi_transfer_one(
 	else if (rs->rx)
 		rs->tmode = CR0_XFM_RO;
 
-	if (master->can_dma && master->can_dma(master, spi, xfer))
+	/* we need prepare dma before spi was enabled */
+	if (master->can_dma && master->can_dma(master, spi, xfer)) {
 		rs->use_dma = 1;
-	else
+		rockchip_spi_prepare_dma(rs);
+	} else {
 		rs->use_dma = 0;
+	}
 
 	rockchip_spi_config(rs);
 
-	if (rs->use_dma)
-		ret = rockchip_spi_dma_transfer(rs);
-	else
+	if (!rs->use_dma)
 		ret = rockchip_spi_pio_transfer(rs);
 
 	return ret;
@@ -678,7 +678,7 @@ static int rockchip_spi_probe(struct platform_device *pdev)
 		rs->dma_tx.addr = (dma_addr_t)(mem->start + ROCKCHIP_SPI_TXDR);
 		rs->dma_rx.addr = (dma_addr_t)(mem->start + ROCKCHIP_SPI_RXDR);
 		rs->dma_tx.direction = DMA_MEM_TO_DEV;
-		rs->dma_tx.direction = DMA_DEV_TO_MEM;
+		rs->dma_rx.direction = DMA_DEV_TO_MEM;
 
 		master->can_dma = rockchip_spi_can_dma;
 		master->dma_tx = rs->dma_tx.ch;
diff --git a/drivers/spi/spi-rspi.c b/drivers/spi/spi-rspi.c
index c850dfdfa9e3..54bb0faec155 100644
--- a/drivers/spi/spi-rspi.c
+++ b/drivers/spi/spi-rspi.c
@@ -87,7 +87,7 @@
 /* RSPI on SH only */
 #define SPCR_TXMD		0x02	/* TX Only Mode (vs. Full Duplex) */
 #define SPCR_SPMS		0x01	/* 3-wire Mode (vs. 4-wire) */
-/* QSPI on R-Car M2 only */
+/* QSPI on R-Car Gen2 only */
 #define SPCR_WSWAP		0x02	/* Word Swap of read-data for DMAC */
 #define SPCR_BSWAP		0x01	/* Byte Swap of read-data for DMAC */
 
@@ -472,25 +472,52 @@ static int rspi_dma_transfer(struct rspi_data *rspi, struct sg_table *tx,
 	dma_cookie_t cookie;
 	int ret;
 
-	if (tx) {
-		desc_tx = dmaengine_prep_slave_sg(rspi->master->dma_tx,
-					tx->sgl, tx->nents, DMA_TO_DEVICE,
-					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-		if (!desc_tx)
-			goto no_dma;
-
-		irq_mask |= SPCR_SPTIE;
-	}
+	/* First prepare and submit the DMA request(s), as this may fail */
 	if (rx) {
 		desc_rx = dmaengine_prep_slave_sg(rspi->master->dma_rx,
 					rx->sgl, rx->nents, DMA_FROM_DEVICE,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-		if (!desc_rx)
-			goto no_dma;
+		if (!desc_rx) {
+			ret = -EAGAIN;
+			goto no_dma_rx;
+		}
+
+		desc_rx->callback = rspi_dma_complete;
+		desc_rx->callback_param = rspi;
+		cookie = dmaengine_submit(desc_rx);
+		if (dma_submit_error(cookie)) {
+			ret = cookie;
+			goto no_dma_rx;
+		}
 
 		irq_mask |= SPCR_SPRIE;
 	}
 
+	if (tx) {
+		desc_tx = dmaengine_prep_slave_sg(rspi->master->dma_tx,
+					tx->sgl, tx->nents, DMA_TO_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc_tx) {
+			ret = -EAGAIN;
+			goto no_dma_tx;
+		}
+
+		if (rx) {
+			/* No callback */
+			desc_tx->callback = NULL;
+		} else {
+			desc_tx->callback = rspi_dma_complete;
+			desc_tx->callback_param = rspi;
+		}
+		cookie = dmaengine_submit(desc_tx);
+		if (dma_submit_error(cookie)) {
+			ret = cookie;
+			goto no_dma_tx;
+		}
+
+		irq_mask |= SPCR_SPTIE;
+	}
+
 	/*
 	 * DMAC needs SPxIE, but if SPxIE is set, the IRQ routine will be
 	 * called. So, this driver disables the IRQ while DMA transfer.
@@ -503,34 +530,24 @@ static int rspi_dma_transfer(struct rspi_data *rspi, struct sg_table *tx,
 	rspi_enable_irq(rspi, irq_mask);
 	rspi->dma_callbacked = 0;
 
-	if (rx) {
-		desc_rx->callback = rspi_dma_complete;
-		desc_rx->callback_param = rspi;
-		cookie = dmaengine_submit(desc_rx);
-		if (dma_submit_error(cookie))
-			return cookie;
+	/* Now start DMA */
+	if (rx)
 		dma_async_issue_pending(rspi->master->dma_rx);
-	}
-	if (tx) {
-		if (rx) {
-			/* No callback */
-			desc_tx->callback = NULL;
-		} else {
-			desc_tx->callback = rspi_dma_complete;
-			desc_tx->callback_param = rspi;
-		}
-		cookie = dmaengine_submit(desc_tx);
-		if (dma_submit_error(cookie))
-			return cookie;
+	if (tx)
 		dma_async_issue_pending(rspi->master->dma_tx);
-	}
 
 	ret = wait_event_interruptible_timeout(rspi->wait,
 					       rspi->dma_callbacked, HZ);
 	if (ret > 0 && rspi->dma_callbacked)
 		ret = 0;
-	else if (!ret)
+	else if (!ret) {
+		dev_err(&rspi->master->dev, "DMA timeout\n");
 		ret = -ETIMEDOUT;
+		if (tx)
+			dmaengine_terminate_all(rspi->master->dma_tx);
+		if (rx)
+			dmaengine_terminate_all(rspi->master->dma_rx);
+	}
 
 	rspi_disable_irq(rspi, irq_mask);
 
@@ -541,11 +558,16 @@ static int rspi_dma_transfer(struct rspi_data *rspi, struct sg_table *tx,
 
 	return ret;
 
-no_dma:
-	pr_warn_once("%s %s: DMA not available, falling back to PIO\n",
-		     dev_driver_string(&rspi->master->dev),
-		     dev_name(&rspi->master->dev));
-	return -EAGAIN;
+no_dma_tx:
+	if (rx)
+		dmaengine_terminate_all(rspi->master->dma_rx);
+no_dma_rx:
+	if (ret == -EAGAIN) {
+		pr_warn_once("%s %s: DMA not available, falling back to PIO\n",
+			     dev_driver_string(&rspi->master->dev),
+			     dev_name(&rspi->master->dev));
+	}
+	return ret;
 }
 
 static void rspi_receive_init(const struct rspi_data *rspi)
@@ -887,20 +909,24 @@ static struct dma_chan *rspi_request_dma_chan(struct device *dev,
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
 
-	chan = dma_request_channel(mask, shdma_chan_filter,
-				   (void *)(unsigned long)id);
+	chan = dma_request_slave_channel_compat(mask, shdma_chan_filter,
+				(void *)(unsigned long)id, dev,
+				dir == DMA_MEM_TO_DEV ? "tx" : "rx");
 	if (!chan) {
-		dev_warn(dev, "dma_request_channel failed\n");
+		dev_warn(dev, "dma_request_slave_channel_compat failed\n");
 		return NULL;
 	}
 
 	memset(&cfg, 0, sizeof(cfg));
 	cfg.slave_id = id;
 	cfg.direction = dir;
-	if (dir == DMA_MEM_TO_DEV)
+	if (dir == DMA_MEM_TO_DEV) {
 		cfg.dst_addr = port_addr;
-	else
+		cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	} else {
 		cfg.src_addr = port_addr;
+		cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_1_BYTE;
+	}
 
 	ret = dmaengine_slave_config(chan, &cfg);
 	if (ret) {
@@ -916,22 +942,30 @@ static int rspi_request_dma(struct device *dev, struct spi_master *master,
 			    const struct resource *res)
 {
 	const struct rspi_plat_data *rspi_pd = dev_get_platdata(dev);
+	unsigned int dma_tx_id, dma_rx_id;
+
+	if (dev->of_node) {
+		/* In the OF case we will get the slave IDs from the DT */
+		dma_tx_id = 0;
+		dma_rx_id = 0;
+	} else if (rspi_pd && rspi_pd->dma_tx_id && rspi_pd->dma_rx_id) {
+		dma_tx_id = rspi_pd->dma_tx_id;
+		dma_rx_id = rspi_pd->dma_rx_id;
+	} else {
+		/* The driver assumes no error. */
+		return 0;
+	}
 
-	if (!rspi_pd || !rspi_pd->dma_rx_id || !rspi_pd->dma_tx_id)
-		return 0;	/* The driver assumes no error. */
-
-	master->dma_rx = rspi_request_dma_chan(dev, DMA_DEV_TO_MEM,
-					       rspi_pd->dma_rx_id,
+	master->dma_tx = rspi_request_dma_chan(dev, DMA_MEM_TO_DEV, dma_tx_id,
 					       res->start + RSPI_SPDR);
-	if (!master->dma_rx)
+	if (!master->dma_tx)
 		return -ENODEV;
 
-	master->dma_tx = rspi_request_dma_chan(dev, DMA_MEM_TO_DEV,
-					       rspi_pd->dma_tx_id,
+	master->dma_rx = rspi_request_dma_chan(dev, DMA_DEV_TO_MEM, dma_rx_id,
 					       res->start + RSPI_SPDR);
-	if (!master->dma_tx) {
-		dma_release_channel(master->dma_rx);
-		master->dma_rx = NULL;
+	if (!master->dma_rx) {
+		dma_release_channel(master->dma_tx);
+		master->dma_tx = NULL;
 		return -ENODEV;
 	}
 
@@ -1024,12 +1058,11 @@ static int rspi_request_irq(struct device *dev, unsigned int irq,
 			    irq_handler_t handler, const char *suffix,
 			    void *dev_id)
 {
-	const char *base = dev_name(dev);
-	size_t len = strlen(base) + strlen(suffix) + 2;
-	char *name = devm_kzalloc(dev, len, GFP_KERNEL);
+	const char *name = devm_kasprintf(dev, GFP_KERNEL, "%s:%s",
+					  dev_name(dev), suffix);
 	if (!name)
 		return -ENOMEM;
-	snprintf(name, len, "%s:%s", base, suffix);
+
 	return devm_request_irq(dev, irq, handler, 0, name, dev_id);
 }
 
@@ -1062,7 +1095,7 @@ static int rspi_probe(struct platform_device *pdev)
 			master->num_chipselect = rspi_pd->num_chipselect;
 		else
 			master->num_chipselect = 2; /* default */
-	};
+	}
 
 	/* ops parameter check */
 	if (!ops->set_config_register) {
diff --git a/drivers/spi/spi-sh-msiof.c b/drivers/spi/spi-sh-msiof.c
index 2a4354dcd661..3f365402fcc0 100644
--- a/drivers/spi/spi-sh-msiof.c
+++ b/drivers/spi/spi-sh-msiof.c
@@ -636,17 +636,7 @@ static int sh_msiof_dma_once(struct sh_msiof_spi_priv *p, const void *tx,
 	dma_cookie_t cookie;
 	int ret;
 
-	if (tx) {
-		ier_bits |= IER_TDREQE | IER_TDMAE;
-		dma_sync_single_for_device(p->master->dma_tx->device->dev,
-					   p->tx_dma_addr, len, DMA_TO_DEVICE);
-		desc_tx = dmaengine_prep_slave_single(p->master->dma_tx,
-					p->tx_dma_addr, len, DMA_TO_DEVICE,
-					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
-		if (!desc_tx)
-			return -EAGAIN;
-	}
-
+	/* First prepare and submit the DMA request(s), as this may fail */
 	if (rx) {
 		ier_bits |= IER_RDREQE | IER_RDMAE;
 		desc_rx = dmaengine_prep_slave_single(p->master->dma_rx,
@@ -654,30 +644,26 @@ static int sh_msiof_dma_once(struct sh_msiof_spi_priv *p, const void *tx,
 					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
 		if (!desc_rx)
 			return -EAGAIN;
-	}
-
-	/* 1 stage FIFO watermarks for DMA */
-	sh_msiof_write(p, FCTR, FCTR_TFWM_1 | FCTR_RFWM_1);
 
-	/* setup msiof transfer mode registers (32-bit words) */
-	sh_msiof_spi_set_mode_regs(p, tx, rx, 32, len / 4);
-
-	sh_msiof_write(p, IER, ier_bits);
-
-	reinit_completion(&p->done);
-
-	if (rx) {
 		desc_rx->callback = sh_msiof_dma_complete;
 		desc_rx->callback_param = p;
 		cookie = dmaengine_submit(desc_rx);
-		if (dma_submit_error(cookie)) {
-			ret = cookie;
-			goto stop_ier;
-		}
-		dma_async_issue_pending(p->master->dma_rx);
+		if (dma_submit_error(cookie))
+			return cookie;
 	}
 
 	if (tx) {
+		ier_bits |= IER_TDREQE | IER_TDMAE;
+		dma_sync_single_for_device(p->master->dma_tx->device->dev,
+					   p->tx_dma_addr, len, DMA_TO_DEVICE);
+		desc_tx = dmaengine_prep_slave_single(p->master->dma_tx,
+					p->tx_dma_addr, len, DMA_TO_DEVICE,
+					DMA_PREP_INTERRUPT | DMA_CTRL_ACK);
+		if (!desc_tx) {
+			ret = -EAGAIN;
+			goto no_dma_tx;
+		}
+
 		if (rx) {
 			/* No callback */
 			desc_tx->callback = NULL;
@@ -688,15 +674,30 @@ static int sh_msiof_dma_once(struct sh_msiof_spi_priv *p, const void *tx,
 		cookie = dmaengine_submit(desc_tx);
 		if (dma_submit_error(cookie)) {
 			ret = cookie;
-			goto stop_rx;
+			goto no_dma_tx;
 		}
-		dma_async_issue_pending(p->master->dma_tx);
 	}
 
+	/* 1 stage FIFO watermarks for DMA */
+	sh_msiof_write(p, FCTR, FCTR_TFWM_1 | FCTR_RFWM_1);
+
+	/* setup msiof transfer mode registers (32-bit words) */
+	sh_msiof_spi_set_mode_regs(p, tx, rx, 32, len / 4);
+
+	sh_msiof_write(p, IER, ier_bits);
+
+	reinit_completion(&p->done);
+
+	/* Now start DMA */
+	if (rx)
+		dma_async_issue_pending(p->master->dma_rx);
+	if (tx)
+		dma_async_issue_pending(p->master->dma_tx);
+
 	ret = sh_msiof_spi_start(p, rx);
 	if (ret) {
 		dev_err(&p->pdev->dev, "failed to start hardware\n");
-		goto stop_tx;
+		goto stop_dma;
 	}
 
 	/* wait for tx fifo to be emptied / rx fifo to be filled */
@@ -726,13 +727,12 @@ static int sh_msiof_dma_once(struct sh_msiof_spi_priv *p, const void *tx,
 stop_reset:
 	sh_msiof_reset_str(p);
 	sh_msiof_spi_stop(p, rx);
-stop_tx:
+stop_dma:
 	if (tx)
 		dmaengine_terminate_all(p->master->dma_tx);
-stop_rx:
+no_dma_tx:
 	if (rx)
 		dmaengine_terminate_all(p->master->dma_rx);
-stop_ier:
 	sh_msiof_write(p, IER, 0);
 	return ret;
 }
@@ -928,6 +928,9 @@ static const struct of_device_id sh_msiof_match[] = {
 	{ .compatible = "renesas,sh-mobile-msiof", .data = &sh_data },
 	{ .compatible = "renesas,msiof-r8a7790",   .data = &r8a779x_data },
 	{ .compatible = "renesas,msiof-r8a7791",   .data = &r8a779x_data },
+	{ .compatible = "renesas,msiof-r8a7792",   .data = &r8a779x_data },
+	{ .compatible = "renesas,msiof-r8a7793",   .data = &r8a779x_data },
+	{ .compatible = "renesas,msiof-r8a7794",   .data = &r8a779x_data },
 	{},
 };
 MODULE_DEVICE_TABLE(of, sh_msiof_match);
@@ -972,20 +975,24 @@ static struct dma_chan *sh_msiof_request_dma_chan(struct device *dev,
 	dma_cap_zero(mask);
 	dma_cap_set(DMA_SLAVE, mask);
 
-	chan = dma_request_channel(mask, shdma_chan_filter,
-				  (void *)(unsigned long)id);
+	chan = dma_request_slave_channel_compat(mask, shdma_chan_filter,
+				(void *)(unsigned long)id, dev,
+				dir == DMA_MEM_TO_DEV ? "tx" : "rx");
 	if (!chan) {
-		dev_warn(dev, "dma_request_channel failed\n");
+		dev_warn(dev, "dma_request_slave_channel_compat failed\n");
 		return NULL;
 	}
 
 	memset(&cfg, 0, sizeof(cfg));
 	cfg.slave_id = id;
 	cfg.direction = dir;
-	if (dir == DMA_MEM_TO_DEV)
+	if (dir == DMA_MEM_TO_DEV) {
 		cfg.dst_addr = port_addr;
-	else
+		cfg.dst_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	} else {
 		cfg.src_addr = port_addr;
+		cfg.src_addr_width = DMA_SLAVE_BUSWIDTH_4_BYTES;
+	}
 
 	ret = dmaengine_slave_config(chan, &cfg);
 	if (ret) {
@@ -1002,12 +1009,22 @@ static int sh_msiof_request_dma(struct sh_msiof_spi_priv *p)
 	struct platform_device *pdev = p->pdev;
 	struct device *dev = &pdev->dev;
 	const struct sh_msiof_spi_info *info = dev_get_platdata(dev);
+	unsigned int dma_tx_id, dma_rx_id;
 	const struct resource *res;
 	struct spi_master *master;
 	struct device *tx_dev, *rx_dev;
 
-	if (!info || !info->dma_tx_id || !info->dma_rx_id)
-		return 0;	/* The driver assumes no error */
+	if (dev->of_node) {
+		/* In the OF case we will get the slave IDs from the DT */
+		dma_tx_id = 0;
+		dma_rx_id = 0;
+	} else if (info && info->dma_tx_id && info->dma_rx_id) {
+		dma_tx_id = info->dma_tx_id;
+		dma_rx_id = info->dma_rx_id;
+	} else {
+		/* The driver assumes no error */
+		return 0;
+	}
 
 	/* The DMA engine uses the second register set, if present */
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
@@ -1016,13 +1033,13 @@ static int sh_msiof_request_dma(struct sh_msiof_spi_priv *p)
 
 	master = p->master;
 	master->dma_tx = sh_msiof_request_dma_chan(dev, DMA_MEM_TO_DEV,
-						   info->dma_tx_id,
+						   dma_tx_id,
 						   res->start + TFDR);
 	if (!master->dma_tx)
 		return -ENODEV;
 
 	master->dma_rx = sh_msiof_request_dma_chan(dev, DMA_DEV_TO_MEM,
-						   info->dma_rx_id,
+						   dma_rx_id,
 						   res->start + RFDR);
 	if (!master->dma_rx)
 		goto free_tx_chan;
@@ -1205,6 +1222,9 @@ static struct platform_device_id spi_driver_ids[] = {
 	{ "spi_sh_msiof",	(kernel_ulong_t)&sh_data },
 	{ "spi_r8a7790_msiof",	(kernel_ulong_t)&r8a779x_data },
 	{ "spi_r8a7791_msiof",	(kernel_ulong_t)&r8a779x_data },
+	{ "spi_r8a7792_msiof",	(kernel_ulong_t)&r8a779x_data },
+	{ "spi_r8a7793_msiof",	(kernel_ulong_t)&r8a779x_data },
+	{ "spi_r8a7794_msiof",	(kernel_ulong_t)&r8a779x_data },
 	{},
 };
 MODULE_DEVICE_TABLE(platform, spi_driver_ids);
diff --git a/drivers/spi/spi-sirf.c b/drivers/spi/spi-sirf.c
index 95ac276eaafe..39e2c0a55a28 100644
--- a/drivers/spi/spi-sirf.c
+++ b/drivers/spi/spi-sirf.c
@@ -62,15 +62,15 @@
 #define SIRFSOC_SPI_TRAN_DAT_FORMAT_12	(1 << 26)
 #define SIRFSOC_SPI_TRAN_DAT_FORMAT_16	(2 << 26)
 #define SIRFSOC_SPI_TRAN_DAT_FORMAT_32	(3 << 26)
-#define SIRFSOC_SPI_CMD_BYTE_NUM(x)		((x & 3) << 28)
-#define SIRFSOC_SPI_ENA_AUTO_CLR		BIT(30)
-#define SIRFSOC_SPI_MUL_DAT_MODE		BIT(31)
+#define SIRFSOC_SPI_CMD_BYTE_NUM(x)	((x & 3) << 28)
+#define SIRFSOC_SPI_ENA_AUTO_CLR	BIT(30)
+#define SIRFSOC_SPI_MUL_DAT_MODE	BIT(31)
 
 /* Interrupt Enable */
-#define SIRFSOC_SPI_RX_DONE_INT_EN		BIT(0)
-#define SIRFSOC_SPI_TX_DONE_INT_EN		BIT(1)
-#define SIRFSOC_SPI_RX_OFLOW_INT_EN		BIT(2)
-#define SIRFSOC_SPI_TX_UFLOW_INT_EN		BIT(3)
+#define SIRFSOC_SPI_RX_DONE_INT_EN	BIT(0)
+#define SIRFSOC_SPI_TX_DONE_INT_EN	BIT(1)
+#define SIRFSOC_SPI_RX_OFLOW_INT_EN	BIT(2)
+#define SIRFSOC_SPI_TX_UFLOW_INT_EN	BIT(3)
 #define SIRFSOC_SPI_RX_IO_DMA_INT_EN	BIT(4)
 #define SIRFSOC_SPI_TX_IO_DMA_INT_EN	BIT(5)
 #define SIRFSOC_SPI_RXFIFO_FULL_INT_EN	BIT(6)
@@ -79,7 +79,7 @@
 #define SIRFSOC_SPI_TXFIFO_THD_INT_EN	BIT(9)
 #define SIRFSOC_SPI_FRM_END_INT_EN	BIT(10)
 
-#define SIRFSOC_SPI_INT_MASK_ALL		0x1FFF
+#define SIRFSOC_SPI_INT_MASK_ALL	0x1FFF
 
 /* Interrupt status */
 #define SIRFSOC_SPI_RX_DONE		BIT(0)
@@ -170,8 +170,7 @@ struct sirfsoc_spi {
 	 * command model
 	 */
 	bool	tx_by_cmd;
-
-	int chipselect[0];
+	bool	hw_cs;
 };
 
 static void spi_sirfsoc_rx_word_u8(struct sirfsoc_spi *sspi)
@@ -304,7 +303,7 @@ static void spi_sirfsoc_dma_fini_callback(void *data)
 	complete(dma_complete);
 }
 
-static int spi_sirfsoc_cmd_transfer(struct spi_device *spi,
+static void spi_sirfsoc_cmd_transfer(struct spi_device *spi,
 	struct spi_transfer *t)
 {
 	struct sirfsoc_spi *sspi;
@@ -312,6 +311,8 @@ static int spi_sirfsoc_cmd_transfer(struct spi_device *spi,
 	u32 cmd;
 
 	sspi = spi_master_get_devdata(spi->master);
+	writel(SIRFSOC_SPI_FIFO_RESET, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
+	writel(SIRFSOC_SPI_FIFO_START, sspi->base + SIRFSOC_SPI_TXFIFO_OP);
 	memcpy(&cmd, sspi->tx, t->len);
 	if (sspi->word_width == 1 && !(spi->mode & SPI_LSB_FIRST))
 		cmd = cpu_to_be32(cmd) >>
@@ -326,10 +327,9 @@ static int spi_sirfsoc_cmd_transfer(struct spi_device *spi,
 		sspi->base + SIRFSOC_SPI_TX_RX_EN);
 	if (wait_for_completion_timeout(&sspi->tx_done, timeout) == 0) {
 		dev_err(&spi->dev, "cmd transfer timeout\n");
-		return 0;
+		return;
 	}
-
-	return t->len;
+	sspi->left_rx_word -= t->len;
 }
 
 static void spi_sirfsoc_dma_transfer(struct spi_device *spi,
@@ -438,7 +438,8 @@ static void spi_sirfsoc_pio_transfer(struct spi_device *spi,
 			sspi->tx_word(sspi);
 		writel(SIRFSOC_SPI_TXFIFO_EMPTY_INT_EN |
 			SIRFSOC_SPI_TX_UFLOW_INT_EN |
-			SIRFSOC_SPI_RX_OFLOW_INT_EN,
+			SIRFSOC_SPI_RX_OFLOW_INT_EN |
+			SIRFSOC_SPI_RX_IO_DMA_INT_EN,
 			sspi->base + SIRFSOC_SPI_INT_EN);
 		writel(SIRFSOC_SPI_RX_EN | SIRFSOC_SPI_TX_EN,
 			sspi->base + SIRFSOC_SPI_TX_RX_EN);
@@ -484,7 +485,7 @@ static void spi_sirfsoc_chipselect(struct spi_device *spi, int value)
 {
 	struct sirfsoc_spi *sspi = spi_master_get_devdata(spi->master);
 
-	if (sspi->chipselect[spi->chip_select] == 0) {
+	if (sspi->hw_cs) {
 		u32 regval = readl(sspi->base + SIRFSOC_SPI_CTRL);
 		switch (value) {
 		case BITBANG_CS_ACTIVE:
@@ -502,14 +503,13 @@ static void spi_sirfsoc_chipselect(struct spi_device *spi, int value)
 		}
 		writel(regval, sspi->base + SIRFSOC_SPI_CTRL);
 	} else {
-		int gpio = sspi->chipselect[spi->chip_select];
 		switch (value) {
 		case BITBANG_CS_ACTIVE:
-			gpio_direction_output(gpio,
+			gpio_direction_output(spi->cs_gpio,
 					spi->mode & SPI_CS_HIGH ? 1 : 0);
 			break;
 		case BITBANG_CS_INACTIVE:
-			gpio_direction_output(gpio,
+			gpio_direction_output(spi->cs_gpio,
 					spi->mode & SPI_CS_HIGH ? 0 : 1);
 			break;
 		}
@@ -603,8 +603,8 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 		sspi->tx_by_cmd = false;
 	}
 	/*
-	 * set spi controller in RISC chipselect mode, we are controlling CS by
-	 * software BITBANG_CS_ACTIVE and BITBANG_CS_INACTIVE.
+	 * it should never set to hardware cs mode because in hardware cs mode,
+	 * cs signal can't controlled by driver.
 	 */
 	regval |= SIRFSOC_SPI_CS_IO_MODE;
 	writel(regval, sspi->base + SIRFSOC_SPI_CTRL);
@@ -627,9 +627,17 @@ spi_sirfsoc_setup_transfer(struct spi_device *spi, struct spi_transfer *t)
 
 static int spi_sirfsoc_setup(struct spi_device *spi)
 {
+	struct sirfsoc_spi *sspi;
+
 	if (!spi->max_speed_hz)
 		return -EINVAL;
 
+	sspi = spi_master_get_devdata(spi->master);
+
+	if (spi->cs_gpio == -ENOENT)
+		sspi->hw_cs = true;
+	else
+		sspi->hw_cs = false;
 	return spi_sirfsoc_setup_transfer(spi, NULL);
 }
 
@@ -638,19 +646,10 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 	struct sirfsoc_spi *sspi;
 	struct spi_master *master;
 	struct resource *mem_res;
-	int num_cs, cs_gpio, irq;
-	int i;
-	int ret;
+	int irq;
+	int i, ret;
 
-	ret = of_property_read_u32(pdev->dev.of_node,
-			"sirf,spi-num-chipselects", &num_cs);
-	if (ret < 0) {
-		dev_err(&pdev->dev, "Unable to get chip select number\n");
-		goto err_cs;
-	}
-
-	master = spi_alloc_master(&pdev->dev,
-			sizeof(*sspi) + sizeof(int) * num_cs);
+	master = spi_alloc_master(&pdev->dev, sizeof(*sspi));
 	if (!master) {
 		dev_err(&pdev->dev, "Unable to allocate SPI master\n");
 		return -ENOMEM;
@@ -658,32 +657,6 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 	platform_set_drvdata(pdev, master);
 	sspi = spi_master_get_devdata(master);
 
-	master->num_chipselect = num_cs;
-
-	for (i = 0; i < master->num_chipselect; i++) {
-		cs_gpio = of_get_named_gpio(pdev->dev.of_node, "cs-gpios", i);
-		if (cs_gpio < 0) {
-			dev_err(&pdev->dev, "can't get cs gpio from DT\n");
-			ret = -ENODEV;
-			goto free_master;
-		}
-
-		sspi->chipselect[i] = cs_gpio;
-		if (cs_gpio == 0)
-			continue; /* use cs from spi controller */
-
-		ret = gpio_request(cs_gpio, DRIVER_NAME);
-		if (ret) {
-			while (i > 0) {
-				i--;
-				if (sspi->chipselect[i] > 0)
-					gpio_free(sspi->chipselect[i]);
-			}
-			dev_err(&pdev->dev, "fail to request cs gpios\n");
-			goto free_master;
-		}
-	}
-
 	mem_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	sspi->base = devm_ioremap_resource(&pdev->dev, mem_res);
 	if (IS_ERR(sspi->base)) {
@@ -753,7 +726,21 @@ static int spi_sirfsoc_probe(struct platform_device *pdev)
 	ret = spi_bitbang_start(&sspi->bitbang);
 	if (ret)
 		goto free_dummypage;
-
+	for (i = 0; master->cs_gpios && i < master->num_chipselect; i++) {
+		if (master->cs_gpios[i] == -ENOENT)
+			continue;
+		if (!gpio_is_valid(master->cs_gpios[i])) {
+			dev_err(&pdev->dev, "no valid gpio\n");
+			ret = -EINVAL;
+			goto free_dummypage;
+		}
+		ret = devm_gpio_request(&pdev->dev,
+				master->cs_gpios[i], DRIVER_NAME);
+		if (ret) {
+			dev_err(&pdev->dev, "failed to request gpio\n");
+			goto free_dummypage;
+		}
+	}
 	dev_info(&pdev->dev, "registerred, bus number = %d\n", master->bus_num);
 
 	return 0;
@@ -768,7 +755,7 @@ free_rx_dma:
 	dma_release_channel(sspi->rx_chan);
 free_master:
 	spi_master_put(master);
-err_cs:
+
 	return ret;
 }
 
@@ -776,16 +763,11 @@ static int  spi_sirfsoc_remove(struct platform_device *pdev)
 {
 	struct spi_master *master;
 	struct sirfsoc_spi *sspi;
-	int i;
 
 	master = platform_get_drvdata(pdev);
 	sspi = spi_master_get_devdata(master);
 
 	spi_bitbang_stop(&sspi->bitbang);
-	for (i = 0; i < master->num_chipselect; i++) {
-		if (sspi->chipselect[i] > 0)
-			gpio_free(sspi->chipselect[i]);
-	}
 	kfree(sspi->dummypage);
 	clk_disable_unprepare(sspi->clk);
 	clk_put(sspi->clk);
diff --git a/drivers/spi/spi-tegra114.c b/drivers/spi/spi-tegra114.c
index e4a85ada861d..795bcbc0131b 100644
--- a/drivers/spi/spi-tegra114.c
+++ b/drivers/spi/spi-tegra114.c
@@ -302,6 +302,7 @@ static unsigned tegra_spi_fill_tx_fifo_from_client_txbuf(
 		max_n_32bit = DIV_ROUND_UP(nbytes, 4);
 		for (count = 0; count < max_n_32bit; count++) {
 			u32 x = 0;
+
 			for (i = 0; (i < 4) && nbytes; i++, nbytes--)
 				x |= (u32)(*tx_buf++) << (i * 8);
 			tegra_spi_writel(tspi, x, SPI_TX_FIFO);
@@ -312,6 +313,7 @@ static unsigned tegra_spi_fill_tx_fifo_from_client_txbuf(
 		nbytes = written_words * tspi->bytes_per_word;
 		for (count = 0; count < max_n_32bit; count++) {
 			u32 x = 0;
+
 			for (i = 0; nbytes && (i < tspi->bytes_per_word);
 							i++, nbytes--)
 				x |= (u32)(*tx_buf++) << (i * 8);
@@ -338,6 +340,7 @@ static unsigned int tegra_spi_read_rx_fifo_to_client_rxbuf(
 		len = tspi->curr_dma_words * tspi->bytes_per_word;
 		for (count = 0; count < rx_full_count; count++) {
 			u32 x = tegra_spi_readl(tspi, SPI_RX_FIFO);
+
 			for (i = 0; len && (i < 4); i++, len--)
 				*rx_buf++ = (x >> i*8) & 0xFF;
 		}
@@ -345,8 +348,10 @@ static unsigned int tegra_spi_read_rx_fifo_to_client_rxbuf(
 		read_words += tspi->curr_dma_words;
 	} else {
 		u32 rx_mask = ((u32)1 << t->bits_per_word) - 1;
+
 		for (count = 0; count < rx_full_count; count++) {
 			u32 x = tegra_spi_readl(tspi, SPI_RX_FIFO) & rx_mask;
+
 			for (i = 0; (i < tspi->bytes_per_word); i++)
 				*rx_buf++ = (x >> (i*8)) & 0xFF;
 		}
@@ -365,6 +370,7 @@ static void tegra_spi_copy_client_txbuf_to_spi_txbuf(
 
 	if (tspi->is_packed) {
 		unsigned len = tspi->curr_dma_words * tspi->bytes_per_word;
+
 		memcpy(tspi->tx_dma_buf, t->tx_buf + tspi->cur_pos, len);
 	} else {
 		unsigned int i;
@@ -374,6 +380,7 @@ static void tegra_spi_copy_client_txbuf_to_spi_txbuf(
 
 		for (count = 0; count < tspi->curr_dma_words; count++) {
 			u32 x = 0;
+
 			for (i = 0; consume && (i < tspi->bytes_per_word);
 							i++, consume--)
 				x |= (u32)(*tx_buf++) << (i * 8);
@@ -396,6 +403,7 @@ static void tegra_spi_copy_spi_rxbuf_to_client_rxbuf(
 
 	if (tspi->is_packed) {
 		unsigned len = tspi->curr_dma_words * tspi->bytes_per_word;
+
 		memcpy(t->rx_buf + tspi->cur_rx_pos, tspi->rx_dma_buf, len);
 	} else {
 		unsigned int i;
@@ -405,6 +413,7 @@ static void tegra_spi_copy_spi_rxbuf_to_client_rxbuf(
 
 		for (count = 0; count < tspi->curr_dma_words; count++) {
 			u32 x = tspi->rx_dma_buf[count] & rx_mask;
+
 			for (i = 0; (i < tspi->bytes_per_word); i++)
 				*rx_buf++ = (x >> (i*8)) & 0xFF;
 		}
diff --git a/drivers/spi/spi-tegra20-sflash.c b/drivers/spi/spi-tegra20-sflash.c
index 3548ce25c08f..cd66fe7b78a9 100644
--- a/drivers/spi/spi-tegra20-sflash.c
+++ b/drivers/spi/spi-tegra20-sflash.c
@@ -99,7 +99,7 @@
 #define SPI_TX_TRIG_MASK		(0x3 << 16)
 #define SPI_TX_TRIG_1W			(0x0 << 16)
 #define SPI_TX_TRIG_4W			(0x1 << 16)
-#define SPI_DMA_BLK_COUNT(count)	(((count) - 1) & 0xFFFF);
+#define SPI_DMA_BLK_COUNT(count)	(((count) - 1) & 0xFFFF)
 
 #define SPI_TX_FIFO			0x10
 #define SPI_RX_FIFO			0x20
@@ -221,6 +221,7 @@ static int tegra_sflash_read_rx_fifo_to_client_rxbuf(
 	while (!(status & SPI_RXF_EMPTY)) {
 		int i;
 		u32 x = tegra_sflash_readl(tsd, SPI_RX_FIFO);
+
 		for (i = 0; (i < tsd->bytes_per_word); i++)
 			*rx_buf++ = (x >> (i*8)) & 0xFF;
 		read_words++;
diff --git a/drivers/spi/spi-txx9.c b/drivers/spi/spi-txx9.c
index 5f183baa91a9..2501a8373e89 100644
--- a/drivers/spi/spi-txx9.c
+++ b/drivers/spi/spi-txx9.c
@@ -97,6 +97,7 @@ static void txx9spi_cs_func(struct spi_device *spi, struct txx9spi *c,
 		int on, unsigned int cs_delay)
 {
 	int val = (spi->mode & SPI_CS_HIGH) ? on : !on;
+
 	if (on) {
 		/* deselect the chip with cs_change hint in last transfer */
 		if (c->last_chipselect >= 0)
@@ -188,6 +189,7 @@ static void txx9spi_work_one(struct txx9spi *c, struct spi_message *m)
 		if (prev_speed_hz != speed_hz
 				|| prev_bits_per_word != bits_per_word) {
 			int n = DIV_ROUND_UP(c->baseclk, speed_hz) - 1;
+
 			n = clamp(n, SPI_MIN_DIVIDER, SPI_MAX_DIVIDER);
 			/* enter config mode */
 			txx9spi_wr(c, mcr | TXx9_SPMCR_CONFIG | TXx9_SPMCR_BCLR,
diff --git a/drivers/spi/spi-xilinx.c b/drivers/spi/spi-xilinx.c
index 4d8efb16573d..79bd84f43430 100644
--- a/drivers/spi/spi-xilinx.c
+++ b/drivers/spi/spi-xilinx.c
@@ -471,7 +471,6 @@ static struct platform_driver xilinx_spi_driver = {
 	.remove = xilinx_spi_remove,
 	.driver = {
 		.name = XILINX_SPI_NAME,
-		.owner = THIS_MODULE,
 		.of_match_table = xilinx_spi_of_match,
 	},
 };
diff --git a/drivers/spi/spi-xtensa-xtfpga.c b/drivers/spi/spi-xtensa-xtfpga.c
index 41e158187f9d..0dc5df5233a9 100644
--- a/drivers/spi/spi-xtensa-xtfpga.c
+++ b/drivers/spi/spi-xtensa-xtfpga.c
@@ -46,6 +46,7 @@ static inline unsigned int xtfpga_spi_read32(const struct xtfpga_spi *spi,
 static inline void xtfpga_spi_wait_busy(struct xtfpga_spi *xspi)
 {
 	unsigned i;
+
 	for (i = 0; xtfpga_spi_read32(xspi, XTFPGA_SPI_BUSY) &&
 	     i < BUSY_WAIT_US; ++i)
 		udelay(1);
diff --git a/drivers/spi/spi.c b/drivers/spi/spi.c
index e0531baf2782..ebcb33df2eb2 100644
--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -35,6 +35,7 @@
 #include <linux/spi/spi.h>
 #include <linux/of_gpio.h>
 #include <linux/pm_runtime.h>
+#include <linux/pm_domain.h>
 #include <linux/export.h>
 #include <linux/sched/rt.h>
 #include <linux/delay.h>
@@ -264,10 +265,12 @@ static int spi_drv_probe(struct device *dev)
 	if (ret)
 		return ret;
 
-	acpi_dev_pm_attach(dev, true);
-	ret = sdrv->probe(to_spi_device(dev));
-	if (ret)
-		acpi_dev_pm_detach(dev, true);
+	ret = dev_pm_domain_attach(dev, true);
+	if (ret != -EPROBE_DEFER) {
+		ret = sdrv->probe(to_spi_device(dev));
+		if (ret)
+			dev_pm_domain_detach(dev, true);
+	}
 
 	return ret;
 }
@@ -278,7 +281,7 @@ static int spi_drv_remove(struct device *dev)
 	int ret;
 
 	ret = sdrv->remove(to_spi_device(dev));
-	acpi_dev_pm_detach(dev, true);
+	dev_pm_domain_detach(dev, true);
 
 	return ret;
 }
@@ -552,6 +555,9 @@ int spi_register_board_info(struct spi_board_info const *info, unsigned n)
 	struct boardinfo *bi;
 	int i;
 
+	if (!n)
+		return -EINVAL;
+
 	bi = kzalloc(n * sizeof(*bi), GFP_KERNEL);
 	if (!bi)
 		return -ENOMEM;
@@ -789,27 +795,35 @@ static int spi_transfer_one_message(struct spi_master *master,
 	list_for_each_entry(xfer, &msg->transfers, transfer_list) {
 		trace_spi_transfer_start(msg, xfer);
 
-		reinit_completion(&master->xfer_completion);
+		if (xfer->tx_buf || xfer->rx_buf) {
+			reinit_completion(&master->xfer_completion);
 
-		ret = master->transfer_one(master, msg->spi, xfer);
-		if (ret < 0) {
-			dev_err(&msg->spi->dev,
-				"SPI transfer failed: %d\n", ret);
-			goto out;
-		}
+			ret = master->transfer_one(master, msg->spi, xfer);
+			if (ret < 0) {
+				dev_err(&msg->spi->dev,
+					"SPI transfer failed: %d\n", ret);
+				goto out;
+			}
 
-		if (ret > 0) {
-			ret = 0;
-			ms = xfer->len * 8 * 1000 / xfer->speed_hz;
-			ms += ms + 100; /* some tolerance */
+			if (ret > 0) {
+				ret = 0;
+				ms = xfer->len * 8 * 1000 / xfer->speed_hz;
+				ms += ms + 100; /* some tolerance */
 
-			ms = wait_for_completion_timeout(&master->xfer_completion,
-							 msecs_to_jiffies(ms));
-		}
+				ms = wait_for_completion_timeout(&master->xfer_completion,
+								 msecs_to_jiffies(ms));
+			}
 
-		if (ms == 0) {
-			dev_err(&msg->spi->dev, "SPI transfer timed out\n");
-			msg->status = -ETIMEDOUT;
+			if (ms == 0) {
+				dev_err(&msg->spi->dev,
+					"SPI transfer timed out\n");
+				msg->status = -ETIMEDOUT;
+			}
+		} else {
+			if (xfer->len)
+				dev_err(&msg->spi->dev,
+					"Bufferless transfer has length %u\n",
+					xfer->len);
 		}
 
 		trace_spi_transfer_stop(msg, xfer);
@@ -848,6 +862,7 @@ out:
 
 /**
  * spi_finalize_current_transfer - report completion of a transfer
+ * @master: the master reporting completion
  *
  * Called by SPI drivers using the core transfer_one_message()
  * implementation to notify it that the current interrupt driven
diff --git a/drivers/ssb/b43_pci_bridge.c b/drivers/ssb/b43_pci_bridge.c
index 19396dc4ee47..bed2fedeb057 100644
--- a/drivers/ssb/b43_pci_bridge.c
+++ b/drivers/ssb/b43_pci_bridge.c
@@ -38,6 +38,7 @@ static const struct pci_device_id b43_pci_bridge_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432b) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x432c) },
 	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4350) },
+	{ PCI_DEVICE(PCI_VENDOR_ID_BROADCOM, 0x4351) },
 	{ 0, },
 };
 MODULE_DEVICE_TABLE(pci, b43_pci_bridge_tbl);
diff --git a/drivers/ssb/driver_gpio.c b/drivers/ssb/driver_gpio.c
index ba350d2035c0..f92e266d48f8 100644
--- a/drivers/ssb/driver_gpio.c
+++ b/drivers/ssb/driver_gpio.c
@@ -475,7 +475,8 @@ int ssb_gpio_unregister(struct ssb_bus *bus)
 {
 	if (ssb_chipco_available(&bus->chipco) ||
 	    ssb_extif_available(&bus->extif)) {
-		return gpiochip_remove(&bus->gpio);
+		gpiochip_remove(&bus->gpio);
+		return 0;
 	} else {
 		SSB_WARN_ON(1);
 	}
diff --git a/drivers/staging/Kconfig b/drivers/staging/Kconfig
index 2c486ea6236b..4690ae9a267f 100644
--- a/drivers/staging/Kconfig
+++ b/drivers/staging/Kconfig
@@ -24,12 +24,8 @@ menuconfig STAGING
 
 if STAGING
 
-source "drivers/staging/et131x/Kconfig"
-
 source "drivers/staging/slicoss/Kconfig"
 
-source "drivers/staging/usbip/Kconfig"
-
 source "drivers/staging/wlan-ng/Kconfig"
 
 source "drivers/staging/comedi/Kconfig"
@@ -46,12 +42,8 @@ source "drivers/staging/rtl8712/Kconfig"
 
 source "drivers/staging/rtl8188eu/Kconfig"
 
-source "drivers/staging/rtl8192ee/Kconfig"
-
 source "drivers/staging/rtl8723au/Kconfig"
 
-source "drivers/staging/rtl8821ae/Kconfig"
-
 source "drivers/staging/rts5208/Kconfig"
 
 source "drivers/staging/line6/Kconfig"
@@ -106,16 +98,12 @@ source "drivers/staging/mt29f_spinand/Kconfig"
 
 source "drivers/staging/lustre/Kconfig"
 
-source "drivers/staging/xillybus/Kconfig"
-
 source "drivers/staging/dgnc/Kconfig"
 
 source "drivers/staging/dgap/Kconfig"
 
 source "drivers/staging/gs_fpgaboot/Kconfig"
 
-source "drivers/staging/nokia_h4p/Kconfig"
-
 source "drivers/staging/skein/Kconfig"
 
 source "drivers/staging/unisys/Kconfig"
diff --git a/drivers/staging/Makefile b/drivers/staging/Makefile
index 1e1a3a10faf7..c780a0e70e15 100644
--- a/drivers/staging/Makefile
+++ b/drivers/staging/Makefile
@@ -4,9 +4,7 @@
 obj-$(CONFIG_STAGING)		+= staging.o
 
 obj-y				+= media/
-obj-$(CONFIG_ET131X)		+= et131x/
 obj-$(CONFIG_SLICOSS)		+= slicoss/
-obj-$(CONFIG_USBIP_CORE)	+= usbip/
 obj-$(CONFIG_PRISM2_USB)	+= wlan-ng/
 obj-$(CONFIG_COMEDI)		+= comedi/
 obj-$(CONFIG_FB_OLPC_DCON)	+= olpc_dcon/
@@ -15,9 +13,7 @@ obj-$(CONFIG_RTL8192U)		+= rtl8192u/
 obj-$(CONFIG_RTL8192E)		+= rtl8192e/
 obj-$(CONFIG_R8712U)		+= rtl8712/
 obj-$(CONFIG_R8188EU)		+= rtl8188eu/
-obj-$(CONFIG_R8192EE)		+= rtl8192ee/
 obj-$(CONFIG_R8723AU)		+= rtl8723au/
-obj-$(CONFIG_R8821AE)		+= rtl8821ae/
 obj-$(CONFIG_RTS5208)		+= rts5208/
 obj-$(CONFIG_LINE6_USB)		+= line6/
 obj-$(CONFIG_NETLOGIC_XLR_NET)	+= netlogic/
@@ -44,11 +40,9 @@ obj-$(CONFIG_DRM_IMX)		+= imx-drm/
 obj-$(CONFIG_FIREWIRE_SERIAL)	+= fwserial/
 obj-$(CONFIG_GOLDFISH)		+= goldfish/
 obj-$(CONFIG_LUSTRE_FS)		+= lustre/
-obj-$(CONFIG_XILLYBUS)		+= xillybus/
 obj-$(CONFIG_DGNC)			+= dgnc/
 obj-$(CONFIG_DGAP)			+= dgap/
 obj-$(CONFIG_MTD_SPINAND_MT29F)	+= mt29f_spinand/
 obj-$(CONFIG_GS_FPGABOOT)	+= gs_fpgaboot/
-obj-$(CONFIG_BT_NOKIA_H4P)	+= nokia_h4p/
 obj-$(CONFIG_CRYPTO_SKEIN)	+= skein/
 obj-$(CONFIG_UNISYSSPAR)	+= unisys/
diff --git a/drivers/staging/android/ashmem.c b/drivers/staging/android/ashmem.c
index 713a97226787..ad4f5790a76f 100644
--- a/drivers/staging/android/ashmem.c
+++ b/drivers/staging/android/ashmem.c
@@ -339,7 +339,7 @@ static loff_t ashmem_llseek(struct file *file, loff_t offset, int origin)
 		goto out;
 	}
 
-	ret = asma->file->f_op->llseek(asma->file, offset, origin);
+	ret = vfs_llseek(asma->file, offset, origin);
 	if (ret < 0)
 		goto out;
 
diff --git a/drivers/staging/android/binder.c b/drivers/staging/android/binder.c
index 4f34dc0095b5..c69c40d69d5c 100644
--- a/drivers/staging/android/binder.c
+++ b/drivers/staging/android/binder.c
@@ -1198,7 +1198,8 @@ static void binder_send_failed_reply(struct binder_transaction *t,
 			if (target_thread->return_error == BR_OK) {
 				binder_debug(BINDER_DEBUG_FAILED_TRANSACTION,
 					     "send failed reply for transaction %d to %d:%d\n",
-					      t->debug_id, target_thread->proc->pid,
+					      t->debug_id,
+					      target_thread->proc->pid,
 					      target_thread->pid);
 
 				binder_pop_transaction(target_thread, t);
@@ -2198,12 +2199,16 @@ retry:
 		struct binder_work *w;
 		struct binder_transaction *t = NULL;
 
-		if (!list_empty(&thread->todo))
-			w = list_first_entry(&thread->todo, struct binder_work, entry);
-		else if (!list_empty(&proc->todo) && wait_for_proc_work)
-			w = list_first_entry(&proc->todo, struct binder_work, entry);
-		else {
-			if (ptr - buffer == 4 && !(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN)) /* no data added */
+		if (!list_empty(&thread->todo)) {
+			w = list_first_entry(&thread->todo, struct binder_work,
+					     entry);
+		} else if (!list_empty(&proc->todo) && wait_for_proc_work) {
+			w = list_first_entry(&proc->todo, struct binder_work,
+					     entry);
+		} else {
+			/* no data added */
+			if (ptr - buffer == 4 &&
+			    !(thread->looper & BINDER_LOOPER_STATE_NEED_RETURN))
 				goto retry;
 			break;
 		}
diff --git a/drivers/staging/android/ion/compat_ion.c b/drivers/staging/android/ion/compat_ion.c
index ee3a7380e53b..a402fdaf54ca 100644
--- a/drivers/staging/android/ion/compat_ion.c
+++ b/drivers/staging/android/ion/compat_ion.c
@@ -125,7 +125,7 @@ long compat_ion_ioctl(struct file *filp, unsigned int cmd, unsigned long arg)
 {
 	long ret;
 
-	if (!filp->f_op || !filp->f_op->unlocked_ioctl)
+	if (!filp->f_op->unlocked_ioctl)
 		return -ENOTTY;
 
 	switch (cmd) {
diff --git a/drivers/staging/android/ion/ion.c b/drivers/staging/android/ion/ion.c
index 270360912b2c..56604f41ec48 100644
--- a/drivers/staging/android/ion/ion.c
+++ b/drivers/staging/android/ion/ion.c
@@ -805,6 +805,7 @@ struct ion_client *ion_client_create(struct ion_device *dev,
 						client, &debug_client_fops);
 	if (!client->debug_root) {
 		char buf[256], *path;
+
 		path = dentry_path(dev->clients_debug_root, buf, 256);
 		pr_err("Failed to create client debugfs at %s/%s\n",
 			path, client->display_name);
@@ -1056,7 +1057,6 @@ static void *ion_dma_buf_kmap(struct dma_buf *dmabuf, unsigned long offset)
 static void ion_dma_buf_kunmap(struct dma_buf *dmabuf, unsigned long offset,
 			       void *ptr)
 {
-	return;
 }
 
 static int ion_dma_buf_begin_cpu_access(struct dma_buf *dmabuf, size_t start,
diff --git a/drivers/staging/android/ion/ion_carveout_heap.c b/drivers/staging/android/ion/ion_carveout_heap.c
index dcb6f2196c87..9156d8238c97 100644
--- a/drivers/staging/android/ion/ion_carveout_heap.c
+++ b/drivers/staging/android/ion/ion_carveout_heap.c
@@ -133,7 +133,6 @@ static struct sg_table *ion_carveout_heap_map_dma(struct ion_heap *heap,
 static void ion_carveout_heap_unmap_dma(struct ion_heap *heap,
 					struct ion_buffer *buffer)
 {
-	return;
 }
 
 static struct ion_heap_ops carveout_heap_ops = {
diff --git a/drivers/staging/android/ion/ion_chunk_heap.c b/drivers/staging/android/ion/ion_chunk_heap.c
index 9c3e49aa204b..3e6ec2ee6802 100644
--- a/drivers/staging/android/ion/ion_chunk_heap.c
+++ b/drivers/staging/android/ion/ion_chunk_heap.c
@@ -126,7 +126,6 @@ static struct sg_table *ion_chunk_heap_map_dma(struct ion_heap *heap,
 static void ion_chunk_heap_unmap_dma(struct ion_heap *heap,
 				     struct ion_buffer *buffer)
 {
-	return;
 }
 
 static struct ion_heap_ops chunk_heap_ops = {
diff --git a/drivers/staging/android/ion/ion_dummy_driver.c b/drivers/staging/android/ion/ion_dummy_driver.c
index 3a45e79fe444..f3ea1c31e533 100644
--- a/drivers/staging/android/ion/ion_dummy_driver.c
+++ b/drivers/staging/android/ion/ion_dummy_driver.c
@@ -68,7 +68,7 @@ static int __init ion_dummy_init(void)
 	int i, err;
 
 	idev = ion_device_create(NULL);
-	heaps = kzalloc(sizeof(struct ion_heap *) * dummy_ion_pdata.nr,
+	heaps = kcalloc(dummy_ion_pdata.nr, sizeof(struct ion_heap *),
 			GFP_KERNEL);
 	if (!heaps)
 		return -ENOMEM;
@@ -152,7 +152,5 @@ static void __exit ion_dummy_exit(void)
 				dummy_heaps[ION_HEAP_TYPE_CHUNK].size);
 		chunk_ptr = NULL;
 	}
-
-	return;
 }
 __exitcall(ion_dummy_exit);
diff --git a/drivers/staging/android/ion/ion_system_heap.c b/drivers/staging/android/ion/ion_system_heap.c
index 6b77c5195b4d..da2a63c0a9ba 100644
--- a/drivers/staging/android/ion/ion_system_heap.c
+++ b/drivers/staging/android/ion/ion_system_heap.c
@@ -205,7 +205,6 @@ static struct sg_table *ion_system_heap_map_dma(struct ion_heap *heap,
 static void ion_system_heap_unmap_dma(struct ion_heap *heap,
 				      struct ion_buffer *buffer)
 {
-	return;
 }
 
 static int ion_system_heap_shrink(struct ion_heap *heap, gfp_t gfp_mask,
diff --git a/drivers/staging/android/logger.c b/drivers/staging/android/logger.c
index 9b47e66599a3..28b93d39a94e 100644
--- a/drivers/staging/android/logger.c
+++ b/drivers/staging/android/logger.c
@@ -411,69 +411,18 @@ static void fix_up_readers(struct logger_log *log, size_t len)
 }
 
 /*
- * do_write_log - writes 'len' bytes from 'buf' to 'log'
- *
- * The caller needs to hold log->mutex.
- */
-static void do_write_log(struct logger_log *log, const void *buf, size_t count)
-{
-	size_t len;
-
-	len = min(count, log->size - log->w_off);
-	memcpy(log->buffer + log->w_off, buf, len);
-
-	if (count != len)
-		memcpy(log->buffer, buf + len, count - len);
-
-	log->w_off = logger_offset(log, log->w_off + count);
-
-}
-
-/*
- * do_write_log_user - writes 'len' bytes from the user-space buffer 'buf' to
- * the log 'log'
- *
- * The caller needs to hold log->mutex.
- *
- * Returns 'count' on success, negative error code on failure.
- */
-static ssize_t do_write_log_from_user(struct logger_log *log,
-				      const void __user *buf, size_t count)
-{
-	size_t len;
-
-	len = min(count, log->size - log->w_off);
-	if (len && copy_from_user(log->buffer + log->w_off, buf, len))
-		return -EFAULT;
-
-	if (count != len)
-		if (copy_from_user(log->buffer, buf + len, count - len))
-			/*
-			 * Note that by not updating w_off, this abandons the
-			 * portion of the new entry that *was* successfully
-			 * copied, just above.  This is intentional to avoid
-			 * message corruption from missing fragments.
-			 */
-			return -EFAULT;
-
-	log->w_off = logger_offset(log, log->w_off + count);
-
-	return count;
-}
-
-/*
- * logger_aio_write - our write method, implementing support for write(),
+ * logger_write_iter - our write method, implementing support for write(),
  * writev(), and aio_write(). Writes are our fast path, and we try to optimize
  * them above all else.
  */
-static ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
-			 unsigned long nr_segs, loff_t ppos)
+static ssize_t logger_write_iter(struct kiocb *iocb, struct iov_iter *from)
 {
 	struct logger_log *log = file_get_log(iocb->ki_filp);
-	size_t orig;
 	struct logger_entry header;
 	struct timespec now;
-	ssize_t ret = 0;
+	size_t len, count;
+
+	count = min_t(size_t, iocb->ki_nbytes, LOGGER_ENTRY_MAX_PAYLOAD);
 
 	now = current_kernel_time();
 
@@ -482,7 +431,7 @@ static ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
 	header.sec = now.tv_sec;
 	header.nsec = now.tv_nsec;
 	header.euid = current_euid();
-	header.len = min_t(size_t, iocb->ki_nbytes, LOGGER_ENTRY_MAX_PAYLOAD);
+	header.len = count;
 	header.hdr_size = sizeof(struct logger_entry);
 
 	/* null writes succeed, return zero */
@@ -491,8 +440,6 @@ static ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
 
 	mutex_lock(&log->mutex);
 
-	orig = log->w_off;
-
 	/*
 	 * Fix up any readers, pulling them forward to the first readable
 	 * entry after (what will be) the new write offset. We do this now
@@ -501,33 +448,35 @@ static ssize_t logger_aio_write(struct kiocb *iocb, const struct iovec *iov,
 	 */
 	fix_up_readers(log, sizeof(struct logger_entry) + header.len);
 
-	do_write_log(log, &header, sizeof(struct logger_entry));
-
-	while (nr_segs-- > 0) {
-		size_t len;
-		ssize_t nr;
+	len = min(sizeof(header), log->size - log->w_off);
+	memcpy(log->buffer + log->w_off, &header, len);
+	memcpy(log->buffer, (char *)&header + len, sizeof(header) - len);
 
-		/* figure out how much of this vector we can keep */
-		len = min_t(size_t, iov->iov_len, header.len - ret);
+	len = min(count, log->size - log->w_off);
 
-		/* write out this segment's payload */
-		nr = do_write_log_from_user(log, iov->iov_base, len);
-		if (unlikely(nr < 0)) {
-			log->w_off = orig;
-			mutex_unlock(&log->mutex);
-			return nr;
-		}
+	if (copy_from_iter(log->buffer + log->w_off, len, from) != len) {
+		/*
+		 * Note that by not updating w_off, this abandons the
+		 * portion of the new entry that *was* successfully
+		 * copied, just above.  This is intentional to avoid
+		 * message corruption from missing fragments.
+		 */
+		mutex_unlock(&log->mutex);
+		return -EFAULT;
+	}
 
-		iov++;
-		ret += nr;
+	if (copy_from_iter(log->buffer, count - len, from) != count - len) {
+		mutex_unlock(&log->mutex);
+		return -EFAULT;
 	}
 
+	log->w_off = logger_offset(log, log->w_off + count);
 	mutex_unlock(&log->mutex);
 
 	/* wake up any blocked readers */
 	wake_up_interruptible(&log->wq);
 
-	return ret;
+	return len;
 }
 
 static struct logger_log *get_log_from_minor(int minor)
@@ -736,7 +685,7 @@ static long logger_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 static const struct file_operations logger_fops = {
 	.owner = THIS_MODULE,
 	.read = logger_read,
-	.aio_write = logger_aio_write,
+	.write_iter = logger_write_iter,
 	.poll = logger_poll,
 	.unlocked_ioctl = logger_ioctl,
 	.compat_ioctl = logger_ioctl,
@@ -790,7 +739,7 @@ static int __init create_log(char *log_name, int size)
 	if (unlikely(ret)) {
 		pr_err("failed to register misc device for log '%s'!\n",
 				log->misc.name);
-		goto out_free_log;
+		goto out_free_misc_name;
 	}
 
 	pr_info("created %luK log '%s'\n",
@@ -798,6 +747,9 @@ static int __init create_log(char *log_name, int size)
 
 	return 0;
 
+out_free_misc_name:
+	kfree(log->misc.name);
+
 out_free_log:
 	kfree(log);
 
diff --git a/drivers/staging/android/sw_sync.c b/drivers/staging/android/sw_sync.c
index a76db3ff87cb..c90838d36953 100644
--- a/drivers/staging/android/sw_sync.c
+++ b/drivers/staging/android/sw_sync.c
@@ -48,11 +48,11 @@ EXPORT_SYMBOL(sw_sync_pt_create);
 
 static struct sync_pt *sw_sync_pt_dup(struct sync_pt *sync_pt)
 {
-	struct sw_sync_pt *pt = (struct sw_sync_pt *) sync_pt;
+	struct sw_sync_pt *pt = (struct sw_sync_pt *)sync_pt;
 	struct sw_sync_timeline *obj =
 		(struct sw_sync_timeline *)sync_pt_parent(sync_pt);
 
-	return (struct sync_pt *) sw_sync_pt_create(obj, pt->value);
+	return (struct sync_pt *)sw_sync_pt_create(obj, pt->value);
 }
 
 static int sw_sync_pt_has_signaled(struct sync_pt *sync_pt)
@@ -94,9 +94,10 @@ static void sw_sync_timeline_value_str(struct sync_timeline *sync_timeline,
 }
 
 static void sw_sync_pt_value_str(struct sync_pt *sync_pt,
-				       char *str, int size)
+				 char *str, int size)
 {
 	struct sw_sync_pt *pt = (struct sw_sync_pt *)sync_pt;
+
 	snprintf(str, size, "%d", pt->value);
 }
 
@@ -110,7 +111,6 @@ static struct sync_timeline_ops sw_sync_timeline_ops = {
 	.pt_value_str = sw_sync_pt_value_str,
 };
 
-
 struct sw_sync_timeline *sw_sync_timeline_create(const char *name)
 {
 	struct sw_sync_timeline *obj = (struct sw_sync_timeline *)
@@ -156,6 +156,7 @@ static int sw_sync_open(struct inode *inode, struct file *file)
 static int sw_sync_release(struct inode *inode, struct file *file)
 {
 	struct sw_sync_timeline *obj = file->private_data;
+
 	sync_timeline_destroy(&obj->obj);
 	return 0;
 }
diff --git a/drivers/staging/android/sw_sync.h b/drivers/staging/android/sw_sync.h
index 1a50669ec8a9..c87ae9ebf267 100644
--- a/drivers/staging/android/sw_sync.h
+++ b/drivers/staging/android/sw_sync.h
@@ -50,7 +50,7 @@ static inline void sw_sync_timeline_inc(struct sw_sync_timeline *obj, u32 inc)
 }
 
 static inline struct sync_pt *sw_sync_pt_create(struct sw_sync_timeline *obj,
-		u32 value)
+						u32 value)
 {
 	return NULL;
 }
diff --git a/drivers/staging/android/sync.c b/drivers/staging/android/sync.c
index e7b2e0234196..7bdb62bf6b40 100644
--- a/drivers/staging/android/sync.c
+++ b/drivers/staging/android/sync.c
@@ -199,7 +199,6 @@ struct sync_fence *sync_fence_create(const char *name, struct sync_pt *pt)
 	fence->num_fences = 1;
 	atomic_set(&fence->status, 1);
 
-	fence_get(&pt->base);
 	fence->cbs[0].sync_pt = &pt->base;
 	fence->cbs[0].fence = fence;
 	if (fence_add_callback(&pt->base, &fence->cbs[0].cb,
@@ -387,9 +386,9 @@ int sync_fence_wait(struct sync_fence *fence, long timeout)
 					       timeout);
 	trace_sync_wait(fence, 0);
 
-	if (ret < 0)
+	if (ret < 0) {
 		return ret;
-	else if (ret == 0) {
+	} else if (ret == 0) {
 		if (timeout) {
 			pr_info("fence timeout on [%p] after %dms\n", fence,
 				jiffies_to_msecs(timeout));
@@ -705,6 +704,7 @@ static long sync_fence_ioctl(struct file *file, unsigned int cmd,
 			     unsigned long arg)
 {
 	struct sync_fence *fence = file->private_data;
+
 	switch (cmd) {
 	case SYNC_IOC_WAIT:
 		return sync_fence_ioctl_wait(fence, arg);
diff --git a/drivers/staging/android/sync.h b/drivers/staging/android/sync.h
index 66b0f431f63e..a21b79fb4c8e 100644
--- a/drivers/staging/android/sync.h
+++ b/drivers/staging/android/sync.h
@@ -293,7 +293,8 @@ void sync_fence_put(struct sync_fence *fence);
  * @fence:	fence to install
  * @fd:		file descriptor in which to install the fence
  *
- * Installs @fence into @fd.  @fd's should be acquired through get_unused_fd().
+ * Installs @fence into @fd.  @fd's should be acquired through
+ * get_unused_fd_flags(O_CLOEXEC).
  */
 void sync_fence_install(struct sync_fence *fence, int fd);
 
diff --git a/drivers/staging/android/uapi/binder.h b/drivers/staging/android/uapi/binder.h
index 904adb7600cf..dba4cef3a8d3 100644
--- a/drivers/staging/android/uapi/binder.h
+++ b/drivers/staging/android/uapi/binder.h
@@ -169,7 +169,7 @@ struct binder_ptr_cookie {
 struct binder_handle_cookie {
 	__u32 handle;
 	binder_uintptr_t cookie;
-} __attribute__((packed));
+} __packed;
 
 struct binder_pri_desc {
 	__s32 priority;
diff --git a/drivers/staging/bcm/Adapter.h b/drivers/staging/bcm/Adapter.h
index 1b2d9f3bd55a..940c852e17b7 100644
--- a/drivers/staging/bcm/Adapter.h
+++ b/drivers/staging/bcm/Adapter.h
@@ -43,8 +43,10 @@ union u_ip_address {
 		ULONG ulIpv4Mask[MAX_IP_RANGE_LENGTH];
 	};
 	struct {
-		ULONG ulIpv6Addr[MAX_IP_RANGE_LENGTH * 4]; /* Source Ip Address Range */
-		ULONG ulIpv6Mask[MAX_IP_RANGE_LENGTH * 4]; /* Source Ip Mask Address Range */
+		/* Source Ip Address Range */
+		ULONG ulIpv6Addr[MAX_IP_RANGE_LENGTH * 4];
+		/* Source Ip Mask Address Range */
+		ULONG ulIpv6Mask[MAX_IP_RANGE_LENGTH * 4];
 	};
 	struct {
 		UCHAR ucIpv4Address[MAX_IP_RANGE_LENGTH * IP_LENGTH_OF_ADDRESS];
@@ -57,8 +59,10 @@ union u_ip_address {
 };
 
 struct bcm_hdr_suppression_contextinfo {
-	UCHAR ucaHdrSuppressionInBuf[MAX_PHS_LENGTHS]; /* Intermediate buffer to accumulate pkt Header for PHS */
-	UCHAR ucaHdrSuppressionOutBuf[MAX_PHS_LENGTHS + PHSI_LEN]; /* Intermediate buffer containing pkt Header after PHS */
+	/* Intermediate buffer to accumulate pkt Header for PHS */
+	UCHAR ucaHdrSuppressionInBuf[MAX_PHS_LENGTHS];
+	/* Intermediate buffer containing pkt Header after PHS */
+	UCHAR ucaHdrSuppressionOutBuf[MAX_PHS_LENGTHS + PHSI_LEN];
 };
 
 struct bcm_classifier_rule {
@@ -67,12 +71,14 @@ struct bcm_classifier_rule {
 	B_UINT16	uiClassifierRuleIndex;
 	bool		bUsed;
 	USHORT		usVCID_Value;
-	B_UINT8		u8ClassifierRulePriority; /* This field detemines the Classifier Priority */
+	/* This field detemines the Classifier Priority */
+	B_UINT8		u8ClassifierRulePriority;
 	union u_ip_address	stSrcIpAddress;
 	UCHAR		ucIPSourceAddressLength; /* Ip Source Address Length */
 
 	union u_ip_address	stDestIpAddress;
-	UCHAR		ucIPDestinationAddressLength; /* Ip Destination Address Length */
+	/* Ip Destination Address Length */
+	UCHAR		ucIPDestinationAddressLength;
 	UCHAR		ucIPTypeOfServiceLength; /* Type of service Length */
 	UCHAR		ucTosLow; /* Tos Low */
 	UCHAR		ucTosHigh; /* Tos High */
@@ -136,7 +142,8 @@ struct bcm_packet_info {
 
 	B_UINT8		u8QueueType; /* BE or rtPS */
 
-	UINT		uiMaxBucketSize; /* maximum size of the bucket for the queue */
+	/* maximum size of the bucket for the queue */
+	UINT		uiMaxBucketSize;
 	UINT		uiCurrentQueueDepthOnTarget;
 	UINT		uiCurrentBytesOnHost;
 	UINT		uiCurrentPacketsOnHost;
@@ -344,7 +351,8 @@ struct bcm_mini_adapter {
 	bool			bDPLLConfig;
 	UINT32			aTxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
 	UINT32			aRxPktSizeHist[MIBS_MAX_HIST_ENTRIES];
-	struct bcm_fragmented_packet_info astFragmentedPktClassifierTable[MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES];
+	struct bcm_fragmented_packet_info
+		astFragmentedPktClassifierTable[MAX_FRAGMENTEDIP_CLASSIFICATION_ENTRIES];
 	atomic_t		uiMBupdate;
 	UINT32			PmuMode;
 	enum bcm_nvm_type	eNVMType;
@@ -355,7 +363,8 @@ struct bcm_mini_adapter {
 	UINT			uiNVMDSDSize;
 	UINT			uiVendorExtnFlag;
 	/* it will always represent chosen DSD at any point of time.
-	 * Generally it is Active DSD but in case of NVM RD/WR it might be different.
+	 * Generally it is Active DSD but in case of NVM RD/WR it
+	 * might be different.
 	 */
 	UINT			ulFlashCalStart;
 	ULONG			ulFlashControlSectionStart;
@@ -372,25 +381,33 @@ struct bcm_mini_adapter {
 	struct bcm_flash_cs_info *psFlashCSInfo;
 	struct bcm_flash2x_vendor_info *psFlash2xVendorInfo;
 	UINT			uiFlashBaseAdd; /* Flash start address */
-	UINT			uiActiveISOOffset; /* Active ISO offset chosen before f/w download */
+	/* Active ISO offset chosen before f/w download */
+	UINT			uiActiveISOOffset;
 	enum bcm_flash2x_section_val eActiveISO; /* Active ISO section val */
-	enum bcm_flash2x_section_val eActiveDSD; /* Active DSD val chosen before f/w download */
-	UINT			uiActiveDSDOffsetAtFwDld;  /* For accessing Active DSD chosen before f/w download */
+	/* Active DSD val chosen before f/w download */
+	enum bcm_flash2x_section_val eActiveDSD;
+	/* For accessing Active DSD chosen before f/w download */
+	UINT			uiActiveDSDOffsetAtFwDld;
 	UINT			uiFlashLayoutMajorVersion;
 	UINT			uiFlashLayoutMinorVersion;
 	bool			bAllDSDWriteAllow;
 	bool			bSigCorrupted;
-	/* this should be set who so ever want to change the Headers. after Write it should be reset immediately. */
+	/* this should be set who so ever want to change the Headers.
+	 * after Write it should be reset immediately.
+	 */
 	bool			bHeaderChangeAllowed;
 	int			SelectedChip;
 	bool			bEndPointHalted;
-	/* while bFlashRawRead will be true, Driver  ignore map lay out and consider flash as of without any map. */
+	/* while bFlashRawRead will be true, Driver
+	 * ignore map lay out and consider flash as of without any map.
+	 */
 	bool			bFlashRawRead;
 	bool			bPreparingForLowPowerMode;
 	bool			bDoSuspend;
 	UINT			syscfgBefFwDld;
 	bool			StopAllXaction;
-	UINT32			liTimeSinceLastNetEntry; /* Used to Support extended CAPI requirements from */
+	/* Used to Support extended CAPI requirements from */
+	UINT32			liTimeSinceLastNetEntry;
 	struct semaphore	LowPowerModeSync;
 	ULONG			liDrainCalculated;
 	UINT			gpioBitMap;
diff --git a/drivers/staging/bcm/Bcmchar.c b/drivers/staging/bcm/Bcmchar.c
index c1e01f7d64ba..88ce2da531c6 100644
--- a/drivers/staging/bcm/Bcmchar.c
+++ b/drivers/staging/bcm/Bcmchar.c
@@ -2,45 +2,46 @@
 
 #include "headers.h"
 
-static int bcm_handle_nvm_read_cmd(struct bcm_mini_adapter *Adapter,
-	PUCHAR pReadData, struct bcm_nvm_readwrite *stNVMReadWrite)
+static int bcm_handle_nvm_read_cmd(struct bcm_mini_adapter *ad,
+				   PUCHAR read_data,
+				   struct bcm_nvm_readwrite *nvm_rw)
 {
-	INT Status = STATUS_FAILURE;
+	INT status = STATUS_FAILURE;
 
-	down(&Adapter->NVMRdmWrmLock);
+	down(&ad->NVMRdmWrmLock);
 
-	if ((Adapter->IdleMode == TRUE) || (Adapter->bShutStatus == TRUE) ||
-			(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) || (ad->bShutStatus == TRUE) ||
+			(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter,
+		BCM_DEBUG_PRINT(ad,
 			DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"Device is in Idle/Shutdown Mode\n");
-		up(&Adapter->NVMRdmWrmLock);
-		kfree(pReadData);
+		up(&ad->NVMRdmWrmLock);
+		kfree(read_data);
 		return -EACCES;
 	}
 
-	Status = BeceemNVMRead(Adapter, (PUINT)pReadData,
-			       stNVMReadWrite->uiOffset,
-			       stNVMReadWrite->uiNumBytes);
-	up(&Adapter->NVMRdmWrmLock);
+	status = BeceemNVMRead(ad, (PUINT)read_data,
+			       nvm_rw->uiOffset,
+			       nvm_rw->uiNumBytes);
+	up(&ad->NVMRdmWrmLock);
 
-	if (Status != STATUS_SUCCESS) {
-		kfree(pReadData);
-		return Status;
+	if (status != STATUS_SUCCESS) {
+		kfree(read_data);
+		return status;
 	}
 
-	if (copy_to_user(stNVMReadWrite->pBuffer, pReadData,
-			stNVMReadWrite->uiNumBytes)) {
-		kfree(pReadData);
+	if (copy_to_user(nvm_rw->pBuffer, read_data, nvm_rw->uiNumBytes)) {
+		kfree(read_data);
 		return -EFAULT;
 	}
 
 	return STATUS_SUCCESS;
 }
 
-static int handle_flash2x_adapter(struct bcm_mini_adapter *Adapter,
-	PUCHAR pReadData, struct bcm_nvm_readwrite *stNVMReadWrite)
+static int handle_flash2x_adapter(struct bcm_mini_adapter *ad,
+				  PUCHAR read_data,
+				  struct bcm_nvm_readwrite *nvm_rw)
 {
 	/*
 	 * New Requirement:-
@@ -57,33 +58,33 @@ static int handle_flash2x_adapter(struct bcm_mini_adapter *Adapter,
 	 * if DSD sig is corrupted, DSD data won't be
 	 * considered valid.
 	 */
-	INT Status;
-	ULONG ulDSDMagicNumInUsrBuff = 0;
+	INT status;
+	ULONG dsd_magic_num_in_usr_buff = 0;
 
-	Status = BcmFlash2xCorruptSig(Adapter, Adapter->eActiveDSD);
-	if (Status == STATUS_SUCCESS)
+	status = BcmFlash2xCorruptSig(ad, ad->eActiveDSD);
+	if (status == STATUS_SUCCESS)
 		return STATUS_SUCCESS;
 
-	if (((stNVMReadWrite->uiOffset + stNVMReadWrite->uiNumBytes) !=
-			Adapter->uiNVMDSDSize) ||
-			(stNVMReadWrite->uiNumBytes < SIGNATURE_SIZE)) {
+	if (((nvm_rw->uiOffset + nvm_rw->uiNumBytes) !=
+			ad->uiNVMDSDSize) ||
+			(nvm_rw->uiNumBytes < SIGNATURE_SIZE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"DSD Sig is present neither in Flash nor User provided Input..");
-		up(&Adapter->NVMRdmWrmLock);
-		kfree(pReadData);
-		return Status;
+		up(&ad->NVMRdmWrmLock);
+		kfree(read_data);
+		return status;
 	}
 
-	ulDSDMagicNumInUsrBuff =
-		ntohl(*(PUINT)(pReadData + stNVMReadWrite->uiNumBytes -
+	dsd_magic_num_in_usr_buff =
+		ntohl(*(PUINT)(read_data + nvm_rw->uiNumBytes -
 		      SIGNATURE_SIZE));
-	if (ulDSDMagicNumInUsrBuff != DSD_IMAGE_MAGIC_NUMBER) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	if (dsd_magic_num_in_usr_buff != DSD_IMAGE_MAGIC_NUMBER) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"DSD Sig is present neither in Flash nor User provided Input..");
-		up(&Adapter->NVMRdmWrmLock);
-		kfree(pReadData);
-		return Status;
+		up(&ad->NVMRdmWrmLock);
+		kfree(read_data);
+		return status;
 	}
 
 	return STATUS_SUCCESS;
@@ -103,27 +104,27 @@ static int handle_flash2x_adapter(struct bcm_mini_adapter *Adapter,
 
 static int bcm_char_open(struct inode *inode, struct file *filp)
 {
-	struct bcm_mini_adapter *Adapter = NULL;
-	struct bcm_tarang_data *pTarang = NULL;
+	struct bcm_mini_adapter *ad = NULL;
+	struct bcm_tarang_data *tarang = NULL;
 
-	Adapter = GET_BCM_ADAPTER(gblpnetdev);
-	pTarang = kzalloc(sizeof(struct bcm_tarang_data), GFP_KERNEL);
-	if (!pTarang)
+	ad = GET_BCM_ADAPTER(gblpnetdev);
+	tarang = kzalloc(sizeof(struct bcm_tarang_data), GFP_KERNEL);
+	if (!tarang)
 		return -ENOMEM;
 
-	pTarang->Adapter = Adapter;
-	pTarang->RxCntrlMsgBitMask = 0xFFFFFFFF & ~(1 << 0xB);
+	tarang->Adapter = ad;
+	tarang->RxCntrlMsgBitMask = 0xFFFFFFFF & ~(1 << 0xB);
 
-	down(&Adapter->RxAppControlQueuelock);
-	pTarang->next = Adapter->pTarangs;
-	Adapter->pTarangs = pTarang;
-	up(&Adapter->RxAppControlQueuelock);
+	down(&ad->RxAppControlQueuelock);
+	tarang->next = ad->pTarangs;
+	ad->pTarangs = tarang;
+	up(&ad->RxAppControlQueuelock);
 
 	/* Store the Adapter structure */
-	filp->private_data = pTarang;
+	filp->private_data = tarang;
 
 	/* Start Queuing the control response Packets */
-	atomic_inc(&Adapter->ApplicationRunning);
+	atomic_inc(&ad->ApplicationRunning);
 
 	nonseekable_open(inode, filp);
 	return 0;
@@ -131,386 +132,388 @@ static int bcm_char_open(struct inode *inode, struct file *filp)
 
 static int bcm_char_release(struct inode *inode, struct file *filp)
 {
-	struct bcm_tarang_data *pTarang, *tmp, *ptmp;
-	struct bcm_mini_adapter *Adapter = NULL;
+	struct bcm_tarang_data *tarang, *tmp, *ptmp;
+	struct bcm_mini_adapter *ad = NULL;
 	struct sk_buff *pkt, *npkt;
 
-	pTarang = (struct bcm_tarang_data *)filp->private_data;
+	tarang = (struct bcm_tarang_data *)filp->private_data;
 
-	if (pTarang == NULL)
+	if (tarang == NULL)
 		return 0;
 
-	Adapter = pTarang->Adapter;
+	ad = tarang->Adapter;
 
-	down(&Adapter->RxAppControlQueuelock);
+	down(&ad->RxAppControlQueuelock);
 
-	tmp = Adapter->pTarangs;
+	tmp = ad->pTarangs;
 	for (ptmp = NULL; tmp; ptmp = tmp, tmp = tmp->next) {
-		if (tmp == pTarang)
+		if (tmp == tarang)
 			break;
 	}
 
 	if (tmp) {
 		if (!ptmp)
-			Adapter->pTarangs = tmp->next;
+			ad->pTarangs = tmp->next;
 		else
 			ptmp->next = tmp->next;
 	} else {
-		up(&Adapter->RxAppControlQueuelock);
+		up(&ad->RxAppControlQueuelock);
 		return 0;
 	}
 
-	pkt = pTarang->RxAppControlHead;
+	pkt = tarang->RxAppControlHead;
 	while (pkt) {
 		npkt = pkt->next;
 		kfree_skb(pkt);
 		pkt = npkt;
 	}
 
-	up(&Adapter->RxAppControlQueuelock);
+	up(&ad->RxAppControlQueuelock);
 
 	/* Stop Queuing the control response Packets */
-	atomic_dec(&Adapter->ApplicationRunning);
+	atomic_dec(&ad->ApplicationRunning);
 
-	kfree(pTarang);
+	kfree(tarang);
 
 	/* remove this filp from the asynchronously notified filp's */
 	filp->private_data = NULL;
 	return 0;
 }
 
-static ssize_t bcm_char_read(struct file *filp, char __user *buf, size_t size,
+static ssize_t bcm_char_read(struct file *filp,
+			     char __user *buf,
+			     size_t size,
 			     loff_t *f_pos)
 {
-	struct bcm_tarang_data *pTarang = filp->private_data;
-	struct bcm_mini_adapter *Adapter = pTarang->Adapter;
-	struct sk_buff *Packet = NULL;
-	ssize_t PktLen = 0;
+	struct bcm_tarang_data *tarang = filp->private_data;
+	struct bcm_mini_adapter *ad = tarang->Adapter;
+	struct sk_buff *packet = NULL;
+	ssize_t pkt_len = 0;
 	int wait_ret_val = 0;
 	unsigned long ret = 0;
 
 	wait_ret_val = wait_event_interruptible(
-				Adapter->process_read_wait_queue,
-				(pTarang->RxAppControlHead ||
-				Adapter->device_removed));
+				ad->process_read_wait_queue,
+				(tarang->RxAppControlHead ||
+				ad->device_removed));
 
 	if ((wait_ret_val == -ERESTARTSYS)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Exiting as i've been asked to exit!!!\n");
 		return wait_ret_val;
 	}
 
-	if (Adapter->device_removed) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	if (ad->device_removed) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Device Removed... Killing the Apps...\n");
 		return -ENODEV;
 	}
 
-	if (false == Adapter->fw_download_done)
+	if (false == ad->fw_download_done)
 		return -EACCES;
 
-	down(&Adapter->RxAppControlQueuelock);
+	down(&ad->RxAppControlQueuelock);
 
-	if (pTarang->RxAppControlHead) {
-		Packet = pTarang->RxAppControlHead;
-		DEQUEUEPACKET(pTarang->RxAppControlHead,
-			      pTarang->RxAppControlTail);
-		pTarang->AppCtrlQueueLen--;
+	if (tarang->RxAppControlHead) {
+		packet = tarang->RxAppControlHead;
+		DEQUEUEPACKET(tarang->RxAppControlHead,
+			      tarang->RxAppControlTail);
+		tarang->AppCtrlQueueLen--;
 	}
 
-	up(&Adapter->RxAppControlQueuelock);
+	up(&ad->RxAppControlQueuelock);
 
-	if (Packet) {
-		PktLen = Packet->len;
-		ret = copy_to_user(buf, Packet->data,
-				   min_t(size_t, PktLen, size));
+	if (packet) {
+		pkt_len = packet->len;
+		ret = copy_to_user(buf, packet->data,
+				   min_t(size_t, pkt_len, size));
 		if (ret) {
-			dev_kfree_skb(Packet);
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			dev_kfree_skb(packet);
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"Returning from copy to user failure\n");
 			return -EFAULT;
 		}
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Read %zd Bytes From Adapter packet = %p by process %d!\n",
-				PktLen, Packet, current->pid);
-		dev_kfree_skb(Packet);
+				pkt_len, packet, current->pid);
+		dev_kfree_skb(packet);
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "<\n");
-	return PktLen;
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL, "<\n");
+	return pkt_len;
 }
 
 static int bcm_char_ioctl_reg_read_private(void __user *argp,
-					   struct bcm_mini_adapter *Adapter)
+					   struct bcm_mini_adapter *ad)
 {
-	struct bcm_rdm_buffer sRdmBuffer = {0};
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_rdm_buffer rdm_buff = {0};
+	struct bcm_ioctl_buffer io_buff;
 	PCHAR temp_buff;
-	INT Status = STATUS_FAILURE;
-	UINT Bufflen;
+	INT status = STATUS_FAILURE;
+	UINT buff_len;
 	u16 temp_value;
 	int bytes;
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(sRdmBuffer))
+	if (io_buff.InputLength > sizeof(rdm_buff))
 		return -EINVAL;
 
-	if (copy_from_user(&sRdmBuffer, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+	if (copy_from_user(&rdm_buff, io_buff.InputBuffer,
+		io_buff.InputLength))
 		return -EFAULT;
 
-	if (IoBuffer.OutputLength > USHRT_MAX ||
-		IoBuffer.OutputLength == 0) {
+	if (io_buff.OutputLength > USHRT_MAX ||
+		io_buff.OutputLength == 0) {
 		return -EINVAL;
 	}
 
-	Bufflen = IoBuffer.OutputLength;
-	temp_value = 4 - (Bufflen % 4);
-	Bufflen += temp_value % 4;
+	buff_len = io_buff.OutputLength;
+	temp_value = 4 - (buff_len % 4);
+	buff_len += temp_value % 4;
 
-	temp_buff = kmalloc(Bufflen, GFP_KERNEL);
+	temp_buff = kmalloc(buff_len, GFP_KERNEL);
 	if (!temp_buff)
 		return -ENOMEM;
 
-	bytes = rdmalt(Adapter, (UINT)sRdmBuffer.Register,
-			(PUINT)temp_buff, Bufflen);
+	bytes = rdmalt(ad, (UINT)rdm_buff.Register,
+			(PUINT)temp_buff, buff_len);
 	if (bytes > 0) {
-		Status = STATUS_SUCCESS;
-		if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, bytes)) {
+		status = STATUS_SUCCESS;
+		if (copy_to_user(io_buff.OutputBuffer, temp_buff, bytes)) {
 			kfree(temp_buff);
 			return -EFAULT;
 		}
 	} else {
-		Status = bytes;
+		status = bytes;
 	}
 
 	kfree(temp_buff);
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_reg_write_private(void __user *argp,
-					    struct bcm_mini_adapter *Adapter)
+					    struct bcm_mini_adapter *ad)
 {
-	struct bcm_wrm_buffer sWrmBuffer = {0};
-	struct bcm_ioctl_buffer IoBuffer;
-	UINT uiTempVar = 0;
-	INT Status;
+	struct bcm_wrm_buffer wrm_buff = {0};
+	struct bcm_ioctl_buffer io_buff;
+	UINT tmp = 0;
+	INT status;
 
 	/* Copy Ioctl Buffer structure */
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(sWrmBuffer))
+	if (io_buff.InputLength > sizeof(wrm_buff))
 		return -EINVAL;
 
 	/* Get WrmBuffer structure */
-	if (copy_from_user(&sWrmBuffer, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+	if (copy_from_user(&wrm_buff, io_buff.InputBuffer,
+		io_buff.InputLength))
 		return -EFAULT;
 
-	uiTempVar = sWrmBuffer.Register & EEPROM_REJECT_MASK;
-	if (!((Adapter->pstargetparams->m_u32Customize) & VSG_MODE) &&
-		((uiTempVar == EEPROM_REJECT_REG_1) ||
-			(uiTempVar == EEPROM_REJECT_REG_2) ||
-			(uiTempVar == EEPROM_REJECT_REG_3) ||
-			(uiTempVar == EEPROM_REJECT_REG_4))) {
+	tmp = wrm_buff.Register & EEPROM_REJECT_MASK;
+	if (!((ad->pstargetparams->m_u32Customize) & VSG_MODE) &&
+		((tmp == EEPROM_REJECT_REG_1) ||
+			(tmp == EEPROM_REJECT_REG_2) ||
+			(tmp == EEPROM_REJECT_REG_3) ||
+			(tmp == EEPROM_REJECT_REG_4))) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"EEPROM Access Denied, not in VSG Mode\n");
 		return -EFAULT;
 	}
 
-	Status = wrmalt(Adapter, (UINT)sWrmBuffer.Register,
-			(PUINT)sWrmBuffer.Data, sizeof(ULONG));
+	status = wrmalt(ad, (UINT)wrm_buff.Register,
+			(PUINT)wrm_buff.Data, sizeof(ULONG));
 
-	if (Status == STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+	if (status == STATUS_SUCCESS) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL, "WRM Done\n");
 	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL, "WRM Failed\n");
-		Status = -EFAULT;
+		status = -EFAULT;
 	}
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_eeprom_reg_read(void __user *argp,
-					  struct bcm_mini_adapter *Adapter)
+					  struct bcm_mini_adapter *ad)
 {
-	struct bcm_rdm_buffer sRdmBuffer = {0};
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_rdm_buffer rdm_buff = {0};
+	struct bcm_ioctl_buffer io_buff;
 	PCHAR temp_buff = NULL;
-	UINT uiTempVar = 0;
-	INT Status;
+	UINT tmp = 0;
+	INT status;
 	int bytes;
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Device in Idle Mode, Blocking Rdms\n");
 		return -EACCES;
 	}
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(sRdmBuffer))
+	if (io_buff.InputLength > sizeof(rdm_buff))
 		return -EINVAL;
 
-	if (copy_from_user(&sRdmBuffer, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+	if (copy_from_user(&rdm_buff, io_buff.InputBuffer,
+		io_buff.InputLength))
 		return -EFAULT;
 
-	if (IoBuffer.OutputLength > USHRT_MAX ||
-		IoBuffer.OutputLength == 0) {
+	if (io_buff.OutputLength > USHRT_MAX ||
+		io_buff.OutputLength == 0) {
 		return -EINVAL;
 	}
 
-	temp_buff = kmalloc(IoBuffer.OutputLength, GFP_KERNEL);
+	temp_buff = kmalloc(io_buff.OutputLength, GFP_KERNEL);
 	if (!temp_buff)
 		return STATUS_FAILURE;
 
-	if ((((ULONG)sRdmBuffer.Register & 0x0F000000) != 0x0F000000) ||
-		((ULONG)sRdmBuffer.Register & 0x3)) {
+	if ((((ULONG)rdm_buff.Register & 0x0F000000) != 0x0F000000) ||
+		((ULONG)rdm_buff.Register & 0x3)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"RDM Done On invalid Address : %x Access Denied.\n",
-				(int)sRdmBuffer.Register);
+				(int)rdm_buff.Register);
 
 		kfree(temp_buff);
 		return -EINVAL;
 	}
 
-	uiTempVar = sRdmBuffer.Register & EEPROM_REJECT_MASK;
-	bytes = rdmaltWithLock(Adapter, (UINT)sRdmBuffer.Register,
-			       (PUINT)temp_buff, IoBuffer.OutputLength);
+	tmp = rdm_buff.Register & EEPROM_REJECT_MASK;
+	bytes = rdmaltWithLock(ad, (UINT)rdm_buff.Register,
+			       (PUINT)temp_buff, io_buff.OutputLength);
 
 	if (bytes > 0) {
-		Status = STATUS_SUCCESS;
-		if (copy_to_user(IoBuffer.OutputBuffer, temp_buff, bytes)) {
+		status = STATUS_SUCCESS;
+		if (copy_to_user(io_buff.OutputBuffer, temp_buff, bytes)) {
 			kfree(temp_buff);
 			return -EFAULT;
 		}
 	} else {
-		Status = bytes;
+		status = bytes;
 	}
 
 	kfree(temp_buff);
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_eeprom_reg_write(void __user *argp,
-					   struct bcm_mini_adapter *Adapter,
+					   struct bcm_mini_adapter *ad,
 					   UINT cmd)
 {
-	struct bcm_wrm_buffer sWrmBuffer = {0};
-	struct bcm_ioctl_buffer IoBuffer;
-	UINT uiTempVar = 0;
-	INT Status;
+	struct bcm_wrm_buffer wrm_buff = {0};
+	struct bcm_ioctl_buffer io_buff;
+	UINT tmp = 0;
+	INT status;
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Device in Idle Mode, Blocking Wrms\n");
 		return -EACCES;
 	}
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(sWrmBuffer))
+	if (io_buff.InputLength > sizeof(wrm_buff))
 		return -EINVAL;
 
 	/* Get WrmBuffer structure */
-	if (copy_from_user(&sWrmBuffer, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+	if (copy_from_user(&wrm_buff, io_buff.InputBuffer,
+		io_buff.InputLength))
 		return -EFAULT;
 
-	if ((((ULONG)sWrmBuffer.Register & 0x0F000000) != 0x0F000000) ||
-		((ULONG)sWrmBuffer.Register & 0x3)) {
+	if ((((ULONG)wrm_buff.Register & 0x0F000000) != 0x0F000000) ||
+		((ULONG)wrm_buff.Register & 0x3)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"WRM Done On invalid Address : %x Access Denied.\n",
-				(int)sWrmBuffer.Register);
+				(int)wrm_buff.Register);
 		return -EINVAL;
 	}
 
-	uiTempVar = sWrmBuffer.Register & EEPROM_REJECT_MASK;
-	if (!((Adapter->pstargetparams->m_u32Customize) & VSG_MODE) &&
-			((uiTempVar == EEPROM_REJECT_REG_1) ||
-			(uiTempVar == EEPROM_REJECT_REG_2) ||
-			(uiTempVar == EEPROM_REJECT_REG_3) ||
-			(uiTempVar == EEPROM_REJECT_REG_4)) &&
+	tmp = wrm_buff.Register & EEPROM_REJECT_MASK;
+	if (!((ad->pstargetparams->m_u32Customize) & VSG_MODE) &&
+			((tmp == EEPROM_REJECT_REG_1) ||
+			(tmp == EEPROM_REJECT_REG_2) ||
+			(tmp == EEPROM_REJECT_REG_3) ||
+			(tmp == EEPROM_REJECT_REG_4)) &&
 			(cmd == IOCTL_BCM_REGISTER_WRITE)) {
 
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"EEPROM Access Denied, not in VSG Mode\n");
 			return -EFAULT;
 	}
 
-	Status = wrmaltWithLock(Adapter, (UINT)sWrmBuffer.Register,
-				(PUINT)sWrmBuffer.Data,
-				sWrmBuffer.Length);
+	status = wrmaltWithLock(ad, (UINT)wrm_buff.Register,
+				(PUINT)wrm_buff.Data,
+				wrm_buff.Length);
 
-	if (Status == STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, OSAL_DBG,
+	if (status == STATUS_SUCCESS) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, OSAL_DBG,
 				DBG_LVL_ALL, "WRM Done\n");
 	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL, "WRM Failed\n");
-		Status = -EFAULT;
+		status = -EFAULT;
 	}
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_gpio_set_request(void __user *argp,
-					   struct bcm_mini_adapter *Adapter)
+					   struct bcm_mini_adapter *ad)
 {
 	struct bcm_gpio_info gpio_info = {0};
-	struct bcm_ioctl_buffer IoBuffer;
-	UCHAR ucResetValue[4];
+	struct bcm_ioctl_buffer io_buff;
+	UCHAR reset_val[4];
 	UINT value = 0;
-	UINT uiBit = 0;
-	UINT uiOperation = 0;
-	INT Status;
+	UINT bit = 0;
+	UINT operation = 0;
+	INT status;
 	int bytes;
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"GPIO Can't be set/clear in Low power Mode");
 		return -EACCES;
 	}
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(gpio_info))
+	if (io_buff.InputLength > sizeof(gpio_info))
 		return -EINVAL;
 
-	if (copy_from_user(&gpio_info, IoBuffer.InputBuffer,
-			   IoBuffer.InputLength))
+	if (copy_from_user(&gpio_info, io_buff.InputBuffer,
+			   io_buff.InputLength))
 		return -EFAULT;
 
-	uiBit  = gpio_info.uiGpioNumber;
-	uiOperation = gpio_info.uiGpioValue;
-	value = (1<<uiBit);
+	bit  = gpio_info.uiGpioNumber;
+	operation = gpio_info.uiGpioValue;
+	value = (1<<bit);
 
-	if (IsReqGpioIsLedInNVM(Adapter, value) == false) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+	if (IsReqGpioIsLedInNVM(ad, value) == false) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"Sorry, Requested GPIO<0x%X> is not correspond to LED !!!",
 				value);
@@ -518,202 +521,200 @@ static int bcm_char_ioctl_gpio_set_request(void __user *argp,
 	}
 
 	/* Set - setting 1 */
-	if (uiOperation) {
+	if (operation) {
 		/* Set the gpio output register */
-		Status = wrmaltWithLock(Adapter,
+		status = wrmaltWithLock(ad,
 					BCM_GPIO_OUTPUT_SET_REG,
 					(PUINT)(&value), sizeof(UINT));
 
-		if (Status == STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+		if (status == STATUS_SUCCESS) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
 					OSAL_DBG, DBG_LVL_ALL,
 					"Set the GPIO bit\n");
 		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
 					OSAL_DBG, DBG_LVL_ALL,
 					"Failed to set the %dth GPIO\n",
-					uiBit);
-			return Status;
+					bit);
+			return status;
 		}
 	} else {
 		/* Set the gpio output register */
-		Status = wrmaltWithLock(Adapter,
+		status = wrmaltWithLock(ad,
 					BCM_GPIO_OUTPUT_CLR_REG,
 					(PUINT)(&value), sizeof(UINT));
 
-		if (Status == STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+		if (status == STATUS_SUCCESS) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
 					OSAL_DBG, DBG_LVL_ALL,
 					"Set the GPIO bit\n");
 		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
 					OSAL_DBG, DBG_LVL_ALL,
 					"Failed to clear the %dth GPIO\n",
-					uiBit);
-			return Status;
+					bit);
+			return status;
 		}
 	}
 
-	bytes = rdmaltWithLock(Adapter, (UINT)GPIO_MODE_REGISTER,
-			       (PUINT)ucResetValue, sizeof(UINT));
+	bytes = rdmaltWithLock(ad, (UINT)GPIO_MODE_REGISTER,
+			       (PUINT)reset_val, sizeof(UINT));
 	if (bytes < 0) {
-		Status = bytes;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		status = bytes;
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"GPIO_MODE_REGISTER read failed");
-		return Status;
-	} else {
-		Status = STATUS_SUCCESS;
+		return status;
 	}
+	status = STATUS_SUCCESS;
 
 	/* Set the gpio mode register to output */
-	*(UINT *)ucResetValue |= (1<<uiBit);
-	Status = wrmaltWithLock(Adapter, GPIO_MODE_REGISTER,
-				(PUINT)ucResetValue, sizeof(UINT));
+	*(UINT *)reset_val |= (1<<bit);
+	status = wrmaltWithLock(ad, GPIO_MODE_REGISTER,
+				(PUINT)reset_val, sizeof(UINT));
 
-	if (Status == STATUS_SUCCESS) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+	if (status == STATUS_SUCCESS) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"Set the GPIO to output Mode\n");
 	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"Failed to put GPIO in Output Mode\n");
 	}
 
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_led_thread_state_change_req(void __user *argp,
-		struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *ad)
 {
-	struct bcm_user_thread_req threadReq = {0};
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_user_thread_req thread_req = {0};
+	struct bcm_ioctl_buffer io_buff;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"User made LED thread InActive");
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"GPIO Can't be set/clear in Low power Mode");
 		return -EACCES;
 	}
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(threadReq))
+	if (io_buff.InputLength > sizeof(thread_req))
 		return -EINVAL;
 
-	if (copy_from_user(&threadReq, IoBuffer.InputBuffer,
-			   IoBuffer.InputLength))
+	if (copy_from_user(&thread_req, io_buff.InputBuffer,
+			   io_buff.InputLength))
 		return -EFAULT;
 
 	/* if LED thread is running(Actively or Inactively)
 	 * set it state to make inactive
 	 */
-	if (Adapter->LEDInfo.led_thread_running) {
-		if (threadReq.ThreadState == LED_THREAD_ACTIVATION_REQ) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+	if (ad->LEDInfo.led_thread_running) {
+		if (thread_req.ThreadState == LED_THREAD_ACTIVATION_REQ) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
 					OSAL_DBG, DBG_LVL_ALL,
 					"Activating thread req");
-			Adapter->DriverState = LED_THREAD_ACTIVE;
+			ad->DriverState = LED_THREAD_ACTIVE;
 		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS,
 					OSAL_DBG, DBG_LVL_ALL,
 					"DeActivating Thread req.....");
-			Adapter->DriverState = LED_THREAD_INACTIVE;
+			ad->DriverState = LED_THREAD_INACTIVE;
 		}
 
 		/* signal thread. */
-		wake_up(&Adapter->LEDInfo.notify_led_event);
+		wake_up(&ad->LEDInfo.notify_led_event);
 	}
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_gpio_status_request(void __user *argp,
-		struct bcm_mini_adapter *Adapter)
+					      struct bcm_mini_adapter *ad)
 {
 	struct bcm_gpio_info gpio_info = {0};
-	struct bcm_ioctl_buffer IoBuffer;
-	ULONG uiBit = 0;
-	UCHAR ucRead[4];
-	INT Status;
+	struct bcm_ioctl_buffer io_buff;
+	ULONG bit = 0;
+	UCHAR read[4];
+	INT status;
 	int bytes;
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE))
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE))
 		return -EACCES;
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(gpio_info))
+	if (io_buff.InputLength > sizeof(gpio_info))
 		return -EINVAL;
 
-	if (copy_from_user(&gpio_info, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+	if (copy_from_user(&gpio_info, io_buff.InputBuffer,
+		io_buff.InputLength))
 		return -EFAULT;
 
-	uiBit = gpio_info.uiGpioNumber;
+	bit = gpio_info.uiGpioNumber;
 
 	/* Set the gpio output register */
-	bytes = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER,
-				(PUINT)ucRead, sizeof(UINT));
+	bytes = rdmaltWithLock(ad, (UINT)GPIO_PIN_STATE_REGISTER,
+				(PUINT)read, sizeof(UINT));
 
 	if (bytes < 0) {
-		Status = bytes;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		status = bytes;
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"RDM Failed\n");
-		return Status;
-	} else {
-		Status = STATUS_SUCCESS;
+		return status;
 	}
-	return Status;
+	status = STATUS_SUCCESS;
+	return status;
 }
 
 static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
-		struct bcm_mini_adapter *Adapter)
+					     struct bcm_mini_adapter *ad)
 {
 	struct bcm_gpio_multi_info gpio_multi_info[MAX_IDX];
 	struct bcm_gpio_multi_info *pgpio_multi_info =
 		(struct bcm_gpio_multi_info *)gpio_multi_info;
-	struct bcm_ioctl_buffer IoBuffer;
-	UCHAR ucResetValue[4];
-	INT Status = STATUS_FAILURE;
+	struct bcm_ioctl_buffer io_buff;
+	UCHAR reset_val[4];
+	INT status = STATUS_FAILURE;
 	int bytes;
 
 	memset(pgpio_multi_info, 0,
 	       MAX_IDX * sizeof(struct bcm_gpio_multi_info));
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE))
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE))
 		return -EINVAL;
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(gpio_multi_info))
+	if (io_buff.InputLength > sizeof(gpio_multi_info))
 		return -EINVAL;
-	if (IoBuffer.OutputLength > sizeof(gpio_multi_info))
-		IoBuffer.OutputLength = sizeof(gpio_multi_info);
+	if (io_buff.OutputLength > sizeof(gpio_multi_info))
+		io_buff.OutputLength = sizeof(gpio_multi_info);
 
-	if (copy_from_user(&gpio_multi_info, IoBuffer.InputBuffer,
-			   IoBuffer.InputLength))
+	if (copy_from_user(&gpio_multi_info, io_buff.InputBuffer,
+			   io_buff.InputLength))
 		return -EFAULT;
 
-	if (IsReqGpioIsLedInNVM(Adapter, pgpio_multi_info[WIMAX_IDX].uiGPIOMask)
+	if (IsReqGpioIsLedInNVM(ad, pgpio_multi_info[WIMAX_IDX].uiGPIOMask)
 			== false) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"Sorry, Requested GPIO<0x%X> is not correspond to NVM LED bit map<0x%X>!!!",
 				pgpio_multi_info[WIMAX_IDX].uiGPIOMask,
-				Adapter->gpioBitMap);
+				ad->gpioBitMap);
 		return -EINVAL;
 	}
 
@@ -721,428 +722,425 @@ static int bcm_char_ioctl_gpio_multi_request(void __user *argp,
 	if ((pgpio_multi_info[WIMAX_IDX].uiGPIOMask) &
 		(pgpio_multi_info[WIMAX_IDX].uiGPIOCommand)) {
 		/* Set 1's in GPIO OUTPUT REGISTER */
-		*(UINT *)ucResetValue = pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
+		*(UINT *)reset_val = pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOValue;
 
-		if (*(UINT *) ucResetValue)
-			Status = wrmaltWithLock(Adapter,
+		if (*(UINT *) reset_val)
+			status = wrmaltWithLock(ad,
 				BCM_GPIO_OUTPUT_SET_REG,
-				(PUINT)ucResetValue, sizeof(ULONG));
+				(PUINT)reset_val, sizeof(ULONG));
 
-		if (Status != STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		if (status != STATUS_SUCCESS) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"WRM to BCM_GPIO_OUTPUT_SET_REG Failed.");
-			return Status;
+			return status;
 		}
 
 		/* Clear to 0's in GPIO OUTPUT REGISTER */
-		*(UINT *)ucResetValue =
+		*(UINT *)reset_val =
 			(pgpio_multi_info[WIMAX_IDX].uiGPIOMask &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOCommand &
 			(~(pgpio_multi_info[WIMAX_IDX].uiGPIOValue)));
 
-		if (*(UINT *) ucResetValue)
-			Status = wrmaltWithLock(Adapter,
-				BCM_GPIO_OUTPUT_CLR_REG, (PUINT)ucResetValue,
+		if (*(UINT *) reset_val)
+			status = wrmaltWithLock(ad,
+				BCM_GPIO_OUTPUT_CLR_REG, (PUINT)reset_val,
 				sizeof(ULONG));
 
-		if (Status != STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		if (status != STATUS_SUCCESS) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"WRM to BCM_GPIO_OUTPUT_CLR_REG Failed.");
-			return Status;
+			return status;
 		}
 	}
 
 	if (pgpio_multi_info[WIMAX_IDX].uiGPIOMask) {
-		bytes = rdmaltWithLock(Adapter, (UINT)GPIO_PIN_STATE_REGISTER,
-				       (PUINT)ucResetValue, sizeof(UINT));
+		bytes = rdmaltWithLock(ad, (UINT)GPIO_PIN_STATE_REGISTER,
+				       (PUINT)reset_val, sizeof(UINT));
 
 		if (bytes < 0) {
-			Status = bytes;
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			status = bytes;
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"RDM to GPIO_PIN_STATE_REGISTER Failed.");
-			return Status;
-		} else {
-			Status = STATUS_SUCCESS;
+			return status;
 		}
+		status = STATUS_SUCCESS;
 
 		pgpio_multi_info[WIMAX_IDX].uiGPIOValue =
-			(*(UINT *)ucResetValue &
+			(*(UINT *)reset_val &
 			pgpio_multi_info[WIMAX_IDX].uiGPIOMask);
 	}
 
-	Status = copy_to_user(IoBuffer.OutputBuffer, &gpio_multi_info,
-		IoBuffer.OutputLength);
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	status = copy_to_user(io_buff.OutputBuffer, &gpio_multi_info,
+		io_buff.OutputLength);
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Failed while copying Content to IOBufer for user space err:%d",
-			Status);
+			status);
 		return -EFAULT;
 	}
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_gpio_mode_request(void __user *argp,
-		struct bcm_mini_adapter *Adapter)
+					    struct bcm_mini_adapter *ad)
 {
 	struct bcm_gpio_multi_mode gpio_multi_mode[MAX_IDX];
 	struct bcm_gpio_multi_mode *pgpio_multi_mode =
 		(struct bcm_gpio_multi_mode *)gpio_multi_mode;
-	struct bcm_ioctl_buffer IoBuffer;
-	UCHAR ucResetValue[4];
-	INT Status;
+	struct bcm_ioctl_buffer io_buff;
+	UCHAR reset_val[4];
+	INT status;
 	int bytes;
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE))
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE))
 		return -EINVAL;
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength > sizeof(gpio_multi_mode))
+	if (io_buff.InputLength > sizeof(gpio_multi_mode))
 		return -EINVAL;
-	if (IoBuffer.OutputLength > sizeof(gpio_multi_mode))
-		IoBuffer.OutputLength = sizeof(gpio_multi_mode);
+	if (io_buff.OutputLength > sizeof(gpio_multi_mode))
+		io_buff.OutputLength = sizeof(gpio_multi_mode);
 
-	if (copy_from_user(&gpio_multi_mode, IoBuffer.InputBuffer,
-		IoBuffer.InputLength))
+	if (copy_from_user(&gpio_multi_mode, io_buff.InputBuffer,
+		io_buff.InputLength))
 		return -EFAULT;
 
-	bytes = rdmaltWithLock(Adapter, (UINT)GPIO_MODE_REGISTER,
-		(PUINT)ucResetValue, sizeof(UINT));
+	bytes = rdmaltWithLock(ad, (UINT)GPIO_MODE_REGISTER,
+		(PUINT)reset_val, sizeof(UINT));
 
 	if (bytes < 0) {
-		Status = bytes;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		status = bytes;
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Read of GPIO_MODE_REGISTER failed");
-		return Status;
-	} else {
-		Status = STATUS_SUCCESS;
+		return status;
 	}
+	status = STATUS_SUCCESS;
 
 	/* Validating the request */
-	if (IsReqGpioIsLedInNVM(Adapter, pgpio_multi_mode[WIMAX_IDX].uiGPIOMask)
+	if (IsReqGpioIsLedInNVM(ad, pgpio_multi_mode[WIMAX_IDX].uiGPIOMask)
 			== false) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Sorry, Requested GPIO<0x%X> is not correspond to NVM LED bit map<0x%X>!!!",
 				pgpio_multi_mode[WIMAX_IDX].uiGPIOMask,
-				Adapter->gpioBitMap);
+				ad->gpioBitMap);
 		return -EINVAL;
 	}
 
 	if (pgpio_multi_mode[WIMAX_IDX].uiGPIOMask) {
 		/* write all OUT's (1's) */
-		*(UINT *) ucResetValue |=
+		*(UINT *) reset_val |=
 			(pgpio_multi_mode[WIMAX_IDX].uiGPIOMode &
 					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
 
 		/* write all IN's (0's) */
-		*(UINT *) ucResetValue &=
+		*(UINT *) reset_val &=
 			~((~pgpio_multi_mode[WIMAX_IDX].uiGPIOMode) &
 					pgpio_multi_mode[WIMAX_IDX].uiGPIOMask);
 
 		/* Currently implemented return the modes of all GPIO's
 		 * else needs to bit AND with  mask
 		 */
-		pgpio_multi_mode[WIMAX_IDX].uiGPIOMode = *(UINT *)ucResetValue;
+		pgpio_multi_mode[WIMAX_IDX].uiGPIOMode = *(UINT *)reset_val;
 
-		Status = wrmaltWithLock(Adapter, GPIO_MODE_REGISTER,
-			(PUINT)ucResetValue, sizeof(ULONG));
-		if (Status == STATUS_SUCCESS) {
-			BCM_DEBUG_PRINT(Adapter,
+		status = wrmaltWithLock(ad, GPIO_MODE_REGISTER,
+			(PUINT)reset_val, sizeof(ULONG));
+		if (status == STATUS_SUCCESS) {
+			BCM_DEBUG_PRINT(ad,
 				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"WRM to GPIO_MODE_REGISTER Done");
 		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"WRM to GPIO_MODE_REGISTER Failed");
 			return -EFAULT;
 		}
 	} else {
 		/* if uiGPIOMask is 0 then return mode register configuration */
-		pgpio_multi_mode[WIMAX_IDX].uiGPIOMode = *(UINT *)ucResetValue;
+		pgpio_multi_mode[WIMAX_IDX].uiGPIOMode = *(UINT *)reset_val;
 	}
 
-	Status = copy_to_user(IoBuffer.OutputBuffer, &gpio_multi_mode,
-		IoBuffer.OutputLength);
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	status = copy_to_user(io_buff.OutputBuffer, &gpio_multi_mode,
+		io_buff.OutputLength);
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Failed while copying Content to IOBufer for user space err:%d",
-			Status);
+			status);
 		return -EFAULT;
 	}
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_misc_request(void __user *argp,
-		struct bcm_mini_adapter *Adapter)
+				       struct bcm_mini_adapter *ad)
 {
-	struct bcm_ioctl_buffer IoBuffer;
-	PVOID pvBuffer = NULL;
-	INT Status;
+	struct bcm_ioctl_buffer io_buff;
+	PVOID buff = NULL;
+	INT status;
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength < sizeof(struct bcm_link_request))
+	if (io_buff.InputLength < sizeof(struct bcm_link_request))
 		return -EINVAL;
 
-	if (IoBuffer.InputLength > MAX_CNTL_PKT_SIZE)
+	if (io_buff.InputLength > MAX_CNTL_PKT_SIZE)
 		return -EINVAL;
 
-	pvBuffer = memdup_user(IoBuffer.InputBuffer,
-			       IoBuffer.InputLength);
-	if (IS_ERR(pvBuffer))
-		return PTR_ERR(pvBuffer);
+	buff = memdup_user(io_buff.InputBuffer,
+			       io_buff.InputLength);
+	if (IS_ERR(buff))
+		return PTR_ERR(buff);
 
-	down(&Adapter->LowPowerModeSync);
-	Status = wait_event_interruptible_timeout(
-			Adapter->lowpower_mode_wait_queue,
-			!Adapter->bPreparingForLowPowerMode,
+	down(&ad->LowPowerModeSync);
+	status = wait_event_interruptible_timeout(
+			ad->lowpower_mode_wait_queue,
+			!ad->bPreparingForLowPowerMode,
 			(1 * HZ));
 
-	if (Status == -ERESTARTSYS)
+	if (status == -ERESTARTSYS)
 		goto cntrlEnd;
 
-	if (Adapter->bPreparingForLowPowerMode) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	if (ad->bPreparingForLowPowerMode) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Preparing Idle Mode is still True - Hence Rejecting control message\n");
-		Status = STATUS_FAILURE;
+		status = STATUS_FAILURE;
 		goto cntrlEnd;
 	}
-	Status = CopyBufferToControlPacket(Adapter, (PVOID)pvBuffer);
+	status = CopyBufferToControlPacket(ad, (PVOID)buff);
 
 cntrlEnd:
-	up(&Adapter->LowPowerModeSync);
-	kfree(pvBuffer);
-	return Status;
+	up(&ad->LowPowerModeSync);
+	kfree(buff);
+	return status;
 }
 
 static int bcm_char_ioctl_buffer_download_start(
-		struct bcm_mini_adapter *Adapter)
+		struct bcm_mini_adapter *ad)
 {
-	INT Status;
+	INT status;
 
-	if (down_trylock(&Adapter->NVMRdmWrmLock)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	if (down_trylock(&ad->NVMRdmWrmLock)) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"IOCTL_BCM_CHIP_RESET not allowed as EEPROM Read/Write is in progress\n");
 		return -EACCES;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Starting the firmware download PID =0x%x!!!!\n",
 			current->pid);
 
-	if (down_trylock(&Adapter->fw_download_sema))
+	if (down_trylock(&ad->fw_download_sema))
 		return -EBUSY;
 
-	Adapter->bBinDownloaded = false;
-	Adapter->fw_download_process_pid = current->pid;
-	Adapter->bCfgDownloaded = false;
-	Adapter->fw_download_done = false;
-	netif_carrier_off(Adapter->dev);
-	netif_stop_queue(Adapter->dev);
-	Status = reset_card_proc(Adapter);
-	if (Status) {
-		pr_err(PFX "%s: reset_card_proc Failed!\n", Adapter->dev->name);
-		up(&Adapter->fw_download_sema);
-		up(&Adapter->NVMRdmWrmLock);
-		return Status;
+	ad->bBinDownloaded = false;
+	ad->fw_download_process_pid = current->pid;
+	ad->bCfgDownloaded = false;
+	ad->fw_download_done = false;
+	netif_carrier_off(ad->dev);
+	netif_stop_queue(ad->dev);
+	status = reset_card_proc(ad);
+	if (status) {
+		pr_err(PFX "%s: reset_card_proc Failed!\n", ad->dev->name);
+		up(&ad->fw_download_sema);
+		up(&ad->NVMRdmWrmLock);
+		return status;
 	}
 	mdelay(10);
 
-	up(&Adapter->NVMRdmWrmLock);
-	return Status;
+	up(&ad->NVMRdmWrmLock);
+	return status;
 }
 
 static int bcm_char_ioctl_buffer_download(void __user *argp,
-		struct bcm_mini_adapter *Adapter)
+					  struct bcm_mini_adapter *ad)
 {
-	struct bcm_firmware_info *psFwInfo = NULL;
-	struct bcm_ioctl_buffer IoBuffer;
-	INT Status;
+	struct bcm_firmware_info *fw_info = NULL;
+	struct bcm_ioctl_buffer io_buff;
+	INT status;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 		"Starting the firmware download PID =0x%x!!!!\n", current->pid);
 
-	if (!down_trylock(&Adapter->fw_download_sema)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (!down_trylock(&ad->fw_download_sema)) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Invalid way to download buffer. Use Start and then call this!!!\n");
-		up(&Adapter->fw_download_sema);
+		up(&ad->fw_download_sema);
 		return -EINVAL;
 	}
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer))) {
-		up(&Adapter->fw_download_sema);
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer))) {
+		up(&ad->fw_download_sema);
 		return -EFAULT;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Length for FW DLD is : %lx\n", IoBuffer.InputLength);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+			"Length for FW DLD is : %lx\n", io_buff.InputLength);
 
-	if (IoBuffer.InputLength > sizeof(struct bcm_firmware_info)) {
-		up(&Adapter->fw_download_sema);
+	if (io_buff.InputLength > sizeof(struct bcm_firmware_info)) {
+		up(&ad->fw_download_sema);
 		return -EINVAL;
 	}
 
-	psFwInfo = kmalloc(sizeof(*psFwInfo), GFP_KERNEL);
-	if (!psFwInfo) {
-		up(&Adapter->fw_download_sema);
+	fw_info = kmalloc(sizeof(*fw_info), GFP_KERNEL);
+	if (!fw_info) {
+		up(&ad->fw_download_sema);
 		return -ENOMEM;
 	}
 
-	if (copy_from_user(psFwInfo, IoBuffer.InputBuffer,
-		IoBuffer.InputLength)) {
-		up(&Adapter->fw_download_sema);
-		kfree(psFwInfo);
+	if (copy_from_user(fw_info, io_buff.InputBuffer,
+		io_buff.InputLength)) {
+		up(&ad->fw_download_sema);
+		kfree(fw_info);
 		return -EFAULT;
 	}
 
-	if (!psFwInfo->pvMappedFirmwareAddress ||
-		(psFwInfo->u32FirmwareLength == 0)) {
+	if (!fw_info->pvMappedFirmwareAddress ||
+		(fw_info->u32FirmwareLength == 0)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Something else is wrong %lu\n",
-				psFwInfo->u32FirmwareLength);
-		up(&Adapter->fw_download_sema);
-		kfree(psFwInfo);
-		Status = -EINVAL;
-		return Status;
+				fw_info->u32FirmwareLength);
+		up(&ad->fw_download_sema);
+		kfree(fw_info);
+		status = -EINVAL;
+		return status;
 	}
 
-	Status = bcm_ioctl_fw_download(Adapter, psFwInfo);
+	status = bcm_ioctl_fw_download(ad, fw_info);
 
-	if (Status != STATUS_SUCCESS) {
-		if (psFwInfo->u32StartingAddress == CONFIG_BEGIN_ADDR)
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (status != STATUS_SUCCESS) {
+		if (fw_info->u32StartingAddress == CONFIG_BEGIN_ADDR)
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"IOCTL: Configuration File Upload Failed\n");
 		else
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"IOCTL: Firmware File Upload Failed\n");
 
-		/* up(&Adapter->fw_download_sema); */
+		/* up(&ad->fw_download_sema); */
 
-		if (Adapter->LEDInfo.led_thread_running &
+		if (ad->LEDInfo.led_thread_running &
 			BCM_LED_THREAD_RUNNING_ACTIVELY) {
-			Adapter->DriverState = DRIVER_INIT;
-			Adapter->LEDInfo.bLedInitDone = false;
-			wake_up(&Adapter->LEDInfo.notify_led_event);
+			ad->DriverState = DRIVER_INIT;
+			ad->LEDInfo.bLedInitDone = false;
+			wake_up(&ad->LEDInfo.notify_led_event);
 		}
 	}
 
-	if (Status != STATUS_SUCCESS)
-		up(&Adapter->fw_download_sema);
+	if (status != STATUS_SUCCESS)
+		up(&ad->fw_download_sema);
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, OSAL_DBG, DBG_LVL_ALL,
 		"IOCTL: Firmware File Uploaded\n");
-	kfree(psFwInfo);
-	return Status;
+	kfree(fw_info);
+	return status;
 }
 
 static int bcm_char_ioctl_buffer_download_stop(void __user *argp,
-		struct bcm_mini_adapter *Adapter)
+					       struct bcm_mini_adapter *ad)
 {
-	INT Status;
+	INT status;
 	int timeout = 0;
 
-	if (!down_trylock(&Adapter->fw_download_sema)) {
-		up(&Adapter->fw_download_sema);
+	if (!down_trylock(&ad->fw_download_sema)) {
+		up(&ad->fw_download_sema);
 		return -EINVAL;
 	}
 
-	if (down_trylock(&Adapter->NVMRdmWrmLock)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (down_trylock(&ad->NVMRdmWrmLock)) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"FW download blocked as EEPROM Read/Write is in progress\n");
-		up(&Adapter->fw_download_sema);
+		up(&ad->fw_download_sema);
 		return -EACCES;
 	}
 
-	Adapter->bBinDownloaded = TRUE;
-	Adapter->bCfgDownloaded = TRUE;
-	atomic_set(&Adapter->CurrNumFreeTxDesc, 0);
-	Adapter->CurrNumRecvDescs = 0;
-	Adapter->downloadDDR = 0;
+	ad->bBinDownloaded = TRUE;
+	ad->bCfgDownloaded = TRUE;
+	atomic_set(&ad->CurrNumFreeTxDesc, 0);
+	ad->CurrNumRecvDescs = 0;
+	ad->downloadDDR = 0;
 
 	/* setting the Mips to Run */
-	Status = run_card_proc(Adapter);
+	status = run_card_proc(ad);
 
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Firm Download Failed\n");
-		up(&Adapter->fw_download_sema);
-		up(&Adapter->NVMRdmWrmLock);
-		return Status;
-	} else {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-				DBG_LVL_ALL, "Firm Download Over...\n");
+		up(&ad->fw_download_sema);
+		up(&ad->NVMRdmWrmLock);
+		return status;
 	}
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
+			DBG_LVL_ALL, "Firm Download Over...\n");
 
 	mdelay(10);
 
 	/* Wait for MailBox Interrupt */
-	if (StartInterruptUrb((struct bcm_interface_adapter *)Adapter->pvInterfaceAdapter))
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (StartInterruptUrb((struct bcm_interface_adapter *)ad->pvInterfaceAdapter))
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Unable to send interrupt...\n");
 
 	timeout = 5*HZ;
-	Adapter->waiting_to_fw_download_done = false;
-	wait_event_timeout(Adapter->ioctl_fw_dnld_wait_queue,
-			Adapter->waiting_to_fw_download_done, timeout);
-	Adapter->fw_download_process_pid = INVALID_PID;
-	Adapter->fw_download_done = TRUE;
-	atomic_set(&Adapter->CurrNumFreeTxDesc, 0);
-	Adapter->CurrNumRecvDescs = 0;
-	Adapter->PrevNumRecvDescs = 0;
-	atomic_set(&Adapter->cntrlpktCnt, 0);
-	Adapter->LinkUpStatus = 0;
-	Adapter->LinkStatus = 0;
-
-	if (Adapter->LEDInfo.led_thread_running &
+	ad->waiting_to_fw_download_done = false;
+	wait_event_timeout(ad->ioctl_fw_dnld_wait_queue,
+			ad->waiting_to_fw_download_done, timeout);
+	ad->fw_download_process_pid = INVALID_PID;
+	ad->fw_download_done = TRUE;
+	atomic_set(&ad->CurrNumFreeTxDesc, 0);
+	ad->CurrNumRecvDescs = 0;
+	ad->PrevNumRecvDescs = 0;
+	atomic_set(&ad->cntrlpktCnt, 0);
+	ad->LinkUpStatus = 0;
+	ad->LinkStatus = 0;
+
+	if (ad->LEDInfo.led_thread_running &
 		BCM_LED_THREAD_RUNNING_ACTIVELY) {
-		Adapter->DriverState = FW_DOWNLOAD_DONE;
-		wake_up(&Adapter->LEDInfo.notify_led_event);
+		ad->DriverState = FW_DOWNLOAD_DONE;
+		wake_up(&ad->LEDInfo.notify_led_event);
 	}
 
 	if (!timeout)
-		Status = -ENODEV;
+		status = -ENODEV;
 
-	up(&Adapter->fw_download_sema);
-	up(&Adapter->NVMRdmWrmLock);
-	return Status;
+	up(&ad->fw_download_sema);
+	up(&ad->NVMRdmWrmLock);
+	return status;
 }
 
-static int bcm_char_ioctl_chip_reset(struct bcm_mini_adapter *Adapter)
+static int bcm_char_ioctl_chip_reset(struct bcm_mini_adapter *ad)
 {
-	INT Status;
-	INT NVMAccess;
+	INT status;
+	INT nvm_access;
 
-	NVMAccess = down_trylock(&Adapter->NVMRdmWrmLock);
-	if (NVMAccess) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	nvm_access = down_trylock(&ad->NVMRdmWrmLock);
+	if (nvm_access) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			" IOCTL_BCM_CHIP_RESET not allowed as EEPROM Read/Write is in progress\n");
 		return -EACCES;
 	}
 
-	down(&Adapter->RxAppControlQueuelock);
-	Status = reset_card_proc(Adapter);
+	down(&ad->RxAppControlQueuelock);
+	status = reset_card_proc(ad);
 	flushAllAppQ();
-	up(&Adapter->RxAppControlQueuelock);
-	up(&Adapter->NVMRdmWrmLock);
-	ResetCounters(Adapter);
-	return Status;
+	up(&ad->RxAppControlQueuelock);
+	up(&ad->NVMRdmWrmLock);
+	ResetCounters(ad);
+	return status;
 }
 
 static int bcm_char_ioctl_qos_threshold(ULONG arg,
-					struct bcm_mini_adapter *Adapter)
+					struct bcm_mini_adapter *ad)
 {
-	USHORT uiLoopIndex;
+	USHORT i;
 
-	for (uiLoopIndex = 0; uiLoopIndex < NO_OF_QUEUES; uiLoopIndex++) {
-		if (get_user(Adapter->PackInfo[uiLoopIndex].uiThreshold,
+	for (i = 0; i < NO_OF_QUEUES; i++) {
+		if (get_user(ad->PackInfo[i].uiThreshold,
 				(unsigned long __user *)arg)) {
 			return -EFAULT;
 		}
@@ -1151,68 +1149,68 @@ static int bcm_char_ioctl_qos_threshold(ULONG arg,
 }
 
 static int bcm_char_ioctl_switch_transfer_mode(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					       struct bcm_mini_adapter *ad)
 {
-	UINT uiData = 0;
+	UINT data = 0;
 
-	if (copy_from_user(&uiData, argp, sizeof(UINT)))
+	if (copy_from_user(&data, argp, sizeof(UINT)))
 		return -EFAULT;
 
-	if (uiData) {
+	if (data) {
 		/* Allow All Packets */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"IOCTL_BCM_SWITCH_TRANSFER_MODE: ETH_PACKET_TUNNELING_MODE\n");
-			Adapter->TransferMode = ETH_PACKET_TUNNELING_MODE;
+			ad->TransferMode = ETH_PACKET_TUNNELING_MODE;
 	} else {
 		/* Allow IP only Packets */
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"IOCTL_BCM_SWITCH_TRANSFER_MODE: IP_PACKET_ONLY_MODE\n");
-		Adapter->TransferMode = IP_PACKET_ONLY_MODE;
+		ad->TransferMode = IP_PACKET_ONLY_MODE;
 	}
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_get_driver_version(void __user *argp)
 {
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_ioctl_buffer io_buff;
 	ulong len;
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	len = min_t(ulong, IoBuffer.OutputLength, strlen(DRV_VERSION) + 1);
+	len = min_t(ulong, io_buff.OutputLength, strlen(DRV_VERSION) + 1);
 
-	if (copy_to_user(IoBuffer.OutputBuffer, DRV_VERSION, len))
+	if (copy_to_user(io_buff.OutputBuffer, DRV_VERSION, len))
 		return -EFAULT;
 
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_get_current_status(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					     struct bcm_mini_adapter *ad)
 {
 	struct bcm_link_state link_state;
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_ioctl_buffer io_buff;
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer))) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer))) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"copy_from_user failed..\n");
 		return -EFAULT;
 	}
 
-	if (IoBuffer.OutputLength != sizeof(link_state))
+	if (io_buff.OutputLength != sizeof(link_state))
 		return -EINVAL;
 
 	memset(&link_state, 0, sizeof(link_state));
-	link_state.bIdleMode = Adapter->IdleMode;
-	link_state.bShutdownMode = Adapter->bShutStatus;
-	link_state.ucLinkStatus = Adapter->LinkStatus;
+	link_state.bIdleMode = ad->IdleMode;
+	link_state.bShutdownMode = ad->bShutStatus;
+	link_state.ucLinkStatus = ad->LinkStatus;
 
-	if (copy_to_user(IoBuffer.OutputBuffer, &link_state, min_t(size_t,
-		sizeof(link_state), IoBuffer.OutputLength))) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (copy_to_user(io_buff.OutputBuffer, &link_state, min_t(size_t,
+		sizeof(link_state), io_buff.OutputLength))) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Copy_to_user Failed..\n");
 		return -EFAULT;
 	}
@@ -1221,65 +1219,66 @@ static int bcm_char_ioctl_get_current_status(void __user *argp,
 
 
 static int bcm_char_ioctl_set_mac_tracing(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					  struct bcm_mini_adapter *ad)
 {
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_ioctl_buffer io_buff;
 	UINT tracing_flag;
 
 	/* copy ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (copy_from_user(&tracing_flag, IoBuffer.InputBuffer, sizeof(UINT)))
+	if (copy_from_user(&tracing_flag, io_buff.InputBuffer, sizeof(UINT)))
 		return -EFAULT;
 
 	if (tracing_flag)
-		Adapter->pTarangs->MacTracingEnabled = TRUE;
+		ad->pTarangs->MacTracingEnabled = TRUE;
 	else
-		Adapter->pTarangs->MacTracingEnabled = false;
+		ad->pTarangs->MacTracingEnabled = false;
 
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_get_dsx_indication(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					     struct bcm_mini_adapter *ad)
 {
-	struct bcm_ioctl_buffer IoBuffer;
-	ULONG ulSFId = 0;
+	struct bcm_ioctl_buffer io_buff;
+	ULONG sf_id = 0;
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.OutputLength < sizeof(struct bcm_add_indication_alt)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (io_buff.OutputLength < sizeof(struct bcm_add_indication_alt)) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Mismatch req: %lx needed is =0x%zx!!!",
-			IoBuffer.OutputLength,
+			io_buff.OutputLength,
 			sizeof(struct bcm_add_indication_alt));
 		return -EINVAL;
 	}
 
-	if (copy_from_user(&ulSFId, IoBuffer.InputBuffer, sizeof(ulSFId)))
+	if (copy_from_user(&sf_id, io_buff.InputBuffer, sizeof(sf_id)))
 		return -EFAULT;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"Get DSX Data SF ID is =%lx\n", ulSFId);
-	get_dsx_sf_data_to_application(Adapter, ulSFId, IoBuffer.OutputBuffer);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		"Get DSX Data SF ID is =%lx\n", sf_id);
+	get_dsx_sf_data_to_application(ad, sf_id, io_buff.OutputBuffer);
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_get_host_mibs(void __user *argp,
-	struct bcm_mini_adapter *Adapter, struct bcm_tarang_data *pTarang)
+					struct bcm_mini_adapter *ad,
+					struct bcm_tarang_data *tarang)
 {
-	struct bcm_ioctl_buffer IoBuffer;
-	INT Status = STATUS_FAILURE;
+	struct bcm_ioctl_buffer io_buff;
+	INT status = STATUS_FAILURE;
 	PVOID temp_buff;
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.OutputLength != sizeof(struct bcm_host_stats_mibs)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-			"Length Check failed %lu %zd\n", IoBuffer.OutputLength,
+	if (io_buff.OutputLength != sizeof(struct bcm_host_stats_mibs)) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+			"Length Check failed %lu %zd\n", io_buff.OutputLength,
 			sizeof(struct bcm_host_stats_mibs));
 		return -EINVAL;
 	}
@@ -1289,11 +1288,11 @@ static int bcm_char_ioctl_get_host_mibs(void __user *argp,
 	if (!temp_buff)
 		return STATUS_FAILURE;
 
-	Status = ProcessGetHostMibs(Adapter, temp_buff);
-	GetDroppedAppCntrlPktMibs(temp_buff, pTarang);
+	status = ProcessGetHostMibs(ad, temp_buff);
+	GetDroppedAppCntrlPktMibs(temp_buff, tarang);
 
-	if (Status != STATUS_FAILURE) {
-		if (copy_to_user(IoBuffer.OutputBuffer, temp_buff,
+	if (status != STATUS_FAILURE) {
+		if (copy_to_user(io_buff.OutputBuffer, temp_buff,
 			sizeof(struct bcm_host_stats_mibs))) {
 			kfree(temp_buff);
 			return -EFAULT;
@@ -1301,90 +1300,90 @@ static int bcm_char_ioctl_get_host_mibs(void __user *argp,
 	}
 
 	kfree(temp_buff);
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_bulk_wrm(void __user *argp,
-	struct bcm_mini_adapter *Adapter, UINT cmd)
+				   struct bcm_mini_adapter *ad, UINT cmd)
 {
-	struct bcm_bulk_wrm_buffer *pBulkBuffer;
-	struct bcm_ioctl_buffer IoBuffer;
-	UINT uiTempVar = 0;
-	INT Status = STATUS_FAILURE;
-	PCHAR pvBuffer = NULL;
+	struct bcm_bulk_wrm_buffer *bulk_buff;
+	struct bcm_ioctl_buffer io_buff;
+	UINT tmp = 0;
+	INT status = STATUS_FAILURE;
+	PCHAR buff = NULL;
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT (Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
 			"Device in Idle/Shutdown Mode, Blocking Wrms\n");
 		return -EACCES;
 	}
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.InputLength < sizeof(ULONG) * 2)
+	if (io_buff.InputLength < sizeof(ULONG) * 2)
 		return -EINVAL;
 
-	pvBuffer = memdup_user(IoBuffer.InputBuffer,
-			       IoBuffer.InputLength);
-	if (IS_ERR(pvBuffer))
-		return PTR_ERR(pvBuffer);
+	buff = memdup_user(io_buff.InputBuffer,
+			       io_buff.InputLength);
+	if (IS_ERR(buff))
+		return PTR_ERR(buff);
 
-	pBulkBuffer = (struct bcm_bulk_wrm_buffer *)pvBuffer;
+	bulk_buff = (struct bcm_bulk_wrm_buffer *)buff;
 
-	if (((ULONG)pBulkBuffer->Register & 0x0F000000) != 0x0F000000 ||
-		((ULONG)pBulkBuffer->Register & 0x3)) {
-		BCM_DEBUG_PRINT (Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (((ULONG)bulk_buff->Register & 0x0F000000) != 0x0F000000 ||
+		((ULONG)bulk_buff->Register & 0x3)) {
+		BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
 			"WRM Done On invalid Address : %x Access Denied.\n",
-			(int)pBulkBuffer->Register);
-		kfree(pvBuffer);
+			(int)bulk_buff->Register);
+		kfree(buff);
 		return -EINVAL;
 	}
 
-	uiTempVar = pBulkBuffer->Register & EEPROM_REJECT_MASK;
-	if (!((Adapter->pstargetparams->m_u32Customize)&VSG_MODE) &&
-		((uiTempVar == EEPROM_REJECT_REG_1) ||
-			(uiTempVar == EEPROM_REJECT_REG_2) ||
-			(uiTempVar == EEPROM_REJECT_REG_3) ||
-			(uiTempVar == EEPROM_REJECT_REG_4)) &&
+	tmp = bulk_buff->Register & EEPROM_REJECT_MASK;
+	if (!((ad->pstargetparams->m_u32Customize)&VSG_MODE) &&
+		((tmp == EEPROM_REJECT_REG_1) ||
+			(tmp == EEPROM_REJECT_REG_2) ||
+			(tmp == EEPROM_REJECT_REG_3) ||
+			(tmp == EEPROM_REJECT_REG_4)) &&
 		(cmd == IOCTL_BCM_REGISTER_WRITE)) {
 
-		kfree(pvBuffer);
-		BCM_DEBUG_PRINT (Adapter, DBG_TYPE_PRINTK, 0, 0,
+		kfree(buff);
+		BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
 			"EEPROM Access Denied, not in VSG Mode\n");
 		return -EFAULT;
 	}
 
-	if (pBulkBuffer->SwapEndian == false)
-		Status = wrmWithLock(Adapter, (UINT)pBulkBuffer->Register,
-			(PCHAR)pBulkBuffer->Values,
-			IoBuffer.InputLength - 2*sizeof(ULONG));
+	if (bulk_buff->SwapEndian == false)
+		status = wrmWithLock(ad, (UINT)bulk_buff->Register,
+			(PCHAR)bulk_buff->Values,
+			io_buff.InputLength - 2*sizeof(ULONG));
 	else
-		Status = wrmaltWithLock(Adapter, (UINT)pBulkBuffer->Register,
-			(PUINT)pBulkBuffer->Values,
-			IoBuffer.InputLength - 2*sizeof(ULONG));
+		status = wrmaltWithLock(ad, (UINT)bulk_buff->Register,
+			(PUINT)bulk_buff->Values,
+			io_buff.InputLength - 2*sizeof(ULONG));
 
-	if (Status != STATUS_SUCCESS)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0, "WRM Failed\n");
+	if (status != STATUS_SUCCESS)
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0, "WRM Failed\n");
 
-	kfree(pvBuffer);
-	return Status;
+	kfree(buff);
+	return status;
 }
 
 static int bcm_char_ioctl_get_nvm_size(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+				       struct bcm_mini_adapter *ad)
 {
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_ioctl_buffer io_buff;
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (Adapter->eNVMType == NVM_EEPROM || Adapter->eNVMType == NVM_FLASH) {
-		if (copy_to_user(IoBuffer.OutputBuffer, &Adapter->uiNVMDSDSize,
+	if (ad->eNVMType == NVM_EEPROM || ad->eNVMType == NVM_FLASH) {
+		if (copy_to_user(io_buff.OutputBuffer, &ad->uiNVMDSDSize,
 			sizeof(UINT)))
 			return -EFAULT;
 	}
@@ -1393,338 +1392,338 @@ static int bcm_char_ioctl_get_nvm_size(void __user *argp,
 }
 
 static int bcm_char_ioctl_cal_init(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+				   struct bcm_mini_adapter *ad)
 {
-	struct bcm_ioctl_buffer IoBuffer;
-	UINT uiSectorSize = 0;
-	INT Status = STATUS_FAILURE;
+	struct bcm_ioctl_buffer io_buff;
+	UINT sector_size = 0;
+	INT status = STATUS_FAILURE;
 
-	if (Adapter->eNVMType == NVM_FLASH) {
-		if (copy_from_user(&IoBuffer, argp,
+	if (ad->eNVMType == NVM_FLASH) {
+		if (copy_from_user(&io_buff, argp,
 			sizeof(struct bcm_ioctl_buffer)))
 			return -EFAULT;
 
-		if (copy_from_user(&uiSectorSize, IoBuffer.InputBuffer,
+		if (copy_from_user(&sector_size, io_buff.InputBuffer,
 			sizeof(UINT)))
 			return -EFAULT;
 
-		if ((uiSectorSize < MIN_SECTOR_SIZE) ||
-			(uiSectorSize > MAX_SECTOR_SIZE)) {
-			if (copy_to_user(IoBuffer.OutputBuffer,
-				&Adapter->uiSectorSize, sizeof(UINT)))
+		if ((sector_size < MIN_SECTOR_SIZE) ||
+			(sector_size > MAX_SECTOR_SIZE)) {
+			if (copy_to_user(io_buff.OutputBuffer,
+				&ad->uiSectorSize, sizeof(UINT)))
 				return -EFAULT;
 		} else {
-			if (IsFlash2x(Adapter)) {
-				if (copy_to_user(IoBuffer.OutputBuffer,
-					&Adapter->uiSectorSize, sizeof(UINT)))
+			if (IsFlash2x(ad)) {
+				if (copy_to_user(io_buff.OutputBuffer,
+					&ad->uiSectorSize, sizeof(UINT)))
 					return -EFAULT;
 			} else {
-				if ((TRUE == Adapter->bShutStatus) ||
-					(TRUE == Adapter->IdleMode)) {
-					BCM_DEBUG_PRINT(Adapter,
+				if ((TRUE == ad->bShutStatus) ||
+					(TRUE == ad->IdleMode)) {
+					BCM_DEBUG_PRINT(ad,
 						DBG_TYPE_PRINTK, 0, 0,
 						"Device is in Idle/Shutdown Mode\n");
 					return -EACCES;
 				}
 
-				Adapter->uiSectorSize = uiSectorSize;
-				BcmUpdateSectorSize(Adapter,
-					Adapter->uiSectorSize);
+				ad->uiSectorSize = sector_size;
+				BcmUpdateSectorSize(ad,
+					ad->uiSectorSize);
 			}
 		}
-		Status = STATUS_SUCCESS;
+		status = STATUS_SUCCESS;
 	} else {
-		Status = STATUS_FAILURE;
+		status = STATUS_FAILURE;
 	}
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_set_debug(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+				    struct bcm_mini_adapter *ad)
 {
 #ifdef DEBUG
-	struct bcm_ioctl_buffer IoBuffer;
-	struct bcm_user_debug_state sUserDebugState;
+	struct bcm_ioctl_buffer io_buff;
+	struct bcm_user_debug_state user_debug_state;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 		"In SET_DEBUG ioctl\n");
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (copy_from_user(&sUserDebugState, IoBuffer.InputBuffer,
+	if (copy_from_user(&user_debug_state, io_buff.InputBuffer,
 		sizeof(struct bcm_user_debug_state)))
 		return -EFAULT;
 
-	BCM_DEBUG_PRINT (Adapter, DBG_TYPE_PRINTK, 0, 0,
+	BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
 			"IOCTL_BCM_SET_DEBUG: OnOff=%d Type = 0x%x ",
-			sUserDebugState.OnOff, sUserDebugState.Type);
-	/* sUserDebugState.Subtype <<= 1; */
-	sUserDebugState.Subtype = 1 << sUserDebugState.Subtype;
-	BCM_DEBUG_PRINT (Adapter, DBG_TYPE_PRINTK, 0, 0,
-		"actual Subtype=0x%x\n", sUserDebugState.Subtype);
-
-	/* Update new 'DebugState' in the Adapter */
-	Adapter->stDebugState.type |= sUserDebugState.Type;
+			user_debug_state.OnOff, user_debug_state.Type);
+	/* user_debug_state.Subtype <<= 1; */
+	user_debug_state.Subtype = 1 << user_debug_state.Subtype;
+	BCM_DEBUG_PRINT (ad, DBG_TYPE_PRINTK, 0, 0,
+		"actual Subtype=0x%x\n", user_debug_state.Subtype);
+
+	/* Update new 'DebugState' in the ad */
+	ad->stDebugState.type |= user_debug_state.Type;
 	/* Subtype: A bitmap of 32 bits for Subtype per Type.
 	 * Valid indexes in 'subtype' array: 1,2,4,8
 	 * corresponding to valid Type values. Hence we can use the 'Type' field
 	 * as the index value, ignoring the array entries 0,3,5,6,7 !
 	 */
-	if (sUserDebugState.OnOff)
-		Adapter->stDebugState.subtype[sUserDebugState.Type] |=
-			sUserDebugState.Subtype;
+	if (user_debug_state.OnOff)
+		ad->stDebugState.subtype[user_debug_state.Type] |=
+			user_debug_state.Subtype;
 	else
-		Adapter->stDebugState.subtype[sUserDebugState.Type] &=
-			~sUserDebugState.Subtype;
+		ad->stDebugState.subtype[user_debug_state.Type] &=
+			~user_debug_state.Subtype;
 
-	BCM_SHOW_DEBUG_BITMAP(Adapter);
+	BCM_SHOW_DEBUG_BITMAP(ad);
 #endif
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_nvm_rw(void __user *argp,
-	struct bcm_mini_adapter *Adapter, UINT cmd)
+				 struct bcm_mini_adapter *ad, UINT cmd)
 {
-	struct bcm_nvm_readwrite stNVMReadWrite;
+	struct bcm_nvm_readwrite nvm_rw;
 	struct timeval tv0, tv1;
-	struct bcm_ioctl_buffer IoBuffer;
-	PUCHAR pReadData = NULL;
-	INT Status = STATUS_FAILURE;
+	struct bcm_ioctl_buffer io_buff;
+	PUCHAR read_data = NULL;
+	INT status = STATUS_FAILURE;
 
 	memset(&tv0, 0, sizeof(struct timeval));
 	memset(&tv1, 0, sizeof(struct timeval));
-	if ((Adapter->eNVMType == NVM_FLASH) &&
-		(Adapter->uiFlashLayoutMajorVersion == 0)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if ((ad->eNVMType == NVM_FLASH) &&
+		(ad->uiFlashLayoutMajorVersion == 0)) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"The Flash Control Section is Corrupted. Hence Rejection on NVM Read/Write\n");
 		return -EFAULT;
 	}
 
-	if (IsFlash2x(Adapter)) {
-		if ((Adapter->eActiveDSD != DSD0) &&
-			(Adapter->eActiveDSD != DSD1) &&
-			(Adapter->eActiveDSD != DSD2)) {
+	if (IsFlash2x(ad)) {
+		if ((ad->eActiveDSD != DSD0) &&
+			(ad->eActiveDSD != DSD1) &&
+			(ad->eActiveDSD != DSD2)) {
 
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"No DSD is active..hence NVM Command is blocked");
 			return STATUS_FAILURE;
 		}
 	}
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (copy_from_user(&stNVMReadWrite,
+	if (copy_from_user(&nvm_rw,
 				(IOCTL_BCM_NVM_READ == cmd) ?
-				IoBuffer.OutputBuffer : IoBuffer.InputBuffer,
+				io_buff.OutputBuffer : io_buff.InputBuffer,
 				sizeof(struct bcm_nvm_readwrite)))
 		return -EFAULT;
 
 	/*
 	 * Deny the access if the offset crosses the cal area limit.
 	 */
-	if (stNVMReadWrite.uiNumBytes > Adapter->uiNVMDSDSize)
+	if (nvm_rw.uiNumBytes > ad->uiNVMDSDSize)
 		return STATUS_FAILURE;
 
-	if (stNVMReadWrite.uiOffset >
-		Adapter->uiNVMDSDSize - stNVMReadWrite.uiNumBytes)
+	if (nvm_rw.uiOffset >
+		ad->uiNVMDSDSize - nvm_rw.uiNumBytes)
 		return STATUS_FAILURE;
 
-	pReadData = memdup_user(stNVMReadWrite.pBuffer,
-				stNVMReadWrite.uiNumBytes);
-	if (IS_ERR(pReadData))
-		return PTR_ERR(pReadData);
+	read_data = memdup_user(nvm_rw.pBuffer,
+				nvm_rw.uiNumBytes);
+	if (IS_ERR(read_data))
+		return PTR_ERR(read_data);
 
 	do_gettimeofday(&tv0);
 	if (IOCTL_BCM_NVM_READ == cmd) {
-		int ret = bcm_handle_nvm_read_cmd(Adapter, pReadData,
-				&stNVMReadWrite);
+		int ret = bcm_handle_nvm_read_cmd(ad, read_data,
+				&nvm_rw);
 		if (ret != STATUS_SUCCESS)
 			return ret;
 	} else {
-		down(&Adapter->NVMRdmWrmLock);
+		down(&ad->NVMRdmWrmLock);
 
-		if ((Adapter->IdleMode == TRUE) ||
-			(Adapter->bShutStatus == TRUE) ||
-			(Adapter->bPreparingForLowPowerMode == TRUE)) {
+		if ((ad->IdleMode == TRUE) ||
+			(ad->bShutStatus == TRUE) ||
+			(ad->bPreparingForLowPowerMode == TRUE)) {
 
-			BCM_DEBUG_PRINT(Adapter,
+			BCM_DEBUG_PRINT(ad,
 				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Device is in Idle/Shutdown Mode\n");
-			up(&Adapter->NVMRdmWrmLock);
-			kfree(pReadData);
+			up(&ad->NVMRdmWrmLock);
+			kfree(read_data);
 			return -EACCES;
 		}
 
-		Adapter->bHeaderChangeAllowed = TRUE;
-		if (IsFlash2x(Adapter)) {
-			int ret = handle_flash2x_adapter(Adapter,
-							pReadData,
-							&stNVMReadWrite);
+		ad->bHeaderChangeAllowed = TRUE;
+		if (IsFlash2x(ad)) {
+			int ret = handle_flash2x_adapter(ad,
+							read_data,
+							&nvm_rw);
 			if (ret != STATUS_SUCCESS)
 				return ret;
 		}
 
-		Status = BeceemNVMWrite(Adapter, (PUINT)pReadData,
-			stNVMReadWrite.uiOffset, stNVMReadWrite.uiNumBytes,
-			stNVMReadWrite.bVerify);
-		if (IsFlash2x(Adapter))
-			BcmFlash2xWriteSig(Adapter, Adapter->eActiveDSD);
+		status = BeceemNVMWrite(ad, (PUINT)read_data,
+			nvm_rw.uiOffset, nvm_rw.uiNumBytes,
+			nvm_rw.bVerify);
+		if (IsFlash2x(ad))
+			BcmFlash2xWriteSig(ad, ad->eActiveDSD);
 
-		Adapter->bHeaderChangeAllowed = false;
+		ad->bHeaderChangeAllowed = false;
 
-		up(&Adapter->NVMRdmWrmLock);
+		up(&ad->NVMRdmWrmLock);
 
-		if (Status != STATUS_SUCCESS) {
-			kfree(pReadData);
-			return Status;
+		if (status != STATUS_SUCCESS) {
+			kfree(read_data);
+			return status;
 		}
 	}
 
 	do_gettimeofday(&tv1);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 		" timetaken by Write/read :%ld msec\n",
 		(tv1.tv_sec - tv0.tv_sec)*1000 +
 		(tv1.tv_usec - tv0.tv_usec)/1000);
 
-	kfree(pReadData);
+	kfree(read_data);
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_flash2x_section_read(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+	struct bcm_mini_adapter *ad)
 {
-	struct bcm_flash2x_readwrite sFlash2xRead = {0};
-	struct bcm_ioctl_buffer IoBuffer;
-	PUCHAR pReadBuff = NULL;
-	UINT NOB = 0;
-	UINT BuffSize = 0;
-	UINT ReadBytes = 0;
-	UINT ReadOffset = 0;
-	INT Status = STATUS_FAILURE;
+	struct bcm_flash2x_readwrite flash_2x_read = {0};
+	struct bcm_ioctl_buffer io_buff;
+	PUCHAR read_buff = NULL;
+	UINT nob = 0;
+	UINT buff_size = 0;
+	UINT read_bytes = 0;
+	UINT read_offset = 0;
+	INT status = STATUS_FAILURE;
 	void __user *OutPutBuff;
 
-	if (IsFlash2x(Adapter) != TRUE)	{
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (IsFlash2x(ad) != TRUE)	{
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 		DBG_LVL_ALL, "IOCTL_BCM_FLASH2X_SECTION_READ Called");
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
 	/* Reading FLASH 2.x READ structure */
-	if (copy_from_user(&sFlash2xRead, IoBuffer.InputBuffer,
+	if (copy_from_user(&flash_2x_read, io_buff.InputBuffer,
 		sizeof(struct bcm_flash2x_readwrite)))
 		return -EFAULT;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nsFlash2xRead.Section :%x",
-			sFlash2xRead.Section);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nsFlash2xRead.offset :%x",
-			sFlash2xRead.offset);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nsFlash2xRead.numOfBytes :%x",
-			sFlash2xRead.numOfBytes);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"\nsFlash2xRead.bVerify :%x\n",
-			sFlash2xRead.bVerify);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"\nflash_2x_read.Section :%x",
+			flash_2x_read.Section);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"\nflash_2x_read.offset :%x",
+			flash_2x_read.offset);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"\nflash_2x_read.numOfBytes :%x",
+			flash_2x_read.numOfBytes);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"\nflash_2x_read.bVerify :%x\n",
+			flash_2x_read.bVerify);
 
 	/* This was internal to driver for raw read.
 	 * now it has ben exposed to user space app.
 	 */
-	if (validateFlash2xReadWrite(Adapter, &sFlash2xRead) == false)
+	if (validateFlash2xReadWrite(ad, &flash_2x_read) == false)
 		return STATUS_FAILURE;
 
-	NOB = sFlash2xRead.numOfBytes;
-	if (NOB > Adapter->uiSectorSize)
-		BuffSize = Adapter->uiSectorSize;
+	nob = flash_2x_read.numOfBytes;
+	if (nob > ad->uiSectorSize)
+		buff_size = ad->uiSectorSize;
 	else
-		BuffSize = NOB;
+		buff_size = nob;
 
-	ReadOffset = sFlash2xRead.offset;
-	OutPutBuff = IoBuffer.OutputBuffer;
-	pReadBuff = kzalloc(BuffSize , GFP_KERNEL);
+	read_offset = flash_2x_read.offset;
+	OutPutBuff = io_buff.OutputBuffer;
+	read_buff = kzalloc(buff_size , GFP_KERNEL);
 
-	if (pReadBuff == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (read_buff == NULL) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Memory allocation failed for Flash 2.x Read Structure");
 		return -ENOMEM;
 	}
-	down(&Adapter->NVMRdmWrmLock);
+	down(&ad->NVMRdmWrmLock);
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG,
 				DBG_LVL_ALL,
 				"Device is in Idle/Shutdown Mode\n");
-		up(&Adapter->NVMRdmWrmLock);
-		kfree(pReadBuff);
+		up(&ad->NVMRdmWrmLock);
+		kfree(read_buff);
 		return -EACCES;
 	}
 
-	while (NOB) {
-		if (NOB > Adapter->uiSectorSize)
-			ReadBytes = Adapter->uiSectorSize;
+	while (nob) {
+		if (nob > ad->uiSectorSize)
+			read_bytes = ad->uiSectorSize;
 		else
-			ReadBytes = NOB;
+			read_bytes = nob;
 
 		/* Reading the data from Flash 2.x */
-		Status = BcmFlash2xBulkRead(Adapter, (PUINT)pReadBuff,
-			sFlash2xRead.Section, ReadOffset, ReadBytes);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter,
+		status = BcmFlash2xBulkRead(ad, (PUINT)read_buff,
+			flash_2x_read.Section, read_offset, read_bytes);
+		if (status) {
+			BCM_DEBUG_PRINT(ad,
 				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Flash 2x read err with Status :%d",
-				Status);
+				"Flash 2x read err with status :%d",
+				status);
 			break;
 		}
 
-		BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-			DBG_LVL_ALL, pReadBuff, ReadBytes);
+		BCM_DEBUG_PRINT_BUFFER(ad, DBG_TYPE_OTHERS, OSAL_DBG,
+			DBG_LVL_ALL, read_buff, read_bytes);
 
-		Status = copy_to_user(OutPutBuff, pReadBuff, ReadBytes);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter,
+		status = copy_to_user(OutPutBuff, read_buff, read_bytes);
+		if (status) {
+			BCM_DEBUG_PRINT(ad,
 				DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-				"Copy to use failed with status :%d", Status);
-			up(&Adapter->NVMRdmWrmLock);
-			kfree(pReadBuff);
+				"Copy to use failed with status :%d", status);
+			up(&ad->NVMRdmWrmLock);
+			kfree(read_buff);
 			return -EFAULT;
 		}
-		NOB = NOB - ReadBytes;
-		if (NOB) {
-			ReadOffset = ReadOffset + ReadBytes;
-			OutPutBuff = OutPutBuff + ReadBytes;
+		nob = nob - read_bytes;
+		if (nob) {
+			read_offset = read_offset + read_bytes;
+			OutPutBuff = OutPutBuff + read_bytes;
 		}
 	}
 
-	up(&Adapter->NVMRdmWrmLock);
-	kfree(pReadBuff);
-	return Status;
+	up(&ad->NVMRdmWrmLock);
+	kfree(read_buff);
+	return status;
 }
 
 static int bcm_char_ioctl_flash2x_section_write(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+	struct bcm_mini_adapter *ad)
 {
 	struct bcm_flash2x_readwrite sFlash2xWrite = {0};
-	struct bcm_ioctl_buffer IoBuffer;
-	PUCHAR pWriteBuff;
-	void __user *InputAddr;
-	UINT NOB = 0;
-	UINT BuffSize = 0;
-	UINT WriteOffset = 0;
-	UINT WriteBytes = 0;
-	INT Status = STATUS_FAILURE;
-
-	if (IsFlash2x(Adapter) != TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	struct bcm_ioctl_buffer io_buff;
+	PUCHAR write_buff;
+	void __user *input_addr;
+	UINT nob = 0;
+	UINT buff_size = 0;
+	UINT write_off = 0;
+	UINT write_bytes = 0;
+	INT status = STATUS_FAILURE;
+
+	if (IsFlash2x(ad) != TRUE) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
@@ -1732,609 +1731,610 @@ static int bcm_char_ioctl_flash2x_section_write(void __user *argp,
 	/* First make this False so that we can enable the Sector
 	 * Permission Check in BeceemFlashBulkWrite
 	 */
-	Adapter->bAllDSDWriteAllow = false;
+	ad->bAllDSDWriteAllow = false;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 		"IOCTL_BCM_FLASH2X_SECTION_WRITE Called");
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
 	/* Reading FLASH 2.x READ structure */
-	if (copy_from_user(&sFlash2xWrite, IoBuffer.InputBuffer,
+	if (copy_from_user(&sFlash2xWrite, io_buff.InputBuffer,
 		sizeof(struct bcm_flash2x_readwrite)))
 		return -EFAULT;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.Section :%x", sFlash2xWrite.Section);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.offset :%d", sFlash2xWrite.offset);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.numOfBytes :%x", sFlash2xWrite.numOfBytes);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-		"\nsFlash2xRead.bVerify :%x\n", sFlash2xWrite.bVerify);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		"\nsFlash2xWrite.Section :%x", sFlash2xWrite.Section);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		"\nsFlash2xWrite.offset :%d", sFlash2xWrite.offset);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		"\nsFlash2xWrite.numOfBytes :%x", sFlash2xWrite.numOfBytes);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		"\nsFlash2xWrite.bVerify :%x\n", sFlash2xWrite.bVerify);
 
 	if ((sFlash2xWrite.Section != VSA0) && (sFlash2xWrite.Section != VSA1)
 		&& (sFlash2xWrite.Section != VSA2)) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"Only VSA write is allowed");
 		return -EINVAL;
 	}
 
-	if (validateFlash2xReadWrite(Adapter, &sFlash2xWrite) == false)
+	if (validateFlash2xReadWrite(ad, &sFlash2xWrite) == false)
 		return STATUS_FAILURE;
 
-	InputAddr = sFlash2xWrite.pDataBuff;
-	WriteOffset = sFlash2xWrite.offset;
-	NOB = sFlash2xWrite.numOfBytes;
+	input_addr = sFlash2xWrite.pDataBuff;
+	write_off = sFlash2xWrite.offset;
+	nob = sFlash2xWrite.numOfBytes;
 
-	if (NOB > Adapter->uiSectorSize)
-		BuffSize = Adapter->uiSectorSize;
+	if (nob > ad->uiSectorSize)
+		buff_size = ad->uiSectorSize;
 	else
-		BuffSize = NOB;
+		buff_size = nob;
 
-	pWriteBuff = kmalloc(BuffSize, GFP_KERNEL);
+	write_buff = kmalloc(buff_size, GFP_KERNEL);
 
-	if (pWriteBuff == NULL)
+	if (write_buff == NULL)
 		return -ENOMEM;
 
 	/* extracting the remainder of the given offset. */
-	WriteBytes = Adapter->uiSectorSize;
-	if (WriteOffset % Adapter->uiSectorSize) {
-		WriteBytes = Adapter->uiSectorSize -
-			(WriteOffset % Adapter->uiSectorSize);
+	write_bytes = ad->uiSectorSize;
+	if (write_off % ad->uiSectorSize) {
+		write_bytes = ad->uiSectorSize -
+			(write_off % ad->uiSectorSize);
 	}
 
-	if (NOB < WriteBytes)
-		WriteBytes = NOB;
+	if (nob < write_bytes)
+		write_bytes = nob;
 
-	down(&Adapter->NVMRdmWrmLock);
+	down(&ad->NVMRdmWrmLock);
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"Device is in Idle/Shutdown Mode\n");
-		up(&Adapter->NVMRdmWrmLock);
-		kfree(pWriteBuff);
+		up(&ad->NVMRdmWrmLock);
+		kfree(write_buff);
 		return -EACCES;
 	}
 
-	BcmFlash2xCorruptSig(Adapter, sFlash2xWrite.Section);
+	BcmFlash2xCorruptSig(ad, sFlash2xWrite.Section);
 	do {
-		Status = copy_from_user(pWriteBuff, InputAddr, WriteBytes);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Copy to user failed with status :%d", Status);
-			up(&Adapter->NVMRdmWrmLock);
-			kfree(pWriteBuff);
+		status = copy_from_user(write_buff, input_addr, write_bytes);
+		if (status) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+				"Copy to user failed with status :%d", status);
+			up(&ad->NVMRdmWrmLock);
+			kfree(write_buff);
 			return -EFAULT;
 		}
-		BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS,
-			OSAL_DBG, DBG_LVL_ALL, pWriteBuff, WriteBytes);
+		BCM_DEBUG_PRINT_BUFFER(ad, DBG_TYPE_OTHERS,
+			OSAL_DBG, DBG_LVL_ALL, write_buff, write_bytes);
 
 		/* Writing the data from Flash 2.x */
-		Status = BcmFlash2xBulkWrite(Adapter, (PUINT)pWriteBuff,
+		status = BcmFlash2xBulkWrite(ad, (PUINT)write_buff,
 					     sFlash2xWrite.Section,
-					     WriteOffset,
-					     WriteBytes,
+					     write_off,
+					     write_bytes,
 					     sFlash2xWrite.bVerify);
 
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Flash 2x read err with Status :%d", Status);
+		if (status) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+				"Flash 2x read err with status :%d", status);
 			break;
 		}
 
-		NOB = NOB - WriteBytes;
-		if (NOB) {
-			WriteOffset = WriteOffset + WriteBytes;
-			InputAddr = InputAddr + WriteBytes;
-			if (NOB > Adapter->uiSectorSize)
-				WriteBytes = Adapter->uiSectorSize;
+		nob = nob - write_bytes;
+		if (nob) {
+			write_off = write_off + write_bytes;
+			input_addr = input_addr + write_bytes;
+			if (nob > ad->uiSectorSize)
+				write_bytes = ad->uiSectorSize;
 			else
-				WriteBytes = NOB;
+				write_bytes = nob;
 		}
-	} while (NOB > 0);
+	} while (nob > 0);
 
-	BcmFlash2xWriteSig(Adapter, sFlash2xWrite.Section);
-	up(&Adapter->NVMRdmWrmLock);
-	kfree(pWriteBuff);
-	return Status;
+	BcmFlash2xWriteSig(ad, sFlash2xWrite.Section);
+	up(&ad->NVMRdmWrmLock);
+	kfree(write_buff);
+	return status;
 }
 
 static int bcm_char_ioctl_flash2x_section_bitmap(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+	struct bcm_mini_adapter *ad)
 {
-	struct bcm_flash2x_bitmap *psFlash2xBitMap;
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_flash2x_bitmap *flash_2x_bit_map;
+	struct bcm_ioctl_buffer io_buff;
 
-BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 	"IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP Called");
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.OutputLength != sizeof(struct bcm_flash2x_bitmap))
+	if (io_buff.OutputLength != sizeof(struct bcm_flash2x_bitmap))
 		return -EINVAL;
 
-	psFlash2xBitMap = kzalloc(sizeof(struct bcm_flash2x_bitmap),
+	flash_2x_bit_map = kzalloc(sizeof(struct bcm_flash2x_bitmap),
 			GFP_KERNEL);
 
-	if (psFlash2xBitMap == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (flash_2x_bit_map == NULL) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Memory is not available");
 		return -ENOMEM;
 	}
 
 	/* Reading the Flash Sectio Bit map */
-	down(&Adapter->NVMRdmWrmLock);
+	down(&ad->NVMRdmWrmLock);
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"Device is in Idle/Shutdown Mode\n");
-		up(&Adapter->NVMRdmWrmLock);
-		kfree(psFlash2xBitMap);
+		up(&ad->NVMRdmWrmLock);
+		kfree(flash_2x_bit_map);
 		return -EACCES;
 	}
 
-	BcmGetFlash2xSectionalBitMap(Adapter, psFlash2xBitMap);
-	up(&Adapter->NVMRdmWrmLock);
-	if (copy_to_user(IoBuffer.OutputBuffer, psFlash2xBitMap,
+	BcmGetFlash2xSectionalBitMap(ad, flash_2x_bit_map);
+	up(&ad->NVMRdmWrmLock);
+	if (copy_to_user(io_buff.OutputBuffer, flash_2x_bit_map,
 		sizeof(struct bcm_flash2x_bitmap))) {
-		kfree(psFlash2xBitMap);
+		kfree(flash_2x_bit_map);
 		return -EFAULT;
 	}
 
-	kfree(psFlash2xBitMap);
+	kfree(flash_2x_bit_map);
 	return STATUS_FAILURE;
 }
 
 static int bcm_char_ioctl_set_active_section(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					     struct bcm_mini_adapter *ad)
 {
-	enum bcm_flash2x_section_val eFlash2xSectionVal = 0;
-	INT Status = STATUS_FAILURE;
-	struct bcm_ioctl_buffer IoBuffer;
+	enum bcm_flash2x_section_val flash_2x_section_val = 0;
+	INT status = STATUS_FAILURE;
+	struct bcm_ioctl_buffer io_buff;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"IOCTL_BCM_SET_ACTIVE_SECTION Called");
 
-	if (IsFlash2x(Adapter) != TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (IsFlash2x(ad) != TRUE) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp,
+	status = copy_from_user(&io_buff, argp,
 				sizeof(struct bcm_ioctl_buffer));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
 
-	Status = copy_from_user(&eFlash2xSectionVal,
-				IoBuffer.InputBuffer, sizeof(INT));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	status = copy_from_user(&flash_2x_section_val,
+				io_buff.InputBuffer, sizeof(INT));
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 			"Copy of flash section val failed");
 		return -EFAULT;
 	}
 
-	down(&Adapter->NVMRdmWrmLock);
+	down(&ad->NVMRdmWrmLock);
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Device is in Idle/Shutdown Mode\n");
-		up(&Adapter->NVMRdmWrmLock);
+		up(&ad->NVMRdmWrmLock);
 		return -EACCES;
 	}
 
-	Status = BcmSetActiveSection(Adapter, eFlash2xSectionVal);
-	if (Status)
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Failed to make it's priority Highest. Status %d",
-				Status);
+	status = BcmSetActiveSection(ad, flash_2x_section_val);
+	if (status)
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+				"Failed to make it's priority Highest. status %d",
+				status);
 
-	up(&Adapter->NVMRdmWrmLock);
+	up(&ad->NVMRdmWrmLock);
 
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_copy_section(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+				       struct bcm_mini_adapter *ad)
 {
-	struct bcm_flash2x_copy_section sCopySectStrut = {0};
-	struct bcm_ioctl_buffer IoBuffer;
-	INT Status = STATUS_SUCCESS;
+	struct bcm_flash2x_copy_section copy_sect_strut = {0};
+	struct bcm_ioctl_buffer io_buff;
+	INT status = STATUS_SUCCESS;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"IOCTL_BCM_COPY_SECTION  Called");
 
-	Adapter->bAllDSDWriteAllow = false;
-	if (IsFlash2x(Adapter) != TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	ad->bAllDSDWriteAllow = false;
+	if (IsFlash2x(ad) != TRUE) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp,
+	status = copy_from_user(&io_buff, argp,
 				sizeof(struct bcm_ioctl_buffer));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of IOCTL BUFFER failed Status :%d",
-				Status);
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+				"Copy of IOCTL BUFFER failed status :%d",
+				status);
 		return -EFAULT;
 	}
 
-	Status = copy_from_user(&sCopySectStrut, IoBuffer.InputBuffer,
+	status = copy_from_user(&copy_sect_strut, io_buff.InputBuffer,
 				sizeof(struct bcm_flash2x_copy_section));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-				"Copy of Copy_Section_Struct failed with Status :%d",
-				Status);
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+				"Copy of Copy_Section_Struct failed with status :%d",
+				status);
 		return -EFAULT;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Source SEction :%x", sCopySectStrut.SrcSection);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Destination SEction :%x", sCopySectStrut.DstSection);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"offset :%x", sCopySectStrut.offset);
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"NOB :%x", sCopySectStrut.numOfBytes);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"Source SEction :%x", copy_sect_strut.SrcSection);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"Destination SEction :%x", copy_sect_strut.DstSection);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"offset :%x", copy_sect_strut.offset);
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"nob :%x", copy_sect_strut.numOfBytes);
 
-	if (IsSectionExistInFlash(Adapter, sCopySectStrut.SrcSection) == false) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (IsSectionExistInFlash(ad, copy_sect_strut.SrcSection) == false) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Source Section<%x> does not exist in Flash ",
-				sCopySectStrut.SrcSection);
+				copy_sect_strut.SrcSection);
 		return -EINVAL;
 	}
 
-	if (IsSectionExistInFlash(Adapter, sCopySectStrut.DstSection) == false) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (IsSectionExistInFlash(ad, copy_sect_strut.DstSection) == false) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Destinatio Section<%x> does not exist in Flash ",
-				sCopySectStrut.DstSection);
+				copy_sect_strut.DstSection);
 		return -EINVAL;
 	}
 
-	if (sCopySectStrut.SrcSection == sCopySectStrut.DstSection) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	if (copy_sect_strut.SrcSection == copy_sect_strut.DstSection) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Source and Destination section should be different");
 		return -EINVAL;
 	}
 
-	down(&Adapter->NVMRdmWrmLock);
+	down(&ad->NVMRdmWrmLock);
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Device is in Idle/Shutdown Mode\n");
-		up(&Adapter->NVMRdmWrmLock);
+		up(&ad->NVMRdmWrmLock);
 		return -EACCES;
 	}
 
-	if (sCopySectStrut.SrcSection == ISO_IMAGE1 ||
-		sCopySectStrut.SrcSection == ISO_IMAGE2) {
-		if (IsNonCDLessDevice(Adapter)) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (copy_sect_strut.SrcSection == ISO_IMAGE1 ||
+		copy_sect_strut.SrcSection == ISO_IMAGE2) {
+		if (IsNonCDLessDevice(ad)) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"Device is Non-CDLess hence won't have ISO !!");
-			Status = -EINVAL;
-		} else if (sCopySectStrut.numOfBytes == 0) {
-			Status = BcmCopyISO(Adapter, sCopySectStrut);
+			status = -EINVAL;
+		} else if (copy_sect_strut.numOfBytes == 0) {
+			status = BcmCopyISO(ad, copy_sect_strut);
 		} else {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"Partial Copy of ISO section is not Allowed..");
-			Status = STATUS_FAILURE;
+			status = STATUS_FAILURE;
 		}
-		up(&Adapter->NVMRdmWrmLock);
-		return Status;
+		up(&ad->NVMRdmWrmLock);
+		return status;
 	}
 
-	Status = BcmCopySection(Adapter, sCopySectStrut.SrcSection,
-				sCopySectStrut.DstSection,
-				sCopySectStrut.offset,
-				sCopySectStrut.numOfBytes);
-	up(&Adapter->NVMRdmWrmLock);
-	return Status;
+	status = BcmCopySection(ad, copy_sect_strut.SrcSection,
+				copy_sect_strut.DstSection,
+				copy_sect_strut.offset,
+				copy_sect_strut.numOfBytes);
+	up(&ad->NVMRdmWrmLock);
+	return status;
 }
 
 static int bcm_char_ioctl_get_flash_cs_info(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+					    struct bcm_mini_adapter *ad)
 {
-	struct bcm_ioctl_buffer IoBuffer;
-	INT Status = STATUS_SUCCESS;
+	struct bcm_ioctl_buffer io_buff;
+	INT status = STATUS_SUCCESS;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			" IOCTL_BCM_GET_FLASH_CS_INFO Called");
 
-	Status = copy_from_user(&IoBuffer, argp,
+	status = copy_from_user(&io_buff, argp,
 			sizeof(struct bcm_ioctl_buffer));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
 
-	if (Adapter->eNVMType != NVM_FLASH) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (ad->eNVMType != NVM_FLASH) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Connected device does not have flash");
 		return -EINVAL;
 	}
 
-	if (IsFlash2x(Adapter) == TRUE) {
-		if (IoBuffer.OutputLength < sizeof(struct bcm_flash2x_cs_info))
+	if (IsFlash2x(ad) == TRUE) {
+		if (io_buff.OutputLength < sizeof(struct bcm_flash2x_cs_info))
 			return -EINVAL;
 
-		if (copy_to_user(IoBuffer.OutputBuffer,
-				 Adapter->psFlash2xCSInfo,
+		if (copy_to_user(io_buff.OutputBuffer,
+				 ad->psFlash2xCSInfo,
 				 sizeof(struct bcm_flash2x_cs_info)))
 			return -EFAULT;
 	} else {
-		if (IoBuffer.OutputLength < sizeof(struct bcm_flash_cs_info))
+		if (io_buff.OutputLength < sizeof(struct bcm_flash_cs_info))
 			return -EINVAL;
 
-		if (copy_to_user(IoBuffer.OutputBuffer, Adapter->psFlashCSInfo,
+		if (copy_to_user(io_buff.OutputBuffer, ad->psFlashCSInfo,
 				 sizeof(struct bcm_flash_cs_info)))
 			return -EFAULT;
 	}
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_select_dsd(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+				     struct bcm_mini_adapter *ad)
 {
-	struct bcm_ioctl_buffer IoBuffer;
-	INT Status = STATUS_FAILURE;
-	UINT SectOfset = 0;
-	enum bcm_flash2x_section_val eFlash2xSectionVal;
+	struct bcm_ioctl_buffer io_buff;
+	INT status = STATUS_FAILURE;
+	UINT sect_offset = 0;
+	enum bcm_flash2x_section_val flash_2x_section_val;
 
-	eFlash2xSectionVal = NO_SECTION_VAL;
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	flash_2x_section_val = NO_SECTION_VAL;
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"IOCTL_BCM_SELECT_DSD Called");
 
-	if (IsFlash2x(Adapter) != TRUE) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (IsFlash2x(ad) != TRUE) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Flash Does not have 2.x map");
 		return -EINVAL;
 	}
 
-	Status = copy_from_user(&IoBuffer, argp,
+	status = copy_from_user(&io_buff, argp,
 				sizeof(struct bcm_ioctl_buffer));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Copy of IOCTL BUFFER failed");
 		return -EFAULT;
 	}
-	Status = copy_from_user(&eFlash2xSectionVal, IoBuffer.InputBuffer,
+	status = copy_from_user(&flash_2x_section_val, io_buff.InputBuffer,
 		sizeof(INT));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Copy of flash section val failed");
 		return -EFAULT;
 	}
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
-			"Read Section :%d", eFlash2xSectionVal);
-	if ((eFlash2xSectionVal != DSD0) &&
-		(eFlash2xSectionVal != DSD1) &&
-		(eFlash2xSectionVal != DSD2)) {
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+			"Read Section :%d", flash_2x_section_val);
+	if ((flash_2x_section_val != DSD0) &&
+		(flash_2x_section_val != DSD1) &&
+		(flash_2x_section_val != DSD2)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Passed section<%x> is not DSD section",
-				eFlash2xSectionVal);
+				flash_2x_section_val);
 		return STATUS_FAILURE;
 	}
 
-	SectOfset = BcmGetSectionValStartOffset(Adapter, eFlash2xSectionVal);
-	if (SectOfset == INVALID_OFFSET) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	sect_offset = BcmGetSectionValStartOffset(ad, flash_2x_section_val);
+	if (sect_offset == INVALID_OFFSET) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Provided Section val <%d> does not exist in Flash 2.x",
-				eFlash2xSectionVal);
+				flash_2x_section_val);
 		return -EINVAL;
 	}
 
-	Adapter->bAllDSDWriteAllow = TRUE;
-	Adapter->ulFlashCalStart = SectOfset;
-	Adapter->eActiveDSD = eFlash2xSectionVal;
+	ad->bAllDSDWriteAllow = TRUE;
+	ad->ulFlashCalStart = sect_offset;
+	ad->eActiveDSD = flash_2x_section_val;
 
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_nvm_raw_read(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+				       struct bcm_mini_adapter *ad)
 {
-	struct bcm_nvm_readwrite stNVMRead;
-	struct bcm_ioctl_buffer IoBuffer;
-	unsigned int NOB;
-	INT BuffSize;
-	INT ReadOffset = 0;
-	UINT ReadBytes = 0;
-	PUCHAR pReadBuff;
+	struct bcm_nvm_readwrite nvm_read;
+	struct bcm_ioctl_buffer io_buff;
+	unsigned int nob;
+	INT buff_size;
+	INT read_offset = 0;
+	UINT read_bytes = 0;
+	PUCHAR read_buff;
 	void __user *OutPutBuff;
-	INT Status = STATUS_FAILURE;
+	INT status = STATUS_FAILURE;
 
-	if (Adapter->eNVMType != NVM_FLASH) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (ad->eNVMType != NVM_FLASH) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"NVM TYPE is not Flash");
 		return -EINVAL;
 	}
 
 	/* Copy Ioctl Buffer structure */
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer))) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer))) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"copy_from_user 1 failed\n");
 		return -EFAULT;
 	}
 
-	if (copy_from_user(&stNVMRead, IoBuffer.OutputBuffer,
+	if (copy_from_user(&nvm_read, io_buff.OutputBuffer,
 		sizeof(struct bcm_nvm_readwrite)))
 		return -EFAULT;
 
-	NOB = stNVMRead.uiNumBytes;
+	nob = nvm_read.uiNumBytes;
 	/* In Raw-Read max Buff size : 64MB */
 
-	if (NOB > DEFAULT_BUFF_SIZE)
-		BuffSize = DEFAULT_BUFF_SIZE;
+	if (nob > DEFAULT_BUFF_SIZE)
+		buff_size = DEFAULT_BUFF_SIZE;
 	else
-		BuffSize = NOB;
+		buff_size = nob;
 
-	ReadOffset = stNVMRead.uiOffset;
-	OutPutBuff = stNVMRead.pBuffer;
+	read_offset = nvm_read.uiOffset;
+	OutPutBuff = nvm_read.pBuffer;
 
-	pReadBuff = kzalloc(BuffSize , GFP_KERNEL);
-	if (pReadBuff == NULL) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+	read_buff = kzalloc(buff_size , GFP_KERNEL);
+	if (read_buff == NULL) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 				"Memory allocation failed for Flash 2.x Read Structure");
 		return -ENOMEM;
 	}
-	down(&Adapter->NVMRdmWrmLock);
+	down(&ad->NVMRdmWrmLock);
 
-	if ((Adapter->IdleMode == TRUE) ||
-		(Adapter->bShutStatus == TRUE) ||
-		(Adapter->bPreparingForLowPowerMode == TRUE)) {
+	if ((ad->IdleMode == TRUE) ||
+		(ad->bShutStatus == TRUE) ||
+		(ad->bPreparingForLowPowerMode == TRUE)) {
 
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"Device is in Idle/Shutdown Mode\n");
-		kfree(pReadBuff);
-		up(&Adapter->NVMRdmWrmLock);
+		kfree(read_buff);
+		up(&ad->NVMRdmWrmLock);
 		return -EACCES;
 	}
 
-	Adapter->bFlashRawRead = TRUE;
+	ad->bFlashRawRead = TRUE;
 
-	while (NOB) {
-		if (NOB > DEFAULT_BUFF_SIZE)
-			ReadBytes = DEFAULT_BUFF_SIZE;
+	while (nob) {
+		if (nob > DEFAULT_BUFF_SIZE)
+			read_bytes = DEFAULT_BUFF_SIZE;
 		else
-			ReadBytes = NOB;
+			read_bytes = nob;
 
 		/* Reading the data from Flash 2.x */
-		Status = BeceemNVMRead(Adapter, (PUINT)pReadBuff,
-			ReadOffset, ReadBytes);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
-					"Flash 2x read err with Status :%d",
-					Status);
+		status = BeceemNVMRead(ad, (PUINT)read_buff,
+			read_offset, read_bytes);
+		if (status) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
+					"Flash 2x read err with status :%d",
+					status);
 			break;
 		}
 
-		BCM_DEBUG_PRINT_BUFFER(Adapter, DBG_TYPE_OTHERS, OSAL_DBG,
-				       DBG_LVL_ALL, pReadBuff, ReadBytes);
+		BCM_DEBUG_PRINT_BUFFER(ad, DBG_TYPE_OTHERS, OSAL_DBG,
+				       DBG_LVL_ALL, read_buff, read_bytes);
 
-		Status = copy_to_user(OutPutBuff, pReadBuff, ReadBytes);
-		if (Status) {
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_PRINTK, 0, 0,
+		status = copy_to_user(OutPutBuff, read_buff, read_bytes);
+		if (status) {
+			BCM_DEBUG_PRINT(ad, DBG_TYPE_PRINTK, 0, 0,
 					"Copy to use failed with status :%d",
-					Status);
-			up(&Adapter->NVMRdmWrmLock);
-			kfree(pReadBuff);
+					status);
+			up(&ad->NVMRdmWrmLock);
+			kfree(read_buff);
 			return -EFAULT;
 		}
-		NOB = NOB - ReadBytes;
-		if (NOB) {
-			ReadOffset = ReadOffset + ReadBytes;
-			OutPutBuff = OutPutBuff + ReadBytes;
+		nob = nob - read_bytes;
+		if (nob) {
+			read_offset = read_offset + read_bytes;
+			OutPutBuff = OutPutBuff + read_bytes;
 		}
 	}
-	Adapter->bFlashRawRead = false;
-	up(&Adapter->NVMRdmWrmLock);
-	kfree(pReadBuff);
-	return Status;
+	ad->bFlashRawRead = false;
+	up(&ad->NVMRdmWrmLock);
+	kfree(read_buff);
+	return status;
 }
 
 static int bcm_char_ioctl_cntrlmsg_mask(void __user *argp,
-	struct bcm_mini_adapter *Adapter, struct bcm_tarang_data *pTarang)
+					struct bcm_mini_adapter *ad,
+					struct bcm_tarang_data *tarang)
 {
-	struct bcm_ioctl_buffer IoBuffer;
-	INT Status = STATUS_FAILURE;
-	ULONG RxCntrlMsgBitMask = 0;
+	struct bcm_ioctl_buffer io_buff;
+	INT status = STATUS_FAILURE;
+	ULONG rx_cntrl_msg_bit_mask = 0;
 
 	/* Copy Ioctl Buffer structure */
-	Status = copy_from_user(&IoBuffer, argp,
+	status = copy_from_user(&io_buff, argp,
 			sizeof(struct bcm_ioctl_buffer));
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"copy of Ioctl buffer is failed from user space");
 		return -EFAULT;
 	}
 
-	if (IoBuffer.InputLength != sizeof(unsigned long))
+	if (io_buff.InputLength != sizeof(unsigned long))
 		return -EINVAL;
 
-	Status = copy_from_user(&RxCntrlMsgBitMask, IoBuffer.InputBuffer,
-				IoBuffer.InputLength);
-	if (Status) {
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	status = copy_from_user(&rx_cntrl_msg_bit_mask, io_buff.InputBuffer,
+				io_buff.InputLength);
+	if (status) {
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"copy of control bit mask failed from user space");
 		return -EFAULT;
 	}
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"\n Got user defined cntrl msg bit mask :%lx",
-			RxCntrlMsgBitMask);
-	pTarang->RxCntrlMsgBitMask = RxCntrlMsgBitMask;
+			rx_cntrl_msg_bit_mask);
+	tarang->RxCntrlMsgBitMask = rx_cntrl_msg_bit_mask;
 
-	return Status;
+	return status;
 }
 
 static int bcm_char_ioctl_get_device_driver_info(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+	struct bcm_mini_adapter *ad)
 {
-	struct bcm_driver_info DevInfo;
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_driver_info dev_info;
+	struct bcm_ioctl_buffer io_buff;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"Called IOCTL_BCM_GET_DEVICE_DRIVER_INFO\n");
 
-	memset(&DevInfo, 0, sizeof(DevInfo));
-	DevInfo.MaxRDMBufferSize = BUFFER_4K;
-	DevInfo.u32DSDStartOffset = EEPROM_CALPARAM_START;
-	DevInfo.u32RxAlignmentCorrection = 0;
-	DevInfo.u32NVMType = Adapter->eNVMType;
-	DevInfo.u32InterfaceType = BCM_USB;
+	memset(&dev_info, 0, sizeof(dev_info));
+	dev_info.MaxRDMBufferSize = BUFFER_4K;
+	dev_info.u32DSDStartOffset = EEPROM_CALPARAM_START;
+	dev_info.u32RxAlignmentCorrection = 0;
+	dev_info.u32NVMType = ad->eNVMType;
+	dev_info.u32InterfaceType = BCM_USB;
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.OutputLength < sizeof(DevInfo))
+	if (io_buff.OutputLength < sizeof(dev_info))
 		return -EINVAL;
 
-	if (copy_to_user(IoBuffer.OutputBuffer, &DevInfo, sizeof(DevInfo)))
+	if (copy_to_user(io_buff.OutputBuffer, &dev_info, sizeof(dev_info)))
 		return -EFAULT;
 
 	return STATUS_SUCCESS;
 }
 
 static int bcm_char_ioctl_time_since_net_entry(void __user *argp,
-	struct bcm_mini_adapter *Adapter)
+	struct bcm_mini_adapter *ad)
 {
-	struct bcm_time_elapsed stTimeElapsedSinceNetEntry = {0};
-	struct bcm_ioctl_buffer IoBuffer;
+	struct bcm_time_elapsed time_elapsed_since_net_entry = {0};
+	struct bcm_ioctl_buffer io_buff;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"IOCTL_BCM_TIME_SINCE_NET_ENTRY called");
 
-	if (copy_from_user(&IoBuffer, argp, sizeof(struct bcm_ioctl_buffer)))
+	if (copy_from_user(&io_buff, argp, sizeof(struct bcm_ioctl_buffer)))
 		return -EFAULT;
 
-	if (IoBuffer.OutputLength < sizeof(struct bcm_time_elapsed))
+	if (io_buff.OutputLength < sizeof(struct bcm_time_elapsed))
 		return -EINVAL;
 
-	stTimeElapsedSinceNetEntry.ul64TimeElapsedSinceNetEntry =
-		get_seconds() - Adapter->liTimeSinceLastNetEntry;
+	time_elapsed_since_net_entry.ul64TimeElapsedSinceNetEntry =
+		get_seconds() - ad->liTimeSinceLastNetEntry;
 
-	if (copy_to_user(IoBuffer.OutputBuffer, &stTimeElapsedSinceNetEntry,
+	if (copy_to_user(io_buff.OutputBuffer, &time_elapsed_since_net_entry,
 			 sizeof(struct bcm_time_elapsed)))
 		return -EFAULT;
 
@@ -2344,31 +2344,31 @@ static int bcm_char_ioctl_time_since_net_entry(void __user *argp,
 
 static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 {
-	struct bcm_tarang_data *pTarang = filp->private_data;
+	struct bcm_tarang_data *tarang = filp->private_data;
 	void __user *argp = (void __user *)arg;
-	struct bcm_mini_adapter *Adapter = pTarang->Adapter;
-	INT Status = STATUS_FAILURE;
+	struct bcm_mini_adapter *ad = tarang->Adapter;
+	INT status = STATUS_FAILURE;
 
-	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+	BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 			"Parameters Passed to control IOCTL cmd=0x%X arg=0x%lX",
 			cmd, arg);
 
 	if (_IOC_TYPE(cmd) != BCM_IOCTL)
 		return -EFAULT;
 	if (_IOC_DIR(cmd) & _IOC_READ)
-		Status = !access_ok(VERIFY_WRITE, argp, _IOC_SIZE(cmd));
+		status = !access_ok(VERIFY_WRITE, argp, _IOC_SIZE(cmd));
 	else if (_IOC_DIR(cmd) & _IOC_WRITE)
-		Status = !access_ok(VERIFY_READ, argp, _IOC_SIZE(cmd));
+		status = !access_ok(VERIFY_READ, argp, _IOC_SIZE(cmd));
 	else if (_IOC_NONE == (_IOC_DIR(cmd) & _IOC_NONE))
-		Status = STATUS_SUCCESS;
+		status = STATUS_SUCCESS;
 
-	if (Status)
+	if (status)
 		return -EFAULT;
 
-	if (Adapter->device_removed)
+	if (ad->device_removed)
 		return -EFAULT;
 
-	if (false == Adapter->fw_download_done) {
+	if (false == ad->fw_download_done) {
 		switch (cmd) {
 		case IOCTL_MAC_ADDR_REQ:
 		case IOCTL_LINK_REQ:
@@ -2384,50 +2384,50 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 		}
 	}
 
-	Status = vendorextnIoctl(Adapter, cmd, arg);
-	if (Status != CONTINUE_COMMON_PATH)
-		return Status;
+	status = vendorextnIoctl(ad, cmd, arg);
+	if (status != CONTINUE_COMMON_PATH)
+		return status;
 
 	switch (cmd) {
 	/* Rdms for Swin Idle... */
 	case IOCTL_BCM_REGISTER_READ_PRIVATE:
-		Status = bcm_char_ioctl_reg_read_private(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_reg_read_private(argp, ad);
+		return status;
 
 	case IOCTL_BCM_REGISTER_WRITE_PRIVATE:
-		Status = bcm_char_ioctl_reg_write_private(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_reg_write_private(argp, ad);
+		return status;
 
 	case IOCTL_BCM_REGISTER_READ:
 	case IOCTL_BCM_EEPROM_REGISTER_READ:
-		Status = bcm_char_ioctl_eeprom_reg_read(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_eeprom_reg_read(argp, ad);
+		return status;
 
 	case IOCTL_BCM_REGISTER_WRITE:
 	case IOCTL_BCM_EEPROM_REGISTER_WRITE:
-		Status = bcm_char_ioctl_eeprom_reg_write(argp, Adapter, cmd);
-		return Status;
+		status = bcm_char_ioctl_eeprom_reg_write(argp, ad, cmd);
+		return status;
 
 	case IOCTL_BCM_GPIO_SET_REQUEST:
-		Status = bcm_char_ioctl_gpio_set_request(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_gpio_set_request(argp, ad);
+		return status;
 
 	case BCM_LED_THREAD_STATE_CHANGE_REQ:
-		Status = bcm_char_ioctl_led_thread_state_change_req(argp,
-								    Adapter);
-		return Status;
+		status = bcm_char_ioctl_led_thread_state_change_req(argp,
+								    ad);
+		return status;
 
 	case IOCTL_BCM_GPIO_STATUS_REQUEST:
-		Status = bcm_char_ioctl_gpio_status_request(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_gpio_status_request(argp, ad);
+		return status;
 
 	case IOCTL_BCM_GPIO_MULTI_REQUEST:
-		Status = bcm_char_ioctl_gpio_multi_request(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_gpio_multi_request(argp, ad);
+		return status;
 
 	case IOCTL_BCM_GPIO_MODE_REQUEST:
-		Status = bcm_char_ioctl_gpio_mode_request(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_gpio_mode_request(argp, ad);
+		return status;
 
 	case IOCTL_MAC_ADDR_REQ:
 	case IOCTL_LINK_REQ:
@@ -2435,176 +2435,176 @@ static long bcm_char_ioctl(struct file *filp, UINT cmd, ULONG arg)
 	case IOCTL_SS_INFO_REQ:
 	case IOCTL_SEND_CONTROL_MESSAGE:
 	case IOCTL_IDLE_REQ:
-		Status = bcm_char_ioctl_misc_request(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_misc_request(argp, ad);
+		return status;
 
 	case IOCTL_BCM_BUFFER_DOWNLOAD_START:
-		Status = bcm_char_ioctl_buffer_download_start(Adapter);
-		return Status;
+		status = bcm_char_ioctl_buffer_download_start(ad);
+		return status;
 
 	case IOCTL_BCM_BUFFER_DOWNLOAD:
-		Status = bcm_char_ioctl_buffer_download(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_buffer_download(argp, ad);
+		return status;
 
 	case IOCTL_BCM_BUFFER_DOWNLOAD_STOP:
-		Status = bcm_char_ioctl_buffer_download_stop(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_buffer_download_stop(argp, ad);
+		return status;
 
 
 	case IOCTL_BE_BUCKET_SIZE:
-		Status = 0;
-		if (get_user(Adapter->BEBucketSize,
+		status = 0;
+		if (get_user(ad->BEBucketSize,
 			     (unsigned long __user *)arg))
-			Status = -EFAULT;
+			status = -EFAULT;
 		break;
 
 	case IOCTL_RTPS_BUCKET_SIZE:
-		Status = 0;
-		if (get_user(Adapter->rtPSBucketSize,
+		status = 0;
+		if (get_user(ad->rtPSBucketSize,
 			     (unsigned long __user *)arg))
-			Status = -EFAULT;
+			status = -EFAULT;
 		break;
 
 	case IOCTL_CHIP_RESET:
-		Status = bcm_char_ioctl_chip_reset(Adapter);
-		return Status;
+		status = bcm_char_ioctl_chip_reset(ad);
+		return status;
 
 	case IOCTL_QOS_THRESHOLD:
-		Status = bcm_char_ioctl_qos_threshold(arg, Adapter);
-		return Status;
+		status = bcm_char_ioctl_qos_threshold(arg, ad);
+		return status;
 
 	case IOCTL_DUMP_PACKET_INFO:
-		DumpPackInfo(Adapter);
-		DumpPhsRules(&Adapter->stBCMPhsContext);
-		Status = STATUS_SUCCESS;
+		DumpPackInfo(ad);
+		DumpPhsRules(&ad->stBCMPhsContext);
+		status = STATUS_SUCCESS;
 		break;
 
 	case IOCTL_GET_PACK_INFO:
-		if (copy_to_user(argp, &Adapter->PackInfo,
+		if (copy_to_user(argp, &ad->PackInfo,
 				 sizeof(struct bcm_packet_info)*NO_OF_QUEUES))
 			return -EFAULT;
-		Status = STATUS_SUCCESS;
+		status = STATUS_SUCCESS;
 		break;
 
 	case IOCTL_BCM_SWITCH_TRANSFER_MODE:
-		Status = bcm_char_ioctl_switch_transfer_mode(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_switch_transfer_mode(argp, ad);
+		return status;
 
 	case IOCTL_BCM_GET_DRIVER_VERSION:
-		Status = bcm_char_ioctl_get_driver_version(argp);
-		return Status;
+		status = bcm_char_ioctl_get_driver_version(argp);
+		return status;
 
 	case IOCTL_BCM_GET_CURRENT_STATUS:
-		Status = bcm_char_ioctl_get_current_status(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_get_current_status(argp, ad);
+		return status;
 
 	case IOCTL_BCM_SET_MAC_TRACING:
-		Status = bcm_char_ioctl_set_mac_tracing(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_set_mac_tracing(argp, ad);
+		return status;
 
 	case IOCTL_BCM_GET_DSX_INDICATION:
-		Status = bcm_char_ioctl_get_dsx_indication(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_get_dsx_indication(argp, ad);
+		return status;
 
 	case IOCTL_BCM_GET_HOST_MIBS:
-		Status = bcm_char_ioctl_get_host_mibs(argp, Adapter, pTarang);
-		return Status;
+		status = bcm_char_ioctl_get_host_mibs(argp, ad, tarang);
+		return status;
 
 	case IOCTL_BCM_WAKE_UP_DEVICE_FROM_IDLE:
-		if ((false == Adapter->bTriedToWakeUpFromlowPowerMode) &&
-				(TRUE == Adapter->IdleMode)) {
-			Adapter->usIdleModePattern = ABORT_IDLE_MODE;
-			Adapter->bWakeUpDevice = TRUE;
-			wake_up(&Adapter->process_rx_cntrlpkt);
+		if ((false == ad->bTriedToWakeUpFromlowPowerMode) &&
+				(TRUE == ad->IdleMode)) {
+			ad->usIdleModePattern = ABORT_IDLE_MODE;
+			ad->bWakeUpDevice = TRUE;
+			wake_up(&ad->process_rx_cntrlpkt);
 		}
 
-		Status = STATUS_SUCCESS;
+		status = STATUS_SUCCESS;
 		break;
 
 	case IOCTL_BCM_BULK_WRM:
-		Status = bcm_char_ioctl_bulk_wrm(argp, Adapter, cmd);
-		return Status;
+		status = bcm_char_ioctl_bulk_wrm(argp, ad, cmd);
+		return status;
 
 	case IOCTL_BCM_GET_NVM_SIZE:
-		Status = bcm_char_ioctl_get_nvm_size(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_get_nvm_size(argp, ad);
+		return status;
 
 	case IOCTL_BCM_CAL_INIT:
-		Status = bcm_char_ioctl_cal_init(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_cal_init(argp, ad);
+		return status;
 
 	case IOCTL_BCM_SET_DEBUG:
-		Status = bcm_char_ioctl_set_debug(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_set_debug(argp, ad);
+		return status;
 
 	case IOCTL_BCM_NVM_READ:
 	case IOCTL_BCM_NVM_WRITE:
-		Status = bcm_char_ioctl_nvm_rw(argp, Adapter, cmd);
-		return Status;
+		status = bcm_char_ioctl_nvm_rw(argp, ad, cmd);
+		return status;
 
 	case IOCTL_BCM_FLASH2X_SECTION_READ:
-		Status = bcm_char_ioctl_flash2x_section_read(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_flash2x_section_read(argp, ad);
+		return status;
 
 	case IOCTL_BCM_FLASH2X_SECTION_WRITE:
-		Status = bcm_char_ioctl_flash2x_section_write(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_flash2x_section_write(argp, ad);
+		return status;
 
 	case IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP:
-		Status = bcm_char_ioctl_flash2x_section_bitmap(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_flash2x_section_bitmap(argp, ad);
+		return status;
 
 	case IOCTL_BCM_SET_ACTIVE_SECTION:
-		Status = bcm_char_ioctl_set_active_section(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_set_active_section(argp, ad);
+		return status;
 
 	case IOCTL_BCM_IDENTIFY_ACTIVE_SECTION:
 		/* Right Now we are taking care of only DSD */
-		Adapter->bAllDSDWriteAllow = false;
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		ad->bAllDSDWriteAllow = false;
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"IOCTL_BCM_IDENTIFY_ACTIVE_SECTION called");
-		Status = STATUS_SUCCESS;
+		status = STATUS_SUCCESS;
 		break;
 
 	case IOCTL_BCM_COPY_SECTION:
-		Status = bcm_char_ioctl_copy_section(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_copy_section(argp, ad);
+		return status;
 
 	case IOCTL_BCM_GET_FLASH_CS_INFO:
-		Status = bcm_char_ioctl_get_flash_cs_info(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_get_flash_cs_info(argp, ad);
+		return status;
 
 	case IOCTL_BCM_SELECT_DSD:
-		Status = bcm_char_ioctl_select_dsd(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_select_dsd(argp, ad);
+		return status;
 
 	case IOCTL_BCM_NVM_RAW_READ:
-		Status = bcm_char_ioctl_nvm_raw_read(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_nvm_raw_read(argp, ad);
+		return status;
 
 	case IOCTL_BCM_CNTRLMSG_MASK:
-		Status = bcm_char_ioctl_cntrlmsg_mask(argp, Adapter, pTarang);
-		return Status;
+		status = bcm_char_ioctl_cntrlmsg_mask(argp, ad, tarang);
+		return status;
 
 	case IOCTL_BCM_GET_DEVICE_DRIVER_INFO:
-		Status = bcm_char_ioctl_get_device_driver_info(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_get_device_driver_info(argp, ad);
+		return status;
 
 	case IOCTL_BCM_TIME_SINCE_NET_ENTRY:
-		Status = bcm_char_ioctl_time_since_net_entry(argp, Adapter);
-		return Status;
+		status = bcm_char_ioctl_time_since_net_entry(argp, ad);
+		return status;
 
 	case IOCTL_CLOSE_NOTIFICATION:
-		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
+		BCM_DEBUG_PRINT(ad, DBG_TYPE_OTHERS, OSAL_DBG, DBG_LVL_ALL,
 				"IOCTL_CLOSE_NOTIFICATION");
 		break;
 
 	default:
 		pr_info(DRV_NAME ": unknown ioctl cmd=%#x\n", cmd);
-		Status = STATUS_FAILURE;
+		status = STATUS_FAILURE;
 		break;
 	}
-	return Status;
+	return status;
 }
 
 
@@ -2617,36 +2617,36 @@ static const struct file_operations bcm_fops = {
 	.llseek = no_llseek,
 };
 
-int register_control_device_interface(struct bcm_mini_adapter *Adapter)
+int register_control_device_interface(struct bcm_mini_adapter *ad)
 {
 
-	if (Adapter->major > 0)
-		return Adapter->major;
+	if (ad->major > 0)
+		return ad->major;
 
-	Adapter->major = register_chrdev(0, DEV_NAME, &bcm_fops);
-	if (Adapter->major < 0) {
+	ad->major = register_chrdev(0, DEV_NAME, &bcm_fops);
+	if (ad->major < 0) {
 		pr_err(DRV_NAME ": could not created character device\n");
-		return Adapter->major;
+		return ad->major;
 	}
 
-	Adapter->pstCreatedClassDevice = device_create(bcm_class, NULL,
-						       MKDEV(Adapter->major, 0),
-						       Adapter, DEV_NAME);
+	ad->pstCreatedClassDevice = device_create(bcm_class, NULL,
+						       MKDEV(ad->major, 0),
+						       ad, DEV_NAME);
 
-	if (IS_ERR(Adapter->pstCreatedClassDevice)) {
+	if (IS_ERR(ad->pstCreatedClassDevice)) {
 		pr_err(DRV_NAME ": class device create failed\n");
-		unregister_chrdev(Adapter->major, DEV_NAME);
-		return PTR_ERR(Adapter->pstCreatedClassDevice);
+		unregister_chrdev(ad->major, DEV_NAME);
+		return PTR_ERR(ad->pstCreatedClassDevice);
 	}
 
 	return 0;
 }
 
-void unregister_control_device_interface(struct bcm_mini_adapter *Adapter)
+void unregister_control_device_interface(struct bcm_mini_adapter *ad)
 {
-	if (Adapter->major > 0) {
-		device_destroy(bcm_class, MKDEV(Adapter->major, 0));
-		unregister_chrdev(Adapter->major, DEV_NAME);
+	if (ad->major > 0) {
+		device_destroy(bcm_class, MKDEV(ad->major, 0));
+		unregister_chrdev(ad->major, DEV_NAME);
 	}
 }
 
diff --git a/drivers/staging/bcm/Bcmnet.c b/drivers/staging/bcm/Bcmnet.c
index 95a2358267ba..e57767684cee 100644
--- a/drivers/staging/bcm/Bcmnet.c
+++ b/drivers/staging/bcm/Bcmnet.c
@@ -4,23 +4,23 @@ struct net_device *gblpnetdev;
 
 static INT bcm_open(struct net_device *dev)
 {
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
 
-	if (Adapter->fw_download_done == false) {
+	if (ad->fw_download_done == false) {
 		pr_notice(PFX "%s: link up failed (download in progress)\n",
 			  dev->name);
 		return -EBUSY;
 	}
 
-	if (netif_msg_ifup(Adapter))
+	if (netif_msg_ifup(ad))
 		pr_info(PFX "%s: enabling interface\n", dev->name);
 
-	if (Adapter->LinkUpStatus) {
-		if (netif_msg_link(Adapter))
+	if (ad->LinkUpStatus) {
+		if (netif_msg_link(ad))
 			pr_info(PFX "%s: link up\n", dev->name);
 
-		netif_carrier_on(Adapter->dev);
-		netif_start_queue(Adapter->dev);
+		netif_carrier_on(ad->dev);
+		netif_start_queue(ad->dev);
 	}
 
 	return 0;
@@ -28,9 +28,9 @@ static INT bcm_open(struct net_device *dev)
 
 static INT bcm_close(struct net_device *dev)
 {
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
 
-	if (netif_msg_ifdown(Adapter))
+	if (netif_msg_ifdown(ad))
 		pr_info(PFX "%s: disabling interface\n", dev->name);
 
 	netif_carrier_off(dev);
@@ -60,42 +60,42 @@ static u16 bcm_select_queue(struct net_device *dev, struct sk_buff *skb,
 
 static netdev_tx_t bcm_transmit(struct sk_buff *skb, struct net_device *dev)
 {
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
 	u16 qindex = skb_get_queue_mapping(skb);
 
 
-	if (Adapter->device_removed || !Adapter->LinkUpStatus)
+	if (ad->device_removed || !ad->LinkUpStatus)
 		goto drop;
 
-	if (Adapter->TransferMode != IP_PACKET_ONLY_MODE)
+	if (ad->TransferMode != IP_PACKET_ONLY_MODE)
 		goto drop;
 
 	if (INVALID_QUEUE_INDEX == qindex)
 		goto drop;
 
-	if (Adapter->PackInfo[qindex].uiCurrentPacketsOnHost >=
+	if (ad->PackInfo[qindex].uiCurrentPacketsOnHost >=
 	    SF_MAX_ALLOWED_PACKETS_TO_BACKUP)
 		return NETDEV_TX_BUSY;
 
 	/* Now Enqueue the packet */
-	if (netif_msg_tx_queued(Adapter))
+	if (netif_msg_tx_queued(ad))
 		pr_info(PFX "%s: enqueueing packet to queue %d\n",
 			dev->name, qindex);
 
-	spin_lock(&Adapter->PackInfo[qindex].SFQueueLock);
-	Adapter->PackInfo[qindex].uiCurrentBytesOnHost += skb->len;
-	Adapter->PackInfo[qindex].uiCurrentPacketsOnHost++;
+	spin_lock(&ad->PackInfo[qindex].SFQueueLock);
+	ad->PackInfo[qindex].uiCurrentBytesOnHost += skb->len;
+	ad->PackInfo[qindex].uiCurrentPacketsOnHost++;
 
 	*((B_UINT32 *) skb->cb + SKB_CB_LATENCY_OFFSET) = jiffies;
-	ENQUEUEPACKET(Adapter->PackInfo[qindex].FirstTxQueue,
-		      Adapter->PackInfo[qindex].LastTxQueue, skb);
-	atomic_inc(&Adapter->TotalPacketCount);
-	spin_unlock(&Adapter->PackInfo[qindex].SFQueueLock);
+	ENQUEUEPACKET(ad->PackInfo[qindex].FirstTxQueue,
+		      ad->PackInfo[qindex].LastTxQueue, skb);
+	atomic_inc(&ad->TotalPacketCount);
+	spin_unlock(&ad->PackInfo[qindex].SFQueueLock);
 
 	/* FIXME - this is racy and incorrect, replace with work queue */
-	if (!atomic_read(&Adapter->TxPktAvail)) {
-		atomic_set(&Adapter->TxPktAvail, 1);
-		wake_up(&Adapter->tx_packet_wait_queue);
+	if (!atomic_read(&ad->TxPktAvail)) {
+		atomic_set(&ad->TxPktAvail, 1);
+		wake_up(&ad->tx_packet_wait_queue);
 	}
 	return NETDEV_TX_OK;
 
@@ -142,39 +142,38 @@ static int bcm_get_settings(struct net_device *dev, struct ethtool_cmd *cmd)
 static void bcm_get_drvinfo(struct net_device *dev,
 			    struct ethtool_drvinfo *info)
 {
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
-	struct bcm_interface_adapter *psIntfAdapter =
-						Adapter->pvInterfaceAdapter;
-	struct usb_device *udev = interface_to_usbdev(psIntfAdapter->interface);
+	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
+	struct bcm_interface_adapter *intf_ad = ad->pvInterfaceAdapter;
+	struct usb_device *udev = interface_to_usbdev(intf_ad->interface);
 
 	strlcpy(info->driver, DRV_NAME, sizeof(info->driver));
 	strlcpy(info->version, DRV_VERSION, sizeof(info->version));
 	snprintf(info->fw_version, sizeof(info->fw_version), "%u.%u",
-		 Adapter->uiFlashLayoutMajorVersion,
-		 Adapter->uiFlashLayoutMinorVersion);
+		 ad->uiFlashLayoutMajorVersion,
+		 ad->uiFlashLayoutMinorVersion);
 
 	usb_make_path(udev, info->bus_info, sizeof(info->bus_info));
 }
 
 static u32 bcm_get_link(struct net_device *dev)
 {
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
 
-	return Adapter->LinkUpStatus;
+	return ad->LinkUpStatus;
 }
 
 static u32 bcm_get_msglevel(struct net_device *dev)
 {
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
 
-	return Adapter->msg_enable;
+	return ad->msg_enable;
 }
 
 static void bcm_set_msglevel(struct net_device *dev, u32 level)
 {
-	struct bcm_mini_adapter *Adapter = GET_BCM_ADAPTER(dev);
+	struct bcm_mini_adapter *ad = GET_BCM_ADAPTER(dev);
 
-	Adapter->msg_enable = level;
+	ad->msg_enable = level;
 }
 
 static const struct ethtool_ops bcm_ethtool_ops = {
@@ -185,12 +184,12 @@ static const struct ethtool_ops bcm_ethtool_ops = {
 	.set_msglevel	= bcm_set_msglevel,
 };
 
-int register_networkdev(struct bcm_mini_adapter *Adapter)
+int register_networkdev(struct bcm_mini_adapter *ad)
 {
-	struct net_device *net = Adapter->dev;
-	struct bcm_interface_adapter *IntfAdapter = Adapter->pvInterfaceAdapter;
-	struct usb_interface *udev = IntfAdapter->interface;
-	struct usb_device *xdev = IntfAdapter->udev;
+	struct net_device *net = ad->dev;
+	struct bcm_interface_adapter *intf_ad = ad->pvInterfaceAdapter;
+	struct usb_interface *udev = intf_ad->interface;
+	struct usb_device *xdev = intf_ad->udev;
 
 	int result;
 
@@ -205,7 +204,7 @@ int register_networkdev(struct bcm_mini_adapter *Adapter)
 	SET_NETDEV_DEVTYPE(net, &wimax_type);
 
 	/* Read the MAC Address from EEPROM */
-	result = ReadMacAddressFromNVM(Adapter);
+	result = ReadMacAddressFromNVM(ad);
 	if (result != STATUS_SUCCESS) {
 		dev_err(&udev->dev,
 			PFX "Error in Reading the mac Address: %d", result);
@@ -216,9 +215,9 @@ int register_networkdev(struct bcm_mini_adapter *Adapter)
 	if (result)
 		return result;
 
-	gblpnetdev = Adapter->dev;
+	gblpnetdev = ad->dev;
 
-	if (netif_msg_probe(Adapter))
+	if (netif_msg_probe(ad))
 		dev_info(&udev->dev, PFX "%s: register usb-%s-%s %pM\n",
 			 net->name, xdev->bus->bus_name, xdev->devpath,
 			 net->dev_addr);
@@ -226,16 +225,16 @@ int register_networkdev(struct bcm_mini_adapter *Adapter)
 	return 0;
 }
 
-void unregister_networkdev(struct bcm_mini_adapter *Adapter)
+void unregister_networkdev(struct bcm_mini_adapter *ad)
 {
-	struct net_device *net = Adapter->dev;
-	struct bcm_interface_adapter *IntfAdapter = Adapter->pvInterfaceAdapter;
-	struct usb_interface *udev = IntfAdapter->interface;
-	struct usb_device *xdev = IntfAdapter->udev;
+	struct net_device *net = ad->dev;
+	struct bcm_interface_adapter *intf_ad = ad->pvInterfaceAdapter;
+	struct usb_interface *udev = intf_ad->interface;
+	struct usb_device *xdev = intf_ad->udev;
 
-	if (netif_msg_probe(Adapter))
+	if (netif_msg_probe(ad))
 		dev_info(&udev->dev, PFX "%s: unregister usb-%s%s\n",
 			 net->name, xdev->bus->bus_name, xdev->devpath);
 
-	unregister_netdev(Adapter->dev);
+	unregister_netdev(ad->dev);
 }
diff --git a/drivers/staging/bcm/HandleControlPacket.c b/drivers/staging/bcm/HandleControlPacket.c
index 54552214bf6f..dd5d138a6528 100644
--- a/drivers/staging/bcm/HandleControlPacket.c
+++ b/drivers/staging/bcm/HandleControlPacket.c
@@ -157,7 +157,9 @@ static VOID handle_rx_control_packet(struct bcm_mini_adapter *Adapter,
  * @ingroup ctrl_pkt_functions
  * Thread to handle control pkt reception
  */
-int control_packet_handler(struct bcm_mini_adapter *Adapter /* pointer to adapter object*/)
+
+/* pointer to adapter object*/
+int control_packet_handler(struct bcm_mini_adapter *Adapter)
 {
 	struct sk_buff *ctrl_packet = NULL;
 	unsigned long flags = 0;
diff --git a/drivers/staging/bcm/InterfaceIdleMode.c b/drivers/staging/bcm/InterfaceIdleMode.c
index c84ee494f55f..612c89fba341 100644
--- a/drivers/staging/bcm/InterfaceIdleMode.c
+++ b/drivers/staging/bcm/InterfaceIdleMode.c
@@ -188,8 +188,8 @@ static int InterfaceAbortIdlemode(struct bcm_mini_adapter *Adapter,
 			5000);
 		if (status)
 			return status;
-		else
-			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
+
+		BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
 				IDLE_MODE, DBG_LVL_ALL,
 				"NOB Sent down :%d", lenwritten);
 
@@ -211,8 +211,7 @@ static int InterfaceAbortIdlemode(struct bcm_mini_adapter *Adapter,
 		else
 			BCM_DEBUG_PRINT(Adapter, DBG_TYPE_OTHERS,
 				IDLE_MODE, DBG_LVL_ALL,
-				"Number of completed iteration to"
-				"read chip-id :%lu", itr);
+				"Number of completed iteration to read chip-id :%lu", itr);
 
 		status = wrmalt(Adapter, SW_ABORT_IDLEMODE_LOC,
 				&Pattern, sizeof(status));
diff --git a/drivers/staging/bcm/InterfaceMacros.h b/drivers/staging/bcm/InterfaceMacros.h
index 7001caff9e26..fedb79437f33 100644
--- a/drivers/staging/bcm/InterfaceMacros.h
+++ b/drivers/staging/bcm/InterfaceMacros.h
@@ -10,7 +10,7 @@
 
 #define MAX_DATA_BUFFER_SIZE    2048
 
-//Num of Asynchronous reads pending
+/* Num of Asynchronous reads pending */
 #define NUM_RX_DESC 64
 
 #define SYS_CFG 0x0F000C00
diff --git a/drivers/staging/bcm/InterfaceMisc.h b/drivers/staging/bcm/InterfaceMisc.h
index efb686046153..0e5e38b26329 100644
--- a/drivers/staging/bcm/InterfaceMisc.h
+++ b/drivers/staging/bcm/InterfaceMisc.h
@@ -39,4 +39,4 @@ VOID Bcm_kill_all_URBs(struct bcm_interface_adapter *psIntfAdapter);
 
 #define DISABLE_USB_ZERO_LEN_INT 0x0F011878
 
-#endif // __INTERFACE_MISC_H
+#endif /* __INTERFACE_MISC_H */
diff --git a/drivers/staging/bcm/Ioctl.h b/drivers/staging/bcm/Ioctl.h
index 797f862b90cf..fa5f8671612e 100644
--- a/drivers/staging/bcm/Ioctl.h
+++ b/drivers/staging/bcm/Ioctl.h
@@ -87,9 +87,9 @@ struct bcm_user_thread_req {
 #define IOCTL_BCM_FLASH2X_SECTION_WRITE		_IOW(BCM_IOCTL, 0x866, int)
 #define IOCTL_BCM_GET_FLASH2X_SECTION_BITMAP	_IOR(BCM_IOCTL, 0x867, int)
 #define IOCTL_BCM_SET_ACTIVE_SECTION		_IOW(BCM_IOCTL, 0x868, int)
-#define	IOCTL_BCM_IDENTIFY_ACTIVE_SECTION	_IO(BCM_IOCTL, 0x869)
+#define IOCTL_BCM_IDENTIFY_ACTIVE_SECTION	_IO(BCM_IOCTL, 0x869)
 #define IOCTL_BCM_COPY_SECTION			_IOW(BCM_IOCTL, 0x870, int)
-#define	IOCTL_BCM_GET_FLASH_CS_INFO		_IOR(BCM_IOCTL, 0x871, int)
+#define IOCTL_BCM_GET_FLASH_CS_INFO		_IOR(BCM_IOCTL, 0x871, int)
 #define IOCTL_BCM_SELECT_DSD			_IOW(BCM_IOCTL, 0x872, int)
 #define IOCTL_BCM_NVM_RAW_READ			_IOR(BCM_IOCTL, 0x875, int)
 #define IOCTL_BCM_CNTRLMSG_MASK			_IOW(BCM_IOCTL, 0x874, int)
@@ -130,7 +130,7 @@ struct bcm_bulk_wrm_buffer {
 };
 
 enum bcm_flash2x_section_val {
-	NO_SECTION_VAL = 0, /* no section is chosen when absolute offset is given for RD/WR */
+	NO_SECTION_VAL = 0, /* no section chosen when absolute offset is given for RD/WR */
 	ISO_IMAGE1,
 	ISO_IMAGE2,
 	DSD0,
@@ -152,11 +152,11 @@ enum bcm_flash2x_section_val {
  * Structure used for READ/WRITE Flash Map2.x
  */
 struct bcm_flash2x_readwrite {
-	enum bcm_flash2x_section_val Section; /* which section has to be read/written */
-	u32 offset;	     /* Offset within Section. */
-	u32 numOfBytes;	     /* NOB from the offset */
+	enum bcm_flash2x_section_val Section; /* section to be read/written */
+	u32 offset; /* offset within section. */
+	u32 numOfBytes; /* number of bytes from the offset */
 	u32 bVerify;
-	void __user *pDataBuff;	     /* Buffer for reading/writing */
+	void __user *pDataBuff; /* buffer for reading/writing */
 };
 
 /*
@@ -207,20 +207,20 @@ struct bcm_time_elapsed {
 };
 
 enum {
-	WIMAX_IDX = 0,  /* To access WiMAX chip GPIO's for GPIO_MULTI_INFO or GPIO_MULTI_MODE */
-	HOST_IDX,	/* To access Host chip GPIO's for GPIO_MULTI_INFO or GPIO_MULTI_MODE */
+	WIMAX_IDX = 0, /* To access WiMAX chip GPIO's for GPIO_MULTI_INFO or GPIO_MULTI_MODE */
+	HOST_IDX, /* To access Host chip GPIO's for GPIO_MULTI_INFO or GPIO_MULTI_MODE */
 	MAX_IDX
 };
 
 struct bcm_gpio_multi_info {
 	unsigned int uiGPIOCommand; /* 1 for set and 0 for get */
-	unsigned int uiGPIOMask;    /* set the correspondig bit to 1 to access GPIO */
-	unsigned int uiGPIOValue;   /* 0 or 1; value to be set when command is 1. */
+	unsigned int uiGPIOMask; /* set the corresponding bit to 1 to access GPIO */
+	unsigned int uiGPIOValue; /* 0 or 1; value to be set when command is 1. */
 } __packed;
 
 struct bcm_gpio_multi_mode {
-	unsigned int uiGPIOMode;    /* 1 for OUT mode, 0 for IN mode */
-	unsigned int uiGPIOMask;    /* GPIO mask to set mode */
+	unsigned int uiGPIOMode; /* 1 for OUT mode, 0 for IN mode */
+	unsigned int uiGPIOMask; /* GPIO mask to set mode */
 } __packed;
 
 #endif
diff --git a/drivers/staging/bcm/LeakyBucket.c b/drivers/staging/bcm/LeakyBucket.c
index 8c4030dfa7d5..d6b55f993b57 100644
--- a/drivers/staging/bcm/LeakyBucket.c
+++ b/drivers/staging/bcm/LeakyBucket.c
@@ -1,20 +1,16 @@
 /**********************************************************************
-* 			LEAKYBUCKET.C
+*			LEAKYBUCKET.C
 *	This file contains the routines related to Leaky Bucket Algorithm.
 ***********************************************************************/
 #include "headers.h"
 
-/*********************************************************************
-* Function    - UpdateTokenCount()
-*
-* Description - This function calculates the token count for each
-*				channel and updates the same in Adapter strucuture.
-*
-* Parameters  - Adapter: Pointer to the Adapter structure.
-*
-* Returns     - None
-**********************************************************************/
-
+/**
+ * UpdateTokenCount() - Calculates the token count for each channel
+ * and updates the same in Adapter structure
+ * @Adapter:	Pointer to the Adapter structure.
+ *
+ * Return: None
+ */
 static VOID UpdateTokenCount(register struct bcm_mini_adapter *Adapter)
 {
 	ULONG liCurrentTime;
@@ -59,20 +55,16 @@ static VOID UpdateTokenCount(register struct bcm_mini_adapter *Adapter)
 }
 
 
-/*********************************************************************
-* Function    - IsPacketAllowedForFlow()
-*
-* Description - This function checks whether the given packet from the
-*				specified queue can be allowed for transmission by
-*				checking the token count.
-*
-* Parameters  - Adapter	      :	Pointer to the Adpater structure.
-* 			  - iQIndex	      :	The queue Identifier.
-* 			  - ulPacketLength:	Number of bytes to be transmitted.
-*
-* Returns     - The number of bytes allowed for transmission.
-*
-***********************************************************************/
+/**
+ * IsPacketAllowedForFlow() - This function checks whether the given
+ * packet from the specified queue can be allowed for transmission by
+ * checking the token count.
+ * @Adapter:		Pointer to the Adpater structure.
+ * @iQIndex:		The queue Identifier.
+ * @ulPacketLength:	Number of bytes to be transmitted.
+ *
+ * Returns: The number of bytes allowed for transmission.
+ */
 static ULONG GetSFTokenCount(struct bcm_mini_adapter *Adapter, struct bcm_packet_info *psSF)
 {
 	BCM_DEBUG_PRINT(Adapter, DBG_TYPE_TX, TOKEN_COUNTS, DBG_LVL_ALL,
@@ -256,18 +248,14 @@ static void send_control_packet(struct bcm_mini_adapter *ad,
 	}
 }
 
-/************************************************************************
-* Function    - CheckAndSendPacketFromIndex()
-*
-* Description - This function dequeues the data/control packet from the
-*				specified queue for transmission.
-*
-* Parameters  - Adapter : Pointer to the driver control structure.
-* 			  - iQIndex : The queue Identifier.
-*
-* Returns     - None.
-*
-****************************************************************************/
+/**
+ * CheckAndSendPacketFromIndex() - This function dequeues the
+ * data/control packet from the specified queue for transmission.
+ * @Adapter:	Pointer to the driver control structure.
+ * @iQIndex:	The queue Identifier.
+ *
+ * Returns: None.
+ */
 static VOID CheckAndSendPacketFromIndex(struct bcm_mini_adapter *Adapter,
 					struct bcm_packet_info *psSF)
 {
@@ -284,16 +272,13 @@ static VOID CheckAndSendPacketFromIndex(struct bcm_mini_adapter *Adapter,
 }
 
 
-/*******************************************************************
-* Function    - transmit_packets()
-*
-* Description - This function transmits the packets from different
-*				queues, if free descriptors are available on target.
-*
-* Parameters  - Adapter:  Pointer to the Adapter structure.
-*
-* Returns     - None.
-********************************************************************/
+/**
+ * transmit_packets() - This function transmits the packets from
+ * different queues, if free descriptors are available on target.
+ * @Adapter:	Pointer to the Adapter structure.
+ *
+ * Returns: None.
+ */
 VOID transmit_packets(struct bcm_mini_adapter *Adapter)
 {
 	UINT uiPrevTotalCount = 0;
diff --git a/drivers/staging/bcm/headers.h b/drivers/staging/bcm/headers.h
index 6f3270cc4176..a7d4af5ea9a7 100644
--- a/drivers/staging/bcm/headers.h
+++ b/drivers/staging/bcm/headers.h
@@ -1,6 +1,6 @@
 
 /*******************************************************************
-* 		Headers.h
+*		Headers.h
 *******************************************************************/
 #ifndef __HEADERS_H__
 #define __HEADERS_H__
diff --git a/drivers/staging/bcm/sort.c b/drivers/staging/bcm/sort.c
index d518c4217f13..ca0b17991512 100644
--- a/drivers/staging/bcm/sort.c
+++ b/drivers/staging/bcm/sort.c
@@ -28,7 +28,7 @@ VOID SortPackInfo(struct bcm_mini_adapter *Adapter)
 			DBG_LVL_ALL, "<=======");
 
 	sort(Adapter->PackInfo, NO_OF_QUEUES, sizeof(struct bcm_packet_info),
-		compare_packet_info, NULL);
+	     compare_packet_info, NULL);
 }
 
 static int compare_classifiers(void const *a, void const *b)
@@ -48,5 +48,5 @@ VOID SortClassifiers(struct bcm_mini_adapter *Adapter)
 			DBG_LVL_ALL, "<=======");
 
 	sort(Adapter->astClassifierTable, MAX_CLASSIFIERS,
-		sizeof(struct bcm_classifier_rule), compare_classifiers, NULL);
+	     sizeof(struct bcm_classifier_rule), compare_classifiers, NULL);
 }
diff --git a/drivers/staging/bcm/vendorspecificextn.c b/drivers/staging/bcm/vendorspecificextn.c
index 2c57a16788c0..1d9bef6e4273 100644
--- a/drivers/staging/bcm/vendorspecificextn.c
+++ b/drivers/staging/bcm/vendorspecificextn.c
@@ -11,7 +11,8 @@
  *		STATUS_SUCCESS/STATUS_FAILURE
  *
  */
-INT vendorextnGetSectionInfo(PVOID  pContext, struct bcm_flash2x_vendor_info *pVendorInfo)
+INT vendorextnGetSectionInfo(PVOID  pContext,
+			struct bcm_flash2x_vendor_info *pVendorInfo)
 {
 	return STATUS_FAILURE;
 }
@@ -61,7 +62,8 @@ INT vendorextnExit(struct bcm_mini_adapter *Adapter)
  *		arg	-input parameter sent by vendor
  *
  * Returns:
- *		CONTINUE_COMMON_PATH in case it is not meant to be processed by vendor ioctls
+ *		CONTINUE_COMMON_PATH in case it is not meant to be processed
+ *		by vendor ioctls
  *		STATUS_SUCCESS/STATUS_FAILURE as per the IOCTL return value
  */
 
@@ -88,8 +90,8 @@ INT vendorextnIoctl(struct bcm_mini_adapter *Adapter, UINT cmd, ULONG arg)
  *		STATUS_SUCCESS/STATUS_FAILURE
  */
 
-INT vendorextnReadSection(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_section_val SectionVal,
-			UINT offset, UINT numOfBytes)
+INT vendorextnReadSection(PVOID  pContext, PUCHAR pBuffer,
+	enum bcm_flash2x_section_val SectionVal, UINT offset, UINT numOfBytes)
 {
 	return STATUS_FAILURE;
 }
@@ -112,8 +114,9 @@ INT vendorextnReadSection(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_sect
  * Returns:
  *		STATUS_SUCCESS/STATUS_FAILURE
  */
-INT vendorextnWriteSection(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_section_val SectionVal,
-			UINT offset, UINT numOfBytes, bool bVerify)
+INT vendorextnWriteSection(PVOID  pContext, PUCHAR pBuffer,
+	enum bcm_flash2x_section_val SectionVal, UINT offset,
+	UINT numOfBytes, bool bVerify)
 {
 	return STATUS_FAILURE;
 }
@@ -135,8 +138,8 @@ INT vendorextnWriteSection(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_sec
  * Returns:
  *		STATUS_SUCCESS/STATUS_FAILURE
  */
-INT vendorextnWriteSectionWithoutErase(PVOID  pContext, PUCHAR pBuffer, enum bcm_flash2x_section_val SectionVal,
-			UINT offset, UINT numOfBytes)
+INT vendorextnWriteSectionWithoutErase(PVOID  pContext, PUCHAR pBuffer,
+	enum bcm_flash2x_section_val SectionVal, UINT offset, UINT numOfBytes)
 {
 	return STATUS_FAILURE;
 }
diff --git a/drivers/staging/comedi/Kconfig b/drivers/staging/comedi/Kconfig
index 36f2c7159250..a8bc2b567789 100644
--- a/drivers/staging/comedi/Kconfig
+++ b/drivers/staging/comedi/Kconfig
@@ -42,24 +42,20 @@ menuconfig COMEDI_MISC_DRIVERS
 
 if COMEDI_MISC_DRIVERS
 
-config COMEDI_KCOMEDILIB
-	tristate "Comedi kcomedilib"
-	---help---
-	  Build the kcomedilib
-
 config COMEDI_BOND
-	tristate "Device bonding support"
-	depends on COMEDI_KCOMEDILIB
+	tristate "Comedi device bonding support"
+	select COMEDI_KCOMEDILIB
 	---help---
 	  Enable support for a driver to 'bond' (merge) multiple subdevices
 	  from multiple devices together as one.
 
+	  Currently, it only handles digital I/O subdevices.
+
 	  To compile this driver as a module, choose M here: the module will be
 	  called comedi_bond.
 
 config COMEDI_TEST
 	tristate "Fake waveform generator support"
-	select COMEDI_FC
 	---help---
 	  Enable support for the fake waveform generator.
 	  This driver is mainly for testing purposes, but can also be used to
@@ -88,14 +84,6 @@ config COMEDI_SERIAL2002
 	  To compile this driver as a module, choose M here: the module will be
 	  called serial2002.
 
-config COMEDI_SKEL
-	tristate "Comedi skeleton driver"
-	---help---
-	  Build the Skeleton driver, an example for driver writers
-
-	  To compile this driver as a module, choose M here: the module will be
-	  called skel.
-
 config COMEDI_SSV_DNP
 	tristate "SSV Embedded Systems DIL/Net-PC support"
 	depends on X86_32 || COMPILE_TEST
@@ -181,7 +169,6 @@ config COMEDI_PCL730
 config COMEDI_PCL812
 	tristate "Advantech PCL-812/813 and ADlink ACL-8112/8113/8113/8216"
 	depends on VIRT_TO_BUS && ISA_DMA_API
-	select COMEDI_FC
 	---help---
 	  Enable support for Advantech PCL-812/PG, PCL-813/B, ADLink
 	  ACL-8112DG/HG/PG, ACL-8113, ACL-8216, ICP DAS A-821PGH/PGL/PGL-NDA,
@@ -193,7 +180,6 @@ config COMEDI_PCL812
 config COMEDI_PCL816
 	tristate "Advantech PCL-814 and PCL-816 ISA card support"
 	depends on VIRT_TO_BUS && ISA_DMA_API
-	select COMEDI_FC
 	---help---
 	  Enable support for Advantech PCL-814 and PCL-816 ISA cards
 
@@ -203,7 +189,6 @@ config COMEDI_PCL816
 config COMEDI_PCL818
 	tristate "Advantech PCL-718 and PCL-818 ISA card support"
 	depends on VIRT_TO_BUS && ISA_DMA_API
-	select COMEDI_FC
 	---help---
 	  Enable support for Advantech PCL-818 ISA cards
 	  PCL-818L, PCL-818H, PCL-818HD, PCL-818HG, PCL-818 and PCL-718
@@ -275,7 +260,6 @@ config COMEDI_DAC02
 config COMEDI_DAS16M1
 	tristate "MeasurementComputing CIO-DAS16/M1DAS-16 ISA card support"
 	select COMEDI_8255
-	select COMEDI_FC
 	---help---
 	  Enable support for Measurement Computing CIO-DAS16/M1 ISA cards.
 
@@ -299,7 +283,6 @@ config COMEDI_DAS16
 	tristate "DAS-16 compatible ISA and PC/104 card support"
 	depends on ISA_DMA_API
 	select COMEDI_8255
-	select COMEDI_FC
 	---help---
 	  Enable support for Keithley Metrabyte/ComputerBoards DAS16
 	  and compatible ISA and PC/104 cards:
@@ -315,7 +298,6 @@ config COMEDI_DAS16
 
 config COMEDI_DAS800
 	tristate "DAS800 and compatible ISA card support"
-	select COMEDI_FC
 	---help---
 	  Enable support for Keithley Metrabyte DAS800 and compatible ISA cards
 	  Keithley Metrabyte DAS-800, DAS-801, DAS-802
@@ -328,7 +310,6 @@ config COMEDI_DAS800
 config COMEDI_DAS1800
 	tristate "DAS1800 and compatible ISA card support"
 	depends on VIRT_TO_BUS && ISA_DMA_API
-	select COMEDI_FC
 	---help---
 	  Enable support for DAS1800 and compatible ISA cards
 	  Keithley Metrabyte DAS-1701ST, DAS-1701ST-DA, DAS-1701/AO,
@@ -391,7 +372,6 @@ config COMEDI_DT2817
 
 config COMEDI_DT282X
 	tristate "Data Translation DT2821 series and DT-EZ ISA card support"
-	select COMEDI_FC
 	depends on VIRT_TO_BUS && ISA_DMA_API
 	---help---
 	  Enable support for Data Translation DT2821 series including DT-EZ
@@ -480,7 +460,6 @@ config COMEDI_ADQ12B
 
 config COMEDI_NI_AT_A2150
 	tristate "NI AT-A2150 ISA card support"
-	select COMEDI_FC
 	depends on VIRT_TO_BUS && ISA_DMA_API
 	---help---
 	  Enable support for National Instruments AT-A2150 cards
@@ -500,7 +479,6 @@ config COMEDI_NI_ATMIO
 	tristate "NI AT-MIO E series ISA-PNP card support"
 	select COMEDI_8255
 	select COMEDI_NI_TIO
-	select COMEDI_FC
 	---help---
 	  Enable support for National Instruments AT-MIO E series cards
 	  National Instruments AT-MIO-16E-1 (ni_atmio),
@@ -688,8 +666,8 @@ config COMEDI_ADDI_APCI_2200
 
 config COMEDI_ADDI_APCI_3120
 	tristate "ADDI-DATA APCI_3120/3001 support"
+	depends on HAS_DMA
 	depends on VIRT_TO_BUS
-	select COMEDI_FC
 	---help---
 	  Enable support for ADDI-DATA APCI_3120/3001 cards
 
@@ -741,7 +719,6 @@ config COMEDI_ADL_PCI8164
 
 config COMEDI_ADL_PCI9111
 	tristate "ADLink PCI-9111HR support"
-	select COMEDI_FC
 	---help---
 	  Enable support for ADlink PCI9111 cards
 
@@ -750,7 +727,7 @@ config COMEDI_ADL_PCI9111
 
 config COMEDI_ADL_PCI9118
 	tristate "ADLink PCI-9118DG, PCI-9118HG, PCI-9118HR support"
-	select COMEDI_FC
+	depends on HAS_DMA
 	depends on VIRT_TO_BUS
 	---help---
 	  Enable support for ADlink PCI-9118DG, PCI-9118HG, PCI-9118HR cards
@@ -760,7 +737,6 @@ config COMEDI_ADL_PCI9118
 
 config COMEDI_ADV_PCI1710
 	tristate "Advantech PCI-171x, PCI-1720 and PCI-1731 support"
-	select COMEDI_FC
 	---help---
 	  Enable support for Advantech PCI-1710, PCI-1710HG, PCI-1711,
 	  PCI-1713, PCI-1720 and PCI-1731
@@ -828,7 +804,6 @@ config COMEDI_AMPLC_PC263_PCI
 
 config COMEDI_AMPLC_PCI224
 	tristate "Amplicon PCI224 and PCI234 support"
-	select COMEDI_FC
 	---help---
 	  Enable support for Amplicon PCI224 and PCI234 AO boards
 
@@ -864,7 +839,6 @@ config COMEDI_DAS08_PCI
 
 config COMEDI_DT3000
 	tristate "Data Translation DT3000 series support"
-	select COMEDI_FC
 	---help---
 	  Enable support for Data Translation DT3000 series
 	  DT3001, DT3001-PGL, DT3002, DT3003, DT3003-PGL, DT3004, DT3005 and
@@ -884,7 +858,6 @@ config COMEDI_DYNA_PCI10XX
 
 config COMEDI_GSC_HPDI
 	tristate "General Standards PCI-HPDI32 / PMC-HPDI32 support"
-	select COMEDI_FC
 	---help---
 	  Enable support for General Standards Corporation high speed parallel
 	  digital interface rs485 boards PCI-HPDI32 and PMC-HPDI32.
@@ -935,7 +908,6 @@ config COMEDI_KE_COUNTER
 config COMEDI_CB_PCIDAS64
 	tristate "MeasurementComputing PCI-DAS 64xx, 60xx, and 4020 support"
 	select COMEDI_8255
-	select COMEDI_FC
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI-DAS 64xx,
 	  60xx, and 4020 series with the PLX 9080 PCI controller
@@ -946,7 +918,6 @@ config COMEDI_CB_PCIDAS64
 config COMEDI_CB_PCIDAS
 	tristate "MeasurementComputing PCI-DAS support"
 	select COMEDI_8255
-	select COMEDI_FC
 	---help---
 	  Enable support for ComputerBoards/MeasurementComputing PCI-DAS with
 	  AMCC S5933 PCIcontroller: PCI-DAS1602/16, PCI-DAS1602/16jr,
@@ -1069,7 +1040,6 @@ config COMEDI_NI_PCIMIO
 	depends on HAS_DMA
 	select COMEDI_NI_TIOCMD
 	select COMEDI_8255
-	select COMEDI_FC
 	---help---
 	  Enable support for National Instruments PCI-MIO-E series and M series
 	  (all boards): PCI-MIO-16XE-10, PXI-6030E, PCI-MIO-16E-1,
@@ -1095,7 +1065,6 @@ config COMEDI_RTD520
 
 config COMEDI_S626
 	tristate "Sensoray 626 support"
-	select COMEDI_FC
 	---help---
 	  Enable support for Sensoray 626
 
@@ -1104,7 +1073,6 @@ config COMEDI_S626
 
 config COMEDI_MITE
 	depends on HAS_DMA
-	select COMEDI_FC
 	tristate
 
 config COMEDI_NI_TIOCMD
@@ -1172,7 +1140,6 @@ config COMEDI_NI_MIO_CS
 	tristate "NI DAQCard E series PCMCIA support"
 	select COMEDI_NI_TIO
 	select COMEDI_8255
-	select COMEDI_FC
 	---help---
 	  Enable support for the National Instruments PCMCIA DAQCard E series
 	  DAQCard-ai-16xe-50, DAQCard-ai-16e-4, DAQCard-6062E, DAQCard-6024E
@@ -1183,7 +1150,6 @@ config COMEDI_NI_MIO_CS
 
 config COMEDI_QUATECH_DAQP_CS
 	tristate "Quatech DAQP PCMCIA data capture card support"
-	select COMEDI_FC
 	---help---
 	  Enable support for the Quatech DAQP PCMCIA data capture cards
 	  DAQP-208 and DAQP-308
@@ -1209,6 +1175,17 @@ config COMEDI_DT9812
 	  To compile this driver as a module, choose M here: the module will be
 	  called dt9812.
 
+config COMEDI_NI_USB6501
+	tristate "NI USB-6501 support"
+	---help---
+	  Enable support for the National Instruments USB-6501 module.
+
+	  The NI USB-6501 is a Full-Speed USB 2.0 (12 Mbit/s) device that
+	  provides 24 digital I/O lines channels and one 32-bit counter.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called ni_usb6501.
+
 config COMEDI_USBDUX
 	tristate "ITL USB-DUX-D support"
 	---help---
@@ -1219,7 +1196,6 @@ config COMEDI_USBDUX
 
 config COMEDI_USBDUXFAST
 	tristate "ITL USB-DUXfast support"
-	select COMEDI_FC
 	---help---
 	  Enable support for the Incite Technology Ltd USB-DUXfast Board
 
@@ -1228,7 +1204,6 @@ config COMEDI_USBDUXFAST
 
 config COMEDI_USBDUXSIGMA
 	tristate "ITL USB-DUXsigma support"
-	select COMEDI_FC
 	---help---
 	  Enable support for the Incite Technology Ltd USB-DUXsigma Board
 
@@ -1261,8 +1236,19 @@ config COMEDI_8255
 	  To compile this driver as a module, choose M here: the module will be
 	  called 8255.
 
-config COMEDI_FC
-	tristate
+config COMEDI_KCOMEDILIB
+	tristate "Comedi kcomedilib"
+	---help---
+	  Build the kcomedilib.
+
+	  This is a kernel module used to open and manipulate Comedi devices
+	  from within kernel code.  It is currently only used by the
+	  comedi_bond driver, and its functionality has been stripped down to
+	  the needs of that driver, so is currently not very useful for
+	  anything else.
+
+	  To compile kcomedilib as a module, choose M here: the module will be
+	  called kcomedilib.
 
 config COMEDI_AMPLC_DIO200
 	tristate
@@ -1278,7 +1264,6 @@ config COMEDI_DAS08
 config COMEDI_NI_LABPC
 	tristate
 	select COMEDI_8255
-	select COMEDI_FC
 
 config COMEDI_NI_LABPC_ISADMA
 	tristate
diff --git a/drivers/staging/comedi/comedi.h b/drivers/staging/comedi/comedi.h
index 217baf812f87..c8c99e65423b 100644
--- a/drivers/staging/comedi/comedi.h
+++ b/drivers/staging/comedi/comedi.h
@@ -123,38 +123,48 @@
 /* trigger flags */
 /* These flags are used in comedi_trig structures */
 
-#define TRIG_BOGUS	0x0001	/* do the motions */
 #define TRIG_DITHER	0x0002	/* enable dithering */
 #define TRIG_DEGLITCH	0x0004	/* enable deglitching */
-	/*#define TRIG_RT       0x0008 *//* perform op in real time */
 #define TRIG_CONFIG	0x0010	/* perform configuration, not triggering */
-#define TRIG_WAKE_EOS	0x0020	/* wake up on end-of-scan events */
-	/*#define TRIG_WRITE    0x0040*//* write to bidirectional devices */
 
 /* command flags */
 /* These flags are used in comedi_cmd structures */
 
+#define CMDF_BOGUS		0x00000001	/* do the motions */
+
 /* try to use a real-time interrupt while performing command */
 #define CMDF_PRIORITY		0x00000008
 
-#define TRIG_RT		CMDF_PRIORITY	/* compatibility definition */
+/* wake up on end-of-scan events */
+#define CMDF_WAKE_EOS		0x00000020
 
 #define CMDF_WRITE		0x00000040
-#define TRIG_WRITE	CMDF_WRITE	/* compatibility definition */
 
 #define CMDF_RAWDATA		0x00000080
 
+/* timer rounding definitions */
+#define CMDF_ROUND_MASK		0x00030000
+#define CMDF_ROUND_NEAREST	0x00000000
+#define CMDF_ROUND_DOWN		0x00010000
+#define CMDF_ROUND_UP		0x00020000
+#define CMDF_ROUND_UP_NEXT	0x00030000
+
 #define COMEDI_EV_START		0x00040000
 #define COMEDI_EV_SCAN_BEGIN	0x00080000
 #define COMEDI_EV_CONVERT	0x00100000
 #define COMEDI_EV_SCAN_END	0x00200000
 #define COMEDI_EV_STOP		0x00400000
 
-#define TRIG_ROUND_MASK		0x00030000
-#define TRIG_ROUND_NEAREST	0x00000000
-#define TRIG_ROUND_DOWN		0x00010000
-#define TRIG_ROUND_UP		0x00020000
-#define TRIG_ROUND_UP_NEXT	0x00030000
+/* compatibility definitions */
+#define TRIG_BOGUS		CMDF_BOGUS
+#define TRIG_RT			CMDF_PRIORITY
+#define TRIG_WAKE_EOS		CMDF_WAKE_EOS
+#define TRIG_WRITE		CMDF_WRITE
+#define TRIG_ROUND_MASK		CMDF_ROUND_MASK
+#define TRIG_ROUND_NEAREST	CMDF_ROUND_NEAREST
+#define TRIG_ROUND_DOWN		CMDF_ROUND_DOWN
+#define TRIG_ROUND_UP		CMDF_ROUND_UP
+#define TRIG_ROUND_UP_NEXT	CMDF_ROUND_UP_NEXT
 
 /* trigger sources */
 
diff --git a/drivers/staging/comedi/comedi_buf.c b/drivers/staging/comedi/comedi_buf.c
index df4a9c4bca35..c60a45ad12b9 100644
--- a/drivers/staging/comedi/comedi_buf.c
+++ b/drivers/staging/comedi/comedi_buf.c
@@ -3,6 +3,7 @@
  *
  * COMEDI - Linux Control and Measurement Device Interface
  * Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+ * Copyright (C) 2002 Frank Mori Hess <fmhess@users.sourceforge.net>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License as published by
@@ -509,3 +510,68 @@ void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
 	}
 }
 EXPORT_SYMBOL_GPL(comedi_buf_memcpy_from);
+
+/**
+ * comedi_write_array_to_buffer - write data to comedi buffer
+ * @s: comedi_subdevice struct
+ * @data: destination
+ * @num_bytes: number of bytes to write
+ *
+ * Writes up to num_bytes bytes of data to the comedi buffer associated with
+ * the subdevice, marks it as written and updates the acquisition scan
+ * progress.
+ *
+ * Returns the amount of data written in bytes.
+ */
+unsigned int comedi_write_array_to_buffer(struct comedi_subdevice *s,
+					  const void *data,
+					  unsigned int num_bytes)
+{
+	struct comedi_async *async = s->async;
+	unsigned int retval;
+
+	if (num_bytes == 0)
+		return 0;
+
+	retval = comedi_buf_write_alloc(s, num_bytes);
+	if (retval != num_bytes) {
+		dev_warn(s->device->class_dev, "buffer overrun\n");
+		async->events |= COMEDI_CB_OVERFLOW;
+		return 0;
+	}
+
+	comedi_buf_memcpy_to(s, 0, data, num_bytes);
+	comedi_buf_write_free(s, num_bytes);
+	comedi_inc_scan_progress(s, num_bytes);
+	async->events |= COMEDI_CB_BLOCK;
+
+	return num_bytes;
+}
+EXPORT_SYMBOL_GPL(comedi_write_array_to_buffer);
+
+/**
+ * comedi_read_array_from_buffer - read data from comedi buffer
+ * @s: comedi_subdevice struct
+ * @data: destination
+ * @num_bytes: number of bytes to read
+ *
+ * Reads up to num_bytes bytes of data from the comedi buffer associated with
+ * the subdevice, marks it as read and updates the acquisition scan progress.
+ *
+ * Returns the amount of data read in bytes.
+ */
+unsigned int comedi_read_array_from_buffer(struct comedi_subdevice *s,
+					   void *data, unsigned int num_bytes)
+{
+	if (num_bytes == 0)
+		return 0;
+
+	num_bytes = comedi_buf_read_alloc(s, num_bytes);
+	comedi_buf_memcpy_from(s, 0, data, num_bytes);
+	comedi_buf_read_free(s, num_bytes);
+	comedi_inc_scan_progress(s, num_bytes);
+	s->async->events |= COMEDI_CB_BLOCK;
+
+	return num_bytes;
+}
+EXPORT_SYMBOL_GPL(comedi_read_array_from_buffer);
diff --git a/drivers/staging/comedi/comedi_fops.c b/drivers/staging/comedi/comedi_fops.c
index 2182c7463cdb..495969f46e76 100644
--- a/drivers/staging/comedi/comedi_fops.c
+++ b/drivers/staging/comedi/comedi_fops.c
@@ -1532,7 +1532,7 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 
 	ret = s->do_cmdtest(dev, s, &async->cmd);
 
-	if (async->cmd.flags & TRIG_BOGUS || ret) {
+	if (async->cmd.flags & CMDF_BOGUS || ret) {
 		dev_dbg(dev->class_dev, "test returned %d\n", ret);
 		cmd = async->cmd;
 		/* restore chanlist pointer before copying back */
@@ -1558,7 +1558,7 @@ static int do_cmd_ioctl(struct comedi_device *dev,
 	async->cb_mask =
 	    COMEDI_CB_EOA | COMEDI_CB_BLOCK | COMEDI_CB_ERROR |
 	    COMEDI_CB_OVERFLOW;
-	if (async->cmd.flags & TRIG_WAKE_EOS)
+	if (async->cmd.flags & CMDF_WAKE_EOS)
 		async->cb_mask |= COMEDI_CB_EOS;
 
 	comedi_set_subdevice_runflags(s, SRF_ERROR | SRF_RUNNING, SRF_RUNNING);
diff --git a/drivers/staging/comedi/comedi_pci.c b/drivers/staging/comedi/comedi_pci.c
index abbc0e4f5c51..aa0795a2660e 100644
--- a/drivers/staging/comedi/comedi_pci.c
+++ b/drivers/staging/comedi/comedi_pci.c
@@ -17,6 +17,7 @@
  */
 
 #include <linux/pci.h>
+#include <linux/interrupt.h>
 
 #include "comedidev.h"
 
@@ -73,6 +74,29 @@ void comedi_pci_disable(struct comedi_device *dev)
 EXPORT_SYMBOL_GPL(comedi_pci_disable);
 
 /**
+ * comedi_pci_detach() - A generic (*detach) function for PCI drivers.
+ * @dev: comedi_device struct
+ */
+void comedi_pci_detach(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+
+	if (!pcidev || !dev->ioenabled)
+		return;
+
+	if (dev->irq) {
+		free_irq(dev->irq, dev);
+		dev->irq = 0;
+	}
+	if (dev->mmio) {
+		iounmap(dev->mmio);
+		dev->mmio = NULL;
+	}
+	comedi_pci_disable(dev);
+}
+EXPORT_SYMBOL_GPL(comedi_pci_detach);
+
+/**
  * comedi_pci_auto_config() - Configure/probe a comedi PCI driver.
  * @pcidev: pci_dev struct
  * @driver: comedi_driver struct
diff --git a/drivers/staging/comedi/comedidev.h b/drivers/staging/comedi/comedidev.h
index 58e58a32e93d..1b2bbd56f6ef 100644
--- a/drivers/staging/comedi/comedidev.h
+++ b/drivers/staging/comedi/comedidev.h
@@ -88,6 +88,8 @@ struct comedi_subdevice {
 
 	struct device *class_dev;
 	int minor;
+
+	unsigned int *readback;
 };
 
 struct comedi_buf_page {
@@ -267,11 +269,6 @@ struct comedi_device {
 	void (*close)(struct comedi_device *dev);
 };
 
-static inline const void *comedi_board(const struct comedi_device *dev)
-{
-	return dev->board_ptr;
-}
-
 /*
  * function prototypes
  */
@@ -429,6 +426,11 @@ void comedi_buf_memcpy_to(struct comedi_subdevice *s, unsigned int offset,
 			  const void *source, unsigned int num_bytes);
 void comedi_buf_memcpy_from(struct comedi_subdevice *s, unsigned int offset,
 			    void *destination, unsigned int num_bytes);
+unsigned int comedi_write_array_to_buffer(struct comedi_subdevice *s,
+					  const void *data,
+					  unsigned int num_bytes);
+unsigned int comedi_read_array_from_buffer(struct comedi_subdevice *s,
+					   void *data, unsigned int num_bytes);
 
 /* drivers.c - general comedi driver functions */
 
@@ -440,14 +442,24 @@ int comedi_timeout(struct comedi_device *, struct comedi_subdevice *,
 			     struct comedi_insn *, unsigned long context),
 		   unsigned long context);
 
+unsigned int comedi_handle_events(struct comedi_device *dev,
+				  struct comedi_subdevice *s);
+
 int comedi_dio_insn_config(struct comedi_device *, struct comedi_subdevice *,
 			   struct comedi_insn *, unsigned int *data,
 			   unsigned int mask);
 unsigned int comedi_dio_update_state(struct comedi_subdevice *,
 				     unsigned int *data);
+unsigned int comedi_bytes_per_scan(struct comedi_subdevice *s);
+void comedi_inc_scan_progress(struct comedi_subdevice *s,
+			      unsigned int num_bytes);
 
 void *comedi_alloc_devpriv(struct comedi_device *, size_t);
 int comedi_alloc_subdevices(struct comedi_device *, int);
+int comedi_alloc_subdev_readback(struct comedi_subdevice *);
+
+int comedi_readback_insn_read(struct comedi_device *, struct comedi_subdevice *,
+			      struct comedi_insn *, unsigned int *data);
 
 int comedi_load_firmware(struct comedi_device *, struct device *,
 			 const char *name,
@@ -503,6 +515,7 @@ struct pci_dev *comedi_to_pci_dev(struct comedi_device *);
 
 int comedi_pci_enable(struct comedi_device *);
 void comedi_pci_disable(struct comedi_device *);
+void comedi_pci_detach(struct comedi_device *);
 
 int comedi_pci_auto_config(struct pci_dev *, struct comedi_driver *,
 			   unsigned long context);
@@ -547,6 +560,10 @@ static inline void comedi_pci_disable(struct comedi_device *dev)
 {
 }
 
+static inline void comedi_pci_detach(struct comedi_device *dev)
+{
+}
+
 #endif /* CONFIG_COMEDI_PCI_DRIVERS */
 
 #ifdef CONFIG_COMEDI_PCMCIA_DRIVERS
diff --git a/drivers/staging/comedi/drivers.c b/drivers/staging/comedi/drivers.c
index 9ada130f2a76..3e5bccbc9c39 100644
--- a/drivers/staging/comedi/drivers.c
+++ b/drivers/staging/comedi/drivers.c
@@ -4,6 +4,7 @@
 
     COMEDI - Linux Control and Measurement Device Interface
     Copyright (C) 1997-2000 David A. Schleef <ds@schleef.org>
+    Copyright (C) 2002 Frank Mori Hess <fmhess@users.sourceforge.net>
 
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -96,6 +97,22 @@ int comedi_alloc_subdevices(struct comedi_device *dev, int num_subdevices)
 }
 EXPORT_SYMBOL_GPL(comedi_alloc_subdevices);
 
+/**
+ * comedi_alloc_subdev_readback() - Allocate memory for the subdevice readback.
+ * @s: comedi_subdevice struct
+ */
+int comedi_alloc_subdev_readback(struct comedi_subdevice *s)
+{
+	if (!s->n_chan)
+		return -EINVAL;
+
+	s->readback = kcalloc(s->n_chan, sizeof(*s->readback), GFP_KERNEL);
+	if (!s->readback)
+		return -ENOMEM;
+	return 0;
+}
+EXPORT_SYMBOL_GPL(comedi_alloc_subdev_readback);
+
 static void comedi_device_detach_cleanup(struct comedi_device *dev)
 {
 	int i;
@@ -111,6 +128,7 @@ static void comedi_device_detach_cleanup(struct comedi_device *dev)
 				comedi_buf_alloc(dev, s, 0);
 				kfree(s->async);
 			}
+			kfree(s->readback);
 		}
 		kfree(dev->subdevices);
 		dev->subdevices = NULL;
@@ -157,6 +175,31 @@ int insn_inval(struct comedi_device *dev, struct comedi_subdevice *s,
 }
 
 /**
+ * comedi_readback_insn_read() - A generic (*insn_read) for subdevice readback.
+ * @dev: comedi_device struct
+ * @s: comedi_subdevice struct
+ * @insn: comedi_insn struct
+ * @data: pointer to return the readback data
+ */
+int comedi_readback_insn_read(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
+
+	if (!s->readback)
+		return -EINVAL;
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = s->readback[chan];
+
+	return insn->n;
+}
+EXPORT_SYMBOL_GPL(comedi_readback_insn_read);
+
+/**
  * comedi_timeout() - busy-wait for a driver condition to occur.
  * @dev: comedi_device struct
  * @s: comedi_subdevice struct
@@ -248,6 +291,100 @@ unsigned int comedi_dio_update_state(struct comedi_subdevice *s,
 }
 EXPORT_SYMBOL_GPL(comedi_dio_update_state);
 
+/**
+ * comedi_bytes_per_scan - get length of asynchronous command "scan" in bytes
+ * @s: comedi_subdevice struct
+ *
+ * Determines the overall scan length according to the subdevice type and the
+ * number of channels in the scan.
+ *
+ * For digital input, output or input/output subdevices, samples for multiple
+ * channels are assumed to be packed into one or more unsigned short or
+ * unsigned int values according to the subdevice's SDF_LSAMPL flag.  For other
+ * types of subdevice, samples are assumed to occupy a whole unsigned short or
+ * unsigned int according to the SDF_LSAMPL flag.
+ *
+ * Returns the overall scan length in bytes.
+ */
+unsigned int comedi_bytes_per_scan(struct comedi_subdevice *s)
+{
+	struct comedi_cmd *cmd = &s->async->cmd;
+	unsigned int num_samples;
+	unsigned int bits_per_sample;
+
+	switch (s->type) {
+	case COMEDI_SUBD_DI:
+	case COMEDI_SUBD_DO:
+	case COMEDI_SUBD_DIO:
+		bits_per_sample = 8 * bytes_per_sample(s);
+		num_samples = (cmd->chanlist_len + bits_per_sample - 1) /
+				bits_per_sample;
+		break;
+	default:
+		num_samples = cmd->chanlist_len;
+		break;
+	}
+	return num_samples * bytes_per_sample(s);
+}
+EXPORT_SYMBOL_GPL(comedi_bytes_per_scan);
+
+/**
+ * comedi_inc_scan_progress - update scan progress in asynchronous command
+ * @s: comedi_subdevice struct
+ * @num_bytes: amount of data in bytes to increment scan progress
+ *
+ * Increments the scan progress by the number of bytes specified by num_bytes.
+ * If the scan progress reaches or exceeds the scan length in bytes, reduce
+ * it modulo the scan length in bytes and set the "end of scan" asynchronous
+ * event flag to be processed later.
+ */
+void comedi_inc_scan_progress(struct comedi_subdevice *s,
+			      unsigned int num_bytes)
+{
+	struct comedi_async *async = s->async;
+	unsigned int scan_length = comedi_bytes_per_scan(s);
+
+	async->scan_progress += num_bytes;
+	if (async->scan_progress >= scan_length) {
+		async->scan_progress %= scan_length;
+		async->events |= COMEDI_CB_EOS;
+	}
+}
+EXPORT_SYMBOL_GPL(comedi_inc_scan_progress);
+
+/**
+ * comedi_handle_events - handle events and possibly stop acquisition
+ * @dev: comedi_device struct
+ * @s: comedi_subdevice struct
+ *
+ * Handles outstanding asynchronous acquisition event flags associated
+ * with the subdevice.  Call the subdevice's "->cancel()" handler if the
+ * "end of acquisition", "error" or "overflow" event flags are set in order
+ * to stop the acquisition at the driver level.
+ *
+ * Calls comedi_event() to further process the event flags, which may mark
+ * the asynchronous command as no longer running, possibly terminated with
+ * an error, and may wake up tasks.
+ *
+ * Return a bit-mask of the handled events.
+ */
+unsigned int comedi_handle_events(struct comedi_device *dev,
+				  struct comedi_subdevice *s)
+{
+	unsigned int events = s->async->events;
+
+	if (events == 0)
+		return events;
+
+	if (events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW))
+		s->cancel(dev, s);
+
+	comedi_event(dev, s);
+
+	return events;
+}
+EXPORT_SYMBOL_GPL(comedi_handle_events);
+
 static int insn_rw_emulate_bits(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
diff --git a/drivers/staging/comedi/drivers/8253.h b/drivers/staging/comedi/drivers/8253.h
index f8e1ebad304d..9f4c1411719d 100644
--- a/drivers/staging/comedi/drivers/8253.h
+++ b/drivers/staging/comedi/drivers/8253.h
@@ -90,8 +90,8 @@ static inline void i8253_cascade_ns_to_timer(int i8253_osc_base,
 		}
 	}
 
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_NEAREST:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
 	default:
 		ns_high = div1_lub * div2_lub * i8253_osc_base;
 		ns_low = div1_glb * div2_glb * i8253_osc_base;
@@ -103,11 +103,11 @@ static inline void i8253_cascade_ns_to_timer(int i8253_osc_base,
 			div2 = div2_glb;
 		}
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		div1 = div1_lub;
 		div2 = div2_lub;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		div1 = div1_glb;
 		div2 = div2_glb;
 		break;
diff --git a/drivers/staging/comedi/drivers/8255.c b/drivers/staging/comedi/drivers/8255.c
index a33a19622745..34d4d8b5f31e 100644
--- a/drivers/staging/comedi/drivers/8255.c
+++ b/drivers/staging/comedi/drivers/8255.c
@@ -79,49 +79,30 @@ I/O port base address can be found in the output of 'lspci -v'.
 #include "comedi_fc.h"
 #include "8255.h"
 
-#define _8255_SIZE	4
-
-#define _8255_DATA	0
-#define _8255_CR	3
-
-#define CR_C_LO_IO	0x01
-#define CR_B_IO		0x02
-#define CR_B_MODE	0x04
-#define CR_C_HI_IO	0x08
-#define CR_A_IO		0x10
-#define CR_A_MODE(a)	((a)<<5)
-#define CR_CW		0x80
-
 struct subdev_8255_private {
-	unsigned long iobase;
-	int (*io)(int, int, int, unsigned long);
+	unsigned long regbase;
+	int (*io)(struct comedi_device *, int, int, int, unsigned long);
 };
 
-static int subdev_8255_io(int dir, int port, int data, unsigned long iobase)
+static int subdev_8255_io(struct comedi_device *dev,
+			  int dir, int port, int data, unsigned long regbase)
 {
 	if (dir) {
-		outb(data, iobase + port);
+		outb(data, dev->iobase + regbase + port);
 		return 0;
 	}
-	return inb(iobase + port);
+	return inb(dev->iobase + regbase + port);
 }
 
-void subdev_8255_interrupt(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
+static int subdev_8255_mmio(struct comedi_device *dev,
+			    int dir, int port, int data, unsigned long regbase)
 {
-	struct subdev_8255_private *spriv = s->private;
-	unsigned long iobase = spriv->iobase;
-	unsigned short d;
-
-	d = spriv->io(0, _8255_DATA, 0, iobase);
-	d |= (spriv->io(0, _8255_DATA + 1, 0, iobase) << 8);
-
-	comedi_buf_put(s, d);
-	s->async->events |= COMEDI_CB_EOS;
-
-	comedi_event(dev, s);
+	if (dir) {
+		writeb(data, dev->mmio + regbase + port);
+		return 0;
+	}
+	return readb(dev->mmio + regbase + port);
 }
-EXPORT_SYMBOL_GPL(subdev_8255_interrupt);
 
 static int subdev_8255_insn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
@@ -129,25 +110,26 @@ static int subdev_8255_insn(struct comedi_device *dev,
 			    unsigned int *data)
 {
 	struct subdev_8255_private *spriv = s->private;
-	unsigned long iobase = spriv->iobase;
+	unsigned long regbase = spriv->regbase;
 	unsigned int mask;
 	unsigned int v;
 
 	mask = comedi_dio_update_state(s, data);
 	if (mask) {
 		if (mask & 0xff)
-			spriv->io(1, _8255_DATA, s->state & 0xff, iobase);
+			spriv->io(dev, 1, I8255_DATA_A_REG,
+				  s->state & 0xff, regbase);
 		if (mask & 0xff00)
-			spriv->io(1, _8255_DATA + 1, (s->state >> 8) & 0xff,
-				  iobase);
+			spriv->io(dev, 1, I8255_DATA_B_REG,
+				  (s->state >> 8) & 0xff, regbase);
 		if (mask & 0xff0000)
-			spriv->io(1, _8255_DATA + 2, (s->state >> 16) & 0xff,
-				  iobase);
+			spriv->io(dev, 1, I8255_DATA_C_REG,
+				  (s->state >> 16) & 0xff, regbase);
 	}
 
-	v = spriv->io(0, _8255_DATA, 0, iobase);
-	v |= (spriv->io(0, _8255_DATA + 1, 0, iobase) << 8);
-	v |= (spriv->io(0, _8255_DATA + 2, 0, iobase) << 16);
+	v = spriv->io(dev, 0, I8255_DATA_A_REG, 0, regbase);
+	v |= (spriv->io(dev, 0, I8255_DATA_B_REG, 0, regbase) << 8);
+	v |= (spriv->io(dev, 0, I8255_DATA_C_REG, 0, regbase) << 16);
 
 	data[1] = v;
 
@@ -158,21 +140,21 @@ static void subdev_8255_do_config(struct comedi_device *dev,
 				  struct comedi_subdevice *s)
 {
 	struct subdev_8255_private *spriv = s->private;
-	unsigned long iobase = spriv->iobase;
+	unsigned long regbase = spriv->regbase;
 	int config;
 
-	config = CR_CW;
+	config = I8255_CTRL_CW;
 	/* 1 in io_bits indicates output, 1 in config indicates input */
 	if (!(s->io_bits & 0x0000ff))
-		config |= CR_A_IO;
+		config |= I8255_CTRL_A_IO;
 	if (!(s->io_bits & 0x00ff00))
-		config |= CR_B_IO;
+		config |= I8255_CTRL_B_IO;
 	if (!(s->io_bits & 0x0f0000))
-		config |= CR_C_LO_IO;
+		config |= I8255_CTRL_C_LO_IO;
 	if (!(s->io_bits & 0xf00000))
-		config |= CR_C_HI_IO;
+		config |= I8255_CTRL_C_HI_IO;
 
-	spriv->io(1, _8255_CR, config, iobase);
+	spriv->io(dev, 1, I8255_CTRL_REG, config, regbase);
 }
 
 static int subdev_8255_insn_config(struct comedi_device *dev,
@@ -202,67 +184,12 @@ static int subdev_8255_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int subdev_8255_cmdtest(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_cmd *cmd)
-{
-	int err = 0;
-
-	/* Step 1 : check if triggers are trivially valid */
-
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_FOLLOW);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_NONE);
-
-	if (err)
-		return 1;
-
-	/* Step 2a : make sure trigger sources are unique */
-	/* Step 2b : and mutually compatible */
-
-	if (err)
-		return 2;
-
-	/* Step 3: check if arguments are trivially valid */
-
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-
-	if (err)
-		return 3;
-
-	/* step 4 */
-
-	if (err)
-		return 4;
-
-	return 0;
-}
-
-static int subdev_8255_cmd(struct comedi_device *dev,
-			   struct comedi_subdevice *s)
-{
-	/* FIXME */
-
-	return 0;
-}
-
-static int subdev_8255_cancel(struct comedi_device *dev,
-			      struct comedi_subdevice *s)
-{
-	/* FIXME */
-
-	return 0;
-}
-
-int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		     int (*io)(int, int, int, unsigned long),
-		     unsigned long iobase)
+static int __subdev_8255_init(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      int (*io)(struct comedi_device *,
+					int, int, int, unsigned long),
+			      unsigned long regbase,
+			      bool is_mmio)
 {
 	struct subdev_8255_private *spriv;
 
@@ -270,8 +197,13 @@ int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
 	if (!spriv)
 		return -ENOMEM;
 
-	spriv->iobase	= iobase;
-	spriv->io	= io ? io : subdev_8255_io;
+	if (io)
+		spriv->io = io;
+	else if (is_mmio)
+		spriv->io = subdev_8255_mmio;
+	else
+		spriv->io = subdev_8255_io;
+	spriv->regbase	= regbase;
 
 	s->type		= COMEDI_SUBD_DIO;
 	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
@@ -285,27 +217,24 @@ int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(subdev_8255_init);
 
-int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
-			 int (*io)(int, int, int, unsigned long),
-			 unsigned long iobase)
+int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
+		     int (*io)(struct comedi_device *,
+			       int, int, int, unsigned long),
+		     unsigned long regbase)
 {
-	int ret;
-
-	ret = subdev_8255_init(dev, s, io, iobase);
-	if (ret)
-		return ret;
-
-	s->len_chanlist	= 1;
-	s->do_cmdtest	= subdev_8255_cmdtest;
-	s->do_cmd	= subdev_8255_cmd;
-	s->cancel	= subdev_8255_cancel;
-
-	return 0;
+	return __subdev_8255_init(dev, s, io, regbase, false);
 }
-EXPORT_SYMBOL_GPL(subdev_8255_init_irq);
+EXPORT_SYMBOL_GPL(subdev_8255_init);
 
+int subdev_8255_mm_init(struct comedi_device *dev, struct comedi_subdevice *s,
+			int (*io)(struct comedi_device *,
+				  int, int, int, unsigned long),
+			unsigned long regbase)
+{
+	return __subdev_8255_init(dev, s, io, regbase, true);
+}
+EXPORT_SYMBOL_GPL(subdev_8255_mm_init);
 /*
 
    Start of the 8255 standalone device
@@ -316,8 +245,8 @@ static int dev_8255_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
 	struct comedi_subdevice *s;
-	int ret;
 	unsigned long iobase;
+	int ret;
 	int i;
 
 	for (i = 0; i < COMEDI_NDEVCONFOPTS; i++) {
@@ -338,7 +267,14 @@ static int dev_8255_attach(struct comedi_device *dev,
 		s = &dev->subdevices[i];
 		iobase = it->options[i];
 
-		ret = __comedi_request_region(dev, iobase, _8255_SIZE);
+		/*
+		 * __comedi_request_region() does not set dev->iobase.
+		 *
+		 * For 8255 devices that are manually attached using
+		 * comedi_config, the 'iobase' is the actual I/O port
+		 * base address of the chip.
+		 */
+		ret = __comedi_request_region(dev, iobase, I8255_SIZE);
 		if (ret) {
 			s->type = COMEDI_SUBD_UNUSED;
 		} else {
@@ -361,7 +297,7 @@ static void dev_8255_detach(struct comedi_device *dev)
 		s = &dev->subdevices[i];
 		if (s->type != COMEDI_SUBD_UNUSED) {
 			spriv = s->private;
-			release_region(spriv->iobase, _8255_SIZE);
+			release_region(spriv->regbase, I8255_SIZE);
 		}
 	}
 }
diff --git a/drivers/staging/comedi/drivers/8255.h b/drivers/staging/comedi/drivers/8255.h
index 795d232a6c02..5985c8e0330f 100644
--- a/drivers/staging/comedi/drivers/8255.h
+++ b/drivers/staging/comedi/drivers/8255.h
@@ -21,13 +21,28 @@
 
 #include "../comedidev.h"
 
-int subdev_8255_init(struct comedi_device *dev, struct comedi_subdevice *s,
-		     int (*io)(int, int, int, unsigned long),
-		     unsigned long iobase);
-int subdev_8255_init_irq(struct comedi_device *dev, struct comedi_subdevice *s,
-			 int (*io)(int, int, int, unsigned long),
-			 unsigned long iobase);
-void subdev_8255_interrupt(struct comedi_device *dev,
-			   struct comedi_subdevice *s);
+#define I8255_SIZE		0x04
+
+#define I8255_DATA_A_REG	0x00
+#define I8255_DATA_B_REG	0x01
+#define I8255_DATA_C_REG	0x02
+#define I8255_CTRL_REG		0x03
+#define I8255_CTRL_C_LO_IO	(1 << 0)
+#define I8255_CTRL_B_IO		(1 << 1)
+#define I8255_CTRL_B_MODE	(1 << 2)
+#define I8255_CTRL_C_HI_IO	(1 << 3)
+#define I8255_CTRL_A_IO		(1 << 4)
+#define I8255_CTRL_A_MODE(x)	((x) << 5)
+#define I8255_CTRL_CW		(1 << 7)
+
+int subdev_8255_init(struct comedi_device *, struct comedi_subdevice *,
+		     int (*io)(struct comedi_device *,
+			       int, int, int, unsigned long),
+		     unsigned long regbase);
+
+int subdev_8255_mm_init(struct comedi_device *, struct comedi_subdevice *,
+			int (*io)(struct comedi_device *,
+				  int, int, int, unsigned long),
+			unsigned long regbase);
 
 #endif
diff --git a/drivers/staging/comedi/drivers/8255_pci.c b/drivers/staging/comedi/drivers/8255_pci.c
index f21e6567ac2f..8b9589828855 100644
--- a/drivers/staging/comedi/drivers/8255_pci.c
+++ b/drivers/staging/comedi/drivers/8255_pci.c
@@ -190,24 +190,12 @@ static int pci_8255_mite_init(struct pci_dev *pcidev)
 	return 0;
 }
 
-static int pci_8255_mmio(int dir, int port, int data, unsigned long iobase)
-{
-	void __iomem *mmio_base = (void __iomem *)iobase;
-
-	if (dir) {
-		writeb(data, mmio_base + port);
-		return 0;
-	}
-	return readb(mmio_base  + port);
-}
-
 static int pci_8255_auto_attach(struct comedi_device *dev,
 				unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	const struct pci_8255_boardinfo *board = NULL;
 	struct comedi_subdevice *s;
-	bool is_mmio;
 	int ret;
 	int i;
 
@@ -228,9 +216,7 @@ static int pci_8255_auto_attach(struct comedi_device *dev,
 			return ret;
 	}
 
-	is_mmio = (pci_resource_flags(pcidev, board->dio_badr) &
-		   IORESOURCE_MEM) != 0;
-	if (is_mmio) {
+	if ((pci_resource_flags(pcidev, board->dio_badr) & IORESOURCE_MEM)) {
 		dev->mmio = pci_ioremap_bar(pcidev, board->dio_badr);
 		if (!dev->mmio)
 			return -ENOMEM;
@@ -248,16 +234,11 @@ static int pci_8255_auto_attach(struct comedi_device *dev,
 		return ret;
 
 	for (i = 0; i < board->n_8255; i++) {
-		unsigned long iobase;
-
 		s = &dev->subdevices[i];
-		if (is_mmio) {
-			iobase = (unsigned long)(dev->mmio + (i * 4));
-			ret = subdev_8255_init(dev, s, pci_8255_mmio, iobase);
-		} else {
-			iobase = dev->iobase + (i * 4);
-			ret = subdev_8255_init(dev, s, NULL, iobase);
-		}
+		if (dev->mmio)
+			ret = subdev_8255_mm_init(dev, s, NULL, i * I8255_SIZE);
+		else
+			ret = subdev_8255_init(dev, s, NULL, i * I8255_SIZE);
 		if (ret)
 			return ret;
 	}
@@ -265,18 +246,11 @@ static int pci_8255_auto_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void pci_8255_detach(struct comedi_device *dev)
-{
-	if (dev->mmio)
-		iounmap(dev->mmio);
-	comedi_pci_disable(dev);
-}
-
 static struct comedi_driver pci_8255_driver = {
 	.driver_name	= "8255_pci",
 	.module		= THIS_MODULE,
 	.auto_attach	= pci_8255_auto_attach,
-	.detach		= pci_8255_detach,
+	.detach		= comedi_pci_detach,
 };
 
 static int pci_8255_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/Makefile b/drivers/staging/comedi/drivers/Makefile
index 8873d4807a01..6bc9ef3b25b3 100644
--- a/drivers/staging/comedi/drivers/Makefile
+++ b/drivers/staging/comedi/drivers/Makefile
@@ -9,7 +9,6 @@ obj-$(CONFIG_COMEDI_BOND)		+= comedi_bond.o
 obj-$(CONFIG_COMEDI_TEST)		+= comedi_test.o
 obj-$(CONFIG_COMEDI_PARPORT)		+= comedi_parport.o
 obj-$(CONFIG_COMEDI_SERIAL2002)		+= serial2002.o
-obj-$(CONFIG_COMEDI_SKEL)		+= skel.o
 
 # Comedi ISA drivers
 obj-$(CONFIG_COMEDI_AMPLC_DIO200_ISA)	+= amplc_dio200.o
@@ -50,6 +49,7 @@ obj-$(CONFIG_COMEDI_NI_AT_A2150)	+= ni_at_a2150.o
 obj-$(CONFIG_COMEDI_NI_AT_AO)		+= ni_at_ao.o
 obj-$(CONFIG_COMEDI_NI_ATMIO)		+= ni_atmio.o
 obj-$(CONFIG_COMEDI_NI_ATMIO16D)	+= ni_atmio16d.o
+obj-$(CONFIG_COMEDI_NI_LABPC_ISA)	+= ni_labpc.o
 obj-$(CONFIG_COMEDI_PCMAD)		+= pcmad.o
 obj-$(CONFIG_COMEDI_PCMDA12)		+= pcmda12.o
 obj-$(CONFIG_COMEDI_PCMMIO)		+= pcmmio.o
@@ -125,6 +125,7 @@ obj-$(CONFIG_COMEDI_QUATECH_DAQP_CS)	+= quatech_daqp_cs.o
 
 # Comedi USB drivers
 obj-$(CONFIG_COMEDI_DT9812)		+= dt9812.o
+obj-$(CONFIG_COMEDI_NI_USB6501)		+= ni_usb6501.o
 obj-$(CONFIG_COMEDI_USBDUX)		+= usbdux.o
 obj-$(CONFIG_COMEDI_USBDUXFAST)		+= usbduxfast.o
 obj-$(CONFIG_COMEDI_USBDUXSIGMA)	+= usbduxsigma.o
@@ -134,11 +135,10 @@ obj-$(CONFIG_COMEDI_VMK80XX)		+= vmk80xx.o
 obj-$(CONFIG_COMEDI_MITE)		+= mite.o
 obj-$(CONFIG_COMEDI_NI_TIO)		+= ni_tio.o
 obj-$(CONFIG_COMEDI_NI_TIOCMD)		+= ni_tiocmd.o
-obj-$(CONFIG_COMEDI_NI_LABPC)		+= ni_labpc.o
+obj-$(CONFIG_COMEDI_NI_LABPC)		+= ni_labpc_common.o
 obj-$(CONFIG_COMEDI_NI_LABPC_ISADMA)	+= ni_labpc_isadma.o
 
 obj-$(CONFIG_COMEDI_8255)		+= 8255.o
 obj-$(CONFIG_COMEDI_AMPLC_DIO200)	+= amplc_dio200_common.o
 obj-$(CONFIG_COMEDI_AMPLC_PC236)	+= amplc_pc236_common.o
 obj-$(CONFIG_COMEDI_DAS08)		+= das08.o
-obj-$(CONFIG_COMEDI_FC)			+= comedi_fc.o
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.c b/drivers/staging/comedi/drivers/addi-data/addi_common.c
index de5843ab01ae..2e7fb218340f 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.c
@@ -44,7 +44,7 @@ static int i_ADDIDATA_InsnReadEeprom(struct comedi_device *dev,
 				     struct comedi_insn *insn,
 				     unsigned int *data)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	unsigned short w_Address = CR_CHAN(insn->chanspec);
 	unsigned short w_Data;
@@ -59,7 +59,7 @@ static int i_ADDIDATA_InsnReadEeprom(struct comedi_device *dev,
 static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 
 	this_board->interrupt(irq, d);
 	return IRQ_RETVAL(1);
@@ -67,7 +67,7 @@ static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
 
 static int i_ADDI_Reset(struct comedi_device *dev)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 
 	this_board->reset(dev);
 	return 0;
@@ -77,7 +77,7 @@ static int addi_auto_attach(struct comedi_device *dev,
 				      unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret, n_subdevices;
@@ -268,13 +268,7 @@ static int addi_auto_attach(struct comedi_device *dev,
 
 static void i_ADDI_Detach(struct comedi_device *dev)
 {
-	struct addi_private *devpriv = dev->private;
-
-	if (devpriv) {
-		if (dev->iobase)
-			i_ADDI_Reset(dev);
-		if (dev->irq)
-			free_irq(dev->irq, dev);
-	}
-	comedi_pci_disable(dev);
+	if (dev->iobase)
+		i_ADDI_Reset(dev);
+	comedi_pci_detach(dev);
 }
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_common.h b/drivers/staging/comedi/drivers/addi-data/addi_common.h
index a7400a25f620..e2a3ffeee5cf 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_common.h
+++ b/drivers/staging/comedi/drivers/addi-data/addi_common.h
@@ -107,10 +107,9 @@ struct addi_private {
 	unsigned char b_DmaDoubleBuffer;	/*  we can use double buffering */
 	unsigned int ui_DmaActualBuffer;	/*  which buffer is used now */
 	unsigned short *ul_DmaBufferVirtual[2];	/*  pointers to DMA buffer */
-	unsigned int ul_DmaBufferHw[2];	/*  hw address of DMA buff */
+	dma_addr_t ul_DmaBufferHw[2];		/*  hw address of DMA buff */
 	unsigned int ui_DmaBufferSize[2];	/*  size of dma buffer in bytes */
 	unsigned int ui_DmaBufferUsesize[2];	/*  which size we may now used for transfer */
-	unsigned int ui_DmaBufferPages[2];	/*  number of pages in buffer */
 	unsigned char b_DigitalOutputRegister;	/*  Digital Output Register */
 	unsigned char b_OutputMemoryStatus;
 	unsigned char b_TimerSelectMode;	/*  Contain data written at iobase + 0C */
diff --git a/drivers/staging/comedi/drivers/addi-data/addi_eeprom.c b/drivers/staging/comedi/drivers/addi-data/addi_eeprom.c
index aafc172f3a98..b731856c27da 100644
--- a/drivers/staging/comedi/drivers/addi-data/addi_eeprom.c
+++ b/drivers/staging/comedi/drivers/addi-data/addi_eeprom.c
@@ -179,7 +179,7 @@ static void addi_eeprom_read_di_info(struct comedi_device *dev,
 				     unsigned long iobase,
 				     unsigned short addr)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	char *type = this_board->pc_EepromChip;
 	unsigned short tmp;
@@ -200,7 +200,7 @@ static void addi_eeprom_read_do_info(struct comedi_device *dev,
 				     unsigned long iobase,
 				     unsigned short addr)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	char *type = this_board->pc_EepromChip;
 	unsigned short tmp;
@@ -218,7 +218,7 @@ static void addi_eeprom_read_timer_info(struct comedi_device *dev,
 {
 	struct addi_private *devpriv = dev->private;
 #if 0
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	char *type = this_board->pc_EepromChip;
 	unsigned short offset = 0;
 	unsigned short ntimers;
@@ -259,7 +259,7 @@ static void addi_eeprom_read_ao_info(struct comedi_device *dev,
 				     unsigned long iobase,
 				     unsigned short addr)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	char *type = this_board->pc_EepromChip;
 	unsigned short tmp;
@@ -278,7 +278,7 @@ static void addi_eeprom_read_ai_info(struct comedi_device *dev,
 				     unsigned long iobase,
 				     unsigned short addr)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	char *type = this_board->pc_EepromChip;
 	unsigned short offset;
@@ -315,7 +315,7 @@ static void addi_eeprom_read_ai_info(struct comedi_device *dev,
 static void addi_eeprom_read_info(struct comedi_device *dev,
 				  unsigned long iobase)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	char *type = this_board->pc_EepromChip;
 	unsigned short size;
 	unsigned char nfuncs;
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci035.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci035.c
index cad33f1a04fe..53bb51bd77b5 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci035.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci035.c
@@ -477,6 +477,4 @@ static void apci035_interrupt(int irq, void *d)
 		/*  send signal to the sample */
 		send_sig(SIGIO, devpriv->tsk_Current, 0);
 	}
-
-	return;
 }
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c
index 1e2fe66818e4..0ea081e1e119 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1500.c
@@ -172,17 +172,15 @@ static int apci1500_di_config(struct comedi_device *dev,
 
 	if (data[0] == 1) {
 		i_MaxChannel = 8;
-	}			/*  if (data[0] == 1) */
-	else {
+	} else {
 		if (data[0] == 2) {
 			i_MaxChannel = 6;
-		}		/*  if(data[0]==2) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"The specified port event does not exist\n");
 			return -EINVAL;
-		}		/* else if(data[0]==2) */
-	}			/* else  if (data[0] == 1) */
+		}
+	}
 	switch (data[1]) {
 	case 0:
 		data[1] = APCI1500_AND;
@@ -194,10 +192,10 @@ static int apci1500_di_config(struct comedi_device *dev,
 		data[1] = APCI1500_OR_PRIORITY;
 		break;
 	default:
-		dev_warn(dev->hw_dev,
+		dev_warn(dev->class_dev,
 			"The specified interrupt logic does not exist\n");
 		return -EINVAL;
-	}			/* switch(data[1]); */
+	}
 
 	i_Logic = data[1];
 	for (i_Count = i_MaxChannel, i = 0; i_Count > 0; i_Count--, i++) {
@@ -239,11 +237,11 @@ static int apci1500_di_config(struct comedi_device *dev,
 		case 5:
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"The option indicated in the event mask does not exist\n");
 			return -EINVAL;
-		}		/*  switch(i_EventMask) */
-	}			/* for (i_Count = i_MaxChannel; i_Count >0;i_Count --) */
+		}
+	}
 
 	if (data[0] == 1) {
 		/* Test the interrupt logic */
@@ -256,10 +254,10 @@ static int apci1500_di_config(struct comedi_device *dev,
 
 			if (data[1] == APCI1500_OR_PRIORITY
 				&& i_PatternTransition != 0) {
-				dev_warn(dev->hw_dev,
+				dev_warn(dev->class_dev,
 					"Transition error on an OR PRIORITY logic\n");
 				return -EINVAL;
-			}	/*  if (data[1]== APCI1500_OR_PRIORITY && i_PatternTransition != 0) */
+			}
 
 			/* Tests if more than one transition */
 			/* was declared for an AND logic     */
@@ -271,14 +269,14 @@ static int apci1500_di_config(struct comedi_device *dev,
 						((i_PatternTransition >>
 							i_Count) & 0x1);
 
-				}	/* for (i_Count = 0; i_Count < 8; i_Count++) */
+				}
 
 				if (i_PatternTransitionCount > 1) {
-					dev_warn(dev->hw_dev,
+					dev_warn(dev->class_dev,
 						"Transition error on an AND logic\n");
 					return -EINVAL;
-				}	/*  if (i_PatternTransitionCount > 1) */
-			}	/*  if (data[1]== APCI1500_AND) */
+				}
+			}
 
 			/* Selects the APCI1500_RW_MASTER_CONFIGURATION_CONTROL register */
 			outb(APCI1500_RW_MASTER_CONFIGURATION_CONTROL,
@@ -347,13 +345,12 @@ static int apci1500_di_config(struct comedi_device *dev,
 				devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
 
-		}		/*  if(data[1]==APCI1500_AND||data[1]==APCI1500_OR||data[1]==APCI1500_OR_PRIORITY) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"The choice for interrupt logic does not exist\n");
 			return -EINVAL;
-		}		/*  else }// if(data[1]==APCI1500_AND||data[1]==APCI1500_OR||data[1]==APCI1500_OR_PRIORITY) */
-	}			/*    if (data[0]== 1) */
+		}
+	}
 
 	/* Test if event setting for port 2 */
 
@@ -448,13 +445,12 @@ static int apci1500_di_config(struct comedi_device *dev,
 			outb(0xF4,
 				devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
-		}		/*   if (data[1] == APCI1500_OR) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"The choice for interrupt logic does not exist\n");
 			return -EINVAL;
-		}		/* elseif (data[1] == APCI1500_OR) */
-	}			/* if(data[0]==2) */
+		}
+	}
 
 	return insn->n;
 }
@@ -518,13 +514,12 @@ static int apci1500_di_write(struct comedi_device *dev,
 						devpriv->iobase +
 						APCI1500_Z8536_CONTROL_REGISTER);
 
-				}	/*  if(i_Event1Status==1) */
-				else {
-					dev_warn(dev->hw_dev,
+				} else {
+					dev_warn(dev->class_dev,
 						"Event 1 not initialised\n");
 					return -EINVAL;
-				}	/* else if(i_Event1Status==1) */
-			}	/* if (data[1]==1) */
+				}
+			}
 			if (data[1] == 2) {
 
 				if (i_Event2Status == 1) {
@@ -555,19 +550,17 @@ static int apci1500_di_write(struct comedi_device *dev,
 						devpriv->iobase +
 						APCI1500_Z8536_CONTROL_REGISTER);
 					i_Event2InterruptStatus = 1;
-				}	/*  if(i_Event2Status==1) */
-				else {
-					dev_warn(dev->hw_dev,
+				} else {
+					dev_warn(dev->class_dev,
 						"Event 2 not initialised\n");
 					return -EINVAL;
-				}	/* else if(i_Event2Status==1) */
-			}	/*  if(data[1]==2) */
-		}		/*  if (data[1] == 1 || data[0] == 2) */
-		else {
-			dev_warn(dev->hw_dev,
+				}
+			}
+		} else {
+			dev_warn(dev->class_dev,
 				"The port parameter is in error\n");
 			return -EINVAL;
-		}		/* else if (data[1] == 1 || data[0] == 2) */
+		}
 
 		break;
 
@@ -600,13 +593,12 @@ static int apci1500_di_write(struct comedi_device *dev,
 						devpriv->iobase +
 						APCI1500_Z8536_CONTROL_REGISTER);
 					i_Event1InterruptStatus = 0;
-				}	/*  if(i_Event1Status==1) */
-				else {
-					dev_warn(dev->hw_dev,
+				} else {
+					dev_warn(dev->class_dev,
 						"Event 1 not initialised\n");
 					return -EINVAL;
-				}	/* else if(i_Event1Status==1) */
-			}	/* if (data[1]==1) */
+				}
+			}
 			if (data[1] == 2) {
 				/* Test if event initialised */
 				if (i_Event2Status == 1) {
@@ -630,26 +622,25 @@ static int apci1500_di_write(struct comedi_device *dev,
 						devpriv->iobase +
 						APCI1500_Z8536_CONTROL_REGISTER);
 					i_Event2InterruptStatus = 0;
-				}	/*  if(i_Event2Status==1) */
-				else {
-					dev_warn(dev->hw_dev,
+				} else {
+
+					dev_warn(dev->class_dev,
 						"Event 2 not initialised\n");
 					return -EINVAL;
-				}	/* else if(i_Event2Status==1) */
-			}	/* if(data[1]==2) */
+				}
+			}
 
-		}		/*  if (data[1] == 1 || data[1] == 2) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"The port parameter is in error\n");
 			return -EINVAL;
-		}		/* else if (data[1] == 1 || data[1] == 2) */
+		}
 		break;
 	default:
-		dev_warn(dev->hw_dev,
+		dev_warn(dev->class_dev,
 			"The option of START/STOP logic does not exist\n");
 		return -EINVAL;
-	}			/* switch(data[0]) */
+	}
 
 	return insn->n;
 }
@@ -835,17 +826,15 @@ static int apci1500_do_write(struct comedi_device *dev,
 	unsigned int ui_Temp1;
 	unsigned int ui_NoOfChannel = CR_CHAN(insn->chanspec);	/*  get the channel */
 
-	if (!devpriv->b_OutputMemoryStatus) {
+	if (!devpriv->b_OutputMemoryStatus)
 		ui_Temp = 0;
 
-	}			/* if(!devpriv->b_OutputMemoryStatus ) */
 	if (data[3] == 0) {
 		if (data[1] == 0) {
 			data[0] = (data[0] << ui_NoOfChannel) | ui_Temp;
 			outw(data[0],
 				devpriv->i_IobaseAddon + APCI1500_DIGITAL_OP);
-		}		/* if(data[1]==0) */
-		else {
+		} else {
 			if (data[1] == 1) {
 				switch (ui_NoOfChannel) {
 
@@ -876,20 +865,18 @@ static int apci1500_do_write(struct comedi_device *dev,
 						"chan spec wrong\n");
 					return -EINVAL;	/*  "sorry channel spec wrong " */
 
-				}	/* switch(ui_NoOfChannels) */
+				}
 
 				outw(data[0],
 					devpriv->i_IobaseAddon +
 					APCI1500_DIGITAL_OP);
-			}	/*  if(data[1]==1) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Specified channel not supported\n");
 				return -EINVAL;
-			}	/* else if(data[1]==1) */
-		}		/* elseif(data[1]==0) */
-	}			/* if(data[3]==0) */
-	else {
+			}
+		}
+	} else {
 		if (data[3] == 1) {
 			if (data[1] == 0) {
 				data[0] = ~data[0] & 0x1;
@@ -903,8 +890,7 @@ static int apci1500_do_write(struct comedi_device *dev,
 				outw(data[0],
 					devpriv->i_IobaseAddon +
 					APCI1500_DIGITAL_OP);
-			}	/* if(data[1]==0) */
-			else {
+			} else {
 				if (data[1] == 1) {
 					switch (ui_NoOfChannel) {
 
@@ -955,25 +941,23 @@ static int apci1500_do_write(struct comedi_device *dev,
 							"chan spec wrong\n");
 						return -EINVAL;	/*  "sorry channel spec wrong " */
 
-					}	/* switch(ui_NoOfChannels) */
+					}
 
 					outw(data[0],
 						devpriv->i_IobaseAddon +
 						APCI1500_DIGITAL_OP);
-				}	/*  if(data[1]==1) */
-				else {
-					dev_warn(dev->hw_dev,
+				} else {
+					dev_warn(dev->class_dev,
 						"Specified channel not supported\n");
 					return -EINVAL;
-				}	/* else if(data[1]==1) */
-			}	/* elseif(data[1]==0) */
-		}		/* if(data[3]==1); */
-		else {
-			dev_warn(dev->hw_dev,
+				}
+			}
+		} else {
+			dev_warn(dev->class_dev,
 				"Specified functionality does not exist\n");
 			return -EINVAL;
-		}		/* if else data[3]==1) */
-	}			/* if else data[3]==0) */
+		}
+	}
 	ui_Temp = data[0];
 	return insn->n;
 }
@@ -1002,17 +986,16 @@ static int apci1500_timer_config(struct comedi_device *dev,
 
 	devpriv->tsk_Current = current;
 
-/* Selection of the input clock */
+	/* Selection of the input clock */
 	if (data[0] == 0 || data[0] == 1 || data[0] == 2) {
 		outw(data[0], devpriv->i_IobaseAddon + APCI1500_CLK_SELECT);
-	}			/*  if(data[0]==0||data[0]==1||data[0]==2) */
-	else {
+	} else {
 		if (data[0] != 3) {
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"The option for input clock selection does not exist\n");
 			return -EINVAL;
-		}		/*  if(data[0]!=3) */
-	}			/* elseif(data[0]==0||data[0]==1||data[0]==2) */
+		}
+	}
 	/* Select the counter/timer */
 	switch (data[1]) {
 	case COUNTER1:
@@ -1025,10 +1008,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[2] = APCI1500_TIMER;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This choice is not a timer nor a counter\n");
 			return -EINVAL;
-		}		/*  switch(data[2]) */
+		}
 
 		/* Selecting  single or continuous mode */
 		switch (data[4]) {
@@ -1039,10 +1022,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[4] = APCI1500_SINGLE;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This option for single/continuous mode does not exist\n");
 			return -EINVAL;
-		}		/*  switch(data[4]) */
+		}
 
 		i_TimerCounterMode = data[2] | data[4] | 7;
 		/* Test the reload value */
@@ -1133,18 +1116,16 @@ static int apci1500_timer_config(struct comedi_device *dev,
 				outb(0x2,
 					devpriv->iobase +
 					APCI1500_Z8536_CONTROL_REGISTER);
-			}	/* if(data[7]== APCI1500_ENABLE ||data[7]== APCI1500_DISABLE) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Error in selection of interrupt enable or disable\n");
 				return -EINVAL;
-			}	/* elseif(data[7]== APCI1500_ENABLE ||data[7]== APCI1500_DISABLE) */
-		}		/*  if ((data[3]>= 0) && (data[3] <= 65535)) */
-		else {
-			dev_warn(dev->hw_dev,
+			}
+		} else {
+			dev_warn(dev->class_dev,
 				"Error in selection of reload value\n");
 			return -EINVAL;
-		}		/* else if ((data[3]>= 0) && (data[3] <= 65535)) */
+		}
 		i_TimerCounterWatchdogInterrupt = data[7];
 		i_TimerCounter1Init = 1;
 		break;
@@ -1158,10 +1139,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[2] = APCI1500_TIMER;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This choice is not a timer nor a counter\n");
 			return -EINVAL;
-		}		/*  switch(data[2]) */
+		}
 
 		/* Selecting  single or continuous mode */
 		switch (data[4]) {
@@ -1172,10 +1153,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[4] = APCI1500_SINGLE;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This option for single/continuous mode does not exist\n");
 			return -EINVAL;
-		}		/*  switch(data[4]) */
+		}
 
 		/* Selecting  software or hardware trigger */
 		switch (data[5]) {
@@ -1186,10 +1167,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[5] = APCI1500_HARDWARE_TRIGGER;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This choice for software or hardware trigger does not exist\n");
 			return -EINVAL;
-		}		/*  switch(data[5]) */
+		}
 
 		/* Selecting  software or hardware gate */
 		switch (data[6]) {
@@ -1200,10 +1181,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[6] = APCI1500_HARDWARE_GATE;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This choice for software or hardware gate does not exist\n");
 			return -EINVAL;
-		}		/*  switch(data[6]) */
+		}
 
 		i_TimerCounterMode = data[2] | data[4] | data[5] | data[6] | 7;
 
@@ -1295,18 +1276,16 @@ static int apci1500_timer_config(struct comedi_device *dev,
 				outb(0x2,
 					devpriv->iobase +
 					APCI1500_Z8536_CONTROL_REGISTER);
-			}	/* if(data[7]== APCI1500_ENABLE ||data[7]== APCI1500_DISABLE) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Error in selection of interrupt enable or disable\n");
 				return -EINVAL;
-			}	/* elseif(data[7]== APCI1500_ENABLE ||data[7]== APCI1500_DISABLE) */
-		}		/*  if ((data[3]>= 0) && (data[3] <= 65535)) */
-		else {
-			dev_warn(dev->hw_dev,
+			}
+		} else {
+			dev_warn(dev->class_dev,
 				"Error in selection of reload value\n");
 			return -EINVAL;
-		}		/* else if ((data[3]>= 0) && (data[3] <= 65535)) */
+		}
 		i_TimerCounterWatchdogInterrupt = data[7];
 		i_TimerCounter2Init = 1;
 		break;
@@ -1320,10 +1299,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[2] = APCI1500_WATCHDOG;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This choice is not a watchdog nor a counter\n");
 			return -EINVAL;
-		}		/*  switch(data[2]) */
+		}
 
 		/* Selecting  single or continuous mode */
 		switch (data[4]) {
@@ -1334,10 +1313,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[4] = APCI1500_SINGLE;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This option for single/continuous mode does not exist\n");
 			return -EINVAL;
-		}		/*  switch(data[4]) */
+		}
 
 		/* Selecting  software or hardware gate */
 		switch (data[6]) {
@@ -1348,10 +1327,10 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			data[6] = APCI1500_HARDWARE_GATE;
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"This choice for software or hardware gate does not exist\n");
 			return -EINVAL;
-		}		/*  switch(data[6]) */
+		}
 
 		/* Test if used for watchdog */
 
@@ -1360,10 +1339,9 @@ static int apci1500_timer_config(struct comedi_device *dev,
 			/* - Enables retrigger       */
 			/* - Pulses output           */
 			i_TimerCounterMode = data[2] | data[4] | 0x54;
-		}		/* if (data[2] == APCI1500_WATCHDOG) */
-		else {
+		} else {
 			i_TimerCounterMode = data[2] | data[4] | data[6] | 7;
-		}		/* elseif (data[2] == APCI1500_WATCHDOG) */
+		}
 		/* Test the reload value */
 
 		if ((data[3] >= 0) && (data[3] <= 65535)) {
@@ -1455,29 +1433,28 @@ static int apci1500_timer_config(struct comedi_device *dev,
 						devpriv->iobase +
 						APCI1500_Z8536_CONTROL_REGISTER);
 
-				}	/* elseif(data[2]==APCI1500_COUNTER) */
+				}
+
+			} else {
 
-			}	/* if(data[7]== APCI1500_ENABLE ||data[7]== APCI1500_DISABLE) */
-			else {
-				dev_warn(dev->hw_dev,
+				dev_warn(dev->class_dev,
 					"Error in selection of interrupt enable or disable\n");
 				return -EINVAL;
-			}	/* elseif(data[7]== APCI1500_ENABLE ||data[7]== APCI1500_DISABLE) */
-		}		/*  if ((data[3]>= 0) && (data[3] <= 65535)) */
-		else {
-			dev_warn(dev->hw_dev,
+			}
+		} else {
+			dev_warn(dev->class_dev,
 				"Error in selection of reload value\n");
 			return -EINVAL;
-		}		/* else if ((data[3]>= 0) && (data[3] <= 65535)) */
+		}
 		i_TimerCounterWatchdogInterrupt = data[7];
 		i_WatchdogCounter3Init = 1;
 		break;
 
 	default:
-		dev_warn(dev->hw_dev,
+		dev_warn(dev->class_dev,
 			"The specified counter/timer option does not exist\n");
 		return -EINVAL;
-	}			/* switch(data[1]) */
+	}
 	i_CounterLogic = data[2];
 	return insn->n;
 }
@@ -1502,12 +1479,11 @@ static int apci1500_timer_write(struct comedi_device *dev,
 		switch (data[1]) {
 		case START:
 			if (i_TimerCounter1Init == 1) {
-				if (i_TimerCounterWatchdogInterrupt == 1) {
+				if (i_TimerCounterWatchdogInterrupt == 1)
 					i_CommandAndStatusValue = 0xC4;	/* Enable the interrupt */
-				}	/*  if(i_TimerCounterWatchdogInterrupt==1) */
-				else {
+				else
 					i_CommandAndStatusValue = 0xE4;	/* disable the interrupt */
-				}	/* elseif(i_TimerCounterWatchdogInterrupt==1) */
+
 				/* Starts timer/counter 1 */
 				i_TimerCounter1Enabled = 1;
 				/* Selects the commands and status register */
@@ -1517,9 +1493,8 @@ static int apci1500_timer_write(struct comedi_device *dev,
 				outb(i_CommandAndStatusValue,
 					devpriv->iobase +
 					APCI1500_Z8536_CONTROL_REGISTER);
-			}	/* if( i_TimerCounter1Init==1) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Counter/Timer1 not configured\n");
 				return -EINVAL;
 			}
@@ -1545,12 +1520,11 @@ static int apci1500_timer_write(struct comedi_device *dev,
 					/* Set Trigger and gate */
 
 					i_CommandAndStatusValue = 0x6;
-				}	/* if( i_TimerCounter1Enabled==1) */
-				else {
+				} else {
 					/* Set Trigger */
 
 					i_CommandAndStatusValue = 0x2;
-				}	/* elseif(i_TimerCounter1Enabled==1) */
+				}
 
 				/* Selects the commands and status register */
 				outb(APCI1500_RW_CPT_TMR1_CMD_STATUS,
@@ -1559,31 +1533,29 @@ static int apci1500_timer_write(struct comedi_device *dev,
 				outb(i_CommandAndStatusValue,
 					devpriv->iobase +
 					APCI1500_Z8536_CONTROL_REGISTER);
-			}	/* if( i_TimerCounter1Init==1) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Counter/Timer1 not configured\n");
 				return -EINVAL;
 			}
 			break;
 
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"The specified option for start/stop/trigger does not exist\n");
 			return -EINVAL;
-		}		/* switch(data[1]) */
+		}
 		break;
 
 	case COUNTER2:
 		switch (data[1]) {
 		case START:
 			if (i_TimerCounter2Init == 1) {
-				if (i_TimerCounterWatchdogInterrupt == 1) {
+				if (i_TimerCounterWatchdogInterrupt == 1)
 					i_CommandAndStatusValue = 0xC4;	/* Enable the interrupt */
-				}	/*  if(i_TimerCounterWatchdogInterrupt==1) */
-				else {
+				else
 					i_CommandAndStatusValue = 0xE4;	/* disable the interrupt */
-				}	/* elseif(i_TimerCounterWatchdogInterrupt==1) */
+
 				/* Starts timer/counter 2 */
 				i_TimerCounter2Enabled = 1;
 				/* Selects the commands and status register */
@@ -1593,9 +1565,8 @@ static int apci1500_timer_write(struct comedi_device *dev,
 				outb(i_CommandAndStatusValue,
 					devpriv->iobase +
 					APCI1500_Z8536_CONTROL_REGISTER);
-			}	/* if( i_TimerCounter2Init==1) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Counter/Timer2 not configured\n");
 				return -EINVAL;
 			}
@@ -1620,12 +1591,11 @@ static int apci1500_timer_write(struct comedi_device *dev,
 					/* Set Trigger and gate */
 
 					i_CommandAndStatusValue = 0x6;
-				}	/* if( i_TimerCounter2Enabled==1) */
-				else {
+				} else {
 					/* Set Trigger */
 
 					i_CommandAndStatusValue = 0x2;
-				}	/* elseif(i_TimerCounter2Enabled==1) */
+				}
 
 				/* Selects the commands and status register */
 				outb(APCI1500_RW_CPT_TMR2_CMD_STATUS,
@@ -1634,30 +1604,28 @@ static int apci1500_timer_write(struct comedi_device *dev,
 				outb(i_CommandAndStatusValue,
 					devpriv->iobase +
 					APCI1500_Z8536_CONTROL_REGISTER);
-			}	/* if( i_TimerCounter2Init==1) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Counter/Timer2 not configured\n");
 				return -EINVAL;
 			}
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"The specified option for start/stop/trigger does not exist\n");
 			return -EINVAL;
-		}		/* switch(data[1]) */
+		}
 		break;
 	case COUNTER3:
 		switch (data[1]) {
 		case START:
 			if (i_WatchdogCounter3Init == 1) {
 
-				if (i_TimerCounterWatchdogInterrupt == 1) {
+				if (i_TimerCounterWatchdogInterrupt == 1)
 					i_CommandAndStatusValue = 0xC4;	/* Enable the interrupt */
-				}	/*  if(i_TimerCounterWatchdogInterrupt==1) */
-				else {
+				else
 					i_CommandAndStatusValue = 0xE4;	/* disable the interrupt */
-				}	/* elseif(i_TimerCounterWatchdogInterrupt==1) */
+
 				/* Starts Watchdog/counter 3 */
 				i_WatchdogCounter3Enabled = 1;
 				/* Selects the commands and status register */
@@ -1668,9 +1636,8 @@ static int apci1500_timer_write(struct comedi_device *dev,
 					devpriv->iobase +
 					APCI1500_Z8536_CONTROL_REGISTER);
 
-			}	/*  if( i_WatchdogCounter3init==1) */
-			else {
-				dev_warn(dev->hw_dev,
+			} else {
+				dev_warn(dev->class_dev,
 					"Watchdog/Counter3 not configured\n");
 				return -EINVAL;
 			}
@@ -1698,12 +1665,11 @@ static int apci1500_timer_write(struct comedi_device *dev,
 						/* Set Trigger and gate */
 
 						i_CommandAndStatusValue = 0x6;
-					}	/* if( i_WatchdogCounter3Enabled==1) */
-					else {
+					} else {
 						/* Set Trigger */
 
 						i_CommandAndStatusValue = 0x2;
-					}	/* elseif(i_WatchdogCounter3Enabled==1) */
+					}
 
 					/* Selects the commands and status register */
 					outb(APCI1500_RW_CPT_TMR3_CMD_STATUS,
@@ -1712,9 +1678,8 @@ static int apci1500_timer_write(struct comedi_device *dev,
 					outb(i_CommandAndStatusValue,
 						devpriv->iobase +
 						APCI1500_Z8536_CONTROL_REGISTER);
-				}	/* if( i_WatchdogCounter3Init==1) */
-				else {
-					dev_warn(dev->hw_dev,
+				} else {
+					dev_warn(dev->class_dev,
 						"Counter3 not configured\n");
 					return -EINVAL;
 				}
@@ -1730,30 +1695,29 @@ static int apci1500_timer_write(struct comedi_device *dev,
 					outb(0x6,
 						devpriv->iobase +
 						APCI1500_Z8536_CONTROL_REGISTER);
-				}	/* if( i_WatchdogCounter3Init==1) */
-				else {
-					dev_warn(dev->hw_dev,
+				} else {
+					dev_warn(dev->class_dev,
 						"Watchdog 3 not configured\n");
 					return -EINVAL;
 				}
 				break;
 			default:
-				dev_warn(dev->hw_dev,
+				dev_warn(dev->class_dev,
 					"Wrong choice of watchdog/counter3\n");
 				return -EINVAL;
-			}	/* switch(data[2]) */
+			}
 			break;
 		default:
-			dev_warn(dev->hw_dev,
+			dev_warn(dev->class_dev,
 				"The specified option for start/stop/trigger does not exist\n");
 			return -EINVAL;
-		}		/* switch(data[1]) */
+		}
 		break;
 	default:
-		dev_warn(dev->hw_dev,
+		dev_warn(dev->class_dev,
 			"The specified choice for counter/watchdog/timer does not exist\n");
 		return -EINVAL;
-	}			/* switch(data[0]) */
+	}
 	return insn->n;
 }
 
@@ -1778,12 +1742,11 @@ static int apci1500_timer_bits(struct comedi_device *dev,
 				/* Set RCC and gate */
 
 				i_CommandAndStatusValue = 0xC;
-			}	/* if( i_TimerCounter1Init==1) */
-			else {
+			} else {
 				/* Set RCC */
 
 				i_CommandAndStatusValue = 0x8;
-			}	/* elseif(i_TimerCounter1Init==1) */
+			}
 
 			/* Selects the commands and status register */
 			outb(APCI1500_RW_CPT_TMR1_CMD_STATUS,
@@ -1808,12 +1771,11 @@ static int apci1500_timer_bits(struct comedi_device *dev,
 			data[0] =
 				data[0] | inb(devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
-		}		/* if( i_TimerCounter1Init==1) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"Timer/Counter1 not configured\n");
 			return -EINVAL;
-		}		/* elseif( i_TimerCounter1Init==1) */
+		}
 		break;
 	case COUNTER2:
 		/* Read counter/timer2 */
@@ -1822,12 +1784,11 @@ static int apci1500_timer_bits(struct comedi_device *dev,
 				/* Set RCC and gate */
 
 				i_CommandAndStatusValue = 0xC;
-			}	/* if( i_TimerCounter2Init==1) */
-			else {
+			} else {
 				/* Set RCC */
 
 				i_CommandAndStatusValue = 0x8;
-			}	/* elseif(i_TimerCounter2Init==1) */
+			}
 
 			/* Selects the commands and status register */
 			outb(APCI1500_RW_CPT_TMR2_CMD_STATUS,
@@ -1852,12 +1813,11 @@ static int apci1500_timer_bits(struct comedi_device *dev,
 			data[0] =
 				data[0] | inb(devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
-		}		/* if( i_TimerCounter2Init==1) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"Timer/Counter2 not configured\n");
 			return -EINVAL;
-		}		/* elseif( i_TimerCounter2Init==1) */
+		}
 		break;
 	case COUNTER3:
 		/* Read counter/watchdog2 */
@@ -1866,12 +1826,11 @@ static int apci1500_timer_bits(struct comedi_device *dev,
 				/* Set RCC and gate */
 
 				i_CommandAndStatusValue = 0xC;
-			}	/* if( i_TimerCounter2Init==1) */
-			else {
+			} else {
 				/* Set RCC */
 
 				i_CommandAndStatusValue = 0x8;
-			}	/* elseif(i_WatchdogCounter3Init==1) */
+			}
 
 			/* Selects the commands and status register */
 			outb(APCI1500_RW_CPT_TMR3_CMD_STATUS,
@@ -1896,18 +1855,17 @@ static int apci1500_timer_bits(struct comedi_device *dev,
 			data[0] =
 				data[0] | inb(devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
-		}		/* if( i_WatchdogCounter3Init==1) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"WatchdogCounter3 not configured\n");
 			return -EINVAL;
-		}		/* elseif( i_WatchdogCounter3Init==1) */
+		}
 		break;
 	default:
-		dev_warn(dev->hw_dev,
+		dev_warn(dev->class_dev,
 			"The choice of timer/counter/watchdog does not exist\n");
 		return -EINVAL;
-	}			/* switch(data[0]) */
+	}
 
 	return insn->n;
 }
@@ -1946,17 +1904,15 @@ static int apci1500_do_bits(struct comedi_device *dev,
 	outl(0x0, devpriv->i_IobaseAmcc + 0x38);
 	if (data[0] == 1) {
 		i_Constant = 0xC0;
-	}			/* if(data[0]==1) */
-	else {
+	} else {
 		if (data[0] == 0) {
 			i_Constant = 0x00;
-		}		/* if{data[0]==0) */
-		else {
-			dev_warn(dev->hw_dev,
+		} else {
+			dev_warn(dev->class_dev,
 				"The parameter passed to driver is in error for enabling the voltage interrupt\n");
 			return -EINVAL;
-		}		/* else if(data[0]==0) */
-	}			/* elseif(data[0]==1) */
+		}
+	}
 
 	/* Selects the mode specification register of port B */
 	outb(APCI1500_RW_PORT_B_SPECIFICATION,
@@ -2062,6 +2018,8 @@ static void apci1500_interrupt(int irq, void *d)
 	struct addi_private *devpriv = dev->private;
 	unsigned int ui_InterruptStatus = 0;
 	int i_RegValue = 0;
+
+	/* Clear the interrupt mask */
 	i_InterruptMask = 0;
 
 	/* Read the board interrupt status */
@@ -2071,10 +2029,7 @@ static void apci1500_interrupt(int irq, void *d)
 	if ((ui_InterruptStatus & 0x800000) == 0x800000) {
 		/* Disable all Interrupt */
 		/* Selects the master interrupt control register */
-		/* outb(APCI1500_RW_MASTER_INTERRUPT_CONTROL,devpriv->iobase+APCI1500_Z8536_CONTROL_REGISTER); */
 		/* Disables  the main interrupt on the board */
-		/* outb(0x00,devpriv->iobase+APCI1500_Z8536_CONTROL_REGISTER); */
-
 		/* Selects the command and status register of port A */
 		outb(APCI1500_RW_PORT_A_COMMAND_AND_STATUS,
 			devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
@@ -2109,11 +2064,10 @@ static void apci1500_interrupt(int irq, void *d)
 
 				i_InputChannel = 1 + (i_RegValue >> 1);
 
-			}	/*  if(i_Logic==APCI1500_OR_PRIORITY) */
-			else {
+			} else {
 				i_InputChannel = 0;
-			}	/* elseif(i_Logic==APCI1500_OR_PRIORITY) */
-		}		/*  if ((i_RegValue & 0x60) == 0x60) */
+			}
+		}
 
 		/* Selects the command and status register of port B */
 		outb(APCI1500_RW_PORT_B_COMMAND_AND_STATUS,
@@ -2146,17 +2100,16 @@ static void apci1500_interrupt(int irq, void *d)
 				if (i_RegValue & 0x80) {
 					i_InterruptMask =
 						i_InterruptMask | 0x40;
-				}	/* if (i_RegValue & 0x80) */
+				}
 
 				if (i_RegValue & 0x40) {
 					i_InterruptMask =
 						i_InterruptMask | 0x80;
-				}	/* if (i_RegValue & 0x40) */
-			}	/*  if (i_RegValue) */
-			else {
+				}
+			} else {
 				i_InterruptMask = i_InterruptMask | 2;
-			}	/*  if (i_RegValue) */
-		}		/* if ((i_RegValue & 0x60) == 0x60) */
+			}
+		}
 
 		/* Selects the command and status register of timer 1 */
 		outb(APCI1500_RW_CPT_TMR1_CMD_STATUS,
@@ -2174,7 +2127,7 @@ static void apci1500_interrupt(int irq, void *d)
 				devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
 			i_InterruptMask = i_InterruptMask | 4;
-		}		/*  if ((i_RegValue & 0x60) == 0x60) */
+		}
 		/* Selects the command and status register of timer 2 */
 		outb(APCI1500_RW_CPT_TMR2_CMD_STATUS,
 			devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
@@ -2191,7 +2144,7 @@ static void apci1500_interrupt(int irq, void *d)
 				devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
 			i_InterruptMask = i_InterruptMask | 8;
-		}		/*  if ((i_RegValue & 0x60) == 0x60) */
+		}
 
 		/* Selects the command and status register of timer 3 */
 		outb(APCI1500_RW_CPT_TMR3_CMD_STATUS,
@@ -2208,13 +2161,11 @@ static void apci1500_interrupt(int irq, void *d)
 			outb(i_RegValue,
 				devpriv->iobase +
 				APCI1500_Z8536_CONTROL_REGISTER);
-			if (i_CounterLogic == APCI1500_COUNTER) {
+			if (i_CounterLogic == APCI1500_COUNTER)
 				i_InterruptMask = i_InterruptMask | 0x10;
-			}	/* if(i_CounterLogic==APCI1500_COUNTER) */
-			else {
+			else
 				i_InterruptMask = i_InterruptMask | 0x20;
-			}
-		}		/*  if ((i_RegValue & 0x60) == 0x60) */
+		}
 
 		send_sig(SIGIO, devpriv->tsk_Current, 0);	/*  send signal to the sample */
 		/* Enable all Interrupts */
@@ -2224,13 +2175,11 @@ static void apci1500_interrupt(int irq, void *d)
 			devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
 		/* Authorizes the main interrupt on the board */
 		outb(0xD0, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-	}			/*   if ((ui_InterruptStatus & 0x800000) == 0x800000) */
-	else {
-		dev_warn(dev->hw_dev,
+	} else {
+		dev_warn(dev->class_dev,
 			"Interrupt from unknown source\n");
 
-	}			/* else if ((ui_InterruptStatus & 0x800000) == 0x800000) */
-	return;
+	}
 }
 
 static int apci1500_reset(struct comedi_device *dev)
@@ -2379,36 +2328,36 @@ static int apci1500_reset(struct comedi_device *dev)
 	outb(0, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
 	/* reset all the digital outputs */
 	outw(0x0, devpriv->i_IobaseAddon + APCI1500_DIGITAL_OP);
-/* Disable the board interrupt */
+	/* Disable the board interrupt */
 	/* Selects the master interrupt control register */
 	outb(APCI1500_RW_MASTER_INTERRUPT_CONTROL,
 		devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-/* Deactivates all interrupts */
+	/* Deactivates all interrupts */
 	outb(0, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
 	/* Selects the command and status register of port A */
 	outb(APCI1500_RW_PORT_A_COMMAND_AND_STATUS,
 		devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-/* Deactivates all interrupts */
+	/* Deactivates all interrupts */
 	outb(0x00, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
 	/* Selects the command and status register of port B */
 	outb(APCI1500_RW_PORT_B_COMMAND_AND_STATUS,
 		devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-/* Deactivates all interrupts */
+	/* Deactivates all interrupts */
 	outb(0x00, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
 	/* Selects the command and status register of timer 1 */
 	outb(APCI1500_RW_CPT_TMR1_CMD_STATUS,
 		devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-/* Deactivates all interrupts */
+	/* Deactivates all interrupts */
 	outb(0x00, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
 	/* Selects the command and status register of timer 2 */
 	outb(APCI1500_RW_CPT_TMR2_CMD_STATUS,
 		devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-/* Deactivates all interrupts */
+	/* Deactivates all interrupts */
 	outb(0x00, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-/* Selects the command and status register of timer 3*/
+	/* Selects the command and status register of timer 3*/
 	outb(APCI1500_RW_CPT_TMR3_CMD_STATUS,
 		devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
-/* Deactivates all interrupts */
+	/* Deactivates all interrupts */
 	outb(0x00, devpriv->iobase + APCI1500_Z8536_CONTROL_REGISTER);
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
index 8a613ae0acba..98de96953a29 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci1564.c
@@ -1,28 +1,3 @@
-/*
- * Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
- *
- *	ADDI-DATA GmbH
- *	Dieselstrasse 3
- *	D-77833 Ottersweier
- *	Tel: +19(0)7223/9493-0
- *	Fax: +49(0)7223/9493-92
- *	http://www.addi-data.com
- *	info@addi-data.com
- *
- * This program is free software; you can redistribute it and/or modify it under
- * the terms of the GNU General Public License as published by the Free Software
- * Foundation; either version 2 of the License, or (at your option) any later
- * version.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
- * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
- * details.
- *
- */
-
-#define APCI1564_ADDRESS_RANGE				128
-
 /* Digital Input IRQ Function Selection */
 #define APCI1564_DI_INT_OR				(0 << 1)
 #define APCI1564_DI_INT_AND				(1 << 1)
@@ -32,10 +7,10 @@
 #define APCI1564_DI_INT_DISABLE				0xfffffffb
 
 /* Digital Output Interrupt Enable Disable. */
-#define APCI1564_DIGITAL_OP_VCC_INTERRUPT_ENABLE	0x1
-#define APCI1564_DIGITAL_OP_VCC_INTERRUPT_DISABLE	0xfffffffe
-#define APCI1564_DIGITAL_OP_CC_INTERRUPT_ENABLE		0x2
-#define APCI1564_DIGITAL_OP_CC_INTERRUPT_DISABLE	0xfffffffd
+#define APCI1564_DO_VCC_INT_ENABLE			0x1
+#define APCI1564_DO_VCC_INT_DISABLE			0xfffffffe
+#define APCI1564_DO_CC_INT_ENABLE			0x2
+#define APCI1564_DO_CC_INT_DISABLE			0xfffffffd
 
 /* TIMER COUNTER WATCHDOG DEFINES */
 #define ADDIDATA_TIMER					0
@@ -76,55 +51,21 @@
 #define APCI1564_TIMER_WARN_TIMEBASE_REG		0x64
 
 /*
- * dev>iobase Register Map
+ * dev->iobase Register Map
  */
-#define APCI1564_TCW_REG(x)				(0x00 + ((x) * 0x20))
-#define APCI1564_TCW_RELOAD_REG(x)			(0x04 + ((x) * 0x20))
-#define APCI1564_TCW_TIMEBASE_REG(x)			(0x08 + ((x) * 0x20))
-#define APCI1564_TCW_CTRL_REG(x)			(0x0c + ((x) * 0x20))
-#define APCI1564_TCW_STATUS_REG(x)			(0x10 + ((x) * 0x20))
-#define APCI1564_TCW_IRQ_REG(x)				(0x14 + ((x) * 0x20))
-#define APCI1564_TCW_WARN_TIMEVAL_REG(x)		(0x18 + ((x) * 0x20))
-#define APCI1564_TCW_WARN_TIMEBASE_REG(x)		(0x1c + ((x) * 0x20))
-
-/*
- * Configures The Digital Output Subdevice.
- *
- * data[1] 0 = Disable VCC Interrupt, 1 = Enable VCC Interrupt
- * data[2] 0 = Disable CC Interrupt, 1 = Enable CC Interrupt
- */
-static int apci1564_do_config(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct apci1564_private *devpriv = dev->private;
-	unsigned int ul_Command = 0;
-
-	if ((data[0] != 0) && (data[0] != 1)) {
-		dev_err(dev->class_dev, "Data should be 1 or 0\n");
-		return -EINVAL;
-	}
-
-	if (data[1] == 1)
-		ul_Command = ul_Command | 0x1;
-	else
-		ul_Command = ul_Command & 0xFFFFFFFE;
-
-	if (data[2] == 1)
-		ul_Command = ul_Command | 0x2;
-	else
-		ul_Command = ul_Command & 0xFFFFFFFD;
-
-	outl(ul_Command, devpriv->amcc_iobase + APCI1564_DO_INT_CTRL_REG);
-	devpriv->tsk_current = current;
-	return insn->n;
-}
+#define APCI1564_COUNTER_REG(x)				(0x00 + ((x) * 0x20))
+#define APCI1564_COUNTER_RELOAD_REG(x)			(0x04 + ((x) * 0x20))
+#define APCI1564_COUNTER_TIMEBASE_REG(x)		(0x08 + ((x) * 0x20))
+#define APCI1564_COUNTER_CTRL_REG(x)			(0x0c + ((x) * 0x20))
+#define APCI1564_COUNTER_STATUS_REG(x)			(0x10 + ((x) * 0x20))
+#define APCI1564_COUNTER_IRQ_REG(x)			(0x14 + ((x) * 0x20))
+#define APCI1564_COUNTER_WARN_TIMEVAL_REG(x)		(0x18 + ((x) * 0x20))
+#define APCI1564_COUNTER_WARN_TIMEBASE_REG(x)		(0x1c + ((x) * 0x20))
 
 /*
- * Configures The Timer, Counter or Watchdog
+ * Configures The Timer or Counter
  *
- * data[0] Configure as: 0 = Timer, 1 = Counter, 2 = Watchdog
+ * data[0] Configure as: 0 = Timer, 1 = Counter
  * data[1] 1 = Enable Interrupt, 0 = Disable Interrupt
  * data[2] Time Unit
  * data[3] Reload Value
@@ -141,14 +82,7 @@ static int apci1564_timer_config(struct comedi_device *dev,
 	unsigned int ul_Command1 = 0;
 
 	devpriv->tsk_current = current;
-	if (data[0] == ADDIDATA_WATCHDOG) {
-		devpriv->timer_select_mode = ADDIDATA_WATCHDOG;
-
-		/* Disable the watchdog */
-		outl(0x0, devpriv->amcc_iobase + APCI1564_WDOG_CTRL_REG);
-		/* Loading the Reload value */
-		outl(data[3], devpriv->amcc_iobase + APCI1564_WDOG_RELOAD_REG);
-	} else if (data[0] == ADDIDATA_TIMER) {
+	if (data[0] == ADDIDATA_TIMER) {
 		/* First Stop The Timer */
 		ul_Command1 = inl(devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
 		ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
@@ -162,14 +96,14 @@ static int apci1564_timer_config(struct comedi_device *dev,
 			outl(0x0, devpriv->amcc_iobase + APCI1564_DI_IRQ_REG);
 			outl(0x0, devpriv->amcc_iobase + APCI1564_DO_IRQ_REG);
 			outl(0x0, devpriv->amcc_iobase + APCI1564_WDOG_IRQ_REG);
-			outl(0x0,
-			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER1));
-			outl(0x0,
-			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER2));
-			outl(0x0,
-			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER3));
-			outl(0x0,
-			     dev->iobase + APCI1564_TCW_IRQ_REG(APCI1564_COUNTER4));
+			outl(0x0, dev->iobase +
+			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER1));
+			outl(0x0, dev->iobase +
+			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER2));
+			outl(0x0, dev->iobase +
+			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER3));
+			outl(0x0, dev->iobase +
+			    APCI1564_COUNTER_IRQ_REG(APCI1564_COUNTER4));
 		} else {
 			/* disable Timer interrupt */
 			outl(0x0, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
@@ -190,13 +124,16 @@ static int apci1564_timer_config(struct comedi_device *dev,
 		devpriv->mode_select_register = data[5];
 
 		/* First Stop The Counter */
-		ul_Command1 = inl(dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		ul_Command1 = inl(dev->iobase +
+				 APCI1564_COUNTER_CTRL_REG(data[5] - 1));
 		ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
 		/* Stop The Timer */
-		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase +
+					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
 
 		/* Set the reload value */
-		outl(data[3], dev->iobase + APCI1564_TCW_RELOAD_REG(data[5] - 1));
+		outl(data[3], dev->iobase +
+					APCI1564_COUNTER_RELOAD_REG(data[5] - 1));
 
 		/* Set the mode :             */
 		/* - Disable the hardware     */
@@ -209,26 +146,28 @@ static int apci1564_timer_config(struct comedi_device *dev,
 		ul_Command1 =
 			(ul_Command1 & 0xFFFC19E2UL) | 0x80000UL |
 			(unsigned int) ((unsigned int) data[4] << 16UL);
-		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase +
+					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
 
 		/*  Enable or Disable Interrupt */
 		ul_Command1 = (ul_Command1 & 0xFFFFF9FD) | (data[1] << 1);
-		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase +
+					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
 
 		/* Set the Up/Down selection */
 		ul_Command1 = (ul_Command1 & 0xFFFBF9FFUL) | (data[6] << 18);
-		outl(ul_Command1, dev->iobase + APCI1564_TCW_CTRL_REG(data[5] - 1));
+		outl(ul_Command1, dev->iobase +
+					APCI1564_COUNTER_CTRL_REG(data[5] - 1));
 	} else {
 		dev_err(dev->class_dev, "Invalid subdevice.\n");
 	}
-
 	return insn->n;
 }
 
 /*
- * Start / Stop The Selected Timer, Counter or Watchdog
+ * Start / Stop The Selected Timer or Counter
  *
- * data[0] Configure as: 0 = Timer, 1 = Counter, 2 = Watchdog
+ * data[0] Configure as: 0 = Timer, 1 = Counter
  * data[1] 0 = Stop, 1 = Start, 2 = Trigger Clear (Only Counter)
  */
 static int apci1564_timer_write(struct comedi_device *dev,
@@ -239,23 +178,6 @@ static int apci1564_timer_write(struct comedi_device *dev,
 	struct apci1564_private *devpriv = dev->private;
 	unsigned int ul_Command1 = 0;
 
-	if (devpriv->timer_select_mode == ADDIDATA_WATCHDOG) {
-		switch (data[1]) {
-		case 0:	/* stop the watchdog */
-			/* disable the watchdog */
-			outl(0x0, devpriv->amcc_iobase + APCI1564_WDOG_CTRL_REG);
-			break;
-		case 1:	/* start the watchdog */
-			outl(0x0001, devpriv->amcc_iobase + APCI1564_WDOG_CTRL_REG);
-			break;
-		case 2:	/* Software trigger */
-			outl(0x0201, devpriv->amcc_iobase + APCI1564_WDOG_CTRL_REG);
-			break;
-		default:
-			dev_err(dev->class_dev, "Specified functionality does not exist.\n");
-			return -EINVAL;
-		}
-	}
 	if (devpriv->timer_select_mode == ADDIDATA_TIMER) {
 		if (data[1] == 1) {
 			ul_Command1 = inl(devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
@@ -270,11 +192,10 @@ static int apci1564_timer_write(struct comedi_device *dev,
 			ul_Command1 = ul_Command1 & 0xFFFFF9FEUL;
 			outl(ul_Command1, devpriv->amcc_iobase + APCI1564_TIMER_CTRL_REG);
 		}
-	}
-	if (devpriv->timer_select_mode == ADDIDATA_COUNTER) {
+	} else if (devpriv->timer_select_mode == ADDIDATA_COUNTER) {
 		ul_Command1 =
 			inl(dev->iobase +
-			    APCI1564_TCW_CTRL_REG(devpriv->mode_select_register - 1));
+			   APCI1564_COUNTER_CTRL_REG(devpriv->mode_select_register - 1));
 		if (data[1] == 1) {
 			/* Start the Counter subdevice */
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x1UL;
@@ -287,13 +208,15 @@ static int apci1564_timer_write(struct comedi_device *dev,
 			ul_Command1 = (ul_Command1 & 0xFFFFF9FFUL) | 0x400;
 		}
 		outl(ul_Command1, dev->iobase +
-		     APCI1564_TCW_CTRL_REG(devpriv->mode_select_register - 1));
+		     APCI1564_COUNTER_CTRL_REG(devpriv->mode_select_register - 1));
+	} else {
+		dev_err(dev->class_dev, "Invalid subdevice.\n");
 	}
 	return insn->n;
 }
 
 /*
- * Read The Selected Timer, Counter or Watchdog
+ * Read The Selected Timer or Counter
  */
 static int apci1564_timer_read(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
@@ -303,11 +226,7 @@ static int apci1564_timer_read(struct comedi_device *dev,
 	struct apci1564_private *devpriv = dev->private;
 	unsigned int ul_Command1 = 0;
 
-	if (devpriv->timer_select_mode == ADDIDATA_WATCHDOG) {
-		/*  Stores the status of the Watchdog */
-		data[0] = inl(devpriv->amcc_iobase + APCI1564_WDOG_STATUS_REG) & 0x1;
-		data[1] = inl(devpriv->amcc_iobase + APCI1564_WDOG_REG);
-	} else if (devpriv->timer_select_mode == ADDIDATA_TIMER) {
+	if (devpriv->timer_select_mode == ADDIDATA_TIMER) {
 		/*  Stores the status of the Timer */
 		data[0] = inl(devpriv->amcc_iobase + APCI1564_TIMER_STATUS_REG) & 0x1;
 
@@ -317,10 +236,10 @@ static int apci1564_timer_read(struct comedi_device *dev,
 		/*  Read the Counter Actual Value. */
 		data[0] =
 			inl(dev->iobase +
-			    APCI1564_TCW_REG(devpriv->mode_select_register - 1));
+			    APCI1564_COUNTER_REG(devpriv->mode_select_register - 1));
 		ul_Command1 =
 			inl(dev->iobase +
-			    APCI1564_TCW_STATUS_REG(devpriv->mode_select_register - 1));
+			    APCI1564_COUNTER_STATUS_REG(devpriv->mode_select_register - 1));
 
 		/* Get the software trigger status */
 		data[1] = (unsigned char) ((ul_Command1 >> 1) & 1);
@@ -333,10 +252,8 @@ static int apci1564_timer_read(struct comedi_device *dev,
 
 		/* Get the overflow status */
 		data[4] = (unsigned char) ((ul_Command1 >> 0) & 1);
-	} else if ((devpriv->timer_select_mode != ADDIDATA_TIMER)
-		&& (devpriv->timer_select_mode != ADDIDATA_WATCHDOG)
-		&& (devpriv->timer_select_mode != ADDIDATA_COUNTER)) {
-		dev_err(dev->class_dev, "Invalid Subdevice!\n");
+	} else {
+		dev_err(dev->class_dev, "Invalid subdevice.\n");
 	}
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
index 77cee876a374..2950815b65f4 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3120.c
@@ -241,26 +241,19 @@ static const struct comedi_lrange range_apci3120_ao = {
 
 
 /* FUNCTION DEFINITIONS */
-
-/*
-+----------------------------------------------------------------------------+
-|                           ANALOG INPUT SUBDEVICE   		                 |
-+----------------------------------------------------------------------------+
-*/
-
 static int apci3120_ai_insn_config(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn,
 				   unsigned int *data)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	unsigned int i;
 
 	if ((data[0] != APCI3120_EOC_MODE) && (data[0] != APCI3120_EOS_MODE))
 		return -1;
 
-	/*  Check for Conversion time to be added ?? */
+	/*  Check for Conversion time to be added */
 	devpriv->ui_EocEosConversionTime = data[2];
 
 	if (data[0] == APCI3120_EOS_MODE) {
@@ -282,7 +275,6 @@ static int apci3120_ai_insn_config(struct comedi_device *dev,
 		else
 			devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
 		/*  Copy channel list and Range List to devpriv */
-
 		devpriv->ui_AiNbrofChannels = data[3];
 		for (i = 0; i < devpriv->ui_AiNbrofChannels; i++)
 			devpriv->ui_AiChannelList[i] = data[4 + i];
@@ -311,7 +303,7 @@ static int apci3120_setup_chan_list(struct comedi_device *dev,
 				    char check)
 {
 	struct addi_private *devpriv = dev->private;
-	unsigned int i;		/* , differencial=0, bipolar=0; */
+	unsigned int i;
 	unsigned int gain;
 	unsigned short us_TmpValue;
 
@@ -326,7 +318,7 @@ static int apci3120_setup_chan_list(struct comedi_device *dev,
 	if (check)
 		return 1;
 
-	/* Code  to set the PA and PR...Here it set PA to 0.. */
+	/* Code  to set the PA and PR...Here it set PA to 0 */
 	devpriv->us_OutputRegister =
 		devpriv->us_OutputRegister & APCI3120_CLEAR_PA_PR;
 	devpriv->us_OutputRegister = ((n_chan - 1) & 0xf) << 8;
@@ -334,16 +326,16 @@ static int apci3120_setup_chan_list(struct comedi_device *dev,
 
 	for (i = 0; i < n_chan; i++) {
 		/*  store range list to card */
-		us_TmpValue = CR_CHAN(chanlist[i]);	/*  get channel number; */
+		us_TmpValue = CR_CHAN(chanlist[i]);	/*  get channel number */
 
 		if (CR_RANGE(chanlist[i]) < APCI3120_BIPOLAR_RANGES)
 			us_TmpValue &= ((~APCI3120_UNIPOLAR) & 0xff);	/*  set bipolar */
 		else
-			us_TmpValue |= APCI3120_UNIPOLAR;	/*  enable unipolar...... */
+			us_TmpValue |= APCI3120_UNIPOLAR;	/*  enable unipolar */
 
 		gain = CR_RANGE(chanlist[i]);	/*  get gain number */
 		us_TmpValue |= ((gain & 0x03) << 4);	/* <<4 for G0 and G1 bit in RAM */
-		us_TmpValue |= i << 8;	/* To select the RAM LOCATION.... */
+		us_TmpValue |= i << 8;	/* To select the RAM LOCATION */
 		outw(us_TmpValue, dev->iobase + APCI3120_SEQ_RAM_ADDRESS);
 	}
 	return 1;		/*  we can serve this with scan logic */
@@ -359,7 +351,7 @@ static int apci3120_ai_insn_read(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	unsigned short us_ConvertTiming, us_TmpValue, i;
 	unsigned char b_Tmp;
@@ -370,24 +362,21 @@ static int apci3120_ai_insn_read(struct comedi_device *dev,
 	else
 		us_ConvertTiming = (unsigned short) (devpriv->ui_EocEosConversionTime / 1000);	/*  nano to useconds */
 
-	/*  this_board->ai_read(dev,us_ConvertTiming,insn->n,&insn->chanspec,data,insn->unused[0]); */
-
 	/*  Clear software registers */
 	devpriv->b_TimerSelectMode = 0;
 	devpriv->b_ModeSelectRegister = 0;
 	devpriv->us_OutputRegister = 0;
-/* devpriv->b_DigitalOutputRegister=0; */
 
 	if (insn->unused[0] == 222) {	/*  second insn read */
 		for (i = 0; i < insn->n; i++)
 			data[i] = devpriv->ui_AiReadData[i];
 	} else {
 		devpriv->tsk_Current = current;	/*  Save the current process task structure */
-/*
- * Testing if board have the new Quartz and calculate the time value
- * to set in the timer
- */
 
+		/*
+		 * Testing if board have the new Quartz and calculate the time value
+		 * to set in the timer
+		 */
 		us_TmpValue =
 			(unsigned short) inw(devpriv->iobase + APCI3120_RD_STATUS);
 
@@ -406,9 +395,9 @@ static int apci3120_ai_insn_read(struct comedi_device *dev,
 
 		case APCI3120_EOC_MODE:
 
-/*
- * Testing the interrupt flag and set the EOC bit Clears the FIFO
- */
+			/*
+			 * Testing the interrupt flag and set the EOC bit Clears the FIFO
+			 */
 			inw(devpriv->iobase + APCI3120_RESET_FIFO);
 
 			/*  Initialize the sequence array */
@@ -556,7 +545,6 @@ static int apci3120_ai_insn_read(struct comedi_device *dev,
 			inw(devpriv->iobase + APCI3120_RD_STATUS);
 
 			/* Sets gate 0 */
-
 			devpriv->us_OutputRegister =
 				devpriv->
 				us_OutputRegister | APCI3120_ENABLE_TIMER0;
@@ -583,7 +571,7 @@ static int apci3120_ai_insn_read(struct comedi_device *dev,
 					data[i] = (unsigned int) us_TmpValue;
 				}
 
-				devpriv->b_InterruptMode = APCI3120_EOC_MODE;	/*  Restore defaults. */
+				devpriv->b_InterruptMode = APCI3120_EOC_MODE;	/*  Restore defaults */
 			}
 			break;
 
@@ -591,7 +579,7 @@ static int apci3120_ai_insn_read(struct comedi_device *dev,
 			dev_err(dev->class_dev, "inputs wrong\n");
 
 		}
-		devpriv->ui_EocEosConversionTime = 0;	/*  re initializing the variable; */
+		devpriv->ui_EocEosConversionTime = 0;	/*  re initializing the variable */
 	}
 
 	return insn->n;
@@ -623,10 +611,10 @@ static int apci3120_reset(struct comedi_device *dev)
 	devpriv->us_OutputRegister = 0;
 	outw(devpriv->us_OutputRegister, dev->iobase + APCI3120_WR_ADDRESS);
 
-/*
- * Code to set the all anolog o/p channel to 0v 8191 is decimal
- * value for zero(0 v)volt in bipolar mode(default)
- */
+	/*
+	 * Code to set the all anolog o/p channel to 0v 8191 is decimal
+	 * value for zero(0 v)volt in bipolar mode(default)
+	 */
 	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_1, dev->iobase + APCI3120_ANALOG_OUTPUT_1);	/* channel 1 */
 	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_2, dev->iobase + APCI3120_ANALOG_OUTPUT_1);	/* channel 2 */
 	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_3, dev->iobase + APCI3120_ANALOG_OUTPUT_1);	/* channel 3 */
@@ -637,9 +625,6 @@ static int apci3120_reset(struct comedi_device *dev)
 	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_7, dev->iobase + APCI3120_ANALOG_OUTPUT_2);	/* channel 7 */
 	outw(8191 | APCI3120_ANALOG_OP_CHANNEL_8, dev->iobase + APCI3120_ANALOG_OUTPUT_2);	/* channel 8 */
 
-	/*   Reset digital output to L0W */
-
-/* ES05  outb(0x0,dev->iobase+APCI3120_DIGITAL_OUTPUT); */
 	udelay(10);
 
 	inw(dev->iobase + 0);	/* make a dummy read */
@@ -689,12 +674,6 @@ static int apci3120_cancel(struct comedi_device *dev,
 	/* Disable BUS Master PCI */
 	outl(0, devpriv->i_IobaseAmcc + AMCC_OP_REG_MCSR);
 
-	/* outl(inl(devpriv->i_IobaseAmcc+AMCC_OP_REG_INTCSR)&(~AINT_WRITE_COMPL),
-	 * devpriv->i_IobaseAmcc+AMCC_OP_REG_INTCSR);  stop amcc irqs */
-
-	/* outl(inl(devpriv->i_IobaseAmcc+AMCC_OP_REG_MCSR)&(~EN_A2P_TRANSFERS),
-	 * devpriv->i_IobaseAmcc+AMCC_OP_REG_MCSR);  stop DMA */
-
 	/* Disable ext trigger */
 	apci3120_exttrig_disable(dev);
 
@@ -802,7 +781,7 @@ static int apci3120_cyclic_ai(int mode,
 			      struct comedi_device *dev,
 			      struct comedi_subdevice *s)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned char b_Tmp;
@@ -811,53 +790,29 @@ static int apci3120_cyclic_ai(int mode,
 		0, ui_TimerValue0, ui_ConvertTiming;
 	unsigned short us_TmpValue;
 
-	/*******************/
 	/* Resets the FIFO */
-	/*******************/
 	inb(dev->iobase + APCI3120_RESET_FIFO);
 
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	/* inw(dev->iobase+APCI3120_RD_STATUS); */
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
 	devpriv->ai_running = 1;
 
 	/*  clear software  registers */
 	devpriv->b_TimerSelectMode = 0;
 	devpriv->us_OutputRegister = 0;
 	devpriv->b_ModeSelectRegister = 0;
-	/* devpriv->b_DigitalOutputRegister=0; */
-
-	/* COMMENT JK 07.05.04: Followings calls are in i_APCI3120_StartAnalogInputAcquisition */
 
-	/****************************/
 	/* Clear Timer Write TC int */
-	/****************************/
 	outl(APCI3120_CLEAR_WRITE_TC_INT,
 		devpriv->i_IobaseAmcc + APCI3120_AMCC_OP_REG_INTCSR);
 
-	/************************************/
-	/* Clears the timer status register */
-	/************************************/
-
-	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	/* inw(dev->iobase+APCI3120_TIMER_STATUS_REGISTER); */
-	/* inb(dev->iobase + APCI3120_TIMER_STATUS_REGISTER); */
-	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
-	/**************************/
 	/* Disables All Timer     */
 	/* Sets PR and PA to 0    */
-	/**************************/
 	devpriv->us_OutputRegister = devpriv->us_OutputRegister &
 		APCI3120_DISABLE_TIMER0 &
 		APCI3120_DISABLE_TIMER1 & APCI3120_CLEAR_PA_PR;
 
 	outw(devpriv->us_OutputRegister, dev->iobase + APCI3120_WR_ADDRESS);
 
-	/*******************/
 	/* Resets the FIFO */
-	/*******************/
 	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
 	inb(devpriv->iobase + APCI3120_RESET_FIFO);
 	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
@@ -866,44 +821,21 @@ static int apci3120_cyclic_ai(int mode,
 	s->async->cur_chan = 0;
 	devpriv->ui_DmaActualBuffer = 0;
 
-	/*  value for timer2  minus -2 has to be done .....dunno y?? */
+	/* value for timer2  minus -2 has to be done */
 	ui_TimerValue2 = cmd->stop_arg - 2;
 	ui_ConvertTiming = cmd->convert_arg;
 
 	if (mode == 2)
 		ui_DelayTiming = cmd->scan_begin_arg;
 
-   /**********************************/
 	/* Initializes the sequence array */
-   /**********************************/
 	if (!apci3120_setup_chan_list(dev, s, devpriv->ui_AiNbrofChannels,
 			cmd->chanlist, 0))
 		return -EINVAL;
 
 	us_TmpValue = (unsigned short) inw(dev->iobase + APCI3120_RD_STATUS);
-/*** EL241003 : add this section in comment because floats must not be used
-	if((us_TmpValue & 0x00B0)==0x00B0)
-	 {
-		f_ConvertValue=(((float)ui_ConvertTiming * 0.002) - 2);
-		ui_TimerValue0=(unsigned int)f_ConvertValue;
-		if (mode==2)
-		{
-			f_DelayValue     = (((float)ui_DelayTiming * 0.00002) - 2);
-			ui_TimerValue1  =   (unsigned int) f_DelayValue;
-		}
-	 }
-	else
-	 {
-		f_ConvertValue=(((float)ui_ConvertTiming * 0.0012926) - 1);
-		ui_TimerValue0=(unsigned int)f_ConvertValue;
-		if (mode == 2)
-		{
-		     f_DelayValue     = (((float)ui_DelayTiming * 0.000012926) - 1);
-		     ui_TimerValue1  =   (unsigned int) f_DelayValue;
-		}
-	}
-***********************************************************************************************/
-/*** EL241003 Begin : add this section to replace floats calculation by integer calculations **/
+
+	/* EL241003 Begin: add this section to replace floats calculation by integer calculations */
 	/* EL250804: Testing if board APCI3120 have the new Quartz or if it is an APCI3001 */
 	if ((us_TmpValue & 0x00B0) == 0x00B0
 		|| !strcmp(this_board->pc_DriverName, "apci3001")) {
@@ -926,7 +858,7 @@ static int apci3120_cyclic_ai(int mode,
 			ui_TimerValue1 = ui_TimerValue1 / 1000000;
 		}
 	}
-/*** EL241003 End ******************************************************************************/
+	/* EL241003 End */
 
 	if (devpriv->b_ExttrigEnable == APCI3120_ENABLE)
 		apci3120_exttrig_enable(dev);	/*  activate EXT trigger */
@@ -985,15 +917,8 @@ static int apci3120_cyclic_ai(int mode,
 		break;
 
 	}
-	/*    ##########common for all modes################# */
-
-	/***********************/
-	/* Clears the SCAN bit */
-	/***********************/
-
+	/* common for all modes */
 	/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-	/* devpriv->b_ModeSelectRegister=devpriv->b_ModeSelectRegister | APCI3120_DISABLE_SCAN; */
-
 	devpriv->b_ModeSelectRegister = devpriv->b_ModeSelectRegister &
 		APCI3120_DISABLE_SCAN;
 	/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
@@ -1015,10 +940,10 @@ static int apci3120_cyclic_ai(int mode,
 			dev->iobase + APCI3120_WRITE_MODE_SELECT);
 
 		if (cmd->stop_src == TRIG_COUNT) {
-/*
- * configure Timer2 For counting EOS Reset gate 2 of Timer 2 to
- * disable it (Set Bit D14 to 0)
- */
+			/*
+			 * configure Timer2 For counting EOS Reset gate 2 of Timer 2 to
+			 * disable it (Set Bit D14 to 0)
+			 */
 			devpriv->us_OutputRegister =
 				devpriv->
 				us_OutputRegister & APCI3120_DISABLE_TIMER2;
@@ -1084,14 +1009,9 @@ static int apci3120_cyclic_ai(int mode,
 		/* If DMA Enabled */
 		unsigned int scan_bytes = cmd->scan_end_arg * sizeof(short);
 
-		/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-		/* inw(dev->iobase+0); reset EOC bit */
-		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 		devpriv->b_InterruptMode = APCI3120_DMA_MODE;
 
-		/************************************/
 		/* Disables the EOC, EOS interrupt  */
-		/************************************/
 		devpriv->b_ModeSelectRegister = devpriv->b_ModeSelectRegister &
 			APCI3120_DISABLE_EOC_INT & APCI3120_DISABLE_EOS_INT;
 
@@ -1114,7 +1034,7 @@ static int apci3120_cyclic_ai(int mode,
 					  dmalen0;
 		}
 
-		if (cmd->flags & TRIG_WAKE_EOS) {
+		if (cmd->flags & CMDF_WAKE_EOS) {
 			/*  don't we want wake up every scan? */
 			if (dmalen0 > scan_bytes) {
 				dmalen0 = scan_bytes;
@@ -1139,17 +1059,15 @@ static int apci3120_cyclic_ai(int mode,
 
 		/* Initialize DMA */
 
-/*
- * Set Transfer count enable bit and A2P_fifo reset bit in AGCSTS
- * register 1
- */
+		/*
+		 * Set Transfer count enable bit and A2P_fifo reset bit in AGCSTS
+		 * register 1
+		 */
 		ui_Tmp = AGCSTS_TC_ENABLE | AGCSTS_RESET_A2P_FIFO;
 		outl(ui_Tmp, devpriv->i_IobaseAmcc + AMCC_OP_REG_AGCSTS);
 
 		/*  changed  since 16 bit interface for add on */
-		/*********************/
 		/* ENABLE BUS MASTER */
-		/*********************/
 		outw(APCI3120_ADD_ON_AGCSTS_LOW, devpriv->i_IobaseAddon + 0);
 		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_LOW,
 			devpriv->i_IobaseAddon + 2);
@@ -1158,112 +1076,88 @@ static int apci3120_cyclic_ai(int mode,
 		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH,
 			devpriv->i_IobaseAddon + 2);
 
-/*
- * TO VERIFIED BEGIN JK 07.05.04: Comparison between WIN32 and Linux
- * driver
- */
+		/*
+		 * TO VERIFIED BEGIN JK 07.05.04: Comparison between WIN32 and Linux
+		 * driver
+		 */
 		outw(0x1000, devpriv->i_IobaseAddon + 2);
 		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 
 		/* 2 No change */
 		/* A2P FIFO MANAGEMENT */
 		/* A2P fifo reset & transfer control enable */
-
-		/***********************/
-		/* A2P FIFO MANAGEMENT */
-		/***********************/
 		outl(APCI3120_A2P_FIFO_MANAGEMENT, devpriv->i_IobaseAmcc +
 			APCI3120_AMCC_OP_MCSR);
 
-/*
- * 3
- * beginning address of dma buf The 32 bit address of dma buffer
- * is converted into two 16 bit addresses Can done by using _attach
- * and put into into an array array used may be for differnet pages
- */
+		/*
+		 * 3
+		 * beginning address of dma buf The 32 bit address of dma buffer
+		 * is converted into two 16 bit addresses Can done by using _attach
+		 * and put into into an array array used may be for differnet pages
+		 */
 
 		/*  DMA Start Address Low */
 		outw(APCI3120_ADD_ON_MWAR_LOW, devpriv->i_IobaseAddon + 0);
 		outw((devpriv->ul_DmaBufferHw[0] & 0xFFFF),
 			devpriv->i_IobaseAddon + 2);
 
-		/*************************/
 		/* DMA Start Address High */
-		/*************************/
 		outw(APCI3120_ADD_ON_MWAR_HIGH, devpriv->i_IobaseAddon + 0);
 		outw((devpriv->ul_DmaBufferHw[0] / 65536),
 			devpriv->i_IobaseAddon + 2);
 
-/*
- * 4
- * amount of bytes to be transferred set transfer count used ADDON
- * MWTC register commented testing
- * outl(devpriv->ui_DmaBufferUsesize[0],
- * devpriv->i_IobaseAddon+AMCC_OP_REG_AMWTC);
- */
+		/*
+		 * 4
+		 * amount of bytes to be transferred set transfer count used ADDON
+		 * MWTC register commented testing
+		 */
 
-		/**************************/
 		/* Nbr of acquisition LOW */
-		/**************************/
 		outw(APCI3120_ADD_ON_MWTC_LOW, devpriv->i_IobaseAddon + 0);
 		outw((devpriv->ui_DmaBufferUsesize[0] & 0xFFFF),
 			devpriv->i_IobaseAddon + 2);
 
-		/***************************/
 		/* Nbr of acquisition HIGH */
-		/***************************/
 		outw(APCI3120_ADD_ON_MWTC_HIGH, devpriv->i_IobaseAddon + 0);
 		outw((devpriv->ui_DmaBufferUsesize[0] / 65536),
 			devpriv->i_IobaseAddon + 2);
 
-/*
- * 5
- * To configure A2P FIFO testing outl(
- * FIFO_ADVANCE_ON_BYTE_2,devpriv->i_IobaseAmcc+AMCC_OP_REG_INTCSR);
- */
+		/*
+		 * 5
+		 * To configure A2P FIFO testing outl(
+		 * FIFO_ADVANCE_ON_BYTE_2,devpriv->i_IobaseAmcc+AMCC_OP_REG_INTCSR);
+		 */
 
-		/******************/
 		/* A2P FIFO RESET */
-		/******************/
-/*
- * TO VERIFY BEGIN JK 07.05.04: Comparison between WIN32 and Linux
- * driver
- */
+		/*
+		 * TO VERIFY BEGIN JK 07.05.04: Comparison between WIN32 and Linux
+		 * driver
+		 */
 		outl(0x04000000UL, devpriv->i_IobaseAmcc + AMCC_OP_REG_MCSR);
 		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 
-/*
- * 6
- * ENABLE A2P FIFO WRITE AND ENABLE AMWEN AMWEN_ENABLE |
- * A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
- */
-
-		/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-		/* outw(3,devpriv->i_IobaseAddon + 4); */
-		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
-
-/*
- * 7
- * initialise end of dma interrupt AINT_WRITE_COMPL =
- * ENABLE_WRITE_TC_INT(ADDI)
- */
-		/***************************************************/
+		/*
+		 * 6
+		 * ENABLE A2P FIFO WRITE AND ENABLE AMWEN AMWEN_ENABLE |
+		 * A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
+		 */
+
+		/*
+		 * 7
+		 * initialise end of dma interrupt AINT_WRITE_COMPL =
+		 * ENABLE_WRITE_TC_INT(ADDI)
+		 */
 		/* A2P FIFO CONFIGURATE, END OF DMA intERRUPT INIT */
-		/***************************************************/
 		outl((APCI3120_FIFO_ADVANCE_ON_BYTE_2 |
 				APCI3120_ENABLE_WRITE_TC_INT),
 			devpriv->i_IobaseAmcc + AMCC_OP_REG_INTCSR);
 
 		/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
-		/******************************************/
 		/* ENABLE A2P FIFO WRITE AND ENABLE AMWEN */
-		/******************************************/
 		outw(3, devpriv->i_IobaseAddon + 4);
 		/* END JK 07.05.04: Comparison between WIN32 and Linux driver */
 
-		/******************/
 		/* A2P FIFO RESET */
-		/******************/
 		/* BEGIN JK 07.05.04: Comparison between WIN32 and Linux driver */
 		outl(0x04000000UL,
 			devpriv->i_IobaseAmcc + APCI3120_AMCC_OP_MCSR);
@@ -1323,8 +1217,8 @@ static int apci3120_ai_cmd(struct comedi_device *dev,
 
 	if (cmd->scan_begin_src == TRIG_FOLLOW)
 		return apci3120_cyclic_ai(1, dev, s);
-	else	/* TRIG_TIMER */
-		return apci3120_cyclic_ai(2, dev, s);
+	/* TRIG_TIMER */
+	return apci3120_cyclic_ai(2, dev, s);
 }
 
 /*
@@ -1416,11 +1310,11 @@ static void apci3120_interrupt_dma(int irq, void *d)
 		outw(APCI3120_ADD_ON_MWTC_HIGH, devpriv->i_IobaseAddon + 0);
 		outw(high_word, devpriv->i_IobaseAddon + 2);
 
-/*
- * To configure A2P FIFO
- * ENABLE A2P FIFO WRITE AND ENABLE AMWEN
- * AMWEN_ENABLE | A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
- */
+		/*
+		 * To configure A2P FIFO
+		 * ENABLE A2P FIFO WRITE AND ENABLE AMWEN
+		 * AMWEN_ENABLE | A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
+		 */
 		outw(3, devpriv->i_IobaseAddon + 4);
 		/* initialise end of dma interrupt  AINT_WRITE_COMPL = ENABLE_WRITE_TC_INT(ADDI) */
 		outl((APCI3120_FIFO_ADVANCE_ON_BYTE_2 |
@@ -1433,7 +1327,7 @@ static void apci3120_interrupt_dma(int irq, void *d)
 			devpriv->ul_DmaBufferVirtual[devpriv->
 				ui_DmaActualBuffer], samplesinbuf);
 
-		if (!(cmd->flags & TRIG_WAKE_EOS)) {
+		if (!(cmd->flags & CMDF_WAKE_EOS)) {
 			s->async->events |= COMEDI_CB_EOS;
 			comedi_event(dev, s);
 		}
@@ -1450,10 +1344,10 @@ static void apci3120_interrupt_dma(int irq, void *d)
 	if (devpriv->b_DmaDoubleBuffer) {	/*  switch dma buffers */
 		devpriv->ui_DmaActualBuffer = 1 - devpriv->ui_DmaActualBuffer;
 	} else {
-/*
- * restart DMA if is not used double buffering
- * ADDED REINITIALISE THE DMA
- */
+		/*
+		 * restart DMA if is not used double buffering
+		 * ADDED REINITIALISE THE DMA
+		 */
 		ui_Tmp = AGCSTS_TC_ENABLE | AGCSTS_RESET_A2P_FIFO;
 		outl(ui_Tmp, devpriv->i_IobaseAddon + AMCC_OP_REG_AGCSTS);
 
@@ -1462,11 +1356,11 @@ static void apci3120_interrupt_dma(int irq, void *d)
 		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_LOW,
 			devpriv->i_IobaseAddon + 2);
 		outw(APCI3120_ADD_ON_AGCSTS_HIGH, devpriv->i_IobaseAddon + 0);
-		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH, devpriv->i_IobaseAddon + 2);	/*  */
-/*
- * A2P FIFO MANAGEMENT
- * A2P fifo reset & transfer control enable
- */
+		outw(APCI3120_ENABLE_TRANSFER_ADD_ON_HIGH, devpriv->i_IobaseAddon + 2);
+		/*
+		 * A2P FIFO MANAGEMENT
+		 * A2P fifo reset & transfer control enable
+		 */
 		outl(APCI3120_A2P_FIFO_MANAGEMENT,
 			devpriv->i_IobaseAmcc + AMCC_OP_REG_MCSR);
 
@@ -1488,11 +1382,11 @@ static void apci3120_interrupt_dma(int irq, void *d)
 		outw(APCI3120_ADD_ON_MWTC_HIGH, devpriv->i_IobaseAddon + 0);
 		outw(high_word, devpriv->i_IobaseAddon + 2);
 
-/*
- * To configure A2P FIFO
- * ENABLE A2P FIFO WRITE AND ENABLE AMWEN
- * AMWEN_ENABLE | A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
- */
+		/*
+		 * To configure A2P FIFO
+		 * ENABLE A2P FIFO WRITE AND ENABLE AMWEN
+		 * AMWEN_ENABLE | A2P_FIFO_WRITE_ENABLE (0x01|0x02)=0x03
+		 */
 		outw(3, devpriv->i_IobaseAddon + 4);
 		/* initialise end of dma interrupt  AINT_WRITE_COMPL = ENABLE_WRITE_TC_INT(ADDI) */
 		outl((APCI3120_FIFO_ADVANCE_ON_BYTE_2 |
@@ -1570,7 +1464,6 @@ static void apci3120_interrupt(int irq, void *d)
 		if (devpriv->b_EocEosInterrupt == APCI3120_ENABLE) {
 
 			/*  Read the AI Value */
-
 			devpriv->ui_AiReadData[0] =
 				(unsigned int) inw(devpriv->iobase + 0);
 			devpriv->b_EocEosInterrupt = APCI3120_DISABLE;
@@ -1670,7 +1563,6 @@ static void apci3120_interrupt(int irq, void *d)
 		default:
 
 			/*  disable Timer Interrupt */
-
 			devpriv->b_ModeSelectRegister =
 				devpriv->
 				b_ModeSelectRegister &
@@ -1688,17 +1580,12 @@ static void apci3120_interrupt(int irq, void *d)
 	if ((int_daq & 0x4) && (devpriv->b_InterruptMode == APCI3120_DMA_MODE)) {
 		if (devpriv->ai_running) {
 
-			/****************************/
 			/* Clear Timer Write TC int */
-			/****************************/
-
 			outl(APCI3120_CLEAR_WRITE_TC_INT,
 				devpriv->i_IobaseAmcc +
 				APCI3120_AMCC_OP_REG_INTCSR);
 
-			/************************************/
 			/* Clears the timer status register */
-			/************************************/
 			inw(dev->iobase + APCI3120_TIMER_STATUS_REGISTER);
 			/* do some data transfer */
 			apci3120_interrupt_dma(irq, d);
@@ -1711,8 +1598,6 @@ static void apci3120_interrupt(int irq, void *d)
 		}
 
 	}
-
-	return;
 }
 
 /*
@@ -1728,7 +1613,7 @@ static int apci3120_config_insn_timer(struct comedi_device *dev,
 				      struct comedi_insn *insn,
 				      unsigned int *data)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	unsigned int ui_Timervalue2;
 	unsigned short us_TmpValue;
@@ -1741,13 +1626,12 @@ static int apci3120_config_insn_timer(struct comedi_device *dev,
 
 	ui_Timervalue2 = data[1] / 1000;	/*  convert nano seconds  to u seconds */
 
-	/* this_board->timer_config(dev, ui_Timervalue2,(unsigned char)data[0]); */
 	us_TmpValue = (unsigned short) inw(devpriv->iobase + APCI3120_RD_STATUS);
 
-/*
- * EL250804: Testing if board APCI3120 have the new Quartz or if it
- * is an APCI3001 and calculate the time value to set in the timer
- */
+	/*
+	 * EL250804: Testing if board APCI3120 have the new Quartz or if it
+	 * is an APCI3001 and calculate the time value to set in the timer
+	 */
 	if ((us_TmpValue & 0x00B0) == 0x00B0
 		|| !strcmp(this_board->pc_DriverName, "apci3001")) {
 		/* Calculate the time value to set in the timer */
@@ -1775,11 +1659,6 @@ static int apci3120_config_insn_timer(struct comedi_device *dev,
 	outb(devpriv->b_ModeSelectRegister,
 		devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
 	if (data[0] == APCI3120_TIMER) {	/* initialize timer */
-		/* devpriv->b_ModeSelectRegister=devpriv->b_ModeSelectRegister |
-		 * APCI3120_ENABLE_TIMER_INT; */
-
-		/* outb(devpriv->b_ModeSelectRegister,devpriv->iobase+APCI3120_WRITE_MODE_SELECT); */
-
 		/* Set the Timer 2 in mode 2(Timer) */
 		devpriv->b_TimerSelectMode =
 			(devpriv->
@@ -1787,13 +1666,13 @@ static int apci3120_config_insn_timer(struct comedi_device *dev,
 		outb(devpriv->b_TimerSelectMode,
 			devpriv->iobase + APCI3120_TIMER_CRT1);
 
-/*
- * Configure the timer 2 for writing the LOW unsigned short of timer
- * is Delay value You must make a b_tmp variable with
- * DigitalOutPutRegister because at Address_1+APCI3120_TIMER_CRT0
- * you can set the digital output and configure the timer 2,and if
- * you don't make this, digital output are erase (Set to 0)
- */
+		/*
+		 * Configure the timer 2 for writing the LOW unsigned short of timer
+		 * is Delay value You must make a b_tmp variable with
+		 * DigitalOutPutRegister because at Address_1+APCI3120_TIMER_CRT0
+		 * you can set the digital output and configure the timer 2,and if
+		 * you don't make this, digital output are erase (Set to 0)
+		 */
 
 		/* Writing LOW unsigned short */
 		b_Tmp = ((devpriv->
@@ -1816,20 +1695,19 @@ static int apci3120_config_insn_timer(struct comedi_device *dev,
 	} else {			/*  Initialize Watch dog */
 
 		/* Set the Timer 2 in mode 5(Watchdog) */
-
 		devpriv->b_TimerSelectMode =
 			(devpriv->
 			b_TimerSelectMode & 0x0F) | APCI3120_TIMER_2_MODE_5;
 		outb(devpriv->b_TimerSelectMode,
 			devpriv->iobase + APCI3120_TIMER_CRT1);
 
-/*
- * Configure the timer 2 for writing the LOW unsigned short of timer
- * is Delay value You must make a b_tmp variable with
- * DigitalOutPutRegister because at Address_1+APCI3120_TIMER_CRT0
- * you can set the digital output and configure the timer 2,and if
- * you don't make this, digital output are erase (Set to 0)
- */
+		/*
+		 * Configure the timer 2 for writing the LOW unsigned short of timer
+		 * is Delay value You must make a b_tmp variable with
+		 * DigitalOutPutRegister because at Address_1+APCI3120_TIMER_CRT0
+		 * you can set the digital output and configure the timer 2,and if
+		 * you don't make this, digital output are erase (Set to 0)
+		 */
 
 		/* Writing LOW unsigned short */
 		b_Tmp = ((devpriv->
@@ -1873,7 +1751,7 @@ static int apci3120_write_insn_timer(struct comedi_device *dev,
 				     struct comedi_insn *insn,
 				     unsigned int *data)
 {
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 	struct addi_private *devpriv = dev->private;
 	unsigned int ui_Timervalue2 = 0;
 	unsigned short us_TmpValue;
@@ -1898,8 +1776,6 @@ static int apci3120_write_insn_timer(struct comedi_device *dev,
 			ui_Timervalue2 = 0;
 	}
 
-	/* this_board->timer_write(dev,data[0],ui_Timervalue2); */
-
 	switch (data[0]) {
 	case APCI3120_START:
 
@@ -1937,7 +1813,7 @@ static int apci3120_write_insn_timer(struct comedi_device *dev,
 			devpriv->iobase + APCI3120_WRITE_MODE_SELECT);
 
 		if (devpriv->b_Timer2Mode == APCI3120_TIMER) {	/* start timer */
-			/* For Timer mode is  Gate2 must be activated   **timer started */
+			/* For Timer mode is  Gate2 must be activated	timer started */
 			devpriv->us_OutputRegister =
 				devpriv->
 				us_OutputRegister | APCI3120_ENABLE_TIMER2;
@@ -1979,9 +1855,6 @@ static int apci3120_write_insn_timer(struct comedi_device *dev,
 		/*  Reset FC_TIMER BIT */
 		inb(devpriv->iobase + APCI3120_TIMER_STATUS_REGISTER);
 
-		/* Disable timer */
-		/* devpriv->b_Timer2Mode=APCI3120_DISABLE;  */
-
 		break;
 
 	case 2:		/* write new value to Timer */
@@ -1990,14 +1863,13 @@ static int apci3120_write_insn_timer(struct comedi_device *dev,
 				"timer2 not configured in TIMER MODE\n");
 			return -EINVAL;
 		}
-		/*  ui_Timervalue2=data[1]; // passed as argument */
 		us_TmpValue =
 			(unsigned short) inw(devpriv->iobase + APCI3120_RD_STATUS);
 
-/*
- * EL250804: Testing if board APCI3120 have the new Quartz or if it
- * is an APCI3001 and calculate the time value to set in the timer
- */
+		/*
+		 * EL250804: Testing if board APCI3120 have the new Quartz or if it
+		 * is an APCI3001 and calculate the time value to set in the timer
+		 */
 		if ((us_TmpValue & 0x00B0) == 0x00B0
 			|| !strcmp(this_board->pc_DriverName, "apci3001")) {
 			/* Calculate the time value to set in the timer */
@@ -2053,8 +1925,6 @@ static int apci3120_read_insn_timer(struct comedi_device *dev,
 		&& (devpriv->b_Timer2Mode != APCI3120_TIMER)) {
 		dev_err(dev->class_dev, "timer2 not configured\n");
 	}
-
-	/* this_board->timer_read(dev,data); */
 	if (devpriv->b_Timer2Mode == APCI3120_TIMER) {
 
 		/* Read the LOW unsigned short of Timer 2 register */
@@ -2137,7 +2007,6 @@ static int apci3120_ao_insn_write(struct comedi_device *dev,
 	ui_Range = CR_RANGE(insn->chanspec);
 	ui_Channel = CR_CHAN(insn->chanspec);
 
-	/* this_board->ao_write(dev, ui_Range, ui_Channel,data[0]); */
 	if (ui_Range) {		/*  if 1 then unipolar */
 
 		if (data[0] != 0)
@@ -2163,17 +2032,17 @@ static int apci3120_ao_insn_write(struct comedi_device *dev,
 	} while (us_TmpValue != 0x0001);
 
 	if (ui_Channel <= 3)
-/*
- * for channel 0-3 out at the register 1 (wrDac1-8) data[i]
- * typecasted to ushort since word write is to be done
- */
+		/*
+		 * for channel 0-3 out at the register 1 (wrDac1-8) data[i]
+		 * typecasted to ushort since word write is to be done
+		 */
 		outw((unsigned short) data[0],
 			devpriv->iobase + APCI3120_ANALOG_OUTPUT_1);
 	else
-/*
- * for channel 4-7 out at the register 2 (wrDac5-8) data[i]
- * typecasted to ushort since word write is to be done
- */
+		/*
+		 * for channel 4-7 out at the register 2 (wrDac5-8) data[i]
+		 * typecasted to ushort since word write is to be done
+		 */
 		outw((unsigned short) data[0],
 			devpriv->iobase + APCI3120_ANALOG_OUTPUT_2);
 
diff --git a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
index e82c3fcd048b..339519a3d6b5 100644
--- a/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
+++ b/drivers/staging/comedi/drivers/addi-data/hwdrv_apci3501.c
@@ -168,7 +168,7 @@ static int apci3501_read_insn_timer(struct comedi_device *dev,
 
 	else if ((devpriv->b_TimerSelectMode != ADDIDATA_TIMER)
 		&& (devpriv->b_TimerSelectMode != ADDIDATA_WATCHDOG)) {
-		printk("\nIn ReadTimerCounterWatchdog :: Invalid Subdevice \n");
+		dev_err(dev->class_dev, "Invalid subdevice.\n");
 	}
 	return insn->n;
 }
diff --git a/drivers/staging/comedi/drivers/addi_apci_1032.c b/drivers/staging/comedi/drivers/addi_apci_1032.c
index 1b2e7c040c9f..840cb289507a 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1032.c
@@ -190,9 +190,6 @@ static int apci1032_cos_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
@@ -204,10 +201,9 @@ static int apci1032_cos_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: ignored */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
@@ -347,9 +343,7 @@ static void apci1032_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		apci1032_reset(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver apci1032_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_1516.c b/drivers/staging/comedi/drivers/addi_apci_1516.c
index e9c5291c77cd..55d00fd94c91 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1516.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1516.c
@@ -102,7 +102,7 @@ static int apci1516_do_insn_bits(struct comedi_device *dev,
 
 static int apci1516_reset(struct comedi_device *dev)
 {
-	const struct apci1516_boardinfo *this_board = comedi_board(dev);
+	const struct apci1516_boardinfo *this_board = dev->board_ptr;
 	struct apci1516_private *devpriv = dev->private;
 
 	if (!this_board->has_wdog)
@@ -190,7 +190,7 @@ static void apci1516_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		apci1516_reset(dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver apci1516_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_1564.c b/drivers/staging/comedi/drivers/addi_apci_1564.c
index 543cb074213a..688b015a834e 100644
--- a/drivers/staging/comedi/drivers/addi_apci_1564.c
+++ b/drivers/staging/comedi/drivers/addi_apci_1564.c
@@ -1,3 +1,26 @@
+/*
+ * addi_apci_1564.c
+ * Copyright (C) 2004,2005  ADDI-DATA GmbH for the source code of this module.
+ *
+ *	ADDI-DATA GmbH
+ *	Dieselstrasse 3
+ *	D-77833 Ottersweier
+ *	Tel: +19(0)7223/9493-0
+ *	Fax: +49(0)7223/9493-92
+ *	http://www.addi-data.com
+ *	info@addi-data.com
+ *
+ * This program is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License as published by the Free Software
+ * Foundation; either version 2 of the License, or (at your option) any later
+ * version.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
+ * FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
+ * details.
+ */
+
 #include <linux/module.h>
 #include <linux/pci.h>
 #include <linux/interrupt.h>
@@ -42,10 +65,10 @@ static int apci1564_reset(struct comedi_device *dev)
 	outl(0x0, devpriv->amcc_iobase + APCI1564_TIMER_RELOAD_REG);
 
 	/* Reset the counter registers */
-	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER1));
-	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER2));
-	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER3));
-	outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(APCI1564_COUNTER4));
+	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER1));
+	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER2));
+	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER3));
+	outl(0x0, dev->iobase + APCI1564_COUNTER_CTRL_REG(APCI1564_COUNTER4));
 
 	return 0;
 }
@@ -94,17 +117,20 @@ static irqreturn_t apci1564_interrupt(int irq, void *d)
 	}
 
 	for (chan = 0; chan < 4; chan++) {
-		status = inl(dev->iobase + APCI1564_TCW_IRQ_REG(chan));
+		status = inl(dev->iobase + APCI1564_COUNTER_IRQ_REG(chan));
 		if (status & 0x01) {
 			/*  Disable Counter Interrupt */
-			ctrl = inl(dev->iobase + APCI1564_TCW_CTRL_REG(chan));
-			outl(0x0, dev->iobase + APCI1564_TCW_CTRL_REG(chan));
+			ctrl = inl(dev->iobase +
+				  APCI1564_COUNTER_CTRL_REG(chan));
+			outl(0x0, dev->iobase +
+			    APCI1564_COUNTER_CTRL_REG(chan));
 
 			/* Send a signal to from kernel to user space */
 			send_sig(SIGIO, devpriv->tsk_current, 0);
 
 			/*  Enable Counter Interrupt */
-			outl(ctrl, dev->iobase + APCI1564_TCW_CTRL_REG(chan));
+			outl(ctrl, dev->iobase +
+			    APCI1564_COUNTER_CTRL_REG(chan));
 		}
 	}
 
@@ -282,9 +308,6 @@ static int apci1564_cos_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
@@ -296,10 +319,9 @@ static int apci1564_cos_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: ignored */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
@@ -374,53 +396,52 @@ static int apci1564_auto_attach(struct comedi_device *dev,
 
 	/*  Allocate and Initialise DI Subdevice Structures */
 	s = &dev->subdevices[0];
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 32;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = apci1564_di_insn_bits;
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 32;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= apci1564_di_insn_bits;
 
 	/*  Allocate and Initialise DO Subdevice Structures */
 	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITEABLE;
-	s->n_chan = 32;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_config = apci1564_do_config;
-	s->insn_bits = apci1564_do_insn_bits;
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITEABLE;
+	s->n_chan	= 32;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= apci1564_do_insn_bits;
 
 	/* Change-Of-State (COS) interrupt subdevice */
 	s = &dev->subdevices[2];
 	if (dev->irq) {
 		dev->read_subdev = s;
-		s->type = COMEDI_SUBD_DI;
-		s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
-		s->n_chan = 1;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->len_chanlist = 1;
-		s->insn_config = apci1564_cos_insn_config;
-		s->insn_bits = apci1564_cos_insn_bits;
-		s->do_cmdtest = apci1564_cos_cmdtest;
-		s->do_cmd = apci1564_cos_cmd;
-		s->cancel = apci1564_cos_cancel;
+		s->type		= COMEDI_SUBD_DI;
+		s->subdev_flags	= SDF_READABLE | SDF_CMD_READ;
+		s->n_chan	= 1;
+		s->maxdata	= 1;
+		s->range_table	= &range_digital;
+		s->len_chanlist	= 1;
+		s->insn_config	= apci1564_cos_insn_config;
+		s->insn_bits	= apci1564_cos_insn_bits;
+		s->do_cmdtest	= apci1564_cos_cmdtest;
+		s->do_cmd	= apci1564_cos_cmd;
+		s->cancel	= apci1564_cos_cancel;
 	} else {
-		s->type = COMEDI_SUBD_UNUSED;
+		s->type		= COMEDI_SUBD_UNUSED;
 	}
 
 	/*  Allocate and Initialise Timer Subdevice Structures */
 	s = &dev->subdevices[3];
-	s->type = COMEDI_SUBD_TIMER;
-	s->subdev_flags = SDF_WRITEABLE;
-	s->n_chan = 1;
-	s->maxdata = 0;
-	s->len_chanlist = 1;
-	s->range_table = &range_digital;
-	s->insn_write = apci1564_timer_write;
-	s->insn_read = apci1564_timer_read;
-	s->insn_config = apci1564_timer_config;
+	s->type		= COMEDI_SUBD_TIMER;
+	s->subdev_flags	= SDF_WRITEABLE;
+	s->n_chan	= 1;
+	s->maxdata	= 0;
+	s->len_chanlist	= 1;
+	s->range_table	= &range_digital;
+	s->insn_write	= apci1564_timer_write;
+	s->insn_read	= apci1564_timer_read;
+	s->insn_config	= apci1564_timer_config;
 
 	/* Initialize the watchdog subdevice */
 	s = &dev->subdevices[4];
@@ -430,12 +451,12 @@ static int apci1564_auto_attach(struct comedi_device *dev,
 
 	/* Initialize the diagnostic status subdevice */
 	s = &dev->subdevices[5];
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE;
-	s->n_chan = 2;
-	s->maxdata = 1;
-	s->range_table = &range_digital;
-	s->insn_bits = apci1564_diag_insn_bits;
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 2;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= apci1564_diag_insn_bits;
 
 	return 0;
 }
@@ -444,9 +465,7 @@ static void apci1564_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		apci1564_reset(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver apci1564_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_16xx.c b/drivers/staging/comedi/drivers/addi_apci_16xx.c
index 28df4b50b87a..4162e2dc2860 100644
--- a/drivers/staging/comedi/drivers/addi_apci_16xx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_16xx.c
@@ -159,7 +159,7 @@ static struct comedi_driver apci16xx_driver = {
 	.driver_name	= "addi_apci_16xx",
 	.module		= THIS_MODULE,
 	.auto_attach	= apci16xx_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int apci16xx_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/addi_apci_2032.c b/drivers/staging/comedi/drivers/addi_apci_2032.c
index be0a8a7bd3b2..aea3da325359 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2032.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2032.c
@@ -86,30 +86,6 @@ static void apci2032_int_stop(struct comedi_device *dev,
 	outl(0x0, dev->iobase + APCI2032_INT_CTRL_REG);
 }
 
-static bool apci2032_int_start(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       unsigned char enabled_isns)
-{
-	struct apci2032_int_private *subpriv = s->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-	bool do_event;
-
-	subpriv->enabled_isns = enabled_isns;
-	subpriv->stop_count = cmd->stop_arg;
-	if (cmd->stop_src == TRIG_COUNT && subpriv->stop_count == 0) {
-		/* An empty acquisition! */
-		s->async->events |= COMEDI_CB_EOA;
-		subpriv->active = false;
-		do_event = true;
-	} else {
-		subpriv->active = true;
-		outl(enabled_isns, dev->iobase + APCI2032_INT_CTRL_REG);
-		do_event = false;
-	}
-
-	return do_event;
-}
-
 static int apci2032_int_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
@@ -141,16 +117,17 @@ static int apci2032_int_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-	if (cmd->stop_src == TRIG_NONE)
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
 		return 3;
 
-	/* step 4: ignored */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
@@ -163,18 +140,19 @@ static int apci2032_int_cmd(struct comedi_device *dev,
 	unsigned char enabled_isns;
 	unsigned int n;
 	unsigned long flags;
-	bool do_event;
 
 	enabled_isns = 0;
 	for (n = 0; n < cmd->chanlist_len; n++)
 		enabled_isns |= 1 << CR_CHAN(cmd->chanlist[n]);
 
 	spin_lock_irqsave(&subpriv->spinlock, flags);
-	do_event = apci2032_int_start(dev, s, enabled_isns);
-	spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
-	if (do_event)
-		comedi_event(dev, s);
+	subpriv->enabled_isns = enabled_isns;
+	subpriv->stop_count = cmd->stop_arg;
+	subpriv->active = true;
+	outl(enabled_isns, dev->iobase + APCI2032_INT_CTRL_REG);
+
+	spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
 	return 0;
 }
@@ -339,11 +317,9 @@ static void apci2032_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		apci2032_reset(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
+	comedi_pci_detach(dev);
 	if (dev->read_subdev)
 		kfree(dev->read_subdev->private);
-	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci2032_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_2200.c b/drivers/staging/comedi/drivers/addi_apci_2200.c
index e1a916546d18..51ab1f937bae 100644
--- a/drivers/staging/comedi/drivers/addi_apci_2200.c
+++ b/drivers/staging/comedi/drivers/addi_apci_2200.c
@@ -118,7 +118,7 @@ static void apci2200_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		apci2200_reset(dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver apci2200_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_3120.c b/drivers/staging/comedi/drivers/addi_apci_3120.c
index 0b77f1012d47..ba71e24a56fd 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3120.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3120.c
@@ -44,7 +44,7 @@ static const struct addi_board apci3120_boardtypes[] = {
 static irqreturn_t v_ADDI_Interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	const struct addi_board *this_board = comedi_board(dev);
+	const struct addi_board *this_board = dev->board_ptr;
 
 	this_board->interrupt(irq, d);
 	return IRQ_RETVAL(1);
@@ -57,7 +57,7 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 	const struct addi_board *this_board = NULL;
 	struct addi_private *devpriv;
 	struct comedi_subdevice *s;
-	int ret, pages, i;
+	int ret, order, i;
 
 	if (context < ARRAY_SIZE(apci3120_boardtypes))
 		this_board = &apci3120_boardtypes[context];
@@ -88,28 +88,23 @@ static int apci3120_auto_attach(struct comedi_device *dev,
 			dev->irq = pcidev->irq;
 	}
 
-	devpriv->us_UseDma = 1;
-
 	/* Allocate DMA buffers */
-	devpriv->b_DmaDoubleBuffer = 0;
 	for (i = 0; i < 2; i++) {
-		for (pages = 4; pages >= 0; pages--) {
+		for (order = 2; order >= 0; order--) {
 			devpriv->ul_DmaBufferVirtual[i] =
-				(void *) __get_free_pages(GFP_KERNEL, pages);
+			    dma_alloc_coherent(dev->hw_dev, PAGE_SIZE << order,
+					       &devpriv->ul_DmaBufferHw[i],
+					       GFP_KERNEL);
 
 			if (devpriv->ul_DmaBufferVirtual[i])
 				break;
 		}
-		if (devpriv->ul_DmaBufferVirtual[i]) {
-			devpriv->ui_DmaBufferPages[i] = pages;
-			devpriv->ui_DmaBufferSize[i] = PAGE_SIZE * pages;
-			devpriv->ul_DmaBufferHw[i] =
-				virt_to_bus((void *)devpriv->
-				ul_DmaBufferVirtual[i]);
-		}
+		if (!devpriv->ul_DmaBufferVirtual[i])
+			break;
+		devpriv->ui_DmaBufferSize[i] = PAGE_SIZE << order;
 	}
-	if (!devpriv->ul_DmaBufferVirtual[0])
-		devpriv->us_UseDma = 0;
+	if (devpriv->ul_DmaBufferVirtual[0])
+		devpriv->us_UseDma = 1;
 
 	if (devpriv->ul_DmaBufferVirtual[1])
 		devpriv->b_DmaDoubleBuffer = 1;
@@ -195,23 +190,22 @@ static void apci3120_detach(struct comedi_device *dev)
 {
 	struct addi_private *devpriv = dev->private;
 
+	if (dev->iobase)
+		apci3120_reset(dev);
+	comedi_pci_detach(dev);
 	if (devpriv) {
-		if (dev->iobase)
-			apci3120_reset(dev);
-		if (dev->irq)
-			free_irq(dev->irq, dev);
-		if (devpriv->ul_DmaBufferVirtual[0]) {
-			free_pages((unsigned long)devpriv->
-				ul_DmaBufferVirtual[0],
-				devpriv->ui_DmaBufferPages[0]);
-		}
-		if (devpriv->ul_DmaBufferVirtual[1]) {
-			free_pages((unsigned long)devpriv->
-				ul_DmaBufferVirtual[1],
-				devpriv->ui_DmaBufferPages[1]);
+		unsigned int i;
+
+		for (i = 0; i < 2; i++) {
+			if (devpriv->ul_DmaBufferVirtual[i]) {
+				dma_free_coherent(dev->hw_dev,
+						  devpriv->ui_DmaBufferSize[i],
+						  devpriv->
+						  ul_DmaBufferVirtual[i],
+						  devpriv->ul_DmaBufferHw[i]);
+			}
 		}
 	}
-	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver apci3120_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_3501.c b/drivers/staging/comedi/drivers/addi_apci_3501.c
index d9594f48d00f..010efa3fed6c 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3501.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3501.c
@@ -104,9 +104,9 @@ static int apci3501_ao_insn_write(struct comedi_device *dev,
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int range = CR_RANGE(insn->chanspec);
-	unsigned int val = 0;
-	int i;
+	unsigned int cfg = APCI3501_AO_DATA_CHAN(chan);
 	int ret;
+	int i;
 
 	/*
 	 * All analog output channels have the same output range.
@@ -117,14 +117,14 @@ static int apci3501_ao_insn_write(struct comedi_device *dev,
 	if (range) {
 		outl(0, dev->iobase + APCI3501_AO_CTRL_STATUS_REG);
 	} else {
-		val |= APCI3501_AO_DATA_BIPOLAR;
+		cfg |= APCI3501_AO_DATA_BIPOLAR;
 		outl(APCI3501_AO_CTRL_BIPOLAR,
 		     dev->iobase + APCI3501_AO_CTRL_STATUS_REG);
 	}
 
-	val |= APCI3501_AO_DATA_CHAN(chan);
-
 	for (i = 0; i < insn->n; i++) {
+		unsigned int val = data[i];
+
 		if (range == 1) {
 			if (data[i] > 0x1fff) {
 				dev_err(dev->class_dev,
@@ -137,8 +137,10 @@ static int apci3501_ao_insn_write(struct comedi_device *dev,
 		if (ret)
 			return ret;
 
-		outl(val | APCI3501_AO_DATA_VAL(data[i]),
+		outl(cfg | APCI3501_AO_DATA_VAL(val),
 		     dev->iobase + APCI3501_AO_DATA_REG);
+
+		s->readback[chan] = val;
 	}
 
 	return insn->n;
@@ -360,6 +362,11 @@ static int apci3501_auto_attach(struct comedi_device *dev,
 		s->maxdata	= 0x3fff;
 		s->range_table	= &apci3501_ao_range;
 		s->insn_write	= apci3501_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
@@ -410,9 +417,7 @@ static void apci3501_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		apci3501_reset(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver apci3501_driver = {
diff --git a/drivers/staging/comedi/drivers/addi_apci_3xxx.c b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
index 0f0c7fa5daa3..a296bd5b2c0c 100644
--- a/drivers/staging/comedi/drivers/addi_apci_3xxx.c
+++ b/drivers/staging/comedi/drivers/addi_apci_3xxx.c
@@ -472,7 +472,7 @@ static int apci3xxx_ai_insn_read(struct comedi_device *dev,
 static int apci3xxx_ai_ns_to_timer(struct comedi_device *dev,
 				   unsigned int *ns, unsigned int flags)
 {
-	const struct apci3xxx_boardinfo *board = comedi_board(dev);
+	const struct apci3xxx_boardinfo *board = dev->board_ptr;
 	struct apci3xxx_private *devpriv = dev->private;
 	unsigned int base;
 	unsigned int timer;
@@ -496,15 +496,15 @@ static int apci3xxx_ai_ns_to_timer(struct comedi_device *dev,
 			break;
 		}
 
-		switch (flags & TRIG_ROUND_MASK) {
-		case TRIG_ROUND_NEAREST:
+		switch (flags & CMDF_ROUND_MASK) {
+		case CMDF_ROUND_NEAREST:
 		default:
 			timer = (*ns + base / 2) / base;
 			break;
-		case TRIG_ROUND_DOWN:
+		case CMDF_ROUND_DOWN:
 			timer = *ns / base;
 			break;
-		case TRIG_ROUND_UP:
+		case CMDF_ROUND_UP:
 			timer = (*ns + base - 1) / base;
 			break;
 		}
@@ -523,7 +523,7 @@ static int apci3xxx_ai_cmdtest(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_cmd *cmd)
 {
-	const struct apci3xxx_boardinfo *board = comedi_board(dev);
+	const struct apci3xxx_boardinfo *board = dev->board_ptr;
 	int err = 0;
 	unsigned int arg;
 
@@ -628,16 +628,20 @@ static int apci3xxx_ao_insn_write(struct comedi_device *dev,
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
+		unsigned int val = data[i];
+
 		/* Set the range selection */
 		writel(range, dev->mmio + 96);
 
 		/* Write the analog value to the selected channel */
-		writel((data[i] << 8) | chan, dev->mmio + 100);
+		writel((val << 8) | chan, dev->mmio + 100);
 
 		/* Wait the end of transfer */
 		ret = comedi_timeout(dev, s, insn, apci3xxx_ao_eoc, 0);
 		if (ret)
 			return ret;
+
+		s->readback[chan] = val;
 	}
 
 	return insn->n;
@@ -850,6 +854,11 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 		s->maxdata	= 0x0fff;
 		s->range_table	= &apci3xxx_ao_range;
 		s->insn_write	= apci3xxx_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 
 		subdev++;
 	}
@@ -901,17 +910,9 @@ static int apci3xxx_auto_attach(struct comedi_device *dev,
 
 static void apci3xxx_detach(struct comedi_device *dev)
 {
-	struct apci3xxx_private *devpriv = dev->private;
-
-	if (devpriv) {
-		if (dev->iobase)
-			apci3xxx_reset(dev);
-		if (dev->irq)
-			free_irq(dev->irq, dev);
-		if (dev->mmio)
-			iounmap(dev->mmio);
-	}
-	comedi_pci_disable(dev);
+	if (dev->iobase)
+		apci3xxx_reset(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver apci3xxx_driver = {
diff --git a/drivers/staging/comedi/drivers/adl_pci6208.c b/drivers/staging/comedi/drivers/adl_pci6208.c
index 921f6942dfce..0ad46fe492c9 100644
--- a/drivers/staging/comedi/drivers/adl_pci6208.c
+++ b/drivers/staging/comedi/drivers/adl_pci6208.c
@@ -46,8 +46,6 @@
 #define PCI6208_DIO_DI_MASK		(0xf0)
 #define PCI6208_DIO_DI_SHIFT		(4)
 
-#define PCI6208_MAX_AO_CHANNELS		16
-
 enum pci6208_boardid {
 	BOARD_PCI6208,
 	BOARD_PCI6216,
@@ -69,10 +67,6 @@ static const struct pci6208_board pci6208_boards[] = {
 	},
 };
 
-struct pci6208_private {
-	unsigned int ao_readback[PCI6208_MAX_AO_CHANNELS];
-};
-
 static int pci6208_ao_eoc(struct comedi_device *dev,
 			  struct comedi_subdevice *s,
 			  struct comedi_insn *insn,
@@ -91,9 +85,8 @@ static int pci6208_ao_insn_write(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	struct pci6208_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val = devpriv->ao_readback[chan];
+	unsigned int val = s->readback[chan];
 	int ret;
 	int i;
 
@@ -108,23 +101,9 @@ static int pci6208_ao_insn_write(struct comedi_device *dev,
 		/* the hardware expects two's complement values */
 		outw(comedi_offset_munge(s, val),
 		     dev->iobase + PCI6208_AO_CONTROL(chan));
-	}
-	devpriv->ao_readback[chan] = val;
-
-	return insn->n;
-}
 
-static int pci6208_ao_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	struct pci6208_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+		s->readback[chan] = val;
+	}
 
 	return insn->n;
 }
@@ -162,7 +141,6 @@ static int pci6208_auto_attach(struct comedi_device *dev,
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	const struct pci6208_board *boardinfo = NULL;
-	struct pci6208_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int val;
 	int ret;
@@ -174,10 +152,6 @@ static int pci6208_auto_attach(struct comedi_device *dev,
 	dev->board_ptr = boardinfo;
 	dev->board_name = boardinfo->name;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
@@ -195,7 +169,11 @@ static int pci6208_auto_attach(struct comedi_device *dev,
 	s->maxdata	= 0xffff;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= pci6208_ao_insn_write;
-	s->insn_read	= pci6208_ao_insn_read;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[1];
 	/* digital input subdevice */
@@ -230,7 +208,7 @@ static struct comedi_driver adl_pci6208_driver = {
 	.driver_name	= "adl_pci6208",
 	.module		= THIS_MODULE,
 	.auto_attach	= pci6208_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int adl_pci6208_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/adl_pci7x3x.c b/drivers/staging/comedi/drivers/adl_pci7x3x.c
index 5e3cc77a8a0c..fb8e5f582496 100644
--- a/drivers/staging/comedi/drivers/adl_pci7x3x.c
+++ b/drivers/staging/comedi/drivers/adl_pci7x3x.c
@@ -246,7 +246,7 @@ static struct comedi_driver adl_pci7x3x_driver = {
 	.driver_name	= "adl_pci7x3x",
 	.module		= THIS_MODULE,
 	.auto_attach	= adl_pci7x3x_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int adl_pci7x3x_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/adl_pci8164.c b/drivers/staging/comedi/drivers/adl_pci8164.c
index 300df55a2802..72bccb447a74 100644
--- a/drivers/staging/comedi/drivers/adl_pci8164.c
+++ b/drivers/staging/comedi/drivers/adl_pci8164.c
@@ -135,7 +135,7 @@ static struct comedi_driver adl_pci8164_driver = {
 	.driver_name	= "adl_pci8164",
 	.module		= THIS_MODULE,
 	.auto_attach	= adl_pci8164_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int adl_pci8164_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/adl_pci9111.c b/drivers/staging/comedi/drivers/adl_pci9111.c
index 51edfebb952a..d18d8f21af23 100644
--- a/drivers/staging/comedi/drivers/adl_pci9111.c
+++ b/drivers/staging/comedi/drivers/adl_pci9111.c
@@ -139,8 +139,6 @@ struct pci9111_private_data {
 	unsigned int chunk_counter;
 	unsigned int chunk_num_samples;
 
-	int ao_readback;
-
 	unsigned int div1;
 	unsigned int div2;
 
@@ -643,29 +641,15 @@ static int pci9111_ao_insn_write(struct comedi_device *dev,
 				 struct comedi_insn *insn,
 				 unsigned int *data)
 {
-	struct pci9111_private_data *dev_private = dev->private;
-	unsigned int val = 0;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
 		val = data[i];
 		outw(val, dev->iobase + PCI9111_AO_REG);
 	}
-	dev_private->ao_readback = val;
-
-	return insn->n;
-}
-
-static int pci9111_ao_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	struct pci9111_private_data *dev_private = dev->private;
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = dev_private->ao_readback;
+	s->readback[chan] = val;
 
 	return insn->n;
 }
@@ -768,7 +752,11 @@ static int pci9111_auto_attach(struct comedi_device *dev,
 	s->len_chanlist	= 1;
 	s->range_table	= &range_bipolar10;
 	s->insn_write	= pci9111_ao_insn_write;
-	s->insn_read	= pci9111_ao_insn_read;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DI;
@@ -793,9 +781,7 @@ static void pci9111_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		pci9111_reset(dev);
-	if (dev->irq != 0)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver adl_pci9111_driver = {
diff --git a/drivers/staging/comedi/drivers/adl_pci9118.c b/drivers/staging/comedi/drivers/adl_pci9118.c
index f30b84e1987b..e18fd9569a2b 100644
--- a/drivers/staging/comedi/drivers/adl_pci9118.c
+++ b/drivers/staging/comedi/drivers/adl_pci9118.c
@@ -36,8 +36,8 @@
  * b) DMA transfers must have the length aligned to two samples (32 bit),
  *  so there is some problems if cmd->chanlist_len is odd. This driver tries
  *  bypass this with adding one sample to the end of the every scan and discard
- *  it on output but this cann't be used if cmd->scan_begin_src=TRIG_FOLLOW
- *  and is used flag TRIG_WAKE_EOS, then driver switch to interrupt driven mode
+ *  it on output but this can't be used if cmd->scan_begin_src=TRIG_FOLLOW
+ *  and is used flag CMDF_WAKE_EOS, then driver switch to interrupt driven mode
  *  with interrupt after every sample.
  * c) If isn't used DMA then you can use only mode where
  *  cmd->scan_begin_src=TRIG_FOLLOW.
@@ -49,19 +49,13 @@
  *	 card will be used.
  * [2] - 0= standard 8 DIFF/16 SE channels configuration
  *	 n = external multiplexer connected, 1 <= n <= 256
- * [3] - 0=autoselect DMA or EOC interrupts operation
- *	 1 = disable DMA mode
- *	 3 = disable DMA and INT, only insn interface will work
+ * [3] - ignored
  * [4] - sample&hold signal - card can generate signal for external S&H board
  *	 0 = use SSHO(pin 45) signal is generated in onboard hardware S&H logic
  *	 0 != use ADCHN7(pin 23) signal is generated from driver, number say how
  *		long delay is requested in ns and sign polarity of the hold
  *		(in this case external multiplexor can serve only 128 channels)
- * [5] - 0=stop measure on all hardware errors
- *	 2 | = ignore ADOR - A/D Overrun status
- *	 8|=ignore Bover - A/D Burst Mode Overrun status
- *	 256|=ignore nFull - A/D FIFO Full status
- *
+ * [5] - ignored
  */
 
 /*
@@ -91,111 +85,68 @@
 #include "8253.h"
 #include "comedi_fc.h"
 
-/* paranoid checks are broken */
-#undef PCI9118_PARANOIDCHECK	/*
-				 * if defined, then is used code which control
-				 * correct channel number on every 12 bit sample
-				 */
-
 #define IORANGE_9118	64	/* I hope */
 #define PCI9118_CHANLEN	255	/*
 				 * len of chanlist, some source say 256,
 				 * but reality looks like 255 :-(
 				 */
 
-#define PCI9118_CNT0	0x00	/* R/W: 8254 counter 0 */
-#define PCI9118_CNT1	0x04	/* R/W: 8254 counter 0 */
-#define PCI9118_CNT2	0x08	/* R/W: 8254 counter 0 */
-#define PCI9118_CNTCTRL	0x0c	/* W:   8254 counter control */
-#define PCI9118_AD_DATA	0x10	/* R:   A/D data */
-#define PCI9118_DA1	0x10	/* W:   D/A registers */
-#define PCI9118_DA2	0x14
-#define PCI9118_ADSTAT	0x18	/* R:   A/D status register */
-#define PCI9118_ADCNTRL	0x18	/* W:   A/D control register */
-#define PCI9118_DI	0x1c	/* R:   digi input register */
-#define PCI9118_DO	0x1c	/* W:   digi output register */
-#define PCI9118_SOFTTRG	0x20	/* W:   soft trigger for A/D */
-#define PCI9118_GAIN	0x24	/* W:   A/D gain/channel register */
-#define PCI9118_BURST	0x28	/* W:   A/D burst number register */
-#define PCI9118_SCANMOD	0x2c	/* W:   A/D auto scan mode */
-#define PCI9118_ADFUNC	0x30	/* W:   A/D function register */
-#define PCI9118_DELFIFO	0x34	/* W:   A/D data FIFO reset */
-#define PCI9118_INTSRC	0x38	/* R:   interrupt reason register */
-#define PCI9118_INTCTRL	0x38	/* W:   interrupt control register */
-
-/* bits from A/D control register (PCI9118_ADCNTRL) */
-#define AdControl_UniP	0x80	/* 1=bipolar, 0=unipolar */
-#define AdControl_Diff	0x40	/* 1=differential, 0= single end inputs */
-#define AdControl_SoftG	0x20	/* 1=8254 counter works, 0=counter stops */
-#define	AdControl_ExtG	0x10	/*
-				 * 1=8254 countrol controlled by TGIN(pin 46),
-				 * 0=controlled by SoftG
-				 */
-#define AdControl_ExtM	0x08	/*
-				 * 1=external hardware trigger (pin 44),
-				 * 0=internal trigger
-				 */
-#define AdControl_TmrTr	0x04	/*
-				 * 1=8254 is iternal trigger source,
-				 * 0=software trigger is source
-				 * (register PCI9118_SOFTTRG)
-				 */
-#define AdControl_Int	0x02	/* 1=enable INT, 0=disable */
-#define AdControl_Dma	0x01	/* 1=enable DMA, 0=disable */
-
-/* bits from A/D function register (PCI9118_ADFUNC) */
-#define AdFunction_PDTrg	0x80	/*
-					 * 1=positive,
-					 * 0=negative digital trigger
-					 * (only positive is correct)
-					 */
-#define AdFunction_PETrg	0x40	/*
-					 * 1=positive,
-					 * 0=negative external trigger
-					 * (only positive is correct)
-					 */
-#define AdFunction_BSSH		0x20	/* 1=with sample&hold, 0=without */
-#define AdFunction_BM		0x10	/* 1=burst mode, 0=normal mode */
-#define AdFunction_BS		0x08	/*
-					 * 1=burst mode start,
-					 * 0=burst mode stop
-					 */
-#define AdFunction_PM		0x04	/*
-					 * 1=post trigger mode,
-					 * 0=not post trigger
-					 */
-#define AdFunction_AM		0x02	/*
-					 * 1=about trigger mode,
-					 * 0=not about trigger
-					 */
-#define AdFunction_Start	0x01	/* 1=trigger start, 0=trigger stop */
-
-/* bits from A/D status register (PCI9118_ADSTAT) */
-#define AdStatus_nFull	0x100	/* 0=FIFO full (fatal), 1=not full */
-#define AdStatus_nHfull	0x080	/* 0=FIFO half full, 1=FIFO not half full */
-#define AdStatus_nEpty	0x040	/* 0=FIFO empty, 1=FIFO not empty */
-#define AdStatus_Acmp	0x020	/*  */
-#define AdStatus_DTH	0x010	/* 1=external digital trigger */
-#define AdStatus_Bover	0x008	/* 1=burst mode overrun (fatal) */
-#define AdStatus_ADOS	0x004	/* 1=A/D over speed (warning) */
-#define AdStatus_ADOR	0x002	/* 1=A/D overrun (fatal) */
-#define AdStatus_ADrdy	0x001	/* 1=A/D already ready, 0=not ready */
-
-/* bits for interrupt reason and control (PCI9118_INTSRC, PCI9118_INTCTRL) */
-/* 1=interrupt occur, enable source,  0=interrupt not occur, disable source */
-#define Int_Timer	0x08	/* timer interrupt */
-#define Int_About	0x04	/* about trigger complete */
-#define Int_Hfull	0x02	/* A/D FIFO hlaf full */
-#define Int_DTrg	0x01	/* external digital trigger */
+/*
+ * PCI BAR2 Register map (dev->iobase)
+ */
+#define PCI9118_TIMER_REG(x)		(0x00 + ((x) * 4))
+#define PCI9118_TIMER_CTRL_REG		0x0c
+#define PCI9118_AI_FIFO_REG		0x10
+#define PCI9118_AO_REG(x)		(0x10 + ((x) * 4))
+#define PCI9118_AI_STATUS_REG		0x18
+#define PCI9118_AI_STATUS_NFULL		(1 << 8)  /* 0=FIFO full (fatal) */
+#define PCI9118_AI_STATUS_NHFULL	(1 << 7)  /* 0=FIFO half full */
+#define PCI9118_AI_STATUS_NEPTY		(1 << 6)  /* 0=FIFO empty */
+#define PCI9118_AI_STATUS_ACMP		(1 << 5)  /* 1=about trigger complete */
+#define PCI9118_AI_STATUS_DTH		(1 << 4)  /* 1=ext. digital trigger */
+#define PCI9118_AI_STATUS_BOVER		(1 << 3)  /* 1=burst overrun (fatal) */
+#define PCI9118_AI_STATUS_ADOS		(1 << 2)  /* 1=A/D over speed (warn) */
+#define PCI9118_AI_STATUS_ADOR		(1 << 1)  /* 1=A/D overrun (fatal) */
+#define PCI9118_AI_STATUS_ADRDY		(1 << 0)  /* 1=A/D ready */
+#define PCI9118_AI_CTRL_REG		0x18
+#define PCI9118_AI_CTRL_UNIP		(1 << 7)  /* 1=unipolar */
+#define PCI9118_AI_CTRL_DIFF		(1 << 6)  /* 1=differential inputs */
+#define PCI9118_AI_CTRL_SOFTG		(1 << 5)  /* 1=8254 software gate */
+#define PCI9118_AI_CTRL_EXTG		(1 << 4)  /* 1=8254 TGIN(pin 46) gate */
+#define PCI9118_AI_CTRL_EXTM		(1 << 3)  /* 1=ext. trigger (pin 44) */
+#define PCI9118_AI_CTRL_TMRTR		(1 << 2)  /* 1=8254 is trigger source */
+#define PCI9118_AI_CTRL_INT		(1 << 1)  /* 1=enable interrupt */
+#define PCI9118_AI_CTRL_DMA		(1 << 0)  /* 1=enable DMA */
+#define PCI9118_DIO_REG			0x1c
+#define PCI9118_SOFTTRG_REG		0x20
+#define PCI9118_AI_CHANLIST_REG		0x24
+#define PCI9118_AI_CHANLIST_RANGE(x)	(((x) & 0x3) << 8)
+#define PCI9118_AI_CHANLIST_CHAN(x)	((x) << 0)
+#define PCI9118_AI_BURST_NUM_REG	0x28
+#define PCI9118_AI_AUTOSCAN_MODE_REG	0x2c
+#define PCI9118_AI_CFG_REG		0x30
+#define PCI9118_AI_CFG_PDTRG		(1 << 7)  /* 1=positive trigger */
+#define PCI9118_AI_CFG_PETRG		(1 << 6)  /* 1=positive ext. trigger */
+#define PCI9118_AI_CFG_BSSH		(1 << 5)  /* 1=with sample & hold */
+#define PCI9118_AI_CFG_BM		(1 << 4)  /* 1=burst mode */
+#define PCI9118_AI_CFG_BS		(1 << 3)  /* 1=burst mode start */
+#define PCI9118_AI_CFG_PM		(1 << 2)  /* 1=post trigger */
+#define PCI9118_AI_CFG_AM		(1 << 1)  /* 1=about trigger */
+#define PCI9118_AI_CFG_START		(1 << 0)  /* 1=trigger start */
+#define PCI9118_FIFO_RESET_REG		0x34
+#define PCI9118_INT_CTRL_REG		0x38
+#define PCI9118_INT_CTRL_TIMER		(1 << 3)  /* timer interrupt */
+#define PCI9118_INT_CTRL_ABOUT		(1 << 2)  /* about trigger complete */
+#define PCI9118_INT_CTRL_HFULL		(1 << 1)  /* A/D FIFO half full */
+#define PCI9118_INT_CTRL_DTRG		(1 << 0)  /* ext. digital trigger */
 
 #define START_AI_EXT	0x01	/* start measure on external trigger */
 #define STOP_AI_EXT	0x02	/* stop measure on external trigger */
-#define START_AI_INT	0x04	/* start measure on internal trigger */
 #define STOP_AI_INT	0x08	/* stop measure on internal trigger */
 
-#define EXTTRG_AI	0	/* ext trg is used by AI */
+#define PCI9118_HALF_FIFO_SZ	(1024 / 2)
 
-static const struct comedi_lrange range_pci9118dg_hr = {
+static const struct comedi_lrange pci9118_ai_range = {
 	8, {
 		BIP_RANGE(5),
 		BIP_RANGE(2.5),
@@ -208,7 +159,7 @@ static const struct comedi_lrange range_pci9118dg_hr = {
 	}
 };
 
-static const struct comedi_lrange range_pci9118hg = {
+static const struct comedi_lrange pci9118hg_ai_range = {
 	8, {
 		BIP_RANGE(5),
 		BIP_RANGE(0.5),
@@ -226,102 +177,49 @@ static const struct comedi_lrange range_pci9118hg = {
 					 * of BIP/UNI ranges
 					 */
 
-struct boardtype {
-	const char *name;		/* board name */
-	int device_id;			/* PCI device ID of card */
-	int iorange_amcc;		/* iorange for own S5933 region */
-	int iorange_9118;		/* pass thru card region size */
-	int n_aichan;			/* num of A/D chans */
-	int n_aichand;			/* num of A/D chans in diff mode */
-	int mux_aichan;			/*
-					 * num of A/D chans with
-					 * external multiplexor
-					 */
-	int n_aichanlist;		/* len of chanlist */
-	int n_aochan;			/* num of D/A chans */
-	int ai_maxdata;			/* resolution of A/D */
-	int ao_maxdata;			/* resolution of D/A */
-	const struct comedi_lrange *rangelist_ai;	/* rangelist for A/D */
-	const struct comedi_lrange *rangelist_ao;	/* rangelist for D/A */
-	unsigned int ai_ns_min;		/* max sample speed of card v ns */
-	unsigned int ai_pacer_min;	/*
-					 * minimal pacer value
-					 * (c1*c2 or c1 in burst)
-					 */
-	int half_fifo_size;		/* size of FIFO/2 */
+enum pci9118_boardid {
+	BOARD_PCI9118DG,
+	BOARD_PCI9118HG,
+	BOARD_PCI9118HR,
+};
 
+struct pci9118_boardinfo {
+	const char *name;
+	unsigned int ai_is_16bit:1;
+	unsigned int is_hg:1;
 };
 
-static const struct boardtype boardtypes[] = {
-	{
+static const struct pci9118_boardinfo pci9118_boards[] = {
+	[BOARD_PCI9118DG] = {
 		.name		= "pci9118dg",
-		.device_id	= 0x80d9,
-		.iorange_amcc	= AMCC_OP_REG_SIZE,
-		.iorange_9118	= IORANGE_9118,
-		.n_aichan	= 16,
-		.n_aichand	= 8,
-		.mux_aichan	= 256,
-		.n_aichanlist	= PCI9118_CHANLEN,
-		.n_aochan	= 2,
-		.ai_maxdata	= 0x0fff,
-		.ao_maxdata	= 0x0fff,
-		.rangelist_ai	= &range_pci9118dg_hr,
-		.rangelist_ao	= &range_bipolar10,
-		.ai_ns_min	= 3000,
-		.ai_pacer_min	= 12,
-		.half_fifo_size	= 512,
-	}, {
+	},
+	[BOARD_PCI9118HG] = {
 		.name		= "pci9118hg",
-		.device_id	= 0x80d9,
-		.iorange_amcc	= AMCC_OP_REG_SIZE,
-		.iorange_9118	= IORANGE_9118,
-		.n_aichan	= 16,
-		.n_aichand	= 8,
-		.mux_aichan	= 256,
-		.n_aichanlist	= PCI9118_CHANLEN,
-		.n_aochan	= 2,
-		.ai_maxdata	= 0x0fff,
-		.ao_maxdata	= 0x0fff,
-		.rangelist_ai	= &range_pci9118hg,
-		.rangelist_ao	= &range_bipolar10,
-		.ai_ns_min	= 3000,
-		.ai_pacer_min	= 12,
-		.half_fifo_size	= 512,
-	}, {
+		.is_hg		= 1,
+	},
+	[BOARD_PCI9118HR] = {
 		.name		= "pci9118hr",
-		.device_id	= 0x80d9,
-		.iorange_amcc	= AMCC_OP_REG_SIZE,
-		.iorange_9118	= IORANGE_9118,
-		.n_aichan	= 16,
-		.n_aichand	= 8,
-		.mux_aichan	= 256,
-		.n_aichanlist	= PCI9118_CHANLEN,
-		.n_aochan	= 2,
-		.ai_maxdata	= 0xffff,
-		.ao_maxdata	= 0x0fff,
-		.rangelist_ai	= &range_pci9118dg_hr,
-		.rangelist_ao	= &range_bipolar10,
-		.ai_ns_min	= 10000,
-		.ai_pacer_min	= 40,
-		.half_fifo_size	= 512,
+		.ai_is_16bit	= 1,
 	},
 };
 
+struct pci9118_dmabuf {
+	unsigned short *virt;	/* virtual address of buffer */
+	dma_addr_t hw;		/* hardware (bus) address of buffer */
+	unsigned int size;	/* size of dma buffer in bytes */
+	unsigned int use_size;	/* which size we may now use for transfer */
+};
+
 struct pci9118_private {
 	unsigned long iobase_a;	/* base+size for AMCC chip */
-	unsigned int master;	/* master capable */
-	unsigned int usemux;	/* we want to use external multiplexor! */
-#ifdef PCI9118_PARANOIDCHECK
-	unsigned short chanlist[PCI9118_CHANLEN + 1];	/*
-							 * list of
-							 * scanned channel
-							 */
-	unsigned char chanlistlen;	/* number of scanlist */
-#endif
-	unsigned char AdControlReg;	/* A/D control register */
-	unsigned char IntControlReg;	/* Interrupt control register */
-	unsigned char AdFunctionReg;	/* A/D function register */
-	char ai_neverending;		/* we do unlimited AI */
+	unsigned int master:1;
+	unsigned int dma_doublebuf:1;
+	unsigned int ai_neverending:1;
+	unsigned int usedma:1;
+	unsigned int usemux:1;
+	unsigned char ai_ctrl;
+	unsigned char int_ctrl;
+	unsigned char ai_cfg;
 	unsigned int ai_do;		/* what do AI? 0=nothing, 1 to 4 mode */
 	unsigned int ai_act_scan;	/* how many scans we finished */
 	unsigned int ai_n_realscanlen;	/*
@@ -346,29 +244,8 @@ struct pci9118_private {
 						 * divisors for start of measure
 						 * on external start
 						 */
-	unsigned short ao_data[2];		/* data output buffer */
-	char dma_doublebuf;			/* use double buffering */
 	unsigned int dma_actbuf;		/* which buffer is used now */
-	unsigned short *dmabuf_virt[2];		/*
-						 * pointers to begin of
-						 * DMA buffer
-						 */
-	unsigned long dmabuf_hw[2];		/* hw address of DMA buff */
-	unsigned int dmabuf_size[2];		/*
-						 * size of dma buffer in bytes
-						 */
-	unsigned int dmabuf_use_size[2];	/*
-						 * which size we may now use
-						 * for transfer
-						 */
-	unsigned int dmabuf_used_size[2];	/* which size was truly used */
-	unsigned int dmabuf_panic_size[2];
-	int dmabuf_pages[2];			/* number of pages in buffer */
-	unsigned char exttrg_users;		/*
-						 * bit field of external trigger
-						 * users(0-AI, 1-AO, 2-DI, 3-DO)
-						 */
-	unsigned char usedma;		/* =1 use DMA transfer and not INT */
+	struct pci9118_dmabuf dmabuf[2];
 	int softsshdelay;		/*
 					 * >0 use software S&H,
 					 * numer is requested delay in ns
@@ -381,15 +258,74 @@ struct pci9118_private {
 					 * polarity of S&H signal
 					 * in hold state
 					 */
-	unsigned int ai_maskerr;	/* which warning was printed */
-	unsigned int ai_maskharderr;	/* on which error bits stops */
+	unsigned int ai_ns_min;
 };
 
+static void pci9118_amcc_setup_dma(struct comedi_device *dev, unsigned int buf)
+{
+	struct pci9118_private *devpriv = dev->private;
+	struct pci9118_dmabuf *dmabuf = &devpriv->dmabuf[buf];
+
+	/* set the master write address and transfer count */
+	outl(dmabuf->hw, devpriv->iobase_a + AMCC_OP_REG_MWAR);
+	outl(dmabuf->use_size, devpriv->iobase_a + AMCC_OP_REG_MWTC);
+}
+
+static void pci9118_amcc_dma_ena(struct comedi_device *dev, bool enable)
+{
+	struct pci9118_private *devpriv = dev->private;
+	unsigned int mcsr;
+
+	mcsr = inl(devpriv->iobase_a + AMCC_OP_REG_MCSR);
+	if (enable)
+		mcsr |= RESET_A2P_FLAGS | A2P_HI_PRIORITY | EN_A2P_TRANSFERS;
+	else
+		mcsr &= ~EN_A2P_TRANSFERS;
+	outl(mcsr, devpriv->iobase_a + AMCC_OP_REG_MCSR);
+}
+
+static void pci9118_amcc_int_ena(struct comedi_device *dev, bool enable)
+{
+	struct pci9118_private *devpriv = dev->private;
+	unsigned int intcsr;
+
+	/* enable/disable interrupt for AMCC Incoming Mailbox 4 (32-bit) */
+	intcsr = inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+	if (enable)
+		intcsr |= 0x1f00;
+	else
+		intcsr &= ~0x1f00;
+	outl(intcsr, devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+}
+
+static void pci9118_timer_write(struct comedi_device *dev,
+				unsigned int timer, unsigned int val)
+{
+	outl(val & 0xff, dev->iobase + PCI9118_TIMER_REG(timer));
+	outl((val >> 8) & 0xff, dev->iobase + PCI9118_TIMER_REG(timer));
+}
+
+static void pci9118_timer_set_mode(struct comedi_device *dev,
+				   unsigned int timer, unsigned int mode)
+{
+	unsigned int val;
+
+	val = timer << 6;	/* select timer */
+	val |= 0x30;		/* load low then high byte */
+	val |= mode;		/* set timer mode and BCD|binary */
+	outl(val, dev->iobase + PCI9118_TIMER_CTRL_REG);
+}
+
+static void pci9118_ai_reset_fifo(struct comedi_device *dev)
+{
+	/* writing any value resets the A/D FIFO */
+	outl(0, dev->iobase + PCI9118_FIFO_RESET_REG);
+}
+
 static int check_channel_list(struct comedi_device *dev,
 			      struct comedi_subdevice *s, int n_chan,
 			      unsigned int *chanlist, int frontadd, int backadd)
 {
-	const struct boardtype *this_board = comedi_board(dev);
 	struct pci9118_private *devpriv = dev->private;
 	unsigned int i, differencial = 0, bipolar = 0;
 
@@ -423,7 +359,7 @@ static int check_channel_list(struct comedi_device *dev,
 				return 0;
 			}
 			if (!devpriv->usemux && differencial &&
-			    (CR_CHAN(chanlist[i]) >= this_board->n_aichand)) {
+			    (CR_CHAN(chanlist[i]) >= (s->n_chan / 2))) {
 				dev_err(dev->class_dev,
 					"AREF_DIFF is only available for the first 8 channels!\n");
 				return 0;
@@ -433,234 +369,82 @@ static int check_channel_list(struct comedi_device *dev,
 	return 1;
 }
 
-static int setup_channel_list(struct comedi_device *dev,
-			      struct comedi_subdevice *s, int n_chan,
-			      unsigned int *chanlist, int rot, int frontadd,
-			      int backadd, int usedma)
+static void pci9118_set_chanlist(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 int n_chan, unsigned int *chanlist,
+				 int frontadd, int backadd)
 {
 	struct pci9118_private *devpriv = dev->private;
-	unsigned int i, differencial = 0, bipolar = 0;
-	unsigned int scanquad, gain, ssh = 0x00;
-
-	if (usedma == 1) {
-		rot = 8;
-		usedma = 0;
-	}
-
-	if (CR_AREF(chanlist[0]) == AREF_DIFF)
-		differencial = 1;	/* all input must be diff */
-	if (CR_RANGE(chanlist[0]) < PCI9118_BIPOLAR_RANGES)
-		bipolar = 1;	/* all input must be bipolar */
-
-	/* All is ok, so we can setup channel/range list */
-
-	if (!bipolar) {
-		devpriv->AdControlReg |= AdControl_UniP;
-							/* set unibipolar */
-	} else {
-		devpriv->AdControlReg &= ((~AdControl_UniP) & 0xff);
-							/* enable bipolar */
-	}
-
-	if (differencial) {
-		devpriv->AdControlReg |= AdControl_Diff;
-							/* enable diff inputs */
-	} else {
-		devpriv->AdControlReg &= ((~AdControl_Diff) & 0xff);
-						/* set single ended inputs */
-	}
-
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-								/* setup mode */
+	unsigned int chan0 = CR_CHAN(chanlist[0]);
+	unsigned int range0 = CR_RANGE(chanlist[0]);
+	unsigned int aref0 = CR_AREF(chanlist[0]);
+	unsigned int ssh = 0x00;
+	unsigned int val;
+	int i;
 
-	outl(2, dev->iobase + PCI9118_SCANMOD);
-					/* gods know why this sequence! */
-	outl(0, dev->iobase + PCI9118_SCANMOD);
-	outl(1, dev->iobase + PCI9118_SCANMOD);
-
-#ifdef PCI9118_PARANOIDCHECK
-	devpriv->chanlistlen = n_chan;
-	for (i = 0; i < (PCI9118_CHANLEN + 1); i++)
-		devpriv->chanlist[i] = 0x55aa;
-#endif
-
-	if (frontadd) {		/* insert channels for S&H */
+	/*
+	 * Configure analog input based on the first chanlist entry.
+	 * All entries are either unipolar or bipolar and single-ended
+	 * or differential.
+	 */
+	devpriv->ai_ctrl = 0;
+	if (comedi_range_is_unipolar(s, range0))
+		devpriv->ai_ctrl |= PCI9118_AI_CTRL_UNIP;
+	if (aref0 == AREF_DIFF)
+		devpriv->ai_ctrl |= PCI9118_AI_CTRL_DIFF;
+	outl(devpriv->ai_ctrl, dev->iobase + PCI9118_AI_CTRL_REG);
+
+	/* gods know why this sequence! */
+	outl(2, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+	outl(0, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+	outl(1, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+
+	/* insert channels for S&H */
+	if (frontadd) {
+		val = PCI9118_AI_CHANLIST_CHAN(chan0) |
+		      PCI9118_AI_CHANLIST_RANGE(range0);
 		ssh = devpriv->softsshsample;
 		for (i = 0; i < frontadd; i++) {
-						/* store range list to card */
-			scanquad = CR_CHAN(chanlist[0]);
-						/* get channel number; */
-			gain = CR_RANGE(chanlist[0]);
-						/* get gain number */
-			scanquad |= ((gain & 0x03) << 8);
-			outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
+			outl(val | ssh, dev->iobase + PCI9118_AI_CHANLIST_REG);
 			ssh = devpriv->softsshhold;
 		}
 	}
 
-	for (i = 0; i < n_chan; i++) {	/* store range list to card */
-		scanquad = CR_CHAN(chanlist[i]);	/* get channel number */
-#ifdef PCI9118_PARANOIDCHECK
-		devpriv->chanlist[i ^ usedma] = (scanquad & 0xf) << rot;
-#endif
-		gain = CR_RANGE(chanlist[i]);		/* get gain number */
-		scanquad |= ((gain & 0x03) << 8);
-		outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
-	}
+	/* store chanlist */
+	for (i = 0; i < n_chan; i++) {
+		unsigned int chan = CR_CHAN(chanlist[i]);
+		unsigned int range = CR_RANGE(chanlist[i]);
 
-	if (backadd) {		/* insert channels for fit onto 32bit DMA */
-		for (i = 0; i < backadd; i++) {	/* store range list to card */
-			scanquad = CR_CHAN(chanlist[0]);
-							/* get channel number */
-			gain = CR_RANGE(chanlist[0]);	/* get gain number */
-			scanquad |= ((gain & 0x03) << 8);
-			outl(scanquad | ssh, dev->iobase + PCI9118_GAIN);
-		}
+		val = PCI9118_AI_CHANLIST_CHAN(chan) |
+		      PCI9118_AI_CHANLIST_RANGE(range);
+		outl(val | ssh, dev->iobase + PCI9118_AI_CHANLIST_REG);
 	}
-#ifdef PCI9118_PARANOIDCHECK
-	devpriv->chanlist[n_chan ^ usedma] = devpriv->chanlist[0 ^ usedma];
-						/* for 32bit operations */
-#endif
-	outl(0, dev->iobase + PCI9118_SCANMOD);	/* close scan queue */
-	/* udelay(100); important delay, or first sample will be crippled */
-
-	return 1;		/* we can serve this with scan logic */
-}
-
-static int pci9118_ai_eoc(struct comedi_device *dev,
-			  struct comedi_subdevice *s,
-			  struct comedi_insn *insn,
-			  unsigned long context)
-{
-	unsigned int status;
-
-	status = inl(dev->iobase + PCI9118_ADSTAT);
-	if (status & AdStatus_ADrdy)
-		return 0;
-	return -EBUSY;
-}
-
-static int pci9118_insn_read_ai(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	struct pci9118_private *devpriv = dev->private;
-	int ret;
-	int n;
-
-	devpriv->AdControlReg = AdControl_Int & 0xff;
-	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-						/*
-						 * positive triggers, no S&H,
-						 * no burst, burst stop,
-						 * no post trigger,
-						 * no about trigger,
-						 * trigger stop
-						 */
 
-	if (!setup_channel_list(dev, s, 1, &insn->chanspec, 0, 0, 0, 0))
-		return -EINVAL;
-
-	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
-
-	for (n = 0; n < insn->n; n++) {
-		outw(0, dev->iobase + PCI9118_SOFTTRG);	/* start conversion */
-		udelay(2);
-
-		ret = comedi_timeout(dev, s, insn, pci9118_ai_eoc, 0);
-		if (ret) {
-			outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
-			return ret;
-		}
-
-		if (s->maxdata == 0xffff) {
-			data[n] =
-			    (inl(dev->iobase +
-				 PCI9118_AD_DATA) & 0xffff) ^ 0x8000;
-		} else {
-			data[n] =
-			    (inw(dev->iobase + PCI9118_AD_DATA) >> 4) & 0xfff;
-		}
-	}
-
-	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
-	return n;
-
-}
-
-static int pci9118_insn_write_ao(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
-{
-	struct pci9118_private *devpriv = dev->private;
-	int n, chanreg, ch;
-
-	ch = CR_CHAN(insn->chanspec);
-	if (ch)
-		chanreg = PCI9118_DA2;
-	else
-		chanreg = PCI9118_DA1;
-
-
-	for (n = 0; n < insn->n; n++) {
-		outl(data[n], dev->iobase + chanreg);
-		devpriv->ao_data[ch] = data[n];
+	/* insert channels to fit onto 32bit DMA */
+	if (backadd) {
+		val = PCI9118_AI_CHANLIST_CHAN(chan0) |
+		      PCI9118_AI_CHANLIST_RANGE(range0);
+		for (i = 0; i < backadd; i++)
+			outl(val | ssh, dev->iobase + PCI9118_AI_CHANLIST_REG);
 	}
-
-	return n;
-}
-
-static int pci9118_insn_read_ao(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	struct pci9118_private *devpriv = dev->private;
-	int n, chan;
-
-	chan = CR_CHAN(insn->chanspec);
-	for (n = 0; n < insn->n; n++)
-		data[n] = devpriv->ao_data[chan];
-
-	return n;
-}
-
-static int pci9118_insn_bits_di(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	data[1] = inl(dev->iobase + PCI9118_DI) & 0xf;
-
-	return insn->n;
-}
-
-static int pci9118_insn_bits_do(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	if (comedi_dio_update_state(s, data))
-		outl(s->state & 0x0f, dev->iobase + PCI9118_DO);
-
-	data[1] = s->state;
-
-	return insn->n;
+	/* close scan queue */
+	outl(0, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+	/* udelay(100); important delay, or first sample will be crippled */
 }
 
-static void interrupt_pci9118_ai_mode4_switch(struct comedi_device *dev)
+static void interrupt_pci9118_ai_mode4_switch(struct comedi_device *dev,
+					      unsigned int next_buf)
 {
 	struct pci9118_private *devpriv = dev->private;
-
-	devpriv->AdFunctionReg =
-	    AdFunction_PDTrg | AdFunction_PETrg | AdFunction_AM;
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-	outl(0x30, dev->iobase + PCI9118_CNTCTRL);
-	outl((devpriv->dmabuf_hw[1 - devpriv->dma_actbuf] >> 1) & 0xff,
-	     dev->iobase + PCI9118_CNT0);
-	outl((devpriv->dmabuf_hw[1 - devpriv->dma_actbuf] >> 9) & 0xff,
-	     dev->iobase + PCI9118_CNT0);
-	devpriv->AdFunctionReg |= AdFunction_Start;
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
+	struct pci9118_dmabuf *dmabuf = &devpriv->dmabuf[next_buf];
+
+	devpriv->ai_cfg = PCI9118_AI_CFG_PDTRG | PCI9118_AI_CFG_PETRG |
+			  PCI9118_AI_CFG_AM;
+	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
+	pci9118_timer_set_mode(dev, 0, I8254_MODE0);
+	pci9118_timer_write(dev, 0, dmabuf->hw >> 1);
+	devpriv->ai_cfg |= PCI9118_AI_CFG_START;
+	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
 }
 
 static unsigned int defragment_dma_buffer(struct comedi_device *dev,
@@ -689,9 +473,9 @@ static unsigned int defragment_dma_buffer(struct comedi_device *dev,
 }
 
 static int move_block_from_dma(struct comedi_device *dev,
-					struct comedi_subdevice *s,
-					unsigned short *dma_buffer,
-					unsigned int num_samples)
+			       struct comedi_subdevice *s,
+			       unsigned short *dma_buffer,
+			       unsigned int num_samples)
 {
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
@@ -710,100 +494,59 @@ static int move_block_from_dma(struct comedi_device *dev,
 	return 0;
 }
 
-static int pci9118_exttrg_add(struct comedi_device *dev, unsigned char source)
+static void pci9118_exttrg_enable(struct comedi_device *dev, bool enable)
 {
 	struct pci9118_private *devpriv = dev->private;
 
-	if (source > 3)
-		return -1;				/* incorrect source */
-	devpriv->exttrg_users |= (1 << source);
-	devpriv->IntControlReg |= Int_DTrg;
-	outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-	outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | 0x1f00,
-					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-							/* allow INT in AMCC */
-	return 0;
-}
-
-static int pci9118_exttrg_del(struct comedi_device *dev, unsigned char source)
-{
-	struct pci9118_private *devpriv = dev->private;
+	if (enable)
+		devpriv->int_ctrl |= PCI9118_INT_CTRL_DTRG;
+	else
+		devpriv->int_ctrl &= ~PCI9118_INT_CTRL_DTRG;
+	outl(devpriv->int_ctrl, dev->iobase + PCI9118_INT_CTRL_REG);
 
-	if (source > 3)
-		return -1;			/* incorrect source */
-	devpriv->exttrg_users &= ~(1 << source);
-	if (!devpriv->exttrg_users) {	/* shutdown ext trg intterrupts */
-		devpriv->IntControlReg &= ~Int_DTrg;
-		if (!devpriv->IntControlReg)	/* all IRQ disabled */
-			outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) &
-					(~0x00001f00),
-					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-						/* disable int in AMCC */
-		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-	}
-	return 0;
+	if (devpriv->int_ctrl)
+		pci9118_amcc_int_ena(dev, true);
+	else
+		pci9118_amcc_int_ena(dev, false);
 }
 
-static void pci9118_calc_divisors(char mode, struct comedi_device *dev,
+static void pci9118_calc_divisors(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
 				  unsigned int *tim1, unsigned int *tim2,
 				  unsigned int flags, int chans,
 				  unsigned int *div1, unsigned int *div2,
 				  unsigned int chnsshfront)
 {
-	const struct boardtype *this_board = comedi_board(dev);
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	switch (mode) {
-	case 1:
-	case 4:
-		if (*tim2 < this_board->ai_ns_min)
-			*tim2 = this_board->ai_ns_min;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
-					  div1, div2,
-					  tim2, flags & TRIG_ROUND_NEAREST);
-		break;
-	case 2:
-		if (*tim2 < this_board->ai_ns_min)
-			*tim2 = this_board->ai_ns_min;
-		*div1 = *tim2 / I8254_OSC_BASE_4MHZ;
-						/* convert timer (burst) */
-		if (*div1 < this_board->ai_pacer_min)
-			*div1 = this_board->ai_pacer_min;
-		*div2 = *tim1 / I8254_OSC_BASE_4MHZ;	/* scan timer */
-		*div2 = *div2 / *div1;		/* major timer is c1*c2 */
-		if (*div2 < chans)
-			*div2 = chans;
-
-		*tim2 = *div1 * I8254_OSC_BASE_4MHZ;	/* real convert timer */
-
-		if (cmd->convert_src == TRIG_NOW && !chnsshfront) {
-			/* use BSSH signal */
-			if (*div2 < (chans + 2))
-				*div2 = chans + 2;
-		}
+	*div1 = *tim2 / I8254_OSC_BASE_4MHZ;	/* convert timer (burst) */
+	*div2 = *tim1 / I8254_OSC_BASE_4MHZ;	/* scan timer */
+	*div2 = *div2 / *div1;			/* major timer is c1*c2 */
+	if (*div2 < chans)
+		*div2 = chans;
 
-		*tim1 = *div1 * *div2 * I8254_OSC_BASE_4MHZ;
-		break;
+	*tim2 = *div1 * I8254_OSC_BASE_4MHZ;	/* real convert timer */
+
+	if (cmd->convert_src == TRIG_NOW && !chnsshfront) {
+		/* use BSSH signal */
+		if (*div2 < (chans + 2))
+			*div2 = chans + 2;
 	}
+
+	*tim1 = *div1 * *div2 * I8254_OSC_BASE_4MHZ;
 }
 
 static void pci9118_start_pacer(struct comedi_device *dev, int mode)
 {
 	struct pci9118_private *devpriv = dev->private;
-	unsigned int divisor1 = devpriv->ai_divisor1;
-	unsigned int divisor2 = devpriv->ai_divisor2;
 
-	outl(0x74, dev->iobase + PCI9118_CNTCTRL);
-	outl(0xb4, dev->iobase + PCI9118_CNTCTRL);
-/* outl(0x30, dev->iobase + PCI9118_CNTCTRL); */
+	pci9118_timer_set_mode(dev, 1, I8254_MODE2);
+	pci9118_timer_set_mode(dev, 2, I8254_MODE2);
 	udelay(1);
 
 	if ((mode == 1) || (mode == 2) || (mode == 4)) {
-		outl(divisor2 & 0xff, dev->iobase + PCI9118_CNT2);
-		outl((divisor2 >> 8) & 0xff, dev->iobase + PCI9118_CNT2);
-		outl(divisor1 & 0xff, dev->iobase + PCI9118_CNT1);
-		outl((divisor1 >> 8) & 0xff, dev->iobase + PCI9118_CNT1);
+		pci9118_timer_write(dev, 2, devpriv->ai_divisor2);
+		pci9118_timer_write(dev, 1, devpriv->ai_divisor1);
 	}
 }
 
@@ -813,29 +556,24 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 	struct pci9118_private *devpriv = dev->private;
 
 	if (devpriv->usedma)
-		outl(inl(devpriv->iobase_a + AMCC_OP_REG_MCSR) &
-			(~EN_A2P_TRANSFERS),
-			devpriv->iobase_a + AMCC_OP_REG_MCSR);	/* stop DMA */
-	pci9118_exttrg_del(dev, EXTTRG_AI);
+		pci9118_amcc_dma_ena(dev, false);
+	pci9118_exttrg_enable(dev, false);
 	pci9118_start_pacer(dev, 0);	/* stop 8254 counters */
-	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-					/*
-					 * positive triggers, no S&H, no burst,
-					 * burst stop, no post trigger,
-					 * no about trigger, trigger stop
-					 */
-	devpriv->AdControlReg = 0x00;
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-					/*
-					 * bipolar, S.E., use 8254, stop 8354,
-					 * internal trigger, soft trigger,
-					 * disable INT and DMA
-					 */
-	outl(0, dev->iobase + PCI9118_BURST);
-	outl(1, dev->iobase + PCI9118_SCANMOD);
-	outl(2, dev->iobase + PCI9118_SCANMOD);	/* reset scan queue */
-	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
+	/* set default config (disable burst and triggers) */
+	devpriv->ai_cfg = PCI9118_AI_CFG_PDTRG | PCI9118_AI_CFG_PETRG;
+	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
+	/* reset acqusition control */
+	devpriv->ai_ctrl = 0;
+	outl(devpriv->ai_ctrl, dev->iobase + PCI9118_AI_CTRL_REG);
+	outl(0, dev->iobase + PCI9118_AI_BURST_NUM_REG);
+	/* reset scan queue */
+	outl(1, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+	outl(2, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+	pci9118_ai_reset_fifo(dev);
+
+	devpriv->int_ctrl = 0;
+	outl(devpriv->int_ctrl, dev->iobase + PCI9118_INT_CTRL_REG);
+	pci9118_amcc_int_ena(dev, false);
 
 	devpriv->ai_do = 0;
 	devpriv->usedma = 0;
@@ -847,44 +585,6 @@ static int pci9118_ai_cancel(struct comedi_device *dev,
 	devpriv->ai_neverending = 0;
 	devpriv->dma_actbuf = 0;
 
-	if (!devpriv->IntControlReg)
-		outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | 0x1f00,
-					devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-							/* allow INT in AMCC */
-
-	return 0;
-}
-
-static char pci9118_decode_error_status(struct comedi_device *dev,
-					struct comedi_subdevice *s,
-					unsigned char m)
-{
-	struct pci9118_private *devpriv = dev->private;
-
-	if (m & 0x100) {
-		dev_err(dev->class_dev,
-			"A/D FIFO Full status (Fatal Error!)\n");
-		devpriv->ai_maskerr &= ~0x100L;
-	}
-	if (m & 0x008) {
-		dev_err(dev->class_dev,
-			"A/D Burst Mode Overrun Status (Fatal Error!)\n");
-		devpriv->ai_maskerr &= ~0x008L;
-	}
-	if (m & 0x004) {
-		dev_err(dev->class_dev, "A/D Over Speed Status (Warning!)\n");
-		devpriv->ai_maskerr &= ~0x004L;
-	}
-	if (m & 0x002) {
-		dev_err(dev->class_dev, "A/D Overrun Status (Fatal Error!)\n");
-		devpriv->ai_maskerr &= ~0x002L;
-	}
-	if (m & devpriv->ai_maskharderr) {
-		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
-		return 1;
-	}
-
 	return 0;
 }
 
@@ -909,35 +609,14 @@ static void pci9118_ai_munge(struct comedi_device *dev,
 }
 
 static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
-					   struct comedi_subdevice *s,
-					   unsigned short int_adstat,
-					   unsigned int int_amcc,
-					   unsigned short int_daq)
+					   struct comedi_subdevice *s)
 {
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short sampl;
 
-	if (int_adstat & devpriv->ai_maskerr)
-		if (pci9118_decode_error_status(dev, s, int_adstat))
-			return;
-
-	sampl = inw(dev->iobase + PCI9118_AD_DATA);
+	sampl = inl(dev->iobase + PCI9118_AI_FIFO_REG);
 
-#ifdef PCI9118_PARANOIDCHECK
-	if (s->maxdata != 0xffff) {
-		if ((sampl & 0x000f) != devpriv->chanlist[s->async->cur_chan]) {
-							/* data dropout! */
-			dev_info(dev->class_dev,
-				 "A/D  SAMPL - data dropout: received channel %d, expected %d!\n",
-				 sampl & 0x000f,
-				 devpriv->chanlist[s->async->cur_chan]);
-			s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-			cfc_handle_events(dev, s);
-			return;
-		}
-	}
-#endif
 	cfc_write_to_buffer(s, sampl);
 	s->async->cur_chan++;
 	if (s->async->cur_chan >= cmd->scan_end_arg) {
@@ -950,63 +629,33 @@ static void interrupt_pci9118_ai_onesample(struct comedi_device *dev,
 				s->async->events |= COMEDI_CB_EOA;
 		}
 	}
-
-	cfc_handle_events(dev, s);
 }
 
 static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     unsigned short int_adstat,
-				     unsigned int int_amcc,
-				     unsigned short int_daq)
+				     struct comedi_subdevice *s)
 {
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
+	struct pci9118_dmabuf *dmabuf = &devpriv->dmabuf[devpriv->dma_actbuf];
 	unsigned int next_dma_buf, samplesinbuf, sampls, m;
 
-	if (int_amcc & MASTER_ABORT_INT) {
-		dev_err(dev->class_dev, "AMCC IRQ - MASTER DMA ABORT!\n");
-		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
-		return;
-	}
-
-	if (int_amcc & TARGET_ABORT_INT) {
-		dev_err(dev->class_dev, "AMCC IRQ - TARGET DMA ABORT!\n");
-		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
-		return;
-	}
-	if (int_adstat & devpriv->ai_maskerr)
-					/* if (int_adstat & 0x106) */
-		if (pci9118_decode_error_status(dev, s, int_adstat))
-			return;
-
-	samplesinbuf = devpriv->dmabuf_use_size[devpriv->dma_actbuf] >> 1;
-					/* number of received real samples */
+	samplesinbuf = dmabuf->use_size >> 1;	/* number of received samples */
 
 	if (devpriv->dma_doublebuf) {	/*
 					 * switch DMA buffers if is used
 					 * double buffering
 					 */
 		next_dma_buf = 1 - devpriv->dma_actbuf;
-		outl(devpriv->dmabuf_hw[next_dma_buf],
-		     devpriv->iobase_a + AMCC_OP_REG_MWAR);
-		outl(devpriv->dmabuf_use_size[next_dma_buf],
-		     devpriv->iobase_a + AMCC_OP_REG_MWTC);
-		devpriv->dmabuf_used_size[next_dma_buf] =
-		    devpriv->dmabuf_use_size[next_dma_buf];
+		pci9118_amcc_setup_dma(dev, next_dma_buf);
 		if (devpriv->ai_do == 4)
-			interrupt_pci9118_ai_mode4_switch(dev);
+			interrupt_pci9118_ai_mode4_switch(dev, next_dma_buf);
 	}
 
 	if (samplesinbuf) {
 		/* how many samples is to end of buffer */
 		m = s->async->prealloc_bufsz >> 1;
 		sampls = m;
-		move_block_from_dma(dev, s,
-				    devpriv->dmabuf_virt[devpriv->dma_actbuf],
-				    samplesinbuf);
+		move_block_from_dma(dev, s, dmabuf->virt, samplesinbuf);
 		m = m - sampls;		/* m=how many samples was transferred */
 	}
 
@@ -1016,18 +665,15 @@ static void interrupt_pci9118_ai_dma(struct comedi_device *dev,
 			s->async->events |= COMEDI_CB_EOA;
 	}
 
-	if (devpriv->dma_doublebuf) {	/* switch dma buffers */
+	if (devpriv->dma_doublebuf) {
+		/* switch dma buffers */
 		devpriv->dma_actbuf = 1 - devpriv->dma_actbuf;
-	} else {	/* restart DMA if is not used double buffering */
-		outl(devpriv->dmabuf_hw[0],
-		     devpriv->iobase_a + AMCC_OP_REG_MWAR);
-		outl(devpriv->dmabuf_use_size[0],
-		     devpriv->iobase_a + AMCC_OP_REG_MWTC);
+	} else {
+		/* restart DMA if is not used double buffering */
+		pci9118_amcc_setup_dma(dev, 0);
 		if (devpriv->ai_do == 4)
-			interrupt_pci9118_ai_mode4_switch(dev);
+			interrupt_pci9118_ai_mode4_switch(dev, 0);
 	}
-
-	cfc_handle_events(dev, s);
 }
 
 static irqreturn_t pci9118_interrupt(int irq, void *d)
@@ -1042,7 +688,7 @@ static irqreturn_t pci9118_interrupt(int irq, void *d)
 	if (!dev->attached)
 		return IRQ_NONE;
 
-	intsrc = inl(dev->iobase + PCI9118_INTSRC) & 0xf;
+	intsrc = inl(dev->iobase + PCI9118_INT_CTRL_REG) & 0xf;
 	intcsr = inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 
 	if (!intsrc && !(intcsr & ANY_S593X_INT))
@@ -1050,28 +696,63 @@ static irqreturn_t pci9118_interrupt(int irq, void *d)
 
 	outl(intcsr | 0x00ff0000, devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 
-	adstat = inw(dev->iobase + PCI9118_ADSTAT) & 0x1ff;
+	if (intcsr & MASTER_ABORT_INT) {
+		dev_err(dev->class_dev, "AMCC IRQ - MASTER DMA ABORT!\n");
+		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		goto interrupt_exit;
+	}
+
+	if (intcsr & TARGET_ABORT_INT) {
+		dev_err(dev->class_dev, "AMCC IRQ - TARGET DMA ABORT!\n");
+		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		goto interrupt_exit;
+	}
+
+	adstat = inl(dev->iobase + PCI9118_AI_STATUS_REG);
+	if ((adstat & PCI9118_AI_STATUS_NFULL) == 0) {
+		dev_err(dev->class_dev,
+			"A/D FIFO Full status (Fatal Error!)\n");
+		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
+		goto interrupt_exit;
+	}
+	if (adstat & PCI9118_AI_STATUS_BOVER) {
+		dev_err(dev->class_dev,
+			"A/D Burst Mode Overrun Status (Fatal Error!)\n");
+		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
+		goto interrupt_exit;
+	}
+	if (adstat & PCI9118_AI_STATUS_ADOS) {
+		dev_err(dev->class_dev, "A/D Over Speed Status (Warning!)\n");
+		s->async->events |= COMEDI_CB_ERROR;
+		goto interrupt_exit;
+	}
+	if (adstat & PCI9118_AI_STATUS_ADOR) {
+		dev_err(dev->class_dev, "A/D Overrun Status (Fatal Error!)\n");
+		s->async->events |= COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW;
+		goto interrupt_exit;
+	}
 
 	if (!devpriv->ai_do)
 		return IRQ_HANDLED;
 
 	if (devpriv->ai12_startstop) {
-		if ((adstat & AdStatus_DTH) && (intsrc & Int_DTrg)) {
+		if ((adstat & PCI9118_AI_STATUS_DTH) &&
+		    (intsrc & PCI9118_INT_CTRL_DTRG)) {
 			/* start/stop of measure */
 			if (devpriv->ai12_startstop & START_AI_EXT) {
 				/* deactivate EXT trigger */
 				devpriv->ai12_startstop &= ~START_AI_EXT;
 				if (!(devpriv->ai12_startstop & STOP_AI_EXT))
-					pci9118_exttrg_del(dev, EXTTRG_AI);
+					pci9118_exttrg_enable(dev, false);
 
 				/* start pacer */
 				pci9118_start_pacer(dev, devpriv->ai_do);
-				outl(devpriv->AdControlReg,
-				     dev->iobase + PCI9118_ADCNTRL);
+				outl(devpriv->ai_ctrl,
+				     dev->iobase + PCI9118_AI_CTRL_REG);
 			} else if (devpriv->ai12_startstop & STOP_AI_EXT) {
 				/* deactivate EXT trigger */
 				devpriv->ai12_startstop &= ~STOP_AI_EXT;
-				pci9118_exttrg_del(dev, EXTTRG_AI);
+				pci9118_exttrg_enable(dev, false);
 
 				/* on next interrupt measure will stop */
 				devpriv->ai_neverending = 0;
@@ -1080,198 +761,41 @@ static irqreturn_t pci9118_interrupt(int irq, void *d)
 	}
 
 	if (devpriv->usedma)
-		interrupt_pci9118_ai_dma(dev, s, adstat, intcsr, intsrc);
+		interrupt_pci9118_ai_dma(dev, s);
 	else
-		interrupt_pci9118_ai_onesample(dev, s, adstat, intcsr, intsrc);
+		interrupt_pci9118_ai_onesample(dev, s);
 
+interrupt_exit:
+	cfc_handle_events(dev, s);
 	return IRQ_HANDLED;
 }
 
-static int pci9118_ai_inttrig(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      unsigned int trig_num)
+static void pci9118_ai_cmd_start(struct comedi_device *dev)
 {
 	struct pci9118_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	if (trig_num != cmd->start_arg)
-		return -EINVAL;
-
-	devpriv->ai12_startstop &= ~START_AI_INT;
-	s->async->inttrig = NULL;
 
-	outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
+	outl(devpriv->int_ctrl, dev->iobase + PCI9118_INT_CTRL_REG);
+	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
 	if (devpriv->ai_do != 3) {
 		pci9118_start_pacer(dev, devpriv->ai_do);
-		devpriv->AdControlReg |= AdControl_SoftG;
+		devpriv->ai_ctrl |= PCI9118_AI_CTRL_SOFTG;
 	}
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-
-	return 1;
+	outl(devpriv->ai_ctrl, dev->iobase + PCI9118_AI_CTRL_REG);
 }
 
-static int pci9118_ai_cmdtest(struct comedi_device *dev,
+static int pci9118_ai_inttrig(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
-			      struct comedi_cmd *cmd)
+			      unsigned int trig_num)
 {
-	const struct boardtype *this_board = comedi_board(dev);
-	struct pci9118_private *devpriv = dev->private;
-	int err = 0;
-	unsigned int flags;
-	unsigned int arg;
-	unsigned int divisor1 = 0, divisor2 = 0;
-
-	/* Step 1 : check if triggers are trivially valid */
-
-	err |= cfc_check_trigger_src(&cmd->start_src,
-					TRIG_NOW | TRIG_EXT | TRIG_INT);
-
-	flags = TRIG_FOLLOW;
-	if (devpriv->master)
-		flags |= TRIG_TIMER | TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
-
-	flags = TRIG_TIMER | TRIG_EXT;
-	if (devpriv->master)
-		flags |= TRIG_NOW;
-	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
-
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src,
-					TRIG_COUNT | TRIG_NONE | TRIG_EXT);
-
-	if (err)
-		return 1;
-
-	/* Step 2a : make sure trigger sources are unique */
-
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
-
-	/* Step 2b : and mutually compatible */
-
-	if (cmd->start_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT)
-		err |= -EINVAL;
-
-	if (cmd->start_src == TRIG_INT && cmd->scan_begin_src == TRIG_INT)
-		err |= -EINVAL;
-
-	if ((cmd->scan_begin_src & (TRIG_TIMER | TRIG_EXT)) &&
-	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_NOW))))
-		err |= -EINVAL;
-
-	if ((cmd->scan_begin_src == TRIG_FOLLOW) &&
-	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_EXT))))
-		err |= -EINVAL;
-
-	if (cmd->stop_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT)
-		err |= -EINVAL;
-
-	if (err)
-		return 2;
-
-	/* Step 3: check if arguments are trivially valid */
-
-	switch (cmd->start_src) {
-	case TRIG_NOW:
-	case TRIG_EXT:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-		break;
-	case TRIG_INT:
-		/* start_arg is the internal trigger (any value) */
-		break;
-	}
-
-	if (cmd->scan_begin_src & (TRIG_FOLLOW | TRIG_EXT))
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
-
-	if ((cmd->scan_begin_src == TRIG_TIMER) &&
-	    (cmd->convert_src == TRIG_TIMER) && (cmd->scan_end_arg == 1)) {
-		cmd->scan_begin_src = TRIG_FOLLOW;
-		cmd->convert_arg = cmd->scan_begin_arg;
-		cmd->scan_begin_arg = 0;
-	}
-
-	if (cmd->scan_begin_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 this_board->ai_ns_min);
-
-	if (cmd->scan_begin_src == TRIG_EXT)
-		if (cmd->scan_begin_arg) {
-			cmd->scan_begin_arg = 0;
-			err |= -EINVAL;
-			err |= cfc_check_trigger_arg_max(&cmd->scan_end_arg,
-							 65535);
-		}
-
-	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW))
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 this_board->ai_ns_min);
-
-	if (cmd->convert_src == TRIG_EXT)
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
-
-	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
-	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-
-	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
-
-	err |= cfc_check_trigger_arg_min(&cmd->scan_end_arg,
-					 cmd->chanlist_len);
-
-	if ((cmd->scan_end_arg % cmd->chanlist_len)) {
-		cmd->scan_end_arg =
-		    cmd->chanlist_len * (cmd->scan_end_arg / cmd->chanlist_len);
-		err |= -EINVAL;
-	}
-
-	if (err)
-		return 3;
-
-	/* step 4: fix up any arguments */
-
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		arg = cmd->scan_begin_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
-					  &divisor1, &divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
-	}
-
-	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW)) {
-		arg = cmd->convert_arg;
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
-					  &divisor1, &divisor2,
-					  &arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
-
-		if (cmd->scan_begin_src == TRIG_TIMER &&
-		    cmd->convert_src == TRIG_NOW) {
-			if (cmd->convert_arg == 0) {
-				arg = this_board->ai_ns_min *
-				      (cmd->scan_end_arg + 2);
-			} else {
-				arg = cmd->convert_arg * cmd->chanlist_len;
-			}
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 arg);
-		}
-	}
+	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (err)
-		return 4;
+	if (trig_num != cmd->start_arg)
+		return -EINVAL;
 
-	if (cmd->chanlist)
-		if (!check_channel_list(dev, s, cmd->chanlist_len,
-					cmd->chanlist, 0, 0))
-			return 5;	/* incorrect channels list */
+	s->async->inttrig = NULL;
+	pci9118_ai_cmd_start(dev);
 
-	return 0;
+	return 1;
 }
 
 static int Compute_and_setup_dma(struct comedi_device *dev,
@@ -1279,10 +803,12 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 {
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
+	struct pci9118_dmabuf *dmabuf0 = &devpriv->dmabuf[0];
+	struct pci9118_dmabuf *dmabuf1 = &devpriv->dmabuf[1];
 	unsigned int dmalen0, dmalen1, i;
 
-	dmalen0 = devpriv->dmabuf_size[0];
-	dmalen1 = devpriv->dmabuf_size[1];
+	dmalen0 = dmabuf0->size;
+	dmalen1 = dmabuf1->size;
 	/* isn't output buff smaller that our DMA buff? */
 	if (dmalen0 > s->async->prealloc_bufsz) {
 		/* align to 32bit down */
@@ -1294,12 +820,12 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 	}
 
 	/* we want wake up every scan? */
-	if (devpriv->ai_flags & TRIG_WAKE_EOS) {
+	if (devpriv->ai_flags & CMDF_WAKE_EOS) {
 		if (dmalen0 < (devpriv->ai_n_realscanlen << 1)) {
 			/* uff, too short DMA buffer, disable EOS support! */
-			devpriv->ai_flags &= (~TRIG_WAKE_EOS);
+			devpriv->ai_flags &= (~CMDF_WAKE_EOS);
 			dev_info(dev->class_dev,
-				 "WAR: DMA0 buf too short, can't support TRIG_WAKE_EOS (%d<%d)\n",
+				 "WAR: DMA0 buf too short, can't support CMDF_WAKE_EOS (%d<%d)\n",
 				  dmalen0, devpriv->ai_n_realscanlen << 1);
 		} else {
 			/* short first DMA buffer to one scan */
@@ -1312,12 +838,12 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 			}
 		}
 	}
-	if (devpriv->ai_flags & TRIG_WAKE_EOS) {
+	if (devpriv->ai_flags & CMDF_WAKE_EOS) {
 		if (dmalen1 < (devpriv->ai_n_realscanlen << 1)) {
 			/* uff, too short DMA buffer, disable EOS support! */
-			devpriv->ai_flags &= (~TRIG_WAKE_EOS);
+			devpriv->ai_flags &= (~CMDF_WAKE_EOS);
 			dev_info(dev->class_dev,
-				 "WAR: DMA1 buf too short, can't support TRIG_WAKE_EOS (%d<%d)\n",
+				 "WAR: DMA1 buf too short, can't support CMDF_WAKE_EOS (%d<%d)\n",
 				 dmalen1, devpriv->ai_n_realscanlen << 1);
 		} else {
 			/* short second DMA buffer to one scan */
@@ -1331,8 +857,8 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 		}
 	}
 
-	/* transfer without TRIG_WAKE_EOS */
-	if (!(devpriv->ai_flags & TRIG_WAKE_EOS)) {
+	/* transfer without CMDF_WAKE_EOS */
+	if (!(devpriv->ai_flags & CMDF_WAKE_EOS)) {
 		/* if it's possible then align DMA buffers to length of scan */
 		i = dmalen0;
 		dmalen0 =
@@ -1378,37 +904,16 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 
 	/* these DMA buffer size will be used */
 	devpriv->dma_actbuf = 0;
-	devpriv->dmabuf_use_size[0] = dmalen0;
-	devpriv->dmabuf_use_size[1] = dmalen1;
-
-#if 0
-	if (cmd->scan_end_arg < this_board->half_fifo_size) {
-		devpriv->dmabuf_panic_size[0] =
-		    (this_board->half_fifo_size / cmd->scan_end_arg +
-		     1) * cmd->scan_end_arg * sizeof(short);
-		devpriv->dmabuf_panic_size[1] =
-		    (this_board->half_fifo_size / cmd->scan_end_arg +
-		     1) * cmd->scan_end_arg * sizeof(short);
-	} else {
-		devpriv->dmabuf_panic_size[0] =
-		    (cmd->scan_end_arg << 1) % devpriv->dmabuf_size[0];
-		devpriv->dmabuf_panic_size[1] =
-		    (cmd->scan_end_arg << 1) % devpriv->dmabuf_size[1];
-	}
-#endif
+	dmabuf0->use_size = dmalen0;
+	dmabuf1->use_size = dmalen1;
 
-	outl(inl(devpriv->iobase_a + AMCC_OP_REG_MCSR) & (~EN_A2P_TRANSFERS),
-			devpriv->iobase_a + AMCC_OP_REG_MCSR);	/* stop DMA */
-	outl(devpriv->dmabuf_hw[0], devpriv->iobase_a + AMCC_OP_REG_MWAR);
-	outl(devpriv->dmabuf_use_size[0], devpriv->iobase_a + AMCC_OP_REG_MWTC);
+	pci9118_amcc_dma_ena(dev, false);
+	pci9118_amcc_setup_dma(dev, 0);
 	/* init DMA transfer */
 	outl(0x00000000 | AINT_WRITE_COMPL,
 	     devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 /* outl(0x02000000|AINT_WRITE_COMPL, devpriv->iobase_a+AMCC_OP_REG_INTCSR); */
-
-	outl(inl(devpriv->iobase_a +
-		 AMCC_OP_REG_MCSR) | RESET_A2P_FLAGS | A2P_HI_PRIORITY |
-	     EN_A2P_TRANSFERS, devpriv->iobase_a + AMCC_OP_REG_MCSR);
+	pci9118_amcc_dma_ena(dev, true);
 	outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | EN_A2P_TRANSFERS,
 			devpriv->iobase_a + AMCC_OP_REG_INTCSR);
 						/* allow bus mastering */
@@ -1416,135 +921,16 @@ static int Compute_and_setup_dma(struct comedi_device *dev,
 	return 0;
 }
 
-static int pci9118_ai_docmd_sampl(struct comedi_device *dev,
-				  struct comedi_subdevice *s)
-{
-	struct pci9118_private *devpriv = dev->private;
-
-	switch (devpriv->ai_do) {
-	case 1:
-		devpriv->AdControlReg |= AdControl_TmrTr;
-		break;
-	case 2:
-		dev_err(dev->class_dev, "%s mode 2 bug!\n", __func__);
-		return -EIO;
-	case 3:
-		devpriv->AdControlReg |= AdControl_ExtM;
-		break;
-	case 4:
-		dev_err(dev->class_dev, "%s mode 4 bug!\n", __func__);
-		return -EIO;
-	default:
-		dev_err(dev->class_dev, "%s mode number bug!\n", __func__);
-		return -EIO;
-	}
-
-	if (devpriv->ai12_startstop)
-		pci9118_exttrg_add(dev, EXTTRG_AI);
-						/* activate EXT trigger */
-
-	if ((devpriv->ai_do == 1) || (devpriv->ai_do == 2))
-		devpriv->IntControlReg |= Int_Timer;
-
-	devpriv->AdControlReg |= AdControl_Int;
-
-	outl(inl(devpriv->iobase_a + AMCC_OP_REG_INTCSR) | 0x1f00,
-			devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-							/* allow INT in AMCC */
-
-	if (!(devpriv->ai12_startstop & (START_AI_EXT | START_AI_INT))) {
-		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-		outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-		if (devpriv->ai_do != 3) {
-			pci9118_start_pacer(dev, devpriv->ai_do);
-			devpriv->AdControlReg |= AdControl_SoftG;
-		}
-		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-	}
-
-	return 0;
-}
-
-static int pci9118_ai_docmd_dma(struct comedi_device *dev,
-				struct comedi_subdevice *s)
-{
-	struct pci9118_private *devpriv = dev->private;
-	struct comedi_cmd *cmd = &s->async->cmd;
-
-	Compute_and_setup_dma(dev, s);
-
-	switch (devpriv->ai_do) {
-	case 1:
-		devpriv->AdControlReg |=
-		    ((AdControl_TmrTr | AdControl_Dma) & 0xff);
-		break;
-	case 2:
-		devpriv->AdControlReg |=
-		    ((AdControl_TmrTr | AdControl_Dma) & 0xff);
-		devpriv->AdFunctionReg =
-		    AdFunction_PDTrg | AdFunction_PETrg | AdFunction_BM |
-		    AdFunction_BS;
-		if (cmd->convert_src == TRIG_NOW && !devpriv->softsshdelay)
-			devpriv->AdFunctionReg |= AdFunction_BSSH;
-		outl(devpriv->ai_n_realscanlen, dev->iobase + PCI9118_BURST);
-		break;
-	case 3:
-		devpriv->AdControlReg |=
-		    ((AdControl_ExtM | AdControl_Dma) & 0xff);
-		devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
-		break;
-	case 4:
-		devpriv->AdControlReg |=
-		    ((AdControl_TmrTr | AdControl_Dma) & 0xff);
-		devpriv->AdFunctionReg =
-		    AdFunction_PDTrg | AdFunction_PETrg | AdFunction_AM;
-		outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-		outl(0x30, dev->iobase + PCI9118_CNTCTRL);
-		outl((devpriv->dmabuf_hw[0] >> 1) & 0xff,
-		     dev->iobase + PCI9118_CNT0);
-		outl((devpriv->dmabuf_hw[0] >> 9) & 0xff,
-		     dev->iobase + PCI9118_CNT0);
-		devpriv->AdFunctionReg |= AdFunction_Start;
-		break;
-	default:
-		dev_err(dev->class_dev, "%s mode number bug!\n", __func__);
-		return -EIO;
-	}
-
-	if (devpriv->ai12_startstop) {
-		pci9118_exttrg_add(dev, EXTTRG_AI);
-						/* activate EXT trigger */
-	}
-
-	outl(0x02000000 | AINT_WRITE_COMPL,
-	     devpriv->iobase_a + AMCC_OP_REG_INTCSR);
-
-	if (!(devpriv->ai12_startstop & (START_AI_EXT | START_AI_INT))) {
-		outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-		outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-		if (devpriv->ai_do != 3) {
-			pci9118_start_pacer(dev, devpriv->ai_do);
-			devpriv->AdControlReg |= AdControl_SoftG;
-		}
-		outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-	}
-
-	return 0;
-}
-
 static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct boardtype *this_board = comedi_board(dev);
 	struct pci9118_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned int addchans = 0;
-	int ret = 0;
 
 	devpriv->ai12_startstop = 0;
 	devpriv->ai_flags = cmd->flags;
 	devpriv->ai_add_front = 0;
 	devpriv->ai_add_back = 0;
-	devpriv->ai_maskerr = 0x10e;
 
 	/* prepare for start/stop conditions */
 	if (cmd->start_src == TRIG_EXT)
@@ -1553,10 +939,6 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->ai_neverending = 1;
 		devpriv->ai12_startstop |= STOP_AI_EXT;
 	}
-	if (cmd->start_src == TRIG_INT) {
-		devpriv->ai12_startstop |= START_AI_INT;
-		s->async->inttrig = pci9118_ai_inttrig;
-	}
 	if (cmd->stop_src == TRIG_NONE)
 		devpriv->ai_neverending = 1;
 	if (cmd->stop_src == TRIG_COUNT)
@@ -1570,7 +952,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	devpriv->ai_add_back = 0;
 	if (devpriv->master) {
 		devpriv->usedma = 1;
-		if ((cmd->flags & TRIG_WAKE_EOS) &&
+		if ((cmd->flags & CMDF_WAKE_EOS) &&
 		    (cmd->scan_end_arg == 1)) {
 			if (cmd->convert_src == TRIG_NOW)
 				devpriv->ai_add_back = 1;
@@ -1582,7 +964,7 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 					 */
 			}
 		}
-		if ((cmd->flags & TRIG_WAKE_EOS) &&
+		if ((cmd->flags & CMDF_WAKE_EOS) &&
 		    (cmd->scan_end_arg & 1) &&
 		    (cmd->scan_end_arg > 1)) {
 			if (cmd->scan_begin_src == TRIG_FOLLOW) {
@@ -1612,8 +994,8 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			devpriv->ai_add_front++;
 			devpriv->ai_add_back = 0;
 		}
-		if (cmd->convert_arg < this_board->ai_ns_min)
-			cmd->convert_arg = this_board->ai_ns_min;
+		if (cmd->convert_arg < devpriv->ai_ns_min)
+			cmd->convert_arg = devpriv->ai_ns_min;
 		addchans = devpriv->softsshdelay / cmd->convert_arg;
 		if (devpriv->softsshdelay % cmd->convert_arg)
 			addchans++;
@@ -1642,171 +1024,432 @@ static int pci9118_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				cmd->chanlist, devpriv->ai_add_front,
 				devpriv->ai_add_back))
 		return -EINVAL;
-	if (!setup_channel_list(dev, s, cmd->chanlist_len,
-				cmd->chanlist, 0, devpriv->ai_add_front,
-				devpriv->ai_add_back, devpriv->usedma))
-		return -EINVAL;
 
-	/* compute timers settings */
 	/*
-	 * simplest way, fr=4Mhz/(tim1*tim2),
-	 * channel manipulation without timers effect
+	 * Configure analog input and load the chanlist.
+	 * The acqusition control bits are enabled later.
 	 */
-	if (((cmd->scan_begin_src == TRIG_FOLLOW) ||
-		(cmd->scan_begin_src == TRIG_EXT) ||
-		(cmd->scan_begin_src == TRIG_INT)) &&
-		(cmd->convert_src == TRIG_TIMER)) {
-					/* both timer is used for one time */
+	pci9118_set_chanlist(dev, s, cmd->chanlist_len, cmd->chanlist,
+			     devpriv->ai_add_front, devpriv->ai_add_back);
+
+	/* Determine acqusition mode and calculate timing */
+	devpriv->ai_do = 0;
+	if (cmd->scan_begin_src != TRIG_TIMER &&
+	    cmd->convert_src == TRIG_TIMER) {
+		/* cascaded timers 1 and 2 are used for convert timing */
 		if (cmd->scan_begin_src == TRIG_EXT)
 			devpriv->ai_do = 4;
 		else
 			devpriv->ai_do = 1;
-		pci9118_calc_divisors(devpriv->ai_do, dev, s,
-				      &cmd->scan_begin_arg, &cmd->convert_arg,
-				      devpriv->ai_flags,
-				      devpriv->ai_n_realscanlen,
-				      &devpriv->ai_divisor1,
-				      &devpriv->ai_divisor2,
-				      devpriv->ai_add_front);
+
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
+					  &devpriv->ai_divisor1,
+					  &devpriv->ai_divisor2,
+					  &cmd->convert_arg,
+					  devpriv->ai_flags &
+					  CMDF_ROUND_NEAREST);
+
+		devpriv->ai_ctrl |= PCI9118_AI_CTRL_TMRTR;
+
+		if (!devpriv->usedma) {
+			devpriv->ai_ctrl |= PCI9118_AI_CTRL_INT;
+			devpriv->int_ctrl |= PCI9118_INT_CTRL_TIMER;
+		}
+
+		if (cmd->scan_begin_src == TRIG_EXT) {
+			struct pci9118_dmabuf *dmabuf = &devpriv->dmabuf[0];
+
+			devpriv->ai_cfg |= PCI9118_AI_CFG_AM;
+			outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
+			pci9118_timer_set_mode(dev, 0, I8254_MODE0);
+			pci9118_timer_write(dev, 0, dmabuf->hw >> 1);
+			devpriv->ai_cfg |= PCI9118_AI_CFG_START;
+		}
 	}
 
-	if ((cmd->scan_begin_src == TRIG_TIMER) &&
-		((cmd->convert_src == TRIG_TIMER) ||
-		(cmd->convert_src == TRIG_NOW))) {
-						/* double timed action */
+	if (cmd->scan_begin_src == TRIG_TIMER &&
+	    cmd->convert_src != TRIG_EXT) {
 		if (!devpriv->usedma) {
 			dev_err(dev->class_dev,
 				"cmd->scan_begin_src=TRIG_TIMER works only with bus mastering!\n");
 			return -EIO;
 		}
 
+		/* double timed action */
 		devpriv->ai_do = 2;
-		pci9118_calc_divisors(devpriv->ai_do, dev, s,
+
+		pci9118_calc_divisors(dev, s,
 				      &cmd->scan_begin_arg, &cmd->convert_arg,
 				      devpriv->ai_flags,
 				      devpriv->ai_n_realscanlen,
 				      &devpriv->ai_divisor1,
 				      &devpriv->ai_divisor2,
 				      devpriv->ai_add_front);
+
+		devpriv->ai_ctrl |= PCI9118_AI_CTRL_TMRTR;
+		devpriv->ai_cfg |= PCI9118_AI_CFG_BM | PCI9118_AI_CFG_BS;
+		if (cmd->convert_src == TRIG_NOW && !devpriv->softsshdelay)
+			devpriv->ai_cfg |= PCI9118_AI_CFG_BSSH;
+		outl(devpriv->ai_n_realscanlen,
+		     dev->iobase + PCI9118_AI_BURST_NUM_REG);
 	}
 
-	if ((cmd->scan_begin_src == TRIG_FOLLOW)
-	    && (cmd->convert_src == TRIG_EXT)) {
+	if (cmd->scan_begin_src == TRIG_FOLLOW &&
+	    cmd->convert_src == TRIG_EXT) {
+		/* external trigger conversion */
 		devpriv->ai_do = 3;
+
+		devpriv->ai_ctrl |= PCI9118_AI_CTRL_EXTM;
+	}
+
+	if (devpriv->ai_do == 0) {
+		dev_err(dev->class_dev,
+			"Unable to determine acqusition mode! BUG in (*do_cmdtest)?\n");
+		return -EINVAL;
 	}
 
+	if (devpriv->usedma)
+		devpriv->ai_ctrl |= PCI9118_AI_CTRL_DMA;
+
 	pci9118_start_pacer(dev, -1);	/* stop pacer */
 
-	devpriv->AdControlReg = 0;	/*
-					 * bipolar, S.E., use 8254, stop 8354,
-					 * internal trigger, soft trigger,
-					 * disable DMA
-					 */
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
-					/*
-					 * positive triggers, no S&H, no burst,
-					 * burst stop, no post trigger,
-					 * no about trigger, trigger stop
-					 */
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
+	/* set default config (disable burst and triggers) */
+	devpriv->ai_cfg = PCI9118_AI_CFG_PDTRG | PCI9118_AI_CFG_PETRG;
+	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
 	udelay(1);
-	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
-	inl(dev->iobase + PCI9118_ADSTAT);	/*
-						 * flush A/D and INT
-						 * status register
-						 */
-	inl(dev->iobase + PCI9118_INTSRC);
+	pci9118_ai_reset_fifo(dev);
+
+	/* clear A/D and INT status registers */
+	inl(dev->iobase + PCI9118_AI_STATUS_REG);
+	inl(dev->iobase + PCI9118_INT_CTRL_REG);
 
 	devpriv->ai_act_scan = 0;
 	devpriv->ai_act_dmapos = 0;
 	s->async->cur_chan = 0;
 
-	if (devpriv->usedma)
-		ret = pci9118_ai_docmd_dma(dev, s);
-	else
-		ret = pci9118_ai_docmd_sampl(dev, s);
+	if (devpriv->usedma) {
+		Compute_and_setup_dma(dev, s);
+
+		outl(0x02000000 | AINT_WRITE_COMPL,
+		     devpriv->iobase_a + AMCC_OP_REG_INTCSR);
+	} else {
+		pci9118_amcc_int_ena(dev, true);
+	}
 
-	return ret;
+	/* start async command now or wait for internal trigger */
+	if (cmd->start_src == TRIG_NOW)
+		pci9118_ai_cmd_start(dev);
+	else if (cmd->start_src == TRIG_INT)
+		s->async->inttrig = pci9118_ai_inttrig;
+
+	/* enable external trigger for command start/stop */
+	if (cmd->start_src == TRIG_EXT || cmd->stop_src == TRIG_EXT)
+		pci9118_exttrg_enable(dev, true);
+
+	return 0;
 }
 
-static int pci9118_reset(struct comedi_device *dev)
+static int pci9118_ai_cmdtest(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_cmd *cmd)
 {
 	struct pci9118_private *devpriv = dev->private;
+	int err = 0;
+	unsigned int flags;
+	unsigned int arg;
+	unsigned int divisor1 = 0, divisor2 = 0;
 
-	devpriv->IntControlReg = 0;
-	devpriv->exttrg_users = 0;
-	inl(dev->iobase + PCI9118_INTCTRL);
-	outl(devpriv->IntControlReg, dev->iobase + PCI9118_INTCTRL);
-						/* disable interrupts source */
-	outl(0x30, dev->iobase + PCI9118_CNTCTRL);
-/* outl(0xb4, dev->iobase + PCI9118_CNTCTRL); */
-	pci9118_start_pacer(dev, 0);		/* stop 8254 counters */
-	devpriv->AdControlReg = 0;
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-						/*
-						 * bipolar, S.E., use 8254,
-						 * stop 8354, internal trigger,
-						 * soft trigger,
-						 * disable INT and DMA
-						 */
-	outl(0, dev->iobase + PCI9118_BURST);
-	outl(1, dev->iobase + PCI9118_SCANMOD);
-	outl(2, dev->iobase + PCI9118_SCANMOD);	/* reset scan queue */
-	devpriv->AdFunctionReg = AdFunction_PDTrg | AdFunction_PETrg;
-	outl(devpriv->AdFunctionReg, dev->iobase + PCI9118_ADFUNC);
-						/*
-						 * positive triggers, no S&H,
-						 * no burst, burst stop,
-						 * no post trigger,
-						 * no about trigger,
-						 * trigger stop
-						 */
+	/* Step 1 : check if triggers are trivially valid */
 
-	devpriv->ao_data[0] = 2047;
-	devpriv->ao_data[1] = 2047;
-	outl(devpriv->ao_data[0], dev->iobase + PCI9118_DA1);
-						/* reset A/D outs to 0V */
-	outl(devpriv->ao_data[1], dev->iobase + PCI9118_DA2);
-	outl(0, dev->iobase + PCI9118_DO);	/* reset digi outs to L */
-	udelay(10);
-	inl(dev->iobase + PCI9118_AD_DATA);
-	outl(0, dev->iobase + PCI9118_DELFIFO);	/* flush FIFO */
-	outl(0, dev->iobase + PCI9118_INTSRC);	/* remove INT requests */
-	inl(dev->iobase + PCI9118_ADSTAT);	/* flush A/D status register */
-	inl(dev->iobase + PCI9118_INTSRC);	/* flush INT requests */
-	devpriv->AdControlReg = 0;
-	outl(devpriv->AdControlReg, dev->iobase + PCI9118_ADCNTRL);
-						/*
-						 * bipolar, S.E., use 8254,
-						 * stop 8354, internal trigger,
-						 * soft trigger,
-						 * disable INT and DMA
-						 */
+	err |= cfc_check_trigger_src(&cmd->start_src,
+					TRIG_NOW | TRIG_EXT | TRIG_INT);
+
+	flags = TRIG_FOLLOW;
+	if (devpriv->master)
+		flags |= TRIG_TIMER | TRIG_EXT;
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src, flags);
+
+	flags = TRIG_TIMER | TRIG_EXT;
+	if (devpriv->master)
+		flags |= TRIG_NOW;
+	err |= cfc_check_trigger_src(&cmd->convert_src, flags);
+
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+	err |= cfc_check_trigger_src(&cmd->stop_src,
+					TRIG_COUNT | TRIG_NONE | TRIG_EXT);
+
+	if (err)
+		return 1;
+
+	/* Step 2a : make sure trigger sources are unique */
+
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
+
+	if (cmd->start_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT)
+		err |= -EINVAL;
+
+	if (cmd->start_src == TRIG_INT && cmd->scan_begin_src == TRIG_INT)
+		err |= -EINVAL;
 
-	devpriv->exttrg_users = 0;
+	if ((cmd->scan_begin_src & (TRIG_TIMER | TRIG_EXT)) &&
+	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_NOW))))
+		err |= -EINVAL;
+
+	if ((cmd->scan_begin_src == TRIG_FOLLOW) &&
+	    (!(cmd->convert_src & (TRIG_TIMER | TRIG_EXT))))
+		err |= -EINVAL;
+
+	if (cmd->stop_src == TRIG_EXT && cmd->scan_begin_src == TRIG_EXT)
+		err |= -EINVAL;
+
+	if (err)
+		return 2;
+
+	/* Step 3: check if arguments are trivially valid */
+
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+	case TRIG_EXT:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_INT:
+		/* start_arg is the internal trigger (any value) */
+		break;
+	}
+
+	if (cmd->scan_begin_src & (TRIG_FOLLOW | TRIG_EXT))
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, 0);
+
+	if ((cmd->scan_begin_src == TRIG_TIMER) &&
+	    (cmd->convert_src == TRIG_TIMER) && (cmd->scan_end_arg == 1)) {
+		cmd->scan_begin_src = TRIG_FOLLOW;
+		cmd->convert_arg = cmd->scan_begin_arg;
+		cmd->scan_begin_arg = 0;
+	}
+
+	if (cmd->scan_begin_src == TRIG_TIMER)
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+						 devpriv->ai_ns_min);
+
+	if (cmd->scan_begin_src == TRIG_EXT)
+		if (cmd->scan_begin_arg) {
+			cmd->scan_begin_arg = 0;
+			err |= -EINVAL;
+			err |= cfc_check_trigger_arg_max(&cmd->scan_end_arg,
+							 65535);
+		}
+
+	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW))
+		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
+						 devpriv->ai_ns_min);
+
+	if (cmd->convert_src == TRIG_EXT)
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
+
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+
+	err |= cfc_check_trigger_arg_min(&cmd->chanlist_len, 1);
+
+	err |= cfc_check_trigger_arg_min(&cmd->scan_end_arg,
+					 cmd->chanlist_len);
+
+	if ((cmd->scan_end_arg % cmd->chanlist_len)) {
+		cmd->scan_end_arg =
+		    cmd->chanlist_len * (cmd->scan_end_arg / cmd->chanlist_len);
+		err |= -EINVAL;
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		arg = cmd->scan_begin_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
+					  &divisor1, &divisor2,
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
+	}
+
+	if (cmd->convert_src & (TRIG_TIMER | TRIG_NOW)) {
+		arg = cmd->convert_arg;
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_4MHZ,
+					  &divisor1, &divisor2,
+					  &arg, cmd->flags);
+		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
+
+		if (cmd->scan_begin_src == TRIG_TIMER &&
+		    cmd->convert_src == TRIG_NOW) {
+			if (cmd->convert_arg == 0) {
+				arg = devpriv->ai_ns_min *
+				      (cmd->scan_end_arg + 2);
+			} else {
+				arg = cmd->convert_arg * cmd->chanlist_len;
+			}
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+							 arg);
+		}
+	}
+
+	if (err)
+		return 4;
+
+	if (cmd->chanlist)
+		if (!check_channel_list(dev, s, cmd->chanlist_len,
+					cmd->chanlist, 0, 0))
+			return 5;	/* incorrect channels list */
 
 	return 0;
 }
 
-/*
- * FIXME - this is pretty ineffective because all the supported board types
- * have the same device ID!
- */
-static const struct boardtype *pci9118_find_boardinfo(struct pci_dev *pcidev)
+static int pci9118_ai_eoc(struct comedi_device *dev,
+			  struct comedi_subdevice *s,
+			  struct comedi_insn *insn,
+			  unsigned long context)
 {
-	unsigned int i;
+	unsigned int status;
 
-	for (i = 0; i < ARRAY_SIZE(boardtypes); i++)
-		if (pcidev->device == boardtypes[i].device_id)
-			return &boardtypes[i];
-	return NULL;
+	status = inl(dev->iobase + PCI9118_AI_STATUS_REG);
+	if (status & PCI9118_AI_STATUS_ADRDY)
+		return 0;
+	return -EBUSY;
+}
+
+static void pci9118_ai_start_conv(struct comedi_device *dev)
+{
+	/* writing any value triggers an A/D conversion */
+	outl(0, dev->iobase + PCI9118_SOFTTRG_REG);
+}
+
+static int pci9118_ai_insn_read(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	struct pci9118_private *devpriv = dev->private;
+	unsigned int val;
+	int ret;
+	int i;
+
+       /*
+	* Configure analog input based on the chanspec.
+	* Acqusition is software controlled without interrupts.
+	*/
+	pci9118_set_chanlist(dev, s, 1, &insn->chanspec, 0, 0);
+
+	/* set default config (disable burst and triggers) */
+	devpriv->ai_cfg = PCI9118_AI_CFG_PDTRG | PCI9118_AI_CFG_PETRG;
+	outl(devpriv->ai_cfg, dev->iobase + PCI9118_AI_CFG_REG);
+
+	pci9118_ai_reset_fifo(dev);
+
+	for (i = 0; i < insn->n; i++) {
+		pci9118_ai_start_conv(dev);
+
+		ret = comedi_timeout(dev, s, insn, pci9118_ai_eoc, 0);
+		if (ret)
+			return ret;
+
+		val = inl(dev->iobase + PCI9118_AI_FIFO_REG);
+		if (s->maxdata == 0xffff)
+			data[i] = (val & 0xffff) ^ 0x8000;
+		else
+			data[i] = (val >> 4) & 0xfff;
+	}
+
+	return insn->n;
+}
+
+static int pci9118_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
+	int i;
+
+	for (i = 0; i < insn->n; i++) {
+		val = data[i];
+		outl(val, dev->iobase + PCI9118_AO_REG(chan));
+	}
+	s->readback[chan] = val;
+
+	return insn->n;
+}
+
+static int pci9118_di_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	/*
+	 * The digital inputs and outputs share the read register.
+	 * bits [7:4] are the digital outputs
+	 * bits [3:0] are the digital inputs
+	 */
+	data[1] = inl(dev->iobase + PCI9118_DIO_REG) & 0xf;
+
+	return insn->n;
+}
+
+static int pci9118_do_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	/*
+	 * The digital outputs are set with the same register that
+	 * the digital inputs and outputs are read from. But the
+	 * outputs are set with bits [3:0] so we can simply write
+	 * the s->state to set them.
+	 */
+	if (comedi_dio_update_state(s, data))
+		outl(s->state, dev->iobase + PCI9118_DIO_REG);
+
+	data[1] = s->state;
+
+	return insn->n;
+}
+
+static void pci9118_reset(struct comedi_device *dev)
+{
+	/* reset analog input subsystem */
+	outl(0, dev->iobase + PCI9118_INT_CTRL_REG);
+	outl(0, dev->iobase + PCI9118_AI_CTRL_REG);
+	outl(0, dev->iobase + PCI9118_AI_CFG_REG);
+	pci9118_ai_reset_fifo(dev);
+
+	/* clear any pending interrupts and status */
+	inl(dev->iobase + PCI9118_INT_CTRL_REG);
+	inl(dev->iobase + PCI9118_AI_STATUS_REG);
+
+	/* reset and stop counters */
+	pci9118_timer_set_mode(dev, 0, I8254_MODE0);
+	pci9118_start_pacer(dev, 0);
+
+	/* reset DMA and scan queue */
+	outl(0, dev->iobase + PCI9118_AI_BURST_NUM_REG);
+	outl(1, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+	outl(2, dev->iobase + PCI9118_AI_AUTOSCAN_MODE_REG);
+
+	/* reset analog outputs to 0V */
+	outl(2047, dev->iobase + PCI9118_AO_REG(0));
+	outl(2047, dev->iobase + PCI9118_AO_REG(1));
 }
 
 static struct pci_dev *pci9118_find_pci(struct comedi_device *dev,
 					struct comedi_devconfig *it)
 {
-	const struct boardtype *this_board = comedi_board(dev);
 	struct pci_dev *pcidev = NULL;
 	int bus = it->options[0];
 	int slot = it->options[1];
@@ -1814,7 +1457,7 @@ static struct pci_dev *pci9118_find_pci(struct comedi_device *dev,
 	for_each_pci_dev(pcidev) {
 		if (pcidev->vendor != PCI_VENDOR_ID_AMCC)
 			continue;
-		if (pcidev->device != this_board->device_id)
+		if (pcidev->device != 0x80d9)
 			continue;
 		if (bus || slot) {
 			/* requested particular bus/slot */
@@ -1830,56 +1473,85 @@ static struct pci_dev *pci9118_find_pci(struct comedi_device *dev,
 	return NULL;
 }
 
-static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
-				 int master, int ext_mux, int softsshdelay,
-				 int hw_err_mask)
+static void pci9118_alloc_dma(struct comedi_device *dev)
+{
+	struct pci9118_private *devpriv = dev->private;
+	struct pci9118_dmabuf *dmabuf;
+	int order;
+	int i;
+
+	for (i = 0; i < 2; i++) {
+		dmabuf = &devpriv->dmabuf[i];
+		for (order = 2; order >= 0; order--) {
+			dmabuf->virt =
+			    dma_alloc_coherent(dev->hw_dev, PAGE_SIZE << order,
+					       &dmabuf->hw, GFP_KERNEL);
+			if (dmabuf->virt)
+				break;
+		}
+		if (!dmabuf->virt)
+			break;
+		dmabuf->size = PAGE_SIZE << order;
+
+		if (i == 0)
+			devpriv->master = 1;
+		if (i == 1)
+			devpriv->dma_doublebuf = 1;
+	}
+}
+
+static void pci9118_free_dma(struct comedi_device *dev)
 {
-	const struct boardtype *this_board = comedi_board(dev);
 	struct pci9118_private *devpriv = dev->private;
+	struct pci9118_dmabuf *dmabuf;
+	int i;
+
+	if (!devpriv)
+		return;
+
+	for (i = 0; i < 2; i++) {
+		dmabuf = &devpriv->dmabuf[i];
+		if (dmabuf->virt) {
+			dma_free_coherent(dev->hw_dev, dmabuf->size,
+					  dmabuf->virt, dmabuf->hw);
+		}
+	}
+}
+
+static int pci9118_common_attach(struct comedi_device *dev,
+				 int ext_mux, int softsshdelay)
+{
+	const struct pci9118_boardinfo *board = dev->board_ptr;
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct pci9118_private *devpriv;
 	struct comedi_subdevice *s;
-	int ret, pages, i;
+	int ret;
+	int i;
 	u16 u16w;
 
-	dev->board_name = this_board->name;
+	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
+	if (!devpriv)
+		return -ENOMEM;
+
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	if (master)
-		pci_set_master(pcidev);
+	pci_set_master(pcidev);
 
 	devpriv->iobase_a = pci_resource_start(pcidev, 0);
 	dev->iobase = pci_resource_start(pcidev, 2);
 
 	pci9118_reset(dev);
 
-	if (master) {		/* alloc DMA buffers */
-		devpriv->dma_doublebuf = 0;
-		for (i = 0; i < 2; i++) {
-			for (pages = 4; pages >= 0; pages--) {
-				devpriv->dmabuf_virt[i] =
-				    (unsigned short *)
-				    __get_free_pages(GFP_KERNEL, pages);
-				if (devpriv->dmabuf_virt[i])
-					break;
-			}
-			if (devpriv->dmabuf_virt[i]) {
-				devpriv->dmabuf_pages[i] = pages;
-				devpriv->dmabuf_size[i] = PAGE_SIZE * pages;
-				devpriv->dmabuf_hw[i] =
-				    virt_to_bus((void *)
-						devpriv->dmabuf_virt[i]);
-			}
-		}
-		if (!devpriv->dmabuf_virt[0]) {
-			dev_warn(dev->class_dev,
-				 "Can't allocate DMA buffer, DMA disabled!\n");
-			master = 0;
+	if (pcidev->irq) {
+		ret = request_irq(pcidev->irq, pci9118_interrupt, IRQF_SHARED,
+				  dev->board_name, dev);
+		if (ret == 0) {
+			dev->irq = pcidev->irq;
+
+			pci9118_alloc_dma(dev);
 		}
-		if (devpriv->dmabuf_virt[1])
-			devpriv->dma_doublebuf = 1;
 	}
-	devpriv->master = master;
 
 	if (ext_mux > 0) {
 		if (ext_mux > 256)
@@ -1887,7 +1559,7 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 		if (softsshdelay > 0)
 			if (ext_mux > 128)
 				ext_mux = 128;
-		devpriv->usemux = ext_mux;
+		devpriv->usemux = 1;
 	} else {
 		devpriv->usemux = 0;
 	}
@@ -1907,70 +1579,81 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 	pci_write_config_word(pcidev, PCI_COMMAND, u16w | 64);
 				/* Enable parity check for parity error */
 
-	if (!disable_irq && pcidev->irq) {
-		ret = request_irq(pcidev->irq, pci9118_interrupt, IRQF_SHARED,
-				  dev->board_name, dev);
-		if (ret == 0)
-			dev->irq = pcidev->irq;
-	}
-
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
 
+	/* Analog Input subdevice */
 	s = &dev->subdevices[0];
-	s->type = COMEDI_SUBD_AI;
-	s->subdev_flags = SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
-	if (devpriv->usemux)
-		s->n_chan = devpriv->usemux;
-	else
-		s->n_chan = this_board->n_aichan;
-
-	s->maxdata = this_board->ai_maxdata;
-	s->range_table = this_board->rangelist_ai;
-	s->insn_read = pci9118_insn_read_ai;
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_COMMON | SDF_GROUND | SDF_DIFF;
+	s->n_chan	= (devpriv->usemux) ? ext_mux : 16;
+	s->maxdata	= board->ai_is_16bit ? 0xffff : 0x0fff;
+	s->range_table	= board->is_hg ? &pci9118hg_ai_range
+				       : &pci9118_ai_range;
+	s->insn_read	= pci9118_ai_insn_read;
 	if (dev->irq) {
 		dev->read_subdev = s;
-		s->subdev_flags |= SDF_CMD_READ;
-		s->len_chanlist = this_board->n_aichanlist;
-		s->do_cmdtest = pci9118_ai_cmdtest;
-		s->do_cmd = pci9118_ai_cmd;
-		s->cancel = pci9118_ai_cancel;
-		s->munge = pci9118_ai_munge;
+		s->subdev_flags	|= SDF_CMD_READ;
+		s->len_chanlist	= PCI9118_CHANLEN;
+		s->do_cmdtest	= pci9118_ai_cmdtest;
+		s->do_cmd	= pci9118_ai_cmd;
+		s->cancel	= pci9118_ai_cancel;
+		s->munge	= pci9118_ai_munge;
+	}
+
+	if (s->maxdata == 0xffff) {
+		/*
+		 * 16-bit samples are from an ADS7805 A/D converter.
+		 * Minimum sampling rate is 10us.
+		 */
+		devpriv->ai_ns_min = 10000;
+	} else {
+		/*
+		 * 12-bit samples are from an ADS7800 A/D converter.
+		 * Minimum sampling rate is 3us.
+		 */
+		devpriv->ai_ns_min = 3000;
 	}
 
+	/* Analog Output subdevice */
 	s = &dev->subdevices[1];
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = this_board->n_aochan;
-	s->maxdata = this_board->ao_maxdata;
-	s->len_chanlist = this_board->n_aochan;
-	s->range_table = this_board->rangelist_ao;
-	s->insn_write = pci9118_insn_write_ao;
-	s->insn_read = pci9118_insn_read_ao;
+	s->type		= COMEDI_SUBD_AO;
+	s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
+	s->n_chan	= 2;
+	s->maxdata	= 0x0fff;
+	s->range_table	= &range_bipolar10;
+	s->insn_write	= pci9118_ao_insn_write;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
-	s = &dev->subdevices[2];
-	s->type = COMEDI_SUBD_DI;
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = 4;
-	s->maxdata = 1;
-	s->len_chanlist = 4;
-	s->range_table = &range_digital;
-	s->insn_bits = pci9118_insn_bits_di;
+	/* the analog outputs were reset to 0V, make the readback match */
+	for (i = 0; i < s->n_chan; i++)
+		s->readback[i] = 2047;
 
+	/* Digital Input subdevice */
+	s = &dev->subdevices[2];
+	s->type		= COMEDI_SUBD_DI;
+	s->subdev_flags	= SDF_READABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci9118_di_insn_bits;
+
+	/* Digital Output subdevice */
 	s = &dev->subdevices[3];
-	s->type = COMEDI_SUBD_DO;
-	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
-	s->n_chan = 4;
-	s->maxdata = 1;
-	s->len_chanlist = 4;
-	s->range_table = &range_digital;
-	s->insn_bits = pci9118_insn_bits_do;
-
-	devpriv->ai_maskharderr = 0x10a;
-					/* default measure crash condition */
-	if (hw_err_mask)		/* disable some requested */
-		devpriv->ai_maskharderr &= ~hw_err_mask;
+	s->type		= COMEDI_SUBD_DO;
+	s->subdev_flags	= SDF_WRITABLE;
+	s->n_chan	= 4;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= pci9118_do_insn_bits;
+
+	/* get the current state of the digital outputs */
+	s->state = inl(dev->iobase + PCI9118_DIO_REG) >> 4;
 
 	return 0;
 }
@@ -1978,74 +1661,50 @@ static int pci9118_common_attach(struct comedi_device *dev, int disable_irq,
 static int pci9118_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
-	struct pci9118_private *devpriv;
 	struct pci_dev *pcidev;
-	int ext_mux, disable_irq, master, softsshdelay, hw_err_mask;
+	int ext_mux, softsshdelay;
 
 	ext_mux = it->options[2];
-	master = ((it->options[3] & 1) == 0);
-	disable_irq = ((it->options[3] & 2) != 0);
 	softsshdelay = it->options[4];
-	hw_err_mask = it->options[5];
-
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
 
 	pcidev = pci9118_find_pci(dev, it);
 	if (!pcidev)
 		return -EIO;
 	comedi_set_hw_dev(dev, &pcidev->dev);
 
-	return pci9118_common_attach(dev, disable_irq, master, ext_mux,
-				     softsshdelay, hw_err_mask);
+	return pci9118_common_attach(dev, ext_mux, softsshdelay);
 }
 
 static int pci9118_auto_attach(struct comedi_device *dev,
-					 unsigned long context_unused)
+			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	struct pci9118_private *devpriv;
+	const struct pci9118_boardinfo *board = NULL;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
+	if (context < ARRAY_SIZE(pci9118_boards))
+		board = &pci9118_boards[context];
+	if (!board)
+		return -ENODEV;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
-	dev->board_ptr = pci9118_find_boardinfo(pcidev);
-	if (dev->board_ptr == NULL) {
-		dev_err(dev->class_dev,
-			"adl_pci9118: cannot determine board type for pci %s\n",
-			pci_name(pcidev));
-		return -EINVAL;
-	}
 	/*
 	 * Need to 'get' the PCI device to match the 'put' in pci9118_detach().
 	 * (The 'put' also matches the implicit 'get' by pci9118_find_pci().)
 	 */
 	pci_dev_get(pcidev);
-	/* Don't disable irq, use bus master, no external mux,
-	 * no sample-hold delay, no error mask. */
-	return pci9118_common_attach(dev, 0, 1, 0, 0, 0);
+	/* no external mux, no sample-hold delay */
+	return pci9118_common_attach(dev, 0, 0);
 }
 
 static void pci9118_detach(struct comedi_device *dev)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	struct pci9118_private *devpriv = dev->private;
 
-	if (devpriv) {
-		if (dev->iobase)
-			pci9118_reset(dev);
-		if (dev->irq)
-			free_irq(dev->irq, dev);
-		if (devpriv->dmabuf_virt[0])
-			free_pages((unsigned long)devpriv->dmabuf_virt[0],
-				   devpriv->dmabuf_pages[0]);
-		if (devpriv->dmabuf_virt[1])
-			free_pages((unsigned long)devpriv->dmabuf_virt[1],
-				   devpriv->dmabuf_pages[1]);
-	}
-	comedi_pci_disable(dev);
+	if (dev->iobase)
+		pci9118_reset(dev);
+	comedi_pci_detach(dev);
+	pci9118_free_dma(dev);
 	if (pcidev)
 		pci_dev_put(pcidev);
 }
@@ -2056,9 +1715,9 @@ static struct comedi_driver adl_pci9118_driver = {
 	.attach		= pci9118_attach,
 	.auto_attach	= pci9118_auto_attach,
 	.detach		= pci9118_detach,
-	.num_names	= ARRAY_SIZE(boardtypes),
-	.board_name	= &boardtypes[0].name,
-	.offset		= sizeof(struct boardtype),
+	.num_names	= ARRAY_SIZE(pci9118_boards),
+	.board_name	= &pci9118_boards[0].name,
+	.offset		= sizeof(struct pci9118_boardinfo),
 };
 
 static int adl_pci9118_pci_probe(struct pci_dev *dev,
@@ -2068,8 +1727,11 @@ static int adl_pci9118_pci_probe(struct pci_dev *dev,
 				      id->driver_data);
 }
 
+/* FIXME: All the supported board types have the same device ID! */
 static const struct pci_device_id adl_pci9118_pci_table[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMCC, 0x80d9) },
+	{ PCI_VDEVICE(AMCC, 0x80d9), BOARD_PCI9118DG },
+/*	{ PCI_VDEVICE(AMCC, 0x80d9), BOARD_PCI9118HG }, */
+/*	{ PCI_VDEVICE(AMCC, 0x80d9), BOARD_PCI9118HR }, */
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, adl_pci9118_pci_table);
diff --git a/drivers/staging/comedi/drivers/adv_pci1710.c b/drivers/staging/comedi/drivers/adv_pci1710.c
index e19ab958791b..5539bd294862 100644
--- a/drivers/staging/comedi/drivers/adv_pci1710.c
+++ b/drivers/staging/comedi/drivers/adv_pci1710.c
@@ -327,7 +327,7 @@ static int pci171x_ai_dropout(struct comedi_device *dev,
 			      unsigned int chan,
 			      unsigned int val)
 {
-	const struct boardtype *board = comedi_board(dev);
+	const struct boardtype *board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 
 	if (board->cardtype != TYPE_PCI1713) {
@@ -413,7 +413,7 @@ static void setup_channel_list(struct comedi_device *dev,
 			       unsigned int *chanlist, unsigned int n_chan,
 			       unsigned int seglen)
 {
-	const struct boardtype *this_board = comedi_board(dev);
+	const struct boardtype *this_board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 	unsigned int i, range, chanprog;
 
@@ -715,7 +715,7 @@ static int pci1720_insn_write_ao(struct comedi_device *dev,
 static int pci171x_ai_cancel(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
-	const struct boardtype *this_board = comedi_board(dev);
+	const struct boardtype *this_board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 
 	switch (this_board->cardtype) {
@@ -828,7 +828,7 @@ static int move_block_from_fifo(struct comedi_device *dev,
 static void pci1710_handle_fifo(struct comedi_device *dev,
 				struct comedi_subdevice *s)
 {
-	const struct boardtype *this_board = comedi_board(dev);
+	const struct boardtype *this_board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int m, samplesinbuf;
@@ -907,7 +907,7 @@ static irqreturn_t interrupt_service_pci1710(int irq, void *d)
 		return IRQ_HANDLED;
 	}
 
-	if (cmd->flags & TRIG_WAKE_EOS)
+	if (cmd->flags & CMDF_WAKE_EOS)
 		pci1710_handle_every_sample(dev, s);
 	else
 		pci1710_handle_fifo(dev, s);
@@ -932,7 +932,7 @@ static int pci171x_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	s->async->cur_chan = 0;
 
 	devpriv->CntrlReg &= Control_CNT0;
-	if ((cmd->flags & TRIG_WAKE_EOS) == 0)
+	if ((cmd->flags & CMDF_WAKE_EOS) == 0)
 		devpriv->CntrlReg |= Control_ONEFH;
 
 	devpriv->divisor1 = devpriv->next_divisor1;
@@ -968,7 +968,7 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_cmd *cmd)
 {
-	const struct boardtype *this_board = comedi_board(dev);
+	const struct boardtype *this_board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
@@ -1045,7 +1045,7 @@ static int pci171x_ai_cmdtest(struct comedi_device *dev,
 */
 static int pci171x_reset(struct comedi_device *dev)
 {
-	const struct boardtype *this_board = comedi_board(dev);
+	const struct boardtype *this_board = dev->board_ptr;
 	struct pci1710_private *devpriv = dev->private;
 
 	outw(0x30, dev->iobase + PCI171x_CNTCTRL);
@@ -1102,7 +1102,7 @@ static int pci1720_reset(struct comedi_device *dev)
 */
 static int pci1710_reset(struct comedi_device *dev)
 {
-	const struct boardtype *this_board = comedi_board(dev);
+	const struct boardtype *this_board = dev->board_ptr;
 
 	switch (this_board->cardtype) {
 	case TYPE_PCI1720:
@@ -1250,9 +1250,7 @@ static void pci1710_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		pci1710_reset(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver adv_pci1710_driver = {
diff --git a/drivers/staging/comedi/drivers/adv_pci1723.c b/drivers/staging/comedi/drivers/adv_pci1723.c
index 1881df459dae..1610e2b406f3 100644
--- a/drivers/staging/comedi/drivers/adv_pci1723.c
+++ b/drivers/staging/comedi/drivers/adv_pci1723.c
@@ -286,7 +286,7 @@ static void pci1723_detach(struct comedi_device *dev)
 {
 	if (dev->iobase)
 		pci1723_reset(dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver adv_pci1723_driver = {
diff --git a/drivers/staging/comedi/drivers/adv_pci1724.c b/drivers/staging/comedi/drivers/adv_pci1724.c
index bc3c34916768..2697758b1ed9 100644
--- a/drivers/staging/comedi/drivers/adv_pci1724.c
+++ b/drivers/staging/comedi/drivers/adv_pci1724.c
@@ -372,7 +372,7 @@ static struct comedi_driver adv_pci1724_driver = {
 	.driver_name = "adv_pci1724",
 	.module = THIS_MODULE,
 	.auto_attach = adv_pci1724_auto_attach,
-	.detach = comedi_pci_disable,
+	.detach = comedi_pci_detach,
 };
 
 static int adv_pci1724_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/adv_pci_dio.c b/drivers/staging/comedi/drivers/adv_pci_dio.c
index b8c7d9145a54..f2e2d7e163bf 100644
--- a/drivers/staging/comedi/drivers/adv_pci_dio.c
+++ b/drivers/staging/comedi/drivers/adv_pci_dio.c
@@ -66,7 +66,6 @@ enum hw_io_access {
 				 * subdevice) */
 
 #define SIZE_8254	   4	/* 8254 IO space length */
-#define SIZE_8255	   4	/* 8255 IO space length */
 
 #define PCIDIO_MAINREG	   2	/* main I/O region for all Advantech cards? */
 
@@ -394,7 +393,6 @@ static const struct dio_boardtype boardtypes[] = {
 };
 
 struct pci_dio_private {
-	char valid;		/*  card is usable */
 	char GlobalIrqEnabled;	/*  1= any IRQ source is enabled */
 	/*  PCI-1760 specific data */
 	unsigned char IDICntEnable;	/* counter's counting enable status */
@@ -819,7 +817,7 @@ static int pci1760_reset(struct comedi_device *dev)
 */
 static int pci_dio_reset(struct comedi_device *dev)
 {
-	const struct dio_boardtype *this_board = comedi_board(dev);
+	const struct dio_boardtype *this_board = dev->board_ptr;
 
 	switch (this_board->cardtype) {
 	case TYPE_PCI1730:
@@ -977,7 +975,7 @@ static int pci_dio_add_di(struct comedi_device *dev,
 			  struct comedi_subdevice *s,
 			  const struct diosubd_data *d)
 {
-	const struct dio_boardtype *this_board = comedi_board(dev);
+	const struct dio_boardtype *this_board = dev->board_ptr;
 
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_COMMON | d->specflags;
@@ -1007,7 +1005,7 @@ static int pci_dio_add_do(struct comedi_device *dev,
 			  struct comedi_subdevice *s,
 			  const struct diosubd_data *d)
 {
-	const struct dio_boardtype *this_board = comedi_board(dev);
+	const struct dio_boardtype *this_board = dev->board_ptr;
 
 	s->type = COMEDI_SUBD_DO;
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_COMMON;
@@ -1132,9 +1130,8 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 		for (j = 0; j < this_board->sdio[i].regs; j++) {
 			s = &dev->subdevices[subdev];
 			ret = subdev_8255_init(dev, s, NULL,
-					       dev->iobase +
 					       this_board->sdio[i].addr +
-					       SIZE_8255 * j);
+					       j * I8255_SIZE);
 			if (ret)
 				return ret;
 			subdev++;
@@ -1157,8 +1154,6 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 	if (this_board->cardtype == TYPE_PCI1760)
 		pci1760_attach(dev);
 
-	devpriv->valid = 1;
-
 	pci_dio_reset(dev);
 
 	return 0;
@@ -1166,13 +1161,9 @@ static int pci_dio_auto_attach(struct comedi_device *dev,
 
 static void pci_dio_detach(struct comedi_device *dev)
 {
-	struct pci_dio_private *devpriv = dev->private;
-
-	if (devpriv) {
-		if (devpriv->valid)
-			pci_dio_reset(dev);
-	}
-	comedi_pci_disable(dev);
+	if (dev->iobase)
+		pci_dio_reset(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver adv_pci_dio_driver = {
diff --git a/drivers/staging/comedi/drivers/aio_aio12_8.c b/drivers/staging/comedi/drivers/aio_aio12_8.c
index 324746b14931..538277a691b2 100644
--- a/drivers/staging/comedi/drivers/aio_aio12_8.c
+++ b/drivers/staging/comedi/drivers/aio_aio12_8.c
@@ -97,10 +97,6 @@ static const struct aio12_8_boardtype board_types[] = {
 	},
 };
 
-struct aio12_8_private {
-	unsigned int ao_readback[4];
-};
-
 static int aio_aio12_8_ai_eoc(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn,
@@ -149,28 +145,13 @@ static int aio_aio12_8_ai_read(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int aio_aio12_8_ao_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	struct aio12_8_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int val = devpriv->ao_readback[chan];
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = val;
-	return insn->n;
-}
-
-static int aio_aio12_8_ao_write(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+static int aio_aio12_8_ao_insn_write(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_insn *insn,
+				     unsigned int *data)
 {
-	struct aio12_8_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned long port = dev->iobase + AIO12_8_DAC_REG(chan);
-	unsigned int val = 0;
+	unsigned int val = s->readback[chan];
 	int i;
 
 	/* enable DACs */
@@ -178,10 +159,9 @@ static int aio_aio12_8_ao_write(struct comedi_device *dev,
 
 	for (i = 0; i < insn->n; i++) {
 		val = data[i];
-		outw(val, port);
+		outw(val, dev->iobase + AIO12_8_DAC_REG(chan));
 	}
-
-	devpriv->ao_readback[chan] = val;
+	s->readback[chan] = val;
 
 	return insn->n;
 }
@@ -198,8 +178,7 @@ static const struct comedi_lrange range_aio_aio12_8 = {
 static int aio_aio12_8_attach(struct comedi_device *dev,
 			      struct comedi_devconfig *it)
 {
-	const struct aio12_8_boardtype *board = comedi_board(dev);
-	struct aio12_8_private *devpriv;
+	const struct aio12_8_boardtype *board = dev->board_ptr;
 	struct comedi_subdevice *s;
 	int ret;
 
@@ -207,10 +186,6 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_alloc_subdevices(dev, 4);
 	if (ret)
 		return ret;
@@ -236,16 +211,19 @@ static int aio_aio12_8_attach(struct comedi_device *dev,
 		s->n_chan	= 4;
 		s->maxdata	= 0x0fff;
 		s->range_table	= &range_aio_aio12_8;
-		s->insn_read	= aio_aio12_8_ao_read;
-		s->insn_write	= aio_aio12_8_ao_write;
+		s->insn_write	= aio_aio12_8_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
 
 	s = &dev->subdevices[2];
 	/* 8255 Digital i/o subdevice */
-	ret = subdev_8255_init(dev, s, NULL,
-			       dev->iobase + AIO12_8_8255_BASE_REG);
+	ret = subdev_8255_init(dev, s, NULL, AIO12_8_8255_BASE_REG);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.c b/drivers/staging/comedi/drivers/amplc_dio200.c
index 17d2e20663cb..4fe118380218 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200.c
@@ -202,100 +202,69 @@
  */
 static const struct dio200_board dio200_isa_boards[] = {
 	{
-		.name = "pc212e",
-		.bustype = isa_bustype,
-		.mainsize = DIO200_IO_SIZE,
-		.layout = {
-			.n_subdevs = 6,
-			.sdtype = {sd_8255, sd_8254, sd_8254, sd_8254, sd_8254,
-				   sd_intr},
-			.sdinfo = {0x00, 0x08, 0x0C, 0x10, 0x14, 0x3F},
-			.has_int_sce = true,
-			.has_clk_gat_sce = true,
+		.name		= "pc212e",
+		.n_subdevs	= 6,
+		.sdtype		= {
+			sd_8255, sd_8254, sd_8254, sd_8254, sd_8254, sd_intr
 		},
-	},
-	{
-		.name = "pc214e",
-		.bustype = isa_bustype,
-		.mainsize = DIO200_IO_SIZE,
-		.layout = {
-			.n_subdevs = 4,
-			.sdtype = {sd_8255, sd_8255, sd_8254, sd_intr},
-			.sdinfo = {0x00, 0x08, 0x10, 0x01},
+		.sdinfo		= { 0x00, 0x08, 0x0c, 0x10, 0x14, 0x3f },
+		.has_int_sce	= true,
+		.has_clk_gat_sce = true,
+	}, {
+		.name		= "pc214e",
+		.n_subdevs	= 4,
+		.sdtype		= {
+			sd_8255, sd_8255, sd_8254, sd_intr
 		},
-	},
-	{
-		.name = "pc215e",
-		.bustype = isa_bustype,
-		.mainsize = DIO200_IO_SIZE,
-		.layout = {
-			.n_subdevs = 5,
-			.sdtype = {sd_8255, sd_8255, sd_8254, sd_8254, sd_intr},
-			.sdinfo = {0x00, 0x08, 0x10, 0x14, 0x3F},
-			.has_int_sce = true,
-			.has_clk_gat_sce = true,
+		.sdinfo		= { 0x00, 0x08, 0x10, 0x01 },
+	}, {
+		.name		= "pc215e",
+		.n_subdevs	= 5,
+		.sdtype		= {
+			sd_8255, sd_8255, sd_8254, sd_8254, sd_intr
 		},
-	},
-	{
-		.name = "pc218e",
-		.bustype = isa_bustype,
-		.mainsize = DIO200_IO_SIZE,
-		.layout = {
-			.n_subdevs = 7,
-			.sdtype = {sd_8254, sd_8254, sd_8255, sd_8254, sd_8254,
-				   sd_intr},
-			.sdinfo = {0x00, 0x04, 0x08, 0x0C, 0x10, 0x14, 0x3F},
-			.has_int_sce = true,
-			.has_clk_gat_sce = true,
+		.sdinfo		= { 0x00, 0x08, 0x10, 0x14, 0x3f },
+		.has_int_sce	= true,
+		.has_clk_gat_sce = true,
+	}, {
+		.name		= "pc218e",
+		.n_subdevs	= 7,
+		.sdtype		= {
+			sd_8254, sd_8254, sd_8255, sd_8254, sd_8254, sd_intr
 		},
-	},
-	{
-		.name = "pc272e",
-		.bustype = isa_bustype,
-		.mainsize = DIO200_IO_SIZE,
-		.layout = {
-			.n_subdevs = 4,
-			.sdtype = {sd_8255, sd_8255, sd_8255, sd_intr},
-			.sdinfo = {0x00, 0x08, 0x10, 0x3F},
-			.has_int_sce = true,
+		.sdinfo		= { 0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, 0x3f },
+		.has_int_sce	= true,
+		.has_clk_gat_sce = true,
+	}, {
+		.name		= "pc272e",
+		.n_subdevs	= 4,
+		.sdtype		= {
+			sd_8255, sd_8255, sd_8255, sd_intr
 		},
+		.sdinfo		= { 0x00, 0x08, 0x10, 0x3f },
+		.has_int_sce = true,
 	},
 };
 
 static int dio200_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv;
-	unsigned int irq;
 	int ret;
 
-	irq = it->options[1];
-
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
-	ret = comedi_request_region(dev, it->options[0], thisboard->mainsize);
+	ret = comedi_request_region(dev, it->options[0], 0x20);
 	if (ret)
 		return ret;
 
-	return amplc_dio200_common_attach(dev, irq, 0);
-}
-
-static void dio200_detach(struct comedi_device *dev)
-{
-	amplc_dio200_common_detach(dev);
-	comedi_legacy_detach(dev);
+	return amplc_dio200_common_attach(dev, it->options[1], 0);
 }
 
 static struct comedi_driver amplc_dio200_driver = {
-	.driver_name = "amplc_dio200",
-	.module = THIS_MODULE,
-	.attach = dio200_attach,
-	.detach = dio200_detach,
-	.board_name = &dio200_isa_boards[0].name,
-	.offset = sizeof(struct dio200_board),
-	.num_names = ARRAY_SIZE(dio200_isa_boards),
+	.driver_name	= "amplc_dio200",
+	.module		= THIS_MODULE,
+	.attach		= dio200_attach,
+	.detach		= comedi_legacy_detach,
+	.board_name	= &dio200_isa_boards[0].name,
+	.offset		= sizeof(struct dio200_board),
+	.num_names	= ARRAY_SIZE(dio200_isa_boards),
 };
 module_comedi_driver(amplc_dio200_driver);
 
diff --git a/drivers/staging/comedi/drivers/amplc_dio200.h b/drivers/staging/comedi/drivers/amplc_dio200.h
index e0afe2cee2d6..d6d6a265c461 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200.h
+++ b/drivers/staging/comedi/drivers/amplc_dio200.h
@@ -23,10 +23,6 @@
 #ifndef AMPLC_DIO200_H_INCLUDED
 #define AMPLC_DIO200_H_INCLUDED
 
-/* 200 series register area sizes */
-#define DIO200_IO_SIZE		0x20
-#define DIO200_PCIE_IO_SIZE	0x4000
-
 /*
  * Subdevice types.
  */
@@ -35,42 +31,20 @@ enum dio200_sdtype { sd_none, sd_intr, sd_8255, sd_8254, sd_timer };
 #define DIO200_MAX_SUBDEVS	8
 #define DIO200_MAX_ISNS		6
 
-/*
- * Board descriptions.
- */
-
-struct dio200_layout {
+struct dio200_board {
+	const char *name;
+	unsigned char mainbar;
 	unsigned short n_subdevs;	/* number of subdevices */
 	unsigned char sdtype[DIO200_MAX_SUBDEVS];	/* enum dio200_sdtype */
 	unsigned char sdinfo[DIO200_MAX_SUBDEVS];	/* depends on sdtype */
 	bool has_int_sce:1;		/* has interrupt enable/status reg */
 	bool has_clk_gat_sce:1;		/* has clock/gate selection registers */
-	bool has_enhancements:1;	/* has enhanced features */
-};
-
-enum dio200_bustype { isa_bustype, pci_bustype };
-
-struct dio200_board {
-	const char *name;
-	struct dio200_layout layout;
-	enum dio200_bustype bustype;
-	unsigned char mainbar;
-	unsigned char mainshift;
-	unsigned int mainsize;
-};
-
-/*
- * Comedi device private data.
- */
-struct dio200_private {
-	int intr_sd;
+	bool is_pcie:1;			/* has enhanced features */
 };
 
 int amplc_dio200_common_attach(struct comedi_device *dev, unsigned int irq,
 			       unsigned long req_irq_flags);
 
-void amplc_dio200_common_detach(struct comedi_device *dev);
-
 /* Used by initialization of PCIe boards. */
 void amplc_dio200_set_enhance(struct comedi_device *dev, unsigned char val);
 
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_common.c b/drivers/staging/comedi/drivers/amplc_dio200_common.c
index f0d709e0dafc..2c1bfb09601d 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_common.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_common.c
@@ -27,15 +27,7 @@
 #include "amplc_dio200.h"
 #include "comedi_fc.h"
 #include "8253.h"
-
-/* 8255 control register bits */
-#define CR_C_LO_IO	0x01
-#define CR_B_IO		0x02
-#define CR_B_MODE	0x04
-#define CR_C_HI_IO	0x08
-#define CR_A_IO		0x10
-#define CR_A_MODE(a)	((a)<<5)
-#define CR_CW		0x80
+#include "8255.h"		/* only for register defines */
 
 /* 200 series registers */
 #define DIO200_IO_SIZE		0x20
@@ -132,42 +124,26 @@ struct dio200_subdev_intr {
 	bool active:1;
 };
 
-static inline const struct dio200_layout *
-dio200_board_layout(const struct dio200_board *board)
-{
-	return &board->layout;
-}
-
-static inline const struct dio200_layout *
-dio200_dev_layout(struct comedi_device *dev)
-{
-	return dio200_board_layout(comedi_board(dev));
-}
-
-/*
- * Read 8-bit register.
- */
 static unsigned char dio200_read8(struct comedi_device *dev,
 				  unsigned int offset)
 {
-	const struct dio200_board *thisboard = comedi_board(dev);
+	const struct dio200_board *board = dev->board_ptr;
 
-	offset <<= thisboard->mainshift;
+	if (board->is_pcie)
+		offset <<= 3;
 
 	if (dev->mmio)
 		return readb(dev->mmio + offset);
 	return inb(dev->iobase + offset);
 }
 
-/*
- * Write 8-bit register.
- */
-static void dio200_write8(struct comedi_device *dev, unsigned int offset,
-			  unsigned char val)
+static void dio200_write8(struct comedi_device *dev,
+			  unsigned int offset, unsigned char val)
 {
-	const struct dio200_board *thisboard = comedi_board(dev);
+	const struct dio200_board *board = dev->board_ptr;
 
-	offset <<= thisboard->mainshift;
+	if (board->is_pcie)
+		offset <<= 3;
 
 	if (dev->mmio)
 		writeb(val, dev->mmio + offset);
@@ -175,30 +151,26 @@ static void dio200_write8(struct comedi_device *dev, unsigned int offset,
 		outb(val, dev->iobase + offset);
 }
 
-/*
- * Read 32-bit register.
- */
 static unsigned int dio200_read32(struct comedi_device *dev,
 				  unsigned int offset)
 {
-	const struct dio200_board *thisboard = comedi_board(dev);
+	const struct dio200_board *board = dev->board_ptr;
 
-	offset <<= thisboard->mainshift;
+	if (board->is_pcie)
+		offset <<= 3;
 
 	if (dev->mmio)
 		return readl(dev->mmio + offset);
 	return inl(dev->iobase + offset);
 }
 
-/*
- * Write 32-bit register.
- */
-static void dio200_write32(struct comedi_device *dev, unsigned int offset,
-			   unsigned int val)
+static void dio200_write32(struct comedi_device *dev,
+			   unsigned int offset, unsigned int val)
 {
-	const struct dio200_board *thisboard = comedi_board(dev);
+	const struct dio200_board *board = dev->board_ptr;
 
-	offset <<= thisboard->mainshift;
+	if (board->is_pcie)
+		offset <<= 3;
 
 	if (dev->mmio)
 		writel(val, dev->mmio + offset);
@@ -206,18 +178,15 @@ static void dio200_write32(struct comedi_device *dev, unsigned int offset,
 		outl(val, dev->iobase + offset);
 }
 
-/*
- * 'insn_bits' function for an 'INTERRUPT' subdevice.
- */
-static int
-dio200_subdev_intr_insn_bits(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
+static int dio200_subdev_intr_insn_bits(struct comedi_device *dev,
+					struct comedi_subdevice *s,
+					struct comedi_insn *insn,
+					unsigned int *data)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_intr *subpriv = s->private;
 
-	if (layout->has_int_sce) {
+	if (board->has_int_sce) {
 		/* Just read the interrupt status register.  */
 		data[1] = dio200_read8(dev, subpriv->ofs) & subpriv->valid_isns;
 	} else {
@@ -228,54 +197,38 @@ dio200_subdev_intr_insn_bits(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
- * Called to stop acquisition for an 'INTERRUPT' subdevice.
- */
 static void dio200_stop_intr(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_intr *subpriv = s->private;
 
 	subpriv->active = false;
 	subpriv->enabled_isns = 0;
-	if (layout->has_int_sce)
+	if (board->has_int_sce)
 		dio200_write8(dev, subpriv->ofs, 0);
 }
 
-/*
- * Called to start acquisition for an 'INTERRUPT' subdevice.
- */
-static int dio200_start_intr(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
+static void dio200_start_intr(struct comedi_device *dev,
+			      struct comedi_subdevice *s)
 {
-	unsigned int n;
-	unsigned isn_bits;
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_intr *subpriv = s->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
-	int retval = 0;
+	unsigned int n;
+	unsigned isn_bits;
 
-	if (cmd->stop_src == TRIG_COUNT && subpriv->stopcount == 0) {
-		/* An empty acquisition! */
-		s->async->events |= COMEDI_CB_EOA;
-		subpriv->active = false;
-		retval = 1;
-	} else {
-		/* Determine interrupt sources to enable. */
-		isn_bits = 0;
-		if (cmd->chanlist) {
-			for (n = 0; n < cmd->chanlist_len; n++)
-				isn_bits |= (1U << CR_CHAN(cmd->chanlist[n]));
-		}
-		isn_bits &= subpriv->valid_isns;
-		/* Enable interrupt sources. */
-		subpriv->enabled_isns = isn_bits;
-		if (layout->has_int_sce)
-			dio200_write8(dev, subpriv->ofs, isn_bits);
+	/* Determine interrupt sources to enable. */
+	isn_bits = 0;
+	if (cmd->chanlist) {
+		for (n = 0; n < cmd->chanlist_len; n++)
+			isn_bits |= (1U << CR_CHAN(cmd->chanlist[n]));
 	}
-
-	return retval;
+	isn_bits &= subpriv->valid_isns;
+	/* Enable interrupt sources. */
+	subpriv->enabled_isns = isn_bits;
+	if (board->has_int_sce)
+		dio200_write8(dev, subpriv->ofs, isn_bits);
 }
 
 static int dio200_inttrig_start_intr(struct comedi_device *dev,
@@ -285,7 +238,6 @@ static int dio200_inttrig_start_intr(struct comedi_device *dev,
 	struct dio200_subdev_intr *subpriv = s->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
-	int event = 0;
 
 	if (trig_num != cmd->start_arg)
 		return -EINVAL;
@@ -293,13 +245,10 @@ static int dio200_inttrig_start_intr(struct comedi_device *dev,
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 	s->async->inttrig = NULL;
 	if (subpriv->active)
-		event = dio200_start_intr(dev, s);
+		dio200_start_intr(dev, s);
 
 	spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
-	if (event)
-		comedi_event(dev, s);
-
 	return 1;
 }
 
@@ -340,14 +289,10 @@ static void dio200_read_scan_intr(struct comedi_device *dev,
 	}
 }
 
-/*
- * This is called from the interrupt service routine to handle a read
- * scan on an 'INTERRUPT' subdevice.
- */
 static int dio200_handle_read_intr(struct comedi_device *dev,
 				   struct comedi_subdevice *s)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_intr *subpriv = s->private;
 	unsigned triggered;
 	unsigned intstat;
@@ -359,7 +304,7 @@ static int dio200_handle_read_intr(struct comedi_device *dev,
 
 	spin_lock_irqsave(&subpriv->spinlock, flags);
 	oldevents = s->async->events;
-	if (layout->has_int_sce) {
+	if (board->has_int_sce) {
 		/*
 		 * Collect interrupt sources that have triggered and disable
 		 * them temporarily.  Loop around until no extra interrupt
@@ -393,7 +338,7 @@ static int dio200_handle_read_intr(struct comedi_device *dev,
 		 * Reenable them NOW to minimize the time they are disabled.
 		 */
 		cur_enabled = subpriv->enabled_isns;
-		if (layout->has_int_sce)
+		if (board->has_int_sce)
 			dio200_write8(dev, subpriv->ofs, cur_enabled);
 
 		if (subpriv->active) {
@@ -417,9 +362,6 @@ static int dio200_handle_read_intr(struct comedi_device *dev,
 	return (triggered != 0);
 }
 
-/*
- * 'cancel' function for an 'INTERRUPT' subdevice.
- */
 static int dio200_subdev_intr_cancel(struct comedi_device *dev,
 				     struct comedi_subdevice *s)
 {
@@ -435,12 +377,9 @@ static int dio200_subdev_intr_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-/*
- * 'do_cmdtest' function for an 'INTERRUPT' subdevice.
- */
-static int
-dio200_subdev_intr_cmdtest(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
+static int dio200_subdev_intr_cmdtest(struct comedi_device *dev,
+				      struct comedi_subdevice *s,
+				      struct comedi_cmd *cmd)
 {
 	int err = 0;
 
@@ -472,16 +411,10 @@ dio200_subdev_intr_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		/* any count allowed */
-		break;
-	case TRIG_NONE:
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-		break;
-	default:
-		break;
-	}
 
 	if (err)
 		return 3;
@@ -493,47 +426,34 @@ dio200_subdev_intr_cmdtest(struct comedi_device *dev,
 	return 0;
 }
 
-/*
- * 'do_cmd' function for an 'INTERRUPT' subdevice.
- */
 static int dio200_subdev_intr_cmd(struct comedi_device *dev,
 				  struct comedi_subdevice *s)
 {
 	struct comedi_cmd *cmd = &s->async->cmd;
 	struct dio200_subdev_intr *subpriv = s->private;
 	unsigned long flags;
-	int event = 0;
 
 	spin_lock_irqsave(&subpriv->spinlock, flags);
-	subpriv->active = true;
 
-	/* Set up end of acquisition. */
-	if (cmd->stop_src == TRIG_COUNT)
-		subpriv->stopcount = cmd->stop_arg;
-	else	/* TRIG_NONE */
-		subpriv->stopcount = 0;
+	subpriv->active = true;
+	subpriv->stopcount = cmd->stop_arg;
 
 	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = dio200_inttrig_start_intr;
 	else	/* TRIG_NOW */
-		event = dio200_start_intr(dev, s);
+		dio200_start_intr(dev, s);
 
 	spin_unlock_irqrestore(&subpriv->spinlock, flags);
 
-	if (event)
-		comedi_event(dev, s);
-
 	return 0;
 }
 
-/*
- * This function initializes an 'INTERRUPT' subdevice.
- */
-static int
-dio200_subdev_intr_init(struct comedi_device *dev, struct comedi_subdevice *s,
-			unsigned int offset, unsigned valid_isns)
+static int dio200_subdev_intr_init(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   unsigned int offset,
+				   unsigned valid_isns)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_intr *subpriv;
 
 	subpriv = comedi_alloc_spriv(s, sizeof(*subpriv));
@@ -544,13 +464,13 @@ dio200_subdev_intr_init(struct comedi_device *dev, struct comedi_subdevice *s,
 	subpriv->valid_isns = valid_isns;
 	spin_lock_init(&subpriv->spinlock);
 
-	if (layout->has_int_sce)
+	if (board->has_int_sce)
 		/* Disable interrupt sources. */
 		dio200_write8(dev, subpriv->ofs, 0);
 
 	s->type = COMEDI_SUBD_DI;
 	s->subdev_flags = SDF_READABLE | SDF_CMD_READ;
-	if (layout->has_int_sce) {
+	if (board->has_int_sce) {
 		s->n_chan = DIO200_MAX_ISNS;
 		s->len_chanlist = DIO200_MAX_ISNS;
 	} else {
@@ -568,35 +488,23 @@ dio200_subdev_intr_init(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 0;
 }
 
-/*
- * Interrupt service routine.
- */
 static irqreturn_t dio200_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = d;
-	struct dio200_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
+	struct comedi_subdevice *s = dev->read_subdev;
 	int handled;
 
 	if (!dev->attached)
 		return IRQ_NONE;
 
-	if (devpriv->intr_sd >= 0) {
-		s = &dev->subdevices[devpriv->intr_sd];
-		handled = dio200_handle_read_intr(dev, s);
-	} else {
-		handled = 0;
-	}
+	handled = dio200_handle_read_intr(dev, s);
 
 	return IRQ_RETVAL(handled);
 }
 
-/*
- * Read an '8254' counter subdevice channel.
- */
-static unsigned int
-dio200_subdev_8254_read_chan(struct comedi_device *dev,
-			     struct comedi_subdevice *s, unsigned int chan)
+static unsigned int dio200_subdev_8254_read_chan(struct comedi_device *dev,
+						 struct comedi_subdevice *s,
+						 unsigned int chan)
 {
 	struct dio200_subdev_8254 *subpriv = s->private;
 	unsigned int val;
@@ -610,13 +518,10 @@ dio200_subdev_8254_read_chan(struct comedi_device *dev,
 	return val;
 }
 
-/*
- * Write an '8254' subdevice channel.
- */
-static void
-dio200_subdev_8254_write_chan(struct comedi_device *dev,
-			      struct comedi_subdevice *s, unsigned int chan,
-			      unsigned int count)
+static void dio200_subdev_8254_write_chan(struct comedi_device *dev,
+					  struct comedi_subdevice *s,
+					  unsigned int chan,
+					  unsigned int count)
 {
 	struct dio200_subdev_8254 *subpriv = s->private;
 
@@ -625,13 +530,10 @@ dio200_subdev_8254_write_chan(struct comedi_device *dev,
 	dio200_write8(dev, subpriv->ofs + chan, (count >> 8) & 0xff);
 }
 
-/*
- * Set mode of an '8254' subdevice channel.
- */
-static void
-dio200_subdev_8254_set_mode(struct comedi_device *dev,
-			    struct comedi_subdevice *s, unsigned int chan,
-			    unsigned int mode)
+static void dio200_subdev_8254_set_mode(struct comedi_device *dev,
+					struct comedi_subdevice *s,
+					unsigned int chan,
+					unsigned int mode)
 {
 	struct dio200_subdev_8254 *subpriv = s->private;
 	unsigned int byte;
@@ -642,12 +544,9 @@ dio200_subdev_8254_set_mode(struct comedi_device *dev,
 	dio200_write8(dev, subpriv->ofs + i8254_control_reg, byte);
 }
 
-/*
- * Read status byte of an '8254' counter subdevice channel.
- */
-static unsigned int
-dio200_subdev_8254_status(struct comedi_device *dev,
-			  struct comedi_subdevice *s, unsigned int chan)
+static unsigned int dio200_subdev_8254_status(struct comedi_device *dev,
+					      struct comedi_subdevice *s,
+					      unsigned int chan)
 {
 	struct dio200_subdev_8254 *subpriv = s->private;
 
@@ -658,12 +557,10 @@ dio200_subdev_8254_status(struct comedi_device *dev,
 	return dio200_read8(dev, subpriv->ofs + chan);
 }
 
-/*
- * Handle 'insn_read' for an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_read(struct comedi_device *dev, struct comedi_subdevice *s,
-			struct comedi_insn *insn, unsigned int *data)
+static int dio200_subdev_8254_read(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
 	struct dio200_subdev_8254 *subpriv = s->private;
 	int chan = CR_CHAN(insn->chanspec);
@@ -678,12 +575,10 @@ dio200_subdev_8254_read(struct comedi_device *dev, struct comedi_subdevice *s,
 	return insn->n;
 }
 
-/*
- * Handle 'insn_write' for an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_write(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
+static int dio200_subdev_8254_write(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
 {
 	struct dio200_subdev_8254 *subpriv = s->private;
 	int chan = CR_CHAN(insn->chanspec);
@@ -698,24 +593,20 @@ dio200_subdev_8254_write(struct comedi_device *dev, struct comedi_subdevice *s,
 	return insn->n;
 }
 
-/*
- * Set gate source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_set_gate_src(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				unsigned int counter_number,
-				unsigned int gate_src)
+static int dio200_subdev_8254_set_gate_src(struct comedi_device *dev,
+					   struct comedi_subdevice *s,
+					   unsigned int counter_number,
+					   unsigned int gate_src)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_8254 *subpriv = s->private;
 	unsigned char byte;
 
-	if (!layout->has_clk_gat_sce)
+	if (!board->has_clk_gat_sce)
 		return -1;
 	if (counter_number > 2)
 		return -1;
-	if (gate_src > (layout->has_enhancements ? 31 : 7))
+	if (gate_src > (board->is_pcie ? 31 : 7))
 		return -1;
 
 	subpriv->gate_src[counter_number] = gate_src;
@@ -725,18 +616,14 @@ dio200_subdev_8254_set_gate_src(struct comedi_device *dev,
 	return 0;
 }
 
-/*
- * Get gate source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_get_gate_src(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				unsigned int counter_number)
+static int dio200_subdev_8254_get_gate_src(struct comedi_device *dev,
+					   struct comedi_subdevice *s,
+					   unsigned int counter_number)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_8254 *subpriv = s->private;
 
-	if (!layout->has_clk_gat_sce)
+	if (!board->has_clk_gat_sce)
 		return -1;
 	if (counter_number > 2)
 		return -1;
@@ -744,24 +631,20 @@ dio200_subdev_8254_get_gate_src(struct comedi_device *dev,
 	return subpriv->gate_src[counter_number];
 }
 
-/*
- * Set clock source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_set_clock_src(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 unsigned int counter_number,
-				 unsigned int clock_src)
+static int dio200_subdev_8254_set_clock_src(struct comedi_device *dev,
+					    struct comedi_subdevice *s,
+					    unsigned int counter_number,
+					    unsigned int clock_src)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_8254 *subpriv = s->private;
 	unsigned char byte;
 
-	if (!layout->has_clk_gat_sce)
+	if (!board->has_clk_gat_sce)
 		return -1;
 	if (counter_number > 2)
 		return -1;
-	if (clock_src > (layout->has_enhancements ? 31 : 7))
+	if (clock_src > (board->is_pcie ? 31 : 7))
 		return -1;
 
 	subpriv->clock_src[counter_number] = clock_src;
@@ -771,20 +654,16 @@ dio200_subdev_8254_set_clock_src(struct comedi_device *dev,
 	return 0;
 }
 
-/*
- * Get clock source for an '8254' counter subdevice channel.
- */
-static int
-dio200_subdev_8254_get_clock_src(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 unsigned int counter_number,
-				 unsigned int *period_ns)
+static int dio200_subdev_8254_get_clock_src(struct comedi_device *dev,
+					    struct comedi_subdevice *s,
+					    unsigned int counter_number,
+					    unsigned int *period_ns)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_8254 *subpriv = s->private;
 	unsigned clock_src;
 
-	if (!layout->has_clk_gat_sce)
+	if (!board->has_clk_gat_sce)
 		return -1;
 	if (counter_number > 2)
 		return -1;
@@ -794,12 +673,10 @@ dio200_subdev_8254_get_clock_src(struct comedi_device *dev,
 	return clock_src;
 }
 
-/*
- * Handle 'insn_config' for an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+static int dio200_subdev_8254_config(struct comedi_device *dev,
+				     struct comedi_subdevice *s,
+				     struct comedi_insn *insn,
+				     unsigned int *data)
 {
 	struct dio200_subdev_8254 *subpriv = s->private;
 	int ret = 0;
@@ -851,14 +728,11 @@ dio200_subdev_8254_config(struct comedi_device *dev, struct comedi_subdevice *s,
 	return ret < 0 ? ret : insn->n;
 }
 
-/*
- * This function initializes an '8254' counter subdevice.
- */
-static int
-dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
-			unsigned int offset)
+static int dio200_subdev_8254_init(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   unsigned int offset)
 {
-	const struct dio200_layout *layout = dio200_dev_layout(dev);
+	const struct dio200_board *board = dev->board_ptr;
 	struct dio200_subdev_8254 *subpriv;
 	unsigned int chan;
 
@@ -876,7 +750,7 @@ dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	spin_lock_init(&subpriv->spinlock);
 	subpriv->ofs = offset;
-	if (layout->has_clk_gat_sce) {
+	if (board->has_clk_gat_sce) {
 		/* Derive CLK_SCE and GAT_SCE register offsets from
 		 * 8254 offset. */
 		subpriv->clk_sce_ofs = DIO200_XCLK_SCE + (offset >> 3);
@@ -888,7 +762,7 @@ dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
 	for (chan = 0; chan < 3; chan++) {
 		dio200_subdev_8254_set_mode(dev, s, chan,
 					    I8254_MODE0 | I8254_BINARY);
-		if (layout->has_clk_gat_sce) {
+		if (board->has_clk_gat_sce) {
 			/* Gate source 0 is VCC (logic 1). */
 			dio200_subdev_8254_set_gate_src(dev, s, chan, 0);
 			/* Clock source 0 is the dedicated clock input. */
@@ -899,26 +773,23 @@ dio200_subdev_8254_init(struct comedi_device *dev, struct comedi_subdevice *s,
 	return 0;
 }
 
-/*
- * This function sets I/O directions for an '8255' DIO subdevice.
- */
 static void dio200_subdev_8255_set_dir(struct comedi_device *dev,
 				       struct comedi_subdevice *s)
 {
 	struct dio200_subdev_8255 *subpriv = s->private;
 	int config;
 
-	config = CR_CW;
+	config = I8255_CTRL_CW;
 	/* 1 in io_bits indicates output, 1 in config indicates input */
 	if (!(s->io_bits & 0x0000ff))
-		config |= CR_A_IO;
+		config |= I8255_CTRL_A_IO;
 	if (!(s->io_bits & 0x00ff00))
-		config |= CR_B_IO;
+		config |= I8255_CTRL_B_IO;
 	if (!(s->io_bits & 0x0f0000))
-		config |= CR_C_LO_IO;
+		config |= I8255_CTRL_C_LO_IO;
 	if (!(s->io_bits & 0xf00000))
-		config |= CR_C_HI_IO;
-	dio200_write8(dev, subpriv->ofs + 3, config);
+		config |= I8255_CTRL_C_HI_IO;
+	dio200_write8(dev, subpriv->ofs + I8255_CTRL_REG, config);
 }
 
 static int dio200_subdev_8255_bits(struct comedi_device *dev,
@@ -933,27 +804,25 @@ static int dio200_subdev_8255_bits(struct comedi_device *dev,
 	mask = comedi_dio_update_state(s, data);
 	if (mask) {
 		if (mask & 0xff)
-			dio200_write8(dev, subpriv->ofs, s->state & 0xff);
+			dio200_write8(dev, subpriv->ofs + I8255_DATA_A_REG,
+				      s->state & 0xff);
 		if (mask & 0xff00)
-			dio200_write8(dev, subpriv->ofs + 1,
+			dio200_write8(dev, subpriv->ofs + I8255_DATA_B_REG,
 				      (s->state >> 8) & 0xff);
 		if (mask & 0xff0000)
-			dio200_write8(dev, subpriv->ofs + 2,
+			dio200_write8(dev, subpriv->ofs + I8255_DATA_C_REG,
 				      (s->state >> 16) & 0xff);
 	}
 
-	val = dio200_read8(dev, subpriv->ofs);
-	val |= dio200_read8(dev, subpriv->ofs + 1) << 8;
-	val |= dio200_read8(dev, subpriv->ofs + 2) << 16;
+	val = dio200_read8(dev, subpriv->ofs + I8255_DATA_A_REG);
+	val |= dio200_read8(dev, subpriv->ofs + I8255_DATA_B_REG) << 8;
+	val |= dio200_read8(dev, subpriv->ofs + I8255_DATA_C_REG) << 16;
 
 	data[1] = val;
 
 	return insn->n;
 }
 
-/*
- * Handle 'insn_config' for an '8255' DIO subdevice.
- */
 static int dio200_subdev_8255_config(struct comedi_device *dev,
 				     struct comedi_subdevice *s,
 				     struct comedi_insn *insn,
@@ -981,11 +850,6 @@ static int dio200_subdev_8255_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-/*
- * This function initializes an '8255' DIO subdevice.
- *
- * offset is the offset to the 8255 chip.
- */
 static int dio200_subdev_8255_init(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   unsigned int offset)
@@ -1009,9 +873,6 @@ static int dio200_subdev_8255_init(struct comedi_device *dev,
 	return 0;
 }
 
-/*
- * Handle 'insn_read' for a timer subdevice.
- */
 static int dio200_subdev_timer_read(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_insn *insn,
@@ -1024,9 +885,6 @@ static int dio200_subdev_timer_read(struct comedi_device *dev,
 	return n;
 }
 
-/*
- * Reset timer subdevice.
- */
 static void dio200_subdev_timer_reset(struct comedi_device *dev,
 				      struct comedi_subdevice *s)
 {
@@ -1037,9 +895,6 @@ static void dio200_subdev_timer_reset(struct comedi_device *dev,
 	dio200_write32(dev, DIO200_TS_CONFIG, clock);
 }
 
-/*
- * Get timer subdevice clock source and period.
- */
 static void dio200_subdev_timer_get_clock_src(struct comedi_device *dev,
 					      struct comedi_subdevice *s,
 					      unsigned int *src,
@@ -1053,9 +908,6 @@ static void dio200_subdev_timer_get_clock_src(struct comedi_device *dev,
 		  ts_clock_period[clk] : 0;
 }
 
-/*
- * Set timer subdevice clock source.
- */
 static int dio200_subdev_timer_set_clock_src(struct comedi_device *dev,
 					     struct comedi_subdevice *s,
 					     unsigned int src)
@@ -1066,9 +918,6 @@ static int dio200_subdev_timer_set_clock_src(struct comedi_device *dev,
 	return 0;
 }
 
-/*
- * Handle 'insn_config' for a timer subdevice.
- */
 static int dio200_subdev_timer_config(struct comedi_device *dev,
 				      struct comedi_subdevice *s,
 				      struct comedi_insn *insn,
@@ -1095,23 +944,6 @@ static int dio200_subdev_timer_config(struct comedi_device *dev,
 	return ret < 0 ? ret : insn->n;
 }
 
-/*
- * This function initializes a timer subdevice.
- *
- * Uses the timestamp timer registers.  There is only one timestamp timer.
- */
-static int dio200_subdev_timer_init(struct comedi_device *dev,
-				    struct comedi_subdevice *s)
-{
-	s->type = COMEDI_SUBD_TIMER;
-	s->subdev_flags = SDF_READABLE | SDF_LSAMPL;
-	s->n_chan = 1;
-	s->maxdata = 0xFFFFFFFF;
-	s->insn_read = dio200_subdev_timer_read;
-	s->insn_config = dio200_subdev_timer_config;
-	return 0;
-}
-
 void amplc_dio200_set_enhance(struct comedi_device *dev, unsigned char val)
 {
 	dio200_write8(dev, DIO200_ENHANCE, val);
@@ -1121,65 +953,60 @@ EXPORT_SYMBOL_GPL(amplc_dio200_set_enhance);
 int amplc_dio200_common_attach(struct comedi_device *dev, unsigned int irq,
 			       unsigned long req_irq_flags)
 {
-	const struct dio200_board *thisboard = comedi_board(dev);
-	struct dio200_private *devpriv = dev->private;
-	const struct dio200_layout *layout = dio200_board_layout(thisboard);
+	const struct dio200_board *board = dev->board_ptr;
 	struct comedi_subdevice *s;
-	int sdx;
 	unsigned int n;
 	int ret;
 
-	devpriv->intr_sd = -1;
-
-	ret = comedi_alloc_subdevices(dev, layout->n_subdevs);
+	ret = comedi_alloc_subdevices(dev, board->n_subdevs);
 	if (ret)
 		return ret;
 
 	for (n = 0; n < dev->n_subdevices; n++) {
 		s = &dev->subdevices[n];
-		switch (layout->sdtype[n]) {
+		switch (board->sdtype[n]) {
 		case sd_8254:
 			/* counter subdevice (8254) */
 			ret = dio200_subdev_8254_init(dev, s,
-						      layout->sdinfo[n]);
+						      board->sdinfo[n]);
 			if (ret < 0)
 				return ret;
 			break;
 		case sd_8255:
 			/* digital i/o subdevice (8255) */
 			ret = dio200_subdev_8255_init(dev, s,
-						      layout->sdinfo[n]);
+						      board->sdinfo[n]);
 			if (ret < 0)
 				return ret;
 			break;
 		case sd_intr:
 			/* 'INTERRUPT' subdevice */
-			if (irq) {
+			if (irq && !dev->read_subdev) {
 				ret = dio200_subdev_intr_init(dev, s,
 							      DIO200_INT_SCE,
-							      layout->sdinfo[n]
-							     );
+							      board->sdinfo[n]);
 				if (ret < 0)
 					return ret;
-				devpriv->intr_sd = n;
+				dev->read_subdev = s;
 			} else {
 				s->type = COMEDI_SUBD_UNUSED;
 			}
 			break;
 		case sd_timer:
-			ret = dio200_subdev_timer_init(dev, s);
-			if (ret < 0)
-				return ret;
+			s->type		= COMEDI_SUBD_TIMER;
+			s->subdev_flags	= SDF_READABLE | SDF_LSAMPL;
+			s->n_chan	= 1;
+			s->maxdata	= 0xffffffff;
+			s->insn_read	= dio200_subdev_timer_read;
+			s->insn_config	= dio200_subdev_timer_config;
 			break;
 		default:
 			s->type = COMEDI_SUBD_UNUSED;
 			break;
 		}
 	}
-	sdx = devpriv->intr_sd;
-	if (sdx >= 0 && sdx < dev->n_subdevices)
-		dev->read_subdev = &dev->subdevices[sdx];
-	if (irq) {
+
+	if (irq && dev->read_subdev) {
 		if (request_irq(irq, dio200_interrupt, req_irq_flags,
 				dev->board_name, dev) >= 0) {
 			dev->irq = irq;
@@ -1193,15 +1020,6 @@ int amplc_dio200_common_attach(struct comedi_device *dev, unsigned int irq,
 }
 EXPORT_SYMBOL_GPL(amplc_dio200_common_attach);
 
-void amplc_dio200_common_detach(struct comedi_device *dev)
-{
-	if (dev->irq) {
-		free_irq(dev->irq, dev);
-		dev->irq = 0;
-	}
-}
-EXPORT_SYMBOL_GPL(amplc_dio200_common_detach);
-
 static int __init amplc_dio200_common_init(void)
 {
 	return 0;
diff --git a/drivers/staging/comedi/drivers/amplc_dio200_pci.c b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
index fbf05687347f..b83d1f5a8fb9 100644
--- a/drivers/staging/comedi/drivers/amplc_dio200_pci.c
+++ b/drivers/staging/comedi/drivers/amplc_dio200_pci.c
@@ -242,80 +242,70 @@ enum dio200_pci_model {
 
 static const struct dio200_board dio200_pci_boards[] = {
 	[pci215_model] = {
-		.name = "pci215",
-		.bustype = pci_bustype,
-		.mainbar = 2,
-		.mainsize = DIO200_IO_SIZE,
-		.layout = {
-			.n_subdevs = 5,
-			.sdtype = {sd_8255, sd_8255, sd_8254, sd_8254, sd_intr},
-			.sdinfo = {0x00, 0x08, 0x10, 0x14, 0x3F},
-			.has_int_sce = true,
-			.has_clk_gat_sce = true,
+		.name		= "pci215",
+		.mainbar	= 2,
+		.n_subdevs	= 5,
+		.sdtype		= {
+			sd_8255, sd_8255, sd_8254, sd_8254, sd_intr
 		},
+		.sdinfo		= { 0x00, 0x08, 0x10, 0x14, 0x3f },
+		.has_int_sce	= true,
+		.has_clk_gat_sce = true,
 	},
 	[pci272_model] = {
-		.name = "pci272",
-		.bustype = pci_bustype,
-		.mainbar = 2,
-		.mainsize = DIO200_IO_SIZE,
-		.layout = {
-			.n_subdevs = 4,
-			.sdtype = {sd_8255, sd_8255, sd_8255, sd_intr},
-			.sdinfo = {0x00, 0x08, 0x10, 0x3F},
-			.has_int_sce = true,
+		.name		= "pci272",
+		.mainbar	= 2,
+		.n_subdevs	= 4,
+		.sdtype		= {
+			sd_8255, sd_8255, sd_8255, sd_intr
 		},
+		.sdinfo		= { 0x00, 0x08, 0x10, 0x3f },
+		.has_int_sce	= true,
 	},
 	[pcie215_model] = {
-		.name = "pcie215",
-		.bustype = pci_bustype,
-		.mainbar = 1,
-		.mainshift = 3,
-		.mainsize = DIO200_PCIE_IO_SIZE,
-		.layout = {
-			.n_subdevs = 8,
-			.sdtype = {sd_8255, sd_none, sd_8255, sd_none,
-				   sd_8254, sd_8254, sd_timer, sd_intr},
-			.sdinfo = {0x00, 0x00, 0x08, 0x00,
-				   0x10, 0x14, 0x00, 0x3F},
-			.has_int_sce = true,
-			.has_clk_gat_sce = true,
-			.has_enhancements = true,
+		.name		= "pcie215",
+		.mainbar	= 1,
+		.n_subdevs	= 8,
+		.sdtype		= {
+			sd_8255, sd_none, sd_8255, sd_none,
+			sd_8254, sd_8254, sd_timer, sd_intr
 		},
+		.sdinfo		= {
+			0x00, 0x00, 0x08, 0x00, 0x10, 0x14, 0x00, 0x3f
+		},
+		.has_int_sce	= true,
+		.has_clk_gat_sce = true,
+		.is_pcie	= true,
 	},
 	[pcie236_model] = {
-		.name = "pcie236",
-		.bustype = pci_bustype,
-		.mainbar = 1,
-		.mainshift = 3,
-		.mainsize = DIO200_PCIE_IO_SIZE,
-		.layout = {
-			.n_subdevs = 8,
-			.sdtype = {sd_8255, sd_none, sd_none, sd_none,
-				   sd_8254, sd_8254, sd_timer, sd_intr},
-			.sdinfo = {0x00, 0x00, 0x00, 0x00,
-				   0x10, 0x14, 0x00, 0x3F},
-			.has_int_sce = true,
-			.has_clk_gat_sce = true,
-			.has_enhancements = true,
+		.name		= "pcie236",
+		.mainbar	= 1,
+		.n_subdevs	= 8,
+		.sdtype		= {
+			sd_8255, sd_none, sd_none, sd_none,
+			sd_8254, sd_8254, sd_timer, sd_intr
+		},
+		.sdinfo		= {
+			0x00, 0x00, 0x00, 0x00, 0x10, 0x14, 0x00, 0x3f
 		},
+		.has_int_sce	= true,
+		.has_clk_gat_sce = true,
+		.is_pcie	= true,
 	},
 	[pcie296_model] = {
-		.name = "pcie296",
-		.bustype = pci_bustype,
-		.mainbar = 1,
-		.mainshift = 3,
-		.mainsize = DIO200_PCIE_IO_SIZE,
-		.layout = {
-			.n_subdevs = 8,
-			.sdtype = {sd_8255, sd_8255, sd_8255, sd_8255,
-				   sd_8254, sd_8254, sd_timer, sd_intr},
-			.sdinfo = {0x00, 0x04, 0x08, 0x0C,
-				   0x10, 0x14, 0x00, 0x3F},
-			.has_int_sce = true,
-			.has_clk_gat_sce = true,
-			.has_enhancements = true,
+		.name		= "pcie296",
+		.mainbar	= 1,
+		.n_subdevs	= 8,
+		.sdtype		= {
+			sd_8255, sd_8255, sd_8255, sd_8255,
+			sd_8254, sd_8254, sd_timer, sd_intr
+		},
+		.sdinfo		= {
+			0x00, 0x04, 0x08, 0x0c, 0x10, 0x14, 0x00, 0x3f
 		},
+		.has_int_sce	= true,
+		.has_clk_gat_sce = true,
+		.is_pcie	= true,
 	},
 };
 
@@ -358,34 +348,25 @@ static int dio200_pci_auto_attach(struct comedi_device *dev,
 				  unsigned long context_model)
 {
 	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
-	const struct dio200_board *thisboard = NULL;
-	struct dio200_private *devpriv;
+	const struct dio200_board *board = NULL;
 	unsigned int bar;
 	int ret;
 
 	if (context_model < ARRAY_SIZE(dio200_pci_boards))
-		thisboard = &dio200_pci_boards[context_model];
-	if (!thisboard)
+		board = &dio200_pci_boards[context_model];
+	if (!board)
 		return -EINVAL;
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
+	dev->board_ptr = board;
+	dev->board_name = board->name;
 
 	dev_info(dev->class_dev, "%s: attach pci %s (%s)\n",
 		 dev->driver->driver_name, pci_name(pci_dev), dev->board_name);
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
 
-	bar = thisboard->mainbar;
-	if (pci_resource_len(pci_dev, bar) < thisboard->mainsize) {
-		dev_err(dev->class_dev, "error! PCI region size too small!\n");
-		return -EINVAL;
-	}
+	bar = board->mainbar;
 	if (pci_resource_flags(pci_dev, bar) & IORESOURCE_MEM) {
 		dev->mmio = pci_ioremap_bar(pci_dev, bar);
 		if (!dev->mmio) {
@@ -396,33 +377,21 @@ static int dio200_pci_auto_attach(struct comedi_device *dev,
 	} else {
 		dev->iobase = pci_resource_start(pci_dev, bar);
 	}
-	switch (context_model) {
-	case pcie215_model:
-	case pcie236_model:
-	case pcie296_model:
+
+	if (board->is_pcie) {
 		ret = dio200_pcie_board_setup(dev);
 		if (ret < 0)
 			return ret;
-		break;
-	default:
-		break;
 	}
-	return amplc_dio200_common_attach(dev, pci_dev->irq, IRQF_SHARED);
-}
 
-static void dio200_pci_detach(struct comedi_device *dev)
-{
-	amplc_dio200_common_detach(dev);
-	if (dev->mmio)
-		iounmap(dev->mmio);
-	comedi_pci_disable(dev);
+	return amplc_dio200_common_attach(dev, pci_dev->irq, IRQF_SHARED);
 }
 
 static struct comedi_driver dio200_pci_comedi_driver = {
-	.driver_name = "amplc_dio200_pci",
-	.module = THIS_MODULE,
-	.auto_attach = dio200_pci_auto_attach,
-	.detach = dio200_pci_detach,
+	.driver_name	= "amplc_dio200_pci",
+	.module		= THIS_MODULE,
+	.auto_attach	= dio200_pci_auto_attach,
+	.detach		= comedi_pci_detach,
 };
 
 static const struct pci_device_id dio200_pci_table[] = {
@@ -443,10 +412,10 @@ static int dio200_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 }
 
 static struct pci_driver dio200_pci_pci_driver = {
-	.name = "amplc_dio200_pci",
-	.id_table = dio200_pci_table,
-	.probe = dio200_pci_probe,
-	.remove	= comedi_pci_auto_unconfig,
+	.name		= "amplc_dio200_pci",
+	.id_table	= dio200_pci_table,
+	.probe		= dio200_pci_probe,
+	.remove		= comedi_pci_auto_unconfig,
 };
 module_comedi_pci_driver(dio200_pci_comedi_driver, dio200_pci_pci_driver);
 
diff --git a/drivers/staging/comedi/drivers/amplc_pc236_common.c b/drivers/staging/comedi/drivers/amplc_pc236_common.c
index 18e237cca419..963c5d868b81 100644
--- a/drivers/staging/comedi/drivers/amplc_pc236_common.c
+++ b/drivers/staging/comedi/drivers/amplc_pc236_common.c
@@ -29,7 +29,7 @@
 
 static void pc236_intr_update(struct comedi_device *dev, bool enable)
 {
-	const struct pc236_board *thisboard = comedi_board(dev);
+	const struct pc236_board *thisboard = dev->board_ptr;
 	struct pc236_private *devpriv = dev->private;
 	unsigned long flags;
 
@@ -49,7 +49,7 @@ static void pc236_intr_update(struct comedi_device *dev, bool enable)
  */
 static bool pc236_intr_check(struct comedi_device *dev)
 {
-	const struct pc236_board *thisboard = comedi_board(dev);
+	const struct pc236_board *thisboard = dev->board_ptr;
 	struct pc236_private *devpriv = dev->private;
 	bool retval = false;
 	unsigned long flags;
@@ -94,9 +94,6 @@ static int pc236_intr_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check it arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
@@ -108,10 +105,9 @@ static int pc236_intr_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: ignored */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
@@ -160,7 +156,7 @@ int amplc_pc236_common_attach(struct comedi_device *dev, unsigned long iobase,
 
 	s = &dev->subdevices[0];
 	/* digital i/o subdevice (8255) */
-	ret = subdev_8255_init(dev, s, NULL, iobase);
+	ret = subdev_8255_init(dev, s, NULL, 0x00);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/amplc_pci224.c b/drivers/staging/comedi/drivers/amplc_pci224.c
index 45aba1f950fc..3bbbb57f19d6 100644
--- a/drivers/staging/comedi/drivers/amplc_pci224.c
+++ b/drivers/staging/comedi/drivers/amplc_pci224.c
@@ -1,102 +1,106 @@
 /*
-    comedi/drivers/amplc_pci224.c
-    Driver for Amplicon PCI224 and PCI234 AO boards.
-
-    Copyright (C) 2005 MEV Ltd. <http://www.mev.co.uk/>
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 1998,2000 David A. Schleef <ds@schleef.org>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
+ * comedi/drivers/amplc_pci224.c
+ * Driver for Amplicon PCI224 and PCI234 AO boards.
+ *
+ * Copyright (C) 2005 MEV Ltd. <http://www.mev.co.uk/>
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 1998,2000 David A. Schleef <ds@schleef.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
 
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-*/
 /*
-Driver: amplc_pci224
-Description: Amplicon PCI224, PCI234
-Author: Ian Abbott <abbotti@mev.co.uk>
-Devices: [Amplicon] PCI224 (amplc_pci224 or pci224),
-  PCI234 (amplc_pci224 or pci234)
-Updated: Wed, 22 Oct 2008 12:25:08 +0100
-Status: works, but see caveats
-
-Supports:
-
-  - ao_insn read/write
-  - ao_do_cmd mode with the following sources:
-
-    - start_src         TRIG_INT        TRIG_EXT
-    - scan_begin_src    TRIG_TIMER      TRIG_EXT
-    - convert_src       TRIG_NOW
-    - scan_end_src      TRIG_COUNT
-    - stop_src          TRIG_COUNT      TRIG_EXT        TRIG_NONE
-
-    The channel list must contain at least one channel with no repeated
-    channels.  The scan end count must equal the number of channels in
-    the channel list.
-
-    There is only one external trigger source so only one of start_src,
-    scan_begin_src or stop_src may use TRIG_EXT.
-
-Configuration options - PCI224:
-  [0] - PCI bus of device (optional).
-  [1] - PCI slot of device (optional).
-          If bus/slot is not specified, the first available PCI device
-          will be used.
-  [2] - Select available ranges according to jumper LK1.  All channels
-        are set to the same range:
-        0=Jumper position 1-2 (factory default), 4 software-selectable
-          internal voltage references, giving 4 bipolar and 4 unipolar
-          ranges:
-            [-10V,+10V], [-5V,+5V], [-2.5V,+2.5V], [-1.25V,+1.25V],
-            [0,+10V], [0,+5V], [0,+2.5V], [0,1.25V].
-        1=Jumper position 2-3, 1 external voltage reference, giving
-          1 bipolar and 1 unipolar range:
-            [-Vext,+Vext], [0,+Vext].
-
-Configuration options - PCI234:
-  [0] - PCI bus of device (optional).
-  [1] - PCI slot of device (optional).
-          If bus/slot is not specified, the first available PCI device
-          will be used.
-  [2] - Select internal or external voltage reference according to
-        jumper LK1.  This affects all channels:
-        0=Jumper position 1-2 (factory default), Vref=5V internal.
-        1=Jumper position 2-3, Vref=Vext external.
-  [3] - Select channel 0 range according to jumper LK2:
-        0=Jumper position 2-3 (factory default), range [-2*Vref,+2*Vref]
-          (10V bipolar when options[2]=0).
-        1=Jumper position 1-2, range [-Vref,+Vref]
-          (5V bipolar when options[2]=0).
-  [4] - Select channel 1 range according to jumper LK3: cf. options[3].
-  [5] - Select channel 2 range according to jumper LK4: cf. options[3].
-  [6] - Select channel 3 range according to jumper LK5: cf. options[3].
-
-Passing a zero for an option is the same as leaving it unspecified.
-
-Caveats:
-
-  1) All channels on the PCI224 share the same range.  Any change to the
-     range as a result of insn_write or a streaming command will affect
-     the output voltages of all channels, including those not specified
-     by the instruction or command.
-
-  2) For the analog output command,  the first scan may be triggered
-     falsely at the start of acquisition.  This occurs when the DAC scan
-     trigger source is switched from 'none' to 'timer' (scan_begin_src =
-     TRIG_TIMER) or 'external' (scan_begin_src == TRIG_EXT) at the start
-     of acquisition and the trigger source is at logic level 1 at the
-     time of the switch.  This is very likely for TRIG_TIMER.  For
-     TRIG_EXT, it depends on the state of the external line and whether
-     the CR_INVERT flag has been set.  The remaining scans are triggered
-     correctly.
-*/
+ * Driver: amplc_pci224
+ * Description: Amplicon PCI224, PCI234
+ * Author: Ian Abbott <abbotti@mev.co.uk>
+ * Devices: [Amplicon] PCI224 (amplc_pci224), PCI234
+ * Updated: Thu, 31 Jul 2014 11:08:03 +0000
+ * Status: works, but see caveats
+ *
+ * Supports:
+ *
+ *   - ao_insn read/write
+ *   - ao_do_cmd mode with the following sources:
+ *
+ *     - start_src         TRIG_INT        TRIG_EXT
+ *     - scan_begin_src    TRIG_TIMER      TRIG_EXT
+ *     - convert_src       TRIG_NOW
+ *     - scan_end_src      TRIG_COUNT
+ *     - stop_src          TRIG_COUNT      TRIG_EXT        TRIG_NONE
+ *
+ *     The channel list must contain at least one channel with no repeated
+ *     channels.  The scan end count must equal the number of channels in
+ *     the channel list.
+ *
+ *     There is only one external trigger source so only one of start_src,
+ *     scan_begin_src or stop_src may use TRIG_EXT.
+ *
+ * Configuration options:
+ *   none
+ *
+ * Manual configuration of PCI cards is not supported; they are configured
+ * automatically.
+ *
+ * Output range selection - PCI224:
+ *
+ *   Output ranges on PCI224 are partly software-selectable and partly
+ *   hardware-selectable according to jumper LK1.  All channels are set
+ *   to the same range:
+ *
+ *   - LK1 position 1-2 (factory default) corresponds to the following
+ *     comedi ranges:
+ *
+ *       0: [-10V,+10V]; 1: [-5V,+5V]; 2: [-2.5V,+2.5V], 3: [-1.25V,+1.25V],
+ *       4: [0,+10V],    5: [0,+5V],   6: [0,+2.5V],     7: [0,+1.25V]
+ *
+ *   - LK1 position 2-3 corresponds to the following Comedi ranges, using
+ *     an external voltage reference:
+ *
+ *       0: [-Vext,+Vext],
+ *       1: [0,+Vext]
+ *
+ * Output range selection - PCI234:
+ *
+ *   Output ranges on PCI234 are hardware-selectable according to jumper
+ *   LK1 which affects all channels, and jumpers LK2, LK3, LK4 and LK5
+ *   which affect channels 0, 1, 2 and 3 individually.  LK1 chooses between
+ *   an internal 5V reference and an external voltage reference (Vext).
+ *   LK2/3/4/5 choose (per channel) to double the reference or not according
+ *   to the following table:
+ *
+ *     LK1 position   LK2/3/4/5 pos  Comedi range
+ *     -------------  -------------  --------------
+ *     2-3 (factory)  1-2 (factory)  0: [-10V,+10V]
+ *     2-3 (factory)  2-3            1: [-5V,+5V]
+ *     1-2            1-2 (factory)  2: [-2*Vext,+2*Vext]
+ *     1-2            2-3            3: [-Vext,+Vext]
+ *
+ * Caveats:
+ *
+ *   1) All channels on the PCI224 share the same range.  Any change to the
+ *      range as a result of insn_write or a streaming command will affect
+ *      the output voltages of all channels, including those not specified
+ *      by the instruction or command.
+ *
+ *   2) For the analog output command,  the first scan may be triggered
+ *      falsely at the start of acquisition.  This occurs when the DAC scan
+ *      trigger source is switched from 'none' to 'timer' (scan_begin_src =
+ *      TRIG_TIMER) or 'external' (scan_begin_src == TRIG_EXT) at the start
+ *      of acquisition and the trigger source is at logic level 1 at the
+ *      time of the switch.  This is very likely for TRIG_TIMER.  For
+ *      TRIG_EXT, it depends on the state of the external line and whether
+ *      the CR_INVERT flag has been set.  The remaining scans are triggered
+ *      correctly.
+ */
 
 #include <linux/module.h>
 #include <linux/pci.h>
@@ -109,13 +113,6 @@ Caveats:
 #include "8253.h"
 
 /*
- * PCI IDs.
- */
-#define PCI_DEVICE_ID_AMPLICON_PCI224 0x0007
-#define PCI_DEVICE_ID_AMPLICON_PCI234 0x0008
-#define PCI_DEVICE_ID_INVALID 0xffff
-
-/*
  * PCI224/234 i/o space 1 (PCIBAR2) registers.
  */
 #define PCI224_Z2_CT0	0x14	/* 82C54 counter/timer 0 */
@@ -261,9 +258,17 @@ Caveats:
  * Range tables.
  */
 
-/* The software selectable internal ranges for PCI224 (option[2] == 0). */
-static const struct comedi_lrange range_pci224_internal = {
-	8, {
+/*
+ * The ranges for PCI224.
+ *
+ * These are partly hardware-selectable by jumper LK1 and partly
+ * software-selectable.
+ *
+ * All channels share the same hardware range.
+ */
+static const struct comedi_lrange range_pci224 = {
+	10, {
+		/* jumper LK1 in position 1-2 (factory default) */
 		BIP_RANGE(10),
 		BIP_RANGE(5),
 		BIP_RANGE(2.5),
@@ -271,11 +276,15 @@ static const struct comedi_lrange range_pci224_internal = {
 		UNI_RANGE(10),
 		UNI_RANGE(5),
 		UNI_RANGE(2.5),
-		UNI_RANGE(1.25)
+		UNI_RANGE(1.25),
+		/* jumper LK1 in position 2-3 */
+		RANGE_ext(-1, 1),	/* bipolar [-Vext,+Vext] */
+		RANGE_ext(0, 1),	/* unipolar [0,+Vext] */
 	}
 };
 
-static const unsigned short hwrange_pci224_internal[8] = {
+static const unsigned short hwrange_pci224[10] = {
+	/* jumper LK1 in position 1-2 (factory default) */
 	PCI224_DACCON_POLAR_BI | PCI224_DACCON_VREF_10,
 	PCI224_DACCON_POLAR_BI | PCI224_DACCON_VREF_5,
 	PCI224_DACCON_POLAR_BI | PCI224_DACCON_VREF_2_5,
@@ -284,87 +293,87 @@ static const unsigned short hwrange_pci224_internal[8] = {
 	PCI224_DACCON_POLAR_UNI | PCI224_DACCON_VREF_5,
 	PCI224_DACCON_POLAR_UNI | PCI224_DACCON_VREF_2_5,
 	PCI224_DACCON_POLAR_UNI | PCI224_DACCON_VREF_1_25,
-};
-
-/* The software selectable external ranges for PCI224 (option[2] == 1). */
-static const struct comedi_lrange range_pci224_external = {
-	2, {
-		RANGE_ext(-1, 1),	/* bipolar [-Vref,+Vref] */
-		RANGE_ext(0, 1)		/* unipolar [0,+Vref] */
-	}
-};
-
-static const unsigned short hwrange_pci224_external[2] = {
+	/* jumper LK1 in position 2-3 */
 	PCI224_DACCON_POLAR_BI,
 	PCI224_DACCON_POLAR_UNI,
 };
 
-/* The hardware selectable Vref*2 external range for PCI234
- * (option[2] == 1, option[3+n] == 0). */
-static const struct comedi_lrange range_pci234_ext2 = {
-	1, {
-		RANGE_ext(-2, 2)
-	}
+/* Used to check all channels set to the same range on PCI224. */
+static const unsigned char range_check_pci224[10] = {
+	0, 1, 2, 3, 4, 5, 6, 7, 8, 9,
 };
 
-/* The hardware selectable Vref external range for PCI234
- * (option[2] == 1, option[3+n] == 1). */
-static const struct comedi_lrange range_pci234_ext = {
-	1, {
-		RANGE_ext(-1, 1)
+/*
+ * The ranges for PCI234.
+ *
+ * These are all hardware-selectable by jumper LK1 affecting all channels,
+ * and jumpers LK2, LK3, LK4 and LK5 affecting channels 0, 1, 2 and 3
+ * individually.
+ */
+static const struct comedi_lrange range_pci234 = {
+	4, {
+		/* LK1: 1-2 (fact def), LK2/3/4/5: 2-3 (fac def) */
+		BIP_RANGE(10),
+		/* LK1: 1-2 (fact def), LK2/3/4/5: 1-2 */
+		BIP_RANGE(5),
+		/* LK1: 2-3, LK2/3/4/5: 2-3 (fac def) */
+		RANGE_ext(-2, 2),	/* bipolar [-2*Vext,+2*Vext] */
+		/* LK1: 2-3, LK2/3/4/5: 1-2 */
+		RANGE_ext(-1, 1),	/* bipolar [-Vext,+Vext] */
 	}
 };
 
-/* This serves for all the PCI234 ranges. */
-static const unsigned short hwrange_pci234[1] = {
-	PCI224_DACCON_POLAR_BI,	/* bipolar - hardware ignores it! */
+/* N.B. PCI234 ignores the polarity bit, but software uses it. */
+static const unsigned short hwrange_pci234[4] = {
+	PCI224_DACCON_POLAR_BI,
+	PCI224_DACCON_POLAR_BI,
+	PCI224_DACCON_POLAR_BI,
+	PCI224_DACCON_POLAR_BI,
+};
+
+/* Used to check all channels use same LK1 setting on PCI234. */
+static const unsigned char range_check_pci234[4] = {
+	0, 0, 1, 1,
 };
 
 /*
  * Board descriptions.
  */
 
-enum pci224_model { any_model, pci224_model, pci234_model };
+enum pci224_model { pci224_model, pci234_model };
 
 struct pci224_board {
 	const char *name;
-	unsigned short devid;
-	enum pci224_model model;
 	unsigned int ao_chans;
 	unsigned int ao_bits;
+	const struct comedi_lrange *ao_range;
+	const unsigned short *ao_hwrange;
+	const unsigned char *ao_range_check;
 };
 
 static const struct pci224_board pci224_boards[] = {
-	{
-	 .name = "pci224",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCI224,
-	 .model = pci224_model,
-	 .ao_chans = 16,
-	 .ao_bits = 12,
-	 },
-	{
-	 .name = "pci234",
-	 .devid = PCI_DEVICE_ID_AMPLICON_PCI234,
-	 .model = pci234_model,
-	 .ao_chans = 4,
-	 .ao_bits = 16,
-	 },
-	{
-	 .name = "amplc_pci224",
-	 .devid = PCI_DEVICE_ID_INVALID,
-	 .model = any_model,	/* wildcard */
-	 },
+	[pci224_model] = {
+		.name		= "pci224",
+		.ao_chans	= 16,
+		.ao_bits	= 12,
+		.ao_range	= &range_pci224,
+		.ao_hwrange	= &hwrange_pci224[0],
+		.ao_range_check	= &range_check_pci224[0],
+	},
+	[pci234_model] = {
+		.name		= "pci234",
+		.ao_chans	= 4,
+		.ao_bits	= 16,
+		.ao_range	= &range_pci234,
+		.ao_hwrange	= &hwrange_pci234[0],
+		.ao_range_check	= &range_check_pci234[0],
+	},
 };
 
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.  */
 struct pci224_private {
-	const unsigned short *hwrange;
 	unsigned long iobase1;
 	unsigned long state;
-	spinlock_t ao_spinlock;
-	unsigned int *ao_readback;
+	spinlock_t ao_spinlock;	/* spinlock for AO command handling */
 	unsigned short *ao_scan_vals;
 	unsigned char *ao_scan_order;
 	int intr_cpuid;
@@ -384,18 +393,16 @@ static void
 pci224_ao_set_data(struct comedi_device *dev, int chan, int range,
 		   unsigned int data)
 {
-	const struct pci224_board *thisboard = comedi_board(dev);
+	const struct pci224_board *thisboard = dev->board_ptr;
 	struct pci224_private *devpriv = dev->private;
 	unsigned short mangled;
 
-	/* Store unmangled data for readback. */
-	devpriv->ao_readback[chan] = data;
 	/* Enable the channel. */
 	outw(1 << chan, dev->iobase + PCI224_DACCEN);
 	/* Set range and reset FIFO. */
-	devpriv->daccon = COMBINE(devpriv->daccon, devpriv->hwrange[range],
-				  (PCI224_DACCON_POLAR_MASK |
-				   PCI224_DACCON_VREF_MASK));
+	devpriv->daccon = COMBINE(devpriv->daccon, thisboard->ao_hwrange[range],
+				  PCI224_DACCON_POLAR_MASK |
+				  PCI224_DACCON_VREF_MASK);
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 	/*
@@ -414,51 +421,23 @@ pci224_ao_set_data(struct comedi_device *dev, int chan, int range,
 	inw(dev->iobase + PCI224_SOFTTRIG);
 }
 
-/*
- * 'insn_write' function for AO subdevice.
- */
-static int
-pci224_ao_insn_write(struct comedi_device *dev, struct comedi_subdevice *s,
-		     struct comedi_insn *insn, unsigned int *data)
+static int pci224_ao_insn_write(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
-	int chan, range;
-
-	/* Unpack channel and range. */
-	chan = CR_CHAN(insn->chanspec);
-	range = CR_RANGE(insn->chanspec);
-
-	/* Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined. */
-	for (i = 0; i < insn->n; i++)
-		pci224_ao_set_data(dev, chan, range, data[i]);
-
-	return i;
-}
-
-/*
- * 'insn_read' function for AO subdevice.
- *
- * N.B. The value read will not be valid if the DAC channel has
- * never been written successfully since the device was attached
- * or since the channel has been used by an AO streaming write
- * command.
- */
-static int
-pci224_ao_insn_read(struct comedi_device *dev, struct comedi_subdevice *s,
-		    struct comedi_insn *insn, unsigned int *data)
-{
-	struct pci224_private *devpriv = dev->private;
-	int i;
-	int chan;
-
-	chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
 
+	for (i = 0; i < insn->n; i++) {
+		val = data[i];
+		pci224_ao_set_data(dev, chan, range, val);
+	}
+	s->readback[chan] = val;
 
-	return i;
+	return insn->n;
 }
 
 /*
@@ -496,11 +475,10 @@ static void pci224_ao_stop(struct comedi_device *dev,
 	spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
 	/* Reconfigure DAC for insn_write usage. */
 	outw(0, dev->iobase + PCI224_DACCEN);	/* Disable channels. */
-	devpriv->daccon = COMBINE(devpriv->daccon,
-				  PCI224_DACCON_TRIG_SW |
-				  PCI224_DACCON_FIFOINTR_EMPTY,
-				  PCI224_DACCON_TRIG_MASK |
-				  PCI224_DACCON_FIFOINTR_MASK);
+	devpriv->daccon =
+	     COMBINE(devpriv->daccon,
+		     PCI224_DACCON_TRIG_SW | PCI224_DACCON_FIFOINTR_EMPTY,
+		     PCI224_DACCON_TRIG_MASK | PCI224_DACCON_FIFOINTR_MASK);
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 }
@@ -516,21 +494,16 @@ static void pci224_ao_start(struct comedi_device *dev,
 	unsigned long flags;
 
 	set_bit(AO_CMD_STARTED, &devpriv->state);
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_stop_count == 0) {
-		/* An empty acquisition! */
-		s->async->events |= COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
-	} else {
-		/* Enable interrupts. */
-		spin_lock_irqsave(&devpriv->ao_spinlock, flags);
-		if (cmd->stop_src == TRIG_EXT)
-			devpriv->intsce = PCI224_INTR_EXT | PCI224_INTR_DAC;
-		else
-			devpriv->intsce = PCI224_INTR_DAC;
 
-		outb(devpriv->intsce, devpriv->iobase1 + PCI224_INT_SCE);
-		spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
-	}
+	/* Enable interrupts. */
+	spin_lock_irqsave(&devpriv->ao_spinlock, flags);
+	if (cmd->stop_src == TRIG_EXT)
+		devpriv->intsce = PCI224_INTR_EXT | PCI224_INTR_DAC;
+	else
+		devpriv->intsce = PCI224_INTR_DAC;
+
+	outb(devpriv->intsce, devpriv->iobase1 + PCI224_INT_SCE);
+	spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
 }
 
 /*
@@ -553,7 +526,6 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 		/* Fixed number of scans. */
 		if (num_scans > devpriv->ao_stop_count)
 			num_scans = devpriv->ao_stop_count;
-
 	}
 
 	/* Determine how much room is in the FIFO (in samples). */
@@ -561,7 +533,8 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 	switch (dacstat & PCI224_DACCON_FIFOFL_MASK) {
 	case PCI224_DACCON_FIFOFL_EMPTY:
 		room = PCI224_FIFO_ROOM_EMPTY;
-		if (cmd->stop_src == TRIG_COUNT && devpriv->ao_stop_count == 0) {
+		if (cmd->stop_src == TRIG_COUNT &&
+		    devpriv->ao_stop_count == 0) {
 			/* FIFO empty at end of counted acquisition. */
 			s->async->events |= COMEDI_CB_EOA;
 			cfc_handle_events(dev, s);
@@ -639,10 +612,9 @@ static void pci224_ao_handle_fifo(struct comedi_device *dev,
 				trig = PCI224_DACCON_TRIG_EXTN;
 			else
 				trig = PCI224_DACCON_TRIG_EXTP;
-
 		}
-		devpriv->daccon = COMBINE(devpriv->daccon, trig,
-					  PCI224_DACCON_TRIG_MASK);
+		devpriv->daccon =
+		    COMBINE(devpriv->daccon, trig, PCI224_DACCON_TRIG_MASK);
 		outw(devpriv->daccon, dev->iobase + PCI224_DACCON);
 	}
 
@@ -668,13 +640,14 @@ static int pci224_ao_check_chanlist(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_cmd *cmd)
 {
-	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	const struct pci224_board *thisboard = dev->board_ptr;
+	unsigned int range_check_0;
 	unsigned int chan_mask = 0;
 	int i;
 
+	range_check_0 = thisboard->ao_range_check[CR_RANGE(cmd->chanlist[0])];
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
-		unsigned int range = CR_RANGE(cmd->chanlist[i]);
 
 		if (chan_mask & (1 << chan)) {
 			dev_dbg(dev->class_dev,
@@ -682,11 +655,12 @@ static int pci224_ao_check_chanlist(struct comedi_device *dev,
 				__func__);
 			return -EINVAL;
 		}
-		chan_mask |= (1 << chan);
+		chan_mask |= 1 << chan;
 
-		if (range != range0) {
+		if (thisboard->ao_range_check[CR_RANGE(cmd->chanlist[i])] !=
+		    range_check_0) {
 			dev_dbg(dev->class_dev,
-				"%s: entries in chanlist must all have the same range index\n",
+				"%s: entries in chanlist have incompatible ranges\n",
 				__func__);
 			return -EINVAL;
 		}
@@ -714,11 +688,11 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT | TRIG_EXT);
 	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-					TRIG_EXT | TRIG_TIMER);
+				     TRIG_EXT | TRIG_TIMER);
 	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_NOW);
 	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
 	err |= cfc_check_trigger_src(&cmd->stop_src,
-					TRIG_COUNT | TRIG_EXT | TRIG_NONE);
+				     TRIG_COUNT | TRIG_EXT | TRIG_NONE);
 
 	if (err)
 		return 1;
@@ -756,13 +730,13 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
-		if ((cmd->start_arg & ~CR_FLAGS_MASK) != 0) {
-			cmd->start_arg = COMBINE(cmd->start_arg, 0,
-						 ~CR_FLAGS_MASK);
+		if (cmd->start_arg & ~CR_FLAGS_MASK) {
+			cmd->start_arg =
+			    COMBINE(cmd->start_arg, 0, ~CR_FLAGS_MASK);
 			err |= -EINVAL;
 		}
 		/* The only flag allowed is CR_EDGE, which is ignored. */
-		if ((cmd->start_arg & CR_FLAGS_MASK & ~CR_EDGE) != 0) {
+		if (cmd->start_arg & CR_FLAGS_MASK & ~CR_EDGE) {
 			cmd->start_arg = COMBINE(cmd->start_arg, 0,
 						 CR_FLAGS_MASK & ~CR_EDGE);
 			err |= -EINVAL;
@@ -782,17 +756,17 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
-		if ((cmd->scan_begin_arg & ~CR_FLAGS_MASK) != 0) {
-			cmd->scan_begin_arg = COMBINE(cmd->scan_begin_arg, 0,
-						      ~CR_FLAGS_MASK);
+		if (cmd->scan_begin_arg & ~CR_FLAGS_MASK) {
+			cmd->scan_begin_arg =
+			    COMBINE(cmd->scan_begin_arg, 0, ~CR_FLAGS_MASK);
 			err |= -EINVAL;
 		}
 		/* Only allow flags CR_EDGE and CR_INVERT.  Ignore CR_EDGE. */
-		if ((cmd->scan_begin_arg & CR_FLAGS_MASK &
-		     ~(CR_EDGE | CR_INVERT)) != 0) {
-			cmd->scan_begin_arg = COMBINE(cmd->scan_begin_arg, 0,
-						      CR_FLAGS_MASK &
-						      ~(CR_EDGE | CR_INVERT));
+		if (cmd->scan_begin_arg & CR_FLAGS_MASK &
+		    ~(CR_EDGE | CR_INVERT)) {
+			cmd->scan_begin_arg =
+			    COMBINE(cmd->scan_begin_arg, 0,
+				    CR_FLAGS_MASK & ~(CR_EDGE | CR_INVERT));
 			err |= -EINVAL;
 		}
 		break;
@@ -803,19 +777,19 @@ pci224_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 
 	switch (cmd->stop_src) {
 	case TRIG_COUNT:
-		/* Any count allowed. */
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
 		break;
 	case TRIG_EXT:
 		/* Force to external trigger 0. */
-		if ((cmd->stop_arg & ~CR_FLAGS_MASK) != 0) {
-			cmd->stop_arg = COMBINE(cmd->stop_arg, 0,
-						~CR_FLAGS_MASK);
+		if (cmd->stop_arg & ~CR_FLAGS_MASK) {
+			cmd->stop_arg =
+			    COMBINE(cmd->stop_arg, 0, ~CR_FLAGS_MASK);
 			err |= -EINVAL;
 		}
 		/* The only flag allowed is CR_EDGE, which is ignored. */
-		if ((cmd->stop_arg & CR_FLAGS_MASK & ~CR_EDGE) != 0) {
-			cmd->stop_arg = COMBINE(cmd->stop_arg, 0,
-						CR_FLAGS_MASK & ~CR_EDGE);
+		if (cmd->stop_arg & CR_FLAGS_MASK & ~CR_EDGE) {
+			cmd->stop_arg =
+			    COMBINE(cmd->stop_arg, 0, CR_FLAGS_MASK & ~CR_EDGE);
 		}
 		break;
 	case TRIG_NONE:
@@ -880,6 +854,7 @@ static void pci224_ao_start_pacer(struct comedi_device *dev,
 
 static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
+	const struct pci224_board *thisboard = dev->board_ptr;
 	struct pci224_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	int range;
@@ -903,7 +878,6 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		for (j = 0; j < cmd->chanlist_len; j++) {
 			if (CR_CHAN(cmd->chanlist[j]) < ch)
 				rank++;
-
 		}
 		devpriv->ao_scan_order[rank] = i;
 	}
@@ -922,14 +896,12 @@ static int pci224_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 *
 	 * N.B. DAC FIFO interrupts are currently disabled.
 	 */
-	devpriv->daccon = COMBINE(devpriv->daccon,
-				  (devpriv->
-				   hwrange[range] | PCI224_DACCON_TRIG_NONE |
-				   PCI224_DACCON_FIFOINTR_NHALF),
-				  (PCI224_DACCON_POLAR_MASK |
-				   PCI224_DACCON_VREF_MASK |
-				   PCI224_DACCON_TRIG_MASK |
-				   PCI224_DACCON_FIFOINTR_MASK));
+	devpriv->daccon =
+	    COMBINE(devpriv->daccon,
+		    thisboard->ao_hwrange[range] | PCI224_DACCON_TRIG_NONE |
+		    PCI224_DACCON_FIFOINTR_NHALF,
+		    PCI224_DACCON_POLAR_MASK | PCI224_DACCON_VREF_MASK |
+		    PCI224_DACCON_TRIG_MASK | PCI224_DACCON_FIFOINTR_MASK);
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 
@@ -974,8 +946,7 @@ static void
 pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 		void *data, unsigned int num_bytes, unsigned int chan_index)
 {
-	const struct pci224_board *thisboard = comedi_board(dev);
-	struct pci224_private *devpriv = dev->private;
+	const struct pci224_board *thisboard = dev->board_ptr;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned short *array = data;
 	unsigned int length = num_bytes / sizeof(*array);
@@ -986,7 +957,7 @@ pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* The hardware expects 16-bit numbers. */
 	shift = 16 - thisboard->ao_bits;
 	/* Channels will be all bipolar or all unipolar. */
-	if ((devpriv->hwrange[CR_RANGE(cmd->chanlist[0])] &
+	if ((thisboard->ao_hwrange[CR_RANGE(cmd->chanlist[0])] &
 	     PCI224_DACCON_POLAR_MASK) == PCI224_DACCON_POLAR_UNI) {
 		/* Unipolar */
 		offset = 0;
@@ -997,7 +968,6 @@ pci224_ao_munge(struct comedi_device *dev, struct comedi_subdevice *s,
 	/* Munge the data. */
 	for (i = 0; i < length; i++)
 		array[i] = (array[i] << shift) - offset;
-
 }
 
 /*
@@ -1025,7 +995,7 @@ static irqreturn_t pci224_interrupt(int irq, void *d)
 		devpriv->intr_running = 1;
 		devpriv->intr_cpuid = THISCPU;
 		spin_unlock_irqrestore(&devpriv->ao_spinlock, flags);
-		if (valid_intstat != 0) {
+		if (valid_intstat) {
 			cmd = &s->async->cmd;
 			if (valid_intstat & PCI224_INTR_EXT) {
 				devpriv->intsce &= ~PCI224_INTR_EXT;
@@ -1033,11 +1003,9 @@ static irqreturn_t pci224_interrupt(int irq, void *d)
 					pci224_ao_start(dev, s);
 				else if (cmd->stop_src == TRIG_EXT)
 					pci224_ao_stop(dev, s);
-
 			}
 			if (valid_intstat & PCI224_INTR_DAC)
 				pci224_ao_handle_fifo(dev, s);
-
 		}
 		/* Reenable interrupt sources. */
 		spin_lock_irqsave(&devpriv->ao_spinlock, flags);
@@ -1051,77 +1019,32 @@ static irqreturn_t pci224_interrupt(int irq, void *d)
 	return IRQ_RETVAL(retval);
 }
 
-/*
- * This function looks for a board matching the supplied PCI device.
- */
-static const struct pci224_board
-*pci224_find_pci_board(struct pci_dev *pci_dev)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(pci224_boards); i++)
-		if (pci_dev->device == pci224_boards[i].devid)
-			return &pci224_boards[i];
-	return NULL;
-}
-
-/*
- * This function looks for a PCI device matching the requested board name,
- * bus and slot.
- */
-static struct pci_dev *pci224_find_pci_dev(struct comedi_device *dev,
-					   struct comedi_devconfig *it)
-{
-	const struct pci224_board *thisboard = comedi_board(dev);
-	struct pci_dev *pci_dev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pci_dev) {
-		if (bus || slot) {
-			if (bus != pci_dev->bus->number ||
-			    slot != PCI_SLOT(pci_dev->devfn))
-				continue;
-		}
-		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
-			continue;
-
-		if (thisboard->model == any_model) {
-			/* Match any supported model. */
-			const struct pci224_board *board_ptr;
-
-			board_ptr = pci224_find_pci_board(pci_dev);
-			if (board_ptr == NULL)
-				continue;
-			/* Change board_ptr to matched board. */
-			dev->board_ptr = board_ptr;
-		} else {
-			/* Match specific model name. */
-			if (thisboard->devid != pci_dev->device)
-				continue;
-		}
-		return pci_dev;
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-/*
- * Common part of attach and auto_attach.
- */
-static int pci224_attach_common(struct comedi_device *dev,
-				struct pci_dev *pci_dev, int *options)
+static int
+pci224_auto_attach(struct comedi_device *dev, unsigned long context_model)
 {
-	const struct pci224_board *thisboard = comedi_board(dev);
-	struct pci224_private *devpriv = dev->private;
+	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
+	const struct pci224_board *thisboard = NULL;
+	struct pci224_private *devpriv;
 	struct comedi_subdevice *s;
 	unsigned int irq;
-	unsigned n;
 	int ret;
 
-	comedi_set_hw_dev(dev, &pci_dev->dev);
+	if (context_model < ARRAY_SIZE(pci224_boards))
+		thisboard = &pci224_boards[context_model];
+	if (!thisboard || !thisboard->name) {
+		dev_err(dev->class_dev,
+			"amplc_pci224: BUG! cannot determine board type!\n");
+		return -EINVAL;
+	}
+	dev->board_ptr = thisboard;
+	dev->board_name = thisboard->name;
+
+	dev_info(dev->class_dev, "amplc_pci224: attach pci %s - %s\n",
+		 pci_name(pci_dev), dev->board_name);
+
+	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
+	if (!devpriv)
+		return -ENOMEM;
 
 	ret = comedi_pci_enable(dev);
 	if (ret)
@@ -1133,13 +1056,6 @@ static int pci224_attach_common(struct comedi_device *dev,
 	dev->iobase = pci_resource_start(pci_dev, 3);
 	irq = pci_dev->irq;
 
-	/* Allocate readback buffer for AO channels. */
-	devpriv->ao_readback = kmalloc(sizeof(devpriv->ao_readback[0]) *
-				       thisboard->ao_chans, GFP_KERNEL);
-	if (!devpriv->ao_readback)
-		return -ENOMEM;
-
-
 	/* Allocate buffer to hold values for AO channel scan. */
 	devpriv->ao_scan_vals = kmalloc(sizeof(devpriv->ao_scan_vals[0]) *
 					thisboard->ao_chans, GFP_KERNEL);
@@ -1162,9 +1078,8 @@ static int pci224_attach_common(struct comedi_device *dev,
 	outw(PCI224_DACCON_GLOBALRESET, dev->iobase + PCI224_DACCON);
 	outw(0, dev->iobase + PCI224_DACCEN);
 	outw(0, dev->iobase + PCI224_FIFOSIZ);
-	devpriv->daccon = (PCI224_DACCON_TRIG_SW | PCI224_DACCON_POLAR_BI |
-			   PCI224_DACCON_FIFOENAB |
-			   PCI224_DACCON_FIFOINTR_EMPTY);
+	devpriv->daccon = PCI224_DACCON_TRIG_SW | PCI224_DACCON_POLAR_BI |
+			  PCI224_DACCON_FIFOENAB | PCI224_DACCON_FIFOINTR_EMPTY;
 	outw(devpriv->daccon | PCI224_DACCON_FIFORESET,
 	     dev->iobase + PCI224_DACCON);
 
@@ -1178,71 +1093,19 @@ static int pci224_attach_common(struct comedi_device *dev,
 	s->subdev_flags = SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
 	s->n_chan = thisboard->ao_chans;
 	s->maxdata = (1 << thisboard->ao_bits) - 1;
-	s->insn_write = &pci224_ao_insn_write;
-	s->insn_read = &pci224_ao_insn_read;
+	s->range_table = thisboard->ao_range;
+	s->insn_write = pci224_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
 	s->len_chanlist = s->n_chan;
-
 	dev->write_subdev = s;
-	s->do_cmd = &pci224_ao_cmd;
-	s->do_cmdtest = &pci224_ao_cmdtest;
-	s->cancel = &pci224_ao_cancel;
-	s->munge = &pci224_ao_munge;
-
-	/* Sort out channel range options. */
-	if (thisboard->model == pci234_model) {
-		/* PCI234 range options. */
-		const struct comedi_lrange **range_table_list;
-
-		s->range_table_list = range_table_list =
-		    kmalloc(sizeof(struct comedi_lrange *) * s->n_chan,
-			    GFP_KERNEL);
-		if (!s->range_table_list)
-			return -ENOMEM;
-
-		if (options) {
-			for (n = 2; n < 3 + s->n_chan; n++) {
-				if (options[n] < 0 || options[n] > 1) {
-					dev_warn(dev->class_dev,
-						 "warning! bad options[%u]=%d\n",
-						 n, options[n]);
-				}
-			}
-		}
-		for (n = 0; n < s->n_chan; n++) {
-			if (n < COMEDI_NDEVCONFOPTS - 3 && options &&
-			    options[3 + n] == 1) {
-				if (options[2] == 1)
-					range_table_list[n] = &range_pci234_ext;
-				else
-					range_table_list[n] = &range_bipolar5;
-
-			} else {
-				if (options && options[2] == 1) {
-					range_table_list[n] =
-					    &range_pci234_ext2;
-				} else {
-					range_table_list[n] = &range_bipolar10;
-				}
-			}
-		}
-		devpriv->hwrange = hwrange_pci234;
-	} else {
-		/* PCI224 range options. */
-		if (options && options[2] == 1) {
-			s->range_table = &range_pci224_external;
-			devpriv->hwrange = hwrange_pci224_external;
-		} else {
-			if (options && options[2] != 0) {
-				dev_warn(dev->class_dev,
-					 "warning! bad options[2]=%d\n",
-					 options[2]);
-			}
-			s->range_table = &range_pci224_internal;
-			devpriv->hwrange = hwrange_pci224_internal;
-		}
-	}
+	s->do_cmd = pci224_ao_cmd;
+	s->do_cmdtest = pci224_ao_cmdtest;
+	s->cancel = pci224_ao_cancel;
+	s->munge = pci224_ao_munge;
 
-	dev->board_name = thisboard->name;
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	if (irq) {
 		ret = request_irq(irq, pci224_interrupt, IRQF_SHARED,
@@ -1258,80 +1121,20 @@ static int pci224_attach_common(struct comedi_device *dev,
 	return 0;
 }
 
-static int pci224_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	struct pci224_private *devpriv;
-	struct pci_dev *pci_dev;
-
-	dev_info(dev->class_dev, "attach\n");
-
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
-	pci_dev = pci224_find_pci_dev(dev, it);
-	if (!pci_dev)
-		return -EIO;
-
-	return pci224_attach_common(dev, pci_dev, it->options);
-}
-
-static int
-pci224_auto_attach(struct comedi_device *dev, unsigned long context_unused)
-{
-	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
-	struct pci224_private *devpriv;
-
-	dev_info(dev->class_dev, "attach pci %s\n", pci_name(pci_dev));
-
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
-	dev->board_ptr = pci224_find_pci_board(pci_dev);
-	if (dev->board_ptr == NULL) {
-		dev_err(dev->class_dev,
-			"BUG! cannot determine board type!\n");
-		return -EINVAL;
-	}
-	/*
-	 * Need to 'get' the PCI device to match the 'put' in pci224_detach().
-	 * TODO: Remove the pci_dev_get() and matching pci_dev_put() once
-	 * support for manual attachment of PCI devices via pci224_attach()
-	 * has been removed.
-	 */
-	pci_dev_get(pci_dev);
-	return pci224_attach_common(dev, pci_dev, NULL);
-}
-
 static void pci224_detach(struct comedi_device *dev)
 {
 	struct pci224_private *devpriv = dev->private;
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->subdevices) {
-		struct comedi_subdevice *s;
-
-		s = &dev->subdevices[0];
-		/* AO subdevice */
-		kfree(s->range_table_list);
-	}
+	comedi_pci_detach(dev);
 	if (devpriv) {
-		kfree(devpriv->ao_readback);
 		kfree(devpriv->ao_scan_vals);
 		kfree(devpriv->ao_scan_order);
 	}
-	comedi_pci_disable(dev);
-	if (pcidev)
-		pci_dev_put(pcidev);
 }
 
 static struct comedi_driver amplc_pci224_driver = {
 	.driver_name	= "amplc_pci224",
 	.module		= THIS_MODULE,
-	.attach		= pci224_attach,
 	.detach		= pci224_detach,
 	.auto_attach	= pci224_auto_attach,
 	.board_name	= &pci224_boards[0].name,
@@ -1347,8 +1150,8 @@ static int amplc_pci224_pci_probe(struct pci_dev *dev,
 }
 
 static const struct pci_device_id amplc_pci224_pci_table[] = {
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI224) },
-	{ PCI_DEVICE(PCI_VENDOR_ID_AMPLICON, PCI_DEVICE_ID_AMPLICON_PCI234) },
+	{ PCI_VDEVICE(AMPLICON, 0x0007), pci224_model },
+	{ PCI_VDEVICE(AMPLICON, 0x0008), pci234_model },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, amplc_pci224_pci_table);
@@ -1362,5 +1165,5 @@ static struct pci_driver amplc_pci224_pci_driver = {
 module_comedi_pci_driver(amplc_pci224_driver, amplc_pci224_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for Amplicon PCI224 and PCI234 AO boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pci230.c b/drivers/staging/comedi/drivers/amplc_pci230.c
index 684275d76e8c..01796cd28e5b 100644
--- a/drivers/staging/comedi/drivers/amplc_pci230.c
+++ b/drivers/staging/comedi/drivers/amplc_pci230.c
@@ -24,24 +24,19 @@
  * Author: Allan Willcox <allanwillcox@ozemail.com.au>,
  *   Steve D Sharples <steve.sharples@nottingham.ac.uk>,
  *   Ian Abbott <abbotti@mev.co.uk>
- * Updated: Wed, 22 Oct 2008 12:34:49 +0100
- * Devices: [Amplicon] PCI230 (pci230 or amplc_pci230),
- *   PCI230+ (pci230+ or amplc_pci230),
- *   PCI260 (pci260 or amplc_pci230), PCI260+ (pci260+ or amplc_pci230)
+ * Updated: Mon, 01 Sep 2014 10:09:16 +0000
+ * Devices: [Amplicon] PCI230 (amplc_pci230), PCI230+, PCI260, PCI260+
  * Status: works
  *
  * Configuration options:
- *   [0] - PCI bus of device (optional).
- *   [1] - PCI slot of device (optional).
- *           If bus/slot is not specified, the first available PCI device
- *           will be used.
+ *   none
  *
- * Configuring a "amplc_pci230" will match any supported card and it will
- * choose the best match, picking the "+" models if possible.  Configuring
- * a "pci230" will match a PCI230 or PCI230+ card and it will be treated as
- * a PCI230.  Configuring a "pci260" will match a PCI260 or PCI260+ card
- * and it will be treated as a PCI260.  Configuring a "pci230+" will match
- * a PCI230+ card.  Configuring a "pci260+" will match a PCI260+ card.
+ * Manual configuration of PCI cards is not supported; they are configured
+ * automatically.
+ *
+ * The PCI230+ and PCI260+ have the same PCI device IDs as the PCI230 and
+ * PCI260, but can be distinguished by the the size of the PCI regions.  A
+ * card will be configured as a "+" model if detected as such.
  *
  * Subdevices:
  *
@@ -201,7 +196,6 @@
  */
 #define PCI_DEVICE_ID_PCI230 0x0000
 #define PCI_DEVICE_ID_PCI260 0x0006
-#define PCI_DEVICE_ID_INVALID 0xffff
 
 /*
  * PCI230 i/o space 1 registers.
@@ -427,16 +421,15 @@
  * (Potentially) shared resources and their owners
  */
 enum {
-	RES_Z2CT0,		/* Z2-CT0 */
-	RES_Z2CT1,		/* Z2-CT1 */
-	RES_Z2CT2,		/* Z2-CT2 */
-	NUM_RESOURCES		/* Number of (potentially) shared resources. */
+	RES_Z2CT0 = (1U << 0),	/* Z2-CT0 */
+	RES_Z2CT1 = (1U << 1),	/* Z2-CT1 */
+	RES_Z2CT2 = (1U << 2)	/* Z2-CT2 */
 };
 
 enum {
-	OWNER_NONE,		/* Not owned */
 	OWNER_AICMD,		/* Owned by AI command */
-	OWNER_AOCMD		/* Owned by AO command */
+	OWNER_AOCMD,		/* Owned by AO command */
+	NUM_OWNERS		/* Number of owners */
 };
 
 /*
@@ -449,10 +442,6 @@ enum {
 /* Current CPU.  XXX should this be hard_smp_processor_id()? */
 #define THISCPU		smp_processor_id()
 
-/* State flags for atomic bit operations */
-#define AI_CMD_STARTED	0
-#define AO_CMD_STARTED	1
-
 /*
  * Board descriptions for the two boards supported.
  */
@@ -460,52 +449,39 @@ enum {
 struct pci230_board {
 	const char *name;
 	unsigned short id;
-	int ai_chans;
-	int ai_bits;
-	int ao_chans;
-	int ao_bits;
-	int have_dio;
-	unsigned int min_hwver;	/* Minimum hardware version supported. */
+	unsigned char ai_bits;
+	unsigned char ao_bits;
+	unsigned char min_hwver; /* Minimum hardware version supported. */
+	bool have_dio:1;
 };
 
 static const struct pci230_board pci230_boards[] = {
 	{
 		.name		= "pci230+",
 		.id		= PCI_DEVICE_ID_PCI230,
-		.ai_chans	= 16,
 		.ai_bits	= 16,
-		.ao_chans	= 2,
 		.ao_bits	= 12,
-		.have_dio	= 1,
+		.have_dio	= true,
 		.min_hwver	= 1,
 	},
 	{
 		.name		= "pci260+",
 		.id		= PCI_DEVICE_ID_PCI260,
-		.ai_chans	= 16,
 		.ai_bits	= 16,
 		.min_hwver	= 1,
 	},
 	{
 		.name		= "pci230",
 		.id		= PCI_DEVICE_ID_PCI230,
-		.ai_chans	= 16,
 		.ai_bits	= 12,
-		.ao_chans	= 2,
 		.ao_bits	= 12,
-		.have_dio	= 1,
+		.have_dio	= true,
 	},
 	{
 		.name		= "pci260",
 		.id		= PCI_DEVICE_ID_PCI260,
-		.ai_chans	= 16,
 		.ai_bits	= 12,
 	},
-	{
-		/* Wildcard matches any above */
-		.name		= "amplc_pci230",
-		.id		= PCI_DEVICE_ID_INVALID,
-	},
 };
 
 struct pci230_private {
@@ -513,9 +489,7 @@ struct pci230_private {
 	spinlock_t res_spinlock;	/* Shared resources spin lock */
 	spinlock_t ai_stop_spinlock;	/* Spin lock for stopping AI command */
 	spinlock_t ao_stop_spinlock;	/* Spin lock for stopping AO command */
-	unsigned long state;		/* State flags */
-	unsigned long iobase1;		/* PCI230's I/O space 1 */
-	unsigned int ao_readback[2];	/* Used for AO readback */
+	unsigned long daqio;		/* PCI230's DAQ I/O space */
 	unsigned int ai_scan_count;	/* Number of AI scans remaining */
 	unsigned int ai_scan_pos;	/* Current position within AI scan */
 	unsigned int ao_scan_count;	/* Number of AO scans remaining.  */
@@ -525,12 +499,13 @@ struct pci230_private {
 	unsigned short daccon;		/* DACCON register value */
 	unsigned short adcfifothresh;	/* ADC FIFO threshold (PCI230+/260+) */
 	unsigned short adcg;		/* ADCG register value */
-	unsigned char int_en;		/* Interrupt enable bits */
-	unsigned char ai_bipolar;	/* Flag AI range is bipolar */
-	unsigned char ao_bipolar;	/* Flag AO range is bipolar */
-	unsigned char ier;		/* Copy of interrupt enable register */
-	unsigned char intr_running;	/* Flag set in interrupt routine */
-	unsigned char res_owner[NUM_RESOURCES]; /* Shared resource owners */
+	unsigned char ier;		/* Interrupt enable bits */
+	unsigned char res_owned[NUM_OWNERS]; /* Owned resources */
+	bool intr_running:1;		/* Flag set in interrupt routine */
+	bool ai_bipolar:1;		/* Flag AI range is bipolar */
+	bool ao_bipolar:1;		/* Flag AO range is bipolar */
+	bool ai_cmd_started:1;		/* Flag AI command started */
+	bool ao_cmd_started:1;		/* Flag AO command started */
 };
 
 /* PCI230 clock source periods in ns */
@@ -558,9 +533,6 @@ static const struct comedi_lrange pci230_ai_range = {
 /* PCI230 analogue gain bits for each input range. */
 static const unsigned char pci230_ai_gain[7] = { 0, 1, 2, 3, 1, 2, 3 };
 
-/* PCI230 adccon bipolar flag for each analogue input range. */
-static const unsigned char pci230_ai_bipolar[7] = { 1, 1, 1, 1, 0, 0, 0 };
-
 /* PCI230 analogue output range table */
 static const struct comedi_lrange pci230_ao_range = {
 	2, {
@@ -569,170 +541,122 @@ static const struct comedi_lrange pci230_ao_range = {
 	}
 };
 
-/* PCI230 daccon bipolar flag for each analogue output range. */
-static const unsigned char pci230_ao_bipolar[2] = { 0, 1 };
-
 static unsigned short pci230_ai_read(struct comedi_device *dev)
 {
-	const struct pci230_board *thisboard = comedi_board(dev);
+	const struct pci230_board *thisboard = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 	unsigned short data;
 
 	/* Read sample. */
-	data = inw(dev->iobase + PCI230_ADCDATA);
+	data = inw(devpriv->daqio + PCI230_ADCDATA);
 	/*
 	 * PCI230 is 12 bit - stored in upper bits of 16 bit register
 	 * (lower four bits reserved for expansion).  PCI230+ is 16 bit AI.
-	 */
-	data = data >> (16 - thisboard->ai_bits);
-
-	/*
+	 *
 	 * If a bipolar range was specified, mangle it
 	 * (twos complement->straight binary).
 	 */
 	if (devpriv->ai_bipolar)
-		data ^= 1 << (thisboard->ai_bits - 1);
-
+		data ^= 0x8000;
+	data >>= (16 - thisboard->ai_bits);
 	return data;
 }
 
-static inline unsigned short pci230_ao_mangle_datum(struct comedi_device *dev,
-						    unsigned short datum)
+static unsigned short pci230_ao_mangle_datum(struct comedi_device *dev,
+					     unsigned short datum)
 {
-	const struct pci230_board *thisboard = comedi_board(dev);
+	const struct pci230_board *thisboard = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 
 	/*
-	 * If a bipolar range was specified, mangle it
-	 * (straight binary->twos complement).
-	 */
-	if (devpriv->ao_bipolar)
-		datum ^= 1 << (thisboard->ao_bits - 1);
-
-	/*
 	 * PCI230 is 12 bit - stored in upper bits of 16 bit register (lower
 	 * four bits reserved for expansion).  PCI230+ is also 12 bit AO.
 	 */
 	datum <<= (16 - thisboard->ao_bits);
+	/*
+	 * If a bipolar range was specified, mangle it
+	 * (straight binary->twos complement).
+	 */
+	if (devpriv->ao_bipolar)
+		datum ^= 0x8000;
 	return datum;
 }
 
-static inline void pci230_ao_write_nofifo(struct comedi_device *dev,
-					  unsigned short datum,
-					  unsigned int chan)
+static void pci230_ao_write_nofifo(struct comedi_device *dev,
+				   unsigned short datum, unsigned int chan)
 {
 	struct pci230_private *devpriv = dev->private;
 
-	/* Store unmangled datum to be read back later. */
-	devpriv->ao_readback[chan] = datum;
-
 	/* Write mangled datum to appropriate DACOUT register. */
 	outw(pci230_ao_mangle_datum(dev, datum),
-	     dev->iobase + (((chan) == 0) ? PCI230_DACOUT1 : PCI230_DACOUT2));
+	     devpriv->daqio + ((chan == 0) ? PCI230_DACOUT1 : PCI230_DACOUT2));
 }
 
-static inline void pci230_ao_write_fifo(struct comedi_device *dev,
-					unsigned short datum, unsigned int chan)
+static void pci230_ao_write_fifo(struct comedi_device *dev,
+				 unsigned short datum, unsigned int chan)
 {
 	struct pci230_private *devpriv = dev->private;
 
-	/* Store unmangled datum to be read back later. */
-	devpriv->ao_readback[chan] = datum;
-
 	/* Write mangled datum to appropriate DACDATA register. */
 	outw(pci230_ao_mangle_datum(dev, datum),
-	     dev->iobase + PCI230P2_DACDATA);
+	     devpriv->daqio + PCI230P2_DACDATA);
 }
 
-static int get_resources(struct comedi_device *dev, unsigned int res_mask,
-			 unsigned char owner)
+static bool pci230_claim_shared(struct comedi_device *dev,
+				unsigned char res_mask, unsigned int owner)
 {
 	struct pci230_private *devpriv = dev->private;
-	int ok;
-	unsigned int i;
-	unsigned int b;
-	unsigned int claimed;
+	unsigned int o;
 	unsigned long irqflags;
 
-	ok = 1;
-	claimed = 0;
 	spin_lock_irqsave(&devpriv->res_spinlock, irqflags);
-	for (b = 1, i = 0; (i < NUM_RESOURCES) && res_mask; b <<= 1, i++) {
-		if (res_mask & b) {
-			res_mask &= ~b;
-			if (devpriv->res_owner[i] == OWNER_NONE) {
-				devpriv->res_owner[i] = owner;
-				claimed |= b;
-			} else if (devpriv->res_owner[i] != owner) {
-				for (b = 1, i = 0; claimed; b <<= 1, i++) {
-					if (claimed & b) {
-						devpriv->res_owner[i] =
-						    OWNER_NONE;
-						claimed &= ~b;
-					}
-				}
-				ok = 0;
-				break;
-			}
+	for (o = 0; o < NUM_OWNERS; o++) {
+		if (o == owner)
+			continue;
+		if (devpriv->res_owned[o] & res_mask) {
+			spin_unlock_irqrestore(&devpriv->res_spinlock,
+					       irqflags);
+			return false;
 		}
 	}
+	devpriv->res_owned[owner] |= res_mask;
 	spin_unlock_irqrestore(&devpriv->res_spinlock, irqflags);
-	return ok;
-}
-
-static inline int get_one_resource(struct comedi_device *dev,
-				   unsigned int resource, unsigned char owner)
-{
-	return get_resources(dev, (1U << resource), owner);
+	return true;
 }
 
-static void put_resources(struct comedi_device *dev, unsigned int res_mask,
-			  unsigned char owner)
+static void pci230_release_shared(struct comedi_device *dev,
+				  unsigned char res_mask, unsigned int owner)
 {
 	struct pci230_private *devpriv = dev->private;
-	unsigned int i;
-	unsigned int b;
 	unsigned long irqflags;
 
 	spin_lock_irqsave(&devpriv->res_spinlock, irqflags);
-	for (b = 1, i = 0; (i < NUM_RESOURCES) && res_mask; b <<= 1, i++) {
-		if (res_mask & b) {
-			res_mask &= ~b;
-			if (devpriv->res_owner[i] == owner)
-				devpriv->res_owner[i] = OWNER_NONE;
-		}
-	}
+	devpriv->res_owned[owner] &= ~res_mask;
 	spin_unlock_irqrestore(&devpriv->res_spinlock, irqflags);
 }
 
-static inline void put_one_resource(struct comedi_device *dev,
-				    unsigned int resource, unsigned char owner)
+static void pci230_release_all_resources(struct comedi_device *dev,
+					 unsigned int owner)
 {
-	put_resources(dev, (1U << resource), owner);
+	pci230_release_shared(dev, (unsigned char)~0, owner);
 }
 
-static inline void put_all_resources(struct comedi_device *dev,
-				     unsigned char owner)
-{
-	put_resources(dev, (1U << NUM_RESOURCES) - 1, owner);
-}
-
-static unsigned int divide_ns(uint64_t ns, unsigned int timebase,
-			      unsigned int flags)
+static unsigned int pci230_divide_ns(uint64_t ns, unsigned int timebase,
+				     unsigned int flags)
 {
 	uint64_t div;
 	unsigned int rem;
 
 	div = ns;
 	rem = do_div(div, timebase);
-	switch (flags & TRIG_ROUND_MASK) {
+	switch (flags & CMDF_ROUND_MASK) {
 	default:
-	case TRIG_ROUND_NEAREST:
+	case CMDF_ROUND_NEAREST:
 		div += (rem + (timebase / 2)) / timebase;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		div += (rem + timebase - 1) / timebase;
 		break;
 	}
@@ -749,8 +673,8 @@ static unsigned int pci230_choose_clk_count(uint64_t ns, unsigned int *count,
 	unsigned int clk_src, cnt;
 
 	for (clk_src = CLK_10MHZ;; clk_src++) {
-		cnt = divide_ns(ns, pci230_timebase[clk_src], flags);
-		if ((cnt <= 65536) || (clk_src == CLK_1KHZ))
+		cnt = pci230_divide_ns(ns, pci230_timebase[clk_src], flags);
+		if (cnt <= 65536 || clk_src == CLK_1KHZ)
 			break;
 	}
 	*count = cnt;
@@ -770,29 +694,25 @@ static void pci230_ct_setup_ns_mode(struct comedi_device *dev, unsigned int ct,
 				    unsigned int mode, uint64_t ns,
 				    unsigned int flags)
 {
-	struct pci230_private *devpriv = dev->private;
 	unsigned int clk_src;
 	unsigned int count;
 
 	/* Set mode. */
-	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct, mode);
+	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, ct, mode);
 	/* Determine clock source and count. */
 	clk_src = pci230_choose_clk_count(ns, &count, flags);
 	/* Program clock source. */
-	outb(CLK_CONFIG(ct, clk_src), devpriv->iobase1 + PCI230_ZCLK_SCE);
+	outb(CLK_CONFIG(ct, clk_src), dev->iobase + PCI230_ZCLK_SCE);
 	/* Set initial count. */
 	if (count >= 65536)
 		count = 0;
 
-	i8254_write(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct, count);
+	i8254_write(dev->iobase + PCI230_Z2_CT_BASE, 0, ct, count);
 }
 
 static void pci230_cancel_ct(struct comedi_device *dev, unsigned int ct)
 {
-	struct pci230_private *devpriv = dev->private;
-
-	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, ct,
-		       I8254_MODE1);
+	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, ct, I8254_MODE1);
 	/* Counter ct, 8254 mode 1, initial count not written. */
 }
 
@@ -801,17 +721,18 @@ static int pci230_ai_eoc(struct comedi_device *dev,
 			 struct comedi_insn *insn,
 			 unsigned long context)
 {
+	struct pci230_private *devpriv = dev->private;
 	unsigned int status;
 
-	status = inw(dev->iobase + PCI230_ADCCON);
+	status = inw(devpriv->daqio + PCI230_ADCCON);
 	if ((status & PCI230_ADC_FIFO_EMPTY) == 0)
 		return 0;
 	return -EBUSY;
 }
 
-static int pci230_ai_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
+static int pci230_ai_insn_read(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn, unsigned int *data)
 {
 	struct pci230_private *devpriv = dev->private;
 	unsigned int n;
@@ -842,8 +763,8 @@ static int pci230_ai_rinsn(struct comedi_device *dev,
 	 */
 	adccon = PCI230_ADC_TRIG_Z2CT2 | PCI230_ADC_FIFO_EN;
 	/* Set Z2-CT2 output low to avoid any false triggers. */
-	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE0);
-	devpriv->ai_bipolar = pci230_ai_bipolar[range];
+	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE0);
+	devpriv->ai_bipolar = comedi_range_is_bipolar(s, range);
 	if (aref == AREF_DIFF) {
 		/* Differential. */
 		gainshift = chan * 2;
@@ -874,19 +795,18 @@ static int pci230_ai_rinsn(struct comedi_device *dev,
 	else
 		adccon |= PCI230_ADC_IR_UNI;
 
-
 	/*
 	 * Enable only this channel in the scan list - otherwise by default
 	 * we'll get one sample from each channel.
 	 */
-	outw(adcen, dev->iobase + PCI230_ADCEN);
+	outw(adcen, devpriv->daqio + PCI230_ADCEN);
 
 	/* Set gain for channel. */
-	outw(devpriv->adcg, dev->iobase + PCI230_ADCG);
+	outw(devpriv->adcg, devpriv->daqio + PCI230_ADCG);
 
 	/* Specify uni/bip, se/diff, conversion source, and reset FIFO. */
 	devpriv->adccon = adccon;
-	outw(adccon | PCI230_ADC_FIFO_RESET, dev->iobase + PCI230_ADCCON);
+	outw(adccon | PCI230_ADC_FIFO_RESET, devpriv->daqio + PCI230_ADCCON);
 
 	/* Convert n samples */
 	for (n = 0; n < insn->n; n++) {
@@ -894,10 +814,10 @@ static int pci230_ai_rinsn(struct comedi_device *dev,
 		 * Trigger conversion by toggling Z2-CT2 output
 		 * (finish with output high).
 		 */
-		i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, 2,
-			       I8254_MODE0);
-		i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, 2,
-			       I8254_MODE1);
+		i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0,
+			       2, I8254_MODE0);
+		i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0,
+			       2, I8254_MODE1);
 
 		/* wait for conversion to end */
 		ret = comedi_timeout(dev, s, insn, pci230_ai_eoc, 0);
@@ -912,57 +832,31 @@ static int pci230_ai_rinsn(struct comedi_device *dev,
 	return n;
 }
 
-/*
- *  COMEDI_SUBD_AO instructions;
- */
-static int pci230_ao_winsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
+static int pci230_ao_insn_write(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
 	struct pci230_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
-	int chan, range;
-
-	/* Unpack channel and range. */
-	chan = CR_CHAN(insn->chanspec);
-	range = CR_RANGE(insn->chanspec);
 
 	/*
 	 * Set range - see analogue output range table; 0 => unipolar 10V,
 	 * 1 => bipolar +/-10V range scale
 	 */
-	devpriv->ao_bipolar = pci230_ao_bipolar[range];
-	outw(range, dev->iobase + PCI230_DACCON);
+	devpriv->ao_bipolar = comedi_range_is_bipolar(s, range);
+	outw(range, devpriv->daqio + PCI230_DACCON);
 
-	/*
-	 * Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined.
-	 */
 	for (i = 0; i < insn->n; i++) {
-		/* Write value to DAC and store it. */
-		pci230_ao_write_nofifo(dev, data[i], chan);
+		val = data[i];
+		pci230_ao_write_nofifo(dev, val, chan);
 	}
+	s->readback[chan] = val;
 
-	/* return the number of samples read/written */
-	return i;
-}
-
-/*
- * AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv.
- */
-static int pci230_ao_rinsn(struct comedi_device *dev,
-			   struct comedi_subdevice *s, struct comedi_insn *insn,
-			   unsigned int *data)
-{
-	struct pci230_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static int pci230_ao_check_chanlist(struct comedi_device *dev,
@@ -1000,7 +894,7 @@ static int pci230_ao_check_chanlist(struct comedi_device *dev,
 static int pci230_ao_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct pci230_board *thisboard = comedi_board(dev);
+	const struct pci230_board *thisboard = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
@@ -1010,7 +904,7 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_INT);
 
 	tmp = TRIG_TIMER | TRIG_INT;
-	if ((thisboard->min_hwver > 0) && (devpriv->hwver >= 2)) {
+	if (thisboard->min_hwver > 0 && devpriv->hwver >= 2) {
 		/*
 		 * For PCI230+ hardware version 2 onwards, allow external
 		 * trigger from EXTTRIG/EXTCONVCLK input (PCI230+ pin 25).
@@ -1078,11 +972,11 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 		 * The only flags allowed are CR_EDGE and CR_INVERT.
 		 * The CR_EDGE flag is ignored.
 		 */
-		if (cmd->scan_begin_arg &
-		    (CR_FLAGS_MASK & ~(CR_EDGE | CR_INVERT))) {
-			cmd->scan_begin_arg = COMBINE(cmd->scan_begin_arg, 0,
-						      CR_FLAGS_MASK &
-						      ~(CR_EDGE | CR_INVERT));
+		if (cmd->scan_begin_arg & CR_FLAGS_MASK &
+		    ~(CR_EDGE | CR_INVERT)) {
+			cmd->scan_begin_arg =
+			    COMBINE(cmd->scan_begin_arg, 0,
+				    CR_FLAGS_MASK & ~(CR_EDGE | CR_INVERT));
 			err |= -EINVAL;
 		}
 		break;
@@ -1093,7 +987,9 @@ static int pci230_ao_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_NONE)
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -1127,11 +1023,12 @@ static void pci230_ao_stop(struct comedi_device *dev,
 	struct pci230_private *devpriv = dev->private;
 	unsigned long irqflags;
 	unsigned char intsrc;
-	int started;
+	bool started;
 	struct comedi_cmd *cmd;
 
 	spin_lock_irqsave(&devpriv->ao_stop_spinlock, irqflags);
-	started = test_and_clear_bit(AO_CMD_STARTED, &devpriv->state);
+	started = devpriv->ao_cmd_started;
+	devpriv->ao_cmd_started = false;
 	spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
 	if (!started)
 		return;
@@ -1153,15 +1050,12 @@ static void pci230_ao_stop(struct comedi_device *dev,
 	 * unless we are called from the interrupt routine.
 	 */
 	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-	devpriv->int_en &= ~intsrc;
+	devpriv->ier &= ~intsrc;
 	while (devpriv->intr_running && devpriv->intr_cpuid != THISCPU) {
 		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
 	}
-	if (devpriv->ier != devpriv->int_en) {
-		devpriv->ier = devpriv->int_en;
-		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
-	}
+	outb(devpriv->ier, dev->iobase + PCI230_INT_SCE);
 	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 	if (devpriv->hwver >= 2) {
 		/*
@@ -1171,10 +1065,10 @@ static void pci230_ao_stop(struct comedi_device *dev,
 		devpriv->daccon &= PCI230_DAC_OR_MASK;
 		outw(devpriv->daccon | PCI230P2_DAC_FIFO_RESET |
 		     PCI230P2_DAC_FIFO_UNDERRUN_CLEAR,
-		     dev->iobase + PCI230_DACCON);
+		     devpriv->daqio + PCI230_DACCON);
 	}
 	/* Release resources. */
-	put_all_resources(dev, OWNER_AOCMD);
+	pci230_release_all_resources(dev, OWNER_AOCMD);
 }
 
 static void pci230_handle_ao_nofifo(struct comedi_device *dev,
@@ -1189,6 +1083,8 @@ static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_scan_count == 0)
 		return;
 	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+
 		/* Read sample from Comedi's circular buffer. */
 		ret = comedi_buf_get(s, &data);
 		if (ret == 0) {
@@ -1197,8 +1093,8 @@ static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 			dev_err(dev->class_dev, "AO buffer underrun\n");
 			return;
 		}
-		/* Write value to DAC. */
-		pci230_ao_write_nofifo(dev, data, CR_CHAN(cmd->chanlist[i]));
+		pci230_ao_write_nofifo(dev, data, chan);
+		s->readback[chan] = data;
 	}
 	async->events |= COMEDI_CB_BLOCK | COMEDI_CB_EOS;
 	if (cmd->stop_src == TRIG_COUNT) {
@@ -1211,10 +1107,12 @@ static void pci230_handle_ao_nofifo(struct comedi_device *dev,
 	}
 }
 
-/* Loads DAC FIFO (if using it) from buffer. */
-/* Returns 0 if AO finished due to completion or error, 1 if still going. */
-static int pci230_handle_ao_fifo(struct comedi_device *dev,
-				 struct comedi_subdevice *s)
+/*
+ * Loads DAC FIFO (if using it) from buffer.
+ * Returns false if AO finished due to completion or error, true if still going.
+ */
+static bool pci230_handle_ao_fifo(struct comedi_device *dev,
+				  struct comedi_subdevice *s)
 {
 	struct pci230_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
@@ -1224,10 +1122,10 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 	unsigned short dacstat;
 	unsigned int i, n;
 	unsigned int events = 0;
-	int running;
+	bool running;
 
 	/* Get DAC FIFO status. */
-	dacstat = inw(dev->iobase + PCI230_DACCON);
+	dacstat = inw(devpriv->daqio + PCI230_DACCON);
 	/* Determine number of scans available in buffer. */
 	num_scans = comedi_buf_read_n_available(s) / cfc_bytes_per_scan(s);
 	if (cmd->stop_src == TRIG_COUNT) {
@@ -1250,8 +1148,8 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 		 * (otherwise there will be loads of "DAC FIFO not half full"
 		 * interrupts).
 		 */
-		if ((num_scans == 0) &&
-		    ((dacstat & PCI230P2_DAC_FIFO_HALF) == 0)) {
+		if (num_scans == 0 &&
+		    (dacstat & PCI230P2_DAC_FIFO_HALF) == 0) {
 			dev_err(dev->class_dev, "AO buffer underrun\n");
 			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
 		}
@@ -1274,11 +1172,12 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 		/* Process scans. */
 		for (n = 0; n < num_scans; n++) {
 			for (i = 0; i < cmd->chanlist_len; i++) {
+				unsigned int chan = CR_CHAN(cmd->chanlist[i]);
 				unsigned short datum;
 
 				comedi_buf_get(s, &datum);
-				pci230_ao_write_fifo(dev, datum,
-						     CR_CHAN(cmd->chanlist[i]));
+				pci230_ao_write_fifo(dev, datum, chan);
+				s->readback[chan] = datum;
 			}
 		}
 		events |= COMEDI_CB_EOS | COMEDI_CB_BLOCK;
@@ -1295,11 +1194,11 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 				     ~PCI230P2_DAC_INT_FIFO_MASK) |
 				    PCI230P2_DAC_INT_FIFO_EMPTY;
 				outw(devpriv->daccon,
-				     dev->iobase + PCI230_DACCON);
+				     devpriv->daqio + PCI230_DACCON);
 			}
 		}
 		/* Check if FIFO underrun occurred while writing to FIFO. */
-		dacstat = inw(dev->iobase + PCI230_DACCON);
+		dacstat = inw(devpriv->daqio + PCI230_DACCON);
 		if (dacstat & PCI230P2_DAC_FIFO_UNDERRUN_LATCHED) {
 			dev_err(dev->class_dev, "AO FIFO underrun\n");
 			events |= COMEDI_CB_OVERFLOW | COMEDI_CB_ERROR;
@@ -1308,9 +1207,9 @@ static int pci230_handle_ao_fifo(struct comedi_device *dev,
 	if (events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW)) {
 		/* Stopping AO due to completion or error. */
 		pci230_ao_stop(dev, s);
-		running = 0;
+		running = false;
 	} else {
-		running = 1;
+		running = true;
 	}
 	async->events |= events;
 	return running;
@@ -1327,28 +1226,25 @@ static int pci230_ao_inttrig_scan_begin(struct comedi_device *dev,
 		return -EINVAL;
 
 	spin_lock_irqsave(&devpriv->ao_stop_spinlock, irqflags);
-	if (test_bit(AO_CMD_STARTED, &devpriv->state)) {
-		/* Perform scan. */
-		if (devpriv->hwver < 2) {
-			/* Not using DAC FIFO. */
-			spin_unlock_irqrestore(&devpriv->ao_stop_spinlock,
-					       irqflags);
-			pci230_handle_ao_nofifo(dev, s);
-			comedi_event(dev, s);
-		} else {
-			/* Using DAC FIFO. */
-			/* Read DACSWTRIG register to trigger conversion. */
-			inw(dev->iobase + PCI230P2_DACSWTRIG);
-			spin_unlock_irqrestore(&devpriv->ao_stop_spinlock,
-					       irqflags);
-		}
-		/* Delay.  Should driver be responsible for this? */
-		/* XXX TODO: See if DAC busy bit can be used. */
-		udelay(8);
+	if (!devpriv->ao_cmd_started) {
+		spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
+		return 1;
+	}
+	/* Perform scan. */
+	if (devpriv->hwver < 2) {
+		/* Not using DAC FIFO. */
+		spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
+		pci230_handle_ao_nofifo(dev, s);
+		comedi_event(dev, s);
 	} else {
+		/* Using DAC FIFO. */
+		/* Read DACSWTRIG register to trigger conversion. */
+		inw(devpriv->daqio + PCI230P2_DACSWTRIG);
 		spin_unlock_irqrestore(&devpriv->ao_stop_spinlock, irqflags);
 	}
-
+	/* Delay.  Should driver be responsible for this? */
+	/* XXX TODO: See if DAC busy bit can be used. */
+	udelay(8);
 	return 1;
 }
 
@@ -1360,84 +1256,71 @@ static void pci230_ao_start(struct comedi_device *dev,
 	struct comedi_cmd *cmd = &async->cmd;
 	unsigned long irqflags;
 
-	set_bit(AO_CMD_STARTED, &devpriv->state);
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ao_scan_count == 0) {
-		/* An empty acquisition! */
-		async->events |= COMEDI_CB_EOA;
-		pci230_ao_stop(dev, s);
+	devpriv->ao_cmd_started = true;
+
+	if (devpriv->hwver >= 2) {
+		/* Using DAC FIFO. */
+		unsigned short scantrig;
+		bool run;
+
+		/* Preload FIFO data. */
+		run = pci230_handle_ao_fifo(dev, s);
 		comedi_event(dev, s);
-	} else {
-		if (devpriv->hwver >= 2) {
-			/* Using DAC FIFO. */
-			unsigned short scantrig;
-			int run;
-
-			/* Preload FIFO data. */
-			run = pci230_handle_ao_fifo(dev, s);
-			comedi_event(dev, s);
-			if (!run) {
-				/* Stopped. */
-				return;
-			}
-			/* Set scan trigger source. */
-			switch (cmd->scan_begin_src) {
-			case TRIG_TIMER:
-				scantrig = PCI230P2_DAC_TRIG_Z2CT1;
-				break;
-			case TRIG_EXT:
-				/* Trigger on EXTTRIG/EXTCONVCLK pin. */
-				if ((cmd->scan_begin_arg & CR_INVERT) == 0) {
-					/* +ve edge */
-					scantrig = PCI230P2_DAC_TRIG_EXTP;
-				} else {
-					/* -ve edge */
-					scantrig = PCI230P2_DAC_TRIG_EXTN;
-				}
-				break;
-			case TRIG_INT:
-				scantrig = PCI230P2_DAC_TRIG_SW;
-				break;
-			default:
-				/* Shouldn't get here. */
-				scantrig = PCI230P2_DAC_TRIG_NONE;
-				break;
-			}
-			devpriv->daccon =
-			    (devpriv->daccon & ~PCI230P2_DAC_TRIG_MASK) |
-			    scantrig;
-			outw(devpriv->daccon, dev->iobase + PCI230_DACCON);
+		if (!run) {
+			/* Stopped. */
+			return;
 		}
+		/* Set scan trigger source. */
 		switch (cmd->scan_begin_src) {
 		case TRIG_TIMER:
-			if (devpriv->hwver < 2) {
-				/* Not using DAC FIFO. */
-				/* Enable CT1 timer interrupt. */
-				spin_lock_irqsave(&devpriv->isr_spinlock,
-						  irqflags);
-				devpriv->int_en |= PCI230_INT_ZCLK_CT1;
-				devpriv->ier |= PCI230_INT_ZCLK_CT1;
-				outb(devpriv->ier,
-				     devpriv->iobase1 + PCI230_INT_SCE);
-				spin_unlock_irqrestore(&devpriv->isr_spinlock,
-						       irqflags);
+			scantrig = PCI230P2_DAC_TRIG_Z2CT1;
+			break;
+		case TRIG_EXT:
+			/* Trigger on EXTTRIG/EXTCONVCLK pin. */
+			if ((cmd->scan_begin_arg & CR_INVERT) == 0) {
+				/* +ve edge */
+				scantrig = PCI230P2_DAC_TRIG_EXTP;
+			} else {
+				/* -ve edge */
+				scantrig = PCI230P2_DAC_TRIG_EXTN;
 			}
-			/* Set CT1 gate high to start counting. */
-			outb(GAT_CONFIG(1, GAT_VCC),
-			     devpriv->iobase1 + PCI230_ZGAT_SCE);
 			break;
 		case TRIG_INT:
-			async->inttrig = pci230_ao_inttrig_scan_begin;
+			scantrig = PCI230P2_DAC_TRIG_SW;
+			break;
+		default:
+			/* Shouldn't get here. */
+			scantrig = PCI230P2_DAC_TRIG_NONE;
 			break;
 		}
-		if (devpriv->hwver >= 2) {
-			/* Using DAC FIFO.  Enable DAC FIFO interrupt. */
+		devpriv->daccon =
+		    (devpriv->daccon & ~PCI230P2_DAC_TRIG_MASK) | scantrig;
+		outw(devpriv->daccon, devpriv->daqio + PCI230_DACCON);
+	}
+	switch (cmd->scan_begin_src) {
+	case TRIG_TIMER:
+		if (devpriv->hwver < 2) {
+			/* Not using DAC FIFO. */
+			/* Enable CT1 timer interrupt. */
 			spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-			devpriv->int_en |= PCI230P2_INT_DAC;
-			devpriv->ier |= PCI230P2_INT_DAC;
-			outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
+			devpriv->ier |= PCI230_INT_ZCLK_CT1;
+			outb(devpriv->ier, dev->iobase + PCI230_INT_SCE);
 			spin_unlock_irqrestore(&devpriv->isr_spinlock,
 					       irqflags);
 		}
+		/* Set CT1 gate high to start counting. */
+		outb(GAT_CONFIG(1, GAT_VCC), dev->iobase + PCI230_ZGAT_SCE);
+		break;
+	case TRIG_INT:
+		async->inttrig = pci230_ao_inttrig_scan_begin;
+		break;
+	}
+	if (devpriv->hwver >= 2) {
+		/* Using DAC FIFO.  Enable DAC FIFO interrupt. */
+		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
+		devpriv->ier |= PCI230P2_INT_DAC;
+		outb(devpriv->ier, dev->iobase + PCI230_INT_SCE);
+		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 	}
 }
 
@@ -1467,22 +1350,18 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		/* Claim Z2-CT1. */
-		if (!get_one_resource(dev, RES_Z2CT1, OWNER_AOCMD))
+		if (!pci230_claim_shared(dev, RES_Z2CT1, OWNER_AOCMD))
 			return -EBUSY;
 	}
 
-	/* Get number of scans required. */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ao_scan_count = cmd->stop_arg;
-	else	/* TRIG_NONE, user calls cancel */
-		devpriv->ao_scan_count = 0;
+	devpriv->ao_scan_count = cmd->stop_arg;
 
 	/*
 	 * Set range - see analogue output range table; 0 => unipolar 10V,
 	 * 1 => bipolar +/-10V range scale
 	 */
 	range = CR_RANGE(cmd->chanlist[0]);
-	devpriv->ao_bipolar = pci230_ao_bipolar[range];
+	devpriv->ao_bipolar = comedi_range_is_bipolar(s, range);
 	daccon = devpriv->ao_bipolar ? PCI230_DAC_OR_BIP : PCI230_DAC_OR_UNI;
 	/* Use DAC FIFO for hardware version 2 onwards. */
 	if (devpriv->hwver >= 2) {
@@ -1494,7 +1373,7 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			dacen |= 1 << CR_CHAN(cmd->chanlist[i]);
 
 		/* Set channel scan list. */
-		outw(dacen, dev->iobase + PCI230P2_DACEN);
+		outw(dacen, devpriv->daqio + PCI230P2_DACEN);
 		/*
 		 * Enable DAC FIFO.
 		 * Set DAC scan source to 'none'.
@@ -1509,7 +1388,7 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* Set DACCON. */
-	outw(daccon, dev->iobase + PCI230_DACCON);
+	outw(daccon, devpriv->daqio + PCI230_DACCON);
 	/* Preserve most of DACCON apart from write-only, transient bits. */
 	devpriv->daccon = daccon & ~(PCI230P2_DAC_FIFO_RESET |
 				     PCI230P2_DAC_FIFO_UNDERRUN_CLEAR);
@@ -1520,8 +1399,7 @@ static int pci230_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * cmd->scan_begin_arg is sampling period in ns.
 		 * Gate it off for now.
 		 */
-		outb(GAT_CONFIG(1, GAT_GND),
-		     devpriv->iobase1 + PCI230_ZGAT_SCE);
+		outb(GAT_CONFIG(1, GAT_GND), dev->iobase + PCI230_ZGAT_SCE);
 		pci230_ct_setup_ns_mode(dev, 1, I8254_MODE3,
 					cmd->scan_begin_arg,
 					cmd->flags);
@@ -1550,8 +1428,8 @@ static int pci230_ai_check_scan_period(struct comedi_cmd *cmd)
 		chanlist_len = 1;
 
 	min_scan_period = chanlist_len * cmd->convert_arg;
-	if ((min_scan_period < chanlist_len) ||
-	    (min_scan_period < cmd->convert_arg)) {
+	if (min_scan_period < chanlist_len ||
+	    min_scan_period < cmd->convert_arg) {
 		/* Arithmetic overflow. */
 		min_scan_period = UINT_MAX;
 		err++;
@@ -1573,7 +1451,7 @@ static int pci230_ai_check_chanlist(struct comedi_device *dev,
 	unsigned int prev_chan = 0;
 	unsigned int prev_range = 0;
 	unsigned int prev_aref = 0;
-	unsigned int prev_polarity = 0;
+	bool prev_bipolar = false;
 	unsigned int subseq_len = 0;
 	int i;
 
@@ -1582,7 +1460,7 @@ static int pci230_ai_check_chanlist(struct comedi_device *dev,
 		unsigned int chan = CR_CHAN(chanspec);
 		unsigned int range = CR_RANGE(chanspec);
 		unsigned int aref = CR_AREF(chanspec);
-		unsigned int polarity = pci230_ai_bipolar[range];
+		bool bipolar = comedi_range_is_bipolar(s, range);
 
 		if (aref == AREF_DIFF && chan >= max_diff_chan) {
 			dev_dbg(dev->class_dev,
@@ -1614,7 +1492,7 @@ static int pci230_ai_check_chanlist(struct comedi_device *dev,
 				return -EINVAL;
 			}
 
-			if (polarity != prev_polarity) {
+			if (bipolar != prev_bipolar) {
 				dev_dbg(dev->class_dev,
 					"%s: channel sequence ranges must be all bipolar or all unipolar\n",
 					__func__);
@@ -1632,7 +1510,7 @@ static int pci230_ai_check_chanlist(struct comedi_device *dev,
 		prev_chan = chan;
 		prev_range = range;
 		prev_aref = aref;
-		prev_polarity = polarity;
+		prev_bipolar = bipolar;
 	}
 
 	if (subseq_len == 0)
@@ -1670,7 +1548,7 @@ static int pci230_ai_check_chanlist(struct comedi_device *dev,
 static int pci230_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct pci230_board *thisboard = comedi_board(dev);
+	const struct pci230_board *thisboard = dev->board_ptr;
 	struct pci230_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
@@ -1680,7 +1558,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_INT);
 
 	tmp = TRIG_FOLLOW | TRIG_TIMER | TRIG_INT;
-	if ((thisboard->have_dio) || (thisboard->min_hwver > 0)) {
+	if (thisboard->have_dio || thisboard->min_hwver > 0) {
 		/*
 		 * Unfortunately, we cannot trigger a scan off an external
 		 * source on the PCI260 board, since it uses the PPIC0 (DIO)
@@ -1711,8 +1589,8 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 	 * If scan_begin_src is not TRIG_FOLLOW, then a monostable will be
 	 * set up to generate a fixed number of timed conversion pulses.
 	 */
-	if ((cmd->scan_begin_src != TRIG_FOLLOW) &&
-	    (cmd->convert_src != TRIG_TIMER))
+	if (cmd->scan_begin_src != TRIG_FOLLOW &&
+	    cmd->convert_src != TRIG_TIMER)
 		err |= -EINVAL;
 
 	if (err)
@@ -1739,7 +1617,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 			 * PCI230 or PCI260.  Max speed depends whether
 			 * single-ended or pseudo-differential.
 			 */
-			if (cmd->chanlist && (cmd->chanlist_len > 0)) {
+			if (cmd->chanlist && cmd->chanlist_len > 0) {
 				/* Peek analogue reference of first channel. */
 				if (CR_AREF(cmd->chanlist[0]) == AREF_DIFF)
 					max_speed_ai = MAX_SPEED_AI_DIFF;
@@ -1779,13 +1657,12 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 			 * The only flags allowed are CR_INVERT and CR_EDGE.
 			 * CR_EDGE is required.
 			 */
-			if ((cmd->convert_arg &
-			     (CR_FLAGS_MASK & ~CR_INVERT)) != CR_EDGE) {
+			if ((cmd->convert_arg & CR_FLAGS_MASK & ~CR_INVERT) !=
+			    CR_EDGE) {
 				/* Set CR_EDGE, preserve CR_INVERT. */
-				cmd->convert_arg = COMBINE(cmd->start_arg,
-							   (CR_EDGE | 0),
-							   CR_FLAGS_MASK &
-							   ~CR_INVERT);
+				cmd->convert_arg =
+				    COMBINE(cmd->start_arg, CR_EDGE | 0,
+					    CR_FLAGS_MASK & ~CR_INVERT);
 				err |= -EINVAL;
 			}
 		} else {
@@ -1802,7 +1679,9 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_NONE)
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (cmd->scan_begin_src == TRIG_EXT) {
@@ -1850,7 +1729,7 @@ static int pci230_ai_cmdtest(struct comedi_device *dev,
 		if (!pci230_ai_check_scan_period(cmd)) {
 			/* Was below minimum required.  Round up. */
 			pci230_ns_to_single_timer(&cmd->scan_begin_arg,
-						  TRIG_ROUND_UP);
+						  CMDF_ROUND_UP);
 			pci230_ai_check_scan_period(cmd);
 		}
 		if (tmp != cmd->scan_begin_arg)
@@ -1880,37 +1759,30 @@ static void pci230_ai_update_fifo_trigger_level(struct comedi_device *dev,
 	unsigned short triglev;
 	unsigned short adccon;
 
-	if (cmd->flags & TRIG_WAKE_EOS) {
-		/* Wake at end of scan. */
+	if (cmd->flags & CMDF_WAKE_EOS)
 		wake = scanlen - devpriv->ai_scan_pos;
-	} else {
-		if (cmd->stop_src != TRIG_COUNT ||
-		    devpriv->ai_scan_count >= PCI230_ADC_FIFOLEVEL_HALFFULL ||
-		    scanlen >= PCI230_ADC_FIFOLEVEL_HALFFULL) {
-			wake = PCI230_ADC_FIFOLEVEL_HALFFULL;
-		} else {
-			wake = (devpriv->ai_scan_count * scanlen) -
-			       devpriv->ai_scan_pos;
-		}
-	}
+	else if (cmd->stop_src != TRIG_COUNT ||
+		 devpriv->ai_scan_count >= PCI230_ADC_FIFOLEVEL_HALFFULL ||
+		 scanlen >= PCI230_ADC_FIFOLEVEL_HALFFULL)
+		wake = PCI230_ADC_FIFOLEVEL_HALFFULL;
+	else
+		wake = devpriv->ai_scan_count * scanlen - devpriv->ai_scan_pos;
 	if (wake >= PCI230_ADC_FIFOLEVEL_HALFFULL) {
 		triglev = PCI230_ADC_INT_FIFO_HALF;
-	} else {
-		if ((wake > 1) && (devpriv->hwver > 0)) {
-			/* PCI230+/260+ programmable FIFO interrupt level. */
-			if (devpriv->adcfifothresh != wake) {
-				devpriv->adcfifothresh = wake;
-				outw(wake, dev->iobase + PCI230P_ADCFFTH);
-			}
-			triglev = PCI230P_ADC_INT_FIFO_THRESH;
-		} else {
-			triglev = PCI230_ADC_INT_FIFO_NEMPTY;
+	} else if (wake > 1 && devpriv->hwver > 0) {
+		/* PCI230+/260+ programmable FIFO interrupt level. */
+		if (devpriv->adcfifothresh != wake) {
+			devpriv->adcfifothresh = wake;
+			outw(wake, devpriv->daqio + PCI230P_ADCFFTH);
 		}
+		triglev = PCI230P_ADC_INT_FIFO_THRESH;
+	} else {
+		triglev = PCI230_ADC_INT_FIFO_NEMPTY;
 	}
 	adccon = (devpriv->adccon & ~PCI230_ADC_INT_FIFO_MASK) | triglev;
 	if (adccon != devpriv->adccon) {
 		devpriv->adccon = adccon;
-		outw(adccon, dev->iobase + PCI230_ADCCON);
+		outw(adccon, devpriv->daqio + PCI230_ADCCON);
 	}
 }
 
@@ -1920,43 +1792,39 @@ static int pci230_ai_inttrig_convert(struct comedi_device *dev,
 {
 	struct pci230_private *devpriv = dev->private;
 	unsigned long irqflags;
+	unsigned int delayus;
 
 	if (trig_num)
 		return -EINVAL;
 
 	spin_lock_irqsave(&devpriv->ai_stop_spinlock, irqflags);
-	if (test_bit(AI_CMD_STARTED, &devpriv->state)) {
-		unsigned int delayus;
-
-		/*
-		 * Trigger conversion by toggling Z2-CT2 output.
-		 * Finish with output high.
-		 */
-		i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, 2,
-			       I8254_MODE0);
-		i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, 2,
-			       I8254_MODE1);
-		/*
-		 * Delay.  Should driver be responsible for this?  An
-		 * alternative would be to wait until conversion is complete,
-		 * but we can't tell when it's complete because the ADC busy
-		 * bit has a different meaning when FIFO enabled (and when
-		 * FIFO not enabled, it only works for software triggers).
-		 */
-		if (((devpriv->adccon & PCI230_ADC_IM_MASK) ==
-		     PCI230_ADC_IM_DIF) && (devpriv->hwver == 0)) {
-			/* PCI230/260 in differential mode */
-			delayus = 8;
-		} else {
-			/* single-ended or PCI230+/260+ */
-			delayus = 4;
-		}
+	if (!devpriv->ai_cmd_started) {
 		spin_unlock_irqrestore(&devpriv->ai_stop_spinlock, irqflags);
-		udelay(delayus);
+		return 1;
+	}
+	/*
+	 * Trigger conversion by toggling Z2-CT2 output.
+	 * Finish with output high.
+	 */
+	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE0);
+	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE1);
+	/*
+	 * Delay.  Should driver be responsible for this?  An
+	 * alternative would be to wait until conversion is complete,
+	 * but we can't tell when it's complete because the ADC busy
+	 * bit has a different meaning when FIFO enabled (and when
+	 * FIFO not enabled, it only works for software triggers).
+	 */
+	if ((devpriv->adccon & PCI230_ADC_IM_MASK) == PCI230_ADC_IM_DIF &&
+	    devpriv->hwver == 0) {
+		/* PCI230/260 in differential mode */
+		delayus = 8;
 	} else {
-		spin_unlock_irqrestore(&devpriv->ai_stop_spinlock, irqflags);
+		/* single-ended or PCI230+/260+ */
+		delayus = 4;
 	}
-
+	spin_unlock_irqrestore(&devpriv->ai_stop_spinlock, irqflags);
+	udelay(delayus);
 	return 1;
 }
 
@@ -1972,12 +1840,12 @@ static int pci230_ai_inttrig_scan_begin(struct comedi_device *dev,
 		return -EINVAL;
 
 	spin_lock_irqsave(&devpriv->ai_stop_spinlock, irqflags);
-	if (test_bit(AI_CMD_STARTED, &devpriv->state)) {
+	if (devpriv->ai_cmd_started) {
 		/* Trigger scan by waggling CT0 gate source. */
 		zgat = GAT_CONFIG(0, GAT_GND);
-		outb(zgat, devpriv->iobase1 + PCI230_ZGAT_SCE);
+		outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
 		zgat = GAT_CONFIG(0, GAT_VCC);
-		outb(zgat, devpriv->iobase1 + PCI230_ZGAT_SCE);
+		outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
 	}
 	spin_unlock_irqrestore(&devpriv->ai_stop_spinlock, irqflags);
 
@@ -1990,10 +1858,11 @@ static void pci230_ai_stop(struct comedi_device *dev,
 	struct pci230_private *devpriv = dev->private;
 	unsigned long irqflags;
 	struct comedi_cmd *cmd;
-	int started;
+	bool started;
 
 	spin_lock_irqsave(&devpriv->ai_stop_spinlock, irqflags);
-	started = test_and_clear_bit(AI_CMD_STARTED, &devpriv->state);
+	started = devpriv->ai_cmd_started;
+	devpriv->ai_cmd_started = false;
 	spin_unlock_irqrestore(&devpriv->ai_stop_spinlock, irqflags);
 	if (!started)
 		return;
@@ -2011,15 +1880,12 @@ static void pci230_ai_stop(struct comedi_device *dev,
 	 * Disable ADC interrupt and wait for interrupt routine to finish
 	 * running unless we are called from the interrupt routine.
 	 */
-	devpriv->int_en &= ~PCI230_INT_ADC;
+	devpriv->ier &= ~PCI230_INT_ADC;
 	while (devpriv->intr_running && devpriv->intr_cpuid != THISCPU) {
 		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
 	}
-	if (devpriv->ier != devpriv->int_en) {
-		devpriv->ier = devpriv->int_en;
-		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
-	}
+	outb(devpriv->ier, dev->iobase + PCI230_INT_SCE);
 	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 	/*
 	 * Reset FIFO, disable FIFO and set start conversion source to none.
@@ -2029,9 +1895,9 @@ static void pci230_ai_stop(struct comedi_device *dev,
 	    (devpriv->adccon & (PCI230_ADC_IR_MASK | PCI230_ADC_IM_MASK)) |
 	    PCI230_ADC_TRIG_NONE;
 	outw(devpriv->adccon | PCI230_ADC_FIFO_RESET,
-	     dev->iobase + PCI230_ADCCON);
+	     devpriv->daqio + PCI230_ADCCON);
 	/* Release resources. */
-	put_all_resources(dev, OWNER_AICMD);
+	pci230_release_all_resources(dev, OWNER_AICMD);
 }
 
 static void pci230_ai_start(struct comedi_device *dev,
@@ -2043,145 +1909,132 @@ static void pci230_ai_start(struct comedi_device *dev,
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
 
-	set_bit(AI_CMD_STARTED, &devpriv->state);
-	if (cmd->stop_src == TRIG_COUNT && devpriv->ai_scan_count == 0) {
-		/* An empty acquisition! */
-		async->events |= COMEDI_CB_EOA;
-		pci230_ai_stop(dev, s);
-		comedi_event(dev, s);
-	} else {
-		/* Enable ADC FIFO trigger level interrupt. */
-		spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-		devpriv->int_en |= PCI230_INT_ADC;
-		devpriv->ier |= PCI230_INT_ADC;
-		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
-		spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
+	devpriv->ai_cmd_started = true;
 
-		/*
-		 * Update conversion trigger source which is currently set
-		 * to CT2 output, which is currently stuck high.
-		 */
-		switch (cmd->convert_src) {
-		default:
-			conv = PCI230_ADC_TRIG_NONE;
-			break;
-		case TRIG_TIMER:
-			/* Using CT2 output. */
-			conv = PCI230_ADC_TRIG_Z2CT2;
-			break;
-		case TRIG_EXT:
-			if (cmd->convert_arg & CR_EDGE) {
-				if ((cmd->convert_arg & CR_INVERT) == 0) {
-					/* Trigger on +ve edge. */
-					conv = PCI230_ADC_TRIG_EXTP;
-				} else {
-					/* Trigger on -ve edge. */
-					conv = PCI230_ADC_TRIG_EXTN;
-				}
+	/* Enable ADC FIFO trigger level interrupt. */
+	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
+	devpriv->ier |= PCI230_INT_ADC;
+	outb(devpriv->ier, dev->iobase + PCI230_INT_SCE);
+	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
+
+	/*
+	 * Update conversion trigger source which is currently set
+	 * to CT2 output, which is currently stuck high.
+	 */
+	switch (cmd->convert_src) {
+	default:
+		conv = PCI230_ADC_TRIG_NONE;
+		break;
+	case TRIG_TIMER:
+		/* Using CT2 output. */
+		conv = PCI230_ADC_TRIG_Z2CT2;
+		break;
+	case TRIG_EXT:
+		if (cmd->convert_arg & CR_EDGE) {
+			if ((cmd->convert_arg & CR_INVERT) == 0) {
+				/* Trigger on +ve edge. */
+				conv = PCI230_ADC_TRIG_EXTP;
 			} else {
-				/* Backwards compatibility. */
-				if (cmd->convert_arg) {
-					/* Trigger on +ve edge. */
-					conv = PCI230_ADC_TRIG_EXTP;
-				} else {
-					/* Trigger on -ve edge. */
-					conv = PCI230_ADC_TRIG_EXTN;
-				}
+				/* Trigger on -ve edge. */
+				conv = PCI230_ADC_TRIG_EXTN;
+			}
+		} else {
+			/* Backwards compatibility. */
+			if (cmd->convert_arg) {
+				/* Trigger on +ve edge. */
+				conv = PCI230_ADC_TRIG_EXTP;
+			} else {
+				/* Trigger on -ve edge. */
+				conv = PCI230_ADC_TRIG_EXTN;
 			}
-			break;
-		case TRIG_INT:
-			/*
-			 * Use CT2 output for software trigger due to problems
-			 * in differential mode on PCI230/260.
-			 */
-			conv = PCI230_ADC_TRIG_Z2CT2;
-			break;
 		}
-		devpriv->adccon =
-		    (devpriv->adccon & ~PCI230_ADC_TRIG_MASK) | conv;
-		outw(devpriv->adccon, dev->iobase + PCI230_ADCCON);
-		if (cmd->convert_src == TRIG_INT)
-			async->inttrig = pci230_ai_inttrig_convert;
-
+		break;
+	case TRIG_INT:
 		/*
-		 * Update FIFO interrupt trigger level, which is currently
-		 * set to "full".
+		 * Use CT2 output for software trigger due to problems
+		 * in differential mode on PCI230/260.
 		 */
-		pci230_ai_update_fifo_trigger_level(dev, s);
-		if (cmd->convert_src == TRIG_TIMER) {
-			/* Update timer gates. */
-			unsigned char zgat;
+		conv = PCI230_ADC_TRIG_Z2CT2;
+		break;
+	}
+	devpriv->adccon = (devpriv->adccon & ~PCI230_ADC_TRIG_MASK) | conv;
+	outw(devpriv->adccon, devpriv->daqio + PCI230_ADCCON);
+	if (cmd->convert_src == TRIG_INT)
+		async->inttrig = pci230_ai_inttrig_convert;
 
-			if (cmd->scan_begin_src != TRIG_FOLLOW) {
+	/*
+	 * Update FIFO interrupt trigger level, which is currently
+	 * set to "full".
+	 */
+	pci230_ai_update_fifo_trigger_level(dev, s);
+	if (cmd->convert_src == TRIG_TIMER) {
+		/* Update timer gates. */
+		unsigned char zgat;
+
+		if (cmd->scan_begin_src != TRIG_FOLLOW) {
+			/*
+			 * Conversion timer CT2 needs to be gated by
+			 * inverted output of monostable CT2.
+			 */
+			zgat = GAT_CONFIG(2, GAT_NOUTNM2);
+		} else {
+			/*
+			 * Conversion timer CT2 needs to be gated on
+			 * continuously.
+			 */
+			zgat = GAT_CONFIG(2, GAT_VCC);
+		}
+		outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
+		if (cmd->scan_begin_src != TRIG_FOLLOW) {
+			/* Set monostable CT0 trigger source. */
+			switch (cmd->scan_begin_src) {
+			default:
+				zgat = GAT_CONFIG(0, GAT_VCC);
+				break;
+			case TRIG_EXT:
 				/*
-				 * Conversion timer CT2 needs to be gated by
-				 * inverted output of monostable CT2.
+				 * For CT0 on PCI230, the external trigger
+				 * (gate) signal comes from PPC0, which is
+				 * channel 16 of the DIO subdevice.  The
+				 * application needs to configure this as an
+				 * input in order to use it as an external scan
+				 * trigger.
 				 */
-				zgat = GAT_CONFIG(2, GAT_NOUTNM2);
-			} else {
+				zgat = GAT_CONFIG(0, GAT_EXT);
+				break;
+			case TRIG_TIMER:
 				/*
-				 * Conversion timer CT2 needs to be gated on
-				 * continuously.
+				 * Monostable CT0 triggered by rising edge on
+				 * inverted output of CT1 (falling edge on CT1).
 				 */
-				zgat = GAT_CONFIG(2, GAT_VCC);
+				zgat = GAT_CONFIG(0, GAT_NOUTNM2);
+				break;
+			case TRIG_INT:
+				/*
+				 * Monostable CT0 is triggered by inttrig
+				 * function waggling the CT0 gate source.
+				 */
+				zgat = GAT_CONFIG(0, GAT_VCC);
+				break;
 			}
-			outb(zgat, devpriv->iobase1 + PCI230_ZGAT_SCE);
-			if (cmd->scan_begin_src != TRIG_FOLLOW) {
-				/* Set monostable CT0 trigger source. */
-				switch (cmd->scan_begin_src) {
-				default:
-					zgat = GAT_CONFIG(0, GAT_VCC);
-					break;
-				case TRIG_EXT:
-					/*
-					 * For CT0 on PCI230, the external
-					 * trigger (gate) signal comes from
-					 * PPC0, which is channel 16 of the DIO
-					 * subdevice.  The application needs to
-					 * configure this as an input in order
-					 * to use it as an external scan
-					 * trigger.
-					 */
-					zgat = GAT_CONFIG(0, GAT_EXT);
-					break;
-				case TRIG_TIMER:
-					/*
-					 * Monostable CT0 triggered by rising
-					 * edge on inverted output of CT1
-					 * (falling edge on CT1).
-					 */
-					zgat = GAT_CONFIG(0, GAT_NOUTNM2);
-					break;
-				case TRIG_INT:
-					/*
-					 * Monostable CT0 is triggered by
-					 * inttrig function waggling the CT0
-					 * gate source.
-					 */
-					zgat = GAT_CONFIG(0, GAT_VCC);
-					break;
-				}
-				outb(zgat, devpriv->iobase1 + PCI230_ZGAT_SCE);
-				switch (cmd->scan_begin_src) {
-				case TRIG_TIMER:
-					/*
-					 * Scan period timer CT1 needs to be
-					 * gated on to start counting.
-					 */
-					zgat = GAT_CONFIG(1, GAT_VCC);
-					outb(zgat, devpriv->iobase1 +
-						   PCI230_ZGAT_SCE);
-					break;
-				case TRIG_INT:
-					async->inttrig =
-					    pci230_ai_inttrig_scan_begin;
-					break;
-				}
+			outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
+			switch (cmd->scan_begin_src) {
+			case TRIG_TIMER:
+				/*
+				 * Scan period timer CT1 needs to be
+				 * gated on to start counting.
+				 */
+				zgat = GAT_CONFIG(1, GAT_VCC);
+				outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
+				break;
+			case TRIG_INT:
+				async->inttrig = pci230_ai_inttrig_scan_begin;
+				break;
 			}
-		} else if (cmd->convert_src != TRIG_INT) {
-			/* No longer need Z2-CT2. */
-			put_one_resource(dev, RES_Z2CT2, OWNER_AICMD);
 		}
+	} else if (cmd->convert_src != TRIG_INT) {
+		/* No longer need Z2-CT2. */
+		pci230_release_shared(dev, RES_Z2CT2, OWNER_AICMD);
 	}
 }
 
@@ -2218,12 +2071,11 @@ static void pci230_handle_ai(struct comedi_device *dev,
 		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
 	} else if (devpriv->ai_scan_count == 0) {
 		todo = 0;
-	} else if ((devpriv->ai_scan_count > PCI230_ADC_FIFOLEVEL_HALFFULL) ||
-		   (scanlen > PCI230_ADC_FIFOLEVEL_HALFFULL)) {
+	} else if (devpriv->ai_scan_count > PCI230_ADC_FIFOLEVEL_HALFFULL ||
+		   scanlen > PCI230_ADC_FIFOLEVEL_HALFFULL) {
 		todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
 	} else {
-		todo = (devpriv->ai_scan_count * scanlen) -
-		       devpriv->ai_scan_pos;
+		todo = devpriv->ai_scan_count * scanlen - devpriv->ai_scan_pos;
 		if (todo > PCI230_ADC_FIFOLEVEL_HALFFULL)
 			todo = PCI230_ADC_FIFOLEVEL_HALFFULL;
 	}
@@ -2233,7 +2085,7 @@ static void pci230_handle_ai(struct comedi_device *dev,
 	for (i = 0; i < todo; i++) {
 		if (fifoamount == 0) {
 			/* Read FIFO state. */
-			status_fifo = inw(dev->iobase + PCI230_ADCCON);
+			status_fifo = inw(devpriv->daqio + PCI230_ADCCON);
 			if (status_fifo & PCI230_ADC_FIFO_FULL_LATCHED) {
 				/*
 				 * Report error otherwise FIFO overruns will go
@@ -2248,19 +2100,15 @@ static void pci230_handle_ai(struct comedi_device *dev,
 			} else if (status_fifo & PCI230_ADC_FIFO_HALF) {
 				/* FIFO half full. */
 				fifoamount = PCI230_ADC_FIFOLEVEL_HALFFULL;
+			} else if (devpriv->hwver > 0) {
+				/* Read PCI230+/260+ ADC FIFO level. */
+				fifoamount = inw(devpriv->daqio +
+						 PCI230P_ADCFFLEV);
+				if (fifoamount == 0)
+					break;	/* Shouldn't happen. */
 			} else {
 				/* FIFO not empty. */
-				if (devpriv->hwver > 0) {
-					/* Read PCI230+/260+ ADC FIFO level. */
-					fifoamount =
-					    inw(dev->iobase + PCI230P_ADCFFLEV);
-					if (fifoamount == 0) {
-						/* Shouldn't happen. */
-						break;
-					}
-				} else {
-					fifoamount = 1;
-				}
+				fifoamount = 1;
 			}
 		}
 		/* Read sample and store in Comedi's circular buffer. */
@@ -2316,25 +2164,20 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 * Need Z2-CT2 to supply a conversion trigger source at a high
 	 * logic level, even if not doing timed conversions.
 	 */
-	res_mask |= (1U << RES_Z2CT2);
+	res_mask |= RES_Z2CT2;
 	if (cmd->scan_begin_src != TRIG_FOLLOW) {
 		/* Using Z2-CT0 monostable to gate Z2-CT2 conversion timer */
-		res_mask |= (1U << RES_Z2CT0);
+		res_mask |= RES_Z2CT0;
 		if (cmd->scan_begin_src == TRIG_TIMER) {
 			/* Using Z2-CT1 for scan frequency */
-			res_mask |= (1U << RES_Z2CT1);
+			res_mask |= RES_Z2CT1;
 		}
 	}
 	/* Claim resources. */
-	if (!get_resources(dev, res_mask, OWNER_AICMD))
+	if (!pci230_claim_shared(dev, res_mask, OWNER_AICMD))
 		return -EBUSY;
 
-
-	/* Get number of scans required. */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->ai_scan_count = cmd->stop_arg;
-	else	/* TRIG_NONE, user calls cancel */
-		devpriv->ai_scan_count = 0;
+	devpriv->ai_scan_count = cmd->stop_arg;
 	devpriv->ai_scan_pos = 0;	/* Position within scan. */
 
 	/*
@@ -2369,7 +2212,7 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	range = CR_RANGE(cmd->chanlist[0]);
-	devpriv->ai_bipolar = pci230_ai_bipolar[range];
+	devpriv->ai_bipolar = comedi_range_is_bipolar(s, range);
 	if (devpriv->ai_bipolar)
 		adccon |= PCI230_ADC_IR_BIP;
 	else
@@ -2396,7 +2239,7 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				adcen |= 1 << gainshift;
 			}
 		} else {
-			gainshift = (chan & ~1);
+			gainshift = chan & ~1;
 			adcen |= 1 << chan;
 		}
 		devpriv->adcg = (devpriv->adcg & ~(3 << gainshift)) |
@@ -2404,16 +2247,16 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	}
 
 	/* Set channel scan list. */
-	outw(adcen, dev->iobase + PCI230_ADCEN);
+	outw(adcen, devpriv->daqio + PCI230_ADCEN);
 
 	/* Set channel gains. */
-	outw(devpriv->adcg, dev->iobase + PCI230_ADCG);
+	outw(devpriv->adcg, devpriv->daqio + PCI230_ADCG);
 
 	/*
 	 * Set counter/timer 2 output high for use as the initial start
 	 * conversion source.
 	 */
-	i8254_set_mode(devpriv->iobase1 + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE1);
+	i8254_set_mode(dev->iobase + PCI230_Z2_CT_BASE, 0, 2, I8254_MODE1);
 
 	/*
 	 * Temporarily use CT2 output as conversion trigger source and
@@ -2429,7 +2272,7 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	 * PCI230/260, but that will be dealt with later.
 	 */
 	devpriv->adccon = adccon;
-	outw(adccon | PCI230_ADC_FIFO_RESET, dev->iobase + PCI230_ADCCON);
+	outw(adccon | PCI230_ADC_FIFO_RESET, devpriv->daqio + PCI230_ADCCON);
 
 	/*
 	 * Delay -
@@ -2443,7 +2286,7 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	usleep_range(25, 100);
 
 	/* Reset FIFO again. */
-	outw(adccon | PCI230_ADC_FIFO_RESET, dev->iobase + PCI230_ADCCON);
+	outw(adccon | PCI230_ADC_FIFO_RESET, devpriv->daqio + PCI230_ADCCON);
 
 	if (cmd->convert_src == TRIG_TIMER) {
 		/*
@@ -2452,7 +2295,7 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		 * connector: PCI230 pin 21, PCI260 pin 18.
 		 */
 		zgat = GAT_CONFIG(2, GAT_GND);
-		outb(zgat, devpriv->iobase1 + PCI230_ZGAT_SCE);
+		outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
 		/* Set counter/timer 2 to the specified conversion period. */
 		pci230_ct_setup_ns_mode(dev, 2, I8254_MODE3, cmd->convert_arg,
 					cmd->flags);
@@ -2470,11 +2313,11 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			 * source will be changed later.
 			 */
 			zgat = GAT_CONFIG(0, GAT_VCC);
-			outb(zgat, devpriv->iobase1 + PCI230_ZGAT_SCE);
+			outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
 			pci230_ct_setup_ns_mode(dev, 0, I8254_MODE1,
 						((uint64_t)cmd->convert_arg *
 						 cmd->scan_end_arg),
-						TRIG_ROUND_UP);
+						CMDF_ROUND_UP);
 			if (cmd->scan_begin_src == TRIG_TIMER) {
 				/*
 				 * Monostable on CT0 will be triggered by
@@ -2483,7 +2326,7 @@ static int pci230_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 				 * Set up CT1 but gate it off for now.
 				 */
 				zgat = GAT_CONFIG(1, GAT_GND);
-				outb(zgat, devpriv->iobase1 + PCI230_ZGAT_SCE);
+				outb(zgat, dev->iobase + PCI230_ZGAT_SCE);
 				pci230_ct_setup_ns_mode(dev, 1, I8254_MODE3,
 							cmd->scan_begin_arg,
 							cmd->flags);
@@ -2509,29 +2352,28 @@ static int pci230_ai_cancel(struct comedi_device *dev,
 /* Interrupt handler */
 static irqreturn_t pci230_interrupt(int irq, void *d)
 {
-	unsigned char status_int, valid_status_int;
+	unsigned char status_int, valid_status_int, temp_ier;
 	struct comedi_device *dev = (struct comedi_device *)d;
 	struct pci230_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	unsigned long irqflags;
 
 	/* Read interrupt status/enable register. */
-	status_int = inb(devpriv->iobase1 + PCI230_INT_STAT);
+	status_int = inb(dev->iobase + PCI230_INT_STAT);
 
 	if (status_int == PCI230_INT_DISABLE)
 		return IRQ_NONE;
 
-
 	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-	valid_status_int = devpriv->int_en & status_int;
+	valid_status_int = devpriv->ier & status_int;
 	/*
 	 * Disable triggered interrupts.
 	 * (Only those interrupts that need re-enabling, are, later in the
 	 * handler).
 	 */
-	devpriv->ier = devpriv->int_en & ~status_int;
-	outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
-	devpriv->intr_running = 1;
+	temp_ier = devpriv->ier & ~status_int;
+	outb(temp_ier, dev->iobase + PCI230_INT_SCE);
+	devpriv->intr_running = true;
 	devpriv->intr_cpuid = THISCPU;
 	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 
@@ -2563,11 +2405,9 @@ static irqreturn_t pci230_interrupt(int irq, void *d)
 
 	/* Reenable interrupts. */
 	spin_lock_irqsave(&devpriv->isr_spinlock, irqflags);
-	if (devpriv->ier != devpriv->int_en) {
-		devpriv->ier = devpriv->int_en;
-		outb(devpriv->ier, devpriv->iobase1 + PCI230_INT_SCE);
-	}
-	devpriv->intr_running = 0;
+	if (devpriv->ier != temp_ier)
+		outb(devpriv->ier, dev->iobase + PCI230_INT_SCE);
+	devpriv->intr_running = false;
 	spin_unlock_irqrestore(&devpriv->isr_spinlock, irqflags);
 
 	return IRQ_HANDLED;
@@ -2603,49 +2443,17 @@ static const struct pci230_board *pci230_find_pci_board(struct pci_dev *pci_dev)
 	return NULL;
 }
 
-/* Look for PCI device matching requested board name, bus and slot. */
-static struct pci_dev *pci230_find_pci_dev(struct comedi_device *dev,
-					   struct comedi_devconfig *it)
-{
-	const struct pci230_board *thisboard = comedi_board(dev);
-	struct pci_dev *pci_dev = NULL;
-	int bus = it->options[0];
-	int slot = it->options[1];
-
-	for_each_pci_dev(pci_dev) {
-		/* Check vendor ID (same for all supported PCI boards). */
-		if (pci_dev->vendor != PCI_VENDOR_ID_AMPLICON)
-			continue;
-		/* If bus/slot specified, check them. */
-		if ((bus || slot) &&
-		    (bus != pci_dev->bus->number ||
-		     slot != PCI_SLOT(pci_dev->devfn)))
-			continue;
-		if (thisboard->id == PCI_DEVICE_ID_INVALID) {
-			/* Wildcard board matches any supported PCI board. */
-			const struct pci230_board *foundboard;
-
-			foundboard = pci230_find_pci_board(pci_dev);
-			if (foundboard == NULL)
-				continue;
-			/* Replace wildcard board_ptr. */
-			dev->board_ptr = foundboard;
-		} else {
-			/* Need to match a specific board. */
-			if (!pci230_match_pci_board(thisboard, pci_dev))
-				continue;
-		}
-		return pci_dev;
-	}
-	dev_err(dev->class_dev,
-		"No supported board found! (req. bus %d, slot %d)\n",
-		bus, slot);
-	return NULL;
-}
-
-static int pci230_alloc_private(struct comedi_device *dev)
+static int pci230_auto_attach(struct comedi_device *dev,
+			      unsigned long context_unused)
 {
+	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
+	const struct pci230_board *thisboard;
 	struct pci230_private *devpriv;
+	struct comedi_subdevice *s;
+	int rc;
+
+	dev_info(dev->class_dev, "amplc_pci230: attach pci %s\n",
+		 pci_name(pci_dev));
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -2655,22 +2463,14 @@ static int pci230_alloc_private(struct comedi_device *dev)
 	spin_lock_init(&devpriv->res_spinlock);
 	spin_lock_init(&devpriv->ai_stop_spinlock);
 	spin_lock_init(&devpriv->ao_stop_spinlock);
-	return 0;
-}
-
-/* Common part of attach and auto_attach. */
-static int pci230_attach_common(struct comedi_device *dev,
-				struct pci_dev *pci_dev)
-{
-	const struct pci230_board *thisboard = comedi_board(dev);
-	struct pci230_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
-	unsigned long iobase1, iobase2;
-	/* PCI230's I/O spaces 1 and 2 respectively. */
-	int rc;
-
-	comedi_set_hw_dev(dev, &pci_dev->dev);
 
+	dev->board_ptr = pci230_find_pci_board(pci_dev);
+	if (dev->board_ptr == NULL) {
+		dev_err(dev->class_dev,
+			"amplc_pci230: BUG! cannot determine board type!\n");
+		return -EINVAL;
+	}
+	thisboard = dev->board_ptr;
 	dev->board_name = thisboard->name;
 
 	rc = comedi_pci_enable(dev);
@@ -2681,15 +2481,14 @@ static int pci230_attach_common(struct comedi_device *dev,
 	 * Read base addresses of the PCI230's two I/O regions from PCI
 	 * configuration register.
 	 */
-	iobase1 = pci_resource_start(pci_dev, 2);
-	iobase2 = pci_resource_start(pci_dev, 3);
+	dev->iobase = pci_resource_start(pci_dev, 2);
+	devpriv->daqio = pci_resource_start(pci_dev, 3);
 	dev_dbg(dev->class_dev,
 		"%s I/O region 1 0x%04lx I/O region 2 0x%04lx\n",
-		dev->board_name, iobase1, iobase2);
-	devpriv->iobase1 = iobase1;
-	dev->iobase = iobase2;
+		dev->board_name, dev->iobase, devpriv->daqio);
 	/* Read bits of DACCON register - only the output range. */
-	devpriv->daccon = inw(dev->iobase + PCI230_DACCON) & PCI230_DAC_OR_MASK;
+	devpriv->daccon = inw(devpriv->daqio + PCI230_DACCON) &
+			  PCI230_DAC_OR_MASK;
 	/*
 	 * Read hardware version register and set extended function register
 	 * if they exist.
@@ -2697,7 +2496,7 @@ static int pci230_attach_common(struct comedi_device *dev,
 	if (pci_resource_len(pci_dev, 3) >= 32) {
 		unsigned short extfunc = 0;
 
-		devpriv->hwver = inw(dev->iobase + PCI230P_HWVER);
+		devpriv->hwver = inw(devpriv->daqio + PCI230P_HWVER);
 		if (devpriv->hwver < thisboard->min_hwver) {
 			dev_err(dev->class_dev,
 				"%s - bad hardware version - got %u, need %u\n",
@@ -2716,13 +2515,12 @@ static int pci230_attach_common(struct comedi_device *dev,
 				 */
 				extfunc |= PCI230P_EXTFUNC_GAT_EXTTRIG;
 			}
-			if ((thisboard->ao_chans > 0) &&
-			    (devpriv->hwver >= 2)) {
+			if (thisboard->ao_bits && devpriv->hwver >= 2) {
 				/* Enable DAC FIFO functionality. */
 				extfunc |= PCI230P2_EXTFUNC_DACFIFO;
 			}
 		}
-		outw(extfunc, dev->iobase + PCI230P_EXTFUNC);
+		outw(extfunc, devpriv->daqio + PCI230P_EXTFUNC);
 		if (extfunc & PCI230P2_EXTFUNC_DACFIFO) {
 			/*
 			 * Temporarily enable DAC FIFO, reset it and disable
@@ -2730,23 +2528,23 @@ static int pci230_attach_common(struct comedi_device *dev,
 			 */
 			outw(devpriv->daccon | PCI230P2_DAC_FIFO_EN |
 			     PCI230P2_DAC_FIFO_RESET,
-			     dev->iobase + PCI230_DACCON);
+			     devpriv->daqio + PCI230_DACCON);
 			/* Clear DAC FIFO channel enable register. */
-			outw(0, dev->iobase + PCI230P2_DACEN);
+			outw(0, devpriv->daqio + PCI230P2_DACEN);
 			/* Disable DAC FIFO. */
-			outw(devpriv->daccon, dev->iobase + PCI230_DACCON);
+			outw(devpriv->daccon, devpriv->daqio + PCI230_DACCON);
 		}
 	}
 	/* Disable board's interrupts. */
-	outb(0, devpriv->iobase1 + PCI230_INT_SCE);
+	outb(0, dev->iobase + PCI230_INT_SCE);
 	/* Set ADC to a reasonable state. */
 	devpriv->adcg = 0;
 	devpriv->adccon = PCI230_ADC_TRIG_NONE | PCI230_ADC_IM_SE |
 			  PCI230_ADC_IR_BIP;
-	outw(1 << 0, dev->iobase + PCI230_ADCEN);
-	outw(devpriv->adcg, dev->iobase + PCI230_ADCG);
+	outw(1 << 0, devpriv->daqio + PCI230_ADCEN);
+	outw(devpriv->adcg, devpriv->daqio + PCI230_ADCG);
 	outw(devpriv->adccon | PCI230_ADC_FIFO_RESET,
-	     dev->iobase + PCI230_ADCCON);
+	     devpriv->daqio + PCI230_ADCCON);
 
 	if (pci_dev->irq) {
 		rc = request_irq(pci_dev->irq, pci230_interrupt, IRQF_SHARED,
@@ -2763,10 +2561,10 @@ static int pci230_attach_common(struct comedi_device *dev,
 	/* analog input subdevice */
 	s->type = COMEDI_SUBD_AI;
 	s->subdev_flags = SDF_READABLE | SDF_DIFF | SDF_GROUND;
-	s->n_chan = thisboard->ai_chans;
+	s->n_chan = 16;
 	s->maxdata = (1 << thisboard->ai_bits) - 1;
 	s->range_table = &pci230_ai_range;
-	s->insn_read = pci230_ai_rinsn;
+	s->insn_read = pci230_ai_insn_read;
 	s->len_chanlist = 256;	/* but there are restrictions. */
 	if (dev->irq) {
 		dev->read_subdev = s;
@@ -2778,15 +2576,15 @@ static int pci230_attach_common(struct comedi_device *dev,
 
 	s = &dev->subdevices[1];
 	/* analog output subdevice */
-	if (thisboard->ao_chans > 0) {
+	if (thisboard->ao_bits) {
 		s->type = COMEDI_SUBD_AO;
 		s->subdev_flags = SDF_WRITABLE | SDF_GROUND;
-		s->n_chan = thisboard->ao_chans;
+		s->n_chan = 2;
 		s->maxdata = (1 << thisboard->ao_bits) - 1;
 		s->range_table = &pci230_ao_range;
-		s->insn_write = pci230_ao_winsn;
-		s->insn_read = pci230_ao_rinsn;
-		s->len_chanlist = thisboard->ao_chans;
+		s->insn_write = pci230_ao_insn_write;
+		s->insn_read = comedi_readback_insn_read;
+		s->len_chanlist = 2;
 		if (dev->irq) {
 			dev->write_subdev = s;
 			s->subdev_flags |= SDF_CMD_WRITE;
@@ -2794,6 +2592,10 @@ static int pci230_attach_common(struct comedi_device *dev,
 			s->do_cmdtest = pci230_ao_cmdtest;
 			s->cancel = pci230_ao_cancel;
 		}
+
+		rc = comedi_alloc_subdev_readback(s);
+		if (rc)
+			return rc;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
@@ -2801,8 +2603,7 @@ static int pci230_attach_common(struct comedi_device *dev,
 	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
 	if (thisboard->have_dio) {
-		rc = subdev_8255_init(dev, s, NULL,
-				      devpriv->iobase1 + PCI230_PPI_X_BASE);
+		rc = subdev_8255_init(dev, s, NULL, PCI230_PPI_X_BASE);
 		if (rc)
 			return rc;
 	} else {
@@ -2812,74 +2613,11 @@ static int pci230_attach_common(struct comedi_device *dev,
 	return 0;
 }
 
-static int pci230_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	const struct pci230_board *thisboard = comedi_board(dev);
-	struct pci_dev *pci_dev;
-	int rc;
-
-	dev_info(dev->class_dev, "amplc_pci230: attach %s %d,%d\n",
-		 thisboard->name, it->options[0], it->options[1]);
-
-	rc = pci230_alloc_private(dev);
-	if (rc)
-		return rc;
-
-	pci_dev = pci230_find_pci_dev(dev, it);
-	if (!pci_dev)
-		return -EIO;
-	return pci230_attach_common(dev, pci_dev);
-}
-
-static int pci230_auto_attach(struct comedi_device *dev,
-			      unsigned long context_unused)
-{
-	struct pci_dev *pci_dev = comedi_to_pci_dev(dev);
-	int rc;
-
-	dev_info(dev->class_dev, "amplc_pci230: attach pci %s\n",
-		 pci_name(pci_dev));
-
-	rc = pci230_alloc_private(dev);
-	if (rc)
-		return rc;
-
-	dev->board_ptr = pci230_find_pci_board(pci_dev);
-	if (dev->board_ptr == NULL) {
-		dev_err(dev->class_dev,
-			"amplc_pci230: BUG! cannot determine board type!\n");
-		return -EINVAL;
-	}
-	/*
-	 * Need to 'get' the PCI device to match the 'put' in pci230_detach().
-	 * TODO: Remove the pci_dev_get() and matching pci_dev_put() once
-	 * support for manual attachment of PCI devices via pci230_attach()
-	 * has been removed.
-	 */
-	pci_dev_get(pci_dev);
-	return pci230_attach_common(dev, pci_dev);
-}
-
-static void pci230_detach(struct comedi_device *dev)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
-	if (pcidev)
-		pci_dev_put(pcidev);
-}
-
 static struct comedi_driver amplc_pci230_driver = {
 	.driver_name	= "amplc_pci230",
 	.module		= THIS_MODULE,
-	.attach		= pci230_attach,
 	.auto_attach	= pci230_auto_attach,
-	.detach		= pci230_detach,
-	.board_name	= &pci230_boards[0].name,
-	.offset		= sizeof(pci230_boards[0]),
-	.num_names	= ARRAY_SIZE(pci230_boards),
+	.detach		= comedi_pci_detach,
 };
 
 static int amplc_pci230_pci_probe(struct pci_dev *dev,
@@ -2905,5 +2643,5 @@ static struct pci_driver amplc_pci230_pci_driver = {
 module_comedi_pci_driver(amplc_pci230_driver, amplc_pci230_pci_driver);
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for Amplicon PCI230(+) and PCI260(+)");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/amplc_pci236.c b/drivers/staging/comedi/drivers/amplc_pci236.c
index 436aebaf7621..ad1e93dd13a0 100644
--- a/drivers/staging/comedi/drivers/amplc_pci236.c
+++ b/drivers/staging/comedi/drivers/amplc_pci236.c
@@ -119,18 +119,11 @@ static int pci236_auto_attach(struct comedi_device *dev,
 					 IRQF_SHARED);
 }
 
-static void pci236_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
-}
-
 static struct comedi_driver amplc_pci236_driver = {
 	.driver_name = "amplc_pci236",
 	.module = THIS_MODULE,
 	.auto_attach = pci236_auto_attach,
-	.detach = pci236_detach,
+	.detach = comedi_pci_detach,
 };
 
 static const struct pci_device_id pci236_pci_table[] = {
diff --git a/drivers/staging/comedi/drivers/amplc_pci263.c b/drivers/staging/comedi/drivers/amplc_pci263.c
index 748a6b108f32..2259bee98d48 100644
--- a/drivers/staging/comedi/drivers/amplc_pci263.c
+++ b/drivers/staging/comedi/drivers/amplc_pci263.c
@@ -86,7 +86,7 @@ static struct comedi_driver amplc_pci263_driver = {
 	.driver_name	= "amplc_pci263",
 	.module		= THIS_MODULE,
 	.auto_attach	= pci263_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static const struct pci_device_id pci263_pci_table[] = {
diff --git a/drivers/staging/comedi/drivers/cb_das16_cs.c b/drivers/staging/comedi/drivers/cb_das16_cs.c
index 853733e28845..f88880aea6da 100644
--- a/drivers/staging/comedi/drivers/cb_das16_cs.c
+++ b/drivers/staging/comedi/drivers/cb_das16_cs.c
@@ -79,7 +79,6 @@ static const struct das16cs_board das16cs_boards[] = {
 };
 
 struct das16cs_private {
-	unsigned int ao_readback[2];
 	unsigned short status1;
 	unsigned short status2;
 };
@@ -153,20 +152,20 @@ static int das16cs_ai_rinsn(struct comedi_device *dev,
 	return i;
 }
 
-static int das16cs_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int das16cs_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
 	struct das16cs_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	unsigned short status1;
-	unsigned short d;
 	int bit;
+	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		devpriv->ao_readback[chan] = data[i];
-		d = data[i];
+		val = data[i];
 
 		outw(devpriv->status1, dev->iobase + DAS16CS_MISC1);
 		udelay(1);
@@ -181,7 +180,7 @@ static int das16cs_ao_winsn(struct comedi_device *dev,
 		udelay(1);
 
 		for (bit = 15; bit >= 0; bit--) {
-			int b = (d >> bit) & 0x1;
+			int b = (val >> bit) & 0x1;
 
 			b <<= 1;
 			outw(status1 | b | 0x0000, dev->iobase + DAS16CS_MISC1);
@@ -195,22 +194,9 @@ static int das16cs_ao_winsn(struct comedi_device *dev,
 		 */
 		outw(status1 | 0x9, dev->iobase + DAS16CS_MISC1);
 	}
+	s->readback[chan] = val;
 
-	return i;
-}
-
-static int das16cs_ao_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
-{
-	struct das16cs_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static int das16cs_dio_insn_bits(struct comedi_device *dev,
@@ -318,8 +304,12 @@ static int das16cs_auto_attach(struct comedi_device *dev,
 		s->n_chan	= board->n_ao_chans;
 		s->maxdata	= 0xffff;
 		s->range_table	= &range_bipolar10;
-		s->insn_write	= &das16cs_ao_winsn;
-		s->insn_read	= &das16cs_ao_rinsn;
+		s->insn_write	= &das16cs_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
diff --git a/drivers/staging/comedi/drivers/cb_pcidas.c b/drivers/staging/comedi/drivers/cb_pcidas.c
index 4a7bd4e5dd72..1ec363b7505c 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas.c
@@ -342,7 +342,6 @@ struct cb_pcidas_private {
 	unsigned long s5933_config;
 	unsigned long control_status;
 	unsigned long adc_fifo;
-	unsigned long pacer_counter_dio;
 	unsigned long ao_registers;
 	/* divisors of master clock for analog input pacing */
 	unsigned int divisor1;
@@ -361,8 +360,6 @@ struct cb_pcidas_private {
 	unsigned int ao_divisor2;
 	/* number of analog output samples remaining */
 	unsigned int ao_count;
-	/* cached values for readback */
-	unsigned short ao_value[2];
 	unsigned int caldac_value[NUM_CHANNELS_8800];
 	unsigned int trimpot_value[NUM_CHANNELS_8402];
 	unsigned int dac08_value;
@@ -485,7 +482,7 @@ static int cb_pcidas_ao_nofifo_winsn(struct comedi_device *dev,
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
 	/* remember value for readback */
-	devpriv->ao_value[chan] = data[0];
+	s->readback[chan] = data[0];
 
 	/* send data */
 	outw(data[0], devpriv->ao_registers + DAC_DATA_REG(chan));
@@ -516,7 +513,7 @@ static int cb_pcidas_ao_fifo_winsn(struct comedi_device *dev,
 	spin_unlock_irqrestore(&dev->spinlock, flags);
 
 	/* remember value for readback */
-	devpriv->ao_value[chan] = data[0];
+	s->readback[chan] = data[0];
 
 	/* send data */
 	outw(data[0], devpriv->ao_registers + DACDATA);
@@ -524,18 +521,6 @@ static int cb_pcidas_ao_fifo_winsn(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int cb_pcidas_ao_readback_insn(struct comedi_device *dev,
-				      struct comedi_subdevice *s,
-				      struct comedi_insn *insn,
-				      unsigned int *data)
-{
-	struct cb_pcidas_private *devpriv = dev->private;
-
-	data[0] = devpriv->ao_value[CR_CHAN(insn->chanspec)];
-
-	return 1;
-}
-
 static int wait_for_nvram_ready(unsigned long s5933_base_addr)
 {
 	static const int timeout = 1000;
@@ -758,7 +743,7 @@ static int trimpot_8402_write(struct comedi_device *dev, unsigned int channel,
 static int cb_pcidas_trimpot_write(struct comedi_device *dev,
 				   unsigned int channel, unsigned int value)
 {
-	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 
 	if (devpriv->trimpot_value[channel] == value)
@@ -832,7 +817,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
-	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
@@ -901,7 +886,9 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_NONE)
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -942,7 +929,7 @@ static int cb_pcidas_ai_cmdtest(struct comedi_device *dev,
 static void cb_pcidas_ai_load_counters(struct comedi_device *dev)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
-	unsigned long timer_base = devpriv->pacer_counter_dio + ADC8254;
+	unsigned long timer_base = dev->iobase + ADC8254;
 
 	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
 	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
@@ -954,7 +941,7 @@ static void cb_pcidas_ai_load_counters(struct comedi_device *dev)
 static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
-	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -996,7 +983,7 @@ static int cb_pcidas_ai_cmd(struct comedi_device *dev,
 	spin_lock_irqsave(&dev->spinlock, flags);
 	devpriv->adc_fifo_bits |= INTE;
 	devpriv->adc_fifo_bits &= ~INT_MASK;
-	if (cmd->flags & TRIG_WAKE_EOS) {
+	if (cmd->flags & CMDF_WAKE_EOS) {
 		if (cmd->convert_src == TRIG_NOW && cmd->chanlist_len > 1) {
 			/* interrupt end of burst */
 			devpriv->adc_fifo_bits |= INT_EOS;
@@ -1057,7 +1044,7 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
-	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
@@ -1094,7 +1081,9 @@ static int cb_pcidas_ao_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_NONE)
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -1149,7 +1138,7 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				unsigned int trig_num)
 {
-	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	unsigned int num_bytes, num_points = thisboard->fifo_size;
 	struct comedi_async *async = s->async;
@@ -1194,7 +1183,7 @@ static int cb_pcidas_ao_inttrig(struct comedi_device *dev,
 static void cb_pcidas_ao_load_counters(struct comedi_device *dev)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
-	unsigned long timer_base = devpriv->pacer_counter_dio + DAC8254;
+	unsigned long timer_base = dev->iobase + DAC8254;
 
 	i8254_set_mode(timer_base, 0, 1, I8254_MODE2 | I8254_BINARY);
 	i8254_set_mode(timer_base, 0, 2, I8254_MODE2 | I8254_BINARY);
@@ -1281,7 +1270,7 @@ static int cb_pcidas_ao_cancel(struct comedi_device *dev,
 
 static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 {
-	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->write_subdev;
 	struct comedi_async *async = s->async;
@@ -1336,7 +1325,7 @@ static void handle_ao_interrupt(struct comedi_device *dev, unsigned int status)
 static irqreturn_t cb_pcidas_interrupt(int irq, void *d)
 {
 	struct comedi_device *dev = (struct comedi_device *)d;
-	const struct cb_pcidas_board *thisboard = comedi_board(dev);
+	const struct cb_pcidas_board *thisboard = dev->board_ptr;
 	struct cb_pcidas_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async;
@@ -1463,7 +1452,7 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 	devpriv->s5933_config = pci_resource_start(pcidev, 0);
 	devpriv->control_status = pci_resource_start(pcidev, 1);
 	devpriv->adc_fifo = pci_resource_start(pcidev, 2);
-	devpriv->pacer_counter_dio = pci_resource_start(pcidev, 3);
+	dev->iobase = pci_resource_start(pcidev, 3);
 	if (thisboard->ao_nchan)
 		devpriv->ao_registers = pci_resource_start(pcidev, 4);
 
@@ -1512,16 +1501,22 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 		 */
 		s->maxdata = (1 << thisboard->ai_bits) - 1;
 		s->range_table = &cb_pcidas_ao_ranges;
-		s->insn_read = cb_pcidas_ao_readback_insn;
+		/* default to no fifo (*insn_write) */
+		s->insn_write = cb_pcidas_ao_nofifo_winsn;
+		s->insn_read = comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
 		if (thisboard->has_ao_fifo) {
 			dev->write_subdev = s;
 			s->subdev_flags |= SDF_CMD_WRITE;
+			/* use fifo (*insn_write) instead */
 			s->insn_write = cb_pcidas_ao_fifo_winsn;
 			s->do_cmdtest = cb_pcidas_ao_cmdtest;
 			s->do_cmd = cb_pcidas_ao_cmd;
 			s->cancel = cb_pcidas_ao_cancel;
-		} else {
-			s->insn_write = cb_pcidas_ao_nofifo_winsn;
 		}
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
@@ -1529,8 +1524,7 @@ static int cb_pcidas_auto_attach(struct comedi_device *dev,
 
 	/* 8255 */
 	s = &dev->subdevices[2];
-	ret = subdev_8255_init(dev, s, NULL,
-			       devpriv->pacer_counter_dio + DIO_8255);
+	ret = subdev_8255_init(dev, s, NULL, DIO_8255);
 	if (ret)
 		return ret;
 
@@ -1599,15 +1593,11 @@ static void cb_pcidas_detach(struct comedi_device *dev)
 {
 	struct cb_pcidas_private *devpriv = dev->private;
 
-	if (devpriv) {
-		if (devpriv->s5933_config) {
-			outl(INTCSR_INBOX_INTR_STATUS,
-			     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
-		}
+	if (devpriv && devpriv->s5933_config) {
+		outl(INTCSR_INBOX_INTR_STATUS,
+		     devpriv->s5933_config + AMCC_OP_REG_INTCSR);
 	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver cb_pcidas_driver = {
diff --git a/drivers/staging/comedi/drivers/cb_pcidas64.c b/drivers/staging/comedi/drivers/cb_pcidas64.c
index fa12614cef2a..3b6bffc66918 100644
--- a/drivers/staging/comedi/drivers/cb_pcidas64.c
+++ b/drivers/staging/comedi/drivers/cb_pcidas64.c
@@ -1038,7 +1038,7 @@ static const struct pcidas64_board pcidas64_boards[] = {
 static inline unsigned short se_diff_bit_6xxx(struct comedi_device *dev,
 					      int use_differential)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 
 	if ((thisboard->layout == LAYOUT_64XX && !use_differential) ||
 	    (thisboard->layout == LAYOUT_60XX && use_differential))
@@ -1089,8 +1089,6 @@ struct pcidas64_private {
 	unsigned int ao_dma_index;
 	/*  number of analog output samples remaining */
 	unsigned long ao_count;
-	/*  remember what the analog outputs are set to, to allow readback */
-	unsigned int ao_value[2];
 	unsigned int hw_revision;	/*  stc chip hardware revision number */
 	/*  last bits sent to INTR_ENABLE_REG register */
 	unsigned int intr_enable_bits;
@@ -1123,7 +1121,7 @@ struct pcidas64_private {
 static unsigned int ai_range_bits_6xxx(const struct comedi_device *dev,
 				       unsigned int range_index)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	const struct comedi_krange *range =
 		&thisboard->ai_range_table->range[range_index];
 	unsigned int bits = 0;
@@ -1168,7 +1166,7 @@ static unsigned int ai_range_bits_6xxx(const struct comedi_device *dev,
 static unsigned int hw_revision(const struct comedi_device *dev,
 				uint16_t hw_status_bits)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 
 	if (thisboard->layout == LAYOUT_4020)
 		return (hw_status_bits >> 13) & 0x7;
@@ -1180,7 +1178,7 @@ static void set_dac_range_bits(struct comedi_device *dev,
 			       uint16_t *bits, unsigned int channel,
 			       unsigned int range)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	unsigned int code = thisboard->ao_range_code[range];
 
 	if (channel > 1)
@@ -1237,7 +1235,7 @@ static void disable_ai_interrupts(struct comedi_device *dev)
 static void enable_ai_interrupts(struct comedi_device *dev,
 				 const struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint32_t bits;
 	unsigned long flags;
@@ -1245,8 +1243,8 @@ static void enable_ai_interrupts(struct comedi_device *dev,
 	bits = EN_ADC_OVERRUN_BIT | EN_ADC_DONE_INTR_BIT |
 	       EN_ADC_ACTIVE_INTR_BIT | EN_ADC_STOP_INTR_BIT;
 	/*  Use pio transfer and interrupt on end of conversion
-	 *  if TRIG_WAKE_EOS flag is set. */
-	if (cmd->flags & TRIG_WAKE_EOS) {
+	 *  if CMDF_WAKE_EOS flag is set. */
+	if (cmd->flags & CMDF_WAKE_EOS) {
 		/*  4020 doesn't support pio transfers except for fifo dregs */
 		if (thisboard->layout != LAYOUT_4020)
 			bits |= ADC_INTR_EOSCAN_BITS | EN_ADC_INTR_SRC_BIT;
@@ -1261,7 +1259,7 @@ static void enable_ai_interrupts(struct comedi_device *dev,
 /* initialize plx9080 chip */
 static void init_plx9080(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint32_t bits;
 	void __iomem *plx_iobase = devpriv->plx9080_iobase;
@@ -1339,7 +1337,7 @@ static void disable_ai_pacing(struct comedi_device *dev)
 static int set_ai_fifo_segment_length(struct comedi_device *dev,
 				      unsigned int num_entries)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	static const int increment_size = 0x100;
 	const struct hw_fifo_info *const fifo = thisboard->ai_fifo;
@@ -1368,7 +1366,7 @@ static int set_ai_fifo_segment_length(struct comedi_device *dev,
 /* adjusts the size of hardware fifo (which determines block size for dma xfers) */
 static int set_ai_fifo_size(struct comedi_device *dev, unsigned int num_samples)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	unsigned int num_fifo_entries;
 	int retval;
 	const struct hw_fifo_info *const fifo = thisboard->ai_fifo;
@@ -1389,7 +1387,7 @@ static int set_ai_fifo_size(struct comedi_device *dev, unsigned int num_samples)
 /* query length of fifo */
 static unsigned int ai_fifo_size(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 
 	return devpriv->ai_fifo_segment_length *
@@ -1399,7 +1397,7 @@ static unsigned int ai_fifo_size(struct comedi_device *dev)
 
 static void init_stc_registers(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint16_t bits;
 	unsigned long flags;
@@ -1445,7 +1443,7 @@ static void init_stc_registers(struct comedi_device *dev)
 
 static int alloc_and_init_dma_members(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct pcidas64_private *devpriv = dev->private;
 	int i;
@@ -1526,6 +1524,46 @@ static int alloc_and_init_dma_members(struct comedi_device *dev)
 	return 0;
 }
 
+static void cb_pcidas64_free_dma(struct comedi_device *dev)
+{
+	const struct pcidas64_board *thisboard = dev->board_ptr;
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct pcidas64_private *devpriv = dev->private;
+	int i;
+
+	if (!devpriv)
+		return;
+
+	/* free pci dma buffers */
+	for (i = 0; i < ai_dma_ring_count(thisboard); i++) {
+		if (devpriv->ai_buffer[i])
+			pci_free_consistent(pcidev,
+					    DMA_BUFFER_SIZE,
+					    devpriv->ai_buffer[i],
+					    devpriv->ai_buffer_bus_addr[i]);
+	}
+	for (i = 0; i < AO_DMA_RING_COUNT; i++) {
+		if (devpriv->ao_buffer[i])
+			pci_free_consistent(pcidev,
+					    DMA_BUFFER_SIZE,
+					    devpriv->ao_buffer[i],
+					    devpriv->ao_buffer_bus_addr[i]);
+	}
+	/* free dma descriptors */
+	if (devpriv->ai_dma_desc)
+		pci_free_consistent(pcidev,
+				    sizeof(struct plx_dma_desc) *
+				    ai_dma_ring_count(thisboard),
+				    devpriv->ai_dma_desc,
+				    devpriv->ai_dma_desc_bus_addr);
+	if (devpriv->ao_dma_desc)
+		pci_free_consistent(pcidev,
+				    sizeof(struct plx_dma_desc) *
+				    AO_DMA_RING_COUNT,
+				    devpriv->ao_dma_desc,
+				    devpriv->ao_dma_desc_bus_addr);
+}
+
 static inline void warn_external_queue(struct comedi_device *dev)
 {
 	dev_err(dev->class_dev,
@@ -1668,7 +1706,7 @@ static int cb_pcidas64_ai_eoc(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned long context)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int status;
 
@@ -1687,7 +1725,7 @@ static int cb_pcidas64_ai_eoc(struct comedi_device *dev,
 static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int bits = 0, n;
 	unsigned int channel, range, aref;
@@ -1807,7 +1845,7 @@ static int ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 static int ai_config_calibration_source(struct comedi_device *dev,
 					unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int source = data[1];
 	int num_calibration_sources;
@@ -1829,7 +1867,7 @@ static int ai_config_calibration_source(struct comedi_device *dev,
 
 static int ai_config_block_size(struct comedi_device *dev, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	int fifo_size;
 	const struct hw_fifo_info *const fifo = thisboard->ai_fifo;
 	unsigned int block_size, requested_block_size;
@@ -1883,7 +1921,7 @@ static int ai_config_master_clock_4020(struct comedi_device *dev,
 /* XXX could add support for 60xx series */
 static int ai_config_master_clock(struct comedi_device *dev, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 
 	switch (thisboard->layout) {
 	case LAYOUT_4020:
@@ -1920,14 +1958,14 @@ static unsigned int get_divisor(unsigned int ns, unsigned int flags)
 {
 	unsigned int divisor;
 
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_UP:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_UP:
 		divisor = (ns + TIMER_BASE - 1) / TIMER_BASE;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		divisor = ns / TIMER_BASE;
 		break;
-	case TRIG_ROUND_NEAREST:
+	case CMDF_ROUND_NEAREST:
 	default:
 		divisor = (ns + TIMER_BASE / 2) / TIMER_BASE;
 		break;
@@ -1941,7 +1979,7 @@ static unsigned int get_divisor(unsigned int ns, unsigned int flags)
  */
 static void check_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	unsigned int convert_divisor = 0, scan_divisor;
 	static const int min_convert_divisor = 3;
 	static const int max_convert_divisor =
@@ -1989,7 +2027,7 @@ static int cb_pcidas64_ai_check_chanlist(struct comedi_device *dev,
 					 struct comedi_subdevice *s,
 					 struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *board = comedi_board(dev);
+	const struct pcidas64_board *board = dev->board_ptr;
 	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
 	int i;
 
@@ -2028,7 +2066,7 @@ static int cb_pcidas64_ai_check_chanlist(struct comedi_device *dev,
 static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	int err = 0;
 	unsigned int tmp_arg, tmp_arg2;
 	unsigned int triggers;
@@ -2178,7 +2216,7 @@ static void setup_sample_counters(struct comedi_device *dev,
 
 static inline unsigned int dma_transfer_size(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned int num_samples;
 
@@ -2265,7 +2303,7 @@ static void select_master_clock_4020(struct comedi_device *dev,
 static void select_master_clock(struct comedi_device *dev,
 				const struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 
 	switch (thisboard->layout) {
 	case LAYOUT_4020:
@@ -2297,7 +2335,7 @@ static inline void dma_start_sync(struct comedi_device *dev,
 
 static void set_ai_pacing(struct comedi_device *dev, struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	uint32_t convert_counter = 0, scan_counter = 0;
 
@@ -2346,7 +2384,7 @@ static int use_internal_queue_6xxx(const struct comedi_cmd *cmd)
 static int setup_channel_queue(struct comedi_device *dev,
 			       const struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	unsigned short bits;
 	int i;
@@ -2483,7 +2521,7 @@ static inline void load_first_dma_descriptor(struct comedi_device *dev,
 
 static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -2541,7 +2579,7 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	/*  clear adc buffer */
 	writew(0, devpriv->main_iobase + ADC_BUFFER_CLEAR_REG);
 
-	if ((cmd->flags & TRIG_WAKE_EOS) == 0 ||
+	if ((cmd->flags & CMDF_WAKE_EOS) == 0 ||
 	    thisboard->layout == LAYOUT_4020) {
 		devpriv->ai_dma_index = 0;
 
@@ -2575,7 +2613,7 @@ static int ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	/* enable pacing, triggering, etc */
 	bits = ADC_ENABLE_BIT | ADC_SOFT_GATE_BITS | ADC_GATE_LEVEL_BIT;
-	if (cmd->flags & TRIG_WAKE_EOS)
+	if (cmd->flags & CMDF_WAKE_EOS)
 		bits |= ADC_DMA_DISABLE_BIT;
 	/*  set start trigger */
 	if (cmd->start_src == TRIG_EXT) {
@@ -2700,7 +2738,7 @@ static void pio_drain_ai_fifo_32(struct comedi_device *dev)
 /* empty fifo */
 static void pio_drain_ai_fifo(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 
 	if (thisboard->layout == LAYOUT_4020)
 		pio_drain_ai_fifo_32(dev);
@@ -2710,7 +2748,7 @@ static void pio_drain_ai_fifo(struct comedi_device *dev)
 
 static void drain_dma_buffers(struct comedi_device *dev, unsigned int channel)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_async *async = dev->read_subdev->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -2755,7 +2793,7 @@ static void handle_ai_interrupt(struct comedi_device *dev,
 				unsigned short status,
 				unsigned int plx_status)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_subdevice *s = dev->read_subdev;
 	struct comedi_async *async = s->async;
@@ -2782,7 +2820,7 @@ static void handle_ai_interrupt(struct comedi_device *dev,
 
 	/*  drain fifo with pio */
 	if ((status & ADC_DONE_BIT) ||
-	    ((cmd->flags & TRIG_WAKE_EOS) &&
+	    ((cmd->flags & CMDF_WAKE_EOS) &&
 	     (status & ADC_INTR_PENDING_BIT) &&
 	     (thisboard->layout != LAYOUT_4020))) {
 		spin_lock_irqsave(&dev->spinlock, flags);
@@ -3037,7 +3075,7 @@ static int ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 		    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 	int range = CR_RANGE(insn->chanspec);
@@ -3061,18 +3099,7 @@ static int ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	}
 
 	/*  remember output value */
-	devpriv->ao_value[chan] = data[0];
-
-	return 1;
-}
-
-static int ao_readback_insn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
-{
-	struct pcidas64_private *devpriv = dev->private;
-
-	data[0] = devpriv->ao_value[CR_CHAN(insn->chanspec)];
+	s->readback[chan] = data[0];
 
 	return 1;
 }
@@ -3200,7 +3227,7 @@ static inline int external_ai_queue_in_use(struct comedi_device *dev,
 					   struct comedi_subdevice *s,
 					   struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 
 	if (s->busy)
 		return 0;
@@ -3284,7 +3311,7 @@ static int cb_pcidas64_ao_check_chanlist(struct comedi_device *dev,
 static int ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	int err = 0;
 	unsigned int tmp_arg;
 
@@ -3369,26 +3396,16 @@ static int ao_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-static int dio_callback(int dir, int port, int data, unsigned long arg)
-{
-	void __iomem *iobase = (void __iomem *)arg;
-
-	if (dir) {
-		writeb(data, iobase + port);
-		return 0;
-	}
-	return readb(iobase + port);
-}
-
-static int dio_callback_4020(int dir, int port, int data, unsigned long arg)
+static int dio_callback_4020(struct comedi_device *dev,
+			     int dir, int port, int data, unsigned long iobase)
 {
-	void __iomem *iobase = (void __iomem *)arg;
+	struct pcidas64_private *devpriv = dev->private;
 
 	if (dir) {
-		writew(data, iobase + 2 * port);
+		writew(data, devpriv->main_iobase + iobase + 2 * port);
 		return 0;
 	}
-	return readw(iobase + 2 * port);
+	return readw(devpriv->main_iobase + iobase + 2 * port);
 }
 
 static int di_rbits(struct comedi_device *dev, struct comedi_subdevice *s,
@@ -3562,7 +3579,7 @@ static int caldac_i2c_write(struct comedi_device *dev,
 static void caldac_write(struct comedi_device *dev, unsigned int channel,
 			 unsigned int value)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 
 	devpriv->caldac_state[channel] = value;
@@ -3748,10 +3765,9 @@ static int eeprom_read_insn(struct comedi_device *dev,
  */
 static int setup_subdevices(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
+	const struct pcidas64_board *thisboard = dev->board_ptr;
 	struct pcidas64_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
-	void __iomem *dio_8255_iobase;
 	int i;
 	int ret;
 
@@ -3799,8 +3815,13 @@ static int setup_subdevices(struct comedi_device *dev)
 		s->n_chan = thisboard->ao_nchan;
 		s->maxdata = (1 << thisboard->ao_bits) - 1;
 		s->range_table = thisboard->ao_range_table;
-		s->insn_read = ao_readback_insn;
 		s->insn_write = ao_winsn;
+		s->insn_read = comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
 		if (ao_cmd_is_supported(thisboard)) {
 			dev->write_subdev = s;
 			s->do_cmdtest = ao_cmdtest;
@@ -3840,13 +3861,11 @@ static int setup_subdevices(struct comedi_device *dev)
 	s = &dev->subdevices[4];
 	if (thisboard->has_8255) {
 		if (thisboard->layout == LAYOUT_4020) {
-			dio_8255_iobase = devpriv->main_iobase + I8255_4020_REG;
 			ret = subdev_8255_init(dev, s, dio_callback_4020,
-					       (unsigned long)dio_8255_iobase);
+					       I8255_4020_REG);
 		} else {
-			dio_8255_iobase = dev->mmio + DIO_8255_OFFSET;
-			ret = subdev_8255_init(dev, s, dio_callback,
-					       (unsigned long)dio_8255_iobase);
+			ret = subdev_8255_mm_init(dev, s, NULL,
+						  DIO_8255_OFFSET);
 		}
 		if (ret)
 			return ret;
@@ -3996,54 +4015,22 @@ static int auto_attach(struct comedi_device *dev,
 
 static void detach(struct comedi_device *dev)
 {
-	const struct pcidas64_board *thisboard = comedi_board(dev);
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct pcidas64_private *devpriv = dev->private;
-	unsigned int i;
 
 	if (dev->irq)
 		free_irq(dev->irq, dev);
 	if (devpriv) {
-		if (pcidev) {
-			if (devpriv->plx9080_iobase) {
-				disable_plx_interrupts(dev);
-				iounmap(devpriv->plx9080_iobase);
-			}
-			if (devpriv->main_iobase)
-				iounmap(devpriv->main_iobase);
-			if (dev->mmio)
-				iounmap(dev->mmio);
-			/*  free pci dma buffers */
-			for (i = 0; i < ai_dma_ring_count(thisboard); i++) {
-				if (devpriv->ai_buffer[i])
-					pci_free_consistent(pcidev,
-						DMA_BUFFER_SIZE,
-						devpriv->ai_buffer[i],
-						devpriv->ai_buffer_bus_addr[i]);
-			}
-			for (i = 0; i < AO_DMA_RING_COUNT; i++) {
-				if (devpriv->ao_buffer[i])
-					pci_free_consistent(pcidev,
-						DMA_BUFFER_SIZE,
-						devpriv->ao_buffer[i],
-						devpriv->ao_buffer_bus_addr[i]);
-			}
-			/*  free dma descriptors */
-			if (devpriv->ai_dma_desc)
-				pci_free_consistent(pcidev,
-					sizeof(struct plx_dma_desc) *
-					ai_dma_ring_count(thisboard),
-					devpriv->ai_dma_desc,
-					devpriv->ai_dma_desc_bus_addr);
-			if (devpriv->ao_dma_desc)
-				pci_free_consistent(pcidev,
-					sizeof(struct plx_dma_desc) *
-					AO_DMA_RING_COUNT,
-					devpriv->ao_dma_desc,
-					devpriv->ao_dma_desc_bus_addr);
+		if (devpriv->plx9080_iobase) {
+			disable_plx_interrupts(dev);
+			iounmap(devpriv->plx9080_iobase);
 		}
+		if (devpriv->main_iobase)
+			iounmap(devpriv->main_iobase);
+		if (dev->mmio)
+			iounmap(dev->mmio);
 	}
 	comedi_pci_disable(dev);
+	cb_pcidas64_free_dma(dev);
 }
 
 static struct comedi_driver cb_pcidas64_driver = {
diff --git a/drivers/staging/comedi/drivers/cb_pcidda.c b/drivers/staging/comedi/drivers/cb_pcidda.c
index 901dc5d1bb72..01875d7c376f 100644
--- a/drivers/staging/comedi/drivers/cb_pcidda.c
+++ b/drivers/staging/comedi/drivers/cb_pcidda.c
@@ -154,6 +154,7 @@ static const struct cb_pcidda_board cb_pcidda_boards[] = {
 };
 
 struct cb_pcidda_private {
+	unsigned long daqio;
 	/* bits last written to da calibration register 1 */
 	unsigned int dac_cal1_bits;
 	/* current range settings for output channels */
@@ -164,13 +165,14 @@ struct cb_pcidda_private {
 /* lowlevel read from eeprom */
 static unsigned int cb_pcidda_serial_in(struct comedi_device *dev)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	unsigned int value = 0;
 	int i;
 	const int value_width = 16;	/*  number of bits wide values are */
 
 	for (i = 1; i <= value_width; i++) {
 		/*  read bits most significant bit first */
-		if (inw_p(dev->iobase + DACALIBRATION1) & SERIAL_OUT_BIT)
+		if (inw_p(devpriv->daqio + DACALIBRATION1) & SERIAL_OUT_BIT)
 			value |= 1 << (value_width - i);
 	}
 
@@ -190,7 +192,7 @@ static void cb_pcidda_serial_out(struct comedi_device *dev, unsigned int value,
 			devpriv->dac_cal1_bits |= SERIAL_IN_BIT;
 		else
 			devpriv->dac_cal1_bits &= ~SERIAL_IN_BIT;
-		outw_p(devpriv->dac_cal1_bits, dev->iobase + DACALIBRATION1);
+		outw_p(devpriv->dac_cal1_bits, devpriv->daqio + DACALIBRATION1);
 	}
 }
 
@@ -198,6 +200,7 @@ static void cb_pcidda_serial_out(struct comedi_device *dev, unsigned int value,
 static unsigned int cb_pcidda_read_eeprom(struct comedi_device *dev,
 					  unsigned int address)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	unsigned int i;
 	unsigned int cal2_bits;
 	unsigned int value;
@@ -213,7 +216,7 @@ static unsigned int cb_pcidda_read_eeprom(struct comedi_device *dev,
 	/*  deactivate caldacs (one caldac for every two channels) */
 	for (i = 0; i < max_num_caldacs; i++)
 		cal2_bits |= DESELECT_CALDAC_BIT(i);
-	outw_p(cal2_bits, dev->iobase + DACALIBRATION2);
+	outw_p(cal2_bits, devpriv->daqio + DACALIBRATION2);
 
 	/*  tell eeprom we want to read */
 	cb_pcidda_serial_out(dev, read_instruction, instruction_length);
@@ -224,7 +227,7 @@ static unsigned int cb_pcidda_read_eeprom(struct comedi_device *dev,
 
 	/*  deactivate eeprom */
 	cal2_bits &= ~SELECT_EEPROM_BIT;
-	outw_p(cal2_bits, dev->iobase + DACALIBRATION2);
+	outw_p(cal2_bits, devpriv->daqio + DACALIBRATION2);
 
 	return value;
 }
@@ -234,6 +237,7 @@ static void cb_pcidda_write_caldac(struct comedi_device *dev,
 				   unsigned int caldac, unsigned int channel,
 				   unsigned int value)
 {
+	struct cb_pcidda_private *devpriv = dev->private;
 	unsigned int cal2_bits;
 	unsigned int i;
 	/* caldacs use 3 bit channel specification */
@@ -256,10 +260,10 @@ static void cb_pcidda_write_caldac(struct comedi_device *dev,
 		cal2_bits |= DESELECT_CALDAC_BIT(i);
 	/*  activate the caldac we want */
 	cal2_bits &= ~DESELECT_CALDAC_BIT(caldac);
-	outw_p(cal2_bits, dev->iobase + DACALIBRATION2);
+	outw_p(cal2_bits, devpriv->daqio + DACALIBRATION2);
 	/*  deactivate caldac */
 	cal2_bits |= DESELECT_CALDAC_BIT(caldac);
-	outw_p(cal2_bits, dev->iobase + DACALIBRATION2);
+	outw_p(cal2_bits, devpriv->daqio + DACALIBRATION2);
 }
 
 /* set caldacs to eeprom values for given channel and range */
@@ -324,9 +328,9 @@ static int cb_pcidda_ao_insn_write(struct comedi_device *dev,
 	if (range > 2)
 		ctrl |= CB_DDA_DA_CTRL_UNIP;
 
-	outw(ctrl, dev->iobase + CB_DDA_DA_CTRL_REG);
+	outw(ctrl, devpriv->daqio + CB_DDA_DA_CTRL_REG);
 
-	outw(data[0], dev->iobase + CB_DDA_DA_DATA_REG(channel));
+	outw(data[0], devpriv->daqio + CB_DDA_DA_DATA_REG(channel));
 
 	return insn->n;
 }
@@ -338,7 +342,6 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 	const struct cb_pcidda_board *thisboard = NULL;
 	struct cb_pcidda_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase_8255;
 	int i;
 	int ret;
 
@@ -356,8 +359,8 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
-	dev->iobase = pci_resource_start(pcidev, 3);
-	iobase_8255 = pci_resource_start(pcidev, 2);
+	dev->iobase = pci_resource_start(pcidev, 2);
+	devpriv->daqio = pci_resource_start(pcidev, 3);
 
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
@@ -375,7 +378,7 @@ static int cb_pcidda_auto_attach(struct comedi_device *dev,
 	/* two 8255 digital io subdevices */
 	for (i = 0; i < 2; i++) {
 		s = &dev->subdevices[1 + i];
-		ret = subdev_8255_init(dev, s, NULL, iobase_8255 + (i * 4));
+		ret = subdev_8255_init(dev, s, NULL, i * I8255_SIZE);
 		if (ret)
 			return ret;
 	}
@@ -395,7 +398,7 @@ static struct comedi_driver cb_pcidda_driver = {
 	.driver_name	= "cb_pcidda",
 	.module		= THIS_MODULE,
 	.auto_attach	= cb_pcidda_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int cb_pcidda_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/cb_pcimdas.c b/drivers/staging/comedi/drivers/cb_pcimdas.c
index ccb9c72bc0c3..fe4d2544f3dc 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdas.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdas.c
@@ -77,10 +77,8 @@ See http://www.mccdaq.com/PDFs/Manuals/pcim-das1602-16.pdf for more details.
  */
 struct cb_pcimdas_private {
 	/* base addresses */
+	unsigned long daqio;
 	unsigned long BADR3;
-
-	/* Used for AO readback */
-	unsigned int ao_readback[2];
 };
 
 static int cb_pcimdas_ai_eoc(struct comedi_device *dev,
@@ -143,7 +141,7 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 	/* convert n samples */
 	for (n = 0; n < insn->n; n++) {
 		/* trigger conversion */
-		outw(0, dev->iobase + 0);
+		outw(0, devpriv->daqio + 0);
 
 		/* wait for conversion to end */
 		ret = comedi_timeout(dev, s, insn, cb_pcimdas_ai_eoc, 0);
@@ -151,55 +149,31 @@ static int cb_pcimdas_ai_rinsn(struct comedi_device *dev,
 			return ret;
 
 		/* read data */
-		data[n] = inw(dev->iobase + 0);
+		data[n] = inw(devpriv->daqio + 0);
 	}
 
 	/* return the number of samples read/written */
 	return n;
 }
 
-static int cb_pcimdas_ao_winsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
+static int cb_pcimdas_ao_insn_write(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
 {
 	struct cb_pcimdas_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
+	unsigned int reg = (chan) ? DAC1_OFFSET : DAC0_OFFSET;
 	int i;
-	int chan = CR_CHAN(insn->chanspec);
 
-	/* Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined. */
 	for (i = 0; i < insn->n; i++) {
-		switch (chan) {
-		case 0:
-			outw(data[i] & 0x0FFF, dev->iobase + DAC0_OFFSET);
-			break;
-		case 1:
-			outw(data[i] & 0x0FFF, dev->iobase + DAC1_OFFSET);
-			break;
-		default:
-			return -1;
-		}
-		devpriv->ao_readback[chan] = data[i];
+		val = data[i];
+		outw(val, devpriv->daqio + reg);
 	}
+	s->readback[chan] = val;
 
-	/* return the number of samples read/written */
-	return i;
-}
-
-/* AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv. */
-static int cb_pcimdas_ao_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	struct cb_pcimdas_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static int cb_pcimdas_auto_attach(struct comedi_device *dev,
@@ -208,7 +182,6 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct cb_pcimdas_private *devpriv;
 	struct comedi_subdevice *s;
-	unsigned long iobase_8255;
 	int ret;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
@@ -219,9 +192,9 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	dev->iobase = pci_resource_start(pcidev, 2);
+	devpriv->daqio = pci_resource_start(pcidev, 2);
 	devpriv->BADR3 = pci_resource_start(pcidev, 3);
-	iobase_8255 = pci_resource_start(pcidev, 4);
+	dev->iobase = pci_resource_start(pcidev, 4);
 
 	ret = comedi_alloc_subdevices(dev, 3);
 	if (ret)
@@ -247,30 +220,27 @@ static int cb_pcimdas_auto_attach(struct comedi_device *dev,
 	s->maxdata = 0xfff;
 	/* ranges are hardware settable, but not software readable. */
 	s->range_table = &range_unknown;
-	s->insn_write = &cb_pcimdas_ao_winsn;
-	s->insn_read = &cb_pcimdas_ao_rinsn;
+	s->insn_write = cb_pcimdas_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
-	ret = subdev_8255_init(dev, s, NULL, iobase_8255);
+	ret = subdev_8255_init(dev, s, NULL, 0x00);
 	if (ret)
 		return ret;
 
 	return 0;
 }
 
-static void cb_pcimdas_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
-}
-
 static struct comedi_driver cb_pcimdas_driver = {
 	.driver_name	= "cb_pcimdas",
 	.module		= THIS_MODULE,
 	.auto_attach	= cb_pcimdas_auto_attach,
-	.detach		= cb_pcimdas_detach,
+	.detach		= comedi_pci_detach,
 };
 
 static int cb_pcimdas_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/cb_pcimdda.c b/drivers/staging/comedi/drivers/cb_pcimdda.c
index 4a2b200de01b..03043e7b9b58 100644
--- a/drivers/staging/comedi/drivers/cb_pcimdda.c
+++ b/drivers/staging/comedi/drivers/cb_pcimdda.c
@@ -90,21 +90,14 @@ Configuration Options: not applicable, uses PCI auto config
 #define PCIMDDA_DA_CHAN(x)		(0x00 + (x) * 2)
 #define PCIMDDA_8255_BASE_REG		0x0c
 
-#define MAX_AO_READBACK_CHANNELS	6
-
-struct cb_pcimdda_private {
-	unsigned int ao_readback[MAX_AO_READBACK_CHANNELS];
-};
-
-static int cb_pcimdda_ao_winsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
+static int cb_pcimdda_ao_insn_write(struct comedi_device *dev,
+				    struct comedi_subdevice *s,
+				    struct comedi_insn *insn,
+				    unsigned int *data)
 {
-	struct cb_pcimdda_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned long offset = dev->iobase + PCIMDDA_DA_CHAN(chan);
-	unsigned int val = 0;
+	unsigned int val = s->readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
@@ -122,45 +115,31 @@ static int cb_pcimdda_ao_winsn(struct comedi_device *dev,
 		outb(val & 0x00ff, offset);
 		outb((val >> 8) & 0x00ff, offset + 1);
 	}
-
-	/* Cache the last value for readback */
-	devpriv->ao_readback[chan] = val;
+	s->readback[chan] = val;
 
 	return insn->n;
 }
 
-static int cb_pcimdda_ao_rinsn(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
+static int cb_pcimdda_ao_insn_read(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
 {
-	struct cb_pcimdda_private *devpriv = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned long offset = dev->iobase + PCIMDDA_DA_CHAN(chan);
-	int i;
-
-	for (i = 0; i < insn->n; i++) {
-		/* Initiate the simultaneous transfer */
-		inw(offset);
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
-		data[i] = devpriv->ao_readback[chan];
-	}
+	/* Initiate the simultaneous transfer */
+	inw(dev->iobase + PCIMDDA_DA_CHAN(chan));
 
-	return insn->n;
+	return comedi_readback_insn_read(dev, s, insn, data);
 }
 
 static int cb_pcimdda_auto_attach(struct comedi_device *dev,
 					    unsigned long context_unused)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	struct cb_pcimdda_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_pci_enable(dev);
 	if (ret)
 		return ret;
@@ -177,13 +156,16 @@ static int cb_pcimdda_auto_attach(struct comedi_device *dev,
 	s->n_chan	= 6;
 	s->maxdata	= 0xffff;
 	s->range_table	= &range_bipolar5;
-	s->insn_write	= cb_pcimdda_ao_winsn;
-	s->insn_read	= cb_pcimdda_ao_rinsn;
+	s->insn_write	= cb_pcimdda_ao_insn_write;
+	s->insn_read	= cb_pcimdda_ao_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[1];
 	/* digital i/o subdevice */
-	ret = subdev_8255_init(dev, s, NULL,
-			dev->iobase + PCIMDDA_8255_BASE_REG);
+	ret = subdev_8255_init(dev, s, NULL, PCIMDDA_8255_BASE_REG);
 	if (ret)
 		return ret;
 
@@ -194,7 +176,7 @@ static struct comedi_driver cb_pcimdda_driver = {
 	.driver_name	= "cb_pcimdda",
 	.module		= THIS_MODULE,
 	.auto_attach	= cb_pcimdda_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int cb_pcimdda_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/comedi_fc.c b/drivers/staging/comedi/drivers/comedi_fc.c
deleted file mode 100644
index c33c3e5680a6..000000000000
--- a/drivers/staging/comedi/drivers/comedi_fc.c
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- * comedi_fc.c
- * This is a place for code driver writers wish to share between
- * two or more drivers.  fc is short for frank-common.
- *
- * Author: Frank Mori Hess <fmhess@users.sourceforge.net>
- * Copyright (C) 2002 Frank Mori Hess
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/module.h>
-#include "../comedidev.h"
-
-#include "comedi_fc.h"
-
-unsigned int cfc_bytes_per_scan(struct comedi_subdevice *s)
-{
-	struct comedi_cmd *cmd = &s->async->cmd;
-	unsigned int num_samples;
-	unsigned int bits_per_sample;
-
-	switch (s->type) {
-	case COMEDI_SUBD_DI:
-	case COMEDI_SUBD_DO:
-	case COMEDI_SUBD_DIO:
-		bits_per_sample = 8 * bytes_per_sample(s);
-		num_samples = (cmd->chanlist_len + bits_per_sample - 1) /
-				bits_per_sample;
-		break;
-	default:
-		num_samples = cmd->chanlist_len;
-		break;
-	}
-	return num_samples * bytes_per_sample(s);
-}
-EXPORT_SYMBOL_GPL(cfc_bytes_per_scan);
-
-void cfc_inc_scan_progress(struct comedi_subdevice *s, unsigned int num_bytes)
-{
-	struct comedi_async *async = s->async;
-	unsigned int scan_length = cfc_bytes_per_scan(s);
-
-	async->scan_progress += num_bytes;
-	if (async->scan_progress >= scan_length) {
-		async->scan_progress %= scan_length;
-		async->events |= COMEDI_CB_EOS;
-	}
-}
-EXPORT_SYMBOL_GPL(cfc_inc_scan_progress);
-
-/* Writes an array of data points to comedi's buffer */
-unsigned int cfc_write_array_to_buffer(struct comedi_subdevice *s,
-				       void *data, unsigned int num_bytes)
-{
-	struct comedi_async *async = s->async;
-	unsigned int retval;
-
-	if (num_bytes == 0)
-		return 0;
-
-	retval = comedi_buf_write_alloc(s, num_bytes);
-	if (retval != num_bytes) {
-		dev_warn(s->device->class_dev, "buffer overrun\n");
-		async->events |= COMEDI_CB_OVERFLOW;
-		return 0;
-	}
-
-	comedi_buf_memcpy_to(s, 0, data, num_bytes);
-	comedi_buf_write_free(s, num_bytes);
-	cfc_inc_scan_progress(s, num_bytes);
-	async->events |= COMEDI_CB_BLOCK;
-
-	return num_bytes;
-}
-EXPORT_SYMBOL_GPL(cfc_write_array_to_buffer);
-
-unsigned int cfc_read_array_from_buffer(struct comedi_subdevice *s,
-					void *data, unsigned int num_bytes)
-{
-	if (num_bytes == 0)
-		return 0;
-
-	num_bytes = comedi_buf_read_alloc(s, num_bytes);
-	comedi_buf_memcpy_from(s, 0, data, num_bytes);
-	comedi_buf_read_free(s, num_bytes);
-	cfc_inc_scan_progress(s, num_bytes);
-	s->async->events |= COMEDI_CB_BLOCK;
-
-	return num_bytes;
-}
-EXPORT_SYMBOL_GPL(cfc_read_array_from_buffer);
-
-unsigned int cfc_handle_events(struct comedi_device *dev,
-			       struct comedi_subdevice *s)
-{
-	unsigned int events = s->async->events;
-
-	if (events == 0)
-		return events;
-
-	if (events & (COMEDI_CB_EOA | COMEDI_CB_ERROR | COMEDI_CB_OVERFLOW))
-		s->cancel(dev, s);
-
-	comedi_event(dev, s);
-
-	return events;
-}
-EXPORT_SYMBOL_GPL(cfc_handle_events);
-
-static int __init comedi_fc_init_module(void)
-{
-	return 0;
-}
-module_init(comedi_fc_init_module);
-
-static void __exit comedi_fc_cleanup_module(void)
-{
-}
-module_exit(comedi_fc_cleanup_module);
-
-MODULE_AUTHOR("Frank Mori Hess <fmhess@users.sourceforge.net>");
-MODULE_DESCRIPTION("Shared functions for Comedi low-level drivers");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/comedi_fc.h b/drivers/staging/comedi/drivers/comedi_fc.h
index 541b9371d3da..ce2835972507 100644
--- a/drivers/staging/comedi/drivers/comedi_fc.h
+++ b/drivers/staging/comedi/drivers/comedi_fc.h
@@ -23,30 +23,48 @@
 
 #include "../comedidev.h"
 
-unsigned int cfc_bytes_per_scan(struct comedi_subdevice *);
-void cfc_inc_scan_progress(struct comedi_subdevice *, unsigned int num_bytes);
+static inline unsigned int cfc_bytes_per_scan(struct comedi_subdevice *s)
+{
+	return comedi_bytes_per_scan(s);
+}
 
-/* Writes an array of data points to comedi's buffer */
-unsigned int cfc_write_array_to_buffer(struct comedi_subdevice *,
-				       void *data, unsigned int num_bytes);
+static inline void cfc_inc_scan_progress(struct comedi_subdevice *s,
+					 unsigned int num_bytes)
+{
+	comedi_inc_scan_progress(s, num_bytes);
+}
+
+static inline unsigned int cfc_write_array_to_buffer(struct comedi_subdevice *s,
+						     const void *data,
+						     unsigned int num_bytes)
+{
+	return comedi_write_array_to_buffer(s, data, num_bytes);
+}
 
 static inline unsigned int cfc_write_to_buffer(struct comedi_subdevice *s,
 					       unsigned short data)
 {
-	return cfc_write_array_to_buffer(s, &data, sizeof(data));
+	return comedi_write_array_to_buffer(s, &data, sizeof(data));
 };
 
 static inline unsigned int cfc_write_long_to_buffer(struct comedi_subdevice *s,
 						    unsigned int data)
 {
-	return cfc_write_array_to_buffer(s, &data, sizeof(data));
+	return comedi_write_array_to_buffer(s, &data, sizeof(data));
 };
 
-unsigned int cfc_read_array_from_buffer(struct comedi_subdevice *,
-					void *data, unsigned int num_bytes);
+static inline unsigned int
+cfc_read_array_from_buffer(struct comedi_subdevice *s, void *data,
+			   unsigned int num_bytes)
+{
+	return comedi_read_array_from_buffer(s, data, num_bytes);
+}
 
-unsigned int cfc_handle_events(struct comedi_device *,
-			       struct comedi_subdevice *);
+static inline unsigned int cfc_handle_events(struct comedi_device *dev,
+					     struct comedi_subdevice *s)
+{
+	return comedi_handle_events(dev, s);
+}
 
 /**
  * cfc_check_trigger_src() - trivially validate a comedi_cmd trigger source
diff --git a/drivers/staging/comedi/drivers/comedi_parport.c b/drivers/staging/comedi/drivers/comedi_parport.c
index a42748692357..bf002988192d 100644
--- a/drivers/staging/comedi/drivers/comedi_parport.c
+++ b/drivers/staging/comedi/drivers/comedi_parport.c
@@ -173,9 +173,6 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
@@ -187,10 +184,9 @@ static int parport_intr_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: ignored */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/comedi_test.c b/drivers/staging/comedi/drivers/comedi_test.c
index 845a67905ca6..00c03df72523 100644
--- a/drivers/staging/comedi/drivers/comedi_test.c
+++ b/drivers/staging/comedi/drivers/comedi_test.c
@@ -302,7 +302,7 @@ static int waveform_ai_cmd(struct comedi_device *dev,
 	struct waveform_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 
-	if (cmd->flags & TRIG_RT) {
+	if (cmd->flags & CMDF_PRIORITY) {
 		dev_err(dev->class_dev,
 			"commands at RT priority not supported in this driver\n");
 		return -1;
@@ -415,14 +415,14 @@ static int waveform_attach(struct comedi_device *dev,
 	for (i = 0; i < s->n_chan; i++)
 		devpriv->ao_loopbacks[i] = s->maxdata / 2;
 
-	init_timer(&(devpriv->timer));
+	init_timer(&devpriv->timer);
 	devpriv->timer.function = waveform_ai_interrupt;
 	devpriv->timer.data = (unsigned long)dev;
 
 	dev_info(dev->class_dev,
-		"%s: %i microvolt, %li microsecond waveform attached\n",
-		dev->board_name,
-		devpriv->uvolt_amplitude, devpriv->usec_period);
+		 "%s: %i microvolt, %li microsecond waveform attached\n",
+		 dev->board_name,
+		 devpriv->uvolt_amplitude, devpriv->usec_period);
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/contec_pci_dio.c b/drivers/staging/comedi/drivers/contec_pci_dio.c
index f066fb06dc1d..205f9df345a2 100644
--- a/drivers/staging/comedi/drivers/contec_pci_dio.c
+++ b/drivers/staging/comedi/drivers/contec_pci_dio.c
@@ -97,7 +97,7 @@ static struct comedi_driver contec_pci_dio_driver = {
 	.driver_name	= "contec_pci_dio",
 	.module		= THIS_MODULE,
 	.auto_attach	= contec_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int contec_pci_dio_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/dac02.c b/drivers/staging/comedi/drivers/dac02.c
index df46e0a5bade..34cbe83f0ce7 100644
--- a/drivers/staging/comedi/drivers/dac02.c
+++ b/drivers/staging/comedi/drivers/dac02.c
@@ -68,10 +68,6 @@ static const struct comedi_lrange das02_ao_ranges = {
 	}
 };
 
-struct dac02_private {
-	unsigned int ao_readback[2];
-};
-
 /*
  * Register I/O map
  */
@@ -83,7 +79,6 @@ static int dac02_ao_insn_write(struct comedi_device *dev,
 			       struct comedi_insn *insn,
 			       unsigned int *data)
 {
-	struct dac02_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int range = CR_RANGE(insn->chanspec);
 	unsigned int val;
@@ -92,7 +87,7 @@ static int dac02_ao_insn_write(struct comedi_device *dev,
 	for (i = 0; i < insn->n; i++) {
 		val = data[i];
 
-		devpriv->ao_readback[chan] = val;
+		s->readback[chan] = val;
 
 		/*
 		 * Unipolar outputs are true binary encoding.
@@ -113,31 +108,11 @@ static int dac02_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int dac02_ao_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct dac02_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int dac02_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	struct dac02_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_request_region(dev, it->options[0], 0x08);
 	if (ret)
 		return ret;
@@ -154,7 +129,11 @@ static int dac02_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0x0fff;
 	s->range_table	= &das02_ao_ranges;
 	s->insn_write	= dac02_ao_insn_write;
-	s->insn_read	= dac02_ao_insn_read;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/daqboard2000.c b/drivers/staging/comedi/drivers/daqboard2000.c
index cd369cd40114..e5b5a8133b34 100644
--- a/drivers/staging/comedi/drivers/daqboard2000.c
+++ b/drivers/staging/comedi/drivers/daqboard2000.c
@@ -275,7 +275,6 @@ struct daqboard2000_private {
 		card_daqboard_2000
 	} card;
 	void __iomem *plx;
-	unsigned int ao_readback[2];
 };
 
 static void writeAcqScanListEntry(struct comedi_device *dev, u16 entry)
@@ -401,21 +400,6 @@ static int daqboard2000_ai_insn_read(struct comedi_device *dev,
 	return i;
 }
 
-static int daqboard2000_ao_insn_read(struct comedi_device *dev,
-				     struct comedi_subdevice *s,
-				     struct comedi_insn *insn,
-				     unsigned int *data)
-{
-	struct daqboard2000_private *devpriv = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
-}
-
 static int daqboard2000_ao_eoc(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn,
@@ -435,38 +419,23 @@ static int daqboard2000_ao_insn_write(struct comedi_device *dev,
 				      struct comedi_insn *insn,
 				      unsigned int *data)
 {
-	struct daqboard2000_private *devpriv = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
-	int ret;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-#if 0
-		/*
-		 * OK, since it works OK without enabling the DAC's,
-		 * let's keep it as simple as possible...
-		 */
-		writew((chan + 2) * 0x0010 | 0x0001, dev->mmio + dacControl);
-		udelay(1000);
-#endif
-		writew(data[i], dev->mmio + dacSetting(chan));
+		unsigned int val = data[i];
+		int ret;
+
+		writew(val, dev->mmio + dacSetting(chan));
 
 		ret = comedi_timeout(dev, s, insn, daqboard2000_ao_eoc, 0);
 		if (ret)
 			return ret;
 
-		devpriv->ao_readback[chan] = data[i];
-#if 0
-		/*
-		 * Since we never enabled the DAC's, we don't need
-		 * to disable it...
-		 */
-		writew((chan + 2) * 0x0010 | 0x0000, dev->mmio + dacControl);
-		udelay(1000);
-#endif
+		s->readback[chan] = val;
 	}
 
-	return i;
+	return insn->n;
 }
 
 static void daqboard2000_resetLocalBus(struct comedi_device *dev)
@@ -651,16 +620,15 @@ static void daqboard2000_initializeDac(struct comedi_device *dev)
 	daqboard2000_dacDisarm(dev);
 }
 
-static int daqboard2000_8255_cb(int dir, int port, int data,
-				unsigned long ioaddr)
+static int daqboard2000_8255_cb(struct comedi_device *dev,
+				int dir, int port, int data,
+				unsigned long iobase)
 {
-	void __iomem *mmio_base = (void __iomem *)ioaddr;
-
 	if (dir) {
-		writew(data, mmio_base + port * 2);
+		writew(data, dev->mmio + iobase + port * 2);
 		return 0;
 	}
-	return readw(mmio_base + port * 2);
+	return readw(dev->mmio + iobase + port * 2);
 }
 
 static const void *daqboard2000_find_boardinfo(struct comedi_device *dev,
@@ -738,13 +706,17 @@ static int daqboard2000_auto_attach(struct comedi_device *dev,
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 2;
 	s->maxdata = 0xffff;
-	s->insn_read = daqboard2000_ao_insn_read;
 	s->insn_write = daqboard2000_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
 	s->range_table = &range_bipolar10;
 
+	result = comedi_alloc_subdev_readback(s);
+	if (result)
+		return result;
+
 	s = &dev->subdevices[2];
 	result = subdev_8255_init(dev, s, daqboard2000_8255_cb,
-			(unsigned long)(dev->mmio + dioP2ExpansionIO8Bit));
+				  dioP2ExpansionIO8Bit);
 	if (result)
 		return result;
 
@@ -755,15 +727,9 @@ static void daqboard2000_detach(struct comedi_device *dev)
 {
 	struct daqboard2000_private *devpriv = dev->private;
 
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (devpriv) {
-		if (dev->mmio)
-			iounmap(dev->mmio);
-		if (devpriv->plx)
-			iounmap(devpriv->plx);
-	}
-	comedi_pci_disable(dev);
+	if (devpriv && devpriv->plx)
+		iounmap(devpriv->plx);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver daqboard2000_driver = {
diff --git a/drivers/staging/comedi/drivers/das08.c b/drivers/staging/comedi/drivers/das08.c
index fcf916a80c8d..bdb671a66e22 100644
--- a/drivers/staging/comedi/drivers/das08.c
+++ b/drivers/staging/comedi/drivers/das08.c
@@ -217,7 +217,7 @@ static int das08_ai_eoc(struct comedi_device *dev,
 static int das08_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	struct das08_private_struct *devpriv = dev->private;
 	int n;
 	int chan;
@@ -337,8 +337,7 @@ static int das08jr_do_wbits(struct comedi_device *dev,
 static void das08_ao_set_data(struct comedi_device *dev,
 			      unsigned int chan, unsigned int data)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
-	struct das08_private_struct *devpriv = dev->private;
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	unsigned char lsb;
 	unsigned char msb;
 
@@ -355,54 +354,29 @@ static void das08_ao_set_data(struct comedi_device *dev,
 		/* load DACs */
 		inb(dev->iobase + DAS08AO_AO_UPDATE);
 	}
-	devpriv->ao_readback[chan] = data;
-}
-
-static void das08_ao_initialize(struct comedi_device *dev,
-				struct comedi_subdevice *s)
-{
-	int n;
-	unsigned int data;
-
-	data = s->maxdata / 2;	/* should be about 0 volts */
-	for (n = 0; n < s->n_chan; n++)
-		das08_ao_set_data(dev, n, data);
-}
-
-static int das08_ao_winsn(struct comedi_device *dev,
-			  struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
-{
-	unsigned int n;
-	unsigned int chan;
-
-	chan = CR_CHAN(insn->chanspec);
-
-	for (n = 0; n < insn->n; n++)
-		das08_ao_set_data(dev, chan, *data);
-
-	return n;
 }
 
-static int das08_ao_rinsn(struct comedi_device *dev,
-			  struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+static int das08_ao_insn_write(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
 {
-	struct das08_private_struct *devpriv = dev->private;
-	unsigned int n;
-	unsigned int chan;
-
-	chan = CR_CHAN(insn->chanspec);
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
+	int i;
 
-	for (n = 0; n < insn->n; n++)
-		data[n] = devpriv->ao_readback[chan];
+	for (i = 0; i < insn->n; i++) {
+		val = data[i];
+		das08_ao_set_data(dev, chan, val);
+	}
+	s->readback[chan] = val;
 
-	return n;
+	return insn->n;
 }
 
 static void i8254_initialize(struct comedi_device *dev)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
 	unsigned int mode = I8254_MODE0 | I8254_BINARY;
 	int i;
@@ -415,7 +389,7 @@ static int das08_counter_read(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
 	int chan = insn->chanspec;
 
@@ -427,7 +401,7 @@ static int das08_counter_write(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
 	int chan = insn->chanspec;
 
@@ -439,7 +413,7 @@ static int das08_counter_config(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	unsigned long i8254_iobase = dev->iobase + thisboard->i8254_offset;
 	int chan = insn->chanspec;
 
@@ -458,10 +432,11 @@ static int das08_counter_config(struct comedi_device *dev,
 
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	struct das08_private_struct *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	int ret;
+	int i;
 
 	dev->iobase = iobase;
 
@@ -498,9 +473,18 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 		s->n_chan = 2;
 		s->maxdata = (1 << thisboard->ao_nbits) - 1;
 		s->range_table = &range_bipolar5;
-		s->insn_write = das08_ao_winsn;
-		s->insn_read = das08_ao_rinsn;
-		das08_ao_initialize(dev, s);
+		s->insn_write = das08_ao_insn_write;
+		s->insn_read = comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
+		/* intialize all channels to 0V */
+		for (i = 0; i < s->n_chan; i++) {
+			s->readback[i] = s->maxdata / 2;
+			das08_ao_set_data(dev, i, s->readback[i]);
+		}
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
@@ -536,8 +520,7 @@ int das08_common_attach(struct comedi_device *dev, unsigned long iobase)
 	s = &dev->subdevices[4];
 	/* 8255 */
 	if (thisboard->i8255_offset != 0) {
-		ret = subdev_8255_init(dev, s, NULL,
-				       dev->iobase + thisboard->i8255_offset);
+		ret = subdev_8255_init(dev, s, NULL, thisboard->i8255_offset);
 		if (ret)
 			return ret;
 	} else {
diff --git a/drivers/staging/comedi/drivers/das08.h b/drivers/staging/comedi/drivers/das08.h
index 18cc170facd0..f86167da5895 100644
--- a/drivers/staging/comedi/drivers/das08.h
+++ b/drivers/staging/comedi/drivers/das08.h
@@ -44,7 +44,6 @@ struct das08_private_struct {
 					 *  without separate do register
 					 */
 	const unsigned int *pg_gainlist;
-	unsigned int ao_readback[2];	/* assume 2 AO channels */
 };
 
 int das08_common_attach(struct comedi_device *dev, unsigned long iobase);
diff --git a/drivers/staging/comedi/drivers/das08_isa.c b/drivers/staging/comedi/drivers/das08_isa.c
index 4fb03d3852d3..e4ba268e78ab 100644
--- a/drivers/staging/comedi/drivers/das08_isa.c
+++ b/drivers/staging/comedi/drivers/das08_isa.c
@@ -174,7 +174,7 @@ static const struct das08_board_struct das08_isa_boards[] = {
 static int das08_isa_attach(struct comedi_device *dev,
 			    struct comedi_devconfig *it)
 {
-	const struct das08_board_struct *thisboard = comedi_board(dev);
+	const struct das08_board_struct *thisboard = dev->board_ptr;
 	struct das08_private_struct *devpriv;
 	int ret;
 
diff --git a/drivers/staging/comedi/drivers/das08_pci.c b/drivers/staging/comedi/drivers/das08_pci.c
index 4ce3eb0a64cc..0987ce554945 100644
--- a/drivers/staging/comedi/drivers/das08_pci.c
+++ b/drivers/staging/comedi/drivers/das08_pci.c
@@ -77,7 +77,7 @@ static struct comedi_driver das08_pci_comedi_driver = {
 	.driver_name	= "pci-das08",
 	.module		= THIS_MODULE,
 	.auto_attach	= das08_pci_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int das08_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/das16.c b/drivers/staging/comedi/drivers/das16.c
index 057bc16f8ddc..2d8e86cec47a 100644
--- a/drivers/staging/comedi/drivers/das16.c
+++ b/drivers/staging/comedi/drivers/das16.c
@@ -631,7 +631,7 @@ static int das16_ai_check_chanlist(struct comedi_device *dev,
 static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 			  struct comedi_cmd *cmd)
 {
-	const struct das16_board *board = comedi_board(dev);
+	const struct das16_board *board = dev->board_ptr;
 	struct das16_private_struct *devpriv = dev->private;
 	int err = 0;
 	unsigned int trig_mask;
@@ -692,7 +692,9 @@ static int das16_cmd_test(struct comedi_device *dev, struct comedi_subdevice *s,
 		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
 						 board->ai_speed);
 
-	if (cmd->stop_src == TRIG_NONE)
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -748,7 +750,7 @@ static unsigned int das16_set_pacer(struct comedi_device *dev, unsigned int ns,
 
 static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct das16_board *board = comedi_board(dev);
+	const struct das16_board *board = dev->board_ptr;
 	struct das16_private_struct *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -756,9 +758,9 @@ static int das16_cmd_exec(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned long flags;
 	int range;
 
-	if (cmd->flags & TRIG_RT) {
+	if (cmd->flags & CMDF_PRIORITY) {
 		dev_err(dev->class_dev,
-			 "isa dma transfers cannot be performed with TRIG_RT, aborting\n");
+			 "isa dma transfers cannot be performed with CMDF_PRIORITY, aborting\n");
 		return -1;
 	}
 
@@ -883,7 +885,7 @@ static int das16_ai_insn_read(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned int *data)
 {
-	const struct das16_board *board = comedi_board(dev);
+	const struct das16_board *board = dev->board_ptr;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int range = CR_RANGE(insn->chanspec);
 	unsigned int val;
@@ -927,11 +929,13 @@ static int das16_ao_insn_write(struct comedi_device *dev,
 			       unsigned int *data)
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val;
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		val = data[i];
+		unsigned int val = data[i];
+
+		s->readback[chan] = val;
+
 		val <<= 4;
 
 		outb(val & 0xff, dev->iobase + DAS16_AO_LSB_REG(chan));
@@ -966,7 +970,7 @@ static int das16_do_insn_bits(struct comedi_device *dev,
 
 static int das16_probe(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct das16_board *board = comedi_board(dev);
+	const struct das16_board *board = dev->board_ptr;
 	int diobits;
 
 	/* diobits indicates boards */
@@ -991,7 +995,7 @@ static void das16_reset(struct comedi_device *dev)
 
 static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct das16_board *board = comedi_board(dev);
+	const struct das16_board *board = dev->board_ptr;
 	struct das16_private_struct *devpriv;
 	struct comedi_subdevice *s;
 	struct comedi_lrange *lrange;
@@ -1163,6 +1167,11 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->maxdata	= 0x0fff;
 		s->range_table	= devpriv->user_ao_range_table;
 		s->insn_write	= das16_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
@@ -1191,8 +1200,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	/* 8255 Digital I/O subdevice */
 	if (board->has_8255) {
 		s = &dev->subdevices[4];
-		ret = subdev_8255_init(dev, s, NULL,
-				       dev->iobase + board->i8255_offset);
+		ret = subdev_8255_init(dev, s, NULL, board->i8255_offset);
 		if (ret)
 			return ret;
 	}
@@ -1213,7 +1221,7 @@ static int das16_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 static void das16_detach(struct comedi_device *dev)
 {
-	const struct das16_board *board = comedi_board(dev);
+	const struct das16_board *board = dev->board_ptr;
 	struct das16_private_struct *devpriv = dev->private;
 	int i;
 
diff --git a/drivers/staging/comedi/drivers/das16m1.c b/drivers/staging/comedi/drivers/das16m1.c
index 5b6998b54060..24b63c452f51 100644
--- a/drivers/staging/comedi/drivers/das16m1.c
+++ b/drivers/staging/comedi/drivers/das16m1.c
@@ -218,12 +218,10 @@ static int das16m1_cmd_test(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {
-		/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -608,7 +606,7 @@ static int das16m1_attach(struct comedi_device *dev,
 
 	s = &dev->subdevices[3];
 	/* 8255 */
-	ret = subdev_8255_init(dev, s, NULL, devpriv->extra_iobase);
+	ret = subdev_8255_init(dev, s, NULL, DAS16M1_82C55);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/das1800.c b/drivers/staging/comedi/drivers/das1800.c
index 0cfca33965f6..a53d87ce9b14 100644
--- a/drivers/staging/comedi/drivers/das1800.c
+++ b/drivers/staging/comedi/drivers/das1800.c
@@ -453,7 +453,7 @@ static const struct comedi_lrange range_ao_2 = {
 static inline uint16_t munge_bipolar_sample(const struct comedi_device *dev,
 					    uint16_t sample)
 {
-	const struct das1800_board *thisboard = comedi_board(dev);
+	const struct das1800_board *thisboard = dev->board_ptr;
 
 	sample += 1 << (thisboard->resolution - 1);
 	return sample;
@@ -731,15 +731,15 @@ static unsigned int burst_convert_arg(unsigned int convert_arg, int flags)
 		convert_arg = 64000;
 
 	/*  the conversion time must be an integral number of microseconds */
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_NEAREST:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
 	default:
 		micro_sec = (convert_arg + 500) / 1000;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		micro_sec = convert_arg / 1000;
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		micro_sec = (convert_arg - 1) / 1000 + 1;
 		break;
 	}
@@ -773,7 +773,7 @@ static int das1800_ai_do_cmdtest(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
 				 struct comedi_cmd *cmd)
 {
-	const struct das1800_board *thisboard = comedi_board(dev);
+	const struct das1800_board *thisboard = dev->board_ptr;
 	struct das1800_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
@@ -1088,14 +1088,14 @@ static int das1800_ai_do_cmd(struct comedi_device *dev,
 	struct comedi_async *async = s->async;
 	const struct comedi_cmd *cmd = &async->cmd;
 
-	/* disable dma on TRIG_WAKE_EOS, or TRIG_RT
+	/* disable dma on CMDF_WAKE_EOS, or CMDF_PRIORITY
 	 * (because dma in handler is unsafe at hard real-time priority) */
-	if (cmd->flags & (TRIG_WAKE_EOS | TRIG_RT))
+	if (cmd->flags & (CMDF_WAKE_EOS | CMDF_PRIORITY))
 		devpriv->irq_dma_bits &= ~DMA_ENABLED;
 	else
 		devpriv->irq_dma_bits |= devpriv->dma_bits;
-	/*  interrupt on end of conversion for TRIG_WAKE_EOS */
-	if (cmd->flags & TRIG_WAKE_EOS) {
+	/*  interrupt on end of conversion for CMDF_WAKE_EOS */
+	if (cmd->flags & CMDF_WAKE_EOS) {
 		/*  interrupt fifo not empty */
 		devpriv->irq_dma_bits &= ~FIMD;
 	} else {
@@ -1136,7 +1136,7 @@ static int das1800_ai_rinsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das1800_board *thisboard = comedi_board(dev);
+	const struct das1800_board *thisboard = dev->board_ptr;
 	int i, n;
 	int chan, range, aref, chan_range;
 	int timeout = 1000;
@@ -1200,7 +1200,7 @@ static int das1800_ao_winsn(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn, unsigned int *data)
 {
-	const struct das1800_board *thisboard = comedi_board(dev);
+	const struct das1800_board *thisboard = dev->board_ptr;
 	struct das1800_private *devpriv = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
 /* int range = CR_RANGE(insn->chanspec); */
@@ -1329,7 +1329,7 @@ static int das1800_init_dma(struct comedi_device *dev, unsigned int dma0,
 
 static int das1800_probe(struct comedi_device *dev)
 {
-	const struct das1800_board *board = comedi_board(dev);
+	const struct das1800_board *board = dev->board_ptr;
 	int index;
 	int id;
 
@@ -1412,7 +1412,7 @@ static int das1800_attach(struct comedi_device *dev,
 	}
 
 	dev->board_ptr = das1800_boards + board;
-	thisboard = comedi_board(dev);
+	thisboard = dev->board_ptr;
 	dev->board_name = thisboard->name;
 
 	/*  if it is an 'ao' board with fancy analog out then we need extra io ports */
diff --git a/drivers/staging/comedi/drivers/das6402.c b/drivers/staging/comedi/drivers/das6402.c
index d18eea6c01aa..ab6e40608885 100644
--- a/drivers/staging/comedi/drivers/das6402.c
+++ b/drivers/staging/comedi/drivers/das6402.c
@@ -143,7 +143,6 @@ struct das6402_private {
 	unsigned int divider2;
 
 	unsigned int ao_range;
-	unsigned int ao_readback[2];
 };
 
 static void das6402_set_mode(struct comedi_device *dev,
@@ -328,7 +327,7 @@ static int das6402_ao_insn_write(struct comedi_device *dev,
 	for (i = 0; i < insn->n; i++) {
 		val = data[i];
 
-		devpriv->ao_readback[chan] = val;
+		s->readback[chan] = val;
 
 		if (s->maxdata == 0x0fff) {
 			/*
@@ -358,9 +357,7 @@ static int das6402_ao_insn_read(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	struct das6402_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
 
 	/*
 	 * If XFER mode is enabled, reading any DAC register
@@ -368,10 +365,7 @@ static int das6402_ao_insn_read(struct comedi_device *dev,
 	 */
 	inw(dev->iobase + DAS6402_AO_LSB_REG(chan));
 
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
+	return comedi_readback_insn_read(dev, s, insn, data);
 }
 
 static int das6402_di_insn_bits(struct comedi_device *dev,
@@ -440,7 +434,7 @@ static void das6402_reset(struct comedi_device *dev)
 static int das6402_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
-	const struct das6402_boardinfo *board = comedi_board(dev);
+	const struct das6402_boardinfo *board = dev->board_ptr;
 	struct das6402_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
@@ -510,6 +504,10 @@ static int das6402_attach(struct comedi_device *dev,
 	s->insn_write	= das6402_ao_insn_write;
 	s->insn_read	= das6402_ao_insn_read;
 
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
 	/* Digital Input subdevice */
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DI;
diff --git a/drivers/staging/comedi/drivers/das800.c b/drivers/staging/comedi/drivers/das800.c
index cbbb29797b83..d75e5528258c 100644
--- a/drivers/staging/comedi/drivers/das800.c
+++ b/drivers/staging/comedi/drivers/das800.c
@@ -248,7 +248,7 @@ static unsigned das800_ind_read(struct comedi_device *dev, unsigned reg)
 
 static void das800_enable(struct comedi_device *dev)
 {
-	const struct das800_board *thisboard = comedi_board(dev);
+	const struct das800_board *thisboard = dev->board_ptr;
 	struct das800_private *devpriv = dev->private;
 	unsigned long irq_flags;
 
@@ -325,7 +325,7 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_cmd *cmd)
 {
-	const struct das800_board *thisboard = comedi_board(dev);
+	const struct das800_board *thisboard = dev->board_ptr;
 	struct das800_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
@@ -398,7 +398,7 @@ static int das800_ai_do_cmdtest(struct comedi_device *dev,
 static int das800_ai_do_cmd(struct comedi_device *dev,
 			    struct comedi_subdevice *s)
 {
-	const struct das800_board *thisboard = comedi_board(dev);
+	const struct das800_board *thisboard = dev->board_ptr;
 	struct das800_private *devpriv = dev->private;
 	struct comedi_async *async = s->async;
 	struct comedi_cmd *cmd = &async->cmd;
@@ -633,7 +633,7 @@ static int das800_do_insn_bits(struct comedi_device *dev,
 
 static int das800_probe(struct comedi_device *dev)
 {
-	const struct das800_board *thisboard = comedi_board(dev);
+	const struct das800_board *thisboard = dev->board_ptr;
 	int board = thisboard ? thisboard - das800_boards : -EINVAL;
 	int id_bits;
 	unsigned long irq_flags;
@@ -695,7 +695,7 @@ static int das800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		return -ENODEV;
 	}
 	dev->board_ptr = das800_boards + board;
-	thisboard = comedi_board(dev);
+	thisboard = dev->board_ptr;
 	dev->board_name = thisboard->name;
 
 	if (irq > 1 && irq <= 7) {
diff --git a/drivers/staging/comedi/drivers/dmm32at.c b/drivers/staging/comedi/drivers/dmm32at.c
index e9cd2517ad81..7215e09305cf 100644
--- a/drivers/staging/comedi/drivers/dmm32at.c
+++ b/drivers/staging/comedi/drivers/dmm32at.c
@@ -47,9 +47,10 @@ Configuration Options:
 #define DMM32AT_AILOW 0x02
 #define DMM32AT_AIHIGH 0x03
 
-#define DMM32AT_DACLSB 0x04
 #define DMM32AT_DACSTAT 0x04
-#define DMM32AT_DACMSB 0x05
+#define DMM32AT_DACLSB_REG	0x04
+#define DMM32AT_DACMSB_REG	0x05
+#define DMM32AT_DACMSB_CHAN(x)	((x) << 6)
 
 #define DMM32AT_FIFOCNTRL 0x07
 #define DMM32AT_FIFOSTAT 0x07
@@ -150,15 +151,10 @@ static const struct comedi_lrange dmm32at_aoranges = {
 };
 
 struct dmm32at_private {
-
 	int data;
 	int ai_inuse;
 	unsigned int ai_scans_left;
-
-	/* Used for AO readback */
-	unsigned int ao_readback[4];
 	unsigned char dio_config;
-
 };
 
 static int dmm32at_ai_status(struct comedi_device *dev,
@@ -540,56 +536,35 @@ static int dmm32at_ao_eoc(struct comedi_device *dev,
 	return -EBUSY;
 }
 
-static int dmm32at_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int dmm32at_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	struct dmm32at_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int i;
-	int chan = CR_CHAN(insn->chanspec);
-	unsigned char hi, lo, status;
-	int ret;
 
-	/* Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined. */
 	for (i = 0; i < insn->n; i++) {
+		unsigned int val = data[i];
+		int ret;
 
-		devpriv->ao_readback[chan] = data[i];
-
-		/* get the low byte */
-		lo = data[i] & 0x00ff;
-		/* high byte also contains channel number */
-		hi = (data[i] >> 8) + chan * (1 << 6);
-		/* write the low and high values to the board */
-		outb(lo, dev->iobase + DMM32AT_DACLSB);
-		outb(hi, dev->iobase + DMM32AT_DACMSB);
+		/* write LSB then MSB + chan to load DAC */
+		outb(val & 0xff, dev->iobase + DMM32AT_DACLSB_REG);
+		outb((val >> 8) | DMM32AT_DACMSB_CHAN(chan),
+		     dev->iobase + DMM32AT_DACMSB_REG);
 
 		/* wait for circuit to settle */
 		ret = comedi_timeout(dev, s, insn, dmm32at_ao_eoc, 0);
 		if (ret)
 			return ret;
 
-		/* dummy read to update trigger the output */
-		status = inb(dev->iobase + DMM32AT_DACMSB);
+		/* dummy read to update DAC */
+		inb(dev->iobase + DMM32AT_DACMSB_REG);
 
+		s->readback[chan] = val;
 	}
 
-	/* return the number of samples read/written */
-	return i;
-}
-
-static int dmm32at_ao_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
-{
-	struct dmm32at_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static int dmm32at_dio_insn_bits(struct comedi_device *dev,
@@ -764,8 +739,12 @@ static int dmm32at_attach(struct comedi_device *dev,
 	s->n_chan = 4;
 	s->maxdata = 0x0fff;
 	s->range_table = &dmm32at_aoranges;
-	s->insn_write = dmm32at_ao_winsn;
-	s->insn_read = dmm32at_ao_rinsn;
+	s->insn_write = dmm32at_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
diff --git a/drivers/staging/comedi/drivers/dt2801.c b/drivers/staging/comedi/drivers/dt2801.c
index ad8ba0be4878..e97386343a0e 100644
--- a/drivers/staging/comedi/drivers/dt2801.c
+++ b/drivers/staging/comedi/drivers/dt2801.c
@@ -207,9 +207,7 @@ static const struct dt2801_board boardtypes[] = {
 };
 
 struct dt2801_private {
-
 	const struct comedi_lrange *dac_range_types[2];
-	unsigned int ao_readback[2];
 };
 
 /* These are the low-level routines:
@@ -309,7 +307,7 @@ static int dt2801_wait_for_ready(struct comedi_device *dev)
 	return -ETIME;
 }
 
-static int dt2801_writecmd(struct comedi_device *dev, int command)
+static void dt2801_writecmd(struct comedi_device *dev, int command)
 {
 	int stat;
 
@@ -323,8 +321,6 @@ static int dt2801_writecmd(struct comedi_device *dev, int command)
 	if (!(stat & DT_S_READY))
 		dev_dbg(dev->class_dev, "!ready in %s, ignoring\n", __func__);
 	outb_p(command, dev->iobase + DT2801_CMD);
-
-	return 0;
 }
 
 static int dt2801_reset(struct comedi_device *dev)
@@ -380,7 +376,7 @@ static int probe_number_of_ai_chans(struct comedi_device *dev)
 	int data;
 
 	for (n_chans = 0; n_chans < 16; n_chans++) {
-		stat = dt2801_writecmd(dev, DT_C_READ_ADIM);
+		dt2801_writecmd(dev, DT_C_READ_ADIM);
 		dt2801_writedata(dev, 0);
 		dt2801_writedata(dev, n_chans);
 		stat = dt2801_readdata2(dev, &data);
@@ -451,7 +447,7 @@ static int dt2801_ai_insn_read(struct comedi_device *dev,
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		stat = dt2801_writecmd(dev, DT_C_READ_ADIM);
+		dt2801_writecmd(dev, DT_C_READ_ADIM);
 		dt2801_writedata(dev, CR_RANGE(insn->chanspec));
 		dt2801_writedata(dev, CR_CHAN(insn->chanspec));
 		stat = dt2801_readdata2(dev, &d);
@@ -465,28 +461,18 @@ static int dt2801_ai_insn_read(struct comedi_device *dev,
 	return i;
 }
 
-static int dt2801_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	struct dt2801_private *devpriv = dev->private;
-
-	data[0] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
-
-	return 1;
-}
-
 static int dt2801_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	struct dt2801_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 
 	dt2801_writecmd(dev, DT_C_WRITE_DAIM);
-	dt2801_writedata(dev, CR_CHAN(insn->chanspec));
+	dt2801_writedata(dev, chan);
 	dt2801_writedata2(dev, data[0]);
 
-	devpriv->ao_readback[CR_CHAN(insn->chanspec)] = data[0];
+	s->readback[chan] = data[0];
 
 	return 1;
 }
@@ -571,7 +557,7 @@ static int dt2801_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 
 havetype:
 	dev->board_ptr = boardtypes + type;
-	board = comedi_board(dev);
+	board = dev->board_ptr;
 
 	n_ai_chans = probe_number_of_ai_chans(dev);
 
@@ -610,8 +596,12 @@ havetype:
 	s->range_table_list = devpriv->dac_range_types;
 	devpriv->dac_range_types[0] = dac_range_lkup(it->options[4]);
 	devpriv->dac_range_types[1] = dac_range_lkup(it->options[5]);
-	s->insn_read = dt2801_ao_insn_read;
 	s->insn_write = dt2801_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* 1st digital subdevice */
diff --git a/drivers/staging/comedi/drivers/dt2811.c b/drivers/staging/comedi/drivers/dt2811.c
index a2e9caf3256f..1736e397ad2c 100644
--- a/drivers/staging/comedi/drivers/dt2811.c
+++ b/drivers/staging/comedi/drivers/dt2811.c
@@ -213,7 +213,6 @@ struct dt2811_private {
 		dac_bipolar_5, dac_bipolar_2_5, dac_unipolar_5
 	} dac_range[2];
 	const struct comedi_lrange *range_type_list[2];
-	unsigned int ao_readback[2];
 };
 
 static const struct comedi_lrange *dac_range_types[] = {
@@ -257,39 +256,24 @@ static int dt2811_ai_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return i;
 }
 
-static int dt2811_ao_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			  struct comedi_insn *insn, unsigned int *data)
+static int dt2811_ao_insn_write(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	struct dt2811_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
-	int chan;
-
-	chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++) {
-		outb(data[i] & 0xff, dev->iobase + DT2811_DADAT0LO + 2 * chan);
-		outb((data[i] >> 8) & 0xff,
+		val = data[i];
+		outb(val & 0xff, dev->iobase + DT2811_DADAT0LO + 2 * chan);
+		outb((val >> 8) & 0xff,
 		     dev->iobase + DT2811_DADAT0HI + 2 * chan);
-		devpriv->ao_readback[chan] = data[i];
 	}
+	s->readback[chan] = val;
 
-	return i;
-}
-
-static int dt2811_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	struct dt2811_private *devpriv = dev->private;
-	int i;
-	int chan;
-
-	chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static int dt2811_di_insn_bits(struct comedi_device *dev,
@@ -337,7 +321,7 @@ static int dt2811_do_insn_bits(struct comedi_device *dev,
 static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	/* int i; */
-	const struct dt2811_board *board = comedi_board(dev);
+	const struct dt2811_board *board = dev->board_ptr;
 	struct dt2811_private *devpriv;
 	int ret;
 	struct comedi_subdevice *s;
@@ -429,12 +413,16 @@ static int dt2811_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 2;
-	s->insn_write = dt2811_ao_insn;
-	s->insn_read = dt2811_ao_insn_read;
 	s->maxdata = 0xfff;
 	s->range_table_list = devpriv->range_type_list;
 	devpriv->range_type_list[0] = dac_range_types[devpriv->dac_range[0]];
 	devpriv->range_type_list[1] = dac_range_types[devpriv->dac_range[1]];
+	s->insn_write = dt2811_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* di subdevice */
diff --git a/drivers/staging/comedi/drivers/dt282x.c b/drivers/staging/comedi/drivers/dt282x.c
index 5de26745783a..cc974a5e5cf6 100644
--- a/drivers/staging/comedi/drivers/dt282x.c
+++ b/drivers/staging/comedi/drivers/dt282x.c
@@ -315,8 +315,6 @@ struct dt282x_private {
 
 	unsigned int divisor;
 
-	unsigned short ao_readback[2];
-
 	int dacsr;	/* software copies of registers */
 	int adcsr;
 	int supcsr;
@@ -405,15 +403,15 @@ static unsigned int dt282x_ns_to_timer(unsigned int *ns, unsigned int flags)
 		if (prescale == 1)
 			continue;
 		base = 250 * (1 << prescale);
-		switch (flags & TRIG_ROUND_MASK) {
-		case TRIG_ROUND_NEAREST:
+		switch (flags & CMDF_ROUND_MASK) {
+		case CMDF_ROUND_NEAREST:
 		default:
 			divider = (*ns + base / 2) / base;
 			break;
-		case TRIG_ROUND_DOWN:
+		case CMDF_ROUND_DOWN:
 			divider = (*ns) / base;
 			break;
-		case TRIG_ROUND_UP:
+		case CMDF_ROUND_UP:
 			divider = (*ns + base - 1) / base;
 			break;
 		}
@@ -683,7 +681,7 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_cmd *cmd)
 {
-	const struct dt282x_board *board = comedi_board(dev);
+	const struct dt282x_board *board = dev->board_ptr;
 	struct dt282x_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
@@ -730,11 +728,10 @@ static int dt282x_ai_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_min(&cmd->convert_arg, board->ai_speed);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {	/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_EXT | TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -826,21 +823,6 @@ static int dt282x_ai_cancel(struct comedi_device *dev,
 	return 0;
 }
 
-static int dt282x_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct dt282x_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int dt282x_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn,
@@ -849,14 +831,14 @@ static int dt282x_ao_insn_write(struct comedi_device *dev,
 	struct dt282x_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int range = CR_RANGE(insn->chanspec);
-	unsigned int val;
 	int i;
 
 	devpriv->dacsr |= DT2821_DACSR_SSEL | DT2821_DACSR_YSEL(chan);
 
 	for (i = 0; i < insn->n; i++) {
-		val = data[i];
-		devpriv->ao_readback[chan] = val;
+		unsigned int val = data[i];
+
+		s->readback[chan] = val;
 
 		if (comedi_range_is_bipolar(s, range))
 			val = comedi_offset_munge(s, val);
@@ -907,11 +889,10 @@ static int dt282x_ao_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {	/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_EXT | TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -1166,7 +1147,7 @@ static int dt282x_initialize(struct comedi_device *dev)
  */
 static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct dt282x_board *board = comedi_board(dev);
+	const struct dt282x_board *board = dev->board_ptr;
 	struct dt282x_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
@@ -1252,12 +1233,10 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->subdev_flags	= SDF_WRITABLE;
 		s->n_chan	= board->dachan;
 		s->maxdata	= board->ao_maxdata;
-
 		/* ranges are per-channel, set by jumpers on the board */
 		s->range_table	= &dt282x_ao_range;
-
-		s->insn_read	= dt282x_ao_insn_read;
 		s->insn_write	= dt282x_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
 		if (dev->irq) {
 			dev->write_subdev = s;
 			s->subdev_flags	|= SDF_CMD_WRITE;
@@ -1266,6 +1245,10 @@ static int dt282x_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			s->do_cmd	= dt282x_ao_cmd;
 			s->cancel	= dt282x_ao_cancel;
 		}
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
diff --git a/drivers/staging/comedi/drivers/dt3000.c b/drivers/staging/comedi/drivers/dt3000.c
index 56e21cc2dcfe..825561046b6f 100644
--- a/drivers/staging/comedi/drivers/dt3000.c
+++ b/drivers/staging/comedi/drivers/dt3000.c
@@ -245,7 +245,6 @@ static const struct dt3k_boardtype dt3k_boardtypes[] = {
 
 struct dt3k_private {
 	unsigned int lock;
-	unsigned int ao_readback[2];
 	unsigned int ai_front;
 	unsigned int ai_rear;
 };
@@ -378,15 +377,15 @@ static int dt3k_ns_to_timer(unsigned int timer_base, unsigned int *nanosec,
 
 	for (prescale = 0; prescale < 16; prescale++) {
 		base = timer_base * (prescale + 1);
-		switch (flags & TRIG_ROUND_MASK) {
-		case TRIG_ROUND_NEAREST:
+		switch (flags & CMDF_ROUND_MASK) {
+		case CMDF_ROUND_NEAREST:
 		default:
 			divider = (*nanosec + base / 2) / base;
 			break;
-		case TRIG_ROUND_DOWN:
+		case CMDF_ROUND_DOWN:
 			divider = (*nanosec) / base;
 			break;
-		case TRIG_ROUND_UP:
+		case CMDF_ROUND_UP:
 			divider = (*nanosec) / base;
 			break;
 		}
@@ -406,7 +405,7 @@ static int dt3k_ns_to_timer(unsigned int timer_base, unsigned int *nanosec,
 static int dt3k_ai_cmdtest(struct comedi_device *dev,
 			   struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct dt3k_boardtype *this_board = comedi_board(dev);
+	const struct dt3k_boardtype *this_board = dev->board_ptr;
 	int err = 0;
 	unsigned int arg;
 
@@ -424,9 +423,6 @@ static int dt3k_ai_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
@@ -488,7 +484,6 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned int chan, range, aref;
 	unsigned int divider;
 	unsigned int tscandiv;
-	unsigned int mode;
 
 	for (i = 0; i < cmd->chanlist_len; i++) {
 		chan = CR_CHAN(cmd->chanlist[i]);
@@ -513,8 +508,7 @@ static int dt3k_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		writew((tscandiv & 0xffff), dev->mmio + DPR_Params(4));
 	}
 
-	mode = DT3000_AD_RETRIG_INTERNAL | 0 | 0;
-	writew(mode, dev->mmio + DPR_Params(5));
+	writew(DT3000_AD_RETRIG_INTERNAL, dev->mmio + DPR_Params(5));
 	writew(aref == AREF_DIFF, dev->mmio + DPR_Params(6));
 
 	writew(AI_FIFO_DEPTH / 2, dev->mmio + DPR_Params(7));
@@ -550,35 +544,22 @@ static int dt3k_ai_insn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return i;
 }
 
-static int dt3k_ao_insn(struct comedi_device *dev, struct comedi_subdevice *s,
-			struct comedi_insn *insn, unsigned int *data)
+static int dt3k_ao_insn_write(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
 {
-	struct dt3k_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
-	unsigned int chan;
 
-	chan = CR_CHAN(insn->chanspec);
 	for (i = 0; i < insn->n; i++) {
-		dt3k_writesingle(dev, SUBS_AO, chan, data[i]);
-		devpriv->ao_readback[chan] = data[i];
+		val = data[i];
+		dt3k_writesingle(dev, SUBS_AO, chan, val);
 	}
+	s->readback[chan] = val;
 
-	return i;
-}
-
-static int dt3k_ao_insn_read(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn, unsigned int *data)
-{
-	struct dt3k_private *devpriv = dev->private;
-	int i;
-	unsigned int chan;
-
-	chan = CR_CHAN(insn->chanspec);
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static void dt3k_dio_config(struct comedi_device *dev, int bits)
@@ -714,11 +695,15 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 	s->type		= COMEDI_SUBD_AO;
 	s->subdev_flags	= SDF_WRITABLE;
 	s->n_chan	= 2;
-	s->insn_read	= dt3k_ao_insn_read;
-	s->insn_write	= dt3k_ao_insn;
 	s->maxdata	= (1 << this_board->dabits) - 1;
 	s->len_chanlist	= 1;
 	s->range_table	= &range_bipolar10;
+	s->insn_write	= dt3k_ao_insn_write;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* dio subsystem */
@@ -750,20 +735,11 @@ static int dt3000_auto_attach(struct comedi_device *dev,
 	return 0;
 }
 
-static void dt3000_detach(struct comedi_device *dev)
-{
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->mmio)
-		iounmap(dev->mmio);
-	comedi_pci_disable(dev);
-}
-
 static struct comedi_driver dt3000_driver = {
 	.driver_name	= "dt3000",
 	.module		= THIS_MODULE,
 	.auto_attach	= dt3000_auto_attach,
-	.detach		= dt3000_detach,
+	.detach		= comedi_pci_detach,
 };
 
 static int dt3000_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/dt9812.c b/drivers/staging/comedi/drivers/dt9812.c
index bd2ca2b371e6..77bb89fee327 100644
--- a/drivers/staging/comedi/drivers/dt9812.c
+++ b/drivers/staging/comedi/drivers/dt9812.c
@@ -240,7 +240,6 @@ struct dt9812_private {
 		size_t size;
 	} cmd_wr, cmd_rd;
 	u16 device;
-	u16 ao_shadow[2];
 };
 
 static int dt9812_read_info(struct comedi_device *dev,
@@ -546,7 +545,6 @@ static int dt9812_analog_out(struct comedi_device *dev, int channel, u16 value)
 		break;
 	}
 	ret = dt9812_rmw_multiple_registers(dev, 3, rmw);
-	devpriv->ao_shadow[channel] = value;
 
 	up(&devpriv->sem);
 
@@ -609,15 +607,13 @@ static int dt9812_ao_insn_read(struct comedi_device *dev,
 			       unsigned int *data)
 {
 	struct dt9812_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
+	int ret;
 
 	down(&devpriv->sem);
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_shadow[chan];
+	ret = comedi_readback_insn_read(dev, s, insn, data);
 	up(&devpriv->sem);
 
-	return insn->n;
+	return ret;
 }
 
 static int dt9812_ao_insn_write(struct comedi_device *dev,
@@ -626,13 +622,17 @@ static int dt9812_ao_insn_write(struct comedi_device *dev,
 				unsigned int *data)
 {
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	int ret;
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		ret = dt9812_analog_out(dev, chan, data[i]);
+		unsigned int val = data[i];
+		int ret;
+
+		ret = dt9812_analog_out(dev, chan, val);
 		if (ret)
 			return ret;
+
+		s->readback[chan] = val;
 	}
 
 	return insn->n;
@@ -769,6 +769,7 @@ static int dt9812_auto_attach(struct comedi_device *dev,
 	struct comedi_subdevice *s;
 	bool is_unipolar;
 	int ret;
+	int i;
 
 	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
 	if (!devpriv)
@@ -828,8 +829,12 @@ static int dt9812_auto_attach(struct comedi_device *dev,
 	s->insn_write	= dt9812_ao_insn_write;
 	s->insn_read	= dt9812_ao_insn_read;
 
-	devpriv->ao_shadow[0] = is_unipolar ? 0x0000 : 0x0800;
-	devpriv->ao_shadow[1] = is_unipolar ? 0x0000 : 0x0800;
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < s->n_chan; i++)
+		s->readback[i] = is_unipolar ? 0x0000 : 0x0800;
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/dyna_pci10xx.c b/drivers/staging/comedi/drivers/dyna_pci10xx.c
index e5593f8c7406..608aee0c3a15 100644
--- a/drivers/staging/comedi/drivers/dyna_pci10xx.c
+++ b/drivers/staging/comedi/drivers/dyna_pci10xx.c
@@ -243,9 +243,9 @@ static void dyna_pci10xx_detach(struct comedi_device *dev)
 {
 	struct dyna_pci10xx_private *devpriv = dev->private;
 
+	comedi_pci_detach(dev);
 	if (devpriv)
 		mutex_destroy(&devpriv->mutex);
-	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver dyna_pci10xx_driver = {
diff --git a/drivers/staging/comedi/drivers/fl512.c b/drivers/staging/comedi/drivers/fl512.c
index 4e410f3b0e24..5a1e3c8fc01c 100644
--- a/drivers/staging/comedi/drivers/fl512.c
+++ b/drivers/staging/comedi/drivers/fl512.c
@@ -44,10 +44,6 @@
 #define FL512_AO_DATA_REG(x)		(0x04 + ((x) * 2))
 #define FL512_AO_TRIG_REG(x)		(0x04 + ((x) * 2))
 
-struct fl512_private {
-	unsigned short ao_readback[2];
-};
-
 static const struct comedi_lrange range_fl512 = {
 	4, {
 		BIP_RANGE(0.5),
@@ -92,9 +88,8 @@ static int fl512_ao_insn_write(struct comedi_device *dev,
 			       struct comedi_insn *insn,
 			       unsigned int *data)
 {
-	struct fl512_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val = devpriv->ao_readback[chan];
+	unsigned int val = s->readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
@@ -105,29 +100,13 @@ static int fl512_ao_insn_write(struct comedi_device *dev,
 		outb((val >> 8) & 0xf, dev->iobase + FL512_AO_DATA_REG(chan));
 		inb(dev->iobase + FL512_AO_TRIG_REG(chan));
 	}
-	devpriv->ao_readback[chan] = val;
-
-	return insn->n;
-}
-
-static int fl512_ao_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct fl512_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+	s->readback[chan] = val;
 
 	return insn->n;
 }
 
 static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	struct fl512_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
@@ -135,10 +114,6 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	ret = comedi_alloc_subdevices(dev, 2);
 	if (ret)
 		return ret;
@@ -160,7 +135,11 @@ static int fl512_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0x0fff;
 	s->range_table	= &range_fl512;
 	s->insn_write	= fl512_ao_insn_write;
-	s->insn_read	= fl512_ao_insn_read;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/gsc_hpdi.c b/drivers/staging/comedi/drivers/gsc_hpdi.c
index 91c1e8cf5d24..b8975a4606ea 100644
--- a/drivers/staging/comedi/drivers/gsc_hpdi.c
+++ b/drivers/staging/comedi/drivers/gsc_hpdi.c
@@ -422,12 +422,10 @@ static int gsc_hpdi_cmd_test(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: fix up any arguments */
-
-	if (err)
-		return 4;
+	/* Step 4: fix up any arguments */
 
 	/* Step 5: check channel list if it exists */
+
 	if (cmd->chanlist && cmd->chanlist_len > 0)
 		err |= gsc_hpdi_check_chanlist(dev, s, cmd);
 
@@ -507,6 +505,32 @@ static int gsc_hpdi_dio_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
+static void gsc_hpdi_free_dma(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct hpdi_private *devpriv = dev->private;
+	int i;
+
+	if (!devpriv)
+		return;
+
+	/* free pci dma buffers */
+	for (i = 0; i < NUM_DMA_BUFFERS; i++) {
+		if (devpriv->dio_buffer[i])
+			pci_free_consistent(pcidev,
+					    DMA_BUFFER_SIZE,
+					    devpriv->dio_buffer[i],
+					    devpriv->dio_buffer_phys_addr[i]);
+	}
+	/* free dma descriptors */
+	if (devpriv->dma_desc)
+		pci_free_consistent(pcidev,
+				    sizeof(struct plx_dma_desc) *
+				    NUM_DMA_DESCRIPTORS,
+				    devpriv->dma_desc,
+				    devpriv->dma_desc_phys_addr);
+}
+
 static int gsc_hpdi_init(struct comedi_device *dev)
 {
 	struct hpdi_private *devpriv = dev->private;
@@ -681,9 +705,7 @@ static int gsc_hpdi_auto_attach(struct comedi_device *dev,
 
 static void gsc_hpdi_detach(struct comedi_device *dev)
 {
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
 	struct hpdi_private *devpriv = dev->private;
-	unsigned int i;
 
 	if (dev->irq)
 		free_irq(dev->irq, dev);
@@ -694,24 +716,9 @@ static void gsc_hpdi_detach(struct comedi_device *dev)
 		}
 		if (dev->mmio)
 			iounmap(dev->mmio);
-		/*  free pci dma buffers */
-		for (i = 0; i < NUM_DMA_BUFFERS; i++) {
-			if (devpriv->dio_buffer[i])
-				pci_free_consistent(pcidev,
-						    DMA_BUFFER_SIZE,
-						    devpriv->dio_buffer[i],
-						    devpriv->
-						    dio_buffer_phys_addr[i]);
-		}
-		/*  free dma descriptors */
-		if (devpriv->dma_desc)
-			pci_free_consistent(pcidev,
-					    sizeof(struct plx_dma_desc) *
-					    NUM_DMA_DESCRIPTORS,
-					    devpriv->dma_desc,
-					    devpriv->dma_desc_phys_addr);
 	}
 	comedi_pci_disable(dev);
+	gsc_hpdi_free_dma(dev);
 }
 
 static struct comedi_driver gsc_hpdi_driver = {
diff --git a/drivers/staging/comedi/drivers/icp_multi.c b/drivers/staging/comedi/drivers/icp_multi.c
index a98cef2106a9..f4e1c1cf4178 100644
--- a/drivers/staging/comedi/drivers/icp_multi.c
+++ b/drivers/staging/comedi/drivers/icp_multi.c
@@ -107,7 +107,6 @@ static const char range_codes_analog[] = { 0x00, 0x20, 0x10, 0x30 };
 */
 
 struct icp_multi_private {
-	char valid;		/*  card is usable */
 	unsigned int AdcCmdStatus;	/*  ADC Command/Status register */
 	unsigned int DacCmdStatus;	/*  DAC Command/Status register */
 	unsigned int IntEnable;	/*  Interrupt Enable register */
@@ -116,7 +115,6 @@ struct icp_multi_private {
 	unsigned char act_chanlist_len;	/*  len of scanlist */
 	unsigned char act_chanlist_pos;	/*  actual position in MUX list */
 	unsigned int *ai_chanlist;	/*  actaul chanlist */
-	unsigned short ao_data[4];	/*  data output buffer */
 	unsigned int do_data;	/*  Remember digital output data */
 };
 
@@ -240,14 +238,15 @@ static int icp_multi_ao_eoc(struct comedi_device *dev,
 	return -EBUSY;
 }
 
-static int icp_multi_insn_write_ao(struct comedi_device *dev,
+static int icp_multi_ao_insn_write(struct comedi_device *dev,
 				   struct comedi_subdevice *s,
 				   struct comedi_insn *insn,
 				   unsigned int *data)
 {
 	struct icp_multi_private *devpriv = dev->private;
-	int n, chan, range;
-	int ret;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	int i;
 
 	/*  Disable D/A conversion ready interrupt */
 	devpriv->IntEnable &= ~DAC_READY;
@@ -257,10 +256,6 @@ static int icp_multi_insn_write_ao(struct comedi_device *dev,
 	devpriv->IntStatus |= DAC_READY;
 	writew(devpriv->IntStatus, dev->mmio + ICP_MULTI_INT_STAT);
 
-	/*  Get channel number and range */
-	chan = CR_CHAN(insn->chanspec);
-	range = CR_RANGE(insn->chanspec);
-
 	/*  Set up range and channel data */
 	/*  Bit 4 = 1 : Bipolar */
 	/*  Bit 5 = 0 : 5V */
@@ -272,7 +267,10 @@ static int icp_multi_insn_write_ao(struct comedi_device *dev,
 
 	writew(devpriv->DacCmdStatus, dev->mmio + ICP_MULTI_DAC_CSR);
 
-	for (n = 0; n < insn->n; n++) {
+	for (i = 0; i < insn->n; i++) {
+		unsigned int val = data[i];
+		int ret;
+
 		/*  Wait for analogue output data register to be
 		 *  ready for new data, or get fed up waiting */
 		ret = comedi_timeout(dev, s, insn, icp_multi_ao_eoc, 0);
@@ -287,42 +285,20 @@ static int icp_multi_insn_write_ao(struct comedi_device *dev,
 			writew(devpriv->IntStatus,
 			       dev->mmio + ICP_MULTI_INT_STAT);
 
-			/*  Clear data received */
-			devpriv->ao_data[chan] = 0;
-
 			return ret;
 		}
 
-		/*  Write data to analogue output data register */
-		writew(data[n], dev->mmio + ICP_MULTI_AO);
+		writew(val, dev->mmio + ICP_MULTI_AO);
 
 		/*  Set DAC_ST bit to write the data to selected channel */
 		devpriv->DacCmdStatus |= DAC_ST;
 		writew(devpriv->DacCmdStatus, dev->mmio + ICP_MULTI_DAC_CSR);
 		devpriv->DacCmdStatus &= ~DAC_ST;
 
-		/*  Save analogue output data */
-		devpriv->ao_data[chan] = data[n];
+		s->readback[chan] = val;
 	}
 
-	return n;
-}
-
-static int icp_multi_insn_read_ao(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
-{
-	struct icp_multi_private *devpriv = dev->private;
-	int n, chan;
-
-	/*  Get channel number */
-	chan = CR_CHAN(insn->chanspec);
-
-	/*  Read analogue outputs */
-	for (n = 0; n < insn->n; n++)
-		data[n] = devpriv->ao_data[chan];
-
-	return n;
+	return insn->n;
 }
 
 static int icp_multi_insn_bits_di(struct comedi_device *dev,
@@ -518,8 +494,12 @@ static int icp_multi_auto_attach(struct comedi_device *dev,
 	s->maxdata = 0x0fff;
 	s->len_chanlist = 4;
 	s->range_table = &range_analog;
-	s->insn_write = icp_multi_insn_write_ao;
-	s->insn_read = icp_multi_insn_read_ao;
+	s->insn_write = icp_multi_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	s->type = COMEDI_SUBD_DI;
@@ -549,23 +529,14 @@ static int icp_multi_auto_attach(struct comedi_device *dev,
 	s->insn_read = icp_multi_insn_read_ctr;
 	s->insn_write = icp_multi_insn_write_ctr;
 
-	devpriv->valid = 1;
-
 	return 0;
 }
 
 static void icp_multi_detach(struct comedi_device *dev)
 {
-	struct icp_multi_private *devpriv = dev->private;
-
-	if (devpriv)
-		if (devpriv->valid)
-			icp_multi_reset(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
 	if (dev->mmio)
-		iounmap(dev->mmio);
-	comedi_pci_disable(dev);
+		icp_multi_reset(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver icp_multi_driver = {
diff --git a/drivers/staging/comedi/drivers/ii_pci20kc.c b/drivers/staging/comedi/drivers/ii_pci20kc.c
index 687db433e131..cc5fd75b8bc0 100644
--- a/drivers/staging/comedi/drivers/ii_pci20kc.c
+++ b/drivers/staging/comedi/drivers/ii_pci20kc.c
@@ -132,44 +132,25 @@ static const struct comedi_lrange ii20k_ai_ranges = {
 	},
 };
 
-struct ii20k_ao_private {
-	unsigned int last_data[2];
-};
-
 static void __iomem *ii20k_module_iobase(struct comedi_device *dev,
 					 struct comedi_subdevice *s)
 {
 	return dev->mmio + (s->index + 1) * II20K_MOD_OFFSET;
 }
 
-static int ii20k_ao_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct ii20k_ao_private *ao_spriv = s->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = ao_spriv->last_data[chan];
-
-	return insn->n;
-}
-
 static int ii20k_ao_insn_write(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn,
 			       unsigned int *data)
 {
-	struct ii20k_ao_private *ao_spriv = s->private;
 	void __iomem *iobase = ii20k_module_iobase(dev, s);
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val = ao_spriv->last_data[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		val = data[i];
+		unsigned int val = data[i];
+
+		s->readback[chan] = val;
 
 		/* munge data */
 		val += ((s->maxdata + 1) >> 1);
@@ -180,8 +161,6 @@ static int ii20k_ao_insn_write(struct comedi_device *dev,
 		writeb(0x00, iobase + II20K_AO_STRB_REG(chan));
 	}
 
-	ao_spriv->last_data[chan] = val;
-
 	return insn->n;
 }
 
@@ -398,26 +377,26 @@ static int ii20k_dio_insn_bits(struct comedi_device *dev,
 static int ii20k_init_module(struct comedi_device *dev,
 			     struct comedi_subdevice *s)
 {
-	struct ii20k_ao_private *ao_spriv;
 	void __iomem *iobase = ii20k_module_iobase(dev, s);
 	unsigned char id;
+	int ret;
 
 	id = readb(iobase + II20K_ID_REG);
 	switch (id) {
 	case II20K_ID_PCI20006M_1:
 	case II20K_ID_PCI20006M_2:
-		ao_spriv = comedi_alloc_spriv(s, sizeof(*ao_spriv));
-		if (!ao_spriv)
-			return -ENOMEM;
-
 		/* Analog Output subdevice */
 		s->type		= COMEDI_SUBD_AO;
 		s->subdev_flags	= SDF_WRITABLE;
 		s->n_chan	= (id == II20K_ID_PCI20006M_2) ? 2 : 1;
 		s->maxdata	= 0xffff;
 		s->range_table	= &ii20k_ao_ranges;
-		s->insn_read	= ii20k_ao_insn_read;
 		s->insn_write	= ii20k_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 		break;
 	case II20K_ID_PCI20341M_1:
 		/* Analog Input subdevice */
diff --git a/drivers/staging/comedi/drivers/jr3_pci.c b/drivers/staging/comedi/drivers/jr3_pci.c
index 7b20e19ecbf7..81fab2dfafa4 100644
--- a/drivers/staging/comedi/drivers/jr3_pci.c
+++ b/drivers/staging/comedi/drivers/jr3_pci.c
@@ -681,7 +681,7 @@ static int jr3_pci_auto_attach(struct comedi_device *dev,
 			       unsigned long context)
 {
 	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	static const struct jr3_pci_board *board = NULL;
+	static const struct jr3_pci_board *board;
 	struct jr3_pci_dev_private *devpriv;
 	struct jr3_pci_subdev_private *spriv;
 	struct comedi_subdevice *s;
diff --git a/drivers/staging/comedi/drivers/ke_counter.c b/drivers/staging/comedi/drivers/ke_counter.c
index f46722c2648f..77e94a34b51e 100644
--- a/drivers/staging/comedi/drivers/ke_counter.c
+++ b/drivers/staging/comedi/drivers/ke_counter.c
@@ -212,7 +212,7 @@ static struct comedi_driver ke_counter_driver = {
 	.driver_name	= "ke_counter",
 	.module		= THIS_MODULE,
 	.auto_attach	= ke_counter_auto_attach,
-	.detach		= comedi_pci_disable,
+	.detach		= comedi_pci_detach,
 };
 
 static int ke_counter_pci_probe(struct pci_dev *dev,
diff --git a/drivers/staging/comedi/drivers/me4000.c b/drivers/staging/comedi/drivers/me4000.c
index 9a5c535451a1..6561b00bea59 100644
--- a/drivers/staging/comedi/drivers/me4000.c
+++ b/drivers/staging/comedi/drivers/me4000.c
@@ -176,8 +176,6 @@ broken.
 struct me4000_info {
 	unsigned long plx_regbase;
 	unsigned long timer_regbase;
-
-	unsigned int ao_readback[4];
 };
 
 enum me4000_boardid {
@@ -473,7 +471,7 @@ static int me4000_ai_insn_read(struct comedi_device *dev,
 			       struct comedi_subdevice *subdevice,
 			       struct comedi_insn *insn, unsigned int *data)
 {
-	const struct me4000_board *thisboard = comedi_board(dev);
+	const struct me4000_board *thisboard = dev->board_ptr;
 	int chan = CR_CHAN(insn->chanspec);
 	int rang = CR_RANGE(insn->chanspec);
 	int aref = CR_AREF(insn->chanspec);
@@ -601,7 +599,7 @@ static int me4000_ai_check_chanlist(struct comedi_device *dev,
 				    struct comedi_subdevice *s,
 				    struct comedi_cmd *cmd)
 {
-	const struct me4000_board *board = comedi_board(dev);
+	const struct me4000_board *board = dev->board_ptr;
 	unsigned int max_diff_chan = board->ai_diff_nchan;
 	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
 	int i;
@@ -617,7 +615,7 @@ static int me4000_ai_check_chanlist(struct comedi_device *dev,
 			return -EINVAL;
 		}
 
-		if (aref == SDF_DIFF) {
+		if (aref == AREF_DIFF) {
 			if (chan >= max_diff_chan) {
 				dev_dbg(dev->class_dev,
 					"Channel number to high\n");
@@ -652,10 +650,10 @@ static int ai_round_cmd_args(struct comedi_device *dev,
 		*init_ticks = (cmd->start_arg * 33) / 1000;
 		rest = (cmd->start_arg * 33) % 1000;
 
-		if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_NEAREST) {
+		if ((cmd->flags & CMDF_ROUND_MASK) == CMDF_ROUND_NEAREST) {
 			if (rest > 33)
 				(*init_ticks)++;
-		} else if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_UP) {
+		} else if ((cmd->flags & CMDF_ROUND_MASK) == CMDF_ROUND_UP) {
 			if (rest)
 				(*init_ticks)++;
 		}
@@ -665,10 +663,10 @@ static int ai_round_cmd_args(struct comedi_device *dev,
 		*scan_ticks = (cmd->scan_begin_arg * 33) / 1000;
 		rest = (cmd->scan_begin_arg * 33) % 1000;
 
-		if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_NEAREST) {
+		if ((cmd->flags & CMDF_ROUND_MASK) == CMDF_ROUND_NEAREST) {
 			if (rest > 33)
 				(*scan_ticks)++;
-		} else if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_UP) {
+		} else if ((cmd->flags & CMDF_ROUND_MASK) == CMDF_ROUND_UP) {
 			if (rest)
 				(*scan_ticks)++;
 		}
@@ -678,10 +676,10 @@ static int ai_round_cmd_args(struct comedi_device *dev,
 		*chan_ticks = (cmd->convert_arg * 33) / 1000;
 		rest = (cmd->convert_arg * 33) % 1000;
 
-		if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_NEAREST) {
+		if ((cmd->flags & CMDF_ROUND_MASK) == CMDF_ROUND_NEAREST) {
 			if (rest > 33)
 				(*chan_ticks)++;
-		} else if ((cmd->flags & TRIG_ROUND_MASK) == TRIG_ROUND_UP) {
+		} else if ((cmd->flags & CMDF_ROUND_MASK) == CMDF_ROUND_UP) {
 			if (rest)
 				(*chan_ticks)++;
 		}
@@ -731,7 +729,7 @@ static int ai_write_chanlist(struct comedi_device *dev,
 		else
 			entry |= ME4000_AI_LIST_RANGE_BIPOLAR_10;
 
-		if (aref == SDF_DIFF)
+		if (aref == AREF_DIFF)
 			entry |= ME4000_AI_LIST_INPUT_DIFFERENTIAL;
 		else
 			entry |= ME4000_AI_LIST_INPUT_SINGLE_ENDED;
@@ -851,7 +849,7 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 	int err = 0;
 
 	/* Only rounding flags are implemented */
-	cmd->flags &= TRIG_ROUND_NEAREST | TRIG_ROUND_UP | TRIG_ROUND_DOWN;
+	cmd->flags &= CMDF_ROUND_NEAREST | CMDF_ROUND_UP | CMDF_ROUND_DOWN;
 
 	/* Round the timer arguments */
 	ai_round_cmd_args(dev, s, cmd, &init_ticks, &scan_ticks, &chan_ticks);
@@ -925,6 +923,11 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 		err |= -EINVAL;
 	}
 
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+
 	if (err)
 		return 3;
 
@@ -1031,13 +1034,6 @@ static int me4000_ai_do_cmd_test(struct comedi_device *dev,
 			err++;
 		}
 	}
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (cmd->stop_arg == 0) {
-			dev_err(dev->class_dev, "Invalid stop arg\n");
-			cmd->stop_arg = 1;
-			err++;
-		}
-	}
 	if (cmd->scan_end_src == TRIG_COUNT) {
 		if (cmd->scan_end_arg == 0) {
 			dev_err(dev->class_dev, "Invalid scan end arg\n");
@@ -1188,44 +1184,14 @@ static irqreturn_t me4000_ai_isr(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-/*=============================================================================
-  Analog output section
-  ===========================================================================*/
-
 static int me4000_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
+				struct comedi_insn *insn,
+				unsigned int *data)
 {
-	const struct me4000_board *thisboard = comedi_board(dev);
-	struct me4000_info *info = dev->private;
 	int chan = CR_CHAN(insn->chanspec);
-	int rang = CR_RANGE(insn->chanspec);
-	int aref = CR_AREF(insn->chanspec);
 	unsigned int tmp;
 
-	if (insn->n == 0) {
-		return 0;
-	} else if (insn->n > 1) {
-		dev_err(dev->class_dev, "Invalid instruction length %d\n",
-			insn->n);
-		return -EINVAL;
-	}
-
-	if (chan >= thisboard->ao_nchan) {
-		dev_err(dev->class_dev, "Invalid channel %d\n", insn->n);
-		return -EINVAL;
-	}
-
-	if (rang != 0) {
-		dev_err(dev->class_dev, "Invalid range %d\n", insn->n);
-		return -EINVAL;
-	}
-
-	if (aref != AREF_GROUND && aref != AREF_COMMON) {
-		dev_err(dev->class_dev, "Invalid aref %d\n", insn->n);
-		return -EINVAL;
-	}
-
 	/* Stop any running conversion */
 	tmp = inl(dev->iobase + ME4000_AO_CTRL_REG(chan));
 	tmp |= ME4000_AO_CTRL_BIT_IMMEDIATE_STOP;
@@ -1238,26 +1204,7 @@ static int me4000_ao_insn_write(struct comedi_device *dev,
 	outl(data[0], dev->iobase + ME4000_AO_SINGLE_REG(chan));
 
 	/* Store in the mirror */
-	info->ao_readback[chan] = data[0];
-
-	return 1;
-}
-
-static int me4000_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn, unsigned int *data)
-{
-	struct me4000_info *info = dev->private;
-	int chan = CR_CHAN(insn->chanspec);
-
-	if (insn->n == 0) {
-		return 0;
-	} else if (insn->n > 1) {
-		dev_err(dev->class_dev, "Invalid instruction length\n");
-		return -EINVAL;
-	}
-
-	data[0] = info->ao_readback[chan];
+	s->readback[chan] = data[0];
 
 	return 1;
 }
@@ -1507,7 +1454,11 @@ static int me4000_auto_attach(struct comedi_device *dev,
 		s->maxdata = 0xFFFF;	/*  16 bit DAC */
 		s->range_table = &range_bipolar10;
 		s->insn_write = me4000_ao_insn_write;
-		s->insn_read = me4000_ao_insn_read;
+		s->insn_read = comedi_readback_insn_read;
+
+		result = comedi_alloc_subdev_readback(s);
+		if (result)
+			return result;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
@@ -1563,11 +1514,9 @@ static int me4000_auto_attach(struct comedi_device *dev,
 
 static void me4000_detach(struct comedi_device *dev)
 {
-	if (dev->irq)
-		free_irq(dev->irq, dev);
 	if (dev->iobase)
 		me4000_reset(dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver me4000_driver = {
diff --git a/drivers/staging/comedi/drivers/me_daq.c b/drivers/staging/comedi/drivers/me_daq.c
index 37a6fa92c656..00eaaf8ac148 100644
--- a/drivers/staging/comedi/drivers/me_daq.c
+++ b/drivers/staging/comedi/drivers/me_daq.c
@@ -171,7 +171,6 @@ struct me_private_data {
 	unsigned short control_1;	/* Mirror of CONTROL_1 register */
 	unsigned short control_2;	/* Mirror of CONTROL_2 register */
 	unsigned short dac_control;	/* Mirror of the DAC_CONTROL register */
-	int ao_readback[4];	/* Mirror of analog output data */
 };
 
 static inline void sleep(unsigned sec)
@@ -325,6 +324,7 @@ static int me_ao_insn_write(struct comedi_device *dev,
 	struct me_private_data *dev_private = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int rang = CR_RANGE(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
 
 	/* Enable all DAC */
@@ -353,10 +353,11 @@ static int me_ao_insn_write(struct comedi_device *dev,
 
 	/* Set data register */
 	for (i = 0; i < insn->n; i++) {
-		writew((data[0] & s->maxdata),
-		       dev->mmio + ME_DAC_DATA_A + (chan << 1));
-		dev_private->ao_readback[chan] = (data[0] & s->maxdata);
+		val = data[i];
+
+		writew(val, dev->mmio + ME_DAC_DATA_A + (chan << 1));
 	}
+	s->readback[chan] = val;
 
 	/* Update dac with data registers */
 	readw(dev->mmio + ME_DAC_UPDATE);
@@ -364,21 +365,6 @@ static int me_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int me_ao_insn_read(struct comedi_device *dev,
-			   struct comedi_subdevice *s,
-			   struct comedi_insn *insn,
-			   unsigned int *data)
-{
-	struct me_private_data *dev_private = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = dev_private->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int me2600_xilinx_download(struct comedi_device *dev,
 				  const u8 *data, size_t size,
 				  unsigned long context)
@@ -530,8 +516,12 @@ static int me_auto_attach(struct comedi_device *dev,
 		s->maxdata	= 0x0fff;
 		s->len_chanlist	= 4;
 		s->range_table	= &me_ao_range;
-		s->insn_read	= me_ao_insn_read;
 		s->insn_write	= me_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type = COMEDI_SUBD_UNUSED;
 	}
@@ -554,14 +544,12 @@ static void me_detach(struct comedi_device *dev)
 	struct me_private_data *dev_private = dev->private;
 
 	if (dev_private) {
-		if (dev->mmio) {
+		if (dev->mmio)
 			me_reset(dev);
-			iounmap(dev->mmio);
-		}
 		if (dev_private->plx_regbase)
 			iounmap(dev_private->plx_regbase);
 	}
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver me_daq_driver = {
diff --git a/drivers/staging/comedi/drivers/mf6x4.c b/drivers/staging/comedi/drivers/mf6x4.c
index 464f4b4745c7..c8d3a22c5896 100644
--- a/drivers/staging/comedi/drivers/mf6x4.c
+++ b/drivers/staging/comedi/drivers/mf6x4.c
@@ -58,7 +58,6 @@
 #define MF6X4_DA7_R					0x2e
 /* Map DAC cahnnel id to real HW-dependent offset value */
 #define MF6X4_DAC_R(x)					(0x20 + ((x) * 2))
-#define MF6X4_DA_M					0x3fff
 
 /* BAR2 registers */
 #define MF634_GPIOC_R					0x68
@@ -101,9 +100,6 @@ struct mf6x4_private {
 	 * offsets -- this variable makes the access easier
 	 */
 	void __iomem *gpioc_R;
-
-	/* DAC value cache -- used for insn_read function */
-	int ao_readback[8];
 };
 
 static int mf6x4_di_insn_bits(struct comedi_device *dev,
@@ -182,6 +178,7 @@ static int mf6x4_ao_insn_write(struct comedi_device *dev,
 {
 	struct mf6x4_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	uint32_t gpioc;
 	int i;
 
@@ -191,24 +188,10 @@ static int mf6x4_ao_insn_write(struct comedi_device *dev,
 		  devpriv->gpioc_R);
 
 	for (i = 0; i < insn->n; i++) {
-		iowrite16(data[i] & MF6X4_DA_M, dev->mmio + MF6X4_DAC_R(chan));
-
-		devpriv->ao_readback[chan] = data[i];
+		val = data[i];
+		iowrite16(val, dev->mmio + MF6X4_DAC_R(chan));
 	}
-
-	return insn->n;
-}
-
-static int mf6x4_ao_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn, unsigned int *data)
-{
-	struct mf6x4_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+	s->readback[chan] = val;
 
 	return insn->n;
 }
@@ -276,7 +259,11 @@ static int mf6x4_auto_attach(struct comedi_device *dev, unsigned long context)
 	s->maxdata = 0x3fff; /* 14 bits DAC */
 	s->range_table = &range_bipolar10;
 	s->insn_write = mf6x4_ao_insn_write;
-	s->insn_read = mf6x4_ao_insn_read;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	/* DIN */
 	s = &dev->subdevices[2];
@@ -303,14 +290,13 @@ static void mf6x4_detach(struct comedi_device *dev)
 {
 	struct mf6x4_private *devpriv = dev->private;
 
-	if (devpriv->bar0_mem)
-		iounmap(devpriv->bar0_mem);
-	if (dev->mmio)
-		iounmap(dev->mmio);
-	if (devpriv->bar2_mem)
-		iounmap(devpriv->bar2_mem);
-
-	comedi_pci_disable(dev);
+	if (devpriv) {
+		if (devpriv->bar0_mem)
+			iounmap(devpriv->bar0_mem);
+		if (devpriv->bar2_mem)
+			iounmap(devpriv->bar2_mem);
+	}
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver mf6x4_driver = {
diff --git a/drivers/staging/comedi/drivers/multiq3.c b/drivers/staging/comedi/drivers/multiq3.c
index e841a5a3ec4f..f710c8e81320 100644
--- a/drivers/staging/comedi/drivers/multiq3.c
+++ b/drivers/staging/comedi/drivers/multiq3.c
@@ -75,10 +75,6 @@ Devices: [Quanser Consulting] MultiQ-3 (multiq3)
 
 #define MULTIQ3_TIMEOUT 30
 
-struct multiq3_private {
-	unsigned int ao_readback[2];
-};
-
 static int multiq3_ai_status(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn,
@@ -126,38 +122,25 @@ static int multiq3_ai_insn_read(struct comedi_device *dev,
 	return n;
 }
 
-static int multiq3_ao_insn_read(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn, unsigned int *data)
-{
-	struct multiq3_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
-}
-
 static int multiq3_ao_insn_write(struct comedi_device *dev,
 				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	struct multiq3_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
-	int chan = CR_CHAN(insn->chanspec);
 
 	for (i = 0; i < insn->n; i++) {
+		val = data[i];
 		outw(MULTIQ3_CONTROL_MUST | MULTIQ3_DA_LOAD | chan,
 		     dev->iobase + MULTIQ3_CONTROL);
-		outw(data[i], dev->iobase + MULTIQ3_DAC_DATA);
+		outw(val, dev->iobase + MULTIQ3_DAC_DATA);
 		outw(MULTIQ3_CONTROL_MUST, dev->iobase + MULTIQ3_CONTROL);
-
-		devpriv->ao_readback[chan] = data[i];
 	}
+	s->readback[chan] = val;
 
-	return i;
+	return insn->n;
 }
 
 static int multiq3_di_insn_bits(struct comedi_device *dev,
@@ -227,7 +210,6 @@ static void encoder_reset(struct comedi_device *dev)
 static int multiq3_attach(struct comedi_device *dev,
 			  struct comedi_devconfig *it)
 {
-	struct multiq3_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
 
@@ -239,10 +221,6 @@ static int multiq3_attach(struct comedi_device *dev,
 	if (ret)
 		return ret;
 
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
 	s = &dev->subdevices[0];
 	/* ai subdevice */
 	s->type = COMEDI_SUBD_AI;
@@ -257,10 +235,14 @@ static int multiq3_attach(struct comedi_device *dev,
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 8;
-	s->insn_read = multiq3_ao_insn_read;
-	s->insn_write = multiq3_ao_insn_write;
 	s->maxdata = 0xfff;
 	s->range_table = &range_bipolar5;
+	s->insn_write = multiq3_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* di subdevice */
diff --git a/drivers/staging/comedi/drivers/ni_6527.c b/drivers/staging/comedi/drivers/ni_6527.c
index e84dac2bf3b2..45fb601e4080 100644
--- a/drivers/staging/comedi/drivers/ni_6527.c
+++ b/drivers/staging/comedi/drivers/ni_6527.c
@@ -252,10 +252,9 @@ static int ni6527_intr_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: fix up any arguments */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
@@ -472,11 +471,7 @@ static void ni6527_detach(struct comedi_device *dev)
 {
 	if (dev->mmio)
 		ni6527_reset(dev);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	if (dev->mmio)
-		iounmap(dev->mmio);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver ni6527_driver = {
diff --git a/drivers/staging/comedi/drivers/ni_65xx.c b/drivers/staging/comedi/drivers/ni_65xx.c
index 873941be56cb..3b642861eb36 100644
--- a/drivers/staging/comedi/drivers/ni_65xx.c
+++ b/drivers/staging/comedi/drivers/ni_65xx.c
@@ -294,7 +294,7 @@ MODULE_PARM_DESC(legacy_invert_outputs,
 
 static unsigned int ni_65xx_num_ports(struct comedi_device *dev)
 {
-	const struct ni_65xx_board *board = comedi_board(dev);
+	const struct ni_65xx_board *board = dev->board_ptr;
 
 	return board->num_dio_ports + board->num_di_ports + board->num_do_ports;
 }
@@ -548,10 +548,9 @@ static int ni_65xx_intr_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: fix up any arguments */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
@@ -793,13 +792,9 @@ static int ni_65xx_auto_attach(struct comedi_device *dev,
 
 static void ni_65xx_detach(struct comedi_device *dev)
 {
-	if (dev->mmio) {
+	if (dev->mmio)
 		writeb(0x00, dev->mmio + NI_65XX_CTRL_REG);
-		iounmap(dev->mmio);
-	}
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
 }
 
 static struct comedi_driver ni_65xx_driver = {
diff --git a/drivers/staging/comedi/drivers/ni_660x.c b/drivers/staging/comedi/drivers/ni_660x.c
index b0b03d4d6081..5b6794c8232e 100644
--- a/drivers/staging/comedi/drivers/ni_660x.c
+++ b/drivers/staging/comedi/drivers/ni_660x.c
@@ -433,7 +433,7 @@ struct ni_660x_private {
 
 static inline unsigned ni_660x_num_counters(struct comedi_device *dev)
 {
-	const struct ni_660x_board *board = comedi_board(dev);
+	const struct ni_660x_board *board = dev->board_ptr;
 
 	return board->n_chips * counters_per_chip;
 }
@@ -852,7 +852,7 @@ static int ni_660x_allocate_private(struct comedi_device *dev)
 
 static int ni_660x_alloc_mite_rings(struct comedi_device *dev)
 {
-	const struct ni_660x_board *board = comedi_board(dev);
+	const struct ni_660x_board *board = dev->board_ptr;
 	struct ni_660x_private *devpriv = dev->private;
 	unsigned i;
 	unsigned j;
@@ -870,7 +870,7 @@ static int ni_660x_alloc_mite_rings(struct comedi_device *dev)
 
 static void ni_660x_free_mite_rings(struct comedi_device *dev)
 {
-	const struct ni_660x_board *board = comedi_board(dev);
+	const struct ni_660x_board *board = dev->board_ptr;
 	struct ni_660x_private *devpriv = dev->private;
 	unsigned i;
 	unsigned j;
@@ -924,7 +924,7 @@ static void ni_660x_select_pfi_output(struct comedi_device *dev,
 				      unsigned pfi_channel,
 				      unsigned output_select)
 {
-	const struct ni_660x_board *board = comedi_board(dev);
+	const struct ni_660x_board *board = dev->board_ptr;
 	static const unsigned counter_4_7_first_pfi = 8;
 	static const unsigned counter_4_7_last_pfi = 23;
 	unsigned active_chipset = 0;
diff --git a/drivers/staging/comedi/drivers/ni_670x.c b/drivers/staging/comedi/drivers/ni_670x.c
index f5caefad0b59..54721deb80cc 100644
--- a/drivers/staging/comedi/drivers/ni_670x.c
+++ b/drivers/staging/comedi/drivers/ni_670x.c
@@ -83,52 +83,38 @@ static const struct ni_670x_board ni_670x_boards[] = {
 struct ni_670x_private {
 	int boardtype;
 	int dio;
-	unsigned int ao_readback[32];
 };
 
-static int ni_670x_ao_winsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
+static int ni_670x_ao_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
 {
-	struct ni_670x_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	/* Channel number mapping :
-
-	   NI 6703/ NI 6704     | NI 6704 Only
-	   ----------------------------------------------------
-	   vch(0)       :       0       | ich(16)       :       1
-	   vch(1)       :       2       | ich(17)       :       3
-	   .    :       .       |   .                   .
-	   .    :       .       |   .                   .
-	   .    :       .       |   .                   .
-	   vch(15)      :       30      | ich(31)       :       31      */
 
+	/*
+	 * Channel number mapping:
+	 *
+	 * NI 6703/ NI 6704 | NI 6704 Only
+	 * -------------------------------
+	 * vch(0)  :  0     | ich(16) :  1
+	 * vch(1)  :  2     | ich(17) :  3
+	 * ...              | ...
+	 * vch(15) : 30     | ich(31) : 31
+	 */
 	for (i = 0; i < insn->n; i++) {
+		val = data[i];
 		/* First write in channel register which channel to use */
 		writel(((chan & 15) << 1) | ((chan & 16) >> 4),
 		       dev->mmio + AO_CHAN_OFFSET);
 		/* write channel value */
-		writel(data[i], dev->mmio + AO_VALUE_OFFSET);
-		devpriv->ao_readback[chan] = data[i];
+		writel(val, dev->mmio + AO_VALUE_OFFSET);
 	}
+	s->readback[chan] = val;
 
-	return i;
-}
-
-static int ni_670x_ao_rinsn(struct comedi_device *dev,
-			    struct comedi_subdevice *s,
-			    struct comedi_insn *insn, unsigned int *data)
-{
-	struct ni_670x_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static int ni_670x_dio_insn_bits(struct comedi_device *dev,
@@ -241,8 +227,12 @@ static int ni_670x_auto_attach(struct comedi_device *dev,
 	} else {
 		s->range_table = &range_bipolar10;
 	}
-	s->insn_write = &ni_670x_ao_winsn;
-	s->insn_read = &ni_670x_ao_rinsn;
+	s->insn_write = ni_670x_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[1];
 	/* digital i/o subdevice */
@@ -266,14 +256,12 @@ static void ni_670x_detach(struct comedi_device *dev)
 {
 	struct comedi_subdevice *s;
 
+	comedi_pci_detach(dev);
 	if (dev->n_subdevices) {
 		s = &dev->subdevices[0];
 		if (s)
 			kfree(s->range_table_list);
 	}
-	if (dev->mmio)
-		iounmap(dev->mmio);
-	comedi_pci_disable(dev);
 }
 
 static struct comedi_driver ni_670x_driver = {
diff --git a/drivers/staging/comedi/drivers/ni_at_a2150.c b/drivers/staging/comedi/drivers/ni_at_a2150.c
index de67161f6185..72ec857d073e 100644
--- a/drivers/staging/comedi/drivers/ni_at_a2150.c
+++ b/drivers/staging/comedi/drivers/ni_at_a2150.c
@@ -287,7 +287,7 @@ static int a2150_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 			    unsigned int flags)
 {
-	const struct a2150_board *thisboard = comedi_board(dev);
+	const struct a2150_board *thisboard = dev->board_ptr;
 	struct a2150_private *devpriv = dev->private;
 	int lub, glb, temp;
 	int lub_divisor_shift, lub_index, glb_divisor_shift, glb_index;
@@ -326,8 +326,8 @@ static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 			}
 		}
 	}
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_NEAREST:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
 	default:
 		/*  if least upper bound is better approximation */
 		if (lub - *period < *period - glb)
@@ -335,10 +335,10 @@ static int a2150_get_timing(struct comedi_device *dev, unsigned int *period,
 		else
 			*period = glb;
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		*period = lub;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		*period = glb;
 		break;
 	}
@@ -436,7 +436,7 @@ static int a2150_ai_check_chanlist(struct comedi_device *dev,
 static int a2150_ai_cmdtest(struct comedi_device *dev,
 			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct a2150_board *thisboard = comedi_board(dev);
+	const struct a2150_board *thisboard = dev->board_ptr;
 	int err = 0;
 	unsigned int arg;
 
@@ -511,9 +511,9 @@ static int a2150_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	unsigned int old_config_bits = devpriv->config_bits;
 	unsigned int trigger_bits;
 
-	if (cmd->flags & TRIG_RT) {
+	if (cmd->flags & CMDF_PRIORITY) {
 		dev_err(dev->class_dev,
-			"dma incompatible with hard real-time interrupt (TRIG_RT), aborting\n");
+			"dma incompatible with hard real-time interrupt (CMDF_PRIORITY), aborting\n");
 		return -1;
 	}
 	/*  clear fifo and reset triggering circuitry */
@@ -705,8 +705,12 @@ static int a2150_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	if (ret)
 		return ret;
 
-	dev->board_ptr = a2150_boards + a2150_probe(dev);
-	thisboard = comedi_board(dev);
+	i = a2150_probe(dev);
+	if (i >= ARRAY_SIZE(a2150_boards))
+		return -ENODEV;
+
+	dev->board_ptr = a2150_boards + i;
+	thisboard = dev->board_ptr;
 	dev->board_name = thisboard->name;
 
 	if ((irq >= 3 && irq <= 7) || (irq >= 9 && irq <= 12) ||
diff --git a/drivers/staging/comedi/drivers/ni_at_ao.c b/drivers/staging/comedi/drivers/ni_at_ao.c
index c93b47bcca51..3e1ce5866147 100644
--- a/drivers/staging/comedi/drivers/ni_at_ao.c
+++ b/drivers/staging/comedi/drivers/ni_at_ao.c
@@ -118,9 +118,6 @@ struct atao_private {
 	unsigned short cfg1;
 	unsigned short cfg3;
 
-	/* Used for AO readback */
-	unsigned int ao_readback[10];
-
 	/* Used for caldac readback */
 	unsigned char caldac[21];
 };
@@ -141,9 +138,8 @@ static int atao_ao_insn_write(struct comedi_device *dev,
 			      struct comedi_insn *insn,
 			      unsigned int *data)
 {
-	struct atao_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val;
+	unsigned int val = s->readback[chan];
 	int i;
 
 	if (chan == 0)
@@ -151,12 +147,12 @@ static int atao_ao_insn_write(struct comedi_device *dev,
 
 	for (i = 0; i < insn->n; i++) {
 		val = data[i];
-		devpriv->ao_readback[chan] = val;
 
-		/* munge offset binary (unsigned) to two's complement */
-		val = comedi_offset_munge(s, val);
-		outw(val, dev->iobase + ATAO_AO_REG(chan));
+		/* the hardware expects two's complement values */
+		outw(comedi_offset_munge(s, val),
+		     dev->iobase + ATAO_AO_REG(chan));
 	}
+	s->readback[chan] = val;
 
 	if (chan == 0)
 		atao_select_reg_group(dev, 0);
@@ -164,21 +160,6 @@ static int atao_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int atao_ao_insn_read(struct comedi_device *dev,
-			     struct comedi_subdevice *s,
-			     struct comedi_insn *insn,
-			     unsigned int *data)
-{
-	struct atao_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int atao_dio_insn_bits(struct comedi_device *dev,
 			      struct comedi_subdevice *s,
 			      struct comedi_insn *insn,
@@ -338,7 +319,7 @@ static void atao_reset(struct comedi_device *dev)
 
 static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct atao_board *board = comedi_board(dev);
+	const struct atao_board *board = dev->board_ptr;
 	struct atao_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
@@ -363,7 +344,11 @@ static int atao_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0x0fff;
 	s->range_table	= it->options[3] ? &range_unipolar10 : &range_bipolar10;
 	s->insn_write	= atao_ao_insn_write;
-	s->insn_read	= atao_ao_insn_read;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	/* Digital I/O subdevice */
 	s = &dev->subdevices[1];
diff --git a/drivers/staging/comedi/drivers/ni_atmio.c b/drivers/staging/comedi/drivers/ni_atmio.c
index 2bd9f692a7ae..0c5ff287dcef 100644
--- a/drivers/staging/comedi/drivers/ni_atmio.c
+++ b/drivers/staging/comedi/drivers/ni_atmio.c
@@ -336,7 +336,7 @@ static int ni_atmio_attach(struct comedi_device *dev,
 		return -EIO;
 
 	dev->board_ptr = ni_boards + board;
-	boardtype = comedi_board(dev);
+	boardtype = dev->board_ptr;
 	dev->board_name = boardtype->name;
 
 	/* irq stuff */
diff --git a/drivers/staging/comedi/drivers/ni_atmio16d.c b/drivers/staging/comedi/drivers/ni_atmio16d.c
index 9c08da9508f4..fc3c19de7005 100644
--- a/drivers/staging/comedi/drivers/ni_atmio16d.c
+++ b/drivers/staging/comedi/drivers/ni_atmio16d.c
@@ -138,7 +138,6 @@ struct atmio16d_private {
 	enum { dac_internal, dac_external } dac0_reference, dac1_reference;
 	enum { dac_2comp, dac_straight } dac0_coding, dac1_coding;
 	const struct comedi_lrange *ao_range_type_list[2];
-	unsigned int ao_readback[2];
 	unsigned int com_reg_1_state; /* current state of command register 1 */
 	unsigned int com_reg_2_state; /* current state of command register 2 */
 };
@@ -275,11 +274,10 @@ static int atmio16d_ai_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {	/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -496,48 +494,34 @@ static int atmio16d_ai_insn_read(struct comedi_device *dev,
 	return i;
 }
 
-static int atmio16d_ao_insn_read(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn, unsigned int *data)
-{
-	struct atmio16d_private *devpriv = dev->private;
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
-	return i;
-}
-
 static int atmio16d_ao_insn_write(struct comedi_device *dev,
 				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn, unsigned int *data)
+				  struct comedi_insn *insn,
+				  unsigned int *data)
 {
 	struct atmio16d_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int reg = (chan) ? DAC1_REG : DAC0_REG;
+	bool munge = false;
 	int i;
-	int chan;
-	int d;
 
-	chan = CR_CHAN(insn->chanspec);
+	if (chan == 0 && devpriv->dac0_coding == dac_2comp)
+		munge = true;
+	if (chan == 1 && devpriv->dac1_coding == dac_2comp)
+		munge = true;
 
 	for (i = 0; i < insn->n; i++) {
-		d = data[i];
-		switch (chan) {
-		case 0:
-			if (devpriv->dac0_coding == dac_2comp)
-				d ^= 0x800;
-			outw(d, dev->iobase + DAC0_REG);
-			break;
-		case 1:
-			if (devpriv->dac1_coding == dac_2comp)
-				d ^= 0x800;
-			outw(d, dev->iobase + DAC1_REG);
-			break;
-		default:
-			return -EINVAL;
-		}
-		devpriv->ao_readback[chan] = data[i];
+		unsigned int val = data[i];
+
+		s->readback[chan] = val;
+
+		if (munge)
+			val ^= 0x800;
+
+		outw(val, dev->iobase + reg);
 	}
-	return i;
+
+	return insn->n;
 }
 
 static int atmio16d_dio_insn_bits(struct comedi_device *dev,
@@ -617,7 +601,7 @@ static int atmio16d_dio_insn_config(struct comedi_device *dev,
 static int atmio16d_attach(struct comedi_device *dev,
 			   struct comedi_devconfig *it)
 {
-	const struct atmio16_board_t *board = comedi_board(dev);
+	const struct atmio16_board_t *board = dev->board_ptr;
 	struct atmio16d_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
@@ -688,8 +672,6 @@ static int atmio16d_attach(struct comedi_device *dev,
 	s->type = COMEDI_SUBD_AO;
 	s->subdev_flags = SDF_WRITABLE;
 	s->n_chan = 2;
-	s->insn_read = atmio16d_ao_insn_read;
-	s->insn_write = atmio16d_ao_insn_write;
 	s->maxdata = 0xfff;	/* 4095 decimal */
 	s->range_table_list = devpriv->ao_range_type_list;
 	switch (devpriv->dac0_range) {
@@ -708,6 +690,12 @@ static int atmio16d_attach(struct comedi_device *dev,
 		devpriv->ao_range_type_list[1] = &range_unipolar10;
 		break;
 	}
+	s->insn_write = atmio16d_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	/* Digital I/O */
 	s = &dev->subdevices[2];
@@ -722,7 +710,7 @@ static int atmio16d_attach(struct comedi_device *dev,
 	/* 8255 subdevice */
 	s = &dev->subdevices[3];
 	if (board->has_8255) {
-		ret = subdev_8255_init(dev, s, NULL, dev->iobase);
+		ret = subdev_8255_init(dev, s, NULL, 0x00);
 		if (ret)
 			return ret;
 	} else {
diff --git a/drivers/staging/comedi/drivers/ni_daq_dio24.c b/drivers/staging/comedi/drivers/ni_daq_dio24.c
index 925e82c65b2d..8cfabdbaa30c 100644
--- a/drivers/staging/comedi/drivers/ni_daq_dio24.c
+++ b/drivers/staging/comedi/drivers/ni_daq_dio24.c
@@ -59,7 +59,7 @@ static int dio24_auto_attach(struct comedi_device *dev,
 
 	/* 8255 dio */
 	s = &dev->subdevices[0];
-	ret = subdev_8255_init(dev, s, NULL, dev->iobase);
+	ret = subdev_8255_init(dev, s, NULL, 0x00);
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/comedi/drivers/ni_labpc.c b/drivers/staging/comedi/drivers/ni_labpc.c
index 126d65cb39f2..1fbfdb4c80c0 100644
--- a/drivers/staging/comedi/drivers/ni_labpc.c
+++ b/drivers/staging/comedi/drivers/ni_labpc.c
@@ -58,99 +58,12 @@
  */
 
 #include <linux/module.h>
-#include <linux/interrupt.h>
-#include <linux/slab.h>
-#include <linux/io.h>
-#include <linux/delay.h>
 
 #include "../comedidev.h"
 
-#include "8253.h"
-#include "8255.h"
-#include "comedi_fc.h"
 #include "ni_labpc.h"
-#include "ni_labpc_regs.h"
 #include "ni_labpc_isadma.h"
 
-enum scan_mode {
-	MODE_SINGLE_CHAN,
-	MODE_SINGLE_CHAN_INTERVAL,
-	MODE_MULT_CHAN_UP,
-	MODE_MULT_CHAN_DOWN,
-};
-
-static const struct comedi_lrange range_labpc_plus_ai = {
-	16, {
-		BIP_RANGE(5),
-		BIP_RANGE(4),
-		BIP_RANGE(2.5),
-		BIP_RANGE(1),
-		BIP_RANGE(0.5),
-		BIP_RANGE(0.25),
-		BIP_RANGE(0.1),
-		BIP_RANGE(0.05),
-		UNI_RANGE(10),
-		UNI_RANGE(8),
-		UNI_RANGE(5),
-		UNI_RANGE(2),
-		UNI_RANGE(1),
-		UNI_RANGE(0.5),
-		UNI_RANGE(0.2),
-		UNI_RANGE(0.1)
-	}
-};
-
-static const struct comedi_lrange range_labpc_1200_ai = {
-	14, {
-		BIP_RANGE(5),
-		BIP_RANGE(2.5),
-		BIP_RANGE(1),
-		BIP_RANGE(0.5),
-		BIP_RANGE(0.25),
-		BIP_RANGE(0.1),
-		BIP_RANGE(0.05),
-		UNI_RANGE(10),
-		UNI_RANGE(5),
-		UNI_RANGE(2),
-		UNI_RANGE(1),
-		UNI_RANGE(0.5),
-		UNI_RANGE(0.2),
-		UNI_RANGE(0.1)
-	}
-};
-
-static const struct comedi_lrange range_labpc_ao = {
-	2, {
-		BIP_RANGE(5),
-		UNI_RANGE(10)
-	}
-};
-
-/* functions that do inb/outb and readb/writeb so we can use
- * function pointers to decide which to use */
-static unsigned int labpc_inb(struct comedi_device *dev, unsigned long reg)
-{
-	return inb(dev->iobase + reg);
-}
-
-static void labpc_outb(struct comedi_device *dev,
-		       unsigned int byte, unsigned long reg)
-{
-	outb(byte, dev->iobase + reg);
-}
-
-static unsigned int labpc_readb(struct comedi_device *dev, unsigned long reg)
-{
-	return readb(dev->mmio + reg);
-}
-
-static void labpc_writeb(struct comedi_device *dev,
-			 unsigned int byte, unsigned long reg)
-{
-	writeb(byte, dev->mmio + reg);
-}
-
-#if IS_ENABLED(CONFIG_COMEDI_NI_LABPC_ISA)
 static const struct labpc_boardinfo labpc_boards[] = {
 	{
 		.name			= "lab-pc-1200",
@@ -169,1284 +82,7 @@ static const struct labpc_boardinfo labpc_boards[] = {
 		.has_ao			= 1,
 	},
 };
-#endif
-
-static void labpc_counter_load(struct comedi_device *dev,
-			       unsigned long reg,
-			       unsigned int counter_number,
-			       unsigned int count,
-			       unsigned int mode)
-{
-	if (dev->mmio) {
-		i8254_mm_set_mode(dev->mmio + reg, 0, counter_number, mode);
-		i8254_mm_write(dev->mmio + reg, 0, counter_number, count);
-	} else {
-		i8254_set_mode(dev->iobase + reg, 0, counter_number, mode);
-		i8254_write(dev->iobase + reg, 0, counter_number, count);
-	}
-}
-
-static void labpc_counter_set_mode(struct comedi_device *dev,
-				   unsigned long reg,
-				   unsigned int counter_number,
-				   unsigned int mode)
-{
-	if (dev->mmio)
-		i8254_mm_set_mode(dev->mmio + reg, 0, counter_number, mode);
-	else
-		i8254_set_mode(dev->iobase + reg, 0, counter_number, mode);
-}
-
-static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned long flags;
-
-	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->cmd2 &= ~(CMD2_SWTRIG | CMD2_HWTRIG | CMD2_PRETRIG);
-	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
-
-	devpriv->cmd3 = 0;
-	devpriv->write_byte(dev, devpriv->cmd3, CMD3_REG);
-
-	return 0;
-}
-
-static void labpc_ai_set_chan_and_gain(struct comedi_device *dev,
-				       enum scan_mode mode,
-				       unsigned int chan,
-				       unsigned int range,
-				       unsigned int aref)
-{
-	const struct labpc_boardinfo *board = comedi_board(dev);
-	struct labpc_private *devpriv = dev->private;
-
-	if (board->is_labpc1200) {
-		/*
-		 * The LabPC-1200 boards do not have a gain
-		 * of '0x10'. Skip the range values that would
-		 * result in this gain.
-		 */
-		range += (range > 0) + (range > 7);
-	}
-
-	/* munge channel bits for differential/scan disabled mode */
-	if ((mode == MODE_SINGLE_CHAN || mode == MODE_SINGLE_CHAN_INTERVAL) &&
-	    aref == AREF_DIFF)
-		chan *= 2;
-	devpriv->cmd1 = CMD1_MA(chan);
-	devpriv->cmd1 |= CMD1_GAIN(range);
-
-	devpriv->write_byte(dev, devpriv->cmd1, CMD1_REG);
-}
-
-static void labpc_setup_cmd6_reg(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 enum scan_mode mode,
-				 enum transfer_type xfer,
-				 unsigned int range,
-				 unsigned int aref,
-				 bool ena_intr)
-{
-	const struct labpc_boardinfo *board = comedi_board(dev);
-	struct labpc_private *devpriv = dev->private;
-
-	if (!board->is_labpc1200)
-		return;
-
-	/* reference inputs to ground or common? */
-	if (aref != AREF_GROUND)
-		devpriv->cmd6 |= CMD6_NRSE;
-	else
-		devpriv->cmd6 &= ~CMD6_NRSE;
-
-	/* bipolar or unipolar range? */
-	if (comedi_range_is_unipolar(s, range))
-		devpriv->cmd6 |= CMD6_ADCUNI;
-	else
-		devpriv->cmd6 &= ~CMD6_ADCUNI;
-
-	/*  interrupt on fifo half full? */
-	if (xfer == fifo_half_full_transfer)
-		devpriv->cmd6 |= CMD6_HFINTEN;
-	else
-		devpriv->cmd6 &= ~CMD6_HFINTEN;
-
-	/* enable interrupt on counter a1 terminal count? */
-	if (ena_intr)
-		devpriv->cmd6 |= CMD6_DQINTEN;
-	else
-		devpriv->cmd6 &= ~CMD6_DQINTEN;
-
-	/* are we scanning up or down through channels? */
-	if (mode == MODE_MULT_CHAN_UP)
-		devpriv->cmd6 |= CMD6_SCANUP;
-	else
-		devpriv->cmd6 &= ~CMD6_SCANUP;
-
-	devpriv->write_byte(dev, devpriv->cmd6, CMD6_REG);
-}
-
-static unsigned int labpc_read_adc_fifo(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int lsb = devpriv->read_byte(dev, ADC_FIFO_REG);
-	unsigned int msb = devpriv->read_byte(dev, ADC_FIFO_REG);
-
-	return (msb << 8) | lsb;
-}
-
-static void labpc_clear_adc_fifo(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
-	labpc_read_adc_fifo(dev);
-}
-
-static int labpc_ai_eoc(struct comedi_device *dev,
-			struct comedi_subdevice *s,
-			struct comedi_insn *insn,
-			unsigned long context)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
-	if (devpriv->stat1 & STAT1_DAVAIL)
-		return 0;
-	return -EBUSY;
-}
-
-static int labpc_ai_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int range = CR_RANGE(insn->chanspec);
-	unsigned int aref = CR_AREF(insn->chanspec);
-	int ret;
-	int i;
-
-	/* disable timed conversions, interrupt generation and dma */
-	labpc_cancel(dev, s);
-
-	labpc_ai_set_chan_and_gain(dev, MODE_SINGLE_CHAN, chan, range, aref);
-
-	labpc_setup_cmd6_reg(dev, s, MODE_SINGLE_CHAN, fifo_not_empty_transfer,
-			     range, aref, false);
-
-	/* setup cmd4 register */
-	devpriv->cmd4 = 0;
-	devpriv->cmd4 |= CMD4_ECLKRCV;
-	/* single-ended/differential */
-	if (aref == AREF_DIFF)
-		devpriv->cmd4 |= CMD4_SEDIFF;
-	devpriv->write_byte(dev, devpriv->cmd4, CMD4_REG);
-
-	/* initialize pacer counter to prevent any problems */
-	labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 0, I8254_MODE2);
-
-	labpc_clear_adc_fifo(dev);
-
-	for (i = 0; i < insn->n; i++) {
-		/* trigger conversion */
-		devpriv->write_byte(dev, 0x1, ADC_START_CONVERT_REG);
-
-		ret = comedi_timeout(dev, s, insn, labpc_ai_eoc, 0);
-		if (ret)
-			return ret;
-
-		data[i] = labpc_read_adc_fifo(dev);
-	}
-
-	return insn->n;
-}
-
-static bool labpc_use_continuous_mode(const struct comedi_cmd *cmd,
-				      enum scan_mode mode)
-{
-	if (mode == MODE_SINGLE_CHAN || cmd->scan_begin_src == TRIG_FOLLOW)
-		return true;
-
-	return false;
-}
-
-static unsigned int labpc_ai_convert_period(const struct comedi_cmd *cmd,
-					    enum scan_mode mode)
-{
-	if (cmd->convert_src != TRIG_TIMER)
-		return 0;
-
-	if (mode == MODE_SINGLE_CHAN && cmd->scan_begin_src == TRIG_TIMER)
-		return cmd->scan_begin_arg;
-
-	return cmd->convert_arg;
-}
-
-static void labpc_set_ai_convert_period(struct comedi_cmd *cmd,
-					enum scan_mode mode, unsigned int ns)
-{
-	if (cmd->convert_src != TRIG_TIMER)
-		return;
-
-	if (mode == MODE_SINGLE_CHAN &&
-	    cmd->scan_begin_src == TRIG_TIMER) {
-		cmd->scan_begin_arg = ns;
-		if (cmd->convert_arg > cmd->scan_begin_arg)
-			cmd->convert_arg = cmd->scan_begin_arg;
-	} else
-		cmd->convert_arg = ns;
-}
-
-static unsigned int labpc_ai_scan_period(const struct comedi_cmd *cmd,
-					enum scan_mode mode)
-{
-	if (cmd->scan_begin_src != TRIG_TIMER)
-		return 0;
-
-	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
-		return 0;
-
-	return cmd->scan_begin_arg;
-}
-
-static void labpc_set_ai_scan_period(struct comedi_cmd *cmd,
-				     enum scan_mode mode, unsigned int ns)
-{
-	if (cmd->scan_begin_src != TRIG_TIMER)
-		return;
-
-	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
-		return;
-
-	cmd->scan_begin_arg = ns;
-}
-
-/* figures out what counter values to use based on command */
-static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
-			     enum scan_mode mode)
-{
-	struct labpc_private *devpriv = dev->private;
-	/* max value for 16 bit counter in mode 2 */
-	const int max_counter_value = 0x10000;
-	/* min value for 16 bit counter in mode 2 */
-	const int min_counter_value = 2;
-	unsigned int base_period;
-	unsigned int scan_period;
-	unsigned int convert_period;
-
-	/*
-	 * if both convert and scan triggers are TRIG_TIMER, then they
-	 * both rely on counter b0
-	 */
-	convert_period = labpc_ai_convert_period(cmd, mode);
-	scan_period = labpc_ai_scan_period(cmd, mode);
-	if (convert_period && scan_period) {
-		/*
-		 * pick the lowest b0 divisor value we can (for maximum input
-		 * clock speed on convert and scan counters)
-		 */
-		devpriv->divisor_b0 = (scan_period - 1) /
-		    (I8254_OSC_BASE_2MHZ * max_counter_value) + 1;
-		if (devpriv->divisor_b0 < min_counter_value)
-			devpriv->divisor_b0 = min_counter_value;
-		if (devpriv->divisor_b0 > max_counter_value)
-			devpriv->divisor_b0 = max_counter_value;
-
-		base_period = I8254_OSC_BASE_2MHZ * devpriv->divisor_b0;
-
-		/*  set a0 for conversion frequency and b1 for scan frequency */
-		switch (cmd->flags & TRIG_ROUND_MASK) {
-		default:
-		case TRIG_ROUND_NEAREST:
-			devpriv->divisor_a0 =
-			    (convert_period + (base_period / 2)) / base_period;
-			devpriv->divisor_b1 =
-			    (scan_period + (base_period / 2)) / base_period;
-			break;
-		case TRIG_ROUND_UP:
-			devpriv->divisor_a0 =
-			    (convert_period + (base_period - 1)) / base_period;
-			devpriv->divisor_b1 =
-			    (scan_period + (base_period - 1)) / base_period;
-			break;
-		case TRIG_ROUND_DOWN:
-			devpriv->divisor_a0 = convert_period / base_period;
-			devpriv->divisor_b1 = scan_period / base_period;
-			break;
-		}
-		/*  make sure a0 and b1 values are acceptable */
-		if (devpriv->divisor_a0 < min_counter_value)
-			devpriv->divisor_a0 = min_counter_value;
-		if (devpriv->divisor_a0 > max_counter_value)
-			devpriv->divisor_a0 = max_counter_value;
-		if (devpriv->divisor_b1 < min_counter_value)
-			devpriv->divisor_b1 = min_counter_value;
-		if (devpriv->divisor_b1 > max_counter_value)
-			devpriv->divisor_b1 = max_counter_value;
-		/*  write corrected timings to command */
-		labpc_set_ai_convert_period(cmd, mode,
-					    base_period * devpriv->divisor_a0);
-		labpc_set_ai_scan_period(cmd, mode,
-					 base_period * devpriv->divisor_b1);
-		/*
-		 * if only one TRIG_TIMER is used, we can employ the generic
-		 * cascaded timing functions
-		 */
-	} else if (scan_period) {
-		/*
-		 * calculate cascaded counter values
-		 * that give desired scan timing
-		 */
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
-					  &devpriv->divisor_b1,
-					  &devpriv->divisor_b0,
-					  &scan_period, cmd->flags);
-		labpc_set_ai_scan_period(cmd, mode, scan_period);
-	} else if (convert_period) {
-		/*
-		 * calculate cascaded counter values
-		 * that give desired conversion timing
-		 */
-		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
-					  &devpriv->divisor_a0,
-					  &devpriv->divisor_b0,
-					  &convert_period, cmd->flags);
-		labpc_set_ai_convert_period(cmd, mode, convert_period);
-	}
-}
-
-static enum scan_mode labpc_ai_scan_mode(const struct comedi_cmd *cmd)
-{
-	unsigned int chan0;
-	unsigned int chan1;
-
-	if (cmd->chanlist_len == 1)
-		return MODE_SINGLE_CHAN;
-
-	/* chanlist may be NULL during cmdtest */
-	if (cmd->chanlist == NULL)
-		return MODE_MULT_CHAN_UP;
-
-	chan0 = CR_CHAN(cmd->chanlist[0]);
-	chan1 = CR_CHAN(cmd->chanlist[1]);
-
-	if (chan0 < chan1)
-		return MODE_MULT_CHAN_UP;
-
-	if (chan0 > chan1)
-		return MODE_MULT_CHAN_DOWN;
-
-	return MODE_SINGLE_CHAN_INTERVAL;
-}
-
-static int labpc_ai_check_chanlist(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_cmd *cmd)
-{
-	enum scan_mode mode = labpc_ai_scan_mode(cmd);
-	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
-	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
-	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
-	int i;
-
-	if (mode == MODE_SINGLE_CHAN)
-		return 0;
-
-	for (i = 0; i < cmd->chanlist_len; i++) {
-		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
-		unsigned int range = CR_RANGE(cmd->chanlist[i]);
-		unsigned int aref = CR_AREF(cmd->chanlist[i]);
-
-		switch (mode) {
-		case MODE_SINGLE_CHAN:
-			break;
-		case MODE_SINGLE_CHAN_INTERVAL:
-			if (chan != chan0) {
-				dev_dbg(dev->class_dev,
-					"channel scanning order specified in chanlist is not supported by hardware\n");
-				return -EINVAL;
-			}
-			break;
-		case MODE_MULT_CHAN_UP:
-			if (chan != i) {
-				dev_dbg(dev->class_dev,
-					"channel scanning order specified in chanlist is not supported by hardware\n");
-				return -EINVAL;
-			}
-			break;
-		case MODE_MULT_CHAN_DOWN:
-			if (chan != (cmd->chanlist_len - i - 1)) {
-				dev_dbg(dev->class_dev,
-					"channel scanning order specified in chanlist is not supported by hardware\n");
-				return -EINVAL;
-			}
-			break;
-		}
-
-		if (range != range0) {
-			dev_dbg(dev->class_dev,
-				"entries in chanlist must all have the same range\n");
-			return -EINVAL;
-		}
-
-		if (aref != aref0) {
-			dev_dbg(dev->class_dev,
-				"entries in chanlist must all have the same reference\n");
-			return -EINVAL;
-		}
-	}
-
-	return 0;
-}
-
-static int labpc_ai_cmdtest(struct comedi_device *dev,
-			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
-{
-	const struct labpc_boardinfo *board = comedi_board(dev);
-	int err = 0;
-	int tmp, tmp2;
-	unsigned int stop_mask;
-	enum scan_mode mode;
-
-	/* Step 1 : check if triggers are trivially valid */
-
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-
-	stop_mask = TRIG_COUNT | TRIG_NONE;
-	if (board->is_labpc1200)
-		stop_mask |= TRIG_EXT;
-	err |= cfc_check_trigger_src(&cmd->stop_src, stop_mask);
-
-	if (err)
-		return 1;
-
-	/* Step 2a : make sure trigger sources are unique */
-
-	err |= cfc_check_trigger_is_unique(cmd->start_src);
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
-
-	/* Step 2b : and mutually compatible */
-
-	/* can't have external stop and start triggers at once */
-	if (cmd->start_src == TRIG_EXT && cmd->stop_src == TRIG_EXT)
-		err++;
-
-	if (err)
-		return 2;
-
-	/* Step 3: check if arguments are trivially valid */
-
-	switch (cmd->start_src) {
-	case TRIG_NOW:
-		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-		break;
-	case TRIG_EXT:
-		/* start_arg value is ignored */
-		break;
-	}
-
-	if (!cmd->chanlist_len)
-		err |= -EINVAL;
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-
-	if (cmd->convert_src == TRIG_TIMER)
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
-						 board->ai_speed);
-
-	/* make sure scan timing is not too fast */
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		if (cmd->convert_src == TRIG_TIMER)
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-					cmd->convert_arg * cmd->chanlist_len);
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-				board->ai_speed * cmd->chanlist_len);
-	}
-
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
-		break;
-	case TRIG_NONE:
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-		break;
-		/*
-		 * TRIG_EXT doesn't care since it doesn't
-		 * trigger off a numbered channel
-		 */
-	default:
-		break;
-	}
-
-	if (err)
-		return 3;
-
-	/* step 4: fix up any arguments */
-
-	tmp = cmd->convert_arg;
-	tmp2 = cmd->scan_begin_arg;
-	mode = labpc_ai_scan_mode(cmd);
-	labpc_adc_timing(dev, cmd, mode);
-	if (tmp != cmd->convert_arg || tmp2 != cmd->scan_begin_arg)
-		err++;
-
-	if (err)
-		return 4;
-
-	/* Step 5: check channel list if it exists */
-	if (cmd->chanlist && cmd->chanlist_len > 0)
-		err |= labpc_ai_check_chanlist(dev, s, cmd);
-
-	if (err)
-		return 5;
-
-	return 0;
-}
-
-static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
-{
-	const struct labpc_boardinfo *board = comedi_board(dev);
-	struct labpc_private *devpriv = dev->private;
-	struct comedi_async *async = s->async;
-	struct comedi_cmd *cmd = &async->cmd;
-	enum scan_mode mode = labpc_ai_scan_mode(cmd);
-	unsigned int chanspec = (mode == MODE_MULT_CHAN_UP)
-				? cmd->chanlist[cmd->chanlist_len - 1]
-				: cmd->chanlist[0];
-	unsigned int chan = CR_CHAN(chanspec);
-	unsigned int range = CR_RANGE(chanspec);
-	unsigned int aref = CR_AREF(chanspec);
-	enum transfer_type xfer;
-	unsigned long flags;
-
-	/* make sure board is disabled before setting up acquisition */
-	labpc_cancel(dev, s);
-
-	/*  initialize software conversion count */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->count = cmd->stop_arg * cmd->chanlist_len;
-
-	/*  setup hardware conversion counter */
-	if (cmd->stop_src == TRIG_EXT) {
-		/*
-		 * load counter a1 with count of 3
-		 * (pc+ manual says this is minimum allowed) using mode 0
-		 */
-		labpc_counter_load(dev, COUNTER_A_BASE_REG,
-				   1, 3, I8254_MODE0);
-	} else	{
-		/* just put counter a1 in mode 0 to set its output low */
-		labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 1, I8254_MODE0);
-	}
-
-	/* figure out what method we will use to transfer data */
-	if (labpc_have_dma_chan(dev) &&
-	    /* dma unsafe at RT priority,
-	     * and too much setup time for TRIG_WAKE_EOS */
-	    (cmd->flags & (TRIG_WAKE_EOS | TRIG_RT)) == 0)
-		xfer = isa_dma_transfer;
-	else if (/* pc-plus has no fifo-half full interrupt */
-		 board->is_labpc1200 &&
-		 /* wake-end-of-scan should interrupt on fifo not empty */
-		 (cmd->flags & TRIG_WAKE_EOS) == 0 &&
-		 /* make sure we are taking more than just a few points */
-		 (cmd->stop_src != TRIG_COUNT || devpriv->count > 256))
-		xfer = fifo_half_full_transfer;
-	else
-		xfer = fifo_not_empty_transfer;
-	devpriv->current_transfer = xfer;
-
-	labpc_ai_set_chan_and_gain(dev, mode, chan, range, aref);
-
-	labpc_setup_cmd6_reg(dev, s, mode, xfer, range, aref,
-			     (cmd->stop_src == TRIG_EXT));
 
-	/* manual says to set scan enable bit on second pass */
-	if (mode == MODE_MULT_CHAN_UP || mode == MODE_MULT_CHAN_DOWN) {
-		devpriv->cmd1 |= CMD1_SCANEN;
-		/* need a brief delay before enabling scan, or scan
-		 * list will get screwed when you switch
-		 * between scan up to scan down mode - dunno why */
-		udelay(1);
-		devpriv->write_byte(dev, devpriv->cmd1, CMD1_REG);
-	}
-
-	devpriv->write_byte(dev, cmd->chanlist_len, INTERVAL_COUNT_REG);
-	/*  load count */
-	devpriv->write_byte(dev, 0x1, INTERVAL_STROBE_REG);
-
-	if (cmd->convert_src == TRIG_TIMER ||
-	    cmd->scan_begin_src == TRIG_TIMER) {
-		/*  set up pacing */
-		labpc_adc_timing(dev, cmd, mode);
-		/*  load counter b0 in mode 3 */
-		labpc_counter_load(dev, COUNTER_B_BASE_REG,
-				   0, devpriv->divisor_b0, I8254_MODE3);
-	}
-	/*  set up conversion pacing */
-	if (labpc_ai_convert_period(cmd, mode)) {
-		/*  load counter a0 in mode 2 */
-		labpc_counter_load(dev, COUNTER_A_BASE_REG,
-				   0, devpriv->divisor_a0, I8254_MODE2);
-	} else {
-		/* initialize pacer counter to prevent any problems */
-		labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 0, I8254_MODE2);
-	}
-
-	/*  set up scan pacing */
-	if (labpc_ai_scan_period(cmd, mode)) {
-		/*  load counter b1 in mode 2 */
-		labpc_counter_load(dev, COUNTER_B_BASE_REG,
-				   1, devpriv->divisor_b1, I8254_MODE2);
-	}
-
-	labpc_clear_adc_fifo(dev);
-
-	if (xfer == isa_dma_transfer)
-		labpc_setup_dma(dev, s);
-
-	/*  enable error interrupts */
-	devpriv->cmd3 |= CMD3_ERRINTEN;
-	/*  enable fifo not empty interrupt? */
-	if (xfer == fifo_not_empty_transfer)
-		devpriv->cmd3 |= CMD3_FIFOINTEN;
-	devpriv->write_byte(dev, devpriv->cmd3, CMD3_REG);
-
-	/*  setup any external triggering/pacing (cmd4 register) */
-	devpriv->cmd4 = 0;
-	if (cmd->convert_src != TRIG_EXT)
-		devpriv->cmd4 |= CMD4_ECLKRCV;
-	/* XXX should discard first scan when using interval scanning
-	 * since manual says it is not synced with scan clock */
-	if (!labpc_use_continuous_mode(cmd, mode)) {
-		devpriv->cmd4 |= CMD4_INTSCAN;
-		if (cmd->scan_begin_src == TRIG_EXT)
-			devpriv->cmd4 |= CMD4_EOIRCV;
-	}
-	/*  single-ended/differential */
-	if (aref == AREF_DIFF)
-		devpriv->cmd4 |= CMD4_SEDIFF;
-	devpriv->write_byte(dev, devpriv->cmd4, CMD4_REG);
-
-	/*  startup acquisition */
-
-	spin_lock_irqsave(&dev->spinlock, flags);
-
-	/* use 2 cascaded counters for pacing */
-	devpriv->cmd2 |= CMD2_TBSEL;
-
-	devpriv->cmd2 &= ~(CMD2_SWTRIG | CMD2_HWTRIG | CMD2_PRETRIG);
-	if (cmd->start_src == TRIG_EXT)
-		devpriv->cmd2 |= CMD2_HWTRIG;
-	else
-		devpriv->cmd2 |= CMD2_SWTRIG;
-	if (cmd->stop_src == TRIG_EXT)
-		devpriv->cmd2 |= (CMD2_HWTRIG | CMD2_PRETRIG);
-
-	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
-
-	spin_unlock_irqrestore(&dev->spinlock, flags);
-
-	return 0;
-}
-
-/* read all available samples from ai fifo */
-static int labpc_drain_fifo(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-	struct comedi_async *async = dev->read_subdev->async;
-	struct comedi_cmd *cmd = &async->cmd;
-	unsigned short data;
-	const int timeout = 10000;
-	unsigned int i;
-
-	devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
-
-	for (i = 0; (devpriv->stat1 & STAT1_DAVAIL) && i < timeout;
-	     i++) {
-		/*  quit if we have all the data we want */
-		if (cmd->stop_src == TRIG_COUNT) {
-			if (devpriv->count == 0)
-				break;
-			devpriv->count--;
-		}
-		data = labpc_read_adc_fifo(dev);
-		cfc_write_to_buffer(dev->read_subdev, data);
-		devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
-	}
-	if (i == timeout) {
-		dev_err(dev->class_dev, "ai timeout, fifo never empties\n");
-		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		return -1;
-	}
-
-	return 0;
-}
-
-/* makes sure all data acquired by board is transferred to comedi (used
- * when acquisition is terminated by stop_src == TRIG_EXT). */
-static void labpc_drain_dregs(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	if (devpriv->current_transfer == isa_dma_transfer)
-		labpc_drain_dma(dev);
-
-	labpc_drain_fifo(dev);
-}
-
-/* interrupt service routine */
-static irqreturn_t labpc_interrupt(int irq, void *d)
-{
-	struct comedi_device *dev = d;
-	const struct labpc_boardinfo *board = comedi_board(dev);
-	struct labpc_private *devpriv = dev->private;
-	struct comedi_subdevice *s = dev->read_subdev;
-	struct comedi_async *async;
-	struct comedi_cmd *cmd;
-
-	if (!dev->attached) {
-		dev_err(dev->class_dev, "premature interrupt\n");
-		return IRQ_HANDLED;
-	}
-
-	async = s->async;
-	cmd = &async->cmd;
-
-	/* read board status */
-	devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
-	if (board->is_labpc1200)
-		devpriv->stat2 = devpriv->read_byte(dev, STAT2_REG);
-
-	if ((devpriv->stat1 & (STAT1_GATA0 | STAT1_CNTINT | STAT1_OVERFLOW |
-			       STAT1_OVERRUN | STAT1_DAVAIL)) == 0
-	    && (devpriv->stat2 & STAT2_OUTA1) == 0
-	    && (devpriv->stat2 & STAT2_FIFONHF)) {
-		return IRQ_NONE;
-	}
-
-	if (devpriv->stat1 & STAT1_OVERRUN) {
-		/* clear error interrupt */
-		devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
-		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
-		dev_err(dev->class_dev, "overrun\n");
-		return IRQ_HANDLED;
-	}
-
-	if (devpriv->current_transfer == isa_dma_transfer)
-		labpc_handle_dma_status(dev);
-	else
-		labpc_drain_fifo(dev);
-
-	if (devpriv->stat1 & STAT1_CNTINT) {
-		dev_err(dev->class_dev, "handled timer interrupt?\n");
-		/*  clear it */
-		devpriv->write_byte(dev, 0x1, TIMER_CLEAR_REG);
-	}
-
-	if (devpriv->stat1 & STAT1_OVERFLOW) {
-		/*  clear error interrupt */
-		devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
-		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
-		cfc_handle_events(dev, s);
-		dev_err(dev->class_dev, "overflow\n");
-		return IRQ_HANDLED;
-	}
-	/*  handle external stop trigger */
-	if (cmd->stop_src == TRIG_EXT) {
-		if (devpriv->stat2 & STAT2_OUTA1) {
-			labpc_drain_dregs(dev);
-			async->events |= COMEDI_CB_EOA;
-		}
-	}
-
-	/* TRIG_COUNT end of acquisition */
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (devpriv->count == 0)
-			async->events |= COMEDI_CB_EOA;
-	}
-
-	cfc_handle_events(dev, s);
-	return IRQ_HANDLED;
-}
-
-static int labpc_ao_insn_write(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	const struct labpc_boardinfo *board = comedi_board(dev);
-	struct labpc_private *devpriv = dev->private;
-	int channel, range;
-	unsigned long flags;
-	int lsb, msb;
-
-	channel = CR_CHAN(insn->chanspec);
-
-	/* turn off pacing of analog output channel */
-	/* note: hardware bug in daqcard-1200 means pacing cannot
-	 * be independently enabled/disabled for its the two channels */
-	spin_lock_irqsave(&dev->spinlock, flags);
-	devpriv->cmd2 &= ~CMD2_LDAC(channel);
-	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
-	spin_unlock_irqrestore(&dev->spinlock, flags);
-
-	/* set range */
-	if (board->is_labpc1200) {
-		range = CR_RANGE(insn->chanspec);
-		if (comedi_range_is_unipolar(s, range))
-			devpriv->cmd6 |= CMD6_DACUNI(channel);
-		else
-			devpriv->cmd6 &= ~CMD6_DACUNI(channel);
-		/*  write to register */
-		devpriv->write_byte(dev, devpriv->cmd6, CMD6_REG);
-	}
-	/* send data */
-	lsb = data[0] & 0xff;
-	msb = (data[0] >> 8) & 0xff;
-	devpriv->write_byte(dev, lsb, DAC_LSB_REG(channel));
-	devpriv->write_byte(dev, msb, DAC_MSB_REG(channel));
-
-	/* remember value for readback */
-	devpriv->ao_value[channel] = data[0];
-
-	return 1;
-}
-
-static int labpc_ao_insn_read(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	data[0] = devpriv->ao_value[CR_CHAN(insn->chanspec)];
-
-	return 1;
-}
-
-static int labpc_8255_mmio(int dir, int port, int data, unsigned long arg)
-{
-	struct comedi_device *dev = (struct comedi_device *)arg;
-
-	if (dir) {
-		writeb(data, dev->mmio + DIO_BASE_REG + port);
-		return 0;
-	}
-
-	return readb(dev->mmio + DIO_BASE_REG + port);
-}
-
-/* lowlevel write to eeprom/dac */
-static void labpc_serial_out(struct comedi_device *dev, unsigned int value,
-			     unsigned int value_width)
-{
-	struct labpc_private *devpriv = dev->private;
-	int i;
-
-	for (i = 1; i <= value_width; i++) {
-		/*  clear serial clock */
-		devpriv->cmd5 &= ~CMD5_SCLK;
-		/*  send bits most significant bit first */
-		if (value & (1 << (value_width - i)))
-			devpriv->cmd5 |= CMD5_SDATA;
-		else
-			devpriv->cmd5 &= ~CMD5_SDATA;
-		udelay(1);
-		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-		/*  set clock to load bit */
-		devpriv->cmd5 |= CMD5_SCLK;
-		udelay(1);
-		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-	}
-}
-
-/* lowlevel read from eeprom */
-static unsigned int labpc_serial_in(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int value = 0;
-	int i;
-	const int value_width = 8;	/*  number of bits wide values are */
-
-	for (i = 1; i <= value_width; i++) {
-		/*  set serial clock */
-		devpriv->cmd5 |= CMD5_SCLK;
-		udelay(1);
-		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-		/*  clear clock bit */
-		devpriv->cmd5 &= ~CMD5_SCLK;
-		udelay(1);
-		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-		/*  read bits most significant bit first */
-		udelay(1);
-		devpriv->stat2 = devpriv->read_byte(dev, STAT2_REG);
-		if (devpriv->stat2 & STAT2_PROMOUT)
-			value |= 1 << (value_width - i);
-	}
-
-	return value;
-}
-
-static unsigned int labpc_eeprom_read(struct comedi_device *dev,
-				      unsigned int address)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int value;
-	/*  bits to tell eeprom to expect a read */
-	const int read_instruction = 0x3;
-	/*  8 bit write lengths to eeprom */
-	const int write_length = 8;
-
-	/*  enable read/write to eeprom */
-	devpriv->cmd5 &= ~CMD5_EEPROMCS;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	/*  send read instruction */
-	labpc_serial_out(dev, read_instruction, write_length);
-	/*  send 8 bit address to read from */
-	labpc_serial_out(dev, address, write_length);
-	/*  read result */
-	value = labpc_serial_in(dev);
-
-	/*  disable read/write to eeprom */
-	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	return value;
-}
-
-static unsigned int labpc_eeprom_read_status(struct comedi_device *dev)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int value;
-	const int read_status_instruction = 0x5;
-	const int write_length = 8;	/*  8 bit write lengths to eeprom */
-
-	/*  enable read/write to eeprom */
-	devpriv->cmd5 &= ~CMD5_EEPROMCS;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	/*  send read status instruction */
-	labpc_serial_out(dev, read_status_instruction, write_length);
-	/*  read result */
-	value = labpc_serial_in(dev);
-
-	/*  disable read/write to eeprom */
-	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	return value;
-}
-
-static int labpc_eeprom_write(struct comedi_device *dev,
-				unsigned int address, unsigned int value)
-{
-	struct labpc_private *devpriv = dev->private;
-	const int write_enable_instruction = 0x6;
-	const int write_instruction = 0x2;
-	const int write_length = 8;	/*  8 bit write lengths to eeprom */
-	const int write_in_progress_bit = 0x1;
-	const int timeout = 10000;
-	int i;
-
-	/*  make sure there isn't already a write in progress */
-	for (i = 0; i < timeout; i++) {
-		if ((labpc_eeprom_read_status(dev) & write_in_progress_bit) ==
-		    0)
-			break;
-	}
-	if (i == timeout) {
-		dev_err(dev->class_dev, "eeprom write timed out\n");
-		return -ETIME;
-	}
-	/*  update software copy of eeprom */
-	devpriv->eeprom_data[address] = value;
-
-	/*  enable read/write to eeprom */
-	devpriv->cmd5 &= ~CMD5_EEPROMCS;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	/*  send write_enable instruction */
-	labpc_serial_out(dev, write_enable_instruction, write_length);
-	devpriv->cmd5 &= ~CMD5_EEPROMCS;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	/*  send write instruction */
-	devpriv->cmd5 |= CMD5_EEPROMCS;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-	labpc_serial_out(dev, write_instruction, write_length);
-	/*  send 8 bit address to write to */
-	labpc_serial_out(dev, address, write_length);
-	/*  write value */
-	labpc_serial_out(dev, value, write_length);
-	devpriv->cmd5 &= ~CMD5_EEPROMCS;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	/*  disable read/write to eeprom */
-	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	return 0;
-}
-
-/* writes to 8 bit calibration dacs */
-static void write_caldac(struct comedi_device *dev, unsigned int channel,
-			 unsigned int value)
-{
-	struct labpc_private *devpriv = dev->private;
-
-	if (value == devpriv->caldac[channel])
-		return;
-	devpriv->caldac[channel] = value;
-
-	/*  clear caldac load bit and make sure we don't write to eeprom */
-	devpriv->cmd5 &= ~(CMD5_CALDACLD | CMD5_EEPROMCS | CMD5_WRTPRT);
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-
-	/*  write 4 bit channel */
-	labpc_serial_out(dev, channel, 4);
-	/*  write 8 bit caldac value */
-	labpc_serial_out(dev, value, 8);
-
-	/*  set and clear caldac bit to load caldac value */
-	devpriv->cmd5 |= CMD5_CALDACLD;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-	devpriv->cmd5 &= ~CMD5_CALDACLD;
-	udelay(1);
-	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-}
-
-static int labpc_calib_insn_write(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn,
-				  unsigned int *data)
-{
-	unsigned int chan = CR_CHAN(insn->chanspec);
-
-	/*
-	 * Only write the last data value to the caldac. Preceding
-	 * data would be overwritten anyway.
-	 */
-	if (insn->n > 0)
-		write_caldac(dev, chan, data[insn->n - 1]);
-
-	return insn->n;
-}
-
-static int labpc_calib_insn_read(struct comedi_device *dev,
-				 struct comedi_subdevice *s,
-				 struct comedi_insn *insn,
-				 unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->caldac[chan];
-
-	return insn->n;
-}
-
-static int labpc_eeprom_insn_write(struct comedi_device *dev,
-				   struct comedi_subdevice *s,
-				   struct comedi_insn *insn,
-				   unsigned int *data)
-{
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int ret;
-
-	/* only allow writes to user area of eeprom */
-	if (chan < 16 || chan > 127)
-		return -EINVAL;
-
-	/*
-	 * Only write the last data value to the eeprom. Preceding
-	 * data would be overwritten anyway.
-	 */
-	if (insn->n > 0) {
-		ret = labpc_eeprom_write(dev, chan, data[insn->n - 1]);
-		if (ret)
-			return ret;
-	}
-
-	return insn->n;
-}
-
-static int labpc_eeprom_insn_read(struct comedi_device *dev,
-				  struct comedi_subdevice *s,
-				  struct comedi_insn *insn,
-				  unsigned int *data)
-{
-	struct labpc_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->eeprom_data[chan];
-
-	return insn->n;
-}
-
-int labpc_common_attach(struct comedi_device *dev,
-			unsigned int irq, unsigned long isr_flags)
-{
-	const struct labpc_boardinfo *board = comedi_board(dev);
-	struct labpc_private *devpriv = dev->private;
-	struct comedi_subdevice *s;
-	int ret;
-	int i;
-
-	if (dev->mmio) {
-		devpriv->read_byte = labpc_readb;
-		devpriv->write_byte = labpc_writeb;
-	} else {
-		devpriv->read_byte = labpc_inb;
-		devpriv->write_byte = labpc_outb;
-	}
-
-	/* initialize board's command registers */
-	devpriv->write_byte(dev, devpriv->cmd1, CMD1_REG);
-	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
-	devpriv->write_byte(dev, devpriv->cmd3, CMD3_REG);
-	devpriv->write_byte(dev, devpriv->cmd4, CMD4_REG);
-	if (board->is_labpc1200) {
-		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
-		devpriv->write_byte(dev, devpriv->cmd6, CMD6_REG);
-	}
-
-	if (irq) {
-		ret = request_irq(irq, labpc_interrupt, isr_flags,
-				  dev->board_name, dev);
-		if (ret == 0)
-			dev->irq = irq;
-	}
-
-	ret = comedi_alloc_subdevices(dev, 5);
-	if (ret)
-		return ret;
-
-	/* analog input subdevice */
-	s = &dev->subdevices[0];
-	s->type		= COMEDI_SUBD_AI;
-	s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF;
-	s->n_chan	= 8;
-	s->len_chanlist	= 8;
-	s->maxdata	= 0x0fff;
-	s->range_table	= board->is_labpc1200
-				? &range_labpc_1200_ai : &range_labpc_plus_ai;
-	s->insn_read	= labpc_ai_insn_read;
-	if (dev->irq) {
-		dev->read_subdev = s;
-		s->subdev_flags	|= SDF_CMD_READ;
-		s->do_cmd	= labpc_ai_cmd;
-		s->do_cmdtest	= labpc_ai_cmdtest;
-		s->cancel	= labpc_cancel;
-	}
-
-	/* analog output */
-	s = &dev->subdevices[1];
-	if (board->has_ao) {
-		s->type		= COMEDI_SUBD_AO;
-		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
-		s->n_chan	= NUM_AO_CHAN;
-		s->maxdata	= 0x0fff;
-		s->range_table	= &range_labpc_ao;
-		s->insn_read	= labpc_ao_insn_read;
-		s->insn_write	= labpc_ao_insn_write;
-
-		/* initialize analog outputs to a known value */
-		for (i = 0; i < s->n_chan; i++) {
-			short lsb, msb;
-
-			devpriv->ao_value[i] = s->maxdata / 2;
-			lsb = devpriv->ao_value[i] & 0xff;
-			msb = (devpriv->ao_value[i] >> 8) & 0xff;
-			devpriv->write_byte(dev, lsb, DAC_LSB_REG(i));
-			devpriv->write_byte(dev, msb, DAC_MSB_REG(i));
-		}
-	} else {
-		s->type		= COMEDI_SUBD_UNUSED;
-	}
-
-	/* 8255 dio */
-	s = &dev->subdevices[2];
-	if (dev->mmio) {
-		ret = subdev_8255_init(dev, s, labpc_8255_mmio,
-				       (unsigned long)dev);
-	} else {
-		ret = subdev_8255_init(dev, s, NULL,
-				       dev->iobase + DIO_BASE_REG);
-	}
-	if (ret)
-		return ret;
-
-	/*  calibration subdevices for boards that have one */
-	s = &dev->subdevices[3];
-	if (board->is_labpc1200) {
-		s->type		= COMEDI_SUBD_CALIB;
-		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-		s->n_chan	= 16;
-		s->maxdata	= 0xff;
-		s->insn_read	= labpc_calib_insn_read;
-		s->insn_write	= labpc_calib_insn_write;
-
-		for (i = 0; i < s->n_chan; i++)
-			write_caldac(dev, i, s->maxdata / 2);
-	} else
-		s->type		= COMEDI_SUBD_UNUSED;
-
-	/* EEPROM */
-	s = &dev->subdevices[4];
-	if (board->is_labpc1200) {
-		s->type		= COMEDI_SUBD_MEMORY;
-		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
-		s->n_chan	= EEPROM_SIZE;
-		s->maxdata	= 0xff;
-		s->insn_read	= labpc_eeprom_insn_read;
-		s->insn_write	= labpc_eeprom_insn_write;
-
-		for (i = 0; i < s->n_chan; i++)
-			devpriv->eeprom_data[i] = labpc_eeprom_read(dev, i);
-	} else
-		s->type		= COMEDI_SUBD_UNUSED;
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(labpc_common_attach);
-
-#if IS_ENABLED(CONFIG_COMEDI_NI_LABPC_ISA)
 static int labpc_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
 	struct labpc_private *devpriv;
@@ -1492,19 +128,7 @@ static struct comedi_driver labpc_driver = {
 	.offset		= sizeof(struct labpc_boardinfo),
 };
 module_comedi_driver(labpc_driver);
-#else
-static int __init labpc_common_init(void)
-{
-	return 0;
-}
-module_init(labpc_common_init);
-
-static void __exit labpc_common_exit(void)
-{
-}
-module_exit(labpc_common_exit);
-#endif
 
 MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
+MODULE_DESCRIPTION("Comedi driver for NI Lab-PC ISA boards");
 MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_labpc_common.c b/drivers/staging/comedi/drivers/ni_labpc_common.c
new file mode 100644
index 000000000000..35bc2c25ddfb
--- /dev/null
+++ b/drivers/staging/comedi/drivers/ni_labpc_common.c
@@ -0,0 +1,1387 @@
+/*
+ * comedi/drivers/ni_labpc_common.c
+ *
+ * Common support code for "ni_labpc", "ni_labpc_pci" and "ni_labpc_cs".
+ *
+ * Copyright (C) 2001-2003 Frank Mori Hess <fmhess@users.sourceforge.net>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/delay.h>
+
+#include "../comedidev.h"
+
+#include "8253.h"
+#include "8255.h"
+#include "comedi_fc.h"
+#include "ni_labpc.h"
+#include "ni_labpc_regs.h"
+#include "ni_labpc_isadma.h"
+
+enum scan_mode {
+	MODE_SINGLE_CHAN,
+	MODE_SINGLE_CHAN_INTERVAL,
+	MODE_MULT_CHAN_UP,
+	MODE_MULT_CHAN_DOWN,
+};
+
+static const struct comedi_lrange range_labpc_plus_ai = {
+	16, {
+		BIP_RANGE(5),
+		BIP_RANGE(4),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1),
+		BIP_RANGE(0.5),
+		BIP_RANGE(0.25),
+		BIP_RANGE(0.1),
+		BIP_RANGE(0.05),
+		UNI_RANGE(10),
+		UNI_RANGE(8),
+		UNI_RANGE(5),
+		UNI_RANGE(2),
+		UNI_RANGE(1),
+		UNI_RANGE(0.5),
+		UNI_RANGE(0.2),
+		UNI_RANGE(0.1)
+	}
+};
+
+static const struct comedi_lrange range_labpc_1200_ai = {
+	14, {
+		BIP_RANGE(5),
+		BIP_RANGE(2.5),
+		BIP_RANGE(1),
+		BIP_RANGE(0.5),
+		BIP_RANGE(0.25),
+		BIP_RANGE(0.1),
+		BIP_RANGE(0.05),
+		UNI_RANGE(10),
+		UNI_RANGE(5),
+		UNI_RANGE(2),
+		UNI_RANGE(1),
+		UNI_RANGE(0.5),
+		UNI_RANGE(0.2),
+		UNI_RANGE(0.1)
+	}
+};
+
+static const struct comedi_lrange range_labpc_ao = {
+	2, {
+		BIP_RANGE(5),
+		UNI_RANGE(10)
+	}
+};
+
+/* functions that do inb/outb and readb/writeb so we can use
+ * function pointers to decide which to use */
+static unsigned int labpc_inb(struct comedi_device *dev, unsigned long reg)
+{
+	return inb(dev->iobase + reg);
+}
+
+static void labpc_outb(struct comedi_device *dev,
+		       unsigned int byte, unsigned long reg)
+{
+	outb(byte, dev->iobase + reg);
+}
+
+static unsigned int labpc_readb(struct comedi_device *dev, unsigned long reg)
+{
+	return readb(dev->mmio + reg);
+}
+
+static void labpc_writeb(struct comedi_device *dev,
+			 unsigned int byte, unsigned long reg)
+{
+	writeb(byte, dev->mmio + reg);
+}
+
+static void labpc_counter_load(struct comedi_device *dev,
+			       unsigned long reg,
+			       unsigned int counter_number,
+			       unsigned int count,
+			       unsigned int mode)
+{
+	if (dev->mmio) {
+		i8254_mm_set_mode(dev->mmio + reg, 0, counter_number, mode);
+		i8254_mm_write(dev->mmio + reg, 0, counter_number, count);
+	} else {
+		i8254_set_mode(dev->iobase + reg, 0, counter_number, mode);
+		i8254_write(dev->iobase + reg, 0, counter_number, count);
+	}
+}
+
+static void labpc_counter_set_mode(struct comedi_device *dev,
+				   unsigned long reg,
+				   unsigned int counter_number,
+				   unsigned int mode)
+{
+	if (dev->mmio)
+		i8254_mm_set_mode(dev->mmio + reg, 0, counter_number, mode);
+	else
+		i8254_set_mode(dev->iobase + reg, 0, counter_number, mode);
+}
+
+static int labpc_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->spinlock, flags);
+	devpriv->cmd2 &= ~(CMD2_SWTRIG | CMD2_HWTRIG | CMD2_PRETRIG);
+	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	devpriv->cmd3 = 0;
+	devpriv->write_byte(dev, devpriv->cmd3, CMD3_REG);
+
+	return 0;
+}
+
+static void labpc_ai_set_chan_and_gain(struct comedi_device *dev,
+				       enum scan_mode mode,
+				       unsigned int chan,
+				       unsigned int range,
+				       unsigned int aref)
+{
+	const struct labpc_boardinfo *board = dev->board_ptr;
+	struct labpc_private *devpriv = dev->private;
+
+	if (board->is_labpc1200) {
+		/*
+		 * The LabPC-1200 boards do not have a gain
+		 * of '0x10'. Skip the range values that would
+		 * result in this gain.
+		 */
+		range += (range > 0) + (range > 7);
+	}
+
+	/* munge channel bits for differential/scan disabled mode */
+	if ((mode == MODE_SINGLE_CHAN || mode == MODE_SINGLE_CHAN_INTERVAL) &&
+	    aref == AREF_DIFF)
+		chan *= 2;
+	devpriv->cmd1 = CMD1_MA(chan);
+	devpriv->cmd1 |= CMD1_GAIN(range);
+
+	devpriv->write_byte(dev, devpriv->cmd1, CMD1_REG);
+}
+
+static void labpc_setup_cmd6_reg(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 enum scan_mode mode,
+				 enum transfer_type xfer,
+				 unsigned int range,
+				 unsigned int aref,
+				 bool ena_intr)
+{
+	const struct labpc_boardinfo *board = dev->board_ptr;
+	struct labpc_private *devpriv = dev->private;
+
+	if (!board->is_labpc1200)
+		return;
+
+	/* reference inputs to ground or common? */
+	if (aref != AREF_GROUND)
+		devpriv->cmd6 |= CMD6_NRSE;
+	else
+		devpriv->cmd6 &= ~CMD6_NRSE;
+
+	/* bipolar or unipolar range? */
+	if (comedi_range_is_unipolar(s, range))
+		devpriv->cmd6 |= CMD6_ADCUNI;
+	else
+		devpriv->cmd6 &= ~CMD6_ADCUNI;
+
+	/*  interrupt on fifo half full? */
+	if (xfer == fifo_half_full_transfer)
+		devpriv->cmd6 |= CMD6_HFINTEN;
+	else
+		devpriv->cmd6 &= ~CMD6_HFINTEN;
+
+	/* enable interrupt on counter a1 terminal count? */
+	if (ena_intr)
+		devpriv->cmd6 |= CMD6_DQINTEN;
+	else
+		devpriv->cmd6 &= ~CMD6_DQINTEN;
+
+	/* are we scanning up or down through channels? */
+	if (mode == MODE_MULT_CHAN_UP)
+		devpriv->cmd6 |= CMD6_SCANUP;
+	else
+		devpriv->cmd6 &= ~CMD6_SCANUP;
+
+	devpriv->write_byte(dev, devpriv->cmd6, CMD6_REG);
+}
+
+static unsigned int labpc_read_adc_fifo(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int lsb = devpriv->read_byte(dev, ADC_FIFO_REG);
+	unsigned int msb = devpriv->read_byte(dev, ADC_FIFO_REG);
+
+	return (msb << 8) | lsb;
+}
+
+static void labpc_clear_adc_fifo(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
+	labpc_read_adc_fifo(dev);
+}
+
+static int labpc_ai_eoc(struct comedi_device *dev,
+			struct comedi_subdevice *s,
+			struct comedi_insn *insn,
+			unsigned long context)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
+	if (devpriv->stat1 & STAT1_DAVAIL)
+		return 0;
+	return -EBUSY;
+}
+
+static int labpc_ai_insn_read(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int range = CR_RANGE(insn->chanspec);
+	unsigned int aref = CR_AREF(insn->chanspec);
+	int ret;
+	int i;
+
+	/* disable timed conversions, interrupt generation and dma */
+	labpc_cancel(dev, s);
+
+	labpc_ai_set_chan_and_gain(dev, MODE_SINGLE_CHAN, chan, range, aref);
+
+	labpc_setup_cmd6_reg(dev, s, MODE_SINGLE_CHAN, fifo_not_empty_transfer,
+			     range, aref, false);
+
+	/* setup cmd4 register */
+	devpriv->cmd4 = 0;
+	devpriv->cmd4 |= CMD4_ECLKRCV;
+	/* single-ended/differential */
+	if (aref == AREF_DIFF)
+		devpriv->cmd4 |= CMD4_SEDIFF;
+	devpriv->write_byte(dev, devpriv->cmd4, CMD4_REG);
+
+	/* initialize pacer counter to prevent any problems */
+	labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 0, I8254_MODE2);
+
+	labpc_clear_adc_fifo(dev);
+
+	for (i = 0; i < insn->n; i++) {
+		/* trigger conversion */
+		devpriv->write_byte(dev, 0x1, ADC_START_CONVERT_REG);
+
+		ret = comedi_timeout(dev, s, insn, labpc_ai_eoc, 0);
+		if (ret)
+			return ret;
+
+		data[i] = labpc_read_adc_fifo(dev);
+	}
+
+	return insn->n;
+}
+
+static bool labpc_use_continuous_mode(const struct comedi_cmd *cmd,
+				      enum scan_mode mode)
+{
+	if (mode == MODE_SINGLE_CHAN || cmd->scan_begin_src == TRIG_FOLLOW)
+		return true;
+
+	return false;
+}
+
+static unsigned int labpc_ai_convert_period(const struct comedi_cmd *cmd,
+					    enum scan_mode mode)
+{
+	if (cmd->convert_src != TRIG_TIMER)
+		return 0;
+
+	if (mode == MODE_SINGLE_CHAN && cmd->scan_begin_src == TRIG_TIMER)
+		return cmd->scan_begin_arg;
+
+	return cmd->convert_arg;
+}
+
+static void labpc_set_ai_convert_period(struct comedi_cmd *cmd,
+					enum scan_mode mode, unsigned int ns)
+{
+	if (cmd->convert_src != TRIG_TIMER)
+		return;
+
+	if (mode == MODE_SINGLE_CHAN &&
+	    cmd->scan_begin_src == TRIG_TIMER) {
+		cmd->scan_begin_arg = ns;
+		if (cmd->convert_arg > cmd->scan_begin_arg)
+			cmd->convert_arg = cmd->scan_begin_arg;
+	} else {
+		cmd->convert_arg = ns;
+	}
+}
+
+static unsigned int labpc_ai_scan_period(const struct comedi_cmd *cmd,
+					 enum scan_mode mode)
+{
+	if (cmd->scan_begin_src != TRIG_TIMER)
+		return 0;
+
+	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
+		return 0;
+
+	return cmd->scan_begin_arg;
+}
+
+static void labpc_set_ai_scan_period(struct comedi_cmd *cmd,
+				     enum scan_mode mode, unsigned int ns)
+{
+	if (cmd->scan_begin_src != TRIG_TIMER)
+		return;
+
+	if (mode == MODE_SINGLE_CHAN && cmd->convert_src == TRIG_TIMER)
+		return;
+
+	cmd->scan_begin_arg = ns;
+}
+
+/* figures out what counter values to use based on command */
+static void labpc_adc_timing(struct comedi_device *dev, struct comedi_cmd *cmd,
+			     enum scan_mode mode)
+{
+	struct labpc_private *devpriv = dev->private;
+	/* max value for 16 bit counter in mode 2 */
+	const int max_counter_value = 0x10000;
+	/* min value for 16 bit counter in mode 2 */
+	const int min_counter_value = 2;
+	unsigned int base_period;
+	unsigned int scan_period;
+	unsigned int convert_period;
+
+	/*
+	 * if both convert and scan triggers are TRIG_TIMER, then they
+	 * both rely on counter b0
+	 */
+	convert_period = labpc_ai_convert_period(cmd, mode);
+	scan_period = labpc_ai_scan_period(cmd, mode);
+	if (convert_period && scan_period) {
+		/*
+		 * pick the lowest b0 divisor value we can (for maximum input
+		 * clock speed on convert and scan counters)
+		 */
+		devpriv->divisor_b0 = (scan_period - 1) /
+		    (I8254_OSC_BASE_2MHZ * max_counter_value) + 1;
+		if (devpriv->divisor_b0 < min_counter_value)
+			devpriv->divisor_b0 = min_counter_value;
+		if (devpriv->divisor_b0 > max_counter_value)
+			devpriv->divisor_b0 = max_counter_value;
+
+		base_period = I8254_OSC_BASE_2MHZ * devpriv->divisor_b0;
+
+		/*  set a0 for conversion frequency and b1 for scan frequency */
+		switch (cmd->flags & CMDF_ROUND_MASK) {
+		default:
+		case CMDF_ROUND_NEAREST:
+			devpriv->divisor_a0 =
+			    (convert_period + (base_period / 2)) / base_period;
+			devpriv->divisor_b1 =
+			    (scan_period + (base_period / 2)) / base_period;
+			break;
+		case CMDF_ROUND_UP:
+			devpriv->divisor_a0 =
+			    (convert_period + (base_period - 1)) / base_period;
+			devpriv->divisor_b1 =
+			    (scan_period + (base_period - 1)) / base_period;
+			break;
+		case CMDF_ROUND_DOWN:
+			devpriv->divisor_a0 = convert_period / base_period;
+			devpriv->divisor_b1 = scan_period / base_period;
+			break;
+		}
+		/*  make sure a0 and b1 values are acceptable */
+		if (devpriv->divisor_a0 < min_counter_value)
+			devpriv->divisor_a0 = min_counter_value;
+		if (devpriv->divisor_a0 > max_counter_value)
+			devpriv->divisor_a0 = max_counter_value;
+		if (devpriv->divisor_b1 < min_counter_value)
+			devpriv->divisor_b1 = min_counter_value;
+		if (devpriv->divisor_b1 > max_counter_value)
+			devpriv->divisor_b1 = max_counter_value;
+		/*  write corrected timings to command */
+		labpc_set_ai_convert_period(cmd, mode,
+					    base_period * devpriv->divisor_a0);
+		labpc_set_ai_scan_period(cmd, mode,
+					 base_period * devpriv->divisor_b1);
+		/*
+		 * if only one TRIG_TIMER is used, we can employ the generic
+		 * cascaded timing functions
+		 */
+	} else if (scan_period) {
+		/*
+		 * calculate cascaded counter values
+		 * that give desired scan timing
+		 */
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
+					  &devpriv->divisor_b1,
+					  &devpriv->divisor_b0,
+					  &scan_period, cmd->flags);
+		labpc_set_ai_scan_period(cmd, mode, scan_period);
+	} else if (convert_period) {
+		/*
+		 * calculate cascaded counter values
+		 * that give desired conversion timing
+		 */
+		i8253_cascade_ns_to_timer(I8254_OSC_BASE_2MHZ,
+					  &devpriv->divisor_a0,
+					  &devpriv->divisor_b0,
+					  &convert_period, cmd->flags);
+		labpc_set_ai_convert_period(cmd, mode, convert_period);
+	}
+}
+
+static enum scan_mode labpc_ai_scan_mode(const struct comedi_cmd *cmd)
+{
+	unsigned int chan0;
+	unsigned int chan1;
+
+	if (cmd->chanlist_len == 1)
+		return MODE_SINGLE_CHAN;
+
+	/* chanlist may be NULL during cmdtest */
+	if (cmd->chanlist == NULL)
+		return MODE_MULT_CHAN_UP;
+
+	chan0 = CR_CHAN(cmd->chanlist[0]);
+	chan1 = CR_CHAN(cmd->chanlist[1]);
+
+	if (chan0 < chan1)
+		return MODE_MULT_CHAN_UP;
+
+	if (chan0 > chan1)
+		return MODE_MULT_CHAN_DOWN;
+
+	return MODE_SINGLE_CHAN_INTERVAL;
+}
+
+static int labpc_ai_check_chanlist(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_cmd *cmd)
+{
+	enum scan_mode mode = labpc_ai_scan_mode(cmd);
+	unsigned int chan0 = CR_CHAN(cmd->chanlist[0]);
+	unsigned int range0 = CR_RANGE(cmd->chanlist[0]);
+	unsigned int aref0 = CR_AREF(cmd->chanlist[0]);
+	int i;
+
+	if (mode == MODE_SINGLE_CHAN)
+		return 0;
+
+	for (i = 0; i < cmd->chanlist_len; i++) {
+		unsigned int chan = CR_CHAN(cmd->chanlist[i]);
+		unsigned int range = CR_RANGE(cmd->chanlist[i]);
+		unsigned int aref = CR_AREF(cmd->chanlist[i]);
+
+		switch (mode) {
+		case MODE_SINGLE_CHAN:
+			break;
+		case MODE_SINGLE_CHAN_INTERVAL:
+			if (chan != chan0) {
+				dev_dbg(dev->class_dev,
+					"channel scanning order specified in chanlist is not supported by hardware\n");
+				return -EINVAL;
+			}
+			break;
+		case MODE_MULT_CHAN_UP:
+			if (chan != i) {
+				dev_dbg(dev->class_dev,
+					"channel scanning order specified in chanlist is not supported by hardware\n");
+				return -EINVAL;
+			}
+			break;
+		case MODE_MULT_CHAN_DOWN:
+			if (chan != (cmd->chanlist_len - i - 1)) {
+				dev_dbg(dev->class_dev,
+					"channel scanning order specified in chanlist is not supported by hardware\n");
+				return -EINVAL;
+			}
+			break;
+		}
+
+		if (range != range0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same range\n");
+			return -EINVAL;
+		}
+
+		if (aref != aref0) {
+			dev_dbg(dev->class_dev,
+				"entries in chanlist must all have the same reference\n");
+			return -EINVAL;
+		}
+	}
+
+	return 0;
+}
+
+static int labpc_ai_cmdtest(struct comedi_device *dev,
+			    struct comedi_subdevice *s, struct comedi_cmd *cmd)
+{
+	const struct labpc_boardinfo *board = dev->board_ptr;
+	int err = 0;
+	int tmp, tmp2;
+	unsigned int stop_mask;
+	enum scan_mode mode;
+
+	/* Step 1 : check if triggers are trivially valid */
+
+	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
+					TRIG_TIMER | TRIG_FOLLOW | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
+	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
+
+	stop_mask = TRIG_COUNT | TRIG_NONE;
+	if (board->is_labpc1200)
+		stop_mask |= TRIG_EXT;
+	err |= cfc_check_trigger_src(&cmd->stop_src, stop_mask);
+
+	if (err)
+		return 1;
+
+	/* Step 2a : make sure trigger sources are unique */
+
+	err |= cfc_check_trigger_is_unique(cmd->start_src);
+	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
+	err |= cfc_check_trigger_is_unique(cmd->convert_src);
+	err |= cfc_check_trigger_is_unique(cmd->stop_src);
+
+	/* Step 2b : and mutually compatible */
+
+	/* can't have external stop and start triggers at once */
+	if (cmd->start_src == TRIG_EXT && cmd->stop_src == TRIG_EXT)
+		err++;
+
+	if (err)
+		return 2;
+
+	/* Step 3: check if arguments are trivially valid */
+
+	switch (cmd->start_src) {
+	case TRIG_NOW:
+		err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
+		break;
+	case TRIG_EXT:
+		/* start_arg value is ignored */
+		break;
+	}
+
+	if (!cmd->chanlist_len)
+		err |= -EINVAL;
+	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
+
+	if (cmd->convert_src == TRIG_TIMER)
+		err |= cfc_check_trigger_arg_min(&cmd->convert_arg,
+						 board->ai_speed);
+
+	/* make sure scan timing is not too fast */
+	if (cmd->scan_begin_src == TRIG_TIMER) {
+		if (cmd->convert_src == TRIG_TIMER)
+			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+					cmd->convert_arg * cmd->chanlist_len);
+		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
+				board->ai_speed * cmd->chanlist_len);
+	}
+
+	switch (cmd->stop_src) {
+	case TRIG_COUNT:
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+		break;
+	case TRIG_NONE:
+		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
+		break;
+		/*
+		 * TRIG_EXT doesn't care since it doesn't
+		 * trigger off a numbered channel
+		 */
+	default:
+		break;
+	}
+
+	if (err)
+		return 3;
+
+	/* step 4: fix up any arguments */
+
+	tmp = cmd->convert_arg;
+	tmp2 = cmd->scan_begin_arg;
+	mode = labpc_ai_scan_mode(cmd);
+	labpc_adc_timing(dev, cmd, mode);
+	if (tmp != cmd->convert_arg || tmp2 != cmd->scan_begin_arg)
+		err++;
+
+	if (err)
+		return 4;
+
+	/* Step 5: check channel list if it exists */
+	if (cmd->chanlist && cmd->chanlist_len > 0)
+		err |= labpc_ai_check_chanlist(dev, s, cmd);
+
+	if (err)
+		return 5;
+
+	return 0;
+}
+
+static int labpc_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
+{
+	const struct labpc_boardinfo *board = dev->board_ptr;
+	struct labpc_private *devpriv = dev->private;
+	struct comedi_async *async = s->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	enum scan_mode mode = labpc_ai_scan_mode(cmd);
+	unsigned int chanspec = (mode == MODE_MULT_CHAN_UP) ?
+				cmd->chanlist[cmd->chanlist_len - 1] :
+				cmd->chanlist[0];
+	unsigned int chan = CR_CHAN(chanspec);
+	unsigned int range = CR_RANGE(chanspec);
+	unsigned int aref = CR_AREF(chanspec);
+	enum transfer_type xfer;
+	unsigned long flags;
+
+	/* make sure board is disabled before setting up acquisition */
+	labpc_cancel(dev, s);
+
+	/*  initialize software conversion count */
+	if (cmd->stop_src == TRIG_COUNT)
+		devpriv->count = cmd->stop_arg * cmd->chanlist_len;
+
+	/*  setup hardware conversion counter */
+	if (cmd->stop_src == TRIG_EXT) {
+		/*
+		 * load counter a1 with count of 3
+		 * (pc+ manual says this is minimum allowed) using mode 0
+		 */
+		labpc_counter_load(dev, COUNTER_A_BASE_REG,
+				   1, 3, I8254_MODE0);
+	} else	{
+		/* just put counter a1 in mode 0 to set its output low */
+		labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 1, I8254_MODE0);
+	}
+
+	/* figure out what method we will use to transfer data */
+	if (labpc_have_dma_chan(dev) &&
+	    /* dma unsafe at RT priority,
+	     * and too much setup time for CMDF_WAKE_EOS */
+	    (cmd->flags & (CMDF_WAKE_EOS | CMDF_PRIORITY)) == 0)
+		xfer = isa_dma_transfer;
+	else if (/* pc-plus has no fifo-half full interrupt */
+		 board->is_labpc1200 &&
+		 /* wake-end-of-scan should interrupt on fifo not empty */
+		 (cmd->flags & CMDF_WAKE_EOS) == 0 &&
+		 /* make sure we are taking more than just a few points */
+		 (cmd->stop_src != TRIG_COUNT || devpriv->count > 256))
+		xfer = fifo_half_full_transfer;
+	else
+		xfer = fifo_not_empty_transfer;
+	devpriv->current_transfer = xfer;
+
+	labpc_ai_set_chan_and_gain(dev, mode, chan, range, aref);
+
+	labpc_setup_cmd6_reg(dev, s, mode, xfer, range, aref,
+			     (cmd->stop_src == TRIG_EXT));
+
+	/* manual says to set scan enable bit on second pass */
+	if (mode == MODE_MULT_CHAN_UP || mode == MODE_MULT_CHAN_DOWN) {
+		devpriv->cmd1 |= CMD1_SCANEN;
+		/* need a brief delay before enabling scan, or scan
+		 * list will get screwed when you switch
+		 * between scan up to scan down mode - dunno why */
+		udelay(1);
+		devpriv->write_byte(dev, devpriv->cmd1, CMD1_REG);
+	}
+
+	devpriv->write_byte(dev, cmd->chanlist_len, INTERVAL_COUNT_REG);
+	/*  load count */
+	devpriv->write_byte(dev, 0x1, INTERVAL_STROBE_REG);
+
+	if (cmd->convert_src == TRIG_TIMER ||
+	    cmd->scan_begin_src == TRIG_TIMER) {
+		/*  set up pacing */
+		labpc_adc_timing(dev, cmd, mode);
+		/*  load counter b0 in mode 3 */
+		labpc_counter_load(dev, COUNTER_B_BASE_REG,
+				   0, devpriv->divisor_b0, I8254_MODE3);
+	}
+	/*  set up conversion pacing */
+	if (labpc_ai_convert_period(cmd, mode)) {
+		/*  load counter a0 in mode 2 */
+		labpc_counter_load(dev, COUNTER_A_BASE_REG,
+				   0, devpriv->divisor_a0, I8254_MODE2);
+	} else {
+		/* initialize pacer counter to prevent any problems */
+		labpc_counter_set_mode(dev, COUNTER_A_BASE_REG, 0, I8254_MODE2);
+	}
+
+	/*  set up scan pacing */
+	if (labpc_ai_scan_period(cmd, mode)) {
+		/*  load counter b1 in mode 2 */
+		labpc_counter_load(dev, COUNTER_B_BASE_REG,
+				   1, devpriv->divisor_b1, I8254_MODE2);
+	}
+
+	labpc_clear_adc_fifo(dev);
+
+	if (xfer == isa_dma_transfer)
+		labpc_setup_dma(dev, s);
+
+	/*  enable error interrupts */
+	devpriv->cmd3 |= CMD3_ERRINTEN;
+	/*  enable fifo not empty interrupt? */
+	if (xfer == fifo_not_empty_transfer)
+		devpriv->cmd3 |= CMD3_FIFOINTEN;
+	devpriv->write_byte(dev, devpriv->cmd3, CMD3_REG);
+
+	/*  setup any external triggering/pacing (cmd4 register) */
+	devpriv->cmd4 = 0;
+	if (cmd->convert_src != TRIG_EXT)
+		devpriv->cmd4 |= CMD4_ECLKRCV;
+	/* XXX should discard first scan when using interval scanning
+	 * since manual says it is not synced with scan clock */
+	if (!labpc_use_continuous_mode(cmd, mode)) {
+		devpriv->cmd4 |= CMD4_INTSCAN;
+		if (cmd->scan_begin_src == TRIG_EXT)
+			devpriv->cmd4 |= CMD4_EOIRCV;
+	}
+	/*  single-ended/differential */
+	if (aref == AREF_DIFF)
+		devpriv->cmd4 |= CMD4_SEDIFF;
+	devpriv->write_byte(dev, devpriv->cmd4, CMD4_REG);
+
+	/*  startup acquisition */
+
+	spin_lock_irqsave(&dev->spinlock, flags);
+
+	/* use 2 cascaded counters for pacing */
+	devpriv->cmd2 |= CMD2_TBSEL;
+
+	devpriv->cmd2 &= ~(CMD2_SWTRIG | CMD2_HWTRIG | CMD2_PRETRIG);
+	if (cmd->start_src == TRIG_EXT)
+		devpriv->cmd2 |= CMD2_HWTRIG;
+	else
+		devpriv->cmd2 |= CMD2_SWTRIG;
+	if (cmd->stop_src == TRIG_EXT)
+		devpriv->cmd2 |= (CMD2_HWTRIG | CMD2_PRETRIG);
+
+	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
+
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	return 0;
+}
+
+/* read all available samples from ai fifo */
+static int labpc_drain_fifo(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+	struct comedi_async *async = dev->read_subdev->async;
+	struct comedi_cmd *cmd = &async->cmd;
+	unsigned short data;
+	const int timeout = 10000;
+	unsigned int i;
+
+	devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
+
+	for (i = 0; (devpriv->stat1 & STAT1_DAVAIL) && i < timeout;
+	     i++) {
+		/*  quit if we have all the data we want */
+		if (cmd->stop_src == TRIG_COUNT) {
+			if (devpriv->count == 0)
+				break;
+			devpriv->count--;
+		}
+		data = labpc_read_adc_fifo(dev);
+		cfc_write_to_buffer(dev->read_subdev, data);
+		devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
+	}
+	if (i == timeout) {
+		dev_err(dev->class_dev, "ai timeout, fifo never empties\n");
+		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		return -1;
+	}
+
+	return 0;
+}
+
+/* makes sure all data acquired by board is transferred to comedi (used
+ * when acquisition is terminated by stop_src == TRIG_EXT). */
+static void labpc_drain_dregs(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	if (devpriv->current_transfer == isa_dma_transfer)
+		labpc_drain_dma(dev);
+
+	labpc_drain_fifo(dev);
+}
+
+/* interrupt service routine */
+static irqreturn_t labpc_interrupt(int irq, void *d)
+{
+	struct comedi_device *dev = d;
+	const struct labpc_boardinfo *board = dev->board_ptr;
+	struct labpc_private *devpriv = dev->private;
+	struct comedi_subdevice *s = dev->read_subdev;
+	struct comedi_async *async;
+	struct comedi_cmd *cmd;
+
+	if (!dev->attached) {
+		dev_err(dev->class_dev, "premature interrupt\n");
+		return IRQ_HANDLED;
+	}
+
+	async = s->async;
+	cmd = &async->cmd;
+
+	/* read board status */
+	devpriv->stat1 = devpriv->read_byte(dev, STAT1_REG);
+	if (board->is_labpc1200)
+		devpriv->stat2 = devpriv->read_byte(dev, STAT2_REG);
+
+	if ((devpriv->stat1 & (STAT1_GATA0 | STAT1_CNTINT | STAT1_OVERFLOW |
+			       STAT1_OVERRUN | STAT1_DAVAIL)) == 0 &&
+	    (devpriv->stat2 & STAT2_OUTA1) == 0 &&
+	    (devpriv->stat2 & STAT2_FIFONHF)) {
+		return IRQ_NONE;
+	}
+
+	if (devpriv->stat1 & STAT1_OVERRUN) {
+		/* clear error interrupt */
+		devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
+		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		cfc_handle_events(dev, s);
+		dev_err(dev->class_dev, "overrun\n");
+		return IRQ_HANDLED;
+	}
+
+	if (devpriv->current_transfer == isa_dma_transfer)
+		labpc_handle_dma_status(dev);
+	else
+		labpc_drain_fifo(dev);
+
+	if (devpriv->stat1 & STAT1_CNTINT) {
+		dev_err(dev->class_dev, "handled timer interrupt?\n");
+		/*  clear it */
+		devpriv->write_byte(dev, 0x1, TIMER_CLEAR_REG);
+	}
+
+	if (devpriv->stat1 & STAT1_OVERFLOW) {
+		/*  clear error interrupt */
+		devpriv->write_byte(dev, 0x1, ADC_FIFO_CLEAR_REG);
+		async->events |= COMEDI_CB_ERROR | COMEDI_CB_EOA;
+		cfc_handle_events(dev, s);
+		dev_err(dev->class_dev, "overflow\n");
+		return IRQ_HANDLED;
+	}
+	/*  handle external stop trigger */
+	if (cmd->stop_src == TRIG_EXT) {
+		if (devpriv->stat2 & STAT2_OUTA1) {
+			labpc_drain_dregs(dev);
+			async->events |= COMEDI_CB_EOA;
+		}
+	}
+
+	/* TRIG_COUNT end of acquisition */
+	if (cmd->stop_src == TRIG_COUNT) {
+		if (devpriv->count == 0)
+			async->events |= COMEDI_CB_EOA;
+	}
+
+	cfc_handle_events(dev, s);
+	return IRQ_HANDLED;
+}
+
+static int labpc_ao_insn_write(struct comedi_device *dev,
+			       struct comedi_subdevice *s,
+			       struct comedi_insn *insn,
+			       unsigned int *data)
+{
+	const struct labpc_boardinfo *board = dev->board_ptr;
+	struct labpc_private *devpriv = dev->private;
+	int channel, range;
+	unsigned long flags;
+	int lsb, msb;
+
+	channel = CR_CHAN(insn->chanspec);
+
+	/* turn off pacing of analog output channel */
+	/* note: hardware bug in daqcard-1200 means pacing cannot
+	 * be independently enabled/disabled for its the two channels */
+	spin_lock_irqsave(&dev->spinlock, flags);
+	devpriv->cmd2 &= ~CMD2_LDAC(channel);
+	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
+	spin_unlock_irqrestore(&dev->spinlock, flags);
+
+	/* set range */
+	if (board->is_labpc1200) {
+		range = CR_RANGE(insn->chanspec);
+		if (comedi_range_is_unipolar(s, range))
+			devpriv->cmd6 |= CMD6_DACUNI(channel);
+		else
+			devpriv->cmd6 &= ~CMD6_DACUNI(channel);
+		/*  write to register */
+		devpriv->write_byte(dev, devpriv->cmd6, CMD6_REG);
+	}
+	/* send data */
+	lsb = data[0] & 0xff;
+	msb = (data[0] >> 8) & 0xff;
+	devpriv->write_byte(dev, lsb, DAC_LSB_REG(channel));
+	devpriv->write_byte(dev, msb, DAC_MSB_REG(channel));
+
+	/* remember value for readback */
+	devpriv->ao_value[channel] = data[0];
+
+	return 1;
+}
+
+static int labpc_ao_insn_read(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	data[0] = devpriv->ao_value[CR_CHAN(insn->chanspec)];
+
+	return 1;
+}
+
+/* lowlevel write to eeprom/dac */
+static void labpc_serial_out(struct comedi_device *dev, unsigned int value,
+			     unsigned int value_width)
+{
+	struct labpc_private *devpriv = dev->private;
+	int i;
+
+	for (i = 1; i <= value_width; i++) {
+		/*  clear serial clock */
+		devpriv->cmd5 &= ~CMD5_SCLK;
+		/*  send bits most significant bit first */
+		if (value & (1 << (value_width - i)))
+			devpriv->cmd5 |= CMD5_SDATA;
+		else
+			devpriv->cmd5 &= ~CMD5_SDATA;
+		udelay(1);
+		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+		/*  set clock to load bit */
+		devpriv->cmd5 |= CMD5_SCLK;
+		udelay(1);
+		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+	}
+}
+
+/* lowlevel read from eeprom */
+static unsigned int labpc_serial_in(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int value = 0;
+	int i;
+	const int value_width = 8;	/*  number of bits wide values are */
+
+	for (i = 1; i <= value_width; i++) {
+		/*  set serial clock */
+		devpriv->cmd5 |= CMD5_SCLK;
+		udelay(1);
+		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+		/*  clear clock bit */
+		devpriv->cmd5 &= ~CMD5_SCLK;
+		udelay(1);
+		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+		/*  read bits most significant bit first */
+		udelay(1);
+		devpriv->stat2 = devpriv->read_byte(dev, STAT2_REG);
+		if (devpriv->stat2 & STAT2_PROMOUT)
+			value |= 1 << (value_width - i);
+	}
+
+	return value;
+}
+
+static unsigned int labpc_eeprom_read(struct comedi_device *dev,
+				      unsigned int address)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int value;
+	/*  bits to tell eeprom to expect a read */
+	const int read_instruction = 0x3;
+	/*  8 bit write lengths to eeprom */
+	const int write_length = 8;
+
+	/*  enable read/write to eeprom */
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	/*  send read instruction */
+	labpc_serial_out(dev, read_instruction, write_length);
+	/*  send 8 bit address to read from */
+	labpc_serial_out(dev, address, write_length);
+	/*  read result */
+	value = labpc_serial_in(dev);
+
+	/*  disable read/write to eeprom */
+	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	return value;
+}
+
+static unsigned int labpc_eeprom_read_status(struct comedi_device *dev)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int value;
+	const int read_status_instruction = 0x5;
+	const int write_length = 8;	/*  8 bit write lengths to eeprom */
+
+	/*  enable read/write to eeprom */
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	/*  send read status instruction */
+	labpc_serial_out(dev, read_status_instruction, write_length);
+	/*  read result */
+	value = labpc_serial_in(dev);
+
+	/*  disable read/write to eeprom */
+	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	return value;
+}
+
+static int labpc_eeprom_write(struct comedi_device *dev,
+			      unsigned int address, unsigned int value)
+{
+	struct labpc_private *devpriv = dev->private;
+	const int write_enable_instruction = 0x6;
+	const int write_instruction = 0x2;
+	const int write_length = 8;	/*  8 bit write lengths to eeprom */
+	const int write_in_progress_bit = 0x1;
+	const int timeout = 10000;
+	int i;
+
+	/*  make sure there isn't already a write in progress */
+	for (i = 0; i < timeout; i++) {
+		if ((labpc_eeprom_read_status(dev) & write_in_progress_bit) ==
+		    0)
+			break;
+	}
+	if (i == timeout) {
+		dev_err(dev->class_dev, "eeprom write timed out\n");
+		return -ETIME;
+	}
+	/*  update software copy of eeprom */
+	devpriv->eeprom_data[address] = value;
+
+	/*  enable read/write to eeprom */
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+	devpriv->cmd5 |= (CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	/*  send write_enable instruction */
+	labpc_serial_out(dev, write_enable_instruction, write_length);
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	/*  send write instruction */
+	devpriv->cmd5 |= CMD5_EEPROMCS;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+	labpc_serial_out(dev, write_instruction, write_length);
+	/*  send 8 bit address to write to */
+	labpc_serial_out(dev, address, write_length);
+	/*  write value */
+	labpc_serial_out(dev, value, write_length);
+	devpriv->cmd5 &= ~CMD5_EEPROMCS;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	/*  disable read/write to eeprom */
+	devpriv->cmd5 &= ~(CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	return 0;
+}
+
+/* writes to 8 bit calibration dacs */
+static void write_caldac(struct comedi_device *dev, unsigned int channel,
+			 unsigned int value)
+{
+	struct labpc_private *devpriv = dev->private;
+
+	if (value == devpriv->caldac[channel])
+		return;
+	devpriv->caldac[channel] = value;
+
+	/*  clear caldac load bit and make sure we don't write to eeprom */
+	devpriv->cmd5 &= ~(CMD5_CALDACLD | CMD5_EEPROMCS | CMD5_WRTPRT);
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+
+	/*  write 4 bit channel */
+	labpc_serial_out(dev, channel, 4);
+	/*  write 8 bit caldac value */
+	labpc_serial_out(dev, value, 8);
+
+	/*  set and clear caldac bit to load caldac value */
+	devpriv->cmd5 |= CMD5_CALDACLD;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+	devpriv->cmd5 &= ~CMD5_CALDACLD;
+	udelay(1);
+	devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+}
+
+static int labpc_calib_insn_write(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+
+	/*
+	 * Only write the last data value to the caldac. Preceding
+	 * data would be overwritten anyway.
+	 */
+	if (insn->n > 0)
+		write_caldac(dev, chan, data[insn->n - 1]);
+
+	return insn->n;
+}
+
+static int labpc_calib_insn_read(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->caldac[chan];
+
+	return insn->n;
+}
+
+static int labpc_eeprom_insn_write(struct comedi_device *dev,
+				   struct comedi_subdevice *s,
+				   struct comedi_insn *insn,
+				   unsigned int *data)
+{
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int ret;
+
+	/* only allow writes to user area of eeprom */
+	if (chan < 16 || chan > 127)
+		return -EINVAL;
+
+	/*
+	 * Only write the last data value to the eeprom. Preceding
+	 * data would be overwritten anyway.
+	 */
+	if (insn->n > 0) {
+		ret = labpc_eeprom_write(dev, chan, data[insn->n - 1]);
+		if (ret)
+			return ret;
+	}
+
+	return insn->n;
+}
+
+static int labpc_eeprom_insn_read(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	struct labpc_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
+	int i;
+
+	for (i = 0; i < insn->n; i++)
+		data[i] = devpriv->eeprom_data[chan];
+
+	return insn->n;
+}
+
+int labpc_common_attach(struct comedi_device *dev,
+			unsigned int irq, unsigned long isr_flags)
+{
+	const struct labpc_boardinfo *board = dev->board_ptr;
+	struct labpc_private *devpriv = dev->private;
+	struct comedi_subdevice *s;
+	int ret;
+	int i;
+
+	if (dev->mmio) {
+		devpriv->read_byte = labpc_readb;
+		devpriv->write_byte = labpc_writeb;
+	} else {
+		devpriv->read_byte = labpc_inb;
+		devpriv->write_byte = labpc_outb;
+	}
+
+	/* initialize board's command registers */
+	devpriv->write_byte(dev, devpriv->cmd1, CMD1_REG);
+	devpriv->write_byte(dev, devpriv->cmd2, CMD2_REG);
+	devpriv->write_byte(dev, devpriv->cmd3, CMD3_REG);
+	devpriv->write_byte(dev, devpriv->cmd4, CMD4_REG);
+	if (board->is_labpc1200) {
+		devpriv->write_byte(dev, devpriv->cmd5, CMD5_REG);
+		devpriv->write_byte(dev, devpriv->cmd6, CMD6_REG);
+	}
+
+	if (irq) {
+		ret = request_irq(irq, labpc_interrupt, isr_flags,
+				  dev->board_name, dev);
+		if (ret == 0)
+			dev->irq = irq;
+	}
+
+	ret = comedi_alloc_subdevices(dev, 5);
+	if (ret)
+		return ret;
+
+	/* analog input subdevice */
+	s = &dev->subdevices[0];
+	s->type		= COMEDI_SUBD_AI;
+	s->subdev_flags	= SDF_READABLE | SDF_GROUND | SDF_COMMON | SDF_DIFF;
+	s->n_chan	= 8;
+	s->len_chanlist	= 8;
+	s->maxdata	= 0x0fff;
+	s->range_table	= board->is_labpc1200 ?
+			  &range_labpc_1200_ai : &range_labpc_plus_ai;
+	s->insn_read	= labpc_ai_insn_read;
+	if (dev->irq) {
+		dev->read_subdev = s;
+		s->subdev_flags	|= SDF_CMD_READ;
+		s->do_cmd	= labpc_ai_cmd;
+		s->do_cmdtest	= labpc_ai_cmdtest;
+		s->cancel	= labpc_cancel;
+	}
+
+	/* analog output */
+	s = &dev->subdevices[1];
+	if (board->has_ao) {
+		s->type		= COMEDI_SUBD_AO;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_GROUND;
+		s->n_chan	= NUM_AO_CHAN;
+		s->maxdata	= 0x0fff;
+		s->range_table	= &range_labpc_ao;
+		s->insn_read	= labpc_ao_insn_read;
+		s->insn_write	= labpc_ao_insn_write;
+
+		/* initialize analog outputs to a known value */
+		for (i = 0; i < s->n_chan; i++) {
+			short lsb, msb;
+
+			devpriv->ao_value[i] = s->maxdata / 2;
+			lsb = devpriv->ao_value[i] & 0xff;
+			msb = (devpriv->ao_value[i] >> 8) & 0xff;
+			devpriv->write_byte(dev, lsb, DAC_LSB_REG(i));
+			devpriv->write_byte(dev, msb, DAC_MSB_REG(i));
+		}
+	} else {
+		s->type		= COMEDI_SUBD_UNUSED;
+	}
+
+	/* 8255 dio */
+	s = &dev->subdevices[2];
+	if (dev->mmio)
+		ret = subdev_8255_mm_init(dev, s, NULL, DIO_BASE_REG);
+	else
+		ret = subdev_8255_init(dev, s, NULL, DIO_BASE_REG);
+	if (ret)
+		return ret;
+
+	/*  calibration subdevices for boards that have one */
+	s = &dev->subdevices[3];
+	if (board->is_labpc1200) {
+		s->type		= COMEDI_SUBD_CALIB;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+		s->n_chan	= 16;
+		s->maxdata	= 0xff;
+		s->insn_read	= labpc_calib_insn_read;
+		s->insn_write	= labpc_calib_insn_write;
+
+		for (i = 0; i < s->n_chan; i++)
+			write_caldac(dev, i, s->maxdata / 2);
+	} else {
+		s->type		= COMEDI_SUBD_UNUSED;
+	}
+
+	/* EEPROM */
+	s = &dev->subdevices[4];
+	if (board->is_labpc1200) {
+		s->type		= COMEDI_SUBD_MEMORY;
+		s->subdev_flags	= SDF_READABLE | SDF_WRITABLE | SDF_INTERNAL;
+		s->n_chan	= EEPROM_SIZE;
+		s->maxdata	= 0xff;
+		s->insn_read	= labpc_eeprom_insn_read;
+		s->insn_write	= labpc_eeprom_insn_write;
+
+		for (i = 0; i < s->n_chan; i++)
+			devpriv->eeprom_data[i] = labpc_eeprom_read(dev, i);
+	} else {
+		s->type		= COMEDI_SUBD_UNUSED;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(labpc_common_attach);
+
+static int __init labpc_common_init(void)
+{
+	return 0;
+}
+module_init(labpc_common_init);
+
+static void __exit labpc_common_exit(void)
+{
+}
+module_exit(labpc_common_exit);
+
+MODULE_AUTHOR("Comedi http://www.comedi.org");
+MODULE_DESCRIPTION("Comedi helper for ni_labpc, ni_labpc_pci, ni_labpc_cs");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/ni_labpc_isadma.c b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
index cb7d1c952cf2..967202e0635e 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_isadma.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_isadma.c
@@ -152,7 +152,7 @@ static void handle_isa_dma(struct comedi_device *dev)
 
 void labpc_handle_dma_status(struct comedi_device *dev)
 {
-	const struct labpc_boardinfo *board = comedi_board(dev);
+	const struct labpc_boardinfo *board = dev->board_ptr;
 	struct labpc_private *devpriv = dev->private;
 
 	/*
diff --git a/drivers/staging/comedi/drivers/ni_labpc_pci.c b/drivers/staging/comedi/drivers/ni_labpc_pci.c
index 65984ea0a3ee..3fc420406564 100644
--- a/drivers/staging/comedi/drivers/ni_labpc_pci.c
+++ b/drivers/staging/comedi/drivers/ni_labpc_pci.c
@@ -108,20 +108,11 @@ static int labpc_pci_auto_attach(struct comedi_device *dev,
 	return labpc_common_attach(dev, pcidev->irq, IRQF_SHARED);
 }
 
-static void labpc_pci_detach(struct comedi_device *dev)
-{
-	if (dev->mmio)
-		iounmap(dev->mmio);
-	if (dev->irq)
-		free_irq(dev->irq, dev);
-	comedi_pci_disable(dev);
-}
-
 static struct comedi_driver labpc_pci_comedi_driver = {
 	.driver_name	= "labpc_pci",
 	.module		= THIS_MODULE,
 	.auto_attach	= labpc_pci_auto_attach,
-	.detach		= labpc_pci_detach,
+	.detach		= comedi_pci_detach,
 };
 
 static const struct pci_device_id labpc_pci_table[] = {
diff --git a/drivers/staging/comedi/drivers/ni_mio_common.c b/drivers/staging/comedi/drivers/ni_mio_common.c
index 297c95d2e0a3..320b080149b6 100644
--- a/drivers/staging/comedi/drivers/ni_mio_common.c
+++ b/drivers/staging/comedi/drivers/ni_mio_common.c
@@ -1186,7 +1186,7 @@ static void ni_ao_fifo_load(struct comedi_device *dev,
 static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 				 struct comedi_subdevice *s)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	int n;
 
 	n = comedi_buf_read_n_available(s);
@@ -1209,7 +1209,7 @@ static int ni_ao_fifo_half_empty(struct comedi_device *dev,
 static int ni_ao_prep_fifo(struct comedi_device *dev,
 			   struct comedi_subdevice *s)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	int n;
 
@@ -1296,7 +1296,7 @@ static void ni_ai_fifo_read(struct comedi_device *dev,
 
 static void ni_handle_fifo_half_full(struct comedi_device *dev)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct comedi_subdevice *s = dev->read_subdev;
 	int n;
 
@@ -1881,7 +1881,7 @@ static void ni_m_series_load_channelgain_list(struct comedi_device *dev,
 					      unsigned int n_chan,
 					      unsigned int *list)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	unsigned int chan, range, aref;
 	unsigned int i;
@@ -1988,7 +1988,7 @@ static void ni_load_channelgain_list(struct comedi_device *dev,
 				     struct comedi_subdevice *s,
 				     unsigned int n_chan, unsigned int *list)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	unsigned int offset = (s->maxdata + 1) >> 1;
 	unsigned int chan, range, aref;
@@ -2108,7 +2108,7 @@ static int ni_ai_insn_read(struct comedi_device *dev,
 	unsigned int mask = (s->maxdata + 1) >> 1;
 	int i, n;
 	unsigned signbits;
-	unsigned short d;
+	unsigned int d;
 	unsigned long dl;
 
 	ni_load_channelgain_list(dev, s, 1, &insn->chanspec);
@@ -2206,15 +2206,15 @@ static int ni_ns_to_timer(const struct comedi_device *dev, unsigned nanosec,
 	struct ni_private *devpriv = dev->private;
 	int divider;
 
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_NEAREST:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
 	default:
 		divider = (nanosec + devpriv->clock_ns / 2) / devpriv->clock_ns;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		divider = (nanosec) / devpriv->clock_ns;
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		divider = (nanosec + devpriv->clock_ns - 1) / devpriv->clock_ns;
 		break;
 	}
@@ -2231,7 +2231,7 @@ static unsigned ni_timer_to_ns(const struct comedi_device *dev, int timer)
 static unsigned ni_min_ai_scan_period_ns(struct comedi_device *dev,
 					 unsigned num_channels)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 
 	/* simultaneously-sampled inputs */
@@ -2245,7 +2245,7 @@ static unsigned ni_min_ai_scan_period_ns(struct comedi_device *dev,
 static int ni_ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_cmd *cmd)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
@@ -2541,7 +2541,7 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 		/* load SI */
 		timer = ni_ns_to_timer(dev, cmd->scan_begin_arg,
-				       TRIG_ROUND_NEAREST);
+				       CMDF_ROUND_NEAREST);
 		ni_stc_writel(dev, timer, AI_SI_Load_A_Registers);
 		ni_stc_writew(dev, AI_SI_Load, AI_Command_1_Register);
 		break;
@@ -2569,7 +2569,7 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 			timer = 1;
 		else
 			timer = ni_ns_to_timer(dev, cmd->convert_arg,
-					       TRIG_ROUND_NEAREST);
+					       CMDF_ROUND_NEAREST);
 		/* 0,0 does not work */
 		ni_stc_writew(dev, 1, AI_SI2_Load_A_Register);
 		ni_stc_writew(dev, timer, AI_SI2_Load_B_Register);
@@ -2610,7 +2610,7 @@ static int ni_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		interrupt_a_enable |= AI_FIFO_Interrupt_Enable;
 #endif
 
-		if (cmd->flags & TRIG_WAKE_EOS
+		if (cmd->flags & CMDF_WAKE_EOS
 		    || (devpriv->ai_cmd2 & AI_End_On_End_Of_Scan)) {
 			/* wake on end-of-scan */
 			devpriv->aimode = AIMODE_SCAN;
@@ -2732,9 +2732,6 @@ static int ni_ai_insn_config(struct comedi_device *dev,
 
 			calib_source = data[1] & 0xf;
 
-			if (calib_source > 0xF)
-				return -EINVAL;
-
 			devpriv->ai_calib_source = calib_source;
 			ni_writew(dev, calib_source, Calibration_Channel_6143);
 		} else {
@@ -2921,21 +2918,6 @@ static int ni_ao_config_chanlist(struct comedi_device *dev,
 		return ni_old_ao_config_chanlist(dev, s, chanspec, n_chans);
 }
 
-static int ni_ao_insn_read(struct comedi_device *dev,
-			   struct comedi_subdevice *s,
-			   struct comedi_insn *insn,
-			   unsigned int *data)
-{
-	struct ni_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao[chan];
-
-	return insn->n;
-}
-
 static int ni_ao_insn_write(struct comedi_device *dev,
 			    struct comedi_subdevice *s,
 			    struct comedi_insn *insn,
@@ -2962,7 +2944,7 @@ static int ni_ao_insn_write(struct comedi_device *dev,
 	for (i = 0; i < insn->n; i++) {
 		unsigned int val = data[i];
 
-		devpriv->ao[chan] = val;
+		s->readback[chan] = val;
 
 		if (devpriv->is_6xxx) {
 			/*
@@ -2997,7 +2979,7 @@ static int ni_ao_insn_config(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn, unsigned int *data)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 
 	switch (data[0]) {
@@ -3098,7 +3080,7 @@ static int ni_ao_inttrig(struct comedi_device *dev,
 
 static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	const struct comedi_cmd *cmd = &s->async->cmd;
 	int bits;
@@ -3208,7 +3190,7 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		devpriv->ao_cmd2 &= ~AO_BC_Gate_Enable;
 		trigvar =
 		    ni_ns_to_timer(dev, cmd->scan_begin_arg,
-				   TRIG_ROUND_NEAREST);
+				   CMDF_ROUND_NEAREST);
 		ni_stc_writel(dev, 1, AO_UI_Load_A_Register);
 		ni_stc_writew(dev, AO_UI_Load, AO_Command_1_Register);
 		ni_stc_writel(dev, trigvar, AO_UI_Load_A_Register);
@@ -3299,7 +3281,7 @@ static int ni_ao_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 static int ni_ao_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 			 struct comedi_cmd *cmd)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int tmp;
@@ -3552,12 +3534,10 @@ static int ni_cdio_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: fix up any arguments */
-
-	if (err)
-		return 4;
+	/* Step 4: fix up any arguments */
 
 	/* Step 5: check channel list if it exists */
+
 	if (cmd->chanlist && cmd->chanlist_len > 0)
 		err |= ni_cdio_check_chanlist(dev, s, cmd);
 
@@ -4176,16 +4156,15 @@ static int ni_freq_out_insn_config(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int ni_8255_callback(int dir, int port, int data, unsigned long arg)
+static int ni_8255_callback(struct comedi_device *dev,
+			    int dir, int port, int data, unsigned long iobase)
 {
-	struct comedi_device *dev = (struct comedi_device *)arg;
-
 	if (dir) {
-		ni_writeb(dev, data, Port_A + 2 * port);
+		ni_writeb(dev, data, iobase + 2 * port);
 		return 0;
 	}
 
-	return ni_readb(dev, Port_A + 2 * port);
+	return ni_readb(dev, iobase + 2 * port);
 }
 
 static int ni_get_pwm_config(struct comedi_device *dev, unsigned int *data)
@@ -4208,15 +4187,15 @@ static int ni_m_series_pwm_config(struct comedi_device *dev,
 	switch (data[0]) {
 	case INSN_CONFIG_PWM_OUTPUT:
 		switch (data[1]) {
-		case TRIG_ROUND_NEAREST:
+		case CMDF_ROUND_NEAREST:
 			up_count =
 			    (data[2] +
 			     devpriv->clock_ns / 2) / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_DOWN:
+		case CMDF_ROUND_DOWN:
 			up_count = data[2] / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_UP:
+		case CMDF_ROUND_UP:
 			up_count =
 			    (data[2] + devpriv->clock_ns -
 			     1) / devpriv->clock_ns;
@@ -4225,15 +4204,15 @@ static int ni_m_series_pwm_config(struct comedi_device *dev,
 			return -EINVAL;
 		}
 		switch (data[3]) {
-		case TRIG_ROUND_NEAREST:
+		case CMDF_ROUND_NEAREST:
 			down_count =
 			    (data[4] +
 			     devpriv->clock_ns / 2) / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_DOWN:
+		case CMDF_ROUND_DOWN:
 			down_count = data[4] / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_UP:
+		case CMDF_ROUND_UP:
 			down_count =
 			    (data[4] + devpriv->clock_ns -
 			     1) / devpriv->clock_ns;
@@ -4272,15 +4251,15 @@ static int ni_6143_pwm_config(struct comedi_device *dev,
 	switch (data[0]) {
 	case INSN_CONFIG_PWM_OUTPUT:
 		switch (data[1]) {
-		case TRIG_ROUND_NEAREST:
+		case CMDF_ROUND_NEAREST:
 			up_count =
 			    (data[2] +
 			     devpriv->clock_ns / 2) / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_DOWN:
+		case CMDF_ROUND_DOWN:
 			up_count = data[2] / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_UP:
+		case CMDF_ROUND_UP:
 			up_count =
 			    (data[2] + devpriv->clock_ns -
 			     1) / devpriv->clock_ns;
@@ -4289,15 +4268,15 @@ static int ni_6143_pwm_config(struct comedi_device *dev,
 			return -EINVAL;
 		}
 		switch (data[3]) {
-		case TRIG_ROUND_NEAREST:
+		case CMDF_ROUND_NEAREST:
 			down_count =
 			    (data[4] +
 			     devpriv->clock_ns / 2) / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_DOWN:
+		case CMDF_ROUND_DOWN:
 			down_count = data[4] / devpriv->clock_ns;
 			break;
-		case TRIG_ROUND_UP:
+		case CMDF_ROUND_UP:
 			down_count =
 			    (data[4] + devpriv->clock_ns -
 			     1) / devpriv->clock_ns;
@@ -4390,7 +4369,7 @@ static struct caldac_struct caldacs[] = {
 
 static void ni_write_caldac(struct comedi_device *dev, int addr, int val)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	unsigned int loadbit = 0, bits = 0, bit, bitstring = 0;
 	int i;
@@ -4448,7 +4427,7 @@ static int ni_calib_insn_read(struct comedi_device *dev,
 
 static void caldac_setup(struct comedi_device *dev, struct comedi_subdevice *s)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	int i, j;
 	int n_dacs;
@@ -5418,7 +5397,7 @@ static int ni_alloc_private(struct comedi_device *dev)
 static int ni_E_init(struct comedi_device *dev,
 		     unsigned interrupt_pin, unsigned irq_polarity)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	int ret;
@@ -5491,9 +5470,13 @@ static int ni_E_init(struct comedi_device *dev,
 		s->n_chan	= board->n_aochan;
 		s->maxdata	= board->ao_maxdata;
 		s->range_table	= board->ao_range_table;
-		s->insn_read	= ni_ao_insn_read;
-		s->insn_write	= ni_ao_insn_write;
 		s->insn_config	= ni_ao_insn_config;
+		s->insn_write	= ni_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 
 		/*
 		 * Along with the IRQ we need either a FIFO or DMA for
@@ -5560,8 +5543,7 @@ static int ni_E_init(struct comedi_device *dev,
 	/* 8255 device */
 	s = &dev->subdevices[NI_8255_DIO_SUBDEV];
 	if (board->has_8255) {
-		ret = subdev_8255_init(dev, s, ni_8255_callback,
-				       (unsigned long)dev);
+		ret = subdev_8255_init(dev, s, ni_8255_callback, Port_A);
 		if (ret)
 			return ret;
 	} else {
diff --git a/drivers/staging/comedi/drivers/ni_pcidio.c b/drivers/staging/comedi/drivers/ni_pcidio.c
index b5b36af80205..5252cba82e5e 100644
--- a/drivers/staging/comedi/drivers/ni_pcidio.c
+++ b/drivers/staging/comedi/drivers/ni_pcidio.c
@@ -530,15 +530,15 @@ static int ni_pcidio_ns_to_timer(int *nanosec, unsigned int flags)
 
 	base = TIMER_BASE;
 
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_NEAREST:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
 	default:
 		divider = (*nanosec + base / 2) / base;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		divider = (*nanosec) / base;
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		divider = (*nanosec + base - 1) / base;
 		break;
 	}
@@ -598,11 +598,10 @@ static int ni_pcidio_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* no limit */
-	} else {	/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -669,7 +668,7 @@ static int ni_pcidio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		writeb(3, dev->mmio + LinePolarities);
 		writeb(0xc0, dev->mmio + AckSer);
 		writel(ni_pcidio_ns_to_timer(&cmd->scan_begin_arg,
-					     TRIG_ROUND_NEAREST),
+					     CMDF_ROUND_NEAREST),
 		       dev->mmio + StartDelay);
 		writeb(1, dev->mmio + ReqDelay);
 		writeb(1, dev->mmio + ReqNotDelay);
diff --git a/drivers/staging/comedi/drivers/ni_pcimio.c b/drivers/staging/comedi/drivers/ni_pcimio.c
index da61fa70decf..3b2bdebbca59 100644
--- a/drivers/staging/comedi/drivers/ni_pcimio.c
+++ b/drivers/staging/comedi/drivers/ni_pcimio.c
@@ -1082,7 +1082,7 @@ static void m_series_init_eeprom_buffer(struct comedi_device *dev)
 
 static void init_6143(struct comedi_device *dev)
 {
-	const struct ni_board_struct *board = comedi_board(dev);
+	const struct ni_board_struct *board = dev->board_ptr;
 	struct ni_private *devpriv = dev->private;
 
 	/*  Disable interrupts */
diff --git a/drivers/staging/comedi/drivers/ni_stc.h b/drivers/staging/comedi/drivers/ni_stc.h
index a2841292ddd4..29efce30eb7f 100644
--- a/drivers/staging/comedi/drivers/ni_stc.h
+++ b/drivers/staging/comedi/drivers/ni_stc.h
@@ -1423,7 +1423,6 @@ struct ni_private {
 	unsigned int changain_spec;
 
 	unsigned int caldac_maxdata_list[MAX_N_CALDACS];
-	unsigned short ao[MAX_N_AO_CHAN];
 	unsigned short caldacs[MAX_N_CALDACS];
 
 	unsigned short ai_cmd2;
diff --git a/drivers/staging/comedi/drivers/ni_tiocmd.c b/drivers/staging/comedi/drivers/ni_tiocmd.c
index 299ceddfb233..26e7291c4a51 100644
--- a/drivers/staging/comedi/drivers/ni_tiocmd.c
+++ b/drivers/staging/comedi/drivers/ni_tiocmd.c
@@ -185,7 +185,7 @@ static int ni_tio_cmd_setup(struct comedi_subdevice *s)
 	}
 	if (set_gate_source)
 		retval = ni_tio_set_gate_src(counter, 0, gate_source);
-	if (cmd->flags & TRIG_WAKE_EOS) {
+	if (cmd->flags & CMDF_WAKE_EOS) {
 		ni_tio_set_bits(counter, NITIO_INT_ENA_REG(cidx),
 				GI_GATE_INTERRUPT_ENABLE(cidx),
 				GI_GATE_INTERRUPT_ENABLE(cidx));
@@ -286,10 +286,9 @@ int ni_tio_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: fix up any arguments */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
diff --git a/drivers/staging/comedi/drivers/ni_usb6501.c b/drivers/staging/comedi/drivers/ni_usb6501.c
new file mode 100644
index 000000000000..df7ada8611f4
--- /dev/null
+++ b/drivers/staging/comedi/drivers/ni_usb6501.c
@@ -0,0 +1,621 @@
+/*
+ * comedi/drivers/ni_usb6501.c
+ * Comedi driver for National Instruments USB-6501
+ *
+ * COMEDI - Linux Control and Measurement Device Interface
+ * Copyright (C) 2014 Luca Ellero <luca.ellero@brickedbrain.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+/*
+ * Driver: ni_usb6501
+ * Description: National Instruments USB-6501 module
+ * Devices: [National Instruments] USB-6501 (ni_usb6501)
+ * Author: Luca Ellero <luca.ellero@brickedbrain.com>
+ * Updated: 8 Sep 2014
+ * Status: works
+ *
+ *
+ * Configuration Options:
+ * none
+ */
+
+/*
+ * NI-6501 - USB PROTOCOL DESCRIPTION
+ *
+ * Every command is composed by two USB packets:
+ *	- request (out)
+ *	- response (in)
+ *
+ * Every packet is at least 12 bytes long, here is the meaning of
+ * every field (all values are hex):
+ *
+ *	byte 0 is always 00
+ *	byte 1 is always 01
+ *	byte 2 is always 00
+ *	byte 3 is the total packet length
+ *
+ *	byte 4 is always 00
+ *	byte 5 is is the total packet length - 4
+ *	byte 6 is always 01
+ *	byte 7 is the command
+ *
+ *	byte 8 is 02 (request) or 00 (response)
+ *	byte 9 is 00 (response) or 10 (port request) or 20 (counter request)
+ *	byte 10 is always 00
+ *	byte 11 is 00 (request) or 02 (response)
+ *
+ * PORT PACKETS
+ *
+ *	CMD: 0xE READ_PORT
+ *	REQ: 00 01 00 10 00 0C 01 0E 02 10 00 00 00 03 <PORT> 00
+ *	RES: 00 01 00 10 00 0C 01 00 00 00 00 02 00 03 <BMAP> 00
+ *
+ *	CMD: 0xF WRITE_PORT
+ *	REQ: 00 01 00 14 00 10 01 0F 02 10 00 00 00 03 <PORT> 00 03 <BMAP> 00 00
+ *	RES: 00 01 00 0C 00 08 01 00 00 00 00 02
+ *
+ *	CMD: 0x12 SET_PORT_DIR (0 = input, 1 = output)
+ *	REQ: 00 01 00 18 00 14 01 12 02 10 00 00
+ *	     00 05 <PORT 0> <PORT 1> <PORT 2> 00 05 00 00 00 00 00
+ *	RES: 00 01 00 0C 00 08 01 00 00 00 00 02
+ *
+ * COUNTER PACKETS
+ *
+ *	CMD 0x9: START_COUNTER
+ *	REQ: 00 01 00 0C 00 08 01 09 02 20 00 00
+ *	RES: 00 01 00 0C 00 08 01 00 00 00 00 02
+ *
+ *	CMD 0xC: STOP_COUNTER
+ *	REQ: 00 01 00 0C 00 08 01 0C 02 20 00 00
+ *	RES: 00 01 00 0C 00 08 01 00 00 00 00 02
+ *
+ *	CMD 0xE: READ_COUNTER
+ *	REQ: 00 01 00 0C 00 08 01 0E 02 20 00 00
+ *	RES: 00 01 00 10 00 0C 01 00 00 00 00 02 <u32 counter value, Big Endian>
+ *
+ *	CMD 0xF: WRITE_COUNTER
+ *	REQ: 00 01 00 10 00 0C 01 0F 02 20 00 00 <u32 counter value, Big Endian>
+ *	RES: 00 01 00 0C 00 08 01 00 00 00 00 02
+ *
+ *
+ *	Please  visit http://www.brickedbrain.com if you need
+ *	additional information or have any questions.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/usb.h>
+
+#include "../comedidev.h"
+
+#define	NI6501_TIMEOUT	1000
+
+/* Port request packets */
+static const u8 READ_PORT_REQUEST[]	= {0x00, 0x01, 0x00, 0x10,
+					   0x00, 0x0C, 0x01, 0x0E,
+					   0x02, 0x10, 0x00, 0x00,
+					   0x00, 0x03, 0x00, 0x00};
+
+static const u8 WRITE_PORT_REQUEST[]	= {0x00, 0x01, 0x00, 0x14,
+					   0x00, 0x10, 0x01, 0x0F,
+					   0x02, 0x10, 0x00, 0x00,
+					   0x00, 0x03, 0x00, 0x00,
+					   0x03, 0x00, 0x00, 0x00};
+
+static const u8 SET_PORT_DIR_REQUEST[]	= {0x00, 0x01, 0x00, 0x18,
+					   0x00, 0x14, 0x01, 0x12,
+					   0x02, 0x10, 0x00, 0x00,
+					   0x00, 0x05, 0x00, 0x00,
+					   0x00, 0x00, 0x05, 0x00,
+					   0x00, 0x00, 0x00, 0x00};
+
+/* Counter request packets */
+static const u8 START_COUNTER_REQUEST[]	= {0x00, 0x01, 0x00, 0x0C,
+					   0x00, 0x08, 0x01, 0x09,
+					   0x02, 0x20, 0x00, 0x00};
+
+static const u8 STOP_COUNTER_REQUEST[]	= {0x00, 0x01, 0x00, 0x0C,
+					   0x00, 0x08, 0x01, 0x0C,
+					   0x02, 0x20, 0x00, 0x00};
+
+static const u8 READ_COUNTER_REQUEST[]	= {0x00, 0x01, 0x00, 0x0C,
+					   0x00, 0x08, 0x01, 0x0E,
+					   0x02, 0x20, 0x00, 0x00};
+
+static const u8 WRITE_COUNTER_REQUEST[]	= {0x00, 0x01, 0x00, 0x10,
+					   0x00, 0x0C, 0x01, 0x0F,
+					   0x02, 0x20, 0x00, 0x00,
+					   0x00, 0x00, 0x00, 0x00};
+
+/* Response packets */
+static const u8 GENERIC_RESPONSE[]	= {0x00, 0x01, 0x00, 0x0C,
+					   0x00, 0x08, 0x01, 0x00,
+					   0x00, 0x00, 0x00, 0x02};
+
+static const u8 READ_PORT_RESPONSE[]	= {0x00, 0x01, 0x00, 0x10,
+					   0x00, 0x0C, 0x01, 0x00,
+					   0x00, 0x00, 0x00, 0x02,
+					   0x00, 0x03, 0x00, 0x00};
+
+static const u8 READ_COUNTER_RESPONSE[]	= {0x00, 0x01, 0x00, 0x10,
+					   0x00, 0x0C, 0x01, 0x00,
+					   0x00, 0x00, 0x00, 0x02,
+					   0x00, 0x00, 0x00, 0x00};
+
+enum commands {
+	READ_PORT,
+	WRITE_PORT,
+	SET_PORT_DIR,
+	START_COUNTER,
+	STOP_COUNTER,
+	READ_COUNTER,
+	WRITE_COUNTER
+};
+
+struct ni6501_private {
+	struct usb_endpoint_descriptor *ep_rx;
+	struct usb_endpoint_descriptor *ep_tx;
+	struct semaphore sem;
+	u8 *usb_rx_buf;
+	u8 *usb_tx_buf;
+};
+
+static int ni6501_port_command(struct comedi_device *dev, int command,
+			       const u8 *port, u8 *bitmap)
+{
+	struct usb_device *usb = comedi_to_usb_dev(dev);
+	struct ni6501_private *devpriv = dev->private;
+	int request_size, response_size;
+	u8 *tx = devpriv->usb_tx_buf;
+	int ret;
+
+	if (command != SET_PORT_DIR && !bitmap)
+		return -EINVAL;
+
+	down(&devpriv->sem);
+
+	switch (command) {
+	case READ_PORT:
+		request_size = sizeof(READ_PORT_REQUEST);
+		response_size = sizeof(READ_PORT_RESPONSE);
+		memcpy(tx, READ_PORT_REQUEST, request_size);
+		tx[14] = port[0];
+		break;
+	case WRITE_PORT:
+		request_size = sizeof(WRITE_PORT_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+		memcpy(tx, WRITE_PORT_REQUEST, request_size);
+		tx[14] = port[0];
+		tx[17] = bitmap[0];
+		break;
+	case SET_PORT_DIR:
+		request_size = sizeof(SET_PORT_DIR_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+		memcpy(tx, SET_PORT_DIR_REQUEST, request_size);
+		tx[14] = port[0];
+		tx[15] = port[1];
+		tx[16] = port[2];
+		break;
+	default:
+		ret = -EINVAL;
+		goto end;
+	}
+
+	ret = usb_bulk_msg(usb,
+			   usb_sndbulkpipe(usb,
+					   devpriv->ep_tx->bEndpointAddress),
+			   devpriv->usb_tx_buf,
+			   request_size,
+			   NULL,
+			   NI6501_TIMEOUT);
+	if (ret)
+		goto end;
+
+	ret = usb_bulk_msg(usb,
+			   usb_rcvbulkpipe(usb,
+					   devpriv->ep_rx->bEndpointAddress),
+			   devpriv->usb_rx_buf,
+			   response_size,
+			   NULL,
+			   NI6501_TIMEOUT);
+	if (ret)
+		goto end;
+
+	/* Check if results are valid */
+
+	if (command == READ_PORT) {
+		bitmap[0] = devpriv->usb_rx_buf[14];
+		/* mask bitmap for comparing */
+		devpriv->usb_rx_buf[14] = 0x00;
+
+		if (memcmp(devpriv->usb_rx_buf, READ_PORT_RESPONSE,
+			   sizeof(READ_PORT_RESPONSE))) {
+			ret = -EINVAL;
+		}
+	} else if (memcmp(devpriv->usb_rx_buf, GENERIC_RESPONSE,
+			  sizeof(GENERIC_RESPONSE))) {
+		ret = -EINVAL;
+	}
+end:
+	up(&devpriv->sem);
+
+	return ret;
+}
+
+static int ni6501_counter_command(struct comedi_device *dev, int command,
+				  u32 *val)
+{
+	struct usb_device *usb = comedi_to_usb_dev(dev);
+	struct ni6501_private *devpriv = dev->private;
+	int request_size, response_size;
+	u8 *tx = devpriv->usb_tx_buf;
+	int ret;
+
+	if ((command == READ_COUNTER || command ==  WRITE_COUNTER) && !val)
+		return -EINVAL;
+
+	down(&devpriv->sem);
+
+	switch (command) {
+	case START_COUNTER:
+		request_size = sizeof(START_COUNTER_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+		memcpy(tx, START_COUNTER_REQUEST, request_size);
+		break;
+	case STOP_COUNTER:
+		request_size = sizeof(STOP_COUNTER_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+		memcpy(tx, STOP_COUNTER_REQUEST, request_size);
+		break;
+	case READ_COUNTER:
+		request_size = sizeof(READ_COUNTER_REQUEST);
+		response_size = sizeof(READ_COUNTER_RESPONSE);
+		memcpy(tx, READ_COUNTER_REQUEST, request_size);
+		break;
+	case WRITE_COUNTER:
+		request_size = sizeof(WRITE_COUNTER_REQUEST);
+		response_size = sizeof(GENERIC_RESPONSE);
+		memcpy(tx, WRITE_COUNTER_REQUEST, request_size);
+		/* Setup tx packet: bytes 12,13,14,15 hold the */
+		/* u32 counter value (Big Endian)	       */
+		*((__be32 *)&tx[12]) = cpu_to_be32(*val);
+		break;
+	default:
+		ret = -EINVAL;
+		goto end;
+	}
+
+	ret = usb_bulk_msg(usb,
+			   usb_sndbulkpipe(usb,
+					   devpriv->ep_tx->bEndpointAddress),
+			   devpriv->usb_tx_buf,
+			   request_size,
+			   NULL,
+			   NI6501_TIMEOUT);
+	if (ret)
+		goto end;
+
+	ret = usb_bulk_msg(usb,
+			   usb_rcvbulkpipe(usb,
+					   devpriv->ep_rx->bEndpointAddress),
+			   devpriv->usb_rx_buf,
+			   response_size,
+			   NULL,
+			   NI6501_TIMEOUT);
+	if (ret)
+		goto end;
+
+	/* Check if results are valid */
+
+	if (command == READ_COUNTER) {
+		int i;
+
+		/* Read counter value: bytes 12,13,14,15 of rx packet */
+		/* hold the u32 counter value (Big Endian)	      */
+		*val = be32_to_cpu(*((__be32 *)&devpriv->usb_rx_buf[12]));
+
+		/* mask counter value for comparing */
+		for (i = 12; i < sizeof(READ_COUNTER_RESPONSE); ++i)
+			devpriv->usb_rx_buf[i] = 0x00;
+
+		if (memcmp(devpriv->usb_rx_buf, READ_COUNTER_RESPONSE,
+			   sizeof(READ_COUNTER_RESPONSE))) {
+			ret = -EINVAL;
+		}
+	} else if (memcmp(devpriv->usb_rx_buf, GENERIC_RESPONSE,
+			  sizeof(GENERIC_RESPONSE))) {
+		ret = -EINVAL;
+	}
+end:
+	up(&devpriv->sem);
+
+	return ret;
+}
+
+static int ni6501_dio_insn_config(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	int ret;
+	u8 port[3];
+
+	ret = comedi_dio_insn_config(dev, s, insn, data, 0);
+	if (ret)
+		return ret;
+
+	port[0] = (s->io_bits) & 0xff;
+	port[1] = (s->io_bits >> 8) & 0xff;
+	port[2] = (s->io_bits >> 16) & 0xff;
+
+	ret = ni6501_port_command(dev, SET_PORT_DIR, port, NULL);
+	if (ret)
+		return ret;
+
+	return insn->n;
+}
+
+static int ni6501_dio_insn_bits(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	unsigned int mask;
+	int ret;
+	u8 port;
+	u8 bitmap;
+
+	mask = comedi_dio_update_state(s, data);
+
+	for (port = 0; port < 3; port++) {
+		if (mask & (0xFF << port * 8)) {
+			bitmap = (s->state >> port * 8) & 0xFF;
+			ret = ni6501_port_command(dev, WRITE_PORT,
+						  &port, &bitmap);
+			if (ret)
+				return ret;
+		}
+	}
+
+	data[1] = 0;
+
+	for (port = 0; port < 3; port++) {
+		ret = ni6501_port_command(dev, READ_PORT, &port, &bitmap);
+		if (ret)
+			return ret;
+		data[1] |= bitmap << port * 8;
+	}
+
+	return insn->n;
+}
+
+static int ni6501_cnt_insn_config(struct comedi_device *dev,
+				  struct comedi_subdevice *s,
+				  struct comedi_insn *insn,
+				  unsigned int *data)
+{
+	int ret;
+	u32 val = 0;
+
+	switch (data[0]) {
+	case INSN_CONFIG_ARM:
+		ret = ni6501_counter_command(dev, START_COUNTER, NULL);
+		break;
+	case INSN_CONFIG_DISARM:
+		ret = ni6501_counter_command(dev, STOP_COUNTER, NULL);
+		break;
+	case INSN_CONFIG_RESET:
+		ret = ni6501_counter_command(dev, STOP_COUNTER, NULL);
+		if (ret)
+			break;
+		ret = ni6501_counter_command(dev, WRITE_COUNTER, &val);
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	return ret ? ret : insn->n;
+}
+
+static int ni6501_cnt_insn_read(struct comedi_device *dev,
+				struct comedi_subdevice *s,
+				struct comedi_insn *insn,
+				unsigned int *data)
+{
+	int ret;
+	u32 val;
+	unsigned int i;
+
+	for (i = 0; i < insn->n; i++) {
+		ret = ni6501_counter_command(dev, READ_COUNTER,	&val);
+		if (ret)
+			return ret;
+		data[i] = val;
+	}
+
+	return insn->n;
+}
+
+static int ni6501_cnt_insn_write(struct comedi_device *dev,
+				 struct comedi_subdevice *s,
+				 struct comedi_insn *insn,
+				 unsigned int *data)
+{
+	int ret;
+
+	if (insn->n) {
+		u32 val = data[insn->n - 1];
+
+		ret = ni6501_counter_command(dev, WRITE_COUNTER, &val);
+		if (ret)
+			return ret;
+	}
+
+	return insn->n;
+}
+
+static int ni6501_alloc_usb_buffers(struct comedi_device *dev)
+{
+	struct ni6501_private *devpriv = dev->private;
+	size_t size;
+
+	size = le16_to_cpu(devpriv->ep_rx->wMaxPacketSize);
+	devpriv->usb_rx_buf = kzalloc(size, GFP_KERNEL);
+	if (!devpriv->usb_rx_buf)
+		return -ENOMEM;
+
+	size = le16_to_cpu(devpriv->ep_tx->wMaxPacketSize);
+	devpriv->usb_tx_buf = kzalloc(size, GFP_KERNEL);
+	if (!devpriv->usb_tx_buf) {
+		kfree(devpriv->usb_rx_buf);
+		return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static int ni6501_find_endpoints(struct comedi_device *dev)
+{
+	struct usb_interface *intf = comedi_to_usb_interface(dev);
+	struct ni6501_private *devpriv = dev->private;
+	struct usb_host_interface *iface_desc = intf->cur_altsetting;
+	struct usb_endpoint_descriptor *ep_desc;
+	int i;
+
+	if (iface_desc->desc.bNumEndpoints != 2) {
+		dev_err(dev->class_dev, "Wrong number of endpoints\n");
+		return -ENODEV;
+	}
+
+	for (i = 0; i < iface_desc->desc.bNumEndpoints; i++) {
+		ep_desc = &iface_desc->endpoint[i].desc;
+
+		if (usb_endpoint_is_bulk_in(ep_desc)) {
+			if (!devpriv->ep_rx)
+				devpriv->ep_rx = ep_desc;
+			continue;
+		}
+
+		if (usb_endpoint_is_bulk_out(ep_desc)) {
+			if (!devpriv->ep_tx)
+				devpriv->ep_tx = ep_desc;
+			continue;
+		}
+	}
+
+	if (!devpriv->ep_rx || !devpriv->ep_tx)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int ni6501_auto_attach(struct comedi_device *dev,
+			      unsigned long context)
+{
+	struct usb_interface *intf = comedi_to_usb_interface(dev);
+	struct ni6501_private *devpriv;
+	struct comedi_subdevice *s;
+	int ret;
+
+	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
+	if (!devpriv)
+		return -ENOMEM;
+
+	ret = ni6501_find_endpoints(dev);
+	if (ret)
+		return ret;
+
+	ret = ni6501_alloc_usb_buffers(dev);
+	if (ret)
+		return ret;
+
+	sema_init(&devpriv->sem, 1);
+	usb_set_intfdata(intf, devpriv);
+
+	ret = comedi_alloc_subdevices(dev, 2);
+	if (ret)
+		return ret;
+
+	/* Digital Input/Output subdevice */
+	s = &dev->subdevices[0];
+	s->type		= COMEDI_SUBD_DIO;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITABLE;
+	s->n_chan	= 24;
+	s->maxdata	= 1;
+	s->range_table	= &range_digital;
+	s->insn_bits	= ni6501_dio_insn_bits;
+	s->insn_config	= ni6501_dio_insn_config;
+
+	/* Counter subdevice */
+	s = &dev->subdevices[1];
+	s->type		= COMEDI_SUBD_COUNTER;
+	s->subdev_flags	= SDF_READABLE | SDF_WRITEABLE | SDF_LSAMPL;
+	s->n_chan	= 1;
+	s->maxdata	= 0xffffffff;
+	s->insn_read	= ni6501_cnt_insn_read;
+	s->insn_write	= ni6501_cnt_insn_write;
+	s->insn_config	= ni6501_cnt_insn_config;
+
+	return 0;
+}
+
+static void ni6501_detach(struct comedi_device *dev)
+{
+	struct usb_interface *intf = comedi_to_usb_interface(dev);
+	struct ni6501_private *devpriv = dev->private;
+
+	if (!devpriv)
+		return;
+
+	down(&devpriv->sem);
+
+	usb_set_intfdata(intf, NULL);
+
+	kfree(devpriv->usb_rx_buf);
+	kfree(devpriv->usb_tx_buf);
+
+	up(&devpriv->sem);
+}
+
+static struct comedi_driver ni6501_driver = {
+	.module		= THIS_MODULE,
+	.driver_name	= "ni6501",
+	.auto_attach	= ni6501_auto_attach,
+	.detach		= ni6501_detach,
+};
+
+static int ni6501_usb_probe(struct usb_interface *intf,
+			    const struct usb_device_id *id)
+{
+	return comedi_usb_auto_config(intf, &ni6501_driver, id->driver_info);
+}
+
+static const struct usb_device_id ni6501_usb_table[] = {
+	{ USB_DEVICE(0x3923, 0x718a) },
+	{ }
+};
+MODULE_DEVICE_TABLE(usb, ni6501_usb_table);
+
+static struct usb_driver ni6501_usb_driver = {
+	.name		= "ni6501",
+	.id_table	= ni6501_usb_table,
+	.probe		= ni6501_usb_probe,
+	.disconnect	= comedi_usb_auto_unconfig,
+};
+module_comedi_usb_driver(ni6501_driver, ni6501_usb_driver);
+
+MODULE_AUTHOR("Luca Ellero");
+MODULE_DESCRIPTION("Comedi driver for National Instruments USB-6501");
+MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/pcl711.c b/drivers/staging/comedi/drivers/pcl711.c
index 40f9136f0bb6..47f4887108a7 100644
--- a/drivers/staging/comedi/drivers/pcl711.c
+++ b/drivers/staging/comedi/drivers/pcl711.c
@@ -157,7 +157,6 @@ static const struct pcl711_board boardtypes[] = {
 
 struct pcl711_private {
 	unsigned int ntrig;
-	unsigned int ao_readback[2];
 	unsigned int divisor1;
 	unsigned int divisor2;
 };
@@ -335,7 +334,9 @@ static int pcl711_ai_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_NONE)
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
 	if (err)
@@ -377,15 +378,8 @@ static int pcl711_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 
 	pcl711_set_changain(dev, s, cmd->chanlist[0]);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		if (cmd->stop_arg == 0) {
-			/* an empty acquisition */
-			s->async->events |= COMEDI_CB_EOA;
-			comedi_event(dev, s);
-			return 0;
-		}
+	if (cmd->stop_src == TRIG_COUNT)
 		devpriv->ntrig = cmd->stop_arg;
-	}
 
 	if (cmd->scan_begin_src == TRIG_TIMER) {
 		pcl711_ai_load_counters(dev);
@@ -410,31 +404,15 @@ static int pcl711_ao_insn_write(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	struct pcl711_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val = devpriv->ao_readback[chan];
+	unsigned int val = s->readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
 		val = data[i];
 		pcl711_ao_write(dev, chan, val);
 	}
-	devpriv->ao_readback[chan] = val;
-
-	return insn->n;
-}
-
-static int pcl711_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct pcl711_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+	s->readback[chan] = val;
 
 	return insn->n;
 }
@@ -476,7 +454,7 @@ static int pcl711_do_insn_bits(struct comedi_device *dev,
 
 static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct pcl711_board *board = comedi_board(dev);
+	const struct pcl711_board *board = dev->board_ptr;
 	struct pcl711_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
@@ -527,7 +505,11 @@ static int pcl711_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->maxdata	= 0xfff;
 	s->range_table	= &range_bipolar5;
 	s->insn_write	= pcl711_ao_insn_write;
-	s->insn_read	= pcl711_ao_insn_read;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	/* Digital Input subdevice */
 	s = &dev->subdevices[2];
diff --git a/drivers/staging/comedi/drivers/pcl724.c b/drivers/staging/comedi/drivers/pcl724.c
index c7f8eb1cf8de..fcc440855e66 100644
--- a/drivers/staging/comedi/drivers/pcl724.c
+++ b/drivers/staging/comedi/drivers/pcl724.c
@@ -32,8 +32,6 @@
 
 #include "8255.h"
 
-#define SIZE_8255	4
-
 struct pcl724_board {
 	const char *name;
 	unsigned int io_range;
@@ -81,10 +79,11 @@ static const struct pcl724_board boardtypes[] = {
 	},
 };
 
-static int pcl724_8255mapped_io(int dir, int port, int data,
+static int pcl724_8255mapped_io(struct comedi_device *dev,
+				int dir, int port, int data,
 				unsigned long iobase)
 {
-	int movport = SIZE_8255 * (iobase >> 12);
+	int movport = I8255_SIZE * (iobase >> 12);
 
 	iobase &= 0x0fff;
 
@@ -99,7 +98,7 @@ static int pcl724_8255mapped_io(int dir, int port, int data,
 static int pcl724_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it)
 {
-	const struct pcl724_board *board = comedi_board(dev);
+	const struct pcl724_board *board = dev->board_ptr;
 	struct comedi_subdevice *s;
 	unsigned long iobase;
 	unsigned int iorange;
@@ -132,8 +131,7 @@ static int pcl724_attach(struct comedi_device *dev,
 			ret = subdev_8255_init(dev, s, pcl724_8255mapped_io,
 					       iobase);
 		} else {
-			iobase = dev->iobase + (i * SIZE_8255);
-			ret = subdev_8255_init(dev, s, NULL, iobase);
+			ret = subdev_8255_init(dev, s, NULL, i * I8255_SIZE);
 		}
 		if (ret)
 			return ret;
diff --git a/drivers/staging/comedi/drivers/pcl726.c b/drivers/staging/comedi/drivers/pcl726.c
index 74f6489bd124..dc179bd02dfd 100644
--- a/drivers/staging/comedi/drivers/pcl726.c
+++ b/drivers/staging/comedi/drivers/pcl726.c
@@ -156,7 +156,6 @@ static const struct pcl726_board pcl726_boards[] = {
 
 struct pcl726_private {
 	const struct comedi_lrange *rangelist[12];
-	unsigned int ao_readback[12];
 	unsigned int cmd_running:1;
 };
 
@@ -189,9 +188,6 @@ static int pcl726_intr_cmdtest(struct comedi_device *dev,
 	/* Step 2a : make sure trigger sources are unique */
 	/* Step 2b : and mutually compatible */
 
-	if (err)
-		return 2;
-
 	/* Step 3: check if arguments are trivially valid */
 
 	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
@@ -203,10 +199,9 @@ static int pcl726_intr_cmdtest(struct comedi_device *dev,
 	if (err)
 		return 3;
 
-	/* step 4: ignored */
+	/* Step 4: fix up any arguments */
 
-	if (err)
-		return 4;
+	/* Step 5: check channel list if it exists */
 
 	return 0;
 }
@@ -253,15 +248,14 @@ static int pcl726_ao_insn_write(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	struct pcl726_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int range = CR_RANGE(insn->chanspec);
-	unsigned int val;
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		val = data[i];
-		devpriv->ao_readback[chan] = val;
+		unsigned int val = data[i];
+
+		s->readback[chan] = val;
 
 		/* bipolar data to the DAC is two's complement */
 		if (comedi_chan_range_is_bipolar(s, chan, range))
@@ -275,27 +269,12 @@ static int pcl726_ao_insn_write(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int pcl726_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct pcl726_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int pcl726_di_insn_bits(struct comedi_device *dev,
 			       struct comedi_subdevice *s,
 			       struct comedi_insn *insn,
 			       unsigned int *data)
 {
-	const struct pcl726_board *board = comedi_board(dev);
+	const struct pcl726_board *board = dev->board_ptr;
 	unsigned int val;
 
 	if (board->is_pcl727) {
@@ -316,7 +295,7 @@ static int pcl726_do_insn_bits(struct comedi_device *dev,
 			       struct comedi_insn *insn,
 			       unsigned int *data)
 {
-	const struct pcl726_board *board = comedi_board(dev);
+	const struct pcl726_board *board = dev->board_ptr;
 	unsigned long io = dev->iobase;
 	unsigned int mask;
 
@@ -343,7 +322,7 @@ static int pcl726_do_insn_bits(struct comedi_device *dev,
 static int pcl726_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it)
 {
-	const struct pcl726_board *board = comedi_board(dev);
+	const struct pcl726_board *board = dev->board_ptr;
 	struct pcl726_private *devpriv;
 	struct comedi_subdevice *s;
 	int subdev;
@@ -398,7 +377,11 @@ static int pcl726_attach(struct comedi_device *dev,
 	s->maxdata	= 0x0fff;
 	s->range_table_list = devpriv->rangelist;
 	s->insn_write	= pcl726_ao_insn_write;
-	s->insn_read	= pcl726_ao_insn_read;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	if (board->have_dio) {
 		/* Digital Input subdevice */
diff --git a/drivers/staging/comedi/drivers/pcl730.c b/drivers/staging/comedi/drivers/pcl730.c
index bdce24c42940..a6c5770b2808 100644
--- a/drivers/staging/comedi/drivers/pcl730.c
+++ b/drivers/staging/comedi/drivers/pcl730.c
@@ -270,7 +270,7 @@ static int pcl730_di_insn_bits(struct comedi_device *dev,
 static int pcl730_attach(struct comedi_device *dev,
 			 struct comedi_devconfig *it)
 {
-	const struct pcl730_board *board = comedi_board(dev);
+	const struct pcl730_board *board = dev->board_ptr;
 	struct comedi_subdevice *s;
 	int subdev;
 	int ret;
diff --git a/drivers/staging/comedi/drivers/pcl812.c b/drivers/staging/comedi/drivers/pcl812.c
index 803e7790538c..fd5ea6e01619 100644
--- a/drivers/staging/comedi/drivers/pcl812.c
+++ b/drivers/staging/comedi/drivers/pcl812.c
@@ -522,7 +522,6 @@ struct pcl812_private {
 	unsigned int dma_runs_to_end;	/*  how many times we must switch DMA buffers */
 	unsigned int last_dma_run;	/*  how many bytes to transfer on last DMA buffer */
 	unsigned int max_812_ai_mode0_rangewait;	/*  setling time for gain */
-	unsigned int ao_readback[2];	/*  data for AO readback */
 	unsigned int divisor1;
 	unsigned int divisor2;
 	unsigned int use_diff:1;
@@ -714,7 +713,7 @@ static int pcl812_ai_eoc(struct comedi_device *dev,
 static int pcl812_ai_cmdtest(struct comedi_device *dev,
 			     struct comedi_subdevice *s, struct comedi_cmd *cmd)
 {
-	const struct pcl812_board *board = comedi_board(dev);
+	const struct pcl812_board *board = dev->board_ptr;
 	struct pcl812_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int flags;
@@ -813,7 +812,7 @@ static int pcl812_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	s->async->cur_chan = 0;
 
 	/*  don't we want wake up every scan? */
-	if (cmd->flags & TRIG_WAKE_EOS) {
+	if (cmd->flags & CMDF_WAKE_EOS) {
 		devpriv->ai_eos = 1;
 
 		/*  DMA is useless for this situation */
@@ -1039,32 +1038,16 @@ static int pcl812_ao_insn_write(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	struct pcl812_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		outb((data[i] & 0xff),
-		     dev->iobase + PCL812_AO_LSB_REG(chan));
-		outb((data[i] >> 8) & 0x0f,
-		     dev->iobase + PCL812_AO_MSB_REG(chan));
-		devpriv->ao_readback[chan] = data[i];
+		val = data[i];
+		outb(val & 0xff, dev->iobase + PCL812_AO_LSB_REG(chan));
+		outb((val >> 8) & 0x0f, dev->iobase + PCL812_AO_MSB_REG(chan));
 	}
-
-	return insn->n;
-}
-
-static int pcl812_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct pcl812_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+	s->readback[chan] = val;
 
 	return insn->n;
 }
@@ -1097,7 +1080,7 @@ static int pcl812_do_insn_bits(struct comedi_device *dev,
 
 static void pcl812_reset(struct comedi_device *dev)
 {
-	const struct pcl812_board *board = comedi_board(dev);
+	const struct pcl812_board *board = dev->board_ptr;
 	struct pcl812_private *devpriv = dev->private;
 	unsigned int chan;
 
@@ -1134,7 +1117,7 @@ static void pcl812_set_ai_range_table(struct comedi_device *dev,
 				      struct comedi_subdevice *s,
 				      struct comedi_devconfig *it)
 {
-	const struct pcl812_board *board = comedi_board(dev);
+	const struct pcl812_board *board = dev->board_ptr;
 	struct pcl812_private *devpriv = dev->private;
 
 	/* default to the range table from the boardinfo */
@@ -1222,7 +1205,7 @@ static void pcl812_set_ai_range_table(struct comedi_device *dev,
 
 static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct pcl812_board *board = comedi_board(dev);
+	const struct pcl812_board *board = dev->board_ptr;
 	struct pcl812_private *devpriv;
 	struct comedi_subdevice *s;
 	int n_subdevices;
@@ -1336,8 +1319,6 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->n_chan	= board->n_aochan;
 		s->maxdata	= 0xfff;
 		s->range_table	= &range_unipolar5;
-		s->insn_read	= pcl812_ao_insn_read;
-		s->insn_write	= pcl812_ao_insn_write;
 		switch (board->board_type) {
 		case boardA821:
 			if (it->options[3] == 1)
@@ -1353,6 +1334,13 @@ static int pcl812_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 				s->range_table = &range_unknown;
 			break;
 		}
+		s->insn_write	= pcl812_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
+
 		subdev++;
 	}
 
diff --git a/drivers/staging/comedi/drivers/pcl816.c b/drivers/staging/comedi/drivers/pcl816.c
index 54732c5cab97..aa6487132017 100644
--- a/drivers/staging/comedi/drivers/pcl816.c
+++ b/drivers/staging/comedi/drivers/pcl816.c
@@ -671,7 +671,7 @@ static void pcl816_reset(struct comedi_device *dev)
 
 static int pcl816_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct pcl816_board *board = comedi_board(dev);
+	const struct pcl816_board *board = dev->board_ptr;
 	struct pcl816_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
diff --git a/drivers/staging/comedi/drivers/pcl818.c b/drivers/staging/comedi/drivers/pcl818.c
index 000dbf841e45..ac19e83ce62a 100644
--- a/drivers/staging/comedi/drivers/pcl818.c
+++ b/drivers/staging/comedi/drivers/pcl818.c
@@ -319,7 +319,6 @@ struct pcl818_private {
 	unsigned int act_chanlist_len;	/*  how long is actual MUX list */
 	unsigned int act_chanlist_pos;	/*  actual position in MUX list */
 	unsigned int ai_data_len;	/*  len of data buffer */
-	unsigned int ao_readback[2];
 	unsigned int divisor1;
 	unsigned int divisor2;
 	unsigned int usefifo:1;
@@ -739,7 +738,7 @@ static int check_single_ended(unsigned int port)
 static int ai_cmdtest(struct comedi_device *dev, struct comedi_subdevice *s,
 		      struct comedi_cmd *cmd)
 {
-	const struct pcl818_board *board = comedi_board(dev);
+	const struct pcl818_board *board = dev->board_ptr;
 	struct pcl818_private *devpriv = dev->private;
 	int err = 0;
 	unsigned int arg;
@@ -937,32 +936,18 @@ static int pcl818_ao_insn_write(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	struct pcl818_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
+	unsigned int val = s->readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		devpriv->ao_readback[chan] = data[i];
-		outb((data[i] & 0x000f) << 4,
+		val = data[i];
+		outb((val & 0x000f) << 4,
 		     dev->iobase + PCL818_AO_LSB_REG(chan));
-		outb((data[i] & 0x0ff0) >> 4,
+		outb((val & 0x0ff0) >> 4,
 		     dev->iobase + PCL818_AO_MSB_REG(chan));
 	}
-
-	return insn->n;
-}
-
-static int pcl818_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct pcl818_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+	s->readback[chan] = val;
 
 	return insn->n;
 }
@@ -995,7 +980,7 @@ static int pcl818_do_insn_bits(struct comedi_device *dev,
 
 static void pcl818_reset(struct comedi_device *dev)
 {
-	const struct pcl818_board *board = comedi_board(dev);
+	const struct pcl818_board *board = dev->board_ptr;
 	unsigned long timer_base = dev->iobase + PCL818_TIMER_BASE;
 	unsigned int chan;
 
@@ -1033,7 +1018,7 @@ static void pcl818_set_ai_range_table(struct comedi_device *dev,
 				      struct comedi_subdevice *s,
 				      struct comedi_devconfig *it)
 {
-	const struct pcl818_board *board = comedi_board(dev);
+	const struct pcl818_board *board = dev->board_ptr;
 
 	/* default to the range table from the boardinfo */
 	s->range_table = board->ai_range_type;
@@ -1082,7 +1067,7 @@ static void pcl818_set_ai_range_table(struct comedi_device *dev,
 
 static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct pcl818_board *board = comedi_board(dev);
+	const struct pcl818_board *board = dev->board_ptr;
 	struct pcl818_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
@@ -1172,8 +1157,6 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->n_chan	= board->n_aochan;
 		s->maxdata	= 0x0fff;
 		s->range_table	= &range_unipolar5;
-		s->insn_read	= pcl818_ao_insn_read;
-		s->insn_write	= pcl818_ao_insn_write;
 		if (board->is_818) {
 			if ((it->options[4] == 1) || (it->options[4] == 10))
 				s->range_table = &range_unipolar10;
@@ -1185,6 +1168,12 @@ static int pcl818_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			if (it->options[5] == 2)
 				s->range_table = &range_unknown;
 		}
+		s->insn_write	= pcl818_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
diff --git a/drivers/staging/comedi/drivers/pcm3724.c b/drivers/staging/comedi/drivers/pcm3724.c
index 6e0d78f6095b..6176dfa24801 100644
--- a/drivers/staging/comedi/drivers/pcm3724.c
+++ b/drivers/staging/comedi/drivers/pcm3724.c
@@ -33,8 +33,6 @@ Copy/pasted/hacked from pcm724.c
 
 #include "8255.h"
 
-#define SIZE_8255	4
-
 #define BUF_C0 0x1
 #define BUF_B0 0x2
 #define BUF_A0 0x4
@@ -49,16 +47,6 @@ Copy/pasted/hacked from pcm724.c
 #define GATE_B1	0x10
 #define GATE_C1 0x8
 
-/* from 8255.c */
-#define CR_CW		0x80
-#define _8255_CR 3
-#define CR_B_IO		0x02
-#define CR_B_MODE	0x04
-#define CR_C_IO	        0x09
-#define CR_A_IO		0x10
-#define CR_A_MODE(a)	((a)<<5)
-#define CR_CW		0x80
-
 /* used to track configured dios */
 struct priv_pcm3724 {
 	int dio_1;
@@ -98,26 +86,26 @@ static void do_3724_config(struct comedi_device *dev,
 	int buffer_config;
 	unsigned long port_8255_cfg;
 
-	config = CR_CW;
+	config = I8255_CTRL_CW;
 	buffer_config = 0;
 
 	/* 1 in io_bits indicates output, 1 in config indicates input */
 	if (!(s->io_bits & 0x0000ff))
-		config |= CR_A_IO;
+		config |= I8255_CTRL_A_IO;
 
 	if (!(s->io_bits & 0x00ff00))
-		config |= CR_B_IO;
+		config |= I8255_CTRL_B_IO;
 
 	if (!(s->io_bits & 0xff0000))
-		config |= CR_C_IO;
+		config |= I8255_CTRL_C_HI_IO | I8255_CTRL_C_LO_IO;
 
 	buffer_config = compute_buffer(0, 0, s_dio1);
 	buffer_config = compute_buffer(buffer_config, 1, s_dio2);
 
 	if (s == s_dio1)
-		port_8255_cfg = dev->iobase + _8255_CR;
+		port_8255_cfg = dev->iobase + I8255_CTRL_REG;
 	else
-		port_8255_cfg = dev->iobase + SIZE_8255 + _8255_CR;
+		port_8255_cfg = dev->iobase + I8255_SIZE + I8255_CTRL_REG;
 
 	outb(buffer_config, dev->iobase + 8);	/* update buffer register */
 
@@ -211,8 +199,7 @@ static int pcm3724_attach(struct comedi_device *dev,
 
 	for (i = 0; i < dev->n_subdevices; i++) {
 		s = &dev->subdevices[i];
-		ret = subdev_8255_init(dev, s, NULL,
-				       dev->iobase + SIZE_8255 * i);
+		ret = subdev_8255_init(dev, s, NULL, i * I8255_SIZE);
 		if (ret)
 			return ret;
 		s->insn_config = subdev_3724_insn_config;
diff --git a/drivers/staging/comedi/drivers/pcmad.c b/drivers/staging/comedi/drivers/pcmad.c
index 87c61d9b11da..e3ac8ac6190e 100644
--- a/drivers/staging/comedi/drivers/pcmad.c
+++ b/drivers/staging/comedi/drivers/pcmad.c
@@ -112,7 +112,7 @@ static int pcmad_ai_insn_read(struct comedi_device *dev,
 
 static int pcmad_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct pcmad_board_struct *board = comedi_board(dev);
+	const struct pcmad_board_struct *board = dev->board_ptr;
 	struct comedi_subdevice *s;
 	int ret;
 
diff --git a/drivers/staging/comedi/drivers/pcmda12.c b/drivers/staging/comedi/drivers/pcmda12.c
index 1c7a135c91d6..59108c06cedc 100644
--- a/drivers/staging/comedi/drivers/pcmda12.c
+++ b/drivers/staging/comedi/drivers/pcmda12.c
@@ -61,7 +61,6 @@ static const struct comedi_lrange pcmda12_ranges = {
 };
 
 struct pcmda12_private {
-	unsigned int ao_readback[8];
 	int simultaneous_xfer_mode;
 };
 
@@ -72,7 +71,7 @@ static int pcmda12_ao_insn_write(struct comedi_device *dev,
 {
 	struct pcmda12_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val = devpriv->ao_readback[chan];
+	unsigned int val = s->readback[chan];
 	unsigned long ioreg = dev->iobase + (chan * 2);
 	int i;
 
@@ -88,7 +87,7 @@ static int pcmda12_ao_insn_write(struct comedi_device *dev,
 		if (!devpriv->simultaneous_xfer_mode)
 			inb(ioreg);
 	}
-	devpriv->ao_readback[chan] = val;
+	s->readback[chan] = val;
 
 	return insn->n;
 }
@@ -99,8 +98,6 @@ static int pcmda12_ao_insn_read(struct comedi_device *dev,
 				unsigned int *data)
 {
 	struct pcmda12_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
 
 	/*
 	 * Initiate simultaneaous xfer mode by reading one of the
@@ -109,10 +106,7 @@ static int pcmda12_ao_insn_read(struct comedi_device *dev,
 	if (devpriv->simultaneous_xfer_mode)
 		inb(dev->iobase);
 
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
+	return comedi_readback_insn_read(dev, s, insn, data);
 }
 
 static void pcmda12_ao_reset(struct comedi_device *dev,
@@ -158,6 +152,10 @@ static int pcmda12_attach(struct comedi_device *dev,
 	s->insn_write	= pcmda12_ao_insn_write;
 	s->insn_read	= pcmda12_ao_insn_read;
 
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
 	pcmda12_ao_reset(dev, s);
 
 	return 0;
diff --git a/drivers/staging/comedi/drivers/pcmmio.c b/drivers/staging/comedi/drivers/pcmmio.c
index fed7e77e0305..fc40ee2b34e9 100644
--- a/drivers/staging/comedi/drivers/pcmmio.c
+++ b/drivers/staging/comedi/drivers/pcmmio.c
@@ -192,8 +192,6 @@ struct pcmmio_private {
 	unsigned int enabled_mask;
 	unsigned int stop_count;
 	unsigned int active:1;
-
-	unsigned int ao_readback[8];
 };
 
 static void pcmmio_dio_write(struct comedi_device *dev, unsigned int val,
@@ -408,8 +406,8 @@ static irqreturn_t interrupt_pcmmio(int irq, void *d)
 }
 
 /* devpriv->spinlock is already locked */
-static int pcmmio_start_intr(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
+static void pcmmio_start_intr(struct comedi_device *dev,
+			      struct comedi_subdevice *s)
 {
 	struct pcmmio_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
@@ -417,13 +415,6 @@ static int pcmmio_start_intr(struct comedi_device *dev,
 	unsigned int pol_bits = 0;
 	int i;
 
-	if (cmd->stop_src == TRIG_COUNT && devpriv->stop_count == 0) {
-		/* An empty acquisition! */
-		s->async->events |= COMEDI_CB_EOA;
-		devpriv->active = 0;
-		return 1;
-	}
-
 	devpriv->enabled_mask = 0;
 	devpriv->active = 1;
 	if (cmd->chanlist) {
@@ -443,8 +434,6 @@ static int pcmmio_start_intr(struct comedi_device *dev,
 	/* set polarity and enable interrupts */
 	pcmmio_dio_write(dev, pol_bits, PCMMIO_PAGE_POL, 0);
 	pcmmio_dio_write(dev, bits, PCMMIO_PAGE_ENAB, 0);
-
-	return 0;
 }
 
 static int pcmmio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -467,7 +456,6 @@ static int pcmmio_inttrig_start_intr(struct comedi_device *dev,
 	struct pcmmio_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
-	int event = 0;
 
 	if (trig_num != cmd->start_arg)
 		return -EINVAL;
@@ -475,12 +463,9 @@ static int pcmmio_inttrig_start_intr(struct comedi_device *dev,
 	spin_lock_irqsave(&devpriv->spinlock, flags);
 	s->async->inttrig = NULL;
 	if (devpriv->active)
-		event = pcmmio_start_intr(dev, s);
+		pcmmio_start_intr(dev, s);
 	spin_unlock_irqrestore(&devpriv->spinlock, flags);
 
-	if (event)
-		comedi_event(dev, s);
-
 	return 1;
 }
 
@@ -492,28 +477,20 @@ static int pcmmio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	struct pcmmio_private *devpriv = dev->private;
 	struct comedi_cmd *cmd = &s->async->cmd;
 	unsigned long flags;
-	int event = 0;
 
 	spin_lock_irqsave(&devpriv->spinlock, flags);
 	devpriv->active = 1;
 
-	/* Set up end of acquisition. */
-	if (cmd->stop_src == TRIG_COUNT)
-		devpriv->stop_count = cmd->stop_arg;
-	else	/* TRIG_NONE */
-		devpriv->stop_count = 0;
+	devpriv->stop_count = cmd->stop_arg;
 
 	/* Set up start of acquisition. */
 	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmmio_inttrig_start_intr;
 	else	/* TRIG_NOW */
-		event = pcmmio_start_intr(dev, s);
+		pcmmio_start_intr(dev, s);
 
 	spin_unlock_irqrestore(&devpriv->spinlock, flags);
 
-	if (event)
-		comedi_event(dev, s);
-
 	return 0;
 }
 
@@ -551,16 +528,10 @@ static int pcmmio_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		/* any count allowed */
-		break;
-	case TRIG_NONE:
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-		break;
-	default:
-		break;
-	}
 
 	if (err)
 		return 3;
@@ -655,21 +626,6 @@ static int pcmmio_ai_insn_read(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int pcmmio_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct pcmmio_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int pcmmio_ao_eoc(struct comedi_device *dev,
 			 struct comedi_subdevice *s,
 			 struct comedi_insn *insn,
@@ -688,11 +644,9 @@ static int pcmmio_ao_insn_write(struct comedi_device *dev,
 				struct comedi_insn *insn,
 				unsigned int *data)
 {
-	struct pcmmio_private *devpriv = dev->private;
 	unsigned long iobase = dev->iobase;
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	unsigned int range = CR_RANGE(insn->chanspec);
-	unsigned int val = devpriv->ao_readback[chan];
 	unsigned char cmd = 0;
 	int ret;
 	int i;
@@ -719,7 +673,7 @@ static int pcmmio_ao_insn_write(struct comedi_device *dev,
 		return ret;
 
 	for (i = 0; i < insn->n; i++) {
-		val = data[i];
+		unsigned int val = data[i];
 
 		/* write the data to the channel */
 		outb(val & 0xff, iobase + PCMMIO_AO_LSB_REG);
@@ -731,7 +685,7 @@ static int pcmmio_ao_insn_write(struct comedi_device *dev,
 		if (ret)
 			return ret;
 
-		devpriv->ao_readback[chan] = val;
+		s->readback[chan] = val;
 	}
 
 	return insn->n;
@@ -796,8 +750,12 @@ static int pcmmio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->n_chan	= 8;
 	s->maxdata	= 0xffff;
 	s->range_table	= &pcmmio_ao_ranges;
-	s->insn_read	= pcmmio_ao_insn_read;
 	s->insn_write	= pcmmio_ao_insn_write;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	/* initialize the resource enable register by clearing it */
 	outb(0, dev->iobase + PCMMIO_AO_RESOURCE_ENA_REG);
diff --git a/drivers/staging/comedi/drivers/pcmuio.c b/drivers/staging/comedi/drivers/pcmuio.c
index 1bca3fba0950..d4fe2ec25ecf 100644
--- a/drivers/staging/comedi/drivers/pcmuio.c
+++ b/drivers/staging/comedi/drivers/pcmuio.c
@@ -132,7 +132,6 @@ struct pcmuio_asic {
 	unsigned int enabled_mask;
 	unsigned int stop_count;
 	unsigned int active:1;
-	unsigned int continuous:1;
 };
 
 struct pcmuio_private {
@@ -279,7 +278,7 @@ static int pcmuio_dio_insn_config(struct comedi_device *dev,
 
 static void pcmuio_reset(struct comedi_device *dev)
 {
-	const struct pcmuio_board *board = comedi_board(dev);
+	const struct pcmuio_board *board = dev->board_ptr;
 	int asic;
 
 	for (asic = 0; asic < board->num_asics; ++asic) {
@@ -349,8 +348,7 @@ static void pcmuio_handle_intr_subdev(struct comedi_device *dev,
 	}
 
 	/* Check for end of acquisition. */
-	if (!chip->continuous) {
-		/* stop_src == TRIG_COUNT */
+	if (cmd->stop_src == TRIG_COUNT) {
 		if (chip->stop_count > 0) {
 			chip->stop_count--;
 			if (chip->stop_count == 0) {
@@ -405,8 +403,8 @@ static irqreturn_t pcmuio_interrupt(int irq, void *d)
 }
 
 /* chip->spinlock is already locked */
-static int pcmuio_start_intr(struct comedi_device *dev,
-			     struct comedi_subdevice *s)
+static void pcmuio_start_intr(struct comedi_device *dev,
+			      struct comedi_subdevice *s)
 {
 	struct pcmuio_private *devpriv = dev->private;
 	int asic = pcmuio_subdevice_to_asic(s);
@@ -416,13 +414,6 @@ static int pcmuio_start_intr(struct comedi_device *dev,
 	unsigned int pol_bits = 0;
 	int i;
 
-	if (!chip->continuous && chip->stop_count == 0) {
-		/* An empty acquisition! */
-		s->async->events |= COMEDI_CB_EOA;
-		chip->active = 0;
-		return 1;
-	}
-
 	chip->enabled_mask = 0;
 	chip->active = 1;
 	if (cmd->chanlist) {
@@ -442,8 +433,6 @@ static int pcmuio_start_intr(struct comedi_device *dev,
 	/* set pol and enab intrs for this subdev.. */
 	pcmuio_write(dev, pol_bits, asic, PCMUIO_PAGE_POL, 0);
 	pcmuio_write(dev, bits, asic, PCMUIO_PAGE_ENAB, 0);
-
-	return 0;
 }
 
 static int pcmuio_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
@@ -470,7 +459,6 @@ static int pcmuio_inttrig_start_intr(struct comedi_device *dev,
 	int asic = pcmuio_subdevice_to_asic(s);
 	struct pcmuio_asic *chip = &devpriv->asics[asic];
 	unsigned long flags;
-	int event = 0;
 
 	if (trig_num != cmd->start_arg)
 		return -EINVAL;
@@ -478,13 +466,10 @@ static int pcmuio_inttrig_start_intr(struct comedi_device *dev,
 	spin_lock_irqsave(&chip->spinlock, flags);
 	s->async->inttrig = NULL;
 	if (chip->active)
-		event = pcmuio_start_intr(dev, s);
+		pcmuio_start_intr(dev, s);
 
 	spin_unlock_irqrestore(&chip->spinlock, flags);
 
-	if (event)
-		comedi_event(dev, s);
-
 	return 1;
 }
 
@@ -498,35 +483,20 @@ static int pcmuio_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	int asic = pcmuio_subdevice_to_asic(s);
 	struct pcmuio_asic *chip = &devpriv->asics[asic];
 	unsigned long flags;
-	int event = 0;
 
 	spin_lock_irqsave(&chip->spinlock, flags);
 	chip->active = 1;
 
-	/* Set up end of acquisition. */
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		chip->continuous = 0;
-		chip->stop_count = cmd->stop_arg;
-		break;
-	default:
-		/* TRIG_NONE */
-		chip->continuous = 1;
-		chip->stop_count = 0;
-		break;
-	}
+	chip->stop_count = cmd->stop_arg;
 
 	/* Set up start of acquisition. */
 	if (cmd->start_src == TRIG_INT)
 		s->async->inttrig = pcmuio_inttrig_start_intr;
 	else	/* TRIG_NOW */
-		event = pcmuio_start_intr(dev, s);
+		pcmuio_start_intr(dev, s);
 
 	spin_unlock_irqrestore(&chip->spinlock, flags);
 
-	if (event)
-		comedi_event(dev, s);
-
 	return 0;
 }
 
@@ -564,16 +534,10 @@ static int pcmuio_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->convert_arg, 0);
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		/* any count allowed */
-		break;
-	case TRIG_NONE:
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-		break;
-	default:
-		break;
-	}
 
 	if (err)
 		return 3;
@@ -587,7 +551,7 @@ static int pcmuio_cmdtest(struct comedi_device *dev,
 
 static int pcmuio_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct pcmuio_board *board = comedi_board(dev);
+	const struct pcmuio_board *board = dev->board_ptr;
 	struct comedi_subdevice *s;
 	struct pcmuio_private *devpriv;
 	int ret;
diff --git a/drivers/staging/comedi/drivers/quatech_daqp_cs.c b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
index b1db61d9d834..6407df0404f0 100644
--- a/drivers/staging/comedi/drivers/quatech_daqp_cs.c
+++ b/drivers/staging/comedi/drivers/quatech_daqp_cs.c
@@ -638,7 +638,6 @@ static int daqp_ao_insn_write(struct comedi_device *dev,
 {
 	struct daqp_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val;
 	int i;
 
 	if (devpriv->stop)
@@ -648,7 +647,10 @@ static int daqp_ao_insn_write(struct comedi_device *dev,
 	outb(0, dev->iobase + DAQP_AUX);
 
 	for (i = 0; i > insn->n; i++) {
-		val = data[0];
+		unsigned val = data[i];
+
+		s->readback[chan] = val;
+
 		val &= 0x0fff;
 		val ^= 0x0800;		/* Flip the sign */
 		val |= (chan << 12);
@@ -739,6 +741,11 @@ static int daqp_auto_attach(struct comedi_device *dev,
 	s->maxdata	= 0x0fff;
 	s->range_table	= &range_bipolar5;
 	s->insn_write	= daqp_ao_insn_write;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DI;
diff --git a/drivers/staging/comedi/drivers/rtd520.c b/drivers/staging/comedi/drivers/rtd520.c
index 6fc4ed33f66c..7d4cb140959c 100644
--- a/drivers/staging/comedi/drivers/rtd520.c
+++ b/drivers/staging/comedi/drivers/rtd520.c
@@ -380,7 +380,6 @@ struct rtd_private {
 	int xfer_count;		/* # to transfer data. 0->1/2FIFO */
 	int flags;		/* flag event modes */
 	DECLARE_BITMAP(chan_is_bipolar, RTD_MAX_CHANLIST);
-	unsigned int ao_readback[2];
 	unsigned fifosz;
 };
 
@@ -400,15 +399,15 @@ static int rtd_ns_to_timer_base(unsigned int *nanosec,
 {
 	int divider;
 
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_NEAREST:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
 	default:
 		divider = (*nanosec + base / 2) / base;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		divider = (*nanosec) / base;
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		divider = (*nanosec + base - 1) / base;
 		break;
 	}
@@ -438,7 +437,7 @@ static int rtd_ns_to_timer(unsigned int *ns, unsigned int flags)
 static unsigned short rtd_convert_chan_gain(struct comedi_device *dev,
 					    unsigned int chanspec, int index)
 {
-	const struct rtd_boardinfo *board = comedi_board(dev);
+	const struct rtd_boardinfo *board = dev->board_ptr;
 	struct rtd_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(chanspec);
 	unsigned int range = CR_RANGE(chanspec);
@@ -809,26 +808,26 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 			if (cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
 						      RTD_MAX_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
-						TRIG_ROUND_UP);
+						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
 			if (cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
 						      RTD_MIN_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
-						TRIG_ROUND_DOWN);
+						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
 			}
 		} else {
 			if (cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
 						      RTD_MAX_SPEED)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
-						TRIG_ROUND_UP);
+						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
 			if (cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
 						      RTD_MIN_SPEED)) {
 				rtd_ns_to_timer(&cmd->scan_begin_arg,
-						TRIG_ROUND_DOWN);
+						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
 			}
 		}
@@ -844,26 +843,26 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 			if (cfc_check_trigger_arg_min(&cmd->convert_arg,
 						      RTD_MAX_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
-						TRIG_ROUND_UP);
+						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
 			if (cfc_check_trigger_arg_max(&cmd->convert_arg,
 						      RTD_MIN_SPEED_1)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
-						TRIG_ROUND_DOWN);
+						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
 			}
 		} else {
 			if (cfc_check_trigger_arg_min(&cmd->convert_arg,
 						      RTD_MAX_SPEED)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
-						TRIG_ROUND_UP);
+						CMDF_ROUND_UP);
 				err |= -EINVAL;
 			}
 			if (cfc_check_trigger_arg_max(&cmd->convert_arg,
 						      RTD_MIN_SPEED)) {
 				rtd_ns_to_timer(&cmd->convert_arg,
-						TRIG_ROUND_DOWN);
+						CMDF_ROUND_DOWN);
 				err |= -EINVAL;
 			}
 		}
@@ -875,12 +874,10 @@ static int rtd_ai_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* TODO check for rounding error due to counter wrap */
-	} else {
-		/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -956,7 +953,7 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	if (TRIG_TIMER == cmd->scan_begin_src) {
 		/* scan_begin_arg is in nanoseconds */
 		/* find out how many samples to wait before transferring */
-		if (cmd->flags & TRIG_WAKE_EOS) {
+		if (cmd->flags & CMDF_WAKE_EOS) {
 			/*
 			 * this may generate un-sustainable interrupt rates
 			 * the application is responsible for doing the
@@ -1020,7 +1017,7 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 	switch (cmd->scan_begin_src) {
 	case TRIG_TIMER:	/* periodic scanning */
 		timer = rtd_ns_to_timer(&cmd->scan_begin_arg,
-					TRIG_ROUND_NEAREST);
+					CMDF_ROUND_NEAREST);
 		/* set PACER clock */
 		writel(timer & 0xffffff, dev->mmio + LAS0_PCLK);
 
@@ -1038,7 +1035,7 @@ static int rtd_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		if (cmd->chanlist_len > 1) {
 			/* only needed for multi-channel */
 			timer = rtd_ns_to_timer(&cmd->convert_arg,
-						TRIG_ROUND_NEAREST);
+						CMDF_ROUND_NEAREST);
 			/* setup BURST clock */
 			writel(timer & 0x3ff, dev->mmio + LAS0_BCLK);
 		}
@@ -1138,7 +1135,7 @@ static int rtd_ao_winsn(struct comedi_device *dev,
 			((chan == 0) ? LAS1_DAC1_FIFO : LAS1_DAC2_FIFO));
 		writew(0, dev->mmio + ((chan == 0) ? LAS0_DAC1 : LAS0_DAC2));
 
-		devpriv->ao_readback[chan] = data[i];
+		s->readback[chan] = data[i];
 
 		ret = comedi_timeout(dev, s, insn, rtd_ao_eoc, 0);
 		if (ret)
@@ -1149,23 +1146,6 @@ static int rtd_ao_winsn(struct comedi_device *dev,
 	return i;
 }
 
-/* AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv. */
-static int rtd_ao_rinsn(struct comedi_device *dev,
-			struct comedi_subdevice *s, struct comedi_insn *insn,
-			unsigned int *data)
-{
-	struct rtd_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-
-	return i;
-}
-
 static int rtd_dio_insn_bits(struct comedi_device *dev,
 			     struct comedi_subdevice *s,
 			     struct comedi_insn *insn,
@@ -1323,7 +1303,11 @@ static int rtd_auto_attach(struct comedi_device *dev,
 	s->maxdata	= 0x0fff;
 	s->range_table	= &rtd_ao_range;
 	s->insn_write	= rtd_ao_winsn;
-	s->insn_read	= rtd_ao_rinsn;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* digital i/o subdevice */
diff --git a/drivers/staging/comedi/drivers/rti800.c b/drivers/staging/comedi/drivers/rti800.c
index 2b1db9783bd6..e3d9f44cefb9 100644
--- a/drivers/staging/comedi/drivers/rti800.c
+++ b/drivers/staging/comedi/drivers/rti800.c
@@ -137,7 +137,6 @@ struct rti800_private {
 	bool adc_2comp;
 	bool dac_2comp[2];
 	const struct comedi_lrange *ao_range_type_list[2];
-	unsigned int ao_readback[2];
 	unsigned char muxgain_bits;
 };
 
@@ -207,21 +206,6 @@ static int rti800_ai_insn_read(struct comedi_device *dev,
 	return insn->n;
 }
 
-static int rti800_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct rti800_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int rti800_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn,
@@ -231,11 +215,13 @@ static int rti800_ao_insn_write(struct comedi_device *dev,
 	unsigned int chan = CR_CHAN(insn->chanspec);
 	int reg_lo = chan ? RTI800_DAC1LO : RTI800_DAC0LO;
 	int reg_hi = chan ? RTI800_DAC1HI : RTI800_DAC0HI;
-	int val = devpriv->ao_readback[chan];
 	int i;
 
 	for (i = 0; i < insn->n; i++) {
-		val = data[i];
+		unsigned int val = data[i];
+
+		s->readback[chan] = val;
+
 		if (devpriv->dac_2comp[chan])
 			val ^= 0x800;
 
@@ -243,8 +229,6 @@ static int rti800_ao_insn_write(struct comedi_device *dev,
 		outb((val >> 8) & 0xff, dev->iobase + reg_hi);
 	}
 
-	devpriv->ao_readback[chan] = val;
-
 	return insn->n;
 }
 
@@ -274,7 +258,7 @@ static int rti800_do_insn_bits(struct comedi_device *dev,
 
 static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 {
-	const struct rti800_board *board = comedi_board(dev);
+	const struct rti800_board *board = dev->board_ptr;
 	struct rti800_private *devpriv;
 	struct comedi_subdevice *s;
 	int ret;
@@ -318,8 +302,6 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 		s->type		= COMEDI_SUBD_AO;
 		s->subdev_flags	= SDF_WRITABLE;
 		s->n_chan	= 2;
-		s->insn_read	= rti800_ao_insn_read;
-		s->insn_write	= rti800_ao_insn_write;
 		s->maxdata	= 0x0fff;
 		s->range_table_list = devpriv->ao_range_type_list;
 		devpriv->ao_range_type_list[0] =
@@ -330,6 +312,12 @@ static int rti800_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 			(it->options[7] < ARRAY_SIZE(rti800_ao_ranges))
 				? rti800_ao_ranges[it->options[7]]
 				: &range_unknown;
+		s->insn_write	= rti800_ao_insn_write;
+		s->insn_read	= comedi_readback_insn_read;
+
+		ret = comedi_alloc_subdev_readback(s);
+		if (ret)
+			return ret;
 	} else {
 		s->type		= COMEDI_SUBD_UNUSED;
 	}
diff --git a/drivers/staging/comedi/drivers/rti802.c b/drivers/staging/comedi/drivers/rti802.c
index 605a31d702e0..c81b01c40f12 100644
--- a/drivers/staging/comedi/drivers/rti802.c
+++ b/drivers/staging/comedi/drivers/rti802.c
@@ -45,24 +45,8 @@ struct rti802_private {
 		dac_2comp, dac_straight
 	} dac_coding[8];
 	const struct comedi_lrange *range_type_list[8];
-	unsigned int ao_readback[8];
 };
 
-static int rti802_ao_insn_read(struct comedi_device *dev,
-			       struct comedi_subdevice *s,
-			       struct comedi_insn *insn,
-			       unsigned int *data)
-{
-	struct rti802_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return insn->n;
-}
-
 static int rti802_ao_insn_write(struct comedi_device *dev,
 				struct comedi_subdevice *s,
 				struct comedi_insn *insn,
@@ -70,15 +54,14 @@ static int rti802_ao_insn_write(struct comedi_device *dev,
 {
 	struct rti802_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val;
 	int i;
 
 	outb(chan, dev->iobase + RTI802_SELECT);
 
 	for (i = 0; i < insn->n; i++) {
-		val = data[i];
+		unsigned int val = data[i];
 
-		devpriv->ao_readback[chan] = val;
+		s->readback[chan] = val;
 
 		/* munge offset binary to two's complement if needed */
 		if (devpriv->dac_coding[chan] == dac_2comp)
@@ -116,10 +99,14 @@ static int rti802_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->subdev_flags	= SDF_WRITABLE;
 	s->maxdata	= 0xfff;
 	s->n_chan	= 8;
-	s->insn_read	= rti802_ao_insn_read;
 	s->insn_write	= rti802_ao_insn_write;
-	s->range_table_list = devpriv->range_type_list;
+	s->insn_read	= comedi_readback_insn_read;
 
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
+	s->range_table_list = devpriv->range_type_list;
 	for (i = 0; i < 8; i++) {
 		devpriv->dac_coding[i] = (it->options[3 + 2 * i])
 			? (dac_straight) : (dac_2comp);
diff --git a/drivers/staging/comedi/drivers/s526.c b/drivers/staging/comedi/drivers/s526.c
index 83f7433c2452..75872c6aec2a 100644
--- a/drivers/staging/comedi/drivers/s526.c
+++ b/drivers/staging/comedi/drivers/s526.c
@@ -111,7 +111,6 @@ union cmReg {
 };
 
 struct s526_private {
-	unsigned int ao_readback[2];
 	unsigned int gpct_config[4];
 	unsigned short ai_config;
 };
@@ -467,38 +466,26 @@ static int s526_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
 	return n;
 }
 
-static int s526_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
+static int s526_ao_insn_write(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
 {
-	struct s526_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned short val;
+	unsigned int val = s->readback[chan];
 	int i;
 
-	val = chan << 1;
-	outw(val, dev->iobase + REG_DAC);
+	outw(chan << 1, dev->iobase + REG_DAC);
 
 	for (i = 0; i < insn->n; i++) {
-		outw(data[i], dev->iobase + REG_ADD);
-		devpriv->ao_readback[chan] = data[i];
+		val = data[i];
+		outw(val, dev->iobase + REG_ADD);
 		/* starts the D/A conversion */
-		outw(val + 1, dev->iobase + REG_DAC);
+		outw((chan << 1) | 1, dev->iobase + REG_DAC);
 	}
+	s->readback[chan] = val;
 
-	return i;
-}
-
-static int s526_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
-{
-	struct s526_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
+	return insn->n;
 }
 
 static int s526_dio_insn_bits(struct comedi_device *dev,
@@ -595,8 +582,12 @@ static int s526_attach(struct comedi_device *dev, struct comedi_devconfig *it)
 	s->n_chan = 4;
 	s->maxdata = 0xffff;
 	s->range_table = &range_bipolar10;
-	s->insn_write = s526_ao_winsn;
-	s->insn_read = s526_ao_rinsn;
+	s->insn_write = s526_ao_insn_write;
+	s->insn_read = comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[3];
 	/* digital i/o subdevice */
diff --git a/drivers/staging/comedi/drivers/s626.c b/drivers/staging/comedi/drivers/s626.c
index 080608a840ac..0e7621e890c3 100644
--- a/drivers/staging/comedi/drivers/s626.c
+++ b/drivers/staging/comedi/drivers/s626.c
@@ -78,7 +78,6 @@ struct s626_buffer_dma {
 
 struct s626_private {
 	uint8_t ai_cmd_running;		/* ai_cmd is running */
-	uint8_t ai_continuous;		/* continuous acquisition */
 	int ai_sample_count;		/* number of samples to acquire */
 	unsigned int ai_sample_timer;	/* time between samples in
 					 * units of the timer */
@@ -98,7 +97,6 @@ struct s626_private {
 	uint8_t trim_setpoint[12];	/* images of TrimDAC setpoints */
 	uint32_t i2c_adrs;		/* I2C device address for onboard EEPROM
 					 * (board rev dependent) */
-	unsigned int ao_readback[S626_DAC_CHANNELS];
 };
 
 /* Counter overflow/index event flag masks for RDMISC2. */
@@ -1399,7 +1397,6 @@ static void s626_check_dio_interrupts(struct comedi_device *dev)
 	uint8_t group;
 
 	for (group = 0; group < S626_DIO_BANKS; group++) {
-		irqbit = 0;
 		/* read interrupt type */
 		irqbit = s626_debi_read(dev, S626_LP_RDCAPFLG(group));
 
@@ -1504,19 +1501,20 @@ static bool s626_handle_eos_interrupt(struct comedi_device *dev)
 	/* end of scan occurs */
 	async->events |= COMEDI_CB_EOS;
 
-	if (!devpriv->ai_continuous)
+	if (cmd->stop_src == TRIG_COUNT) {
 		devpriv->ai_sample_count--;
-	if (devpriv->ai_sample_count <= 0) {
-		devpriv->ai_cmd_running = 0;
+		if (devpriv->ai_sample_count <= 0) {
+			devpriv->ai_cmd_running = 0;
 
-		/* Stop RPS program */
-		s626_mc_disable(dev, S626_MC1_ERPS1, S626_P_MC1);
+			/* Stop RPS program */
+			s626_mc_disable(dev, S626_MC1_ERPS1, S626_P_MC1);
 
-		/* send end of acquisition */
-		async->events |= COMEDI_CB_EOA;
+			/* send end of acquisition */
+			async->events |= COMEDI_CB_EOA;
 
-		/* disable master interrupt */
-		finished = true;
+			/* disable master interrupt */
+			finished = true;
+		}
 	}
 
 	if (devpriv->ai_cmd_running && cmd->scan_begin_src == TRIG_EXT)
@@ -1969,15 +1967,15 @@ static int s626_ns_to_timer(unsigned int *nanosec, unsigned int flags)
 
 	base = 500;		/* 2MHz internal clock */
 
-	switch (flags & TRIG_ROUND_MASK) {
-	case TRIG_ROUND_NEAREST:
+	switch (flags & CMDF_ROUND_MASK) {
+	case CMDF_ROUND_NEAREST:
 	default:
 		divider = (*nanosec + base / 2) / base;
 		break;
-	case TRIG_ROUND_DOWN:
+	case CMDF_ROUND_DOWN:
 		divider = (*nanosec) / base;
 		break;
-	case TRIG_ROUND_UP:
+	case CMDF_ROUND_UP:
 		divider = (*nanosec + base - 1) / base;
 		break;
 	}
@@ -2104,18 +2102,7 @@ static int s626_ai_cmd(struct comedi_device *dev, struct comedi_subdevice *s)
 		break;
 	}
 
-	switch (cmd->stop_src) {
-	case TRIG_COUNT:
-		/* data arrives as one packet */
-		devpriv->ai_sample_count = cmd->stop_arg;
-		devpriv->ai_continuous = 0;
-		break;
-	case TRIG_NONE:
-		/* continuous acquisition */
-		devpriv->ai_continuous = 1;
-		devpriv->ai_sample_count = 1;
-		break;
-	}
+	devpriv->ai_sample_count = cmd->stop_arg;
 
 	s626_reset_adc(dev, ppl);
 
@@ -2221,7 +2208,7 @@ static int s626_ai_cmdtest(struct comedi_device *dev,
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
 	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
 	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
 
@@ -2269,38 +2256,28 @@ static int s626_ai_cancel(struct comedi_device *dev, struct comedi_subdevice *s)
 	return 0;
 }
 
-static int s626_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
+static int s626_ao_insn_write(struct comedi_device *dev,
+			      struct comedi_subdevice *s,
+			      struct comedi_insn *insn,
+			      unsigned int *data)
 {
-	struct s626_private *devpriv = dev->private;
+	unsigned int chan = CR_CHAN(insn->chanspec);
 	int i;
-	int ret;
-	uint16_t chan = CR_CHAN(insn->chanspec);
-	int16_t dacdata;
 
 	for (i = 0; i < insn->n; i++) {
-		dacdata = (int16_t) data[i];
-		devpriv->ao_readback[CR_CHAN(insn->chanspec)] = data[i];
+		int16_t dacdata = (int16_t)data[i];
+		int ret;
+
 		dacdata -= (0x1fff);
 
 		ret = s626_set_dac(dev, chan, dacdata);
 		if (ret)
 			return ret;
-	}
-
-	return i;
-}
 
-static int s626_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
-{
-	struct s626_private *devpriv = dev->private;
-	int i;
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[CR_CHAN(insn->chanspec)];
+		s->readback[chan] = data[i];
+	}
 
-	return i;
+	return insn->n;
 }
 
 /* *************** DIGITAL I/O FUNCTIONS *************** */
@@ -2457,26 +2434,6 @@ static void s626_write_misc2(struct comedi_device *dev, uint16_t new_image)
 	s626_debi_write(dev, S626_LP_MISC1, S626_MISC1_WDISABLE);
 }
 
-static void s626_close_dma_b(struct comedi_device *dev,
-			     struct s626_buffer_dma *pdma, size_t bsize)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	void *vbptr;
-	dma_addr_t vpptr;
-
-	if (pdma == NULL)
-		return;
-
-	/* find the matching allocation from the board struct */
-	vbptr = pdma->logical_base;
-	vpptr = pdma->physical_base;
-	if (vbptr) {
-		pci_free_consistent(pcidev, bsize, vbptr, vpptr);
-		pdma->logical_base = NULL;
-		pdma->physical_base = 0;
-	}
-}
-
 static void s626_counters_init(struct comedi_device *dev)
 {
 	int chan;
@@ -2527,6 +2484,24 @@ static int s626_allocate_dma_buffers(struct comedi_device *dev)
 	return 0;
 }
 
+static void s626_free_dma_buffers(struct comedi_device *dev)
+{
+	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
+	struct s626_private *devpriv = dev->private;
+
+	if (!devpriv)
+		return;
+
+	if (devpriv->rps_buf.logical_base)
+		pci_free_consistent(pcidev, S626_DMABUF_SIZE,
+				    devpriv->rps_buf.logical_base,
+				    devpriv->rps_buf.physical_base);
+	if (devpriv->ana_buf.logical_base)
+		pci_free_consistent(pcidev, S626_DMABUF_SIZE,
+				    devpriv->ana_buf.logical_base,
+				    devpriv->ana_buf.physical_base);
+}
+
 static int s626_initialize(struct comedi_device *dev)
 {
 	struct s626_private *devpriv = dev->private;
@@ -2844,8 +2819,12 @@ static int s626_auto_attach(struct comedi_device *dev,
 	s->n_chan	= S626_DAC_CHANNELS;
 	s->maxdata	= 0x3fff;
 	s->range_table	= &range_bipolar10;
-	s->insn_write	= s626_ao_winsn;
-	s->insn_read	= s626_ao_rinsn;
+	s->insn_write	= s626_ao_insn_write;
+	s->insn_read	= comedi_readback_insn_read;
+
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
 
 	s = &dev->subdevices[2];
 	/* digital I/O subdevice */
@@ -2923,19 +2902,10 @@ static void s626_detach(struct comedi_device *dev)
 			/* Close all interfaces on 7146 device */
 			writel(S626_MC1_SHUTDOWN, dev->mmio + S626_P_MC1);
 			writel(S626_ACON1_BASE, dev->mmio + S626_P_ACON1);
-
-			s626_close_dma_b(dev, &devpriv->rps_buf,
-					 S626_DMABUF_SIZE);
-			s626_close_dma_b(dev, &devpriv->ana_buf,
-					 S626_DMABUF_SIZE);
 		}
-
-		if (dev->irq)
-			free_irq(dev->irq, dev);
-		if (dev->mmio)
-			iounmap(dev->mmio);
 	}
-	comedi_pci_disable(dev);
+	comedi_pci_detach(dev);
+	s626_free_dma_buffers(dev);
 }
 
 static struct comedi_driver s626_driver = {
diff --git a/drivers/staging/comedi/drivers/skel.c b/drivers/staging/comedi/drivers/skel.c
deleted file mode 100644
index a118678c24a1..000000000000
--- a/drivers/staging/comedi/drivers/skel.c
+++ /dev/null
@@ -1,726 +0,0 @@
-/*
-    comedi/drivers/skel.c
-    Skeleton code for a Comedi driver
-
-    COMEDI - Linux Control and Measurement Device Interface
-    Copyright (C) 2000 David A. Schleef <ds@schleef.org>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-*/
-/*
-Driver: skel
-Description: Skeleton driver, an example for driver writers
-Devices:
-Author: ds
-Updated: Mon, 18 Mar 2002 15:34:01 -0800
-Status: works
-
-This driver is a documented example on how Comedi drivers are
-written.
-
-Configuration Options:
-  none
-*/
-
-/*
- * The previous block comment is used to automatically generate
- * documentation in Comedi and Comedilib.  The fields:
- *
- *  Driver: the name of the driver
- *  Description: a short phrase describing the driver.  Don't list boards.
- *  Devices: a full list of the boards that attempt to be supported by
- *    the driver.  Format is "(manufacturer) board name [comedi name]",
- *    where comedi_name is the name that is used to configure the board.
- *    See the comment near board_name: in the struct comedi_driver structure
- *    below.  If (manufacturer) or [comedi name] is missing, the previous
- *    value is used.
- *  Author: you
- *  Updated: date when the _documentation_ was last updated.  Use 'date -R'
- *    to get a value for this.
- *  Status: a one-word description of the status.  Valid values are:
- *    works - driver works correctly on most boards supported, and
- *      passes comedi_test.
- *    unknown - unknown.  Usually put there by ds.
- *    experimental - may not work in any particular release.  Author
- *      probably wants assistance testing it.
- *    bitrotten - driver has not been update in a long time, probably
- *      doesn't work, and probably is missing support for significant
- *      Comedi interface features.
- *    untested - author probably wrote it "blind", and is believed to
- *      work, but no confirmation.
- *
- * These headers should be followed by a blank line, and any comments
- * you wish to say about the driver.  The comment area is the place
- * to put any known bugs, limitations, unsupported features, supported
- * command triggers, whether or not commands are supported on particular
- * subdevices, etc.
- *
- * Somewhere in the comment should be information about configuration
- * options that are used with comedi_config.
- */
-
-#include <linux/module.h>
-#include <linux/pci.h>
-
-#include "../comedidev.h"
-
-#include "comedi_fc.h"
-
-/* Imaginary registers for the imaginary board */
-#define SKEL_START_AI_CONV	0
-#define SKEL_AI_READ		0
-
-/*
- * Board descriptions for two imaginary boards.  Describing the
- * boards in this way is optional, and completely driver-dependent.
- * Some drivers use arrays such as this, other do not.
- */
-enum skel_boardid {
-	BOARD_SKEL100,
-	BOARD_SKEL200,
-};
-
-struct skel_board {
-	const char *name;
-	int ai_chans;
-	int ai_bits;
-	int have_dio;
-};
-
-static const struct skel_board skel_boards[] = {
-	[BOARD_SKEL100] = {
-		.name		= "skel-100",
-		.ai_chans	= 16,
-		.ai_bits	= 12,
-		.have_dio	= 1,
-	},
-	[BOARD_SKEL200] = {
-		.name		= "skel-200",
-		.ai_chans	= 8,
-		.ai_bits	= 16,
-	},
-};
-
-/* this structure is for data unique to this hardware driver.  If
-   several hardware drivers keep similar information in this structure,
-   feel free to suggest moving the variable to the struct comedi_device struct.
- */
-struct skel_private {
-
-	int data;
-
-	/* Used for AO readback */
-	unsigned int ao_readback[2];
-};
-
-/* This function doesn't require a particular form, this is just
- * what happens to be used in some of the drivers.  It should
- * convert ns nanoseconds to a counter value suitable for programming
- * the device.  Also, it should adjust ns so that it cooresponds to
- * the actual time that the device will use. */
-static int skel_ns_to_timer(unsigned int *ns, unsigned int flags)
-{
-	/* trivial timer */
-	/* if your timing is done through two cascaded timers, the
-	 * i8253_cascade_ns_to_timer() function in 8253.h can be
-	 * very helpful.  There are also i8254_load() and i8254_mm_load()
-	 * which can be used to load values into the ubiquitous 8254 counters
-	 */
-
-	return *ns;
-}
-
-/*
- * This function doesn't require a particular form, this is just
- * what happens to be used in some of the drivers. The comedi_timeout()
- * helper uses this callback to check for the end-of-conversion while
- * waiting for up to 1 second. This function should return 0 when the
- * conversion is finished and -EBUSY to keep waiting. Any other errno
- * will terminate comedi_timeout() and return that errno to the caller.
- * If the timeout occurs, comedi_timeout() will return -ETIMEDOUT.
- */
-static int skel_ai_eoc(struct comedi_device *dev,
-		       struct comedi_subdevice *s,
-		       struct comedi_insn *insn,
-		       unsigned long context)
-{
-	unsigned int status;
-
-	/* status = inb(dev->iobase + SKEL_STATUS); */
-	status = 1;
-	if (status)
-		return 0;
-	return -EBUSY;
-}
-
-/*
- * "instructions" read/write data in "one-shot" or "software-triggered"
- * mode.
- */
-static int skel_ai_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
-{
-	const struct skel_board *thisboard = comedi_board(dev);
-	int n;
-	unsigned int d;
-	int ret;
-
-	/* a typical programming sequence */
-
-	/* write channel to multiplexer */
-	/* outw(chan,dev->iobase + SKEL_MUX); */
-
-	/* don't wait for mux to settle */
-
-	/* convert n samples */
-	for (n = 0; n < insn->n; n++) {
-		/* trigger conversion */
-		/* outw(0,dev->iobase + SKEL_CONVERT); */
-
-		/* wait for conversion to end */
-		ret = comedi_timeout(dev, s, insn, skel_ai_eoc, 0);
-		if (ret)
-			return ret;
-
-		/* read data */
-		/* d = inw(dev->iobase + SKEL_AI_DATA); */
-		d = 0;
-
-		/* mangle the data as necessary */
-		d ^= 1 << (thisboard->ai_bits - 1);
-
-		data[n] = d;
-	}
-
-	/* return the number of samples read/written */
-	return n;
-}
-
-/*
- * cmdtest tests a particular command to see if it is valid.
- * Using the cmdtest ioctl, a user can create a valid cmd
- * and then have it executes by the cmd ioctl.
- *
- * cmdtest returns 1,2,3,4 or 0, depending on which tests
- * the command passes.
- */
-static int skel_ai_cmdtest(struct comedi_device *dev,
-			   struct comedi_subdevice *s,
-			   struct comedi_cmd *cmd)
-{
-	int err = 0;
-	unsigned int arg;
-
-	/* Step 1 : check if triggers are trivially valid */
-
-	err |= cfc_check_trigger_src(&cmd->start_src, TRIG_NOW);
-	err |= cfc_check_trigger_src(&cmd->scan_begin_src,
-					TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->convert_src, TRIG_TIMER | TRIG_EXT);
-	err |= cfc_check_trigger_src(&cmd->scan_end_src, TRIG_COUNT);
-	err |= cfc_check_trigger_src(&cmd->stop_src, TRIG_COUNT | TRIG_NONE);
-
-	if (err)
-		return 1;
-
-	/* Step 2a : make sure trigger sources are unique */
-
-	err |= cfc_check_trigger_is_unique(cmd->scan_begin_src);
-	err |= cfc_check_trigger_is_unique(cmd->convert_src);
-	err |= cfc_check_trigger_is_unique(cmd->stop_src);
-
-	/* Step 2b : and mutually compatible */
-
-	if (err)
-		return 2;
-
-	/* Step 3: check if arguments are trivially valid */
-
-	err |= cfc_check_trigger_arg_is(&cmd->start_arg, 0);
-
-#define MAX_SPEED	10000	/* in nanoseconds */
-#define MIN_SPEED	1000000000	/* in nanoseconds */
-
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-						 MAX_SPEED);
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg,
-						 MIN_SPEED);
-	} else {
-		/* external trigger */
-		/* should be level/edge, hi/lo specification here */
-		/* should specify multiple external triggers */
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 9);
-	}
-
-	if (cmd->convert_src == TRIG_TIMER) {
-		err |= cfc_check_trigger_arg_min(&cmd->convert_arg, MAX_SPEED);
-		err |= cfc_check_trigger_arg_max(&cmd->convert_arg, MIN_SPEED);
-	} else {
-		/* external trigger */
-		/* see above */
-		err |= cfc_check_trigger_arg_max(&cmd->scan_begin_arg, 9);
-	}
-
-	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
-
-	if (cmd->stop_src == TRIG_COUNT)
-		err |= cfc_check_trigger_arg_max(&cmd->stop_arg, 0x00ffffff);
-	else	/* TRIG_NONE */
-		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-
-	if (err)
-		return 3;
-
-	/* step 4: fix up any arguments */
-
-	if (cmd->scan_begin_src == TRIG_TIMER) {
-		arg = cmd->scan_begin_arg;
-		skel_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->scan_begin_arg, arg);
-	}
-	if (cmd->convert_src == TRIG_TIMER) {
-		arg = cmd->convert_arg;
-		skel_ns_to_timer(&arg, cmd->flags);
-		err |= cfc_check_trigger_arg_is(&cmd->convert_arg, arg);
-
-		if (cmd->scan_begin_src == TRIG_TIMER) {
-			arg = cmd->convert_arg * cmd->scan_end_arg;
-			err |= cfc_check_trigger_arg_min(&cmd->scan_begin_arg,
-							 arg);
-		}
-	}
-
-	if (err)
-		return 4;
-
-	return 0;
-}
-
-static int skel_ao_winsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
-{
-	struct skel_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	/* Writing a list of values to an AO channel is probably not
-	 * very useful, but that's how the interface is defined. */
-	for (i = 0; i < insn->n; i++) {
-		/* a typical programming sequence */
-		/* outw(data[i],dev->iobase + SKEL_DA0 + chan); */
-		devpriv->ao_readback[chan] = data[i];
-	}
-
-	/* return the number of samples read/written */
-	return i;
-}
-
-/* AO subdevices should have a read insn as well as a write insn.
- * Usually this means copying a value stored in devpriv. */
-static int skel_ao_rinsn(struct comedi_device *dev, struct comedi_subdevice *s,
-			 struct comedi_insn *insn, unsigned int *data)
-{
-	struct skel_private *devpriv = dev->private;
-	int i;
-	int chan = CR_CHAN(insn->chanspec);
-
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
-
-	return i;
-}
-
-/*
- * DIO devices are slightly special. Although it is possible to
- * implement the insn_read/insn_write interface, it is much more
- * useful to applications if you implement the insn_bits interface.
- * This allows packed reading/writing of the DIO channels. The
- * comedi core can convert between insn_bits and insn_read/write.
- */
-static int skel_dio_insn_bits(struct comedi_device *dev,
-			      struct comedi_subdevice *s,
-			      struct comedi_insn *insn,
-			      unsigned int *data)
-{
-	/*
-	 * The insn data is a mask in data[0] and the new data
-	 * in data[1], each channel cooresponding to a bit.
-	 *
-	 * The core provided comedi_dio_update_state() function can
-	 * be used to handle the internal state update to DIO subdevices
-	 * with <= 32 channels. This function will return '0' if the
-	 * state does not change or the mask of the channels that need
-	 * to be updated.
-	 */
-	if (comedi_dio_update_state(s, data)) {
-		/* Write out the new digital output lines */
-		/* outw(s->state, dev->iobase + SKEL_DIO); */
-	}
-
-	/*
-	 * On return, data[1] contains the value of the digital
-	 * input and output lines.
-	 */
-	/* data[1] = inw(dev->iobase + SKEL_DIO); */
-
-	/*
-	 * Or we could just return the software copy of the output
-	 * values if it was a purely digital output subdevice.
-	 */
-	/* data[1] = s->state; */
-
-	return insn->n;
-}
-
-static int skel_dio_insn_config(struct comedi_device *dev,
-				struct comedi_subdevice *s,
-				struct comedi_insn *insn,
-				unsigned int *data)
-{
-	int ret;
-
-	/*
-	 * The input or output configuration of each digital line is
-	 * configured by special insn_config instructions.
-	 *
-	 * chanspec contains the channel to be changed
-	 * data[0] contains the instruction to perform on the channel
-	 *
-	 * Normally the core provided comedi_dio_insn_config() function
-	 * can be used to handle the boilerplpate.
-	 */
-	ret = comedi_dio_insn_config(dev, s, insn, data, 0);
-	if (ret)
-		return ret;
-
-	/* Update the hardware to the new configuration */
-	/* outw(s->io_bits, dev->iobase + SKEL_DIO_CONFIG); */
-
-	return insn->n;
-}
-
-/*
- * Handle common part of skel_attach() and skel_auto_attach().
- */
-static int skel_common_attach(struct comedi_device *dev)
-{
-	const struct skel_board *thisboard = comedi_board(dev);
-	struct comedi_subdevice *s;
-	int ret;
-
-	ret = comedi_alloc_subdevices(dev, 3);
-	if (ret)
-		return ret;
-
-	s = &dev->subdevices[0];
-	/* dev->read_subdev=s; */
-	/* analog input subdevice */
-	s->type = COMEDI_SUBD_AI;
-	/* we support single-ended (ground) and differential */
-	s->subdev_flags = SDF_READABLE | SDF_GROUND | SDF_DIFF;
-	s->n_chan = thisboard->ai_chans;
-	s->maxdata = (1 << thisboard->ai_bits) - 1;
-	s->range_table = &range_bipolar10;
-	s->len_chanlist = 16;	/* This is the maximum chanlist length that
-				   the board can handle */
-	s->insn_read = skel_ai_rinsn;
-/*
-*       s->subdev_flags |= SDF_CMD_READ;
-*       s->do_cmd = skel_ai_cmd;
-*/
-	s->do_cmdtest = skel_ai_cmdtest;
-
-	s = &dev->subdevices[1];
-	/* analog output subdevice */
-	s->type = COMEDI_SUBD_AO;
-	s->subdev_flags = SDF_WRITABLE;
-	s->n_chan = 1;
-	s->maxdata = 0xffff;
-	s->range_table = &range_bipolar5;
-	s->insn_write = skel_ao_winsn;
-	s->insn_read = skel_ao_rinsn;
-
-	s = &dev->subdevices[2];
-	/* digital i/o subdevice */
-	if (thisboard->have_dio) {
-		s->type = COMEDI_SUBD_DIO;
-		s->subdev_flags = SDF_READABLE | SDF_WRITABLE;
-		s->n_chan = 16;
-		s->maxdata = 1;
-		s->range_table = &range_digital;
-		s->insn_bits = skel_dio_insn_bits;
-		s->insn_config = skel_dio_insn_config;
-	} else {
-		s->type = COMEDI_SUBD_UNUSED;
-	}
-
-	return 0;
-}
-
-/*
- * _attach is called by the Comedi core to configure the driver
- * for a particular board in response to the COMEDI_DEVCONFIG ioctl for
- * a matching board or driver name.  If you specified a board_name array
- * in the driver structure, dev->board_ptr contains that address.
- *
- * Drivers that handle only PCI or USB devices do not usually support
- * manual attachment of those devices via the COMEDI_DEVCONFIG ioctl, so
- * those drivers do not have an _attach function; they just have an
- * _auto_attach function instead.  (See skel_auto_attach() for an example
- * of such a function.)
- */
-static int skel_attach(struct comedi_device *dev, struct comedi_devconfig *it)
-{
-	const struct skel_board *thisboard;
-	struct skel_private *devpriv;
-
-/*
- * If you can probe the device to determine what device in a series
- * it is, this is the place to do it.  Otherwise, dev->board_ptr
- * should already be initialized.
- */
-	/* dev->board_ptr = skel_probe(dev, it); */
-
-	thisboard = comedi_board(dev);
-
-	/*
-	 * The dev->board_name is initialized by the comedi core before
-	 * calling the (*attach) function. It can be optionally set by
-	 * the driver if additional probing has been done.
-	 */
-	/* dev->board_name = thisboard->name; */
-
-	/* Allocate the private data */
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
-/*
- * Supported boards are usually either auto-attached via the
- * Comedi driver's _auto_attach routine, or manually attached via the
- * Comedi driver's _attach routine.  In most cases, attempts to
- * manual attach boards that are usually auto-attached should be
- * rejected by this function.
- */
-/*
- *	if (thisboard->bustype == pci_bustype) {
- *		dev_err(dev->class_dev,
- *			"Manual attachment of PCI board '%s' not supported\n",
- *			thisboard->name);
- *	}
- */
-
-/*
- * For ISA boards, get the i/o base address from it->options[],
- * request the i/o region and set dev->iobase * from it->options[].
- * If using interrupts, get the IRQ number from it->options[].
- */
-
-	/*
-	 * Call a common function to handle the remaining things to do for
-	 * attaching ISA or PCI boards.  (Extra parameters could be added
-	 * to pass additional information such as IRQ number.)
-	 */
-	return skel_common_attach(dev);
-}
-
-/*
- * _auto_attach is called via comedi_pci_auto_config() (or
- * comedi_usb_auto_config(), etc.) to handle devices that can be attached
- * to the Comedi core automatically without the COMEDI_DEVCONFIG ioctl.
- *
- * The context parameter is driver dependent.
- */
-static int skel_auto_attach(struct comedi_device *dev,
-			    unsigned long context)
-{
-	struct pci_dev *pcidev = comedi_to_pci_dev(dev);
-	const struct skel_board *thisboard = NULL;
-	struct skel_private *devpriv;
-	int ret;
-
-	/* Hack to allow unused code to be optimized out. */
-	if (!IS_ENABLED(CONFIG_COMEDI_PCI_DRIVERS))
-		return -EINVAL;
-
-	/*
-	 * In this example, the _auto_attach is for a PCI device.
-	 *
-	 * The 'context' passed to this function is the id->driver_data
-	 * associated with the PCI device found in the id_table during
-	 * the modprobe. This 'context' is the index of the entry in
-	 * skel_boards[i] that contains the boardinfo for the PCI device.
-	 */
-	if (context < ARRAY_SIZE(skel_boards))
-		thisboard = &skel_boards[context];
-	if (!thisboard)
-		return -ENODEV;
-
-	/*
-	 * Point the struct comedi_device to the matching board info
-	 * and set the board name.
-	 */
-	dev->board_ptr = thisboard;
-	dev->board_name = thisboard->name;
-
-	/* Allocate the private data */
-	devpriv = comedi_alloc_devpriv(dev, sizeof(*devpriv));
-	if (!devpriv)
-		return -ENOMEM;
-
-	/* Enable the PCI device. */
-	ret = comedi_pci_enable(dev);
-	if (ret)
-		return ret;
-
-	/*
-	 * Record the fact that the PCI device is enabled so that it can
-	 * be disabled during _detach().
-	 *
-	 * For this example driver, we assume PCI BAR 0 is the main I/O
-	 * region for the board registers and use dev->iobase to hold the
-	 * I/O base address and to indicate that the PCI device has been
-	 * enabled.
-	 *
-	 * (For boards with memory-mapped registers, dev->iobase is not
-	 * usually needed for register access, so can just be set to 1
-	 * to indicate that the PCI device has been enabled.)
-	 */
-	dev->iobase = pci_resource_start(pcidev, 0);
-
-	/*
-	 * Call a common function to handle the remaining things to do for
-	 * attaching ISA or PCI boards.  (Extra parameters could be added
-	 * to pass additional information such as IRQ number.)
-	 */
-	return skel_common_attach(dev);
-}
-
-/*
- * _detach is called to deconfigure a device.  It should deallocate
- * resources.
- * This function is also called when _attach() fails, so it should be
- * careful not to release resources that were not necessarily
- * allocated by _attach().  dev->private and dev->subdevices are
- * deallocated automatically by the core.
- */
-static void skel_detach(struct comedi_device *dev)
-{
-	const struct skel_board *thisboard = comedi_board(dev);
-	struct skel_private *devpriv = dev->private;
-
-	if (!thisboard || !devpriv)
-		return;
-
-/*
- * Do common stuff such as freeing IRQ, unmapping remapped memory
- * regions, etc., being careful to check that the stuff is valid given
- * that _detach() is called even when _attach() or _auto_attach() return
- * an error.
- */
-
-	if (IS_ENABLED(CONFIG_COMEDI_PCI_DRIVERS) /* &&
-	    thisboard->bustype == pci_bustype */) {
-		/*
-		 * PCI board
-		 *
-		 * If PCI device enabled by _auto_attach() (or _attach()),
-		 * disable it here.
-		 */
-		comedi_pci_disable(dev);
-	} else {
-		/*
-		 * ISA board
-		 *
-		 * Release the first I/O region requested during the
-		 * _attach(). This is safe to call even if the request
-		 * failed. If any additional I/O regions are requested
-		 * they need to be released by the driver.
-		 */
-		comedi_legacy_detach(dev);
-	}
-}
-
-/*
- * The struct comedi_driver structure tells the Comedi core module
- * which functions to call to configure/deconfigure (attach/detach)
- * the board, and also about the kernel module that contains
- * the device code.
- */
-static struct comedi_driver skel_driver = {
-	.driver_name = "dummy",
-	.module = THIS_MODULE,
-	.attach = skel_attach,
-	.auto_attach = skel_auto_attach,
-	.detach = skel_detach,
-/* It is not necessary to implement the following members if you are
- * writing a driver for a ISA PnP or PCI card */
-	/* Most drivers will support multiple types of boards by
-	 * having an array of board structures.  These were defined
-	 * in skel_boards[] above.  Note that the element 'name'
-	 * was first in the structure -- Comedi uses this fact to
-	 * extract the name of the board without knowing any details
-	 * about the structure except for its length.
-	 * When a device is attached (by comedi_config), the name
-	 * of the device is given to Comedi, and Comedi tries to
-	 * match it by going through the list of board names.  If
-	 * there is a match, the address of the pointer is put
-	 * into dev->board_ptr and driver->attach() is called.
-	 *
-	 * Note that these are not necessary if you can determine
-	 * the type of board in software.  ISA PnP, PCI, and PCMCIA
-	 * devices are such boards.
-	 */
-	.board_name = &skel_boards[0].name,
-	.offset = sizeof(struct skel_board),
-	.num_names = ARRAY_SIZE(skel_boards),
-};
-
-#ifdef CONFIG_COMEDI_PCI_DRIVERS
-
-static int skel_pci_probe(struct pci_dev *dev,
-			  const struct pci_device_id *id)
-{
-	return comedi_pci_auto_config(dev, &skel_driver, id->driver_data);
-}
-
-/*
- * Please add your PCI vendor ID to comedidev.h, and it will
- * be forwarded upstream.
- */
-#define PCI_VENDOR_ID_SKEL	0xdafe
-
-/*
- * This is used by modprobe to translate PCI IDs to drivers.
- * Should only be used for PCI and ISA-PnP devices
- */
-static const struct pci_device_id skel_pci_table[] = {
-	{ PCI_VDEVICE(SKEL, 0x0100), BOARD_SKEL100 },
-	{ PCI_VDEVICE(SKEL, 0x0200), BOARD_SKEL200 },
-	{ 0 }
-};
-MODULE_DEVICE_TABLE(pci, skel_pci_table);
-
-static struct pci_driver skel_pci_driver = {
-	.name		= "dummy",
-	.id_table	= skel_pci_table,
-	.probe		= skel_pci_probe,
-	.remove		= comedi_pci_auto_unconfig,
-};
-module_comedi_pci_driver(skel_driver, skel_pci_driver);
-#else
-module_comedi_driver(skel_driver);
-#endif
-
-MODULE_AUTHOR("Comedi http://www.comedi.org");
-MODULE_DESCRIPTION("Comedi low-level driver");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/comedi/drivers/usbdux.c b/drivers/staging/comedi/drivers/usbdux.c
index 053bc5090530..5adbfedf780f 100644
--- a/drivers/staging/comedi/drivers/usbdux.c
+++ b/drivers/staging/comedi/drivers/usbdux.c
@@ -109,8 +109,6 @@ sampling rate. If you sample two channels you get 4kHz and so on.
 #define USBDUX_CMD_PWM_ON	7
 #define USBDUX_CMD_PWM_OFF	8
 
-#define USBDUX_NUM_AO_CHAN	4
-
 /* timeout for the USB-transfer in ms */
 #define BULK_TIMEOUT		1000
 
@@ -198,11 +196,9 @@ struct usbdux_private {
 	/* size of the PWM buffer which holds the bit pattern */
 	int pwm_buf_sz;
 	/* input buffer for the ISO-transfer */
-	uint16_t *in_buf;
+	__le16 *in_buf;
 	/* input buffer for single insn */
-	uint16_t *insn_buf;
-
-	unsigned int ao_readback[USBDUX_NUM_AO_CHAN];
+	__le16 *insn_buf;
 
 	unsigned int high_speed:1;
 	unsigned int ai_cmd_running:1;
@@ -490,7 +486,7 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 			*datap++ = val & 0xff;
 			*datap++ = (val >> 8) & 0xff;
 			*datap++ = chan << 6;
-			devpriv->ao_readback[chan] = val;
+			s->readback[chan] = val;
 
 			s->async->events |= COMEDI_CB_BLOCK;
 			comedi_event(dev, s);
@@ -513,7 +509,7 @@ static void usbduxsub_ao_isoc_irq(struct urb *urb)
 			dev_err(dev->class_dev,
 				"ao urb resubm failed in int-cont. ret=%d",
 				ret);
-			if (ret == EL2NSYNC)
+			if (ret == -EL2NSYNC)
 				dev_err(dev->class_dev,
 					"buggy USB host controller or bug in IRQ handling!\n");
 
@@ -627,12 +623,10 @@ static int usbdux_ai_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {
-		/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -855,15 +849,13 @@ static int usbdux_ao_insn_read(struct comedi_device *dev,
 			       unsigned int *data)
 {
 	struct usbdux_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
+	int ret;
 
 	down(&devpriv->sem);
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+	ret = comedi_readback_insn_read(dev, s, insn, data);
 	up(&devpriv->sem);
 
-	return insn->n;
+	return ret;
 }
 
 static int usbdux_ao_insn_write(struct comedi_device *dev,
@@ -873,8 +865,8 @@ static int usbdux_ao_insn_write(struct comedi_device *dev,
 {
 	struct usbdux_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	unsigned int val = devpriv->ao_readback[chan];
-	uint16_t *p = (uint16_t *)&devpriv->dux_commands[2];
+	unsigned int val = s->readback[chan];
+	__le16 *p = (__le16 *)&devpriv->dux_commands[2];
 	int ret = -EBUSY;
 	int i;
 
@@ -897,8 +889,9 @@ static int usbdux_ao_insn_write(struct comedi_device *dev,
 		ret = send_dux_commands(dev, USBDUX_CMD_AO);
 		if (ret < 0)
 			goto ao_write_exit;
+
+		s->readback[chan] = val;
 	}
-	devpriv->ao_readback[chan] = val;
 
 ao_write_exit:
 	up(&devpriv->sem);
@@ -1008,12 +1001,10 @@ static int usbdux_ao_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {
-		/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -1185,7 +1176,7 @@ static int usbdux_counter_write(struct comedi_device *dev,
 {
 	struct usbdux_private *devpriv = dev->private;
 	unsigned int chan = CR_CHAN(insn->chanspec);
-	uint16_t *p = (uint16_t *)&devpriv->dux_commands[2];
+	__le16 *p = (__le16 *)&devpriv->dux_commands[2];
 	int ret = 0;
 	int i;
 
@@ -1294,7 +1285,7 @@ static void usbduxsub_pwm_irq(struct urb *urb)
 			dev_err(dev->class_dev,
 				"pwm urb resubm failed in int-cont. ret=%d",
 				ret);
-			if (ret == EL2NSYNC)
+			if (ret == -EL2NSYNC)
 				dev_err(dev->class_dev,
 					"buggy USB host controller or bug in IRQ handling!\n");
 
@@ -1720,7 +1711,7 @@ static int usbdux_auto_attach(struct comedi_device *dev,
 	dev->write_subdev = s;
 	s->type		= COMEDI_SUBD_AO;
 	s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
-	s->n_chan	= USBDUX_NUM_AO_CHAN;
+	s->n_chan	= 4;
 	s->maxdata	= 0x0fff;
 	s->len_chanlist	= s->n_chan;
 	s->range_table	= &range_usbdux_ao_range;
@@ -1730,6 +1721,10 @@ static int usbdux_auto_attach(struct comedi_device *dev,
 	s->insn_read	= usbdux_ao_insn_read;
 	s->insn_write	= usbdux_ao_insn_write;
 
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
 	/* Digital I/O subdevice */
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DIO;
diff --git a/drivers/staging/comedi/drivers/usbduxsigma.c b/drivers/staging/comedi/drivers/usbduxsigma.c
index 94a09c16de8b..ebd68e365bac 100644
--- a/drivers/staging/comedi/drivers/usbduxsigma.c
+++ b/drivers/staging/comedi/drivers/usbduxsigma.c
@@ -75,8 +75,6 @@
 /* Number of channels (16 AD and offset)*/
 #define NUMCHANNELS 16
 
-#define USBDUXSIGMA_NUM_AO_CHAN		4
-
 /* Size of one A/D value */
 #define SIZEADIN          ((sizeof(uint32_t)))
 
@@ -157,12 +155,10 @@ struct usbduxsigma_private {
 	/* size of the PWM buffer which holds the bit pattern */
 	int pwm_buf_sz;
 	/* input buffer for the ISO-transfer */
-	uint32_t *in_buf;
+	__be32 *in_buf;
 	/* input buffer for single insn */
 	uint8_t *insn_buf;
 
-	unsigned int ao_readback[USBDUXSIGMA_NUM_AO_CHAN];
-
 	unsigned high_speed:1;
 	unsigned ai_cmd_running:1;
 	unsigned ao_cmd_running:1;
@@ -428,7 +424,7 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 			}
 			*datap++ = val;
 			*datap++ = chan;
-			devpriv->ao_readback[chan] = val;
+			s->readback[chan] = val;
 
 			s->async->events |= COMEDI_CB_BLOCK;
 			comedi_event(dev, s);
@@ -451,7 +447,7 @@ static void usbduxsigma_ao_urb_complete(struct urb *urb)
 		dev_err(dev->class_dev,
 			"%s: urb resubmit failed (%d)\n",
 			__func__, ret);
-		if (ret == EL2NSYNC)
+		if (ret == -EL2NSYNC)
 			dev_err(dev->class_dev,
 				"buggy USB host controller or bug in IRQ handler\n");
 		usbduxsigma_ao_stop(dev, 0);	/* w/o unlink */
@@ -562,12 +558,10 @@ static int usbduxsigma_ai_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {
-		/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -788,7 +782,7 @@ static int usbduxsigma_ai_insn_read(struct comedi_device *dev,
 		}
 
 		/* 32 bits big endian from the A/D converter */
-		val = be32_to_cpu(get_unaligned((uint32_t
+		val = be32_to_cpu(get_unaligned((__be32
 						 *)(devpriv->insn_buf + 1)));
 		val &= 0x00ffffff;	/* strip status byte */
 		val ^= 0x00800000;	/* convert to unsigned */
@@ -806,15 +800,13 @@ static int usbduxsigma_ao_insn_read(struct comedi_device *dev,
 				    unsigned int *data)
 {
 	struct usbduxsigma_private *devpriv = dev->private;
-	unsigned int chan = CR_CHAN(insn->chanspec);
-	int i;
+	int ret;
 
 	down(&devpriv->sem);
-	for (i = 0; i < insn->n; i++)
-		data[i] = devpriv->ao_readback[chan];
+	ret = comedi_readback_insn_read(dev, s, insn, data);
 	up(&devpriv->sem);
 
-	return insn->n;
+	return ret;
 }
 
 static int usbduxsigma_ao_insn_write(struct comedi_device *dev,
@@ -842,7 +834,7 @@ static int usbduxsigma_ao_insn_write(struct comedi_device *dev,
 			up(&devpriv->sem);
 			return ret;
 		}
-		devpriv->ao_readback[chan] = data[i];
+		s->readback[chan] = data[i];
 	}
 	up(&devpriv->sem);
 
@@ -941,12 +933,10 @@ static int usbduxsigma_ao_cmdtest(struct comedi_device *dev,
 
 	err |= cfc_check_trigger_arg_is(&cmd->scan_end_arg, cmd->chanlist_len);
 
-	if (cmd->stop_src == TRIG_COUNT) {
-		/* any count is allowed */
-	} else {
-		/* TRIG_NONE */
+	if (cmd->stop_src == TRIG_COUNT)
+		err |= cfc_check_trigger_arg_min(&cmd->stop_arg, 1);
+	else	/* TRIG_NONE */
 		err |= cfc_check_trigger_arg_is(&cmd->stop_arg, 0);
-	}
 
 	if (err)
 		return 3;
@@ -1148,7 +1138,7 @@ static void usbduxsigma_pwm_urb_complete(struct urb *urb)
 	if (ret < 0) {
 		dev_err(dev->class_dev, "%s: urb resubmit failed (%d)\n",
 			__func__, ret);
-		if (ret == EL2NSYNC)
+		if (ret == -EL2NSYNC)
 			dev_err(dev->class_dev,
 				"buggy USB host controller or bug in IRQ handler\n");
 		usbduxsigma_pwm_stop(dev, 0);	/* w/o unlink */
@@ -1349,7 +1339,7 @@ static int usbduxsigma_getstatusinfo(struct comedi_device *dev, int chan)
 		return ret;
 
 	/* 32 bits big endian from the A/D converter */
-	val = be32_to_cpu(get_unaligned((uint32_t *)(devpriv->insn_buf + 1)));
+	val = be32_to_cpu(get_unaligned((__be32 *)(devpriv->insn_buf + 1)));
 	val &= 0x00ffffff;	/* strip status byte */
 	val ^= 0x00800000;	/* convert to unsigned */
 
@@ -1437,10 +1427,8 @@ static int usbduxsigma_alloc_usb_buffers(struct comedi_device *dev)
 	devpriv->dux_commands = kzalloc(SIZEOFDUXBUFFER, GFP_KERNEL);
 	devpriv->in_buf = kzalloc(SIZEINBUF, GFP_KERNEL);
 	devpriv->insn_buf = kzalloc(SIZEINSNBUF, GFP_KERNEL);
-	devpriv->ai_urbs = kcalloc(devpriv->n_ai_urbs, sizeof(*urb),
-				   GFP_KERNEL);
-	devpriv->ao_urbs = kcalloc(devpriv->n_ao_urbs, sizeof(*urb),
-				   GFP_KERNEL);
+	devpriv->ai_urbs = kcalloc(devpriv->n_ai_urbs, sizeof(urb), GFP_KERNEL);
+	devpriv->ao_urbs = kcalloc(devpriv->n_ao_urbs, sizeof(urb), GFP_KERNEL);
 	if (!devpriv->dux_commands || !devpriv->in_buf || !devpriv->insn_buf ||
 	    !devpriv->ai_urbs || !devpriv->ao_urbs)
 		return -ENOMEM;
@@ -1613,7 +1601,7 @@ static int usbduxsigma_auto_attach(struct comedi_device *dev,
 	dev->write_subdev = s;
 	s->type		= COMEDI_SUBD_AO;
 	s->subdev_flags	= SDF_WRITABLE | SDF_GROUND | SDF_CMD_WRITE;
-	s->n_chan	= USBDUXSIGMA_NUM_AO_CHAN;
+	s->n_chan	= 4;
 	s->len_chanlist	= s->n_chan;
 	s->maxdata	= 0x00ff;
 	s->range_table	= &range_unipolar2_5;
@@ -1623,6 +1611,10 @@ static int usbduxsigma_auto_attach(struct comedi_device *dev,
 	s->do_cmd	= usbduxsigma_ao_cmd;
 	s->cancel	= usbduxsigma_ao_cancel;
 
+	ret = comedi_alloc_subdev_readback(s);
+	if (ret)
+		return ret;
+
 	/* Digital I/O subdevice */
 	s = &dev->subdevices[2];
 	s->type		= COMEDI_SUBD_DIO;
diff --git a/drivers/staging/comedi/drivers/vmk80xx.c b/drivers/staging/comedi/drivers/vmk80xx.c
index 831c3b702899..71003416edcf 100644
--- a/drivers/staging/comedi/drivers/vmk80xx.c
+++ b/drivers/staging/comedi/drivers/vmk80xx.c
@@ -767,7 +767,7 @@ static int vmk80xx_alloc_usb_buffers(struct comedi_device *dev)
 
 static int vmk80xx_init_subdevices(struct comedi_device *dev)
 {
-	const struct vmk80xx_board *boardinfo = comedi_board(dev);
+	const struct vmk80xx_board *boardinfo = dev->board_ptr;
 	struct vmk80xx_private *devpriv = dev->private;
 	struct comedi_subdevice *s;
 	int n_subd;
diff --git a/drivers/staging/dgap/dgap.c b/drivers/staging/dgap/dgap.c
index 06c55cb57090..d0be1cebf5a9 100644
--- a/drivers/staging/dgap/dgap.c
+++ b/drivers/staging/dgap/dgap.c
@@ -41,6 +41,8 @@
  */
 #undef DIGI_CONCENTRATORS_SUPPORTED
 
+#define pr_fmt(fmt) "dgap: " fmt
+
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/pci.h>
@@ -86,7 +88,8 @@ static int dgap_block_til_ready(struct tty_struct *tty, struct file *file,
 				struct channel_t *ch);
 static int dgap_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 				unsigned long arg);
-static int dgap_tty_digigeta(struct channel_t *ch, struct digi_t __user *retinfo);
+static int dgap_tty_digigeta(struct channel_t *ch,
+			     struct digi_t __user *retinfo);
 static int dgap_tty_digiseta(struct channel_t *ch, struct board_t *bd,
 			     struct un_t *un, struct digi_t __user *new_info);
 static int dgap_tty_digigetedelay(struct tty_struct *tty, int __user *retinfo);
@@ -102,8 +105,9 @@ static void dgap_tty_flush_chars(struct tty_struct *tty);
 static void dgap_tty_flush_buffer(struct tty_struct *tty);
 static void dgap_tty_hangup(struct tty_struct *tty);
 static int dgap_wait_for_drain(struct tty_struct *tty);
-static int dgap_set_modem_info(struct channel_t *ch, struct board_t *bd, struct un_t *un,
-			       unsigned int command, unsigned int __user *value);
+static int dgap_set_modem_info(struct channel_t *ch, struct board_t *bd,
+			       struct un_t *un, unsigned int command,
+			       unsigned int __user *value);
 static int dgap_get_modem_info(struct channel_t *ch,
 				unsigned int __user *value);
 static int dgap_tty_digisetcustombaud(struct channel_t *ch, struct board_t *bd,
@@ -153,17 +157,10 @@ static void dgap_firmware_reset_port(struct channel_t *ch);
 static int dgap_gettok(char **in);
 static char *dgap_getword(char **in);
 static int dgap_checknode(struct cnode *p);
-static void dgap_err(char *s);
 
 /*
  * Function prototypes from dgap_sysfs.h
  */
-struct board_t;
-struct channel_t;
-struct un_t;
-struct pci_driver;
-struct class_device;
-
 static void dgap_create_ports_sysfiles(struct board_t *bd);
 static void dgap_remove_ports_sysfiles(struct board_t *bd);
 
@@ -183,7 +180,6 @@ static char *dgap_create_config_string(struct board_t *bd, char *string);
 static uint dgap_config_get_useintr(struct board_t *bd);
 static uint dgap_config_get_altpin(struct board_t *bd);
 
-static int dgap_ms_sleep(ulong ms);
 static void dgap_do_bios_load(struct board_t *brd, const u8 *ubios, int len);
 static void dgap_do_fep_load(struct board_t *brd, const u8 *ufep, int len);
 #ifdef DIGI_CONCENTRATORS_SUPPORTED
@@ -201,6 +197,7 @@ static int dgap_test_fep(struct board_t *brd);
 static int dgap_tty_register_ports(struct board_t *brd);
 static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
 			      struct board_t *brd);
+static void dgap_cleanup_nodes(void);
 
 static void dgap_cleanup_module(void);
 
@@ -404,10 +401,7 @@ struct toklist {
 	char *string;
 };
 
-static struct toklist dgap_tlist[] = {
-	{ BEGIN,	"config_begin" },
-	{ END,		"config_end" },
-	{ BOARD,	"board"	},
+static struct toklist dgap_brdtype[] = {
 	{ PCX,		"Digi_AccelePort_C/X_PCI" },
 	{ PEPC,		"Digi_AccelePort_EPC/X_PCI" },
 	{ PPCM,		"Digi_AccelePort_Xem_PCI" },
@@ -416,6 +410,13 @@ static struct toklist dgap_tlist[] = {
 	{ APORT8_920P,	"Digi_AccelePort_8r_920_PCI" },
 	{ PAPORT4,	"Digi_AccelePort_4r_PCI(EIA-232/RS-422)" },
 	{ PAPORT8,	"Digi_AccelePort_8r_PCI(EIA-232/RS-422)" },
+	{ 0, NULL }
+};
+
+static struct toklist dgap_tlist[] = {
+	{ BEGIN,	"config_begin" },
+	{ END,		"config_end" },
+	{ BOARD,	"board"	},
 	{ IO,		"io" },
 	{ PCIINFO,	"pciinfo" },
 	{ LINE,		"line" },
@@ -619,6 +620,7 @@ unregister_tty:
 free_flipbuf:
 	dgap_free_flipbuf(brd);
 cleanup_brd:
+	dgap_cleanup_nodes();
 	dgap_release_remap(brd);
 	kfree(brd);
 
@@ -659,6 +661,8 @@ static void dgap_cleanup_module(void)
 		dgap_cleanup_board(dgap_board[i]);
 	}
 
+	dgap_cleanup_nodes();
+
 	if (dgap_numboards)
 		pci_unregister_driver(&dgap_driver);
 }
@@ -679,17 +683,7 @@ static void dgap_cleanup_board(struct board_t *brd)
 
 	tasklet_kill(&brd->helper_tasklet);
 
-	if (brd->re_map_port) {
-		release_mem_region(brd->membase + 0x200000, 0x200000);
-		iounmap(brd->re_map_port);
-		brd->re_map_port = NULL;
-	}
-
-	if (brd->re_map_membase) {
-		release_mem_region(brd->membase, 0x200000);
-		iounmap(brd->re_map_membase);
-		brd->re_map_membase = NULL;
-	}
+	dgap_release_remap(brd);
 
 	/* Free all allocated channels structs */
 	for (i = 0; i < MAXPORTS ; i++)
@@ -738,7 +732,6 @@ static struct board_t *dgap_found_board(struct pci_dev *pdev, int id,
 
 	spin_lock_init(&brd->bd_lock);
 
-	brd->runwait		= 0;
 	brd->inhibit_poller	= FALSE;
 	brd->wait_for_bios	= 0;
 	brd->wait_for_fep	= 0;
@@ -834,23 +827,16 @@ static int dgap_request_irq(struct board_t *brd)
 	if (!brd || brd->magic != DGAP_BOARD_MAGIC)
 		return -ENODEV;
 
-	brd->use_interrupts = dgap_config_get_useintr(brd);
-
 	/*
 	 * Set up our interrupt handler if we are set to do interrupts.
 	 */
-	if (brd->use_interrupts && brd->irq) {
+	if (dgap_config_get_useintr(brd) && brd->irq) {
 
 		rc = request_irq(brd->irq, dgap_intr, IRQF_SHARED, "DGAP", brd);
 
-		if (rc)
-			brd->intr_used = 0;
-		else
+		if (!rc)
 			brd->intr_used = 1;
-	} else {
-		brd->intr_used = 0;
 	}
-
 	return 0;
 }
 
@@ -875,7 +861,7 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
 		ret = request_firmware(&fw, fw_info[card_type].conf_name,
 					 &pdev->dev);
 		if (ret) {
-			pr_err("dgap: config file %s not found\n",
+			dev_err(&pdev->dev, "config file %s not found\n",
 				fw_info[card_type].conf_name);
 			return ret;
 		}
@@ -920,7 +906,7 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
 			dgap_find_config(PAPORT4, brd->pci_bus, brd->pci_slot);
 
 	if (!brd->bd_config) {
-		pr_err("dgap: No valid configuration found\n");
+		dev_err(&pdev->dev, "No valid configuration found\n");
 		return -EINVAL;
 	}
 
@@ -928,7 +914,7 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
 		ret = request_firmware(&fw, fw_info[card_type].bios_name,
 					&pdev->dev);
 		if (ret) {
-			pr_err("dgap: bios file %s not found\n",
+			dev_err(&pdev->dev, "bios file %s not found\n",
 				fw_info[card_type].bios_name);
 			return ret;
 		}
@@ -945,7 +931,7 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
 		ret = request_firmware(&fw, fw_info[card_type].fep_name,
 					&pdev->dev);
 		if (ret) {
-			pr_err("dgap: fep file %s not found\n",
+			dev_err(&pdev->dev, "dgap: fep file %s not found\n",
 				fw_info[card_type].fep_name);
 			return ret;
 		}
@@ -974,7 +960,7 @@ static int dgap_firmware_load(struct pci_dev *pdev, int card_type,
 		ret = request_firmware(&fw, fw_info[card_type].con_name,
 					&pdev->dev);
 		if (ret) {
-			pr_err("dgap: conc file %s not found\n",
+			dev_err(&pdev->dev, "conc file %s not found\n",
 				fw_info[card_type].con_name);
 			return ret;
 		}
@@ -1027,9 +1013,15 @@ static int dgap_do_remap(struct board_t *brd)
 
 static void dgap_release_remap(struct board_t *brd)
 {
-	release_mem_region(brd->membase, 0x200000);
-	release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
-	iounmap(brd->re_map_membase);
+	if (brd->re_map_membase) {
+		release_mem_region(brd->membase, 0x200000);
+		iounmap(brd->re_map_membase);
+	}
+
+	if (brd->re_map_port) {
+		release_mem_region(brd->membase + PCI_IO_OFFSET, 0x200000);
+		iounmap(brd->re_map_port);
+	}
 }
 /*****************************************************************************
 *
@@ -1202,26 +1194,6 @@ static void dgap_init_globals(void)
 
 /************************************************************************
  *
- * Utility functions
- *
- ************************************************************************/
-
-/*
- * dgap_ms_sleep()
- *
- * Put the driver to sleep for x ms's
- *
- * Returns 0 if timed out, !0 (showing signal) if interrupted by a signal.
- */
-static int dgap_ms_sleep(ulong ms)
-{
-	current->state = TASK_INTERRUPTIBLE;
-	schedule_timeout((ms * HZ) / 1000);
-	return signal_pending(current);
-}
-
-/************************************************************************
- *
  * TTY Initialization/Cleanup Functions
  *
  ************************************************************************/
@@ -1316,11 +1288,9 @@ static int dgap_tty_register(struct board_t *brd)
 	if (rc < 0)
 		goto unregister_serial_drv;
 
-	brd->dgap_major_serial_registered = TRUE;
 	dgap_boards_by_major[brd->serial_driver->major] = brd;
 	brd->dgap_serial_major = brd->serial_driver->major;
 
-	brd->dgap_major_transparent_print_registered = TRUE;
 	dgap_boards_by_major[brd->print_driver->major] = brd;
 	brd->dgap_transparent_print_major = brd->print_driver->major;
 
@@ -1378,19 +1348,17 @@ static int dgap_tty_init(struct board_t *brd)
 		brd->nasync = brd->maxports;
 
 	if (true_count != brd->nasync) {
-		if ((brd->type == PPCM) && (true_count == 64)) {
-			pr_warn("dgap: %s configured for %d ports, has %d ports.\n",
-				brd->name, brd->nasync, true_count);
-			pr_warn("dgap: Please make SURE the EBI cable running from the card\n");
-			pr_warn("dgap: to each EM module is plugged into EBI IN!\n");
-		} else if ((brd->type == PPCM) && (true_count == 0)) {
-			pr_warn("dgap: %s configured for %d ports, has %d ports.\n",
-				brd->name, brd->nasync, true_count);
-			pr_warn("dgap: Please make SURE the EBI cable running from the card\n");
-			pr_warn("dgap: to each EM module is plugged into EBI IN!\n");
-		} else
-			pr_warn("dgap: %s configured for %d ports, has %d ports.\n",
-				brd->name, brd->nasync, true_count);
+		dev_warn(&brd->pdev->dev,
+			 "%s configured for %d ports, has %d ports.\n",
+			 brd->name, brd->nasync, true_count);
+
+		if ((brd->type == PPCM) &&
+		    (true_count == 64 || true_count == 0)) {
+			dev_warn(&brd->pdev->dev,
+				 "Please make SURE the EBI cable running from the card\n");
+			dev_warn(&brd->pdev->dev,
+				 "to each EM module is plugged into EBI IN!\n");
+		}
 
 		brd->nasync = true_count;
 
@@ -1468,9 +1436,6 @@ static int dgap_tty_init(struct board_t *brd)
 		ch->ch_tstart = 0;
 		ch->ch_rstart = 0;
 
-		/* .25 second delay */
-		ch->ch_close_delay = 250;
-
 		/*
 		 * Set queue water marks, interrupt mask,
 		 * and general tty parameters.
@@ -1540,35 +1505,29 @@ static void dgap_cleanup_tty(struct board_t *brd)
 	struct device *dev;
 	int i;
 
-	if (brd->dgap_major_serial_registered) {
-		dgap_boards_by_major[brd->serial_driver->major] = NULL;
-		brd->dgap_serial_major = 0;
-		for (i = 0; i < brd->nasync; i++) {
-			tty_port_destroy(&brd->serial_ports[i]);
-			dev = brd->channels[i]->ch_tun.un_sysfs;
-			dgap_remove_tty_sysfs(dev);
-			tty_unregister_device(brd->serial_driver, i);
-		}
-		tty_unregister_driver(brd->serial_driver);
-		put_tty_driver(brd->serial_driver);
-		kfree(brd->serial_ports);
-		brd->dgap_major_serial_registered = FALSE;
-	}
-
-	if (brd->dgap_major_transparent_print_registered) {
-		dgap_boards_by_major[brd->print_driver->major] = NULL;
-		brd->dgap_transparent_print_major = 0;
-		for (i = 0; i < brd->nasync; i++) {
-			tty_port_destroy(&brd->printer_ports[i]);
-			dev = brd->channels[i]->ch_pun.un_sysfs;
-			dgap_remove_tty_sysfs(dev);
-			tty_unregister_device(brd->print_driver, i);
-		}
-		tty_unregister_driver(brd->print_driver);
-		put_tty_driver(brd->print_driver);
-		kfree(brd->printer_ports);
-		brd->dgap_major_transparent_print_registered = FALSE;
+	dgap_boards_by_major[brd->serial_driver->major] = NULL;
+	brd->dgap_serial_major = 0;
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_destroy(&brd->serial_ports[i]);
+		dev = brd->channels[i]->ch_tun.un_sysfs;
+		dgap_remove_tty_sysfs(dev);
+		tty_unregister_device(brd->serial_driver, i);
+	}
+	tty_unregister_driver(brd->serial_driver);
+	put_tty_driver(brd->serial_driver);
+	kfree(brd->serial_ports);
+
+	dgap_boards_by_major[brd->print_driver->major] = NULL;
+	brd->dgap_transparent_print_major = 0;
+	for (i = 0; i < brd->nasync; i++) {
+		tty_port_destroy(&brd->printer_ports[i]);
+		dev = brd->channels[i]->ch_pun.un_sysfs;
+		dgap_remove_tty_sysfs(dev);
+		tty_unregister_device(brd->print_driver, i);
 	}
+	tty_unregister_driver(brd->print_driver);
+	put_tty_driver(brd->print_driver);
+	kfree(brd->printer_ports);
 }
 
 /*=======================================================================
@@ -2309,12 +2268,13 @@ static void dgap_tty_close(struct tty_struct *tty, struct file *file)
 			 * Go to sleep to ensure RTS/DTR
 			 * have been dropped for modems to see it.
 			 */
-			if (ch->ch_close_delay) {
-				spin_unlock_irqrestore(&ch->ch_lock,
-						       lock_flags);
-				dgap_ms_sleep(ch->ch_close_delay);
-				spin_lock_irqsave(&ch->ch_lock, lock_flags);
-			}
+			spin_unlock_irqrestore(&ch->ch_lock,
+					lock_flags);
+
+			/* .25 second delay for dropping RTS/DTR */
+			schedule_timeout_interruptible(msecs_to_jiffies(250));
+
+			spin_lock_irqsave(&ch->ch_lock, lock_flags);
 		}
 
 		ch->pscan_state = 0;
@@ -3090,8 +3050,9 @@ static int dgap_get_modem_info(struct channel_t *ch, unsigned int __user *value)
  *
  * Set modem signals, called by ld.
  */
-static int dgap_set_modem_info(struct channel_t *ch, struct board_t *bd, struct un_t *un,
-			       unsigned int command, unsigned int __user *value)
+static int dgap_set_modem_info(struct channel_t *ch, struct board_t *bd,
+			       struct un_t *un, unsigned int command,
+			       unsigned int __user *value)
 {
 	int ret;
 	unsigned int arg;
@@ -3167,7 +3128,8 @@ static int dgap_set_modem_info(struct channel_t *ch, struct board_t *bd, struct
  *
  *
  */
-static int dgap_tty_digigeta(struct channel_t *ch, struct digi_t __user *retinfo)
+static int dgap_tty_digigeta(struct channel_t *ch,
+			     struct digi_t __user *retinfo)
 {
 	struct digi_t tmp;
 	ulong lock_flags;
@@ -4215,7 +4177,7 @@ static int dgap_test_bios(struct board_t *brd)
 	/* Gave up on board after too long of time taken */
 	err1 = readw(addr + SEQUENCE);
 	err2 = readw(addr + ERROR);
-	pr_warn("dgap: %s failed diagnostics.  Error #(%x,%x).\n",
+	dev_warn(&brd->pdev->dev, "%s failed diagnostics.  Error #(%x,%x).\n",
 		brd->name, err1, err2);
 	brd->state = BOARD_FAILED;
 	brd->dpastatus = BD_NOBIOS;
@@ -4310,8 +4272,9 @@ static int dgap_test_fep(struct board_t *brd)
 	/* Gave up on board after too long of time taken */
 	err1 = readw(addr + SEQUENCE);
 	err2 = readw(addr + ERROR);
-	pr_warn("dgap: FEPOS for %s not functioning.  Error #(%x,%x).\n",
-		brd->name, err1, err2);
+	dev_warn(&brd->pdev->dev,
+		 "FEPOS for %s not functioning.  Error #(%x,%x).\n",
+		 brd->name, err1, err2);
 	brd->state = BOARD_FAILED;
 	brd->dpastatus = BD_NOFEP;
 
@@ -4343,7 +4306,8 @@ static void dgap_do_reset_board(struct board_t *brd)
 
 	}
 	if (i > 1000) {
-		pr_warn("dgap: Board not resetting...  Failing board.\n");
+		dev_warn(&brd->pdev->dev,
+			 "dgap: Board not resetting...  Failing board.\n");
 		brd->state = BOARD_FAILED;
 		brd->dpastatus = BD_NOFEP;
 		return;
@@ -4358,8 +4322,9 @@ static void dgap_do_reset_board(struct board_t *brd)
 	check2 = readl(brd->re_map_membase + HIGHMEM);
 
 	if ((check1 != 0xa55a3cc3) || (check2 != 0x5aa5c33c)) {
-		pr_warn("dgap: No memory at %p for board.\n",
-			brd->re_map_membase);
+		dev_warn(&brd->pdev->dev,
+			 "No memory at %p for board.\n",
+			 brd->re_map_membase);
 		brd->state = BOARD_FAILED;
 		brd->dpastatus = BD_NOFEP;
 		return;
@@ -6323,6 +6288,54 @@ static void dgap_remove_tty_sysfs(struct device *c)
 	sysfs_remove_group(&c->kobj, &dgap_tty_attribute_group);
 }
 
+static void dgap_cleanup_nodes(void)
+{
+	struct cnode *p;
+
+	p = &dgap_head;
+
+	while (p) {
+		struct cnode *tmp = p->next;
+
+		if (p->type == NULLNODE) {
+			p = tmp;
+			continue;
+		}
+
+		switch (p->type) {
+		case BNODE:
+			kfree(p->u.board.portstr);
+			kfree(p->u.board.addrstr);
+			kfree(p->u.board.pcibusstr);
+			kfree(p->u.board.pcislotstr);
+			kfree(p->u.board.method);
+			break;
+		case CNODE:
+			kfree(p->u.conc.id);
+			kfree(p->u.conc.connect);
+			break;
+		case MNODE:
+			kfree(p->u.module.id);
+			break;
+		case TNODE:
+			kfree(p->u.ttyname);
+			break;
+		case CUNODE:
+			kfree(p->u.cuname);
+			break;
+		case LNODE:
+			kfree(p->u.line.cable);
+			break;
+		case PNODE:
+			kfree(p->u.printname);
+			break;
+		}
+
+		kfree(p->u.board.status);
+		kfree(p);
+		p = tmp;
+	}
+}
 /*
  * Parse a configuration file read into memory as a string.
  */
@@ -6343,21 +6356,25 @@ static int dgap_parsefile(char **in)
 	/* file must start with a BEGIN */
 	while ((rc = dgap_gettok(in)) != BEGIN) {
 		if (rc == 0) {
-			dgap_err("unexpected EOF");
+			pr_err("unexpected EOF");
 			return -1;
 		}
 	}
 
 	for (; ;) {
+		int board_type = 0;
+		int conc_type = 0;
+		int module_type = 0;
+
 		rc = dgap_gettok(in);
 		if (rc == 0) {
-			dgap_err("unexpected EOF");
+			pr_err("unexpected EOF");
 			return -1;
 		}
 
 		switch (rc) {
 		case BEGIN:	/* should only be 1 begin */
-			dgap_err("unexpected config_begin\n");
+			pr_err("unexpected config_begin\n");
 			return -1;
 
 		case END:
@@ -6368,10 +6385,9 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
+
 			p = p->next;
 
 			p->type = BNODE;
@@ -6379,103 +6395,30 @@ static int dgap_parsefile(char **in)
 			line = conc = NULL;
 			brd = p;
 			linecnt = -1;
-			break;
-
-		case APORT2_920P:	/* AccelePort_4 */
-			if (p->type != BNODE) {
-				dgap_err("unexpected Digi_2r_920 string");
-				return -1;
-			}
-			p->u.board.type = APORT2_920P;
-			p->u.board.v_type = 1;
-			break;
 
-		case APORT4_920P:	/* AccelePort_4 */
-			if (p->type != BNODE) {
-				dgap_err("unexpected Digi_4r_920 string");
+			board_type = dgap_gettok(in);
+			if (board_type == 0) {
+				pr_err("board !!type not specified");
 				return -1;
 			}
-			p->u.board.type = APORT4_920P;
-			p->u.board.v_type = 1;
-			break;
 
-		case APORT8_920P:	/* AccelePort_8 */
-			if (p->type != BNODE) {
-				dgap_err("unexpected Digi_8r_920 string");
-				return -1;
-			}
-			p->u.board.type = APORT8_920P;
-			p->u.board.v_type = 1;
-			break;
+			p->u.board.type = board_type;
 
-		case PAPORT4:	/* AccelePort_4 PCI */
-			if (p->type != BNODE) {
-				dgap_err("unexpected Digi_4r(PCI) string");
-				return -1;
-			}
-			p->u.board.type = PAPORT4;
-			p->u.board.v_type = 1;
-			break;
-
-		case PAPORT8:	/* AccelePort_8 PCI */
-			if (p->type != BNODE) {
-				dgap_err("unexpected Digi_8r string");
-				return -1;
-			}
-			p->u.board.type = PAPORT8;
-			p->u.board.v_type = 1;
-			break;
-
-		case PCX:	/* PCI C/X */
-			if (p->type != BNODE) {
-				dgap_err("unexpected Digi_C/X_(PCI) string");
-				return -1;
-			}
-			p->u.board.type = PCX;
-			p->u.board.v_type = 1;
-			p->u.board.conc1 = 0;
-			p->u.board.conc2 = 0;
-			p->u.board.module1 = 0;
-			p->u.board.module2 = 0;
-			break;
-
-		case PEPC:	/* PCI EPC/X */
-			if (p->type != BNODE) {
-				dgap_err("unexpected \"Digi_EPC/X_(PCI)\" string");
-				return -1;
-			}
-			p->u.board.type = PEPC;
-			p->u.board.v_type = 1;
-			p->u.board.conc1 = 0;
-			p->u.board.conc2 = 0;
-			p->u.board.module1 = 0;
-			p->u.board.module2 = 0;
-			break;
-
-		case PPCM:	/* PCI/Xem */
-			if (p->type != BNODE) {
-				dgap_err("unexpected PCI/Xem string");
-				return -1;
-			}
-			p->u.board.type = PPCM;
-			p->u.board.v_type = 1;
-			p->u.board.conc1 = 0;
-			p->u.board.conc2 = 0;
 			break;
 
 		case IO:	/* i/o port */
 			if (p->type != BNODE) {
-				dgap_err("IO port only vaild for boards");
+				pr_err("IO port only vaild for boards");
 				return -1;
 			}
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			p->u.board.portstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtol(s, 0, &p->u.board.port)) {
-				dgap_err("bad number for IO port");
+				pr_err("bad number for IO port");
 				return -1;
 			}
 			p->u.board.v_port = 1;
@@ -6483,17 +6426,17 @@ static int dgap_parsefile(char **in)
 
 		case MEM:	/* memory address */
 			if (p->type != BNODE) {
-				dgap_err("memory address only vaild for boards");
+				pr_err("memory address only vaild for boards");
 				return -1;
 			}
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			p->u.board.addrstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.addr)) {
-				dgap_err("bad number for memory address");
+				pr_err("bad number for memory address");
 				return -1;
 			}
 			p->u.board.v_addr = 1;
@@ -6501,28 +6444,28 @@ static int dgap_parsefile(char **in)
 
 		case PCIINFO:	/* pci information */
 			if (p->type != BNODE) {
-				dgap_err("memory address only vaild for boards");
+				pr_err("memory address only vaild for boards");
 				return -1;
 			}
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			p->u.board.pcibusstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.pcibus)) {
-				dgap_err("bad number for pci bus");
+				pr_err("bad number for pci bus");
 				return -1;
 			}
 			p->u.board.v_pcibus = 1;
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			p->u.board.pcislotstr = kstrdup(s, GFP_KERNEL);
 			if (kstrtoul(s, 0, &p->u.board.pcislot)) {
-				dgap_err("bad number for pci slot");
+				pr_err("bad number for pci slot");
 				return -1;
 			}
 			p->u.board.v_pcislot = 1;
@@ -6530,12 +6473,12 @@ static int dgap_parsefile(char **in)
 
 		case METHOD:
 			if (p->type != BNODE) {
-				dgap_err("install method only vaild for boards");
+				pr_err("install method only vaild for boards");
 				return -1;
 			}
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			p->u.board.method = kstrdup(s, GFP_KERNEL);
@@ -6544,12 +6487,12 @@ static int dgap_parsefile(char **in)
 
 		case STATUS:
 			if (p->type != BNODE) {
-				dgap_err("config status only vaild for boards");
+				pr_err("config status only vaild for boards");
 				return -1;
 			}
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			p->u.board.status = kstrdup(s, GFP_KERNEL);
@@ -6559,38 +6502,38 @@ static int dgap_parsefile(char **in)
 			if (p->type == BNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.board.nport)) {
-					dgap_err("bad number for number of ports");
+					pr_err("bad number for number of ports");
 					return -1;
 				}
 				p->u.board.v_nport = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.conc.nport)) {
-					dgap_err("bad number for number of ports");
+					pr_err("bad number for number of ports");
 					return -1;
 				}
 				p->u.conc.v_nport = 1;
 			} else if (p->type == MNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.module.nport)) {
-					dgap_err("bad number for number of ports");
+					pr_err("bad number for number of ports");
 					return -1;
 				}
 				p->u.module.v_nport = 1;
 			} else {
-				dgap_err("nports only valid for concentrators or modules");
+				pr_err("nports only valid for concentrators or modules");
 				return -1;
 			}
 			break;
@@ -6598,7 +6541,7 @@ static int dgap_parsefile(char **in)
 		case ID:	/* letter ID used in tty name */
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 
@@ -6611,7 +6554,7 @@ static int dgap_parsefile(char **in)
 				p->u.module.id = kstrdup(s, GFP_KERNEL);
 				p->u.module.v_id = 1;
 			} else {
-				dgap_err("id only valid for concentrators or modules");
+				pr_err("id only valid for concentrators or modules");
 				return -1;
 			}
 			break;
@@ -6620,38 +6563,38 @@ static int dgap_parsefile(char **in)
 			if (p->type == BNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.board.start)) {
-					dgap_err("bad number for start of tty count");
+					pr_err("bad number for start of tty count");
 					return -1;
 				}
 				p->u.board.v_start = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.conc.start)) {
-					dgap_err("bad number for start of tty count");
+					pr_err("bad number for start of tty count");
 					return -1;
 				}
 				p->u.conc.v_start = 1;
 			} else if (p->type == MNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.module.start)) {
-					dgap_err("bad number for start of tty count");
+					pr_err("bad number for start of tty count");
 					return -1;
 				}
 				p->u.module.v_start = 1;
 			} else {
-				dgap_err("start only valid for concentrators or modules");
+				pr_err("start only valid for concentrators or modules");
 				return -1;
 			}
 			break;
@@ -6661,24 +6604,21 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = TNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpeced end of file");
+				pr_err("unexpeced end of file");
 				return -1;
 			}
 			p->u.ttyname = kstrdup(s, GFP_KERNEL);
-			if (!p->u.ttyname) {
-				dgap_err("out of memory");
+			if (!p->u.ttyname)
 				return -1;
-			}
+
 			break;
 
 		case CU:	/* cu name prefix */
@@ -6686,44 +6626,39 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = CUNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpeced end of file");
+				pr_err("unexpeced end of file");
 				return -1;
 			}
 			p->u.cuname = kstrdup(s, GFP_KERNEL);
-			if (!p->u.cuname) {
-				dgap_err("out of memory");
+			if (!p->u.cuname)
 				return -1;
-			}
+
 			break;
 
 		case LINE:	/* line information */
 			if (dgap_checknode(p))
 				return -1;
 			if (!brd) {
-				dgap_err("must specify board before line info");
+				pr_err("must specify board before line info");
 				return -1;
 			}
 			switch (brd->u.board.type) {
 			case PPCM:
-				dgap_err("line not vaild for PC/em");
+				pr_err("line not vaild for PC/em");
 				return -1;
 			}
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = LNODE;
@@ -6736,15 +6671,13 @@ static int dgap_parsefile(char **in)
 			if (dgap_checknode(p))
 				return -1;
 			if (!line) {
-				dgap_err("must specify line info before concentrator");
+				pr_err("must specify line info before concentrator");
 				return -1;
 			}
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = CNODE;
@@ -6755,31 +6688,22 @@ static int dgap_parsefile(char **in)
 			else
 				brd->u.board.conc1++;
 
-			break;
-
-		case CX:	/* c/x type concentrator */
-			if (p->type != CNODE) {
-				dgap_err("cx only valid for concentrators");
+			conc_type = dgap_gettok(in);
+			if (conc_type == 0 || conc_type != CX ||
+			    conc_type != EPC) {
+				pr_err("failed to set a type of concentratros");
 				return -1;
 			}
-			p->u.conc.type = CX;
-			p->u.conc.v_type = 1;
-			break;
 
-		case EPC:	/* epc type concentrator */
-			if (p->type != CNODE) {
-				dgap_err("cx only valid for concentrators");
-				return -1;
-			}
-			p->u.conc.type = EPC;
-			p->u.conc.v_type = 1;
+			p->u.conc.type = conc_type;
+
 			break;
 
 		case MOD:	/* EBI module */
 			if (dgap_checknode(p))
 				return -1;
 			if (!brd) {
-				dgap_err("must specify board info before EBI modules");
+				pr_err("must specify board info before EBI modules");
 				return -1;
 			}
 			switch (brd->u.board.type) {
@@ -6788,16 +6712,15 @@ static int dgap_parsefile(char **in)
 				break;
 			default:
 				if (!conc) {
-					dgap_err("must specify concentrator info before EBI module");
+					pr_err("must specify concentrator info before EBI module");
 					return -1;
 				}
 			}
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
+
 			p = p->next;
 			p->type = MNODE;
 
@@ -6806,31 +6729,22 @@ static int dgap_parsefile(char **in)
 			else
 				brd->u.board.module1++;
 
-			break;
-
-		case PORTS:	/* ports type EBI module */
-			if (p->type != MNODE) {
-				dgap_err("ports only valid for EBI modules");
+			module_type = dgap_gettok(in);
+			if (module_type == 0 || module_type != PORTS ||
+			    module_type != MODEM) {
+				pr_err("failed to set a type of module");
 				return -1;
 			}
-			p->u.module.type = PORTS;
-			p->u.module.v_type = 1;
-			break;
 
-		case MODEM:	/* ports type EBI module */
-			if (p->type != MNODE) {
-				dgap_err("modem only valid for modem modules");
-				return -1;
-			}
-			p->u.module.type = MODEM;
-			p->u.module.v_type = 1;
+			p->u.module.type = module_type;
+
 			break;
 
 		case CABLE:
 			if (p->type == LNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				p->u.line.cable = kstrdup(s, GFP_KERNEL);
@@ -6842,27 +6756,27 @@ static int dgap_parsefile(char **in)
 			if (p->type == LNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.line.speed)) {
-					dgap_err("bad number for line speed");
+					pr_err("bad number for line speed");
 					return -1;
 				}
 				p->u.line.v_speed = 1;
 			} else if (p->type == CNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				if (kstrtol(s, 0, &p->u.conc.speed)) {
-					dgap_err("bad number for line speed");
+					pr_err("bad number for line speed");
 					return -1;
 				}
 				p->u.conc.v_speed = 1;
 			} else {
-				dgap_err("speed valid only for lines or concentrators.");
+				pr_err("speed valid only for lines or concentrators.");
 				return -1;
 			}
 			break;
@@ -6871,7 +6785,7 @@ static int dgap_parsefile(char **in)
 			if (p->type == CNODE) {
 				s = dgap_getword(in);
 				if (!s) {
-					dgap_err("unexpected end of file");
+					pr_err("unexpected end of file");
 					return -1;
 				}
 				p->u.conc.connect = kstrdup(s, GFP_KERNEL);
@@ -6883,24 +6797,21 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = PNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpeced end of file");
+				pr_err("unexpeced end of file");
 				return -1;
 			}
 			p->u.printname = kstrdup(s, GFP_KERNEL);
-			if (!p->u.printname) {
-				dgap_err("out of memory");
+			if (!p->u.printname)
 				return -1;
-			}
+
 			break;
 
 		case CMAJOR:	/* major number */
@@ -6908,21 +6819,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = JNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.majornumber)) {
-				dgap_err("bad number for major number");
+				pr_err("bad number for major number");
 				return -1;
 			}
 			break;
@@ -6932,21 +6841,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = ANODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.altpin)) {
-				dgap_err("bad number for altpin");
+				pr_err("bad number for altpin");
 				return -1;
 			}
 			break;
@@ -6956,19 +6863,18 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
+
 			p = p->next;
 			p->type = INTRNODE;
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.useintr)) {
-				dgap_err("bad number for useintr");
+				pr_err("bad number for useintr");
 				return -1;
 			}
 			break;
@@ -6978,21 +6884,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = TSNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.ttysize)) {
-				dgap_err("bad number for ttysize");
+				pr_err("bad number for ttysize");
 				return -1;
 			}
 			break;
@@ -7002,21 +6906,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = CSNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.chsize)) {
-				dgap_err("bad number for chsize");
+				pr_err("bad number for chsize");
 				return -1;
 			}
 			break;
@@ -7026,21 +6928,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = BSNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.bssize)) {
-				dgap_err("bad number for bssize");
+				pr_err("bad number for bssize");
 				return -1;
 			}
 			break;
@@ -7050,21 +6950,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = USNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.unsize)) {
-				dgap_err("bad number for schedsize");
+				pr_err("bad number for schedsize");
 				return -1;
 			}
 			break;
@@ -7074,21 +6972,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = FSNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.f2size)) {
-				dgap_err("bad number for f2200size");
+				pr_err("bad number for f2200size");
 				return -1;
 			}
 			break;
@@ -7098,21 +6994,19 @@ static int dgap_parsefile(char **in)
 				return -1;
 
 			p->next = kzalloc(sizeof(struct cnode), GFP_KERNEL);
-			if (!p->next) {
-				dgap_err("out of memory");
+			if (!p->next)
 				return -1;
-			}
 
 			p = p->next;
 			p->type = VSNODE;
 
 			s = dgap_getword(in);
 			if (!s) {
-				dgap_err("unexpected end of file");
+				pr_err("unexpected end of file");
 				return -1;
 			}
 			if (kstrtol(s, 0, &p->u.vpixsize)) {
-				dgap_err("bad number for vpixsize");
+				pr_err("bad number for vpixsize");
 				return -1;
 			}
 			break;
@@ -7165,12 +7059,10 @@ static int dgap_gettok(char **in)
 	if (strstr(dgap_cword, "board")) {
 		w = dgap_getword(in);
 		snprintf(dgap_cword, MAXCWORD, "%s", w);
-		for (t = dgap_tlist; t->token != 0; t++) {
+		for (t = dgap_brdtype; t->token != 0; t++) {
 			if (!strcmp(w, t->string))
 				return t->token;
 		}
-		dgap_err("board !!type not specified");
-		return 1;
 	} else {
 		while ((w = dgap_getword(in))) {
 			snprintf(dgap_cword, MAXCWORD, "%s", w);
@@ -7179,8 +7071,9 @@ static int dgap_gettok(char **in)
 					return t->token;
 			}
 		}
-		return 0;
 	}
+
+	return 0;
 }
 
 /*
@@ -7213,66 +7106,41 @@ static char *dgap_getword(char **in)
 }
 
 /*
- * print an error message, giving the line number in the file where
- * the error occurred.
- */
-static void dgap_err(char *s)
-{
-	pr_err("dgap: parse: %s\n", s);
-}
-
-/*
  * dgap_checknode: see if all the necessary info has been supplied for a node
  * before creating the next node.
  */
 static int dgap_checknode(struct cnode *p)
 {
 	switch (p->type) {
-	case BNODE:
-		if (p->u.board.v_type == 0) {
-			dgap_err("board type !not specified");
-			return 1;
-		}
-
-		return 0;
-
 	case LNODE:
 		if (p->u.line.v_speed == 0) {
-			dgap_err("line speed not specified");
+			pr_err("line speed not specified");
 			return 1;
 		}
 		return 0;
 
 	case CNODE:
-		if (p->u.conc.v_type == 0) {
-			dgap_err("concentrator type not specified");
-			return 1;
-		}
 		if (p->u.conc.v_speed == 0) {
-			dgap_err("concentrator line speed not specified");
+			pr_err("concentrator line speed not specified");
 			return 1;
 		}
 		if (p->u.conc.v_nport == 0) {
-			dgap_err("number of ports on concentrator not specified");
+			pr_err("number of ports on concentrator not specified");
 			return 1;
 		}
 		if (p->u.conc.v_id == 0) {
-			dgap_err("concentrator id letter not specified");
+			pr_err("concentrator id letter not specified");
 			return 1;
 		}
 		return 0;
 
 	case MNODE:
-		if (p->u.module.v_type == 0) {
-			dgap_err("EBI module type not specified");
-			return 1;
-		}
 		if (p->u.module.v_nport == 0) {
-			dgap_err("number of ports on EBI module not specified");
+			pr_err("number of ports on EBI module not specified");
 			return 1;
 		}
 		if (p->u.module.v_id == 0) {
-			dgap_err("EBI module id letter not specified");
+			pr_err("EBI module id letter not specified");
 			return 1;
 		}
 		return 0;
diff --git a/drivers/staging/dgap/dgap.h b/drivers/staging/dgap/dgap.h
index 9728d59c94d1..14e2ed0fe39b 100644
--- a/drivers/staging/dgap/dgap.h
+++ b/drivers/staging/dgap/dgap.h
@@ -558,7 +558,6 @@ struct board_t {
 
 	u16		nasync;		/* Number of ports on card */
 
-	u32		use_interrupts;	/* Should we be interrupt driven? */
 	ulong		irq;		/* Interrupt request number */
 	ulong		intr_count;	/* Count of interrupts */
 	u32		intr_used;	/* Non-zero if using interrupts */
@@ -573,7 +572,6 @@ struct board_t {
 	u8 __iomem	*re_map_port;	/* Remapped io port of the card */
 	u8 __iomem	*re_map_membase;/* Remapped memory of the card */
 
-	u8		runwait;	/* # Processes waiting for FEP  */
 	u8		inhibit_poller; /* Tells the poller to leave us alone */
 
 	struct channel_t *channels[MAXPORTS]; /* array of pointers to our */
@@ -586,9 +584,6 @@ struct board_t {
 	struct tty_port *printer_ports;
 	char		print_name[200];
 
-	u32		dgap_major_serial_registered;
-	u32		dgap_major_transparent_print_registered;
-
 	u32		dgap_serial_major;
 	u32		dgap_transparent_print_major;
 
@@ -986,9 +981,6 @@ struct channel_t {
 	u32 ch_open_count;		/* open count			*/
 	u32	ch_flags;		/* Channel flags                */
 
-	u32	ch_close_delay;		/* How long we should drop      */
-					/* RTS/DTR for                  */
-
 	u32	ch_cpstime;		/* Time for CPS calculations    */
 
 	tcflag_t ch_c_iflag;		/* channel iflags               */
@@ -1175,7 +1167,6 @@ struct cnode {
 			char  *id;	/* tty id		*/
 			long  start;	/* start of tty counting */
 			char  *method;  /* Install method       */
-			char  v_type;
 			char  v_port;
 			char  v_addr;
 			char  v_pcibus;
@@ -1210,7 +1201,6 @@ struct cnode {
 			char  *id;
 			char  *idstr;
 			long  start;
-			char  v_type;
 			char  v_connect;
 			char  v_speed;
 			char  v_nport;
@@ -1224,7 +1214,6 @@ struct cnode {
 			char *id;
 			char *idstr;
 			long start;
-			char v_type;
 			char v_nport;
 			char v_id;
 			char v_start;
diff --git a/drivers/staging/dgnc/Makefile b/drivers/staging/dgnc/Makefile
index 733434f63700..b69f7b6b1143 100644
--- a/drivers/staging/dgnc/Makefile
+++ b/drivers/staging/dgnc/Makefile
@@ -4,4 +4,5 @@ obj-$(CONFIG_DGNC) += dgnc.o
 
 dgnc-objs :=   dgnc_cls.o dgnc_driver.o\
                dgnc_mgmt.o dgnc_neo.o\
-               dgnc_tty.o dgnc_sysfs.o
+               dgnc_tty.o dgnc_sysfs.o\
+	       dgnc_utils.o
diff --git a/drivers/staging/dgnc/TODO b/drivers/staging/dgnc/TODO
index 1ff2d1874aa2..22adff1078f3 100644
--- a/drivers/staging/dgnc/TODO
+++ b/drivers/staging/dgnc/TODO
@@ -1,11 +1,4 @@
-* remove kzalloc casts
 * checkpatch fixes
-* sparse fixes
-* fix use of sizeof(). Example replace sizeof(struct board_t) 
-  with sizeof(*brd) and remove sizeof(char)
-* change name of board_t to dgnc_board
-* split two assignments into the two assignments on two lines;
-  don't use two equals signs
 * remove unecessary comments
 * remove unecessary error messages. Example kzalloc() has its 
   own error message. Adding an extra one is useless.
diff --git a/drivers/staging/dgnc/dgnc_cls.c b/drivers/staging/dgnc/dgnc_cls.c
index cfa8384c0077..a17f4f6095c8 100644
--- a/drivers/staging/dgnc/dgnc_cls.c
+++ b/drivers/staging/dgnc/dgnc_cls.c
@@ -50,7 +50,7 @@ static inline void cls_set_ixon_flow_control(struct channel_t *ch);
 static inline void cls_set_ixoff_flow_control(struct channel_t *ch);
 static inline void cls_set_no_output_flow_control(struct channel_t *ch);
 static inline void cls_set_no_input_flow_control(struct channel_t *ch);
-static void cls_parse_modem(struct channel_t *ch, uchar signals);
+static void cls_parse_modem(struct channel_t *ch, unsigned char signals);
 static void cls_tasklet(unsigned long data);
 static void cls_vpd(struct dgnc_board *brd);
 static void cls_uart_init(struct channel_t *ch);
@@ -95,11 +95,10 @@ struct board_ops dgnc_cls_ops = {
 
 static inline void cls_set_cts_flow_control(struct channel_t *ch)
 {
-	uchar lcrb = readb(&ch->ch_cls_uart->lcr);
-	uchar ier = readb(&ch->ch_cls_uart->ier);
-	uchar isr_fcr = 0;
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char ier = readb(&ch->ch_cls_uart->ier);
+	unsigned char isr_fcr = 0;
 
-	DPR_PARAM(("Setting CTSFLOW\n"));
 
 	/*
 	 * The Enhanced Register Set may only be accessed when
@@ -140,11 +139,10 @@ static inline void cls_set_cts_flow_control(struct channel_t *ch)
 
 static inline void cls_set_ixon_flow_control(struct channel_t *ch)
 {
-	uchar lcrb = readb(&ch->ch_cls_uart->lcr);
-	uchar ier = readb(&ch->ch_cls_uart->ier);
-	uchar isr_fcr = 0;
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char ier = readb(&ch->ch_cls_uart->ier);
+	unsigned char isr_fcr = 0;
 
-	DPR_PARAM(("Setting IXON FLOW\n"));
 
 	/*
 	 * The Enhanced Register Set may only be accessed when
@@ -189,11 +187,10 @@ static inline void cls_set_ixon_flow_control(struct channel_t *ch)
 
 static inline void cls_set_no_output_flow_control(struct channel_t *ch)
 {
-	uchar lcrb = readb(&ch->ch_cls_uart->lcr);
-	uchar ier = readb(&ch->ch_cls_uart->ier);
-	uchar isr_fcr = 0;
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char ier = readb(&ch->ch_cls_uart->ier);
+	unsigned char isr_fcr = 0;
 
-	DPR_PARAM(("Unsetting Output FLOW\n"));
 
 	/*
 	 * The Enhanced Register Set may only be accessed when
@@ -236,11 +233,10 @@ static inline void cls_set_no_output_flow_control(struct channel_t *ch)
 
 static inline void cls_set_rts_flow_control(struct channel_t *ch)
 {
-	uchar lcrb = readb(&ch->ch_cls_uart->lcr);
-	uchar ier = readb(&ch->ch_cls_uart->ier);
-	uchar isr_fcr = 0;
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char ier = readb(&ch->ch_cls_uart->ier);
+	unsigned char isr_fcr = 0;
 
-	DPR_PARAM(("Setting RTSFLOW\n"));
 
 	/*
 	 * The Enhanced Register Set may only be accessed when
@@ -279,11 +275,10 @@ static inline void cls_set_rts_flow_control(struct channel_t *ch)
 
 static inline void cls_set_ixoff_flow_control(struct channel_t *ch)
 {
-	uchar lcrb = readb(&ch->ch_cls_uart->lcr);
-	uchar ier = readb(&ch->ch_cls_uart->ier);
-	uchar isr_fcr = 0;
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char ier = readb(&ch->ch_cls_uart->ier);
+	unsigned char isr_fcr = 0;
 
-	DPR_PARAM(("Setting IXOFF FLOW\n"));
 
 	/*
 	 * The Enhanced Register Set may only be accessed when
@@ -324,11 +319,10 @@ static inline void cls_set_ixoff_flow_control(struct channel_t *ch)
 
 static inline void cls_set_no_input_flow_control(struct channel_t *ch)
 {
-	uchar lcrb = readb(&ch->ch_cls_uart->lcr);
-	uchar ier = readb(&ch->ch_cls_uart->ier);
-	uchar isr_fcr = 0;
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char ier = readb(&ch->ch_cls_uart->ier);
+	unsigned char isr_fcr = 0;
 
-	DPR_PARAM(("Unsetting Input FLOW\n"));
 
 	/*
 	 * The Enhanced Register Set may only be accessed when
@@ -373,31 +367,30 @@ static inline void cls_set_no_input_flow_control(struct channel_t *ch)
  */
 static inline void cls_clear_break(struct channel_t *ch, int force)
 {
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* Bail if we aren't currently sending a break. */
 	if (!ch->ch_stop_sending_break) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
 	/* Turn break off, and unset some variables */
 	if (ch->ch_flags & CH_BREAK_SENDING) {
 		if (time_after(jiffies, ch->ch_stop_sending_break) || force) {
-			uchar temp = readb(&ch->ch_cls_uart->lcr);
+			unsigned char temp = readb(&ch->ch_cls_uart->lcr);
+
 			writeb((temp & ~UART_LCR_SBC), &ch->ch_cls_uart->lcr);
 			ch->ch_flags &= ~(CH_BREAK_SENDING);
 			ch->ch_stop_sending_break = 0;
-			DPR_IOCTL(("Finishing UART_LCR_SBC! finished: %lx\n",
-								jiffies));
 		}
 	}
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -405,8 +398,8 @@ static inline void cls_clear_break(struct channel_t *ch, int force)
 static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 {
 	struct channel_t *ch;
-	uchar isr = 0;
-	ulong lock_flags;
+	unsigned char isr = 0;
+	unsigned long flags;
 
 	/*
 	 * No need to verify board pointer, it was already
@@ -429,9 +422,6 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 		if (isr & UART_IIR_NO_INT)
 			break;
 
-		DPR_INTR(("%s:%d port: %x isr: %x\n", __FILE__, __LINE__,
-								 port, isr));
-
 		/* Receive Interrupt pending */
 		if (isr & (UART_IIR_RDI | UART_IIR_RDI_TIMEOUT)) {
 			/* Read data from uart -> queue */
@@ -444,11 +434,11 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 		/* Transmit Hold register empty pending */
 		if (isr & UART_IIR_THRI) {
 			/* Transfer data (if any) from Write Queue -> UART. */
-			DGNC_LOCK(ch->ch_lock, lock_flags);
+			spin_lock_irqsave(&ch->ch_lock, flags);
 			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 			brd->intr_tx++;
 			ch->ch_intr_tx++;
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			cls_copy_data_from_queue_to_uart(ch);
 		}
 
@@ -463,7 +453,6 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
 		}
 
 		/* Parse any modem signal changes */
-		DPR_INTR(("MOD_STAT: sending to parse_modem_sigs\n"));
 		cls_parse_modem(ch, readb(&ch->ch_cls_uart->msr));
 	}
 }
@@ -475,10 +464,10 @@ static inline void cls_parse_isr(struct dgnc_board *brd, uint port)
  */
 static void cls_param(struct tty_struct *tty)
 {
-	uchar lcr = 0;
-	uchar uart_lcr = 0;
-	uchar ier = 0;
-	uchar uart_ier = 0;
+	unsigned char lcr = 0;
+	unsigned char uart_lcr = 0;
+	unsigned char ier = 0;
+	unsigned char uart_ier = 0;
 	uint baud = 9600;
 	int quot = 0;
 	struct dgnc_board *bd;
@@ -500,17 +489,16 @@ static void cls_param(struct tty_struct *tty)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DPR_PARAM(("param start: tdev: %x cflags: %x oflags: %x iflags: %x\n",
-		ch->ch_tun.un_dev, ch->ch_c_cflag, ch->ch_c_oflag,
-							 ch->ch_c_iflag));
-
 	/*
 	 * If baud rate is zero, flush queues, and set mval to drop DTR.
 	 */
 	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
-		ch->ch_r_head = ch->ch_r_tail = 0;
-		ch->ch_e_head = ch->ch_e_tail = 0;
-		ch->ch_w_head = ch->ch_w_tail = 0;
+		ch->ch_r_head = 0;
+		ch->ch_r_tail = 0;
+		ch->ch_e_head = 0;
+		ch->ch_e_tail = 0;
+		ch->ch_w_head = 0;
+		ch->ch_w_tail = 0;
 
 		cls_flush_uart_write(ch);
 		cls_flush_uart_read(ch);
@@ -587,8 +575,6 @@ static void cls_param(struct tty_struct *tty)
 								(jindex < 16)) {
 			baud = bauds[iindex][jindex];
 		} else {
-			DPR_IOCTL(("baud indices were out of range (%d)(%d)",
-				iindex, jindex));
 			baud = 0;
 		}
 
@@ -644,7 +630,8 @@ static void cls_param(struct tty_struct *tty)
 		break;
 	}
 
-	ier = uart_ier = readb(&ch->ch_cls_uart->ier);
+	uart_ier = readb(&ch->ch_cls_uart->ier);
+	ier =  uart_ier;
 	uart_lcr = readb(&ch->ch_cls_uart->lcr);
 
 	if (baud == 0)
@@ -732,7 +719,7 @@ static void cls_tasklet(unsigned long data)
 {
 	struct dgnc_board *bd = (struct dgnc_board *) data;
 	struct channel_t *ch;
-	ulong  lock_flags;
+	unsigned long flags;
 	int i;
 	int state = 0;
 	int ports = 0;
@@ -743,16 +730,16 @@ static void cls_tasklet(unsigned long data)
 	}
 
 	/* Cache a couple board values */
-	DGNC_LOCK(bd->bd_lock, lock_flags);
+	spin_lock_irqsave(&bd->bd_lock, flags);
 	state = bd->state;
 	ports = bd->nasync;
-	DGNC_UNLOCK(bd->bd_lock, lock_flags);
+	spin_unlock_irqrestore(&bd->bd_lock, flags);
 
 	/*
 	 * Do NOT allow the interrupt routine to read the intr registers
 	 * Until we release this lock.
 	 */
-	DGNC_LOCK(bd->bd_intr_lock, lock_flags);
+	spin_lock_irqsave(&bd->bd_intr_lock, flags);
 
 	/*
 	 * If board is ready, parse deeper to see if there is anything to do.
@@ -795,7 +782,7 @@ static void cls_tasklet(unsigned long data)
 		}
 	}
 
-	DGNC_UNLOCK(bd->bd_intr_lock, lock_flags);
+	spin_unlock_irqrestore(&bd->bd_intr_lock, flags);
 
 }
 
@@ -809,8 +796,8 @@ static irqreturn_t cls_intr(int irq, void *voidbrd)
 {
 	struct dgnc_board *brd = (struct dgnc_board *) voidbrd;
 	uint i = 0;
-	uchar poll_reg;
-	unsigned long lock_flags;
+	unsigned char poll_reg;
+	unsigned long flags;
 
 	if (!brd) {
 		APR(("Received interrupt (%d) with null board associated\n",
@@ -827,7 +814,7 @@ static irqreturn_t cls_intr(int irq, void *voidbrd)
 		return IRQ_NONE;
 	}
 
-	DGNC_LOCK(brd->bd_intr_lock, lock_flags);
+	spin_lock_irqsave(&brd->bd_intr_lock, flags);
 
 	brd->intr_count++;
 
@@ -839,14 +826,10 @@ static irqreturn_t cls_intr(int irq, void *voidbrd)
 
 	/* If 0, no interrupts pending */
 	if (!poll_reg) {
-		DPR_INTR((
-			 "Kernel interrupted to me, but no pending interrupts...\n"));
-		DGNC_UNLOCK(brd->bd_intr_lock, lock_flags);
+		spin_unlock_irqrestore(&brd->bd_intr_lock, flags);
 		return IRQ_NONE;
 	}
 
-	DPR_INTR(("%s:%d poll_reg: %x\n", __FILE__, __LINE__, poll_reg));
-
 	/* Parse each port to find out what caused the interrupt */
 	for (i = 0; i < brd->nasync; i++)
 		cls_parse_isr(brd, i);
@@ -856,16 +839,16 @@ static irqreturn_t cls_intr(int irq, void *voidbrd)
 	 */
 	tasklet_schedule(&brd->helper_tasklet);
 
-	DGNC_UNLOCK(brd->bd_intr_lock, lock_flags);
+	spin_unlock_irqrestore(&brd->bd_intr_lock, flags);
 
-	DPR_INTR(("dgnc_intr finish.\n"));
 	return IRQ_HANDLED;
 }
 
 
 static void cls_disable_receiver(struct channel_t *ch)
 {
-	uchar tmp = readb(&ch->ch_cls_uart->ier);
+	unsigned char tmp = readb(&ch->ch_cls_uart->ier);
+
 	tmp &= ~(UART_IER_RDI);
 	writeb(tmp, &ch->ch_cls_uart->ier);
 }
@@ -873,7 +856,8 @@ static void cls_disable_receiver(struct channel_t *ch)
 
 static void cls_enable_receiver(struct channel_t *ch)
 {
-	uchar tmp = readb(&ch->ch_cls_uart->ier);
+	unsigned char tmp = readb(&ch->ch_cls_uart->ier);
+
 	tmp |= (UART_IER_RDI);
 	writeb(tmp, &ch->ch_cls_uart->ier);
 }
@@ -882,16 +866,16 @@ static void cls_enable_receiver(struct channel_t *ch)
 static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 {
 	int qleft = 0;
-	uchar linestatus = 0;
-	uchar error_mask = 0;
+	unsigned char linestatus = 0;
+	unsigned char error_mask = 0;
 	ushort head;
 	ushort tail;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* cache head and tail of queue */
 	head = ch->ch_r_head;
@@ -919,7 +903,8 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * Discard character if we are ignoring the error mask.
 		*/
 		if (linestatus & error_mask)  {
-			uchar discard;
+			unsigned char discard;
+
 			linestatus = 0;
 			discard = readb(&ch->ch_cls_uart->txrx);
 			continue;
@@ -934,10 +919,8 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * I hope thats okay with everyone? Yes? Good.
 		 */
 		while (qleft < 1) {
-			DPR_READ(("Queue full, dropping DATA:%x LSR:%x\n",
-				ch->ch_rqueue[tail], ch->ch_equeue[tail]));
-
-			ch->ch_r_tail = tail = (tail + 1) & RQUEUEMASK;
+			tail = (tail + 1) & RQUEUEMASK;
+			ch->ch_r_tail = tail;
 			ch->ch_err_overrun++;
 			qleft++;
 		}
@@ -950,9 +933,6 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 
 		qleft--;
 
-		DPR_READ(("DATA/LSR pair: %x %x\n", ch->ch_rqueue[head],
-							 ch->ch_equeue[head]));
-
 		if (ch->ch_equeue[head] & UART_LSR_PE)
 			ch->ch_err_parity++;
 		if (ch->ch_equeue[head] & UART_LSR_BI)
@@ -971,7 +951,7 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
 	ch->ch_r_head = head & RQUEUEMASK;
 	ch->ch_e_head = head & EQUEUEMASK;
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -981,7 +961,7 @@ static void cls_copy_data_from_uart_to_queue(struct channel_t *ch)
  */
 static int cls_drain(struct tty_struct *tty, uint seconds)
 {
-	ulong lock_flags;
+	unsigned long flags;
 	struct channel_t *ch;
 	struct un_t *un;
 	int rc = 0;
@@ -997,9 +977,9 @@ static int cls_drain(struct tty_struct *tty, uint seconds)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return -ENXIO;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 	un->un_flags |= UN_EMPTY;
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	/*
 	 * NOTE: Do something with time passed in.
@@ -1008,8 +988,6 @@ static int cls_drain(struct tty_struct *tty, uint seconds)
 					 ((un->un_flags & UN_EMPTY) == 0));
 
 	/* If ret is non-zero, user ctrl-c'ed us */
-	if (rc)
-		DPR_IOCTL(("%d Drain - User ctrl c'ed\n", __LINE__));
 
 	return rc;
 }
@@ -1057,28 +1035,28 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 	int n;
 	int qlen;
 	uint len_written = 0;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* No data to write to the UART */
 	if (ch->ch_w_tail == ch->ch_w_head) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
 	/* If port is "stopped", don't send any data to the UART */
 	if ((ch->ch_flags & CH_FORCED_STOP) ||
 				 (ch->ch_flags & CH_BREAK_SENDING)) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
 	if (!(ch->ch_flags & (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM))) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
@@ -1122,7 +1100,6 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 		writeb(ch->ch_wqueue[ch->ch_w_tail], &ch->ch_cls_uart->txrx);
 		dgnc_sniff_nowait_nolock(ch, "UART WRITE",
 					    ch->ch_wqueue + ch->ch_w_tail, 1);
-		DPR_WRITE(("Tx data: %x\n", ch->ch_wqueue[ch->ch_w_tail]));
 		ch->ch_w_tail++;
 		ch->ch_w_tail &= WQUEUEMASK;
 		ch->ch_txcount++;
@@ -1133,30 +1110,26 @@ static void cls_copy_data_from_queue_to_uart(struct channel_t *ch)
 	if (len_written > 0)
 		ch->ch_flags &= ~(CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	return;
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
-static void cls_parse_modem(struct channel_t *ch, uchar signals)
+static void cls_parse_modem(struct channel_t *ch, unsigned char signals)
 {
-	uchar msignals = signals;
-	ulong lock_flags;
+	unsigned char msignals = signals;
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DPR_MSIGS(("cls_parse_modem: port: %d signals: %d\n",
-					 ch->ch_portnum, msignals));
-
 	/*
 	 * Do altpin switching. Altpin switches DCD and DSR.
 	 * This prolly breaks DSRPACE, so we should be more clever here.
 	 */
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 	if (ch->ch_digi.digi_flags & DIGI_ALTPIN) {
-		uchar mswap = signals;
+		unsigned char mswap = signals;
+
 		if (mswap & UART_MSR_DDCD) {
 			msignals &= ~UART_MSR_DDCD;
 			msignals |= UART_MSR_DDSR;
@@ -1174,7 +1147,7 @@ static void cls_parse_modem(struct channel_t *ch, uchar signals)
 			msignals |= UART_MSR_DCD;
 		}
 	}
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	/*
 	 * Scrub off lower bits. They signify delta's, which I don't
@@ -1182,7 +1155,7 @@ static void cls_parse_modem(struct channel_t *ch, uchar signals)
 	 */
 	signals &= 0xf0;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 	if (msignals & UART_MSR_DCD)
 		ch->ch_mistat |= UART_MSR_DCD;
 	else
@@ -1202,25 +1175,14 @@ static void cls_parse_modem(struct channel_t *ch, uchar signals)
 		ch->ch_mistat |= UART_MSR_CTS;
 	else
 		ch->ch_mistat &= ~UART_MSR_CTS;
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-
-	DPR_MSIGS((
-		"Port: %d DTR: %d RTS: %d CTS: %d DSR: %d " "RI: %d CD: %d\n",
-		ch->ch_portnum,
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MCR_DTR),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MCR_RTS),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_CTS),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_DSR),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_RI),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_DCD)));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
 /* Make the UART raise any of the output signals we want up */
 static void cls_assert_modem_signals(struct channel_t *ch)
 {
-	uchar out;
+	unsigned char out;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
@@ -1264,8 +1226,8 @@ static void cls_send_stop_character(struct channel_t *ch)
 /* Inits UART */
 static void cls_uart_init(struct channel_t *ch)
 {
-	uchar lcrb = readb(&ch->ch_cls_uart->lcr);
-	uchar isr_fcr = 0;
+	unsigned char lcrb = readb(&ch->ch_cls_uart->lcr);
+	unsigned char isr_fcr = 0;
 
 	writeb(0, &ch->ch_cls_uart->ier);
 
@@ -1316,8 +1278,8 @@ static void cls_uart_off(struct channel_t *ch)
  */
 static uint cls_get_uart_bytes_left(struct channel_t *ch)
 {
-	uchar left = 0;
-	uchar lsr = 0;
+	unsigned char left = 0;
+	unsigned char lsr = 0;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return 0;
@@ -1355,12 +1317,11 @@ static void cls_send_break(struct channel_t *ch, int msecs)
 	if (msecs == 0) {
 		/* Turn break off, and unset some variables */
 		if (ch->ch_flags & CH_BREAK_SENDING) {
-			uchar temp = readb(&ch->ch_cls_uart->lcr);
+			unsigned char temp = readb(&ch->ch_cls_uart->lcr);
+
 			writeb((temp & ~UART_LCR_SBC), &ch->ch_cls_uart->lcr);
 			ch->ch_flags &= ~(CH_BREAK_SENDING);
 			ch->ch_stop_sending_break = 0;
-			DPR_IOCTL(("Finishing UART_LCR_SBC! finished: %lx\n",
-								     jiffies));
 		}
 		return;
 	}
@@ -1374,12 +1335,10 @@ static void cls_send_break(struct channel_t *ch, int msecs)
 
 	/* Tell the UART to start sending the break */
 	if (!(ch->ch_flags & CH_BREAK_SENDING)) {
-		uchar temp = readb(&ch->ch_cls_uart->lcr);
+		unsigned char temp = readb(&ch->ch_cls_uart->lcr);
+
 		writeb((temp | UART_LCR_SBC), &ch->ch_cls_uart->lcr);
 		ch->ch_flags |= (CH_BREAK_SENDING);
-		DPR_IOCTL((
-			"Port %d. Starting UART_LCR_SBC! start: %lx should end: %lx\n",
-			ch->ch_portnum, jiffies, ch->ch_stop_sending_break));
 	}
 }
 
diff --git a/drivers/staging/dgnc/dgnc_driver.c b/drivers/staging/dgnc/dgnc_driver.c
index 764613b2f4b4..21546659ff07 100644
--- a/drivers/staging/dgnc/dgnc_driver.c
+++ b/drivers/staging/dgnc/dgnc_driver.c
@@ -57,7 +57,7 @@ MODULE_SUPPORTED_DEVICE("dgnc");
  */
 PARM_INT(debug,		0x00,		0644,	"Driver debugging level");
 PARM_INT(rawreadok,	1,		0644,	"Bypass flip buffers on input");
-PARM_INT(trcbuf_size,	0x100000,	0644, 	"Debugging trace buffer size.");
+PARM_INT(trcbuf_size,	0x100000,	0644,	"Debugging trace buffer size.");
 
 /**************************************************************************
  *
@@ -70,26 +70,15 @@ static void		dgnc_init_globals(void);
 static int		dgnc_found_board(struct pci_dev *pdev, int id);
 static void		dgnc_cleanup_board(struct dgnc_board *brd);
 static void		dgnc_poll_handler(ulong dummy);
-static int		dgnc_init_pci(void);
 static int		dgnc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent);
-static void		dgnc_remove_one(struct pci_dev *dev);
-static int		dgnc_probe1(struct pci_dev *pdev, int card_type);
 static void		dgnc_do_remap(struct dgnc_board *brd);
 
-/* Driver load/unload functions */
-int		dgnc_init_module(void);
-void		dgnc_cleanup_module(void);
-
-module_init(dgnc_init_module);
-module_exit(dgnc_cleanup_module);
-
-
 /*
  * File operations permitted on Control/Management major.
  */
 static const struct file_operations dgnc_BoardFops = {
 	.owner		=	THIS_MODULE,
-	.unlocked_ioctl =  	dgnc_mgmt_ioctl,
+	.unlocked_ioctl =	dgnc_mgmt_ioctl,
 	.open		=	dgnc_mgmt_open,
 	.release	=	dgnc_mgmt_close
 };
@@ -101,23 +90,18 @@ static const struct file_operations dgnc_BoardFops = {
 uint			dgnc_NumBoards;
 struct dgnc_board		*dgnc_Board[MAXBOARDS];
 DEFINE_SPINLOCK(dgnc_global_lock);
-int			dgnc_driver_state = DRIVER_INITIALIZED;
-ulong			dgnc_poll_counter;
 uint			dgnc_Major;
 int			dgnc_poll_tick = 20;	/* Poll interval - 20 ms */
 
 /*
  * Static vars.
  */
-static uint		dgnc_Major_Control_Registered = FALSE;
-static uint		dgnc_driver_start = FALSE;
-
 static struct class *dgnc_class;
 
 /*
  * Poller stuff
  */
-static 			DEFINE_SPINLOCK(dgnc_poll_lock);	/* Poll scheduling lock */
+static DEFINE_SPINLOCK(dgnc_poll_lock); /* Poll scheduling lock */
 static ulong		dgnc_poll_time;				/* Time of next poll */
 static uint		dgnc_poll_stop;				/* Used to tell poller to stop */
 static struct timer_list dgnc_poll_timer;
@@ -128,25 +112,12 @@ static struct pci_device_id dgnc_pci_tbl[] = {
 	{	DIGI_VID, PCI_DEVICE_CLASSIC_4_422_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	1 },
 	{	DIGI_VID, PCI_DEVICE_CLASSIC_8_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	2 },
 	{	DIGI_VID, PCI_DEVICE_CLASSIC_8_422_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	3 },
-	{	DIGI_VID, PCI_DEVICE_NEO_4_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,		4 },
-	{	DIGI_VID, PCI_DEVICE_NEO_8_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,		5 },
-	{	DIGI_VID, PCI_DEVICE_NEO_2DB9_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	6 },
-	{	DIGI_VID, PCI_DEVICE_NEO_2DB9PRI_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	7 },
-	{	DIGI_VID, PCI_DEVICE_NEO_2RJ45_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	8 },
-	{	DIGI_VID, PCI_DEVICE_NEO_2RJ45PRI_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	9 },
-	{	DIGI_VID, PCI_DEVICE_NEO_1_422_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	10 },
-	{	DIGI_VID, PCI_DEVICE_NEO_1_422_485_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	11 },
-	{	DIGI_VID, PCI_DEVICE_NEO_2_422_485_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	12 },
-	{	DIGI_VID, PCI_DEVICE_NEO_EXPRESS_8_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	 13 },
-	{	DIGI_VID, PCI_DEVICE_NEO_EXPRESS_4_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	14 },
-	{	DIGI_VID, PCI_DEVICE_NEO_EXPRESS_4RJ45_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	15 },
-	{	DIGI_VID, PCI_DEVICE_NEO_EXPRESS_8RJ45_DID, PCI_ANY_ID, PCI_ANY_ID, 0, 0,	16 },
 	{0,}						/* 0 terminated list. */
 };
 MODULE_DEVICE_TABLE(pci, dgnc_pci_tbl);
 
 struct board_id {
-	uchar *name;
+	unsigned char *name;
 	uint maxports;
 	unsigned int is_pci_express;
 };
@@ -176,7 +147,6 @@ static struct pci_driver dgnc_driver = {
 	.name		= "dgnc",
 	.probe		= dgnc_init_one,
 	.id_table       = dgnc_pci_tbl,
-	.remove		= dgnc_remove_one,
 };
 
 
@@ -186,12 +156,6 @@ char *dgnc_state_text[] = {
 	"Board READY",
 };
 
-char *dgnc_driver_state_text[] = {
-	"Driver Initialized",
-	"Driver Ready."
-};
-
-
 
 /************************************************************************
  *
@@ -199,13 +163,47 @@ char *dgnc_driver_state_text[] = {
  *
  ************************************************************************/
 
+/*
+ * dgnc_cleanup_module()
+ *
+ * Module unload.  This is where it all ends.
+ */
+static void dgnc_cleanup_module(void)
+{
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dgnc_poll_lock, flags);
+	dgnc_poll_stop = 1;
+	spin_unlock_irqrestore(&dgnc_poll_lock, flags);
+
+	/* Turn off poller right away. */
+	del_timer_sync(&dgnc_poll_timer);
+
+	dgnc_remove_driver_sysfiles(&dgnc_driver);
+
+	device_destroy(dgnc_class, MKDEV(dgnc_Major, 0));
+	class_destroy(dgnc_class);
+	unregister_chrdev(dgnc_Major, "dgnc");
+
+	for (i = 0; i < dgnc_NumBoards; ++i) {
+		dgnc_remove_ports_sysfiles(dgnc_Board[i]);
+		dgnc_tty_uninit(dgnc_Board[i]);
+		dgnc_cleanup_board(dgnc_Board[i]);
+	}
+
+	dgnc_tty_post_uninit();
+
+	if (dgnc_NumBoards)
+		pci_unregister_driver(&dgnc_driver);
+}
 
 /*
  * init_module()
  *
  * Module load.  This is where it all starts.
  */
-int dgnc_init_module(void)
+static int __init dgnc_init_module(void)
 {
 	int rc = 0;
 
@@ -222,7 +220,7 @@ int dgnc_init_module(void)
 	/*
 	 * Find and configure all the cards
 	 */
-	rc = dgnc_init_pci();
+	rc = pci_register_driver(&dgnc_driver);
 
 	/*
 	 * If something went wrong in the scan, bail out of driver.
@@ -239,10 +237,11 @@ int dgnc_init_module(void)
 		dgnc_create_driver_sysfiles(&dgnc_driver);
 	}
 
-	DPR_INIT(("Finished init_module. Returning %d\n", rc));
 	return rc;
 }
 
+module_init(dgnc_init_module);
+module_exit(dgnc_cleanup_module);
 
 /*
  * Start of driver.
@@ -252,77 +251,54 @@ static int dgnc_start(void)
 	int rc = 0;
 	unsigned long flags;
 
-	if (dgnc_driver_start == FALSE) {
-
-		dgnc_driver_start = TRUE;
-
-		/* make sure that the globals are init'd before we do anything else */
-		dgnc_init_globals();
+	/* make sure that the globals are init'd before we do anything else */
+	dgnc_init_globals();
 
-		dgnc_NumBoards = 0;
+	APR(("For the tools package or updated drivers please visit http://www.digi.com\n"));
 
-		APR(("For the tools package or updated drivers please visit http://www.digi.com\n"));
-
-		/*
-		 * Register our base character device into the kernel.
-		 * This allows the download daemon to connect to the downld device
-		 * before any of the boards are init'ed.
-		 */
-		if (!dgnc_Major_Control_Registered) {
-			/*
-			 * Register management/dpa devices
-			 */
-			rc = register_chrdev(0, "dgnc", &dgnc_BoardFops);
-			if (rc <= 0) {
-				APR(("Can't register dgnc driver device (%d)\n", rc));
-				rc = -ENXIO;
-				return rc;
-			}
-			dgnc_Major = rc;
-
-			dgnc_class = class_create(THIS_MODULE, "dgnc_mgmt");
-			device_create(dgnc_class, NULL,
-				MKDEV(dgnc_Major, 0),
-				NULL, "dgnc_mgmt");
-			dgnc_Major_Control_Registered = TRUE;
-		}
+	/*
+	 * Register our base character device into the kernel.
+	 * This allows the download daemon to connect to the downld device
+	 * before any of the boards are init'ed.
+	 *
+	 * Register management/dpa devices
+	 */
+	rc = register_chrdev(0, "dgnc", &dgnc_BoardFops);
+	if (rc <= 0) {
+		APR(("Can't register dgnc driver device (%d)\n", rc));
+		return -ENXIO;
+	}
+	dgnc_Major = rc;
 
-		/*
-		 * Init any global tty stuff.
-		 */
-		rc = dgnc_tty_preinit();
+	dgnc_class = class_create(THIS_MODULE, "dgnc_mgmt");
+	device_create(dgnc_class, NULL,
+		MKDEV(dgnc_Major, 0),
+		NULL, "dgnc_mgmt");
 
-		if (rc < 0) {
-			APR(("tty preinit - not enough memory (%d)\n", rc));
-			return rc;
-		}
+	/*
+	 * Init any global tty stuff.
+	 */
+	rc = dgnc_tty_preinit();
 
-		/* Start the poller */
-		DGNC_LOCK(dgnc_poll_lock, flags);
-		init_timer(&dgnc_poll_timer);
-		dgnc_poll_timer.function = dgnc_poll_handler;
-		dgnc_poll_timer.data = 0;
-		dgnc_poll_time = jiffies + dgnc_jiffies_from_ms(dgnc_poll_tick);
-		dgnc_poll_timer.expires = dgnc_poll_time;
-		DGNC_UNLOCK(dgnc_poll_lock, flags);
+	if (rc < 0) {
+		APR(("tty preinit - not enough memory (%d)\n", rc));
+		return rc;
+	}
 
-		add_timer(&dgnc_poll_timer);
+	/* Start the poller */
+	spin_lock_irqsave(&dgnc_poll_lock, flags);
+	init_timer(&dgnc_poll_timer);
+	dgnc_poll_timer.function = dgnc_poll_handler;
+	dgnc_poll_timer.data = 0;
+	dgnc_poll_time = jiffies + dgnc_jiffies_from_ms(dgnc_poll_tick);
+	dgnc_poll_timer.expires = dgnc_poll_time;
+	spin_unlock_irqrestore(&dgnc_poll_lock, flags);
 
-		dgnc_driver_state = DRIVER_READY;
-	}
+	add_timer(&dgnc_poll_timer);
 
 	return rc;
 }
 
-/*
- * Register pci driver, and return how many boards we have.
- */
-static int dgnc_init_pci(void)
-{
-	return pci_register_driver(&dgnc_driver);
-}
-
-
 /* returns count (>= 0), or negative on error */
 static int dgnc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 {
@@ -334,64 +310,13 @@ static int dgnc_init_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	if (rc < 0) {
 		rc = -EIO;
 	} else {
-		rc = dgnc_probe1(pdev, ent->driver_data);
-		if (rc == 0) {
+		rc = dgnc_found_board(pdev, ent->driver_data);
+		if (rc == 0)
 			dgnc_NumBoards++;
-			DPR_INIT(("Incrementing numboards to %d\n", dgnc_NumBoards));
-		}
 	}
 	return rc;
 }
 
-static int dgnc_probe1(struct pci_dev *pdev, int card_type)
-{
-	return dgnc_found_board(pdev, card_type);
-}
-
-
-static void dgnc_remove_one(struct pci_dev *dev)
-{
-	/* Do Nothing */
-}
-
-/*
- * dgnc_cleanup_module()
- *
- * Module unload.  This is where it all ends.
- */
-void dgnc_cleanup_module(void)
-{
-	int i;
-	ulong lock_flags;
-
-	DGNC_LOCK(dgnc_poll_lock, lock_flags);
-	dgnc_poll_stop = 1;
-	DGNC_UNLOCK(dgnc_poll_lock, lock_flags);
-
-	/* Turn off poller right away. */
-	del_timer_sync(&dgnc_poll_timer);
-
-	dgnc_remove_driver_sysfiles(&dgnc_driver);
-
-	if (dgnc_Major_Control_Registered) {
-		device_destroy(dgnc_class, MKDEV(dgnc_Major, 0));
-		class_destroy(dgnc_class);
-		unregister_chrdev(dgnc_Major, "dgnc");
-	}
-
-	for (i = 0; i < dgnc_NumBoards; ++i) {
-		dgnc_remove_ports_sysfiles(dgnc_Board[i]);
-		dgnc_tty_uninit(dgnc_Board[i]);
-		dgnc_cleanup_board(dgnc_Board[i]);
-	}
-
-	dgnc_tty_post_uninit();
-
-	if (dgnc_NumBoards)
-		pci_unregister_driver(&dgnc_driver);
-}
-
-
 /*
  * dgnc_cleanup_board()
  *
@@ -431,12 +356,12 @@ static void dgnc_cleanup_board(struct dgnc_board *brd)
 	if (brd->msgbuf_head) {
 		unsigned long flags;
 
-		DGNC_LOCK(dgnc_global_lock, flags);
+		spin_lock_irqsave(&dgnc_global_lock, flags);
 		brd->msgbuf = NULL;
 		printk("%s", brd->msgbuf_head);
 		kfree(brd->msgbuf_head);
 		brd->msgbuf_head = NULL;
-		DGNC_UNLOCK(dgnc_global_lock, flags);
+		spin_unlock_irqrestore(&dgnc_global_lock, flags);
 	}
 
 	/* Free all allocated channels structs */
@@ -472,14 +397,16 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	unsigned long flags;
 
 	/* get the board structure and prep it */
-	brd = dgnc_Board[dgnc_NumBoards] =
-		kzalloc(sizeof(*brd), GFP_KERNEL);
+	dgnc_Board[dgnc_NumBoards] = kzalloc(sizeof(*brd), GFP_KERNEL);
+	brd = dgnc_Board[dgnc_NumBoards];
+
 	if (!brd)
 		return -ENOMEM;
 
 	/* make a temporary message buffer for the boot messages */
-	brd->msgbuf = brd->msgbuf_head =
-		kzalloc(sizeof(u8) * 8192, GFP_KERNEL);
+	brd->msgbuf_head = kzalloc(sizeof(u8) * 8192, GFP_KERNEL);
+	brd->msgbuf = brd->msgbuf_head;
+
 	if (!brd->msgbuf) {
 		kfree(brd);
 		return -ENOMEM;
@@ -500,8 +427,8 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	brd->dpastatus = BD_NOFEP;
 	init_waitqueue_head(&brd->state_wait);
 
-	DGNC_SPINLOCK_INIT(brd->bd_lock);
-	DGNC_SPINLOCK_INIT(brd->bd_intr_lock);
+	spin_lock_init(&brd->bd_lock);
+	spin_lock_init(&brd->bd_intr_lock);
 
 	brd->state		= BOARD_FOUND;
 
@@ -526,8 +453,6 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 
 		brd->dpatype = T_CLASSIC | T_PCIBUS;
 
-		DPR_INIT(("dgnc_found_board - Classic.\n"));
-
 		/*
 		 * For PCI ClassicBoards
 		 * PCI Local Address (i.e. "resource" number) space
@@ -602,8 +527,6 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 		else
 			brd->dpatype = T_NEO | T_PCIBUS;
 
-		DPR_INIT(("dgnc_found_board - NEO.\n"));
-
 		/* get the PCI Base Address Registers */
 		brd->membase     = pci_resource_start(pdev, 0);
 		brd->membase_end = pci_resource_end(pdev, 0);
@@ -677,13 +600,12 @@ static int dgnc_found_board(struct pci_dev *pdev, int id)
 	/* init our poll helper tasklet */
 	tasklet_init(&brd->helper_tasklet, brd->bd_ops->tasklet, (unsigned long) brd);
 
-	DPR_INIT(("dgnc_scan(%d) - printing out the msgbuf\n", i));
-	DGNC_LOCK(dgnc_global_lock, flags);
+	spin_lock_irqsave(&dgnc_global_lock, flags);
 	brd->msgbuf = NULL;
 	printk("%s", brd->msgbuf_head);
 	kfree(brd->msgbuf_head);
 	brd->msgbuf_head = NULL;
-	DGNC_UNLOCK(dgnc_global_lock, flags);
+	spin_unlock_irqrestore(&dgnc_global_lock, flags);
 
 	/*
 	 * allocate flip buffer for board.
@@ -708,13 +630,9 @@ static int dgnc_finalize_board_init(struct dgnc_board *brd)
 {
 	int rc = 0;
 
-	DPR_INIT(("dgnc_finalize_board_init() - start\n"));
-
 	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
 		return -ENODEV;
 
-	DPR_INIT(("dgnc_finalize_board_init() - start #2\n"));
-
 	if (brd->irq) {
 		rc = request_irq(brd->irq, brd->bd_ops->intr,
 				 IRQF_SHARED, "DGNC", brd);
@@ -725,9 +643,6 @@ static int dgnc_finalize_board_init(struct dgnc_board *brd)
 			brd->state = BOARD_FAILED;
 			brd->dpastatus = BD_NOFEP;
 			rc = -ENODEV;
-		} else {
-			DPR_INIT(("Requested and received usage of IRQ %d\n",
-				  brd->irq));
 		}
 	}
 	return rc;
@@ -743,8 +658,6 @@ static void dgnc_do_remap(struct dgnc_board *brd)
 		return;
 
 	brd->re_map_membase = ioremap(brd->membase, 0x1000);
-
-	DPR_INIT(("remapped mem: 0x%p\n", brd->re_map_membase));
 }
 
 
@@ -777,44 +690,32 @@ static void dgnc_do_remap(struct dgnc_board *brd)
 static void dgnc_poll_handler(ulong dummy)
 {
 	struct dgnc_board *brd;
-	unsigned long lock_flags;
+	unsigned long flags;
 	int i;
 	unsigned long new_time;
 
-	dgnc_poll_counter++;
-
-	/*
-	 * Do not start the board state machine until
-	 * driver tells us its up and running, and has
-	 * everything it needs.
-	 */
-	if (dgnc_driver_state != DRIVER_READY)
-		goto schedule_poller;
-
 	/* Go thru each board, kicking off a tasklet for each if needed */
 	for (i = 0; i < dgnc_NumBoards; i++) {
 		brd = dgnc_Board[i];
 
-		DGNC_LOCK(brd->bd_lock, lock_flags);
+		spin_lock_irqsave(&brd->bd_lock, flags);
 
 		/* If board is in a failed state, don't bother scheduling a tasklet */
 		if (brd->state == BOARD_FAILED) {
-			DGNC_UNLOCK(brd->bd_lock, lock_flags);
+			spin_unlock_irqrestore(&brd->bd_lock, flags);
 			continue;
 		}
 
 		/* Schedule a poll helper task */
 		tasklet_schedule(&brd->helper_tasklet);
 
-		DGNC_UNLOCK(brd->bd_lock, lock_flags);
+		spin_unlock_irqrestore(&brd->bd_lock, flags);
 	}
 
-schedule_poller:
-
 	/*
 	 * Schedule ourself back at the nominal wakeup interval.
 	 */
-	DGNC_LOCK(dgnc_poll_lock, lock_flags);
+	spin_lock_irqsave(&dgnc_poll_lock, flags);
 	dgnc_poll_time += dgnc_jiffies_from_ms(dgnc_poll_tick);
 
 	new_time = dgnc_poll_time - jiffies;
@@ -826,7 +727,7 @@ schedule_poller:
 	dgnc_poll_timer.function = dgnc_poll_handler;
 	dgnc_poll_timer.data = 0;
 	dgnc_poll_timer.expires = dgnc_poll_time;
-	DGNC_UNLOCK(dgnc_poll_lock, lock_flags);
+	spin_unlock_irqrestore(&dgnc_poll_lock, flags);
 
 	if (!dgnc_poll_stop)
 		add_timer(&dgnc_poll_timer);
@@ -846,6 +747,7 @@ static void dgnc_init_globals(void)
 	dgnc_rawreadok		= rawreadok;
 	dgnc_trcbuf_size	= trcbuf_size;
 	dgnc_debug		= debug;
+	dgnc_NumBoards		= 0;
 
 	for (i = 0; i < MAXBOARDS; i++)
 		dgnc_Board[i] = NULL;
@@ -853,77 +755,3 @@ static void dgnc_init_globals(void)
 	init_timer(&dgnc_poll_timer);
 }
 
-
-/************************************************************************
- *
- * Utility functions
- *
- ************************************************************************/
-
-/*
- * dgnc_ms_sleep()
- *
- * Put the driver to sleep for x ms's
- *
- * Returns 0 if timed out, !0 (showing signal) if interrupted by a signal.
- */
-int dgnc_ms_sleep(ulong ms)
-{
-	current->state = TASK_INTERRUPTIBLE;
-	schedule_timeout((ms * HZ) / 1000);
-	return signal_pending(current);
-}
-
-
-
-/*
- *      dgnc_ioctl_name() : Returns a text version of each ioctl value.
- */
-char *dgnc_ioctl_name(int cmd)
-{
-	switch (cmd) {
-
-	case TCGETA:		return "TCGETA";
-	case TCGETS:		return "TCGETS";
-	case TCSETA:		return "TCSETA";
-	case TCSETS:		return "TCSETS";
-	case TCSETAW:		return "TCSETAW";
-	case TCSETSW:		return "TCSETSW";
-	case TCSETAF:		return "TCSETAF";
-	case TCSETSF:		return "TCSETSF";
-	case TCSBRK:		return "TCSBRK";
-	case TCXONC:		return "TCXONC";
-	case TCFLSH:		return "TCFLSH";
-	case TIOCGSID:		return "TIOCGSID";
-
-	case TIOCGETD:		return "TIOCGETD";
-	case TIOCSETD:		return "TIOCSETD";
-	case TIOCGWINSZ:	return "TIOCGWINSZ";
-	case TIOCSWINSZ:	return "TIOCSWINSZ";
-
-	case TIOCMGET:		return "TIOCMGET";
-	case TIOCMSET:		return "TIOCMSET";
-	case TIOCMBIS:		return "TIOCMBIS";
-	case TIOCMBIC:		return "TIOCMBIC";
-
-	/* from digi.h */
-	case DIGI_SETA:		return "DIGI_SETA";
-	case DIGI_SETAW:	return "DIGI_SETAW";
-	case DIGI_SETAF:	return "DIGI_SETAF";
-	case DIGI_SETFLOW:	return "DIGI_SETFLOW";
-	case DIGI_SETAFLOW:	return "DIGI_SETAFLOW";
-	case DIGI_GETFLOW:	return "DIGI_GETFLOW";
-	case DIGI_GETAFLOW:	return "DIGI_GETAFLOW";
-	case DIGI_GETA:		return "DIGI_GETA";
-	case DIGI_GEDELAY:	return "DIGI_GEDELAY";
-	case DIGI_SEDELAY:	return "DIGI_SEDELAY";
-	case DIGI_GETCUSTOMBAUD: return "DIGI_GETCUSTOMBAUD";
-	case DIGI_SETCUSTOMBAUD: return "DIGI_SETCUSTOMBAUD";
-	case TIOCMODG:		return "TIOCMODG";
-	case TIOCMODS:		return "TIOCMODS";
-	case TIOCSDTR:		return "TIOCSDTR";
-	case TIOCCDTR:		return "TIOCCDTR";
-
-	default:		return "unknown";
-	}
-}
diff --git a/drivers/staging/dgnc/dgnc_driver.h b/drivers/staging/dgnc/dgnc_driver.h
index 58b5aa7b68ed..f901957c757f 100644
--- a/drivers/staging/dgnc/dgnc_driver.h
+++ b/drivers/staging/dgnc/dgnc_driver.h
@@ -49,16 +49,14 @@
  * three lines, and the driver printk's will all automagically change.
  *
  * APR((fmt, args, ...));	Always prints message
- * DPR((fmt, args, ...));	Only prints if DGNC_TRACER is defined at
- *				  compile time and dgnc_debug!=0
  */
 #define	PROCSTR		"dgnc"			/* /proc entries	 */
 #define	DEVSTR		"/dev/dg/dgnc"		/* /dev entries		 */
 #define	DRVSTR		"dgnc"			/* Driver name string
 						 * displayed by APR	 */
-#define	APR(args)	do { PRINTF_TO_KMEM(args); printk(DRVSTR": "); printk args; \
+#define	APR(args)	do { printk(DRVSTR": "); printk args; \
 			   } while (0)
-#define	RAPR(args)	do { PRINTF_TO_KMEM(args); printk args; } while (0)
+#define	RAPR(args)	do { printk args; } while (0)
 
 #define TRC_TO_CONSOLE 1
 
@@ -91,28 +89,6 @@
 
 #define	DBG_CARR		(dgnc_debug & 0x10000)
 
-#define PRINTF_TO_KMEM(args)
-# define TRC(ARGS)
-# define DPR_INIT(ARGS)
-# define DPR_BASIC(ARGS)
-# define DPR_CORE(ARGS)
-# define DPR_OPEN(ARGS)
-# define DPR_CLOSE(ARGS)
-# define DPR_READ(ARGS)
-# define DPR_WRITE(ARGS)
-# define DPR_IOCTL(ARGS)
-# define DPR_PROC(ARGS)
-# define DPR_PARAM(ARGS)
-# define DPR_PSCAN(ARGS)
-# define DPR_EVENT(ARGS)
-# define DPR_DRAIN(ARGS)
-# define DPR_CARR(ARGS)
-# define DPR_MGMT(ARGS)
-# define DPR_INTR(ARGS)
-# define DPR_MSIGS(ARGS)
-
-# define DPR(args)
-
 /* Number of boards we support at once. */
 #define	MAXBOARDS	20
 #define	MAXPORTS	8
@@ -162,14 +138,6 @@
 #define SNIFF_MASK	(SNIFF_MAX - 1)	/* Sniff wrap mask */
 
 /*
- * Lock function/defines.
- * Makes spotting lock/unlock locations easier.
- */
-# define DGNC_SPINLOCK_INIT(x)		spin_lock_init(&(x))
-# define DGNC_LOCK(x, y)		spin_lock_irqsave(&(x), y)
-# define DGNC_UNLOCK(x, y)		spin_unlock_irqrestore(&(x), y)
-
-/*
  * All the possible states the driver can be while being loaded.
  */
 enum {
@@ -200,24 +168,24 @@ struct channel_t;
  * Per board operations structure				       *
  ************************************************************************/
 struct board_ops {
-	void (*tasklet) (unsigned long data);
-	irqreturn_t (*intr) (int irq, void *voidbrd);
-	void (*uart_init) (struct channel_t *ch);
-	void (*uart_off) (struct channel_t *ch);
-	int  (*drain) (struct tty_struct *tty, uint seconds);
-	void (*param) (struct tty_struct *tty);
-	void (*vpd) (struct dgnc_board *brd);
-	void (*assert_modem_signals) (struct channel_t *ch);
-	void (*flush_uart_write) (struct channel_t *ch);
-	void (*flush_uart_read) (struct channel_t *ch);
-	void (*disable_receiver) (struct channel_t *ch);
-	void (*enable_receiver) (struct channel_t *ch);
-	void (*send_break) (struct channel_t *ch, int);
-	void (*send_start_character) (struct channel_t *ch);
-	void (*send_stop_character) (struct channel_t *ch);
-	void (*copy_data_from_queue_to_uart) (struct channel_t *ch);
-	uint (*get_uart_bytes_left) (struct channel_t *ch);
-	void (*send_immediate_char) (struct channel_t *ch, unsigned char);
+	void (*tasklet)(unsigned long data);
+	irqreturn_t (*intr)(int irq, void *voidbrd);
+	void (*uart_init)(struct channel_t *ch);
+	void (*uart_off)(struct channel_t *ch);
+	int  (*drain)(struct tty_struct *tty, uint seconds);
+	void (*param)(struct tty_struct *tty);
+	void (*vpd)(struct dgnc_board *brd);
+	void (*assert_modem_signals)(struct channel_t *ch);
+	void (*flush_uart_write)(struct channel_t *ch);
+	void (*flush_uart_read)(struct channel_t *ch);
+	void (*disable_receiver)(struct channel_t *ch);
+	void (*enable_receiver)(struct channel_t *ch);
+	void (*send_break)(struct channel_t *ch, int);
+	void (*send_start_character)(struct channel_t *ch);
+	void (*send_stop_character)(struct channel_t *ch);
+	void (*copy_data_from_queue_to_uart)(struct channel_t *ch);
+	uint (*get_uart_bytes_left)(struct channel_t *ch);
+	void (*send_immediate_char)(struct channel_t *ch, unsigned char);
 };
 
 /************************************************************************
@@ -241,13 +209,13 @@ struct dgnc_board {
 	u16		device;		/* PCI device ID */
 	u16		subvendor;	/* PCI subsystem vendor ID */
 	u16		subdevice;	/* PCI subsystem device ID */
-	uchar		rev;		/* PCI revision ID */
+	unsigned char	rev;		/* PCI revision ID */
 	uint		pci_bus;	/* PCI bus value */
 	uint		pci_slot;	/* PCI slot value */
 	uint		maxports;	/* MAX ports this board can handle */
-	uchar		dvid;		/* Board specific device id */
-	uchar		vpd[128];	/* VPD of board, if found */
-	uchar		serial_num[20];	/* Serial number of board, if found in VPD */
+	unsigned char	dvid;		/* Board specific device id */
+	unsigned char	vpd[128];	/* VPD of board, if found */
+	unsigned char	serial_num[20];	/* Serial number of board, if found in VPD */
 
 	spinlock_t	bd_lock;	/* Used to protect board */
 
@@ -416,41 +384,41 @@ struct channel_t {
 	tcflag_t	ch_c_cflag;	/* channel cflags	       */
 	tcflag_t	ch_c_oflag;	/* channel oflags	       */
 	tcflag_t	ch_c_lflag;	/* channel lflags	       */
-	uchar		ch_stopc;	/* Stop character	       */
-	uchar		ch_startc;	/* Start character	      */
+	unsigned char	ch_stopc;	/* Stop character	       */
+	unsigned char	ch_startc;	/* Start character	      */
 
 	uint		ch_old_baud;	/* Cache of the current baud */
 	uint		ch_custom_speed;/* Custom baud, if set */
 
 	uint		ch_wopen;	/* Waiting for open process cnt */
 
-	uchar		ch_mostat;	/* FEP output modem status      */
-	uchar		ch_mistat;	/* FEP input modem status       */
+	unsigned char		ch_mostat;	/* FEP output modem status      */
+	unsigned char		ch_mistat;	/* FEP input modem status       */
 
 	struct neo_uart_struct __iomem *ch_neo_uart;	/* Pointer to the "mapped" UART struct */
 	struct cls_uart_struct __iomem *ch_cls_uart;	/* Pointer to the "mapped" UART struct */
 
-	uchar		ch_cached_lsr;	/* Cached value of the LSR register */
+	unsigned char	ch_cached_lsr;	/* Cached value of the LSR register */
 
-	uchar		*ch_rqueue;	/* Our read queue buffer - malloc'ed */
+	unsigned char	*ch_rqueue;	/* Our read queue buffer - malloc'ed */
 	ushort		ch_r_head;	/* Head location of the read queue */
 	ushort		ch_r_tail;	/* Tail location of the read queue */
 
-	uchar		*ch_equeue;	/* Our error queue buffer - malloc'ed */
+	unsigned char	*ch_equeue;	/* Our error queue buffer - malloc'ed */
 	ushort		ch_e_head;	/* Head location of the error queue */
 	ushort		ch_e_tail;	/* Tail location of the error queue */
 
-	uchar		*ch_wqueue;	/* Our write queue buffer - malloc'ed */
+	unsigned char	*ch_wqueue;	/* Our write queue buffer - malloc'ed */
 	ushort		ch_w_head;	/* Head location of the write queue */
 	ushort		ch_w_tail;	/* Tail location of the write queue */
 
 	ulong		ch_rxcount;	/* total of data received so far */
 	ulong		ch_txcount;	/* total of data transmitted so far */
 
-	uchar		ch_r_tlevel;	/* Receive Trigger level */
-	uchar		ch_t_tlevel;	/* Transmit Trigger level */
+	unsigned char		ch_r_tlevel;	/* Receive Trigger level */
+	unsigned char		ch_t_tlevel;	/* Transmit Trigger level */
 
-	uchar		ch_r_watermark;	/* Receive Watermark */
+	unsigned char		ch_r_watermark;	/* Receive Watermark */
 
 	ulong		ch_stop_sending_break;	/* Time we should STOP sending a break */
 
@@ -481,20 +449,9 @@ struct channel_t {
 	wait_queue_head_t ch_sniff_wait;
 };
 
-
-/*************************************************************************
- *
- * Prototypes for non-static functions used in more than one module
- *
- *************************************************************************/
-
-extern int		dgnc_ms_sleep(ulong ms);
-extern char		*dgnc_ioctl_name(int cmd);
-
 /*
  * Our Global Variables.
  */
-extern int		dgnc_driver_state;	/* The state of the driver	*/
 extern uint		dgnc_Major;		/* Our driver/mgmt major	*/
 extern int		dgnc_debug;		/* Debug variable		*/
 extern int		dgnc_rawreadok;		/* Set if user wants rawreads	*/
@@ -503,8 +460,6 @@ extern int		dgnc_trcbuf_size;	/* Size of the ringbuffer	*/
 extern spinlock_t	dgnc_global_lock;	/* Driver global spinlock	*/
 extern uint		dgnc_NumBoards;		/* Total number of boards	*/
 extern struct dgnc_board	*dgnc_Board[MAXBOARDS];	/* Array of board structs	*/
-extern ulong		dgnc_poll_counter;	/* Times the poller has run	*/
 extern char		*dgnc_state_text[];	/* Array of state text		*/
-extern char		*dgnc_driver_state_text[];/* Array of driver state text */
 
 #endif
diff --git a/drivers/staging/dgnc/dgnc_mgmt.c b/drivers/staging/dgnc/dgnc_mgmt.c
index c5b425bf6692..5544a8e7f4bc 100644
--- a/drivers/staging/dgnc/dgnc_mgmt.c
+++ b/drivers/staging/dgnc/dgnc_mgmt.c
@@ -62,29 +62,25 @@ static int dgnc_mgmt_in_use[MAXMGMTDEVICES];
  */
 int dgnc_mgmt_open(struct inode *inode, struct file *file)
 {
-	unsigned long lock_flags;
+	unsigned long flags;
 	unsigned int minor = iminor(inode);
 
-	DPR_MGMT(("dgnc_mgmt_open start.\n"));
-
-	DGNC_LOCK(dgnc_global_lock, lock_flags);
+	spin_lock_irqsave(&dgnc_global_lock, flags);
 
 	/* mgmt device */
 	if (minor < MAXMGMTDEVICES) {
 		/* Only allow 1 open at a time on mgmt device */
 		if (dgnc_mgmt_in_use[minor]) {
-			DGNC_UNLOCK(dgnc_global_lock, lock_flags);
+			spin_unlock_irqrestore(&dgnc_global_lock, flags);
 			return -EBUSY;
 		}
 		dgnc_mgmt_in_use[minor]++;
 	} else {
-		DGNC_UNLOCK(dgnc_global_lock, lock_flags);
+		spin_unlock_irqrestore(&dgnc_global_lock, flags);
 		return -ENXIO;
 	}
 
-	DGNC_UNLOCK(dgnc_global_lock, lock_flags);
-
-	DPR_MGMT(("dgnc_mgmt_open finish.\n"));
+	spin_unlock_irqrestore(&dgnc_global_lock, flags);
 
 	return 0;
 }
@@ -97,21 +93,17 @@ int dgnc_mgmt_open(struct inode *inode, struct file *file)
  */
 int dgnc_mgmt_close(struct inode *inode, struct file *file)
 {
-	unsigned long lock_flags;
+	unsigned long flags;
 	unsigned int minor = iminor(inode);
 
-	DPR_MGMT(("dgnc_mgmt_close start.\n"));
-
-	DGNC_LOCK(dgnc_global_lock, lock_flags);
+	spin_lock_irqsave(&dgnc_global_lock, flags);
 
 	/* mgmt device */
 	if (minor < MAXMGMTDEVICES) {
 		if (dgnc_mgmt_in_use[minor])
 			dgnc_mgmt_in_use[minor] = 0;
 	}
-	DGNC_UNLOCK(dgnc_global_lock, lock_flags);
-
-	DPR_MGMT(("dgnc_mgmt_close finish.\n"));
+	spin_unlock_irqrestore(&dgnc_global_lock, flags);
 
 	return 0;
 }
@@ -125,11 +117,9 @@ int dgnc_mgmt_close(struct inode *inode, struct file *file)
 
 long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 {
-	unsigned long lock_flags;
+	unsigned long flags;
 	void __user *uarg = (void __user *) arg;
 
-	DPR_MGMT(("dgnc_mgmt_ioctl start.\n"));
-
 	switch (cmd) {
 
 	case DIGI_GETDD:
@@ -141,15 +131,12 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		 */
 		struct digi_dinfo ddi;
 
-		DGNC_LOCK(dgnc_global_lock, lock_flags);
+		spin_lock_irqsave(&dgnc_global_lock, flags);
 
 		ddi.dinfo_nboards = dgnc_NumBoards;
 		sprintf(ddi.dinfo_version, "%s", DG_PART);
 
-		DGNC_UNLOCK(dgnc_global_lock, lock_flags);
-
-		DPR_MGMT(("DIGI_GETDD returning numboards: %d version: %s\n",
-			ddi.dinfo_nboards, ddi.dinfo_version));
+		spin_unlock_irqrestore(&dgnc_global_lock, flags);
 
 		if (copy_to_user(uarg, &ddi, sizeof(ddi)))
 			return -EFAULT;
@@ -166,8 +153,6 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		if (copy_from_user(&brd, uarg, sizeof(int)))
 			return -EFAULT;
 
-		DPR_MGMT(("DIGI_GETBD asking about board: %d\n", brd));
-
 		if ((brd < 0) || (brd > dgnc_NumBoards) ||
 		    (dgnc_NumBoards == 0))
 			return -ENODEV;
@@ -176,7 +161,7 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 		di.info_bdnum = brd;
 
-		DGNC_LOCK(dgnc_Board[brd]->bd_lock, lock_flags);
+		spin_lock_irqsave(&dgnc_Board[brd]->bd_lock, flags);
 
 		di.info_bdtype = dgnc_Board[brd]->dpatype;
 		di.info_bdstate = dgnc_Board[brd]->dpastatus;
@@ -188,10 +173,7 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		else
 			di.info_nports = 0;
 
-		DGNC_UNLOCK(dgnc_Board[brd]->bd_lock, lock_flags);
-
-		DPR_MGMT(("DIGI_GETBD returning type: %x state: %x ports: %x size: %x\n",
-			di.info_bdtype, di.info_bdstate, di.info_nports, di.info_physsize));
+		spin_unlock_irqrestore(&dgnc_Board[brd]->bd_lock, flags);
 
 		if (copy_to_user(uarg, &di, sizeof(di)))
 			return -EFAULT;
@@ -203,16 +185,13 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	{
 		struct channel_t *ch;
 		struct ni_info ni;
-		uchar mstat = 0;
+		unsigned char mstat = 0;
 		uint board = 0;
 		uint channel = 0;
 
 		if (copy_from_user(&ni, uarg, sizeof(ni)))
 			return -EFAULT;
 
-		DPR_MGMT(("DIGI_GETBD asking about board: %d channel: %d\n",
-			ni.board, ni.channel));
-
 		board = ni.board;
 		channel = ni.channel;
 
@@ -233,7 +212,7 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		ni.board = board;
 		ni.channel = channel;
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 
 		mstat = (ch->ch_mostat | ch->ch_mistat);
 
@@ -287,7 +266,7 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 		ni.baud = ch->ch_old_baud;
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		if (copy_to_user(uarg, &ni, sizeof(ni)))
 			return -EFAULT;
@@ -298,7 +277,5 @@ long dgnc_mgmt_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 	}
 
-	DPR_MGMT(("dgnc_mgmt_ioctl finish.\n"));
-
 	return 0;
 }
diff --git a/drivers/staging/dgnc/dgnc_neo.c b/drivers/staging/dgnc/dgnc_neo.c
index 68ff1161e677..a5bd08fef270 100644
--- a/drivers/staging/dgnc/dgnc_neo.c
+++ b/drivers/staging/dgnc/dgnc_neo.c
@@ -34,7 +34,7 @@
 #include <linux/sched.h>	/* For jiffies, task states */
 #include <linux/interrupt.h>    /* For tasklet and interrupt structs/defines */
 #include <linux/delay.h>	/* For udelay */
-#include <asm/io.h>		/* For read[bwl]/write[bwl] */
+#include <linux/io.h>		/* For read[bwl]/write[bwl] */
 #include <linux/serial.h>	/* For struct async_serial */
 #include <linux/serial_reg.h>	/* For the various UART offsets */
 
@@ -53,7 +53,7 @@ static inline void neo_set_ixoff_flow_control(struct channel_t *ch);
 static inline void neo_set_no_output_flow_control(struct channel_t *ch);
 static inline void neo_set_no_input_flow_control(struct channel_t *ch);
 static inline void neo_set_new_start_stop_chars(struct channel_t *ch);
-static void neo_parse_modem(struct channel_t *ch, uchar signals);
+static void neo_parse_modem(struct channel_t *ch, unsigned char signals);
 static void neo_tasklet(unsigned long data);
 static void neo_vpd(struct dgnc_board *brd);
 static void neo_uart_init(struct channel_t *ch);
@@ -113,12 +113,10 @@ static inline void neo_pci_posting_flush(struct dgnc_board *bd)
 
 static inline void neo_set_cts_flow_control(struct channel_t *ch)
 {
-	uchar ier = readb(&ch->ch_neo_uart->ier);
-	uchar efr = readb(&ch->ch_neo_uart->efr);
+	unsigned char ier = readb(&ch->ch_neo_uart->ier);
+	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 
-	DPR_PARAM(("Setting CTSFLOW\n"));
-
 	/* Turn on auto CTS flow control */
 #if 1
 	ier |= (UART_17158_IER_CTSDSR);
@@ -152,10 +150,8 @@ static inline void neo_set_cts_flow_control(struct channel_t *ch)
 
 static inline void neo_set_rts_flow_control(struct channel_t *ch)
 {
-	uchar ier = readb(&ch->ch_neo_uart->ier);
-	uchar efr = readb(&ch->ch_neo_uart->efr);
-
-	DPR_PARAM(("Setting RTSFLOW\n"));
+	unsigned char ier = readb(&ch->ch_neo_uart->ier);
+	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn on auto RTS flow control */
 #if 1
@@ -197,10 +193,8 @@ static inline void neo_set_rts_flow_control(struct channel_t *ch)
 
 static inline void neo_set_ixon_flow_control(struct channel_t *ch)
 {
-	uchar ier = readb(&ch->ch_neo_uart->ier);
-	uchar efr = readb(&ch->ch_neo_uart->efr);
-
-	DPR_PARAM(("Setting IXON FLOW\n"));
+	unsigned char ier = readb(&ch->ch_neo_uart->ier);
+	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto CTS flow control */
 	ier &= ~(UART_17158_IER_CTSDSR);
@@ -236,10 +230,8 @@ static inline void neo_set_ixon_flow_control(struct channel_t *ch)
 
 static inline void neo_set_ixoff_flow_control(struct channel_t *ch)
 {
-	uchar ier = readb(&ch->ch_neo_uart->ier);
-	uchar efr = readb(&ch->ch_neo_uart->efr);
-
-	DPR_PARAM(("Setting IXOFF FLOW\n"));
+	unsigned char ier = readb(&ch->ch_neo_uart->ier);
+	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto RTS flow control */
 	ier &= ~(UART_17158_IER_RTSDTR);
@@ -276,10 +268,8 @@ static inline void neo_set_ixoff_flow_control(struct channel_t *ch)
 
 static inline void neo_set_no_input_flow_control(struct channel_t *ch)
 {
-	uchar ier = readb(&ch->ch_neo_uart->ier);
-	uchar efr = readb(&ch->ch_neo_uart->efr);
-
-	DPR_PARAM(("Unsetting Input FLOW\n"));
+	unsigned char ier = readb(&ch->ch_neo_uart->ier);
+	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto RTS flow control */
 	ier &= ~(UART_17158_IER_RTSDTR);
@@ -318,10 +308,8 @@ static inline void neo_set_no_input_flow_control(struct channel_t *ch)
 
 static inline void neo_set_no_output_flow_control(struct channel_t *ch)
 {
-	uchar ier = readb(&ch->ch_neo_uart->ier);
-	uchar efr = readb(&ch->ch_neo_uart->efr);
-
-	DPR_PARAM(("Unsetting Output FLOW\n"));
+	unsigned char ier = readb(&ch->ch_neo_uart->ier);
+	unsigned char efr = readb(&ch->ch_neo_uart->efr);
 
 	/* Turn off auto CTS flow control */
 	ier &= ~(UART_17158_IER_CTSDSR);
@@ -364,8 +352,6 @@ static inline void neo_set_new_start_stop_chars(struct channel_t *ch)
 	if (ch->ch_digi.digi_flags & (CTSPACE | RTSPACE) || ch->ch_c_cflag & CRTSCTS)
 		return;
 
-	DPR_PARAM(("In new start stop chars\n"));
-
 	/* Tell UART what start/stop chars it should be looking for */
 	writeb(ch->ch_startc, &ch->ch_neo_uart->xonchar1);
 	writeb(0, &ch->ch_neo_uart->xonchar2);
@@ -382,13 +368,13 @@ static inline void neo_set_new_start_stop_chars(struct channel_t *ch)
  */
 static inline void neo_clear_break(struct channel_t *ch, int force)
 {
-	ulong lock_flags;
+	unsigned long flags;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* Bail if we aren't currently sending a break. */
 	if (!ch->ch_stop_sending_break) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
@@ -396,15 +382,15 @@ static inline void neo_clear_break(struct channel_t *ch, int force)
 	if (ch->ch_flags & CH_BREAK_SENDING) {
 		if (time_after_eq(jiffies, ch->ch_stop_sending_break)
 		    || force) {
-			uchar temp = readb(&ch->ch_neo_uart->lcr);
+			unsigned char temp = readb(&ch->ch_neo_uart->lcr);
+
 			writeb((temp & ~UART_LCR_SBC), &ch->ch_neo_uart->lcr);
 			neo_pci_posting_flush(ch->ch_bd);
 			ch->ch_flags &= ~(CH_BREAK_SENDING);
 			ch->ch_stop_sending_break = 0;
-			DPR_IOCTL(("Finishing UART_LCR_SBC! finished: %lx\n", jiffies));
 		}
 	}
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -414,9 +400,9 @@ static inline void neo_clear_break(struct channel_t *ch, int force)
 static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 {
 	struct channel_t *ch;
-	uchar isr;
-	uchar cause;
-	ulong lock_flags;
+	unsigned char isr;
+	unsigned char cause;
+	unsigned long flags;
 
 	if (!brd || brd->magic != DGNC_BOARD_MAGIC)
 		return;
@@ -442,8 +428,6 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 		 */
 		isr &= ~(UART_17158_IIR_FIFO_ENABLED);
 
-		DPR_INTR(("%s:%d isr: %x\n", __FILE__, __LINE__, isr));
-
 		if (isr & (UART_17158_IIR_RDI_TIMEOUT | UART_IIR_RDI)) {
 			/* Read data from uart -> queue */
 			brd->intr_rx++;
@@ -451,26 +435,24 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 			neo_copy_data_from_uart_to_queue(ch);
 
 			/* Call our tty layer to enforce queue flow control if needed. */
-			DGNC_LOCK(ch->ch_lock, lock_flags);
+			spin_lock_irqsave(&ch->ch_lock, flags);
 			dgnc_check_queue_flow_control(ch);
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 		}
 
 		if (isr & UART_IIR_THRI) {
 			brd->intr_tx++;
 			ch->ch_intr_tx++;
 			/* Transfer data (if any) from Write Queue -> UART. */
-			DGNC_LOCK(ch->ch_lock, lock_flags);
+			spin_lock_irqsave(&ch->ch_lock, flags);
 			ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			neo_copy_data_from_queue_to_uart(ch);
 		}
 
 		if (isr & UART_17158_IIR_XONXOFF) {
 			cause = readb(&ch->ch_neo_uart->xoffchar1);
 
-			DPR_INTR(("Port %d. Got ISR_XONXOFF: cause:%x\n", port, cause));
-
 			/*
 			 * Since the UART detected either an XON or
 			 * XOFF match, we need to figure out which
@@ -479,19 +461,20 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 			if (cause == UART_17158_XON_DETECT) {
 				/* Is output stopped right now, if so, resume it */
 				if (brd->channels[port]->ch_flags & CH_STOP) {
-					DGNC_LOCK(ch->ch_lock, lock_flags);
+					spin_lock_irqsave(&ch->ch_lock,
+							  flags);
 					ch->ch_flags &= ~(CH_STOP);
-					DGNC_UNLOCK(ch->ch_lock, lock_flags);
+					spin_unlock_irqrestore(&ch->ch_lock,
+							       flags);
 				}
-				DPR_INTR(("Port %d. XON detected in incoming data\n", port));
 			} else if (cause == UART_17158_XOFF_DETECT) {
 				if (!(brd->channels[port]->ch_flags & CH_STOP)) {
-					DGNC_LOCK(ch->ch_lock, lock_flags);
+					spin_lock_irqsave(&ch->ch_lock,
+							  flags);
 					ch->ch_flags |= CH_STOP;
-					DGNC_UNLOCK(ch->ch_lock, lock_flags);
-					DPR_INTR(("Setting CH_STOP\n"));
+					spin_unlock_irqrestore(&ch->ch_lock,
+							       flags);
 				}
-				DPR_INTR(("Port: %d. XOFF detected in incoming data\n", port));
 			}
 		}
 
@@ -506,29 +489,36 @@ static inline void neo_parse_isr(struct dgnc_board *brd, uint port)
 			/* Which pin is doing auto flow? RTS or DTR? */
 			if ((cause & 0x4) == 0) {
 				if (cause & UART_MCR_RTS) {
-					DGNC_LOCK(ch->ch_lock, lock_flags);
+					spin_lock_irqsave(&ch->ch_lock,
+							  flags);
 					ch->ch_mostat |= UART_MCR_RTS;
-					DGNC_UNLOCK(ch->ch_lock, lock_flags);
+					spin_unlock_irqrestore(&ch->ch_lock,
+							       flags);
 				} else {
-					DGNC_LOCK(ch->ch_lock, lock_flags);
+					spin_lock_irqsave(&ch->ch_lock,
+							  flags);
 					ch->ch_mostat &= ~(UART_MCR_RTS);
-					DGNC_UNLOCK(ch->ch_lock, lock_flags);
+					spin_unlock_irqrestore(&ch->ch_lock,
+							       flags);
 				}
 			} else {
 				if (cause & UART_MCR_DTR) {
-					DGNC_LOCK(ch->ch_lock, lock_flags);
+					spin_lock_irqsave(&ch->ch_lock,
+							  flags);
 					ch->ch_mostat |= UART_MCR_DTR;
-					DGNC_UNLOCK(ch->ch_lock, lock_flags);
+					spin_unlock_irqrestore(&ch->ch_lock,
+							       flags);
 				} else {
-					DGNC_LOCK(ch->ch_lock, lock_flags);
+					spin_lock_irqsave(&ch->ch_lock,
+							  flags);
 					ch->ch_mostat &= ~(UART_MCR_DTR);
-					DGNC_UNLOCK(ch->ch_lock, lock_flags);
+					spin_unlock_irqrestore(&ch->ch_lock,
+							       flags);
 				}
 			}
 		}
 
 		/* Parse any modem signal changes */
-		DPR_INTR(("MOD_STAT: sending to parse_modem_sigs\n"));
 		neo_parse_modem(ch, readb(&ch->ch_neo_uart->msr));
 	}
 }
@@ -538,7 +528,7 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 {
 	struct channel_t *ch;
 	int linestatus;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!brd)
 		return;
@@ -555,8 +545,6 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 
 	linestatus = readb(&ch->ch_neo_uart->lsr);
 
-	DPR_INTR(("%s:%d port: %d linestatus: %x\n", __FILE__, __LINE__, port, linestatus));
-
 	ch->ch_cached_lsr |= linestatus;
 
 	if (ch->ch_cached_lsr & UART_LSR_DR) {
@@ -564,20 +552,9 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 		ch->ch_intr_rx++;
 		/* Read data from uart -> queue */
 		neo_copy_data_from_uart_to_queue(ch);
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 		dgnc_check_queue_flow_control(ch);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-	}
-
-	/*
-	 * This is a special flag. It indicates that at least 1
-	 * RX error (parity, framing, or break) has happened.
-	 * Mark this in our struct, which will tell me that I have
-	 *to do the special RX+LSR read for this FIFO load.
-	 */
-	if (linestatus & UART_17158_RX_FIFO_DATA_ERROR) {
-		DPR_INTR(("%s:%d Port: %d Got an RX error, need to parse LSR\n",
-			__FILE__, __LINE__, port));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 	}
 
 	/*
@@ -585,20 +562,14 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 	 * should encapsulate all 3... At least, thats what Exar says.
 	 */
 
-	if (linestatus & UART_LSR_PE) {
+	if (linestatus & UART_LSR_PE)
 		ch->ch_err_parity++;
-		DPR_INTR(("%s:%d Port: %d. PAR ERR!\n", __FILE__, __LINE__, port));
-	}
 
-	if (linestatus & UART_LSR_FE) {
+	if (linestatus & UART_LSR_FE)
 		ch->ch_err_frame++;
-		DPR_INTR(("%s:%d Port: %d. FRM ERR!\n", __FILE__, __LINE__, port));
-	}
 
-	if (linestatus & UART_LSR_BI) {
+	if (linestatus & UART_LSR_BI)
 		ch->ch_err_break++;
-		DPR_INTR(("%s:%d Port: %d. BRK INTR!\n", __FILE__, __LINE__, port));
-	}
 
 	if (linestatus & UART_LSR_OE) {
 		/*
@@ -608,24 +579,23 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
 		 * Probably we should eventually have an orun stat in our driver...
 		 */
 		ch->ch_err_overrun++;
-		DPR_INTR(("%s:%d Port: %d. Rx Overrun!\n", __FILE__, __LINE__, port));
 	}
 
 	if (linestatus & UART_LSR_THRE) {
 		brd->intr_tx++;
 		ch->ch_intr_tx++;
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 		ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		/* Transfer data (if any) from Write Queue -> UART. */
 		neo_copy_data_from_queue_to_uart(ch);
 	} else if (linestatus & UART_17158_TX_AND_FIFO_CLR) {
 		brd->intr_tx++;
 		ch->ch_intr_tx++;
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 		ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		/* Transfer data (if any) from Write Queue -> UART. */
 		neo_copy_data_from_queue_to_uart(ch);
@@ -639,10 +609,10 @@ static inline void neo_parse_lsr(struct dgnc_board *brd, uint port)
  */
 static void neo_param(struct tty_struct *tty)
 {
-	uchar lcr = 0;
-	uchar uart_lcr = 0;
-	uchar ier = 0;
-	uchar uart_ier = 0;
+	unsigned char lcr = 0;
+	unsigned char uart_lcr = 0;
+	unsigned char ier = 0;
+	unsigned char uart_ier = 0;
 	uint baud = 9600;
 	int quot = 0;
 	struct dgnc_board *bd;
@@ -664,16 +634,16 @@ static void neo_param(struct tty_struct *tty)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DPR_PARAM(("param start: tdev: %x cflags: %x oflags: %x iflags: %x\n",
-		ch->ch_tun.un_dev, ch->ch_c_cflag, ch->ch_c_oflag, ch->ch_c_iflag));
-
 	/*
 	 * If baud rate is zero, flush queues, and set mval to drop DTR.
 	 */
 	if ((ch->ch_c_cflag & (CBAUD)) == 0) {
-		ch->ch_r_head = ch->ch_r_tail = 0;
-		ch->ch_e_head = ch->ch_e_tail = 0;
-		ch->ch_w_head = ch->ch_w_tail = 0;
+		ch->ch_r_head = 0;
+		ch->ch_r_tail = 0;
+		ch->ch_e_head = 0;
+		ch->ch_e_tail = 0;
+		ch->ch_w_head = 0;
+		ch->ch_w_tail = 0;
 
 		neo_flush_uart_write(ch);
 		neo_flush_uart_read(ch);
@@ -742,13 +712,10 @@ static void neo_param(struct tty_struct *tty)
 
 		jindex = baud;
 
-		if ((iindex >= 0) && (iindex < 4) && (jindex >= 0) && (jindex < 16)) {
+		if ((iindex >= 0) && (iindex < 4) && (jindex >= 0) && (jindex < 16))
 			baud = bauds[iindex][jindex];
-		} else {
-			DPR_IOCTL(("baud indices were out of range (%d)(%d)",
-				iindex, jindex));
+		else
 			baud = 0;
-		}
 
 		if (baud == 0)
 			baud = 9600;
@@ -802,7 +769,9 @@ static void neo_param(struct tty_struct *tty)
 		break;
 	}
 
-	ier = uart_ier = readb(&ch->ch_neo_uart->ier);
+	uart_ier = readb(&ch->ch_neo_uart->ier);
+	ier = uart_ier;
+
 	uart_lcr = readb(&ch->ch_neo_uart->lcr);
 
 	if (baud == 0)
@@ -895,7 +864,7 @@ static void neo_tasklet(unsigned long data)
 {
 	struct dgnc_board *bd = (struct dgnc_board *) data;
 	struct channel_t *ch;
-	ulong  lock_flags;
+	unsigned long flags;
 	int i;
 	int state = 0;
 	int ports = 0;
@@ -906,16 +875,16 @@ static void neo_tasklet(unsigned long data)
 	}
 
 	/* Cache a couple board values */
-	DGNC_LOCK(bd->bd_lock, lock_flags);
+	spin_lock_irqsave(&bd->bd_lock, flags);
 	state = bd->state;
 	ports = bd->nasync;
-	DGNC_UNLOCK(bd->bd_lock, lock_flags);
+	spin_unlock_irqrestore(&bd->bd_lock, flags);
 
 	/*
 	 * Do NOT allow the interrupt routine to read the intr registers
 	 * Until we release this lock.
 	 */
-	DGNC_LOCK(bd->bd_intr_lock, lock_flags);
+	spin_lock_irqsave(&bd->bd_intr_lock, flags);
 
 	/*
 	 * If board is ready, parse deeper to see if there is anything to do.
@@ -964,7 +933,7 @@ static void neo_tasklet(unsigned long data)
 	}
 
 	/* Allow interrupt routine to access the interrupt register again */
-	DGNC_UNLOCK(bd->bd_intr_lock, lock_flags);
+	spin_unlock_irqrestore(&bd->bd_intr_lock, flags);
 
 }
 
@@ -983,8 +952,8 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 	int current_port;
 	u32 tmp;
 	u32 uart_poll;
-	unsigned long lock_flags;
-	unsigned long lock_flags2;
+	unsigned long flags;
+	unsigned long flags2;
 
 	if (!brd) {
 		APR(("Received interrupt (%d) with null board associated\n", irq));
@@ -1002,7 +971,7 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 	brd->intr_count++;
 
 	/* Lock out the slow poller from running on this board. */
-	DGNC_LOCK(brd->bd_intr_lock, lock_flags);
+	spin_lock_irqsave(&brd->bd_intr_lock, flags);
 
 	/*
 	 * Read in "extended" IRQ information from the 32bit Neo register.
@@ -1011,15 +980,12 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 	 */
 	uart_poll = readl(brd->re_map_membase + UART_17158_POLL_ADDR_OFFSET);
 
-	DPR_INTR(("%s:%d uart_poll: %x\n", __FILE__, __LINE__, uart_poll));
-
 	/*
 	 * If 0, no interrupts pending.
 	 * This can happen if the IRQ is shared among a couple Neo/Classic boards.
 	 */
 	if (!uart_poll) {
-		DPR_INTR(("Kernel interrupted to me, but no pending interrupts...\n"));
-		DGNC_UNLOCK(brd->bd_intr_lock, lock_flags);
+		spin_unlock_irqrestore(&brd->bd_intr_lock, flags);
 		return IRQ_NONE;
 	}
 
@@ -1042,14 +1008,11 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 			continue;
 		}
 
-		DPR_INTR(("%s:%d port: %x type: %x\n", __FILE__, __LINE__, port, type));
-
 		/* Remove this port + type from uart_poll */
 		uart_poll &= ~(dgnc_offset_table[port]);
 
 		if (!type) {
 			/* If no type, just ignore it, and move onto next port */
-			DPR_INTR(("Interrupt with no type! port: %d\n", port));
 			continue;
 		}
 
@@ -1070,9 +1033,9 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 			neo_copy_data_from_uart_to_queue(ch);
 
 			/* Call our tty layer to enforce queue flow control if needed. */
-			DGNC_LOCK(ch->ch_lock, lock_flags2);
+			spin_lock_irqsave(&ch->ch_lock, flags2);
 			dgnc_check_queue_flow_control(ch);
-			DGNC_UNLOCK(ch->ch_lock, lock_flags2);
+			spin_unlock_irqrestore(&ch->ch_lock, flags2);
 
 			continue;
 
@@ -1113,7 +1076,6 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 			 * these once and awhile.
 			 * Its harmless, just ignore it and move on.
 			 */
-			DPR_INTR(("%s:%d Unknown Interrupt type: %x\n", __FILE__, __LINE__, type));
 			continue;
 		}
 	}
@@ -1123,9 +1085,8 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
 	 */
 	tasklet_schedule(&brd->helper_tasklet);
 
-	DGNC_UNLOCK(brd->bd_intr_lock, lock_flags);
+	spin_unlock_irqrestore(&brd->bd_intr_lock, flags);
 
-	DPR_INTR(("dgnc_intr finish.\n"));
 	return IRQ_HANDLED;
 }
 
@@ -1137,7 +1098,8 @@ static irqreturn_t neo_intr(int irq, void *voidbrd)
  */
 static void neo_disable_receiver(struct channel_t *ch)
 {
-	uchar tmp = readb(&ch->ch_neo_uart->ier);
+	unsigned char tmp = readb(&ch->ch_neo_uart->ier);
+
 	tmp &= ~(UART_IER_RDI);
 	writeb(tmp, &ch->ch_neo_uart->ier);
 	neo_pci_posting_flush(ch->ch_bd);
@@ -1151,7 +1113,8 @@ static void neo_disable_receiver(struct channel_t *ch)
  */
 static void neo_enable_receiver(struct channel_t *ch)
 {
-	uchar tmp = readb(&ch->ch_neo_uart->ier);
+	unsigned char tmp = readb(&ch->ch_neo_uart->ier);
+
 	tmp |= (UART_IER_RDI);
 	writeb(tmp, &ch->ch_neo_uart->ier);
 	neo_pci_posting_flush(ch->ch_bd);
@@ -1161,18 +1124,18 @@ static void neo_enable_receiver(struct channel_t *ch)
 static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 {
 	int qleft = 0;
-	uchar linestatus = 0;
-	uchar error_mask = 0;
+	unsigned char linestatus = 0;
+	unsigned char error_mask = 0;
 	int n = 0;
 	int total = 0;
 	ushort head;
 	ushort tail;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* cache head and tail of queue */
 	head = ch->ch_r_head & RQUEUEMASK;
@@ -1323,7 +1286,8 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * Discard character if we are ignoring the error mask.
 		 */
 		if (linestatus & error_mask)  {
-			uchar discard;
+			unsigned char discard;
+
 			linestatus = 0;
 			memcpy_fromio(&discard, &ch->ch_neo_uart->txrxburst, 1);
 			continue;
@@ -1338,20 +1302,16 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 		 * I hope thats okay with everyone? Yes? Good.
 		 */
 		while (qleft < 1) {
-			DPR_READ(("Queue full, dropping DATA:%x LSR:%x\n",
-				ch->ch_rqueue[tail], ch->ch_equeue[tail]));
-
-			ch->ch_r_tail = tail = (tail + 1) & RQUEUEMASK;
+			tail = (tail + 1) & RQUEUEMASK;
+			ch->ch_r_tail = tail;
 			ch->ch_err_overrun++;
 			qleft++;
 		}
 
 		memcpy_fromio(ch->ch_rqueue + head, &ch->ch_neo_uart->txrxburst, 1);
-		ch->ch_equeue[head] = (uchar) linestatus;
+		ch->ch_equeue[head] = (unsigned char) linestatus;
 		dgnc_sniff_nowait_nolock(ch, "UART READ", ch->ch_rqueue + head, 1);
 
-		DPR_READ(("DATA/LSR pair: %x %x\n", ch->ch_rqueue[head], ch->ch_equeue[head]));
-
 		/* Ditch any remaining linestatus value. */
 		linestatus = 0;
 
@@ -1368,7 +1328,7 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
 	ch->ch_r_head = head & RQUEUEMASK;
 	ch->ch_e_head = head & EQUEUEMASK;
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -1378,7 +1338,7 @@ static void neo_copy_data_from_uart_to_queue(struct channel_t *ch)
  */
 static int neo_drain(struct tty_struct *tty, uint seconds)
 {
-	ulong lock_flags;
+	unsigned long flags;
 	struct channel_t *ch;
 	struct un_t *un;
 	int rc = 0;
@@ -1394,11 +1354,9 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return -ENXIO;
 
-	DPR_IOCTL(("%d Drain wait started.\n", __LINE__));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 	un->un_flags |= UN_EMPTY;
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	/*
 	 * Go to sleep waiting for the tty layer to wake me back up when
@@ -1409,11 +1367,6 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
 	rc = wait_event_interruptible(un->un_flags_wait, ((un->un_flags & UN_EMPTY) == 0));
 
 	/* If ret is non-zero, user ctrl-c'ed us */
-	if (rc)
-		DPR_IOCTL(("%d Drain - User ctrl c'ed\n", __LINE__));
-	else
-		DPR_IOCTL(("%d Drain wait finished.\n", __LINE__));
-
 	return rc;
 }
 
@@ -1425,7 +1378,7 @@ static int neo_drain(struct tty_struct *tty, uint seconds)
  */
 static void neo_flush_uart_write(struct channel_t *ch)
 {
-	uchar tmp = 0;
+	unsigned char tmp = 0;
 	int i = 0;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
@@ -1438,10 +1391,9 @@ static void neo_flush_uart_write(struct channel_t *ch)
 
 		/* Check to see if the UART feels it completely flushed the FIFO. */
 		tmp = readb(&ch->ch_neo_uart->isr_fcr);
-		if (tmp & 4) {
-			DPR_IOCTL(("Still flushing TX UART... i: %d\n", i));
+		if (tmp & 4)
 			udelay(10);
-		} else
+		else
 			break;
 	}
 
@@ -1456,7 +1408,7 @@ static void neo_flush_uart_write(struct channel_t *ch)
  */
 static void neo_flush_uart_read(struct channel_t *ch)
 {
-	uchar tmp = 0;
+	unsigned char tmp = 0;
 	int i = 0;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
@@ -1469,10 +1421,9 @@ static void neo_flush_uart_read(struct channel_t *ch)
 
 		/* Check to see if the UART feels it completely flushed the FIFO. */
 		tmp = readb(&ch->ch_neo_uart->isr_fcr);
-		if (tmp & 2) {
-			DPR_IOCTL(("Still flushing RX UART... i: %d\n", i));
+		if (tmp & 2)
 			udelay(10);
-		} else
+		else
 			break;
 	}
 }
@@ -1486,22 +1437,22 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 	int s;
 	int qlen;
 	uint len_written = 0;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* No data to write to the UART */
 	if (ch->ch_w_tail == ch->ch_w_head) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
 	/* If port is "stopped", don't send any data to the UART */
 	if ((ch->ch_flags & CH_FORCED_STOP) || (ch->ch_flags & CH_BREAK_SENDING)) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
@@ -1509,7 +1460,7 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 	 * If FIFOs are disabled. Send data directly to txrx register
 	 */
 	if (!(ch->ch_flags & CH_FIFO_ENABLED)) {
-		uchar lsrbits = readb(&ch->ch_neo_uart->lsr);
+		unsigned char lsrbits = readb(&ch->ch_neo_uart->lsr);
 
 		/* Cache the LSR bits for later parsing */
 		ch->ch_cached_lsr |= lsrbits;
@@ -1540,12 +1491,11 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 			}
 
 			writeb(ch->ch_wqueue[ch->ch_w_tail], &ch->ch_neo_uart->txrx);
-			DPR_WRITE(("Tx data: %x\n", ch->ch_wqueue[ch->ch_w_head]));
 			ch->ch_w_tail++;
 			ch->ch_w_tail &= WQUEUEMASK;
 			ch->ch_txcount++;
 		}
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
@@ -1554,7 +1504,7 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 	 */
 	if ((ch->ch_bd->dvid & 0xf0) < UART_XR17E158_DVID) {
 		if (!(ch->ch_flags & (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM))) {
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			return;
 		}
 
@@ -1563,7 +1513,7 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		n = readb(&ch->ch_neo_uart->tfifo);
 
 		if ((unsigned int) n > ch->ch_t_tlevel) {
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			return;
 		}
 
@@ -1630,25 +1580,23 @@ static void neo_copy_data_from_queue_to_uart(struct channel_t *ch)
 		ch->ch_flags &= ~(CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
 	}
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
-static void neo_parse_modem(struct channel_t *ch, uchar signals)
+static void neo_parse_modem(struct channel_t *ch, unsigned char signals)
 {
-	volatile uchar msignals = signals;
+	unsigned char msignals = signals;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DPR_MSIGS(("neo_parse_modem: port: %d msignals: %x\n", ch->ch_portnum, msignals));
-
 	/*
 	 * Do altpin switching. Altpin switches DCD and DSR.
 	 * This prolly breaks DSRPACE, so we should be more clever here.
 	 */
 	if (ch->ch_digi.digi_flags & DIGI_ALTPIN) {
-		uchar mswap = msignals;
+		unsigned char mswap = msignals;
 
 		if (mswap & UART_MSR_DDCD) {
 			msignals &= ~UART_MSR_DDCD;
@@ -1690,22 +1638,13 @@ static void neo_parse_modem(struct channel_t *ch, uchar signals)
 		ch->ch_mistat |= UART_MSR_CTS;
 	else
 		ch->ch_mistat &= ~UART_MSR_CTS;
-
-	DPR_MSIGS(("Port: %d DTR: %d RTS: %d CTS: %d DSR: %d " "RI: %d CD: %d\n",
-		ch->ch_portnum,
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MCR_DTR),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MCR_RTS),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_CTS),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_DSR),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_RI),
-		!!((ch->ch_mistat | ch->ch_mostat) & UART_MSR_DCD)));
 }
 
 
 /* Make the UART raise any of the output signals we want up */
 static void neo_assert_modem_signals(struct channel_t *ch)
 {
-	uchar out;
+	unsigned char out;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
@@ -1792,17 +1731,16 @@ static void neo_uart_off(struct channel_t *ch)
 
 static uint neo_get_uart_bytes_left(struct channel_t *ch)
 {
-	uchar left = 0;
-	uchar lsr = readb(&ch->ch_neo_uart->lsr);
+	unsigned char left = 0;
+	unsigned char lsr = readb(&ch->ch_neo_uart->lsr);
 
 	/* We must cache the LSR as some of the bits get reset once read... */
 	ch->ch_cached_lsr |= lsr;
 
 	/* Determine whether the Transmitter is empty or not */
 	if (!(lsr & UART_LSR_TEMT)) {
-		if (ch->ch_flags & CH_TX_FIFO_EMPTY) {
+		if (ch->ch_flags & CH_TX_FIFO_EMPTY)
 			tasklet_schedule(&ch->ch_bd->helper_tasklet);
-		}
 		left = 1;
 	} else {
 		ch->ch_flags |= (CH_TX_FIFO_EMPTY | CH_TX_FIFO_LWM);
@@ -1821,12 +1759,12 @@ static void neo_send_break(struct channel_t *ch, int msecs)
 	 */
 	if (msecs == 0) {
 		if (ch->ch_flags & CH_BREAK_SENDING) {
-			uchar temp = readb(&ch->ch_neo_uart->lcr);
+			unsigned char temp = readb(&ch->ch_neo_uart->lcr);
+
 			writeb((temp & ~UART_LCR_SBC), &ch->ch_neo_uart->lcr);
 			neo_pci_posting_flush(ch->ch_bd);
 			ch->ch_flags &= ~(CH_BREAK_SENDING);
 			ch->ch_stop_sending_break = 0;
-			DPR_IOCTL(("Finishing UART_LCR_SBC! finished: %lx\n", jiffies));
 		}
 		return;
 	}
@@ -1840,12 +1778,11 @@ static void neo_send_break(struct channel_t *ch, int msecs)
 
 	/* Tell the UART to start sending the break */
 	if (!(ch->ch_flags & CH_BREAK_SENDING)) {
-		uchar temp = readb(&ch->ch_neo_uart->lcr);
+		unsigned char temp = readb(&ch->ch_neo_uart->lcr);
+
 		writeb((temp | UART_LCR_SBC), &ch->ch_neo_uart->lcr);
 		neo_pci_posting_flush(ch->ch_bd);
 		ch->ch_flags |= (CH_BREAK_SENDING);
-		DPR_IOCTL(("Port %d. Starting UART_LCR_SBC! start: %lx should end: %lx\n",
-			ch->ch_portnum, jiffies, ch->ch_stop_sending_break));
 	}
 }
 
@@ -1929,8 +1866,8 @@ static void neo_vpd(struct dgnc_board *brd)
 
 	if  (((brd->vpd[0x08] != 0x82)	   /* long resource name tag */
 		&&  (brd->vpd[0x10] != 0x82))   /* long resource name tag (PCI-66 files)*/
-		||  (brd->vpd[0x7F] != 0x78))   /* small resource end tag */
-	{
+		||  (brd->vpd[0x7F] != 0x78)) { /* small resource end tag */
+
 		memset(brd->vpd, '\0', NEO_VPD_IMAGESIZE);
 	} else {
 		/* Search for the serial number */
diff --git a/drivers/staging/dgnc/dgnc_sysfs.c b/drivers/staging/dgnc/dgnc_sysfs.c
index 3f321bb2b79d..6c3b387622e9 100644
--- a/drivers/staging/dgnc/dgnc_sysfs.c
+++ b/drivers/staging/dgnc/dgnc_sysfs.c
@@ -63,21 +63,6 @@ static ssize_t dgnc_driver_maxboards_show(struct device_driver *ddp, char *buf)
 }
 static DRIVER_ATTR(maxboards, S_IRUSR, dgnc_driver_maxboards_show, NULL);
 
-
-static ssize_t dgnc_driver_pollcounter_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%ld\n", dgnc_poll_counter);
-}
-static DRIVER_ATTR(pollcounter, S_IRUSR, dgnc_driver_pollcounter_show, NULL);
-
-
-static ssize_t dgnc_driver_state_show(struct device_driver *ddp, char *buf)
-{
-	return snprintf(buf, PAGE_SIZE, "%s\n", dgnc_driver_state_text[dgnc_driver_state]);
-}
-static DRIVER_ATTR(state, S_IRUSR, dgnc_driver_state_show, NULL);
-
-
 static ssize_t dgnc_driver_debug_show(struct device_driver *ddp, char *buf)
 {
 	return snprintf(buf, PAGE_SIZE, "0x%x\n", dgnc_debug);
@@ -85,7 +70,11 @@ static ssize_t dgnc_driver_debug_show(struct device_driver *ddp, char *buf)
 
 static ssize_t dgnc_driver_debug_store(struct device_driver *ddp, const char *buf, size_t count)
 {
-	sscanf(buf, "0x%x\n", &dgnc_debug);
+	int ret;
+
+	ret = sscanf(buf, "0x%x\n", &dgnc_debug);
+	if (ret != 1)
+		return -EINVAL;
 	return count;
 }
 static DRIVER_ATTR(debug, (S_IRUSR | S_IWUSR), dgnc_driver_debug_show, dgnc_driver_debug_store);
@@ -98,7 +87,11 @@ static ssize_t dgnc_driver_rawreadok_show(struct device_driver *ddp, char *buf)
 
 static ssize_t dgnc_driver_rawreadok_store(struct device_driver *ddp, const char *buf, size_t count)
 {
-	sscanf(buf, "0x%x\n", &dgnc_rawreadok);
+	int ret;
+
+	ret = sscanf(buf, "0x%x\n", &dgnc_rawreadok);
+	if (ret != 1)
+		return -EINVAL;
 	return count;
 }
 static DRIVER_ATTR(rawreadok, (S_IRUSR | S_IWUSR), dgnc_driver_rawreadok_show, dgnc_driver_rawreadok_store);
@@ -111,7 +104,11 @@ static ssize_t dgnc_driver_pollrate_show(struct device_driver *ddp, char *buf)
 
 static ssize_t dgnc_driver_pollrate_store(struct device_driver *ddp, const char *buf, size_t count)
 {
-	sscanf(buf, "%d\n", &dgnc_poll_tick);
+	int ret;
+
+	ret = sscanf(buf, "%d\n", &dgnc_poll_tick);
+	if (ret != 1)
+		return -EINVAL;
 	return count;
 }
 static DRIVER_ATTR(pollrate, (S_IRUSR | S_IWUSR), dgnc_driver_pollrate_show, dgnc_driver_pollrate_store);
@@ -128,25 +125,21 @@ void dgnc_create_driver_sysfiles(struct pci_driver *dgnc_driver)
 	rc |= driver_create_file(driverfs, &driver_attr_debug);
 	rc |= driver_create_file(driverfs, &driver_attr_rawreadok);
 	rc |= driver_create_file(driverfs, &driver_attr_pollrate);
-	rc |= driver_create_file(driverfs, &driver_attr_pollcounter);
-	rc |= driver_create_file(driverfs, &driver_attr_state);
-	if (rc) {
+	if (rc)
 		printk(KERN_ERR "DGNC: sysfs driver_create_file failed!\n");
-	}
 }
 
 
 void dgnc_remove_driver_sysfiles(struct pci_driver *dgnc_driver)
 {
 	struct device_driver *driverfs = &dgnc_driver->driver;
+
 	driver_remove_file(driverfs, &driver_attr_version);
 	driver_remove_file(driverfs, &driver_attr_boards);
 	driver_remove_file(driverfs, &driver_attr_maxboards);
 	driver_remove_file(driverfs, &driver_attr_debug);
 	driver_remove_file(driverfs, &driver_attr_rawreadok);
 	driver_remove_file(driverfs, &driver_attr_pollrate);
-	driver_remove_file(driverfs, &driver_attr_pollcounter);
-	driver_remove_file(driverfs, &driver_attr_state);
 }
 
 
@@ -403,9 +396,8 @@ void dgnc_create_ports_sysfiles(struct dgnc_board *bd)
 	rc |= device_create_file(&(bd->pdev->dev), &dev_attr_ports_txcount);
 	rc |= device_create_file(&(bd->pdev->dev), &dev_attr_vpd);
 	rc |= device_create_file(&(bd->pdev->dev), &dev_attr_serial_number);
-	if (rc) {
+	if (rc)
 		printk(KERN_ERR "DGNC: sysfs device_create_file failed!\n");
-	}
 }
 
 
diff --git a/drivers/staging/dgnc/dgnc_tty.c b/drivers/staging/dgnc/dgnc_tty.c
index c712b431f969..03c15069731f 100644
--- a/drivers/staging/dgnc/dgnc_tty.c
+++ b/drivers/staging/dgnc/dgnc_tty.c
@@ -47,7 +47,7 @@
 #include <linux/serial_reg.h>
 #include <linux/slab.h>
 #include <linux/delay.h>	/* For udelay */
-#include <asm/uaccess.h>	/* For copy_from_user/copy_to_user */
+#include <linux/uaccess.h>	/* For copy_from_user/copy_to_user */
 #include <linux/pci.h>
 
 #include "dgnc_driver.h"
@@ -57,6 +57,7 @@
 #include "dgnc_cls.h"
 #include "dpacompat.h"
 #include "dgnc_sysfs.h"
+#include "dgnc_utils.h"
 
 #define init_MUTEX(sem)	 sema_init(sem, 1)
 #define DECLARE_MUTEX(name)     \
@@ -66,7 +67,7 @@
  * internal variables
  */
 static struct dgnc_board	*dgnc_BoardsByMajor[256];
-static uchar		*dgnc_TmpWriteBuf = NULL;
+static unsigned char		*dgnc_TmpWriteBuf;
 static DECLARE_MUTEX(dgnc_TmpWriteSem);
 
 /*
@@ -92,8 +93,7 @@ static struct digi_t dgnc_digi_init = {
  * This defines a raw port at 9600 baud, 8 data bits, no parity,
  * 1 stop bit.
  */
-static struct ktermios DgncDefaultTermios =
-{
+static struct ktermios DgncDefaultTermios = {
 	.c_iflag =	(DEFAULT_IFLAGS),	/* iflags */
 	.c_oflag =	(DEFAULT_OFLAGS),	/* oflags */
 	.c_cflag =	(DEFAULT_CFLAGS),	/* cflags */
@@ -179,10 +179,8 @@ int dgnc_tty_preinit(void)
 	 */
 	dgnc_TmpWriteBuf = kmalloc(WRITEBUFLEN, GFP_KERNEL);
 
-	if (!dgnc_TmpWriteBuf) {
-		DPR_INIT(("unable to allocate tmp write buf"));
+	if (!dgnc_TmpWriteBuf)
 		return -ENOMEM;
-	}
 
 	return 0;
 }
@@ -197,8 +195,6 @@ int dgnc_tty_register(struct dgnc_board *brd)
 {
 	int rc = 0;
 
-	DPR_INIT(("tty_register start\n"));
-
 	brd->SerialDriver.magic = TTY_DRIVER_MAGIC;
 
 	snprintf(brd->SerialName, MAXTTYNAMELEN, "tty_dgnc_%d_", brd->boardnum);
@@ -218,12 +214,12 @@ int dgnc_tty_register(struct dgnc_board *brd)
 	 * The kernel wants space to store pointers to
 	 * tty_struct's and termios's.
 	 */
-	brd->SerialDriver.ttys = kzalloc(brd->maxports * sizeof(*brd->SerialDriver.ttys), GFP_KERNEL);
+	brd->SerialDriver.ttys = kcalloc(brd->maxports, sizeof(*brd->SerialDriver.ttys), GFP_KERNEL);
 	if (!brd->SerialDriver.ttys)
 		return -ENOMEM;
 
 	kref_init(&brd->SerialDriver.kref);
-	brd->SerialDriver.termios = kzalloc(brd->maxports * sizeof(*brd->SerialDriver.termios), GFP_KERNEL);
+	brd->SerialDriver.termios = kcalloc(brd->maxports, sizeof(*brd->SerialDriver.termios), GFP_KERNEL);
 	if (!brd->SerialDriver.termios)
 		return -ENOMEM;
 
@@ -267,11 +263,11 @@ int dgnc_tty_register(struct dgnc_board *brd)
 	 * tty_struct's and termios's.  Must be separated from
 	 * the Serial Driver so we don't get confused
 	 */
-	brd->PrintDriver.ttys = kzalloc(brd->maxports * sizeof(*brd->PrintDriver.ttys), GFP_KERNEL);
+	brd->PrintDriver.ttys = kcalloc(brd->maxports, sizeof(*brd->PrintDriver.ttys), GFP_KERNEL);
 	if (!brd->PrintDriver.ttys)
 		return -ENOMEM;
 	kref_init(&brd->PrintDriver.kref);
-	brd->PrintDriver.termios = kzalloc(brd->maxports * sizeof(*brd->PrintDriver.termios), GFP_KERNEL);
+	brd->PrintDriver.termios = kcalloc(brd->maxports, sizeof(*brd->PrintDriver.termios), GFP_KERNEL);
 	if (!brd->PrintDriver.termios)
 		return -ENOMEM;
 
@@ -295,8 +291,6 @@ int dgnc_tty_register(struct dgnc_board *brd)
 	brd->dgnc_Serial_Major = brd->SerialDriver.major;
 	brd->dgnc_TransparentPrint_Major = brd->PrintDriver.major;
 
-	DPR_INIT(("DGNC REGISTER TTY: MAJOR: %d\n", brd->SerialDriver.major));
-
 	return rc;
 }
 
@@ -316,8 +310,6 @@ int dgnc_tty_init(struct dgnc_board *brd)
 	if (!brd)
 		return -ENXIO;
 
-	DPR_INIT(("dgnc_tty_init start\n"));
-
 	/*
 	 * Initialize board structure elements.
 	 */
@@ -338,10 +330,6 @@ int dgnc_tty_init(struct dgnc_board *brd)
 			 * interrupt context, and there are no locks held.
 			 */
 			brd->channels[i] = kzalloc(sizeof(*brd->channels[i]), GFP_KERNEL);
-			if (!brd->channels[i]) {
-				DPR_CORE(("%s:%d Unable to allocate memory for channel struct\n",
-				    __FILE__, __LINE__));
-			}
 		}
 	}
 
@@ -354,7 +342,7 @@ int dgnc_tty_init(struct dgnc_board *brd)
 		if (!brd->channels[i])
 			continue;
 
-		DGNC_SPINLOCK_INIT(ch->ch_lock);
+		spin_lock_init(&ch->ch_lock);
 
 		/* Store all our magic numbers */
 		ch->magic = DGNC_CHANNEL_MAGIC;
@@ -387,6 +375,7 @@ int dgnc_tty_init(struct dgnc_board *brd)
 
 		{
 			struct device *classp;
+
 			classp = tty_register_device(&brd->SerialDriver, i,
 				&(ch->ch_bd->pdev->dev));
 			ch->ch_tun.un_sysfs = classp;
@@ -400,8 +389,6 @@ int dgnc_tty_init(struct dgnc_board *brd)
 
 	}
 
-	DPR_INIT(("dgnc_tty_init finish\n"));
-
 	return 0;
 }
 
@@ -463,7 +450,7 @@ void dgnc_tty_uninit(struct dgnc_board *brd)
  * dgnc_sniff - Dump data out to the "sniff" buffer if the
  * proc sniff file is opened...
  */
-void dgnc_sniff_nowait_nolock(struct channel_t *ch, uchar *text, uchar *buf, int len)
+void dgnc_sniff_nowait_nolock(struct channel_t *ch, unsigned char *text, unsigned char *buf, int len)
 {
 	struct timeval tv;
 	int n;
@@ -471,13 +458,18 @@ void dgnc_sniff_nowait_nolock(struct channel_t *ch, uchar *text, uchar *buf, int
 	int nbuf;
 	int i;
 	int tmpbuflen;
-	char tmpbuf[TMPBUFLEN];
-	char *p = tmpbuf;
+	char *tmpbuf;
+	char *p;
 	int too_much_data;
 
+	tmpbuf = kzalloc(TMPBUFLEN, GFP_ATOMIC);
+	if (!tmpbuf)
+		return;
+	p = tmpbuf;
+
 	/* Leave if sniff not open */
 	if (!(ch->ch_sniff_flags & SNIFF_OPEN))
-		return;
+		goto exit;
 
 	do_gettimeofday(&tv);
 
@@ -524,7 +516,7 @@ void dgnc_sniff_nowait_nolock(struct channel_t *ch, uchar *text, uchar *buf, int
 			 * function was probably called by the interrupt/timer routines!
 			 */
 			if (n == 0)
-				return;
+				goto exit;
 
 			/*
 			 * Copy as much data as will fit.
@@ -569,6 +561,9 @@ void dgnc_sniff_nowait_nolock(struct channel_t *ch, uchar *text, uchar *buf, int
 		}
 
 	} while (too_much_data);
+
+exit:
+	kfree(tmpbuf);
 }
 
 
@@ -637,7 +632,7 @@ void dgnc_input(struct channel_t *ch)
 	ushort	head;
 	ushort	tail;
 	int	data_len;
-	ulong	lock_flags;
+	unsigned long flags;
 	int flip_len;
 	int len = 0;
 	int n = 0;
@@ -653,7 +648,7 @@ void dgnc_input(struct channel_t *ch)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/*
 	 *      Figure the number of characters in the buffer.
@@ -665,12 +660,10 @@ void dgnc_input(struct channel_t *ch)
 	data_len = (head - tail) & rmask;
 
 	if (data_len == 0) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
-	DPR_READ(("dgnc_input start\n"));
-
 	/*
 	 * If the device is not open, or CREAD is off,
 	 * flush input data and return immediately.
@@ -678,16 +671,12 @@ void dgnc_input(struct channel_t *ch)
 	if (!tp || (tp->magic != TTY_MAGIC) || !(ch->ch_tun.un_flags & UN_ISOPEN) ||
 	    !(tp->termios.c_cflag & CREAD) || (ch->ch_tun.un_flags & UN_CLOSING)) {
 
-		DPR_READ(("input. dropping %d bytes on port %d...\n", data_len, ch->ch_portnum));
-		DPR_READ(("input. tp: %p tp->magic: %x MAGIC:%x ch flags: %x\n",
-			tp, tp ? tp->magic : 0, TTY_MAGIC, ch->ch_tun.un_flags));
-
 		ch->ch_r_head = tail;
 
 		/* Force queue flow control to be released, if needed */
 		dgnc_check_queue_flow_control(ch);
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
@@ -695,14 +684,10 @@ void dgnc_input(struct channel_t *ch)
 	 * If we are throttled, simply don't read any data.
 	 */
 	if (ch->ch_flags & CH_FORCED_STOPI) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		DPR_READ(("Port %d throttled, not reading any data. head: %x tail: %x\n",
-			ch->ch_portnum, head, tail));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
-	DPR_READ(("dgnc_input start 2\n"));
-
 	flip_len = TTY_FLIPBUF_SIZE;
 
 	/* Chop down the length, if needed */
@@ -740,7 +725,7 @@ void dgnc_input(struct channel_t *ch)
 	}
 
 	if (len <= 0) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		if (ld)
 			tty_ldisc_deref(ld);
 		return;
@@ -807,15 +792,13 @@ void dgnc_input(struct channel_t *ch)
 	ch->ch_r_tail = tail & rmask;
 	ch->ch_e_tail = tail & rmask;
 	dgnc_check_queue_flow_control(ch);
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	/* Tell the tty layer its okay to "eat" the data now */
 	tty_flip_buffer_push(tp->port);
 
 	if (ld)
 		tty_ldisc_deref(ld);
-
-	DPR_READ(("dgnc_input - finish\n"));
 }
 
 
@@ -830,8 +813,6 @@ void dgnc_carrier(struct channel_t *ch)
 	int virt_carrier = 0;
 	int phys_carrier = 0;
 
-	DPR_CARR(("dgnc_carrier called...\n"));
-
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
@@ -840,10 +821,8 @@ void dgnc_carrier(struct channel_t *ch)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	if (ch->ch_mistat & UART_MSR_DCD) {
-		DPR_CARR(("mistat: %x  D_CD: %x\n", ch->ch_mistat, ch->ch_mistat & UART_MSR_DCD));
+	if (ch->ch_mistat & UART_MSR_DCD)
 		phys_carrier = 1;
-	}
 
 	if (ch->ch_digi.digi_flags & DIGI_FORCEDCD)
 		virt_carrier = 1;
@@ -851,8 +830,6 @@ void dgnc_carrier(struct channel_t *ch)
 	if (ch->ch_c_cflag & CLOCAL)
 		virt_carrier = 1;
 
-	DPR_CARR(("DCD: physical: %d virt: %d\n", phys_carrier, virt_carrier));
-
 	/*
 	 * Test for a VIRTUAL carrier transition to HIGH.
 	 */
@@ -863,8 +840,6 @@ void dgnc_carrier(struct channel_t *ch)
 		 * for carrier in the open routine.
 		 */
 
-		DPR_CARR(("carrier: virt DCD rose\n"));
-
 		if (waitqueue_active(&(ch->ch_flags_wait)))
 			wake_up_interruptible(&ch->ch_flags_wait);
 	}
@@ -879,8 +854,6 @@ void dgnc_carrier(struct channel_t *ch)
 		 * for carrier in the open routine.
 		 */
 
-		DPR_CARR(("carrier: physical DCD rose\n"));
-
 		if (waitqueue_active(&(ch->ch_flags_wait)))
 			wake_up_interruptible(&ch->ch_flags_wait);
 	}
@@ -895,8 +868,7 @@ void dgnc_carrier(struct channel_t *ch)
 	 *  "make pretend that carrier is there".
 	 */
 	if ((virt_carrier == 0) && ((ch->ch_flags & CH_CD) != 0) &&
-	    (phys_carrier == 0))
-	{
+	    (phys_carrier == 0)) {
 
 		/*
 		 *   When carrier drops:
@@ -913,15 +885,11 @@ void dgnc_carrier(struct channel_t *ch)
 		if (waitqueue_active(&(ch->ch_flags_wait)))
 			wake_up_interruptible(&ch->ch_flags_wait);
 
-		if (ch->ch_tun.un_open_count > 0) {
-			DPR_CARR(("Sending tty hangup\n"));
+		if (ch->ch_tun.un_open_count > 0)
 			tty_hangup(ch->ch_tun.un_tty);
-		}
 
-		if (ch->ch_pun.un_open_count > 0) {
-			DPR_CARR(("Sending pr hangup\n"));
+		if (ch->ch_pun.un_open_count > 0)
 			tty_hangup(ch->ch_pun.un_tty);
-		}
 	}
 
 	/*
@@ -1031,8 +999,6 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 			if (!(ch->ch_flags & CH_RECEIVER_OFF)) {
 				ch->ch_bd->bd_ops->disable_receiver(ch);
 				ch->ch_flags |= (CH_RECEIVER_OFF);
-				DPR_READ(("Internal queue hit hilevel mark (%d)! Turning off interrupts.\n",
-					qleft));
 			}
 		}
 		/* SWFLOW */
@@ -1040,7 +1006,6 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 			if (ch->ch_stops_sent <= MAX_STOPS_SENT) {
 				ch->ch_bd->bd_ops->send_stop_character(ch);
 				ch->ch_stops_sent++;
-				DPR_READ(("Sending stop char!  Times sent: %x\n", ch->ch_stops_sent));
 			}
 		}
 		/* No FLOW */
@@ -1070,15 +1035,12 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 			if (ch->ch_flags & CH_RECEIVER_OFF) {
 				ch->ch_bd->bd_ops->enable_receiver(ch);
 				ch->ch_flags &= ~(CH_RECEIVER_OFF);
-				DPR_READ(("Internal queue hit lowlevel mark (%d)! Turning on interrupts.\n",
-					qleft));
 			}
 		}
 		/* SWFLOW */
 		else if (ch->ch_c_iflag & IXOFF && ch->ch_stops_sent) {
 			ch->ch_stops_sent = 0;
 			ch->ch_bd->bd_ops->send_start_character(ch);
-			DPR_READ(("Sending start char!\n"));
 		}
 		/* No FLOW */
 		else {
@@ -1091,12 +1053,12 @@ void dgnc_check_queue_flow_control(struct channel_t *ch)
 void dgnc_wakeup_writes(struct channel_t *ch)
 {
 	int qlen = 0;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/*
 	 * If channel now has space, wake up anyone waiting on the condition.
@@ -1106,17 +1068,16 @@ void dgnc_wakeup_writes(struct channel_t *ch)
 		qlen += WQUEUESIZE;
 
 	if (qlen >= (WQUEUESIZE - 256)) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
 	if (ch->ch_tun.un_flags & UN_ISOPEN) {
 		if ((ch->ch_tun.un_tty->flags & (1 << TTY_DO_WRITE_WAKEUP)) &&
-			ch->ch_tun.un_tty->ldisc->ops->write_wakeup)
-		{
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			ch->ch_tun.un_tty->ldisc->ops->write_wakeup) {
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			(ch->ch_tun.un_tty->ldisc->ops->write_wakeup)(ch->ch_tun.un_tty);
-			DGNC_LOCK(ch->ch_lock, lock_flags);
+			spin_lock_irqsave(&ch->ch_lock, flags);
 		}
 
 		wake_up_interruptible(&ch->ch_tun.un_tty->write_wait);
@@ -1154,11 +1115,10 @@ void dgnc_wakeup_writes(struct channel_t *ch)
 
 	if (ch->ch_pun.un_flags & UN_ISOPEN) {
 		if ((ch->ch_pun.un_tty->flags & (1 << TTY_DO_WRITE_WAKEUP)) &&
-			ch->ch_pun.un_tty->ldisc->ops->write_wakeup)
-		{
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			ch->ch_pun.un_tty->ldisc->ops->write_wakeup) {
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			(ch->ch_pun.un_tty->ldisc->ops->write_wakeup)(ch->ch_pun.un_tty);
-			DGNC_LOCK(ch->ch_lock, lock_flags);
+			spin_lock_irqsave(&ch->ch_lock, flags);
 		}
 
 		wake_up_interruptible(&ch->ch_pun.un_tty->write_wait);
@@ -1168,15 +1128,14 @@ void dgnc_wakeup_writes(struct channel_t *ch)
 		 * the queue AND FIFO are both empty.
 		 */
 		if (ch->ch_pun.un_flags & UN_EMPTY) {
-			if ((qlen == 0) && (ch->ch_bd->bd_ops->get_uart_bytes_left(ch) == 0)) {
+			if ((qlen == 0) && (ch->ch_bd->bd_ops->get_uart_bytes_left(ch) == 0))
 				ch->ch_pun.un_flags &= ~(UN_EMPTY);
-			}
 		}
 
 		wake_up_interruptible(&ch->ch_pun.un_flags_wait);
 	}
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -1199,7 +1158,7 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	uint		major = 0;
 	uint		minor = 0;
 	int		rc = 0;
-	ulong		lock_flags;
+	unsigned long flags;
 
 	rc = 0;
 
@@ -1224,25 +1183,25 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	if (rc)
 		return rc;
 
-	DGNC_LOCK(brd->bd_lock, lock_flags);
+	spin_lock_irqsave(&brd->bd_lock, flags);
 
 	/* If opened device is greater than our number of ports, bail. */
 	if (PORT_NUM(minor) > brd->nasync) {
-		DGNC_UNLOCK(brd->bd_lock, lock_flags);
+		spin_unlock_irqrestore(&brd->bd_lock, flags);
 		return -ENXIO;
 	}
 
 	ch = brd->channels[PORT_NUM(minor)];
 	if (!ch) {
-		DGNC_UNLOCK(brd->bd_lock, lock_flags);
+		spin_unlock_irqrestore(&brd->bd_lock, flags);
 		return -ENXIO;
 	}
 
 	/* Drop board lock */
-	DGNC_UNLOCK(brd->bd_lock, lock_flags);
+	spin_unlock_irqrestore(&brd->bd_lock, flags);
 
 	/* Grab channel lock */
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* Figure out our type */
 	if (!IS_PRINT(minor)) {
@@ -1252,8 +1211,7 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 		un = &brd->channels[PORT_NUM(minor)]->ch_pun;
 		un->un_type = DGNC_PRINT;
 	} else {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-		DPR_OPEN(("%d Unknown TYPE!\n", __LINE__));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return -ENXIO;
 	}
 
@@ -1262,15 +1220,13 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	 * where we simply cannot safely keep going, wait until the
 	 * state clears.
 	 */
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	rc = wait_event_interruptible(ch->ch_flags_wait, ((ch->ch_flags & CH_OPENING) == 0));
 
 	/* If ret is non-zero, user ctrl-c'ed us */
-	if (rc) {
-		DPR_OPEN(("%d User ctrl c'ed\n", __LINE__));
+	if (rc)
 		return -EINTR;
-	}
 
 	/*
 	 * If either unit is in the middle of the fragile part of close,
@@ -1283,21 +1239,15 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 		(((ch->ch_tun.un_flags | ch->ch_pun.un_flags) & UN_CLOSING) == 0));
 
 	/* If ret is non-zero, user ctrl-c'ed us */
-	if (rc) {
-		DPR_OPEN(("%d User ctrl c'ed\n", __LINE__));
+	if (rc)
 		return -EINTR;
-	}
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 
 	/* Store our unit into driver_data, so we always have it available. */
 	tty->driver_data = un;
 
-	DPR_OPEN(("Open called. MAJOR: %d MINOR:%d PORT_NUM: %x unit: %p NAME: %s\n",
-		MAJOR(tty_devnum(tty)), MINOR(tty_devnum(tty)), PORT_NUM(minor), un, brd->name));
-
-	DPR_OPEN(("%d: tflag=%x  pflag=%x\n", __LINE__, ch->ch_tun.un_flags, ch->ch_pun.un_flags));
 
 	/*
 	 * Initialize tty's
@@ -1317,7 +1267,7 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	ch->ch_flags |= (CH_OPENING);
 
 	/* Drop locks, as malloc with GFP_KERNEL can sleep */
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	if (!ch->ch_rqueue)
 		ch->ch_rqueue = kzalloc(RQUEUESIZE, GFP_KERNEL);
@@ -1326,7 +1276,7 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	if (!ch->ch_wqueue)
 		ch->ch_wqueue = kzalloc(WQUEUESIZE, GFP_KERNEL);
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_flags &= ~(CH_OPENING);
 	wake_up_interruptible(&ch->ch_flags_wait);
@@ -1336,14 +1286,15 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	 */
 	if (!((ch->ch_tun.un_flags | ch->ch_pun.un_flags) & UN_ISOPEN)) {
 
-		DPR_OPEN(("dgnc_open: initializing channel in open...\n"));
-
 		/*
 		 * Flush input queues.
 		 */
-		ch->ch_r_head = ch->ch_r_tail = 0;
-		ch->ch_e_head = ch->ch_e_tail = 0;
-		ch->ch_w_head = ch->ch_w_tail = 0;
+		ch->ch_r_head = 0;
+		ch->ch_r_tail = 0;
+		ch->ch_e_head = 0;
+		ch->ch_e_tail = 0;
+		ch->ch_w_head = 0;
+		ch->ch_w_tail = 0;
 
 		brd->bd_ops->flush_uart_write(ch);
 		brd->bd_ops->flush_uart_read(ch);
@@ -1384,22 +1335,17 @@ static int dgnc_tty_open(struct tty_struct *tty, struct file *file)
 	 * follow protocol for opening port
 	 */
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	rc = dgnc_block_til_ready(tty, file, ch);
 
-	if (rc)
-		DPR_OPEN(("dgnc_tty_open returning after dgnc_block_til_ready "
-			"with %d\n", rc));
-
 	/* No going back now, increment our unit and channel counters */
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 	ch->ch_open_count++;
 	un->un_open_count++;
 	un->un_flags |= (UN_ISOPEN);
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	DPR_OPEN(("dgnc_tty_open finished\n"));
 	return rc;
 }
 
@@ -1413,21 +1359,18 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 {
 	int retval = 0;
 	struct un_t *un = NULL;
-	ulong   lock_flags;
+	unsigned long flags;
 	uint	old_flags = 0;
 	int	sleep_on_un_flags = 0;
 
-	if (!tty || tty->magic != TTY_MAGIC || !file || !ch || ch->magic != DGNC_CHANNEL_MAGIC) {
+	if (!tty || tty->magic != TTY_MAGIC || !file || !ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return -ENXIO;
-	}
 
 	un = tty->driver_data;
 	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return -ENXIO;
 
-	DPR_OPEN(("dgnc_block_til_ready - before block.\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_wopen++;
 
@@ -1474,15 +1417,11 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 				break;
 			}
 
-			if (ch->ch_flags & CH_CD) {
-				DPR_OPEN(("%d: ch_flags: %x\n", __LINE__, ch->ch_flags));
+			if (ch->ch_flags & CH_CD)
 				break;
-			}
 
-			if (ch->ch_flags & CH_FCAR) {
-				DPR_OPEN(("%d: ch_flags: %x\n", __LINE__, ch->ch_flags));
+			if (ch->ch_flags & CH_FCAR)
 				break;
-			}
 		} else {
 			sleep_on_un_flags = 1;
 		}
@@ -1493,13 +1432,10 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 		 * Leave loop with error set.
 		 */
 		if (signal_pending(current)) {
-			DPR_OPEN(("%d: signal pending...\n", __LINE__));
 			retval = -ERESTARTSYS;
 			break;
 		}
 
-		DPR_OPEN(("dgnc_block_til_ready - blocking.\n"));
-
 		/*
 		 * Store the flags before we let go of channel lock
 		 */
@@ -1514,10 +1450,7 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 		 * eventually goes active.
 		 */
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-		DPR_OPEN(("Going to sleep on %s flags...\n",
-			(sleep_on_un_flags ? "un" : "ch")));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		/*
 		 * Wait for something in the flags to change from the current value.
@@ -1529,27 +1462,19 @@ static int dgnc_block_til_ready(struct tty_struct *tty, struct file *file, struc
 			retval = wait_event_interruptible(ch->ch_flags_wait,
 				(old_flags != ch->ch_flags));
 
-		DPR_OPEN(("After sleep... retval: %x\n", retval));
-
 		/*
 		 * We got woken up for some reason.
 		 * Before looping around, grab our channel lock.
 		 */
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 	}
 
 	ch->ch_wopen--;
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	DPR_OPEN(("dgnc_block_til_ready - after blocking.\n"));
-
-	if (retval) {
-		DPR_OPEN(("dgnc_block_til_ready - done. error. retval: %x\n", retval));
+	if (retval)
 		return retval;
-	}
-
-	DPR_OPEN(("dgnc_block_til_ready - done no error. jiffies: %lu\n", jiffies));
 
 	return 0;
 }
@@ -1571,14 +1496,9 @@ static void dgnc_tty_hangup(struct tty_struct *tty)
 	if (!un || un->magic != DGNC_UNIT_MAGIC)
 		return;
 
-	DPR_CLOSE(("dgnc_hangup called. ch->ch_open_count: %d un->un_open_count: %d\n",
-		un->un_ch->ch_open_count, un->un_open_count));
-
 	/* flush the transmit queues */
 	dgnc_tty_flush_buffer(tty);
 
-	DPR_CLOSE(("dgnc_hangup finished. ch->ch_open_count: %d un->un_open_count: %d\n",
-		un->un_ch->ch_open_count, un->un_open_count));
 }
 
 
@@ -1592,7 +1512,7 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong lock_flags;
+	unsigned long flags;
 	int rc = 0;
 
 	if (!tty || tty->magic != TTY_MAGIC)
@@ -1612,9 +1532,7 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 
 	ts = &tty->termios;
 
-	DPR_CLOSE(("Close called\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/*
 	 * Determine if this is the last close or not - and if we agree about
@@ -1640,16 +1558,11 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 	ch->ch_open_count--;
 
 	if (ch->ch_open_count && un->un_open_count) {
-		DPR_CLOSE(("dgnc_tty_close: not last close ch: %d un:%d\n",
-			ch->ch_open_count, un->un_open_count));
-
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return;
 	}
 
 	/* OK, its the last close on the unit */
-	DPR_CLOSE(("dgnc_tty_close - last close on unit procedures\n"));
-
 	un->un_flags |= UN_CLOSING;
 
 	tty->closing = 1;
@@ -1672,22 +1585,16 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 			ch->ch_flags &= ~CH_PRON;
 		}
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		/* wait for output to drain */
 		/* This will also return if we take an interrupt */
 
-		DPR_CLOSE(("Calling wait_for_drain\n"));
 		rc = bd->bd_ops->drain(tty, 0);
 
-		DPR_CLOSE(("After calling wait_for_drain\n"));
-
-		if (rc)
-			DPR_BASIC(("dgnc_tty_close - bad return: %d ", rc));
-
 		dgnc_tty_flush_buffer(tty);
 		tty_ldisc_flush(tty);
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 
 		tty->closing = 0;
 
@@ -1695,7 +1602,6 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 		 * If we have HUPCL set, lower DTR and RTS
 		 */
 		if (ch->ch_c_cflag & HUPCL) {
-			DPR_CLOSE(("Close. HUPCL set, dropping DTR/RTS\n"));
 
 			/* Drop RTS/DTR */
 			ch->ch_mostat &= ~(UART_MCR_DTR | UART_MCR_RTS);
@@ -1706,13 +1612,10 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 			 * have been dropped for modems to see it.
 			 */
 			if (ch->ch_close_delay) {
-				DPR_CLOSE(("Close. Sleeping for RTS/DTR drop\n"));
-
-				DGNC_UNLOCK(ch->ch_lock, lock_flags);
+				spin_unlock_irqrestore(&ch->ch_lock,
+						       flags);
 				dgnc_ms_sleep(ch->ch_close_delay);
-				DGNC_LOCK(ch->ch_lock, lock_flags);
-
-				DPR_CLOSE(("Close. After sleeping for RTS/DTR drop\n"));
+				spin_lock_irqsave(&ch->ch_lock, flags);
 			}
 		}
 
@@ -1734,13 +1637,10 @@ static void dgnc_tty_close(struct tty_struct *tty, struct file *file)
 	un->un_tty = NULL;
 	un->un_flags &= ~(UN_ISOPEN | UN_CLOSING);
 
-	DPR_CLOSE(("Close. Doing wakeups\n"));
 	wake_up_interruptible(&ch->ch_flags_wait);
 	wake_up_interruptible(&un->un_flags_wait);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_BASIC(("dgnc_tty_close - complete\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -1760,7 +1660,7 @@ static int dgnc_tty_chars_in_buffer(struct tty_struct *tty)
 	ushort ttail;
 	uint tmask;
 	uint chars = 0;
-	ulong   lock_flags = 0;
+	unsigned long flags;
 
 	if (tty == NULL)
 		return 0;
@@ -1773,13 +1673,13 @@ static int dgnc_tty_chars_in_buffer(struct tty_struct *tty)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return 0;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	tmask = WQUEUEMASK;
 	thead = ch->ch_w_head & tmask;
 	ttail = ch->ch_w_tail & tmask;
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	if (ttail == thead) {
 		chars = 0;
@@ -1790,9 +1690,6 @@ static int dgnc_tty_chars_in_buffer(struct tty_struct *tty)
 			chars = thead - ttail + WQUEUESIZE;
 	}
 
-	DPR_WRITE(("dgnc_tty_chars_in_buffer. Port: %x - %d (head: %d tail: %d)\n",
-		ch->ch_portnum, chars, thead, ttail));
-
 	return chars;
 }
 
@@ -1866,7 +1763,7 @@ static int dgnc_tty_write_room(struct tty_struct *tty)
 	ushort tail;
 	ushort tmask;
 	int ret = 0;
-	ulong   lock_flags = 0;
+	unsigned long flags;
 
 	if (tty == NULL || dgnc_TmpWriteBuf == NULL)
 		return 0;
@@ -1879,7 +1776,7 @@ static int dgnc_tty_write_room(struct tty_struct *tty)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return 0;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	tmask = WQUEUEMASK;
 	head = (ch->ch_w_head) & tmask;
@@ -1908,9 +1805,7 @@ static int dgnc_tty_write_room(struct tty_struct *tty)
 	if (ret < 0)
 		ret = 0;
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_WRITE(("dgnc_tty_write_room - %d tail: %d head: %d\n", ret, tail, head));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return ret;
 }
@@ -1928,7 +1823,6 @@ static int dgnc_tty_put_char(struct tty_struct *tty, unsigned char c)
 	/*
 	 * Simply call tty_write.
 	 */
-	DPR_WRITE(("dgnc_tty_put_char called\n"));
 	dgnc_tty_write(tty, &c, 1);
 	return 1;
 }
@@ -1947,7 +1841,7 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	struct un_t *un = NULL;
 	int bufcount = 0, n = 0;
 	int orig_count = 0;
-	ulong lock_flags;
+	unsigned long flags;
 	ushort head;
 	ushort tail;
 	ushort tmask;
@@ -1968,9 +1862,6 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	if (!count)
 		return 0;
 
-	DPR_WRITE(("dgnc_tty_write: Port: %x tty=%p user=%d len=%d\n",
-		ch->ch_portnum, tty, from_user, count));
-
 	/*
 	 * Store original amount of characters passed in.
 	 * This helps to figure out if we should ask the FEP
@@ -1978,7 +1869,7 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	 */
 	orig_count = count;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* Get our space available for the channel from the board */
 	tmask = WQUEUEMASK;
@@ -1989,9 +1880,6 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	if (bufcount < 0)
 		bufcount += WQUEUESIZE;
 
-	DPR_WRITE(("%d: bufcount: %x count: %x tail: %x head: %x tmask: %x\n",
-		__LINE__, bufcount, count, tail, head, tmask));
-
 	/*
 	 * Limit printer output to maxcps overall, with bursts allowed
 	 * up to bufsize characters.
@@ -2008,7 +1896,7 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	 * Bail if no space left.
 	 */
 	if (count <= 0) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return 0;
 	}
 
@@ -2038,7 +1926,7 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	 * If there is nothing left to copy, or I can't handle any more data, leave.
 	 */
 	if (count <= 0) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return 0;
 	}
 
@@ -2046,7 +1934,7 @@ static int dgnc_tty_write(struct tty_struct *tty,
 
 		count = min(count, WRITEBUFLEN);
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		/*
 		 * If data is coming from user space, copy it into a temporary
@@ -2061,14 +1949,14 @@ static int dgnc_tty_write(struct tty_struct *tty,
 		 * copy_from_user() returns the number
 		 * of bytes that could *NOT* be copied.
 		 */
-		count -= copy_from_user(dgnc_TmpWriteBuf, (const uchar __user *) buf, count);
+		count -= copy_from_user(dgnc_TmpWriteBuf, (const unsigned char __user *) buf, count);
 
 		if (!count) {
 			up(&dgnc_TmpWriteSem);
 			return -EFAULT;
 		}
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 
 		buf = dgnc_TmpWriteBuf;
 
@@ -2113,14 +2001,12 @@ static int dgnc_tty_write(struct tty_struct *tty,
 	}
 
 	if (from_user) {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		up(&dgnc_TmpWriteSem);
 	} else {
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 	}
 
-	DPR_WRITE(("Write finished - Write %d bytes of %d.\n", count, orig_count));
-
 	if (count) {
 		/*
 		 * Channel lock is grabbed and then released
@@ -2142,8 +2028,8 @@ static int dgnc_tty_tiocmget(struct tty_struct *tty)
 	struct channel_t *ch;
 	struct un_t *un;
 	int result = -EIO;
-	uchar mstat = 0;
-	ulong   lock_flags;
+	unsigned char mstat = 0;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return result;
@@ -2156,13 +2042,11 @@ static int dgnc_tty_tiocmget(struct tty_struct *tty)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return result;
 
-	DPR_IOCTL(("dgnc_tty_tiocmget start\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	mstat = (ch->ch_mostat | ch->ch_mistat);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	result = 0;
 
@@ -2179,8 +2063,6 @@ static int dgnc_tty_tiocmget(struct tty_struct *tty)
 	if (mstat & UART_MSR_DCD)
 		result |= TIOCM_CD;
 
-	DPR_IOCTL(("dgnc_tty_tiocmget finish\n"));
-
 	return result;
 }
 
@@ -2198,7 +2080,7 @@ static int dgnc_tty_tiocmset(struct tty_struct *tty,
 	struct channel_t *ch;
 	struct un_t *un;
 	int ret = -EIO;
-	ulong   lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return ret;
@@ -2215,10 +2097,7 @@ static int dgnc_tty_tiocmset(struct tty_struct *tty,
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return ret;
 
-	DPR_IOCTL(("dgnc_tty_tiocmset start\n"));
-
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	if (set & TIOCM_RTS)
 		ch->ch_mostat |= UART_MCR_RTS;
@@ -2234,9 +2113,7 @@ static int dgnc_tty_tiocmset(struct tty_struct *tty,
 
 	ch->ch_bd->bd_ops->assert_modem_signals(ch);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_tiocmset finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return 0;
 }
@@ -2253,7 +2130,7 @@ static int dgnc_tty_send_break(struct tty_struct *tty, int msec)
 	struct channel_t *ch;
 	struct un_t *un;
 	int ret = -EIO;
-	ulong   lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return ret;
@@ -2281,15 +2158,11 @@ static int dgnc_tty_send_break(struct tty_struct *tty, int msec)
 		break;
 	}
 
-	DPR_IOCTL(("dgnc_tty_send_break start 1.  %lx\n", jiffies));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_bd->bd_ops->send_break(ch, msec);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_send_break finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return 0;
 
@@ -2324,11 +2197,6 @@ static void dgnc_tty_wait_until_sent(struct tty_struct *tty, int timeout)
 		return;
 
 	rc = bd->bd_ops->drain(tty, 0);
-	if (rc) {
-		DPR_IOCTL(("dgnc_tty_ioctl - bad return: %d ", rc));
-		return;
-	}
-	return;
 }
 
 
@@ -2342,7 +2210,7 @@ static void dgnc_tty_send_xchar(struct tty_struct *tty, char c)
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2359,16 +2227,13 @@ static void dgnc_tty_send_xchar(struct tty_struct *tty, char c)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DPR_IOCTL(("dgnc_tty_send_xchar start\n"));
-	printk("dgnc_tty_send_xchar start\n");
+	dev_dbg(tty->dev, "dgnc_tty_send_xchar start\n");
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 	bd->bd_ops->send_immediate_char(ch, c);
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
-	DPR_IOCTL(("dgnc_tty_send_xchar finish\n"));
-	printk("dgnc_tty_send_xchar finish\n");
-	return;
+	dev_dbg(tty->dev, "dgnc_tty_send_xchar finish\n");
 }
 
 
@@ -2381,18 +2246,16 @@ static inline int dgnc_get_mstat(struct channel_t *ch)
 {
 	unsigned char mstat;
 	int result = -EIO;
-	ulong   lock_flags;
-
-	DPR_IOCTL(("dgnc_getmstat start\n"));
+	unsigned long flags;
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return -ENXIO;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	mstat = (ch->ch_mostat | ch->ch_mistat);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	result = 0;
 
@@ -2409,8 +2272,6 @@ static inline int dgnc_get_mstat(struct channel_t *ch)
 	if (mstat & UART_MSR_DCD)
 		result |= TIOCM_CD;
 
-	DPR_IOCTL(("dgnc_getmstat finish\n"));
-
 	return result;
 }
 
@@ -2422,9 +2283,6 @@ static inline int dgnc_get_mstat(struct channel_t *ch)
 static int dgnc_get_modem_info(struct channel_t *ch, unsigned int  __user *value)
 {
 	int result;
-	int rc;
-
-	DPR_IOCTL(("dgnc_get_modem_info start\n"));
 
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return -ENXIO;
@@ -2434,10 +2292,7 @@ static int dgnc_get_modem_info(struct channel_t *ch, unsigned int  __user *value
 	if (result < 0)
 		return -ENXIO;
 
-	rc = put_user(result, value);
-
-	DPR_IOCTL(("dgnc_get_modem_info finish\n"));
-	return rc;
+	return put_user(result, value);
 }
 
 
@@ -2453,7 +2308,7 @@ static int dgnc_set_modem_info(struct tty_struct *tty, unsigned int command, uns
 	struct un_t *un;
 	int ret = -ENXIO;
 	unsigned int arg = 0;
-	ulong   lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return ret;
@@ -2472,8 +2327,6 @@ static int dgnc_set_modem_info(struct tty_struct *tty, unsigned int command, uns
 
 	ret = 0;
 
-	DPR_IOCTL(("dgnc_set_modem_info() start\n"));
-
 	ret = get_user(arg, value);
 	if (ret)
 		return ret;
@@ -2515,13 +2368,11 @@ static int dgnc_set_modem_info(struct tty_struct *tty, unsigned int command, uns
 		return -EINVAL;
 	}
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_bd->bd_ops->assert_modem_signals(ch);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_set_modem_info finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return 0;
 }
@@ -2540,7 +2391,7 @@ static int dgnc_tty_digigeta(struct tty_struct *tty, struct digi_t __user *retin
 	struct channel_t *ch;
 	struct un_t *un;
 	struct digi_t tmp;
-	ulong   lock_flags;
+	unsigned long flags;
 
 	if (!retinfo)
 		return -EFAULT;
@@ -2558,9 +2409,9 @@ static int dgnc_tty_digigeta(struct tty_struct *tty, struct digi_t __user *retin
 
 	memset(&tmp, 0, sizeof(tmp));
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 	memcpy(&tmp, &ch->ch_digi, sizeof(tmp));
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	if (copy_to_user(retinfo, &tmp, sizeof(*retinfo)))
 		return -EFAULT;
@@ -2583,9 +2434,7 @@ static int dgnc_tty_digiseta(struct tty_struct *tty, struct digi_t __user *new_i
 	struct channel_t *ch;
 	struct un_t *un;
 	struct digi_t new_digi;
-	ulong lock_flags;
-
-	DPR_IOCTL(("DIGI_SETA start\n"));
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return -EFAULT;
@@ -2602,12 +2451,10 @@ static int dgnc_tty_digiseta(struct tty_struct *tty, struct digi_t __user *new_i
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return -EFAULT;
 
-	if (copy_from_user(&new_digi, new_info, sizeof(new_digi))) {
-		DPR_IOCTL(("DIGI_SETA failed copy_from_user\n"));
+	if (copy_from_user(&new_digi, new_info, sizeof(new_digi)))
 		return -EFAULT;
-	}
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/*
 	 * Handle transistions to and from RTS Toggle.
@@ -2650,9 +2497,7 @@ static int dgnc_tty_digiseta(struct tty_struct *tty, struct digi_t __user *new_i
 
 	ch->ch_bd->bd_ops->param(tty);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("DIGI_SETA finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 	return 0;
 }
@@ -2666,7 +2511,7 @@ static void dgnc_tty_set_termios(struct tty_struct *tty, struct ktermios *old_te
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	unsigned long lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2683,7 +2528,7 @@ static void dgnc_tty_set_termios(struct tty_struct *tty, struct ktermios *old_te
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_c_cflag   = tty->termios.c_cflag;
 	ch->ch_c_iflag   = tty->termios.c_iflag;
@@ -2695,7 +2540,7 @@ static void dgnc_tty_set_termios(struct tty_struct *tty, struct ktermios *old_te
 	ch->ch_bd->bd_ops->param(tty);
 	dgnc_carrier(ch);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -2703,7 +2548,7 @@ static void dgnc_tty_throttle(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags = 0;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2716,15 +2561,11 @@ static void dgnc_tty_throttle(struct tty_struct *tty)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DPR_IOCTL(("dgnc_tty_throttle start\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_flags |= (CH_FORCED_STOPI);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_throttle finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -2732,7 +2573,7 @@ static void dgnc_tty_unthrottle(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong   lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2745,15 +2586,11 @@ static void dgnc_tty_unthrottle(struct tty_struct *tty)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DPR_IOCTL(("dgnc_tty_unthrottle start\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_flags &= ~(CH_FORCED_STOPI);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_unthrottle finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -2762,7 +2599,7 @@ static void dgnc_tty_start(struct tty_struct *tty)
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2779,15 +2616,11 @@ static void dgnc_tty_start(struct tty_struct *tty)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DPR_IOCTL(("dgcn_tty_start start\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_flags &= ~(CH_FORCED_STOP);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_start finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -2796,7 +2629,7 @@ static void dgnc_tty_stop(struct tty_struct *tty)
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2813,15 +2646,11 @@ static void dgnc_tty_stop(struct tty_struct *tty)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DPR_IOCTL(("dgnc_tty_stop start\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_flags |= (CH_FORCED_STOP);
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_stop finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -2843,7 +2672,7 @@ static void dgnc_tty_flush_chars(struct tty_struct *tty)
 	struct dgnc_board *bd;
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2860,15 +2689,11 @@ static void dgnc_tty_flush_chars(struct tty_struct *tty)
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return;
 
-	DPR_IOCTL(("dgnc_tty_flush_chars start\n"));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	/* Do something maybe here */
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_flush_chars finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -2882,7 +2707,7 @@ static void dgnc_tty_flush_buffer(struct tty_struct *tty)
 {
 	struct channel_t *ch;
 	struct un_t *un;
-	ulong lock_flags;
+	unsigned long flags;
 
 	if (!tty || tty->magic != TTY_MAGIC)
 		return;
@@ -2895,9 +2720,7 @@ static void dgnc_tty_flush_buffer(struct tty_struct *tty)
 	if (!ch || ch->magic != DGNC_CHANNEL_MAGIC)
 		return;
 
-	DPR_IOCTL(("dgnc_tty_flush_buffer on port: %d start\n", ch->ch_portnum));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	ch->ch_flags &= ~CH_STOP;
 
@@ -2916,9 +2739,7 @@ static void dgnc_tty_flush_buffer(struct tty_struct *tty)
 		wake_up_interruptible(&ch->ch_pun.un_flags_wait);
 	}
 
-	DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-	DPR_IOCTL(("dgnc_tty_flush_buffer finish\n"));
+	spin_unlock_irqrestore(&ch->ch_lock, flags);
 }
 
 
@@ -2941,7 +2762,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	struct channel_t *ch;
 	struct un_t *un;
 	int rc;
-	ulong lock_flags;
+	unsigned long flags;
 	void __user *uarg = (void __user *) arg;
 
 	if (!tty || tty->magic != TTY_MAGIC)
@@ -2959,14 +2780,10 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	if (!bd || bd->magic != DGNC_BOARD_MAGIC)
 		return -ENODEV;
 
-	DPR_IOCTL(("dgnc_tty_ioctl start on port %d - cmd %s (%x), arg %lx\n",
-		ch->ch_portnum, dgnc_ioctl_name(cmd), cmd, arg));
-
-	DGNC_LOCK(ch->ch_lock, lock_flags);
+	spin_lock_irqsave(&ch->ch_lock, flags);
 
 	if (un->un_open_count <= 0) {
-		DPR_BASIC(("dgnc_tty_ioctl - unit not open.\n"));
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return -EIO;
 	}
 
@@ -2984,27 +2801,21 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 * in the middle: 0.375 seconds.
 		 */
 		rc = tty_check_change(tty);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		if (rc)
 			return rc;
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
 
-		if (rc) {
-			DPR_IOCTL(("dgnc_tty_ioctl - bad return: %d ", rc));
+		if (rc)
 			return -EINTR;
-		}
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 
-		if (((cmd == TCSBRK) && (!arg)) || (cmd == TCSBRKP)) {
+		if (((cmd == TCSBRK) && (!arg)) || (cmd == TCSBRKP))
 			ch->ch_bd->bd_ops->send_break(ch, 250);
-		}
-
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
 
-		DPR_IOCTL(("dgnc_tty_ioctl finish on port %d - cmd %s (%x), arg %lx\n",
-			ch->ch_portnum, dgnc_ioctl_name(cmd), cmd, arg));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		return 0;
 
@@ -3016,84 +2827,74 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 * in the middle: 0.375 seconds.
 		 */
 		rc = tty_check_change(tty);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		if (rc)
 			return rc;
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
-		if (rc) {
-			DPR_IOCTL(("dgnc_tty_ioctl - bad return: %d ", rc));
+		if (rc)
 			return -EINTR;
-		}
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 
 		ch->ch_bd->bd_ops->send_break(ch, 250);
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-		DPR_IOCTL(("dgnc_tty_ioctl finish on port %d - cmd %s (%x), arg %lx\n",
-			ch->ch_portnum, dgnc_ioctl_name(cmd), cmd, arg));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		return 0;
 
 	case TIOCSBRK:
 		rc = tty_check_change(tty);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		if (rc)
 			return rc;
 
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
-		if (rc) {
-			DPR_IOCTL(("dgnc_tty_ioctl - bad return: %d ", rc));
+		if (rc)
 			return -EINTR;
-		}
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 
 		ch->ch_bd->bd_ops->send_break(ch, 250);
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-		DPR_IOCTL(("dgnc_tty_ioctl finish on port %d - cmd %s (%x), arg %lx\n",
-			ch->ch_portnum, dgnc_ioctl_name(cmd), cmd, arg));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		return 0;
 
 	case TIOCCBRK:
 		/* Do Nothing */
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return 0;
 
 	case TIOCGSOFTCAR:
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		rc = put_user(C_CLOCAL(tty) ? 1 : 0, (unsigned long __user *) arg);
 		return rc;
 
 	case TIOCSSOFTCAR:
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = get_user(arg, (unsigned long __user *) arg);
 		if (rc)
 			return rc;
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 		tty->termios.c_cflag = ((tty->termios.c_cflag & ~CLOCAL) | (arg ? CLOCAL : 0));
 		ch->ch_bd->bd_ops->param(tty);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		return 0;
 
 	case TIOCMGET:
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return dgnc_get_modem_info(ch, uarg);
 
 	case TIOCMBIS:
 	case TIOCMBIC:
 	case TIOCMSET:
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return dgnc_set_modem_info(tty, cmd, uarg);
 
 		/*
@@ -3112,7 +2913,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		 */
 		rc = tty_check_change(tty);
 		if (rc) {
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			return rc;
 		}
 
@@ -3142,7 +2943,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		}
 
 		/* pretend we didn't recognize this IOCTL */
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return -ENOIOCTLCMD;
 	case TCSETSF:
 	case TCSETSW:
@@ -3165,39 +2966,32 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		}
 
 		/* now wait for all the output to drain */
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
-		if (rc) {
-			DPR_IOCTL(("dgnc_tty_ioctl - bad return: %d\n", rc));
+		if (rc)
 			return -EINTR;
-		}
-
-		DPR_IOCTL(("dgnc_tty_ioctl finish on port %d - cmd %s (%x), arg %lx\n",
-			ch->ch_portnum, dgnc_ioctl_name(cmd), cmd, arg));
 
 		/* pretend we didn't recognize this */
 		return -ENOIOCTLCMD;
 
 	case TCSETAW:
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = ch->ch_bd->bd_ops->drain(tty, 0);
-		if (rc) {
-			DPR_IOCTL(("dgnc_tty_ioctl - bad return: %d ", rc));
+		if (rc)
 			return -EINTR;
-		}
 
 		/* pretend we didn't recognize this */
 		return -ENOIOCTLCMD;
 
 	case TCXONC:
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		/* Make the ld do it */
 		return -ENOIOCTLCMD;
 
 	case DIGI_GETA:
 		/* get information for ditty */
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return dgnc_tty_digigeta(tty, uarg);
 
 	case DIGI_SETAW:
@@ -3206,31 +3000,31 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		/* set information for ditty */
 		if (cmd == (DIGI_SETAW)) {
 
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			rc = ch->ch_bd->bd_ops->drain(tty, 0);
-			if (rc) {
-				DPR_IOCTL(("dgnc_tty_ioctl - bad return: %d ", rc));
+
+			if (rc)
 				return -EINTR;
-			}
-			DGNC_LOCK(ch->ch_lock, lock_flags);
+
+			spin_lock_irqsave(&ch->ch_lock, flags);
 		} else {
 			tty_ldisc_flush(tty);
 		}
 		/* fall thru */
 
 	case DIGI_SETA:
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return dgnc_tty_digiseta(tty, uarg);
 
 	case DIGI_LOOPBACK:
 		{
 			uint loopback = 0;
 			/* Let go of locks when accessing user space, could sleep */
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			rc = get_user(loopback, (unsigned int __user *) arg);
 			if (rc)
 				return rc;
-			DGNC_LOCK(ch->ch_lock, lock_flags);
+			spin_lock_irqsave(&ch->ch_lock, flags);
 
 			/* Enable/disable internal loopback for this port */
 			if (loopback)
@@ -3239,12 +3033,12 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 				ch->ch_flags &= ~(CH_LOOPBACK);
 
 			ch->ch_bd->bd_ops->param(tty);
-			DGNC_UNLOCK(ch->ch_lock, lock_flags);
+			spin_unlock_irqrestore(&ch->ch_lock, flags);
 			return 0;
 		}
 
 	case DIGI_GETCUSTOMBAUD:
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = put_user(ch->ch_custom_speed, (unsigned int __user *) arg);
 		return rc;
 
@@ -3252,14 +3046,14 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	{
 		int new_rate;
 		/* Let go of locks when accessing user space, could sleep */
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = get_user(new_rate, (int __user *) arg);
 		if (rc)
 			return rc;
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 		dgnc_set_custom_speed(ch, new_rate);
 		ch->ch_bd->bd_ops->param(tty);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return 0;
 	}
 
@@ -3273,13 +3067,14 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 	case DIGI_REALPORT_SENDIMMEDIATE:
 	{
 		unsigned char c;
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = get_user(c, (unsigned char __user *) arg);
 		if (rc)
 			return rc;
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 		ch->ch_bd->bd_ops->send_immediate_char(ch, c);
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		return 0;
 	}
 
@@ -3301,7 +3096,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		buf.rbytes = ch->ch_rxcount;
 		buf.tbytes = ch->ch_txcount;
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		if (copy_to_user(uarg, &buf, sizeof(buf)))
 			return -EFAULT;
@@ -3325,11 +3120,10 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		if ((ch->ch_flags & CH_STOP) || (ch->ch_flags & CH_FORCED_STOP))
 			events |= (EV_OPU | EV_OPS);
 
-		if ((ch->ch_flags & CH_STOPI) || (ch->ch_flags & CH_FORCED_STOPI)) {
+		if ((ch->ch_flags & CH_STOPI) || (ch->ch_flags & CH_FORCED_STOPI))
 			events |= (EV_IPU | EV_IPS);
-		}
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 		rc = put_user(events, (unsigned int __user *) arg);
 		return rc;
 	}
@@ -3346,7 +3140,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		int tdist;
 		int count;
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		/*
 		 * Get data from user first.
@@ -3354,7 +3148,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		if (copy_from_user(&buf, uarg, sizeof(buf)))
 			return -EFAULT;
 
-		DGNC_LOCK(ch->ch_lock, lock_flags);
+		spin_lock_irqsave(&ch->ch_lock, flags);
 
 		/*
 		 * Figure out how much data is in our RX and TX queues.
@@ -3392,7 +3186,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		else
 			buf.txdone = 1;
 
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		if (copy_to_user(uarg, &buf, sizeof(buf)))
 			return -EFAULT;
@@ -3400,11 +3194,7 @@ static int dgnc_tty_ioctl(struct tty_struct *tty, unsigned int cmd,
 		return 0;
 	}
 	default:
-		DGNC_UNLOCK(ch->ch_lock, lock_flags);
-
-		DPR_IOCTL(("dgnc_tty_ioctl - in default\n"));
-		DPR_IOCTL(("dgnc_tty_ioctl end - cmd %s (%x), arg %lx\n",
-			dgnc_ioctl_name(cmd), cmd, arg));
+		spin_unlock_irqrestore(&ch->ch_lock, flags);
 
 		return -ENOIOCTLCMD;
 	}
diff --git a/drivers/staging/dgnc/dgnc_tty.h b/drivers/staging/dgnc/dgnc_tty.h
index 9d1c2847bd94..58eef257c2ec 100644
--- a/drivers/staging/dgnc/dgnc_tty.h
+++ b/drivers/staging/dgnc/dgnc_tty.h
@@ -37,6 +37,6 @@ void	dgnc_carrier(struct channel_t *ch);
 void	dgnc_wakeup_writes(struct channel_t *ch);
 void	dgnc_check_queue_flow_control(struct channel_t *ch);
 
-void	dgnc_sniff_nowait_nolock(struct channel_t *ch, uchar *text, uchar *buf, int nbuf);
+void	dgnc_sniff_nowait_nolock(struct channel_t *ch, unsigned char *text, unsigned char *buf, int nbuf);
 
 #endif
diff --git a/drivers/staging/dgnc/dgnc_types.h b/drivers/staging/dgnc/dgnc_types.h
index 4fa358535f84..3aafcedbb0d9 100644
--- a/drivers/staging/dgnc/dgnc_types.h
+++ b/drivers/staging/dgnc/dgnc_types.h
@@ -30,7 +30,4 @@
 # define FALSE 0
 #endif
 
-/* Required for our shared headers! */
-typedef unsigned char uchar;
-
 #endif
diff --git a/drivers/staging/dgnc/dgnc_utils.c b/drivers/staging/dgnc/dgnc_utils.c
new file mode 100644
index 000000000000..61efc13ec160
--- /dev/null
+++ b/drivers/staging/dgnc/dgnc_utils.c
@@ -0,0 +1,70 @@
+#include <linux/tty.h>
+#include <linux/sched.h>
+#include "dgnc_utils.h"
+#include "digi.h"
+
+/*
+ * dgnc_ms_sleep()
+ *
+ * Put the driver to sleep for x ms's
+ *
+ * Returns 0 if timed out, !0 (showing signal) if interrupted by a signal.
+ */
+int dgnc_ms_sleep(ulong ms)
+{
+	current->state = TASK_INTERRUPTIBLE;
+	schedule_timeout((ms * HZ) / 1000);
+	return signal_pending(current);
+}
+
+/*
+ *      dgnc_ioctl_name() : Returns a text version of each ioctl value.
+ */
+char *dgnc_ioctl_name(int cmd)
+{
+	switch (cmd) {
+
+	case TCGETA:		return "TCGETA";
+	case TCGETS:		return "TCGETS";
+	case TCSETA:		return "TCSETA";
+	case TCSETS:		return "TCSETS";
+	case TCSETAW:		return "TCSETAW";
+	case TCSETSW:		return "TCSETSW";
+	case TCSETAF:		return "TCSETAF";
+	case TCSETSF:		return "TCSETSF";
+	case TCSBRK:		return "TCSBRK";
+	case TCXONC:		return "TCXONC";
+	case TCFLSH:		return "TCFLSH";
+	case TIOCGSID:		return "TIOCGSID";
+
+	case TIOCGETD:		return "TIOCGETD";
+	case TIOCSETD:		return "TIOCSETD";
+	case TIOCGWINSZ:	return "TIOCGWINSZ";
+	case TIOCSWINSZ:	return "TIOCSWINSZ";
+
+	case TIOCMGET:		return "TIOCMGET";
+	case TIOCMSET:		return "TIOCMSET";
+	case TIOCMBIS:		return "TIOCMBIS";
+	case TIOCMBIC:		return "TIOCMBIC";
+
+	/* from digi.h */
+	case DIGI_SETA:		return "DIGI_SETA";
+	case DIGI_SETAW:	return "DIGI_SETAW";
+	case DIGI_SETAF:	return "DIGI_SETAF";
+	case DIGI_SETFLOW:	return "DIGI_SETFLOW";
+	case DIGI_SETAFLOW:	return "DIGI_SETAFLOW";
+	case DIGI_GETFLOW:	return "DIGI_GETFLOW";
+	case DIGI_GETAFLOW:	return "DIGI_GETAFLOW";
+	case DIGI_GETA:		return "DIGI_GETA";
+	case DIGI_GEDELAY:	return "DIGI_GEDELAY";
+	case DIGI_SEDELAY:	return "DIGI_SEDELAY";
+	case DIGI_GETCUSTOMBAUD: return "DIGI_GETCUSTOMBAUD";
+	case DIGI_SETCUSTOMBAUD: return "DIGI_SETCUSTOMBAUD";
+	case TIOCMODG:		return "TIOCMODG";
+	case TIOCMODS:		return "TIOCMODS";
+	case TIOCSDTR:		return "TIOCSDTR";
+	case TIOCCDTR:		return "TIOCCDTR";
+
+	default:		return "unknown";
+	}
+}
diff --git a/drivers/staging/dgnc/dgnc_utils.h b/drivers/staging/dgnc/dgnc_utils.h
new file mode 100644
index 000000000000..cebf60163a26
--- /dev/null
+++ b/drivers/staging/dgnc/dgnc_utils.h
@@ -0,0 +1,7 @@
+#ifndef __DGNC_UTILS_H
+#define __DGNC_UTILS_H
+
+int dgnc_ms_sleep(ulong ms);
+char *dgnc_ioctl_name(int cmd);
+
+#endif
diff --git a/drivers/staging/dgnc/digi.h b/drivers/staging/dgnc/digi.h
index 252791835044..3181a3590465 100644
--- a/drivers/staging/dgnc/digi.h
+++ b/drivers/staging/dgnc/digi.h
@@ -86,11 +86,11 @@
 						/* Adapter Memory	*/
 
 #define	DIGI_GETFLOW	('e'<<8) | 99		/* Get startc/stopc flow */
-						/* control characters 	 */
+						/* control characters	 */
 #define	DIGI_SETFLOW	('e'<<8) | 100		/* Set startc/stopc flow */
 						/* control characters	 */
 #define	DIGI_GETAFLOW	('e'<<8) | 101		/* Get Aux. startc/stopc */
-						/* flow control chars 	 */
+						/* flow control chars	 */
 #define	DIGI_SETAFLOW	('e'<<8) | 102		/* Set Aux. startc/stopc */
 						/* flow control chars	 */
 
@@ -222,7 +222,7 @@ struct shrink_buf_struct {
 						   xxinit call. */
 
 	unsigned char	shrink_buf_anports;	/* Number of async ports  */
-	unsigned char	shrink_buf_snports; 	/* Number of sync  ports */
+	unsigned char	shrink_buf_snports;	/* Number of sync  ports */
 	unsigned char	shrink_buf_type;	/* Board type 1 = PC/Xi,
 							      2 = PC/Xm,
 							      3 = PC/Xe
@@ -285,9 +285,9 @@ struct digi_stat {
 struct digi_ch {
 	unsigned int	info_bdnum;		/* Board number (0 based)  */
 	unsigned int	info_channel;		/* Channel index number    */
-	unsigned int	info_ch_cflag;		/* Channel cflag   	   */
-	unsigned int	info_ch_iflag;		/* Channel iflag   	   */
-	unsigned int	info_ch_oflag;		/* Channel oflag   	   */
+	unsigned int	info_ch_cflag;		/* Channel cflag	   */
+	unsigned int	info_ch_iflag;		/* Channel iflag	   */
+	unsigned int	info_ch_oflag;		/* Channel oflag	   */
 	unsigned int	info_chsize;		/* Channel structure size  */
 	unsigned int	info_sleep_stat;	/* sleep status		   */
 	dev_t		info_dev;		/* device number	   */
@@ -389,10 +389,10 @@ struct digi_getcounter {
 #define DIGI_SETCUSTOMBAUD	_IOW('e', 106, int)	/* Set integer baud rate */
 #define DIGI_GETCUSTOMBAUD	_IOR('e', 107, int)	/* Get integer baud rate */
 
-#define DIGI_REALPORT_GETBUFFERS ('e'<<8 ) | 108
-#define DIGI_REALPORT_SENDIMMEDIATE ('e'<<8 ) | 109
-#define DIGI_REALPORT_GETCOUNTERS ('e'<<8 ) | 110
-#define DIGI_REALPORT_GETEVENTS ('e'<<8 ) | 111
+#define DIGI_REALPORT_GETBUFFERS ('e'<<8) | 108
+#define DIGI_REALPORT_SENDIMMEDIATE ('e'<<8) | 109
+#define DIGI_REALPORT_GETCOUNTERS ('e'<<8) | 110
+#define DIGI_REALPORT_GETEVENTS ('e'<<8) | 111
 
 #define EV_OPU		0x0001		/* !<Output paused by client */
 #define EV_OPS		0x0002		/* !<Output paused by reqular sw flowctrl */
diff --git a/drivers/staging/dgnc/dpacompat.h b/drivers/staging/dgnc/dpacompat.h
index f96963b9843c..b2d2dc08f869 100644
--- a/drivers/staging/dgnc/dpacompat.h
+++ b/drivers/staging/dgnc/dpacompat.h
@@ -24,7 +24,7 @@
  * This structure holds data needed for the intelligent <--> nonintelligent
  * DPA translation
  */
- struct ni_info {
+struct ni_info {
 	int board;
 	int channel;
 	int dtr;
diff --git a/drivers/staging/emxx_udc/emxx_udc.c b/drivers/staging/emxx_udc/emxx_udc.c
index b2eaf0108e46..1ae0013fb118 100644
--- a/drivers/staging/emxx_udc/emxx_udc.c
+++ b/drivers/staging/emxx_udc/emxx_udc.c
@@ -77,14 +77,14 @@ struct nbu2ss_udc udc_controller;
 /* Read */
 static inline u32 _nbu2ss_readl(void *address)
 {
-	return __raw_readl(address) ;
+	return __raw_readl(address);
 }
 
 /*-------------------------------------------------------------------------*/
 /* Write */
 static inline void _nbu2ss_writel(void *address, u32 udata)
 {
-	__raw_writel(udata, address) ;
+	__raw_writel(udata, address);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -92,7 +92,8 @@ static inline void _nbu2ss_writel(void *address, u32 udata)
 static inline void _nbu2ss_bitset(void *address, u32 udata)
 {
 	u32	reg_dt = __raw_readl(address) | (udata);
-	__raw_writel(reg_dt, address) ;
+
+	__raw_writel(reg_dt, address);
 }
 
 /*-------------------------------------------------------------------------*/
@@ -100,7 +101,8 @@ static inline void _nbu2ss_bitset(void *address, u32 udata)
 static inline void _nbu2ss_bitclr(void *address, u32 udata)
 {
 	u32	reg_dt = __raw_readl(address) & ~(udata);
-	__raw_writel(reg_dt, address) ;
+
+	__raw_writel(reg_dt, address);
 }
 
 #ifdef UDC_DEBUG_DUMP
@@ -119,23 +121,23 @@ static void _nbu2ss_dump_register(struct nbu2ss_udc *udc)
 
 	spin_unlock(&udc->lock);
 
-	printk(KERN_DEBUG "\n-USB REG-\n");
+	dev_dbg(&udc->dev, "\n-USB REG-\n");
 	for (i = 0x0 ; i < USB_BASE_SIZE ; i += 16) {
 		reg_data =   _nbu2ss_readl(
 			(u32 *)IO_ADDRESS(USB_BASE_ADDRESS + i));
-		printk(KERN_DEBUG "USB%04x =%08x", i, (int)reg_data);
+		dev_dbg(&udc->dev, "USB%04x =%08x", i, (int)reg_data);
 
 		reg_data =  _nbu2ss_readl(
 			(u32 *)IO_ADDRESS(USB_BASE_ADDRESS + i + 4));
-		printk(KERN_DEBUG " %08x", (int)reg_data);
+		dev_dbg(&udc->dev, " %08x", (int)reg_data);
 
 		reg_data =  _nbu2ss_readl(
 			(u32 *)IO_ADDRESS(USB_BASE_ADDRESS + i + 8));
-		printk(KERN_DEBUG " %08x", (int)reg_data);
+		dev_dbg(&udc->dev, " %08x", (int)reg_data);
 
 		reg_data =  _nbu2ss_readl(
 			(u32 *)IO_ADDRESS(USB_BASE_ADDRESS + i + 12));
-		printk(KERN_DEBUG " %08x\n", (int)reg_data);
+		dev_dbg(&udc->dev, " %08x\n", (int)reg_data);
 
 	}
 
@@ -471,8 +473,6 @@ static void _nbu2ss_ep_in_end(
 
 		_nbu2ss_bitset(&preg->EP_REGS[num].EP_CONTROL, EPn_AUTO);
 	}
-
-	return;
 }
 
 #ifdef USE_DMA
@@ -705,7 +705,7 @@ static int _nbu2ss_ep0_in_transfer(
 	if (req->req.actual == req->req.length) {
 		if ((req->req.actual % EP0_PACKETSIZE) == 0) {
 			if (req->zero) {
-				req->zero = 0;
+				req->zero = false;
 				EP0_send_NULL(udc, FALSE);
 				return 1;
 			}
@@ -795,7 +795,7 @@ static int _nbu2ss_ep0_out_transfer(
 	if (req->req.actual == req->req.length) {
 		if ((req->req.actual % EP0_PACKETSIZE) == 0) {
 			if (req->zero) {
-				req->zero = 0;
+				req->zero = false;
 				EP0_receive_NULL(udc, FALSE);
 				return 1;
 			}
@@ -971,7 +971,7 @@ static int _nbu2ss_epn_out_data(
 		&& (iBufSize  >= sizeof(u32))) {
 		nret = _nbu2ss_out_dma(udc, req, num, iBufSize);
 	} else {
-		iBufSize = min(iBufSize, (u32)ep->ep.maxpacket);
+		iBufSize = min_t(u32, iBufSize, ep->ep.maxpacket);
 		nret = _nbu2ss_epn_out_pio(udc, ep, req, iBufSize);
 	}
 
@@ -1019,7 +1019,7 @@ static int _nbu2ss_epn_out_transfer(
 	if (result == 0) {
 		if ((req->req.actual % ep->ep.maxpacket) == 0) {
 			if (req->zero) {
-				req->zero = 0;
+				req->zero = false;
 				return 1;
 			}
 		}
@@ -1185,7 +1185,7 @@ static int _nbu2ss_epn_in_data(
 		&& (data_size >= sizeof(u32))) {
 		nret = _nbu2ss_in_dma(udc, ep, req, num, data_size);
 	} else {
-		data_size = min(data_size, (u32)ep->ep.maxpacket);
+		data_size = min_t(u32, data_size, ep->ep.maxpacket);
 		nret = _nbu2ss_epn_in_pio(udc, ep, req, data_size);
 	}
 
@@ -1246,12 +1246,12 @@ static int _nbu2ss_start_transfer(
 	req->div_len = 0;
 
 	if (req->req.length == 0)
-		req->zero = 0;
+		req->zero = false;
 	else {
 		if ((req->req.length % ep->ep.maxpacket) == 0)
 			req->zero = req->req.zero;
 		else
-			req->zero = 0;
+			req->zero = false;
 	}
 
 	if (ep->epnum == 0) {
@@ -1396,8 +1396,6 @@ static void _nbu2ss_set_endpoint_stall(
 			}
 		}
 	}
-
-	return;
 }
 
 
@@ -1406,13 +1404,13 @@ static void _nbu2ss_set_endpoint_stall(
 static struct usb_device_descriptor device_desc = {
 	.bLength              = sizeof(device_desc),
 	.bDescriptorType      = USB_DT_DEVICE,
-	.bcdUSB               = __constant_cpu_to_le16(0x0200),
+	.bcdUSB               = cpu_to_le16(0x0200),
 	.bDeviceClass         = USB_CLASS_VENDOR_SPEC,
 	.bDeviceSubClass      = 0x00,
 	.bDeviceProtocol      = 0x00,
 	.bMaxPacketSize0      = 64,
-	.idVendor             = __constant_cpu_to_le16 (0x0409),
-	.idProduct            = __constant_cpu_to_le16 (0xfff0),
+	.idVendor             = cpu_to_le16(0x0409),
+	.idProduct            = cpu_to_le16(0xfff0),
 	.bcdDevice            = 0xffff,
 	.iManufacturer        = 0x00,
 	.iProduct             = 0x00,
@@ -1493,8 +1491,7 @@ static int _nbu2ss_get_ep_stall(struct nbu2ss_udc *udc, u8 ep_adrs)
 
 	if ((data & bit_data) == 0)
 		return 0;
-	else
-		return 1;
+	return 1;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -1606,7 +1603,7 @@ static int std_req_get_status(struct nbu2ss_udc *udc)
 		return result;
 	}
 
-	length = min(udc->ctrl.wLength, (u16)sizeof(status_data));
+	length = min_t(u16, udc->ctrl.wLength, sizeof(status_data));
 
 	switch (recipient) {
 	case USB_RECIP_DEVICE:
@@ -2011,8 +2008,7 @@ static inline void _nbu2ss_epn_in_int(
 		result = _nbu2ss_epn_in_transfer(udc, ep, req);
 
 	} else {
-		if ((req->zero != 0)
-		&& ((req->req.actual % ep->ep.maxpacket) == 0)) {
+		if (req->zero && ((req->req.actual % ep->ep.maxpacket) == 0)) {
 
 			status =
 			_nbu2ss_readl(&preg->EP_REGS[ep->epnum-1].EP_STATUS);
@@ -2020,7 +2016,7 @@ static inline void _nbu2ss_epn_in_int(
 			if ((status & EPn_IN_FULL) == 0) {
 				/*-----------------------------------------*/
 				/* 0 Length Packet */
-				req->zero = 0;
+				req->zero = false;
 				_nbu2ss_zero_len_pkt(udc, ep->epnum);
 			}
 			return;
@@ -2045,8 +2041,6 @@ static inline void _nbu2ss_epn_out_int(
 	result = _nbu2ss_epn_out_transfer(udc, ep, req);
 	if (result <= 0)
 		_nbu2ss_ep_done(ep, req, result);
-
-	return;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -2084,8 +2078,6 @@ static inline void _nbu2ss_epn_in_dma_int(
 			_nbu2ss_epn_in_int(udc, ep, req);
 		}
 	}
-
-	return;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -2103,8 +2095,7 @@ static inline void _nbu2ss_epn_out_dma_int(
 	num = ep->epnum - 1;
 
 	if (req->req.actual == req->req.length) {
-		if ((req->req.length % ep->ep.maxpacket)
-				&& (req->zero == 0)) {
+		if ((req->req.length % ep->ep.maxpacket) && !req->zero) {
 			req->div_len = 0;
 			req->dma_flag = FALSE;
 			_nbu2ss_ep_done(ep, req, 0);
@@ -2171,7 +2162,7 @@ static inline void _nbu2ss_epn_int(struct nbu2ss_udc *udc, u32 epnum)
 		req = list_entry(ep->queue.next, struct nbu2ss_req, queue);
 
 	if (req == NULL) {
-		/* pr_warning("=== %s(%d) req == NULL\n", __func__, epnum); */
+		/* pr_warn("=== %s(%d) req == NULL\n", __func__, epnum); */
 		return;
 	}
 
@@ -2206,8 +2197,6 @@ static void _nbu2ss_ep0_enable(struct nbu2ss_udc *udc)
 {
 	_nbu2ss_bitset(&udc->p_regs->EP0_CONTROL, (EP0_AUTO | EP0_BCLR));
 	_nbu2ss_writel(&udc->p_regs->EP0_INT_ENA, EP0_INT_EN_BIT);
-
-	return;
 }
 
 #if 0
@@ -2220,8 +2209,6 @@ static void _nbu2ss_ep0_disable(struct nbu2ss_udc *udc)
 			, (EP0_BCLR | EP0_INAK | EP0_ONAK | EP0_BCLR));
 
 	_nbu2ss_bitclr(&udc->p_regs->EP0_CONTROL, EP0_AUTO);
-
-	return;
 }
 #endif
 
@@ -2479,8 +2466,6 @@ static inline void _nbu2ss_check_vbus(struct nbu2ss_udc *udc)
 				_nbu2ss_pullup(udc, 1);
 		}
 	}
-
-	return;
 }
 
 /*-------------------------------------------------------------------------*/
@@ -3111,7 +3096,7 @@ static int nbu2ss_gad_wakeup(struct usb_gadget *pgadget)
 
 	data = gpio_get_value(VBUS_VALUE);
 	if (data == 0) {
-		pr_warning("VBUS LEVEL = %d\n", data);
+		pr_warn("VBUS LEVEL = %d\n", data);
 		return -EINVAL;
 	}
 
@@ -3197,7 +3182,7 @@ static int nbu2ss_gad_pullup(struct usb_gadget *pgadget, int is_on)
 	udc = container_of(pgadget, struct nbu2ss_udc, gadget);
 
 	if (udc->driver == NULL) {
-		pr_warning("%s, Not Regist Driver\n", __func__);
+		pr_warn("%s, Not Regist Driver\n", __func__);
 		return -EINVAL;
 	}
 
@@ -3232,22 +3217,22 @@ static const struct usb_gadget_ops nbu2ss_gadget_ops = {
 	.ioctl			= nbu2ss_gad_ioctl,
 };
 
-static char g_ep0_name[] = "ep0";
-static char g_ep1_name[] = "ep1-bulk";
-static char g_ep2_name[] = "ep2-bulk";
-static char g_ep3_name[] = "ep3in-int";
-static char g_ep4_name[] = "ep4-iso";
-static char g_ep5_name[] = "ep5-iso";
-static char g_ep6_name[] = "ep6-bulk";
-static char g_ep7_name[] = "ep7-bulk";
-static char g_ep8_name[] = "ep8in-int";
-static char g_ep9_name[] = "ep9-iso";
-static char g_epa_name[] = "epa-iso";
-static char g_epb_name[] = "epb-bulk";
-static char g_epc_name[] = "epc-nulk";
-static char g_epd_name[] = "epdin-int";
-
-static char *gp_ep_name[NUM_ENDPOINTS] = {
+static const char g_ep0_name[] = "ep0";
+static const char g_ep1_name[] = "ep1-bulk";
+static const char g_ep2_name[] = "ep2-bulk";
+static const char g_ep3_name[] = "ep3in-int";
+static const char g_ep4_name[] = "ep4-iso";
+static const char g_ep5_name[] = "ep5-iso";
+static const char g_ep6_name[] = "ep6-bulk";
+static const char g_ep7_name[] = "ep7-bulk";
+static const char g_ep8_name[] = "ep8in-int";
+static const char g_ep9_name[] = "ep9-iso";
+static const char g_epa_name[] = "epa-iso";
+static const char g_epb_name[] = "epb-bulk";
+static const char g_epc_name[] = "epc-nulk";
+static const char g_epd_name[] = "epdin-int";
+
+static const char *gp_ep_name[NUM_ENDPOINTS] = {
 	g_ep0_name,
 	g_ep1_name,
 	g_ep2_name,
@@ -3268,7 +3253,7 @@ static char *gp_ep_name[NUM_ENDPOINTS] = {
 static void __init nbu2ss_drv_set_ep_info(
 	struct nbu2ss_udc	*udc,
 	struct nbu2ss_ep	*ep,
-	u8 *name)
+	const char *name)
 {
 	ep->udc = udc;
 	ep->desc = NULL;
diff --git a/drivers/staging/emxx_udc/emxx_udc.h b/drivers/staging/emxx_udc/emxx_udc.h
index 578fdcfed5e9..ee1b80d705fa 100644
--- a/drivers/staging/emxx_udc/emxx_udc.h
+++ b/drivers/staging/emxx_udc/emxx_udc.h
@@ -437,10 +437,6 @@
 #define PLL_RESUME			BIT05		/* RW */
 #define PLL_LOCK			BIT04		/* R */
 
-#ifdef CONFIG_MACH_EMGR
-#define PLL_RST				BIT02		/* RW */
-#endif
-
 #define EPC_RST				BIT00		/* RW */
 
 /*------- (0x1014) USBF_EPTEST Register */
diff --git a/drivers/staging/et131x/Kconfig b/drivers/staging/et131x/Kconfig
deleted file mode 100644
index 8190f2aaf53b..000000000000
--- a/drivers/staging/et131x/Kconfig
+++ /dev/null
@@ -1,10 +0,0 @@
-config ET131X
-	tristate "Agere ET-1310 Gigabit Ethernet support"
-	depends on PCI && NET && NETDEVICES
-	select PHYLIB
-	default n
-	---help---
-	  This driver supports Agere ET-1310 ethernet adapters.
-
-	  To compile this driver as a module, choose M here. The module
-	  will be called et131x.
diff --git a/drivers/staging/et131x/README b/drivers/staging/et131x/README
deleted file mode 100644
index 3befc45fab8a..000000000000
--- a/drivers/staging/et131x/README
+++ /dev/null
@@ -1,20 +0,0 @@
-This is a driver for the ET1310 network device.
-
-Based on the driver found at https://sourceforge.net/projects/et131x/
-
-Cleaned up immensely by Olaf Hartman and Christoph Hellwig <hch@infradead.org>
-
-Note, the powermanagement options were removed from the vendor provided
-driver as they did not build properly at the time.
-
-TODO:
-	- Look at reducing the number of spinlocks
-	- Simplify code in nic_rx_pkts(), when determining multicast_pkts_rcvd
-	- Implement NAPI support
-	- In et131x_tx(), don't return NETDEV_TX_BUSY, just drop the packet with kfree_skb().
-	- Reduce the number of split lines by careful consideration of variable names etc.
-
-Please send patches to:
-	Greg Kroah-Hartman <gregkh@linuxfoundation.org>
-	Mark Einon <mark.einon@gmail.com>
-
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
index afaab07862fb..c1856f7d1e26 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_dnld.c
@@ -30,19 +30,13 @@
 #include <linux/netdevice.h>
 #include <linux/timer.h>
 #include <linux/delay.h>
-#include <asm/io.h>
-#include <asm/uaccess.h>
+#include <linux/io.h>
+#include <linux/uaccess.h>
 #include <linux/vmalloc.h>
 
 #include "ft1000.h"
 #include "boot.h"
 
-#ifdef FT_DEBUG
-#define DEBUG(n, args...) printk(KERN_DEBUG args);
-#else
-#define DEBUG(n, args...)
-#endif
-
 #define  MAX_DSP_WAIT_LOOPS      100
 #define  DSP_WAIT_SLEEP_TIME     1	/* 1 millisecond */
 
@@ -127,7 +121,7 @@ void card_bootload(struct net_device *dev)
 	u32 i;
 	u32 templong;
 
-	DEBUG(0, "card_bootload is called\n");
+	netdev_dbg(dev, "card_bootload is called\n");
 
 	pdata = (u32 *) bootimage;
 	size = sizeof(bootimage);
@@ -174,10 +168,9 @@ u16 get_handshake(struct net_device *dev, u16 expected_value)
 		if ((handshake == expected_value)
 			|| (handshake == HANDSHAKE_RESET_VALUE)) {
 			return handshake;
-		} else {
-			loopcnt++;
-			mdelay(DSP_WAIT_SLEEP_TIME);
 		}
+		loopcnt++;
+		mdelay(DSP_WAIT_SLEEP_TIME);
 
 	}
 
@@ -436,7 +429,7 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 				request = get_request_type(dev);
 				switch (request) {
 				case REQUEST_FILE_CHECKSUM:
-					DEBUG(0,
+					netdev_dbg(dev,
 						  "ft1000_dnld: REQUEST_FOR_CHECKSUM\n");
 					put_request_value(dev, image_chksum);
 					break;
@@ -651,7 +644,7 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 								(u32)
 								pDspImageInfoV6->
 								checksum;
-							DEBUG(0,
+							netdev_dbg(dev,
 								  "ft1000_dnld: image_chksum = 0x%8x\n",
 								  (unsigned
 								   int)
@@ -694,11 +687,11 @@ int card_download(struct net_device *dev, const u8 *pFileStart,
 				|| pHdr->portsrc == 0x10 /* FMM */)) {
 				uiState = STATE_SECTION_PROV;
 			} else {
-				DEBUG(1,
+				netdev_dbg(dev,
 					  "FT1000:download:Download error: Bad Port IDs in Pseudo Record\n");
-				DEBUG(1, "\t Port Source = 0x%2.2x\n",
+				netdev_dbg(dev, "\t Port Source = 0x%2.2x\n",
 					  pHdr->portsrc);
-				DEBUG(1, "\t Port Destination = 0x%2.2x\n",
+				netdev_dbg(dev, "\t Port Destination = 0x%2.2x\n",
 					  pHdr->portdest);
 				Status = FAILURE;
 			}
diff --git a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
index 21f09fe168e9..44575c78cf0c 100644
--- a/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-pcmcia/ft1000_hw.c
@@ -62,8 +62,8 @@ static struct timer_list poll_timer = {
 
 static u16 cmdbuffer[1024];
 static u8 tempbuffer[1600];
-static u8 ft1000_card_present = 0;
-static u8 flarion_ft1000_cnt = 0;
+static u8 ft1000_card_present;
+static u8 flarion_ft1000_cnt;
 
 static irqreturn_t ft1000_interrupt(int irq, void *dev_id);
 static void ft1000_enable_interrupts(struct net_device *dev);
@@ -125,7 +125,7 @@ u16 ft1000_read_dpram(struct net_device *dev, int offset)
 	data = ft1000_read_reg(dev, FT1000_REG_DPRAM_DATA);
 	spin_unlock_irqrestore(&info->dpram_lock, flags);
 
-	return (data);
+	return data;
 }
 
 /*---------------------------------------------------------------------------
@@ -183,7 +183,7 @@ u16 ft1000_read_dpram_mag_16(struct net_device *dev, int offset, int Index)
 	}
 	spin_unlock_irqrestore(&info->dpram_lock, flags);
 
-	return (data);
+	return data;
 }
 
 /*---------------------------------------------------------------------------
@@ -240,7 +240,7 @@ u32 ft1000_read_dpram_mag_32(struct net_device *dev, int offset)
 	data = inl(dev->base_addr + FT1000_REG_MAG_DPDATAL);
 	spin_unlock_irqrestore(&info->dpram_lock, flags);
 
-	return (data);
+	return data;
 }
 
 /*---------------------------------------------------------------------------
@@ -1077,7 +1077,7 @@ static void ft1000_proc_drvmsg(struct net_device *dev)
 			}
             }
             else {
-                DEBUG(1,"Media is down\n");
+                DEBUG(1, "Media is down\n");
                 if (info->mediastate == 1) {
                     info->mediastate = 0;
                     netif_carrier_off(dev);
@@ -1922,7 +1922,8 @@ static int ft1000_copy_down_pkt(struct net_device *dev, u16 * packet, u16 len)
 static struct net_device_stats *ft1000_stats(struct net_device *dev)
 {
 	struct ft1000_info *info = netdev_priv(dev);
-	return (&info->stats);
+
+	return &info->stats;
 }
 
 static int ft1000_open(struct net_device *dev)
@@ -2062,7 +2063,7 @@ static irqreturn_t ft1000_interrupt(int irq, void *dev_id)
 
 		/* Read interrupt type */
 		inttype = ft1000_read_reg (dev, FT1000_REG_SUP_ISR);
-		DEBUG(1,"ft1000_hw: interrupt status register after clear = 0x%x\n",inttype);
+		DEBUG(1, "ft1000_hw: interrupt status register after clear = 0x%x\n", inttype);
 	}
 	ft1000_enable_interrupts(dev);
 	return IRQ_HANDLED;
@@ -2097,7 +2098,7 @@ void stop_ft1000_card(struct net_device *dev)
 	}
 
 	free_irq(dev->irq, dev);
-	release_region(dev->base_addr,256);
+	release_region(dev->base_addr, 256);
 	release_firmware(fw_entry);
 	flarion_ft1000_cnt--;
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
index 9f4c7858a059..0f347ab0d300 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_debug.c
@@ -390,7 +390,7 @@ static unsigned int ft1000_poll_dev(struct file *file, poll_table *wait)
     poll_wait(file, &dev->app_info[i].wait_dpram_msg, wait);
     /* DEBUG("FT1000:ft1000_poll_dev:Polling for data from DSP\n"); */
 
-    return (0);
+	return 0;
 }
 
 /*
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
index 65f980167130..37707da09e9c 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_download.c
@@ -129,10 +129,9 @@ static int check_usb_db(struct ft1000_usb *ft1000dev)
 			status = ft1000_write_register(ft1000dev,  0x8000,
 						FT1000_REG_DOORBELL);
 			break;
-		} else {
-			loopcnt++;
-			msleep(10);
 		}
+		loopcnt++;
+		msleep(10);
 
 	}
 
@@ -190,10 +189,9 @@ static u16 get_handshake(struct ft1000_usb *ft1000dev, u16 expected_value)
 		if ((handshake == expected_value) ||
 		    (handshake == HANDSHAKE_RESET_VALUE_USB)) {
 			return handshake;
-		} else	{
-			loopcnt++;
-			msleep(10);
 		}
+		loopcnt++;
+		msleep(10);
 	}
 
 	return HANDSHAKE_TIMEOUT_VALUE;
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
index 7012e09a1599..2e13e7b7ec10 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_hw.c
@@ -566,10 +566,9 @@ static int ft1000_copy_down_pkt(struct net_device *netdev, u8 *packet, u16 len)
 	if (ret) {
 		DEBUG("ft1000 failed tx_urb %d\n", ret);
 		return ret;
-	} else {
-		pInfo->stats.tx_packets++;
-		pInfo->stats.tx_bytes += (len + 14);
 	}
+	pInfo->stats.tx_packets++;
+	pInfo->stats.tx_bytes += (len + 14);
 
 	return 0;
 }
@@ -998,53 +997,52 @@ static bool ft1000_receive_cmd(struct ft1000_usb *dev, u16 *pbuffer,
 		DEBUG("FT1000:ft1000_receive_cmd:Invalid command length = %d\n",
 		      size);
 		return FALSE;
-	} else {
-		ppseudohdr = (u16 *) pbuffer;
-		ft1000_write_register(dev, FT1000_DPRAM_MAG_RX_BASE,
-				      FT1000_REG_DPRAM_ADDR);
-		ret =
-		    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
-		pbuffer++;
-		ft1000_write_register(dev, FT1000_DPRAM_MAG_RX_BASE + 1,
-				      FT1000_REG_DPRAM_ADDR);
-		for (i = 0; i <= (size >> 2); i++) {
-			ret =
-			    ft1000_read_register(dev, pbuffer,
-						 FT1000_REG_MAG_DPDATAL);
-			pbuffer++;
-			ret =
-			    ft1000_read_register(dev, pbuffer,
-						 FT1000_REG_MAG_DPDATAH);
-			pbuffer++;
-		}
-		/* copy odd aligned word */
+	}
+	ppseudohdr = (u16 *) pbuffer;
+	ft1000_write_register(dev, FT1000_DPRAM_MAG_RX_BASE,
+			      FT1000_REG_DPRAM_ADDR);
+	ret =
+	    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
+	pbuffer++;
+	ft1000_write_register(dev, FT1000_DPRAM_MAG_RX_BASE + 1,
+			      FT1000_REG_DPRAM_ADDR);
+	for (i = 0; i <= (size >> 2); i++) {
 		ret =
-		    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAL);
-
+		    ft1000_read_register(dev, pbuffer,
+					 FT1000_REG_MAG_DPDATAL);
 		pbuffer++;
 		ret =
-		    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
-
+		    ft1000_read_register(dev, pbuffer,
+					 FT1000_REG_MAG_DPDATAH);
 		pbuffer++;
-		if (size & 0x0001) {
-			/* copy odd byte from fifo */
-			ret =
-			    ft1000_read_register(dev, &tempword,
-						 FT1000_REG_DPRAM_DATA);
-			*pbuffer = ntohs(tempword);
-		}
-		/* Check if pseudo header checksum is good
-		 * Calculate pseudo header checksum
-		 */
-		tempword = *ppseudohdr++;
-		for (i = 1; i < 7; i++)
-			tempword ^= *ppseudohdr++;
+	}
+	/* copy odd aligned word */
+	ret =
+	    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAL);
 
-		if ((tempword != *ppseudohdr))
-			return FALSE;
+	pbuffer++;
+	ret =
+	    ft1000_read_register(dev, pbuffer, FT1000_REG_MAG_DPDATAH);
 
-		return TRUE;
+	pbuffer++;
+	if (size & 0x0001) {
+		/* copy odd byte from fifo */
+		ret =
+		    ft1000_read_register(dev, &tempword,
+					 FT1000_REG_DPRAM_DATA);
+		*pbuffer = ntohs(tempword);
 	}
+	/* Check if pseudo header checksum is good
+	 * Calculate pseudo header checksum
+	 */
+	tempword = *ppseudohdr++;
+	for (i = 1; i < 7; i++)
+		tempword ^= *ppseudohdr++;
+
+	if (tempword != *ppseudohdr)
+		return FALSE;
+
+	return TRUE;
 }
 
 static int ft1000_dsp_prov(void *arg)
@@ -1153,6 +1151,7 @@ static int ft1000_proc_drvmsg(struct ft1000_usb *dev, u16 size)
 	} convert;
 
 	char *cmdbuffer = kmalloc(1600, GFP_KERNEL);
+
 	if (!cmdbuffer)
 		return -ENOMEM;
 
diff --git a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
index cc740c96845d..39be30c0eedf 100644
--- a/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
+++ b/drivers/staging/ft1000/ft1000-usb/ft1000_usb.c
@@ -41,7 +41,7 @@ static int ft1000_poll_thread(void *arg)
 	int ret;
 
 	while (!kthread_should_stop()) {
-		msleep(10);
+		usleep_range(10000, 11000);
 		if (!gPollingfailed) {
 			ret = ft1000_poll(arg);
 			if (ret != 0) {
@@ -246,8 +246,6 @@ static void ft1000_disconnect(struct usb_interface *interface)
 		kfree(ft1000dev);
 	}
 	kfree(pFileStart);
-
-	return;
 }
 
 static struct usb_driver ft1000_usb_driver = {
diff --git a/drivers/staging/gdm724x/gdm_mux.c b/drivers/staging/gdm724x/gdm_mux.c
index 10ce2c1805bb..b5b063a738f8 100644
--- a/drivers/staging/gdm724x/gdm_mux.c
+++ b/drivers/staging/gdm724x/gdm_mux.c
@@ -517,6 +517,7 @@ static int gdm_mux_probe(struct usb_interface *intf,
 	int ret;
 	int i;
 	struct usb_device *usbdev = interface_to_usbdev(intf);
+
 	bInterfaceNumber = intf->cur_altsetting->desc.bInterfaceNumber;
 
 	idVendor = __le16_to_cpu(usbdev->descriptor.idVendor);
diff --git a/drivers/staging/gdm724x/gdm_usb.c b/drivers/staging/gdm724x/gdm_usb.c
index 483185bb4ecf..d2a3b350ba6d 100644
--- a/drivers/staging/gdm724x/gdm_usb.c
+++ b/drivers/staging/gdm724x/gdm_usb.c
@@ -664,9 +664,8 @@ static void do_tx(struct work_struct *work)
 	if (!udev->send_complete) {
 		spin_unlock_irqrestore(&tx->lock, flags);
 		return;
-	} else {
-		udev->send_complete = 0;
 	}
+	udev->send_complete = 0;
 
 	if (!list_empty(&tx->hci_list)) {
 		t = list_entry(tx->hci_list.next, struct usb_tx, list);
@@ -682,6 +681,10 @@ static void do_tx(struct work_struct *work)
 		}
 
 		t = alloc_tx_struct(TX_BUF_SIZE);
+		if (t == NULL) {
+			spin_unlock_irqrestore(&tx->lock, flags);
+			return;
+		}
 		t->callback = NULL;
 		t->tx = tx;
 		t->is_sdu = 1;
diff --git a/drivers/staging/gdm72xx/netlink_k.c b/drivers/staging/gdm72xx/netlink_k.c
index 9bf00e683f13..9d78bfcdb2c3 100644
--- a/drivers/staging/gdm72xx/netlink_k.c
+++ b/drivers/staging/gdm72xx/netlink_k.c
@@ -145,14 +145,12 @@ int netlink_send(struct sock *sock, int group, u16 type, void *msg, int len)
 
 	ret = netlink_broadcast(sock, skb, 0, group+1, GFP_ATOMIC);
 
-	if (!ret) {
+	if (!ret)
 		return len;
-	} else {
-		if (ret != -ESRCH) {
-			pr_err("netlink_broadcast g=%d, t=%d, l=%d, r=%d\n",
-			       group, type, len, ret);
-		}
-		ret = 0;
+	if (ret != -ESRCH) {
+		pr_err("netlink_broadcast g=%d, t=%d, l=%d, r=%d\n",
+		       group, type, len, ret);
 	}
+	ret = 0;
 	return ret;
 }
diff --git a/drivers/staging/goldfish/goldfish_audio.c b/drivers/staging/goldfish/goldfish_audio.c
index c89d0b87a446..f200359c4443 100644
--- a/drivers/staging/goldfish/goldfish_audio.c
+++ b/drivers/staging/goldfish/goldfish_audio.c
@@ -36,6 +36,7 @@ MODULE_VERSION("1.0");
 struct goldfish_audio {
 	char __iomem *reg_base;
 	int irq;
+	/* lock protects access to buffer_status and to device registers */
 	spinlock_t lock;
 	wait_queue_head_t wait;
 
@@ -108,12 +109,10 @@ enum {
 					  AUDIO_INT_READ_BUFFER_FULL,
 };
 
-
 static atomic_t open_count = ATOMIC_INIT(0);
 
-
 static ssize_t goldfish_audio_read(struct file *fp, char __user *buf,
-						size_t count, loff_t *pos)
+				   size_t count, loff_t *pos)
 {
 	struct goldfish_audio *data = fp->private_data;
 	int length;
@@ -126,11 +125,10 @@ static ssize_t goldfish_audio_read(struct file *fp, char __user *buf,
 		length = (count > READ_BUFFER_SIZE ? READ_BUFFER_SIZE : count);
 		AUDIO_WRITE(data, AUDIO_START_READ, length);
 
-		wait_event_interruptible(data->wait,
-			(data->buffer_status & AUDIO_INT_READ_BUFFER_FULL));
+		wait_event_interruptible(data->wait, (data->buffer_status &
+					 AUDIO_INT_READ_BUFFER_FULL));
 
-		length = AUDIO_READ(data,
-						AUDIO_READ_BUFFER_AVAILABLE);
+		length = AUDIO_READ(data, AUDIO_READ_BUFFER_AVAILABLE);
 
 		/* copy data to user space */
 		if (copy_to_user(buf, data->read_buffer, length))
@@ -144,7 +142,7 @@ static ssize_t goldfish_audio_read(struct file *fp, char __user *buf,
 }
 
 static ssize_t goldfish_audio_write(struct file *fp, const char __user *buf,
-						 size_t count, loff_t *pos)
+				    size_t count, loff_t *pos)
 {
 	struct goldfish_audio *data = fp->private_data;
 	unsigned long irq_flags;
@@ -218,7 +216,7 @@ static int goldfish_audio_release(struct inode *ip, struct file *fp)
 }
 
 static long goldfish_audio_ioctl(struct file *fp, unsigned int cmd,
-							unsigned long arg)
+				 unsigned long arg)
 {
 	/* temporary workaround, until we switch to the ALSA API */
 	if (cmd == 315)
@@ -307,7 +305,7 @@ static int goldfish_audio_probe(struct platform_device *pdev)
 	data->read_buffer = data->buffer_virt + 2 * WRITE_BUFFER_SIZE;
 
 	ret = devm_request_irq(&pdev->dev, data->irq, goldfish_audio_interrupt,
-					IRQF_SHARED, pdev->name, data);
+			       IRQF_SHARED, pdev->name, data);
 	if (ret) {
 		dev_err(&pdev->dev, "request_irq failed\n");
 		return ret;
@@ -322,18 +320,18 @@ static int goldfish_audio_probe(struct platform_device *pdev)
 	}
 
 	AUDIO_WRITE64(data, AUDIO_SET_WRITE_BUFFER_1,
-				AUDIO_SET_WRITE_BUFFER_1_HIGH, buf_addr);
+		      AUDIO_SET_WRITE_BUFFER_1_HIGH, buf_addr);
 	buf_addr += WRITE_BUFFER_SIZE;
 
 	AUDIO_WRITE64(data, AUDIO_SET_WRITE_BUFFER_2,
-				AUDIO_SET_WRITE_BUFFER_2_HIGH, buf_addr);
+		      AUDIO_SET_WRITE_BUFFER_2_HIGH, buf_addr);
 
 	buf_addr += WRITE_BUFFER_SIZE;
 
 	data->read_supported = AUDIO_READ(data, AUDIO_READ_SUPPORTED);
 	if (data->read_supported)
 		AUDIO_WRITE64(data, AUDIO_SET_READ_BUFFER,
-				AUDIO_SET_READ_BUFFER_HIGH, buf_addr);
+			      AUDIO_SET_READ_BUFFER_HIGH, buf_addr);
 
 	audio_data = data;
 	return 0;
diff --git a/drivers/staging/goldfish/goldfish_nand.c b/drivers/staging/goldfish/goldfish_nand.c
index 092604c698b6..d68f216a7e77 100644
--- a/drivers/staging/goldfish/goldfish_nand.c
+++ b/drivers/staging/goldfish/goldfish_nand.c
@@ -31,6 +31,7 @@
 #include "goldfish_nand_reg.h"
 
 struct goldfish_nand {
+	/* lock protects access to the device registers */
 	struct mutex            lock;
 	unsigned char __iomem  *base;
 	struct cmd_params       *cmd_params;
@@ -39,8 +40,8 @@ struct goldfish_nand {
 };
 
 static u32 goldfish_nand_cmd_with_params(struct mtd_info *mtd,
-			enum nand_cmd cmd, u64 addr, u32 len,
-			void *ptr, u32 *rv)
+					 enum nand_cmd cmd, u64 addr, u32 len,
+					 void *ptr, u32 *rv)
 {
 	u32 cmdp;
 	struct goldfish_nand *nand = mtd->priv;
@@ -74,7 +75,7 @@ static u32 goldfish_nand_cmd_with_params(struct mtd_info *mtd,
 }
 
 static u32 goldfish_nand_cmd(struct mtd_info *mtd, enum nand_cmd cmd,
-				u64 addr, u32 len, void *ptr)
+			     u64 addr, u32 len, void *ptr)
 {
 	struct goldfish_nand *nand = mtd->priv;
 	u32 rv;
@@ -113,7 +114,7 @@ static int goldfish_nand_erase(struct mtd_info *mtd, struct erase_info *instr)
 
 	if (goldfish_nand_cmd(mtd, NAND_CMD_ERASE, ofs, len, NULL) != len) {
 		pr_err("goldfish_nand_erase: erase failed, start %llx, len %x, dev_size %llx, erase_size %x\n",
-			ofs, len, mtd->size, mtd->erasesize);
+		       ofs, len, mtd->size, mtd->erasesize);
 		return -EIO;
 	}
 
@@ -124,12 +125,12 @@ static int goldfish_nand_erase(struct mtd_info *mtd, struct erase_info *instr)
 
 invalid_arg:
 	pr_err("goldfish_nand_erase: invalid erase, start %llx, len %x, dev_size %llx, erase_size %x\n",
-		ofs, len, mtd->size, mtd->erasesize);
+	       ofs, len, mtd->size, mtd->erasesize);
 	return -EINVAL;
 }
 
 static int goldfish_nand_read_oob(struct mtd_info *mtd, loff_t ofs,
-				struct mtd_oob_ops *ops)
+				  struct mtd_oob_ops *ops)
 {
 	u32 rem;
 
@@ -156,12 +157,12 @@ static int goldfish_nand_read_oob(struct mtd_info *mtd, loff_t ofs,
 
 invalid_arg:
 	pr_err("goldfish_nand_read_oob: invalid read, start %llx, len %zx, ooblen %zx, dev_size %llx, write_size %x\n",
-		ofs, ops->len, ops->ooblen, mtd->size, mtd->writesize);
+	       ofs, ops->len, ops->ooblen, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
 static int goldfish_nand_write_oob(struct mtd_info *mtd, loff_t ofs,
-				struct mtd_oob_ops *ops)
+				   struct mtd_oob_ops *ops)
 {
 	u32 rem;
 
@@ -188,12 +189,12 @@ static int goldfish_nand_write_oob(struct mtd_info *mtd, loff_t ofs,
 
 invalid_arg:
 	pr_err("goldfish_nand_write_oob: invalid write, start %llx, len %zx, ooblen %zx, dev_size %llx, write_size %x\n",
-		ofs, ops->len, ops->ooblen, mtd->size, mtd->writesize);
+	       ofs, ops->len, ops->ooblen, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
 static int goldfish_nand_read(struct mtd_info *mtd, loff_t from, size_t len,
-				size_t *retlen, u_char *buf)
+			      size_t *retlen, u_char *buf)
 {
 	u32 rem;
 
@@ -210,12 +211,12 @@ static int goldfish_nand_read(struct mtd_info *mtd, loff_t from, size_t len,
 
 invalid_arg:
 	pr_err("goldfish_nand_read: invalid read, start %llx, len %zx, dev_size %llx, write_size %x\n",
-		from, len, mtd->size, mtd->writesize);
+	       from, len, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
 static int goldfish_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
-				size_t *retlen, const u_char *buf)
+			       size_t *retlen, const u_char *buf)
 {
 	u32 rem;
 
@@ -232,7 +233,7 @@ static int goldfish_nand_write(struct mtd_info *mtd, loff_t to, size_t len,
 
 invalid_arg:
 	pr_err("goldfish_nand_write: invalid write, start %llx, len %zx, dev_size %llx, write_size %x\n",
-		to, len, mtd->size, mtd->writesize);
+	       to, len, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
@@ -253,7 +254,7 @@ static int goldfish_nand_block_isbad(struct mtd_info *mtd, loff_t ofs)
 
 invalid_arg:
 	pr_err("goldfish_nand_block_isbad: invalid arg, ofs %llx, dev_size %llx, write_size %x\n",
-		ofs, mtd->size, mtd->writesize);
+	       ofs, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
@@ -276,12 +277,12 @@ static int goldfish_nand_block_markbad(struct mtd_info *mtd, loff_t ofs)
 
 invalid_arg:
 	pr_err("goldfish_nand_block_markbad: invalid arg, ofs %llx, dev_size %llx, write_size %x\n",
-		ofs, mtd->size, mtd->writesize);
+	       ofs, mtd->size, mtd->writesize);
 	return -EINVAL;
 }
 
 static int nand_setup_cmd_params(struct platform_device *pdev,
-						struct goldfish_nand *nand)
+				 struct goldfish_nand *nand)
 {
 	u64 paddr;
 	unsigned char __iomem  *base = nand->base;
@@ -298,7 +299,7 @@ static int nand_setup_cmd_params(struct platform_device *pdev,
 }
 
 static int goldfish_nand_init_device(struct platform_device *pdev,
-					struct goldfish_nand *nand, int id)
+				     struct goldfish_nand *nand, int id)
 {
 	u32 name_len;
 	u32 result;
@@ -328,19 +329,20 @@ static int goldfish_nand_init_device(struct platform_device *pdev,
 
 	mtd->priv = nand;
 
-	mtd->name = name = devm_kzalloc(&pdev->dev, name_len + 1, GFP_KERNEL);
+	name = devm_kzalloc(&pdev->dev, name_len + 1, GFP_KERNEL);
 	if (name == NULL)
 		return -ENOMEM;
+	mtd->name = name;
 
 	result = goldfish_nand_cmd(mtd, NAND_CMD_GET_DEV_NAME, 0, name_len,
-									name);
+				   name);
 	if (result != name_len) {
 		dev_err(&pdev->dev,
 			"goldfish_nand_init_device failed to get dev name %d != %d\n",
 			       result, name_len);
 		return -ENODEV;
 	}
-	((char *) mtd->name)[name_len] = '\0';
+	((char *)mtd->name)[name_len] = '\0';
 
 	/* Setup the MTD structure */
 	mtd->type = MTD_NANDFLASH;
diff --git a/drivers/staging/gs_fpgaboot/README b/drivers/staging/gs_fpgaboot/README
index cfa8624304e5..8d793c1769b0 100644
--- a/drivers/staging/gs_fpgaboot/README
+++ b/drivers/staging/gs_fpgaboot/README
@@ -25,13 +25,12 @@ TABLE OF CONTENTS.
 
 3. DESIGN
 
-	- load Xilinx FPGA bitstream format[1] firmware image file using 
+	- load Xilinx FPGA bitstream format[1] firmware image file using
 	  kernel firmware framework, request_firmware()
 	- program the Xilinx FPGA using SelectMAP (parallel) mode [2]
 	- FPGA prgram is done by gpio based bit-banging, as an example
 	- platform independent file: gs_fpgaboot.c
 	- platform dependent file: io.c
-	
 
 4. HOW TO USE
 
diff --git a/drivers/staging/gs_fpgaboot/io.c b/drivers/staging/gs_fpgaboot/io.c
index 23c12f47c8f1..b260e45c6698 100644
--- a/drivers/staging/gs_fpgaboot/io.c
+++ b/drivers/staging/gs_fpgaboot/io.c
@@ -31,10 +31,6 @@
 
 #include "io.h"
 
-#ifdef CONFIG_B4860G100
-static struct gpiobus gbus;
-#endif /* CONFIG_B4860G100 */
-
 static inline void byte0_out(unsigned char data);
 static inline void byte1_out(unsigned char data);
 static inline void xl_cclk_b(int32_t i);
@@ -92,176 +88,16 @@ static inline unsigned char bitswap(unsigned char s)
 	return d;
 }
 
-#ifdef CONFIG_B4860G100
-/*
- * ======================================================================
- * board specific configuration
- */
-
-static inline void mpc85xx_gpio_set_dir(
-			int32_t port,
-			uint32_t mask,
-			uint32_t dir)
-{
-	dir |= (in_be32(gbus.r[port]+GPDIR) & ~mask);
-	out_be32(gbus.r[port]+GPDIR, dir);
-}
-
-static inline void mpc85xx_gpio_set(int32_t port, uint32_t mask, uint32_t val)
-{
-	/* First mask off the unwanted parts of "dir" and "val" */
-	val &= mask;
-
-	/* Now read in the values we're supposed to preserve */
-	val |= (in_be32(gbus.r[port]+GPDAT) & ~mask);
-
-	out_be32(gbus.r[port]+GPDAT, val);
-}
-
-static inline uint32_t mpc85xx_gpio_get(int32_t port, uint32_t mask)
-{
-	/* Read the requested values */
-	return in_be32(gbus.r[port]+GPDAT) & mask;
-}
-
-static inline void mpc85xx_gpio_set_low(int32_t port, uint32_t gpios)
-{
-	mpc85xx_gpio_set(port, gpios, 0x00000000);
-}
-
-static inline void mpc85xx_gpio_set_high(int32_t port, uint32_t gpios)
-{
-	mpc85xx_gpio_set(port, gpios, 0xFFFFFFFF);
-}
-
-static inline void gpio_set_value(int32_t port, uint32_t gpio, uint32_t value)
-{
-	int32_t g;
-
-	g = 31 - gpio;
-	if (value)
-		mpc85xx_gpio_set_high(port, 1U << g);
-	else
-		mpc85xx_gpio_set_low(port, 1U << g);
-}
-
-static inline int gpio_get_value(int32_t port, uint32_t gpio)
-{
-	int32_t g;
-
-	g = 31 - gpio;
-	return !!mpc85xx_gpio_get(port, 1U << g);
-}
-
-static inline void xl_cclk_b(int32_t i)
-{
-	gpio_set_value(XL_CCLK_PORT, XL_CCLK_PIN, i);
-}
-
 void xl_program_b(int32_t i)
 {
-	gpio_set_value(XL_PROGN_PORT, XL_PROGN_PIN, i);
 }
 
 void xl_rdwr_b(int32_t i)
 {
-	gpio_set_value(XL_RDWRN_PORT, XL_RDWRN_PIN, i);
 }
 
 void xl_csi_b(int32_t i)
 {
-	gpio_set_value(XL_CSIN_PORT, XL_CSIN_PIN, i);
-}
-
-int xl_get_init_b(void)
-{
-	return gpio_get_value(XL_INITN_PORT, XL_INITN_PIN);
-}
-
-int xl_get_done_b(void)
-{
-	return gpio_get_value(XL_DONE_PORT, XL_DONE_PIN);
-}
-
-
-/* G100 specific bit swap and remmap (to gpio pins) for byte 0 */
-static inline uint32_t bit_remap_byte0(uint32_t s)
-{
-	uint32_t d;
-
-	d = (((s&0x80)>>7) | ((s&0x40)>>5) | ((s&0x20)>>3) | ((s&0x10)>>1) |
-		((s&0x08)<<1) | ((s&0x04)<<3) | ((s&0x02)<<6) | ((s&0x01)<<9));
-	return d;
-}
-
-/*
- * G100 specific MSB, in this order [byte0 | byte1], out
- */
-static inline void byte0_out(unsigned char data)
-{
-	uint32_t swap32;
-
-	swap32 =  bit_remap_byte0((uint32_t) data) << 8;
-
-	mpc85xx_gpio_set(0, 0x0002BF00, (uint32_t) swap32);
-}
-
-/*
- * G100 specific LSB, in this order [byte0 | byte1], out
- */
-static inline void byte1_out(unsigned char data)
-{
-	mpc85xx_gpio_set(0, 0x000000FF, (uint32_t) bitswap(data));
-}
-
-/*
- * configurable per device type for different I/O config
- */
-int xl_init_io(void)
-{
-	struct device_node *np;
-	const u32 *p_reg;
-	int reg, cnt;
-
-	cnt = 0;
-	memset(&gbus, 0, sizeof(struct gpiobus));
-	for_each_compatible_node(np, NULL, "fsl,qoriq-gpio") {
-		p_reg = of_get_property(np, "reg", NULL);
-		if (p_reg == NULL)
-			break;
-		reg = (int) *p_reg;
-		gbus.r[cnt] = of_iomap(np, 0);
-
-		if (!gbus.r[cnt]) {
-			pr_err("not findding gpio cell-index %d\n", cnt);
-			return -ENODEV;
-		}
-		cnt++;
-	}
-	mpc85xx_gpio_set_dir(0, 0x0002BFFF, 0x0002BFFF);
-	mpc85xx_gpio_set_dir(1, 0x00240060, 0x00240060);
-
-	gbus.ngpio = cnt;
-
-	return 0;
-}
-
-
-#else	/* placeholder for boards with different config */
-
-void xl_program_b(int32_t i)
-{
-	return;
-}
-
-void xl_rdwr_b(int32_t i)
-{
-	return;
-}
-
-void xl_csi_b(int32_t i)
-{
-	return;
 }
 
 int xl_get_init_b(void)
@@ -276,17 +112,14 @@ int xl_get_done_b(void)
 
 static inline void byte0_out(unsigned char data)
 {
-	return;
 }
 
 static inline void byte1_out(unsigned char data)
 {
-	return;
 }
 
 static inline void xl_cclk_b(int32_t i)
 {
-	return;
 }
 
 /*
@@ -296,5 +129,3 @@ int xl_init_io(void)
 {
 	return -1;
 }
-
-#endif /* CONFIG_B4860G100 */
diff --git a/drivers/staging/iio/accel/adis16201_core.c b/drivers/staging/iio/accel/adis16201_core.c
index 50ba1fa7f98a..7eae5fd0f393 100644
--- a/drivers/staging/iio/accel/adis16201_core.c
+++ b/drivers/staging/iio/accel/adis16201_core.c
@@ -111,6 +111,7 @@ static int adis16201_write_raw(struct iio_dev *indio_dev,
 	int bits;
 	s16 val16;
 	u8 addr;
+
 	switch (mask) {
 	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
@@ -131,8 +132,8 @@ static int adis16201_write_raw(struct iio_dev *indio_dev,
 }
 
 static const struct iio_chan_spec adis16201_channels[] = {
-  ADIS_SUPPLY_CHAN(ADIS16201_SUPPLY_OUT, ADIS16201_SCAN_SUPPLY, 0, 12),
-  ADIS_TEMP_CHAN(ADIS16201_TEMP_OUT, ADIS16201_SCAN_TEMP, 0, 12),
+	ADIS_SUPPLY_CHAN(ADIS16201_SUPPLY_OUT, ADIS16201_SCAN_SUPPLY, 0, 12),
+	ADIS_TEMP_CHAN(ADIS16201_TEMP_OUT, ADIS16201_SCAN_TEMP, 0, 12),
 	ADIS_ACCEL_CHAN(X, ADIS16201_XACCL_OUT, ADIS16201_SCAN_ACC_X,
 		BIT(IIO_CHAN_INFO_CALIBBIAS), 0, 14),
 	ADIS_ACCEL_CHAN(Y, ADIS16201_YACCL_OUT, ADIS16201_SCAN_ACC_Y,
diff --git a/drivers/staging/iio/accel/adis16203_core.c b/drivers/staging/iio/accel/adis16203_core.c
index f472137b0069..fbbe93f26bbe 100644
--- a/drivers/staging/iio/accel/adis16203_core.c
+++ b/drivers/staging/iio/accel/adis16203_core.c
@@ -37,6 +37,7 @@ static int adis16203_write_raw(struct iio_dev *indio_dev,
 	struct adis *st = iio_priv(indio_dev);
 	/* currently only one writable parameter which keeps this simple */
 	u8 addr = adis16203_addresses[chan->scan_index];
+
 	return adis_write_reg_16(st, addr, val & 0x3FFF);
 }
 
@@ -50,6 +51,7 @@ static int adis16203_read_raw(struct iio_dev *indio_dev,
 	int bits;
 	u8 addr;
 	s16 val16;
+
 	switch (mask) {
 	case IIO_CHAN_INFO_RAW:
 		return adis_single_conversion(indio_dev, chan,
diff --git a/drivers/staging/iio/accel/adis16204_core.c b/drivers/staging/iio/accel/adis16204_core.c
index 19eaebc77d7a..4c8acbc2e44e 100644
--- a/drivers/staging/iio/accel/adis16204_core.c
+++ b/drivers/staging/iio/accel/adis16204_core.c
@@ -119,6 +119,7 @@ static int adis16204_write_raw(struct iio_dev *indio_dev,
 	int bits;
 	s16 val16;
 	u8 addr;
+
 	switch (mask) {
 	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
diff --git a/drivers/staging/iio/accel/adis16209_core.c b/drivers/staging/iio/accel/adis16209_core.c
index 374dc6edbcf5..b2c7aeda75f0 100644
--- a/drivers/staging/iio/accel/adis16209_core.c
+++ b/drivers/staging/iio/accel/adis16209_core.c
@@ -44,6 +44,7 @@ static int adis16209_write_raw(struct iio_dev *indio_dev,
 	int bits;
 	s16 val16;
 	u8 addr;
+
 	switch (mask) {
 	case IIO_CHAN_INFO_CALIBBIAS:
 		switch (chan->type) {
diff --git a/drivers/staging/iio/accel/adis16220_core.c b/drivers/staging/iio/accel/adis16220_core.c
index 6f38ca95f9bb..d478f5130a0f 100644
--- a/drivers/staging/iio/accel/adis16220_core.c
+++ b/drivers/staging/iio/accel/adis16220_core.c
@@ -70,7 +70,7 @@ static int adis16220_capture(struct iio_dev *indio_dev)
 	if (ret)
 		dev_err(&indio_dev->dev, "problem beginning capture");
 
-	msleep(10); /* delay for capture to finish */
+	usleep_range(10000, 11000); /* delay for capture to finish */
 
 	return ret;
 }
@@ -323,15 +323,14 @@ static int adis16220_read_raw(struct iio_dev *indio_dev,
 		sval = (s16)(sval << (16 - bits)) >> (16 - bits);
 		*val = sval;
 		return IIO_VAL_INT;
-	} else {
-		ret = adis_read_reg_16(&st->adis, addr->addr, &uval);
-		if (ret)
-			return ret;
-		bits = addr->bits;
-		uval &= (1 << bits) - 1;
-		*val = uval;
-		return IIO_VAL_INT;
 	}
+	ret = adis_read_reg_16(&st->adis, addr->addr, &uval);
+	if (ret)
+		return ret;
+	bits = addr->bits;
+	uval &= (1 << bits) - 1;
+	*val = uval;
+	return IIO_VAL_INT;
 }
 
 static const struct iio_chan_spec adis16220_channels[] = {
diff --git a/drivers/staging/iio/accel/adis16240_core.c b/drivers/staging/iio/accel/adis16240_core.c
index 74ace2a8769d..3f46086eab3a 100644
--- a/drivers/staging/iio/accel/adis16240_core.c
+++ b/drivers/staging/iio/accel/adis16240_core.c
@@ -99,9 +99,8 @@ static int adis16240_read_raw(struct iio_dev *indio_dev,
 				*val = 4;
 				*val2 = 880000; /* 4.88 mV */
 				return IIO_VAL_INT_PLUS_MICRO;
-			} else {
-				return -EINVAL;
 			}
+			return -EINVAL;
 		case IIO_TEMP:
 			*val = 244; /* 0.244 C */
 			*val2 = 0;
@@ -163,6 +162,7 @@ static int adis16240_write_raw(struct iio_dev *indio_dev,
 	int bits = 10;
 	s16 val16;
 	u8 addr;
+
 	switch (mask) {
 	case IIO_CHAN_INFO_CALIBBIAS:
 		val16 = val & ((1 << bits) - 1);
diff --git a/drivers/staging/iio/accel/lis3l02dq_core.c b/drivers/staging/iio/accel/lis3l02dq_core.c
index 898653c09279..f5e145caffa9 100644
--- a/drivers/staging/iio/accel/lis3l02dq_core.c
+++ b/drivers/staging/iio/accel/lis3l02dq_core.c
@@ -212,6 +212,7 @@ static int lis3l02dq_write_thresh(struct iio_dev *indio_dev,
 				  int val, int val2)
 {
 	u16 value = val;
+
 	return lis3l02dq_spi_write_reg_s16(indio_dev,
 					   LIS3L02DQ_REG_THS_L_ADDR,
 					   value);
@@ -226,6 +227,7 @@ static int lis3l02dq_write_raw(struct iio_dev *indio_dev,
 	int ret = -EINVAL, reg;
 	u8 uval;
 	s8 sval;
+
 	switch (mask) {
 	case IIO_CHAN_INFO_CALIBBIAS:
 		if (val > 255 || val < -256)
@@ -302,6 +304,7 @@ static ssize_t lis3l02dq_read_frequency(struct device *dev,
 	struct iio_dev *indio_dev = dev_to_iio_dev(dev);
 	int ret, len = 0;
 	s8 t;
+
 	ret = lis3l02dq_spi_read_reg_8(indio_dev,
 				       LIS3L02DQ_REG_CTRL_1_ADDR,
 				       (u8 *)&t);
@@ -565,6 +568,7 @@ static int lis3l02dq_read_event_config(struct iio_dev *indio_dev,
 	u8 val;
 	int ret;
 	u8 mask = (1 << (chan->channel2*2 + (dir == IIO_EV_DIR_RISING)));
+
 	ret = lis3l02dq_spi_read_reg_8(indio_dev,
 				       LIS3L02DQ_REG_WAKE_UP_CFG_ADDR,
 				       &val);
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c
index bf33fdead479..61f94221b8b7 100644
--- a/drivers/staging/iio/accel/lis3l02dq_ring.c
+++ b/drivers/staging/iio/accel/lis3l02dq_ring.c
@@ -19,6 +19,7 @@ static inline u16 combine_8_to_16(u8 lower, u8 upper)
 {
 	u16 _lower = lower;
 	u16 _upper = upper;
+
 	return _lower | (_upper << 8);
 }
 
@@ -33,8 +34,8 @@ irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
 	if (st->trigger_on) {
 		iio_trigger_poll(st->trig);
 		return IRQ_HANDLED;
-	} else
-		return IRQ_WAKE_THREAD;
+	}
+	return IRQ_WAKE_THREAD;
 }
 
 static const u8 read_all_tx_array[] = {
diff --git a/drivers/staging/iio/accel/sca3000_core.c b/drivers/staging/iio/accel/sca3000_core.c
index ed30e32e60de..e4e56391487a 100644
--- a/drivers/staging/iio/accel/sca3000_core.c
+++ b/drivers/staging/iio/accel/sca3000_core.c
@@ -506,7 +506,8 @@ static int sca3000_read_raw(struct iio_dev *indio_dev,
 				mutex_unlock(&st->lock);
 				return ret;
 			}
-			*val = ((st->rx[0] & 0x3F) << 3) | ((st->rx[1] & 0xE0) >> 5);
+			*val = ((st->rx[0] & 0x3F) << 3) |
+			       ((st->rx[1] & 0xE0) >> 5);
 		}
 		mutex_unlock(&st->lock);
 		return IIO_VAL_INT;
@@ -713,6 +714,7 @@ static int sca3000_read_thresh(struct iio_dev *indio_dev,
 	int ret, i;
 	struct sca3000_state *st = iio_priv(indio_dev);
 	int num = chan->channel2;
+
 	mutex_lock(&st->lock);
 	ret = sca3000_read_ctrl_reg(st, sca3000_addresses[num][1]);
 	mutex_unlock(&st->lock);
diff --git a/drivers/staging/iio/accel/sca3000_ring.c b/drivers/staging/iio/accel/sca3000_ring.c
index 33f0e9235be7..157827651bfa 100644
--- a/drivers/staging/iio/accel/sca3000_ring.c
+++ b/drivers/staging/iio/accel/sca3000_ring.c
@@ -117,7 +117,7 @@ static int sca3000_read_first_n_hw_rb(struct iio_buffer *r,
 		goto error_ret;
 
 	for (i = 0; i < num_read; i++)
-		*(((u16 *)rx) + i) = be16_to_cpup((u16 *)rx + i);
+		*(((u16 *)rx) + i) = be16_to_cpup((__be16 *)rx + i);
 
 	if (copy_to_user(buf, rx, num_read))
 		ret = -EFAULT;
diff --git a/drivers/staging/iio/adc/Kconfig b/drivers/staging/iio/adc/Kconfig
index d01c7076a342..d0016ce6e658 100644
--- a/drivers/staging/iio/adc/Kconfig
+++ b/drivers/staging/iio/adc/Kconfig
@@ -22,6 +22,9 @@ config AD7606_IFACE_PARALLEL
 	  Say yes here to include parallel interface support on the AD7606
 	  ADC driver.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad7606_iface_parallel.
+
 config AD7606_IFACE_SPI
 	tristate "spi interface support"
 	depends on AD7606
@@ -30,6 +33,9 @@ config AD7606_IFACE_SPI
 	  Say yes here to include parallel interface support on the AD7606
 	  ADC driver.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad7606_iface_spi.
+
 config AD7780
 	tristate "Analog Devices AD7780 and similar ADCs driver"
 	depends on SPI
@@ -107,4 +113,6 @@ config SPEAR_ADC
 	  Say yes here to build support for the integrated ADC inside the
 	  ST SPEAr SoC. Provides direct access via sysfs.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called spear_adc.
 endmenu
diff --git a/drivers/staging/iio/adc/ad7192.c b/drivers/staging/iio/adc/ad7192.c
index 83bb44b38152..c110a255d4e8 100644
--- a/drivers/staging/iio/adc/ad7192.c
+++ b/drivers/staging/iio/adc/ad7192.c
@@ -213,7 +213,7 @@ static int ad7192_setup(struct ad7192_state *st,
 	ret = spi_write(st->sd.spi, &ones, 6);
 	if (ret < 0)
 		goto out;
-	msleep(1); /* Wait for at least 500us */
+	usleep_range(500, 1000); /* Wait for at least 500us */
 
 	/* write/read test for device presence */
 	ret = ad_sd_read_reg(&st->sd, AD7192_REG_ID, 1, &id);
diff --git a/drivers/staging/iio/adc/ad7606_core.c b/drivers/staging/iio/adc/ad7606_core.c
index f0f05f195d2c..bf2c8013134c 100644
--- a/drivers/staging/iio/adc/ad7606_core.c
+++ b/drivers/staging/iio/adc/ad7606_core.c
@@ -140,7 +140,7 @@ static ssize_t ad7606_store_range(struct device *dev,
 	return count;
 }
 
-static IIO_DEVICE_ATTR(in_voltage_range, S_IRUGO | S_IWUSR, \
+static IIO_DEVICE_ATTR(in_voltage_range, S_IRUGO | S_IWUSR,
 		       ad7606_show_range, ad7606_store_range, 0);
 static IIO_CONST_ATTR(in_voltage_range_available, "5000 10000");
 
diff --git a/drivers/staging/iio/adc/ad7816.c b/drivers/staging/iio/adc/ad7816.c
index 158d770f961a..734a7e4886a0 100644
--- a/drivers/staging/iio/adc/ad7816.c
+++ b/drivers/staging/iio/adc/ad7816.c
@@ -118,8 +118,7 @@ static ssize_t ad7816_show_mode(struct device *dev,
 
 	if (chip->mode)
 		return sprintf(buf, "power-save\n");
-	else
-		return sprintf(buf, "full\n");
+	return sprintf(buf, "full\n");
 }
 
 static ssize_t ad7816_store_mode(struct device *dev,
@@ -226,8 +225,8 @@ static ssize_t ad7816_show_value(struct device *dev,
 		if (value < 0)
 			data = (1 << AD7816_TEMP_FLOAT_OFFSET) - data;
 		return sprintf(buf, "%d.%.2d\n", value, data * 25);
-	} else
-		return sprintf(buf, "%u\n", data);
+	}
+	return sprintf(buf, "%u\n", data);
 }
 
 static IIO_DEVICE_ATTR(value, S_IRUGO, ad7816_show_value, NULL, 0);
@@ -275,8 +274,8 @@ static ssize_t ad7816_show_oti(struct device *dev,
 			(chip->oti_data[chip->channel_id] -
 			AD7816_BOUND_VALUE_BASE);
 		return sprintf(buf, "%d\n", value);
-	} else
-		return sprintf(buf, "%u\n", chip->oti_data[chip->channel_id]);
+	}
+	return sprintf(buf, "%u\n", chip->oti_data[chip->channel_id]);
 }
 
 static inline ssize_t ad7816_set_oti(struct device *dev,
@@ -442,6 +441,5 @@ static struct spi_driver ad7816_driver = {
 module_spi_driver(ad7816_driver);
 
 MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
-MODULE_DESCRIPTION("Analog Devices AD7816/7/8 digital"
-			" temperature sensor driver");
+MODULE_DESCRIPTION("Analog Devices AD7816/7/8 digital temperature sensor driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/adc/mxs-lradc.c b/drivers/staging/iio/adc/mxs-lradc.c
index 468327f4a753..32a19264a170 100644
--- a/drivers/staging/iio/adc/mxs-lradc.c
+++ b/drivers/staging/iio/adc/mxs-lradc.c
@@ -365,56 +365,49 @@ static u32 mxs_lradc_plate_mask(struct mxs_lradc *lradc)
 {
 	if (lradc->soc == IMX23_LRADC)
 		return LRADC_CTRL0_MX23_PLATE_MASK;
-	else
-		return LRADC_CTRL0_MX28_PLATE_MASK;
+	return LRADC_CTRL0_MX28_PLATE_MASK;
 }
 
 static u32 mxs_lradc_irq_en_mask(struct mxs_lradc *lradc)
 {
 	if (lradc->soc == IMX23_LRADC)
 		return LRADC_CTRL1_MX23_LRADC_IRQ_EN_MASK;
-	else
-		return LRADC_CTRL1_MX28_LRADC_IRQ_EN_MASK;
+	return LRADC_CTRL1_MX28_LRADC_IRQ_EN_MASK;
 }
 
 static u32 mxs_lradc_irq_mask(struct mxs_lradc *lradc)
 {
 	if (lradc->soc == IMX23_LRADC)
 		return LRADC_CTRL1_MX23_LRADC_IRQ_MASK;
-	else
-		return LRADC_CTRL1_MX28_LRADC_IRQ_MASK;
+	return LRADC_CTRL1_MX28_LRADC_IRQ_MASK;
 }
 
 static u32 mxs_lradc_touch_detect_bit(struct mxs_lradc *lradc)
 {
 	if (lradc->soc == IMX23_LRADC)
 		return LRADC_CTRL0_MX23_TOUCH_DETECT_ENABLE;
-	else
-		return LRADC_CTRL0_MX28_TOUCH_DETECT_ENABLE;
+	return LRADC_CTRL0_MX28_TOUCH_DETECT_ENABLE;
 }
 
 static u32 mxs_lradc_drive_x_plate(struct mxs_lradc *lradc)
 {
 	if (lradc->soc == IMX23_LRADC)
 		return LRADC_CTRL0_MX23_XP | LRADC_CTRL0_MX23_XM;
-	else
-		return LRADC_CTRL0_MX28_XPPSW | LRADC_CTRL0_MX28_XNNSW;
+	return LRADC_CTRL0_MX28_XPPSW | LRADC_CTRL0_MX28_XNNSW;
 }
 
 static u32 mxs_lradc_drive_y_plate(struct mxs_lradc *lradc)
 {
 	if (lradc->soc == IMX23_LRADC)
 		return LRADC_CTRL0_MX23_YP | LRADC_CTRL0_MX23_YM;
-	else
-		return LRADC_CTRL0_MX28_YPPSW | LRADC_CTRL0_MX28_YNNSW;
+	return LRADC_CTRL0_MX28_YPPSW | LRADC_CTRL0_MX28_YNNSW;
 }
 
 static u32 mxs_lradc_drive_pressure(struct mxs_lradc *lradc)
 {
 	if (lradc->soc == IMX23_LRADC)
 		return LRADC_CTRL0_MX23_YP | LRADC_CTRL0_MX23_XM;
-	else
-		return LRADC_CTRL0_MX28_YPPSW | LRADC_CTRL0_MX28_XNNSW;
+	return LRADC_CTRL0_MX28_YPPSW | LRADC_CTRL0_MX28_XNNSW;
 }
 
 static bool mxs_lradc_check_touch_event(struct mxs_lradc *lradc)
@@ -1280,7 +1273,7 @@ static int mxs_lradc_buffer_preenable(struct iio_dev *iio)
 	if (!ret)
 		return -EBUSY;
 
-	lradc->buffer = kmalloc(len * sizeof(*lradc->buffer), GFP_KERNEL);
+	lradc->buffer = kmalloc_array(len, sizeof(*lradc->buffer), GFP_KERNEL);
 	if (!lradc->buffer) {
 		ret = -ENOMEM;
 		goto err_mem;
@@ -1427,6 +1420,7 @@ static int mxs_lradc_hw_init(struct mxs_lradc *lradc)
 		(LRADC_DELAY_TIMER_PER << LRADC_DELAY_DELAY_OFFSET);
 
 	int ret = stmp_reset_block(lradc->base);
+
 	if (ret)
 		return ret;
 
diff --git a/drivers/staging/iio/addac/adt7316.c b/drivers/staging/iio/addac/adt7316.c
index 5f1770e6f6c3..5b11b42c0254 100644
--- a/drivers/staging/iio/addac/adt7316.c
+++ b/drivers/staging/iio/addac/adt7316.c
@@ -263,8 +263,8 @@ static ssize_t adt7316_store_enabled(struct device *dev,
 
 	if (_adt7316_store_enabled(chip, enable) < 0)
 		return -EIO;
-	else
-		return len;
+
+	return len;
 }
 
 static IIO_DEVICE_ATTR(enabled, S_IRUGO | S_IWUSR,
@@ -325,8 +325,8 @@ static ssize_t adt7316_show_mode(struct device *dev,
 
 	if (chip->config2 & ADT7316_AD_SINGLE_CH_MODE)
 		return sprintf(buf, "single_channel\n");
-	else
-		return sprintf(buf, "round_robin\n");
+
+	return sprintf(buf, "round_robin\n");
 }
 
 static ssize_t adt7316_store_mode(struct device *dev,
@@ -385,18 +385,18 @@ static ssize_t adt7316_show_ad_channel(struct device *dev,
 		if (((chip->id & ID_FAMILY_MASK) == ID_ADT75XX) &&
 			(chip->config1 & ADT7516_SEL_AIN1_2_EX_TEMP_MASK) == 0)
 			return sprintf(buf, "2 - AIN1\n");
-		else
-			return sprintf(buf, "2 - External Temperature\n");
+
+		return sprintf(buf, "2 - External Temperature\n");
 	case ADT7516_AD_SINGLE_CH_AIN2:
 		if ((chip->config1 & ADT7516_SEL_AIN1_2_EX_TEMP_MASK) == 0)
 			return sprintf(buf, "3 - AIN2\n");
-		else
-			return sprintf(buf, "N/A\n");
+
+		return sprintf(buf, "N/A\n");
 	case ADT7516_AD_SINGLE_CH_AIN3:
 		if (chip->config1 & ADT7516_SEL_AIN3)
 			return sprintf(buf, "4 - AIN3\n");
-		else
-			return sprintf(buf, "N/A\n");
+
+		return sprintf(buf, "N/A\n");
 	case ADT7516_AD_SINGLE_CH_AIN4:
 		return sprintf(buf, "5 - AIN4\n");
 	default:
@@ -856,20 +856,19 @@ static ssize_t adt7316_show_DAC_update_mode(struct device *dev,
 
 	if (!(chip->config3 & ADT7316_DA_EN_VIA_DAC_LDCA))
 		return sprintf(buf, "manual\n");
-	else {
-		switch (chip->dac_config & ADT7316_DA_EN_MODE_MASK) {
-		case ADT7316_DA_EN_MODE_SINGLE:
-			return sprintf(buf,
-				"0 - auto at any MSB DAC writing\n");
-		case ADT7316_DA_EN_MODE_AB_CD:
-			return sprintf(buf,
-				"1 - auto at MSB DAC AB and CD writing\n");
-		case ADT7316_DA_EN_MODE_ABCD:
-			return sprintf(buf,
-				"2 - auto at MSB DAC ABCD writing\n");
-		default: /* ADT7316_DA_EN_MODE_LDAC */
-			return sprintf(buf, "3 - manual\n");
-		}
+
+	switch (chip->dac_config & ADT7316_DA_EN_MODE_MASK) {
+	case ADT7316_DA_EN_MODE_SINGLE:
+		return sprintf(buf,
+			"0 - auto at any MSB DAC writing\n");
+	case ADT7316_DA_EN_MODE_AB_CD:
+		return sprintf(buf,
+			"1 - auto at MSB DAC AB and CD writing\n");
+	case ADT7316_DA_EN_MODE_ABCD:
+		return sprintf(buf,
+			"2 - auto at MSB DAC ABCD writing\n");
+	default: /* ADT7316_DA_EN_MODE_LDAC */
+		return sprintf(buf, "3 - manual\n");
 	}
 }
 
@@ -1180,8 +1179,8 @@ static ssize_t adt7316_show_ad(struct adt7316_chip_info *chip,
 
 		if ((chip->id & ID_FAMILY_MASK) == ID_ADT75XX)
 			return sprintf(buf, "%d\n", data);
-		else
-			break;
+
+		break;
 	}
 
 	if (data & ADT7316_T_VALUE_SIGN) {
@@ -1648,8 +1647,8 @@ static ssize_t adt7316_show_bus_type(struct device *dev,
 
 	if (stat)
 		return sprintf(buf, "spi\n");
-	else
-		return sprintf(buf, "i2c\n");
+
+	return sprintf(buf, "i2c\n");
 }
 
 static IIO_DEVICE_ATTR(bus_type, S_IRUGO, adt7316_show_bus_type, NULL, 0);
@@ -2185,6 +2184,5 @@ int adt7316_probe(struct device *dev, struct adt7316_bus *bus,
 EXPORT_SYMBOL(adt7316_probe);
 
 MODULE_AUTHOR("Sonic Zhang <sonic.zhang@analog.com>");
-MODULE_DESCRIPTION("Analog Devices ADT7316/7/8 and ADT7516/7/9 digital"
-			" temperature sensor, ADC and DAC driver");
+MODULE_DESCRIPTION("Analog Devices ADT7316/7/8 and ADT7516/7/9 digital temperature sensor, ADC and DAC driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/staging/iio/cdc/ad7150.c b/drivers/staging/iio/cdc/ad7150.c
index 047af2376300..a2b7ae3329c0 100644
--- a/drivers/staging/iio/cdc/ad7150.c
+++ b/drivers/staging/iio/cdc/ad7150.c
@@ -143,19 +143,15 @@ static int ad7150_read_event_config(struct iio_dev *indio_dev,
 	case IIO_EV_TYPE_MAG_ADAPTIVE:
 		if (dir == IIO_EV_DIR_RISING)
 			return adaptive && (threshtype == 0x1);
-		else
-			return adaptive && (threshtype == 0x0);
+		return adaptive && (threshtype == 0x0);
 	case IIO_EV_TYPE_THRESH_ADAPTIVE:
 		if (dir == IIO_EV_DIR_RISING)
 			return adaptive && (threshtype == 0x3);
-		else
-			return adaptive && (threshtype == 0x2);
-
+		return adaptive && (threshtype == 0x2);
 	case IIO_EV_TYPE_THRESH:
 		if (dir == IIO_EV_DIR_RISING)
 			return !adaptive && (threshtype == 0x1);
-		else
-			return !adaptive && (threshtype == 0x0);
+		return !adaptive && (threshtype == 0x0);
 	default:
 		break;
 	}
diff --git a/drivers/staging/iio/frequency/Kconfig b/drivers/staging/iio/frequency/Kconfig
index 93b7141b2c1f..fc726d3c64a6 100644
--- a/drivers/staging/iio/frequency/Kconfig
+++ b/drivers/staging/iio/frequency/Kconfig
@@ -3,13 +3,6 @@
 #
 menu "Direct Digital Synthesis"
 
-config AD5930
-	tristate "Analog Devices ad5930/5932 driver"
-	depends on SPI
-	help
-	  Say yes here to build support for Analog Devices DDS chip
-	  ad5930/ad5932, provides direct access via sysfs.
-
 config AD9832
 	tristate "Analog Devices ad9832/5 driver"
 	depends on SPI
@@ -30,32 +23,4 @@ config AD9834
 	  To compile this driver as a module, choose M here: the
 	  module will be called ad9834.
 
-config AD9850
-	tristate "Analog Devices ad9850/1 driver"
-	depends on SPI
-	help
-	  Say yes here to build support for Analog Devices DDS chip
-	  ad9850/1, provides direct access via sysfs.
-
-config AD9852
-	tristate "Analog Devices ad9852/4 driver"
-	depends on SPI
-	help
-	  Say yes here to build support for Analog Devices DDS chip
-	  ad9852/4, provides direct access via sysfs.
-
-config AD9910
-	tristate "Analog Devices ad9910 driver"
-	depends on SPI
-	help
-	  Say yes here to build support for Analog Devices DDS chip
-	  ad9910, provides direct access via sysfs.
-
-config AD9951
-	tristate "Analog Devices ad9951 driver"
-	depends on SPI
-	help
-	  Say yes here to build support for Analog Devices DDS chip
-	  ad9951, provides direct access via sysfs.
-
 endmenu
diff --git a/drivers/staging/iio/frequency/Makefile b/drivers/staging/iio/frequency/Makefile
index 147746176b9b..e5dbcfce44f9 100644
--- a/drivers/staging/iio/frequency/Makefile
+++ b/drivers/staging/iio/frequency/Makefile
@@ -2,10 +2,5 @@
 # Makefile for Direct Digital Synthesis drivers
 #
 
-obj-$(CONFIG_AD5930) += ad5930.o
 obj-$(CONFIG_AD9832) += ad9832.o
 obj-$(CONFIG_AD9834) += ad9834.o
-obj-$(CONFIG_AD9850) += ad9850.o
-obj-$(CONFIG_AD9852) += ad9852.o
-obj-$(CONFIG_AD9910) += ad9910.o
-obj-$(CONFIG_AD9951) += ad9951.o
diff --git a/drivers/staging/iio/frequency/ad5930.c b/drivers/staging/iio/frequency/ad5930.c
deleted file mode 100644
index a4aeee6ffdf2..000000000000
--- a/drivers/staging/iio/frequency/ad5930.c
+++ /dev/null
@@ -1,140 +0,0 @@
-/*
- * Driver for ADI Direct Digital Synthesis ad5930
- *
- * Copyright (c) 2010-2010 Analog Devices Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- */
-#include <linux/types.h>
-#include <linux/mutex.h>
-#include <linux/device.h>
-#include <linux/spi/spi.h>
-#include <linux/slab.h>
-#include <linux/sysfs.h>
-#include <linux/module.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/sysfs.h>
-
-#define DRV_NAME "ad5930"
-
-#define value_mask (u16)0xf000
-#define addr_shift 12
-
-/* Register format: 4 bits addr + 12 bits value */
-struct ad5903_config {
-	u16 control;
-	u16 incnum;
-	u16 frqdelt[2];
-	u16 incitvl;
-	u16 buritvl;
-	u16 strtfrq[2];
-};
-
-struct ad5930_state {
-	struct mutex lock;
-	struct spi_device *sdev;
-};
-
-static ssize_t ad5930_set_parameter(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf,
-					size_t len)
-{
-	struct spi_transfer xfer;
-	int ret;
-	struct ad5903_config *config = (struct ad5903_config *)buf;
-	struct iio_dev *idev = dev_to_iio_dev(dev);
-	struct ad5930_state *st = iio_priv(idev);
-
-	config->control = (config->control & ~value_mask);
-	config->incnum = (config->control & ~value_mask) | (1 << addr_shift);
-	config->frqdelt[0] = (config->control & ~value_mask) | (2 << addr_shift);
-	config->frqdelt[1] = (config->control & ~value_mask) | 3 << addr_shift;
-	config->incitvl = (config->control & ~value_mask) | 4 << addr_shift;
-	config->buritvl = (config->control & ~value_mask) | 8 << addr_shift;
-	config->strtfrq[0] = (config->control & ~value_mask) | 0xc << addr_shift;
-	config->strtfrq[1] = (config->control & ~value_mask) | 0xd << addr_shift;
-
-	xfer.len = len;
-	xfer.tx_buf = config;
-	mutex_lock(&st->lock);
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-error_ret:
-	mutex_unlock(&st->lock);
-
-	return ret ? ret : len;
-}
-
-static IIO_DEVICE_ATTR(dds, S_IWUSR, NULL, ad5930_set_parameter, 0);
-
-static struct attribute *ad5930_attributes[] = {
-	&iio_dev_attr_dds.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad5930_attribute_group = {
-	.attrs = ad5930_attributes,
-};
-
-static const struct iio_info ad5930_info = {
-	.attrs = &ad5930_attribute_group,
-	.driver_module = THIS_MODULE,
-};
-
-static int ad5930_probe(struct spi_device *spi)
-{
-	struct ad5930_state *st;
-	struct iio_dev *idev;
-	int ret = 0;
-
-	idev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (!idev)
-		return -ENOMEM;
-	spi_set_drvdata(spi, idev);
-	st = iio_priv(idev);
-
-	mutex_init(&st->lock);
-	st->sdev = spi;
-	idev->dev.parent = &spi->dev;
-	idev->info = &ad5930_info;
-	idev->modes = INDIO_DIRECT_MODE;
-
-	ret = iio_device_register(idev);
-	if (ret)
-		return ret;
-	spi->max_speed_hz = 2000000;
-	spi->mode = SPI_MODE_3;
-	spi->bits_per_word = 16;
-	spi_setup(spi);
-
-	return 0;
-}
-
-static int ad5930_remove(struct spi_device *spi)
-{
-	iio_device_unregister(spi_get_drvdata(spi));
-
-	return 0;
-}
-
-static struct spi_driver ad5930_driver = {
-	.driver = {
-		.name = DRV_NAME,
-		.owner = THIS_MODULE,
-	},
-	.probe = ad5930_probe,
-	.remove = ad5930_remove,
-};
-module_spi_driver(ad5930_driver);
-
-MODULE_AUTHOR("Cliff Cai");
-MODULE_DESCRIPTION("Analog Devices ad5930 driver");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/frequency/ad9834.c b/drivers/staging/iio/frequency/ad9834.c
index 86cda6176093..5c803191c2ce 100644
--- a/drivers/staging/iio/frequency/ad9834.c
+++ b/drivers/staging/iio/frequency/ad9834.c
@@ -28,6 +28,7 @@
 static unsigned int ad9834_calc_freqreg(unsigned long mclk, unsigned long fout)
 {
 	unsigned long long freqreg = (u64) fout * (u64) (1 << AD9834_FREQ_BITS);
+
 	do_div(freqreg, mclk);
 	return freqreg;
 }
diff --git a/drivers/staging/iio/frequency/ad9850.c b/drivers/staging/iio/frequency/ad9850.c
deleted file mode 100644
index 8727933cafcf..000000000000
--- a/drivers/staging/iio/frequency/ad9850.c
+++ /dev/null
@@ -1,120 +0,0 @@
-/*
- * Driver for ADI Direct Digital Synthesis ad9850
- *
- * Copyright (c) 2010-2010 Analog Devices Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- */
-#include <linux/types.h>
-#include <linux/mutex.h>
-#include <linux/device.h>
-#include <linux/spi/spi.h>
-#include <linux/slab.h>
-#include <linux/sysfs.h>
-#include <linux/module.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/sysfs.h>
-
-#define DRV_NAME "ad9850"
-
-/* Register format: 4 bits addr + 12 bits value */
-struct ad9850_config {
-	u8 control[5];
-};
-
-struct ad9850_state {
-	struct mutex lock;
-	struct spi_device *sdev;
-};
-
-static ssize_t ad9850_set_parameter(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf,
-					size_t len)
-{
-	struct spi_transfer xfer;
-	int ret;
-	struct ad9850_config *config = (struct ad9850_config *)buf;
-	struct iio_dev *idev = dev_to_iio_dev(dev);
-	struct ad9850_state *st = iio_priv(idev);
-
-	xfer.len = len;
-	xfer.tx_buf = config;
-	mutex_lock(&st->lock);
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	mutex_unlock(&st->lock);
-
-	return ret ? ret : len;
-}
-
-static IIO_DEVICE_ATTR(dds, S_IWUSR, NULL, ad9850_set_parameter, 0);
-
-static struct attribute *ad9850_attributes[] = {
-	&iio_dev_attr_dds.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad9850_attribute_group = {
-	.attrs = ad9850_attributes,
-};
-
-static const struct iio_info ad9850_info = {
-	.attrs = &ad9850_attribute_group,
-	.driver_module = THIS_MODULE,
-};
-
-static int ad9850_probe(struct spi_device *spi)
-{
-	struct ad9850_state *st;
-	struct iio_dev *idev;
-	int ret = 0;
-
-	idev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (!idev)
-		return -ENOMEM;
-	spi_set_drvdata(spi, idev);
-	st = iio_priv(idev);
-	mutex_init(&st->lock);
-	st->sdev = spi;
-
-	idev->dev.parent = &spi->dev;
-	idev->info = &ad9850_info;
-	idev->modes = INDIO_DIRECT_MODE;
-
-	ret = iio_device_register(idev);
-	if (ret)
-		return ret;
-	spi->max_speed_hz = 2000000;
-	spi->mode = SPI_MODE_3;
-	spi->bits_per_word = 16;
-	spi_setup(spi);
-
-	return 0;
-}
-
-static int ad9850_remove(struct spi_device *spi)
-{
-	iio_device_unregister(spi_get_drvdata(spi));
-
-	return 0;
-}
-
-static struct spi_driver ad9850_driver = {
-	.driver = {
-		.name = DRV_NAME,
-		.owner = THIS_MODULE,
-	},
-	.probe = ad9850_probe,
-	.remove = ad9850_remove,
-};
-module_spi_driver(ad9850_driver);
-
-MODULE_AUTHOR("Cliff Cai");
-MODULE_DESCRIPTION("Analog Devices ad9850 driver");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/frequency/ad9852.c b/drivers/staging/iio/frequency/ad9852.c
deleted file mode 100644
index 11e4367375d2..000000000000
--- a/drivers/staging/iio/frequency/ad9852.c
+++ /dev/null
@@ -1,253 +0,0 @@
-/*
- * Driver for ADI Direct Digital Synthesis ad9852
- *
- * Copyright (c) 2010 Analog Devices Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- */
-#include <linux/types.h>
-#include <linux/mutex.h>
-#include <linux/device.h>
-#include <linux/spi/spi.h>
-#include <linux/slab.h>
-#include <linux/sysfs.h>
-#include <linux/module.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/sysfs.h>
-
-#define DRV_NAME "ad9852"
-
-#define addr_phaad1 0x0
-#define addr_phaad2 0x1
-#define addr_fretu1 0x2
-#define addr_fretu2 0x3
-#define addr_delfre 0x4
-#define addr_updclk 0x5
-#define addr_ramclk 0x6
-#define addr_contrl 0x7
-#define addr_optskm 0x8
-#define addr_optskr 0xa
-#define addr_dacctl 0xb
-
-#define COMPPD		(1 << 4)
-#define REFMULT2	(1 << 2)
-#define BYPPLL		(1 << 5)
-#define PLLRANG		(1 << 6)
-#define IEUPCLK		(1)
-#define OSKEN		(1 << 5)
-
-#define read_bit	(1 << 7)
-
-/* Register format: 1 byte addr + value */
-struct ad9852_config {
-	u8 phajst0[3];
-	u8 phajst1[3];
-	u8 fretun1[6];
-	u8 fretun2[6];
-	u8 dltafre[6];
-	u8 updtclk[5];
-	u8 ramprat[4];
-	u8 control[5];
-	u8 outpskm[3];
-	u8 outpskr[2];
-	u8 daccntl[3];
-};
-
-struct ad9852_state {
-	struct mutex lock;
-	struct spi_device *sdev;
-};
-
-static ssize_t ad9852_set_parameter(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf,
-					size_t len)
-{
-	struct spi_transfer xfer;
-	int ret;
-	struct ad9852_config *config = (struct ad9852_config *)buf;
-	struct iio_dev *idev = dev_to_iio_dev(dev);
-	struct ad9852_state *st = iio_priv(idev);
-
-	xfer.len = 3;
-	xfer.tx_buf = &config->phajst0[0];
-	mutex_lock(&st->lock);
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 3;
-	xfer.tx_buf = &config->phajst1[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 6;
-	xfer.tx_buf = &config->fretun1[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 6;
-	xfer.tx_buf = &config->fretun2[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 6;
-	xfer.tx_buf = &config->dltafre[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->updtclk[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 4;
-	xfer.tx_buf = &config->ramprat[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->control[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 3;
-	xfer.tx_buf = &config->outpskm[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 2;
-	xfer.tx_buf = &config->outpskr[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 3;
-	xfer.tx_buf = &config->daccntl[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-error_ret:
-	mutex_unlock(&st->lock);
-
-	return ret ? ret : len;
-}
-
-static IIO_DEVICE_ATTR(dds, S_IWUSR, NULL, ad9852_set_parameter, 0);
-
-static void ad9852_init(struct ad9852_state *st)
-{
-	struct spi_transfer xfer;
-	int ret;
-	u8 config[5];
-
-	config[0] = addr_contrl;
-	config[1] = COMPPD;
-	config[2] = REFMULT2 | BYPPLL | PLLRANG;
-	config[3] = IEUPCLK;
-	config[4] = OSKEN;
-
-	mutex_lock(&st->lock);
-
-	xfer.len = 5;
-	xfer.tx_buf = &config;
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-error_ret:
-	mutex_unlock(&st->lock);
-
-
-
-}
-
-static struct attribute *ad9852_attributes[] = {
-	&iio_dev_attr_dds.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad9852_attribute_group = {
-	.attrs = ad9852_attributes,
-};
-
-static const struct iio_info ad9852_info = {
-	.attrs = &ad9852_attribute_group,
-	.driver_module = THIS_MODULE,
-};
-
-static int ad9852_probe(struct spi_device *spi)
-{
-	struct ad9852_state *st;
-	struct iio_dev *idev;
-	int ret = 0;
-
-	idev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (!idev)
-		return -ENOMEM;
-	st = iio_priv(idev);
-	spi_set_drvdata(spi, idev);
-	mutex_init(&st->lock);
-	st->sdev = spi;
-
-	idev->dev.parent = &spi->dev;
-	idev->info = &ad9852_info;
-	idev->modes = INDIO_DIRECT_MODE;
-
-	ret = iio_device_register(idev);
-	if (ret)
-		return ret;
-	spi->max_speed_hz = 2000000;
-	spi->mode = SPI_MODE_3;
-	spi->bits_per_word = 8;
-	spi_setup(spi);
-	ad9852_init(st);
-
-	return 0;
-}
-
-static int ad9852_remove(struct spi_device *spi)
-{
-	iio_device_unregister(spi_get_drvdata(spi));
-
-	return 0;
-}
-
-static struct spi_driver ad9852_driver = {
-	.driver = {
-		.name = DRV_NAME,
-		.owner = THIS_MODULE,
-	},
-	.probe = ad9852_probe,
-	.remove = ad9852_remove,
-};
-module_spi_driver(ad9852_driver);
-
-MODULE_AUTHOR("Cliff Cai");
-MODULE_DESCRIPTION("Analog Devices ad9852 driver");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/frequency/ad9910.c b/drivers/staging/iio/frequency/ad9910.c
deleted file mode 100644
index 755e0482681a..000000000000
--- a/drivers/staging/iio/frequency/ad9910.c
+++ /dev/null
@@ -1,371 +0,0 @@
-/*
- * Driver for ADI Direct Digital Synthesis ad9910
- *
- * Copyright (c) 2010 Analog Devices Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- */
-#include <linux/types.h>
-#include <linux/mutex.h>
-#include <linux/device.h>
-#include <linux/spi/spi.h>
-#include <linux/slab.h>
-#include <linux/sysfs.h>
-#include <linux/module.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/sysfs.h>
-
-#define DRV_NAME "ad9910"
-
-#define CFR1 0x0
-#define CFR2 0x1
-#define CFR3 0x2
-
-#define AUXDAC 0x3
-#define IOUPD 0x4
-#define FTW 0x7
-#define POW 0x8
-#define ASF 0x9
-#define MULTC 0x0A
-#define DIG_RAMPL 0x0B
-#define DIG_RAMPS 0x0C
-#define DIG_RAMPR 0x0D
-#define SIN_TONEP0 0x0E
-#define SIN_TONEP1 0x0F
-#define SIN_TONEP2 0x10
-#define SIN_TONEP3 0x11
-#define SIN_TONEP4 0x12
-#define SIN_TONEP5 0x13
-#define SIN_TONEP6 0x14
-#define SIN_TONEP7 0x15
-
-#define RAM_ENABLE	(1 << 7)
-
-#define MANUAL_OSK	(1 << 7)
-#define INVSIC		(1 << 6)
-#define DDS_SINEOP	(1)
-
-#define AUTO_OSK	(1)
-#define OSKEN		(1 << 1)
-#define LOAD_ARR	(1 << 2)
-#define CLR_PHA		(1 << 3)
-#define CLR_DIG		(1 << 4)
-#define ACLR_PHA	(1 << 5)
-#define ACLR_DIG	(1 << 6)
-#define LOAD_LRR	(1 << 7)
-
-#define LSB_FST		(1)
-#define SDIO_IPT	(1 << 1)
-#define EXT_PWD		(1 << 3)
-#define ADAC_PWD	(1 << 4)
-#define REFCLK_PWD	(1 << 5)
-#define DAC_PWD		(1 << 6)
-#define DIG_PWD		(1 << 7)
-
-#define ENA_AMP		(1)
-#define READ_FTW	(1)
-#define DIGR_LOW	(1 << 1)
-#define DIGR_HIGH	(1 << 2)
-#define DIGR_ENA	(1 << 3)
-#define SYNCCLK_ENA	(1 << 6)
-#define ITER_IOUPD	(1 << 7)
-
-#define TX_ENA		(1 << 1)
-#define PDCLK_INV	(1 << 2)
-#define PDCLK_ENB	(1 << 3)
-
-#define PARA_ENA	(1 << 4)
-#define SYNC_DIS	(1 << 5)
-#define DATA_ASS	(1 << 6)
-#define MATCH_ENA	(1 << 7)
-
-#define PLL_ENA		(1)
-#define PFD_RST		(1 << 2)
-#define REFCLK_RST	(1 << 6)
-#define REFCLK_BYP	(1 << 7)
-
-/* Register format: 1 byte addr + value */
-struct ad9910_config {
-	u8 auxdac[5];
-	u8 ioupd[5];
-	u8 ftw[5];
-	u8 pow[3];
-	u8 asf[5];
-	u8 multc[5];
-	u8 dig_rampl[9];
-	u8 dig_ramps[9];
-	u8 dig_rampr[5];
-	u8 sin_tonep0[9];
-	u8 sin_tonep1[9];
-	u8 sin_tonep2[9];
-	u8 sin_tonep3[9];
-	u8 sin_tonep4[9];
-	u8 sin_tonep5[9];
-	u8 sin_tonep6[9];
-	u8 sin_tonep7[9];
-};
-
-struct ad9910_state {
-	struct mutex lock;
-	struct spi_device *sdev;
-};
-
-static ssize_t ad9910_set_parameter(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf,
-					size_t len)
-{
-	struct spi_transfer xfer;
-	int ret;
-	struct ad9910_config *config = (struct ad9910_config *)buf;
-	struct iio_dev *idev = dev_to_iio_dev(dev);
-	struct ad9910_state *st = iio_priv(idev);
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->auxdac[0];
-	mutex_lock(&st->lock);
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->ioupd[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->ftw[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 3;
-	xfer.tx_buf = &config->pow[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->asf[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->multc[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->dig_rampl[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->dig_ramps[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->dig_rampr[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep0[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep1[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep2[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep3[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep4[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep5[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep6[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 9;
-	xfer.tx_buf = &config->sin_tonep7[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-error_ret:
-	mutex_unlock(&st->lock);
-
-	return ret ? ret : len;
-}
-
-static IIO_DEVICE_ATTR(dds, S_IWUSR, NULL, ad9910_set_parameter, 0);
-
-static void ad9910_init(struct ad9910_state *st)
-{
-	struct spi_transfer xfer;
-	int ret;
-	u8 cfr[5];
-
-	cfr[0] = CFR1;
-	cfr[1] = 0;
-	cfr[2] = MANUAL_OSK | INVSIC | DDS_SINEOP;
-	cfr[3] = AUTO_OSK | OSKEN | ACLR_PHA | ACLR_DIG | LOAD_LRR;
-	cfr[4] = 0;
-
-	mutex_lock(&st->lock);
-
-	xfer.len = 5;
-	xfer.tx_buf = &cfr;
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	cfr[0] = CFR2;
-	cfr[1] = ENA_AMP;
-	cfr[2] = READ_FTW | DIGR_ENA | ITER_IOUPD;
-	cfr[3] = TX_ENA | PDCLK_INV | PDCLK_ENB;
-	cfr[4] = PARA_ENA;
-
-	xfer.len = 5;
-	xfer.tx_buf = &cfr;
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	cfr[0] = CFR3;
-	cfr[1] = PLL_ENA;
-	cfr[2] = 0;
-	cfr[3] = REFCLK_RST | REFCLK_BYP;
-	cfr[4] = 0;
-
-	xfer.len = 5;
-	xfer.tx_buf = &cfr;
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-error_ret:
-	mutex_unlock(&st->lock);
-
-
-
-}
-
-static struct attribute *ad9910_attributes[] = {
-	&iio_dev_attr_dds.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad9910_attribute_group = {
-	.attrs = ad9910_attributes,
-};
-
-static const struct iio_info ad9910_info = {
-	.attrs = &ad9910_attribute_group,
-	.driver_module = THIS_MODULE,
-};
-
-static int ad9910_probe(struct spi_device *spi)
-{
-	struct ad9910_state *st;
-	struct iio_dev *idev;
-	int ret = 0;
-
-	idev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (!idev)
-		return -ENOMEM;
-	spi_set_drvdata(spi, idev);
-	st = iio_priv(idev);
-	mutex_init(&st->lock);
-	st->sdev = spi;
-
-	idev->dev.parent = &spi->dev;
-	idev->info = &ad9910_info;
-	idev->modes = INDIO_DIRECT_MODE;
-
-	ret = iio_device_register(idev);
-	if (ret)
-		return ret;
-	spi->max_speed_hz = 2000000;
-	spi->mode = SPI_MODE_3;
-	spi->bits_per_word = 8;
-	spi_setup(spi);
-	ad9910_init(st);
-	return 0;
-}
-
-static int ad9910_remove(struct spi_device *spi)
-{
-	iio_device_unregister(spi_get_drvdata(spi));
-
-	return 0;
-}
-
-static struct spi_driver ad9910_driver = {
-	.driver = {
-		.name = DRV_NAME,
-		.owner = THIS_MODULE,
-	},
-	.probe = ad9910_probe,
-	.remove = ad9910_remove,
-};
-module_spi_driver(ad9910_driver);
-
-MODULE_AUTHOR("Cliff Cai");
-MODULE_DESCRIPTION("Analog Devices ad9910 driver");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/frequency/ad9951.c b/drivers/staging/iio/frequency/ad9951.c
deleted file mode 100644
index 5e8990a0210b..000000000000
--- a/drivers/staging/iio/frequency/ad9951.c
+++ /dev/null
@@ -1,209 +0,0 @@
-/*
- * Driver for ADI Direct Digital Synthesis ad9951
- *
- * Copyright (c) 2010 Analog Devices Inc.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- */
-#include <linux/types.h>
-#include <linux/mutex.h>
-#include <linux/device.h>
-#include <linux/spi/spi.h>
-#include <linux/slab.h>
-#include <linux/sysfs.h>
-#include <linux/module.h>
-
-#include <linux/iio/iio.h>
-#include <linux/iio/sysfs.h>
-
-#define DRV_NAME "ad9951"
-
-#define CFR1 0x0
-#define CFR2 0x1
-
-#define AUTO_OSK	(1)
-#define OSKEN		(1 << 1)
-#define LOAD_ARR	(1 << 2)
-
-#define AUTO_SYNC	(1 << 7)
-
-#define LSB_FST		(1)
-#define SDIO_IPT	(1 << 1)
-#define CLR_PHA		(1 << 2)
-#define SINE_OPT	(1 << 4)
-#define ACLR_PHA	(1 << 5)
-
-#define VCO_RANGE	(1 << 2)
-
-#define CRS_OPT		(1 << 1)
-#define HMANU_SYNC	(1 << 2)
-#define HSPD_SYNC	(1 << 3)
-
-/* Register format: 1 byte addr + value */
-struct ad9951_config {
-	u8 asf[3];
-	u8 arr[2];
-	u8 ftw0[5];
-	u8 ftw1[3];
-};
-
-struct ad9951_state {
-	struct mutex lock;
-	struct spi_device *sdev;
-};
-
-static ssize_t ad9951_set_parameter(struct device *dev,
-					struct device_attribute *attr,
-					const char *buf,
-					size_t len)
-{
-	struct spi_transfer xfer;
-	int ret;
-	struct ad9951_config *config = (struct ad9951_config *)buf;
-	struct iio_dev *idev = dev_to_iio_dev(dev);
-	struct ad9951_state *st = iio_priv(idev);
-
-	xfer.len = 3;
-	xfer.tx_buf = &config->asf[0];
-	mutex_lock(&st->lock);
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 2;
-	xfer.tx_buf = &config->arr[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 5;
-	xfer.tx_buf = &config->ftw0[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	xfer.len = 3;
-	xfer.tx_buf = &config->ftw1[0];
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-error_ret:
-	mutex_unlock(&st->lock);
-
-	return ret ? ret : len;
-}
-
-static IIO_DEVICE_ATTR(dds, S_IWUSR, NULL, ad9951_set_parameter, 0);
-
-static void ad9951_init(struct ad9951_state *st)
-{
-	struct spi_transfer xfer;
-	int ret;
-	u8 cfr[5];
-
-	cfr[0] = CFR1;
-	cfr[1] = 0;
-	cfr[2] = LSB_FST | CLR_PHA | SINE_OPT | ACLR_PHA;
-	cfr[3] = AUTO_OSK | OSKEN | LOAD_ARR;
-	cfr[4] = 0;
-
-	mutex_lock(&st->lock);
-
-	xfer.len = 5;
-	xfer.tx_buf = &cfr;
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-	cfr[0] = CFR2;
-	cfr[1] = VCO_RANGE;
-	cfr[2] = HSPD_SYNC;
-	cfr[3] = 0;
-
-	xfer.len = 4;
-	xfer.tx_buf = &cfr;
-
-	ret = spi_sync_transfer(st->sdev, &xfer, 1);
-	if (ret)
-		goto error_ret;
-
-error_ret:
-	mutex_unlock(&st->lock);
-
-
-
-}
-
-static struct attribute *ad9951_attributes[] = {
-	&iio_dev_attr_dds.dev_attr.attr,
-	NULL,
-};
-
-static const struct attribute_group ad9951_attribute_group = {
-	.attrs = ad9951_attributes,
-};
-
-static const struct iio_info ad9951_info = {
-	.attrs = &ad9951_attribute_group,
-	.driver_module = THIS_MODULE,
-};
-
-static int ad9951_probe(struct spi_device *spi)
-{
-	struct ad9951_state *st;
-	struct iio_dev *idev;
-	int ret = 0;
-
-	idev = devm_iio_device_alloc(&spi->dev, sizeof(*st));
-	if (!idev)
-		return -ENOMEM;
-	spi_set_drvdata(spi, idev);
-	st = iio_priv(idev);
-	mutex_init(&st->lock);
-	st->sdev = spi;
-
-	idev->dev.parent = &spi->dev;
-
-	idev->info = &ad9951_info;
-	idev->modes = INDIO_DIRECT_MODE;
-
-	ret = iio_device_register(idev);
-	if (ret)
-		return ret;
-	spi->max_speed_hz = 2000000;
-	spi->mode = SPI_MODE_3;
-	spi->bits_per_word = 8;
-	spi_setup(spi);
-	ad9951_init(st);
-	return 0;
-}
-
-static int ad9951_remove(struct spi_device *spi)
-{
-	iio_device_unregister(spi_get_drvdata(spi));
-
-	return 0;
-}
-
-static struct spi_driver ad9951_driver = {
-	.driver = {
-		.name = DRV_NAME,
-		.owner = THIS_MODULE,
-	},
-	.probe = ad9951_probe,
-	.remove = ad9951_remove,
-};
-module_spi_driver(ad9951_driver);
-
-MODULE_AUTHOR("Cliff Cai");
-MODULE_DESCRIPTION("Analog Devices ad9951 driver");
-MODULE_LICENSE("GPL v2");
-MODULE_ALIAS("spi:" DRV_NAME);
diff --git a/drivers/staging/iio/frequency/dds.h b/drivers/staging/iio/frequency/dds.h
index c3342f6e052b..611e2b0cfc4c 100644
--- a/drivers/staging/iio/frequency/dds.h
+++ b/drivers/staging/iio/frequency/dds.h
@@ -27,7 +27,7 @@
 
 #define IIO_DEV_ATTR_FREQSYMBOL(_channel, _mode, _show, _store, _addr)	\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_frequencysymbol,	\
-			_mode, _show, _store, _addr);
+			_mode, _show, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_phaseY
@@ -50,7 +50,7 @@
 
 #define IIO_DEV_ATTR_PHASESYMBOL(_channel, _mode, _show, _store, _addr)	\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_phasesymbol,		\
-			_mode, _show, _store, _addr);
+			_mode, _show, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_en
@@ -58,7 +58,7 @@
 
 #define IIO_DEV_ATTR_PINCONTROL_EN(_channel, _mode, _show, _store, _addr)\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_en,	\
-			_mode, _show, _store, _addr);
+			_mode, _show, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_frequency_en
@@ -66,7 +66,7 @@
 
 #define IIO_DEV_ATTR_PINCONTROL_FREQ_EN(_channel, _mode, _show, _store, _addr)\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_frequency_en,\
-			_mode, _show, _store, _addr);
+			_mode, _show, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_pincontrol_phase_en
@@ -74,7 +74,7 @@
 
 #define IIO_DEV_ATTR_PINCONTROL_PHASE_EN(_channel, _mode, _show, _store, _addr)\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_pincontrol_phase_en,	\
-			_mode, _show, _store, _addr);
+			_mode, _show, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_out_enable
@@ -82,7 +82,7 @@
 
 #define IIO_DEV_ATTR_OUT_ENABLE(_channel, _mode, _show, _store, _addr)	\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out_enable,		\
-			_mode, _show, _store, _addr);
+			_mode, _show, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_outY_enable
@@ -91,7 +91,7 @@
 #define IIO_DEV_ATTR_OUTY_ENABLE(_channel, _output,			\
 			_mode, _show, _store, _addr)			\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out##_output##_enable,\
-			_mode, _show, _store, _addr);
+			_mode, _show, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype
@@ -99,7 +99,7 @@
 
 #define IIO_DEV_ATTR_OUT_WAVETYPE(_channel, _output, _store, _addr)	\
 	IIO_DEVICE_ATTR(out_altvoltage##_channel##_out##_output##_wavetype,\
-			S_IWUSR, NULL, _store, _addr);
+			S_IWUSR, NULL, _store, _addr)
 
 /**
  * /sys/bus/iio/devices/.../out_altvoltageX_outY_wavetype_available
@@ -107,4 +107,4 @@
 
 #define IIO_CONST_ATTR_OUT_WAVETYPES_AVAILABLE(_channel, _output, _modes)\
 	IIO_CONST_ATTR(							\
-	out_altvoltage##_channel##_out##_output##_wavetype_available, _modes);
+	out_altvoltage##_channel##_out##_output##_wavetype_available, _modes)
diff --git a/drivers/staging/iio/gyro/adis16060_core.c b/drivers/staging/iio/gyro/adis16060_core.c
index d5d395c2e3e4..4c5869dd8223 100644
--- a/drivers/staging/iio/gyro/adis16060_core.c
+++ b/drivers/staging/iio/gyro/adis16060_core.c
@@ -180,6 +180,7 @@ static int adis16060_w_probe(struct spi_device *spi)
 	int ret;
 	struct iio_dev *indio_dev = adis16060_iio_dev;
 	struct adis16060_state *st;
+
 	if (!indio_dev) {
 		ret =  -ENODEV;
 		goto error_ret;
diff --git a/drivers/staging/iio/iio_dummy_evgen.c b/drivers/staging/iio/iio_dummy_evgen.c
index 132d278c5010..5a804f16ec2f 100644
--- a/drivers/staging/iio/iio_dummy_evgen.c
+++ b/drivers/staging/iio/iio_dummy_evgen.c
@@ -202,6 +202,7 @@ static struct device iio_evgen_dev = {
 static __init int iio_dummy_evgen_init(void)
 {
 	int ret = iio_dummy_evgen_create();
+
 	if (ret < 0)
 		return ret;
 	device_initialize(&iio_evgen_dev);
diff --git a/drivers/staging/iio/iio_dummy_evgen.h b/drivers/staging/iio/iio_dummy_evgen.h
index d8845e2288b3..3a180811b315 100644
--- a/drivers/staging/iio/iio_dummy_evgen.h
+++ b/drivers/staging/iio/iio_dummy_evgen.h
@@ -1,2 +1,7 @@
+#ifndef _IIO_DUMMY_EVGEN_H_
+#define _IIO_DUMMY_EVGEN_H_
+
 int iio_dummy_evgen_get_irq(void);
 int iio_dummy_evgen_release_irq(int irq);
+
+#endif /* _IIO_DUMMY_EVGEN_H_ */
diff --git a/drivers/staging/iio/iio_simple_dummy.h b/drivers/staging/iio/iio_simple_dummy.h
index b126196cdf3d..3027aed79093 100644
--- a/drivers/staging/iio/iio_simple_dummy.h
+++ b/drivers/staging/iio/iio_simple_dummy.h
@@ -8,6 +8,8 @@
  * Join together the various functionality of iio_simple_dummy driver
  */
 
+#ifndef _IIO_SIMPLE_DUMMY_H_
+#define _IIO_SIMPLE_DUMMY_H_
 #include <linux/kernel.h>
 
 struct iio_dummy_accel_calibscale;
@@ -117,4 +119,6 @@ static inline int iio_simple_dummy_configure_buffer(struct iio_dev *indio_dev,
 static inline
 void iio_simple_dummy_unconfigure_buffer(struct iio_dev *indio_dev)
 {};
+
 #endif /* CONFIG_IIO_SIMPLE_DUMMY_BUFFER */
+#endif /* _IIO_SIMPLE_DUMMY_H_ */
diff --git a/drivers/staging/iio/iio_simple_dummy_buffer.c b/drivers/staging/iio/iio_simple_dummy_buffer.c
index 46c134b2a5d1..fd74f9166a5f 100644
--- a/drivers/staging/iio/iio_simple_dummy_buffer.c
+++ b/drivers/staging/iio/iio_simple_dummy_buffer.c
@@ -71,6 +71,7 @@ static irqreturn_t iio_simple_dummy_trigger_h(int irq, void *p)
 		 * in the constant table fakedata.
 		 */
 		int i, j;
+
 		for (i = 0, j = 0;
 		     i < bitmap_weight(indio_dev->active_scan_mask,
 				       indio_dev->masklength);
diff --git a/drivers/staging/iio/iio_simple_dummy_events.c b/drivers/staging/iio/iio_simple_dummy_events.c
index 812ebd05a7fe..64b45b077549 100644
--- a/drivers/staging/iio/iio_simple_dummy_events.c
+++ b/drivers/staging/iio/iio_simple_dummy_events.c
@@ -148,6 +148,7 @@ int iio_simple_dummy_write_event_value(struct iio_dev *indio_dev,
 static irqreturn_t iio_simple_dummy_event_handler(int irq, void *private)
 {
 	struct iio_dev *indio_dev = private;
+
 	iio_push_event(indio_dev,
 		       IIO_EVENT_CODE(IIO_VOLTAGE, 0, 0,
 				      IIO_EV_DIR_RISING,
diff --git a/drivers/staging/iio/impedance-analyzer/ad5933.c b/drivers/staging/iio/impedance-analyzer/ad5933.c
index 2b96665da8a2..d0c89d0457de 100644
--- a/drivers/staging/iio/impedance-analyzer/ad5933.c
+++ b/drivers/staging/iio/impedance-analyzer/ad5933.c
@@ -193,6 +193,7 @@ static int ad5933_cmd(struct ad5933_state *st, unsigned char cmd)
 static int ad5933_reset(struct ad5933_state *st)
 {
 	unsigned char dat = st->ctrl_lb | AD5933_CTRL_RESET;
+
 	return ad5933_i2c_write(st->client,
 			AD5933_REG_CONTROL_LB, 1, &dat);
 }
@@ -220,7 +221,7 @@ static int ad5933_set_freq(struct ad5933_state *st,
 {
 	unsigned long long freqreg;
 	union {
-		u32 d32;
+		__be32 d32;
 		u8 d8[4];
 	} dat;
 
@@ -244,7 +245,7 @@ static int ad5933_set_freq(struct ad5933_state *st,
 
 static int ad5933_setup(struct ad5933_state *st)
 {
-	unsigned short dat;
+	__be16 dat;
 	int ret;
 
 	ret = ad5933_reset(st);
@@ -297,7 +298,7 @@ static ssize_t ad5933_show_frequency(struct device *dev,
 	int ret;
 	unsigned long long freqreg;
 	union {
-		u32 d32;
+		__be32 d32;
 		u8 d8[4];
 	} dat;
 
@@ -402,7 +403,7 @@ static ssize_t ad5933_store(struct device *dev,
 	struct iio_dev_attr *this_attr = to_iio_dev_attr(attr);
 	u16 val;
 	int i, ret = 0;
-	unsigned short dat;
+	__be16 dat;
 
 	if (this_attr->address != AD5933_IN_PGA_GAIN) {
 		ret = kstrtou16(buf, 10, &val);
@@ -521,7 +522,7 @@ static int ad5933_read_raw(struct iio_dev *indio_dev,
 			   long m)
 {
 	struct ad5933_state *st = iio_priv(indio_dev);
-	unsigned short dat;
+	__be16 dat;
 	int ret = -EINVAL;
 
 	mutex_lock(&indio_dev->mlock);
diff --git a/drivers/staging/iio/light/isl29018.c b/drivers/staging/iio/light/isl29018.c
index 3660a43b5f08..a3489187aeb0 100644
--- a/drivers/staging/iio/light/isl29018.c
+++ b/drivers/staging/iio/light/isl29018.c
@@ -1,5 +1,5 @@
 /*
- * A iio driver for the light sensor ISL 29018.
+ * A iio driver for the light sensor ISL 29018/29023/29035.
  *
  * IIO driver for monitoring ambient light intensity in luxi, proximity
  * sensing and infrared sensing.
@@ -30,6 +30,7 @@
 #include <linux/slab.h>
 #include <linux/iio/iio.h>
 #include <linux/iio/sysfs.h>
+#include <linux/acpi.h>
 
 #define CONVERSION_TIME_MS		100
 
@@ -58,10 +59,18 @@
 #define ISL29018_TEST_SHIFT		0
 #define ISL29018_TEST_MASK		(0xFF << ISL29018_TEST_SHIFT)
 
+#define ISL29035_REG_DEVICE_ID		0x0F
+#define ISL29035_DEVICE_ID_SHIFT	0x03
+#define ISL29035_DEVICE_ID_MASK		(0x7 << ISL29035_DEVICE_ID_SHIFT)
+#define ISL29035_DEVICE_ID		0x5
+#define ISL29035_BOUT_SHIFT		0x07
+#define ISL29035_BOUT_MASK		(0x01 << ISL29035_BOUT_SHIFT)
+
 struct isl29018_chip {
 	struct device		*dev;
 	struct regmap		*regmap;
 	struct mutex		lock;
+	int			type;
 	unsigned int		lux_scale;
 	unsigned int		lux_uscale;
 	unsigned int		range;
@@ -407,23 +416,35 @@ static int isl29018_read_raw(struct iio_dev *indio_dev,
 	return ret;
 }
 
+#define ISL29018_LIGHT_CHANNEL {					\
+	.type = IIO_LIGHT,						\
+	.indexed = 1,							\
+	.channel = 0,							\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) |		\
+	BIT(IIO_CHAN_INFO_CALIBSCALE),					\
+}
+
+#define ISL29018_IR_CHANNEL {						\
+	.type = IIO_INTENSITY,						\
+	.modified = 1,							\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+	.channel2 = IIO_MOD_LIGHT_IR,					\
+}
+
+#define ISL29018_PROXIMITY_CHANNEL {					\
+	.type = IIO_PROXIMITY,						\
+	.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),			\
+}
+
 static const struct iio_chan_spec isl29018_channels[] = {
-	{
-		.type = IIO_LIGHT,
-		.indexed = 1,
-		.channel = 0,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_PROCESSED) |
-		BIT(IIO_CHAN_INFO_CALIBSCALE),
-	}, {
-		.type = IIO_INTENSITY,
-		.modified = 1,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
-		.channel2 = IIO_MOD_LIGHT_IR,
-	}, {
-		/* Unindexed in current ABI.  But perhaps it should be. */
-		.type = IIO_PROXIMITY,
-		.info_mask_separate = BIT(IIO_CHAN_INFO_RAW),
-	}
+	ISL29018_LIGHT_CHANNEL,
+	ISL29018_IR_CHANNEL,
+	ISL29018_PROXIMITY_CHANNEL,
+};
+
+static const struct iio_chan_spec isl29023_channels[] = {
+	ISL29018_LIGHT_CHANNEL,
+	ISL29018_IR_CHANNEL,
 };
 
 static IIO_DEVICE_ATTR(range, S_IRUGO | S_IWUSR, show_range, store_range, 0);
@@ -447,16 +468,63 @@ static struct attribute *isl29018_attributes[] = {
 	NULL
 };
 
-static const struct attribute_group isl29108_group = {
+static struct attribute *isl29023_attributes[] = {
+	ISL29018_DEV_ATTR(range),
+	ISL29018_CONST_ATTR(range_available),
+	ISL29018_DEV_ATTR(adc_resolution),
+	ISL29018_CONST_ATTR(adc_resolution_available),
+	NULL
+};
+
+static const struct attribute_group isl29018_group = {
 	.attrs = isl29018_attributes,
 };
 
+static const struct attribute_group isl29023_group = {
+	.attrs = isl29023_attributes,
+};
+
+static int isl29035_detect(struct isl29018_chip *chip)
+{
+	int status;
+	unsigned int id;
+
+	status = regmap_read(chip->regmap, ISL29035_REG_DEVICE_ID, &id);
+	if (status < 0) {
+		dev_err(chip->dev,
+			"Error reading ID register with error %d\n",
+			status);
+		return status;
+	}
+
+	id = (id & ISL29035_DEVICE_ID_MASK) >> ISL29035_DEVICE_ID_SHIFT;
+
+	if (id != ISL29035_DEVICE_ID)
+		return -ENODEV;
+
+	/* clear out brownout bit */
+	return regmap_update_bits(chip->regmap, ISL29035_REG_DEVICE_ID,
+				  ISL29035_BOUT_MASK, 0);
+}
+
+enum {
+	isl29018,
+	isl29023,
+	isl29035,
+};
+
 static int isl29018_chip_init(struct isl29018_chip *chip)
 {
 	int status;
-	int new_adc_bit;
+	unsigned int new_adc_bit;
 	unsigned int new_range;
 
+	if (chip->type == isl29035) {
+		status = isl29035_detect(chip);
+		if (status < 0)
+			return status;
+	}
+
 	/* Code added per Intersil Application Note 1534:
 	 *     When VDD sinks to approximately 1.8V or below, some of
 	 * the part's registers may change their state. When VDD
@@ -479,8 +547,8 @@ static int isl29018_chip_init(struct isl29018_chip *chip)
 	 */
 	status = regmap_write(chip->regmap, ISL29018_REG_TEST, 0x0);
 	if (status < 0) {
-		dev_err(chip->dev, "Failed to clear isl29018 TEST reg."
-					"(%d)\n", status);
+		dev_err(chip->dev, "Failed to clear isl29018 TEST reg.(%d)\n",
+			status);
 		return status;
 	}
 
@@ -490,12 +558,12 @@ static int isl29018_chip_init(struct isl29018_chip *chip)
 	 */
 	status = regmap_write(chip->regmap, ISL29018_REG_ADD_COMMAND1, 0);
 	if (status < 0) {
-		dev_err(chip->dev, "Failed to clear isl29018 CMD1 reg."
-					"(%d)\n", status);
+		dev_err(chip->dev, "Failed to clear isl29018 CMD1 reg.(%d)\n",
+			status);
 		return status;
 	}
 
-	msleep(1);	/* per data sheet, page 10 */
+	usleep_range(1000, 2000);	/* per data sheet, page 10 */
 
 	/* set defaults */
 	status = isl29018_set_range(chip, chip->range, &new_range);
@@ -510,8 +578,15 @@ static int isl29018_chip_init(struct isl29018_chip *chip)
 	return 0;
 }
 
-static const struct iio_info isl29108_info = {
-	.attrs = &isl29108_group,
+static const struct iio_info isl29018_info = {
+	.attrs = &isl29018_group,
+	.driver_module = THIS_MODULE,
+	.read_raw = &isl29018_read_raw,
+	.write_raw = &isl29018_write_raw,
+};
+
+static const struct iio_info isl29023_info = {
+	.attrs = &isl29023_group,
 	.driver_module = THIS_MODULE,
 	.read_raw = &isl29018_read_raw,
 	.write_raw = &isl29018_write_raw,
@@ -524,6 +599,7 @@ static bool is_volatile_reg(struct device *dev, unsigned int reg)
 	case ISL29018_REG_ADD_DATA_MSB:
 	case ISL29018_REG_ADD_COMMAND1:
 	case ISL29018_REG_TEST:
+	case ISL29035_REG_DEVICE_ID:
 		return true;
 	default:
 		return false;
@@ -543,12 +619,66 @@ static const struct regmap_config isl29018_regmap_config = {
 	.cache_type = REGCACHE_RBTREE,
 };
 
+/* isl29035_regmap_config: regmap configuration for ISL29035 */
+static const struct regmap_config isl29035_regmap_config = {
+	.reg_bits = 8,
+	.val_bits = 8,
+	.volatile_reg = is_volatile_reg,
+	.max_register = ISL29035_REG_DEVICE_ID,
+	.num_reg_defaults_raw = ISL29035_REG_DEVICE_ID + 1,
+	.cache_type = REGCACHE_RBTREE,
+};
+
+struct chip_info {
+	const struct iio_chan_spec *channels;
+	int num_channels;
+	const struct iio_info *indio_info;
+	const struct regmap_config *regmap_cfg;
+};
+
+static const struct chip_info chip_info_tbl[] = {
+	[isl29018] = {
+		.channels = isl29018_channels,
+		.num_channels = ARRAY_SIZE(isl29018_channels),
+		.indio_info = &isl29018_info,
+		.regmap_cfg = &isl29018_regmap_config,
+	},
+	[isl29023] = {
+		.channels = isl29023_channels,
+		.num_channels = ARRAY_SIZE(isl29023_channels),
+		.indio_info = &isl29023_info,
+		.regmap_cfg = &isl29018_regmap_config,
+	},
+	[isl29035] = {
+		.channels = isl29023_channels,
+		.num_channels = ARRAY_SIZE(isl29023_channels),
+		.indio_info = &isl29023_info,
+		.regmap_cfg = &isl29035_regmap_config,
+	},
+};
+
+static const char *isl29018_match_acpi_device(struct device *dev, int *data)
+{
+	const struct acpi_device_id *id;
+
+	id = acpi_match_device(dev->driver->acpi_match_table, dev);
+
+	if (!id)
+		return NULL;
+
+	*data = (int) id->driver_data;
+
+	return dev_name(dev);
+}
+
 static int isl29018_probe(struct i2c_client *client,
 			 const struct i2c_device_id *id)
 {
 	struct isl29018_chip *chip;
 	struct iio_dev *indio_dev;
 	int err;
+	const char *name = NULL;
+	int dev_id = 0;
 
 	indio_dev = devm_iio_device_alloc(&client->dev, sizeof(*chip));
 	if (indio_dev == NULL) {
@@ -560,15 +690,25 @@ static int isl29018_probe(struct i2c_client *client,
 	i2c_set_clientdata(client, indio_dev);
 	chip->dev = &client->dev;
 
+	if (id) {
+		name = id->name;
+		dev_id = id->driver_data;
+	}
+
+	if (ACPI_HANDLE(&client->dev))
+		name = isl29018_match_acpi_device(&client->dev, &dev_id);
+
 	mutex_init(&chip->lock);
 
+	chip->type = dev_id;
 	chip->lux_scale = 1;
 	chip->lux_uscale = 0;
 	chip->range = 1000;
 	chip->adc_bit = 16;
 	chip->suspended = false;
 
-	chip->regmap = devm_regmap_init_i2c(client, &isl29018_regmap_config);
+	chip->regmap = devm_regmap_init_i2c(client,
+				chip_info_tbl[dev_id].regmap_cfg);
 	if (IS_ERR(chip->regmap)) {
 		err = PTR_ERR(chip->regmap);
 		dev_err(chip->dev, "regmap initialization failed: %d\n", err);
@@ -579,10 +719,10 @@ static int isl29018_probe(struct i2c_client *client,
 	if (err)
 		return err;
 
-	indio_dev->info = &isl29108_info;
-	indio_dev->channels = isl29018_channels;
-	indio_dev->num_channels = ARRAY_SIZE(isl29018_channels);
-	indio_dev->name = id->name;
+	indio_dev->info = chip_info_tbl[dev_id].indio_info;
+	indio_dev->channels = chip_info_tbl[dev_id].channels;
+	indio_dev->num_channels = chip_info_tbl[dev_id].num_channels;
+	indio_dev->name = name;
 	indio_dev->dev.parent = &client->dev;
 	indio_dev->modes = INDIO_DIRECT_MODE;
 	err = devm_iio_device_register(&client->dev, indio_dev);
@@ -632,8 +772,18 @@ static SIMPLE_DEV_PM_OPS(isl29018_pm_ops, isl29018_suspend, isl29018_resume);
 #define ISL29018_PM_OPS NULL
 #endif
 
+static const struct acpi_device_id isl29018_acpi_match[] = {
+	{"ISL29018", isl29018},
+	{"ISL29023", isl29023},
+	{"ISL29035", isl29035},
+	{},
+};
+MODULE_DEVICE_TABLE(acpi, isl29018_acpi_match);
+
 static const struct i2c_device_id isl29018_id[] = {
-	{"isl29018", 0},
+	{"isl29018", isl29018},
+	{"isl29023", isl29023},
+	{"isl29035", isl29035},
 	{}
 };
 
@@ -641,6 +791,8 @@ MODULE_DEVICE_TABLE(i2c, isl29018_id);
 
 static const struct of_device_id isl29018_of_match[] = {
 	{ .compatible = "isil,isl29018", },
+	{ .compatible = "isil,isl29023", },
+	{ .compatible = "isil,isl29035", },
 	{ },
 };
 MODULE_DEVICE_TABLE(of, isl29018_of_match);
@@ -649,6 +801,7 @@ static struct i2c_driver isl29018_driver = {
 	.class	= I2C_CLASS_HWMON,
 	.driver	 = {
 			.name = "isl29018",
+			.acpi_match_table = ACPI_PTR(isl29018_acpi_match),
 			.pm = ISL29018_PM_OPS,
 			.owner = THIS_MODULE,
 			.of_match_table = isl29018_of_match,
diff --git a/drivers/staging/iio/light/tsl2583.c b/drivers/staging/iio/light/tsl2583.c
index fa9649879662..cc4ddcce4ff9 100644
--- a/drivers/staging/iio/light/tsl2583.c
+++ b/drivers/staging/iio/light/tsl2583.c
@@ -230,6 +230,7 @@ static int taos_get_lux(struct iio_dev *indio_dev)
 
 	for (i = 0; i < 4; i++) {
 		int reg = TSL258X_CMD_REG | (TSL258X_ALS_CHAN0LO + i);
+
 		ret = taos_i2c_read(chip->client, reg, &buf[i], 1);
 		if (ret < 0) {
 			dev_err(&chip->client->dev,
diff --git a/drivers/staging/iio/light/tsl2x7x.h b/drivers/staging/iio/light/tsl2x7x.h
index c4acf5ff1794..ecae92211216 100644
--- a/drivers/staging/iio/light/tsl2x7x.h
+++ b/drivers/staging/iio/light/tsl2x7x.h
@@ -91,8 +91,8 @@ struct tsl2x7x_settings {
  */
 struct tsl2X7X_platform_data {
 	int (*platform_power)(struct device *dev, pm_message_t);
-	int (*power_on)      (struct iio_dev *indio_dev);
-	int (*power_off)     (struct i2c_client *dev);
+	int (*power_on)(struct iio_dev *indio_dev);
+	int (*power_off)(struct i2c_client *dev);
 	struct tsl2x7x_lux platform_lux_table[TSL2X7X_MAX_LUX_TABLE_SIZE];
 	struct tsl2x7x_settings *platform_default_settings;
 };
diff --git a/drivers/staging/iio/light/tsl2x7x_core.c b/drivers/staging/iio/light/tsl2x7x_core.c
index ab338e3ddd05..e0d88fa2a5b5 100644
--- a/drivers/staging/iio/light/tsl2x7x_core.c
+++ b/drivers/staging/iio/light/tsl2x7x_core.c
@@ -1964,6 +1964,7 @@ static int tsl2x7x_suspend(struct device *dev)
 
 	if (chip->pdata && chip->pdata->platform_power) {
 		pm_message_t pmm = {PM_EVENT_SUSPEND};
+
 		chip->pdata->platform_power(dev, pmm);
 	}
 
@@ -1978,6 +1979,7 @@ static int tsl2x7x_resume(struct device *dev)
 
 	if (chip->pdata && chip->pdata->platform_power) {
 		pm_message_t pmm = {PM_EVENT_RESUME};
+
 		chip->pdata->platform_power(dev, pmm);
 	}
 
diff --git a/drivers/staging/iio/magnetometer/hmc5843_core.c b/drivers/staging/iio/magnetometer/hmc5843_core.c
index 914ae1acd31d..fd171d8b38fb 100644
--- a/drivers/staging/iio/magnetometer/hmc5843_core.c
+++ b/drivers/staging/iio/magnetometer/hmc5843_core.c
@@ -131,7 +131,7 @@ static s32 hmc5843_set_mode(struct hmc5843_data *data, u8 operating_mode)
 static int hmc5843_wait_measurement(struct hmc5843_data *data)
 {
 	int tries = 150;
-	int val;
+	unsigned int val;
 	int ret;
 
 	while (tries-- > 0) {
@@ -209,7 +209,7 @@ static ssize_t hmc5843_show_measurement_configuration(struct device *dev,
 						char *buf)
 {
 	struct hmc5843_data *data = iio_priv(dev_to_iio_dev(dev));
-	int val;
+	unsigned int val;
 	int ret;
 
 	ret = regmap_read(data->regmap, HMC5843_CONFIG_REG_A, &val);
@@ -344,7 +344,7 @@ static int hmc5843_read_raw(struct iio_dev *indio_dev,
 			    int *val, int *val2, long mask)
 {
 	struct hmc5843_data *data = iio_priv(indio_dev);
-	int rval;
+	unsigned int rval;
 	int ret;
 
 	switch (mask) {
diff --git a/drivers/staging/iio/meter/ade7753.c b/drivers/staging/iio/meter/ade7753.c
index 00492cad7c57..78e8f560eeec 100644
--- a/drivers/staging/iio/meter/ade7753.c
+++ b/drivers/staging/iio/meter/ade7753.c
@@ -322,6 +322,7 @@ static int ade7753_set_irq(struct device *dev, bool enable)
 {
 	int ret;
 	u8 irqen;
+
 	ret = ade7753_spi_read_reg_8(dev, ADE7753_IRQEN, &irqen);
 	if (ret)
 		goto error_ret;
@@ -377,9 +378,10 @@ static ssize_t ade7753_read_frequency(struct device *dev,
 		struct device_attribute *attr,
 		char *buf)
 {
-	int ret, len = 0;
+	int ret;
 	u16 t;
 	int sps;
+
 	ret = ade7753_spi_read_reg_16(dev, ADE7753_MODE, &t);
 	if (ret)
 		return ret;
@@ -387,8 +389,7 @@ static ssize_t ade7753_read_frequency(struct device *dev,
 	t = (t >> 11) & 0x3;
 	sps = 27900 / (1 + t);
 
-	len = sprintf(buf, "%d\n", sps);
-	return len;
+	return sprintf(buf, "%d\n", sps);
 }
 
 static ssize_t ade7753_write_frequency(struct device *dev,
diff --git a/drivers/staging/iio/meter/ade7754.c b/drivers/staging/iio/meter/ade7754.c
index e0aa13ab3657..81f67318974a 100644
--- a/drivers/staging/iio/meter/ade7754.c
+++ b/drivers/staging/iio/meter/ade7754.c
@@ -340,6 +340,7 @@ static int ade7754_set_irq(struct device *dev, bool enable)
 {
 	int ret;
 	u16 irqen;
+
 	ret = ade7754_spi_read_reg_16(dev, ADE7754_IRQEN, &irqen);
 	if (ret)
 		goto error_ret;
@@ -399,6 +400,7 @@ static ssize_t ade7754_read_frequency(struct device *dev,
 	int ret;
 	u8 t;
 	int sps;
+
 	ret = ade7754_spi_read_reg_8(dev,
 			ADE7754_WAVMODE,
 			&t);
diff --git a/drivers/staging/iio/meter/ade7758_core.c b/drivers/staging/iio/meter/ade7758_core.c
index cba183e24838..abc60067cd72 100644
--- a/drivers/staging/iio/meter/ade7758_core.c
+++ b/drivers/staging/iio/meter/ade7758_core.c
@@ -302,6 +302,7 @@ static int ade7758_reset(struct device *dev)
 {
 	int ret;
 	u8 val;
+
 	ade7758_spi_read_reg_8(dev,
 			ADE7758_OPMODE,
 			&val);
@@ -418,6 +419,7 @@ int ade7758_set_irq(struct device *dev, bool enable)
 {
 	int ret;
 	u32 irqen;
+
 	ret = ade7758_spi_read_reg_24(dev, ADE7758_MASK, &irqen);
 	if (ret)
 		goto error_ret;
@@ -441,6 +443,7 @@ static int ade7758_stop_device(struct device *dev)
 {
 	int ret;
 	u8 val;
+
 	ade7758_spi_read_reg_8(dev,
 			ADE7758_OPMODE,
 			&val);
@@ -483,6 +486,7 @@ static ssize_t ade7758_read_frequency(struct device *dev,
 	int ret, len = 0;
 	u8 t;
 	int sps;
+
 	ret = ade7758_spi_read_reg_8(dev,
 			ADE7758_WAVMODE,
 			&t);
diff --git a/drivers/staging/iio/meter/ade7758_trigger.c b/drivers/staging/iio/meter/ade7758_trigger.c
index ea01b8f7a2c3..6f45ce0478d7 100644
--- a/drivers/staging/iio/meter/ade7758_trigger.c
+++ b/drivers/staging/iio/meter/ade7758_trigger.c
@@ -85,7 +85,7 @@ int ade7758_probe_trigger(struct iio_dev *indio_dev)
 	ret = iio_trigger_register(st->trig);
 
 	/* select default trigger */
-	indio_dev->trig = st->trig;
+	indio_dev->trig = iio_trigger_get(st->trig);
 	if (ret)
 		goto error_free_irq;
 
diff --git a/drivers/staging/iio/meter/ade7759.c b/drivers/staging/iio/meter/ade7759.c
index ea0c9debf8bf..7d217430616a 100644
--- a/drivers/staging/iio/meter/ade7759.c
+++ b/drivers/staging/iio/meter/ade7759.c
@@ -217,6 +217,7 @@ static int ade7759_reset(struct device *dev)
 {
 	int ret;
 	u16 val;
+
 	ade7759_spi_read_reg_16(dev,
 			ADE7759_MODE,
 			&val);
@@ -280,6 +281,7 @@ static int ade7759_set_irq(struct device *dev, bool enable)
 {
 	int ret;
 	u8 irqen;
+
 	ret = ade7759_spi_read_reg_8(dev, ADE7759_IRQEN, &irqen);
 	if (ret)
 		goto error_ret;
@@ -340,6 +342,7 @@ static ssize_t ade7759_read_frequency(struct device *dev,
 	int ret;
 	u16 t;
 	int sps;
+
 	ret = ade7759_spi_read_reg_16(dev,
 			ADE7759_MODE,
 			&t);
diff --git a/drivers/staging/iio/meter/ade7854.h b/drivers/staging/iio/meter/ade7854.h
index 06534577f6c3..52ca5412a18d 100644
--- a/drivers/staging/iio/meter/ade7854.h
+++ b/drivers/staging/iio/meter/ade7854.h
@@ -153,14 +153,14 @@
 struct ade7854_state {
 	struct spi_device	*spi;
 	struct i2c_client	*i2c;
-	int			(*read_reg_8) (struct device *, u16, u8 *);
-	int			(*read_reg_16) (struct device *, u16, u16 *);
-	int			(*read_reg_24) (struct device *, u16, u32 *);
-	int			(*read_reg_32) (struct device *, u16, u32 *);
-	int			(*write_reg_8) (struct device *, u16, u8);
-	int			(*write_reg_16) (struct device *, u16, u16);
-	int			(*write_reg_24) (struct device *, u16, u32);
-	int			(*write_reg_32) (struct device *, u16, u32);
+	int			(*read_reg_8)(struct device *, u16, u8 *);
+	int			(*read_reg_16)(struct device *, u16, u16 *);
+	int			(*read_reg_24)(struct device *, u16, u32 *);
+	int			(*read_reg_32)(struct device *, u16, u32 *);
+	int			(*write_reg_8)(struct device *, u16, u8);
+	int			(*write_reg_16)(struct device *, u16, u16);
+	int			(*write_reg_24)(struct device *, u16, u32);
+	int			(*write_reg_32)(struct device *, u16, u32);
 	int			irq;
 	struct mutex		buf_lock;
 	u8			tx[ADE7854_MAX_TX] ____cacheline_aligned;
diff --git a/drivers/staging/iio/meter/meter.h b/drivers/staging/iio/meter/meter.h
index 23e1b5f480a9..8f0de02839b7 100644
--- a/drivers/staging/iio/meter/meter.h
+++ b/drivers/staging/iio/meter/meter.h
@@ -20,61 +20,61 @@
 #define IIO_DEV_ATTR_VOLT_C_OFFSET(_mode, _show, _store, _addr)      \
 	IIO_DEVICE_ATTR(volt_c_offset, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_REACTIVE_POWER_A_OFFSET(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_REACTIVE_POWER_A_OFFSET(_mode, _show, _store, _addr)   \
 	IIO_DEVICE_ATTR(reactive_power_a_offset, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_REACTIVE_POWER_B_OFFSET(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_REACTIVE_POWER_B_OFFSET(_mode, _show, _store, _addr)   \
 	IIO_DEVICE_ATTR(reactive_power_b_offset, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_REACTIVE_POWER_C_OFFSET(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_REACTIVE_POWER_C_OFFSET(_mode, _show, _store, _addr)   \
 	IIO_DEVICE_ATTR(reactive_power_c_offset, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_ACTIVE_POWER_A_OFFSET(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_ACTIVE_POWER_A_OFFSET(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(active_power_a_offset, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_ACTIVE_POWER_B_OFFSET(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_ACTIVE_POWER_B_OFFSET(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(active_power_b_offset, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_ACTIVE_POWER_C_OFFSET(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_ACTIVE_POWER_C_OFFSET(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(active_power_c_offset, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_CURRENT_A_GAIN(_mode, _show, _store, _addr)		\
+#define IIO_DEV_ATTR_CURRENT_A_GAIN(_mode, _show, _store, _addr)	\
 	IIO_DEVICE_ATTR(current_a_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_CURRENT_B_GAIN(_mode, _show, _store, _addr)		\
+#define IIO_DEV_ATTR_CURRENT_B_GAIN(_mode, _show, _store, _addr)	\
 	IIO_DEVICE_ATTR(current_b_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_CURRENT_C_GAIN(_mode, _show, _store, _addr)		\
+#define IIO_DEV_ATTR_CURRENT_C_GAIN(_mode, _show, _store, _addr)	\
 	IIO_DEVICE_ATTR(current_c_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_APPARENT_POWER_A_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_APPARENT_POWER_A_GAIN(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(apparent_power_a_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_APPARENT_POWER_B_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_APPARENT_POWER_B_GAIN(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(apparent_power_b_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_APPARENT_POWER_C_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_APPARENT_POWER_C_GAIN(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(apparent_power_c_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_ACTIVE_POWER_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_ACTIVE_POWER_GAIN(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(active_power_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_ACTIVE_POWER_A_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_ACTIVE_POWER_A_GAIN(_mode, _show, _store, _addr)   \
 	IIO_DEVICE_ATTR(active_power_a_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_ACTIVE_POWER_B_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_ACTIVE_POWER_B_GAIN(_mode, _show, _store, _addr)   \
 	IIO_DEVICE_ATTR(active_power_b_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_ACTIVE_POWER_C_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_ACTIVE_POWER_C_GAIN(_mode, _show, _store, _addr)   \
 	IIO_DEVICE_ATTR(active_power_c_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_REACTIVE_POWER_A_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_REACTIVE_POWER_A_GAIN(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(reactive_power_a_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_REACTIVE_POWER_B_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_REACTIVE_POWER_B_GAIN(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(reactive_power_b_gain, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_REACTIVE_POWER_C_GAIN(_mode, _show, _store, _addr)                \
+#define IIO_DEV_ATTR_REACTIVE_POWER_C_GAIN(_mode, _show, _store, _addr)     \
 	IIO_DEVICE_ATTR(reactive_power_c_gain, _mode, _show, _store, _addr)
 
 #define IIO_DEV_ATTR_CURRENT_A(_show, _addr)			\
@@ -332,20 +332,20 @@
 #define IIO_DEV_ATTR_IPEAK(_mode, _show, _store, _addr)			\
 	IIO_DEVICE_ATTR(ipeak, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_RIPEAK(_mode, _show, _store, _addr)			\
+#define IIO_DEV_ATTR_RIPEAK(_mode, _show, _store, _addr)		\
 	IIO_DEVICE_ATTR(ripeak, _mode, _show, _store, _addr)
 
 #define IIO_DEV_ATTR_VPEAK(_mode, _show, _store, _addr)			\
 	IIO_DEVICE_ATTR(vpeak, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_RVPEAK(_mode, _show, _store, _addr)			\
+#define IIO_DEV_ATTR_RVPEAK(_mode, _show, _store, _addr)		\
 	IIO_DEVICE_ATTR(rvpeak, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_VPERIOD(_mode, _show, _store, _addr)			\
+#define IIO_DEV_ATTR_VPERIOD(_mode, _show, _store, _addr)		\
 	IIO_DEVICE_ATTR(vperiod, _mode, _show, _store, _addr)
 
-#define IIO_DEV_ATTR_CH_OFF(_num, _mode, _show, _store, _addr)			\
-  IIO_DEVICE_ATTR(choff_##_num, _mode, _show, _store, _addr)
+#define IIO_DEV_ATTR_CH_OFF(_num, _mode, _show, _store, _addr)		\
+	IIO_DEVICE_ATTR(choff_##_num, _mode, _show, _store, _addr)
 
 /* active energy register, AENERGY, is more than half full */
 #define IIO_EVENT_ATTR_AENERGY_HALF_FULL(_evlist, _show, _store, _mask) \
diff --git a/drivers/staging/iio/resolver/Kconfig b/drivers/staging/iio/resolver/Kconfig
index ce360f163216..c7a742ec1227 100644
--- a/drivers/staging/iio/resolver/Kconfig
+++ b/drivers/staging/iio/resolver/Kconfig
@@ -10,6 +10,9 @@ config AD2S90
 	  Say yes here to build support for Analog Devices spi resolver
 	  to digital converters, ad2s90, provides direct access via sysfs.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad2s90.
+
 config AD2S1200
 	tristate "Analog Devices ad2s1200/ad2s1205 driver"
 	depends on SPI
@@ -19,6 +22,9 @@ config AD2S1200
 	  to digital converters, ad2s1200 and ad2s1205, provides direct access
 	  via sysfs.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad2s1200.
+
 config AD2S1210
 	tristate "Analog Devices ad2s1210 driver"
 	depends on SPI
@@ -27,4 +33,7 @@ config AD2S1210
 	  Say yes here to build support for Analog Devices spi resolver
 	  to digital converters, ad2s1210, provides direct access via sysfs.
 
+	  To compile this driver as a module, choose M here: the
+	  module will be called ad2s1210.
+
 endmenu
diff --git a/drivers/staging/iio/resolver/ad2s1210.c b/drivers/staging/iio/resolver/ad2s1210.c
index 7fbaba41c872..b4c14ba5fdee 100644
--- a/drivers/staging/iio/resolver/ad2s1210.c
+++ b/drivers/staging/iio/resolver/ad2s1210.c
@@ -197,6 +197,7 @@ static ssize_t ad2s1210_show_fclkin(struct device *dev,
 				    char *buf)
 {
 	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+
 	return sprintf(buf, "%d\n", st->fclkin);
 }
 
@@ -235,6 +236,7 @@ static ssize_t ad2s1210_show_fexcit(struct device *dev,
 				    char *buf)
 {
 	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+
 	return sprintf(buf, "%d\n", st->fexcit);
 }
 
@@ -271,6 +273,7 @@ static ssize_t ad2s1210_show_control(struct device *dev,
 {
 	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
 	int ret;
+
 	mutex_lock(&st->lock);
 	ret = ad2s1210_config_read(st, AD2S1210_REG_CONTROL);
 	mutex_unlock(&st->lock);
@@ -328,6 +331,7 @@ static ssize_t ad2s1210_show_resolution(struct device *dev,
 			struct device_attribute *attr, char *buf)
 {
 	struct ad2s1210_state *st = iio_priv(dev_to_iio_dev(dev));
+
 	return sprintf(buf, "%d\n", st->resolution);
 }
 
@@ -491,7 +495,7 @@ static int ad2s1210_read_raw(struct iio_dev *indio_dev,
 
 	switch (chan->type) {
 	case IIO_ANGL:
-		pos = be16_to_cpup((u16 *)st->rx);
+		pos = be16_to_cpup((__be16 *) st->rx);
 		if (st->hysteresis)
 			pos >>= 16 - st->resolution;
 		*val = pos;
@@ -499,7 +503,7 @@ static int ad2s1210_read_raw(struct iio_dev *indio_dev,
 		break;
 	case IIO_ANGL_VEL:
 		negative = st->rx[0] & 0x80;
-		vel = be16_to_cpup((s16 *)st->rx);
+		vel = be16_to_cpup((__be16 *) st->rx);
 		vel >>= 16 - st->resolution;
 		if (vel & 0x8000) {
 			negative = (0xffff >> st->resolution) << st->resolution;
diff --git a/drivers/staging/iio/ring_hw.h b/drivers/staging/iio/ring_hw.h
index 39c14a715868..75bf47bfee78 100644
--- a/drivers/staging/iio/ring_hw.h
+++ b/drivers/staging/iio/ring_hw.h
@@ -9,6 +9,9 @@
  *
  */
 
+#ifndef _RING_HW_H_
+#define _RING_HW_H_
+
 /**
  * struct iio_hw_ring_buffer- hardware ring buffer
  * @buf:	generic ring buffer elements
@@ -20,3 +23,5 @@ struct iio_hw_buffer {
 };
 
 #define iio_to_hw_buf(r) container_of(r, struct iio_hw_buffer, buf)
+
+#endif /* _RING_HW_H_ */
diff --git a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
index b1aeb88273c9..8f0a2ffa7150 100644
--- a/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
+++ b/drivers/staging/iio/trigger/iio-trig-periodic-rtc.c
@@ -33,6 +33,7 @@ static int iio_trig_periodic_rtc_set_state(struct iio_trigger *trig, bool state)
 {
 	struct iio_prtc_trigger_info *trig_info = iio_trigger_get_drvdata(trig);
 	int ret;
+
 	if (trig_info->frequency == 0 && state)
 		return -EINVAL;
 	dev_dbg(&trig_info->rtc->dev, "trigger frequency is %d\n",
@@ -50,6 +51,7 @@ static ssize_t iio_trig_periodic_read_freq(struct device *dev,
 {
 	struct iio_trigger *trig = to_iio_trigger(dev);
 	struct iio_prtc_trigger_info *trig_info = iio_trigger_get_drvdata(trig);
+
 	return sprintf(buf, "%u\n", trig_info->frequency);
 }
 
@@ -183,6 +185,7 @@ static int iio_trig_periodic_rtc_remove(struct platform_device *dev)
 {
 	struct iio_trigger *trig, *trig2;
 	struct iio_prtc_trigger_info *trig_info;
+
 	mutex_lock(&iio_prtc_trigger_list_lock);
 	list_for_each_entry_safe(trig,
 				 trig2,
diff --git a/drivers/staging/imx-drm/imx-drm-core.c b/drivers/staging/imx-drm/imx-drm-core.c
index 6b22106534d8..9cb222e2996f 100644
--- a/drivers/staging/imx-drm/imx-drm-core.c
+++ b/drivers/staging/imx-drm/imx-drm-core.c
@@ -87,6 +87,8 @@ static int imx_drm_driver_unload(struct drm_device *drm)
 	drm_vblank_cleanup(drm);
 	drm_mode_config_cleanup(drm);
 
+	platform_set_drvdata(drm->platformdev, NULL);
+
 	return 0;
 }
 
@@ -427,6 +429,7 @@ static uint32_t imx_drm_find_crtc_mask(struct imx_drm_device *imxdrm,
 
 	for (i = 0; i < MAX_CRTC; i++) {
 		struct imx_drm_crtc *imx_drm_crtc = imxdrm->crtc[i];
+
 		if (imx_drm_crtc && imx_drm_crtc->port == port)
 			return drm_crtc_mask(imx_drm_crtc->crtc);
 	}
@@ -438,6 +441,7 @@ static struct device_node *imx_drm_of_get_next_endpoint(
 		const struct device_node *parent, struct device_node *prev)
 {
 	struct device_node *node = of_graph_get_next_endpoint(parent, prev);
+
 	of_node_put(prev);
 	return node;
 }
@@ -471,8 +475,7 @@ int imx_drm_encoder_parse_of(struct drm_device *drm,
 		crtc_mask |= mask;
 	}
 
-	if (ep)
-		of_node_put(ep);
+	of_node_put(ep);
 	if (i == 0)
 		return -ENOENT;
 
@@ -528,6 +531,7 @@ static struct drm_driver imx_drm_driver = {
 	.unload			= imx_drm_driver_unload,
 	.lastclose		= imx_drm_driver_lastclose,
 	.preclose		= imx_drm_driver_preclose,
+	.set_busid		= drm_platform_set_busid,
 	.gem_free_object	= drm_gem_cma_free_object,
 	.gem_vm_ops		= &drm_gem_cma_vm_ops,
 	.dumb_create		= drm_gem_cma_dumb_create,
@@ -647,6 +651,36 @@ static int imx_drm_platform_remove(struct platform_device *pdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int imx_drm_suspend(struct device *dev)
+{
+	struct drm_device *drm_dev = dev_get_drvdata(dev);
+
+	/* The drm_dev is NULL before .load hook is called */
+	if (drm_dev == NULL)
+		return 0;
+
+	drm_kms_helper_poll_disable(drm_dev);
+
+	return 0;
+}
+
+static int imx_drm_resume(struct device *dev)
+{
+	struct drm_device *drm_dev = dev_get_drvdata(dev);
+
+	if (drm_dev == NULL)
+		return 0;
+
+	drm_helper_resume_force_mode(drm_dev);
+	drm_kms_helper_poll_enable(drm_dev);
+
+	return 0;
+}
+#endif
+
+static SIMPLE_DEV_PM_OPS(imx_drm_pm_ops, imx_drm_suspend, imx_drm_resume);
+
 static const struct of_device_id imx_drm_dt_ids[] = {
 	{ .compatible = "fsl,imx-display-subsystem", },
 	{ /* sentinel */ },
@@ -659,6 +693,7 @@ static struct platform_driver imx_drm_pdrv = {
 	.driver		= {
 		.owner	= THIS_MODULE,
 		.name	= "imx-drm",
+		.pm	= &imx_drm_pm_ops,
 		.of_match_table = imx_drm_dt_ids,
 	},
 };
diff --git a/drivers/staging/imx-drm/imx-hdmi.c b/drivers/staging/imx-drm/imx-hdmi.c
index 18c9ccd460b7..aaec6b2cdf56 100644
--- a/drivers/staging/imx-drm/imx-hdmi.c
+++ b/drivers/staging/imx-drm/imx-hdmi.c
@@ -323,8 +323,7 @@ static unsigned int hdmi_compute_cts(unsigned int freq, unsigned long pixel_clk,
 	}
 	if (ratio == 100)
 		return cts;
-	else
-		return (cts * ratio) / 100;
+	return (cts * ratio) / 100;
 }
 
 static void hdmi_set_clk_regenerator(struct imx_hdmi *hdmi,
diff --git a/drivers/staging/imx-drm/imx-ldb.c b/drivers/staging/imx-drm/imx-ldb.c
index 7e3f019d7e72..4662e00b456a 100644
--- a/drivers/staging/imx-drm/imx-ldb.c
+++ b/drivers/staging/imx-drm/imx-ldb.c
@@ -574,6 +574,9 @@ static void imx_ldb_unbind(struct device *dev, struct device *master,
 	for (i = 0; i < 2; i++) {
 		struct imx_ldb_channel *channel = &imx_ldb->channel[i];
 
+		if (!channel->connector.funcs)
+			continue;
+
 		channel->connector.funcs->destroy(&channel->connector);
 		channel->encoder.funcs->destroy(&channel->encoder);
 	}
diff --git a/drivers/staging/imx-drm/imx-tve.c b/drivers/staging/imx-drm/imx-tve.c
index c628fcdc22ae..42c651be6c20 100644
--- a/drivers/staging/imx-drm/imx-tve.c
+++ b/drivers/staging/imx-drm/imx-tve.c
@@ -133,6 +133,7 @@ static void tve_lock(void *__tve)
 __acquires(&tve->lock)
 {
 	struct imx_tve *tve = __tve;
+
 	spin_lock(&tve->lock);
 }
 
@@ -140,6 +141,7 @@ static void tve_unlock(void *__tve)
 __releases(&tve->lock)
 {
 	struct imx_tve *tve = __tve;
+
 	spin_unlock(&tve->lock);
 }
 
@@ -430,8 +432,7 @@ static long clk_tve_di_round_rate(struct clk_hw *hw, unsigned long rate,
 		return *prate / 4;
 	else if (div >= 2)
 		return *prate / 2;
-	else
-		return *prate;
+	return *prate;
 }
 
 static int clk_tve_di_set_rate(struct clk_hw *hw, unsigned long rate,
@@ -536,7 +537,7 @@ static struct regmap_config tve_regmap_config = {
 	.max_register = 0xdc,
 };
 
-static const char *imx_tve_modes[] = {
+static const char * const imx_tve_modes[] = {
 	[TVE_MODE_TVOUT]  = "tvout",
 	[TVE_MODE_VGA] = "vga",
 };
diff --git a/drivers/staging/imx-drm/ipuv3-crtc.c b/drivers/staging/imx-drm/ipuv3-crtc.c
index 720868bff35b..11e84a251773 100644
--- a/drivers/staging/imx-drm/ipuv3-crtc.c
+++ b/drivers/staging/imx-drm/ipuv3-crtc.c
@@ -201,7 +201,8 @@ static int ipu_crtc_mode_set(struct drm_crtc *crtc,
 		return ret;
 	}
 
-	return ipu_plane_mode_set(ipu_crtc->plane[0], crtc, mode, crtc->primary->fb,
+	return ipu_plane_mode_set(ipu_crtc->plane[0], crtc, mode,
+				  crtc->primary->fb,
 				  0, 0, mode->hdisplay, mode->vdisplay,
 				  x, y, mode->hdisplay, mode->vdisplay);
 }
@@ -226,9 +227,11 @@ static irqreturn_t ipu_irq_handler(int irq, void *dev_id)
 	imx_drm_handle_vblank(ipu_crtc->imx_crtc);
 
 	if (ipu_crtc->newfb) {
+		struct ipu_plane *plane = ipu_crtc->plane[0];
+
 		ipu_crtc->newfb = NULL;
-		ipu_plane_set_base(ipu_crtc->plane[0], ipu_crtc->base.primary->fb,
-				ipu_crtc->plane[0]->x, ipu_crtc->plane[0]->y);
+		ipu_plane_set_base(plane, ipu_crtc->base.primary->fb,
+				   plane->x, plane->y);
 		ipu_crtc_handle_pageflip(ipu_crtc);
 	}
 
diff --git a/drivers/staging/imx-drm/ipuv3-plane.c b/drivers/staging/imx-drm/ipuv3-plane.c
index 6f393a11f44d..944962b692bb 100644
--- a/drivers/staging/imx-drm/ipuv3-plane.c
+++ b/drivers/staging/imx-drm/ipuv3-plane.c
@@ -62,7 +62,6 @@ static inline int calc_bandwidth(int width, int height, unsigned int vref)
 int ipu_plane_set_base(struct ipu_plane *ipu_plane, struct drm_framebuffer *fb,
 		       int x, int y)
 {
-	struct ipu_ch_param __iomem *cpmem;
 	struct drm_gem_cma_object *cma_obj;
 	unsigned long eba;
 
@@ -75,13 +74,12 @@ int ipu_plane_set_base(struct ipu_plane *ipu_plane, struct drm_framebuffer *fb,
 	dev_dbg(ipu_plane->base.dev->dev, "phys = %pad, x = %d, y = %d",
 		&cma_obj->paddr, x, y);
 
-	cpmem = ipu_get_cpmem(ipu_plane->ipu_ch);
-	ipu_cpmem_set_stride(cpmem, fb->pitches[0]);
+	ipu_cpmem_set_stride(ipu_plane->ipu_ch, fb->pitches[0]);
 
 	eba = cma_obj->paddr + fb->offsets[0] +
 	      fb->pitches[0] * y + (fb->bits_per_pixel >> 3) * x;
-	ipu_cpmem_set_buffer(cpmem, 0, eba);
-	ipu_cpmem_set_buffer(cpmem, 1, eba);
+	ipu_cpmem_set_buffer(ipu_plane->ipu_ch, 0, eba);
+	ipu_cpmem_set_buffer(ipu_plane->ipu_ch, 1, eba);
 
 	/* cache offsets for subsequent pageflips */
 	ipu_plane->x = x;
@@ -97,7 +95,6 @@ int ipu_plane_mode_set(struct ipu_plane *ipu_plane, struct drm_crtc *crtc,
 		       uint32_t src_x, uint32_t src_y,
 		       uint32_t src_w, uint32_t src_h)
 {
-	struct ipu_ch_param __iomem *cpmem;
 	struct device *dev = ipu_plane->base.dev->dev;
 	int ret;
 
@@ -175,10 +172,9 @@ int ipu_plane_mode_set(struct ipu_plane *ipu_plane, struct drm_crtc *crtc,
 		return ret;
 	}
 
-	cpmem = ipu_get_cpmem(ipu_plane->ipu_ch);
-	ipu_ch_param_zero(cpmem);
-	ipu_cpmem_set_resolution(cpmem, src_w, src_h);
-	ret = ipu_cpmem_set_fmt(cpmem, fb->pixel_format);
+	ipu_cpmem_zero(ipu_plane->ipu_ch);
+	ipu_cpmem_set_resolution(ipu_plane->ipu_ch, src_w, src_h);
+	ret = ipu_cpmem_set_fmt(ipu_plane->ipu_ch, fb->pixel_format);
 	if (ret < 0) {
 		dev_err(dev, "unsupported pixel format 0x%08x\n",
 			fb->pixel_format);
@@ -263,28 +259,6 @@ void ipu_plane_disable(struct ipu_plane *ipu_plane)
 		ipu_dp_disable(ipu_plane->ipu);
 }
 
-static void ipu_plane_dpms(struct ipu_plane *ipu_plane, int mode)
-{
-	bool enable;
-
-	DRM_DEBUG_KMS("mode = %d", mode);
-
-	enable = (mode == DRM_MODE_DPMS_ON);
-
-	if (enable == ipu_plane->enabled)
-		return;
-
-	if (enable) {
-		ipu_plane_enable(ipu_plane);
-	} else {
-		ipu_plane_disable(ipu_plane);
-
-		ipu_idmac_put(ipu_plane->ipu_ch);
-		ipu_dmfc_put(ipu_plane->dmfc);
-		ipu_dp_put(ipu_plane->dp);
-	}
-}
-
 /*
  * drm_plane API
  */
@@ -318,7 +292,8 @@ static int ipu_update_plane(struct drm_plane *plane, struct drm_crtc *crtc,
 				plane->crtc, crtc);
 	plane->crtc = crtc;
 
-	ipu_plane_dpms(ipu_plane, DRM_MODE_DPMS_ON);
+	if (!ipu_plane->enabled)
+		ipu_plane_enable(ipu_plane);
 
 	return 0;
 }
@@ -329,7 +304,8 @@ static int ipu_disable_plane(struct drm_plane *plane)
 
 	DRM_DEBUG_KMS("[%d] %s\n", __LINE__, __func__);
 
-	ipu_plane_dpms(ipu_plane, DRM_MODE_DPMS_OFF);
+	if (ipu_plane->enabled)
+		ipu_plane_disable(ipu_plane);
 
 	ipu_plane_put_resources(ipu_plane);
 
diff --git a/drivers/staging/imx-drm/parallel-display.c b/drivers/staging/imx-drm/parallel-display.c
index 4ca61afdf622..015a454b87e1 100644
--- a/drivers/staging/imx-drm/parallel-display.c
+++ b/drivers/staging/imx-drm/parallel-display.c
@@ -70,6 +70,7 @@ static int imx_pd_connector_get_modes(struct drm_connector *connector)
 
 	if (imxpd->mode_valid) {
 		struct drm_display_mode *mode = drm_mode_create(connector->dev);
+
 		if (!mode)
 			return -EINVAL;
 		drm_mode_copy(mode, &imxpd->mode);
@@ -80,6 +81,7 @@ static int imx_pd_connector_get_modes(struct drm_connector *connector)
 
 	if (np) {
 		struct drm_display_mode *mode = drm_mode_create(connector->dev);
+
 		if (!mode)
 			return -EINVAL;
 		of_get_drm_display_mode(np, &imxpd->mode, OF_USE_NATIVE_MODE);
@@ -226,7 +228,8 @@ static int imx_pd_bind(struct device *dev, struct device *master, void *data)
 		else if (!strcmp(fmt, "bgr666"))
 			imxpd->interface_pix_fmt = V4L2_PIX_FMT_BGR666;
 		else if (!strcmp(fmt, "lvds666"))
-			imxpd->interface_pix_fmt = v4l2_fourcc('L', 'V', 'D', '6');
+			imxpd->interface_pix_fmt =
+					v4l2_fourcc('L', 'V', 'D', '6');
 	}
 
 	panel_node = of_parse_phandle(np, "fsl,panel", 0);
diff --git a/drivers/staging/line6/midibuf.c b/drivers/staging/line6/midibuf.c
index f0adb7baa603..1ff856989fd6 100644
--- a/drivers/staging/line6/midibuf.c
+++ b/drivers/staging/line6/midibuf.c
@@ -15,11 +15,14 @@
 
 static int midibuf_message_length(unsigned char code)
 {
+	int message_length;
+
 	if (code < 0x80)
-		return -1;
+		message_length = -1;
 	else if (code < 0xf0) {
 		static const int length[] = { 3, 3, 3, 3, 2, 2, 3 };
-		return length[(code >> 4) - 8];
+
+		message_length = length[(code >> 4) - 8];
 	} else {
 		/*
 		   Note that according to the MIDI specification 0xf2 is
@@ -29,8 +32,10 @@ static int midibuf_message_length(unsigned char code)
 		static const int length[] = { -1, 2, -1, 2, -1, -1, 1, 1, 1, 1,
 			1, 1, 1, -1, 1, 1
 		};
-		return length[code & 0x0f];
+		message_length = length[code & 0x0f];
 	}
+
+	return message_length;
 }
 
 static int midibuf_is_empty(struct midi_buffer *this)
diff --git a/drivers/staging/lustre/README.txt b/drivers/staging/lustre/README.txt
new file mode 100644
index 000000000000..cf0ca50ff83b
--- /dev/null
+++ b/drivers/staging/lustre/README.txt
@@ -0,0 +1,87 @@
+Lustre Parallel Filesystem Client
+=================================
+
+The Lustre file system is an open-source, parallel file system
+that supports many requirements of leadership class HPC simulation
+environments.
+Born from from a research project at Carnegie Mellon University,
+the Lustre file system is a widely-used option in HPC.
+The Lustre file system provides a POSIX compliant file system interface,
+can scale to thousands of clients, petabytes of storage and
+hundreds of gigabytes per second of I/O bandwidth.
+
+Unlike shared disk storage cluster filesystems (e.g. OCFS2, GFS, GPFS),
+Lustre has independent Metadata and Data servers that clients can access
+in parallel to maximize performance.
+
+In order to use Lustre client you will need to download lustre client
+tools from
+https://downloads.hpdd.intel.com/public/lustre/latest-feature-release/
+the package name is lustre-client.
+
+You will need to install and configure your Lustre servers separately.
+
+Mount Syntax
+============
+After you installed the lustre-client tools including mount.lustre binary
+you can mount your Lustre filesystem with:
+
+mount -t lustre mgs:/fsname mnt
+
+where mgs is the host name or ip address of your Lustre MGS(management service)
+fsname is the name of the filesystem you would like to mount.
+
+
+Mount Options
+=============
+
+  noflock
+	Disable posix file locking (Applications trying to use
+	the functionality will get ENOSYS)
+
+  localflock
+	Enable local flock support, using only client-local flock
+	(faster, for applications that require flock but do not run
+	 on multiple nodes).
+
+  flock
+	Enable cluster-global posix file locking coherent across all
+	client nodes.
+
+  user_xattr, nouser_xattr
+	Support "user." extended attributes (or not)
+
+  user_fid2path, nouser_fid2path
+	Enable FID to path translation by regular users (or not)
+
+  checksum, nochecksum
+	Verify data consistency on the wire and in memory as it passes
+	between the layers (or not).
+
+  lruresize, nolruresize
+	Allow lock LRU to be controlled by memory pressure on the server
+	(or only 100 (default, controlled by lru_size proc parameter) locks
+	 per CPU per server on this client).
+
+  lazystatfs, nolazystatfs
+	Do not block in statfs() if some of the servers are down.
+
+  32bitapi
+	Shrink inode numbers to fit into 32 bits. This is necessary
+	if you plan to reexport Lustre filesystem from this client via
+	NFSv4.
+
+  verbose, noverbose
+	Enable mount/umount console messages (or not)
+
+More Information
+================
+You can get more information at
+OpenSFS website: http://lustre.opensfs.org/about/
+Intel HPDD wiki: https://wiki.hpdd.intel.com
+
+Out of tree Lustre client and server code is available at:
+http://git.whamcloud.com/fs/lustre-release.git
+
+Latest binary packages:
+http://lustre.opensfs.org/download-lustre/
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs.h b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
index 7d37bec918f3..a6b2f906bb1a 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs.h
@@ -165,7 +165,6 @@ void cfs_get_random_bytes(void *buf, int size);
 #include "libcfs_kernelcomm.h"
 #include "libcfs_workitem.h"
 #include "libcfs_hash.h"
-#include "libcfs_heap.h"
 #include "libcfs_fail.h"
 #include "libcfs_crypto.h"
 
@@ -174,8 +173,7 @@ static inline void *__container_of(void *ptr, unsigned long shift)
 {
 	if (unlikely(IS_ERR(ptr) || ptr == NULL))
 		return ptr;
-	else
-		return (char *)ptr - shift;
+	return (char *)ptr - shift;
 }
 
 #define container_of0(ptr, type, member) \
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
index a140e5d91860..787867847483 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_cpu.h
@@ -102,13 +102,13 @@ struct cfs_cpt_table {
 static inline cpumask_t *
 cfs_cpt_cpumask(struct cfs_cpt_table *cptab, int cpt)
 {
-       return NULL;
+	return NULL;
 }
 
 static inline int
 cfs_cpt_table_print(struct cfs_cpt_table *cptab, char *buf, int len)
 {
-       return 0;
+	return 0;
 }
 #endif /* CONFIG_SMP */
 
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
index 9e610a9ce51f..e8663697e7a6 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_crypto.h
@@ -83,8 +83,7 @@ static inline const char *cfs_crypto_hash_name(unsigned char hash_alg)
 	ht = cfs_crypto_hash_type(hash_alg);
 	if (ht)
 		return ht->cht_name;
-	else
-		return "unknown";
+	return "unknown";
 }
 
 /**     Return digest size for valid algorithm identifier or 0 */
@@ -95,8 +94,7 @@ static inline int cfs_crypto_hash_digestsize(unsigned char hash_alg)
 	ht = cfs_crypto_hash_type(hash_alg);
 	if (ht)
 		return ht->cht_size;
-	else
-		return 0;
+	return 0;
 }
 
 /**     Return hash identifier for valid hash algorithm name or 0xFF */
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
index 30098f39181f..8888b2756174 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_debug.h
@@ -80,7 +80,7 @@ struct ptldebug_header {
 	__u32 ph_pid;
 	__u32 ph_extern_pid;
 	__u32 ph_line_num;
-} __attribute__((packed));
+} __packed;
 
 #define PH_FLAG_FIRST_RECORD 1
 
@@ -265,9 +265,9 @@ int libcfs_debug_vmsg2(struct libcfs_debug_msg_data *msgdata,
 
 /* other external symbols that tracefile provides: */
 int cfs_trace_copyin_string(char *knl_buffer, int knl_buffer_nob,
-				   const char *usr_buffer, int usr_buffer_nob);
-int cfs_trace_copyout_string(char *usr_buffer, int usr_buffer_nob,
-				    const char *knl_buffer, char *append);
+		const char __user *usr_buffer, int usr_buffer_nob);
+int cfs_trace_copyout_string(char __user *usr_buffer, int usr_buffer_nob,
+		const char *knl_buffer, char *append);
 
 #define LIBCFS_DEBUG_FILE_PATH_DEFAULT "/tmp/lustre-log"
 
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_fail.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_fail.h
index 1934ec20e536..eea55d94e6be 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_fail.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_fail.h
@@ -124,8 +124,7 @@ static inline int cfs_fail_timeout_set(__u32 id, __u32 value, int ms, int set)
 {
 	if (unlikely(CFS_FAIL_PRECHECK(id)))
 		return __cfs_fail_timeout_set(id, value, ms, set);
-	else
-		return 0;
+	return 0;
 }
 
 /* If id hit cfs_fail_loc, sleep for seconds or milliseconds */
@@ -156,8 +155,8 @@ static inline void cfs_race(__u32 id)
 
 			cfs_race_state = 0;
 			CERROR("cfs_race id %x sleeping\n", id);
-			cfs_wait_event_interruptible(cfs_race_waitq,
-						     cfs_race_state != 0, rc);
+			rc = wait_event_interruptible(cfs_race_waitq,
+						      cfs_race_state != 0);
 			CERROR("cfs_fail_race id %x awake, rc=%d\n", id, rc);
 		} else {
 			CERROR("cfs_fail_race id %x waking\n", id);
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_heap.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_heap.h
deleted file mode 100644
index bfa6d7b245ea..000000000000
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_heap.h
+++ /dev/null
@@ -1,200 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License version 2 for more details.  A copy is
- * included in the COPYING file that accompanied this code.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2011 Intel Corporation
- */
-/*
- * libcfs/include/libcfs/heap.h
- *
- * Author: Eric Barton	<eeb@whamcloud.com>
- *	   Liang Zhen	<liang@whamcloud.com>
- */
-
-#ifndef __LIBCFS_HEAP_H__
-#define __LIBCFS_HEAP_H__
-
-/** \defgroup heap Binary heap
- *
- * The binary heap is a scalable data structure created using a binary tree. It
- * is capable of maintaining large sets of elements sorted usually by one or
- * more element properties, but really based on anything that can be used as a
- * binary predicate in order to determine the relevant ordering of any two nodes
- * that belong to the set. There is no search operation, rather the intention is
- * for the element of the lowest priority which will always be at the root of
- * the tree (as this is an implementation of a min-heap) to be removed by users
- * for consumption.
- *
- * Users of the heap should embed a \e cfs_binheap_node_t object instance on
- * every object of the set that they wish the binary heap instance to handle,
- * and (at a minimum) provide a cfs_binheap_ops_t::hop_compare() implementation
- * which is used by the heap as the binary predicate during its internal sorting
- * operations.
- *
- * The current implementation enforces no locking scheme, and so assumes the
- * user caters for locking between calls to insert, delete and lookup
- * operations. Since the only consumer for the data structure at this point
- * are NRS policies, and these operate on a per-CPT basis, binary heap instances
- * are tied to a specific CPT.
- * @{
- */
-
-/**
- * Binary heap node.
- *
- * Objects of this type are embedded into objects of the ordered set that is to
- * be maintained by a \e cfs_binheap_t instance.
- */
-typedef struct {
-	/** Index into the binary tree */
-	unsigned int	chn_index;
-} cfs_binheap_node_t;
-
-#define CBH_SHIFT	9
-#define CBH_SIZE       (1 << CBH_SHIFT)		    /* # ptrs per level */
-#define CBH_MASK       (CBH_SIZE - 1)
-#define CBH_NOB	(CBH_SIZE * sizeof(cfs_binheap_node_t *))
-
-#define CBH_POISON	0xdeadbeef
-
-/**
- * Binary heap flags.
- */
-enum {
-	CBH_FLAG_ATOMIC_GROW	= 1,
-};
-
-struct cfs_binheap;
-
-/**
- * Binary heap operations.
- */
-typedef struct {
-	/**
-	 * Called right before inserting a node into the binary heap.
-	 *
-	 * Implementing this operation is optional.
-	 *
-	 * \param[in] h The heap
-	 * \param[in] e The node
-	 *
-	 * \retval 0 success
-	 * \retval != 0 error
-	 */
-	int		(*hop_enter)(struct cfs_binheap *h,
-				     cfs_binheap_node_t *e);
-	/**
-	 * Called right after removing a node from the binary heap.
-	 *
-	 * Implementing this operation is optional.
-	 *
-	 * \param[in] h The heap
-	 * \param[in] e The node
-	 */
-	void		(*hop_exit)(struct cfs_binheap *h,
-				    cfs_binheap_node_t *e);
-	/**
-	 * A binary predicate which is called during internal heap sorting
-	 * operations, and used in order to determine the relevant ordering of
-	 * two heap nodes.
-	 *
-	 * Implementing this operation is mandatory.
-	 *
-	 * \param[in] a The first heap node
-	 * \param[in] b The second heap node
-	 *
-	 * \retval 0 Node a > node b
-	 * \retval 1 Node a < node b
-	 *
-	 * \see cfs_binheap_bubble()
-	 * \see cfs_biheap_sink()
-	 */
-	int		(*hop_compare)(cfs_binheap_node_t *a,
-				       cfs_binheap_node_t *b);
-} cfs_binheap_ops_t;
-
-/**
- * Binary heap object.
- *
- * Sorts elements of type \e cfs_binheap_node_t
- */
-typedef struct cfs_binheap {
-	/** Triple indirect */
-	cfs_binheap_node_t  ****cbh_elements3;
-	/** double indirect */
-	cfs_binheap_node_t   ***cbh_elements2;
-	/** single indirect */
-	cfs_binheap_node_t    **cbh_elements1;
-	/** # elements referenced */
-	unsigned int		cbh_nelements;
-	/** high water mark */
-	unsigned int		cbh_hwm;
-	/** user flags */
-	unsigned int		cbh_flags;
-	/** operations table */
-	cfs_binheap_ops_t      *cbh_ops;
-	/** private data */
-	void		       *cbh_private;
-	/** associated CPT table */
-	struct cfs_cpt_table   *cbh_cptab;
-	/** associated CPT id of this cfs_binheap_t::cbh_cptab */
-	int			cbh_cptid;
-} cfs_binheap_t;
-
-void cfs_binheap_destroy(cfs_binheap_t *h);
-cfs_binheap_t *cfs_binheap_create(cfs_binheap_ops_t *ops, unsigned int flags,
-				  unsigned count, void *arg,
-				  struct cfs_cpt_table *cptab, int cptid);
-cfs_binheap_node_t *cfs_binheap_find(cfs_binheap_t *h, unsigned int idx);
-int cfs_binheap_insert(cfs_binheap_t *h, cfs_binheap_node_t *e);
-void cfs_binheap_remove(cfs_binheap_t *h, cfs_binheap_node_t *e);
-
-static inline int
-cfs_binheap_size(cfs_binheap_t *h)
-{
-	return h->cbh_nelements;
-}
-
-static inline int
-cfs_binheap_is_empty(cfs_binheap_t *h)
-{
-	return h->cbh_nelements == 0;
-}
-
-static inline cfs_binheap_node_t *
-cfs_binheap_root(cfs_binheap_t *h)
-{
-	return cfs_binheap_find(h, 0);
-}
-
-static inline cfs_binheap_node_t *
-cfs_binheap_remove_root(cfs_binheap_t *h)
-{
-	cfs_binheap_node_t *e = cfs_binheap_find(h, 0);
-
-	if (e != NULL)
-		cfs_binheap_remove(h, e);
-	return e;
-}
-
-/** @} heap */
-
-#endif /* __LIBCFS_HEAP_H__ */
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h
index 87f2d901c7c1..3ee38782ad8c 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_ioctl.h
@@ -75,8 +75,7 @@ struct libcfs_ioctl_hdr {
 	__u32 ioc_version;
 };
 
-struct libcfs_debug_ioctl_data
-{
+struct libcfs_debug_ioctl_data {
 	struct libcfs_ioctl_hdr hdr;
 	unsigned int subs;
 	unsigned int debug;
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_prim.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_prim.h
index a38209506d6c..978d3e2f16d3 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_prim.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_prim.h
@@ -83,6 +83,5 @@ static inline void cfs_memory_pressure_restore(int old)
 		memory_pressure_set();
 	else
 		memory_pressure_clr();
-	return;
 }
 #endif
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
index 82a269cee6dd..2817112c0633 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_private.h
@@ -453,8 +453,7 @@ int	 cfs_match_nid(lnet_nid_t nid, struct list_head *list);
  * Support for temporary event tracing with minimal Heisenberg effect.
  * -------------------------------------------------------------------- */
 
-struct libcfs_device_userstate
-{
+struct libcfs_device_userstate {
 	int	   ldu_memhog_pages;
 	struct page   *ldu_memhog_root_page;
 };
diff --git a/drivers/staging/lustre/include/linux/libcfs/libcfs_time.h b/drivers/staging/lustre/include/linux/libcfs/libcfs_time.h
index 1344139c46c3..5de6da085712 100644
--- a/drivers/staging/lustre/include/linux/libcfs/libcfs_time.h
+++ b/drivers/staging/lustre/include/linux/libcfs/libcfs_time.h
@@ -94,7 +94,7 @@ static inline void cfs_slow_warning(unsigned long now, int seconds, char *msg)
 	/*						      \
 	 * XXX nikita: non-portable initializer		 \
 	 */						     \
-	static time_t __next_message = 0;		       \
+	static time_t __next_message;		       \
 	int result;					     \
 								\
 	if (cfs_time_after(cfs_time_current(), __next_message)) \
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h b/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h
index ccc55fc41a9e..4fe50841e8e3 100644
--- a/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h
+++ b/drivers/staging/lustre/include/linux/libcfs/linux/libcfs.h
@@ -78,15 +78,14 @@
 #include <linux/unistd.h>
 #include <linux/vmalloc.h>
 #include <net/sock.h>
-#include <asm/atomic.h>
+#include <linux/atomic.h>
 #include <asm/div64.h>
-#include <asm/timex.h>
-#include <asm/uaccess.h>
+#include <linux/timex.h>
+#include <linux/uaccess.h>
 #include <stdarg.h>
 #include "linux-cpu.h"
 #include "linux-time.h"
 #include "linux-mem.h"
-#include "portals_compat25.h"
 
 
 #define LUSTRE_TRACE_SIZE (THREAD_SIZE >> 5)
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/linux-time.h b/drivers/staging/lustre/include/linux/libcfs/linux/linux-time.h
index 1158a3112bcc..0fc490bac2b5 100644
--- a/drivers/staging/lustre/include/linux/libcfs/linux/linux-time.h
+++ b/drivers/staging/lustre/include/linux/libcfs/linux/linux-time.h
@@ -53,8 +53,6 @@
 #include <linux/time.h>
 #include <asm/div64.h>
 
-#include "portals_compat25.h"
-
 /*
  * post 2.5 kernels.
  */
@@ -108,8 +106,7 @@ static inline void cfs_duration_usec(long d, struct timeval *s)
 	s->tv_usec = t;
 #else
 	s->tv_sec = d / HZ;
-	s->tv_usec = ((d - (long)s->tv_sec * HZ) * \
-		ONE_MILLION) / HZ;
+	s->tv_usec = ((d - (long)s->tv_sec * HZ) * ONE_MILLION) / HZ;
 #endif
 }
 
diff --git a/drivers/staging/lustre/include/linux/libcfs/linux/portals_compat25.h b/drivers/staging/lustre/include/linux/libcfs/linux/portals_compat25.h
deleted file mode 100644
index 442d61be1c18..000000000000
--- a/drivers/staging/lustre/include/linux/libcfs/linux/portals_compat25.h
+++ /dev/null
@@ -1,81 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LIBCFS_LINUX_PORTALS_COMPAT_H__
-#define __LIBCFS_LINUX_PORTALS_COMPAT_H__
-
-/* XXX BUG 1511 -- remove this stanza and all callers when bug 1511 is resolved */
-#if defined(SPINLOCK_DEBUG) && SPINLOCK_DEBUG
-#  define SIGNAL_MASK_ASSERT() \
-   LASSERT(current->sighand->siglock.magic == SPINLOCK_MAGIC)
-#else
-# define SIGNAL_MASK_ASSERT()
-#endif
-/* XXX BUG 1511 -- remove this stanza and all callers when bug 1511 is resolved */
-
-#define SIGNAL_MASK_LOCK(task, flags)				  \
-	spin_lock_irqsave(&task->sighand->siglock, flags)
-#define SIGNAL_MASK_UNLOCK(task, flags)				\
-	spin_unlock_irqrestore(&task->sighand->siglock, flags)
-#define USERMODEHELPER(path, argv, envp)			       \
-	call_usermodehelper(path, argv, envp, 1)
-#define clear_tsk_thread_flag(current, TIF_SIGPENDING)	  clear_tsk_thread_flag(current,       \
-							TIF_SIGPENDING)
-# define smp_num_cpus	      num_online_cpus()
-
-#define cfs_wait_event_interruptible(wq, condition, ret)	       \
-	ret = wait_event_interruptible(wq, condition)
-#define cfs_wait_event_interruptible_exclusive(wq, condition, ret)     \
-	ret = wait_event_interruptible_exclusive(wq, condition)
-
-#define THREAD_NAME(comm, len, fmt, a...)			      \
-	snprintf(comm, len, fmt, ## a)
-
-/* 2.6 alloc_page users can use page->lru */
-#define PAGE_LIST_ENTRY lru
-#define PAGE_LIST(page) ((page)->lru)
-
-#ifndef __user
-#define __user
-#endif
-
-#ifndef __fls
-#define __cfs_fls fls
-#else
-#define __cfs_fls __fls
-#endif
-
-#endif /* _PORTALS_COMPAT_H */
diff --git a/drivers/staging/lustre/include/linux/libcfs/lucache.h b/drivers/staging/lustre/include/linux/libcfs/lucache.h
deleted file mode 100644
index 5d6e1b9de206..000000000000
--- a/drivers/staging/lustre/include/linux/libcfs/lucache.h
+++ /dev/null
@@ -1,162 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef _LUCACHE_H
-#define _LUCACHE_H
-
-#include "libcfs.h"
-
-/** \defgroup ucache ucache
- *
- * @{
- */
-
-#define UC_CACHE_NEW	    0x01
-#define UC_CACHE_ACQUIRING      0x02
-#define UC_CACHE_INVALID	0x04
-#define UC_CACHE_EXPIRED	0x08
-
-#define UC_CACHE_IS_NEW(i)	  ((i)->ue_flags & UC_CACHE_NEW)
-#define UC_CACHE_IS_INVALID(i)      ((i)->ue_flags & UC_CACHE_INVALID)
-#define UC_CACHE_IS_ACQUIRING(i)    ((i)->ue_flags & UC_CACHE_ACQUIRING)
-#define UC_CACHE_IS_EXPIRED(i)      ((i)->ue_flags & UC_CACHE_EXPIRED)
-#define UC_CACHE_IS_VALID(i)	((i)->ue_flags == 0)
-
-#define UC_CACHE_SET_NEW(i)	 (i)->ue_flags |= UC_CACHE_NEW
-#define UC_CACHE_SET_INVALID(i)     (i)->ue_flags |= UC_CACHE_INVALID
-#define UC_CACHE_SET_ACQUIRING(i)   (i)->ue_flags |= UC_CACHE_ACQUIRING
-#define UC_CACHE_SET_EXPIRED(i)     (i)->ue_flags |= UC_CACHE_EXPIRED
-#define UC_CACHE_SET_VALID(i)       (i)->ue_flags = 0
-
-#define UC_CACHE_CLEAR_NEW(i)       (i)->ue_flags &= ~UC_CACHE_NEW
-#define UC_CACHE_CLEAR_ACQUIRING(i) (i)->ue_flags &= ~UC_CACHE_ACQUIRING
-#define UC_CACHE_CLEAR_INVALID(i)   (i)->ue_flags &= ~UC_CACHE_INVALID
-#define UC_CACHE_CLEAR_EXPIRED(i)   (i)->ue_flags &= ~UC_CACHE_EXPIRED
-
-struct upcall_cache_entry;
-
-struct md_perm {
-	lnet_nid_t      mp_nid;
-	__u32	   mp_perm;
-};
-
-struct md_identity {
-	struct upcall_cache_entry *mi_uc_entry;
-	uid_t		      mi_uid;
-	gid_t		      mi_gid;
-	struct group_info	*mi_ginfo;
-	int			mi_nperms;
-	struct md_perm	    *mi_perms;
-};
-
-struct upcall_cache_entry {
-	struct list_head	      ue_hash;
-	__u64		   ue_key;
-	atomic_t	    ue_refcount;
-	int		     ue_flags;
-	wait_queue_head_t	     ue_waitq;
-	unsigned long	      ue_acquire_expire;
-	unsigned long	      ue_expire;
-	union {
-		struct md_identity     identity;
-	} u;
-};
-
-#define UC_CACHE_HASH_SIZE	(128)
-#define UC_CACHE_HASH_INDEX(id)   ((id) & (UC_CACHE_HASH_SIZE - 1))
-#define UC_CACHE_UPCALL_MAXPATH   (1024UL)
-
-struct upcall_cache;
-
-struct upcall_cache_ops {
-	void	    (*init_entry)(struct upcall_cache_entry *, void *args);
-	void	    (*free_entry)(struct upcall_cache *,
-				  struct upcall_cache_entry *);
-	int	     (*upcall_compare)(struct upcall_cache *,
-				       struct upcall_cache_entry *,
-					  __u64 key, void *args);
-	int	     (*downcall_compare)(struct upcall_cache *,
-					 struct upcall_cache_entry *,
-					    __u64 key, void *args);
-	int	     (*do_upcall)(struct upcall_cache *,
-				  struct upcall_cache_entry *);
-	int	     (*parse_downcall)(struct upcall_cache *,
-				       struct upcall_cache_entry *, void *);
-};
-
-struct upcall_cache {
-	struct list_head		uc_hashtable[UC_CACHE_HASH_SIZE];
-	spinlock_t		uc_lock;
-	rwlock_t		uc_upcall_rwlock;
-
-	char			uc_name[40];		/* for upcall */
-	char			uc_upcall[UC_CACHE_UPCALL_MAXPATH];
-	int			uc_acquire_expire;	/* seconds */
-	int			uc_entry_expire;	/* seconds */
-	struct upcall_cache_ops	*uc_ops;
-};
-
-struct upcall_cache_entry *upcall_cache_get_entry(struct upcall_cache *cache,
-						  __u64 key, void *args);
-void upcall_cache_put_entry(struct upcall_cache *cache,
-			    struct upcall_cache_entry *entry);
-int upcall_cache_downcall(struct upcall_cache *cache, __u32 err, __u64 key,
-			  void *args);
-void upcall_cache_flush_idle(struct upcall_cache *cache);
-void upcall_cache_flush_all(struct upcall_cache *cache);
-void upcall_cache_flush_one(struct upcall_cache *cache, __u64 key, void *args);
-struct upcall_cache *upcall_cache_init(const char *name, const char *upcall,
-				       struct upcall_cache_ops *ops);
-void upcall_cache_cleanup(struct upcall_cache *cache);
-
-#if 0
-struct upcall_cache_entry *upcall_cache_get_entry(struct upcall_cache *hash,
-						  __u64 key, __u32 primary,
-						  __u32 ngroups, __u32 *groups);
-void upcall_cache_put_entry(struct upcall_cache *hash,
-			    struct upcall_cache_entry *entry);
-int upcall_cache_downcall(struct upcall_cache *hash, __u32 err, __u64 key,
-			  __u32 primary, __u32 ngroups, __u32 *groups);
-void upcall_cache_flush_idle(struct upcall_cache *cache);
-void upcall_cache_flush_all(struct upcall_cache *cache);
-struct upcall_cache *upcall_cache_init(const char *name);
-void upcall_cache_cleanup(struct upcall_cache *hash);
-
-#endif
-
-/** @} ucache */
-
-#endif /* _LUCACHE_H */
diff --git a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
index 5e8ea222b49f..7e89b3be1a74 100644
--- a/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
+++ b/drivers/staging/lustre/include/linux/lnet/lib-lnet.h
@@ -94,8 +94,8 @@ static inline int lnet_md_exhausted(lnet_libmd_t *md)
 static inline int lnet_md_unlinkable(lnet_libmd_t *md)
 {
 	/* Should unlink md when its refcount is 0 and either:
-	 *  - md has been flagged for deletion (by auto unlink or LNetM[DE]Unlink,
-	 *    in the latter case md may not be exhausted).
+	 *  - md has been flagged for deletion (by auto unlink or
+	 *    LNetM[DE]Unlink, in the latter case md may not be exhausted).
 	 *  - auto unlink is on and md is exhausted.
 	 */
 	if (md->md_refcount != 0)
@@ -644,7 +644,8 @@ lnet_ni_t *lnet_net2ni_locked(__u32 net, int cpt);
 lnet_ni_t *lnet_net2ni(__u32 net);
 
 int lnet_notify(lnet_ni_t *ni, lnet_nid_t peer, int alive, unsigned long when);
-void lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive, unsigned long when);
+void lnet_notify_locked(lnet_peer_t *lp, int notifylnd, int alive,
+			unsigned long when);
 int lnet_add_route(__u32 net, unsigned int hops, lnet_nid_t gateway_nid,
 		   unsigned int priority);
 int lnet_check_routes(void);
@@ -791,8 +792,8 @@ lnet_copy_iov2flat(int dlen, void *dest, unsigned int doffset,
 
 static inline void
 lnet_copy_kiov2flat(int dlen, void *dest, unsigned int doffset,
-		    unsigned int nsiov, lnet_kiov_t *skiov, unsigned int soffset,
-		    unsigned int nob)
+		    unsigned int nsiov, lnet_kiov_t *skiov,
+		    unsigned int soffset, unsigned int nob)
 {
 	struct iovec diov = {/* .iov_base = */ dest, /* .iov_len = */ dlen};
 
@@ -811,8 +812,9 @@ lnet_copy_flat2iov(unsigned int ndiov, struct iovec *diov, unsigned int doffset,
 }
 
 static inline void
-lnet_copy_flat2kiov(unsigned int ndiov, lnet_kiov_t *dkiov, unsigned int doffset,
-		    int slen, void *src, unsigned int soffset, unsigned int nob)
+lnet_copy_flat2kiov(unsigned int ndiov, lnet_kiov_t *dkiov,
+		    unsigned int doffset, int slen, void *src,
+		    unsigned int soffset, unsigned int nob)
 {
 	struct iovec siov = {/* .iov_base = */ src, /* .iov_len = */ slen};
 
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
index ab1643943496..8f5cdd584f85 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.c
@@ -1025,7 +1025,7 @@ kiblnd_ctl(lnet_ni_t *ni, unsigned int cmd, void *arg)
 	struct libcfs_ioctl_data *data = arg;
 	int		       rc = -EINVAL;
 
-	switch(cmd) {
+	switch (cmd) {
 	case IOC_LIBCFS_GET_PEER: {
 		lnet_nid_t   nid = 0;
 		int	  count = 0;
@@ -1202,7 +1202,7 @@ kiblnd_map_rx_descs(kib_conn_t *conn)
 						   rx->rx_msgaddr));
 		KIBLND_UNMAP_ADDR_SET(rx, rx_msgunmap, rx->rx_msgaddr);
 
-		CDEBUG(D_NET,"rx %d: %p %#llx(%#llx)\n",
+		CDEBUG(D_NET, "rx %d: %p %#llx(%#llx)\n",
 		       i, rx->rx_msg, rx->rx_msgaddr,
 		       lnet_page2phys(pg) + pg_off);
 
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
index 4306d9804a10..b02b4ec1e29d 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd.h
@@ -79,8 +79,7 @@
 #define IBLND_N_SCHED			2
 #define IBLND_N_SCHED_HIGH		4
 
-typedef struct
-{
+typedef struct {
 	int	      *kib_dev_failover;     /* HCA failover */
 	unsigned int     *kib_service;	  /* IB service number */
 	int	      *kib_min_reconnect_interval; /* first failed connection retry... */
@@ -186,8 +185,7 @@ struct kib_hca_dev;
 #define KIB_IFNAME_SIZE	      256
 #endif
 
-typedef struct
-{
+typedef struct {
 	struct list_head	   ibd_list;	  /* chain on kib_devs */
 	struct list_head	   ibd_fail_list;     /* chain on kib_failed_devs */
 	__u32		ibd_ifip;	  /* IPoIB interface IP */
@@ -203,8 +201,7 @@ typedef struct
 	struct kib_hca_dev  *ibd_hdev;
 } kib_dev_t;
 
-typedef struct kib_hca_dev
-{
+typedef struct kib_hca_dev {
 	struct rdma_cm_id   *ibh_cmid;	  /* listener cmid */
 	struct ib_device    *ibh_ibdev;	 /* IB device */
 	int		  ibh_page_shift;    /* page shift of current HCA */
@@ -224,8 +221,7 @@ typedef struct kib_hca_dev
 /** # of seconds to retry if allocation failed */
 #define IBLND_POOL_RETRY	1
 
-typedef struct
-{
+typedef struct {
 	int		     ibp_npages;	     /* # pages */
 	struct page	    *ibp_pages[0];	   /* page array */
 } kib_pages_t;
@@ -254,8 +250,7 @@ struct kib_net;
 
 #define IBLND_POOL_NAME_LEN     32
 
-typedef struct kib_poolset
-{
+typedef struct kib_poolset {
 	spinlock_t		ps_lock;		/* serialize */
 	struct kib_net	 *ps_net;		 /* network it belongs to */
 	char		    ps_name[IBLND_POOL_NAME_LEN]; /* pool set name */
@@ -272,8 +267,7 @@ typedef struct kib_poolset
 	kib_ps_node_fini_t      ps_node_fini;	   /* finalize node */
 } kib_poolset_t;
 
-typedef struct kib_pool
-{
+typedef struct kib_pool {
 	struct list_head	      po_list;		/* chain on pool list */
 	struct list_head	      po_free_list;	   /* pre-allocated node */
 	kib_poolset_t	  *po_owner;	       /* pool_set of this pool */
@@ -304,8 +298,7 @@ typedef struct kib_pmr_pool {
 	kib_pool_t	      ppo_pool;	       /* pool */
 } kib_pmr_pool_t;
 
-typedef struct
-{
+typedef struct {
 	spinlock_t		fps_lock;		/* serialize */
 	struct kib_net	 *fps_net;		/* IB network */
 	struct list_head	      fps_pool_list;	  /* FMR pool list */
@@ -320,8 +313,7 @@ typedef struct
 	unsigned long		fps_next_retry;
 } kib_fmr_poolset_t;
 
-typedef struct
-{
+typedef struct {
 	struct list_head	      fpo_list;	       /* chain on pool list */
 	struct kib_hca_dev     *fpo_hdev;	       /* device for this pool */
 	kib_fmr_poolset_t      *fpo_owner;	      /* owner of this pool */
@@ -336,8 +328,7 @@ typedef struct {
 	kib_fmr_pool_t	 *fmr_pool;	       /* pool of FMR */
 } kib_fmr_t;
 
-typedef struct kib_net
-{
+typedef struct kib_net {
 	struct list_head	   ibn_list;	  /* chain on kib_dev_t::ibd_nets */
 	__u64		ibn_incarnation;   /* my epoch */
 	int		  ibn_init;	  /* initialisation state */
@@ -372,8 +363,7 @@ struct kib_sched_info {
 	int			ibs_cpt;	/* CPT id */
 };
 
-typedef struct
-{
+typedef struct {
 	int			kib_init;	/* initialisation state */
 	int			kib_shutdown;	/* shut down? */
 	struct list_head		kib_devs;	/* IB devices extant */
@@ -411,60 +401,51 @@ typedef struct
  * These are sent in sender's byte order (i.e. receiver flips).
  */
 
-typedef struct kib_connparams
-{
+typedef struct kib_connparams {
 	__u16	     ibcp_queue_depth;
 	__u16	     ibcp_max_frags;
 	__u32	     ibcp_max_msg_size;
 } WIRE_ATTR kib_connparams_t;
 
-typedef struct
-{
+typedef struct {
 	lnet_hdr_t	ibim_hdr;	     /* portals header */
 	char	      ibim_payload[0];      /* piggy-backed payload */
 } WIRE_ATTR kib_immediate_msg_t;
 
-typedef struct
-{
+typedef struct {
 	__u32	     rf_nob;	       /* # bytes this frag */
 	__u64	     rf_addr;	      /* CAVEAT EMPTOR: misaligned!! */
 } WIRE_ATTR kib_rdma_frag_t;
 
-typedef struct
-{
+typedef struct {
 	__u32	     rd_key;	       /* local/remote key */
 	__u32	     rd_nfrags;	    /* # fragments */
 	kib_rdma_frag_t   rd_frags[0];	  /* buffer frags */
 } WIRE_ATTR kib_rdma_desc_t;
 
-typedef struct
-{
+typedef struct {
 	lnet_hdr_t	ibprm_hdr;	    /* portals header */
 	__u64	     ibprm_cookie;	 /* opaque completion cookie */
 } WIRE_ATTR kib_putreq_msg_t;
 
-typedef struct
-{
+typedef struct {
 	__u64	     ibpam_src_cookie;     /* reflected completion cookie */
 	__u64	     ibpam_dst_cookie;     /* opaque completion cookie */
 	kib_rdma_desc_t   ibpam_rd;	     /* sender's sink buffer */
 } WIRE_ATTR kib_putack_msg_t;
 
-typedef struct
-{
+typedef struct {
 	lnet_hdr_t	ibgm_hdr;	     /* portals header */
 	__u64	     ibgm_cookie;	  /* opaque completion cookie */
 	kib_rdma_desc_t   ibgm_rd;	      /* rdma descriptor */
 } WIRE_ATTR kib_get_msg_t;
 
-typedef struct
-{
+typedef struct {
 	__u64	     ibcm_cookie;	  /* opaque completion cookie */
 	__s32	     ibcm_status;	  /* < 0 failure: >= 0 length */
 } WIRE_ATTR kib_completion_msg_t;
 
-typedef struct
-{
+typedef struct {
 	/* First 2 fields fixed FOR ALL TIME */
 	__u32	     ibm_magic;	    /* I'm an ibnal message */
 	__u16	     ibm_version;	  /* this is my version number */
@@ -574,14 +555,12 @@ typedef struct kib_tx			   /* transmit message */
 	int		       tx_dmadir;    /* dma direction */
 } kib_tx_t;
 
-typedef struct kib_connvars
-{
+typedef struct kib_connvars {
 	/* connection-in-progress variables */
 	kib_msg_t		 cv_msg;
 } kib_connvars_t;
 
-typedef struct kib_conn
-{
+typedef struct kib_conn {
 	struct kib_sched_info *ibc_sched;	/* scheduler information */
 	struct kib_peer     *ibc_peer;	  /* owning peer */
 	kib_hca_dev_t       *ibc_hdev;	  /* HCA bound on */
@@ -629,8 +608,7 @@ typedef struct kib_conn
 #define IBLND_CONN_CLOSING	    4	 /* being closed */
 #define IBLND_CONN_DISCONNECTED       5	 /* disconnected */
 
-typedef struct kib_peer
-{
+typedef struct kib_peer {
 	struct list_head	   ibp_list;	   /* stash on global peer list */
 	lnet_nid_t	   ibp_nid;	    /* who's on the other end(s) */
 	lnet_ni_t	   *ibp_ni;	     /* LNet interface */
diff --git a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
index 306d72876432..14c9c8d18d02 100644
--- a/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/o2iblnd/o2iblnd_cb.c
@@ -1487,7 +1487,7 @@ kiblnd_send (lnet_ni_t *ni, void *private, lnet_msg_t *lntmsg)
 	switch (type) {
 	default:
 		LBUG();
-		return (-EIO);
+		return -EIO;
 
 	case LNET_MSG_ACK:
 		LASSERT (payload_nob == 0);
@@ -2080,7 +2080,7 @@ kiblnd_connreq_done(kib_conn_t *conn, int status)
 
 	active = (conn->ibc_state == IBLND_CONN_ACTIVE_CONNECT);
 
-	CDEBUG(D_NET,"%s: active(%d), version(%x), status(%d)\n",
+	CDEBUG(D_NET, "%s: active(%d), version(%x), status(%d)\n",
 	       libcfs_nid2str(peer->ibp_nid), active,
 	       conn->ibc_version, status);
 
@@ -2848,7 +2848,7 @@ kiblnd_cm_callback(struct rdma_cm_id *cmid, struct rdma_cm_event *event)
 	case RDMA_CM_EVENT_ADDR_RESOLVED:
 		peer = (kib_peer_t *)cmid->context;
 
-		CDEBUG(D_NET,"%s Addr resolved: %d\n",
+		CDEBUG(D_NET, "%s Addr resolved: %d\n",
 		       libcfs_nid2str(peer->ibp_nid), event->status);
 
 		if (event->status != 0) {
@@ -2878,7 +2878,7 @@ kiblnd_cm_callback(struct rdma_cm_id *cmid, struct rdma_cm_event *event)
 
 	case RDMA_CM_EVENT_ROUTE_RESOLVED:
 		peer = (kib_peer_t *)cmid->context;
-		CDEBUG(D_NET,"%s Route resolved: %d\n",
+		CDEBUG(D_NET, "%s Route resolved: %d\n",
 		       libcfs_nid2str(peer->ibp_nid), event->status);
 
 		if (event->status == 0)
@@ -3215,7 +3215,6 @@ kiblnd_connd (void *arg)
 
 		schedule_timeout(timeout);
 
-		set_current_state(TASK_RUNNING);
 		remove_wait_queue(&kiblnd_data.kib_connd_waitq, &wait);
 		spin_lock_irqsave(&kiblnd_data.kib_connd_lock, flags);
 	}
@@ -3432,7 +3431,6 @@ kiblnd_scheduler(void *arg)
 		busy_loops = 0;
 
 		remove_wait_queue(&sched->ibs_waitq, &wait);
-		set_current_state(TASK_RUNNING);
 		spin_lock_irqsave(&sched->ibs_lock, flags);
 	}
 
@@ -3507,7 +3505,6 @@ kiblnd_failover_thread(void *arg)
 
 		rc = schedule_timeout(long_sleep ? cfs_time_seconds(10) :
 						   cfs_time_seconds(1));
-		set_current_state(TASK_RUNNING);
 		remove_wait_queue(&kiblnd_data.kib_failover_waitq, &wait);
 		write_lock_irqsave(glock, flags);
 
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
index f7cce9d9b7ba..a29d4da6e343 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd.h
@@ -93,8 +93,7 @@ typedef struct				  /* in-use interface */
 	char		ksni_name[IFNAMSIZ];	/* interface name */
 } ksock_interface_t;
 
-typedef struct
-{
+typedef struct {
 	/* "stuck" socket timeout (seconds) */
 	int	      *ksnd_timeout;
 	/* # scheduler threads in each pool while starting */
@@ -126,8 +125,7 @@ typedef struct
 	int	      *ksnd_zc_recv_min_nfrags; /* minimum # of fragments to enable ZC receive */
 } ksock_tunables_t;
 
-typedef struct
-{
+typedef struct {
 	__u64		  ksnn_incarnation;	/* my epoch */
 	spinlock_t	  ksnn_lock;		/* serialise */
 	struct list_head	  ksnn_list;		/* chain on global list */
@@ -142,8 +140,7 @@ typedef struct
 /** reserved thread for accepting & creating new connd */
 #define SOCKNAL_CONND_RESV     1
 
-typedef struct
-{
+typedef struct {
 	int			ksnd_init;	/* initialisation state */
 	int			ksnd_nnets;	/* # networks set up */
 	struct list_head		ksnd_nets;	/* list of nets */
@@ -257,8 +254,7 @@ typedef union {
 #define SOCKNAL_RX_LNET_PAYLOAD 5	       /* reading lnet payload (to deliver here) */
 #define SOCKNAL_RX_SLOP	 6	       /* skipping body */
 
-typedef struct ksock_conn
-{
+typedef struct ksock_conn {
 	struct ksock_peer  *ksnc_peer;	 /* owning peer */
 	struct ksock_route *ksnc_route;	/* owning route */
 	struct list_head	  ksnc_list;	 /* stash on peer's conn list */
@@ -313,8 +309,7 @@ typedef struct ksock_conn
 	unsigned long	    ksnc_tx_last_post;  /* time stamp of the last posted TX */
 } ksock_conn_t;
 
-typedef struct ksock_route
-{
+typedef struct ksock_route {
 	struct list_head	    ksnr_list;	/* chain on peer route list */
 	struct list_head	    ksnr_connd_list;  /* chain on ksnr_connd_routes */
 	struct ksock_peer    *ksnr_peer;	/* owning peer */
@@ -334,8 +329,7 @@ typedef struct ksock_route
 
 #define SOCKNAL_KEEPALIVE_PING	  1       /* cookie for keepalive ping */
 
-typedef struct ksock_peer
-{
+typedef struct ksock_peer {
 	struct list_head	    ksnp_list;	/* stash on global peer list */
 	unsigned long	    ksnp_last_alive;  /* when (in jiffies) I was last alive */
 	lnet_process_id_t     ksnp_id;       /* who's on the other end(s) */
@@ -358,8 +352,7 @@ typedef struct ksock_peer
 	__u32		 ksnp_passive_ips[LNET_MAX_INTERFACES]; /* preferred local interfaces */
 } ksock_peer_t;
 
-typedef struct ksock_connreq
-{
+typedef struct ksock_connreq {
 	struct list_head	    ksncr_list;     /* stash on ksnd_connd_connreqs */
 	lnet_ni_t	    *ksncr_ni;       /* chosen NI */
 	struct socket	 *ksncr_sock;     /* accepted socket */
@@ -372,8 +365,7 @@ extern ksock_tunables_t ksocknal_tunables;
 #define SOCKNAL_MATCH_YES       1	/* TX matches type of connection */
 #define SOCKNAL_MATCH_MAY       2	/* TX can be sent on the connection, but not preferred */
 
-typedef struct ksock_proto
-{
+typedef struct ksock_proto {
 	int	   pro_version;					      /* version number of protocol */
 	int	 (*pro_send_hello)(ksock_conn_t *, ksock_hello_msg_t *);     /* handshake function */
 	int	 (*pro_recv_hello)(ksock_conn_t *, ksock_hello_msg_t *, int);/* handshake function */
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
index 521439954fcb..d29f5f134b89 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_cb.c
@@ -107,7 +107,7 @@ ksocknal_free_tx (ksock_tx_t *tx)
 	}
 }
 
-int
+static int
 ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	struct iovec  *iov = tx->tx_iov;
@@ -144,7 +144,7 @@ ksocknal_send_iov (ksock_conn_t *conn, ksock_tx_t *tx)
 	return rc;
 }
 
-int
+static int
 ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	lnet_kiov_t    *kiov = tx->tx_kiov;
@@ -182,7 +182,7 @@ ksocknal_send_kiov (ksock_conn_t *conn, ksock_tx_t *tx)
 	return rc;
 }
 
-int
+static int
 ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	int      rc;
@@ -232,7 +232,7 @@ ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 				rc = -EAGAIN;
 
 			/* Check if EAGAIN is due to memory pressure */
-			if(rc == -EAGAIN && ksocknal_lib_memory_pressure(conn))
+			if (rc == -EAGAIN && ksocknal_lib_memory_pressure(conn))
 				rc = -ENOMEM;
 
 			break;
@@ -248,7 +248,7 @@ ksocknal_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	return rc;
 }
 
-int
+static int
 ksocknal_recv_iov (ksock_conn_t *conn)
 {
 	struct iovec *iov = conn->ksnc_rx_iov;
@@ -281,7 +281,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 
 		if (nob < (int)iov->iov_len) {
 			iov->iov_len -= nob;
-			iov->iov_base = (void *)((char *)iov->iov_base + nob);
+			iov->iov_base += nob;
 			return -EAGAIN;
 		}
 
@@ -293,7 +293,7 @@ ksocknal_recv_iov (ksock_conn_t *conn)
 	return rc;
 }
 
-int
+static int
 ksocknal_recv_kiov (ksock_conn_t *conn)
 {
 	lnet_kiov_t   *kiov = conn->ksnc_rx_kiov;
@@ -337,7 +337,7 @@ ksocknal_recv_kiov (ksock_conn_t *conn)
 	return 1;
 }
 
-int
+static int
 ksocknal_receive (ksock_conn_t *conn)
 {
 	/* Return 1 on success, 0 on EOF, < 0 on error.
@@ -499,7 +499,7 @@ ksocknal_uncheck_zc_req(ksock_tx_t *tx)
 	ksocknal_tx_decref(tx);
 }
 
-int
+static int
 ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 {
 	int	    rc;
@@ -579,7 +579,7 @@ ksocknal_process_transmit (ksock_conn_t *conn, ksock_tx_t *tx)
 	return rc;
 }
 
-void
+static void
 ksocknal_launch_connection_locked (ksock_route_t *route)
 {
 
@@ -1109,7 +1109,7 @@ ksocknal_new_packet (ksock_conn_t *conn, int nob_to_skip)
 	return 0;
 }
 
-int
+static int
 ksocknal_process_receive (ksock_conn_t *conn)
 {
 	lnet_hdr_t	*lhdr;
@@ -1537,9 +1537,9 @@ int ksocknal_scheduler(void *arg)
 			nloops = 0;
 
 			if (!did_something) {   /* wait for something to do */
-				cfs_wait_event_interruptible_exclusive(
+				rc = wait_event_interruptible_exclusive(
 					sched->kss_waitq,
-					!ksocknal_sched_cansleep(sched), rc);
+					!ksocknal_sched_cansleep(sched));
 				LASSERT (rc == 0);
 			} else {
 				cond_resched();
@@ -1594,8 +1594,8 @@ void ksocknal_write_callback (ksock_conn_t *conn)
 
 	conn->ksnc_tx_ready = 1;
 
-	if (!conn->ksnc_tx_scheduled && // not being progressed
-	    !list_empty(&conn->ksnc_tx_queue)){//packets to send
+	if (!conn->ksnc_tx_scheduled && /* not being progressed */
+	    !list_empty(&conn->ksnc_tx_queue)) { /* packets to send */
 		list_add_tail (&conn->ksnc_tx_list,
 				   &sched->kss_tx_conns);
 		conn->ksnc_tx_scheduled = 1;
@@ -1608,7 +1608,7 @@ void ksocknal_write_callback (ksock_conn_t *conn)
 	spin_unlock_bh(&sched->kss_lock);
 }
 
-ksock_proto_t *
+static ksock_proto_t *
 ksocknal_parse_proto_version (ksock_hello_msg_t *hello)
 {
 	__u32   version = 0;
@@ -1672,11 +1672,10 @@ ksocknal_send_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 	return conn->ksnc_proto->pro_send_hello(conn, hello);
 }
 
-int
+static int
 ksocknal_invert_type(int type)
 {
-	switch (type)
-	{
+	switch (type) {
 	case SOCKLND_CONN_ANY:
 	case SOCKLND_CONN_CONTROL:
 		return type;
@@ -1837,7 +1836,7 @@ ksocknal_recv_hello (lnet_ni_t *ni, ksock_conn_t *conn,
 	return 0;
 }
 
-int
+static int
 ksocknal_connect (ksock_route_t *route)
 {
 	LIST_HEAD    (zombies);
@@ -2233,7 +2232,6 @@ ksocknal_connd (void *arg)
 		nloops = 0;
 		schedule_timeout(timeout);
 
-		set_current_state(TASK_RUNNING);
 		remove_wait_queue(&ksocknal_data.ksnd_connd_waitq, &wait);
 		spin_lock_bh(connd_lock);
 	}
@@ -2244,7 +2242,7 @@ ksocknal_connd (void *arg)
 	return 0;
 }
 
-ksock_conn_t *
+static ksock_conn_t *
 ksocknal_find_timed_out_conn (ksock_peer_t *peer)
 {
 	/* We're called with a shared lock on ksnd_global_lock */
@@ -2354,7 +2352,7 @@ ksocknal_flush_stale_txs(ksock_peer_t *peer)
 	ksocknal_txlist_done(peer->ksnp_ni, &stale_txs, 1);
 }
 
-int
+static int
 ksocknal_send_keepalive_locked(ksock_peer_t *peer)
 {
 	ksock_sched_t  *sched;
@@ -2415,7 +2413,7 @@ ksocknal_send_keepalive_locked(ksock_peer_t *peer)
 }
 
 
-void
+static void
 ksocknal_check_peer_timeouts (int idx)
 {
 	struct list_head       *peers = &ksocknal_data.ksnd_peers[idx];
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
index f14a60ce0916..7a793d2d3582 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_lib-linux.h
@@ -63,7 +63,6 @@
 #include <linux/syscalls.h>
 
 #include "../../../include/linux/libcfs/libcfs.h"
-#include "../../../include/linux/libcfs/linux/portals_compat25.h"
 
 #include <linux/crc32.h>
 static inline __u32 ksocknal_csum(__u32 crc, unsigned char const *p, size_t len)
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
index 54c0019904ff..66d78c9be650 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_modparams.c
@@ -33,7 +33,7 @@ static int peer_credits = 8;
 module_param(peer_credits, int, 0444);
 MODULE_PARM_DESC(peer_credits, "# concurrent sends to 1 peer");
 
-static int peer_buffer_credits = 0;
+static int peer_buffer_credits;
 module_param(peer_buffer_credits, int, 0444);
 MODULE_PARM_DESC(peer_buffer_credits, "# per-peer router buffer credits");
 
@@ -85,7 +85,7 @@ static int rx_buffer_size = DEFAULT_BUFFER_SIZE;
 module_param(rx_buffer_size, int, 0644);
 MODULE_PARM_DESC(rx_buffer_size, "socket rx buffer size (0 for system default)");
 
-static int nagle = 0;
+static int nagle;
 module_param(nagle, int, 0644);
 MODULE_PARM_DESC(nagle, "enable NAGLE?");
 
@@ -110,11 +110,11 @@ static int keepalive_intvl = 5;
 module_param(keepalive_intvl, int, 0644);
 MODULE_PARM_DESC(keepalive_intvl, "seconds between probes");
 
-static int enable_csum = 0;
+static int enable_csum;
 module_param(enable_csum, int, 0644);
 MODULE_PARM_DESC(enable_csum, "enable check sum");
 
-static int inject_csum_error = 0;
+static int inject_csum_error;
 module_param(inject_csum_error, int, 0644);
 MODULE_PARM_DESC(inject_csum_error, "set non-zero to inject a checksum error");
 
@@ -126,7 +126,7 @@ static unsigned int zc_min_payload = (16 << 10);
 module_param(zc_min_payload, int, 0644);
 MODULE_PARM_DESC(zc_min_payload, "minimum payload size to zero copy");
 
-static unsigned int zc_recv = 0;
+static unsigned int zc_recv;
 module_param(zc_recv, int, 0644);
 MODULE_PARM_DESC(zc_recv, "enable ZC recv for Chelsio driver");
 
diff --git a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
index 050a58d08809..9dde548070af 100644
--- a/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
+++ b/drivers/staging/lustre/lnet/klnds/socklnd/socklnd_proto.c
@@ -495,7 +495,8 @@ ksocknal_send_hello_v1 (ksock_conn_t *conn, ksock_hello_msg_t *hello)
 	hdr->msg.hello.type = cpu_to_le32 (hello->kshm_ctype);
 	hdr->msg.hello.incarnation = cpu_to_le64 (hello->kshm_src_incarnation);
 
-	rc = libcfs_sock_write(sock, hdr, sizeof(*hdr),lnet_acceptor_timeout());
+	rc = libcfs_sock_write(sock, hdr, sizeof(*hdr),
+			       lnet_acceptor_timeout());
 
 	if (rc != 0) {
 		CNETERR("Error %d sending HELLO hdr to %pI4h/%d\n",
@@ -568,7 +569,8 @@ ksocknal_send_hello_v2 (ksock_conn_t *conn, ksock_hello_msg_t *hello)
 }
 
 static int
-ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,int timeout)
+ksocknal_recv_hello_v1(ksock_conn_t *conn, ksock_hello_msg_t *hello,
+		       int timeout)
 {
 	struct socket	*sock = conn->ksnc_sock;
 	lnet_hdr_t	  *hdr;
@@ -756,8 +758,7 @@ ksocknal_unpack_msg_v2(ksock_msg_t *msg)
 	return;  /* Do nothing */
 }
 
-ksock_proto_t  ksocknal_protocol_v1x =
-{
+ksock_proto_t  ksocknal_protocol_v1x = {
 	.pro_version	    = KSOCK_PROTO_V1,
 	.pro_send_hello	 = ksocknal_send_hello_v1,
 	.pro_recv_hello	 = ksocknal_recv_hello_v1,
@@ -770,8 +771,7 @@ ksock_proto_t  ksocknal_protocol_v1x =
 	.pro_match_tx	   = ksocknal_match_tx
 };
 
-ksock_proto_t  ksocknal_protocol_v2x =
-{
+ksock_proto_t  ksocknal_protocol_v2x = {
 	.pro_version	    = KSOCK_PROTO_V2,
 	.pro_send_hello	 = ksocknal_send_hello_v2,
 	.pro_recv_hello	 = ksocknal_recv_hello_v2,
@@ -784,8 +784,7 @@ ksock_proto_t  ksocknal_protocol_v2x =
 	.pro_match_tx	   = ksocknal_match_tx
 };
 
-ksock_proto_t  ksocknal_protocol_v3x =
-{
+ksock_proto_t  ksocknal_protocol_v3x = {
 	.pro_version	    = KSOCK_PROTO_V3,
 	.pro_send_hello	 = ksocknal_send_hello_v2,
 	.pro_recv_hello	 = ksocknal_recv_hello_v2,
diff --git a/drivers/staging/lustre/lnet/lnet/acceptor.c b/drivers/staging/lustre/lnet/lnet/acceptor.c
index 5dfb887a03ae..72fd1bf70ca0 100644
--- a/drivers/staging/lustre/lnet/lnet/acceptor.c
+++ b/drivers/staging/lustre/lnet/lnet/acceptor.c
@@ -434,11 +434,11 @@ accept2secure(const char *acc, long *sec)
 		return 1;
 	} else if (!strcmp(acc, "none")) {
 		return 0;
-	} else {
-		LCONSOLE_ERROR_MSG(0x124, "Can't parse 'accept=\"%s\"'\n",
-				   acc);
-		return -EINVAL;
 	}
+
+	LCONSOLE_ERROR_MSG(0x124, "Can't parse 'accept=\"%s\"'\n",
+			   acc);
+	return -EINVAL;
 }
 
 int
diff --git a/drivers/staging/lustre/lnet/lnet/api-ni.c b/drivers/staging/lustre/lnet/lnet/api-ni.c
index b28734a76fac..60bc2ae4fdf1 100644
--- a/drivers/staging/lustre/lnet/lnet/api-ni.c
+++ b/drivers/staging/lustre/lnet/lnet/api-ni.c
@@ -73,8 +73,7 @@ lnet_get_networks(void)
 	int     rc;
 
 	if (*networks != 0 && *ip2nets != 0) {
-		LCONSOLE_ERROR_MSG(0x101, "Please specify EITHER 'networks' or "
-				   "'ip2nets' but not both at once\n");
+		LCONSOLE_ERROR_MSG(0x101, "Please specify EITHER 'networks' or 'ip2nets' but not both at once\n");
 		return NULL;
 	}
 
@@ -177,7 +176,7 @@ lnet_create_locks(void)
 	return -ENOMEM;
 }
 
-static void lnet_assert_wire_constants (void)
+static void lnet_assert_wire_constants(void)
 {
 	/* Wire protocol assertions generated by 'wirecheck'
 	 * running on Linux robert.bartonsoftware.com 2.6.8-1.521
@@ -185,93 +184,93 @@ static void lnet_assert_wire_constants (void)
 	 * with gcc version 3.3.3 20040412 (Red Hat Linux 3.3.3-7) */
 
 	/* Constants... */
-	CLASSERT (LNET_PROTO_TCP_MAGIC == 0xeebc0ded);
-	CLASSERT (LNET_PROTO_TCP_VERSION_MAJOR == 1);
-	CLASSERT (LNET_PROTO_TCP_VERSION_MINOR == 0);
-	CLASSERT (LNET_MSG_ACK == 0);
-	CLASSERT (LNET_MSG_PUT == 1);
-	CLASSERT (LNET_MSG_GET == 2);
-	CLASSERT (LNET_MSG_REPLY == 3);
-	CLASSERT (LNET_MSG_HELLO == 4);
+	CLASSERT(LNET_PROTO_TCP_MAGIC == 0xeebc0ded);
+	CLASSERT(LNET_PROTO_TCP_VERSION_MAJOR == 1);
+	CLASSERT(LNET_PROTO_TCP_VERSION_MINOR == 0);
+	CLASSERT(LNET_MSG_ACK == 0);
+	CLASSERT(LNET_MSG_PUT == 1);
+	CLASSERT(LNET_MSG_GET == 2);
+	CLASSERT(LNET_MSG_REPLY == 3);
+	CLASSERT(LNET_MSG_HELLO == 4);
 
 	/* Checks for struct ptl_handle_wire_t */
-	CLASSERT ((int)sizeof(lnet_handle_wire_t) == 16);
-	CLASSERT ((int)offsetof(lnet_handle_wire_t, wh_interface_cookie) == 0);
-	CLASSERT ((int)sizeof(((lnet_handle_wire_t *)0)->wh_interface_cookie) == 8);
-	CLASSERT ((int)offsetof(lnet_handle_wire_t, wh_object_cookie) == 8);
-	CLASSERT ((int)sizeof(((lnet_handle_wire_t *)0)->wh_object_cookie) == 8);
+	CLASSERT((int)sizeof(lnet_handle_wire_t) == 16);
+	CLASSERT((int)offsetof(lnet_handle_wire_t, wh_interface_cookie) == 0);
+	CLASSERT((int)sizeof(((lnet_handle_wire_t *)0)->wh_interface_cookie) == 8);
+	CLASSERT((int)offsetof(lnet_handle_wire_t, wh_object_cookie) == 8);
+	CLASSERT((int)sizeof(((lnet_handle_wire_t *)0)->wh_object_cookie) == 8);
 
 	/* Checks for struct lnet_magicversion_t */
-	CLASSERT ((int)sizeof(lnet_magicversion_t) == 8);
-	CLASSERT ((int)offsetof(lnet_magicversion_t, magic) == 0);
-	CLASSERT ((int)sizeof(((lnet_magicversion_t *)0)->magic) == 4);
-	CLASSERT ((int)offsetof(lnet_magicversion_t, version_major) == 4);
-	CLASSERT ((int)sizeof(((lnet_magicversion_t *)0)->version_major) == 2);
-	CLASSERT ((int)offsetof(lnet_magicversion_t, version_minor) == 6);
-	CLASSERT ((int)sizeof(((lnet_magicversion_t *)0)->version_minor) == 2);
+	CLASSERT((int)sizeof(lnet_magicversion_t) == 8);
+	CLASSERT((int)offsetof(lnet_magicversion_t, magic) == 0);
+	CLASSERT((int)sizeof(((lnet_magicversion_t *)0)->magic) == 4);
+	CLASSERT((int)offsetof(lnet_magicversion_t, version_major) == 4);
+	CLASSERT((int)sizeof(((lnet_magicversion_t *)0)->version_major) == 2);
+	CLASSERT((int)offsetof(lnet_magicversion_t, version_minor) == 6);
+	CLASSERT((int)sizeof(((lnet_magicversion_t *)0)->version_minor) == 2);
 
 	/* Checks for struct lnet_hdr_t */
-	CLASSERT ((int)sizeof(lnet_hdr_t) == 72);
-	CLASSERT ((int)offsetof(lnet_hdr_t, dest_nid) == 0);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->dest_nid) == 8);
-	CLASSERT ((int)offsetof(lnet_hdr_t, src_nid) == 8);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->src_nid) == 8);
-	CLASSERT ((int)offsetof(lnet_hdr_t, dest_pid) == 16);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->dest_pid) == 4);
-	CLASSERT ((int)offsetof(lnet_hdr_t, src_pid) == 20);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->src_pid) == 4);
-	CLASSERT ((int)offsetof(lnet_hdr_t, type) == 24);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->type) == 4);
-	CLASSERT ((int)offsetof(lnet_hdr_t, payload_length) == 28);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->payload_length) == 4);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg) == 32);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg) == 40);
+	CLASSERT((int)sizeof(lnet_hdr_t) == 72);
+	CLASSERT((int)offsetof(lnet_hdr_t, dest_nid) == 0);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->dest_nid) == 8);
+	CLASSERT((int)offsetof(lnet_hdr_t, src_nid) == 8);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->src_nid) == 8);
+	CLASSERT((int)offsetof(lnet_hdr_t, dest_pid) == 16);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->dest_pid) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, src_pid) == 20);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->src_pid) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, type) == 24);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->type) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, payload_length) == 28);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->payload_length) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg) == 32);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg) == 40);
 
 	/* Ack */
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.ack.dst_wmd) == 32);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.ack.dst_wmd) == 16);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.ack.match_bits) == 48);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.ack.match_bits) == 8);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.ack.mlength) == 56);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.ack.mlength) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.ack.dst_wmd) == 32);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.ack.dst_wmd) == 16);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.ack.match_bits) == 48);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.ack.match_bits) == 8);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.ack.mlength) == 56);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.ack.mlength) == 4);
 
 	/* Put */
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.put.ack_wmd) == 32);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.put.ack_wmd) == 16);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.put.match_bits) == 48);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.put.match_bits) == 8);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.put.hdr_data) == 56);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.put.hdr_data) == 8);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.put.ptl_index) == 64);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.put.ptl_index) == 4);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.put.offset) == 68);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.put.offset) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.put.ack_wmd) == 32);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.put.ack_wmd) == 16);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.put.match_bits) == 48);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.put.match_bits) == 8);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.put.hdr_data) == 56);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.put.hdr_data) == 8);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.put.ptl_index) == 64);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.put.ptl_index) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.put.offset) == 68);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.put.offset) == 4);
 
 	/* Get */
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.get.return_wmd) == 32);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.get.return_wmd) == 16);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.get.match_bits) == 48);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.get.match_bits) == 8);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.get.ptl_index) == 56);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.get.ptl_index) == 4);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.get.src_offset) == 60);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.get.src_offset) == 4);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.get.sink_length) == 64);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.get.sink_length) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.get.return_wmd) == 32);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.get.return_wmd) == 16);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.get.match_bits) == 48);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.get.match_bits) == 8);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.get.ptl_index) == 56);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.get.ptl_index) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.get.src_offset) == 60);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.get.src_offset) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.get.sink_length) == 64);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.get.sink_length) == 4);
 
 	/* Reply */
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.reply.dst_wmd) == 32);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.reply.dst_wmd) == 16);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.reply.dst_wmd) == 32);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.reply.dst_wmd) == 16);
 
 	/* Hello */
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.hello.incarnation) == 32);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.hello.incarnation) == 8);
-	CLASSERT ((int)offsetof(lnet_hdr_t, msg.hello.type) == 40);
-	CLASSERT ((int)sizeof(((lnet_hdr_t *)0)->msg.hello.type) == 4);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.hello.incarnation) == 32);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.hello.incarnation) == 8);
+	CLASSERT((int)offsetof(lnet_hdr_t, msg.hello.type) == 40);
+	CLASSERT((int)sizeof(((lnet_hdr_t *)0)->msg.hello.type) == 4);
 }
 
 static lnd_t *
-lnet_find_lnd_by_type (int type)
+lnet_find_lnd_by_type(int type)
 {
 	lnd_t	      *lnd;
 	struct list_head	 *tmp;
@@ -288,15 +287,15 @@ lnet_find_lnd_by_type (int type)
 }
 
 void
-lnet_register_lnd (lnd_t *lnd)
+lnet_register_lnd(lnd_t *lnd)
 {
 	LNET_MUTEX_LOCK(&the_lnet.ln_lnd_mutex);
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (libcfs_isknown_lnd(lnd->lnd_type));
-	LASSERT (lnet_find_lnd_by_type(lnd->lnd_type) == NULL);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(libcfs_isknown_lnd(lnd->lnd_type));
+	LASSERT(lnet_find_lnd_by_type(lnd->lnd_type) == NULL);
 
-	list_add_tail (&lnd->lnd_list, &the_lnet.ln_lnds);
+	list_add_tail(&lnd->lnd_list, &the_lnet.ln_lnds);
 	lnd->lnd_refcount = 0;
 
 	CDEBUG(D_NET, "%s LND registered\n", libcfs_lnd2str(lnd->lnd_type));
@@ -306,15 +305,15 @@ lnet_register_lnd (lnd_t *lnd)
 EXPORT_SYMBOL(lnet_register_lnd);
 
 void
-lnet_unregister_lnd (lnd_t *lnd)
+lnet_unregister_lnd(lnd_t *lnd)
 {
 	LNET_MUTEX_LOCK(&the_lnet.ln_lnd_mutex);
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (lnet_find_lnd_by_type(lnd->lnd_type) == lnd);
-	LASSERT (lnd->lnd_refcount == 0);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(lnet_find_lnd_by_type(lnd->lnd_type) == lnd);
+	LASSERT(lnd->lnd_refcount == 0);
 
-	list_del (&lnd->lnd_list);
+	list_del(&lnd->lnd_list);
 	CDEBUG(D_NET, "%s LND unregistered\n", libcfs_lnd2str(lnd->lnd_type));
 
 	LNET_MUTEX_UNLOCK(&the_lnet.ln_lnd_mutex);
@@ -367,35 +366,34 @@ EXPORT_SYMBOL(lnet_counters_reset);
 #ifdef LNET_USE_LIB_FREELIST
 
 int
-lnet_freelist_init (lnet_freelist_t *fl, int n, int size)
+lnet_freelist_init(lnet_freelist_t *fl, int n, int size)
 {
 	char *space;
 
-	LASSERT (n > 0);
+	LASSERT(n > 0);
 
-	size += offsetof (lnet_freeobj_t, fo_contents);
+	size += offsetof(lnet_freeobj_t, fo_contents);
 
 	LIBCFS_ALLOC(space, n * size);
 	if (space == NULL)
-		return (-ENOMEM);
+		return -ENOMEM;
 
-	INIT_LIST_HEAD (&fl->fl_list);
+	INIT_LIST_HEAD(&fl->fl_list);
 	fl->fl_objs = space;
 	fl->fl_nobjs = n;
 	fl->fl_objsize = size;
 
-	do
-	{
-		memset (space, 0, size);
-		list_add ((struct list_head *)space, &fl->fl_list);
+	do {
+		memset(space, 0, size);
+		list_add((struct list_head *)space, &fl->fl_list);
 		space += size;
 	} while (--n != 0);
 
-	return (0);
+	return 0;
 }
 
 void
-lnet_freelist_fini (lnet_freelist_t *fl)
+lnet_freelist_fini(lnet_freelist_t *fl)
 {
 	struct list_head       *el;
 	int	       count;
@@ -407,16 +405,16 @@ lnet_freelist_fini (lnet_freelist_t *fl)
 	for (el = fl->fl_list.next; el != &fl->fl_list; el = el->next)
 		count++;
 
-	LASSERT (count == fl->fl_nobjs);
+	LASSERT(count == fl->fl_nobjs);
 
 	LIBCFS_FREE(fl->fl_objs, fl->fl_nobjs * fl->fl_objsize);
-	memset (fl, 0, sizeof (*fl));
+	memset(fl, 0, sizeof(*fl));
 }
 
 #endif /* LNET_USE_LIB_FREELIST */
 
 static __u64
-lnet_create_interface_cookie (void)
+lnet_create_interface_cookie(void)
 {
 	/* NB the interface cookie in wire handles guards against delayed
 	 * replies and ACKs appearing valid after reboot. Initialisation time,
@@ -424,6 +422,7 @@ lnet_create_interface_cookie (void)
 	 * easily good enough. */
 	struct timeval tv;
 	__u64	  cookie;
+
 	do_gettimeofday(&tv);
 	cookie = tv.tv_sec;
 	cookie *= 1000000;
@@ -615,11 +614,11 @@ lnet_prepare(lnet_pid_t requested_pid)
 	struct lnet_res_container **recs;
 	int			  rc = 0;
 
-	LASSERT (the_lnet.ln_refcount == 0);
+	LASSERT(the_lnet.ln_refcount == 0);
 
 	the_lnet.ln_routing = 0;
 
-	LASSERT ((requested_pid & LNET_PID_USERFLAG) == 0);
+	LASSERT((requested_pid & LNET_PID_USERFLAG) == 0);
 	the_lnet.ln_pid = requested_pid;
 
 	INIT_LIST_HEAD(&the_lnet.ln_test_peers);
@@ -684,7 +683,7 @@ lnet_prepare(lnet_pid_t requested_pid)
 }
 
 int
-lnet_unprepare (void)
+lnet_unprepare(void)
 {
 	/* NB no LNET_LOCK since this is the last reference.  All LND instances
 	 * have shut down already, so it is safe to unlink and free all
@@ -875,7 +874,7 @@ lnet_islocalnid(lnet_nid_t nid)
 }
 
 int
-lnet_count_acceptor_nis (void)
+lnet_count_acceptor_nis(void)
 {
 	/* Return the # of NIs that need the acceptor. */
 	int		count = 0;
@@ -914,7 +913,7 @@ lnet_ni_tq_credits(lnet_ni_t *ni)
 }
 
 static void
-lnet_shutdown_lndnis (void)
+lnet_shutdown_lndnis(void)
 {
 	int		i;
 	int		islo;
@@ -1004,7 +1003,7 @@ lnet_shutdown_lndnis (void)
 
 		islo = ni->ni_lnd->lnd_type == LOLND;
 
-		LASSERT (!in_interrupt ());
+		LASSERT(!in_interrupt());
 		(ni->ni_lnd->lnd_shutdown)(ni);
 
 		/* can't deref lnd anymore now; it might have unregistered
@@ -1031,7 +1030,7 @@ lnet_shutdown_lndnis (void)
 }
 
 static int
-lnet_startup_lndnis (void)
+lnet_startup_lndnis(void)
 {
 	lnd_t			*lnd;
 	struct lnet_ni		*ni;
@@ -1056,7 +1055,7 @@ lnet_startup_lndnis (void)
 		ni = list_entry(nilist.next, lnet_ni_t, ni_list);
 		lnd_type = LNET_NETTYP(LNET_NIDNET(ni->ni_nid));
 
-		LASSERT (libcfs_isknown_lnd(lnd_type));
+		LASSERT(libcfs_isknown_lnd(lnd_type));
 
 		if (lnd_type == CIBLND    ||
 		    lnd_type == OPENIBLND ||
@@ -1097,8 +1096,7 @@ lnet_startup_lndnis (void)
 		LNET_MUTEX_UNLOCK(&the_lnet.ln_lnd_mutex);
 
 		if (rc != 0) {
-			LCONSOLE_ERROR_MSG(0x105, "Error %d starting up LNI %s"
-					   "\n",
+			LCONSOLE_ERROR_MSG(0x105, "Error %d starting up LNI %s\n",
 					   rc, libcfs_lnd2str(lnd->lnd_type));
 			lnet_net_lock(LNET_LOCK_EX);
 			lnd->lnd_refcount--;
@@ -1106,7 +1104,7 @@ lnet_startup_lndnis (void)
 			goto failed;
 		}
 
-		LASSERT (ni->ni_peertimeout <= 0 || lnd->lnd_query != NULL);
+		LASSERT(ni->ni_peertimeout <= 0 || lnd->lnd_query != NULL);
 
 		list_del(&ni->ni_list);
 
@@ -1124,7 +1122,7 @@ lnet_startup_lndnis (void)
 
 		if (lnd->lnd_type == LOLND) {
 			lnet_ni_addref(ni);
-			LASSERT (the_lnet.ln_loni == NULL);
+			LASSERT(the_lnet.ln_loni == NULL);
 			the_lnet.ln_loni = ni;
 			continue;
 		}
@@ -1154,8 +1152,7 @@ lnet_startup_lndnis (void)
 
 	if (the_lnet.ln_eq_waitni != NULL && nicount > 1) {
 		lnd_type = the_lnet.ln_eq_waitni->ni_lnd->lnd_type;
-		LCONSOLE_ERROR_MSG(0x109, "LND %s can only run single-network"
-				   "\n",
+		LCONSOLE_ERROR_MSG(0x109, "LND %s can only run single-network\n",
 				   libcfs_lnd2str(lnd_type));
 		goto failed;
 	}
@@ -1201,8 +1198,7 @@ LNetInit(void)
 	LASSERT(the_lnet.ln_cpt_number > 0);
 	if (the_lnet.ln_cpt_number > LNET_CPT_MAX) {
 		/* we are under risk of consuming all lh_cookie */
-		CERROR("Can't have %d CPTs for LNet (max allowed is %d), "
-		       "please change setting of CPT-table and retry\n",
+		CERROR("Can't have %d CPTs for LNet (max allowed is %d), please change setting of CPT-table and retry\n",
 		       the_lnet.ln_cpt_number, LNET_CPT_MAX);
 		return -1;
 	}
@@ -1289,7 +1285,7 @@ LNetNIInit(lnet_pid_t requested_pid)
 
 	LNET_MUTEX_LOCK(&the_lnet.ln_api_mutex);
 
-	LASSERT (the_lnet.ln_init);
+	LASSERT(the_lnet.ln_init);
 	CDEBUG(D_OTHER, "refs %d\n", the_lnet.ln_refcount);
 
 	if (the_lnet.ln_refcount > 0) {
@@ -1356,7 +1352,7 @@ LNetNIInit(lnet_pid_t requested_pid)
  failed1:
 	lnet_unprepare();
  failed0:
-	LASSERT (rc < 0);
+	LASSERT(rc < 0);
  out:
 	LNET_MUTEX_UNLOCK(&the_lnet.ln_api_mutex);
 	return rc;
@@ -1377,13 +1373,13 @@ LNetNIFini(void)
 {
 	LNET_MUTEX_LOCK(&the_lnet.ln_api_mutex);
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (the_lnet.ln_refcount > 0);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(the_lnet.ln_refcount > 0);
 
 	if (the_lnet.ln_refcount != 1) {
 		the_lnet.ln_refcount--;
 	} else {
-		LASSERT (!the_lnet.ln_niinit_self);
+		LASSERT(!the_lnet.ln_niinit_self);
 
 		lnet_proc_fini();
 		lnet_router_checker_stop();
@@ -1424,8 +1420,8 @@ LNetCtl(unsigned int cmd, void *arg)
 	lnet_ni_t		*ni;
 	int		       rc;
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (the_lnet.ln_refcount > 0);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(the_lnet.ln_refcount > 0);
 
 	switch (cmd) {
 	case IOC_LIBCFS_GET_NI:
@@ -1595,7 +1591,7 @@ lnet_create_ping_info(void)
 		if (rc == -ENOENT)
 			break;
 
-		LASSERT (rc == 0);
+		LASSERT(rc == 0);
 	}
 
 	infosz = offsetof(lnet_ping_info_t, pi_ni[n]);
@@ -1614,7 +1610,7 @@ lnet_create_ping_info(void)
 		lnet_ni_status_t *ns = &pinfo->pi_ni[i];
 
 		rc = LNetGetId(i, &id);
-		LASSERT (rc == 0);
+		LASSERT(rc == 0);
 
 		ns->ns_nid    = id.nid;
 		ns->ns_status = LNET_NI_STATUS_UP;
@@ -1718,10 +1714,10 @@ lnet_ping_target_init(void)
 
  failed_2:
 	rc2 = LNetMEUnlink(meh);
-	LASSERT (rc2 == 0);
+	LASSERT(rc2 == 0);
  failed_1:
 	rc2 = LNetEQFree(the_lnet.ln_ping_target_eq);
-	LASSERT (rc2 == 0);
+	LASSERT(rc2 == 0);
  failed_0:
 	lnet_destroy_ping_info();
 	return rc;
@@ -1744,7 +1740,7 @@ lnet_ping_target_fini(void)
 				timeout_ms, &event, &which);
 
 		/* I expect overflow... */
-		LASSERT (rc >= 0 || rc == -EOVERFLOW);
+		LASSERT(rc >= 0 || rc == -EOVERFLOW);
 
 		if (rc == 0) {
 			/* timed out: provide a diagnostic */
@@ -1759,13 +1755,13 @@ lnet_ping_target_fini(void)
 	}
 
 	rc = LNetEQFree(the_lnet.ln_ping_target_eq);
-	LASSERT (rc == 0);
+	LASSERT(rc == 0);
 	lnet_destroy_ping_info();
 	cfs_restore_sigs(blocked);
 }
 
 int
-lnet_ping (lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_ids)
+lnet_ping(lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_ids)
 {
 	lnet_handle_eq_t     eqh;
 	lnet_handle_md_t     mdh;
@@ -1827,7 +1823,7 @@ lnet_ping (lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_i
 		/* Don't CERROR; this could be deliberate! */
 
 		rc2 = LNetMDUnlink(mdh);
-		LASSERT (rc2 == 0);
+		LASSERT(rc2 == 0);
 
 		/* NB must wait for the UNLINK event below... */
 		unlinked = 1;
@@ -1849,7 +1845,7 @@ lnet_ping (lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_i
 		       (rc2 <= 0) ? -1 : event.status,
 		       (rc2 > 0 && event.unlinked) ? " unlinked" : "");
 
-		LASSERT (rc2 != -EOVERFLOW);     /* can't miss anything */
+		LASSERT(rc2 != -EOVERFLOW);     /* can't miss anything */
 
 		if (rc2 <= 0 || event.status != 0) {
 			/* timeout or error */
@@ -1885,7 +1881,7 @@ lnet_ping (lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_i
 	}
 
 	nob = rc;
-	LASSERT (nob >= 0 && nob <= infosz);
+	LASSERT(nob >= 0 && nob <= infosz);
 
 	rc = -EPROTO;			   /* if I can't parse... */
 
@@ -1940,7 +1936,7 @@ lnet_ping (lnet_process_id_t id, int timeout_ms, lnet_process_id_t *ids, int n_i
 	rc2 = LNetEQFree(eqh);
 	if (rc2 != 0)
 		CERROR("rc2 %d\n", rc2);
-	LASSERT (rc2 == 0);
+	LASSERT(rc2 == 0);
 
  out_0:
 	LIBCFS_FREE(info, infosz);
diff --git a/drivers/staging/lustre/lnet/lnet/config.c b/drivers/staging/lustre/lnet/lnet/config.c
index 7c8b9476bfbb..af171e25a5ec 100644
--- a/drivers/staging/lustre/lnet/lnet/config.c
+++ b/drivers/staging/lustre/lnet/lnet/config.c
@@ -47,7 +47,7 @@ static int lnet_tbnob;			/* track text buf allocation */
 #define LNET_MAX_TEXTBUF_NOB     (64<<10)	/* bound allocation */
 #define LNET_SINGLE_TEXTBUF_NOB  (4<<10)
 
-void
+static void
 lnet_syntax(char *name, char *str, int offset, int width)
 {
 	static char dots[LNET_SINGLE_TEXTBUF_NOB];
@@ -64,7 +64,7 @@ lnet_syntax(char *name, char *str, int offset, int width)
 			    (width < 1) ? 0 : width - 1, dashes);
 }
 
-int
+static int
 lnet_issep(char c)
 {
 	switch (c) {
@@ -77,7 +77,7 @@ lnet_issep(char c)
 	}
 }
 
-int
+static int
 lnet_net_unique(__u32 net, struct list_head *nilist)
 {
 	struct list_head       *tmp;
@@ -108,7 +108,7 @@ lnet_ni_free(struct lnet_ni *ni)
 	LIBCFS_FREE(ni, sizeof(*ni));
 }
 
-lnet_ni_t *
+static lnet_ni_t *
 lnet_ni_alloc(__u32 net, struct cfs_expr_list *el, struct list_head *nilist)
 {
 	struct lnet_tx_queue	*tq;
@@ -365,7 +365,7 @@ lnet_parse_networks(struct list_head *nilist, char *networks)
 	return -EINVAL;
 }
 
-lnet_text_buf_t *
+static lnet_text_buf_t *
 lnet_new_text_buf(int str_len)
 {
 	lnet_text_buf_t *ltb;
@@ -394,14 +394,14 @@ lnet_new_text_buf(int str_len)
 	return ltb;
 }
 
-void
+static void
 lnet_free_text_buf(lnet_text_buf_t *ltb)
 {
 	lnet_tbnob -= ltb->ltb_size;
 	LIBCFS_FREE(ltb, ltb->ltb_size);
 }
 
-void
+static void
 lnet_free_text_bufs(struct list_head *tbs)
 {
 	lnet_text_buf_t  *ltb;
@@ -414,22 +414,7 @@ lnet_free_text_bufs(struct list_head *tbs)
 	}
 }
 
-void
-lnet_print_text_bufs(struct list_head *tbs)
-{
-	struct list_head	*tmp;
-	lnet_text_buf_t   *ltb;
-
-	list_for_each(tmp, tbs) {
-		ltb = list_entry(tmp, lnet_text_buf_t, ltb_list);
-
-		CDEBUG(D_WARNING, "%s\n", ltb->ltb_text);
-	}
-
-	CDEBUG(D_WARNING, "%d allocated\n", lnet_tbnob);
-}
-
-int
+static int
 lnet_str2tbs_sep(struct list_head *tbs, char *str)
 {
 	struct list_head	pending;
@@ -487,7 +472,7 @@ lnet_str2tbs_sep(struct list_head *tbs, char *str)
 	return 0;
 }
 
-int
+static int
 lnet_expand1tb(struct list_head *list,
 	       char *str, char *sep1, char *sep2,
 	       char *item, int itemlen)
@@ -512,7 +497,7 @@ lnet_expand1tb(struct list_head *list,
 	return 0;
 }
 
-int
+static int
 lnet_str2tbs_expand(struct list_head *tbs, char *str)
 {
 	char	      num[16];
@@ -592,7 +577,7 @@ lnet_str2tbs_expand(struct list_head *tbs, char *str)
 	return -1;
 }
 
-int
+static int
 lnet_parse_hops(char *str, unsigned int *hops)
 {
 	int     len = strlen(str);
@@ -605,7 +590,7 @@ lnet_parse_hops(char *str, unsigned int *hops)
 
 #define LNET_PRIORITY_SEPARATOR (':')
 
-int
+static int
 lnet_parse_priority(char *str, unsigned int *priority, char **token)
 {
 	int   nob;
@@ -635,7 +620,7 @@ lnet_parse_priority(char *str, unsigned int *priority, char **token)
 	return 0;
 }
 
-int
+static int
 lnet_parse_route(char *str, int *im_a_router)
 {
 	/* static scratch buffer OK (single threaded) */
@@ -778,7 +763,7 @@ lnet_parse_route(char *str, int *im_a_router)
 	return myrc;
 }
 
-int
+static int
 lnet_parse_route_tbs(struct list_head *tbs, int *im_a_router)
 {
 	lnet_text_buf_t   *ltb;
@@ -819,7 +804,7 @@ lnet_parse_routes(char *routes, int *im_a_router)
 	return rc;
 }
 
-int
+static int
 lnet_match_network_token(char *token, int len, __u32 *ipaddrs, int nip)
 {
 	LIST_HEAD(list);
@@ -838,7 +823,7 @@ lnet_match_network_token(char *token, int len, __u32 *ipaddrs, int nip)
 	return rc;
 }
 
-int
+static int
 lnet_match_network_tokens(char *net_entry, __u32 *ipaddrs, int nip)
 {
 	static char tokens[LNET_SINGLE_TEXTBUF_NOB];
@@ -895,7 +880,7 @@ lnet_match_network_tokens(char *net_entry, __u32 *ipaddrs, int nip)
 	return 1;
 }
 
-__u32
+static __u32
 lnet_netspec2net(char *netspec)
 {
 	char   *bracket = strchr(netspec, '(');
@@ -912,7 +897,7 @@ lnet_netspec2net(char *netspec)
 	return net;
 }
 
-int
+static int
 lnet_splitnets(char *source, struct list_head *nets)
 {
 	int	       offset = 0;
@@ -992,7 +977,7 @@ lnet_splitnets(char *source, struct list_head *nets)
 	}
 }
 
-int
+static int
 lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 {
 	static char	networks[LNET_SINGLE_TEXTBUF_NOB];
@@ -1112,13 +1097,13 @@ lnet_match_networks(char **networksp, char *ip2nets, __u32 *ipaddrs, int nip)
 	return count;
 }
 
-void
+static void
 lnet_ipaddr_free_enumeration(__u32 *ipaddrs, int nip)
 {
 	LIBCFS_FREE(ipaddrs, nip * sizeof(*ipaddrs));
 }
 
-int
+static int
 lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 {
 	int	up;
@@ -1187,7 +1172,7 @@ lnet_ipaddr_enumerate(__u32 **ipaddrsp)
 int
 lnet_parse_ip2nets(char **networksp, char *ip2nets)
 {
-	__u32     *ipaddrs;
+	__u32     *ipaddrs = NULL;
 	int	nip = lnet_ipaddr_enumerate(&ipaddrs);
 	int	rc;
 
diff --git a/drivers/staging/lustre/lnet/lnet/lib-eq.c b/drivers/staging/lustre/lnet/lnet/lib-eq.c
index bd45478e9948..863cc3735edf 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-eq.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-eq.c
@@ -72,8 +72,8 @@ LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
 {
 	lnet_eq_t     *eq;
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (the_lnet.ln_refcount > 0);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(the_lnet.ln_refcount > 0);
 
 	/* We need count to be a power of 2 so that when eq_{enq,deq}_seq
 	 * overflow, they don't skip entries, so the queue has the same
@@ -82,10 +82,7 @@ LNetEQAlloc(unsigned int count, lnet_eq_handler_t callback,
 	count = cfs_power2_roundup(count);
 
 	if (callback != LNET_EQ_HANDLER_NONE && count != 0) {
-		CWARN("EQ callback is guaranteed to get every event, "
-		      "do you still want to set eqcount %d for polling "
-		      "event which will have locking overhead? "
-		      "Please contact with developer to confirm\n", count);
+		CWARN("EQ callback is guaranteed to get every event, do you still want to set eqcount %d for polling event which will have locking overhead? Please contact with developer to confirm\n", count);
 	}
 
 	/* count can be 0 if only need callback, we can eliminate
@@ -287,7 +284,7 @@ lnet_eq_dequeue_event(lnet_eq_t *eq, lnet_event_t *ev)
  * EQ has been dropped due to limited space in the EQ.
  */
 int
-LNetEQGet (lnet_handle_eq_t eventq, lnet_event_t *event)
+LNetEQGet(lnet_handle_eq_t eventq, lnet_event_t *event)
 {
 	int which;
 
@@ -313,7 +310,7 @@ EXPORT_SYMBOL(LNetEQGet);
  * EQ has been dropped due to limited space in the EQ.
  */
 int
-LNetEQWait (lnet_handle_eq_t eventq, lnet_event_t *event)
+LNetEQWait(lnet_handle_eq_t eventq, lnet_event_t *event)
 {
 	int which;
 
@@ -400,8 +397,8 @@ LNetEQPoll(lnet_handle_eq_t *eventqs, int neq, int timeout_ms,
 	int	rc;
 	int	i;
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (the_lnet.ln_refcount > 0);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(the_lnet.ln_refcount > 0);
 
 	if (neq < 1)
 		return -ENOENT;
diff --git a/drivers/staging/lustre/lnet/lnet/lib-md.c b/drivers/staging/lustre/lnet/lnet/lib-md.c
index 1f386e09b530..e4d906a65635 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-md.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-md.c
@@ -52,7 +52,8 @@ lnet_md_unlink(lnet_libmd_t *md)
 
 		md->md_flags |= LNET_MD_FLAG_ZOMBIE;
 
-		/* Disassociate from ME (if any), and unlink it if it was created
+		/* Disassociate from ME (if any),
+		 * and unlink it if it was created
 		 * with LNET_UNLINK */
 		if (me != NULL) {
 			/* detach MD from portal */
@@ -109,11 +110,12 @@ lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
 
 		lmd->md_niov = niov = umd->length;
 		memcpy(lmd->md_iov.iov, umd->start,
-		       niov * sizeof (lmd->md_iov.iov[0]));
+		       niov * sizeof(lmd->md_iov.iov[0]));
 
 		for (i = 0; i < (int)niov; i++) {
 			/* We take the base address on trust */
-			if (lmd->md_iov.iov[i].iov_len <= 0) /* invalid length */
+			/* invalid length */
+			if (lmd->md_iov.iov[i].iov_len <= 0)
 				return -EINVAL;
 
 			total_length += lmd->md_iov.iov[i].iov_len;
@@ -123,18 +125,18 @@ lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
 
 		if ((umd->options & LNET_MD_MAX_SIZE) != 0 && /* max size used */
 		    (umd->max_size < 0 ||
-		     umd->max_size > total_length)) // illegal max_size
+		     umd->max_size > total_length)) /* illegal max_size */
 			return -EINVAL;
 
 	} else if ((umd->options & LNET_MD_KIOV) != 0) {
 		lmd->md_niov = niov = umd->length;
 		memcpy(lmd->md_iov.kiov, umd->start,
-		       niov * sizeof (lmd->md_iov.kiov[0]));
+		       niov * sizeof(lmd->md_iov.kiov[0]));
 
 		for (i = 0; i < (int)niov; i++) {
 			/* We take the page pointer on trust */
 			if (lmd->md_iov.kiov[i].kiov_offset +
-			    lmd->md_iov.kiov[i].kiov_len > PAGE_CACHE_SIZE )
+			    lmd->md_iov.kiov[i].kiov_len > PAGE_CACHE_SIZE)
 				return -EINVAL; /* invalid length */
 
 			total_length += lmd->md_iov.kiov[i].kiov_len;
@@ -144,7 +146,7 @@ lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
 
 		if ((umd->options & LNET_MD_MAX_SIZE) != 0 && /* max size used */
 		    (umd->max_size < 0 ||
-		     umd->max_size > total_length)) // illegal max_size
+		     umd->max_size > total_length)) /* illegal max_size */
 			return -EINVAL;
 	} else {   /* contiguous */
 		lmd->md_length = umd->length;
@@ -154,7 +156,7 @@ lnet_md_build(lnet_libmd_t *lmd, lnet_md_t *umd, int unlink)
 
 		if ((umd->options & LNET_MD_MAX_SIZE) != 0 && /* max size used */
 		    (umd->max_size < 0 ||
-		     umd->max_size > (int)umd->length)) // illegal max_size
+		     umd->max_size > (int)umd->length)) /* illegal max_size */
 			return -EINVAL;
 	}
 
@@ -206,7 +208,8 @@ lnet_md_deconstruct(lnet_libmd_t *lmd, lnet_md_t *umd)
 	 * and that's all.
 	 */
 	umd->start = lmd->md_start;
-	umd->length = ((lmd->md_options & (LNET_MD_IOVEC | LNET_MD_KIOV)) == 0) ?
+	umd->length = ((lmd->md_options &
+			(LNET_MD_IOVEC | LNET_MD_KIOV)) == 0) ?
 		      lmd->md_length : lmd->md_niov;
 	umd->threshold = lmd->md_threshold;
 	umd->max_size = lmd->md_max_size;
@@ -263,15 +266,15 @@ int
 LNetMDAttach(lnet_handle_me_t meh, lnet_md_t umd,
 	     lnet_unlink_t unlink, lnet_handle_md_t *handle)
 {
-	LIST_HEAD		(matches);
-	LIST_HEAD		(drops);
+	LIST_HEAD(matches);
+	LIST_HEAD(drops);
 	struct lnet_me		*me;
 	struct lnet_libmd	*md;
 	int			cpt;
 	int			rc;
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (the_lnet.ln_refcount > 0);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(the_lnet.ln_refcount > 0);
 
 	if (lnet_md_validate(&umd) != 0)
 		return -EINVAL;
@@ -347,8 +350,8 @@ LNetMDBind(lnet_md_t umd, lnet_unlink_t unlink, lnet_handle_md_t *handle)
 	int		cpt;
 	int		rc;
 
-	LASSERT (the_lnet.ln_init);
-	LASSERT (the_lnet.ln_refcount > 0);
+	LASSERT(the_lnet.ln_init);
+	LASSERT(the_lnet.ln_refcount > 0);
 
 	if (lnet_md_validate(&umd) != 0)
 		return -EINVAL;
@@ -416,7 +419,7 @@ EXPORT_SYMBOL(LNetMDBind);
  * \retval -ENOENT If \a mdh does not point to a valid MD object.
  */
 int
-LNetMDUnlink (lnet_handle_md_t mdh)
+LNetMDUnlink(lnet_handle_md_t mdh)
 {
 	lnet_event_t	ev;
 	lnet_libmd_t	*md;
diff --git a/drivers/staging/lustre/lnet/lnet/lib-ptl.c b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
index 91767c9e15dd..720c73be4d3c 100644
--- a/drivers/staging/lustre/lnet/lnet/lib-ptl.c
+++ b/drivers/staging/lustre/lnet/lnet/lib-ptl.c
@@ -712,7 +712,7 @@ lnet_ptl_attach_md(lnet_me_t *me, lnet_libmd_t *md,
 	lnet_ptl_unlock(ptl);
 }
 
-void
+static void
 lnet_ptl_cleanup(struct lnet_portal *ptl)
 {
 	struct lnet_match_table	*mtable;
@@ -750,7 +750,7 @@ lnet_ptl_cleanup(struct lnet_portal *ptl)
 	ptl->ptl_mtables = NULL;
 }
 
-int
+static int
 lnet_ptl_setup(struct lnet_portal *ptl, int index)
 {
 	struct lnet_match_table	*mtable;
diff --git a/drivers/staging/lustre/lnet/lnet/router.c b/drivers/staging/lustre/lnet/lnet/router.c
index ac38ad271071..b5b8fb576bfb 100644
--- a/drivers/staging/lustre/lnet/lnet/router.c
+++ b/drivers/staging/lustre/lnet/lnet/router.c
@@ -321,7 +321,7 @@ lnet_add_route(__u32 net, unsigned int hops, lnet_nid_t gateway,
 	    LNET_NETTYP(net) == LOLND ||
 	    LNET_NIDNET(gateway) == net ||
 	    hops < 1 || hops > 255)
-		return (-EINVAL);
+		return -EINVAL;
 
 	if (lnet_islocalnet(net))	       /* it's a local network */
 		return 0;		       /* ignore the route entry */
diff --git a/drivers/staging/lustre/lnet/lnet/router_proc.c b/drivers/staging/lustre/lnet/lnet/router_proc.c
index 166c1e647e2b..6e8f7e2bbcfc 100644
--- a/drivers/staging/lustre/lnet/lnet/router_proc.c
+++ b/drivers/staging/lustre/lnet/lnet/router_proc.c
@@ -90,9 +90,10 @@ enum {
 
 #define LNET_PROC_VERSION(v)	((unsigned int)((v) & LNET_PROC_VER_MASK))
 
-static int proc_call_handler(void *data, int write, loff_t *ppos, void *buffer,
-			     size_t *lenp, int (*handler)(void *data, int write,
-			     loff_t pos, void *buffer, int len))
+static int proc_call_handler(void *data, int write, loff_t *ppos,
+		void __user *buffer, size_t *lenp,
+		int (*handler)(void *data, int write,
+		loff_t pos, void __user *buffer, int len))
 {
 	int rc = handler(data, write, *ppos, buffer, *lenp);
 
@@ -109,7 +110,7 @@ static int proc_call_handler(void *data, int write, loff_t *ppos, void *buffer,
 }
 
 static int __proc_lnet_stats(void *data, int write,
-			     loff_t pos, void *buffer, int nob)
+			     loff_t pos, void __user *buffer, int nob)
 {
 	int	      rc;
 	lnet_counters_t *ctrs;
@@ -592,7 +593,7 @@ int proc_lnet_peers(struct ctl_table *table, int write, void __user *buffer,
 }
 
 static int __proc_lnet_buffers(void *data, int write,
-			       loff_t pos, void *buffer, int nob)
+			       loff_t pos, void __user *buffer, int nob)
 {
 	char	    *s;
 	char	    *tmpstr;
@@ -815,7 +816,7 @@ static struct lnet_portal_rotors	portal_rotors[] = {
 extern int portal_rotor;
 
 static int __proc_lnet_portal_rotor(void *data, int write,
-				    loff_t pos, void *buffer, int nob)
+				    loff_t pos, void __user *buffer, int nob)
 {
 	const int	buf_len	= 128;
 	char		*buf;
diff --git a/drivers/staging/lustre/lnet/selftest/brw_test.c b/drivers/staging/lustre/lnet/selftest/brw_test.c
index bcce919c0433..a94f336d578c 100644
--- a/drivers/staging/lustre/lnet/selftest/brw_test.c
+++ b/drivers/staging/lustre/lnet/selftest/brw_test.c
@@ -66,7 +66,7 @@ brw_client_fini(sfw_test_instance_t *tsi)
 	}
 }
 
-int
+static int
 brw_client_init(sfw_test_instance_t *tsi)
 {
 	sfw_session_t	 *sn = tsi->tsi_batch->bat_session;
@@ -131,7 +131,7 @@ brw_client_init(sfw_test_instance_t *tsi)
 #define BRW_MAGIC       0xeeb0eeb1eeb2eeb3ULL
 #define BRW_MSIZE       sizeof(__u64)
 
-int
+static int
 brw_inject_one_error(void)
 {
 	struct timeval tv;
@@ -147,7 +147,7 @@ brw_inject_one_error(void)
 	return brw_inject_errors--;
 }
 
-void
+static void
 brw_fill_page(struct page *pg, int pattern, __u64 magic)
 {
 	char *addr = page_address(pg);
@@ -178,7 +178,7 @@ brw_fill_page(struct page *pg, int pattern, __u64 magic)
 	return;
 }
 
-int
+static int
 brw_check_page(struct page *pg, int pattern, __u64 magic)
 {
 	char  *addr = page_address(pg);
@@ -221,7 +221,7 @@ bad_data:
 	return 1;
 }
 
-void
+static void
 brw_fill_bulk(srpc_bulk_t *bk, int pattern, __u64 magic)
 {
 	int	 i;
diff --git a/drivers/staging/lustre/lnet/selftest/console.c b/drivers/staging/lustre/lnet/selftest/console.c
index 89e1b4bd5a50..5dad9f1f9462 100644
--- a/drivers/staging/lustre/lnet/selftest/console.c
+++ b/drivers/staging/lustre/lnet/selftest/console.c
@@ -910,7 +910,7 @@ lstcon_batch_list(int index, int len, char *name_up)
 
 	list_for_each_entry(bat, &console_session.ses_bat_list, bat_link) {
 		if (index-- == 0) {
-			return copy_to_user(name_up,bat->bat_name, len) ?
+			return copy_to_user(name_up, bat->bat_name, len) ?
 			       -EFAULT: 0;
 		}
 	}
@@ -1977,7 +1977,7 @@ out:
 }
 
 srpc_service_t lstcon_acceptor_service;
-void lstcon_init_acceptor_service(void)
+static void lstcon_init_acceptor_service(void)
 {
 	/* initialize selftest console acceptor service table */
 	lstcon_acceptor_service.sv_name    = "join session";
diff --git a/drivers/staging/lustre/lnet/selftest/framework.c b/drivers/staging/lustre/lnet/selftest/framework.c
index 7e83dff2fcb4..df04ab7de835 100644
--- a/drivers/staging/lustre/lnet/selftest/framework.c
+++ b/drivers/staging/lustre/lnet/selftest/framework.c
@@ -1286,7 +1286,7 @@ sfw_handle_server_rpc(struct srpc_server_rpc *rpc)
 		goto out;
 	}
 
-	switch(sv->sv_id) {
+	switch (sv->sv_id) {
 	default:
 		LBUG ();
 	case SRPC_SERVICE_TEST:
@@ -1592,8 +1592,7 @@ sfw_post_rpc (srpc_client_rpc_t *rpc)
 	return;
 }
 
-static srpc_service_t sfw_services[] =
-{
+static srpc_service_t sfw_services[] = {
 	{
 		/* sv_id */    SRPC_SERVICE_DEBUG,
 		/* sv_name */  "debug",
diff --git a/drivers/staging/lustre/lustre/Makefile b/drivers/staging/lustre/lustre/Makefile
index d1eb0bdef06b..35d8b0b2dff4 100644
--- a/drivers/staging/lustre/lustre/Makefile
+++ b/drivers/staging/lustre/lustre/Makefile
@@ -1,2 +1,2 @@
-obj-$(CONFIG_LUSTRE_FS) += libcfs/ lvfs/ obdclass/ ptlrpc/ fld/ osc/ mgc/ \
+obj-$(CONFIG_LUSTRE_FS) += libcfs/ obdclass/ ptlrpc/ fld/ osc/ mgc/ \
 			   fid/ lov/ mdc/ lmv/ llite/ obdecho/
diff --git a/drivers/staging/lustre/lustre/fid/fid_request.c b/drivers/staging/lustre/lustre/fid/fid_request.c
index 992d07591b08..64b1d80c64b0 100644
--- a/drivers/staging/lustre/lustre/fid/fid_request.c
+++ b/drivers/staging/lustre/lustre/fid/fid_request.c
@@ -113,7 +113,7 @@ static int seq_client_rpc(struct lu_client_seq *seq,
 	if (seq->lcs_type == LUSTRE_SEQ_METADATA)
 		mdc_put_rpc_lock(exp->exp_obd->u.cli.cl_rpc_lock, NULL);
 	if (rc)
-		GOTO(out_req, rc);
+		goto out_req;
 
 	out = req_capsule_server_get(&req->rq_pill, &RMF_SEQ_RANGE);
 	*output = *out;
@@ -121,13 +121,15 @@ static int seq_client_rpc(struct lu_client_seq *seq,
 	if (!range_is_sane(output)) {
 		CERROR("%s: Invalid range received from server: "
 		       DRANGE"\n", seq->lcs_name, PRANGE(output));
-		GOTO(out_req, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out_req;
 	}
 
 	if (range_is_exhausted(output)) {
 		CERROR("%s: Range received from server is exhausted: "
 		       DRANGE"]\n", seq->lcs_name, PRANGE(output));
-		GOTO(out_req, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out_req;
 	}
 
 	CDEBUG_LIMIT(debug_mask, "%s: Allocated %s-sequence "DRANGE"]\n",
@@ -187,7 +189,7 @@ static int seq_client_alloc_meta(const struct lu_env *env,
 
 /* Allocate new sequence for client. */
 static int seq_client_alloc_seq(const struct lu_env *env,
-				struct lu_client_seq *seq, seqno_t *seqnr)
+				struct lu_client_seq *seq, u64 *seqnr)
 {
 	int rc;
 
@@ -249,7 +251,7 @@ static void seq_fid_alloc_fini(struct lu_client_seq *seq)
  * Allocate the whole seq to the caller.
  **/
 int seq_client_get_seq(const struct lu_env *env,
-		       struct lu_client_seq *seq, seqno_t *seqnr)
+		       struct lu_client_seq *seq, u64 *seqnr)
 {
 	wait_queue_t link;
 	int rc;
@@ -313,7 +315,7 @@ int seq_client_alloc_fid(const struct lu_env *env,
 		seq->lcs_fid.f_oid = seq->lcs_width;
 
 	while (1) {
-		seqno_t seqnr;
+		u64 seqnr;
 
 		if (!fid_is_zero(&seq->lcs_fid) &&
 		    fid_oid(&seq->lcs_fid) < seq->lcs_width) {
@@ -430,7 +432,7 @@ static int seq_client_proc_init(struct lu_client_seq *seq)
 	if (rc) {
 		CERROR("%s: Can't init sequence manager proc, rc %d\n",
 		       seq->lcs_name, rc);
-		GOTO(out_cleanup, rc);
+		goto out_cleanup;
 	}
 
 	return 0;
@@ -508,8 +510,10 @@ int client_fid_init(struct obd_device *obd,
 		return -ENOMEM;
 
 	OBD_ALLOC(prefix, MAX_OBD_NAME + 5);
-	if (prefix == NULL)
-		GOTO(out_free_seq, rc = -ENOMEM);
+	if (prefix == NULL) {
+		rc = -ENOMEM;
+		goto out_free_seq;
+	}
 
 	snprintf(prefix, MAX_OBD_NAME + 5, "cli-%s", obd->obd_name);
 
@@ -517,7 +521,7 @@ int client_fid_init(struct obd_device *obd,
 	rc = seq_client_init(cli->cl_seq, exp, type, prefix, NULL);
 	OBD_FREE(prefix, MAX_OBD_NAME + 5);
 	if (rc)
-		GOTO(out_free_seq, rc);
+		goto out_free_seq;
 
 	return rc;
 out_free_seq:
diff --git a/drivers/staging/lustre/lustre/fid/lproc_fid.c b/drivers/staging/lustre/lustre/fid/lproc_fid.c
index 92a27fa9667c..6a21f078fefa 100644
--- a/drivers/staging/lustre/lustre/fid/lproc_fid.c
+++ b/drivers/staging/lustre/lustre/fid/lproc_fid.c
@@ -48,7 +48,6 @@
 #include "../include/obd.h"
 #include "../include/obd_class.h"
 #include "../include/dt_object.h"
-#include "../include/md_object.h"
 #include "../include/obd_support.h"
 #include "../include/lustre_req_layout.h"
 #include "../include/lustre_fid.h"
@@ -84,8 +83,8 @@ static int lprocfs_fid_write_common(const char __user *buffer, size_t count,
 
 	/* of the form "[0x0000000240000400 - 0x000000028000400]" */
 	rc = sscanf(kernbuf, "[%llx - %llx]\n",
-		    (long long unsigned *)&tmp.lsr_start,
-		    (long long unsigned *)&tmp.lsr_end);
+		    (unsigned long long *)&tmp.lsr_start,
+		    (unsigned long long *)&tmp.lsr_end);
 	if (!range_is_sane(&tmp) || range_is_zero(&tmp) ||
 	    tmp.lsr_start < range->lsr_start || tmp.lsr_end > range->lsr_end)
 		return -EINVAL;
diff --git a/drivers/staging/lustre/lustre/fld/fld_cache.c b/drivers/staging/lustre/lustre/fld/fld_cache.c
index 759a233a7028..5d95d0b358b8 100644
--- a/drivers/staging/lustre/lustre/fld/fld_cache.c
+++ b/drivers/staging/lustre/lustre/fld/fld_cache.c
@@ -54,7 +54,6 @@
 #include "../include/lprocfs_status.h"
 
 #include "../include/dt_object.h"
-#include "../include/md_object.h"
 #include "../include/lustre_req_layout.h"
 #include "../include/lustre_fld.h"
 #include "fld_internal.h"
@@ -263,8 +262,8 @@ void fld_cache_punch_hole(struct fld_cache *cache,
 			  struct fld_cache_entry *f_new)
 {
 	const struct lu_seq_range *range = &f_new->fce_range;
-	const seqno_t new_start  = range->lsr_start;
-	const seqno_t new_end  = range->lsr_end;
+	const u64 new_start  = range->lsr_start;
+	const u64 new_end  = range->lsr_end;
 	struct fld_cache_entry *fldt;
 
 	OBD_ALLOC_GFP(fldt, sizeof(*fldt), GFP_ATOMIC);
@@ -302,9 +301,9 @@ static void fld_cache_overlap_handle(struct fld_cache *cache,
 				struct fld_cache_entry *f_new)
 {
 	const struct lu_seq_range *range = &f_new->fce_range;
-	const seqno_t new_start  = range->lsr_start;
-	const seqno_t new_end  = range->lsr_end;
-	const mdsno_t mdt = range->lsr_index;
+	const u64 new_start  = range->lsr_start;
+	const u64 new_end  = range->lsr_end;
+	const u32 mdt = range->lsr_index;
 
 	/* this is overlap case, these case are checking overlapping with
 	 * prev range only. fixup will handle overlapping with next range. */
@@ -386,8 +385,8 @@ int fld_cache_insert_nolock(struct fld_cache *cache,
 	struct fld_cache_entry *n;
 	struct list_head *head;
 	struct list_head *prev = NULL;
-	const seqno_t new_start  = f_new->fce_range.lsr_start;
-	const seqno_t new_end  = f_new->fce_range.lsr_end;
+	const u64 new_start  = f_new->fce_range.lsr_start;
+	const u64 new_end  = f_new->fce_range.lsr_end;
 	__u32 new_flags  = f_new->fce_range.lsr_flags;
 
 	/*
@@ -516,7 +515,7 @@ struct fld_cache_entry
  * lookup \a seq sequence for range in fld cache.
  */
 int fld_cache_lookup(struct fld_cache *cache,
-		     const seqno_t seq, struct lu_seq_range *range)
+		     const u64 seq, struct lu_seq_range *range)
 {
 	struct fld_cache_entry *flde;
 	struct fld_cache_entry *prev = NULL;
diff --git a/drivers/staging/lustre/lustre/fld/fld_internal.h b/drivers/staging/lustre/lustre/fld/fld_internal.h
index 5da0c1da0d39..8806b6096953 100644
--- a/drivers/staging/lustre/lustre/fld/fld_internal.h
+++ b/drivers/staging/lustre/lustre/fld/fld_internal.h
@@ -167,7 +167,7 @@ void fld_cache_delete(struct fld_cache *cache,
 void fld_cache_delete_nolock(struct fld_cache *cache,
 			     const struct lu_seq_range *range);
 int fld_cache_lookup(struct fld_cache *cache,
-		     const seqno_t seq, struct lu_seq_range *range);
+		     const u64 seq, struct lu_seq_range *range);
 
 struct fld_cache_entry*
 fld_cache_entry_lookup(struct fld_cache *cache, struct lu_seq_range *range);
diff --git a/drivers/staging/lustre/lustre/fld/fld_request.c b/drivers/staging/lustre/lustre/fld/fld_request.c
index 8e512f9c3db0..0d361ff43212 100644
--- a/drivers/staging/lustre/lustre/fld/fld_request.c
+++ b/drivers/staging/lustre/lustre/fld/fld_request.c
@@ -53,7 +53,6 @@
 #include "../include/lprocfs_status.h"
 
 #include "../include/dt_object.h"
-#include "../include/md_object.h"
 #include "../include/lustre_req_layout.h"
 #include "../include/lustre_fld.h"
 #include "../include/lustre_mdc.h"
@@ -110,15 +109,14 @@ static void fld_exit_request(struct client_obd *cli)
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 }
 
-static int fld_rrb_hash(struct lu_client_fld *fld,
-			seqno_t seq)
+static int fld_rrb_hash(struct lu_client_fld *fld, u64 seq)
 {
 	LASSERT(fld->lcf_count > 0);
 	return do_div(seq, fld->lcf_count);
 }
 
 static struct lu_fld_target *
-fld_rrb_scan(struct lu_client_fld *fld, seqno_t seq)
+fld_rrb_scan(struct lu_client_fld *fld, u64 seq)
 {
 	struct lu_fld_target *target;
 	int hash;
@@ -173,7 +171,7 @@ struct lu_fld_hash fld_hash[] = {
 };
 
 static struct lu_fld_target *
-fld_client_get_target(struct lu_client_fld *fld, seqno_t seq)
+fld_client_get_target(struct lu_client_fld *fld, u64 seq)
 {
 	struct lu_fld_target *target;
 
@@ -294,7 +292,7 @@ static int fld_client_proc_init(struct lu_client_fld *fld)
 	if (rc) {
 		CERROR("%s: Can't init FLD proc, rc %d\n",
 		       fld->lcf_name, rc);
-		GOTO(out_cleanup, rc);
+		goto out_cleanup;
 	}
 
 	return 0;
@@ -364,12 +362,12 @@ int fld_client_init(struct lu_client_fld *fld,
 	if (IS_ERR(fld->lcf_cache)) {
 		rc = PTR_ERR(fld->lcf_cache);
 		fld->lcf_cache = NULL;
-		GOTO(out, rc);
+		goto out;
 	}
 
 	rc = fld_client_proc_init(fld);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 out:
 	if (rc)
 		fld_client_fini(fld);
@@ -428,6 +426,7 @@ int fld_client_rpc(struct obd_export *exp,
 
 	ptlrpc_request_set_replen(req);
 	req->rq_request_portal = FLD_REQUEST_PORTAL;
+	req->rq_reply_portal = MDC_REPLY_PORTAL;
 	ptlrpc_at_set_req_timeout(req);
 
 	if (fld_op == FLD_LOOKUP &&
@@ -442,18 +441,20 @@ int fld_client_rpc(struct obd_export *exp,
 	if (fld_op != FLD_LOOKUP)
 		mdc_put_rpc_lock(exp->exp_obd->u.cli.cl_rpc_lock, NULL);
 	if (rc)
-		GOTO(out_req, rc);
+		goto out_req;
 
 	prange = req_capsule_server_get(&req->rq_pill, &RMF_FLD_MDFLD);
-	if (prange == NULL)
-		GOTO(out_req, rc = -EFAULT);
+	if (prange == NULL) {
+		rc = -EFAULT;
+		goto out_req;
+	}
 	*range = *prange;
 out_req:
 	ptlrpc_req_finished(req);
 	return rc;
 }
 
-int fld_client_lookup(struct lu_client_fld *fld, seqno_t seq, mdsno_t *mds,
+int fld_client_lookup(struct lu_client_fld *fld, u64 seq, u32 *mds,
 		      __u32 flags, const struct lu_env *env)
 {
 	struct lu_seq_range res = { 0 };
diff --git a/drivers/staging/lustre/lustre/fld/lproc_fld.c b/drivers/staging/lustre/lustre/fld/lproc_fld.c
index 7f6bcc63c5eb..95e7de18d2f1 100644
--- a/drivers/staging/lustre/lustre/fld/lproc_fld.c
+++ b/drivers/staging/lustre/lustre/fld/lproc_fld.c
@@ -49,7 +49,6 @@
 #include "../include/obd.h"
 #include "../include/obd_class.h"
 #include "../include/dt_object.h"
-#include "../include/md_object.h"
 #include "../include/obd_support.h"
 #include "../include/lustre_req_layout.h"
 #include "../include/lustre_fld.h"
diff --git a/drivers/staging/lustre/lustre/include/cl_object.h b/drivers/staging/lustre/lustre/include/cl_object.h
index e51cd690f907..d56c8bea89c4 100644
--- a/drivers/staging/lustre/lustre/include/cl_object.h
+++ b/drivers/staging/lustre/lustre/include/cl_object.h
@@ -98,7 +98,7 @@
  * super-class definitions.
  */
 #include "lu_object.h"
-#include "lvfs.h"
+#include "linux/lustre_compat25.h"
 #include <linux/mutex.h>
 #include <linux/radix-tree.h>
 
@@ -2508,7 +2508,7 @@ struct cl_req_operations {
 	void (*cro_attr_set)(const struct lu_env *env,
 			     const struct cl_req_slice *slice,
 			     const struct cl_object *obj,
-			     struct cl_req_attr *attr, obd_valid flags);
+			     struct cl_req_attr *attr, u64 flags);
 	/**
 	 * Called top-to-bottom from cl_req_completion() to notify layers that
 	 * transfer completed. Has to free all state allocated by
@@ -3182,7 +3182,7 @@ void cl_req_page_add  (const struct lu_env *env, struct cl_req *req,
 void cl_req_page_done (const struct lu_env *env, struct cl_page *page);
 int  cl_req_prep      (const struct lu_env *env, struct cl_req *req);
 void cl_req_attr_set  (const struct lu_env *env, struct cl_req *req,
-		       struct cl_req_attr *attr, obd_valid flags);
+		       struct cl_req_attr *attr, u64 flags);
 void cl_req_completion(const struct lu_env *env, struct cl_req *req, int ioret);
 
 /** \defgroup cl_sync_io cl_sync_io
diff --git a/drivers/staging/lustre/lustre/include/interval_tree.h b/drivers/staging/lustre/lustre/include/interval_tree.h
index 1815783edae6..bf9027d5f773 100644
--- a/drivers/staging/lustre/lustre/include/interval_tree.h
+++ b/drivers/staging/lustre/lustre/include/interval_tree.h
@@ -112,7 +112,7 @@ enum interval_iter interval_search(struct interval_node *root,
 enum interval_iter interval_iterate(struct interval_node *root,
 				    interval_callback_t func, void *data);
 enum interval_iter interval_iterate_reverse(struct interval_node *root,
-				    interval_callback_t func,void *data);
+				    interval_callback_t func, void *data);
 
 void interval_expand(struct interval_node *root,
 		     struct interval_node_extent *ext,
diff --git a/drivers/staging/lustre/lustre/include/lclient.h b/drivers/staging/lustre/lustre/include/lclient.h
index 386a36c00f57..b3b841f4d6e6 100644
--- a/drivers/staging/lustre/lustre/include/lclient.h
+++ b/drivers/staging/lustre/lustre/include/lclient.h
@@ -279,7 +279,7 @@ int ccc_req_init(const struct lu_env *env, struct cl_device *dev,
 void ccc_umount(const struct lu_env *env, struct cl_device *dev);
 int ccc_global_init(struct lu_device_type *device_type);
 void ccc_global_fini(struct lu_device_type *device_type);
-int ccc_object_init0(const struct lu_env *env,struct ccc_object *vob,
+int ccc_object_init0(const struct lu_env *env, struct ccc_object *vob,
 		     const struct cl_object_conf *conf);
 int ccc_object_init(const struct lu_env *env, struct lu_object *obj,
 		    const struct lu_object_conf *conf);
@@ -319,11 +319,12 @@ int ccc_transient_page_prep(const struct lu_env *env,
 			    struct cl_io *io);
 void ccc_lock_delete(const struct lu_env *env,
 		     const struct cl_lock_slice *slice);
-void ccc_lock_fini(const struct lu_env *env,struct cl_lock_slice *slice);
-int ccc_lock_enqueue(const struct lu_env *env,const struct cl_lock_slice *slice,
+void ccc_lock_fini(const struct lu_env *env, struct cl_lock_slice *slice);
+int ccc_lock_enqueue(const struct lu_env *env,
+		     const struct cl_lock_slice *slice,
 		     struct cl_io *io, __u32 enqflags);
-int ccc_lock_unuse(const struct lu_env *env,const struct cl_lock_slice *slice);
-int ccc_lock_wait(const struct lu_env *env,const struct cl_lock_slice *slice);
+int ccc_lock_unuse(const struct lu_env *env, const struct cl_lock_slice *slice);
+int ccc_lock_wait(const struct lu_env *env, const struct cl_lock_slice *slice);
 int ccc_lock_fits_into(const struct lu_env *env,
 		       const struct cl_lock_slice *slice,
 		       const struct cl_lock_descr *need,
@@ -348,9 +349,10 @@ int ccc_prep_size(const struct lu_env *env, struct cl_object *obj,
 		  struct cl_io *io, loff_t start, size_t count, int *exceed);
 void ccc_req_completion(const struct lu_env *env,
 			const struct cl_req_slice *slice, int ioret);
-void ccc_req_attr_set(const struct lu_env *env,const struct cl_req_slice *slice,
+void ccc_req_attr_set(const struct lu_env *env,
+		      const struct cl_req_slice *slice,
 		      const struct cl_object *obj,
-		      struct cl_req_attr *oa, obd_valid flags);
+		      struct cl_req_attr *oa, u64 flags);
 
 struct lu_device   *ccc2lu_dev      (struct ccc_device *vdv);
 struct lu_object   *ccc2lu	  (struct ccc_object *vob);
diff --git a/drivers/staging/lustre/lustre/include/linux/lprocfs_status.h b/drivers/staging/lustre/lustre/include/linux/lprocfs_status.h
deleted file mode 100644
index cb76b5d6f71a..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lprocfs_status.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/linux/lprocfs_status.h
- *
- * Top level header file for LProc SNMP
- *
- * Author: Hariharan Thantry thantry@users.sourceforge.net
- */
-#ifndef _LINUX_LPROCFS_SNMP_H
-#define _LINUX_LPROCFS_SNMP_H
-
-#ifndef _LPROCFS_SNMP_H
-#error Do not #include this file directly. #include <lprocfs_status.h> instead
-#endif
-
-#include <linux/proc_fs.h>
-#include <linux/seq_file.h>
-#include <linux/smp.h>
-#include <linux/rwsem.h>
-#include "../../../include/linux/libcfs/libcfs.h"
-#include <linux/statfs.h>
-
-
-#endif /* LPROCFS_SNMP_H */
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_acl.h b/drivers/staging/lustre/lustre/include/linux/lustre_acl.h
deleted file mode 100644
index b17273d403bb..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_acl.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * Copyright (c) 2011, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/lustre/include/lustre_acl.h
- *
- * MDS data structures.
- * See also lustre_idl.h for wire formats of requests.
- */
-
-#ifndef _LUSTRE_LINUX_ACL_H
-#define _LUSTRE_LINUX_ACL_H
-
-#ifndef	_LUSTRE_ACL_H
-#error	Should not include directly. use #include <lustre_acl.h> instead
-#endif
-
-#include <linux/fs.h>
-#include <linux/dcache.h>
-
-#include <linux/posix_acl_xattr.h>
-#define LUSTRE_POSIX_ACL_MAX_ENTRIES	32
-#define LUSTRE_POSIX_ACL_MAX_SIZE					\
-	(sizeof(posix_acl_xattr_header) +				\
-	 LUSTRE_POSIX_ACL_MAX_ENTRIES * sizeof(posix_acl_xattr_entry))
-
-#include "lustre_intent.h"
-#include <linux/xattr.h> /* XATTR_{REPLACE,CREATE} */
-
-#ifndef LUSTRE_POSIX_ACL_MAX_SIZE
-# define LUSTRE_POSIX_ACL_MAX_SIZE   0
-#endif
-
-#endif /* _LUSTRE_LINUX_ACL_H */
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_common.h b/drivers/staging/lustre/lustre/include/linux/lustre_common.h
deleted file mode 100644
index d1783a33d8ca..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_common.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#ifndef LUSTRE_COMMON_H
-#define LUSTRE_COMMON_H
-
-#include <linux/sched.h>
-
-static inline int cfs_cleanup_group_info(void)
-{
-	struct group_info *ginfo;
-
-	ginfo = groups_alloc(0);
-	if (!ginfo)
-		return -ENOMEM;
-
-	set_current_groups(ginfo);
-	put_group_info(ginfo);
-
-	return 0;
-}
-
-#define ll_inode_blksize(a)		(1<<(a)->i_blkbits)
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h b/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
index 8621bac5058b..e94ab343ab25 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
+++ b/drivers/staging/lustre/lustre/include/linux/lustre_compat25.h
@@ -39,7 +39,6 @@
 
 #include <linux/fs_struct.h>
 #include <linux/namei.h>
-#include "../../../include/linux/libcfs/linux/portals_compat25.h"
 
 #include "lustre_patchless_compat.h"
 
@@ -99,17 +98,19 @@ static inline void ll_set_fs_pwd(struct fs_struct *fs, struct vfsmount *mnt,
 #define FS_HAS_FIEMAP			(0)
 #endif
 
-#define ll_vfs_rmdir(dir,entry,mnt)	     vfs_rmdir(dir,entry)
-#define ll_vfs_mkdir(inode,dir,mnt,mode)	vfs_mkdir(inode,dir,mode)
-#define ll_vfs_link(old,mnt,dir,new,mnt1)       vfs_link(old,dir,new)
-#define ll_vfs_unlink(inode,entry,mnt)	  vfs_unlink(inode,entry)
-#define ll_vfs_mknod(dir,entry,mnt,mode,dev)    vfs_mknod(dir,entry,mode,dev)
-#define ll_security_inode_unlink(dir,entry,mnt) security_inode_unlink(dir,entry)
+#define ll_vfs_rmdir(dir, entry, mnt)	     vfs_rmdir(dir, entry)
+#define ll_vfs_mkdir(inode, dir, mnt, mode)	vfs_mkdir(inode, dir, mode)
+#define ll_vfs_link(old, mnt, dir, new, mnt1)       vfs_link(old, dir, new)
+#define ll_vfs_unlink(inode, entry, mnt)	  vfs_unlink(inode, entry)
+#define ll_vfs_mknod(dir, entry, mnt, mode, dev) \
+		     vfs_mknod(dir, entry, mode, dev)
+#define ll_security_inode_unlink(dir, entry, mnt) \
+				 security_inode_unlink(dir, entry)
 #define ll_vfs_rename(old, old_dir, mnt, new, new_dir, mnt1) \
 		vfs_rename(old, old_dir, new, new_dir, NULL, 0)
 
-#define cfs_bio_io_error(a,b)   bio_io_error((a))
-#define cfs_bio_endio(a,b,c)    bio_endio((a),(c))
+#define cfs_bio_io_error(a, b)   bio_io_error((a))
+#define cfs_bio_endio(a, b, c)    bio_endio((a), (c))
 
 #define cfs_fs_pwd(fs)       ((fs)->pwd.dentry)
 #define cfs_fs_mnt(fs)       ((fs)->pwd.mnt)
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_dlm.h b/drivers/staging/lustre/lustre/include/linux/lustre_dlm.h
deleted file mode 100644
index 207df03f6149..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_dlm.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef _LINUX_LUSTRE_DLM_H__
-#define _LINUX_LUSTRE_DLM_H__
-
-#ifndef _LUSTRE_DLM_H__
-#error Do not #include this file directly. #include <lprocfs_status.h> instead
-#endif
-
-# include <linux/proc_fs.h>
-#  include <asm/processor.h>
-#  include <linux/bit_spinlock.h>
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_fsfilt.h b/drivers/staging/lustre/lustre/include/linux/lustre_fsfilt.h
deleted file mode 100644
index d5c97beb66f5..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_fsfilt.h
+++ /dev/null
@@ -1,171 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/linux/lustre_fsfilt.h
- *
- * Filesystem interface helper.
- */
-
-#ifndef _LINUX_LUSTRE_FSFILT_H
-#define _LINUX_LUSTRE_FSFILT_H
-
-#ifndef _LUSTRE_FSFILT_H
-#error Do not #include this file directly. #include <lustre_fsfilt.h> instead
-#endif
-
-
-#include "../obd.h"
-#include "../obd_class.h"
-
-typedef void (*fsfilt_cb_t)(struct obd_device *obd, __u64 last_rcvd,
-			    void *data, int error);
-
-struct fsfilt_operations {
-	struct list_head fs_list;
-	struct module *fs_owner;
-	char   *fs_type;
-	char   *(* fs_getlabel)(struct super_block *sb);
-	void   *(* fs_start)(struct inode *inode, int op, void *desc_private,
-			     int logs);
-	int     (* fs_commit)(struct inode *inode, void *handle,int force_sync);
-	int     (* fs_map_inode_pages)(struct inode *inode, struct page **page,
-				       int pages, unsigned long *blocks,
-				       int create, struct mutex *sem);
-	int     (* fs_write_record)(struct file *, void *, int size, loff_t *,
-				    int force_sync);
-	int     (* fs_read_record)(struct file *, void *, int size, loff_t *);
-	int     (* fs_setup)(struct super_block *sb);
-};
-
-extern int fsfilt_register_ops(struct fsfilt_operations *fs_ops);
-extern void fsfilt_unregister_ops(struct fsfilt_operations *fs_ops);
-extern struct fsfilt_operations *fsfilt_get_ops(const char *type);
-extern void fsfilt_put_ops(struct fsfilt_operations *fs_ops);
-
-static inline char *fsfilt_get_label(struct obd_device *obd,
-				     struct super_block *sb)
-{
-	if (obd->obd_fsops->fs_getlabel == NULL)
-		return NULL;
-	if (obd->obd_fsops->fs_getlabel(sb)[0] == '\0')
-		return NULL;
-
-	return obd->obd_fsops->fs_getlabel(sb);
-}
-
-#define FSFILT_OP_UNLINK		1
-#define FSFILT_OP_CANCEL_UNLINK	 10
-
-#define __fsfilt_check_slow(obd, start, msg)			      \
-do {								      \
-	if (time_before(jiffies, start + 15 * HZ))		\
-		break;						    \
-	else if (time_before(jiffies, start + 30 * HZ))	   \
-		CDEBUG(D_VFSTRACE, "%s: slow %s %lus\n", obd->obd_name,   \
-		       msg, (jiffies-start) / HZ);		    \
-	else if (time_before(jiffies, start + DISK_TIMEOUT * HZ)) \
-		CWARN("%s: slow %s %lus\n", obd->obd_name, msg,	   \
-		      (jiffies - start) / HZ);			\
-	else							      \
-		CERROR("%s: slow %s %lus\n", obd->obd_name, msg,	  \
-		       (jiffies - start) / HZ);		       \
-} while (0)
-
-#define fsfilt_check_slow(obd, start, msg)	      \
-do {						    \
-	__fsfilt_check_slow(obd, start, msg);	   \
-	start = jiffies;				\
-} while (0)
-
-static inline void *fsfilt_start_log(struct obd_device *obd,
-				     struct inode *inode, int op,
-				     struct obd_trans_info *oti, int logs)
-{
-	unsigned long now = jiffies;
-	void *parent_handle = oti ? oti->oti_handle : NULL;
-	void *handle;
-
-	handle = obd->obd_fsops->fs_start(inode, op, parent_handle, logs);
-	CDEBUG(D_INFO, "started handle %p (%p)\n", handle, parent_handle);
-
-	if (oti != NULL) {
-		if (parent_handle == NULL) {
-			oti->oti_handle = handle;
-		} else if (handle != parent_handle) {
-			CERROR("mismatch: parent %p, handle %p, oti %p\n",
-			       parent_handle, handle, oti);
-			LBUG();
-		}
-	}
-	fsfilt_check_slow(obd, now, "journal start");
-	return handle;
-}
-
-static inline int fsfilt_commit(struct obd_device *obd, struct inode *inode,
-				void *handle, int force_sync)
-{
-	unsigned long now = jiffies;
-	int rc = obd->obd_fsops->fs_commit(inode, handle, force_sync);
-	CDEBUG(D_INFO, "committing handle %p\n", handle);
-
-	fsfilt_check_slow(obd, now, "journal start");
-
-	return rc;
-}
-
-static inline int fsfilt_read_record(struct obd_device *obd, struct file *file,
-				     void *buf, loff_t size, loff_t *offs)
-{
-	return obd->obd_fsops->fs_read_record(file, buf, size, offs);
-}
-
-static inline int fsfilt_write_record(struct obd_device *obd, struct file *file,
-				      void *buf, loff_t size, loff_t *offs,
-				      int force_sync)
-{
-	return obd->obd_fsops->fs_write_record(file, buf, size,offs,force_sync);
-}
-
-static inline int fsfilt_setup(struct obd_device *obd, struct super_block *fs)
-{
-	if (obd->obd_fsops->fs_setup)
-		return obd->obd_fsops->fs_setup(fs);
-	return 0;
-}
-
-
-
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_handles.h b/drivers/staging/lustre/lustre/include/linux/lustre_handles.h
deleted file mode 100644
index 459b2380600f..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_handles.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LINUX_LUSTRE_HANDLES_H_
-#define __LINUX_LUSTRE_HANDLES_H_
-
-#ifndef __LUSTRE_HANDLES_H_
-#error Do not #include this file directly. #include <lustre_handles.h> instead
-#endif
-
-#include <asm/types.h>
-#include <asm/atomic.h>
-#include <linux/list.h>
-#include <linux/spinlock.h>
-#include <linux/types.h>
-
-#include <linux/rcupdate.h> /* for rcu_head{} */
-typedef struct rcu_head cfs_rcu_head_t;
-
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_lib.h b/drivers/staging/lustre/lustre/include/linux/lustre_lib.h
deleted file mode 100644
index 0a4c65ae9242..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_lib.h
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2001, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/linux/lustre_lib.h
- *
- * Basic Lustre library routines.
- */
-
-#ifndef _LINUX_LUSTRE_LIB_H
-#define _LINUX_LUSTRE_LIB_H
-
-#ifndef _LUSTRE_LIB_H
-#error Do not #include this file directly. #include <lustre_lib.h> instead
-#endif
-
-# include <linux/rwsem.h>
-# include <linux/sched.h>
-# include <linux/signal.h>
-# include <linux/types.h>
-#include "lustre_compat25.h"
-#include "lustre_common.h"
-
-#ifndef LP_POISON
-# define LI_POISON 0x5a5a5a5a
-#if BITS_PER_LONG > 32
-# define LL_POISON 0x5a5a5a5a5a5a5a5aL
-#else
-# define LL_POISON 0x5a5a5a5aL
-#endif
-# define LP_POISON ((void *)LL_POISON)
-#endif
-
-/* This macro is only for compatibility reasons with older Linux Lustre user
- * tools. New ioctls should NOT use this macro as the ioctl "size". Instead
- * the ioctl should get a "size" argument which is the actual data type used
- * by the ioctl, to ensure the ioctl interface is versioned correctly. */
-#define OBD_IOC_DATA_TYPE	       long
-
-#define LUSTRE_FATAL_SIGS (sigmask(SIGKILL) | sigmask(SIGINT) |		\
-			   sigmask(SIGTERM) | sigmask(SIGQUIT) |	       \
-			   sigmask(SIGALRM))
-
-/* initialize ost_lvb according to inode */
-static inline void inode_init_lvb(struct inode *inode, struct ost_lvb *lvb)
-{
-	lvb->lvb_size = i_size_read(inode);
-	lvb->lvb_blocks = inode->i_blocks;
-	lvb->lvb_mtime = LTIME_S(inode->i_mtime);
-	lvb->lvb_atime = LTIME_S(inode->i_atime);
-	lvb->lvb_ctime = LTIME_S(inode->i_ctime);
-}
-
-#endif /* _LUSTRE_LIB_H */
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_lite.h b/drivers/staging/lustre/lustre/include/linux/lustre_lite.h
index 99eed4987635..a7658a99a08d 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_lite.h
+++ b/drivers/staging/lustre/lustre/include/linux/lustre_lite.h
@@ -52,7 +52,6 @@
 
 #include <linux/rbtree.h>
 #include "../../include/linux/lustre_compat25.h"
-#include "../../include/linux/lustre_common.h"
 #include <linux/pagemap.h>
 
 /* lprocfs.c */
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_log.h b/drivers/staging/lustre/lustre/include/linux/lustre_log.h
deleted file mode 100644
index e9c8e56737d2..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_log.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/linux/lustre_log.h
- *
- * Generic infrastructure for managing a collection of logs.
- * These logs are used for:
- *  - orphan recovery: OST adds record on create
- *  - mtime/size consistency: the OST adds a record on first write
- *  - open/unlinked objects: OST adds a record on destroy
- *
- *  - mds unlink log: the MDS adds an entry upon delete
- *
- *  - raid1 replication log between OST's
- *  - MDS replication logs
- */
-
-#ifndef _LINUX_LUSTRE_LOG_H
-#define _LINUX_LUSTRE_LOG_H
-
-#ifndef _LUSTRE_LOG_H
-#error Do not #include this file directly. #include <lustre_log.h> instead
-#endif
-
-#define LUSTRE_LOG_SERVER
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_net.h b/drivers/staging/lustre/lustre/include/linux/lustre_net.h
deleted file mode 100644
index 05de4d87db9a..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_net.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef _LINUX_LUSTRE_NET_H
-#define _LINUX_LUSTRE_NET_H
-
-#ifndef _LUSTRE_NET_H
-#error Do not #include this file directly. #include <lustre_net.h> instead
-#endif
-
-#include <linux/workqueue.h>
-
-/* XXX Liang: should be moved to other header instead of here */
-#ifndef WITH_GROUP_INFO
-#define WITH_GROUP_INFO
-#endif
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_quota.h b/drivers/staging/lustre/lustre/include/linux/lustre_quota.h
deleted file mode 100644
index a39505014c82..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lustre_quota.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef _LINUX_LUSTRE_QUOTA_H
-#define _LINUX_LUSTRE_QUOTA_H
-
-#ifndef _LUSTRE_QUOTA_H
-#error Do not #include this file directly. #include <lustre_quota.h> instead
-#endif
-
-#include <linux/fs.h>
-#include <linux/quota.h>
-#include <linux/quotaops.h>
-
-#endif /* _LUSTRE_QUOTA_H */
diff --git a/drivers/staging/lustre/lustre/include/linux/lvfs.h b/drivers/staging/lustre/lustre/include/linux/lvfs.h
deleted file mode 100644
index dd1be9c39829..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lvfs.h
+++ /dev/null
@@ -1,134 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/linux/lvfs.h
- *
- * lustre VFS/process permission interface
- */
-
-#ifndef __LINUX_LVFS_H__
-#define __LINUX_LVFS_H__
-
-#ifndef __LVFS_H__
-#error Do not #include this file directly. #include <lvfs.h> instead
-#endif
-
-#include "lustre_compat25.h"
-#include "lustre_common.h"
-#include "lvfs_linux.h"
-
-#define LLOG_LVFS
-
-/* simple.c */
-
-struct lvfs_ucred {
-	kuid_t		luc_uid;
-	kgid_t		luc_gid;
-	kuid_t		luc_fsuid;
-	kgid_t		luc_fsgid;
-	kernel_cap_t	luc_cap;
-	__u32		   luc_umask;
-	struct group_info      *luc_ginfo;
-	struct md_identity     *luc_identity;
-};
-
-struct lvfs_callback_ops {
-	struct dentry *(*l_fid2dentry)(__u64 id_ino, __u32 gen, __u64 gr, void *data);
-};
-
-#define OBD_RUN_CTXT_MAGIC      0xC0FFEEAA
-#define OBD_CTXT_DEBUG	  /* development-only debugging */
-struct lvfs_run_ctxt {
-	struct vfsmount	 *pwdmnt;
-	struct dentry	   *pwd;
-	mm_segment_t	     fs;
-	struct lvfs_ucred	luc;
-	int		      ngroups;
-	struct lvfs_callback_ops cb_ops;
-	struct group_info       *group_info;
-	struct dt_device	*dt;
-#ifdef OBD_CTXT_DEBUG
-	__u32		    magic;
-#endif
-};
-
-#ifdef OBD_CTXT_DEBUG
-#define OBD_SET_CTXT_MAGIC(ctxt) (ctxt)->magic = OBD_RUN_CTXT_MAGIC
-#else
-#define OBD_SET_CTXT_MAGIC(ctxt) do {} while(0)
-#endif
-
-
-int lustre_rename(struct dentry *dir, struct vfsmount *mnt, char *oldname,
-		  char *newname);
-
-static inline void l_dput(struct dentry *de)
-{
-	if (!de || IS_ERR(de))
-		return;
-	//shrink_dcache_parent(de);
-	LASSERT(d_count(de) > 0);
-	dput(de);
-}
-
-/* We need to hold the inode semaphore over the dcache lookup itself, or we
- * run the risk of entering the filesystem lookup path concurrently on SMP
- * systems, and instantiating two inodes for the same entry.  We still
- * protect against concurrent addition/removal races with the DLM locking.
- */
-static inline struct dentry *ll_lookup_one_len(const char *fid_name,
-					       struct dentry *dparent,
-					       int fid_namelen)
-{
-	struct dentry *dchild;
-
-	mutex_lock(&dparent->d_inode->i_mutex);
-	dchild = lookup_one_len(fid_name, dparent, fid_namelen);
-	mutex_unlock(&dparent->d_inode->i_mutex);
-
-	if (IS_ERR(dchild) || dchild->d_inode == NULL)
-		return dchild;
-
-	if (is_bad_inode(dchild->d_inode)) {
-		CERROR("bad inode returned %lu/%u\n",
-		       dchild->d_inode->i_ino, dchild->d_inode->i_generation);
-		dput(dchild);
-		dchild = ERR_PTR(-ENOENT);
-	}
-	return dchild;
-}
-
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/linux/lvfs_linux.h b/drivers/staging/lustre/lustre/include/linux/lvfs_linux.h
deleted file mode 100644
index fff22ec1f869..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/lvfs_linux.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LVFS_LINUX_H__
-#define __LVFS_LINUX_H__
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/fs.h>
-#include <linux/namei.h>
-#include <linux/sched.h>
-
-#include "../lvfs.h"
-
-struct lvfs_run_ctxt;
-struct file *l_dentry_open(struct lvfs_run_ctxt *, struct dentry *,
-			     int flags);
-
-struct l_linux_dirent {
-	struct list_head      lld_list;
-	ino_t	   lld_ino;
-	unsigned long   lld_off;
-	char	    lld_name[LL_FID_NAMELEN];
-};
-struct l_readdir_callback {
-	struct l_linux_dirent *lrc_dirent;
-	struct list_head	    *lrc_list;
-};
-
-#endif /*  __LVFS_LINUX_H__ */
diff --git a/drivers/staging/lustre/lustre/include/linux/obd.h b/drivers/staging/lustre/lustre/include/linux/obd.h
index 2a3a88cbb192..9d7e28ace42d 100644
--- a/drivers/staging/lustre/lustre/include/linux/obd.h
+++ b/drivers/staging/lustre/lustre/include/linux/obd.h
@@ -48,7 +48,7 @@
 # include <linux/sched.h>  /* for struct task_struct, for current.h */
 # include <linux/proc_fs.h>
 # include <linux/mount.h>
-#include "lustre_intent.h"
+#include "../lustre_intent.h"
 
 struct ll_iattr {
 	struct iattr	iattr;
diff --git a/drivers/staging/lustre/lustre/include/linux/obd_class.h b/drivers/staging/lustre/lustre/include/linux/obd_class.h
deleted file mode 100644
index 021ead6639fc..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/obd_class.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 1999, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef __LINUX_CLASS_OBD_H
-#define __LINUX_CLASS_OBD_H
-
-#ifndef __CLASS_OBD_H
-#error Do not #include this file directly. #include <obd_class.h> instead
-#endif
-
-#include <asm/uaccess.h>
-#include <linux/types.h>
-#include <linux/fs.h>
-#include <linux/time.h>
-#include <linux/timer.h>
-
-/* obdo.c */
-void obdo_from_la(struct obdo *dst, struct lu_attr *la, __u64 valid);
-void la_from_obdo(struct lu_attr *la, struct obdo *dst, obd_flag valid);
-void obdo_refresh_inode(struct inode *dst, struct obdo *src, obd_flag valid);
-void obdo_to_inode(struct inode *dst, struct obdo *src, obd_flag valid);
-#define ll_inode_flags(inode)	 (inode->i_flags)
-
-
-#endif /* __LINUX_OBD_CLASS_H */
diff --git a/drivers/staging/lustre/lustre/include/linux/obd_support.h b/drivers/staging/lustre/lustre/include/linux/obd_support.h
deleted file mode 100644
index ea03b8434f62..000000000000
--- a/drivers/staging/lustre/lustre/include/linux/obd_support.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 1999, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#ifndef _LINUX_OBD_SUPPORT
-#define _LINUX_OBD_SUPPORT
-
-#ifndef _OBD_SUPPORT
-#error Do not #include this file directly. #include <obd_support.h> instead
-#endif
-
-#ifdef CONFIG_X86
-#include <asm/cpufeature.h>
-#endif
-#include <asm/processor.h>
-#include <linux/seq_file.h>
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/highmem.h>
-#include <linux/swap.h>
-#include "../../include/linux/lustre_compat25.h"
-#include "lustre_common.h"
-#include "../../../include/linux/libcfs/libcfs.h"
-#include "../lustre/lustre_idl.h"
-
-
-# include <linux/types.h>
-# include <linux/blkdev.h>
-# include "../lvfs.h"
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/lprocfs_status.h b/drivers/staging/lustre/lustre/include/lprocfs_status.h
index 16b3d6887ff6..ccb6cd42a67d 100644
--- a/drivers/staging/lustre/lustre/include/lprocfs_status.h
+++ b/drivers/staging/lustre/lustre/include/lprocfs_status.h
@@ -42,7 +42,11 @@
 #ifndef _LPROCFS_SNMP_H
 #define _LPROCFS_SNMP_H
 
-#include "linux/lprocfs_status.h"
+#include <linux/proc_fs.h>
+#include <linux/seq_file.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
+
 #include "lustre/lustre_idl.h"
 
 struct lprocfs_vars {
@@ -351,7 +355,7 @@ struct obd_histogram;
 
 /* Days / hours / mins / seconds format */
 struct dhms {
-	int d,h,m,s;
+	int d, h, m, s;
 };
 static inline void s2dhms(struct dhms *ts, time_t secs)
 {
@@ -607,7 +611,7 @@ extern int lprocfs_rd_atomic(struct seq_file *m, void *data);
 extern int lprocfs_wr_atomic(struct file *file, const char __user *buffer,
 			     unsigned long count, void *data);
 extern int lprocfs_rd_uint(struct seq_file *m, void *data);
-extern int lprocfs_wr_uint(struct file *file, const char *buffer,
+extern int lprocfs_wr_uint(struct file *file, const char __user *buffer,
 			   unsigned long count, void *data);
 extern int lprocfs_rd_uuid(struct seq_file *m, void *data);
 extern int lprocfs_rd_name(struct seq_file *m, void *data);
@@ -673,7 +677,7 @@ extern int lprocfs_seq_release(struct inode *, struct file *);
 	     up_read(&(obd)->u.cli.cl_sem); \
 	     return -ENODEV;		    \
 	}				       \
-} while(0)
+} while (0)
 #define LPROCFS_CLIMP_EXIT(obd)		 \
 	up_read(&(obd)->u.cli.cl_sem);
 
@@ -864,7 +868,8 @@ static inline void lprocfs_free_md_stats(struct obd_device *obddev)
 struct obd_export;
 static inline int lprocfs_add_clear_entry(struct obd_export *exp)
 { return 0; }
-static inline int lprocfs_exp_setup(struct obd_export *exp,lnet_nid_t *peer_nid,
+static inline int lprocfs_exp_setup(struct obd_export *exp,
+				    lnet_nid_t *peer_nid,
 				    int *newnid)
 { return 0; }
 static inline int lprocfs_exp_cleanup(struct obd_export *exp)
diff --git a/drivers/staging/lustre/lustre/include/lu_object.h b/drivers/staging/lustre/lustre/include/lu_object.h
index d5c368bab5bd..6015ee5c4b64 100644
--- a/drivers/staging/lustre/lustre/include/lu_object.h
+++ b/drivers/staging/lustre/lustre/include/lu_object.h
@@ -404,11 +404,11 @@ struct lu_attr {
 	/** size in bytes */
 	__u64	  la_size;
 	/** modification time in seconds since Epoch */
-	obd_time       la_mtime;
+	s64	  la_mtime;
 	/** access time in seconds since Epoch */
-	obd_time       la_atime;
+	s64	  la_atime;
 	/** change time in seconds since Epoch */
-	obd_time       la_ctime;
+	s64	  la_ctime;
 	/** 512-byte blocks allocated to object */
 	__u64	  la_blocks;
 	/** permission bits and file type */
@@ -1146,8 +1146,8 @@ struct lu_context_key {
 	struct __##mod##__dummy_fini {;} /* semicolon catcher */
 
 #define LU_KEY_INIT_FINI(mod, type)   \
-	LU_KEY_INIT(mod,type);	\
-	LU_KEY_FINI(mod,type)
+	LU_KEY_INIT(mod, type);	\
+	LU_KEY_FINI(mod, type)
 
 #define LU_CONTEXT_KEY_DEFINE(mod, tags)		\
 	struct lu_context_key mod##_thread_key = {      \
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
index 757146273724..7b7457cf70e3 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_idl.h
@@ -128,7 +128,7 @@
 //#define PTLBD_BULK_PORTAL	      21
 #define MDS_SETATTR_PORTAL	     22
 #define MDS_READPAGE_PORTAL	    23
-#define MDS_MDS_PORTAL		 24
+#define OUT_PORTAL		    24
 
 #define MGC_REPLY_PORTAL	       25
 #define MGS_REQUEST_PORTAL	     26
@@ -165,22 +165,6 @@
 #define LUSTRE_LOG_VERSION  0x00050000
 #define LUSTRE_MGS_VERSION  0x00060000
 
-typedef __u32 mdsno_t;
-typedef __u64 seqno_t;
-typedef __u64 obd_id;
-typedef __u64 obd_seq;
-typedef __s64 obd_time;
-typedef __u64 obd_size;
-typedef __u64 obd_off;
-typedef __u64 obd_blocks;
-typedef __u64 obd_valid;
-typedef __u32 obd_blksize;
-typedef __u32 obd_mode;
-typedef __u32 obd_uid;
-typedef __u32 obd_gid;
-typedef __u32 obd_flag;
-typedef __u32 obd_count;
-
 /**
  * Describes a range of sequence, lsr_start is included but lsr_end is
  * not in the range.
@@ -337,49 +321,6 @@ enum lma_incompat {
 };
 #define LMA_INCOMPAT_SUPP	(LMAI_AGENT | LMAI_REMOTE_PARENT)
 
-extern void lustre_lma_swab(struct lustre_mdt_attrs *lma);
-extern void lustre_lma_init(struct lustre_mdt_attrs *lma,
-			    const struct lu_fid *fid, __u32 incompat);
-/**
- * SOM on-disk attributes stored in a separate xattr.
- */
-struct som_attrs {
-	/** Bitfield for supported data in this structure. For future use. */
-	__u32	som_compat;
-
-	/** Incompat feature list. The supported feature mask is available in
-	 * SOM_INCOMPAT_SUPP */
-	__u32	som_incompat;
-
-	/** IO Epoch SOM attributes belongs to */
-	__u64	som_ioepoch;
-	/** total file size in objects */
-	__u64	som_size;
-	/** total fs blocks in objects */
-	__u64	som_blocks;
-	/** mds mount id the size is valid for */
-	__u64	som_mountid;
-};
-extern void lustre_som_swab(struct som_attrs *attrs);
-
-#define SOM_INCOMPAT_SUPP 0x0
-
-/**
- * HSM on-disk attributes stored in a separate xattr.
- */
-struct hsm_attrs {
-	/** Bitfield for supported data in this structure. For future use. */
-	__u32	hsm_compat;
-
-	/** HSM flags, see hsm_flags enum below */
-	__u32	hsm_flags;
-	/** backend archive id associated with the file */
-	__u64	hsm_arch_id;
-	/** version associated with the last archiving, if any */
-	__u64	hsm_arch_ver;
-};
-extern void lustre_hsm_swab(struct hsm_attrs *attrs);
-
 /**
  * fid constants
  */
@@ -413,7 +354,7 @@ static inline void fid_zero(struct lu_fid *fid)
 	memset(fid, 0, sizeof(*fid));
 }
 
-static inline obd_id fid_ver_oid(const struct lu_fid *fid)
+static inline __u64 fid_ver_oid(const struct lu_fid *fid)
 {
 	return ((__u64)fid_ver(fid) << 32 | fid_oid(fid));
 }
@@ -477,7 +418,7 @@ enum dot_lustre_oid {
 	FID_OID_DOT_LUSTRE_OBF = 2UL,
 };
 
-static inline int fid_seq_is_mdt0(obd_seq seq)
+static inline int fid_seq_is_mdt0(__u64 seq)
 {
 	return (seq == FID_SEQ_OST_MDT0);
 }
@@ -487,7 +428,7 @@ static inline int fid_seq_is_mdt(const __u64 seq)
 	return seq == FID_SEQ_OST_MDT0 || seq >= FID_SEQ_NORMAL;
 };
 
-static inline int fid_seq_is_echo(obd_seq seq)
+static inline int fid_seq_is_echo(__u64 seq)
 {
 	return (seq == FID_SEQ_ECHO);
 }
@@ -497,7 +438,7 @@ static inline int fid_is_echo(const struct lu_fid *fid)
 	return fid_seq_is_echo(fid_seq(fid));
 }
 
-static inline int fid_seq_is_llog(obd_seq seq)
+static inline int fid_seq_is_llog(__u64 seq)
 {
 	return (seq == FID_SEQ_LLOG);
 }
@@ -597,13 +538,13 @@ static inline int fid_is_norm(const struct lu_fid *fid)
 }
 
 /* convert an OST objid into an IDIF FID SEQ number */
-static inline obd_seq fid_idif_seq(obd_id id, __u32 ost_idx)
+static inline __u64 fid_idif_seq(__u64 id, __u32 ost_idx)
 {
 	return FID_SEQ_IDIF | (ost_idx << 16) | ((id >> 32) & 0xffff);
 }
 
 /* convert a packed IDIF FID into an OST objid */
-static inline obd_id fid_idif_id(obd_seq seq, __u32 oid, __u32 ver)
+static inline __u64 fid_idif_id(__u64 seq, __u32 oid, __u32 ver)
 {
 	return ((__u64)ver << 48) | ((seq & 0xffff) << 32) | oid;
 }
@@ -615,7 +556,7 @@ static inline __u32 fid_idif_ost_idx(const struct lu_fid *fid)
 }
 
 /* extract OST sequence (group) from a wire ost_id (id/seq) pair */
-static inline obd_seq ostid_seq(const struct ost_id *ostid)
+static inline __u64 ostid_seq(const struct ost_id *ostid)
 {
 	if (fid_seq_is_mdt0(ostid->oi.oi_seq))
 		return FID_SEQ_OST_MDT0;
@@ -630,7 +571,7 @@ static inline obd_seq ostid_seq(const struct ost_id *ostid)
 }
 
 /* extract OST objid from a wire ost_id (id/seq) pair */
-static inline obd_id ostid_id(const struct ost_id *ostid)
+static inline __u64 ostid_id(const struct ost_id *ostid)
 {
 	if (fid_seq_is_mdt0(ostid_seq(ostid)))
 		return ostid->oi.oi_id & IDIF_OID_MASK;
@@ -1371,8 +1312,9 @@ extern void lustre_swab_ptlrpc_body(struct ptlrpc_body *pb);
 #define CLIENT_CONNECT_MDT_REQD (OBD_CONNECT_IBITS | OBD_CONNECT_FID | \
 				 OBD_CONNECT_FULL20)
 
-#define OBD_OCD_VERSION(major,minor,patch,fix) (((major)<<24) + ((minor)<<16) +\
-						((patch)<<8) + (fix))
+#define OBD_OCD_VERSION(major, minor, patch, fix) (((major)<<24) + \
+						  ((minor)<<16) + \
+						  ((patch)<<8) + (fix))
 #define OBD_OCD_VERSION_MAJOR(version) ((int)((version)>>24)&255)
 #define OBD_OCD_VERSION_MINOR(version) ((int)((version)>>16)&255)
 #define OBD_OCD_VERSION_PATCH(version) ((int)((version)>>8)&255)
@@ -1857,9 +1799,9 @@ extern void lustre_swab_niobuf_remote (struct niobuf_remote *nbr);
 
 struct ost_lvb_v1 {
 	__u64		lvb_size;
-	obd_time	lvb_mtime;
-	obd_time	lvb_atime;
-	obd_time	lvb_ctime;
+	__s64		lvb_mtime;
+	__s64		lvb_atime;
+	__s64		lvb_ctime;
 	__u64		lvb_blocks;
 };
 
@@ -1867,9 +1809,9 @@ extern void lustre_swab_ost_lvb_v1(struct ost_lvb_v1 *lvb);
 
 struct ost_lvb {
 	__u64		lvb_size;
-	obd_time	lvb_mtime;
-	obd_time	lvb_atime;
-	obd_time	lvb_ctime;
+	__s64		lvb_mtime;
+	__s64		lvb_atime;
+	__s64		lvb_ctime;
 	__u64		lvb_blocks;
 	__u32		lvb_mtime_ns;
 	__u32		lvb_atime_ns;
@@ -2250,9 +2192,9 @@ struct mdt_body {
 	struct lustre_handle handle;
 	__u64	  valid;
 	__u64	  size;   /* Offset, in the case of MDS_READPAGE */
-       obd_time	mtime;
-       obd_time	atime;
-       obd_time	ctime;
+	__s64	  mtime;
+	__s64	  atime;
+	__s64	  ctime;
 	__u64	  blocks; /* XID, in the case of MDS_READPAGE */
 	__u64	  ioepoch;
 	__u64	       t_state; /* transient file state defined in
@@ -2335,9 +2277,9 @@ struct mdt_rec_setattr {
 	__u32	   sa_gid;
 	__u64	   sa_size;
 	__u64	   sa_blocks;
-	obd_time	sa_mtime;
-	obd_time	sa_atime;
-	obd_time	sa_ctime;
+	__s64	   sa_mtime;
+	__s64	   sa_atime;
+	__s64	   sa_ctime;
 	__u32	   sa_attr_flags;
 	__u32	   sa_mode;
 	__u32	   sa_bias;      /* some operation flags */
@@ -2466,7 +2408,7 @@ struct mdt_rec_create {
 	struct lu_fid   cr_fid1;
 	struct lu_fid   cr_fid2;
 	struct lustre_handle cr_old_handle; /* handle in case of open replay */
-	obd_time	cr_time;
+	__s64	   cr_time;
 	__u64	   cr_rdev;
 	__u64	   cr_ioepoch;
 	__u64	   cr_padding_1;   /* rr_blocks */
@@ -2506,7 +2448,7 @@ struct mdt_rec_link {
 	__u32	   lk_suppgid2_h;
 	struct lu_fid   lk_fid1;
 	struct lu_fid   lk_fid2;
-	obd_time	lk_time;
+	__s64	   lk_time;
 	__u64	   lk_padding_1;   /* rr_atime */
 	__u64	   lk_padding_2;   /* rr_ctime */
 	__u64	   lk_padding_3;   /* rr_size */
@@ -2533,7 +2475,7 @@ struct mdt_rec_unlink {
 	__u32	   ul_suppgid2_h;
 	struct lu_fid   ul_fid1;
 	struct lu_fid   ul_fid2;
-	obd_time	ul_time;
+	__s64	   ul_time;
 	__u64	   ul_padding_2;   /* rr_atime */
 	__u64	   ul_padding_3;   /* rr_ctime */
 	__u64	   ul_padding_4;   /* rr_size */
@@ -2560,7 +2502,7 @@ struct mdt_rec_rename {
 	__u32	   rn_suppgid2_h;
 	struct lu_fid   rn_fid1;
 	struct lu_fid   rn_fid2;
-	obd_time	rn_time;
+	__s64	   rn_time;
 	__u64	   rn_padding_1;   /* rr_atime */
 	__u64	   rn_padding_2;   /* rr_ctime */
 	__u64	   rn_padding_3;   /* rr_size */
@@ -2590,7 +2532,7 @@ struct mdt_rec_setxattr {
 	__u32	   sx_padding_2;
 	__u32	   sx_padding_3;
 	__u64	   sx_valid;
-	obd_time	sx_time;
+	__s64	   sx_time;
 	__u64	   sx_padding_5;   /* rr_ctime */
 	__u64	   sx_padding_6;   /* rr_size */
 	__u64	   sx_padding_7;   /* rr_blocks */
@@ -2623,9 +2565,9 @@ struct mdt_rec_reint {
 	__u32	   rr_suppgid2_h;
 	struct lu_fid   rr_fid1;
 	struct lu_fid   rr_fid2;
-	obd_time	rr_mtime;
-	obd_time	rr_atime;
-	obd_time	rr_ctime;
+	__s64	   rr_mtime;
+	__s64	   rr_atime;
+	__s64	   rr_ctime;
 	__u64	   rr_size;
 	__u64	   rr_blocks;
 	__u32	   rr_bias;
@@ -2871,7 +2813,7 @@ extern void lustre_swab_ldlm_request (struct ldlm_request *rq);
 
 /* If LDLM_ENQUEUE, 1 slot is already occupied, 1 is available.
  * Otherwise, 2 are available. */
-#define ldlm_request_bufsize(count,type)				\
+#define ldlm_request_bufsize(count, type)				\
 ({								      \
 	int _avail = LDLM_LOCKREQ_HANDLES;			      \
 	_avail -= (type == LDLM_ENQUEUE ? LDLM_ENQUEUE_CANCEL_OFF : 0); \
@@ -2979,8 +2921,8 @@ struct cfg_marker {
 	__u32	     cm_flags;
 	__u32	     cm_vers;       /* lustre release version number */
 	__u32	     cm_padding;    /* 64 bit align */
-	obd_time	  cm_createtime; /*when this record was first created */
-	obd_time	  cm_canceltime; /*when this record is no longer valid*/
+	__s64	     cm_createtime; /*when this record was first created */
+	__s64	     cm_canceltime; /*when this record is no longer valid*/
 	char	      cm_tgtname[MTI_NAME_MAXLEN];
 	char	      cm_comment[MTI_NAME_MAXLEN];
 };
@@ -3084,16 +3026,16 @@ struct llog_logid_rec {
 
 struct llog_unlink_rec {
 	struct llog_rec_hdr	lur_hdr;
-	obd_id			lur_oid;
-	obd_count		lur_oseq;
-	obd_count		lur_count;
+	__u64			lur_oid;
+	__u32			lur_oseq;
+	__u32			lur_count;
 	struct llog_rec_tail	lur_tail;
 } __attribute__((packed));
 
 struct llog_unlink64_rec {
 	struct llog_rec_hdr	lur_hdr;
 	struct lu_fid		lur_fid;
-	obd_count		lur_count; /* to destroy the lost precreated */
+	__u32			lur_count; /* to destroy the lost precreated */
 	__u32			lur_padding1;
 	__u64			lur_padding2;
 	__u64			lur_padding3;
@@ -3238,7 +3180,7 @@ enum llog_flag {
 
 struct llog_log_hdr {
 	struct llog_rec_hdr     llh_hdr;
-	obd_time		llh_timestamp;
+	__s64		   llh_timestamp;
 	__u32		   llh_count;
 	__u32		   llh_bitmap_offset;
 	__u32		   llh_size;
@@ -3296,25 +3238,25 @@ struct llogd_conn_body {
 
 /* Note: 64-bit types are 64-bit aligned in structure */
 struct obdo {
-	obd_valid	       o_valid;	/* hot fields in this obdo */
-	struct ost_id	   o_oi;
-	obd_id		  o_parent_seq;
-	obd_size		o_size;	 /* o_size-o_blocks == ost_lvb */
-	obd_time		o_mtime;
-	obd_time		o_atime;
-	obd_time		o_ctime;
-	obd_blocks	      o_blocks;       /* brw: cli sent cached bytes */
-	obd_size		o_grant;
+	__u64		o_valid;	/* hot fields in this obdo */
+	struct ost_id	o_oi;
+	__u64		o_parent_seq;
+	__u64		o_size;	 /* o_size-o_blocks == ost_lvb */
+	__s64		o_mtime;
+	__s64		o_atime;
+	__s64		o_ctime;
+	__u64		o_blocks;       /* brw: cli sent cached bytes */
+	__u64		o_grant;
 
 	/* 32-bit fields start here: keep an even number of them via padding */
-	obd_blksize	     o_blksize;      /* optimal IO blocksize */
-	obd_mode		o_mode;	 /* brw: cli sent cache remain */
-	obd_uid		 o_uid;
-	obd_gid		 o_gid;
-	obd_flag		o_flags;
-	obd_count	       o_nlink;	/* brw: checksum */
-	obd_count	       o_parent_oid;
-	obd_count		o_misc;		/* brw: o_dropped */
+	__u32		o_blksize;      /* optimal IO blocksize */
+	__u32		o_mode;	 /* brw: cli sent cache remain */
+	__u32		o_uid;
+	__u32		o_gid;
+	__u32		o_flags;
+	__u32		o_nlink;	/* brw: checksum */
+	__u32		o_parent_oid;
+	__u32		o_misc;		/* brw: o_dropped */
 
 	__u64		   o_ioepoch;      /* epoch in ost writes */
 	__u32		   o_stripe_idx;   /* holds stripe idx */
@@ -3363,7 +3305,7 @@ static inline void lustre_get_wire_obdo(struct obd_connect_data *ocd,
 					struct obdo *lobdo,
 					const struct obdo *wobdo)
 {
-	obd_flag local_flags = 0;
+	__u32 local_flags = 0;
 
 	if (lobdo->o_valid & OBD_MD_FLFLAGS)
 		 local_flags = lobdo->o_flags & OBD_FL_LOCAL_MASK;
@@ -3401,7 +3343,7 @@ struct ll_fiemap_info_key {
 };
 
 extern void lustre_swab_ost_body (struct ost_body *b);
-extern void lustre_swab_ost_last_id(obd_id *id);
+extern void lustre_swab_ost_last_id(__u64 *id);
 extern void lustre_swab_fiemap(struct ll_user_fiemap *fiemap);
 
 extern void lustre_swab_lov_user_md_v1(struct lov_user_md_v1 *lum);
diff --git a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
index a69b27a78042..89794fdfec9d 100644
--- a/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
+++ b/drivers/staging/lustre/lustre/include/lustre/lustre_user.h
@@ -841,7 +841,7 @@ struct ioc_data_version {
 				version. Dirty caches are left unchanged. */
 
 #ifndef offsetof
-# define offsetof(typ,memb)     ((unsigned long)((char *)&(((typ *)0)->memb)))
+# define offsetof(typ, memb)     ((unsigned long)((char *)&(((typ *)0)->memb)))
 #endif
 
 #define dot_lustre_name ".lustre"
@@ -997,12 +997,25 @@ static inline void *hur_data(struct hsm_user_request *hur)
 	return &(hur->hur_user_item[hur->hur_request.hr_itemcount]);
 }
 
-/** Compute the current length of the provided hsm_user_request. */
-static inline int hur_len(struct hsm_user_request *hur)
+/**
+ * Compute the current length of the provided hsm_user_request.  This returns -1
+ * instead of an errno because ssize_t is defined to be only [ -1, SSIZE_MAX ]
+ *
+ * return -1 on bounds check error.
+ */
+static inline ssize_t hur_len(struct hsm_user_request *hur)
 {
-	return offsetof(struct hsm_user_request,
-			hur_user_item[hur->hur_request.hr_itemcount]) +
-		hur->hur_request.hr_data_len;
+	__u64	size;
+
+	/* can't overflow a __u64 since hr_itemcount is only __u32 */
+	size = offsetof(struct hsm_user_request, hur_user_item[0]) +
+		(__u64)hur->hur_request.hr_itemcount *
+		sizeof(hur->hur_user_item[0]) + hur->hur_request.hr_data_len;
+
+	if (size != (ssize_t)size)
+		return -1;
+
+	return size;
 }
 
 /****** HSM RPCs to copytool *****/
@@ -1061,8 +1074,7 @@ static inline char *hai_dump_data_field(struct hsm_action_item *hai,
 	ptr = buffer;
 	sz = len;
 	data_len = hai->hai_len - sizeof(*hai);
-	for (i = 0 ; (i < data_len) && (sz > 0) ; i++)
-	{
+	for (i = 0 ; (i < data_len) && (sz > 0) ; i++) {
 		int cnt;
 
 		cnt = snprintf(ptr, sz, "%.2X",
@@ -1098,7 +1110,7 @@ static inline int cfs_size_round (int val)
 #endif
 
 /* Return pointer to first hai in action list */
-static inline struct hsm_action_item * hai_zero(struct hsm_action_list *hal)
+static inline struct hsm_action_item *hai_zero(struct hsm_action_list *hal)
 {
 	return (struct hsm_action_item *)(hal->hal_fsname +
 					  cfs_size_round(strlen(hal-> \
@@ -1106,7 +1118,7 @@ static inline struct hsm_action_item * hai_zero(struct hsm_action_list *hal)
 							 + 1));
 }
 /* Return pointer to next hai */
-static inline struct hsm_action_item * hai_next(struct hsm_action_item *hai)
+static inline struct hsm_action_item *hai_next(struct hsm_action_item *hai)
 {
 	return (struct hsm_action_item *)((char *)hai +
 					  cfs_size_round(hai->hai_len));
diff --git a/drivers/staging/lustre/lustre/include/lustre_acl.h b/drivers/staging/lustre/lustre/include/lustre_acl.h
index 0b6ea0782229..aa4cfa7b749d 100644
--- a/drivers/staging/lustre/lustre/include/lustre_acl.h
+++ b/drivers/staging/lustre/lustre/include/lustre_acl.h
@@ -37,6 +37,13 @@
 #ifndef _LUSTRE_ACL_H
 #define _LUSTRE_ACL_H
 
-#include "linux/lustre_acl.h"
+#include <linux/fs.h>
+#include <linux/dcache.h>
+#include <linux/posix_acl_xattr.h>
+
+#define LUSTRE_POSIX_ACL_MAX_ENTRIES	32
+#define LUSTRE_POSIX_ACL_MAX_SIZE					\
+	(sizeof(posix_acl_xattr_header) +				\
+	 LUSTRE_POSIX_ACL_MAX_ENTRIES * sizeof(posix_acl_xattr_entry))
 
 #endif
diff --git a/drivers/staging/lustre/lustre/include/lustre_cfg.h b/drivers/staging/lustre/lustre/include/lustre_cfg.h
index 03017fe49d46..7b385b87261a 100644
--- a/drivers/staging/lustre/lustre/include/lustre_cfg.h
+++ b/drivers/staging/lustre/lustre/include/lustre_cfg.h
@@ -290,4 +290,4 @@ static inline int lustre_cfg_sanity_check(void *buf, int len)
 
 /** @} cfg */
 
-#endif // _LUSTRE_CFG_H
+#endif /* _LUSTRE_CFG_H */
diff --git a/drivers/staging/lustre/lustre/include/lustre_disk.h b/drivers/staging/lustre/lustre/include/lustre_disk.h
index 7f191eed2a81..515b835ce14d 100644
--- a/drivers/staging/lustre/lustre/include/lustre_disk.h
+++ b/drivers/staging/lustre/lustre/include/lustre_disk.h
@@ -545,4 +545,4 @@ int mgc_fsname2resid(char *fsname, struct ldlm_res_id *res_id, int type);
 
 /** @} disk */
 
-#endif // _LUSTRE_DISK_H
+#endif /* _LUSTRE_DISK_H */
diff --git a/drivers/staging/lustre/lustre/include/lustre_dlm.h b/drivers/staging/lustre/lustre/include/lustre_dlm.h
index 30b1812f4bf6..14ac46f45fd1 100644
--- a/drivers/staging/lustre/lustre/include/lustre_dlm.h
+++ b/drivers/staging/lustre/lustre/include/lustre_dlm.h
@@ -48,8 +48,6 @@
 #ifndef _LUSTRE_DLM_H__
 #define _LUSTRE_DLM_H__
 
-#include "linux/lustre_dlm.h"
-
 #include "lustre_lib.h"
 #include "lustre_net.h"
 #include "lustre_import.h"
@@ -1072,7 +1070,7 @@ extern char *ldlm_it2str(int it);
 	    ((libcfs_debug & (mask)) != 0 &&			    \
 	     (libcfs_subsystem_debug & DEBUG_SUBSYSTEM) != 0))	  \
 		_ldlm_lock_debug(lock, msgdata, fmt, ##a);	      \
-} while(0)
+} while (0)
 
 void _ldlm_lock_debug(struct ldlm_lock *lock,
 		      struct libcfs_debug_msg_data *data,
diff --git a/drivers/staging/lustre/lustre/include/lustre_export.h b/drivers/staging/lustre/lustre/include/lustre_export.h
index e5f8e86cd887..9c06a49f12a4 100644
--- a/drivers/staging/lustre/lustre/include/lustre_export.h
+++ b/drivers/staging/lustre/lustre/include/lustre_export.h
@@ -125,14 +125,14 @@ struct nid_stat {
 #define nidstat_getref(nidstat)						\
 do {									   \
 	atomic_inc(&(nidstat)->nid_exp_ref_count);			 \
-} while(0)
+} while (0)
 
 #define nidstat_putref(nidstat)						\
 do {									   \
 	atomic_dec(&(nidstat)->nid_exp_ref_count);			 \
 	LASSERTF(atomic_read(&(nidstat)->nid_exp_ref_count) >= 0,	  \
 		 "stat %p nid_exp_ref_count < 0\n", nidstat);		  \
-} while(0)
+} while (0)
 
 enum obd_option {
 	OBD_OPT_FORCE =	 0x0001,
diff --git a/drivers/staging/lustre/lustre/include/lustre_fid.h b/drivers/staging/lustre/lustre/include/lustre_fid.h
index ffb00f171240..2d6fbb4b1b39 100644
--- a/drivers/staging/lustre/lustre/include/lustre_fid.h
+++ b/drivers/staging/lustre/lustre/include/lustre_fid.h
@@ -469,7 +469,7 @@ void seq_client_flush(struct lu_client_seq *seq);
 int seq_client_alloc_fid(const struct lu_env *env, struct lu_client_seq *seq,
 			 struct lu_fid *fid);
 int seq_client_get_seq(const struct lu_env *env, struct lu_client_seq *seq,
-		       seqno_t *seqnr);
+		       u64 *seqnr);
 int seq_site_fini(const struct lu_env *env, struct seq_server_site *ss);
 /* Fids common stuff */
 int fid_is_local(const struct lu_env *env,
diff --git a/drivers/staging/lustre/lustre/include/lustre_fld.h b/drivers/staging/lustre/lustre/include/lustre_fld.h
index ce6330f9bb50..64c504849a22 100644
--- a/drivers/staging/lustre/lustre/include/lustre_fld.h
+++ b/drivers/staging/lustre/lustre/include/lustre_fld.h
@@ -137,15 +137,14 @@ void fld_client_fini(struct lu_client_fld *fld);
 
 void fld_client_flush(struct lu_client_fld *fld);
 
-int fld_client_lookup(struct lu_client_fld *fld, seqno_t seq, mdsno_t *mds,
+int fld_client_lookup(struct lu_client_fld *fld, u64 seq, u32 *mds,
 		      __u32 flags, const struct lu_env *env);
 
 int fld_client_create(struct lu_client_fld *fld,
 		      struct lu_seq_range *range,
 		      const struct lu_env *env);
 
-int fld_client_delete(struct lu_client_fld *fld,
-		      seqno_t seq,
+int fld_client_delete(struct lu_client_fld *fld, u64 seq,
 		      const struct lu_env *env);
 
 int fld_client_add_target(struct lu_client_fld *fld,
diff --git a/drivers/staging/lustre/lustre/include/lustre_fsfilt.h b/drivers/staging/lustre/lustre/include/lustre_fsfilt.h
deleted file mode 100644
index 9749c3f5568e..000000000000
--- a/drivers/staging/lustre/lustre/include/lustre_fsfilt.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2005, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/lustre_fsfilt.h
- *
- * Filesystem interface helper.
- */
-
-#ifndef _LUSTRE_FSFILT_H
-#define _LUSTRE_FSFILT_H
-
-#include "../include/linux/lustre_fsfilt.h"
-
-#define LU221_BAD_TIME (0x80000000U + 24 * 3600)
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/lustre_handles.h b/drivers/staging/lustre/lustre/include/lustre_handles.h
index 94989c5e361f..726bbd3eaf55 100644
--- a/drivers/staging/lustre/lustre/include/lustre_handles.h
+++ b/drivers/staging/lustre/lustre/include/lustre_handles.h
@@ -42,7 +42,11 @@
  * @{
  */
 
-#include "linux/lustre_handles.h"
+#include <linux/atomic.h>
+#include <linux/list.h>
+#include <linux/rcupdate.h>
+#include <linux/spinlock.h>
+#include <linux/types.h>
 
 #include "../../include/linux/libcfs/libcfs.h"
 
@@ -69,7 +73,7 @@ struct portals_handle {
 	struct portals_handle_ops	*h_ops;
 
 	/* newly added fields to handle the RCU issue. -jxiong */
-	cfs_rcu_head_t			h_rcu;
+	struct rcu_head			h_rcu;
 	spinlock_t			h_lock;
 	unsigned int			h_size:31;
 	unsigned int			h_in:1;
@@ -84,7 +88,7 @@ void class_handle_hash(struct portals_handle *,
 void class_handle_unhash(struct portals_handle *);
 void class_handle_hash_back(struct portals_handle *);
 void *class_handle2object(__u64 cookie);
-void class_handle_free_cb(cfs_rcu_head_t *);
+void class_handle_free_cb(struct rcu_head *rcu);
 int class_handle_init(void);
 void class_handle_cleanup(void);
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_idmap.h b/drivers/staging/lustre/lustre/include/lustre_idmap.h
deleted file mode 100644
index 5624b8bae919..000000000000
--- a/drivers/staging/lustre/lustre/include/lustre_idmap.h
+++ /dev/null
@@ -1,104 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/lustre/include/lustre_idmap.h
- *
- * MDS data structures.
- * See also lustre_idl.h for wire formats of requests.
- */
-
-#ifndef _LUSTRE_IDMAP_H
-#define _LUSTRE_IDMAP_H
-
-/** \defgroup idmap idmap
- *
- * @{
- */
-
-#include "../../include/linux/libcfs/libcfs.h"
-
-#define CFS_NGROUPS_PER_BLOCK   ((int)(PAGE_CACHE_SIZE / sizeof(gid_t)))
-
-#define CFS_GROUP_AT(gi, i) \
-	((gi)->blocks[(i) / CFS_NGROUPS_PER_BLOCK][(i) % CFS_NGROUPS_PER_BLOCK])
-
-enum {
-	CFS_IC_NOTHING     = 0,    /* convert nothing */
-	CFS_IC_ALL	 = 1,    /* convert all items */
-	CFS_IC_MAPPED      = 2,    /* convert mapped uid/gid */
-	CFS_IC_UNMAPPED    = 3     /* convert unmapped uid/gid */
-};
-
-#define  CFS_IDMAP_NOTFOUND     (-1)
-
-#define CFS_IDMAP_HASHSIZE      32
-
-enum lustre_idmap_idx {
-	RMT_UIDMAP_IDX,
-	LCL_UIDMAP_IDX,
-	RMT_GIDMAP_IDX,
-	LCL_GIDMAP_IDX,
-	CFS_IDMAP_N_HASHES
-};
-
-struct lustre_idmap_table {
-	spinlock_t	lit_lock;
-	struct list_head	lit_idmaps[CFS_IDMAP_N_HASHES][CFS_IDMAP_HASHSIZE];
-};
-
-struct lu_ucred;
-
-extern void lustre_groups_from_list(struct group_info *ginfo, gid_t *glist);
-extern void lustre_groups_sort(struct group_info *group_info);
-extern int lustre_in_group_p(struct lu_ucred *mu, gid_t grp);
-
-extern int lustre_idmap_add(struct lustre_idmap_table *t,
-			    uid_t ruid, uid_t luid,
-			    gid_t rgid, gid_t lgid);
-extern int lustre_idmap_del(struct lustre_idmap_table *t,
-			    uid_t ruid, uid_t luid,
-			    gid_t rgid, gid_t lgid);
-extern int lustre_idmap_lookup_uid(struct lu_ucred *mu,
-				   struct lustre_idmap_table *t,
-				   int reverse, uid_t uid);
-extern int lustre_idmap_lookup_gid(struct lu_ucred *mu,
-				   struct lustre_idmap_table *t,
-				   int reverse, gid_t gid);
-extern struct lustre_idmap_table *lustre_idmap_init(void);
-extern void lustre_idmap_fini(struct lustre_idmap_table *t);
-
-/** @} idmap */
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/lustre_import.h b/drivers/staging/lustre/lustre/include/lustre_import.h
index 8304a55b92f9..51f3e98f94e2 100644
--- a/drivers/staging/lustre/lustre/include/lustre_import.h
+++ b/drivers/staging/lustre/lustre/include/lustre_import.h
@@ -103,9 +103,9 @@ enum lustre_imp_state {
 };
 
 /** Returns test string representation of numeric import state \a state */
-static inline char * ptlrpc_import_state_name(enum lustre_imp_state state)
+static inline char *ptlrpc_import_state_name(enum lustre_imp_state state)
 {
-	static char* import_state_names[] = {
+	static char *import_state_names[] = {
 		"<UNKNOWN>", "CLOSED",  "NEW", "DISCONN",
 		"CONNECTING", "REPLAY", "REPLAY_LOCKS", "REPLAY_WAIT",
 		"RECOVER", "FULL", "EVICTED",
@@ -346,21 +346,24 @@ static inline unsigned int at_timeout2est(unsigned int val)
 	return (max((val << 2) / 5, 5U) - 4);
 }
 
-static inline void at_reset(struct adaptive_timeout *at, int val) {
+static inline void at_reset(struct adaptive_timeout *at, int val)
+{
 	spin_lock(&at->at_lock);
 	at->at_current = val;
 	at->at_worst_ever = val;
 	at->at_worst_time = get_seconds();
 	spin_unlock(&at->at_lock);
 }
-static inline void at_init(struct adaptive_timeout *at, int val, int flags) {
+static inline void at_init(struct adaptive_timeout *at, int val, int flags)
+{
 	memset(at, 0, sizeof(*at));
 	spin_lock_init(&at->at_lock);
 	at->at_flags = flags;
 	at_reset(at, val);
 }
 extern unsigned int at_min;
-static inline int at_get(struct adaptive_timeout *at) {
+static inline int at_get(struct adaptive_timeout *at)
+{
 	return (at->at_current > at_min) ? at->at_current : at_min;
 }
 int at_measured(struct adaptive_timeout *at, unsigned int val);
diff --git a/drivers/staging/lustre/lustre/include/linux/lustre_intent.h b/drivers/staging/lustre/lustre/include/lustre_intent.h
index c491d52d86a2..c491d52d86a2 100644
--- a/drivers/staging/lustre/lustre/include/linux/lustre_intent.h
+++ b/drivers/staging/lustre/lustre/include/lustre_intent.h
diff --git a/drivers/staging/lustre/lustre/include/lustre_lib.h b/drivers/staging/lustre/lustre/include/lustre_lib.h
index de493fabab46..12c7590e61fa 100644
--- a/drivers/staging/lustre/lustre/include/lustre_lib.h
+++ b/drivers/staging/lustre/lustre/include/lustre_lib.h
@@ -46,27 +46,36 @@
  * @{
  */
 
+#include <linux/sched.h>
+#include <linux/signal.h>
+#include <linux/types.h>
 #include "../../include/linux/libcfs/libcfs.h"
 #include "lustre/lustre_idl.h"
 #include "lustre_ver.h"
 #include "lustre_cfg.h"
-#include "linux/lustre_lib.h"
 
 /* target.c */
+struct kstatfs;
 struct ptlrpc_request;
 struct obd_export;
 struct lu_target;
 struct l_wait_info;
 #include "lustre_ha.h"
 #include "lustre_net.h"
-#include "lvfs.h"
 
+#define LI_POISON 0x5a5a5a5a
+#if BITS_PER_LONG > 32
+# define LL_POISON 0x5a5a5a5a5a5a5a5aL
+#else
+# define LL_POISON 0x5a5a5a5aL
+#endif
+#define LP_POISON ((void *)LL_POISON)
 
 int target_pack_pool_reply(struct ptlrpc_request *req);
 int do_set_info_async(struct obd_import *imp,
 		      int opcode, int version,
-		      obd_count keylen, void *key,
-		      obd_count vallen, void *val,
+		      u32 keylen, void *key,
+		      u32 vallen, void *val,
 		      struct ptlrpc_request_set *set);
 
 #define OBD_RECOVERY_MAX_TIME (obd_timeout * 18) /* b13079 */
@@ -133,8 +142,8 @@ struct obd_ioctl_data {
 	struct obdo ioc_obdo1;
 	struct obdo ioc_obdo2;
 
-	obd_size ioc_count;
-	obd_off  ioc_offset;
+	u64	 ioc_count;
+	u64	 ioc_offset;
 	__u32    ioc_dev;
 	__u32    ioc_command;
 
@@ -269,6 +278,8 @@ static inline void obd_ioctl_freedata(char *buf, int len)
  * we change _IOR to _IOWR so BSD will copyin obd_ioctl_data
  * for us. Does this change affect Linux?  (XXX Liang)
  */
+#define OBD_IOC_DATA_TYPE long
+
 #define OBD_IOC_CREATE		 _IOWR('f', 101, OBD_IOC_DATA_TYPE)
 #define OBD_IOC_DESTROY		_IOW ('f', 104, OBD_IOC_DATA_TYPE)
 #define OBD_IOC_PREALLOCATE	    _IOWR('f', 105, OBD_IOC_DATA_TYPE)
@@ -508,6 +519,10 @@ struct l_wait_info {
 
 #define LWI_INTR(cb, data)  LWI_TIMEOUT_INTR(0, NULL, cb, data)
 
+#define LUSTRE_FATAL_SIGS (sigmask(SIGKILL) | sigmask(SIGINT) |		\
+			   sigmask(SIGTERM) | sigmask(SIGQUIT) |	\
+			   sigmask(SIGALRM))
+
 
 /*
  * wait for @condition to become true, but no longer than timeout, specified
diff --git a/drivers/staging/lustre/lustre/include/lustre_linkea.h b/drivers/staging/lustre/lustre/include/lustre_linkea.h
deleted file mode 100644
index 500ace30cfbf..000000000000
--- a/drivers/staging/lustre/lustre/include/lustre_linkea.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; if not, write to the
- * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 021110-1307, USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2013, Intel Corporation.
- * Use is subject to license terms.
- *
- * Author: di wang <di.wang@intel.com>
- */
-
-struct linkea_data {
-	/**
-	 * Buffer to keep link EA body.
-	 */
-	struct lu_buf		*ld_buf;
-	/**
-	 * The matched header, entry and its length in the EA
-	 */
-	struct link_ea_header	*ld_leh;
-	struct link_ea_entry	*ld_lee;
-	int			ld_reclen;
-};
-
-int linkea_data_new(struct linkea_data *ldata, struct lu_buf *buf);
-int linkea_init(struct linkea_data *ldata);
-void linkea_entry_unpack(const struct link_ea_entry *lee, int *reclen,
-			 struct lu_name *lname, struct lu_fid *pfid);
-int linkea_add_buf(struct linkea_data *ldata, const struct lu_name *lname,
-		   const struct lu_fid *pfid);
-void linkea_del_buf(struct linkea_data *ldata, const struct lu_name *lname);
-int linkea_links_find(struct linkea_data *ldata, const struct lu_name *lname,
-		      const struct lu_fid  *pfid);
-
-#define LINKEA_NEXT_ENTRY(ldata)	\
-	(struct link_ea_entry *)((char *)ldata.ld_lee + ldata.ld_reclen)
-
-#define LINKEA_FIRST_ENTRY(ldata)	\
-	(struct link_ea_entry *)(ldata.ld_leh + 1)
diff --git a/drivers/staging/lustre/lustre/include/lustre_lite.h b/drivers/staging/lustre/lustre/include/lustre_lite.h
index eee900638720..df557c22abbe 100644
--- a/drivers/staging/lustre/lustre/include/lustre_lite.h
+++ b/drivers/staging/lustre/lustre/include/lustre_lite.h
@@ -45,7 +45,6 @@
 #include "linux/lustre_lite.h"
 
 #include "obd_class.h"
-#include "obd_ost.h"
 #include "lustre_net.h"
 #include "lustre_mds.h"
 #include "lustre_ha.h"
@@ -60,7 +59,7 @@
 struct lustre_rw_params {
 	int		lrp_lock_mode;
 	ldlm_policy_data_t lrp_policy;
-	obd_flag	   lrp_brw_flags;
+	u32		lrp_brw_flags;
 	int		lrp_ast_flags;
 };
 
diff --git a/drivers/staging/lustre/lustre/include/lustre_log.h b/drivers/staging/lustre/lustre/include/lustre_log.h
index d02ea24a7e40..2187fb615e9a 100644
--- a/drivers/staging/lustre/lustre/include/lustre_log.h
+++ b/drivers/staging/lustre/lustre/include/lustre_log.h
@@ -56,10 +56,7 @@
  * @{
  */
 
-#include "linux/lustre_log.h"
-
 #include "obd_class.h"
-#include "obd_ost.h"
 #include "lustre/lustre_idl.h"
 #include "dt_object.h"
 
@@ -209,15 +206,6 @@ int llog_sync(struct llog_ctxt *ctxt, struct obd_export *exp, int flags);
 int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
 		struct llog_cookie *cookies, int flags);
 
-int obd_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-		  struct obd_device *disk_obd, int *idx);
-
-int obd_llog_finish(struct obd_device *obd, int count);
-
-/* llog_ioctl.c */
-int llog_ioctl(const struct lu_env *env, struct llog_ctxt *ctxt, int cmd,
-	       struct obd_ioctl_data *data);
-
 /* llog_net.c */
 int llog_initiator_connect(struct llog_ctxt *ctxt);
 
@@ -315,18 +303,6 @@ struct llog_handle {
 	atomic_t		 lgh_refcount;
 };
 
-/* llog_lvfs.c */
-extern struct llog_operations llog_lvfs_ops;
-
-/* llog_osd.c */
-extern struct llog_operations llog_osd_ops;
-int llog_osd_get_cat_list(const struct lu_env *env, struct dt_device *d,
-			  int idx, int count,
-			  struct llog_catid *idarray);
-int llog_osd_put_cat_list(const struct lu_env *env, struct dt_device *d,
-			  int idx, int count,
-			  struct llog_catid *idarray);
-
 #define LLOG_CTXT_FLAG_UNINITIALIZED     0x00000001
 #define LLOG_CTXT_FLAG_STOP		 0x00000002
 
@@ -461,7 +437,7 @@ static inline int llog_group_ctxt_null(struct obd_llog_group *olg, int index)
 
 static inline int llog_ctxt_null(struct obd_device *obd, int index)
 {
-	return (llog_group_ctxt_null(&obd->obd_olg, index));
+	return llog_group_ctxt_null(&obd->obd_olg, index);
 }
 
 static inline int llog_destroy(const struct lu_env *env,
diff --git a/drivers/staging/lustre/lustre/include/lustre_mdc.h b/drivers/staging/lustre/lustre/include/lustre_mdc.h
index 39bbc3634b06..b1b05c8a371a 100644
--- a/drivers/staging/lustre/lustre/include/lustre_mdc.h
+++ b/drivers/staging/lustre/lustre/include/lustre_mdc.h
@@ -50,7 +50,7 @@
 
 #include <linux/fs.h>
 #include <linux/dcache.h>
-#include "linux/lustre_intent.h"
+#include "lustre_intent.h"
 #include "lustre_handles.h"
 #include "../../include/linux/libcfs/libcfs.h"
 #include "obd_class.h"
diff --git a/drivers/staging/lustre/lustre/include/lustre_net.h b/drivers/staging/lustre/lustre/include/lustre_net.h
index d6af2ca3acdb..0a024d3cfeb7 100644
--- a/drivers/staging/lustre/lustre/include/lustre_net.h
+++ b/drivers/staging/lustre/lustre/include/lustre_net.h
@@ -55,8 +55,6 @@
  * @{
  */
 
-#include "linux/lustre_net.h"
-
 #include "../../include/linux/libcfs/libcfs.h"
 // #include <obd.h>
 #include "../../include/linux/lnet/lnet.h"
@@ -1183,276 +1181,6 @@ struct nrs_fifo_req {
 /** @} fifo */
 
 /**
- * \name CRR-N
- *
- * CRR-N, Client Round Robin over NIDs
- * @{
- */
-
-/**
- * private data structure for CRR-N NRS
- */
-struct nrs_crrn_net {
-	struct ptlrpc_nrs_resource	cn_res;
-	cfs_binheap_t		       *cn_binheap;
-	struct cfs_hash		       *cn_cli_hash;
-	/**
-	 * Used when a new scheduling round commences, in order to synchronize
-	 * all clients with the new round number.
-	 */
-	__u64				cn_round;
-	/**
-	 * Determines the relevant ordering amongst request batches within a
-	 * scheduling round.
-	 */
-	__u64				cn_sequence;
-	/**
-	 * Round Robin quantum; the maximum number of RPCs that each request
-	 * batch for each client can have in a scheduling round.
-	 */
-	__u16				cn_quantum;
-};
-
-/**
- * Object representing a client in CRR-N, as identified by its NID
- */
-struct nrs_crrn_client {
-	struct ptlrpc_nrs_resource	cc_res;
-	struct hlist_node		cc_hnode;
-	lnet_nid_t			cc_nid;
-	/**
-	 * The round number against which this client is currently scheduling
-	 * requests.
-	 */
-	__u64				cc_round;
-	/**
-	 * The sequence number used for requests scheduled by this client during
-	 * the current round number.
-	 */
-	__u64				cc_sequence;
-	atomic_t			cc_ref;
-	/**
-	 * Round Robin quantum; the maximum number of RPCs the client is allowed
-	 * to schedule in a single batch of each round.
-	 */
-	__u16				cc_quantum;
-	/**
-	 * # of pending requests for this client, on all existing rounds
-	 */
-	__u16				cc_active;
-};
-
-/**
- * CRR-N NRS request definition
- */
-struct nrs_crrn_req {
-	/**
-	 * Round number for this request; shared with all other requests in the
-	 * same batch.
-	 */
-	__u64			cr_round;
-	/**
-	 * Sequence number for this request; shared with all other requests in
-	 * the same batch.
-	 */
-	__u64			cr_sequence;
-};
-
-/**
- * CRR-N policy operations.
- */
-enum nrs_ctl_crr {
-	/**
-	 * Read the RR quantum size of a CRR-N policy.
-	 */
-	NRS_CTL_CRRN_RD_QUANTUM = PTLRPC_NRS_CTL_1ST_POL_SPEC,
-	/**
-	 * Write the RR quantum size of a CRR-N policy.
-	 */
-	NRS_CTL_CRRN_WR_QUANTUM,
-};
-
-/** @} CRR-N */
-
-/**
- * \name ORR/TRR
- *
- * ORR/TRR (Object-based Round Robin/Target-based Round Robin) NRS policies
- * @{
- */
-
-/**
- * Lower and upper byte offsets of a brw RPC
- */
-struct nrs_orr_req_range {
-	__u64		or_start;
-	__u64		or_end;
-};
-
-/**
- * RPC types supported by the ORR/TRR policies
- */
-enum nrs_orr_supp {
-	NOS_OST_READ  = (1 << 0),
-	NOS_OST_WRITE = (1 << 1),
-	NOS_OST_RW    = (NOS_OST_READ | NOS_OST_WRITE),
-	/**
-	 * Default value for policies.
-	 */
-	NOS_DFLT      = NOS_OST_READ
-};
-
-/**
- * As unique keys for grouping RPCs together, we use the object's OST FID for
- * the ORR policy, and the OST index for the TRR policy.
- *
- * XXX: We waste some space for TRR policy instances by using a union, but it
- *	allows to consolidate some of the code between ORR and TRR, and these
- *	policies will probably eventually merge into one anyway.
- */
-struct nrs_orr_key {
-	union {
-		/** object FID for ORR */
-		struct lu_fid	ok_fid;
-		/** OST index for TRR */
-		__u32		ok_idx;
-	};
-};
-
-/**
- * The largest base string for unique hash/slab object names is
- * "nrs_orr_reg_", so 13 characters. We add 3 to this to be used for the CPT
- * id number, so this _should_ be more than enough for the maximum number of
- * CPTs on any system. If it does happen that this statement is incorrect,
- * nrs_orr_genobjname() will inevitably yield a non-unique name and cause
- * kmem_cache_create() to complain (on Linux), so the erroneous situation
- * will hopefully not go unnoticed.
- */
-#define NRS_ORR_OBJ_NAME_MAX	(sizeof("nrs_orr_reg_") + 3)
-
-/**
- * private data structure for ORR and TRR NRS
- */
-struct nrs_orr_data {
-	struct ptlrpc_nrs_resource	od_res;
-	cfs_binheap_t		       *od_binheap;
-	struct cfs_hash		       *od_obj_hash;
-	struct kmem_cache		       *od_cache;
-	/**
-	 * Used when a new scheduling round commences, in order to synchronize
-	 * all object or OST batches with the new round number.
-	 */
-	__u64				od_round;
-	/**
-	 * Determines the relevant ordering amongst request batches within a
-	 * scheduling round.
-	 */
-	__u64				od_sequence;
-	/**
-	 * RPC types that are currently supported.
-	 */
-	enum nrs_orr_supp		od_supp;
-	/**
-	 * Round Robin quantum; the maximum number of RPCs that each request
-	 * batch for each object or OST can have in a scheduling round.
-	 */
-	__u16				od_quantum;
-	/**
-	 * Whether to use physical disk offsets or logical file offsets.
-	 */
-	bool				od_physical;
-	/**
-	 * XXX: We need to provide a persistently allocated string to hold
-	 * unique object names for this policy, since in currently supported
-	 * versions of Linux by Lustre, kmem_cache_create() just sets a pointer
-	 * to the name string provided. kstrdup() is used in the version of
-	 * kmeme_cache_create() in current Linux mainline, so we may be able to
-	 * remove this in the future.
-	 */
-	char				od_objname[NRS_ORR_OBJ_NAME_MAX];
-};
-
-/**
- * Represents a backend-fs object or OST in the ORR and TRR policies
- * respectively
- */
-struct nrs_orr_object {
-	struct ptlrpc_nrs_resource	oo_res;
-	struct hlist_node		oo_hnode;
-	/**
-	 * The round number against which requests are being scheduled for this
-	 * object or OST
-	 */
-	__u64				oo_round;
-	/**
-	 * The sequence number used for requests scheduled for this object or
-	 * OST during the current round number.
-	 */
-	__u64				oo_sequence;
-	/**
-	 * The key of the object or OST for which this structure instance is
-	 * scheduling RPCs
-	 */
-	struct nrs_orr_key		oo_key;
-	atomic_t			oo_ref;
-	/**
-	 * Round Robin quantum; the maximum number of RPCs that are allowed to
-	 * be scheduled for the object or OST in a single batch of each round.
-	 */
-	__u16				oo_quantum;
-	/**
-	 * # of pending requests for this object or OST, on all existing rounds
-	 */
-	__u16				oo_active;
-};
-
-/**
- * ORR/TRR NRS request definition
- */
-struct nrs_orr_req {
-	/**
-	 * The offset range this request covers
-	 */
-	struct nrs_orr_req_range	or_range;
-	/**
-	 * Round number for this request; shared with all other requests in the
-	 * same batch.
-	 */
-	__u64				or_round;
-	/**
-	 * Sequence number for this request; shared with all other requests in
-	 * the same batch.
-	 */
-	__u64				or_sequence;
-	/**
-	 * For debugging purposes.
-	 */
-	struct nrs_orr_key		or_key;
-	/**
-	 * An ORR policy instance has filled in request information while
-	 * enqueueing the request on the service partition's regular NRS head.
-	 */
-	unsigned int			or_orr_set:1;
-	/**
-	 * A TRR policy instance has filled in request information while
-	 * enqueueing the request on the service partition's regular NRS head.
-	 */
-	unsigned int			or_trr_set:1;
-	/**
-	 * Request offset ranges have been filled in with logical offset
-	 * values.
-	 */
-	unsigned int			or_logical_set:1;
-	/**
-	 * Request offset ranges have been filled in with physical offset
-	 * values.
-	 */
-	unsigned int			or_physical_set:1;
-};
-
-/** @} ORR/TRR */
-
-/**
  * NRS request
  *
  * Instances of this object exist embedded within ptlrpc_request; the main
@@ -1476,7 +1204,6 @@ struct ptlrpc_nrs_request {
 	unsigned			nr_enqueued:1;
 	unsigned			nr_started:1;
 	unsigned			nr_finalized:1;
-	cfs_binheap_node_t		nr_node;
 
 	/**
 	 * Policy-specific fields, used for determining a request's scheduling
@@ -1487,12 +1214,6 @@ struct ptlrpc_nrs_request {
 		 * Fields for the FIFO policy
 		 */
 		struct nrs_fifo_req	fifo;
-		/**
-		 * CRR-N request definition
-		 */
-		struct nrs_crrn_req	crr;
-		/** ORR and TRR share the same request definition */
-		struct nrs_orr_req	orr;
 	} nr_u;
 	/**
 	 * Externally-registering policies may want to use this to allocate
@@ -1966,7 +1687,7 @@ do {									  \
 	    ((libcfs_debug & (mask)) != 0 &&				  \
 	     (libcfs_subsystem_debug & DEBUG_SUBSYSTEM) != 0))		\
 		_debug_req((req), msgdata, fmt, ##a);			 \
-} while(0)
+} while (0)
 
 /**
  * This is the debug print function you need to use to print request structure
@@ -2912,11 +2633,13 @@ __u32 lustre_msg_calc_cksum(struct lustre_msg *msg, int compat18);
 # warning "remove checksum compatibility support for b1_8"
 __u32 lustre_msg_calc_cksum(struct lustre_msg *msg);
 #endif
-void lustre_msg_set_handle(struct lustre_msg *msg,struct lustre_handle *handle);
+void lustre_msg_set_handle(struct lustre_msg *msg,
+			   struct lustre_handle *handle);
 void lustre_msg_set_type(struct lustre_msg *msg, __u32 type);
 void lustre_msg_set_opc(struct lustre_msg *msg, __u32 opc);
 void lustre_msg_set_last_xid(struct lustre_msg *msg, __u64 last_xid);
-void lustre_msg_set_last_committed(struct lustre_msg *msg,__u64 last_committed);
+void lustre_msg_set_last_committed(struct lustre_msg *msg,
+				   __u64 last_committed);
 void lustre_msg_set_versions(struct lustre_msg *msg, __u64 *versions);
 void lustre_msg_set_transno(struct lustre_msg *msg, __u64 transno);
 void lustre_msg_set_status(struct lustre_msg *msg, __u32 status);
@@ -3173,7 +2896,7 @@ int ptlrpc_add_timeout_client(int time, enum timeout_event event,
 			      struct list_head *obd_list);
 int ptlrpc_del_timeout_client(struct list_head *obd_list,
 			      enum timeout_event event);
-struct ptlrpc_request * ptlrpc_prep_ping(struct obd_import *imp);
+struct ptlrpc_request *ptlrpc_prep_ping(struct obd_import *imp);
 int ptlrpc_obd_ping(struct obd_device *obd);
 void ping_evictor_start(void);
 void ping_evictor_stop(void);
diff --git a/drivers/staging/lustre/lustre/include/lustre_quota.h b/drivers/staging/lustre/lustre/include/lustre_quota.h
index 1ae72e3ff1bf..2643f28070a2 100644
--- a/drivers/staging/lustre/lustre/include/lustre_quota.h
+++ b/drivers/staging/lustre/lustre/include/lustre_quota.h
@@ -32,7 +32,9 @@
  *
  */
 
-#include "linux/lustre_quota.h"
+#include <linux/fs.h>
+#include <linux/quota.h>
+#include <linux/quotaops.h>
 
 #include "dt_object.h"
 #include "lustre_fid.h"
diff --git a/drivers/staging/lustre/lustre/include/lustre_sec.h b/drivers/staging/lustre/lustre/include/lustre_sec.h
index dc31bf93a1d7..dff70a5b9bc4 100644
--- a/drivers/staging/lustre/lustre/include/lustre_sec.h
+++ b/drivers/staging/lustre/lustre/include/lustre_sec.h
@@ -347,9 +347,6 @@ void sptlrpc_conf_log_stop(const char *logname);
 void sptlrpc_conf_log_update_begin(const char *logname);
 void sptlrpc_conf_log_update_end(const char *logname);
 void sptlrpc_conf_client_adapt(struct obd_device *obd);
-int  sptlrpc_conf_target_get_rules(struct obd_device *obd,
-				   struct sptlrpc_rule_set *rset,
-				   int initial);
 void sptlrpc_target_choose_flavor(struct sptlrpc_rule_set *rset,
 				  enum lustre_sec_part from,
 				  lnet_nid_t nid,
@@ -885,7 +882,7 @@ enum sptlrpc_bulk_hash_alg {
 	BULK_HASH_ALG_MAX
 };
 
-const char * sptlrpc_get_hash_name(__u8 hash_alg);
+const char *sptlrpc_get_hash_name(__u8 hash_alg);
 __u8 sptlrpc_get_hash_alg(const char *algname);
 
 enum {
@@ -1060,7 +1057,7 @@ void sptlrpc_gc_del_sec(struct ptlrpc_sec *sec);
 void sptlrpc_gc_add_ctx(struct ptlrpc_cli_ctx *ctx);
 
 /* misc */
-const char * sec2target_str(struct ptlrpc_sec *sec);
+const char *sec2target_str(struct ptlrpc_sec *sec);
 /*
  * lprocfs
  */
diff --git a/drivers/staging/lustre/lustre/include/lustre_ver.h b/drivers/staging/lustre/lustre/include/lustre_ver.h
index dc187b8f741f..caa4da12f37a 100644
--- a/drivers/staging/lustre/lustre/include/lustre_ver.h
+++ b/drivers/staging/lustre/lustre/include/lustre_ver.h
@@ -10,7 +10,9 @@
 #define LUSTRE_FIX 0
 #define LUSTRE_VERSION_STRING "2.3.64"
 
-#define LUSTRE_VERSION_CODE OBD_OCD_VERSION(LUSTRE_MAJOR,LUSTRE_MINOR,LUSTRE_PATCH,LUSTRE_FIX)
+#define LUSTRE_VERSION_CODE OBD_OCD_VERSION(LUSTRE_MAJOR, \
+					    LUSTRE_MINOR, LUSTRE_PATCH, \
+					    LUSTRE_FIX)
 
 /* liblustre clients are only allowed to connect if their LUSTRE_FIX mismatches
  * by this amount (set in lustre/autoconf/lustre-version.ac). */
diff --git a/drivers/staging/lustre/lustre/include/lvfs.h b/drivers/staging/lustre/lustre/include/lvfs.h
deleted file mode 100644
index 32dcd57c47cc..000000000000
--- a/drivers/staging/lustre/lustre/include/lvfs.h
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/lvfs.h
- *
- * lustre VFS/process permission interface
- */
-
-#ifndef __LVFS_H__
-#define __LVFS_H__
-
-#define LL_FID_NAMELEN (16 + 1 + 8 + 1)
-
-#include "../../include/linux/libcfs/libcfs.h"
-#include "linux/lvfs.h"
-
-#include "../../include/linux/libcfs/lucache.h"
-
-
-/* lvfs_common.c */
-struct dentry *lvfs_fid2dentry(struct lvfs_run_ctxt *, __u64, __u32, __u64 ,void *data);
-
-void push_ctxt(struct lvfs_run_ctxt *save, struct lvfs_run_ctxt *new_ctx,
-	       struct lvfs_ucred *cred);
-void pop_ctxt(struct lvfs_run_ctxt *saved, struct lvfs_run_ctxt *new_ctx,
-	      struct lvfs_ucred *cred);
-#endif
diff --git a/drivers/staging/lustre/lustre/include/md_object.h b/drivers/staging/lustre/lustre/include/md_object.h
deleted file mode 100644
index 2e5d55030a63..000000000000
--- a/drivers/staging/lustre/lustre/include/md_object.h
+++ /dev/null
@@ -1,903 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/md_object.h
- *
- * Extension of lu_object.h for metadata objects
- */
-
-#ifndef _LUSTRE_MD_OBJECT_H
-#define _LUSTRE_MD_OBJECT_H
-
-/** \defgroup md md
- * Sub-class of lu_object with methods common for "meta-data" objects in MDT
- * stack.
- *
- * Meta-data objects implement namespace operations: you can link, unlink
- * them, and treat them as directories.
- *
- * Examples: mdt, cmm, and mdt are implementations of md interface.
- * @{
- */
-
-
-/*
- * super-class definitions.
- */
-#include "dt_object.h"
-
-struct md_device;
-struct md_device_operations;
-struct md_object;
-struct obd_export;
-
-enum {
-	UCRED_INVALID   = -1,
-	UCRED_INIT      = 0,
-	UCRED_OLD       = 1,
-	UCRED_NEW       = 2
-};
-
-enum {
-	MD_CAPAINFO_MAX = 5
-};
-
-/** there are at most 5 fids in one operation, see rename, NOTE the last one
- * is a temporary one used for is_subdir() */
-struct md_capainfo {
-	__u32		   mc_auth;
-	__u32		   mc_padding;
-	struct lu_fid	   mc_fid[MD_CAPAINFO_MAX];
-	struct lustre_capa     *mc_capa[MD_CAPAINFO_MAX];
-};
-
-struct md_quota {
-	struct obd_export       *mq_exp;
-};
-
-/**
- * Implemented in mdd/mdd_handler.c.
- *
- * XXX should be moved into separate .h/.c together with all md security
- * related definitions.
- */
-struct md_capainfo *md_capainfo(const struct lu_env *env);
-struct md_quota *md_quota(const struct lu_env *env);
-
-/** metadata attributes */
-enum ma_valid {
-	MA_INODE     = (1 << 0),
-	MA_LOV       = (1 << 1),
-	MA_COOKIE    = (1 << 2),
-	MA_FLAGS     = (1 << 3),
-	MA_LMV       = (1 << 4),
-	MA_ACL_DEF   = (1 << 5),
-	MA_LOV_DEF   = (1 << 6),
-	MA_LAY_GEN   = (1 << 7),
-	MA_HSM       = (1 << 8),
-	MA_SOM       = (1 << 9),
-	MA_PFID      = (1 << 10)
-};
-
-typedef enum {
-	MDL_MINMODE  = 0,
-	MDL_EX       = 1,
-	MDL_PW       = 2,
-	MDL_PR       = 4,
-	MDL_CW       = 8,
-	MDL_CR       = 16,
-	MDL_NL       = 32,
-	MDL_GROUP    = 64,
-	MDL_MAXMODE
-} mdl_mode_t;
-
-typedef enum {
-	MDT_NUL_LOCK = 0,
-	MDT_REG_LOCK = (1 << 0),
-	MDT_PDO_LOCK = (1 << 1)
-} mdl_type_t;
-
-/* memory structure for hsm attributes
- * for fields description see the on disk structure hsm_attrs
- * which is defined in lustre_idl.h
- */
-struct md_hsm {
-	__u32	mh_compat;
-	__u32	mh_flags;
-	__u64	mh_arch_id;
-	__u64	mh_arch_ver;
-};
-
-#define IOEPOCH_INVAL 0
-
-/* memory structure for som attributes
- * for fields description see the on disk structure som_attrs
- * which is defined in lustre_idl.h
- */
-struct md_som_data {
-	__u32	msd_compat;
-	__u32	msd_incompat;
-	__u64	msd_ioepoch;
-	__u64	msd_size;
-	__u64	msd_blocks;
-	__u64	msd_mountid;
-};
-
-struct md_attr {
-	__u64		   ma_valid;
-	__u64		   ma_need;
-	__u64		   ma_attr_flags;
-	struct lu_attr	  ma_attr;
-	struct lu_fid	   ma_pfid;
-	struct md_hsm	   ma_hsm;
-	struct lov_mds_md      *ma_lmm;
-	struct lmv_stripe_md   *ma_lmv;
-	void		   *ma_acl;
-	struct llog_cookie     *ma_cookie;
-	struct lustre_capa     *ma_capa;
-	struct md_som_data     *ma_som;
-	int		     ma_lmm_size;
-	int		     ma_lmv_size;
-	int		     ma_acl_size;
-	int		     ma_cookie_size;
-	__u16		   ma_layout_gen;
-};
-
-/** Additional parameters for create */
-struct md_op_spec {
-	union {
-		/** symlink target */
-		const char	       *sp_symname;
-		/** parent FID for cross-ref mkdir */
-		const struct lu_fid      *sp_pfid;
-		/** eadata for regular files */
-		struct md_spec_reg {
-			/** lov objs exist already */
-			const struct lu_fid   *fid;
-			const void *eadata;
-			int  eadatalen;
-		} sp_ea;
-	} u;
-
-	/** Create flag from client: such as MDS_OPEN_CREAT, and others. */
-	__u64      sp_cr_flags;
-
-	/** don't create lov objects or llog cookie - this replay */
-	unsigned int no_create:1,
-		     sp_cr_lookup:1, /* do lookup sanity check or not. */
-		     sp_rm_entry:1;  /* only remove name entry */
-
-	/** Current lock mode for parent dir where create is performing. */
-	mdl_mode_t sp_cr_mode;
-
-	/** to create directory */
-	const struct dt_index_features *sp_feat;
-};
-
-/**
- * Operations implemented for each md object (both directory and leaf).
- */
-struct md_object_operations {
-	int (*moo_permission)(const struct lu_env *env,
-			      struct md_object *pobj, struct md_object *cobj,
-			      struct md_attr *attr, int mask);
-
-	int (*moo_attr_get)(const struct lu_env *env, struct md_object *obj,
-			    struct md_attr *attr);
-
-	int (*moo_attr_set)(const struct lu_env *env, struct md_object *obj,
-			    const struct md_attr *attr);
-
-	int (*moo_xattr_get)(const struct lu_env *env, struct md_object *obj,
-			     struct lu_buf *buf, const char *name);
-
-	int (*moo_xattr_list)(const struct lu_env *env, struct md_object *obj,
-			      struct lu_buf *buf);
-
-	int (*moo_xattr_set)(const struct lu_env *env, struct md_object *obj,
-			     const struct lu_buf *buf, const char *name,
-			     int fl);
-
-	int (*moo_xattr_del)(const struct lu_env *env, struct md_object *obj,
-			     const char *name);
-
-	/** This method is used to swap the layouts between 2 objects */
-	int (*moo_swap_layouts)(const struct lu_env *env,
-			       struct md_object *obj1, struct md_object *obj2,
-			       __u64 flags);
-
-	/** \retval number of bytes actually read upon success */
-	int (*moo_readpage)(const struct lu_env *env, struct md_object *obj,
-			    const struct lu_rdpg *rdpg);
-
-	int (*moo_readlink)(const struct lu_env *env, struct md_object *obj,
-			    struct lu_buf *buf);
-	int (*moo_changelog)(const struct lu_env *env,
-			     enum changelog_rec_type type, int flags,
-			     struct md_object *obj);
-	/** part of cross-ref operation */
-	int (*moo_object_create)(const struct lu_env *env,
-				 struct md_object *obj,
-				 const struct md_op_spec *spec,
-				 struct md_attr *ma);
-
-	int (*moo_ref_add)(const struct lu_env *env,
-			   struct md_object *obj,
-			   const struct md_attr *ma);
-
-	int (*moo_ref_del)(const struct lu_env *env,
-			   struct md_object *obj,
-			   struct md_attr *ma);
-
-	int (*moo_open)(const struct lu_env *env,
-			struct md_object *obj, int flag);
-
-	int (*moo_close)(const struct lu_env *env, struct md_object *obj,
-			 struct md_attr *ma, int mode);
-
-	int (*moo_capa_get)(const struct lu_env *, struct md_object *,
-			    struct lustre_capa *, int renewal);
-
-	int (*moo_object_sync)(const struct lu_env *, struct md_object *);
-
-	int (*moo_file_lock)(const struct lu_env *env, struct md_object *obj,
-			     struct lov_mds_md *lmm, struct ldlm_extent *extent,
-			     struct lustre_handle *lockh);
-	int (*moo_file_unlock)(const struct lu_env *env, struct md_object *obj,
-			       struct lov_mds_md *lmm,
-			       struct lustre_handle *lockh);
-	int (*moo_object_lock)(const struct lu_env *env, struct md_object *obj,
-			       struct lustre_handle *lh,
-			       struct ldlm_enqueue_info *einfo,
-			       void *policy);
-};
-
-/**
- * Operations implemented for each directory object.
- */
-struct md_dir_operations {
-	int (*mdo_is_subdir) (const struct lu_env *env, struct md_object *obj,
-			      const struct lu_fid *fid, struct lu_fid *sfid);
-
-	int (*mdo_lookup)(const struct lu_env *env, struct md_object *obj,
-			  const struct lu_name *lname, struct lu_fid *fid,
-			  struct md_op_spec *spec);
-
-	mdl_mode_t (*mdo_lock_mode)(const struct lu_env *env,
-				    struct md_object *obj,
-				    mdl_mode_t mode);
-
-	int (*mdo_create)(const struct lu_env *env, struct md_object *pobj,
-			  const struct lu_name *lname, struct md_object *child,
-			  struct md_op_spec *spec,
-			  struct md_attr *ma);
-
-	/** This method is used for creating data object for this meta object*/
-	int (*mdo_create_data)(const struct lu_env *env, struct md_object *p,
-			       struct md_object *o,
-			       const struct md_op_spec *spec,
-			       struct md_attr *ma);
-
-	int (*mdo_rename)(const struct lu_env *env, struct md_object *spobj,
-			  struct md_object *tpobj, const struct lu_fid *lf,
-			  const struct lu_name *lsname, struct md_object *tobj,
-			  const struct lu_name *ltname, struct md_attr *ma);
-
-	int (*mdo_link)(const struct lu_env *env, struct md_object *tgt_obj,
-			struct md_object *src_obj, const struct lu_name *lname,
-			struct md_attr *ma);
-
-	int (*mdo_unlink)(const struct lu_env *env, struct md_object *pobj,
-			  struct md_object *cobj, const struct lu_name *lname,
-			  struct md_attr *ma, int no_name);
-
-	/** This method is used to compare a requested layout to an existing
-	 * layout (struct lov_mds_md_v1/3 vs struct lov_mds_md_v1/3) */
-	int (*mdo_lum_lmm_cmp)(const struct lu_env *env,
-			       struct md_object *cobj,
-			       const struct md_op_spec *spec,
-			       struct md_attr *ma);
-
-	/** partial ops for cross-ref case */
-	int (*mdo_name_insert)(const struct lu_env *env,
-			       struct md_object *obj,
-			       const struct lu_name *lname,
-			       const struct lu_fid *fid,
-			       const struct md_attr *ma);
-
-	int (*mdo_name_remove)(const struct lu_env *env,
-			       struct md_object *obj,
-			       const struct lu_name *lname,
-			       const struct md_attr *ma);
-
-	int (*mdo_rename_tgt)(const struct lu_env *env, struct md_object *pobj,
-			      struct md_object *tobj, const struct lu_fid *fid,
-			      const struct lu_name *lname, struct md_attr *ma);
-};
-
-struct md_device_operations {
-	/** meta-data device related handlers. */
-	int (*mdo_root_get)(const struct lu_env *env, struct md_device *m,
-			    struct lu_fid *f);
-
-	int (*mdo_maxeasize_get)(const struct lu_env *env, struct md_device *m,
-				int *easize);
-
-	int (*mdo_statfs)(const struct lu_env *env, struct md_device *m,
-			  struct obd_statfs *sfs);
-
-	int (*mdo_init_capa_ctxt)(const struct lu_env *env, struct md_device *m,
-				  int mode, unsigned long timeout, __u32 alg,
-				  struct lustre_capa_key *keys);
-
-	int (*mdo_update_capa_key)(const struct lu_env *env,
-				   struct md_device *m,
-				   struct lustre_capa_key *key);
-
-	int (*mdo_llog_ctxt_get)(const struct lu_env *env,
-				 struct md_device *m, int idx, void **h);
-
-	int (*mdo_iocontrol)(const struct lu_env *env, struct md_device *m,
-			     unsigned int cmd, int len, void *data);
-};
-
-enum md_upcall_event {
-	/** Sync the md layer*/
-	MD_LOV_SYNC = (1 << 0),
-	/** Just for split, no need trans, for replay */
-	MD_NO_TRANS = (1 << 1),
-	MD_LOV_CONFIG = (1 << 2),
-	/** Trigger quota recovery */
-	MD_LOV_QUOTA = (1 << 3)
-};
-
-struct md_upcall {
-	/** this lock protects upcall using against its removal
-	 * read lock is for usage the upcall, write - for init/fini */
-	struct rw_semaphore	mu_upcall_sem;
-	/** device to call, upper layer normally */
-	struct md_device       *mu_upcall_dev;
-	/** upcall function */
-	int (*mu_upcall)(const struct lu_env *env, struct md_device *md,
-			 enum md_upcall_event ev, void *data);
-};
-
-struct md_device {
-	struct lu_device		   md_lu_dev;
-	const struct md_device_operations *md_ops;
-	struct md_upcall		   md_upcall;
-};
-
-static inline void md_upcall_init(struct md_device *m, void *upcl)
-{
-	init_rwsem(&m->md_upcall.mu_upcall_sem);
-	m->md_upcall.mu_upcall_dev = NULL;
-	m->md_upcall.mu_upcall = upcl;
-}
-
-static inline void md_upcall_dev_set(struct md_device *m, struct md_device *up)
-{
-	down_write(&m->md_upcall.mu_upcall_sem);
-	m->md_upcall.mu_upcall_dev = up;
-	up_write(&m->md_upcall.mu_upcall_sem);
-}
-
-static inline void md_upcall_fini(struct md_device *m)
-{
-	down_write(&m->md_upcall.mu_upcall_sem);
-	m->md_upcall.mu_upcall_dev = NULL;
-	m->md_upcall.mu_upcall = NULL;
-	up_write(&m->md_upcall.mu_upcall_sem);
-}
-
-static inline int md_do_upcall(const struct lu_env *env, struct md_device *m,
-				enum md_upcall_event ev, void *data)
-{
-	int rc = 0;
-	down_read(&m->md_upcall.mu_upcall_sem);
-	if (m->md_upcall.mu_upcall_dev != NULL &&
-	    m->md_upcall.mu_upcall_dev->md_upcall.mu_upcall != NULL) {
-		rc = m->md_upcall.mu_upcall_dev->md_upcall.mu_upcall(env,
-					      m->md_upcall.mu_upcall_dev,
-					      ev, data);
-	}
-	up_read(&m->md_upcall.mu_upcall_sem);
-	return rc;
-}
-
-struct md_object {
-	struct lu_object		   mo_lu;
-	const struct md_object_operations *mo_ops;
-	const struct md_dir_operations    *mo_dir_ops;
-};
-
-/**
- * seq-server site.
- */
-struct seq_server_site {
-	struct lu_site	     *ss_lu;
-	/**
-	 * mds number of this site.
-	 */
-	mdsno_t	       ss_node_id;
-	/**
-	 * Fid location database
-	 */
-	struct lu_server_fld *ss_server_fld;
-	struct lu_client_fld *ss_client_fld;
-
-	/**
-	 * Server Seq Manager
-	 */
-	struct lu_server_seq *ss_server_seq;
-
-	/**
-	 * Controller Seq Manager
-	 */
-	struct lu_server_seq *ss_control_seq;
-	struct obd_export    *ss_control_exp;
-
-	/**
-	 * Client Seq Manager
-	 */
-	struct lu_client_seq *ss_client_seq;
-};
-
-static inline struct md_device *lu2md_dev(const struct lu_device *d)
-{
-	LASSERT(IS_ERR(d) || lu_device_is_md(d));
-	return container_of0(d, struct md_device, md_lu_dev);
-}
-
-static inline struct lu_device *md2lu_dev(struct md_device *d)
-{
-	return &d->md_lu_dev;
-}
-
-static inline struct md_object *lu2md(const struct lu_object *o)
-{
-	LASSERT(o == NULL || IS_ERR(o) || lu_device_is_md(o->lo_dev));
-	return container_of0(o, struct md_object, mo_lu);
-}
-
-static inline struct md_object *md_object_next(const struct md_object *obj)
-{
-	return (obj ? lu2md(lu_object_next(&obj->mo_lu)) : NULL);
-}
-
-static inline struct md_device *md_obj2dev(const struct md_object *o)
-{
-	LASSERT(o == NULL || IS_ERR(o) || lu_device_is_md(o->mo_lu.lo_dev));
-	return container_of0(o->mo_lu.lo_dev, struct md_device, md_lu_dev);
-}
-
-static inline int md_device_init(struct md_device *md, struct lu_device_type *t)
-{
-	return lu_device_init(&md->md_lu_dev, t);
-}
-
-static inline void md_device_fini(struct md_device *md)
-{
-	lu_device_fini(&md->md_lu_dev);
-}
-
-static inline struct md_object *md_object_find_slice(const struct lu_env *env,
-						     struct md_device *md,
-						     const struct lu_fid *f)
-{
-	return lu2md(lu_object_find_slice(env, md2lu_dev(md), f, NULL));
-}
-
-
-/** md operations */
-static inline int mo_permission(const struct lu_env *env,
-				struct md_object *p,
-				struct md_object *c,
-				struct md_attr *at,
-				int mask)
-{
-	LASSERT(c->mo_ops->moo_permission);
-	return c->mo_ops->moo_permission(env, p, c, at, mask);
-}
-
-static inline int mo_attr_get(const struct lu_env *env,
-			      struct md_object *m,
-			      struct md_attr *at)
-{
-	LASSERT(m->mo_ops->moo_attr_get);
-	return m->mo_ops->moo_attr_get(env, m, at);
-}
-
-static inline int mo_readlink(const struct lu_env *env,
-			      struct md_object *m,
-			      struct lu_buf *buf)
-{
-	LASSERT(m->mo_ops->moo_readlink);
-	return m->mo_ops->moo_readlink(env, m, buf);
-}
-
-static inline int mo_changelog(const struct lu_env *env,
-			       enum changelog_rec_type type,
-			       int flags, struct md_object *m)
-{
-	LASSERT(m->mo_ops->moo_changelog);
-	return m->mo_ops->moo_changelog(env, type, flags, m);
-}
-
-static inline int mo_attr_set(const struct lu_env *env,
-			      struct md_object *m,
-			      const struct md_attr *at)
-{
-	LASSERT(m->mo_ops->moo_attr_set);
-	return m->mo_ops->moo_attr_set(env, m, at);
-}
-
-static inline int mo_xattr_get(const struct lu_env *env,
-			       struct md_object *m,
-			       struct lu_buf *buf,
-			       const char *name)
-{
-	LASSERT(m->mo_ops->moo_xattr_get);
-	return m->mo_ops->moo_xattr_get(env, m, buf, name);
-}
-
-static inline int mo_xattr_del(const struct lu_env *env,
-			       struct md_object *m,
-			       const char *name)
-{
-	LASSERT(m->mo_ops->moo_xattr_del);
-	return m->mo_ops->moo_xattr_del(env, m, name);
-}
-
-static inline int mo_xattr_set(const struct lu_env *env,
-			       struct md_object *m,
-			       const struct lu_buf *buf,
-			       const char *name,
-			       int flags)
-{
-	LASSERT(m->mo_ops->moo_xattr_set);
-	return m->mo_ops->moo_xattr_set(env, m, buf, name, flags);
-}
-
-static inline int mo_xattr_list(const struct lu_env *env,
-				struct md_object *m,
-				struct lu_buf *buf)
-{
-	LASSERT(m->mo_ops->moo_xattr_list);
-	return m->mo_ops->moo_xattr_list(env, m, buf);
-}
-
-static inline int mo_swap_layouts(const struct lu_env *env,
-				  struct md_object *o1,
-				  struct md_object *o2, __u64 flags)
-{
-	LASSERT(o1->mo_ops->moo_swap_layouts);
-	LASSERT(o2->mo_ops->moo_swap_layouts);
-	if (o1->mo_ops->moo_swap_layouts != o2->mo_ops->moo_swap_layouts)
-		return -EPERM;
-	return o1->mo_ops->moo_swap_layouts(env, o1, o2, flags);
-}
-
-static inline int mo_open(const struct lu_env *env,
-			  struct md_object *m,
-			  int flags)
-{
-	LASSERT(m->mo_ops->moo_open);
-	return m->mo_ops->moo_open(env, m, flags);
-}
-
-static inline int mo_close(const struct lu_env *env,
-			   struct md_object *m,
-			   struct md_attr *ma,
-			   int mode)
-{
-	LASSERT(m->mo_ops->moo_close);
-	return m->mo_ops->moo_close(env, m, ma, mode);
-}
-
-static inline int mo_readpage(const struct lu_env *env,
-			      struct md_object *m,
-			      const struct lu_rdpg *rdpg)
-{
-	LASSERT(m->mo_ops->moo_readpage);
-	return m->mo_ops->moo_readpage(env, m, rdpg);
-}
-
-static inline int mo_object_create(const struct lu_env *env,
-				   struct md_object *m,
-				   const struct md_op_spec *spc,
-				   struct md_attr *at)
-{
-	LASSERT(m->mo_ops->moo_object_create);
-	return m->mo_ops->moo_object_create(env, m, spc, at);
-}
-
-static inline int mo_ref_add(const struct lu_env *env,
-			     struct md_object *m,
-			     const struct md_attr *ma)
-{
-	LASSERT(m->mo_ops->moo_ref_add);
-	return m->mo_ops->moo_ref_add(env, m, ma);
-}
-
-static inline int mo_ref_del(const struct lu_env *env,
-			     struct md_object *m,
-			     struct md_attr *ma)
-{
-	LASSERT(m->mo_ops->moo_ref_del);
-	return m->mo_ops->moo_ref_del(env, m, ma);
-}
-
-static inline int mo_capa_get(const struct lu_env *env,
-			      struct md_object *m,
-			      struct lustre_capa *c,
-			      int renewal)
-{
-	LASSERT(m->mo_ops->moo_capa_get);
-	return m->mo_ops->moo_capa_get(env, m, c, renewal);
-}
-
-static inline int mo_object_sync(const struct lu_env *env, struct md_object *m)
-{
-	LASSERT(m->mo_ops->moo_object_sync);
-	return m->mo_ops->moo_object_sync(env, m);
-}
-
-static inline int mo_file_lock(const struct lu_env *env, struct md_object *m,
-			       struct lov_mds_md *lmm,
-			       struct ldlm_extent *extent,
-			       struct lustre_handle *lockh)
-{
-	LASSERT(m->mo_ops->moo_file_lock);
-	return m->mo_ops->moo_file_lock(env, m, lmm, extent, lockh);
-}
-
-static inline int mo_file_unlock(const struct lu_env *env, struct md_object *m,
-				 struct lov_mds_md *lmm,
-				 struct lustre_handle *lockh)
-{
-	LASSERT(m->mo_ops->moo_file_unlock);
-	return m->mo_ops->moo_file_unlock(env, m, lmm, lockh);
-}
-
-static inline int mo_object_lock(const struct lu_env *env,
-				 struct md_object *m,
-				 struct lustre_handle *lh,
-				 struct ldlm_enqueue_info *einfo,
-				 void *policy)
-{
-	LASSERT(m->mo_ops->moo_object_lock);
-	return m->mo_ops->moo_object_lock(env, m, lh, einfo, policy);
-}
-
-static inline int mdo_lookup(const struct lu_env *env,
-			     struct md_object *p,
-			     const struct lu_name *lname,
-			     struct lu_fid *f,
-			     struct md_op_spec *spec)
-{
-	LASSERT(p->mo_dir_ops->mdo_lookup);
-	return p->mo_dir_ops->mdo_lookup(env, p, lname, f, spec);
-}
-
-static inline mdl_mode_t mdo_lock_mode(const struct lu_env *env,
-				       struct md_object *mo,
-				       mdl_mode_t lm)
-{
-	if (mo->mo_dir_ops->mdo_lock_mode == NULL)
-		return MDL_MINMODE;
-	return mo->mo_dir_ops->mdo_lock_mode(env, mo, lm);
-}
-
-static inline int mdo_create(const struct lu_env *env,
-			     struct md_object *p,
-			     const struct lu_name *lchild_name,
-			     struct md_object *c,
-			     struct md_op_spec *spc,
-			     struct md_attr *at)
-{
-	LASSERT(p->mo_dir_ops->mdo_create);
-	return p->mo_dir_ops->mdo_create(env, p, lchild_name, c, spc, at);
-}
-
-static inline int mdo_create_data(const struct lu_env *env,
-				  struct md_object *p,
-				  struct md_object *c,
-				  const struct md_op_spec *spec,
-				  struct md_attr *ma)
-{
-	LASSERT(c->mo_dir_ops->mdo_create_data);
-	return c->mo_dir_ops->mdo_create_data(env, p, c, spec, ma);
-}
-
-static inline int mdo_rename(const struct lu_env *env,
-			     struct md_object *sp,
-			     struct md_object *tp,
-			     const struct lu_fid *lf,
-			     const struct lu_name *lsname,
-			     struct md_object *t,
-			     const struct lu_name *ltname,
-			     struct md_attr *ma)
-{
-	LASSERT(tp->mo_dir_ops->mdo_rename);
-	return tp->mo_dir_ops->mdo_rename(env, sp, tp, lf, lsname, t, ltname,
-					  ma);
-}
-
-static inline int mdo_is_subdir(const struct lu_env *env,
-				struct md_object *mo,
-				const struct lu_fid *fid,
-				struct lu_fid *sfid)
-{
-	LASSERT(mo->mo_dir_ops->mdo_is_subdir);
-	return mo->mo_dir_ops->mdo_is_subdir(env, mo, fid, sfid);
-}
-
-static inline int mdo_link(const struct lu_env *env,
-			   struct md_object *p,
-			   struct md_object *s,
-			   const struct lu_name *lname,
-			   struct md_attr *ma)
-{
-	LASSERT(s->mo_dir_ops->mdo_link);
-	return s->mo_dir_ops->mdo_link(env, p, s, lname, ma);
-}
-
-static inline int mdo_unlink(const struct lu_env *env,
-			     struct md_object *p,
-			     struct md_object *c,
-			     const struct lu_name *lname,
-			     struct md_attr *ma, int no_name)
-{
-	LASSERT(p->mo_dir_ops->mdo_unlink);
-	return p->mo_dir_ops->mdo_unlink(env, p, c, lname, ma, no_name);
-}
-
-static inline int mdo_lum_lmm_cmp(const struct lu_env *env,
-				  struct md_object *c,
-				  const struct md_op_spec *spec,
-				  struct md_attr *ma)
-{
-	LASSERT(c->mo_dir_ops->mdo_lum_lmm_cmp);
-	return c->mo_dir_ops->mdo_lum_lmm_cmp(env, c, spec, ma);
-}
-
-static inline int mdo_name_insert(const struct lu_env *env,
-				  struct md_object *p,
-				  const struct lu_name *lname,
-				  const struct lu_fid *f,
-				  const struct md_attr *ma)
-{
-	LASSERT(p->mo_dir_ops->mdo_name_insert);
-	return p->mo_dir_ops->mdo_name_insert(env, p, lname, f, ma);
-}
-
-static inline int mdo_name_remove(const struct lu_env *env,
-				  struct md_object *p,
-				  const struct lu_name *lname,
-				  const struct md_attr *ma)
-{
-	LASSERT(p->mo_dir_ops->mdo_name_remove);
-	return p->mo_dir_ops->mdo_name_remove(env, p, lname, ma);
-}
-
-static inline int mdo_rename_tgt(const struct lu_env *env,
-				 struct md_object *p,
-				 struct md_object *t,
-				 const struct lu_fid *lf,
-				 const struct lu_name *lname,
-				 struct md_attr *ma)
-{
-	if (t) {
-		LASSERT(t->mo_dir_ops->mdo_rename_tgt);
-		return t->mo_dir_ops->mdo_rename_tgt(env, p, t, lf, lname, ma);
-	} else {
-		LASSERT(p->mo_dir_ops->mdo_rename_tgt);
-		return p->mo_dir_ops->mdo_rename_tgt(env, p, t, lf, lname, ma);
-	}
-}
-
-/**
- * Used in MDD/OUT layer for object lock rule
- **/
-enum mdd_object_role {
-	MOR_SRC_PARENT,
-	MOR_SRC_CHILD,
-	MOR_TGT_PARENT,
-	MOR_TGT_CHILD,
-	MOR_TGT_ORPHAN
-};
-
-struct dt_device;
-/**
- * Structure to hold object information. This is used to create object
- * \pre llod_dir exist
- */
-struct lu_local_obj_desc {
-	const char		      *llod_dir;
-	const char		      *llod_name;
-	__u32			    llod_oid;
-	int			      llod_is_index;
-	const struct dt_index_features  *llod_feat;
-	struct list_head		       llod_linkage;
-};
-
-int lustre_buf2som(void *buf, int rc, struct md_som_data *msd);
-int lustre_buf2hsm(void *buf, int rc, struct md_hsm *mh);
-void lustre_hsm2buf(void *buf, struct md_hsm *mh);
-
-struct lu_ucred {
-	__u32	       uc_valid;
-	__u32	       uc_o_uid;
-	__u32	       uc_o_gid;
-	__u32	       uc_o_fsuid;
-	__u32	       uc_o_fsgid;
-	__u32	       uc_uid;
-	__u32	       uc_gid;
-	__u32	       uc_fsuid;
-	__u32	       uc_fsgid;
-	__u32	       uc_suppgids[2];
-	cfs_cap_t	   uc_cap;
-	__u32	       uc_umask;
-	struct group_info *uc_ginfo;
-	struct md_identity *uc_identity;
-};
-
-struct lu_ucred *lu_ucred(const struct lu_env *env);
-
-struct lu_ucred *lu_ucred_check(const struct lu_env *env);
-
-struct lu_ucred *lu_ucred_assert(const struct lu_env *env);
-
-int lu_ucred_global_init(void);
-
-void lu_ucred_global_fini(void);
-
-#define md_cap_t(x) (x)
-
-#define MD_CAP_TO_MASK(x) (1 << (x))
-
-#define md_cap_raised(c, flag) (md_cap_t(c) & MD_CAP_TO_MASK(flag))
-
-/* capable() is copied from linux kernel! */
-static inline int md_capable(struct lu_ucred *uc, cfs_cap_t cap)
-{
-	if (md_cap_raised(uc->uc_cap, cap))
-		return 1;
-	return 0;
-}
-
-/** @} md */
-#endif /* _LINUX_MD_OBJECT_H */
diff --git a/drivers/staging/lustre/lustre/include/obd.h b/drivers/staging/lustre/lustre/include/obd.h
index 489bdd399627..2a88b806fca5 100644
--- a/drivers/staging/lustre/lustre/include/obd.h
+++ b/drivers/staging/lustre/lustre/include/obd.h
@@ -212,7 +212,7 @@ static inline int lov_lum_swab_if_needed(struct lov_user_md_v3 *lumv3,
 					 int *lmm_magic,
 					 struct lov_user_md *lum)
 {
-	if (lum && copy_from_user(lumv3, lum,sizeof(struct lov_user_md_v1)))
+	if (lum && copy_from_user(lumv3, lum, sizeof(struct lov_user_md_v1)))
 		return -EFAULT;
 
 	*lmm_magic = lumv3->lmm_magic;
@@ -252,10 +252,10 @@ struct obd_type {
 };
 
 struct brw_page {
-	obd_off  off;
+	u64 off;
 	struct page *pg;
 	int count;
-	obd_flag flag;
+	u32 flag;
 };
 
 /* llog contexts */
@@ -336,10 +336,10 @@ struct client_obd {
 	 * grant before trying to dirty a page and unreserve the rest.
 	 * See osc_{reserve|unreserve}_grant for details. */
 	long		 cl_reserved_grant;
-	struct list_head	   cl_cache_waiters; /* waiting for cache/grant */
-	unsigned long	   cl_next_shrink_grant;   /* jiffies */
-	struct list_head	   cl_grant_shrink_list;  /* Timeout event list */
-	int		  cl_grant_shrink_interval; /* seconds */
+	struct list_head cl_cache_waiters; /* waiting for cache/grant */
+	unsigned long	 cl_next_shrink_grant;   /* jiffies */
+	struct list_head cl_grant_shrink_list;  /* Timeout event list */
+	int		 cl_grant_shrink_interval; /* seconds */
 
 	/* A chunk is an optimal size used by osc_extent to determine
 	 * the extent size. A chunk is max(PAGE_CACHE_SIZE, OST block size) */
@@ -389,12 +389,12 @@ struct client_obd {
 
 	/* lru for osc caching pages */
 	struct cl_client_cache	*cl_cache;
-	struct list_head		 cl_lru_osc; /* member of cl_cache->ccc_lru */
+	struct list_head	 cl_lru_osc; /* member of cl_cache->ccc_lru */
 	atomic_t		*cl_lru_left;
 	atomic_t		 cl_lru_busy;
 	atomic_t		 cl_lru_shrinkers;
 	atomic_t		 cl_lru_in_list;
-	struct list_head		 cl_lru_list; /* lru page list */
+	struct list_head	 cl_lru_list; /* lru page list */
 	client_obd_lock_t	 cl_lru_list_lock; /* page list protector */
 
 	/* number of in flight destroy rpcs is limited to max_rpcs_in_flight */
@@ -442,7 +442,7 @@ struct client_obd {
 
 struct obd_id_info {
 	__u32   idx;
-	obd_id  *data;
+	u64	*data;
 };
 
 struct echo_client_obd {
@@ -500,9 +500,9 @@ struct lov_statfs_data {
 };
 /* Stripe placement optimization */
 struct lov_qos {
-	struct list_head	  lq_oss_list; /* list of OSSs that targets use */
+	struct list_head    lq_oss_list; /* list of OSSs that targets use */
 	struct rw_semaphore lq_rw_sem;
-	__u32	       lq_active_oss_count;
+	__u32		lq_active_oss_count;
 	unsigned int	lq_prio_free;   /* priority for free space */
 	unsigned int	lq_threshold_rr;/* priority for rr */
 	struct lov_qos_rr   lq_rr;	  /* round robin qos data */
@@ -545,8 +545,8 @@ struct pool_desc {
 	struct hlist_node      pool_hash;	      /* access by poolname */
 	struct list_head	    pool_list;	      /* serial access */
 	struct proc_dir_entry *pool_proc_entry;	/* file in /proc */
-	struct obd_device    *pool_lobd;	      /* obd of the lov/lod to which
-						       * this pool belongs */
+	struct obd_device    *pool_lobd;	/* obd of the lov/lod to which
+						*  this pool belongs */
 };
 
 struct lov_obd {
@@ -564,7 +564,7 @@ struct lov_obd {
 	int		     lov_connects;
 	int		     lov_pool_count;
 	struct cfs_hash	     *lov_pools_hash_body; /* used for key access */
-	struct list_head	      lov_pool_list; /* used for sequential access */
+	struct list_head	lov_pool_list; /* used for sequential access */
 	struct proc_dir_entry   *lov_pool_proc_entry;
 	enum lustre_sec_part    lov_sp_me;
 
@@ -589,13 +589,11 @@ enum placement_policy {
 	PLACEMENT_MAX_POLICY
 };
 
-typedef enum placement_policy placement_policy_t;
-
 struct lmv_obd {
 	int			refcount;
 	struct lu_client_fld	lmv_fld;
 	spinlock_t		lmv_lock;
-	placement_policy_t	lmv_placement;
+	enum placement_policy	lmv_placement;
 	struct lmv_desc		desc;
 	struct obd_uuid		cluuid;
 	struct obd_export	*exp;
@@ -660,35 +658,6 @@ struct niobuf_local {
 #define LUSTRE_MGS_OBDNAME "MGS"
 #define LUSTRE_MGC_OBDNAME "MGC"
 
-static inline int is_osp_on_mdt(char *name)
-{
-	char   *ptr;
-
-	ptr = strrchr(name, '-');
-	if (ptr == NULL) {
-		CERROR("%s is not a obdname\n", name);
-		return 0;
-	}
-
-	/* 1.8 OSC/OSP name on MDT is fsname-OSTxxxx-osc */
-	if (strncmp(ptr + 1, "osc", 3) == 0)
-		return 1;
-
-	if (strncmp(ptr + 1, "MDT", 3) != 0)
-		return 0;
-
-	while (*(--ptr) != '-' && ptr != name);
-
-	if (ptr == name)
-		return 0;
-
-	if (strncmp(ptr + 1, LUSTRE_OSP_NAME, strlen(LUSTRE_OSP_NAME)) != 0 &&
-	    strncmp(ptr + 1, LUSTRE_OSC_NAME, strlen(LUSTRE_OSC_NAME)) != 0)
-		return 0;
-
-	return 1;
-}
-
 /* Don't conflict with on-wire flags OBD_BRW_WRITE, etc */
 #define N_LOCAL_TEMP_PAGE 0x10000000
 
@@ -708,7 +677,7 @@ struct obd_trans_info {
 	unsigned long		 oti_sync_write:1;
 
 	/* initial thread handling transaction */
-	struct ptlrpc_thread *   oti_thread;
+	struct ptlrpc_thread *oti_thread;
 	__u32		    oti_conn_cnt;
 	/** VBR: versions */
 	__u64		    oti_pre_version;
@@ -733,6 +702,7 @@ static inline void oti_init(struct obd_trans_info *oti,
 	if (req->rq_reqmsg != NULL &&
 	    lustre_msg_get_flags(req->rq_reqmsg) & MSG_REPLAY) {
 		__u64 *pre_version = lustre_msg_get_versions(req->rq_reqmsg);
+
 		oti->oti_pre_version = pre_version ? pre_version[0] : 0;
 		oti->oti_transno = lustre_msg_get_transno(req->rq_reqmsg);
 	}
@@ -745,7 +715,8 @@ static inline void oti_init(struct obd_trans_info *oti,
 		oti->oti_conn_cnt = lustre_msg_get_conn_cnt(req->rq_reqmsg);
 }
 
-static inline void oti_alloc_cookies(struct obd_trans_info *oti,int num_cookies)
+static inline void oti_alloc_cookies(struct obd_trans_info *oti,
+				     int num_cookies)
 {
 	if (!oti)
 		return;
@@ -829,6 +800,10 @@ struct obd_llog_group {
 #define OBD_DEVICE_MAGIC	0XAB5CD6EF
 #define OBD_DEV_BY_DEVNAME      0xffffd0de
 
+struct lvfs_run_ctxt {
+	struct dt_device *dt;
+};
+
 struct obd_device {
 	struct obd_type	*obd_type;
 	__u32		   obd_magic;
@@ -882,7 +857,6 @@ struct obd_device {
 	spinlock_t		obd_dev_lock; /* protect OBD bitfield above */
 	struct mutex		obd_dev_mutex;
 	__u64			obd_last_committed;
-	struct fsfilt_operations *obd_fsops;
 	spinlock_t		obd_osfs_lock;
 	struct obd_statfs	obd_osfs;       /* locked by obd_osfs_lock */
 	__u64			obd_osfs_age;
@@ -908,9 +882,9 @@ struct obd_device {
 	int			      obd_requests_queued_for_recovery;
 	wait_queue_head_t		      obd_next_transno_waitq;
 	/* protected by obd_recovery_task_lock */
-	struct timer_list	      obd_recovery_timer;
-	time_t			   obd_recovery_start; /* seconds */
-	time_t			   obd_recovery_end; /* seconds, for lprocfs_status */
+	struct timer_list	obd_recovery_timer;
+	time_t			obd_recovery_start; /* seconds */
+	time_t			obd_recovery_end; /* seconds, for lprocfs_status */
 	int			      obd_recovery_time_hard;
 	int			      obd_recovery_timeout;
 	int			      obd_recovery_ir_factor;
@@ -946,7 +920,7 @@ struct obd_device {
 	struct lprocfs_stats  *obd_svc_stats;
 	atomic_t	   obd_evict_inprogress;
 	wait_queue_head_t	    obd_evict_inprogress_waitq;
-	struct list_head	     obd_evict_list; /* protected with pet_lock */
+	struct list_head	obd_evict_list; /* protected with pet_lock */
 
 	/**
 	 * Ldlm pool part. Save last calculated SLV and Limit.
@@ -1057,9 +1031,9 @@ struct md_op_data {
 	struct lu_fid	   op_fid2; /* operation fid2 (usually child) */
 	struct lu_fid	   op_fid3; /* 2 extra fids to find conflicting */
 	struct lu_fid	   op_fid4; /* to the operation locks. */
-	mdsno_t		 op_mds;  /* what mds server open will go to */
+	u32			op_mds;  /* what mds server open will go to */
 	struct lustre_handle    op_handle;
-	obd_time		op_mod_time;
+	s64			op_mod_time;
 	const char	     *op_name;
 	int		     op_namelen;
 	__u32		   op_mode;
@@ -1113,16 +1087,14 @@ enum op_cli_flags {
 
 struct md_enqueue_info;
 /* metadata stat-ahead */
-typedef int (* md_enqueue_cb_t)(struct ptlrpc_request *req,
-				struct md_enqueue_info *minfo,
-				int rc);
 
 struct md_enqueue_info {
 	struct md_op_data       mi_data;
 	struct lookup_intent    mi_it;
 	struct lustre_handle    mi_lockh;
 	struct inode	   *mi_dir;
-	md_enqueue_cb_t	 mi_cb;
+	int (*mi_cb)(struct ptlrpc_request *req,
+		struct md_enqueue_info *minfo, int rc);
 	__u64		   mi_cbdata;
 	unsigned int	    mi_generation;
 };
@@ -1138,14 +1110,13 @@ struct obd_ops {
 				__u32 keylen, void *key,
 				__u32 vallen, void *val,
 				struct ptlrpc_request_set *set);
-	int (*o_attach)(struct obd_device *dev, obd_count len, void *data);
+	int (*o_attach)(struct obd_device *dev, u32 len, void *data);
 	int (*o_detach)(struct obd_device *dev);
-	int (*o_setup) (struct obd_device *dev, struct lustre_cfg *cfg);
+	int (*o_setup)(struct obd_device *dev, struct lustre_cfg *cfg);
 	int (*o_precleanup)(struct obd_device *dev,
 			    enum obd_cleanup_stage cleanup_stage);
 	int (*o_cleanup)(struct obd_device *dev);
-	int (*o_process_config)(struct obd_device *dev, obd_count len,
-				void *data);
+	int (*o_process_config)(struct obd_device *dev, u32 len, void *data);
 	int (*o_postrecov)(struct obd_device *dev);
 	int (*o_add_conn)(struct obd_import *imp, struct obd_uuid *uuid,
 			  int priority);
@@ -1184,18 +1155,14 @@ struct obd_ops {
 			      __u64 max_age, struct ptlrpc_request_set *set);
 	int (*o_packmd)(struct obd_export *exp, struct lov_mds_md **disk_tgt,
 			struct lov_stripe_md *mem_src);
-	int (*o_unpackmd)(struct obd_export *exp,struct lov_stripe_md **mem_tgt,
+	int (*o_unpackmd)(struct obd_export *exp,
+			  struct lov_stripe_md **mem_tgt,
 			  struct lov_mds_md *disk_src, int disk_len);
-	int (*o_preallocate)(struct lustre_handle *, obd_count *req,
-			     obd_id *ids);
+	int (*o_preallocate)(struct lustre_handle *, u32 *req, u64 *ids);
 	/* FIXME: add fid capability support for create & destroy! */
-	int (*o_precreate)(struct obd_export *exp);
 	int (*o_create)(const struct lu_env *env, struct obd_export *exp,
 			struct obdo *oa, struct lov_stripe_md **ea,
 			struct obd_trans_info *oti);
-	int (*o_create_async)(struct obd_export *exp,  struct obd_info *oinfo,
-			      struct lov_stripe_md **ea,
-			      struct obd_trans_info *oti);
 	int (*o_destroy)(const struct lu_env *env, struct obd_export *exp,
 			 struct obdo *oa, struct lov_stripe_md *ea,
 			 struct obd_trans_info *oti, struct obd_export *md_exp,
@@ -1209,28 +1176,8 @@ struct obd_ops {
 			 struct obd_info *oinfo);
 	int (*o_getattr_async)(struct obd_export *exp, struct obd_info *oinfo,
 			       struct ptlrpc_request_set *set);
-	int (*o_brw)(int rw, struct obd_export *exp, struct obd_info *oinfo,
-		     obd_count oa_bufs, struct brw_page *pgarr,
-		     struct obd_trans_info *oti);
-	int (*o_merge_lvb)(struct obd_export *exp, struct lov_stripe_md *lsm,
-			   struct ost_lvb *lvb, int kms_only);
 	int (*o_adjust_kms)(struct obd_export *exp, struct lov_stripe_md *lsm,
-			    obd_off size, int shrink);
-	int (*o_punch)(const struct lu_env *, struct obd_export *exp,
-		       struct obd_info *oinfo, struct obd_trans_info *oti,
-		       struct ptlrpc_request_set *rqset);
-	int (*o_sync)(const struct lu_env *env, struct obd_export *exp,
-		      struct obd_info *oinfo, obd_size start, obd_size end,
-		      struct ptlrpc_request_set *set);
-	int (*o_migrate)(struct lustre_handle *conn, struct lov_stripe_md *dst,
-			 struct lov_stripe_md *src, obd_size start,
-			 obd_size end, struct obd_trans_info *oti);
-	int (*o_copy)(struct lustre_handle *dstconn, struct lov_stripe_md *dst,
-		      struct lustre_handle *srconn, struct lov_stripe_md *src,
-		      obd_size start, obd_size end, struct obd_trans_info *);
-	int (*o_iterate)(struct lustre_handle *conn,
-			 int (*)(obd_id, obd_seq, void *),
-			 obd_id *startid, obd_seq seq, void *data);
+			    u64 size, int shrink);
 	int (*o_preprw)(const struct lu_env *env, int cmd,
 			struct obd_export *exp, struct obdo *oa, int objcount,
 			struct obd_ioobj *obj, struct niobuf_remote *remote,
@@ -1242,33 +1189,12 @@ struct obd_ops {
 			  struct niobuf_remote *remote, int pages,
 			  struct niobuf_local *local,
 			  struct obd_trans_info *oti, int rc);
-	int (*o_enqueue)(struct obd_export *, struct obd_info *oinfo,
-			 struct ldlm_enqueue_info *einfo,
-			 struct ptlrpc_request_set *rqset);
-	int (*o_change_cbdata)(struct obd_export *, struct lov_stripe_md *,
-			       ldlm_iterator_t it, void *data);
 	int (*o_find_cbdata)(struct obd_export *, struct lov_stripe_md *,
 			     ldlm_iterator_t it, void *data);
-	int (*o_cancel)(struct obd_export *, struct lov_stripe_md *md,
-			__u32 mode, struct lustre_handle *);
-	int (*o_cancel_unused)(struct obd_export *, struct lov_stripe_md *,
-			       ldlm_cancel_flags_t flags, void *opaque);
 	int (*o_init_export)(struct obd_export *exp);
 	int (*o_destroy_export)(struct obd_export *exp);
-	int (*o_extent_calc)(struct obd_export *, struct lov_stripe_md *,
-			     int cmd, obd_off *);
-
-	/* llog related obd_methods */
-	int (*o_llog_init)(struct obd_device *obd, struct obd_llog_group *grp,
-			   struct obd_device *disk_obd, int *idx);
-	int (*o_llog_finish)(struct obd_device *obd, int count);
-	int (*o_llog_connect)(struct obd_export *, struct llogd_conn_body *);
 
 	/* metadata-only methods */
-	int (*o_pin)(struct obd_export *, const struct lu_fid *fid,
-		     struct obd_capa *, struct obd_client_handle *, int flag);
-	int (*o_unpin)(struct obd_export *, struct obd_client_handle *, int);
-
 	int (*o_import_event)(struct obd_device *, struct obd_import *,
 			      enum obd_import_event);
 
@@ -1276,7 +1202,7 @@ struct obd_ops {
 			enum obd_notify_event ev, void *data);
 
 	int (*o_health_check)(const struct lu_env *env, struct obd_device *);
-	struct obd_uuid *(*o_get_uuid) (struct obd_export *exp);
+	struct obd_uuid *(*o_get_uuid)(struct obd_export *exp);
 
 	/* quota methods */
 	int (*o_quotacheck)(struct obd_device *, struct obd_export *,
@@ -1284,8 +1210,6 @@ struct obd_ops {
 	int (*o_quotactl)(struct obd_device *, struct obd_export *,
 			  struct obd_quotactl *);
 
-	int (*o_ping)(const struct lu_env *, struct obd_export *exp);
-
 	/* pools methods */
 	int (*o_pool_new)(struct obd_device *obd, char *poolname);
 	int (*o_pool_del)(struct obd_device *obd, char *poolname);
@@ -1385,12 +1309,12 @@ struct md_ops {
 			struct ptlrpc_request **);
 
 	int (*m_setxattr)(struct obd_export *, const struct lu_fid *,
-			  struct obd_capa *, obd_valid, const char *,
+			  struct obd_capa *, u64, const char *,
 			  const char *, int, int, int, __u32,
 			  struct ptlrpc_request **);
 
 	int (*m_getxattr)(struct obd_export *, const struct lu_fid *,
-			  struct obd_capa *, obd_valid, const char *,
+			  struct obd_capa *, u64, const char *,
 			  const char *, int, int, int,
 			  struct ptlrpc_request **);
 
@@ -1444,13 +1368,13 @@ struct lsm_operations {
 	void (*lsm_free)(struct lov_stripe_md *);
 	int (*lsm_destroy)(struct lov_stripe_md *, struct obdo *oa,
 			   struct obd_export *md_exp);
-	void (*lsm_stripe_by_index)(struct lov_stripe_md *, int *, obd_off *,
-				    obd_off *);
-	void (*lsm_stripe_by_offset)(struct lov_stripe_md *, int *, obd_off *,
-				     obd_off *);
-	int (*lsm_lmm_verify) (struct lov_mds_md *lmm, int lmm_bytes,
+	void (*lsm_stripe_by_index)(struct lov_stripe_md *, int *, u64 *,
+				    u64 *);
+	void (*lsm_stripe_by_offset)(struct lov_stripe_md *, int *, u64 *,
+				     u64 *);
+	int (*lsm_lmm_verify)(struct lov_mds_md *lmm, int lmm_bytes,
 			       __u16 *stripe_count);
-	int (*lsm_unpackmd) (struct lov_obd *lov, struct lov_stripe_md *lsm,
+	int (*lsm_unpackmd)(struct lov_obd *lov, struct lov_stripe_md *lsm,
 			     struct lov_mds_md *lmm);
 };
 
@@ -1458,7 +1382,7 @@ extern const struct lsm_operations lsm_v1_ops;
 extern const struct lsm_operations lsm_v3_ops;
 static inline const struct lsm_operations *lsm_op_find(int magic)
 {
-	switch(magic) {
+	switch (magic) {
 	case LOV_MAGIC_V1:
 	       return &lsm_v1_ops;
 	case LOV_MAGIC_V3:
@@ -1481,6 +1405,7 @@ static inline struct lustre_capa *oinfo_capa(struct obd_info *oinfo)
 static inline struct md_open_data *obd_mod_alloc(void)
 {
 	struct md_open_data *mod;
+
 	OBD_ALLOC_PTR(mod);
 	if (mod == NULL)
 		return NULL;
@@ -1498,7 +1423,7 @@ static inline struct md_open_data *obd_mod_alloc(void)
 	}						       \
 })
 
-void obdo_from_inode(struct obdo *dst, struct inode *src, obd_flag valid);
+void obdo_from_inode(struct obdo *dst, struct inode *src, u32 valid);
 void obdo_set_parent_fid(struct obdo *dst, const struct lu_fid *parent);
 
 /* return 1 if client should be resend request */
diff --git a/drivers/staging/lustre/lustre/include/obd_cksum.h b/drivers/staging/lustre/lustre/include/obd_cksum.h
index 662a78062963..3a63462aa943 100644
--- a/drivers/staging/lustre/lustre/include/obd_cksum.h
+++ b/drivers/staging/lustre/lustre/include/obd_cksum.h
@@ -64,10 +64,10 @@ static inline unsigned char cksum_obd2cfs(cksum_type_t cksum_type)
  * because that is supported by all clients since 1.8
  *
  * In case multiple algorithms are supported the best one is used. */
-static inline obd_flag cksum_type_pack(cksum_type_t cksum_type)
+static inline u32 cksum_type_pack(cksum_type_t cksum_type)
 {
 	unsigned int    performance = 0, tmp;
-	obd_flag	flag = OBD_FL_CKSUM_ADLER;
+	u32		flag = OBD_FL_CKSUM_ADLER;
 
 	if (cksum_type & OBD_CKSUM_CRC32) {
 		tmp = cfs_crypto_hash_speed(cksum_obd2cfs(OBD_CKSUM_CRC32));
@@ -98,7 +98,7 @@ static inline obd_flag cksum_type_pack(cksum_type_t cksum_type)
 	return flag;
 }
 
-static inline cksum_type_t cksum_type_unpack(obd_flag o_flags)
+static inline cksum_type_t cksum_type_unpack(u32 o_flags)
 {
 	switch (o_flags & OBD_FL_CKSUM_ALL) {
 	case OBD_FL_CKSUM_CRC32C:
diff --git a/drivers/staging/lustre/lustre/include/obd_class.h b/drivers/staging/lustre/lustre/include/obd_class.h
index 1d401c9e5e8c..882e40bd584c 100644
--- a/drivers/staging/lustre/lustre/include/obd_class.h
+++ b/drivers/staging/lustre/lustre/include/obd_class.h
@@ -45,8 +45,6 @@
 #include "lustre/lustre_idl.h"
 #include "lprocfs_status.h"
 
-#include "linux/obd_class.h"
-
 #define OBD_STATFS_NODELAY      0x0001  /* requests should be send without delay
 					 * and resends for avoid deadlocks */
 #define OBD_STATFS_FROM_CACHE   0x0002  /* the statfs callback should not update
@@ -88,12 +86,12 @@ struct obd_device *class_name2obd(const char *name);
 int class_uuid2dev(struct obd_uuid *uuid);
 struct obd_device *class_uuid2obd(struct obd_uuid *uuid);
 void class_obd_list(void);
-struct obd_device * class_find_client_obd(struct obd_uuid *tgt_uuid,
-					  const char * typ_name,
+struct obd_device *class_find_client_obd(struct obd_uuid *tgt_uuid,
+					  const char *typ_name,
 					  struct obd_uuid *grp_uuid);
-struct obd_device * class_devices_in_group(struct obd_uuid *grp_uuid,
+struct obd_device *class_devices_in_group(struct obd_uuid *grp_uuid,
 					   int *next);
-struct obd_device * class_num2obd(int num);
+struct obd_device *class_num2obd(int num);
 int get_devices_count(void);
 
 int class_notify_sptlrpc_conf(const char *fsname, int namelen);
@@ -110,7 +108,7 @@ void obd_zombie_impexp_cull(void);
 void obd_zombie_barrier(void);
 void obd_exports_barrier(struct obd_device *obd);
 int kuc_len(int payload_len);
-struct kuc_hdr * kuc_ptr(void *p);
+struct kuc_hdr *kuc_ptr(void *p);
 int kuc_ispayload(void *p);
 void *kuc_alloc(int payload_len, int transport, int type);
 void kuc_free(void *p, int payload_len);
@@ -207,7 +205,7 @@ struct lustre_profile {
 	char	    *lp_md;
 };
 
-struct lustre_profile *class_get_profile(const char * prof);
+struct lustre_profile *class_get_profile(const char *prof);
 void class_del_profile(const char *prof);
 void class_del_profiles(void);
 
@@ -219,8 +217,8 @@ extern void (*class_export_dump_hook)(struct obd_export *);
 
 #else
 
-#define __class_export_add_lock_ref(exp, lock)	     do {} while(0)
-#define __class_export_del_lock_ref(exp, lock)	     do {} while(0)
+#define __class_export_add_lock_ref(exp, lock)	     do {} while (0)
+#define __class_export_del_lock_ref(exp, lock)	     do {} while (0)
 
 #endif
 
@@ -307,13 +305,20 @@ static inline enum obd_option exp_flags_from_obd(struct obd_device *obd)
 		0);
 }
 
+struct inode;
+struct lu_attr;
+struct obdo;
+void obdo_from_la(struct obdo *dst, struct lu_attr *la, __u64 valid);
+void la_from_obdo(struct lu_attr *la, struct obdo *dst, u32 valid);
+void obdo_refresh_inode(struct inode *dst, struct obdo *src, u32 valid);
+void obdo_to_inode(struct inode *dst, struct obdo *src, u32 valid);
 
-void obdo_cpy_md(struct obdo *dst, struct obdo *src, obd_flag valid);
+void obdo_cpy_md(struct obdo *dst, struct obdo *src, u32 valid);
 void obdo_to_ioobj(struct obdo *oa, struct obd_ioobj *ioobj);
 void obdo_from_iattr(struct obdo *oa, struct iattr *attr,
 		     unsigned int ia_valid);
-void iattr_from_obdo(struct iattr *attr, struct obdo *oa, obd_flag valid);
-void md_from_obdo(struct md_op_data *op_data, struct obdo *oa, obd_flag valid);
+void iattr_from_obdo(struct iattr *attr, struct obdo *oa, u32 valid);
+void md_from_obdo(struct md_op_data *op_data, struct obdo *oa, u32 valid);
 void obdo_from_md(struct obdo *oa, struct md_op_data *op_data,
 		  unsigned int valid);
 
@@ -510,8 +515,8 @@ static inline int obd_get_info(const struct lu_env *env,
 }
 
 static inline int obd_set_info_async(const struct lu_env *env,
-				     struct obd_export *exp, obd_count keylen,
-				     void *key, obd_count vallen, void *val,
+				     struct obd_export *exp, u32 keylen,
+				     void *key, u32 vallen, void *val,
 				     struct ptlrpc_request_set *set)
 {
 	int rc;
@@ -771,31 +776,6 @@ static inline int obd_free_memmd(struct obd_export *exp,
 	return rc;
 }
 
-static inline int obd_precreate(struct obd_export *exp)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, precreate);
-	OBD_COUNTER_INCREMENT(exp->exp_obd, precreate);
-
-	rc = OBP(exp->exp_obd, precreate)(exp);
-	return rc;
-}
-
-static inline int obd_create_async(struct obd_export *exp,
-				   struct obd_info *oinfo,
-				   struct lov_stripe_md **ea,
-				   struct obd_trans_info *oti)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, create_async);
-	EXP_COUNTER_INCREMENT(exp, create_async);
-
-	rc = OBP(exp->exp_obd, create_async)(exp, oinfo, ea, oti);
-	return rc;
-}
-
 static inline int obd_create(const struct lu_env *env, struct obd_export *exp,
 			     struct obdo *obdo, struct lov_stripe_md **ea,
 			     struct obd_trans_info *oti)
@@ -943,7 +923,7 @@ static inline struct obd_uuid *obd_get_uuid(struct obd_export *exp)
  *    by obd are returned.
  */
 static inline int obd_connect(const struct lu_env *env,
-			      struct obd_export **exp,struct obd_device *obd,
+			      struct obd_export **exp, struct obd_device *obd,
 			      struct obd_uuid *cluuid,
 			      struct obd_connect_data *data,
 			      void *localdata)
@@ -1032,17 +1012,6 @@ static inline int obd_fid_alloc(struct obd_export *exp,
 	return rc;
 }
 
-static inline int obd_ping(const struct lu_env *env, struct obd_export *exp)
-{
-	int rc;
-
-	OBD_CHECK_DT_OP(exp->exp_obd, ping, 0);
-	EXP_COUNTER_INCREMENT(exp, ping);
-
-	rc = OBP(exp->exp_obd, ping)(env, exp);
-	return rc;
-}
-
 static inline int obd_pool_new(struct obd_device *obd, char *poolname)
 {
 	int rc;
@@ -1121,27 +1090,6 @@ static inline int obd_destroy_export(struct obd_export *exp)
 	return 0;
 }
 
-static inline int obd_extent_calc(struct obd_export *exp,
-				  struct lov_stripe_md *md,
-				  int cmd, obd_off *offset)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, extent_calc);
-	rc = OBP(exp->exp_obd, extent_calc)(exp, md, cmd, offset);
-	return rc;
-}
-
-static inline struct dentry *
-obd_lvfs_fid2dentry(struct obd_export *exp, struct ost_id *oi, __u32 gen)
-{
-	struct lvfs_run_ctxt *ctxt = &exp->exp_obd->obd_lvfs_ctxt;
-	LASSERT(exp->exp_obd);
-
-	return ctxt->cb_ops.l_fid2dentry(ostid_id(oi), gen, ostid_seq(oi),
-					 exp->exp_obd);
-}
-
 /* @max_age is the oldest time in jiffies that we accept using a cached data.
  * If the cache is older than @max_age we will get a new value from the
  * target.  Use a value of "cfs_time_current() + HZ" to guarantee freshness. */
@@ -1165,7 +1113,8 @@ static inline int obd_statfs_async(struct obd_export *exp,
 	if (cfs_time_before_64(obd->obd_osfs_age, max_age)) {
 		rc = OBP(obd, statfs_async)(exp, oinfo, max_age, rqset);
 	} else {
-		CDEBUG(D_SUPER,"%s: use %p cache blocks %llu/%llu objects %llu/%llu\n",
+		CDEBUG(D_SUPER,
+		       "%s: use %p cache blocks %llu/%llu objects %llu/%llu\n",
 		       obd->obd_name, &obd->obd_osfs,
 		       obd->obd_osfs.os_bavail, obd->obd_osfs.os_blocks,
 		       obd->obd_osfs.os_ffree, obd->obd_osfs.os_files);
@@ -1238,92 +1187,6 @@ static inline int obd_statfs(const struct lu_env *env, struct obd_export *exp,
 	return rc;
 }
 
-static inline int obd_sync_rqset(struct obd_export *exp, struct obd_info *oinfo,
-				 obd_size start, obd_size end)
-{
-	struct ptlrpc_request_set *set = NULL;
-	int rc;
-
-	OBD_CHECK_DT_OP(exp->exp_obd, sync, -EOPNOTSUPP);
-	EXP_COUNTER_INCREMENT(exp, sync);
-
-	set =  ptlrpc_prep_set();
-	if (set == NULL)
-		return -ENOMEM;
-
-	rc = OBP(exp->exp_obd, sync)(NULL, exp, oinfo, start, end, set);
-	if (rc == 0)
-		rc = ptlrpc_set_wait(set);
-	ptlrpc_set_destroy(set);
-	return rc;
-}
-
-static inline int obd_sync(const struct lu_env *env, struct obd_export *exp,
-			   struct obd_info *oinfo, obd_size start, obd_size end,
-			   struct ptlrpc_request_set *set)
-{
-	int rc;
-
-	OBD_CHECK_DT_OP(exp->exp_obd, sync, -EOPNOTSUPP);
-	EXP_COUNTER_INCREMENT(exp, sync);
-
-	rc = OBP(exp->exp_obd, sync)(env, exp, oinfo, start, end, set);
-	return rc;
-}
-
-static inline int obd_punch_rqset(struct obd_export *exp,
-				  struct obd_info *oinfo,
-				  struct obd_trans_info *oti)
-{
-	struct ptlrpc_request_set *set = NULL;
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, punch);
-	EXP_COUNTER_INCREMENT(exp, punch);
-
-	set =  ptlrpc_prep_set();
-	if (set == NULL)
-		return -ENOMEM;
-
-	rc = OBP(exp->exp_obd, punch)(NULL, exp, oinfo, oti, set);
-	if (rc == 0)
-		rc = ptlrpc_set_wait(set);
-	ptlrpc_set_destroy(set);
-	return rc;
-}
-
-static inline int obd_punch(const struct lu_env *env, struct obd_export *exp,
-			    struct obd_info *oinfo, struct obd_trans_info *oti,
-			    struct ptlrpc_request_set *rqset)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, punch);
-	EXP_COUNTER_INCREMENT(exp, punch);
-
-	rc = OBP(exp->exp_obd, punch)(env, exp, oinfo, oti, rqset);
-	return rc;
-}
-
-static inline int obd_brw(int cmd, struct obd_export *exp,
-			  struct obd_info *oinfo, obd_count oa_bufs,
-			  struct brw_page *pg, struct obd_trans_info *oti)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, brw);
-	EXP_COUNTER_INCREMENT(exp, brw);
-
-	if (!(cmd & (OBD_BRW_RWMASK | OBD_BRW_CHECK))) {
-		CERROR("obd_brw: cmd must be OBD_BRW_READ, OBD_BRW_WRITE, "
-		       "or OBD_BRW_CHECK\n");
-		LBUG();
-	}
-
-	rc = OBP(exp->exp_obd, brw)(cmd, exp, oinfo, oa_bufs, pg, oti);
-	return rc;
-}
-
 static inline int obd_preprw(const struct lu_env *env, int cmd,
 			     struct obd_export *exp, struct obdo *oa,
 			     int objcount, struct obd_ioobj *obj,
@@ -1357,21 +1220,8 @@ static inline int obd_commitrw(const struct lu_env *env, int cmd,
 	return rc;
 }
 
-static inline int obd_merge_lvb(struct obd_export *exp,
-				struct lov_stripe_md *lsm,
-				struct ost_lvb *lvb, int kms_only)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, merge_lvb);
-	EXP_COUNTER_INCREMENT(exp, merge_lvb);
-
-	rc = OBP(exp->exp_obd, merge_lvb)(exp, lsm, lvb, kms_only);
-	return rc;
-}
-
 static inline int obd_adjust_kms(struct obd_export *exp,
-				 struct lov_stripe_md *lsm, obd_off size,
+				 struct lov_stripe_md *lsm, u64 size,
 				 int shrink)
 {
 	int rc;
@@ -1395,54 +1245,6 @@ static inline int obd_iocontrol(unsigned int cmd, struct obd_export *exp,
 	return rc;
 }
 
-static inline int obd_enqueue_rqset(struct obd_export *exp,
-				    struct obd_info *oinfo,
-				    struct ldlm_enqueue_info *einfo)
-{
-	struct ptlrpc_request_set *set = NULL;
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, enqueue);
-	EXP_COUNTER_INCREMENT(exp, enqueue);
-
-	set =  ptlrpc_prep_set();
-	if (set == NULL)
-		return -ENOMEM;
-
-	rc = OBP(exp->exp_obd, enqueue)(exp, oinfo, einfo, set);
-	if (rc == 0)
-		rc = ptlrpc_set_wait(set);
-	ptlrpc_set_destroy(set);
-	return rc;
-}
-
-static inline int obd_enqueue(struct obd_export *exp,
-			      struct obd_info *oinfo,
-			      struct ldlm_enqueue_info *einfo,
-			      struct ptlrpc_request_set *set)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, enqueue);
-	EXP_COUNTER_INCREMENT(exp, enqueue);
-
-	rc = OBP(exp->exp_obd, enqueue)(exp, oinfo, einfo, set);
-	return rc;
-}
-
-static inline int obd_change_cbdata(struct obd_export *exp,
-				    struct lov_stripe_md *lsm,
-				    ldlm_iterator_t it, void *data)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, change_cbdata);
-	EXP_COUNTER_INCREMENT(exp, change_cbdata);
-
-	rc = OBP(exp->exp_obd, change_cbdata)(exp, lsm, it, data);
-	return rc;
-}
-
 static inline int obd_find_cbdata(struct obd_export *exp,
 				  struct lov_stripe_md *lsm,
 				  ldlm_iterator_t it, void *data)
@@ -1456,59 +1258,6 @@ static inline int obd_find_cbdata(struct obd_export *exp,
 	return rc;
 }
 
-static inline int obd_cancel(struct obd_export *exp,
-			     struct lov_stripe_md *ea, __u32 mode,
-			     struct lustre_handle *lockh)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, cancel);
-	EXP_COUNTER_INCREMENT(exp, cancel);
-
-	rc = OBP(exp->exp_obd, cancel)(exp, ea, mode, lockh);
-	return rc;
-}
-
-static inline int obd_cancel_unused(struct obd_export *exp,
-				    struct lov_stripe_md *ea,
-				    ldlm_cancel_flags_t flags,
-				    void *opaque)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, cancel_unused);
-	EXP_COUNTER_INCREMENT(exp, cancel_unused);
-
-	rc = OBP(exp->exp_obd, cancel_unused)(exp, ea, flags, opaque);
-	return rc;
-}
-
-static inline int obd_pin(struct obd_export *exp, const struct lu_fid *fid,
-			  struct obd_capa *oc, struct obd_client_handle *handle,
-			  int flag)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, pin);
-	EXP_COUNTER_INCREMENT(exp, pin);
-
-	rc = OBP(exp->exp_obd, pin)(exp, fid, oc, handle, flag);
-	return rc;
-}
-
-static inline int obd_unpin(struct obd_export *exp,
-			    struct obd_client_handle *handle, int flag)
-{
-	int rc;
-
-	EXP_CHECK_DT_OP(exp, unpin);
-	EXP_COUNTER_INCREMENT(exp, unpin);
-
-	rc = OBP(exp->exp_obd, unpin)(exp, handle, flag);
-	return rc;
-}
-
-
 static inline void obd_import_event(struct obd_device *obd,
 				    struct obd_import *imp,
 				    enum obd_import_event event)
@@ -1523,19 +1272,6 @@ static inline void obd_import_event(struct obd_device *obd,
 	}
 }
 
-static inline int obd_llog_connect(struct obd_export *exp,
-				   struct llogd_conn_body *body)
-{
-	int rc;
-
-	OBD_CHECK_DT_OP(exp->exp_obd, llog_connect, 0);
-	EXP_COUNTER_INCREMENT(exp, llog_connect);
-
-	rc = OBP(exp->exp_obd, llog_connect)(exp, body);
-	return rc;
-}
-
-
 static inline int obd_notify(struct obd_device *obd,
 			     struct obd_device *watched,
 			     enum obd_notify_event ev,
@@ -1655,25 +1391,6 @@ static inline int obd_register_observer(struct obd_device *obd,
 	return 0;
 }
 
-static inline int obd_pin_observer(struct obd_device *obd,
-				   struct obd_device **observer)
-{
-	down_read(&obd->obd_observer_link_sem);
-	if (!obd->obd_observer) {
-		*observer = NULL;
-		up_read(&obd->obd_observer_link_sem);
-		return -ENOENT;
-	}
-	*observer = obd->obd_observer;
-	return 0;
-}
-
-static inline int obd_unpin_observer(struct obd_device *obd)
-{
-	up_read(&obd->obd_observer_link_sem);
-	return 0;
-}
-
 #if 0
 static inline int obd_register_page_removal_cb(struct obd_export *exp,
 					       obd_page_removal_cb_t cb,
@@ -1962,7 +1679,7 @@ static inline int md_free_lustre_md(struct obd_export *exp,
 
 static inline int md_setxattr(struct obd_export *exp,
 			      const struct lu_fid *fid, struct obd_capa *oc,
-			      obd_valid valid, const char *name,
+			      u64 valid, const char *name,
 			      const char *input, int input_size,
 			      int output_size, int flags, __u32 suppgid,
 			      struct ptlrpc_request **request)
@@ -1976,7 +1693,7 @@ static inline int md_setxattr(struct obd_export *exp,
 
 static inline int md_getxattr(struct obd_export *exp,
 			      const struct lu_fid *fid, struct obd_capa *oc,
-			      obd_valid valid, const char *name,
+			      u64 valid, const char *name,
 			      const char *input, int input_size,
 			      int output_size, int flags,
 			      struct ptlrpc_request **request)
@@ -2124,12 +1841,12 @@ extern struct kmem_cache *obdo_cachep;
 #define OBDO_ALLOC(ptr)						       \
 do {									  \
 	OBD_SLAB_ALLOC_PTR_GFP((ptr), obdo_cachep, GFP_NOFS);             \
-} while(0)
+} while (0)
 
 #define OBDO_FREE(ptr)							\
 do {									  \
 	OBD_SLAB_FREE_PTR((ptr), obdo_cachep);				\
-} while(0)
+} while (0)
 
 
 static inline void obdo2fid(struct obdo *oa, struct lu_fid *fid)
@@ -2176,10 +1893,6 @@ int class_check_uuid(struct obd_uuid *uuid, __u64 nid);
 void class_init_uuidlist(void);
 void class_exit_uuidlist(void);
 
-/* mea.c */
-int mea_name2idx(struct lmv_stripe_md *mea, const char *name, int namelen);
-int raw_name2idx(int hashtype, int count, const char *name, int namelen);
-
 /* class_obd.c */
 extern char obd_jobid_node[];
 
diff --git a/drivers/staging/lustre/lustre/include/obd_ost.h b/drivers/staging/lustre/lustre/include/obd_ost.h
deleted file mode 100644
index 60de42972ec9..000000000000
--- a/drivers/staging/lustre/lustre/include/obd_ost.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/include/obd_ost.h
- *
- * Data structures for object storage targets and client: OST & OSC's
- *
- * See also lustre_idl.h for wire formats of requests.
- */
-
-#ifndef _LUSTRE_OST_H
-#define _LUSTRE_OST_H
-
-#include "obd_class.h"
-
-struct osc_brw_async_args {
-	struct obdo       *aa_oa;
-	int		aa_requested_nob;
-	int		aa_nio_count;
-	obd_count	  aa_page_count;
-	int		aa_resends;
-	struct brw_page  **aa_ppga;
-	struct client_obd *aa_cli;
-	struct list_head	 aa_oaps;
-	struct list_head	 aa_exts;
-	struct obd_capa   *aa_ocapa;
-	struct cl_req     *aa_clerq;
-};
-
-#define osc_grant_args osc_brw_async_args
-struct osc_async_args {
-	struct obd_info   *aa_oi;
-};
-
-struct osc_setattr_args {
-	struct obdo	 *sa_oa;
-	obd_enqueue_update_f sa_upcall;
-	void		*sa_cookie;
-};
-
-struct osc_fsync_args {
-	struct obd_info     *fa_oi;
-	obd_enqueue_update_f fa_upcall;
-	void		*fa_cookie;
-};
-
-struct osc_enqueue_args {
-	struct obd_export	*oa_exp;
-	__u64		    *oa_flags;
-	obd_enqueue_update_f      oa_upcall;
-	void		     *oa_cookie;
-	struct ost_lvb	   *oa_lvb;
-	struct lustre_handle     *oa_lockh;
-	struct ldlm_enqueue_info *oa_ei;
-	unsigned int	      oa_agl:1;
-};
-
-extern void osc_update_enqueue(struct lustre_handle *lov_lockhp,
-			       struct lov_oinfo *loi, __u64 flags,
-			       struct ost_lvb *lvb, __u32 mode, int rc);
-
-#if 0
-int osc_extent_blocking_cb(struct ldlm_lock *lock,
-			   struct ldlm_lock_desc *new, void *data,
-			   int flag);
-#endif
-
-#endif
diff --git a/drivers/staging/lustre/lustre/include/obd_support.h b/drivers/staging/lustre/lustre/include/obd_support.h
index 92c89925ff67..2991d2ee780b 100644
--- a/drivers/staging/lustre/lustre/include/obd_support.h
+++ b/drivers/staging/lustre/lustre/include/obd_support.h
@@ -37,10 +37,10 @@
 #ifndef _OBD_SUPPORT
 #define _OBD_SUPPORT
 
+#include <linux/slab.h>
 #include "../../include/linux/libcfs/libcfs.h"
-#include "lvfs.h"
+#include "linux/lustre_compat25.h"
 #include "lprocfs_status.h"
-#include "linux/obd_support.h"
 
 /* global variables */
 extern struct lprocfs_stats *obd_memory;
@@ -127,12 +127,12 @@ int obd_alloc_fail(const void *ptr, const char *name, const char *type,
  /* Max connect interval for nonresponsive servers; ~50s to avoid building up
     connect requests in the LND queues, but within obd_timeout so we don't
     miss the recovery window */
-#define CONNECTION_SWITCH_MAX min(50U, max(CONNECTION_SWITCH_MIN,obd_timeout))
+#define CONNECTION_SWITCH_MAX min(50U, max(CONNECTION_SWITCH_MIN, obd_timeout))
 #define CONNECTION_SWITCH_INC 5  /* Connection timeout backoff */
 /* In general this should be low to have quick detection of a system
    running on a backup server. (If it's too low, import_select_connection
    will increase the timeout anyhow.)  */
-#define INITIAL_CONNECT_TIMEOUT max(CONNECTION_SWITCH_MIN,obd_timeout/20)
+#define INITIAL_CONNECT_TIMEOUT max(CONNECTION_SWITCH_MIN, obd_timeout/20)
 /* The max delay between connects is SWITCH_MAX + SWITCH_INC + INITIAL */
 #define RECONNECT_DELAY_MAX (CONNECTION_SWITCH_MAX + CONNECTION_SWITCH_INC + \
 			     INITIAL_CONNECT_TIMEOUT)
@@ -402,6 +402,7 @@ int obd_alloc_fail(const void *ptr, const char *name, const char *type,
 #define OBD_FAIL_TGT_LAST_REPLAY	 0x710
 #define OBD_FAIL_TGT_CLIENT_ADD	  0x711
 #define OBD_FAIL_TGT_RCVG_FLAG	   0x712
+#define OBD_FAIL_TGT_DELAY_CONDITIONAL	 0x713
 
 #define OBD_FAIL_MDC_REVALIDATE_PAUSE    0x800
 #define OBD_FAIL_MDC_ENQUEUE_PAUSE       0x801
@@ -667,7 +668,7 @@ do {									      \
 	} else {							      \
 		OBD_ALLOC_POST(ptr, size, "vmalloced");		       \
 	}								     \
-} while(0)
+} while (0)
 
 # define OBD_VMALLOC(ptr, size)						      \
 	 __OBD_VMALLOC_VEROBSE(ptr, NULL, 0, size)
@@ -729,7 +730,7 @@ do {									  \
 	OBD_FREE_PRE(ptr, size, "kfreed");				    \
 	kfree(ptr);							\
 	POISON_PTR(ptr);						      \
-} while(0)
+} while (0)
 
 
 #define OBD_FREE_RCU(ptr, size, handle)					      \
@@ -741,7 +742,7 @@ do {									      \
 	__h->h_size = (size);						      \
 	call_rcu(&__h->h_rcu, class_handle_free_cb);			      \
 	POISON_PTR(ptr);						      \
-} while(0)
+} while (0)
 
 
 #define OBD_VFREE(ptr, size)				\
@@ -775,7 +776,7 @@ do {									      \
 		    OBD_SLAB_FREE_RTN0(ptr, slab)))) {			\
 		OBD_ALLOC_POST(ptr, size, "slab-alloced");		    \
 	}								     \
-} while(0)
+} while (0)
 
 #define OBD_SLAB_ALLOC_GFP(ptr, slab, size, flags)			      \
 	__OBD_SLAB_ALLOC_VERBOSE(ptr, slab, NULL, 0, size, flags)
@@ -789,7 +790,7 @@ do {									  \
 	OBD_FREE_PRE(ptr, size, "slab-freed");				\
 	kmem_cache_free(slab, ptr);					\
 	POISON_PTR(ptr);						      \
-} while(0)
+} while (0)
 
 #define OBD_SLAB_ALLOC(ptr, slab, size)					      \
 	OBD_SLAB_ALLOC_GFP(ptr, slab, size, GFP_NOFS)
diff --git a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
index 94f759d0b5ad..24d26ab35346 100644
--- a/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
+++ b/drivers/staging/lustre/lustre/lclient/lcommon_cl.c
@@ -895,11 +895,11 @@ void ccc_req_completion(const struct lu_env *env,
 void ccc_req_attr_set(const struct lu_env *env,
 		      const struct cl_req_slice *slice,
 		      const struct cl_object *obj,
-		      struct cl_req_attr *attr, obd_valid flags)
+		      struct cl_req_attr *attr, u64 flags)
 {
 	struct inode *inode;
 	struct obdo  *oa;
-	obd_flag      valid_flags;
+	u32	      valid_flags;
 
 	oa = attr->cra_oa;
 	inode = ccc_object_inode(obj);
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
index fda9926bea3b..c21e30a074b9 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lib.c
@@ -74,8 +74,10 @@ static int import_set_conn(struct obd_import *imp, struct obd_uuid *uuid,
 
 	if (create) {
 		OBD_ALLOC(imp_conn, sizeof(*imp_conn));
-		if (!imp_conn)
-			GOTO(out_put, rc = -ENOMEM);
+		if (!imp_conn) {
+			rc = -ENOMEM;
+			goto out_put;
+		}
 	}
 
 	spin_lock(&imp->imp_lock);
@@ -91,7 +93,8 @@ static int import_set_conn(struct obd_import *imp, struct obd_uuid *uuid,
 			       imp, imp->imp_obd->obd_name, uuid->uuid,
 			       (priority ? ", moved to head" : ""));
 			spin_unlock(&imp->imp_lock);
-			GOTO(out_free, rc = 0);
+			rc = 0;
+			goto out_free;
 		}
 	}
 	/* No existing import connection found for \a uuid. */
@@ -109,7 +112,8 @@ static int import_set_conn(struct obd_import *imp, struct obd_uuid *uuid,
 		       (priority ? "head" : "tail"));
 	} else {
 		spin_unlock(&imp->imp_lock);
-		GOTO(out_free, rc = -ENOENT);
+		rc = -ENOENT;
+		goto out_free;
 	}
 
 	spin_unlock(&imp->imp_lock);
@@ -143,7 +147,7 @@ int client_import_del_conn(struct obd_import *imp, struct obd_uuid *uuid)
 	spin_lock(&imp->imp_lock);
 	if (list_empty(&imp->imp_conn_list)) {
 		LASSERT(!imp->imp_connection);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	list_for_each_entry(imp_conn, &imp->imp_conn_list, oic_item) {
@@ -157,7 +161,8 @@ int client_import_del_conn(struct obd_import *imp, struct obd_uuid *uuid)
 			if (imp->imp_state != LUSTRE_IMP_CLOSED &&
 			    imp->imp_state != LUSTRE_IMP_DISCON) {
 				CERROR("can't remove current connection\n");
-				GOTO(out, rc = -EBUSY);
+				rc = -EBUSY;
+				goto out;
 			}
 
 			ptlrpc_connection_put(imp->imp_connection);
@@ -262,15 +267,10 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 	char *name = obddev->obd_type->typ_name;
 	ldlm_ns_type_t ns_type = LDLM_NS_TYPE_UNKNOWN;
 	int rc;
-	char	*cli_name = lustre_cfg_buf(lcfg, 0);
 
 	/* In a more perfect world, we would hang a ptlrpc_client off of
 	 * obd_type and just use the values from there. */
-	if (!strcmp(name, LUSTRE_OSC_NAME) ||
-	    (!(strcmp(name, LUSTRE_OSP_NAME)) &&
-	     (is_osp_on_mdt(cli_name) &&
-	       strstr(lustre_cfg_buf(lcfg, 1), "OST") != NULL))) {
-		/* OSC or OSP_on_MDT for OSTs */
+	if (!strcmp(name, LUSTRE_OSC_NAME)) {
 		rq_portal = OST_REQUEST_PORTAL;
 		rp_portal = OSC_REPLY_PORTAL;
 		connect_op = OST_CONNECT;
@@ -278,17 +278,29 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 		cli->cl_sp_to = LUSTRE_SP_OST;
 		ns_type = LDLM_NS_TYPE_OSC;
 	} else if (!strcmp(name, LUSTRE_MDC_NAME) ||
-		   !strcmp(name, LUSTRE_LWP_NAME) ||
-		   (!strcmp(name, LUSTRE_OSP_NAME) &&
-		    (is_osp_on_mdt(cli_name) &&
-		     strstr(lustre_cfg_buf(lcfg, 1), "OST") == NULL))) {
-		/* MDC or OSP_on_MDT for other MDTs */
+		   !strcmp(name, LUSTRE_LWP_NAME)) {
 		rq_portal = MDS_REQUEST_PORTAL;
 		rp_portal = MDC_REPLY_PORTAL;
 		connect_op = MDS_CONNECT;
 		cli->cl_sp_me = LUSTRE_SP_CLI;
 		cli->cl_sp_to = LUSTRE_SP_MDT;
 		ns_type = LDLM_NS_TYPE_MDC;
+	} else if (!strcmp(name, LUSTRE_OSP_NAME)) {
+		if (strstr(lustre_cfg_buf(lcfg, 1), "OST") == NULL) {
+			/* OSP_on_MDT for other MDTs */
+			connect_op = MDS_CONNECT;
+			cli->cl_sp_to = LUSTRE_SP_MDT;
+			ns_type = LDLM_NS_TYPE_MDC;
+			rq_portal = OUT_PORTAL;
+		} else {
+			/* OSP on MDT for OST */
+			connect_op = OST_CONNECT;
+			cli->cl_sp_to = LUSTRE_SP_OST;
+			ns_type = LDLM_NS_TYPE_OSC;
+			rq_portal = OST_REQUEST_PORTAL;
+		}
+		rp_portal = OSC_REPLY_PORTAL;
+		cli->cl_sp_me = LUSTRE_SP_CLI;
 	} else if (!strcmp(name, LUSTRE_MGC_NAME)) {
 		rq_portal = MGS_REQUEST_PORTAL;
 		rp_portal = MGC_REPLY_PORTAL;
@@ -398,15 +410,17 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 	rc = ldlm_get_ref();
 	if (rc) {
 		CERROR("ldlm_get_ref failed: %d\n", rc);
-		GOTO(err, rc);
+		goto err;
 	}
 
 	ptlrpc_init_client(rq_portal, rp_portal, name,
 			   &obddev->obd_ldlm_client);
 
 	imp = class_new_import(obddev);
-	if (imp == NULL)
-		GOTO(err_ldlm, rc = -ENOENT);
+	if (imp == NULL) {
+		rc = -ENOENT;
+		goto err_ldlm;
+	}
 	imp->imp_client = &obddev->obd_ldlm_client;
 	imp->imp_connect_op = connect_op;
 	memcpy(cli->cl_target_uuid.uuid, lustre_cfg_buf(lcfg, 1),
@@ -416,7 +430,7 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 	rc = client_import_add_conn(imp, &server_uuid, 1);
 	if (rc) {
 		CERROR("can't add initial connection\n");
-		GOTO(err_import, rc);
+		goto err_import;
 	}
 
 	cli->cl_import = imp;
@@ -442,7 +456,8 @@ int client_obd_setup(struct obd_device *obddev, struct lustre_cfg *lcfg)
 	if (obddev->obd_namespace == NULL) {
 		CERROR("Unable to create client namespace - %s\n",
 		       obddev->obd_name);
-		GOTO(err_import, rc = -ENOMEM);
+		rc = -ENOMEM;
+		goto err_import;
 	}
 
 	cli->cl_qchk_stat = CL_NOT_QUOTACHECKED;
@@ -485,12 +500,14 @@ int client_connect_import(const struct lu_env *env,
 
 	*exp = NULL;
 	down_write(&cli->cl_sem);
-	if (cli->cl_conn_count > 0 )
-		GOTO(out_sem, rc = -EALREADY);
+	if (cli->cl_conn_count > 0) {
+		rc = -EALREADY;
+		goto out_sem;
+	}
 
 	rc = class_connect(&conn, obd, cluuid);
 	if (rc)
-		GOTO(out_sem, rc);
+		goto out_sem;
 
 	cli->cl_conn_count++;
 	*exp = class_conn2export(&conn);
@@ -500,7 +517,7 @@ int client_connect_import(const struct lu_env *env,
 	imp->imp_dlm_handle = conn;
 	rc = ptlrpc_init_import(imp);
 	if (rc != 0)
-		GOTO(out_ldlm, rc);
+		goto out_ldlm;
 
 	ocd = &imp->imp_connect_data;
 	if (data) {
@@ -511,7 +528,7 @@ int client_connect_import(const struct lu_env *env,
 	rc = ptlrpc_connect_import(imp);
 	if (rc != 0) {
 		LASSERT(imp->imp_state == LUSTRE_IMP_DISCON);
-		GOTO(out_ldlm, rc);
+		goto out_ldlm;
 	}
 	LASSERT(*exp != NULL && (*exp)->exp_connection);
 
@@ -560,12 +577,15 @@ int client_disconnect_export(struct obd_export *exp)
 	if (!cli->cl_conn_count) {
 		CERROR("disconnecting disconnected device (%s)\n",
 		       obd->obd_name);
-		GOTO(out_disconnect, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out_disconnect;
 	}
 
 	cli->cl_conn_count--;
-	if (cli->cl_conn_count)
-		GOTO(out_disconnect, rc = 0);
+	if (cli->cl_conn_count) {
+		rc = 0;
+		goto out_disconnect;
+	}
 
 	/* Mark import deactivated now, so we don't try to reconnect if any
 	 * of the cleanup RPCs fails (e.g. LDLM cancel, etc).  We don't
@@ -640,18 +660,18 @@ int target_send_reply_msg(struct ptlrpc_request *req, int rc, int fail_id)
 {
 	if (OBD_FAIL_CHECK_ORSET(fail_id & ~OBD_FAIL_ONCE, OBD_FAIL_ONCE)) {
 		DEBUG_REQ(D_ERROR, req, "dropping reply");
-		return (-ECOMM);
+		return -ECOMM;
 	}
 
 	if (unlikely(rc)) {
 		DEBUG_REQ(D_NET, req, "processing error (%d)", rc);
 		req->rq_status = rc;
-		return (ptlrpc_send_error(req, 1));
+		return ptlrpc_send_error(req, 1);
 	} else {
 		DEBUG_REQ(D_NET, req, "sending reply");
 	}
 
-	return (ptlrpc_send_reply(req, PTLRPC_REPLY_MAYBE_DIFFICULT));
+	return ptlrpc_send_reply(req, PTLRPC_REPLY_MAYBE_DIFFICULT);
 }
 
 void target_send_reply(struct ptlrpc_request *req, int rc, int fail_id)
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
index d022666fb705..6140130b6056 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lock.c
@@ -42,7 +42,7 @@
 #define DEBUG_SUBSYSTEM S_LDLM
 
 #include "../../include/linux/libcfs/libcfs.h"
-#include "../include/linux/lustre_intent.h"
+#include "../include/lustre_intent.h"
 #include "../include/obd_class.h"
 #include "ldlm_internal.h"
 
@@ -613,50 +613,12 @@ EXPORT_SYMBOL(__ldlm_handle2lock);
  */
 void ldlm_lock2desc(struct ldlm_lock *lock, struct ldlm_lock_desc *desc)
 {
-	struct obd_export *exp = lock->l_export ?: lock->l_conn_export;
-
-	/* INODEBITS_INTEROP: If the other side does not support
-	 * inodebits, reply with a plain lock descriptor. */
-	if ((lock->l_resource->lr_type == LDLM_IBITS) &&
-	    (exp && !(exp_connect_flags(exp) & OBD_CONNECT_IBITS))) {
-		/* Make sure all the right bits are set in this lock we
-		   are going to pass to client */
-		LASSERTF(lock->l_policy_data.l_inodebits.bits ==
-			 (MDS_INODELOCK_LOOKUP | MDS_INODELOCK_UPDATE |
-			  MDS_INODELOCK_LAYOUT),
-			 "Inappropriate inode lock bits during conversion %llu\n",
-			 lock->l_policy_data.l_inodebits.bits);
-
-		ldlm_res2desc(lock->l_resource, &desc->l_resource);
-		desc->l_resource.lr_type = LDLM_PLAIN;
-
-		/* Convert "new" lock mode to something old client can
-		   understand */
-		if ((lock->l_req_mode == LCK_CR) ||
-		    (lock->l_req_mode == LCK_CW))
-			desc->l_req_mode = LCK_PR;
-		else
-			desc->l_req_mode = lock->l_req_mode;
-		if ((lock->l_granted_mode == LCK_CR) ||
-		    (lock->l_granted_mode == LCK_CW)) {
-			desc->l_granted_mode = LCK_PR;
-		} else {
-			/* We never grant PW/EX locks to clients */
-			LASSERT((lock->l_granted_mode != LCK_PW) &&
-				(lock->l_granted_mode != LCK_EX));
-			desc->l_granted_mode = lock->l_granted_mode;
-		}
-
-		/* We do not copy policy here, because there is no
-		   policy for plain locks */
-	} else {
-		ldlm_res2desc(lock->l_resource, &desc->l_resource);
-		desc->l_req_mode = lock->l_req_mode;
-		desc->l_granted_mode = lock->l_granted_mode;
-		ldlm_convert_policy_to_wire(lock->l_resource->lr_type,
-					    &lock->l_policy_data,
-					    &desc->l_policy_data);
-	}
+	ldlm_res2desc(lock->l_resource, &desc->l_resource);
+	desc->l_req_mode = lock->l_req_mode;
+	desc->l_granted_mode = lock->l_granted_mode;
+	ldlm_convert_policy_to_wire(lock->l_resource->lr_type,
+				    &lock->l_policy_data,
+				    &desc->l_policy_data);
 }
 EXPORT_SYMBOL(ldlm_lock2desc);
 
@@ -1008,7 +970,8 @@ static void search_granted_lock(struct list_head *queue,
 			prev->policy_link = &req->l_sl_policy;
 			return;
 		} else {
-			LDLM_ERROR(lock,"is not LDLM_PLAIN or LDLM_IBITS lock");
+			LDLM_ERROR(lock,
+				   "is not LDLM_PLAIN or LDLM_IBITS lock");
 			LBUG();
 		}
 	}
@@ -1291,18 +1254,26 @@ ldlm_mode_t ldlm_lock_match(struct ldlm_namespace *ns, __u64 flags,
 
 	lock = search_queue(&res->lr_granted, &mode, policy, old_lock,
 			    flags, unref);
-	if (lock != NULL)
-		GOTO(out, rc = 1);
-	if (flags & LDLM_FL_BLOCK_GRANTED)
-		GOTO(out, rc = 0);
+	if (lock != NULL) {
+		rc = 1;
+		goto out;
+	}
+	if (flags & LDLM_FL_BLOCK_GRANTED) {
+		rc = 0;
+		goto out;
+	}
 	lock = search_queue(&res->lr_converting, &mode, policy, old_lock,
 			    flags, unref);
-	if (lock != NULL)
-		GOTO(out, rc = 1);
+	if (lock != NULL) {
+		rc = 1;
+		goto out;
+	}
 	lock = search_queue(&res->lr_waiting, &mode, policy, old_lock,
 			    flags, unref);
-	if (lock != NULL)
-		GOTO(out, rc = 1);
+	if (lock != NULL) {
+		rc = 1;
+		goto out;
+	}
 
  out:
 	unlock_res(res);
@@ -1392,11 +1363,11 @@ ldlm_mode_t ldlm_revalidate_lock_handle(struct lustre_handle *lockh,
 	if (lock != NULL) {
 		lock_res_and_lock(lock);
 		if (lock->l_flags & LDLM_FL_GONE_MASK)
-			GOTO(out, mode);
+			goto out;
 
 		if (lock->l_flags & LDLM_FL_CBPENDING &&
 		    lock->l_readers == 0 && lock->l_writers == 0)
-			GOTO(out, mode);
+			goto out;
 
 		if (bits)
 			*bits = lock->l_policy_data.l_inodebits.bits;
@@ -1550,19 +1521,19 @@ struct ldlm_lock *ldlm_lock_create(struct ldlm_namespace *ns,
 	/* if this is the extent lock, allocate the interval tree node */
 	if (type == LDLM_EXTENT) {
 		if (ldlm_interval_alloc(lock) == NULL)
-			GOTO(out, 0);
+			goto out;
 	}
 
 	if (lvb_len) {
 		lock->l_lvb_len = lvb_len;
 		OBD_ALLOC(lock->l_lvb_data, lvb_len);
 		if (lock->l_lvb_data == NULL)
-			GOTO(out, 0);
+			goto out;
 	}
 
 	lock->l_lvb_type = lvb_type;
 	if (OBD_FAIL_CHECK(OBD_FAIL_LDLM_NEW_LOCK))
-		GOTO(out, 0);
+		goto out;
 
 	return lock;
 
@@ -1630,14 +1601,15 @@ ldlm_error_t ldlm_lock_enqueue(struct ldlm_namespace *ns,
 		 * need to do anything else. */
 		*flags &= ~(LDLM_FL_BLOCK_GRANTED |
 			    LDLM_FL_BLOCK_CONV | LDLM_FL_BLOCK_WAIT);
-		GOTO(out, ELDLM_OK);
+		goto out;
 	}
 
 	ldlm_resource_unlink_lock(lock);
 	if (res->lr_type == LDLM_EXTENT && lock->l_tree_node == NULL) {
 		if (node == NULL) {
 			ldlm_lock_destroy_nolock(lock);
-			GOTO(out, rc = -ENOMEM);
+			rc = -ENOMEM;
+			goto out;
 		}
 
 		INIT_LIST_HEAD(&node->li_group);
@@ -1667,7 +1639,7 @@ ldlm_error_t ldlm_lock_enqueue(struct ldlm_namespace *ns,
 			ldlm_resource_add_lock(res, &res->lr_waiting, lock);
 		else
 			ldlm_grant_lock(lock, NULL);
-		GOTO(out, ELDLM_OK);
+		goto out;
 	} else {
 		CERROR("This is client-side-only module, cannot handle "
 		       "LDLM_NAMESPACE_SERVER resource type lock.\n");
@@ -1784,7 +1756,7 @@ ldlm_work_revoke_ast_lock(struct ptlrpc_request_set *rqset, void *opaq)
 	desc.l_req_mode = LCK_EX;
 	desc.l_granted_mode = 0;
 
-	rc = lock->l_blocking_ast(lock, &desc, (void*)arg, LDLM_CB_BLOCKING);
+	rc = lock->l_blocking_ast(lock, &desc, (void *)arg, LDLM_CB_BLOCKING);
 	LDLM_LOCK_RELEASE(lock);
 
 	return rc;
@@ -1813,7 +1785,7 @@ int ldlm_work_gl_ast_lock(struct ptlrpc_request_set *rqset, void *opaq)
 	arg->gl_desc = gl_work->gl_desc;
 
 	/* invoke the actual glimpse callback */
-	if (lock->l_glimpse_ast(lock, (void*)arg) == 0)
+	if (lock->l_glimpse_ast(lock, (void *)arg) == 0)
 		rc = 1;
 
 	LDLM_LOCK_RELEASE(lock);
@@ -1874,14 +1846,16 @@ int ldlm_run_ast_work(struct ldlm_namespace *ns, struct list_head *rpc_list,
 	 * to keep the number of requests in flight to ns_max_parallel_ast */
 	arg->set = ptlrpc_prep_fcset(ns->ns_max_parallel_ast ? : UINT_MAX,
 				     work_ast_lock, arg);
-	if (arg->set == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (arg->set == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	ptlrpc_set_wait(arg->set);
 	ptlrpc_set_destroy(arg->set);
 
 	rc = atomic_read(&arg->restart) ? -ERESTART : 0;
-	GOTO(out, rc);
+	goto out;
 out:
 	OBD_FREE_PTR(arg);
 	return rc;
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
index 952e10eb6178..91cf7ebae114 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_lockd.c
@@ -202,7 +202,8 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 	lvb_len = req_capsule_get_size(&req->rq_pill, &RMF_DLM_LVB, RCL_CLIENT);
 	if (lvb_len < 0) {
 		LDLM_ERROR(lock, "Fail to get lvb_len, rc = %d", lvb_len);
-		GOTO(out, rc = lvb_len);
+		rc = lvb_len;
+		goto out;
 	} else if (lvb_len > 0) {
 		if (lock->l_lvb_len > 0) {
 			/* for extent lock, lvb contains ost_lvb{}. */
@@ -213,7 +214,8 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 					   "expectation, expected = %d, "
 					   "replied = %d",
 					   lock->l_lvb_len, lvb_len);
-				GOTO(out, rc = -EINVAL);
+				rc = -EINVAL;
+				goto out;
 			}
 		} else if (ldlm_has_layout(lock)) { /* for layout lock, lvb has
 						     * variable length */
@@ -222,7 +224,8 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 			OBD_ALLOC(lvb_data, lvb_len);
 			if (lvb_data == NULL) {
 				LDLM_ERROR(lock, "No memory: %d.\n", lvb_len);
-				GOTO(out, rc = -ENOMEM);
+				rc = -ENOMEM;
+				goto out;
 			}
 
 			lock_res_and_lock(lock);
@@ -240,7 +243,8 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 		/* bug 11300: the lock has already been granted */
 		unlock_res_and_lock(lock);
 		LDLM_DEBUG(lock, "Double grant race happened");
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 
 	/* If we receive the completion AST before the actual enqueue returned,
@@ -267,7 +271,7 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 				&dlm_req->lock_desc.l_resource.lr_name);
 		if (rc < 0) {
 			LDLM_ERROR(lock, "Failed to allocate resource");
-			GOTO(out, rc);
+			goto out;
 		}
 		LDLM_DEBUG(lock, "completion AST, new resource");
 		CERROR("change resource!\n");
@@ -287,7 +291,7 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 				   lock->l_lvb_data, lvb_len);
 		if (rc < 0) {
 			unlock_res_and_lock(lock);
-			GOTO(out, rc);
+			goto out;
 		}
 	}
 
@@ -304,7 +308,7 @@ static void ldlm_handle_cp_callback(struct ptlrpc_request *req,
 
 	LDLM_DEBUG_NOLOCK("client completion callback handler END (lock %p)",
 			  lock);
-	GOTO(out, rc);
+	goto out;
 
 out:
 	if (rc < 0) {
@@ -980,7 +984,7 @@ int ldlm_init_export(struct obd_export *exp)
 
 	rc = ldlm_init_flock_export(exp);
 	if (rc)
-		GOTO(err, rc);
+		goto err;
 
 	return 0;
 err:
@@ -1014,7 +1018,7 @@ static int ldlm_setup(void)
 
 	rc = ldlm_proc_setup();
 	if (rc != 0)
-		GOTO(out, rc);
+		goto out;
 
 	memset(&conf, 0, sizeof(conf));
 	conf = (typeof(conf)) {
@@ -1051,13 +1055,15 @@ static int ldlm_setup(void)
 		CERROR("failed to start service\n");
 		rc = PTR_ERR(ldlm_state->ldlm_cb_service);
 		ldlm_state->ldlm_cb_service = NULL;
-		GOTO(out, rc);
+		goto out;
 	}
 
 
 	OBD_ALLOC(blp, sizeof(*blp));
-	if (blp == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (blp == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 	ldlm_state->ldlm_bl_pool = blp;
 
 	spin_lock_init(&blp->blp_lock);
@@ -1079,14 +1085,14 @@ static int ldlm_setup(void)
 	for (i = 0; i < blp->blp_min_threads; i++) {
 		rc = ldlm_bl_thread_start(blp);
 		if (rc < 0)
-			GOTO(out, rc);
+			goto out;
 	}
 
 
 	rc = ldlm_pools_init();
 	if (rc) {
 		CERROR("Failed to initialize LDLM pools: %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 	return 0;
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
index db36b3f46e49..6054eee848d3 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_pool.c
@@ -631,7 +631,7 @@ EXPORT_SYMBOL(ldlm_pool_shrink);
 int ldlm_pool_setup(struct ldlm_pool *pl, int limit)
 {
 	if (pl->pl_ops->po_setup != NULL)
-		return(pl->pl_ops->po_setup(pl, limit));
+		return pl->pl_ops->po_setup(pl, limit);
 	return 0;
 }
 EXPORT_SYMBOL(ldlm_pool_setup);
@@ -735,7 +735,8 @@ static int ldlm_pool_proc_init(struct ldlm_pool *pl)
 	if (parent_ns_proc == NULL) {
 		CERROR("%s: proc entry is not initialized\n",
 		       ldlm_ns_name(ns));
-		GOTO(out_free_name, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out_free_name;
 	}
 	pl->pl_proc_dir = lprocfs_register("pool", parent_ns_proc,
 					   NULL, NULL);
@@ -743,7 +744,7 @@ static int ldlm_pool_proc_init(struct ldlm_pool *pl)
 		CERROR("LProcFS failed in ldlm-pool-init\n");
 		rc = PTR_ERR(pl->pl_proc_dir);
 		pl->pl_proc_dir = NULL;
-		GOTO(out_free_name, rc);
+		goto out_free_name;
 	}
 
 	var_name[MAX_STRING_SIZE] = '\0';
@@ -767,8 +768,10 @@ static int ldlm_pool_proc_init(struct ldlm_pool *pl)
 
 	pl->pl_stats = lprocfs_alloc_stats(LDLM_POOL_LAST_STAT -
 					   LDLM_POOL_FIRST_STAT, 0);
-	if (!pl->pl_stats)
-		GOTO(out_free_name, rc = -ENOMEM);
+	if (!pl->pl_stats) {
+		rc = -ENOMEM;
+		goto out_free_name;
+	}
 
 	lprocfs_counter_init(pl->pl_stats, LDLM_POOL_GRANTED_STAT,
 			     LPROCFS_CNTR_AVGMINMAX | LPROCFS_CNTR_STDDEV,
@@ -1168,8 +1171,7 @@ int ldlm_pools_recalc(ldlm_side_t client)
 		 */
 		mutex_lock(ldlm_namespace_lock(client));
 		list_for_each_entry(ns, ldlm_namespace_list(client),
-					ns_list_chain)
-		{
+					ns_list_chain) {
 			if (ns->ns_appetite != LDLM_NAMESPACE_MODEST)
 				continue;
 
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
index 8867dc175325..9ce437b18793 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_request.c
@@ -423,8 +423,10 @@ int ldlm_cli_enqueue_local(struct ldlm_namespace *ns,
 
 	lock = ldlm_lock_create(ns, res_id, type, mode, &cbs, data, lvb_len,
 				lvb_type);
-	if (unlikely(!lock))
-		GOTO(out_nolock, err = -ENOMEM);
+	if (unlikely(!lock)) {
+		err = -ENOMEM;
+		goto out_nolock;
+	}
 
 	ldlm_lock2handle(lock, lockh);
 
@@ -444,7 +446,7 @@ int ldlm_cli_enqueue_local(struct ldlm_namespace *ns,
 
 	err = ldlm_lock_enqueue(ns, &lock, policy, flags);
 	if (unlikely(err != ELDLM_OK))
-		GOTO(out, err);
+		goto out;
 
 	if (policy != NULL)
 		*policy = lock->l_policy_data;
@@ -510,7 +512,7 @@ static void failed_lock_cleanup(struct ldlm_namespace *ns,
 int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 			  ldlm_type_t type, __u8 with_policy, ldlm_mode_t mode,
 			  __u64 *flags, void *lvb, __u32 lvb_len,
-			  struct lustre_handle *lockh,int rc)
+			  struct lustre_handle *lockh, int rc)
 {
 	struct ldlm_namespace *ns = exp->exp_obd->obd_namespace;
 	int is_replay = *flags & LDLM_FL_REPLAY;
@@ -535,13 +537,15 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 			   rc == ELDLM_LOCK_ABORTED ? "ABORTED" : "FAILED");
 
 		if (rc != ELDLM_LOCK_ABORTED)
-			GOTO(cleanup, rc);
+			goto cleanup;
 	}
 
 	/* Before we return, swab the reply */
 	reply = req_capsule_server_get(&req->rq_pill, &RMF_DLM_REP);
-	if (reply == NULL)
-		GOTO(cleanup, rc = -EPROTO);
+	if (reply == NULL) {
+		rc = -EPROTO;
+		goto cleanup;
+	}
 
 	if (lvb_len != 0) {
 		LASSERT(lvb != NULL);
@@ -550,12 +554,14 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 					    RCL_SERVER);
 		if (size < 0) {
 			LDLM_ERROR(lock, "Fail to get lvb_len, rc = %d", size);
-			GOTO(cleanup, rc = size);
+			rc = size;
+			goto cleanup;
 		} else if (unlikely(size > lvb_len)) {
 			LDLM_ERROR(lock, "Replied LVB is larger than "
 				   "expectation, expected = %d, replied = %d",
 				   lvb_len, size);
-			GOTO(cleanup, rc = -EINVAL);
+			rc = -EINVAL;
+			goto cleanup;
 		}
 	}
 
@@ -563,7 +569,9 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 		if (lvb_len != 0)
 			rc = ldlm_fill_lvb(lock, &req->rq_pill, RCL_SERVER,
 					   lvb, size);
-		GOTO(cleanup, rc = (rc != 0 ? rc : ELDLM_LOCK_ABORTED));
+		if (rc == 0)
+			rc = ELDLM_LOCK_ABORTED;
+		goto cleanup;
 	}
 
 	/* lock enqueued on the server */
@@ -616,8 +624,10 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 
 			rc = ldlm_lock_change_resource(ns, lock,
 					&reply->lock_desc.l_resource.lr_name);
-			if (rc || lock->l_resource == NULL)
-				GOTO(cleanup, rc = -ENOMEM);
+			if (rc || lock->l_resource == NULL) {
+				rc = -ENOMEM;
+				goto cleanup;
+			}
 			LDLM_DEBUG(lock, "client-side enqueue, new resource");
 		}
 		if (with_policy)
@@ -629,7 +639,8 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 						&reply->lock_desc.l_policy_data,
 						&lock->l_policy_data);
 		if (type != LDLM_PLAIN)
-			LDLM_DEBUG(lock,"client-side enqueue, new policy data");
+			LDLM_DEBUG(lock,
+				   "client-side enqueue, new policy data");
 	}
 
 	if ((*flags) & LDLM_FL_AST_SENT ||
@@ -657,7 +668,7 @@ int ldlm_cli_enqueue_fini(struct obd_export *exp, struct ptlrpc_request *req,
 		unlock_res_and_lock(lock);
 		if (rc < 0) {
 			cleanup_phase = 1;
-			GOTO(cleanup, rc);
+			goto cleanup;
 		}
 	}
 
@@ -876,21 +887,8 @@ int ldlm_cli_enqueue(struct obd_export *exp, struct ptlrpc_request **reqp,
 		/* for the local lock, add the reference */
 		ldlm_lock_addref_internal(lock, einfo->ei_mode);
 		ldlm_lock2handle(lock, lockh);
-		if (policy != NULL) {
-			/* INODEBITS_INTEROP: If the server does not support
-			 * inodebits, we will request a plain lock in the
-			 * descriptor (ldlm_lock2desc() below) but use an
-			 * inodebits lock internally with both bits set.
-			 */
-			if (einfo->ei_type == LDLM_IBITS &&
-			    !(exp_connect_flags(exp) &
-			      OBD_CONNECT_IBITS))
-				lock->l_policy_data.l_inodebits.bits =
-					MDS_INODELOCK_LOOKUP |
-					MDS_INODELOCK_UPDATE;
-			else
+		if (policy != NULL)
 				lock->l_policy_data = *policy;
-		}
 
 		if (einfo->ei_type == LDLM_EXTENT)
 			lock->l_req_extent = policy->l_extent;
@@ -1049,14 +1047,18 @@ int ldlm_cli_convert(struct lustre_handle *lockh, int new_mode, __u32 *flags)
 	ptlrpc_request_set_replen(req);
 	rc = ptlrpc_queue_wait(req);
 	if (rc != ELDLM_OK)
-		GOTO(out, rc);
+		goto out;
 
 	reply = req_capsule_server_get(&req->rq_pill, &RMF_DLM_REP);
-	if (reply == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (reply == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
-	if (req->rq_status)
-		GOTO(out, rc = req->rq_status);
+	if (req->rq_status) {
+		rc = req->rq_status;
+		goto out;
+	}
 
 	res = ldlm_lock_convert(lock, new_mode, &reply->lock_flags);
 	if (res != NULL) {
@@ -1067,7 +1069,7 @@ int ldlm_cli_convert(struct lustre_handle *lockh, int new_mode, __u32 *flags)
 			rc = lock->l_completion_ast(lock, LDLM_FL_WAIT_NOREPROC,
 						    NULL);
 			if (rc)
-				GOTO(out, rc);
+				goto out;
 		}
 	} else {
 		rc = LUSTRE_EDEADLK;
@@ -1191,8 +1193,10 @@ int ldlm_cli_cancel_req(struct obd_export *exp, struct list_head *cancels,
 		}
 
 		req = ptlrpc_request_alloc(imp, &RQF_LDLM_CANCEL);
-		if (req == NULL)
-			GOTO(out, rc = -ENOMEM);
+		if (req == NULL) {
+			rc = -ENOMEM;
+			goto out;
+		}
 
 		req_capsule_filled_sizes(&req->rq_pill, RCL_CLIENT);
 		req_capsule_set_size(&req->rq_pill, &RMF_DLM_REQ, RCL_CLIENT,
@@ -1201,7 +1205,7 @@ int ldlm_cli_cancel_req(struct obd_export *exp, struct list_head *cancels,
 		rc = ptlrpc_request_pack(req, LUSTRE_DLM_VERSION, LDLM_CANCEL);
 		if (rc) {
 			ptlrpc_request_free(req);
-			GOTO(out, rc);
+			goto out;
 		}
 
 		req->rq_request_portal = LDLM_CANCEL_REQUEST_PORTAL;
@@ -1214,7 +1218,7 @@ int ldlm_cli_cancel_req(struct obd_export *exp, struct list_head *cancels,
 		if (flags & LCF_ASYNC) {
 			ptlrpcd_add_req(req, PDL_POLICY_LOCAL, -1);
 			sent = count;
-			GOTO(out, 0);
+			goto out;
 		} else {
 			rc = ptlrpc_queue_wait(req);
 		}
@@ -1261,8 +1265,7 @@ int ldlm_cli_update_pool(struct ptlrpc_request *req)
 	__u32 new_limit;
 
 	if (unlikely(!req->rq_import || !req->rq_import->imp_obd ||
-		     !imp_connect_lru_resize(req->rq_import)))
-	{
+		     !imp_connect_lru_resize(req->rq_import))) {
 		/*
 		 * Do nothing for corner cases.
 		 */
@@ -1974,22 +1977,28 @@ int ldlm_resource_foreach(struct ldlm_resource *res, ldlm_iterator_t iter,
 	list_for_each_safe(tmp, next, &res->lr_granted) {
 		lock = list_entry(tmp, struct ldlm_lock, l_res_link);
 
-		if (iter(lock, closure) == LDLM_ITER_STOP)
-			GOTO(out, rc = LDLM_ITER_STOP);
+		if (iter(lock, closure) == LDLM_ITER_STOP) {
+			rc = LDLM_ITER_STOP;
+			goto out;
+		}
 	}
 
 	list_for_each_safe(tmp, next, &res->lr_converting) {
 		lock = list_entry(tmp, struct ldlm_lock, l_res_link);
 
-		if (iter(lock, closure) == LDLM_ITER_STOP)
-			GOTO(out, rc = LDLM_ITER_STOP);
+		if (iter(lock, closure) == LDLM_ITER_STOP) {
+			rc = LDLM_ITER_STOP;
+			goto out;
+		}
 	}
 
 	list_for_each_safe(tmp, next, &res->lr_waiting) {
 		lock = list_entry(tmp, struct ldlm_lock, l_res_link);
 
-		if (iter(lock, closure) == LDLM_ITER_STOP)
-			GOTO(out, rc = LDLM_ITER_STOP);
+		if (iter(lock, closure) == LDLM_ITER_STOP) {
+			rc = LDLM_ITER_STOP;
+			goto out;
+		}
 	}
  out:
 	unlock_res(res);
@@ -2094,12 +2103,14 @@ static int replay_lock_interpret(const struct lu_env *env,
 
 	atomic_dec(&req->rq_import->imp_replay_inflight);
 	if (rc != ELDLM_OK)
-		GOTO(out, rc);
+		goto out;
 
 
 	reply = req_capsule_server_get(&req->rq_pill, &RMF_DLM_REP);
-	if (reply == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (reply == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	lock = ldlm_handle2lock(&aa->lock_handle);
 	if (!lock) {
@@ -2108,7 +2119,8 @@ static int replay_lock_interpret(const struct lu_env *env,
 		       aa->lock_handle.cookie, reply->lock_handle.cookie,
 		       req->rq_export->exp_client_uuid.uuid,
 		       libcfs_id2str(req->rq_peer));
-		GOTO(out, rc = -ESTALE);
+		rc = -ESTALE;
+		goto out;
 	}
 
 	/* Key change rehash lock in per-export hash with new key */
diff --git a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
index efd45e513416..a785b7a7d1b3 100644
--- a/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
+++ b/drivers/staging/lustre/lustre/ldlm/ldlm_resource.c
@@ -102,7 +102,7 @@ int ldlm_proc_setup(void)
 	if (IS_ERR(ldlm_type_proc_dir)) {
 		CERROR("LProcFS failed in ldlm-init\n");
 		rc = PTR_ERR(ldlm_type_proc_dir);
-		GOTO(err, rc);
+		goto err;
 	}
 
 	ldlm_ns_proc_dir = lprocfs_register("namespaces",
@@ -111,7 +111,7 @@ int ldlm_proc_setup(void)
 	if (IS_ERR(ldlm_ns_proc_dir)) {
 		CERROR("LProcFS failed in ldlm-init\n");
 		rc = PTR_ERR(ldlm_ns_proc_dir);
-		GOTO(err_type, rc);
+		goto err_type;
 	}
 
 	ldlm_svc_proc_dir = lprocfs_register("services",
@@ -120,7 +120,7 @@ int ldlm_proc_setup(void)
 	if (IS_ERR(ldlm_svc_proc_dir)) {
 		CERROR("LProcFS failed in ldlm-init\n");
 		rc = PTR_ERR(ldlm_svc_proc_dir);
-		GOTO(err_ns, rc);
+		goto err_ns;
 	}
 
 	rc = lprocfs_add_vars(ldlm_type_proc_dir, list, NULL);
@@ -511,8 +511,7 @@ typedef struct {
 	cfs_hash_ops_t *nsd_hops;
 } ldlm_ns_hash_def_t;
 
-ldlm_ns_hash_def_t ldlm_ns_hash_defs[] =
-{
+ldlm_ns_hash_def_t ldlm_ns_hash_defs[] = {
 	{
 		.nsd_type       = LDLM_NS_TYPE_MDC,
 		.nsd_bkt_bits   = 11,
@@ -581,7 +580,7 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 		nsd = &ldlm_ns_hash_defs[idx];
 		if (nsd->nsd_type == LDLM_NS_TYPE_UNKNOWN) {
 			CERROR("Unknown type %d for ns %s\n", ns_type, name);
-			GOTO(out_ref, NULL);
+			goto out_ref;
 		}
 
 		if (nsd->nsd_type == ns_type)
@@ -590,7 +589,7 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 
 	OBD_ALLOC_PTR(ns);
 	if (!ns)
-		GOTO(out_ref, NULL);
+		goto out_ref;
 
 	ns->ns_rs_hash = cfs_hash_create(name,
 					 nsd->nsd_all_bits, nsd->nsd_all_bits,
@@ -603,7 +602,7 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 					 CFS_HASH_SPIN_BKTLOCK |
 					 CFS_HASH_NO_ITEMREF);
 	if (ns->ns_rs_hash == NULL)
-		GOTO(out_ns, NULL);
+		goto out_ns;
 
 	cfs_hash_for_each_bucket(ns->ns_rs_hash, &bd, idx) {
 		nsb = cfs_hash_bd_extra_get(ns->ns_rs_hash, &bd);
@@ -637,14 +636,14 @@ struct ldlm_namespace *ldlm_namespace_new(struct obd_device *obd, char *name,
 	rc = ldlm_namespace_proc_register(ns);
 	if (rc != 0) {
 		CERROR("Can't initialize ns proc, rc %d\n", rc);
-		GOTO(out_hash, rc);
+		goto out_hash;
 	}
 
 	idx = ldlm_namespace_nr_read(client);
 	rc = ldlm_pool_init(&ns->ns_pool, ns, idx, client);
 	if (rc) {
 		CERROR("Can't initialize lock pool, rc %d\n", rc);
-		GOTO(out_proc, rc);
+		goto out_proc;
 	}
 
 	ldlm_namespace_register(ns, client);
@@ -823,7 +822,7 @@ force_wait:
 				       "namespace with %d resources in use, "
 				       "(rc=%d)\n", ldlm_ns_name(ns),
 				       atomic_read(&ns->ns_bref), rc);
-			GOTO(force_wait, rc);
+			goto force_wait;
 		}
 
 		if (atomic_read(&ns->ns_bref)) {
diff --git a/drivers/staging/lustre/lustre/libcfs/Makefile b/drivers/staging/lustre/lustre/libcfs/Makefile
index aeeaab73a1fd..fcecbd2271af 100644
--- a/drivers/staging/lustre/lustre/libcfs/Makefile
+++ b/drivers/staging/lustre/lustre/libcfs/Makefile
@@ -12,7 +12,7 @@ libcfs-linux-objs := $(addprefix linux/,$(libcfs-linux-objs))
 
 libcfs-all-objs := debug.o fail.o nidstrings.o module.o tracefile.o \
 		   libcfs_string.o hash.o kernel_user_comm.o \
-		   prng.o workitem.o upcall_cache.o libcfs_cpu.o \
+		   prng.o workitem.o libcfs_cpu.o \
 		   libcfs_mem.o libcfs_lock.o
 
 libcfs-objs := $(libcfs-linux-objs) $(libcfs-all-objs)
diff --git a/drivers/staging/lustre/lustre/libcfs/debug.c b/drivers/staging/lustre/lustre/libcfs/debug.c
index 6b584698d3ae..ba43b3067fa3 100644
--- a/drivers/staging/lustre/lustre/libcfs/debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/debug.c
@@ -314,9 +314,8 @@ libcfs_debug_str2mask(int *mask, const char *str, int is_subsys)
 		if (!isspace(str[n-1]))
 			break;
 	matched = n;
-
-	if ((t = sscanf(str, "%i%n", &m, &matched)) >= 1 &&
-	    matched == n) {
+	t = sscanf(str, "%i%n", &m, &matched);
+	if (t >= 1 && matched == n) {
 		/* don't print warning for lctl set_param debug=0 or -1 */
 		if (m != 0 && m != -1)
 			CWARN("You are trying to use a numerical value for the "
@@ -344,7 +343,7 @@ void libcfs_debug_dumplog_internal(void *arg)
 		snprintf(debug_file_name, sizeof(debug_file_name) - 1,
 			 "%s.%ld.%ld", libcfs_debug_file_path_arr,
 			 get_seconds(), (long_ptr_t)arg);
-		printk(KERN_ALERT "LustreError: dumping log to %s\n",
+		pr_alert("LustreError: dumping log to %s\n",
 		       debug_file_name);
 		cfs_tracefile_dump_all_pages(debug_file_name);
 		libcfs_run_debug_log_upcall(debug_file_name);
@@ -376,7 +375,7 @@ void libcfs_debug_dumplog(void)
 			     (void *)(long)current_pid(),
 			     "libcfs_debug_dumper");
 	if (IS_ERR(dumper))
-		printk(KERN_ERR "LustreError: cannot start log dump thread:"
+		pr_err("LustreError: cannot start log dump thread:"
 		       " %ld\n", PTR_ERR(dumper));
 	else
 		schedule();
@@ -402,9 +401,9 @@ int libcfs_debug_init(unsigned long bufsize)
 	}
 
 	if (libcfs_debug_file_path != NULL) {
-		memset(libcfs_debug_file_path_arr, 0, PATH_MAX);
 		strncpy(libcfs_debug_file_path_arr,
 			libcfs_debug_file_path, PATH_MAX-1);
+		libcfs_debug_file_path_arr[PATH_MAX - 1] = '\0';
 	}
 
 	/* If libcfs_debug_mb is set to an invalid value or uninitialized
@@ -442,9 +441,11 @@ int libcfs_debug_clear_buffer(void)
 #define DEBUG_SUBSYSTEM S_UNDEFINED
 int libcfs_debug_mark_buffer(const char *text)
 {
-	CDEBUG(D_TRACE,"***************************************************\n");
+	CDEBUG(D_TRACE,
+	       "***************************************************\n");
 	LCONSOLE(D_WARNING, "DEBUG MARKER: %s\n", text);
-	CDEBUG(D_TRACE,"***************************************************\n");
+	CDEBUG(D_TRACE,
+	       "***************************************************\n");
 
 	return 0;
 }
@@ -453,7 +454,7 @@ int libcfs_debug_mark_buffer(const char *text)
 
 void libcfs_debug_set_level(unsigned int debug_level)
 {
-	printk(KERN_WARNING "Lustre: Setting portals debug level to %08x\n",
+	pr_warn("Lustre: Setting portals debug level to %08x\n",
 	       debug_level);
 	libcfs_debug = debug_level;
 }
diff --git a/drivers/staging/lustre/lustre/libcfs/fail.c b/drivers/staging/lustre/lustre/libcfs/fail.c
index 1bf9c90b4789..e73ca3df9734 100644
--- a/drivers/staging/lustre/lustre/libcfs/fail.c
+++ b/drivers/staging/lustre/lustre/libcfs/fail.c
@@ -131,7 +131,6 @@ int __cfs_fail_timeout_set(__u32 id, __u32 value, int ms, int set)
 		       id, ms);
 		set_current_state(TASK_UNINTERRUPTIBLE);
 		schedule_timeout(cfs_time_seconds(ms) / 1000);
-		set_current_state(TASK_RUNNING);
 		CERROR("cfs_fail_timeout id %x awake\n", id);
 	}
 	return ret;
diff --git a/drivers/staging/lustre/lustre/libcfs/hash.c b/drivers/staging/lustre/lustre/libcfs/hash.c
index 8ef1deb59d4a..3b67b7b6428c 100644
--- a/drivers/staging/lustre/lustre/libcfs/hash.c
+++ b/drivers/staging/lustre/lustre/libcfs/hash.c
@@ -155,8 +155,7 @@ cfs_hash_rw_unlock(union cfs_hash_lock *lock, int exclusive)
 }
 
 /** No lock hash */
-static cfs_hash_lock_ops_t cfs_hash_nl_lops =
-{
+static cfs_hash_lock_ops_t cfs_hash_nl_lops = {
 	.hs_lock	= cfs_hash_nl_lock,
 	.hs_unlock      = cfs_hash_nl_unlock,
 	.hs_bkt_lock    = cfs_hash_nl_lock,
@@ -164,8 +163,7 @@ static cfs_hash_lock_ops_t cfs_hash_nl_lops =
 };
 
 /** no bucket lock, one spinlock to protect everything */
-static cfs_hash_lock_ops_t cfs_hash_nbl_lops =
-{
+static cfs_hash_lock_ops_t cfs_hash_nbl_lops = {
 	.hs_lock	= cfs_hash_spin_lock,
 	.hs_unlock      = cfs_hash_spin_unlock,
 	.hs_bkt_lock    = cfs_hash_nl_lock,
@@ -173,8 +171,7 @@ static cfs_hash_lock_ops_t cfs_hash_nbl_lops =
 };
 
 /** spin bucket lock, rehash is enabled */
-static cfs_hash_lock_ops_t cfs_hash_bkt_spin_lops =
-{
+static cfs_hash_lock_ops_t cfs_hash_bkt_spin_lops = {
 	.hs_lock	= cfs_hash_rw_lock,
 	.hs_unlock      = cfs_hash_rw_unlock,
 	.hs_bkt_lock    = cfs_hash_spin_lock,
@@ -182,8 +179,7 @@ static cfs_hash_lock_ops_t cfs_hash_bkt_spin_lops =
 };
 
 /** rw bucket lock, rehash is enabled */
-static cfs_hash_lock_ops_t cfs_hash_bkt_rw_lops =
-{
+static cfs_hash_lock_ops_t cfs_hash_bkt_rw_lops = {
 	.hs_lock	= cfs_hash_rw_lock,
 	.hs_unlock      = cfs_hash_rw_unlock,
 	.hs_bkt_lock    = cfs_hash_rw_lock,
@@ -191,8 +187,7 @@ static cfs_hash_lock_ops_t cfs_hash_bkt_rw_lops =
 };
 
 /** spin bucket lock, rehash is disabled */
-static cfs_hash_lock_ops_t cfs_hash_nr_bkt_spin_lops =
-{
+static cfs_hash_lock_ops_t cfs_hash_nr_bkt_spin_lops = {
 	.hs_lock	= cfs_hash_nl_lock,
 	.hs_unlock      = cfs_hash_nl_unlock,
 	.hs_bkt_lock    = cfs_hash_spin_lock,
@@ -200,8 +195,7 @@ static cfs_hash_lock_ops_t cfs_hash_nr_bkt_spin_lops =
 };
 
 /** rw bucket lock, rehash is disabled */
-static cfs_hash_lock_ops_t cfs_hash_nr_bkt_rw_lops =
-{
+static cfs_hash_lock_ops_t cfs_hash_nr_bkt_rw_lops = {
 	.hs_lock	= cfs_hash_nl_lock,
 	.hs_unlock      = cfs_hash_nl_unlock,
 	.hs_bkt_lock    = cfs_hash_rw_lock,
@@ -2098,7 +2092,7 @@ int cfs_hash_debug_str(struct cfs_hash *hs, struct seq_file *m)
 			maxdepb = ffz(~maxdep);
 		}
 		total += bd.bd_bucket->hsb_count;
-		dist[min(__cfs_fls(bd.bd_bucket->hsb_count/max(theta,1)),7)]++;
+		dist[min(fls(bd.bd_bucket->hsb_count / max(theta, 1)), 7)]++;
 		cfs_hash_bd_unlock(hs, &bd, 0);
 	}
 
diff --git a/drivers/staging/lustre/lustre/libcfs/heap.c b/drivers/staging/lustre/lustre/libcfs/heap.c
deleted file mode 100644
index bf6d0b91c35f..000000000000
--- a/drivers/staging/lustre/lustre/libcfs/heap.c
+++ /dev/null
@@ -1,475 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License version 2 for more details.  A copy is
- * included in the COPYING file that accompanied this code.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2011 Intel Corporation
- */
-/*
- * libcfs/libcfs/heap.c
- *
- * Author: Eric Barton	<eeb@whamcloud.com>
- *	   Liang Zhen	<liang@whamcloud.com>
- */
-/** \addtogroup heap
- *
- * @{
- */
-
-#define DEBUG_SUBSYSTEM S_LNET
-
-#include "../../include/linux/libcfs/libcfs.h"
-
-#define CBH_ALLOC(ptr, h)						\
-do {									\
-	if ((h)->cbh_flags & CBH_FLAG_ATOMIC_GROW)			\
-		LIBCFS_CPT_ALLOC_GFP((ptr), h->cbh_cptab, h->cbh_cptid,	\
-				     CBH_NOB, GFP_ATOMIC);	\
-	else								\
-		LIBCFS_CPT_ALLOC((ptr), h->cbh_cptab, h->cbh_cptid,	\
-				 CBH_NOB);				\
-} while (0)
-
-#define CBH_FREE(ptr)	LIBCFS_FREE(ptr, CBH_NOB)
-
-/**
- * Grows the capacity of a binary heap so that it can handle a larger number of
- * \e cfs_binheap_node_t objects.
- *
- * \param[in] h The binary heap
- *
- * \retval 0	   Successfully grew the heap
- * \retval -ENOMEM OOM error
- */
-static int
-cfs_binheap_grow(cfs_binheap_t *h)
-{
-	cfs_binheap_node_t ***frag1 = NULL;
-	cfs_binheap_node_t  **frag2;
-	int hwm = h->cbh_hwm;
-
-	/* need a whole new chunk of pointers */
-	LASSERT((h->cbh_hwm & CBH_MASK) == 0);
-
-	if (hwm == 0) {
-		/* first use of single indirect */
-		CBH_ALLOC(h->cbh_elements1, h);
-		if (h->cbh_elements1 == NULL)
-			return -ENOMEM;
-
-		goto out;
-	}
-
-	hwm -= CBH_SIZE;
-	if (hwm < CBH_SIZE * CBH_SIZE) {
-		/* not filled double indirect */
-		CBH_ALLOC(frag2, h);
-		if (frag2 == NULL)
-			return -ENOMEM;
-
-		if (hwm == 0) {
-			/* first use of double indirect */
-			CBH_ALLOC(h->cbh_elements2, h);
-			if (h->cbh_elements2 == NULL) {
-				CBH_FREE(frag2);
-				return -ENOMEM;
-			}
-		}
-
-		h->cbh_elements2[hwm >> CBH_SHIFT] = frag2;
-		goto out;
-	}
-
-	hwm -= CBH_SIZE * CBH_SIZE;
-#if (CBH_SHIFT * 3 < 32)
-	if (hwm >= CBH_SIZE * CBH_SIZE * CBH_SIZE) {
-		/* filled triple indirect */
-		return -ENOMEM;
-	}
-#endif
-	CBH_ALLOC(frag2, h);
-	if (frag2 == NULL)
-		return -ENOMEM;
-
-	if (((hwm >> CBH_SHIFT) & CBH_MASK) == 0) {
-		/* first use of this 2nd level index */
-		CBH_ALLOC(frag1, h);
-		if (frag1 == NULL) {
-			CBH_FREE(frag2);
-			return -ENOMEM;
-		}
-	}
-
-	if (hwm == 0) {
-		/* first use of triple indirect */
-		CBH_ALLOC(h->cbh_elements3, h);
-		if (h->cbh_elements3 == NULL) {
-			CBH_FREE(frag2);
-			CBH_FREE(frag1);
-			return -ENOMEM;
-		}
-	}
-
-	if (frag1 != NULL) {
-		LASSERT(h->cbh_elements3[hwm >> (2 * CBH_SHIFT)] == NULL);
-		h->cbh_elements3[hwm >> (2 * CBH_SHIFT)] = frag1;
-	} else {
-		frag1 = h->cbh_elements3[hwm >> (2 * CBH_SHIFT)];
-		LASSERT(frag1 != NULL);
-	}
-
-	frag1[(hwm >> CBH_SHIFT) & CBH_MASK] = frag2;
-
- out:
-	h->cbh_hwm += CBH_SIZE;
-	return 0;
-}
-
-/**
- * Creates and initializes a binary heap instance.
- *
- * \param[in] ops   The operations to be used
- * \param[in] flags The heap flags
- * \parm[in]  count The initial heap capacity in # of elements
- * \param[in] arg   An optional private argument
- * \param[in] cptab The CPT table this heap instance will operate over
- * \param[in] cptid The CPT id of \a cptab this heap instance will operate over
- *
- * \retval valid-pointer A newly-created and initialized binary heap object
- * \retval NULL		 error
- */
-cfs_binheap_t *
-cfs_binheap_create(cfs_binheap_ops_t *ops, unsigned int flags,
-		   unsigned count, void *arg, struct cfs_cpt_table *cptab,
-		   int cptid)
-{
-	cfs_binheap_t *h;
-
-	LASSERT(ops != NULL);
-	LASSERT(ops->hop_compare != NULL);
-	LASSERT(cptab != NULL);
-	LASSERT(cptid == CFS_CPT_ANY ||
-	       (cptid >= 0 && cptid < cptab->ctb_nparts));
-
-	LIBCFS_CPT_ALLOC(h, cptab, cptid, sizeof(*h));
-	if (h == NULL)
-		return NULL;
-
-	h->cbh_ops	  = ops;
-	h->cbh_nelements  = 0;
-	h->cbh_hwm	  = 0;
-	h->cbh_private	  = arg;
-	h->cbh_flags	  = flags & (~CBH_FLAG_ATOMIC_GROW);
-	h->cbh_cptab	  = cptab;
-	h->cbh_cptid	  = cptid;
-
-	while (h->cbh_hwm < count) { /* preallocate */
-		if (cfs_binheap_grow(h) != 0) {
-			cfs_binheap_destroy(h);
-			return NULL;
-		}
-	}
-
-	h->cbh_flags |= flags & CBH_FLAG_ATOMIC_GROW;
-
-	return h;
-}
-EXPORT_SYMBOL(cfs_binheap_create);
-
-/**
- * Releases all resources associated with a binary heap instance.
- *
- * Deallocates memory for all indirection levels and the binary heap object
- * itself.
- *
- * \param[in] h The binary heap object
- */
-void
-cfs_binheap_destroy(cfs_binheap_t *h)
-{
-	int idx0;
-	int idx1;
-	int n;
-
-	LASSERT(h != NULL);
-
-	n = h->cbh_hwm;
-
-	if (n > 0) {
-		CBH_FREE(h->cbh_elements1);
-		n -= CBH_SIZE;
-	}
-
-	if (n > 0) {
-		for (idx0 = 0; idx0 < CBH_SIZE && n > 0; idx0++) {
-			CBH_FREE(h->cbh_elements2[idx0]);
-			n -= CBH_SIZE;
-		}
-
-		CBH_FREE(h->cbh_elements2);
-	}
-
-	if (n > 0) {
-		for (idx0 = 0; idx0 < CBH_SIZE && n > 0; idx0++) {
-
-			for (idx1 = 0; idx1 < CBH_SIZE && n > 0; idx1++) {
-				CBH_FREE(h->cbh_elements3[idx0][idx1]);
-				n -= CBH_SIZE;
-			}
-
-			CBH_FREE(h->cbh_elements3[idx0]);
-		}
-
-		CBH_FREE(h->cbh_elements3);
-	}
-
-	LIBCFS_FREE(h, sizeof(*h));
-}
-EXPORT_SYMBOL(cfs_binheap_destroy);
-
-/**
- * Obtains a double pointer to a heap element, given its index into the binary
- * tree.
- *
- * \param[in] h	  The binary heap instance
- * \param[in] idx The requested node's index
- *
- * \retval valid-pointer A double pointer to a heap pointer entry
- */
-static cfs_binheap_node_t **
-cfs_binheap_pointer(cfs_binheap_t *h, unsigned int idx)
-{
-	if (idx < CBH_SIZE)
-		return &(h->cbh_elements1[idx]);
-
-	idx -= CBH_SIZE;
-	if (idx < CBH_SIZE * CBH_SIZE)
-		return &(h->cbh_elements2[idx >> CBH_SHIFT][idx & CBH_MASK]);
-
-	idx -= CBH_SIZE * CBH_SIZE;
-	return &(h->cbh_elements3[idx >> (2 * CBH_SHIFT)]\
-				 [(idx >> CBH_SHIFT) & CBH_MASK]\
-				 [idx & CBH_MASK]);
-}
-
-/**
- * Obtains a pointer to a heap element, given its index into the binary tree.
- *
- * \param[in] h	  The binary heap
- * \param[in] idx The requested node's index
- *
- * \retval valid-pointer The requested heap node
- * \retval NULL		 Supplied index is out of bounds
- */
-cfs_binheap_node_t *
-cfs_binheap_find(cfs_binheap_t *h, unsigned int idx)
-{
-	if (idx >= h->cbh_nelements)
-		return NULL;
-
-	return *cfs_binheap_pointer(h, idx);
-}
-EXPORT_SYMBOL(cfs_binheap_find);
-
-/**
- * Moves a node upwards, towards the root of the binary tree.
- *
- * \param[in] h The heap
- * \param[in] e The node
- *
- * \retval 1 The position of \a e in the tree was changed at least once
- * \retval 0 The position of \a e in the tree was not changed
- */
-static int
-cfs_binheap_bubble(cfs_binheap_t *h, cfs_binheap_node_t *e)
-{
-	unsigned int	     cur_idx = e->chn_index;
-	cfs_binheap_node_t **cur_ptr;
-	unsigned int	     parent_idx;
-	cfs_binheap_node_t **parent_ptr;
-	int		     did_sth = 0;
-
-	cur_ptr = cfs_binheap_pointer(h, cur_idx);
-	LASSERT(*cur_ptr == e);
-
-	while (cur_idx > 0) {
-		parent_idx = (cur_idx - 1) >> 1;
-
-		parent_ptr = cfs_binheap_pointer(h, parent_idx);
-		LASSERT((*parent_ptr)->chn_index == parent_idx);
-
-		if (h->cbh_ops->hop_compare(*parent_ptr, e))
-			break;
-
-		(*parent_ptr)->chn_index = cur_idx;
-		*cur_ptr = *parent_ptr;
-		cur_ptr = parent_ptr;
-		cur_idx = parent_idx;
-		did_sth = 1;
-	}
-
-	e->chn_index = cur_idx;
-	*cur_ptr = e;
-
-	return did_sth;
-}
-
-/**
- * Moves a node downwards, towards the last level of the binary tree.
- *
- * \param[in] h The heap
- * \param[in] e The node
- *
- * \retval 1 The position of \a e in the tree was changed at least once
- * \retval 0 The position of \a e in the tree was not changed
- */
-static int
-cfs_binheap_sink(cfs_binheap_t *h, cfs_binheap_node_t *e)
-{
-	unsigned int	     n = h->cbh_nelements;
-	unsigned int	     child_idx;
-	cfs_binheap_node_t **child_ptr;
-	cfs_binheap_node_t  *child;
-	unsigned int	     child2_idx;
-	cfs_binheap_node_t **child2_ptr;
-	cfs_binheap_node_t  *child2;
-	unsigned int	     cur_idx;
-	cfs_binheap_node_t **cur_ptr;
-	int		     did_sth = 0;
-
-	cur_idx = e->chn_index;
-	cur_ptr = cfs_binheap_pointer(h, cur_idx);
-	LASSERT(*cur_ptr == e);
-
-	while (cur_idx < n) {
-		child_idx = (cur_idx << 1) + 1;
-		if (child_idx >= n)
-			break;
-
-		child_ptr = cfs_binheap_pointer(h, child_idx);
-		child = *child_ptr;
-
-		child2_idx = child_idx + 1;
-		if (child2_idx < n) {
-			child2_ptr = cfs_binheap_pointer(h, child2_idx);
-			child2 = *child2_ptr;
-
-			if (h->cbh_ops->hop_compare(child2, child)) {
-				child_idx = child2_idx;
-				child_ptr = child2_ptr;
-				child = child2;
-			}
-		}
-
-		LASSERT(child->chn_index == child_idx);
-
-		if (h->cbh_ops->hop_compare(e, child))
-			break;
-
-		child->chn_index = cur_idx;
-		*cur_ptr = child;
-		cur_ptr = child_ptr;
-		cur_idx = child_idx;
-		did_sth = 1;
-	}
-
-	e->chn_index = cur_idx;
-	*cur_ptr = e;
-
-	return did_sth;
-}
-
-/**
- * Sort-inserts a node into the binary heap.
- *
- * \param[in] h The heap
- * \param[in] e The node
- *
- * \retval 0	Element inserted successfully
- * \retval != 0 error
- */
-int
-cfs_binheap_insert(cfs_binheap_t *h, cfs_binheap_node_t *e)
-{
-	cfs_binheap_node_t **new_ptr;
-	unsigned int	     new_idx = h->cbh_nelements;
-	int		     rc;
-
-	if (new_idx == h->cbh_hwm) {
-		rc = cfs_binheap_grow(h);
-		if (rc != 0)
-			return rc;
-	}
-
-	if (h->cbh_ops->hop_enter) {
-		rc = h->cbh_ops->hop_enter(h, e);
-		if (rc != 0)
-			return rc;
-	}
-
-	e->chn_index = new_idx;
-	new_ptr = cfs_binheap_pointer(h, new_idx);
-	h->cbh_nelements++;
-	*new_ptr = e;
-
-	cfs_binheap_bubble(h, e);
-
-	return 0;
-}
-EXPORT_SYMBOL(cfs_binheap_insert);
-
-/**
- * Removes a node from the binary heap.
- *
- * \param[in] h The heap
- * \param[in] e The node
- */
-void
-cfs_binheap_remove(cfs_binheap_t *h, cfs_binheap_node_t *e)
-{
-	unsigned int	     n = h->cbh_nelements;
-	unsigned int	     cur_idx = e->chn_index;
-	cfs_binheap_node_t **cur_ptr;
-	cfs_binheap_node_t  *last;
-
-	LASSERT(cur_idx != CBH_POISON);
-	LASSERT(cur_idx < n);
-
-	cur_ptr = cfs_binheap_pointer(h, cur_idx);
-	LASSERT(*cur_ptr == e);
-
-	n--;
-	last = *cfs_binheap_pointer(h, n);
-	h->cbh_nelements = n;
-	if (last == e)
-		return;
-
-	last->chn_index = cur_idx;
-	*cur_ptr = last;
-	if (!cfs_binheap_bubble(h, *cur_ptr))
-		cfs_binheap_sink(h, *cur_ptr);
-
-	e->chn_index = CBH_POISON;
-	if (h->cbh_ops->hop_exit)
-		h->cbh_ops->hop_exit(h, e);
-}
-EXPORT_SYMBOL(cfs_binheap_remove);
-
-/** @} heap */
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
index eaa423d13650..3298ddf6d3be 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-debug.c
@@ -56,7 +56,6 @@
 # define DEBUG_SUBSYSTEM S_LNET
 
 #include "../../../include/linux/libcfs/libcfs.h"
-#include "../../../include/linux/libcfs/linux/portals_compat25.h"
 
 #include "../tracefile.h"
 
@@ -82,11 +81,11 @@ void libcfs_run_debug_log_upcall(char *file)
 	argv[0] = lnet_debug_log_upcall;
 
 	LASSERTF(file != NULL, "called on a null filename\n");
-	argv[1] = file; //only need to pass the path of the file
+	argv[1] = file; /* only need to pass the path of the file */
 
 	argv[2] = NULL;
 
-	rc = USERMODEHELPER(argv[0], argv, envp);
+	rc = call_usermodehelper(argv[0], argv, envp, 1);
 	if (rc < 0 && rc != -ENOENT) {
 		CERROR("Error %d invoking LNET debug log upcall %s %s; "
 		       "check /proc/sys/lnet/debug_log_upcall\n",
@@ -113,7 +112,7 @@ void libcfs_run_upcall(char **argv)
 
 	LASSERT(argc >= 2);
 
-	rc = USERMODEHELPER(argv[0], argv, envp);
+	rc = call_usermodehelper(argv[0], argv, envp, 1);
 	if (rc < 0 && rc != -ENOENT) {
 		CERROR("Error %d invoking LNET upcall %s %s%s%s%s%s%s%s%s; "
 		       "check /proc/sys/lnet/upcall\n",
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
index de3c199654a0..a5effcd9c679 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-module.c
@@ -105,12 +105,12 @@ libcfs_psdev_open(struct inode *inode, struct file *file)
 	int    rc = 0;
 
 	if (!inode)
-		return (-EINVAL);
+		return -EINVAL;
 	pdu = (struct libcfs_device_userstate **)&file->private_data;
 	if (libcfs_psdev_ops.p_open != NULL)
 		rc = libcfs_psdev_ops.p_open(0, (void *)pdu);
 	else
-		return (-EPERM);
+		return -EPERM;
 	return rc;
 }
 
@@ -122,7 +122,7 @@ libcfs_psdev_release(struct inode *inode, struct file *file)
 	int    rc = 0;
 
 	if (!inode)
-		return (-EINVAL);
+		return -EINVAL;
 	pdu = file->private_data;
 	if (libcfs_psdev_ops.p_close != NULL)
 		rc = libcfs_psdev_ops.p_close(0, (void *)pdu);
@@ -145,14 +145,14 @@ static long libcfs_ioctl(struct file *file,
 	     _IOC_NR(cmd) > IOC_LIBCFS_MAX_NR) {
 		CDEBUG(D_IOCTL, "invalid ioctl ( type %d, nr %d, size %d )\n",
 		       _IOC_TYPE(cmd), _IOC_NR(cmd), _IOC_SIZE(cmd));
-		return (-EINVAL);
+		return -EINVAL;
 	}
 
 	/* Handle platform-dependent IOC requests */
 	switch (cmd) {
 	case IOC_LIBCFS_PANIC:
 		if (!capable(CFS_CAP_SYS_BOOT))
-			return (-EPERM);
+			return -EPERM;
 		panic("debugctl-invoked panic");
 		return 0;
 	case IOC_LIBCFS_MEMHOG:
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c
index 871ba44b29f3..19f405e64e68 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-prim.c
@@ -130,11 +130,11 @@ cfs_block_allsigs(void)
 	unsigned long	  flags;
 	sigset_t	old;
 
-	SIGNAL_MASK_LOCK(current, flags);
+	spin_lock_irqsave(&current->sighand->siglock, flags);
 	old = current->blocked;
 	sigfillset(&current->blocked);
 	recalc_sigpending();
-	SIGNAL_MASK_UNLOCK(current, flags);
+	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 
 	return old;
 }
@@ -144,11 +144,11 @@ sigset_t cfs_block_sigs(unsigned long sigs)
 	unsigned long  flags;
 	sigset_t	old;
 
-	SIGNAL_MASK_LOCK(current, flags);
+	spin_lock_irqsave(&current->sighand->siglock, flags);
 	old = current->blocked;
 	sigaddsetmask(&current->blocked, sigs);
 	recalc_sigpending();
-	SIGNAL_MASK_UNLOCK(current, flags);
+	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 	return old;
 }
 
@@ -158,11 +158,11 @@ sigset_t cfs_block_sigsinv(unsigned long sigs)
 	unsigned long flags;
 	sigset_t old;
 
-	SIGNAL_MASK_LOCK(current, flags);
+	spin_lock_irqsave(&current->sighand->siglock, flags);
 	old = current->blocked;
 	sigaddsetmask(&current->blocked, ~sigs);
 	recalc_sigpending();
-	SIGNAL_MASK_UNLOCK(current, flags);
+	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 
 	return old;
 }
@@ -172,10 +172,10 @@ cfs_restore_sigs (sigset_t old)
 {
 	unsigned long  flags;
 
-	SIGNAL_MASK_LOCK(current, flags);
+	spin_lock_irqsave(&current->sighand->siglock, flags);
 	current->blocked = old;
 	recalc_sigpending();
-	SIGNAL_MASK_UNLOCK(current, flags);
+	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 }
 
 int
@@ -189,9 +189,9 @@ cfs_clear_sigpending(void)
 {
 	unsigned long flags;
 
-	SIGNAL_MASK_LOCK(current, flags);
+	spin_lock_irqsave(&current->sighand->siglock, flags);
 	clear_tsk_thread_flag(current, TIF_SIGPENDING);
-	SIGNAL_MASK_UNLOCK(current, flags);
+	spin_unlock_irqrestore(&current->sighand->siglock, flags);
 }
 
 int
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
index 13a9266acfa0..bbe2c68c18a6 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-proc.c
@@ -98,9 +98,10 @@ enum {
 	PSDEV_LNET_FAIL_VAL,      /* userdata for fail loc */
 };
 
-static int proc_call_handler(void *data, int write, loff_t *ppos, void *buffer,
-			     size_t *lenp, int (*handler)(void *data, int write,
-			     loff_t pos, void *buffer, int len))
+static int proc_call_handler(void *data, int write, loff_t *ppos,
+		void __user *buffer, size_t *lenp,
+		int (*handler)(void *data, int write,
+		loff_t pos, void __user *buffer, int len))
 {
 	int rc = handler(data, write, *ppos, buffer, *lenp);
 
@@ -117,7 +118,7 @@ static int proc_call_handler(void *data, int write, loff_t *ppos, void *buffer,
 }
 
 static int __proc_dobitmasks(void *data, int write,
-			     loff_t pos, void *buffer, int nob)
+			     loff_t pos, void __user *buffer, int nob)
 {
 	const int     tmpstrlen = 512;
 	char	 *tmpstr;
@@ -168,7 +169,7 @@ static int min_watchdog_ratelimit = 0;	  /* disable ratelimiting */
 static int max_watchdog_ratelimit = (24*60*60); /* limit to once per day */
 
 static int __proc_dump_kernel(void *data, int write,
-			      loff_t pos, void *buffer, int nob)
+			      loff_t pos, void __user *buffer, int nob)
 {
 	if (!write)
 		return 0;
@@ -184,7 +185,7 @@ static int proc_dump_kernel(struct ctl_table *table, int write,
 }
 
 static int __proc_daemon_file(void *data, int write,
-			      loff_t pos, void *buffer, int nob)
+			      loff_t pos, void __user *buffer, int nob)
 {
 	if (!write) {
 		int len = strlen(cfs_tracefile);
@@ -207,7 +208,7 @@ static int proc_daemon_file(struct ctl_table *table, int write,
 }
 
 static int __proc_debug_mb(void *data, int write,
-			   loff_t pos, void *buffer, int nob)
+			   loff_t pos, void __user *buffer, int nob)
 {
 	if (!write) {
 		char tmpstr[32];
@@ -344,7 +345,7 @@ int proc_fail_loc(struct ctl_table *table, int write, void __user *buffer,
 }
 
 static int __proc_cpt_table(void *data, int write,
-			    loff_t pos, void *buffer, int nob)
+			    loff_t pos, void __user *buffer, int nob)
 {
 	char *buf = NULL;
 	int   len = 4096;
diff --git a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
index a93b6210705b..939b33dd6520 100644
--- a/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
+++ b/drivers/staging/lustre/lustre/libcfs/linux/linux-tcpip.c
@@ -298,7 +298,7 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 
 		if (rc == 0) {
 			CERROR ("Unexpected zero rc\n");
-			return (-ECONNABORTED);
+			return -ECONNABORTED;
 		}
 
 		if (ticks <= 0)
@@ -308,7 +308,7 @@ libcfs_sock_write (struct socket *sock, void *buffer, int nob, int timeout)
 		nob -= rc;
 	}
 
-	return (0);
+	return 0;
 }
 EXPORT_SYMBOL(libcfs_sock_write);
 
@@ -384,7 +384,7 @@ libcfs_sock_create (struct socket **sockp, int *fatal,
 	*sockp = sock;
 	if (rc != 0) {
 		CERROR ("Can't create socket: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	option = 1;
@@ -436,7 +436,7 @@ libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
 		if (rc != 0) {
 			CERROR ("Can't set send buffer %d: %d\n",
 				option, rc);
-			return (rc);
+			return rc;
 		}
 	}
 
@@ -447,7 +447,7 @@ libcfs_sock_setbuf (struct socket *sock, int txbufsize, int rxbufsize)
 		if (rc != 0) {
 			CERROR ("Can't set receive buffer %d: %d\n",
 				option, rc);
-			return (rc);
+			return rc;
 		}
 	}
 
diff --git a/drivers/staging/lustre/lustre/libcfs/module.c b/drivers/staging/lustre/lustre/libcfs/module.c
index 3396858098b0..2c4fc74505bc 100644
--- a/drivers/staging/lustre/lustre/libcfs/module.c
+++ b/drivers/staging/lustre/lustre/libcfs/module.c
@@ -119,7 +119,7 @@ kportal_memhog_alloc(struct libcfs_device_userstate *ldu, int npages,
 	       count1 < PAGE_CACHE_SIZE/sizeof(struct page *)) {
 
 		if (cfs_signal_pending())
-			return (-EINTR);
+			return -EINTR;
 
 		*level1p = alloc_page(flags);
 		if (*level1p == NULL)
@@ -134,11 +134,11 @@ kportal_memhog_alloc(struct libcfs_device_userstate *ldu, int npages,
 		       count2 < PAGE_CACHE_SIZE/sizeof(struct page *)) {
 
 			if (cfs_signal_pending())
-				return (-EINTR);
+				return -EINTR;
 
 			*level2p = alloc_page(flags);
 			if (*level2p == NULL)
-				return (-ENOMEM);
+				return -ENOMEM;
 			ldu->ldu_memhog_pages++;
 
 			level2p++;
@@ -217,7 +217,7 @@ int libcfs_deregister_ioctl(struct libcfs_ioctl_handler *hand)
 }
 EXPORT_SYMBOL(libcfs_deregister_ioctl);
 
-static int libcfs_ioctl_int(struct cfs_psdev_file *pfile,unsigned long cmd,
+static int libcfs_ioctl_int(struct cfs_psdev_file *pfile, unsigned long cmd,
 			    void *arg, struct libcfs_ioctl_data *data)
 {
 	int err = -EINVAL;
@@ -301,7 +301,8 @@ static int libcfs_ioctl(struct cfs_psdev_file *pfile, unsigned long cmd, void *a
 	/* 'cmd' and permissions get checked in our arch-specific caller */
 	if (libcfs_ioctl_getdata(buf, buf + 800, (void *)arg)) {
 		CERROR("PORTALS ioctl: data error\n");
-		GOTO(out, err = -EINVAL);
+		err = -EINVAL;
+		goto out;
 	}
 	data = (struct libcfs_ioctl_data *)buf;
 
@@ -351,7 +352,7 @@ static int init_libcfs_module(void)
 	rc = libcfs_debug_init(5 * 1024 * 1024);
 	if (rc < 0) {
 		printk(KERN_ERR "LustreError: libcfs_debug_init: %d\n", rc);
-		return (rc);
+		return rc;
 	}
 
 	rc = cfs_cpu_init();
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.c b/drivers/staging/lustre/lustre/libcfs/tracefile.c
index 0569bf8c75f3..7e3f6a45da00 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.c
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.c
@@ -755,7 +755,7 @@ void cfs_trace_flush_pages(void)
 }
 
 int cfs_trace_copyin_string(char *knl_buffer, int knl_buffer_nob,
-			    const char *usr_buffer, int usr_buffer_nob)
+			    const char __user *usr_buffer, int usr_buffer_nob)
 {
 	int    nob;
 
@@ -763,7 +763,7 @@ int cfs_trace_copyin_string(char *knl_buffer, int knl_buffer_nob,
 		return -EOVERFLOW;
 
 	if (copy_from_user((void *)knl_buffer,
-			   (void *)usr_buffer, usr_buffer_nob))
+			   usr_buffer, usr_buffer_nob))
 		return -EFAULT;
 
 	nob = strnlen(knl_buffer, usr_buffer_nob);
@@ -782,7 +782,7 @@ int cfs_trace_copyin_string(char *knl_buffer, int knl_buffer_nob,
 }
 EXPORT_SYMBOL(cfs_trace_copyin_string);
 
-int cfs_trace_copyout_string(char *usr_buffer, int usr_buffer_nob,
+int cfs_trace_copyout_string(char __user *usr_buffer, int usr_buffer_nob,
 			     const char *knl_buffer, char *append)
 {
 	/* NB if 'append' != NULL, it's a single character to append to the
@@ -824,7 +824,7 @@ void cfs_trace_free_string_buffer(char *str, int nob)
 	kfree(str);
 }
 
-int cfs_trace_dump_debug_buffer_usrstr(void *usr_str, int usr_str_nob)
+int cfs_trace_dump_debug_buffer_usrstr(void __user *usr_str, int usr_str_nob)
 {
 	char	 *str;
 	int	   rc;
@@ -886,7 +886,7 @@ int cfs_trace_daemon_command(char *str)
 	return rc;
 }
 
-int cfs_trace_daemon_command_usrstr(void *usr_str, int usr_str_nob)
+int cfs_trace_daemon_command_usrstr(void __user *usr_str, int usr_str_nob)
 {
 	char *str;
 	int   rc;
@@ -939,7 +939,7 @@ int cfs_trace_set_debug_mb(int mb)
 	return 0;
 }
 
-int cfs_trace_set_debug_mb_usrstr(void *usr_str, int usr_str_nob)
+int cfs_trace_set_debug_mb_usrstr(void __user *usr_str, int usr_str_nob)
 {
 	char     str[32];
 	int      rc;
diff --git a/drivers/staging/lustre/lustre/libcfs/tracefile.h b/drivers/staging/lustre/lustre/libcfs/tracefile.h
index 8df4af36c91d..0601476e1dc3 100644
--- a/drivers/staging/lustre/lustre/libcfs/tracefile.h
+++ b/drivers/staging/lustre/lustre/libcfs/tracefile.h
@@ -68,16 +68,16 @@ void cfs_tracefile_exit(void);
 
 
 int cfs_trace_copyin_string(char *knl_buffer, int knl_buffer_nob,
-			    const char *usr_buffer, int usr_buffer_nob);
-int cfs_trace_copyout_string(char *usr_buffer, int usr_buffer_nob,
+			    const char __user *usr_buffer, int usr_buffer_nob);
+int cfs_trace_copyout_string(char __user *usr_buffer, int usr_buffer_nob,
 			     const char *knl_str, char *append);
 int cfs_trace_allocate_string_buffer(char **str, int nob);
 void cfs_trace_free_string_buffer(char *str, int nob);
-int cfs_trace_dump_debug_buffer_usrstr(void *usr_str, int usr_str_nob);
+int cfs_trace_dump_debug_buffer_usrstr(void __user *usr_str, int usr_str_nob);
 int cfs_trace_daemon_command(char *str);
-int cfs_trace_daemon_command_usrstr(void *usr_str, int usr_str_nob);
+int cfs_trace_daemon_command_usrstr(void __user *usr_str, int usr_str_nob);
 int cfs_trace_set_debug_mb(int mb);
-int cfs_trace_set_debug_mb_usrstr(void *usr_str, int usr_str_nob);
+int cfs_trace_set_debug_mb_usrstr(void __user *usr_str, int usr_str_nob);
 int cfs_trace_get_debug_mb(void);
 
 extern void libcfs_debug_dumplog_internal(void *arg);
diff --git a/drivers/staging/lustre/lustre/libcfs/upcall_cache.c b/drivers/staging/lustre/lustre/libcfs/upcall_cache.c
deleted file mode 100644
index 88af82034e92..000000000000
--- a/drivers/staging/lustre/lustre/libcfs/upcall_cache.c
+++ /dev/null
@@ -1,449 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * libcfs/libcfs/upcall_cache.c
- *
- * Supplementary groups cache.
- */
-#define DEBUG_SUBSYSTEM S_SEC
-
-#include "../../include/linux/libcfs/lucache.h"
-
-static struct upcall_cache_entry *alloc_entry(struct upcall_cache *cache,
-					      __u64 key, void *args)
-{
-	struct upcall_cache_entry *entry;
-
-	LIBCFS_ALLOC(entry, sizeof(*entry));
-	if (!entry)
-		return NULL;
-
-	UC_CACHE_SET_NEW(entry);
-	INIT_LIST_HEAD(&entry->ue_hash);
-	entry->ue_key = key;
-	atomic_set(&entry->ue_refcount, 0);
-	init_waitqueue_head(&entry->ue_waitq);
-	if (cache->uc_ops->init_entry)
-		cache->uc_ops->init_entry(entry, args);
-	return entry;
-}
-
-/* protected by cache lock */
-static void free_entry(struct upcall_cache *cache,
-		       struct upcall_cache_entry *entry)
-{
-	if (cache->uc_ops->free_entry)
-		cache->uc_ops->free_entry(cache, entry);
-
-	list_del(&entry->ue_hash);
-	CDEBUG(D_OTHER, "destroy cache entry %p for key %llu\n",
-	       entry, entry->ue_key);
-	LIBCFS_FREE(entry, sizeof(*entry));
-}
-
-static inline int upcall_compare(struct upcall_cache *cache,
-				 struct upcall_cache_entry *entry,
-				 __u64 key, void *args)
-{
-	if (entry->ue_key != key)
-		return -1;
-
-	if (cache->uc_ops->upcall_compare)
-		return cache->uc_ops->upcall_compare(cache, entry, key, args);
-
-	return 0;
-}
-
-static inline int downcall_compare(struct upcall_cache *cache,
-				   struct upcall_cache_entry *entry,
-				   __u64 key, void *args)
-{
-	if (entry->ue_key != key)
-		return -1;
-
-	if (cache->uc_ops->downcall_compare)
-		return cache->uc_ops->downcall_compare(cache, entry, key, args);
-
-	return 0;
-}
-
-static inline void get_entry(struct upcall_cache_entry *entry)
-{
-	atomic_inc(&entry->ue_refcount);
-}
-
-static inline void put_entry(struct upcall_cache *cache,
-			     struct upcall_cache_entry *entry)
-{
-	if (atomic_dec_and_test(&entry->ue_refcount) &&
-	    (UC_CACHE_IS_INVALID(entry) || UC_CACHE_IS_EXPIRED(entry))) {
-		free_entry(cache, entry);
-	}
-}
-
-static int check_unlink_entry(struct upcall_cache *cache,
-			      struct upcall_cache_entry *entry)
-{
-	if (UC_CACHE_IS_VALID(entry) &&
-	    time_before(cfs_time_current(), entry->ue_expire))
-		return 0;
-
-	if (UC_CACHE_IS_ACQUIRING(entry)) {
-		if (entry->ue_acquire_expire == 0 ||
-		    time_before(cfs_time_current(), entry->ue_acquire_expire))
-			return 0;
-
-		UC_CACHE_SET_EXPIRED(entry);
-		wake_up_all(&entry->ue_waitq);
-	} else if (!UC_CACHE_IS_INVALID(entry)) {
-		UC_CACHE_SET_EXPIRED(entry);
-	}
-
-	list_del_init(&entry->ue_hash);
-	if (!atomic_read(&entry->ue_refcount))
-		free_entry(cache, entry);
-	return 1;
-}
-
-static inline int refresh_entry(struct upcall_cache *cache,
-			 struct upcall_cache_entry *entry)
-{
-	LASSERT(cache->uc_ops->do_upcall);
-	return cache->uc_ops->do_upcall(cache, entry);
-}
-
-struct upcall_cache_entry *upcall_cache_get_entry(struct upcall_cache *cache,
-						  __u64 key, void *args)
-{
-	struct upcall_cache_entry *entry = NULL, *new = NULL, *next;
-	struct list_head *head;
-	wait_queue_t wait;
-	int rc, found;
-
-	LASSERT(cache);
-
-	head = &cache->uc_hashtable[UC_CACHE_HASH_INDEX(key)];
-find_again:
-	found = 0;
-	spin_lock(&cache->uc_lock);
-	list_for_each_entry_safe(entry, next, head, ue_hash) {
-		/* check invalid & expired items */
-		if (check_unlink_entry(cache, entry))
-			continue;
-		if (upcall_compare(cache, entry, key, args) == 0) {
-			found = 1;
-			break;
-		}
-	}
-
-	if (!found) {
-		if (!new) {
-			spin_unlock(&cache->uc_lock);
-			new = alloc_entry(cache, key, args);
-			if (!new) {
-				CERROR("fail to alloc entry\n");
-				return ERR_PTR(-ENOMEM);
-			}
-			goto find_again;
-		} else {
-			list_add(&new->ue_hash, head);
-			entry = new;
-		}
-	} else {
-		if (new) {
-			free_entry(cache, new);
-			new = NULL;
-		}
-		list_move(&entry->ue_hash, head);
-	}
-	get_entry(entry);
-
-	/* acquire for new one */
-	if (UC_CACHE_IS_NEW(entry)) {
-		UC_CACHE_SET_ACQUIRING(entry);
-		UC_CACHE_CLEAR_NEW(entry);
-		spin_unlock(&cache->uc_lock);
-		rc = refresh_entry(cache, entry);
-		spin_lock(&cache->uc_lock);
-		entry->ue_acquire_expire =
-			cfs_time_shift(cache->uc_acquire_expire);
-		if (rc < 0) {
-			UC_CACHE_CLEAR_ACQUIRING(entry);
-			UC_CACHE_SET_INVALID(entry);
-			wake_up_all(&entry->ue_waitq);
-			if (unlikely(rc == -EREMCHG)) {
-				put_entry(cache, entry);
-				GOTO(out, entry = ERR_PTR(rc));
-			}
-		}
-	}
-	/* someone (and only one) is doing upcall upon this item,
-	 * wait it to complete */
-	if (UC_CACHE_IS_ACQUIRING(entry)) {
-		long expiry = (entry == new) ?
-			      cfs_time_seconds(cache->uc_acquire_expire) :
-			      MAX_SCHEDULE_TIMEOUT;
-		long left;
-
-		init_waitqueue_entry(&wait, current);
-		add_wait_queue(&entry->ue_waitq, &wait);
-		set_current_state(TASK_INTERRUPTIBLE);
-		spin_unlock(&cache->uc_lock);
-
-		left = schedule_timeout(expiry);
-
-		spin_lock(&cache->uc_lock);
-		remove_wait_queue(&entry->ue_waitq, &wait);
-		if (UC_CACHE_IS_ACQUIRING(entry)) {
-			/* we're interrupted or upcall failed in the middle */
-			rc = left > 0 ? -EINTR : -ETIMEDOUT;
-			CERROR("acquire for key %llu: error %d\n",
-			       entry->ue_key, rc);
-			put_entry(cache, entry);
-			GOTO(out, entry = ERR_PTR(rc));
-		}
-	}
-
-	/* invalid means error, don't need to try again */
-	if (UC_CACHE_IS_INVALID(entry)) {
-		put_entry(cache, entry);
-		GOTO(out, entry = ERR_PTR(-EIDRM));
-	}
-
-	/* check expired
-	 * We can't refresh the existing one because some
-	 * memory might be shared by multiple processes.
-	 */
-	if (check_unlink_entry(cache, entry)) {
-		/* if expired, try again. but if this entry is
-		 * created by me but too quickly turn to expired
-		 * without any error, should at least give a
-		 * chance to use it once.
-		 */
-		if (entry != new) {
-			put_entry(cache, entry);
-			spin_unlock(&cache->uc_lock);
-			new = NULL;
-			goto find_again;
-		}
-	}
-
-	/* Now we know it's good */
-out:
-	spin_unlock(&cache->uc_lock);
-	return entry;
-}
-EXPORT_SYMBOL(upcall_cache_get_entry);
-
-void upcall_cache_put_entry(struct upcall_cache *cache,
-			    struct upcall_cache_entry *entry)
-{
-	if (!entry) {
-		return;
-	}
-
-	LASSERT(atomic_read(&entry->ue_refcount) > 0);
-	spin_lock(&cache->uc_lock);
-	put_entry(cache, entry);
-	spin_unlock(&cache->uc_lock);
-}
-EXPORT_SYMBOL(upcall_cache_put_entry);
-
-int upcall_cache_downcall(struct upcall_cache *cache, __u32 err, __u64 key,
-			  void *args)
-{
-	struct upcall_cache_entry *entry = NULL;
-	struct list_head *head;
-	int found = 0, rc = 0;
-
-	LASSERT(cache);
-
-	head = &cache->uc_hashtable[UC_CACHE_HASH_INDEX(key)];
-
-	spin_lock(&cache->uc_lock);
-	list_for_each_entry(entry, head, ue_hash) {
-		if (downcall_compare(cache, entry, key, args) == 0) {
-			found = 1;
-			get_entry(entry);
-			break;
-		}
-	}
-
-	if (!found) {
-		CDEBUG(D_OTHER, "%s: upcall for key %llu not expected\n",
-		       cache->uc_name, key);
-		/* haven't found, it's possible */
-		spin_unlock(&cache->uc_lock);
-		return -EINVAL;
-	}
-
-	if (err) {
-		CDEBUG(D_OTHER, "%s: upcall for key %llu returned %d\n",
-		       cache->uc_name, entry->ue_key, err);
-		GOTO(out, rc = -EINVAL);
-	}
-
-	if (!UC_CACHE_IS_ACQUIRING(entry)) {
-		CDEBUG(D_RPCTRACE,"%s: found uptodate entry %p (key %llu)\n",
-		       cache->uc_name, entry, entry->ue_key);
-		GOTO(out, rc = 0);
-	}
-
-	if (UC_CACHE_IS_INVALID(entry) || UC_CACHE_IS_EXPIRED(entry)) {
-		CERROR("%s: found a stale entry %p (key %llu) in ioctl\n",
-		       cache->uc_name, entry, entry->ue_key);
-		GOTO(out, rc = -EINVAL);
-	}
-
-	spin_unlock(&cache->uc_lock);
-	if (cache->uc_ops->parse_downcall)
-		rc = cache->uc_ops->parse_downcall(cache, entry, args);
-	spin_lock(&cache->uc_lock);
-	if (rc)
-		GOTO(out, rc);
-
-	entry->ue_expire = cfs_time_shift(cache->uc_entry_expire);
-	UC_CACHE_SET_VALID(entry);
-	CDEBUG(D_OTHER, "%s: created upcall cache entry %p for key %llu\n",
-	       cache->uc_name, entry, entry->ue_key);
-out:
-	if (rc) {
-		UC_CACHE_SET_INVALID(entry);
-		list_del_init(&entry->ue_hash);
-	}
-	UC_CACHE_CLEAR_ACQUIRING(entry);
-	spin_unlock(&cache->uc_lock);
-	wake_up_all(&entry->ue_waitq);
-	put_entry(cache, entry);
-
-	return rc;
-}
-EXPORT_SYMBOL(upcall_cache_downcall);
-
-static void cache_flush(struct upcall_cache *cache, int force)
-{
-	struct upcall_cache_entry *entry, *next;
-	int i;
-
-	spin_lock(&cache->uc_lock);
-	for (i = 0; i < UC_CACHE_HASH_SIZE; i++) {
-		list_for_each_entry_safe(entry, next,
-					 &cache->uc_hashtable[i], ue_hash) {
-			if (!force && atomic_read(&entry->ue_refcount)) {
-				UC_CACHE_SET_EXPIRED(entry);
-				continue;
-			}
-			LASSERT(!atomic_read(&entry->ue_refcount));
-			free_entry(cache, entry);
-		}
-	}
-	spin_unlock(&cache->uc_lock);
-}
-
-void upcall_cache_flush_idle(struct upcall_cache *cache)
-{
-	cache_flush(cache, 0);
-}
-EXPORT_SYMBOL(upcall_cache_flush_idle);
-
-void upcall_cache_flush_all(struct upcall_cache *cache)
-{
-	cache_flush(cache, 1);
-}
-EXPORT_SYMBOL(upcall_cache_flush_all);
-
-void upcall_cache_flush_one(struct upcall_cache *cache, __u64 key, void *args)
-{
-	struct list_head *head;
-	struct upcall_cache_entry *entry;
-	int found = 0;
-
-	head = &cache->uc_hashtable[UC_CACHE_HASH_INDEX(key)];
-
-	spin_lock(&cache->uc_lock);
-	list_for_each_entry(entry, head, ue_hash) {
-		if (upcall_compare(cache, entry, key, args) == 0) {
-			found = 1;
-			break;
-		}
-	}
-
-	if (found) {
-		CWARN("%s: flush entry %p: key %llu, ref %d, fl %x, cur %lu, ex %ld/%ld\n",
-		      cache->uc_name, entry, entry->ue_key,
-		      atomic_read(&entry->ue_refcount), entry->ue_flags,
-		      get_seconds(), entry->ue_acquire_expire,
-		      entry->ue_expire);
-		UC_CACHE_SET_EXPIRED(entry);
-		if (!atomic_read(&entry->ue_refcount))
-			free_entry(cache, entry);
-	}
-	spin_unlock(&cache->uc_lock);
-}
-EXPORT_SYMBOL(upcall_cache_flush_one);
-
-struct upcall_cache *upcall_cache_init(const char *name, const char *upcall,
-				       struct upcall_cache_ops *ops)
-{
-	struct upcall_cache *cache;
-	int i;
-
-	LIBCFS_ALLOC(cache, sizeof(*cache));
-	if (!cache)
-		return ERR_PTR(-ENOMEM);
-
-	spin_lock_init(&cache->uc_lock);
-	rwlock_init(&cache->uc_upcall_rwlock);
-	for (i = 0; i < UC_CACHE_HASH_SIZE; i++)
-		INIT_LIST_HEAD(&cache->uc_hashtable[i]);
-	strncpy(cache->uc_name, name, sizeof(cache->uc_name) - 1);
-	/* upcall pathname proc tunable */
-	strncpy(cache->uc_upcall, upcall, sizeof(cache->uc_upcall) - 1);
-	cache->uc_entry_expire = 20 * 60;
-	cache->uc_acquire_expire = 30;
-	cache->uc_ops = ops;
-
-	return cache;
-}
-EXPORT_SYMBOL(upcall_cache_init);
-
-void upcall_cache_cleanup(struct upcall_cache *cache)
-{
-	if (!cache)
-		return;
-	upcall_cache_flush_all(cache);
-	LIBCFS_FREE(cache, sizeof(*cache));
-}
-EXPORT_SYMBOL(upcall_cache_cleanup);
diff --git a/drivers/staging/lustre/lustre/libcfs/workitem.c b/drivers/staging/lustre/lustre/libcfs/workitem.c
index 65629579bd7d..c4afaeaf8310 100644
--- a/drivers/staging/lustre/lustre/libcfs/workitem.c
+++ b/drivers/staging/lustre/lustre/libcfs/workitem.c
@@ -288,8 +288,8 @@ cfs_wi_scheduler (void *arg)
 		}
 
 		cfs_wi_sched_unlock(sched);
-		cfs_wait_event_interruptible_exclusive(sched->ws_waitq,
-				!cfs_wi_sched_cansleep(sched), rc);
+		rc = wait_event_interruptible_exclusive(sched->ws_waitq,
+						!cfs_wi_sched_cansleep(sched));
 		cfs_wi_sched_lock(sched);
 	}
 
@@ -365,6 +365,7 @@ cfs_wi_sched_create(char *name, struct cfs_cpt_table *cptab,
 		return -ENOMEM;
 
 	strncpy(sched->ws_name, name, CFS_WS_NAME_LEN);
+	sched->ws_name[CFS_WS_NAME_LEN - 1] = '\0';
 	sched->ws_cptab = cptab;
 	sched->ws_cpt = cpt;
 
diff --git a/drivers/staging/lustre/lustre/llite/dcache.c b/drivers/staging/lustre/lustre/llite/dcache.c
index 49ae207ad425..439e4875b05c 100644
--- a/drivers/staging/lustre/lustre/llite/dcache.c
+++ b/drivers/staging/lustre/lustre/llite/dcache.c
@@ -187,8 +187,8 @@ int ll_d_init(struct dentry *de)
 	if (de->d_fsdata == NULL) {
 		struct ll_dentry_data *lld;
 
-		OBD_ALLOC_PTR(lld);
-		if (likely(lld != NULL)) {
+		lld = kzalloc(sizeof(*lld), GFP_NOFS);
+		if (likely(lld)) {
 			spin_lock(&de->d_lock);
 			if (likely(de->d_fsdata == NULL)) {
 				de->d_fsdata = lld;
diff --git a/drivers/staging/lustre/lustre/llite/dir.c b/drivers/staging/lustre/lustre/llite/dir.c
index efa2faf080d7..b0bb7095dde5 100644
--- a/drivers/staging/lustre/lustre/llite/dir.c
+++ b/drivers/staging/lustre/lustre/llite/dir.c
@@ -42,7 +42,7 @@
 #include <linux/pagemap.h>
 #include <linux/mm.h>
 #include <asm/uaccess.h>
-#include <linux/buffer_head.h>   // for wait_on_buffer
+#include <linux/buffer_head.h>   /* for wait_on_buffer */
 #include <linux/pagevec.h>
 #include <linux/prefetch.h>
 
@@ -163,8 +163,8 @@ static int ll_dir_filler(void *_hash, struct page *page0)
 
 	LASSERT(max_pages > 0 && max_pages <= MD_MAX_BRW_PAGES);
 
-	OBD_ALLOC(page_pool, sizeof(page) * max_pages);
-	if (page_pool != NULL) {
+	page_pool = kzalloc(sizeof(page) * max_pages, GFP_NOFS);
+	if (page_pool) {
 		page_pool[0] = page0;
 	} else {
 		page_pool = &page0;
@@ -398,7 +398,7 @@ struct page *ll_get_dir_page(struct inode *dir, __u64 hash,
 	if (IS_ERR(page)) {
 		CERROR("dir page locate: "DFID" at %llu: rc %ld\n",
 		       PFID(ll_inode2fid(dir)), lhash, PTR_ERR(page));
-		GOTO(out_unlock, page);
+		goto out_unlock;
 	} else if (page != NULL) {
 		/*
 		 * XXX nikita: not entirely correct handling of a corner case:
@@ -414,7 +414,7 @@ struct page *ll_get_dir_page(struct inode *dir, __u64 hash,
 		 * it as an "overflow" page. 1. invalidate all pages at
 		 * once. 2. use HASH|1 as an index for P1.
 		 */
-		GOTO(hash_collision, page);
+		goto hash_collision;
 	}
 
 	page = read_cache_page(mapping, hash_x_index(hash, hash64),
@@ -422,7 +422,7 @@ struct page *ll_get_dir_page(struct inode *dir, __u64 hash,
 	if (IS_ERR(page)) {
 		CERROR("read cache page: "DFID" at %llu: rc %ld\n",
 		       PFID(ll_inode2fid(dir)), hash, PTR_ERR(page));
-		GOTO(out_unlock, page);
+		goto out_unlock;
 	}
 
 	wait_on_page_locked(page);
@@ -604,11 +604,13 @@ static int ll_readdir(struct file *filp, struct dir_context *ctx)
 	       " 32bit_api %d\n", inode->i_ino, inode->i_generation,
 	       inode, (unsigned long)lfd->lfd_pos, i_size_read(inode), api32);
 
-	if (lfd->lfd_pos == MDS_DIR_END_OFF)
+	if (lfd->lfd_pos == MDS_DIR_END_OFF) {
 		/*
 		 * end-of-file.
 		 */
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
+	}
 
 	ctx->pos = lfd->lfd_pos;
 	rc = ll_dir_read(inode, ctx);
@@ -636,7 +638,7 @@ static int ll_send_mgc_param(struct obd_export *mgc, char *string)
 	struct mgs_send_param *msp;
 	int rc = 0;
 
-	OBD_ALLOC_PTR(msp);
+	msp = kzalloc(sizeof(*msp), GFP_NOFS);
 	if (!msp)
 		return -ENOMEM;
 
@@ -663,8 +665,10 @@ int ll_dir_setdirstripe(struct inode *dir, struct lmv_user_md *lump,
 	op_data = ll_prep_md_op_data(NULL, dir, NULL, filename,
 				     strlen(filename), mode, LUSTRE_OPC_MKDIR,
 				     lump);
-	if (IS_ERR(op_data))
-		GOTO(err_exit, err = PTR_ERR(op_data));
+	if (IS_ERR(op_data)) {
+		err = PTR_ERR(op_data);
+		goto err_exit;
+	}
 
 	op_data->op_cli_flags |= CLI_SET_MEA;
 	err = md_create(sbi->ll_md_exp, op_data, lump, sizeof(*lump), mode,
@@ -673,7 +677,7 @@ int ll_dir_setdirstripe(struct inode *dir, struct lmv_user_md *lump,
 			cfs_curproc_cap_pack(), 0, &request);
 	ll_finish_md_op_data(op_data);
 	if (err)
-		GOTO(err_exit, err);
+		goto err_exit;
 err_exit:
 	ptlrpc_req_finished(request);
 	return err;
@@ -747,9 +751,11 @@ int ll_dir_setstripe(struct inode *inode, struct lov_user_md *lump,
 		char *param = NULL;
 		char *buf;
 
-		OBD_ALLOC(param, MGS_PARAM_MAXLEN);
-		if (param == NULL)
-			GOTO(end, rc = -ENOMEM);
+		param = kzalloc(MGS_PARAM_MAXLEN, GFP_NOFS);
+		if (!param) {
+			rc = -ENOMEM;
+			goto end;
+		}
 
 		buf = param;
 		/* Get fsname and assume devname to be -MDT0000. */
@@ -762,14 +768,14 @@ int ll_dir_setstripe(struct inode *inode, struct lov_user_md *lump,
 			lump ? le32_to_cpu(lump->lmm_stripe_size) : 0);
 		rc = ll_send_mgc_param(mgc->u.cli.cl_mgc_mgsexp, param);
 		if (rc)
-			GOTO(end, rc);
+			goto end;
 
 		/* Set root stripecount */
 		sprintf(buf, ".stripecount=%hd",
 			lump ? le16_to_cpu(lump->lmm_stripe_count) : 0);
 		rc = ll_send_mgc_param(mgc->u.cli.cl_mgc_mgsexp, param);
 		if (rc)
-			GOTO(end, rc);
+			goto end;
 
 		/* Set root stripeoffset */
 		sprintf(buf, ".stripeoffset=%hd",
@@ -811,7 +817,7 @@ int ll_dir_getstripe(struct inode *inode, struct lov_mds_md **lmmp,
 		CDEBUG(D_INFO, "md_getattr failed on inode "
 		       "%lu/%u: rc %d\n", inode->i_ino,
 		       inode->i_generation, rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
@@ -821,7 +827,8 @@ int ll_dir_getstripe(struct inode *inode, struct lov_mds_md **lmmp,
 
 	if (!(body->valid & (OBD_MD_FLEASIZE | OBD_MD_FLDIREA)) ||
 	    lmmsize == 0) {
-		GOTO(out, rc = -ENODATA);
+		rc = -ENODATA;
+		goto out;
 	}
 
 	lmm = req_capsule_server_sized_get(&req->rq_pill,
@@ -917,7 +924,8 @@ static int ll_ioc_copy_start(struct super_block *sb, struct hsm_copy *copy)
 			hpk.hpk_flags |= HP_FLAG_RETRY;
 			/* hpk_errval is >= 0 */
 			hpk.hpk_errval = -PTR_ERR(inode);
-			GOTO(progress, rc = PTR_ERR(inode));
+			rc = PTR_ERR(inode);
+			goto progress;
 		}
 
 		/* Read current file data version */
@@ -931,7 +939,7 @@ static int ll_ioc_copy_start(struct super_block *sb, struct hsm_copy *copy)
 			hpk.hpk_flags |= HP_FLAG_RETRY;
 			/* hpk_errval must be >= 0 */
 			hpk.hpk_errval = -rc;
-			GOTO(progress, rc);
+			goto progress;
 		}
 
 		/* Store it the hsm_copy for later copytool use.
@@ -997,7 +1005,8 @@ static int ll_ioc_copy_end(struct super_block *sb, struct hsm_copy *copy)
 			hpk.hpk_flags |= HP_FLAG_RETRY;
 			/* hpk_errval must be >= 0 */
 			hpk.hpk_errval = -PTR_ERR(inode);
-			GOTO(progress, rc = PTR_ERR(inode));
+			rc = PTR_ERR(inode);
+			goto progress;
 		}
 
 		rc = ll_data_version(inode, &data_version,
@@ -1008,7 +1017,7 @@ static int ll_ioc_copy_end(struct super_block *sb, struct hsm_copy *copy)
 				      "Request could not be confirmed.\n");
 			if (hpk.hpk_errval == 0)
 				hpk.hpk_errval = -rc;
-			GOTO(progress, rc);
+			goto progress;
 		}
 
 		/* Store it the hsm_copy for later copytool use.
@@ -1052,8 +1061,8 @@ static int copy_and_ioctl(int cmd, struct obd_export *exp,
 	void *copy;
 	int rc;
 
-	OBD_ALLOC(copy, size);
-	if (copy == NULL)
+	copy = kzalloc(size, GFP_NOFS);
+	if (!copy)
 		return -ENOMEM;
 
 	if (copy_from_user(copy, data, size)) {
@@ -1143,8 +1152,8 @@ static int quotactl_ioctl(struct ll_sb_info *sbi, struct if_quotactl *qctl)
 	} else {
 		struct obd_quotactl *oqctl;
 
-		OBD_ALLOC_PTR(oqctl);
-		if (oqctl == NULL)
+		oqctl = kzalloc(sizeof(*oqctl), GFP_NOFS);
+		if (!oqctl)
 			return -ENOMEM;
 
 		QCTL_COPY(oqctl, qctl);
@@ -1164,9 +1173,11 @@ static int quotactl_ioctl(struct ll_sb_info *sbi, struct if_quotactl *qctl)
 		    !oqctl->qc_dqblk.dqb_curspace) {
 			struct obd_quotactl *oqctl_tmp;
 
-			OBD_ALLOC_PTR(oqctl_tmp);
-			if (oqctl_tmp == NULL)
-				GOTO(out, rc = -ENOMEM);
+			oqctl_tmp = kzalloc(sizeof(*oqctl_tmp), GFP_NOFS);
+			if (!oqctl_tmp) {
+				rc = -ENOMEM;
+				goto out;
+			}
 
 			oqctl_tmp->qc_cmd = Q_GETOQUOTA;
 			oqctl_tmp->qc_id = oqctl->qc_id;
@@ -1244,7 +1255,7 @@ static long ll_dir_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		return -ENOTTY;
 
 	ll_stats_ops_tally(ll_i2sbi(inode), LPROC_LL_IOCTL, 1);
-	switch(cmd) {
+	switch (cmd) {
 	case FSFILT_IOC_GETFLAGS:
 	case FSFILT_IOC_SETFLAGS:
 		return ll_iocontrol(inode, file, cmd, arg);
@@ -1286,20 +1297,23 @@ static long ll_dir_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 
 		if (namelen < 1) {
 			CDEBUG(D_INFO, "IOC_MDC_LOOKUP missing filename\n");
-			GOTO(out_free, rc = -EINVAL);
+			rc = -EINVAL;
+			goto out_free;
 		}
 
 		op_data = ll_prep_md_op_data(NULL, inode, NULL, filename, namelen,
 					     0, LUSTRE_OPC_ANY, NULL);
-		if (IS_ERR(op_data))
-			GOTO(out_free, rc = PTR_ERR(op_data));
+		if (IS_ERR(op_data)) {
+			rc = PTR_ERR(op_data);
+			goto out_free;
+		}
 
 		op_data->op_valid = OBD_MD_FLID;
 		rc = md_getattr_name(sbi->ll_md_exp, op_data, &request);
 		ll_finish_md_op_data(op_data);
 		if (rc < 0) {
 			CDEBUG(D_INFO, "md_getattr_name: %d\n", rc);
-			GOTO(out_free, rc);
+			goto out_free;
 		}
 		ptlrpc_req_finished(request);
 out_free:
@@ -1321,15 +1335,18 @@ out_free:
 
 		data = (void *)buf;
 		if (data->ioc_inlbuf1 == NULL || data->ioc_inlbuf2 == NULL ||
-		    data->ioc_inllen1 == 0 || data->ioc_inllen2 == 0)
-			GOTO(lmv_out_free, rc = -EINVAL);
+		    data->ioc_inllen1 == 0 || data->ioc_inllen2 == 0) {
+			rc = -EINVAL;
+			goto lmv_out_free;
+		}
 
 		filename = data->ioc_inlbuf1;
 		namelen = data->ioc_inllen1;
 
 		if (namelen < 1) {
 			CDEBUG(D_INFO, "IOC_MDC_LOOKUP missing filename\n");
-			GOTO(lmv_out_free, rc = -EINVAL);
+			rc = -EINVAL;
+			goto lmv_out_free;
 		}
 		lum = (struct lmv_user_md *)data->ioc_inlbuf2;
 		lumlen = data->ioc_inllen2;
@@ -1338,7 +1355,8 @@ out_free:
 		    lumlen != sizeof(*lum)) {
 			CERROR("%s: wrong lum magic %x or size %d: rc = %d\n",
 			       filename, lum->lum_magic, lumlen, -EFAULT);
-			GOTO(lmv_out_free, rc = -EINVAL);
+			rc = -EINVAL;
+			goto lmv_out_free;
 		}
 
 		/**
@@ -1394,23 +1412,29 @@ lmv_out_free:
 			return -EINVAL;
 
 		lum_size = lmv_user_md_size(1, LMV_MAGIC_V1);
-		OBD_ALLOC(tmp, lum_size);
-		if (tmp == NULL)
-			GOTO(free_lmv, rc = -ENOMEM);
+		tmp = kzalloc(lum_size, GFP_NOFS);
+		if (!tmp) {
+			rc = -ENOMEM;
+			goto free_lmv;
+		}
 
 		*tmp = lum;
 		tmp->lum_type = LMV_STRIPE_TYPE;
 		tmp->lum_stripe_count = 1;
 		mdtindex = ll_get_mdt_idx(inode);
-		if (mdtindex < 0)
-			GOTO(free_lmv, rc = -ENOMEM);
+		if (mdtindex < 0) {
+			rc = -ENOMEM;
+			goto free_lmv;
+		}
 
 		tmp->lum_stripe_offset = mdtindex;
 		tmp->lum_objects[0].lum_mds = mdtindex;
 		memcpy(&tmp->lum_objects[0].lum_fid, ll_inode2fid(inode),
 		       sizeof(struct lu_fid));
-		if (copy_to_user((void *)arg, tmp, lum_size))
-			GOTO(free_lmv, rc = -EFAULT);
+		if (copy_to_user((void *)arg, tmp, lum_size)) {
+			rc = -EFAULT;
+			goto free_lmv;
+		}
 free_lmv:
 		if (tmp)
 			OBD_FREE(tmp, lum_size);
@@ -1434,8 +1458,10 @@ free_lmv:
 			return PTR_ERR(filename);
 
 		namelen = strlen(filename);
-		if (namelen < 1)
-			GOTO(out_rmdir, rc = -EINVAL);
+		if (namelen < 1) {
+			rc = -EINVAL;
+			goto out_rmdir;
+		}
 
 		rc = ll_rmdir_entry(inode, filename, namelen);
 out_rmdir:
@@ -1475,15 +1501,17 @@ out_rmdir:
 						      &RMF_MDT_BODY);
 			LASSERT(body != NULL);
 		} else {
-			GOTO(out_req, rc);
+			goto out_req;
 		}
 
 		if (rc < 0) {
 			if (rc == -ENODATA && (cmd == IOC_MDC_GETFILEINFO ||
-					       cmd == LL_IOC_MDC_GETINFO))
-				GOTO(skip_lmm, rc = 0);
+					       cmd == LL_IOC_MDC_GETINFO)) {
+				rc = 0;
+				goto skip_lmm;
+			}
 			else
-				GOTO(out_req, rc);
+				goto out_req;
 		}
 
 		if (cmd == IOC_MDC_GETFILESTRIPE ||
@@ -1495,8 +1523,10 @@ out_rmdir:
 			lump = &lmdp->lmd_lmm;
 		}
 		if (copy_to_user(lump, lmm, lmmsize)) {
-			if (copy_to_user(lump, lmm, sizeof(*lump)))
-				GOTO(out_req, rc = -EFAULT);
+			if (copy_to_user(lump, lmm, sizeof(*lump))) {
+				rc = -EFAULT;
+				goto out_req;
+			}
 			rc = -EOVERFLOW;
 		}
 skip_lmm:
@@ -1519,8 +1549,10 @@ skip_lmm:
 			st.st_ino     = inode->i_ino;
 
 			lmdp = (struct lov_user_mds_data *)arg;
-			if (copy_to_user(&lmdp->lmd_st, &st, sizeof(st)))
-				GOTO(out_req, rc = -EFAULT);
+			if (copy_to_user(&lmdp->lmd_st, &st, sizeof(st))) {
+				rc = -EFAULT;
+				goto out_req;
+			}
 		}
 
 out_req:
@@ -1547,8 +1579,10 @@ out_req:
 		OBD_ALLOC_LARGE(lmm, lmmsize);
 		if (lmm == NULL)
 			return -ENOMEM;
-		if (copy_from_user(lmm, lum, lmmsize))
-			GOTO(free_lmm, rc = -EFAULT);
+		if (copy_from_user(lmm, lum, lmmsize)) {
+			rc = -EFAULT;
+			goto free_lmm;
+		}
 
 		switch (lmm->lmm_magic) {
 		case LOV_USER_MAGIC_V1:
@@ -1570,22 +1604,27 @@ out_req:
 			lustre_swab_lov_user_md_v3((struct lov_user_md_v3 *)lmm);
 			break;
 		default:
-			GOTO(free_lmm, rc = -EINVAL);
+			rc = -EINVAL;
+			goto free_lmm;
 		}
 
 		rc = obd_unpackmd(sbi->ll_dt_exp, &lsm, lmm, lmmsize);
-		if (rc < 0)
-			GOTO(free_lmm, rc = -ENOMEM);
+		if (rc < 0) {
+			rc = -ENOMEM;
+			goto free_lmm;
+		}
 
 		/* Perform glimpse_size operation. */
 		memset(&st, 0, sizeof(st));
 
 		rc = ll_glimpse_ioctl(sbi, lsm, &st);
 		if (rc)
-			GOTO(free_lsm, rc);
+			goto free_lsm;
 
-		if (copy_to_user(&lumd->lmd_st, &st, sizeof(st)))
-			GOTO(free_lsm, rc = -EFAULT);
+		if (copy_to_user(&lumd->lmd_st, &st, sizeof(st))) {
+			rc = -EFAULT;
+			goto free_lsm;
+		}
 
 free_lsm:
 		obd_free_memmd(sbi->ll_dt_exp, &lsm);
@@ -1604,7 +1643,7 @@ free_lmm:
 		    sbi->ll_flags & LL_SBI_RMT_CLIENT)
 			return -EPERM;
 
-		OBD_ALLOC_PTR(oqctl);
+		oqctl = kzalloc(sizeof(*oqctl), GFP_NOFS);
 		if (!oqctl)
 			return -ENOMEM;
 		oqctl->qc_type = arg;
@@ -1628,7 +1667,7 @@ free_lmm:
 		    sbi->ll_flags & LL_SBI_RMT_CLIENT)
 			return -EPERM;
 
-		OBD_ALLOC_PTR(check);
+		check = kzalloc(sizeof(*check), GFP_NOFS);
 		if (!check)
 			return -ENOMEM;
 
@@ -1639,7 +1678,7 @@ free_lmm:
 			if (copy_to_user((void *)arg, check,
 					     sizeof(*check)))
 				CDEBUG(D_QUOTA, "copy_to_user failed\n");
-			GOTO(out_poll, rc);
+			goto out_poll;
 		}
 
 		rc = obd_iocontrol(cmd, sbi->ll_dt_exp, 0, (void *)check,
@@ -1649,7 +1688,7 @@ free_lmm:
 			if (copy_to_user((void *)arg, check,
 					     sizeof(*check)))
 				CDEBUG(D_QUOTA, "copy_to_user failed\n");
-			GOTO(out_poll, rc);
+			goto out_poll;
 		}
 out_poll:
 		OBD_FREE_PTR(check);
@@ -1662,16 +1701,20 @@ out_poll:
 		struct if_quotactl_18 *qctl_18;
 		struct if_quotactl *qctl_20;
 
-		OBD_ALLOC_PTR(qctl_18);
+		qctl_18 = kzalloc(sizeof(*qctl_18), GFP_NOFS);
 		if (!qctl_18)
 			return -ENOMEM;
 
-		OBD_ALLOC_PTR(qctl_20);
-		if (!qctl_20)
-			GOTO(out_quotactl_18, rc = -ENOMEM);
+		qctl_20 = kzalloc(sizeof(*qctl_20), GFP_NOFS);
+		if (!qctl_20) {
+			rc = -ENOMEM;
+			goto out_quotactl_18;
+		}
 
-		if (copy_from_user(qctl_18, (void *)arg, sizeof(*qctl_18)))
-			GOTO(out_quotactl_20, rc = -ENOMEM);
+		if (copy_from_user(qctl_18, (void *)arg, sizeof(*qctl_18))) {
+			rc = -ENOMEM;
+			goto out_quotactl_20;
+		}
 
 		QCTL_COPY(qctl_20, qctl_18);
 		qctl_20->qc_idx = 0;
@@ -1712,16 +1755,18 @@ out_quotactl_18:
 	case LL_IOC_QUOTACTL: {
 		struct if_quotactl *qctl;
 
-		OBD_ALLOC_PTR(qctl);
+		qctl = kzalloc(sizeof(*qctl), GFP_NOFS);
 		if (!qctl)
 			return -ENOMEM;
 
-		if (copy_from_user(qctl, (void *)arg, sizeof(*qctl)))
-			GOTO(out_quotactl, rc = -EFAULT);
+		if (copy_from_user(qctl, (void *)arg, sizeof(*qctl))) {
+			rc = -EFAULT;
+			goto out_quotactl;
+		}
 
 		rc = quotactl_ioctl(sbi, qctl);
 
-		if (rc == 0 && copy_to_user((void *)arg,qctl,sizeof(*qctl)))
+		if (rc == 0 && copy_to_user((void *)arg, qctl, sizeof(*qctl)))
 			rc = -EFAULT;
 
 out_quotactl:
@@ -1787,10 +1832,10 @@ out_quotactl:
 		return ll_fid2path(inode, (void *)arg);
 	case LL_IOC_HSM_REQUEST: {
 		struct hsm_user_request	*hur;
-		int			 totalsize;
+		ssize_t			 totalsize;
 
-		OBD_ALLOC_PTR(hur);
-		if (hur == NULL)
+		hur = kzalloc(sizeof(*hur), GFP_NOFS);
+		if (!hur)
 			return -ENOMEM;
 
 		/* We don't know the true size yet; copy the fixed-size part */
@@ -1802,6 +1847,8 @@ out_quotactl:
 		/* Compute the whole struct size */
 		totalsize = hur_len(hur);
 		OBD_FREE_PTR(hur);
+		if (totalsize < 0)
+			return -E2BIG;
 
 		/* Final size will be more than double totalsize */
 		if (totalsize >= MDS_MAXREQSIZE / 3)
@@ -1873,8 +1920,8 @@ out_quotactl:
 		struct hsm_copy	*copy;
 		int		 rc;
 
-		OBD_ALLOC_PTR(copy);
-		if (copy == NULL)
+		copy = kzalloc(sizeof(*copy), GFP_NOFS);
+		if (!copy)
 			return -ENOMEM;
 		if (copy_from_user(copy, (char *)arg, sizeof(*copy))) {
 			OBD_FREE_PTR(copy);
@@ -1892,8 +1939,8 @@ out_quotactl:
 		struct hsm_copy	*copy;
 		int		 rc;
 
-		OBD_ALLOC_PTR(copy);
-		if (copy == NULL)
+		copy = kzalloc(sizeof(*copy), GFP_NOFS);
+		if (!copy)
 			return -ENOMEM;
 		if (copy_from_user(copy, (char *)arg, sizeof(*copy))) {
 			OBD_FREE_PTR(copy);
@@ -1929,14 +1976,14 @@ static loff_t ll_dir_seek(struct file *file, loff_t offset, int origin)
 			break;
 		case SEEK_END:
 			if (offset > 0)
-				GOTO(out, ret);
+				goto out;
 			if (api32)
 				offset += LL_DIR_END_OFF_32BIT;
 			else
 				offset += LL_DIR_END_OFF;
 			break;
 		default:
-			GOTO(out, ret);
+			goto out;
 	}
 
 	if (offset >= 0 &&
@@ -1955,7 +2002,7 @@ static loff_t ll_dir_seek(struct file *file, loff_t offset, int origin)
 		}
 		ret = offset;
 	}
-	GOTO(out, ret);
+	goto out;
 
 out:
 	mutex_unlock(&inode->i_mutex);
diff --git a/drivers/staging/lustre/lustre/llite/file.c b/drivers/staging/lustre/lustre/llite/file.c
index fd1b75a3a569..c99b74117152 100644
--- a/drivers/staging/lustre/lustre/llite/file.c
+++ b/drivers/staging/lustre/lustre/llite/file.c
@@ -142,12 +142,16 @@ static int ll_close_inode_openhandle(struct obd_export *md_exp,
 		 */
 		CERROR("Invalid MDC connection handle %#llx\n",
 		       ll_i2mdexp(inode)->exp_handle.h_cookie);
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 
-	OBD_ALLOC_PTR(op_data);
-	if (op_data == NULL)
-		GOTO(out, rc = -ENOMEM); // XXX We leak openhandle and request here.
+	op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
+	if (!op_data) {
+		/* XXX We leak openhandle and request here. */
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	ll_prepare_close(inode, op_data, och);
 	if (data_version != NULL) {
@@ -283,7 +287,7 @@ static int ll_md_close(struct obd_export *md_exp, struct inode *inode,
 	if (fd->fd_och != NULL) {
 		rc = ll_close_inode_openhandle(md_exp, inode, fd->fd_och, NULL);
 		fd->fd_och = NULL;
-		GOTO(out, rc);
+		goto out;
 	}
 
 	/* Let's see if we have good enough OPEN lock on the file and if
@@ -436,18 +440,20 @@ static int ll_intent_file_open(struct file *file, void *lmm,
 		*/
 		if (!it_disposition(itp, DISP_OPEN_OPEN) ||
 		     it_open_error(DISP_OPEN_OPEN, itp))
-			GOTO(out, rc);
+			goto out;
 		ll_release_openhandle(file->f_dentry, itp);
-		GOTO(out, rc);
+		goto out;
 	}
 
-	if (it_disposition(itp, DISP_LOOKUP_NEG))
-		GOTO(out, rc = -ENOENT);
+	if (it_disposition(itp, DISP_LOOKUP_NEG)) {
+		rc = -ENOENT;
+		goto out;
+	}
 
 	if (rc != 0 || it_open_error(DISP_OPEN_OPEN, itp)) {
 		rc = rc ? rc : it_open_error(DISP_OPEN_OPEN, itp);
 		CDEBUG(D_VFSTRACE, "lock enqueue: err: %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	rc = ll_prep_inode(&file->f_dentry->d_inode, req, NULL, itp);
@@ -551,8 +557,10 @@ int ll_file_open(struct inode *inode, struct file *file)
 	file->private_data = NULL; /* prevent ll_local_open assertion */
 
 	fd = ll_file_data_get();
-	if (fd == NULL)
-		GOTO(out_openerr, rc = -ENOMEM);
+	if (fd == NULL) {
+		rc = -ENOMEM;
+		goto out_openerr;
+	}
 
 	fd->fd_file = file;
 	if (S_ISDIR(inode->i_mode)) {
@@ -621,7 +629,7 @@ restart:
 			rc = it_open_error(DISP_OPEN_OPEN, it);
 			if (rc) {
 				mutex_unlock(&lli->lli_och_mutex);
-				GOTO(out_openerr, rc);
+				goto out_openerr;
 			}
 
 			ll_release_openhandle(file->f_dentry, it);
@@ -632,7 +640,7 @@ restart:
 		if (rc) {
 			(*och_usecount)--;
 			mutex_unlock(&lli->lli_och_mutex);
-			GOTO(out_openerr, rc);
+			goto out_openerr;
 		}
 	} else {
 		LASSERT(*och_usecount == 0);
@@ -647,13 +655,15 @@ restart:
 			rc = ll_intent_file_open(file, NULL, 0, it);
 			it->it_create_mode &= ~M_CHECK_STALE;
 			if (rc)
-				GOTO(out_openerr, rc);
+				goto out_openerr;
 
 			goto restart;
 		}
-		OBD_ALLOC(*och_p, sizeof (struct obd_client_handle));
-		if (!*och_p)
-			GOTO(out_och_free, rc = -ENOMEM);
+		*och_p = kzalloc(sizeof(struct obd_client_handle), GFP_NOFS);
+		if (!*och_p) {
+			rc = -ENOMEM;
+			goto out_och_free;
+		}
 
 		(*och_usecount)++;
 
@@ -664,13 +674,13 @@ restart:
 		 * just open error? */
 		rc = it_open_error(DISP_OPEN_OPEN, it);
 		if (rc)
-			GOTO(out_och_free, rc);
+			goto out_och_free;
 
 		LASSERT(it_disposition(it, DISP_ENQ_OPEN_REF));
 
 		rc = ll_local_open(file, it, fd, *och_p);
 		if (rc)
-			GOTO(out_och_free, rc);
+			goto out_och_free;
 	}
 	mutex_unlock(&lli->lli_och_mutex);
 	fd = NULL;
@@ -679,7 +689,7 @@ restart:
 	   different kind of OPEN lock for this same inode gets cancelled
 	   by ldlm_cancel_lru */
 	if (!S_ISREG(inode->i_mode))
-		GOTO(out_och_free, rc);
+		goto out_och_free;
 
 	ll_capa_open(inode);
 
@@ -687,10 +697,10 @@ restart:
 	    (cl_is_lov_delay_create(file->f_flags) ||
 	     (file->f_mode & FMODE_WRITE) == 0)) {
 		CDEBUG(D_INODE, "object creation was delayed\n");
-		GOTO(out_och_free, rc);
+		goto out_och_free;
 	}
 	cl_lov_delay_create_clear(&file->f_flags);
-	GOTO(out_och_free, rc);
+	goto out_och_free;
 
 out_och_free:
 	if (rc) {
@@ -801,14 +811,16 @@ ll_lease_open(struct inode *inode, struct file *file, fmode_t fmode,
 		old_handle = fd->fd_och->och_fh;
 	}
 
-	OBD_ALLOC_PTR(och);
-	if (och == NULL)
+	och = kzalloc(sizeof(*och), GFP_NOFS);
+	if (!och)
 		return ERR_PTR(-ENOMEM);
 
 	op_data = ll_prep_md_op_data(NULL, inode, inode, NULL, 0, 0,
 					LUSTRE_OPC_ANY, NULL);
-	if (IS_ERR(op_data))
-		GOTO(out, rc = PTR_ERR(op_data));
+	if (IS_ERR(op_data)) {
+		rc = PTR_ERR(op_data);
+		goto out;
+	}
 
 	/* To tell the MDT this openhandle is from the same owner */
 	op_data->op_handle = old_handle;
@@ -827,20 +839,24 @@ ll_lease_open(struct inode *inode, struct file *file, fmode_t fmode,
 	ll_finish_md_op_data(op_data);
 	ptlrpc_req_finished(req);
 	if (rc < 0)
-		GOTO(out_release_it, rc);
+		goto out_release_it;
 
-	if (it_disposition(&it, DISP_LOOKUP_NEG))
-		GOTO(out_release_it, rc = -ENOENT);
+	if (it_disposition(&it, DISP_LOOKUP_NEG)) {
+		rc = -ENOENT;
+		goto out_release_it;
+	}
 
 	rc = it_open_error(DISP_OPEN_OPEN, &it);
 	if (rc)
-		GOTO(out_release_it, rc);
+		goto out_release_it;
 
 	LASSERT(it_disposition(&it, DISP_ENQ_OPEN_REF));
 	ll_och_fill(sbi->ll_md_exp, &it, och);
 
-	if (!it_disposition(&it, DISP_OPEN_LEASE)) /* old server? */
-		GOTO(out_close, rc = -EOPNOTSUPP);
+	if (!it_disposition(&it, DISP_OPEN_LEASE)) /* old server? */ {
+		rc = -EOPNOTSUPP;
+		goto out_close;
+	}
 
 	/* already get lease, handle lease lock */
 	ll_set_lock_data(sbi->ll_md_exp, inode, &it, NULL);
@@ -850,7 +866,8 @@ ll_lease_open(struct inode *inode, struct file *file, fmode_t fmode,
 		CERROR(DFID "lease granted but no open lock, %d/%llu.\n",
 			PFID(ll_inode2fid(inode)), it.d.lustre.it_lock_mode,
 			it.d.lustre.it_lock_bits);
-		GOTO(out_close, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out_close;
 	}
 
 	ll_intent_release(&it);
@@ -974,7 +991,7 @@ int ll_inode_getattr(struct inode *inode, struct obdo *obdo,
 		CDEBUG(D_INODE, "objid "DOSTID" size %llu, blocks %llu,"
 		       " blksize %lu\n", POSTID(oi), i_size_read(inode),
 		       (unsigned long long)inode->i_blocks,
-		       (unsigned long)ll_inode_blksize(inode));
+		       1UL << inode->i_blkbits);
 	}
 	ccc_inode_lsm_put(inode, lsm);
 	return rc;
@@ -994,7 +1011,12 @@ int ll_merge_lvb(const struct lu_env *env, struct inode *inode)
 	LTIME_S(inode->i_atime) = lli->lli_lvb.lvb_atime;
 	LTIME_S(inode->i_mtime) = lli->lli_lvb.lvb_mtime;
 	LTIME_S(inode->i_ctime) = lli->lli_lvb.lvb_ctime;
-	inode_init_lvb(inode, &lvb);
+
+	lvb.lvb_size = i_size_read(inode);
+	lvb.lvb_blocks = inode->i_blocks;
+	lvb.lvb_mtime = LTIME_S(inode->i_mtime);
+	lvb.lvb_atime = LTIME_S(inode->i_atime);
+	lvb.lvb_ctime = LTIME_S(inode->i_ctime);
 
 	cl_object_attr_lock(obj);
 	rc = cl_object_attr_get(env, obj, attr);
@@ -1119,8 +1141,10 @@ restart:
 			if ((iot == CIT_WRITE) &&
 			    !(cio->cui_fd->fd_flags & LL_FILE_GROUP_LOCKED)) {
 				if (mutex_lock_interruptible(&lli->
-							       lli_write_mutex))
-					GOTO(out, result = -ERESTARTSYS);
+							       lli_write_mutex)) {
+					result = -ERESTARTSYS;
+					goto out;
+				}
 				write_mutex_locked = 1;
 			} else if (iot == CIT_READ) {
 				down_read(&lli->lli_trunc_sem);
@@ -1148,7 +1172,7 @@ restart:
 		result = io->ci_nob;
 		*ppos = io->u.ci_wr.wr.crw_pos;
 	}
-	GOTO(out, result);
+	goto out;
 out:
 	cl_io_fini(env, io);
 	/* If any bit been read/written (result != 0), we just return
@@ -1248,8 +1272,7 @@ static ssize_t ll_file_splice_read(struct file *in_file, loff_t *ppos,
 	return result;
 }
 
-static int ll_lov_recreate(struct inode *inode, struct ost_id *oi,
-			   obd_count ost_idx)
+static int ll_lov_recreate(struct inode *inode, struct ost_id *oi, u32 ost_idx)
 {
 	struct obd_export *exp = ll_i2dtexp(inode);
 	struct obd_trans_info oti = { 0 };
@@ -1263,15 +1286,19 @@ static int ll_lov_recreate(struct inode *inode, struct ost_id *oi,
 		return -ENOMEM;
 
 	lsm = ccc_inode_lsm_get(inode);
-	if (!lsm_has_objects(lsm))
-		GOTO(out, rc = -ENOENT);
+	if (!lsm_has_objects(lsm)) {
+		rc = -ENOENT;
+		goto out;
+	}
 
 	lsm_size = sizeof(*lsm) + (sizeof(struct lov_oinfo) *
 		   (lsm->lsm_stripe_count));
 
 	OBD_ALLOC_LARGE(lsm2, lsm_size);
-	if (lsm2 == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (lsm2 == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	oa->o_oi = *oi;
 	oa->o_nlink = ost_idx;
@@ -1286,7 +1313,7 @@ static int ll_lov_recreate(struct inode *inode, struct ost_id *oi,
 	ll_inode_size_unlock(inode);
 
 	OBD_FREE_LARGE(lsm2, lsm_size);
-	GOTO(out, rc);
+	goto out;
 out:
 	ccc_inode_lsm_put(inode, lsm);
 	OBDO_FREE(oa);
@@ -1314,7 +1341,7 @@ static int ll_lov_recreate_fid(struct inode *inode, unsigned long arg)
 {
 	struct lu_fid	fid;
 	struct ost_id	oi;
-	obd_count	ost_idx;
+	u32		ost_idx;
 
 	if (!capable(CFS_CAP_SYS_ADMIN))
 		return -EPERM;
@@ -1339,16 +1366,17 @@ int ll_lov_setstripe_ea_info(struct inode *inode, struct file *file,
 		ccc_inode_lsm_put(inode, lsm);
 		CDEBUG(D_IOCTL, "stripe already exists for ino %lu\n",
 		       inode->i_ino);
-		GOTO(out, rc = -EEXIST);
+		rc = -EEXIST;
+		goto out;
 	}
 
 	ll_inode_size_lock(inode);
 	rc = ll_intent_file_open(file, lum, lum_size, &oit);
 	if (rc)
-		GOTO(out_unlock, rc);
+		goto out_unlock;
 	rc = oit.d.lustre.it_status;
 	if (rc < 0)
-		GOTO(out_req_free, rc);
+		goto out_req_free;
 
 	ll_release_openhandle(file->f_dentry, &oit);
 
@@ -1391,7 +1419,7 @@ int ll_lov_getstripe_ea_info(struct inode *inode, const char *filename,
 	if (rc < 0) {
 		CDEBUG(D_INFO, "md_getattr_name failed "
 		       "on %s: rc %d\n", filename, rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
@@ -1401,7 +1429,8 @@ int ll_lov_getstripe_ea_info(struct inode *inode, const char *filename,
 
 	if (!(body->valid & (OBD_MD_FLEASIZE | OBD_MD_FLDIREA)) ||
 			lmmsize == 0) {
-		GOTO(out, rc = -ENODATA);
+		rc = -ENODATA;
+		goto out;
 	}
 
 	lmm = req_capsule_server_sized_get(&req->rq_pill, &RMF_MDT_MD, lmmsize);
@@ -1409,7 +1438,8 @@ int ll_lov_getstripe_ea_info(struct inode *inode, const char *filename,
 
 	if ((lmm->lmm_magic != cpu_to_le32(LOV_MAGIC_V1)) &&
 	    (lmm->lmm_magic != cpu_to_le32(LOV_MAGIC_V3))) {
-		GOTO(out, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out;
 	}
 
 	/*
@@ -1464,7 +1494,7 @@ static int ll_lov_setea(struct inode *inode, struct file *file,
 	if (lump == NULL)
 		return -ENOMEM;
 
-	if (copy_from_user(lump, (struct lov_user_md  *)arg, lum_size)) {
+	if (copy_from_user(lump, (struct lov_user_md *)arg, lum_size)) {
 		OBD_FREE_LARGE(lump, lum_size);
 		return -EFAULT;
 	}
@@ -1625,9 +1655,11 @@ int ll_release_openhandle(struct dentry *dentry, struct lookup_intent *it)
 
 	LASSERT(it_open_error(DISP_OPEN_OPEN, it) == 0);
 
-	OBD_ALLOC(och, sizeof(*och));
-	if (!och)
-		GOTO(out, rc = -ENOMEM);
+	och = kzalloc(sizeof(*och), GFP_NOFS);
+	if (!och) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	ll_och_fill(ll_i2sbi(inode)->ll_md_exp, it, och);
 
@@ -1676,8 +1708,10 @@ static int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
 	 * DEVICE_ORDER flag, then it cannot interpret the extents correctly.
 	 */
 	if (lsm->lsm_stripe_count > 1 &&
-	    !(fiemap->fm_flags & FIEMAP_FLAG_DEVICE_ORDER))
-		GOTO(out, rc = -EOPNOTSUPP);
+	    !(fiemap->fm_flags & FIEMAP_FLAG_DEVICE_ORDER)) {
+		rc = -EOPNOTSUPP;
+		goto out;
+	}
 
 	fm_key.oa.o_oi = lsm->lsm_oi;
 	fm_key.oa.o_valid = OBD_MD_FLID | OBD_MD_FLGROUP;
@@ -1687,7 +1721,8 @@ static int ll_do_fiemap(struct inode *inode, struct ll_user_fiemap *fiemap,
 	/* If filesize is 0, then there would be no objects for mapping */
 	if (fm_key.oa.o_size == 0) {
 		fiemap->fm_mapped_extents = 0;
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 
 	memcpy(&fm_key.fiemap, fiemap, sizeof(*fiemap));
@@ -1702,38 +1737,41 @@ out:
 	return rc;
 }
 
-int ll_fid2path(struct inode *inode, void *arg)
+int ll_fid2path(struct inode *inode, void __user *arg)
 {
-	struct obd_export	*exp = ll_i2mdexp(inode);
-	struct getinfo_fid2path	*gfout, *gfin;
-	int			 outsize, rc;
+	struct obd_export *exp = ll_i2mdexp(inode);
+	const struct getinfo_fid2path __user *gfin = arg;
+	struct getinfo_fid2path *gfout;
+	u32 pathlen;
+	size_t outsize;
+	int rc;
 
 	if (!capable(CFS_CAP_DAC_READ_SEARCH) &&
 	    !(ll_i2sbi(inode)->ll_flags & LL_SBI_USER_FID2PATH))
 		return -EPERM;
 
-	/* Need to get the buflen */
-	OBD_ALLOC_PTR(gfin);
-	if (gfin == NULL)
-		return -ENOMEM;
-	if (copy_from_user(gfin, arg, sizeof(*gfin))) {
-		OBD_FREE_PTR(gfin);
+	/* Only need to get the buflen */
+	if (get_user(pathlen, &gfin->gf_pathlen))
 		return -EFAULT;
-	}
 
-	outsize = sizeof(*gfout) + gfin->gf_pathlen;
-	OBD_ALLOC(gfout, outsize);
-	if (gfout == NULL) {
-		OBD_FREE_PTR(gfin);
+	if (pathlen > PATH_MAX)
+		return -EINVAL;
+
+	outsize = sizeof(*gfout) + pathlen;
+
+	gfout = kzalloc(outsize, GFP_NOFS);
+	if (!gfout)
 		return -ENOMEM;
+
+	if (copy_from_user(gfout, arg, sizeof(*gfout))) {
+		rc = -EFAULT;
+		goto gf_free;
 	}
-	memcpy(gfout, gfin, sizeof(*gfout));
-	OBD_FREE_PTR(gfin);
 
 	/* Call mdc_iocontrol */
 	rc = obd_iocontrol(OBD_IOC_FID2PATH, exp, outsize, gfout, NULL);
-	if (rc)
-		GOTO(gf_free, rc);
+	if (rc != 0)
+		goto gf_free;
 
 	if (copy_to_user(arg, gfout, outsize))
 		rc = -EFAULT;
@@ -1768,8 +1806,10 @@ static int ll_ioctl_fiemap(struct inode *inode, unsigned long arg)
 
 	/* get the fiemap value */
 	if (copy_from_user(fiemap_s, (struct ll_user_fiemap __user *)arg,
-			   sizeof(*fiemap_s)))
-		GOTO(error, rc = -EFAULT);
+			   sizeof(*fiemap_s))) {
+		rc = -EFAULT;
+		goto error;
+	}
 
 	/* If fm_extent_count is non-zero, read the first extent since
 	 * it is used to calculate end_offset and device from previous
@@ -1777,13 +1817,15 @@ static int ll_ioctl_fiemap(struct inode *inode, unsigned long arg)
 	if (extent_count) {
 		if (copy_from_user(&fiemap_s->fm_extents[0],
 		    (char __user *)arg + sizeof(*fiemap_s),
-		    sizeof(struct ll_fiemap_extent)))
-			GOTO(error, rc = -EFAULT);
+		    sizeof(struct ll_fiemap_extent))) {
+			rc = -EFAULT;
+			goto error;
+		}
 	}
 
 	rc = ll_do_fiemap(inode, fiemap_s, num_bytes);
 	if (rc)
-		GOTO(error, rc);
+		goto error;
 
 	ret_bytes = sizeof(struct ll_user_fiemap);
 
@@ -1821,12 +1863,15 @@ int ll_data_version(struct inode *inode, __u64 *data_version,
 	if (!lsm_has_objects(lsm)) {
 		*data_version = 0;
 		CDEBUG(D_INODE, "No object for inode\n");
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 
-	OBD_ALLOC_PTR(obdo);
-	if (obdo == NULL)
-		GOTO(out, rc = -ENOMEM);
+	obdo = kzalloc(sizeof(*obdo), GFP_NOFS);
+	if (!obdo) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	rc = ll_lsm_getattr(lsm, sbi->ll_dt_exp, NULL, obdo, 0, extent_lock);
 	if (rc == 0) {
@@ -1859,17 +1904,21 @@ int ll_hsm_release(struct inode *inode)
 	       PFID(&ll_i2info(inode)->lli_fid));
 
 	och = ll_lease_open(inode, NULL, FMODE_WRITE, MDS_OPEN_RELEASE);
-	if (IS_ERR(och))
-		GOTO(out, rc = PTR_ERR(och));
+	if (IS_ERR(och)) {
+		rc = PTR_ERR(och);
+		goto out;
+	}
 
 	/* Grab latest data_version and [am]time values */
 	rc = ll_data_version(inode, &data_version, 1);
 	if (rc != 0)
-		GOTO(out, rc);
+		goto out;
 
 	env = cl_env_nested_get(&nest);
-	if (IS_ERR(env))
-		GOTO(out, rc = PTR_ERR(env));
+	if (IS_ERR(env)) {
+		rc = PTR_ERR(env);
+		goto out;
+	}
 
 	ll_merge_lvb(env, inode);
 	cl_env_nested_put(&nest, env);
@@ -1906,22 +1955,28 @@ static int ll_swap_layouts(struct file *file1, struct file *file2,
 	struct ll_swap_stack	*llss = NULL;
 	int			 rc;
 
-	OBD_ALLOC_PTR(llss);
-	if (llss == NULL)
+	llss = kzalloc(sizeof(*llss), GFP_NOFS);
+	if (!llss)
 		return -ENOMEM;
 
 	llss->inode1 = file1->f_dentry->d_inode;
 	llss->inode2 = file2->f_dentry->d_inode;
 
-	if (!S_ISREG(llss->inode2->i_mode))
-		GOTO(free, rc = -EINVAL);
+	if (!S_ISREG(llss->inode2->i_mode)) {
+		rc = -EINVAL;
+		goto free;
+	}
 
 	if (inode_permission(llss->inode1, MAY_WRITE) ||
-	    inode_permission(llss->inode2, MAY_WRITE))
-		GOTO(free, rc = -EPERM);
+	    inode_permission(llss->inode2, MAY_WRITE)) {
+		rc = -EPERM;
+		goto free;
+	}
 
-	if (llss->inode2->i_sb != llss->inode1->i_sb)
-		GOTO(free, rc = -EXDEV);
+	if (llss->inode2->i_sb != llss->inode1->i_sb) {
+		rc = -EXDEV;
+		goto free;
+	}
 
 	/* we use 2 bool because it is easier to swap than 2 bits */
 	if (lsl->sl_flags & SWAP_LAYOUTS_CHECK_DV1)
@@ -1935,8 +1990,10 @@ static int ll_swap_layouts(struct file *file1, struct file *file2,
 	llss->dv2 = lsl->sl_dv2;
 
 	rc = lu_fid_cmp(ll_inode2fid(llss->inode1), ll_inode2fid(llss->inode2));
-	if (rc == 0) /* same file, done! */
-		GOTO(free, rc = 0);
+	if (rc == 0) /* same file, done! */ {
+		rc = 0;
+		goto free;
+	}
 
 	if (rc < 0) { /* sequentialize it */
 		swap(llss->inode1, llss->inode2);
@@ -1949,12 +2006,12 @@ static int ll_swap_layouts(struct file *file1, struct file *file2,
 	if (gid != 0) { /* application asks to flush dirty cache */
 		rc = ll_get_grouplock(llss->inode1, file1, gid);
 		if (rc < 0)
-			GOTO(free, rc);
+			goto free;
 
 		rc = ll_get_grouplock(llss->inode2, file2, gid);
 		if (rc < 0) {
 			ll_put_grouplock(llss->inode1, file1, gid);
-			GOTO(free, rc);
+			goto free;
 		}
 	}
 
@@ -1975,17 +2032,21 @@ static int ll_swap_layouts(struct file *file1, struct file *file2,
 	if (llss->check_dv1) {
 		rc = ll_data_version(llss->inode1, &dv, 0);
 		if (rc)
-			GOTO(putgl, rc);
-		if (dv != llss->dv1)
-			GOTO(putgl, rc = -EAGAIN);
+			goto putgl;
+		if (dv != llss->dv1) {
+			rc = -EAGAIN;
+			goto putgl;
+		}
 	}
 
 	if (llss->check_dv2) {
 		rc = ll_data_version(llss->inode2, &dv, 0);
 		if (rc)
-			GOTO(putgl, rc);
-		if (dv != llss->dv2)
-			GOTO(putgl, rc = -EAGAIN);
+			goto putgl;
+		if (dv != llss->dv2) {
+			rc = -EAGAIN;
+			goto putgl;
+		}
 	}
 
 	/* struct md_op_data is used to send the swap args to the mdt
@@ -1997,8 +2058,10 @@ static int ll_swap_layouts(struct file *file1, struct file *file2,
 	rc = -ENOMEM;
 	op_data = ll_prep_md_op_data(NULL, llss->inode1, llss->inode2, NULL, 0,
 				     0, LUSTRE_OPC_ANY, &msl);
-	if (IS_ERR(op_data))
-		GOTO(free, rc = PTR_ERR(op_data));
+	if (IS_ERR(op_data)) {
+		rc = PTR_ERR(op_data);
+		goto free;
+	}
 
 	rc = obd_iocontrol(LL_IOC_LOV_SWAP_LAYOUTS, ll_i2mdexp(llss->inode1),
 			   sizeof(*op_data), op_data, NULL);
@@ -2012,7 +2075,7 @@ putgl:
 
 	/* rc can be set from obd_iocontrol() or from a GOTO(putgl, ...) */
 	if (rc != 0)
-		GOTO(free, rc);
+		goto free;
 
 	/* clear useless flags */
 	if (!(lsl->sl_flags & SWAP_LAYOUTS_KEEP_MTIME)) {
@@ -2086,20 +2149,24 @@ static int ll_hsm_import(struct inode *inode, struct file *file,
 		return -EINVAL;
 
 	/* set HSM flags */
-	OBD_ALLOC_PTR(hss);
-	if (hss == NULL)
-		GOTO(out, rc = -ENOMEM);
+	hss = kzalloc(sizeof(*hss), GFP_NOFS);
+	if (!hss) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	hss->hss_valid = HSS_SETMASK | HSS_ARCHIVE_ID;
 	hss->hss_archive_id = hui->hui_archive_id;
 	hss->hss_setmask = HS_ARCHIVED | HS_EXISTS | HS_RELEASED;
 	rc = ll_hsm_state_set(inode, hss);
 	if (rc != 0)
-		GOTO(out, rc);
+		goto out;
 
-	OBD_ALLOC_PTR(attr);
-	if (attr == NULL)
-		GOTO(out, rc = -ENOMEM);
+	attr = kzalloc(sizeof(*attr), GFP_NOFS);
+	if (!attr) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	attr->ia_mode = hui->hui_mode & (S_IRWXU | S_IRWXG | S_IRWXO);
 	attr->ia_mode |= S_IFREG;
@@ -2116,10 +2183,14 @@ static int ll_hsm_import(struct inode *inode, struct file *file,
 			 ATTR_MTIME | ATTR_MTIME_SET |
 			 ATTR_ATIME | ATTR_ATIME_SET;
 
+	mutex_lock(&inode->i_mutex);
+
 	rc = ll_setattr_raw(file->f_dentry, attr, true);
 	if (rc == -ENODATA)
 		rc = 0;
 
+	mutex_unlock(&inode->i_mutex);
+
 out:
 	if (hss != NULL)
 		OBD_FREE_PTR(hss);
@@ -2145,7 +2216,7 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	if (_IOC_TYPE(cmd) == 'T' || _IOC_TYPE(cmd) == 't') /* tty ioctls */
 		return -ENOTTY;
 
-	switch(cmd) {
+	switch (cmd) {
 	case LL_IOC_GETFLAGS:
 		/* Get the current value of the file flags */
 		return put_user(fd->fd_flags, (int *)arg);
@@ -2257,7 +2328,7 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		if (mdtidx < 0)
 			return mdtidx;
 
-		if (put_user((int)mdtidx, (int*)arg))
+		if (put_user((int)mdtidx, (int *)arg))
 			return -EFAULT;
 
 		return 0;
@@ -2270,8 +2341,8 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		struct hsm_user_state	*hus;
 		int			 rc;
 
-		OBD_ALLOC_PTR(hus);
-		if (hus == NULL)
+		hus = kzalloc(sizeof(*hus), GFP_NOFS);
+		if (!hus)
 			return -ENOMEM;
 
 		op_data = ll_prep_md_op_data(NULL, inode, NULL, NULL, 0, 0,
@@ -2295,8 +2366,8 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		struct hsm_state_set	*hss;
 		int			 rc;
 
-		OBD_ALLOC_PTR(hss);
-		if (hss == NULL)
+		hss = kzalloc(sizeof(*hss), GFP_NOFS);
+		if (!hss)
 			return -ENOMEM;
 
 		if (copy_from_user(hss, (char *)arg, sizeof(*hss))) {
@@ -2314,8 +2385,8 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 		struct hsm_current_action	*hca;
 		int				 rc;
 
-		OBD_ALLOC_PTR(hca);
-		if (hca == NULL)
+		hca = kzalloc(sizeof(*hca), GFP_NOFS);
+		if (!hca)
 			return -ENOMEM;
 
 		op_data = ll_prep_md_op_data(NULL, inode, NULL, NULL, 0, 0,
@@ -2422,8 +2493,8 @@ ll_file_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 	case LL_IOC_HSM_IMPORT: {
 		struct hsm_user_import *hui;
 
-		OBD_ALLOC_PTR(hui);
-		if (hui == NULL)
+		hui = kzalloc(sizeof(*hui), GFP_NOFS);
+		if (!hui)
 			return -ENOMEM;
 
 		if (copy_from_user(hui, (void *)arg, sizeof(*hui))) {
@@ -2869,13 +2940,13 @@ static int __ll_inode_revalidate(struct dentry *dentry, __u64 ibits)
 		oit.it_create_mode &= ~M_CHECK_STALE;
 		if (rc < 0) {
 			rc = ll_inode_revalidate_fini(inode, rc);
-			GOTO (out, rc);
+			goto out;
 		}
 
 		rc = ll_revalidate_it_finish(req, &oit, dentry);
 		if (rc != 0) {
 			ll_intent_release(&oit);
-			GOTO(out, rc);
+			goto out;
 		}
 
 		/* Unlinked? Unhash dentry, so it is not picked up later by
@@ -2888,7 +2959,7 @@ static int __ll_inode_revalidate(struct dentry *dentry, __u64 ibits)
 		ll_lookup_finish_locks(&oit, dentry);
 	} else if (!ll_have_md_lock(dentry->d_inode, &ibits, LCK_MINMODE)) {
 		struct ll_sb_info *sbi = ll_i2sbi(dentry->d_inode);
-		obd_valid valid = OBD_MD_FLGETATTR;
+		u64 valid = OBD_MD_FLGETATTR;
 		struct md_op_data *op_data;
 		int ealen = 0;
 
@@ -3158,8 +3229,8 @@ void *ll_iocontrol_register(llioc_callback_t cb, int count, unsigned int *cmd)
 		return NULL;
 
 	size = sizeof(*in_data) + count * sizeof(unsigned int);
-	OBD_ALLOC(in_data, size);
-	if (in_data == NULL)
+	in_data = kzalloc(size, GFP_NOFS);
+	if (!in_data)
 		return NULL;
 
 	memset(in_data, 0, sizeof(*in_data));
@@ -3299,20 +3370,28 @@ static int ll_layout_fetch(struct inode *inode, struct ldlm_lock *lock)
 		return rc;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
-	if (body == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (body == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	lmmsize = body->eadatasize;
-	if (lmmsize == 0) /* empty layout */
-		GOTO(out, rc = 0);
+	if (lmmsize == 0) /* empty layout */ {
+		rc = 0;
+		goto out;
+	}
 
 	lmm = req_capsule_server_sized_get(&req->rq_pill, &RMF_EADATA, lmmsize);
-	if (lmm == NULL)
-		GOTO(out, rc = -EFAULT);
+	if (lmm == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	OBD_ALLOC_LARGE(lvbdata, lmmsize);
-	if (lvbdata == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (lvbdata == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	memcpy(lvbdata, lmm, lmmsize);
 	lock_res_and_lock(lock);
@@ -3369,12 +3448,12 @@ static int ll_layout_lock_set(struct lustre_handle *lockh, ldlm_mode_t mode,
 			*gen = ll_layout_version_get(lli);
 			rc = 0;
 		}
-		GOTO(out, rc);
+		goto out;
 	}
 
 	rc = ll_layout_fetch(inode, lock);
 	if (rc < 0)
-		GOTO(out, rc);
+		goto out;
 
 	/* for layout lock, lmm is returned in lock's lvb.
 	 * lvb_data is immutable if the lock is held so it's safe to access it
@@ -3395,7 +3474,7 @@ static int ll_layout_lock_set(struct lustre_handle *lockh, ldlm_mode_t mode,
 		}
 	}
 	if (rc < 0)
-		GOTO(out, rc);
+		goto out;
 
 	/* set layout to file. Unlikely this will fail as old layout was
 	 * surely eliminated */
@@ -3539,8 +3618,8 @@ int ll_layout_restore(struct inode *inode)
 
 	len = sizeof(struct hsm_user_request) +
 	      sizeof(struct hsm_user_item);
-	OBD_ALLOC(hur, len);
-	if (hur == NULL)
+	hur = kzalloc(len, GFP_NOFS);
+	if (!hur)
 		return -ENOMEM;
 
 	hur->hur_request.hr_action = HUA_RESTORE;
diff --git a/drivers/staging/lustre/lustre/llite/llite_capa.c b/drivers/staging/lustre/lustre/llite/llite_capa.c
index 023c40518c6d..b1e39ee412cd 100644
--- a/drivers/staging/lustre/lustre/llite/llite_capa.c
+++ b/drivers/staging/lustre/lustre/llite/llite_capa.c
@@ -543,7 +543,7 @@ static int ll_update_capa(struct obd_capa *ocapa, struct lustre_capa *capa)
 					   "renewal failed: -EIO, "
 					   "retry in 2 mins");
 				ll_capa_renewal_retries++;
-				GOTO(retry, rc);
+				goto retry;
 			} else {
 				DEBUG_CAPA(D_ERROR, &ocapa->c_capa,
 					   "renewal failed(rc: %d) for", rc);
diff --git a/drivers/staging/lustre/lustre/llite/llite_close.c b/drivers/staging/lustre/lustre/llite/llite_close.c
index 6a3a7a303043..84e0003f2daf 100644
--- a/drivers/staging/lustre/lustre/llite/llite_close.c
+++ b/drivers/staging/lustre/lustre/llite/llite_close.c
@@ -155,7 +155,7 @@ void ll_ioepoch_close(struct inode *inode, struct md_op_data *op_data,
 
 			inode = igrab(inode);
 			LASSERT(inode);
-			GOTO(out, 0);
+			goto out;
 		}
 		if (flags & LLIF_DONE_WRITING) {
 			/* Some pages are still dirty, it is early to send
@@ -167,7 +167,7 @@ void ll_ioepoch_close(struct inode *inode, struct md_op_data *op_data,
 
 			inode = igrab(inode);
 			LASSERT(inode);
-			GOTO(out, 0);
+			goto out;
 		}
 	}
 	CDEBUG(D_INODE, "Epoch %llu closed on "DFID"\n",
@@ -184,14 +184,14 @@ void ll_ioepoch_close(struct inode *inode, struct md_op_data *op_data,
 		/* Pack Size-on-MDS inode attributes only if they has changed */
 		if (!(lli->lli_flags & LLIF_SOM_DIRTY)) {
 			spin_unlock(&lli->lli_lock);
-			GOTO(out, 0);
+			goto out;
 		}
 
 		/* There is a pending DONE_WRITE -- close epoch with no
 		 * attribute change. */
 		if (lli->lli_flags & LLIF_EPOCH_PENDING) {
 			spin_unlock(&lli->lli_lock);
-			GOTO(out, 0);
+			goto out;
 		}
 	}
 
@@ -285,8 +285,8 @@ static void ll_done_writing(struct inode *inode)
 
 	LASSERT(exp_connect_som(ll_i2mdexp(inode)));
 
-	OBD_ALLOC_PTR(op_data);
-	if (op_data == NULL) {
+	op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
+	if (!op_data) {
 		CERROR("can't allocate op_data\n");
 		return;
 	}
@@ -294,7 +294,7 @@ static void ll_done_writing(struct inode *inode)
 	ll_prepare_done_writing(inode, op_data, &och);
 	/* If there is no @och, we do not do D_W yet. */
 	if (och == NULL)
-		GOTO(out, 0);
+		goto out;
 
 	rc = md_done_writing(ll_i2sbi(inode)->ll_md_exp, op_data, NULL);
 	if (rc == -EAGAIN) {
@@ -367,8 +367,8 @@ int ll_close_thread_start(struct ll_close_queue **lcq_ret)
 	if (OBD_FAIL_CHECK(OBD_FAIL_LDLM_CLOSE_THREAD))
 		return -EINTR;
 
-	OBD_ALLOC(lcq, sizeof(*lcq));
-	if (lcq == NULL)
+	lcq = kzalloc(sizeof(*lcq), GFP_NOFS);
+	if (!lcq)
 		return -ENOMEM;
 
 	spin_lock_init(&lcq->lcq_lock);
diff --git a/drivers/staging/lustre/lustre/llite/llite_internal.h b/drivers/staging/lustre/lustre/llite/llite_internal.h
index 634ffa645e06..36aa0fd147f2 100644
--- a/drivers/staging/lustre/lustre/llite/llite_internal.h
+++ b/drivers/staging/lustre/lustre/llite/llite_internal.h
@@ -45,7 +45,7 @@
 #include "../include/cl_object.h"
 #include "../include/lclient.h"
 #include "../include/lustre_mdc.h"
-#include "../include/linux/lustre_intent.h"
+#include "../include/lustre_intent.h"
 #include <linux/compat.h>
 #include <linux/posix_acl_xattr.h>
 
@@ -305,8 +305,8 @@ int ll_xattr_cache_get(struct inode *inode,
 void ll_inode_size_lock(struct inode *inode);
 void ll_inode_size_unlock(struct inode *inode);
 
-// FIXME: replace the name of this with LL_I to conform to kernel stuff
-// static inline struct ll_inode_info *LL_I(struct inode *inode)
+/* FIXME: replace the name of this with LL_I to conform to kernel stuff */
+/* static inline struct ll_inode_info *LL_I(struct inode *inode) */
 static inline struct ll_inode_info *ll_i2info(struct inode *inode)
 {
 	return container_of(inode, struct ll_inode_info, lli_vfs_inode);
@@ -652,7 +652,7 @@ static inline struct inode *ll_info2i(struct ll_inode_info *lli)
 }
 
 __u32 ll_i2suppgid(struct inode *i);
-void ll_i2gids(__u32 *suppgids, struct inode *i1,struct inode *i2);
+void ll_i2gids(__u32 *suppgids, struct inode *i1, struct inode *i2);
 
 static inline int ll_need_32bit_api(struct ll_sb_info *sbi)
 {
@@ -775,7 +775,7 @@ int ll_dir_getstripe(struct inode *inode, struct lov_mds_md **lmmp,
 		     int *lmm_size, struct ptlrpc_request **request);
 int ll_fsync(struct file *file, loff_t start, loff_t end, int data);
 int ll_merge_lvb(const struct lu_env *env, struct inode *inode);
-int ll_fid2path(struct inode *inode, void *arg);
+int ll_fid2path(struct inode *inode, void __user *arg);
 int ll_data_version(struct inode *inode, __u64 *data_version, int extent_lock);
 int ll_hsm_release(struct inode *inode);
 
@@ -894,6 +894,10 @@ struct vvp_io {
 				 * fault API used bitflags for return code.
 				 */
 				unsigned int    ft_flags;
+				/**
+				 * check that flags are from filemap_fault
+				 */
+				bool		ft_flags_valid;
 			} fault;
 		} fault;
 	} u;
@@ -991,7 +995,7 @@ int ll_close_thread_start(struct ll_close_queue **lcq_ret);
 /* llite/llite_mmap.c */
 
 int ll_teardown_mmaps(struct address_space *mapping, __u64 first, __u64 last);
-int ll_file_mmap(struct file * file, struct vm_area_struct * vma);
+int ll_file_mmap(struct file *file, struct vm_area_struct *vma);
 void policy_from_vma(ldlm_policy_data_t *policy,
 		struct vm_area_struct *vma, unsigned long addr, size_t count);
 struct vm_area_struct *our_vma(struct mm_struct *mm, unsigned long addr,
@@ -1032,7 +1036,7 @@ static inline struct client_obd *sbi2mdc(struct ll_sb_info *sbi)
 	return &obd->u.cli;
 }
 
-// FIXME: replace the name of this with LL_SB to conform to kernel stuff
+/* FIXME: replace the name of this with LL_SB to conform to kernel stuff */
 static inline struct ll_sb_info *ll_i2sbi(struct inode *inode)
 {
 	return ll_s2sbi(inode->i_sb);
@@ -1124,7 +1128,7 @@ struct eacl_entry {
 	ext_acl_xattr_header *ee_acl;
 };
 
-obd_valid rce_ops2valid(int ops);
+u64 rce_ops2valid(int ops);
 struct rmtacl_ctl_entry *rct_search(struct rmtacl_ctl_table *rct, pid_t key);
 int rct_add(struct rmtacl_ctl_table *rct, pid_t key, int ops);
 int rct_del(struct rmtacl_ctl_table *rct, pid_t key);
@@ -1140,7 +1144,7 @@ void et_search_free(struct eacl_table *et, pid_t key);
 void et_init(struct eacl_table *et);
 void et_fini(struct eacl_table *et);
 #else
-static inline obd_valid rce_ops2valid(int ops)
+static inline u64 rce_ops2valid(int ops)
 {
 	return 0;
 }
diff --git a/drivers/staging/lustre/lustre/llite/llite_lib.c b/drivers/staging/lustre/lustre/llite/llite_lib.c
index 0367f5a2cfe4..a8bcc51057f1 100644
--- a/drivers/staging/lustre/lustre/llite/llite_lib.c
+++ b/drivers/staging/lustre/lustre/llite/llite_lib.c
@@ -41,6 +41,7 @@
 #define DEBUG_SUBSYSTEM S_LLITE
 
 #include <linux/module.h>
+#include <linux/statfs.h>
 #include <linux/types.h>
 #include <linux/mm.h>
 
@@ -74,7 +75,7 @@ static struct ll_sb_info *ll_init_sbi(void)
 	class_uuid_t uuid;
 	int i;
 
-	OBD_ALLOC(sbi, sizeof(*sbi));
+	sbi = kzalloc(sizeof(*sbi), GFP_NOFS);
 	if (!sbi)
 		return NULL;
 
@@ -162,7 +163,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	struct obd_uuid *uuid;
 	struct md_op_data *op_data;
 	struct lustre_md lmd;
-	obd_valid valid;
+	u64 valid;
 	int size, err, checksum;
 
 	obd = class_name2obd(md);
@@ -171,12 +172,12 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 		return -EINVAL;
 	}
 
-	OBD_ALLOC_PTR(data);
-	if (data == NULL)
+	data = kzalloc(sizeof(*data), GFP_NOFS);
+	if (!data)
 		return -ENOMEM;
 
-	OBD_ALLOC_PTR(osfs);
-	if (osfs == NULL) {
+	osfs = kzalloc(sizeof(*osfs), GFP_NOFS);
+	if (!osfs) {
 		OBD_FREE_PTR(data);
 		return -ENOMEM;
 	}
@@ -255,10 +256,10 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 				   "recovery, of which this client is not a "
 				   "part. Please wait for recovery to complete,"
 				   " abort, or time out.\n", md);
-		GOTO(out, err);
+		goto out;
 	} else if (err) {
 		CERROR("cannot connect to %s: rc = %d\n", md, err);
-		GOTO(out, err);
+		goto out;
 	}
 
 	sbi->ll_md_exp->exp_connect_data = *data;
@@ -268,7 +269,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (err) {
 		CERROR("%s: Can't init metadata layer FID infrastructure, "
 		       "rc = %d\n", sbi->ll_md_exp->exp_obd->obd_name, err);
-		GOTO(out_md, err);
+		goto out_md;
 	}
 
 	/* For mount, we only need fs info from MDT0, and also in DNE, it
@@ -278,7 +279,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 			cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 			OBD_STATFS_FOR_MDT0);
 	if (err)
-		GOTO(out_md_fid, err);
+		goto out_md_fid;
 
 	/* This needs to be after statfs to ensure connect has finished.
 	 * Note that "data" does NOT contain the valid connect reply.
@@ -292,7 +293,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	    valid != CLIENT_CONNECT_MDT_REQD) {
 		char *buf;
 
-		OBD_ALLOC_WAIT(buf, PAGE_CACHE_SIZE);
+		buf = kzalloc(PAGE_CACHE_SIZE, GFP_KERNEL);
 		obd_connect_flags2str(buf, PAGE_CACHE_SIZE,
 				      valid ^ CLIENT_CONNECT_MDT_REQD, ",");
 		LCONSOLE_ERROR_MSG(0x170, "Server %s does not support "
@@ -301,7 +302,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 				   "server or downgrade client.\n",
 				   sbi->ll_md_exp->exp_obd->obd_name, buf);
 		OBD_FREE(buf, PAGE_CACHE_SIZE);
-		GOTO(out_md_fid, err = -EPROTO);
+		err = -EPROTO;
+		goto out_md_fid;
 	}
 
 	size = sizeof(*data);
@@ -310,7 +312,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (err) {
 		CERROR("%s: Get connect data failed: rc = %d\n",
 		       sbi->ll_md_exp->exp_obd->obd_name, err);
-		GOTO(out_md_fid, err);
+		goto out_md_fid;
 	}
 
 	LASSERT(osfs->os_bsize);
@@ -391,7 +393,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	obd = class_name2obd(dt);
 	if (!obd) {
 		CERROR("DT %s: not setup or attached\n", dt);
-		GOTO(out_md_fid, err = -ENODEV);
+		err = -ENODEV;
+		goto out_md_fid;
 	}
 
 	data->ocd_connect_flags = OBD_CONNECT_GRANT     | OBD_CONNECT_VERSION  |
@@ -442,11 +445,11 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 				   "recovery, of which this client is not a "
 				   "part.  Please wait for recovery to "
 				   "complete, abort, or time out.\n", dt);
-		GOTO(out_md, err);
+		goto out_md;
 	} else if (err) {
 		CERROR("%s: Cannot connect to %s: rc = %d\n",
 		       sbi->ll_dt_exp->exp_obd->obd_name, dt, err);
-		GOTO(out_md, err);
+		goto out_md;
 	}
 
 	sbi->ll_dt_exp->exp_connect_data = *data;
@@ -456,7 +459,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (err) {
 		CERROR("%s: Can't init data layer FID infrastructure, "
 		       "rc = %d\n", sbi->ll_dt_exp->exp_obd->obd_name, err);
-		GOTO(out_dt, err);
+		goto out_dt;
 	}
 
 	mutex_lock(&sbi->ll_lco.lco_lock);
@@ -469,13 +472,14 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	err = md_getstatus(sbi->ll_md_exp, &sbi->ll_root_fid, &oc);
 	if (err) {
 		CERROR("cannot mds_connect: rc = %d\n", err);
-		GOTO(out_lock_cn_cb, err);
+		goto out_lock_cn_cb;
 	}
 	if (!fid_is_sane(&sbi->ll_root_fid)) {
 		CERROR("%s: Invalid root fid "DFID" during mount\n",
 		       sbi->ll_md_exp->exp_obd->obd_name,
 		       PFID(&sbi->ll_root_fid));
-		GOTO(out_lock_cn_cb, err = -EINVAL);
+		err = -EINVAL;
+		goto out_lock_cn_cb;
 	}
 	CDEBUG(D_SUPER, "rootfid "DFID"\n", PFID(&sbi->ll_root_fid));
 
@@ -492,9 +496,11 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	else if (sbi->ll_flags & LL_SBI_ACL)
 		valid |= OBD_MD_FLACL;
 
-	OBD_ALLOC_PTR(op_data);
-	if (op_data == NULL)
-		GOTO(out_lock_cn_cb, err = -ENOMEM);
+	op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
+	if (!op_data) {
+		err = -ENOMEM;
+		goto out_lock_cn_cb;
+	}
 
 	op_data->op_fid1 = sbi->ll_root_fid;
 	op_data->op_mode = 0;
@@ -508,7 +514,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (err) {
 		CERROR("%s: md_getattr failed for root: rc = %d\n",
 		       sbi->ll_md_exp->exp_obd->obd_name, err);
-		GOTO(out_lock_cn_cb, err);
+		goto out_lock_cn_cb;
 	}
 
 	err = md_get_lustre_md(sbi->ll_md_exp, request, sbi->ll_dt_exp,
@@ -516,7 +522,7 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (err) {
 		CERROR("failed to understand root inode md: rc = %d\n", err);
 		ptlrpc_req_finished(request);
-		GOTO(out_lock_cn_cb, err);
+		goto out_lock_cn_cb;
 	}
 
 	LASSERT(fid_is_sane(&sbi->ll_root_fid));
@@ -538,13 +544,13 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 		err = IS_ERR(root) ? PTR_ERR(root) : -EBADF;
 		root = NULL;
 		CERROR("lustre_lite: bad iget4 for root\n");
-		GOTO(out_root, err);
+		goto out_root;
 	}
 
 	err = ll_close_thread_start(&sbi->ll_lcq);
 	if (err) {
 		CERROR("cannot start close thread: rc %d\n", err);
-		GOTO(out_root, err);
+		goto out_root;
 	}
 
 #ifdef CONFIG_FS_POSIX_ACL
@@ -568,7 +574,8 @@ static int client_common_fill_super(struct super_block *sb, char *md, char *dt,
 	if (sb->s_root == NULL) {
 		CERROR("%s: can't make root dentry\n",
 			ll_get_fsname(sb, NULL, 0));
-		GOTO(out_root, err = -ENOMEM);
+		err = -ENOMEM;
+		goto out_lock_cn_cb;
 	}
 
 	sbi->ll_sdev_orig = sb->s_dev;
@@ -761,9 +768,9 @@ void ll_kill_super(struct super_block *sb)
 static inline int ll_set_opt(const char *opt, char *data, int fl)
 {
 	if (strncmp(opt, data, strlen(opt)) != 0)
-		return(0);
+		return 0;
 	else
-		return(fl);
+		return fl;
 }
 
 /* non-client-specific mount options are parsed in lmd_parse */
@@ -986,8 +993,8 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 
 	CDEBUG(D_VFSTRACE, "VFS Op: sb %p\n", sb);
 
-	OBD_ALLOC_PTR(cfg);
-	if (cfg == NULL)
+	cfg = kzalloc(sizeof(*cfg), GFP_NOFS);
+	if (!cfg)
 		return -ENOMEM;
 
 	try_module_get(THIS_MODULE);
@@ -1002,16 +1009,16 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 
 	err = ll_options(lsi->lsi_lmd->lmd_opts, &sbi->ll_flags);
 	if (err)
-		GOTO(out_free, err);
+		goto out_free;
 
 	err = bdi_init(&lsi->lsi_bdi);
 	if (err)
-		GOTO(out_free, err);
+		goto out_free;
 	lsi->lsi_flags |= LSI_BDI_INITIALIZED;
 	lsi->lsi_bdi.capabilities = BDI_CAP_MAP_COPY;
 	err = ll_bdi_register(&lsi->lsi_bdi);
 	if (err)
-		GOTO(out_free, err);
+		goto out_free;
 
 	sb->s_bdi = &lsi->lsi_bdi;
 	/* kernel >= 2.6.38 store dentry operations in sb->s_d_op. */
@@ -1027,7 +1034,7 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 	err = lustre_process_log(sb, profilenm, cfg);
 	if (err < 0) {
 		CERROR("Unable to process log: %d\n", err);
-		GOTO(out_free, err);
+		goto out_free;
 	}
 
 	/* Profile set with LCFG_MOUNTOPT so we can find our mdc and osc obds */
@@ -1036,19 +1043,24 @@ int ll_fill_super(struct super_block *sb, struct vfsmount *mnt)
 		LCONSOLE_ERROR_MSG(0x156, "The client profile '%s' could not be"
 				   " read from the MGS.  Does that filesystem "
 				   "exist?\n", profilenm);
-		GOTO(out_free, err = -EINVAL);
+		err = -EINVAL;
+		goto out_free;
 	}
 	CDEBUG(D_CONFIG, "Found profile %s: mdc=%s osc=%s\n", profilenm,
 	       lprof->lp_md, lprof->lp_dt);
 
-	OBD_ALLOC(dt, strlen(lprof->lp_dt) + instlen + 2);
-	if (!dt)
-		GOTO(out_free, err = -ENOMEM);
+	dt = kzalloc(strlen(lprof->lp_dt) + instlen + 2, GFP_NOFS);
+	if (!dt) {
+		err = -ENOMEM;
+		goto out_free;
+	}
 	sprintf(dt, "%s-%p", lprof->lp_dt, cfg->cfg_instance);
 
-	OBD_ALLOC(md, strlen(lprof->lp_md) + instlen + 2);
-	if (!md)
-		GOTO(out_free, err = -ENOMEM);
+	md = kzalloc(strlen(lprof->lp_md) + instlen + 2, GFP_NOFS);
+	if (!md) {
+		err = -ENOMEM;
+		goto out_free;
+	}
 	sprintf(md, "%s-%p", lprof->lp_md, cfg->cfg_instance);
 
 	/* connections, registrations, sb setup */
@@ -1139,7 +1151,7 @@ struct inode *ll_inode_from_resource_lock(struct ldlm_lock *lock)
 	/* NOTE: we depend on atomic igrab() -bzzz */
 	lock_res_and_lock(lock);
 	if (lock->l_resource->lr_lvb_inode) {
-		struct ll_inode_info * lli;
+		struct ll_inode_info *lli;
 		lli = ll_i2info(lock->l_resource->lr_lvb_inode);
 		if (lli->lli_inode_magic == LLI_INODE_MAGIC) {
 			inode = igrab(lock->l_resource->lr_lvb_inode);
@@ -1225,7 +1237,7 @@ void ll_clear_inode(struct inode *inode)
 	lli->lli_has_smd = false;
 }
 
-int ll_md_setattr(struct dentry *dentry, struct md_op_data *op_data,
+static int ll_md_setattr(struct dentry *dentry, struct md_op_data *op_data,
 		  struct md_open_data **mod)
 {
 	struct lustre_md md;
@@ -1377,7 +1389,7 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 		 * OST maximum object size and number of stripes.  This
 		 * needs another check in addition to the VFS check above. */
 		if (attr->ia_size > ll_file_maxbytes(inode)) {
-			CDEBUG(D_INODE,"file "DFID" too large %llu > %llu\n",
+			CDEBUG(D_INODE, "file "DFID" too large %llu > %llu\n",
 			       PFID(&lli->lli_fid), attr->ia_size,
 			       ll_file_maxbytes(inode));
 			return -EFBIG;
@@ -1425,8 +1437,8 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 	/* We always do an MDS RPC, even if we're only changing the size;
 	 * only the MDS knows whether truncate() should fail with -ETXTBUSY */
 
-	OBD_ALLOC_PTR(op_data);
-	if (op_data == NULL)
+	op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
+	if (!op_data)
 		return -ENOMEM;
 
 	if (!S_ISDIR(inode->i_mode)) {
@@ -1466,14 +1478,15 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 
 	rc = ll_md_setattr(dentry, op_data, &mod);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* truncate failed (only when non HSM import), others succeed */
 	if (file_is_released) {
 		if ((attr->ia_valid & ATTR_SIZE) && !hsm_import)
-			GOTO(out, rc = -ENODATA);
+			rc = -ENODATA;
 		else
-			GOTO(out, rc = 0);
+			rc = 0;
+		goto out;
 	}
 
 	/* RPC to MDT is sent, cancel data modification flag */
@@ -1484,8 +1497,10 @@ int ll_setattr_raw(struct dentry *dentry, struct iattr *attr, bool hsm_import)
 	}
 
 	ll_ioepoch_open(lli, op_data->op_ioepoch);
-	if (!S_ISREG(inode->i_mode))
-		GOTO(out, rc = 0);
+	if (!S_ISREG(inode->i_mode)) {
+		rc = 0;
+		goto out;
+	}
 
 	if (attr->ia_valid & (ATTR_SIZE |
 			      ATTR_ATIME | ATTR_ATIME_SET |
@@ -1568,7 +1583,8 @@ int ll_statfs_internal(struct super_block *sb, struct obd_statfs *osfs,
 	osfs->os_type = sb->s_magic;
 
 	CDEBUG(D_SUPER, "MDC blocks %llu/%llu objects %llu/%llu\n",
-	       osfs->os_bavail, osfs->os_blocks, osfs->os_ffree,osfs->os_files);
+	       osfs->os_bavail, osfs->os_blocks, osfs->os_ffree,
+	       osfs->os_files);
 
 	if (sbi->ll_flags & LL_SBI_LAZYSTATFS)
 		flags |= OBD_STATFS_NODELAY;
@@ -1885,7 +1901,7 @@ int ll_iocontrol(struct inode *inode, struct file *file,
 	struct ptlrpc_request *req = NULL;
 	int rc, flags = 0;
 
-	switch(cmd) {
+	switch (cmd) {
 	case FSFILT_IOC_GETFLAGS: {
 		struct mdt_body *body;
 		struct md_op_data *op_data;
@@ -2013,7 +2029,7 @@ void ll_umount_begin(struct super_block *sb)
 	}
 	obd->obd_force = 1;
 
-	OBD_ALLOC_PTR(ioc_data);
+	ioc_data = kzalloc(sizeof(*ioc_data), GFP_NOFS);
 	if (ioc_data) {
 		obd_iocontrol(IOC_OSC_SET_ACTIVE, sbi->ll_md_exp,
 			      sizeof(*ioc_data), ioc_data, NULL);
@@ -2101,7 +2117,7 @@ int ll_prep_inode(struct inode **inode, struct ptlrpc_request *req,
 			rc = IS_ERR(*inode) ? PTR_ERR(*inode) : -ENOMEM;
 			*inode = NULL;
 			CERROR("new_inode -fatal: rc %d\n", rc);
-			GOTO(out, rc);
+			goto out;
 		}
 	}
 
@@ -2150,36 +2166,44 @@ int ll_obd_statfs(struct inode *inode, void *arg)
 	__u32 flags;
 	int len = 0, rc;
 
-	if (!inode || !(sbi = ll_i2sbi(inode)))
-		GOTO(out_statfs, rc = -EINVAL);
+	if (!inode || !(sbi = ll_i2sbi(inode))) {
+		rc = -EINVAL;
+		goto out_statfs;
+	}
 
 	rc = obd_ioctl_getdata(&buf, &len, arg);
 	if (rc)
-		GOTO(out_statfs, rc);
+		goto out_statfs;
 
-	data = (void*)buf;
+	data = (void *)buf;
 	if (!data->ioc_inlbuf1 || !data->ioc_inlbuf2 ||
-	    !data->ioc_pbuf1 || !data->ioc_pbuf2)
-		GOTO(out_statfs, rc = -EINVAL);
+	    !data->ioc_pbuf1 || !data->ioc_pbuf2) {
+		rc = -EINVAL;
+		goto out_statfs;
+	}
 
 	if (data->ioc_inllen1 != sizeof(__u32) ||
 	    data->ioc_inllen2 != sizeof(__u32) ||
 	    data->ioc_plen1 != sizeof(struct obd_statfs) ||
-	    data->ioc_plen2 != sizeof(struct obd_uuid))
-		GOTO(out_statfs, rc = -EINVAL);
+	    data->ioc_plen2 != sizeof(struct obd_uuid)) {
+		rc = -EINVAL;
+		goto out_statfs;
+	}
 
 	memcpy(&type, data->ioc_inlbuf1, sizeof(__u32));
 	if (type & LL_STATFS_LMV)
 		exp = sbi->ll_md_exp;
 	else if (type & LL_STATFS_LOV)
 		exp = sbi->ll_dt_exp;
-	else
-		GOTO(out_statfs, rc = -ENODEV);
+	else {
+		rc = -ENODEV;
+		goto out_statfs;
+	}
 
 	flags = (type & LL_STATFS_NODELAY) ? OBD_STATFS_NODELAY : 0;
 	rc = obd_iocontrol(IOC_OBD_STATFS, exp, len, buf, &flags);
 	if (rc)
-		GOTO(out_statfs, rc);
+		goto out_statfs;
 out_statfs:
 	if (buf)
 		obd_ioctl_freedata(buf, len);
@@ -2212,11 +2236,11 @@ int ll_process_config(struct lustre_cfg *lcfg)
 				      lcfg, sb);
 	if (rc > 0)
 		rc = 0;
-	return(rc);
+	return rc;
 }
 
 /* this function prepares md_op_data hint for passing ot down to MD stack. */
-struct md_op_data * ll_prep_md_op_data(struct md_op_data *op_data,
+struct md_op_data *ll_prep_md_op_data(struct md_op_data *op_data,
 				       struct inode *i1, struct inode *i2,
 				       const char *name, int namelen,
 				       int mode, __u32 opc, void *data)
@@ -2227,7 +2251,7 @@ struct md_op_data * ll_prep_md_op_data(struct md_op_data *op_data,
 		return ERR_PTR(-ENAMETOOLONG);
 
 	if (op_data == NULL)
-		OBD_ALLOC_PTR(op_data);
+		op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
 
 	if (op_data == NULL)
 		return ERR_PTR(-ENOMEM);
diff --git a/drivers/staging/lustre/lustre/llite/llite_mmap.c b/drivers/staging/lustre/lustre/llite/llite_mmap.c
index 7dae610f5c86..ae605a6d9dc2 100644
--- a/drivers/staging/lustre/lustre/llite/llite_mmap.c
+++ b/drivers/staging/lustre/lustre/llite/llite_mmap.c
@@ -71,7 +71,7 @@ struct vm_area_struct *our_vma(struct mm_struct *mm, unsigned long addr,
 	/* mmap_sem must have been held by caller. */
 	LASSERT(!down_write_trylock(&mm->mmap_sem));
 
-	for(vma = find_vma(mm, addr);
+	for (vma = find_vma(mm, addr);
 	    vma != NULL && vma->vm_start < (addr + count); vma = vma->vm_next) {
 		if (vma->vm_ops && vma->vm_ops == &ll_file_vm_ops &&
 		    vma->vm_flags & VM_SHARED) {
@@ -181,12 +181,14 @@ static int ll_page_mkwrite0(struct vm_area_struct *vma, struct page *vmpage,
 	LASSERT(vmpage != NULL);
 
 	io = ll_fault_io_init(vma, &env,  &nest, vmpage->index, NULL);
-	if (IS_ERR(io))
-		GOTO(out, result = PTR_ERR(io));
+	if (IS_ERR(io)) {
+		result = PTR_ERR(io);
+		goto out;
+	}
 
 	result = io->ci_result;
 	if (result < 0)
-		GOTO(out_io, result);
+		goto out_io;
 
 	io->u.ci_fault.ft_mkwrite = 1;
 	io->u.ci_fault.ft_writable = 1;
@@ -261,7 +263,7 @@ out:
 
 static inline int to_fault_error(int result)
 {
-	switch(result) {
+	switch (result) {
 	case 0:
 		result = VM_FAULT_LOCKED;
 		break;
@@ -310,10 +312,16 @@ static int ll_fault0(struct vm_area_struct *vma, struct vm_fault *vmf)
 		vio->u.fault.ft_vma       = vma;
 		vio->u.fault.ft_vmpage    = NULL;
 		vio->u.fault.fault.ft_vmf = vmf;
+		vio->u.fault.fault.ft_flags = 0;
+		vio->u.fault.fault.ft_flags_valid = 0;
 
 		result = cl_io_loop(env, io);
 
-		fault_ret = vio->u.fault.fault.ft_flags;
+		/* ft_flags are only valid if we reached
+		 * the call to filemap_fault */
+		if (vio->u.fault.fault.ft_flags_valid)
+			fault_ret = vio->u.fault.fault.ft_flags;
+
 		vmpage = vio->u.fault.ft_vmpage;
 		if (result != 0 && vmpage != NULL) {
 			page_cache_release(vmpage);
@@ -393,7 +401,7 @@ static int ll_page_mkwrite(struct vm_area_struct *vma, struct vm_fault *vmf)
 		}
 	} while (retry);
 
-	switch(result) {
+	switch (result) {
 	case 0:
 		LASSERT(PageLocked(vmf->page));
 		result = VM_FAULT_LOCKED;
@@ -420,7 +428,7 @@ static int ll_page_mkwrite(struct vm_area_struct *vma, struct vm_fault *vmf)
  *  To avoid cancel the locks covering mmapped region for lock cache pressure,
  *  we track the mapped vma count in ccc_object::cob_mmap_cnt.
  */
-static void ll_vm_open(struct vm_area_struct * vma)
+static void ll_vm_open(struct vm_area_struct *vma)
 {
 	struct inode *inode    = vma->vm_file->f_dentry->d_inode;
 	struct ccc_object *vob = cl_inode2ccc(inode);
@@ -466,7 +474,7 @@ static const struct vm_operations_struct ll_file_vm_ops = {
 	.close			= ll_vm_close,
 };
 
-int ll_file_mmap(struct file *file, struct vm_area_struct * vma)
+int ll_file_mmap(struct file *file, struct vm_area_struct *vma)
 {
 	struct inode *inode = file->f_dentry->d_inode;
 	int rc;
diff --git a/drivers/staging/lustre/lustre/llite/llite_nfs.c b/drivers/staging/lustre/lustre/llite/llite_nfs.c
index 8fdd6e093d1a..ae3a12ab7fa1 100644
--- a/drivers/staging/lustre/lustre/llite/llite_nfs.c
+++ b/drivers/staging/lustre/lustre/llite/llite_nfs.c
@@ -106,8 +106,8 @@ struct inode *search_inode_for_lustre(struct super_block *sb,
 
 	/* Because inode is NULL, ll_prep_md_op_data can not
 	 * be used here. So we allocate op_data ourselves */
-	OBD_ALLOC_PTR(op_data);
-	if (op_data == NULL)
+	op_data = kzalloc(sizeof(*op_data), GFP_NOFS);
+	if (!op_data)
 		return ERR_PTR(-ENOMEM);
 
 	op_data->op_fid1 = *fid;
@@ -236,11 +236,15 @@ static int ll_get_name(struct dentry *dentry, char *name,
 		.ctx.actor = ll_nfs_get_name_filldir,
 	};
 
-	if (!dir || !S_ISDIR(dir->i_mode))
-		GOTO(out, rc = -ENOTDIR);
+	if (!dir || !S_ISDIR(dir->i_mode)) {
+		rc = -ENOTDIR;
+		goto out;
+	}
 
-	if (!dir->i_fop)
-		GOTO(out, rc = -EINVAL);
+	if (!dir->i_fop) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	mutex_lock(&dir->i_mutex);
 	rc = ll_dir_read(dir, &lgd.ctx);
diff --git a/drivers/staging/lustre/lustre/llite/llite_rmtacl.c b/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
index be0c3eff108c..586f49a374ec 100644
--- a/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
+++ b/drivers/staging/lustre/lustre/llite/llite_rmtacl.c
@@ -58,7 +58,7 @@ static inline __u32 ee_hashfunc(uid_t id)
 	return id & (EE_HASHES - 1);
 }
 
-obd_valid rce_ops2valid(int ops)
+u64 rce_ops2valid(int ops)
 {
 	switch (ops) {
 	case RMT_LSETFACL:
@@ -78,7 +78,7 @@ static struct rmtacl_ctl_entry *rce_alloc(pid_t key, int ops)
 {
 	struct rmtacl_ctl_entry *rce;
 
-	OBD_ALLOC_PTR(rce);
+	rce = kzalloc(sizeof(*rce), GFP_NOFS);
 	if (!rce)
 		return NULL;
 
@@ -184,7 +184,7 @@ static struct eacl_entry *ee_alloc(pid_t key, struct lu_fid *fid, int type,
 {
 	struct eacl_entry *ee;
 
-	OBD_ALLOC_PTR(ee);
+	ee = kzalloc(sizeof(*ee), GFP_NOFS);
 	if (!ee)
 		return NULL;
 
diff --git a/drivers/staging/lustre/lustre/llite/lloop.c b/drivers/staging/lustre/lustre/llite/lloop.c
index 808663898b73..264e5ec3fed6 100644
--- a/drivers/staging/lustre/lustre/llite/lloop.c
+++ b/drivers/staging/lustre/lustre/llite/lloop.c
@@ -126,7 +126,7 @@ struct lloop_device {
 	struct block_device *lo_device;
 	unsigned	     lo_blocksize;
 
-	int		  old_gfp_mask;
+	gfp_t		  old_gfp_mask;
 
 	spinlock_t		lo_lock;
 	struct bio		*lo_bio;
@@ -192,7 +192,7 @@ static int do_bio_lustrebacked(struct lloop_device *lo, struct bio *head)
 	pgoff_t	       offset;
 	int		   ret;
 	int		   rw;
-	obd_count	     page_count = 0;
+	u32		   page_count = 0;
 	struct bio_vec       bvec;
 	struct bvec_iter   iter;
 	struct bio	   *bio;
@@ -409,8 +409,10 @@ static int loop_thread(void *data)
 	lo->lo_state = LLOOP_BOUND;
 
 	env = cl_env_get(&refcheck);
-	if (IS_ERR(env))
-		GOTO(out, ret = PTR_ERR(env));
+	if (IS_ERR(env)) {
+		ret = PTR_ERR(env);
+		goto out;
+	}
 
 	lo->lo_env = env;
 	memset(&lo->lo_pvec, 0, sizeof(lo->lo_pvec));
@@ -546,7 +548,7 @@ static int loop_clr_fd(struct lloop_device *lo, struct block_device *bdev,
 		       int count)
 {
 	struct file *filp = lo->lo_backing_file;
-	int gfp = lo->old_gfp_mask;
+	gfp_t gfp = lo->old_gfp_mask;
 
 	if (lo->lo_state != LLOOP_BOUND)
 		return -ENXIO;
@@ -670,8 +672,10 @@ static enum llioc_iter lloop_ioctl(struct inode *unused, struct file *file,
 	if (magic != ll_iocontrol_magic)
 		return LLIOC_CONT;
 
-	if (disks == NULL)
-		GOTO(out1, err = -ENODEV);
+	if (disks == NULL) {
+		err = -ENODEV;
+		goto out1;
+	}
 
 	CWARN("Enter llop_ioctl\n");
 
@@ -692,19 +696,25 @@ static enum llioc_iter lloop_ioctl(struct inode *unused, struct file *file,
 			    file->f_dentry->d_inode)
 				break;
 		}
-		if (lo || !lo_free)
-			GOTO(out, err = -EBUSY);
+		if (lo || !lo_free) {
+			err = -EBUSY;
+			goto out;
+		}
 
 		lo = lo_free;
 		dev = MKDEV(lloop_major, lo->lo_number);
 
 		/* quit if the used pointer is writable */
-		if (put_user((long)old_encode_dev(dev), (long*)arg))
-			GOTO(out, err = -EFAULT);
+		if (put_user((long)old_encode_dev(dev), (long *)arg)) {
+			err = -EFAULT;
+			goto out;
+		}
 
 		bdev = blkdev_get_by_dev(dev, file->f_mode, NULL);
-		if (IS_ERR(bdev))
-			GOTO(out, err = PTR_ERR(bdev));
+		if (IS_ERR(bdev)) {
+			err = PTR_ERR(bdev);
+			goto out;
+		}
 
 		get_file(file);
 		err = loop_set_fd(lo, NULL, bdev, file);
@@ -720,16 +730,22 @@ static enum llioc_iter lloop_ioctl(struct inode *unused, struct file *file,
 		int minor;
 
 		dev = old_decode_dev(arg);
-		if (MAJOR(dev) != lloop_major)
-			GOTO(out, err = -EINVAL);
+		if (MAJOR(dev) != lloop_major) {
+			err = -EINVAL;
+			goto out;
+		}
 
 		minor = MINOR(dev);
-		if (minor > max_loop - 1)
-			GOTO(out, err = -EINVAL);
+		if (minor > max_loop - 1) {
+			err = -EINVAL;
+			goto out;
+		}
 
 		lo = &loop_dev[minor];
-		if (lo->lo_state != LLOOP_BOUND)
-			GOTO(out, err = -EINVAL);
+		if (lo->lo_state != LLOOP_BOUND) {
+			err = -EINVAL;
+			goto out;
+		}
 
 		bdev = lo->lo_device;
 		err = loop_clr_fd(lo, bdev, 1);
@@ -777,11 +793,11 @@ static int __init lloop_init(void)
 	if (ll_iocontrol_magic == NULL)
 		goto out_mem1;
 
-	OBD_ALLOC_WAIT(loop_dev, max_loop * sizeof(*loop_dev));
+	loop_dev = kzalloc(max_loop * sizeof(*loop_dev), GFP_KERNEL);
 	if (!loop_dev)
 		goto out_mem1;
 
-	OBD_ALLOC_WAIT(disks, max_loop * sizeof(*disks));
+	disks = kzalloc(max_loop * sizeof(*disks), GFP_KERNEL);
 	if (!disks)
 		goto out_mem2;
 
diff --git a/drivers/staging/lustre/lustre/llite/lproc_llite.c b/drivers/staging/lustre/lustre/llite/lproc_llite.c
index 77f68b507fea..3b3df9f03422 100644
--- a/drivers/staging/lustre/lustre/llite/lproc_llite.c
+++ b/drivers/staging/lustre/lustre/llite/lproc_llite.c
@@ -410,7 +410,8 @@ static ssize_t ll_max_cached_mb_seq_write(struct file *file,
 	/* easy - add more LRU slots. */
 	if (diff >= 0) {
 		atomic_add(diff, &cache->ccc_lru_left);
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 
 	diff = -diff;
@@ -997,8 +998,10 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 	/* File operations stats */
 	sbi->ll_stats = lprocfs_alloc_stats(LPROC_LL_FILE_OPCODES,
 					    LPROCFS_STATS_FLAG_NONE);
-	if (sbi->ll_stats == NULL)
-		GOTO(out, err = -ENOMEM);
+	if (sbi->ll_stats == NULL) {
+		err = -ENOMEM;
+		goto out;
+	}
 	/* do counter init */
 	for (id = 0; id < LPROC_LL_FILE_OPCODES; id++) {
 		__u32 type = llite_opcode_table[id].type;
@@ -1016,12 +1019,14 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 	}
 	err = lprocfs_register_stats(sbi->ll_proc_root, "stats", sbi->ll_stats);
 	if (err)
-		GOTO(out, err);
+		goto out;
 
 	sbi->ll_ra_stats = lprocfs_alloc_stats(ARRAY_SIZE(ra_stat_string),
 					       LPROCFS_STATS_FLAG_NONE);
-	if (sbi->ll_ra_stats == NULL)
-		GOTO(out, err = -ENOMEM);
+	if (sbi->ll_ra_stats == NULL) {
+		err = -ENOMEM;
+		goto out;
+	}
 
 	for (id = 0; id < ARRAY_SIZE(ra_stat_string); id++)
 		lprocfs_counter_init(sbi->ll_ra_stats, id, 0,
@@ -1029,12 +1034,12 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 	err = lprocfs_register_stats(sbi->ll_proc_root, "read_ahead_stats",
 				     sbi->ll_ra_stats);
 	if (err)
-		GOTO(out, err);
+		goto out;
 
 
 	err = lprocfs_add_vars(sbi->ll_proc_root, lprocfs_llite_obd_vars, sb);
 	if (err)
-		GOTO(out, err);
+		goto out;
 
 	/* MDC info */
 	obd = class_name2obd(mdc);
@@ -1044,20 +1049,22 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 	LASSERT(obd->obd_type->typ_name != NULL);
 
 	dir = proc_mkdir(obd->obd_type->typ_name, sbi->ll_proc_root);
-	if (dir == NULL)
-		GOTO(out, err = -ENOMEM);
+	if (dir == NULL) {
+		err = -ENOMEM;
+		goto out;
+	}
 
 	snprintf(name, MAX_STRING_SIZE, "common_name");
 	lvars[0].fops = &llite_name_fops;
 	err = lprocfs_add_vars(dir, lvars, obd);
 	if (err)
-		GOTO(out, err);
+		goto out;
 
 	snprintf(name, MAX_STRING_SIZE, "uuid");
 	lvars[0].fops = &llite_uuid_fops;
 	err = lprocfs_add_vars(dir, lvars, obd);
 	if (err)
-		GOTO(out, err);
+		goto out;
 
 	/* OSC */
 	obd = class_name2obd(osc);
@@ -1067,14 +1074,16 @@ int lprocfs_register_mountpoint(struct proc_dir_entry *parent,
 	LASSERT(obd->obd_type->typ_name != NULL);
 
 	dir = proc_mkdir(obd->obd_type->typ_name, sbi->ll_proc_root);
-	if (dir == NULL)
-		GOTO(out, err = -ENOMEM);
+	if (dir == NULL) {
+		err = -ENOMEM;
+		goto out;
+	}
 
 	snprintf(name, MAX_STRING_SIZE, "common_name");
 	lvars[0].fops = &llite_name_fops;
 	err = lprocfs_add_vars(dir, lvars, obd);
 	if (err)
-		GOTO(out, err);
+		goto out;
 
 	snprintf(name, MAX_STRING_SIZE, "uuid");
 	lvars[0].fops = &llite_uuid_fops;
@@ -1098,7 +1107,7 @@ void lprocfs_unregister_mountpoint(struct ll_sb_info *sbi)
 }
 #undef MAX_STRING_SIZE
 
-#define pct(a,b) (b ? a * 100 / b : 0)
+#define pct(a, b) (b ? a * 100 / b : 0)
 
 static void ll_display_extents_info(struct ll_rw_extents_info *io_extents,
 				   struct seq_file *seq, int which)
@@ -1113,12 +1122,12 @@ static void ll_display_extents_info(struct ll_rw_extents_info *io_extents,
 	write_cum = 0;
 	start = 0;
 
-	for(i = 0; i < LL_HIST_MAX; i++) {
+	for (i = 0; i < LL_HIST_MAX; i++) {
 		read_tot += pp_info->pp_r_hist.oh_buckets[i];
 		write_tot += pp_info->pp_w_hist.oh_buckets[i];
 	}
 
-	for(i = 0; i < LL_HIST_MAX; i++) {
+	for (i = 0; i < LL_HIST_MAX; i++) {
 		r = pp_info->pp_r_hist.oh_buckets[i];
 		w = pp_info->pp_w_hist.oh_buckets[i];
 		read_cum += r;
@@ -1305,15 +1314,15 @@ void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
 	int *process_count = &sbi->ll_offset_process_count;
 	struct ll_rw_extents_info *io_extents = &sbi->ll_rw_extents_info;
 
-	if(!sbi->ll_rw_stats_on)
+	if (!sbi->ll_rw_stats_on)
 		return;
 	process = sbi->ll_rw_process_info;
 	offset = sbi->ll_rw_offset_info;
 
 	spin_lock(&sbi->ll_pp_extent_lock);
 	/* Extent statistics */
-	for(i = 0; i < LL_PROCESS_HIST_MAX; i++) {
-		if(io_extents->pp_extents[i].pid == pid) {
+	for (i = 0; i < LL_PROCESS_HIST_MAX; i++) {
+		if (io_extents->pp_extents[i].pid == pid) {
 			cur = i;
 			break;
 		}
@@ -1376,9 +1385,9 @@ void ll_rw_stats_tally(struct ll_sb_info *sbi, pid_t pid,
 				process[i].rw_offset = pos -
 					process[i].rw_last_file_pos;
 			}
-			if(process[i].rw_smallest_extent > count)
+			if (process[i].rw_smallest_extent > count)
 				process[i].rw_smallest_extent = count;
-			if(process[i].rw_largest_extent < count)
+			if (process[i].rw_largest_extent < count)
 				process[i].rw_largest_extent = count;
 			process[i].rw_last_file_pos = pos + count;
 			spin_unlock(&sbi->ll_process_lock);
@@ -1421,7 +1430,7 @@ static int ll_rw_offset_stats_seq_show(struct seq_file *seq, void *v)
 		   "R/W", "PID", "RANGE START", "RANGE END",
 		   "SMALLEST EXTENT", "LARGEST EXTENT", "OFFSET");
 	/* We stored the discontiguous offsets here; print them first */
-	for(i = 0; i < LL_OFFSET_HIST_MAX; i++) {
+	for (i = 0; i < LL_OFFSET_HIST_MAX; i++) {
 		if (offset[i].rw_pid != 0)
 			seq_printf(seq,
 				   "%3c %10d %14Lu %14Lu %17lu %17lu %14Lu",
@@ -1434,7 +1443,7 @@ static int ll_rw_offset_stats_seq_show(struct seq_file *seq, void *v)
 				   offset[i].rw_offset);
 	}
 	/* Then print the current offsets for each process */
-	for(i = 0; i < LL_PROCESS_HIST_MAX; i++) {
+	for (i = 0; i < LL_PROCESS_HIST_MAX; i++) {
 		if (process[i].rw_pid != 0)
 			seq_printf(seq,
 				   "%3c %10d %14Lu %14Lu %17lu %17lu %14Lu",
diff --git a/drivers/staging/lustre/lustre/llite/namei.c b/drivers/staging/lustre/lustre/llite/namei.c
index 0dc7173bbd41..7a68c1e027e0 100644
--- a/drivers/staging/lustre/lustre/llite/namei.c
+++ b/drivers/staging/lustre/lustre/llite/namei.c
@@ -523,8 +523,10 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 		rc = ll_statahead_enter(parent, &dentry, 0);
 		if (rc == 1) {
 			if (dentry == save)
-				GOTO(out, retval = NULL);
-			GOTO(out, retval = dentry);
+				retval = NULL;
+			else
+				retval = dentry;
+			goto out;
 		}
 	}
 
@@ -546,13 +548,16 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	rc = md_intent_lock(ll_i2mdexp(parent), op_data, NULL, 0, it,
 			    lookup_flags, &req, ll_md_blocking_ast, 0);
 	ll_finish_md_op_data(op_data);
-	if (rc < 0)
-		GOTO(out, retval = ERR_PTR(rc));
+	if (rc < 0) {
+		retval = ERR_PTR(rc);
+		goto out;
+	}
 
 	rc = ll_lookup_it_finish(req, it, parent, &dentry);
 	if (rc != 0) {
 		ll_intent_release(it);
-		GOTO(out, retval = ERR_PTR(rc));
+		retval = ERR_PTR(rc);
+		goto out;
 	}
 
 	if ((it->it_op & IT_OPEN) && dentry->d_inode &&
@@ -563,9 +568,10 @@ static struct dentry *ll_lookup_it(struct inode *parent, struct dentry *dentry,
 	ll_lookup_finish_locks(it, dentry);
 
 	if (dentry == save)
-		GOTO(out, retval = NULL);
+		retval = NULL;
 	else
-		GOTO(out, retval = dentry);
+		retval = dentry;
+	goto out;
  out:
 	if (req)
 		ptlrpc_req_finished(req);
@@ -618,7 +624,7 @@ static int ll_atomic_open(struct inode *dir, struct dentry *dentry,
 	       dentry->d_name.len, dentry->d_name.name, dir->i_ino,
 	       dir->i_generation, dir, file, open_flags, mode, *opened);
 
-	OBD_ALLOC(it, sizeof(*it));
+	it = kzalloc(sizeof(*it), GFP_NOFS);
 	if (!it)
 		return -ENOMEM;
 
@@ -697,8 +703,10 @@ static struct inode *ll_create_node(struct inode *dir, struct lookup_intent *it)
 	request = it->d.lustre.it_data;
 	it_clear_disposition(it, DISP_ENQ_CREATE_REF);
 	rc = ll_prep_inode(&inode, request, dir->i_sb, it);
-	if (rc)
-		GOTO(out, inode = ERR_PTR(rc));
+	if (rc) {
+		inode = ERR_PTR(rc);
+		goto out;
+	}
 
 	LASSERT(ll_d_hlist_empty(&inode->i_dentry));
 
@@ -783,8 +791,10 @@ static int ll_new_node(struct inode *dir, struct qstr *name,
 
 	op_data = ll_prep_md_op_data(NULL, dir, NULL, name->name,
 				     name->len, 0, opc, NULL);
-	if (IS_ERR(op_data))
-		GOTO(err_exit, err = PTR_ERR(op_data));
+	if (IS_ERR(op_data)) {
+		err = PTR_ERR(op_data);
+		goto err_exit;
+	}
 
 	err = md_create(sbi->ll_md_exp, op_data, tgt, tgt_len, mode,
 			from_kuid(&init_user_ns, current_fsuid()),
@@ -792,14 +802,14 @@ static int ll_new_node(struct inode *dir, struct qstr *name,
 			cfs_curproc_cap_pack(), rdev, &request);
 	ll_finish_md_op_data(op_data);
 	if (err)
-		GOTO(err_exit, err);
+		goto err_exit;
 
 	ll_update_times(request, dir);
 
 	if (dchild) {
 		err = ll_prep_inode(&inode, request, dchild->d_sb, NULL);
 		if (err)
-		     GOTO(err_exit, err);
+			goto err_exit;
 
 		d_instantiate(dchild, inode);
 	}
@@ -907,7 +917,7 @@ static int ll_link_generic(struct inode *src,  struct inode *dir,
 	err = md_link(sbi->ll_md_exp, op_data, &request);
 	ll_finish_md_op_data(op_data);
 	if (err)
-		GOTO(out, err);
+		goto out;
 
 	ll_update_times(request, dir);
 	ll_stats_ops_tally(sbi, LPROC_LL_LINK, 1);
@@ -1028,7 +1038,8 @@ int ll_objects_destroy(struct ptlrpc_request *request, struct inode *dir)
 
 	if (body->eadatasize == 0) {
 		CERROR("OBD_MD_FLEASIZE set but eadatasize zero\n");
-		GOTO(out, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out;
 	}
 
 	/* The MDS sent back the EA because we unlinked the last reference
@@ -1042,13 +1053,15 @@ int ll_objects_destroy(struct ptlrpc_request *request, struct inode *dir)
 	rc = obd_unpackmd(ll_i2dtexp(dir), &lsm, eadata, body->eadatasize);
 	if (rc < 0) {
 		CERROR("obd_unpackmd: %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 	LASSERT(rc >= sizeof(*lsm));
 
 	OBDO_ALLOC(oa);
-	if (oa == NULL)
-		GOTO(out_free_memmd, rc = -ENOMEM);
+	if (oa == NULL) {
+		rc = -ENOMEM;
+		goto out_free_memmd;
+	}
 
 	oa->o_oi = lsm->lsm_oi;
 	oa->o_mode = body->mode & S_IFMT;
@@ -1070,7 +1083,7 @@ int ll_objects_destroy(struct ptlrpc_request *request, struct inode *dir)
 	if (body->valid & OBD_MD_FLOSSCAPA) {
 		rc = md_unpack_capa(ll_i2mdexp(dir), request, &RMF_CAPA2, &oc);
 		if (rc)
-			GOTO(out_free_memmd, rc);
+			goto out_free_memmd;
 	}
 
 	rc = obd_destroy(NULL, ll_i2dtexp(dir), oa, lsm, &oti,
@@ -1116,7 +1129,7 @@ static int ll_unlink_generic(struct inode *dir, struct dentry *dparent,
 	rc = md_unlink(ll_i2sbi(dir)->ll_md_exp, op_data, &request);
 	ll_finish_md_op_data(op_data);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	ll_update_times(request, dir);
 	ll_stats_ops_tally(ll_i2sbi(dir), LPROC_LL_UNLINK, 1);
@@ -1137,7 +1150,8 @@ static int ll_rename_generic(struct inode *src, struct dentry *src_dparent,
 	struct md_op_data *op_data;
 	int err;
 
-	CDEBUG(D_VFSTRACE,"VFS Op:oldname=%.*s,src_dir=%lu/%u(%p),newname=%.*s,"
+	CDEBUG(D_VFSTRACE,
+	       "VFS Op:oldname=%.*s,src_dir=%lu/%u(%p),newname=%.*s,"
 	       "tgt_dir=%lu/%u(%p)\n", src_name->len, src_name->name,
 	       src->i_ino, src->i_generation, src, tgt_name->len,
 	       tgt_name->name, tgt->i_ino, tgt->i_generation, tgt);
diff --git a/drivers/staging/lustre/lustre/llite/remote_perm.c b/drivers/staging/lustre/lustre/llite/remote_perm.c
index f61fefc9baf0..c05a9126cfe3 100644
--- a/drivers/staging/lustre/lustre/llite/remote_perm.c
+++ b/drivers/staging/lustre/lustre/llite/remote_perm.c
@@ -100,7 +100,7 @@ void free_rmtperm_hash(struct hlist_head *hash)
 	struct ll_remote_perm *lrp;
 	struct hlist_node *next;
 
-	if(!hash)
+	if (!hash)
 		return;
 
 	for (i = 0; i < REMOTE_PERM_HASHSIZE; i++)
@@ -144,8 +144,10 @@ static int do_check_remote_perm(struct ll_inode_info *lli, int mask)
 		break;
 	}
 
-	if (!found)
-		GOTO(out, rc = -ENOENT);
+	if (!found) {
+		rc = -ENOENT;
+		goto out;
+	}
 
 	CDEBUG(D_SEC, "found remote perm: %u/%u/%u/%u - %#x\n",
 	       lrp->lrp_uid, lrp->lrp_gid, lrp->lrp_fsuid, lrp->lrp_fsgid,
diff --git a/drivers/staging/lustre/lustre/llite/rw.c b/drivers/staging/lustre/lustre/llite/rw.c
index ecd7a229cf9b..1f53b9863385 100644
--- a/drivers/staging/lustre/lustre/llite/rw.c
+++ b/drivers/staging/lustre/lustre/llite/rw.c
@@ -317,8 +317,10 @@ static unsigned long ll_ra_count_get(struct ll_sb_info *sbi,
 	 * otherwise it will form small read RPC(< 1M), which hurt server
 	 * performance a lot. */
 	ret = min(ra->ra_max_pages - atomic_read(&ra->ra_cur_pages), pages);
-	if (ret < 0 || ret < min_t(long, PTLRPC_MAX_BRW_PAGES, pages))
-		GOTO(out, ret = 0);
+	if (ret < 0 || ret < min_t(long, PTLRPC_MAX_BRW_PAGES, pages)) {
+		ret = 0;
+		goto out;
+	}
 
 	/* If the non-strided (ria_pages == 0) readahead window
 	 * (ria_start + ret) has grown across an RPC boundary, then trim
@@ -1018,7 +1020,7 @@ void ras_update(struct ll_sb_info *sbi, struct inode *inode,
 			ras->ras_next_readahead = 0;
 			ras->ras_window_len = min(ra->ra_max_pages_per_file,
 				ra->ra_max_read_ahead_whole_pages);
-			GOTO(out_unlock, 0);
+			goto out_unlock;
 		}
 	}
 	if (zero) {
@@ -1033,7 +1035,7 @@ void ras_update(struct ll_sb_info *sbi, struct inode *inode,
 			}
 			ras_reset(inode, ras, index);
 			ras->ras_consecutive_pages++;
-			GOTO(out_unlock, 0);
+			goto out_unlock;
 		} else {
 			ras->ras_consecutive_pages = 0;
 			ras->ras_consecutive_requests = 0;
@@ -1058,7 +1060,7 @@ void ras_update(struct ll_sb_info *sbi, struct inode *inode,
 				ras_reset(inode, ras, index);
 				ras->ras_consecutive_pages++;
 				ras_stride_reset(ras);
-				GOTO(out_unlock, 0);
+				goto out_unlock;
 			}
 		} else if (stride_io_mode(ras)) {
 			/* If this is contiguous read but in stride I/O mode
@@ -1090,7 +1092,7 @@ void ras_update(struct ll_sb_info *sbi, struct inode *inode,
 	 * is not incremented and thus can't be used to trigger RA */
 	if (!ras->ras_window_len && ras->ras_consecutive_pages == 4) {
 		ras->ras_window_len = RAS_INCREASE_STEP(inode);
-		GOTO(out_unlock, 0);
+		goto out_unlock;
 	}
 
 	/* Initially reset the stride window offset to next_readahead*/
@@ -1136,8 +1138,10 @@ int ll_writepage(struct page *vmpage, struct writeback_control *wbc)
 	LASSERT(ll_i2dtexp(inode) != NULL);
 
 	env = cl_env_nested_get(&nest);
-	if (IS_ERR(env))
-		GOTO(out, result = PTR_ERR(env));
+	if (IS_ERR(env)) {
+		result = PTR_ERR(env);
+		goto out;
+	}
 
 	clob  = ll_i2info(inode)->lli_clob;
 	LASSERT(clob != NULL);
@@ -1197,7 +1201,7 @@ int ll_writepage(struct page *vmpage, struct writeback_control *wbc)
 	}
 
 	cl_env_nested_put(&nest, env);
-	GOTO(out, result);
+	goto out;
 
 out:
 	if (result < 0) {
diff --git a/drivers/staging/lustre/lustre/llite/rw26.c b/drivers/staging/lustre/lustre/llite/rw26.c
index 3f157e76a15c..4c77ae8b9350 100644
--- a/drivers/staging/lustre/lustre/llite/rw26.c
+++ b/drivers/staging/lustre/lustre/llite/rw26.c
@@ -436,12 +436,12 @@ static ssize_t ll_direct_IO_26(int rw, struct kiocb *iocb,
 				size = ((((size / 2) - 1) |
 					 ~CFS_PAGE_MASK) + 1) &
 					CFS_PAGE_MASK;
-				CDEBUG(D_VFSTRACE,"DIO size now %lu\n",
+				CDEBUG(D_VFSTRACE, "DIO size now %lu\n",
 				       size);
 				continue;
 			}
 
-			GOTO(out, result);
+			goto out;
 		}
 		iov_iter_advance(iter, result);
 		tot_bytes += result;
diff --git a/drivers/staging/lustre/lustre/llite/statahead.c b/drivers/staging/lustre/lustre/llite/statahead.c
index c39cf8d47d6e..06b71bcf97a7 100644
--- a/drivers/staging/lustre/lustre/llite/statahead.c
+++ b/drivers/staging/lustre/lustre/llite/statahead.c
@@ -202,8 +202,8 @@ ll_sa_entry_alloc(struct ll_statahead_info *sai, __u64 index,
 	char		 *dname;
 
 	entry_size = sizeof(struct ll_sa_entry) + (len & ~3) + 4;
-	OBD_ALLOC(entry, entry_size);
-	if (unlikely(entry == NULL))
+	entry = kzalloc(entry_size, GFP_NOFS);
+	if (unlikely(!entry))
 		return ERR_PTR(-ENOMEM);
 
 	CDEBUG(D_READA, "alloc sa entry %.*s(%p) index %llu\n",
@@ -465,7 +465,7 @@ static struct ll_statahead_info *ll_sai_alloc(void)
 	struct ll_statahead_info *sai;
 	int		       i;
 
-	OBD_ALLOC_PTR(sai);
+	sai = kzalloc(sizeof(*sai), GFP_NOFS);
 	if (!sai)
 		return NULL;
 
@@ -528,7 +528,7 @@ static void ll_sai_put(struct ll_statahead_info *sai)
 		spin_unlock(&lli->lli_sa_lock);
 
 		if (sai->sai_sent > sai->sai_replied)
-			CDEBUG(D_READA,"statahead for dir "DFID
+			CDEBUG(D_READA, "statahead for dir "DFID
 			      " does not finish: [sent:%llu] [replied:%llu]\n",
 			      PFID(&lli->lli_fid),
 			      sai->sai_sent, sai->sai_replied);
@@ -637,8 +637,10 @@ static void ll_post_statahead(struct ll_statahead_info *sai)
 	it = &minfo->mi_it;
 	req = entry->se_req;
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
-	if (body == NULL)
-		GOTO(out, rc = -EFAULT);
+	if (body == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	child = entry->se_inode;
 	if (child == NULL) {
@@ -649,8 +651,10 @@ static void ll_post_statahead(struct ll_statahead_info *sai)
 
 		/* XXX: No fid in reply, this is probably cross-ref case.
 		 * SA can't handle it yet. */
-		if (body->valid & OBD_MD_MDS)
-			GOTO(out, rc = -EAGAIN);
+		if (body->valid & OBD_MD_MDS) {
+			rc = -EAGAIN;
+			goto out;
+		}
 	} else {
 		/*
 		 * revalidate.
@@ -665,12 +669,14 @@ static void ll_post_statahead(struct ll_statahead_info *sai)
 
 	it->d.lustre.it_lock_handle = entry->se_handle;
 	rc = md_revalidate_lock(ll_i2mdexp(dir), it, ll_inode2fid(dir), NULL);
-	if (rc != 1)
-		GOTO(out, rc = -EAGAIN);
+	if (rc != 1) {
+		rc = -EAGAIN;
+		goto out;
+	}
 
 	rc = ll_prep_inode(&child, req, dir->i_sb, it);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	CDEBUG(D_DLMTRACE, "setting l_data to inode %p (%lu/%u)\n",
 	       child, child->i_ino, child->i_generation);
@@ -711,20 +717,23 @@ static int ll_statahead_interpret(struct ptlrpc_request *req,
 	if (unlikely(lli->lli_sai == NULL ||
 		     lli->lli_sai->sai_generation != minfo->mi_generation)) {
 		spin_unlock(&lli->lli_sa_lock);
-		GOTO(out, rc = -ESTALE);
+		rc = -ESTALE;
+		goto out;
 	} else {
 		sai = ll_sai_get(lli->lli_sai);
 		if (unlikely(!thread_is_running(&sai->sai_thread))) {
 			sai->sai_replied++;
 			spin_unlock(&lli->lli_sa_lock);
-			GOTO(out, rc = -EBADFD);
+			rc = -EBADFD;
+			goto out;
 		}
 
 		entry = ll_sa_entry_get_byindex(sai, minfo->mi_cbdata);
 		if (entry == NULL) {
 			sai->sai_replied++;
 			spin_unlock(&lli->lli_sa_lock);
-			GOTO(out, rc = -EIDRM);
+			rc = -EIDRM;
+			goto out;
 		}
 
 		if (rc != 0) {
@@ -793,12 +802,12 @@ static int sa_args_init(struct inode *dir, struct inode *child,
 	struct ldlm_enqueue_info *einfo;
 	struct md_op_data	*op_data;
 
-	OBD_ALLOC_PTR(einfo);
-	if (einfo == NULL)
+	einfo = kzalloc(sizeof(*einfo), GFP_NOFS);
+	if (!einfo)
 		return -ENOMEM;
 
-	OBD_ALLOC_PTR(minfo);
-	if (minfo == NULL) {
+	minfo = kzalloc(sizeof(*minfo), GFP_NOFS);
+	if (!minfo) {
 		OBD_FREE_PTR(einfo);
 		return -ENOMEM;
 	}
@@ -878,7 +887,8 @@ static int do_sa_revalidate(struct inode *dir, struct ll_sa_entry *entry,
 		return 1;
 
 	entry->se_inode = igrab(inode);
-	rc = md_revalidate_lock(ll_i2mdexp(dir), &it, ll_inode2fid(inode),NULL);
+	rc = md_revalidate_lock(ll_i2mdexp(dir), &it, ll_inode2fid(inode),
+				NULL);
 	if (rc == 1) {
 		entry->se_handle = it.d.lustre.it_lock_handle;
 		ll_intent_release(&it);
@@ -1084,7 +1094,7 @@ static int ll_statahead_thread(void *arg)
 			CDEBUG(D_READA, "error reading dir "DFID" at %llu/%llu: [rc %d] [parent %u]\n",
 			       PFID(ll_inode2fid(dir)), pos, sai->sai_index,
 			       rc, plli->lli_opendir_pid);
-			GOTO(out, rc);
+			goto out;
 		}
 
 		dp = page_address(page);
@@ -1149,7 +1159,8 @@ interpret_it:
 
 			if (unlikely(!thread_is_running(thread))) {
 				ll_release_page(page, 0);
-				GOTO(out, rc = 0);
+				rc = 0;
+				goto out;
 			}
 
 			/* If no window for metadata statahead, but there are
@@ -1170,7 +1181,8 @@ interpret_it:
 					if (unlikely(
 						!thread_is_running(thread))) {
 						ll_release_page(page, 0);
-						GOTO(out, rc = 0);
+						rc = 0;
+						goto out;
 					}
 
 					if (!sa_sent_full(sai))
@@ -1202,8 +1214,10 @@ do_it:
 				while (!sa_received_empty(sai))
 					ll_post_statahead(sai);
 
-				if (unlikely(!thread_is_running(thread)))
-					GOTO(out, rc = 0);
+				if (unlikely(!thread_is_running(thread))) {
+					rc = 0;
+					goto out;
+				}
 
 				if (sai->sai_sent == sai->sai_replied &&
 				    sa_received_empty(sai))
@@ -1221,7 +1235,8 @@ do_it:
 			}
 			spin_unlock(&plli->lli_agl_lock);
 
-			GOTO(out, rc = 0);
+			rc = 0;
+			goto out;
 		} else if (1) {
 			/*
 			 * chain is exhausted.
@@ -1421,7 +1436,7 @@ static int is_first_dirent(struct inode *dir, struct dentry *dentry)
 				rc = LS_FIRST_DOT_DE;
 
 			ll_release_page(page, 0);
-			GOTO(out, rc);
+			goto out;
 		}
 		pos = le64_to_cpu(dp->ldp_hash_end);
 		if (pos == MDS_DIR_END_OFF) {
@@ -1625,20 +1640,25 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 
 	/* I am the "lli_opendir_pid" owner, only me can set "lli_sai". */
 	rc = is_first_dirent(dir, *dentryp);
-	if (rc == LS_NONE_FIRST_DE)
+	if (rc == LS_NONE_FIRST_DE) {
 		/* It is not "ls -{a}l" operation, no need statahead for it. */
-		GOTO(out, rc = -EAGAIN);
+		rc = -EAGAIN;
+		goto out;
+	}
 
 	sai = ll_sai_alloc();
-	if (sai == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (sai == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	sai->sai_ls_all = (rc == LS_FIRST_DOT_DE);
 	sai->sai_inode = igrab(dir);
 	if (unlikely(sai->sai_inode == NULL)) {
 		CWARN("Do not start stat ahead on dying inode "DFID"\n",
 		      PFID(&lli->lli_fid));
-		GOTO(out, rc = -ESTALE);
+		rc = -ESTALE;
+		goto out;
 	}
 
 	/* get parent reference count here, and put it in ll_statahead_thread */
@@ -1652,7 +1672,8 @@ int do_statahead_enter(struct inode *dir, struct dentry **dentryp,
 		      PFID(&lli->lli_fid), PFID(&nlli->lli_fid));
 		dput(parent);
 		iput(sai->sai_inode);
-		GOTO(out, rc = -EAGAIN);
+		rc = -EAGAIN;
+		goto out;
 	}
 
 	CDEBUG(D_READA, "start statahead thread: sai %p, parent %.*s\n",
diff --git a/drivers/staging/lustre/lustre/llite/super25.c b/drivers/staging/lustre/lustre/llite/super25.c
index 078c0e95f4cf..e61dbed120a3 100644
--- a/drivers/staging/lustre/lustre/llite/super25.c
+++ b/drivers/staging/lustre/lustre/llite/super25.c
@@ -88,8 +88,7 @@ static void ll_destroy_inodecache(void)
 }
 
 /* exported operations */
-struct super_operations lustre_super_operations =
-{
+struct super_operations lustre_super_operations = {
 	.alloc_inode   = ll_alloc_inode,
 	.destroy_inode = ll_destroy_inode,
 	.evict_inode   = ll_delete_inode,
diff --git a/drivers/staging/lustre/lustre/llite/symlink.c b/drivers/staging/lustre/lustre/llite/symlink.c
index 20e678b31c7b..eccd3a717a4d 100644
--- a/drivers/staging/lustre/lustre/llite/symlink.c
+++ b/drivers/staging/lustre/lustre/llite/symlink.c
@@ -77,21 +77,23 @@ static int ll_readlink_internal(struct inode *inode,
 	if (rc) {
 		if (rc != -ENOENT)
 			CERROR("inode %lu: rc = %d\n", inode->i_ino, rc);
-		GOTO (failed, rc);
+		goto failed;
 	}
 
 	body = req_capsule_server_get(&(*request)->rq_pill, &RMF_MDT_BODY);
 	LASSERT(body != NULL);
 	if ((body->valid & OBD_MD_LINKNAME) == 0) {
 		CERROR("OBD_MD_LINKNAME not set on reply\n");
-		GOTO(failed, rc = -EPROTO);
+		rc = -EPROTO;
+		goto failed;
 	}
 
 	LASSERT(symlen != 0);
 	if (body->eadatasize != symlen) {
 		CERROR("inode %lu: symlink length %d not expected %d\n",
 			inode->i_ino, body->eadatasize - 1, symlen - 1);
-		GOTO(failed, rc = -EPROTO);
+		rc = -EPROTO;
+		goto failed;
 	}
 
 	*symname = req_capsule_server_get(&(*request)->rq_pill, &RMF_MDT_MD);
@@ -100,10 +102,11 @@ static int ll_readlink_internal(struct inode *inode,
 		/* not full/NULL terminated */
 		CERROR("inode %lu: symlink not NULL terminated string"
 			"of length %d\n", inode->i_ino, symlen - 1);
-		GOTO(failed, rc = -EPROTO);
+		rc = -EPROTO;
+		goto failed;
 	}
 
-	OBD_ALLOC(lli->lli_symlink_name, symlen);
+	lli->lli_symlink_name = kzalloc(symlen, GFP_NOFS);
 	/* do not return an error if we cannot cache the symlink locally */
 	if (lli->lli_symlink_name) {
 		memcpy(lli->lli_symlink_name, *symname, symlen);
diff --git a/drivers/staging/lustre/lustre/llite/vvp_dev.c b/drivers/staging/lustre/lustre/llite/vvp_dev.c
index 0f2e79d6b7f8..5a1078a4198d 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_dev.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_dev.c
@@ -394,7 +394,7 @@ static loff_t vvp_pgcache_find(const struct lu_env *env,
 		seq_printf(seq, "%s"#flag, has_flags ? "|" : "");       \
 		has_flags = 1;					  \
 	}							       \
-} while(0)
+} while (0)
 
 static void vvp_pgcache_page_show(const struct lu_env *env,
 				  struct seq_file *seq, struct cl_page *page)
@@ -405,7 +405,7 @@ static void vvp_pgcache_page_show(const struct lu_env *env,
 
 	cpg = cl2ccc_page(cl_page_at(page, &vvp_device_type));
 	vmpage = cpg->cpg_page;
-	seq_printf(seq," %5i | %p %p %s %s %s %s | %p %lu/%u(%p) %lu %u [",
+	seq_printf(seq, " %5i | %p %p %s %s %s %s | %p %lu/%u(%p) %lu %u [",
 		   0 /* gen */,
 		   cpg, page,
 		   "none",
diff --git a/drivers/staging/lustre/lustre/llite/vvp_io.c b/drivers/staging/lustre/lustre/llite/vvp_io.c
index a4117d6a3866..d3f967a78138 100644
--- a/drivers/staging/lustre/lustre/llite/vvp_io.c
+++ b/drivers/staging/lustre/lustre/llite/vvp_io.c
@@ -238,7 +238,7 @@ static int vvp_mmap_locks(const struct lu_env *env,
 		addr &= CFS_PAGE_MASK;
 
 		down_read(&mm->mmap_sem);
-		while((vma = our_vma(mm, addr, count)) != NULL) {
+		while ((vma = our_vma(mm, addr, count)) != NULL) {
 			struct inode *inode = vma->vm_file->f_dentry->d_inode;
 			int flags = CEF_MUST;
 
@@ -615,6 +615,7 @@ static int vvp_io_kernel_fault(struct vvp_fault_io *cfio)
 	struct vm_fault *vmf = cfio->fault.ft_vmf;
 
 	cfio->fault.ft_flags = filemap_fault(cfio->ft_vma, vmf);
+	cfio->fault.ft_flags_valid = 1;
 
 	if (vmf->page) {
 		CDEBUG(D_PAGE,
@@ -703,7 +704,8 @@ static int vvp_io_fault_start(const struct lu_env *env,
 
 		/* return +1 to stop cl_io_loop() and ll_fault() will catch
 		 * and retry. */
-		GOTO(out, result = +1);
+		result = +1;
+		goto out;
 	}
 
 
@@ -720,7 +722,7 @@ static int vvp_io_fault_start(const struct lu_env *env,
 			CDEBUG(D_PAGE,
 				"llite: mkwrite and truncate race happened: "
 				"%p: 0x%lx 0x%lx\n",
-				vmpage->mapping,fio->ft_index,last_index);
+				vmpage->mapping, fio->ft_index, last_index);
 			/*
 			 * We need to return if we are
 			 * passed the end of the file. This will propagate
@@ -732,13 +734,16 @@ static int vvp_io_fault_start(const struct lu_env *env,
 			 * in ll_page_mkwrite0. Thus we return -ENODATA
 			 * to handle both cases
 			 */
-			GOTO(out, result = -ENODATA);
+			result = -ENODATA;
+			goto out;
 		}
 	}
 
 	page = cl_page_find(env, obj, fio->ft_index, vmpage, CPT_CACHEABLE);
-	if (IS_ERR(page))
-		GOTO(out, result = PTR_ERR(page));
+	if (IS_ERR(page)) {
+		result = PTR_ERR(page);
+		goto out;
+	}
 
 	/* if page is going to be written, we should add this page into cache
 	 * earlier. */
@@ -770,7 +775,7 @@ static int vvp_io_fault_start(const struct lu_env *env,
 				/* we're in big trouble, what can we do now? */
 				if (result == -EDQUOT)
 					result = -ENOSPC;
-				GOTO(out, result);
+				goto out;
 			} else
 				cl_page_disown(env, io, page);
 		}
diff --git a/drivers/staging/lustre/lustre/llite/xattr.c b/drivers/staging/lustre/lustre/llite/xattr.c
index 665ca572027f..252a6194ed9b 100644
--- a/drivers/staging/lustre/lustre/llite/xattr.c
+++ b/drivers/staging/lustre/lustre/llite/xattr.c
@@ -364,7 +364,7 @@ do_getxattr:
 		if (rc == -EAGAIN)
 			goto getxattr_nocache;
 		if (rc < 0)
-			GOTO(out_xattr, rc);
+			goto out_xattr;
 
 		/* Add "system.posix_acl_access" to the list */
 		if (lli->lli_posix_acl != NULL && valid & OBD_MD_FLXATTRLS) {
@@ -375,7 +375,8 @@ do_getxattr:
 				       sizeof(XATTR_NAME_ACL_ACCESS));
 				rc += sizeof(XATTR_NAME_ACL_ACCESS);
 			} else {
-				GOTO(out_xattr, rc = -ERANGE);
+				rc = -ERANGE;
+				goto out_xattr;
 			}
 		}
 	} else {
@@ -387,29 +388,36 @@ getxattr_nocache:
 		capa_put(oc);
 
 		if (rc < 0)
-			GOTO(out_xattr, rc);
+			goto out_xattr;
 
 		body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
 		LASSERT(body);
 
 		/* only detect the xattr size */
-		if (size == 0)
-			GOTO(out, rc = body->eadatasize);
+		if (size == 0) {
+			rc = body->eadatasize;
+			goto out;
+		}
 
 		if (size < body->eadatasize) {
 			CERROR("server bug: replied size %u > %u\n",
 				body->eadatasize, (int)size);
-			GOTO(out, rc = -ERANGE);
+			rc = -ERANGE;
+			goto out;
 		}
 
-		if (body->eadatasize == 0)
-			GOTO(out, rc = -ENODATA);
+		if (body->eadatasize == 0) {
+			rc = -ENODATA;
+			goto out;
+		}
 
 		/* do not need swab xattr data */
 		xdata = req_capsule_server_sized_get(&req->rq_pill, &RMF_EADATA,
 							body->eadatasize);
-		if (!xdata)
-			GOTO(out, rc = -EFAULT);
+		if (!xdata) {
+			rc = -EFAULT;
+			goto out;
+		}
 
 		memcpy(buffer, xdata, body->eadatasize);
 		rc = body->eadatasize;
@@ -421,14 +429,16 @@ getxattr_nocache:
 
 		acl = lustre_posix_acl_xattr_2ext(
 					(posix_acl_xattr_header *)buffer, rc);
-		if (IS_ERR(acl))
-			GOTO(out, rc = PTR_ERR(acl));
+		if (IS_ERR(acl)) {
+			rc = PTR_ERR(acl);
+			goto out;
+		}
 
 		rc = ee_add(&sbi->ll_et, current_pid(), ll_inode2fid(inode),
 			    xattr_type, acl);
 		if (unlikely(rc < 0)) {
 			lustre_ext_acl_xattr_free(acl);
-			GOTO(out, rc);
+			goto out;
 		}
 	}
 #endif
@@ -476,7 +486,8 @@ ssize_t ll_getxattr(struct dentry *dentry, const char *name,
 		if (size == 0 && S_ISDIR(inode->i_mode)) {
 			/* XXX directory EA is fix for now, optimize to save
 			 * RPC transfer */
-			GOTO(out, rc = sizeof(struct lov_user_md));
+			rc = sizeof(struct lov_user_md);
+			goto out;
 		}
 
 		lsm = ccc_inode_lsm_get(inode);
@@ -496,7 +507,7 @@ ssize_t ll_getxattr(struct dentry *dentry, const char *name,
 		ccc_inode_lsm_put(inode, lsm);
 
 		if (rc < 0)
-		       GOTO(out, rc);
+			goto out;
 
 		if (size == 0) {
 			/* used to call ll_get_max_mdsize() forward to get
@@ -504,13 +515,14 @@ ssize_t ll_getxattr(struct dentry *dentry, const char *name,
 			 * rsync 3.0.x) care much about the exact xattr value
 			 * size */
 			rc = lmmsize;
-			GOTO(out, rc);
+			goto out;
 		}
 
 		if (size < lmmsize) {
 			CERROR("server bug: replied size %d > %d for %s (%s)\n",
 			       lmmsize, (int)size, dentry->d_name.name, name);
-			GOTO(out, rc = -ERANGE);
+			rc = -ERANGE;
+			goto out;
 		}
 
 		lump = (struct lov_user_md *)buffer;
@@ -526,7 +538,7 @@ out:
 			ptlrpc_req_finished(request);
 		else if (lmm)
 			obd_free_diskmd(ll_i2dtexp(inode), &lmm);
-		return(rc);
+		return rc;
 	}
 
 	return ll_getxattr_common(inode, name, buffer, size, OBD_MD_FLXATTR);
@@ -549,7 +561,7 @@ ssize_t ll_listxattr(struct dentry *dentry, char *buffer, size_t size)
 
 	rc = ll_getxattr_common(inode, NULL, buffer, size, OBD_MD_FLXATTRLS);
 	if (rc < 0)
-		GOTO(out, rc);
+		goto out;
 
 	if (buffer != NULL) {
 		struct ll_sb_info *sbi = ll_i2sbi(inode);
@@ -582,7 +594,8 @@ ssize_t ll_listxattr(struct dentry *dentry, char *buffer, size_t size)
 	}
 
 	if (rc2 < 0) {
-		GOTO(out, rc2 = 0);
+		rc2 = 0;
+		goto out;
 	} else if (S_ISREG(inode->i_mode) || S_ISDIR(inode->i_mode)) {
 		const int prefix_len = sizeof(XATTR_LUSTRE_PREFIX) - 1;
 		const size_t name_len   = sizeof("lov") - 1;
diff --git a/drivers/staging/lustre/lustre/llite/xattr_cache.c b/drivers/staging/lustre/lustre/llite/xattr_cache.c
index edec945d2eb3..627cbe242f22 100644
--- a/drivers/staging/lustre/lustre/llite/xattr_cache.c
+++ b/drivers/staging/lustre/lustre/llite/xattr_cache.c
@@ -128,13 +128,13 @@ static int ll_xattr_cache_add(struct list_head *cache,
 
 	xattr->xe_namelen = strlen(xattr_name) + 1;
 
-	OBD_ALLOC(xattr->xe_name, xattr->xe_namelen);
+	xattr->xe_name = kzalloc(xattr->xe_namelen, GFP_NOFS);
 	if (!xattr->xe_name) {
 		CDEBUG(D_CACHE, "failed to alloc xattr name %u\n",
 		       xattr->xe_namelen);
 		goto err_name;
 	}
-	OBD_ALLOC(xattr->xe_value, xattr_val_len);
+	xattr->xe_value = kzalloc(xattr_val_len, GFP_NOFS);
 	if (!xattr->xe_value) {
 		CDEBUG(D_CACHE, "failed to alloc xattr value %d\n",
 		       xattr_val_len);
@@ -365,18 +365,20 @@ static int ll_xattr_cache_refill(struct inode *inode, struct lookup_intent *oit)
 
 	rc = ll_xattr_find_get_lock(inode, oit, &req);
 	if (rc)
-		GOTO(out_no_unlock, rc);
+		goto out_no_unlock;
 
 	/* Do we have the data at this point? */
 	if (ll_xattr_cache_valid(lli)) {
 		ll_stats_ops_tally(sbi, LPROC_LL_GETXATTR_HITS, 1);
-		GOTO(out_maybe_drop, rc = 0);
+		rc = 0;
+		goto out_maybe_drop;
 	}
 
 	/* Matched but no cache? Cancelled on error by a parallel refill. */
 	if (unlikely(req == NULL)) {
 		CDEBUG(D_CACHE, "cancelled by a parallel getxattr\n");
-		GOTO(out_maybe_drop, rc = -EIO);
+		rc = -EIO;
+		goto out_maybe_drop;
 	}
 
 	if (oit->d.lustre.it_status < 0) {
@@ -386,13 +388,14 @@ static int ll_xattr_cache_refill(struct inode *inode, struct lookup_intent *oit)
 		/* xattr data is so large that we don't want to cache it */
 		if (rc == -ERANGE)
 			rc = -EAGAIN;
-		GOTO(out_destroy, rc);
+		goto out_destroy;
 	}
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
 	if (body == NULL) {
 		CERROR("no MDT BODY in the refill xattr reply\n");
-		GOTO(out_destroy, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out_destroy;
 	}
 	/* do not need swab xattr data */
 	xdata = req_capsule_server_sized_get(&req->rq_pill, &RMF_EADATA,
@@ -403,7 +406,8 @@ static int ll_xattr_cache_refill(struct inode *inode, struct lookup_intent *oit)
 					      body->max_mdsize * sizeof(__u32));
 	if (xdata == NULL || xval == NULL || xsizes == NULL) {
 		CERROR("wrong setxattr reply\n");
-		GOTO(out_destroy, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out_destroy;
 	}
 
 	xtail = xdata + body->eadatasize;
@@ -435,7 +439,7 @@ static int ll_xattr_cache_refill(struct inode *inode, struct lookup_intent *oit)
 		}
 		if (rc < 0) {
 			ll_xattr_cache_destroy_locked(lli);
-			GOTO(out_destroy, rc);
+			goto out_destroy;
 		}
 		xdata += strlen(xdata) + 1;
 		xval  += *xsizes;
@@ -447,7 +451,7 @@ static int ll_xattr_cache_refill(struct inode *inode, struct lookup_intent *oit)
 
 	ll_set_lock_data(sbi->ll_md_exp, inode, oit, NULL);
 
-	GOTO(out_maybe_drop, rc);
+	goto out_maybe_drop;
 out_maybe_drop:
 
 		ll_intent_drop_lock(oit);
@@ -528,7 +532,7 @@ int ll_xattr_cache_get(struct inode *inode,
 					 size ? buffer : NULL, size);
 	}
 
-	GOTO(out, rc);
+	goto out;
 out:
 	up_read(&lli->lli_xattrs_list_rwsem);
 
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_fld.c b/drivers/staging/lustre/lustre/lmv/lmv_fld.c
index 8289bcc5f8e1..e8421f04beda 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_fld.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_fld.c
@@ -53,7 +53,7 @@
 
 int lmv_fld_lookup(struct lmv_obd *lmv,
 		   const struct lu_fid *fid,
-		   mdsno_t *mds)
+		   u32 *mds)
 {
 	int rc;
 
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_intent.c b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
index aba698f4489c..5106124b7d92 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_intent.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_intent.c
@@ -41,7 +41,7 @@
 #include <asm/div64.h>
 #include <linux/seq_file.h>
 #include <linux/namei.h>
-#include "../include/linux/lustre_intent.h"
+#include "../include/lustre_intent.h"
 #include "../include/obd_support.h"
 #include "../include/lustre/lustre_idl.h"
 #include "../include/lustre_lib.h"
@@ -94,12 +94,16 @@ static int lmv_intent_remote(struct obd_export *exp, void *lmm,
 	LASSERT(fid_is_sane(&body->fid1));
 
 	tgt = lmv_find_target(lmv, &body->fid1);
-	if (IS_ERR(tgt))
-		GOTO(out, rc = PTR_ERR(tgt));
+	if (IS_ERR(tgt)) {
+		rc = PTR_ERR(tgt);
+		goto out;
+	}
 
 	OBD_ALLOC_PTR(op_data);
-	if (op_data == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (op_data == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	op_data->op_fid1 = body->fid1;
 	/* Sent the parent FID to the remote MDT */
@@ -121,7 +125,7 @@ static int lmv_intent_remote(struct obd_export *exp, void *lmm,
 	rc = md_intent_lock(tgt->ltd_exp, op_data, lmm, lmmsize, it,
 			    flags, &req, cb_blocking, extra_lock_flags);
 	if (rc)
-		GOTO(out_free_op_data, rc);
+		goto out_free_op_data;
 
 	/*
 	 * LLite needs LOOKUP lock to track dentry revocation in order to
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_internal.h b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
index eb18a5900e13..b911e7643874 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_internal.h
+++ b/drivers/staging/lustre/lustre/lmv/lmv_internal.h
@@ -70,10 +70,8 @@ int lmv_intent_open(struct obd_export *exp, struct md_op_data *op_data,
 
 int lmv_blocking_ast(struct ldlm_lock *, struct ldlm_lock_desc *,
 		     void *, int);
-int lmv_fld_lookup(struct lmv_obd *lmv, const struct lu_fid *fid,
-		   mdsno_t *mds);
-int __lmv_fid_alloc(struct lmv_obd *lmv, struct lu_fid *fid,
-		    mdsno_t mds);
+int lmv_fld_lookup(struct lmv_obd *lmv, const struct lu_fid *fid, u32 *mds);
+int __lmv_fid_alloc(struct lmv_obd *lmv, struct lu_fid *fid, u32 mds);
 int lmv_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
 		  struct md_op_data *op_data);
 
@@ -95,7 +93,7 @@ static inline struct lmv_stripe_md *lmv_get_mea(struct ptlrpc_request *req)
 
 	if (mea->mea_count == 0)
 		return NULL;
-	if( mea->mea_magic != MEA_MAGIC_LAST_CHAR &&
+	if (mea->mea_magic != MEA_MAGIC_LAST_CHAR &&
 		mea->mea_magic != MEA_MAGIC_ALL_CHARS &&
 		mea->mea_magic != MEA_MAGIC_HASH_SEGMENT)
 		return NULL;
@@ -111,7 +109,7 @@ static inline int lmv_get_easize(struct lmv_obd *lmv)
 }
 
 static inline struct lmv_tgt_desc *
-lmv_get_target(struct lmv_obd *lmv, mdsno_t mds)
+lmv_get_target(struct lmv_obd *lmv, u32 mds)
 {
 	int count = lmv->desc.ld_tgt_count;
 	int i;
@@ -130,7 +128,7 @@ lmv_get_target(struct lmv_obd *lmv, mdsno_t mds)
 static inline struct lmv_tgt_desc *
 lmv_find_target(struct lmv_obd *lmv, const struct lu_fid *fid)
 {
-	mdsno_t mds = 0;
+	u32 mds = 0;
 	int rc;
 
 	if (lmv->desc.ld_tgt_count > 1) {
@@ -146,7 +144,7 @@ struct lmv_tgt_desc
 *lmv_locate_mds(struct lmv_obd *lmv, struct md_op_data *op_data,
 		struct lu_fid *fid);
 /* lproc_lmv.c */
-#if defined (CONFIG_PROC_FS)
+#if defined(CONFIG_PROC_FS)
 void lprocfs_lmv_init_vars(struct lprocfs_static_vars *lvars);
 #else
 static inline void lprocfs_lmv_init_vars(struct lprocfs_static_vars *lvars)
diff --git a/drivers/staging/lustre/lustre/lmv/lmv_obd.c b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
index a66b3e000d57..1a5821289c39 100644
--- a/drivers/staging/lustre/lustre/lmv/lmv_obd.c
+++ b/drivers/staging/lustre/lustre/lmv/lmv_obd.c
@@ -97,12 +97,16 @@ static int lmv_set_mdc_active(struct lmv_obd *lmv, struct obd_uuid *uuid,
 			break;
 	}
 
-	if (i == lmv->desc.ld_tgt_count)
-		GOTO(out_lmv_lock, rc = -EINVAL);
+	if (i == lmv->desc.ld_tgt_count) {
+		rc = -EINVAL;
+		goto out_lmv_lock;
+	}
 
 	obd = class_exp2obd(tgt->ltd_exp);
-	if (obd == NULL)
-		GOTO(out_lmv_lock, rc = -ENOTCONN);
+	if (obd == NULL) {
+		rc = -ENOTCONN;
+		goto out_lmv_lock;
+	}
 
 	CDEBUG(D_INFO, "Found OBD %s=%s device %d (%p) type %s at LMV idx %d\n",
 	       obd->obd_name, obd->obd_uuid.uuid, obd->obd_minor, obd,
@@ -112,7 +116,7 @@ static int lmv_set_mdc_active(struct lmv_obd *lmv, struct obd_uuid *uuid,
 	if (tgt->ltd_active == activate) {
 		CDEBUG(D_INFO, "OBD %p already %sactive!\n", obd,
 		       activate ? "" : "in");
-		GOTO(out_lmv_lock, rc);
+		goto out_lmv_lock;
 	}
 
 	CDEBUG(D_INFO, "Marking OBD %p %sactive\n", obd,
@@ -574,7 +578,7 @@ int lmv_check_connect(struct obd_device *obd)
 			continue;
 		rc = lmv_connect_mdc(obd, tgt);
 		if (rc)
-			GOTO(out_disc, rc);
+			goto out_disc;
 	}
 
 	lmv_set_timeouts(obd);
@@ -712,7 +716,7 @@ static int lmv_fid2path(struct obd_export *exp, int len, void *karg, void *uarg)
 repeat_fid2path:
 	rc = obd_iocontrol(OBD_IOC_FID2PATH, tgt->ltd_exp, len, gf, uarg);
 	if (rc != 0 && rc != -EREMOTE)
-		GOTO(out_fid2path, rc);
+		goto out_fid2path;
 
 	/* If remote_gf != NULL, it means just building the
 	 * path on the remote MDT, copy this path segment to gf */
@@ -722,8 +726,10 @@ repeat_fid2path:
 
 		ori_gf = (struct getinfo_fid2path *)karg;
 		if (strlen(ori_gf->gf_path) +
-		    strlen(gf->gf_path) > ori_gf->gf_pathlen)
-			GOTO(out_fid2path, rc = -EOVERFLOW);
+		    strlen(gf->gf_path) > ori_gf->gf_pathlen) {
+			rc = -EOVERFLOW;
+			goto out_fid2path;
+		}
 
 		ptr = ori_gf->gf_path;
 
@@ -741,14 +747,16 @@ repeat_fid2path:
 	       gf->gf_linkno);
 
 	if (rc == 0)
-		GOTO(out_fid2path, rc);
+		goto out_fid2path;
 
 	/* sigh, has to go to another MDT to do path building further */
 	if (remote_gf == NULL) {
 		remote_gf_size = sizeof(*remote_gf) + PATH_MAX;
 		OBD_ALLOC(remote_gf, remote_gf_size);
-		if (remote_gf == NULL)
-			GOTO(out_fid2path, rc = -ENOMEM);
+		if (remote_gf == NULL) {
+			rc = -ENOMEM;
+			goto out_fid2path;
+		}
 		remote_gf->gf_pathlen = PATH_MAX;
 	}
 
@@ -756,12 +764,15 @@ repeat_fid2path:
 		CERROR("%s: invalid FID "DFID": rc = %d\n",
 		       tgt->ltd_exp->exp_obd->obd_name,
 		       PFID(&gf->gf_fid), -EINVAL);
-		GOTO(out_fid2path, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out_fid2path;
 	}
 
 	tgt = lmv_find_target(lmv, &gf->gf_fid);
-	if (IS_ERR(tgt))
-		GOTO(out_fid2path, rc = -EINVAL);
+	if (IS_ERR(tgt)) {
+		rc = -EINVAL;
+		goto out_fid2path;
+	}
 
 	remote_gf->gf_fid = gf->gf_fid;
 	remote_gf->gf_recno = -1;
@@ -1178,7 +1189,7 @@ static int lmv_nid_policy(struct lmv_obd *lmv)
 }
 
 static int lmv_choose_mds(struct lmv_obd *lmv, struct md_op_data *op_data,
-			  placement_policy_t placement)
+			  enum placement_policy placement)
 {
 	switch (placement) {
 	case PLACEMENT_CHAR_POLICY:
@@ -1201,8 +1212,7 @@ static int lmv_choose_mds(struct lmv_obd *lmv, struct md_op_data *op_data,
  * This is _inode_ placement policy function (not name).
  */
 static int lmv_placement_policy(struct obd_device *obd,
-				struct md_op_data *op_data,
-				mdsno_t *mds)
+				struct md_op_data *op_data, u32 *mds)
 {
 	struct lmv_obd	  *lmv = &obd->u.lmv;
 
@@ -1241,8 +1251,7 @@ static int lmv_placement_policy(struct obd_device *obd,
 	return 0;
 }
 
-int __lmv_fid_alloc(struct lmv_obd *lmv, struct lu_fid *fid,
-		    mdsno_t mds)
+int __lmv_fid_alloc(struct lmv_obd *lmv, struct lu_fid *fid, u32 mds)
 {
 	struct lmv_tgt_desc	*tgt;
 	int			 rc;
@@ -1257,8 +1266,10 @@ int __lmv_fid_alloc(struct lmv_obd *lmv, struct lu_fid *fid,
 	 */
 	mutex_lock(&tgt->ltd_fid_mutex);
 
-	if (tgt->ltd_active == 0 || tgt->ltd_exp == NULL)
-		GOTO(out, rc = -ENODEV);
+	if (tgt->ltd_active == 0 || tgt->ltd_exp == NULL) {
+		rc = -ENODEV;
+		goto out;
+	}
 
 	/*
 	 * Asking underlaying tgt layer to allocate new fid.
@@ -1279,7 +1290,7 @@ int lmv_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
 {
 	struct obd_device     *obd = class_exp2obd(exp);
 	struct lmv_obd	*lmv = &obd->u.lmv;
-	mdsno_t		mds = 0;
+	u32		       mds = 0;
 	int		    rc;
 
 	LASSERT(op_data != NULL);
@@ -1352,7 +1363,7 @@ static int lmv_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 			     LUSTRE_CLI_FLD_HASH_DHT);
 	if (rc) {
 		CERROR("Can't init FLD, err %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	return 0;
@@ -1379,7 +1390,7 @@ static int lmv_cleanup(struct obd_device *obd)
 	return 0;
 }
 
-static int lmv_process_config(struct obd_device *obd, obd_count len, void *buf)
+static int lmv_process_config(struct obd_device *obd, u32 len, void *buf)
 {
 	struct lustre_cfg	*lcfg = buf;
 	struct obd_uuid		obd_uuid;
@@ -1391,20 +1402,27 @@ static int lmv_process_config(struct obd_device *obd, obd_count len, void *buf)
 	case LCFG_ADD_MDC:
 		/* modify_mdc_tgts add 0:lustre-clilmv  1:lustre-MDT0000_UUID
 		 * 2:0  3:1  4:lustre-MDT0000-mdc_UUID */
-		if (LUSTRE_CFG_BUFLEN(lcfg, 1) > sizeof(obd_uuid.uuid))
-			GOTO(out, rc = -EINVAL);
+		if (LUSTRE_CFG_BUFLEN(lcfg, 1) > sizeof(obd_uuid.uuid)) {
+			rc = -EINVAL;
+			goto out;
+		}
 
 		obd_str2uuid(&obd_uuid,  lustre_cfg_buf(lcfg, 1));
 
-		if (sscanf(lustre_cfg_buf(lcfg, 2), "%d", &index) != 1)
-			GOTO(out, rc = -EINVAL);
-		if (sscanf(lustre_cfg_buf(lcfg, 3), "%d", &gen) != 1)
-			GOTO(out, rc = -EINVAL);
+		if (sscanf(lustre_cfg_buf(lcfg, 2), "%d", &index) != 1) {
+			rc = -EINVAL;
+			goto out;
+		}
+		if (sscanf(lustre_cfg_buf(lcfg, 3), "%d", &gen) != 1) {
+			rc = -EINVAL;
+			goto out;
+		}
 		rc = lmv_add_target(obd, &obd_uuid, index, gen);
-		GOTO(out, rc);
+		goto out;
 	default:
 		CERROR("Unknown command: %d\n", lcfg->lcfg_command);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 out:
 	return rc;
@@ -1437,7 +1455,7 @@ static int lmv_statfs(const struct lu_env *env, struct obd_export *exp,
 			CERROR("can't stat MDS #%d (%s), error %d\n", i,
 			       lmv->tgts[i]->ltd_exp->exp_obd->obd_name,
 			       rc);
-			GOTO(out_free_temp, rc);
+			goto out_free_temp;
 		}
 
 		if (i == 0) {
@@ -1449,7 +1467,7 @@ static int lmv_statfs(const struct lu_env *env, struct obd_export *exp,
 			 * And also clients can be mounted as long as
 			 * MDT0 is in service*/
 			if (flags & OBD_STATFS_FOR_MDT0)
-				GOTO(out_free_temp, rc);
+				goto out_free_temp;
 		} else {
 			osfs->os_bavail += temp->os_bavail;
 			osfs->os_blocks += temp->os_blocks;
@@ -1480,7 +1498,7 @@ static int lmv_getstatus(struct obd_export *exp,
 }
 
 static int lmv_getxattr(struct obd_export *exp, const struct lu_fid *fid,
-			struct obd_capa *oc, obd_valid valid, const char *name,
+			struct obd_capa *oc, u64 valid, const char *name,
 			const char *input, int input_size, int output_size,
 			int flags, struct ptlrpc_request **request)
 {
@@ -1504,7 +1522,7 @@ static int lmv_getxattr(struct obd_export *exp, const struct lu_fid *fid,
 }
 
 static int lmv_setxattr(struct obd_export *exp, const struct lu_fid *fid,
-			struct obd_capa *oc, obd_valid valid, const char *name,
+			struct obd_capa *oc, u64 valid, const char *name,
 			const char *input, int input_size, int output_size,
 			int flags, __u32 suppgid,
 			struct ptlrpc_request **request)
@@ -1750,12 +1768,16 @@ lmv_enqueue_remote(struct obd_export *exp, struct ldlm_enqueue_info *einfo,
 	ptlrpc_req_finished(req);
 
 	tgt = lmv_find_target(lmv, &fid1);
-	if (IS_ERR(tgt))
-		GOTO(out, rc = PTR_ERR(tgt));
+	if (IS_ERR(tgt)) {
+		rc = PTR_ERR(tgt);
+		goto out;
+	}
 
 	OBD_ALLOC_PTR(rdata);
-	if (rdata == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (rdata == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	rdata->op_fid1 = fid1;
 	rdata->op_bias = MDS_CROSS_REF;
@@ -1804,7 +1826,7 @@ lmv_enqueue(struct obd_export *exp, struct ldlm_enqueue_info *einfo,
 }
 
 static int
-lmv_getattr_name(struct obd_export *exp,struct md_op_data *op_data,
+lmv_getattr_name(struct obd_export *exp, struct md_op_data *op_data,
 		 struct ptlrpc_request **request)
 {
 	struct ptlrpc_request   *req = NULL;
@@ -2371,7 +2393,7 @@ static int lmv_get_info(const struct lu_env *env, struct obd_export *exp,
 }
 
 int lmv_set_info_async(const struct lu_env *env, struct obd_export *exp,
-		       obd_count keylen, void *key, obd_count vallen,
+		       u32 keylen, void *key, u32 vallen,
 		       void *val, struct ptlrpc_request_set *set)
 {
 	struct lmv_tgt_desc    *tgt;
@@ -2488,8 +2510,7 @@ int lmv_unpackmd(struct obd_export *exp, struct lov_stripe_md **lsmp,
 
 	if (mea->mea_magic == MEA_MAGIC_LAST_CHAR ||
 	    mea->mea_magic == MEA_MAGIC_ALL_CHARS ||
-	    mea->mea_magic == MEA_MAGIC_HASH_SEGMENT)
-	{
+	    mea->mea_magic == MEA_MAGIC_HASH_SEGMENT) {
 		magic = le32_to_cpu(mea->mea_magic);
 	} else {
 		/*
diff --git a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
index 310df44f948d..117002097b28 100644
--- a/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
+++ b/drivers/staging/lustre/lustre/lmv/lproc_lmv.c
@@ -37,7 +37,7 @@
 #define DEBUG_SUBSYSTEM S_CLASS
 
 #include <linux/seq_file.h>
-#include <asm/statfs.h>
+#include <linux/statfs.h>
 #include "../include/lprocfs_status.h"
 #include "../include/obd_class.h"
 
@@ -58,7 +58,7 @@ static const char *placement_name[] = {
 	[PLACEMENT_INVAL_POLICY]  = "INVAL"
 };
 
-static placement_policy_t placement_name2policy(char *name, int len)
+static enum placement_policy placement_name2policy(char *name, int len)
 {
 	int		     i;
 
@@ -69,7 +69,7 @@ static placement_policy_t placement_name2policy(char *name, int len)
 	return PLACEMENT_INVAL_POLICY;
 }
 
-static const char *placement_policy2name(placement_policy_t placement)
+static const char *placement_policy2name(enum placement_policy placement)
 {
 	LASSERT(placement < PLACEMENT_MAX_POLICY);
 	return placement_name[placement];
@@ -94,7 +94,7 @@ static ssize_t lmv_placement_seq_write(struct file *file,
 	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
 	char		     dummy[MAX_POLICY_STRING_SIZE + 1];
 	int		      len = count;
-	placement_policy_t       policy;
+	enum placement_policy       policy;
 	struct lmv_obd	  *lmv;
 
 	if (copy_from_user(dummy, buffer, MAX_POLICY_STRING_SIZE))
diff --git a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
index 99ade92c5e64..314ce8525aed 100644
--- a/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_cl_internal.h
@@ -515,12 +515,12 @@ struct lov_io {
 	 * starting position within a file, for the current io loop iteration
 	 * (stripe), used by ci_io_loop().
 	 */
-	obd_off	    lis_pos;
+	u64	    lis_pos;
 	/**
 	 * end position with in a file, for the current stripe io. This is
 	 * exclusive (i.e., next offset after last byte affected by io).
 	 */
-	obd_off	    lis_endpos;
+	u64	    lis_endpos;
 
 	int		lis_mem_frozen;
 	int		lis_stripe_count;
diff --git a/drivers/staging/lustre/lustre/lov/lov_ea.c b/drivers/staging/lustre/lustre/lov/lov_ea.c
index 2401ca872507..9e21e5efcdb6 100644
--- a/drivers/staging/lustre/lustre/lov/lov_ea.c
+++ b/drivers/staging/lustre/lustre/lov/lov_ea.c
@@ -143,18 +143,18 @@ static void lsm_unpackmd_common(struct lov_stripe_md *lsm,
 
 static void
 lsm_stripe_by_index_plain(struct lov_stripe_md *lsm, int *stripeno,
-			   obd_off *lov_off, obd_off *swidth)
+			   u64 *lov_off, u64 *swidth)
 {
 	if (swidth)
-		*swidth = (obd_off)lsm->lsm_stripe_size * lsm->lsm_stripe_count;
+		*swidth = (u64)lsm->lsm_stripe_size * lsm->lsm_stripe_count;
 }
 
 static void
 lsm_stripe_by_offset_plain(struct lov_stripe_md *lsm, int *stripeno,
-			   obd_off *lov_off, obd_off *swidth)
+			   u64 *lov_off, u64 *swidth)
 {
 	if (swidth)
-		*swidth = (obd_off)lsm->lsm_stripe_size * lsm->lsm_stripe_count;
+		*swidth = (u64)lsm->lsm_stripe_size * lsm->lsm_stripe_count;
 }
 
 static int lsm_destroy_plain(struct lov_stripe_md *lsm, struct obdo *oa,
diff --git a/drivers/staging/lustre/lustre/lov/lov_internal.h b/drivers/staging/lustre/lustre/lov/lov_internal.h
index 017961a5cc3e..8c8508bbbb80 100644
--- a/drivers/staging/lustre/lustre/lov/lov_internal.h
+++ b/drivers/staging/lustre/lustre/lov/lov_internal.h
@@ -90,8 +90,8 @@ struct lov_request {
 	int		      rq_rc;
 	int		      rq_buflen;     /* length of sub_md */
 
-	obd_count		rq_oabufs;
-	obd_count		rq_pgaidx;
+	u32		      rq_oabufs;
+	u32		      rq_pgaidx;
 };
 
 struct lov_request_set {
@@ -109,7 +109,7 @@ struct lov_request_set {
 	struct llog_cookie		*set_cookies;
 	int				set_cookie_sent;
 	struct obd_trans_info		*set_oti;
-	obd_count			set_oabufs;
+	u32				set_oabufs;
 	struct brw_page			*set_pga;
 	struct lov_lock_handles		*set_lockh;
 	struct list_head			set_list;
@@ -140,7 +140,7 @@ static inline struct lov_lock_handles *
 lov_handle2llh(struct lustre_handle *handle)
 {
 	LASSERT(handle != NULL);
-	return(class_handle2object(handle->cookie));
+	return class_handle2object(handle->cookie);
 }
 
 static inline void lov_llh_put(struct lov_lock_handles *llh)
@@ -166,26 +166,24 @@ static inline void lov_llh_put(struct lov_lock_handles *llh)
 	(char *)((lv)->lov_tgts[index]->ltd_uuid.uuid)
 
 /* lov_merge.c */
-void lov_merge_attrs(struct obdo *tgt, struct obdo *src, obd_valid valid,
+void lov_merge_attrs(struct obdo *tgt, struct obdo *src, u64 valid,
 		     struct lov_stripe_md *lsm, int stripeno, int *set);
-int lov_merge_lvb(struct obd_export *exp, struct lov_stripe_md *lsm,
-		  struct ost_lvb *lvb, int kms_only);
 int lov_adjust_kms(struct obd_export *exp, struct lov_stripe_md *lsm,
-		   obd_off size, int shrink);
+		   u64 size, int shrink);
 int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 		      struct ost_lvb *lvb, __u64 *kms_place);
 
 /* lov_offset.c */
-obd_size lov_stripe_size(struct lov_stripe_md *lsm, obd_size ost_size,
+u64 lov_stripe_size(struct lov_stripe_md *lsm, u64 ost_size,
 			 int stripeno);
-int lov_stripe_offset(struct lov_stripe_md *lsm, obd_off lov_off,
-		      int stripeno, obd_off *obd_off);
-obd_off lov_size_to_stripe(struct lov_stripe_md *lsm, obd_off file_size,
+int lov_stripe_offset(struct lov_stripe_md *lsm, u64 lov_off,
+		      int stripeno, u64 *u64);
+u64 lov_size_to_stripe(struct lov_stripe_md *lsm, u64 file_size,
 			   int stripeno);
 int lov_stripe_intersects(struct lov_stripe_md *lsm, int stripeno,
-			  obd_off start, obd_off end,
-			  obd_off *obd_start, obd_off *obd_end);
-int lov_stripe_number(struct lov_stripe_md *lsm, obd_off lov_off);
+			  u64 start, u64 end,
+			  u64 *obd_start, u64 *obd_end);
+int lov_stripe_number(struct lov_stripe_md *lsm, u64 lov_off);
 
 /* lov_qos.c */
 #define LOV_USES_ASSIGNED_STRIPE	0
@@ -207,17 +205,6 @@ void lov_update_set(struct lov_request_set *set,
 int lov_update_common_set(struct lov_request_set *set,
 			  struct lov_request *req, int rc);
 int lov_check_and_wait_active(struct lov_obd *lov, int ost_idx);
-int lov_prep_create_set(struct obd_export *exp, struct obd_info *oifo,
-			struct lov_stripe_md **ea, struct obdo *src_oa,
-			struct obd_trans_info *oti,
-			struct lov_request_set **reqset);
-int cb_create_update(void *cookie, int rc);
-int lov_fini_create_set(struct lov_request_set *set, struct lov_stripe_md **ea);
-int lov_prep_brw_set(struct obd_export *exp, struct obd_info *oinfo,
-		     obd_count oa_bufs, struct brw_page *pga,
-		     struct obd_trans_info *oti,
-		     struct lov_request_set **reqset);
-int lov_fini_brw_set(struct lov_request_set *set);
 int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 			 struct lov_request_set **reqset);
 int lov_fini_getattr_set(struct lov_request_set *set);
@@ -225,8 +212,6 @@ int lov_prep_destroy_set(struct obd_export *exp, struct obd_info *oinfo,
 			 struct obdo *src_oa, struct lov_stripe_md *lsm,
 			 struct obd_trans_info *oti,
 			 struct lov_request_set **reqset);
-int lov_update_destroy_set(struct lov_request_set *set,
-			   struct lov_request *req, int rc);
 int lov_fini_destroy_set(struct lov_request_set *set);
 int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 			 struct obd_trans_info *oti,
@@ -234,30 +219,6 @@ int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 int lov_update_setattr_set(struct lov_request_set *set,
 			   struct lov_request *req, int rc);
 int lov_fini_setattr_set(struct lov_request_set *set);
-int lov_prep_punch_set(struct obd_export *exp, struct obd_info *oinfo,
-		       struct obd_trans_info *oti,
-		       struct lov_request_set **reqset);
-int lov_fini_punch_set(struct lov_request_set *set);
-int lov_prep_sync_set(struct obd_export *exp, struct obd_info *obd_info,
-		      obd_off start, obd_off end,
-		      struct lov_request_set **reqset);
-int lov_fini_sync_set(struct lov_request_set *set);
-int lov_prep_enqueue_set(struct obd_export *exp, struct obd_info *oinfo,
-			 struct ldlm_enqueue_info *einfo,
-			 struct lov_request_set **reqset);
-int lov_fini_enqueue_set(struct lov_request_set *set, __u32 mode, int rc,
-			 struct ptlrpc_request_set *rqset);
-int lov_prep_match_set(struct obd_export *exp, struct obd_info *oinfo,
-		       struct lov_stripe_md *lsm,
-		       ldlm_policy_data_t *policy, __u32 mode,
-		       struct lustre_handle *lockh,
-		       struct lov_request_set **reqset);
-int lov_fini_match_set(struct lov_request_set *set, __u32 mode, __u64 flags);
-int lov_prep_cancel_set(struct obd_export *exp, struct obd_info *oinfo,
-			struct lov_stripe_md *lsm,
-			__u32 mode, struct lustre_handle *lockh,
-			struct lov_request_set **reqset);
-int lov_fini_cancel_set(struct lov_request_set *set);
 int lov_prep_statfs_set(struct obd_device *obd, struct obd_info *oinfo,
 			struct lov_request_set **reqset);
 void lov_update_statfs(struct obd_statfs *osfs, struct obd_statfs *lov_sfs,
@@ -287,10 +248,6 @@ int lov_packmd(struct obd_export *exp, struct lov_mds_md **lmm,
 	       struct lov_stripe_md *lsm);
 int lov_unpackmd(struct obd_export *exp, struct lov_stripe_md **lsmp,
 		 struct lov_mds_md *lmm, int lmm_bytes);
-int lov_setstripe(struct obd_export *exp, int max_lmm_size,
-		  struct lov_stripe_md **lsmp, struct lov_user_md *lump);
-int lov_setea(struct obd_export *exp, struct lov_stripe_md **lsmp,
-	      struct lov_user_md *lump);
 int lov_getstripe(struct obd_export *exp,
 		  struct lov_stripe_md *lsm, struct lov_user_md *lump);
 int lov_alloc_memmd(struct lov_stripe_md **lsmp, __u16 stripe_count,
@@ -307,8 +264,6 @@ struct lov_stripe_md *lsm_alloc_plain(__u16 stripe_count, int *size);
 void lsm_free_plain(struct lov_stripe_md *lsm);
 void dump_lsm(unsigned int level, const struct lov_stripe_md *lsm);
 
-int lovea_destroy_object(struct lov_obd *lov, struct lov_stripe_md *lsm,
-			 struct obdo *oa, void *data);
 /* lproc_lov.c */
 #if defined (CONFIG_PROC_FS)
 extern const struct file_operations lov_proc_target_fops;
diff --git a/drivers/staging/lustre/lustre/lov/lov_io.c b/drivers/staging/lustre/lustre/lov/lov_io.c
index ce074c54a003..f1f6db3f664a 100644
--- a/drivers/staging/lustre/lustre/lov/lov_io.c
+++ b/drivers/staging/lustre/lustre/lov/lov_io.c
@@ -366,7 +366,7 @@ static void lov_io_fini(const struct lu_env *env, const struct cl_io_slice *ios)
 		wake_up_all(&lov->lo_waitq);
 }
 
-static obd_off lov_offset_mod(obd_off val, int delta)
+static u64 lov_offset_mod(u64 val, int delta)
 {
 	if (val != OBD_OBJECT_EOF)
 		val += delta;
@@ -379,9 +379,9 @@ static int lov_io_iter_init(const struct lu_env *env,
 	struct lov_io	*lio = cl2lov_io(env, ios);
 	struct lov_stripe_md *lsm = lio->lis_object->lo_lsm;
 	struct lov_io_sub    *sub;
-	obd_off endpos;
-	obd_off start;
-	obd_off end;
+	u64 endpos;
+	u64 start;
+	u64 end;
 	int stripe;
 	int rc = 0;
 
diff --git a/drivers/staging/lustre/lustre/lov/lov_lock.c b/drivers/staging/lustre/lustre/lov/lov_lock.c
index 08ac3745f0da..49e694222ac8 100644
--- a/drivers/staging/lustre/lustre/lov/lov_lock.c
+++ b/drivers/staging/lustre/lustre/lov/lov_lock.c
@@ -290,10 +290,10 @@ static int lov_lock_sub_init(const struct lu_env *env,
 	int result = 0;
 	int i;
 	int nr;
-	obd_off start;
-	obd_off end;
-	obd_off file_start;
-	obd_off file_end;
+	u64 start;
+	u64 end;
+	u64 file_start;
+	u64 file_end;
 
 	struct lov_object       *loo    = cl2lov(lck->lls_cl.cls_obj);
 	struct lov_layout_raid0 *r0     = lov_r0(loo);
@@ -860,10 +860,10 @@ static int lock_lock_multi_match()
 	struct lov_layout_raid0 *r0      = lov_r0(loo);
 	struct lov_lock_sub     *sub;
 	struct cl_object	*subobj;
-	obd_off  fstart;
-	obd_off  fend;
-	obd_off  start;
-	obd_off  end;
+	u64  fstart;
+	u64  fend;
+	u64  start;
+	u64  end;
 	int i;
 
 	fstart = cl_offset(need->cld_obj, need->cld_start);
@@ -900,8 +900,8 @@ static int lov_lock_stripe_is_matching(const struct lu_env *env,
 				       const struct cl_lock_descr *descr)
 {
 	struct lov_stripe_md *lsm = lov->lo_lsm;
-	obd_off start;
-	obd_off end;
+	u64 start;
+	u64 end;
 	int result;
 
 	if (lov_r0(lov)->lo_nr == 1)
@@ -919,8 +919,8 @@ static int lov_lock_stripe_is_matching(const struct lu_env *env,
 		 stripe == lov_stripe_number(lsm, end);
 	if (result) {
 		struct cl_lock_descr *subd = &lov_env_info(env)->lti_ldescr;
-		obd_off sub_start;
-		obd_off sub_end;
+		u64 sub_start;
+		u64 sub_end;
 
 		subd->cld_obj  = NULL;   /* don't need sub object at all */
 		subd->cld_mode = descr->cld_mode;
diff --git a/drivers/staging/lustre/lustre/lov/lov_merge.c b/drivers/staging/lustre/lustre/lov/lov_merge.c
index 85144b8da96d..b7e7bfabe382 100644
--- a/drivers/staging/lustre/lustre/lov/lov_merge.c
+++ b/drivers/staging/lustre/lustre/lov/lov_merge.c
@@ -52,9 +52,9 @@ int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 	__u64 size = 0;
 	__u64 kms = 0;
 	__u64 blocks = 0;
-	obd_time current_mtime = lvb->lvb_mtime;
-	obd_time current_atime = lvb->lvb_atime;
-	obd_time current_ctime = lvb->lvb_ctime;
+	s64 current_mtime = lvb->lvb_mtime;
+	s64 current_atime = lvb->lvb_atime;
+	s64 current_ctime = lvb->lvb_ctime;
 	int i;
 	int rc = 0;
 
@@ -66,7 +66,7 @@ int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 	       lvb->lvb_atime, lvb->lvb_ctime, lvb->lvb_blocks);
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
 		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-		obd_size lov_size, tmpsize;
+		u64 lov_size, tmpsize;
 
 		if (OST_LVB_IS_ERR(loi->loi_lvb.lvb_blocks)) {
 			rc = OST_LVB_GET_ERR(loi->loi_lvb.lvb_blocks);
@@ -109,36 +109,9 @@ int lov_merge_lvb_kms(struct lov_stripe_md *lsm,
 	return rc;
 }
 
-/** Merge the lock value block(&lvb) attributes from each of the stripes in a
- * file into a single lvb. It is expected that the caller initializes the
- * current atime, mtime, ctime to avoid regressing a more uptodate time on
- * the local client.
- *
- * If \a kms_only is set then we do not consider the recently seen size (rss)
- * when updating the known minimum size (kms).  Even when merging RSS, we will
- * take the KMS value if it's larger.  This prevents getattr from stomping on
- * dirty cached pages which extend the file size. */
-int lov_merge_lvb(struct obd_export *exp,
-		  struct lov_stripe_md *lsm, struct ost_lvb *lvb, int kms_only)
-{
-	int   rc;
-	__u64 kms;
-
-	lov_stripe_lock(lsm);
-	rc = lov_merge_lvb_kms(lsm, lvb, &kms);
-	lov_stripe_unlock(lsm);
-	if (kms_only)
-		lvb->lvb_size = kms;
-
-	CDEBUG(D_INODE, "merged for ID "DOSTID" s=%llu m=%llu a=%llu c=%llu b=%llu\n",
-	       POSTID(&lsm->lsm_oi), lvb->lvb_size, lvb->lvb_mtime,
-	       lvb->lvb_atime, lvb->lvb_ctime, lvb->lvb_blocks);
-	return rc;
-}
-
 /* Must be called under the lov_stripe_lock() */
 int lov_adjust_kms(struct obd_export *exp, struct lov_stripe_md *lsm,
-		   obd_off size, int shrink)
+		   u64 size, int shrink)
 {
 	struct lov_oinfo *loi;
 	int stripe = 0;
@@ -173,7 +146,7 @@ int lov_adjust_kms(struct obd_export *exp, struct lov_stripe_md *lsm,
 	return 0;
 }
 
-void lov_merge_attrs(struct obdo *tgt, struct obdo *src, obd_valid valid,
+void lov_merge_attrs(struct obdo *tgt, struct obdo *src, u64 valid,
 		     struct lov_stripe_md *lsm, int stripeno, int *set)
 {
 	valid &= src->o_valid;
@@ -181,7 +154,7 @@ void lov_merge_attrs(struct obdo *tgt, struct obdo *src, obd_valid valid,
 	if (*set) {
 		if (valid & OBD_MD_FLSIZE) {
 			/* this handles sparse files properly */
-			obd_size lov_size;
+			u64 lov_size;
 
 			lov_size = lov_stripe_size(lsm, src->o_size, stripeno);
 			if (lov_size > tgt->o_size)
diff --git a/drivers/staging/lustre/lustre/lov/lov_obd.c b/drivers/staging/lustre/lustre/lov/lov_obd.c
index e4f4fe3f71c7..94dfd64bd283 100644
--- a/drivers/staging/lustre/lustre/lov/lov_obd.c
+++ b/drivers/staging/lustre/lustre/lov/lov_obd.c
@@ -51,7 +51,6 @@
 #include "../include/lustre_dlm.h"
 #include "../include/lustre_mds.h"
 #include "../include/obd_class.h"
-#include "../include/obd_ost.h"
 #include "../include/lprocfs_status.h"
 #include "../include/lustre_param.h"
 #include "../include/cl_object.h"
@@ -389,8 +388,10 @@ static int lov_set_osc_active(struct obd_device *obd, struct obd_uuid *uuid,
 			break;
 	}
 
-	if (index == lov->desc.ld_tgt_count)
-		GOTO(out, index = -EINVAL);
+	if (index == lov->desc.ld_tgt_count) {
+		index = -EINVAL;
+		goto out;
+	}
 
 	if (ev == OBD_NOTIFY_DEACTIVATE || ev == OBD_NOTIFY_ACTIVATE) {
 		activate = (ev == OBD_NOTIFY_ACTIVATE) ? 1 : 0;
@@ -410,7 +411,7 @@ static int lov_set_osc_active(struct obd_device *obd, struct obd_uuid *uuid,
 		if (lov->lov_tgts[index]->ltd_active == active) {
 			CDEBUG(D_INFO, "OSC %s already %sactive!\n",
 			       uuid->uuid, active ? "" : "in");
-			GOTO(out, index);
+			goto out;
 		} else {
 			CDEBUG(D_CONFIG, "Marking OSC %s %sactive\n",
 			       obd_uuid2str(uuid), active ? "" : "in");
@@ -618,11 +619,13 @@ static int lov_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 
 	rc = lov_connect_obd(obd, index, active, &lov->lov_ocd);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* connect to administrative disabled ost */
-	if (!tgt->ltd_exp)
-		GOTO(out, rc = 0);
+	if (!tgt->ltd_exp) {
+		rc = 0;
+		goto out;
+	}
 
 	if (lov->lov_cache != NULL) {
 		rc = obd_set_info_async(NULL, tgt->ltd_exp,
@@ -630,7 +633,7 @@ static int lov_add_target(struct obd_device *obd, struct obd_uuid *uuidp,
 				sizeof(struct cl_client_cache), lov->lov_cache,
 				NULL);
 		if (rc < 0)
-			GOTO(out, rc);
+			goto out;
 	}
 
 	rc = lov_notify(obd, tgt->ltd_exp->exp_obd,
@@ -667,14 +670,16 @@ int lov_del_target(struct obd_device *obd, __u32 index,
 
 	if (!lov->lov_tgts[index]) {
 		CERROR("LOV target at index %d is not setup.\n", index);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	if (uuidp && !obd_uuid_equals(uuidp, &lov->lov_tgts[index]->ltd_uuid)) {
 		CERROR("LOV target UUID %s at index %d doesn't match %s.\n",
 		       lov_uuid2str(lov, index), index,
 		       obd_uuid2str(uuidp));
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	CDEBUG(D_CONFIG, "uuid: %s idx: %d gen: %d exp: %p active: %d\n",
@@ -816,7 +821,7 @@ int lov_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 	lov->lov_pool_count = 0;
 	rc = lov_ost_pool_init(&lov->lov_packed, 0);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	lprocfs_lov_init_vars(&lvars);
 	lprocfs_obd_setup(obd, lvars.obd_vars);
@@ -923,15 +928,21 @@ int lov_process_config_base(struct obd_device *obd, struct lustre_cfg *lcfg,
 		__u32 index;
 		int gen;
 		/* lov_modify_tgts add  0:lov_mdsA  1:ost1_UUID  2:0  3:1 */
-		if (LUSTRE_CFG_BUFLEN(lcfg, 1) > sizeof(obd_uuid.uuid))
-			GOTO(out, rc = -EINVAL);
+		if (LUSTRE_CFG_BUFLEN(lcfg, 1) > sizeof(obd_uuid.uuid)) {
+			rc = -EINVAL;
+			goto out;
+		}
 
 		obd_str2uuid(&obd_uuid,  lustre_cfg_buf(lcfg, 1));
 
-		if (sscanf(lustre_cfg_buf(lcfg, 2), "%d", indexp) != 1)
-			GOTO(out, rc = -EINVAL);
-		if (sscanf(lustre_cfg_buf(lcfg, 3), "%d", genp) != 1)
-			GOTO(out, rc = -EINVAL);
+		if (sscanf(lustre_cfg_buf(lcfg, 2), "%d", indexp) != 1) {
+			rc = -EINVAL;
+			goto out;
+		}
+		if (sscanf(lustre_cfg_buf(lcfg, 3), "%d", genp) != 1) {
+			rc = -EINVAL;
+			goto out;
+		}
 		index = *indexp;
 		gen = *genp;
 		if (cmd == LCFG_LOV_ADD_OBD)
@@ -940,14 +951,16 @@ int lov_process_config_base(struct obd_device *obd, struct lustre_cfg *lcfg,
 			rc = lov_add_target(obd, &obd_uuid, index, gen, 0);
 		else
 			rc = lov_del_target(obd, index, &obd_uuid, gen);
-		GOTO(out, rc);
+		goto out;
 	}
 	case LCFG_PARAM: {
 		struct lprocfs_static_vars lvars = { NULL };
 		struct lov_desc *desc = &(obd->u.lov.desc);
 
-		if (!desc)
-			GOTO(out, rc = -EINVAL);
+		if (!desc) {
+			rc = -EINVAL;
+			goto out;
+		}
 
 		lprocfs_lov_init_vars(&lvars);
 
@@ -955,17 +968,18 @@ int lov_process_config_base(struct obd_device *obd, struct lustre_cfg *lcfg,
 					      lcfg, obd);
 		if (rc > 0)
 			rc = 0;
-		GOTO(out, rc);
+		goto out;
 	}
 	case LCFG_POOL_NEW:
 	case LCFG_POOL_ADD:
 	case LCFG_POOL_DEL:
 	case LCFG_POOL_REM:
-		GOTO(out, rc);
+		goto out;
 
 	default: {
 		CERROR("Unknown command: %d\n", lcfg->lcfg_command);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 
 	}
 	}
@@ -990,22 +1004,30 @@ static int lov_recreate(struct obd_export *exp, struct obdo *src_oa,
 
 	ost_idx = src_oa->o_nlink;
 	lsm = *ea;
-	if (lsm == NULL)
-		GOTO(out, rc = -EINVAL);
+	if (lsm == NULL) {
+		rc = -EINVAL;
+		goto out;
+	}
 	if (ost_idx >= lov->desc.ld_tgt_count ||
-	    !lov->lov_tgts[ost_idx])
-		GOTO(out, rc = -EINVAL);
+	    !lov->lov_tgts[ost_idx]) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
 		if (lsm->lsm_oinfo[i]->loi_ost_idx == ost_idx) {
 			if (ostid_id(&lsm->lsm_oinfo[i]->loi_oi) !=
-					ostid_id(&src_oa->o_oi))
-				GOTO(out, rc = -EINVAL);
+					ostid_id(&src_oa->o_oi)) {
+				rc = -EINVAL;
+				goto out;
+			}
 			break;
 		}
 	}
-	if (i == lsm->lsm_stripe_count)
-		GOTO(out, rc = -EINVAL);
+	if (i == lsm->lsm_stripe_count) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	rc = obd_create(NULL, lov->lov_tgts[ost_idx]->ltd_exp,
 			src_oa, &obj_mdp, oti);
@@ -1081,7 +1103,7 @@ static int lov_destroy(const struct lu_env *env, struct obd_export *exp,
 	obd_getref(exp->exp_obd);
 	rc = lov_prep_destroy_set(exp, &oinfo, oa, lsm, oti, &set);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	list_for_each(pos, &set->set_list) {
 		req = list_entry(pos, struct lov_request, rq_link);
@@ -1113,54 +1135,6 @@ out:
 	return rc ? rc : err;
 }
 
-static int lov_getattr(const struct lu_env *env, struct obd_export *exp,
-		       struct obd_info *oinfo)
-{
-	struct lov_request_set *set;
-	struct lov_request *req;
-	struct list_head *pos;
-	struct lov_obd *lov;
-	int err = 0, rc = 0;
-
-	LASSERT(oinfo);
-	ASSERT_LSM_MAGIC(oinfo->oi_md);
-
-	if (!exp || !exp->exp_obd)
-		return -ENODEV;
-
-	lov = &exp->exp_obd->u.lov;
-
-	rc = lov_prep_getattr_set(exp, oinfo, &set);
-	if (rc)
-		return rc;
-
-	list_for_each(pos, &set->set_list) {
-		req = list_entry(pos, struct lov_request, rq_link);
-
-		CDEBUG(D_INFO, "objid "DOSTID"[%d] has subobj "DOSTID" at idx"
-		       " %u\n", POSTID(&oinfo->oi_oa->o_oi), req->rq_stripe,
-		       POSTID(&req->rq_oi.oi_oa->o_oi), req->rq_idx);
-
-		rc = obd_getattr(env, lov->lov_tgts[req->rq_idx]->ltd_exp,
-				 &req->rq_oi);
-		err = lov_update_common_set(set, req, rc);
-		if (err) {
-			CERROR("%s: getattr objid "DOSTID" subobj "
-			       DOSTID" on OST idx %d: rc = %d\n",
-			       exp->exp_obd->obd_name,
-			       POSTID(&oinfo->oi_oa->o_oi),
-			       POSTID(&req->rq_oi.oi_oa->o_oi),
-			       req->rq_idx, err);
-			break;
-		}
-	}
-
-	rc = lov_fini_getattr_set(set);
-	if (err)
-		rc = err;
-	return rc;
-}
-
 static int lov_getattr_interpret(struct ptlrpc_request_set *rqset,
 				 void *data, int rc)
 {
@@ -1214,7 +1188,7 @@ static int lov_getattr_async(struct obd_export *exp, struct obd_info *oinfo,
 			       POSTID(&oinfo->oi_oa->o_oi),
 			       POSTID(&req->rq_oi.oi_oa->o_oi),
 			       req->rq_idx, rc);
-			GOTO(out, rc);
+			goto out;
 		}
 	}
 
@@ -1232,57 +1206,6 @@ out:
 	return rc ? rc : err;
 }
 
-static int lov_setattr(const struct lu_env *env, struct obd_export *exp,
-		       struct obd_info *oinfo, struct obd_trans_info *oti)
-{
-	struct lov_request_set *set;
-	struct lov_obd *lov;
-	struct list_head *pos;
-	struct lov_request *req;
-	int err = 0, rc = 0;
-
-	LASSERT(oinfo);
-	ASSERT_LSM_MAGIC(oinfo->oi_md);
-
-	if (!exp || !exp->exp_obd)
-		return -ENODEV;
-
-	/* for now, we only expect the following updates here */
-	LASSERT(!(oinfo->oi_oa->o_valid & ~(OBD_MD_FLID | OBD_MD_FLTYPE |
-					    OBD_MD_FLMODE | OBD_MD_FLATIME |
-					    OBD_MD_FLMTIME | OBD_MD_FLCTIME |
-					    OBD_MD_FLFLAGS | OBD_MD_FLSIZE |
-					    OBD_MD_FLGROUP | OBD_MD_FLUID |
-					    OBD_MD_FLGID | OBD_MD_FLFID |
-					    OBD_MD_FLGENER)));
-	lov = &exp->exp_obd->u.lov;
-	rc = lov_prep_setattr_set(exp, oinfo, oti, &set);
-	if (rc)
-		return rc;
-
-	list_for_each(pos, &set->set_list) {
-		req = list_entry(pos, struct lov_request, rq_link);
-
-		rc = obd_setattr(env, lov->lov_tgts[req->rq_idx]->ltd_exp,
-				 &req->rq_oi, NULL);
-		err = lov_update_setattr_set(set, req, rc);
-		if (err) {
-			CERROR("%s: setattr objid "DOSTID" subobj "
-			       DOSTID" on OST idx %d: rc = %d\n",
-			       exp->exp_obd->obd_name,
-			       POSTID(&set->set_oi->oi_oa->o_oi),
-			       POSTID(&req->rq_oi.oi_oa->o_oi), req->rq_idx,
-			       err);
-			if (!rc)
-				rc = err;
-		}
-	}
-	err = lov_fini_setattr_set(set);
-	if (!rc)
-		rc = err;
-	return rc;
-}
-
 static int lov_setattr_interpret(struct ptlrpc_request_set *rqset,
 				 void *data, int rc)
 {
@@ -1365,297 +1288,6 @@ static int lov_setattr_async(struct obd_export *exp, struct obd_info *oinfo,
 	return 0;
 }
 
-static int lov_punch_interpret(struct ptlrpc_request_set *rqset,
-			       void *data, int rc)
-{
-	struct lov_request_set *lovset = (struct lov_request_set *)data;
-	int err;
-
-	if (rc)
-		atomic_set(&lovset->set_completes, 0);
-	err = lov_fini_punch_set(lovset);
-	return rc ? rc : err;
-}
-
-/* FIXME: maybe we'll just make one node the authoritative attribute node, then
- * we can send this 'punch' to just the authoritative node and the nodes
- * that the punch will affect. */
-static int lov_punch(const struct lu_env *env, struct obd_export *exp,
-		     struct obd_info *oinfo, struct obd_trans_info *oti,
-		     struct ptlrpc_request_set *rqset)
-{
-	struct lov_request_set *set;
-	struct lov_obd *lov;
-	struct list_head *pos;
-	struct lov_request *req;
-	int rc = 0;
-
-	LASSERT(oinfo);
-	ASSERT_LSM_MAGIC(oinfo->oi_md);
-
-	if (!exp || !exp->exp_obd)
-		return -ENODEV;
-
-	lov = &exp->exp_obd->u.lov;
-	rc = lov_prep_punch_set(exp, oinfo, oti, &set);
-	if (rc)
-		return rc;
-
-	list_for_each(pos, &set->set_list) {
-		req = list_entry(pos, struct lov_request, rq_link);
-
-		rc = obd_punch(env, lov->lov_tgts[req->rq_idx]->ltd_exp,
-			       &req->rq_oi, NULL, rqset);
-		if (rc) {
-			CERROR("%s: punch objid "DOSTID" subobj "DOSTID
-			       " on OST idx %d: rc = %d\n",
-			       exp->exp_obd->obd_name,
-			       POSTID(&set->set_oi->oi_oa->o_oi),
-			       POSTID(&req->rq_oi.oi_oa->o_oi), req->rq_idx, rc);
-			break;
-		}
-	}
-
-	if (rc || list_empty(&rqset->set_requests)) {
-		int err;
-		err = lov_fini_punch_set(set);
-		return rc ? rc : err;
-	}
-
-	LASSERT(rqset->set_interpret == NULL);
-	rqset->set_interpret = lov_punch_interpret;
-	rqset->set_arg = (void *)set;
-
-	return 0;
-}
-
-static int lov_sync_interpret(struct ptlrpc_request_set *rqset,
-			      void *data, int rc)
-{
-	struct lov_request_set *lovset = data;
-	int err;
-
-	if (rc)
-		atomic_set(&lovset->set_completes, 0);
-	err = lov_fini_sync_set(lovset);
-	return rc ?: err;
-}
-
-static int lov_sync(const struct lu_env *env, struct obd_export *exp,
-		    struct obd_info *oinfo, obd_off start, obd_off end,
-		    struct ptlrpc_request_set *rqset)
-{
-	struct lov_request_set *set = NULL;
-	struct lov_obd *lov;
-	struct list_head *pos;
-	struct lov_request *req;
-	int rc = 0;
-
-	ASSERT_LSM_MAGIC(oinfo->oi_md);
-	LASSERT(rqset != NULL);
-
-	if (!exp->exp_obd)
-		return -ENODEV;
-
-	lov = &exp->exp_obd->u.lov;
-	rc = lov_prep_sync_set(exp, oinfo, start, end, &set);
-	if (rc)
-		return rc;
-
-	CDEBUG(D_INFO, "fsync objid "DOSTID" [%#llx, %#llx]\n",
-	       POSTID(&set->set_oi->oi_oa->o_oi), start, end);
-
-	list_for_each(pos, &set->set_list) {
-		req = list_entry(pos, struct lov_request, rq_link);
-
-		rc = obd_sync(env, lov->lov_tgts[req->rq_idx]->ltd_exp,
-			      &req->rq_oi, req->rq_oi.oi_policy.l_extent.start,
-			      req->rq_oi.oi_policy.l_extent.end, rqset);
-		if (rc) {
-			CERROR("%s: fsync objid "DOSTID" subobj "DOSTID
-			       " on OST idx %d: rc = %d\n",
-			       exp->exp_obd->obd_name,
-			       POSTID(&set->set_oi->oi_oa->o_oi),
-			       POSTID(&req->rq_oi.oi_oa->o_oi), req->rq_idx,
-			       rc);
-			break;
-		}
-	}
-
-	/* If we are not waiting for responses on async requests, return. */
-	if (rc || list_empty(&rqset->set_requests)) {
-		int err = lov_fini_sync_set(set);
-
-		return rc ?: err;
-	}
-
-	LASSERT(rqset->set_interpret == NULL);
-	rqset->set_interpret = lov_sync_interpret;
-	rqset->set_arg = (void *)set;
-
-	return 0;
-}
-
-static int lov_brw_check(struct lov_obd *lov, struct obd_info *lov_oinfo,
-			 obd_count oa_bufs, struct brw_page *pga)
-{
-	struct obd_info oinfo = { { { 0 } } };
-	int i, rc = 0;
-
-	oinfo.oi_oa = lov_oinfo->oi_oa;
-
-	/* The caller just wants to know if there's a chance that this
-	 * I/O can succeed */
-	for (i = 0; i < oa_bufs; i++) {
-		int stripe = lov_stripe_number(lov_oinfo->oi_md, pga[i].off);
-		int ost = lov_oinfo->oi_md->lsm_oinfo[stripe]->loi_ost_idx;
-		obd_off start, end;
-
-		if (!lov_stripe_intersects(lov_oinfo->oi_md, i, pga[i].off,
-					   pga[i].off + pga[i].count - 1,
-					   &start, &end))
-			continue;
-
-		if (!lov->lov_tgts[ost] || !lov->lov_tgts[ost]->ltd_active) {
-			CDEBUG(D_HA, "lov idx %d inactive\n", ost);
-			return -EIO;
-		}
-
-		rc = obd_brw(OBD_BRW_CHECK, lov->lov_tgts[ost]->ltd_exp, &oinfo,
-			     1, &pga[i], NULL);
-		if (rc)
-			break;
-	}
-	return rc;
-}
-
-static int lov_brw(int cmd, struct obd_export *exp, struct obd_info *oinfo,
-		   obd_count oa_bufs, struct brw_page *pga,
-		   struct obd_trans_info *oti)
-{
-	struct lov_request_set *set;
-	struct lov_request *req;
-	struct list_head *pos;
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-	int err, rc = 0;
-
-	ASSERT_LSM_MAGIC(oinfo->oi_md);
-
-	if (cmd == OBD_BRW_CHECK) {
-		rc = lov_brw_check(lov, oinfo, oa_bufs, pga);
-		return rc;
-	}
-
-	rc = lov_prep_brw_set(exp, oinfo, oa_bufs, pga, oti, &set);
-	if (rc)
-		return rc;
-
-	list_for_each(pos, &set->set_list) {
-		struct obd_export *sub_exp;
-		struct brw_page *sub_pga;
-		req = list_entry(pos, struct lov_request, rq_link);
-
-		sub_exp = lov->lov_tgts[req->rq_idx]->ltd_exp;
-		sub_pga = set->set_pga + req->rq_pgaidx;
-		rc = obd_brw(cmd, sub_exp, &req->rq_oi, req->rq_oabufs,
-			     sub_pga, oti);
-		if (rc)
-			break;
-		lov_update_common_set(set, req, rc);
-	}
-
-	err = lov_fini_brw_set(set);
-	if (!rc)
-		rc = err;
-	return rc;
-}
-
-static int lov_enqueue_interpret(struct ptlrpc_request_set *rqset,
-				 void *data, int rc)
-{
-	struct lov_request_set *lovset = (struct lov_request_set *)data;
-
-	rc = lov_fini_enqueue_set(lovset, lovset->set_ei->ei_mode, rc, rqset);
-	return rc;
-}
-
-static int lov_enqueue(struct obd_export *exp, struct obd_info *oinfo,
-		       struct ldlm_enqueue_info *einfo,
-		       struct ptlrpc_request_set *rqset)
-{
-	ldlm_mode_t mode = einfo->ei_mode;
-	struct lov_request_set *set;
-	struct lov_request *req;
-	struct list_head *pos;
-	struct lov_obd *lov;
-	ldlm_error_t rc;
-
-	LASSERT(oinfo);
-	ASSERT_LSM_MAGIC(oinfo->oi_md);
-	LASSERT(mode == (mode & -mode));
-
-	/* we should never be asked to replay a lock this way. */
-	LASSERT((oinfo->oi_flags & LDLM_FL_REPLAY) == 0);
-
-	if (!exp || !exp->exp_obd)
-		return -ENODEV;
-
-	lov = &exp->exp_obd->u.lov;
-	rc = lov_prep_enqueue_set(exp, oinfo, einfo, &set);
-	if (rc)
-		return rc;
-
-	list_for_each(pos, &set->set_list) {
-		req = list_entry(pos, struct lov_request, rq_link);
-
-		rc = obd_enqueue(lov->lov_tgts[req->rq_idx]->ltd_exp,
-				 &req->rq_oi, einfo, rqset);
-		if (rc != ELDLM_OK)
-			GOTO(out, rc);
-	}
-
-	if (rqset && !list_empty(&rqset->set_requests)) {
-		LASSERT(rc == 0);
-		LASSERT(rqset->set_interpret == NULL);
-		rqset->set_interpret = lov_enqueue_interpret;
-		rqset->set_arg = (void *)set;
-		return rc;
-	}
-out:
-	rc = lov_fini_enqueue_set(set, mode, rc, rqset);
-	return rc;
-}
-
-static int lov_change_cbdata(struct obd_export *exp,
-			     struct lov_stripe_md *lsm, ldlm_iterator_t it,
-			     void *data)
-{
-	struct lov_obd *lov;
-	int rc = 0, i;
-
-	ASSERT_LSM_MAGIC(lsm);
-
-	if (!exp || !exp->exp_obd)
-		return -ENODEV;
-
-	lov = &exp->exp_obd->u.lov;
-	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		struct lov_stripe_md submd;
-		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-
-		if (!lov->lov_tgts[loi->loi_ost_idx]) {
-			CDEBUG(D_HA, "lov idx %d NULL \n", loi->loi_ost_idx);
-			continue;
-		}
-
-		submd.lsm_oi = loi->loi_oi;
-		submd.lsm_stripe_count = 0;
-		rc = obd_change_cbdata(lov->lov_tgts[loi->loi_ost_idx]->ltd_exp,
-				       &submd, it, data);
-	}
-	return rc;
-}
-
 /* find any ldlm lock of the inode in lov
  * return 0    not find
  *	1    find one
@@ -1691,106 +1323,6 @@ static int lov_find_cbdata(struct obd_export *exp,
 	return rc;
 }
 
-static int lov_cancel(struct obd_export *exp, struct lov_stripe_md *lsm,
-		      __u32 mode, struct lustre_handle *lockh)
-{
-	struct lov_request_set *set;
-	struct obd_info oinfo;
-	struct lov_request *req;
-	struct list_head *pos;
-	struct lov_obd *lov;
-	struct lustre_handle *lov_lockhp;
-	int err = 0, rc = 0;
-
-	ASSERT_LSM_MAGIC(lsm);
-
-	if (!exp || !exp->exp_obd)
-		return -ENODEV;
-
-	LASSERT(lockh);
-	lov = &exp->exp_obd->u.lov;
-	rc = lov_prep_cancel_set(exp, &oinfo, lsm, mode, lockh, &set);
-	if (rc)
-		return rc;
-
-	list_for_each(pos, &set->set_list) {
-		req = list_entry(pos, struct lov_request, rq_link);
-		lov_lockhp = set->set_lockh->llh_handles + req->rq_stripe;
-
-		rc = obd_cancel(lov->lov_tgts[req->rq_idx]->ltd_exp,
-				req->rq_oi.oi_md, mode, lov_lockhp);
-		rc = lov_update_common_set(set, req, rc);
-		if (rc) {
-			CERROR("%s: cancel objid "DOSTID" subobj "
-			       DOSTID" on OST idx %d: rc = %d\n",
-			       exp->exp_obd->obd_name, POSTID(&lsm->lsm_oi),
-			       POSTID(&req->rq_oi.oi_md->lsm_oi),
-			       req->rq_idx, rc);
-			err = rc;
-		}
-
-	}
-	lov_fini_cancel_set(set);
-	return err;
-}
-
-static int lov_cancel_unused(struct obd_export *exp,
-			     struct lov_stripe_md *lsm,
-			     ldlm_cancel_flags_t flags, void *opaque)
-{
-	struct lov_obd *lov;
-	int rc = 0, i;
-
-	if (!exp || !exp->exp_obd)
-		return -ENODEV;
-
-	lov = &exp->exp_obd->u.lov;
-	if (lsm == NULL) {
-		for (i = 0; i < lov->desc.ld_tgt_count; i++) {
-			int err;
-			if (!lov->lov_tgts[i] || !lov->lov_tgts[i]->ltd_exp)
-				continue;
-
-			err = obd_cancel_unused(lov->lov_tgts[i]->ltd_exp, NULL,
-						flags, opaque);
-			if (!rc)
-				rc = err;
-		}
-		return rc;
-	}
-
-	ASSERT_LSM_MAGIC(lsm);
-
-	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		struct lov_stripe_md submd;
-		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-		int idx = loi->loi_ost_idx;
-		int err;
-
-		if (!lov->lov_tgts[idx]) {
-			CDEBUG(D_HA, "lov idx %d NULL\n", idx);
-			continue;
-		}
-
-		if (!lov->lov_tgts[idx]->ltd_active)
-			CDEBUG(D_HA, "lov idx %d inactive\n", idx);
-
-		submd.lsm_oi = loi->loi_oi;
-		submd.lsm_stripe_count = 0;
-		err = obd_cancel_unused(lov->lov_tgts[idx]->ltd_exp,
-					&submd, flags, opaque);
-		if (err && lov->lov_tgts[idx]->ltd_active) {
-			CERROR("%s: cancel unused objid "DOSTID
-			       " subobj "DOSTID" on OST idx %d: rc = %d\n",
-			       exp->exp_obd->obd_name, POSTID(&lsm->lsm_oi),
-			       POSTID(&loi->loi_oi), idx, err);
-			if (!rc)
-				rc = err;
-		}
-	}
-	return rc;
-}
-
 int lov_statfs_interpret(struct ptlrpc_request_set *rqset, void *data, int rc)
 {
 	struct lov_request_set *lovset = (struct lov_request_set *)data;
@@ -1960,15 +1492,9 @@ static int lov_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		obd_ioctl_freedata(buf, len);
 		break;
 	}
-	case LL_IOC_LOV_SETSTRIPE:
-		rc = lov_setstripe(exp, len, karg, uarg);
-		break;
 	case LL_IOC_LOV_GETSTRIPE:
 		rc = lov_getstripe(exp, karg, uarg);
 		break;
-	case LL_IOC_LOV_SETEA:
-		rc = lov_setea(exp, karg, uarg);
-		break;
 	case OBD_IOC_QUOTACTL: {
 		struct if_quotactl *qctl = karg;
 		struct lov_tgt_desc *tgt = NULL;
@@ -2080,13 +1606,13 @@ static int lov_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
  * \param fm_end logical end of mapping
  * \param start_stripe starting stripe will be returned in this
  */
-obd_size fiemap_calc_fm_end_offset(struct ll_user_fiemap *fiemap,
-				   struct lov_stripe_md *lsm, obd_size fm_start,
-				   obd_size fm_end, int *start_stripe)
+u64 fiemap_calc_fm_end_offset(struct ll_user_fiemap *fiemap,
+				   struct lov_stripe_md *lsm, u64 fm_start,
+				   u64 fm_end, int *start_stripe)
 {
-	obd_size local_end = fiemap->fm_extents[0].fe_logical;
-	obd_off lun_start, lun_end;
-	obd_size fm_end_offset;
+	u64 local_end = fiemap->fm_extents[0].fe_logical;
+	u64 lun_start, lun_end;
+	u64 fm_end_offset;
 	int stripe_no = -1, i;
 
 	if (fiemap->fm_extent_count == 0 ||
@@ -2137,12 +1663,12 @@ obd_size fiemap_calc_fm_end_offset(struct ll_user_fiemap *fiemap,
  *
  * \retval last_stripe return the last stripe of the mapping
  */
-int fiemap_calc_last_stripe(struct lov_stripe_md *lsm, obd_size fm_start,
-			    obd_size fm_end, int start_stripe,
+int fiemap_calc_last_stripe(struct lov_stripe_md *lsm, u64 fm_start,
+			    u64 fm_end, int start_stripe,
 			    int *stripe_count)
 {
 	int last_stripe;
-	obd_off obd_start, obd_end;
+	u64 obd_start, obd_end;
 	int i, j;
 
 	if (fm_end - fm_start > lsm->lsm_stripe_size * lsm->lsm_stripe_count) {
@@ -2206,8 +1732,8 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 	int count_local;
 	unsigned int get_num_extents = 0;
 	int ost_index = 0, actual_start_stripe, start_stripe;
-	obd_size fm_start, fm_end, fm_length, fm_end_offset;
-	obd_size curr_loc;
+	u64 fm_start, fm_end, fm_length, fm_end_offset;
+	u64 curr_loc;
 	int current_extent = 0, rc = 0, i;
 	int ost_eof = 0; /* EOF for object */
 	int ost_done = 0; /* done with required mapping for this OST? */
@@ -2215,15 +1741,19 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 	int cur_stripe = 0, cur_stripe_wrap = 0, stripe_count;
 	unsigned int buffer_size = FIEMAP_BUFFER_SIZE;
 
-	if (!lsm_has_objects(lsm))
-		GOTO(out, rc = 0);
+	if (!lsm_has_objects(lsm)) {
+		rc = 0;
+		goto out;
+	}
 
 	if (fiemap_count_to_size(fm_key->fiemap.fm_extent_count) < buffer_size)
 		buffer_size = fiemap_count_to_size(fm_key->fiemap.fm_extent_count);
 
 	OBD_ALLOC_LARGE(fm_local, buffer_size);
-	if (fm_local == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (fm_local == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 	lcl_fm_ext = &fm_local->fm_extents[0];
 
 	count_local = fiemap_size_to_count(buffer_size);
@@ -2244,8 +1774,10 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 
 	fm_end_offset = fiemap_calc_fm_end_offset(fiemap, lsm, fm_start,
 						  fm_end, &start_stripe);
-	if (fm_end_offset == -EINVAL)
-		GOTO(out, rc = -EINVAL);
+	if (fm_end_offset == -EINVAL) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	if (fiemap_count_to_size(fiemap->fm_extent_count) > *vallen)
 		fiemap->fm_extent_count = fiemap_size_to_count(*vallen);
@@ -2256,9 +1788,9 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 	/* Check each stripe */
 	for (cur_stripe = start_stripe, i = 0; i < stripe_count;
 	     i++, cur_stripe = (cur_stripe + 1) % lsm->lsm_stripe_count) {
-		obd_size req_fm_len; /* Stores length of required mapping */
-		obd_size len_mapped_single_call;
-		obd_off lun_start, lun_end, obd_object_end;
+		u64 req_fm_len; /* Stores length of required mapping */
+		u64 len_mapped_single_call;
+		u64 lun_start, lun_end, obd_object_end;
 		unsigned int ext_count;
 
 		cur_stripe_wrap = cur_stripe;
@@ -2314,8 +1846,11 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 			fm_key->oa.o_oi = lsm->lsm_oinfo[cur_stripe]->loi_oi;
 			ost_index = lsm->lsm_oinfo[cur_stripe]->loi_ost_idx;
 
-			if (ost_index < 0 || ost_index >=lov->desc.ld_tgt_count)
-				GOTO(out, rc = -EINVAL);
+			if (ost_index < 0 ||
+			    ost_index >= lov->desc.ld_tgt_count) {
+				rc = -EINVAL;
+				goto out;
+			}
 
 			/* If OST is inactive, return extent with UNKNOWN flag */
 			if (!lov->lov_tgts[ost_index]->ltd_active) {
@@ -2338,7 +1873,7 @@ static int lov_fiemap(struct lov_obd *lov, __u32 keylen, void *key,
 					  lov->lov_tgts[ost_index]->ltd_exp,
 					  keylen, key, vallen, fm_local, lsm);
 			if (rc != 0)
-				GOTO(out, rc);
+				goto out;
 
 inactive_tgt:
 			ext_count = fm_local->fm_mapped_extents;
@@ -2441,8 +1976,10 @@ static int lov_get_info(const struct lu_env *env, struct obd_export *exp,
 		struct lov_oinfo *loi;
 		__u32 *stripe = val;
 
-		if (*vallen < sizeof(*stripe))
-			GOTO(out, rc = -EFAULT);
+		if (*vallen < sizeof(*stripe)) {
+			rc = -EFAULT;
+			goto out;
+		}
 		*vallen = sizeof(*stripe);
 
 		/* XXX This is another one of those bits that will need to
@@ -2459,34 +1996,40 @@ static int lov_get_info(const struct lu_env *env, struct obd_export *exp,
 			    data->lock->l_conn_export &&
 			    ostid_res_name_eq(&loi->loi_oi, res_id)) {
 				*stripe = i;
-				GOTO(out, rc = 0);
+				rc = 0;
+				goto out;
 			}
 		}
 		LDLM_ERROR(data->lock, "lock on inode without such object");
 		dump_lsm(D_ERROR, lsm);
-		GOTO(out, rc = -ENXIO);
+		rc = -ENXIO;
+		goto out;
 	} else if (KEY_IS(KEY_LAST_ID)) {
 		struct obd_id_info *info = val;
-		__u32 size = sizeof(obd_id);
+		__u32 size = sizeof(u64);
 		struct lov_tgt_desc *tgt;
 
 		LASSERT(*vallen == sizeof(struct obd_id_info));
 		tgt = lov->lov_tgts[info->idx];
 
-		if (!tgt || !tgt->ltd_active)
-			GOTO(out, rc = -ESRCH);
+		if (!tgt || !tgt->ltd_active) {
+			rc = -ESRCH;
+			goto out;
+		}
 
 		rc = obd_get_info(env, tgt->ltd_exp, keylen, key,
 				  &size, info->data, NULL);
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	} else if (KEY_IS(KEY_LOVDESC)) {
 		struct lov_desc *desc_ret = val;
 		*desc_ret = lov->desc;
 
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	} else if (KEY_IS(KEY_FIEMAP)) {
 		rc = lov_fiemap(lov, keylen, key, vallen, val, lsm);
-		GOTO(out, rc);
+		goto out;
 	} else if (KEY_IS(KEY_CONNECT_FLAG)) {
 		struct lov_tgt_desc *tgt;
 		__u64 ost_idx = *((__u64 *)val);
@@ -2495,14 +2038,18 @@ static int lov_get_info(const struct lu_env *env, struct obd_export *exp,
 		LASSERT(ost_idx < lov->desc.ld_tgt_count);
 		tgt = lov->lov_tgts[ost_idx];
 
-		if (!tgt || !tgt->ltd_exp)
-			GOTO(out, rc = -ESRCH);
+		if (!tgt || !tgt->ltd_exp) {
+			rc = -ESRCH;
+			goto out;
+		}
 
 		*((__u64 *)val) = exp_connect_flags(tgt->ltd_exp);
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	} else if (KEY_IS(KEY_TGT_COUNT)) {
 		*((int *)val) = lov->desc.ld_tgt_count;
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 
 	rc = -EINVAL;
@@ -2513,12 +2060,12 @@ out:
 }
 
 static int lov_set_info_async(const struct lu_env *env, struct obd_export *exp,
-			      obd_count keylen, void *key, obd_count vallen,
+			      u32 keylen, void *key, u32 vallen,
 			      void *val, struct ptlrpc_request_set *set)
 {
 	struct obd_device *obddev = class_exp2obd(exp);
 	struct lov_obd *lov = &obddev->u.lov;
-	obd_count count;
+	u32 count;
 	int i, rc = 0, err;
 	struct lov_tgt_desc *tgt;
 	unsigned incr, check_uuid,
@@ -2538,7 +2085,7 @@ static int lov_set_info_async(const struct lu_env *env, struct obd_export *exp,
 
 	if (KEY_IS(KEY_NEXT_ID)) {
 		count = vallen / sizeof(struct obd_id_info);
-		vallen = sizeof(obd_id);
+		vallen = sizeof(u64);
 		incr = sizeof(struct obd_id_info);
 		do_inactive = 1;
 		next_id = 1;
@@ -2625,30 +2172,8 @@ static int lov_set_info_async(const struct lu_env *env, struct obd_export *exp,
 	return rc;
 }
 
-static int lov_extent_calc(struct obd_export *exp, struct lov_stripe_md *lsm,
-			   int cmd, __u64 *offset)
-{
-	__u32 ssize = lsm->lsm_stripe_size;
-	__u64 start;
-
-	start = *offset;
-	lov_do_div64(start, ssize);
-	start = start * ssize;
-
-	CDEBUG(D_DLMTRACE, "offset %llu, stripe %u, start %llu, end %llu\n",
-	       *offset, ssize, start, start + ssize - 1);
-	if (cmd == OBD_CALC_STRIPE_END) {
-		*offset = start + ssize - 1;
-	} else if (cmd == OBD_CALC_STRIPE_START) {
-		*offset = start;
-	} else {
-		LBUG();
-	}
-
-	return 0;
-}
-
 void lov_stripe_lock(struct lov_stripe_md *md)
+		__acquires(&md->lsm_lock)
 {
 	LASSERT(md->lsm_lock_owner != current_pid());
 	spin_lock(&md->lsm_lock);
@@ -2658,6 +2183,7 @@ void lov_stripe_lock(struct lov_stripe_md *md)
 EXPORT_SYMBOL(lov_stripe_lock);
 
 void lov_stripe_unlock(struct lov_stripe_md *md)
+		__releases(&md->lsm_lock)
 {
 	LASSERT(md->lsm_lock_owner == current_pid());
 	md->lsm_lock_owner = 0;
@@ -2783,24 +2309,13 @@ struct obd_ops lov_obd_ops = {
 	.o_unpackmd	    = lov_unpackmd,
 	.o_create	      = lov_create,
 	.o_destroy	     = lov_destroy,
-	.o_getattr	     = lov_getattr,
 	.o_getattr_async       = lov_getattr_async,
-	.o_setattr	     = lov_setattr,
 	.o_setattr_async       = lov_setattr_async,
-	.o_brw		 = lov_brw,
-	.o_merge_lvb	   = lov_merge_lvb,
 	.o_adjust_kms	  = lov_adjust_kms,
-	.o_punch	       = lov_punch,
-	.o_sync		= lov_sync,
-	.o_enqueue	     = lov_enqueue,
-	.o_change_cbdata       = lov_change_cbdata,
 	.o_find_cbdata	 = lov_find_cbdata,
-	.o_cancel	      = lov_cancel,
-	.o_cancel_unused       = lov_cancel_unused,
 	.o_iocontrol	   = lov_iocontrol,
 	.o_get_info	    = lov_get_info,
 	.o_set_info_async      = lov_set_info_async,
-	.o_extent_calc	 = lov_extent_calc,
 	.o_notify	      = lov_notify,
 	.o_pool_new	    = lov_pool_new,
 	.o_pool_rem	    = lov_pool_remove,
diff --git a/drivers/staging/lustre/lustre/lov/lov_object.c b/drivers/staging/lustre/lustre/lov/lov_object.c
index 992c80ab3d53..4cab730ab429 100644
--- a/drivers/staging/lustre/lustre/lov/lov_object.c
+++ b/drivers/staging/lustre/lustre/lov/lov_object.c
@@ -233,7 +233,7 @@ static int lov_init_raid0(const struct lu_env *env,
 			result = ostid_to_fid(ofid, &oinfo->loi_oi,
 					      oinfo->loi_ost_idx);
 			if (result != 0)
-				GOTO(out, result);
+				goto out;
 
 			subdev = lovsub2cl_dev(dev->ld_target[ost_idx]);
 			subconf->u.coc_oinfo = oinfo;
@@ -747,7 +747,8 @@ static int lov_conf_set(const struct lu_env *env, struct cl_object *obj,
 	lov_conf_lock(lov);
 	if (conf->coc_opc == OBJECT_CONF_INVALIDATE) {
 		lov->lo_layout_invalid = true;
-		GOTO(out, result = 0);
+		result = 0;
+		goto out;
 	}
 
 	if (conf->coc_opc == OBJECT_CONF_WAIT) {
@@ -757,7 +758,7 @@ static int lov_conf_set(const struct lu_env *env, struct cl_object *obj,
 			result = lov_layout_wait(env, lov);
 			lov_conf_lock(lov);
 		}
-		GOTO(out, result);
+		goto out;
 	}
 
 	LASSERT(conf->coc_opc == OBJECT_CONF_SET);
@@ -770,13 +771,15 @@ static int lov_conf_set(const struct lu_env *env, struct cl_object *obj,
 	     (lov->lo_lsm->lsm_pattern == lsm->lsm_pattern))) {
 		/* same version of layout */
 		lov->lo_layout_invalid = false;
-		GOTO(out, result = 0);
+		result = 0;
+		goto out;
 	}
 
 	/* will change layout - check if there still exists active IO. */
 	if (atomic_read(&lov->lo_active_ios) > 0) {
 		lov->lo_layout_invalid = true;
-		GOTO(out, result = -EBUSY);
+		result = -EBUSY;
+		goto out;
 	}
 
 	lov->lo_layout_invalid = lov_layout_change(env, lov, conf);
diff --git a/drivers/staging/lustre/lustre/lov/lov_offset.c b/drivers/staging/lustre/lustre/lov/lov_offset.c
index 8e1c3bacc0a0..9c8c77c05a8a 100644
--- a/drivers/staging/lustre/lustre/lov/lov_offset.c
+++ b/drivers/staging/lustre/lustre/lov/lov_offset.c
@@ -43,13 +43,13 @@
 #include "lov_internal.h"
 
 /* compute object size given "stripeno" and the ost size */
-obd_size lov_stripe_size(struct lov_stripe_md *lsm, obd_size ost_size,
+u64 lov_stripe_size(struct lov_stripe_md *lsm, u64 ost_size,
 			 int stripeno)
 {
 	unsigned long ssize = lsm->lsm_stripe_size;
 	unsigned long stripe_size;
-	obd_off swidth;
-	obd_size lov_size;
+	u64 swidth;
+	u64 lov_size;
 	int magic = lsm->lsm_magic;
 
 	if (ost_size == 0)
@@ -116,11 +116,11 @@ obd_size lov_stripe_size(struct lov_stripe_md *lsm, obd_size ost_size,
  * was moved forward to the start of the stripe in question;  0 when it
  * falls in the stripe and no shifting was done; > 0 when the offset
  * was outside the stripe and was pulled back to its final byte. */
-int lov_stripe_offset(struct lov_stripe_md *lsm, obd_off lov_off,
-		      int stripeno, obd_off *obdoff)
+int lov_stripe_offset(struct lov_stripe_md *lsm, u64 lov_off,
+		      int stripeno, u64 *obdoff)
 {
 	unsigned long ssize  = lsm->lsm_stripe_size;
-	obd_off stripe_off, this_stripe, swidth;
+	u64 stripe_off, this_stripe, swidth;
 	int magic = lsm->lsm_magic;
 	int ret = 0;
 
@@ -137,7 +137,7 @@ int lov_stripe_offset(struct lov_stripe_md *lsm, obd_off lov_off,
 	/* lov_do_div64(a, b) returns a % b, and a = a / b */
 	stripe_off = lov_do_div64(lov_off, swidth);
 
-	this_stripe = (obd_off)stripeno * ssize;
+	this_stripe = (u64)stripeno * ssize;
 	if (stripe_off < this_stripe) {
 		stripe_off = 0;
 		ret = -1;
@@ -173,11 +173,11 @@ int lov_stripe_offset(struct lov_stripe_md *lsm, obd_off lov_off,
  * |    0    |     1     |     2     |    0    |     1     |     2     |
  * ---------------------------------------------------------------------
  */
-obd_off lov_size_to_stripe(struct lov_stripe_md *lsm, obd_off file_size,
-			   int stripeno)
+u64 lov_size_to_stripe(struct lov_stripe_md *lsm, u64 file_size,
+		       int stripeno)
 {
 	unsigned long ssize  = lsm->lsm_stripe_size;
-	obd_off stripe_off, this_stripe, swidth;
+	u64 stripe_off, this_stripe, swidth;
 	int magic = lsm->lsm_magic;
 
 	if (file_size == OBD_OBJECT_EOF)
@@ -190,7 +190,7 @@ obd_off lov_size_to_stripe(struct lov_stripe_md *lsm, obd_off file_size,
 	/* lov_do_div64(a, b) returns a % b, and a = a / b */
 	stripe_off = lov_do_div64(file_size, swidth);
 
-	this_stripe = (obd_off)stripeno * ssize;
+	this_stripe = (u64)stripeno * ssize;
 	if (stripe_off < this_stripe) {
 		/* Move to end of previous stripe, or zero */
 		if (file_size > 0) {
@@ -215,8 +215,7 @@ obd_off lov_size_to_stripe(struct lov_stripe_md *lsm, obd_off file_size,
  * that is contained within the lov extent.  this returns true if the given
  * stripe does intersect with the lov extent. */
 int lov_stripe_intersects(struct lov_stripe_md *lsm, int stripeno,
-			  obd_off start, obd_off end,
-			  obd_off *obd_start, obd_off *obd_end)
+			  u64 start, u64 end, u64 *obd_start, u64 *obd_end)
 {
 	int start_side, end_side;
 
@@ -247,10 +246,10 @@ int lov_stripe_intersects(struct lov_stripe_md *lsm, int stripeno,
 }
 
 /* compute which stripe number "lov_off" will be written into */
-int lov_stripe_number(struct lov_stripe_md *lsm, obd_off lov_off)
+int lov_stripe_number(struct lov_stripe_md *lsm, u64 lov_off)
 {
 	unsigned long ssize  = lsm->lsm_stripe_size;
-	obd_off stripe_off, swidth;
+	u64 stripe_off, swidth;
 	int magic = lsm->lsm_magic;
 
 	LASSERT(lsm_op_find(magic) != NULL);
diff --git a/drivers/staging/lustre/lustre/lov/lov_pack.c b/drivers/staging/lustre/lustre/lov/lov_pack.c
index a5b190f32c0f..5edd6a3a9c54 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pack.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pack.c
@@ -95,7 +95,7 @@ void lov_dump_lmm_v1(int level, struct lov_mds_md_v1 *lmm)
 void lov_dump_lmm_v3(int level, struct lov_mds_md_v3 *lmm)
 {
 	lov_dump_lmm_common(level, lmm);
-	CDEBUG(level,"pool_name "LOV_POOLNAMEF"\n", lmm->lmm_pool_name);
+	CDEBUG(level, "pool_name "LOV_POOLNAMEF"\n", lmm->lmm_pool_name);
 	lov_dump_lmm_objects(level, lmm->lmm_objects,
 			     le16_to_cpu(lmm->lmm_stripe_count));
 }
@@ -125,7 +125,7 @@ void lov_dump_lmm(int level, void *lmm)
  *
  * XXX In the future, this will be enhanced to get the EA size from the
  *     underlying OSC device(s) to get their EA sizes so we can stack
- *     LOVs properly.  For now lov_mds_md_size() just assumes one obd_id
+ *     LOVs properly.  For now lov_mds_md_size() just assumes one u64
  *     per stripe.
  */
 int lov_packmd(struct obd_export *exp, struct lov_mds_md **lmmp,
@@ -403,178 +403,6 @@ int lov_unpackmd(struct obd_export *exp,  struct lov_stripe_md **lsmp,
 	return lsm_size;
 }
 
-static int __lov_setstripe(struct obd_export *exp, int max_lmm_size,
-			   struct lov_stripe_md **lsmp,
-			   struct lov_user_md *lump)
-{
-	struct obd_device *obd = class_exp2obd(exp);
-	struct lov_obd *lov = &obd->u.lov;
-	char buffer[sizeof(struct lov_user_md_v3)];
-	struct lov_user_md_v3 *lumv3 = (struct lov_user_md_v3 *)&buffer[0];
-	struct lov_user_md_v1 *lumv1 = (struct lov_user_md_v1 *)&buffer[0];
-	int lmm_magic;
-	__u16 stripe_count;
-	int rc;
-	int cplen = 0;
-
-	rc = lov_lum_swab_if_needed(lumv3, &lmm_magic, lump);
-	if (rc)
-		return rc;
-
-	/* in the rest of the tests, as *lumv1 and lumv3 have the same
-	 * fields, we use lumv1 to avoid code duplication */
-
-	if (lumv1->lmm_pattern == 0) {
-		lumv1->lmm_pattern = lov->desc.ld_pattern ?
-			lov->desc.ld_pattern : LOV_PATTERN_RAID0;
-	}
-
-	if (lov_pattern(lumv1->lmm_pattern) != LOV_PATTERN_RAID0) {
-		CDEBUG(D_IOCTL, "bad userland stripe pattern: %#x\n",
-		       lumv1->lmm_pattern);
-		return -EINVAL;
-	}
-
-	/* 64kB is the largest common page size we see (ia64), and matches the
-	 * check in lfs */
-	if (lumv1->lmm_stripe_size & (LOV_MIN_STRIPE_SIZE - 1)) {
-		CDEBUG(D_IOCTL, "stripe size %u not multiple of %u, fixing\n",
-		       lumv1->lmm_stripe_size, LOV_MIN_STRIPE_SIZE);
-		lumv1->lmm_stripe_size = LOV_MIN_STRIPE_SIZE;
-	}
-
-	if ((lumv1->lmm_stripe_offset >= lov->desc.ld_tgt_count) &&
-	    (lumv1->lmm_stripe_offset !=
-	     (typeof(lumv1->lmm_stripe_offset))(-1))) {
-		CDEBUG(D_IOCTL, "stripe offset %u > number of OSTs %u\n",
-		       lumv1->lmm_stripe_offset, lov->desc.ld_tgt_count);
-		return -EINVAL;
-	}
-	stripe_count = lov_get_stripecnt(lov, lmm_magic,
-					 lumv1->lmm_stripe_count);
-
-	if (max_lmm_size) {
-		int max_stripes = (max_lmm_size -
-				   lov_mds_md_size(0, lmm_magic)) /
-				   sizeof(struct lov_ost_data_v1);
-		if (unlikely(max_stripes < stripe_count)) {
-			CDEBUG(D_IOCTL, "stripe count reset from %d to %d\n",
-			       stripe_count, max_stripes);
-			stripe_count = max_stripes;
-		}
-	}
-
-	if (lmm_magic == LOV_USER_MAGIC_V3) {
-		struct pool_desc *pool;
-
-		/* In the function below, .hs_keycmp resolves to
-		 * pool_hashkey_keycmp() */
-		/* coverity[overrun-buffer-val] */
-		pool = lov_find_pool(lov, lumv3->lmm_pool_name);
-		if (pool != NULL) {
-			if (lumv3->lmm_stripe_offset !=
-			    (typeof(lumv3->lmm_stripe_offset))(-1)) {
-				rc = lov_check_index_in_pool(
-					lumv3->lmm_stripe_offset, pool);
-				if (rc < 0) {
-					lov_pool_putref(pool);
-					return -EINVAL;
-				}
-			}
-
-			if (stripe_count > pool_tgt_count(pool))
-				stripe_count = pool_tgt_count(pool);
-
-			lov_pool_putref(pool);
-		}
-	}
-
-	if (lumv1->lmm_pattern & LOV_PATTERN_F_RELEASED)
-		stripe_count = 0;
-
-	rc = lov_alloc_memmd(lsmp, stripe_count, lumv1->lmm_pattern, lmm_magic);
-
-	if (rc >= 0) {
-		(*lsmp)->lsm_oinfo[0]->loi_ost_idx = lumv1->lmm_stripe_offset;
-		(*lsmp)->lsm_stripe_size = lumv1->lmm_stripe_size;
-		if (lmm_magic == LOV_USER_MAGIC_V3) {
-			cplen = strlcpy((*lsmp)->lsm_pool_name,
-					lumv3->lmm_pool_name,
-					sizeof((*lsmp)->lsm_pool_name));
-			if (cplen >= sizeof((*lsmp)->lsm_pool_name))
-				rc = -E2BIG;
-		}
-		rc = 0;
-	}
-
-	return rc;
-}
-
-/* Configure object striping information on a new file.
- *
- * @lmmu is a pointer to a user struct with one or more of the fields set to
- * indicate the application preference: lmm_stripe_count, lmm_stripe_size,
- * lmm_stripe_offset, and lmm_stripe_pattern.  lmm_magic must be LOV_MAGIC.
- * @lsmp is a pointer to an in-core stripe MD that needs to be filled in.
- */
-int lov_setstripe(struct obd_export *exp, int max_lmm_size,
-		  struct lov_stripe_md **lsmp, struct lov_user_md *lump)
-{
-	int rc;
-	mm_segment_t seg;
-
-	seg = get_fs();
-	set_fs(KERNEL_DS);
-
-	rc = __lov_setstripe(exp, max_lmm_size, lsmp, lump);
-	set_fs(seg);
-	return rc;
-}
-
-int lov_setea(struct obd_export *exp, struct lov_stripe_md **lsmp,
-	      struct lov_user_md *lump)
-{
-	int i;
-	int rc;
-	struct obd_export *oexp;
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-	obd_id last_id = 0;
-	struct lov_user_ost_data_v1 *lmm_objects;
-
-	if (lump->lmm_magic == LOV_USER_MAGIC_V3)
-		lmm_objects = ((struct lov_user_md_v3 *)lump)->lmm_objects;
-	else
-		lmm_objects = lump->lmm_objects;
-
-	for (i = 0; i < lump->lmm_stripe_count; i++) {
-		__u32 len = sizeof(last_id);
-		oexp = lov->lov_tgts[lmm_objects[i].l_ost_idx]->ltd_exp;
-		rc = obd_get_info(NULL, oexp, sizeof(KEY_LAST_ID), KEY_LAST_ID,
-				  &len, &last_id, NULL);
-		if (rc)
-			return rc;
-		if (ostid_id(&lmm_objects[i].l_ost_oi) > last_id) {
-			CERROR("Setting EA for object > than last id on"
-			       " ost idx %d "DOSTID" > %lld \n",
-			       lmm_objects[i].l_ost_idx,
-			       POSTID(&lmm_objects[i].l_ost_oi), last_id);
-			return -EINVAL;
-		}
-	}
-
-	rc = lov_setstripe(exp, 0, lsmp, lump);
-	if (rc)
-		return rc;
-
-	for (i = 0; i < lump->lmm_stripe_count; i++) {
-		(*lsmp)->lsm_oinfo[i]->loi_ost_idx =
-			lmm_objects[i].l_ost_idx;
-		(*lsmp)->lsm_oinfo[i]->loi_oi = lmm_objects[i].l_ost_oi;
-	}
-	return 0;
-}
-
-
 /* Retrieve object striping information.
  *
  * @lump is a pointer to an in-core struct with lmm_ost_count indicating
@@ -607,22 +435,27 @@ int lov_getstripe(struct obd_export *exp, struct lov_stripe_md *lsm,
 	/* we only need the header part from user space to get lmm_magic and
 	 * lmm_stripe_count, (the header part is common to v1 and v3) */
 	lum_size = sizeof(struct lov_user_md_v1);
-	if (copy_from_user(&lum, lump, lum_size))
-		GOTO(out_set, rc = -EFAULT);
+	if (copy_from_user(&lum, lump, lum_size)) {
+		rc = -EFAULT;
+		goto out_set;
+	}
 	else if ((lum.lmm_magic != LOV_USER_MAGIC) &&
-		 (lum.lmm_magic != LOV_USER_MAGIC_V3))
-		GOTO(out_set, rc = -EINVAL);
+		 (lum.lmm_magic != LOV_USER_MAGIC_V3)) {
+		rc = -EINVAL;
+		goto out_set;
+	}
 
 	if (lum.lmm_stripe_count &&
 	    (lum.lmm_stripe_count < lsm->lsm_stripe_count)) {
 		/* Return right size of stripe to user */
 		lum.lmm_stripe_count = lsm->lsm_stripe_count;
 		rc = copy_to_user(lump, &lum, lum_size);
-		GOTO(out_set, rc = -EOVERFLOW);
+		rc = -EOVERFLOW;
+		goto out_set;
 	}
 	rc = lov_packmd(exp, &lmmk, lsm);
 	if (rc < 0)
-		GOTO(out_set, rc);
+		goto out_set;
 	lmm_size = rc;
 	rc = 0;
 
@@ -657,8 +490,10 @@ int lov_getstripe(struct obd_export *exp, struct lov_stripe_md *lsm,
 	/* User wasn't expecting this many OST entries */
 	if (lum.lmm_stripe_count == 0)
 		lmm_size = lum_size;
-	else if (lum.lmm_stripe_count < lmmk->lmm_stripe_count)
-		GOTO(out_set, rc = -EOVERFLOW);
+	else if (lum.lmm_stripe_count < lmmk->lmm_stripe_count) {
+		rc = -EOVERFLOW;
+		goto out_set;
+	}
 	/*
 	 * Have a difference between lov_mds_md & lov_user_md.
 	 * So we have to re-order the data before copy to user.
diff --git a/drivers/staging/lustre/lustre/lov/lov_page.c b/drivers/staging/lustre/lustre/lov/lov_page.c
index 674e61781c20..c4596e8e5783 100644
--- a/drivers/staging/lustre/lustre/lov/lov_page.c
+++ b/drivers/staging/lustre/lustre/lov/lov_page.c
@@ -165,7 +165,7 @@ int lov_page_init_raid0(const struct lu_env *env, struct cl_object *obj,
 	struct lov_io_sub *sub;
 	struct lov_page   *lpg = cl_object_page_slice(obj, page);
 	loff_t	     offset;
-	obd_off	    suboff;
+	u64	    suboff;
 	int		stripe;
 	int		rc;
 
@@ -180,15 +180,19 @@ int lov_page_init_raid0(const struct lu_env *env, struct cl_object *obj,
 	cl_page_slice_add(page, &lpg->lps_cl, obj, &lov_page_ops);
 
 	sub = lov_sub_get(env, lio, stripe);
-	if (IS_ERR(sub))
-		GOTO(out, rc = PTR_ERR(sub));
+	if (IS_ERR(sub)) {
+		rc = PTR_ERR(sub);
+		goto out;
+	}
 
 	subobj = lovsub2cl(r0->lo_sub[stripe]);
 	subpage = cl_page_find_sub(sub->sub_env, subobj,
 				   cl_index(subobj, suboff), vmpage, page);
 	lov_sub_put(sub);
-	if (IS_ERR(subpage))
-		GOTO(out, rc = PTR_ERR(subpage));
+	if (IS_ERR(subpage)) {
+		rc = PTR_ERR(subpage);
+		goto out;
+	}
 
 	if (likely(subpage->cp_parent == page)) {
 		lu_ref_add(&subpage->cp_reference, "lov", page);
diff --git a/drivers/staging/lustre/lustre/lov/lov_pool.c b/drivers/staging/lustre/lustre/lov/lov_pool.c
index 91b3509a8083..0e0ea60de4c8 100644
--- a/drivers/staging/lustre/lustre/lov/lov_pool.c
+++ b/drivers/staging/lustre/lustre/lov/lov_pool.c
@@ -107,7 +107,7 @@ static void *pool_key(struct hlist_node *hnode)
 	struct pool_desc *pool;
 
 	pool = hlist_entry(hnode, struct pool_desc, pool_hash);
-	return (pool->pool_name);
+	return pool->pool_name;
 }
 
 static int pool_hashkey_keycmp(const void *key, struct hlist_node *compared_hnode)
@@ -367,12 +367,14 @@ int lov_ost_pool_add(struct ost_pool *op, __u32 idx, unsigned int min_count)
 
 	rc = lov_ost_pool_extend(op, min_count);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* search ost in pool array */
 	for (i = 0; i < op->op_count; i++) {
-		if (op->op_array[i] == idx)
-			GOTO(out, rc = -EEXIST);
+		if (op->op_array[i] == idx) {
+			rc = -EEXIST;
+			goto out;
+		}
 	}
 	/* ost not found we add it */
 	op->op_array[op->op_count] = idx;
@@ -443,12 +445,12 @@ int lov_pool_new(struct obd_device *obd, char *poolname)
 	atomic_set(&new_pool->pool_refcount, 1);
 	rc = lov_ost_pool_init(&new_pool->pool_obds, 0);
 	if (rc)
-	       GOTO(out_err, rc);
+		goto out_err;
 
 	memset(&(new_pool->pool_rr), 0, sizeof(struct lov_qos_rr));
 	rc = lov_ost_pool_init(&new_pool->pool_rr.lqr_pool, 0);
 	if (rc)
-		GOTO(out_free_pool_obds, rc);
+		goto out_free_pool_obds;
 
 	INIT_HLIST_NODE(&new_pool->pool_hash);
 
@@ -475,8 +477,10 @@ int lov_pool_new(struct obd_device *obd, char *poolname)
 	/* add to find only when it fully ready  */
 	rc = cfs_hash_add_unique(lov->lov_pools_hash_body, poolname,
 				 &new_pool->pool_hash);
-	if (rc)
-		GOTO(out_err, rc = -EEXIST);
+	if (rc) {
+		rc = -EEXIST;
+		goto out_err;
+	}
 
 	CDEBUG(D_CONFIG, LOV_POOLNAMEF" is pool #%d\n",
 	       poolname, lov->lov_pool_count);
@@ -555,12 +559,14 @@ int lov_pool_add(struct obd_device *obd, char *poolname, char *ostname)
 			break;
 	}
 	/* test if ost found in lov */
-	if (lov_idx == lov->desc.ld_tgt_count)
-		GOTO(out, rc = -EINVAL);
+	if (lov_idx == lov->desc.ld_tgt_count) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	rc = lov_ost_pool_add(&pool->pool_obds, lov_idx, lov->lov_tgt_size);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	pool->pool_rr.lqr_dirty = 1;
 
@@ -601,8 +607,10 @@ int lov_pool_remove(struct obd_device *obd, char *poolname, char *ostname)
 	}
 
 	/* test if ost found in lov */
-	if (lov_idx == lov->desc.ld_tgt_count)
-		GOTO(out, rc = -EINVAL);
+	if (lov_idx == lov->desc.ld_tgt_count) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	lov_ost_pool_remove(&pool->pool_obds, lov_idx);
 
@@ -630,8 +638,10 @@ int lov_check_index_in_pool(__u32 idx, struct pool_desc *pool)
 	down_read(&pool_tgt_rw_sem(pool));
 
 	for (i = 0; i < pool_tgt_count(pool); i++) {
-		if (pool_tgt_array(pool)[i] == idx)
-			GOTO(out, rc = 0);
+		if (pool_tgt_array(pool)[i] == idx) {
+			rc = 0;
+			goto out;
+		}
 	}
 	rc = -ENOENT;
 out:
diff --git a/drivers/staging/lustre/lustre/lov/lov_request.c b/drivers/staging/lustre/lustre/lov/lov_request.c
index e4bb02a54b0d..7358b9d29103 100644
--- a/drivers/staging/lustre/lustre/lov/lov_request.c
+++ b/drivers/staging/lustre/lustre/lov/lov_request.c
@@ -39,7 +39,6 @@
 #include "../../include/linux/libcfs/libcfs.h"
 
 #include "../include/obd_class.h"
-#include "../include/obd_ost.h"
 #include "../include/lustre/lustre_idl.h"
 #include "lov_internal.h"
 
@@ -168,14 +167,20 @@ int lov_check_and_wait_active(struct lov_obd *lov, int ost_idx)
 
 	tgt = lov->lov_tgts[ost_idx];
 
-	if (unlikely(tgt == NULL))
-		GOTO(out, rc = 0);
+	if (unlikely(tgt == NULL)) {
+		rc = 0;
+		goto out;
+	}
 
-	if (likely(tgt->ltd_active))
-		GOTO(out, rc = 1);
+	if (likely(tgt->ltd_active)) {
+		rc = 1;
+		goto out;
+	}
 
-	if (tgt->ltd_exp && class_exp2cliimp(tgt->ltd_exp)->imp_connect_tried)
-		GOTO(out, rc = 0);
+	if (tgt->ltd_exp && class_exp2cliimp(tgt->ltd_exp)->imp_connect_tried) {
+		rc = 0;
+		goto out;
+	}
 
 	mutex_unlock(&lov->lov_lock);
 
@@ -194,418 +199,6 @@ out:
 	return rc;
 }
 
-static int lov_update_enqueue_lov(struct obd_export *exp,
-				  struct lustre_handle *lov_lockhp,
-				  struct lov_oinfo *loi, __u64 flags, int idx,
-				  struct ost_id *oi, int rc)
-{
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-
-	if (rc != ELDLM_OK &&
-	    !(rc == ELDLM_LOCK_ABORTED && (flags & LDLM_FL_HAS_INTENT))) {
-		memset(lov_lockhp, 0, sizeof(*lov_lockhp));
-		if (lov->lov_tgts[idx] && lov->lov_tgts[idx]->ltd_active) {
-			/* -EUSERS used by OST to report file contention */
-			if (rc != -EINTR && rc != -EUSERS)
-				CERROR("%s: enqueue objid "DOSTID" subobj"
-				       DOSTID" on OST idx %d: rc %d\n",
-				       exp->exp_obd->obd_name,
-				       POSTID(oi), POSTID(&loi->loi_oi),
-				       loi->loi_ost_idx, rc);
-		} else
-			rc = ELDLM_OK;
-	}
-	return rc;
-}
-
-int lov_update_enqueue_set(struct lov_request *req, __u32 mode, int rc)
-{
-	struct lov_request_set *set = req->rq_rqset;
-	struct lustre_handle *lov_lockhp;
-	struct obd_info *oi = set->set_oi;
-	struct lov_oinfo *loi;
-
-	LASSERT(oi != NULL);
-
-	lov_lockhp = set->set_lockh->llh_handles + req->rq_stripe;
-	loi = oi->oi_md->lsm_oinfo[req->rq_stripe];
-
-	/* XXX LOV STACKING: OSC gets a copy, created in lov_prep_enqueue_set
-	 * and that copy can be arbitrarily out of date.
-	 *
-	 * The LOV API is due for a serious rewriting anyways, and this
-	 * can be addressed then. */
-
-	lov_stripe_lock(oi->oi_md);
-	osc_update_enqueue(lov_lockhp, loi, oi->oi_flags,
-			   &req->rq_oi.oi_md->lsm_oinfo[0]->loi_lvb, mode, rc);
-	if (rc == ELDLM_LOCK_ABORTED && (oi->oi_flags & LDLM_FL_HAS_INTENT))
-		memset(lov_lockhp, 0, sizeof(*lov_lockhp));
-	rc = lov_update_enqueue_lov(set->set_exp, lov_lockhp, loi, oi->oi_flags,
-				    req->rq_idx, &oi->oi_md->lsm_oi, rc);
-	lov_stripe_unlock(oi->oi_md);
-	lov_update_set(set, req, rc);
-	return rc;
-}
-
-/* The callback for osc_enqueue that updates lov info for every OSC request. */
-static int cb_update_enqueue(void *cookie, int rc)
-{
-	struct obd_info *oinfo = cookie;
-	struct ldlm_enqueue_info *einfo;
-	struct lov_request *lovreq;
-
-	lovreq = container_of(oinfo, struct lov_request, rq_oi);
-	einfo = lovreq->rq_rqset->set_ei;
-	return lov_update_enqueue_set(lovreq, einfo->ei_mode, rc);
-}
-
-static int enqueue_done(struct lov_request_set *set, __u32 mode)
-{
-	struct lov_request *req;
-	struct lov_obd *lov = &set->set_exp->exp_obd->u.lov;
-	int completes = atomic_read(&set->set_completes);
-	int rc = 0;
-
-	/* enqueue/match success, just return */
-	if (completes && completes == atomic_read(&set->set_success))
-		return 0;
-
-	/* cancel enqueued/matched locks */
-	list_for_each_entry(req, &set->set_list, rq_link) {
-		struct lustre_handle *lov_lockhp;
-
-		if (!req->rq_complete || req->rq_rc)
-			continue;
-
-		lov_lockhp = set->set_lockh->llh_handles + req->rq_stripe;
-		LASSERT(lov_lockhp);
-		if (!lustre_handle_is_used(lov_lockhp))
-			continue;
-
-		rc = obd_cancel(lov->lov_tgts[req->rq_idx]->ltd_exp,
-				req->rq_oi.oi_md, mode, lov_lockhp);
-		if (rc && lov->lov_tgts[req->rq_idx] &&
-		    lov->lov_tgts[req->rq_idx]->ltd_active)
-			CERROR("%s: cancelling obdjid "DOSTID" on OST"
-			       "idx %d error: rc = %d\n",
-			       set->set_exp->exp_obd->obd_name,
-			       POSTID(&req->rq_oi.oi_md->lsm_oi),
-			       req->rq_idx, rc);
-	}
-	if (set->set_lockh)
-		lov_llh_put(set->set_lockh);
-	return rc;
-}
-
-int lov_fini_enqueue_set(struct lov_request_set *set, __u32 mode, int rc,
-			 struct ptlrpc_request_set *rqset)
-{
-	int ret = 0;
-
-	if (set == NULL)
-		return 0;
-	LASSERT(set->set_exp);
-	/* Do enqueue_done only for sync requests and if any request
-	 * succeeded. */
-	if (!rqset) {
-		if (rc)
-			atomic_set(&set->set_completes, 0);
-		ret = enqueue_done(set, mode);
-	} else if (set->set_lockh)
-		lov_llh_put(set->set_lockh);
-
-	lov_put_reqset(set);
-
-	return rc ? rc : ret;
-}
-
-static void lov_llh_addref(void *llhp)
-{
-	struct lov_lock_handles *llh = llhp;
-
-	atomic_inc(&llh->llh_refcount);
-	CDEBUG(D_INFO, "GETting llh %p : new refcount %d\n", llh,
-	       atomic_read(&llh->llh_refcount));
-}
-
-static struct portals_handle_ops lov_handle_ops = {
-	.hop_addref = lov_llh_addref,
-	.hop_free   = NULL,
-};
-
-static struct lov_lock_handles *lov_llh_new(struct lov_stripe_md *lsm)
-{
-	struct lov_lock_handles *llh;
-
-	OBD_ALLOC(llh, sizeof(*llh) +
-		  sizeof(*llh->llh_handles) * lsm->lsm_stripe_count);
-	if (llh == NULL)
-		return NULL;
-
-	atomic_set(&llh->llh_refcount, 2);
-	llh->llh_stripe_count = lsm->lsm_stripe_count;
-	INIT_LIST_HEAD(&llh->llh_handle.h_link);
-	class_handle_hash(&llh->llh_handle, &lov_handle_ops);
-
-	return llh;
-}
-
-int lov_prep_enqueue_set(struct obd_export *exp, struct obd_info *oinfo,
-			 struct ldlm_enqueue_info *einfo,
-			 struct lov_request_set **reqset)
-{
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-	struct lov_request_set *set;
-	int i, rc = 0;
-
-	OBD_ALLOC(set, sizeof(*set));
-	if (set == NULL)
-		return -ENOMEM;
-	lov_init_set(set);
-
-	set->set_exp = exp;
-	set->set_oi = oinfo;
-	set->set_ei = einfo;
-	set->set_lockh = lov_llh_new(oinfo->oi_md);
-	if (set->set_lockh == NULL)
-		GOTO(out_set, rc = -ENOMEM);
-	oinfo->oi_lockh->cookie = set->set_lockh->llh_handle.h_cookie;
-
-	for (i = 0; i < oinfo->oi_md->lsm_stripe_count; i++) {
-		struct lov_oinfo *loi;
-		struct lov_request *req;
-		obd_off start, end;
-
-		loi = oinfo->oi_md->lsm_oinfo[i];
-		if (!lov_stripe_intersects(oinfo->oi_md, i,
-					   oinfo->oi_policy.l_extent.start,
-					   oinfo->oi_policy.l_extent.end,
-					   &start, &end))
-			continue;
-
-		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
-			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
-			continue;
-		}
-
-		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
-
-		req->rq_buflen = sizeof(*req->rq_oi.oi_md) +
-			sizeof(struct lov_oinfo *) +
-			sizeof(struct lov_oinfo);
-		OBD_ALLOC_LARGE(req->rq_oi.oi_md, req->rq_buflen);
-		if (req->rq_oi.oi_md == NULL) {
-			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
-		}
-		req->rq_oi.oi_md->lsm_oinfo[0] =
-			((void *)req->rq_oi.oi_md) + sizeof(*req->rq_oi.oi_md) +
-			sizeof(struct lov_oinfo *);
-
-		/* Set lov request specific parameters. */
-		req->rq_oi.oi_lockh = set->set_lockh->llh_handles + i;
-		req->rq_oi.oi_cb_up = cb_update_enqueue;
-		req->rq_oi.oi_flags = oinfo->oi_flags;
-
-		LASSERT(req->rq_oi.oi_lockh);
-
-		req->rq_oi.oi_policy.l_extent.gid =
-			oinfo->oi_policy.l_extent.gid;
-		req->rq_oi.oi_policy.l_extent.start = start;
-		req->rq_oi.oi_policy.l_extent.end = end;
-
-		req->rq_idx = loi->loi_ost_idx;
-		req->rq_stripe = i;
-
-		/* XXX LOV STACKING: submd should be from the subobj */
-		req->rq_oi.oi_md->lsm_oi = loi->loi_oi;
-		req->rq_oi.oi_md->lsm_stripe_count = 0;
-		req->rq_oi.oi_md->lsm_oinfo[0]->loi_kms_valid =
-			loi->loi_kms_valid;
-		req->rq_oi.oi_md->lsm_oinfo[0]->loi_kms = loi->loi_kms;
-		req->rq_oi.oi_md->lsm_oinfo[0]->loi_lvb = loi->loi_lvb;
-
-		lov_set_add_req(req, set);
-	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
-	*reqset = set;
-	return 0;
-out_set:
-	lov_fini_enqueue_set(set, einfo->ei_mode, rc, NULL);
-	return rc;
-}
-
-int lov_fini_match_set(struct lov_request_set *set, __u32 mode, __u64 flags)
-{
-	int rc = 0;
-
-	if (set == NULL)
-		return 0;
-	LASSERT(set->set_exp);
-	rc = enqueue_done(set, mode);
-	if ((set->set_count == atomic_read(&set->set_success)) &&
-	    (flags & LDLM_FL_TEST_LOCK))
-		lov_llh_put(set->set_lockh);
-
-	lov_put_reqset(set);
-
-	return rc;
-}
-
-int lov_prep_match_set(struct obd_export *exp, struct obd_info *oinfo,
-		       struct lov_stripe_md *lsm, ldlm_policy_data_t *policy,
-		       __u32 mode, struct lustre_handle *lockh,
-		       struct lov_request_set **reqset)
-{
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-	struct lov_request_set *set;
-	int i, rc = 0;
-
-	OBD_ALLOC(set, sizeof(*set));
-	if (set == NULL)
-		return -ENOMEM;
-	lov_init_set(set);
-
-	set->set_exp = exp;
-	set->set_oi = oinfo;
-	set->set_oi->oi_md = lsm;
-	set->set_lockh = lov_llh_new(lsm);
-	if (set->set_lockh == NULL)
-		GOTO(out_set, rc = -ENOMEM);
-	lockh->cookie = set->set_lockh->llh_handle.h_cookie;
-
-	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		struct lov_oinfo *loi;
-		struct lov_request *req;
-		obd_off start, end;
-
-		loi = lsm->lsm_oinfo[i];
-		if (!lov_stripe_intersects(lsm, i, policy->l_extent.start,
-					   policy->l_extent.end, &start, &end))
-			continue;
-
-		/* FIXME raid1 should grace this error */
-		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
-			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
-			GOTO(out_set, rc = -EIO);
-		}
-
-		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
-
-		req->rq_buflen = sizeof(*req->rq_oi.oi_md);
-		OBD_ALLOC_LARGE(req->rq_oi.oi_md, req->rq_buflen);
-		if (req->rq_oi.oi_md == NULL) {
-			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
-		}
-
-		req->rq_oi.oi_policy.l_extent.start = start;
-		req->rq_oi.oi_policy.l_extent.end = end;
-		req->rq_oi.oi_policy.l_extent.gid = policy->l_extent.gid;
-
-		req->rq_idx = loi->loi_ost_idx;
-		req->rq_stripe = i;
-
-		/* XXX LOV STACKING: submd should be from the subobj */
-		req->rq_oi.oi_md->lsm_oi = loi->loi_oi;
-		req->rq_oi.oi_md->lsm_stripe_count = 0;
-
-		lov_set_add_req(req, set);
-	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
-	*reqset = set;
-	return rc;
-out_set:
-	lov_fini_match_set(set, mode, 0);
-	return rc;
-}
-
-int lov_fini_cancel_set(struct lov_request_set *set)
-{
-	int rc = 0;
-
-	if (set == NULL)
-		return 0;
-
-	LASSERT(set->set_exp);
-	if (set->set_lockh)
-		lov_llh_put(set->set_lockh);
-
-	lov_put_reqset(set);
-
-	return rc;
-}
-
-int lov_prep_cancel_set(struct obd_export *exp, struct obd_info *oinfo,
-			struct lov_stripe_md *lsm, __u32 mode,
-			struct lustre_handle *lockh,
-			struct lov_request_set **reqset)
-{
-	struct lov_request_set *set;
-	int i, rc = 0;
-
-	OBD_ALLOC(set, sizeof(*set));
-	if (set == NULL)
-		return -ENOMEM;
-	lov_init_set(set);
-
-	set->set_exp = exp;
-	set->set_oi = oinfo;
-	set->set_oi->oi_md = lsm;
-	set->set_lockh = lov_handle2llh(lockh);
-	if (set->set_lockh == NULL) {
-		CERROR("LOV: invalid lov lock handle %p\n", lockh);
-		GOTO(out_set, rc = -EINVAL);
-	}
-	lockh->cookie = set->set_lockh->llh_handle.h_cookie;
-
-	for (i = 0; i < lsm->lsm_stripe_count; i++) {
-		struct lov_request *req;
-		struct lustre_handle *lov_lockhp;
-		struct lov_oinfo *loi = lsm->lsm_oinfo[i];
-
-		lov_lockhp = set->set_lockh->llh_handles + i;
-		if (!lustre_handle_is_used(lov_lockhp)) {
-			CDEBUG(D_INFO, "lov idx %d subobj "DOSTID" no lock\n",
-			       loi->loi_ost_idx, POSTID(&loi->loi_oi));
-			continue;
-		}
-
-		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
-
-		req->rq_buflen = sizeof(*req->rq_oi.oi_md);
-		OBD_ALLOC_LARGE(req->rq_oi.oi_md, req->rq_buflen);
-		if (req->rq_oi.oi_md == NULL) {
-			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
-		}
-
-		req->rq_idx = loi->loi_ost_idx;
-		req->rq_stripe = i;
-
-		/* XXX LOV STACKING: submd should be from the subobj */
-		req->rq_oi.oi_md->lsm_oi = loi->loi_oi;
-		req->rq_oi.oi_md->lsm_stripe_count = 0;
-
-		lov_set_add_req(req, set);
-	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
-	*reqset = set;
-	return rc;
-out_set:
-	lov_fini_cancel_set(set);
-	return rc;
-}
 static int common_attr_done(struct lov_request_set *set)
 {
 	struct list_head *pos;
@@ -622,8 +215,10 @@ static int common_attr_done(struct lov_request_set *set)
 		return -EIO;
 
 	OBDO_ALLOC(tmp_oa);
-	if (tmp_oa == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (tmp_oa == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	list_for_each(pos, &set->set_list) {
 		req = list_entry(pos, struct lov_request, rq_link);
@@ -645,7 +240,8 @@ static int common_attr_done(struct lov_request_set *set)
 		/* When we take attributes of some epoch, we require all the
 		 * ost to be active. */
 		CERROR("Not all the stripes had valid attrs\n");
-		GOTO(out, rc = -EIO);
+		rc = -EIO;
+		goto out;
 	}
 
 	tmp_oa->o_oi = set->set_oi->oi_oa->o_oi;
@@ -657,164 +253,6 @@ out:
 
 }
 
-static int brw_done(struct lov_request_set *set)
-{
-	struct lov_stripe_md *lsm = set->set_oi->oi_md;
-	struct lov_oinfo     *loi = NULL;
-	struct list_head *pos;
-	struct lov_request *req;
-
-	list_for_each(pos, &set->set_list) {
-		req = list_entry(pos, struct lov_request, rq_link);
-
-		if (!req->rq_complete || req->rq_rc)
-			continue;
-
-		loi = lsm->lsm_oinfo[req->rq_stripe];
-
-		if (req->rq_oi.oi_oa->o_valid & OBD_MD_FLBLOCKS)
-			loi->loi_lvb.lvb_blocks = req->rq_oi.oi_oa->o_blocks;
-	}
-
-	return 0;
-}
-
-int lov_fini_brw_set(struct lov_request_set *set)
-{
-	int rc = 0;
-
-	if (set == NULL)
-		return 0;
-	LASSERT(set->set_exp);
-	if (atomic_read(&set->set_completes)) {
-		rc = brw_done(set);
-		/* FIXME update qos data here */
-	}
-	lov_put_reqset(set);
-
-	return rc;
-}
-
-int lov_prep_brw_set(struct obd_export *exp, struct obd_info *oinfo,
-		     obd_count oa_bufs, struct brw_page *pga,
-		     struct obd_trans_info *oti,
-		     struct lov_request_set **reqset)
-{
-	struct {
-		obd_count       index;
-		obd_count       count;
-		obd_count       off;
-	} *info = NULL;
-	struct lov_request_set *set;
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-	int rc = 0, i, shift;
-
-	OBD_ALLOC(set, sizeof(*set));
-	if (set == NULL)
-		return -ENOMEM;
-	lov_init_set(set);
-
-	set->set_exp = exp;
-	set->set_oti = oti;
-	set->set_oi = oinfo;
-	set->set_oabufs = oa_bufs;
-	OBD_ALLOC_LARGE(set->set_pga, oa_bufs * sizeof(*set->set_pga));
-	if (!set->set_pga)
-		GOTO(out, rc = -ENOMEM);
-
-	OBD_ALLOC_LARGE(info, sizeof(*info) * oinfo->oi_md->lsm_stripe_count);
-	if (!info)
-		GOTO(out, rc = -ENOMEM);
-
-	/* calculate the page count for each stripe */
-	for (i = 0; i < oa_bufs; i++) {
-		int stripe = lov_stripe_number(oinfo->oi_md, pga[i].off);
-		info[stripe].count++;
-	}
-
-	/* alloc and initialize lov request */
-	shift = 0;
-	for (i = 0; i < oinfo->oi_md->lsm_stripe_count; i++) {
-		struct lov_oinfo *loi = NULL;
-		struct lov_request *req;
-
-		if (info[i].count == 0)
-			continue;
-
-		loi = oinfo->oi_md->lsm_oinfo[i];
-		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
-			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
-			GOTO(out, rc = -EIO);
-		}
-
-		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out, rc = -ENOMEM);
-
-		OBDO_ALLOC(req->rq_oi.oi_oa);
-		if (req->rq_oi.oi_oa == NULL) {
-			OBD_FREE(req, sizeof(*req));
-			GOTO(out, rc = -ENOMEM);
-		}
-
-		if (oinfo->oi_oa) {
-			memcpy(req->rq_oi.oi_oa, oinfo->oi_oa,
-			       sizeof(*req->rq_oi.oi_oa));
-		}
-		req->rq_oi.oi_oa->o_oi = loi->loi_oi;
-		req->rq_oi.oi_oa->o_stripe_idx = i;
-
-		req->rq_buflen = sizeof(*req->rq_oi.oi_md);
-		OBD_ALLOC_LARGE(req->rq_oi.oi_md, req->rq_buflen);
-		if (req->rq_oi.oi_md == NULL) {
-			OBDO_FREE(req->rq_oi.oi_oa);
-			OBD_FREE(req, sizeof(*req));
-			GOTO(out, rc = -ENOMEM);
-		}
-
-		req->rq_idx = loi->loi_ost_idx;
-		req->rq_stripe = i;
-
-		/* XXX LOV STACKING */
-		req->rq_oi.oi_md->lsm_oi = loi->loi_oi;
-		req->rq_oabufs = info[i].count;
-		req->rq_pgaidx = shift;
-		shift += req->rq_oabufs;
-
-		/* remember the index for sort brw_page array */
-		info[i].index = req->rq_pgaidx;
-
-		req->rq_oi.oi_capa = oinfo->oi_capa;
-
-		lov_set_add_req(req, set);
-	}
-	if (!set->set_count)
-		GOTO(out, rc = -EIO);
-
-	/* rotate & sort the brw_page array */
-	for (i = 0; i < oa_bufs; i++) {
-		int stripe = lov_stripe_number(oinfo->oi_md, pga[i].off);
-
-		shift = info[stripe].index + info[stripe].off;
-		LASSERT(shift < oa_bufs);
-		set->set_pga[shift] = pga[i];
-		lov_stripe_offset(oinfo->oi_md, pga[i].off, stripe,
-				  &set->set_pga[shift].off);
-		info[stripe].off++;
-	}
-out:
-	if (info)
-		OBD_FREE_LARGE(info,
-			       sizeof(*info) * oinfo->oi_md->lsm_stripe_count);
-
-	if (rc == 0)
-		*reqset = set;
-	else
-		lov_fini_brw_set(set);
-
-	return rc;
-}
-
 int lov_fini_getattr_set(struct lov_request_set *set)
 {
 	int rc = 0;
@@ -863,15 +301,19 @@ int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 		loi = oinfo->oi_md->lsm_oinfo[i];
 		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
 			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
-			if (oinfo->oi_oa->o_valid & OBD_MD_FLEPOCH)
+			if (oinfo->oi_oa->o_valid & OBD_MD_FLEPOCH) {
 				/* SOM requires all the OSTs to be active. */
-				GOTO(out_set, rc = -EIO);
+				rc = -EIO;
+				goto out_set;
+			}
 			continue;
 		}
 
 		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
+		if (req == NULL) {
+			rc = -ENOMEM;
+			goto out_set;
+		}
 
 		req->rq_stripe = i;
 		req->rq_idx = loi->loi_ost_idx;
@@ -879,7 +321,8 @@ int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 		OBDO_ALLOC(req->rq_oi.oi_oa);
 		if (req->rq_oi.oi_oa == NULL) {
 			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
+			rc = -ENOMEM;
+			goto out_set;
 		}
 		memcpy(req->rq_oi.oi_oa, oinfo->oi_oa,
 		       sizeof(*req->rq_oi.oi_oa));
@@ -889,8 +332,10 @@ int lov_prep_getattr_set(struct obd_export *exp, struct obd_info *oinfo,
 
 		lov_set_add_req(req, set);
 	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
+	if (!set->set_count) {
+		rc = -EIO;
+		goto out_set;
+	}
 	*reqset = set;
 	return rc;
 out_set:
@@ -945,8 +390,10 @@ int lov_prep_destroy_set(struct obd_export *exp, struct obd_info *oinfo,
 		}
 
 		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
+		if (req == NULL) {
+			rc = -ENOMEM;
+			goto out_set;
+		}
 
 		req->rq_stripe = i;
 		req->rq_idx = loi->loi_ost_idx;
@@ -954,14 +401,17 @@ int lov_prep_destroy_set(struct obd_export *exp, struct obd_info *oinfo,
 		OBDO_ALLOC(req->rq_oi.oi_oa);
 		if (req->rq_oi.oi_oa == NULL) {
 			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
+			rc = -ENOMEM;
+			goto out_set;
 		}
 		memcpy(req->rq_oi.oi_oa, src_oa, sizeof(*req->rq_oi.oi_oa));
 		req->rq_oi.oi_oa->o_oi = loi->loi_oi;
 		lov_set_add_req(req, set);
 	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
+	if (!set->set_count) {
+		rc = -EIO;
+		goto out_set;
+	}
 	*reqset = set;
 	return rc;
 out_set:
@@ -1053,15 +503,18 @@ int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 		}
 
 		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
+		if (req == NULL) {
+			rc = -ENOMEM;
+			goto out_set;
+		}
 		req->rq_stripe = i;
 		req->rq_idx = loi->loi_ost_idx;
 
 		OBDO_ALLOC(req->rq_oi.oi_oa);
 		if (req->rq_oi.oi_oa == NULL) {
 			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
+			rc = -ENOMEM;
+			goto out_set;
 		}
 		memcpy(req->rq_oi.oi_oa, oinfo->oi_oa,
 		       sizeof(*req->rq_oi.oi_oa));
@@ -1084,225 +537,14 @@ int lov_prep_setattr_set(struct obd_export *exp, struct obd_info *oinfo,
 		}
 		lov_set_add_req(req, set);
 	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
-	*reqset = set;
-	return rc;
-out_set:
-	lov_fini_setattr_set(set);
-	return rc;
-}
-
-int lov_fini_punch_set(struct lov_request_set *set)
-{
-	int rc = 0;
-
-	if (set == NULL)
-		return 0;
-	LASSERT(set->set_exp);
-	if (atomic_read(&set->set_completes)) {
+	if (!set->set_count) {
 		rc = -EIO;
-		/* FIXME update qos data here */
-		if (atomic_read(&set->set_success))
-			rc = common_attr_done(set);
+		goto out_set;
 	}
-
-	lov_put_reqset(set);
-
-	return rc;
-}
-
-int lov_update_punch_set(struct lov_request_set *set,
-			 struct lov_request *req, int rc)
-{
-	struct lov_obd *lov = &req->rq_rqset->set_exp->exp_obd->u.lov;
-	struct lov_stripe_md *lsm = req->rq_rqset->set_oi->oi_md;
-
-	lov_update_set(set, req, rc);
-
-	/* grace error on inactive ost */
-	if (rc && !lov->lov_tgts[req->rq_idx]->ltd_active)
-		rc = 0;
-
-	if (rc == 0) {
-		lov_stripe_lock(lsm);
-		if (req->rq_oi.oi_oa->o_valid & OBD_MD_FLBLOCKS) {
-			lsm->lsm_oinfo[req->rq_stripe]->loi_lvb.lvb_blocks =
-				req->rq_oi.oi_oa->o_blocks;
-		}
-
-		lov_stripe_unlock(lsm);
-	}
-
-	return rc;
-}
-
-/* The callback for osc_punch that finalizes a request info when a response
- * is received. */
-static int cb_update_punch(void *cookie, int rc)
-{
-	struct obd_info *oinfo = cookie;
-	struct lov_request *lovreq;
-
-	lovreq = container_of(oinfo, struct lov_request, rq_oi);
-	return lov_update_punch_set(lovreq->rq_rqset, lovreq, rc);
-}
-
-int lov_prep_punch_set(struct obd_export *exp, struct obd_info *oinfo,
-		       struct obd_trans_info *oti,
-		       struct lov_request_set **reqset)
-{
-	struct lov_request_set *set;
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-	int rc = 0, i;
-
-	OBD_ALLOC(set, sizeof(*set));
-	if (set == NULL)
-		return -ENOMEM;
-	lov_init_set(set);
-
-	set->set_oi = oinfo;
-	set->set_exp = exp;
-
-	for (i = 0; i < oinfo->oi_md->lsm_stripe_count; i++) {
-		struct lov_oinfo *loi = oinfo->oi_md->lsm_oinfo[i];
-		struct lov_request *req;
-		obd_off rs, re;
-
-		if (!lov_stripe_intersects(oinfo->oi_md, i,
-					   oinfo->oi_policy.l_extent.start,
-					   oinfo->oi_policy.l_extent.end,
-					   &rs, &re))
-			continue;
-
-		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
-			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
-			GOTO(out_set, rc = -EIO);
-		}
-
-		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
-		req->rq_stripe = i;
-		req->rq_idx = loi->loi_ost_idx;
-
-		OBDO_ALLOC(req->rq_oi.oi_oa);
-		if (req->rq_oi.oi_oa == NULL) {
-			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
-		}
-		memcpy(req->rq_oi.oi_oa, oinfo->oi_oa,
-		       sizeof(*req->rq_oi.oi_oa));
-		req->rq_oi.oi_oa->o_oi = loi->loi_oi;
-		req->rq_oi.oi_oa->o_valid |= OBD_MD_FLGROUP;
-
-		req->rq_oi.oi_oa->o_stripe_idx = i;
-		req->rq_oi.oi_cb_up = cb_update_punch;
-
-		req->rq_oi.oi_policy.l_extent.start = rs;
-		req->rq_oi.oi_policy.l_extent.end = re;
-		req->rq_oi.oi_policy.l_extent.gid = -1;
-
-		req->rq_oi.oi_capa = oinfo->oi_capa;
-
-		lov_set_add_req(req, set);
-	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
 	*reqset = set;
 	return rc;
 out_set:
-	lov_fini_punch_set(set);
-	return rc;
-}
-
-int lov_fini_sync_set(struct lov_request_set *set)
-{
-	int rc = 0;
-
-	if (set == NULL)
-		return 0;
-	LASSERT(set->set_exp);
-	if (atomic_read(&set->set_completes)) {
-		if (!atomic_read(&set->set_success))
-			rc = -EIO;
-		/* FIXME update qos data here */
-	}
-
-	lov_put_reqset(set);
-
-	return rc;
-}
-
-/* The callback for osc_sync that finalizes a request info when a
- * response is received. */
-static int cb_sync_update(void *cookie, int rc)
-{
-	struct obd_info *oinfo = cookie;
-	struct lov_request *lovreq;
-
-	lovreq = container_of(oinfo, struct lov_request, rq_oi);
-	return lov_update_common_set(lovreq->rq_rqset, lovreq, rc);
-}
-
-int lov_prep_sync_set(struct obd_export *exp, struct obd_info *oinfo,
-		      obd_off start, obd_off end,
-		      struct lov_request_set **reqset)
-{
-	struct lov_request_set *set;
-	struct lov_obd *lov = &exp->exp_obd->u.lov;
-	int rc = 0, i;
-
-	OBD_ALLOC_PTR(set);
-	if (set == NULL)
-		return -ENOMEM;
-	lov_init_set(set);
-
-	set->set_exp = exp;
-	set->set_oi = oinfo;
-
-	for (i = 0; i < oinfo->oi_md->lsm_stripe_count; i++) {
-		struct lov_oinfo *loi = oinfo->oi_md->lsm_oinfo[i];
-		struct lov_request *req;
-		obd_off rs, re;
-
-		if (!lov_check_and_wait_active(lov, loi->loi_ost_idx)) {
-			CDEBUG(D_HA, "lov idx %d inactive\n", loi->loi_ost_idx);
-			continue;
-		}
-
-		if (!lov_stripe_intersects(oinfo->oi_md, i, start, end, &rs,
-					   &re))
-			continue;
-
-		OBD_ALLOC_PTR(req);
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
-		req->rq_stripe = i;
-		req->rq_idx = loi->loi_ost_idx;
-
-		OBDO_ALLOC(req->rq_oi.oi_oa);
-		if (req->rq_oi.oi_oa == NULL) {
-			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
-		}
-		*req->rq_oi.oi_oa = *oinfo->oi_oa;
-		req->rq_oi.oi_oa->o_oi = loi->loi_oi;
-		req->rq_oi.oi_oa->o_stripe_idx = i;
-
-		req->rq_oi.oi_policy.l_extent.start = rs;
-		req->rq_oi.oi_policy.l_extent.end = re;
-		req->rq_oi.oi_policy.l_extent.gid = -1;
-		req->rq_oi.oi_cb_up = cb_sync_update;
-
-		lov_set_add_req(req, set);
-	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
-	*reqset = set;
-	return rc;
-out_set:
-	lov_fini_sync_set(set);
+	lov_fini_setattr_set(set);
 	return rc;
 }
 
@@ -1315,7 +557,8 @@ out_set:
 			(tot) += (add);				 \
 	} while (0)
 
-int lov_fini_statfs(struct obd_device *obd, struct obd_statfs *osfs,int success)
+int lov_fini_statfs(struct obd_device *obd, struct obd_statfs *osfs,
+		    int success)
 {
 	if (success) {
 		__u32 expected_stripes = lov_get_stripecnt(&obd->u.lov,
@@ -1429,12 +672,12 @@ static int cb_statfs_update(void *cookie, int rc)
 	   lovset->set_exp is not initialized. */
 	lov_update_set(set, lovreq, rc);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	obd_getref(lovobd);
 	tgt = lov->lov_tgts[lovreq->rq_idx];
 	if (!tgt || !tgt->ltd_active)
-		GOTO(out_update, rc);
+		goto out_update;
 
 	tgtobd = class_exp2obd(tgt->ltd_exp);
 	spin_lock(&tgtobd->obd_osfs_lock);
@@ -1491,13 +734,16 @@ int lov_prep_statfs_set(struct obd_device *obd, struct obd_info *oinfo,
 		}
 
 		OBD_ALLOC(req, sizeof(*req));
-		if (req == NULL)
-			GOTO(out_set, rc = -ENOMEM);
+		if (req == NULL) {
+			rc = -ENOMEM;
+			goto out_set;
+		}
 
 		OBD_ALLOC(req->rq_oi.oi_osfs, sizeof(*req->rq_oi.oi_osfs));
 		if (req->rq_oi.oi_osfs == NULL) {
 			OBD_FREE(req, sizeof(*req));
-			GOTO(out_set, rc = -ENOMEM);
+			rc = -ENOMEM;
+			goto out_set;
 		}
 
 		req->rq_idx = i;
@@ -1506,8 +752,10 @@ int lov_prep_statfs_set(struct obd_device *obd, struct obd_info *oinfo,
 
 		lov_set_add_req(req, set);
 	}
-	if (!set->set_count)
-		GOTO(out_set, rc = -EIO);
+	if (!set->set_count) {
+		rc = -EIO;
+		goto out_set;
+	}
 	*reqset = set;
 	return rc;
 out_set:
diff --git a/drivers/staging/lustre/lustre/lov/lovsub_dev.c b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
index 52fb6c162ad7..42336f13a76f 100644
--- a/drivers/staging/lustre/lustre/lov/lovsub_dev.c
+++ b/drivers/staging/lustre/lustre/lov/lovsub_dev.c
@@ -67,7 +67,7 @@ static void lovsub_req_completion(const struct lu_env *env,
 static void lovsub_req_attr_set(const struct lu_env *env,
 				const struct cl_req_slice *slice,
 				const struct cl_object *obj,
-				struct cl_req_attr *attr, obd_valid flags)
+				struct cl_req_attr *attr, u64 flags)
 {
 	struct lovsub_object *subobj;
 
diff --git a/drivers/staging/lustre/lustre/lvfs/Makefile b/drivers/staging/lustre/lustre/lvfs/Makefile
deleted file mode 100644
index 387eee307e2d..000000000000
--- a/drivers/staging/lustre/lustre/lvfs/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-obj-$(CONFIG_LUSTRE_FS) += lvfs.o
-
-lvfs-y := lvfs_linux.o fsfilt.o
-lvfs-$(CONFIG_PROC_FS) += lvfs_lib.o
diff --git a/drivers/staging/lustre/lustre/lvfs/fsfilt.c b/drivers/staging/lustre/lustre/lvfs/fsfilt.c
deleted file mode 100644
index a4df056a2853..000000000000
--- a/drivers/staging/lustre/lustre/lvfs/fsfilt.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#define DEBUG_SUBSYSTEM S_FILTER
-
-#include <linux/fs.h>
-#include <linux/module.h>
-#include <linux/kmod.h>
-#include <linux/slab.h>
-#include "../../include/linux/libcfs/libcfs.h"
-#include "../include/lustre_fsfilt.h"
-
-LIST_HEAD(fsfilt_types);
-
-static struct fsfilt_operations *fsfilt_search_type(const char *type)
-{
-	struct fsfilt_operations *found;
-	struct list_head *p;
-
-	list_for_each(p, &fsfilt_types) {
-		found = list_entry(p, struct fsfilt_operations, fs_list);
-		if (!strcmp(found->fs_type, type))
-			return found;
-	}
-	return NULL;
-}
-
-int fsfilt_register_ops(struct fsfilt_operations *fs_ops)
-{
-	struct fsfilt_operations *found;
-
-	/* lock fsfilt_types list */
-	found = fsfilt_search_type(fs_ops->fs_type);
-	if (found) {
-		if (found != fs_ops) {
-			CERROR("different operations for type %s\n",
-			       fs_ops->fs_type);
-			/* unlock fsfilt_types list */
-			return -EEXIST;
-		}
-	} else {
-		try_module_get(THIS_MODULE);
-		list_add(&fs_ops->fs_list, &fsfilt_types);
-	}
-
-	/* unlock fsfilt_types list */
-	return 0;
-}
-EXPORT_SYMBOL(fsfilt_register_ops);
-
-void fsfilt_unregister_ops(struct fsfilt_operations *fs_ops)
-{
-	struct list_head *p;
-
-	/* lock fsfilt_types list */
-	list_for_each(p, &fsfilt_types) {
-		struct fsfilt_operations *found;
-
-		found = list_entry(p, typeof(*found), fs_list);
-		if (found == fs_ops) {
-			list_del(p);
-			module_put(THIS_MODULE);
-			break;
-		}
-	}
-	/* unlock fsfilt_types list */
-}
-EXPORT_SYMBOL(fsfilt_unregister_ops);
-
-struct fsfilt_operations *fsfilt_get_ops(const char *type)
-{
-	struct fsfilt_operations *fs_ops;
-
-	/* lock fsfilt_types list */
-	fs_ops = fsfilt_search_type(type);
-	if (!fs_ops) {
-		char name[32];
-		int rc;
-
-		snprintf(name, sizeof(name) - 1, "fsfilt_%s", type);
-		name[sizeof(name) - 1] = '\0';
-
-		rc = request_module("%s", name);
-		if (!rc) {
-			fs_ops = fsfilt_search_type(type);
-			CDEBUG(D_INFO, "Loaded module '%s'\n", name);
-			if (!fs_ops)
-				rc = -ENOENT;
-		}
-
-		if (rc) {
-			CERROR("Can't find %s interface\n", name);
-			return ERR_PTR(rc < 0 ? rc : -rc);
-			/* unlock fsfilt_types list */
-		}
-	}
-	try_module_get(fs_ops->fs_owner);
-	/* unlock fsfilt_types list */
-
-	return fs_ops;
-}
-EXPORT_SYMBOL(fsfilt_get_ops);
-
-void fsfilt_put_ops(struct fsfilt_operations *fs_ops)
-{
-	module_put(fs_ops->fs_owner);
-}
-EXPORT_SYMBOL(fsfilt_put_ops);
diff --git a/drivers/staging/lustre/lustre/lvfs/lvfs_linux.c b/drivers/staging/lustre/lustre/lvfs/lvfs_linux.c
deleted file mode 100644
index eea0b2c94ba9..000000000000
--- a/drivers/staging/lustre/lustre/lvfs/lvfs_linux.c
+++ /dev/null
@@ -1,293 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/lvfs/lvfs_linux.c
- *
- * Author: Andreas Dilger <adilger@clusterfs.com>
- */
-
-#define DEBUG_SUBSYSTEM S_FILTER
-
-#include <linux/fs.h>
-#include <asm/unistd.h>
-#include <linux/slab.h>
-#include <linux/pagemap.h>
-#include <linux/quotaops.h>
-#include <linux/module.h>
-#include "../../include/linux/libcfs/libcfs.h"
-#include "../include/linux/lustre_compat25.h"
-#include "../include/lvfs.h"
-
-#include "../include/obd.h"
-#include "../include/lustre_lib.h"
-
-struct lprocfs_stats *obd_memory = NULL;
-EXPORT_SYMBOL(obd_memory);
-/* refine later and change to seqlock or similar from libcfs */
-
-/* Debugging check only needed during development */
-#ifdef OBD_CTXT_DEBUG
-# define ASSERT_CTXT_MAGIC(magic) LASSERT((magic) == OBD_RUN_CTXT_MAGIC)
-# define ASSERT_NOT_KERNEL_CTXT(msg) LASSERTF(!segment_eq(get_fs(), get_ds()),\
-					      msg)
-# define ASSERT_KERNEL_CTXT(msg) LASSERTF(segment_eq(get_fs(), get_ds()), msg)
-#else
-# define ASSERT_CTXT_MAGIC(magic) do {} while (0)
-# define ASSERT_NOT_KERNEL_CTXT(msg) do {} while (0)
-# define ASSERT_KERNEL_CTXT(msg) do {} while (0)
-#endif
-
-static void push_group_info(struct lvfs_run_ctxt *save,
-			    struct group_info *ginfo)
-{
-	if (!ginfo) {
-		save->ngroups = current_ngroups;
-		current_ngroups = 0;
-	} else {
-		struct cred *cred;
-		task_lock(current);
-		save->group_info = current_cred()->group_info;
-		cred = prepare_creds();
-		if (cred) {
-			cred->group_info = ginfo;
-			commit_creds(cred);
-		}
-		task_unlock(current);
-	}
-}
-
-static void pop_group_info(struct lvfs_run_ctxt *save,
-			   struct group_info *ginfo)
-{
-	if (!ginfo) {
-		current_ngroups = save->ngroups;
-	} else {
-		struct cred *cred;
-		task_lock(current);
-		cred = prepare_creds();
-		if (cred) {
-			cred->group_info = save->group_info;
-			commit_creds(cred);
-		}
-		task_unlock(current);
-	}
-}
-
-/* push / pop to root of obd store */
-void push_ctxt(struct lvfs_run_ctxt *save, struct lvfs_run_ctxt *new_ctx,
-	       struct lvfs_ucred *uc)
-{
-	/* if there is underlaying dt_device then push_ctxt is not needed */
-	if (new_ctx->dt != NULL)
-		return;
-
-	/* ASSERT_NOT_KERNEL_CTXT("already in kernel context!\n"); */
-	ASSERT_CTXT_MAGIC(new_ctx->magic);
-	OBD_SET_CTXT_MAGIC(save);
-
-	save->fs = get_fs();
-	LASSERT(d_count(cfs_fs_pwd(current->fs)));
-	LASSERT(d_count(new_ctx->pwd));
-	save->pwd = dget(cfs_fs_pwd(current->fs));
-	save->pwdmnt = mntget(cfs_fs_mnt(current->fs));
-	save->luc.luc_umask = current_umask();
-	save->ngroups = current_cred()->group_info->ngroups;
-
-	LASSERT(save->pwd);
-	LASSERT(save->pwdmnt);
-	LASSERT(new_ctx->pwd);
-	LASSERT(new_ctx->pwdmnt);
-
-	if (uc) {
-		struct cred *cred;
-		save->luc.luc_uid = current_uid();
-		save->luc.luc_gid = current_gid();
-		save->luc.luc_fsuid = current_fsuid();
-		save->luc.luc_fsgid = current_fsgid();
-		save->luc.luc_cap = current_cap();
-
-		cred = prepare_creds();
-		if (cred) {
-			cred->uid = uc->luc_uid;
-			cred->gid = uc->luc_gid;
-			cred->fsuid = uc->luc_fsuid;
-			cred->fsgid = uc->luc_fsgid;
-			cred->cap_effective = uc->luc_cap;
-			commit_creds(cred);
-		}
-
-		push_group_info(save,
-				uc->luc_ginfo ?:
-				uc->luc_identity ? uc->luc_identity->mi_ginfo :
-						   NULL);
-	}
-	current->fs->umask = 0; /* umask already applied on client */
-	set_fs(new_ctx->fs);
-	ll_set_fs_pwd(current->fs, new_ctx->pwdmnt, new_ctx->pwd);
-}
-EXPORT_SYMBOL(push_ctxt);
-
-void pop_ctxt(struct lvfs_run_ctxt *saved, struct lvfs_run_ctxt *new_ctx,
-	      struct lvfs_ucred *uc)
-{
-	/* if there is underlaying dt_device then pop_ctxt is not needed */
-	if (new_ctx->dt != NULL)
-		return;
-
-	ASSERT_CTXT_MAGIC(saved->magic);
-	ASSERT_KERNEL_CTXT("popping non-kernel context!\n");
-
-	LASSERTF(cfs_fs_pwd(current->fs) == new_ctx->pwd, "%p != %p\n",
-		 cfs_fs_pwd(current->fs), new_ctx->pwd);
-	LASSERTF(cfs_fs_mnt(current->fs) == new_ctx->pwdmnt, "%p != %p\n",
-		 cfs_fs_mnt(current->fs), new_ctx->pwdmnt);
-
-	set_fs(saved->fs);
-	ll_set_fs_pwd(current->fs, saved->pwdmnt, saved->pwd);
-
-	dput(saved->pwd);
-	mntput(saved->pwdmnt);
-	current->fs->umask = saved->luc.luc_umask;
-	if (uc) {
-		struct cred *cred;
-		cred = prepare_creds();
-		if (cred) {
-			cred->uid = saved->luc.luc_uid;
-			cred->gid = saved->luc.luc_gid;
-			cred->fsuid = saved->luc.luc_fsuid;
-			cred->fsgid = saved->luc.luc_fsgid;
-			cred->cap_effective = saved->luc.luc_cap;
-			commit_creds(cred);
-		}
-
-		pop_group_info(saved,
-			       uc->luc_ginfo ?:
-			       uc->luc_identity ? uc->luc_identity->mi_ginfo :
-						  NULL);
-	}
-}
-EXPORT_SYMBOL(pop_ctxt);
-
-/* utility to rename a file */
-int lustre_rename(struct dentry *dir, struct vfsmount *mnt,
-		  char *oldname, char *newname)
-{
-	struct dentry *dchild_old, *dchild_new;
-	int err = 0;
-
-	ASSERT_KERNEL_CTXT("kernel doing rename outside kernel context\n");
-	CDEBUG(D_INODE, "renaming file %.*s to %.*s\n",
-	       (int)strlen(oldname), oldname, (int)strlen(newname), newname);
-
-	dchild_old = ll_lookup_one_len(oldname, dir, strlen(oldname));
-	if (IS_ERR(dchild_old))
-		return PTR_ERR(dchild_old);
-
-	if (!dchild_old->d_inode)
-		GOTO(put_old, err = -ENOENT);
-
-	dchild_new = ll_lookup_one_len(newname, dir, strlen(newname));
-	if (IS_ERR(dchild_new))
-		GOTO(put_old, err = PTR_ERR(dchild_new));
-
-	err = ll_vfs_rename(dir->d_inode, dchild_old, mnt,
-			    dir->d_inode, dchild_new, mnt);
-
-	dput(dchild_new);
-put_old:
-	dput(dchild_old);
-	return err;
-}
-EXPORT_SYMBOL(lustre_rename);
-
-/* Note: dput(dchild) will *not* be called if there is an error */
-struct file *l_dentry_open(struct lvfs_run_ctxt *ctxt, struct dentry *de,
-			   int flags)
-{
-	struct path path = {
-		.dentry = de,
-		.mnt = ctxt->pwdmnt,
-	};
-	return dentry_open(&path, flags, current_cred());
-}
-EXPORT_SYMBOL(l_dentry_open);
-
-#if defined (CONFIG_PROC_FS)
-__s64 lprocfs_read_helper(struct lprocfs_counter *lc,
-			  struct lprocfs_counter_header *header,
-			  enum lprocfs_stats_flags flags,
-			  enum lprocfs_fields_flags field)
-{
-	__s64 ret = 0;
-
-	if (lc == NULL || header == NULL)
-		return 0;
-
-	switch (field) {
-	case LPROCFS_FIELDS_FLAGS_CONFIG:
-		ret = header->lc_config;
-		break;
-	case LPROCFS_FIELDS_FLAGS_SUM:
-		ret = lc->lc_sum;
-		if ((flags & LPROCFS_STATS_FLAG_IRQ_SAFE) != 0)
-			ret += lc->lc_sum_irq;
-		break;
-	case LPROCFS_FIELDS_FLAGS_MIN:
-		ret = lc->lc_min;
-		break;
-	case LPROCFS_FIELDS_FLAGS_MAX:
-		ret = lc->lc_max;
-		break;
-	case LPROCFS_FIELDS_FLAGS_AVG:
-		ret = (lc->lc_max - lc->lc_min) / 2;
-		break;
-	case LPROCFS_FIELDS_FLAGS_SUMSQUARE:
-		ret = lc->lc_sumsquare;
-		break;
-	case LPROCFS_FIELDS_FLAGS_COUNT:
-		ret = lc->lc_count;
-		break;
-	default:
-		break;
-	}
-
-	return ret;
-}
-EXPORT_SYMBOL(lprocfs_read_helper);
-#endif /* CONFIG_PROC_FS*/
-
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
-MODULE_DESCRIPTION("Lustre VFS Filesystem Helper v0.1");
-MODULE_LICENSE("GPL");
diff --git a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
index d1d891b91663..16341c818358 100644
--- a/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
+++ b/drivers/staging/lustre/lustre/mdc/lproc_mdc.c
@@ -56,7 +56,8 @@ static ssize_t mdc_max_rpcs_in_flight_seq_write(struct file *file,
 						size_t count,
 						loff_t *off)
 {
-	struct obd_device *dev = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *dev =
+			((struct seq_file *)file->private_data)->private;
 	struct client_obd *cli = &dev->u.cli;
 	int val, rc;
 
@@ -84,7 +85,8 @@ static int mdc_kuc_open(struct inode *inode, struct file *file)
 static ssize_t mdc_kuc_write(struct file *file, const char *buffer,
 			     size_t count, loff_t *off)
 {
-	struct obd_device *obd = ((struct seq_file *)file->private_data)->private;
+	struct obd_device *obd =
+			((struct seq_file *)file->private_data)->private;
 	struct kuc_hdr		*lh;
 	struct hsm_action_list	*hal;
 	struct hsm_action_item	*hai;
@@ -209,6 +211,6 @@ static struct lprocfs_vars lprocfs_mdc_module_vars[] = {
 
 void lprocfs_mdc_init_vars(struct lprocfs_static_vars *lvars)
 {
-    lvars->module_vars  = lprocfs_mdc_module_vars;
-    lvars->obd_vars     = lprocfs_mdc_obd_vars;
+	lvars->module_vars  = lprocfs_mdc_module_vars;
+	lvars->obd_vars     = lprocfs_mdc_obd_vars;
 }
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_internal.h b/drivers/staging/lustre/lustre/mdc/mdc_internal.h
index e8235559e27f..81780c943a08 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_internal.h
+++ b/drivers/staging/lustre/lustre/mdc/mdc_internal.h
@@ -40,7 +40,7 @@
 #include "../include/lustre_mdc.h"
 #include "../include/lustre_mds.h"
 
-#if defined (CONFIG_PROC_FS)
+#if defined CONFIG_PROC_FS
 void lprocfs_mdc_init_vars(struct lprocfs_static_vars *lvars);
 #else
 static inline void lprocfs_mdc_init_vars(struct lprocfs_static_vars *lvars)
@@ -108,7 +108,7 @@ int mdc_resource_get_unused(struct obd_export *exp, const struct lu_fid *fid,
 int mdc_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
 		  struct md_op_data *op_data);
 
-int mdc_open(struct obd_export *exp, obd_id ino, int type, int flags,
+int mdc_open(struct obd_export *exp, u64 ino, int type, int flags,
 	     struct lov_mds_md *lmm, int lmm_size, struct lustre_handle *fh,
 	     struct ptlrpc_request **);
 
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_lib.c b/drivers/staging/lustre/lustre/mdc/mdc_lib.c
index f54dd90c7e50..e8732cc30ce2 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_lib.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_lib.c
@@ -42,7 +42,7 @@
 
 static void __mdc_pack_body(struct mdt_body *b, __u32 suppgid)
 {
-	LASSERT (b != NULL);
+	LASSERT(b != NULL);
 
 	b->suppgid = suppgid;
 	b->uid = from_kuid(&init_user_ns, current_uid());
@@ -52,7 +52,8 @@ static void __mdc_pack_body(struct mdt_body *b, __u32 suppgid)
 	b->capability = cfs_curproc_cap_pack();
 }
 
-void mdc_pack_capa(struct ptlrpc_request *req, const struct req_msg_field *field,
+void mdc_pack_capa(struct ptlrpc_request *req,
+		   const struct req_msg_field *field,
 		   struct obd_capa *oc)
 {
 	struct req_capsule *pill = &req->rq_pill;
@@ -256,7 +257,8 @@ void mdc_open_pack(struct ptlrpc_request *req, struct md_op_data *op_data,
 	set_mrc_cr_flags(rec, cr_flags);
 }
 
-static inline __u64 attr_pack(unsigned int ia_valid) {
+static inline __u64 attr_pack(unsigned int ia_valid)
+{
 	__u64 sa_valid = 0;
 
 	if (ia_valid & ATTR_MODE)
@@ -316,7 +318,8 @@ static void mdc_setattr_pack_rec(struct mdt_rec_setattr *rec,
 	rec->sa_atime  = LTIME_S(op_data->op_attr.ia_atime);
 	rec->sa_mtime  = LTIME_S(op_data->op_attr.ia_mtime);
 	rec->sa_ctime  = LTIME_S(op_data->op_attr.ia_ctime);
-	rec->sa_attr_flags = ((struct ll_iattr *)&op_data->op_attr)->ia_attr_flags;
+	rec->sa_attr_flags =
+			((struct ll_iattr *)&op_data->op_attr)->ia_attr_flags;
 	if ((op_data->op_attr.ia_valid & ATTR_GID) &&
 	    in_group_p(op_data->op_attr.ia_gid))
 		rec->sa_suppgid =
@@ -342,7 +345,8 @@ void mdc_setattr_pack(struct ptlrpc_request *req, struct md_op_data *op_data,
 	struct mdt_ioepoch *epoch;
 	struct lov_user_md *lum = NULL;
 
-	CLASSERT(sizeof(struct mdt_rec_reint) ==sizeof(struct mdt_rec_setattr));
+	CLASSERT(sizeof(struct mdt_rec_reint) ==
+					sizeof(struct mdt_rec_setattr));
 	rec = req_capsule_client_get(&req->rq_pill, &RMF_REC_REINT);
 	mdc_setattr_pack_rec(rec, op_data);
 
@@ -382,18 +386,18 @@ void mdc_unlink_pack(struct ptlrpc_request *req, struct md_op_data *op_data)
 	rec = req_capsule_client_get(&req->rq_pill, &RMF_REC_REINT);
 	LASSERT(rec != NULL);
 
-	rec->ul_opcode  = op_data->op_cli_flags & CLI_RM_ENTRY ?
+	rec->ul_opcode   = op_data->op_cli_flags & CLI_RM_ENTRY ?
 					REINT_RMENTRY : REINT_UNLINK;
-	rec->ul_fsuid   = op_data->op_fsuid;
-	rec->ul_fsgid   = op_data->op_fsgid;
-	rec->ul_cap     = op_data->op_cap;
-	rec->ul_mode    = op_data->op_mode;
-	rec->ul_suppgid1= op_data->op_suppgids[0];
-	rec->ul_suppgid2= -1;
-	rec->ul_fid1    = op_data->op_fid1;
-	rec->ul_fid2    = op_data->op_fid2;
-	rec->ul_time    = op_data->op_mod_time;
-	rec->ul_bias    = op_data->op_bias;
+	rec->ul_fsuid    = op_data->op_fsuid;
+	rec->ul_fsgid    = op_data->op_fsgid;
+	rec->ul_cap      = op_data->op_cap;
+	rec->ul_mode     = op_data->op_mode;
+	rec->ul_suppgid1 = op_data->op_suppgids[0];
+	rec->ul_suppgid2 = -1;
+	rec->ul_fid1     = op_data->op_fid1;
+	rec->ul_fid2     = op_data->op_fid2;
+	rec->ul_time     = op_data->op_mod_time;
+	rec->ul_bias     = op_data->op_bias;
 
 	mdc_pack_capa(req, &RMF_CAPA1, op_data->op_capa1);
 
@@ -409,12 +413,12 @@ void mdc_link_pack(struct ptlrpc_request *req, struct md_op_data *op_data)
 
 	CLASSERT(sizeof(struct mdt_rec_reint) == sizeof(struct mdt_rec_link));
 	rec = req_capsule_client_get(&req->rq_pill, &RMF_REC_REINT);
-	LASSERT (rec != NULL);
+	LASSERT(rec != NULL);
 
 	rec->lk_opcode   = REINT_LINK;
-	rec->lk_fsuid    = op_data->op_fsuid;//current->fsuid;
-	rec->lk_fsgid    = op_data->op_fsgid;//current->fsgid;
-	rec->lk_cap      = op_data->op_cap;//current->cap_effective;
+	rec->lk_fsuid    = op_data->op_fsuid; /* current->fsuid; */
+	rec->lk_fsgid    = op_data->op_fsgid; /* current->fsgid; */
+	rec->lk_cap      = op_data->op_cap;   /* current->cap_effective; */
 	rec->lk_suppgid1 = op_data->op_suppgids[0];
 	rec->lk_suppgid2 = op_data->op_suppgids[1];
 	rec->lk_fid1     = op_data->op_fid1;
@@ -486,6 +490,7 @@ void mdc_getattr_pack(struct ptlrpc_request *req, __u64 valid, int flags,
 
 	if (op_data->op_name) {
 		char *tmp = req_capsule_client_get(&req->rq_pill, &RMF_NAME);
+
 		LOGL0(op_data->op_name, op_data->op_namelen, tmp);
 
 	}
@@ -551,7 +556,8 @@ int mdc_enter_request(struct client_obd *cli)
 		list_add_tail(&mcw.mcw_entry, &cli->cl_cache_waiters);
 		init_waitqueue_head(&mcw.mcw_waitq);
 		client_obd_list_unlock(&cli->cl_loi_list_lock);
-		rc = l_wait_event(mcw.mcw_waitq, mdc_req_avail(cli, &mcw), &lwi);
+		rc = l_wait_event(mcw.mcw_waitq, mdc_req_avail(cli, &mcw),
+				  &lwi);
 		if (rc) {
 			client_obd_list_lock(&cli->cl_loi_list_lock);
 			if (list_empty(&mcw.mcw_entry))
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_locks.c b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
index 71219b90e22b..b58147ee62b6 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_locks.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_locks.c
@@ -38,7 +38,7 @@
 
 # include <linux/module.h>
 
-#include "../include/linux/lustre_intent.h"
+#include "../include/lustre_intent.h"
 #include "../include/obd.h"
 #include "../include/obd_class.h"
 #include "../include/lustre_dlm.h"
@@ -134,12 +134,12 @@ int mdc_set_lock_data(struct obd_export *exp, __u64 *lockh, void *data,
 	if (lock->l_resource->lr_lvb_inode &&
 	    lock->l_resource->lr_lvb_inode != data) {
 		struct inode *old_inode = lock->l_resource->lr_lvb_inode;
+
 		LASSERTF(old_inode->i_state & I_FREEING,
-			 "Found existing inode %p/%lu/%u state %lu in lock: "
-			 "setting data to %p/%lu/%u\n", old_inode,
-			 old_inode->i_ino, old_inode->i_generation,
-			 old_inode->i_state,
-			 new_inode, new_inode->i_ino, new_inode->i_generation);
+			 "Found existing inode %p/%lu/%u state %lu in lock: setting data to %p/%lu/%u\n",
+			 old_inode, old_inode->i_ino, old_inode->i_generation,
+			 old_inode->i_state, new_inode, new_inode->i_ino,
+			 new_inode->i_generation);
 	}
 	lock->l_resource->lr_lvb_inode = new_inode;
 	if (bits)
@@ -218,7 +218,7 @@ int mdc_find_cbdata(struct obd_export *exp,
 	struct ldlm_res_id res_id;
 	int rc = 0;
 
-	fid_build_reg_res_name((struct lu_fid*)fid, &res_id);
+	fid_build_reg_res_name((struct lu_fid *)fid, &res_id);
 	rc = ldlm_resource_iterate(class_exp2obd(exp)->obd_namespace, &res_id,
 				   it, data);
 	if (rc == LDLM_ITER_STOP)
@@ -450,12 +450,12 @@ static struct ptlrpc_request *mdc_intent_unlink_pack(struct obd_export *exp,
 }
 
 static struct ptlrpc_request *mdc_intent_getattr_pack(struct obd_export *exp,
-						      struct lookup_intent *it,
-						      struct md_op_data *op_data)
+						    struct lookup_intent *it,
+						    struct md_op_data *op_data)
 {
 	struct ptlrpc_request *req;
 	struct obd_device     *obddev = class_exp2obd(exp);
-	obd_valid	      valid = OBD_MD_FLGETATTR | OBD_MD_FLEASIZE |
+	u64		       valid = OBD_MD_FLGETATTR | OBD_MD_FLEASIZE |
 				       OBD_MD_FLMODEASIZE | OBD_MD_FLDIREA |
 				       OBD_MD_FLMDSCAPA | OBD_MD_MEA |
 				       (client_is_remote(exp) ?
@@ -621,7 +621,7 @@ static int mdc_finish_enqueue(struct obd_export *exp,
 	 * function without doing so, and try to replay a failed create
 	 * (bug 3440) */
 	if (it->it_op & IT_OPEN && req->rq_replay &&
-	    (!it_disposition(it, DISP_OPEN_OPEN) ||intent->it_status != 0))
+	    (!it_disposition(it, DISP_OPEN_OPEN) || intent->it_status != 0))
 		mdc_clear_replay_flag(req, intent->it_status);
 
 	DEBUG_REQ(D_RPCTRACE, req, "op: %d disposition: %x, status: %d",
@@ -633,7 +633,7 @@ static int mdc_finish_enqueue(struct obd_export *exp,
 
 		body = req_capsule_server_get(pill, &RMF_MDT_BODY);
 		if (body == NULL) {
-			CERROR ("Can't swab mdt_body\n");
+			CERROR("Can't swab mdt_body\n");
 			return -EPROTO;
 		}
 
@@ -678,6 +678,7 @@ static int mdc_finish_enqueue(struct obd_export *exp,
 			 */
 			if ((it->it_op & IT_OPEN) && req->rq_replay) {
 				void *lmm;
+
 				if (req_capsule_get_size(pill, &RMF_EADATA,
 							 RCL_CLIENT) <
 				    body->eadatasize)
@@ -777,25 +778,29 @@ static int mdc_finish_enqueue(struct obd_export *exp,
 int mdc_enqueue(struct obd_export *exp, struct ldlm_enqueue_info *einfo,
 		struct lookup_intent *it, struct md_op_data *op_data,
 		struct lustre_handle *lockh, void *lmm, int lmmsize,
-		struct ptlrpc_request **reqp, __u64 extra_lock_flags)
+		struct ptlrpc_request **reqp, u64 extra_lock_flags)
 {
-	struct obd_device     *obddev = class_exp2obd(exp);
-	struct ptlrpc_request *req = NULL;
-	__u64		  flags, saved_flags = extra_lock_flags;
-	int		    rc;
-	struct ldlm_res_id res_id;
-	static const ldlm_policy_data_t lookup_policy =
-			    { .l_inodebits = { MDS_INODELOCK_LOOKUP } };
-	static const ldlm_policy_data_t update_policy =
-			    { .l_inodebits = { MDS_INODELOCK_UPDATE } };
-	static const ldlm_policy_data_t layout_policy =
-			    { .l_inodebits = { MDS_INODELOCK_LAYOUT } };
+	static const ldlm_policy_data_t lookup_policy = {
+		.l_inodebits = { MDS_INODELOCK_LOOKUP }
+	};
+	static const ldlm_policy_data_t update_policy = {
+		.l_inodebits = { MDS_INODELOCK_UPDATE }
+	};
+	static const ldlm_policy_data_t layout_policy = {
+		.l_inodebits = { MDS_INODELOCK_LAYOUT }
+	};
 	static const ldlm_policy_data_t getxattr_policy = {
-			      .l_inodebits = { MDS_INODELOCK_XATTR } };
+		.l_inodebits = { MDS_INODELOCK_XATTR }
+	};
 	ldlm_policy_data_t const *policy = &lookup_policy;
-	int		    generation, resends = 0;
-	struct ldlm_reply     *lockrep;
-	enum lvb_type	       lvb_type = 0;
+	struct obd_device *obddev = class_exp2obd(exp);
+	struct ptlrpc_request *req;
+	u64 flags, saved_flags = extra_lock_flags;
+	struct ldlm_res_id res_id;
+	int generation, resends = 0;
+	struct ldlm_reply *lockrep;
+	enum lvb_type lvb_type = LVB_T_NONE;
+	int rc;
 
 	LASSERTF(!it || einfo->ei_type == LDLM_IBITS, "lock type %d\n",
 		 einfo->ei_type);
@@ -939,7 +944,12 @@ resend:
 			memset(lockh, 0, sizeof(*lockh));
 		}
 		ptlrpc_req_finished(req);
+
+		it->d.lustre.it_lock_handle = 0;
+		it->d.lustre.it_lock_mode = 0;
+		it->d.lustre.it_data = NULL;
 	}
+
 	return rc;
 }
 
@@ -1029,6 +1039,7 @@ static int mdc_finish_intent_lock(struct obd_export *exp,
 	lock = ldlm_handle2lock(lockh);
 	if (lock) {
 		ldlm_policy_data_t policy = lock->l_policy_data;
+
 		LDLM_DEBUG(lock, "matching against this");
 
 		LASSERTF(fid_res_name_eq(&mdt_body->fid1,
@@ -1039,14 +1050,16 @@ static int mdc_finish_intent_lock(struct obd_export *exp,
 
 		memcpy(&old_lock, lockh, sizeof(*lockh));
 		if (ldlm_lock_match(NULL, LDLM_FL_BLOCK_GRANTED, NULL,
-				    LDLM_IBITS, &policy, LCK_NL, &old_lock, 0)) {
+				    LDLM_IBITS, &policy, LCK_NL,
+				    &old_lock, 0)) {
 			ldlm_lock_decref_and_cancel(lockh,
 						    it->d.lustre.it_lock_mode);
 			memcpy(lockh, &old_lock, sizeof(old_lock));
 			it->d.lustre.it_lock_handle = lockh->cookie;
 		}
 	}
-	CDEBUG(D_DENTRY,"D_IT dentry %.*s intent: %s status %d disp %x rc %d\n",
+	CDEBUG(D_DENTRY,
+	       "D_IT dentry %.*s intent: %s status %d disp %x rc %d\n",
 	       op_data->op_namelen, op_data->op_name, ldlm_it2str(it->it_op),
 	       it->d.lustre.it_status, it->d.lustre.it_disposition, rc);
 	return rc;
@@ -1223,7 +1236,7 @@ static int mdc_intent_getattr_async_interpret(const struct lu_env *env,
 	if (rc < 0) {
 		CERROR("ldlm_cli_enqueue_fini: %d\n", rc);
 		mdc_clear_replay_flag(req, rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	lockrep = req_capsule_server_get(&req->rq_pill, &RMF_DLM_REP);
@@ -1234,7 +1247,7 @@ static int mdc_intent_getattr_async_interpret(const struct lu_env *env,
 
 	rc = mdc_finish_enqueue(exp, req, einfo, it, lockh, rc);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	rc = mdc_finish_intent_lock(exp, req, &minfo->mi_data, it, lockh);
 
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_reint.c b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
index c5420a42bc33..5e9c6296c39d 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_reint.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_reint.c
@@ -57,9 +57,9 @@ static int mdc_reint(struct ptlrpc_request *request,
 	mdc_put_rpc_lock(rpc_lock, NULL);
 	if (rc)
 		CDEBUG(D_INFO, "error in handling %d\n", rc);
-	else if (!req_capsule_server_get(&request->rq_pill, &RMF_MDT_BODY)) {
+	else if (!req_capsule_server_get(&request->rq_pill, &RMF_MDT_BODY))
 		rc = -EPROTO;
-	}
+
 	return rc;
 }
 
@@ -71,7 +71,7 @@ int mdc_resource_get_unused(struct obd_export *exp, const struct lu_fid *fid,
 			    __u64 bits)
 {
 	struct ldlm_namespace *ns = exp->exp_obd->obd_namespace;
-	ldlm_policy_data_t policy = {{0}};
+	ldlm_policy_data_t policy = {};
 	struct ldlm_res_id res_id;
 	struct ldlm_resource *res;
 	int count;
@@ -152,14 +152,12 @@ int mdc_setattr(struct obd_export *exp, struct md_op_data *op_data,
 
 	ptlrpc_request_set_replen(req);
 	if (mod && (op_data->op_flags & MF_EPOCH_OPEN) &&
-	    req->rq_import->imp_replayable)
-	{
+	    req->rq_import->imp_replayable) {
 		LASSERT(*mod == NULL);
 
 		*mod = obd_mod_alloc();
 		if (*mod == NULL) {
-			DEBUG_REQ(D_ERROR, req, "Can't allocate "
-				  "md_open_data");
+			DEBUG_REQ(D_ERROR, req, "Can't allocate md_open_data");
 		} else {
 			req->rq_replay = 1;
 			req->rq_cb_data = *mod;
diff --git a/drivers/staging/lustre/lustre/mdc/mdc_request.c b/drivers/staging/lustre/lustre/mdc/mdc_request.c
index 4a1cc4eb73d5..14e1ba1675f6 100644
--- a/drivers/staging/lustre/lustre/mdc/mdc_request.c
+++ b/drivers/staging/lustre/lustre/mdc/mdc_request.c
@@ -122,16 +122,18 @@ static int send_getstatus(struct obd_import *imp, struct lu_fid *rootfid,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
-	if (body == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (body == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	if (body->valid & OBD_MD_FLMDSCAPA) {
 		rc = mdc_unpack_capa(NULL, req, &RMF_CAPA1, pc);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 	}
 
 	*rootfid = body->fid1;
@@ -203,6 +205,7 @@ static int mdc_getattr_common(struct obd_export *exp,
 
 	if (body->valid & OBD_MD_FLMDSCAPA) {
 		struct lustre_capa *capa;
+
 		capa = req_capsule_server_get(pill, &RMF_CAPA1);
 		if (capa == NULL)
 			return -EPROTO;
@@ -283,6 +286,7 @@ int mdc_getattr_name(struct obd_export *exp, struct md_op_data *op_data,
 
 	if (op_data->op_name) {
 		char *name = req_capsule_client_get(&req->rq_pill, &RMF_NAME);
+
 		LASSERT(strnlen(op_data->op_name, op_data->op_namelen) ==
 				op_data->op_namelen);
 		memcpy(name, op_data->op_name, op_data->op_namelen);
@@ -326,9 +330,10 @@ static int mdc_is_subdir(struct obd_export *exp,
 	return rc;
 }
 
-static int mdc_xattr_common(struct obd_export *exp,const struct req_format *fmt,
+static int mdc_xattr_common(struct obd_export *exp,
+			    const struct req_format *fmt,
 			    const struct lu_fid *fid,
-			    struct obd_capa *oc, int opcode, obd_valid valid,
+			    struct obd_capa *oc, int opcode, u64 valid,
 			    const char *xattr_name, const char *input,
 			    int input_size, int output_size, int flags,
 			    __u32 suppgid, struct ptlrpc_request **request)
@@ -437,7 +442,7 @@ static int mdc_xattr_common(struct obd_export *exp,const struct req_format *fmt,
 }
 
 int mdc_setxattr(struct obd_export *exp, const struct lu_fid *fid,
-		 struct obd_capa *oc, obd_valid valid, const char *xattr_name,
+		 struct obd_capa *oc, u64 valid, const char *xattr_name,
 		 const char *input, int input_size, int output_size,
 		 int flags, __u32 suppgid, struct ptlrpc_request **request)
 {
@@ -448,7 +453,7 @@ int mdc_setxattr(struct obd_export *exp, const struct lu_fid *fid,
 }
 
 int mdc_getxattr(struct obd_export *exp, const struct lu_fid *fid,
-		 struct obd_capa *oc, obd_valid valid, const char *xattr_name,
+		 struct obd_capa *oc, u64 valid, const char *xattr_name,
 		 const char *input, int input_size, int output_size,
 		 int flags, struct ptlrpc_request **request)
 {
@@ -514,64 +519,73 @@ int mdc_get_lustre_md(struct obd_export *exp, struct ptlrpc_request *req,
 		struct lov_mds_md *lmm;
 
 		if (!S_ISREG(md->body->mode)) {
-			CDEBUG(D_INFO, "OBD_MD_FLEASIZE set, should be a "
-			       "regular file, but is not\n");
-			GOTO(out, rc = -EPROTO);
+			CDEBUG(D_INFO,
+			       "OBD_MD_FLEASIZE set, should be a regular file, but is not\n");
+			rc = -EPROTO;
+			goto out;
 		}
 
 		if (md->body->eadatasize == 0) {
-			CDEBUG(D_INFO, "OBD_MD_FLEASIZE set, "
-			       "but eadatasize 0\n");
-			GOTO(out, rc = -EPROTO);
+			CDEBUG(D_INFO,
+			       "OBD_MD_FLEASIZE set, but eadatasize 0\n");
+			rc = -EPROTO;
+			goto out;
 		}
 		lmmsize = md->body->eadatasize;
 		lmm = req_capsule_server_sized_get(pill, &RMF_MDT_MD, lmmsize);
-		if (!lmm)
-			GOTO(out, rc = -EPROTO);
+		if (!lmm) {
+			rc = -EPROTO;
+			goto out;
+		}
 
 		rc = obd_unpackmd(dt_exp, &md->lsm, lmm, lmmsize);
 		if (rc < 0)
-			GOTO(out, rc);
+			goto out;
 
 		if (rc < sizeof(*md->lsm)) {
-			CDEBUG(D_INFO, "lsm size too small: "
-			       "rc < sizeof (*md->lsm) (%d < %d)\n",
+			CDEBUG(D_INFO,
+			       "lsm size too small: rc < sizeof (*md->lsm) (%d < %d)\n",
 			       rc, (int)sizeof(*md->lsm));
-			GOTO(out, rc = -EPROTO);
+			rc = -EPROTO;
+			goto out;
 		}
 
 	} else if (md->body->valid & OBD_MD_FLDIREA) {
 		int lmvsize;
 		struct lov_mds_md *lmv;
 
-		if(!S_ISDIR(md->body->mode)) {
-			CDEBUG(D_INFO, "OBD_MD_FLDIREA set, should be a "
-			       "directory, but is not\n");
-			GOTO(out, rc = -EPROTO);
+		if (!S_ISDIR(md->body->mode)) {
+			CDEBUG(D_INFO,
+			       "OBD_MD_FLDIREA set, should be a directory, but is not\n");
+			rc = -EPROTO;
+			goto out;
 		}
 
 		if (md->body->eadatasize == 0) {
-			CDEBUG(D_INFO, "OBD_MD_FLDIREA is set, "
-			       "but eadatasize 0\n");
+			CDEBUG(D_INFO,
+			       "OBD_MD_FLDIREA is set, but eadatasize 0\n");
 			return -EPROTO;
 		}
 		if (md->body->valid & OBD_MD_MEA) {
 			lmvsize = md->body->eadatasize;
 			lmv = req_capsule_server_sized_get(pill, &RMF_MDT_MD,
 							   lmvsize);
-			if (!lmv)
-				GOTO(out, rc = -EPROTO);
+			if (!lmv) {
+				rc = -EPROTO;
+				goto out;
+			}
 
 			rc = obd_unpackmd(md_exp, (void *)&md->mea, lmv,
 					  lmvsize);
 			if (rc < 0)
-				GOTO(out, rc);
+				goto out;
 
 			if (rc < sizeof(*md->mea)) {
-				CDEBUG(D_INFO, "size too small:  "
-				       "rc < sizeof(*md->mea) (%d < %d)\n",
+				CDEBUG(D_INFO,
+				       "size too small: rc < sizeof(*md->mea) (%d < %d)\n",
 					rc, (int)sizeof(*md->mea));
-				GOTO(out, rc = -EPROTO);
+				rc = -EPROTO;
+				goto out;
 			}
 		}
 	}
@@ -582,10 +596,11 @@ int mdc_get_lustre_md(struct obd_export *exp, struct ptlrpc_request *req,
 		LASSERT(client_is_remote(exp));
 		md->remote_perm = req_capsule_server_swab_get(pill, &RMF_ACL,
 						lustre_swab_mdt_remote_perm);
-		if (!md->remote_perm)
-			GOTO(out, rc = -EPROTO);
-	}
-	else if (md->body->valid & OBD_MD_FLACL) {
+		if (!md->remote_perm) {
+			rc = -EPROTO;
+			goto out;
+		}
+	} else if (md->body->valid & OBD_MD_FLACL) {
 		/* for ACL, it's possible that FLACL is set but aclsize is zero.
 		 * only when aclsize != 0 there's an actual segment for ACL
 		 * in reply buffer.
@@ -593,7 +608,7 @@ int mdc_get_lustre_md(struct obd_export *exp, struct ptlrpc_request *req,
 		if (md->body->aclsize) {
 			rc = mdc_unpack_acl(req, md);
 			if (rc)
-				GOTO(out, rc);
+				goto out;
 #ifdef CONFIG_FS_POSIX_ACL
 		} else {
 			md->posix_acl = NULL;
@@ -605,7 +620,7 @@ int mdc_get_lustre_md(struct obd_export *exp, struct ptlrpc_request *req,
 
 		rc = mdc_unpack_capa(NULL, req, &RMF_CAPA1, &oc);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 		md->mds_capa = oc;
 	}
 
@@ -614,7 +629,7 @@ int mdc_get_lustre_md(struct obd_export *exp, struct ptlrpc_request *req,
 
 		rc = mdc_unpack_capa(NULL, req, &RMF_CAPA2, &oc);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 		md->oss_capa = oc;
 	}
 
@@ -695,6 +710,7 @@ void mdc_replay_open(struct ptlrpc_request *req)
 void mdc_commit_open(struct ptlrpc_request *req)
 {
 	struct md_open_data *mod = req->rq_cb_data;
+
 	if (mod == NULL)
 		return;
 
@@ -774,8 +790,8 @@ int mdc_set_open_replay_data(struct obd_export *exp,
 	rec->cr_old_handle.cookie = body->handle.cookie;
 	open_req->rq_replay_cb = mdc_replay_open;
 	if (!fid_is_sane(&body->fid1)) {
-		DEBUG_REQ(D_ERROR, open_req, "Saving replay request with "
-			  "insane fid");
+		DEBUG_REQ(D_ERROR, open_req,
+			  "Saving replay request with insane fid");
 		LBUG();
 	}
 
@@ -897,7 +913,8 @@ int mdc_close(struct obd_export *exp, struct md_op_data *op_data,
 		mod->mod_open_req->rq_replay = 0;
 		spin_unlock(&mod->mod_open_req->rq_lock);
 	} else {
-		 CDEBUG(D_HA, "couldn't find open req; expecting close error\n");
+		 CDEBUG(D_HA,
+			"couldn't find open req; expecting close error\n");
 	}
 
 	mdc_close_pack(req, op_data);
@@ -923,8 +940,8 @@ int mdc_close(struct obd_export *exp, struct md_op_data *op_data,
 
 		rc = lustre_msg_get_status(req->rq_repmsg);
 		if (lustre_msg_get_type(req->rq_repmsg) == PTL_RPC_MSG_ERR) {
-			DEBUG_REQ(D_ERROR, req, "type == PTL_RPC_MSG_ERR, err "
-				  "= %d", rc);
+			DEBUG_REQ(D_ERROR, req,
+				  "type == PTL_RPC_MSG_ERR, err = %d", rc);
 			if (rc > 0)
 				rc = -rc;
 		}
@@ -1084,7 +1101,8 @@ restart_bulk:
 			CERROR("too many resend retries, returning error\n");
 			return -EIO;
 		}
-		lwi = LWI_TIMEOUT_INTR(cfs_time_seconds(resends), NULL, NULL, NULL);
+		lwi = LWI_TIMEOUT_INTR(cfs_time_seconds(resends),
+				       NULL, NULL, NULL);
 		l_wait_event(waitq, 0, &lwi);
 
 		goto restart_bulk;
@@ -1132,8 +1150,10 @@ static int mdc_statfs(const struct lu_env *env,
 
 	req = ptlrpc_request_alloc_pack(imp, &RQF_MDS_STATFS,
 					LUSTRE_MDS_VERSION, MDS_STATFS);
-	if (req == NULL)
-		GOTO(output, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto output;
+	}
 
 	ptlrpc_request_set_replen(req);
 
@@ -1148,12 +1168,14 @@ static int mdc_statfs(const struct lu_env *env,
 		/* check connection error first */
 		if (imp->imp_connect_error)
 			rc = imp->imp_connect_error;
-		GOTO(out, rc);
+		goto out;
 	}
 
 	msfs = req_capsule_server_get(&req->rq_pill, &RMF_OBD_STATFS);
-	if (msfs == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (msfs == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	*osfs = *msfs;
 out:
@@ -1185,20 +1207,25 @@ static int mdc_ioc_fid2path(struct obd_export *exp, struct getinfo_fid2path *gf)
 	CDEBUG(D_IOCTL, "path get "DFID" from %llu #%d\n",
 	       PFID(&gf->gf_fid), gf->gf_recno, gf->gf_linkno);
 
-	if (!fid_is_sane(&gf->gf_fid))
-		GOTO(out, rc = -EINVAL);
+	if (!fid_is_sane(&gf->gf_fid)) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	/* Val is struct getinfo_fid2path result plus path */
 	vallen = sizeof(*gf) + gf->gf_pathlen;
 
 	rc = obd_get_info(NULL, exp, keylen, key, &vallen, gf, NULL);
 	if (rc != 0 && rc != -EREMOTE)
-		GOTO(out, rc);
+		goto out;
 
-	if (vallen <= sizeof(*gf))
-		GOTO(out, rc = -EPROTO);
-	else if (vallen > sizeof(*gf) + gf->gf_pathlen)
-		GOTO(out, rc = -EOVERFLOW);
+	if (vallen <= sizeof(*gf)) {
+		rc = -EPROTO;
+		goto out;
+	} else if (vallen > sizeof(*gf) + gf->gf_pathlen) {
+		rc = -EOVERFLOW;
+		goto out;
+	}
 
 	CDEBUG(D_IOCTL, "path get "DFID" from %llu #%d\n%s\n",
 	       PFID(&gf->gf_fid), gf->gf_recno, gf->gf_linkno, gf->gf_path);
@@ -1218,15 +1245,19 @@ static int mdc_ioc_hsm_progress(struct obd_export *exp,
 
 	req = ptlrpc_request_alloc_pack(imp, &RQF_MDS_HSM_PROGRESS,
 					LUSTRE_MDS_VERSION, MDS_HSM_PROGRESS);
-	if (req == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	mdc_pack_body(req, NULL, NULL, OBD_MD_FLRMTPERM, 0, 0, 0);
 
 	/* Copy hsm_progress struct */
 	req_hpk = req_capsule_client_get(&req->rq_pill, &RMF_MDS_HSM_PROGRESS);
-	if (req_hpk == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (req_hpk == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	*req_hpk = *hpk;
 	req_hpk->hpk_errval = lustre_errno_hton(hpk->hpk_errval);
@@ -1234,7 +1265,7 @@ static int mdc_ioc_hsm_progress(struct obd_export *exp,
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	GOTO(out, rc);
+	goto out;
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -1249,23 +1280,27 @@ static int mdc_ioc_hsm_ct_register(struct obd_import *imp, __u32 archives)
 	req = ptlrpc_request_alloc_pack(imp, &RQF_MDS_HSM_CT_REGISTER,
 					LUSTRE_MDS_VERSION,
 					MDS_HSM_CT_REGISTER);
-	if (req == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	mdc_pack_body(req, NULL, NULL, OBD_MD_FLRMTPERM, 0, 0, 0);
 
 	/* Copy hsm_progress struct */
 	archive_mask = req_capsule_client_get(&req->rq_pill,
 					      &RMF_MDS_HSM_ARCHIVE);
-	if (archive_mask == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (archive_mask == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	*archive_mask = archives;
 
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	GOTO(out, rc);
+	goto out;
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -1299,12 +1334,14 @@ static int mdc_ioc_hsm_current_action(struct obd_export *exp,
 
 	rc = mdc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	req_hca = req_capsule_server_get(&req->rq_pill,
 					 &RMF_MDS_HSM_CURRENT_ACTION);
-	if (req_hca == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (req_hca == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	*hca = *req_hca;
 
@@ -1321,15 +1358,17 @@ static int mdc_ioc_hsm_ct_unregister(struct obd_import *imp)
 	req = ptlrpc_request_alloc_pack(imp, &RQF_MDS_HSM_CT_UNREGISTER,
 					LUSTRE_MDS_VERSION,
 					MDS_HSM_CT_UNREGISTER);
-	if (req == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	mdc_pack_body(req, NULL, NULL, OBD_MD_FLRMTPERM, 0, 0, 0);
 
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	GOTO(out, rc);
+	goto out;
 out:
 	ptlrpc_req_finished(req);
 	return rc;
@@ -1363,11 +1402,13 @@ static int mdc_ioc_hsm_state_get(struct obd_export *exp,
 
 	rc = mdc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	req_hus = req_capsule_server_get(&req->rq_pill, &RMF_HSM_USER_STATE);
-	if (req_hus == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (req_hus == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	*hus = *req_hus;
 
@@ -1402,14 +1443,16 @@ static int mdc_ioc_hsm_state_set(struct obd_export *exp,
 
 	/* Copy states */
 	req_hss = req_capsule_client_get(&req->rq_pill, &RMF_HSM_STATE_SET);
-	if (req_hss == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (req_hss == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 	*req_hss = *hss;
 
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	GOTO(out, rc);
+	goto out;
 
 out:
 	ptlrpc_req_finished(req);
@@ -1427,8 +1470,10 @@ static int mdc_ioc_hsm_request(struct obd_export *exp,
 	int			 rc;
 
 	req = ptlrpc_request_alloc(imp, &RQF_MDS_HSM_REQUEST);
-	if (req == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	req_capsule_set_size(&req->rq_pill, &RMF_MDS_HSM_USER_ITEM, RCL_CLIENT,
 			     hur->hur_request.hr_itemcount
@@ -1446,27 +1491,33 @@ static int mdc_ioc_hsm_request(struct obd_export *exp,
 
 	/* Copy hsm_request struct */
 	req_hr = req_capsule_client_get(&req->rq_pill, &RMF_MDS_HSM_REQUEST);
-	if (req_hr == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (req_hr == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 	*req_hr = hur->hur_request;
 
 	/* Copy hsm_user_item structs */
 	req_hui = req_capsule_client_get(&req->rq_pill, &RMF_MDS_HSM_USER_ITEM);
-	if (req_hui == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (req_hui == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 	memcpy(req_hui, hur->hur_user_item,
 	       hur->hur_request.hr_itemcount * sizeof(struct hsm_user_item));
 
 	/* Copy opaque field */
 	req_opaque = req_capsule_client_get(&req->rq_pill, &RMF_GENERIC_DATA);
-	if (req_opaque == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (req_opaque == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 	memcpy(req_opaque, hur_data(hur), hur->hur_request.hr_data_len);
 
 	ptlrpc_request_set_replen(req);
 
 	rc = mdc_queue_wait(req);
-	GOTO(out, rc);
+	goto out;
 
 out:
 	ptlrpc_req_finished(req);
@@ -1534,7 +1585,7 @@ static int changelog_kkuc_cb(const struct lu_env *env, struct llog_handle *llh,
 	memcpy(lh + 1, &rec->cr, len - sizeof(*lh));
 
 	rc = libcfs_kkuc_msg_put(cs->cs_fp, lh);
-	CDEBUG(D_CHANGELOG, "kucmsg fp %p len %d rc %d\n", cs->cs_fp, len,rc);
+	CDEBUG(D_CHANGELOG, "kucmsg fp %p len %d rc %d\n", cs->cs_fp, len, rc);
 
 	return rc;
 }
@@ -1551,24 +1602,28 @@ static int mdc_changelog_send_thread(void *csdata)
 	       cs->cs_fp, cs->cs_startrec);
 
 	OBD_ALLOC(cs->cs_buf, KUC_CHANGELOG_MSG_MAXSIZE);
-	if (cs->cs_buf == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (cs->cs_buf == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	/* Set up the remote catalog handle */
 	ctxt = llog_get_context(cs->cs_obd, LLOG_CHANGELOG_REPL_CTXT);
-	if (ctxt == NULL)
-		GOTO(out, rc = -ENOENT);
+	if (ctxt == NULL) {
+		rc = -ENOENT;
+		goto out;
+	}
 	rc = llog_open(NULL, ctxt, &llh, NULL, CHANGELOG_CATALOG,
 		       LLOG_OPEN_EXISTS);
 	if (rc) {
 		CERROR("%s: fail to open changelog catalog: rc = %d\n",
 		       cs->cs_obd->obd_name, rc);
-		GOTO(out, rc);
+		goto out;
 	}
 	rc = llog_init_handle(NULL, llh, LLOG_F_IS_CAT, NULL);
 	if (rc) {
 		CERROR("llog_init_handle failed %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	rc = llog_cat_process(NULL, llh, changelog_kkuc_cb, cs, 0, 0);
@@ -1702,7 +1757,7 @@ static int mdc_quotactl(struct obd_device *unused, struct obd_export *exp,
 		if (oqc) {
 			*oqctl = *oqc;
 		} else if (!rc) {
-			CERROR ("Can't unpack obd_quotactl\n");
+			CERROR("Can't unpack obd_quotactl\n");
 			rc = -EPROTO;
 		}
 	} else if (!rc) {
@@ -1762,7 +1817,7 @@ static int mdc_ioc_swap_layouts(struct obd_export *exp,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 out:
 	ptlrpc_req_finished(req);
@@ -1775,7 +1830,6 @@ static int mdc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 	struct obd_device *obd = exp->exp_obd;
 	struct obd_ioctl_data *data = karg;
 	struct obd_import *imp = obd->u.cli.cl_import;
-	struct llog_ctxt *ctxt;
 	int rc;
 
 	if (!try_module_get(THIS_MODULE)) {
@@ -1785,68 +1839,58 @@ static int mdc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 	switch (cmd) {
 	case OBD_IOC_CHANGELOG_SEND:
 		rc = mdc_ioc_changelog_send(obd, karg);
-		GOTO(out, rc);
+		goto out;
 	case OBD_IOC_CHANGELOG_CLEAR: {
 		struct ioc_changelog *icc = karg;
-		struct changelog_setinfo cs =
-			{.cs_recno = icc->icc_recno, .cs_id = icc->icc_id};
+		struct changelog_setinfo cs = {
+			.cs_recno = icc->icc_recno,
+			.cs_id = icc->icc_id
+		};
+
 		rc = obd_set_info_async(NULL, exp, strlen(KEY_CHANGELOG_CLEAR),
 					KEY_CHANGELOG_CLEAR, sizeof(cs), &cs,
 					NULL);
-		GOTO(out, rc);
+		goto out;
 	}
 	case OBD_IOC_FID2PATH:
 		rc = mdc_ioc_fid2path(exp, karg);
-		GOTO(out, rc);
+		goto out;
 	case LL_IOC_HSM_CT_START:
 		rc = mdc_ioc_hsm_ct_start(exp, karg);
 		/* ignore if it was already registered on this MDS. */
 		if (rc == -EEXIST)
 			rc = 0;
-		GOTO(out, rc);
+		goto out;
 	case LL_IOC_HSM_PROGRESS:
 		rc = mdc_ioc_hsm_progress(exp, karg);
-		GOTO(out, rc);
+		goto out;
 	case LL_IOC_HSM_STATE_GET:
 		rc = mdc_ioc_hsm_state_get(exp, karg);
-		GOTO(out, rc);
+		goto out;
 	case LL_IOC_HSM_STATE_SET:
 		rc = mdc_ioc_hsm_state_set(exp, karg);
-		GOTO(out, rc);
+		goto out;
 	case LL_IOC_HSM_ACTION:
 		rc = mdc_ioc_hsm_current_action(exp, karg);
-		GOTO(out, rc);
+		goto out;
 	case LL_IOC_HSM_REQUEST:
 		rc = mdc_ioc_hsm_request(exp, karg);
-		GOTO(out, rc);
+		goto out;
 	case OBD_IOC_CLIENT_RECOVER:
 		rc = ptlrpc_recover_import(imp, data->ioc_inlbuf1, 0);
 		if (rc < 0)
-			GOTO(out, rc);
-		GOTO(out, rc = 0);
+			goto out;
+		rc = 0;
+		goto out;
 	case IOC_OSC_SET_ACTIVE:
 		rc = ptlrpc_set_import_active(imp, data->ioc_offset);
-		GOTO(out, rc);
-	case OBD_IOC_PARSE: {
-		ctxt = llog_get_context(exp->exp_obd, LLOG_CONFIG_REPL_CTXT);
-		rc = class_config_parse_llog(NULL, ctxt, data->ioc_inlbuf1,
-					     NULL);
-		llog_ctxt_put(ctxt);
-		GOTO(out, rc);
-	}
-	case OBD_IOC_LLOG_INFO:
-	case OBD_IOC_LLOG_PRINT: {
-		ctxt = llog_get_context(obd, LLOG_CONFIG_REPL_CTXT);
-		rc = llog_ioctl(NULL, ctxt, cmd, data);
-		llog_ctxt_put(ctxt);
-		GOTO(out, rc);
-	}
+		goto out;
 	case OBD_IOC_POLL_QUOTACHECK:
 		rc = mdc_quota_poll_check(exp, (struct if_quotacheck *)karg);
-		GOTO(out, rc);
+		goto out;
 	case OBD_IOC_PING_TARGET:
 		rc = ptlrpc_obd_ping(obd);
-		GOTO(out, rc);
+		goto out;
 	/*
 	 * Normally IOC_OBD_STATFS, OBD_IOC_QUOTACTL iocontrol are handled by
 	 * LMV instead of MDC. But when the cluster is upgraded from 1.8,
@@ -1857,35 +1901,44 @@ static int mdc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 	case IOC_OBD_STATFS: {
 		struct obd_statfs stat_buf = {0};
 
-		if (*((__u32 *) data->ioc_inlbuf2) != 0)
-			GOTO(out, rc = -ENODEV);
+		if (*((__u32 *) data->ioc_inlbuf2) != 0) {
+			rc = -ENODEV;
+			goto out;
+		}
 
 		/* copy UUID */
 		if (copy_to_user(data->ioc_pbuf2, obd2cli_tgt(obd),
 				     min((int) data->ioc_plen2,
-					 (int) sizeof(struct obd_uuid))))
-			GOTO(out, rc = -EFAULT);
+					 (int) sizeof(struct obd_uuid)))) {
+			rc = -EFAULT;
+			goto out;
+		}
 
 		rc = mdc_statfs(NULL, obd->obd_self_export, &stat_buf,
 				cfs_time_shift_64(-OBD_STATFS_CACHE_SECONDS),
 				0);
 		if (rc != 0)
-			GOTO(out, rc);
+			goto out;
 
 		if (copy_to_user(data->ioc_pbuf1, &stat_buf,
 				     min((int) data->ioc_plen1,
-					 (int) sizeof(stat_buf))))
-			GOTO(out, rc = -EFAULT);
+					 (int) sizeof(stat_buf)))) {
+			rc = -EFAULT;
+			goto out;
+		}
 
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 	case OBD_IOC_QUOTACTL: {
 		struct if_quotactl *qctl = karg;
 		struct obd_quotactl *oqctl;
 
 		OBD_ALLOC_PTR(oqctl);
-		if (oqctl == NULL)
-			GOTO(out, rc = -ENOMEM);
+		if (oqctl == NULL) {
+			rc = -ENOMEM;
+			goto out;
+		}
 
 		QCTL_COPY(oqctl, qctl);
 		rc = obd_quotactl(exp, oqctl);
@@ -1896,20 +1949,24 @@ static int mdc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		}
 
 		OBD_FREE_PTR(oqctl);
-		GOTO(out, rc);
+		goto out;
 	}
 	case LL_IOC_GET_CONNECT_FLAGS:
 		if (copy_to_user(uarg, exp_connect_flags_ptr(exp),
-				 sizeof(*exp_connect_flags_ptr(exp))))
-			GOTO(out, rc = -EFAULT);
+				 sizeof(*exp_connect_flags_ptr(exp)))) {
+			rc = -EFAULT;
+			goto out;
+		}
 
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	case LL_IOC_LOV_SWAP_LAYOUTS:
 		rc = mdc_ioc_swap_layouts(exp, karg);
-		GOTO(out, rc);
+		goto out;
 	default:
 		CERROR("unrecognised ioctl: cmd = %#x\n", cmd);
-		GOTO(out, rc = -ENOTTY);
+		rc = -ENOTTY;
+		goto out;
 	}
 out:
 	module_put(THIS_MODULE);
@@ -1918,7 +1975,7 @@ out:
 }
 
 int mdc_get_info_rpc(struct obd_export *exp,
-		     obd_count keylen, void *key,
+		     u32 keylen, void *key,
 		     int vallen, void *val)
 {
 	struct obd_import      *imp = class_exp2cliimp(exp);
@@ -2049,8 +2106,8 @@ static int mdc_hsm_copytool_send(int len, void *val)
 		return -EPROTO;
 	}
 
-	CDEBUG(D_HSM, " Received message mg=%x t=%d m=%d l=%d actions=%d "
-	       "on %s\n",
+	CDEBUG(D_HSM,
+	       "Received message mg=%x t=%d m=%d l=%d actions=%d on %s\n",
 	       lh->kuc_magic, lh->kuc_transport, lh->kuc_msgtype,
 	       lh->kuc_msglen, hal->hal_count, hal->hal_fsname);
 
@@ -2093,8 +2150,8 @@ static int mdc_kuc_reregister(struct obd_import *imp)
 
 int mdc_set_info_async(const struct lu_env *env,
 		       struct obd_export *exp,
-		       obd_count keylen, void *key,
-		       obd_count vallen, void *val,
+		       u32 keylen, void *key,
+		       u32 vallen, void *val,
 		       struct ptlrpc_request_set *set)
 {
 	struct obd_import	*imp = class_exp2cliimp(exp);
@@ -2206,91 +2263,6 @@ int mdc_get_info(const struct lu_env *env, struct obd_export *exp,
 	return rc;
 }
 
-static int mdc_pin(struct obd_export *exp, const struct lu_fid *fid,
-		   struct obd_capa *oc, struct obd_client_handle *handle,
-		   int flags)
-{
-	struct ptlrpc_request *req;
-	struct mdt_body       *body;
-	int		    rc;
-
-	req = ptlrpc_request_alloc(class_exp2cliimp(exp), &RQF_MDS_PIN);
-	if (req == NULL)
-		return -ENOMEM;
-
-	mdc_set_capa_size(req, &RMF_CAPA1, oc);
-
-	rc = ptlrpc_request_pack(req, LUSTRE_MDS_VERSION, MDS_PIN);
-	if (rc) {
-		ptlrpc_request_free(req);
-		return rc;
-	}
-
-	mdc_pack_body(req, fid, oc, 0, 0, -1, flags);
-
-	ptlrpc_request_set_replen(req);
-
-	mdc_get_rpc_lock(exp->exp_obd->u.cli.cl_rpc_lock, NULL);
-	rc = ptlrpc_queue_wait(req);
-	mdc_put_rpc_lock(exp->exp_obd->u.cli.cl_rpc_lock, NULL);
-	if (rc) {
-		CERROR("Pin failed: %d\n", rc);
-		GOTO(err_out, rc);
-	}
-
-	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
-	if (body == NULL)
-		GOTO(err_out, rc = -EPROTO);
-
-	handle->och_fh = body->handle;
-	handle->och_magic = OBD_CLIENT_HANDLE_MAGIC;
-
-	handle->och_mod = obd_mod_alloc();
-	if (handle->och_mod == NULL) {
-		DEBUG_REQ(D_ERROR, req, "can't allocate md_open_data");
-		GOTO(err_out, rc = -ENOMEM);
-	}
-	handle->och_mod->mod_open_req = req; /* will be dropped by unpin */
-
-	return 0;
-
-err_out:
-	ptlrpc_req_finished(req);
-	return rc;
-}
-
-static int mdc_unpin(struct obd_export *exp, struct obd_client_handle *handle,
-		     int flag)
-{
-	struct ptlrpc_request *req;
-	struct mdt_body       *body;
-	int		    rc;
-
-	req = ptlrpc_request_alloc_pack(class_exp2cliimp(exp), &RQF_MDS_UNPIN,
-					LUSTRE_MDS_VERSION, MDS_UNPIN);
-	if (req == NULL)
-		return -ENOMEM;
-
-	body = req_capsule_client_get(&req->rq_pill, &RMF_MDT_BODY);
-	body->handle = handle->och_fh;
-	body->flags = flag;
-
-	ptlrpc_request_set_replen(req);
-
-	mdc_get_rpc_lock(exp->exp_obd->u.cli.cl_rpc_lock, NULL);
-	rc = ptlrpc_queue_wait(req);
-	mdc_put_rpc_lock(exp->exp_obd->u.cli.cl_rpc_lock, NULL);
-
-	if (rc != 0)
-		CERROR("Unpin failed: %d\n", rc);
-
-	ptlrpc_req_finished(req);
-	ptlrpc_req_finished(handle->och_mod->mod_open_req);
-
-	obd_mod_put(handle->och_mod);
-	return rc;
-}
-
 int mdc_sync(struct obd_export *exp, const struct lu_fid *fid,
 	     struct obd_capa *oc, struct ptlrpc_request **request)
 {
@@ -2384,8 +2356,10 @@ int mdc_fid_alloc(struct obd_export *exp, struct lu_fid *fid,
 	return seq_client_alloc_fid(NULL, seq, fid);
 }
 
-struct obd_uuid *mdc_get_uuid(struct obd_export *exp) {
+struct obd_uuid *mdc_get_uuid(struct obd_export *exp)
+{
 	struct client_obd *cli = &exp->exp_obd->u.cli;
+
 	return &cli->cl_target_uuid;
 }
 
@@ -2420,27 +2394,56 @@ struct ldlm_valblock_ops inode_lvbo = {
 	.lvbo_free = mdc_resource_inode_free,
 };
 
+static int mdc_llog_init(struct obd_device *obd)
+{
+	struct obd_llog_group	*olg = &obd->obd_olg;
+	struct llog_ctxt	*ctxt;
+	int			 rc;
+
+	rc = llog_setup(NULL, obd, olg, LLOG_CHANGELOG_REPL_CTXT, obd,
+			&llog_client_ops);
+	if (rc)
+		return rc;
+
+	ctxt = llog_group_get_ctxt(olg, LLOG_CHANGELOG_REPL_CTXT);
+	llog_initiator_connect(ctxt);
+	llog_ctxt_put(ctxt);
+
+	return 0;
+}
+
+static void mdc_llog_finish(struct obd_device *obd)
+{
+	struct llog_ctxt *ctxt;
+
+	ctxt = llog_get_context(obd, LLOG_CHANGELOG_REPL_CTXT);
+	if (ctxt)
+		llog_cleanup(NULL, ctxt);
+}
+
 static int mdc_setup(struct obd_device *obd, struct lustre_cfg *cfg)
 {
 	struct client_obd *cli = &obd->u.cli;
 	struct lprocfs_static_vars lvars = { NULL };
 	int rc;
 
-	OBD_ALLOC(cli->cl_rpc_lock, sizeof (*cli->cl_rpc_lock));
+	OBD_ALLOC(cli->cl_rpc_lock, sizeof(*cli->cl_rpc_lock));
 	if (!cli->cl_rpc_lock)
 		return -ENOMEM;
 	mdc_init_rpc_lock(cli->cl_rpc_lock);
 
 	ptlrpcd_addref();
 
-	OBD_ALLOC(cli->cl_close_lock, sizeof (*cli->cl_close_lock));
-	if (!cli->cl_close_lock)
-		GOTO(err_rpc_lock, rc = -ENOMEM);
+	OBD_ALLOC(cli->cl_close_lock, sizeof(*cli->cl_close_lock));
+	if (!cli->cl_close_lock) {
+		rc = -ENOMEM;
+		goto err_rpc_lock;
+	}
 	mdc_init_rpc_lock(cli->cl_close_lock);
 
 	rc = client_obd_setup(obd, cfg);
 	if (rc)
-		GOTO(err_close_lock, rc);
+		goto err_close_lock;
 	lprocfs_mdc_init_vars(&lvars);
 	lprocfs_obd_setup(obd, lvars.obd_vars);
 	sptlrpc_lprocfs_cliobd_attach(obd);
@@ -2450,7 +2453,7 @@ static int mdc_setup(struct obd_device *obd, struct lustre_cfg *cfg)
 
 	obd->obd_namespace->ns_lvbo = &inode_lvbo;
 
-	rc = obd_llog_init(obd, &obd->obd_olg, obd, NULL);
+	rc = mdc_llog_init(obd);
 	if (rc) {
 		mdc_cleanup(obd);
 		CERROR("failed to setup llogging subsystems\n");
@@ -2459,9 +2462,9 @@ static int mdc_setup(struct obd_device *obd, struct lustre_cfg *cfg)
 	return rc;
 
 err_close_lock:
-	OBD_FREE(cli->cl_close_lock, sizeof (*cli->cl_close_lock));
+	OBD_FREE(cli->cl_close_lock, sizeof(*cli->cl_close_lock));
 err_rpc_lock:
-	OBD_FREE(cli->cl_rpc_lock, sizeof (*cli->cl_rpc_lock));
+	OBD_FREE(cli->cl_rpc_lock, sizeof(*cli->cl_rpc_lock));
 	ptlrpcd_decref();
 	return rc;
 }
@@ -2497,8 +2500,6 @@ static int mdc_init_ea_size(struct obd_export *exp, int easize,
 
 static int mdc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 {
-	int rc = 0;
-
 	switch (stage) {
 	case OBD_CLEANUP_EARLY:
 		break;
@@ -2511,59 +2512,25 @@ static int mdc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 		ptlrpc_lprocfs_unregister_obd(obd);
 		lprocfs_obd_cleanup(obd);
 
-		rc = obd_llog_finish(obd, 0);
-		if (rc != 0)
-			CERROR("failed to cleanup llogging subsystems\n");
+		mdc_llog_finish(obd);
 		break;
 	}
-	return rc;
+	return 0;
 }
 
 static int mdc_cleanup(struct obd_device *obd)
 {
 	struct client_obd *cli = &obd->u.cli;
 
-	OBD_FREE(cli->cl_rpc_lock, sizeof (*cli->cl_rpc_lock));
-	OBD_FREE(cli->cl_close_lock, sizeof (*cli->cl_close_lock));
+	OBD_FREE(cli->cl_rpc_lock, sizeof(*cli->cl_rpc_lock));
+	OBD_FREE(cli->cl_close_lock, sizeof(*cli->cl_close_lock));
 
 	ptlrpcd_decref();
 
 	return client_obd_cleanup(obd);
 }
 
-
-static int mdc_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-			 struct obd_device *tgt, int *index)
-{
-	struct llog_ctxt	*ctxt;
-	int			 rc;
-
-	LASSERT(olg == &obd->obd_olg);
-
-	rc = llog_setup(NULL, obd, olg, LLOG_CHANGELOG_REPL_CTXT, tgt,
-			&llog_client_ops);
-	if (rc)
-		return rc;
-
-	ctxt = llog_group_get_ctxt(olg, LLOG_CHANGELOG_REPL_CTXT);
-	llog_initiator_connect(ctxt);
-	llog_ctxt_put(ctxt);
-
-	return 0;
-}
-
-static int mdc_llog_finish(struct obd_device *obd, int count)
-{
-	struct llog_ctxt *ctxt;
-
-	ctxt = llog_get_context(obd, LLOG_CHANGELOG_REPL_CTXT);
-	if (ctxt)
-		llog_cleanup(NULL, ctxt);
-
-	return 0;
-}
-
-static int mdc_process_config(struct obd_device *obd, obd_count len, void *buf)
+static int mdc_process_config(struct obd_device *obd, u32 len, void *buf)
 {
 	struct lustre_cfg *lcfg = buf;
 	struct lprocfs_static_vars lvars = { NULL };
@@ -2578,7 +2545,7 @@ static int mdc_process_config(struct obd_device *obd, obd_count len, void *buf)
 			rc = 0;
 		break;
 	}
-	return(rc);
+	return rc;
 }
 
 
@@ -2628,19 +2595,27 @@ static int mdc_interpret_renew_capa(const struct lu_env *env,
 	struct mdt_body *body = NULL;
 	struct lustre_capa *capa;
 
-	if (status)
-		GOTO(out, capa = ERR_PTR(status));
+	if (status) {
+		capa = ERR_PTR(status);
+		goto out;
+	}
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_MDT_BODY);
-	if (body == NULL)
-		GOTO(out, capa = ERR_PTR(-EFAULT));
+	if (body == NULL) {
+		capa = ERR_PTR(-EFAULT);
+		goto out;
+	}
 
-	if ((body->valid & OBD_MD_FLOSSCAPA) == 0)
-		GOTO(out, capa = ERR_PTR(-ENOENT));
+	if ((body->valid & OBD_MD_FLOSSCAPA) == 0) {
+		capa = ERR_PTR(-ENOENT);
+		goto out;
+	}
 
 	capa = req_capsule_server_get(&req->rq_pill, &RMF_CAPA2);
-	if (!capa)
-		GOTO(out, capa = ERR_PTR(-EFAULT));
+	if (!capa) {
+		capa = ERR_PTR(-EFAULT);
+		goto out;
+	}
 out:
 	ra->ra_cb(ra->ra_oc, capa);
 	return 0;
@@ -2684,14 +2659,10 @@ struct obd_ops mdc_obd_ops = {
 	.o_iocontrol	= mdc_iocontrol,
 	.o_set_info_async   = mdc_set_info_async,
 	.o_statfs	   = mdc_statfs,
-	.o_pin	      = mdc_pin,
-	.o_unpin	    = mdc_unpin,
 	.o_fid_init	    = client_fid_init,
 	.o_fid_fini	    = client_fid_fini,
 	.o_fid_alloc	= mdc_fid_alloc,
 	.o_import_event     = mdc_import_event,
-	.o_llog_init	= mdc_llog_init,
-	.o_llog_finish      = mdc_llog_finish,
 	.o_get_info	 = mdc_get_info,
 	.o_process_config   = mdc_process_config,
 	.o_get_uuid	 = mdc_get_uuid,
@@ -2738,6 +2709,7 @@ int __init mdc_init(void)
 {
 	int rc;
 	struct lprocfs_static_vars lvars = { NULL };
+
 	lprocfs_mdc_init_vars(&lvars);
 
 	rc = class_register_type(&mdc_obd_ops, &mdc_md_ops, lvars.module_vars,
diff --git a/drivers/staging/lustre/lustre/mgc/libmgc.c b/drivers/staging/lustre/lustre/mgc/libmgc.c
deleted file mode 100644
index 8012f0f1bfcd..000000000000
--- a/drivers/staging/lustre/lustre/mgc/libmgc.c
+++ /dev/null
@@ -1,158 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/mgc/libmgc.c
- *
- * Lustre Management Client
- * Author: Nathan Rutman <nathan@clusterfs.com>
- */
-
-/* Minimal MGC for liblustre: only used to read the config log from the MGS
-   at setup time, no updates. */
-
-#define DEBUG_SUBSYSTEM S_MGC
-
-#include "../include/liblustre.h"
-
-#include "../include/obd_class.h"
-#include "../include/lustre_dlm.h"
-#include "../include/lustre_log.h"
-#include "../include/lustre_fsfilt.h"
-#include "../include/lustre_disk.h"
-
-
-static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
-{
-	int rc;
-
-	ptlrpcd_addref();
-
-	rc = client_obd_setup(obd, lcfg);
-	if (rc)
-		GOTO(err_decref, rc);
-
-	/* liblustre only support null flavor to MGS */
-	obd->u.cli.cl_flvr_mgc.sf_rpc = SPTLRPC_FLVR_NULL;
-
-	rc = obd_llog_init(obd, &obd->obd_olg, obd, NULL);
-	if (rc) {
-		CERROR("failed to setup llogging subsystems\n");
-		GOTO(err_cleanup, rc);
-	}
-
-	return rc;
-
-err_cleanup:
-	client_obd_cleanup(obd);
-err_decref:
-	ptlrpcd_decref();
-	return rc;
-}
-
-static int mgc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
-{
-	int rc = 0;
-
-	switch (stage) {
-	case OBD_CLEANUP_EARLY:
-	case OBD_CLEANUP_EXPORTS:
-		obd_cleanup_client_import(obd);
-		rc = obd_llog_finish(obd, 0);
-		if (rc != 0)
-			CERROR("failed to cleanup llogging subsystems\n");
-		break;
-	}
-	return rc;
-}
-
-static int mgc_cleanup(struct obd_device *obd)
-{
-	int rc;
-
-	ptlrpcd_decref();
-
-	rc = client_obd_cleanup(obd);
-	return rc;
-}
-
-static int mgc_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-			 struct obd_device *tgt, int *index)
-{
-	struct llog_ctxt *ctxt;
-	int rc;
-
-	LASSERT(olg == &obd->obd_olg);
-	rc = llog_setup(NULL, obd, olg, LLOG_CONFIG_REPL_CTXT, tgt,
-			&llog_client_ops);
-	if (rc < 0)
-		return rc;
-
-	ctxt = llog_group_get_ctxt(olg, LLOG_CONFIG_REPL_CTXT);
-	llog_initiator_connect(ctxt);
-	llog_ctxt_put(ctxt);
-
-	return rc;
-}
-
-static int mgc_llog_finish(struct obd_device *obd, int count)
-{
-	struct llog_ctxt *ctxt;
-
-
-	ctxt = llog_get_context(obd, LLOG_CONFIG_REPL_CTXT);
-	if (ctxt)
-		llog_cleanup(NULL, ctxt);
-
-	return 0;
-}
-
-struct obd_ops mgc_obd_ops = {
-	.o_owner	= THIS_MODULE,
-	.o_setup	= mgc_setup,
-	.o_precleanup   = mgc_precleanup,
-	.o_cleanup      = mgc_cleanup,
-	.o_add_conn     = client_import_add_conn,
-	.o_del_conn     = client_import_del_conn,
-	.o_connect      = client_connect_import,
-	.o_disconnect   = client_disconnect_export,
-	.o_llog_init    = mgc_llog_init,
-	.o_llog_finish  = mgc_llog_finish,
-};
-
-int __init mgc_init(void)
-{
-	return class_register_type(&mgc_obd_ops, NULL,
-				   NULL, LUSTRE_MGC_NAME, NULL);
-}
diff --git a/drivers/staging/lustre/lustre/mgc/mgc_request.c b/drivers/staging/lustre/lustre/mgc/mgc_request.c
index f520591d5784..bc263adf09d4 100644
--- a/drivers/staging/lustre/lustre/mgc/mgc_request.c
+++ b/drivers/staging/lustre/lustre/mgc/mgc_request.c
@@ -47,7 +47,6 @@
 #include "../include/lprocfs_status.h"
 #include "../include/lustre_log.h"
 #include "../include/lustre_disk.h"
-#include "../include/dt_object.h"
 
 #include "mgc_internal.h"
 
@@ -329,7 +328,8 @@ static int config_log_add(struct obd_device *obd, char *logname,
 						CONFIG_T_SPTLRPC, NULL, NULL);
 		if (IS_ERR(sptlrpc_cld)) {
 			CERROR("can't create sptlrpc log: %s\n", seclogname);
-			GOTO(out_err, rc = PTR_ERR(sptlrpc_cld));
+			rc = PTR_ERR(sptlrpc_cld);
+			goto out_err;
 		}
 	}
 	params_cld = config_params_log_add(obd, cfg, sb);
@@ -337,13 +337,14 @@ static int config_log_add(struct obd_device *obd, char *logname,
 		rc = PTR_ERR(params_cld);
 		CERROR("%s: can't create params log: rc = %d\n",
 		       obd->obd_name, rc);
-		GOTO(out_err1, rc);
+		goto out_err1;
 	}
 
 	cld = do_config_log_add(obd, logname, CONFIG_T_CONFIG, cfg, sb);
 	if (IS_ERR(cld)) {
 		CERROR("can't create log: %s\n", logname);
-		GOTO(out_err2, rc = PTR_ERR(cld));
+		rc = PTR_ERR(cld);
+		goto out_err2;
 	}
 
 	cld->cld_sptlrpc = sptlrpc_cld;
@@ -354,8 +355,10 @@ static int config_log_add(struct obd_device *obd, char *logname,
 		struct config_llog_data *recover_cld;
 		*strrchr(seclogname, '-') = 0;
 		recover_cld = config_recover_log_add(obd, seclogname, cfg, sb);
-		if (IS_ERR(recover_cld))
-			GOTO(out_err3, rc = PTR_ERR(recover_cld));
+		if (IS_ERR(recover_cld)) {
+			rc = PTR_ERR(recover_cld);
+			goto out_err3;
+		}
 		cld->cld_recover = recover_cld;
 	}
 
@@ -624,146 +627,6 @@ static void mgc_requeue_add(struct config_llog_data *cld)
 	}
 }
 
-/********************** class fns **********************/
-static int mgc_local_llog_init(const struct lu_env *env,
-			       struct obd_device *obd,
-			       struct obd_device *disk)
-{
-	struct llog_ctxt	*ctxt;
-	int			 rc;
-
-	rc = llog_setup(env, obd, &obd->obd_olg, LLOG_CONFIG_ORIG_CTXT, disk,
-			&llog_osd_ops);
-	if (rc)
-		return rc;
-
-	ctxt = llog_get_context(obd, LLOG_CONFIG_ORIG_CTXT);
-	LASSERT(ctxt);
-	ctxt->loc_dir = obd->u.cli.cl_mgc_configs_dir;
-	llog_ctxt_put(ctxt);
-
-	return 0;
-}
-
-static int mgc_local_llog_fini(const struct lu_env *env,
-			       struct obd_device *obd)
-{
-	struct llog_ctxt *ctxt;
-
-	ctxt = llog_get_context(obd, LLOG_CONFIG_ORIG_CTXT);
-	llog_cleanup(env, ctxt);
-
-	return 0;
-}
-
-static int mgc_fs_setup(struct obd_device *obd, struct super_block *sb)
-{
-	struct lustre_sb_info	*lsi = s2lsi(sb);
-	struct client_obd	*cli = &obd->u.cli;
-	struct lu_fid		 rfid, fid;
-	struct dt_object	*root, *dto;
-	struct lu_env		*env;
-	int			 rc = 0;
-
-	LASSERT(lsi);
-	LASSERT(lsi->lsi_dt_dev);
-
-	OBD_ALLOC_PTR(env);
-	if (env == NULL)
-		return -ENOMEM;
-
-	/* The mgc fs exclusion mutex. Only one fs can be setup at a time. */
-	mutex_lock(&cli->cl_mgc_mutex);
-
-	cfs_cleanup_group_info();
-
-	/* Setup the configs dir */
-	rc = lu_env_init(env, LCT_MG_THREAD);
-	if (rc)
-		GOTO(out_err, rc);
-
-	fid.f_seq = FID_SEQ_LOCAL_NAME;
-	fid.f_oid = 1;
-	fid.f_ver = 0;
-	rc = local_oid_storage_init(env, lsi->lsi_dt_dev, &fid,
-				    &cli->cl_mgc_los);
-	if (rc)
-		GOTO(out_env, rc);
-
-	rc = dt_root_get(env, lsi->lsi_dt_dev, &rfid);
-	if (rc)
-		GOTO(out_env, rc);
-
-	root = dt_locate_at(env, lsi->lsi_dt_dev, &rfid,
-			    &cli->cl_mgc_los->los_dev->dd_lu_dev);
-	if (unlikely(IS_ERR(root)))
-		GOTO(out_los, rc = PTR_ERR(root));
-
-	dto = local_file_find_or_create(env, cli->cl_mgc_los, root,
-					MOUNT_CONFIGS_DIR,
-					S_IFDIR | S_IRUGO | S_IWUSR | S_IXUGO);
-	lu_object_put_nocache(env, &root->do_lu);
-	if (IS_ERR(dto))
-		GOTO(out_los, rc = PTR_ERR(dto));
-
-	cli->cl_mgc_configs_dir = dto;
-
-	LASSERT(lsi->lsi_osd_exp->exp_obd->obd_lvfs_ctxt.dt);
-	rc = mgc_local_llog_init(env, obd, lsi->lsi_osd_exp->exp_obd);
-	if (rc)
-		GOTO(out_llog, rc);
-
-	/* We take an obd ref to insure that we can't get to mgc_cleanup
-	 * without calling mgc_fs_cleanup first. */
-	class_incref(obd, "mgc_fs", obd);
-
-	/* We keep the cl_mgc_sem until mgc_fs_cleanup */
-out_llog:
-	if (rc) {
-		lu_object_put(env, &cli->cl_mgc_configs_dir->do_lu);
-		cli->cl_mgc_configs_dir = NULL;
-	}
-out_los:
-	if (rc < 0) {
-		local_oid_storage_fini(env, cli->cl_mgc_los);
-		cli->cl_mgc_los = NULL;
-		mutex_unlock(&cli->cl_mgc_mutex);
-	}
-out_env:
-	lu_env_fini(env);
-out_err:
-	OBD_FREE_PTR(env);
-	return rc;
-}
-
-static int mgc_fs_cleanup(struct obd_device *obd)
-{
-	struct lu_env		 env;
-	struct client_obd	*cli = &obd->u.cli;
-	int			 rc;
-
-	LASSERT(cli->cl_mgc_los != NULL);
-
-	rc = lu_env_init(&env, LCT_MG_THREAD);
-	if (rc)
-		GOTO(unlock, rc);
-
-	mgc_local_llog_fini(&env, obd);
-
-	lu_object_put_nocache(&env, &cli->cl_mgc_configs_dir->do_lu);
-	cli->cl_mgc_configs_dir = NULL;
-
-	local_oid_storage_fini(&env, cli->cl_mgc_los);
-	cli->cl_mgc_los = NULL;
-	lu_env_fini(&env);
-
-unlock:
-	class_decref(obd, "mgc_fs", obd);
-	mutex_unlock(&cli->cl_mgc_mutex);
-
-	return 0;
-}
-
 static int mgc_llog_init(const struct lu_env *env, struct obd_device *obd)
 {
 	struct llog_ctxt	*ctxt;
@@ -853,12 +716,12 @@ static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 
 	rc = client_obd_setup(obd, lcfg);
 	if (rc)
-		GOTO(err_decref, rc);
+		goto err_decref;
 
 	rc = mgc_llog_init(NULL, obd);
 	if (rc) {
 		CERROR("failed to setup llogging subsystems\n");
-		GOTO(err_cleanup, rc);
+		goto err_cleanup;
 	}
 
 	lprocfs_mgc_init_vars(&lvars);
@@ -876,7 +739,7 @@ static int mgc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 			CERROR("%s: Cannot start requeue thread (%d),"
 			       "no more log updates!\n",
 			       obd->obd_name, rc);
-			GOTO(err_cleanup, rc);
+			goto err_cleanup;
 		}
 		/* rc is the task_struct pointer of mgc_requeue_thread. */
 		rc = 0;
@@ -1041,14 +904,6 @@ static int mgc_enqueue(struct obd_export *exp, struct lov_stripe_md *lsm,
 	return rc;
 }
 
-static int mgc_cancel(struct obd_export *exp, struct lov_stripe_md *md,
-		      __u32 mode, struct lustre_handle *lockh)
-{
-	ldlm_lock_decref(lockh, mode);
-
-	return 0;
-}
-
 static void mgc_notify_active(struct obd_device *unused)
 {
 	/* wakeup mgc_requeue_thread to requeue mgc lock */
@@ -1100,7 +955,7 @@ static int mgc_target_register(struct obd_export *exp,
 }
 
 int mgc_set_info_async(const struct lu_env *env, struct obd_export *exp,
-		       obd_count keylen, void *key, obd_count vallen,
+		       u32 keylen, void *key, u32 vallen,
 		       void *val, struct ptlrpc_request_set *set)
 {
 	int rc = -EINVAL;
@@ -1123,38 +978,6 @@ int mgc_set_info_async(const struct lu_env *env, struct obd_export *exp,
 			ptlrpc_reconnect_import(imp);
 		return 0;
 	}
-	/* FIXME move this to mgc_process_config */
-	if (KEY_IS(KEY_REGISTER_TARGET)) {
-		struct mgs_target_info *mti;
-		if (vallen != sizeof(struct mgs_target_info))
-			return -EINVAL;
-		mti = (struct mgs_target_info *)val;
-		CDEBUG(D_MGC, "register_target %s %#x\n",
-		       mti->mti_svname, mti->mti_flags);
-		rc =  mgc_target_register(exp, mti);
-		return rc;
-	}
-	if (KEY_IS(KEY_SET_FS)) {
-		struct super_block *sb = (struct super_block *)val;
-
-		if (vallen != sizeof(struct super_block))
-			return -EINVAL;
-
-		rc = mgc_fs_setup(exp->exp_obd, sb);
-		if (rc)
-			CERROR("set_fs got %d\n", rc);
-
-		return rc;
-	}
-	if (KEY_IS(KEY_CLEAR_FS)) {
-		if (vallen != 0)
-			return -EINVAL;
-		rc = mgc_fs_cleanup(exp->exp_obd);
-		if (rc)
-			CERROR("clear_fs got %d\n", rc);
-
-		return rc;
-	}
 	if (KEY_IS(KEY_SET_INFO)) {
 		struct mgs_send_param *msp;
 
@@ -1504,13 +1327,17 @@ static int mgc_process_recover_log(struct obd_device *obd,
 		nrpages = CONFIG_READ_NRPAGES_INIT;
 
 	OBD_ALLOC(pages, sizeof(*pages) * nrpages);
-	if (pages == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (pages == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	for (i = 0; i < nrpages; i++) {
 		pages[i] = alloc_page(GFP_IOFS);
-		if (pages[i] == NULL)
-			GOTO(out, rc = -ENOMEM);
+		if (pages[i] == NULL) {
+			rc = -ENOMEM;
+			goto out;
+		}
 	}
 
 again:
@@ -1518,20 +1345,24 @@ again:
 	LASSERT(mutex_is_locked(&cld->cld_lock));
 	req = ptlrpc_request_alloc(class_exp2cliimp(cld->cld_mgcexp),
 				   &RQF_MGS_CONFIG_READ);
-	if (req == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	rc = ptlrpc_request_pack(req, LUSTRE_MGS_VERSION, MGS_CONFIG_READ);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* pack request */
 	body = req_capsule_client_get(&req->rq_pill, &RMF_MGS_CONFIG_BODY);
 	LASSERT(body != NULL);
 	LASSERT(sizeof(body->mcb_name) > strlen(cld->cld_logname));
 	if (strlcpy(body->mcb_name, cld->cld_logname, sizeof(body->mcb_name))
-	    >= sizeof(body->mcb_name))
-		GOTO(out, rc = -E2BIG);
+	    >= sizeof(body->mcb_name)) {
+		rc = -E2BIG;
+		goto out;
+	}
 	body->mcb_offset = cfg->cfg_last_idx + 1;
 	body->mcb_type   = cld->cld_type;
 	body->mcb_bits   = PAGE_CACHE_SHIFT;
@@ -1540,8 +1371,10 @@ again:
 	/* allocate bulk transfer descriptor */
 	desc = ptlrpc_prep_bulk_imp(req, nrpages, 1, BULK_PUT_SINK,
 				    MGS_BULK_PORTAL);
-	if (desc == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (desc == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	for (i = 0; i < nrpages; i++)
 		ptlrpc_prep_bulk_page_pin(desc, pages[i], 0, PAGE_CACHE_SIZE);
@@ -1549,11 +1382,13 @@ again:
 	ptlrpc_request_set_replen(req);
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	res = req_capsule_server_get(&req->rq_pill, &RMF_MGS_CONFIG_RES);
-	if (res->mcr_size < res->mcr_offset)
-		GOTO(out, rc = -EINVAL);
+	if (res->mcr_size < res->mcr_offset) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	/* always update the index even though it might have errors with
 	 * handling the recover logs */
@@ -1564,16 +1399,20 @@ again:
 	       res->mcr_offset, eof == false);
 
 	ealen = sptlrpc_cli_unwrap_bulk_read(req, req->rq_bulk, 0);
-	if (ealen < 0)
-		GOTO(out, rc = ealen);
+	if (ealen < 0) {
+		rc = ealen;
+		goto out;
+	}
 
-	if (ealen > nrpages << PAGE_CACHE_SHIFT)
-		GOTO(out, rc = -EINVAL);
+	if (ealen > nrpages << PAGE_CACHE_SHIFT) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	if (ealen == 0) { /* no logs transferred */
 		if (!eof)
 			rc = -EINVAL;
-		GOTO(out, rc);
+		goto out;
 	}
 
 	mne_swab = !!ptlrpc_rep_need_swab(req);
@@ -1622,54 +1461,11 @@ out:
 	return rc;
 }
 
-static int mgc_llog_local_copy(const struct lu_env *env,
-			       struct obd_device *obd,
-			       struct llog_ctxt *rctxt,
-			       struct llog_ctxt *lctxt, char *logname)
-{
-	char	*temp_log;
-	int	 rc;
-
-
-
-	/*
-	 * - copy it to backup using llog_backup()
-	 * - copy remote llog to logname using llog_backup()
-	 * - if failed then move backup to logname again
-	 */
-
-	OBD_ALLOC(temp_log, strlen(logname) + 1);
-	if (!temp_log)
-		return -ENOMEM;
-	sprintf(temp_log, "%sT", logname);
-
-	/* make a copy of local llog at first */
-	rc = llog_backup(env, obd, lctxt, lctxt, logname, temp_log);
-	if (rc < 0 && rc != -ENOENT)
-		GOTO(out, rc);
-	/* copy remote llog to the local copy */
-	rc = llog_backup(env, obd, rctxt, lctxt, logname, logname);
-	if (rc == -ENOENT) {
-		/* no remote llog, delete local one too */
-		llog_erase(env, lctxt, NULL, logname);
-	} else if (rc < 0) {
-		/* error during backup, get local one back from the copy */
-		llog_backup(env, obd, lctxt, lctxt, temp_log, logname);
-out:
-		CERROR("%s: failed to copy remote log %s: rc = %d\n",
-		       obd->obd_name, logname, rc);
-	}
-	llog_erase(env, lctxt, NULL, temp_log);
-	OBD_FREE(temp_log, strlen(logname) + 1);
-	return rc;
-}
-
 /* local_only means it cannot get remote llogs */
 static int mgc_process_cfg_log(struct obd_device *mgc,
 			       struct config_llog_data *cld, int local_only)
 {
-	struct llog_ctxt	*ctxt, *lctxt = NULL;
-	struct dt_object        *cl_mgc_dir = mgc->u.cli.cl_mgc_configs_dir;
+	struct llog_ctxt	*ctxt;
 	struct lustre_sb_info	*lsi = NULL;
 	int			 rc = 0;
 	bool			 sptlrpc_started = false;
@@ -1694,42 +1490,14 @@ static int mgc_process_cfg_log(struct obd_device *mgc,
 
 	rc = lu_env_init(env, LCT_MG_THREAD);
 	if (rc)
-		GOTO(out_free, rc);
+		goto out_free;
 
 	ctxt = llog_get_context(mgc, LLOG_CONFIG_REPL_CTXT);
 	LASSERT(ctxt);
 
-	lctxt = llog_get_context(mgc, LLOG_CONFIG_ORIG_CTXT);
-
-	/* Copy the setup log locally if we can. Don't mess around if we're
-	 * running an MGS though (logs are already local). */
-	if (lctxt && lsi && IS_SERVER(lsi) && !IS_MGS(lsi) &&
-	    cl_mgc_dir != NULL &&
-	    lu2dt_dev(cl_mgc_dir->do_lu.lo_dev) == lsi->lsi_dt_dev) {
-		if (!local_only)
-			/* Only try to copy log if we have the lock. */
-			rc = mgc_llog_local_copy(env, mgc, ctxt, lctxt,
-						 cld->cld_logname);
-		if (local_only || rc) {
-			if (llog_is_empty(env, lctxt, cld->cld_logname)) {
-				LCONSOLE_ERROR_MSG(0x13a,
-						   "Failed to get MGS log %s and no local copy.\n",
-						   cld->cld_logname);
-				GOTO(out_pop, rc = -ENOENT);
-			}
-			CDEBUG(D_MGC,
-			       "Failed to get MGS log %s, using local copy for now, will try to update later.\n",
-			       cld->cld_logname);
-		}
-		/* Now, whether we copied or not, start using the local llog.
-		 * If we failed to copy, we'll start using whatever the old
-		 * log has. */
-		llog_ctxt_put(ctxt);
-		ctxt = lctxt;
-		lctxt = NULL;
-	} else {
-		if (local_only) /* no local log at client side */
-			GOTO(out_pop, rc = -EIO);
+	if (local_only) /* no local log at client side */ {
+		rc = -EIO;
+		goto out_pop;
 	}
 
 	if (cld_is_sptlrpc(cld)) {
@@ -1745,8 +1513,6 @@ static int mgc_process_cfg_log(struct obd_device *mgc,
 
 out_pop:
 	__llog_ctxt_put(env, ctxt);
-	if (lctxt)
-		__llog_ctxt_put(env, lctxt);
 
 	/*
 	 * update settings on existing OBDs. doing it inside
@@ -1829,12 +1595,8 @@ int mgc_process_log(struct obd_device *mgc, struct config_llog_data *cld)
 	mutex_unlock(&cld->cld_lock);
 
 	/* Now drop the lock so MGS can revoke it */
-	if (!rcl) {
-		rcl = mgc_cancel(mgc->u.cli.cl_mgc_mgsexp, NULL,
-				 LCK_CR, &lockh);
-		if (rcl)
-			CERROR("Can't drop cfg lock: %d\n", rcl);
-	}
+	if (!rcl)
+		ldlm_lock_decref(&lockh, LCK_CR);
 
 	return rc;
 }
@@ -1844,7 +1606,7 @@ int mgc_process_log(struct obd_device *mgc, struct config_llog_data *cld)
  * LCFG_LOG_START gets the config log from the MGS, processes it to start
  * any services, and adds it to the list logs to watch (follow).
  */
-static int mgc_process_config(struct obd_device *obd, obd_count len, void *buf)
+static int mgc_process_config(struct obd_device *obd, u32 len, void *buf)
 {
 	struct lustre_cfg *lcfg = buf;
 	struct config_llog_instance *cfg = NULL;
@@ -1857,8 +1619,10 @@ static int mgc_process_config(struct obd_device *obd, obd_count len, void *buf)
 		struct mgs_target_info *mti;
 
 		if (LUSTRE_CFG_BUFLEN(lcfg, 1) !=
-		    sizeof(struct mgs_target_info))
-			GOTO(out, rc = -EINVAL);
+		    sizeof(struct mgs_target_info)) {
+			rc = -EINVAL;
+			goto out;
+		}
 
 		mti = (struct mgs_target_info *)lustre_cfg_buf(lcfg, 1);
 		CDEBUG(D_MGC, "add_target %s %#x\n",
@@ -1943,7 +1707,8 @@ static int mgc_process_config(struct obd_device *obd, obd_count len, void *buf)
 	}
 	default: {
 		CERROR("Unknown command: %d\n", lcfg->lcfg_command);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 
 	}
 	}
@@ -1961,7 +1726,6 @@ struct obd_ops mgc_obd_ops = {
 	.o_connect      = client_connect_import,
 	.o_disconnect   = client_disconnect_export,
 	/* .o_enqueue      = mgc_enqueue, */
-	.o_cancel       = mgc_cancel,
 	/* .o_iocontrol    = mgc_iocontrol, */
 	.o_set_info_async = mgc_set_info_async,
 	.o_get_info       = mgc_get_info,
diff --git a/drivers/staging/lustre/lustre/obdclass/Makefile b/drivers/staging/lustre/lustre/obdclass/Makefile
index ba10043fdd50..e894681797c2 100644
--- a/drivers/staging/lustre/lustre/obdclass/Makefile
+++ b/drivers/staging/lustre/lustre/obdclass/Makefile
@@ -1,10 +1,11 @@
-obj-$(CONFIG_LUSTRE_FS) += obdclass.o llog_test.o
+obj-$(CONFIG_LUSTRE_FS) += obdclass.o
 
 obdclass-y := linux/linux-module.o linux/linux-obdo.o linux/linux-sysctl.o \
 	      llog.o llog_cat.o llog_obd.o llog_swab.o class_obd.o debug.o \
-	      genops.o uuid.o llog_ioctl.o lprocfs_status.o		   \
-	      lustre_handles.o lustre_peer.o llog_osd.o \
-	      local_storage.o statfs_pack.o obdo.o obd_config.o obd_mount.o\
-	      mea.o lu_object.o dt_object.o capa.o cl_object.o   \
-	      cl_page.o cl_lock.o cl_io.o lu_ref.o acl.o idmap.o	   \
-	      lu_ucred.o
+	      genops.o uuid.o lprocfs_status.o \
+	      lustre_handles.o lustre_peer.o \
+	      statfs_pack.o obdo.o obd_config.o obd_mount.o \
+	      lu_object.o dt_object.o capa.o cl_object.o   \
+	      cl_page.o cl_lock.o cl_io.o lu_ref.o acl.o
+
+obdclass-$(CONFIG_PROC_FS) += lprocfs_counters.o
diff --git a/drivers/staging/lustre/lustre/obdclass/acl.c b/drivers/staging/lustre/lustre/obdclass/acl.c
index 3b394a0bff9d..2619bfeceb8b 100644
--- a/drivers/staging/lustre/lustre/obdclass/acl.c
+++ b/drivers/staging/lustre/lustre/obdclass/acl.c
@@ -196,8 +196,10 @@ int lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, int size,
 		case ACL_GROUP_OBJ:
 		case ACL_MASK:
 		case ACL_OTHER:
-			if (id != ACL_UNDEFINED_ID)
-				GOTO(_out, rc = -EIO);
+			if (id != ACL_UNDEFINED_ID) {
+				rc = -EIO;
+				goto _out;
+			}
 
 			memcpy(&new->a_entries[j++], &header->a_entries[i],
 			       sizeof(posix_acl_xattr_entry));
@@ -215,7 +217,8 @@ int lustre_posix_acl_xattr_filter(posix_acl_xattr_header *header, int size,
 				       sizeof(posix_acl_xattr_entry));
 			break;
 		default:
-			GOTO(_out, rc = -EIO);
+			rc = -EIO;
+			goto _out;
 		}
 	}
 
@@ -318,8 +321,10 @@ int lustre_acl_xattr_merge2posix(posix_acl_xattr_header *posix_header, int size,
 			case ACL_USER_OBJ:
 			case ACL_GROUP_OBJ:
 			case ACL_OTHER:
-				if (ae.e_id != ACL_UNDEFINED_ID)
-					GOTO(_out, rc = -EIO);
+				if (ae.e_id != ACL_UNDEFINED_ID) {
+					rc = -EIO;
+					goto _out;
+				}
 
 				if (ae.e_stat != ES_DEL) {
 					new->a_entries[j].e_tag =
@@ -336,7 +341,8 @@ int lustre_acl_xattr_merge2posix(posix_acl_xattr_header *posix_header, int size,
 				if (ae.e_stat == ES_DEL)
 					break;
 			default:
-				GOTO(_out, rc = -EIO);
+				rc = -EIO;
+				goto _out;
 			}
 		}
 	} else {
@@ -437,8 +443,10 @@ lustre_acl_xattr_merge2ext(posix_acl_xattr_header *posix_header, int size,
 		case ACL_GROUP_OBJ:
 		case ACL_MASK:
 		case ACL_OTHER:
-			if (pae.e_id != ACL_UNDEFINED_ID)
-				GOTO(out, rc = -EIO);
+			if (pae.e_id != ACL_UNDEFINED_ID) {
+				rc = -EIO;
+				goto out;
+		}
 		case ACL_USER:
 			/* ignore "nobody" entry. */
 			if (pae.e_id == NOBODY_UID)
@@ -501,7 +509,8 @@ lustre_acl_xattr_merge2ext(posix_acl_xattr_header *posix_header, int size,
 			}
 			break;
 		default:
-			GOTO(out, rc = -EIO);
+			rc = -EIO;
+			goto out;
 		}
 	}
 
diff --git a/drivers/staging/lustre/lustre/obdclass/capa.c b/drivers/staging/lustre/lustre/obdclass/capa.c
index 5af61a8c0b85..cd1abce378ea 100644
--- a/drivers/staging/lustre/lustre/obdclass/capa.c
+++ b/drivers/staging/lustre/lustre/obdclass/capa.c
@@ -213,12 +213,11 @@ struct obd_capa *capa_add(struct hlist_head *hash, struct lustre_capa *capa)
 			capa_delete_lru(list);
 		spin_unlock(&capa_lock);
 		return ocapa;
-	} else {
-		capa_get(old);
-		spin_unlock(&capa_lock);
-		capa_put(ocapa);
-		return old;
 	}
+	capa_get(old);
+	spin_unlock(&capa_lock);
+	capa_put(ocapa);
+	return old;
 }
 EXPORT_SYMBOL(capa_add);
 
@@ -312,13 +311,14 @@ int capa_encrypt_id(__u32 *d, __u32 *s, __u8 *key, int keylen)
 	min = ll_crypto_tfm_alg_min_keysize(tfm);
 	if (keylen < min) {
 		CERROR("keylen at least %d bits for aes\n", min * 8);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	rc = crypto_blkcipher_setkey(tfm, key, min);
 	if (rc) {
 		CERROR("failed to setting key for aes\n");
-		GOTO(out, rc);
+		goto out;
 	}
 
 	sg_init_table(&sd, 1);
@@ -334,7 +334,7 @@ int capa_encrypt_id(__u32 *d, __u32 *s, __u8 *key, int keylen)
 	rc = crypto_blkcipher_encrypt(&desc, &sd, &ss, 16);
 	if (rc) {
 		CERROR("failed to encrypt for aes\n");
-		GOTO(out, rc);
+		goto out;
 	}
 
 out:
@@ -364,13 +364,14 @@ int capa_decrypt_id(__u32 *d, __u32 *s, __u8 *key, int keylen)
 	min = ll_crypto_tfm_alg_min_keysize(tfm);
 	if (keylen < min) {
 		CERROR("keylen at least %d bits for aes\n", min * 8);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	rc = crypto_blkcipher_setkey(tfm, key, min);
 	if (rc) {
 		CERROR("failed to setting key for aes\n");
-		GOTO(out, rc);
+		goto out;
 	}
 
 	sg_init_table(&sd, 1);
@@ -387,7 +388,7 @@ int capa_decrypt_id(__u32 *d, __u32 *s, __u8 *key, int keylen)
 	rc = crypto_blkcipher_decrypt(&desc, &sd, &ss, 16);
 	if (rc) {
 		CERROR("failed to decrypt for aes\n");
-		GOTO(out, rc);
+		goto out;
 	}
 
 out:
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_io.c b/drivers/staging/lustre/lustre/obdclass/cl_io.c
index 6870ee823736..f2383a497cbe 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_io.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_io.c
@@ -126,7 +126,7 @@ void cl_io_fini(const struct lu_env *env, struct cl_io *io)
 		info->clt_current_io = NULL;
 
 	/* sanity check for layout change */
-	switch(io->ci_type) {
+	switch (io->ci_type) {
 	case CIT_READ:
 	case CIT_WRITE:
 		break;
@@ -1452,12 +1452,13 @@ struct cl_req *cl_req_alloc(const struct lu_env *env, struct cl_page *page,
 	if (req != NULL) {
 		int result;
 
+		req->crq_type = crt;
+		INIT_LIST_HEAD(&req->crq_pages);
+		INIT_LIST_HEAD(&req->crq_layers);
+
 		OBD_ALLOC(req->crq_o, nr_objects * sizeof(req->crq_o[0]));
 		if (req->crq_o != NULL) {
 			req->crq_nrobjs = nr_objects;
-			req->crq_type = crt;
-			INIT_LIST_HEAD(&req->crq_pages);
-			INIT_LIST_HEAD(&req->crq_layers);
 			result = cl_req_init(env, req, page);
 		} else
 			result = -ENOMEM;
@@ -1559,7 +1560,7 @@ EXPORT_SYMBOL(cl_req_prep);
  * for the same request.
  */
 void cl_req_attr_set(const struct lu_env *env, struct cl_req *req,
-		     struct cl_req_attr *attr, obd_valid flags)
+		     struct cl_req_attr *attr, u64 flags)
 {
 	const struct cl_req_slice *slice;
 	struct cl_page	    *page;
diff --git a/drivers/staging/lustre/lustre/obdclass/cl_lock.c b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
index 7d99319b714e..b204531ef710 100644
--- a/drivers/staging/lustre/lustre/obdclass/cl_lock.c
+++ b/drivers/staging/lustre/lustre/obdclass/cl_lock.c
@@ -1938,7 +1938,7 @@ int cl_lock_discard_pages(const struct lu_env *env, struct cl_lock *lock)
 	io->ci_ignore_layout = 1;
 	result = cl_io_init(env, io, CIT_MISC, io->ci_obj);
 	if (result != 0)
-		GOTO(out, result);
+		goto out;
 
 	cb = descr->cld_mode == CLM_READ ? check_and_discard_cb : discard_cb;
 	info->clt_fn_index = info->clt_next_index = descr->cld_start;
diff --git a/drivers/staging/lustre/lustre/obdclass/class_obd.c b/drivers/staging/lustre/lustre/obdclass/class_obd.c
index 8b19f3caa68f..7265ecbc6f9d 100644
--- a/drivers/staging/lustre/lustre/obdclass/class_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/class_obd.c
@@ -35,7 +35,7 @@
  */
 
 #define DEBUG_SUBSYSTEM S_CLASS
-# include <asm/atomic.h>
+# include <linux/atomic.h>
 
 #include "../include/obd_support.h"
 #include "../include/obd_class.h"
@@ -177,18 +177,21 @@ int class_resolve_dev_name(__u32 len, const char *name)
 
 	if (!len || !name) {
 		CERROR("No name passed,!\n");
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 	if (name[len - 1] != 0) {
 		CERROR("Name not nul terminated!\n");
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	CDEBUG(D_IOCTL, "device name %s\n", name);
 	dev = class_name2dev(name);
 	if (dev == -1) {
 		CDEBUG(D_IOCTL, "No device for name %s!\n", name);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	CDEBUG(D_IOCTL, "device name %s, dev %d\n", name, dev);
@@ -208,7 +211,7 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 
 	/* only for debugging */
 	if (cmd == LIBCFS_IOC_DEBUG_MASK) {
-		debug_data = (struct libcfs_debug_ioctl_data*)arg;
+		debug_data = (struct libcfs_debug_ioctl_data *)arg;
 		libcfs_subsystem_debug = debug_data->subs;
 		libcfs_debug = debug_data->debug;
 		return 0;
@@ -227,11 +230,14 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 
 		if (!data->ioc_plen1 || !data->ioc_pbuf1) {
 			CERROR("No config buffer passed!\n");
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 		OBD_ALLOC(lcfg, data->ioc_plen1);
-		if (lcfg == NULL)
-			GOTO(out, err = -ENOMEM);
+		if (lcfg == NULL) {
+			err = -ENOMEM;
+			goto out;
+		}
 		err = copy_from_user(lcfg, data->ioc_pbuf1,
 					 data->ioc_plen1);
 		if (!err)
@@ -240,18 +246,20 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 			err = class_process_config(lcfg);
 
 		OBD_FREE(lcfg, data->ioc_plen1);
-		GOTO(out, err);
+		goto out;
 	}
 
 	case OBD_GET_VERSION:
 		if (!data->ioc_inlbuf1) {
 			CERROR("No buffer passed in ioctl\n");
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 
 		if (strlen(BUILD_VERSION) + 1 > data->ioc_inllen1) {
 			CERROR("ioctl buffer too small to hold version\n");
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 
 		memcpy(data->ioc_bulk, BUILD_VERSION,
@@ -260,7 +268,7 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 		err = obd_ioctl_popdata((void *)arg, data, len);
 		if (err)
 			err = -EFAULT;
-		GOTO(out, err);
+		goto out;
 
 	case OBD_IOC_NAME2DEV: {
 		/* Resolve a device name.  This does not change the
@@ -271,13 +279,15 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 		dev = class_resolve_dev_name(data->ioc_inllen1,
 					     data->ioc_inlbuf1);
 		data->ioc_dev = dev;
-		if (dev < 0)
-			GOTO(out, err = -EINVAL);
+		if (dev < 0) {
+			err = -EINVAL;
+			goto out;
+		}
 
 		err = obd_ioctl_popdata((void *)arg, data, sizeof(*data));
 		if (err)
 			err = -EFAULT;
-		GOTO(out, err);
+		goto out;
 	}
 
 	case OBD_IOC_UUID2DEV: {
@@ -289,11 +299,13 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 
 		if (!data->ioc_inllen1 || !data->ioc_inlbuf1) {
 			CERROR("No UUID passed!\n");
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 		if (data->ioc_inlbuf1[data->ioc_inllen1 - 1] != 0) {
 			CERROR("UUID not NUL terminated!\n");
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 
 		CDEBUG(D_IOCTL, "device name %s\n", data->ioc_inlbuf1);
@@ -303,7 +315,8 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 		if (dev == -1) {
 			CDEBUG(D_IOCTL, "No device for UUID %s!\n",
 			       data->ioc_inlbuf1);
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 
 		CDEBUG(D_IOCTL, "device name %s, dev %d\n", data->ioc_inlbuf1,
@@ -311,13 +324,14 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 		err = obd_ioctl_popdata((void *)arg, data, sizeof(*data));
 		if (err)
 			err = -EFAULT;
-		GOTO(out, err);
+		goto out;
 	}
 
 	case OBD_IOC_CLOSE_UUID: {
 		CDEBUG(D_IOCTL, "closing all connections to uuid %s (NOOP)\n",
 		       data->ioc_inlbuf1);
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 
 	case OBD_IOC_GETDEVICE: {
@@ -326,16 +340,20 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 
 		if (!data->ioc_inlbuf1) {
 			CERROR("No buffer passed in ioctl\n");
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 		if (data->ioc_inllen1 < 128) {
 			CERROR("ioctl buffer too small to hold version\n");
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 
 		obd = class_num2obd(index);
-		if (!obd)
-			GOTO(out, err = -ENOENT);
+		if (!obd) {
+			err = -ENOENT;
+			goto out;
+		}
 
 		if (obd->obd_stopping)
 			status = "ST";
@@ -352,56 +370,66 @@ int class_handle_ioctl(unsigned int cmd, unsigned long arg)
 			 atomic_read(&obd->obd_refcount));
 		err = obd_ioctl_popdata((void *)arg, data, len);
 
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 
 	}
 
 	if (data->ioc_dev == OBD_DEV_BY_DEVNAME) {
-		if (data->ioc_inllen4 <= 0 || data->ioc_inlbuf4 == NULL)
-			GOTO(out, err = -EINVAL);
-		if (strnlen(data->ioc_inlbuf4, MAX_OBD_NAME) >= MAX_OBD_NAME)
-			GOTO(out, err = -EINVAL);
+		if (data->ioc_inllen4 <= 0 || data->ioc_inlbuf4 == NULL) {
+			err = -EINVAL;
+			goto out;
+		}
+		if (strnlen(data->ioc_inlbuf4, MAX_OBD_NAME) >= MAX_OBD_NAME) {
+			err = -EINVAL;
+			goto out;
+		}
 		obd = class_name2obd(data->ioc_inlbuf4);
 	} else if (data->ioc_dev < class_devno_max()) {
 		obd = class_num2obd(data->ioc_dev);
 	} else {
 		CERROR("OBD ioctl: No device\n");
-		GOTO(out, err = -EINVAL);
+		err = -EINVAL;
+		goto out;
 	}
 
 	if (obd == NULL) {
 		CERROR("OBD ioctl : No Device %d\n", data->ioc_dev);
-		GOTO(out, err = -EINVAL);
+		err = -EINVAL;
+		goto out;
 	}
 	LASSERT(obd->obd_magic == OBD_DEVICE_MAGIC);
 
 	if (!obd->obd_set_up || obd->obd_stopping) {
-		CERROR("OBD ioctl: device not setup %d \n", data->ioc_dev);
-		GOTO(out, err = -EINVAL);
+		CERROR("OBD ioctl: device not setup %d\n", data->ioc_dev);
+		err = -EINVAL;
+		goto out;
 	}
 
-	switch(cmd) {
+	switch (cmd) {
 	case OBD_IOC_NO_TRANSNO: {
 		if (!obd->obd_attached) {
 			CERROR("Device %d not attached\n", obd->obd_minor);
-			GOTO(out, err = -ENODEV);
+			err = -ENODEV;
+			goto out;
 		}
 		CDEBUG(D_HA, "%s: disabling committed-transno notification\n",
 		       obd->obd_name);
 		obd->obd_no_transno = 1;
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 
 	default: {
 		err = obd_iocontrol(cmd, obd->obd_self_export, len, data, NULL);
 		if (err)
-			GOTO(out, err);
+			goto out;
 
 		err = obd_ioctl_popdata((void *)arg, data, len);
 		if (err)
 			err = -EFAULT;
-		GOTO(out, err);
+		goto out;
 	}
 	}
 
@@ -506,20 +534,23 @@ static int __init init_obdclass(void)
 	spin_lock_init(&obd_types_lock);
 	obd_zombie_impexp_init();
 
-	obd_memory = lprocfs_alloc_stats(OBD_STATS_NUM,
-					 LPROCFS_STATS_FLAG_NONE |
-					 LPROCFS_STATS_FLAG_IRQ_SAFE);
-	if (obd_memory == NULL) {
-		CERROR("kmalloc of 'obd_memory' failed\n");
-		return -ENOMEM;
-	}
+	if (IS_ENABLED(CONFIG_PROC_FS)) {
+		obd_memory = lprocfs_alloc_stats(OBD_STATS_NUM,
+						 LPROCFS_STATS_FLAG_NONE |
+						 LPROCFS_STATS_FLAG_IRQ_SAFE);
 
-	lprocfs_counter_init(obd_memory, OBD_MEMORY_STAT,
-			     LPROCFS_CNTR_AVGMINMAX,
-			     "memused", "bytes");
-	lprocfs_counter_init(obd_memory, OBD_MEMORY_PAGES_STAT,
-			     LPROCFS_CNTR_AVGMINMAX,
-			     "pagesused", "pages");
+		if (obd_memory == NULL) {
+			CERROR("kmalloc of 'obd_memory' failed\n");
+			return -ENOMEM;
+		}
+
+		lprocfs_counter_init(obd_memory, OBD_MEMORY_STAT,
+				     LPROCFS_CNTR_AVGMINMAX,
+				     "memused", "bytes");
+		lprocfs_counter_init(obd_memory, OBD_MEMORY_PAGES_STAT,
+				     LPROCFS_CNTR_AVGMINMAX,
+				     "pagesused", "pages");
+	}
 
 	err = obd_init_checks();
 	if (err == -EOVERFLOW)
diff --git a/drivers/staging/lustre/lustre/obdclass/debug.c b/drivers/staging/lustre/lustre/obdclass/debug.c
index e8aa42beb3c7..d0f8f875ddd6 100644
--- a/drivers/staging/lustre/lustre/obdclass/debug.c
+++ b/drivers/staging/lustre/lustre/obdclass/debug.c
@@ -41,7 +41,6 @@
 #define DEBUG_SUBSYSTEM D_OTHER
 
 
-#include "../include/obd_ost.h"
 #include "../include/obd_support.h"
 #include "../include/lustre_debug.h"
 #include "../include/lustre_net.h"
diff --git a/drivers/staging/lustre/lustre/obdclass/dt_object.c b/drivers/staging/lustre/lustre/obdclass/dt_object.c
index 130b8dd0b418..52256c26bf07 100644
--- a/drivers/staging/lustre/lustre/obdclass/dt_object.c
+++ b/drivers/staging/lustre/lustre/obdclass/dt_object.c
@@ -384,26 +384,30 @@ struct dt_object *dt_find_or_create(const struct lu_env *env,
 		return dto;
 
 	th = dt_trans_create(env, dt);
-	if (IS_ERR(th))
-		GOTO(out, rc = PTR_ERR(th));
+	if (IS_ERR(th)) {
+		rc = PTR_ERR(th);
+		goto out;
+	}
 
 	rc = dt_declare_create(env, dto, at, NULL, dof, th);
 	if (rc)
-		GOTO(trans_stop, rc);
+		goto trans_stop;
 
 	rc = dt_trans_start_local(env, dt, th);
 	if (rc)
-		GOTO(trans_stop, rc);
+		goto trans_stop;
 
 	dt_write_lock(env, dto, 0);
-	if (dt_object_exists(dto))
-		GOTO(unlock, rc = 0);
+	if (dt_object_exists(dto)) {
+		rc = 0;
+		goto unlock;
+	}
 
 	CDEBUG(D_OTHER, "create new object "DFID"\n", PFID(fid));
 
 	rc = dt_create(env, dto, at, NULL, dof, th);
 	if (rc)
-		GOTO(unlock, rc);
+		goto unlock;
 	LASSERT(dt_object_exists(dto));
 unlock:
 	dt_write_unlock(env, dto);
@@ -683,14 +687,18 @@ static int dt_index_page_build(const struct lu_env *env, union lu_page *lp,
 		ii->ii_hash_end = hash;
 
 		if (OBD_FAIL_CHECK(OBD_FAIL_OBD_IDX_READ_BREAK)) {
-			if (lip->lip_nr != 0)
-				GOTO(out, rc = 0);
+			if (lip->lip_nr != 0) {
+				rc = 0;
+				goto out;
+			}
 		}
 
 		if (nob < size) {
 			if (lip->lip_nr == 0)
-				GOTO(out, rc = -EINVAL);
-			GOTO(out, rc = 0);
+				rc = -EINVAL;
+			else
+				rc = 0;
+			goto out;
 		}
 
 		if ((ii->ii_flags & II_FL_NOHASH) == 0) {
@@ -710,7 +718,7 @@ static int dt_index_page_build(const struct lu_env *env, union lu_page *lp,
 		rc = iops->rec(env, it, (struct dt_rec *)tmp_entry, attr);
 		if (rc != -ESTALE) {
 			if (rc != 0)
-				GOTO(out, rc);
+				goto out;
 
 			/* hash/key/record successfully copied! */
 			lip->lip_nr++;
@@ -727,7 +735,7 @@ static int dt_index_page_build(const struct lu_env *env, union lu_page *lp,
 
 	} while (rc == 0);
 
-	GOTO(out, rc);
+	goto out;
 out:
 	if (rc >= 0 && lip->lip_nr > 0)
 		/* one more container */
@@ -869,20 +877,24 @@ int dt_index_read(const struct lu_env *env, struct dt_device *dev,
 	obj = dt_locate(env, dev, &ii->ii_fid);
 	if (IS_ERR(obj))
 		return PTR_ERR(obj);
-	if (dt_object_exists(obj) == 0)
-		GOTO(out, rc = -ENOENT);
+	if (dt_object_exists(obj) == 0) {
+		rc = -ENOENT;
+		goto out;
+	}
 
 	/* fetch index features associated with index object */
 	feat = dt_index_feat_select(fid_seq(&ii->ii_fid),
 				    lu_object_attr(&obj->do_lu));
-	if (IS_ERR(feat))
-		GOTO(out, rc = PTR_ERR(feat));
+	if (IS_ERR(feat)) {
+		rc = PTR_ERR(feat);
+		goto out;
+	}
 
 	/* load index feature if not done already */
 	if (obj->do_index_ops == NULL) {
 		rc = obj->do_ops->do_index_try(env, obj, feat);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 	}
 
 	/* fill ii_flags with supported index features */
@@ -893,7 +905,8 @@ int dt_index_read(const struct lu_env *env, struct dt_device *dev,
 		/* key size is variable */
 		ii->ii_flags |= II_FL_VARKEY;
 		/* we don't support variable key size for the time being */
-		GOTO(out, rc = -EOPNOTSUPP);
+		rc = -EOPNOTSUPP;
+		goto out;
 	}
 
 	ii->ii_recsize = feat->dif_recsize_max;
@@ -901,7 +914,8 @@ int dt_index_read(const struct lu_env *env, struct dt_device *dev,
 		/* record size is variable */
 		ii->ii_flags |= II_FL_VARREC;
 		/* we don't support variable record size for the time being */
-		GOTO(out, rc = -EOPNOTSUPP);
+		rc = -EOPNOTSUPP;
+		goto out;
 	}
 
 	if ((feat->dif_flags & DT_IND_NONUNQ) != 0)
@@ -922,7 +936,7 @@ int dt_index_read(const struct lu_env *env, struct dt_device *dev,
 		ii->ii_hash_end = II_END_OFF;
 	}
 
-	GOTO(out, rc);
+	goto out;
 out:
 	lu_object_put(env, &obj->do_lu);
 	return rc;
diff --git a/drivers/staging/lustre/lustre/obdclass/genops.c b/drivers/staging/lustre/lustre/obdclass/genops.c
index 504c59aabaef..c314e9c2343e 100644
--- a/drivers/staging/lustre/lustre/obdclass/genops.c
+++ b/drivers/staging/lustre/lustre/obdclass/genops.c
@@ -40,7 +40,6 @@
  */
 
 #define DEBUG_SUBSYSTEM S_CLASS
-#include "../include/obd_ost.h"
 #include "../include/obd_class.h"
 #include "../include/lprocfs_status.h"
 
@@ -184,7 +183,7 @@ int class_register_type(struct obd_ops *dt_ops, struct md_ops *md_ops,
 	if (type->typ_dt_ops == NULL ||
 	    type->typ_md_ops == NULL ||
 	    type->typ_name == NULL)
-		GOTO (failed, rc);
+		goto failed;
 
 	*(type->typ_dt_ops) = *dt_ops;
 	/* md_ops is optional */
@@ -198,14 +197,14 @@ int class_register_type(struct obd_ops *dt_ops, struct md_ops *md_ops,
 	if (IS_ERR(type->typ_procroot)) {
 		rc = PTR_ERR(type->typ_procroot);
 		type->typ_procroot = NULL;
-		GOTO (failed, rc);
+		goto failed;
 	}
 
 	if (ldt != NULL) {
 		type->typ_lu = ldt;
 		rc = lu_device_type_init(ldt);
 		if (rc != 0)
-			GOTO (failed, rc);
+			goto failed;
 	}
 
 	spin_lock(&obd_types_lock);
@@ -295,8 +294,10 @@ struct obd_device *class_newdev(const char *type_name, const char *name)
 	}
 
 	newdev = obd_device_alloc();
-	if (newdev == NULL)
-		GOTO(out_type, result = ERR_PTR(-ENOMEM));
+	if (newdev == NULL) {
+		result = ERR_PTR(-ENOMEM);
+		goto out_type;
+	}
 
 	LASSERT(newdev->obd_magic == OBD_DEVICE_MAGIC);
 
@@ -336,11 +337,12 @@ struct obd_device *class_newdev(const char *type_name, const char *name)
 	if (result == NULL && i >= class_devno_max()) {
 		CERROR("all %u OBD devices used, increase MAX_OBD_DEVICES\n",
 		       class_devno_max());
-		GOTO(out, result = ERR_PTR(-EOVERFLOW));
+		result = ERR_PTR(-EOVERFLOW);
+		goto out;
 	}
 
 	if (IS_ERR(result))
-		GOTO(out, result);
+		goto out;
 
 	CDEBUG(D_IOCTL, "Adding new device %s (%p)\n",
 	       result->obd_name, result);
@@ -656,26 +658,26 @@ int obd_init_caches(void)
 						 sizeof(struct obd_device),
 						 0, 0, NULL);
 	if (!obd_device_cachep)
-		GOTO(out, -ENOMEM);
+		goto out;
 
 	LASSERT(obdo_cachep == NULL);
 	obdo_cachep = kmem_cache_create("ll_obdo_cache", sizeof(struct obdo),
 					   0, 0, NULL);
 	if (!obdo_cachep)
-		GOTO(out, -ENOMEM);
+		goto out;
 
 	LASSERT(import_cachep == NULL);
 	import_cachep = kmem_cache_create("ll_import_cache",
 					     sizeof(struct obd_import),
 					     0, 0, NULL);
 	if (!import_cachep)
-		GOTO(out, -ENOMEM);
+		goto out;
 
 	LASSERT(capa_cachep == NULL);
 	capa_cachep = kmem_cache_create("capa_cache",
 					   sizeof(struct obd_capa), 0, 0, NULL);
 	if (!capa_cachep)
-		GOTO(out, -ENOMEM);
+		goto out;
 
 	return 0;
  out:
@@ -857,12 +859,16 @@ struct obd_export *class_new_export(struct obd_device *obd,
 
 	spin_lock(&obd->obd_dev_lock);
 	/* shouldn't happen, but might race */
-	if (obd->obd_stopping)
-		GOTO(exit_unlock, rc = -ENODEV);
+	if (obd->obd_stopping) {
+		rc = -ENODEV;
+		goto exit_unlock;
+	}
 
 	hash = cfs_hash_getref(obd->obd_uuid_hash);
-	if (hash == NULL)
-		GOTO(exit_unlock, rc = -ENODEV);
+	if (hash == NULL) {
+		rc = -ENODEV;
+		goto exit_unlock;
+	}
 	spin_unlock(&obd->obd_dev_lock);
 
 	if (!obd_uuid_equals(cluuid, &obd->obd_uuid)) {
@@ -870,14 +876,16 @@ struct obd_export *class_new_export(struct obd_device *obd,
 		if (rc != 0) {
 			LCONSOLE_WARN("%s: denying duplicate export for %s, %d\n",
 				      obd->obd_name, cluuid->uuid, rc);
-			GOTO(exit_err, rc = -EALREADY);
+			rc = -EALREADY;
+			goto exit_err;
 		}
 	}
 
 	spin_lock(&obd->obd_dev_lock);
 	if (obd->obd_stopping) {
 		cfs_hash_del(hash, cluuid, &export->exp_uuid_hash);
-		GOTO(exit_unlock, rc = -ENODEV);
+		rc = -ENODEV;
+		goto exit_unlock;
 	}
 
 	class_incref(obd, "export", export);
@@ -1187,7 +1195,7 @@ int class_disconnect(struct obd_export *export)
 	 * call extra class_export_puts(). */
 	if (already_disconnected) {
 		LASSERT(hlist_unhashed(&export->exp_nid_hash));
-		GOTO(no_disconn, already_disconnected);
+		goto no_disconn;
 	}
 
 	CDEBUG(D_IOCTL, "disconnect: cookie %#llx\n",
@@ -1425,7 +1433,8 @@ int obd_export_evict_by_nid(struct obd_device *obd, const char *nid)
 	cfs_hash_putref(nid_hash);
 
 	if (!exports_evicted)
-		CDEBUG(D_HA,"%s: can't disconnect NID '%s': no exports found\n",
+		CDEBUG(D_HA,
+		       "%s: can't disconnect NID '%s': no exports found\n",
 		       obd->obd_name, nid);
 	return exports_evicted;
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/idmap.c b/drivers/staging/lustre/lustre/obdclass/idmap.c
deleted file mode 100644
index 1190885c06b6..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/idmap.c
+++ /dev/null
@@ -1,477 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2008, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/obdclass/idmap.c
- *
- * Lustre user identity mapping.
- *
- * Author: Fan Yong <fanyong@clusterfs.com>
- */
-
-#define DEBUG_SUBSYSTEM S_SEC
-
-#include "../include/lustre_idmap.h"
-#include "../include/md_object.h"
-#include "../include/obd_support.h"
-
-#define lustre_get_group_info(group_info) do {	     \
-	atomic_inc(&(group_info)->usage);	      \
-} while (0)
-
-#define lustre_put_group_info(group_info) do {	     \
-	if (atomic_dec_and_test(&(group_info)->usage)) \
-		groups_free(group_info);	       \
-} while (0)
-
-/*
- * groups_search() is copied from linux kernel!
- * A simple bsearch.
- */
-static int lustre_groups_search(const struct group_info *group_info, gid_t grp)
-{
-	int left, right;
-
-	if (!group_info)
-		return 0;
-
-	left = 0;
-	right = group_info->ngroups;
-	while (left < right) {
-		int mid = (left + right) / 2;
-		int cmp = grp -
-			from_kgid(&init_user_ns, CFS_GROUP_AT(group_info, mid));
-
-		if (cmp > 0)
-			left = mid + 1;
-		else if (cmp < 0)
-			right = mid;
-		else
-			return 1;
-	}
-	return 0;
-}
-
-void lustre_groups_from_list(struct group_info *ginfo, gid_t *glist)
-{
-	int i;
-	int count = ginfo->ngroups;
-
-	/* fill group_info from gid array */
-	for (i = 0; i < ginfo->nblocks && count > 0; i++) {
-		int cp_count = min(CFS_NGROUPS_PER_BLOCK, count);
-		int off = i * CFS_NGROUPS_PER_BLOCK;
-		int len = cp_count * sizeof(*glist);
-
-		memcpy(ginfo->blocks[i], glist + off, len);
-		count -= cp_count;
-	}
-}
-EXPORT_SYMBOL(lustre_groups_from_list);
-
-/* groups_sort() is copied from linux kernel! */
-/* a simple shell-metzner sort */
-void lustre_groups_sort(struct group_info *group_info)
-{
-	int base, max, stride;
-	int gidsetsize = group_info->ngroups;
-
-	for (stride = 1; stride < gidsetsize; stride = 3 * stride + 1)
-		; /* nothing */
-	stride /= 3;
-
-	while (stride) {
-		max = gidsetsize - stride;
-		for (base = 0; base < max; base++) {
-			int left = base;
-			int right = left + stride;
-			gid_t tmp = from_kgid(&init_user_ns,
-					      CFS_GROUP_AT(group_info, right));
-
-			while (left >= 0 &&
-			       tmp < from_kgid(&init_user_ns,
-					       CFS_GROUP_AT(group_info, left))) {
-				CFS_GROUP_AT(group_info, right) =
-				    CFS_GROUP_AT(group_info, left);
-				right = left;
-				left -= stride;
-			}
-			CFS_GROUP_AT(group_info, right) =
-						make_kgid(&init_user_ns, tmp);
-		}
-		stride /= 3;
-	}
-}
-EXPORT_SYMBOL(lustre_groups_sort);
-
-int lustre_in_group_p(struct lu_ucred *mu, gid_t grp)
-{
-	int rc = 1;
-
-	if (grp != mu->uc_fsgid) {
-		struct group_info *group_info = NULL;
-
-		if (mu->uc_ginfo || !mu->uc_identity ||
-		    mu->uc_valid == UCRED_OLD)
-			if (grp == mu->uc_suppgids[0] ||
-			    grp == mu->uc_suppgids[1])
-				return 1;
-
-		if (mu->uc_ginfo)
-			group_info = mu->uc_ginfo;
-		else if (mu->uc_identity)
-			group_info = mu->uc_identity->mi_ginfo;
-
-		if (!group_info)
-			return 0;
-
-		lustre_get_group_info(group_info);
-		rc = lustre_groups_search(group_info, grp);
-		lustre_put_group_info(group_info);
-	}
-	return rc;
-}
-EXPORT_SYMBOL(lustre_in_group_p);
-
-struct lustre_idmap_entry {
-	struct list_head       lie_rmt_uid_hash; /* hashed as lie_rmt_uid; */
-	struct list_head       lie_lcl_uid_hash; /* hashed as lie_lcl_uid; */
-	struct list_head       lie_rmt_gid_hash; /* hashed as lie_rmt_gid; */
-	struct list_head       lie_lcl_gid_hash; /* hashed as lie_lcl_gid; */
-	uid_t	    lie_rmt_uid;      /* remote uid */
-	uid_t	    lie_lcl_uid;      /* local uid */
-	gid_t	    lie_rmt_gid;      /* remote gid */
-	gid_t	    lie_lcl_gid;      /* local gid */
-};
-
-static inline __u32 lustre_idmap_hashfunc(__u32 id)
-{
-	return id & (CFS_IDMAP_HASHSIZE - 1);
-}
-
-static
-struct lustre_idmap_entry *idmap_entry_alloc(uid_t rmt_uid, uid_t lcl_uid,
-					     gid_t rmt_gid, gid_t lcl_gid)
-{
-	struct lustre_idmap_entry *e;
-
-	OBD_ALLOC_PTR(e);
-	if (e == NULL)
-		return NULL;
-
-	INIT_LIST_HEAD(&e->lie_rmt_uid_hash);
-	INIT_LIST_HEAD(&e->lie_lcl_uid_hash);
-	INIT_LIST_HEAD(&e->lie_rmt_gid_hash);
-	INIT_LIST_HEAD(&e->lie_lcl_gid_hash);
-	e->lie_rmt_uid = rmt_uid;
-	e->lie_lcl_uid = lcl_uid;
-	e->lie_rmt_gid = rmt_gid;
-	e->lie_lcl_gid = lcl_gid;
-
-	return e;
-}
-
-static void idmap_entry_free(struct lustre_idmap_entry *e)
-{
-	if (!list_empty(&e->lie_rmt_uid_hash))
-		list_del(&e->lie_rmt_uid_hash);
-	if (!list_empty(&e->lie_lcl_uid_hash))
-		list_del(&e->lie_lcl_uid_hash);
-	if (!list_empty(&e->lie_rmt_gid_hash))
-		list_del(&e->lie_rmt_gid_hash);
-	if (!list_empty(&e->lie_lcl_gid_hash))
-		list_del(&e->lie_lcl_gid_hash);
-	OBD_FREE_PTR(e);
-}
-
-/*
- * return value
- * NULL: not found entry
- * ERR_PTR(-EACCES): found 1(remote):N(local) mapped entry
- * others: found normal entry
- */
-static
-struct lustre_idmap_entry *idmap_search_entry(struct lustre_idmap_table *t,
-					      uid_t rmt_uid, uid_t lcl_uid,
-					      gid_t rmt_gid, gid_t lcl_gid)
-{
-	struct list_head *head;
-	struct lustre_idmap_entry *e;
-
-	head = &t->lit_idmaps[RMT_UIDMAP_IDX][lustre_idmap_hashfunc(rmt_uid)];
-	list_for_each_entry(e, head, lie_rmt_uid_hash)
-		if (e->lie_rmt_uid == rmt_uid) {
-			if (e->lie_lcl_uid == lcl_uid) {
-				if (e->lie_rmt_gid == rmt_gid &&
-				    e->lie_lcl_gid == lcl_gid)
-					/* must be quaternion match */
-					return e;
-			} else {
-				/* 1:N uid mapping */
-				CERROR("rmt uid %u already be mapped to %u"
-				       " (new %u)\n", e->lie_rmt_uid,
-				       e->lie_lcl_uid, lcl_uid);
-				return ERR_PTR(-EACCES);
-			}
-		}
-
-	head = &t->lit_idmaps[RMT_GIDMAP_IDX][lustre_idmap_hashfunc(rmt_gid)];
-	list_for_each_entry(e, head, lie_rmt_gid_hash)
-		if (e->lie_rmt_gid == rmt_gid) {
-			if (e->lie_lcl_gid == lcl_gid) {
-				if (unlikely(e->lie_rmt_uid == rmt_uid &&
-				    e->lie_lcl_uid == lcl_uid))
-					/* after uid mapping search above,
-					 * we should never come here */
-					LBUG();
-			} else {
-				/* 1:N gid mapping */
-				CERROR("rmt gid %u already be mapped to %u"
-				       " (new %u)\n", e->lie_rmt_gid,
-				       e->lie_lcl_gid, lcl_gid);
-				return ERR_PTR(-EACCES);
-			}
-		}
-
-	return NULL;
-}
-
-static __u32 idmap_lookup_uid(struct list_head *hash, int reverse,
-			      __u32 uid)
-{
-	struct list_head *head = &hash[lustre_idmap_hashfunc(uid)];
-	struct lustre_idmap_entry *e;
-
-	if (!reverse) {
-		list_for_each_entry(e, head, lie_rmt_uid_hash)
-			if (e->lie_rmt_uid == uid)
-				return e->lie_lcl_uid;
-	} else {
-		list_for_each_entry(e, head, lie_lcl_uid_hash)
-			if (e->lie_lcl_uid == uid)
-				return e->lie_rmt_uid;
-	}
-
-	return CFS_IDMAP_NOTFOUND;
-}
-
-static __u32 idmap_lookup_gid(struct list_head *hash, int reverse, __u32 gid)
-{
-	struct list_head *head = &hash[lustre_idmap_hashfunc(gid)];
-	struct lustre_idmap_entry *e;
-
-	if (!reverse) {
-		list_for_each_entry(e, head, lie_rmt_gid_hash)
-			if (e->lie_rmt_gid == gid)
-				return e->lie_lcl_gid;
-	} else {
-		list_for_each_entry(e, head, lie_lcl_gid_hash)
-			if (e->lie_lcl_gid == gid)
-				return e->lie_rmt_gid;
-	}
-
-	return CFS_IDMAP_NOTFOUND;
-}
-
-int lustre_idmap_add(struct lustre_idmap_table *t,
-		     uid_t ruid, uid_t luid,
-		     gid_t rgid, gid_t lgid)
-{
-	struct lustre_idmap_entry *e0, *e1;
-
-	LASSERT(t);
-
-	spin_lock(&t->lit_lock);
-	e0 = idmap_search_entry(t, ruid, luid, rgid, lgid);
-	spin_unlock(&t->lit_lock);
-	if (!e0) {
-		e0 = idmap_entry_alloc(ruid, luid, rgid, lgid);
-		if (!e0)
-			return -ENOMEM;
-
-		spin_lock(&t->lit_lock);
-		e1 = idmap_search_entry(t, ruid, luid, rgid, lgid);
-		if (e1 == NULL) {
-			list_add_tail(&e0->lie_rmt_uid_hash,
-					  &t->lit_idmaps[RMT_UIDMAP_IDX]
-					  [lustre_idmap_hashfunc(ruid)]);
-			list_add_tail(&e0->lie_lcl_uid_hash,
-					  &t->lit_idmaps[LCL_UIDMAP_IDX]
-					  [lustre_idmap_hashfunc(luid)]);
-			list_add_tail(&e0->lie_rmt_gid_hash,
-					  &t->lit_idmaps[RMT_GIDMAP_IDX]
-					  [lustre_idmap_hashfunc(rgid)]);
-			list_add_tail(&e0->lie_lcl_gid_hash,
-					  &t->lit_idmaps[LCL_GIDMAP_IDX]
-					  [lustre_idmap_hashfunc(lgid)]);
-		}
-		spin_unlock(&t->lit_lock);
-		if (e1 != NULL) {
-			idmap_entry_free(e0);
-			if (IS_ERR(e1))
-				return PTR_ERR(e1);
-		}
-	} else if (IS_ERR(e0)) {
-		return PTR_ERR(e0);
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL(lustre_idmap_add);
-
-int lustre_idmap_del(struct lustre_idmap_table *t,
-		    uid_t ruid, uid_t luid,
-		    gid_t rgid, gid_t lgid)
-{
-	struct lustre_idmap_entry *e;
-	int rc = 0;
-
-	LASSERT(t);
-
-	spin_lock(&t->lit_lock);
-	e = idmap_search_entry(t, ruid, luid, rgid, lgid);
-	if (IS_ERR(e))
-		rc = PTR_ERR(e);
-	else if (e)
-		idmap_entry_free(e);
-	spin_unlock(&t->lit_lock);
-
-	return rc;
-}
-EXPORT_SYMBOL(lustre_idmap_del);
-
-int lustre_idmap_lookup_uid(struct lu_ucred *mu,
-			    struct lustre_idmap_table *t,
-			    int reverse, uid_t uid)
-{
-	struct list_head *hash;
-
-	if (mu && (mu->uc_valid == UCRED_OLD || mu->uc_valid == UCRED_NEW)) {
-		if (!reverse) {
-			if (uid == mu->uc_o_uid)
-				return mu->uc_uid;
-			else if (uid == mu->uc_o_fsuid)
-				return mu->uc_fsuid;
-		} else {
-			if (uid == mu->uc_uid)
-				return mu->uc_o_uid;
-			else if (uid == mu->uc_fsuid)
-				return mu->uc_o_fsuid;
-		}
-	}
-
-	if (t == NULL)
-		return CFS_IDMAP_NOTFOUND;
-
-	hash = t->lit_idmaps[reverse ? LCL_UIDMAP_IDX : RMT_UIDMAP_IDX];
-
-	spin_lock(&t->lit_lock);
-	uid = idmap_lookup_uid(hash, reverse, uid);
-	spin_unlock(&t->lit_lock);
-
-	return uid;
-}
-EXPORT_SYMBOL(lustre_idmap_lookup_uid);
-
-int lustre_idmap_lookup_gid(struct lu_ucred *mu, struct lustre_idmap_table *t,
-			    int reverse, gid_t gid)
-{
-	struct list_head *hash;
-
-	if (mu && (mu->uc_valid == UCRED_OLD || mu->uc_valid == UCRED_NEW)) {
-		if (!reverse) {
-			if (gid == mu->uc_o_gid)
-				return mu->uc_gid;
-			else if (gid == mu->uc_o_fsgid)
-				return mu->uc_fsgid;
-		} else {
-			if (gid == mu->uc_gid)
-				return mu->uc_o_gid;
-			else if (gid == mu->uc_fsgid)
-				return mu->uc_o_fsgid;
-		}
-	}
-
-	if (t == NULL)
-		return CFS_IDMAP_NOTFOUND;
-
-	hash = t->lit_idmaps[reverse ? LCL_GIDMAP_IDX : RMT_GIDMAP_IDX];
-
-	spin_lock(&t->lit_lock);
-	gid = idmap_lookup_gid(hash, reverse, gid);
-	spin_unlock(&t->lit_lock);
-
-	return gid;
-}
-EXPORT_SYMBOL(lustre_idmap_lookup_gid);
-
-struct lustre_idmap_table *lustre_idmap_init(void)
-{
-	struct lustre_idmap_table *t;
-	int i, j;
-
-	OBD_ALLOC_PTR(t);
-	if(unlikely(t == NULL))
-		return (ERR_PTR(-ENOMEM));
-
-	spin_lock_init(&t->lit_lock);
-	for (i = 0; i < ARRAY_SIZE(t->lit_idmaps); i++)
-		for (j = 0; j < ARRAY_SIZE(t->lit_idmaps[i]); j++)
-			INIT_LIST_HEAD(&t->lit_idmaps[i][j]);
-
-	return t;
-}
-EXPORT_SYMBOL(lustre_idmap_init);
-
-void lustre_idmap_fini(struct lustre_idmap_table *t)
-{
-	struct list_head *list;
-	struct lustre_idmap_entry *e;
-	int i;
-	LASSERT(t);
-
-	list = t->lit_idmaps[RMT_UIDMAP_IDX];
-	spin_lock(&t->lit_lock);
-	for (i = 0; i < CFS_IDMAP_HASHSIZE; i++)
-		while (!list_empty(&list[i])) {
-			e = list_entry(list[i].next,
-					   struct lustre_idmap_entry,
-					   lie_rmt_uid_hash);
-			idmap_entry_free(e);
-		}
-	spin_unlock(&t->lit_lock);
-
-	OBD_FREE_PTR(t);
-}
-EXPORT_SYMBOL(lustre_idmap_fini);
diff --git a/drivers/staging/lustre/lustre/obdclass/linkea.c b/drivers/staging/lustre/lustre/obdclass/linkea.c
deleted file mode 100644
index 8a1c7b6fefe9..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/linkea.c
+++ /dev/null
@@ -1,194 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; if not, write to the
- * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 021110-1307, USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2013, Intel Corporation.
- * Use is subject to license terms.
- *
- * Author: Di Wang <di.wang@intel.com>
- */
-
-#include "../include/lustre/lustre_idl.h"
-#include "../include/obd.h"
-#include "../include/lustre_linkea.h"
-
-int linkea_data_new(struct linkea_data *ldata, struct lu_buf *buf)
-{
-	ldata->ld_buf = lu_buf_check_and_alloc(buf, PAGE_CACHE_SIZE);
-	if (ldata->ld_buf->lb_buf == NULL)
-		return -ENOMEM;
-	ldata->ld_leh = ldata->ld_buf->lb_buf;
-	ldata->ld_leh->leh_magic = LINK_EA_MAGIC;
-	ldata->ld_leh->leh_len = sizeof(struct link_ea_header);
-	ldata->ld_leh->leh_reccount = 0;
-	return 0;
-}
-EXPORT_SYMBOL(linkea_data_new);
-
-int linkea_init(struct linkea_data *ldata)
-{
-	struct link_ea_header *leh;
-
-	LASSERT(ldata->ld_buf != NULL);
-	leh = ldata->ld_buf->lb_buf;
-	if (leh->leh_magic == __swab32(LINK_EA_MAGIC)) {
-		leh->leh_magic = LINK_EA_MAGIC;
-		leh->leh_reccount = __swab32(leh->leh_reccount);
-		leh->leh_len = __swab64(leh->leh_len);
-		/* entries are swabbed by linkea_entry_unpack */
-	}
-	if (leh->leh_magic != LINK_EA_MAGIC)
-		return -EINVAL;
-	if (leh->leh_reccount == 0)
-		return -ENODATA;
-
-	ldata->ld_leh = leh;
-	return 0;
-}
-EXPORT_SYMBOL(linkea_init);
-
-/**
- * Pack a link_ea_entry.
- * All elements are stored as chars to avoid alignment issues.
- * Numbers are always big-endian
- * \retval record length
- */
-static int linkea_entry_pack(struct link_ea_entry *lee,
-			     const struct lu_name *lname,
-			     const struct lu_fid *pfid)
-{
-	struct lu_fid   tmpfid;
-	int	     reclen;
-
-	fid_cpu_to_be(&tmpfid, pfid);
-	if (OBD_FAIL_CHECK(OBD_FAIL_LFSCK_LINKEA_CRASH))
-		tmpfid.f_ver = ~0;
-	memcpy(&lee->lee_parent_fid, &tmpfid, sizeof(tmpfid));
-	memcpy(lee->lee_name, lname->ln_name, lname->ln_namelen);
-	reclen = sizeof(struct link_ea_entry) + lname->ln_namelen;
-
-	lee->lee_reclen[0] = (reclen >> 8) & 0xff;
-	lee->lee_reclen[1] = reclen & 0xff;
-	return reclen;
-}
-
-void linkea_entry_unpack(const struct link_ea_entry *lee, int *reclen,
-			 struct lu_name *lname, struct lu_fid *pfid)
-{
-	*reclen = (lee->lee_reclen[0] << 8) | lee->lee_reclen[1];
-	memcpy(pfid, &lee->lee_parent_fid, sizeof(*pfid));
-	fid_be_to_cpu(pfid, pfid);
-	lname->ln_name = lee->lee_name;
-	lname->ln_namelen = *reclen - sizeof(struct link_ea_entry);
-}
-EXPORT_SYMBOL(linkea_entry_unpack);
-
-/**
- * Add a record to the end of link ea buf
- **/
-int linkea_add_buf(struct linkea_data *ldata, const struct lu_name *lname,
-		   const struct lu_fid *pfid)
-{
-	LASSERT(ldata->ld_leh != NULL);
-
-	if (lname == NULL || pfid == NULL)
-		return -EINVAL;
-
-	ldata->ld_reclen = lname->ln_namelen + sizeof(struct link_ea_entry);
-	if (ldata->ld_leh->leh_len + ldata->ld_reclen >
-	    ldata->ld_buf->lb_len) {
-		if (lu_buf_check_and_grow(ldata->ld_buf,
-					  ldata->ld_leh->leh_len +
-					  ldata->ld_reclen) < 0)
-			return -ENOMEM;
-	}
-
-	ldata->ld_leh = ldata->ld_buf->lb_buf;
-	ldata->ld_lee = ldata->ld_buf->lb_buf + ldata->ld_leh->leh_len;
-	ldata->ld_reclen = linkea_entry_pack(ldata->ld_lee, lname, pfid);
-	ldata->ld_leh->leh_len += ldata->ld_reclen;
-	ldata->ld_leh->leh_reccount++;
-	CDEBUG(D_INODE, "New link_ea name '%.*s' is added\n",
-	       lname->ln_namelen, lname->ln_name);
-	return 0;
-}
-EXPORT_SYMBOL(linkea_add_buf);
-
-/** Del the current record from the link ea buf */
-void linkea_del_buf(struct linkea_data *ldata, const struct lu_name *lname)
-{
-	LASSERT(ldata->ld_leh != NULL && ldata->ld_lee != NULL);
-
-	ldata->ld_leh->leh_reccount--;
-	ldata->ld_leh->leh_len -= ldata->ld_reclen;
-	memmove(ldata->ld_lee, (char *)ldata->ld_lee + ldata->ld_reclen,
-		(char *)ldata->ld_leh + ldata->ld_leh->leh_len -
-		(char *)ldata->ld_lee);
-	CDEBUG(D_INODE, "Old link_ea name '%.*s' is removed\n",
-	       lname->ln_namelen, lname->ln_name);
-}
-EXPORT_SYMBOL(linkea_del_buf);
-
-/**
- * Check if such a link exists in linkEA.
- *
- * \param ldata link data the search to be done on
- * \param lname name in the parent's directory entry pointing to this object
- * \param pfid parent fid the link to be found for
- *
- * \retval   0 success
- * \retval -ENOENT link does not exist
- * \retval -ve on error
- */
-int linkea_links_find(struct linkea_data *ldata, const struct lu_name *lname,
-		      const struct lu_fid  *pfid)
-{
-	struct lu_name tmpname;
-	struct lu_fid  tmpfid;
-	int count;
-
-	LASSERT(ldata->ld_leh != NULL);
-
-	/* link #0 */
-	ldata->ld_lee = (struct link_ea_entry *)(ldata->ld_leh + 1);
-
-	for (count = 0; count < ldata->ld_leh->leh_reccount; count++) {
-		linkea_entry_unpack(ldata->ld_lee, &ldata->ld_reclen,
-				    &tmpname, &tmpfid);
-		if (tmpname.ln_namelen == lname->ln_namelen &&
-		    lu_fid_eq(&tmpfid, pfid) &&
-		    (strncmp(tmpname.ln_name, lname->ln_name,
-			     tmpname.ln_namelen) == 0))
-			break;
-		ldata->ld_lee = (struct link_ea_entry *)((char *)ldata->ld_lee +
-							 ldata->ld_reclen);
-	}
-
-	if (count == ldata->ld_leh->leh_reccount) {
-		CDEBUG(D_INODE, "Old link_ea name '%.*s' not found\n",
-		       lname->ln_namelen, lname->ln_name);
-		ldata->ld_lee = NULL;
-		return -ENOENT;
-	}
-	return 0;
-}
-EXPORT_SYMBOL(linkea_links_find);
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
index 9ce3d36c685d..7eaaaa648dfb 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-module.c
@@ -58,10 +58,10 @@
 #include <linux/poll.h>
 #include <linux/list.h>
 #include <linux/highmem.h>
-#include <asm/io.h>
+#include <linux/io.h>
 #include <asm/ioctls.h>
-#include <asm/poll.h>
-#include <asm/uaccess.h>
+#include <linux/poll.h>
+#include <linux/uaccess.h>
 #include <linux/miscdevice.h>
 #include <linux/seq_file.h>
 
@@ -84,7 +84,7 @@ int obd_ioctl_getdata(char **buf, int *len, void *arg)
 	int offset = 0;
 
 	err = copy_from_user(&hdr, (void *)arg, sizeof(hdr));
-	if ( err )
+	if (err)
 		return err;
 
 	if (hdr.ioc_version != OBD_IOCTL_VERSION) {
@@ -118,7 +118,7 @@ int obd_ioctl_getdata(char **buf, int *len, void *arg)
 	data = (struct obd_ioctl_data *)*buf;
 
 	err = copy_from_user(*buf, (void *)arg, hdr.ioc_len);
-	if ( err ) {
+	if (err) {
 		OBD_FREE_LARGE(*buf, hdr.ioc_len);
 		return err;
 	}
diff --git a/drivers/staging/lustre/lustre/obdclass/linux/linux-obdo.c b/drivers/staging/lustre/lustre/obdclass/linux/linux-obdo.c
index bb15202f1aae..62ed706b136d 100644
--- a/drivers/staging/lustre/lustre/obdclass/linux/linux-obdo.c
+++ b/drivers/staging/lustre/lustre/obdclass/linux/linux-obdo.c
@@ -52,7 +52,7 @@
 /*FIXME: Just copy from obdo_from_inode*/
 void obdo_from_la(struct obdo *dst, struct lu_attr *la, __u64 valid)
 {
-	obd_flag newvalid = 0;
+	u32 newvalid = 0;
 
 	if (valid & LA_ATIME) {
 		dst->o_atime = la->la_atime;
@@ -97,7 +97,7 @@ void obdo_from_la(struct obdo *dst, struct lu_attr *la, __u64 valid)
 EXPORT_SYMBOL(obdo_from_la);
 
 /*FIXME: Just copy from obdo_from_inode*/
-void la_from_obdo(struct lu_attr *dst, struct obdo *obdo, obd_flag valid)
+void la_from_obdo(struct lu_attr *dst, struct obdo *obdo, u32 valid)
 {
 	__u64 newvalid = 0;
 
@@ -145,7 +145,7 @@ void la_from_obdo(struct lu_attr *dst, struct obdo *obdo, obd_flag valid)
 }
 EXPORT_SYMBOL(la_from_obdo);
 
-void obdo_refresh_inode(struct inode *dst, struct obdo *src, obd_flag valid)
+void obdo_refresh_inode(struct inode *dst, struct obdo *src, u32 valid)
 {
 	valid &= src->o_valid;
 
@@ -180,7 +180,7 @@ void obdo_refresh_inode(struct inode *dst, struct obdo *src, obd_flag valid)
 }
 EXPORT_SYMBOL(obdo_refresh_inode);
 
-void obdo_to_inode(struct inode *dst, struct obdo *src, obd_flag valid)
+void obdo_to_inode(struct inode *dst, struct obdo *src, u32 valid)
 {
 	valid &= src->o_valid;
 
diff --git a/drivers/staging/lustre/lustre/obdclass/llog.c b/drivers/staging/lustre/lustre/obdclass/llog.c
index cce86890c563..3ab05292152c 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog.c
@@ -56,7 +56,7 @@
  * Allocate a new log or catalog handle
  * Used inside llog_open().
  */
-struct llog_handle *llog_alloc_handle(void)
+static struct llog_handle *llog_alloc_handle(void)
 {
 	struct llog_handle *loghandle;
 
@@ -75,7 +75,7 @@ struct llog_handle *llog_alloc_handle(void)
 /*
  * Free llog handle and header data if exists. Used in llog_close() only
  */
-void llog_free_handle(struct llog_handle *loghandle)
+static void llog_free_handle(struct llog_handle *loghandle)
 {
 	LASSERT(loghandle != NULL);
 
@@ -140,7 +140,7 @@ int llog_cancel_rec(const struct lu_env *env, struct llog_handle *loghandle,
 			       loghandle->lgh_ctxt->loc_obd->obd_name,
 			       POSTID(&loghandle->lgh_id.lgl_oi),
 			       loghandle->lgh_id.lgl_ogen, rc);
-			GOTO(out_err, rc);
+			goto out_err;
 		}
 		return 1;
 	}
@@ -153,7 +153,7 @@ int llog_cancel_rec(const struct lu_env *env, struct llog_handle *loghandle,
 		       loghandle->lgh_ctxt->loc_obd->obd_name,
 		       POSTID(&loghandle->lgh_id.lgl_oi),
 		       loghandle->lgh_id.lgl_ogen, rc);
-		GOTO(out_err, rc);
+		goto out_err;
 	}
 	return 0;
 out_err:
@@ -224,7 +224,8 @@ int llog_init_handle(const struct lu_env *env, struct llog_handle *handle,
 			       llh->llh_flags & LLOG_F_IS_CAT ?
 			       "catalog" : "plain",
 			       flags & LLOG_F_IS_CAT ? "catalog" : "plain");
-			GOTO(out, rc = -EINVAL);
+			rc = -EINVAL;
+			goto out;
 		} else if (llh->llh_flags &
 			   (LLOG_F_IS_PLAIN | LLOG_F_IS_CAT)) {
 			/*
@@ -235,7 +236,8 @@ int llog_init_handle(const struct lu_env *env, struct llog_handle *handle,
 		} else {
 			/* for some reason the llh_flags has no type set */
 			CERROR("llog type is not specified!\n");
-			GOTO(out, rc = -EINVAL);
+			rc = -EINVAL;
+			goto out;
 		}
 		if (unlikely(uuid &&
 			     !obd_uuid_equals(uuid, &llh->llh_tgtuuid))) {
@@ -243,7 +245,8 @@ int llog_init_handle(const struct lu_env *env, struct llog_handle *handle,
 			       handle->lgh_ctxt->loc_obd->obd_name,
 			       (char *)uuid->uuid,
 			       (char *)llh->llh_tgtuuid.uuid);
-			GOTO(out, rc = -EEXIST);
+			rc = -EEXIST;
+			goto out;
 		}
 	}
 	if (flags & LLOG_F_IS_CAT) {
@@ -316,7 +319,7 @@ repeat:
 		rc = llog_next_block(lpi->lpi_env, loghandle, &saved_index,
 				     index, &cur_offset, buf, LLOG_CHUNK_SIZE);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 
 		/* NB: when rec->lrh_len is accessed it is already swabbed
 		 * since it is used at the "end" of the loop and the rec
@@ -336,16 +339,18 @@ repeat:
 
 			if (rec->lrh_index == 0) {
 				/* probably another rec just got added? */
+				rc = 0;
 				if (index <= loghandle->lgh_last_idx)
-					GOTO(repeat, rc = 0);
-				GOTO(out, rc = 0); /* no more records */
+					goto repeat;
+				goto out; /* no more records */
 			}
 			if (rec->lrh_len == 0 ||
 			    rec->lrh_len > LLOG_CHUNK_SIZE) {
 				CWARN("invalid length %d in llog record for "
 				      "index %d/%d\n", rec->lrh_len,
 				      rec->lrh_index, index);
-				GOTO(out, rc = -EINVAL);
+				rc = -EINVAL;
+				goto out;
 			}
 
 			if (rec->lrh_index < index) {
@@ -369,7 +374,7 @@ repeat:
 						 lpi->lpi_cbdata);
 				last_called_index = index;
 				if (rc == LLOG_PROC_BREAK) {
-					GOTO(out, rc);
+					goto out;
 				} else if (rc == LLOG_DEL_RECORD) {
 					llog_cancel_rec(lpi->lpi_env,
 							loghandle,
@@ -377,15 +382,17 @@ repeat:
 					rc = 0;
 				}
 				if (rc)
-					GOTO(out, rc);
+					goto out;
 			} else {
 				CDEBUG(D_OTHER, "Skipped index %d\n", index);
 			}
 
 			/* next record, still in buffer? */
 			++index;
-			if (index > last_index)
-				GOTO(out, rc = 0);
+			if (index > last_index) {
+				rc = 0;
+				goto out;
+			}
 		}
 	}
 
@@ -506,7 +513,7 @@ int llog_reverse_process(const struct lu_env *env,
 		rc = llog_prev_block(env, loghandle, index, buf,
 				     LLOG_CHUNK_SIZE);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 
 		rec = buf;
 		idx = rec->lrh_index;
@@ -522,8 +529,11 @@ int llog_reverse_process(const struct lu_env *env,
 
 		/* process records in buffer, starting where we found one */
 		while ((void *)tail > buf) {
-			if (tail->lrt_index == 0)
-				GOTO(out, rc = 0); /* no more records */
+			if (tail->lrt_index == 0) {
+				/* no more records */
+				rc = 0;
+				goto out;
+			}
 
 			/* if set, process the callback on this record */
 			if (ext2_test_bit(index, llh->llh_bitmap)) {
@@ -532,20 +542,22 @@ int llog_reverse_process(const struct lu_env *env,
 
 				rc = cb(env, loghandle, rec, data);
 				if (rc == LLOG_PROC_BREAK) {
-					GOTO(out, rc);
+					goto out;
 				} else if (rc == LLOG_DEL_RECORD) {
 					llog_cancel_rec(env, loghandle,
 							tail->lrt_index);
 					rc = 0;
 				}
 				if (rc)
-					GOTO(out, rc);
+					goto out;
 			}
 
 			/* previous record, still in buffer? */
 			--index;
-			if (index < first_index)
-				GOTO(out, rc = 0);
+			if (index < first_index) {
+				rc = 0;
+				goto out;
+			}
 			tail = (void *)tail - tail->lrt_len;
 		}
 	}
@@ -750,8 +762,10 @@ int llog_open_create(const struct lu_env *env, struct llog_ctxt *ctxt,
 	d = lu2dt_dev((*res)->lgh_obj->do_lu.lo_dev);
 
 	th = dt_trans_create(env, d);
-	if (IS_ERR(th))
-		GOTO(out, rc = PTR_ERR(th));
+	if (IS_ERR(th)) {
+		rc = PTR_ERR(th);
+		goto out;
+	}
 
 	rc = llog_declare_create(env, *res, th);
 	if (rc == 0) {
@@ -820,11 +834,11 @@ int llog_write(const struct lu_env *env, struct llog_handle *loghandle,
 
 	rc = llog_declare_write_rec(env, loghandle, rec, idx, th);
 	if (rc)
-		GOTO(out_trans, rc);
+		goto out_trans;
 
 	rc = dt_trans_start_local(env, dt, th);
 	if (rc)
-		GOTO(out_trans, rc);
+		goto out_trans;
 
 	down_write(&loghandle->lgh_lock);
 	rc = llog_write_rec(env, loghandle, rec, reccookie,
@@ -878,9 +892,11 @@ int llog_close(const struct lu_env *env, struct llog_handle *loghandle)
 
 	rc = llog_handle2ops(loghandle, &lop);
 	if (rc)
-		GOTO(out, rc);
-	if (lop->lop_close == NULL)
-		GOTO(out, rc = -EOPNOTSUPP);
+		goto out;
+	if (lop->lop_close == NULL) {
+		rc = -EOPNOTSUPP;
+		goto out;
+	}
 	rc = lop->lop_close(env, loghandle);
 out:
 	llog_handle_put(loghandle);
@@ -898,12 +914,12 @@ int llog_is_empty(const struct lu_env *env, struct llog_ctxt *ctxt,
 	if (rc < 0) {
 		if (likely(rc == -ENOENT))
 			rc = 0;
-		GOTO(out, rc);
+		goto out;
 	}
 
 	rc = llog_init_handle(env, llh, LLOG_F_IS_PLAIN, NULL);
 	if (rc)
-		GOTO(out_close, rc);
+		goto out_close;
 	rc = llog_get_size(llh);
 
 out_close:
@@ -948,19 +964,19 @@ int llog_backup(const struct lu_env *env, struct obd_device *obd,
 
 	rc = llog_init_handle(env, llh, LLOG_F_IS_PLAIN, NULL);
 	if (rc)
-		GOTO(out_close, rc);
+		goto out_close;
 
 	/* Make sure there's no old backup log */
 	rc = llog_erase(env, bctxt, NULL, backup);
 	if (rc < 0 && rc != -ENOENT)
-		GOTO(out_close, rc);
+		goto out_close;
 
 	/* open backup log */
 	rc = llog_open_create(env, bctxt, &bllh, NULL, backup);
 	if (rc) {
 		CERROR("%s: failed to open backup logfile %s: rc = %d\n",
 		       obd->obd_name, backup, rc);
-		GOTO(out_close, rc);
+		goto out_close;
 	}
 
 	/* check that backup llog is not the same object as original one */
@@ -968,12 +984,13 @@ int llog_backup(const struct lu_env *env, struct obd_device *obd,
 		CERROR("%s: backup llog %s to itself (%s), objects %p/%p\n",
 		       obd->obd_name, name, backup, llh->lgh_obj,
 		       bllh->lgh_obj);
-		GOTO(out_backup, rc = -EEXIST);
+		rc = -EEXIST;
+		goto out_backup;
 	}
 
 	rc = llog_init_handle(env, bllh, LLOG_F_IS_PLAIN, NULL);
 	if (rc)
-		GOTO(out_backup, rc);
+		goto out_backup;
 
 	/* Copy log record by record */
 	rc = llog_process_or_fork(env, llh, llog_copy_handler, (void *)bllh,
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_cat.c b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
index ca9927ccde68..6e139cf372c4 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_cat.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_cat.c
@@ -96,7 +96,7 @@ static int llog_cat_new_log(const struct lu_env *env,
 			      LLOG_F_IS_PLAIN | LLOG_F_ZAP_WHEN_EMPTY,
 			      &cathandle->lgh_hdr->llh_tgtuuid);
 	if (rc)
-		GOTO(out_destroy, rc);
+		goto out_destroy;
 
 	if (index == 0)
 		index = 1;
@@ -114,7 +114,8 @@ static int llog_cat_new_log(const struct lu_env *env,
 	cathandle->lgh_last_idx = index;
 	llh->llh_tail.lrt_index = index;
 
-	CDEBUG(D_RPCTRACE,"new recovery log "DOSTID":%x for index %u of catalog"
+	CDEBUG(D_RPCTRACE,
+	       "new recovery log "DOSTID":%x for index %u of catalog"
 	       DOSTID"\n", POSTID(&loghandle->lgh_id.lgl_oi),
 	       loghandle->lgh_id.lgl_ogen, index,
 	       POSTID(&cathandle->lgh_id.lgl_oi));
@@ -130,7 +131,7 @@ static int llog_cat_new_log(const struct lu_env *env,
 	rc = llog_write_rec(env, cathandle, &rec.lid_hdr,
 			    &loghandle->u.phd.phd_cookie, 1, NULL, index, th);
 	if (rc < 0)
-		GOTO(out_destroy, rc);
+		goto out_destroy;
 
 	loghandle->lgh_hdr->llh_cat_idx = index;
 	return 0;
@@ -173,7 +174,8 @@ int llog_cat_id2handle(const struct lu_env *env, struct llog_handle *cathandle,
 			}
 			loghandle->u.phd.phd_cat_handle = cathandle;
 			up_write(&cathandle->lgh_lock);
-			GOTO(out, rc = 0);
+			rc = 0;
+			goto out;
 		}
 	}
 	up_write(&cathandle->lgh_lock);
@@ -403,20 +405,20 @@ int llog_cat_declare_add_rec(const struct lu_env *env,
 		up_write(&cathandle->lgh_lock);
 	}
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	if (!llog_exist(cathandle->u.chd.chd_current_log)) {
 		rc = llog_declare_create(env, cathandle->u.chd.chd_current_log,
 					 th);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 		llog_declare_write_rec(env, cathandle, NULL, -1, th);
 	}
 	/* declare records in the llogs */
 	rc = llog_declare_write_rec(env, cathandle->u.chd.chd_current_log,
 				    rec, -1, th);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	next = cathandle->u.chd.chd_next_log;
 	if (next) {
@@ -454,11 +456,11 @@ int llog_cat_add(const struct lu_env *env, struct llog_handle *cathandle,
 
 		rc = llog_cat_declare_add_rec(env, cathandle, rec, th);
 		if (rc)
-			GOTO(out_trans, rc);
+			goto out_trans;
 
 		rc = dt_trans_start_local(env, dt, th);
 		if (rc)
-			GOTO(out_trans, rc);
+			goto out_trans;
 		rc = llog_cat_add_rec(env, cathandle, rec, reccookie, buf, th);
 out_trans:
 		dt_trans_stop(env, dt, th);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c b/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
deleted file mode 100644
index 9b7fa1d5e79a..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/llog_ioctl.c
+++ /dev/null
@@ -1,418 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#define DEBUG_SUBSYSTEM S_LOG
-
-#include "../include/obd_class.h"
-#include "../include/lustre_log.h"
-#include "llog_internal.h"
-
-static int str2logid(struct llog_logid *logid, char *str, int len)
-{
-	char *start, *end, *endp;
-	__u64 id, seq;
-
-	start = str;
-	if (*start != '#')
-		return -EINVAL;
-
-	start++;
-	if (start - str >= len - 1)
-		return -EINVAL;
-	end = strchr(start, '#');
-	if (end == NULL || end == start)
-		return -EINVAL;
-
-	*end = '\0';
-	id = simple_strtoull(start, &endp, 0);
-	if (endp != end)
-		return -EINVAL;
-
-	start = ++end;
-	if (start - str >= len - 1)
-		return -EINVAL;
-	end = strchr(start, '#');
-	if (end == NULL || end == start)
-		return -EINVAL;
-
-	*end = '\0';
-	seq = simple_strtoull(start, &endp, 0);
-	if (endp != end)
-		return -EINVAL;
-
-	ostid_set_seq(&logid->lgl_oi, seq);
-	ostid_set_id(&logid->lgl_oi, id);
-
-	start = ++end;
-	if (start - str >= len - 1)
-		return -EINVAL;
-	logid->lgl_ogen = simple_strtoul(start, &endp, 16);
-	if (*endp != '\0')
-		return -EINVAL;
-
-	return 0;
-}
-
-static int llog_check_cb(const struct lu_env *env, struct llog_handle *handle,
-			 struct llog_rec_hdr *rec, void *data)
-{
-	struct obd_ioctl_data *ioc_data = (struct obd_ioctl_data *)data;
-	static int l, remains, from, to;
-	static char *out;
-	char *endp;
-	int cur_index, rc = 0;
-
-	if (ioc_data && ioc_data->ioc_inllen1 > 0) {
-		l = 0;
-		remains = ioc_data->ioc_inllen4 +
-			cfs_size_round(ioc_data->ioc_inllen1) +
-			cfs_size_round(ioc_data->ioc_inllen2) +
-			cfs_size_round(ioc_data->ioc_inllen3);
-		from = simple_strtol(ioc_data->ioc_inlbuf2, &endp, 0);
-		if (*endp != '\0')
-			return -EINVAL;
-		to = simple_strtol(ioc_data->ioc_inlbuf3, &endp, 0);
-		if (*endp != '\0')
-			return -EINVAL;
-		ioc_data->ioc_inllen1 = 0;
-		out = ioc_data->ioc_bulk;
-	}
-
-	cur_index = rec->lrh_index;
-	if (cur_index < from)
-		return 0;
-	if (to > 0 && cur_index > to)
-		return -LLOG_EEMPTY;
-
-	if (handle->lgh_hdr->llh_flags & LLOG_F_IS_CAT) {
-		struct llog_logid_rec	*lir = (struct llog_logid_rec *)rec;
-		struct llog_handle	*loghandle;
-
-		if (rec->lrh_type != LLOG_LOGID_MAGIC) {
-			l = snprintf(out, remains, "[index]: %05d  [type]: "
-				     "%02x  [len]: %04d failed\n",
-				     cur_index, rec->lrh_type,
-				     rec->lrh_len);
-		}
-		if (handle->lgh_ctxt == NULL)
-			return -EOPNOTSUPP;
-		rc = llog_cat_id2handle(env, handle, &loghandle, &lir->lid_id);
-		if (rc) {
-			CDEBUG(D_IOCTL, "cannot find log #"DOSTID"#%08x\n",
-			       POSTID(&lir->lid_id.lgl_oi),
-			       lir->lid_id.lgl_ogen);
-			return rc;
-		}
-		rc = llog_process(env, loghandle, llog_check_cb, NULL, NULL);
-		llog_handle_put(loghandle);
-	} else {
-		bool ok;
-
-		switch (rec->lrh_type) {
-		case OST_SZ_REC:
-		case MDS_UNLINK_REC:
-		case MDS_UNLINK64_REC:
-		case MDS_SETATTR64_REC:
-		case OBD_CFG_REC:
-		case LLOG_GEN_REC:
-		case LLOG_HDR_MAGIC:
-			ok = true;
-			break;
-		default:
-			ok = false;
-		}
-
-		l = snprintf(out, remains, "[index]: %05d  [type]: "
-			     "%02x  [len]: %04d %s\n",
-			     cur_index, rec->lrh_type, rec->lrh_len,
-			     ok ? "ok" : "failed");
-		out += l;
-		remains -= l;
-		if (remains <= 0) {
-			CERROR("%s: no space to print log records\n",
-			       handle->lgh_ctxt->loc_obd->obd_name);
-			return -LLOG_EEMPTY;
-		}
-	}
-	return rc;
-}
-
-static int llog_print_cb(const struct lu_env *env, struct llog_handle *handle,
-			 struct llog_rec_hdr *rec, void *data)
-{
-	struct obd_ioctl_data *ioc_data = (struct obd_ioctl_data *)data;
-	static int l, remains, from, to;
-	static char *out;
-	char *endp;
-	int cur_index;
-
-	if (ioc_data != NULL && ioc_data->ioc_inllen1 > 0) {
-		l = 0;
-		remains = ioc_data->ioc_inllen4 +
-			cfs_size_round(ioc_data->ioc_inllen1) +
-			cfs_size_round(ioc_data->ioc_inllen2) +
-			cfs_size_round(ioc_data->ioc_inllen3);
-		from = simple_strtol(ioc_data->ioc_inlbuf2, &endp, 0);
-		if (*endp != '\0')
-			return -EINVAL;
-		to = simple_strtol(ioc_data->ioc_inlbuf3, &endp, 0);
-		if (*endp != '\0')
-			return -EINVAL;
-		out = ioc_data->ioc_bulk;
-		ioc_data->ioc_inllen1 = 0;
-	}
-
-	cur_index = rec->lrh_index;
-	if (cur_index < from)
-		return 0;
-	if (to > 0 && cur_index > to)
-		return -LLOG_EEMPTY;
-
-	if (handle->lgh_hdr->llh_flags & LLOG_F_IS_CAT) {
-		struct llog_logid_rec *lir = (struct llog_logid_rec *)rec;
-
-		if (rec->lrh_type != LLOG_LOGID_MAGIC) {
-			CERROR("invalid record in catalog\n");
-			return -EINVAL;
-		}
-
-		l = snprintf(out, remains,
-			     "[index]: %05d  [logid]: #"DOSTID"#%08x\n",
-			     cur_index, POSTID(&lir->lid_id.lgl_oi),
-			     lir->lid_id.lgl_ogen);
-	} else if (rec->lrh_type == OBD_CFG_REC) {
-		int rc;
-
-		rc = class_config_parse_rec(rec, out, remains);
-		if (rc < 0)
-			return rc;
-		l = rc;
-	} else {
-		l = snprintf(out, remains,
-			     "[index]: %05d  [type]: %02x  [len]: %04d\n",
-			     cur_index, rec->lrh_type, rec->lrh_len);
-	}
-	out += l;
-	remains -= l;
-	if (remains <= 0) {
-		CERROR("not enough space for print log records\n");
-		return -LLOG_EEMPTY;
-	}
-
-	return 0;
-}
-static int llog_remove_log(const struct lu_env *env, struct llog_handle *cat,
-			   struct llog_logid *logid)
-{
-	struct llog_handle	*log;
-	int			 rc;
-
-	rc = llog_cat_id2handle(env, cat, &log, logid);
-	if (rc) {
-		CDEBUG(D_IOCTL, "cannot find log #"DOSTID"#%08x\n",
-		       POSTID(&logid->lgl_oi), logid->lgl_ogen);
-		return -ENOENT;
-	}
-
-	rc = llog_destroy(env, log);
-	if (rc) {
-		CDEBUG(D_IOCTL, "cannot destroy log\n");
-		GOTO(out, rc);
-	}
-	llog_cat_cleanup(env, cat, log, log->u.phd.phd_cookie.lgc_index);
-out:
-	llog_handle_put(log);
-	return rc;
-
-}
-
-static int llog_delete_cb(const struct lu_env *env, struct llog_handle *handle,
-			  struct llog_rec_hdr *rec, void *data)
-{
-	struct llog_logid_rec	*lir = (struct llog_logid_rec *)rec;
-	int			 rc;
-
-	if (rec->lrh_type != LLOG_LOGID_MAGIC)
-		return -EINVAL;
-	rc = llog_remove_log(env, handle, &lir->lid_id);
-
-	return rc;
-}
-
-
-int llog_ioctl(const struct lu_env *env, struct llog_ctxt *ctxt, int cmd,
-	       struct obd_ioctl_data *data)
-{
-	struct llog_logid	 logid;
-	int			 rc = 0;
-	struct llog_handle	*handle = NULL;
-
-	if (*data->ioc_inlbuf1 == '#') {
-		rc = str2logid(&logid, data->ioc_inlbuf1, data->ioc_inllen1);
-		if (rc)
-			return rc;
-		rc = llog_open(env, ctxt, &handle, &logid, NULL,
-			       LLOG_OPEN_EXISTS);
-		if (rc)
-			return rc;
-	} else if (*data->ioc_inlbuf1 == '$') {
-		char *name = data->ioc_inlbuf1 + 1;
-
-		rc = llog_open(env, ctxt, &handle, NULL, name,
-			       LLOG_OPEN_EXISTS);
-		if (rc)
-			return rc;
-	} else {
-		return -EINVAL;
-	}
-
-	rc = llog_init_handle(env, handle, 0, NULL);
-	if (rc)
-		GOTO(out_close, rc = -ENOENT);
-
-	switch (cmd) {
-	case OBD_IOC_LLOG_INFO: {
-		int	 l;
-		int	 remains = data->ioc_inllen2 +
-				   cfs_size_round(data->ioc_inllen1);
-		char	*out = data->ioc_bulk;
-
-		l = snprintf(out, remains,
-			     "logid:	    #"DOSTID"#%08x\n"
-			     "flags:	    %x (%s)\n"
-			     "records count:    %d\n"
-			     "last index:       %d\n",
-			     POSTID(&handle->lgh_id.lgl_oi),
-			     handle->lgh_id.lgl_ogen,
-			     handle->lgh_hdr->llh_flags,
-			     handle->lgh_hdr->llh_flags &
-			     LLOG_F_IS_CAT ? "cat" : "plain",
-			     handle->lgh_hdr->llh_count,
-			     handle->lgh_last_idx);
-		out += l;
-		remains -= l;
-		if (remains <= 0) {
-			CERROR("%s: not enough space for log header info\n",
-			       ctxt->loc_obd->obd_name);
-			rc = -ENOSPC;
-		}
-		break;
-	}
-	case OBD_IOC_LLOG_CHECK:
-		LASSERT(data->ioc_inllen1 > 0);
-		rc = llog_process(env, handle, llog_check_cb, data, NULL);
-		if (rc == -LLOG_EEMPTY)
-			rc = 0;
-		else if (rc)
-			GOTO(out_close, rc);
-		break;
-	case OBD_IOC_LLOG_PRINT:
-		LASSERT(data->ioc_inllen1 > 0);
-		rc = llog_process(env, handle, llog_print_cb, data, NULL);
-		if (rc == -LLOG_EEMPTY)
-			rc = 0;
-		else if (rc)
-			GOTO(out_close, rc);
-		break;
-	case OBD_IOC_LLOG_CANCEL: {
-		struct llog_cookie cookie;
-		struct llog_logid plain;
-		char *endp;
-
-		cookie.lgc_index = simple_strtoul(data->ioc_inlbuf3, &endp, 0);
-		if (*endp != '\0')
-			GOTO(out_close, rc = -EINVAL);
-
-		if (handle->lgh_hdr->llh_flags & LLOG_F_IS_PLAIN) {
-			rc = llog_cancel_rec(NULL, handle, cookie.lgc_index);
-			GOTO(out_close, rc);
-		} else if (!(handle->lgh_hdr->llh_flags & LLOG_F_IS_CAT)) {
-			GOTO(out_close, rc = -EINVAL);
-		}
-
-		if (data->ioc_inlbuf2 == NULL) /* catalog but no logid */
-			GOTO(out_close, rc = -ENOTTY);
-
-		rc = str2logid(&plain, data->ioc_inlbuf2, data->ioc_inllen2);
-		if (rc)
-			GOTO(out_close, rc);
-		cookie.lgc_lgl = plain;
-		rc = llog_cat_cancel_records(env, handle, 1, &cookie);
-		if (rc)
-			GOTO(out_close, rc);
-		break;
-	}
-	case OBD_IOC_LLOG_REMOVE: {
-		struct llog_logid plain;
-
-		if (handle->lgh_hdr->llh_flags & LLOG_F_IS_PLAIN) {
-			rc = llog_destroy(env, handle);
-			GOTO(out_close, rc);
-		} else if (!(handle->lgh_hdr->llh_flags & LLOG_F_IS_CAT)) {
-			GOTO(out_close, rc = -EINVAL);
-		}
-
-		if (data->ioc_inllen2 > 0) {
-			/* remove indicate log from the catalog */
-			rc = str2logid(&plain, data->ioc_inlbuf2,
-				       data->ioc_inllen2);
-			if (rc)
-				GOTO(out_close, rc);
-			rc = llog_remove_log(env, handle, &plain);
-		} else {
-			/* remove all the log of the catalog */
-			rc = llog_process(env, handle, llog_delete_cb, NULL,
-					  NULL);
-			if (rc)
-				GOTO(out_close, rc);
-		}
-		break;
-	}
-	default:
-		CERROR("%s: Unknown ioctl cmd %#x\n",
-		       ctxt->loc_obd->obd_name, cmd);
-		GOTO(out_close, rc = -ENOTTY);
-	}
-
-out_close:
-	if (handle->lgh_hdr &&
-	    handle->lgh_hdr->llh_flags & LLOG_F_IS_CAT)
-		llog_cat_close(env, handle);
-	else
-		llog_close(env, handle);
-	return rc;
-}
-EXPORT_SYMBOL(llog_ioctl);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_lvfs.c b/drivers/staging/lustre/lustre/obdclass/llog_lvfs.c
deleted file mode 100644
index fd48d59cf315..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/llog_lvfs.c
+++ /dev/null
@@ -1,847 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/obdclass/llog_lvfs.c
- *
- * OST<->MDS recovery logging infrastructure.
- * Invariants in implementation:
- * - we do not share logs among different OST<->MDS connections, so that
- *   if an OST or MDS fails it need only look at log(s) relevant to itself
- *
- * Author: Andreas Dilger <adilger@clusterfs.com>
- */
-
-#define DEBUG_SUBSYSTEM S_LOG
-
-
-#include "../include/obd.h"
-#include "../include/obd_class.h"
-#include "../include/lustre_log.h"
-#include "../include/obd_ost.h"
-#include <linux/list.h>
-#include "../include/lvfs.h"
-#include "../include/lustre_fsfilt.h"
-#include "../include/lustre_disk.h"
-#include "llog_internal.h"
-
-#if  defined(LLOG_LVFS)
-
-static int llog_lvfs_pad(struct obd_device *obd, struct file *file, int len,
-			 int index)
-{
-	struct llog_rec_hdr rec = { 0 };
-	struct llog_rec_tail tail;
-	int rc;
-
-	LASSERT(len >= LLOG_MIN_REC_SIZE && (len & 0x7) == 0);
-
-	tail.lrt_len = rec.lrh_len = len;
-	tail.lrt_index = rec.lrh_index = index;
-	rec.lrh_type = LLOG_PAD_MAGIC;
-
-	rc = fsfilt_write_record(obd, file, &rec, sizeof(rec), &file->f_pos, 0);
-	if (rc) {
-		CERROR("error writing padding record: rc %d\n", rc);
-		goto out;
-	}
-
-	file->f_pos += len - sizeof(rec) - sizeof(tail);
-	rc = fsfilt_write_record(obd, file, &tail, sizeof(tail),&file->f_pos,0);
-	if (rc) {
-		CERROR("error writing padding record: rc %d\n", rc);
-		goto out;
-	}
-
- out:
-	return rc;
-}
-
-static int llog_lvfs_write_blob(struct obd_device *obd, struct file *file,
-				struct llog_rec_hdr *rec, void *buf, loff_t off)
-{
-	int rc;
-	struct llog_rec_tail end;
-	loff_t saved_off = file->f_pos;
-	int buflen = rec->lrh_len;
-
-	file->f_pos = off;
-
-	if (buflen == 0)
-		CWARN("0-length record\n");
-
-	if (!buf) {
-		rc = fsfilt_write_record(obd, file, rec, buflen,&file->f_pos,0);
-		if (rc) {
-			CERROR("error writing log record: rc %d\n", rc);
-			goto out;
-		}
-		GOTO(out, rc = 0);
-	}
-
-	/* the buf case */
-	rec->lrh_len = sizeof(*rec) + buflen + sizeof(end);
-	rc = fsfilt_write_record(obd, file, rec, sizeof(*rec), &file->f_pos, 0);
-	if (rc) {
-		CERROR("error writing log hdr: rc %d\n", rc);
-		goto out;
-	}
-
-	rc = fsfilt_write_record(obd, file, buf, buflen, &file->f_pos, 0);
-	if (rc) {
-		CERROR("error writing log buffer: rc %d\n", rc);
-		goto out;
-	}
-
-	end.lrt_len = rec->lrh_len;
-	end.lrt_index = rec->lrh_index;
-	rc = fsfilt_write_record(obd, file, &end, sizeof(end), &file->f_pos, 0);
-	if (rc) {
-		CERROR("error writing log tail: rc %d\n", rc);
-		goto out;
-	}
-
-	rc = 0;
- out:
-	if (saved_off > file->f_pos)
-		file->f_pos = saved_off;
-	LASSERT(rc <= 0);
-	return rc;
-}
-
-static int llog_lvfs_read_blob(struct obd_device *obd, struct file *file,
-				void *buf, int size, loff_t off)
-{
-	loff_t offset = off;
-	int rc;
-
-	rc = fsfilt_read_record(obd, file, buf, size, &offset);
-	if (rc) {
-		CERROR("error reading log record: rc %d\n", rc);
-		return rc;
-	}
-	return 0;
-}
-
-static int llog_lvfs_read_header(const struct lu_env *env,
-				 struct llog_handle *handle)
-{
-	struct obd_device *obd;
-	int rc;
-
-	LASSERT(sizeof(*handle->lgh_hdr) == LLOG_CHUNK_SIZE);
-
-	obd = handle->lgh_ctxt->loc_exp->exp_obd;
-
-	if (i_size_read(handle->lgh_file->f_dentry->d_inode) == 0) {
-		CDEBUG(D_HA, "not reading header from 0-byte log\n");
-		return LLOG_EEMPTY;
-	}
-
-	rc = llog_lvfs_read_blob(obd, handle->lgh_file, handle->lgh_hdr,
-				 LLOG_CHUNK_SIZE, 0);
-	if (rc) {
-		CERROR("error reading log header from %.*s\n",
-		       handle->lgh_file->f_dentry->d_name.len,
-		       handle->lgh_file->f_dentry->d_name.name);
-	} else {
-		struct llog_rec_hdr *llh_hdr = &handle->lgh_hdr->llh_hdr;
-
-		if (LLOG_REC_HDR_NEEDS_SWABBING(llh_hdr))
-			lustre_swab_llog_hdr(handle->lgh_hdr);
-
-		if (llh_hdr->lrh_type != LLOG_HDR_MAGIC) {
-			CERROR("bad log %.*s header magic: %#x (expected %#x)\n",
-			       handle->lgh_file->f_dentry->d_name.len,
-			       handle->lgh_file->f_dentry->d_name.name,
-			       llh_hdr->lrh_type, LLOG_HDR_MAGIC);
-			rc = -EIO;
-		} else if (llh_hdr->lrh_len != LLOG_CHUNK_SIZE) {
-			CERROR("incorrectly sized log %.*s header: %#x "
-			       "(expected %#x)\n",
-			       handle->lgh_file->f_dentry->d_name.len,
-			       handle->lgh_file->f_dentry->d_name.name,
-			       llh_hdr->lrh_len, LLOG_CHUNK_SIZE);
-			CERROR("you may need to re-run lconf --write_conf.\n");
-			rc = -EIO;
-		}
-	}
-
-	handle->lgh_last_idx = handle->lgh_hdr->llh_tail.lrt_index;
-	handle->lgh_file->f_pos = i_size_read(handle->lgh_file->f_dentry->d_inode);
-
-	return rc;
-}
-
-/* returns negative in on error; 0 if success && reccookie == 0; 1 otherwise */
-/* appends if idx == -1, otherwise overwrites record idx. */
-static int llog_lvfs_write_rec(const struct lu_env *env,
-			       struct llog_handle *loghandle,
-			       struct llog_rec_hdr *rec,
-			       struct llog_cookie *reccookie, int cookiecount,
-			       void *buf, int idx, struct thandle *th)
-{
-	struct llog_log_hdr *llh;
-	int reclen = rec->lrh_len, index, rc;
-	struct llog_rec_tail *lrt;
-	struct obd_device *obd;
-	struct file *file;
-	size_t left;
-
-	llh = loghandle->lgh_hdr;
-	file = loghandle->lgh_file;
-	obd = loghandle->lgh_ctxt->loc_exp->exp_obd;
-
-	/* record length should not bigger than LLOG_CHUNK_SIZE */
-	if (buf)
-		rc = (reclen > LLOG_CHUNK_SIZE - sizeof(struct llog_rec_hdr) -
-		      sizeof(struct llog_rec_tail)) ? -E2BIG : 0;
-	else
-		rc = (reclen > LLOG_CHUNK_SIZE) ? -E2BIG : 0;
-	if (rc)
-		return rc;
-
-	if (buf)
-		/* write_blob adds header and tail to lrh_len. */
-		reclen = sizeof(*rec) + rec->lrh_len +
-			 sizeof(struct llog_rec_tail);
-
-	if (idx != -1) {
-		loff_t saved_offset;
-
-		/* no header: only allowed to insert record 1 */
-		if (idx != 1 && !i_size_read(file->f_dentry->d_inode)) {
-			CERROR("idx != -1 in empty log\n");
-			LBUG();
-		}
-
-		if (idx && llh->llh_size && llh->llh_size != rec->lrh_len)
-			return -EINVAL;
-
-		if (!ext2_test_bit(idx, llh->llh_bitmap))
-			CERROR("Modify unset record %u\n", idx);
-		if (idx != rec->lrh_index)
-			CERROR("Index mismatch %d %u\n", idx, rec->lrh_index);
-
-		rc = llog_lvfs_write_blob(obd, file, &llh->llh_hdr, NULL, 0);
-		/* we are done if we only write the header or on error */
-		if (rc || idx == 0)
-			return rc;
-
-		if (buf) {
-			/* We assume that caller has set lgh_cur_* */
-			saved_offset = loghandle->lgh_cur_offset;
-			CDEBUG(D_OTHER,
-			       "modify record "DOSTID": idx:%d/%u/%d, len:%u "
-			       "offset %llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi), idx, rec->lrh_index,
-			       loghandle->lgh_cur_idx, rec->lrh_len,
-			       (long long)(saved_offset - sizeof(*llh)));
-			if (rec->lrh_index != loghandle->lgh_cur_idx) {
-				CERROR("modify idx mismatch %u/%d\n",
-				       idx, loghandle->lgh_cur_idx);
-				return -EFAULT;
-			}
-		} else {
-			/* Assumes constant lrh_len */
-			saved_offset = sizeof(*llh) + (idx - 1) * reclen;
-		}
-
-		rc = llog_lvfs_write_blob(obd, file, rec, buf, saved_offset);
-		if (rc == 0 && reccookie) {
-			reccookie->lgc_lgl = loghandle->lgh_id;
-			reccookie->lgc_index = idx;
-			rc = 1;
-		}
-		return rc;
-	}
-
-	/* Make sure that records don't cross a chunk boundary, so we can
-	 * process them page-at-a-time if needed.  If it will cross a chunk
-	 * boundary, write in a fake (but referenced) entry to pad the chunk.
-	 *
-	 * We know that llog_current_log() will return a loghandle that is
-	 * big enough to hold reclen, so all we care about is padding here.
-	 */
-	left = LLOG_CHUNK_SIZE - (file->f_pos & (LLOG_CHUNK_SIZE - 1));
-
-	/* NOTE: padding is a record, but no bit is set */
-	if (left != 0 && left != reclen &&
-	    left < (reclen + LLOG_MIN_REC_SIZE)) {
-		 index = loghandle->lgh_last_idx + 1;
-		 rc = llog_lvfs_pad(obd, file, left, index);
-		 if (rc)
-			 return rc;
-		 loghandle->lgh_last_idx++; /*for pad rec*/
-	 }
-	 /* if it's the last idx in log file, then return -ENOSPC */
-	 if (loghandle->lgh_last_idx >= LLOG_BITMAP_SIZE(llh) - 1)
-		 return -ENOSPC;
-	loghandle->lgh_last_idx++;
-	index = loghandle->lgh_last_idx;
-	LASSERT(index < LLOG_BITMAP_SIZE(llh));
-	rec->lrh_index = index;
-	if (buf == NULL) {
-		lrt = (struct llog_rec_tail *)
-			((char *)rec + rec->lrh_len - sizeof(*lrt));
-		lrt->lrt_len = rec->lrh_len;
-		lrt->lrt_index = rec->lrh_index;
-	}
-	/*The caller should make sure only 1 process access the lgh_last_idx,
-	 *Otherwise it might hit the assert.*/
-	LASSERT(index < LLOG_BITMAP_SIZE(llh));
-	spin_lock(&loghandle->lgh_hdr_lock);
-	if (ext2_set_bit(index, llh->llh_bitmap)) {
-		CERROR("argh, index %u already set in log bitmap?\n", index);
-		spin_unlock(&loghandle->lgh_hdr_lock);
-		LBUG(); /* should never happen */
-	}
-	llh->llh_count++;
-	spin_unlock(&loghandle->lgh_hdr_lock);
-	llh->llh_tail.lrt_index = index;
-
-	rc = llog_lvfs_write_blob(obd, file, &llh->llh_hdr, NULL, 0);
-	if (rc)
-		return rc;
-
-	rc = llog_lvfs_write_blob(obd, file, rec, buf, file->f_pos);
-	if (rc)
-		return rc;
-
-	CDEBUG(D_RPCTRACE, "added record "DOSTID": idx: %u, %u \n",
-	       POSTID(&loghandle->lgh_id.lgl_oi), index, rec->lrh_len);
-	if (rc == 0 && reccookie) {
-		reccookie->lgc_lgl = loghandle->lgh_id;
-		reccookie->lgc_index = index;
-		if ((rec->lrh_type == MDS_UNLINK_REC) ||
-		    (rec->lrh_type == MDS_SETATTR64_REC))
-			reccookie->lgc_subsys = LLOG_MDS_OST_ORIG_CTXT;
-		else if (rec->lrh_type == OST_SZ_REC)
-			reccookie->lgc_subsys = LLOG_SIZE_ORIG_CTXT;
-		else
-			reccookie->lgc_subsys = -1;
-		rc = 1;
-	}
-	if (rc == 0 && rec->lrh_type == LLOG_GEN_REC)
-		rc = 1;
-
-	return rc;
-}
-
-/* We can skip reading at least as many log blocks as the number of
-* minimum sized log records we are skipping.  If it turns out
-* that we are not far enough along the log (because the
-* actual records are larger than minimum size) we just skip
-* some more records. */
-
-static void llog_skip_over(__u64 *off, int curr, int goal)
-{
-	if (goal <= curr)
-		return;
-	*off = (*off + (goal-curr-1) * LLOG_MIN_REC_SIZE) &
-		~(LLOG_CHUNK_SIZE - 1);
-}
-
-
-/* sets:
- *  - cur_offset to the furthest point read in the log file
- *  - cur_idx to the log index preceding cur_offset
- * returns -EIO/-EINVAL on error
- */
-static int llog_lvfs_next_block(const struct lu_env *env,
-				struct llog_handle *loghandle, int *cur_idx,
-				int next_idx, __u64 *cur_offset, void *buf,
-				int len)
-{
-	int rc;
-
-	if (len == 0 || len & (LLOG_CHUNK_SIZE - 1))
-		return -EINVAL;
-
-	CDEBUG(D_OTHER, "looking for log index %u (cur idx %u off %llu)\n",
-	       next_idx, *cur_idx, *cur_offset);
-
-	while (*cur_offset < i_size_read(loghandle->lgh_file->f_dentry->d_inode)) {
-		struct llog_rec_hdr *rec, *last_rec;
-		struct llog_rec_tail *tail;
-		loff_t ppos;
-		int llen;
-
-		llog_skip_over(cur_offset, *cur_idx, next_idx);
-
-		/* read up to next LLOG_CHUNK_SIZE block */
-		ppos = *cur_offset;
-		llen = LLOG_CHUNK_SIZE - (*cur_offset & (LLOG_CHUNK_SIZE - 1));
-		rc = fsfilt_read_record(loghandle->lgh_ctxt->loc_exp->exp_obd,
-					loghandle->lgh_file, buf, llen,
-					cur_offset);
-		if (rc < 0) {
-			CERROR("Cant read llog block at log id "DOSTID
-			       "/%u offset %llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen,
-			       *cur_offset);
-			return rc;
-		}
-
-		/* put number of bytes read into rc to make code simpler */
-		rc = *cur_offset - ppos;
-		if (rc < len) {
-			/* signal the end of the valid buffer to llog_process */
-			memset(buf + rc, 0, len - rc);
-		}
-
-		if (rc == 0) /* end of file, nothing to do */
-			return 0;
-
-		if (rc < sizeof(*tail)) {
-			CERROR("Invalid llog block at log id "DOSTID"/%u offset%llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, *cur_offset);
-			return -EINVAL;
-		}
-
-		rec = buf;
-		if (LLOG_REC_HDR_NEEDS_SWABBING(rec))
-			lustre_swab_llog_rec(rec);
-
-		tail = (struct llog_rec_tail *)(buf + rc -
-						sizeof(struct llog_rec_tail));
-
-		/* get the last record in block */
-		last_rec = (struct llog_rec_hdr *)(buf + rc -
-						   le32_to_cpu(tail->lrt_len));
-
-		if (LLOG_REC_HDR_NEEDS_SWABBING(last_rec))
-			lustre_swab_llog_rec(last_rec);
-		LASSERT(last_rec->lrh_index == tail->lrt_index);
-
-		*cur_idx = tail->lrt_index;
-
-		/* this shouldn't happen */
-		if (tail->lrt_index == 0) {
-			CERROR("Invalid llog tail at log id "DOSTID"/%u offset %llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, *cur_offset);
-			return -EINVAL;
-		}
-		if (tail->lrt_index < next_idx)
-			continue;
-
-		/* sanity check that the start of the new buffer is no farther
-		 * than the record that we wanted.  This shouldn't happen. */
-		if (rec->lrh_index > next_idx) {
-			CERROR("missed desired record? %u > %u\n",
-			       rec->lrh_index, next_idx);
-			return -ENOENT;
-		}
-		return 0;
-	}
-	return -EIO;
-}
-
-static int llog_lvfs_prev_block(const struct lu_env *env,
-				struct llog_handle *loghandle,
-				int prev_idx, void *buf, int len)
-{
-	__u64 cur_offset;
-	int rc;
-
-	if (len == 0 || len & (LLOG_CHUNK_SIZE - 1))
-		return -EINVAL;
-
-	CDEBUG(D_OTHER, "looking for log index %u\n", prev_idx);
-
-	cur_offset = LLOG_CHUNK_SIZE;
-	llog_skip_over(&cur_offset, 0, prev_idx);
-
-	while (cur_offset < i_size_read(loghandle->lgh_file->f_dentry->d_inode)) {
-		struct llog_rec_hdr *rec, *last_rec;
-		struct llog_rec_tail *tail;
-		loff_t ppos = cur_offset;
-
-		rc = fsfilt_read_record(loghandle->lgh_ctxt->loc_exp->exp_obd,
-					loghandle->lgh_file, buf, len,
-					&cur_offset);
-		if (rc < 0) {
-			CERROR("Cant read llog block at log id "DOSTID
-			       "/%u offset %llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen,
-			       cur_offset);
-			return rc;
-		}
-
-		/* put number of bytes read into rc to make code simpler */
-		rc = cur_offset - ppos;
-
-		if (rc == 0) /* end of file, nothing to do */
-			return 0;
-
-		if (rc < sizeof(*tail)) {
-			CERROR("Invalid llog block at log id "DOSTID"/%u offset%llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, cur_offset);
-			return -EINVAL;
-		}
-
-		rec = buf;
-		if (LLOG_REC_HDR_NEEDS_SWABBING(rec))
-			lustre_swab_llog_rec(rec);
-
-		tail = (struct llog_rec_tail *)(buf + rc -
-						sizeof(struct llog_rec_tail));
-
-		/* get the last record in block */
-		last_rec = (struct llog_rec_hdr *)(buf + rc -
-						   le32_to_cpu(tail->lrt_len));
-
-		if (LLOG_REC_HDR_NEEDS_SWABBING(last_rec))
-			lustre_swab_llog_rec(last_rec);
-		LASSERT(last_rec->lrh_index == tail->lrt_index);
-
-		/* this shouldn't happen */
-		if (tail->lrt_index == 0) {
-			CERROR("Invalid llog tail at log id "DOSTID"/%u offset%llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, cur_offset);
-			return -EINVAL;
-		}
-		if (tail->lrt_index < prev_idx)
-			continue;
-
-		/* sanity check that the start of the new buffer is no farther
-		 * than the record that we wanted.  This shouldn't happen. */
-		if (rec->lrh_index > prev_idx) {
-			CERROR("missed desired record? %u > %u\n",
-			       rec->lrh_index, prev_idx);
-			return -ENOENT;
-		}
-		return 0;
-	}
-	return -EIO;
-}
-
-static struct file *llog_filp_open(char *dir, char *name, int flags, int mode)
-{
-	char *logname;
-	struct file *filp;
-	int len;
-
-	OBD_ALLOC(logname, PATH_MAX);
-	if (logname == NULL)
-		return ERR_PTR(-ENOMEM);
-
-	len = snprintf(logname, PATH_MAX, "%s/%s", dir, name);
-	if (len >= PATH_MAX - 1) {
-		filp = ERR_PTR(-ENAMETOOLONG);
-	} else {
-		filp = filp_open(logname, flags, mode);
-		if (IS_ERR(filp) && PTR_ERR(filp) != -ENOENT)
-			CERROR("logfile creation %s: %ld\n", logname,
-			       PTR_ERR(filp));
-	}
-	OBD_FREE(logname, PATH_MAX);
-	return filp;
-}
-
-static int llog_lvfs_open(const struct lu_env *env,  struct llog_handle *handle,
-			  struct llog_logid *logid, char *name,
-			  enum llog_open_param open_param)
-{
-	struct llog_ctxt	*ctxt = handle->lgh_ctxt;
-	struct dentry		*dchild = NULL;
-	struct obd_device	*obd;
-	int			 rc = 0;
-
-	LASSERT(ctxt);
-	LASSERT(ctxt->loc_exp);
-	LASSERT(ctxt->loc_exp->exp_obd);
-	obd = ctxt->loc_exp->exp_obd;
-
-	LASSERT(handle);
-	if (logid != NULL) {
-		dchild = obd_lvfs_fid2dentry(ctxt->loc_exp, &logid->lgl_oi,
-					     logid->lgl_ogen);
-		if (IS_ERR(dchild)) {
-			rc = PTR_ERR(dchild);
-			CERROR("%s: error looking up logfile #"DOSTID "#%08x:"
-			       " rc = %d\n", ctxt->loc_obd->obd_name,
-			       POSTID(&logid->lgl_oi), logid->lgl_ogen, rc);
-			GOTO(out, rc);
-		}
-		if (dchild->d_inode == NULL) {
-			l_dput(dchild);
-			rc = -ENOENT;
-			CERROR("%s: nonexistent llog #"DOSTID"#%08x:"
-			       "rc = %d\n", ctxt->loc_obd->obd_name,
-			       POSTID(&logid->lgl_oi), logid->lgl_ogen, rc);
-			GOTO(out, rc);
-		}
-		handle->lgh_file = l_dentry_open(&obd->obd_lvfs_ctxt, dchild,
-						 O_RDWR | O_LARGEFILE);
-		l_dput(dchild);
-		if (IS_ERR(handle->lgh_file)) {
-			rc = PTR_ERR(handle->lgh_file);
-			handle->lgh_file = NULL;
-			CERROR("%s: error opening llog #"DOSTID"#%08x:"
-			       "rc = %d\n", ctxt->loc_obd->obd_name,
-			       POSTID(&logid->lgl_oi), logid->lgl_ogen, rc);
-			GOTO(out, rc);
-		}
-		handle->lgh_id = *logid;
-	} else if (name) {
-		handle->lgh_file = llog_filp_open(MOUNT_CONFIGS_DIR, name,
-						  O_RDWR | O_LARGEFILE, 0644);
-		if (IS_ERR(handle->lgh_file)) {
-			rc = PTR_ERR(handle->lgh_file);
-			handle->lgh_file = NULL;
-			if (rc == -ENOENT && open_param == LLOG_OPEN_NEW) {
-				OBD_ALLOC(handle->lgh_name, strlen(name) + 1);
-				if (handle->lgh_name)
-					strcpy(handle->lgh_name, name);
-				else
-					GOTO(out, rc = -ENOMEM);
-				rc = 0;
-			} else {
-				GOTO(out, rc);
-			}
-		} else {
-			lustre_build_llog_lvfs_oid(&handle->lgh_id,
-			    handle->lgh_file->f_dentry->d_inode->i_ino,
-			    handle->lgh_file->f_dentry->d_inode->i_generation);
-		}
-	} else {
-		LASSERTF(open_param == LLOG_OPEN_NEW, "%#x\n", open_param);
-		handle->lgh_file = NULL;
-	}
-
-	/* No new llog is expected but doesn't exist */
-	if (open_param != LLOG_OPEN_NEW && handle->lgh_file == NULL)
-		GOTO(out_name, rc = -ENOENT);
-
-	return 0;
-out_name:
-	if (handle->lgh_name != NULL)
-		OBD_FREE(handle->lgh_name, strlen(name) + 1);
-out:
-	return rc;
-}
-
-static int llog_lvfs_exist(struct llog_handle *handle)
-{
-	return (handle->lgh_file != NULL);
-}
-
-/* This is a callback from the llog_* functions.
- * Assumes caller has already pushed us into the kernel context. */
-static int llog_lvfs_create(const struct lu_env *env,
-			    struct llog_handle *handle,
-			    struct thandle *th)
-{
-	struct llog_ctxt	*ctxt = handle->lgh_ctxt;
-	struct obd_device	*obd;
-	struct dentry		*dchild = NULL;
-	struct file		*file;
-	struct obdo		*oa = NULL;
-	int			 rc = 0;
-	int			 open_flags = O_RDWR | O_CREAT | O_LARGEFILE;
-
-	LASSERT(ctxt);
-	LASSERT(ctxt->loc_exp);
-	obd = ctxt->loc_exp->exp_obd;
-	LASSERT(handle->lgh_file == NULL);
-
-	if (handle->lgh_name) {
-		file = llog_filp_open(MOUNT_CONFIGS_DIR, handle->lgh_name,
-				      open_flags, 0644);
-		if (IS_ERR(file))
-			return PTR_ERR(file);
-
-		lustre_build_llog_lvfs_oid(&handle->lgh_id,
-				file->f_dentry->d_inode->i_ino,
-				file->f_dentry->d_inode->i_generation);
-		handle->lgh_file = file;
-	} else {
-		OBDO_ALLOC(oa);
-		if (oa == NULL)
-			return -ENOMEM;
-
-		ostid_set_seq_llog(&oa->o_oi);
-		oa->o_valid = OBD_MD_FLGENER | OBD_MD_FLGROUP;
-
-		rc = obd_create(NULL, ctxt->loc_exp, oa, NULL, NULL);
-		if (rc)
-			GOTO(out, rc);
-
-		/* FIXME: rationalize the misuse of o_generation in
-		 *	this API along with mds_obd_{create,destroy}.
-		 *	Hopefully it is only an internal API issue. */
-#define o_generation o_parent_oid
-		dchild = obd_lvfs_fid2dentry(ctxt->loc_exp, &oa->o_oi,
-					     oa->o_generation);
-		if (IS_ERR(dchild))
-			GOTO(out, rc = PTR_ERR(dchild));
-
-		file = l_dentry_open(&obd->obd_lvfs_ctxt, dchild, open_flags);
-		l_dput(dchild);
-		if (IS_ERR(file))
-			GOTO(out, rc = PTR_ERR(file));
-		handle->lgh_id.lgl_oi = oa->o_oi;
-		handle->lgh_id.lgl_ogen = oa->o_generation;
-		handle->lgh_file = file;
-out:
-		OBDO_FREE(oa);
-	}
-	return rc;
-}
-
-static int llog_lvfs_close(const struct lu_env *env,
-			   struct llog_handle *handle)
-{
-	int rc;
-
-	if (handle->lgh_file == NULL)
-		return 0;
-	rc = filp_close(handle->lgh_file, 0);
-	if (rc)
-		CERROR("%s: error closing llog #"DOSTID"#%08x: "
-		       "rc = %d\n", handle->lgh_ctxt->loc_obd->obd_name,
-		       POSTID(&handle->lgh_id.lgl_oi),
-		       handle->lgh_id.lgl_ogen, rc);
-	handle->lgh_file = NULL;
-	if (handle->lgh_name) {
-		OBD_FREE(handle->lgh_name, strlen(handle->lgh_name) + 1);
-		handle->lgh_name = NULL;
-	}
-	return rc;
-}
-
-static int llog_lvfs_destroy(const struct lu_env *env,
-			     struct llog_handle *handle)
-{
-	struct dentry *fdentry;
-	struct obdo *oa;
-	struct obd_device *obd = handle->lgh_ctxt->loc_exp->exp_obd;
-	char *dir;
-	void *th;
-	struct inode *inode;
-	int rc, rc1;
-
-	dir = MOUNT_CONFIGS_DIR;
-
-	LASSERT(handle->lgh_file);
-	fdentry = handle->lgh_file->f_dentry;
-	inode = fdentry->d_parent->d_inode;
-	if (strcmp(fdentry->d_parent->d_name.name, dir) == 0) {
-		struct lvfs_run_ctxt saved;
-		struct vfsmount *mnt = mntget(handle->lgh_file->f_vfsmnt);
-
-		push_ctxt(&saved, &obd->obd_lvfs_ctxt, NULL);
-		dget(fdentry);
-		rc = llog_lvfs_close(env, handle);
-		if (rc == 0) {
-			mutex_lock_nested(&inode->i_mutex, I_MUTEX_PARENT);
-			rc = ll_vfs_unlink(inode, fdentry, mnt);
-			mutex_unlock(&inode->i_mutex);
-		}
-		mntput(mnt);
-
-		dput(fdentry);
-		pop_ctxt(&saved, &obd->obd_lvfs_ctxt, NULL);
-		return rc;
-	}
-
-	OBDO_ALLOC(oa);
-	if (oa == NULL)
-		return -ENOMEM;
-
-	oa->o_oi = handle->lgh_id.lgl_oi;
-	oa->o_generation = handle->lgh_id.lgl_ogen;
-#undef o_generation
-	oa->o_valid = OBD_MD_FLID | OBD_MD_FLGROUP | OBD_MD_FLGENER;
-
-	rc = llog_lvfs_close(env, handle);
-	if (rc)
-		GOTO(out, rc);
-
-	th = fsfilt_start_log(obd, inode, FSFILT_OP_UNLINK, NULL, 1);
-	if (IS_ERR(th)) {
-		CERROR("fsfilt_start failed: %ld\n", PTR_ERR(th));
-		GOTO(out, rc = PTR_ERR(th));
-	}
-
-	rc = obd_destroy(NULL, handle->lgh_ctxt->loc_exp, oa,
-			 NULL, NULL, NULL, NULL);
-
-	rc1 = fsfilt_commit(obd, inode, th, 0);
-	if (rc == 0 && rc1 != 0)
-		rc = rc1;
- out:
-	OBDO_FREE(oa);
-	return rc;
-}
-
-static int llog_lvfs_declare_create(const struct lu_env *env,
-				    struct llog_handle *res,
-				    struct thandle *th)
-{
-	return 0;
-}
-
-static int llog_lvfs_declare_write_rec(const struct lu_env *env,
-				       struct llog_handle *loghandle,
-				       struct llog_rec_hdr *rec,
-				       int idx, struct thandle *th)
-{
-	return 0;
-}
-
-struct llog_operations llog_lvfs_ops = {
-	.lop_write_rec		= llog_lvfs_write_rec,
-	.lop_next_block		= llog_lvfs_next_block,
-	.lop_prev_block		= llog_lvfs_prev_block,
-	.lop_read_header	= llog_lvfs_read_header,
-	.lop_create		= llog_lvfs_create,
-	.lop_destroy		= llog_lvfs_destroy,
-	.lop_close		= llog_lvfs_close,
-	.lop_open		= llog_lvfs_open,
-	.lop_exist		= llog_lvfs_exist,
-	.lop_declare_create	= llog_lvfs_declare_create,
-	.lop_declare_write_rec	= llog_lvfs_declare_write_rec,
-};
-EXPORT_SYMBOL(llog_lvfs_ops);
-#else /* !__KERNEL__ */
-struct llog_operations llog_lvfs_ops = {};
-#endif
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_obd.c b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
index 8ff01d3f90b4..da769db0af77 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_obd.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_obd.c
@@ -242,31 +242,6 @@ int llog_cancel(const struct lu_env *env, struct llog_ctxt *ctxt,
 }
 EXPORT_SYMBOL(llog_cancel);
 
-int obd_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-		  struct obd_device *disk_obd, int *index)
-{
-	int rc;
-
-	OBD_CHECK_DT_OP(obd, llog_init, 0);
-	OBD_COUNTER_INCREMENT(obd, llog_init);
-
-	rc = OBP(obd, llog_init)(obd, olg, disk_obd, index);
-	return rc;
-}
-EXPORT_SYMBOL(obd_llog_init);
-
-int obd_llog_finish(struct obd_device *obd, int count)
-{
-	int rc;
-
-	OBD_CHECK_DT_OP(obd, llog_finish, 0);
-	OBD_COUNTER_INCREMENT(obd, llog_finish);
-
-	rc = OBP(obd, llog_finish)(obd, count);
-	return rc;
-}
-EXPORT_SYMBOL(obd_llog_finish);
-
 /* context key constructor/destructor: llog_key_init, llog_key_fini */
 LU_KEY_INIT_FINI(llog, struct llog_thread_info);
 /* context key: llog_thread_key */
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_osd.c b/drivers/staging/lustre/lustre/obdclass/llog_osd.c
deleted file mode 100644
index 2c6a51e90697..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/llog_osd.c
+++ /dev/null
@@ -1,1290 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2009, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/obdclass/llog_osd.c - low level llog routines on top of OSD API
- *
- * Author: Alexey Zhuravlev <alexey.zhuravlev@intel.com>
- * Author: Mikhail Pershin <mike.pershin@intel.com>
- */
-
-#define DEBUG_SUBSYSTEM S_LOG
-
-#include "../include/obd.h"
-#include "../include/obd_class.h"
-#include "../include/lustre_fid.h"
-#include "../include/dt_object.h"
-
-#include "llog_internal.h"
-#include "local_storage.h"
-
-/*
- * - multi-chunks or big-declaration approach
- * - use unique sequence instead of llog sb tracking unique ids
- * - re-use existing environment
- * - named llog support (can be used for testing only at the present)
- * - llog_origin_connect() work with OSD API
- */
-
-static int llog_osd_declare_new_object(const struct lu_env *env,
-				       struct local_oid_storage *los,
-				       struct dt_object *o,
-				       struct thandle *th)
-{
-	struct llog_thread_info *lgi = llog_info(env);
-
-	lgi->lgi_attr.la_valid = LA_MODE;
-	lgi->lgi_attr.la_mode = S_IFREG | S_IRUGO | S_IWUSR;
-	lgi->lgi_dof.dof_type = dt_mode_to_dft(S_IFREG);
-
-	return local_object_declare_create(env, los, o, &lgi->lgi_attr,
-					   &lgi->lgi_dof, th);
-}
-
-static int llog_osd_create_new_object(const struct lu_env *env,
-				      struct local_oid_storage *los,
-				      struct dt_object *o,
-				      struct thandle *th)
-{
-	struct llog_thread_info *lgi = llog_info(env);
-
-	lgi->lgi_attr.la_valid = LA_MODE;
-	lgi->lgi_attr.la_mode = S_IFREG | S_IRUGO | S_IWUSR;
-	lgi->lgi_dof.dof_type = dt_mode_to_dft(S_IFREG);
-
-	return local_object_create(env, los, o, &lgi->lgi_attr,
-				   &lgi->lgi_dof, th);
-}
-
-static int llog_osd_pad(const struct lu_env *env, struct dt_object *o,
-			loff_t *off, int len, int index, struct thandle *th)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	int			 rc;
-
-	LASSERT(th);
-	LASSERT(off);
-	LASSERT(len >= LLOG_MIN_REC_SIZE && (len & 0x7) == 0);
-
-	lgi->lgi_tail.lrt_len = lgi->lgi_lrh.lrh_len = len;
-	lgi->lgi_tail.lrt_index = lgi->lgi_lrh.lrh_index = index;
-	lgi->lgi_lrh.lrh_type = LLOG_PAD_MAGIC;
-
-	lgi->lgi_buf.lb_buf = &lgi->lgi_lrh;
-	lgi->lgi_buf.lb_len = sizeof(lgi->lgi_lrh);
-	dt_write_lock(env, o, 0);
-	rc = dt_record_write(env, o, &lgi->lgi_buf, off, th);
-	if (rc) {
-		CERROR("%s: error writing padding record: rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name, rc);
-		GOTO(out, rc);
-	}
-
-	lgi->lgi_buf.lb_buf = &lgi->lgi_tail;
-	lgi->lgi_buf.lb_len = sizeof(lgi->lgi_tail);
-	*off += len - sizeof(lgi->lgi_lrh) - sizeof(lgi->lgi_tail);
-	rc = dt_record_write(env, o, &lgi->lgi_buf, off, th);
-	if (rc)
-		CERROR("%s: error writing padding record: rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name, rc);
-out:
-	dt_write_unlock(env, o);
-	return rc;
-}
-
-static int llog_osd_write_blob(const struct lu_env *env, struct dt_object *o,
-			       struct llog_rec_hdr *rec, void *buf,
-			       loff_t *off, struct thandle *th)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	int			 buflen = rec->lrh_len;
-	int			 rc;
-
-	LASSERT(env);
-	LASSERT(o);
-
-	if (buflen == 0)
-		CWARN("0-length record\n");
-
-	CDEBUG(D_OTHER, "write blob with type %x, buf %p/%u at off %llu\n",
-	       rec->lrh_type, buf, buflen, *off);
-
-	lgi->lgi_attr.la_valid = LA_SIZE;
-	lgi->lgi_attr.la_size = *off;
-
-	if (!buf) {
-		lgi->lgi_buf.lb_len = buflen;
-		lgi->lgi_buf.lb_buf = rec;
-		rc = dt_record_write(env, o, &lgi->lgi_buf, off, th);
-		if (rc)
-			CERROR("%s: error writing log record: rc = %d\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name, rc);
-		GOTO(out, rc);
-	}
-
-	/* the buf case */
-	/* protect the following 3 writes from concurrent read */
-	dt_write_lock(env, o, 0);
-	rec->lrh_len = sizeof(*rec) + buflen + sizeof(lgi->lgi_tail);
-	lgi->lgi_buf.lb_len = sizeof(*rec);
-	lgi->lgi_buf.lb_buf = rec;
-	rc = dt_record_write(env, o, &lgi->lgi_buf, off, th);
-	if (rc) {
-		CERROR("%s: error writing log hdr: rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name, rc);
-		GOTO(out_unlock, rc);
-	}
-
-	lgi->lgi_buf.lb_len = buflen;
-	lgi->lgi_buf.lb_buf = buf;
-	rc = dt_record_write(env, o, &lgi->lgi_buf, off, th);
-	if (rc) {
-		CERROR("%s: error writing log buffer: rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name,  rc);
-		GOTO(out_unlock, rc);
-	}
-
-	lgi->lgi_tail.lrt_len = rec->lrh_len;
-	lgi->lgi_tail.lrt_index = rec->lrh_index;
-	lgi->lgi_buf.lb_len = sizeof(lgi->lgi_tail);
-	lgi->lgi_buf.lb_buf = &lgi->lgi_tail;
-	rc = dt_record_write(env, o, &lgi->lgi_buf, off, th);
-	if (rc)
-		CERROR("%s: error writing log tail: rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name, rc);
-
-out_unlock:
-	dt_write_unlock(env, o);
-
-out:
-	/* cleanup the content written above */
-	if (rc) {
-		dt_punch(env, o, lgi->lgi_attr.la_size, OBD_OBJECT_EOF, th,
-			 BYPASS_CAPA);
-		dt_attr_set(env, o, &lgi->lgi_attr, th, BYPASS_CAPA);
-	}
-
-	return rc;
-}
-
-static int llog_osd_read_header(const struct lu_env *env,
-				struct llog_handle *handle)
-{
-	struct llog_rec_hdr	*llh_hdr;
-	struct dt_object	*o;
-	struct llog_thread_info	*lgi;
-	int			 rc;
-
-	LASSERT(sizeof(*handle->lgh_hdr) == LLOG_CHUNK_SIZE);
-
-	o = handle->lgh_obj;
-	LASSERT(o);
-
-	lgi = llog_info(env);
-
-	rc = dt_attr_get(env, o, &lgi->lgi_attr, NULL);
-	if (rc)
-		return rc;
-
-	LASSERT(lgi->lgi_attr.la_valid & LA_SIZE);
-
-	if (lgi->lgi_attr.la_size == 0) {
-		CDEBUG(D_HA, "not reading header from 0-byte log\n");
-		return LLOG_EEMPTY;
-	}
-
-	lgi->lgi_off = 0;
-	lgi->lgi_buf.lb_buf = handle->lgh_hdr;
-	lgi->lgi_buf.lb_len = LLOG_CHUNK_SIZE;
-
-	rc = dt_record_read(env, o, &lgi->lgi_buf, &lgi->lgi_off);
-	if (rc) {
-		CERROR("%s: error reading log header from "DFID": rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name,
-		       PFID(lu_object_fid(&o->do_lu)), rc);
-		return rc;
-	}
-
-	llh_hdr = &handle->lgh_hdr->llh_hdr;
-	if (LLOG_REC_HDR_NEEDS_SWABBING(llh_hdr))
-		lustre_swab_llog_hdr(handle->lgh_hdr);
-
-	if (llh_hdr->lrh_type != LLOG_HDR_MAGIC) {
-		CERROR("%s: bad log %s "DFID" header magic: %#x "
-		       "(expected %#x)\n", o->do_lu.lo_dev->ld_obd->obd_name,
-		       handle->lgh_name ? handle->lgh_name : "",
-		       PFID(lu_object_fid(&o->do_lu)),
-		       llh_hdr->lrh_type, LLOG_HDR_MAGIC);
-		return -EIO;
-	} else if (llh_hdr->lrh_len != LLOG_CHUNK_SIZE) {
-		CERROR("%s: incorrectly sized log %s "DFID" header: "
-		       "%#x (expected %#x)\n"
-		       "you may need to re-run lconf --write_conf.\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name,
-		       handle->lgh_name ? handle->lgh_name : "",
-		       PFID(lu_object_fid(&o->do_lu)),
-		       llh_hdr->lrh_len, LLOG_CHUNK_SIZE);
-		return -EIO;
-	}
-
-	handle->lgh_last_idx = handle->lgh_hdr->llh_tail.lrt_index;
-
-	return 0;
-}
-
-static int llog_osd_declare_write_rec(const struct lu_env *env,
-				      struct llog_handle *loghandle,
-				      struct llog_rec_hdr *rec,
-				      int idx, struct thandle *th)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	struct dt_object	*o;
-	int			 rc;
-
-	LASSERT(env);
-	LASSERT(th);
-	LASSERT(loghandle);
-
-	o = loghandle->lgh_obj;
-	LASSERT(o);
-
-	/* each time we update header */
-	rc = dt_declare_record_write(env, o, sizeof(struct llog_log_hdr), 0,
-				     th);
-	if (rc || idx == 0) /* if error or just header */
-		return rc;
-
-	if (dt_object_exists(o)) {
-		rc = dt_attr_get(env, o, &lgi->lgi_attr, BYPASS_CAPA);
-		lgi->lgi_off = lgi->lgi_attr.la_size;
-		LASSERT(ergo(rc == 0, lgi->lgi_attr.la_valid & LA_SIZE));
-		if (rc)
-			return rc;
-
-		rc = dt_declare_punch(env, o, lgi->lgi_off, OBD_OBJECT_EOF, th);
-		if (rc)
-			return rc;
-	} else {
-		lgi->lgi_off = 0;
-	}
-
-	/* XXX: implement declared window or multi-chunks approach */
-	rc = dt_declare_record_write(env, o, 32 * 1024, lgi->lgi_off, th);
-
-	return rc;
-}
-
-/* returns negative in on error; 0 if success && reccookie == 0; 1 otherwise */
-/* appends if idx == -1, otherwise overwrites record idx. */
-static int llog_osd_write_rec(const struct lu_env *env,
-			      struct llog_handle *loghandle,
-			      struct llog_rec_hdr *rec,
-			      struct llog_cookie *reccookie, int cookiecount,
-			      void *buf, int idx, struct thandle *th)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	struct llog_log_hdr	*llh;
-	int			 reclen = rec->lrh_len;
-	int			 index, rc, old_tail_idx;
-	struct llog_rec_tail	*lrt;
-	struct dt_object	*o;
-	size_t			 left;
-
-	LASSERT(env);
-	llh = loghandle->lgh_hdr;
-	LASSERT(llh);
-	o = loghandle->lgh_obj;
-	LASSERT(o);
-	LASSERT(th);
-
-	CDEBUG(D_OTHER, "new record %x to "DFID"\n",
-	       rec->lrh_type, PFID(lu_object_fid(&o->do_lu)));
-
-	/* record length should not bigger than LLOG_CHUNK_SIZE */
-	if (buf)
-		rc = (reclen > LLOG_CHUNK_SIZE - sizeof(struct llog_rec_hdr) -
-		      sizeof(struct llog_rec_tail)) ? -E2BIG : 0;
-	else
-		rc = (reclen > LLOG_CHUNK_SIZE) ? -E2BIG : 0;
-	if (rc)
-		return rc;
-
-	rc = dt_attr_get(env, o, &lgi->lgi_attr, NULL);
-	if (rc)
-		return rc;
-
-	if (buf)
-		/* write_blob adds header and tail to lrh_len. */
-		reclen = sizeof(*rec) + rec->lrh_len +
-			 sizeof(struct llog_rec_tail);
-
-	if (idx != -1) {
-		/* no header: only allowed to insert record 1 */
-		if (idx != 1 && lgi->lgi_attr.la_size == 0)
-			LBUG();
-
-		if (idx && llh->llh_size && llh->llh_size != rec->lrh_len)
-			return -EINVAL;
-
-		if (!ext2_test_bit(idx, llh->llh_bitmap))
-			CERROR("%s: modify unset record %u\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name, idx);
-		if (idx != rec->lrh_index)
-			CERROR("%s: index mismatch %d %u\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name, idx,
-			       rec->lrh_index);
-
-		lgi->lgi_off = 0;
-		rc = llog_osd_write_blob(env, o, &llh->llh_hdr, NULL,
-					 &lgi->lgi_off, th);
-		/* we are done if we only write the header or on error */
-		if (rc || idx == 0)
-			return rc;
-
-		if (buf) {
-			/* We assume that caller has set lgh_cur_* */
-			lgi->lgi_off = loghandle->lgh_cur_offset;
-			CDEBUG(D_OTHER,
-			       "modify record "DOSTID": idx:%d/%u/%d, len:%u "
-			       "offset %llu\n",
-			       POSTID(&loghandle->lgh_id.lgl_oi), idx,
-			       rec->lrh_index,
-			       loghandle->lgh_cur_idx, rec->lrh_len,
-			       (long long)(lgi->lgi_off - sizeof(*llh)));
-			if (rec->lrh_index != loghandle->lgh_cur_idx) {
-				CERROR("%s: modify idx mismatch %u/%d\n",
-				       o->do_lu.lo_dev->ld_obd->obd_name, idx,
-				       loghandle->lgh_cur_idx);
-				return -EFAULT;
-			}
-		} else {
-			/* Assumes constant lrh_len */
-			lgi->lgi_off = sizeof(*llh) + (idx - 1) * reclen;
-		}
-
-		rc = llog_osd_write_blob(env, o, rec, buf, &lgi->lgi_off, th);
-		if (rc == 0 && reccookie) {
-			reccookie->lgc_lgl = loghandle->lgh_id;
-			reccookie->lgc_index = idx;
-			rc = 1;
-		}
-		return rc;
-	}
-
-	/* Make sure that records don't cross a chunk boundary, so we can
-	 * process them page-at-a-time if needed.  If it will cross a chunk
-	 * boundary, write in a fake (but referenced) entry to pad the chunk.
-	 *
-	 * We know that llog_current_log() will return a loghandle that is
-	 * big enough to hold reclen, so all we care about is padding here.
-	 */
-	LASSERT(lgi->lgi_attr.la_valid & LA_SIZE);
-	lgi->lgi_off = lgi->lgi_attr.la_size;
-	left = LLOG_CHUNK_SIZE - (lgi->lgi_off & (LLOG_CHUNK_SIZE - 1));
-	/* NOTE: padding is a record, but no bit is set */
-	if (left != 0 && left != reclen &&
-	    left < (reclen + LLOG_MIN_REC_SIZE)) {
-		index = loghandle->lgh_last_idx + 1;
-		rc = llog_osd_pad(env, o, &lgi->lgi_off, left, index, th);
-		if (rc)
-			return rc;
-		loghandle->lgh_last_idx++; /*for pad rec*/
-	}
-	/* if it's the last idx in log file, then return -ENOSPC */
-	if (loghandle->lgh_last_idx >= LLOG_BITMAP_SIZE(llh) - 1)
-		return -ENOSPC;
-
-	loghandle->lgh_last_idx++;
-	index = loghandle->lgh_last_idx;
-	LASSERT(index < LLOG_BITMAP_SIZE(llh));
-	rec->lrh_index = index;
-	if (buf == NULL) {
-		lrt = (struct llog_rec_tail *)((char *)rec + rec->lrh_len -
-					       sizeof(*lrt));
-		lrt->lrt_len = rec->lrh_len;
-		lrt->lrt_index = rec->lrh_index;
-	}
-	/* The caller should make sure only 1 process access the lgh_last_idx,
-	 * Otherwise it might hit the assert.*/
-	LASSERT(index < LLOG_BITMAP_SIZE(llh));
-	spin_lock(&loghandle->lgh_hdr_lock);
-	if (ext2_set_bit(index, llh->llh_bitmap)) {
-		CERROR("%s: index %u already set in log bitmap\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name, index);
-		spin_unlock(&loghandle->lgh_hdr_lock);
-		LBUG(); /* should never happen */
-	}
-	llh->llh_count++;
-	spin_unlock(&loghandle->lgh_hdr_lock);
-	old_tail_idx = llh->llh_tail.lrt_index;
-	llh->llh_tail.lrt_index = index;
-
-	lgi->lgi_off = 0;
-	rc = llog_osd_write_blob(env, o, &llh->llh_hdr, NULL, &lgi->lgi_off,
-				 th);
-	if (rc)
-		GOTO(out, rc);
-
-	rc = dt_attr_get(env, o, &lgi->lgi_attr, NULL);
-	if (rc)
-		GOTO(out, rc);
-
-	LASSERT(lgi->lgi_attr.la_valid & LA_SIZE);
-	lgi->lgi_off = lgi->lgi_attr.la_size;
-
-	rc = llog_osd_write_blob(env, o, rec, buf, &lgi->lgi_off, th);
-
-out:
-	/* cleanup llog for error case */
-	if (rc) {
-		spin_lock(&loghandle->lgh_hdr_lock);
-		ext2_clear_bit(index, llh->llh_bitmap);
-		llh->llh_count--;
-		spin_unlock(&loghandle->lgh_hdr_lock);
-
-		/* restore the header */
-		loghandle->lgh_last_idx--;
-		llh->llh_tail.lrt_index = old_tail_idx;
-		lgi->lgi_off = 0;
-		llog_osd_write_blob(env, o, &llh->llh_hdr, NULL,
-				    &lgi->lgi_off, th);
-	}
-
-	CDEBUG(D_RPCTRACE, "added record "DOSTID": idx: %u, %u\n",
-	       POSTID(&loghandle->lgh_id.lgl_oi), index, rec->lrh_len);
-	if (rc == 0 && reccookie) {
-		reccookie->lgc_lgl = loghandle->lgh_id;
-		reccookie->lgc_index = index;
-		if ((rec->lrh_type == MDS_UNLINK_REC) ||
-		    (rec->lrh_type == MDS_SETATTR64_REC))
-			reccookie->lgc_subsys = LLOG_MDS_OST_ORIG_CTXT;
-		else if (rec->lrh_type == OST_SZ_REC)
-			reccookie->lgc_subsys = LLOG_SIZE_ORIG_CTXT;
-		else
-			reccookie->lgc_subsys = -1;
-		rc = 1;
-	}
-	return rc;
-}
-
-/* We can skip reading at least as many log blocks as the number of
- * minimum sized log records we are skipping.  If it turns out
- * that we are not far enough along the log (because the
- * actual records are larger than minimum size) we just skip
- * some more records.
- */
-static void llog_skip_over(__u64 *off, int curr, int goal)
-{
-	if (goal <= curr)
-		return;
-	*off = (*off + (goal - curr - 1) * LLOG_MIN_REC_SIZE) &
-		~(LLOG_CHUNK_SIZE - 1);
-}
-
-/* sets:
- *  - cur_offset to the furthest point read in the log file
- *  - cur_idx to the log index preceding cur_offset
- * returns -EIO/-EINVAL on error
- */
-static int llog_osd_next_block(const struct lu_env *env,
-			       struct llog_handle *loghandle, int *cur_idx,
-			       int next_idx, __u64 *cur_offset, void *buf,
-			       int len)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	struct dt_object	*o;
-	struct dt_device	*dt;
-	int			 rc;
-
-	LASSERT(env);
-	LASSERT(lgi);
-
-	if (len == 0 || len & (LLOG_CHUNK_SIZE - 1))
-		return -EINVAL;
-
-	CDEBUG(D_OTHER, "looking for log index %u (cur idx %u off %llu)\n",
-	       next_idx, *cur_idx, *cur_offset);
-
-	LASSERT(loghandle);
-	LASSERT(loghandle->lgh_ctxt);
-
-	o = loghandle->lgh_obj;
-	LASSERT(o);
-	LASSERT(dt_object_exists(o));
-	dt = lu2dt_dev(o->do_lu.lo_dev);
-	LASSERT(dt);
-
-	rc = dt_attr_get(env, o, &lgi->lgi_attr, BYPASS_CAPA);
-	if (rc)
-		GOTO(out, rc);
-
-	while (*cur_offset < lgi->lgi_attr.la_size) {
-		struct llog_rec_hdr	*rec, *last_rec;
-		struct llog_rec_tail	*tail;
-
-		llog_skip_over(cur_offset, *cur_idx, next_idx);
-
-		/* read up to next LLOG_CHUNK_SIZE block */
-		lgi->lgi_buf.lb_len = LLOG_CHUNK_SIZE -
-				      (*cur_offset & (LLOG_CHUNK_SIZE - 1));
-		lgi->lgi_buf.lb_buf = buf;
-
-		/* Note: read lock is not needed around la_size get above at
-		 * the time of dt_attr_get(). There are only two cases that
-		 * matter. Either la_size == cur_offset, in which case the
-		 * entire read is skipped, or la_size > cur_offset and the loop
-		 * is entered and this thread is blocked at dt_read_lock()
-		 * until the write is completed. When the write completes, then
-		 * the dt_read() will be done with the full length, and will
-		 * get the full data.
-		 */
-		dt_read_lock(env, o, 0);
-		rc = dt_read(env, o, &lgi->lgi_buf, cur_offset);
-		dt_read_unlock(env, o);
-		if (rc < 0) {
-			CERROR("%s: can't read llog block from log "DFID
-			       " offset %llu: rc = %d\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       PFID(lu_object_fid(&o->do_lu)), *cur_offset,
-			       rc);
-			GOTO(out, rc);
-		}
-
-		if (rc < len) {
-			/* signal the end of the valid buffer to
-			 * llog_process */
-			memset(buf + rc, 0, len - rc);
-		}
-
-		if (rc == 0) /* end of file, nothing to do */
-			GOTO(out, rc);
-
-		if (rc < sizeof(*tail)) {
-			CERROR("%s: invalid llog block at log id "DOSTID"/%u "
-			       "offset %llu\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, *cur_offset);
-			GOTO(out, rc = -EINVAL);
-		}
-
-		rec = buf;
-		if (LLOG_REC_HDR_NEEDS_SWABBING(rec))
-			lustre_swab_llog_rec(rec);
-
-		tail = (struct llog_rec_tail *)((char *)buf + rc -
-						sizeof(struct llog_rec_tail));
-		/* get the last record in block */
-		last_rec = (struct llog_rec_hdr *)((char *)buf + rc -
-						   le32_to_cpu(tail->lrt_len));
-
-		if (LLOG_REC_HDR_NEEDS_SWABBING(last_rec))
-			lustre_swab_llog_rec(last_rec);
-		LASSERT(last_rec->lrh_index == tail->lrt_index);
-
-		*cur_idx = tail->lrt_index;
-
-		/* this shouldn't happen */
-		if (tail->lrt_index == 0) {
-			CERROR("%s: invalid llog tail at log id "DOSTID"/%u "
-			       "offset %llu\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, *cur_offset);
-			GOTO(out, rc = -EINVAL);
-		}
-		if (tail->lrt_index < next_idx)
-			continue;
-
-		/* sanity check that the start of the new buffer is no farther
-		 * than the record that we wanted.  This shouldn't happen. */
-		if (rec->lrh_index > next_idx) {
-			CERROR("%s: missed desired record? %u > %u\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       rec->lrh_index, next_idx);
-			GOTO(out, rc = -ENOENT);
-		}
-		GOTO(out, rc = 0);
-	}
-	GOTO(out, rc = -EIO);
-out:
-	return rc;
-}
-
-static int llog_osd_prev_block(const struct lu_env *env,
-			       struct llog_handle *loghandle,
-			       int prev_idx, void *buf, int len)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	struct dt_object	*o;
-	struct dt_device	*dt;
-	loff_t			 cur_offset;
-	int			 rc;
-
-	if (len == 0 || len & (LLOG_CHUNK_SIZE - 1))
-		return -EINVAL;
-
-	CDEBUG(D_OTHER, "looking for log index %u\n", prev_idx);
-
-	LASSERT(loghandle);
-	LASSERT(loghandle->lgh_ctxt);
-
-	o = loghandle->lgh_obj;
-	LASSERT(o);
-	LASSERT(dt_object_exists(o));
-	dt = lu2dt_dev(o->do_lu.lo_dev);
-	LASSERT(dt);
-
-	cur_offset = LLOG_CHUNK_SIZE;
-	llog_skip_over(&cur_offset, 0, prev_idx);
-
-	rc = dt_attr_get(env, o, &lgi->lgi_attr, BYPASS_CAPA);
-	if (rc)
-		GOTO(out, rc);
-
-	while (cur_offset < lgi->lgi_attr.la_size) {
-		struct llog_rec_hdr	*rec, *last_rec;
-		struct llog_rec_tail	*tail;
-
-		lgi->lgi_buf.lb_len = len;
-		lgi->lgi_buf.lb_buf = buf;
-		/* It is OK to have locking around dt_read() only, see
-		 * comment in llog_osd_next_block for details
-		 */
-		dt_read_lock(env, o, 0);
-		rc = dt_read(env, o, &lgi->lgi_buf, &cur_offset);
-		dt_read_unlock(env, o);
-		if (rc < 0) {
-			CERROR("%s: can't read llog block from log "DFID
-			       " offset %llu: rc = %d\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       PFID(lu_object_fid(&o->do_lu)), cur_offset, rc);
-			GOTO(out, rc);
-		}
-
-		if (rc == 0) /* end of file, nothing to do */
-			GOTO(out, rc);
-
-		if (rc < sizeof(*tail)) {
-			CERROR("%s: invalid llog block at log id "DOSTID"/%u "
-			       "offset %llu\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, cur_offset);
-			GOTO(out, rc = -EINVAL);
-		}
-
-		rec = buf;
-		if (LLOG_REC_HDR_NEEDS_SWABBING(rec))
-			lustre_swab_llog_rec(rec);
-
-		tail = (struct llog_rec_tail *)((char *)buf + rc -
-						sizeof(struct llog_rec_tail));
-		/* get the last record in block */
-		last_rec = (struct llog_rec_hdr *)((char *)buf + rc -
-						   le32_to_cpu(tail->lrt_len));
-
-		if (LLOG_REC_HDR_NEEDS_SWABBING(last_rec))
-			lustre_swab_llog_rec(last_rec);
-		LASSERT(last_rec->lrh_index == tail->lrt_index);
-
-		/* this shouldn't happen */
-		if (tail->lrt_index == 0) {
-			CERROR("%s: invalid llog tail at log id "DOSTID"/%u "
-			       "offset %llu\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       POSTID(&loghandle->lgh_id.lgl_oi),
-			       loghandle->lgh_id.lgl_ogen, cur_offset);
-			GOTO(out, rc = -EINVAL);
-		}
-		if (tail->lrt_index < prev_idx)
-			continue;
-
-		/* sanity check that the start of the new buffer is no farther
-		 * than the record that we wanted.  This shouldn't happen. */
-		if (rec->lrh_index > prev_idx) {
-			CERROR("%s: missed desired record? %u > %u\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       rec->lrh_index, prev_idx);
-			GOTO(out, rc = -ENOENT);
-		}
-		GOTO(out, rc = 0);
-	}
-	GOTO(out, rc = -EIO);
-out:
-	return rc;
-}
-
-struct dt_object *llog_osd_dir_get(const struct lu_env *env,
-				   struct llog_ctxt *ctxt)
-{
-	struct dt_device	*dt;
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*dir;
-	int			 rc;
-
-	dt = ctxt->loc_exp->exp_obd->obd_lvfs_ctxt.dt;
-	if (ctxt->loc_dir == NULL) {
-		rc = dt_root_get(env, dt, &dti->dti_fid);
-		if (rc)
-			return ERR_PTR(rc);
-		dir = dt_locate(env, dt, &dti->dti_fid);
-	} else {
-		lu_object_get(&ctxt->loc_dir->do_lu);
-		dir = ctxt->loc_dir;
-	}
-
-	return dir;
-}
-
-static int llog_osd_open(const struct lu_env *env, struct llog_handle *handle,
-			 struct llog_logid *logid, char *name,
-			 enum llog_open_param open_param)
-{
-	struct llog_thread_info		*lgi = llog_info(env);
-	struct llog_ctxt		*ctxt = handle->lgh_ctxt;
-	struct dt_object		*o;
-	struct dt_device		*dt;
-	struct ls_device		*ls;
-	struct local_oid_storage	*los;
-	int				 rc = 0;
-
-	LASSERT(env);
-	LASSERT(ctxt);
-	LASSERT(ctxt->loc_exp);
-	LASSERT(ctxt->loc_exp->exp_obd);
-	dt = ctxt->loc_exp->exp_obd->obd_lvfs_ctxt.dt;
-	LASSERT(dt);
-
-	ls = ls_device_get(dt);
-	if (IS_ERR(ls))
-		return PTR_ERR(ls);
-
-	mutex_lock(&ls->ls_los_mutex);
-	los = dt_los_find(ls, name != NULL ? FID_SEQ_LLOG_NAME : FID_SEQ_LLOG);
-	mutex_unlock(&ls->ls_los_mutex);
-	LASSERT(los);
-	ls_device_put(env, ls);
-
-	LASSERT(handle);
-
-	if (logid != NULL) {
-		logid_to_fid(logid, &lgi->lgi_fid);
-	} else if (name) {
-		struct dt_object *llog_dir;
-
-		llog_dir = llog_osd_dir_get(env, ctxt);
-		if (IS_ERR(llog_dir))
-			GOTO(out, rc = PTR_ERR(llog_dir));
-		dt_read_lock(env, llog_dir, 0);
-		rc = dt_lookup_dir(env, llog_dir, name, &lgi->lgi_fid);
-		dt_read_unlock(env, llog_dir);
-		lu_object_put(env, &llog_dir->do_lu);
-		if (rc == -ENOENT && open_param == LLOG_OPEN_NEW) {
-			/* generate fid for new llog */
-			rc = local_object_fid_generate(env, los,
-						       &lgi->lgi_fid);
-		}
-		if (rc < 0)
-			GOTO(out, rc);
-		OBD_ALLOC(handle->lgh_name, strlen(name) + 1);
-		if (handle->lgh_name)
-			strcpy(handle->lgh_name, name);
-		else
-			GOTO(out, rc = -ENOMEM);
-	} else {
-		LASSERTF(open_param & LLOG_OPEN_NEW, "%#x\n", open_param);
-		/* generate fid for new llog */
-		rc = local_object_fid_generate(env, los, &lgi->lgi_fid);
-		if (rc < 0)
-			GOTO(out, rc);
-	}
-
-	o = ls_locate(env, ls, &lgi->lgi_fid);
-	if (IS_ERR(o))
-		GOTO(out_name, rc = PTR_ERR(o));
-
-	/* No new llog is expected but doesn't exist */
-	if (open_param != LLOG_OPEN_NEW && !dt_object_exists(o))
-		GOTO(out_put, rc = -ENOENT);
-
-	fid_to_logid(&lgi->lgi_fid, &handle->lgh_id);
-	handle->lgh_obj = o;
-	handle->private_data = los;
-	LASSERT(handle->lgh_ctxt);
-
-	return rc;
-
-out_put:
-	lu_object_put(env, &o->do_lu);
-out_name:
-	if (handle->lgh_name != NULL)
-		OBD_FREE(handle->lgh_name, strlen(name) + 1);
-out:
-	dt_los_put(los);
-	return rc;
-}
-
-static int llog_osd_exist(struct llog_handle *handle)
-{
-	LASSERT(handle->lgh_obj);
-	return (dt_object_exists(handle->lgh_obj) &&
-		!lu_object_is_dying(handle->lgh_obj->do_lu.lo_header));
-}
-
-static int llog_osd_declare_create(const struct lu_env *env,
-				   struct llog_handle *res, struct thandle *th)
-{
-	struct llog_thread_info		*lgi = llog_info(env);
-	struct local_oid_storage	*los;
-	struct dt_object		*o;
-	int				 rc;
-
-	LASSERT(res->lgh_obj);
-	LASSERT(th);
-
-	/* object can be created by another thread */
-	o = res->lgh_obj;
-	if (dt_object_exists(o))
-		return 0;
-
-	los = res->private_data;
-	LASSERT(los);
-
-	rc = llog_osd_declare_new_object(env, los, o, th);
-	if (rc)
-		return rc;
-
-	rc = dt_declare_record_write(env, o, LLOG_CHUNK_SIZE, 0, th);
-	if (rc)
-		return rc;
-
-	if (res->lgh_name) {
-		struct dt_object *llog_dir;
-
-		llog_dir = llog_osd_dir_get(env, res->lgh_ctxt);
-		if (IS_ERR(llog_dir))
-			return PTR_ERR(llog_dir);
-		logid_to_fid(&res->lgh_id, &lgi->lgi_fid);
-		rc = dt_declare_insert(env, llog_dir,
-				       (struct dt_rec *)&lgi->lgi_fid,
-				       (struct dt_key *)res->lgh_name, th);
-		lu_object_put(env, &llog_dir->do_lu);
-		if (rc)
-			CERROR("%s: can't declare named llog %s: rc = %d\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       res->lgh_name, rc);
-	}
-	return rc;
-}
-
-/* This is a callback from the llog_* functions.
- * Assumes caller has already pushed us into the kernel context. */
-static int llog_osd_create(const struct lu_env *env, struct llog_handle *res,
-			   struct thandle *th)
-{
-	struct llog_thread_info *lgi = llog_info(env);
-	struct local_oid_storage *los;
-	struct dt_object	*o;
-	int		      rc = 0;
-
-	LASSERT(env);
-	o = res->lgh_obj;
-	LASSERT(o);
-
-	/* llog can be already created */
-	if (dt_object_exists(o))
-		return -EEXIST;
-
-	los = res->private_data;
-	LASSERT(los);
-
-	dt_write_lock(env, o, 0);
-	if (!dt_object_exists(o))
-		rc = llog_osd_create_new_object(env, los, o, th);
-	else
-		rc = -EEXIST;
-
-	dt_write_unlock(env, o);
-	if (rc)
-		return rc;
-
-	if (res->lgh_name) {
-		struct dt_object *llog_dir;
-
-		llog_dir = llog_osd_dir_get(env, res->lgh_ctxt);
-		if (IS_ERR(llog_dir))
-			return PTR_ERR(llog_dir);
-
-		logid_to_fid(&res->lgh_id, &lgi->lgi_fid);
-		dt_read_lock(env, llog_dir, 0);
-		rc = dt_insert(env, llog_dir,
-			       (struct dt_rec *)&lgi->lgi_fid,
-			       (struct dt_key *)res->lgh_name,
-			       th, BYPASS_CAPA, 1);
-		dt_read_unlock(env, llog_dir);
-		lu_object_put(env, &llog_dir->do_lu);
-		if (rc)
-			CERROR("%s: can't create named llog %s: rc = %d\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       res->lgh_name, rc);
-	}
-	return rc;
-}
-
-static int llog_osd_close(const struct lu_env *env, struct llog_handle *handle)
-{
-	struct local_oid_storage	*los;
-	int				 rc = 0;
-
-	LASSERT(handle->lgh_obj);
-
-	lu_object_put(env, &handle->lgh_obj->do_lu);
-
-	los = handle->private_data;
-	LASSERT(los);
-	dt_los_put(los);
-
-	if (handle->lgh_name)
-		OBD_FREE(handle->lgh_name, strlen(handle->lgh_name) + 1);
-
-	return rc;
-}
-
-static int llog_osd_destroy(const struct lu_env *env,
-			    struct llog_handle *loghandle)
-{
-	struct llog_ctxt	*ctxt;
-	struct dt_object	*o, *llog_dir = NULL;
-	struct dt_device	*d;
-	struct thandle		*th;
-	char			*name = NULL;
-	int			 rc;
-
-	ctxt = loghandle->lgh_ctxt;
-	LASSERT(ctxt);
-
-	o = loghandle->lgh_obj;
-	LASSERT(o);
-
-	d = lu2dt_dev(o->do_lu.lo_dev);
-	LASSERT(d);
-	LASSERT(d == ctxt->loc_exp->exp_obd->obd_lvfs_ctxt.dt);
-
-	th = dt_trans_create(env, d);
-	if (IS_ERR(th))
-		return PTR_ERR(th);
-
-	if (loghandle->lgh_name) {
-		llog_dir = llog_osd_dir_get(env, ctxt);
-		if (IS_ERR(llog_dir))
-			GOTO(out_trans, rc = PTR_ERR(llog_dir));
-
-		name = loghandle->lgh_name;
-		rc = dt_declare_delete(env, llog_dir,
-				       (struct dt_key *)name, th);
-		if (rc)
-			GOTO(out_trans, rc);
-	}
-
-	dt_declare_ref_del(env, o, th);
-
-	rc = dt_declare_destroy(env, o, th);
-	if (rc)
-		GOTO(out_trans, rc);
-
-	rc = dt_trans_start_local(env, d, th);
-	if (rc)
-		GOTO(out_trans, rc);
-
-	dt_write_lock(env, o, 0);
-	if (dt_object_exists(o)) {
-		if (name) {
-			dt_read_lock(env, llog_dir, 0);
-			rc = dt_delete(env, llog_dir,
-				       (struct dt_key *) name,
-				       th, BYPASS_CAPA);
-			dt_read_unlock(env, llog_dir);
-			if (rc) {
-				CERROR("%s: can't remove llog %s: rc = %d\n",
-				       o->do_lu.lo_dev->ld_obd->obd_name,
-				       name, rc);
-				GOTO(out_unlock, rc);
-			}
-		}
-		dt_ref_del(env, o, th);
-		rc = dt_destroy(env, o, th);
-		if (rc)
-			GOTO(out_unlock, rc);
-	}
-out_unlock:
-	dt_write_unlock(env, o);
-out_trans:
-	dt_trans_stop(env, d, th);
-	if (llog_dir != NULL)
-		lu_object_put(env, &llog_dir->do_lu);
-	return rc;
-}
-
-static int llog_osd_setup(const struct lu_env *env, struct obd_device *obd,
-			  struct obd_llog_group *olg, int ctxt_idx,
-			  struct obd_device *disk_obd)
-{
-	struct local_oid_storage	*los;
-	struct llog_thread_info		*lgi = llog_info(env);
-	struct llog_ctxt		*ctxt;
-	int				 rc = 0;
-
-	LASSERT(obd);
-	LASSERT(olg->olg_ctxts[ctxt_idx]);
-
-	ctxt = llog_ctxt_get(olg->olg_ctxts[ctxt_idx]);
-	LASSERT(ctxt);
-
-	/* initialize data allowing to generate new fids,
-	 * literally we need a sequence */
-	lgi->lgi_fid.f_seq = FID_SEQ_LLOG;
-	lgi->lgi_fid.f_oid = 1;
-	lgi->lgi_fid.f_ver = 0;
-	rc = local_oid_storage_init(env, disk_obd->obd_lvfs_ctxt.dt,
-				    &lgi->lgi_fid, &los);
-	if (rc < 0)
-		return rc;
-
-	lgi->lgi_fid.f_seq = FID_SEQ_LLOG_NAME;
-	lgi->lgi_fid.f_oid = 1;
-	lgi->lgi_fid.f_ver = 0;
-	rc = local_oid_storage_init(env, disk_obd->obd_lvfs_ctxt.dt,
-				    &lgi->lgi_fid, &los);
-	llog_ctxt_put(ctxt);
-	return rc;
-}
-
-static int llog_osd_cleanup(const struct lu_env *env, struct llog_ctxt *ctxt)
-{
-	struct dt_device		*dt;
-	struct ls_device		*ls;
-	struct local_oid_storage	*los, *nlos;
-
-	LASSERT(ctxt->loc_exp->exp_obd);
-	dt = ctxt->loc_exp->exp_obd->obd_lvfs_ctxt.dt;
-	ls = ls_device_get(dt);
-	if (IS_ERR(ls))
-		return PTR_ERR(ls);
-
-	mutex_lock(&ls->ls_los_mutex);
-	los = dt_los_find(ls, FID_SEQ_LLOG);
-	nlos = dt_los_find(ls, FID_SEQ_LLOG_NAME);
-	mutex_unlock(&ls->ls_los_mutex);
-	if (los != NULL) {
-		dt_los_put(los);
-		local_oid_storage_fini(env, los);
-	}
-	if (nlos != NULL) {
-		dt_los_put(nlos);
-		local_oid_storage_fini(env, nlos);
-	}
-	ls_device_put(env, ls);
-	return 0;
-}
-
-struct llog_operations llog_osd_ops = {
-	.lop_next_block		= llog_osd_next_block,
-	.lop_prev_block		= llog_osd_prev_block,
-	.lop_read_header	= llog_osd_read_header,
-	.lop_destroy		= llog_osd_destroy,
-	.lop_setup		= llog_osd_setup,
-	.lop_cleanup		= llog_osd_cleanup,
-	.lop_open		= llog_osd_open,
-	.lop_exist		= llog_osd_exist,
-	.lop_declare_create	= llog_osd_declare_create,
-	.lop_create		= llog_osd_create,
-	.lop_declare_write_rec	= llog_osd_declare_write_rec,
-	.lop_write_rec		= llog_osd_write_rec,
-	.lop_close		= llog_osd_close,
-};
-EXPORT_SYMBOL(llog_osd_ops);
-
-/* reads the catalog list */
-int llog_osd_get_cat_list(const struct lu_env *env, struct dt_device *d,
-			  int idx, int count, struct llog_catid *idarray)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	struct dt_object	*o = NULL;
-	struct thandle		*th;
-	int			 rc, size;
-
-	LASSERT(d);
-
-	size = sizeof(*idarray) * count;
-	lgi->lgi_off = idx *  sizeof(*idarray);
-
-	lu_local_obj_fid(&lgi->lgi_fid, LLOG_CATALOGS_OID);
-
-	o = dt_locate(env, d, &lgi->lgi_fid);
-	if (IS_ERR(o))
-		return PTR_ERR(o);
-
-	if (!dt_object_exists(o)) {
-		th = dt_trans_create(env, d);
-		if (IS_ERR(th))
-			GOTO(out, rc = PTR_ERR(th));
-
-		lgi->lgi_attr.la_valid = LA_MODE;
-		lgi->lgi_attr.la_mode = S_IFREG | S_IRUGO | S_IWUSR;
-		lgi->lgi_dof.dof_type = dt_mode_to_dft(S_IFREG);
-
-		rc = dt_declare_create(env, o, &lgi->lgi_attr, NULL,
-				       &lgi->lgi_dof, th);
-		if (rc)
-			GOTO(out_trans, rc);
-
-		rc = dt_trans_start_local(env, d, th);
-		if (rc)
-			GOTO(out_trans, rc);
-
-		dt_write_lock(env, o, 0);
-		if (!dt_object_exists(o))
-			rc = dt_create(env, o, &lgi->lgi_attr, NULL,
-				       &lgi->lgi_dof, th);
-		dt_write_unlock(env, o);
-out_trans:
-		dt_trans_stop(env, d, th);
-		if (rc)
-			GOTO(out, rc);
-	}
-
-	rc = dt_attr_get(env, o, &lgi->lgi_attr, BYPASS_CAPA);
-	if (rc)
-		GOTO(out, rc);
-
-	if (!S_ISREG(lgi->lgi_attr.la_mode)) {
-		CERROR("%s: CATALOGS is not a regular file!: mode = %o\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name,
-		       lgi->lgi_attr.la_mode);
-		GOTO(out, rc = -ENOENT);
-	}
-
-	CDEBUG(D_CONFIG, "cat list: disk size=%d, read=%d\n",
-	       (int)lgi->lgi_attr.la_size, size);
-
-	/* return just number of llogs */
-	if (idarray == NULL) {
-		rc = lgi->lgi_attr.la_size / sizeof(*idarray);
-		GOTO(out, rc);
-	}
-
-	/* read for new ost index or for empty file */
-	memset(idarray, 0, size);
-	if (lgi->lgi_attr.la_size < lgi->lgi_off + size)
-		GOTO(out, rc = 0);
-	if (lgi->lgi_attr.la_size < lgi->lgi_off + size)
-		size = lgi->lgi_attr.la_size - lgi->lgi_off;
-
-	lgi->lgi_buf.lb_buf = idarray;
-	lgi->lgi_buf.lb_len = size;
-	rc = dt_record_read(env, o, &lgi->lgi_buf, &lgi->lgi_off);
-	if (rc) {
-		CERROR("%s: error reading CATALOGS: rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name,  rc);
-		GOTO(out, rc);
-	}
-
-out:
-	lu_object_put(env, &o->do_lu);
-	return rc;
-}
-EXPORT_SYMBOL(llog_osd_get_cat_list);
-
-/* writes the cat list */
-int llog_osd_put_cat_list(const struct lu_env *env, struct dt_device *d,
-			  int idx, int count, struct llog_catid *idarray)
-{
-	struct llog_thread_info	*lgi = llog_info(env);
-	struct dt_object	*o = NULL;
-	struct thandle		*th;
-	int			 rc, size;
-
-	if (!count)
-		return 0;
-
-	LASSERT(d);
-
-	size = sizeof(*idarray) * count;
-	lgi->lgi_off = idx * sizeof(*idarray);
-
-	lu_local_obj_fid(&lgi->lgi_fid, LLOG_CATALOGS_OID);
-
-	o = dt_locate(env, d, &lgi->lgi_fid);
-	if (IS_ERR(o))
-		return PTR_ERR(o);
-
-	if (!dt_object_exists(o))
-		GOTO(out, rc = -ENOENT);
-
-	rc = dt_attr_get(env, o, &lgi->lgi_attr, BYPASS_CAPA);
-	if (rc)
-		GOTO(out, rc);
-
-	if (!S_ISREG(lgi->lgi_attr.la_mode)) {
-		CERROR("%s: CATALOGS is not a regular file!: mode = %o\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name,
-		       lgi->lgi_attr.la_mode);
-		GOTO(out, rc = -ENOENT);
-	}
-
-	th = dt_trans_create(env, d);
-	if (IS_ERR(th))
-		GOTO(out, rc = PTR_ERR(th));
-
-	rc = dt_declare_record_write(env, o, size, lgi->lgi_off, th);
-	if (rc)
-		GOTO(out, rc);
-
-	rc = dt_trans_start_local(env, d, th);
-	if (rc)
-		GOTO(out_trans, rc);
-
-	lgi->lgi_buf.lb_buf = idarray;
-	lgi->lgi_buf.lb_len = size;
-	rc = dt_record_write(env, o, &lgi->lgi_buf, &lgi->lgi_off, th);
-	if (rc)
-		CDEBUG(D_INODE, "error writing CATALOGS: rc = %d\n", rc);
-out_trans:
-	dt_trans_stop(env, d, th);
-out:
-	lu_object_put(env, &o->do_lu);
-	return rc;
-}
-EXPORT_SYMBOL(llog_osd_put_cat_list);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_swab.c b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
index b3247fb7a35a..bfac8387021e 100644
--- a/drivers/staging/lustre/lustre/obdclass/llog_swab.c
+++ b/drivers/staging/lustre/lustre/obdclass/llog_swab.c
@@ -61,9 +61,9 @@ static void print_llogd_body(struct llogd_body *d)
 
 void lustre_swab_lu_fid(struct lu_fid *fid)
 {
-	__swab64s (&fid->f_seq);
-	__swab32s (&fid->f_oid);
-	__swab32s (&fid->f_ver);
+	__swab64s(&fid->f_seq);
+	__swab32s(&fid->f_oid);
+	__swab32s(&fid->f_ver);
 }
 EXPORT_SYMBOL(lustre_swab_lu_fid);
 
@@ -82,47 +82,47 @@ void lustre_swab_llog_id(struct llog_logid *log_id)
 {
 	__swab64s(&log_id->lgl_oi.oi.oi_id);
 	__swab64s(&log_id->lgl_oi.oi.oi_seq);
-        __swab32s(&log_id->lgl_ogen);
+	__swab32s(&log_id->lgl_ogen);
 }
 EXPORT_SYMBOL(lustre_swab_llog_id);
 
-void lustre_swab_llogd_body (struct llogd_body *d)
+void lustre_swab_llogd_body(struct llogd_body *d)
 {
 	print_llogd_body(d);
 	lustre_swab_llog_id(&d->lgd_logid);
-	__swab32s (&d->lgd_ctxt_idx);
-	__swab32s (&d->lgd_llh_flags);
-	__swab32s (&d->lgd_index);
-	__swab32s (&d->lgd_saved_index);
-	__swab32s (&d->lgd_len);
-	__swab64s (&d->lgd_cur_offset);
+	__swab32s(&d->lgd_ctxt_idx);
+	__swab32s(&d->lgd_llh_flags);
+	__swab32s(&d->lgd_index);
+	__swab32s(&d->lgd_saved_index);
+	__swab32s(&d->lgd_len);
+	__swab64s(&d->lgd_cur_offset);
 	print_llogd_body(d);
 }
 EXPORT_SYMBOL(lustre_swab_llogd_body);
 
-void lustre_swab_llogd_conn_body (struct llogd_conn_body *d)
+void lustre_swab_llogd_conn_body(struct llogd_conn_body *d)
 {
-	__swab64s (&d->lgdc_gen.mnt_cnt);
-	__swab64s (&d->lgdc_gen.conn_cnt);
+	__swab64s(&d->lgdc_gen.mnt_cnt);
+	__swab64s(&d->lgdc_gen.conn_cnt);
 	lustre_swab_llog_id(&d->lgdc_logid);
-	__swab32s (&d->lgdc_ctxt_idx);
+	__swab32s(&d->lgdc_ctxt_idx);
 }
 EXPORT_SYMBOL(lustre_swab_llogd_conn_body);
 
 void lustre_swab_ll_fid(struct ll_fid *fid)
 {
-	__swab64s (&fid->id);
-	__swab32s (&fid->generation);
-	__swab32s (&fid->f_type);
+	__swab64s(&fid->id);
+	__swab32s(&fid->generation);
+	__swab32s(&fid->f_type);
 }
 EXPORT_SYMBOL(lustre_swab_ll_fid);
 
 void lustre_swab_lu_seq_range(struct lu_seq_range *range)
 {
-	__swab64s (&range->lsr_start);
-	__swab64s (&range->lsr_end);
-	__swab32s (&range->lsr_index);
-	__swab32s (&range->lsr_flags);
+	__swab64s(&range->lsr_start);
+	__swab64s(&range->lsr_end);
+	__swab32s(&range->lsr_index);
+	__swab32s(&range->lsr_flags);
 }
 EXPORT_SYMBOL(lustre_swab_lu_seq_range);
 
@@ -168,7 +168,7 @@ void lustre_swab_llog_rec(struct llog_rec_hdr *rec)
 	}
 	case CHANGELOG_REC:
 	{
-		struct llog_changelog_rec *cr = (struct llog_changelog_rec*)rec;
+		struct llog_changelog_rec *cr = (struct llog_changelog_rec *)rec;
 
 		__swab16s(&cr->cr.cr_namelen);
 		__swab16s(&cr->cr.cr_flags);
@@ -193,7 +193,7 @@ void lustre_swab_llog_rec(struct llog_rec_hdr *rec)
 	case CHANGELOG_USER_REC:
 	{
 		struct llog_changelog_user_rec *cur =
-			(struct llog_changelog_user_rec*)rec;
+			(struct llog_changelog_user_rec *)rec;
 
 		__swab32s(&cur->cur_id);
 		__swab64s(&cur->cur_endrec);
@@ -294,7 +294,7 @@ static void print_llog_hdr(struct llog_log_hdr *h)
 	CDEBUG(D_OTHER, "\tllh_tail.lrt_len: %#x\n", h->llh_tail.lrt_len);
 }
 
-void lustre_swab_llog_hdr (struct llog_log_hdr *h)
+void lustre_swab_llog_hdr(struct llog_log_hdr *h)
 {
 	print_llog_hdr(h);
 
@@ -367,7 +367,7 @@ struct cfg_marker32 {
 
 void lustre_swab_cfg_marker(struct cfg_marker *marker, int swab, int size)
 {
-	struct cfg_marker32 *cm32 = (struct cfg_marker32*)marker;
+	struct cfg_marker32 *cm32 = (struct cfg_marker32 *)marker;
 
 	if (swab) {
 		__swab32s(&marker->cm_step);
diff --git a/drivers/staging/lustre/lustre/obdclass/llog_test.c b/drivers/staging/lustre/lustre/obdclass/llog_test.c
deleted file mode 100644
index ef008abd331c..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/llog_test.c
+++ /dev/null
@@ -1,1072 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2003, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/obdclass/llog_test.c
- *
- * Author: Phil Schwan <phil@clusterfs.com>
- * Author: Mikhail Pershin <mike.pershin@intel.com>
- */
-
-#define DEBUG_SUBSYSTEM S_CLASS
-
-#include <linux/module.h>
-#include <linux/init.h>
-
-#include "../include/obd_class.h"
-#include "../include/lustre_fid.h"
-#include "../include/lustre_log.h"
-
-/* This is slightly more than the number of records that can fit into a
- * single llog file, because the llog_log_header takes up some of the
- * space in the first block that cannot be used for the bitmap. */
-#define LLOG_TEST_RECNUM  (LLOG_CHUNK_SIZE * 8)
-
-static int llog_test_rand;
-static struct obd_uuid uuid = { .uuid = "test_uuid" };
-static struct llog_logid cat_logid;
-
-struct llog_mini_rec {
-	struct llog_rec_hdr     lmr_hdr;
-	struct llog_rec_tail    lmr_tail;
-} __attribute__((packed));
-
-static int verify_handle(char *test, struct llog_handle *llh, int num_recs)
-{
-	int i;
-	int last_idx = 0;
-	int active_recs = 0;
-
-	for (i = 0; i < LLOG_BITMAP_BYTES * 8; i++) {
-		if (ext2_test_bit(i, llh->lgh_hdr->llh_bitmap)) {
-			last_idx = i;
-			active_recs++;
-		}
-	}
-
-	if (active_recs != num_recs) {
-		CERROR("%s: expected %d active recs after write, found %d\n",
-		       test, num_recs, active_recs);
-		return -ERANGE;
-	}
-
-	if (llh->lgh_hdr->llh_count != num_recs) {
-		CERROR("%s: handle->count is %d, expected %d after write\n",
-		       test, llh->lgh_hdr->llh_count, num_recs);
-		return -ERANGE;
-	}
-
-	if (llh->lgh_last_idx < last_idx) {
-		CERROR("%s: handle->last_idx is %d, expected %d after write\n",
-		       test, llh->lgh_last_idx, last_idx);
-		return -ERANGE;
-	}
-
-	return 0;
-}
-
-/* Test named-log create/open, close */
-static int llog_test_1(const struct lu_env *env,
-		       struct obd_device *obd, char *name)
-{
-	struct llog_handle	*llh;
-	struct llog_ctxt	*ctxt;
-	int rc;
-	int rc2;
-
-	CWARN("1a: create a log with name: %s\n", name);
-	ctxt = llog_get_context(obd, LLOG_TEST_ORIG_CTXT);
-	LASSERT(ctxt);
-
-	rc = llog_open_create(env, ctxt, &llh, NULL, name);
-	if (rc) {
-		CERROR("1a: llog_create with name %s failed: %d\n", name, rc);
-		GOTO(out, rc);
-	}
-	rc = llog_init_handle(env, llh, LLOG_F_IS_PLAIN, &uuid);
-	if (rc) {
-		CERROR("1a: can't init llog handle: %d\n", rc);
-		GOTO(out_close, rc);
-	}
-
-	rc = verify_handle("1", llh, 1);
-
-	CWARN("1b: close newly-created log\n");
-out_close:
-	rc2 = llog_close(env, llh);
-	if (rc2) {
-		CERROR("1b: close log %s failed: %d\n", name, rc2);
-		if (rc == 0)
-			rc = rc2;
-	}
-out:
-	llog_ctxt_put(ctxt);
-	return rc;
-}
-
-/* Test named-log reopen; returns opened log on success */
-static int llog_test_2(const struct lu_env *env, struct obd_device *obd,
-		       char *name, struct llog_handle **llh)
-{
-	struct llog_ctxt	*ctxt;
-	struct llog_handle	*loghandle;
-	struct llog_logid	 logid;
-	int			 rc;
-
-	CWARN("2a: re-open a log with name: %s\n", name);
-	ctxt = llog_get_context(obd, LLOG_TEST_ORIG_CTXT);
-	LASSERT(ctxt);
-
-	rc = llog_open(env, ctxt, llh, NULL, name, LLOG_OPEN_EXISTS);
-	if (rc) {
-		CERROR("2a: re-open log with name %s failed: %d\n", name, rc);
-		GOTO(out_put, rc);
-	}
-
-	rc = llog_init_handle(env, *llh, LLOG_F_IS_PLAIN, &uuid);
-	if (rc) {
-		CERROR("2a: can't init llog handle: %d\n", rc);
-		GOTO(out_close_llh, rc);
-	}
-
-	rc = verify_handle("2", *llh, 1);
-	if (rc)
-		GOTO(out_close_llh, rc);
-
-	/* XXX: there is known issue with tests 2b, MGS is not able to create
-	 * anonymous llog, exit now to allow following tests run.
-	 * It is fixed in upcoming llog over OSD code */
-	GOTO(out_put, rc);
-
-	CWARN("2b: create a log without specified NAME & LOGID\n");
-	rc = llog_open_create(env, ctxt, &loghandle, NULL, NULL);
-	if (rc) {
-		CERROR("2b: create log failed\n");
-		GOTO(out_close_llh, rc);
-	}
-	rc = llog_init_handle(env, loghandle, LLOG_F_IS_PLAIN, &uuid);
-	if (rc) {
-		CERROR("2b: can't init llog handle: %d\n", rc);
-		GOTO(out_close, rc);
-	}
-
-	logid = loghandle->lgh_id;
-	llog_close(env, loghandle);
-
-	CWARN("2c: re-open the log by LOGID\n");
-	rc = llog_open(env, ctxt, &loghandle, &logid, NULL, LLOG_OPEN_EXISTS);
-	if (rc) {
-		CERROR("2c: re-open log by LOGID failed\n");
-		GOTO(out_close_llh, rc);
-	}
-
-	rc = llog_init_handle(env, loghandle, LLOG_F_IS_PLAIN, &uuid);
-	if (rc) {
-		CERROR("2c: can't init llog handle: %d\n", rc);
-		GOTO(out_close, rc);
-	}
-
-	CWARN("2b: destroy this log\n");
-	rc = llog_destroy(env, loghandle);
-	if (rc)
-		CERROR("2d: destroy log failed\n");
-out_close:
-	llog_close(env, loghandle);
-out_close_llh:
-	if (rc)
-		llog_close(env, *llh);
-out_put:
-	llog_ctxt_put(ctxt);
-
-	return rc;
-}
-
-/* Test record writing, single and in bulk */
-static int llog_test_3(const struct lu_env *env, struct obd_device *obd,
-		       struct llog_handle *llh)
-{
-	struct llog_gen_rec	 lgr;
-	int			 rc, i;
-	int			 num_recs = 1; /* 1 for the header */
-
-	lgr.lgr_hdr.lrh_len = lgr.lgr_tail.lrt_len = sizeof(lgr);
-	lgr.lgr_hdr.lrh_type = LLOG_GEN_REC;
-
-	CWARN("3a: write one create_rec\n");
-	rc = llog_write(env, llh,  &lgr.lgr_hdr, NULL, 0, NULL, -1);
-	num_recs++;
-	if (rc < 0) {
-		CERROR("3a: write one log record failed: %d\n", rc);
-		return rc;
-	}
-
-	rc = verify_handle("3a", llh, num_recs);
-	if (rc)
-		return rc;
-
-	CWARN("3b: write 10 cfg log records with 8 bytes bufs\n");
-	for (i = 0; i < 10; i++) {
-		struct llog_rec_hdr	hdr;
-		char			buf[8];
-
-		hdr.lrh_len = 8;
-		hdr.lrh_type = OBD_CFG_REC;
-		memset(buf, 0, sizeof(buf));
-		rc = llog_write(env, llh, &hdr, NULL, 0, buf, -1);
-		if (rc < 0) {
-			CERROR("3b: write 10 records failed at #%d: %d\n",
-			       i + 1, rc);
-			return rc;
-		}
-		num_recs++;
-	}
-
-	rc = verify_handle("3b", llh, num_recs);
-	if (rc)
-		return rc;
-
-	CWARN("3c: write 1000 more log records\n");
-	for (i = 0; i < 1000; i++) {
-		rc = llog_write(env, llh, &lgr.lgr_hdr, NULL, 0, NULL, -1);
-		if (rc < 0) {
-			CERROR("3c: write 1000 records failed at #%d: %d\n",
-			       i + 1, rc);
-			return rc;
-		}
-		num_recs++;
-	}
-
-	rc = verify_handle("3c", llh, num_recs);
-	if (rc)
-		return rc;
-
-	CWARN("3d: write log more than BITMAP_SIZE, return -ENOSPC\n");
-	for (i = 0; i < LLOG_BITMAP_SIZE(llh->lgh_hdr) + 1; i++) {
-		struct llog_rec_hdr	hdr;
-		char			buf_even[24];
-		char			buf_odd[32];
-
-		memset(buf_odd, 0, sizeof(buf_odd));
-		memset(buf_even, 0, sizeof(buf_even));
-		if ((i % 2) == 0) {
-			hdr.lrh_len = 24;
-			hdr.lrh_type = OBD_CFG_REC;
-			rc = llog_write(env, llh, &hdr, NULL, 0, buf_even, -1);
-		} else {
-			hdr.lrh_len = 32;
-			hdr.lrh_type = OBD_CFG_REC;
-			rc = llog_write(env, llh, &hdr, NULL, 0, buf_odd, -1);
-		}
-		if (rc == -ENOSPC) {
-			break;
-		} else if (rc < 0) {
-			CERROR("3d: write recs failed at #%d: %d\n",
-			       i + 1, rc);
-			return rc;
-		}
-		num_recs++;
-	}
-	if (rc != -ENOSPC) {
-		CWARN("3d: write record more than BITMAP size!\n");
-		return -EINVAL;
-	}
-	CWARN("3d: wrote %d more records before end of llog is reached\n",
-	      num_recs);
-
-	rc = verify_handle("3d", llh, num_recs);
-
-	return rc;
-}
-
-/* Test catalogue additions */
-static int llog_test_4(const struct lu_env *env, struct obd_device *obd)
-{
-	struct llog_handle	*cath;
-	char			 name[10];
-	int			 rc, rc2, i, buflen;
-	struct llog_mini_rec	 lmr;
-	struct llog_cookie	 cookie;
-	struct llog_ctxt	*ctxt;
-	int			 num_recs = 0;
-	char			*buf;
-	struct llog_rec_hdr	 rec;
-
-	ctxt = llog_get_context(obd, LLOG_TEST_ORIG_CTXT);
-	LASSERT(ctxt);
-
-	lmr.lmr_hdr.lrh_len = lmr.lmr_tail.lrt_len = LLOG_MIN_REC_SIZE;
-	lmr.lmr_hdr.lrh_type = 0xf00f00;
-
-	sprintf(name, "%x", llog_test_rand + 1);
-	CWARN("4a: create a catalog log with name: %s\n", name);
-	rc = llog_open_create(env, ctxt, &cath, NULL, name);
-	if (rc) {
-		CERROR("4a: llog_create with name %s failed: %d\n", name, rc);
-		GOTO(ctxt_release, rc);
-	}
-	rc = llog_init_handle(env, cath, LLOG_F_IS_CAT, &uuid);
-	if (rc) {
-		CERROR("4a: can't init llog handle: %d\n", rc);
-		GOTO(out, rc);
-	}
-
-	num_recs++;
-	cat_logid = cath->lgh_id;
-
-	CWARN("4b: write 1 record into the catalog\n");
-	rc = llog_cat_add(env, cath, &lmr.lmr_hdr, &cookie, NULL);
-	if (rc != 1) {
-		CERROR("4b: write 1 catalog record failed at: %d\n", rc);
-		GOTO(out, rc);
-	}
-	num_recs++;
-	rc = verify_handle("4b", cath, 2);
-	if (rc)
-		GOTO(out, rc);
-
-	rc = verify_handle("4b", cath->u.chd.chd_current_log, num_recs);
-	if (rc)
-		GOTO(out, rc);
-
-	CWARN("4c: cancel 1 log record\n");
-	rc = llog_cat_cancel_records(env, cath, 1, &cookie);
-	if (rc) {
-		CERROR("4c: cancel 1 catalog based record failed: %d\n", rc);
-		GOTO(out, rc);
-	}
-	num_recs--;
-
-	rc = verify_handle("4c", cath->u.chd.chd_current_log, num_recs);
-	if (rc)
-		GOTO(out, rc);
-
-	CWARN("4d: write %d more log records\n", LLOG_TEST_RECNUM);
-	for (i = 0; i < LLOG_TEST_RECNUM; i++) {
-		rc = llog_cat_add(env, cath, &lmr.lmr_hdr, NULL, NULL);
-		if (rc) {
-			CERROR("4d: write %d records failed at #%d: %d\n",
-			       LLOG_TEST_RECNUM, i + 1, rc);
-			GOTO(out, rc);
-		}
-		num_recs++;
-	}
-
-	/* make sure new plain llog appears */
-	rc = verify_handle("4d", cath, 3);
-	if (rc)
-		GOTO(out, rc);
-
-	CWARN("4e: add 5 large records, one record per block\n");
-	buflen = LLOG_CHUNK_SIZE - sizeof(struct llog_rec_hdr) -
-		 sizeof(struct llog_rec_tail);
-	OBD_ALLOC(buf, buflen);
-	if (buf == NULL)
-		GOTO(out, rc = -ENOMEM);
-	for (i = 0; i < 5; i++) {
-		rec.lrh_len = buflen;
-		rec.lrh_type = OBD_CFG_REC;
-		rc = llog_cat_add(env, cath, &rec, NULL, buf);
-		if (rc) {
-			CERROR("4e: write 5 records failed at #%d: %d\n",
-			       i + 1, rc);
-			GOTO(out_free, rc);
-		}
-		num_recs++;
-	}
-out_free:
-	OBD_FREE(buf, buflen);
-out:
-	CWARN("4f: put newly-created catalog\n");
-	rc2 = llog_cat_close(env, cath);
-	if (rc2) {
-		CERROR("4: close log %s failed: %d\n", name, rc2);
-		if (rc == 0)
-			rc = rc2;
-	}
-ctxt_release:
-	llog_ctxt_put(ctxt);
-	return rc;
-}
-
-static int cat_counter;
-
-static int cat_print_cb(const struct lu_env *env, struct llog_handle *llh,
-			struct llog_rec_hdr *rec, void *data)
-{
-	struct llog_logid_rec	*lir = (struct llog_logid_rec *)rec;
-	struct lu_fid		 fid = {0};
-
-	if (rec->lrh_type != LLOG_LOGID_MAGIC) {
-		CERROR("invalid record in catalog\n");
-		return -EINVAL;
-	}
-
-	logid_to_fid(&lir->lid_id, &fid);
-
-	CWARN("seeing record at index %d - "DFID" in log "DFID"\n",
-	      rec->lrh_index, PFID(&fid),
-	      PFID(lu_object_fid(&llh->lgh_obj->do_lu)));
-
-	cat_counter++;
-
-	return 0;
-}
-
-static int plain_counter;
-
-static int plain_print_cb(const struct lu_env *env, struct llog_handle *llh,
-			  struct llog_rec_hdr *rec, void *data)
-{
-	struct lu_fid fid = {0};
-
-	if (!(llh->lgh_hdr->llh_flags & LLOG_F_IS_PLAIN)) {
-		CERROR("log is not plain\n");
-		return -EINVAL;
-	}
-
-	logid_to_fid(&llh->lgh_id, &fid);
-
-	CDEBUG(D_INFO, "seeing record at index %d in log "DFID"\n",
-	       rec->lrh_index, PFID(&fid));
-
-	plain_counter++;
-
-	return 0;
-}
-
-static int cancel_count;
-
-static int llog_cancel_rec_cb(const struct lu_env *env,
-			      struct llog_handle *llh,
-			      struct llog_rec_hdr *rec, void *data)
-{
-	struct llog_cookie cookie;
-
-	if (!(llh->lgh_hdr->llh_flags & LLOG_F_IS_PLAIN)) {
-		CERROR("log is not plain\n");
-		return -EINVAL;
-	}
-
-	cookie.lgc_lgl = llh->lgh_id;
-	cookie.lgc_index = rec->lrh_index;
-
-	llog_cat_cancel_records(env, llh->u.phd.phd_cat_handle, 1, &cookie);
-	cancel_count++;
-	if (cancel_count == LLOG_TEST_RECNUM)
-		return -LLOG_EEMPTY;
-	return 0;
-}
-
-/* Test log and catalogue processing */
-static int llog_test_5(const struct lu_env *env, struct obd_device *obd)
-{
-	struct llog_handle	*llh = NULL;
-	char			 name[10];
-	int			 rc, rc2;
-	struct llog_mini_rec	 lmr;
-	struct llog_ctxt	*ctxt;
-
-	ctxt = llog_get_context(obd, LLOG_TEST_ORIG_CTXT);
-	LASSERT(ctxt);
-
-	lmr.lmr_hdr.lrh_len = lmr.lmr_tail.lrt_len = LLOG_MIN_REC_SIZE;
-	lmr.lmr_hdr.lrh_type = 0xf00f00;
-
-	CWARN("5a: re-open catalog by id\n");
-	rc = llog_open(env, ctxt, &llh, &cat_logid, NULL, LLOG_OPEN_EXISTS);
-	if (rc) {
-		CERROR("5a: llog_create with logid failed: %d\n", rc);
-		GOTO(out_put, rc);
-	}
-
-	rc = llog_init_handle(env, llh, LLOG_F_IS_CAT, &uuid);
-	if (rc) {
-		CERROR("5a: can't init llog handle: %d\n", rc);
-		GOTO(out, rc);
-	}
-
-	CWARN("5b: print the catalog entries.. we expect 2\n");
-	cat_counter = 0;
-	rc = llog_process(env, llh, cat_print_cb, "test 5", NULL);
-	if (rc) {
-		CERROR("5b: process with cat_print_cb failed: %d\n", rc);
-		GOTO(out, rc);
-	}
-	if (cat_counter != 2) {
-		CERROR("5b: %d entries in catalog\n", cat_counter);
-		GOTO(out, rc = -EINVAL);
-	}
-
-	CWARN("5c: Cancel %d records, see one log zapped\n", LLOG_TEST_RECNUM);
-	cancel_count = 0;
-	rc = llog_cat_process(env, llh, llog_cancel_rec_cb, "foobar", 0, 0);
-	if (rc != -LLOG_EEMPTY) {
-		CERROR("5c: process with cat_cancel_cb failed: %d\n", rc);
-		GOTO(out, rc);
-	}
-
-	CWARN("5c: print the catalog entries.. we expect 1\n");
-	cat_counter = 0;
-	rc = llog_process(env, llh, cat_print_cb, "test 5", NULL);
-	if (rc) {
-		CERROR("5c: process with cat_print_cb failed: %d\n", rc);
-		GOTO(out, rc);
-	}
-	if (cat_counter != 1) {
-		CERROR("5c: %d entries in catalog\n", cat_counter);
-		GOTO(out, rc = -EINVAL);
-	}
-
-	CWARN("5d: add 1 record to the log with many canceled empty pages\n");
-	rc = llog_cat_add(env, llh, &lmr.lmr_hdr, NULL, NULL);
-	if (rc) {
-		CERROR("5d: add record to the log with many canceled empty "
-		       "pages failed\n");
-		GOTO(out, rc);
-	}
-
-	CWARN("5e: print plain log entries.. expect 6\n");
-	plain_counter = 0;
-	rc = llog_cat_process(env, llh, plain_print_cb, "foobar", 0, 0);
-	if (rc) {
-		CERROR("5e: process with plain_print_cb failed: %d\n", rc);
-		GOTO(out, rc);
-	}
-	if (plain_counter != 6) {
-		CERROR("5e: found %d records\n", plain_counter);
-		GOTO(out, rc = -EINVAL);
-	}
-
-	CWARN("5f: print plain log entries reversely.. expect 6\n");
-	plain_counter = 0;
-	rc = llog_cat_reverse_process(env, llh, plain_print_cb, "foobar");
-	if (rc) {
-		CERROR("5f: reversely process with plain_print_cb failed:"
-		       "%d\n", rc);
-		GOTO(out, rc);
-	}
-	if (plain_counter != 6) {
-		CERROR("5f: found %d records\n", plain_counter);
-		GOTO(out, rc = -EINVAL);
-	}
-
-out:
-	CWARN("5g: close re-opened catalog\n");
-	rc2 = llog_cat_close(env, llh);
-	if (rc2) {
-		CERROR("5g: close log %s failed: %d\n", name, rc2);
-		if (rc == 0)
-			rc = rc2;
-	}
-out_put:
-	llog_ctxt_put(ctxt);
-
-	return rc;
-}
-
-/* Test client api; open log by name and process */
-static int llog_test_6(const struct lu_env *env, struct obd_device *obd,
-		       char *name)
-{
-	struct obd_device	*mgc_obd;
-	struct llog_ctxt	*ctxt;
-	struct obd_uuid		*mgs_uuid;
-	struct obd_export	*exp;
-	struct obd_uuid		 uuid = { "LLOG_TEST6_UUID" };
-	struct llog_handle	*llh = NULL;
-	struct llog_ctxt	*nctxt;
-	int			 rc, rc2;
-
-	ctxt = llog_get_context(obd, LLOG_TEST_ORIG_CTXT);
-	LASSERT(ctxt);
-	mgs_uuid = &ctxt->loc_exp->exp_obd->obd_uuid;
-
-	CWARN("6a: re-open log %s using client API\n", name);
-	mgc_obd = class_find_client_obd(mgs_uuid, LUSTRE_MGC_NAME, NULL);
-	if (mgc_obd == NULL) {
-		CERROR("6a: no MGC devices connected to %s found.\n",
-		       mgs_uuid->uuid);
-		GOTO(ctxt_release, rc = -ENOENT);
-	}
-
-	rc = obd_connect(NULL, &exp, mgc_obd, &uuid,
-			 NULL /* obd_connect_data */, NULL);
-	if (rc != -EALREADY) {
-		CERROR("6a: connect on connected MGC (%s) failed to return"
-		       " -EALREADY", mgc_obd->obd_name);
-		if (rc == 0)
-			obd_disconnect(exp);
-		GOTO(ctxt_release, rc = -EINVAL);
-	}
-
-	nctxt = llog_get_context(mgc_obd, LLOG_CONFIG_REPL_CTXT);
-	rc = llog_open(env, nctxt, &llh, NULL, name, LLOG_OPEN_EXISTS);
-	if (rc) {
-		CERROR("6a: llog_open failed %d\n", rc);
-		GOTO(nctxt_put, rc);
-	}
-
-	rc = llog_init_handle(env, llh, LLOG_F_IS_PLAIN, NULL);
-	if (rc) {
-		CERROR("6a: llog_init_handle failed %d\n", rc);
-		GOTO(parse_out, rc);
-	}
-
-	plain_counter = 1; /* llog header is first record */
-	CWARN("6b: process log %s using client API\n", name);
-	rc = llog_process(env, llh, plain_print_cb, NULL, NULL);
-	if (rc)
-		CERROR("6b: llog_process failed %d\n", rc);
-	CWARN("6b: processed %d records\n", plain_counter);
-
-	rc = verify_handle("6b", llh, plain_counter);
-	if (rc)
-		GOTO(parse_out, rc);
-
-	plain_counter = 1; /* llog header is first record */
-	CWARN("6c: process log %s reversely using client API\n", name);
-	rc = llog_reverse_process(env, llh, plain_print_cb, NULL, NULL);
-	if (rc)
-		CERROR("6c: llog_reverse_process failed %d\n", rc);
-	CWARN("6c: processed %d records\n", plain_counter);
-
-	rc = verify_handle("6c", llh, plain_counter);
-	if (rc)
-		GOTO(parse_out, rc);
-
-parse_out:
-	rc2 = llog_close(env, llh);
-	if (rc2) {
-		CERROR("6: llog_close failed: rc = %d\n", rc2);
-		if (rc == 0)
-			rc = rc2;
-	}
-nctxt_put:
-	llog_ctxt_put(nctxt);
-ctxt_release:
-	llog_ctxt_put(ctxt);
-	return rc;
-}
-
-static union {
-	struct llog_rec_hdr		lrh;   /* common header */
-	struct llog_logid_rec		llr;   /* LLOG_LOGID_MAGIC */
-	struct llog_unlink64_rec	lur;   /* MDS_UNLINK64_REC */
-	struct llog_setattr64_rec	lsr64; /* MDS_SETATTR64_REC */
-	struct llog_size_change_rec	lscr;  /* OST_SZ_REC */
-	struct llog_changelog_rec	lcr;   /* CHANGELOG_REC */
-	struct llog_changelog_user_rec	lcur;  /* CHANGELOG_USER_REC */
-	struct llog_gen_rec		lgr;   /* LLOG_GEN_REC */
-} llog_records;
-
-static int test_7_print_cb(const struct lu_env *env, struct llog_handle *llh,
-			   struct llog_rec_hdr *rec, void *data)
-{
-	struct lu_fid fid = {0};
-
-	logid_to_fid(&llh->lgh_id, &fid);
-
-	CDEBUG(D_OTHER, "record type %#x at index %d in log "DFID"\n",
-	       rec->lrh_type, rec->lrh_index, PFID(&fid));
-
-	plain_counter++;
-	return 0;
-}
-
-static int test_7_cancel_cb(const struct lu_env *env, struct llog_handle *llh,
-			    struct llog_rec_hdr *rec, void *data)
-{
-	plain_counter++;
-	/* test LLOG_DEL_RECORD is working */
-	return LLOG_DEL_RECORD;
-}
-
-static int llog_test_7_sub(const struct lu_env *env, struct llog_ctxt *ctxt)
-{
-	struct llog_handle	*llh;
-	int			 rc = 0, i, process_count;
-	int			 num_recs = 0;
-
-	rc = llog_open_create(env, ctxt, &llh, NULL, NULL);
-	if (rc) {
-		CERROR("7_sub: create log failed\n");
-		return rc;
-	}
-
-	rc = llog_init_handle(env, llh,
-			      LLOG_F_IS_PLAIN | LLOG_F_ZAP_WHEN_EMPTY,
-			      &uuid);
-	if (rc) {
-		CERROR("7_sub: can't init llog handle: %d\n", rc);
-		GOTO(out_close, rc);
-	}
-	for (i = 0; i < LLOG_BITMAP_SIZE(llh->lgh_hdr); i++) {
-		rc = llog_write(env, llh, &llog_records.lrh, NULL, 0,
-				NULL, -1);
-		if (rc == -ENOSPC) {
-			break;
-		} else if (rc < 0) {
-			CERROR("7_sub: write recs failed at #%d: %d\n",
-			       i + 1, rc);
-			GOTO(out_close, rc);
-		}
-		num_recs++;
-	}
-	if (rc != -ENOSPC) {
-		CWARN("7_sub: write record more than BITMAP size!\n");
-		GOTO(out_close, rc = -EINVAL);
-	}
-
-	rc = verify_handle("7_sub", llh, num_recs + 1);
-	if (rc) {
-		CERROR("7_sub: verify handle failed: %d\n", rc);
-		GOTO(out_close, rc);
-	}
-	if (num_recs < LLOG_BITMAP_SIZE(llh->lgh_hdr) - 1)
-		CWARN("7_sub: records are not aligned, written %d from %u\n",
-		      num_recs, LLOG_BITMAP_SIZE(llh->lgh_hdr) - 1);
-
-	plain_counter = 0;
-	rc = llog_process(env, llh, test_7_print_cb, "test 7", NULL);
-	if (rc) {
-		CERROR("7_sub: llog process failed: %d\n", rc);
-		GOTO(out_close, rc);
-	}
-	process_count = plain_counter;
-	if (process_count != num_recs) {
-		CERROR("7_sub: processed %d records from %d total\n",
-		       process_count, num_recs);
-		GOTO(out_close, rc = -EINVAL);
-	}
-
-	plain_counter = 0;
-	rc = llog_reverse_process(env, llh, test_7_cancel_cb, "test 7", NULL);
-	if (rc) {
-		CERROR("7_sub: reverse llog process failed: %d\n", rc);
-		GOTO(out_close, rc);
-	}
-	if (process_count != plain_counter) {
-		CERROR("7_sub: Reverse/direct processing found different"
-		       "number of records: %d/%d\n",
-		       plain_counter, process_count);
-		GOTO(out_close, rc = -EINVAL);
-	}
-	if (llog_exist(llh)) {
-		CERROR("7_sub: llog exists but should be zapped\n");
-		GOTO(out_close, rc = -EEXIST);
-	}
-
-	rc = verify_handle("7_sub", llh, 1);
-out_close:
-	if (rc)
-		llog_destroy(env, llh);
-	llog_close(env, llh);
-	return rc;
-}
-
-/* Test all llog records writing and processing */
-static int llog_test_7(const struct lu_env *env, struct obd_device *obd)
-{
-	struct llog_ctxt	*ctxt;
-	int			 rc;
-
-	ctxt = llog_get_context(obd, LLOG_TEST_ORIG_CTXT);
-
-	CWARN("7a: test llog_logid_rec\n");
-	llog_records.llr.lid_hdr.lrh_len = sizeof(llog_records.llr);
-	llog_records.llr.lid_tail.lrt_len = sizeof(llog_records.llr);
-	llog_records.llr.lid_hdr.lrh_type = LLOG_LOGID_MAGIC;
-
-	rc = llog_test_7_sub(env, ctxt);
-	if (rc) {
-		CERROR("7a: llog_logid_rec test failed\n");
-		GOTO(out, rc);
-	}
-
-	CWARN("7b: test llog_unlink64_rec\n");
-	llog_records.lur.lur_hdr.lrh_len = sizeof(llog_records.lur);
-	llog_records.lur.lur_tail.lrt_len = sizeof(llog_records.lur);
-	llog_records.lur.lur_hdr.lrh_type = MDS_UNLINK64_REC;
-
-	rc = llog_test_7_sub(env, ctxt);
-	if (rc) {
-		CERROR("7b: llog_unlink_rec test failed\n");
-		GOTO(out, rc);
-	}
-
-	CWARN("7c: test llog_setattr64_rec\n");
-	llog_records.lsr64.lsr_hdr.lrh_len = sizeof(llog_records.lsr64);
-	llog_records.lsr64.lsr_tail.lrt_len = sizeof(llog_records.lsr64);
-	llog_records.lsr64.lsr_hdr.lrh_type = MDS_SETATTR64_REC;
-
-	rc = llog_test_7_sub(env, ctxt);
-	if (rc) {
-		CERROR("7c: llog_setattr64_rec test failed\n");
-		GOTO(out, rc);
-	}
-
-	CWARN("7d: test llog_size_change_rec\n");
-	llog_records.lscr.lsc_hdr.lrh_len = sizeof(llog_records.lscr);
-	llog_records.lscr.lsc_tail.lrt_len = sizeof(llog_records.lscr);
-	llog_records.lscr.lsc_hdr.lrh_type = OST_SZ_REC;
-
-	rc = llog_test_7_sub(env, ctxt);
-	if (rc) {
-		CERROR("7d: llog_size_change_rec test failed\n");
-		GOTO(out, rc);
-	}
-
-	CWARN("7e: test llog_changelog_rec\n");
-	llog_records.lcr.cr_hdr.lrh_len = sizeof(llog_records.lcr);
-	llog_records.lcr.cr_tail.lrt_len = sizeof(llog_records.lcr);
-	llog_records.lcr.cr_hdr.lrh_type = CHANGELOG_REC;
-
-	rc = llog_test_7_sub(env, ctxt);
-	if (rc) {
-		CERROR("7e: llog_changelog_rec test failed\n");
-		GOTO(out, rc);
-	}
-
-	CWARN("7f: test llog_changelog_user_rec\n");
-	llog_records.lcur.cur_hdr.lrh_len = sizeof(llog_records.lcur);
-	llog_records.lcur.cur_tail.lrt_len = sizeof(llog_records.lcur);
-	llog_records.lcur.cur_hdr.lrh_type = CHANGELOG_USER_REC;
-
-	rc = llog_test_7_sub(env, ctxt);
-	if (rc) {
-		CERROR("7f: llog_changelog_user_rec test failed\n");
-		GOTO(out, rc);
-	}
-
-	CWARN("7g: test llog_gen_rec\n");
-	llog_records.lgr.lgr_hdr.lrh_len = sizeof(llog_records.lgr);
-	llog_records.lgr.lgr_tail.lrt_len = sizeof(llog_records.lgr);
-	llog_records.lgr.lgr_hdr.lrh_type = LLOG_GEN_REC;
-
-	rc = llog_test_7_sub(env, ctxt);
-	if (rc) {
-		CERROR("7g: llog_size_change_rec test failed\n");
-		GOTO(out, rc);
-	}
-out:
-	llog_ctxt_put(ctxt);
-	return rc;
-}
-
-/* -------------------------------------------------------------------------
- * Tests above, boring obd functions below
- * ------------------------------------------------------------------------- */
-static int llog_run_tests(const struct lu_env *env, struct obd_device *obd)
-{
-	struct llog_handle	*llh = NULL;
-	struct llog_ctxt	*ctxt;
-	int			 rc, err;
-	char			 name[10];
-
-	ctxt = llog_get_context(obd, LLOG_TEST_ORIG_CTXT);
-	LASSERT(ctxt);
-
-	sprintf(name, "%x", llog_test_rand);
-
-	rc = llog_test_1(env, obd, name);
-	if (rc)
-		GOTO(cleanup_ctxt, rc);
-
-	rc = llog_test_2(env, obd, name, &llh);
-	if (rc)
-		GOTO(cleanup_ctxt, rc);
-
-	rc = llog_test_3(env, obd, llh);
-	if (rc)
-		GOTO(cleanup, rc);
-
-	rc = llog_test_4(env, obd);
-	if (rc)
-		GOTO(cleanup, rc);
-
-	rc = llog_test_5(env, obd);
-	if (rc)
-		GOTO(cleanup, rc);
-
-	rc = llog_test_6(env, obd, name);
-	if (rc)
-		GOTO(cleanup, rc);
-
-	rc = llog_test_7(env, obd);
-	if (rc)
-		GOTO(cleanup, rc);
-
-cleanup:
-	err = llog_destroy(env, llh);
-	if (err)
-		CERROR("cleanup: llog_destroy failed: %d\n", err);
-	llog_close(env, llh);
-	if (rc == 0)
-		rc = err;
-cleanup_ctxt:
-	llog_ctxt_put(ctxt);
-	return rc;
-}
-
-#if defined (CONFIG_PROC_FS)
-static struct lprocfs_vars lprocfs_llog_test_obd_vars[] = { { NULL } };
-static struct lprocfs_vars lprocfs_llog_test_module_vars[] = { { NULL } };
-static void lprocfs_llog_test_init_vars(struct lprocfs_static_vars *lvars)
-{
-    lvars->module_vars  = lprocfs_llog_test_module_vars;
-    lvars->obd_vars     = lprocfs_llog_test_obd_vars;
-}
-#else
-static void lprocfs_llog_test_init_vars(struct lprocfs_static_vars *lvars)
-{
-}
-#endif
-
-static int llog_test_cleanup(struct obd_device *obd)
-{
-	struct obd_device	*tgt;
-	struct lu_env		 env;
-	int			 rc;
-
-	rc = lu_env_init(&env, LCT_LOCAL | LCT_MG_THREAD);
-	if (rc)
-		return rc;
-
-	tgt = obd->obd_lvfs_ctxt.dt->dd_lu_dev.ld_obd;
-	rc = llog_cleanup(&env, llog_get_context(tgt, LLOG_TEST_ORIG_CTXT));
-	if (rc)
-		CERROR("failed to llog_test_llog_finish: %d\n", rc);
-	lu_env_fini(&env);
-	return rc;
-}
-
-static int llog_test_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
-{
-	struct obd_device	*tgt;
-	struct llog_ctxt	*ctxt;
-	struct dt_object	*o;
-	struct lu_env		 env;
-	struct lu_context	 test_session;
-	int			 rc;
-
-	if (lcfg->lcfg_bufcount < 2) {
-		CERROR("requires a TARGET OBD name\n");
-		return -EINVAL;
-	}
-
-	if (lcfg->lcfg_buflens[1] < 1) {
-		CERROR("requires a TARGET OBD name\n");
-		return -EINVAL;
-	}
-
-	/* disk obd */
-	tgt = class_name2obd(lustre_cfg_string(lcfg, 1));
-	if (!tgt || !tgt->obd_attached || !tgt->obd_set_up) {
-		CERROR("target device not attached or not set up (%s)\n",
-		       lustre_cfg_string(lcfg, 1));
-		return -EINVAL;
-	}
-
-	rc = lu_env_init(&env, LCT_LOCAL | LCT_MG_THREAD);
-	if (rc)
-		return rc;
-
-	rc = lu_context_init(&test_session, LCT_SESSION);
-	if (rc)
-		GOTO(cleanup_env, rc);
-	test_session.lc_thread = (struct ptlrpc_thread *)current;
-	lu_context_enter(&test_session);
-	env.le_ses = &test_session;
-
-	CWARN("Setup llog-test device over %s device\n",
-	      lustre_cfg_string(lcfg, 1));
-
-	OBD_SET_CTXT_MAGIC(&obd->obd_lvfs_ctxt);
-	obd->obd_lvfs_ctxt.dt = lu2dt_dev(tgt->obd_lu_dev);
-
-	rc = llog_setup(&env, tgt, &tgt->obd_olg, LLOG_TEST_ORIG_CTXT, tgt,
-			&llog_osd_ops);
-	if (rc)
-		GOTO(cleanup_session, rc);
-
-	/* use MGS llog dir for tests */
-	ctxt = llog_get_context(tgt, LLOG_CONFIG_ORIG_CTXT);
-	LASSERT(ctxt);
-	o = ctxt->loc_dir;
-	llog_ctxt_put(ctxt);
-
-	ctxt = llog_get_context(tgt, LLOG_TEST_ORIG_CTXT);
-	LASSERT(ctxt);
-	ctxt->loc_dir = o;
-	llog_ctxt_put(ctxt);
-
-	llog_test_rand = cfs_rand();
-
-	rc = llog_run_tests(&env, tgt);
-	if (rc)
-		llog_test_cleanup(obd);
-cleanup_session:
-	lu_context_exit(&test_session);
-	lu_context_fini(&test_session);
-cleanup_env:
-	lu_env_fini(&env);
-	return rc;
-}
-
-static struct obd_ops llog_obd_ops = {
-	.o_owner       = THIS_MODULE,
-	.o_setup       = llog_test_setup,
-	.o_cleanup     = llog_test_cleanup,
-};
-
-static int __init llog_test_init(void)
-{
-	struct lprocfs_static_vars uninitialized_var(lvars);
-
-	lprocfs_llog_test_init_vars(&lvars);
-	return class_register_type(&llog_obd_ops, NULL,
-				   lvars.module_vars, "llog_test", NULL);
-}
-
-static void __exit llog_test_exit(void)
-{
-	class_unregister_type("llog_test");
-}
-
-MODULE_AUTHOR("Sun Microsystems, Inc. <http://www.lustre.org/>");
-MODULE_DESCRIPTION("llog test module");
-MODULE_LICENSE("GPL");
-
-module_init(llog_test_init);
-module_exit(llog_test_exit);
diff --git a/drivers/staging/lustre/lustre/obdclass/local_storage.c b/drivers/staging/lustre/lustre/obdclass/local_storage.c
deleted file mode 100644
index 78190225ac7a..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/local_storage.c
+++ /dev/null
@@ -1,894 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License version 2 for more details.  A copy is
- * included in the COPYING file that accompanied this code.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * lustre/obdclass/local_storage.c
- *
- * Local storage for file/objects with fid generation. Works on top of OSD.
- *
- * Author: Mikhail Pershin <mike.pershin@intel.com>
- */
-
-#define DEBUG_SUBSYSTEM S_CLASS
-
-#include "local_storage.h"
-
-/* all initialized local storages on this node are linked on this */
-static LIST_HEAD(ls_list_head);
-static DEFINE_MUTEX(ls_list_mutex);
-
-static int ls_object_init(const struct lu_env *env, struct lu_object *o,
-			  const struct lu_object_conf *unused)
-{
-	struct ls_device	*ls;
-	struct lu_object	*below;
-	struct lu_device	*under;
-
-	ls = container_of0(o->lo_dev, struct ls_device, ls_top_dev.dd_lu_dev);
-	under = &ls->ls_osd->dd_lu_dev;
-	below = under->ld_ops->ldo_object_alloc(env, o->lo_header, under);
-	if (below == NULL)
-		return -ENOMEM;
-
-	lu_object_add(o, below);
-
-	return 0;
-}
-
-static void ls_object_free(const struct lu_env *env, struct lu_object *o)
-{
-	struct ls_object	*obj = lu2ls_obj(o);
-	struct lu_object_header	*h = o->lo_header;
-
-	dt_object_fini(&obj->ls_obj);
-	lu_object_header_fini(h);
-	OBD_FREE_PTR(obj);
-}
-
-struct lu_object_operations ls_lu_obj_ops = {
-	.loo_object_init  = ls_object_init,
-	.loo_object_free  = ls_object_free,
-};
-
-struct lu_object *ls_object_alloc(const struct lu_env *env,
-				  const struct lu_object_header *_h,
-				  struct lu_device *d)
-{
-	struct lu_object_header	*h;
-	struct ls_object	*o;
-	struct lu_object	*l;
-
-	LASSERT(_h == NULL);
-
-	OBD_ALLOC_PTR(o);
-	if (o != NULL) {
-		l = &o->ls_obj.do_lu;
-		h = &o->ls_header;
-
-		lu_object_header_init(h);
-		dt_object_init(&o->ls_obj, h, d);
-		lu_object_add_top(h, l);
-
-		l->lo_ops = &ls_lu_obj_ops;
-
-		return l;
-	} else {
-		return NULL;
-	}
-}
-
-static struct lu_device_operations ls_lu_dev_ops = {
-	.ldo_object_alloc =	ls_object_alloc
-};
-
-static struct ls_device *__ls_find_dev(struct dt_device *dev)
-{
-	struct ls_device *ls, *ret = NULL;
-
-	list_for_each_entry(ls, &ls_list_head, ls_linkage) {
-		if (ls->ls_osd == dev) {
-			atomic_inc(&ls->ls_refcount);
-			ret = ls;
-			break;
-		}
-	}
-	return ret;
-}
-
-struct ls_device *ls_find_dev(struct dt_device *dev)
-{
-	struct ls_device *ls;
-
-	mutex_lock(&ls_list_mutex);
-	ls = __ls_find_dev(dev);
-	mutex_unlock(&ls_list_mutex);
-
-	return ls;
-}
-
-static struct lu_device_type_operations ls_device_type_ops = {
-	.ldto_start = NULL,
-	.ldto_stop  = NULL,
-};
-
-static struct lu_device_type ls_lu_type = {
-	.ldt_name = "local_storage",
-	.ldt_ops  = &ls_device_type_ops,
-};
-
-struct ls_device *ls_device_get(struct dt_device *dev)
-{
-	struct ls_device *ls;
-
-	mutex_lock(&ls_list_mutex);
-	ls = __ls_find_dev(dev);
-	if (ls)
-		GOTO(out_ls, ls);
-
-	/* not found, then create */
-	OBD_ALLOC_PTR(ls);
-	if (ls == NULL)
-		GOTO(out_ls, ls = ERR_PTR(-ENOMEM));
-
-	atomic_set(&ls->ls_refcount, 1);
-	INIT_LIST_HEAD(&ls->ls_los_list);
-	mutex_init(&ls->ls_los_mutex);
-
-	ls->ls_osd = dev;
-
-	LASSERT(dev->dd_lu_dev.ld_site);
-	lu_device_init(&ls->ls_top_dev.dd_lu_dev, &ls_lu_type);
-	ls->ls_top_dev.dd_lu_dev.ld_ops = &ls_lu_dev_ops;
-	ls->ls_top_dev.dd_lu_dev.ld_site = dev->dd_lu_dev.ld_site;
-
-	/* finally add ls to the list */
-	list_add(&ls->ls_linkage, &ls_list_head);
-out_ls:
-	mutex_unlock(&ls_list_mutex);
-	return ls;
-}
-
-void ls_device_put(const struct lu_env *env, struct ls_device *ls)
-{
-	LASSERT(env);
-	if (!atomic_dec_and_test(&ls->ls_refcount))
-		return;
-
-	mutex_lock(&ls_list_mutex);
-	if (atomic_read(&ls->ls_refcount) == 0) {
-		LASSERT(list_empty(&ls->ls_los_list));
-		list_del(&ls->ls_linkage);
-		lu_site_purge(env, ls->ls_top_dev.dd_lu_dev.ld_site, ~0);
-		lu_device_fini(&ls->ls_top_dev.dd_lu_dev);
-		OBD_FREE_PTR(ls);
-	}
-	mutex_unlock(&ls_list_mutex);
-}
-
-/**
- * local file fid generation
- */
-int local_object_fid_generate(const struct lu_env *env,
-			      struct local_oid_storage *los,
-			      struct lu_fid *fid)
-{
-	LASSERT(los->los_dev);
-	LASSERT(los->los_obj);
-
-	/* take next OID */
-
-	/* to make it unique after reboot we store
-	 * the latest generated fid atomically with
-	 * object creation see local_object_create() */
-
-	mutex_lock(&los->los_id_lock);
-	fid->f_seq = los->los_seq;
-	fid->f_oid = ++los->los_last_oid;
-	fid->f_ver = 0;
-	mutex_unlock(&los->los_id_lock);
-
-	return 0;
-}
-
-int local_object_declare_create(const struct lu_env *env,
-				struct local_oid_storage *los,
-				struct dt_object *o, struct lu_attr *attr,
-				struct dt_object_format *dof,
-				struct thandle *th)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	int			 rc;
-
-	/* update fid generation file */
-	if (los != NULL) {
-		LASSERT(dt_object_exists(los->los_obj));
-		rc = dt_declare_record_write(env, los->los_obj,
-					     sizeof(struct los_ondisk), 0, th);
-		if (rc)
-			return rc;
-	}
-
-	rc = dt_declare_create(env, o, attr, NULL, dof, th);
-	if (rc)
-		return rc;
-
-	dti->dti_lb.lb_buf = NULL;
-	dti->dti_lb.lb_len = sizeof(dti->dti_lma);
-	rc = dt_declare_xattr_set(env, o, &dti->dti_lb, XATTR_NAME_LMA, 0, th);
-
-	return rc;
-}
-
-int local_object_create(const struct lu_env *env,
-			struct local_oid_storage *los,
-			struct dt_object *o, struct lu_attr *attr,
-			struct dt_object_format *dof, struct thandle *th)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	__le64			 lastid;
-	int			 rc;
-
-	rc = dt_create(env, o, attr, NULL, dof, th);
-	if (rc)
-		return rc;
-
-	if (los == NULL)
-		return rc;
-
-	LASSERT(los->los_obj);
-	LASSERT(dt_object_exists(los->los_obj));
-
-	/* many threads can be updated this, serialize
-	 * them here to avoid the race where one thread
-	 * takes the value first, but writes it last */
-	mutex_lock(&los->los_id_lock);
-
-	/* update local oid number on disk so that
-	 * we know the last one used after reboot */
-	lastid = cpu_to_le64(los->los_last_oid);
-
-	dti->dti_off = 0;
-	dti->dti_lb.lb_buf = &lastid;
-	dti->dti_lb.lb_len = sizeof(lastid);
-	rc = dt_record_write(env, los->los_obj, &dti->dti_lb, &dti->dti_off,
-			     th);
-	mutex_unlock(&los->los_id_lock);
-
-	return rc;
-}
-
-/*
- * Create local named object (file, directory or index) in parent directory.
- */
-struct dt_object *__local_file_create(const struct lu_env *env,
-				      const struct lu_fid *fid,
-				      struct local_oid_storage *los,
-				      struct ls_device *ls,
-				      struct dt_object *parent,
-				      const char *name, struct lu_attr *attr,
-				      struct dt_object_format *dof)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*dto;
-	struct thandle		*th;
-	int			 rc;
-
-	dto = ls_locate(env, ls, fid);
-	if (unlikely(IS_ERR(dto)))
-		return dto;
-
-	LASSERT(dto != NULL);
-	if (dt_object_exists(dto))
-		GOTO(out, rc = -EEXIST);
-
-	th = dt_trans_create(env, ls->ls_osd);
-	if (IS_ERR(th))
-		GOTO(out, rc = PTR_ERR(th));
-
-	rc = local_object_declare_create(env, los, dto, attr, dof, th);
-	if (rc)
-		GOTO(trans_stop, rc);
-
-	if (dti->dti_dof.dof_type == DFT_DIR) {
-		dt_declare_ref_add(env, dto, th);
-		dt_declare_ref_add(env, parent, th);
-	}
-
-	rc = dt_declare_insert(env, parent, (void *)fid, (void *)name, th);
-	if (rc)
-		GOTO(trans_stop, rc);
-
-	rc = dt_trans_start_local(env, ls->ls_osd, th);
-	if (rc)
-		GOTO(trans_stop, rc);
-
-	dt_write_lock(env, dto, 0);
-	if (dt_object_exists(dto))
-		GOTO(unlock, rc = 0);
-
-	CDEBUG(D_OTHER, "create new object "DFID"\n",
-	       PFID(lu_object_fid(&dto->do_lu)));
-	rc = local_object_create(env, los, dto, attr, dof, th);
-	if (rc)
-		GOTO(unlock, rc);
-	LASSERT(dt_object_exists(dto));
-
-	if (dti->dti_dof.dof_type == DFT_DIR) {
-		if (!dt_try_as_dir(env, dto))
-			GOTO(destroy, rc = -ENOTDIR);
-		/* Add "." and ".." for newly created dir */
-		rc = dt_insert(env, dto, (void *)fid, (void *)".", th,
-			       BYPASS_CAPA, 1);
-		if (rc)
-			GOTO(destroy, rc);
-		dt_ref_add(env, dto, th);
-		rc = dt_insert(env, dto, (void *)lu_object_fid(&parent->do_lu),
-			       (void *)"..", th, BYPASS_CAPA, 1);
-		if (rc)
-			GOTO(destroy, rc);
-	}
-
-	dt_write_lock(env, parent, 0);
-	rc = dt_insert(env, parent, (const struct dt_rec *)fid,
-		       (const struct dt_key *)name, th, BYPASS_CAPA, 1);
-	if (dti->dti_dof.dof_type == DFT_DIR)
-		dt_ref_add(env, parent, th);
-	dt_write_unlock(env, parent);
-	if (rc)
-		GOTO(destroy, rc);
-destroy:
-	if (rc)
-		dt_destroy(env, dto, th);
-unlock:
-	dt_write_unlock(env, dto);
-trans_stop:
-	dt_trans_stop(env, ls->ls_osd, th);
-out:
-	if (rc) {
-		lu_object_put_nocache(env, &dto->do_lu);
-		dto = ERR_PTR(rc);
-	}
-	return dto;
-}
-
-/*
- * Look up and create (if it does not exist) a local named file or directory in
- * parent directory.
- */
-struct dt_object *local_file_find_or_create(const struct lu_env *env,
-					    struct local_oid_storage *los,
-					    struct dt_object *parent,
-					    const char *name, __u32 mode)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*dto;
-	int			 rc;
-
-	LASSERT(parent);
-
-	rc = dt_lookup_dir(env, parent, name, &dti->dti_fid);
-	if (rc == 0)
-		/* name is found, get the object */
-		dto = ls_locate(env, dt2ls_dev(los->los_dev), &dti->dti_fid);
-	else if (rc != -ENOENT)
-		dto = ERR_PTR(rc);
-	else {
-		rc = local_object_fid_generate(env, los, &dti->dti_fid);
-		if (rc < 0) {
-			dto = ERR_PTR(rc);
-		} else {
-			/* create the object */
-			dti->dti_attr.la_valid	= LA_MODE;
-			dti->dti_attr.la_mode	= mode;
-			dti->dti_dof.dof_type	= dt_mode_to_dft(mode & S_IFMT);
-			dto = __local_file_create(env, &dti->dti_fid, los,
-						  dt2ls_dev(los->los_dev),
-						  parent, name, &dti->dti_attr,
-						  &dti->dti_dof);
-		}
-	}
-	return dto;
-}
-EXPORT_SYMBOL(local_file_find_or_create);
-
-struct dt_object *local_file_find_or_create_with_fid(const struct lu_env *env,
-						     struct dt_device *dt,
-						     const struct lu_fid *fid,
-						     struct dt_object *parent,
-						     const char *name,
-						     __u32 mode)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*dto;
-	int			 rc;
-
-	LASSERT(parent);
-
-	rc = dt_lookup_dir(env, parent, name, &dti->dti_fid);
-	if (rc == 0) {
-		dto = dt_locate(env, dt, &dti->dti_fid);
-	} else if (rc != -ENOENT) {
-		dto = ERR_PTR(rc);
-	} else {
-		struct ls_device *ls;
-
-		ls = ls_device_get(dt);
-		if (IS_ERR(ls)) {
-			dto = ERR_CAST(ls);
-		} else {
-			/* create the object */
-			dti->dti_attr.la_valid	= LA_MODE;
-			dti->dti_attr.la_mode	= mode;
-			dti->dti_dof.dof_type	= dt_mode_to_dft(mode & S_IFMT);
-			dto = __local_file_create(env, fid, NULL, ls, parent,
-						  name, &dti->dti_attr,
-						  &dti->dti_dof);
-			/* ls_device_put() will finalize the ls device, we
-			 * have to open the object in other device stack */
-			if (!IS_ERR(dto)) {
-				dti->dti_fid = dto->do_lu.lo_header->loh_fid;
-				lu_object_put_nocache(env, &dto->do_lu);
-				dto = dt_locate(env, dt, &dti->dti_fid);
-			}
-			ls_device_put(env, ls);
-		}
-	}
-	return dto;
-}
-EXPORT_SYMBOL(local_file_find_or_create_with_fid);
-
-/*
- * Look up and create (if it does not exist) a local named index file in parent
- * directory.
- */
-struct dt_object *local_index_find_or_create(const struct lu_env *env,
-					     struct local_oid_storage *los,
-					     struct dt_object *parent,
-					     const char *name, __u32 mode,
-					     const struct dt_index_features *ft)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*dto;
-	int			 rc;
-
-	LASSERT(parent);
-
-	rc = dt_lookup_dir(env, parent, name, &dti->dti_fid);
-	if (rc == 0) {
-		/* name is found, get the object */
-		dto = ls_locate(env, dt2ls_dev(los->los_dev), &dti->dti_fid);
-	} else if (rc != -ENOENT) {
-		dto = ERR_PTR(rc);
-	} else {
-		rc = local_object_fid_generate(env, los, &dti->dti_fid);
-		if (rc < 0) {
-			dto = ERR_PTR(rc);
-		} else {
-			/* create the object */
-			dti->dti_attr.la_valid		= LA_MODE;
-			dti->dti_attr.la_mode		= mode;
-			dti->dti_dof.dof_type		= DFT_INDEX;
-			dti->dti_dof.u.dof_idx.di_feat	= ft;
-			dto = __local_file_create(env, &dti->dti_fid, los,
-						  dt2ls_dev(los->los_dev),
-						  parent, name, &dti->dti_attr,
-						  &dti->dti_dof);
-		}
-	}
-	return dto;
-
-}
-EXPORT_SYMBOL(local_index_find_or_create);
-
-struct dt_object *
-local_index_find_or_create_with_fid(const struct lu_env *env,
-				    struct dt_device *dt,
-				    const struct lu_fid *fid,
-				    struct dt_object *parent,
-				    const char *name, __u32 mode,
-				    const struct dt_index_features *ft)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*dto;
-	int			 rc;
-
-	LASSERT(parent);
-
-	rc = dt_lookup_dir(env, parent, name, &dti->dti_fid);
-	if (rc == 0) {
-		/* name is found, get the object */
-		if (!lu_fid_eq(fid, &dti->dti_fid))
-			dto = ERR_PTR(-EINVAL);
-		else
-			dto = dt_locate(env, dt, fid);
-	} else if (rc != -ENOENT) {
-		dto = ERR_PTR(rc);
-	} else {
-		struct ls_device *ls;
-
-		ls = ls_device_get(dt);
-		if (IS_ERR(ls)) {
-			dto = ERR_CAST(ls);
-		} else {
-			/* create the object */
-			dti->dti_attr.la_valid		= LA_MODE;
-			dti->dti_attr.la_mode		= mode;
-			dti->dti_dof.dof_type		= DFT_INDEX;
-			dti->dti_dof.u.dof_idx.di_feat  = ft;
-			dto = __local_file_create(env, fid, NULL, ls, parent,
-						  name, &dti->dti_attr,
-						  &dti->dti_dof);
-			/* ls_device_put() will finalize the ls device, we
-			 * have to open the object in other device stack */
-			if (!IS_ERR(dto)) {
-				dti->dti_fid = dto->do_lu.lo_header->loh_fid;
-				lu_object_put_nocache(env, &dto->do_lu);
-				dto = dt_locate(env, dt, &dti->dti_fid);
-			}
-			ls_device_put(env, ls);
-		}
-	}
-	return dto;
-}
-EXPORT_SYMBOL(local_index_find_or_create_with_fid);
-
-static int local_object_declare_unlink(const struct lu_env *env,
-				       struct dt_device *dt,
-				       struct dt_object *p,
-				       struct dt_object *c, const char *name,
-				       struct thandle *th)
-{
-	int rc;
-
-	rc = dt_declare_delete(env, p, (const struct dt_key *)name, th);
-	if (rc < 0)
-		return rc;
-
-	rc = dt_declare_ref_del(env, c, th);
-	if (rc < 0)
-		return rc;
-
-	return dt_declare_destroy(env, c, th);
-}
-
-int local_object_unlink(const struct lu_env *env, struct dt_device *dt,
-			struct dt_object *parent, const char *name)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*dto;
-	struct thandle		*th;
-	int			 rc;
-
-	rc = dt_lookup_dir(env, parent, name, &dti->dti_fid);
-	if (rc == -ENOENT)
-		return 0;
-	else if (rc < 0)
-		return rc;
-
-	dto = dt_locate(env, dt, &dti->dti_fid);
-	if (unlikely(IS_ERR(dto)))
-		return PTR_ERR(dto);
-
-	th = dt_trans_create(env, dt);
-	if (IS_ERR(th))
-		GOTO(out, rc = PTR_ERR(th));
-
-	rc = local_object_declare_unlink(env, dt, parent, dto, name, th);
-	if (rc < 0)
-		GOTO(stop, rc);
-
-	rc = dt_trans_start_local(env, dt, th);
-	if (rc < 0)
-		GOTO(stop, rc);
-
-	dt_write_lock(env, dto, 0);
-	rc = dt_delete(env, parent, (struct dt_key *)name, th, BYPASS_CAPA);
-	if (rc < 0)
-		GOTO(unlock, rc);
-
-	rc = dt_ref_del(env, dto, th);
-	if (rc < 0) {
-		rc = dt_insert(env, parent,
-			       (const struct dt_rec *)&dti->dti_fid,
-			       (const struct dt_key *)name, th, BYPASS_CAPA, 1);
-		GOTO(unlock, rc);
-	}
-
-	rc = dt_destroy(env, dto, th);
-unlock:
-	dt_write_unlock(env, dto);
-stop:
-	dt_trans_stop(env, dt, th);
-out:
-	lu_object_put_nocache(env, &dto->do_lu);
-	return rc;
-}
-EXPORT_SYMBOL(local_object_unlink);
-
-struct local_oid_storage *dt_los_find(struct ls_device *ls, __u64 seq)
-{
-	struct local_oid_storage *los, *ret = NULL;
-
-	list_for_each_entry(los, &ls->ls_los_list, los_list) {
-		if (los->los_seq == seq) {
-			atomic_inc(&los->los_refcount);
-			ret = los;
-			break;
-		}
-	}
-	return ret;
-}
-
-void dt_los_put(struct local_oid_storage *los)
-{
-	if (atomic_dec_and_test(&los->los_refcount))
-		/* should never happen, only local_oid_storage_fini should
-		 * drop refcount to zero */
-		LBUG();
-	return;
-}
-
-/* after Lustre 2.3 release there may be old file to store last generated FID
- * If such file exists then we have to read its content
- */
-int lastid_compat_check(const struct lu_env *env, struct dt_device *dev,
-			__u64 lastid_seq, __u32 *first_oid, struct ls_device *ls)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct dt_object	*root = NULL;
-	struct los_ondisk	 losd;
-	struct dt_object	*o = NULL;
-	int			 rc = 0;
-
-	rc = dt_root_get(env, dev, &dti->dti_fid);
-	if (rc)
-		return rc;
-
-	root = ls_locate(env, ls, &dti->dti_fid);
-	if (IS_ERR(root))
-		return PTR_ERR(root);
-
-	/* find old last_id file */
-	snprintf(dti->dti_buf, sizeof(dti->dti_buf), "seq-%#llx-lastid",
-		 lastid_seq);
-	rc = dt_lookup_dir(env, root, dti->dti_buf, &dti->dti_fid);
-	lu_object_put_nocache(env, &root->do_lu);
-	if (rc == -ENOENT) {
-		/* old llog lastid accessed by FID only */
-		if (lastid_seq != FID_SEQ_LLOG)
-			return 0;
-		dti->dti_fid.f_seq = FID_SEQ_LLOG;
-		dti->dti_fid.f_oid = 1;
-		dti->dti_fid.f_ver = 0;
-		o = ls_locate(env, ls, &dti->dti_fid);
-		if (IS_ERR(o))
-			return PTR_ERR(o);
-
-		if (!dt_object_exists(o)) {
-			lu_object_put_nocache(env, &o->do_lu);
-			return 0;
-		}
-		CDEBUG(D_INFO, "Found old llog lastid file\n");
-	} else if (rc < 0) {
-		return rc;
-	} else {
-		CDEBUG(D_INFO, "Found old lastid file for sequence %#llx\n",
-		       lastid_seq);
-		o = ls_locate(env, ls, &dti->dti_fid);
-		if (IS_ERR(o))
-			return PTR_ERR(o);
-	}
-	/* let's read seq-NNNNNN-lastid file value */
-	LASSERT(dt_object_exists(o));
-	dti->dti_off = 0;
-	dti->dti_lb.lb_buf = &losd;
-	dti->dti_lb.lb_len = sizeof(losd);
-	dt_read_lock(env, o, 0);
-	rc = dt_record_read(env, o, &dti->dti_lb, &dti->dti_off);
-	dt_read_unlock(env, o);
-	lu_object_put_nocache(env, &o->do_lu);
-	if (rc == 0 && le32_to_cpu(losd.lso_magic) != LOS_MAGIC) {
-		CERROR("%s: wrong content of seq-%#llx-lastid file, magic %x\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name, lastid_seq,
-		       le32_to_cpu(losd.lso_magic));
-		return -EINVAL;
-	} else if (rc < 0) {
-		CERROR("%s: failed to read seq-%#llx-lastid: rc = %d\n",
-		       o->do_lu.lo_dev->ld_obd->obd_name, lastid_seq, rc);
-		return rc;
-	}
-	*first_oid = le32_to_cpu(losd.lso_next_oid);
-	return rc;
-}
-
-/**
- * Initialize local OID storage for required sequence.
- * That may be needed for services that uses local files and requires
- * dynamic OID allocation for them.
- *
- * Per each sequence we have an object with 'first_fid' identificator
- * containing the counter for OIDs of locally created files with that
- * sequence.
- *
- * It is used now by llog subsystem and MGS for NID tables
- *
- * Function gets first_fid to create counter object.
- * All dynamic fids will be generated with the same sequence and incremented
- * OIDs
- *
- * Returned local_oid_storage is in-memory representation of OID storage
- */
-int local_oid_storage_init(const struct lu_env *env, struct dt_device *dev,
-			   const struct lu_fid *first_fid,
-			   struct local_oid_storage **los)
-{
-	struct dt_thread_info	*dti = dt_info(env);
-	struct ls_device	*ls;
-	obd_id			 lastid;
-	struct dt_object	*o = NULL;
-	struct thandle		*th;
-	__u32			 first_oid = fid_oid(first_fid);
-	int			 rc = 0;
-
-	ls = ls_device_get(dev);
-	if (IS_ERR(ls))
-		return PTR_ERR(ls);
-
-	mutex_lock(&ls->ls_los_mutex);
-	*los = dt_los_find(ls, fid_seq(first_fid));
-	if (*los != NULL)
-		GOTO(out, rc = 0);
-
-	/* not found, then create */
-	OBD_ALLOC_PTR(*los);
-	if (*los == NULL)
-		GOTO(out, rc = -ENOMEM);
-
-	atomic_set(&(*los)->los_refcount, 1);
-	mutex_init(&(*los)->los_id_lock);
-	(*los)->los_dev = &ls->ls_top_dev;
-	atomic_inc(&ls->ls_refcount);
-	list_add(&(*los)->los_list, &ls->ls_los_list);
-
-	/* Use {seq, 0, 0} to create the LAST_ID file for every
-	 * sequence.  OIDs start at LUSTRE_FID_INIT_OID.
-	 */
-	dti->dti_fid.f_seq = fid_seq(first_fid);
-	dti->dti_fid.f_oid = LUSTRE_FID_LASTID_OID;
-	dti->dti_fid.f_ver = 0;
-	o = ls_locate(env, ls, &dti->dti_fid);
-	if (IS_ERR(o))
-		GOTO(out_los, rc = PTR_ERR(o));
-
-	if (!dt_object_exists(o)) {
-		rc = lastid_compat_check(env, dev, fid_seq(first_fid),
-					 &first_oid, ls);
-		if (rc < 0)
-			GOTO(out_los, rc);
-
-		th = dt_trans_create(env, dev);
-		if (IS_ERR(th))
-			GOTO(out_los, rc = PTR_ERR(th));
-
-		dti->dti_attr.la_valid = LA_MODE | LA_TYPE;
-		dti->dti_attr.la_mode = S_IFREG | S_IRUGO | S_IWUSR;
-		dti->dti_dof.dof_type = dt_mode_to_dft(S_IFREG);
-
-		rc = dt_declare_create(env, o, &dti->dti_attr, NULL,
-				       &dti->dti_dof, th);
-		if (rc)
-			GOTO(out_trans, rc);
-
-		rc = dt_declare_record_write(env, o, sizeof(lastid), 0, th);
-		if (rc)
-			GOTO(out_trans, rc);
-
-		rc = dt_trans_start_local(env, dev, th);
-		if (rc)
-			GOTO(out_trans, rc);
-
-		dt_write_lock(env, o, 0);
-		if (dt_object_exists(o))
-			GOTO(out_lock, rc = 0);
-
-		rc = dt_create(env, o, &dti->dti_attr, NULL, &dti->dti_dof,
-			       th);
-		if (rc)
-			GOTO(out_lock, rc);
-
-		lastid = cpu_to_le64(first_oid);
-
-		dti->dti_off = 0;
-		dti->dti_lb.lb_buf = &lastid;
-		dti->dti_lb.lb_len = sizeof(lastid);
-		rc = dt_record_write(env, o, &dti->dti_lb, &dti->dti_off, th);
-		if (rc)
-			GOTO(out_lock, rc);
-out_lock:
-		dt_write_unlock(env, o);
-out_trans:
-		dt_trans_stop(env, dev, th);
-	} else {
-		dti->dti_off = 0;
-		dti->dti_lb.lb_buf = &lastid;
-		dti->dti_lb.lb_len = sizeof(lastid);
-		dt_read_lock(env, o, 0);
-		rc = dt_record_read(env, o, &dti->dti_lb, &dti->dti_off);
-		dt_read_unlock(env, o);
-		if (rc == 0 && le64_to_cpu(lastid) > OBIF_MAX_OID) {
-			CERROR("%s: bad oid %llu is read from LAST_ID\n",
-			       o->do_lu.lo_dev->ld_obd->obd_name,
-			       le64_to_cpu(lastid));
-			rc = -EINVAL;
-		}
-	}
-out_los:
-	if (rc != 0) {
-		list_del(&(*los)->los_list);
-		atomic_dec(&ls->ls_refcount);
-		OBD_FREE_PTR(*los);
-		*los = NULL;
-		if (o != NULL && !IS_ERR(o))
-			lu_object_put_nocache(env, &o->do_lu);
-	} else {
-		(*los)->los_seq = fid_seq(first_fid);
-		(*los)->los_last_oid = le64_to_cpu(lastid);
-		(*los)->los_obj = o;
-		/* Read value should not be less than initial one
-		 * but possible after upgrade from older fs.
-		 * In this case just switch to the first_oid in memory and
-		 * it will be updated on disk with first object generated */
-		if ((*los)->los_last_oid < first_oid)
-			(*los)->los_last_oid = first_oid;
-	}
-out:
-	mutex_unlock(&ls->ls_los_mutex);
-	ls_device_put(env, ls);
-	return rc;
-}
-EXPORT_SYMBOL(local_oid_storage_init);
-
-void local_oid_storage_fini(const struct lu_env *env,
-			    struct local_oid_storage *los)
-{
-	struct ls_device *ls;
-
-	if (!atomic_dec_and_test(&los->los_refcount))
-		return;
-
-	LASSERT(env);
-	LASSERT(los->los_dev);
-	ls = dt2ls_dev(los->los_dev);
-
-	mutex_lock(&ls->ls_los_mutex);
-	if (atomic_read(&los->los_refcount) == 0) {
-		if (los->los_obj)
-			lu_object_put_nocache(env, &los->los_obj->do_lu);
-		list_del(&los->los_list);
-		OBD_FREE_PTR(los);
-	}
-	mutex_unlock(&ls->ls_los_mutex);
-	ls_device_put(env, ls);
-}
-EXPORT_SYMBOL(local_oid_storage_fini);
diff --git a/drivers/staging/lustre/lustre/obdclass/local_storage.h b/drivers/staging/lustre/lustre/obdclass/local_storage.h
deleted file mode 100644
index 0b9ad33d1152..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/local_storage.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License version 2 for more details.  A copy is
- * included in the COPYING file that accompanied this code.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2012, Intel Corporation.
- */
-/*
- * lustre/obdclass/local_storage.c
- *
- * Local storage for file/objects with fid generation. Works on top of OSD.
- *
- * Author: Mikhail Pershin <mike.pershin@intel.com>
- */
-#ifndef __LOCAL_STORAGE_H
-#define __LOCAL_STORAGE_H
-
-#include "../include/dt_object.h"
-#include "../include/obd.h"
-#include "../include/lustre_fid.h"
-#include "../include/lustre_disk.h"
-
-struct ls_device {
-	struct dt_device	 ls_top_dev;
-	/* all initialized ls_devices on this node linked by this */
-	struct list_head		 ls_linkage;
-	/* how many handle's reference this local storage */
-	atomic_t		 ls_refcount;
-	/* underlaying OSD device */
-	struct dt_device	*ls_osd;
-	/* list of all local OID storages */
-	struct list_head		 ls_los_list;
-	struct mutex		 ls_los_mutex;
-};
-
-static inline struct ls_device *dt2ls_dev(struct dt_device *d)
-{
-	return container_of0(d, struct ls_device, ls_top_dev);
-}
-
-struct ls_object {
-	struct lu_object_header	 ls_header;
-	struct dt_object	 ls_obj;
-};
-
-static inline struct ls_object *lu2ls_obj(struct lu_object *o)
-{
-	return container_of0(o, struct ls_object, ls_obj.do_lu);
-}
-
-static inline struct dt_object *ls_locate(const struct lu_env *env,
-					  struct ls_device *ls,
-					  const struct lu_fid *fid)
-{
-	return dt_locate_at(env, ls->ls_osd, fid, &ls->ls_top_dev.dd_lu_dev);
-}
-
-struct ls_device *ls_device_get(struct dt_device *dev);
-void ls_device_put(const struct lu_env *env, struct ls_device *ls);
-struct local_oid_storage *dt_los_find(struct ls_device *ls, __u64 seq);
-void dt_los_put(struct local_oid_storage *los);
-
-/* Lustre 2.3 on-disk structure describing local object OIDs storage
- * the structure to be used with any sequence managed by
- * local object library.
- * Obsoleted since 2.4 but is kept for compatibility reasons,
- * see lastid_compat_check() in obdclass/local_storage.c */
-struct los_ondisk {
-	__u32 lso_magic;
-	__u32 lso_next_oid;
-};
-
-#define LOS_MAGIC	0xdecafbee
-#endif
diff --git a/drivers/staging/lustre/lustre/lvfs/lvfs_lib.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_counters.c
index cfc4f896b127..c49dfe541925 100644
--- a/drivers/staging/lustre/lustre/lvfs/lvfs_lib.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_counters.c
@@ -15,11 +15,8 @@
  *
  * You should have received a copy of the GNU General Public License
  * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
  *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
+ * http://www.gnu.org/licenses/gpl-2.0.html
  *
  * GPL HEADER END
  */
@@ -27,21 +24,25 @@
  * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
  * Use is subject to license terms.
  *
- * Copyright (c) 2012, Intel Corporation.
+ * Copyright (c) 2012, 2013, Intel Corporation.
  */
 /*
  * This file is part of Lustre, http://www.lustre.org/
  * Lustre is a trademark of Sun Microsystems, Inc.
  *
- * lustre/lvfs/lvfs_lib.c
+ * lustre/obdclass/lprocfs_counters.c
  *
- * Lustre filesystem abstraction routines
+ * Lustre lprocfs counter routines
  *
- * Author: Andreas Dilger <adilger@clusterfs.com>
+ * Author: Andreas Dilger <andreas.dilger@intel.com>
  */
+
 #include <linux/module.h>
-#include "../include/lustre_lib.h"
 #include "../include/lprocfs_status.h"
+#include "../include/obd_support.h"
+
+struct lprocfs_stats *obd_memory = NULL;
+EXPORT_SYMBOL(obd_memory);
 
 void lprocfs_counter_add(struct lprocfs_stats *stats, int idx, long amount)
 {
@@ -53,6 +54,9 @@ void lprocfs_counter_add(struct lprocfs_stats *stats, int idx, long amount)
 	if (stats == NULL)
 		return;
 
+	LASSERTF(0 <= idx && idx < stats->ls_num,
+		 "idx %d, ls_num %hu\n", idx, stats->ls_num);
+
 	/* With per-client stats, statistics are allocated only for
 	 * single CPU area, so the smp_id should be 0 always. */
 	smp_id = lprocfs_stats_lock(stats, LPROCFS_GET_SMP_ID, &flags);
@@ -101,6 +105,9 @@ void lprocfs_counter_sub(struct lprocfs_stats *stats, int idx, long amount)
 	if (stats == NULL)
 		return;
 
+	LASSERTF(0 <= idx && idx < stats->ls_num,
+		 "idx %d, ls_num %hu\n", idx, stats->ls_num);
+
 	/* With per-client stats, statistics are allocated only for
 	 * single CPU area, so the smp_id should be 0 always. */
 	smp_id = lprocfs_stats_lock(stats, LPROCFS_GET_SMP_ID, &flags);
@@ -130,41 +137,3 @@ void lprocfs_counter_sub(struct lprocfs_stats *stats, int idx, long amount)
 	lprocfs_stats_unlock(stats, LPROCFS_GET_SMP_ID, &flags);
 }
 EXPORT_SYMBOL(lprocfs_counter_sub);
-
-int lprocfs_stats_alloc_one(struct lprocfs_stats *stats, unsigned int cpuid)
-{
-	struct lprocfs_counter	*cntr;
-	unsigned int		percpusize;
-	int			rc = -ENOMEM;
-	unsigned long		flags = 0;
-	int			i;
-
-	LASSERT(stats->ls_percpu[cpuid] == NULL);
-	LASSERT((stats->ls_flags & LPROCFS_STATS_FLAG_NOPERCPU) == 0);
-
-	percpusize = lprocfs_stats_counter_size(stats);
-	LIBCFS_ALLOC_ATOMIC(stats->ls_percpu[cpuid], percpusize);
-	if (stats->ls_percpu[cpuid] != NULL) {
-		rc = 0;
-		if (unlikely(stats->ls_biggest_alloc_num <= cpuid)) {
-			if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
-				spin_lock_irqsave(&stats->ls_lock, flags);
-			else
-				spin_lock(&stats->ls_lock);
-			if (stats->ls_biggest_alloc_num <= cpuid)
-				stats->ls_biggest_alloc_num = cpuid + 1;
-			if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
-				spin_unlock_irqrestore(&stats->ls_lock, flags);
-			else
-				spin_unlock(&stats->ls_lock);
-		}
-		/* initialize the ls_percpu[cpuid] non-zero counter */
-		for (i = 0; i < stats->ls_num; ++i) {
-			cntr = lprocfs_stats_counter_get(stats, cpuid, i);
-			cntr->lc_min = LC_MIN_INIT;
-		}
-	}
-
-	return rc;
-}
-EXPORT_SYMBOL(lprocfs_stats_alloc_one);
diff --git a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
index 8309d4ce6d60..61e04af2464f 100644
--- a/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
+++ b/drivers/staging/lustre/lustre/obdclass/lprocfs_status.c
@@ -356,8 +356,10 @@ struct proc_dir_entry *lprocfs_register(const char *name,
 	struct proc_dir_entry *entry;
 
 	entry = proc_mkdir(name, parent);
-	if (entry == NULL)
-		GOTO(out, entry = ERR_PTR(-ENOMEM));
+	if (entry == NULL) {
+		entry = ERR_PTR(-ENOMEM);
+		goto out;
+	}
 
 	if (list != NULL) {
 		int rc = lprocfs_add_vars(entry, list, data);
@@ -751,7 +753,7 @@ int lprocfs_rd_import(struct seq_file *m, void *data)
 		      ret.lc_sum, header->lc_units);
 
 	k = 0;
-	for(j = 0; j < IMP_AT_MAX_PORTALS; j++) {
+	for (j = 0; j < IMP_AT_MAX_PORTALS; j++) {
 		if (imp->imp_at.iat_portal[j] == 0)
 			break;
 		k = max_t(unsigned int, k,
@@ -883,7 +885,7 @@ int lprocfs_rd_timeouts(struct seq_file *m, void *data)
 		       "network", cur, worst, worstt, DHMS_VARS(&ts));
 	lprocfs_at_hist_helper(m, &imp->imp_at.iat_net_latency);
 
-	for(i = 0; i < IMP_AT_MAX_PORTALS; i++) {
+	for (i = 0; i < IMP_AT_MAX_PORTALS; i++) {
 		if (imp->imp_at.iat_portal[i] == 0)
 			break;
 		cur = at_get(&imp->imp_at.iat_service_estimate[i]);
@@ -927,7 +929,7 @@ EXPORT_SYMBOL(lprocfs_rd_num_exports);
 
 int lprocfs_rd_numrefs(struct seq_file *m, void *data)
 {
-	struct obd_type *class = (struct obd_type*) data;
+	struct obd_type *class = (struct obd_type *) data;
 
 	LASSERT(class != NULL);
 	return seq_printf(m, "%d\n", class->typ_refcnt);
@@ -947,7 +949,8 @@ int lprocfs_obd_setup(struct obd_device *obd, struct lprocfs_vars *list)
 					       list, obd);
 	if (IS_ERR(obd->obd_proc_entry)) {
 		rc = PTR_ERR(obd->obd_proc_entry);
-		CERROR("error %d setting up lprocfs for %s\n",rc,obd->obd_name);
+		CERROR("error %d setting up lprocfs for %s\n",
+		       rc, obd->obd_name);
 		obd->obd_proc_entry = NULL;
 	}
 	return rc;
@@ -1011,6 +1014,43 @@ void lprocfs_free_per_client_stats(struct obd_device *obd)
 }
 EXPORT_SYMBOL(lprocfs_free_per_client_stats);
 
+int lprocfs_stats_alloc_one(struct lprocfs_stats *stats, unsigned int cpuid)
+{
+	struct lprocfs_counter  *cntr;
+	unsigned int            percpusize;
+	int                     rc = -ENOMEM;
+	unsigned long           flags = 0;
+	int                     i;
+
+	LASSERT(stats->ls_percpu[cpuid] == NULL);
+	LASSERT((stats->ls_flags & LPROCFS_STATS_FLAG_NOPERCPU) == 0);
+
+	percpusize = lprocfs_stats_counter_size(stats);
+	LIBCFS_ALLOC_ATOMIC(stats->ls_percpu[cpuid], percpusize);
+	if (stats->ls_percpu[cpuid] != NULL) {
+		rc = 0;
+		if (unlikely(stats->ls_biggest_alloc_num <= cpuid)) {
+			if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
+				spin_lock_irqsave(&stats->ls_lock, flags);
+			else
+				spin_lock(&stats->ls_lock);
+			if (stats->ls_biggest_alloc_num <= cpuid)
+				stats->ls_biggest_alloc_num = cpuid + 1;
+			if (stats->ls_flags & LPROCFS_STATS_FLAG_IRQ_SAFE)
+				spin_unlock_irqrestore(&stats->ls_lock, flags);
+			else
+				spin_unlock(&stats->ls_lock);
+		}
+		/* initialize the ls_percpu[cpuid] non-zero counter */
+		for (i = 0; i < stats->ls_num; ++i) {
+			cntr = lprocfs_stats_counter_get(stats, cpuid, i);
+			cntr->lc_min = LC_MIN_INIT;
+		}
+	}
+	return rc;
+}
+EXPORT_SYMBOL(lprocfs_stats_alloc_one);
+
 struct lprocfs_stats *lprocfs_alloc_stats(unsigned int num,
 					  enum lprocfs_stats_flags flags)
 {
@@ -1308,44 +1348,24 @@ void lprocfs_init_ops_stats(int num_private_stats, struct lprocfs_stats *stats)
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, packmd);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, unpackmd);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, preallocate);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, precreate);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, create);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, create_async);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, destroy);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, setattr);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, setattr_async);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, getattr);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, getattr_async);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, brw);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, merge_lvb);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, adjust_kms);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, punch);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, sync);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, migrate);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, copy);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, iterate);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, preprw);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, commitrw);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, enqueue);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, change_cbdata);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, find_cbdata);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, cancel);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, cancel_unused);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, init_export);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, destroy_export);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, extent_calc);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, llog_init);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, llog_connect);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, llog_finish);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, pin);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, unpin);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, import_event);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, notify);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, health_check);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, get_uuid);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, quotacheck);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, quotactl);
-	LPROCFS_OBD_OP_INIT(num_private_stats, stats, ping);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, pool_new);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, pool_rem);
 	LPROCFS_OBD_OP_INIT(num_private_stats, stats, pool_add);
@@ -1572,7 +1592,10 @@ lproc_exp_hash_seq_show(struct seq_file *m, void *unused)
 {
 	struct nid_stat *stats = (struct nid_stat *)m->private;
 	struct obd_device *obd = stats->nid_obd;
-	struct exp_hash_cb_data cb_data = {m, true};
+	struct exp_hash_cb_data cb_data = {
+		.m = m,
+		.first = true
+	};
 
 	cfs_hash_for_each_key(obd->obd_nid_hash, &stats->nid,
 			      lprocfs_exp_print_hash, &cb_data);
@@ -1593,7 +1616,7 @@ static int lprocfs_nid_stats_clear_write_cb(void *obj, void *data)
 {
 	struct nid_stat *stat = obj;
 
-	CDEBUG(D_INFO,"refcnt %d\n", atomic_read(&stat->nid_exp_ref_count));
+	CDEBUG(D_INFO, "refcnt %d\n", atomic_read(&stat->nid_exp_ref_count));
 	if (atomic_read(&stat->nid_exp_ref_count) == 1) {
 		/* object has only hash references. */
 		spin_lock(&stat->nid_obd->obd_nid_lock);
@@ -1679,12 +1702,15 @@ int lprocfs_exp_setup(struct obd_export *exp, lnet_nid_t *nid, int *newnid)
 	 * entry already has been created */
 	if (old_stat != new_stat) {
 		exp->exp_nid_stats = old_stat;
-		GOTO(destroy_new, rc = -EALREADY);
+		rc = -EALREADY;
+		goto destroy_new;
 	}
 	/* not found - create */
 	OBD_ALLOC(buffer, LNET_NIDSTR_SIZE);
-	if (buffer == NULL)
-		GOTO(destroy_new, rc = -ENOMEM);
+	if (buffer == NULL) {
+		rc = -ENOMEM;
+		goto destroy_new;
+	}
 
 	memcpy(buffer, libcfs_nid2str(*nid), LNET_NIDSTR_SIZE);
 	new_stat->nid_proc = lprocfs_register(buffer,
@@ -1697,7 +1723,7 @@ int lprocfs_exp_setup(struct obd_export *exp, lnet_nid_t *nid, int *newnid)
 		       libcfs_nid2str(*nid));
 		rc = PTR_ERR(new_stat->nid_proc);
 		new_stat->nid_proc = NULL;
-		GOTO(destroy_new_ns, rc);
+		goto destroy_new_ns;
 	}
 
 	entry = lprocfs_add_simple(new_stat->nid_proc, "uuid",
@@ -1705,7 +1731,7 @@ int lprocfs_exp_setup(struct obd_export *exp, lnet_nid_t *nid, int *newnid)
 	if (IS_ERR(entry)) {
 		CWARN("Error adding the NID stats file\n");
 		rc = PTR_ERR(entry);
-		GOTO(destroy_new_ns, rc);
+		goto destroy_new_ns;
 	}
 
 	entry = lprocfs_add_simple(new_stat->nid_proc, "hash",
@@ -1713,7 +1739,7 @@ int lprocfs_exp_setup(struct obd_export *exp, lnet_nid_t *nid, int *newnid)
 	if (IS_ERR(entry)) {
 		CWARN("Error adding the hash file\n");
 		rc = PTR_ERR(entry);
-		GOTO(destroy_new_ns, rc);
+		goto destroy_new_ns;
 	}
 
 	exp->exp_nid_stats = new_stat;
@@ -1741,7 +1767,7 @@ int lprocfs_exp_cleanup(struct obd_export *exp)
 {
 	struct nid_stat *stat = exp->exp_nid_stats;
 
-	if(!stat || !exp->exp_obd)
+	if (!stat || !exp->exp_obd)
 		return 0;
 
 	nidstat_putref(exp->exp_nid_stats);
@@ -1751,6 +1777,48 @@ int lprocfs_exp_cleanup(struct obd_export *exp)
 }
 EXPORT_SYMBOL(lprocfs_exp_cleanup);
 
+__s64 lprocfs_read_helper(struct lprocfs_counter *lc,
+			  struct lprocfs_counter_header *header,
+			  enum lprocfs_stats_flags flags,
+			  enum lprocfs_fields_flags field)
+{
+	__s64 ret = 0;
+
+	if (lc == NULL || header == NULL)
+		return 0;
+
+	switch (field) {
+	case LPROCFS_FIELDS_FLAGS_CONFIG:
+		ret = header->lc_config;
+		break;
+	case LPROCFS_FIELDS_FLAGS_SUM:
+		ret = lc->lc_sum;
+		if ((flags & LPROCFS_STATS_FLAG_IRQ_SAFE) != 0)
+			ret += lc->lc_sum_irq;
+		break;
+	case LPROCFS_FIELDS_FLAGS_MIN:
+		ret = lc->lc_min;
+		break;
+	case LPROCFS_FIELDS_FLAGS_MAX:
+		ret = lc->lc_max;
+		break;
+	case LPROCFS_FIELDS_FLAGS_AVG:
+		ret = (lc->lc_max - lc->lc_min) / 2;
+		break;
+	case LPROCFS_FIELDS_FLAGS_SUMSQUARE:
+		ret = lc->lc_sumsquare;
+		break;
+	case LPROCFS_FIELDS_FLAGS_COUNT:
+		ret = lc->lc_count;
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+EXPORT_SYMBOL(lprocfs_read_helper);
+
 int lprocfs_write_helper(const char *buffer, unsigned long count,
 			 int *val)
 {
@@ -1783,7 +1851,8 @@ int lprocfs_seq_read_frac_helper(struct seq_file *m, long val, int mult)
 }
 EXPORT_SYMBOL(lprocfs_seq_read_frac_helper);
 
-int lprocfs_write_u64_helper(const char *buffer, unsigned long count,__u64 *val)
+int lprocfs_write_u64_helper(const char *buffer, unsigned long count,
+			     __u64 *val)
 {
 	return lprocfs_write_frac_u64_helper(buffer, count, val, 1);
 }
@@ -1828,7 +1897,7 @@ int lprocfs_write_frac_u64_helper(const char *buffer, unsigned long count,
 	}
 
 	units = 1;
-	switch(*end) {
+	switch (*end) {
 	case 'p': case 'P':
 		units <<= 10;
 	case 't': case 'T':
@@ -1922,8 +1991,8 @@ int lprocfs_obd_seq_create(struct obd_device *dev,
 			   const struct file_operations *seq_fops,
 			   void *data)
 {
-	return (lprocfs_seq_create(dev->obd_proc_entry, name,
-				   mode, seq_fops, data));
+	return lprocfs_seq_create(dev->obd_proc_entry, name,
+				  mode, seq_fops, data);
 }
 EXPORT_SYMBOL(lprocfs_obd_seq_create);
 
diff --git a/drivers/staging/lustre/lustre/obdclass/lu_ucred.c b/drivers/staging/lustre/lustre/obdclass/lu_ucred.c
deleted file mode 100644
index 3676563ab330..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/lu_ucred.c
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2007, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2011, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/obdclass/lu_ucred.c
- *
- * Lustre user credentials context infrastructure.
- *
- *   Author: Nikita Danilov <nikita.danilov@sun.com>
- *   Author: Fan Yong <fan.yong@intel.com>
- *   Author: Vitaly Fertman <vitaly_fertman@xyratex.com>
- */
-
-#define DEBUG_SUBSYSTEM S_CLASS
-
-#include "../../include/linux/libcfs/libcfs.h"
-#include "../include/obd_support.h"
-#include "../include/lu_object.h"
-#include "../include/md_object.h"
-
-/* context key constructor/destructor: lu_ucred_key_init, lu_ucred_key_fini */
-LU_KEY_INIT_FINI(lu_ucred, struct lu_ucred);
-
-static struct lu_context_key lu_ucred_key = {
-	.lct_tags = LCT_SESSION,
-	.lct_init = lu_ucred_key_init,
-	.lct_fini = lu_ucred_key_fini
-};
-
-/**
- * Get ucred key if session exists and ucred key is allocated on it.
- * Return NULL otherwise.
- */
-struct lu_ucred *lu_ucred(const struct lu_env *env)
-{
-	if (!env->le_ses)
-		return NULL;
-	return lu_context_key_get(env->le_ses, &lu_ucred_key);
-}
-EXPORT_SYMBOL(lu_ucred);
-
-/**
- * Get ucred key and check if it is properly initialized.
- * Return NULL otherwise.
- */
-struct lu_ucred *lu_ucred_check(const struct lu_env *env)
-{
-	struct lu_ucred *uc = lu_ucred(env);
-	if (uc && uc->uc_valid != UCRED_OLD && uc->uc_valid != UCRED_NEW)
-		return NULL;
-	return uc;
-}
-EXPORT_SYMBOL(lu_ucred_check);
-
-/**
- * Get ucred key, which must exist and must be properly initialized.
- * Assert otherwise.
- */
-struct lu_ucred *lu_ucred_assert(const struct lu_env *env)
-{
-	struct lu_ucred *uc = lu_ucred_check(env);
-	LASSERT(uc != NULL);
-	return uc;
-}
-EXPORT_SYMBOL(lu_ucred_assert);
-
-int lu_ucred_global_init(void)
-{
-	LU_CONTEXT_KEY_INIT(&lu_ucred_key);
-	return lu_context_key_register(&lu_ucred_key);
-}
-
-void lu_ucred_global_fini(void)
-{
-	lu_context_key_degister(&lu_ucred_key);
-}
diff --git a/drivers/staging/lustre/lustre/obdclass/lustre_handles.c b/drivers/staging/lustre/lustre/obdclass/lustre_handles.c
index 2010463429b1..f720e3183295 100644
--- a/drivers/staging/lustre/lustre/obdclass/lustre_handles.c
+++ b/drivers/staging/lustre/lustre/obdclass/lustre_handles.c
@@ -178,7 +178,7 @@ void *class_handle2object(__u64 cookie)
 }
 EXPORT_SYMBOL(class_handle2object);
 
-void class_handle_free_cb(cfs_rcu_head_t *rcu)
+void class_handle_free_cb(struct rcu_head *rcu)
 {
 	struct portals_handle *h = RCU2HANDLE(rcu);
 	void *ptr = (void *)(unsigned long)h->h_cookie;
diff --git a/drivers/staging/lustre/lustre/obdclass/md_attrs.c b/drivers/staging/lustre/lustre/obdclass/md_attrs.c
deleted file mode 100644
index d9e6348de4fa..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/md_attrs.c
+++ /dev/null
@@ -1,199 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; if not, write to the
- * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
- * Boston, MA 021110-1307, USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2012, Intel Corporation.
- * Use is subject to license terms.
- *
- * Author: Johann Lombardi <johann.lombardi@intel.com>
- */
-
-#include "../include/lustre/lustre_idl.h"
-#include "../include/obd.h"
-#include "../include/md_object.h"
-
-/**
- * Initialize new \a lma. Only fid is stored.
- *
- * \param lma - is the new LMA structure to be initialized
- * \param fid - is the FID of the object this LMA belongs to
- * \param incompat - features that MDS must understand to access object
- */
-void lustre_lma_init(struct lustre_mdt_attrs *lma, const struct lu_fid *fid,
-		     __u32 incompat)
-{
-	lma->lma_compat   = 0;
-	lma->lma_incompat = incompat;
-	lma->lma_self_fid = *fid;
-
-	/* If a field is added in struct lustre_mdt_attrs, zero it explicitly
-	 * and change the test below. */
-	LASSERT(sizeof(*lma) ==
-		(offsetof(struct lustre_mdt_attrs, lma_self_fid) +
-		 sizeof(lma->lma_self_fid)));
-};
-EXPORT_SYMBOL(lustre_lma_init);
-
-/**
- * Swab, if needed, LMA structure which is stored on-disk in little-endian order.
- *
- * \param lma - is a pointer to the LMA structure to be swabbed.
- */
-void lustre_lma_swab(struct lustre_mdt_attrs *lma)
-{
-	/* Use LUSTRE_MSG_MAGIC to detect local endianness. */
-	if (LUSTRE_MSG_MAGIC != cpu_to_le32(LUSTRE_MSG_MAGIC)) {
-		__swab32s(&lma->lma_compat);
-		__swab32s(&lma->lma_incompat);
-		lustre_swab_lu_fid(&lma->lma_self_fid);
-	}
-};
-EXPORT_SYMBOL(lustre_lma_swab);
-
-/**
- * Swab, if needed, SOM structure which is stored on-disk in little-endian
- * order.
- *
- * \param attrs - is a pointer to the SOM structure to be swabbed.
- */
-void lustre_som_swab(struct som_attrs *attrs)
-{
-	/* Use LUSTRE_MSG_MAGIC to detect local endianness. */
-	if (LUSTRE_MSG_MAGIC != cpu_to_le32(LUSTRE_MSG_MAGIC)) {
-		__swab32s(&attrs->som_compat);
-		__swab32s(&attrs->som_incompat);
-		__swab64s(&attrs->som_ioepoch);
-		__swab64s(&attrs->som_size);
-		__swab64s(&attrs->som_blocks);
-		__swab64s(&attrs->som_mountid);
-	}
-};
-EXPORT_SYMBOL(lustre_som_swab);
-
-/*
- * Swab and extract SOM attributes from on-disk xattr.
- *
- * \param buf - is a buffer containing the on-disk SOM extended attribute.
- * \param rc  - is the SOM xattr stored in \a buf
- * \param msd - is the md_som_data structure where to extract SOM attributes.
- */
-int lustre_buf2som(void *buf, int rc, struct md_som_data *msd)
-{
-	struct som_attrs *attrs = (struct som_attrs *)buf;
-
-	if (rc == 0 ||  rc == -ENODATA)
-		/* no SOM attributes */
-		return -ENODATA;
-
-	if (rc < 0)
-		/* error hit while fetching xattr */
-		return rc;
-
-	/* check SOM compatibility */
-	if (attrs->som_incompat & ~cpu_to_le32(SOM_INCOMPAT_SUPP))
-		return -ENODATA;
-
-	/* unpack SOM attributes */
-	lustre_som_swab(attrs);
-
-	/* fill in-memory msd structure */
-	msd->msd_compat   = attrs->som_compat;
-	msd->msd_incompat = attrs->som_incompat;
-	msd->msd_ioepoch  = attrs->som_ioepoch;
-	msd->msd_size     = attrs->som_size;
-	msd->msd_blocks   = attrs->som_blocks;
-	msd->msd_mountid  = attrs->som_mountid;
-
-	return 0;
-}
-EXPORT_SYMBOL(lustre_buf2som);
-
-/**
- * Swab, if needed, HSM structure which is stored on-disk in little-endian
- * order.
- *
- * \param attrs - is a pointer to the HSM structure to be swabbed.
- */
-void lustre_hsm_swab(struct hsm_attrs *attrs)
-{
-	/* Use LUSTRE_MSG_MAGIC to detect local endianness. */
-	if (LUSTRE_MSG_MAGIC != cpu_to_le32(LUSTRE_MSG_MAGIC)) {
-		__swab32s(&attrs->hsm_compat);
-		__swab32s(&attrs->hsm_flags);
-		__swab64s(&attrs->hsm_arch_id);
-		__swab64s(&attrs->hsm_arch_ver);
-	}
-};
-EXPORT_SYMBOL(lustre_hsm_swab);
-
-/*
- * Swab and extract HSM attributes from on-disk xattr.
- *
- * \param buf - is a buffer containing the on-disk HSM extended attribute.
- * \param rc  - is the HSM xattr stored in \a buf
- * \param mh  - is the md_hsm structure where to extract HSM attributes.
- */
-int lustre_buf2hsm(void *buf, int rc, struct md_hsm *mh)
-{
-	struct hsm_attrs *attrs = (struct hsm_attrs *)buf;
-
-	if (rc == 0 ||  rc == -ENODATA)
-		/* no HSM attributes */
-		return -ENODATA;
-
-	if (rc < 0)
-		/* error hit while fetching xattr */
-		return rc;
-
-	/* unpack HSM attributes */
-	lustre_hsm_swab(attrs);
-
-	/* fill md_hsm structure */
-	mh->mh_compat   = attrs->hsm_compat;
-	mh->mh_flags    = attrs->hsm_flags;
-	mh->mh_arch_id  = attrs->hsm_arch_id;
-	mh->mh_arch_ver = attrs->hsm_arch_ver;
-
-	return 0;
-}
-EXPORT_SYMBOL(lustre_buf2hsm);
-
-/*
- * Pack HSM attributes.
- *
- * \param buf - is the output buffer where to pack the on-disk HSM xattr.
- * \param mh  - is the md_hsm structure to pack.
- */
-void lustre_hsm2buf(void *buf, struct md_hsm *mh)
-{
-	struct hsm_attrs *attrs = (struct hsm_attrs *)buf;
-
-	/* copy HSM attributes */
-	attrs->hsm_compat   = mh->mh_compat;
-	attrs->hsm_flags    = mh->mh_flags;
-	attrs->hsm_arch_id  = mh->mh_arch_id;
-	attrs->hsm_arch_ver = mh->mh_arch_ver;
-
-	/* pack xattr */
-	lustre_hsm_swab(attrs);
-}
-EXPORT_SYMBOL(lustre_hsm2buf);
diff --git a/drivers/staging/lustre/lustre/obdclass/mea.c b/drivers/staging/lustre/lustre/obdclass/mea.c
deleted file mode 100644
index d6ce084da902..000000000000
--- a/drivers/staging/lustre/lustre/obdclass/mea.c
+++ /dev/null
@@ -1,112 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2004, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- */
-
-#define DEBUG_SUBSYSTEM S_CLASS
-#include "../include/obd_class.h"
-#include <linux/kmod.h>   /* for request_module() */
-#include <linux/module.h>
-#include <linux/slab.h>
-#include <linux/pagemap.h>
-#include "../include/lprocfs_status.h"
-#include "../include/lustre/lustre_idl.h"
-
-static int mea_last_char_hash(int count, char *name, int namelen)
-{
-	unsigned int c;
-
-	c = name[namelen - 1];
-	if (c == 0)
-		CWARN("looks like wrong len is passed\n");
-	c = c % count;
-	return c;
-}
-
-static int mea_all_chars_hash(int count, char *name, int namelen)
-{
-	unsigned int c = 0;
-
-	while (--namelen >= 0)
-		c += name[namelen];
-	c = c % count;
-	return c;
-}
-
-int raw_name2idx(int hashtype, int count, const char *name, int namelen)
-{
-	unsigned int	c = 0;
-	int		idx;
-
-	LASSERT(namelen > 0);
-
-	if (filename_is_volatile(name, namelen, &idx)) {
-		if ((idx >= 0) && (idx < count))
-			return idx;
-		goto hashchoice;
-	}
-
-	if (count <= 1)
-		return 0;
-
-hashchoice:
-	switch (hashtype) {
-	case MEA_MAGIC_LAST_CHAR:
-		c = mea_last_char_hash(count, (char *)name, namelen);
-		break;
-	case MEA_MAGIC_ALL_CHARS:
-		c = mea_all_chars_hash(count, (char *)name, namelen);
-		break;
-	case MEA_MAGIC_HASH_SEGMENT:
-		CERROR("Unsupported hash type MEA_MAGIC_HASH_SEGMENT\n");
-		break;
-	default:
-		CERROR("Unknown hash type 0x%x\n", hashtype);
-	}
-
-	LASSERT(c < count);
-	return c;
-}
-EXPORT_SYMBOL(raw_name2idx);
-
-int mea_name2idx(struct lmv_stripe_md *mea, const char *name, int namelen)
-{
-	unsigned int c;
-
-	LASSERT(mea && mea->mea_count);
-
-	c = raw_name2idx(mea->mea_magic, mea->mea_count, name, namelen);
-
-	LASSERT(c < mea->mea_count);
-	return c;
-}
-EXPORT_SYMBOL(mea_name2idx);
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_config.c b/drivers/staging/lustre/lustre/obdclass/obd_config.c
index 0d81d3232f31..5e7b3d7cc984 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_config.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_config.c
@@ -365,7 +365,7 @@ int class_attach(struct lustre_cfg *lcfg)
 		obd = NULL;
 		CERROR("Cannot create device %s of type %s : %d\n",
 		       name, typename, rc);
-		GOTO(out, rc);
+		goto out;
 	}
 	LASSERTF(obd != NULL, "Cannot get obd device %s of type %s\n",
 		 name, typename);
@@ -412,15 +412,18 @@ int class_attach(struct lustre_cfg *lcfg)
 	if (len >= sizeof(obd->obd_uuid)) {
 		CERROR("uuid must be < %d bytes long\n",
 		       (int)sizeof(obd->obd_uuid));
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 	memcpy(obd->obd_uuid.uuid, uuid, len);
 
 	/* do the attach */
 	if (OBP(obd, attach)) {
 		rc = OBP(obd, attach)(obd, sizeof(*lcfg), lcfg);
-		if (rc)
-			GOTO(out, rc = -EINVAL);
+		if (rc) {
+			rc = -EINVAL;
+			goto out;
+		}
 	}
 
 	/* Detach drops this */
@@ -494,8 +497,10 @@ int class_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 					     CFS_HASH_MIN_THETA,
 					     CFS_HASH_MAX_THETA,
 					     &uuid_hash_ops, CFS_HASH_DEFAULT);
-	if (!obd->obd_uuid_hash)
-		GOTO(err_hash, err = -ENOMEM);
+	if (!obd->obd_uuid_hash) {
+		err = -ENOMEM;
+		goto err_hash;
+	}
 
 	/* create a nid-export lustre hash */
 	obd->obd_nid_hash = cfs_hash_create("NID_HASH",
@@ -505,8 +510,10 @@ int class_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 					    CFS_HASH_MIN_THETA,
 					    CFS_HASH_MAX_THETA,
 					    &nid_hash_ops, CFS_HASH_DEFAULT);
-	if (!obd->obd_nid_hash)
-		GOTO(err_hash, err = -ENOMEM);
+	if (!obd->obd_nid_hash) {
+		err = -ENOMEM;
+		goto err_hash;
+	}
 
 	/* create a nid-stats lustre hash */
 	obd->obd_nid_stats_hash = cfs_hash_create("NID_STATS",
@@ -516,12 +523,16 @@ int class_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 						  CFS_HASH_MIN_THETA,
 						  CFS_HASH_MAX_THETA,
 						  &nid_stat_hash_ops, CFS_HASH_DEFAULT);
-	if (!obd->obd_nid_stats_hash)
-		GOTO(err_hash, err = -ENOMEM);
+	if (!obd->obd_nid_stats_hash) {
+		err = -ENOMEM;
+		goto err_hash;
+	}
 
 	exp = class_new_export(obd, &obd->obd_uuid);
-	if (IS_ERR(exp))
-		GOTO(err_hash, err = PTR_ERR(exp));
+	if (IS_ERR(exp)) {
+		err = PTR_ERR(exp);
+		goto err_hash;
+	}
 
 	obd->obd_self_export = exp;
 	list_del_init(&exp->exp_obd_chain_timed);
@@ -529,7 +540,7 @@ int class_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 
 	err = obd_setup(obd, lcfg);
 	if (err)
-		GOTO(err_exp, err);
+		goto err_exp;
 
 	obd->obd_set_up = 1;
 
@@ -856,21 +867,27 @@ int class_add_profile(int proflen, char *prof, int osclen, char *osc,
 
 	LASSERT(proflen == (strlen(prof) + 1));
 	OBD_ALLOC(lprof->lp_profile, proflen);
-	if (lprof->lp_profile == NULL)
-		GOTO(out, err = -ENOMEM);
+	if (lprof->lp_profile == NULL) {
+		err = -ENOMEM;
+		goto out;
+	}
 	memcpy(lprof->lp_profile, prof, proflen);
 
 	LASSERT(osclen == (strlen(osc) + 1));
 	OBD_ALLOC(lprof->lp_dt, osclen);
-	if (lprof->lp_dt == NULL)
-		GOTO(out, err = -ENOMEM);
+	if (lprof->lp_dt == NULL) {
+		err = -ENOMEM;
+		goto out;
+	}
 	memcpy(lprof->lp_dt, osc, osclen);
 
 	if (mdclen > 0) {
 		LASSERT(mdclen == (strlen(mdc) + 1));
 		OBD_ALLOC(lprof->lp_md, mdclen);
-		if (lprof->lp_md == NULL)
-			GOTO(out, err = -ENOMEM);
+		if (lprof->lp_md == NULL) {
+			err = -ENOMEM;
+			goto out;
+		}
 		memcpy(lprof->lp_md, mdc, mdclen);
 	}
 
@@ -1050,7 +1067,7 @@ static int process_param2_config(struct lustre_cfg *lcfg)
 	}
 
 	do_gettimeofday(&start);
-	rc = USERMODEHELPER(argv[0], argv, NULL);
+	rc = call_usermodehelper(argv[0], argv, NULL, 1);
 	do_gettimeofday(&end);
 
 	if (rc < 0) {
@@ -1087,10 +1104,10 @@ int class_process_config(struct lustre_cfg *lcfg)
 	CDEBUG(D_IOCTL, "processing cmd: %x\n", lcfg->lcfg_command);
 
 	/* Commands that don't need a device */
-	switch(lcfg->lcfg_command) {
+	switch (lcfg->lcfg_command) {
 	case LCFG_ATTACH: {
 		err = class_attach(lcfg);
-		GOTO(out, err);
+		goto out;
 	}
 	case LCFG_ADD_UUID: {
 		CDEBUG(D_IOCTL, "adding mapping from uuid %s to nid %#llx (%s)\n",
@@ -1098,7 +1115,7 @@ int class_process_config(struct lustre_cfg *lcfg)
 		       libcfs_nid2str(lcfg->lcfg_nid));
 
 		err = class_add_uuid(lustre_cfg_string(lcfg, 1), lcfg->lcfg_nid);
-		GOTO(out, err);
+		goto out;
 	}
 	case LCFG_DEL_UUID: {
 		CDEBUG(D_IOCTL, "removing mappings for uuid %s\n",
@@ -1106,7 +1123,7 @@ int class_process_config(struct lustre_cfg *lcfg)
 		       ? "<all uuids>" : lustre_cfg_string(lcfg, 1));
 
 		err = class_del_uuid(lustre_cfg_string(lcfg, 1));
-		GOTO(out, err);
+		goto out;
 	}
 	case LCFG_MOUNTOPT: {
 		CDEBUG(D_IOCTL, "mountopt: profile %s osc %s mdc %s\n",
@@ -1121,20 +1138,22 @@ int class_process_config(struct lustre_cfg *lcfg)
 					lustre_cfg_string(lcfg, 2),
 					LUSTRE_CFG_BUFLEN(lcfg, 3),
 					lustre_cfg_string(lcfg, 3));
-		GOTO(out, err);
+		goto out;
 	}
 	case LCFG_DEL_MOUNTOPT: {
 		CDEBUG(D_IOCTL, "mountopt: profile %s\n",
 		       lustre_cfg_string(lcfg, 1));
 		class_del_profile(lustre_cfg_string(lcfg, 1));
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_SET_TIMEOUT: {
 		CDEBUG(D_IOCTL, "changing lustre timeout from %d to %d\n",
 		       obd_timeout, lcfg->lcfg_num);
 		obd_timeout = max(lcfg->lcfg_num, 1U);
 		obd_timeout_set = 1;
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_SET_LDLM_TIMEOUT: {
 		CDEBUG(D_IOCTL, "changing lustre ldlm_timeout from %d to %d\n",
@@ -1143,19 +1162,22 @@ int class_process_config(struct lustre_cfg *lcfg)
 		if (ldlm_timeout >= obd_timeout)
 			ldlm_timeout = max(obd_timeout / 3, 1U);
 		ldlm_timeout_set = 1;
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_SET_UPCALL: {
 		LCONSOLE_ERROR_MSG(0x15a, "recovery upcall is deprecated\n");
 		/* COMPAT_146 Don't fail on old configs */
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_MARKER: {
 		struct cfg_marker *marker;
 		marker = lustre_cfg_buf(lcfg, 1);
 		CDEBUG(D_IOCTL, "marker %d (%#x) %.16s %s\n", marker->cm_step,
 		       marker->cm_flags, marker->cm_tgtname, marker->cm_comment);
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_PARAM: {
 		char *tmp;
@@ -1164,7 +1186,7 @@ int class_process_config(struct lustre_cfg *lcfg)
 				       PARAM_LLITE, NULL) == 0) &&
 		    client_process_config) {
 			err = (*client_process_config)(lcfg);
-			GOTO(out, err);
+			goto out;
 		} else if ((class_match_param(lustre_cfg_string(lcfg, 1),
 					      PARAM_SYS, &tmp) == 0)) {
 			/* Global param settings */
@@ -1176,19 +1198,20 @@ int class_process_config(struct lustre_cfg *lcfg)
 			if (err != 0)
 				CWARN("Ignoring unknown param %s\n", tmp);
 
-			GOTO(out, err = 0);
+			err = 0;
+			goto out;
 		} else if ((class_match_param(lustre_cfg_string(lcfg, 1),
 					      PARAM_QUOTA, &tmp) == 0) &&
 			   quota_process_config) {
 			err = (*quota_process_config)(lcfg);
-			GOTO(out, err);
+			goto out;
 		}
 
 		break;
 	}
 	case LCFG_SET_PARAM: {
 		err = process_param2_config(lcfg);
-		GOTO(out, 0);
+		goto out;
 	}
 	}
 	/* Commands that require a device */
@@ -1200,55 +1223,60 @@ int class_process_config(struct lustre_cfg *lcfg)
 			CERROR("no device for: %s\n",
 			       lustre_cfg_string(lcfg, 0));
 
-		GOTO(out, err = -EINVAL);
+		err = -EINVAL;
+		goto out;
 	}
 
-	switch(lcfg->lcfg_command) {
+	switch (lcfg->lcfg_command) {
 	case LCFG_SETUP: {
 		err = class_setup(obd, lcfg);
-		GOTO(out, err);
+		goto out;
 	}
 	case LCFG_DETACH: {
 		err = class_detach(obd, lcfg);
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_CLEANUP: {
 		err = class_cleanup(obd, lcfg);
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_ADD_CONN: {
 		err = class_add_conn(obd, lcfg);
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_DEL_CONN: {
 		err = class_del_conn(obd, lcfg);
-		GOTO(out, err = 0);
+		err = 0;
+		goto out;
 	}
 	case LCFG_POOL_NEW: {
 		err = obd_pool_new(obd, lustre_cfg_string(lcfg, 2));
-		GOTO(out, err = 0);
-		break;
+		err = 0;
+		goto out;
 	}
 	case LCFG_POOL_ADD: {
 		err = obd_pool_add(obd, lustre_cfg_string(lcfg, 2),
 				   lustre_cfg_string(lcfg, 3));
-		GOTO(out, err = 0);
-		break;
+		err = 0;
+		goto out;
 	}
 	case LCFG_POOL_REM: {
 		err = obd_pool_rem(obd, lustre_cfg_string(lcfg, 2),
 				   lustre_cfg_string(lcfg, 3));
-		GOTO(out, err = 0);
-		break;
+		err = 0;
+		goto out;
 	}
 	case LCFG_POOL_DEL: {
 		err = obd_pool_del(obd, lustre_cfg_string(lcfg, 2));
-		GOTO(out, err = 0);
-		break;
+		err = 0;
+		goto out;
 	}
 	default: {
 		err = obd_process_config(obd, sizeof(*lcfg), lcfg);
-		GOTO(out, err);
+		goto out;
 
 	}
 	}
@@ -1361,7 +1389,7 @@ int class_config_llog_handler(const struct lu_env *env,
 {
 	struct config_llog_instance *clli = data;
 	int cfg_len = rec->lrh_len;
-	char *cfg_buf = (char*) (rec + 1);
+	char *cfg_buf = (char *) (rec + 1);
 	int rc = 0;
 
 	//class_config_dump_handler(handle, rec, data);
@@ -1382,7 +1410,7 @@ int class_config_llog_handler(const struct lu_env *env,
 
 		rc = lustre_cfg_sanity_check(cfg_buf, cfg_len);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 
 		/* Figure out config state info */
 		if (lcfg->lcfg_command == LCFG_MARKER) {
@@ -1469,8 +1497,10 @@ int class_config_llog_handler(const struct lu_env *env,
 			inst_len = LUSTRE_CFG_BUFLEN(lcfg, 0) +
 				   sizeof(clli->cfg_instance) * 2 + 4;
 			OBD_ALLOC(inst_name, inst_len);
-			if (inst_name == NULL)
-				GOTO(out, rc = -ENOMEM);
+			if (inst_name == NULL) {
+				rc = -ENOMEM;
+				goto out;
+			}
 			sprintf(inst_name, "%s-%p",
 				lustre_cfg_string(lcfg, 0),
 				clli->cfg_instance);
@@ -1562,7 +1592,7 @@ int class_config_parse_llog(const struct lu_env *env, struct llog_ctxt *ctxt,
 
 	rc = llog_init_handle(env, llh, LLOG_F_IS_PLAIN, NULL);
 	if (rc)
-		GOTO(parse_out, rc);
+		goto parse_out;
 
 	/* continue processing from where we last stopped to end-of-log */
 	if (cfg) {
@@ -1674,7 +1704,7 @@ int class_config_dump_llog(const struct lu_env *env, struct llog_ctxt *ctxt,
 
 	rc = llog_init_handle(env, llh, LLOG_F_IS_PLAIN, NULL);
 	if (rc)
-		GOTO(parse_out, rc);
+		goto parse_out;
 
 	rc = llog_process(env, llh, class_config_dump_handler, cfg, NULL);
 parse_out:
@@ -1717,7 +1747,7 @@ int class_manual_cleanup(struct obd_device *obd)
 	rc = class_process_config(lcfg);
 	if (rc) {
 		CERROR("cleanup failed %d: %s\n", rc, obd->obd_name);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	/* the lcfg is almost the same for both ops */
diff --git a/drivers/staging/lustre/lustre/obdclass/obd_mount.c b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
index d972f71c9d97..1260c8713bc6 100644
--- a/drivers/staging/lustre/lustre/obdclass/obd_mount.c
+++ b/drivers/staging/lustre/lustre/obdclass/obd_mount.c
@@ -46,7 +46,7 @@
 #define PRINT_CMD CDEBUG
 
 #include "../include/obd.h"
-#include "../include/lvfs.h"
+#include "../include/linux/lustre_compat25.h"
 #include "../include/obd_class.h"
 #include "../include/lustre/lustre_user.h"
 #include "../include/lustre_log.h"
@@ -174,7 +174,7 @@ int do_lcfg(char *cfgname, lnet_nid_t nid, int cmd,
 	lcfg->lcfg_nid = nid;
 	rc = class_process_config(lcfg);
 	lustre_cfg_free(lcfg);
-	return(rc);
+	return rc;
 }
 EXPORT_SYMBOL(do_lcfg);
 
@@ -256,15 +256,19 @@ int lustre_start_mgc(struct super_block *sb)
 	len = strlen(LUSTRE_MGC_OBDNAME) + strlen(libcfs_nid2str(nid)) + 1;
 	OBD_ALLOC(mgcname, len);
 	OBD_ALLOC(niduuid, len + 2);
-	if (!mgcname || !niduuid)
-		GOTO(out_free, rc = -ENOMEM);
+	if (!mgcname || !niduuid) {
+		rc = -ENOMEM;
+		goto out_free;
+	}
 	sprintf(mgcname, "%s%s", LUSTRE_MGC_OBDNAME, libcfs_nid2str(nid));
 
 	mgssec = lsi->lsi_lmd->lmd_mgssec ? lsi->lsi_lmd->lmd_mgssec : "";
 
 	OBD_ALLOC_PTR(data);
-	if (data == NULL)
-		GOTO(out_free, rc = -ENOMEM);
+	if (data == NULL) {
+		rc = -ENOMEM;
+		goto out_free;
+	}
 
 	obd = class_name2obd(mgcname);
 	if (obd && !obd->obd_stopping) {
@@ -274,7 +278,7 @@ int lustre_start_mgc(struct super_block *sb)
 					strlen(KEY_MGSSEC), KEY_MGSSEC,
 					strlen(mgssec), mgssec, NULL);
 		if (rc)
-			GOTO(out_free, rc);
+			goto out_free;
 
 		/* Re-using an existing MGC */
 		atomic_inc(&obd->u.cli.cl_mgc_refcount);
@@ -318,12 +322,14 @@ int lustre_start_mgc(struct super_block *sb)
 		   (using its local copy of the log), but we do want to connect
 		   if at all possible. */
 		recov_bk++;
-		CDEBUG(D_MOUNT, "%s: Set MGC reconnect %d\n", mgcname,recov_bk);
+		CDEBUG(D_MOUNT, "%s: Set MGC reconnect %d\n", mgcname,
+		       recov_bk);
 		rc = obd_set_info_async(NULL, obd->obd_self_export,
 					sizeof(KEY_INIT_RECOV_BACKUP),
 					KEY_INIT_RECOV_BACKUP,
 					sizeof(recov_bk), &recov_bk, NULL);
-		GOTO(out, rc = 0);
+		rc = 0;
+		goto out;
 	}
 
 	CDEBUG(D_MOUNT, "Start MGC '%s'\n", mgcname);
@@ -349,7 +355,8 @@ int lustre_start_mgc(struct super_block *sb)
 			} else if (class_find_param(ptr, PARAM_MGSNODE,
 						    &ptr) != 0) {
 				CERROR("No MGS nids given.\n");
-				GOTO(out_free, rc = -EINVAL);
+				rc = -EINVAL;
+				goto out_free;
 			}
 			while (class_parse_nid(ptr, &nid, &ptr) == 0) {
 				rc = do_lcfg(mgcname, nid,
@@ -372,7 +379,8 @@ int lustre_start_mgc(struct super_block *sb)
 	}
 	if (i == 0) {
 		CERROR("No valid MGS nids found.\n");
-		GOTO(out_free, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out_free;
 	}
 	lsi->lsi_lmd->lmd_mgs_failnodes = 1;
 
@@ -387,7 +395,7 @@ int lustre_start_mgc(struct super_block *sb)
 				 niduuid, NULL, NULL);
 	OBD_FREE_PTR(uuid);
 	if (rc)
-		GOTO(out_free, rc);
+		goto out_free;
 
 	/* Add any failover MGS nids */
 	i = 1;
@@ -417,14 +425,15 @@ int lustre_start_mgc(struct super_block *sb)
 	obd = class_name2obd(mgcname);
 	if (!obd) {
 		CERROR("Can't find mgcobd %s\n", mgcname);
-		GOTO(out_free, rc = -ENOTCONN);
+		rc = -ENOTCONN;
+		goto out_free;
 	}
 
 	rc = obd_set_info_async(NULL, obd->obd_self_export,
 				strlen(KEY_MGSSEC), KEY_MGSSEC,
 				strlen(mgssec), mgssec, NULL);
 	if (rc)
-		GOTO(out_free, rc);
+		goto out_free;
 
 	/* Keep a refcount of servers/clients who started with "mount",
 	   so we know when we can get rid of the mgc. */
@@ -448,7 +457,7 @@ int lustre_start_mgc(struct super_block *sb)
 	rc = obd_connect(NULL, &exp, obd, &(obd->obd_uuid), data, NULL);
 	if (rc) {
 		CERROR("connect failed %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	obd->u.cli.cl_mgc_mgsexp = exp;
@@ -490,7 +499,8 @@ static int lustre_stop_mgc(struct super_block *sb)
 		   will call in here. */
 		CDEBUG(D_MOUNT, "mgc still has %d references.\n",
 		       atomic_read(&obd->u.cli.cl_mgc_refcount));
-		GOTO(out, rc = -EBUSY);
+		rc = -EBUSY;
+		goto out;
 	}
 
 	/* The MGC has no recoverable data in any case.
@@ -516,11 +526,13 @@ static int lustre_stop_mgc(struct super_block *sb)
 
 	rc = class_manual_cleanup(obd);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* Clean the nid uuids */
-	if (!niduuid)
-		GOTO(out, rc = -ENOMEM);
+	if (!niduuid) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	for (i = 0; i < lsi->lsi_lmd->lmd_mgs_failnodes; i++) {
 		sprintf(ptr, "_%x", i);
@@ -817,7 +829,7 @@ int lustre_check_exclusion(struct super_block *sb, char *svname)
 	CDEBUG(D_MOUNT, "Check exclusion %s (%d) in %d of %s\n", svname,
 	       index, lmd->lmd_exclude_count, lmd->lmd_dev);
 
-	for(i = 0; i < lmd->lmd_exclude_count; i++) {
+	for (i = 0; i < lmd->lmd_exclude_count; i++) {
 		if (index == lmd->lmd_exclude[i]) {
 			CWARN("Excluding %s (on exclusion list)\n", svname);
 			return 1;
@@ -1211,7 +1223,8 @@ int lustre_fill_super(struct super_block *sb, void *data, int silent)
 	/* Figure out the lmd from the mount options */
 	if (lmd_parse((char *)(lmd2->lmd2_data), lmd)) {
 		lustre_put_lsi(sb);
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	if (lmd_is_client(lmd)) {
@@ -1228,7 +1241,7 @@ int lustre_fill_super(struct super_block *sb, void *data, int silent)
 			rc = lustre_start_mgc(sb);
 			if (rc) {
 				lustre_put_lsi(sb);
-				GOTO(out, rc);
+				goto out;
 			}
 			/* Connect and start */
 			/* (should always be ll_fill_super) */
@@ -1243,7 +1256,7 @@ int lustre_fill_super(struct super_block *sb, void *data, int silent)
 
 	/* If error happens in fill_super() call, @lsi will be killed there.
 	 * This is why we do not put it here. */
-	GOTO(out, rc);
+	goto out;
 out:
 	if (rc) {
 		CERROR("Unable to mount %s (%d)\n",
@@ -1276,7 +1289,10 @@ EXPORT_SYMBOL(lustre_register_kill_super_cb);
 struct dentry *lustre_mount(struct file_system_type *fs_type, int flags,
 				const char *devname, void *data)
 {
-	struct lustre_mount_data2 lmd2 = { data, NULL };
+	struct lustre_mount_data2 lmd2 = {
+		.lmd2_data = data,
+		.lmd2_mnt = NULL
+	};
 
 	return mount_nodev(fs_type, flags, &lmd2, lustre_fill_super);
 }
diff --git a/drivers/staging/lustre/lustre/obdclass/obdo.c b/drivers/staging/lustre/lustre/obdclass/obdo.c
index c9fa36b17919..307ffe347186 100644
--- a/drivers/staging/lustre/lustre/obdclass/obdo.c
+++ b/drivers/staging/lustre/lustre/obdclass/obdo.c
@@ -56,9 +56,9 @@ EXPORT_SYMBOL(obdo_set_parent_fid);
 
 /* WARNING: the file systems must take care not to tinker with
    attributes they don't manage (such as blocks). */
-void obdo_from_inode(struct obdo *dst, struct inode *src, obd_flag valid)
+void obdo_from_inode(struct obdo *dst, struct inode *src, u32 valid)
 {
-	obd_flag newvalid = 0;
+	u32 newvalid = 0;
 
 	if (valid & (OBD_MD_FLCTIME | OBD_MD_FLMTIME))
 		CDEBUG(D_INODE, "valid %x, new time %lu/%lu\n",
@@ -86,7 +86,7 @@ void obdo_from_inode(struct obdo *dst, struct inode *src, obd_flag valid)
 		newvalid |= OBD_MD_FLBLOCKS;
 	}
 	if (valid & OBD_MD_FLBLKSZ) {   /* optimal block size */
-		dst->o_blksize = ll_inode_blksize(src);
+		dst->o_blksize = 1 << src->i_blkbits;
 		newvalid |= OBD_MD_FLBLKSZ;
 	}
 	if (valid & OBD_MD_FLTYPE) {
@@ -108,14 +108,14 @@ void obdo_from_inode(struct obdo *dst, struct inode *src, obd_flag valid)
 		newvalid |= OBD_MD_FLGID;
 	}
 	if (valid & OBD_MD_FLFLAGS) {
-		dst->o_flags = ll_inode_flags(src);
+		dst->o_flags = src->i_flags;
 		newvalid |= OBD_MD_FLFLAGS;
 	}
 	dst->o_valid |= newvalid;
 }
 EXPORT_SYMBOL(obdo_from_inode);
 
-void obdo_cpy_md(struct obdo *dst, struct obdo *src, obd_flag valid)
+void obdo_cpy_md(struct obdo *dst, struct obdo *src, u32 valid)
 {
 	CDEBUG(D_INODE, "src obdo "DOSTID" valid %#llx, dst obdo "DOSTID"\n",
 	       POSTID(&src->o_oi), src->o_valid, POSTID(&dst->o_oi));
@@ -157,43 +157,43 @@ void obdo_cpy_md(struct obdo *dst, struct obdo *src, obd_flag valid)
 EXPORT_SYMBOL(obdo_cpy_md);
 
 /* returns FALSE if comparison (by flags) is same, TRUE if changed */
-int obdo_cmp_md(struct obdo *dst, struct obdo *src, obd_flag compare)
+int obdo_cmp_md(struct obdo *dst, struct obdo *src, u32 compare)
 {
 	int res = 0;
 
-	if ( compare & OBD_MD_FLATIME )
-		res = (res || (dst->o_atime != src->o_atime));
-	if ( compare & OBD_MD_FLMTIME )
-		res = (res || (dst->o_mtime != src->o_mtime));
-	if ( compare & OBD_MD_FLCTIME )
-		res = (res || (dst->o_ctime != src->o_ctime));
-	if ( compare & OBD_MD_FLSIZE )
-		res = (res || (dst->o_size != src->o_size));
-	if ( compare & OBD_MD_FLBLOCKS ) /* allocation of space */
-		res = (res || (dst->o_blocks != src->o_blocks));
-	if ( compare & OBD_MD_FLBLKSZ )
-		res = (res || (dst->o_blksize != src->o_blksize));
-	if ( compare & OBD_MD_FLTYPE )
-		res = (res || (((dst->o_mode ^ src->o_mode) & S_IFMT) != 0));
-	if ( compare & OBD_MD_FLMODE )
-		res = (res || (((dst->o_mode ^ src->o_mode) & ~S_IFMT) != 0));
-	if ( compare & OBD_MD_FLUID )
-		res = (res || (dst->o_uid != src->o_uid));
-	if ( compare & OBD_MD_FLGID )
-		res = (res || (dst->o_gid != src->o_gid));
-	if ( compare & OBD_MD_FLFLAGS )
-		res = (res || (dst->o_flags != src->o_flags));
-	if ( compare & OBD_MD_FLNLINK )
-		res = (res || (dst->o_nlink != src->o_nlink));
-	if ( compare & OBD_MD_FLFID ) {
-		res = (res || (dst->o_parent_seq != src->o_parent_seq));
-		res = (res || (dst->o_parent_ver != src->o_parent_ver));
-	}
-	if ( compare & OBD_MD_FLGENER )
-		res = (res || (dst->o_parent_oid != src->o_parent_oid));
+	if (compare & OBD_MD_FLATIME)
+		res |= dst->o_atime != src->o_atime;
+	if (compare & OBD_MD_FLMTIME)
+		res |= dst->o_mtime != src->o_mtime;
+	if (compare & OBD_MD_FLCTIME)
+		res |= dst->o_ctime != src->o_ctime;
+	if (compare & OBD_MD_FLSIZE)
+		res |= dst->o_size != src->o_size;
+	if (compare & OBD_MD_FLBLOCKS) /* allocation of space */
+		res |= dst->o_blocks != src->o_blocks;
+	if (compare & OBD_MD_FLBLKSZ)
+		res |= dst->o_blksize != src->o_blksize;
+	if (compare & OBD_MD_FLTYPE)
+		res |= ((dst->o_mode ^ src->o_mode) & S_IFMT) != 0;
+	if (compare & OBD_MD_FLMODE)
+		res |= ((dst->o_mode ^ src->o_mode) & ~S_IFMT) != 0;
+	if (compare & OBD_MD_FLUID)
+		res |= dst->o_uid != src->o_uid;
+	if (compare & OBD_MD_FLGID)
+		res |= dst->o_gid != src->o_gid;
+	if (compare & OBD_MD_FLFLAGS)
+		res |= dst->o_flags != src->o_flags;
+	if (compare & OBD_MD_FLNLINK)
+		res |= dst->o_nlink != src->o_nlink;
+	if (compare & OBD_MD_FLFID) {
+		res |= dst->o_parent_seq != src->o_parent_seq;
+		res |= dst->o_parent_ver != src->o_parent_ver;
+	}
+	if (compare & OBD_MD_FLGENER)
+		res |= dst->o_parent_oid != src->o_parent_oid;
 	/* XXX Don't know if these should be included here - wasn't previously
 	if ( compare & OBD_MD_FLINLINE )
-		res = (res || memcmp(dst->o_inline, src->o_inline));
+		res |= memcmp(dst->o_inline, src->o_inline);
 	*/
 	return res;
 }
@@ -247,7 +247,7 @@ void obdo_from_iattr(struct obdo *oa, struct iattr *attr, unsigned int ia_valid)
 }
 EXPORT_SYMBOL(obdo_from_iattr);
 
-void iattr_from_obdo(struct iattr *attr, struct obdo *oa, obd_flag valid)
+void iattr_from_obdo(struct iattr *attr, struct obdo *oa, u32 valid)
 {
 	valid &= oa->o_valid;
 
@@ -296,7 +296,7 @@ void iattr_from_obdo(struct iattr *attr, struct obdo *oa, obd_flag valid)
 }
 EXPORT_SYMBOL(iattr_from_obdo);
 
-void md_from_obdo(struct md_op_data *op_data, struct obdo *oa, obd_flag valid)
+void md_from_obdo(struct md_op_data *op_data, struct obdo *oa, u32 valid)
 {
 	iattr_from_obdo(&op_data->op_attr, oa, valid);
 	if (valid & OBD_MD_FLBLOCKS) {
diff --git a/drivers/staging/lustre/lustre/obdclass/statfs_pack.c b/drivers/staging/lustre/lustre/obdclass/statfs_pack.c
index 33b1a83f4014..cc785ab3f29a 100644
--- a/drivers/staging/lustre/lustre/obdclass/statfs_pack.c
+++ b/drivers/staging/lustre/lustre/obdclass/statfs_pack.c
@@ -40,7 +40,7 @@
 
 #define DEBUG_SUBSYSTEM S_CLASS
 
-
+#include <linux/statfs.h>
 #include "../include/lustre_export.h"
 #include "../include/lustre_net.h"
 #include "../include/obd_support.h"
diff --git a/drivers/staging/lustre/lustre/obdecho/echo.c b/drivers/staging/lustre/lustre/obdecho/echo.c
deleted file mode 100644
index dae1599af384..000000000000
--- a/drivers/staging/lustre/lustre/obdecho/echo.c
+++ /dev/null
@@ -1,671 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License version 2 for more details (a copy is included
- * in the LICENSE file that accompanied this code).
- *
- * You should have received a copy of the GNU General Public License
- * version 2 along with this program; If not, see
- * http://www.sun.com/software/products/lustre/docs/GPLv2.pdf
- *
- * Please contact Sun Microsystems, Inc., 4150 Network Circle, Santa Clara,
- * CA 95054 USA or visit www.sun.com if you need additional information or
- * have any questions.
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2002, 2010, Oracle and/or its affiliates. All rights reserved.
- * Use is subject to license terms.
- *
- * Copyright (c) 2010, 2012, Intel Corporation.
- */
-/*
- * This file is part of Lustre, http://www.lustre.org/
- * Lustre is a trademark of Sun Microsystems, Inc.
- *
- * lustre/obdecho/echo.c
- *
- * Author: Peter Braam <braam@clusterfs.com>
- * Author: Andreas Dilger <adilger@clusterfs.com>
- */
-
-#define DEBUG_SUBSYSTEM S_ECHO
-
-#include "../include/obd_support.h"
-#include "../include/obd_class.h"
-#include "../include/lustre_debug.h"
-#include "../include/lustre_dlm.h"
-#include "../include/lprocfs_status.h"
-
-#include "echo_internal.h"
-
-/* The echo objid needs to be below 2^32, because regular FID numbers are
- * limited to 2^32 objects in f_oid for the FID_SEQ_ECHO range. b=23335 */
-#define ECHO_INIT_OID	0x10000000ULL
-#define ECHO_HANDLE_MAGIC    0xabcd0123fedc9876ULL
-
-#define ECHO_PERSISTENT_PAGES (ECHO_PERSISTENT_SIZE >> PAGE_CACHE_SHIFT)
-static struct page *echo_persistent_pages[ECHO_PERSISTENT_PAGES];
-
-enum {
-	LPROC_ECHO_READ_BYTES = 1,
-	LPROC_ECHO_WRITE_BYTES = 2,
-	LPROC_ECHO_LAST = LPROC_ECHO_WRITE_BYTES +1
-};
-
-static int echo_connect(const struct lu_env *env,
-			struct obd_export **exp, struct obd_device *obd,
-			struct obd_uuid *cluuid, struct obd_connect_data *data,
-			void *localdata)
-{
-	struct lustre_handle conn = { 0 };
-	int rc;
-
-	data->ocd_connect_flags &= ECHO_CONNECT_SUPPORTED;
-	rc = class_connect(&conn, obd, cluuid);
-	if (rc) {
-		CERROR("can't connect %d\n", rc);
-		return rc;
-	}
-	*exp = class_conn2export(&conn);
-
-	return 0;
-}
-
-static int echo_disconnect(struct obd_export *exp)
-{
-	LASSERT (exp != NULL);
-
-	return server_disconnect_export(exp);
-}
-
-static int echo_init_export(struct obd_export *exp)
-{
-	return ldlm_init_export(exp);
-}
-
-static int echo_destroy_export(struct obd_export *exp)
-{
-	target_destroy_export(exp);
-	ldlm_destroy_export(exp);
-
-	return 0;
-}
-
- static __u64 echo_next_id(struct obd_device *obddev)
-{
-	obd_id id;
-
-	spin_lock(&obddev->u.echo.eo_lock);
-	id = ++obddev->u.echo.eo_lastino;
-	spin_unlock(&obddev->u.echo.eo_lock);
-
-	return id;
-}
-
-static int echo_create(const struct lu_env *env, struct obd_export *exp,
-		       struct obdo *oa, struct lov_stripe_md **ea,
-		       struct obd_trans_info *oti)
-{
-	struct obd_device *obd = class_exp2obd(exp);
-
-	if (!obd) {
-		CERROR("invalid client cookie %#llx\n",
-		       exp->exp_handle.h_cookie);
-		return -EINVAL;
-	}
-
-	if (!(oa->o_mode & S_IFMT)) {
-		CERROR("echo obd: no type!\n");
-		return -ENOENT;
-	}
-
-	if (!(oa->o_valid & OBD_MD_FLTYPE)) {
-		CERROR("invalid o_valid %#llx\n", oa->o_valid);
-		return -EINVAL;
-	}
-
-	ostid_set_seq_echo(&oa->o_oi);
-	ostid_set_id(&oa->o_oi, echo_next_id(obd));
-	oa->o_valid = OBD_MD_FLID;
-
-	return 0;
-}
-
-static int echo_destroy(const struct lu_env *env, struct obd_export *exp,
-			struct obdo *oa, struct lov_stripe_md *ea,
-			struct obd_trans_info *oti, struct obd_export *md_exp,
-			void *capa)
-{
-	struct obd_device *obd = class_exp2obd(exp);
-
-	if (!obd) {
-		CERROR("invalid client cookie %#llx\n",
-		       exp->exp_handle.h_cookie);
-		return -EINVAL;
-	}
-
-	if (!(oa->o_valid & OBD_MD_FLID)) {
-		CERROR("obdo missing FLID valid flag: %#llx\n", oa->o_valid);
-		return -EINVAL;
-	}
-
-	if (ostid_id(&oa->o_oi) > obd->u.echo.eo_lastino ||
-	    ostid_id(&oa->o_oi) < ECHO_INIT_OID) {
-		CERROR("bad destroy objid: "DOSTID"\n", POSTID(&oa->o_oi));
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int echo_getattr(const struct lu_env *env, struct obd_export *exp,
-			struct obd_info *oinfo)
-{
-	struct obd_device *obd = class_exp2obd(exp);
-	obd_id id = ostid_id(&oinfo->oi_oa->o_oi);
-
-	if (!obd) {
-		CERROR("invalid client cookie %#llx\n",
-		       exp->exp_handle.h_cookie);
-		return -EINVAL;
-	}
-
-	if (!(oinfo->oi_oa->o_valid & OBD_MD_FLID)) {
-		CERROR("obdo missing FLID valid flag: %#llx\n",
-		       oinfo->oi_oa->o_valid);
-		return -EINVAL;
-	}
-
-	obdo_cpy_md(oinfo->oi_oa, &obd->u.echo.eo_oa, oinfo->oi_oa->o_valid);
-	ostid_set_seq_echo(&oinfo->oi_oa->o_oi);
-	ostid_set_id(&oinfo->oi_oa->o_oi, id);
-
-	return 0;
-}
-
-static int echo_setattr(const struct lu_env *env, struct obd_export *exp,
-			struct obd_info *oinfo, struct obd_trans_info *oti)
-{
-	struct obd_device *obd = class_exp2obd(exp);
-
-	if (!obd) {
-		CERROR("invalid client cookie %#llx\n",
-		       exp->exp_handle.h_cookie);
-		return -EINVAL;
-	}
-
-	if (!(oinfo->oi_oa->o_valid & OBD_MD_FLID)) {
-		CERROR("obdo missing FLID valid flag: %#llx\n",
-		       oinfo->oi_oa->o_valid);
-		return -EINVAL;
-	}
-
-	memcpy(&obd->u.echo.eo_oa, oinfo->oi_oa, sizeof(*oinfo->oi_oa));
-
-	if (ostid_id(&oinfo->oi_oa->o_oi) & 4) {
-		/* Save lock to force ACKed reply */
-		ldlm_lock_addref (&obd->u.echo.eo_nl_lock, LCK_NL);
-		oti->oti_ack_locks[0].mode = LCK_NL;
-		oti->oti_ack_locks[0].lock = obd->u.echo.eo_nl_lock;
-	}
-
-	return 0;
-}
-
-static void
-echo_page_debug_setup(struct page *page, int rw, obd_id id,
-		      __u64 offset, int len)
-{
-	int   page_offset = offset & ~CFS_PAGE_MASK;
-	char *addr	= ((char *)kmap(page)) + page_offset;
-
-	if (len % OBD_ECHO_BLOCK_SIZE != 0)
-		CERROR("Unexpected block size %d\n", len);
-
-	while (len > 0) {
-		if (rw & OBD_BRW_READ)
-			block_debug_setup(addr, OBD_ECHO_BLOCK_SIZE,
-					  offset, id);
-		else
-			block_debug_setup(addr, OBD_ECHO_BLOCK_SIZE,
-					  0xecc0ecc0ecc0ecc0ULL,
-					  0xecc0ecc0ecc0ecc0ULL);
-
-		addr   += OBD_ECHO_BLOCK_SIZE;
-		offset += OBD_ECHO_BLOCK_SIZE;
-		len    -= OBD_ECHO_BLOCK_SIZE;
-	}
-
-	kunmap(page);
-}
-
-static int
-echo_page_debug_check(struct page *page, obd_id id,
-		      __u64 offset, int len)
-{
-	int   page_offset = offset & ~CFS_PAGE_MASK;
-	char *addr	= ((char *)kmap(page)) + page_offset;
-	int   rc	  = 0;
-	int   rc2;
-
-	if (len % OBD_ECHO_BLOCK_SIZE != 0)
-		CERROR("Unexpected block size %d\n", len);
-
-	while (len > 0) {
-		rc2 = block_debug_check("echo", addr, OBD_ECHO_BLOCK_SIZE,
-					offset, id);
-
-		if (rc2 != 0 && rc == 0)
-			rc = rc2;
-
-		addr   += OBD_ECHO_BLOCK_SIZE;
-		offset += OBD_ECHO_BLOCK_SIZE;
-		len    -= OBD_ECHO_BLOCK_SIZE;
-	}
-
-	kunmap(page);
-
-	return (rc);
-}
-
-/* This allows us to verify that desc_private is passed unmolested */
-#define DESC_PRIV 0x10293847
-
-static int echo_map_nb_to_lb(struct obdo *oa, struct obd_ioobj *obj,
-			     struct niobuf_remote *nb, int *pages,
-			     struct niobuf_local *lb, int cmd, int *left)
-{
-	int gfp_mask = (ostid_id(&obj->ioo_oid) & 1) ?
-			GFP_HIGHUSER : GFP_IOFS;
-	int ispersistent = ostid_id(&obj->ioo_oid) == ECHO_PERSISTENT_OBJID;
-	int debug_setup = (!ispersistent &&
-			   (oa->o_valid & OBD_MD_FLFLAGS) != 0 &&
-			   (oa->o_flags & OBD_FL_DEBUG_CHECK) != 0);
-	struct niobuf_local *res = lb;
-	obd_off offset = nb->offset;
-	int len = nb->len;
-
-	while (len > 0) {
-		int plen = PAGE_CACHE_SIZE - (offset & (PAGE_CACHE_SIZE-1));
-		if (len < plen)
-			plen = len;
-
-		/* check for local buf overflow */
-		if (*left == 0)
-			return -EINVAL;
-
-		res->lnb_file_offset = offset;
-		res->len = plen;
-		LASSERT((res->lnb_file_offset & ~CFS_PAGE_MASK) + res->len <=
-			PAGE_CACHE_SIZE);
-
-		if (ispersistent &&
-		    ((res->lnb_file_offset >> PAGE_CACHE_SHIFT) <
-		      ECHO_PERSISTENT_PAGES)) {
-			res->page =
-				echo_persistent_pages[res->lnb_file_offset >>
-						      PAGE_CACHE_SHIFT];
-			/* Take extra ref so __free_pages() can be called OK */
-			get_page (res->page);
-		} else {
-			OBD_PAGE_ALLOC(res->page, gfp_mask);
-			if (res->page == NULL) {
-				CERROR("can't get page for id " DOSTID"\n",
-				       POSTID(&obj->ioo_oid));
-				return -ENOMEM;
-			}
-		}
-
-		CDEBUG(D_PAGE, "$$$$ get page %p @ %llu for %d\n",
-		       res->page, res->lnb_file_offset, res->len);
-
-		if (cmd & OBD_BRW_READ)
-			res->rc = res->len;
-
-		if (debug_setup)
-			echo_page_debug_setup(res->page, cmd,
-					      ostid_id(&obj->ioo_oid),
-					      res->lnb_file_offset, res->len);
-
-		offset += plen;
-		len -= plen;
-		res++;
-
-		(*left)--;
-		(*pages)++;
-	}
-
-	return 0;
-}
-
-static int echo_finalize_lb(struct obdo *oa, struct obd_ioobj *obj,
-			    struct niobuf_remote *rb, int *pgs,
-			    struct niobuf_local *lb, int verify)
-{
-	struct niobuf_local *res = lb;
-	obd_off start  = rb->offset >> PAGE_CACHE_SHIFT;
-	obd_off end    = (rb->offset + rb->len + PAGE_CACHE_SIZE - 1) >> PAGE_CACHE_SHIFT;
-	int     count  = (int)(end - start);
-	int     rc     = 0;
-	int     i;
-
-	for (i = 0; i < count; i++, (*pgs) ++, res++) {
-		struct page *page = res->page;
-		void       *addr;
-
-		if (page == NULL) {
-			CERROR("null page objid %llu:%p, buf %d/%d\n",
-			       ostid_id(&obj->ioo_oid), page, i,
-			       obj->ioo_bufcnt);
-			return -EFAULT;
-		}
-
-		addr = kmap(page);
-
-		CDEBUG(D_PAGE, "$$$$ use page %p, addr %p@%llu\n",
-		       res->page, addr, res->lnb_file_offset);
-
-		if (verify) {
-			int vrc = echo_page_debug_check(page,
-							ostid_id(&obj->ioo_oid),
-							res->lnb_file_offset,
-							res->len);
-			/* check all the pages always */
-			if (vrc != 0 && rc == 0)
-				rc = vrc;
-		}
-
-		kunmap(page);
-		/* NB see comment above regarding persistent pages */
-		OBD_PAGE_FREE(page);
-	}
-
-	return rc;
-}
-
-static int echo_preprw(const struct lu_env *env, int cmd,
-		       struct obd_export *export, struct obdo *oa,
-		       int objcount, struct obd_ioobj *obj,
-		       struct niobuf_remote *nb, int *pages,
-		       struct niobuf_local *res, struct obd_trans_info *oti,
-		       struct lustre_capa *unused)
-{
-	struct obd_device *obd;
-	int tot_bytes = 0;
-	int rc = 0;
-	int i, left;
-
-	obd = export->exp_obd;
-	if (obd == NULL)
-		return -EINVAL;
-
-	/* Temp fix to stop falling foul of osc_announce_cached() */
-	oa->o_valid &= ~(OBD_MD_FLBLOCKS | OBD_MD_FLGRANT);
-
-	memset(res, 0, sizeof(*res) * *pages);
-
-	CDEBUG(D_PAGE, "%s %d obdos with %d IOs\n",
-	       cmd == OBD_BRW_READ ? "reading" : "writing", objcount, *pages);
-
-	if (oti)
-		oti->oti_handle = (void *)DESC_PRIV;
-
-	left = *pages;
-	*pages = 0;
-
-	for (i = 0; i < objcount; i++, obj++) {
-		int j;
-
-		for (j = 0 ; j < obj->ioo_bufcnt ; j++, nb++) {
-
-			rc = echo_map_nb_to_lb(oa, obj, nb, pages,
-					       res + *pages, cmd, &left);
-			if (rc)
-				GOTO(preprw_cleanup, rc);
-
-			tot_bytes += nb->len;
-		}
-	}
-
-	atomic_add(*pages, &obd->u.echo.eo_prep);
-
-	if (cmd & OBD_BRW_READ)
-		lprocfs_counter_add(obd->obd_stats, LPROC_ECHO_READ_BYTES,
-				    tot_bytes);
-	else
-		lprocfs_counter_add(obd->obd_stats, LPROC_ECHO_WRITE_BYTES,
-				    tot_bytes);
-
-	CDEBUG(D_PAGE, "%d pages allocated after prep\n",
-	       atomic_read(&obd->u.echo.eo_prep));
-
-	return 0;
-
-preprw_cleanup:
-	/* It is possible that we would rather handle errors by  allow
-	 * any already-set-up pages to complete, rather than tearing them
-	 * all down again.  I believe that this is what the in-kernel
-	 * prep/commit operations do.
-	 */
-	CERROR("cleaning up %u pages (%d obdos)\n", *pages, objcount);
-	for (i = 0; i < *pages; i++) {
-		kunmap(res[i].page);
-		/* NB if this is a persistent page, __free_pages will just
-		 * lose the extra ref gained above */
-		OBD_PAGE_FREE(res[i].page);
-		res[i].page = NULL;
-		atomic_dec(&obd->u.echo.eo_prep);
-	}
-
-	return rc;
-}
-
-static int echo_commitrw(const struct lu_env *env, int cmd,
-			 struct obd_export *export, struct obdo *oa,
-			 int objcount, struct obd_ioobj *obj,
-			 struct niobuf_remote *rb, int niocount,
-			 struct niobuf_local *res, struct obd_trans_info *oti,
-			 int rc)
-{
-	struct obd_device *obd;
-	int pgs = 0;
-	int i;
-
-	obd = export->exp_obd;
-	if (obd == NULL)
-		return -EINVAL;
-
-	if (rc)
-		GOTO(commitrw_cleanup, rc);
-
-	if ((cmd & OBD_BRW_RWMASK) == OBD_BRW_READ) {
-		CDEBUG(D_PAGE, "reading %d obdos with %d IOs\n",
-		       objcount, niocount);
-	} else {
-		CDEBUG(D_PAGE, "writing %d obdos with %d IOs\n",
-		       objcount, niocount);
-	}
-
-	if (niocount && res == NULL) {
-		CERROR("NULL res niobuf with niocount %d\n", niocount);
-		return -EINVAL;
-	}
-
-	LASSERT(oti == NULL || oti->oti_handle == (void *)DESC_PRIV);
-
-	for (i = 0; i < objcount; i++, obj++) {
-		int verify = (rc == 0 &&
-			     ostid_id(&obj->ioo_oid) != ECHO_PERSISTENT_OBJID &&
-			      (oa->o_valid & OBD_MD_FLFLAGS) != 0 &&
-			      (oa->o_flags & OBD_FL_DEBUG_CHECK) != 0);
-		int j;
-
-		for (j = 0 ; j < obj->ioo_bufcnt ; j++, rb++) {
-			int vrc = echo_finalize_lb(oa, obj, rb, &pgs, &res[pgs],
-						   verify);
-			if (vrc == 0)
-				continue;
-
-			if (vrc == -EFAULT)
-				GOTO(commitrw_cleanup, rc = vrc);
-
-			if (rc == 0)
-				rc = vrc;
-		}
-
-	}
-
-	atomic_sub(pgs, &obd->u.echo.eo_prep);
-
-	CDEBUG(D_PAGE, "%d pages remain after commit\n",
-	       atomic_read(&obd->u.echo.eo_prep));
-	return rc;
-
-commitrw_cleanup:
-	atomic_sub(pgs, &obd->u.echo.eo_prep);
-
-	CERROR("cleaning up %d pages (%d obdos)\n",
-	       niocount - pgs - 1, objcount);
-
-	while (pgs < niocount) {
-		struct page *page = res[pgs++].page;
-
-		if (page == NULL)
-			continue;
-
-		/* NB see comment above regarding persistent pages */
-		OBD_PAGE_FREE(page);
-		atomic_dec(&obd->u.echo.eo_prep);
-	}
-	return rc;
-}
-
-static int echo_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
-{
-	struct lprocfs_static_vars lvars;
-	int			rc;
-	__u64		      lock_flags = 0;
-	struct ldlm_res_id	 res_id = {.name = {1}};
-	char		       ns_name[48];
-
-	obd->u.echo.eo_obt.obt_magic = OBT_MAGIC;
-	spin_lock_init(&obd->u.echo.eo_lock);
-	obd->u.echo.eo_lastino = ECHO_INIT_OID;
-
-	sprintf(ns_name, "echotgt-%s", obd->obd_uuid.uuid);
-	obd->obd_namespace = ldlm_namespace_new(obd, ns_name,
-						LDLM_NAMESPACE_SERVER,
-						LDLM_NAMESPACE_MODEST,
-						LDLM_NS_TYPE_OST);
-	if (obd->obd_namespace == NULL) {
-		LBUG();
-		return -ENOMEM;
-	}
-
-	rc = ldlm_cli_enqueue_local(obd->obd_namespace, &res_id, LDLM_PLAIN,
-				    NULL, LCK_NL, &lock_flags, NULL,
-				    ldlm_completion_ast, NULL, NULL, 0,
-				    LVB_T_NONE, NULL, &obd->u.echo.eo_nl_lock);
-	LASSERT (rc == ELDLM_OK);
-
-	lprocfs_echo_init_vars(&lvars);
-	if (lprocfs_obd_setup(obd, lvars.obd_vars) == 0 &&
-	    lprocfs_alloc_obd_stats(obd, LPROC_ECHO_LAST) == 0) {
-		lprocfs_counter_init(obd->obd_stats, LPROC_ECHO_READ_BYTES,
-				     LPROCFS_CNTR_AVGMINMAX,
-				     "read_bytes", "bytes");
-		lprocfs_counter_init(obd->obd_stats, LPROC_ECHO_WRITE_BYTES,
-				     LPROCFS_CNTR_AVGMINMAX,
-				     "write_bytes", "bytes");
-	}
-
-	ptlrpc_init_client (LDLM_CB_REQUEST_PORTAL, LDLM_CB_REPLY_PORTAL,
-			    "echo_ldlm_cb_client", &obd->obd_ldlm_client);
-	return 0;
-}
-
-static int echo_cleanup(struct obd_device *obd)
-{
-	int leaked;
-
-	lprocfs_obd_cleanup(obd);
-	lprocfs_free_obd_stats(obd);
-
-	ldlm_lock_decref(&obd->u.echo.eo_nl_lock, LCK_NL);
-
-	/* XXX Bug 3413; wait for a bit to ensure the BL callback has
-	 * happened before calling ldlm_namespace_free() */
-	set_current_state(TASK_UNINTERRUPTIBLE);
-	schedule_timeout(cfs_time_seconds(1));
-
-	ldlm_namespace_free(obd->obd_namespace, NULL, obd->obd_force);
-	obd->obd_namespace = NULL;
-
-	leaked = atomic_read(&obd->u.echo.eo_prep);
-	if (leaked != 0)
-		CERROR("%d prep/commitrw pages leaked\n", leaked);
-
-	return 0;
-}
-
-struct obd_ops echo_obd_ops = {
-	.o_owner	   = THIS_MODULE,
-	.o_connect	 = echo_connect,
-	.o_disconnect      = echo_disconnect,
-	.o_init_export     = echo_init_export,
-	.o_destroy_export  = echo_destroy_export,
-	.o_create	  = echo_create,
-	.o_destroy	 = echo_destroy,
-	.o_getattr	 = echo_getattr,
-	.o_setattr	 = echo_setattr,
-	.o_preprw	  = echo_preprw,
-	.o_commitrw	= echo_commitrw,
-	.o_setup	   = echo_setup,
-	.o_cleanup	 = echo_cleanup
-};
-
-void echo_persistent_pages_fini(void)
-{
-	int     i;
-
-	for (i = 0; i < ECHO_PERSISTENT_PAGES; i++)
-		if (echo_persistent_pages[i] != NULL) {
-			OBD_PAGE_FREE(echo_persistent_pages[i]);
-			echo_persistent_pages[i] = NULL;
-		}
-}
-
-int echo_persistent_pages_init(void)
-{
-	struct page *pg;
-	int	  i;
-
-	for (i = 0; i < ECHO_PERSISTENT_PAGES; i++) {
-		int gfp_mask = (i < ECHO_PERSISTENT_PAGES/2) ?
-			GFP_IOFS : GFP_HIGHUSER;
-
-		OBD_PAGE_ALLOC(pg, gfp_mask);
-		if (pg == NULL) {
-			echo_persistent_pages_fini ();
-			return (-ENOMEM);
-		}
-
-		memset (kmap (pg), 0, PAGE_CACHE_SIZE);
-		kunmap (pg);
-
-		echo_persistent_pages[i] = pg;
-	}
-
-	return (0);
-}
diff --git a/drivers/staging/lustre/lustre/obdecho/echo_client.c b/drivers/staging/lustre/lustre/obdecho/echo_client.c
index f1847f3f579d..98e4290919d0 100644
--- a/drivers/staging/lustre/lustre/obdecho/echo_client.c
+++ b/drivers/staging/lustre/lustre/obdecho/echo_client.c
@@ -43,7 +43,6 @@
 #include "../include/lustre_debug.h"
 #include "../include/lprocfs_status.h"
 #include "../include/cl_object.h"
-#include "../include/md_object.h"
 #include "../include/lustre_fid.h"
 #include "../include/lustre_acl.h"
 #include "../include/lustre_net.h"
@@ -62,8 +61,6 @@ struct echo_device {
 	struct cl_site	 *ed_site;
 	struct lu_device       *ed_next;
 	int		     ed_next_islov;
-	int		     ed_next_ismd;
-	struct lu_client_seq   *ed_cl_seq;
 };
 
 struct echo_object {
@@ -96,16 +93,6 @@ struct echo_lock {
 	atomic_t	   el_refcount;
 };
 
-struct echo_io {
-	struct cl_io_slice     ei_cl;
-};
-
-#if 0
-struct echo_req {
-	struct cl_req_slice er_cl;
-};
-#endif
-
 static int echo_client_setup(const struct lu_env *env,
 			     struct obd_device *obddev,
 			     struct lustre_cfg *lcfg);
@@ -175,10 +162,10 @@ struct echo_object_conf *cl2echo_conf(const struct cl_object_conf *c)
 static struct echo_object *cl_echo_object_find(struct echo_device *d,
 					       struct lov_stripe_md **lsm);
 static int cl_echo_object_put(struct echo_object *eco);
-static int cl_echo_enqueue   (struct echo_object *eco, obd_off start,
-			      obd_off end, int mode, __u64 *cookie);
-static int cl_echo_cancel    (struct echo_device *d, __u64 cookie);
-static int cl_echo_object_brw(struct echo_object *eco, int rw, obd_off offset,
+static int cl_echo_enqueue(struct echo_object *eco, u64 start,
+			   u64 end, int mode, __u64 *cookie);
+static int cl_echo_cancel(struct echo_device *d, __u64 cookie);
+static int cl_echo_object_brw(struct echo_object *eco, int rw, u64 offset,
 			      struct page **pages, int npages, int async);
 
 static struct echo_thread_info *echo_env_info(const struct lu_env *env);
@@ -192,17 +179,6 @@ struct echo_thread_info {
 	struct cl_lock_descr    eti_descr;
 	struct lu_fid	   eti_fid;
 	struct lu_fid		eti_fid2;
-	struct md_op_spec       eti_spec;
-	struct lov_mds_md_v3    eti_lmm;
-	struct lov_user_md_v3   eti_lum;
-	struct md_attr	  eti_ma;
-	struct lu_name	  eti_lname;
-	/* per-thread values, can be re-used */
-	void			*eti_big_lmm;
-	int			eti_big_lmmsize;
-	char		    eti_name[20];
-	struct lu_buf	   eti_buf;
-	char		    eti_xattr_buf[LUSTRE_POSIX_ACL_MAX_SIZE];
 };
 
 /* No session used right now */
@@ -214,7 +190,6 @@ static struct kmem_cache *echo_lock_kmem;
 static struct kmem_cache *echo_object_kmem;
 static struct kmem_cache *echo_thread_kmem;
 static struct kmem_cache *echo_session_kmem;
-//static struct kmem_cache *echo_req_kmem;
 
 static struct lu_kmem_descr echo_caches[] = {
 	{
@@ -237,13 +212,6 @@ static struct lu_kmem_descr echo_caches[] = {
 		.ckd_name  = "echo_session_kmem",
 		.ckd_size  = sizeof (struct echo_session_info)
 	},
-#if 0
-	{
-		.ckd_cache = &echo_req_kmem,
-		.ckd_name  = "echo_req_kmem",
-		.ckd_size  = sizeof (struct echo_req)
-	},
-#endif
 	{
 		.ckd_cache = NULL
 	}
@@ -463,6 +431,8 @@ static int echo_object_init(const struct lu_env *env, struct lu_object *obj,
 	struct echo_device *ed	 = cl2echo_dev(lu2cl_dev(obj->lo_dev));
 	struct echo_client_obd *ec     = ed->ed_ec;
 	struct echo_object *eco	= cl2echo_obj(lu2cl(obj));
+	const struct cl_object_conf *cconf;
+	struct echo_object_conf *econf;
 
 	if (ed->ed_next) {
 		struct lu_object  *below;
@@ -476,17 +446,13 @@ static int echo_object_init(const struct lu_env *env, struct lu_object *obj,
 		lu_object_add(obj, below);
 	}
 
-	if (!ed->ed_next_ismd) {
-		const struct cl_object_conf *cconf = lu2cl_conf(conf);
-		struct echo_object_conf *econf = cl2echo_conf(cconf);
+	cconf = lu2cl_conf(conf);
+	econf = cl2echo_conf(cconf);
 
-		LASSERT(econf->eoc_md);
-		eco->eo_lsm = *econf->eoc_md;
-		/* clear the lsm pointer so that it won't get freed. */
-		*econf->eoc_md = NULL;
-	} else {
-		eco->eo_lsm = NULL;
-	}
+	LASSERT(econf->eoc_md);
+	eco->eo_lsm = *econf->eoc_md;
+	/* clear the lsm pointer so that it won't get freed. */
+	*econf->eoc_md = NULL;
 
 	eco->eo_dev = ed;
 	atomic_set(&eco->eo_npages, 0);
@@ -651,8 +617,7 @@ static int echo_site_init(const struct lu_env *env, struct echo_device *ed)
 static void echo_site_fini(const struct lu_env *env, struct echo_device *ed)
 {
 	if (ed->ed_site) {
-		if (!ed->ed_next_ismd)
-			cl_site_fini(ed->ed_site);
+		cl_site_fini(ed->ed_site);
 		ed->ed_site = NULL;
 	}
 }
@@ -719,53 +684,6 @@ static struct lu_context_key echo_session_key = {
 
 LU_TYPE_INIT_FINI(echo, &echo_thread_key, &echo_session_key);
 
-#define ECHO_SEQ_WIDTH 0xffffffff
-static int echo_fid_init(struct echo_device *ed, char *obd_name,
-			 struct seq_server_site *ss)
-{
-	char *prefix;
-	int rc;
-
-	OBD_ALLOC_PTR(ed->ed_cl_seq);
-	if (ed->ed_cl_seq == NULL)
-		return -ENOMEM;
-
-	OBD_ALLOC(prefix, MAX_OBD_NAME + 5);
-	if (prefix == NULL)
-		GOTO(out_free_seq, rc = -ENOMEM);
-
-	snprintf(prefix, MAX_OBD_NAME + 5, "srv-%s", obd_name);
-
-	/* Init client side sequence-manager */
-	rc = seq_client_init(ed->ed_cl_seq, NULL,
-			     LUSTRE_SEQ_METADATA,
-			     prefix, ss->ss_server_seq);
-	ed->ed_cl_seq->lcs_width = ECHO_SEQ_WIDTH;
-	OBD_FREE(prefix, MAX_OBD_NAME + 5);
-	if (rc)
-		GOTO(out_free_seq, rc);
-
-	return 0;
-
-out_free_seq:
-	OBD_FREE_PTR(ed->ed_cl_seq);
-	ed->ed_cl_seq = NULL;
-	return rc;
-}
-
-static int echo_fid_fini(struct obd_device *obddev)
-{
-	struct echo_device *ed = obd2echo_dev(obddev);
-
-	if (ed->ed_cl_seq != NULL) {
-		seq_client_fini(ed->ed_cl_seq);
-		OBD_FREE_PTR(ed->ed_cl_seq);
-		ed->ed_cl_seq = NULL;
-	}
-
-	return 0;
-}
-
 static struct lu_device *echo_device_alloc(const struct lu_env *env,
 					   struct lu_device_type *t,
 					   struct lustre_cfg *cfg)
@@ -806,13 +724,14 @@ static struct lu_device *echo_device_alloc(const struct lu_env *env,
 
 	next = tgt->obd_lu_dev;
 	if (!strcmp(tgt->obd_type->typ_name, LUSTRE_MDT_NAME)) {
-		ed->ed_next_ismd = 1;
-	} else {
-		ed->ed_next_ismd = 0;
-		rc = echo_site_init(env, ed);
-		if (rc)
-			GOTO(out, rc);
+		CERROR("echo MDT client must be run on server\n");
+		GOTO(out, rc = -EOPNOTSUPP);
 	}
+
+	rc = echo_site_init(env, ed);
+	if (rc)
+		GOTO(out, rc);
+
 	cleanup = 3;
 
 	rc = echo_client_setup(env, obd, cfg);
@@ -822,88 +741,40 @@ static struct lu_device *echo_device_alloc(const struct lu_env *env,
 	ed->ed_ec = &obd->u.echo_client;
 	cleanup = 4;
 
-	if (ed->ed_next_ismd) {
-		/* Suppose to connect to some Metadata layer */
-		struct lu_site *ls;
-		struct lu_device *ld;
-		int    found = 0;
-
-		if (next == NULL) {
-			CERROR("%s is not lu device type!\n",
-			       lustre_cfg_string(cfg, 1));
-			GOTO(out, rc = -EINVAL);
-		}
-
-		tgt_type_name = lustre_cfg_string(cfg, 2);
-		if (!tgt_type_name) {
-			CERROR("%s no type name for echo %s setup\n",
-				lustre_cfg_string(cfg, 1),
-				tgt->obd_type->typ_name);
-			GOTO(out, rc = -EINVAL);
-		}
-
-		ls = next->ld_site;
-
-		spin_lock(&ls->ls_ld_lock);
-		list_for_each_entry(ld, &ls->ls_ld_linkage, ld_linkage) {
-			if (strcmp(ld->ld_type->ldt_name, tgt_type_name) == 0) {
-				found = 1;
-				break;
-			}
-		}
-		spin_unlock(&ls->ls_ld_lock);
-
-		if (found == 0) {
-			CERROR("%s is not lu device type!\n",
-			       lustre_cfg_string(cfg, 1));
-			GOTO(out, rc = -EINVAL);
-		}
-
-		next = ld;
-		/* For MD echo client, it will use the site in MDS stack */
-		ed->ed_site_myself.cs_lu = *ls;
-		ed->ed_site = &ed->ed_site_myself;
-		ed->ed_cl.cd_lu_dev.ld_site = &ed->ed_site_myself.cs_lu;
-		rc = echo_fid_init(ed, obd->obd_name, lu_site2seq(ls));
-		if (rc) {
-			CERROR("echo fid init error %d\n", rc);
+	/* if echo client is to be stacked upon ost device, the next is
+	 * NULL since ost is not a clio device so far */
+	if (next != NULL && !lu_device_is_cl(next))
+		next = NULL;
+
+	tgt_type_name = tgt->obd_type->typ_name;
+	if (next != NULL) {
+		LASSERT(next != NULL);
+		if (next->ld_site != NULL)
+			GOTO(out, rc = -EBUSY);
+
+		next->ld_site = &ed->ed_site->cs_lu;
+		rc = next->ld_type->ldt_ops->ldto_device_init(env, next,
+						next->ld_type->ldt_name,
+							      NULL);
+		if (rc)
 			GOTO(out, rc);
-		}
+
+		/* Tricky case, I have to determine the obd type since
+		 * CLIO uses the different parameters to initialize
+		 * objects for lov & osc. */
+		if (strcmp(tgt_type_name, LUSTRE_LOV_NAME) == 0)
+			ed->ed_next_islov = 1;
+		else
+			LASSERT(strcmp(tgt_type_name,
+				       LUSTRE_OSC_NAME) == 0);
 	} else {
-		 /* if echo client is to be stacked upon ost device, the next is
-		  * NULL since ost is not a clio device so far */
-		if (next != NULL && !lu_device_is_cl(next))
-			next = NULL;
-
-		tgt_type_name = tgt->obd_type->typ_name;
-		if (next != NULL) {
-			LASSERT(next != NULL);
-			if (next->ld_site != NULL)
-				GOTO(out, rc = -EBUSY);
-
-			next->ld_site = &ed->ed_site->cs_lu;
-			rc = next->ld_type->ldt_ops->ldto_device_init(env, next,
-						     next->ld_type->ldt_name,
-						     NULL);
-			if (rc)
-				GOTO(out, rc);
-
-			/* Tricky case, I have to determine the obd type since
-			 * CLIO uses the different parameters to initialize
-			 * objects for lov & osc. */
-			if (strcmp(tgt_type_name, LUSTRE_LOV_NAME) == 0)
-				ed->ed_next_islov = 1;
-			else
-				LASSERT(strcmp(tgt_type_name,
-					       LUSTRE_OSC_NAME) == 0);
-		} else
-			LASSERT(strcmp(tgt_type_name, LUSTRE_OST_NAME) == 0);
+		LASSERT(strcmp(tgt_type_name, LUSTRE_OST_NAME) == 0);
 	}
 
 	ed->ed_next = next;
 	return &cd->cd_lu_dev;
 out:
-	switch(cleanup) {
+	switch (cleanup) {
 	case 4: {
 		int rc2;
 		rc2 = echo_client_cleanup(obd);
@@ -922,7 +793,7 @@ out:
 	default:
 		break;
 	}
-	return(ERR_PTR(rc));
+	return ERR_PTR(rc);
 }
 
 static int echo_device_init(const struct lu_env *env, struct lu_device *d,
@@ -938,7 +809,7 @@ static struct lu_device *echo_device_fini(const struct lu_env *env,
 	struct echo_device *ed = cl2echo_dev(lu2cl_dev(d));
 	struct lu_device *next = ed->ed_next;
 
-	while (next && !ed->ed_next_ismd)
+	while (next)
 		next = next->ld_type->ldt_ops->ldto_device_fini(env, next);
 	return NULL;
 }
@@ -1008,8 +879,8 @@ static struct lu_device *echo_device_free(const struct lu_env *env,
 	CDEBUG(D_INFO, "No object exists, exiting...\n");
 
 	echo_client_cleanup(d->ld_obd);
-	echo_fid_fini(d->ld_obd);
-	while (next && !ed->ed_next_ismd)
+
+	while (next)
 		next = next->ld_type->ldt_ops->ldto_device_free(env, next);
 
 	LASSERT(ed->ed_site == lu2cl_site(d->ld_site));
@@ -1037,7 +908,7 @@ static struct lu_device_type echo_device_type = {
 	.ldt_tags     = LU_DEVICE_CL,
 	.ldt_name     = LUSTRE_ECHO_CLIENT_NAME,
 	.ldt_ops      = &echo_device_type_ops,
-	.ldt_ctx_tags = LCT_CL_THREAD | LCT_MD_THREAD | LCT_DT_THREAD,
+	.ldt_ctx_tags = LCT_CL_THREAD,
 };
 /** @} echo_init */
 
@@ -1141,7 +1012,7 @@ static int cl_echo_object_put(struct echo_object *eco)
 }
 
 static int cl_echo_enqueue0(struct lu_env *env, struct echo_object *eco,
-			    obd_off start, obd_off end, int mode,
+			    u64 start, u64 end, int mode,
 			    __u64 *cookie , __u32 enqflags)
 {
 	struct cl_io *io;
@@ -1186,7 +1057,7 @@ static int cl_echo_enqueue0(struct lu_env *env, struct echo_object *eco,
 	return rc;
 }
 
-static int cl_echo_enqueue(struct echo_object *eco, obd_off start, obd_off end,
+static int cl_echo_enqueue(struct echo_object *eco, u64 start, u64 end,
 			   int mode, __u64 *cookie)
 {
 	struct echo_thread_info *info;
@@ -1280,7 +1151,7 @@ static int cl_echo_async_brw(const struct lu_env *env, struct cl_io *io,
 	return result;
 }
 
-static int cl_echo_object_brw(struct echo_object *eco, int rw, obd_off offset,
+static int cl_echo_object_brw(struct echo_object *eco, int rw, u64 offset,
 			      struct page **pages, int npages, int async)
 {
 	struct lu_env	   *env;
@@ -1374,7 +1245,7 @@ out:
 /** @} echo_exports */
 
 
-static obd_id last_object_id;
+static u64 last_object_id;
 
 static int
 echo_copyout_lsm (struct lov_stripe_md *lsm, void *_ulsm, int ulsm_nob)
@@ -1384,15 +1255,15 @@ echo_copyout_lsm (struct lov_stripe_md *lsm, void *_ulsm, int ulsm_nob)
 
 	nob = offsetof (struct lov_stripe_md, lsm_oinfo[lsm->lsm_stripe_count]);
 	if (nob > ulsm_nob)
-		return (-EINVAL);
+		return -EINVAL;
 
 	if (copy_to_user (ulsm, lsm, sizeof(*ulsm)))
-		return (-EFAULT);
+		return -EFAULT;
 
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
 		if (copy_to_user (ulsm->lsm_oinfo[i], lsm->lsm_oinfo[i],
 				      sizeof(lsm->lsm_oinfo[0])))
-			return (-EFAULT);
+			return -EFAULT;
 	}
 	return 0;
 }
@@ -1405,16 +1276,16 @@ echo_copyin_lsm (struct echo_device *ed, struct lov_stripe_md *lsm,
 	int		     i;
 
 	if (ulsm_nob < sizeof (*lsm))
-		return (-EINVAL);
+		return -EINVAL;
 
 	if (copy_from_user (lsm, ulsm, sizeof (*lsm)))
-		return (-EFAULT);
+		return -EFAULT;
 
 	if (lsm->lsm_stripe_count > ec->ec_nstripes ||
 	    lsm->lsm_magic != LOV_MAGIC ||
 	    (lsm->lsm_stripe_size & (~CFS_PAGE_MASK)) != 0 ||
 	    ((__u64)lsm->lsm_stripe_size * lsm->lsm_stripe_count > ~0UL))
-		return (-EINVAL);
+		return -EINVAL;
 
 
 	for (i = 0; i < lsm->lsm_stripe_count; i++) {
@@ -1422,778 +1293,11 @@ echo_copyin_lsm (struct echo_device *ed, struct lov_stripe_md *lsm,
 				       ((struct lov_stripe_md *)ulsm)-> \
 				       lsm_oinfo[i],
 				       sizeof(lsm->lsm_oinfo[0])))
-			return (-EFAULT);
-	}
-	return (0);
-}
-
-static inline void echo_md_build_name(struct lu_name *lname, char *name,
-				      __u64 id)
-{
-	sprintf(name, "%llu", id);
-	lname->ln_name = name;
-	lname->ln_namelen = strlen(name);
-}
-
-/* similar to mdt_attr_get_complex */
-static int echo_big_lmm_get(const struct lu_env *env, struct md_object *o,
-			    struct md_attr *ma)
-{
-	struct echo_thread_info	*info = echo_env_info(env);
-	int			 rc;
-
-	LASSERT(ma->ma_lmm_size > 0);
-
-	rc = mo_xattr_get(env, o, &LU_BUF_NULL, XATTR_NAME_LOV);
-	if (rc < 0)
-		return rc;
-
-	/* big_lmm may need to be grown */
-	if (info->eti_big_lmmsize < rc) {
-		int size = size_roundup_power2(rc);
-
-		if (info->eti_big_lmmsize > 0) {
-			/* free old buffer */
-			LASSERT(info->eti_big_lmm);
-			OBD_FREE_LARGE(info->eti_big_lmm,
-				       info->eti_big_lmmsize);
-			info->eti_big_lmm = NULL;
-			info->eti_big_lmmsize = 0;
-		}
-
-		OBD_ALLOC_LARGE(info->eti_big_lmm, size);
-		if (info->eti_big_lmm == NULL)
-			return -ENOMEM;
-		info->eti_big_lmmsize = size;
-	}
-	LASSERT(info->eti_big_lmmsize >= rc);
-
-	info->eti_buf.lb_buf = info->eti_big_lmm;
-	info->eti_buf.lb_len = info->eti_big_lmmsize;
-	rc = mo_xattr_get(env, o, &info->eti_buf, XATTR_NAME_LOV);
-	if (rc < 0)
-		return rc;
-
-	ma->ma_valid |= MA_LOV;
-	ma->ma_lmm = info->eti_big_lmm;
-	ma->ma_lmm_size = rc;
-
-	return 0;
-}
-
-int echo_attr_get_complex(const struct lu_env *env, struct md_object *next,
-			  struct md_attr *ma)
-{
-	struct echo_thread_info	*info = echo_env_info(env);
-	struct lu_buf		*buf = &info->eti_buf;
-	umode_t		 mode = lu_object_attr(&next->mo_lu);
-	int			 need = ma->ma_need;
-	int			 rc = 0, rc2;
-
-	ma->ma_valid = 0;
-
-	if (need & MA_INODE) {
-		ma->ma_need = MA_INODE;
-		rc = mo_attr_get(env, next, ma);
-		if (rc)
-			GOTO(out, rc);
-		ma->ma_valid |= MA_INODE;
-	}
-
-	if (need & MA_LOV) {
-		if (S_ISREG(mode) || S_ISDIR(mode)) {
-			LASSERT(ma->ma_lmm_size > 0);
-			buf->lb_buf = ma->ma_lmm;
-			buf->lb_len = ma->ma_lmm_size;
-			rc2 = mo_xattr_get(env, next, buf, XATTR_NAME_LOV);
-			if (rc2 > 0) {
-				ma->ma_lmm_size = rc2;
-				ma->ma_valid |= MA_LOV;
-			} else if (rc2 == -ENODATA) {
-				/* no LOV EA */
-				ma->ma_lmm_size = 0;
-			} else if (rc2 == -ERANGE) {
-				rc2 = echo_big_lmm_get(env, next, ma);
-				if (rc2 < 0)
-					GOTO(out, rc = rc2);
-			} else {
-				GOTO(out, rc = rc2);
-			}
-		}
-	}
-
-#ifdef CONFIG_FS_POSIX_ACL
-	if (need & MA_ACL_DEF && S_ISDIR(mode)) {
-		buf->lb_buf = ma->ma_acl;
-		buf->lb_len = ma->ma_acl_size;
-		rc2 = mo_xattr_get(env, next, buf, XATTR_NAME_ACL_DEFAULT);
-		if (rc2 > 0) {
-			ma->ma_acl_size = rc2;
-			ma->ma_valid |= MA_ACL_DEF;
-		} else if (rc2 == -ENODATA) {
-			/* no ACLs */
-			ma->ma_acl_size = 0;
-		} else {
-			GOTO(out, rc = rc2);
-		}
-	}
-#endif
-out:
-	ma->ma_need = need;
-	CDEBUG(D_INODE, "after getattr rc = %d, ma_valid = %#llx ma_lmm=%p\n",
-	       rc, ma->ma_valid, ma->ma_lmm);
-	return rc;
-}
-
-static int
-echo_md_create_internal(const struct lu_env *env, struct echo_device *ed,
-			struct md_object *parent, struct lu_fid *fid,
-			struct lu_name *lname, struct md_op_spec *spec,
-			struct md_attr *ma)
-{
-	struct lu_object	*ec_child, *child;
-	struct lu_device	*ld = ed->ed_next;
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_fid		*fid2 = &info->eti_fid2;
-	struct lu_object_conf    conf = { .loc_flags = LOC_F_NEW };
-	int			 rc;
-
-	rc = mdo_lookup(env, parent, lname, fid2, spec);
-	if (rc == 0)
-		return -EEXIST;
-	else if (rc != -ENOENT)
-		return rc;
-
-	ec_child = lu_object_find_at(env, &ed->ed_cl.cd_lu_dev,
-				     fid, &conf);
-	if (IS_ERR(ec_child)) {
-		CERROR("Can not find the child "DFID": rc = %ld\n", PFID(fid),
-			PTR_ERR(ec_child));
-		return PTR_ERR(ec_child);
-	}
-
-	child = lu_object_locate(ec_child->lo_header, ld->ld_type);
-	if (child == NULL) {
-		CERROR("Can not locate the child "DFID"\n", PFID(fid));
-		GOTO(out_put, rc = -EINVAL);
-	}
-
-	CDEBUG(D_RPCTRACE, "Start creating object "DFID" %s %p\n",
-	       PFID(lu_object_fid(&parent->mo_lu)), lname->ln_name, parent);
-
-	/*
-	 * Do not perform lookup sanity check. We know that name does not exist.
-	 */
-	spec->sp_cr_lookup = 0;
-	rc = mdo_create(env, parent, lname, lu2md(child), spec, ma);
-	if (rc) {
-		CERROR("Can not create child "DFID": rc = %d\n", PFID(fid), rc);
-		GOTO(out_put, rc);
-	}
-	CDEBUG(D_RPCTRACE, "End creating object "DFID" %s %p rc  = %d\n",
-	       PFID(lu_object_fid(&parent->mo_lu)), lname->ln_name, parent, rc);
-out_put:
-	lu_object_put(env, ec_child);
-	return rc;
-}
-
-static int echo_set_lmm_size(const struct lu_env *env, struct lu_device *ld,
-			     struct md_attr *ma)
-{
-	struct echo_thread_info *info = echo_env_info(env);
-
-	if (strcmp(ld->ld_type->ldt_name, LUSTRE_MDD_NAME)) {
-		ma->ma_lmm = (void *)&info->eti_lmm;
-		ma->ma_lmm_size = sizeof(info->eti_lmm);
-	} else {
-		LASSERT(info->eti_big_lmmsize);
-		ma->ma_lmm = info->eti_big_lmm;
-		ma->ma_lmm_size = info->eti_big_lmmsize;
+			return -EFAULT;
 	}
-
 	return 0;
 }
 
-static int echo_create_md_object(const struct lu_env *env,
-				 struct echo_device *ed,
-				 struct lu_object *ec_parent,
-				 struct lu_fid *fid,
-				 char *name, int namelen,
-				 __u64 id, __u32 mode, int count,
-				 int stripe_count, int stripe_offset)
-{
-	struct lu_object	*parent;
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_name	  *lname = &info->eti_lname;
-	struct md_op_spec       *spec = &info->eti_spec;
-	struct md_attr	  *ma = &info->eti_ma;
-	struct lu_device	*ld = ed->ed_next;
-	int		      rc = 0;
-	int		      i;
-
-	if (ec_parent == NULL)
-		return -1;
-	parent = lu_object_locate(ec_parent->lo_header, ld->ld_type);
-	if (parent == NULL)
-		return -ENXIO;
-
-	memset(ma, 0, sizeof(*ma));
-	memset(spec, 0, sizeof(*spec));
-	if (stripe_count != 0) {
-		spec->sp_cr_flags |= FMODE_WRITE;
-		echo_set_lmm_size(env, ld, ma);
-		if (stripe_count != -1) {
-			struct lov_user_md_v3 *lum = &info->eti_lum;
-
-			lum->lmm_magic = LOV_USER_MAGIC_V3;
-			lum->lmm_stripe_count = stripe_count;
-			lum->lmm_stripe_offset = stripe_offset;
-			lum->lmm_pattern = 0;
-			spec->u.sp_ea.eadata = lum;
-			spec->u.sp_ea.eadatalen = sizeof(*lum);
-			spec->sp_cr_flags |= MDS_OPEN_HAS_EA;
-		}
-	}
-
-	ma->ma_attr.la_mode = mode;
-	ma->ma_attr.la_valid = LA_CTIME | LA_MODE;
-	ma->ma_attr.la_ctime = cfs_time_current_64();
-
-	if (name != NULL) {
-		lname->ln_name = name;
-		lname->ln_namelen = namelen;
-		/* If name is specified, only create one object by name */
-		rc = echo_md_create_internal(env, ed, lu2md(parent), fid, lname,
-					     spec, ma);
-		return rc;
-	}
-
-	/* Create multiple object sequenced by id */
-	for (i = 0; i < count; i++) {
-		char *tmp_name = info->eti_name;
-
-		echo_md_build_name(lname, tmp_name, id);
-
-		rc = echo_md_create_internal(env, ed, lu2md(parent), fid, lname,
-					     spec, ma);
-		if (rc) {
-			CERROR("Can not create child %s: rc = %d\n", tmp_name,
-				rc);
-			break;
-		}
-		id++;
-		fid->f_oid++;
-	}
-
-	return rc;
-}
-
-static struct lu_object *echo_md_lookup(const struct lu_env *env,
-					struct echo_device *ed,
-					struct md_object *parent,
-					struct lu_name *lname)
-{
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_fid	   *fid = &info->eti_fid;
-	struct lu_object	*child;
-	int    rc;
-
-	CDEBUG(D_INFO, "lookup %s in parent "DFID" %p\n", lname->ln_name,
-	       PFID(fid), parent);
-	rc = mdo_lookup(env, parent, lname, fid, NULL);
-	if (rc) {
-		CERROR("lookup %s: rc = %d\n", lname->ln_name, rc);
-		return ERR_PTR(rc);
-	}
-
-	/* In the function below, .hs_keycmp resolves to
-	 * lu_obj_hop_keycmp() */
-	/* coverity[overrun-buffer-val] */
-	child = lu_object_find_at(env, &ed->ed_cl.cd_lu_dev, fid, NULL);
-
-	return child;
-}
-
-static int echo_setattr_object(const struct lu_env *env,
-			       struct echo_device *ed,
-			       struct lu_object *ec_parent,
-			       __u64 id, int count)
-{
-	struct lu_object	*parent;
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_name	  *lname = &info->eti_lname;
-	char		    *name = info->eti_name;
-	struct lu_device	*ld = ed->ed_next;
-	struct lu_buf	   *buf = &info->eti_buf;
-	int		      rc = 0;
-	int		      i;
-
-	if (ec_parent == NULL)
-		return -1;
-	parent = lu_object_locate(ec_parent->lo_header, ld->ld_type);
-	if (parent == NULL)
-		return -ENXIO;
-
-	for (i = 0; i < count; i++) {
-		struct lu_object *ec_child, *child;
-
-		echo_md_build_name(lname, name, id);
-
-		ec_child = echo_md_lookup(env, ed, lu2md(parent), lname);
-		if (IS_ERR(ec_child)) {
-			CERROR("Can't find child %s: rc = %ld\n",
-				lname->ln_name, PTR_ERR(ec_child));
-			return PTR_ERR(ec_child);
-		}
-
-		child = lu_object_locate(ec_child->lo_header, ld->ld_type);
-		if (child == NULL) {
-			CERROR("Can not locate the child %s\n", lname->ln_name);
-			lu_object_put(env, ec_child);
-			rc = -EINVAL;
-			break;
-		}
-
-		CDEBUG(D_RPCTRACE, "Start setattr object "DFID"\n",
-		       PFID(lu_object_fid(child)));
-
-		buf->lb_buf = info->eti_xattr_buf;
-		buf->lb_len = sizeof(info->eti_xattr_buf);
-
-		sprintf(name, "%s.test1", XATTR_USER_PREFIX);
-		rc = mo_xattr_set(env, lu2md(child), buf, name,
-				  LU_XATTR_CREATE);
-		if (rc < 0) {
-			CERROR("Can not setattr child "DFID": rc = %d\n",
-				PFID(lu_object_fid(child)), rc);
-			lu_object_put(env, ec_child);
-			break;
-		}
-		CDEBUG(D_RPCTRACE, "End setattr object "DFID"\n",
-		       PFID(lu_object_fid(child)));
-		id++;
-		lu_object_put(env, ec_child);
-	}
-	return rc;
-}
-
-static int echo_getattr_object(const struct lu_env *env,
-			       struct echo_device *ed,
-			       struct lu_object *ec_parent,
-			       __u64 id, int count)
-{
-	struct lu_object	*parent;
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_name	  *lname = &info->eti_lname;
-	char		    *name = info->eti_name;
-	struct md_attr	  *ma = &info->eti_ma;
-	struct lu_device	*ld = ed->ed_next;
-	int		      rc = 0;
-	int		      i;
-
-	if (ec_parent == NULL)
-		return -1;
-	parent = lu_object_locate(ec_parent->lo_header, ld->ld_type);
-	if (parent == NULL)
-		return -ENXIO;
-
-	memset(ma, 0, sizeof(*ma));
-	ma->ma_need |= MA_INODE | MA_LOV | MA_PFID | MA_HSM | MA_ACL_DEF;
-	ma->ma_acl = info->eti_xattr_buf;
-	ma->ma_acl_size = sizeof(info->eti_xattr_buf);
-
-	for (i = 0; i < count; i++) {
-		struct lu_object *ec_child, *child;
-
-		ma->ma_valid = 0;
-		echo_md_build_name(lname, name, id);
-		echo_set_lmm_size(env, ld, ma);
-
-		ec_child = echo_md_lookup(env, ed, lu2md(parent), lname);
-		if (IS_ERR(ec_child)) {
-			CERROR("Can't find child %s: rc = %ld\n",
-			       lname->ln_name, PTR_ERR(ec_child));
-			return PTR_ERR(ec_child);
-		}
-
-		child = lu_object_locate(ec_child->lo_header, ld->ld_type);
-		if (child == NULL) {
-			CERROR("Can not locate the child %s\n", lname->ln_name);
-			lu_object_put(env, ec_child);
-			return -EINVAL;
-		}
-
-		CDEBUG(D_RPCTRACE, "Start getattr object "DFID"\n",
-		       PFID(lu_object_fid(child)));
-		rc = echo_attr_get_complex(env, lu2md(child), ma);
-		if (rc) {
-			CERROR("Can not getattr child "DFID": rc = %d\n",
-				PFID(lu_object_fid(child)), rc);
-			lu_object_put(env, ec_child);
-			break;
-		}
-		CDEBUG(D_RPCTRACE, "End getattr object "DFID"\n",
-		       PFID(lu_object_fid(child)));
-		id++;
-		lu_object_put(env, ec_child);
-	}
-
-	return rc;
-}
-
-static int echo_lookup_object(const struct lu_env *env,
-			      struct echo_device *ed,
-			      struct lu_object *ec_parent,
-			      __u64 id, int count)
-{
-	struct lu_object	*parent;
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_name	  *lname = &info->eti_lname;
-	char		    *name = info->eti_name;
-	struct lu_fid	   *fid = &info->eti_fid;
-	struct lu_device	*ld = ed->ed_next;
-	int		      rc = 0;
-	int		      i;
-
-	if (ec_parent == NULL)
-		return -1;
-	parent = lu_object_locate(ec_parent->lo_header, ld->ld_type);
-	if (parent == NULL)
-		return -ENXIO;
-
-	/*prepare the requests*/
-	for (i = 0; i < count; i++) {
-		echo_md_build_name(lname, name, id);
-
-		CDEBUG(D_RPCTRACE, "Start lookup object "DFID" %s %p\n",
-		       PFID(lu_object_fid(parent)), lname->ln_name, parent);
-
-		rc = mdo_lookup(env, lu2md(parent), lname, fid, NULL);
-		if (rc) {
-			CERROR("Can not lookup child %s: rc = %d\n", name, rc);
-			break;
-		}
-		CDEBUG(D_RPCTRACE, "End lookup object "DFID" %s %p\n",
-		       PFID(lu_object_fid(parent)), lname->ln_name, parent);
-
-		id++;
-	}
-	return rc;
-}
-
-static int echo_md_destroy_internal(const struct lu_env *env,
-				    struct echo_device *ed,
-				    struct md_object *parent,
-				    struct lu_name *lname,
-				    struct md_attr *ma)
-{
-	struct lu_device   *ld = ed->ed_next;
-	struct lu_object   *ec_child;
-	struct lu_object   *child;
-	int		 rc;
-
-	ec_child = echo_md_lookup(env, ed, parent, lname);
-	if (IS_ERR(ec_child)) {
-		CERROR("Can't find child %s: rc = %ld\n", lname->ln_name,
-			PTR_ERR(ec_child));
-		return PTR_ERR(ec_child);
-	}
-
-	child = lu_object_locate(ec_child->lo_header, ld->ld_type);
-	if (child == NULL) {
-		CERROR("Can not locate the child %s\n", lname->ln_name);
-		GOTO(out_put, rc = -EINVAL);
-	}
-
-	CDEBUG(D_RPCTRACE, "Start destroy object "DFID" %s %p\n",
-	       PFID(lu_object_fid(&parent->mo_lu)), lname->ln_name, parent);
-
-	rc = mdo_unlink(env, parent, lu2md(child), lname, ma, 0);
-	if (rc) {
-		CERROR("Can not unlink child %s: rc = %d\n",
-			lname->ln_name, rc);
-		GOTO(out_put, rc);
-	}
-	CDEBUG(D_RPCTRACE, "End destroy object "DFID" %s %p\n",
-	       PFID(lu_object_fid(&parent->mo_lu)), lname->ln_name, parent);
-out_put:
-	lu_object_put(env, ec_child);
-	return rc;
-}
-
-static int echo_destroy_object(const struct lu_env *env,
-			       struct echo_device *ed,
-			       struct lu_object *ec_parent,
-			       char *name, int namelen,
-			       __u64 id, __u32 mode,
-			       int count)
-{
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_name	  *lname = &info->eti_lname;
-	struct md_attr	  *ma = &info->eti_ma;
-	struct lu_device	*ld = ed->ed_next;
-	struct lu_object	*parent;
-	int		      rc = 0;
-	int		      i;
-
-	parent = lu_object_locate(ec_parent->lo_header, ld->ld_type);
-	if (parent == NULL)
-		return -EINVAL;
-
-	memset(ma, 0, sizeof(*ma));
-	ma->ma_attr.la_mode = mode;
-	ma->ma_attr.la_valid = LA_CTIME;
-	ma->ma_attr.la_ctime = cfs_time_current_64();
-	ma->ma_need = MA_INODE;
-	ma->ma_valid = 0;
-
-	if (name != NULL) {
-		lname->ln_name = name;
-		lname->ln_namelen = namelen;
-		rc = echo_md_destroy_internal(env, ed, lu2md(parent), lname,
-					      ma);
-		return rc;
-	}
-
-	/*prepare the requests*/
-	for (i = 0; i < count; i++) {
-		char *tmp_name = info->eti_name;
-
-		ma->ma_valid = 0;
-		echo_md_build_name(lname, tmp_name, id);
-
-		rc = echo_md_destroy_internal(env, ed, lu2md(parent), lname,
-					      ma);
-		if (rc) {
-			CERROR("Can not unlink child %s: rc = %d\n", name, rc);
-			break;
-		}
-		id++;
-	}
-
-	return rc;
-}
-
-static struct lu_object *echo_resolve_path(const struct lu_env *env,
-					   struct echo_device *ed, char *path,
-					   int path_len)
-{
-	struct lu_device	*ld = ed->ed_next;
-	struct md_device	*md = lu2md_dev(ld);
-	struct echo_thread_info *info = echo_env_info(env);
-	struct lu_fid	   *fid = &info->eti_fid;
-	struct lu_name	  *lname = &info->eti_lname;
-	struct lu_object	*parent = NULL;
-	struct lu_object	*child = NULL;
-	int rc = 0;
-
-	/*Only support MDD layer right now*/
-	rc = md->md_ops->mdo_root_get(env, md, fid);
-	if (rc) {
-		CERROR("get root error: rc = %d\n", rc);
-		return ERR_PTR(rc);
-	}
-
-	/* In the function below, .hs_keycmp resolves to
-	 * lu_obj_hop_keycmp() */
-	/* coverity[overrun-buffer-val] */
-	parent = lu_object_find_at(env, &ed->ed_cl.cd_lu_dev, fid, NULL);
-	if (IS_ERR(parent)) {
-		CERROR("Can not find the parent "DFID": rc = %ld\n",
-			PFID(fid), PTR_ERR(parent));
-		return parent;
-	}
-
-	while (1) {
-		struct lu_object *ld_parent;
-		char *e;
-
-		e = strsep(&path, "/");
-		if (e == NULL)
-			break;
-
-		if (e[0] == 0) {
-			if (!path || path[0] == '\0')
-				break;
-			continue;
-		}
-
-		lname->ln_name = e;
-		lname->ln_namelen = strlen(e);
-
-		ld_parent = lu_object_locate(parent->lo_header, ld->ld_type);
-		if (ld_parent == NULL) {
-			lu_object_put(env, parent);
-			rc = -EINVAL;
-			break;
-		}
-
-		child = echo_md_lookup(env, ed, lu2md(ld_parent), lname);
-		lu_object_put(env, parent);
-		if (IS_ERR(child)) {
-			rc = (int)PTR_ERR(child);
-			CERROR("lookup %s under parent "DFID": rc = %d\n",
-				lname->ln_name, PFID(lu_object_fid(ld_parent)),
-				rc);
-			break;
-		}
-		parent = child;
-	}
-	if (rc)
-		return ERR_PTR(rc);
-
-	return parent;
-}
-
-static void echo_ucred_init(struct lu_env *env)
-{
-	struct lu_ucred *ucred = lu_ucred(env);
-
-	ucred->uc_valid = UCRED_INVALID;
-
-	ucred->uc_suppgids[0] = -1;
-	ucred->uc_suppgids[1] = -1;
-
-	ucred->uc_uid   = ucred->uc_o_uid   =
-				from_kuid(&init_user_ns, current_uid());
-	ucred->uc_gid   = ucred->uc_o_gid   =
-				from_kgid(&init_user_ns, current_gid());
-	ucred->uc_fsuid = ucred->uc_o_fsuid =
-				from_kuid(&init_user_ns, current_fsuid());
-	ucred->uc_fsgid = ucred->uc_o_fsgid =
-				from_kgid(&init_user_ns, current_fsgid());
-	ucred->uc_cap   = cfs_curproc_cap_pack();
-
-	/* remove fs privilege for non-root user. */
-	if (ucred->uc_fsuid)
-		ucred->uc_cap &= ~CFS_CAP_FS_MASK;
-	ucred->uc_valid = UCRED_NEW;
-}
-
-static void echo_ucred_fini(struct lu_env *env)
-{
-	struct lu_ucred *ucred = lu_ucred(env);
-	ucred->uc_valid = UCRED_INIT;
-}
-
-#define ECHO_MD_CTX_TAG (LCT_REMEMBER | LCT_MD_THREAD)
-#define ECHO_MD_SES_TAG (LCT_REMEMBER | LCT_SESSION)
-static int echo_md_handler(struct echo_device *ed, int command,
-			   char *path, int path_len, __u64 id, int count,
-			   struct obd_ioctl_data *data)
-{
-	struct echo_thread_info *info;
-	struct lu_device      *ld = ed->ed_next;
-	struct lu_env	 *env;
-	int		    refcheck;
-	struct lu_object      *parent;
-	char		  *name = NULL;
-	int		    namelen = data->ioc_plen2;
-	int		    rc = 0;
-
-	if (ld == NULL) {
-		CERROR("MD echo client is not being initialized properly\n");
-		return -EINVAL;
-	}
-
-	if (strcmp(ld->ld_type->ldt_name, LUSTRE_MDD_NAME)) {
-		CERROR("Only support MDD layer right now!\n");
-		return -EINVAL;
-	}
-
-	env = cl_env_get(&refcheck);
-	if (IS_ERR(env))
-		return PTR_ERR(env);
-
-	rc = lu_env_refill_by_tags(env, ECHO_MD_CTX_TAG, ECHO_MD_SES_TAG);
-	if (rc != 0)
-		GOTO(out_env, rc);
-
-	/* init big_lmm buffer */
-	info = echo_env_info(env);
-	LASSERT(info->eti_big_lmm == NULL);
-	OBD_ALLOC_LARGE(info->eti_big_lmm, MIN_MD_SIZE);
-	if (info->eti_big_lmm == NULL)
-		GOTO(out_env, rc = -ENOMEM);
-	info->eti_big_lmmsize = MIN_MD_SIZE;
-
-	parent = echo_resolve_path(env, ed, path, path_len);
-	if (IS_ERR(parent)) {
-		CERROR("Can not resolve the path %s: rc = %ld\n", path,
-			PTR_ERR(parent));
-		GOTO(out_free, rc = PTR_ERR(parent));
-	}
-
-	if (namelen > 0) {
-		OBD_ALLOC(name, namelen + 1);
-		if (name == NULL)
-			GOTO(out_put, rc = -ENOMEM);
-		if (copy_from_user(name, data->ioc_pbuf2, namelen))
-			GOTO(out_name, rc = -EFAULT);
-	}
-
-	echo_ucred_init(env);
-
-	switch (command) {
-	case ECHO_MD_CREATE:
-	case ECHO_MD_MKDIR: {
-		struct echo_thread_info *info = echo_env_info(env);
-		__u32 mode = data->ioc_obdo2.o_mode;
-		struct lu_fid *fid = &info->eti_fid;
-		int stripe_count = (int)data->ioc_obdo2.o_misc;
-		int stripe_index = (int)data->ioc_obdo2.o_stripe_idx;
-
-		rc = ostid_to_fid(fid, &data->ioc_obdo1.o_oi, 0);
-		if (rc != 0)
-			break;
-
-		/* In the function below, .hs_keycmp resolves to
-		 * lu_obj_hop_keycmp() */
-		/* coverity[overrun-buffer-val] */
-		rc = echo_create_md_object(env, ed, parent, fid, name, namelen,
-					   id, mode, count, stripe_count,
-					   stripe_index);
-		break;
-	}
-	case ECHO_MD_DESTROY:
-	case ECHO_MD_RMDIR: {
-		__u32 mode = data->ioc_obdo2.o_mode;
-
-		rc = echo_destroy_object(env, ed, parent, name, namelen,
-					 id, mode, count);
-		break;
-	}
-	case ECHO_MD_LOOKUP:
-		rc = echo_lookup_object(env, ed, parent, id, count);
-		break;
-	case ECHO_MD_GETATTR:
-		rc = echo_getattr_object(env, ed, parent, id, count);
-		break;
-	case ECHO_MD_SETATTR:
-		rc = echo_setattr_object(env, ed, parent, id, count);
-		break;
-	default:
-		CERROR("unknown command %d\n", command);
-		rc = -EINVAL;
-		break;
-	}
-	echo_ucred_fini(env);
-
-out_name:
-	if (name != NULL)
-		OBD_FREE(name, namelen + 1);
-out_put:
-	lu_object_put(env, parent);
-out_free:
-	LASSERT(info->eti_big_lmm);
-	OBD_FREE_LARGE(info->eti_big_lmm, info->eti_big_lmmsize);
-	info->eti_big_lmm = NULL;
-	info->eti_big_lmmsize = 0;
-out_env:
-	cl_env_put(env, &refcheck);
-	return rc;
-}
-
 static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 			      int on_target, struct obdo *oa, void *ulsm,
 			      int ulsm_nob, struct obd_trans_info *oti)
@@ -2282,7 +1386,7 @@ static int echo_create_object(const struct lu_env *env, struct echo_device *ed,
 		echo_free_memmd(ed, &lsm);
 	if (rc)
 		CERROR("create object failed with: rc = %d\n", rc);
-	return (rc);
+	return rc;
 }
 
 static int echo_get_object(struct echo_object **ecop, struct echo_device *ed,
@@ -2324,14 +1428,14 @@ static void echo_put_object(struct echo_object *eco)
 }
 
 static void
-echo_get_stripe_off_id (struct lov_stripe_md *lsm, obd_off *offp, obd_id *idp)
+echo_get_stripe_off_id(struct lov_stripe_md *lsm, u64 *offp, u64 *idp)
 {
 	unsigned long stripe_count;
 	unsigned long stripe_size;
 	unsigned long width;
 	unsigned long woffset;
 	int	   stripe_index;
-	obd_off       offset;
+	u64       offset;
 
 	if (lsm->lsm_stripe_count <= 1)
 		return;
@@ -2354,12 +1458,12 @@ echo_get_stripe_off_id (struct lov_stripe_md *lsm, obd_off *offp, obd_id *idp)
 
 static void
 echo_client_page_debug_setup(struct lov_stripe_md *lsm,
-			     struct page *page, int rw, obd_id id,
-			     obd_off offset, obd_off count)
+			     struct page *page, int rw, u64 id,
+			     u64 offset, u64 count)
 {
 	char    *addr;
-	obd_off  stripe_off;
-	obd_id   stripe_id;
+	u64	 stripe_off;
+	u64	 stripe_id;
 	int      delta;
 
 	/* no partial pages on the client */
@@ -2384,11 +1488,11 @@ echo_client_page_debug_setup(struct lov_stripe_md *lsm,
 }
 
 static int echo_client_page_debug_check(struct lov_stripe_md *lsm,
-					struct page *page, obd_id id,
-					obd_off offset, obd_off count)
+					struct page *page, u64 id,
+					u64 offset, u64 count)
 {
-	obd_off stripe_off;
-	obd_id  stripe_id;
+	u64	stripe_off;
+	u64	stripe_id;
 	char   *addr;
 	int     delta;
 	int     rc;
@@ -2418,16 +1522,16 @@ static int echo_client_page_debug_check(struct lov_stripe_md *lsm,
 }
 
 static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
-			    struct echo_object *eco, obd_off offset,
-			    obd_size count, int async,
+			    struct echo_object *eco, u64 offset,
+			    u64 count, int async,
 			    struct obd_trans_info *oti)
 {
 	struct lov_stripe_md   *lsm = eco->eo_lsm;
-	obd_count	       npages;
+	u32	       npages;
 	struct brw_page	*pga;
 	struct brw_page	*pgp;
 	struct page	    **pages;
-	obd_off		 off;
+	u64		 off;
 	int		     i;
 	int		     rc;
 	int		     verify;
@@ -2516,16 +1620,16 @@ static int echo_client_kbrw(struct echo_device *ed, int rw, struct obdo *oa,
 static int echo_client_prep_commit(const struct lu_env *env,
 				   struct obd_export *exp, int rw,
 				   struct obdo *oa, struct echo_object *eco,
-				   obd_off offset, obd_size count,
-				   obd_size batch, struct obd_trans_info *oti,
+				   u64 offset, u64 count,
+				   u64 batch, struct obd_trans_info *oti,
 				   int async)
 {
 	struct lov_stripe_md *lsm = eco->eo_lsm;
 	struct obd_ioobj ioo;
 	struct niobuf_local *lnb;
 	struct niobuf_remote *rnb;
-	obd_off off;
-	obd_size npages, tot_pages;
+	u64 off;
+	u64 npages, tot_pages;
 	int i, ret = 0, brw_flags = 0;
 
 	if (count <= 0 || (count & (~CFS_PAGE_MASK)) != 0 ||
@@ -2548,7 +1652,7 @@ static int echo_client_prep_commit(const struct lu_env *env,
 
 	off = offset;
 
-	for(; tot_pages; tot_pages -= npages) {
+	for (; tot_pages; tot_pages -= npages) {
 		int lpages;
 
 		if (tot_pages < npages)
@@ -2677,12 +1781,12 @@ static int echo_client_brw_ioctl(const struct lu_env *env, int rw,
 
 static int
 echo_client_enqueue(struct obd_export *exp, struct obdo *oa,
-		    int mode, obd_off offset, obd_size nob)
+		    int mode, u64 offset, u64 nob)
 {
 	struct echo_device     *ed = obd2echo_dev(exp->exp_obd);
 	struct lustre_handle   *ulh = &oa->o_handle;
 	struct echo_object     *eco;
-	obd_off		 end;
+	u64		 end;
 	int		     rc;
 
 	if (ed->ed_next == NULL)
@@ -2699,7 +1803,7 @@ echo_client_enqueue(struct obd_export *exp, struct obdo *oa,
 	if (rc != 0)
 		return rc;
 
-	end = (nob == 0) ? ((obd_off) -1) : (offset + nob - 1);
+	end = (nob == 0) ? ((u64) -1) : (offset + nob - 1);
 	rc = cl_echo_enqueue(eco, offset, end, mode, &ulh->cookie);
 	if (rc == 0) {
 		oa->o_valid |= OBD_MD_FLHANDLE;
@@ -2770,72 +1874,6 @@ echo_client_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 					data->ioc_plen1, &dummy_oti);
 		GOTO(out, rc);
 
-	case OBD_IOC_ECHO_MD: {
-		int count;
-		int cmd;
-		char *dir = NULL;
-		int dirlen;
-		__u64 id;
-
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO(out, rc = -EPERM);
-
-		count = data->ioc_count;
-		cmd = data->ioc_command;
-
-		id = ostid_id(&data->ioc_obdo2.o_oi);
-
-		dirlen = data->ioc_plen1;
-		OBD_ALLOC(dir, dirlen + 1);
-		if (dir == NULL)
-			GOTO(out, rc = -ENOMEM);
-
-		if (copy_from_user(dir, data->ioc_pbuf1, dirlen)) {
-			OBD_FREE(dir, data->ioc_plen1 + 1);
-			GOTO(out, rc = -EFAULT);
-		}
-
-		rc = echo_md_handler(ed, cmd, dir, dirlen, id, count, data);
-		OBD_FREE(dir, dirlen + 1);
-		GOTO(out, rc);
-	}
-	case OBD_IOC_ECHO_ALLOC_SEQ: {
-		struct lu_env   *cl_env;
-		int	      refcheck;
-		__u64	    seq;
-		int	      max_count;
-
-		if (!capable(CFS_CAP_SYS_ADMIN))
-			GOTO(out, rc = -EPERM);
-
-		cl_env = cl_env_get(&refcheck);
-		if (IS_ERR(cl_env))
-			GOTO(out, rc = PTR_ERR(cl_env));
-
-		rc = lu_env_refill_by_tags(cl_env, ECHO_MD_CTX_TAG,
-					    ECHO_MD_SES_TAG);
-		if (rc != 0) {
-			cl_env_put(cl_env, &refcheck);
-			GOTO(out, rc);
-		}
-
-		rc = seq_client_get_seq(cl_env, ed->ed_cl_seq, &seq);
-		cl_env_put(cl_env, &refcheck);
-		if (rc < 0) {
-			CERROR("%s: Can not alloc seq: rc = %d\n",
-			       obd->obd_name, rc);
-			GOTO(out, rc);
-		}
-
-		if (copy_to_user(data->ioc_pbuf1, &seq, data->ioc_plen1))
-			return -EFAULT;
-
-		max_count = LUSTRE_METADATA_SEQ_MAX_WIDTH;
-		if (copy_to_user(data->ioc_pbuf2, &max_count,
-				     data->ioc_plen2))
-			return -EFAULT;
-		GOTO(out, rc);
-	}
 	case OBD_IOC_DESTROY:
 		if (!capable(CFS_CAP_SYS_ADMIN))
 			GOTO (out, rc = -EPERM);
@@ -2973,12 +2011,6 @@ static int echo_client_setup(const struct lu_env *env,
 	ec->ec_unique = 0;
 	ec->ec_nstripes = 0;
 
-	if (!strcmp(tgt->obd_type->typ_name, LUSTRE_MDT_NAME)) {
-		lu_context_tags_update(ECHO_MD_CTX_TAG);
-		lu_session_tags_update(ECHO_MD_SES_TAG);
-		return 0;
-	}
-
 	OBD_ALLOC(ocd, sizeof(*ocd));
 	if (ocd == NULL) {
 		CERROR("Can't alloc ocd connecting to %s\n",
@@ -3008,7 +2040,7 @@ static int echo_client_setup(const struct lu_env *env,
 	if (rc != 0) {
 		CERROR("fail to connect to device %s\n",
 		       lustre_cfg_string(lcfg, 1));
-		return (rc);
+		return rc;
 	}
 
 	return rc;
@@ -3016,20 +2048,9 @@ static int echo_client_setup(const struct lu_env *env,
 
 static int echo_client_cleanup(struct obd_device *obddev)
 {
-	struct echo_device *ed = obd2echo_dev(obddev);
 	struct echo_client_obd *ec = &obddev->u.echo_client;
 	int rc;
 
-	/*Do nothing for Metadata echo client*/
-	if (ed == NULL )
-		return 0;
-
-	if (ed->ed_next_ismd) {
-		lu_context_tags_clear(ECHO_MD_CTX_TAG);
-		lu_session_tags_clear(ECHO_MD_SES_TAG);
-		return 0;
-	}
-
 	if (!list_empty(&obddev->obd_exports)) {
 		CERROR("still has clients!\n");
 		return -EBUSY;
@@ -3061,37 +2082,11 @@ static int echo_client_connect(const struct lu_env *env,
 
 static int echo_client_disconnect(struct obd_export *exp)
 {
-#if 0
-	struct obd_device      *obd;
-	struct echo_client_obd *ec;
-	struct ec_lock	 *ecl;
-#endif
 	int		     rc;
 
 	if (exp == NULL)
 		GOTO(out, rc = -EINVAL);
 
-#if 0
-	obd = exp->exp_obd;
-	ec = &obd->u.echo_client;
-
-	/* no more contention on export's lock list */
-	while (!list_empty (&exp->exp_ec_data.eced_locks)) {
-		ecl = list_entry (exp->exp_ec_data.eced_locks.next,
-				      struct ec_lock, ecl_exp_chain);
-		list_del (&ecl->ecl_exp_chain);
-
-		rc = obd_cancel(ec->ec_exp, ecl->ecl_object->eco_lsm,
-				 ecl->ecl_mode, &ecl->ecl_lock_handle);
-
-		CDEBUG (D_INFO, "Cancel lock on object %#llx on disconnect "
-			"(%d)\n", ecl->ecl_object->eco_id, rc);
-
-		echo_put_object (ecl->ecl_object);
-		OBD_FREE (ecl, sizeof (*ecl));
-	}
-#endif
-
 	rc = class_disconnect(exp);
 	GOTO(out, rc);
  out:
@@ -3100,12 +2095,6 @@ static int echo_client_disconnect(struct obd_export *exp)
 
 static struct obd_ops echo_client_obd_ops = {
 	.o_owner       = THIS_MODULE,
-
-#if 0
-	.o_setup       = echo_client_setup,
-	.o_cleanup     = echo_client_cleanup,
-#endif
-
 	.o_iocontrol   = echo_client_iocontrol,
 	.o_connect     = echo_client_connect,
 	.o_disconnect  = echo_client_disconnect
diff --git a/drivers/staging/lustre/lustre/osc/lproc_osc.c b/drivers/staging/lustre/lustre/osc/lproc_osc.c
index 2ab403548b5e..9f719bcecab3 100644
--- a/drivers/staging/lustre/lustre/osc/lproc_osc.c
+++ b/drivers/staging/lustre/lustre/osc/lproc_osc.c
@@ -148,7 +148,7 @@ static ssize_t osc_max_dirty_mb_seq_write(struct file *file, const char *buffer,
 		return -ERANGE;
 
 	client_obd_list_lock(&cli->cl_loi_list_lock);
-	cli->cl_dirty_max = (obd_count)(pages_number << PAGE_CACHE_SHIFT);
+	cli->cl_dirty_max = (u32)(pages_number << PAGE_CACHE_SHIFT);
 	osc_wake_cache_waiters(cli);
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
 
@@ -565,7 +565,7 @@ static struct lprocfs_vars lprocfs_osc_module_vars[] = {
 	{ NULL }
 };
 
-#define pct(a,b) (b ? a * 100 / b : 0)
+#define pct(a, b) (b ? a * 100 / b : 0)
 
 static int osc_rpc_stats_seq_show(struct seq_file *seq, void *v)
 {
diff --git a/drivers/staging/lustre/lustre/osc/osc_cache.c b/drivers/staging/lustre/lustre/osc/osc_cache.c
index 57d7dba23479..7734d666b7a1 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cache.c
+++ b/drivers/staging/lustre/lustre/osc/osc_cache.c
@@ -1885,7 +1885,7 @@ osc_send_write_rpc(const struct lu_env *env, struct client_obd *cli,
 	struct osc_extent *ext;
 	struct osc_extent *tmp;
 	struct osc_extent *first = NULL;
-	obd_count page_count = 0;
+	u32 page_count = 0;
 	int srvlock = 0;
 	int rc = 0;
 
diff --git a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
index 2d1f977dca36..ebbd95c0cea8 100644
--- a/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_cl_internal.h
@@ -50,7 +50,6 @@
 
 #include "../include/obd.h"
 /* osc_build_res_name() */
-#include "../include/obd_ost.h"
 #include "../include/cl_object.h"
 #include "../include/lclient.h"
 #include "osc_internal.h"
@@ -435,7 +434,7 @@ void osc_page_submit(const struct lu_env *env, struct osc_page *opg,
 		     enum cl_req_type crt, int brw_flags);
 int osc_cancel_async_page(const struct lu_env *env, struct osc_page *ops);
 int osc_set_async_flags(struct osc_object *obj, struct osc_page *opg,
-			obd_flag async_flags);
+			u32 async_flags);
 int osc_prep_async_page(struct osc_object *osc, struct osc_page *ops,
 			struct page *page, loff_t offset);
 int osc_queue_async_io(const struct lu_env *env, struct cl_io *io,
diff --git a/drivers/staging/lustre/lustre/osc/osc_internal.h b/drivers/staging/lustre/lustre/osc/osc_internal.h
index f67a70083621..d788dac93cd0 100644
--- a/drivers/staging/lustre/lustre/osc/osc_internal.h
+++ b/drivers/staging/lustre/lustre/osc/osc_internal.h
@@ -59,7 +59,7 @@ struct osc_async_page {
 	struct list_head	      oap_pending_item;
 	struct list_head	      oap_rpc_item;
 
-	obd_off		 oap_obj_off;
+	u64		 oap_obj_off;
 	unsigned		oap_page_off;
 	enum async_flags	oap_async_flags;
 
@@ -161,7 +161,7 @@ static inline unsigned long rpcs_in_flight(struct client_obd *cli)
 }
 
 #ifndef min_t
-#define min_t(type,x,y) \
+#define min_t(type, x, y) \
 	({ type __x = (x); type __y = (y); __x < __y ? __x: __y; })
 #endif
 
@@ -192,12 +192,12 @@ extern struct kmem_cache *osc_quota_kmem;
 struct osc_quota_info {
 	/** linkage for quota hash table */
 	struct hlist_node oqi_hash;
-	obd_uid	  oqi_id;
+	u32	  oqi_id;
 };
 int osc_quota_setup(struct obd_device *obd);
 int osc_quota_cleanup(struct obd_device *obd);
 int osc_quota_setdq(struct client_obd *cli, const unsigned int qid[],
-		    obd_flag valid, obd_flag flags);
+		    u32 valid, u32 flags);
 int osc_quota_chkdq(struct client_obd *cli, const unsigned int qid[]);
 int osc_quotactl(struct obd_device *unused, struct obd_export *exp,
 		 struct obd_quotactl *oqctl);
diff --git a/drivers/staging/lustre/lustre/osc/osc_io.c b/drivers/staging/lustre/lustre/osc/osc_io.c
index 54fe836a64cd..fad4c135f928 100644
--- a/drivers/staging/lustre/lustre/osc/osc_io.c
+++ b/drivers/staging/lustre/lustre/osc/osc_io.c
@@ -63,6 +63,7 @@ static struct osc_io *cl2osc_io(const struct lu_env *env,
 				const struct cl_io_slice *slice)
 {
 	struct osc_io *oio = container_of0(slice, struct osc_io, oi_cl);
+
 	LINVRNT(oio == osc_env_io(env));
 	return oio;
 }
@@ -360,6 +361,7 @@ static int trunc_check_cb(const struct lu_env *env, struct cl_io *io,
 
 	{
 		struct page *vmpage = cl_page_vmpage(env, page);
+
 		if (PageLocked(vmpage))
 			CDEBUG(D_CACHE, "page %p index %lu locked for %d.\n",
 			       ops, page->cp_index,
@@ -498,6 +500,7 @@ static void osc_io_setattr_end(const struct lu_env *env,
 
 	if (cl_io_is_trunc(io)) {
 		__u64 size = io->u.ci_setattr.sa_attr.lvb_size;
+
 		osc_trunc_check(env, io, oio, size);
 		if (oio->oi_trunc != NULL) {
 			osc_cache_truncate_end(env, oio, cl2osc(obj));
@@ -711,7 +714,7 @@ static void osc_req_completion(const struct lu_env *env,
 static void osc_req_attr_set(const struct lu_env *env,
 			     const struct cl_req_slice *slice,
 			     const struct cl_object *obj,
-			     struct cl_req_attr *attr, obd_valid flags)
+			     struct cl_req_attr *attr, u64 flags)
 {
 	struct lov_oinfo *oinfo;
 	struct cl_req    *clerq;
diff --git a/drivers/staging/lustre/lustre/osc/osc_quota.c b/drivers/staging/lustre/lustre/osc/osc_quota.c
index 3563809072b4..6690f149a04c 100644
--- a/drivers/staging/lustre/lustre/osc/osc_quota.c
+++ b/drivers/staging/lustre/lustre/osc/osc_quota.c
@@ -28,10 +28,10 @@
  * Code originally extracted from quota directory
  */
 
-#include "../include/obd_ost.h"
+#include "../include/obd_class.h"
 #include "osc_internal.h"
 
-static inline struct osc_quota_info *osc_oqi_alloc(obd_uid id)
+static inline struct osc_quota_info *osc_oqi_alloc(u32 id)
 {
 	struct osc_quota_info *oqi;
 
@@ -71,7 +71,7 @@ int osc_quota_chkdq(struct client_obd *cli, const unsigned int qid[])
 						: OBD_FL_NO_GRPQUOTA)
 
 int osc_quota_setdq(struct client_obd *cli, const unsigned int qid[],
-		    obd_flag valid, obd_flag flags)
+		    u32 valid, u32 flags)
 {
 	int type;
 	int rc = 0;
@@ -138,17 +138,17 @@ int osc_quota_setdq(struct client_obd *cli, const unsigned int qid[],
 static unsigned
 oqi_hashfn(struct cfs_hash *hs, const void *key, unsigned mask)
 {
-	return cfs_hash_u32_hash(*((__u32*)key), mask);
+	return cfs_hash_u32_hash(*((__u32 *)key), mask);
 }
 
 static int
 oqi_keycmp(const void *key, struct hlist_node *hnode)
 {
 	struct osc_quota_info *oqi;
-	obd_uid uid;
+	u32 uid;
 
 	LASSERT(key != NULL);
-	uid = *((obd_uid*)key);
+	uid = *((u32 *)key);
 	oqi = hlist_entry(hnode, struct osc_quota_info, oqi_hash);
 
 	return uid == oqi->oqi_id;
diff --git a/drivers/staging/lustre/lustre/osc/osc_request.c b/drivers/staging/lustre/lustre/osc/osc_request.c
index fb0d9fb9cebc..44657a06b8a5 100644
--- a/drivers/staging/lustre/lustre/osc/osc_request.c
+++ b/drivers/staging/lustre/lustre/osc/osc_request.c
@@ -43,18 +43,58 @@
 #include "../include/lustre_net.h"
 #include "../include/lustre/lustre_user.h"
 #include "../include/obd_cksum.h"
-#include "../include/obd_ost.h"
 
 #include "../include/lustre_ha.h"
 #include "../include/lprocfs_status.h"
-#include "../include/lustre_log.h"
 #include "../include/lustre_debug.h"
 #include "../include/lustre_param.h"
 #include "../include/lustre_fid.h"
+#include "../include/obd_class.h"
 #include "osc_internal.h"
 #include "osc_cl_internal.h"
 
-static void osc_release_ppga(struct brw_page **ppga, obd_count count);
+struct osc_brw_async_args {
+	struct obdo       *aa_oa;
+	int		aa_requested_nob;
+	int		aa_nio_count;
+	u32		aa_page_count;
+	int		aa_resends;
+	struct brw_page  **aa_ppga;
+	struct client_obd *aa_cli;
+	struct list_head	 aa_oaps;
+	struct list_head	 aa_exts;
+	struct obd_capa   *aa_ocapa;
+	struct cl_req     *aa_clerq;
+};
+
+struct osc_async_args {
+	struct obd_info   *aa_oi;
+};
+
+struct osc_setattr_args {
+	struct obdo	 *sa_oa;
+	obd_enqueue_update_f sa_upcall;
+	void		*sa_cookie;
+};
+
+struct osc_fsync_args {
+	struct obd_info     *fa_oi;
+	obd_enqueue_update_f fa_upcall;
+	void		*fa_cookie;
+};
+
+struct osc_enqueue_args {
+	struct obd_export	*oa_exp;
+	__u64		    *oa_flags;
+	obd_enqueue_update_f      oa_upcall;
+	void		     *oa_cookie;
+	struct ost_lvb	   *oa_lvb;
+	struct lustre_handle     *oa_lockh;
+	struct ldlm_enqueue_info *oa_ei;
+	unsigned int	      oa_agl:1;
+};
+
+static void osc_release_ppga(struct brw_page **ppga, u32 count);
 static int brw_interpret(const struct lu_env *env,
 			 struct ptlrpc_request *req, void *data, int rc);
 int osc_cleanup(struct obd_device *obd);
@@ -197,7 +237,7 @@ static int osc_getattr_interpret(const struct lu_env *env,
 	struct ost_body *body;
 
 	if (rc != 0)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_OST_BODY);
 	if (body) {
@@ -273,11 +313,13 @@ static int osc_getattr(const struct lu_env *env, struct obd_export *exp,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_OST_BODY);
-	if (body == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (body == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	CDEBUG(D_INODE, "mode: %o\n", body->oa.o_mode);
 	lustre_get_wire_obdo(&req->rq_import->imp_connect_data, oinfo->oi_oa,
@@ -317,11 +359,13 @@ static int osc_setattr(const struct lu_env *env, struct obd_export *exp,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_OST_BODY);
-	if (body == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (body == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	lustre_get_wire_obdo(&req->rq_import->imp_connect_data, oinfo->oi_oa,
 			     &body->oa);
@@ -338,11 +382,13 @@ static int osc_setattr_interpret(const struct lu_env *env,
 	struct ost_body *body;
 
 	if (rc != 0)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_OST_BODY);
-	if (body == NULL)
-		GOTO(out, rc = -EPROTO);
+	if (body == NULL) {
+		rc = -EPROTO;
+		goto out;
+	}
 
 	lustre_get_wire_obdo(&req->rq_import->imp_connect_data, sa->sa_oa,
 			     &body->oa);
@@ -428,13 +474,15 @@ int osc_real_create(struct obd_export *exp, struct obdo *oa,
 	}
 
 	req = ptlrpc_request_alloc(class_exp2cliimp(exp), &RQF_OST_CREATE);
-	if (req == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	rc = ptlrpc_request_pack(req, LUSTRE_OST_VERSION, OST_CREATE);
 	if (rc) {
 		ptlrpc_request_free(req);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	body = req_capsule_client_get(&req->rq_pill, &RMF_OST_BODY);
@@ -454,11 +502,13 @@ int osc_real_create(struct obd_export *exp, struct obdo *oa,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out_req, rc);
+		goto out_req;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_OST_BODY);
-	if (body == NULL)
-		GOTO(out_req, rc = -EPROTO);
+	if (body == NULL) {
+		rc = -EPROTO;
+		goto out_req;
+	}
 
 	CDEBUG(D_INFO, "oa flags %x\n", oa->o_flags);
 	lustre_get_wire_obdo(&req->rq_import->imp_connect_data, oa, &body->oa);
@@ -537,17 +587,6 @@ int osc_punch_base(struct obd_export *exp, struct obd_info *oinfo,
 	return 0;
 }
 
-static int osc_punch(const struct lu_env *env, struct obd_export *exp,
-		     struct obd_info *oinfo, struct obd_trans_info *oti,
-		     struct ptlrpc_request_set *rqset)
-{
-	oinfo->oi_oa->o_size   = oinfo->oi_policy.l_extent.start;
-	oinfo->oi_oa->o_blocks = oinfo->oi_policy.l_extent.end;
-	oinfo->oi_oa->o_valid |= OBD_MD_FLSIZE | OBD_MD_FLBLOCKS;
-	return osc_punch_base(exp, oinfo,
-			      oinfo->oi_cb_up, oinfo, rqset);
-}
-
 static int osc_sync_interpret(const struct lu_env *env,
 			      struct ptlrpc_request *req,
 			      void *arg, int rc)
@@ -556,12 +595,13 @@ static int osc_sync_interpret(const struct lu_env *env,
 	struct ost_body *body;
 
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_OST_BODY);
 	if (body == NULL) {
 		CERROR ("can't unpack ost_body\n");
-		GOTO(out, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out;
 	}
 
 	*fa->fa_oi->oi_oa = body->oa;
@@ -614,22 +654,6 @@ int osc_sync_base(struct obd_export *exp, struct obd_info *oinfo,
 	return 0;
 }
 
-static int osc_sync(const struct lu_env *env, struct obd_export *exp,
-		    struct obd_info *oinfo, obd_size start, obd_size end,
-		    struct ptlrpc_request_set *set)
-{
-	if (!oinfo->oi_oa) {
-		CDEBUG(D_INFO, "oa NULL\n");
-		return -EINVAL;
-	}
-
-	oinfo->oi_oa->o_size = start;
-	oinfo->oi_oa->o_blocks = end;
-	oinfo->oi_oa->o_valid |= (OBD_MD_FLSIZE | OBD_MD_FLBLOCKS);
-
-	return osc_sync_base(exp, oinfo, oinfo->oi_cb_up, oinfo, set);
-}
-
 /* Find and cancel locally locks matched by @mode in the resource found by
  * @objid. Found locks are added into @cancel list. Returns the amount of
  * locks added to @cancels list. */
@@ -799,7 +823,7 @@ static int osc_destroy(const struct lu_env *env, struct obd_export *exp,
 static void osc_announce_cached(struct client_obd *cli, struct obdo *oa,
 				long writing_bytes)
 {
-	obd_flag bits = OBD_MD_FLBLOCKS|OBD_MD_FLGRANT;
+	u32 bits = OBD_MD_FLBLOCKS|OBD_MD_FLGRANT;
 
 	LASSERT(!(oa->o_valid & bits));
 
@@ -836,7 +860,7 @@ static void osc_announce_cached(struct client_obd *cli, struct obdo *oa,
 	oa->o_dropped = cli->cl_lost_grant;
 	cli->cl_lost_grant = 0;
 	client_obd_list_unlock(&cli->cl_loi_list_lock);
-	CDEBUG(D_CACHE,"dirty: %llu undirty: %u dropped %u grant: %llu\n",
+	CDEBUG(D_CACHE, "dirty: %llu undirty: %u dropped %u grant: %llu\n",
 	       oa->o_dirty, oa->o_undirty, oa->o_dropped, oa->o_grant);
 
 }
@@ -849,7 +873,7 @@ void osc_update_next_shrink(struct client_obd *cli)
 	       cli->cl_next_shrink_grant);
 }
 
-static void __osc_update_grant(struct client_obd *cli, obd_size grant)
+static void __osc_update_grant(struct client_obd *cli, u64 grant)
 {
 	client_obd_list_lock(&cli->cl_loi_list_lock);
 	cli->cl_avail_grant += grant;
@@ -865,7 +889,7 @@ static void osc_update_grant(struct client_obd *cli, struct ost_body *body)
 }
 
 static int osc_set_info_async(const struct lu_env *env, struct obd_export *exp,
-			      obd_count keylen, void *key, obd_count vallen,
+			      u32 keylen, void *key, u32 vallen,
 			      void *val, struct ptlrpc_request_set *set);
 
 static int osc_shrink_grant_interpret(const struct lu_env *env,
@@ -873,12 +897,12 @@ static int osc_shrink_grant_interpret(const struct lu_env *env,
 				      void *aa, int rc)
 {
 	struct client_obd *cli = &req->rq_import->imp_obd->u.cli;
-	struct obdo *oa = ((struct osc_grant_args *)aa)->aa_oa;
+	struct obdo *oa = ((struct osc_brw_async_args *)aa)->aa_oa;
 	struct ost_body *body;
 
 	if (rc != 0) {
 		__osc_update_grant(cli, oa->o_grant);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_OST_BODY);
@@ -1067,7 +1091,7 @@ static void osc_init_grant(struct client_obd *cli, struct obd_connect_data *ocd)
  * beyond the end of a stripe file; i.e. lustre is reading a sparse file
  * via the LOV, and it _knows_ it's reading inside the file, it's just that
  * this stripe never got written at or beyond this stripe offset yet. */
-static void handle_short_read(int nob_read, obd_count page_count,
+static void handle_short_read(int nob_read, u32 page_count,
 			      struct brw_page **pga)
 {
 	char *ptr;
@@ -1104,7 +1128,7 @@ static void handle_short_read(int nob_read, obd_count page_count,
 
 static int check_write_rcs(struct ptlrpc_request *req,
 			   int requested_nob, int niocount,
-			   obd_count page_count, struct brw_page **pga)
+			   u32 page_count, struct brw_page **pga)
 {
 	int     i;
 	__u32   *remote_rcs;
@@ -1114,28 +1138,28 @@ static int check_write_rcs(struct ptlrpc_request *req,
 						  niocount);
 	if (remote_rcs == NULL) {
 		CDEBUG(D_INFO, "Missing/short RC vector on BRW_WRITE reply\n");
-		return(-EPROTO);
+		return -EPROTO;
 	}
 
 	/* return error if any niobuf was in error */
 	for (i = 0; i < niocount; i++) {
 		if ((int)remote_rcs[i] < 0)
-			return(remote_rcs[i]);
+			return remote_rcs[i];
 
 		if (remote_rcs[i] != 0) {
 			CDEBUG(D_INFO, "rc[%d] invalid (%d) req %p\n",
 				i, remote_rcs[i], req);
-			return(-EPROTO);
+			return -EPROTO;
 		}
 	}
 
 	if (req->rq_bulk->bd_nob_transferred != requested_nob) {
 		CERROR("Unexpected # bytes transferred: %d (requested %d)\n",
 		       req->rq_bulk->bd_nob_transferred, requested_nob);
-		return(-EPROTO);
+		return -EPROTO;
 	}
 
-	return (0);
+	return 0;
 }
 
 static inline int can_merge_pages(struct brw_page *p1, struct brw_page *p2)
@@ -1157,7 +1181,7 @@ static inline int can_merge_pages(struct brw_page *p1, struct brw_page *p2)
 	return (p1->off + p1->count == p2->off);
 }
 
-static obd_count osc_checksum_bulk(int nob, obd_count pg_count,
+static u32 osc_checksum_bulk(int nob, u32 pg_count,
 				   struct brw_page **pga, int opc,
 				   cksum_type_t cksum_type)
 {
@@ -1218,8 +1242,9 @@ static obd_count osc_checksum_bulk(int nob, obd_count pg_count,
 	return cksum;
 }
 
-static int osc_brw_prep_request(int cmd, struct client_obd *cli,struct obdo *oa,
-				struct lov_stripe_md *lsm, obd_count page_count,
+static int osc_brw_prep_request(int cmd, struct client_obd *cli,
+				struct obdo *oa,
+				struct lov_stripe_md *lsm, u32 page_count,
 				struct brw_page **pga,
 				struct ptlrpc_request **reqp,
 				struct obd_capa *ocapa, int reserve,
@@ -1280,8 +1305,10 @@ static int osc_brw_prep_request(int cmd, struct client_obd *cli,struct obdo *oa,
 		opc == OST_WRITE ? BULK_GET_SOURCE : BULK_PUT_SINK,
 		OST_BULK_PORTAL);
 
-	if (desc == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (desc == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 	/* NB request now owns desc and will free it when it gets freed */
 
 	body = req_capsule_client_get(pill, &RMF_OST_BODY);
@@ -1422,7 +1449,7 @@ static int osc_brw_prep_request(int cmd, struct client_obd *cli,struct obdo *oa,
 
 static int check_write_checksum(struct obdo *oa, const lnet_process_id_t *peer,
 				__u32 client_cksum, __u32 server_cksum, int nob,
-				obd_count page_count, struct brw_page **pga,
+				u32 page_count, struct brw_page **pga,
 				cksum_type_t client_cksum_type)
 {
 	__u32 new_cksum;
@@ -1523,17 +1550,20 @@ static int osc_brw_fini_request(struct ptlrpc_request *req, int rc)
 					 cksum_type_unpack(aa->aa_oa->o_flags)))
 			return -EAGAIN;
 
-		rc = check_write_rcs(req, aa->aa_requested_nob,aa->aa_nio_count,
+		rc = check_write_rcs(req, aa->aa_requested_nob,
+				     aa->aa_nio_count,
 				     aa->aa_page_count, aa->aa_ppga);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	/* The rest of this function executes only for OST_READs */
 
 	/* if unwrap_bulk failed, return -EAGAIN to retry */
 	rc = sptlrpc_cli_unwrap_bulk_read(req, req->rq_bulk, rc);
-	if (rc < 0)
-		GOTO(out, rc = -EAGAIN);
+	if (rc < 0) {
+		rc = -EAGAIN;
+		goto out;
+	}
 
 	if (rc > aa->aa_requested_nob) {
 		CERROR("Unexpected rc %d (%d requested)\n", rc,
@@ -1544,7 +1574,7 @@ static int osc_brw_fini_request(struct ptlrpc_request *req, int rc)
 	if (rc != req->rq_bulk->bd_nob_transferred) {
 		CERROR ("Unexpected rc %d (%d transferred)\n",
 			rc, req->rq_bulk->bd_nob_transferred);
-		return (-EPROTO);
+		return -EPROTO;
 	}
 
 	if (rc < aa->aa_requested_nob)
@@ -1616,74 +1646,6 @@ out:
 	return rc;
 }
 
-static int osc_brw_internal(int cmd, struct obd_export *exp, struct obdo *oa,
-			    struct lov_stripe_md *lsm,
-			    obd_count page_count, struct brw_page **pga,
-			    struct obd_capa *ocapa)
-{
-	struct ptlrpc_request *req;
-	int		    rc;
-	wait_queue_head_t	    waitq;
-	int		    generation, resends = 0;
-	struct l_wait_info     lwi;
-
-	init_waitqueue_head(&waitq);
-	generation = exp->exp_obd->u.cli.cl_import->imp_generation;
-
-restart_bulk:
-	rc = osc_brw_prep_request(cmd, &exp->exp_obd->u.cli, oa, lsm,
-				  page_count, pga, &req, ocapa, 0, resends);
-	if (rc != 0)
-		return (rc);
-
-	if (resends) {
-		req->rq_generation_set = 1;
-		req->rq_import_generation = generation;
-		req->rq_sent = get_seconds() + resends;
-	}
-
-	rc = ptlrpc_queue_wait(req);
-
-	if (rc == -ETIMEDOUT && req->rq_resend) {
-		DEBUG_REQ(D_HA, req,  "BULK TIMEOUT");
-		ptlrpc_req_finished(req);
-		goto restart_bulk;
-	}
-
-	rc = osc_brw_fini_request(req, rc);
-
-	ptlrpc_req_finished(req);
-	/* When server return -EINPROGRESS, client should always retry
-	 * regardless of the number of times the bulk was resent already.*/
-	if (osc_recoverable_error(rc)) {
-		resends++;
-		if (rc != -EINPROGRESS &&
-		    !client_should_resend(resends, &exp->exp_obd->u.cli)) {
-			CERROR("%s: too many resend retries for object: "
-			       ""DOSTID", rc = %d.\n", exp->exp_obd->obd_name,
-			       POSTID(&oa->o_oi), rc);
-			goto out;
-		}
-		if (generation !=
-		    exp->exp_obd->u.cli.cl_import->imp_generation) {
-			CDEBUG(D_HA, "%s: resend cross eviction for object: "
-			       ""DOSTID", rc = %d.\n", exp->exp_obd->obd_name,
-			       POSTID(&oa->o_oi), rc);
-			goto out;
-		}
-
-		lwi = LWI_TIMEOUT_INTR(cfs_time_seconds(resends), NULL, NULL,
-				       NULL);
-		l_wait_event(waitq, 0, &lwi);
-
-		goto restart_bulk;
-	}
-out:
-	if (rc == -EAGAIN || rc == -EINPROGRESS)
-		rc = -EIO;
-	return rc;
-}
-
 static int osc_brw_redo_request(struct ptlrpc_request *request,
 				struct osc_brw_async_args *aa, int rc)
 {
@@ -1787,125 +1749,12 @@ static void sort_brw_pages(struct brw_page **array, int num)
 	} while (stride > 1);
 }
 
-static obd_count max_unfragmented_pages(struct brw_page **pg, obd_count pages)
-{
-	int count = 1;
-	int offset;
-	int i = 0;
-
-	LASSERT (pages > 0);
-	offset = pg[i]->off & ~CFS_PAGE_MASK;
-
-	for (;;) {
-		pages--;
-		if (pages == 0)	 /* that's all */
-			return count;
-
-		if (offset + pg[i]->count < PAGE_CACHE_SIZE)
-			return count;   /* doesn't end on page boundary */
-
-		i++;
-		offset = pg[i]->off & ~CFS_PAGE_MASK;
-		if (offset != 0)	/* doesn't start on page boundary */
-			return count;
-
-		count++;
-	}
-}
-
-static struct brw_page **osc_build_ppga(struct brw_page *pga, obd_count count)
-{
-	struct brw_page **ppga;
-	int i;
-
-	OBD_ALLOC(ppga, sizeof(*ppga) * count);
-	if (ppga == NULL)
-		return NULL;
-
-	for (i = 0; i < count; i++)
-		ppga[i] = pga + i;
-	return ppga;
-}
-
-static void osc_release_ppga(struct brw_page **ppga, obd_count count)
+static void osc_release_ppga(struct brw_page **ppga, u32 count)
 {
 	LASSERT(ppga != NULL);
 	OBD_FREE(ppga, sizeof(*ppga) * count);
 }
 
-static int osc_brw(int cmd, struct obd_export *exp, struct obd_info *oinfo,
-		   obd_count page_count, struct brw_page *pga,
-		   struct obd_trans_info *oti)
-{
-	struct obdo *saved_oa = NULL;
-	struct brw_page **ppga, **orig;
-	struct obd_import *imp = class_exp2cliimp(exp);
-	struct client_obd *cli;
-	int rc, page_count_orig;
-
-	LASSERT((imp != NULL) && (imp->imp_obd != NULL));
-	cli = &imp->imp_obd->u.cli;
-
-	if (cmd & OBD_BRW_CHECK) {
-		/* The caller just wants to know if there's a chance that this
-		 * I/O can succeed */
-
-		if (imp->imp_invalid)
-			return -EIO;
-		return 0;
-	}
-
-	/* test_brw with a failed create can trip this, maybe others. */
-	LASSERT(cli->cl_max_pages_per_rpc);
-
-	rc = 0;
-
-	orig = ppga = osc_build_ppga(pga, page_count);
-	if (ppga == NULL)
-		return -ENOMEM;
-	page_count_orig = page_count;
-
-	sort_brw_pages(ppga, page_count);
-	while (page_count) {
-		obd_count pages_per_brw;
-
-		if (page_count > cli->cl_max_pages_per_rpc)
-			pages_per_brw = cli->cl_max_pages_per_rpc;
-		else
-			pages_per_brw = page_count;
-
-		pages_per_brw = max_unfragmented_pages(ppga, pages_per_brw);
-
-		if (saved_oa != NULL) {
-			/* restore previously saved oa */
-			*oinfo->oi_oa = *saved_oa;
-		} else if (page_count > pages_per_brw) {
-			/* save a copy of oa (brw will clobber it) */
-			OBDO_ALLOC(saved_oa);
-			if (saved_oa == NULL)
-				GOTO(out, rc = -ENOMEM);
-			*saved_oa = *oinfo->oi_oa;
-		}
-
-		rc = osc_brw_internal(cmd, exp, oinfo->oi_oa, oinfo->oi_md,
-				      pages_per_brw, ppga, oinfo->oi_capa);
-
-		if (rc != 0)
-			break;
-
-		page_count -= pages_per_brw;
-		ppga += pages_per_brw;
-	}
-
-out:
-	osc_release_ppga(orig, page_count_orig);
-
-	if (saved_oa != NULL)
-		OBDO_FREE(saved_oa);
-
-	return rc;
-}
-
 static int brw_interpret(const struct lu_env *env,
 			 struct ptlrpc_request *req, void *data, int rc)
 {
@@ -2029,8 +1878,8 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 								      CRT_READ;
 	struct ldlm_lock		*lock = NULL;
 	struct cl_req_attr		*crattr = NULL;
-	obd_off				starting_offset = OBD_OBJECT_EOF;
-	obd_off				ending_offset = 0;
+	u64				starting_offset = OBD_OBJECT_EOF;
+	u64				ending_offset = 0;
 	int				mpflag = 0;
 	int				mem_tight = 0;
 	int				page_count = 0;
@@ -2064,16 +1913,22 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 		mpflag = cfs_memory_pressure_get_and_set();
 
 	OBD_ALLOC(crattr, sizeof(*crattr));
-	if (crattr == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (crattr == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	OBD_ALLOC(pga, sizeof(*pga) * page_count);
-	if (pga == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (pga == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	OBDO_ALLOC(oa);
-	if (oa == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (oa == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	i = 0;
 	list_for_each_entry(oap, &rpc_list, oap_rpc_item) {
@@ -2081,8 +1936,10 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 		if (clerq == NULL) {
 			clerq = cl_req_alloc(env, page, crt,
 					     1 /* only 1-object rpcs for now */);
-			if (IS_ERR(clerq))
-				GOTO(out, rc = PTR_ERR(clerq));
+			if (IS_ERR(clerq)) {
+				rc = PTR_ERR(clerq);
+				goto out;
+			}
 			lock = oap->oap_ldlm_lock;
 		}
 		if (mem_tight)
@@ -2108,7 +1965,7 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 	rc = cl_req_prep(env, clerq);
 	if (rc != 0) {
 		CERROR("cl_req_prep failed: %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	sort_brw_pages(pga, page_count);
@@ -2116,7 +1973,7 @@ int osc_build_rpc(const struct lu_env *env, struct client_obd *cli,
 			pga, &req, crattr->cra_capa, 1, 0);
 	if (rc != 0) {
 		CERROR("prep_req failed: %d\n", rc);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	req->rq_interpret_reply = brw_interpret;
@@ -2265,17 +2122,6 @@ static int osc_set_data_with_check(struct lustre_handle *lockh,
 	return set;
 }
 
-static int osc_change_cbdata(struct obd_export *exp, struct lov_stripe_md *lsm,
-			     ldlm_iterator_t replace, void *data)
-{
-	struct ldlm_res_id res_id;
-	struct obd_device *obd = class_exp2obd(exp);
-
-	ostid_build_res_name(&lsm->lsm_oi, &res_id);
-	ldlm_resource_iterate(obd->obd_namespace, &res_id, replace, data);
-	return 0;
-}
-
 /* find any ldlm lock of the inode in osc
  * return 0    not find
  *	1    find one
@@ -2290,10 +2136,10 @@ static int osc_find_cbdata(struct obd_export *exp, struct lov_stripe_md *lsm,
 	ostid_build_res_name(&lsm->lsm_oi, &res_id);
 	rc = ldlm_resource_iterate(obd->obd_namespace, &res_id, replace, data);
 	if (rc == LDLM_ITER_STOP)
-		return(1);
+		return 1;
 	if (rc == LDLM_ITER_CONTINUE)
-		return(0);
-	return(rc);
+		return 0;
+	return rc;
 }
 
 static int osc_enqueue_fini(struct ptlrpc_request *req, struct ost_lvb *lvb,
@@ -2320,7 +2166,7 @@ static int osc_enqueue_fini(struct ptlrpc_request *req, struct ost_lvb *lvb,
 	if ((intent != 0 && rc == ELDLM_LOCK_ABORTED && agl == 0) ||
 	    (rc == 0)) {
 		*flags |= LDLM_FL_LVB_READY;
-		CDEBUG(D_INODE,"got kms %llu blocks %llu mtime %llu\n",
+		CDEBUG(D_INODE, "got kms %llu blocks %llu mtime %llu\n",
 		       lvb->lvb_size, lvb->lvb_blocks, lvb->lvb_mtime);
 	}
 
@@ -2391,50 +2237,6 @@ static int osc_enqueue_interpret(const struct lu_env *env,
 	return rc;
 }
 
-void osc_update_enqueue(struct lustre_handle *lov_lockhp,
-			struct lov_oinfo *loi, __u64 flags,
-			struct ost_lvb *lvb, __u32 mode, int rc)
-{
-	struct ldlm_lock *lock = ldlm_handle2lock(lov_lockhp);
-
-	if (rc == ELDLM_OK) {
-		__u64 tmp;
-
-		LASSERT(lock != NULL);
-		loi->loi_lvb = *lvb;
-		tmp = loi->loi_lvb.lvb_size;
-		/* Extend KMS up to the end of this lock and no further
-		 * A lock on [x,y] means a KMS of up to y + 1 bytes! */
-		if (tmp > lock->l_policy_data.l_extent.end)
-			tmp = lock->l_policy_data.l_extent.end + 1;
-		if (tmp >= loi->loi_kms) {
-			LDLM_DEBUG(lock, "lock acquired, setting rss=%llu, kms=%llu",
-				   loi->loi_lvb.lvb_size, tmp);
-			loi_kms_set(loi, tmp);
-		} else {
-			LDLM_DEBUG(lock, "lock acquired, setting rss=%llu; leaving kms=%llu, end=%llu",
-				   loi->loi_lvb.lvb_size, loi->loi_kms,
-				   lock->l_policy_data.l_extent.end);
-		}
-		ldlm_lock_allow_match(lock);
-	} else if (rc == ELDLM_LOCK_ABORTED && (flags & LDLM_FL_HAS_INTENT)) {
-		LASSERT(lock != NULL);
-		loi->loi_lvb = *lvb;
-		ldlm_lock_allow_match(lock);
-		CDEBUG(D_INODE, "glimpsed, setting rss=%llu; leaving kms=%llu\n",
-		       loi->loi_lvb.lvb_size, loi->loi_kms);
-		rc = ELDLM_OK;
-	}
-
-	if (lock != NULL) {
-		if (rc != ELDLM_OK)
-			ldlm_lock_fail_match(lock);
-
-		LDLM_LOCK_PUT(lock);
-	}
-}
-EXPORT_SYMBOL(osc_update_enqueue);
-
 struct ptlrpc_request_set *PTLRPCD_SET = (void *)1;
 
 /* When enqueuing asynchronously, locks are not ordered, we can obtain a lock
@@ -2588,22 +2390,6 @@ int osc_enqueue_base(struct obd_export *exp, struct ldlm_res_id *res_id,
 	return rc;
 }
 
-static int osc_enqueue(struct obd_export *exp, struct obd_info *oinfo,
-		       struct ldlm_enqueue_info *einfo,
-		       struct ptlrpc_request_set *rqset)
-{
-	struct ldlm_res_id res_id;
-	int rc;
-
-	ostid_build_res_name(&oinfo->oi_md->lsm_oi, &res_id);
-	rc = osc_enqueue_base(exp, &res_id, &oinfo->oi_flags, &oinfo->oi_policy,
-			      &oinfo->oi_md->lsm_oinfo[0]->loi_lvb,
-			      oinfo->oi_md->lsm_oinfo[0]->loi_kms_valid,
-			      oinfo->oi_cb_up, oinfo, einfo, oinfo->oi_lockh,
-			      rqset, rqset != NULL, 0);
-	return rc;
-}
-
 int osc_match_base(struct obd_export *exp, struct ldlm_res_id *res_id,
 		   __u32 type, ldlm_policy_data_t *policy, __u32 mode,
 		   __u64 *flags, void *data, struct lustre_handle *lockh,
@@ -2657,28 +2443,6 @@ int osc_cancel_base(struct lustre_handle *lockh, __u32 mode)
 	return 0;
 }
 
-static int osc_cancel(struct obd_export *exp, struct lov_stripe_md *md,
-		      __u32 mode, struct lustre_handle *lockh)
-{
-	return osc_cancel_base(lockh, mode);
-}
-
-static int osc_cancel_unused(struct obd_export *exp,
-			     struct lov_stripe_md *lsm,
-			     ldlm_cancel_flags_t flags,
-			     void *opaque)
-{
-	struct obd_device *obd = class_exp2obd(exp);
-	struct ldlm_res_id res_id, *resp = NULL;
-
-	if (lsm != NULL) {
-		ostid_build_res_name(&lsm->lsm_oi, &res_id);
-		resp = &res_id;
-	}
-
-	return ldlm_cli_cancel_unused(obd->obd_namespace, resp, flags, opaque);
-}
-
 static int osc_statfs_interpret(const struct lu_env *env,
 				struct ptlrpc_request *req,
 				struct osc_async_args *aa, int rc)
@@ -2694,15 +2458,18 @@ static int osc_statfs_interpret(const struct lu_env *env,
 		 return rc;
 
 	if ((rc == -ENOTCONN || rc == -EAGAIN) &&
-	    (aa->aa_oi->oi_flags & OBD_STATFS_NODELAY))
-		GOTO(out, rc = 0);
+	    (aa->aa_oi->oi_flags & OBD_STATFS_NODELAY)) {
+		rc = 0;
+		goto out;
+	}
 
 	if (rc != 0)
-		GOTO(out, rc);
+		goto out;
 
 	msfs = req_capsule_server_get(&req->rq_pill, &RMF_OBD_STATFS);
 	if (msfs == NULL) {
-		GOTO(out, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out;
 	}
 
 	*aa->aa_oi->oi_osfs = *msfs;
@@ -2802,11 +2569,12 @@ static int osc_statfs(const struct lu_env *env, struct obd_export *exp,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	msfs = req_capsule_server_get(&req->rq_pill, &RMF_OBD_STATFS);
 	if (msfs == NULL) {
-		GOTO(out, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out;
 	}
 
 	*osfs = *msfs;
@@ -2898,19 +2666,23 @@ static int osc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 
 		buf = NULL;
 		len = 0;
-		if (obd_ioctl_getdata(&buf, &len, (void *)uarg))
-			GOTO(out, err = -EINVAL);
+		if (obd_ioctl_getdata(&buf, &len, (void *)uarg)) {
+			err = -EINVAL;
+			goto out;
+		}
 
 		data = (struct obd_ioctl_data *)buf;
 
 		if (sizeof(*desc) > data->ioc_inllen1) {
 			obd_ioctl_freedata(buf, len);
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 
 		if (data->ioc_inllen2 < sizeof(uuid)) {
 			obd_ioctl_freedata(buf, len);
-			GOTO(out, err = -EINVAL);
+			err = -EINVAL;
+			goto out;
 		}
 
 		desc = (struct lov_desc *)data->ioc_inlbuf1;
@@ -2928,36 +2700,37 @@ static int osc_iocontrol(unsigned int cmd, struct obd_export *exp, int len,
 		if (err)
 			err = -EFAULT;
 		obd_ioctl_freedata(buf, len);
-		GOTO(out, err);
+		goto out;
 	}
 	case LL_IOC_LOV_SETSTRIPE:
 		err = obd_alloc_memmd(exp, karg);
 		if (err > 0)
 			err = 0;
-		GOTO(out, err);
+		goto out;
 	case LL_IOC_LOV_GETSTRIPE:
 		err = osc_getstripe(karg, uarg);
-		GOTO(out, err);
+		goto out;
 	case OBD_IOC_CLIENT_RECOVER:
 		err = ptlrpc_recover_import(obd->u.cli.cl_import,
 					    data->ioc_inlbuf1, 0);
 		if (err > 0)
 			err = 0;
-		GOTO(out, err);
+		goto out;
 	case IOC_OSC_SET_ACTIVE:
 		err = ptlrpc_set_import_active(obd->u.cli.cl_import,
 					       data->ioc_offset);
-		GOTO(out, err);
+		goto out;
 	case OBD_IOC_POLL_QUOTACHECK:
 		err = osc_quota_poll_check(exp, (struct if_quotacheck *)karg);
-		GOTO(out, err);
+		goto out;
 	case OBD_IOC_PING_TARGET:
 		err = ptlrpc_obd_ping(obd);
-		GOTO(out, err);
+		goto out;
 	default:
 		CDEBUG(D_INODE, "unrecognised ioctl %#x by %s\n",
 		       cmd, current_comm());
-		GOTO(out, err = -ENOTTY);
+		err = -ENOTTY;
+		goto out;
 	}
 out:
 	module_put(THIS_MODULE);
@@ -2965,7 +2738,7 @@ out:
 }
 
 static int osc_get_info(const struct lu_env *env, struct obd_export *exp,
-			obd_count keylen, void *key, __u32 *vallen, void *val,
+			u32 keylen, void *key, __u32 *vallen, void *val,
 			struct lov_stripe_md *lsm)
 {
 	if (!vallen || !val)
@@ -2978,7 +2751,7 @@ static int osc_get_info(const struct lu_env *env, struct obd_export *exp,
 		return 0;
 	} else if (KEY_IS(KEY_LAST_ID)) {
 		struct ptlrpc_request *req;
-		obd_id		*reply;
+		u64		*reply;
 		char		  *tmp;
 		int		    rc;
 
@@ -3002,13 +2775,15 @@ static int osc_get_info(const struct lu_env *env, struct obd_export *exp,
 		ptlrpc_request_set_replen(req);
 		rc = ptlrpc_queue_wait(req);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 
 		reply = req_capsule_server_get(&req->rq_pill, &RMF_OBD_ID);
-		if (reply == NULL)
-			GOTO(out, rc = -EPROTO);
+		if (reply == NULL) {
+			rc = -EPROTO;
+			goto out;
+		}
 
-		*((obd_id *)val) = *reply;
+		*((u64 *)val) = *reply;
 	out:
 		ptlrpc_req_finished(req);
 		return rc;
@@ -3057,8 +2832,10 @@ static int osc_get_info(const struct lu_env *env, struct obd_export *exp,
 skip_locking:
 		req = ptlrpc_request_alloc(class_exp2cliimp(exp),
 					   &RQF_OST_GET_INFO_FIEMAP);
-		if (req == NULL)
-			GOTO(drop_lock, rc = -ENOMEM);
+		if (req == NULL) {
+			rc = -ENOMEM;
+			goto drop_lock;
+		}
 
 		req_capsule_set_size(&req->rq_pill, &RMF_FIEMAP_KEY,
 				     RCL_CLIENT, keylen);
@@ -3070,7 +2847,7 @@ skip_locking:
 		rc = ptlrpc_request_pack(req, LUSTRE_OST_VERSION, OST_GET_INFO);
 		if (rc) {
 			ptlrpc_request_free(req);
-			GOTO(drop_lock, rc);
+			goto drop_lock;
 		}
 
 		tmp = req_capsule_client_get(&req->rq_pill, &RMF_FIEMAP_KEY);
@@ -3081,11 +2858,13 @@ skip_locking:
 		ptlrpc_request_set_replen(req);
 		rc = ptlrpc_queue_wait(req);
 		if (rc)
-			GOTO(fini_req, rc);
+			goto fini_req;
 
 		reply = req_capsule_server_get(&req->rq_pill, &RMF_FIEMAP_VAL);
-		if (reply == NULL)
-			GOTO(fini_req, rc = -EPROTO);
+		if (reply == NULL) {
+			rc = -EPROTO;
+			goto fini_req;
+		}
 
 		memcpy(val, reply, *vallen);
 fini_req:
@@ -3100,7 +2879,7 @@ drop_lock:
 }
 
 static int osc_set_info_async(const struct lu_env *env, struct obd_export *exp,
-			      obd_count keylen, void *key, obd_count vallen,
+			      u32 keylen, void *key, u32 vallen,
 			      void *val, struct ptlrpc_request_set *set)
 {
 	struct ptlrpc_request *req;
@@ -3190,7 +2969,7 @@ static int osc_set_info_async(const struct lu_env *env, struct obd_export *exp,
 	memcpy(tmp, val, vallen);
 
 	if (KEY_IS(KEY_GRANT_SHRINK)) {
-		struct osc_grant_args *aa;
+		struct osc_brw_async_args *aa;
 		struct obdo *oa;
 
 		CLASSERT(sizeof(*aa) <= sizeof(req->rq_async_args));
@@ -3216,32 +2995,6 @@ static int osc_set_info_async(const struct lu_env *env, struct obd_export *exp,
 	return 0;
 }
 
-
-static int osc_llog_init(struct obd_device *obd, struct obd_llog_group *olg,
-			 struct obd_device *disk_obd, int *index)
-{
-	/* this code is not supposed to be used with LOD/OSP
-	 * to be removed soon */
-	LBUG();
-	return 0;
-}
-
-static int osc_llog_finish(struct obd_device *obd, int count)
-{
-	struct llog_ctxt *ctxt;
-
-	ctxt = llog_get_context(obd, LLOG_MDS_OST_ORIG_CTXT);
-	if (ctxt) {
-		llog_cat_close(NULL, ctxt->loc_handle);
-		llog_cleanup(NULL, ctxt);
-	}
-
-	ctxt = llog_get_context(obd, LLOG_SIZE_REPL_CTXT);
-	if (ctxt)
-		llog_cleanup(NULL, ctxt);
-	return 0;
-}
-
 static int osc_reconnect(const struct lu_env *env,
 			 struct obd_export *exp, struct obd_device *obd,
 			 struct obd_uuid *cluuid,
@@ -3271,22 +3024,8 @@ static int osc_reconnect(const struct lu_env *env,
 static int osc_disconnect(struct obd_export *exp)
 {
 	struct obd_device *obd = class_exp2obd(exp);
-	struct llog_ctxt  *ctxt;
 	int rc;
 
-	ctxt = llog_get_context(obd, LLOG_SIZE_REPL_CTXT);
-	if (ctxt) {
-		if (obd->u.cli.cl_conn_count == 1) {
-			/* Flush any remaining cancel messages out to the
-			 * target */
-			llog_sync(ctxt, exp, 0);
-		}
-		llog_ctxt_put(ctxt);
-	} else {
-		CDEBUG(D_HA, "No LLOG_SIZE_REPL_CTXT found in obd %p\n",
-		       obd);
-	}
-
 	rc = client_disconnect_export(exp);
 	/**
 	 * Initially we put del_shrink_grant before disconnect_export, but it
@@ -3432,16 +3171,18 @@ int osc_setup(struct obd_device *obd, struct lustre_cfg *lcfg)
 
 	rc = client_obd_setup(obd, lcfg);
 	if (rc)
-		GOTO(out_ptlrpcd, rc);
+		goto out_ptlrpcd;
 
 	handler = ptlrpcd_alloc_work(cli->cl_import, brw_queue_work, cli);
-	if (IS_ERR(handler))
-		GOTO(out_client_setup, rc = PTR_ERR(handler));
+	if (IS_ERR(handler)) {
+		rc = PTR_ERR(handler);
+		goto out_client_setup;
+	}
 	cli->cl_writeback_work = handler;
 
 	rc = osc_quota_setup(obd);
 	if (rc)
-		GOTO(out_ptlrpcd_work, rc);
+		goto out_ptlrpcd_work;
 
 	cli->cl_grant_shrink_interval = GRANT_SHRINK_INTERVAL;
 	lprocfs_osc_init_vars(&lvars);
@@ -3509,9 +3250,6 @@ static int osc_precleanup(struct obd_device *obd, enum obd_cleanup_stage stage)
 		obd_cleanup_client_import(obd);
 		ptlrpc_lprocfs_unregister_obd(obd);
 		lprocfs_obd_cleanup(obd);
-		rc = obd_llog_finish(obd, 0);
-		if (rc != 0)
-			CERROR("failed to cleanup llogging subsystems\n");
 		break;
 		}
 	}
@@ -3559,10 +3297,10 @@ int osc_process_config_base(struct obd_device *obd, struct lustre_cfg *lcfg)
 		break;
 	}
 
-	return(rc);
+	return rc;
 }
 
-static int osc_process_config(struct obd_device *obd, obd_count len, void *buf)
+static int osc_process_config(struct obd_device *obd, u32 len, void *buf)
 {
 	return osc_process_config_base(obd, buf);
 }
@@ -3587,20 +3325,11 @@ struct obd_ops osc_obd_ops = {
 	.o_getattr_async	= osc_getattr_async,
 	.o_setattr	      = osc_setattr,
 	.o_setattr_async	= osc_setattr_async,
-	.o_brw		  = osc_brw,
-	.o_punch		= osc_punch,
-	.o_sync		 = osc_sync,
-	.o_enqueue	      = osc_enqueue,
-	.o_change_cbdata	= osc_change_cbdata,
 	.o_find_cbdata	  = osc_find_cbdata,
-	.o_cancel	       = osc_cancel,
-	.o_cancel_unused	= osc_cancel_unused,
 	.o_iocontrol	    = osc_iocontrol,
 	.o_get_info	     = osc_get_info,
 	.o_set_info_async       = osc_set_info_async,
 	.o_import_event	 = osc_import_event,
-	.o_llog_init	    = osc_llog_init,
-	.o_llog_finish	  = osc_llog_finish,
 	.o_process_config       = osc_process_config,
 	.o_quotactl	     = osc_quotactl,
 	.o_quotacheck	   = osc_quotacheck,
diff --git a/drivers/staging/lustre/lustre/ptlrpc/client.c b/drivers/staging/lustre/lustre/ptlrpc/client.c
index 4146e8b29a6d..38cc931a189c 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/client.c
@@ -267,7 +267,7 @@ static void ptlrpc_at_adj_service(struct ptlrpc_request *req,
 	if (oldse != 0)
 		CDEBUG(D_ADAPTTO, "The RPC service estimate for %s ptl %d "
 		       "has changed from %d to %d\n",
-		       req->rq_import->imp_obd->obd_name,req->rq_request_portal,
+		       req->rq_import->imp_obd->obd_name, req->rq_request_portal,
 		       oldse, at_get(&at->iat_service_estimate[idx]));
 }
 
@@ -289,7 +289,7 @@ static void ptlrpc_at_adj_net_latency(struct ptlrpc_request *req,
 	at = &req->rq_import->imp_at;
 
 	/* Network latency is total time less server processing time */
-	nl = max_t(int, now - req->rq_sent - service_time, 0) +1/*st rounding*/;
+	nl = max_t(int, now - req->rq_sent - service_time, 0) + 1/*st rounding*/;
 	if (service_time > now - req->rq_sent + 3 /* bz16408 */)
 		CWARN("Reported service time %u > total measured time "
 		      CFS_DURATION_T"\n", service_time,
@@ -313,14 +313,14 @@ static int unpack_reply(struct ptlrpc_request *req)
 		rc = ptlrpc_unpack_rep_msg(req, req->rq_replen);
 		if (rc) {
 			DEBUG_REQ(D_ERROR, req, "unpack_rep failed: %d", rc);
-			return(-EPROTO);
+			return -EPROTO;
 		}
 	}
 
 	rc = lustre_unpack_rep_ptlrpc_body(req, MSG_PTLRPC_BODY_OFF);
 	if (rc) {
 		DEBUG_REQ(D_ERROR, req, "unpack ptlrpc body failed: %d", rc);
-		return(-EPROTO);
+		return -EPROTO;
 	}
 	return 0;
 }
@@ -394,7 +394,7 @@ void ptlrpc_request_cache_fini(void)
 	kmem_cache_destroy(request_cache);
 }
 
-struct ptlrpc_request *ptlrpc_request_cache_alloc(int flags)
+struct ptlrpc_request *ptlrpc_request_cache_alloc(gfp_t flags)
 {
 	struct ptlrpc_request *req;
 
@@ -578,7 +578,7 @@ static int __ptlrpc_request_bufs_pack(struct ptlrpc_request *request,
 	else {
 		rc = sptlrpc_req_get_ctx(request);
 		if (rc)
-			GOTO(out_free, rc);
+			goto out_free;
 	}
 
 	sptlrpc_req_set_flavor(request, opcode);
@@ -587,7 +587,7 @@ static int __ptlrpc_request_bufs_pack(struct ptlrpc_request *request,
 				 lengths, bufs);
 	if (rc) {
 		LASSERT(!request->rq_pool);
-		GOTO(out_ctx, rc);
+		goto out_ctx;
 	}
 
 	lustre_msg_add_version(request->rq_reqmsg, version);
@@ -721,7 +721,7 @@ struct ptlrpc_request *__ptlrpc_request_alloc(struct obd_import *imp,
  */
 static struct ptlrpc_request *
 ptlrpc_request_alloc_internal(struct obd_import *imp,
-			      struct ptlrpc_request_pool * pool,
+			      struct ptlrpc_request_pool *pool,
 			      const struct req_format *format)
 {
 	struct ptlrpc_request *request;
@@ -751,7 +751,7 @@ EXPORT_SYMBOL(ptlrpc_request_alloc);
  * initialize its buffer structure according to capsule template \a format.
  */
 struct ptlrpc_request *ptlrpc_request_alloc_pool(struct obd_import *imp,
-					    struct ptlrpc_request_pool * pool,
+					    struct ptlrpc_request_pool *pool,
 					    const struct req_format *format)
 {
 	return ptlrpc_request_alloc_internal(imp, pool, format);
@@ -1585,7 +1585,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 			continue;
 
 		if (req->rq_phase == RQ_PHASE_INTERPRET)
-			GOTO(interpret, req->rq_status);
+			goto interpret;
 
 		/*
 		 * Note that this also will start async reply unlink.
@@ -1604,7 +1604,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 				if (req->rq_status == 0)
 					req->rq_status = -EIO;
 				ptlrpc_rqphase_move(req, RQ_PHASE_INTERPRET);
-				GOTO(interpret, req->rq_status);
+				goto interpret;
 			} else {
 				continue;
 			}
@@ -1617,7 +1617,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 			if (req->rq_status == 0)
 				req->rq_status = -EIO;
 			ptlrpc_rqphase_move(req, RQ_PHASE_INTERPRET);
-			GOTO(interpret, req->rq_status);
+			goto interpret;
 		}
 
 		/* ptlrpc_set_wait->l_wait_event sets lwi_allow_intr
@@ -1633,7 +1633,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 				     req->rq_wait_ctx)) {
 			req->rq_status = -EINTR;
 			ptlrpc_rqphase_move(req, RQ_PHASE_INTERPRET);
-			GOTO(interpret, req->rq_status);
+			goto interpret;
 		}
 
 		if (req->rq_phase == RQ_PHASE_RPC) {
@@ -1662,7 +1662,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 					ptlrpc_rqphase_move(req,
 						RQ_PHASE_INTERPRET);
 					spin_unlock(&imp->imp_lock);
-					GOTO(interpret, req->rq_status);
+					goto interpret;
 				}
 				if (ptlrpc_no_resend(req) &&
 				    !req->rq_wait_ctx) {
@@ -1670,7 +1670,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 					ptlrpc_rqphase_move(req,
 							    RQ_PHASE_INTERPRET);
 					spin_unlock(&imp->imp_lock);
-					GOTO(interpret, req->rq_status);
+					goto interpret;
 				}
 
 				list_del_init(&req->rq_list);
@@ -1780,7 +1780,7 @@ int ptlrpc_check_set(const struct lu_env *env, struct ptlrpc_request_set *set)
 			 */
 			if (req->rq_bulk == NULL || req->rq_status < 0) {
 				ptlrpc_rqphase_move(req, RQ_PHASE_INTERPRET);
-				GOTO(interpret, req->rq_status);
+				goto interpret;
 			}
 
 			ptlrpc_rqphase_move(req, RQ_PHASE_BULK);
@@ -2041,8 +2041,6 @@ int ptlrpc_set_next_timeout(struct ptlrpc_request_set *set)
 	struct ptlrpc_request *req;
 	int		    deadline;
 
-	SIGNAL_MASK_ASSERT(); /* XXX BUG 1511 */
-
 	list_for_each(tmp, &set->set_requests) {
 		req = list_entry(tmp, struct ptlrpc_request, rq_set_chain);
 
@@ -2132,7 +2130,7 @@ int ptlrpc_set_wait(struct ptlrpc_request_set *set)
 			 * interrupts are allowed. Wait until all
 			 * complete, or an in-flight req times out.
 			 */
-			lwi = LWI_TIMEOUT(cfs_time_seconds(timeout? timeout : 1),
+			lwi = LWI_TIMEOUT(cfs_time_seconds(timeout ? timeout : 1),
 					  ptlrpc_expired_set, set);
 
 		rc = l_wait_event(set->set_waitq, ptlrpc_check_set(NULL, set), &lwi);
@@ -2186,7 +2184,7 @@ int ptlrpc_set_wait(struct ptlrpc_request_set *set)
 	}
 
 	if (set->set_interpret != NULL) {
-		int (*interpreter)(struct ptlrpc_request_set *set,void *,int) =
+		int (*interpreter)(struct ptlrpc_request_set *set, void *, int) =
 			set->set_interpret;
 		rc = interpreter (set, set->set_arg, rc);
 	} else {
@@ -2222,7 +2220,7 @@ static void __ptlrpc_free_req(struct ptlrpc_request *request, int locked)
 	}
 
 	LASSERTF(!request->rq_receiving_reply, "req %p\n", request);
-	LASSERTF(request->rq_rqbd == NULL, "req %p\n",request);/* client-side */
+	LASSERTF(request->rq_rqbd == NULL, "req %p\n", request);/* client-side */
 	LASSERTF(list_empty(&request->rq_list), "req %p\n", request);
 	LASSERTF(list_empty(&request->rq_set_chain), "req %p\n", request);
 	LASSERTF(list_empty(&request->rq_exp_list), "req %p\n", request);
@@ -2491,7 +2489,7 @@ void ptlrpc_free_committed(struct obd_import *imp)
 		}
 		if (req->rq_import_generation < imp->imp_generation) {
 			DEBUG_REQ(D_RPCTRACE, req, "free request with old gen");
-			GOTO(free_req, 0);
+			goto free_req;
 		}
 
 		/* not yet committed */
@@ -2692,7 +2690,7 @@ struct ptlrpc_replay_async_args {
  */
 static int ptlrpc_replay_interpret(const struct lu_env *env,
 				   struct ptlrpc_request *req,
-				   void * data, int rc)
+				   void *data, int rc)
 {
 	struct ptlrpc_replay_async_args *aa = data;
 	struct obd_import *imp = req->rq_import;
@@ -2701,13 +2699,16 @@ static int ptlrpc_replay_interpret(const struct lu_env *env,
 
 	if (!ptlrpc_client_replied(req)) {
 		CERROR("request replay timed out, restarting recovery\n");
-		GOTO(out, rc = -ETIMEDOUT);
+		rc = -ETIMEDOUT;
+		goto out;
 	}
 
 	if (lustre_msg_get_type(req->rq_repmsg) == PTL_RPC_MSG_ERR &&
 	    (lustre_msg_get_status(req->rq_repmsg) == -ENOTCONN ||
-	     lustre_msg_get_status(req->rq_repmsg) == -ENODEV))
-		GOTO(out, rc = lustre_msg_get_status(req->rq_repmsg));
+	     lustre_msg_get_status(req->rq_repmsg) == -ENODEV)) {
+		rc = lustre_msg_get_status(req->rq_repmsg);
+		goto out;
+	}
 
 	/** VBR: check version failure */
 	if (lustre_msg_get_status(req->rq_repmsg) == -EOVERFLOW) {
@@ -2742,7 +2743,8 @@ static int ptlrpc_replay_interpret(const struct lu_env *env,
 			  "Reported transno %llu is bigger than the replayed one: %llu",
 			  req->rq_transno,
 			  lustre_msg_get_transno(req->rq_reqmsg));
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	DEBUG_REQ(D_HA, req, "got rep");
diff --git a/drivers/staging/lustre/lustre/ptlrpc/connection.c b/drivers/staging/lustre/lustre/ptlrpc/connection.c
index adff1ab4f5a4..2a875ab57911 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/connection.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/connection.c
@@ -41,7 +41,7 @@
 
 #include "ptlrpc_internal.h"
 
-static struct cfs_hash *conn_hash = NULL;
+static struct cfs_hash *conn_hash;
 static cfs_hash_ops_t conn_hash_ops;
 
 struct ptlrpc_connection *
@@ -52,7 +52,7 @@ ptlrpc_connection_get(lnet_process_id_t peer, lnet_nid_t self,
 
 	conn = cfs_hash_lookup(conn_hash, &peer);
 	if (conn)
-		GOTO(out, conn);
+		goto out;
 
 	OBD_ALLOC_PTR(conn);
 	if (!conn)
@@ -173,7 +173,7 @@ conn_keycmp(const void *key, struct hlist_node *hnode)
 	const lnet_process_id_t *conn_key;
 
 	LASSERT(key != NULL);
-	conn_key = (lnet_process_id_t*)key;
+	conn_key = (lnet_process_id_t *)key;
 	conn = hlist_entry(hnode, struct ptlrpc_connection, c_hash);
 
 	return conn_key->nid == conn->c_peer.nid &&
diff --git a/drivers/staging/lustre/lustre/ptlrpc/events.c b/drivers/staging/lustre/lustre/ptlrpc/events.c
index c3ec21d5d29f..32dfffa76a5e 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/events.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/events.c
@@ -186,7 +186,8 @@ void client_bulk_callback(lnet_event_t *ev)
 	if (CFS_FAIL_CHECK_ORSET(OBD_FAIL_PTLRPC_CLIENT_BULK_CB, CFS_FAIL_ONCE))
 		ev->status = -EIO;
 
-	if (CFS_FAIL_CHECK_ORSET(OBD_FAIL_PTLRPC_CLIENT_BULK_CB2,CFS_FAIL_ONCE))
+	if (CFS_FAIL_CHECK_ORSET(OBD_FAIL_PTLRPC_CLIENT_BULK_CB2,
+				 CFS_FAIL_ONCE))
 		ev->status = -EIO;
 
 	CDEBUG((ev->status == 0) ? D_NET : D_ERROR,
@@ -481,7 +482,7 @@ int ptlrpc_uuid_to_peer(struct obd_uuid *uuid,
 		}
 	}
 
-	CDEBUG(D_NET,"%s->%s\n", uuid->uuid, libcfs_id2str(*peer));
+	CDEBUG(D_NET, "%s->%s\n", uuid->uuid, libcfs_id2str(*peer));
 	return rc;
 }
 
@@ -541,7 +542,7 @@ int ptlrpc_ni_init(void)
 	rc = LNetNIInit(pid);
 	if (rc < 0) {
 		CDEBUG(D_NET, "Can't init network interface: %d\n", rc);
-		return (-ENOENT);
+		return -ENOENT;
 	}
 
 	/* CAVEAT EMPTOR: how we process portals events is _radically_
@@ -557,7 +558,7 @@ int ptlrpc_ni_init(void)
 	CERROR("Failed to allocate event queue: %d\n", rc);
 	LNetNIFini();
 
-	return (-ENOMEM);
+	return -ENOMEM;
 }
 
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/import.c b/drivers/staging/lustre/lustre/ptlrpc/import.c
index f522fc5d3a93..2e7e7171ca63 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/import.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/import.c
@@ -93,7 +93,7 @@ do {									\
 
 static int ptlrpc_connect_interpret(const struct lu_env *env,
 				    struct ptlrpc_request *request,
-				    void * data, int rc);
+				    void *data, int rc);
 int ptlrpc_import_recovery_state_machine(struct obd_import *imp);
 
 /* Only this function is allowed to change the import state when it is
@@ -297,7 +297,8 @@ void ptlrpc_invalidate_import(struct obd_import *imp)
 			timeout = 1;
 		}
 
-		CDEBUG(D_RPCTRACE,"Sleeping %d sec for inflight to error out\n",
+		CDEBUG(D_RPCTRACE,
+		       "Sleeping %d sec for inflight to error out\n",
 		       timeout);
 
 		/* Wait for all requests to error out and call completion
@@ -668,11 +669,11 @@ int ptlrpc_connect_import(struct obd_import *imp)
 
 	rc = import_select_connection(imp);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	rc = sptlrpc_import_sec_adapt(imp, NULL, NULL);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* Reset connect flags to the originally requested flags, in case
 	 * the server is updated on-the-fly we will get the new features. */
@@ -685,17 +686,19 @@ int ptlrpc_connect_import(struct obd_import *imp)
 	rc = obd_reconnect(NULL, imp->imp_obd->obd_self_export, obd,
 			   &obd->obd_uuid, &imp->imp_connect_data, NULL);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	request = ptlrpc_request_alloc(imp, &RQF_MDS_CONNECT);
-	if (request == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (request == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	rc = ptlrpc_request_bufs_pack(request, LUSTRE_OBD_VERSION,
 				      imp->imp_connect_op, bufs, NULL);
 	if (rc) {
 		ptlrpc_request_free(request);
-		GOTO(out, rc);
+		goto out;
 	}
 
 	/* Report the rpc service time to the server so that it knows how long
@@ -803,7 +806,7 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 		imp->imp_force_reconnect = ptlrpc_busy_reconnect(rc);
 		spin_unlock(&imp->imp_lock);
 		ptlrpc_maybe_ping_import_soon(imp);
-		GOTO(out, rc);
+		goto out;
 	}
 	spin_unlock(&imp->imp_lock);
 
@@ -821,7 +824,7 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 		CERROR("%s: no connect data from server\n",
 		       imp->imp_obd->obd_name);
 		rc = -EPROTO;
-		GOTO(out, rc);
+		goto out;
 	}
 
 	spin_lock(&imp->imp_lock);
@@ -843,9 +846,10 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 	if ((ocd->ocd_connect_flags & imp->imp_connect_flags_orig) !=
 	    ocd->ocd_connect_flags) {
 		CERROR("%s: Server didn't granted asked subset of flags: asked=%#llx grranted=%#llx\n",
-		       imp->imp_obd->obd_name,imp->imp_connect_flags_orig,
+		       imp->imp_obd->obd_name, imp->imp_connect_flags_orig,
 		       ocd->ocd_connect_flags);
-		GOTO(out, rc = -EPROTO);
+		rc = -EPROTO;
+		goto out;
 	}
 
 	if (!exp) {
@@ -853,7 +857,8 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 		   connect attempt */
 		CERROR("%s: missing export after connect\n",
 		       imp->imp_obd->obd_name);
-		GOTO(out, rc = -ENODEV);
+		rc = -ENODEV;
+		goto out;
 	}
 	old_connect_flags = exp_connect_flags(exp);
 	exp->exp_connect_data = *ocd;
@@ -892,7 +897,8 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 			ptlrpc_activate_import(imp);
 		}
 
-		GOTO(finish, rc = 0);
+		rc = 0;
+		goto finish;
 	}
 
 	/* Determine what recovery state to move the import to. */
@@ -904,7 +910,8 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 				      obd2cli_tgt(imp->imp_obd),
 				      imp->imp_connection->c_remote_uuid.uuid,
 				      imp->imp_dlm_handle.cookie);
-			GOTO(out, rc = -ENOTCONN);
+			rc = -ENOTCONN;
+			goto out;
 		}
 
 		if (memcmp(&imp->imp_remote_handle,
@@ -921,7 +928,7 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 			 * participate since we can reestablish all of our state
 			 * with server again */
 			if ((MSG_CONNECT_RECOVERING & msg_flags)) {
-				CDEBUG(level,"%s@%s changed server handle from %#llx to %#llx but is still in recovery\n",
+				CDEBUG(level, "%s@%s changed server handle from %#llx to %#llx but is still in recovery\n",
 				       obd2cli_tgt(imp->imp_obd),
 				       imp->imp_connection->c_remote_uuid.uuid,
 				       imp->imp_remote_handle.cookie,
@@ -944,7 +951,8 @@ static int ptlrpc_connect_interpret(const struct lu_env *env,
 
 			if (!(MSG_CONNECT_RECOVERING & msg_flags)) {
 				IMPORT_SET_STATE(imp, LUSTRE_IMP_EVICTED);
-				GOTO(finish, rc = 0);
+				rc = 0;
+				goto finish;
 			}
 
 		} else {
@@ -1024,10 +1032,18 @@ finish:
 
 		spin_unlock(&imp->imp_lock);
 
-		if (!ocd->ocd_ibits_known &&
-		    ocd->ocd_connect_flags & OBD_CONNECT_IBITS)
-			CERROR("Inodebits aware server returned zero compatible"
-			       " bits?\n");
+		if ((imp->imp_connect_flags_orig & OBD_CONNECT_IBITS) &&
+		    !(ocd->ocd_connect_flags & OBD_CONNECT_IBITS)) {
+			LCONSOLE_WARN("%s: MDS %s does not support ibits "
+				      "lock, either very old or invalid: "
+				      "requested %llx, replied %llx\n",
+				      imp->imp_obd->obd_name,
+				      imp->imp_connection->c_remote_uuid.uuid,
+				      imp->imp_connect_flags_orig,
+				      ocd->ocd_connect_flags);
+			rc = -EPROTO;
+			goto out;
+		}
 
 		if ((ocd->ocd_connect_flags & OBD_CONNECT_VERSION) &&
 		    (ocd->ocd_version > LUSTRE_VERSION_CODE +
@@ -1096,7 +1112,7 @@ finish:
 			 * Enforce ADLER for backward compatibility*/
 			cli->cl_supp_cksum_types = OBD_CKSUM_ADLER;
 		}
-		cli->cl_cksum_type =cksum_type_select(cli->cl_supp_cksum_types);
+		cli->cl_cksum_type = cksum_type_select(cli->cl_supp_cksum_types);
 
 		if (ocd->ocd_connect_flags & OBD_CONNECT_BRW_SIZE)
 			cli->cl_max_pages_per_rpc =
@@ -1209,7 +1225,7 @@ out:
  */
 static int completed_replay_interpret(const struct lu_env *env,
 				      struct ptlrpc_request *req,
-				      void * data, int rc)
+				      void *data, int rc)
 {
 	atomic_dec(&req->rq_import->imp_replay_inflight);
 	if (req->rq_status == 0 &&
@@ -1370,7 +1386,7 @@ int ptlrpc_import_recovery_state_machine(struct obd_import *imp)
 			IMPORT_SET_STATE(imp, LUSTRE_IMP_REPLAY_LOCKS);
 			rc = ldlm_replay_locks(imp);
 			if (rc)
-				GOTO(out, rc);
+				goto out;
 		}
 		rc = 0;
 	}
@@ -1380,7 +1396,7 @@ int ptlrpc_import_recovery_state_machine(struct obd_import *imp)
 			IMPORT_SET_STATE(imp, LUSTRE_IMP_REPLAY_WAIT);
 			rc = signal_completed_replay(imp);
 			if (rc)
-				GOTO(out, rc);
+				goto out;
 		}
 
 	}
@@ -1398,7 +1414,7 @@ int ptlrpc_import_recovery_state_machine(struct obd_import *imp)
 
 		rc = ptlrpc_resend(imp);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 		IMPORT_SET_STATE(imp, LUSTRE_IMP_FULL);
 		ptlrpc_activate_import(imp);
 
@@ -1425,7 +1441,7 @@ int ptlrpc_disconnect_import(struct obd_import *imp, int noclose)
 	int rq_opc, rc = 0;
 
 	if (imp->imp_obd->obd_force)
-		GOTO(set_state, rc);
+		goto set_state;
 
 	switch (imp->imp_connect_op) {
 	case OST_CONNECT:
@@ -1471,7 +1487,7 @@ int ptlrpc_disconnect_import(struct obd_import *imp, int noclose)
 
 	spin_lock(&imp->imp_lock);
 	if (imp->imp_state != LUSTRE_IMP_FULL)
-		GOTO(out, 0);
+		goto out;
 	spin_unlock(&imp->imp_lock);
 
 	req = ptlrpc_request_alloc_pack(imp, &RQF_MDS_DISCONNECT,
diff --git a/drivers/staging/lustre/lustre/ptlrpc/layout.c b/drivers/staging/lustre/lustre/ptlrpc/layout.c
index 511cb9cbf0d4..5b8337187b59 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/layout.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/layout.c
@@ -1094,8 +1094,8 @@ struct req_msg_field RMF_EAVALS_LENS =
 EXPORT_SYMBOL(RMF_EAVALS_LENS);
 
 struct req_msg_field RMF_OBD_ID =
-	DEFINE_MSGF("obd_id", 0,
-		    sizeof(obd_id), lustre_swab_ost_last_id, NULL);
+	DEFINE_MSGF("u64", 0,
+		    sizeof(u64), lustre_swab_ost_last_id, NULL);
 EXPORT_SYMBOL(RMF_OBD_ID);
 
 struct req_msg_field RMF_FID =
diff --git a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
index 6a1ab5c98bc6..56f825fbb17c 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/llog_client.c
@@ -93,8 +93,10 @@ static int llog_client_open(const struct lu_env *env,
 	LASSERT(lgh);
 
 	req = ptlrpc_request_alloc(imp, &RQF_LLOG_ORIGIN_HANDLE_CREATE);
-	if (req == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	if (name)
 		req_capsule_set_size(&req->rq_pill, &RMF_NAME, RCL_CLIENT,
@@ -105,7 +107,7 @@ static int llog_client_open(const struct lu_env *env,
 	if (rc) {
 		ptlrpc_request_free(req);
 		req = NULL;
-		GOTO(out, rc);
+		goto out;
 	}
 	ptlrpc_request_set_replen(req);
 
@@ -124,11 +126,13 @@ static int llog_client_open(const struct lu_env *env,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_LLOGD_BODY);
-	if (body == NULL)
-		GOTO(out, rc = -EFAULT);
+	if (body == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	lgh->lgh_id = body->lgd_logid;
 	lgh->lgh_ctxt = ctxt;
@@ -150,8 +154,10 @@ static int llog_client_destroy(const struct lu_env *env,
 	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_DESTROY,
 					LUSTRE_LOG_VERSION,
 					LLOG_ORIGIN_HANDLE_DESTROY);
-	if (req == NULL)
-		GOTO(err_exit, rc =-ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto err_exit;
+	}
 
 	body = req_capsule_client_get(&req->rq_pill, &RMF_LLOGD_BODY);
 	body->lgd_logid = loghandle->lgh_id;
@@ -186,8 +192,10 @@ static int llog_client_next_block(const struct lu_env *env,
 	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_NEXT_BLOCK,
 					LUSTRE_LOG_VERSION,
 					LLOG_ORIGIN_HANDLE_NEXT_BLOCK);
-	if (req == NULL)
-		GOTO(err_exit, rc =-ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto err_exit;
+	}
 
 	body = req_capsule_client_get(&req->rq_pill, &RMF_LLOGD_BODY);
 	body->lgd_logid = loghandle->lgh_id;
@@ -202,16 +210,20 @@ static int llog_client_next_block(const struct lu_env *env,
 	ptlrpc_request_set_replen(req);
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_LLOGD_BODY);
-	if (body == NULL)
-		GOTO(out, rc =-EFAULT);
+	if (body == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	/* The log records are swabbed as they are processed */
 	ptr = req_capsule_server_get(&req->rq_pill, &RMF_EADATA);
-	if (ptr == NULL)
-		GOTO(out, rc =-EFAULT);
+	if (ptr == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	*cur_idx = body->lgd_saved_index;
 	*cur_offset = body->lgd_cur_offset;
@@ -238,8 +250,10 @@ static int llog_client_prev_block(const struct lu_env *env,
 	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_PREV_BLOCK,
 					LUSTRE_LOG_VERSION,
 					LLOG_ORIGIN_HANDLE_PREV_BLOCK);
-	if (req == NULL)
-		GOTO(err_exit, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto err_exit;
+	}
 
 	body = req_capsule_client_get(&req->rq_pill, &RMF_LLOGD_BODY);
 	body->lgd_logid = loghandle->lgh_id;
@@ -253,15 +267,19 @@ static int llog_client_prev_block(const struct lu_env *env,
 
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	body = req_capsule_server_get(&req->rq_pill, &RMF_LLOGD_BODY);
-	if (body == NULL)
-		GOTO(out, rc =-EFAULT);
+	if (body == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	ptr = req_capsule_server_get(&req->rq_pill, &RMF_EADATA);
-	if (ptr == NULL)
-		GOTO(out, rc =-EFAULT);
+	if (ptr == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	memcpy(buf, ptr, len);
 out:
@@ -282,11 +300,13 @@ static int llog_client_read_header(const struct lu_env *env,
 	int		    rc;
 
 	LLOG_CLIENT_ENTRY(handle->lgh_ctxt, imp);
-	req = ptlrpc_request_alloc_pack(imp,&RQF_LLOG_ORIGIN_HANDLE_READ_HEADER,
+	req = ptlrpc_request_alloc_pack(imp, &RQF_LLOG_ORIGIN_HANDLE_READ_HEADER,
 					LUSTRE_LOG_VERSION,
 					LLOG_ORIGIN_HANDLE_READ_HEADER);
-	if (req == NULL)
-		GOTO(err_exit, rc = -ENOMEM);
+	if (req == NULL) {
+		rc = -ENOMEM;
+		goto err_exit;
+	}
 
 	body = req_capsule_client_get(&req->rq_pill, &RMF_LLOGD_BODY);
 	body->lgd_logid = handle->lgh_id;
@@ -296,11 +316,13 @@ static int llog_client_read_header(const struct lu_env *env,
 	ptlrpc_request_set_replen(req);
 	rc = ptlrpc_queue_wait(req);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	hdr = req_capsule_server_get(&req->rq_pill, &RMF_LLOG_LOG_HDR);
-	if (hdr == NULL)
-		GOTO(out, rc =-EFAULT);
+	if (hdr == NULL) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	memcpy(handle->lgh_hdr, hdr, sizeof(*hdr));
 	handle->lgh_last_idx = handle->lgh_hdr->llh_tail.lrt_index;
@@ -331,7 +353,7 @@ static int llog_client_close(const struct lu_env *env,
 	/* this doesn't call LLOG_ORIGIN_HANDLE_CLOSE because
 	   the servers all close the file at the end of every
 	   other LLOG_ RPC. */
-	return(0);
+	return 0;
 }
 
 struct llog_operations llog_client_ops = {
diff --git a/drivers/staging/lustre/lustre/ptlrpc/llog_net.c b/drivers/staging/lustre/lustre/ptlrpc/llog_net.c
index e9052bba6692..dac66f5b39da 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/llog_net.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/llog_net.c
@@ -51,8 +51,6 @@
 #include "../include/obd_class.h"
 #include "../include/lustre_log.h"
 #include <linux/list.h>
-#include "../include/lvfs.h"
-#include "../include/lustre_fsfilt.h"
 
 int llog_initiator_connect(struct llog_ctxt *ctxt)
 {
diff --git a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
index bc220308e7d7..87b9764a4f19 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/lproc_ptlrpc.c
@@ -119,7 +119,7 @@ struct ll_rpc_opcode {
 	{ OBD_IDX_READ,	    "dt_index_read" },
 	{ LLOG_ORIGIN_HANDLE_CREATE,	 "llog_origin_handle_open" },
 	{ LLOG_ORIGIN_HANDLE_NEXT_BLOCK, "llog_origin_handle_next_block" },
-	{ LLOG_ORIGIN_HANDLE_READ_HEADER,"llog_origin_handle_read_header" },
+	{ LLOG_ORIGIN_HANDLE_READ_HEADER, "llog_origin_handle_read_header" },
 	{ LLOG_ORIGIN_HANDLE_WRITE_REC,  "llog_origin_handle_write_rec" },
 	{ LLOG_ORIGIN_HANDLE_CLOSE,      "llog_origin_handle_close" },
 	{ LLOG_ORIGIN_CONNECT,	   "llog_origin_connect" },
@@ -130,7 +130,7 @@ struct ll_rpc_opcode {
 	{ QUOTA_DQREL,      "quota_release" },
 	{ SEQ_QUERY,	"seq_query" },
 	{ SEC_CTX_INIT,     "sec_ctx_init" },
-	{ SEC_CTX_INIT_CONT,"sec_ctx_init_cont" },
+	{ SEC_CTX_INIT_CONT, "sec_ctx_init_cont" },
 	{ SEC_CTX_FINI,     "sec_ctx_fini" },
 	{ FLD_QUERY,	"fld_query" },
 	{ UPDATE_OBJ,	    "update_obj" },
@@ -194,7 +194,8 @@ void ptlrpc_lprocfs_register(struct proc_dir_entry *root, char *dir,
 	LASSERT(*procroot_ret == NULL);
 	LASSERT(*stats_ret == NULL);
 
-	svc_stats = lprocfs_alloc_stats(EXTRA_MAX_OPCODES+LUSTRE_MAX_OPCODES,0);
+	svc_stats = lprocfs_alloc_stats(EXTRA_MAX_OPCODES+LUSTRE_MAX_OPCODES,
+					0);
 	if (svc_stats == NULL)
 		return;
 
@@ -499,8 +500,10 @@ static int ptlrpc_lprocfs_nrs_seq_show(struct seq_file *m, void *n)
 	spin_unlock(&nrs->nrs_lock);
 
 	OBD_ALLOC(infos, num_pols * sizeof(*infos));
-	if (infos == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (infos == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 again:
 
 	ptlrpc_service_for_each_part(svcpt, i, svc) {
@@ -639,26 +642,34 @@ static ssize_t ptlrpc_lprocfs_nrs_seq_write(struct file *file,
 	char			       *token;
 	int				rc = 0;
 
-	if (count >= LPROCFS_NRS_WR_MAX_CMD)
-		GOTO(out, rc = -EINVAL);
+	if (count >= LPROCFS_NRS_WR_MAX_CMD) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	OBD_ALLOC(cmd, LPROCFS_NRS_WR_MAX_CMD);
-	if (cmd == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (cmd == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 	/**
 	 * strsep() modifies its argument, so keep a copy
 	 */
 	cmd_copy = cmd;
 
-	if (copy_from_user(cmd, buffer, count))
-		GOTO(out, rc = -EFAULT);
+	if (copy_from_user(cmd, buffer, count)) {
+		rc = -EFAULT;
+		goto out;
+	}
 
 	cmd[count] = '\0';
 
 	token = strsep(&cmd, " ");
 
-	if (strlen(token) > NRS_POL_NAME_MAX - 1)
-		GOTO(out, rc = -EINVAL);
+	if (strlen(token) > NRS_POL_NAME_MAX - 1) {
+		rc = -EINVAL;
+		goto out;
+	}
 
 	/**
 	 * No [reg|hp] token has been specified
@@ -673,13 +684,17 @@ static ssize_t ptlrpc_lprocfs_nrs_seq_write(struct file *file,
 		queue = PTLRPC_NRS_QUEUE_REG;
 	else if (strcmp(cmd, "hp") == 0)
 		queue = PTLRPC_NRS_QUEUE_HP;
-	else
-		GOTO(out, rc = -EINVAL);
+	else {
+		rc = -EINVAL;
+		goto out;
+	}
 
 default_queue:
 
-	if (queue == PTLRPC_NRS_QUEUE_HP && !nrs_svc_has_hp(svc))
-		GOTO(out, rc = -ENODEV);
+	if (queue == PTLRPC_NRS_QUEUE_HP && !nrs_svc_has_hp(svc)) {
+		rc = -ENODEV;
+		goto out;
+	}
 	else if (queue == PTLRPC_NRS_QUEUE_BOTH && !nrs_svc_has_hp(svc))
 		queue = PTLRPC_NRS_QUEUE_REG;
 
@@ -1257,14 +1272,18 @@ int lprocfs_wr_import(struct file *file, const char *buffer,
 	if (kbuf == NULL)
 		return -ENOMEM;
 
-	if (copy_from_user(kbuf, buffer, count))
-		GOTO(out, count = -EFAULT);
+	if (copy_from_user(kbuf, buffer, count)) {
+		count = -EFAULT;
+		goto out;
+	}
 
 	kbuf[count] = 0;
 
 	/* only support connection=uuid::instance now */
-	if (strncmp(prefix, kbuf, prefix_len) != 0)
-		GOTO(out, count = -EINVAL);
+	if (strncmp(prefix, kbuf, prefix_len) != 0) {
+		count = -EINVAL;
+		goto out;
+	}
 
 	uuid = kbuf + prefix_len;
 	ptr = strstr(uuid, "::");
diff --git a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
index 89fc7f77b498..c1e8aa4d5ec4 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/niobuf.c
@@ -442,8 +442,7 @@ int ptlrpc_reply(struct ptlrpc_request *req)
 {
 	if (req->rq_no_reply)
 		return 0;
-	else
-		return (ptlrpc_send_reply(req, 0));
+	return ptlrpc_send_reply(req, 0);
 }
 EXPORT_SYMBOL(ptlrpc_reply);
 
@@ -537,13 +536,13 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 
 	rc = sptlrpc_cli_wrap_request(request);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* bulk register should be done after wrap_request() */
 	if (request->rq_bulk != NULL) {
 		rc = ptlrpc_register_bulk(request);
 		if (rc != 0)
-			GOTO(out, rc);
+			goto out;
 	}
 
 	if (!noreply) {
@@ -560,7 +559,7 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 				request->rq_err = 1;
 				spin_unlock(&request->rq_lock);
 				request->rq_status = rc;
-				GOTO(cleanup_bulk, rc);
+				goto cleanup_bulk;
 			}
 		} else {
 			request->rq_repdata = NULL;
@@ -573,7 +572,8 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 		if (rc != 0) {
 			CERROR("LNetMEAttach failed: %d\n", rc);
 			LASSERT(rc == -ENOMEM);
-			GOTO(cleanup_bulk, rc = -ENOMEM);
+			rc = -ENOMEM;
+			goto cleanup_bulk;
 		}
 	}
 
@@ -616,7 +616,8 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 			/* ...but the MD attach didn't succeed... */
 			request->rq_receiving_reply = 0;
 			spin_unlock(&request->rq_lock);
-			GOTO(cleanup_me, rc = -ENOMEM);
+			rc = -ENOMEM;
+			goto cleanup_me;
 		}
 
 		CDEBUG(D_NET, "Setup reply buffer: %u bytes, xid %llu, portal %u\n",
@@ -650,11 +651,11 @@ int ptl_send_rpc(struct ptlrpc_request *request, int noreply)
 			  request->rq_request_portal,
 			  request->rq_xid, 0);
 	if (rc == 0)
-		GOTO(out, rc);
+		goto out;
 
 	ptlrpc_req_finished(request);
 	if (noreply)
-		GOTO(out, rc);
+		goto out;
 
  cleanup_me:
 	/* MEUnlink is safe; the PUT didn't even get off the ground, and
@@ -691,7 +692,7 @@ int ptlrpc_register_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 	       service->srv_req_portal);
 
 	if (OBD_FAIL_CHECK(OBD_FAIL_PTLRPC_RQBD))
-		return (-ENOMEM);
+		return -ENOMEM;
 
 	/* NB: CPT affinity service should use new LNet flag LNET_INS_LOCAL,
 	 * which means buffer can only be attached on local CPT, and LND
@@ -702,7 +703,7 @@ int ptlrpc_register_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 			  LNET_INS_LOCAL : LNET_INS_AFTER, &me_h);
 	if (rc != 0) {
 		CERROR("LNetMEAttach failed: %d\n", rc);
-		return (-ENOMEM);
+		return -ENOMEM;
 	}
 
 	LASSERT(rqbd->rqbd_refcount == 0);
@@ -718,7 +719,7 @@ int ptlrpc_register_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 
 	rc = LNetMDAttach(me_h, md, LNET_UNLINK, &rqbd->rqbd_md_h);
 	if (rc == 0)
-		return (0);
+		return 0;
 
 	CERROR("LNetMDAttach failed: %d;\n", rc);
 	LASSERT(rc == -ENOMEM);
@@ -726,5 +727,5 @@ int ptlrpc_register_rqbd(struct ptlrpc_request_buffer_desc *rqbd)
 	LASSERT(rc == 0);
 	rqbd->rqbd_refcount = 0;
 
-	return (-ENOMEM);
+	return -ENOMEM;
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs.c b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
index 9ea24f8d9865..181301bd2083 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs.c
@@ -264,7 +264,7 @@ static int nrs_policy_start_locked(struct ptlrpc_nrs_policy *policy)
 				module_put(policy->pol_desc->pd_owner);
 
 			policy->pol_state = NRS_POL_STATE_STOPPED;
-			GOTO(out, rc);
+			goto out;
 		}
 	}
 
@@ -327,7 +327,7 @@ static void nrs_policy_put(struct ptlrpc_nrs_policy *policy)
 /**
  * Find and return a policy by name.
  */
-static struct ptlrpc_nrs_policy * nrs_policy_find_locked(struct ptlrpc_nrs *nrs,
+static struct ptlrpc_nrs_policy *nrs_policy_find_locked(struct ptlrpc_nrs *nrs,
 							 char *name)
 {
 	struct ptlrpc_nrs_policy *tmp;
@@ -377,7 +377,7 @@ static void nrs_resource_put(struct ptlrpc_nrs_resource *res)
  * \see ptlrpc_nrs_pol_ops::op_res_get()
  */
 static
-struct ptlrpc_nrs_resource * nrs_resource_get(struct ptlrpc_nrs_policy *policy,
+struct ptlrpc_nrs_resource *nrs_resource_get(struct ptlrpc_nrs_policy *policy,
 					      struct ptlrpc_nrs_request *nrq,
 					      bool moving_req)
 {
@@ -530,7 +530,7 @@ static void nrs_resource_put_safe(struct ptlrpc_nrs_resource **resp)
  * \retval the NRS request to be handled
  */
 static inline
-struct ptlrpc_nrs_request * nrs_request_get(struct ptlrpc_nrs_policy *policy,
+struct ptlrpc_nrs_request *nrs_request_get(struct ptlrpc_nrs_policy *policy,
 					    bool peek, bool force)
 {
 	struct ptlrpc_nrs_request *nrq;
@@ -635,8 +635,10 @@ static int nrs_policy_ctl(struct ptlrpc_nrs *nrs, char *name,
 	spin_lock(&nrs->nrs_lock);
 
 	policy = nrs_policy_find_locked(nrs, name);
-	if (policy == NULL)
-		GOTO(out, rc = -ENOENT);
+	if (policy == NULL) {
+		rc = -ENOENT;
+		goto out;
+	}
 
 	switch (opc) {
 		/**
@@ -956,19 +958,21 @@ static int nrs_svcpt_setup_locked(struct ptlrpc_service_part *svcpt)
 	nrs = nrs_svcpt2nrs(svcpt, false);
 	rc = nrs_svcpt_setup_locked0(nrs, svcpt);
 	if (rc < 0)
-		GOTO(out, rc);
+		goto out;
 
 	/**
 	 * Optionally allocate a high-priority NRS head.
 	 */
 	if (svcpt->scp_service->srv_ops.so_hpreq_handler == NULL)
-		GOTO(out, rc);
+		goto out;
 
 	OBD_CPT_ALLOC_PTR(svcpt->scp_nrs_hp,
 			  svcpt->scp_service->srv_cptable,
 			  svcpt->scp_cpt);
-	if (svcpt->scp_nrs_hp == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (svcpt->scp_nrs_hp == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	nrs = nrs_svcpt2nrs(svcpt, true);
 	rc = nrs_svcpt_setup_locked0(nrs, svcpt);
@@ -1154,12 +1158,15 @@ int ptlrpc_nrs_policy_register(struct ptlrpc_nrs_pol_conf *conf)
 		CERROR("NRS: failing to register policy %s which has already "
 		       "been registered with NRS core!\n",
 		       conf->nc_name);
-		GOTO(fail, rc = -EEXIST);
+		rc = -EEXIST;
+		goto fail;
 	}
 
 	OBD_ALLOC_PTR(desc);
-	if (desc == NULL)
-		GOTO(fail, rc = -ENOMEM);
+	if (desc == NULL) {
+		rc = -ENOMEM;
+		goto fail;
+	}
 
 	strncpy(desc->pd_name, conf->nc_name, NRS_POL_NAME_MAX);
 	desc->pd_ops		 = conf->nc_ops;
@@ -1214,7 +1221,7 @@ again:
 				LASSERT(rc2 == 0);
 				mutex_unlock(&ptlrpc_all_services_mutex);
 				OBD_FREE_PTR(desc);
-				GOTO(fail, rc);
+				goto fail;
 			}
 
 			if (!hp && nrs_svc_has_hp(svc)) {
@@ -1237,7 +1244,7 @@ again:
 				LASSERT(rc2 == 0);
 				mutex_unlock(&ptlrpc_all_services_mutex);
 				OBD_FREE_PTR(desc);
-				GOTO(fail, rc);
+				goto fail;
 			}
 		}
 	}
@@ -1288,7 +1295,8 @@ int ptlrpc_nrs_policy_unregister(struct ptlrpc_nrs_pol_conf *conf)
 		CERROR("Failing to unregister NRS policy %s which has "
 		       "not been registered with NRS core!\n",
 		       conf->nc_name);
-		GOTO(not_exist, rc = -ENOENT);
+		rc = -ENOENT;
+		goto not_exist;
 	}
 
 	mutex_lock(&ptlrpc_all_services_mutex);
@@ -1299,7 +1307,7 @@ int ptlrpc_nrs_policy_unregister(struct ptlrpc_nrs_pol_conf *conf)
 			CERROR("Please first stop policy %s on all service "
 			       "partitions and then retry to unregister the "
 			       "policy.\n", conf->nc_name);
-		GOTO(fail, rc);
+		goto fail;
 	}
 
 	CDEBUG(D_INFO, "Unregistering policy %s from NRS core.\n",
@@ -1347,7 +1355,7 @@ int ptlrpc_service_nrs_setup(struct ptlrpc_service *svc)
 	ptlrpc_service_for_each_part(svcpt, i, svc) {
 		rc = nrs_svcpt_setup_locked(svcpt);
 		if (rc != 0)
-			GOTO(failed, rc);
+			goto failed;
 	}
 
 	/**
@@ -1361,7 +1369,7 @@ int ptlrpc_service_nrs_setup(struct ptlrpc_service *svc)
 		if (desc->pd_ops->op_lprocfs_init != NULL) {
 			rc = desc->pd_ops->op_lprocfs_init(svc);
 			if (rc != 0)
-				GOTO(failed, rc);
+				goto failed;
 		}
 	}
 
@@ -1679,7 +1687,7 @@ int ptlrpc_nrs_policy_control(const struct ptlrpc_service *svc,
 					    opc, arg);
 			if (rc != 0 || (queue == PTLRPC_NRS_QUEUE_REG &&
 					single))
-				GOTO(out, rc);
+				goto out;
 		}
 
 		if ((queue & PTLRPC_NRS_QUEUE_HP) != 0) {
@@ -1694,7 +1702,7 @@ int ptlrpc_nrs_policy_control(const struct ptlrpc_service *svc,
 			rc = nrs_policy_ctl(nrs_svcpt2nrs(svcpt, true), name,
 					    opc, arg);
 			if (rc != 0 || single)
-				GOTO(out, rc);
+				goto out;
 		}
 	}
 out:
@@ -1721,7 +1729,7 @@ int ptlrpc_nrs_init(void)
 
 	rc = ptlrpc_nrs_policy_register(&nrs_conf_fifo);
 	if (rc != 0)
-		GOTO(fail, rc);
+		goto fail;
 
 
 	return rc;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs_crr.c b/drivers/staging/lustre/lustre/ptlrpc/nrs_crr.c
deleted file mode 100644
index ddfb5102d822..000000000000
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs_crr.c
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * GPL HEADER START
- *
- * DO NOT ALTER OR REMOVE COPYRIGHT NOTICES OR THIS FILE HEADER.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 only,
- * as published by the Free Software Foundation.
-
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License version 2 for more details.  A copy is
- * included in the COPYING file that accompanied this code.
-
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * GPL HEADER END
- */
-/*
- * Copyright (c) 2011 Intel Corporation
- *
- * Copyright 2012 Xyratex Technology Limited
- */
-/*
- * lustre/ptlrpc/nrs_crr.c
- *
- * Network Request Scheduler (NRS) CRR-N policy
- *
- * Request ordering in a batched Round-Robin manner over client NIDs
- *
- * Author: Liang Zhen <liang@whamcloud.com>
- * Author: Nikitas Angelinas <nikitas_angelinas@xyratex.com>
- */
-/**
- * \addtogoup nrs
- * @{
- */
diff --git a/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c b/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c
index 28363307ee35..eb40c01db612 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/nrs_fifo.c
@@ -157,7 +157,7 @@ static int nrs_fifo_res_get(struct ptlrpc_nrs_policy *policy,
  * \see nrs_request_get()
  */
 static
-struct ptlrpc_nrs_request * nrs_fifo_req_get(struct ptlrpc_nrs_policy *policy,
+struct ptlrpc_nrs_request *nrs_fifo_req_get(struct ptlrpc_nrs_policy *policy,
 					     bool peek, bool force)
 {
 	struct nrs_fifo_head	  *head = policy->pol_private;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
index ac562932ccdf..50556db15f79 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pack_generic.c
@@ -1600,8 +1600,8 @@ EXPORT_SYMBOL(ptlrpc_req_set_repsize);
  */
 int do_set_info_async(struct obd_import *imp,
 		      int opcode, int version,
-		      obd_count keylen, void *key,
-		      obd_count vallen, void *val,
+		      u32 keylen, void *key,
+		      u32 vallen, void *val,
 		      struct ptlrpc_request_set *set)
 {
 	struct ptlrpc_request *req;
@@ -1797,7 +1797,7 @@ void lustre_swab_ost_body(struct ost_body *b)
 }
 EXPORT_SYMBOL(lustre_swab_ost_body);
 
-void lustre_swab_ost_last_id(obd_id *id)
+void lustre_swab_ost_last_id(u64 *id)
 {
 	__swab64s(id);
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/pinger.c b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
index 5e4e49fab63c..20341b27a06a 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/pinger.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/pinger.c
@@ -281,7 +281,8 @@ static int ptlrpc_pinger_main(void *arg)
 		   ping will get sent 2 timeouts from now!  Beware. */
 		CDEBUG(D_INFO, "next wakeup in "CFS_DURATION_T" ("
 		       CFS_TIME_T")\n", time_to_next_wake,
-		       cfs_time_add(this_ping,cfs_time_seconds(PING_INTERVAL)));
+		       cfs_time_add(this_ping,
+				    cfs_time_seconds(PING_INTERVAL)));
 		if (time_to_next_wake > 0) {
 			lwi = LWI_TIMEOUT(max_t(long, time_to_next_wake,
 						cfs_time_seconds(1)),
@@ -434,7 +435,7 @@ struct timeout_item* ptlrpc_new_timeout(int time, enum timeout_event event,
 
 	OBD_ALLOC_PTR(ti);
 	if (!ti)
-		return(NULL);
+		return NULL;
 
 	INIT_LIST_HEAD(&ti->ti_obd_list);
 	INIT_LIST_HEAD(&ti->ti_chain);
@@ -489,7 +490,7 @@ int ptlrpc_add_timeout_client(int time, enum timeout_event event,
 	ti = ptlrpc_pinger_register_timeout(time, event, cb, data);
 	if (!ti) {
 		mutex_unlock(&pinger_mutex);
-		return (-EINVAL);
+		return -EINVAL;
 	}
 	list_add(obd_list, &ti->ti_obd_list);
 	mutex_unlock(&pinger_mutex);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
index f7be007c88cb..7fe2e584ffd7 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_internal.h
@@ -57,7 +57,7 @@ struct ptlrpc_bulk_desc *ptlrpc_new_bulk(unsigned npages, unsigned max_brw,
 					 unsigned type, unsigned portal);
 int ptlrpc_request_cache_init(void);
 void ptlrpc_request_cache_fini(void);
-struct ptlrpc_request *ptlrpc_request_cache_alloc(int flags);
+struct ptlrpc_request *ptlrpc_request_cache_alloc(gfp_t flags);
 void ptlrpc_request_cache_free(struct ptlrpc_request *req);
 void ptlrpc_init_xid(void);
 
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
index 6d92a56da620..5268887ca6b3 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpc_module.c
@@ -75,45 +75,45 @@ __init int ptlrpc_init(void)
 	cleanup_phase = 1;
 	rc = ptlrpc_request_cache_init();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 
 	cleanup_phase = 2;
 	rc = ptlrpc_init_portals();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 
 	cleanup_phase = 3;
 
 	rc = ptlrpc_connection_init();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 
 	cleanup_phase = 4;
 	ptlrpc_put_connection_superhack = ptlrpc_connection_put;
 
 	rc = ptlrpc_start_pinger();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 
 	cleanup_phase = 5;
 	rc = ldlm_init();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 
 	cleanup_phase = 6;
 	rc = sptlrpc_init();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 
 	cleanup_phase = 7;
 	rc = ptlrpc_nrs_init();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 
 	cleanup_phase = 8;
 	rc = tgt_mod_init();
 	if (rc)
-		GOTO(cleanup, rc);
+		goto cleanup;
 	return 0;
 
 cleanup:
diff --git a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
index 9c60e2af43bf..357ea9f8bd57 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/ptlrpcd.c
@@ -615,8 +615,10 @@ int ptlrpcd_start(int index, int max, const char *name, struct ptlrpcd_ctl *pc)
 	spin_lock_init(&pc->pc_lock);
 	strlcpy(pc->pc_name, name, sizeof(pc->pc_name));
 	pc->pc_set = ptlrpc_prep_set();
-	if (pc->pc_set == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (pc->pc_set == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	/*
 	 * So far only "client" ptlrpcd uses an environment. In the future,
@@ -625,19 +627,21 @@ int ptlrpcd_start(int index, int max, const char *name, struct ptlrpcd_ctl *pc)
 	 */
 	rc = lu_context_init(&pc->pc_env.le_ctx, LCT_CL_THREAD|LCT_REMEMBER);
 	if (rc != 0)
-		GOTO(out_set, rc);
+		goto out_set;
 
 	{
 		struct task_struct *task;
 		if (index >= 0) {
 			rc = ptlrpcd_bind(index, max);
 			if (rc < 0)
-				GOTO(out_env, rc);
+				goto out_env;
 		}
 
 		task = kthread_run(ptlrpcd, pc, "%s", pc->pc_name);
-		if (IS_ERR(task))
-			GOTO(out_env, rc = PTR_ERR(task));
+		if (IS_ERR(task)) {
+			rc = PTR_ERR(task);
+			goto out_env;
+		}
 
 		wait_for_completion(&pc->pc_starting);
 	}
@@ -741,14 +745,16 @@ static int ptlrpcd_init(void)
 
 	size = offsetof(struct ptlrpcd, pd_threads[nthreads]);
 	OBD_ALLOC(ptlrpcds, size);
-	if (ptlrpcds == NULL)
-		GOTO(out, rc = -ENOMEM);
+	if (ptlrpcds == NULL) {
+		rc = -ENOMEM;
+		goto out;
+	}
 
 	snprintf(name, sizeof(name), "ptlrpcd_rcv");
 	set_bit(LIOD_RECOVERY, &ptlrpcds->pd_thread_rcv.pc_flags);
 	rc = ptlrpcd_start(-1, nthreads, name, &ptlrpcds->pd_thread_rcv);
 	if (rc < 0)
-		GOTO(out, rc);
+		goto out;
 
 	/* XXX: We start nthreads ptlrpc daemons. Each of them can process any
 	 *      non-recovery async RPC to improve overall async RPC efficiency.
@@ -766,7 +772,7 @@ static int ptlrpcd_init(void)
 		snprintf(name, sizeof(name), "ptlrpcd_%d", i);
 		rc = ptlrpcd_start(i, nthreads, name, &ptlrpcds->pd_threads[i]);
 		if (rc < 0)
-			GOTO(out, rc);
+			goto out;
 	}
 
 	ptlrpcds->pd_size = size;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/recover.c b/drivers/staging/lustre/lustre/ptlrpc/recover.c
index 5e4a1a52e0da..e1bc77b83ffb 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/recover.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/recover.c
@@ -47,7 +47,6 @@
 #include "../include/lustre_import.h"
 #include "../include/lustre_export.h"
 #include "../include/obd.h"
-#include "../include/obd_ost.h"
 #include "../include/obd_class.h"
 #include <linux/list.h>
 
@@ -317,7 +316,7 @@ int ptlrpc_recover_import(struct obd_import *imp, char *new_uuid, int async)
 		rc = -EINVAL;
 	spin_unlock(&imp->imp_lock);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	/* force import to be disconnected. */
 	ptlrpc_set_import_discon(imp, 0);
@@ -329,7 +328,7 @@ int ptlrpc_recover_import(struct obd_import *imp, char *new_uuid, int async)
 		obd_str2uuid(&uuid, new_uuid);
 		rc = import_set_conn_priority(imp, &uuid);
 		if (rc)
-			GOTO(out, rc);
+			goto out;
 	}
 
 	/* Check if reconnect is already in progress */
@@ -340,11 +339,11 @@ int ptlrpc_recover_import(struct obd_import *imp, char *new_uuid, int async)
 	}
 	spin_unlock(&imp->imp_lock);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	rc = ptlrpc_connect_import(imp);
 	if (rc)
-		GOTO(out, rc);
+		goto out;
 
 	if (!async) {
 		struct l_wait_info lwi;
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec.c b/drivers/staging/lustre/lustre/ptlrpc/sec.c
index 5cff7ee6ee7b..4ce76852e6ee 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec.c
@@ -110,7 +110,7 @@ int sptlrpc_unregister_policy(struct ptlrpc_sec_policy *policy)
 EXPORT_SYMBOL(sptlrpc_unregister_policy);
 
 static
-struct ptlrpc_sec_policy * sptlrpc_wireflavor2policy(__u32 flavor)
+struct ptlrpc_sec_policy *sptlrpc_wireflavor2policy(__u32 flavor)
 {
 	static DEFINE_MUTEX(load_mutex);
 	static atomic_t       loaded = ATOMIC_INIT(0);
@@ -1095,15 +1095,18 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 	early_size = req->rq_nob_received;
 	early_bufsz = size_roundup_power2(early_size);
 	OBD_ALLOC_LARGE(early_buf, early_bufsz);
-	if (early_buf == NULL)
-		GOTO(err_req, rc = -ENOMEM);
+	if (early_buf == NULL) {
+		rc = -ENOMEM;
+		goto err_req;
+	}
 
 	/* sanity checkings and copy data out, do it inside spinlock */
 	spin_lock(&req->rq_lock);
 
 	if (req->rq_replied) {
 		spin_unlock(&req->rq_lock);
-		GOTO(err_buf, rc = -EALREADY);
+		rc = -EALREADY;
+		goto err_buf;
 	}
 
 	LASSERT(req->rq_repbuf);
@@ -1113,7 +1116,8 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 	if (req->rq_reply_off != 0) {
 		CERROR("early reply with offset %u\n", req->rq_reply_off);
 		spin_unlock(&req->rq_lock);
-		GOTO(err_buf, rc = -EPROTO);
+		rc = -EPROTO;
+		goto err_buf;
 	}
 
 	if (req->rq_nob_received != early_size) {
@@ -1121,14 +1125,16 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 		CERROR("data size has changed from %u to %u\n",
 		       early_size, req->rq_nob_received);
 		spin_unlock(&req->rq_lock);
-		GOTO(err_buf, rc = -EINVAL);
+		rc = -EINVAL;
+		goto err_buf;
 	}
 
 	if (req->rq_nob_received < sizeof(struct lustre_msg)) {
 		CERROR("early reply length %d too small\n",
 		       req->rq_nob_received);
 		spin_unlock(&req->rq_lock);
-		GOTO(err_buf, rc = -EALREADY);
+		rc = -EALREADY;
+		goto err_buf;
 	}
 
 	memcpy(early_buf, req->rq_repbuf, early_size);
@@ -1148,7 +1154,7 @@ int sptlrpc_cli_unwrap_early_reply(struct ptlrpc_request *req,
 	if (rc) {
 		DEBUG_REQ(D_ADAPTTO, early_req,
 			  "error %d unwrap early reply", rc);
-		GOTO(err_ctx, rc);
+		goto err_ctx;
 	}
 
 	LASSERT(early_req->rq_repmsg);
@@ -1267,7 +1273,7 @@ EXPORT_SYMBOL(sptlrpc_sec_put);
  * policy module is responsible for taking reference of import
  */
 static
-struct ptlrpc_sec * sptlrpc_sec_create(struct obd_import *imp,
+struct ptlrpc_sec *sptlrpc_sec_create(struct obd_import *imp,
 				       struct ptlrpc_svc_ctx *svc_ctx,
 				       struct sptlrpc_flavor *sf,
 				       enum lustre_sec_part sp)
@@ -1434,7 +1440,7 @@ int sptlrpc_import_sec_adapt(struct obd_import *imp,
 		char    str2[24];
 
 		if (flavor_equal(&sf, &sec->ps_flvr))
-			GOTO(out, rc);
+			goto out;
 
 		CDEBUG(D_SEC, "import %s->%s: changing flavor %s -> %s\n",
 		       imp->imp_obd->obd_name,
@@ -1447,7 +1453,7 @@ int sptlrpc_import_sec_adapt(struct obd_import *imp,
 		    SPTLRPC_FLVR_MECH(sf.sf_rpc) ==
 		    SPTLRPC_FLVR_MECH(sec->ps_flvr.sf_rpc)) {
 			sptlrpc_import_sec_adapt_inplace(imp, sec, &sf);
-			GOTO(out, rc);
+			goto out;
 		}
 	} else if (SPTLRPC_FLVR_BASE(sf.sf_rpc) !=
 		   SPTLRPC_FLVR_BASE(SPTLRPC_FLVR_NULL)) {
@@ -2366,7 +2372,7 @@ EXPORT_SYMBOL(sptlrpc_unpack_user_desc);
  * misc helpers			 *
  ****************************************/
 
-const char * sec2target_str(struct ptlrpc_sec *sec)
+const char *sec2target_str(struct ptlrpc_sec *sec)
 {
 	if (!sec || !sec->ps_import || !sec->ps_import->imp_obd)
 		return "*";
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
index 874789b200a3..cc68a1cf24e3 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_bulk.c
@@ -365,7 +365,7 @@ static void enc_pools_insert(struct page ***pools, int npools, int npages)
 	 */
 	cur_npools = (page_pools.epp_total_pages + PAGES_PER_POOL - 1) /
 		     PAGES_PER_POOL;
-	end_npools = (page_pools.epp_total_pages + npages + PAGES_PER_POOL -1) /
+	end_npools = (page_pools.epp_total_pages + npages + PAGES_PER_POOL - 1) /
 		     PAGES_PER_POOL;
 	LASSERT(end_npools <= page_pools.epp_max_pools);
 
@@ -794,7 +794,7 @@ static int cfs_hash_alg_id[] = {
 	[BULK_HASH_ALG_SHA384]	= CFS_HASH_ALG_SHA384,
 	[BULK_HASH_ALG_SHA512]	= CFS_HASH_ALG_SHA512,
 };
-const char * sptlrpc_get_hash_name(__u8 hash_alg)
+const char *sptlrpc_get_hash_name(__u8 hash_alg)
 {
 	return cfs_crypto_hash_name(cfs_hash_alg_id[hash_alg]);
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
index 01f8b0d6660e..56ba9e4e5297 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_config.c
@@ -41,13 +41,8 @@
 #include <linux/key.h>
 
 #include "../include/obd.h"
-#include "../include/obd_class.h"
 #include "../include/obd_support.h"
-#include "../include/lustre_net.h"
 #include "../include/lustre_import.h"
-#include "../include/lustre_log.h"
-#include "../include/lustre_disk.h"
-#include "../include/lustre_dlm.h"
 #include "../include/lustre_param.h"
 #include "../include/lustre_sec.h"
 
@@ -450,46 +445,6 @@ void sptlrpc_rule_set_dump(struct sptlrpc_rule_set *rset)
 }
 EXPORT_SYMBOL(sptlrpc_rule_set_dump);
 
-static int sptlrpc_rule_set_extract(struct sptlrpc_rule_set *gen,
-				    struct sptlrpc_rule_set *tgt,
-				    enum lustre_sec_part from,
-				    enum lustre_sec_part to,
-				    struct sptlrpc_rule_set *rset)
-{
-	struct sptlrpc_rule_set *src[2] = { gen, tgt };
-	struct sptlrpc_rule     *rule;
-	int		      i, n, rc;
-
-	might_sleep();
-
-	/* merge general rules firstly, then target-specific rules */
-	for (i = 0; i < 2; i++) {
-		if (src[i] == NULL)
-			continue;
-
-		for (n = 0; n < src[i]->srs_nrule; n++) {
-			rule = &src[i]->srs_rules[n];
-
-			if (from != LUSTRE_SP_ANY &&
-			    rule->sr_from != LUSTRE_SP_ANY &&
-			    rule->sr_from != from)
-				continue;
-			if (to != LUSTRE_SP_ANY &&
-			    rule->sr_to != LUSTRE_SP_ANY &&
-			    rule->sr_to != to)
-				continue;
-
-			rc = sptlrpc_rule_set_merge(rset, rule);
-			if (rc) {
-				CERROR("can't merge: %d\n", rc);
-				return rc;
-			}
-		}
-	}
-
-	return 0;
-}
-
 /**********************************
  * sptlrpc configuration support  *
  **********************************/
@@ -746,7 +701,7 @@ void sptlrpc_conf_log_update_begin(const char *logname)
 
 	conf = sptlrpc_conf_get(fsname, 0);
 	if (conf) {
-		if(conf->sc_local) {
+		if (conf->sc_local) {
 			LASSERT(conf->sc_updated == 0);
 			sptlrpc_conf_free_rsets(conf);
 		}
@@ -815,7 +770,7 @@ void sptlrpc_conf_log_stop(const char *logname)
 }
 EXPORT_SYMBOL(sptlrpc_conf_log_stop);
 
-static void inline flavor_set_flags(struct sptlrpc_flavor *sf,
+static inline void flavor_set_flags(struct sptlrpc_flavor *sf,
 				    enum lustre_sec_part from,
 				    enum lustre_sec_part to,
 				    unsigned int fl_udesc)
@@ -908,7 +863,7 @@ void sptlrpc_conf_client_adapt(struct obd_device *obd)
 	struct obd_import  *imp;
 
 	LASSERT(strcmp(obd->obd_type->typ_name, LUSTRE_MDC_NAME) == 0 ||
-		strcmp(obd->obd_type->typ_name, LUSTRE_OSC_NAME) ==0);
+		strcmp(obd->obd_type->typ_name, LUSTRE_OSC_NAME) == 0);
 	CDEBUG(D_SEC, "obd %s\n", obd->u.cli.cl_target_uuid.uuid);
 
 	/* serialize with connect/disconnect import */
@@ -927,288 +882,6 @@ void sptlrpc_conf_client_adapt(struct obd_device *obd)
 }
 EXPORT_SYMBOL(sptlrpc_conf_client_adapt);
 
-
-static void rule2string(struct sptlrpc_rule *r, char *buf, int buflen)
-{
-	char    dirbuf[8];
-	char   *net;
-	char   *ptr = buf;
-
-	if (r->sr_netid == LNET_NIDNET(LNET_NID_ANY))
-		net = "default";
-	else
-		net = libcfs_net2str(r->sr_netid);
-
-	if (r->sr_from == LUSTRE_SP_ANY && r->sr_to == LUSTRE_SP_ANY)
-		dirbuf[0] = '\0';
-	else
-		snprintf(dirbuf, sizeof(dirbuf), ".%s2%s",
-			 sptlrpc_part2name(r->sr_from),
-			 sptlrpc_part2name(r->sr_to));
-
-	ptr += snprintf(buf, buflen, "srpc.flavor.%s%s=", net, dirbuf);
-
-	sptlrpc_flavor2name(&r->sr_flvr, ptr, buflen - (ptr - buf));
-	buf[buflen - 1] = '\0';
-}
-
-static int sptlrpc_record_rule_set(struct llog_handle *llh,
-				   char *target,
-				   struct sptlrpc_rule_set *rset)
-{
-	struct lustre_cfg_bufs  bufs;
-	struct lustre_cfg      *lcfg;
-	struct llog_rec_hdr     rec;
-	int		     buflen;
-	char		    param[48];
-	int		     i, rc;
-
-	for (i = 0; i < rset->srs_nrule; i++) {
-		rule2string(&rset->srs_rules[i], param, sizeof(param));
-
-		lustre_cfg_bufs_reset(&bufs, NULL);
-		lustre_cfg_bufs_set_string(&bufs, 1, target);
-		lustre_cfg_bufs_set_string(&bufs, 2, param);
-		lcfg = lustre_cfg_new(LCFG_SPTLRPC_CONF, &bufs);
-		LASSERT(lcfg);
-
-		buflen = lustre_cfg_len(lcfg->lcfg_bufcount,
-					lcfg->lcfg_buflens);
-		rec.lrh_len = llog_data_len(buflen);
-		rec.lrh_type = OBD_CFG_REC;
-		rc = llog_write(NULL, llh, &rec, NULL, 0, (void *)lcfg, -1);
-		if (rc)
-			CERROR("failed to write a rec: rc = %d\n", rc);
-		lustre_cfg_free(lcfg);
-	}
-	return 0;
-}
-
-static int sptlrpc_record_rules(struct llog_handle *llh,
-				struct sptlrpc_conf *conf)
-{
-	struct sptlrpc_conf_tgt *conf_tgt;
-
-	sptlrpc_record_rule_set(llh, conf->sc_fsname, &conf->sc_rset);
-
-	list_for_each_entry(conf_tgt, &conf->sc_tgts, sct_list) {
-		sptlrpc_record_rule_set(llh, conf_tgt->sct_name,
-					&conf_tgt->sct_rset);
-	}
-	return 0;
-}
-
-#define LOG_SPTLRPC_TMP "sptlrpc.tmp"
-#define LOG_SPTLRPC     "sptlrpc"
-
-static
-int sptlrpc_target_local_copy_conf(struct obd_device *obd,
-				   struct sptlrpc_conf *conf)
-{
-	struct llog_handle   *llh = NULL;
-	struct llog_ctxt     *ctxt;
-	struct lvfs_run_ctxt  saved;
-	struct dentry	*dentry;
-	int		   rc;
-
-	ctxt = llog_get_context(obd, LLOG_CONFIG_ORIG_CTXT);
-	if (ctxt == NULL)
-		return -EINVAL;
-
-	push_ctxt(&saved, &obd->obd_lvfs_ctxt, NULL);
-
-	dentry = ll_lookup_one_len(MOUNT_CONFIGS_DIR, cfs_fs_pwd(current->fs),
-				   strlen(MOUNT_CONFIGS_DIR));
-	if (IS_ERR(dentry)) {
-		rc = PTR_ERR(dentry);
-		CERROR("cannot lookup %s directory: rc = %d\n",
-		       MOUNT_CONFIGS_DIR, rc);
-		GOTO(out_ctx, rc);
-	}
-
-	/* erase the old tmp log */
-	rc = llog_erase(NULL, ctxt, NULL, LOG_SPTLRPC_TMP);
-	if (rc < 0 && rc != -ENOENT) {
-		CERROR("%s: cannot erase temporary sptlrpc log: rc = %d\n",
-		       obd->obd_name, rc);
-		GOTO(out_dput, rc);
-	}
-
-	/* write temporary log */
-	rc = llog_open_create(NULL, ctxt, &llh, NULL, LOG_SPTLRPC_TMP);
-	if (rc)
-		GOTO(out_dput, rc);
-	rc = llog_init_handle(NULL, llh, LLOG_F_IS_PLAIN, NULL);
-	if (rc)
-		GOTO(out_close, rc);
-
-	rc = sptlrpc_record_rules(llh, conf);
-
-out_close:
-	llog_close(NULL, llh);
-	if (rc == 0)
-		rc = lustre_rename(dentry, obd->obd_lvfs_ctxt.pwdmnt,
-				   LOG_SPTLRPC_TMP, LOG_SPTLRPC);
-out_dput:
-	l_dput(dentry);
-out_ctx:
-	pop_ctxt(&saved, &obd->obd_lvfs_ctxt, NULL);
-	llog_ctxt_put(ctxt);
-	CDEBUG(D_SEC, "target %s: write local sptlrpc conf: rc = %d\n",
-	       obd->obd_name, rc);
-	return rc;
-}
-
-static int local_read_handler(const struct lu_env *env,
-			      struct llog_handle *llh,
-			      struct llog_rec_hdr *rec, void *data)
-{
-	struct sptlrpc_conf  *conf = (struct sptlrpc_conf *) data;
-	struct lustre_cfg    *lcfg = (struct lustre_cfg *)(rec + 1);
-	int		   cfg_len, rc;
-
-	if (rec->lrh_type != OBD_CFG_REC) {
-		CERROR("unhandled lrh_type: %#x\n", rec->lrh_type);
-		return -EINVAL;
-	}
-
-	cfg_len = rec->lrh_len - sizeof(struct llog_rec_hdr) -
-		  sizeof(struct llog_rec_tail);
-
-	rc = lustre_cfg_sanity_check(lcfg, cfg_len);
-	if (rc) {
-		CERROR("Insane cfg\n");
-		return rc;
-	}
-
-	if (lcfg->lcfg_command != LCFG_SPTLRPC_CONF) {
-		CERROR("invalid command (%x)\n", lcfg->lcfg_command);
-		return -EINVAL;
-	}
-
-	return __sptlrpc_process_config(lcfg, conf);
-}
-
-static
-int sptlrpc_target_local_read_conf(struct obd_device *obd,
-				   struct sptlrpc_conf *conf)
-{
-	struct llog_handle    *llh = NULL;
-	struct llog_ctxt      *ctxt;
-	struct lvfs_run_ctxt   saved;
-	int		    rc;
-
-	LASSERT(conf->sc_updated == 0 && conf->sc_local == 0);
-
-	ctxt = llog_get_context(obd, LLOG_CONFIG_ORIG_CTXT);
-	if (ctxt == NULL) {
-		CERROR("missing llog context\n");
-		return -EINVAL;
-	}
-
-	push_ctxt(&saved, &obd->obd_lvfs_ctxt, NULL);
-
-	rc = llog_open(NULL, ctxt, &llh, NULL, LOG_SPTLRPC, LLOG_OPEN_EXISTS);
-	if (rc < 0) {
-		if (rc == -ENOENT)
-			rc = 0;
-		GOTO(out_pop, rc);
-	}
-
-	rc = llog_init_handle(NULL, llh, LLOG_F_IS_PLAIN, NULL);
-	if (rc)
-		GOTO(out_close, rc);
-
-	if (llog_get_size(llh) <= 1) {
-		CDEBUG(D_SEC, "no local sptlrpc copy found\n");
-		GOTO(out_close, rc = 0);
-	}
-
-	rc = llog_process(NULL, llh, local_read_handler, (void *)conf, NULL);
-
-	if (rc == 0) {
-		conf->sc_local = 1;
-	} else {
-		sptlrpc_conf_free_rsets(conf);
-	}
-
-out_close:
-	llog_close(NULL, llh);
-out_pop:
-	pop_ctxt(&saved, &obd->obd_lvfs_ctxt, NULL);
-	llog_ctxt_put(ctxt);
-	CDEBUG(D_SEC, "target %s: read local sptlrpc conf: rc = %d\n",
-	       obd->obd_name, rc);
-	return rc;
-}
-
-
-/**
- * called by target devices, extract sptlrpc rules which applies to
- * this target, to be used for future rpc flavor checking.
- */
-int sptlrpc_conf_target_get_rules(struct obd_device *obd,
-				  struct sptlrpc_rule_set *rset,
-				  int initial)
-{
-	struct sptlrpc_conf      *conf;
-	struct sptlrpc_conf_tgt  *conf_tgt;
-	enum lustre_sec_part      sp_dst;
-	char		      fsname[MTI_NAME_MAXLEN];
-	int		       rc = 0;
-
-	if (strcmp(obd->obd_type->typ_name, LUSTRE_MDT_NAME) == 0) {
-		sp_dst = LUSTRE_SP_MDT;
-	} else if (strcmp(obd->obd_type->typ_name, LUSTRE_OST_NAME) == 0) {
-		sp_dst = LUSTRE_SP_OST;
-	} else {
-		CERROR("unexpected obd type %s\n", obd->obd_type->typ_name);
-		return -EINVAL;
-	}
-	CDEBUG(D_SEC, "get rules for target %s\n", obd->obd_uuid.uuid);
-
-	target2fsname(obd->obd_uuid.uuid, fsname, sizeof(fsname));
-
-	mutex_lock(&sptlrpc_conf_lock);
-
-	conf = sptlrpc_conf_get(fsname, 0);
-	if (conf == NULL) {
-		CERROR("missing sptlrpc config log\n");
-		GOTO(out, rc);
-	}
-
-	if (conf->sc_updated  == 0) {
-		/*
-		 * always read from local copy. here another option is
-		 * if we already have a local copy (read from another
-		 * target device hosted on the same node) we simply use that.
-		 */
-		if (conf->sc_local)
-			sptlrpc_conf_free_rsets(conf);
-
-		sptlrpc_target_local_read_conf(obd, conf);
-	} else {
-		LASSERT(conf->sc_local == 0);
-
-		/* write a local copy */
-		if (initial || conf->sc_modified)
-			sptlrpc_target_local_copy_conf(obd, conf);
-		else
-			CDEBUG(D_SEC, "unchanged, skip updating local copy\n");
-	}
-
-	/* extract rule set for this target */
-	conf_tgt = sptlrpc_conf_get_tgt(conf, obd->obd_name, 0);
-
-	rc = sptlrpc_rule_set_extract(&conf->sc_rset,
-				      conf_tgt ? &conf_tgt->sct_rset: NULL,
-				      LUSTRE_SP_ANY, sp_dst, rset);
-out:
-	mutex_unlock(&sptlrpc_conf_lock);
-	return rc;
-}
-EXPORT_SYMBOL(sptlrpc_conf_target_get_rules);
-
 int  sptlrpc_conf_init(void)
 {
 	mutex_init(&sptlrpc_conf_lock);
diff --git a/drivers/staging/lustre/lustre/ptlrpc/sec_null.c b/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
index a47791411149..099cec3b669f 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/sec_null.c
@@ -471,5 +471,6 @@ void sptlrpc_null_fini(void)
 
 	rc = sptlrpc_unregister_policy(&null_policy);
 	if (rc)
-		CERROR("failed to unregister %s: %d\n", null_policy.sp_name,rc);
+		CERROR("failed to unregister %s: %d\n",
+		       null_policy.sp_name, rc);
 }
diff --git a/drivers/staging/lustre/lustre/ptlrpc/service.c b/drivers/staging/lustre/lustre/ptlrpc/service.c
index c88eae27bbf4..a8df8a792333 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/service.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/service.c
@@ -781,13 +781,15 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 			cpt = cpts != NULL ? cpts[i] : i;
 
 		OBD_CPT_ALLOC(svcpt, cptable, cpt, sizeof(*svcpt));
-		if (svcpt == NULL)
-			GOTO(failed, rc = -ENOMEM);
+		if (svcpt == NULL) {
+			rc = -ENOMEM;
+			goto failed;
+		}
 
 		service->srv_parts[i] = svcpt;
 		rc = ptlrpc_service_part_init(service, svcpt, cpt);
 		if (rc != 0)
-			GOTO(failed, rc);
+			goto failed;
 	}
 
 	ptlrpc_server_nthreads_check(service, conf);
@@ -804,7 +806,7 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 
 	rc = ptlrpc_service_nrs_setup(service);
 	if (rc != 0)
-		GOTO(failed, rc);
+		goto failed;
 
 	CDEBUG(D_NET, "%s: Started, listening on portal %d\n",
 	       service->srv_name, service->srv_req_portal);
@@ -813,7 +815,7 @@ ptlrpc_register_service(struct ptlrpc_service_conf *conf,
 	if (rc != 0) {
 		CERROR("Failed to start threads for service %s: %d\n",
 		       service->srv_name, rc);
-		GOTO(failed, rc);
+		goto failed;
 	}
 
 	return service;
@@ -1172,13 +1174,13 @@ static int ptlrpc_at_add_timed(struct ptlrpc_request *req)
 	__u32 index;
 
 	if (AT_OFF)
-		return(0);
+		return 0;
 
 	if (req->rq_no_reply)
 		return 0;
 
 	if ((lustre_msghdr_get_flags(req->rq_reqmsg) & MSGHDR_AT_SUPPORT) == 0)
-		return(-ENOSYS);
+		return -ENOSYS;
 
 	spin_lock(&svcpt->scp_at_lock);
 	LASSERT(list_empty(&req->rq_timed_list));
@@ -1308,8 +1310,10 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 	if (reqcopy == NULL)
 		return -ENOMEM;
 	OBD_ALLOC_LARGE(reqmsg, req->rq_reqlen);
-	if (!reqmsg)
-		GOTO(out_free, rc = -ENOMEM);
+	if (!reqmsg) {
+		rc = -ENOMEM;
+		goto out_free;
+	}
 
 	*reqcopy = *req;
 	reqcopy->rq_reply_state = NULL;
@@ -1327,24 +1331,29 @@ static int ptlrpc_at_send_early_reply(struct ptlrpc_request *req)
 	if (atomic_read(&req->rq_refcount) == 1) {
 		DEBUG_REQ(D_ADAPTTO, reqcopy, "Normal reply already sent out, "
 			  "abort sending early reply\n");
-		GOTO(out, rc = -EINVAL);
+		rc = -EINVAL;
+		goto out;
 	}
 
 	/* Connection ref */
 	reqcopy->rq_export = class_conn2export(
 				     lustre_msg_get_handle(reqcopy->rq_reqmsg));
-	if (reqcopy->rq_export == NULL)
-		GOTO(out, rc = -ENODEV);
+	if (reqcopy->rq_export == NULL) {
+		rc = -ENODEV;
+		goto out;
+	}
 
 	/* RPC ref */
 	class_export_rpc_inc(reqcopy->rq_export);
 	if (reqcopy->rq_export->exp_obd &&
-	    reqcopy->rq_export->exp_obd->obd_fail)
-		GOTO(out_put, rc = -ENODEV);
+	    reqcopy->rq_export->exp_obd->obd_fail) {
+		rc = -ENODEV;
+		goto out_put;
+	}
 
 	rc = lustre_pack_reply_flags(reqcopy, 1, NULL, NULL, LPRFL_EARLY_REPLY);
 	if (rc)
-		GOTO(out_put, rc);
+		goto out_put;
 
 	rc = ptlrpc_send_reply(reqcopy, PTLRPC_REPLY_EARLY);
 
@@ -1849,7 +1858,7 @@ ptlrpc_server_handle_req_in(struct ptlrpc_service_part *svcpt,
 	/* Move it over to the request processing queue */
 	rc = ptlrpc_server_request_add(svcpt, req);
 	if (rc)
-		GOTO(err_req, rc);
+		goto err_req;
 
 	wake_up(&svcpt->scp_waitq);
 	return 1;
@@ -1896,7 +1905,8 @@ ptlrpc_server_handle_request(struct ptlrpc_service_part *svcpt,
 		libcfs_debug_dumplog();
 
 	do_gettimeofday(&work_start);
-	timediff = cfs_timeval_sub(&work_start, &request->rq_arrival_time,NULL);
+	timediff = cfs_timeval_sub(&work_start, &request->rq_arrival_time,
+				   NULL);
 	if (likely(svc->srv_stats != NULL)) {
 		lprocfs_counter_add(svc->srv_stats, PTLRPC_REQWAIT_CNTR,
 				    timediff);
@@ -2262,9 +2272,7 @@ static int ptlrpc_main(void *arg)
 	struct ptlrpc_service_part	*svcpt = thread->t_svcpt;
 	struct ptlrpc_service		*svc = svcpt->scp_service;
 	struct ptlrpc_reply_state	*rs;
-#ifdef WITH_GROUP_INFO
 	struct group_info *ginfo = NULL;
-#endif
 	struct lu_env *env;
 	int counter = 0, rc = 0;
 
@@ -2280,7 +2288,6 @@ static int ptlrpc_main(void *arg)
 		      svc->srv_name, thread->t_name, svcpt->scp_cpt);
 	}
 
-#ifdef WITH_GROUP_INFO
 	ginfo = groups_alloc(0);
 	if (!ginfo) {
 		rc = -ENOMEM;
@@ -2289,7 +2296,6 @@ static int ptlrpc_main(void *arg)
 
 	set_current_groups(ginfo);
 	put_group_info(ginfo);
-#endif
 
 	if (svc->srv_ops.so_thr_init != NULL) {
 		rc = svc->srv_ops.so_thr_init(thread);
@@ -2790,8 +2796,10 @@ int ptlrpc_hr_init(void)
 		LASSERT(hrp->hrp_nthrs > 0);
 		OBD_CPT_ALLOC(hrp->hrp_thrs, ptlrpc_hr.hr_cpt_table, i,
 			      hrp->hrp_nthrs * sizeof(*hrt));
-		if (hrp->hrp_thrs == NULL)
-			GOTO(out, rc = -ENOMEM);
+		if (hrp->hrp_thrs == NULL) {
+			rc = -ENOMEM;
+			goto out;
+		}
 
 		for (j = 0; j < hrp->hrp_nthrs; j++) {
 			hrt = &hrp->hrp_thrs[j];
diff --git a/drivers/staging/lustre/lustre/ptlrpc/wiretest.c b/drivers/staging/lustre/lustre/ptlrpc/wiretest.c
index 0624420135c2..d6d92046c348 100644
--- a/drivers/staging/lustre/lustre/ptlrpc/wiretest.c
+++ b/drivers/staging/lustre/lustre/ptlrpc/wiretest.c
@@ -460,54 +460,6 @@ void lustre_assert_wire_constants(void)
 	LASSERTF(OBJ_INDEX_DELETE == 11, "found %lld\n",
 		 (long long)OBJ_INDEX_DELETE);
 
-	/* Checks for struct som_attrs */
-	LASSERTF((int)sizeof(struct som_attrs) == 40, "found %lld\n",
-		 (long long)(int)sizeof(struct som_attrs));
-	LASSERTF((int)offsetof(struct som_attrs, som_compat) == 0, "found %lld\n",
-		 (long long)(int)offsetof(struct som_attrs, som_compat));
-	LASSERTF((int)sizeof(((struct som_attrs *)0)->som_compat) == 4, "found %lld\n",
-		 (long long)(int)sizeof(((struct som_attrs *)0)->som_compat));
-	LASSERTF((int)offsetof(struct som_attrs, som_incompat) == 4, "found %lld\n",
-		 (long long)(int)offsetof(struct som_attrs, som_incompat));
-	LASSERTF((int)sizeof(((struct som_attrs *)0)->som_incompat) == 4, "found %lld\n",
-		 (long long)(int)sizeof(((struct som_attrs *)0)->som_incompat));
-	LASSERTF((int)offsetof(struct som_attrs, som_ioepoch) == 8, "found %lld\n",
-		 (long long)(int)offsetof(struct som_attrs, som_ioepoch));
-	LASSERTF((int)sizeof(((struct som_attrs *)0)->som_ioepoch) == 8, "found %lld\n",
-		 (long long)(int)sizeof(((struct som_attrs *)0)->som_ioepoch));
-	LASSERTF((int)offsetof(struct som_attrs, som_size) == 16, "found %lld\n",
-		 (long long)(int)offsetof(struct som_attrs, som_size));
-	LASSERTF((int)sizeof(((struct som_attrs *)0)->som_size) == 8, "found %lld\n",
-		 (long long)(int)sizeof(((struct som_attrs *)0)->som_size));
-	LASSERTF((int)offsetof(struct som_attrs, som_blocks) == 24, "found %lld\n",
-		 (long long)(int)offsetof(struct som_attrs, som_blocks));
-	LASSERTF((int)sizeof(((struct som_attrs *)0)->som_blocks) == 8, "found %lld\n",
-		 (long long)(int)sizeof(((struct som_attrs *)0)->som_blocks));
-	LASSERTF((int)offsetof(struct som_attrs, som_mountid) == 32, "found %lld\n",
-		 (long long)(int)offsetof(struct som_attrs, som_mountid));
-	LASSERTF((int)sizeof(((struct som_attrs *)0)->som_mountid) == 8, "found %lld\n",
-		 (long long)(int)sizeof(((struct som_attrs *)0)->som_mountid));
-
-	/* Checks for struct hsm_attrs */
-	LASSERTF((int)sizeof(struct hsm_attrs) == 24, "found %lld\n",
-		 (long long)(int)sizeof(struct hsm_attrs));
-	LASSERTF((int)offsetof(struct hsm_attrs, hsm_compat) == 0, "found %lld\n",
-		 (long long)(int)offsetof(struct hsm_attrs, hsm_compat));
-	LASSERTF((int)sizeof(((struct hsm_attrs *)0)->hsm_compat) == 4, "found %lld\n",
-		 (long long)(int)sizeof(((struct hsm_attrs *)0)->hsm_compat));
-	LASSERTF((int)offsetof(struct hsm_attrs, hsm_flags) == 4, "found %lld\n",
-		 (long long)(int)offsetof(struct hsm_attrs, hsm_flags));
-	LASSERTF((int)sizeof(((struct hsm_attrs *)0)->hsm_flags) == 4, "found %lld\n",
-		 (long long)(int)sizeof(((struct hsm_attrs *)0)->hsm_flags));
-	LASSERTF((int)offsetof(struct hsm_attrs, hsm_arch_id) == 8, "found %lld\n",
-		 (long long)(int)offsetof(struct hsm_attrs, hsm_arch_id));
-	LASSERTF((int)sizeof(((struct hsm_attrs *)0)->hsm_arch_id) == 8, "found %lld\n",
-		 (long long)(int)sizeof(((struct hsm_attrs *)0)->hsm_arch_id));
-	LASSERTF((int)offsetof(struct hsm_attrs, hsm_arch_ver) == 16, "found %lld\n",
-		 (long long)(int)offsetof(struct hsm_attrs, hsm_arch_ver));
-	LASSERTF((int)sizeof(((struct hsm_attrs *)0)->hsm_arch_ver) == 8, "found %lld\n",
-		 (long long)(int)sizeof(((struct hsm_attrs *)0)->hsm_arch_ver));
-
 	/* Checks for struct ost_id */
 	LASSERTF((int)sizeof(struct ost_id) == 16, "found %lld\n",
 		 (long long)(int)sizeof(struct ost_id));
diff --git a/drivers/staging/media/Kconfig b/drivers/staging/media/Kconfig
index 3323eb5e77b0..655cf5037b0b 100644
--- a/drivers/staging/media/Kconfig
+++ b/drivers/staging/media/Kconfig
@@ -19,8 +19,6 @@ menuconfig STAGING_MEDIA
 if STAGING_MEDIA
 
 # Please keep them in alphabetic order
-source "drivers/staging/media/as102/Kconfig"
-
 source "drivers/staging/media/bcm2048/Kconfig"
 
 source "drivers/staging/media/cxd2099/Kconfig"
diff --git a/drivers/staging/media/Makefile b/drivers/staging/media/Makefile
index 7db83f373f63..6dbe578178cd 100644
--- a/drivers/staging/media/Makefile
+++ b/drivers/staging/media/Makefile
@@ -1,4 +1,3 @@
-obj-$(CONFIG_DVB_AS102)		+= as102/
 obj-$(CONFIG_I2C_BCM2048)	+= bcm2048/
 obj-$(CONFIG_DVB_CXD2099)	+= cxd2099/
 obj-$(CONFIG_LIRC_STAGING)	+= lirc/
diff --git a/drivers/staging/media/as102/as102_fe.c b/drivers/staging/media/as102/as102_fe.c
deleted file mode 100644
index b686b7617cdc..000000000000
--- a/drivers/staging/media/as102/as102_fe.c
+++ /dev/null
@@ -1,571 +0,0 @@
-/*
- * Abilis Systems Single DVB-T Receiver
- * Copyright (C) 2008 Pierrick Hascoet <pierrick.hascoet@abilis.com>
- * Copyright (C) 2010 Devin Heitmueller <dheitmueller@kernellabs.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- */
-#include "as102_drv.h"
-#include "as10x_types.h"
-#include "as10x_cmd.h"
-
-static void as10x_fe_copy_tps_parameters(struct dtv_frontend_properties *dst,
-					 struct as10x_tps *src);
-
-static void as102_fe_copy_tune_parameters(struct as10x_tune_args *dst,
-					  struct dtv_frontend_properties *src);
-
-static int as102_fe_set_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
-	int ret = 0;
-	struct as102_dev_t *dev;
-	struct as10x_tune_args tune_args = { 0 };
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -ENODEV;
-
-	if (mutex_lock_interruptible(&dev->bus_adap.lock))
-		return -EBUSY;
-
-	as102_fe_copy_tune_parameters(&tune_args, p);
-
-	/* send abilis command: SET_TUNE */
-	ret =  as10x_cmd_set_tune(&dev->bus_adap, &tune_args);
-	if (ret != 0)
-		dprintk(debug, "as10x_cmd_set_tune failed. (err = %d)\n", ret);
-
-	mutex_unlock(&dev->bus_adap.lock);
-
-	return (ret < 0) ? -EINVAL : 0;
-}
-
-static int as102_fe_get_frontend(struct dvb_frontend *fe)
-{
-	struct dtv_frontend_properties *p = &fe->dtv_property_cache;
-	int ret = 0;
-	struct as102_dev_t *dev;
-	struct as10x_tps tps = { 0 };
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -EINVAL;
-
-	if (mutex_lock_interruptible(&dev->bus_adap.lock))
-		return -EBUSY;
-
-	/* send abilis command: GET_TPS */
-	ret = as10x_cmd_get_tps(&dev->bus_adap, &tps);
-
-	if (ret == 0)
-		as10x_fe_copy_tps_parameters(p, &tps);
-
-	mutex_unlock(&dev->bus_adap.lock);
-
-	return (ret < 0) ? -EINVAL : 0;
-}
-
-static int as102_fe_get_tune_settings(struct dvb_frontend *fe,
-			struct dvb_frontend_tune_settings *settings) {
-
-#if 0
-	dprintk(debug, "step_size    = %d\n", settings->step_size);
-	dprintk(debug, "max_drift    = %d\n", settings->max_drift);
-	dprintk(debug, "min_delay_ms = %d -> %d\n", settings->min_delay_ms,
-		1000);
-#endif
-
-	settings->min_delay_ms = 1000;
-
-	return 0;
-}
-
-
-static int as102_fe_read_status(struct dvb_frontend *fe, fe_status_t *status)
-{
-	int ret = 0;
-	struct as102_dev_t *dev;
-	struct as10x_tune_status tstate = { 0 };
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -ENODEV;
-
-	if (mutex_lock_interruptible(&dev->bus_adap.lock))
-		return -EBUSY;
-
-	/* send abilis command: GET_TUNE_STATUS */
-	ret = as10x_cmd_get_tune_status(&dev->bus_adap, &tstate);
-	if (ret < 0) {
-		dprintk(debug, "as10x_cmd_get_tune_status failed (err = %d)\n",
-			ret);
-		goto out;
-	}
-
-	dev->signal_strength  = tstate.signal_strength;
-	dev->ber  = tstate.BER;
-
-	switch (tstate.tune_state) {
-	case TUNE_STATUS_SIGNAL_DVB_OK:
-		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER;
-		break;
-	case TUNE_STATUS_STREAM_DETECTED:
-		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER | FE_HAS_SYNC;
-		break;
-	case TUNE_STATUS_STREAM_TUNED:
-		*status = FE_HAS_SIGNAL | FE_HAS_CARRIER | FE_HAS_SYNC |
-			FE_HAS_LOCK;
-		break;
-	default:
-		*status = TUNE_STATUS_NOT_TUNED;
-	}
-
-	dprintk(debug, "tuner status: 0x%02x, strength %d, per: %d, ber: %d\n",
-			tstate.tune_state, tstate.signal_strength,
-			tstate.PER, tstate.BER);
-
-	if (*status & FE_HAS_LOCK) {
-		if (as10x_cmd_get_demod_stats(&dev->bus_adap,
-			(struct as10x_demod_stats *) &dev->demod_stats) < 0) {
-			memset(&dev->demod_stats, 0, sizeof(dev->demod_stats));
-			dprintk(debug,
-				"as10x_cmd_get_demod_stats failed (probably not tuned)\n");
-		} else {
-			dprintk(debug,
-				"demod status: fc: 0x%08x, bad fc: 0x%08x, "
-				"bytes corrected: 0x%08x , MER: 0x%04x\n",
-				dev->demod_stats.frame_count,
-				dev->demod_stats.bad_frame_count,
-				dev->demod_stats.bytes_fixed_by_rs,
-				dev->demod_stats.mer);
-		}
-	} else {
-		memset(&dev->demod_stats, 0, sizeof(dev->demod_stats));
-	}
-
-out:
-	mutex_unlock(&dev->bus_adap.lock);
-	return ret;
-}
-
-/*
- * Note:
- * - in AS102 SNR=MER
- *   - the SNR will be returned in linear terms, i.e. not in dB
- *   - the accuracy equals ±2dB for a SNR range from 4dB to 30dB
- *   - the accuracy is >2dB for SNR values outside this range
- */
-static int as102_fe_read_snr(struct dvb_frontend *fe, u16 *snr)
-{
-	struct as102_dev_t *dev;
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -ENODEV;
-
-	*snr = dev->demod_stats.mer;
-
-	return 0;
-}
-
-static int as102_fe_read_ber(struct dvb_frontend *fe, u32 *ber)
-{
-	struct as102_dev_t *dev;
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -ENODEV;
-
-	*ber = dev->ber;
-
-	return 0;
-}
-
-static int as102_fe_read_signal_strength(struct dvb_frontend *fe,
-					 u16 *strength)
-{
-	struct as102_dev_t *dev;
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -ENODEV;
-
-	*strength = (((0xffff * 400) * dev->signal_strength + 41000) * 2);
-
-	return 0;
-}
-
-static int as102_fe_read_ucblocks(struct dvb_frontend *fe, u32 *ucblocks)
-{
-	struct as102_dev_t *dev;
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -ENODEV;
-
-	if (dev->demod_stats.has_started)
-		*ucblocks = dev->demod_stats.bad_frame_count;
-	else
-		*ucblocks = 0;
-
-	return 0;
-}
-
-static int as102_fe_ts_bus_ctrl(struct dvb_frontend *fe, int acquire)
-{
-	struct as102_dev_t *dev;
-	int ret;
-
-	dev = (struct as102_dev_t *) fe->tuner_priv;
-	if (dev == NULL)
-		return -ENODEV;
-
-	if (mutex_lock_interruptible(&dev->bus_adap.lock))
-		return -EBUSY;
-
-	if (acquire) {
-		if (elna_enable)
-			as10x_cmd_set_context(&dev->bus_adap,
-					      CONTEXT_LNA, dev->elna_cfg);
-
-		ret = as10x_cmd_turn_on(&dev->bus_adap);
-	} else {
-		ret = as10x_cmd_turn_off(&dev->bus_adap);
-	}
-
-	mutex_unlock(&dev->bus_adap.lock);
-
-	return ret;
-}
-
-static struct dvb_frontend_ops as102_fe_ops = {
-	.delsys = { SYS_DVBT },
-	.info = {
-		.name			= "Unknown AS102 device",
-		.frequency_min		= 174000000,
-		.frequency_max		= 862000000,
-		.frequency_stepsize	= 166667,
-		.caps = FE_CAN_INVERSION_AUTO
-			| FE_CAN_FEC_1_2 | FE_CAN_FEC_2_3 | FE_CAN_FEC_3_4
-			| FE_CAN_FEC_5_6 | FE_CAN_FEC_7_8 | FE_CAN_FEC_AUTO
-			| FE_CAN_QAM_16 | FE_CAN_QAM_64 | FE_CAN_QPSK
-			| FE_CAN_QAM_AUTO
-			| FE_CAN_TRANSMISSION_MODE_AUTO
-			| FE_CAN_GUARD_INTERVAL_AUTO
-			| FE_CAN_HIERARCHY_AUTO
-			| FE_CAN_RECOVER
-			| FE_CAN_MUTE_TS
-	},
-
-	.set_frontend		= as102_fe_set_frontend,
-	.get_frontend		= as102_fe_get_frontend,
-	.get_tune_settings	= as102_fe_get_tune_settings,
-
-	.read_status		= as102_fe_read_status,
-	.read_snr		= as102_fe_read_snr,
-	.read_ber		= as102_fe_read_ber,
-	.read_signal_strength	= as102_fe_read_signal_strength,
-	.read_ucblocks		= as102_fe_read_ucblocks,
-	.ts_bus_ctrl		= as102_fe_ts_bus_ctrl,
-};
-
-int as102_dvb_unregister_fe(struct dvb_frontend *fe)
-{
-	/* unregister frontend */
-	dvb_unregister_frontend(fe);
-
-	/* detach frontend */
-	dvb_frontend_detach(fe);
-
-	return 0;
-}
-
-int as102_dvb_register_fe(struct as102_dev_t *as102_dev,
-			  struct dvb_frontend *dvb_fe)
-{
-	int errno;
-	struct dvb_adapter *dvb_adap;
-
-	if (as102_dev == NULL)
-		return -EINVAL;
-
-	/* extract dvb_adapter */
-	dvb_adap = &as102_dev->dvb_adap;
-
-	/* init frontend callback ops */
-	memcpy(&dvb_fe->ops, &as102_fe_ops, sizeof(struct dvb_frontend_ops));
-	strncpy(dvb_fe->ops.info.name, as102_dev->name,
-		sizeof(dvb_fe->ops.info.name));
-
-	/* register dvb frontend */
-	errno = dvb_register_frontend(dvb_adap, dvb_fe);
-	if (errno == 0)
-		dvb_fe->tuner_priv = as102_dev;
-
-	return errno;
-}
-
-static void as10x_fe_copy_tps_parameters(struct dtv_frontend_properties *fe_tps,
-					 struct as10x_tps *as10x_tps)
-{
-
-	/* extract constellation */
-	switch (as10x_tps->modulation) {
-	case CONST_QPSK:
-		fe_tps->modulation = QPSK;
-		break;
-	case CONST_QAM16:
-		fe_tps->modulation = QAM_16;
-		break;
-	case CONST_QAM64:
-		fe_tps->modulation = QAM_64;
-		break;
-	}
-
-	/* extract hierarchy */
-	switch (as10x_tps->hierarchy) {
-	case HIER_NONE:
-		fe_tps->hierarchy = HIERARCHY_NONE;
-		break;
-	case HIER_ALPHA_1:
-		fe_tps->hierarchy = HIERARCHY_1;
-		break;
-	case HIER_ALPHA_2:
-		fe_tps->hierarchy = HIERARCHY_2;
-		break;
-	case HIER_ALPHA_4:
-		fe_tps->hierarchy = HIERARCHY_4;
-		break;
-	}
-
-	/* extract code rate HP */
-	switch (as10x_tps->code_rate_HP) {
-	case CODE_RATE_1_2:
-		fe_tps->code_rate_HP = FEC_1_2;
-		break;
-	case CODE_RATE_2_3:
-		fe_tps->code_rate_HP = FEC_2_3;
-		break;
-	case CODE_RATE_3_4:
-		fe_tps->code_rate_HP = FEC_3_4;
-		break;
-	case CODE_RATE_5_6:
-		fe_tps->code_rate_HP = FEC_5_6;
-		break;
-	case CODE_RATE_7_8:
-		fe_tps->code_rate_HP = FEC_7_8;
-		break;
-	}
-
-	/* extract code rate LP */
-	switch (as10x_tps->code_rate_LP) {
-	case CODE_RATE_1_2:
-		fe_tps->code_rate_LP = FEC_1_2;
-		break;
-	case CODE_RATE_2_3:
-		fe_tps->code_rate_LP = FEC_2_3;
-		break;
-	case CODE_RATE_3_4:
-		fe_tps->code_rate_LP = FEC_3_4;
-		break;
-	case CODE_RATE_5_6:
-		fe_tps->code_rate_LP = FEC_5_6;
-		break;
-	case CODE_RATE_7_8:
-		fe_tps->code_rate_LP = FEC_7_8;
-		break;
-	}
-
-	/* extract guard interval */
-	switch (as10x_tps->guard_interval) {
-	case GUARD_INT_1_32:
-		fe_tps->guard_interval = GUARD_INTERVAL_1_32;
-		break;
-	case GUARD_INT_1_16:
-		fe_tps->guard_interval = GUARD_INTERVAL_1_16;
-		break;
-	case GUARD_INT_1_8:
-		fe_tps->guard_interval = GUARD_INTERVAL_1_8;
-		break;
-	case GUARD_INT_1_4:
-		fe_tps->guard_interval = GUARD_INTERVAL_1_4;
-		break;
-	}
-
-	/* extract transmission mode */
-	switch (as10x_tps->transmission_mode) {
-	case TRANS_MODE_2K:
-		fe_tps->transmission_mode = TRANSMISSION_MODE_2K;
-		break;
-	case TRANS_MODE_8K:
-		fe_tps->transmission_mode = TRANSMISSION_MODE_8K;
-		break;
-	}
-}
-
-static uint8_t as102_fe_get_code_rate(fe_code_rate_t arg)
-{
-	uint8_t c;
-
-	switch (arg) {
-	case FEC_1_2:
-		c = CODE_RATE_1_2;
-		break;
-	case FEC_2_3:
-		c = CODE_RATE_2_3;
-		break;
-	case FEC_3_4:
-		c = CODE_RATE_3_4;
-		break;
-	case FEC_5_6:
-		c = CODE_RATE_5_6;
-		break;
-	case FEC_7_8:
-		c = CODE_RATE_7_8;
-		break;
-	default:
-		c = CODE_RATE_UNKNOWN;
-		break;
-	}
-
-	return c;
-}
-
-static void as102_fe_copy_tune_parameters(struct as10x_tune_args *tune_args,
-			  struct dtv_frontend_properties *params)
-{
-
-	/* set frequency */
-	tune_args->freq = params->frequency / 1000;
-
-	/* fix interleaving_mode */
-	tune_args->interleaving_mode = INTLV_NATIVE;
-
-	switch (params->bandwidth_hz) {
-	case 8000000:
-		tune_args->bandwidth = BW_8_MHZ;
-		break;
-	case 7000000:
-		tune_args->bandwidth = BW_7_MHZ;
-		break;
-	case 6000000:
-		tune_args->bandwidth = BW_6_MHZ;
-		break;
-	default:
-		tune_args->bandwidth = BW_8_MHZ;
-	}
-
-	switch (params->guard_interval) {
-	case GUARD_INTERVAL_1_32:
-		tune_args->guard_interval = GUARD_INT_1_32;
-		break;
-	case GUARD_INTERVAL_1_16:
-		tune_args->guard_interval = GUARD_INT_1_16;
-		break;
-	case GUARD_INTERVAL_1_8:
-		tune_args->guard_interval = GUARD_INT_1_8;
-		break;
-	case GUARD_INTERVAL_1_4:
-		tune_args->guard_interval = GUARD_INT_1_4;
-		break;
-	case GUARD_INTERVAL_AUTO:
-	default:
-		tune_args->guard_interval = GUARD_UNKNOWN;
-		break;
-	}
-
-	switch (params->modulation) {
-	case QPSK:
-		tune_args->modulation = CONST_QPSK;
-		break;
-	case QAM_16:
-		tune_args->modulation = CONST_QAM16;
-		break;
-	case QAM_64:
-		tune_args->modulation = CONST_QAM64;
-		break;
-	default:
-		tune_args->modulation = CONST_UNKNOWN;
-		break;
-	}
-
-	switch (params->transmission_mode) {
-	case TRANSMISSION_MODE_2K:
-		tune_args->transmission_mode = TRANS_MODE_2K;
-		break;
-	case TRANSMISSION_MODE_8K:
-		tune_args->transmission_mode = TRANS_MODE_8K;
-		break;
-	default:
-		tune_args->transmission_mode = TRANS_MODE_UNKNOWN;
-	}
-
-	switch (params->hierarchy) {
-	case HIERARCHY_NONE:
-		tune_args->hierarchy = HIER_NONE;
-		break;
-	case HIERARCHY_1:
-		tune_args->hierarchy = HIER_ALPHA_1;
-		break;
-	case HIERARCHY_2:
-		tune_args->hierarchy = HIER_ALPHA_2;
-		break;
-	case HIERARCHY_4:
-		tune_args->hierarchy = HIER_ALPHA_4;
-		break;
-	case HIERARCHY_AUTO:
-		tune_args->hierarchy = HIER_UNKNOWN;
-		break;
-	}
-
-	dprintk(debug, "tuner parameters: freq: %d  bw: 0x%02x  gi: 0x%02x\n",
-			params->frequency,
-			tune_args->bandwidth,
-			tune_args->guard_interval);
-
-	/*
-	 * Detect a hierarchy selection
-	 * if HP/LP are both set to FEC_NONE, HP will be selected.
-	 */
-	if ((tune_args->hierarchy != HIER_NONE) &&
-		       ((params->code_rate_LP == FEC_NONE) ||
-			(params->code_rate_HP == FEC_NONE))) {
-
-		if (params->code_rate_LP == FEC_NONE) {
-			tune_args->hier_select = HIER_HIGH_PRIORITY;
-			tune_args->code_rate =
-			   as102_fe_get_code_rate(params->code_rate_HP);
-		}
-
-		if (params->code_rate_HP == FEC_NONE) {
-			tune_args->hier_select = HIER_LOW_PRIORITY;
-			tune_args->code_rate =
-			   as102_fe_get_code_rate(params->code_rate_LP);
-		}
-
-		dprintk(debug,
-			"\thierarchy: 0x%02x  selected: %s  code_rate_%s: 0x%02x\n",
-			tune_args->hierarchy,
-			tune_args->hier_select == HIER_HIGH_PRIORITY ?
-			"HP" : "LP",
-			tune_args->hier_select == HIER_HIGH_PRIORITY ?
-			"HP" : "LP",
-			tune_args->code_rate);
-	} else {
-		tune_args->code_rate =
-			as102_fe_get_code_rate(params->code_rate_HP);
-	}
-}
diff --git a/drivers/staging/media/cxd2099/cxd2099.c b/drivers/staging/media/cxd2099/cxd2099.c
index a2a5182570c5..73e7b2c9e4a7 100644
--- a/drivers/staging/media/cxd2099/cxd2099.c
+++ b/drivers/staging/media/cxd2099/cxd2099.c
@@ -152,6 +152,7 @@ static int write_pccard(struct cxd *ci, u16 address, u8 *data, u8 n)
 	status = i2c_write(ci->i2c, ci->cfg.adr, addr, 3);
 	if (!status) {
 		u8 buf[256] = {3};
+
 		memcpy(buf+1, data, n);
 		status = i2c_write(ci->i2c, ci->cfg.adr, buf, n+1);
 	}
@@ -201,6 +202,7 @@ static int write_io_data(struct cxd *ci, u8 *data, u8 n)
 	status = i2c_write(ci->i2c, ci->cfg.adr, addr, 3);
 	if (!status) {
 		u8 buf[256] = {3};
+
 		memcpy(buf+1, data, n);
 		status = i2c_write(ci->i2c, ci->cfg.adr, buf, n + 1);
 	}
@@ -448,6 +450,7 @@ static int read_attribute_mem(struct dvb_ca_en50221 *ca,
 	return ci->amem[address];
 #else
 	u8 val;
+
 	mutex_lock(&ci->lock);
 	set_mode(ci, 1);
 	read_pccard(ci, address, &val, 1);
diff --git a/drivers/staging/media/davinci_vpfe/Kconfig b/drivers/staging/media/davinci_vpfe/Kconfig
index 12f321dd2399..4de2f082491d 100644
--- a/drivers/staging/media/davinci_vpfe/Kconfig
+++ b/drivers/staging/media/davinci_vpfe/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_DM365_VPFE
 	tristate "DM365 VPFE Media Controller Capture Driver"
 	depends on VIDEO_V4L2 && ARCH_DAVINCI_DM365 && !VIDEO_DM365_ISIF
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	help
 	  Support for DM365 VPFE based Media Controller Capture driver.
diff --git a/drivers/staging/media/davinci_vpfe/dm365_isif.c b/drivers/staging/media/davinci_vpfe/dm365_isif.c
index b942bf73c43f..0d535b062e4e 100644
--- a/drivers/staging/media/davinci_vpfe/dm365_isif.c
+++ b/drivers/staging/media/davinci_vpfe/dm365_isif.c
@@ -440,14 +440,12 @@ static int isif_validate_df_csc_params(struct vpfe_isif_df_csc *df_csc)
 {
 	struct vpfe_isif_color_space_conv *csc;
 	int err = -EINVAL;
-	int csc_df_en;
 	int i;
 
 	if (!df_csc->df_or_csc) {
 		/* csc configuration */
 		csc = &df_csc->csc;
 		if (csc->en) {
-			csc_df_en = 1;
 			for (i = 0; i < VPFE_ISIF_CSC_NUM_COEFF; i++)
 				if (csc->coeff[i].integer >
 				    ISIF_CSC_COEF_INTEG_MASK ||
diff --git a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
index cda8388cbb89..a862b28092e4 100644
--- a/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
+++ b/drivers/staging/media/davinci_vpfe/vpfe_mc_capture.c
@@ -228,10 +228,8 @@ static int vpfe_enable_clock(struct vpfe_device *vpfe_dev)
 
 	vpfe_dev->clks = kzalloc(vpfe_cfg->num_clocks *
 				   sizeof(struct clock *), GFP_KERNEL);
-	if (vpfe_dev->clks == NULL) {
-		v4l2_err(vpfe_dev->pdev->driver, "Memory allocation failed\n");
+	if (vpfe_dev->clks == NULL)
 		return -ENOMEM;
-	}
 
 	for (i = 0; i < vpfe_cfg->num_clocks; i++) {
 		if (vpfe_cfg->clocks[i] == NULL) {
@@ -349,11 +347,8 @@ static int register_i2c_devices(struct vpfe_device *vpfe_dev)
 	num_subdevs = vpfe_cfg->num_subdevs;
 	vpfe_dev->sd =
 		  kzalloc(sizeof(struct v4l2_subdev *)*num_subdevs, GFP_KERNEL);
-	if (vpfe_dev->sd == NULL) {
-		v4l2_err(&vpfe_dev->v4l2_dev,
-			"unable to allocate memory for subdevice\n");
+	if (vpfe_dev->sd == NULL)
 		return -ENOMEM;
-	}
 
 	for (i = 0, k = 0; i < num_subdevs; i++) {
 		sdinfo = &vpfe_cfg->sub_devs[i];
@@ -582,11 +577,8 @@ static int vpfe_probe(struct platform_device *pdev)
 	int ret = -ENOMEM;
 
 	vpfe_dev = kzalloc(sizeof(*vpfe_dev), GFP_KERNEL);
-	if (!vpfe_dev) {
-		v4l2_err(pdev->dev.driver,
-			"Failed to allocate memory for vpfe_dev\n");
+	if (!vpfe_dev)
 		return ret;
-	}
 
 	if (pdev->dev.platform_data == NULL) {
 		v4l2_err(pdev->dev.driver, "Unable to get vpfe config\n");
diff --git a/drivers/staging/media/dt3155v4l/Kconfig b/drivers/staging/media/dt3155v4l/Kconfig
index 226a1ca90b3c..2d496001b6e8 100644
--- a/drivers/staging/media/dt3155v4l/Kconfig
+++ b/drivers/staging/media/dt3155v4l/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_DT3155
 	tristate "DT3155 frame grabber, Video4Linux interface"
 	depends on PCI && VIDEO_DEV && VIDEO_V4L2
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	default n
 	---help---
diff --git a/drivers/staging/media/lirc/lirc_bt829.c b/drivers/staging/media/lirc/lirc_bt829.c
index fe010542ab4b..4c806ba41323 100644
--- a/drivers/staging/media/lirc/lirc_bt829.c
+++ b/drivers/staging/media/lirc/lirc_bt829.c
@@ -71,6 +71,7 @@ static struct lirc_driver atir_driver;
 static struct pci_dev *do_pci_probe(void)
 {
 	struct pci_dev *my_dev;
+
 	my_dev = pci_get_device(PCI_VENDOR_ID_ATI,
 				PCI_DEVICE_ID_ATI_264VT, NULL);
 	if (my_dev) {
@@ -96,6 +97,7 @@ static int atir_add_to_buf(void *data, struct lirc_buffer *buf)
 {
 	unsigned char key;
 	int status;
+
 	status = poll_main();
 	key = (status >> 8) & 0xFF;
 	if (status & 0xFF) {
@@ -229,6 +231,7 @@ static void do_i2c_start(void)
 static void do_i2c_stop(void)
 {
 	unsigned char bits;
+
 	bits =  do_get_bits() & 0xFD;
 	do_set_bits(bits);
 	cycle_delay(1);
@@ -333,6 +336,7 @@ static unsigned char seems_rd_byte(void)
 static void do_set_bits(unsigned char new_bits)
 {
 	int reg_val;
+
 	reg_val = read_index(0x34);
 	if (new_bits & 2) {
 		reg_val &= 0xFFFFFFDF;
@@ -383,16 +387,15 @@ static unsigned char do_get_bits(void)
 static unsigned int read_index(unsigned char index)
 {
 	unsigned char __iomem *addr;
-	unsigned int value;
 	/*  addr = pci_addr_lin + DATA_PCI_OFF + ((index & 0xFF) << 2); */
 	addr = pci_addr_lin + ((index & 0xFF) << 2);
-	value = readl(addr);
-	return value;
+	return readl(addr);
 }
 
 static void write_index(unsigned char index, unsigned int reg_val)
 {
 	unsigned char __iomem *addr;
+
 	addr = pci_addr_lin + ((index & 0xFF) << 2);
 	writel(reg_val, addr);
 }
diff --git a/drivers/staging/media/lirc/lirc_imon.c b/drivers/staging/media/lirc/lirc_imon.c
index 96c76b33770b..7aca44f28c5a 100644
--- a/drivers/staging/media/lirc/lirc_imon.c
+++ b/drivers/staging/media/lirc/lirc_imon.c
@@ -414,6 +414,7 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 	data_buf = memdup_user(buf, n_bytes);
 	if (IS_ERR(data_buf)) {
 		retval = PTR_ERR(data_buf);
+		data_buf = NULL;
 		goto exit;
 	}
 
@@ -489,7 +490,6 @@ static void usb_tx_callback(struct urb *urb)
  */
 static int ir_open(void *data)
 {
-	int retval = 0;
 	struct imon_context *context;
 
 	/* prevent races with disconnect */
@@ -506,7 +506,7 @@ static int ir_open(void *data)
 	dev_info(context->driver->dev, "IR port opened\n");
 
 	mutex_unlock(&driver_lock);
-	return retval;
+	return 0;
 }
 
 /**
@@ -615,8 +615,8 @@ static void imon_incoming_packet(struct imon_context *context,
 		return;
 
 	if (len != 8) {
-		dev_warn(dev, "imon %s: invalid incoming packet "
-			 "size (len = %d, intf%d)\n", __func__, len, intf);
+		dev_warn(dev, "imon %s: invalid incoming packet size (len = %d, intf%d)\n",
+			__func__, len, intf);
 		return;
 	}
 
@@ -925,9 +925,8 @@ static int imon_probe(struct usb_interface *interface,
 		}
 	}
 
-	dev_info(dev, "iMON device (%04x:%04x, intf%d) on "
-		 "usb<%d:%d> initialized\n", vendor, product, ifnum,
-		 usbdev->bus->busnum, usbdev->devnum);
+	dev_info(dev, "iMON device (%04x:%04x, intf%d) on usb<%d:%d> initialized\n",
+		vendor, product, ifnum, usbdev->bus->busnum, usbdev->devnum);
 
 unlock:
 	mutex_unlock(&context->ctx_lock);
@@ -1021,7 +1020,6 @@ static int imon_suspend(struct usb_interface *intf, pm_message_t message)
 
 static int imon_resume(struct usb_interface *intf)
 {
-	int rc = 0;
 	struct imon_context *context = usb_get_intfdata(intf);
 
 	usb_fill_int_urb(context->rx_urb, context->usbdev,
@@ -1031,9 +1029,7 @@ static int imon_resume(struct usb_interface *intf)
 		usb_rx_callback, context,
 		context->rx_endpoint->bInterval);
 
-	rc = usb_submit_urb(context->rx_urb, GFP_ATOMIC);
-
-	return rc;
+	return usb_submit_urb(context->rx_urb, GFP_ATOMIC);
 }
 
 module_usb_driver(imon_driver);
diff --git a/drivers/staging/media/lirc/lirc_parallel.c b/drivers/staging/media/lirc/lirc_parallel.c
index 672858a089f3..5156c2181016 100644
--- a/drivers/staging/media/lirc/lirc_parallel.c
+++ b/drivers/staging/media/lirc/lirc_parallel.c
@@ -177,10 +177,9 @@ static unsigned int init_lirc_timer(void)
 			pr_notice("using default timer: %u Hz\n",
 				  default_timer);
 			return default_timer;
-		} else {
-			pr_info("%u Hz timer detected\n", newtimer);
-			return newtimer; /* use detected value */
 		}
+		pr_info("%u Hz timer detected\n", newtimer);
+		return newtimer; /* use detected value */
 	}
 
 	pr_notice("no timer detected\n");
diff --git a/drivers/staging/media/lirc/lirc_sasem.c b/drivers/staging/media/lirc/lirc_sasem.c
index 81f90e17e1e6..c20ef56202bf 100644
--- a/drivers/staging/media/lirc/lirc_sasem.c
+++ b/drivers/staging/media/lirc/lirc_sasem.c
@@ -392,6 +392,7 @@ static ssize_t vfd_write(struct file *file, const char __user *buf,
 	data_buf = memdup_user((void const __user *)buf, n_bytes);
 	if (IS_ERR(data_buf)) {
 		retval = PTR_ERR(data_buf);
+		data_buf = NULL;
 		goto exit;
 	}
 
@@ -474,8 +475,6 @@ static void usb_tx_callback(struct urb *urb)
 	/* notify waiters that write has finished */
 	atomic_set(&context->tx.busy, 0);
 	complete(&context->tx.finished);
-
-	return;
 }
 
 /**
@@ -562,7 +561,6 @@ static void ir_close(void *data)
 	}
 
 	mutex_unlock(&context->ctx_lock);
-	return;
 }
 
 /**
@@ -664,7 +662,6 @@ static void usb_rx_callback(struct urb *urb)
 	}
 
 	usb_submit_urb(context->rx_urb, GFP_ATOMIC);
-	return;
 }
 
 
@@ -713,6 +710,7 @@ static int sasem_probe(struct usb_interface *interface,
 		struct usb_endpoint_descriptor *ep;
 		int ep_dir;
 		int ep_type;
+
 		ep = &iface_desc->endpoint [i].desc;
 		ep_dir = ep->bEndpointAddress & USB_ENDPOINT_DIR_MASK;
 		ep_type = ep->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
diff --git a/drivers/staging/media/lirc/lirc_serial.c b/drivers/staging/media/lirc/lirc_serial.c
index bae0d467093e..181b92bfe25d 100644
--- a/drivers/staging/media/lirc/lirc_serial.c
+++ b/drivers/staging/media/lirc/lirc_serial.c
@@ -496,6 +496,7 @@ static long send_pulse_homebrew_softcarrier(unsigned long length)
 {
 	int flag;
 	unsigned long actual, target, d;
+
 	length <<= 8;
 
 	actual = 0; target = 0; flag = 0;
@@ -528,11 +529,10 @@ static long send_pulse_homebrew(unsigned long length)
 
 	if (softcarrier)
 		return send_pulse_homebrew_softcarrier(length);
-	else {
-		on();
-		safe_udelay(length);
-		return 0;
-	}
+
+	on();
+	safe_udelay(length);
+	return 0;
 }
 
 static void send_space_irdeo(long length)
@@ -948,7 +948,6 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 
 	case LIRC_GET_LENGTH:
 		return -ENOIOCTLCMD;
-		break;
 
 	case LIRC_SET_SEND_DUTY_CYCLE:
 		dprintk("SET_SEND_DUTY_CYCLE\n");
@@ -961,7 +960,6 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (value <= 0 || value > 100)
 			return -EINVAL;
 		return init_timing_params(value, freq);
-		break;
 
 	case LIRC_SET_SEND_CARRIER:
 		dprintk("SET_SEND_CARRIER\n");
@@ -974,7 +972,6 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 		if (value > 500000 || value < 20000)
 			return -EINVAL;
 		return init_timing_params(duty_cycle, value);
-		break;
 
 	default:
 		return lirc_dev_fop_ioctl(filep, cmd, arg);
diff --git a/drivers/staging/media/lirc/lirc_sir.c b/drivers/staging/media/lirc/lirc_sir.c
index 79da3adf1bd5..2ee55eaf2a53 100644
--- a/drivers/staging/media/lirc/lirc_sir.c
+++ b/drivers/staging/media/lirc/lirc_sir.c
@@ -280,6 +280,7 @@ static long lirc_ioctl(struct file *filep, unsigned int cmd, unsigned long arg)
 	u32 __user *uptr = (u32 __user *)arg;
 	int retval = 0;
 	u32 value = 0;
+
 	if (cmd == LIRC_GET_FEATURES)
 		value = LIRC_CAN_SEND_PULSE | LIRC_CAN_REC_MODE2;
 	else if (cmd == LIRC_GET_SEND_MODE)
diff --git a/drivers/staging/media/lirc/lirc_zilog.c b/drivers/staging/media/lirc/lirc_zilog.c
index 3259aacfd350..567feba0011c 100644
--- a/drivers/staging/media/lirc/lirc_zilog.c
+++ b/drivers/staging/media/lirc/lirc_zilog.c
@@ -258,7 +258,6 @@ static void release_ir_rx(struct kref *ref)
 	/* Don't put_ir_device(rx->ir) here; lock can't be freed yet */
 	ir->rx = NULL;
 	/* Don't do the kfree(rx) here; we still need to kill the poll thread */
-	return;
 }
 
 static int put_ir_rx(struct IR_rx *rx, bool ir_devices_lock_held)
@@ -512,7 +511,6 @@ static int set_use_inc(void *data)
 
 static void set_use_dec(void *data)
 {
-	return;
 }
 
 /* safe read of a uint32 (always network byte order) */
@@ -618,6 +616,7 @@ static int get_key_data(unsigned char *buf,
 	for (base = 0, lim = keys - 1; lim; lim >>= 1) {
 		/* Seek to block */
 		unsigned char *key_data;
+
 		pos = base + (lim >> 1);
 		key_data = key_block + (ndiffs + 1) * pos;
 
@@ -628,6 +627,7 @@ static int get_key_data(unsigned char *buf,
 			/* found, so unpack the diffs */
 			for (i = 0; i < ndiffs; ++i) {
 				unsigned char val;
+
 				if (!read_uint8(&key_data, endp, &val) ||
 				    diffs[i] >= TX_BLOCK_SIZE)
 					goto corrupt;
@@ -656,6 +656,7 @@ static int send_data_block(struct IR_tx *tx, unsigned char *data_block)
 
 	for (i = 0; i < TX_BLOCK_SIZE;) {
 		int tosend = TX_BLOCK_SIZE - i;
+
 		if (tosend > 4)
 			tosend = 4;
 		buf[0] = (unsigned char)(i + 1);
@@ -838,6 +839,7 @@ static int fw_load(struct IR_tx *tx)
 		goto corrupt;
 	for (i = 0; i < num_global_fixed; ++i) {
 		unsigned char pos, val;
+
 		if (!read_uint8(&data, tx_data->endp, &pos) ||
 		    !read_uint8(&data, tx_data->endp, &val) ||
 		    pos >= TX_BLOCK_SIZE)
@@ -1336,6 +1338,7 @@ static int close(struct inode *node, struct file *filep)
 {
 	/* find our IR struct */
 	struct IR *ir = filep->private_data;
+
 	if (ir == NULL) {
 		zilog_error("close: no private_data attached to the file!\n");
 		return -ENODEV;
@@ -1402,6 +1405,7 @@ static int ir_remove(struct i2c_client *client)
 {
 	if (strncmp("ir_tx_z8", client->name, 8) == 0) {
 		struct IR_tx *tx = i2c_get_clientdata(client);
+
 		if (tx != NULL) {
 			mutex_lock(&tx->client_lock);
 			tx->c = NULL;
@@ -1410,6 +1414,7 @@ static int ir_remove(struct i2c_client *client)
 		}
 	} else if (strncmp("ir_rx_z8", client->name, 8) == 0) {
 		struct IR_rx *rx = i2c_get_clientdata(client);
+
 		if (rx != NULL) {
 			mutex_lock(&rx->client_lock);
 			rx->c = NULL;
diff --git a/drivers/staging/media/omap24xx/omap24xxcam-dma.c b/drivers/staging/media/omap24xx/omap24xxcam-dma.c
index 9c00776d6583..c427eb94ea66 100644
--- a/drivers/staging/media/omap24xx/omap24xxcam-dma.c
+++ b/drivers/staging/media/omap24xx/omap24xxcam-dma.c
@@ -149,8 +149,8 @@ static void omap24xxcam_dmahw_transfer_chain(void __iomem *base, int dmach,
 			 */
 			omap24xxcam_dmahw_transfer_start(base, dmach);
 			break;
-		} else
-			ch = (ch + 1) % NUM_CAMDMA_CHANNELS;
+		}
+		ch = (ch + 1) % NUM_CAMDMA_CHANNELS;
 	}
 }
 
@@ -332,15 +332,14 @@ void omap24xxcam_dma_isr(struct omap24xxcam_dma *dma)
 			spin_unlock(&dma->lock);
 			omap24xxcam_dma_stop(dma, csr);
 			return;
-		} else {
-			callback = dma->ch_state[dmach].callback;
-			arg = dma->ch_state[dmach].arg;
-			dma->free_dmach++;
-			if (callback) {
-				spin_unlock(&dma->lock);
-				(*callback) (dma, csr, arg);
-				spin_lock(&dma->lock);
-			}
+		}
+		callback = dma->ch_state[dmach].callback;
+		arg = dma->ch_state[dmach].arg;
+		dma->free_dmach++;
+		if (callback) {
+			spin_unlock(&dma->lock);
+			(*callback) (dma, csr, arg);
+			spin_lock(&dma->lock);
 		}
 	}
 
@@ -475,17 +474,14 @@ void omap24xxcam_sgdma_process(struct omap24xxcam_sgdma *sgdma)
 				/* DMA start failed */
 				spin_unlock_irqrestore(&sgdma->lock, flags);
 				return;
-			} else {
-				unsigned long expires;
-				/* DMA start was successful */
-				sg_state->next_sglist++;
-				sg_state->bytes_read += len;
-				sg_state->queued_sglist++;
-
-				/* We start the reset timer */
-				expires = jiffies + HZ;
-				mod_timer(&sgdma->reset_timer, expires);
 			}
+			/* DMA start was successful */
+			sg_state->next_sglist++;
+			sg_state->bytes_read += len;
+			sg_state->queued_sglist++;
+
+			/* We start the reset timer */
+			mod_timer(&sgdma->reset_timer, jiffies + HZ);
 		}
 		queued_sgdma--;
 		sgslot = (sgslot + 1) % NUM_SG_DMA;
@@ -563,6 +559,7 @@ void omap24xxcam_sgdma_sync(struct omap24xxcam_sgdma *sgdma)
 			/* This DMA transfer was in progress, so abort it. */
 			sgdma_callback_t callback = sg_state->callback;
 			void *arg = sg_state->arg;
+
 			sgdma->free_sgdma++;
 			if (callback) {
 				/* leave interrupts masked */
diff --git a/drivers/staging/media/omap24xx/omap24xxcam.c b/drivers/staging/media/omap24xx/omap24xxcam.c
index d2b440c842b3..d590b3e8b70c 100644
--- a/drivers/staging/media/omap24xx/omap24xxcam.c
+++ b/drivers/staging/media/omap24xx/omap24xxcam.c
@@ -1003,11 +1003,8 @@ static int vidioc_enum_fmt_vid_cap(struct file *file, void *fh,
 {
 	struct omap24xxcam_fh *ofh = fh;
 	struct omap24xxcam_device *cam = ofh->cam;
-	int rval;
-
-	rval = vidioc_int_enum_fmt_cap(cam->sdev, f);
 
-	return rval;
+	return vidioc_int_enum_fmt_cap(cam->sdev, f);
 }
 
 static int vidioc_g_fmt_vid_cap(struct file *file, void *fh,
@@ -1245,11 +1242,8 @@ static int vidioc_queryctrl(struct file *file, void *fh,
 {
 	struct omap24xxcam_fh *ofh = fh;
 	struct omap24xxcam_device *cam = ofh->cam;
-	int rval;
-
-	rval = vidioc_int_queryctrl(cam->sdev, a);
 
-	return rval;
+	return vidioc_int_queryctrl(cam->sdev, a);
 }
 
 static int vidioc_g_ctrl(struct file *file, void *fh,
diff --git a/drivers/staging/media/omap24xx/tcm825x.c b/drivers/staging/media/omap24xx/tcm825x.c
index 3367ccd1d1e8..9d9ecf1fc4ae 100644
--- a/drivers/staging/media/omap24xx/tcm825x.c
+++ b/drivers/staging/media/omap24xx/tcm825x.c
@@ -542,6 +542,7 @@ static int ioctl_s_ctrl(struct v4l2_int_device *s,
 	/* exposure time is special, spread across 2 registers */
 	if (vc->id == V4L2_CID_EXPOSURE) {
 		int val_lower, val_upper;
+
 		val_lower = val & TCM825X_MASK(TCM825X_ESRSPD_L);
 		val_upper = (val >> 8) & TCM825X_MASK(TCM825X_ESRSPD_U);
 
diff --git a/drivers/staging/media/omap4iss/Kconfig b/drivers/staging/media/omap4iss/Kconfig
index 8afc6fee40c5..b78643f907e7 100644
--- a/drivers/staging/media/omap4iss/Kconfig
+++ b/drivers/staging/media/omap4iss/Kconfig
@@ -1,6 +1,7 @@
 config VIDEO_OMAP4
 	bool "OMAP 4 Camera support"
 	depends on VIDEO_V4L2=y && VIDEO_V4L2_SUBDEV_API && I2C=y && ARCH_OMAP4
+	depends on HAS_DMA
 	select VIDEOBUF2_DMA_CONTIG
 	---help---
 	  Driver for an OMAP 4 ISS controller.
diff --git a/drivers/staging/media/omap4iss/iss_csi2.c b/drivers/staging/media/omap4iss/iss_csi2.c
index 9ae4871928d8..92c2d5b743c7 100644
--- a/drivers/staging/media/omap4iss/iss_csi2.c
+++ b/drivers/staging/media/omap4iss/iss_csi2.c
@@ -421,6 +421,7 @@ static void csi2_irq_ctx_set(struct iss_csi2_device *csi2, int enable)
 static void csi2_irq_complexio1_set(struct iss_csi2_device *csi2, int enable)
 {
 	u32 reg;
+
 	reg = CSI2_COMPLEXIO_IRQ_STATEALLULPMEXIT |
 		CSI2_COMPLEXIO_IRQ_STATEALLULPMENTER |
 		CSI2_COMPLEXIO_IRQ_STATEULPM5 |
@@ -464,6 +465,7 @@ static void csi2_irq_complexio1_set(struct iss_csi2_device *csi2, int enable)
 static void csi2_irq_status_set(struct iss_csi2_device *csi2, int enable)
 {
 	u32 reg;
+
 	reg = CSI2_IRQ_OCP_ERR |
 		CSI2_IRQ_SHORT_PACKET |
 		CSI2_IRQ_ECC_CORRECTION |
@@ -806,8 +808,8 @@ __csi2_get_format(struct iss_csi2_device *csi2, struct v4l2_subdev_fh *fh,
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(fh, pad);
-	else
-		return &csi2->formats[pad];
+
+	return &csi2->formats[pad];
 }
 
 static void
diff --git a/drivers/staging/media/omap4iss/iss_ipipe.c b/drivers/staging/media/omap4iss/iss_ipipe.c
index 6eaafc5e2eea..54042008154c 100644
--- a/drivers/staging/media/omap4iss/iss_ipipe.c
+++ b/drivers/staging/media/omap4iss/iss_ipipe.c
@@ -181,8 +181,8 @@ __ipipe_get_format(struct iss_ipipe_device *ipipe, struct v4l2_subdev_fh *fh,
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(fh, pad);
-	else
-		return &ipipe->formats[pad];
+
+	return &ipipe->formats[pad];
 }
 
 /*
diff --git a/drivers/staging/media/omap4iss/iss_ipipeif.c b/drivers/staging/media/omap4iss/iss_ipipeif.c
index 7bc145762499..75f6a15ad202 100644
--- a/drivers/staging/media/omap4iss/iss_ipipeif.c
+++ b/drivers/staging/media/omap4iss/iss_ipipeif.c
@@ -382,8 +382,7 @@ __ipipeif_get_format(struct iss_ipipeif_device *ipipeif,
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(fh, pad);
-	else
-		return &ipipeif->formats[pad];
+	return &ipipeif->formats[pad];
 }
 
 /*
diff --git a/drivers/staging/media/omap4iss/iss_resizer.c b/drivers/staging/media/omap4iss/iss_resizer.c
index ae831b8985c9..a21e356cce3a 100644
--- a/drivers/staging/media/omap4iss/iss_resizer.c
+++ b/drivers/staging/media/omap4iss/iss_resizer.c
@@ -441,8 +441,7 @@ __resizer_get_format(struct iss_resizer_device *resizer,
 {
 	if (which == V4L2_SUBDEV_FORMAT_TRY)
 		return v4l2_subdev_get_try_format(fh, pad);
-	else
-		return &resizer->formats[pad];
+	return &resizer->formats[pad];
 }
 
 /*
diff --git a/drivers/staging/media/omap4iss/iss_video.c b/drivers/staging/media/omap4iss/iss_video.c
index 943b5b09c632..5d6250337fec 100644
--- a/drivers/staging/media/omap4iss/iss_video.c
+++ b/drivers/staging/media/omap4iss/iss_video.c
@@ -908,6 +908,7 @@ iss_video_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
 	 */
 	if (pipe->input == NULL) {
 		unsigned long flags;
+
 		ret = omap4iss_pipeline_set_stream(pipe,
 					      ISS_PIPELINE_STREAM_CONTINUOUS);
 		if (ret < 0)
diff --git a/drivers/staging/mt29f_spinand/mt29f_spinand.c b/drivers/staging/mt29f_spinand/mt29f_spinand.c
index 3464e0c521e6..3628bcb840c3 100644
--- a/drivers/staging/mt29f_spinand/mt29f_spinand.c
+++ b/drivers/staging/mt29f_spinand/mt29f_spinand.c
@@ -252,15 +252,13 @@ static int spinand_enable_ecc(struct spi_device *spi_nand)
 	if (retval < 0)
 		return retval;
 
-	if ((otp & OTP_ECC_MASK) == OTP_ECC_MASK) {
+	if ((otp & OTP_ECC_MASK) == OTP_ECC_MASK)
 		return 0;
-	} else {
-		otp |= OTP_ECC_MASK;
-		retval = spinand_set_otp(spi_nand, &otp);
-		if (retval < 0)
-			return retval;
-		return spinand_get_otp(spi_nand, &otp);
-	}
+	otp |= OTP_ECC_MASK;
+	retval = spinand_set_otp(spi_nand, &otp);
+	if (retval < 0)
+		return retval;
+	return spinand_get_otp(spi_nand, &otp);
 }
 #endif
 
@@ -279,8 +277,8 @@ static int spinand_disable_ecc(struct spi_device *spi_nand)
 		if (retval < 0)
 			return retval;
 		return spinand_get_otp(spi_nand, &otp);
-	} else
-		return 0;
+	}
+	return 0;
 }
 
 /**
@@ -529,8 +527,8 @@ static int spinand_program_page(struct spi_device *spi_nand,
 				dev_err(&spi_nand->dev,
 					"program error, page %d\n", page_id);
 				return -1;
-			} else
-				break;
+			}
+			break;
 		}
 	}
 #ifdef CONFIG_MTD_SPINAND_ONDIEECC
@@ -605,8 +603,8 @@ static int spinand_erase_block(struct spi_device *spi_nand, u16 block_id)
 				dev_err(&spi_nand->dev,
 					"erase error, block %d\n", block_id);
 				return -1;
-			} else
-				break;
+			}
+			break;
 		}
 	}
 	return 0;
diff --git a/drivers/staging/netlogic/TODO b/drivers/staging/netlogic/TODO
index 08e6d5218b3b..8f172b017b94 100644
--- a/drivers/staging/netlogic/TODO
+++ b/drivers/staging/netlogic/TODO
@@ -1,6 +1,5 @@
 * Implementing 64bit stat counter in software
 * All memory allocation should be changed to DMA allocations
-* All the netdev should be linked to single pdev as parent
 * Changing comments in to linux standred format
 
 Please send patches
diff --git a/drivers/staging/netlogic/platform_net.c b/drivers/staging/netlogic/platform_net.c
index 61f20e10d636..77c3c3522afa 100644
--- a/drivers/staging/netlogic/platform_net.c
+++ b/drivers/staging/netlogic/platform_net.c
@@ -72,116 +72,125 @@ static u32 xlr_gmac_irqs[] = { PIC_GMAC_0_IRQ, PIC_GMAC_1_IRQ,
 	PIC_GMAC_6_IRQ, PIC_GMAC_7_IRQ
 };
 
-static struct xlr_net_data ndata[MAX_NUM_GMAC];
-static struct resource xlr_net_res[8][2];
-static struct platform_device xlr_net_dev[8];
-static u32 __iomem *gmac0_addr;
+static struct resource xlr_net0_res[8];
+static struct resource xlr_net1_res[8];
 static u32 __iomem *gmac4_addr;
 static u32 __iomem *gpio_addr;
 
-static void config_mac(struct xlr_net_data *nd, int phy, u32 __iomem *serdes,
-		u32 __iomem *pcs, int rfr, int tx, int *bkt_size,
-		struct xlr_fmn_info *gmac_fmn_info, int phy_addr)
+static void xlr_resource_init(struct resource *res, int offset, int irq)
 {
-	nd->cpu_mask = nlm_current_node()->coremask;
-	nd->phy_interface = phy;
-	nd->rfr_station = rfr;
-	nd->tx_stnid = tx;
-	nd->mii_addr = gmac0_addr;
-	nd->serdes_addr = serdes;
-	nd->pcs_addr = pcs;
-	nd->gpio_addr = gpio_addr;
-
-	nd->bucket_size = bkt_size;
-	nd->gmac_fmn_info = gmac_fmn_info;
-	nd->phy_addr = phy_addr;
+	res->name = "gmac";
+
+	res->start = CPHYSADDR(nlm_mmio_base(offset));
+	res->end = res->start + 0xfff;
+	res->flags = IORESOURCE_MEM;
+
+	res++;
+	res->name = "gmac";
+	res->start = res->end = irq;
+	res->flags = IORESOURCE_IRQ;
 }
 
-static void net_device_init(int id, struct resource *res, int offset, int irq)
+static struct platform_device *gmac_controller2_init(void *gmac0_addr)
 {
-	res[0].name = "gmac";
-	res[0].start = CPHYSADDR(nlm_mmio_base(offset));
-	res[0].end = res[0].start + 0xfff;
-	res[0].flags = IORESOURCE_MEM;
-
-	res[1].name = "gmac";
-	res[1].start = irq;
-	res[1].end = irq;
-	res[1].flags = IORESOURCE_IRQ;
-
-	xlr_net_dev[id].name = "xlr-net";
-	xlr_net_dev[id].id = id;
-	xlr_net_dev[id].num_resources = 2;
-	xlr_net_dev[id].resource = res;
-	xlr_net_dev[id].dev.platform_data = &ndata[id];
+	int mac;
+	static struct xlr_net_data ndata1 = {
+		.phy_interface	= PHY_INTERFACE_MODE_SGMII,
+		.rfr_station	= FMN_STNID_GMAC1_FR_0,
+		.bucket_size	= xlr_board_fmn_config.bucket_size,
+		.gmac_fmn_info	= &xlr_board_fmn_config.gmac[1],
+	};
+
+	static struct platform_device xlr_net_dev1 = {
+		.name		= "xlr-net",
+		.id		= 1,
+		.dev.platform_data = &ndata1,
+	};
+
+	gmac4_addr = ioremap(CPHYSADDR(
+		nlm_mmio_base(NETLOGIC_IO_GMAC_4_OFFSET)), 0xfff);
+	ndata1.serdes_addr = gmac4_addr;
+	ndata1.pcs_addr	= gmac4_addr;
+	ndata1.mii_addr	= gmac0_addr;
+	ndata1.gpio_addr = gpio_addr;
+	ndata1.cpu_mask = nlm_current_node()->coremask;
+
+	xlr_net_dev1.resource = xlr_net1_res;
+
+	for (mac = 0; mac < 4; mac++) {
+		ndata1.tx_stnid[mac] = FMN_STNID_GMAC1_TX0 + mac;
+		ndata1.phy_addr[mac] = mac + 4 + 0x10;
+
+		xlr_resource_init(&xlr_net1_res[mac * 2],
+				xlr_gmac_offsets[mac + 4],
+				xlr_gmac_irqs[mac + 4]);
+	}
+	xlr_net_dev1.num_resources = 8;
+
+	return &xlr_net_dev1;
 }
 
 static void xls_gmac_init(void)
 {
 	int mac;
+	struct platform_device *xlr_net_dev1;
+	void __iomem *gmac0_addr = ioremap(CPHYSADDR(
+		nlm_mmio_base(NETLOGIC_IO_GMAC_0_OFFSET)), 0xfff);
 
-	gmac4_addr = ioremap(CPHYSADDR(
-		nlm_mmio_base(NETLOGIC_IO_GMAC_4_OFFSET)), 0xfff);
-	/* Passing GPIO base for serdes init. Only needed on sgmii ports*/
+	static struct xlr_net_data ndata0 = {
+		.rfr_station	= FMN_STNID_GMACRFR_0,
+		.bucket_size	= xlr_board_fmn_config.bucket_size,
+		.gmac_fmn_info	= &xlr_board_fmn_config.gmac[0],
+	};
+
+	static struct platform_device xlr_net_dev0 = {
+		.name		= "xlr-net",
+		.id		= 0,
+	};
+	xlr_net_dev0.dev.platform_data = &ndata0;
+	ndata0.serdes_addr = gmac0_addr;
+	ndata0.pcs_addr	= gmac0_addr;
+	ndata0.mii_addr	= gmac0_addr;
+
+	/* Passing GPIO base for serdes init. Only needed on sgmii ports */
 	gpio_addr = ioremap(CPHYSADDR(
 		nlm_mmio_base(NETLOGIC_IO_GPIO_OFFSET)), 0xfff);
+	ndata0.gpio_addr = gpio_addr;
+	ndata0.cpu_mask = nlm_current_node()->coremask;
+
+	xlr_net_dev0.resource = xlr_net0_res;
 
 	switch (nlm_prom_info.board_major_version) {
 	case 12:
 		/* first block RGMII or XAUI, use RGMII */
-		config_mac(&ndata[0],
-			PHY_INTERFACE_MODE_RGMII,
-			gmac0_addr,	/* serdes */
-			gmac0_addr,	/* pcs */
-			FMN_STNID_GMACRFR_0,
-			FMN_STNID_GMAC0_TX0,
-			xlr_board_fmn_config.bucket_size,
-			&xlr_board_fmn_config.gmac[0],
-			0);
-
-		net_device_init(0, xlr_net_res[0], xlr_gmac_offsets[0],
+		ndata0.phy_interface = PHY_INTERFACE_MODE_RGMII,
+		ndata0.tx_stnid[0] = FMN_STNID_GMAC0_TX0;
+		ndata0.phy_addr[0] = 0;
+
+		xlr_net_dev0.num_resources = 2;
+
+		xlr_resource_init(&xlr_net0_res[0], xlr_gmac_offsets[0],
 				xlr_gmac_irqs[0]);
-		platform_device_register(&xlr_net_dev[0]);
+		platform_device_register(&xlr_net_dev0);
 
 		/* second block is XAUI, not supported yet */
 		break;
 	default:
 		/* default XLS config, all ports SGMII */
+		ndata0.phy_interface = PHY_INTERFACE_MODE_SGMII;
 		for (mac = 0; mac < 4; mac++) {
-			config_mac(&ndata[mac],
-				PHY_INTERFACE_MODE_SGMII,
-				gmac0_addr,	/* serdes */
-				gmac0_addr,	/* pcs */
-				FMN_STNID_GMACRFR_0,
-				FMN_STNID_GMAC0_TX0 + mac,
-				xlr_board_fmn_config.bucket_size,
-				&xlr_board_fmn_config.gmac[0],
-				/* PHY address according to chip/board */
-				mac + 0x10);
-
-			net_device_init(mac, xlr_net_res[mac],
-					xlr_gmac_offsets[mac],
-					xlr_gmac_irqs[mac]);
-			platform_device_register(&xlr_net_dev[mac]);
-		}
+			ndata0.tx_stnid[mac] = FMN_STNID_GMAC0_TX0 + mac;
+			ndata0.phy_addr[mac] = mac + 0x10;
 
-		for (mac = 4; mac < MAX_NUM_XLS_GMAC; mac++) {
-			config_mac(&ndata[mac],
-				PHY_INTERFACE_MODE_SGMII,
-				gmac4_addr,	/* serdes */
-				gmac4_addr,	/* pcs */
-				FMN_STNID_GMAC1_FR_0,
-				FMN_STNID_GMAC1_TX0 + mac - 4,
-				xlr_board_fmn_config.bucket_size,
-				&xlr_board_fmn_config.gmac[1],
-				/* PHY address according to chip/board */
-				mac + 0x10);
-
-			net_device_init(mac, xlr_net_res[mac],
+			xlr_resource_init(&xlr_net0_res[mac * 2],
 					xlr_gmac_offsets[mac],
 					xlr_gmac_irqs[mac]);
-			platform_device_register(&xlr_net_dev[mac]);
 		}
+		xlr_net_dev0.num_resources = 8;
+		platform_device_register(&xlr_net_dev0);
+
+		xlr_net_dev1 = gmac_controller2_init(gmac0_addr);
+		platform_device_register(xlr_net_dev1);
 	}
 }
 
@@ -190,28 +199,41 @@ static void xlr_gmac_init(void)
 	int mac;
 
 	/* assume all GMACs for now */
+	static struct xlr_net_data ndata0 = {
+		.phy_interface	= PHY_INTERFACE_MODE_RGMII,
+		.serdes_addr	= NULL,
+		.pcs_addr	= NULL,
+		.rfr_station	= FMN_STNID_GMACRFR_0,
+		.bucket_size	= xlr_board_fmn_config.bucket_size,
+		.gmac_fmn_info	= &xlr_board_fmn_config.gmac[0],
+		.gpio_addr	= NULL,
+	};
+
+
+	static struct platform_device xlr_net_dev0 = {
+		.name		= "xlr-net",
+		.id		= 0,
+		.dev.platform_data = &ndata0,
+	};
+	ndata0.mii_addr = ioremap(CPHYSADDR(
+		nlm_mmio_base(NETLOGIC_IO_GMAC_0_OFFSET)), 0xfff);
+
+	ndata0.cpu_mask = nlm_current_node()->coremask;
+
 	for (mac = 0; mac < MAX_NUM_XLR_GMAC; mac++) {
-		config_mac(&ndata[mac],
-			PHY_INTERFACE_MODE_RGMII,
-			0,
-			0,
-			FMN_STNID_GMACRFR_0,
-			FMN_STNID_GMAC0_TX0,
-			xlr_board_fmn_config.bucket_size,
-			&xlr_board_fmn_config.gmac[0],
-			mac);
-
-		net_device_init(mac, xlr_net_res[mac], xlr_gmac_offsets[mac],
+		ndata0.tx_stnid[mac] = FMN_STNID_GMAC0_TX0 + mac;
+		ndata0.phy_addr[mac] = mac;
+		xlr_resource_init(&xlr_net0_res[mac * 2], xlr_gmac_offsets[mac],
 				xlr_gmac_irqs[mac]);
-		platform_device_register(&xlr_net_dev[mac]);
 	}
+	xlr_net_dev0.num_resources = 8;
+	xlr_net_dev0.resource = xlr_net0_res;
+
+	platform_device_register(&xlr_net_dev0);
 }
 
 static int __init xlr_net_init(void)
 {
-	gmac0_addr = ioremap(CPHYSADDR(
-		nlm_mmio_base(NETLOGIC_IO_GMAC_0_OFFSET)), 0xfff);
-
 	if (nlm_chip_is_xls())
 		xls_gmac_init();
 	else
diff --git a/drivers/staging/netlogic/platform_net.h b/drivers/staging/netlogic/platform_net.h
index 29deeea72ca1..e1b27f649590 100644
--- a/drivers/staging/netlogic/platform_net.h
+++ b/drivers/staging/netlogic/platform_net.h
@@ -31,6 +31,9 @@
  * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
  * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
+
+#define PORTS_PER_CONTROLLER		4
+
 struct xlr_net_data {
 	int cpu_mask;
 	u32 __iomem *mii_addr;
@@ -39,8 +42,8 @@ struct xlr_net_data {
 	u32 __iomem *gpio_addr;
 	int phy_interface;
 	int rfr_station;
-	int tx_stnid;
+	int tx_stnid[PORTS_PER_CONTROLLER];
 	int *bucket_size;
-	int phy_addr;
+	int phy_addr[PORTS_PER_CONTROLLER];
 	struct xlr_fmn_info *gmac_fmn_info;
 };
diff --git a/drivers/staging/netlogic/xlr_net.c b/drivers/staging/netlogic/xlr_net.c
index 9bf407d6241a..ad19ba9df3c7 100644
--- a/drivers/staging/netlogic/xlr_net.c
+++ b/drivers/staging/netlogic/xlr_net.c
@@ -78,39 +78,7 @@ static inline void xlr_reg_update(u32 *base_addr,
 	xlr_nae_wreg(base_addr, off, (tmp & ~mask) | (val & mask));
 }
 
-/*
- * Table of net_device pointers indexed by port, this will be used to
- * lookup the net_device corresponding to a port by the message ring handler.
- *
- * Maximum ports in XLR/XLS is 8(8 GMAC on XLS, 4 GMAC + 2 XGMAC on XLR)
- */
-static struct net_device *mac_to_ndev[8];
-
-static inline struct sk_buff *mac_get_skb_back_ptr(void *addr)
-{
-	struct sk_buff **back_ptr;
-
-	/*
-	 * this function should be used only for newly allocated packets.
-	 * It assumes the first cacheline is for the back pointer related
-	 * book keeping info.
-	 */
-	back_ptr = (struct sk_buff **)(addr - MAC_SKB_BACK_PTR_SIZE);
-	return *back_ptr;
-}
-
-static inline void mac_put_skb_back_ptr(struct sk_buff *skb)
-{
-	struct sk_buff **back_ptr = (struct sk_buff **)skb->data;
-
-	/*
-	 * this function should be used only for newly allocated packets.
-	 * It assumes the first cacheline is for the back pointer related
-	 * book keeping info.
-	 */
-	skb_reserve(skb, MAC_SKB_BACK_PTR_SIZE);
-	*back_ptr = skb;
-}
+#define MAC_SKB_BACK_PTR_SIZE SMP_CACHE_BYTES
 
 static int send_to_rfr_fifo(struct xlr_net_priv *priv, void *addr)
 {
@@ -125,9 +93,9 @@ static int send_to_rfr_fifo(struct xlr_net_priv *priv, void *addr)
 	msg.msg3 = 0;
 	stnid = priv->nd->rfr_station;
 	do {
-		mflags = nlm_cop2_enable();
+		mflags = nlm_cop2_enable_irqsave();
 		ret = nlm_fmn_send(1, 0, stnid, &msg);
-		nlm_cop2_restore(mflags);
+		nlm_cop2_disable_irqrestore(mflags);
 		if (ret == 0)
 			return 0;
 	} while (++num_try < 10000);
@@ -136,41 +104,51 @@ static int send_to_rfr_fifo(struct xlr_net_priv *priv, void *addr)
 	return ret;
 }
 
-static inline struct sk_buff *xlr_alloc_skb(void)
+static inline unsigned char *xlr_alloc_skb(void)
 {
 	struct sk_buff *skb;
+	int buf_len = sizeof(struct sk_buff *);
+	unsigned char *skb_data;
 
 	/* skb->data is cache aligned */
 	skb = alloc_skb(XLR_RX_BUF_SIZE, GFP_ATOMIC);
-	if (!skb) {
-		pr_err("SKB allocation failed\n");
+	if (!skb)
 		return NULL;
-	}
-	mac_put_skb_back_ptr(skb);
-	return skb;
+	skb_data = skb->data;
+	skb_put(skb, MAC_SKB_BACK_PTR_SIZE);
+	skb_pull(skb, MAC_SKB_BACK_PTR_SIZE);
+	memcpy(skb_data, &skb, buf_len);
+
+	return skb->data;
 }
 
 static void xlr_net_fmn_handler(int bkt, int src_stnid, int size,
 		int code, struct nlm_fmn_msg *msg, void *arg)
 {
-	struct sk_buff *skb, *skb_new = NULL;
+	struct sk_buff *skb;
+	void *skb_data = NULL;
 	struct net_device *ndev;
 	struct xlr_net_priv *priv;
-	u64 length, port;
-	void *addr;
+	u32 port, length;
+	unsigned char *addr;
+	struct xlr_adapter *adapter = (struct xlr_adapter *) arg;
 
 	length = (msg->msg0 >> 40) & 0x3fff;
 	if (length == 0) {
 		addr = bus_to_virt(msg->msg0 & 0xffffffffffULL);
-		dev_kfree_skb_any(addr);
-	} else if (length) {
-		addr = bus_to_virt(msg->msg0 & 0xffffffffe0ULL);
+		addr = addr - MAC_SKB_BACK_PTR_SIZE;
+		skb = (struct sk_buff *) *(unsigned long *)addr;
+		dev_kfree_skb_any((struct sk_buff *)addr);
+	} else {
+		addr = (unsigned char *)
+			bus_to_virt(msg->msg0 & 0xffffffffe0ULL);
 		length = length - BYTE_OFFSET - MAC_CRC_LEN;
-		port = msg->msg0 & 0x0f;
-		if (src_stnid == FMN_STNID_GMAC1)
-			port = port + 4;
-		skb = mac_get_skb_back_ptr(addr);
-		skb->dev = mac_to_ndev[port];
+		port = ((int)msg->msg0) & 0x0f;
+		addr = addr - MAC_SKB_BACK_PTR_SIZE;
+		skb = (struct sk_buff *) *(unsigned long *)addr;
+		skb->dev = adapter->netdev[port];
+		if (skb->dev == NULL)
+			return;
 		ndev = skb->dev;
 		priv = netdev_priv(ndev);
 
@@ -181,13 +159,15 @@ static void xlr_net_fmn_handler(int bkt, int src_stnid, int size,
 		skb->dev->last_rx = jiffies;
 		netif_rx(skb);
 		/* Fill rx ring */
-		skb_new = xlr_alloc_skb();
-		if (skb_new)
-			send_to_rfr_fifo(priv, skb_new->data);
+		skb_data = xlr_alloc_skb();
+		if (skb_data)
+			send_to_rfr_fifo(priv, skb_data);
 	}
 }
 
-/* Ethtool operation */
+/*
+ * Ethtool operation
+ */
 static int xlr_get_settings(struct net_device *ndev, struct ethtool_cmd *ecmd)
 {
 	struct xlr_net_priv *priv = netdev_priv(ndev);
@@ -213,18 +193,22 @@ static struct ethtool_ops xlr_ethtool_ops = {
 	.set_settings = xlr_set_settings,
 };
 
-/* Net operations */
+/*
+ * Net operations
+ */
 static int xlr_net_fill_rx_ring(struct net_device *ndev)
 {
-	struct sk_buff *skb;
+	void *skb_data;
 	struct xlr_net_priv *priv = netdev_priv(ndev);
 	int i;
 
-	for (i = 0; i < MAX_FRIN_SPILL/2; i++) {
-		skb = xlr_alloc_skb();
-		if (!skb)
+	for (i = 0; i < MAX_FRIN_SPILL/4; i++) {
+		skb_data = xlr_alloc_skb();
+		if (!skb_data) {
+			pr_err("SKB allocation failed\n");
 			return -ENOMEM;
-		send_to_rfr_fifo(priv, skb->data);
+		}
+		send_to_rfr_fifo(priv, skb_data);
 	}
 	pr_info("Rx ring setup done\n");
 	return 0;
@@ -244,10 +228,11 @@ static int xlr_net_open(struct net_device *ndev)
 		pr_err("Autoneg failed\n");
 		return err;
 	}
-
 	/* Setup the speed from PHY to internal reg*/
 	xlr_set_gmac_speed(priv);
+
 	netif_tx_start_all_queues(ndev);
+
 	return 0;
 }
 
@@ -298,9 +283,9 @@ static netdev_tx_t xlr_net_start_xmit(struct sk_buff *skb,
 	u32 flags;
 
 	xlr_make_tx_desc(&msg, virt_to_phys(skb->data), skb);
-	flags = nlm_cop2_enable();
-	ret = nlm_fmn_send(2, 0, priv->nd->tx_stnid, &msg);
-	nlm_cop2_restore(flags);
+	flags = nlm_cop2_enable_irqsave();
+	ret = nlm_fmn_send(2, 0, priv->tx_stnid, &msg);
+	nlm_cop2_disable_irqrestore(flags);
 	if (ret)
 		dev_kfree_skb_any(skb);
 	return NETDEV_TX_OK;
@@ -432,7 +417,9 @@ static struct net_device_ops xlr_netdev_ops = {
 	.ndo_get_stats64 = xlr_get_stats64,
 };
 
-/* Gmac init */
+/*
+ * Gmac init
+ */
 static void *xlr_config_spill(struct xlr_net_priv *priv, int reg_start_0,
 		int reg_start_1, int reg_size, int size)
 {
@@ -538,13 +525,13 @@ static void xlr_config_pde(struct xlr_net_priv *priv)
  * Setup the Message ring credits, bucket size and other
  * common configuration
  */
-static void xlr_config_common(struct xlr_net_priv *priv)
+static int xlr_config_common(struct xlr_net_priv *priv)
 {
 	struct xlr_fmn_info *gmac = priv->nd->gmac_fmn_info;
 	int start_stn_id = gmac->start_stn_id;
 	int end_stn_id = gmac->end_stn_id;
 	int *bucket_size = priv->nd->bucket_size;
-	int i, j;
+	int i, j, err;
 
 	/* Setting non-core MsgBktSize(0x321 - 0x325) */
 	for (i = start_stn_id; i <= end_stn_id; i++) {
@@ -571,9 +558,12 @@ static void xlr_config_common(struct xlr_net_priv *priv)
 	xlr_nae_wreg(priv->base_addr, R_DMACR3, 0xffffffff);
 	xlr_nae_wreg(priv->base_addr, R_FREEQCARVE, 0);
 
-	xlr_net_fill_rx_ring(priv->ndev);
+	err = xlr_net_fill_rx_ring(priv->ndev);
+	if (err)
+		return err;
 	nlm_register_fmn_handler(start_stn_id, end_stn_id, xlr_net_fmn_handler,
-					NULL);
+			priv->adapter);
+	return 0;
 }
 
 static void xlr_config_translate_table(struct xlr_net_priv *priv)
@@ -703,7 +693,6 @@ static int xlr_phy_read(u32 *base_addr, int phy_addr, int regnum)
 	xlr_nae_wreg(base_addr, R_MII_MGMT_COMMAND,
 			(1 << O_MII_MGMT_COMMAND__rstat));
 
-
 	/* poll for the read cycle to complete */
 	while (!timedout) {
 		checktime = jiffies;
@@ -775,7 +764,7 @@ static void xlr_sgmii_init(struct xlr_net_priv *priv)
 	xlr_nae_wreg(priv->gpio_addr, 0x21, 0x7104);
 
 	/* enable autoneg - more magic */
-	phy = priv->port_id % 4 + 27;
+	phy = priv->phy_addr % 4 + 27;
 	xlr_phy_write(priv->pcs_addr, phy, 0, 0x1000);
 	xlr_phy_write(priv->pcs_addr, phy, 0, 0x0200);
 }
@@ -789,7 +778,6 @@ void xlr_set_gmac_speed(struct xlr_net_priv *priv)
 		xlr_sgmii_init(priv);
 
 	if (phydev->speed != priv->phy_speed) {
-		pr_info("change %d to %d\n", priv->phy_speed, phydev->speed);
 		speed = phydev->speed;
 		if (speed == SPEED_1000) {
 			/* Set interface to Byte mode */
@@ -831,12 +819,12 @@ static void xlr_gmac_link_adjust(struct net_device *ndev)
 	intreg = xlr_nae_rdreg(priv->base_addr, R_INTREG);
 	if (phydev->link) {
 		if (phydev->speed != priv->phy_speed) {
-			pr_info("gmac%d : Link up\n", priv->port_id);
 			xlr_set_gmac_speed(priv);
+			pr_info("gmac%d : Link up\n", priv->port_id);
 		}
 	} else {
-		pr_info("gmac%d : Link down\n", priv->port_id);
 		xlr_set_gmac_speed(priv);
+		pr_info("gmac%d : Link down\n", priv->port_id);
 	}
 }
 
@@ -876,7 +864,6 @@ static int xlr_setup_mdio(struct xlr_net_priv *priv,
 {
 	int err;
 
-	priv->phy_addr = priv->nd->phy_addr;
 	priv->mii_bus = mdiobus_alloc();
 	if (!priv->mii_bus) {
 		pr_err("mdiobus alloc failed\n");
@@ -896,6 +883,7 @@ static int xlr_setup_mdio(struct xlr_net_priv *priv,
 		mdiobus_free(priv->mii_bus);
 		return -ENOMEM;
 	}
+
 	priv->mii_bus->irq[priv->phy_addr] = priv->ndev->irq;
 
 	/* Scan only the enabled address */
@@ -966,7 +954,9 @@ static void xlr_port_disable(struct xlr_net_priv *priv)
 		1 << O_RX_CONTROL__RxEnable, 0);
 }
 
-/* Initialization of gmac */
+/*
+ * Initialization of gmac
+ */
 static int xlr_gmac_init(struct xlr_net_priv *priv,
 		struct platform_device *pdev)
 {
@@ -975,6 +965,7 @@ static int xlr_gmac_init(struct xlr_net_priv *priv,
 	pr_info("Initializing the gmac%d\n", priv->port_id);
 
 	xlr_port_disable(priv);
+
 	xlr_nae_wreg(priv->base_addr, R_DESC_PACK_CTRL,
 			(1 << O_DESC_PACK_CTRL__MaxEntry)
 			| (BYTE_OFFSET << O_DESC_PACK_CTRL__ByteOffset)
@@ -1003,8 +994,8 @@ static int xlr_gmac_init(struct xlr_net_priv *priv,
 	/* Clear all stats */
 	xlr_reg_update(priv->base_addr, R_STATCTRL,
 		0, 1 << O_STATCTRL__ClrCnt);
-	xlr_reg_update(priv->base_addr, R_STATCTRL,
-		1 << O_STATCTRL__ClrCnt, 1 << O_STATCTRL__ClrCnt);
+	xlr_reg_update(priv->base_addr, R_STATCTRL, 1 << 2,
+		1 << 2);
 	return 0;
 }
 
@@ -1013,85 +1004,110 @@ static int xlr_net_probe(struct platform_device *pdev)
 	struct xlr_net_priv *priv = NULL;
 	struct net_device *ndev;
 	struct resource *res;
-	int mac, err;
+	struct xlr_adapter *adapter;
+	int err, port;
 
-	mac = pdev->id;
-	ndev = alloc_etherdev_mq(sizeof(struct xlr_net_priv), 32);
-	if (!ndev) {
-		pr_err("Allocation of Ethernet device failed\n");
-		return -ENOMEM;
+	pr_info("XLR/XLS Ethernet Driver controller %d\n", pdev->id);
+	/*
+	 * Allocate our adapter data structure and attach it to the device.
+	 */
+	adapter = (struct xlr_adapter *)
+		devm_kzalloc(&pdev->dev, sizeof(adapter), GFP_KERNEL);
+	if (!adapter) {
+		err = -ENOMEM;
+		return err;
 	}
 
-	priv = netdev_priv(ndev);
-	priv->pdev = pdev;
-	priv->ndev = ndev;
-	priv->port_id = mac;
-	priv->nd = (struct xlr_net_data *)pdev->dev.platform_data;
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	if (res == NULL) {
-		pr_err("No memory resource for MAC %d\n", mac);
-		err = -ENODEV;
-		goto err_gmac;
-	}
+	/*
+	 * XLR and XLS have 1 and 2 NAE controller respectively
+	 * Each controller has 4 gmac ports, mapping each controller
+	 * under one parent device, 4 gmac ports under one device.
+	 */
+	for (port = 0; port < pdev->num_resources/2; port++) {
+		ndev = alloc_etherdev_mq(sizeof(struct xlr_net_priv), 32);
+		if (!ndev) {
+			pr_err("Allocation of Ethernet device failed\n");
+			return -ENOMEM;
+		}
 
-	ndev->base_addr = (unsigned long) devm_ioremap_resource
-		(&pdev->dev, res);
-	if (IS_ERR_VALUE(ndev->base_addr)) {
-		err = ndev->base_addr;
-		goto err_gmac;
-	}
+		priv = netdev_priv(ndev);
+		priv->pdev = pdev;
+		priv->ndev = ndev;
+		priv->port_id = (pdev->id * 4) + port;
+		priv->nd = (struct xlr_net_data *)pdev->dev.platform_data;
+		res = platform_get_resource(pdev, IORESOURCE_MEM, port);
+
+		if (res == NULL) {
+			pr_err("No memory resource for MAC %d\n",
+					priv->port_id);
+			err = -ENODEV;
+			goto err_gmac;
+		}
+		priv->base_addr = devm_ioremap_resource(&pdev->dev, res);
+		if (IS_ERR(priv->base_addr)) {
+			err = PTR_ERR(priv->base_addr);
+			goto err_gmac;
+		}
+		priv->adapter = adapter;
+		adapter->netdev[port] = ndev;
 
-	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (res == NULL) {
-		pr_err("No irq resource for MAC %d\n", mac);
-		err = -ENODEV;
-		goto err_gmac;
-	}
-	ndev->irq = res->start;
+		res = platform_get_resource(pdev, IORESOURCE_IRQ, port);
+		if (res == NULL) {
+			pr_err("No irq resource for MAC %d\n", priv->port_id);
+			err = -ENODEV;
+			goto err_gmac;
+		}
 
-	priv->mii_addr = priv->nd->mii_addr;
-	priv->serdes_addr = priv->nd->serdes_addr;
-	priv->pcs_addr = priv->nd->pcs_addr;
-	priv->gpio_addr = priv->nd->gpio_addr;
-	priv->base_addr = (u32 *) ndev->base_addr;
+		ndev->irq = res->start;
 
-	mac_to_ndev[mac] = ndev;
-	ndev->netdev_ops = &xlr_netdev_ops;
-	ndev->watchdog_timeo = HZ;
+		priv->phy_addr = priv->nd->phy_addr[port];
+		priv->tx_stnid = priv->nd->tx_stnid[port];
+		priv->mii_addr = priv->nd->mii_addr;
+		priv->serdes_addr = priv->nd->serdes_addr;
+		priv->pcs_addr = priv->nd->pcs_addr;
+		priv->gpio_addr = priv->nd->gpio_addr;
 
-	/* Setup Mac address and Rx mode */
-	eth_hw_addr_random(ndev);
-	xlr_hw_set_mac_addr(ndev);
-	xlr_set_rx_mode(ndev);
+		ndev->netdev_ops = &xlr_netdev_ops;
+		ndev->watchdog_timeo = HZ;
 
-	priv->num_rx_desc += MAX_NUM_DESC_SPILL;
-	ndev->ethtool_ops = &xlr_ethtool_ops;
-	SET_NETDEV_DEV(ndev, &pdev->dev);
+		/* Setup Mac address and Rx mode */
+		eth_hw_addr_random(ndev);
+		xlr_hw_set_mac_addr(ndev);
+		xlr_set_rx_mode(ndev);
+
+		priv->num_rx_desc += MAX_NUM_DESC_SPILL;
+		ndev->ethtool_ops = &xlr_ethtool_ops;
+		SET_NETDEV_DEV(ndev, &pdev->dev);
 
-	/* Common registers, do one time initialization */
-	if (mac == 0 || mac == 4) {
 		xlr_config_fifo_spill_area(priv);
 		/* Configure PDE to Round-Robin pkt distribution */
 		xlr_config_pde(priv);
 		xlr_config_parser(priv);
-	}
-	/* Call init with respect to port */
-	if (strcmp(res->name, "gmac") == 0) {
-		err = xlr_gmac_init(priv, pdev);
+
+		/* Call init with respect to port */
+		if (strcmp(res->name, "gmac") == 0) {
+			err = xlr_gmac_init(priv, pdev);
+			if (err) {
+				pr_err("gmac%d init failed\n", priv->port_id);
+				goto err_gmac;
+			}
+		}
+
+		if (priv->port_id == 0 || priv->port_id == 4) {
+			err = xlr_config_common(priv);
+			if (err)
+				goto err_netdev;
+		}
+
+		err = register_netdev(ndev);
 		if (err) {
-			pr_err("gmac%d init failed\n", mac);
-			goto err_gmac;
+			pr_err("Registering netdev failed for gmac%d\n",
+					priv->port_id);
+			goto err_netdev;
 		}
+		platform_set_drvdata(pdev, priv);
 	}
 
-	if (mac == 0 || mac == 4)
-		xlr_config_common(priv);
-
-	err = register_netdev(ndev);
-	if (err)
-		goto err_netdev;
-	platform_set_drvdata(pdev, priv);
 	return 0;
 
 err_netdev:
@@ -1104,6 +1120,7 @@ err_gmac:
 static int xlr_net_remove(struct platform_device *pdev)
 {
 	struct xlr_net_priv *priv = platform_get_drvdata(pdev);
+
 	unregister_netdev(priv->ndev);
 	mdiobus_unregister(priv->mii_bus);
 	mdiobus_free(priv->mii_bus);
diff --git a/drivers/staging/netlogic/xlr_net.h b/drivers/staging/netlogic/xlr_net.h
index cea796633711..13e03f0a0a46 100644
--- a/drivers/staging/netlogic/xlr_net.h
+++ b/drivers/staging/netlogic/xlr_net.h
@@ -1069,14 +1069,20 @@ enum tsv_rsv_reg {
 	CARRY_REG_2 = 0x4d,
 };
 
+struct xlr_adapter {
+	struct net_device *netdev[4];
+};
+
 struct xlr_net_priv {
 	u32 __iomem *base_addr;
 	struct net_device *ndev;
+	struct xlr_adapter *adapter;
 	struct mii_bus *mii_bus;
 	int num_rx_desc;
 	int phy_addr;	/* PHY addr on MDIO bus */
 	int pcs_id;	/* PCS id on MDIO bus */
 	int port_id;	/* Port(gmac/xgmac) number, i.e 0-7 */
+	int tx_stnid;
 	u32 __iomem *mii_addr;
 	u32 __iomem *serdes_addr;
 	u32 __iomem *pcs_addr;
@@ -1096,4 +1102,4 @@ struct xlr_net_priv {
 	u64 *class_3_spill;
 };
 
-void xlr_set_gmac_speed(struct xlr_net_priv *priv);
+extern void xlr_set_gmac_speed(struct xlr_net_priv *priv);
diff --git a/drivers/staging/nokia_h4p/Kconfig b/drivers/staging/nokia_h4p/Kconfig
deleted file mode 100644
index 4336c0ad065b..000000000000
--- a/drivers/staging/nokia_h4p/Kconfig
+++ /dev/null
@@ -1,9 +0,0 @@
-config BT_NOKIA_H4P
-	tristate "HCI driver with H4 Nokia extensions"
-	depends on BT && ARCH_OMAP
-	help
-	  Bluetooth HCI driver with H4 extensions.  This driver provides
-	  support for H4+ Bluetooth chip with vendor-specific H4 extensions.
-
-	  Say Y here to compile support for h4 extended devices into the kernel
-	  or say M to compile it as module (btnokia_h4p).
diff --git a/drivers/staging/nokia_h4p/Makefile b/drivers/staging/nokia_h4p/Makefile
deleted file mode 100644
index 9625db4a9af3..000000000000
--- a/drivers/staging/nokia_h4p/Makefile
+++ /dev/null
@@ -1,6 +0,0 @@
-
-obj-$(CONFIG_BT_NOKIA_H4P)		+= btnokia_h4p.o
-btnokia_h4p-objs := nokia_core.o nokia_fw.o nokia_uart.o nokia_fw-csr.o \
-		nokia_fw-bcm.o nokia_fw-ti1273.o
-
-ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/nokia_h4p/TODO b/drivers/staging/nokia_h4p/TODO
deleted file mode 100644
index 0ec5823e0ca8..000000000000
--- a/drivers/staging/nokia_h4p/TODO
+++ /dev/null
@@ -1,132 +0,0 @@
-Few attempts to submission have been made, last review comments were received in
-
-Date: Wed, 15 Jan 2014 19:01:51 -0800
-From: Marcel Holtmann <marcel@holtmann.org>
-Subject: Re: [PATCH v6] Bluetooth: Add hci_h4p driver
-
-Some code refactoring is still needed.
-
-TODO:
-
-> +++ b/drivers/bluetooth/hci_h4p.h
-
-can we please get the naming straight. File names do not start with
-hci_ anymore. We moved away from it since that term is too generic.
-
-> +struct hci_h4p_info {
-
-Can we please get rid of the hci_ prefix for everything. Copying from
-drivers that are over 10 years old is not a good idea. Please look at
-recent ones.
-
-> +     struct timer_list lazy_release;
-
-Timer? Not delayed work?
-
-> +void hci_h4p_outb(struct hci_h4p_info *info, unsigned int offset, u8 val);
-> +u8 hci_h4p_inb(struct hci_h4p_info *info, unsigned int offset);
-> +void hci_h4p_set_rts(struct hci_h4p_info *info, int active);
-> +int hci_h4p_wait_for_cts(struct hci_h4p_info *info, int active, int timeout_ms);
-> +void __hci_h4p_set_auto_ctsrts(struct hci_h4p_info *info, int on, u8 which);
-> +void hci_h4p_set_auto_ctsrts(struct hci_h4p_info *info, int on, u8 which);
-> +void hci_h4p_change_speed(struct hci_h4p_info *info, unsigned long speed);
-> +int hci_h4p_reset_uart(struct hci_h4p_info *info);
-> +void hci_h4p_init_uart(struct hci_h4p_info *info);
-> +void hci_h4p_enable_tx(struct hci_h4p_info *info);
-> +void hci_h4p_store_regs(struct hci_h4p_info *info);
-> +void hci_h4p_restore_regs(struct hci_h4p_info *info);
-> +void hci_h4p_smart_idle(struct hci_h4p_info *info, bool enable);
-
-These are a lot of public functions. Are they all really needed or can
-the code be done smart.
-
-> +static ssize_t hci_h4p_store_bdaddr(struct device *dev,
-> +                                 struct device_attribute *attr,
-> +                                 const char *buf, size_t count)
-> +{
-> +     struct hci_h4p_info *info = dev_get_drvdata(dev);
-
-Since none of these devices can function without having a valid
-address, the way this should work is that we should not register the
-HCI device when probing the platform device.
-    
-The HCI device should be registered once a valid address has been
-written into the sysfs file. I do not want to play the tricks with
-bringing up the device without a valid address.
-
-> +     hdev->close = hci_h4p_hci_close;
-> +     hdev->flush = hci_h4p_hci_flush;
-> +     hdev->send = hci_h4p_hci_send_frame;
-    
-It needs to use hdev->setup to load the firmware. I assume the
-firmware only needs to be loaded once. That is exactly what
-hdev->setup does. It gets executed once.
-    
-> +     set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
-
-Is this quirk really needed? Normally only Bluetooth 1.1 and early
-devices qualify for it.
-
-> +static int hci_h4p_bcm_set_bdaddr(struct hci_h4p_info *info, struct sk_buff *skb)
-> +{
-> +     int i;
-> +     static const u8 nokia_oui[3] = {0x00, 0x1f, 0xdf};
-> +     int not_valid;
-
-Has this actually been confirmed that we can just randomly set an
-address out of the Nokia range. I do not think so. This is a pretty
-bad idea.
-
-I have no interest in merging a driver with such a hack.
-
-> +     not_valid = 1;
-> +     for (i = 0; i < 6; i++) {
-> +             if (info->bd_addr[i] != 0x00) {
-> +                     not_valid = 0;
-> +                     break;
-> +             }   
-> +     }
-
-Anybody every heard of memcmp or bacmp and BDADDR_ANY?
-
-> +             if (not_valid) {
-> +                     dev_info(info->dev, "Valid bluetooth address not found,"
-> +                                     " setting some random\n");
-> +                     /* When address is not valid, use some random */
-> +                     memcpy(info->bd_addr, nokia_oui, 3);
-> +                     get_random_bytes(info->bd_addr + 3, 3);
-> +             }
-
-
-And why does every single chip firmware does this differently. Seriously, this is a mess.
-
-> +void hci_h4p_parse_fw_event(struct hci_h4p_info *info, struct sk_buff *skb)
-> +{
-> +     switch (info->man_id) {
-> +     case H4P_ID_CSR:
-> +             hci_h4p_bc4_parse_fw_event(info, skb);
-> +             break;
-...
-> +}
-
-We have proper HCI sync command handling in recent kernels. I really
-do not know why this is hand coded these days. Check how the Intel
-firmware loading inside btusb.c does it.
-
-> +inline u8 hci_h4p_inb(struct hci_h4p_info *info, unsigned int offset)
-> +{ 
-> +     return __raw_readb(info->uart_base + (offset << 2));
-> +}
-
-Inline in a *.c file for a non-static function. Makes no sense to me.
-
-> +/**
-> + * struct hci_h4p_platform data - hci_h4p Platform data structure
-> + */
-> +struct hci_h4p_platform_data {
-
-please have a proper name here. For example
-btnokia_h4p_platform_data.
-
-Please send patches to Greg Kroah-Hartman <greg@kroah.com> and Cc:
-Pavel Machek <pavel@ucw.cz>
diff --git a/drivers/staging/nokia_h4p/hci_h4p.h b/drivers/staging/nokia_h4p/hci_h4p.h
deleted file mode 100644
index 99c4da61a56c..000000000000
--- a/drivers/staging/nokia_h4p/hci_h4p.h
+++ /dev/null
@@ -1,222 +0,0 @@
-/*
- * This file is part of Nokia H4P bluetooth driver
- *
- * Copyright (C) 2005-2008 Nokia Corporation.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#ifndef __DRIVERS_BLUETOOTH_HCI_H4P_H
-#define __DRIVERS_BLUETOOTH_HCI_H4P_H
-
-#include <net/bluetooth/bluetooth.h>
-#include <net/bluetooth/hci_core.h>
-#include <net/bluetooth/hci.h>
-
-#define UART_SYSC_OMAP_RESET	0x03
-#define UART_SYSS_RESETDONE	0x01
-#define UART_OMAP_SCR_EMPTY_THR	0x08
-#define UART_OMAP_SCR_WAKEUP	0x10
-#define UART_OMAP_SSR_WAKEUP	0x02
-#define UART_OMAP_SSR_TXFULL	0x01
-
-#define UART_OMAP_SYSC_IDLEMODE		0x03
-#define UART_OMAP_SYSC_IDLEMASK		(3 << UART_OMAP_SYSC_IDLEMODE)
-
-#define UART_OMAP_SYSC_FORCE_IDLE	(0 << UART_OMAP_SYSC_IDLEMODE)
-#define UART_OMAP_SYSC_NO_IDLE		(1 << UART_OMAP_SYSC_IDLEMODE)
-#define UART_OMAP_SYSC_SMART_IDLE	(2 << UART_OMAP_SYSC_IDLEMODE)
-
-#define H4P_TRANSFER_MODE		1
-#define H4P_SCHED_TRANSFER_MODE		2
-#define H4P_ACTIVE_MODE			3
-
-struct hci_h4p_info {
-	struct timer_list lazy_release;
-	struct hci_dev *hdev;
-	spinlock_t lock;
-
-	void __iomem *uart_base;
-	unsigned long uart_phys_base;
-	int irq;
-	struct device *dev;
-	u8 chip_type;
-	u8 bt_wakeup_gpio;
-	u8 host_wakeup_gpio;
-	u8 reset_gpio;
-	u8 reset_gpio_shared;
-	u8 bt_sysclk;
-	u8 man_id;
-	u8 ver_id;
-
-	struct sk_buff_head fw_queue;
-	struct sk_buff *alive_cmd_skb;
-	struct completion init_completion;
-	struct completion fw_completion;
-	struct completion test_completion;
-	int fw_error;
-	int init_error;
-
-	struct sk_buff_head txq;
-
-	struct sk_buff *rx_skb;
-	long rx_count;
-	unsigned long rx_state;
-	unsigned long garbage_bytes;
-
-	u8 bd_addr[6];
-	struct sk_buff_head *fw_q;
-
-	int pm_enabled;
-	int tx_enabled;
-	int autorts;
-	int rx_enabled;
-	unsigned long pm_flags;
-
-	int tx_clocks_en;
-	int rx_clocks_en;
-	spinlock_t clocks_lock;
-	struct clk *uart_iclk;
-	struct clk *uart_fclk;
-	atomic_t clk_users;
-	u16 dll;
-	u16 dlh;
-	u16 ier;
-	u16 mdr1;
-	u16 efr;
-};
-
-struct hci_h4p_radio_hdr {
-	__u8 evt;
-	__u8 dlen;
-} __packed;
-
-struct hci_h4p_neg_hdr {
-	__u8 dlen;
-} __packed;
-#define H4P_NEG_HDR_SIZE 1
-
-#define H4P_NEG_REQ	0x00
-#define H4P_NEG_ACK	0x20
-#define H4P_NEG_NAK	0x40
-
-#define H4P_PROTO_PKT	0x44
-#define H4P_PROTO_BYTE	0x4c
-
-#define H4P_ID_CSR	0x02
-#define H4P_ID_BCM2048	0x04
-#define H4P_ID_TI1271	0x31
-
-struct hci_h4p_neg_cmd {
-	__u8	ack;
-	__u16	baud;
-	__u16	unused1;
-	__u8	proto;
-	__u16	sys_clk;
-	__u16	unused2;
-} __packed;
-
-struct hci_h4p_neg_evt {
-	__u8	ack;
-	__u16	baud;
-	__u16	unused1;
-	__u8	proto;
-	__u16	sys_clk;
-	__u16	unused2;
-	__u8	man_id;
-	__u8	ver_id;
-} __packed;
-
-#define H4P_ALIVE_REQ	0x55
-#define H4P_ALIVE_RESP	0xcc
-
-struct hci_h4p_alive_hdr {
-	__u8	dlen;
-} __packed;
-#define H4P_ALIVE_HDR_SIZE 1
-
-struct hci_h4p_alive_pkt {
-	__u8	mid;
-	__u8	unused;
-} __packed;
-
-#define MAX_BAUD_RATE		921600
-#define BC4_MAX_BAUD_RATE	3692300
-#define UART_CLOCK		48000000
-#define BT_INIT_DIVIDER		320
-#define BT_BAUDRATE_DIVIDER	384000000
-#define BT_SYSCLK_DIV		1000
-#define INIT_SPEED		120000
-
-#define H4_TYPE_SIZE		1
-#define H4_RADIO_HDR_SIZE	2
-
-/* H4+ packet types */
-#define H4_CMD_PKT		0x01
-#define H4_ACL_PKT		0x02
-#define H4_SCO_PKT		0x03
-#define H4_EVT_PKT		0x04
-#define H4_NEG_PKT		0x06
-#define H4_ALIVE_PKT		0x07
-#define H4_RADIO_PKT		0x08
-
-/* TX states */
-#define WAIT_FOR_PKT_TYPE	1
-#define WAIT_FOR_HEADER		2
-#define WAIT_FOR_DATA		3
-
-struct hci_fw_event {
-	struct hci_event_hdr hev;
-	struct hci_ev_cmd_complete cmd;
-	u8 status;
-} __packed;
-
-int hci_h4p_send_alive_packet(struct hci_h4p_info *info);
-
-void hci_h4p_bcm_parse_fw_event(struct hci_h4p_info *info,
-				struct sk_buff *skb);
-int hci_h4p_bcm_send_fw(struct hci_h4p_info *info,
-			struct sk_buff_head *fw_queue);
-
-void hci_h4p_bc4_parse_fw_event(struct hci_h4p_info *info,
-				struct sk_buff *skb);
-int hci_h4p_bc4_send_fw(struct hci_h4p_info *info,
-			struct sk_buff_head *fw_queue);
-
-void hci_h4p_ti1273_parse_fw_event(struct hci_h4p_info *info,
-				    struct sk_buff *skb);
-int hci_h4p_ti1273_send_fw(struct hci_h4p_info *info,
-			    struct sk_buff_head *fw_queue);
-
-int hci_h4p_read_fw(struct hci_h4p_info *info, struct sk_buff_head *fw_queue);
-int hci_h4p_send_fw(struct hci_h4p_info *info, struct sk_buff_head *fw_queue);
-void hci_h4p_parse_fw_event(struct hci_h4p_info *info, struct sk_buff *skb);
-
-void hci_h4p_outb(struct hci_h4p_info *info, unsigned int offset, u8 val);
-u8 hci_h4p_inb(struct hci_h4p_info *info, unsigned int offset);
-void hci_h4p_set_rts(struct hci_h4p_info *info, int active);
-int hci_h4p_wait_for_cts(struct hci_h4p_info *info, int active, int timeout_ms);
-void __hci_h4p_set_auto_ctsrts(struct hci_h4p_info *info, int on, u8 which);
-void hci_h4p_set_auto_ctsrts(struct hci_h4p_info *info, int on, u8 which);
-void hci_h4p_change_speed(struct hci_h4p_info *info, unsigned long speed);
-int hci_h4p_reset_uart(struct hci_h4p_info *info);
-void hci_h4p_init_uart(struct hci_h4p_info *info);
-void hci_h4p_enable_tx(struct hci_h4p_info *info);
-void hci_h4p_store_regs(struct hci_h4p_info *info);
-void hci_h4p_restore_regs(struct hci_h4p_info *info);
-void hci_h4p_smart_idle(struct hci_h4p_info *info, bool enable);
-
-#endif /* __DRIVERS_BLUETOOTH_HCI_H4P_H */
diff --git a/drivers/staging/nokia_h4p/nokia_core.c b/drivers/staging/nokia_h4p/nokia_core.c
deleted file mode 100644
index 775e1d043230..000000000000
--- a/drivers/staging/nokia_h4p/nokia_core.c
+++ /dev/null
@@ -1,1207 +0,0 @@
-/*
- * This file is part of Nokia H4P bluetooth driver
- *
- * Copyright (C) 2005-2008 Nokia Corporation.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- * Thanks to all the Nokia people that helped with this driver,
- * including Ville Tervo and Roger Quadros.
- *
- * Power saving functionality was removed from this driver to make
- * merging easier.
- */
-
-#include <linux/module.h>
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/errno.h>
-#include <linux/delay.h>
-#include <linux/spinlock.h>
-#include <linux/serial_reg.h>
-#include <linux/skbuff.h>
-#include <linux/device.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/interrupt.h>
-#include <linux/gpio.h>
-#include <linux/timer.h>
-#include <linux/kthread.h>
-#include <linux/io.h>
-#include <linux/completion.h>
-#include <linux/sizes.h>
-
-#include <net/bluetooth/bluetooth.h>
-#include <net/bluetooth/hci_core.h>
-#include <net/bluetooth/hci.h>
-
-#include <linux/platform_data/bt-nokia-h4p.h>
-
-#include "hci_h4p.h"
-
-/* This should be used in function that cannot release clocks */
-static void hci_h4p_set_clk(struct hci_h4p_info *info, int *clock, int enable)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&info->clocks_lock, flags);
-	if (enable && !*clock) {
-		BT_DBG("Enabling %p", clock);
-		clk_prepare_enable(info->uart_fclk);
-		clk_prepare_enable(info->uart_iclk);
-		if (atomic_read(&info->clk_users) == 0)
-			hci_h4p_restore_regs(info);
-		atomic_inc(&info->clk_users);
-	}
-
-	if (!enable && *clock) {
-		BT_DBG("Disabling %p", clock);
-		if (atomic_dec_and_test(&info->clk_users))
-			hci_h4p_store_regs(info);
-		clk_disable_unprepare(info->uart_fclk);
-		clk_disable_unprepare(info->uart_iclk);
-	}
-
-	*clock = enable;
-	spin_unlock_irqrestore(&info->clocks_lock, flags);
-}
-
-static void hci_h4p_lazy_clock_release(unsigned long data)
-{
-	struct hci_h4p_info *info = (struct hci_h4p_info *)data;
-	unsigned long flags;
-
-	spin_lock_irqsave(&info->lock, flags);
-	if (!info->tx_enabled)
-		hci_h4p_set_clk(info, &info->tx_clocks_en, 0);
-	spin_unlock_irqrestore(&info->lock, flags);
-}
-
-/* Power management functions */
-void hci_h4p_smart_idle(struct hci_h4p_info *info, bool enable)
-{
-	u8 v;
-
-	v = hci_h4p_inb(info, UART_OMAP_SYSC);
-	v &= ~(UART_OMAP_SYSC_IDLEMASK);
-
-	if (enable)
-		v |= UART_OMAP_SYSC_SMART_IDLE;
-	else
-		v |= UART_OMAP_SYSC_NO_IDLE;
-
-	hci_h4p_outb(info, UART_OMAP_SYSC, v);
-}
-
-static inline void h4p_schedule_pm(struct hci_h4p_info *info)
-{
-}
-
-static void hci_h4p_disable_tx(struct hci_h4p_info *info)
-{
-	if (!info->pm_enabled)
-		return;
-
-	/* Re-enable smart-idle */
-	hci_h4p_smart_idle(info, 1);
-
-	gpio_set_value(info->bt_wakeup_gpio, 0);
-	mod_timer(&info->lazy_release, jiffies + msecs_to_jiffies(100));
-	info->tx_enabled = 0;
-}
-
-void hci_h4p_enable_tx(struct hci_h4p_info *info)
-{
-	unsigned long flags;
-
-	if (!info->pm_enabled)
-		return;
-
-	h4p_schedule_pm(info);
-
-	spin_lock_irqsave(&info->lock, flags);
-	del_timer(&info->lazy_release);
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 1);
-	info->tx_enabled = 1;
-	gpio_set_value(info->bt_wakeup_gpio, 1);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-		     UART_IER_THRI);
-	/*
-	 * Disable smart-idle as UART TX interrupts
-	 * are not wake-up capable
-	 */
-	hci_h4p_smart_idle(info, 0);
-
-	spin_unlock_irqrestore(&info->lock, flags);
-}
-
-static void hci_h4p_disable_rx(struct hci_h4p_info *info)
-{
-	if (!info->pm_enabled)
-		return;
-
-	info->rx_enabled = 0;
-
-	if (hci_h4p_inb(info, UART_LSR) & UART_LSR_DR)
-		return;
-
-	if (!(hci_h4p_inb(info, UART_LSR) & UART_LSR_TEMT))
-		return;
-
-	__hci_h4p_set_auto_ctsrts(info, 0, UART_EFR_RTS);
-	info->autorts = 0;
-	hci_h4p_set_clk(info, &info->rx_clocks_en, 0);
-}
-
-static void hci_h4p_enable_rx(struct hci_h4p_info *info)
-{
-	if (!info->pm_enabled)
-		return;
-
-	h4p_schedule_pm(info);
-
-	hci_h4p_set_clk(info, &info->rx_clocks_en, 1);
-	info->rx_enabled = 1;
-
-	if (!(hci_h4p_inb(info, UART_LSR) & UART_LSR_TEMT))
-		return;
-
-	__hci_h4p_set_auto_ctsrts(info, 1, UART_EFR_RTS);
-	info->autorts = 1;
-}
-
-/* Negotiation functions */
-int hci_h4p_send_alive_packet(struct hci_h4p_info *info)
-{
-	struct hci_h4p_alive_hdr *hdr;
-	struct hci_h4p_alive_pkt *pkt;
-	struct sk_buff *skb;
-	unsigned long flags;
-	int len;
-
-	BT_DBG("Sending alive packet");
-
-	len = H4_TYPE_SIZE + sizeof(*hdr) + sizeof(*pkt);
-	skb = bt_skb_alloc(len, GFP_KERNEL);
-	if (!skb)
-		return -ENOMEM;
-
-	memset(skb->data, 0x00, len);
-	*skb_put(skb, 1) = H4_ALIVE_PKT;
-	hdr = (struct hci_h4p_alive_hdr *)skb_put(skb, sizeof(*hdr));
-	hdr->dlen = sizeof(*pkt);
-	pkt = (struct hci_h4p_alive_pkt *)skb_put(skb, sizeof(*pkt));
-	pkt->mid = H4P_ALIVE_REQ;
-
-	skb_queue_tail(&info->txq, skb);
-	spin_lock_irqsave(&info->lock, flags);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-		     UART_IER_THRI);
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	BT_DBG("Alive packet sent");
-
-	return 0;
-}
-
-static void hci_h4p_alive_packet(struct hci_h4p_info *info,
-				 struct sk_buff *skb)
-{
-	struct hci_h4p_alive_hdr *hdr;
-	struct hci_h4p_alive_pkt *pkt;
-
-	BT_DBG("Received alive packet");
-	hdr = (struct hci_h4p_alive_hdr *)skb->data;
-	if (hdr->dlen != sizeof(*pkt)) {
-		dev_err(info->dev, "Corrupted alive message\n");
-		info->init_error = -EIO;
-		goto finish_alive;
-	}
-
-	pkt = (struct hci_h4p_alive_pkt *)skb_pull(skb, sizeof(*hdr));
-	if (pkt->mid != H4P_ALIVE_RESP) {
-		dev_err(info->dev, "Could not negotiate hci_h4p settings\n");
-		info->init_error = -EINVAL;
-	}
-
-finish_alive:
-	complete(&info->init_completion);
-	kfree_skb(skb);
-}
-
-static int hci_h4p_send_negotiation(struct hci_h4p_info *info)
-{
-	struct hci_h4p_neg_cmd *neg_cmd;
-	struct hci_h4p_neg_hdr *neg_hdr;
-	struct sk_buff *skb;
-	unsigned long flags;
-	int err, len;
-	u16 sysclk;
-
-	BT_DBG("Sending negotiation..");
-
-	switch (info->bt_sysclk) {
-	case 1:
-		sysclk = 12000;
-		break;
-	case 2:
-		sysclk = 38400;
-		break;
-	default:
-		return -EINVAL;
-	}
-
-	len = sizeof(*neg_cmd) + sizeof(*neg_hdr) + H4_TYPE_SIZE;
-	skb = bt_skb_alloc(len, GFP_KERNEL);
-	if (!skb)
-		return -ENOMEM;
-
-	memset(skb->data, 0x00, len);
-	*skb_put(skb, 1) = H4_NEG_PKT;
-	neg_hdr = (struct hci_h4p_neg_hdr *)skb_put(skb, sizeof(*neg_hdr));
-	neg_cmd = (struct hci_h4p_neg_cmd *)skb_put(skb, sizeof(*neg_cmd));
-
-	neg_hdr->dlen = sizeof(*neg_cmd);
-	neg_cmd->ack = H4P_NEG_REQ;
-	neg_cmd->baud = cpu_to_le16(BT_BAUDRATE_DIVIDER/MAX_BAUD_RATE);
-	neg_cmd->proto = H4P_PROTO_BYTE;
-	neg_cmd->sys_clk = cpu_to_le16(sysclk);
-
-	hci_h4p_change_speed(info, INIT_SPEED);
-
-	hci_h4p_set_rts(info, 1);
-	info->init_error = 0;
-	init_completion(&info->init_completion);
-	skb_queue_tail(&info->txq, skb);
-	spin_lock_irqsave(&info->lock, flags);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-		     UART_IER_THRI);
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	if (!wait_for_completion_interruptible_timeout(&info->init_completion,
-				msecs_to_jiffies(1000)))
-		return -ETIMEDOUT;
-
-	if (info->init_error < 0)
-		return info->init_error;
-
-	/* Change to operational settings */
-	hci_h4p_set_auto_ctsrts(info, 0, UART_EFR_RTS);
-	hci_h4p_set_rts(info, 0);
-	hci_h4p_change_speed(info, MAX_BAUD_RATE);
-
-	err = hci_h4p_wait_for_cts(info, 1, 100);
-	if (err < 0)
-		return err;
-
-	hci_h4p_set_auto_ctsrts(info, 1, UART_EFR_RTS);
-	init_completion(&info->init_completion);
-	err = hci_h4p_send_alive_packet(info);
-
-	if (err < 0)
-		return err;
-
-	if (!wait_for_completion_interruptible_timeout(&info->init_completion,
-				msecs_to_jiffies(1000)))
-		return -ETIMEDOUT;
-
-	if (info->init_error < 0)
-		return info->init_error;
-
-	BT_DBG("Negotiation successful");
-	return 0;
-}
-
-static void hci_h4p_negotiation_packet(struct hci_h4p_info *info,
-				       struct sk_buff *skb)
-{
-	struct hci_h4p_neg_hdr *hdr;
-	struct hci_h4p_neg_evt *evt;
-
-	hdr = (struct hci_h4p_neg_hdr *)skb->data;
-	if (hdr->dlen != sizeof(*evt)) {
-		info->init_error = -EIO;
-		goto finish_neg;
-	}
-
-	evt = (struct hci_h4p_neg_evt *)skb_pull(skb, sizeof(*hdr));
-
-	if (evt->ack != H4P_NEG_ACK) {
-		dev_err(info->dev, "Could not negotiate hci_h4p settings\n");
-		info->init_error = -EINVAL;
-	}
-
-	info->man_id = evt->man_id;
-	info->ver_id = evt->ver_id;
-
-finish_neg:
-
-	complete(&info->init_completion);
-	kfree_skb(skb);
-}
-
-/* H4 packet handling functions */
-static int hci_h4p_get_hdr_len(struct hci_h4p_info *info, u8 pkt_type)
-{
-	long retval;
-
-	switch (pkt_type) {
-	case H4_EVT_PKT:
-		retval = HCI_EVENT_HDR_SIZE;
-		break;
-	case H4_ACL_PKT:
-		retval = HCI_ACL_HDR_SIZE;
-		break;
-	case H4_SCO_PKT:
-		retval = HCI_SCO_HDR_SIZE;
-		break;
-	case H4_NEG_PKT:
-		retval = H4P_NEG_HDR_SIZE;
-		break;
-	case H4_ALIVE_PKT:
-		retval = H4P_ALIVE_HDR_SIZE;
-		break;
-	case H4_RADIO_PKT:
-		retval = H4_RADIO_HDR_SIZE;
-		break;
-	default:
-		dev_err(info->dev, "Unknown H4 packet type 0x%.2x\n", pkt_type);
-		retval = -1;
-		break;
-	}
-
-	return retval;
-}
-
-static unsigned int hci_h4p_get_data_len(struct hci_h4p_info *info,
-					 struct sk_buff *skb)
-{
-	long retval = -1;
-	struct hci_acl_hdr *acl_hdr;
-	struct hci_sco_hdr *sco_hdr;
-	struct hci_event_hdr *evt_hdr;
-	struct hci_h4p_neg_hdr *neg_hdr;
-	struct hci_h4p_alive_hdr *alive_hdr;
-	struct hci_h4p_radio_hdr *radio_hdr;
-
-	switch (bt_cb(skb)->pkt_type) {
-	case H4_EVT_PKT:
-		evt_hdr = (struct hci_event_hdr *)skb->data;
-		retval = evt_hdr->plen;
-		break;
-	case H4_ACL_PKT:
-		acl_hdr = (struct hci_acl_hdr *)skb->data;
-		retval = le16_to_cpu(acl_hdr->dlen);
-		break;
-	case H4_SCO_PKT:
-		sco_hdr = (struct hci_sco_hdr *)skb->data;
-		retval = sco_hdr->dlen;
-		break;
-	case H4_RADIO_PKT:
-		radio_hdr = (struct hci_h4p_radio_hdr *)skb->data;
-		retval = radio_hdr->dlen;
-		break;
-	case H4_NEG_PKT:
-		neg_hdr = (struct hci_h4p_neg_hdr *)skb->data;
-		retval = neg_hdr->dlen;
-		break;
-	case H4_ALIVE_PKT:
-		alive_hdr = (struct hci_h4p_alive_hdr *)skb->data;
-		retval = alive_hdr->dlen;
-		break;
-	}
-
-	return retval;
-}
-
-static inline void hci_h4p_recv_frame(struct hci_h4p_info *info,
-				      struct sk_buff *skb)
-{
-	if (unlikely(!test_bit(HCI_RUNNING, &info->hdev->flags))) {
-		switch (bt_cb(skb)->pkt_type) {
-		case H4_NEG_PKT:
-			hci_h4p_negotiation_packet(info, skb);
-			info->rx_state = WAIT_FOR_PKT_TYPE;
-			return;
-		case H4_ALIVE_PKT:
-			hci_h4p_alive_packet(info, skb);
-			info->rx_state = WAIT_FOR_PKT_TYPE;
-			return;
-		}
-
-		if (!test_bit(HCI_UP, &info->hdev->flags)) {
-			BT_DBG("fw_event");
-			hci_h4p_parse_fw_event(info, skb);
-			return;
-		}
-	}
-
-	hci_recv_frame(info->hdev, skb);
-	BT_DBG("Frame sent to upper layer");
-}
-
-static inline void hci_h4p_handle_byte(struct hci_h4p_info *info, u8 byte)
-{
-	switch (info->rx_state) {
-	case WAIT_FOR_PKT_TYPE:
-		bt_cb(info->rx_skb)->pkt_type = byte;
-		info->rx_count = hci_h4p_get_hdr_len(info, byte);
-		if (info->rx_count < 0) {
-			info->hdev->stat.err_rx++;
-			kfree_skb(info->rx_skb);
-			info->rx_skb = NULL;
-		} else {
-			info->rx_state = WAIT_FOR_HEADER;
-		}
-		break;
-	case WAIT_FOR_HEADER:
-		info->rx_count--;
-		*skb_put(info->rx_skb, 1) = byte;
-		if (info->rx_count != 0)
-			break;
-		info->rx_count = hci_h4p_get_data_len(info, info->rx_skb);
-		if (info->rx_count > skb_tailroom(info->rx_skb)) {
-			dev_err(info->dev, "frame too long\n");
-			info->garbage_bytes = info->rx_count
-				- skb_tailroom(info->rx_skb);
-			kfree_skb(info->rx_skb);
-			info->rx_skb = NULL;
-			break;
-		}
-		info->rx_state = WAIT_FOR_DATA;
-		break;
-	case WAIT_FOR_DATA:
-		info->rx_count--;
-		*skb_put(info->rx_skb, 1) = byte;
-		break;
-	default:
-		WARN_ON(1);
-		break;
-	}
-
-	if (info->rx_count == 0) {
-		/* H4+ devices should always send word aligned packets */
-		if (!(info->rx_skb->len % 2))
-			info->garbage_bytes++;
-		hci_h4p_recv_frame(info, info->rx_skb);
-		info->rx_skb = NULL;
-	}
-}
-
-static void hci_h4p_rx_tasklet(unsigned long data)
-{
-	u8 byte;
-	struct hci_h4p_info *info = (struct hci_h4p_info *)data;
-
-	BT_DBG("tasklet woke up");
-	BT_DBG("rx_tasklet woke up");
-
-	while (hci_h4p_inb(info, UART_LSR) & UART_LSR_DR) {
-		byte = hci_h4p_inb(info, UART_RX);
-		if (info->garbage_bytes) {
-			info->garbage_bytes--;
-			continue;
-		}
-		if (info->rx_skb == NULL) {
-			info->rx_skb = bt_skb_alloc(HCI_MAX_FRAME_SIZE,
-						    GFP_ATOMIC | GFP_DMA);
-			if (!info->rx_skb) {
-				dev_err(info->dev,
-					"No memory for new packet\n");
-				goto finish_rx;
-			}
-			info->rx_state = WAIT_FOR_PKT_TYPE;
-			info->rx_skb->dev = (void *)info->hdev;
-		}
-		info->hdev->stat.byte_rx++;
-		hci_h4p_handle_byte(info, byte);
-	}
-
-	if (!info->rx_enabled) {
-		if (hci_h4p_inb(info, UART_LSR) & UART_LSR_TEMT &&
-						  info->autorts) {
-			__hci_h4p_set_auto_ctsrts(info, 0 , UART_EFR_RTS);
-			info->autorts = 0;
-		}
-		/* Flush posted write to avoid spurious interrupts */
-		hci_h4p_inb(info, UART_OMAP_SCR);
-		hci_h4p_set_clk(info, &info->rx_clocks_en, 0);
-	}
-
-finish_rx:
-	BT_DBG("rx_ended");
-}
-
-static void hci_h4p_tx_tasklet(unsigned long data)
-{
-	unsigned int sent = 0;
-	struct sk_buff *skb;
-	struct hci_h4p_info *info = (struct hci_h4p_info *)data;
-
-	BT_DBG("tasklet woke up");
-	BT_DBG("tx_tasklet woke up");
-
-	if (info->autorts != info->rx_enabled) {
-		if (hci_h4p_inb(info, UART_LSR) & UART_LSR_TEMT) {
-			if (info->autorts && !info->rx_enabled) {
-				__hci_h4p_set_auto_ctsrts(info, 0,
-							  UART_EFR_RTS);
-				info->autorts = 0;
-			}
-			if (!info->autorts && info->rx_enabled) {
-				__hci_h4p_set_auto_ctsrts(info, 1,
-							  UART_EFR_RTS);
-				info->autorts = 1;
-			}
-		} else {
-			hci_h4p_outb(info, UART_OMAP_SCR,
-				     hci_h4p_inb(info, UART_OMAP_SCR) |
-				     UART_OMAP_SCR_EMPTY_THR);
-			goto finish_tx;
-		}
-	}
-
-	skb = skb_dequeue(&info->txq);
-	if (!skb) {
-		/* No data in buffer */
-		BT_DBG("skb ready");
-		if (hci_h4p_inb(info, UART_LSR) & UART_LSR_TEMT) {
-			hci_h4p_outb(info, UART_IER,
-				     hci_h4p_inb(info, UART_IER) &
-				     ~UART_IER_THRI);
-			hci_h4p_inb(info, UART_OMAP_SCR);
-			hci_h4p_disable_tx(info);
-			return;
-		}
-		hci_h4p_outb(info, UART_OMAP_SCR,
-			     hci_h4p_inb(info, UART_OMAP_SCR) |
-			     UART_OMAP_SCR_EMPTY_THR);
-		goto finish_tx;
-	}
-
-	/* Copy data to tx fifo */
-	while (!(hci_h4p_inb(info, UART_OMAP_SSR) & UART_OMAP_SSR_TXFULL) &&
-	       (sent < skb->len)) {
-		hci_h4p_outb(info, UART_TX, skb->data[sent]);
-		sent++;
-	}
-
-	info->hdev->stat.byte_tx += sent;
-	if (skb->len == sent) {
-		kfree_skb(skb);
-	} else {
-		skb_pull(skb, sent);
-		skb_queue_head(&info->txq, skb);
-	}
-
-	hci_h4p_outb(info, UART_OMAP_SCR, hci_h4p_inb(info, UART_OMAP_SCR) &
-						     ~UART_OMAP_SCR_EMPTY_THR);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-						 UART_IER_THRI);
-
-finish_tx:
-	/* Flush posted write to avoid spurious interrupts */
-	hci_h4p_inb(info, UART_OMAP_SCR);
-
-}
-
-static irqreturn_t hci_h4p_interrupt(int irq, void *data)
-{
-	struct hci_h4p_info *info = (struct hci_h4p_info *)data;
-	u8 iir, msr;
-	int ret;
-
-	ret = IRQ_NONE;
-
-	iir = hci_h4p_inb(info, UART_IIR);
-	if (iir & UART_IIR_NO_INT)
-		return IRQ_HANDLED;
-
-	BT_DBG("In interrupt handler iir 0x%.2x", iir);
-
-	iir &= UART_IIR_ID;
-
-	if (iir == UART_IIR_MSI) {
-		msr = hci_h4p_inb(info, UART_MSR);
-		ret = IRQ_HANDLED;
-	}
-	if (iir == UART_IIR_RLSI) {
-		hci_h4p_inb(info, UART_RX);
-		hci_h4p_inb(info, UART_LSR);
-		ret = IRQ_HANDLED;
-	}
-
-	if (iir == UART_IIR_RDI) {
-		hci_h4p_rx_tasklet((unsigned long)data);
-		ret = IRQ_HANDLED;
-	}
-
-	if (iir == UART_IIR_THRI) {
-		hci_h4p_tx_tasklet((unsigned long)data);
-		ret = IRQ_HANDLED;
-	}
-
-	return ret;
-}
-
-static irqreturn_t hci_h4p_wakeup_interrupt(int irq, void *dev_inst)
-{
-	struct hci_h4p_info *info = dev_inst;
-	int should_wakeup;
-	struct hci_dev *hdev;
-
-	if (!info->hdev)
-		return IRQ_HANDLED;
-
-	should_wakeup = gpio_get_value(info->host_wakeup_gpio);
-	hdev = info->hdev;
-
-	if (!test_bit(HCI_RUNNING, &hdev->flags)) {
-		if (should_wakeup == 1)
-			complete_all(&info->test_completion);
-
-		return IRQ_HANDLED;
-	}
-
-	BT_DBG("gpio interrupt %d", should_wakeup);
-
-	/* Check if wee have missed some interrupts */
-	if (info->rx_enabled == should_wakeup)
-		return IRQ_HANDLED;
-
-	if (should_wakeup)
-		hci_h4p_enable_rx(info);
-	else
-		hci_h4p_disable_rx(info);
-
-	return IRQ_HANDLED;
-}
-
-static inline void hci_h4p_set_pm_limits(struct hci_h4p_info *info, bool set)
-{
-	struct hci_h4p_platform_data *bt_plat_data = info->dev->platform_data;
-	const char *sset = set ? "set" : "clear";
-
-	if (unlikely(!bt_plat_data || !bt_plat_data->set_pm_limits))
-		return;
-
-	if (set != !!test_bit(H4P_ACTIVE_MODE, &info->pm_flags)) {
-		bt_plat_data->set_pm_limits(info->dev, set);
-		if (set)
-			set_bit(H4P_ACTIVE_MODE, &info->pm_flags);
-		else
-			clear_bit(H4P_ACTIVE_MODE, &info->pm_flags);
-		BT_DBG("Change pm constraints to: %s", sset);
-		return;
-	}
-
-	BT_DBG("pm constraints remains: %s", sset);
-}
-
-static int hci_h4p_reset(struct hci_h4p_info *info)
-{
-	int err;
-
-	err = hci_h4p_reset_uart(info);
-	if (err < 0) {
-		dev_err(info->dev, "Uart reset failed\n");
-		return err;
-	}
-	hci_h4p_init_uart(info);
-	hci_h4p_set_rts(info, 0);
-
-	gpio_set_value(info->reset_gpio, 0);
-	gpio_set_value(info->bt_wakeup_gpio, 1);
-	msleep(10);
-
-	if (gpio_get_value(info->host_wakeup_gpio) == 1) {
-		dev_err(info->dev, "host_wakeup_gpio not low\n");
-		return -EPROTO;
-	}
-
-	init_completion(&info->test_completion);
-	gpio_set_value(info->reset_gpio, 1);
-
-	if (!wait_for_completion_interruptible_timeout(&info->test_completion,
-						       msecs_to_jiffies(100))) {
-		dev_err(info->dev, "wakeup test timed out\n");
-		complete_all(&info->test_completion);
-		return -EPROTO;
-	}
-
-	err = hci_h4p_wait_for_cts(info, 1, 100);
-	if (err < 0) {
-		dev_err(info->dev, "No cts from bt chip\n");
-		return err;
-	}
-
-	hci_h4p_set_rts(info, 1);
-
-	return 0;
-}
-
-/* hci callback functions */
-static int hci_h4p_hci_flush(struct hci_dev *hdev)
-{
-	struct hci_h4p_info *info = hci_get_drvdata(hdev);
-
-	skb_queue_purge(&info->txq);
-
-	return 0;
-}
-
-static int hci_h4p_bt_wakeup_test(struct hci_h4p_info *info)
-{
-	/*
-	 * Test Sequence:
-	 * Host de-asserts the BT_WAKE_UP line.
-	 * Host polls the UART_CTS line, waiting for it to be de-asserted.
-	 * Host asserts the BT_WAKE_UP line.
-	 * Host polls the UART_CTS line, waiting for it to be asserted.
-	 * Host de-asserts the BT_WAKE_UP line (allow the Bluetooth device to
-	 * sleep).
-	 * Host polls the UART_CTS line, waiting for it to be de-asserted.
-	 */
-	int err;
-	int ret = -ECOMM;
-
-	if (!info)
-		return -EINVAL;
-
-	/* Disable wakeup interrupts */
-	disable_irq(gpio_to_irq(info->host_wakeup_gpio));
-
-	gpio_set_value(info->bt_wakeup_gpio, 0);
-	err = hci_h4p_wait_for_cts(info, 0, 100);
-	if (err) {
-		dev_warn(info->dev,
-				"bt_wakeup_test: fail: CTS low timed out: %d\n",
-				err);
-		goto out;
-	}
-
-	gpio_set_value(info->bt_wakeup_gpio, 1);
-	err = hci_h4p_wait_for_cts(info, 1, 100);
-	if (err) {
-		dev_warn(info->dev,
-				"bt_wakeup_test: fail: CTS high timed out: %d\n",
-				err);
-		goto out;
-	}
-
-	gpio_set_value(info->bt_wakeup_gpio, 0);
-	err = hci_h4p_wait_for_cts(info, 0, 100);
-	if (err) {
-		dev_warn(info->dev,
-				"bt_wakeup_test: fail: CTS re-low timed out: %d\n",
-				err);
-		goto out;
-	}
-
-	ret = 0;
-
-out:
-
-	/* Re-enable wakeup interrupts */
-	enable_irq(gpio_to_irq(info->host_wakeup_gpio));
-
-	return ret;
-}
-
-static int hci_h4p_hci_open(struct hci_dev *hdev)
-{
-	struct hci_h4p_info *info;
-	int err, retries = 0;
-	struct sk_buff_head fw_queue;
-	unsigned long flags;
-
-	info = hci_get_drvdata(hdev);
-
-	if (test_bit(HCI_RUNNING, &hdev->flags))
-		return 0;
-
-	/* TI1271 has HW bug and boot up might fail. Retry up to three times */
-again:
-
-	info->rx_enabled = 1;
-	info->rx_state = WAIT_FOR_PKT_TYPE;
-	info->rx_count = 0;
-	info->garbage_bytes = 0;
-	info->rx_skb = NULL;
-	info->pm_enabled = 0;
-	init_completion(&info->fw_completion);
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 1);
-	hci_h4p_set_clk(info, &info->rx_clocks_en, 1);
-	skb_queue_head_init(&fw_queue);
-
-	err = hci_h4p_reset(info);
-	if (err < 0)
-		goto err_clean;
-
-	hci_h4p_set_auto_ctsrts(info, 1, UART_EFR_CTS | UART_EFR_RTS);
-	info->autorts = 1;
-
-	err = hci_h4p_send_negotiation(info);
-
-	err = hci_h4p_read_fw(info, &fw_queue);
-	if (err < 0) {
-		dev_err(info->dev, "Cannot read firmware\n");
-		goto err_clean;
-	}
-
-	err = hci_h4p_send_fw(info, &fw_queue);
-	if (err < 0) {
-		dev_err(info->dev, "Sending firmware failed.\n");
-		goto err_clean;
-	}
-
-	info->pm_enabled = 1;
-
-	err = hci_h4p_bt_wakeup_test(info);
-	if (err < 0) {
-		dev_err(info->dev, "BT wakeup test failed.\n");
-		goto err_clean;
-	}
-
-	spin_lock_irqsave(&info->lock, flags);
-	info->rx_enabled = gpio_get_value(info->host_wakeup_gpio);
-	hci_h4p_set_clk(info, &info->rx_clocks_en, info->rx_enabled);
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 0);
-
-	kfree_skb(info->alive_cmd_skb);
-	info->alive_cmd_skb = NULL;
-	set_bit(HCI_RUNNING, &hdev->flags);
-
-	BT_DBG("hci up and running");
-	return 0;
-
-err_clean:
-	hci_h4p_hci_flush(hdev);
-	hci_h4p_reset_uart(info);
-	del_timer_sync(&info->lazy_release);
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 0);
-	hci_h4p_set_clk(info, &info->rx_clocks_en, 0);
-	gpio_set_value(info->reset_gpio, 0);
-	gpio_set_value(info->bt_wakeup_gpio, 0);
-	skb_queue_purge(&fw_queue);
-	kfree_skb(info->alive_cmd_skb);
-	info->alive_cmd_skb = NULL;
-	kfree_skb(info->rx_skb);
-	info->rx_skb = NULL;
-
-	if (retries++ < 3) {
-		dev_err(info->dev, "FW loading try %d fail. Retry.\n", retries);
-		goto again;
-	}
-
-	return err;
-}
-
-static int hci_h4p_hci_close(struct hci_dev *hdev)
-{
-	struct hci_h4p_info *info = hci_get_drvdata(hdev);
-
-	if (!test_and_clear_bit(HCI_RUNNING, &hdev->flags))
-		return 0;
-
-	hci_h4p_hci_flush(hdev);
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 1);
-	hci_h4p_set_clk(info, &info->rx_clocks_en, 1);
-	hci_h4p_reset_uart(info);
-	del_timer_sync(&info->lazy_release);
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 0);
-	hci_h4p_set_clk(info, &info->rx_clocks_en, 0);
-	gpio_set_value(info->reset_gpio, 0);
-	gpio_set_value(info->bt_wakeup_gpio, 0);
-	kfree_skb(info->rx_skb);
-
-	return 0;
-}
-
-static int hci_h4p_hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb)
-{
-	struct hci_h4p_info *info;
-	int err = 0;
-
-	BT_DBG("dev %p, skb %p", hdev, skb);
-
-	info = hci_get_drvdata(hdev);
-
-	if (!test_bit(HCI_RUNNING, &hdev->flags)) {
-		dev_warn(info->dev, "Frame for non-running device\n");
-		return -EIO;
-	}
-
-	switch (bt_cb(skb)->pkt_type) {
-	case HCI_COMMAND_PKT:
-		hdev->stat.cmd_tx++;
-		break;
-	case HCI_ACLDATA_PKT:
-		hdev->stat.acl_tx++;
-		break;
-	case HCI_SCODATA_PKT:
-		hdev->stat.sco_tx++;
-		break;
-	}
-
-	/* Push frame type to skb */
-	*skb_push(skb, 1) = (bt_cb(skb)->pkt_type);
-	/* We should allways send word aligned data to h4+ devices */
-	if (skb->len % 2) {
-		err = skb_pad(skb, 1);
-		if (!err)
-			*skb_put(skb, 1) = 0x00;
-	}
-	if (err)
-		return err;
-
-	skb_queue_tail(&info->txq, skb);
-	hci_h4p_enable_tx(info);
-
-	return 0;
-}
-
-static ssize_t hci_h4p_store_bdaddr(struct device *dev,
-				    struct device_attribute *attr,
-				    const char *buf, size_t count)
-{
-	struct hci_h4p_info *info = dev_get_drvdata(dev);
-	unsigned int bdaddr[6];
-	int ret, i;
-
-	ret = sscanf(buf, "%2x:%2x:%2x:%2x:%2x:%2x\n",
-			&bdaddr[0], &bdaddr[1], &bdaddr[2],
-			&bdaddr[3], &bdaddr[4], &bdaddr[5]);
-
-	if (ret != 6)
-		return -EINVAL;
-
-	for (i = 0; i < 6; i++) {
-		if (bdaddr[i] > 0xff)
-			return -EINVAL;
-		info->bd_addr[i] = bdaddr[i] & 0xff;
-	}
-
-	return count;
-}
-
-static ssize_t hci_h4p_show_bdaddr(struct device *dev,
-				   struct device_attribute *attr, char *buf)
-{
-	struct hci_h4p_info *info = dev_get_drvdata(dev);
-
-	return sprintf(buf, "%pMR\n", info->bd_addr);
-}
-
-static DEVICE_ATTR(bdaddr, S_IRUGO | S_IWUSR, hci_h4p_show_bdaddr,
-		   hci_h4p_store_bdaddr);
-
-static int hci_h4p_sysfs_create_files(struct device *dev)
-{
-	return device_create_file(dev, &dev_attr_bdaddr);
-}
-
-static void hci_h4p_sysfs_remove_files(struct device *dev)
-{
-	device_remove_file(dev, &dev_attr_bdaddr);
-}
-
-static int hci_h4p_register_hdev(struct hci_h4p_info *info)
-{
-	struct hci_dev *hdev;
-
-	/* Initialize and register HCI device */
-
-	hdev = hci_alloc_dev();
-	if (!hdev) {
-		dev_err(info->dev, "Can't allocate memory for device\n");
-		return -ENOMEM;
-	}
-	info->hdev = hdev;
-
-	hdev->bus = HCI_UART;
-	hci_set_drvdata(hdev, info);
-
-	hdev->open = hci_h4p_hci_open;
-	hdev->close = hci_h4p_hci_close;
-	hdev->flush = hci_h4p_hci_flush;
-	hdev->send = hci_h4p_hci_send_frame;
-	set_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks);
-
-	SET_HCIDEV_DEV(hdev, info->dev);
-
-	if (hci_h4p_sysfs_create_files(info->dev) < 0) {
-		dev_err(info->dev, "failed to create sysfs files\n");
-		goto free;
-	}
-
-	if (hci_register_dev(hdev) >= 0)
-		return 0;
-
-	dev_err(info->dev, "hci_register failed %s.\n", hdev->name);
-	hci_h4p_sysfs_remove_files(info->dev);
-free:
-	hci_free_dev(info->hdev);
-	return -ENODEV;
-}
-
-static int hci_h4p_probe(struct platform_device *pdev)
-{
-	struct hci_h4p_platform_data *bt_plat_data;
-	struct hci_h4p_info *info;
-	int err;
-
-	dev_info(&pdev->dev, "Registering HCI H4P device\n");
-	info = devm_kzalloc(&pdev->dev, sizeof(struct hci_h4p_info),
-			GFP_KERNEL);
-	if (!info)
-		return -ENOMEM;
-
-	info->dev = &pdev->dev;
-	info->tx_enabled = 1;
-	info->rx_enabled = 1;
-	spin_lock_init(&info->lock);
-	spin_lock_init(&info->clocks_lock);
-	skb_queue_head_init(&info->txq);
-
-	if (pdev->dev.platform_data == NULL) {
-		dev_err(&pdev->dev, "Could not get Bluetooth config data\n");
-		return -ENODATA;
-	}
-
-	bt_plat_data = pdev->dev.platform_data;
-	info->chip_type = bt_plat_data->chip_type;
-	info->bt_wakeup_gpio = bt_plat_data->bt_wakeup_gpio;
-	info->host_wakeup_gpio = bt_plat_data->host_wakeup_gpio;
-	info->reset_gpio = bt_plat_data->reset_gpio;
-	info->reset_gpio_shared = bt_plat_data->reset_gpio_shared;
-	info->bt_sysclk = bt_plat_data->bt_sysclk;
-
-	BT_DBG("RESET gpio: %d", info->reset_gpio);
-	BT_DBG("BTWU gpio: %d", info->bt_wakeup_gpio);
-	BT_DBG("HOSTWU gpio: %d", info->host_wakeup_gpio);
-	BT_DBG("sysclk: %d", info->bt_sysclk);
-
-	init_completion(&info->test_completion);
-	complete_all(&info->test_completion);
-
-	if (!info->reset_gpio_shared) {
-		err = devm_gpio_request_one(&pdev->dev, info->reset_gpio,
-					    GPIOF_OUT_INIT_LOW, "bt_reset");
-		if (err < 0) {
-			dev_err(&pdev->dev, "Cannot get GPIO line %d\n",
-				info->reset_gpio);
-			return err;
-		}
-	}
-
-	err = devm_gpio_request_one(&pdev->dev, info->bt_wakeup_gpio,
-				    GPIOF_OUT_INIT_LOW, "bt_wakeup");
-
-	if (err < 0) {
-		dev_err(info->dev, "Cannot get GPIO line 0x%d",
-			info->bt_wakeup_gpio);
-		return err;
-	}
-
-	err = devm_gpio_request_one(&pdev->dev, info->host_wakeup_gpio,
-				    GPIOF_DIR_IN, "host_wakeup");
-	if (err < 0) {
-		dev_err(info->dev, "Cannot get GPIO line %d",
-		       info->host_wakeup_gpio);
-		return err;
-	}
-
-	info->irq = bt_plat_data->uart_irq;
-	info->uart_base = devm_ioremap(&pdev->dev, bt_plat_data->uart_base,
-					SZ_2K);
-	info->uart_iclk = devm_clk_get(&pdev->dev, bt_plat_data->uart_iclk);
-	info->uart_fclk = devm_clk_get(&pdev->dev, bt_plat_data->uart_fclk);
-
-	err = devm_request_irq(&pdev->dev, info->irq, hci_h4p_interrupt,
-				IRQF_DISABLED, "hci_h4p", info);
-	if (err < 0) {
-		dev_err(info->dev, "hci_h4p: unable to get IRQ %d\n",
-			info->irq);
-		return err;
-	}
-
-	err = devm_request_irq(&pdev->dev, gpio_to_irq(info->host_wakeup_gpio),
-			  hci_h4p_wakeup_interrupt,  IRQF_TRIGGER_FALLING |
-			  IRQF_TRIGGER_RISING | IRQF_DISABLED,
-			  "hci_h4p_wkup", info);
-	if (err < 0) {
-		dev_err(info->dev, "hci_h4p: unable to get wakeup IRQ %d\n",
-			  gpio_to_irq(info->host_wakeup_gpio));
-		return err;
-	}
-
-	err = irq_set_irq_wake(gpio_to_irq(info->host_wakeup_gpio), 1);
-	if (err < 0) {
-		dev_err(info->dev, "hci_h4p: unable to set wakeup for IRQ %d\n",
-				gpio_to_irq(info->host_wakeup_gpio));
-		return err;
-	}
-
-	init_timer_deferrable(&info->lazy_release);
-	info->lazy_release.function = hci_h4p_lazy_clock_release;
-	info->lazy_release.data = (unsigned long)info;
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 1);
-	err = hci_h4p_reset_uart(info);
-	if (err < 0)
-		return err;
-	gpio_set_value(info->reset_gpio, 0);
-	hci_h4p_set_clk(info, &info->tx_clocks_en, 0);
-
-	platform_set_drvdata(pdev, info);
-
-	if (hci_h4p_register_hdev(info) < 0) {
-		dev_err(info->dev, "failed to register hci_h4p hci device\n");
-		return -EINVAL;
-	}
-
-	return 0;
-}
-
-static int hci_h4p_remove(struct platform_device *pdev)
-{
-	struct hci_h4p_info *info;
-
-	info = platform_get_drvdata(pdev);
-
-	hci_h4p_sysfs_remove_files(info->dev);
-	hci_h4p_hci_close(info->hdev);
-	hci_unregister_dev(info->hdev);
-	hci_free_dev(info->hdev);
-
-	return 0;
-}
-
-static struct platform_driver hci_h4p_driver = {
-	.probe		= hci_h4p_probe,
-	.remove		= hci_h4p_remove,
-	.driver		= {
-		.name	= "hci_h4p",
-	},
-};
-
-module_platform_driver(hci_h4p_driver);
-
-MODULE_ALIAS("platform:hci_h4p");
-MODULE_DESCRIPTION("Bluetooth h4 driver with nokia extensions");
-MODULE_LICENSE("GPL");
-MODULE_AUTHOR("Ville Tervo");
diff --git a/drivers/staging/nokia_h4p/nokia_fw-bcm.c b/drivers/staging/nokia_h4p/nokia_fw-bcm.c
deleted file mode 100644
index b55f5ba1134e..000000000000
--- a/drivers/staging/nokia_h4p/nokia_fw-bcm.c
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * This file is part of Nokia H4P bluetooth driver
- *
- * Copyright (C) 2005-2008 Nokia Corporation.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#include <linux/skbuff.h>
-#include <linux/delay.h>
-#include <linux/serial_reg.h>
-
-#include "hci_h4p.h"
-
-static int hci_h4p_bcm_set_bdaddr(struct hci_h4p_info *info,
-				struct sk_buff *skb)
-{
-	int i;
-	static const u8 nokia_oui[3] = {0x00, 0x1f, 0xdf};
-	int not_valid;
-
-	not_valid = 1;
-	for (i = 0; i < 6; i++) {
-		if (info->bd_addr[i] != 0x00) {
-			not_valid = 0;
-			break;
-		}
-	}
-
-	if (not_valid) {
-		dev_info(info->dev, "Valid bluetooth address not found, setting some random\n");
-		/* When address is not valid, use some random but Nokia MAC */
-		memcpy(info->bd_addr, nokia_oui, 3);
-		get_random_bytes(info->bd_addr + 3, 3);
-	}
-
-	for (i = 0; i < 6; i++)
-		skb->data[9 - i] = info->bd_addr[i];
-
-	return 0;
-}
-
-void hci_h4p_bcm_parse_fw_event(struct hci_h4p_info *info, struct sk_buff *skb)
-{
-	struct sk_buff *fw_skb;
-	int err;
-	unsigned long flags;
-
-	if (skb->data[5] != 0x00) {
-		dev_err(info->dev, "Firmware sending command failed 0x%.2x\n",
-			skb->data[5]);
-		info->fw_error = -EPROTO;
-	}
-
-	kfree_skb(skb);
-
-	fw_skb = skb_dequeue(info->fw_q);
-	if (fw_skb == NULL || info->fw_error) {
-		complete(&info->fw_completion);
-		return;
-	}
-
-	if (fw_skb->data[1] == 0x01 && fw_skb->data[2] == 0xfc &&
-			fw_skb->len >= 10) {
-		BT_DBG("Setting bluetooth address");
-		err = hci_h4p_bcm_set_bdaddr(info, fw_skb);
-		if (err < 0) {
-			kfree_skb(fw_skb);
-			info->fw_error = err;
-			complete(&info->fw_completion);
-			return;
-		}
-	}
-
-	skb_queue_tail(&info->txq, fw_skb);
-	spin_lock_irqsave(&info->lock, flags);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-			UART_IER_THRI);
-	spin_unlock_irqrestore(&info->lock, flags);
-}
-
-
-int hci_h4p_bcm_send_fw(struct hci_h4p_info *info,
-			struct sk_buff_head *fw_queue)
-{
-	struct sk_buff *skb;
-	unsigned long flags, time;
-
-	info->fw_error = 0;
-
-	BT_DBG("Sending firmware");
-
-	time = jiffies;
-
-	info->fw_q = fw_queue;
-	skb = skb_dequeue(fw_queue);
-	if (!skb)
-		return -ENODATA;
-
-	BT_DBG("Sending commands");
-
-	/*
-	 * Disable smart-idle as UART TX interrupts
-	 * are not wake-up capable
-	 */
-	hci_h4p_smart_idle(info, 0);
-
-	/* Check if this is bd_address packet */
-	init_completion(&info->fw_completion);
-	skb_queue_tail(&info->txq, skb);
-	spin_lock_irqsave(&info->lock, flags);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-			UART_IER_THRI);
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	if (!wait_for_completion_timeout(&info->fw_completion,
-				msecs_to_jiffies(2000))) {
-		dev_err(info->dev, "No reply to fw command\n");
-		return -ETIMEDOUT;
-	}
-
-	if (info->fw_error) {
-		dev_err(info->dev, "FW error\n");
-		return -EPROTO;
-	}
-
-	BT_DBG("Firmware sent in %d msecs",
-		   jiffies_to_msecs(jiffies-time));
-
-	hci_h4p_set_auto_ctsrts(info, 0, UART_EFR_RTS);
-	hci_h4p_set_rts(info, 0);
-	hci_h4p_change_speed(info, BC4_MAX_BAUD_RATE);
-	hci_h4p_set_auto_ctsrts(info, 1, UART_EFR_RTS);
-
-	return 0;
-}
diff --git a/drivers/staging/nokia_h4p/nokia_fw-csr.c b/drivers/staging/nokia_h4p/nokia_fw-csr.c
deleted file mode 100644
index fe6b704b3d97..000000000000
--- a/drivers/staging/nokia_h4p/nokia_fw-csr.c
+++ /dev/null
@@ -1,149 +0,0 @@
-/*
- * This file is part of Nokia H4P bluetooth driver
- *
- * Copyright (C) 2005-2008 Nokia Corporation.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#include <linux/skbuff.h>
-#include <linux/delay.h>
-#include <linux/serial_reg.h>
-
-#include "hci_h4p.h"
-
-void hci_h4p_bc4_parse_fw_event(struct hci_h4p_info *info, struct sk_buff *skb)
-{
-	/* Check if this is fw packet */
-	if (skb->data[0] != 0xff) {
-		hci_recv_frame(info->hdev, skb);
-		return;
-	}
-
-	if (skb->data[11] || skb->data[12]) {
-		dev_err(info->dev, "Firmware sending command failed\n");
-		info->fw_error = -EPROTO;
-	}
-
-	kfree_skb(skb);
-	complete(&info->fw_completion);
-}
-
-int hci_h4p_bc4_send_fw(struct hci_h4p_info *info,
-			struct sk_buff_head *fw_queue)
-{
-	static const u8 nokia_oui[3] = {0x00, 0x19, 0x4F};
-	struct sk_buff *skb;
-	unsigned int offset;
-	int retries, count, i, not_valid;
-	unsigned long flags;
-
-	info->fw_error = 0;
-
-	BT_DBG("Sending firmware");
-	skb = skb_dequeue(fw_queue);
-
-	if (!skb)
-		return -ENOMSG;
-
-	/* Check if this is bd_address packet */
-	if (skb->data[15] == 0x01 && skb->data[16] == 0x00) {
-		offset = 21;
-		skb->data[offset + 1] = 0x00;
-		skb->data[offset + 5] = 0x00;
-
-		not_valid = 1;
-		for (i = 0; i < 6; i++) {
-			if (info->bd_addr[i] != 0x00) {
-				not_valid = 0;
-				break;
-			}
-		}
-
-		if (not_valid) {
-			dev_info(info->dev, "Valid bluetooth address not found, setting some random\n");
-			/* When address is not valid, use some random */
-			memcpy(info->bd_addr, nokia_oui, 3);
-			get_random_bytes(info->bd_addr + 3, 3);
-		}
-
-		skb->data[offset + 7] = info->bd_addr[0];
-		skb->data[offset + 6] = info->bd_addr[1];
-		skb->data[offset + 4] = info->bd_addr[2];
-		skb->data[offset + 0] = info->bd_addr[3];
-		skb->data[offset + 3] = info->bd_addr[4];
-		skb->data[offset + 2] = info->bd_addr[5];
-	}
-
-	for (count = 1; ; count++) {
-		BT_DBG("Sending firmware command %d", count);
-		init_completion(&info->fw_completion);
-		skb_queue_tail(&info->txq, skb);
-		spin_lock_irqsave(&info->lock, flags);
-		hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-							 UART_IER_THRI);
-		spin_unlock_irqrestore(&info->lock, flags);
-
-		skb = skb_dequeue(fw_queue);
-		if (!skb)
-			break;
-
-		if (!wait_for_completion_timeout(&info->fw_completion,
-						 msecs_to_jiffies(1000))) {
-			dev_err(info->dev, "No reply to fw command\n");
-			return -ETIMEDOUT;
-		}
-
-		if (info->fw_error) {
-			dev_err(info->dev, "FW error\n");
-			return -EPROTO;
-		}
-	};
-
-	/* Wait for chip warm reset */
-	retries = 100;
-	while ((!skb_queue_empty(&info->txq) ||
-	       !(hci_h4p_inb(info, UART_LSR) & UART_LSR_TEMT)) &&
-	       retries--) {
-		msleep(10);
-	}
-	if (!retries) {
-		dev_err(info->dev, "Transmitter not empty\n");
-		return -ETIMEDOUT;
-	}
-
-	hci_h4p_change_speed(info, BC4_MAX_BAUD_RATE);
-
-	if (hci_h4p_wait_for_cts(info, 1, 100)) {
-		dev_err(info->dev, "cts didn't deassert after final speed\n");
-		return -ETIMEDOUT;
-	}
-
-	retries = 100;
-	do {
-		init_completion(&info->init_completion);
-		hci_h4p_send_alive_packet(info);
-		retries--;
-	} while (!wait_for_completion_timeout(&info->init_completion, 100) &&
-		 retries > 0);
-
-	if (!retries) {
-		dev_err(info->dev, "No alive reply after speed change\n");
-		return -ETIMEDOUT;
-	}
-
-	return 0;
-}
diff --git a/drivers/staging/nokia_h4p/nokia_fw-ti1273.c b/drivers/staging/nokia_h4p/nokia_fw-ti1273.c
deleted file mode 100644
index f5500f71c839..000000000000
--- a/drivers/staging/nokia_h4p/nokia_fw-ti1273.c
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * This file is part of Nokia H4P bluetooth driver
- *
- * Copyright (C) 2009 Nokia Corporation.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#include <linux/skbuff.h>
-#include <linux/delay.h>
-#include <linux/serial_reg.h>
-
-#include "hci_h4p.h"
-
-static struct sk_buff_head *fw_q;
-
-void hci_h4p_ti1273_parse_fw_event(struct hci_h4p_info *info,
-			struct sk_buff *skb)
-{
-	struct sk_buff *fw_skb;
-	unsigned long flags;
-
-	if (skb->data[5] != 0x00) {
-		dev_err(info->dev, "Firmware sending command failed 0x%.2x\n",
-			skb->data[5]);
-		info->fw_error = -EPROTO;
-	}
-
-	kfree_skb(skb);
-
-	fw_skb = skb_dequeue(fw_q);
-	if (fw_skb == NULL || info->fw_error) {
-		complete(&info->fw_completion);
-		return;
-	}
-
-	skb_queue_tail(&info->txq, fw_skb);
-	spin_lock_irqsave(&info->lock, flags);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-			UART_IER_THRI);
-	spin_unlock_irqrestore(&info->lock, flags);
-}
-
-
-int hci_h4p_ti1273_send_fw(struct hci_h4p_info *info,
-			struct sk_buff_head *fw_queue)
-{
-	struct sk_buff *skb;
-	unsigned long flags, time;
-
-	info->fw_error = 0;
-
-	BT_DBG("Sending firmware");
-
-	time = jiffies;
-
-	fw_q = fw_queue;
-	skb = skb_dequeue(fw_queue);
-	if (!skb)
-		return -ENODATA;
-
-	BT_DBG("Sending commands");
-	/* Check if this is bd_address packet */
-	init_completion(&info->fw_completion);
-	hci_h4p_smart_idle(info, 0);
-	skb_queue_tail(&info->txq, skb);
-	spin_lock_irqsave(&info->lock, flags);
-	hci_h4p_outb(info, UART_IER, hci_h4p_inb(info, UART_IER) |
-			UART_IER_THRI);
-	spin_unlock_irqrestore(&info->lock, flags);
-
-	if (!wait_for_completion_timeout(&info->fw_completion,
-				msecs_to_jiffies(2000))) {
-		dev_err(info->dev, "No reply to fw command\n");
-		return -ETIMEDOUT;
-	}
-
-	if (info->fw_error) {
-		dev_err(info->dev, "FW error\n");
-		return -EPROTO;
-	}
-
-	BT_DBG("Firmware sent in %d msecs",
-		   jiffies_to_msecs(jiffies-time));
-
-	hci_h4p_set_auto_ctsrts(info, 0, UART_EFR_RTS);
-	hci_h4p_set_rts(info, 0);
-	hci_h4p_change_speed(info, BC4_MAX_BAUD_RATE);
-	if (hci_h4p_wait_for_cts(info, 1, 100)) {
-		dev_err(info->dev,
-			"cts didn't go down after final speed change\n");
-		return -ETIMEDOUT;
-	}
-	hci_h4p_set_auto_ctsrts(info, 1, UART_EFR_RTS);
-
-	return 0;
-}
diff --git a/drivers/staging/nokia_h4p/nokia_fw.c b/drivers/staging/nokia_h4p/nokia_fw.c
deleted file mode 100644
index 14ba2193efb6..000000000000
--- a/drivers/staging/nokia_h4p/nokia_fw.c
+++ /dev/null
@@ -1,208 +0,0 @@
-/*
- * This file is part of hci_h4p bluetooth driver
- *
- * Copyright (C) 2005, 2006 Nokia Corporation.
- *
- * Contact: Ville Tervo <ville.tervo@nokia.com>
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#include <linux/module.h>
-#include <linux/skbuff.h>
-#include <linux/firmware.h>
-#include <linux/clk.h>
-
-#include <net/bluetooth/bluetooth.h>
-
-#include "hci_h4p.h"
-
-#define FW_NAME_TI1271_PRELE	"ti1273_prele.bin"
-#define FW_NAME_TI1271_LE	"ti1273_le.bin"
-#define FW_NAME_TI1271		"ti1273.bin"
-#define FW_NAME_BCM2048		"bcmfw.bin"
-#define FW_NAME_CSR		"bc4fw.bin"
-
-static int fw_pos;
-
-/* Firmware handling */
-static int hci_h4p_open_firmware(struct hci_h4p_info *info,
-				 const struct firmware **fw_entry)
-{
-	int err;
-
-	fw_pos = 0;
-	BT_DBG("Opening firmware man_id 0x%.2x ver_id 0x%.2x",
-			info->man_id, info->ver_id);
-	switch (info->man_id) {
-	case H4P_ID_TI1271:
-		switch (info->ver_id) {
-		case 0xe1:
-			err = request_firmware(fw_entry, FW_NAME_TI1271_PRELE,
-						info->dev);
-			break;
-		case 0xd1:
-		case 0xf1:
-			err = request_firmware(fw_entry, FW_NAME_TI1271_LE,
-						info->dev);
-			break;
-		default:
-			err = request_firmware(fw_entry, FW_NAME_TI1271,
-						info->dev);
-		}
-		break;
-	case H4P_ID_CSR:
-		err = request_firmware(fw_entry, FW_NAME_CSR, info->dev);
-		break;
-	case H4P_ID_BCM2048:
-		err = request_firmware(fw_entry, FW_NAME_BCM2048, info->dev);
-		break;
-	default:
-		dev_err(info->dev, "Invalid chip type\n");
-		*fw_entry = NULL;
-		err = -EINVAL;
-	}
-
-	return err;
-}
-
-static void hci_h4p_close_firmware(const struct firmware *fw_entry)
-{
-	release_firmware(fw_entry);
-}
-
-/* Read fw. Return length of the command. If no more commands in
- * fw 0 is returned. In error case return value is negative.
- */
-static int hci_h4p_read_fw_cmd(struct hci_h4p_info *info, struct sk_buff **skb,
-			       const struct firmware *fw_entry, gfp_t how)
-{
-	unsigned int cmd_len;
-
-	if (fw_pos >= fw_entry->size)
-		return 0;
-
-	if (fw_pos + 2 > fw_entry->size) {
-		dev_err(info->dev, "Corrupted firmware image 1\n");
-		return -EMSGSIZE;
-	}
-
-	cmd_len = fw_entry->data[fw_pos++];
-	cmd_len += fw_entry->data[fw_pos++] << 8;
-	if (cmd_len == 0)
-		return 0;
-
-	if (fw_pos + cmd_len > fw_entry->size) {
-		dev_err(info->dev, "Corrupted firmware image 2\n");
-		return -EMSGSIZE;
-	}
-
-	*skb = bt_skb_alloc(cmd_len, how);
-	if (!*skb) {
-		dev_err(info->dev, "Cannot reserve memory for buffer\n");
-		return -ENOMEM;
-	}
-	memcpy(skb_put(*skb, cmd_len), &fw_entry->data[fw_pos], cmd_len);
-
-	fw_pos += cmd_len;
-
-	return (*skb)->len;
-}
-
-int hci_h4p_read_fw(struct hci_h4p_info *info, struct sk_buff_head *fw_queue)
-{
-	const struct firmware *fw_entry = NULL;
-	struct sk_buff *skb = NULL;
-	int err;
-
-	err = hci_h4p_open_firmware(info, &fw_entry);
-	if (err < 0 || !fw_entry)
-		goto err_clean;
-
-	while ((err = hci_h4p_read_fw_cmd(info, &skb, fw_entry, GFP_KERNEL))) {
-		if (err < 0 || !skb)
-			goto err_clean;
-
-		skb_queue_tail(fw_queue, skb);
-	}
-
-	/* Chip detection code does neg and alive stuff
-	 * discard two first skbs */
-	skb = skb_dequeue(fw_queue);
-	if (!skb) {
-		err = -EMSGSIZE;
-		goto err_clean;
-	}
-	kfree_skb(skb);
-	skb = skb_dequeue(fw_queue);
-	if (!skb) {
-		err = -EMSGSIZE;
-		goto err_clean;
-	}
-	kfree_skb(skb);
-
-err_clean:
-	hci_h4p_close_firmware(fw_entry);
-	return err;
-}
-
-int hci_h4p_send_fw(struct hci_h4p_info *info, struct sk_buff_head *fw_queue)
-{
-	int err;
-
-	switch (info->man_id) {
-	case H4P_ID_CSR:
-		err = hci_h4p_bc4_send_fw(info, fw_queue);
-		break;
-	case H4P_ID_TI1271:
-		err = hci_h4p_ti1273_send_fw(info, fw_queue);
-		break;
-	case H4P_ID_BCM2048:
-		err = hci_h4p_bcm_send_fw(info, fw_queue);
-		break;
-	default:
-		dev_err(info->dev, "Don't know how to send firmware\n");
-		err = -EINVAL;
-	}
-
-	return err;
-}
-
-void hci_h4p_parse_fw_event(struct hci_h4p_info *info, struct sk_buff *skb)
-{
-	switch (info->man_id) {
-	case H4P_ID_CSR:
-		hci_h4p_bc4_parse_fw_event(info, skb);
-		break;
-	case H4P_ID_TI1271:
-		hci_h4p_ti1273_parse_fw_event(info, skb);
-		break;
-	case H4P_ID_BCM2048:
-		hci_h4p_bcm_parse_fw_event(info, skb);
-		break;
-	default:
-		dev_err(info->dev, "Don't know how to parse fw event\n");
-		info->fw_error = -EINVAL;
-	}
-
-	return;
-}
-
-MODULE_FIRMWARE(FW_NAME_TI1271_PRELE);
-MODULE_FIRMWARE(FW_NAME_TI1271_LE);
-MODULE_FIRMWARE(FW_NAME_TI1271);
-MODULE_FIRMWARE(FW_NAME_BCM2048);
-MODULE_FIRMWARE(FW_NAME_CSR);
diff --git a/drivers/staging/nokia_h4p/nokia_uart.c b/drivers/staging/nokia_h4p/nokia_uart.c
deleted file mode 100644
index 0fb57de4b750..000000000000
--- a/drivers/staging/nokia_h4p/nokia_uart.c
+++ /dev/null
@@ -1,199 +0,0 @@
-/*
- * This file is part of Nokia H4P bluetooth driver
- *
- * Copyright (C) 2005, 2006 Nokia Corporation.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * version 2 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- * General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
- * 02110-1301 USA
- *
- */
-
-#include <linux/serial_reg.h>
-#include <linux/delay.h>
-#include <linux/clk.h>
-
-#include <linux/io.h>
-
-#include "hci_h4p.h"
-
-inline void hci_h4p_outb(struct hci_h4p_info *info, unsigned int offset, u8 val)
-{
-	__raw_writeb(val, info->uart_base + (offset << 2));
-}
-
-inline u8 hci_h4p_inb(struct hci_h4p_info *info, unsigned int offset)
-{
-	return __raw_readb(info->uart_base + (offset << 2));
-}
-
-void hci_h4p_set_rts(struct hci_h4p_info *info, int active)
-{
-	u8 b;
-
-	b = hci_h4p_inb(info, UART_MCR);
-	if (active)
-		b |= UART_MCR_RTS;
-	else
-		b &= ~UART_MCR_RTS;
-	hci_h4p_outb(info, UART_MCR, b);
-}
-
-int hci_h4p_wait_for_cts(struct hci_h4p_info *info, int active,
-			 int timeout_ms)
-{
-	unsigned long timeout;
-	int state;
-
-	timeout = jiffies + msecs_to_jiffies(timeout_ms);
-	for (;;) {
-		state = hci_h4p_inb(info, UART_MSR) & UART_MSR_CTS;
-		if (active) {
-			if (state)
-				return 0;
-		} else {
-			if (!state)
-				return 0;
-		}
-		if (time_after(jiffies, timeout))
-			return -ETIMEDOUT;
-		msleep(1);
-	}
-}
-
-void __hci_h4p_set_auto_ctsrts(struct hci_h4p_info *info, int on, u8 which)
-{
-	u8 lcr, b;
-
-	lcr = hci_h4p_inb(info, UART_LCR);
-	hci_h4p_outb(info, UART_LCR, 0xbf);
-	b = hci_h4p_inb(info, UART_EFR);
-	if (on)
-		b |= which;
-	else
-		b &= ~which;
-	hci_h4p_outb(info, UART_EFR, b);
-	hci_h4p_outb(info, UART_LCR, lcr);
-}
-
-void hci_h4p_set_auto_ctsrts(struct hci_h4p_info *info, int on, u8 which)
-{
-	unsigned long flags;
-
-	spin_lock_irqsave(&info->lock, flags);
-	__hci_h4p_set_auto_ctsrts(info, on, which);
-	spin_unlock_irqrestore(&info->lock, flags);
-}
-
-void hci_h4p_change_speed(struct hci_h4p_info *info, unsigned long speed)
-{
-	unsigned int divisor;
-	u8 lcr, mdr1;
-
-	BT_DBG("Setting speed %lu", speed);
-
-	if (speed >= 460800) {
-		divisor = UART_CLOCK / 13 / speed;
-		mdr1 = 3;
-	} else {
-		divisor = UART_CLOCK / 16 / speed;
-		mdr1 = 0;
-	}
-
-	/* Make sure UART mode is disabled */
-	hci_h4p_outb(info, UART_OMAP_MDR1, 7);
-
-	lcr = hci_h4p_inb(info, UART_LCR);
-	hci_h4p_outb(info, UART_LCR, UART_LCR_DLAB);     /* Set DLAB */
-	hci_h4p_outb(info, UART_DLL, divisor & 0xff);    /* Set speed */
-	hci_h4p_outb(info, UART_DLM, divisor >> 8);
-	hci_h4p_outb(info, UART_LCR, lcr);
-
-	/* Make sure UART mode is enabled */
-	hci_h4p_outb(info, UART_OMAP_MDR1, mdr1);
-}
-
-int hci_h4p_reset_uart(struct hci_h4p_info *info)
-{
-	int count = 0;
-
-	/* Reset the UART */
-	hci_h4p_outb(info, UART_OMAP_SYSC, UART_SYSC_OMAP_RESET);
-	while (!(hci_h4p_inb(info, UART_OMAP_SYSS) & UART_SYSS_RESETDONE)) {
-		if (count++ > 100) {
-			dev_err(info->dev, "hci_h4p: UART reset timeout\n");
-			return -ENODEV;
-		}
-		udelay(1);
-	}
-
-	return 0;
-}
-
-void hci_h4p_store_regs(struct hci_h4p_info *info)
-{
-	u16 lcr = 0;
-
-	lcr = hci_h4p_inb(info, UART_LCR);
-	hci_h4p_outb(info, UART_LCR, 0xBF);
-	info->dll = hci_h4p_inb(info, UART_DLL);
-	info->dlh = hci_h4p_inb(info, UART_DLM);
-	info->efr = hci_h4p_inb(info, UART_EFR);
-	hci_h4p_outb(info, UART_LCR, lcr);
-	info->mdr1 = hci_h4p_inb(info, UART_OMAP_MDR1);
-	info->ier = hci_h4p_inb(info, UART_IER);
-}
-
-void hci_h4p_restore_regs(struct hci_h4p_info *info)
-{
-	u16 lcr = 0;
-
-	hci_h4p_init_uart(info);
-
-	hci_h4p_outb(info, UART_OMAP_MDR1, 7);
-	lcr = hci_h4p_inb(info, UART_LCR);
-	hci_h4p_outb(info, UART_LCR, 0xBF);
-	hci_h4p_outb(info, UART_DLL, info->dll);    /* Set speed */
-	hci_h4p_outb(info, UART_DLM, info->dlh);
-	hci_h4p_outb(info, UART_EFR, info->efr);
-	hci_h4p_outb(info, UART_LCR, lcr);
-	hci_h4p_outb(info, UART_OMAP_MDR1, info->mdr1);
-	hci_h4p_outb(info, UART_IER, info->ier);
-}
-
-void hci_h4p_init_uart(struct hci_h4p_info *info)
-{
-	u8 mcr, efr;
-
-	/* Enable and setup FIFO */
-	hci_h4p_outb(info, UART_OMAP_MDR1, 0x00);
-
-	hci_h4p_outb(info, UART_LCR, 0xbf);
-	efr = hci_h4p_inb(info, UART_EFR);
-	hci_h4p_outb(info, UART_EFR, UART_EFR_ECB);
-	hci_h4p_outb(info, UART_LCR, UART_LCR_DLAB);
-	mcr = hci_h4p_inb(info, UART_MCR);
-	hci_h4p_outb(info, UART_MCR, UART_MCR_TCRTLR);
-	hci_h4p_outb(info, UART_FCR, UART_FCR_ENABLE_FIFO |
-			UART_FCR_CLEAR_RCVR | UART_FCR_CLEAR_XMIT |
-			(3 << 6) | (0 << 4));
-	hci_h4p_outb(info, UART_LCR, 0xbf);
-	hci_h4p_outb(info, UART_TI752_TLR, 0xed);
-	hci_h4p_outb(info, UART_TI752_TCR, 0xef);
-	hci_h4p_outb(info, UART_EFR, efr);
-	hci_h4p_outb(info, UART_LCR, UART_LCR_DLAB);
-	hci_h4p_outb(info, UART_MCR, 0x00);
-	hci_h4p_outb(info, UART_LCR, UART_LCR_WLEN8);
-	hci_h4p_outb(info, UART_IER, UART_IER_RDI);
-	hci_h4p_outb(info, UART_OMAP_SYSC, (1 << 0) | (1 << 2) | (2 << 3));
-}
diff --git a/drivers/staging/octeon-usb/octeon-hcd.c b/drivers/staging/octeon-usb/octeon-hcd.c
index 095cc146eefc..5f9db4cbb381 100644
--- a/drivers/staging/octeon-usb/octeon-hcd.c
+++ b/drivers/staging/octeon-usb/octeon-hcd.c
@@ -170,27 +170,6 @@ struct cvmx_usb_port_status {
 };
 
 /**
- * union cvmx_usb_control_header - the structure of a Control packet header
- *
- * @s.request_type:	Bit 7 tells the direction: 1=IN, 0=OUT
- * @s.request		The standard usb request to make
- * @s.value		Value parameter for the request in little endian format
- * @s.index		Index for the request in little endian format
- * @s.length		Length of the data associated with this request in
- *			little endian format
- */
-union cvmx_usb_control_header {
-	uint64_t u64;
-	struct {
-		uint64_t request_type   : 8;
-		uint64_t request        : 8;
-		uint64_t value          : 16;
-		uint64_t index          : 16;
-		uint64_t length         : 16;
-	} s;
-};
-
-/**
  * struct cvmx_usb_iso_packet - descriptor for Isochronous packets
  *
  * @offset:	This is the offset in bytes into the main buffer where this data
@@ -1353,8 +1332,6 @@ static void __cvmx_usb_poll_rx_fifo(struct cvmx_usb_state *usb)
 		bytes -= 4;
 	}
 	CVMX_SYNCW;
-
-	return;
 }
 
 
@@ -1458,8 +1435,6 @@ static void __cvmx_usb_poll_tx_fifo(struct cvmx_usb_state *usb)
 					union cvmx_usbcx_gintmsk,
 					nptxfempmsk, 0);
 	}
-
-	return;
 }
 
 
@@ -1511,8 +1486,6 @@ static void __cvmx_usb_fill_tx_fifo(struct cvmx_usb_state *usb, int channel)
 		fifo->head = 0;
 
 	__cvmx_usb_poll_tx_fifo(usb);
-
-	return;
 }
 
 /**
@@ -1531,7 +1504,7 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	struct cvmx_usb_transaction *transaction =
 		list_first_entry(&pipe->transactions, typeof(*transaction),
 				 node);
-	union cvmx_usb_control_header *header =
+	struct usb_ctrlrequest *header =
 		cvmx_phys_to_ptr(transaction->control_header);
 	int bytes_to_transfer = transaction->buffer_length -
 		transaction->actual_bytes;
@@ -1575,24 +1548,24 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 	case CVMX_USB_STAGE_DATA:
 		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
 		if (__cvmx_usb_pipe_needs_split(usb, pipe)) {
-			if (header->s.request_type & 0x80)
+			if (header->bRequestType & USB_DIR_IN)
 				bytes_to_transfer = 0;
 			else if (bytes_to_transfer > pipe->max_packet)
 				bytes_to_transfer = pipe->max_packet;
 		}
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
 				union cvmx_usbcx_hccharx, epdir,
-				((header->s.request_type & 0x80) ?
+				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_IN :
 					CVMX_USB_DIRECTION_OUT));
 		break;
 	case CVMX_USB_STAGE_DATA_SPLIT_COMPLETE:
 		usbc_hctsiz.s.pid = __cvmx_usb_get_data_pid(pipe);
-		if (!(header->s.request_type & 0x80))
+		if (!(header->bRequestType & USB_DIR_IN))
 			bytes_to_transfer = 0;
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
 				union cvmx_usbcx_hccharx, epdir,
-				((header->s.request_type & 0x80) ?
+				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_IN :
 					CVMX_USB_DIRECTION_OUT));
 		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index),
@@ -1603,7 +1576,7 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 		bytes_to_transfer = 0;
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
 				union cvmx_usbcx_hccharx, epdir,
-				((header->s.request_type & 0x80) ?
+				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_OUT :
 					CVMX_USB_DIRECTION_IN));
 		break;
@@ -1612,7 +1585,7 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 		bytes_to_transfer = 0;
 		USB_SET_FIELD32(CVMX_USBCX_HCCHARX(channel, usb->index),
 				union cvmx_usbcx_hccharx, epdir,
-				((header->s.request_type & 0x80) ?
+				((header->bRequestType & USB_DIR_IN) ?
 					CVMX_USB_DIRECTION_OUT :
 					CVMX_USB_DIRECTION_IN));
 		USB_SET_FIELD32(CVMX_USBCX_HCSPLTX(channel, usb->index),
@@ -1661,7 +1634,6 @@ static void __cvmx_usb_start_channel_control(struct cvmx_usb_state *usb,
 
 	__cvmx_usb_write_csr32(usb, CVMX_USBCX_HCTSIZX(channel, usb->index),
 			       usbc_hctsiz.u32);
-	return;
 }
 
 
@@ -2008,7 +1980,6 @@ static void __cvmx_usb_start_channel(struct cvmx_usb_state *usb,
 			union cvmx_usbcx_hccharx, chena, 1);
 	if (usb->init_flags & CVMX_USB_INITIALIZE_FLAGS_NO_DMA)
 		__cvmx_usb_fill_tx_fifo(usb, channel);
-	return;
 }
 
 
@@ -2137,7 +2108,6 @@ done:
 	}
 	USB_SET_FIELD32(CVMX_USBCX_GINTMSK(usb->index),
 			union cvmx_usbcx_gintmsk, sofmsk, need_sof);
-	return;
 }
 
 static inline struct octeon_hcd *cvmx_usb_to_octeon(struct cvmx_usb_state *p)
@@ -2283,7 +2253,7 @@ static void __cvmx_usb_perform_complete(
 			/* Increment to the next location in our packet array */
 			transaction->iso_packets++;
 			transaction->stage = CVMX_USB_STAGE_NON_CONTROL;
-			goto done;
+			return;
 		}
 	}
 
@@ -2296,8 +2266,6 @@ static void __cvmx_usb_perform_complete(
 					 transaction->actual_bytes,
 					 transaction->urb);
 	kfree(transaction);
-done:
-	return;
 }
 
 
@@ -2443,11 +2411,10 @@ static struct cvmx_usb_transaction *cvmx_usb_submit_control(
 {
 	int buffer_length = urb->transfer_buffer_length;
 	uint64_t control_header = urb->setup_dma;
-	union cvmx_usb_control_header *header =
-		cvmx_phys_to_ptr(control_header);
+	struct usb_ctrlrequest *header = cvmx_phys_to_ptr(control_header);
 
-	if ((header->s.request_type & 0x80) == 0)
-		buffer_length = le16_to_cpu(header->s.length);
+	if ((header->bRequestType & USB_DIR_IN) == 0)
+		buffer_length = le16_to_cpu(header->wLength);
 
 	return __cvmx_usb_submit_transaction(usb, pipe,
 					     CVMX_USB_TRANSFER_CONTROL,
@@ -2911,9 +2878,9 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 					transaction->stage =
 						CVMX_USB_STAGE_SETUP_SPLIT_COMPLETE;
 				else {
-					union cvmx_usb_control_header *header =
+					struct usb_ctrlrequest *header =
 						cvmx_phys_to_ptr(transaction->control_header);
-					if (header->s.length)
+					if (header->wLength)
 						transaction->stage = CVMX_USB_STAGE_DATA;
 					else
 						transaction->stage = CVMX_USB_STAGE_STATUS;
@@ -2921,9 +2888,9 @@ static int __cvmx_usb_poll_channel(struct cvmx_usb_state *usb, int channel)
 				break;
 			case CVMX_USB_STAGE_SETUP_SPLIT_COMPLETE:
 				{
-					union cvmx_usb_control_header *header =
+					struct usb_ctrlrequest *header =
 						cvmx_phys_to_ptr(transaction->control_header);
-					if (header->s.length)
+					if (header->wLength)
 						transaction->stage = CVMX_USB_STAGE_DATA;
 					else
 						transaction->stage = CVMX_USB_STAGE_STATUS;
diff --git a/drivers/staging/octeon/ethernet-rgmii.c b/drivers/staging/octeon/ethernet-rgmii.c
index 651be7e1a8a7..e36f9bc69543 100644
--- a/drivers/staging/octeon/ethernet-rgmii.c
+++ b/drivers/staging/octeon/ethernet-rgmii.c
@@ -83,6 +83,7 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 			int interface = INTERFACE(priv->port);
 			int index = INDEX(priv->port);
 			union cvmx_gmxx_rxx_int_reg gmxx_rxx_int_reg;
+
 			gmxx_rxx_int_reg.u64 =
 			    cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
 					  (index, interface));
@@ -120,8 +121,7 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 				cvmx_write_csr(CVMX_GMXX_RXX_INT_REG
 					       (index, interface),
 					       gmxx_rxx_int_reg.u64);
-				printk_ratelimited("%s: Using 10Mbps with software "
-						   "preamble removal\n",
+				printk_ratelimited("%s: Using 10Mbps with software preamble removal\n",
 						   dev->name);
 			}
 		}
@@ -177,15 +177,13 @@ static void cvm_oct_rgmii_poll(struct net_device *dev)
 			if (!netif_carrier_ok(dev))
 				netif_carrier_on(dev);
 			if (priv->queue != -1)
-				printk_ratelimited("%s: %u Mbps %s duplex, "
-						   "port %2d, queue %2d\n",
+				printk_ratelimited("%s: %u Mbps %s duplex, port %2d, queue %2d\n",
 						   dev->name, link_info.s.speed,
 						   (link_info.s.full_duplex) ?
 						   "Full" : "Half",
 						   priv->port, priv->queue);
 			else
-				printk_ratelimited("%s: %u Mbps %s duplex, "
-						   "port %2d, POW\n",
+				printk_ratelimited("%s: %u Mbps %s duplex, port %2d, POW\n",
 						   dev->name, link_info.s.speed,
 						   (link_info.s.full_duplex) ?
 						   "Full" : "Half",
@@ -217,6 +215,7 @@ static irqreturn_t cvm_oct_rgmii_rml_interrupt(int cpl, void *dev_id)
 
 			/* Read the GMX interrupt status bits */
 			union cvmx_gmxx_rxx_int_reg gmx_rx_int_reg;
+
 			gmx_rx_int_reg.u64 =
 			    cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
 					  (index, interface));
@@ -261,6 +260,7 @@ static irqreturn_t cvm_oct_rgmii_rml_interrupt(int cpl, void *dev_id)
 
 			/* Read the GMX interrupt status bits */
 			union cvmx_gmxx_rxx_int_reg gmx_rx_int_reg;
+
 			gmx_rx_int_reg.u64 =
 			    cvmx_read_csr(CVMX_GMXX_RXX_INT_REG
 					  (index, interface));
@@ -316,6 +316,7 @@ int cvm_oct_rgmii_open(struct net_device *dev)
 	if (!octeon_is_simulation()) {
 		if (priv->phydev) {
 			int r = phy_read_status(priv->phydev);
+
 			if (r == 0 && priv->phydev->link == 0)
 				netif_carrier_off(dev);
 			cvm_oct_adjust_link(dev);
@@ -407,6 +408,7 @@ int cvm_oct_rgmii_init(struct net_device *dev)
 void cvm_oct_rgmii_uninit(struct net_device *dev)
 {
 	struct octeon_ethernet *priv = netdev_priv(dev);
+
 	cvm_oct_common_uninit(dev);
 
 	/*
diff --git a/drivers/staging/octeon/ethernet-rx.c b/drivers/staging/octeon/ethernet-rx.c
index a0f4868cfa13..b2b6c3cd2bed 100644
--- a/drivers/staging/octeon/ethernet-rx.c
+++ b/drivers/staging/octeon/ethernet-rx.c
@@ -203,8 +203,7 @@ static inline int cvm_oct_check_rcv_error(cvmx_wqe_t *work)
 					ptr++;
 				}
 			} else {
-				printk_ratelimited("Port %d unknown preamble, packet "
-						   "dropped\n",
+				printk_ratelimited("Port %d unknown preamble, packet dropped\n",
 						   work->ipprt);
 				/*
 				   cvmx_helper_dump_packet(work);
diff --git a/drivers/staging/octeon/ethernet-sgmii.c b/drivers/staging/octeon/ethernet-sgmii.c
index e1878449e683..21a7a17acb79 100644
--- a/drivers/staging/octeon/ethernet-sgmii.c
+++ b/drivers/staging/octeon/ethernet-sgmii.c
@@ -99,6 +99,7 @@ int cvm_oct_sgmii_open(struct net_device *dev)
 
 	if (priv->phydev) {
 		int r = phy_read_status(priv->phydev);
+
 		if (r == 0 && priv->phydev->link == 0)
 			netif_carrier_off(dev);
 		cvm_oct_adjust_link(dev);
diff --git a/drivers/staging/octeon/ethernet-util.h b/drivers/staging/octeon/ethernet-util.h
index 2da5ce17ead0..0f9b4a18fc27 100644
--- a/drivers/staging/octeon/ethernet-util.h
+++ b/drivers/staging/octeon/ethernet-util.h
@@ -53,8 +53,7 @@ static inline int INTERFACE(int ipd_port)
 		return 3;
 	else if (ipd_port == 40)	/* Non existent interface for POW0 */
 		return 4;
-	else
-		panic("Illegal ipd_port %d passed to INTERFACE\n", ipd_port);
+	panic("Illegal ipd_port %d passed to INTERFACE\n", ipd_port);
 }
 
 /**
@@ -67,6 +66,5 @@ static inline int INDEX(int ipd_port)
 {
 	if (ipd_port < 32)
 		return ipd_port & 15;
-	else
-		return ipd_port & 3;
+	return ipd_port & 3;
 }
diff --git a/drivers/staging/octeon/ethernet-xaui.c b/drivers/staging/octeon/ethernet-xaui.c
index 3cc286ee3599..fd9d103d8e56 100644
--- a/drivers/staging/octeon/ethernet-xaui.c
+++ b/drivers/staging/octeon/ethernet-xaui.c
@@ -99,6 +99,7 @@ int cvm_oct_xaui_open(struct net_device *dev)
 
 	if (priv->phydev) {
 		int r = phy_read_status(priv->phydev);
+
 		if (r == 0 && priv->phydev->link == 0)
 			netif_carrier_off(dev);
 		cvm_oct_adjust_link(dev);
diff --git a/drivers/staging/octeon/ethernet.c b/drivers/staging/octeon/ethernet.c
index 2aa723562155..8f9e3fb4871d 100644
--- a/drivers/staging/octeon/ethernet.c
+++ b/drivers/staging/octeon/ethernet.c
@@ -305,6 +305,7 @@ static int cvm_oct_common_change_mtu(struct net_device *dev, int new_mtu)
 			 * than the MTU and smaller the 64 bytes.
 			 */
 			union cvmx_pip_frm_len_chkx frm_len_chk;
+
 			frm_len_chk.u64 = 0;
 			frm_len_chk.s.minlen = 64;
 			frm_len_chk.s.maxlen = max_packet;
@@ -337,6 +338,7 @@ static void cvm_oct_common_set_multicast_list(struct net_device *dev)
 	    && (cvmx_helper_interface_get_mode(interface) !=
 		CVMX_HELPER_INTERFACE_MODE_SPI)) {
 		union cvmx_gmxx_rxx_adr_ctl control;
+
 		control.u64 = 0;
 		control.s.bcst = 1;	/* Allow broadcast MAC addresses */
 
@@ -397,6 +399,7 @@ static int cvm_oct_set_mac_filter(struct net_device *dev)
 		int i;
 		uint8_t *ptr = dev->dev_addr;
 		uint64_t mac = 0;
+
 		for (i = 0; i < 6; i++)
 			mac = (mac << 8) | (uint64_t)ptr[i];
 
@@ -648,6 +651,7 @@ static int cvm_oct_probe(struct platform_device *pdev)
 		     port < cvmx_helper_get_ipd_port(interface, num_ports);
 		     port++) {
 			union cvmx_pip_prt_tagx pip_prt_tagx;
+
 			pip_prt_tagx.u64 =
 			    cvmx_read_csr(CVMX_PIP_PRT_TAGX(port));
 			pip_prt_tagx.s.grp = pow_receive_group;
@@ -671,6 +675,7 @@ static int cvm_oct_probe(struct platform_device *pdev)
 
 	if ((pow_send_group != -1)) {
 		struct net_device *dev;
+
 		pr_info("\tConfiguring device for POW only access\n");
 		dev = alloc_etherdev(sizeof(struct octeon_ethernet));
 		if (dev) {
@@ -715,7 +720,8 @@ static int cvm_oct_probe(struct platform_device *pdev)
 			struct net_device *dev =
 			    alloc_etherdev(sizeof(struct octeon_ethernet));
 			if (!dev) {
-				pr_err("Failed to allocate ethernet device for port %d\n", port);
+				pr_err("Failed to allocate ethernet device for port %d\n",
+				       port);
 				continue;
 			}
 
@@ -780,8 +786,7 @@ static int cvm_oct_probe(struct platform_device *pdev)
 			if (!dev->netdev_ops) {
 				free_netdev(dev);
 			} else if (register_netdev(dev) < 0) {
-				pr_err("Failed to register ethernet device "
-					 "for interface %d, port %d\n",
+				pr_err("Failed to register ethernet device for interface %d, port %d\n",
 					 interface, priv->port);
 				free_netdev(dev);
 			} else {
@@ -833,6 +838,7 @@ static int cvm_oct_remove(struct platform_device *pdev)
 		if (cvm_oct_device[port]) {
 			struct net_device *dev = cvm_oct_device[port];
 			struct octeon_ethernet *priv = netdev_priv(dev);
+
 			cancel_delayed_work_sync(&priv->port_periodic_work);
 
 			cvm_oct_tx_shutdown_dev(dev);
diff --git a/drivers/staging/ozwpan/ozcdev.c b/drivers/staging/ozwpan/ozcdev.c
index 10c0a96ce8bb..da0e1fd50f26 100644
--- a/drivers/staging/ozwpan/ozcdev.c
+++ b/drivers/staging/ozwpan/ozcdev.c
@@ -49,11 +49,11 @@ static struct oz_serial_ctx *oz_cdev_claim_ctx(struct oz_pd *pd)
 {
 	struct oz_serial_ctx *ctx;
 
-	spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
-	ctx = (struct oz_serial_ctx *)pd->app_ctx[OZ_APPID_SERIAL-1];
+	spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
+	ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL];
 	if (ctx)
 		atomic_inc(&ctx->ref_count);
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
 	return ctx;
 }
 
@@ -182,8 +182,8 @@ static ssize_t oz_cdev_write(struct file *filp, const char __user *buf,
 	app_hdr->app_id = OZ_APPID_SERIAL;
 	if (copy_from_user(app_hdr+1, buf, count))
 		goto out;
-	spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]);
-	ctx = (struct oz_serial_ctx *)pd->app_ctx[OZ_APPID_SERIAL-1];
+	spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
+	ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL];
 	if (ctx) {
 		app_hdr->elt_seq_num = ctx->tx_seq_num++;
 		if (ctx->tx_seq_num == 0)
@@ -193,7 +193,7 @@ static ssize_t oz_cdev_write(struct file *filp, const char __user *buf,
 			ei = NULL;
 		spin_unlock(&eb->lock);
 	}
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]);
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
 out:
 	if (ei) {
 		count = 0;
@@ -263,6 +263,7 @@ static long oz_cdev_ioctl(struct file *filp, unsigned int cmd,
 	switch (cmd) {
 	case OZ_IOCTL_GET_PD_LIST: {
 			struct oz_pd_list list;
+
 			oz_dbg(ON, "OZ_IOCTL_GET_PD_LIST\n");
 			memset(&list, 0, sizeof(list));
 			list.count = oz_get_pd_list(list.addr, OZ_MAX_PDS);
@@ -273,6 +274,7 @@ static long oz_cdev_ioctl(struct file *filp, unsigned int cmd,
 		break;
 	case OZ_IOCTL_SET_ACTIVE_PD: {
 			u8 addr[ETH_ALEN];
+
 			oz_dbg(ON, "OZ_IOCTL_SET_ACTIVE_PD\n");
 			if (copy_from_user(addr, (void __user *)arg, ETH_ALEN))
 				return -EFAULT;
@@ -281,6 +283,7 @@ static long oz_cdev_ioctl(struct file *filp, unsigned int cmd,
 		break;
 	case OZ_IOCTL_GET_ACTIVE_PD: {
 			u8 addr[ETH_ALEN];
+
 			oz_dbg(ON, "OZ_IOCTL_GET_ACTIVE_PD\n");
 			spin_lock_bh(&g_cdev.lock);
 			ether_addr_copy(addr, g_cdev.active_addr);
@@ -292,6 +295,7 @@ static long oz_cdev_ioctl(struct file *filp, unsigned int cmd,
 	case OZ_IOCTL_ADD_BINDING:
 	case OZ_IOCTL_REMOVE_BINDING: {
 			struct oz_binding_info b;
+
 			if (copy_from_user(&b, (void __user *)arg,
 				sizeof(struct oz_binding_info))) {
 				return -EFAULT;
@@ -320,6 +324,7 @@ static unsigned int oz_cdev_poll(struct file *filp, poll_table *wait)
 	spin_lock_bh(&dev->lock);
 	if (dev->active_pd) {
 		struct oz_serial_ctx *ctx = oz_cdev_claim_ctx(dev->active_pd);
+
 		if (ctx) {
 			if (ctx->rd_in != ctx->rd_out)
 				ret |= POLLIN | POLLRDNORM;
@@ -360,7 +365,6 @@ int oz_cdev_register(void)
 	       MAJOR(g_cdev.devnum), MINOR(g_cdev.devnum));
 	cdev_init(&g_cdev.cdev, &oz_fops);
 	g_cdev.cdev.owner = THIS_MODULE;
-	g_cdev.cdev.ops = &oz_fops;
 	spin_lock_init(&g_cdev.lock);
 	init_waitqueue_head(&g_cdev.rdq);
 	err = cdev_add(&g_cdev.cdev, g_cdev.devnum, 1);
@@ -437,14 +441,14 @@ int oz_cdev_start(struct oz_pd *pd, int resume)
 		return -ENOMEM;
 	atomic_set(&ctx->ref_count, 1);
 	ctx->tx_seq_num = 1;
-	spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
-	old_ctx = pd->app_ctx[OZ_APPID_SERIAL-1];
+	spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
+	old_ctx = pd->app_ctx[OZ_APPID_SERIAL];
 	if (old_ctx) {
-		spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
+		spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
 		kfree(ctx);
 	} else {
-		pd->app_ctx[OZ_APPID_SERIAL-1] = ctx;
-		spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
+		pd->app_ctx[OZ_APPID_SERIAL] = ctx;
+		spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
 	}
 	spin_lock(&g_cdev.lock);
 	if ((g_cdev.active_pd == NULL) &&
@@ -469,10 +473,10 @@ void oz_cdev_stop(struct oz_pd *pd, int pause)
 		oz_dbg(ON, "Serial service paused\n");
 		return;
 	}
-	spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
-	ctx = (struct oz_serial_ctx *)pd->app_ctx[OZ_APPID_SERIAL-1];
-	pd->app_ctx[OZ_APPID_SERIAL-1] = NULL;
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]);
+	spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
+	ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL];
+	pd->app_ctx[OZ_APPID_SERIAL] = NULL;
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
 	if (ctx)
 		oz_cdev_release_ctx(ctx);
 	spin_lock(&g_cdev.lock);
diff --git a/drivers/staging/ozwpan/ozeltbuf.c b/drivers/staging/ozwpan/ozeltbuf.c
index bd560c67fc8c..01b25da44241 100644
--- a/drivers/staging/ozwpan/ozeltbuf.c
+++ b/drivers/staging/ozwpan/ozeltbuf.c
@@ -10,21 +10,16 @@
 #include "ozeltbuf.h"
 #include "ozpd.h"
 
-#define OZ_ELT_INFO_MAGIC_USED	0x35791057
-#define OZ_ELT_INFO_MAGIC_FREE	0x78940102
-
 /*
  * Context: softirq-serialized
  */
-int oz_elt_buf_init(struct oz_elt_buf *buf)
+void oz_elt_buf_init(struct oz_elt_buf *buf)
 {
 	memset(buf, 0, sizeof(struct oz_elt_buf));
 	INIT_LIST_HEAD(&buf->stream_list);
 	INIT_LIST_HEAD(&buf->order_list);
 	INIT_LIST_HEAD(&buf->isoc_list);
-	buf->max_free_elts = 32;
 	spin_lock_init(&buf->lock);
-	return 0;
 }
 
 /*
@@ -32,32 +27,12 @@ int oz_elt_buf_init(struct oz_elt_buf *buf)
  */
 void oz_elt_buf_term(struct oz_elt_buf *buf)
 {
-	struct list_head *e;
-	int i;
+	struct oz_elt_info *ei, *n;
 
-	/* Free any elements in the order or isoc lists. */
-	for (i = 0; i < 2; i++) {
-		struct list_head *list;
-		if (i)
-			list = &buf->order_list;
-		else
-			list = &buf->isoc_list;
-		e = list->next;
-		while (e != list) {
-			struct oz_elt_info *ei =
-				container_of(e, struct oz_elt_info, link_order);
-			e = e->next;
-			kfree(ei);
-		}
-	}
-	/* Free any elelment in the pool. */
-	while (buf->elt_pool) {
-		struct oz_elt_info *ei =
-			container_of(buf->elt_pool, struct oz_elt_info, link);
-		buf->elt_pool = buf->elt_pool->next;
+	list_for_each_entry_safe(ei, n, &buf->isoc_list, link_order)
+		kfree(ei);
+	list_for_each_entry_safe(ei, n, &buf->order_list, link_order)
 		kfree(ei);
-	}
-	buf->free_elts = 0;
 }
 
 /*
@@ -67,27 +42,8 @@ struct oz_elt_info *oz_elt_info_alloc(struct oz_elt_buf *buf)
 {
 	struct oz_elt_info *ei;
 
-	spin_lock_bh(&buf->lock);
-	if (buf->free_elts && buf->elt_pool) {
-		ei = container_of(buf->elt_pool, struct oz_elt_info, link);
-		buf->elt_pool = ei->link.next;
-		buf->free_elts--;
-		spin_unlock_bh(&buf->lock);
-		if (ei->magic != OZ_ELT_INFO_MAGIC_FREE) {
-			oz_dbg(ON, "%s: ei with bad magic: 0x%x\n",
-			       __func__, ei->magic);
-		}
-	} else {
-		spin_unlock_bh(&buf->lock);
-		ei = kmalloc(sizeof(struct oz_elt_info), GFP_ATOMIC);
-	}
+	ei = kmem_cache_zalloc(oz_elt_info_cache, GFP_ATOMIC);
 	if (ei) {
-		ei->flags = 0;
-		ei->app_id = 0;
-		ei->callback = NULL;
-		ei->context = 0;
-		ei->stream = NULL;
-		ei->magic = OZ_ELT_INFO_MAGIC_USED;
 		INIT_LIST_HEAD(&ei->link);
 		INIT_LIST_HEAD(&ei->link_order);
 	}
@@ -100,17 +56,8 @@ struct oz_elt_info *oz_elt_info_alloc(struct oz_elt_buf *buf)
  */
 void oz_elt_info_free(struct oz_elt_buf *buf, struct oz_elt_info *ei)
 {
-	if (ei) {
-		if (ei->magic == OZ_ELT_INFO_MAGIC_USED) {
-			buf->free_elts++;
-			ei->link.next = buf->elt_pool;
-			buf->elt_pool = &ei->link;
-			ei->magic = OZ_ELT_INFO_MAGIC_FREE;
-		} else {
-			oz_dbg(ON, "%s: bad magic ei: %p magic: 0x%x\n",
-			       __func__, ei, ei->magic);
-		}
-	}
+	if (ei)
+		kmem_cache_free(oz_elt_info_cache, ei);
 }
 
 /*------------------------------------------------------------------------------
@@ -118,16 +65,11 @@ void oz_elt_info_free(struct oz_elt_buf *buf, struct oz_elt_info *ei)
  */
 void oz_elt_info_free_chain(struct oz_elt_buf *buf, struct list_head *list)
 {
-	struct list_head *e;
+	struct oz_elt_info *ei, *n;
 
-	e = list->next;
 	spin_lock_bh(&buf->lock);
-	while (e != list) {
-		struct oz_elt_info *ei;
-		ei = container_of(e, struct oz_elt_info, link);
-		e = e->next;
+	list_for_each_entry_safe(ei, n, list->next, link)
 		oz_elt_info_free(buf, ei);
-	}
 	spin_unlock_bh(&buf->lock);
 }
 
@@ -152,14 +94,13 @@ int oz_elt_stream_create(struct oz_elt_buf *buf, u8 id, int max_buf_count)
 
 int oz_elt_stream_delete(struct oz_elt_buf *buf, u8 id)
 {
-	struct list_head *e;
+	struct list_head *e, *n;
 	struct oz_elt_stream *st = NULL;
 
 	oz_dbg(ON, "%s: (0x%x)\n", __func__, id);
 	spin_lock_bh(&buf->lock);
-	e = buf->stream_list.next;
-	while (e != &buf->stream_list) {
-		st = container_of(e, struct oz_elt_stream, link);
+	list_for_each(e, &buf->stream_list) {
+		st = list_entry(e, struct oz_elt_stream, link);
 		if (st->id == id) {
 			list_del(e);
 			break;
@@ -170,11 +111,9 @@ int oz_elt_stream_delete(struct oz_elt_buf *buf, u8 id)
 		spin_unlock_bh(&buf->lock);
 		return -1;
 	}
-	e = st->elt_list.next;
-	while (e != &st->elt_list) {
+	list_for_each_safe(e, n, &st->elt_list) {
 		struct oz_elt_info *ei =
-			container_of(e, struct oz_elt_info, link);
-		e = e->next;
+			list_entry(e, struct oz_elt_info, link);
 		list_del_init(&ei->link);
 		list_del_init(&ei->link_order);
 		st->buf_count -= ei->length;
@@ -214,7 +153,7 @@ int oz_queue_elt_info(struct oz_elt_buf *buf, u8 isoc, u8 id,
 
 	if (id) {
 		list_for_each(e, &buf->stream_list) {
-			st = container_of(e, struct oz_elt_stream, link);
+			st = list_entry(e, struct oz_elt_stream, link);
 			if (st->id == id)
 				break;
 		}
@@ -235,6 +174,7 @@ int oz_queue_elt_info(struct oz_elt_buf *buf, u8 isoc, u8 id,
 			== OZ_USB_ENDPOINT_DATA) &&
 			(body->format == OZ_DATA_F_ISOC_FIXED)) {
 			u8 unit_count = body->frame_number;
+
 			body->frame_number = st->frame_number;
 			st->frame_number += unit_count;
 		}
@@ -269,22 +209,18 @@ int oz_select_elts_for_tx(struct oz_elt_buf *buf, u8 isoc, unsigned *len,
 		unsigned max_len, struct list_head *list)
 {
 	int count = 0;
-	struct list_head *e;
 	struct list_head *el;
-	struct oz_elt_info *ei;
+	struct oz_elt_info *ei, *n;
 
 	spin_lock_bh(&buf->lock);
 	if (isoc)
 		el = &buf->isoc_list;
 	else
 		el = &buf->order_list;
-	e = el->next;
-	while (e != el) {
-		struct oz_app_hdr *app_hdr;
-		ei = container_of(e, struct oz_elt_info, link_order);
-		e = e->next;
+
+	list_for_each_entry_safe(ei, n, el, link_order) {
 		if ((*len + ei->length) <= max_len) {
-			app_hdr = (struct oz_app_hdr *)
+			struct oz_app_hdr *app_hdr = (struct oz_app_hdr *)
 				&ei->data[sizeof(struct oz_elt)];
 			app_hdr->elt_seq_num = buf->tx_seq_num[ei->app_id]++;
 			if (buf->tx_seq_num[ei->app_id] == 0)
@@ -312,27 +248,5 @@ int oz_select_elts_for_tx(struct oz_elt_buf *buf, u8 isoc, unsigned *len,
 
 int oz_are_elts_available(struct oz_elt_buf *buf)
 {
-	return buf->order_list.next != &buf->order_list;
-}
-
-void oz_trim_elt_pool(struct oz_elt_buf *buf)
-{
-	struct list_head *free = NULL;
-	struct list_head *e;
-
-	spin_lock_bh(&buf->lock);
-	while (buf->free_elts > buf->max_free_elts) {
-		e = buf->elt_pool;
-		buf->elt_pool = e->next;
-		e->next = free;
-		free = e;
-		buf->free_elts--;
-	}
-	spin_unlock_bh(&buf->lock);
-	while (free) {
-		struct oz_elt_info *ei =
-			container_of(free, struct oz_elt_info, link);
-		free = free->next;
-		kfree(ei);
-	}
+	return !list_empty(&buf->order_list);
 }
diff --git a/drivers/staging/ozwpan/ozeltbuf.h b/drivers/staging/ozwpan/ozeltbuf.h
index 03c12f57b9bb..f09f5fe3ffbe 100644
--- a/drivers/staging/ozwpan/ozeltbuf.h
+++ b/drivers/staging/ozwpan/ozeltbuf.h
@@ -34,7 +34,6 @@ struct oz_elt_info {
 	struct oz_elt_stream *stream;
 	u8 data[sizeof(struct oz_elt) + OZ_MAX_ELT_PAYLOAD];
 	int length;
-	unsigned magic;
 };
 /* Flags values */
 #define OZ_EI_F_MARKED		0x1
@@ -44,13 +43,10 @@ struct oz_elt_buf {
 	struct list_head stream_list;
 	struct list_head order_list;
 	struct list_head isoc_list;
-	struct list_head *elt_pool;
-	int free_elts;
-	int max_free_elts;
 	u8 tx_seq_num[OZ_NB_APPS];
 };
 
-int oz_elt_buf_init(struct oz_elt_buf *buf);
+void oz_elt_buf_init(struct oz_elt_buf *buf);
 void oz_elt_buf_term(struct oz_elt_buf *buf);
 struct oz_elt_info *oz_elt_info_alloc(struct oz_elt_buf *buf);
 void oz_elt_info_free(struct oz_elt_buf *buf, struct oz_elt_info *ei);
@@ -64,7 +60,6 @@ int oz_queue_elt_info(struct oz_elt_buf *buf, u8 isoc, u8 id,
 int oz_select_elts_for_tx(struct oz_elt_buf *buf, u8 isoc, unsigned *len,
 		unsigned max_len, struct list_head *list);
 int oz_are_elts_available(struct oz_elt_buf *buf);
-void oz_trim_elt_pool(struct oz_elt_buf *buf);
 
 #endif /* _OZELTBUF_H */
 
diff --git a/drivers/staging/ozwpan/ozhcd.c b/drivers/staging/ozwpan/ozhcd.c
index 30bd9286d47d..e88045228607 100644
--- a/drivers/staging/ozwpan/ozhcd.c
+++ b/drivers/staging/ozwpan/ozhcd.c
@@ -45,14 +45,6 @@
  */
 #define OZ_PLAT_DEV_NAME	"ozwpan"
 
-/* Maximum number of free urb links that can be kept in the pool.
- */
-#define OZ_MAX_LINK_POOL_SIZE	16
-
-/* Get endpoint object from the containing link.
- */
-#define ep_from_link(__e) container_of((__e), struct oz_endpoint, link)
-
 /*EP0 timeout before ep0 request is again added to TX queue. (13*8 = 98mSec)
  */
 #define EP0_TIMEOUT_COUNTER 13
@@ -75,6 +67,8 @@ struct oz_urb_link {
 	unsigned submit_counter;
 };
 
+static struct kmem_cache *oz_urb_link_cache;
+
 /* Holds state information about a USB endpoint.
  */
 #define OZ_EP_BUFFER_SIZE_ISOC  (1024 * 24)
@@ -198,9 +192,6 @@ static struct platform_device *g_plat_dev;
 static struct oz_hcd *g_ozhcd;
 static DEFINE_SPINLOCK(g_hcdlock);	/* Guards g_ozhcd. */
 static const char g_hcd_name[] = "Ozmo WPAN";
-static struct list_head *g_link_pool;
-static int g_link_pool_size;
-static DEFINE_SPINLOCK(g_link_lock);
 static DEFINE_SPINLOCK(g_tasklet_lock);
 static struct tasklet_struct g_urb_process_tasklet;
 static struct tasklet_struct g_urb_cancel_tasklet;
@@ -265,68 +256,22 @@ static int oz_get_port_from_addr(struct oz_hcd *ozhcd, u8 bus_addr)
 }
 
 /*
- * Allocates an urb link, first trying the pool but going to heap if empty.
  * Context: any
  */
 static struct oz_urb_link *oz_alloc_urb_link(void)
 {
-	struct oz_urb_link *urbl = NULL;
-	unsigned long irq_state;
-
-	spin_lock_irqsave(&g_link_lock, irq_state);
-	if (g_link_pool) {
-		urbl = container_of(g_link_pool, struct oz_urb_link, link);
-		g_link_pool = urbl->link.next;
-		--g_link_pool_size;
-	}
-	spin_unlock_irqrestore(&g_link_lock, irq_state);
-	if (urbl == NULL)
-		urbl = kmalloc(sizeof(struct oz_urb_link), GFP_ATOMIC);
-	return urbl;
+	return kmem_cache_alloc(oz_urb_link_cache, GFP_ATOMIC);
 }
 
 /*
- * Frees an urb link by putting it in the pool if there is enough space or
- * deallocating it to heap otherwise.
  * Context: any
  */
 static void oz_free_urb_link(struct oz_urb_link *urbl)
 {
-	if (urbl) {
-		unsigned long irq_state;
-
-		spin_lock_irqsave(&g_link_lock, irq_state);
-		if (g_link_pool_size < OZ_MAX_LINK_POOL_SIZE) {
-			urbl->link.next = g_link_pool;
-			g_link_pool = &urbl->link;
-			urbl = NULL;
-			g_link_pool_size++;
-		}
-		spin_unlock_irqrestore(&g_link_lock, irq_state);
-		kfree(urbl);
-	}
-}
-
-/*
- * Deallocates all the urb links in the pool.
- * Context: unknown
- */
-static void oz_empty_link_pool(void)
-{
-	struct list_head *e;
-	unsigned long irq_state;
+	if (!urbl)
+		return;
 
-	spin_lock_irqsave(&g_link_lock, irq_state);
-	e = g_link_pool;
-	g_link_pool = NULL;
-	g_link_pool_size = 0;
-	spin_unlock_irqrestore(&g_link_lock, irq_state);
-	while (e) {
-		struct oz_urb_link *urbl =
-			container_of(e, struct oz_urb_link, link);
-		e = e->next;
-		kfree(urbl);
-	}
+	kmem_cache_free(oz_urb_link_cache, urbl);
 }
 
 /*
@@ -359,12 +304,10 @@ static struct oz_urb_link *oz_uncancel_urb(struct oz_hcd *ozhcd,
 		struct urb *urb)
 {
 	struct oz_urb_link *urbl;
-	struct list_head *e;
 
-	list_for_each(e, &ozhcd->urb_cancel_list) {
-		urbl = container_of(e, struct oz_urb_link, link);
+	list_for_each_entry(urbl, &ozhcd->urb_cancel_list, link) {
 		if (urb == urbl->urb) {
-			list_del_init(e);
+			list_del_init(&urbl->link);
 			return urbl;
 		}
 	}
@@ -423,10 +366,9 @@ static void oz_complete_urb(struct usb_hcd *hcd, struct urb *urb,
 static void oz_ep_free(struct oz_port *port, struct oz_endpoint *ep)
 {
 	if (port) {
-		struct list_head list;
+		LIST_HEAD(list);
 		struct oz_hcd *ozhcd = port->ozhcd;
 
-		INIT_LIST_HEAD(&list);
 		if (ep->flags & OZ_F_EP_HAVE_STREAM)
 			oz_usb_stream_delete(port->hpd, ep->ep_num);
 		/* Transfer URBs to the orphanage while we hold the lock. */
@@ -572,7 +514,7 @@ static int oz_dequeue_ep_urb(struct oz_port *port, u8 ep_addr, int in_dir,
 		struct list_head *e;
 
 		list_for_each(e, &ep->urb_list) {
-			urbl = container_of(e, struct oz_urb_link, link);
+			urbl = list_entry(e, struct oz_urb_link, link);
 			if (urbl->urb == urb) {
 				list_del_init(e);
 				break;
@@ -604,7 +546,7 @@ static struct urb *oz_find_urb_by_id(struct oz_port *port, int ep_ix,
 		struct list_head *e;
 
 		list_for_each(e, &ep->urb_list) {
-			urbl = container_of(e, struct oz_urb_link, link);
+			urbl = list_entry(e, struct oz_urb_link, link);
 			if (urbl->req_id == req_id) {
 				urb = urbl->urb;
 				list_del_init(e);
@@ -1068,10 +1010,9 @@ void oz_hcd_data_ind(void *hport, u8 endpoint, const u8 *data, int data_len)
 			urb->actual_length = copy_len;
 			oz_complete_urb(port->ozhcd->hcd, urb, 0);
 			return;
-		} else {
-			oz_dbg(ON, "buffering frame as URB is not available\n");
-			oz_hcd_buffer_data(ep, data, data_len);
 		}
+		oz_dbg(ON, "buffering frame as URB is not available\n");
+		oz_hcd_buffer_data(ep, data, data_len);
 		break;
 	case USB_ENDPOINT_XFER_ISOC:
 		oz_hcd_buffer_data(ep, data, data_len);
@@ -1097,21 +1038,17 @@ int oz_hcd_heartbeat(void *hport)
 	int rc = 0;
 	struct oz_port *port = (struct oz_port *)hport;
 	struct oz_hcd *ozhcd = port->ozhcd;
-	struct oz_urb_link *urbl;
-	struct list_head xfr_list;
-	struct list_head *e;
-	struct list_head *n;
+	struct oz_urb_link *urbl, *n;
+	LIST_HEAD(xfr_list);
 	struct urb *urb;
 	struct oz_endpoint *ep;
 	struct timespec ts, delta;
 
 	getrawmonotonic(&ts);
-	INIT_LIST_HEAD(&xfr_list);
 	/* Check the OUT isoc endpoints to see if any URB data can be sent.
 	 */
 	spin_lock_bh(&ozhcd->hcd_lock);
-	list_for_each(e, &port->isoc_out_ep) {
-		ep = ep_from_link(e);
+	list_for_each_entry(ep, &port->isoc_out_ep, link) {
 		if (ep->credit < 0)
 			continue;
 		delta = timespec_sub(ts, ep->timestamp);
@@ -1134,10 +1071,9 @@ int oz_hcd_heartbeat(void *hport)
 	spin_unlock_bh(&ozhcd->hcd_lock);
 	/* Send to PD and complete URBs.
 	 */
-	list_for_each_safe(e, n, &xfr_list) {
-		urbl = container_of(e, struct oz_urb_link, link);
+	list_for_each_entry_safe(urbl, n, &xfr_list, link) {
 		urb = urbl->urb;
-		list_del_init(e);
+		list_del_init(&urbl->link);
 		urb->error_count = 0;
 		urb->start_frame = oz_usb_get_frame_number();
 		oz_usb_send_isoc(port->hpd, urbl->ep_num, urb);
@@ -1147,9 +1083,7 @@ int oz_hcd_heartbeat(void *hport)
 	/* Check the IN isoc endpoints to see if any URBs can be completed.
 	 */
 	spin_lock_bh(&ozhcd->hcd_lock);
-	list_for_each(e, &port->isoc_in_ep) {
-		struct oz_endpoint *ep = ep_from_link(e);
-
+	list_for_each_entry(ep, &port->isoc_in_ep, link) {
 		if (ep->flags & OZ_F_EP_BUFFERING) {
 			if (ep->buffered_units >= OZ_IN_BUFFERING_UNITS) {
 				ep->flags &= ~OZ_F_EP_BUFFERING;
@@ -1162,10 +1096,7 @@ int oz_hcd_heartbeat(void *hport)
 		delta = timespec_sub(ts, ep->timestamp);
 		ep->credit += div_u64(timespec_to_ns(&delta), NSEC_PER_MSEC);
 		ep->timestamp = ts;
-		while (!list_empty(&ep->urb_list)) {
-			struct oz_urb_link *urbl =
-				list_first_entry(&ep->urb_list,
-					struct oz_urb_link, link);
+		list_for_each_entry_safe(urbl, n, &ep->urb_list, link) {
 			struct urb *urb = urbl->urb;
 			int len = 0;
 			int copy_len;
@@ -1212,10 +1143,9 @@ int oz_hcd_heartbeat(void *hport)
 	spin_unlock_bh(&ozhcd->hcd_lock);
 	/* Complete the filled URBs.
 	 */
-	list_for_each_safe(e, n, &xfr_list) {
-		urbl = container_of(e, struct oz_urb_link, link);
+	list_for_each_entry_safe(urbl, n, &xfr_list, link) {
 		urb = urbl->urb;
-		list_del_init(e);
+		list_del_init(&urbl->link);
 		oz_free_urb_link(urbl);
 		oz_complete_urb(port->ozhcd->hcd, urb, 0);
 	}
@@ -1224,15 +1154,11 @@ int oz_hcd_heartbeat(void *hport)
 	 */
 	ep = port->out_ep[0];
 	if (ep) {
-		struct list_head *e;
-		struct list_head *n;
-
 		spin_lock_bh(&ozhcd->hcd_lock);
-		list_for_each_safe(e, n, &ep->urb_list) {
-			urbl = container_of(e, struct oz_urb_link, link);
+		list_for_each_entry_safe(urbl, n, &ep->urb_list, link) {
 			if (urbl->submit_counter > EP0_TIMEOUT_COUNTER) {
 				oz_dbg(ON, "Request 0x%p timeout\n", urbl->urb);
-				list_move_tail(e, &xfr_list);
+				list_move_tail(&urbl->link, &xfr_list);
 				urbl->submit_counter = 0;
 			} else {
 				urbl->submit_counter++;
@@ -1241,10 +1167,7 @@ int oz_hcd_heartbeat(void *hport)
 		if (!list_empty(&ep->urb_list))
 			rc = 1;
 		spin_unlock_bh(&ozhcd->hcd_lock);
-		e = xfr_list.next;
-		while (e != &xfr_list) {
-			urbl = container_of(e, struct oz_urb_link, link);
-			e = e->next;
+		list_for_each_entry_safe(urbl, n, &xfr_list, link) {
 			oz_dbg(ON, "Resending request to PD\n");
 			oz_process_ep0_urb(ozhcd, urbl->urb, GFP_ATOMIC);
 			oz_free_urb_link(urbl);
@@ -1343,12 +1266,12 @@ static void oz_clean_endpoints_for_interface(struct usb_hcd *hcd,
 	struct oz_hcd *ozhcd = port->ozhcd;
 	unsigned mask;
 	int i;
-	struct list_head ep_list;
+	LIST_HEAD(ep_list);
+	struct oz_endpoint *ep, *n;
 
 	oz_dbg(ON, "Deleting endpoints for interface %d\n", if_ix);
 	if (if_ix >= port->num_iface)
 		return;
-	INIT_LIST_HEAD(&ep_list);
 	spin_lock_bh(&ozhcd->hcd_lock);
 	mask = port->iface[if_ix].ep_mask;
 	port->iface[if_ix].ep_mask = 0;
@@ -1372,9 +1295,7 @@ static void oz_clean_endpoints_for_interface(struct usb_hcd *hcd,
 		}
 	}
 	spin_unlock_bh(&ozhcd->hcd_lock);
-	while (!list_empty(&ep_list)) {
-		struct oz_endpoint *ep =
-			list_first_entry(&ep_list, struct oz_endpoint, link);
+	list_for_each_entry_safe(ep, n, &ep_list, link) {
 		list_del_init(&ep->link);
 		oz_ep_free(port, ep);
 	}
@@ -1394,8 +1315,8 @@ static int oz_build_endpoints_for_config(struct usb_hcd *hcd,
 	if (num_iface) {
 		struct oz_interface *iface;
 
-		iface = kmalloc(num_iface*sizeof(struct oz_interface),
-				mem_flags | __GFP_ZERO);
+		iface = kmalloc_array(num_iface, sizeof(struct oz_interface),
+					mem_flags | __GFP_ZERO);
 		if (!iface)
 			return -ENOMEM;
 		spin_lock_bh(&ozhcd->hcd_lock);
@@ -1645,6 +1566,7 @@ static void oz_urb_process_tasklet(unsigned long unused)
 	unsigned long irq_state;
 	struct urb *urb;
 	struct oz_hcd *ozhcd = oz_hcd_claim();
+	struct oz_urb_link *urbl, *n;
 	int rc = 0;
 
 	if (ozhcd == NULL)
@@ -1654,10 +1576,7 @@ static void oz_urb_process_tasklet(unsigned long unused)
 	 * appropriately while removing urbs.
 	 */
 	spin_lock_irqsave(&g_tasklet_lock, irq_state);
-	while (!list_empty(&ozhcd->urb_pending_list)) {
-		struct oz_urb_link *urbl =
-			list_first_entry(&ozhcd->urb_pending_list,
-				struct oz_urb_link, link);
+	list_for_each_entry_safe(urbl, n, &ozhcd->urb_pending_list, link) {
 		list_del_init(&urbl->link);
 		spin_unlock_irqrestore(&g_tasklet_lock, irq_state);
 		urb = urbl->urb;
@@ -1702,7 +1621,7 @@ static void oz_urb_cancel(struct oz_port *port, u8 ep_num, struct urb *urb)
 	 */
 	spin_lock_irqsave(&g_tasklet_lock, irq_state);
 	list_for_each(e, &ozhcd->urb_cancel_list) {
-		urbl = container_of(e, struct oz_urb_link, link);
+		urbl = list_entry(e, struct oz_urb_link, link);
 		if (urb == urbl->urb) {
 			list_del_init(e);
 			spin_unlock_irqrestore(&g_tasklet_lock, irq_state);
@@ -1716,7 +1635,7 @@ static void oz_urb_cancel(struct oz_port *port, u8 ep_num, struct urb *urb)
 	 */
 	spin_lock_irqsave(&ozhcd->hcd_lock, irq_state);
 	list_for_each(e, &ozhcd->orphanage) {
-		urbl = container_of(e, struct oz_urb_link, link);
+		urbl = list_entry(e, struct oz_urb_link, link);
 		if (urbl->urb == urb) {
 			list_del(e);
 			oz_dbg(ON, "Found urb in orphanage\n");
@@ -1746,15 +1665,13 @@ static void oz_urb_cancel_tasklet(unsigned long unused)
 {
 	unsigned long irq_state;
 	struct urb *urb;
+	struct oz_urb_link *urbl, *n;
 	struct oz_hcd *ozhcd = oz_hcd_claim();
 
 	if (ozhcd == NULL)
 		return;
 	spin_lock_irqsave(&g_tasklet_lock, irq_state);
-	while (!list_empty(&ozhcd->urb_cancel_list)) {
-		struct oz_urb_link *urbl =
-			list_first_entry(&ozhcd->urb_cancel_list,
-				struct oz_urb_link, link);
+	list_for_each_entry_safe(urbl, n, &ozhcd->urb_cancel_list, link) {
 		list_del_init(&urbl->link);
 		spin_unlock_irqrestore(&g_tasklet_lock, irq_state);
 		urb = urbl->urb;
@@ -1773,11 +1690,9 @@ static void oz_urb_cancel_tasklet(unsigned long unused)
 static void oz_hcd_clear_orphanage(struct oz_hcd *ozhcd, int status)
 {
 	if (ozhcd) {
-		struct oz_urb_link *urbl;
+		struct oz_urb_link *urbl, *n;
 
-		while (!list_empty(&ozhcd->orphanage)) {
-			urbl = list_first_entry(&ozhcd->orphanage,
-				struct oz_urb_link, link);
+		list_for_each_entry_safe(urbl, n, &ozhcd->orphanage, link) {
 			list_del(&urbl->link);
 			oz_complete_urb(ozhcd->hcd, urbl->urb, status);
 			oz_free_urb_link(urbl);
@@ -1875,14 +1790,13 @@ static struct oz_urb_link *oz_remove_urb(struct oz_endpoint *ep,
 				struct urb *urb)
 {
 	struct oz_urb_link *urbl;
-	struct list_head *e;
 
 	if (unlikely(ep == NULL))
 		return NULL;
-	list_for_each(e, &ep->urb_list) {
-		urbl = container_of(e, struct oz_urb_link, link);
+
+	list_for_each_entry(urbl, &ep->urb_list, link) {
 		if (urbl->urb == urb) {
-			list_del_init(e);
+			list_del_init(&urbl->link);
 			if (usb_pipeisoc(urb->pipe)) {
 				ep->credit -= urb->number_of_packets;
 				if (ep->credit < 0)
@@ -1988,8 +1902,7 @@ static int oz_hcd_hub_status_data(struct usb_hcd *hcd, char *buf)
 	spin_unlock_bh(&ozhcd->hcd_lock);
 	if (buf[0] != 0 || buf[1] != 0)
 		return 2;
-	else
-		return 0;
+	return 0;
 }
 
 /*
@@ -2311,7 +2224,6 @@ static int oz_plat_remove(struct platform_device *dev)
 	oz_dbg(ON, "Removing hcd\n");
 	usb_remove_hcd(hcd);
 	usb_put_hcd(hcd);
-	oz_empty_link_pool();
 	return 0;
 }
 
@@ -2341,6 +2253,11 @@ int oz_hcd_init(void)
 
 	if (usb_disabled())
 		return -ENODEV;
+
+	oz_urb_link_cache = KMEM_CACHE(oz_urb_link, 0);
+	if (!oz_urb_link_cache)
+		return -ENOMEM;
+
 	tasklet_init(&g_urb_process_tasklet, oz_urb_process_tasklet, 0);
 	tasklet_init(&g_urb_cancel_tasklet, oz_urb_cancel_tasklet, 0);
 	err = platform_driver_register(&g_oz_plat_drv);
@@ -2380,4 +2297,5 @@ void oz_hcd_term(void)
 	platform_device_unregister(g_plat_dev);
 	platform_driver_unregister(&g_oz_plat_drv);
 	oz_dbg(ON, "Pending urbs:%d\n", atomic_read(&g_pending_urbs));
+	kmem_cache_destroy(oz_urb_link_cache);
 }
diff --git a/drivers/staging/ozwpan/ozmain.c b/drivers/staging/ozwpan/ozmain.c
index d1a5b7a2c16d..7d6ef4cadf1a 100644
--- a/drivers/staging/ozwpan/ozmain.c
+++ b/drivers/staging/ozwpan/ozmain.c
@@ -25,6 +25,9 @@ unsigned int oz_dbg_mask = OZ_DEFAULT_DBG_MASK;
  * netcards. Bindings can be added later using an IOCTL.
  */
 static char *g_net_dev = "";
+module_param(g_net_dev, charp, S_IRUGO);
+MODULE_PARM_DESC(g_net_dev, "The device(s) to bind to; "
+	"'*' means all, '' (empty string; default) means none.");
 
 /*
  * Context: process
@@ -48,7 +51,6 @@ static void __exit ozwpan_exit(void)
 	oz_cdev_deregister();
 }
 
-module_param(g_net_dev, charp, S_IRUGO);
 module_init(ozwpan_init);
 module_exit(ozwpan_exit);
 
diff --git a/drivers/staging/ozwpan/ozpd.c b/drivers/staging/ozwpan/ozpd.c
index 10f1b3ac8832..852c288aaf13 100644
--- a/drivers/staging/ozwpan/ozpd.c
+++ b/drivers/staging/ozwpan/ozpd.c
@@ -21,8 +21,6 @@
 #include <linux/uaccess.h>
 #include <net/psnap.h>
 
-#define OZ_MAX_TX_POOL_SIZE	6
-
 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd);
 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f);
 static void oz_tx_isoc_free(struct oz_pd *pd, struct oz_tx_frame *f);
@@ -32,11 +30,6 @@ static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f);
 static void oz_isoc_stream_free(struct oz_isoc_stream *st);
 static int oz_send_next_queued_frame(struct oz_pd *pd, int more_data);
 static void oz_isoc_destructor(struct sk_buff *skb);
-static int oz_def_app_init(void);
-static void oz_def_app_term(void);
-static int oz_def_app_start(struct oz_pd *pd, int resume);
-static void oz_def_app_stop(struct oz_pd *pd, int pause);
-static void oz_def_app_rx(struct oz_pd *pd, struct oz_elt *elt);
 
 /*
  * Counts the uncompleted isoc frames submitted to netcard.
@@ -45,80 +38,25 @@ static atomic_t g_submitted_isoc = ATOMIC_INIT(0);
 
 /* Application handler functions.
  */
-static const struct oz_app_if g_app_if[OZ_APPID_MAX] = {
-	{oz_usb_init,
-	oz_usb_term,
-	oz_usb_start,
-	oz_usb_stop,
-	oz_usb_rx,
-	oz_usb_heartbeat,
-	oz_usb_farewell,
-	OZ_APPID_USB},
-
-	{oz_def_app_init,
-	oz_def_app_term,
-	oz_def_app_start,
-	oz_def_app_stop,
-	oz_def_app_rx,
-	NULL,
-	NULL,
-	OZ_APPID_UNUSED1},
-
-	{oz_def_app_init,
-	oz_def_app_term,
-	oz_def_app_start,
-	oz_def_app_stop,
-	oz_def_app_rx,
-	NULL,
-	NULL,
-	OZ_APPID_UNUSED2},
-
-	{oz_cdev_init,
-	oz_cdev_term,
-	oz_cdev_start,
-	oz_cdev_stop,
-	oz_cdev_rx,
-	NULL,
-	NULL,
-	OZ_APPID_SERIAL},
+static const struct oz_app_if g_app_if[OZ_NB_APPS] = {
+	[OZ_APPID_USB] = {
+		.init      = oz_usb_init,
+		.term      = oz_usb_term,
+		.start     = oz_usb_start,
+		.stop      = oz_usb_stop,
+		.rx        = oz_usb_rx,
+		.heartbeat = oz_usb_heartbeat,
+		.farewell  = oz_usb_farewell,
+	},
+	[OZ_APPID_SERIAL] = {
+		.init      = oz_cdev_init,
+		.term      = oz_cdev_term,
+		.start     = oz_cdev_start,
+		.stop      = oz_cdev_stop,
+		.rx        = oz_cdev_rx,
+	},
 };
 
-/*
- * Context: process
- */
-static int oz_def_app_init(void)
-{
-	return 0;
-}
-
-/*
- * Context: process
- */
-static void oz_def_app_term(void)
-{
-}
-
-/*
- * Context: softirq
- */
-static int oz_def_app_start(struct oz_pd *pd, int resume)
-{
-	return 0;
-}
-
-/*
- * Context: softirq
- */
-static void oz_def_app_stop(struct oz_pd *pd, int pause)
-{
-}
-
-/*
- * Context: softirq
- */
-static void oz_def_app_rx(struct oz_pd *pd, struct oz_elt *elt)
-{
-}
 
 /*
  * Context: softirq or process
@@ -168,17 +106,15 @@ struct oz_pd *oz_pd_alloc(const u8 *mac_addr)
 
 	if (pd) {
 		int i;
+
 		atomic_set(&pd->ref_count, 2);
-		for (i = 0; i < OZ_APPID_MAX; i++)
+		for (i = 0; i < OZ_NB_APPS; i++)
 			spin_lock_init(&pd->app_lock[i]);
 		pd->last_rx_pkt_num = 0xffffffff;
 		oz_pd_set_state(pd, OZ_PD_S_IDLE);
 		pd->max_tx_size = OZ_MAX_TX_SIZE;
 		ether_addr_copy(pd->mac_addr, mac_addr);
-		if (0 != oz_elt_buf_init(&pd->elt_buff)) {
-			kfree(pd);
-			pd = NULL;
-		}
+		oz_elt_buf_init(&pd->elt_buff);
 		spin_lock_init(&pd->tx_frame_lock);
 		INIT_LIST_HEAD(&pd->tx_queue);
 		INIT_LIST_HEAD(&pd->farewell_list);
@@ -202,10 +138,7 @@ struct oz_pd *oz_pd_alloc(const u8 *mac_addr)
  */
 static void oz_pd_free(struct work_struct *work)
 {
-	struct list_head *e;
-	struct oz_tx_frame *f;
-	struct oz_isoc_stream *st;
-	struct oz_farewell *fwell;
+	struct list_head *e, *n;
 	struct oz_pd *pd;
 
 	oz_pd_dbg(pd, ON, "Destroying PD\n");
@@ -213,40 +146,25 @@ static void oz_pd_free(struct work_struct *work)
 	/*Disable timer tasklets*/
 	tasklet_kill(&pd->heartbeat_tasklet);
 	tasklet_kill(&pd->timeout_tasklet);
-	/* Delete any streams.
-	 */
-	e = pd->stream_list.next;
-	while (e != &pd->stream_list) {
-		st = container_of(e, struct oz_isoc_stream, link);
-		e = e->next;
-		oz_isoc_stream_free(st);
-	}
-	/* Free any queued tx frames.
-	 */
-	e = pd->tx_queue.next;
-	while (e != &pd->tx_queue) {
-		f = container_of(e, struct oz_tx_frame, link);
-		e = e->next;
+
+	/* Free streams, queued tx frames and farewells. */
+
+	list_for_each_safe(e, n, &pd->stream_list)
+		oz_isoc_stream_free(list_entry(e, struct oz_isoc_stream, link));
+
+	list_for_each_safe(e, n, &pd->tx_queue) {
+		struct oz_tx_frame *f = list_entry(e, struct oz_tx_frame, link);
+
 		if (f->skb != NULL)
 			kfree_skb(f->skb);
 		oz_retire_frame(pd, f);
 	}
+
 	oz_elt_buf_term(&pd->elt_buff);
-	/* Free any farewells.
-	 */
-	e = pd->farewell_list.next;
-	while (e != &pd->farewell_list) {
-		fwell = container_of(e, struct oz_farewell, link);
-		e = e->next;
-		kfree(fwell);
-	}
-	/* Deallocate all frames in tx pool.
-	 */
-	while (pd->tx_pool) {
-		e = pd->tx_pool;
-		pd->tx_pool = e->next;
-		kfree(container_of(e, struct oz_tx_frame, link));
-	}
+
+	list_for_each_safe(e, n, &pd->farewell_list)
+		kfree(list_entry(e, struct oz_farewell, link));
+
 	if (pd->net_dev)
 		dev_put(pd->net_dev);
 	kfree(pd);
@@ -272,23 +190,21 @@ void oz_pd_destroy(struct oz_pd *pd)
  */
 int oz_services_start(struct oz_pd *pd, u16 apps, int resume)
 {
-	const struct oz_app_if *ai;
-	int rc = 0;
+	int i, rc = 0;
 
 	oz_pd_dbg(pd, ON, "%s: (0x%x) resume(%d)\n", __func__, apps, resume);
-	for (ai = g_app_if; ai < &g_app_if[OZ_APPID_MAX]; ai++) {
-		if (apps & (1<<ai->app_id)) {
-			if (ai->start(pd, resume)) {
+	for (i = 0; i < OZ_NB_APPS; i++) {
+		if (g_app_if[i].start && (apps & (1 << i))) {
+			if (g_app_if[i].start(pd, resume)) {
 				rc = -1;
 				oz_pd_dbg(pd, ON,
-					  "Unable to start service %d\n",
-					  ai->app_id);
+					  "Unable to start service %d\n", i);
 				break;
 			}
 			spin_lock_bh(&g_polling_lock);
-			pd->total_apps |= (1<<ai->app_id);
+			pd->total_apps |= (1 << i);
 			if (resume)
-				pd->paused_apps &= ~(1<<ai->app_id);
+				pd->paused_apps &= ~(1 << i);
 			spin_unlock_bh(&g_polling_lock);
 		}
 	}
@@ -300,20 +216,20 @@ int oz_services_start(struct oz_pd *pd, u16 apps, int resume)
  */
 void oz_services_stop(struct oz_pd *pd, u16 apps, int pause)
 {
-	const struct oz_app_if *ai;
+	int i;
 
 	oz_pd_dbg(pd, ON, "%s: (0x%x) pause(%d)\n", __func__, apps, pause);
-	for (ai = g_app_if; ai < &g_app_if[OZ_APPID_MAX]; ai++) {
-		if (apps & (1<<ai->app_id)) {
+	for (i = 0; i < OZ_NB_APPS; i++) {
+		if (g_app_if[i].stop && (apps & (1 << i))) {
 			spin_lock_bh(&g_polling_lock);
 			if (pause) {
-				pd->paused_apps |= (1<<ai->app_id);
+				pd->paused_apps |=  (1 << i);
 			} else {
-				pd->total_apps &= ~(1<<ai->app_id);
-				pd->paused_apps &= ~(1<<ai->app_id);
+				pd->total_apps  &= ~(1 << i);
+				pd->paused_apps &= ~(1 << i);
 			}
 			spin_unlock_bh(&g_polling_lock);
-			ai->stop(pd, pause);
+			g_app_if[i].stop(pd, pause);
 		}
 	}
 }
@@ -323,12 +239,11 @@ void oz_services_stop(struct oz_pd *pd, u16 apps, int pause)
  */
 void oz_pd_heartbeat(struct oz_pd *pd, u16 apps)
 {
-	const struct oz_app_if *ai;
-	int more = 0;
+	int i, more = 0;
 
-	for (ai = g_app_if; ai < &g_app_if[OZ_APPID_MAX]; ai++) {
-		if (ai->heartbeat && (apps & (1<<ai->app_id))) {
-			if (ai->heartbeat(pd))
+	for (i = 0; i < OZ_NB_APPS; i++) {
+		if (g_app_if[i].heartbeat && (apps & (1 << i))) {
+			if (g_app_if[i].heartbeat(pd))
 				more = 1;
 		}
 	}
@@ -336,6 +251,7 @@ void oz_pd_heartbeat(struct oz_pd *pd, u16 apps)
 		hrtimer_cancel(&pd->heartbeat);
 	if (pd->mode & OZ_F_ISOC_ANYTIME) {
 		int count = 8;
+
 		while (count-- && (oz_send_isoc_frame(pd) >= 0))
 			;
 	}
@@ -399,17 +315,9 @@ int oz_pd_sleep(struct oz_pd *pd)
  */
 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd)
 {
-	struct oz_tx_frame *f = NULL;
+	struct oz_tx_frame *f;
 
-	spin_lock_bh(&pd->tx_frame_lock);
-	if (pd->tx_pool) {
-		f = container_of(pd->tx_pool, struct oz_tx_frame, link);
-		pd->tx_pool = pd->tx_pool->next;
-		pd->tx_pool_count--;
-	}
-	spin_unlock_bh(&pd->tx_frame_lock);
-	if (f == NULL)
-		f = kmalloc(sizeof(struct oz_tx_frame), GFP_ATOMIC);
+	f = kmem_cache_alloc(oz_tx_frame_cache, GFP_ATOMIC);
 	if (f) {
 		f->total_size = sizeof(struct oz_hdr);
 		INIT_LIST_HEAD(&f->link);
@@ -425,13 +333,9 @@ static void oz_tx_isoc_free(struct oz_pd *pd, struct oz_tx_frame *f)
 {
 	pd->nb_queued_isoc_frames--;
 	list_del_init(&f->link);
-	if (pd->tx_pool_count < OZ_MAX_TX_POOL_SIZE) {
-		f->link.next = pd->tx_pool;
-		pd->tx_pool = &f->link;
-		pd->tx_pool_count++;
-	} else {
-		kfree(f);
-	}
+
+	kmem_cache_free(oz_tx_frame_cache, f);
+
 	oz_dbg(TX_FRAMES, "Releasing ISOC Frame isoc_nb= %d\n",
 	       pd->nb_queued_isoc_frames);
 }
@@ -441,15 +345,7 @@ static void oz_tx_isoc_free(struct oz_pd *pd, struct oz_tx_frame *f)
  */
 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f)
 {
-	spin_lock_bh(&pd->tx_frame_lock);
-	if (pd->tx_pool_count < OZ_MAX_TX_POOL_SIZE) {
-		f->link.next = pd->tx_pool;
-		pd->tx_pool = &f->link;
-		pd->tx_pool_count++;
-		f = NULL;
-	}
-	spin_unlock_bh(&pd->tx_frame_lock);
-	kfree(f);
+	kmem_cache_free(oz_tx_frame_cache, f);
 }
 
 /*
@@ -513,7 +409,7 @@ static struct sk_buff *oz_build_frame(struct oz_pd *pd, struct oz_tx_frame *f)
 	struct net_device *dev = pd->net_dev;
 	struct oz_hdr *oz_hdr;
 	struct oz_elt *elt;
-	struct list_head *e;
+	struct oz_elt_info *ei;
 
 	/* Allocate skb with enough space for the lower layers as well
 	 * as the space we need.
@@ -538,9 +434,7 @@ static struct sk_buff *oz_build_frame(struct oz_pd *pd, struct oz_tx_frame *f)
 	/* Copy the elements into the frame body.
 	 */
 	elt = (struct oz_elt *)(oz_hdr+1);
-	for (e = f->elt_list.next; e != &f->elt_list; e = e->next) {
-		struct oz_elt_info *ei;
-		ei = container_of(e, struct oz_elt_info, link);
+	list_for_each_entry(ei, &f->elt_list, link) {
 		memcpy(elt, ei->data, ei->length);
 		elt = oz_next_elt(elt);
 	}
@@ -555,13 +449,9 @@ fail:
  */
 static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f)
 {
-	struct list_head *e;
-	struct oz_elt_info *ei;
+	struct oz_elt_info *ei, *n;
 
-	e = f->elt_list.next;
-	while (e != &f->elt_list) {
-		ei = container_of(e, struct oz_elt_info, link);
-		e = e->next;
+	list_for_each_entry_safe(ei, n, &f->elt_list, link) {
 		list_del_init(&ei->link);
 		if (ei->callback)
 			ei->callback(pd, ei->context);
@@ -570,8 +460,6 @@ static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f)
 		spin_unlock_bh(&pd->elt_buff.lock);
 	}
 	oz_tx_frame_free(pd, f);
-	if (pd->elt_buff.free_elts > pd->elt_buff.max_free_elts)
-		oz_trim_elt_pool(&pd->elt_buff);
 }
 
 /*
@@ -589,7 +477,7 @@ static int oz_send_next_queued_frame(struct oz_pd *pd, int more_data)
 		spin_unlock(&pd->tx_frame_lock);
 		return -1;
 	}
-	f = container_of(e, struct oz_tx_frame, link);
+	f = list_entry(e, struct oz_tx_frame, link);
 
 	if (f->skb != NULL) {
 		skb = f->skb;
@@ -608,11 +496,10 @@ static int oz_send_next_queued_frame(struct oz_pd *pd, int more_data)
 			oz_dbg(TX_FRAMES, "Sending ISOC Frame, nb_isoc= %d\n",
 			       pd->nb_queued_isoc_frames);
 			return 0;
-		} else {
-			kfree_skb(skb);
-			oz_dbg(TX_FRAMES, "Dropping ISOC Frame>\n");
-			return -1;
 		}
+		kfree_skb(skb);
+		oz_dbg(TX_FRAMES, "Dropping ISOC Frame>\n");
+		return -1;
 	}
 
 	pd->last_sent_frame = e;
@@ -677,15 +564,13 @@ static int oz_send_isoc_frame(struct oz_pd *pd)
 	struct net_device *dev = pd->net_dev;
 	struct oz_hdr *oz_hdr;
 	struct oz_elt *elt;
-	struct list_head *e;
-	struct list_head list;
+	struct oz_elt_info *ei;
+	LIST_HEAD(list);
 	int total_size = sizeof(struct oz_hdr);
 
-	INIT_LIST_HEAD(&list);
-
 	oz_select_elts_for_tx(&pd->elt_buff, 1, &total_size,
 		pd->max_tx_size, &list);
-	if (list.next == &list)
+	if (list_empty(&list))
 		return 0;
 	skb = alloc_skb(total_size + OZ_ALLOCATED_SPACE(dev), GFP_ATOMIC);
 	if (skb == NULL) {
@@ -707,9 +592,7 @@ static int oz_send_isoc_frame(struct oz_pd *pd)
 	oz_hdr->last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
 	elt = (struct oz_elt *)(oz_hdr+1);
 
-	for (e = list.next; e != &list; e = e->next) {
-		struct oz_elt_info *ei;
-		ei = container_of(e, struct oz_elt_info, link);
+	list_for_each_entry(ei, &list, link) {
 		memcpy(elt, ei->data, ei->length);
 		elt = oz_next_elt(elt);
 	}
@@ -723,41 +606,30 @@ static int oz_send_isoc_frame(struct oz_pd *pd)
  */
 void oz_retire_tx_frames(struct oz_pd *pd, u8 lpn)
 {
-	struct list_head *e;
-	struct oz_tx_frame *f;
-	struct list_head *first = NULL;
-	struct list_head *last = NULL;
+	struct oz_tx_frame *f, *tmp = NULL;
 	u8 diff;
 	u32 pkt_num;
 
+	LIST_HEAD(list);
+
 	spin_lock(&pd->tx_frame_lock);
-	e = pd->tx_queue.next;
-	while (e != &pd->tx_queue) {
-		f = container_of(e, struct oz_tx_frame, link);
+	list_for_each_entry(f, &pd->tx_queue, link) {
 		pkt_num = le32_to_cpu(get_unaligned(&f->hdr.pkt_num));
 		diff = (lpn - (pkt_num & OZ_LAST_PN_MASK)) & OZ_LAST_PN_MASK;
 		if ((diff > OZ_LAST_PN_HALF_CYCLE) || (pkt_num == 0))
 			break;
 		oz_dbg(TX_FRAMES, "Releasing pkt_num= %u, nb= %d\n",
 		       pkt_num, pd->nb_queued_frames);
-		if (first == NULL)
-			first = e;
-		last = e;
-		e = e->next;
+		tmp = f;
 		pd->nb_queued_frames--;
 	}
-	if (first) {
-		last->next->prev = &pd->tx_queue;
-		pd->tx_queue.next = last->next;
-		last->next = NULL;
-	}
+	if (tmp)
+		list_cut_position(&list, &pd->tx_queue, &tmp->link);
 	pd->last_sent_frame = &pd->tx_queue;
 	spin_unlock(&pd->tx_frame_lock);
-	while (first) {
-		f = container_of(first, struct oz_tx_frame, link);
-		first = first->next;
+
+	list_for_each_entry_safe(f, tmp, &list, link)
 		oz_retire_frame(pd, f);
-	}
 }
 
 /*
@@ -766,11 +638,9 @@ void oz_retire_tx_frames(struct oz_pd *pd, u8 lpn)
  */
 static struct oz_isoc_stream *pd_stream_find(struct oz_pd *pd, u8 ep_num)
 {
-	struct list_head *e;
 	struct oz_isoc_stream *st;
 
-	list_for_each(e, &pd->stream_list) {
-		st = container_of(e, struct oz_isoc_stream, link);
+	list_for_each_entry(st, &pd->stream_list, link) {
 		if (st->ep_num == ep_num)
 			return st;
 	}
@@ -884,6 +754,7 @@ int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, const u8 *data, int len)
 	} else {
 		struct oz_hdr oz;
 		struct oz_isoc_large iso;
+
 		spin_lock_bh(&pd->stream_lock);
 		iso.frame_number = st->frame_num;
 		st->frame_num += nb_units;
@@ -906,15 +777,14 @@ int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, const u8 *data, int len)
 		if (!(pd->mode & OZ_F_ISOC_ANYTIME)) {
 			struct oz_tx_frame *isoc_unit = NULL;
 			int nb = pd->nb_queued_isoc_frames;
+
 			if (nb >= pd->isoc_latency) {
-				struct list_head *e;
 				struct oz_tx_frame *f;
+
 				oz_dbg(TX_FRAMES, "Dropping ISOC Unit nb= %d\n",
 				       nb);
 				spin_lock(&pd->tx_frame_lock);
-				list_for_each(e, &pd->tx_queue) {
-					f = container_of(e, struct oz_tx_frame,
-									link);
+				list_for_each_entry(f, &pd->tx_queue, link) {
 					if (f->skb != NULL) {
 						oz_tx_isoc_free(pd, f);
 						break;
@@ -942,8 +812,7 @@ int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, const u8 *data, int len)
 			atomic_inc(&g_submitted_isoc);
 			if (dev_queue_xmit(skb) < 0)
 				return -1;
-			else
-				return 0;
+			return 0;
 		}
 
 out:	kfree_skb(skb);
@@ -960,9 +829,10 @@ void oz_apps_init(void)
 {
 	int i;
 
-	for (i = 0; i < OZ_APPID_MAX; i++)
+	for (i = 0; i < OZ_NB_APPS; i++) {
 		if (g_app_if[i].init)
 			g_app_if[i].init();
+	}
 }
 
 /*
@@ -973,9 +843,10 @@ void oz_apps_term(void)
 	int i;
 
 	/* Terminate all the apps. */
-	for (i = 0; i < OZ_APPID_MAX; i++)
+	for (i = 0; i < OZ_NB_APPS; i++) {
 		if (g_app_if[i].term)
 			g_app_if[i].term();
+	}
 }
 
 /*
@@ -983,12 +854,8 @@ void oz_apps_term(void)
  */
 void oz_handle_app_elt(struct oz_pd *pd, u8 app_id, struct oz_elt *elt)
 {
-	const struct oz_app_if *ai;
-
-	if (app_id == 0 || app_id > OZ_APPID_MAX)
-		return;
-	ai = &g_app_if[app_id-1];
-	ai->rx(pd, elt);
+	if (app_id < OZ_NB_APPS && g_app_if[app_id].rx)
+		g_app_if[app_id].rx(pd, elt);
 }
 
 /*
@@ -997,7 +864,7 @@ void oz_handle_app_elt(struct oz_pd *pd, u8 app_id, struct oz_elt *elt)
 void oz_pd_indicate_farewells(struct oz_pd *pd)
 {
 	struct oz_farewell *f;
-	const struct oz_app_if *ai = &g_app_if[OZ_APPID_USB-1];
+	const struct oz_app_if *ai = &g_app_if[OZ_APPID_USB];
 
 	while (1) {
 		spin_lock_bh(&g_polling_lock);
diff --git a/drivers/staging/ozwpan/ozpd.h b/drivers/staging/ozwpan/ozpd.h
index ad5fe7a6e619..212fab0d807a 100644
--- a/drivers/staging/ozwpan/ozpd.h
+++ b/drivers/staging/ozwpan/ozpd.h
@@ -81,8 +81,8 @@ struct oz_pd {
 	unsigned long	presleep;
 	unsigned long	keep_alive;
 	struct oz_elt_buf elt_buff;
-	void		*app_ctx[OZ_APPID_MAX];
-	spinlock_t	app_lock[OZ_APPID_MAX];
+	void		*app_ctx[OZ_NB_APPS];
+	spinlock_t	app_lock[OZ_NB_APPS];
 	int		max_tx_size;
 	u8		mode;
 	u8		ms_per_isoc;
@@ -90,8 +90,6 @@ struct oz_pd {
 	unsigned	max_stream_buffering;
 	int		nb_queued_frames;
 	int		nb_queued_isoc_frames;
-	struct list_head *tx_pool;
-	int		tx_pool_count;
 	spinlock_t	tx_frame_lock;
 	struct list_head *last_sent_frame;
 	struct list_head tx_queue;
@@ -130,4 +128,7 @@ void oz_handle_app_elt(struct oz_pd *pd, u8 app_id, struct oz_elt *elt);
 void oz_apps_init(void);
 void oz_apps_term(void);
 
+extern struct kmem_cache *oz_elt_info_cache;
+extern struct kmem_cache *oz_tx_frame_cache;
+
 #endif /* Sentry */
diff --git a/drivers/staging/ozwpan/ozproto.c b/drivers/staging/ozwpan/ozproto.c
index 110205599190..3d3a3a890f73 100644
--- a/drivers/staging/ozwpan/ozproto.c
+++ b/drivers/staging/ozwpan/ozproto.c
@@ -11,6 +11,7 @@
 #include <linux/etherdevice.h>
 #include <linux/errno.h>
 #include <linux/ieee80211.h>
+#include <linux/slab.h>
 #include "ozdbg.h"
 #include "ozprotocol.h"
 #include "ozeltbuf.h"
@@ -29,8 +30,6 @@
 #define OZ_DO_STOP		1
 #define OZ_DO_SLEEP		2
 
-#define OZ_MAX_TIMER_POOL_SIZE	16
-
 struct oz_binding {
 	struct packet_type ptype;
 	char name[OZ_MAX_BINDING_LEN];
@@ -53,6 +52,9 @@ static u8 g_session_id;
 static u16 g_apps = 0x1;
 static int g_processing_rx;
 
+struct kmem_cache *oz_elt_info_cache;
+struct kmem_cache *oz_tx_frame_cache;
+
 /*
  * Context: softirq-serialized
  */
@@ -110,7 +112,6 @@ static void oz_send_conn_rsp(struct oz_pd *pd, u8 status)
 	}
 	oz_dbg(ON, "TX: OZ_ELT_CONNECT_RSP %d", status);
 	dev_queue_xmit(skb);
-	return;
 }
 
 /*
@@ -177,13 +178,14 @@ static struct oz_pd *oz_connect_req(struct oz_pd *cur_pd, struct oz_elt *elt,
 	} else {
 		struct oz_pd *pd2 = NULL;
 		struct list_head *e;
+
 		pd = oz_pd_alloc(pd_addr);
 		if (pd == NULL)
 			return NULL;
 		getnstimeofday(&pd->last_rx_timestamp);
 		spin_lock_bh(&g_polling_lock);
 		list_for_each(e, &g_pd_list) {
-			pd2 = container_of(e, struct oz_pd, link);
+			pd2 = list_entry(e, struct oz_pd, link);
 			if (ether_addr_equal(pd2->mac_addr, pd_addr)) {
 				free_pd = pd;
 				pd = pd2;
@@ -260,6 +262,7 @@ done:
 		u16 start_apps = new_apps & ~pd->total_apps & ~0x1;
 		u16 stop_apps = pd->total_apps & ~new_apps & ~0x1;
 		u16 resume_apps = new_apps & pd->paused_apps  & ~0x1;
+
 		spin_unlock_bh(&g_polling_lock);
 		oz_pd_set_state(pd, OZ_PD_S_CONNECTED);
 		oz_dbg(ON, "new_apps=0x%x total_apps=0x%x paused_apps=0x%x\n",
@@ -381,6 +384,7 @@ static void oz_rx_frame(struct sk_buff *skb)
 		if ((oz_hdr->control & OZ_F_ACK_REQUESTED) &&
 				(pd->state == OZ_PD_S_CONNECTED)) {
 			int backlog = pd->nb_queued_frames;
+
 			pd->trigger_pkt_num = pkt_num;
 			/* Send queued frames */
 			oz_send_queued_frames(pd, backlog);
@@ -481,6 +485,9 @@ void oz_protocol_term(void)
 	}
 	spin_unlock_bh(&g_polling_lock);
 	oz_dbg(ON, "Protocol stopped\n");
+
+	kmem_cache_destroy(oz_tx_frame_cache);
+	kmem_cache_destroy(oz_elt_info_cache);
 }
 
 /*
@@ -596,13 +603,11 @@ void oz_pd_request_heartbeat(struct oz_pd *pd)
 struct oz_pd *oz_pd_find(const u8 *mac_addr)
 {
 	struct oz_pd *pd;
-	struct list_head *e;
 
 	spin_lock_bh(&g_polling_lock);
-	list_for_each(e, &g_pd_list) {
-		pd = container_of(e, struct oz_pd, link);
+	list_for_each_entry(pd, &g_pd_list, link) {
 		if (ether_addr_equal(pd->mac_addr, mac_addr)) {
-			atomic_inc(&pd->ref_count);
+			oz_pd_get(pd);
 			spin_unlock_bh(&g_polling_lock);
 			return pd;
 		}
@@ -616,7 +621,7 @@ struct oz_pd *oz_pd_find(const u8 *mac_addr)
  */
 void oz_app_enable(int app_id, int enable)
 {
-	if (app_id <= OZ_APPID_MAX) {
+	if (app_id < OZ_NB_APPS) {
 		spin_lock_bh(&g_polling_lock);
 		if (enable)
 			g_apps |= (1<<app_id);
@@ -695,11 +700,10 @@ void oz_binding_add(const char *net_dev)
  */
 static void pd_stop_all_for_device(struct net_device *net_dev)
 {
-	struct list_head h;
+	LIST_HEAD(h);
 	struct oz_pd *pd;
 	struct oz_pd *n;
 
-	INIT_LIST_HEAD(&h);
 	spin_lock_bh(&g_polling_lock);
 	list_for_each_entry_safe(pd, n, &g_pd_list, link) {
 		if (pd->net_dev == net_dev) {
@@ -764,11 +768,22 @@ static char *oz_get_next_device_name(char *s, char *dname, int max_size)
  */
 int oz_protocol_init(char *devs)
 {
+	oz_elt_info_cache = KMEM_CACHE(oz_elt_info, 0);
+	if (!oz_elt_info_cache)
+		return -ENOMEM;
+
+	oz_tx_frame_cache = KMEM_CACHE(oz_tx_frame, 0);
+	if (!oz_tx_frame_cache) {
+		kmem_cache_destroy(oz_elt_info_cache);
+		return -ENOMEM;
+	}
+
 	skb_queue_head_init(&g_rx_queue);
 	if (devs[0] == '*') {
 		oz_binding_add(NULL);
 	} else {
 		char d[32];
+
 		while (*devs) {
 			devs = oz_get_next_device_name(devs, d, sizeof(d));
 			if (d[0])
@@ -784,14 +799,12 @@ int oz_protocol_init(char *devs)
 int oz_get_pd_list(struct oz_mac_addr *addr, int max_count)
 {
 	struct oz_pd *pd;
-	struct list_head *e;
 	int count = 0;
 
 	spin_lock_bh(&g_polling_lock);
-	list_for_each(e, &g_pd_list) {
+	list_for_each_entry(pd, &g_pd_list, link) {
 		if (count >= max_count)
 			break;
-		pd = container_of(e, struct oz_pd, link);
 		ether_addr_copy((u8 *)&addr[count++], pd->mac_addr);
 	}
 	spin_unlock_bh(&g_polling_lock);
diff --git a/drivers/staging/ozwpan/ozproto.h b/drivers/staging/ozwpan/ozproto.h
index cb38e02c968e..30c2db91c000 100644
--- a/drivers/staging/ozwpan/ozproto.h
+++ b/drivers/staging/ozwpan/ozproto.h
@@ -29,24 +29,14 @@
 
 /* Application handler functions.
  */
-typedef int (*oz_app_init_fn_t)(void);
-typedef void (*oz_app_term_fn_t)(void);
-typedef int (*oz_app_start_fn_t)(struct oz_pd *pd, int resume);
-typedef void (*oz_app_stop_fn_t)(struct oz_pd *pd, int pause);
-typedef void (*oz_app_rx_fn_t)(struct oz_pd *pd, struct oz_elt *elt);
-typedef int (*oz_app_hearbeat_fn_t)(struct oz_pd *pd);
-typedef void (*oz_app_farewell_fn_t)(struct oz_pd *pd, u8 ep_num,
-			u8 *data, u8 len);
-
 struct oz_app_if {
-	oz_app_init_fn_t	init;
-	oz_app_term_fn_t	term;
-	oz_app_start_fn_t	start;
-	oz_app_stop_fn_t	stop;
-	oz_app_rx_fn_t		rx;
-	oz_app_hearbeat_fn_t	heartbeat;
-	oz_app_farewell_fn_t	farewell;
-	int			app_id;
+	int  (*init)(void);
+	void (*term)(void);
+	int  (*start)(struct oz_pd *pd, int resume);
+	void (*stop)(struct oz_pd *pd, int pause);
+	void (*rx)(struct oz_pd *pd, struct oz_elt *elt);
+	int  (*heartbeat)(struct oz_pd *pd);
+	void (*farewell)(struct oz_pd *pd, u8 ep_num, u8 *data, u8 len);
 };
 
 int oz_protocol_init(char *devs);
@@ -66,4 +56,7 @@ enum hrtimer_restart oz_pd_timeout_event(struct hrtimer *timer);
 int oz_get_pd_status_list(char *pd_list, int max_count);
 int oz_get_binding_list(char *buf, int max_if);
 
+extern struct kmem_cache *oz_elt_info_cache;
+extern struct kmem_cache *oz_tx_frame_cache;
+
 #endif /* _OZPROTO_H */
diff --git a/drivers/staging/ozwpan/ozprotocol.h b/drivers/staging/ozwpan/ozprotocol.h
index 9bbb182f2776..4642072596da 100644
--- a/drivers/staging/ozwpan/ozprotocol.h
+++ b/drivers/staging/ozwpan/ozprotocol.h
@@ -139,8 +139,6 @@ struct oz_app_hdr {
 /* Values for app_id.
  */
 #define OZ_APPID_USB				0x1
-#define OZ_APPID_UNUSED1			0x2
-#define OZ_APPID_UNUSED2			0x3
 #define OZ_APPID_SERIAL				0x4
 #define OZ_APPID_MAX				OZ_APPID_SERIAL
 #define OZ_NB_APPS				(OZ_APPID_MAX+1)
diff --git a/drivers/staging/ozwpan/ozusbsvc.c b/drivers/staging/ozwpan/ozusbsvc.c
index edd44c457a4b..bf15dc301cb5 100644
--- a/drivers/staging/ozwpan/ozusbsvc.c
+++ b/drivers/staging/ozwpan/ozusbsvc.c
@@ -73,12 +73,12 @@ int oz_usb_start(struct oz_pd *pd, int resume)
 	 * If it does already have one then destroy the one we have just
 	 * created.
 	 */
-	spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]);
-	old_ctx = pd->app_ctx[OZ_APPID_USB-1];
+	spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
+	old_ctx = pd->app_ctx[OZ_APPID_USB];
 	if (old_ctx == NULL)
-		pd->app_ctx[OZ_APPID_USB-1] = usb_ctx;
-	oz_usb_get(pd->app_ctx[OZ_APPID_USB-1]);
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]);
+		pd->app_ctx[OZ_APPID_USB] = usb_ctx;
+	oz_usb_get(pd->app_ctx[OZ_APPID_USB]);
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
 	if (old_ctx) {
 		oz_dbg(ON, "Already have USB context\n");
 		kfree(usb_ctx);
@@ -99,9 +99,9 @@ int oz_usb_start(struct oz_pd *pd, int resume)
 		usb_ctx->hport = oz_hcd_pd_arrived(usb_ctx);
 		if (usb_ctx->hport == NULL) {
 			oz_dbg(ON, "USB hub returned null port\n");
-			spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]);
-			pd->app_ctx[OZ_APPID_USB-1] = NULL;
-			spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]);
+			spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
+			pd->app_ctx[OZ_APPID_USB] = NULL;
+			spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
 			oz_usb_put(usb_ctx);
 			rc = -1;
 		}
@@ -122,12 +122,13 @@ void oz_usb_stop(struct oz_pd *pd, int pause)
 		oz_dbg(ON, "USB service paused\n");
 		return;
 	}
-	spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]);
-	usb_ctx = (struct oz_usb_ctx *)pd->app_ctx[OZ_APPID_USB-1];
-	pd->app_ctx[OZ_APPID_USB-1] = NULL;
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]);
+	spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
+	usb_ctx = (struct oz_usb_ctx *) pd->app_ctx[OZ_APPID_USB];
+	pd->app_ctx[OZ_APPID_USB] = NULL;
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
 	if (usb_ctx) {
 		struct timespec ts, now;
+
 		getnstimeofday(&ts);
 		oz_dbg(ON, "USB service stopping...\n");
 		usb_ctx->stopped = 1;
@@ -187,11 +188,11 @@ int oz_usb_heartbeat(struct oz_pd *pd)
 	struct oz_usb_ctx *usb_ctx;
 	int rc = 0;
 
-	spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]);
-	usb_ctx = (struct oz_usb_ctx *)pd->app_ctx[OZ_APPID_USB-1];
+	spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
+	usb_ctx = (struct oz_usb_ctx *) pd->app_ctx[OZ_APPID_USB];
 	if (usb_ctx)
 		oz_usb_get(usb_ctx);
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]);
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
 	if (usb_ctx == NULL)
 		return rc;
 	if (usb_ctx->stopped)
@@ -235,6 +236,7 @@ int oz_usb_stream_delete(void *hpd, u8 ep_num)
 
 	if (usb_ctx) {
 		struct oz_pd *pd = usb_ctx->pd;
+
 		if (pd) {
 			oz_dbg(ON, "%s: (0x%x)\n", __func__, ep_num);
 			if (pd->mode & OZ_F_ISOC_NO_ELTS) {
diff --git a/drivers/staging/ozwpan/ozusbsvc1.c b/drivers/staging/ozwpan/ozusbsvc1.c
index f32d01427f77..be7ee01c50ab 100644
--- a/drivers/staging/ozwpan/ozusbsvc1.c
+++ b/drivers/staging/ozwpan/ozusbsvc1.c
@@ -213,6 +213,7 @@ int oz_usb_control_req(void *hpd, u8 req_id, struct usb_ctrlrequest *setup,
 		case USB_REQ_SET_INTERFACE: {
 				u8 if_num = (u8)windex;
 				u8 alt = (u8)wvalue;
+
 				rc = oz_usb_set_interface_req(hpd, req_id,
 					if_num, alt);
 			}
@@ -254,6 +255,7 @@ int oz_usb_send_isoc(void *hpd, u8 ep_num, struct urb *urb)
 	if (pd->mode & OZ_F_ISOC_NO_ELTS) {
 		for (i = 0; i < urb->number_of_packets; i++) {
 			u8 *data;
+
 			desc = &urb->iso_frame_desc[i];
 			data = ((u8 *)urb->transfer_buffer)+desc->offset;
 			oz_send_isoc_unit(pd, ep_num, data, desc->length);
@@ -271,6 +273,7 @@ int oz_usb_send_isoc(void *hpd, u8 ep_num, struct urb *urb)
 		int unit_count;
 		int unit_size;
 		int rem;
+
 		if (ei == NULL)
 			return -1;
 		rem = MAX_ISOC_FIXED_DATA;
@@ -340,6 +343,7 @@ static void oz_usb_handle_ep_data(struct oz_usb_ctx *usb_ctx,
 			u8 *data = body->data;
 			int count;
 			int i;
+
 			if (!unit_size)
 				break;
 			count = data_len/unit_size;
@@ -364,11 +368,11 @@ void oz_usb_rx(struct oz_pd *pd, struct oz_elt *elt)
 	struct oz_usb_hdr *usb_hdr = (struct oz_usb_hdr *)(elt + 1);
 	struct oz_usb_ctx *usb_ctx;
 
-	spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]);
-	usb_ctx = (struct oz_usb_ctx *)pd->app_ctx[OZ_APPID_USB-1];
+	spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
+	usb_ctx = (struct oz_usb_ctx *)pd->app_ctx[OZ_APPID_USB];
 	if (usb_ctx)
 		oz_usb_get(usb_ctx);
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]);
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
 	if (usb_ctx == NULL)
 		return; /* Context has gone so nothing to do. */
 	if (usb_ctx->stopped)
@@ -434,11 +438,11 @@ void oz_usb_farewell(struct oz_pd *pd, u8 ep_num, u8 *data, u8 len)
 {
 	struct oz_usb_ctx *usb_ctx;
 
-	spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]);
-	usb_ctx = (struct oz_usb_ctx *)pd->app_ctx[OZ_APPID_USB-1];
+	spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
+	usb_ctx = (struct oz_usb_ctx *)pd->app_ctx[OZ_APPID_USB];
 	if (usb_ctx)
 		oz_usb_get(usb_ctx);
-	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]);
+	spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
 	if (usb_ctx == NULL)
 		return; /* Context has gone so nothing to do. */
 	if (!usb_ctx->stopped) {
diff --git a/drivers/staging/rtl8188eu/Makefile b/drivers/staging/rtl8188eu/Makefile
index aeebf9311f15..31ac15961a00 100644
--- a/drivers/staging/rtl8188eu/Makefile
+++ b/drivers/staging/rtl8188eu/Makefile
@@ -17,25 +17,23 @@ r8188eu-y :=				\
 		core/rtw_sta_mgt.o	\
 		core/rtw_wlan_util.o	\
 		core/rtw_xmit.o		\
-		hal/HalHWImg8188E_MAC.o	\
-		hal/HalHWImg8188E_BB.o	\
-		hal/HalHWImg8188E_RF.o	\
-		hal/HalPhyRf_8188e.o	\
-		hal/HalPwrSeqCmd.o	\
-		hal/Hal8188EPwrSeq.o	\
+		hal/fw.o	\
+		hal/mac_cfg.o \
+		hal/bb_cfg.o \
+		hal/rf_cfg.o \
+		hal/pwrseqcmd.o \
+		hal/pwrseq.o \
 		hal/Hal8188ERateAdaptive.o\
 		hal/hal_intf.o		\
 		hal/hal_com.o		\
 		hal/odm.o		\
-		hal/odm_debug.o		\
 		hal/odm_HWConfig.o	\
-		hal/odm_RegConfig8188E.o\
 		hal/odm_RTL8188E.o	\
 		hal/rtl8188e_cmd.o	\
 		hal/rtl8188e_dm.o	\
 		hal/rtl8188e_hal_init.o	\
-		hal/rtl8188e_phycfg.o	\
-		hal/rtl8188e_rf6052.o	\
+		hal/phy.o \
+		hal/rf.o \
 		hal/rtl8188e_rxdesc.o	\
 		hal/rtl8188e_xmit.o	\
 		hal/rtl8188eu_led.o	\
diff --git a/drivers/staging/rtl8188eu/core/rtw_cmd.c b/drivers/staging/rtl8188eu/core/rtw_cmd.c
index 104b01fa0fad..9935e66935af 100644
--- a/drivers/staging/rtl8188eu/core/rtw_cmd.c
+++ b/drivers/staging/rtl8188eu/core/rtw_cmd.c
@@ -69,7 +69,7 @@ exit:
 	return _SUCCESS;
 }
 
-struct	cmd_obj	* rtw_dequeue_cmd(struct __queue *queue)
+struct	cmd_obj	*rtw_dequeue_cmd(struct __queue *queue)
 {
 	unsigned long irqL;
 	struct cmd_obj *obj;
@@ -98,6 +98,7 @@ static int rtw_cmd_filter(struct cmd_priv *pcmdpriv, struct cmd_obj *cmd_obj)
 	    (!pcmdpriv->padapter->registrypriv.usbss_enable)) {
 		if (cmd_obj->cmdcode == GEN_CMD_CODE(_Set_Drv_Extra)) {
 			struct drvextra_cmd_parm	*pdrvextra_cmd_parm = (struct drvextra_cmd_parm	*)cmd_obj->parmbuf;
+
 			if (pdrvextra_cmd_parm->ec_id == POWER_SAVING_CTRL_WK_CID)
 				bAllow = true;
 		}
@@ -296,6 +297,7 @@ u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid,
 	/* prepare ssid list */
 	if (ssid) {
 		int i;
+
 		for (i = 0; i < ssid_num && i < RTW_SSID_SCAN_AMOUNT; i++) {
 			if (ssid[i].SsidLength) {
 				memcpy(&psurveyPara->ssid[i], &ssid[i], sizeof(struct ndis_802_11_ssid));
@@ -307,6 +309,7 @@ u8 rtw_sitesurvey_cmd(struct adapter  *padapter, struct ndis_802_11_ssid *ssid,
 	/* prepare channel list */
 	if (ch) {
 		int i;
+
 		for (i = 0; i < ch_num && i < RTW_CHANNEL_SCAN_AMOUNT; i++) {
 			if (ch[i].hw_value && !(ch[i].flags & RTW_IEEE80211_CHAN_DISABLED)) {
 				memcpy(&psurveyPara->ch[i], &ch[i], sizeof(struct rtw_ieee80211_channel));
diff --git a/drivers/staging/rtl8188eu/core/rtw_efuse.c b/drivers/staging/rtl8188eu/core/rtw_efuse.c
index 5b997b2b404a..7006088d1ad0 100644
--- a/drivers/staging/rtl8188eu/core/rtw_efuse.c
+++ b/drivers/staging/rtl8188eu/core/rtw_efuse.c
@@ -440,7 +440,7 @@ u8 Efuse_WordEnableDataWrite(struct adapter *pAdapter, u16 efuse_addr, u8 word_e
 	return badworden;
 }
 
-u16 Efuse_GetCurrentSize(struct adapter *pAdapter)
+static u16 Efuse_GetCurrentSize(struct adapter *pAdapter)
 {
 	int	bContinual = true;
 	u16	efuse_addr = 0;
diff --git a/drivers/staging/rtl8188eu/core/rtw_iol.c b/drivers/staging/rtl8188eu/core/rtw_iol.c
index 7796287be8f4..cdcf0eacc0e0 100644
--- a/drivers/staging/rtl8188eu/core/rtw_iol.c
+++ b/drivers/staging/rtl8188eu/core/rtw_iol.c
@@ -20,65 +20,6 @@
 
 #include<rtw_iol.h>
 
-struct xmit_frame	*rtw_IOL_accquire_xmit_frame(struct adapter  *adapter)
-{
-	struct xmit_frame	*xmit_frame;
-	struct xmit_buf	*xmitbuf;
-	struct pkt_attrib	*pattrib;
-	struct xmit_priv	*pxmitpriv = &(adapter->xmitpriv);
-
-	xmit_frame = rtw_alloc_xmitframe(pxmitpriv);
-	if (xmit_frame == NULL) {
-		DBG_88E("%s rtw_alloc_xmitframe return null\n", __func__);
-		goto exit;
-	}
-
-	xmitbuf = rtw_alloc_xmitbuf(pxmitpriv);
-	if (xmitbuf == NULL) {
-		DBG_88E("%s rtw_alloc_xmitbuf return null\n", __func__);
-		rtw_free_xmitframe(pxmitpriv, xmit_frame);
-		xmit_frame = NULL;
-		goto exit;
-	}
-
-	xmit_frame->frame_tag = MGNT_FRAMETAG;
-	xmit_frame->pxmitbuf = xmitbuf;
-	xmit_frame->buf_addr = xmitbuf->pbuf;
-	xmitbuf->priv_data = xmit_frame;
-
-	pattrib = &xmit_frame->attrib;
-	update_mgntframe_attrib(adapter, pattrib);
-	pattrib->qsel = 0x10;/* Beacon */
-	pattrib->subtype = WIFI_BEACON;
-	pattrib->pktlen = 0;
-	pattrib->last_txcmdsz = 0;
-exit:
-	return xmit_frame;
-}
-
-int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds, u32 cmd_len)
-{
-	struct pkt_attrib	*pattrib = &xmit_frame->attrib;
-	u16 buf_offset;
-	u32 ori_len;
-
-	buf_offset = TXDESC_OFFSET;
-	ori_len = buf_offset+pattrib->pktlen;
-
-	/* check if the io_buf can accommodate new cmds */
-	if (ori_len + cmd_len + 8 > MAX_XMITBUF_SZ) {
-		DBG_88E("%s %u is large than MAX_XMITBUF_SZ:%u, can't accommodate new cmds\n",
-			__func__ , ori_len + cmd_len + 8, MAX_XMITBUF_SZ);
-		return _FAIL;
-	}
-
-	memcpy(xmit_frame->buf_addr + buf_offset + pattrib->pktlen, IOL_cmds, cmd_len);
-	pattrib->pktlen += cmd_len;
-	pattrib->last_txcmdsz += cmd_len;
-
-	return _SUCCESS;
-}
-
 bool rtw_IOL_applied(struct adapter  *adapter)
 {
 	if (1 == adapter->registrypriv.fw_iol)
@@ -88,108 +29,3 @@ bool rtw_IOL_applied(struct adapter  *adapter)
 		return true;
 	return false;
 }
-
-int rtw_IOL_exec_cmds_sync(struct adapter  *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt)
-{
-	return rtw_hal_iol_cmd(adapter, xmit_frame, max_wating_ms, bndy_cnt);
-}
-
-int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary)
-{
-	return _SUCCESS;
-}
-
-int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr, u8 value, u8 mask)
-{
-	struct ioreg_cfg cmd = {8, IOREG_CMD_WB_REG, 0x0, 0x0, 0x0};
-
-	cmd.address = cpu_to_le16(addr);
-	cmd.data = cpu_to_le32(value);
-
-	if (mask != 0xFF) {
-		cmd.length = 12;
-		cmd.mask = cpu_to_le32(mask);
-	}
-	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
-}
-
-int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr, u32 value, u32 mask)
-{
-	struct ioreg_cfg cmd = {8, IOREG_CMD_WD_REG, 0x0, 0x0, 0x0};
-
-	cmd.address = cpu_to_le16(addr);
-	cmd.data = cpu_to_le32(value);
-
-	if (mask != 0xFFFFFFFF) {
-		cmd.length = 12;
-		cmd.mask =  cpu_to_le32(mask);
-	}
-	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
-}
-
-int _rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path, u16 addr, u32 value, u32 mask)
-{
-	struct ioreg_cfg cmd = {8, IOREG_CMD_W_RF, 0x0, 0x0, 0x0};
-
-	cmd.address = cpu_to_le16((rf_path<<8) | ((addr) & 0xFF));
-	cmd.data = cpu_to_le32(value);
-
-	if (mask != 0x000FFFFF) {
-		cmd.length = 12;
-		cmd.mask =  cpu_to_le32(mask);
-	}
-	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, cmd.length);
-}
-
-int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us)
-{
-	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
-	cmd.address = cpu_to_le16(us);
-
-	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
-}
-
-int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms)
-{
-	struct ioreg_cfg cmd = {4, IOREG_CMD_DELAY_US, 0x0, 0x0, 0x0};
-
-	cmd.address = cpu_to_le16(ms);
-	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
-}
-
-int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame)
-{
-	struct ioreg_cfg cmd = {4, IOREG_CMD_END, cpu_to_le16(0xFFFF), cpu_to_le32(0xFF), 0x0};
-
-	return rtw_IOL_append_cmds(xmit_frame, (u8 *)&cmd, 4);
-}
-
-u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame)
-{
-	u8 is_cmd_bndy = false;
-	if (((pxmit_frame->attrib.pktlen+32)%256) + 8 >= 256) {
-		rtw_IOL_append_END_cmd(pxmit_frame);
-		pxmit_frame->attrib.pktlen = ((((pxmit_frame->attrib.pktlen+32)/256)+1)*256);
-
-		pxmit_frame->attrib.last_txcmdsz = pxmit_frame->attrib.pktlen;
-		is_cmd_bndy = true;
-	}
-	return is_cmd_bndy;
-}
-
-void rtw_IOL_cmd_buf_dump(struct adapter  *Adapter, int buf_len, u8 *pbuf)
-{
-	int i;
-	int j = 1;
-
-	pr_info("###### %s ######\n", __func__);
-	for (i = 0; i < buf_len; i++) {
-		printk("%02x-", *(pbuf+i));
-
-		if (j%32 == 0)
-			printk("\n");
-		j++;
-	}
-	printk("\n");
-	pr_info("=============ioreg_cmd len=%d===============\n", buf_len);
-}
diff --git a/drivers/staging/rtl8188eu/hal/HalHWImg8188E_MAC.c b/drivers/staging/rtl8188eu/hal/HalHWImg8188E_MAC.c
deleted file mode 100644
index b49b5ab48b18..000000000000
--- a/drivers/staging/rtl8188eu/hal/HalHWImg8188E_MAC.c
+++ /dev/null
@@ -1,231 +0,0 @@
-/******************************************************************************
-*
-* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
-*
-* This program is free software; you can redistribute it and/or modify it
-* under the terms of version 2 of the GNU General Public License as
-* published by the Free Software Foundation.
-*
-* This program is distributed in the hope that it will be useful, but WITHOUT
-* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
-* more details.
-*
-* You should have received a copy of the GNU General Public License along with
-* this program; if not, write to the Free Software Foundation, Inc.,
-* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
-*
-*
-******************************************************************************/
-
-#include "odm_precomp.h"
-#include <rtw_iol.h>
-
-static bool Checkcondition(const u32  condition, const u32  hex)
-{
-	u32 _board     = (hex & 0x000000FF);
-	u32 _interface = (hex & 0x0000FF00) >> 8;
-	u32 _platform  = (hex & 0x00FF0000) >> 16;
-	u32 cond = condition;
-
-	if (condition == 0xCDCDCDCD)
-		return true;
-
-	cond = condition & 0x000000FF;
-	if ((_board == cond) && cond != 0x00)
-		return false;
-
-	cond = condition & 0x0000FF00;
-	cond = cond >> 8;
-	if ((_interface & cond) == 0 && cond != 0x07)
-		return false;
-
-	cond = condition & 0x00FF0000;
-	cond = cond >> 16;
-	if ((_platform & cond) == 0 && cond != 0x0F)
-		return false;
-	return true;
-}
-
-
-/******************************************************************************
-*                           MAC_REG.TXT
-******************************************************************************/
-
-static u32 array_MAC_REG_8188E[] = {
-		0x026, 0x00000041,
-		0x027, 0x00000035,
-		0x428, 0x0000000A,
-		0x429, 0x00000010,
-		0x430, 0x00000000,
-		0x431, 0x00000001,
-		0x432, 0x00000002,
-		0x433, 0x00000004,
-		0x434, 0x00000005,
-		0x435, 0x00000006,
-		0x436, 0x00000007,
-		0x437, 0x00000008,
-		0x438, 0x00000000,
-		0x439, 0x00000000,
-		0x43A, 0x00000001,
-		0x43B, 0x00000002,
-		0x43C, 0x00000004,
-		0x43D, 0x00000005,
-		0x43E, 0x00000006,
-		0x43F, 0x00000007,
-		0x440, 0x0000005D,
-		0x441, 0x00000001,
-		0x442, 0x00000000,
-		0x444, 0x00000015,
-		0x445, 0x000000F0,
-		0x446, 0x0000000F,
-		0x447, 0x00000000,
-		0x458, 0x00000041,
-		0x459, 0x000000A8,
-		0x45A, 0x00000072,
-		0x45B, 0x000000B9,
-		0x460, 0x00000066,
-		0x461, 0x00000066,
-		0x480, 0x00000008,
-		0x4C8, 0x000000FF,
-		0x4C9, 0x00000008,
-		0x4CC, 0x000000FF,
-		0x4CD, 0x000000FF,
-		0x4CE, 0x00000001,
-		0x4D3, 0x00000001,
-		0x500, 0x00000026,
-		0x501, 0x000000A2,
-		0x502, 0x0000002F,
-		0x503, 0x00000000,
-		0x504, 0x00000028,
-		0x505, 0x000000A3,
-		0x506, 0x0000005E,
-		0x507, 0x00000000,
-		0x508, 0x0000002B,
-		0x509, 0x000000A4,
-		0x50A, 0x0000005E,
-		0x50B, 0x00000000,
-		0x50C, 0x0000004F,
-		0x50D, 0x000000A4,
-		0x50E, 0x00000000,
-		0x50F, 0x00000000,
-		0x512, 0x0000001C,
-		0x514, 0x0000000A,
-		0x516, 0x0000000A,
-		0x525, 0x0000004F,
-		0x550, 0x00000010,
-		0x551, 0x00000010,
-		0x559, 0x00000002,
-		0x55D, 0x000000FF,
-		0x605, 0x00000030,
-		0x608, 0x0000000E,
-		0x609, 0x0000002A,
-		0x620, 0x000000FF,
-		0x621, 0x000000FF,
-		0x622, 0x000000FF,
-		0x623, 0x000000FF,
-		0x624, 0x000000FF,
-		0x625, 0x000000FF,
-		0x626, 0x000000FF,
-		0x627, 0x000000FF,
-		0x652, 0x00000020,
-		0x63C, 0x0000000A,
-		0x63D, 0x0000000A,
-		0x63E, 0x0000000E,
-		0x63F, 0x0000000E,
-		0x640, 0x00000040,
-		0x66E, 0x00000005,
-		0x700, 0x00000021,
-		0x701, 0x00000043,
-		0x702, 0x00000065,
-		0x703, 0x00000087,
-		0x708, 0x00000021,
-		0x709, 0x00000043,
-		0x70A, 0x00000065,
-		0x70B, 0x00000087,
-};
-
-enum HAL_STATUS ODM_ReadAndConfig_MAC_REG_8188E(struct odm_dm_struct *dm_odm)
-{
-	#define READ_NEXT_PAIR(v1, v2, i) do { i += 2; v1 = array[i]; v2 = array[i+1]; } while (0)
-
-	u32     hex         = 0;
-	u32     i;
-	u8     platform    = dm_odm->SupportPlatform;
-	u8     interface_val   = dm_odm->SupportInterface;
-	u8     board       = dm_odm->BoardType;
-	u32     array_len    = sizeof(array_MAC_REG_8188E)/sizeof(u32);
-	u32    *array       = array_MAC_REG_8188E;
-	bool	biol = false;
-
-	struct adapter *adapt =  dm_odm->Adapter;
-	struct xmit_frame	*pxmit_frame = NULL;
-	u8 bndy_cnt = 1;
-	enum HAL_STATUS rst = HAL_STATUS_SUCCESS;
-	hex += board;
-	hex += interface_val << 8;
-	hex += platform << 16;
-	hex += 0xFF000000;
-
-	biol = rtw_IOL_applied(adapt);
-
-	if (biol) {
-		pxmit_frame = rtw_IOL_accquire_xmit_frame(adapt);
-		if (pxmit_frame == NULL) {
-			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
-			return HAL_STATUS_FAILURE;
-		}
-	}
-
-	for (i = 0; i < array_len; i += 2) {
-		u32 v1 = array[i];
-		u32 v2 = array[i+1];
-
-		/*  This (offset, data) pair meets the condition. */
-		if (v1 < 0xCDCDCDCD) {
-				if (biol) {
-					if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-						bndy_cnt++;
-					rtw_IOL_append_WB_cmd(pxmit_frame, (u16)v1, (u8)v2, 0xFF);
-				} else {
-					odm_ConfigMAC_8188E(dm_odm, v1, (u8)v2);
-				}
-				continue;
-		} else { /*  This line is the start line of branch. */
-			if (!Checkcondition(array[i], hex)) {
-				/*  Discard the following (offset, data) pairs. */
-				READ_NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < array_len - 2) {
-					READ_NEXT_PAIR(v1, v2, i);
-				}
-				i -= 2; /*  prevent from for-loop += 2 */
-			} else { /*  Configure matched pairs and skip to end of if-else. */
-				READ_NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < array_len - 2) {
-					if (biol) {
-						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-							bndy_cnt++;
-						rtw_IOL_append_WB_cmd(pxmit_frame, (u16)v1, (u8)v2, 0xFF);
-					} else {
-						odm_ConfigMAC_8188E(dm_odm, v1, (u8)v2);
-					}
-
-					READ_NEXT_PAIR(v1, v2, i);
-				}
-				while (v2 != 0xDEAD && i < array_len - 2)
-					READ_NEXT_PAIR(v1, v2, i);
-			}
-		}
-	}
-	if (biol) {
-		if (!rtw_IOL_exec_cmds_sync(dm_odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
-			pr_info("~~~ MAC IOL_exec_cmds Failed !!!\n");
-			rst = HAL_STATUS_FAILURE;
-		}
-	}
-	return rst;
-}
diff --git a/drivers/staging/rtl8188eu/hal/HalHWImg8188E_RF.c b/drivers/staging/rtl8188eu/hal/HalHWImg8188E_RF.c
deleted file mode 100644
index 17c6411ce8ac..000000000000
--- a/drivers/staging/rtl8188eu/hal/HalHWImg8188E_RF.c
+++ /dev/null
@@ -1,269 +0,0 @@
-/******************************************************************************
-*
-* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
-*
-* This program is free software; you can redistribute it and/or modify it
-* under the terms of version 2 of the GNU General Public License as
-* published by the Free Software Foundation.
-*
-* This program is distributed in the hope that it will be useful, but WITHOUT
-* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
-* more details.
-*
-* You should have received a copy of the GNU General Public License along with
-* this program; if not, write to the Free Software Foundation, Inc.,
-* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
-*
-*
-******************************************************************************/
-
-#include "odm_precomp.h"
-
-#include <rtw_iol.h>
-
-static bool CheckCondition(const u32  Condition, const u32  Hex)
-{
-	u32 _board     = (Hex & 0x000000FF);
-	u32 _interface = (Hex & 0x0000FF00) >> 8;
-	u32 _platform  = (Hex & 0x00FF0000) >> 16;
-	u32 cond = Condition;
-
-	if (Condition == 0xCDCDCDCD)
-		return true;
-
-	cond = Condition & 0x000000FF;
-	if ((_board == cond) && cond != 0x00)
-		return false;
-
-	cond = Condition & 0x0000FF00;
-	cond = cond >> 8;
-	if ((_interface & cond) == 0 && cond != 0x07)
-		return false;
-
-	cond = Condition & 0x00FF0000;
-	cond = cond >> 16;
-	if ((_platform & cond) == 0 && cond != 0x0F)
-		return false;
-	return true;
-}
-
-
-/******************************************************************************
-*                           RadioA_1T.TXT
-******************************************************************************/
-
-static u32 Array_RadioA_1T_8188E[] = {
-		0x000, 0x00030000,
-		0x008, 0x00084000,
-		0x018, 0x00000407,
-		0x019, 0x00000012,
-		0x01E, 0x00080009,
-		0x01F, 0x00000880,
-		0x02F, 0x0001A060,
-		0x03F, 0x00000000,
-		0x042, 0x000060C0,
-		0x057, 0x000D0000,
-		0x058, 0x000BE180,
-		0x067, 0x00001552,
-		0x083, 0x00000000,
-		0x0B0, 0x000FF8FC,
-		0x0B1, 0x00054400,
-		0x0B2, 0x000CCC19,
-		0x0B4, 0x00043003,
-		0x0B6, 0x0004953E,
-		0x0B7, 0x0001C718,
-		0x0B8, 0x000060FF,
-		0x0B9, 0x00080001,
-		0x0BA, 0x00040000,
-		0x0BB, 0x00000400,
-		0x0BF, 0x000C0000,
-		0x0C2, 0x00002400,
-		0x0C3, 0x00000009,
-		0x0C4, 0x00040C91,
-		0x0C5, 0x00099999,
-		0x0C6, 0x000000A3,
-		0x0C7, 0x00088820,
-		0x0C8, 0x00076C06,
-		0x0C9, 0x00000000,
-		0x0CA, 0x00080000,
-		0x0DF, 0x00000180,
-		0x0EF, 0x000001A0,
-		0x051, 0x0006B27D,
-		0xFF0F041F, 0xABCD,
-		0x052, 0x0007E4DD,
-		0xCDCDCDCD, 0xCDCD,
-		0x052, 0x0007E49D,
-		0xFF0F041F, 0xDEAD,
-		0x053, 0x00000073,
-		0x056, 0x00051FF3,
-		0x035, 0x00000086,
-		0x035, 0x00000186,
-		0x035, 0x00000286,
-		0x036, 0x00001C25,
-		0x036, 0x00009C25,
-		0x036, 0x00011C25,
-		0x036, 0x00019C25,
-		0x0B6, 0x00048538,
-		0x018, 0x00000C07,
-		0x05A, 0x0004BD00,
-		0x019, 0x000739D0,
-		0x034, 0x0000ADF3,
-		0x034, 0x00009DF0,
-		0x034, 0x00008DED,
-		0x034, 0x00007DEA,
-		0x034, 0x00006DE7,
-		0x034, 0x000054EE,
-		0x034, 0x000044EB,
-		0x034, 0x000034E8,
-		0x034, 0x0000246B,
-		0x034, 0x00001468,
-		0x034, 0x0000006D,
-		0x000, 0x00030159,
-		0x084, 0x00068200,
-		0x086, 0x000000CE,
-		0x087, 0x00048A00,
-		0x08E, 0x00065540,
-		0x08F, 0x00088000,
-		0x0EF, 0x000020A0,
-		0x03B, 0x000F02B0,
-		0x03B, 0x000EF7B0,
-		0x03B, 0x000D4FB0,
-		0x03B, 0x000CF060,
-		0x03B, 0x000B0090,
-		0x03B, 0x000A0080,
-		0x03B, 0x00090080,
-		0x03B, 0x0008F780,
-		0x03B, 0x000722B0,
-		0x03B, 0x0006F7B0,
-		0x03B, 0x00054FB0,
-		0x03B, 0x0004F060,
-		0x03B, 0x00030090,
-		0x03B, 0x00020080,
-		0x03B, 0x00010080,
-		0x03B, 0x0000F780,
-		0x0EF, 0x000000A0,
-		0x000, 0x00010159,
-		0x018, 0x0000F407,
-		0xFFE, 0x00000000,
-		0xFFE, 0x00000000,
-		0x01F, 0x00080003,
-		0xFFE, 0x00000000,
-		0xFFE, 0x00000000,
-		0x01E, 0x00000001,
-		0x01F, 0x00080000,
-		0x000, 0x00033E60,
-};
-
-enum HAL_STATUS ODM_ReadAndConfig_RadioA_1T_8188E(struct odm_dm_struct *pDM_Odm)
-{
-	#define READ_NEXT_PAIR(v1, v2, i) do	\
-		 { i += 2; v1 = Array[i];	\
-		 v2 = Array[i+1]; } while (0)
-
-	u32     hex         = 0;
-	u32     i           = 0;
-	u8     platform    = pDM_Odm->SupportPlatform;
-	u8     interfaceValue   = pDM_Odm->SupportInterface;
-	u8     board       = pDM_Odm->BoardType;
-	u32     ArrayLen    = sizeof(Array_RadioA_1T_8188E)/sizeof(u32);
-	u32    *Array       = Array_RadioA_1T_8188E;
-	bool		biol = false;
-	struct adapter *Adapter =  pDM_Odm->Adapter;
-	struct xmit_frame *pxmit_frame = NULL;
-	u8 bndy_cnt = 1;
-	enum HAL_STATUS rst = HAL_STATUS_SUCCESS;
-
-	hex += board;
-	hex += interfaceValue << 8;
-	hex += platform << 16;
-	hex += 0xFF000000;
-	biol = rtw_IOL_applied(Adapter);
-
-	if (biol) {
-		pxmit_frame = rtw_IOL_accquire_xmit_frame(Adapter);
-		if (pxmit_frame == NULL) {
-			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
-			return HAL_STATUS_FAILURE;
-		}
-	}
-
-	for (i = 0; i < ArrayLen; i += 2) {
-		u32 v1 = Array[i];
-		u32 v2 = Array[i+1];
-
-		/*  This (offset, data) pair meets the condition. */
-		if (v1 < 0xCDCDCDCD) {
-			if (biol) {
-				if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-					bndy_cnt++;
-
-				if (v1 == 0xffe)
-					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
-				else if (v1 == 0xfd)
-					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
-				else if (v1 == 0xfc)
-					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
-				else if (v1 == 0xfb)
-					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
-				else if (v1 == 0xfa)
-					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
-				else if (v1 == 0xf9)
-					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
-				else
-					rtw_IOL_append_WRF_cmd(pxmit_frame, RF_PATH_A, (u16)v1, v2, bRFRegOffsetMask);
-			} else {
-				odm_ConfigRF_RadioA_8188E(pDM_Odm, v1, v2);
-			}
-		    continue;
-		} else { /*  This line is the start line of branch. */
-			if (!CheckCondition(Array[i], hex)) {
-				/*  Discard the following (offset, data) pairs. */
-				READ_NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < ArrayLen - 2)
-					READ_NEXT_PAIR(v1, v2, i);
-				i -= 2; /*  prevent from for-loop += 2 */
-			} else { /*  Configure matched pairs and skip to end of if-else. */
-			READ_NEXT_PAIR(v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < ArrayLen - 2) {
-					if (biol) {
-						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-							bndy_cnt++;
-
-						if (v1 == 0xffe)
-							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
-						else if (v1 == 0xfd)
-							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
-						else if (v1 == 0xfc)
-							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
-						else if (v1 == 0xfb)
-							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
-						else if (v1 == 0xfa)
-							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
-						else if (v1 == 0xf9)
-							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
-						else
-							rtw_IOL_append_WRF_cmd(pxmit_frame, RF_PATH_A, (u16)v1, v2, bRFRegOffsetMask);
-					} else {
-						odm_ConfigRF_RadioA_8188E(pDM_Odm, v1, v2);
-					}
-					READ_NEXT_PAIR(v1, v2, i);
-				}
-
-				while (v2 != 0xDEAD && i < ArrayLen - 2)
-					READ_NEXT_PAIR(v1, v2, i);
-			}
-		}
-	}
-	if (biol) {
-		if (!rtw_IOL_exec_cmds_sync(pDM_Odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
-			rst = HAL_STATUS_FAILURE;
-			pr_info("~~~ IOL Config %s Failed !!!\n", __func__);
-		}
-	}
-	return rst;
-}
diff --git a/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c b/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
deleted file mode 100644
index d2bcc1640522..000000000000
--- a/drivers/staging/rtl8188eu/hal/HalPhyRf_8188e.c
+++ /dev/null
@@ -1,1467 +0,0 @@
-/*
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- */
-
-#include "odm_precomp.h"
-
-/*  2010/04/25 MH Define the max tx power tracking tx agc power. */
-#define		ODM_TXPWRTRACK_MAX_IDX_88E		6
-
-
-static u8 ODM_GetRightChnlPlaceforIQK(u8 chnl)
-{
-	u8	channel_all[ODM_TARGET_CHNL_NUM_2G_5G] = {
-		1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
-		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64,
-		100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122,
-		124, 126, 128, 130, 132, 134, 136, 138, 140, 149, 151, 153,
-		155, 157, 159, 161, 163, 165
-	};
-	u8	place = chnl;
-
-	if (chnl > 14) {
-		for (place = 14; place < sizeof(channel_all); place++) {
-			if (channel_all[place] == chnl)
-				return place-13;
-		}
-	}
-	return 0;
-}
-
-/* 3 Tx Power Tracking */
-/*
- * Function:	ODM_TxPwrTrackAdjust88E()
- *
- * Overview:	88E we can not write 0xc80/c94/c4c/ 0xa2x. Instead of write TX agc.
- *				No matter OFDM & CCK use the same method.
- *
- * Revised History:
- *	When		Who		Remark
- *	04/23/2012	MHC		Create Version 0.
- *	04/23/2012	MHC		Adjust TX agc directly not throughput BB digital.
- *
- */
-void ODM_TxPwrTrackAdjust88E(struct odm_dm_struct *dm_odm, u8 Type,/*  0 = OFDM, 1 = CCK */
-	u8 *pDirection, 		/*  1 = +(increase) 2 = -(decrease) */
-	u32 *pOutWriteVal		/*  Tx tracking CCK/OFDM BB swing index adjust */
-	)
-{
-	u8 pwr_value = 0;
-	/*  Tx power tracking BB swing table. */
-	/*  The base index = 12. +((12-n)/2)dB 13~?? = decrease tx pwr by -((n-12)/2)dB */
-	if (Type == 0) {		/*  For OFDM afjust */
-		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
-			     ("BbSwingIdxOfdm = %d BbSwingFlagOfdm=%d\n",
-			     dm_odm->BbSwingIdxOfdm, dm_odm->BbSwingFlagOfdm));
-
-		if (dm_odm->BbSwingIdxOfdm <= dm_odm->BbSwingIdxOfdmBase) {
-			*pDirection	= 1;
-			pwr_value		= (dm_odm->BbSwingIdxOfdmBase - dm_odm->BbSwingIdxOfdm);
-		} else {
-			*pDirection	= 2;
-			pwr_value		= (dm_odm->BbSwingIdxOfdm - dm_odm->BbSwingIdxOfdmBase);
-		}
-
-		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
-			     ("BbSwingIdxOfdm = %d BbSwingFlagOfdm=%d\n",
-			     dm_odm->BbSwingIdxOfdm, dm_odm->BbSwingFlagOfdm));
-	} else if (Type == 1) {	/*  For CCK adjust. */
-		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
-			     ("dm_odm->BbSwingIdxCck = %d dm_odm->BbSwingIdxCckBase = %d\n",
-			     dm_odm->BbSwingIdxCck, dm_odm->BbSwingIdxCckBase));
-
-		if (dm_odm->BbSwingIdxCck <= dm_odm->BbSwingIdxCckBase) {
-			*pDirection	= 1;
-			pwr_value		= (dm_odm->BbSwingIdxCckBase - dm_odm->BbSwingIdxCck);
-		} else {
-			*pDirection	= 2;
-			pwr_value		= (dm_odm->BbSwingIdxCck - dm_odm->BbSwingIdxCckBase);
-		}
-	}
-
-	/*  */
-	/*  2012/04/25 MH According to Ed/Luke.Lees estimate for EVM the max tx power tracking */
-	/*  need to be less than 6 power index for 88E. */
-	/*  */
-	if (pwr_value >= ODM_TXPWRTRACK_MAX_IDX_88E && *pDirection == 1)
-		pwr_value = ODM_TXPWRTRACK_MAX_IDX_88E;
-
-	*pOutWriteVal = pwr_value | (pwr_value<<8) | (pwr_value<<16) | (pwr_value<<24);
-}	/*  ODM_TxPwrTrackAdjust88E */
-
-/*
- * Function:	odm_TxPwrTrackSetPwr88E()
- *
- * Overview:	88E change all channel tx power accordign to flag.
- *				OFDM & CCK are all different.
- */
-static void odm_TxPwrTrackSetPwr88E(struct odm_dm_struct *dm_odm)
-{
-	if (dm_odm->BbSwingFlagOfdm || dm_odm->BbSwingFlagCck) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD, ("odm_TxPwrTrackSetPwr88E CH=%d\n", *(dm_odm->pChannel)));
-		PHY_SetTxPowerLevel8188E(dm_odm->Adapter, *(dm_odm->pChannel));
-		dm_odm->BbSwingFlagOfdm = false;
-		dm_odm->BbSwingFlagCck	= false;
-	}
-}	/*  odm_TxPwrTrackSetPwr88E */
-
-void
-odm_TXPowerTrackingCallback_ThermalMeter_8188E(
-	struct adapter *Adapter
-	)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	u8 ThermalValue = 0, delta, delta_LCK, delta_IQK, offset;
-	u8 ThermalValue_AVG_count = 0;
-	u32 ThermalValue_AVG = 0;
-	s32 ele_A = 0, ele_D, TempCCk, X, value32;
-	s32 Y, ele_C = 0;
-	s8 OFDM_index[2], CCK_index = 0;
-	s8 OFDM_index_old[2] = {0, 0}, CCK_index_old = 0;
-	u32 i = 0, j = 0;
-	bool is2t = false;
-
-	u8 OFDM_min_index = 6, rf; /* OFDM BB Swing should be less than +3.0dB, which is required by Arthur */
-	u8 Indexforchannel = 0/*GetRightChnlPlaceforIQK(pHalData->CurrentChannel)*/;
-	s8 OFDM_index_mapping[2][index_mapping_NUM_88E] = {
-		{0, 0, 2, 3, 4, 4, 		/* 2.4G, decrease power */
-		5, 6, 7, 7, 8, 9,
-		10, 10, 11}, /*  For lower temperature, 20120220 updated on 20120220. */
-		{0, 0, -1, -2, -3, -4, 		/* 2.4G, increase power */
-		-4, -4, -4, -5, -7, -8,
-		-9, -9, -10},
-	};
-	u8 Thermal_mapping[2][index_mapping_NUM_88E] = {
-		{0, 2, 4, 6, 8, 10, 		/* 2.4G, decrease power */
-		12, 14, 16, 18, 20, 22,
-		24, 26, 27},
-		{0, 2, 4, 6, 8, 10, 		/* 2.4G,, increase power */
-		12, 14, 16, 18, 20, 22,
-		25, 25, 25},
-	};
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	/*  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E. */
-	odm_TxPwrTrackSetPwr88E(dm_odm);
-
-	dm_odm->RFCalibrateInfo.TXPowerTrackingCallbackCnt++; /* cosa add for debug */
-	dm_odm->RFCalibrateInfo.bTXPowerTrackingInit = true;
-
-	/*  <Kordan> RFCalibrateInfo.RegA24 will be initialized when ODM HW configuring, but MP configures with para files. */
-	dm_odm->RFCalibrateInfo.RegA24 = 0x090e1317;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("===>dm_TXPowerTrackingCallback_ThermalMeter_8188E txpowercontrol %d\n",
-		     dm_odm->RFCalibrateInfo.TxPowerTrackControl));
-
-	ThermalValue = (u8)PHY_QueryRFReg(Adapter, RF_PATH_A, RF_T_METER_88E, 0xfc00);	/* 0x42: RF Reg[15:10] 88E */
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("Readback Thermal Meter = 0x%x pre thermal meter 0x%x EEPROMthermalmeter 0x%x\n",
-		     ThermalValue, dm_odm->RFCalibrateInfo.ThermalValue, pHalData->EEPROMThermalMeter));
-
-	if (is2t)
-		rf = 2;
-	else
-		rf = 1;
-
-	if (ThermalValue) {
-		/* Query OFDM path A default setting */
-		ele_D = PHY_QueryBBReg(Adapter, rOFDM0_XATxIQImbalance, bMaskDWord)&bMaskOFDM_D;
-		for (i = 0; i < OFDM_TABLE_SIZE_92D; i++) {	/* find the index */
-			if (ele_D == (OFDMSwingTable[i]&bMaskOFDM_D)) {
-				OFDM_index_old[0] = (u8)i;
-				dm_odm->BbSwingIdxOfdmBase = (u8)i;
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("Initial pathA ele_D reg0x%x = 0x%x, OFDM_index=0x%x\n",
-					     rOFDM0_XATxIQImbalance, ele_D, OFDM_index_old[0]));
-				break;
-			}
-		}
-
-		/* Query OFDM path B default setting */
-		if (is2t) {
-			ele_D = PHY_QueryBBReg(Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord)&bMaskOFDM_D;
-			for (i = 0; i < OFDM_TABLE_SIZE_92D; i++) {	/* find the index */
-				if (ele_D == (OFDMSwingTable[i]&bMaskOFDM_D)) {
-					OFDM_index_old[1] = (u8)i;
-					ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-						     ("Initial pathB ele_D reg0x%x = 0x%x, OFDM_index=0x%x\n",
-						rOFDM0_XBTxIQImbalance, ele_D, OFDM_index_old[1]));
-					break;
-				}
-			}
-		}
-
-		/* Query CCK default setting From 0xa24 */
-		TempCCk = dm_odm->RFCalibrateInfo.RegA24;
-
-		for (i = 0; i < CCK_TABLE_SIZE; i++) {
-			if (dm_odm->RFCalibrateInfo.bCCKinCH14) {
-				if (memcmp(&TempCCk, &CCKSwingTable_Ch14[i][2], 4)) {
-					CCK_index_old = (u8)i;
-					dm_odm->BbSwingIdxCckBase = (u8)i;
-					ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-						     ("Initial reg0x%x = 0x%x, CCK_index=0x%x, ch 14 %d\n",
-						     rCCK0_TxFilter2, TempCCk, CCK_index_old, dm_odm->RFCalibrateInfo.bCCKinCH14));
-					break;
-				}
-			} else {
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("RegA24: 0x%X, CCKSwingTable_Ch1_Ch13[%d][2]: CCKSwingTable_Ch1_Ch13[i][2]: 0x%X\n",
-					     TempCCk, i, CCKSwingTable_Ch1_Ch13[i][2]));
-				if (memcmp(&TempCCk, &CCKSwingTable_Ch1_Ch13[i][2], 4)) {
-					CCK_index_old = (u8)i;
-					dm_odm->BbSwingIdxCckBase = (u8)i;
-					ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-						     ("Initial reg0x%x = 0x%x, CCK_index=0x%x, ch14 %d\n",
-						     rCCK0_TxFilter2, TempCCk, CCK_index_old, dm_odm->RFCalibrateInfo.bCCKinCH14));
-					break;
-				}
-			}
-		}
-
-		if (!dm_odm->RFCalibrateInfo.ThermalValue) {
-			dm_odm->RFCalibrateInfo.ThermalValue = pHalData->EEPROMThermalMeter;
-			dm_odm->RFCalibrateInfo.ThermalValue_LCK = ThermalValue;
-			dm_odm->RFCalibrateInfo.ThermalValue_IQK = ThermalValue;
-
-			for (i = 0; i < rf; i++)
-				dm_odm->RFCalibrateInfo.OFDM_index[i] = OFDM_index_old[i];
-			dm_odm->RFCalibrateInfo.CCK_index = CCK_index_old;
-		}
-
-		if (dm_odm->RFCalibrateInfo.bReloadtxpowerindex)
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-				     ("reload ofdm index for band switch\n"));
-
-		/* calculate average thermal meter */
-		dm_odm->RFCalibrateInfo.ThermalValue_AVG[dm_odm->RFCalibrateInfo.ThermalValue_AVG_index] = ThermalValue;
-		dm_odm->RFCalibrateInfo.ThermalValue_AVG_index++;
-		if (dm_odm->RFCalibrateInfo.ThermalValue_AVG_index == AVG_THERMAL_NUM_88E)
-			dm_odm->RFCalibrateInfo.ThermalValue_AVG_index = 0;
-
-		for (i = 0; i < AVG_THERMAL_NUM_88E; i++) {
-			if (dm_odm->RFCalibrateInfo.ThermalValue_AVG[i]) {
-				ThermalValue_AVG += dm_odm->RFCalibrateInfo.ThermalValue_AVG[i];
-				ThermalValue_AVG_count++;
-			}
-		}
-
-		if (ThermalValue_AVG_count) {
-			ThermalValue = (u8)(ThermalValue_AVG / ThermalValue_AVG_count);
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-				     ("AVG Thermal Meter = 0x%x\n", ThermalValue));
-		}
-
-		if (dm_odm->RFCalibrateInfo.bReloadtxpowerindex) {
-			delta = ThermalValue > pHalData->EEPROMThermalMeter ?
-				(ThermalValue - pHalData->EEPROMThermalMeter) :
-				(pHalData->EEPROMThermalMeter - ThermalValue);
-			dm_odm->RFCalibrateInfo.bReloadtxpowerindex = false;
-			dm_odm->RFCalibrateInfo.bDoneTxpower = false;
-		} else if (dm_odm->RFCalibrateInfo.bDoneTxpower) {
-			delta = (ThermalValue > dm_odm->RFCalibrateInfo.ThermalValue) ?
-				(ThermalValue - dm_odm->RFCalibrateInfo.ThermalValue) :
-				(dm_odm->RFCalibrateInfo.ThermalValue - ThermalValue);
-		} else {
-			delta = ThermalValue > pHalData->EEPROMThermalMeter ?
-				(ThermalValue - pHalData->EEPROMThermalMeter) :
-				(pHalData->EEPROMThermalMeter - ThermalValue);
-		}
-		delta_LCK = (ThermalValue > dm_odm->RFCalibrateInfo.ThermalValue_LCK) ?
-			    (ThermalValue - dm_odm->RFCalibrateInfo.ThermalValue_LCK) :
-			    (dm_odm->RFCalibrateInfo.ThermalValue_LCK - ThermalValue);
-		delta_IQK = (ThermalValue > dm_odm->RFCalibrateInfo.ThermalValue_IQK) ?
-			    (ThermalValue - dm_odm->RFCalibrateInfo.ThermalValue_IQK) :
-			    (dm_odm->RFCalibrateInfo.ThermalValue_IQK - ThermalValue);
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-			     ("Readback Thermal Meter = 0x%x pre thermal meter 0x%x EEPROMthermalmeter 0x%x delta 0x%x delta_LCK 0x%x delta_IQK 0x%x\n",
-			     ThermalValue, dm_odm->RFCalibrateInfo.ThermalValue,
-			     pHalData->EEPROMThermalMeter, delta, delta_LCK, delta_IQK));
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-			     ("pre thermal meter LCK 0x%x pre thermal meter IQK 0x%x delta_LCK_bound 0x%x delta_IQK_bound 0x%x\n",
-			     dm_odm->RFCalibrateInfo.ThermalValue_LCK,
-			     dm_odm->RFCalibrateInfo.ThermalValue_IQK,
-			     dm_odm->RFCalibrateInfo.Delta_LCK,
-			     dm_odm->RFCalibrateInfo.Delta_IQK));
-
-		if ((delta_LCK >= 8)) { /*  Delta temperature is equal to or larger than 20 centigrade. */
-			dm_odm->RFCalibrateInfo.ThermalValue_LCK = ThermalValue;
-			PHY_LCCalibrate_8188E(Adapter);
-		}
-
-		if (delta > 0 && dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
-			delta = ThermalValue > pHalData->EEPROMThermalMeter ?
-				(ThermalValue - pHalData->EEPROMThermalMeter) :
-				(pHalData->EEPROMThermalMeter - ThermalValue);
-			/* calculate new OFDM / CCK offset */
-			if (ThermalValue > pHalData->EEPROMThermalMeter)
-				j = 1;
-			else
-				j = 0;
-			for (offset = 0; offset < index_mapping_NUM_88E; offset++) {
-				if (delta < Thermal_mapping[j][offset]) {
-					if (offset != 0)
-						offset--;
-					break;
-				}
-			}
-			if (offset >= index_mapping_NUM_88E)
-				offset = index_mapping_NUM_88E-1;
-			for (i = 0; i < rf; i++)
-				OFDM_index[i] = dm_odm->RFCalibrateInfo.OFDM_index[i] + OFDM_index_mapping[j][offset];
-			CCK_index = dm_odm->RFCalibrateInfo.CCK_index + OFDM_index_mapping[j][offset];
-
-			if (is2t) {
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("temp OFDM_A_index=0x%x, OFDM_B_index=0x%x, CCK_index=0x%x\n",
-					     dm_odm->RFCalibrateInfo.OFDM_index[0],
-					     dm_odm->RFCalibrateInfo.OFDM_index[1],
-					     dm_odm->RFCalibrateInfo.CCK_index));
-			} else {
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("temp OFDM_A_index=0x%x, CCK_index=0x%x\n",
-					     dm_odm->RFCalibrateInfo.OFDM_index[0],
-					     dm_odm->RFCalibrateInfo.CCK_index));
-			}
-
-			for (i = 0; i < rf; i++) {
-				if (OFDM_index[i] > OFDM_TABLE_SIZE_92D-1)
-					OFDM_index[i] = OFDM_TABLE_SIZE_92D-1;
-				else if (OFDM_index[i] < OFDM_min_index)
-					OFDM_index[i] = OFDM_min_index;
-			}
-
-			if (CCK_index > CCK_TABLE_SIZE-1)
-				CCK_index = CCK_TABLE_SIZE-1;
-			else if (CCK_index < 0)
-				CCK_index = 0;
-
-			if (is2t) {
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("new OFDM_A_index=0x%x, OFDM_B_index=0x%x, CCK_index=0x%x\n",
-					     OFDM_index[0], OFDM_index[1], CCK_index));
-			} else {
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("new OFDM_A_index=0x%x, CCK_index=0x%x\n",
-					     OFDM_index[0], CCK_index));
-			}
-
-			/* 2 temporarily remove bNOPG */
-			/* Config by SwingTable */
-			if (dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
-				dm_odm->RFCalibrateInfo.bDoneTxpower = true;
-
-				/* Adujst OFDM Ant_A according to IQK result */
-				ele_D = (OFDMSwingTable[(u8)OFDM_index[0]] & 0xFFC00000)>>22;
-				X = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].Value[0][0];
-				Y = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].Value[0][1];
-
-				/*  Revse TX power table. */
-				dm_odm->BbSwingIdxOfdm		= (u8)OFDM_index[0];
-				dm_odm->BbSwingIdxCck		= (u8)CCK_index;
-
-				if (dm_odm->BbSwingIdxOfdmCurrent != dm_odm->BbSwingIdxOfdm) {
-					dm_odm->BbSwingIdxOfdmCurrent = dm_odm->BbSwingIdxOfdm;
-					dm_odm->BbSwingFlagOfdm = true;
-				}
-
-				if (dm_odm->BbSwingIdxCckCurrent != dm_odm->BbSwingIdxCck) {
-					dm_odm->BbSwingIdxCckCurrent = dm_odm->BbSwingIdxCck;
-					dm_odm->BbSwingFlagCck = true;
-				}
-
-				if (X != 0) {
-					if ((X & 0x00000200) != 0)
-						X = X | 0xFFFFFC00;
-					ele_A = ((X * ele_D)>>8)&0x000003FF;
-
-					/* new element C = element D x Y */
-					if ((Y & 0x00000200) != 0)
-						Y = Y | 0xFFFFFC00;
-					ele_C = ((Y * ele_D)>>8)&0x000003FF;
-
-					/*  2012/04/23 MH According to Luke's suggestion, we can not write BB digital */
-					/*  to increase TX power. Otherwise, EVM will be bad. */
-				}
-
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("TxPwrTracking for path A: X=0x%x, Y=0x%x ele_A=0x%x ele_C=0x%x ele_D=0x%x 0xe94=0x%x 0xe9c=0x%x\n",
-					     (u32)X, (u32)Y, (u32)ele_A, (u32)ele_C, (u32)ele_D, (u32)X, (u32)Y));
-
-				if (is2t) {
-					ele_D = (OFDMSwingTable[(u8)OFDM_index[1]] & 0xFFC00000)>>22;
-
-					/* new element A = element D x X */
-					X = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].Value[0][4];
-					Y = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].Value[0][5];
-
-					if ((X != 0) && (*(dm_odm->pBandType) == ODM_BAND_2_4G)) {
-						if ((X & 0x00000200) != 0)	/* consider minus */
-							X = X | 0xFFFFFC00;
-						ele_A = ((X * ele_D)>>8)&0x000003FF;
-
-						/* new element C = element D x Y */
-						if ((Y & 0x00000200) != 0)
-							Y = Y | 0xFFFFFC00;
-						ele_C = ((Y * ele_D)>>8)&0x00003FF;
-
-						/* wtite new elements A, C, D to regC88 and regC9C, element B is always 0 */
-						value32 = (ele_D<<22) | ((ele_C&0x3F)<<16) | ele_A;
-						PHY_SetBBReg(Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, value32);
-
-						value32 = (ele_C&0x000003C0)>>6;
-						PHY_SetBBReg(Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
-
-						value32 = ((X * ele_D)>>7)&0x01;
-						PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT28, value32);
-					} else {
-						PHY_SetBBReg(Adapter, rOFDM0_XBTxIQImbalance, bMaskDWord, OFDMSwingTable[(u8)OFDM_index[1]]);
-						PHY_SetBBReg(Adapter, rOFDM0_XDTxAFE, bMaskH4Bits, 0x00);
-						PHY_SetBBReg(Adapter, rOFDM0_ECCAThreshold, BIT28, 0x00);
-					}
-
-					ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-						     ("TxPwrTracking path B: X=0x%x, Y=0x%x ele_A=0x%x ele_C=0x%x ele_D=0x%x 0xeb4=0x%x 0xebc=0x%x\n",
-						     (u32)X, (u32)Y, (u32)ele_A,
-						     (u32)ele_C, (u32)ele_D, (u32)X, (u32)Y));
-				}
-
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-					     ("TxPwrTracking 0xc80 = 0x%x, 0xc94 = 0x%x RF 0x24 = 0x%x\n",
-					     PHY_QueryBBReg(Adapter, 0xc80, bMaskDWord), PHY_QueryBBReg(Adapter,
-					     0xc94, bMaskDWord), PHY_QueryRFReg(Adapter, RF_PATH_A, 0x24, bRFRegOffsetMask)));
-			}
-		}
-
-		if (delta_IQK >= 8) { /*  Delta temperature is equal to or larger than 20 centigrade. */
-			dm_odm->RFCalibrateInfo.ThermalValue_IQK = ThermalValue;
-			PHY_IQCalibrate_8188E(Adapter, false);
-		}
-		/* update thermal meter value */
-		if (dm_odm->RFCalibrateInfo.TxPowerTrackControl)
-			dm_odm->RFCalibrateInfo.ThermalValue = ThermalValue;
-	}
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("<===dm_TXPowerTrackingCallback_ThermalMeter_8188E\n"));
-	dm_odm->RFCalibrateInfo.TXPowercount = 0;
-}
-
-/* 1 7.	IQK */
-#define MAX_TOLERANCE		5
-
-static u8 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
-phy_PathA_IQK_8188E(struct adapter *adapt, bool configPathB)
-{
-	u32 regeac, regE94, regE9C, regEA4;
-	u8 result = 0x00;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A IQK!\n"));
-
-	/* 1 Tx IQK */
-	/* path-A IQK setting */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A IQK setting!\n"));
-	PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x10008c1c);
-	PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x30008c1c);
-	PHY_SetBBReg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x8214032a);
-	PHY_SetBBReg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160000);
-
-	/* LO calibration setting */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n"));
-	PHY_SetBBReg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x00462911);
-
-	/* One shot, path A LOK & IQK */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n"));
-	PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
-	PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
-
-	/*  delay x ms */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E));
-	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
-	mdelay(IQK_DELAY_TIME_88E);
-
-	/*  Check failed */
-	regeac = PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xeac = 0x%x\n", regeac));
-	regE94 = PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe94 = 0x%x\n", regE94));
-	regE9C = PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xe9c = 0x%x\n", regE9C));
-	regEA4 = PHY_QueryBBReg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xea4 = 0x%x\n", regEA4));
-
-	if (!(regeac & BIT28) &&
-	    (((regE94 & 0x03FF0000)>>16) != 0x142) &&
-	    (((regE9C & 0x03FF0000)>>16) != 0x42))
-		result |= 0x01;
-	return result;
-}
-
-static u8 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
-phy_PathA_RxIQK(struct adapter *adapt, bool configPathB)
-{
-	u32 regeac, regE94, regE9C, regEA4, u4tmp;
-	u8 result = 0x00;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Rx IQK!\n"));
-
-	/* 1 Get TXIMR setting */
-	/* modify RXIQK mode table */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table!\n"));
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_WE_LUT, bRFRegOffsetMask, 0x800a0);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0000f);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf117B);
-
-	/* PA,PAD off */
-	PHY_SetRFReg(adapt, RF_PATH_A, 0xdf, bRFRegOffsetMask, 0x980);
-	PHY_SetRFReg(adapt, RF_PATH_A, 0x56, bRFRegOffsetMask, 0x51000);
-
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
-
-	/* IQK setting */
-	PHY_SetBBReg(adapt, rTx_IQK, bMaskDWord, 0x01007c00);
-	PHY_SetBBReg(adapt, rRx_IQK, bMaskDWord, 0x81004800);
-
-	/* path-A IQK setting */
-	PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x10008c1c);
-	PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x30008c1c);
-	PHY_SetBBReg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x82160c1f);
-	PHY_SetBBReg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160000);
-
-	/* LO calibration setting */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n"));
-	PHY_SetBBReg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
-
-	/* One shot, path A LOK & IQK */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n"));
-	PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
-	PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
-
-	/*  delay x ms */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("Delay %d ms for One shot, path A LOK & IQK.\n",
-		     IQK_DELAY_TIME_88E));
-	mdelay(IQK_DELAY_TIME_88E);
-
-	/*  Check failed */
-	regeac = PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xeac = 0x%x\n", regeac));
-	regE94 = PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xe94 = 0x%x\n", regE94));
-	regE9C = PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xe9c = 0x%x\n", regE9C));
-
-	if (!(regeac & BIT28) &&
-	    (((regE94 & 0x03FF0000)>>16) != 0x142) &&
-	    (((regE9C & 0x03FF0000)>>16) != 0x42))
-		result |= 0x01;
-	else							/* if Tx not OK, ignore Rx */
-		return result;
-
-	u4tmp = 0x80007C00 | (regE94&0x3FF0000)  | ((regE9C&0x3FF0000) >> 16);
-	PHY_SetBBReg(adapt, rTx_IQK, bMaskDWord, u4tmp);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("0xe40 = 0x%x u4tmp = 0x%x\n", PHY_QueryBBReg(adapt, rTx_IQK, bMaskDWord), u4tmp));
-
-	/* 1 RX IQK */
-	/* modify RXIQK mode table */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path-A Rx IQK modify RXIQK mode table 2!\n"));
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_WE_LUT, bRFRegOffsetMask, 0x800a0);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0000f);
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7ffa);
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
-
-	/* IQK setting */
-	PHY_SetBBReg(adapt, rRx_IQK, bMaskDWord, 0x01004800);
-
-	/* path-A IQK setting */
-	PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
-	PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
-	PHY_SetBBReg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x82160c05);
-	PHY_SetBBReg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160c1f);
-
-	/* LO calibration setting */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("LO calibration setting!\n"));
-	PHY_SetBBReg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
-
-	/* One shot, path A LOK & IQK */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n"));
-	PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
-	PHY_SetBBReg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
-
-	/*  delay x ms */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Delay %d ms for One shot, path A LOK & IQK.\n", IQK_DELAY_TIME_88E));
-	/* PlatformStallExecution(IQK_DELAY_TIME_88E*1000); */
-	mdelay(IQK_DELAY_TIME_88E);
-
-	/*  Check failed */
-	regeac = PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xeac = 0x%x\n", regeac));
-	regE94 = PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xe94 = 0x%x\n", regE94));
-	regE9C = PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xe9c = 0x%x\n", regE9C));
-	regEA4 = PHY_QueryBBReg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("0xea4 = 0x%x\n", regEA4));
-
-	/* reload RF 0xdf */
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
-	PHY_SetRFReg(adapt, RF_PATH_A, 0xdf, bRFRegOffsetMask, 0x180);
-
-	if (!(regeac & BIT27) &&		/* if Tx is OK, check whether Rx is OK */
-	    (((regEA4 & 0x03FF0000)>>16) != 0x132) &&
-	    (((regeac & 0x03FF0000)>>16) != 0x36))
-		result |= 0x02;
-	else
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A Rx IQK fail!!\n"));
-
-	return result;
-}
-
-static u8 /* bit0 = 1 => Tx OK, bit1 = 1 => Rx OK */
-phy_PathB_IQK_8188E(struct adapter *adapt)
-{
-	u32 regeac, regeb4, regebc, regec4, regecc;
-	u8 result = 0x00;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B IQK!\n"));
-
-	/* One shot, path B LOK & IQK */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("One shot, path A LOK & IQK!\n"));
-	PHY_SetBBReg(adapt, rIQK_AGC_Cont, bMaskDWord, 0x00000002);
-	PHY_SetBBReg(adapt, rIQK_AGC_Cont, bMaskDWord, 0x00000000);
-
-	/*  delay x ms */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("Delay %d ms for One shot, path B LOK & IQK.\n",
-		     IQK_DELAY_TIME_88E));
-	mdelay(IQK_DELAY_TIME_88E);
-
-	/*  Check failed */
-	regeac = PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xeac = 0x%x\n", regeac));
-	regeb4 = PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_B, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xeb4 = 0x%x\n", regeb4));
-	regebc = PHY_QueryBBReg(adapt, rTx_Power_After_IQK_B, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xebc = 0x%x\n", regebc));
-	regec4 = PHY_QueryBBReg(adapt, rRx_Power_Before_IQK_B_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xec4 = 0x%x\n", regec4));
-	regecc = PHY_QueryBBReg(adapt, rRx_Power_After_IQK_B_2, bMaskDWord);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("0xecc = 0x%x\n", regecc));
-
-	if (!(regeac & BIT31) &&
-	    (((regeb4 & 0x03FF0000)>>16) != 0x142) &&
-	    (((regebc & 0x03FF0000)>>16) != 0x42))
-		result |= 0x01;
-	else
-		return result;
-
-	if (!(regeac & BIT30) &&
-	    (((regec4 & 0x03FF0000)>>16) != 0x132) &&
-	    (((regecc & 0x03FF0000)>>16) != 0x36))
-		result |= 0x02;
-	else
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path B Rx IQK fail!!\n"));
-	return result;
-}
-
-static void patha_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8], u8 final_candidate, bool txonly)
-{
-	u32 Oldval_0, X, TX0_A, reg;
-	s32 Y, TX0_C;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("Path A IQ Calibration %s !\n",
-		     (iqkok) ? "Success" : "Failed"));
-
-	if (final_candidate == 0xFF) {
-		return;
-	} else if (iqkok) {
-		Oldval_0 = (PHY_QueryBBReg(adapt, rOFDM0_XATxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
-
-		X = result[final_candidate][0];
-		if ((X & 0x00000200) != 0)
-			X = X | 0xFFFFFC00;
-		TX0_A = (X * Oldval_0) >> 8;
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-			     ("X = 0x%x, TX0_A = 0x%x, Oldval_0 0x%x\n",
-			     X, TX0_A, Oldval_0));
-		PHY_SetBBReg(adapt, rOFDM0_XATxIQImbalance, 0x3FF, TX0_A);
-
-		PHY_SetBBReg(adapt, rOFDM0_ECCAThreshold, BIT(31), ((X * Oldval_0>>7) & 0x1));
-
-		Y = result[final_candidate][1];
-		if ((Y & 0x00000200) != 0)
-			Y = Y | 0xFFFFFC00;
-
-		TX0_C = (Y * Oldval_0) >> 8;
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Y = 0x%x, TX = 0x%x\n", Y, TX0_C));
-		PHY_SetBBReg(adapt, rOFDM0_XCTxAFE, 0xF0000000, ((TX0_C&0x3C0)>>6));
-		PHY_SetBBReg(adapt, rOFDM0_XATxIQImbalance, 0x003F0000, (TX0_C&0x3F));
-
-		PHY_SetBBReg(adapt, rOFDM0_ECCAThreshold, BIT(29), ((Y * Oldval_0>>7) & 0x1));
-
-		if (txonly) {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("patha_fill_iqk only Tx OK\n"));
-			return;
-		}
-
-		reg = result[final_candidate][2];
-		PHY_SetBBReg(adapt, rOFDM0_XARxIQImbalance, 0x3FF, reg);
-
-		reg = result[final_candidate][3] & 0x3F;
-		PHY_SetBBReg(adapt, rOFDM0_XARxIQImbalance, 0xFC00, reg);
-
-		reg = (result[final_candidate][3] >> 6) & 0xF;
-		PHY_SetBBReg(adapt, rOFDM0_RxIQExtAnta, 0xF0000000, reg);
-	}
-}
-
-static void pathb_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8], u8 final_candidate, bool txonly)
-{
-	u32 Oldval_1, X, TX1_A, reg;
-	s32 Y, TX1_C;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("Path B IQ Calibration %s !\n",
-		     (iqkok) ? "Success" : "Failed"));
-
-	if (final_candidate == 0xFF) {
-		return;
-	} else if (iqkok) {
-		Oldval_1 = (PHY_QueryBBReg(adapt, rOFDM0_XBTxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
-
-		X = result[final_candidate][4];
-		if ((X & 0x00000200) != 0)
-			X = X | 0xFFFFFC00;
-		TX1_A = (X * Oldval_1) >> 8;
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("X = 0x%x, TX1_A = 0x%x\n", X, TX1_A));
-		PHY_SetBBReg(adapt, rOFDM0_XBTxIQImbalance, 0x3FF, TX1_A);
-
-		PHY_SetBBReg(adapt, rOFDM0_ECCAThreshold, BIT(27), ((X * Oldval_1>>7) & 0x1));
-
-		Y = result[final_candidate][5];
-		if ((Y & 0x00000200) != 0)
-			Y = Y | 0xFFFFFC00;
-
-		TX1_C = (Y * Oldval_1) >> 8;
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Y = 0x%x, TX1_C = 0x%x\n", Y, TX1_C));
-		PHY_SetBBReg(adapt, rOFDM0_XDTxAFE, 0xF0000000, ((TX1_C&0x3C0)>>6));
-		PHY_SetBBReg(adapt, rOFDM0_XBTxIQImbalance, 0x003F0000, (TX1_C&0x3F));
-
-		PHY_SetBBReg(adapt, rOFDM0_ECCAThreshold, BIT(25), ((Y * Oldval_1>>7) & 0x1));
-
-		if (txonly)
-			return;
-
-		reg = result[final_candidate][6];
-		PHY_SetBBReg(adapt, rOFDM0_XBRxIQImbalance, 0x3FF, reg);
-
-		reg = result[final_candidate][7] & 0x3F;
-		PHY_SetBBReg(adapt, rOFDM0_XBRxIQImbalance, 0xFC00, reg);
-
-		reg = (result[final_candidate][7] >> 6) & 0xF;
-		PHY_SetBBReg(adapt, rOFDM0_AGCRSSITable, 0x0000F000, reg);
-	}
-}
-
-void _PHY_SaveADDARegisters(struct adapter *adapt, u32 *ADDAReg, u32 *ADDABackup, u32 RegisterNum)
-{
-	u32 i;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Save ADDA parameters.\n"));
-	for (i = 0; i < RegisterNum; i++) {
-		ADDABackup[i] = PHY_QueryBBReg(adapt, ADDAReg[i], bMaskDWord);
-	}
-}
-
-static void _PHY_SaveMACRegisters(
-		struct adapter *adapt,
-		u32 *MACReg,
-		u32 *MACBackup
-	)
-{
-	u32 i;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Save MAC parameters.\n"));
-	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++) {
-		MACBackup[i] = usb_read8(adapt, MACReg[i]);
-	}
-	MACBackup[i] = usb_read32(adapt, MACReg[i]);
-}
-
-static void reload_adda_reg(struct adapter *adapt, u32 *ADDAReg, u32 *ADDABackup, u32 RegiesterNum)
-{
-	u32 i;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Reload ADDA power saving parameters !\n"));
-	for (i = 0; i < RegiesterNum; i++)
-		PHY_SetBBReg(adapt, ADDAReg[i], bMaskDWord, ADDABackup[i]);
-}
-
-static void
-_PHY_ReloadMACRegisters(
-		struct adapter *adapt,
-		u32 *MACReg,
-		u32 *MACBackup
-	)
-{
-	u32 i;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Reload MAC parameters !\n"));
-	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++) {
-		usb_write8(adapt, MACReg[i], (u8)MACBackup[i]);
-	}
-	usb_write32(adapt, MACReg[i], MACBackup[i]);
-}
-
-void
-_PHY_PathADDAOn(
-		struct adapter *adapt,
-		u32 *ADDAReg,
-		bool isPathAOn,
-		bool is2t
-	)
-{
-	u32 pathOn;
-	u32 i;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("ADDA ON.\n"));
-
-	pathOn = isPathAOn ? 0x04db25a4 : 0x0b1b25a4;
-	if (!is2t) {
-		pathOn = 0x0bdb25a0;
-		PHY_SetBBReg(adapt, ADDAReg[0], bMaskDWord, 0x0b1b25a0);
-	} else {
-		PHY_SetBBReg(adapt, ADDAReg[0], bMaskDWord, pathOn);
-	}
-
-	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
-		PHY_SetBBReg(adapt, ADDAReg[i], bMaskDWord, pathOn);
-}
-
-void
-_PHY_MACSettingCalibration(
-		struct adapter *adapt,
-		u32 *MACReg,
-		u32 *MACBackup
-	)
-{
-	u32 i = 0;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("MAC settings for Calibration.\n"));
-
-	usb_write8(adapt, MACReg[i], 0x3F);
-
-	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++) {
-		usb_write8(adapt, MACReg[i], (u8)(MACBackup[i]&(~BIT3)));
-	}
-	usb_write8(adapt, MACReg[i], (u8)(MACBackup[i]&(~BIT5)));
-}
-
-void
-_PHY_PathAStandBy(
-	struct adapter *adapt
-	)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path-A standby mode!\n"));
-
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x0);
-	PHY_SetBBReg(adapt, 0x840, bMaskDWord, 0x00010000);
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
-}
-
-static void _PHY_PIModeSwitch(
-		struct adapter *adapt,
-		bool PIMode
-	)
-{
-	u32 mode;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("BB Switch to %s mode!\n", (PIMode ? "PI" : "SI")));
-
-	mode = PIMode ? 0x01000100 : 0x01000000;
-	PHY_SetBBReg(adapt, rFPGA0_XA_HSSIParameter1, bMaskDWord, mode);
-	PHY_SetBBReg(adapt, rFPGA0_XB_HSSIParameter1, bMaskDWord, mode);
-}
-
-static bool phy_SimularityCompare_8188E(
-		struct adapter *adapt,
-		s32 resulta[][8],
-		u8  c1,
-		u8  c2
-	)
-{
-	u32 i, j, diff, sim_bitmap, bound = 0;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	u8 final_candidate[2] = {0xFF, 0xFF};	/* for path A and path B */
-	bool result = true;
-	bool is2t;
-	s32 tmp1 = 0, tmp2 = 0;
-
-	if ((dm_odm->RFType == ODM_2T2R) || (dm_odm->RFType == ODM_2T3R) || (dm_odm->RFType == ODM_2T4R))
-		is2t = true;
-	else
-		is2t = false;
-
-	if (is2t)
-		bound = 8;
-	else
-		bound = 4;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("===> IQK:phy_SimularityCompare_8188E c1 %d c2 %d!!!\n", c1, c2));
-
-	sim_bitmap = 0;
-
-	for (i = 0; i < bound; i++) {
-		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
-			if ((resulta[c1][i] & 0x00000200) != 0)
-				tmp1 = resulta[c1][i] | 0xFFFFFC00;
-			else
-				tmp1 = resulta[c1][i];
-
-			if ((resulta[c2][i] & 0x00000200) != 0)
-				tmp2 = resulta[c2][i] | 0xFFFFFC00;
-			else
-				tmp2 = resulta[c2][i];
-		} else {
-			tmp1 = resulta[c1][i];
-			tmp2 = resulta[c2][i];
-		}
-
-		diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
-
-		if (diff > MAX_TOLERANCE) {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-				     ("IQK:phy_SimularityCompare_8188E differnece overflow index %d compare1 0x%x compare2 0x%x!!!\n",
-				     i, resulta[c1][i], resulta[c2][i]));
-
-			if ((i == 2 || i == 6) && !sim_bitmap) {
-				if (resulta[c1][i] + resulta[c1][i+1] == 0)
-					final_candidate[(i/4)] = c2;
-				else if (resulta[c2][i] + resulta[c2][i+1] == 0)
-					final_candidate[(i/4)] = c1;
-				else
-					sim_bitmap = sim_bitmap | (1<<i);
-			} else {
-				sim_bitmap = sim_bitmap | (1<<i);
-			}
-		}
-	}
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK:phy_SimularityCompare_8188E sim_bitmap   %d !!!\n", sim_bitmap));
-
-	if (sim_bitmap == 0) {
-		for (i = 0; i < (bound/4); i++) {
-			if (final_candidate[i] != 0xFF) {
-				for (j = i*4; j < (i+1)*4-2; j++)
-					resulta[3][j] = resulta[final_candidate[i]][j];
-				result = false;
-			}
-		}
-		return result;
-	} else {
-		if (!(sim_bitmap & 0x03)) {		   /* path A TX OK */
-			for (i = 0; i < 2; i++)
-				resulta[3][i] = resulta[c1][i];
-		}
-		if (!(sim_bitmap & 0x0c)) {		   /* path A RX OK */
-			for (i = 2; i < 4; i++)
-				resulta[3][i] = resulta[c1][i];
-		}
-
-		if (!(sim_bitmap & 0x30)) { /* path B TX OK */
-			for (i = 4; i < 6; i++)
-				resulta[3][i] = resulta[c1][i];
-		}
-
-		if (!(sim_bitmap & 0xc0)) { /* path B RX OK */
-			for (i = 6; i < 8; i++)
-				resulta[3][i] = resulta[c1][i];
-		}
-		return false;
-	}
-}
-
-static void phy_IQCalibrate_8188E(struct adapter *adapt, s32 result[][8], u8 t, bool is2t)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	u32 i;
-	u8 PathAOK, PathBOK;
-	u32 ADDA_REG[IQK_ADDA_REG_NUM] = {
-						rFPGA0_XCD_SwitchControl, rBlue_Tooth,
-						rRx_Wait_CCA, 	rTx_CCK_RFON,
-						rTx_CCK_BBON, rTx_OFDM_RFON,
-						rTx_OFDM_BBON, rTx_To_Rx,
-						rTx_To_Tx, 	rRx_CCK,
-						rRx_OFDM, 	rRx_Wait_RIFS,
-						rRx_TO_Rx, 	rStandby,
-						rSleep, 			rPMPD_ANAEN };
-	u32 IQK_MAC_REG[IQK_MAC_REG_NUM] = {
-						REG_TXPAUSE, 	REG_BCN_CTRL,
-						REG_BCN_CTRL_1, REG_GPIO_MUXCFG};
-
-	/* since 92C & 92D have the different define in IQK_BB_REG */
-	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
-							rOFDM0_TRxPathEnable, 	rOFDM0_TRMuxPar,
-							rFPGA0_XCD_RFInterfaceSW, rConfig_AntA, rConfig_AntB,
-							rFPGA0_XAB_RFInterfaceSW, rFPGA0_XA_RFInterfaceOE,
-							rFPGA0_XB_RFInterfaceOE, rFPGA0_RFMOD
-							};
-
-	u32 retryCount = 9;
-	if (*(dm_odm->mp_mode) == 1)
-		retryCount = 9;
-	else
-		retryCount = 2;
-	/*  Note: IQ calibration must be performed after loading */
-	/* 		PHY_REG.txt , and radio_a, radio_b.txt */
-
-	if (t == 0) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQ Calibration for %s for %d times\n", (is2t ? "2T2R" : "1T1R"), t));
-
-		/*  Save ADDA parameters, turn Path A ADDA on */
-		_PHY_SaveADDARegisters(adapt, ADDA_REG, dm_odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
-		_PHY_SaveMACRegisters(adapt, IQK_MAC_REG, dm_odm->RFCalibrateInfo.IQK_MAC_backup);
-		_PHY_SaveADDARegisters(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
-	}
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQ Calibration for %s for %d times\n", (is2t ? "2T2R" : "1T1R"), t));
-
-	_PHY_PathADDAOn(adapt, ADDA_REG, true, is2t);
-	if (t == 0)
-		dm_odm->RFCalibrateInfo.bRfPiEnable = (u8)PHY_QueryBBReg(adapt, rFPGA0_XA_HSSIParameter1, BIT(8));
-
-	if (!dm_odm->RFCalibrateInfo.bRfPiEnable) {
-		/*  Switch BB to PI mode to do IQ Calibration. */
-		_PHY_PIModeSwitch(adapt, true);
-	}
-
-	/* BB setting */
-	PHY_SetBBReg(adapt, rFPGA0_RFMOD, BIT24, 0x00);
-	PHY_SetBBReg(adapt, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
-	PHY_SetBBReg(adapt, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
-	PHY_SetBBReg(adapt, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
-
-	PHY_SetBBReg(adapt, rFPGA0_XAB_RFInterfaceSW, BIT10, 0x01);
-	PHY_SetBBReg(adapt, rFPGA0_XAB_RFInterfaceSW, BIT26, 0x01);
-	PHY_SetBBReg(adapt, rFPGA0_XA_RFInterfaceOE, BIT10, 0x00);
-	PHY_SetBBReg(adapt, rFPGA0_XB_RFInterfaceOE, BIT10, 0x00);
-
-	if (is2t) {
-		PHY_SetBBReg(adapt, rFPGA0_XA_LSSIParameter, bMaskDWord, 0x00010000);
-		PHY_SetBBReg(adapt, rFPGA0_XB_LSSIParameter, bMaskDWord, 0x00010000);
-	}
-
-	/* MAC settings */
-	_PHY_MACSettingCalibration(adapt, IQK_MAC_REG, dm_odm->RFCalibrateInfo.IQK_MAC_backup);
-
-	/* Page B init */
-	/* AP or IQK */
-	PHY_SetBBReg(adapt, rConfig_AntA, bMaskDWord, 0x0f600000);
-
-	if (is2t)
-		PHY_SetBBReg(adapt, rConfig_AntB, bMaskDWord, 0x0f600000);
-
-	/*  IQ calibration setting */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK setting!\n"));
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
-	PHY_SetBBReg(adapt, rTx_IQK, bMaskDWord, 0x01007c00);
-	PHY_SetBBReg(adapt, rRx_IQK, bMaskDWord, 0x81004800);
-
-	for (i = 0; i < retryCount; i++) {
-		PathAOK = phy_PathA_IQK_8188E(adapt, is2t);
-		if (PathAOK == 0x01) {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Tx IQK Success!!\n"));
-				result[t][0] = (PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_A, bMaskDWord)&0x3FF0000)>>16;
-				result[t][1] = (PHY_QueryBBReg(adapt, rTx_Power_After_IQK_A, bMaskDWord)&0x3FF0000)>>16;
-			break;
-		}
-	}
-
-	for (i = 0; i < retryCount; i++) {
-		PathAOK = phy_PathA_RxIQK(adapt, is2t);
-		if (PathAOK == 0x03) {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("Path A Rx IQK Success!!\n"));
-				result[t][2] = (PHY_QueryBBReg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
-				result[t][3] = (PHY_QueryBBReg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord)&0x3FF0000)>>16;
-			break;
-		} else {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A Rx IQK Fail!!\n"));
-		}
-	}
-
-	if (0x00 == PathAOK) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path A IQK failed!!\n"));
-	}
-
-	if (is2t) {
-		_PHY_PathAStandBy(adapt);
-
-		/*  Turn Path B ADDA on */
-		_PHY_PathADDAOn(adapt, ADDA_REG, false, is2t);
-
-		for (i = 0; i < retryCount; i++) {
-			PathBOK = phy_PathB_IQK_8188E(adapt);
-			if (PathBOK == 0x03) {
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B IQK Success!!\n"));
-				result[t][4] = (PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_B, bMaskDWord)&0x3FF0000)>>16;
-				result[t][5] = (PHY_QueryBBReg(adapt, rTx_Power_After_IQK_B, bMaskDWord)&0x3FF0000)>>16;
-				result[t][6] = (PHY_QueryBBReg(adapt, rRx_Power_Before_IQK_B_2, bMaskDWord)&0x3FF0000)>>16;
-				result[t][7] = (PHY_QueryBBReg(adapt, rRx_Power_After_IQK_B_2, bMaskDWord)&0x3FF0000)>>16;
-				break;
-			} else if (i == (retryCount - 1) && PathBOK == 0x01) {	/* Tx IQK OK */
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B Only Tx IQK Success!!\n"));
-				result[t][4] = (PHY_QueryBBReg(adapt, rTx_Power_Before_IQK_B, bMaskDWord)&0x3FF0000)>>16;
-				result[t][5] = (PHY_QueryBBReg(adapt, rTx_Power_After_IQK_B, bMaskDWord)&0x3FF0000)>>16;
-			}
-		}
-
-		if (0x00 == PathBOK) {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("Path B IQK failed!!\n"));
-		}
-	}
-
-	/* Back to BB mode, load original value */
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK:Back to BB mode, load original value!\n"));
-	PHY_SetBBReg(adapt, rFPGA0_IQK, bMaskDWord, 0);
-
-	if (t != 0) {
-		if (!dm_odm->RFCalibrateInfo.bRfPiEnable) {
-			/*  Switch back BB to SI mode after finish IQ Calibration. */
-			_PHY_PIModeSwitch(adapt, false);
-		}
-
-		/*  Reload ADDA power saving parameters */
-		reload_adda_reg(adapt, ADDA_REG, dm_odm->RFCalibrateInfo.ADDA_backup, IQK_ADDA_REG_NUM);
-
-		/*  Reload MAC parameters */
-		_PHY_ReloadMACRegisters(adapt, IQK_MAC_REG, dm_odm->RFCalibrateInfo.IQK_MAC_backup);
-
-		reload_adda_reg(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
-
-		/*  Restore RX initial gain */
-		PHY_SetBBReg(adapt, rFPGA0_XA_LSSIParameter, bMaskDWord, 0x00032ed3);
-		if (is2t)
-			PHY_SetBBReg(adapt, rFPGA0_XB_LSSIParameter, bMaskDWord, 0x00032ed3);
-
-		/* load 0xe30 IQC default value */
-		PHY_SetBBReg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x01008c00);
-		PHY_SetBBReg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x01008c00);
-	}
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("phy_IQCalibrate_8188E() <==\n"));
-}
-
-static void phy_LCCalibrate_8188E(struct adapter *adapt, bool is2t)
-{
-	u8 tmpreg;
-	u32 RF_Amode = 0, RF_Bmode = 0, LC_Cal;
-
-	/* Check continuous TX and Packet TX */
-	tmpreg = usb_read8(adapt, 0xd03);
-
-	if ((tmpreg&0x70) != 0)			/* Deal with contisuous TX case */
-		usb_write8(adapt, 0xd03, tmpreg&0x8F);	/* disable all continuous TX */
-	else							/*  Deal with Packet TX case */
-		usb_write8(adapt, REG_TXPAUSE, 0xFF);			/*  block all queues */
-
-	if ((tmpreg&0x70) != 0) {
-		/* 1. Read original RF mode */
-		/* Path-A */
-		RF_Amode = PHY_QueryRFReg(adapt, RF_PATH_A, RF_AC, bMask12Bits);
-
-		/* Path-B */
-		if (is2t)
-			RF_Bmode = PHY_QueryRFReg(adapt, RF_PATH_B, RF_AC, bMask12Bits);
-
-		/* 2. Set RF mode = standby mode */
-		/* Path-A */
-		PHY_SetRFReg(adapt, RF_PATH_A, RF_AC, bMask12Bits, (RF_Amode&0x8FFFF)|0x10000);
-
-		/* Path-B */
-		if (is2t)
-			PHY_SetRFReg(adapt, RF_PATH_B, RF_AC, bMask12Bits, (RF_Bmode&0x8FFFF)|0x10000);
-	}
-
-	/* 3. Read RF reg18 */
-	LC_Cal = PHY_QueryRFReg(adapt, RF_PATH_A, RF_CHNLBW, bMask12Bits);
-
-	/* 4. Set LC calibration begin	bit15 */
-	PHY_SetRFReg(adapt, RF_PATH_A, RF_CHNLBW, bMask12Bits, LC_Cal|0x08000);
-
-	msleep(100);
-
-	/* Restore original situation */
-	if ((tmpreg&0x70) != 0) {
-		/* Deal with continuous TX case */
-		/* Path-A */
-		usb_write8(adapt, 0xd03, tmpreg);
-		PHY_SetRFReg(adapt, RF_PATH_A, RF_AC, bMask12Bits, RF_Amode);
-
-		/* Path-B */
-		if (is2t)
-			PHY_SetRFReg(adapt, RF_PATH_B, RF_AC, bMask12Bits, RF_Bmode);
-	} else {
-		/*  Deal with Packet TX case */
-		usb_write8(adapt, REG_TXPAUSE, 0x00);
-	}
-}
-
-void PHY_IQCalibrate_8188E(struct adapter *adapt, bool recovery)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-	s32 result[4][8];	/* last is final result */
-	u8 i, final_candidate, Indexforchannel;
-	bool pathaok, pathbok;
-	s32 RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC;
-	bool is12simular, is13simular, is23simular;
-	bool singletone = false, carrier_sup = false;
-	u32 IQK_BB_REG_92C[IQK_BB_REG_NUM] = {
-		rOFDM0_XARxIQImbalance, rOFDM0_XBRxIQImbalance,
-		rOFDM0_ECCAThreshold, rOFDM0_AGCRSSITable,
-		rOFDM0_XATxIQImbalance, rOFDM0_XBTxIQImbalance,
-		rOFDM0_XCTxAFE, rOFDM0_XDTxAFE,
-		rOFDM0_RxIQExtAnta};
-	bool is2t;
-
-	is2t = (dm_odm->RFType == ODM_2T2R) ? true : false;
-
-	if (!(dm_odm->SupportAbility & ODM_RF_CALIBRATION))
-		return;
-
-	/*  20120213<Kordan> Turn on when continuous Tx to pass lab testing. (required by Edlu) */
-	if (singletone || carrier_sup)
-		return;
-
-	if (recovery) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_LOUD, ("PHY_IQCalibrate_8188E: Return due to recovery!\n"));
-		reload_adda_reg(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
-		return;
-	}
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK:Start!!!\n"));
-
-	for (i = 0; i < 8; i++) {
-		result[0][i] = 0;
-		result[1][i] = 0;
-		result[2][i] = 0;
-		if ((i == 0) || (i == 2) || (i == 4)  || (i == 6))
-			result[3][i] = 0x100;
-		else
-			result[3][i] = 0;
-	}
-	final_candidate = 0xff;
-	pathaok = false;
-	pathbok = false;
-	is12simular = false;
-	is23simular = false;
-	is13simular = false;
-
-	for (i = 0; i < 3; i++) {
-		phy_IQCalibrate_8188E(adapt, result, i, is2t);
-
-		if (i == 1) {
-			is12simular = phy_SimularityCompare_8188E(adapt, result, 0, 1);
-			if (is12simular) {
-				final_candidate = 0;
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is12simular final_candidate is %x\n", final_candidate));
-				break;
-			}
-		}
-
-		if (i == 2) {
-			is13simular = phy_SimularityCompare_8188E(adapt, result, 0, 2);
-			if (is13simular) {
-				final_candidate = 0;
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is13simular final_candidate is %x\n", final_candidate));
-
-				break;
-			}
-			is23simular = phy_SimularityCompare_8188E(adapt, result, 1, 2);
-			if (is23simular) {
-				final_candidate = 1;
-				ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD, ("IQK: is23simular final_candidate is %x\n", final_candidate));
-			} else {
-				final_candidate = 3;
-			}
-		}
-	}
-
-	for (i = 0; i < 4; i++) {
-		RegE94 = result[i][0];
-		RegE9C = result[i][1];
-		RegEA4 = result[i][2];
-		RegEAC = result[i][3];
-		RegEB4 = result[i][4];
-		RegEBC = result[i][5];
-		RegEC4 = result[i][6];
-		RegECC = result[i][7];
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-			     ("IQK: RegE94=%x RegE9C=%x RegEA4=%x RegEAC=%x RegEB4=%x RegEBC=%x RegEC4=%x RegECC=%x\n",
-			     RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC));
-	}
-
-	if (final_candidate != 0xff) {
-		RegE94 = result[final_candidate][0];
-		RegE9C = result[final_candidate][1];
-		RegEA4 = result[final_candidate][2];
-		RegEAC = result[final_candidate][3];
-		RegEB4 = result[final_candidate][4];
-		RegEBC = result[final_candidate][5];
-		dm_odm->RFCalibrateInfo.RegE94 = RegE94;
-		dm_odm->RFCalibrateInfo.RegE9C = RegE9C;
-		dm_odm->RFCalibrateInfo.RegEB4 = RegEB4;
-		dm_odm->RFCalibrateInfo.RegEBC = RegEBC;
-		RegEC4 = result[final_candidate][6];
-		RegECC = result[final_candidate][7];
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-			     ("IQK: final_candidate is %x\n", final_candidate));
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-			     ("IQK: RegE94=%x RegE9C=%x RegEA4=%x RegEAC=%x RegEB4=%x RegEBC=%x RegEC4=%x RegECC=%x\n",
-			     RegE94, RegE9C, RegEA4, RegEAC, RegEB4, RegEBC, RegEC4, RegECC));
-		pathaok = true;
-		pathbok = true;
-	} else {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK: FAIL use default value\n"));
-		dm_odm->RFCalibrateInfo.RegE94 = 0x100;
-		dm_odm->RFCalibrateInfo.RegEB4 = 0x100;	/* X default value */
-		dm_odm->RFCalibrateInfo.RegE9C = 0x0;
-		dm_odm->RFCalibrateInfo.RegEBC = 0x0;	/* Y default value */
-	}
-	if (RegE94 != 0)
-		patha_fill_iqk(adapt, pathaok, result, final_candidate, (RegEA4 == 0));
-	if (is2t) {
-		if (RegEB4 != 0)
-			pathb_fill_iqk(adapt, pathbok, result, final_candidate, (RegEC4 == 0));
-	}
-
-	Indexforchannel = ODM_GetRightChnlPlaceforIQK(pHalData->CurrentChannel);
-
-/* To Fix BSOD when final_candidate is 0xff */
-/* by sherry 20120321 */
-	if (final_candidate < 4) {
-		for (i = 0; i < IQK_Matrix_REG_NUM; i++)
-			dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].Value[0][i] = result[final_candidate][i];
-		dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[Indexforchannel].bIQKDone = true;
-	}
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("\nIQK OK Indexforchannel %d.\n", Indexforchannel));
-
-	_PHY_SaveADDARegisters(adapt, IQK_BB_REG_92C, dm_odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,  ("IQK finished\n"));
-}
-
-void PHY_LCCalibrate_8188E(struct adapter *adapt)
-{
-	bool singletone = false, carrier_sup = false;
-	u32 timeout = 2000, timecount = 0;
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	if (!(dm_odm->SupportAbility & ODM_RF_CALIBRATION))
-		return;
-	/*  20120213<Kordan> Turn on when continuous Tx to pass lab testing. (required by Edlu) */
-	if (singletone || carrier_sup)
-		return;
-
-	while (*(dm_odm->pbScanInProcess) && timecount < timeout) {
-		mdelay(50);
-		timecount += 50;
-	}
-
-	dm_odm->RFCalibrateInfo.bLCKInProgress = true;
-
-	if (dm_odm->RFType == ODM_2T2R) {
-		phy_LCCalibrate_8188E(adapt, true);
-	} else {
-		/*  For 88C 1T1R */
-		phy_LCCalibrate_8188E(adapt, false);
-	}
-
-	dm_odm->RFCalibrateInfo.bLCKInProgress = false;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
-		     ("LCK:Finish!!!interface %d\n", dm_odm->InterfaceIndex));
-}
-
-static void phy_setrfpathswitch_8188e(struct adapter *adapt, bool main, bool is2t)
-{
-	if (!adapt->hw_init_completed) {
-		u8 u1btmp;
-		u1btmp = usb_read8(adapt, REG_LEDCFG2) | BIT7;
-		usb_write8(adapt, REG_LEDCFG2, u1btmp);
-		PHY_SetBBReg(adapt, rFPGA0_XAB_RFParameter, BIT13, 0x01);
-	}
-
-	if (is2t) {	/* 92C */
-		if (main)
-			PHY_SetBBReg(adapt, rFPGA0_XB_RFInterfaceOE, BIT5|BIT6, 0x1);	/* 92C_Path_A */
-		else
-			PHY_SetBBReg(adapt, rFPGA0_XB_RFInterfaceOE, BIT5|BIT6, 0x2);	/* BT */
-	} else {			/* 88C */
-		if (main)
-			PHY_SetBBReg(adapt, rFPGA0_XA_RFInterfaceOE, BIT8|BIT9, 0x2);	/* Main */
-		else
-			PHY_SetBBReg(adapt, rFPGA0_XA_RFInterfaceOE, BIT8|BIT9, 0x1);	/* Aux */
-	}
-}
-
-void PHY_SetRFPathSwitch_8188E(struct adapter *adapt, bool main)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(adapt);
-	struct odm_dm_struct *dm_odm = &pHalData->odmpriv;
-
-	if (dm_odm->RFType == ODM_2T2R) {
-		phy_setrfpathswitch_8188e(adapt, main, true);
-	} else {
-		/*  For 88C 1T1R */
-		phy_setrfpathswitch_8188e(adapt, main, false);
-	}
-}
diff --git a/drivers/staging/rtl8188eu/hal/HalHWImg8188E_BB.c b/drivers/staging/rtl8188eu/hal/bb_cfg.c
index 787e8f1f97f9..80e8cc92c10a 100644
--- a/drivers/staging/rtl8188eu/hal/HalHWImg8188E_BB.c
+++ b/drivers/staging/rtl8188eu/hal/bb_cfg.c
@@ -20,7 +20,7 @@
 
 #include "odm_precomp.h"
 
-#include <rtw_iol.h>
+#include <phy.h>
 
 #define read_next_pair(array, v1, v2, i)		\
 	 do {						\
@@ -29,36 +29,8 @@
 		 v2 = array[i+1];			\
 	 } while (0)
 
-static bool CheckCondition(const u32  condition, const u32  hex)
-{
-	u32 _board     = (hex & 0x000000FF);
-	u32 _interface = (hex & 0x0000FF00) >> 8;
-	u32 _platform  = (hex & 0x00FF0000) >> 16;
-	u32 cond = condition;
-
-	if (condition == 0xCDCDCDCD)
-		return true;
-
-	cond = condition & 0x000000FF;
-	if ((_board == cond) && cond != 0x00)
-		return false;
-
-	cond = condition & 0x0000FF00;
-	cond = cond >> 8;
-	if ((_interface & cond) == 0 && cond != 0x07)
-		return false;
-
-	cond = condition & 0x00FF0000;
-	cond = cond >> 16;
-	if ((_platform & cond) == 0 && cond != 0x0F)
-		return false;
-	return true;
-}
 
-
-/******************************************************************************
-*                           AGC_TAB_1T.TXT
-******************************************************************************/
+/* AGC_TAB_1T.TXT */
 
 static u32 array_agc_tab_1t_8188e[] = {
 		0xC78, 0xFB000001,
@@ -191,91 +163,25 @@ static u32 array_agc_tab_1t_8188e[] = {
 		0xC78, 0x407F0001,
 };
 
-enum HAL_STATUS ODM_ReadAndConfig_AGC_TAB_1T_8188E(struct odm_dm_struct *dm_odm)
+static bool set_baseband_agc_config(struct adapter *adapt)
 {
-	u32     hex         = 0;
-	u32     i           = 0;
-	u8     platform    = dm_odm->SupportPlatform;
-	u8     interfaceValue   = dm_odm->SupportInterface;
-	u8     board       = dm_odm->BoardType;
-	u32     arraylen    = sizeof(array_agc_tab_1t_8188e)/sizeof(u32);
-	u32    *array       = array_agc_tab_1t_8188e;
-	bool		biol = false;
-	struct adapter *adapter =  dm_odm->Adapter;
-	struct xmit_frame *pxmit_frame = NULL;
-	u8 bndy_cnt = 1;
-	enum HAL_STATUS rst = HAL_STATUS_SUCCESS;
-
-	hex += board;
-	hex += interfaceValue << 8;
-	hex += platform << 16;
-	hex += 0xFF000000;
-	biol = rtw_IOL_applied(adapter);
-
-	if (biol) {
-		pxmit_frame = rtw_IOL_accquire_xmit_frame(adapter);
-		if (pxmit_frame == NULL) {
-			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
-			return HAL_STATUS_FAILURE;
-		}
-	}
+	u32 i;
+	u32 arraylen = sizeof(array_agc_tab_1t_8188e)/sizeof(u32);
+	u32 *array = array_agc_tab_1t_8188e;
 
 	for (i = 0; i < arraylen; i += 2) {
 		u32 v1 = array[i];
 		u32 v2 = array[i+1];
 
-		/*  This (offset, data) pair meets the condition. */
-		if (v1 < 0xCDCDCDCD) {
-			if (biol) {
-				if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-					bndy_cnt++;
-				rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
-			} else {
-				odm_ConfigBB_AGC_8188E(dm_odm, v1, bMaskDWord, v2);
-			}
-			continue;
-		} else {
-			/*  This line is the start line of branch. */
-			if (!CheckCondition(array[i], hex)) {
-				/*  Discard the following (offset, data) pairs. */
-				read_next_pair(array, v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2)
-					read_next_pair(array, v1, v2, i);
-				i -= 2; /*  prevent from for-loop += 2 */
-			} else { /*  Configure matched pairs and skip to end of if-else. */
-				read_next_pair(array, v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2) {
-					if (biol) {
-						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-							bndy_cnt++;
-						rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
-					} else {
-						odm_ConfigBB_AGC_8188E(dm_odm, v1, bMaskDWord, v2);
-					}
-					read_next_pair(array, v1, v2, i);
-				}
-
-				while (v2 != 0xDEAD && i < arraylen - 2)
-					read_next_pair(array, v1, v2, i);
-			}
-		}
-	}
-	if (biol) {
-		if (!rtw_IOL_exec_cmds_sync(dm_odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
-			printk("~~~ %s IOL_exec_cmds Failed !!!\n", __func__);
-			rst = HAL_STATUS_FAILURE;
+		if (v1 < 0xCDCDCDCD){
+			phy_set_bb_reg(adapt, v1, bMaskDWord, v2);
+			udelay(1);
 		}
 	}
-	return rst;
+	return true;
 }
 
-/******************************************************************************
-*                           PHY_REG_1T.TXT
-******************************************************************************/
+/*  PHY_REG_1T.TXT  */
 
 static u32 array_phy_reg_1t_8188e[] = {
 		0x800, 0x80040000,
@@ -471,122 +377,44 @@ static u32 array_phy_reg_1t_8188e[] = {
 		0xF00, 0x00000300,
 };
 
-enum HAL_STATUS ODM_ReadAndConfig_PHY_REG_1T_8188E(struct odm_dm_struct *dm_odm)
+static void rtl_bb_delay(struct adapter *adapt, u32 addr, u32 data)
 {
-	u32     hex         = 0;
-	u32     i           = 0;
-	u8     platform    = dm_odm->SupportPlatform;
-	u8     interfaceValue   = dm_odm->SupportInterface;
-	u8     board       = dm_odm->BoardType;
-	u32     arraylen    = sizeof(array_phy_reg_1t_8188e)/sizeof(u32);
-	u32    *array       = array_phy_reg_1t_8188e;
-	bool	biol = false;
-	struct adapter *adapter =  dm_odm->Adapter;
-	struct xmit_frame *pxmit_frame = NULL;
-	u8 bndy_cnt = 1;
-	enum HAL_STATUS rst = HAL_STATUS_SUCCESS;
-	hex += board;
-	hex += interfaceValue << 8;
-	hex += platform << 16;
-	hex += 0xFF000000;
-	biol = rtw_IOL_applied(adapter);
-
-	if (biol) {
-		pxmit_frame = rtw_IOL_accquire_xmit_frame(adapter);
-		if (pxmit_frame == NULL) {
-			pr_info("rtw_IOL_accquire_xmit_frame failed\n");
-			return HAL_STATUS_FAILURE;
-		}
+	if (addr == 0xfe) {
+		msleep(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		phy_set_bb_reg(adapt, addr, bMaskDWord, data);
+		/*  Add 1us delay between BB/RF register setting. */
+		udelay(1);
 	}
+}
+
+static bool set_baseband_phy_config(struct adapter *adapt)
+{
+	u32 i;
+	u32 arraylen = sizeof(array_phy_reg_1t_8188e)/sizeof(u32);
+	u32 *array = array_phy_reg_1t_8188e;
 
 	for (i = 0; i < arraylen; i += 2) {
 		u32 v1 = array[i];
 		u32 v2 = array[i+1];
 
-		/*  This (offset, data) pair meets the condition. */
-		if (v1 < 0xCDCDCDCD) {
-			if (biol) {
-				if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-					bndy_cnt++;
-				if (v1 == 0xfe) {
-					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
-				} else if (v1 == 0xfd) {
-					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
-				} else if (v1 == 0xfc) {
-					rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
-				} else if (v1 == 0xfb) {
-					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
-				} else if (v1 == 0xfa) {
-					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
-				} else if (v1 == 0xf9) {
-					rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
-				} else {
-					if (v1 == 0xa24)
-						dm_odm->RFCalibrateInfo.RegA24 = v2;
-					rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
-				}
-			} else {
-				odm_ConfigBB_PHY_8188E(dm_odm, v1, bMaskDWord, v2);
-			}
-			continue;
-		} else { /*  This line is the start line of branch. */
-			if (!CheckCondition(array[i], hex)) {
-				/*  Discard the following (offset, data) pairs. */
-				read_next_pair(array, v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2)
-					read_next_pair(array, v1, v2, i);
-				i -= 2; /*  prevent from for-loop += 2 */
-			} else { /*  Configure matched pairs and skip to end of if-else. */
-				read_next_pair(array, v1, v2, i);
-				while (v2 != 0xDEAD &&
-				       v2 != 0xCDEF &&
-				       v2 != 0xCDCD && i < arraylen - 2) {
-					if (biol) {
-						if (rtw_IOL_cmd_boundary_handle(pxmit_frame))
-							bndy_cnt++;
-						if (v1 == 0xfe) {
-							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 50);
-						} else if (v1 == 0xfd) {
-							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 5);
-						} else if (v1 == 0xfc) {
-							rtw_IOL_append_DELAY_MS_cmd(pxmit_frame, 1);
-						} else if (v1 == 0xfb) {
-							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 50);
-						} else if (v1 == 0xfa) {
-							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 5);
-						} else if (v1 == 0xf9) {
-							rtw_IOL_append_DELAY_US_cmd(pxmit_frame, 1);
-						} else{
-							if (v1 == 0xa24)
-								dm_odm->RFCalibrateInfo.RegA24 = v2;
-
-							rtw_IOL_append_WD_cmd(pxmit_frame, (u16)v1, v2, bMaskDWord);
-						}
-					} else {
-						odm_ConfigBB_PHY_8188E(dm_odm, v1, bMaskDWord, v2);
-					}
-					read_next_pair(array, v1, v2, i);
-				}
-
-				while (v2 != 0xDEAD && i < arraylen - 2)
-					read_next_pair(array, v1, v2, i);
-			}
-		}
+		if (v1 < 0xCDCDCDCD)
+			rtl_bb_delay(adapt, v1, v2);
 	}
-	if (biol) {
-		if (!rtw_IOL_exec_cmds_sync(dm_odm->Adapter, pxmit_frame, 1000, bndy_cnt)) {
-			rst = HAL_STATUS_FAILURE;
-			pr_info("~~~ IOL Config %s Failed !!!\n", __func__);
-		}
-	}
-	return rst;
+	return true;
 }
 
-/******************************************************************************
-*                           PHY_REG_PG.TXT
-******************************************************************************/
+/*  PHY_REG_PG.TXT  */
 
 static u32 array_phy_reg_pg_8188e[] = {
 		0xE00, 0xFFFFFFFF, 0x06070809,
@@ -680,42 +508,208 @@ static u32 array_phy_reg_pg_8188e[] = {
 
 };
 
-void ODM_ReadAndConfig_PHY_REG_PG_8188E(struct odm_dm_struct *dm_odm)
+static void store_pwrindex_offset(struct adapter *Adapter, u32 regaddr, u32 bitmask, u32 data)
 {
-	u32  hex;
-	u32  i           = 0;
-	u8  platform    = dm_odm->SupportPlatform;
-	u8  interfaceValue   = dm_odm->SupportInterface;
-	u8  board       = dm_odm->BoardType;
-	u32  arraylen    = sizeof(array_phy_reg_pg_8188e) / sizeof(u32);
-	u32 *array       = array_phy_reg_pg_8188e;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(Adapter);
+
+	if (regaddr == rTxAGC_A_Rate18_06)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][0] = data;
+	if (regaddr == rTxAGC_A_Rate54_24)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][1] = data;
+	if (regaddr == rTxAGC_A_CCK1_Mcs32)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][6] = data;
+	if (regaddr == rTxAGC_B_CCK11_A_CCK2_11 && bitmask == 0xffffff00)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][7] = data;
+	if (regaddr == rTxAGC_A_Mcs03_Mcs00)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][2] = data;
+	if (regaddr == rTxAGC_A_Mcs07_Mcs04)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][3] = data;
+	if (regaddr == rTxAGC_A_Mcs11_Mcs08)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][4] = data;
+	if (regaddr == rTxAGC_A_Mcs15_Mcs12) {
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][5] = data;
+		if (hal_data->rf_type == RF_1T1R)
+			hal_data->pwrGroupCnt++;
+	}
+	if (regaddr == rTxAGC_B_Rate18_06)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][8] = data;
+	if (regaddr == rTxAGC_B_Rate54_24)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][9] = data;
+	if (regaddr == rTxAGC_B_CCK1_55_Mcs32)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][14] = data;
+	if (regaddr == rTxAGC_B_CCK11_A_CCK2_11 && bitmask == 0x000000ff)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][15] = data;
+	if (regaddr == rTxAGC_B_Mcs03_Mcs00)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][10] = data;
+	if (regaddr == rTxAGC_B_Mcs07_Mcs04)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][11] = data;
+	if (regaddr == rTxAGC_B_Mcs11_Mcs08)
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][12] = data;
+	if (regaddr == rTxAGC_B_Mcs15_Mcs12) {
+		hal_data->MCSTxPowerLevelOriginalOffset[hal_data->pwrGroupCnt][13] = data;
+		if (hal_data->rf_type != RF_1T1R)
+			hal_data->pwrGroupCnt++;
+	}
+}
 
-	hex = board + (interfaceValue << 8);
-	hex += (platform << 16) + 0xFF000000;
+static void rtl_addr_delay(struct adapter *adapt, u32 addr, u32 bit_mask ,u32 data)
+{
+	if (addr == 0xfe) {
+		msleep(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else{
+		store_pwrindex_offset(adapt, addr, bit_mask, data);
+	}
+}
+
+static bool config_bb_with_pgheader(struct adapter *adapt)
+{
+	u32 i = 0;
+	u32 arraylen = sizeof(array_phy_reg_pg_8188e) / sizeof(u32);
+	u32 *array = array_phy_reg_pg_8188e;
 
 	for (i = 0; i < arraylen; i += 3) {
 		u32 v1 = array[i];
 		u32 v2 = array[i+1];
 		u32 v3 = array[i+2];
 
-		/*  this line is a line of pure_body */
-		if (v1 < 0xCDCDCDCD) {
-			odm_ConfigBB_PHY_REG_PG_8188E(dm_odm, v1, v2, v3);
-			continue;
-		} else { /*  this line is the start of branch */
-			if (!CheckCondition(array[i], hex)) {
-				/*  don't need the hw_body */
-				i += 2; /*  skip the pair of expression */
-				v1 = array[i];
-				v2 = array[i+1];
-				v3 = array[i+2];
-				while (v2 != 0xDEAD) {
-					i += 3;
-					v1 = array[i];
-					v2 = array[i+1];
-					v3 = array[i+1];
-				}
-			}
-		}
+		if (v1 < 0xCDCDCDCD)
+			rtl_addr_delay(adapt, v1, v2, v3);
+	}
+	return true;
+}
+
+static void rtl88e_phy_init_bb_rf_register_definition(struct adapter *Adapter)
+{
+	struct hal_data_8188e		*hal_data = GET_HAL_DATA(Adapter);
+
+	hal_data->PHYRegDef[RF_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	hal_data->PHYRegDef[RF_PATH_B].rfintfs = rFPGA0_XAB_RFInterfaceSW;
+	hal_data->PHYRegDef[RF_PATH_C].rfintfs = rFPGA0_XCD_RFInterfaceSW;
+	hal_data->PHYRegDef[RF_PATH_D].rfintfs = rFPGA0_XCD_RFInterfaceSW;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	hal_data->PHYRegDef[RF_PATH_B].rfintfi = rFPGA0_XAB_RFInterfaceRB;
+	hal_data->PHYRegDef[RF_PATH_C].rfintfi = rFPGA0_XCD_RFInterfaceRB;
+	hal_data->PHYRegDef[RF_PATH_D].rfintfi = rFPGA0_XCD_RFInterfaceRB;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfintfo = rFPGA0_XA_RFInterfaceOE;
+	hal_data->PHYRegDef[RF_PATH_B].rfintfo = rFPGA0_XB_RFInterfaceOE;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfintfe = rFPGA0_XA_RFInterfaceOE;
+	hal_data->PHYRegDef[RF_PATH_B].rfintfe = rFPGA0_XB_RFInterfaceOE;
+
+	hal_data->PHYRegDef[RF_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter;
+	hal_data->PHYRegDef[RF_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	hal_data->PHYRegDef[RF_PATH_B].rfLSSI_Select = rFPGA0_XAB_RFParameter;
+	hal_data->PHYRegDef[RF_PATH_C].rfLSSI_Select = rFPGA0_XCD_RFParameter;
+	hal_data->PHYRegDef[RF_PATH_D].rfLSSI_Select = rFPGA0_XCD_RFParameter;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfTxGainStage = rFPGA0_TxGainStage;
+	hal_data->PHYRegDef[RF_PATH_B].rfTxGainStage = rFPGA0_TxGainStage;
+	hal_data->PHYRegDef[RF_PATH_C].rfTxGainStage = rFPGA0_TxGainStage;
+	hal_data->PHYRegDef[RF_PATH_D].rfTxGainStage = rFPGA0_TxGainStage;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfHSSIPara1 = rFPGA0_XA_HSSIParameter1;
+	hal_data->PHYRegDef[RF_PATH_B].rfHSSIPara1 = rFPGA0_XB_HSSIParameter1;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;
+	hal_data->PHYRegDef[RF_PATH_B].rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	hal_data->PHYRegDef[RF_PATH_B].rfSwitchControl = rFPGA0_XAB_SwitchControl;
+	hal_data->PHYRegDef[RF_PATH_C].rfSwitchControl = rFPGA0_XCD_SwitchControl;
+	hal_data->PHYRegDef[RF_PATH_D].rfSwitchControl = rFPGA0_XCD_SwitchControl;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfAGCControl1 = rOFDM0_XAAGCCore1;
+	hal_data->PHYRegDef[RF_PATH_B].rfAGCControl1 = rOFDM0_XBAGCCore1;
+	hal_data->PHYRegDef[RF_PATH_C].rfAGCControl1 = rOFDM0_XCAGCCore1;
+	hal_data->PHYRegDef[RF_PATH_D].rfAGCControl1 = rOFDM0_XDAGCCore1;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfAGCControl2 = rOFDM0_XAAGCCore2;
+	hal_data->PHYRegDef[RF_PATH_B].rfAGCControl2 = rOFDM0_XBAGCCore2;
+	hal_data->PHYRegDef[RF_PATH_C].rfAGCControl2 = rOFDM0_XCAGCCore2;
+	hal_data->PHYRegDef[RF_PATH_D].rfAGCControl2 = rOFDM0_XDAGCCore2;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfRxIQImbalance = rOFDM0_XARxIQImbalance;
+	hal_data->PHYRegDef[RF_PATH_B].rfRxIQImbalance = rOFDM0_XBRxIQImbalance;
+	hal_data->PHYRegDef[RF_PATH_C].rfRxIQImbalance = rOFDM0_XCRxIQImbalance;
+	hal_data->PHYRegDef[RF_PATH_D].rfRxIQImbalance = rOFDM0_XDRxIQImbalance;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfRxAFE = rOFDM0_XARxAFE;
+	hal_data->PHYRegDef[RF_PATH_B].rfRxAFE = rOFDM0_XBRxAFE;
+	hal_data->PHYRegDef[RF_PATH_C].rfRxAFE = rOFDM0_XCRxAFE;
+	hal_data->PHYRegDef[RF_PATH_D].rfRxAFE = rOFDM0_XDRxAFE;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfTxIQImbalance = rOFDM0_XATxIQImbalance;
+	hal_data->PHYRegDef[RF_PATH_B].rfTxIQImbalance = rOFDM0_XBTxIQImbalance;
+	hal_data->PHYRegDef[RF_PATH_C].rfTxIQImbalance = rOFDM0_XCTxIQImbalance;
+	hal_data->PHYRegDef[RF_PATH_D].rfTxIQImbalance = rOFDM0_XDTxIQImbalance;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfTxAFE = rOFDM0_XATxAFE;
+	hal_data->PHYRegDef[RF_PATH_B].rfTxAFE = rOFDM0_XBTxAFE;
+	hal_data->PHYRegDef[RF_PATH_C].rfTxAFE = rOFDM0_XCTxAFE;
+	hal_data->PHYRegDef[RF_PATH_D].rfTxAFE = rOFDM0_XDTxAFE;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
+	hal_data->PHYRegDef[RF_PATH_B].rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
+	hal_data->PHYRegDef[RF_PATH_C].rfLSSIReadBack = rFPGA0_XC_LSSIReadBack;
+	hal_data->PHYRegDef[RF_PATH_D].rfLSSIReadBack = rFPGA0_XD_LSSIReadBack;
+
+	hal_data->PHYRegDef[RF_PATH_A].rfLSSIReadBackPi = TransceiverA_HSPI_Readback;
+	hal_data->PHYRegDef[RF_PATH_B].rfLSSIReadBackPi = TransceiverB_HSPI_Readback;
+}
+
+static bool config_parafile(struct adapter *adapt)
+{
+	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(adapt);
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+
+	set_baseband_phy_config(adapt);
+
+	/* If EEPROM or EFUSE autoload OK, We must config by PHY_REG_PG.txt */
+	if (!pEEPROM->bautoload_fail_flag) {
+		hal_data->pwrGroupCnt = 0;
+		config_bb_with_pgheader(adapt);
 	}
+	set_baseband_agc_config(adapt);
+	return true;
+}
+
+bool rtl88eu_phy_bb_config(struct adapter *adapt)
+{
+	int rtstatus = true;
+	struct hal_data_8188e	*hal_data = GET_HAL_DATA(adapt);
+	u32 regval;
+	u8 crystal_cap;
+
+	rtl88e_phy_init_bb_rf_register_definition(adapt);
+
+	/*  Enable BB and RF */
+	regval = usb_read16(adapt, REG_SYS_FUNC_EN);
+	usb_write16(adapt, REG_SYS_FUNC_EN, (u16)(regval|BIT13|BIT0|BIT1));
+
+	usb_write8(adapt, REG_RF_CTRL, RF_EN|RF_RSTB|RF_SDMRSTB);
+
+	usb_write8(adapt, REG_SYS_FUNC_EN, FEN_USBA | FEN_USBD | FEN_BB_GLB_RSTn | FEN_BBRSTB);
+
+	/*  Config BB and AGC */
+	rtstatus = config_parafile(adapt);
+
+	/*  write 0x24[16:11] = 0x24[22:17] = crystal_cap */
+	crystal_cap = hal_data->CrystalCap & 0x3F;
+	phy_set_bb_reg(adapt, REG_AFE_XTAL_CTRL, 0x7ff800, (crystal_cap | (crystal_cap << 6)));
+
+	return rtstatus;
 }
diff --git a/drivers/staging/rtl8188eu/hal/fw.c b/drivers/staging/rtl8188eu/hal/fw.c
new file mode 100644
index 000000000000..17b7f3750547
--- /dev/null
+++ b/drivers/staging/rtl8188eu/hal/fw.c
@@ -0,0 +1,236 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2009-2013  Realtek Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * wlanfae <wlanfae@realtek.com>
+ * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
+ * Hsinchu 300, Taiwan.
+ *
+ * Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *****************************************************************************/
+
+#include "fw.h"
+#include "drv_types.h"
+#include "usb_ops_linux.h"
+#include "rtl8188e_spec.h"
+#include "rtl8188e_hal.h"
+
+#include <linux/firmware.h>
+#include <linux/kmemleak.h>
+
+static void _rtl88e_enable_fw_download(struct adapter *adapt, bool enable)
+{
+	u8 tmp;
+
+	if (enable) {
+		tmp = usb_read8(adapt, REG_MCUFWDL);
+		usb_write8(adapt, REG_MCUFWDL, tmp | 0x01);
+
+		tmp = usb_read8(adapt, REG_MCUFWDL + 2);
+		usb_write8(adapt, REG_MCUFWDL + 2, tmp & 0xf7);
+	} else {
+		tmp = usb_read8(adapt, REG_MCUFWDL);
+		usb_write8(adapt, REG_MCUFWDL, tmp & 0xfe);
+
+		usb_write8(adapt, REG_MCUFWDL + 1, 0x00);
+	}
+}
+
+static void _rtl88e_fw_block_write(struct adapter *adapt,
+				   const u8 *buffer, u32 size)
+{
+	u32 blk_sz = sizeof(u32);
+	u8 *buf_ptr = (u8 *)buffer;
+	u32 *pu4BytePtr = (u32 *)buffer;
+	u32 i, offset, blk_cnt, remain;
+
+	blk_cnt = size / blk_sz;
+	remain = size % blk_sz;
+
+	for (i = 0; i < blk_cnt; i++) {
+		offset = i * blk_sz;
+		usb_write32(adapt, (FW_8192C_START_ADDRESS + offset),
+				*(pu4BytePtr + i));
+	}
+
+	if (remain) {
+		offset = blk_cnt * blk_sz;
+		buf_ptr += offset;
+		for (i = 0; i < remain; i++) {
+			usb_write8(adapt, (FW_8192C_START_ADDRESS +
+						 offset + i), *(buf_ptr + i));
+		}
+	}
+}
+
+static void _rtl88e_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
+{
+	u32 fwlen = *pfwlen;
+	u8 remain = (u8) (fwlen % 4);
+
+	remain = (remain == 0) ? 0 : (4 - remain);
+
+	while (remain > 0) {
+		pfwbuf[fwlen] = 0;
+		fwlen++;
+		remain--;
+	}
+
+	*pfwlen = fwlen;
+}
+
+static void _rtl88e_fw_page_write(struct adapter *adapt,
+				  u32 page, const u8 *buffer, u32 size)
+{
+	u8 value8;
+	u8 u8page = (u8) (page & 0x07);
+
+	value8 = (usb_read8(adapt, REG_MCUFWDL + 2) & 0xF8) | u8page;
+
+	usb_write8(adapt, (REG_MCUFWDL + 2), value8);
+	_rtl88e_fw_block_write(adapt, buffer, size);
+}
+
+static void _rtl88e_write_fw(struct adapter *adapt, u8 *buffer, u32 size)
+{
+	u8 *buf_ptr = buffer;
+	u32 page_no, remain;
+	u32 page, offset;
+
+	_rtl88e_fill_dummy(buf_ptr, &size);
+
+	page_no = size / FW_8192C_PAGE_SIZE;
+	remain = size % FW_8192C_PAGE_SIZE;
+
+	for (page = 0; page < page_no; page++) {
+		offset = page * FW_8192C_PAGE_SIZE;
+		_rtl88e_fw_page_write(adapt, page, (buf_ptr + offset),
+				      FW_8192C_PAGE_SIZE);
+	}
+
+	if (remain) {
+		offset = page_no * FW_8192C_PAGE_SIZE;
+		page = page_no;
+		_rtl88e_fw_page_write(adapt, page, (buf_ptr + offset), remain);
+	}
+}
+
+static void rtl88e_firmware_selfreset(struct adapter *adapt)
+{
+	u8 u1b_tmp;
+
+	u1b_tmp = usb_read8(adapt, REG_SYS_FUNC_EN+1);
+	usb_write8(adapt, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
+	usb_write8(adapt, REG_SYS_FUNC_EN+1, (u1b_tmp | BIT(2)));
+}
+
+static int _rtl88e_fw_free_to_go(struct adapter *adapt)
+{
+	int err = -EIO;
+	u32 counter = 0;
+	u32 value32;
+
+	do {
+		value32 = usb_read32(adapt, REG_MCUFWDL);
+		if (value32 & FWDL_ChkSum_rpt)
+			break;
+	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
+
+	if (counter >= POLLING_READY_TIMEOUT_COUNT) {
+		goto exit;
+	}
+
+	value32 = usb_read32(adapt, REG_MCUFWDL);
+	value32 |= MCUFWDL_RDY;
+	value32 &= ~WINTINI_RDY;
+	usb_write32(adapt, REG_MCUFWDL, value32);
+
+	rtl88e_firmware_selfreset(adapt);
+	counter = 0;
+
+	do {
+		value32 = usb_read32(adapt, REG_MCUFWDL);
+		if (value32 & WINTINI_RDY) {
+			err = 0;
+			goto exit;
+		}
+
+		udelay(FW_8192C_POLLING_DELAY);
+
+	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
+
+exit:
+	return err;
+}
+
+int rtl88eu_download_fw(struct adapter *adapt)
+{
+	struct hal_data_8188e *rtlhal = GET_HAL_DATA(adapt);
+	struct dvobj_priv *dvobj = adapter_to_dvobj(adapt);
+	struct device *device = dvobj_to_dev(dvobj);
+	const struct firmware *fw;
+	const char fw_name[] = "rtlwifi/rtl8188eufw.bin";
+	struct rtl92c_firmware_header *pfwheader = NULL;
+	u8 *pfwdata;
+	u32 fwsize;
+	int err;
+
+	if (request_firmware(&fw, fw_name, device)){
+		dev_err(device, "Firmware %s not available\n", fw_name);
+		return -ENOENT;
+	}
+
+	if (fw->size > FW_8188E_SIZE) {
+		dev_err(device,"Firmware size exceed 0x%X. Check it.\n",
+			 FW_8188E_SIZE);
+		return -1;
+	}
+
+	pfwdata = kzalloc(FW_8188E_SIZE, GFP_KERNEL);
+	if (!pfwdata)
+		return -ENOMEM;
+
+	rtlhal->pfirmware = pfwdata;
+	memcpy(rtlhal->pfirmware, fw->data, fw->size);
+	rtlhal->fwsize = fw->size;
+	release_firmware(fw);
+
+	fwsize = rtlhal->fwsize;
+	pfwheader = (struct rtl92c_firmware_header *)pfwdata;
+
+	if (IS_FW_HEADER_EXIST(pfwheader)) {
+		pfwdata = pfwdata + 32;
+		fwsize = fwsize - 32;
+	}
+
+	if (usb_read8(adapt, REG_MCUFWDL) & RAM_DL_SEL) {
+		usb_write8(adapt, REG_MCUFWDL, 0);
+		rtl88e_firmware_selfreset(adapt);
+	}
+	_rtl88e_enable_fw_download(adapt, true);
+	usb_write8(adapt, REG_MCUFWDL, usb_read8(adapt, REG_MCUFWDL) | FWDL_ChkSum_rpt);
+	_rtl88e_write_fw(adapt, pfwdata, fwsize);
+	_rtl88e_enable_fw_download(adapt, false);
+
+	err = _rtl88e_fw_free_to_go(adapt);
+
+	return err;
+}
diff --git a/drivers/staging/rtl8188eu/hal/hal_intf.c b/drivers/staging/rtl8188eu/hal/hal_intf.c
index 2faa690f7e26..538a0f65d09c 100644
--- a/drivers/staging/rtl8188eu/hal/hal_intf.c
+++ b/drivers/staging/rtl8188eu/hal/hal_intf.c
@@ -256,22 +256,6 @@ void rtw_hal_add_ra_tid(struct adapter *adapt, u32 bitmap, u8 arg,
 					       rssi_level);
 }
 
-u32 rtw_hal_read_bbreg(struct adapter *adapt, u32 regaddr, u32 bitmask)
-{
-	u32 data = 0;
-
-	if (adapt->HalFunc.read_bbreg)
-		data = adapt->HalFunc.read_bbreg(adapt, regaddr, bitmask);
-	return data;
-}
-
-void rtw_hal_write_bbreg(struct adapter *adapt, u32 regaddr, u32 bitmask,
-			 u32 data)
-{
-	if (adapt->HalFunc.write_bbreg)
-		adapt->HalFunc.write_bbreg(adapt, regaddr, bitmask, data);
-}
-
 u32 rtw_hal_read_rfreg(struct adapter *adapt, enum rf_radio_path rfpath,
 		       u32 regaddr, u32 bitmask)
 {
@@ -354,16 +338,6 @@ u8   rtw_hal_sreset_get_wifi_status(struct adapter *adapt)
 	return status;
 }
 
-int rtw_hal_iol_cmd(struct adapter  *adapter, struct xmit_frame *xmit_frame,
-		    u32 max_wating_ms, u32 bndy_cnt)
-{
-	if (adapter->HalFunc.IOL_exec_cmds_sync)
-		return adapter->HalFunc.IOL_exec_cmds_sync(adapter, xmit_frame,
-							   max_wating_ms,
-							   bndy_cnt);
-	return _FAIL;
-}
-
 void rtw_hal_notch_filter(struct adapter *adapter, bool enable)
 {
 	if (adapter->HalFunc.hal_notch_filter)
diff --git a/drivers/staging/rtl8188eu/hal/mac_cfg.c b/drivers/staging/rtl8188eu/hal/mac_cfg.c
new file mode 100644
index 000000000000..c0e7fa938059
--- /dev/null
+++ b/drivers/staging/rtl8188eu/hal/mac_cfg.c
@@ -0,0 +1,134 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along with
+* this program; if not, write to the Free Software Foundation, Inc.,
+* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+*
+*
+******************************************************************************/
+
+#include "odm_precomp.h"
+#include "phy.h"
+#include <rtw_iol.h>
+
+/* MAC_REG.TXT */
+
+static u32 array_MAC_REG_8188E[] = {
+		0x026, 0x00000041,
+		0x027, 0x00000035,
+		0x428, 0x0000000A,
+		0x429, 0x00000010,
+		0x430, 0x00000000,
+		0x431, 0x00000001,
+		0x432, 0x00000002,
+		0x433, 0x00000004,
+		0x434, 0x00000005,
+		0x435, 0x00000006,
+		0x436, 0x00000007,
+		0x437, 0x00000008,
+		0x438, 0x00000000,
+		0x439, 0x00000000,
+		0x43A, 0x00000001,
+		0x43B, 0x00000002,
+		0x43C, 0x00000004,
+		0x43D, 0x00000005,
+		0x43E, 0x00000006,
+		0x43F, 0x00000007,
+		0x440, 0x0000005D,
+		0x441, 0x00000001,
+		0x442, 0x00000000,
+		0x444, 0x00000015,
+		0x445, 0x000000F0,
+		0x446, 0x0000000F,
+		0x447, 0x00000000,
+		0x458, 0x00000041,
+		0x459, 0x000000A8,
+		0x45A, 0x00000072,
+		0x45B, 0x000000B9,
+		0x460, 0x00000066,
+		0x461, 0x00000066,
+		0x480, 0x00000008,
+		0x4C8, 0x000000FF,
+		0x4C9, 0x00000008,
+		0x4CC, 0x000000FF,
+		0x4CD, 0x000000FF,
+		0x4CE, 0x00000001,
+		0x4D3, 0x00000001,
+		0x500, 0x00000026,
+		0x501, 0x000000A2,
+		0x502, 0x0000002F,
+		0x503, 0x00000000,
+		0x504, 0x00000028,
+		0x505, 0x000000A3,
+		0x506, 0x0000005E,
+		0x507, 0x00000000,
+		0x508, 0x0000002B,
+		0x509, 0x000000A4,
+		0x50A, 0x0000005E,
+		0x50B, 0x00000000,
+		0x50C, 0x0000004F,
+		0x50D, 0x000000A4,
+		0x50E, 0x00000000,
+		0x50F, 0x00000000,
+		0x512, 0x0000001C,
+		0x514, 0x0000000A,
+		0x516, 0x0000000A,
+		0x525, 0x0000004F,
+		0x550, 0x00000010,
+		0x551, 0x00000010,
+		0x559, 0x00000002,
+		0x55D, 0x000000FF,
+		0x605, 0x00000030,
+		0x608, 0x0000000E,
+		0x609, 0x0000002A,
+		0x620, 0x000000FF,
+		0x621, 0x000000FF,
+		0x622, 0x000000FF,
+		0x623, 0x000000FF,
+		0x624, 0x000000FF,
+		0x625, 0x000000FF,
+		0x626, 0x000000FF,
+		0x627, 0x000000FF,
+		0x652, 0x00000020,
+		0x63C, 0x0000000A,
+		0x63D, 0x0000000A,
+		0x63E, 0x0000000E,
+		0x63F, 0x0000000E,
+		0x640, 0x00000040,
+		0x66E, 0x00000005,
+		0x700, 0x00000021,
+		0x701, 0x00000043,
+		0x702, 0x00000065,
+		0x703, 0x00000087,
+		0x708, 0x00000021,
+		0x709, 0x00000043,
+		0x70A, 0x00000065,
+		0x70B, 0x00000087,
+};
+
+bool rtl88eu_phy_mac_config(struct adapter *adapt)
+{
+	u32 i;
+	u32 arraylength;
+	u32 *ptrarray;
+
+	arraylength = sizeof(array_MAC_REG_8188E)/sizeof(u32);
+	ptrarray = array_MAC_REG_8188E;
+
+	for (i = 0; i < arraylength; i = i + 2)
+		usb_write8(adapt, ptrarray[i], (u8) ptrarray[i + 1]);
+
+	usb_write8(adapt, REG_MAX_AGGR_NUM, MAX_AGGR_NUM);
+	return true;
+}
diff --git a/drivers/staging/rtl8188eu/hal/odm.c b/drivers/staging/rtl8188eu/hal/odm.c
index f8dcfdae0727..e4df83710ca6 100644
--- a/drivers/staging/rtl8188eu/hal/odm.c
+++ b/drivers/staging/rtl8188eu/hal/odm.c
@@ -21,7 +21,9 @@
 /*  include files */
 
 #include "odm_precomp.h"
+#include "phy.h"
 
+u32 GlobalDebugLevel;
 static const u16 dB_Invert_Table[8][12] = {
 	{1, 1, 1, 2, 2, 2, 2, 3, 3, 3, 4, 4},
 	{4, 5, 6, 6, 7, 8, 9, 10, 11, 13, 14, 16},
@@ -171,6 +173,13 @@ u8 CCKSwingTable_Ch14[CCK_TABLE_SIZE][8] = {
 #define		RxDefaultAnt1		0x65a9
 #define	RxDefaultAnt2		0x569a
 
+void ODM_InitDebugSetting(struct odm_dm_struct *pDM_Odm)
+{
+	pDM_Odm->DebugLevel = ODM_DBG_TRACE;
+
+	pDM_Odm->DebugComponents = 0;
+}
+
 /* 3 Export Interface */
 
 /*  2011/09/21 MH Add to describe different team necessary resource allocate?? */
@@ -182,7 +191,6 @@ void ODM_DMInit(struct odm_dm_struct *pDM_Odm)
 	odm_DIGInit(pDM_Odm);
 	odm_RateAdaptiveMaskInit(pDM_Odm);
 
-	odm_PrimaryCCA_Init(pDM_Odm);    /*  Gary */
 	odm_DynamicTxPowerInit(pDM_Odm);
 	odm_TXPowerTrackingInit(pDM_Odm);
 	ODM_EdcaTurboInit(pDM_Odm);
@@ -429,8 +437,8 @@ void odm_CommonInfoSelfInit(struct odm_dm_struct *pDM_Odm)
 {
 	struct adapter *adapter = pDM_Odm->Adapter;
 
-	pDM_Odm->bCckHighPower = (bool) PHY_QueryBBReg(adapter, 0x824, BIT9);
-	pDM_Odm->RFPathRxEnable = (u8) PHY_QueryBBReg(adapter, 0xc04, 0x0F);
+	pDM_Odm->bCckHighPower = (bool) phy_query_bb_reg(adapter, 0x824, BIT9);
+	pDM_Odm->RFPathRxEnable = (u8) phy_query_bb_reg(adapter, 0xc04, 0x0F);
 
 	ODM_InitDebugSetting(pDM_Odm);
 }
@@ -511,7 +519,7 @@ void ODM_Write_DIG(struct odm_dm_struct *pDM_Odm, u8 CurrentIGI)
 	struct adapter *adapter = pDM_Odm->Adapter;
 
 	if (pDM_DigTable->CurIGValue != CurrentIGI) {
-		PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N, CurrentIGI);
+		phy_set_bb_reg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N, CurrentIGI);
 		pDM_DigTable->CurIGValue = CurrentIGI;
 	}
 }
@@ -521,7 +529,7 @@ void odm_DIGInit(struct odm_dm_struct *pDM_Odm)
 	struct adapter *adapter = pDM_Odm->Adapter;
 	struct rtw_dig *pDM_DigTable = &pDM_Odm->DM_DigTable;
 
-	pDM_DigTable->CurIGValue = (u8) PHY_QueryBBReg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N);
+	pDM_DigTable->CurIGValue = (u8) phy_query_bb_reg(adapter, ODM_REG_IGI_A_11N, ODM_BIT_IGI_11N);
 	pDM_DigTable->RssiLowThresh	= DM_DIG_THRESH_LOW;
 	pDM_DigTable->RssiHighThresh	= DM_DIG_THRESH_HIGH;
 	pDM_DigTable->FALowThresh	= DM_false_ALARM_THRESH_LOW;
@@ -733,39 +741,39 @@ void odm_FalseAlarmCounterStatistics(struct odm_dm_struct *pDM_Odm)
 		return;
 
 	/* hold ofdm counter */
-	PHY_SetBBReg(adapter, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 1); /* hold page C counter */
-	PHY_SetBBReg(adapter, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 1); /* hold page D counter */
+	phy_set_bb_reg(adapter, ODM_REG_OFDM_FA_HOLDC_11N, BIT31, 1); /* hold page C counter */
+	phy_set_bb_reg(adapter, ODM_REG_OFDM_FA_RSTD_11N, BIT31, 1); /* hold page D counter */
 
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE1_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_Fast_Fsync = (ret_value&0xffff);
 	FalseAlmCnt->Cnt_SB_Search_fail = ((ret_value&0xffff0000)>>16);
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE2_11N, bMaskDWord);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE2_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_OFDM_CCA = (ret_value&0xffff);
 	FalseAlmCnt->Cnt_Parity_Fail = ((ret_value&0xffff0000)>>16);
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE3_11N, bMaskDWord);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE3_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_Rate_Illegal = (ret_value&0xffff);
 	FalseAlmCnt->Cnt_Crc8_fail = ((ret_value&0xffff0000)>>16);
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_OFDM_FA_TYPE4_11N, bMaskDWord);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_OFDM_FA_TYPE4_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_Mcs_fail = (ret_value&0xffff);
 
 	FalseAlmCnt->Cnt_Ofdm_fail = FalseAlmCnt->Cnt_Parity_Fail + FalseAlmCnt->Cnt_Rate_Illegal +
 				     FalseAlmCnt->Cnt_Crc8_fail + FalseAlmCnt->Cnt_Mcs_fail +
 				     FalseAlmCnt->Cnt_Fast_Fsync + FalseAlmCnt->Cnt_SB_Search_fail;
 
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_SC_CNT_11N, bMaskDWord);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_SC_CNT_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_BW_LSC = (ret_value&0xffff);
 	FalseAlmCnt->Cnt_BW_USC = ((ret_value&0xffff0000)>>16);
 
 	/* hold cck counter */
-	PHY_SetBBReg(adapter, ODM_REG_CCK_FA_RST_11N, BIT12, 1);
-	PHY_SetBBReg(adapter, ODM_REG_CCK_FA_RST_11N, BIT14, 1);
+	phy_set_bb_reg(adapter, ODM_REG_CCK_FA_RST_11N, BIT12, 1);
+	phy_set_bb_reg(adapter, ODM_REG_CCK_FA_RST_11N, BIT14, 1);
 
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_CCK_FA_LSB_11N, bMaskByte0);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_CCK_FA_LSB_11N, bMaskByte0);
 	FalseAlmCnt->Cnt_Cck_fail = ret_value;
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_CCK_FA_MSB_11N, bMaskByte3);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_CCK_FA_MSB_11N, bMaskByte3);
 	FalseAlmCnt->Cnt_Cck_fail +=  (ret_value & 0xff)<<8;
 
-	ret_value = PHY_QueryBBReg(adapter, ODM_REG_CCK_CCA_CNT_11N, bMaskDWord);
+	ret_value = phy_query_bb_reg(adapter, ODM_REG_CCK_CCA_CNT_11N, bMaskDWord);
 	FalseAlmCnt->Cnt_CCK_CCA = ((ret_value&0xFF)<<8) | ((ret_value&0xFF00)>>8);
 
 	FalseAlmCnt->Cnt_all = (FalseAlmCnt->Cnt_Fast_Fsync +
@@ -849,10 +857,10 @@ void ODM_RF_Saving(struct odm_dm_struct *pDM_Odm, u8 bForceInNormal)
 		Rssi_Low_bound = 45;
 	}
 	if (pDM_PSTable->initialize == 0) {
-		pDM_PSTable->Reg874 = (PHY_QueryBBReg(adapter, 0x874, bMaskDWord)&0x1CC000)>>14;
-		pDM_PSTable->RegC70 = (PHY_QueryBBReg(adapter, 0xc70, bMaskDWord)&BIT3)>>3;
-		pDM_PSTable->Reg85C = (PHY_QueryBBReg(adapter, 0x85c, bMaskDWord)&0xFF000000)>>24;
-		pDM_PSTable->RegA74 = (PHY_QueryBBReg(adapter, 0xa74, bMaskDWord)&0xF000)>>12;
+		pDM_PSTable->Reg874 = (phy_query_bb_reg(adapter, 0x874, bMaskDWord)&0x1CC000)>>14;
+		pDM_PSTable->RegC70 = (phy_query_bb_reg(adapter, 0xc70, bMaskDWord)&BIT3)>>3;
+		pDM_PSTable->Reg85C = (phy_query_bb_reg(adapter, 0x85c, bMaskDWord)&0xFF000000)>>24;
+		pDM_PSTable->RegA74 = (phy_query_bb_reg(adapter, 0xa74, bMaskDWord)&0xF000)>>12;
 		pDM_PSTable->initialize = 1;
 	}
 
@@ -878,19 +886,19 @@ void ODM_RF_Saving(struct odm_dm_struct *pDM_Odm, u8 bForceInNormal)
 
 	if (pDM_PSTable->PreRFState != pDM_PSTable->CurRFState) {
 		if (pDM_PSTable->CurRFState == RF_Save) {
-			PHY_SetBBReg(adapter, 0x874  , 0x1C0000, 0x2); /* Reg874[20:18]=3'b010 */
-			PHY_SetBBReg(adapter, 0xc70, BIT3, 0); /* RegC70[3]=1'b0 */
-			PHY_SetBBReg(adapter, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]=0x63 */
-			PHY_SetBBReg(adapter, 0x874, 0xC000, 0x2); /* Reg874[15:14]=2'b10 */
-			PHY_SetBBReg(adapter, 0xa74, 0xF000, 0x3); /* RegA75[7:4]=0x3 */
-			PHY_SetBBReg(adapter, 0x818, BIT28, 0x0); /* Reg818[28]=1'b0 */
-			PHY_SetBBReg(adapter, 0x818, BIT28, 0x1); /* Reg818[28]=1'b1 */
+			phy_set_bb_reg(adapter, 0x874  , 0x1C0000, 0x2); /* Reg874[20:18]=3'b010 */
+			phy_set_bb_reg(adapter, 0xc70, BIT3, 0); /* RegC70[3]=1'b0 */
+			phy_set_bb_reg(adapter, 0x85c, 0xFF000000, 0x63); /* Reg85C[31:24]=0x63 */
+			phy_set_bb_reg(adapter, 0x874, 0xC000, 0x2); /* Reg874[15:14]=2'b10 */
+			phy_set_bb_reg(adapter, 0xa74, 0xF000, 0x3); /* RegA75[7:4]=0x3 */
+			phy_set_bb_reg(adapter, 0x818, BIT28, 0x0); /* Reg818[28]=1'b0 */
+			phy_set_bb_reg(adapter, 0x818, BIT28, 0x1); /* Reg818[28]=1'b1 */
 		} else {
-			PHY_SetBBReg(adapter, 0x874  , 0x1CC000, pDM_PSTable->Reg874);
-			PHY_SetBBReg(adapter, 0xc70, BIT3, pDM_PSTable->RegC70);
-			PHY_SetBBReg(adapter, 0x85c, 0xFF000000, pDM_PSTable->Reg85C);
-			PHY_SetBBReg(adapter, 0xa74, 0xF000, pDM_PSTable->RegA74);
-			PHY_SetBBReg(adapter, 0x818, BIT28, 0x0);
+			phy_set_bb_reg(adapter, 0x874  , 0x1CC000, pDM_PSTable->Reg874);
+			phy_set_bb_reg(adapter, 0xc70, BIT3, pDM_PSTable->RegC70);
+			phy_set_bb_reg(adapter, 0x85c, 0xFF000000, pDM_PSTable->Reg85C);
+			phy_set_bb_reg(adapter, 0xa74, 0xF000, pDM_PSTable->RegA74);
+			phy_set_bb_reg(adapter, 0x818, BIT28, 0x0);
 		}
 		pDM_PSTable->PreRFState = pDM_PSTable->CurRFState;
 	}
@@ -1235,12 +1243,12 @@ void odm_TXPowerTrackingCheckCE(struct odm_dm_struct *pDM_Odm)
 		return;
 
 	if (!pDM_Odm->RFCalibrateInfo.TM_Trigger) {		/* at least delay 1 sec */
-		PHY_SetRFReg(Adapter, RF_PATH_A, RF_T_METER_88E, BIT17 | BIT16, 0x03);
+		phy_set_rf_reg(Adapter, RF_PATH_A, RF_T_METER_88E, BIT17 | BIT16, 0x03);
 
 		pDM_Odm->RFCalibrateInfo.TM_Trigger = 1;
 		return;
 	} else {
-		odm_TXPowerTrackingCallback_ThermalMeter_8188E(Adapter);
+		rtl88eu_dm_txpower_tracking_callback_thermalmeter(Adapter);
 		pDM_Odm->RFCalibrateInfo.TM_Trigger = 0;
 	}
 }
@@ -1256,7 +1264,7 @@ void odm_InitHybridAntDiv(struct odm_dm_struct *pDM_Odm)
 		return;
 	}
 
-	ODM_AntennaDiversityInit_88E(pDM_Odm);
+	rtl88eu_dm_antenna_div_init(pDM_Odm);
 }
 
 void odm_HwAntDiv(struct odm_dm_struct *pDM_Odm)
@@ -1266,7 +1274,7 @@ void odm_HwAntDiv(struct odm_dm_struct *pDM_Odm)
 		return;
 	}
 
-	ODM_AntennaDiversity_88E(pDM_Odm);
+	rtl88eu_dm_antenna_diversity(pDM_Odm);
 }
 
 /* EDCA Turbo */
diff --git a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
index f2e1d02b8ae3..4e4e21936e7c 100644
--- a/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
+++ b/drivers/staging/rtl8188eu/hal/odm_HWConfig.c
@@ -315,7 +315,7 @@ static void odm_Process_RSSIForDM(struct odm_dm_struct *dm_odm,
 		if (pPktinfo->bPacketToSelf || pPktinfo->bPacketBeacon) {
 			antsel_tr_mux = (pDM_FatTable->antsel_rx_keep_2<<2) |
 					(pDM_FatTable->antsel_rx_keep_1<<1) | pDM_FatTable->antsel_rx_keep_0;
-			ODM_AntselStatistics_88E(dm_odm, antsel_tr_mux, pPktinfo->StationID, pPhyInfo->RxPWDBAll);
+			rtl88eu_dm_ant_sel_statistics(dm_odm, antsel_tr_mux, pPktinfo->StationID, pPhyInfo->RxPWDBAll);
 		}
 	}
 	/* Smart Antenna Debug Message------------------ */
@@ -431,39 +431,3 @@ void ODM_PhyStatusQuery(struct odm_dm_struct *dm_odm,
 {
 	ODM_PhyStatusQuery_92CSeries(dm_odm, pPhyInfo, pPhyStatus, pPktinfo);
 }
-
-enum HAL_STATUS ODM_ConfigRFWithHeaderFile(struct odm_dm_struct *dm_odm,
-					   enum rf_radio_path content,
-					   enum rf_radio_path rfpath)
-{
-	ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_LOUD, ("===>ODM_ConfigRFWithHeaderFile\n"));
-	if (rfpath == RF_PATH_A)
-		READ_AND_CONFIG(8188E, _RadioA_1T_);
-	ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_LOUD, (" ===> ODM_ConfigRFWithHeaderFile() Radio_A:Rtl8188ERadioA_1TArray\n"));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_LOUD, (" ===> ODM_ConfigRFWithHeaderFile() Radio_B:Rtl8188ERadioB_1TArray\n"));
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_TRACE, ("ODM_ConfigRFWithHeaderFile: Radio No %x\n", rfpath));
-	return HAL_STATUS_SUCCESS;
-}
-
-enum HAL_STATUS ODM_ConfigBBWithHeaderFile(struct odm_dm_struct *dm_odm,
-					   enum odm_bb_config_type config_tp)
-{
-	if (config_tp == CONFIG_BB_PHY_REG) {
-		READ_AND_CONFIG(8188E, _PHY_REG_1T_);
-	} else if (config_tp == CONFIG_BB_AGC_TAB) {
-		READ_AND_CONFIG(8188E, _AGC_TAB_1T_);
-	} else if (config_tp == CONFIG_BB_PHY_REG_PG) {
-		READ_AND_CONFIG(8188E, _PHY_REG_PG_);
-		ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_LOUD,
-			     (" ===> phy_ConfigBBWithHeaderFile() agc:Rtl8188EPHY_REG_PGArray\n"));
-	}
-	return HAL_STATUS_SUCCESS;
-}
-
-enum HAL_STATUS ODM_ConfigMACWithHeaderFile(struct odm_dm_struct *dm_odm)
-{
-	u8 result = HAL_STATUS_SUCCESS;
-	result = READ_AND_CONFIG(8188E, _MAC_REG_);
-	return result;
-}
diff --git a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
index a24d954e2732..5342af778eb0 100644
--- a/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
+++ b/drivers/staging/rtl8188eu/hal/odm_RTL8188E.c
@@ -19,87 +19,91 @@
  ******************************************************************************/
 
 #include "odm_precomp.h"
+#include "phy.h"
 
-static void odm_RX_HWAntDivInit(struct odm_dm_struct *dm_odm)
+static void dm_rx_hw_antena_div_init(struct odm_dm_struct *dm_odm)
 {
 	struct adapter *adapter = dm_odm->Adapter;
-	u32	value32;
+	u32 value32;
 
 	if (*(dm_odm->mp_mode) == 1) {
 		dm_odm->AntDivType = CGCS_RX_SW_ANTDIV;
-		PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, BIT7, 0); /*  disable HW AntDiv */
-		PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT31, 1);  /*  1:CG, 0:CS */
+		phy_set_bb_reg(adapter, ODM_REG_IGI_A_11N, BIT7, 0);
+		phy_set_bb_reg(adapter, ODM_REG_LNA_SWITCH_11N, BIT31, 1);
 		return;
 	}
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("odm_RX_HWAntDivInit()\n"));
 
 	/* MAC Setting */
-	value32 = PHY_QueryBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord);
-	PHY_SetBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord, value32|(BIT23|BIT25)); /* Reg4C[25]=1, Reg4C[23]=1 for pin output */
+	value32 = phy_query_bb_reg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord);
+	phy_set_bb_reg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord,
+		       value32|(BIT23|BIT25));
 	/* Pin Settings */
-	PHY_SetBBReg(adapter, ODM_REG_PIN_CTRL_11N, BIT9|BIT8, 0);/* Reg870[8]=1'b0, Reg870[9]=1'b0	antsel antselb by HW */
-	PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT10, 0);	/* Reg864[10]=1'b0	antsel2 by HW */
-	PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT22, 1);	/* Regb2c[22]=1'b0	disable CS/CG switch */
-	PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT31, 1);	/* Regb2c[31]=1'b1	output at CG only */
+	phy_set_bb_reg(adapter, ODM_REG_PIN_CTRL_11N, BIT9|BIT8, 0);
+	phy_set_bb_reg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT10, 0);
+	phy_set_bb_reg(adapter, ODM_REG_LNA_SWITCH_11N, BIT22, 1);
+	phy_set_bb_reg(adapter, ODM_REG_LNA_SWITCH_11N, BIT31, 1);
 	/* OFDM Settings */
-	PHY_SetBBReg(adapter, ODM_REG_ANTDIV_PARA1_11N, bMaskDWord, 0x000000a0);
+	phy_set_bb_reg(adapter, ODM_REG_ANTDIV_PARA1_11N, bMaskDWord,
+		       0x000000a0);
 	/* CCK Settings */
-	PHY_SetBBReg(adapter, ODM_REG_BB_PWR_SAV4_11N, BIT7, 1); /* Fix CCK PHY status report issue */
-	PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA2_11N, BIT4, 1); /* CCK complete HW AntDiv within 64 samples */
-	ODM_UpdateRxIdleAnt_88E(dm_odm, MAIN_ANT);
-	PHY_SetBBReg(adapter, ODM_REG_ANT_MAPPING1_11N, 0xFFFF, 0x0201);	/* antenna mapping table */
+	phy_set_bb_reg(adapter, ODM_REG_BB_PWR_SAV4_11N, BIT7, 1);
+	phy_set_bb_reg(adapter, ODM_REG_CCK_ANTDIV_PARA2_11N, BIT4, 1);
+	rtl88eu_dm_update_rx_idle_ant(dm_odm, MAIN_ANT);
+	phy_set_bb_reg(adapter, ODM_REG_ANT_MAPPING1_11N, 0xFFFF, 0x0201);
 }
 
-static void odm_TRX_HWAntDivInit(struct odm_dm_struct *dm_odm)
+static void dm_trx_hw_antenna_div_init(struct odm_dm_struct *dm_odm)
 {
 	struct adapter *adapter = dm_odm->Adapter;
 	u32	value32;
 
 	if (*(dm_odm->mp_mode) == 1) {
 		dm_odm->AntDivType = CGCS_RX_SW_ANTDIV;
-		PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, BIT7, 0); /*  disable HW AntDiv */
-		PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT5|BIT4|BIT3, 0); /* Default RX   (0/1) */
+		phy_set_bb_reg(adapter, ODM_REG_IGI_A_11N, BIT7, 0);
+		phy_set_bb_reg(adapter, ODM_REG_RX_ANT_CTRL_11N,
+			       BIT5|BIT4|BIT3, 0);
 		return;
 	}
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("odm_TRX_HWAntDivInit()\n"));
 
 	/* MAC Setting */
-	value32 = PHY_QueryBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord);
-	PHY_SetBBReg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord, value32|(BIT23|BIT25)); /* Reg4C[25]=1, Reg4C[23]=1 for pin output */
+	value32 = phy_query_bb_reg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord);
+	phy_set_bb_reg(adapter, ODM_REG_ANTSEL_PIN_11N, bMaskDWord,
+		       value32|(BIT23|BIT25));
 	/* Pin Settings */
-	PHY_SetBBReg(adapter, ODM_REG_PIN_CTRL_11N, BIT9|BIT8, 0);/* Reg870[8]=1'b0, Reg870[9]=1'b0		antsel antselb by HW */
-	PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT10, 0);	/* Reg864[10]=1'b0	antsel2 by HW */
-	PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT22, 0);	/* Regb2c[22]=1'b0	disable CS/CG switch */
-	PHY_SetBBReg(adapter, ODM_REG_LNA_SWITCH_11N, BIT31, 1);	/* Regb2c[31]=1'b1	output at CG only */
+	phy_set_bb_reg(adapter, ODM_REG_PIN_CTRL_11N, BIT9|BIT8, 0);
+	phy_set_bb_reg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT10, 0);
+	phy_set_bb_reg(adapter, ODM_REG_LNA_SWITCH_11N, BIT22, 0);
+	phy_set_bb_reg(adapter, ODM_REG_LNA_SWITCH_11N, BIT31, 1);
 	/* OFDM Settings */
-	PHY_SetBBReg(adapter, ODM_REG_ANTDIV_PARA1_11N, bMaskDWord, 0x000000a0);
+	phy_set_bb_reg(adapter, ODM_REG_ANTDIV_PARA1_11N, bMaskDWord,
+		       0x000000a0);
 	/* CCK Settings */
-	PHY_SetBBReg(adapter, ODM_REG_BB_PWR_SAV4_11N, BIT7, 1); /* Fix CCK PHY status report issue */
-	PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA2_11N, BIT4, 1); /* CCK complete HW AntDiv within 64 samples */
+	phy_set_bb_reg(adapter, ODM_REG_BB_PWR_SAV4_11N, BIT7, 1);
+	phy_set_bb_reg(adapter, ODM_REG_CCK_ANTDIV_PARA2_11N, BIT4, 1);
 	/* Tx Settings */
-	PHY_SetBBReg(adapter, ODM_REG_TX_ANT_CTRL_11N, BIT21, 0); /* Reg80c[21]=1'b0		from TX Reg */
-	ODM_UpdateRxIdleAnt_88E(dm_odm, MAIN_ANT);
+	phy_set_bb_reg(adapter, ODM_REG_TX_ANT_CTRL_11N, BIT21, 0);
+	rtl88eu_dm_update_rx_idle_ant(dm_odm, MAIN_ANT);
 
 	/* antenna mapping table */
 	if (!dm_odm->bIsMPChip) { /* testchip */
-		PHY_SetBBReg(adapter, ODM_REG_RX_DEFUALT_A_11N, BIT10|BIT9|BIT8, 1);	/* Reg858[10:8]=3'b001 */
-		PHY_SetBBReg(adapter, ODM_REG_RX_DEFUALT_A_11N, BIT13|BIT12|BIT11, 2);	/* Reg858[13:11]=3'b010 */
+		phy_set_bb_reg(adapter, ODM_REG_RX_DEFUALT_A_11N,
+			       BIT10|BIT9|BIT8, 1);
+		phy_set_bb_reg(adapter, ODM_REG_RX_DEFUALT_A_11N,
+			       BIT13|BIT12|BIT11, 2);
 	} else { /* MPchip */
-		PHY_SetBBReg(adapter, ODM_REG_ANT_MAPPING1_11N, bMaskDWord, 0x0201);	/* Reg914=3'b010, Reg915=3'b001 */
+		phy_set_bb_reg(adapter, ODM_REG_ANT_MAPPING1_11N, bMaskDWord,
+			       0x0201);
 	}
 }
 
-static void odm_FastAntTrainingInit(struct odm_dm_struct *dm_odm)
+static void dm_fast_training_init(struct odm_dm_struct *dm_odm)
 {
 	struct adapter *adapter = dm_odm->Adapter;
-	u32	value32, i;
+	u32 value32, i;
 	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
-	u32	AntCombination = 2;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("odm_FastAntTrainingInit()\n"));
+	u32 AntCombination = 2;
 
 	if (*(dm_odm->mp_mode) == 1) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_LOUD, ("dm_odm->AntDivType: %d\n", dm_odm->AntDivType));
 		return;
 	}
 
@@ -113,208 +117,206 @@ static void odm_FastAntTrainingInit(struct odm_dm_struct *dm_odm)
 	dm_fat_tbl->FAT_State = FAT_NORMAL_STATE;
 
 	/* MAC Setting */
-	value32 = PHY_QueryBBReg(adapter, 0x4c, bMaskDWord);
-	PHY_SetBBReg(adapter, 0x4c, bMaskDWord, value32|(BIT23|BIT25)); /* Reg4C[25]=1, Reg4C[23]=1 for pin output */
-	value32 = PHY_QueryBBReg(adapter,  0x7B4, bMaskDWord);
-	PHY_SetBBReg(adapter, 0x7b4, bMaskDWord, value32|(BIT16|BIT17)); /* Reg7B4[16]=1 enable antenna training, Reg7B4[17]=1 enable A2 match */
+	value32 = phy_query_bb_reg(adapter, 0x4c, bMaskDWord);
+	phy_set_bb_reg(adapter, 0x4c, bMaskDWord, value32|(BIT23|BIT25));
+	value32 = phy_query_bb_reg(adapter,  0x7B4, bMaskDWord);
+	phy_set_bb_reg(adapter, 0x7b4, bMaskDWord, value32|(BIT16|BIT17));
 
 	/* Match MAC ADDR */
-	PHY_SetBBReg(adapter, 0x7b4, 0xFFFF, 0);
-	PHY_SetBBReg(adapter, 0x7b0, bMaskDWord, 0);
+	phy_set_bb_reg(adapter, 0x7b4, 0xFFFF, 0);
+	phy_set_bb_reg(adapter, 0x7b0, bMaskDWord, 0);
 
-	PHY_SetBBReg(adapter, 0x870, BIT9|BIT8, 0);/* Reg870[8]=1'b0, Reg870[9]=1'b0		antsel antselb by HW */
-	PHY_SetBBReg(adapter, 0x864, BIT10, 0);	/* Reg864[10]=1'b0	antsel2 by HW */
-	PHY_SetBBReg(adapter, 0xb2c, BIT22, 0);	/* Regb2c[22]=1'b0	disable CS/CG switch */
-	PHY_SetBBReg(adapter, 0xb2c, BIT31, 1);	/* Regb2c[31]=1'b1	output at CG only */
-	PHY_SetBBReg(adapter, 0xca4, bMaskDWord, 0x000000a0);
+	phy_set_bb_reg(adapter, 0x870, BIT9|BIT8, 0);
+	phy_set_bb_reg(adapter, 0x864, BIT10, 0);
+	phy_set_bb_reg(adapter, 0xb2c, BIT22, 0);
+	phy_set_bb_reg(adapter, 0xb2c, BIT31, 1);
+	phy_set_bb_reg(adapter, 0xca4, bMaskDWord, 0x000000a0);
 
 	/* antenna mapping table */
 	if (AntCombination == 2) {
 		if (!dm_odm->bIsMPChip) { /* testchip */
-			PHY_SetBBReg(adapter, 0x858, BIT10|BIT9|BIT8, 1);	/* Reg858[10:8]=3'b001 */
-			PHY_SetBBReg(adapter, 0x858, BIT13|BIT12|BIT11, 2);	/* Reg858[13:11]=3'b010 */
+			phy_set_bb_reg(adapter, 0x858, BIT10|BIT9|BIT8, 1);
+			phy_set_bb_reg(adapter, 0x858, BIT13|BIT12|BIT11, 2);
 		} else { /* MPchip */
-			PHY_SetBBReg(adapter, 0x914, bMaskByte0, 1);
-			PHY_SetBBReg(adapter, 0x914, bMaskByte1, 2);
+			phy_set_bb_reg(adapter, 0x914, bMaskByte0, 1);
+			phy_set_bb_reg(adapter, 0x914, bMaskByte1, 2);
 		}
 	} else if (AntCombination == 7) {
 		if (!dm_odm->bIsMPChip) { /* testchip */
-			PHY_SetBBReg(adapter, 0x858, BIT10|BIT9|BIT8, 0);	/* Reg858[10:8]=3'b000 */
-			PHY_SetBBReg(adapter, 0x858, BIT13|BIT12|BIT11, 1);	/* Reg858[13:11]=3'b001 */
-			PHY_SetBBReg(adapter, 0x878, BIT16, 0);
-			PHY_SetBBReg(adapter, 0x858, BIT15|BIT14, 2);	/* Reg878[0],Reg858[14:15])=3'b010 */
-			PHY_SetBBReg(adapter, 0x878, BIT19|BIT18|BIT17, 3);/* Reg878[3:1]=3b'011 */
-			PHY_SetBBReg(adapter, 0x878, BIT22|BIT21|BIT20, 4);/* Reg878[6:4]=3b'100 */
-			PHY_SetBBReg(adapter, 0x878, BIT25|BIT24|BIT23, 5);/* Reg878[9:7]=3b'101 */
-			PHY_SetBBReg(adapter, 0x878, BIT28|BIT27|BIT26, 6);/* Reg878[12:10]=3b'110 */
-			PHY_SetBBReg(adapter, 0x878, BIT31|BIT30|BIT29, 7);/* Reg878[15:13]=3b'111 */
+			phy_set_bb_reg(adapter, 0x858, BIT10|BIT9|BIT8, 0);
+			phy_set_bb_reg(adapter, 0x858, BIT13|BIT12|BIT11, 1);
+			phy_set_bb_reg(adapter, 0x878, BIT16, 0);
+			phy_set_bb_reg(adapter, 0x858, BIT15|BIT14, 2);
+			phy_set_bb_reg(adapter, 0x878, BIT19|BIT18|BIT17, 3);
+			phy_set_bb_reg(adapter, 0x878, BIT22|BIT21|BIT20, 4);
+			phy_set_bb_reg(adapter, 0x878, BIT25|BIT24|BIT23, 5);
+			phy_set_bb_reg(adapter, 0x878, BIT28|BIT27|BIT26, 6);
+			phy_set_bb_reg(adapter, 0x878, BIT31|BIT30|BIT29, 7);
 		} else { /* MPchip */
-			PHY_SetBBReg(adapter, 0x914, bMaskByte0, 0);
-			PHY_SetBBReg(adapter, 0x914, bMaskByte1, 1);
-			PHY_SetBBReg(adapter, 0x914, bMaskByte2, 2);
-			PHY_SetBBReg(adapter, 0x914, bMaskByte3, 3);
-			PHY_SetBBReg(adapter, 0x918, bMaskByte0, 4);
-			PHY_SetBBReg(adapter, 0x918, bMaskByte1, 5);
-			PHY_SetBBReg(adapter, 0x918, bMaskByte2, 6);
-			PHY_SetBBReg(adapter, 0x918, bMaskByte3, 7);
+			phy_set_bb_reg(adapter, 0x914, bMaskByte0, 0);
+			phy_set_bb_reg(adapter, 0x914, bMaskByte1, 1);
+			phy_set_bb_reg(adapter, 0x914, bMaskByte2, 2);
+			phy_set_bb_reg(adapter, 0x914, bMaskByte3, 3);
+			phy_set_bb_reg(adapter, 0x918, bMaskByte0, 4);
+			phy_set_bb_reg(adapter, 0x918, bMaskByte1, 5);
+			phy_set_bb_reg(adapter, 0x918, bMaskByte2, 6);
+			phy_set_bb_reg(adapter, 0x918, bMaskByte3, 7);
 		}
 	}
 
 	/* Default Ant Setting when no fast training */
-	PHY_SetBBReg(adapter, 0x80c, BIT21, 1); /* Reg80c[21]=1'b1		from TX Info */
-	PHY_SetBBReg(adapter, 0x864, BIT5|BIT4|BIT3, 0);	/* Default RX */
-	PHY_SetBBReg(adapter, 0x864, BIT8|BIT7|BIT6, 1);	/* Optional RX */
+	phy_set_bb_reg(adapter, 0x80c, BIT21, 1);
+	phy_set_bb_reg(adapter, 0x864, BIT5|BIT4|BIT3, 0);
+	phy_set_bb_reg(adapter, 0x864, BIT8|BIT7|BIT6, 1);
 
 	/* Enter Traing state */
-	PHY_SetBBReg(adapter, 0x864, BIT2|BIT1|BIT0, (AntCombination-1));	/* Reg864[2:0]=3'd6	ant combination=reg864[2:0]+1 */
-	PHY_SetBBReg(adapter, 0xc50, BIT7, 1);	/* RegC50[7]=1'b1		enable HW AntDiv */
+	phy_set_bb_reg(adapter, 0x864, BIT2|BIT1|BIT0, (AntCombination-1));
+	phy_set_bb_reg(adapter, 0xc50, BIT7, 1);
 }
 
-void ODM_AntennaDiversityInit_88E(struct odm_dm_struct *dm_odm)
+void rtl88eu_dm_antenna_div_init(struct odm_dm_struct *dm_odm)
 {
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("dm_odm->AntDivType=%d\n", dm_odm->AntDivType));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("dm_odm->bIsMPChip=%s\n", (dm_odm->bIsMPChip ? "true" : "false")));
-
 	if (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV)
-		odm_RX_HWAntDivInit(dm_odm);
+		dm_rx_hw_antena_div_init(dm_odm);
 	else if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
-		odm_TRX_HWAntDivInit(dm_odm);
+		dm_trx_hw_antenna_div_init(dm_odm);
 	else if (dm_odm->AntDivType == CG_TRX_SMART_ANTDIV)
-		odm_FastAntTrainingInit(dm_odm);
+		dm_fast_training_init(dm_odm);
 }
 
-void ODM_UpdateRxIdleAnt_88E(struct odm_dm_struct *dm_odm, u8 Ant)
+void rtl88eu_dm_update_rx_idle_ant(struct odm_dm_struct *dm_odm, u8 ant)
 {
 	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
 	struct adapter *adapter = dm_odm->Adapter;
-	u32	DefaultAnt, OptionalAnt;
-
-	if (dm_fat_tbl->RxIdleAnt != Ant) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("Need to Update Rx Idle Ant\n"));
-		if (Ant == MAIN_ANT) {
-			DefaultAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
-			OptionalAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+	u32 default_ant, optional_ant;
+
+	if (dm_fat_tbl->RxIdleAnt != ant) {
+		if (ant == MAIN_ANT) {
+			default_ant = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ?
+				       MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+			optional_ant = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ?
+					AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
 		} else {
-			DefaultAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
-			OptionalAnt = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ? MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
+			default_ant = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ?
+				       AUX_ANT_CG_TRX : AUX_ANT_CGCS_RX;
+			optional_ant = (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ?
+					MAIN_ANT_CG_TRX : MAIN_ANT_CGCS_RX;
 		}
 
 		if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) {
-			PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT5|BIT4|BIT3, DefaultAnt);	/* Default RX */
-			PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT8|BIT7|BIT6, OptionalAnt);		/* Optional RX */
-			PHY_SetBBReg(adapter, ODM_REG_ANTSEL_CTRL_11N, BIT14|BIT13|BIT12, DefaultAnt);	/* Default TX */
-			PHY_SetBBReg(adapter, ODM_REG_RESP_TX_11N, BIT6|BIT7, DefaultAnt);	/* Resp Tx */
+			phy_set_bb_reg(adapter, ODM_REG_RX_ANT_CTRL_11N,
+				       BIT5|BIT4|BIT3, default_ant);
+			phy_set_bb_reg(adapter, ODM_REG_RX_ANT_CTRL_11N,
+				       BIT8|BIT7|BIT6, optional_ant);
+			phy_set_bb_reg(adapter, ODM_REG_ANTSEL_CTRL_11N,
+				       BIT14|BIT13|BIT12, default_ant);
+			phy_set_bb_reg(adapter, ODM_REG_RESP_TX_11N,
+				       BIT6|BIT7, default_ant);
 		} else if (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV) {
-			PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT5|BIT4|BIT3, DefaultAnt);	/* Default RX */
-			PHY_SetBBReg(adapter, ODM_REG_RX_ANT_CTRL_11N, BIT8|BIT7|BIT6, OptionalAnt);		/* Optional RX */
+			phy_set_bb_reg(adapter, ODM_REG_RX_ANT_CTRL_11N,
+				       BIT5|BIT4|BIT3, default_ant);
+			phy_set_bb_reg(adapter, ODM_REG_RX_ANT_CTRL_11N,
+				       BIT8|BIT7|BIT6, optional_ant);
 		}
 	}
-	dm_fat_tbl->RxIdleAnt = Ant;
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("RxIdleAnt=%s\n", (Ant == MAIN_ANT) ? "MAIN_ANT" : "AUX_ANT"));
-	pr_info("RxIdleAnt=%s\n", (Ant == MAIN_ANT) ? "MAIN_ANT" : "AUX_ANT");
+	dm_fat_tbl->RxIdleAnt = ant;
 }
 
-static void odm_UpdateTxAnt_88E(struct odm_dm_struct *dm_odm, u8 Ant, u32 MacId)
+static void update_tx_ant_88eu(struct odm_dm_struct *dm_odm, u8 ant, u32 mac_id)
 {
 	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
-	u8	TargetAnt;
+	u8 target_ant;
 
-	if (Ant == MAIN_ANT)
-		TargetAnt = MAIN_ANT_CG_TRX;
+	if (ant == MAIN_ANT)
+		target_ant = MAIN_ANT_CG_TRX;
 	else
-		TargetAnt = AUX_ANT_CG_TRX;
-	dm_fat_tbl->antsel_a[MacId] = TargetAnt&BIT0;
-	dm_fat_tbl->antsel_b[MacId] = (TargetAnt&BIT1)>>1;
-	dm_fat_tbl->antsel_c[MacId] = (TargetAnt&BIT2)>>2;
-
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
-		     ("Tx from TxInfo, TargetAnt=%s\n",
-		     (Ant == MAIN_ANT) ? "MAIN_ANT" : "AUX_ANT"));
-	ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
-		     ("antsel_tr_mux=3'b%d%d%d\n",
-		     dm_fat_tbl->antsel_c[MacId], dm_fat_tbl->antsel_b[MacId], dm_fat_tbl->antsel_a[MacId]));
+		target_ant = AUX_ANT_CG_TRX;
+	dm_fat_tbl->antsel_a[mac_id] = target_ant&BIT0;
+	dm_fat_tbl->antsel_b[mac_id] = (target_ant&BIT1)>>1;
+	dm_fat_tbl->antsel_c[mac_id] = (target_ant&BIT2)>>2;
 }
 
-void ODM_SetTxAntByTxInfo_88E(struct odm_dm_struct *dm_odm, u8 *pDesc, u8 macId)
+void rtl88eu_dm_set_tx_ant_by_tx_info(struct odm_dm_struct *dm_odm,
+				      u8 *desc, u8 mac_id)
 {
 	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
 
-	if ((dm_odm->AntDivType == CG_TRX_HW_ANTDIV) || (dm_odm->AntDivType == CG_TRX_SMART_ANTDIV)) {
-		SET_TX_DESC_ANTSEL_A_88E(pDesc, dm_fat_tbl->antsel_a[macId]);
-		SET_TX_DESC_ANTSEL_B_88E(pDesc, dm_fat_tbl->antsel_b[macId]);
-		SET_TX_DESC_ANTSEL_C_88E(pDesc, dm_fat_tbl->antsel_c[macId]);
+	if ((dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ||
+	    (dm_odm->AntDivType == CG_TRX_SMART_ANTDIV)) {
+		SET_TX_DESC_ANTSEL_A_88E(desc, dm_fat_tbl->antsel_a[mac_id]);
+		SET_TX_DESC_ANTSEL_B_88E(desc, dm_fat_tbl->antsel_b[mac_id]);
+		SET_TX_DESC_ANTSEL_C_88E(desc, dm_fat_tbl->antsel_c[mac_id]);
 	}
 }
 
-void ODM_AntselStatistics_88E(struct odm_dm_struct *dm_odm, u8 antsel_tr_mux, u32 MacId, u8 RxPWDBAll)
+void rtl88eu_dm_ant_sel_statistics(struct odm_dm_struct *dm_odm,
+				   u8 antsel_tr_mux, u32 mac_id, u8 rx_pwdb_all)
 {
 	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
 	if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV) {
 		if (antsel_tr_mux == MAIN_ANT_CG_TRX) {
-			dm_fat_tbl->MainAnt_Sum[MacId] += RxPWDBAll;
-			dm_fat_tbl->MainAnt_Cnt[MacId]++;
+			dm_fat_tbl->MainAnt_Sum[mac_id] += rx_pwdb_all;
+			dm_fat_tbl->MainAnt_Cnt[mac_id]++;
 		} else {
-			dm_fat_tbl->AuxAnt_Sum[MacId] += RxPWDBAll;
-			dm_fat_tbl->AuxAnt_Cnt[MacId]++;
+			dm_fat_tbl->AuxAnt_Sum[mac_id] += rx_pwdb_all;
+			dm_fat_tbl->AuxAnt_Cnt[mac_id]++;
 		}
 	} else if (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV) {
 		if (antsel_tr_mux == MAIN_ANT_CGCS_RX) {
-			dm_fat_tbl->MainAnt_Sum[MacId] += RxPWDBAll;
-			dm_fat_tbl->MainAnt_Cnt[MacId]++;
+			dm_fat_tbl->MainAnt_Sum[mac_id] += rx_pwdb_all;
+			dm_fat_tbl->MainAnt_Cnt[mac_id]++;
 		} else {
-			dm_fat_tbl->AuxAnt_Sum[MacId] += RxPWDBAll;
-			dm_fat_tbl->AuxAnt_Cnt[MacId]++;
+			dm_fat_tbl->AuxAnt_Sum[mac_id] += rx_pwdb_all;
+			dm_fat_tbl->AuxAnt_Cnt[mac_id]++;
 		}
 	}
 }
 
-static void odm_HWAntDiv(struct odm_dm_struct *dm_odm)
+static void rtl88eu_dm_hw_ant_div(struct odm_dm_struct *dm_odm)
 {
-	u32	i, MinRSSI = 0xFF, AntDivMaxRSSI = 0, MaxRSSI = 0, LocalMinRSSI, LocalMaxRSSI;
-	u32	Main_RSSI, Aux_RSSI;
-	u8	RxIdleAnt = 0, TargetAnt = 7;
 	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
-	struct rtw_dig *pDM_DigTable = &dm_odm->DM_DigTable;
-	struct sta_info *pEntry;
+	struct rtw_dig *dig_table = &dm_odm->DM_DigTable;
+	struct sta_info *entry;
+	u32 i, min_rssi = 0xFF, ant_div_max_rssi = 0, max_rssi = 0;
+	u32 local_min_rssi,local_max_rssi;
+	u32 main_rssi, aux_rssi;
+	u8 RxIdleAnt = 0, target_ant = 7;
 
 	for (i = 0; i < ODM_ASSOCIATE_ENTRY_NUM; i++) {
-		pEntry = dm_odm->pODM_StaInfo[i];
-		if (IS_STA_VALID(pEntry)) {
+		entry = dm_odm->pODM_StaInfo[i];
+		if (IS_STA_VALID(entry)) {
 			/* 2 Caculate RSSI per Antenna */
-			Main_RSSI = (dm_fat_tbl->MainAnt_Cnt[i] != 0) ? (dm_fat_tbl->MainAnt_Sum[i]/dm_fat_tbl->MainAnt_Cnt[i]) : 0;
-			Aux_RSSI = (dm_fat_tbl->AuxAnt_Cnt[i] != 0) ? (dm_fat_tbl->AuxAnt_Sum[i]/dm_fat_tbl->AuxAnt_Cnt[i]) : 0;
-			TargetAnt = (Main_RSSI >= Aux_RSSI) ? MAIN_ANT : AUX_ANT;
-			ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
-				     ("MacID=%d, MainAnt_Sum=%d, MainAnt_Cnt=%d\n",
-				     i, dm_fat_tbl->MainAnt_Sum[i],
-				     dm_fat_tbl->MainAnt_Cnt[i]));
-			ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
-				     ("MacID=%d, AuxAnt_Sum=%d, AuxAnt_Cnt=%d\n",
-				     i, dm_fat_tbl->AuxAnt_Sum[i], dm_fat_tbl->AuxAnt_Cnt[i]));
-			ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
-				     ("MacID=%d, Main_RSSI= %d, Aux_RSSI= %d\n",
-				     i, Main_RSSI, Aux_RSSI));
-			/* 2 Select MaxRSSI for DIG */
-			LocalMaxRSSI = (Main_RSSI > Aux_RSSI) ? Main_RSSI : Aux_RSSI;
-			if ((LocalMaxRSSI > AntDivMaxRSSI) && (LocalMaxRSSI < 40))
-				AntDivMaxRSSI = LocalMaxRSSI;
-			if (LocalMaxRSSI > MaxRSSI)
-				MaxRSSI = LocalMaxRSSI;
+			main_rssi = (dm_fat_tbl->MainAnt_Cnt[i] != 0) ?
+				     (dm_fat_tbl->MainAnt_Sum[i]/dm_fat_tbl->MainAnt_Cnt[i]) : 0;
+			aux_rssi = (dm_fat_tbl->AuxAnt_Cnt[i] != 0) ?
+				    (dm_fat_tbl->AuxAnt_Sum[i]/dm_fat_tbl->AuxAnt_Cnt[i]) : 0;
+			target_ant = (main_rssi >= aux_rssi) ? MAIN_ANT : AUX_ANT;
+			/* 2 Select max_rssi for DIG */
+			local_max_rssi = (main_rssi > aux_rssi) ?
+					  main_rssi : aux_rssi;
+			if ((local_max_rssi > ant_div_max_rssi) &&
+			    (local_max_rssi < 40))
+				ant_div_max_rssi = local_max_rssi;
+			if (local_max_rssi > max_rssi)
+				max_rssi = local_max_rssi;
 
 			/* 2 Select RX Idle Antenna */
-			if ((dm_fat_tbl->RxIdleAnt == MAIN_ANT) && (Main_RSSI == 0))
-				Main_RSSI = Aux_RSSI;
-			else if ((dm_fat_tbl->RxIdleAnt == AUX_ANT) && (Aux_RSSI == 0))
-				Aux_RSSI = Main_RSSI;
-
-			LocalMinRSSI = (Main_RSSI > Aux_RSSI) ? Aux_RSSI : Main_RSSI;
-			if (LocalMinRSSI < MinRSSI) {
-				MinRSSI = LocalMinRSSI;
-				RxIdleAnt = TargetAnt;
+			if ((dm_fat_tbl->RxIdleAnt == MAIN_ANT) &&
+			    (main_rssi == 0))
+				main_rssi = aux_rssi;
+			else if ((dm_fat_tbl->RxIdleAnt == AUX_ANT) &&
+				 (aux_rssi == 0))
+				aux_rssi = main_rssi;
+
+			local_min_rssi = (main_rssi > aux_rssi) ?
+					  aux_rssi : main_rssi;
+			if (local_min_rssi < min_rssi) {
+				min_rssi = local_min_rssi;
+				RxIdleAnt = target_ant;
 			}
 			/* 2 Select TRX Antenna */
 			if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
-				odm_UpdateTxAnt_88E(dm_odm, TargetAnt, i);
+				update_tx_ant_88eu(dm_odm, target_ant, i);
 		}
 		dm_fat_tbl->MainAnt_Sum[i] = 0;
 		dm_fat_tbl->AuxAnt_Sum[i] = 0;
@@ -323,13 +325,13 @@ static void odm_HWAntDiv(struct odm_dm_struct *dm_odm)
 	}
 
 	/* 2 Set RX Idle Antenna */
-	ODM_UpdateRxIdleAnt_88E(dm_odm, RxIdleAnt);
+	rtl88eu_dm_update_rx_idle_ant(dm_odm, RxIdleAnt);
 
-	pDM_DigTable->AntDiv_RSSI_max = AntDivMaxRSSI;
-	pDM_DigTable->RSSI_max = MaxRSSI;
+	dig_table->AntDiv_RSSI_max = ant_div_max_rssi;
+	dig_table->RSSI_max = max_rssi;
 }
 
-void ODM_AntennaDiversity_88E(struct odm_dm_struct *dm_odm)
+void rtl88eu_dm_antenna_diversity(struct odm_dm_struct *dm_odm)
 {
 	struct fast_ant_train *dm_fat_tbl = &dm_odm->DM_FatTable;
 	struct adapter *adapter = dm_odm->Adapter;
@@ -337,42 +339,34 @@ void ODM_AntennaDiversity_88E(struct odm_dm_struct *dm_odm)
 	if (!(dm_odm->SupportAbility & ODM_BB_ANT_DIV))
 		return;
 	if (!dm_odm->bLinked) {
-		ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("ODM_AntennaDiversity_88E(): No Link.\n"));
+		ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
+			     ("ODM_AntennaDiversity_88E(): No Link.\n"));
 		if (dm_fat_tbl->bBecomeLinked) {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("Need to Turn off HW AntDiv\n"));
-			PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, BIT7, 0);	/* RegC50[7]=1'b1		enable HW AntDiv */
-			PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA1_11N, BIT15, 0); /* Enable CCK AntDiv */
+			ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
+				     ("Need to Turn off HW AntDiv\n"));
+			phy_set_bb_reg(adapter, ODM_REG_IGI_A_11N, BIT7, 0);
+			phy_set_bb_reg(adapter, ODM_REG_CCK_ANTDIV_PARA1_11N,
+				       BIT15, 0);
 			if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
-				PHY_SetBBReg(adapter, ODM_REG_TX_ANT_CTRL_11N, BIT21, 0); /* Reg80c[21]=1'b0		from TX Reg */
+				phy_set_bb_reg(adapter, ODM_REG_TX_ANT_CTRL_11N,
+					       BIT21, 0);
 			dm_fat_tbl->bBecomeLinked = dm_odm->bLinked;
 		}
 		return;
 	} else {
 		if (!dm_fat_tbl->bBecomeLinked) {
-			ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD, ("Need to Turn on HW AntDiv\n"));
-			/* Because HW AntDiv is disabled before Link, we enable HW AntDiv after link */
-			PHY_SetBBReg(adapter, ODM_REG_IGI_A_11N, BIT7, 1);	/* RegC50[7]=1'b1		enable HW AntDiv */
-			PHY_SetBBReg(adapter, ODM_REG_CCK_ANTDIV_PARA1_11N, BIT15, 1); /* Enable CCK AntDiv */
+			ODM_RT_TRACE(dm_odm, ODM_COMP_ANT_DIV, ODM_DBG_LOUD,
+				     ("Need to Turn on HW AntDiv\n"));
+			phy_set_bb_reg(adapter, ODM_REG_IGI_A_11N, BIT7, 1);
+			phy_set_bb_reg(adapter, ODM_REG_CCK_ANTDIV_PARA1_11N,
+				       BIT15, 1);
 			if (dm_odm->AntDivType == CG_TRX_HW_ANTDIV)
-				PHY_SetBBReg(adapter, ODM_REG_TX_ANT_CTRL_11N, BIT21, 1); /* Reg80c[21]=1'b1		from TX Info */
+				phy_set_bb_reg(adapter, ODM_REG_TX_ANT_CTRL_11N,
+					       BIT21, 1);
 			dm_fat_tbl->bBecomeLinked = dm_odm->bLinked;
 		}
 	}
-	if ((dm_odm->AntDivType == CG_TRX_HW_ANTDIV) || (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV))
-		odm_HWAntDiv(dm_odm);
-}
-
-/* 3============================================================ */
-/* 3 Dynamic Primary CCA */
-/* 3============================================================ */
-
-void odm_PrimaryCCA_Init(struct odm_dm_struct *dm_odm)
-{
-	struct dyn_primary_cca *PrimaryCCA = &(dm_odm->DM_PriCCA);
-
-	PrimaryCCA->DupRTS_flag = 0;
-	PrimaryCCA->intf_flag = 0;
-	PrimaryCCA->intf_type = 0;
-	PrimaryCCA->Monitor_flag = 0;
-	PrimaryCCA->PriCCA_flag = 0;
+	if ((dm_odm->AntDivType == CG_TRX_HW_ANTDIV) ||
+	    (dm_odm->AntDivType == CGCS_RX_HW_ANTDIV))
+		rtl88eu_dm_hw_ant_div(dm_odm);
 }
diff --git a/drivers/staging/rtl8188eu/hal/odm_RegConfig8188E.c b/drivers/staging/rtl8188eu/hal/odm_RegConfig8188E.c
deleted file mode 100644
index 4d4978bee51d..000000000000
--- a/drivers/staging/rtl8188eu/hal/odm_RegConfig8188E.c
+++ /dev/null
@@ -1,138 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-
-#include "odm_precomp.h"
-
-void odm_ConfigRFReg_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr,
-			   u32 Data, enum rf_radio_path RF_PATH,
-			   u32 RegAddr)
-{
-	struct adapter *adapter = pDM_Odm->Adapter;
-
-	if (Addr == 0xffe) {
-		msleep(50);
-	} else if (Addr == 0xfd) {
-		mdelay(5);
-	} else if (Addr == 0xfc) {
-		mdelay(1);
-	} else if (Addr == 0xfb) {
-		udelay(50);
-	} else if (Addr == 0xfa) {
-		udelay(5);
-	} else if (Addr == 0xf9) {
-		udelay(1);
-	} else {
-		PHY_SetRFReg(adapter, RF_PATH, RegAddr, bRFRegOffsetMask, Data);
-		/*  Add 1us delay between BB/RF register setting. */
-		udelay(1);
-	}
-}
-
-void odm_ConfigRF_RadioA_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Data)
-{
-	u32  content = 0x1000; /*  RF_Content: radioa_txt */
-	u32 maskforPhySet = (u32)(content&0xE000);
-
-	odm_ConfigRFReg_8188E(pDM_Odm, Addr, Data, RF_PATH_A, Addr|maskforPhySet);
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE, ("===> ODM_ConfigRFWithHeaderFile: [RadioA] %08X %08X\n", Addr, Data));
-}
-
-void odm_ConfigRF_RadioB_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Data)
-{
-	u32  content = 0x1001; /*  RF_Content: radiob_txt */
-	u32 maskforPhySet = (u32)(content&0xE000);
-
-	odm_ConfigRFReg_8188E(pDM_Odm, Addr, Data, RF_PATH_B, Addr|maskforPhySet);
-
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE, ("===> ODM_ConfigRFWithHeaderFile: [RadioB] %08X %08X\n", Addr, Data));
-}
-
-void odm_ConfigMAC_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u8 Data)
-{
-	struct adapter *adapt = pDM_Odm->Adapter;
-
-	usb_write8(adapt, Addr, Data);
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE, ("===> ODM_ConfigMACWithHeaderFile: [MAC_REG] %08X %08X\n", Addr, Data));
-}
-
-void odm_ConfigBB_AGC_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Bitmask, u32 Data)
-{
-	struct adapter *adapter = pDM_Odm->Adapter;
-
-	PHY_SetBBReg(adapter, Addr, Bitmask, Data);
-	/*  Add 1us delay between BB/RF register setting. */
-	udelay(1);
-
-	ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE,
-		     ("===> ODM_ConfigBBWithHeaderFile: [AGC_TAB] %08X %08X\n",
-		     Addr, Data));
-}
-
-void odm_ConfigBB_PHY_REG_PG_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr,
-				   u32 Bitmask, u32 Data)
-{
-	if (Addr == 0xfe) {
-		msleep(50);
-	} else if (Addr == 0xfd) {
-		mdelay(5);
-	} else if (Addr == 0xfc) {
-		mdelay(1);
-	} else if (Addr == 0xfb) {
-		udelay(50);
-	} else if (Addr == 0xfa) {
-		udelay(5);
-	} else if (Addr == 0xf9) {
-		udelay(1);
-	} else{
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_LOUD,
-			     ("===> @@@@@@@ ODM_ConfigBBWithHeaderFile: [PHY_REG] %08X %08X %08X\n",
-			     Addr, Bitmask, Data));
-		storePwrIndexDiffRateOffset(pDM_Odm->Adapter, Addr, Bitmask, Data);
-	}
-}
-
-void odm_ConfigBB_PHY_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Bitmask, u32 Data)
-{
-	struct adapter *adapter = pDM_Odm->Adapter;
-
-	if (Addr == 0xfe) {
-		msleep(50);
-	} else if (Addr == 0xfd) {
-		mdelay(5);
-	} else if (Addr == 0xfc) {
-		mdelay(1);
-	} else if (Addr == 0xfb) {
-		udelay(50);
-	} else if (Addr == 0xfa) {
-		udelay(5);
-	} else if (Addr == 0xf9) {
-		udelay(1);
-	} else {
-		if (Addr == 0xa24)
-			pDM_Odm->RFCalibrateInfo.RegA24 = Data;
-		PHY_SetBBReg(adapter, Addr, Bitmask, Data);
-
-		/*  Add 1us delay between BB/RF register setting. */
-		udelay(1);
-		ODM_RT_TRACE(pDM_Odm, ODM_COMP_INIT, ODM_DBG_TRACE,
-			     ("===> ODM_ConfigBBWithHeaderFile: [PHY_REG] %08X %08X\n",
-			     Addr, Data));
-	}
-}
diff --git a/drivers/staging/rtl8188eu/hal/odm_debug.c b/drivers/staging/rtl8188eu/hal/odm_debug.c
deleted file mode 100644
index 84caadd6c8e5..000000000000
--- a/drivers/staging/rtl8188eu/hal/odm_debug.c
+++ /dev/null
@@ -1,32 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-
-/*  include files */
-
-#include "odm_precomp.h"
-
-void ODM_InitDebugSetting(struct odm_dm_struct *pDM_Odm)
-{
-	pDM_Odm->DebugLevel = ODM_DBG_TRACE;
-
-	pDM_Odm->DebugComponents = 0;
-}
-
-u32 GlobalDebugLevel;
diff --git a/drivers/staging/rtl8188eu/hal/phy.c b/drivers/staging/rtl8188eu/hal/phy.c
new file mode 100644
index 000000000000..c4f7f358a81c
--- /dev/null
+++ b/drivers/staging/rtl8188eu/hal/phy.c
@@ -0,0 +1,1570 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#define _RTL8188E_PHYCFG_C_
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <rtw_iol.h>
+#include <rtl8188e_hal.h>
+#include <rf.h>
+#include <phy.h>
+
+#define MAX_PRECMD_CNT 16
+#define MAX_RFDEPENDCMD_CNT 16
+#define MAX_POSTCMD_CNT 16
+
+#define MAX_DOZE_WAITING_TIMES_9x 64
+
+static u32 cal_bit_shift(u32 bitmask)
+{
+	u32 i;
+
+	for (i = 0; i <= 31; i++) {
+		if (((bitmask >> i) & 0x1) == 1)
+			break;
+	}
+	return i;
+}
+
+u32 phy_query_bb_reg(struct adapter *adapt, u32 regaddr, u32 bitmask)
+{
+	u32 return_value = 0, original_value, bit_shift;
+
+	original_value = usb_read32(adapt, regaddr);
+	bit_shift = cal_bit_shift(bitmask);
+	return_value = (original_value & bitmask) >> bit_shift;
+	return return_value;
+}
+
+void phy_set_bb_reg(struct adapter *adapt, u32 regaddr, u32 bitmask, u32 data)
+{
+	u32 original_value, bit_shift;
+
+	if (bitmask != bMaskDWord) { /* if not "double word" write */
+		original_value = usb_read32(adapt, regaddr);
+		bit_shift = cal_bit_shift(bitmask);
+		data = ((original_value & (~bitmask)) | (data << bit_shift));
+	}
+
+	usb_write32(adapt, regaddr, data);
+}
+
+static u32 rf_serial_read(struct adapter *adapt,
+			enum rf_radio_path rfpath, u32 offset)
+{
+	u32 ret = 0;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct bb_reg_def *phyreg = &hal_data->PHYRegDef[rfpath];
+	u32 newoffset;
+	u32 tmplong, tmplong2;
+	u8 rfpi_enable = 0;
+
+	offset &= 0xff;
+	newoffset = offset;
+
+	tmplong = phy_query_bb_reg(adapt, rFPGA0_XA_HSSIParameter2, bMaskDWord);
+	if (rfpath == RF_PATH_A)
+		tmplong2 = tmplong;
+	else
+		tmplong2 = phy_query_bb_reg(adapt, phyreg->rfHSSIPara2,
+					    bMaskDWord);
+
+	tmplong2 = (tmplong2 & (~bLSSIReadAddress)) |
+		   (newoffset<<23) | bLSSIReadEdge;
+
+	phy_set_bb_reg(adapt, rFPGA0_XA_HSSIParameter2, bMaskDWord,
+		       tmplong&(~bLSSIReadEdge));
+	udelay(10);
+
+	phy_set_bb_reg(adapt, phyreg->rfHSSIPara2, bMaskDWord, tmplong2);
+	udelay(100);
+
+	udelay(10);
+
+	if (rfpath == RF_PATH_A)
+		rfpi_enable = (u8)phy_query_bb_reg(adapt, rFPGA0_XA_HSSIParameter1, BIT8);
+	else if (rfpath == RF_PATH_B)
+		rfpi_enable = (u8)phy_query_bb_reg(adapt, rFPGA0_XB_HSSIParameter1, BIT8);
+
+	if (rfpi_enable)
+		ret = phy_query_bb_reg(adapt, phyreg->rfLSSIReadBackPi,
+				       bLSSIReadBackData);
+	else
+		ret = phy_query_bb_reg(adapt, phyreg->rfLSSIReadBack,
+				       bLSSIReadBackData);
+	return ret;
+}
+
+static void rf_serial_write(struct adapter *adapt,
+			    enum rf_radio_path rfpath, u32 offset,
+			    u32 data)
+{
+	u32 data_and_addr = 0;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct bb_reg_def *phyreg = &hal_data->PHYRegDef[rfpath];
+	u32 newoffset;
+
+	newoffset = offset & 0xff;
+	data_and_addr = ((newoffset<<20) | (data&0x000fffff)) & 0x0fffffff;
+	phy_set_bb_reg(adapt, phyreg->rf3wireOffset, bMaskDWord, data_and_addr);
+}
+
+u32 phy_query_rf_reg(struct adapter *adapt, enum rf_radio_path rf_path,
+		     u32 reg_addr, u32 bit_mask)
+{
+	u32 original_value, readback_value, bit_shift;
+
+	original_value = rf_serial_read(adapt, rf_path, reg_addr);
+	bit_shift =  cal_bit_shift(bit_mask);
+	readback_value = (original_value & bit_mask) >> bit_shift;
+	return readback_value;
+}
+
+void phy_set_rf_reg(struct adapter *adapt, enum rf_radio_path rf_path,
+		     u32 reg_addr, u32 bit_mask, u32 data)
+{
+	u32 original_value, bit_shift;
+
+	/*  RF data is 12 bits only */
+	if (bit_mask != bRFRegOffsetMask) {
+		original_value = rf_serial_read(adapt, rf_path, reg_addr);
+		bit_shift =  cal_bit_shift(bit_mask);
+		data = ((original_value & (~bit_mask)) | (data << bit_shift));
+	}
+
+	rf_serial_write(adapt, rf_path, reg_addr, data);
+}
+
+static void get_tx_power_index(struct adapter *adapt, u8 channel, u8 *cck_pwr,
+			       u8 *ofdm_pwr, u8 *bw20_pwr, u8 *bw40_pwr)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	u8 index = (channel - 1);
+	u8 TxCount = 0, path_nums;
+
+	if ((RF_1T2R == hal_data->rf_type) || (RF_1T1R == hal_data->rf_type))
+		path_nums = 1;
+	else
+		path_nums = 2;
+
+	for (TxCount = 0; TxCount < path_nums; TxCount++) {
+		if (TxCount == RF_PATH_A) {
+			cck_pwr[TxCount] = hal_data->Index24G_CCK_Base[TxCount][index];
+			ofdm_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+					    hal_data->OFDM_24G_Diff[TxCount][RF_PATH_A];
+
+			bw20_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+					    hal_data->BW20_24G_Diff[TxCount][RF_PATH_A];
+			bw40_pwr[TxCount] = hal_data->Index24G_BW40_Base[TxCount][index];
+		} else if (TxCount == RF_PATH_B) {
+			cck_pwr[TxCount] = hal_data->Index24G_CCK_Base[TxCount][index];
+			ofdm_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[TxCount][index];
+
+			bw20_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[TxCount][RF_PATH_A]+
+			hal_data->BW20_24G_Diff[TxCount][index];
+			bw40_pwr[TxCount] = hal_data->Index24G_BW40_Base[TxCount][index];
+		} else if (TxCount == RF_PATH_C) {
+			cck_pwr[TxCount] = hal_data->Index24G_CCK_Base[TxCount][index];
+			ofdm_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_B][index]+
+			hal_data->BW20_24G_Diff[TxCount][index];
+
+			bw20_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_B][index]+
+			hal_data->BW20_24G_Diff[TxCount][index];
+			bw40_pwr[TxCount] = hal_data->Index24G_BW40_Base[TxCount][index];
+		} else if (TxCount == RF_PATH_D) {
+			cck_pwr[TxCount] = hal_data->Index24G_CCK_Base[TxCount][index];
+			ofdm_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_B][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_C][index]+
+			hal_data->BW20_24G_Diff[TxCount][index];
+
+			bw20_pwr[TxCount] = hal_data->Index24G_BW40_Base[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_A][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_B][index]+
+			hal_data->BW20_24G_Diff[RF_PATH_C][index]+
+			hal_data->BW20_24G_Diff[TxCount][index];
+			bw40_pwr[TxCount] = hal_data->Index24G_BW40_Base[TxCount][index];
+		}
+	}
+}
+
+static void phy_power_index_check(struct adapter *adapt, u8 channel,
+				  u8 *cck_pwr, u8 *ofdm_pwr, u8 *bw20_pwr,
+				  u8 *bw40_pwr)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+
+	hal_data->CurrentCckTxPwrIdx = cck_pwr[0];
+	hal_data->CurrentOfdm24GTxPwrIdx = ofdm_pwr[0];
+	hal_data->CurrentBW2024GTxPwrIdx = bw20_pwr[0];
+	hal_data->CurrentBW4024GTxPwrIdx = bw40_pwr[0];
+}
+
+void phy_set_tx_power_level(struct adapter *adapt, u8 channel)
+{
+	u8 cck_pwr[MAX_TX_COUNT] = {0};
+	u8 ofdm_pwr[MAX_TX_COUNT] = {0};/*  [0]:RF-A, [1]:RF-B */
+	u8 bw20_pwr[MAX_TX_COUNT] = {0};
+	u8 bw40_pwr[MAX_TX_COUNT] = {0};
+
+	get_tx_power_index(adapt, channel, &cck_pwr[0], &ofdm_pwr[0],
+			   &bw20_pwr[0], &bw40_pwr[0]);
+
+	phy_power_index_check(adapt, channel, &cck_pwr[0], &ofdm_pwr[0],
+			      &bw20_pwr[0], &bw40_pwr[0]);
+
+	rtl88eu_phy_rf6052_set_cck_txpower(adapt, &cck_pwr[0]);
+	rtl88eu_phy_rf6052_set_ofdm_txpower(adapt, &ofdm_pwr[0], &bw20_pwr[0],
+					  &bw40_pwr[0], channel);
+}
+
+static void phy_set_bw_mode_callback(struct adapter *adapt)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	u8 reg_bw_opmode;
+	u8 reg_prsr_rsc;
+
+	if (hal_data->rf_chip == RF_PSEUDO_11N)
+		return;
+
+	/*  There is no 40MHz mode in RF_8225. */
+	if (hal_data->rf_chip == RF_8225)
+		return;
+
+	if (adapt->bDriverStopped)
+		return;
+
+	/* Set MAC register */
+
+	reg_bw_opmode = usb_read8(adapt, REG_BWOPMODE);
+	reg_prsr_rsc = usb_read8(adapt, REG_RRSR+2);
+
+	switch (hal_data->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		reg_bw_opmode |= BW_OPMODE_20MHZ;
+		usb_write8(adapt, REG_BWOPMODE, reg_bw_opmode);
+		break;
+	case HT_CHANNEL_WIDTH_40:
+		reg_bw_opmode &= ~BW_OPMODE_20MHZ;
+		usb_write8(adapt, REG_BWOPMODE, reg_bw_opmode);
+		reg_prsr_rsc = (reg_prsr_rsc&0x90) |
+			       (hal_data->nCur40MhzPrimeSC<<5);
+		usb_write8(adapt, REG_RRSR+2, reg_prsr_rsc);
+		break;
+	default:
+		break;
+	}
+
+	/* Set PHY related register */
+	switch (hal_data->CurrentChannelBW) {
+	case HT_CHANNEL_WIDTH_20:
+		phy_set_bb_reg(adapt, rFPGA0_RFMOD, bRFMOD, 0x0);
+		phy_set_bb_reg(adapt, rFPGA1_RFMOD, bRFMOD, 0x0);
+		break;
+	case HT_CHANNEL_WIDTH_40:
+		phy_set_bb_reg(adapt, rFPGA0_RFMOD, bRFMOD, 0x1);
+		phy_set_bb_reg(adapt, rFPGA1_RFMOD, bRFMOD, 0x1);
+		/* Set Control channel to upper or lower.
+		 * These settings are required only for 40MHz
+		 */
+		phy_set_bb_reg(adapt, rCCK0_System, bCCKSideBand,
+		    (hal_data->nCur40MhzPrimeSC>>1));
+		phy_set_bb_reg(adapt, rOFDM1_LSTF, 0xC00,
+			       hal_data->nCur40MhzPrimeSC);
+		phy_set_bb_reg(adapt, 0x818, (BIT26 | BIT27),
+		   (hal_data->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
+		break;
+	default:
+		break;
+	}
+
+	/* Set RF related register */
+	switch (hal_data->rf_chip) {
+	case RF_8225:
+		break;
+	case RF_8256:
+		break;
+	case RF_8258:
+		break;
+	case RF_PSEUDO_11N:
+		break;
+	case RF_6052:
+		rtl88eu_phy_rf6052_set_bandwidth(adapt, hal_data->CurrentChannelBW);
+		break;
+	default:
+		break;
+	}
+}
+
+void phy_set_bw_mode(struct adapter *adapt, enum ht_channel_width bandwidth,
+		     unsigned char offset)
+{
+	struct hal_data_8188e	*hal_data = GET_HAL_DATA(adapt);
+	enum ht_channel_width tmp_bw = hal_data->CurrentChannelBW;
+
+	hal_data->CurrentChannelBW = bandwidth;
+	hal_data->nCur40MhzPrimeSC = offset;
+
+	if ((!adapt->bDriverStopped) && (!adapt->bSurpriseRemoved))
+		phy_set_bw_mode_callback(adapt);
+	else
+		hal_data->CurrentChannelBW = tmp_bw;
+}
+
+static void phy_sw_chnl_callback(struct adapter *adapt, u8 channel)
+{
+	u8 rf_path;
+	u32 param1, param2;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+
+	if (adapt->bNotifyChannelChange)
+		DBG_88E("[%s] ch = %d\n", __func__, channel);
+
+	phy_set_tx_power_level(adapt, channel);
+
+	param1 = RF_CHNLBW;
+	param2 = channel;
+	for (rf_path = 0; rf_path < hal_data->NumTotalRFPath; rf_path++) {
+		hal_data->RfRegChnlVal[rf_path] = (hal_data->RfRegChnlVal[rf_path] &
+						  0xfffffc00) | param2;
+		phy_set_rf_reg(adapt, (enum rf_radio_path)rf_path, param1,
+			       bRFRegOffsetMask, hal_data->RfRegChnlVal[rf_path]);
+	}
+}
+
+void phy_sw_chnl(struct adapter *adapt, u8 channel)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	u8 tmpchannel = hal_data->CurrentChannel;
+	bool  result = true;
+
+	if (hal_data->rf_chip == RF_PSEUDO_11N)
+		return;
+
+	if (channel == 0)
+		channel = 1;
+
+	hal_data->CurrentChannel = channel;
+
+	if ((!adapt->bDriverStopped) && (!adapt->bSurpriseRemoved)) {
+		phy_sw_chnl_callback(adapt, channel);
+
+		if (!result)
+			hal_data->CurrentChannel = tmpchannel;
+
+	} else {
+		hal_data->CurrentChannel = tmpchannel;
+	}
+}
+
+#define ODM_TXPWRTRACK_MAX_IDX_88E  6
+
+static u8 get_right_chnl_for_iqk(u8 chnl)
+{
+	u8 channel_all[ODM_TARGET_CHNL_NUM_2G_5G] = {
+		1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14,
+		36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64,
+		100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122,
+		124, 126, 128, 130, 132, 134, 136, 138, 140, 149, 151, 153,
+		155, 157, 159, 161, 163, 165
+	};
+	u8 place = chnl;
+
+	if (chnl > 14) {
+		for (place = 14; place < sizeof(channel_all); place++) {
+			if (channel_all[place] == chnl)
+				return place-13;
+		}
+	}
+	return 0;
+}
+
+void rtl88eu_dm_txpower_track_adjust(struct odm_dm_struct *dm_odm, u8 type,
+				     u8 *direction, u32 *out_write_val)
+{
+	u8 pwr_value = 0;
+	/*  Tx power tracking BB swing table. */
+	if (type == 0) { /* For OFDM adjust */
+		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			     ("BbSwingIdxOfdm = %d BbSwingFlagOfdm=%d\n",
+			     dm_odm->BbSwingIdxOfdm, dm_odm->BbSwingFlagOfdm));
+
+		if (dm_odm->BbSwingIdxOfdm <= dm_odm->BbSwingIdxOfdmBase) {
+			*direction = 1;
+			pwr_value = (dm_odm->BbSwingIdxOfdmBase -
+				     dm_odm->BbSwingIdxOfdm);
+		} else {
+			*direction = 2;
+			pwr_value = (dm_odm->BbSwingIdxOfdm -
+				     dm_odm->BbSwingIdxOfdmBase);
+		}
+
+	} else if (type == 1) { /* For CCK adjust. */
+		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			     ("dm_odm->BbSwingIdxCck = %d dm_odm->BbSwingIdxCckBase = %d\n",
+			     dm_odm->BbSwingIdxCck, dm_odm->BbSwingIdxCckBase));
+
+		if (dm_odm->BbSwingIdxCck <= dm_odm->BbSwingIdxCckBase) {
+			*direction = 1;
+			pwr_value = (dm_odm->BbSwingIdxCckBase -
+				     dm_odm->BbSwingIdxCck);
+		} else {
+			*direction = 2;
+			pwr_value = (dm_odm->BbSwingIdxCck -
+				     dm_odm->BbSwingIdxCckBase);
+		}
+
+	}
+
+	if (pwr_value >= ODM_TXPWRTRACK_MAX_IDX_88E && *direction == 1)
+		pwr_value = ODM_TXPWRTRACK_MAX_IDX_88E;
+
+	*out_write_val = pwr_value | (pwr_value<<8) | (pwr_value<<16) |
+			 (pwr_value<<24);
+}
+
+static void dm_txpwr_track_setpwr(struct odm_dm_struct *dm_odm)
+{
+	if (dm_odm->BbSwingFlagOfdm || dm_odm->BbSwingFlagCck) {
+		ODM_RT_TRACE(dm_odm, ODM_COMP_TX_PWR_TRACK, ODM_DBG_LOUD,
+			     ("dm_txpwr_track_setpwr CH=%d\n", *(dm_odm->pChannel)));
+		phy_set_tx_power_level(dm_odm->Adapter, *(dm_odm->pChannel));
+		dm_odm->BbSwingFlagOfdm = false;
+		dm_odm->BbSwingFlagCck = false;
+	}
+}
+
+void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	u8 thermal_val = 0, delta, delta_lck, delta_iqk, offset;
+	u8 thermal_avg_count = 0;
+	u32 thermal_avg = 0;
+	s32 ele_a = 0, ele_d, temp_cck, x, value32;
+	s32 y, ele_c = 0;
+	s8 ofdm_index[2], cck_index = 0;
+	s8 ofdm_index_old[2] = {0, 0}, cck_index_old = 0;
+	u32 i = 0, j = 0;
+	bool is2t = false;
+
+	u8 ofdm_min_index = 6, rf; /* OFDM BB Swing should be less than +3.0dB */
+	u8 indexforchannel = 0;
+	s8 ofdm_index_mapping[2][index_mapping_NUM_88E] = {
+		/* 2.4G, decrease power */
+		{0, 0, 2, 3, 4, 4, 5, 6, 7, 7, 8, 9, 10, 10, 11},
+		/* 2.4G, increase power */
+		{0, 0, -1, -2, -3, -4,-4, -4, -4, -5, -7, -8,-9, -9, -10},
+	};
+	u8 thermal_mapping[2][index_mapping_NUM_88E] = {
+		/* 2.4G, decrease power */
+		{0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 24, 26, 27},
+		/* 2.4G, increase power */
+		{0, 2, 4, 6, 8, 10, 12, 14, 16, 18, 20, 22, 25, 25, 25},
+	};
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+
+	dm_txpwr_track_setpwr(dm_odm);
+
+	dm_odm->RFCalibrateInfo.TXPowerTrackingCallbackCnt++;
+	dm_odm->RFCalibrateInfo.bTXPowerTrackingInit = true;
+
+	dm_odm->RFCalibrateInfo.RegA24 = 0x090e1317;
+
+	thermal_val = (u8)phy_query_rf_reg(adapt, RF_PATH_A,
+					   RF_T_METER_88E, 0xfc00);
+
+	if (is2t)
+		rf = 2;
+	else
+		rf = 1;
+
+	if (thermal_val) {
+		/* Query OFDM path A default setting */
+		ele_d = phy_query_bb_reg(adapt, rOFDM0_XATxIQImbalance, bMaskDWord)&bMaskOFDM_D;
+		for (i = 0; i < OFDM_TABLE_SIZE_92D; i++) {
+			if (ele_d == (OFDMSwingTable[i]&bMaskOFDM_D)) {
+				ofdm_index_old[0] = (u8)i;
+				dm_odm->BbSwingIdxOfdmBase = (u8)i;
+				break;
+			}
+		}
+
+		/* Query OFDM path B default setting */
+		if (is2t) {
+			ele_d = phy_query_bb_reg(adapt, rOFDM0_XBTxIQImbalance, bMaskDWord)&bMaskOFDM_D;
+			for (i = 0; i < OFDM_TABLE_SIZE_92D; i++) {
+				if (ele_d == (OFDMSwingTable[i]&bMaskOFDM_D)) {
+					ofdm_index_old[1] = (u8)i;
+					break;
+				}
+			}
+		}
+
+		/* Query CCK default setting From 0xa24 */
+		temp_cck = dm_odm->RFCalibrateInfo.RegA24;
+
+		for (i = 0; i < CCK_TABLE_SIZE; i++) {
+			if (dm_odm->RFCalibrateInfo.bCCKinCH14) {
+				if (memcmp(&temp_cck, &CCKSwingTable_Ch14[i][2], 4)) {
+					cck_index_old = (u8)i;
+					dm_odm->BbSwingIdxCckBase = (u8)i;
+					break;
+				}
+			} else {
+				if (memcmp(&temp_cck, &CCKSwingTable_Ch1_Ch13[i][2], 4)) {
+					cck_index_old = (u8)i;
+					dm_odm->BbSwingIdxCckBase = (u8)i;
+					break;
+				}
+			}
+		}
+
+		if (!dm_odm->RFCalibrateInfo.ThermalValue) {
+			dm_odm->RFCalibrateInfo.ThermalValue = hal_data->EEPROMThermalMeter;
+			dm_odm->RFCalibrateInfo.ThermalValue_LCK = thermal_val;
+			dm_odm->RFCalibrateInfo.ThermalValue_IQK = thermal_val;
+
+			for (i = 0; i < rf; i++)
+				dm_odm->RFCalibrateInfo.OFDM_index[i] = ofdm_index_old[i];
+			dm_odm->RFCalibrateInfo.CCK_index = cck_index_old;
+		}
+
+		/* calculate average thermal meter */
+		dm_odm->RFCalibrateInfo.ThermalValue_AVG[dm_odm->RFCalibrateInfo.ThermalValue_AVG_index] = thermal_val;
+		dm_odm->RFCalibrateInfo.ThermalValue_AVG_index++;
+		if (dm_odm->RFCalibrateInfo.ThermalValue_AVG_index == AVG_THERMAL_NUM_88E)
+			dm_odm->RFCalibrateInfo.ThermalValue_AVG_index = 0;
+
+		for (i = 0; i < AVG_THERMAL_NUM_88E; i++) {
+			if (dm_odm->RFCalibrateInfo.ThermalValue_AVG[i]) {
+				thermal_avg += dm_odm->RFCalibrateInfo.ThermalValue_AVG[i];
+				thermal_avg_count++;
+			}
+		}
+
+		if (thermal_avg_count)
+			thermal_val = (u8)(thermal_avg / thermal_avg_count);
+
+		if (dm_odm->RFCalibrateInfo.bReloadtxpowerindex) {
+			delta = thermal_val > hal_data->EEPROMThermalMeter ?
+				(thermal_val - hal_data->EEPROMThermalMeter) :
+				(hal_data->EEPROMThermalMeter - thermal_val);
+			dm_odm->RFCalibrateInfo.bReloadtxpowerindex = false;
+			dm_odm->RFCalibrateInfo.bDoneTxpower = false;
+		} else if (dm_odm->RFCalibrateInfo.bDoneTxpower) {
+			delta = (thermal_val > dm_odm->RFCalibrateInfo.ThermalValue) ?
+				(thermal_val - dm_odm->RFCalibrateInfo.ThermalValue) :
+				(dm_odm->RFCalibrateInfo.ThermalValue - thermal_val);
+		} else {
+			delta = thermal_val > hal_data->EEPROMThermalMeter ?
+				(thermal_val - hal_data->EEPROMThermalMeter) :
+				(hal_data->EEPROMThermalMeter - thermal_val);
+		}
+		delta_lck = (thermal_val > dm_odm->RFCalibrateInfo.ThermalValue_LCK) ?
+			    (thermal_val - dm_odm->RFCalibrateInfo.ThermalValue_LCK) :
+			    (dm_odm->RFCalibrateInfo.ThermalValue_LCK - thermal_val);
+		delta_iqk = (thermal_val > dm_odm->RFCalibrateInfo.ThermalValue_IQK) ?
+			    (thermal_val - dm_odm->RFCalibrateInfo.ThermalValue_IQK) :
+			    (dm_odm->RFCalibrateInfo.ThermalValue_IQK - thermal_val);
+
+		/* Delta temperature is equal to or larger than 20 centigrade.*/
+		if ((delta_lck >= 8)) {
+			dm_odm->RFCalibrateInfo.ThermalValue_LCK = thermal_val;
+			rtl88eu_phy_lc_calibrate(adapt);
+		}
+
+		if (delta > 0 && dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
+			delta = thermal_val > hal_data->EEPROMThermalMeter ?
+				(thermal_val - hal_data->EEPROMThermalMeter) :
+				(hal_data->EEPROMThermalMeter - thermal_val);
+			/* calculate new OFDM / CCK offset */
+			if (thermal_val > hal_data->EEPROMThermalMeter)
+				j = 1;
+			else
+				j = 0;
+			for (offset = 0; offset < index_mapping_NUM_88E; offset++) {
+				if (delta < thermal_mapping[j][offset]) {
+					if (offset != 0)
+						offset--;
+					break;
+				}
+			}
+			if (offset >= index_mapping_NUM_88E)
+				offset = index_mapping_NUM_88E-1;
+			for (i = 0; i < rf; i++)
+				ofdm_index[i] = dm_odm->RFCalibrateInfo.OFDM_index[i] + ofdm_index_mapping[j][offset];
+			cck_index = dm_odm->RFCalibrateInfo.CCK_index + ofdm_index_mapping[j][offset];
+
+			for (i = 0; i < rf; i++) {
+				if (ofdm_index[i] > OFDM_TABLE_SIZE_92D-1)
+					ofdm_index[i] = OFDM_TABLE_SIZE_92D-1;
+				else if (ofdm_index[i] < ofdm_min_index)
+					ofdm_index[i] = ofdm_min_index;
+			}
+
+			if (cck_index > CCK_TABLE_SIZE-1)
+				cck_index = CCK_TABLE_SIZE-1;
+			else if (cck_index < 0)
+				cck_index = 0;
+
+			/* 2 temporarily remove bNOPG */
+			/* Config by SwingTable */
+			if (dm_odm->RFCalibrateInfo.TxPowerTrackControl) {
+				dm_odm->RFCalibrateInfo.bDoneTxpower = true;
+
+				/* Adujst OFDM Ant_A according to IQK result */
+				ele_d = (OFDMSwingTable[(u8)ofdm_index[0]] & 0xFFC00000)>>22;
+				x = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][0];
+				y = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][1];
+
+				/*  Revse TX power table. */
+				dm_odm->BbSwingIdxOfdm = (u8)ofdm_index[0];
+				dm_odm->BbSwingIdxCck = (u8)cck_index;
+
+				if (dm_odm->BbSwingIdxOfdmCurrent != dm_odm->BbSwingIdxOfdm) {
+					dm_odm->BbSwingIdxOfdmCurrent = dm_odm->BbSwingIdxOfdm;
+					dm_odm->BbSwingFlagOfdm = true;
+				}
+
+				if (dm_odm->BbSwingIdxCckCurrent != dm_odm->BbSwingIdxCck) {
+					dm_odm->BbSwingIdxCckCurrent = dm_odm->BbSwingIdxCck;
+					dm_odm->BbSwingFlagCck = true;
+				}
+
+				if (x != 0) {
+					if ((x & 0x00000200) != 0)
+						x = x | 0xFFFFFC00;
+					ele_a = ((x * ele_d)>>8)&0x000003FF;
+
+					/* new element C = element D x Y */
+					if ((y & 0x00000200) != 0)
+						y = y | 0xFFFFFC00;
+					ele_c = ((y * ele_d)>>8)&0x000003FF;
+
+				}
+
+				if (is2t) {
+					ele_d = (OFDMSwingTable[(u8)ofdm_index[1]] & 0xFFC00000)>>22;
+
+					/* new element A = element D x X */
+					x = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][4];
+					y = dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[indexforchannel].Value[0][5];
+
+					if ((x != 0) && (*(dm_odm->pBandType) == ODM_BAND_2_4G)) {
+						if ((x & 0x00000200) != 0)	/* consider minus */
+							x = x | 0xFFFFFC00;
+						ele_a = ((x * ele_d)>>8)&0x000003FF;
+
+						/* new element C = element D x Y */
+						if ((y & 0x00000200) != 0)
+							y = y | 0xFFFFFC00;
+						ele_c = ((y * ele_d)>>8)&0x00003FF;
+
+						/* wtite new elements A, C, D to regC88 and regC9C, element B is always 0 */
+						value32 = (ele_d<<22) | ((ele_c&0x3F)<<16) | ele_a;
+						phy_set_bb_reg(adapt, rOFDM0_XBTxIQImbalance, bMaskDWord, value32);
+
+						value32 = (ele_c&0x000003C0)>>6;
+						phy_set_bb_reg(adapt, rOFDM0_XDTxAFE, bMaskH4Bits, value32);
+
+						value32 = ((x * ele_d)>>7)&0x01;
+						phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT28, value32);
+					} else {
+						phy_set_bb_reg(adapt, rOFDM0_XBTxIQImbalance, bMaskDWord, OFDMSwingTable[(u8)ofdm_index[1]]);
+						phy_set_bb_reg(adapt, rOFDM0_XDTxAFE, bMaskH4Bits, 0x00);
+						phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT28, 0x00);
+					}
+
+				}
+
+			}
+		}
+
+		/* Delta temperature is equal to or larger than 20 centigrade.*/
+		if (delta_iqk >= 8) {
+			dm_odm->RFCalibrateInfo.ThermalValue_IQK = thermal_val;
+			rtl88eu_phy_iq_calibrate(adapt, false);
+		}
+		/* update thermal meter value */
+		if (dm_odm->RFCalibrateInfo.TxPowerTrackControl)
+			dm_odm->RFCalibrateInfo.ThermalValue = thermal_val;
+	}
+	dm_odm->RFCalibrateInfo.TXPowercount = 0;
+}
+
+#define MAX_TOLERANCE 5
+
+static u8 phy_path_a_iqk(struct adapter *adapt, bool config_pathb)
+{
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4;
+	u8 result = 0x00;
+
+	/* 1 Tx IQK */
+	/* path-A IQK setting */
+	phy_set_bb_reg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x10008c1c);
+	phy_set_bb_reg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x30008c1c);
+	phy_set_bb_reg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x8214032a);
+	phy_set_bb_reg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160000);
+
+	/* LO calibration setting */
+	phy_set_bb_reg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x00462911);
+
+	/* One shot, path A LOK & IQK */
+	phy_set_bb_reg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	phy_set_bb_reg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME_88E);
+
+	reg_eac = phy_query_bb_reg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
+	reg_e94 = phy_query_bb_reg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
+	reg_e9c = phy_query_bb_reg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
+	reg_ea4 = phy_query_bb_reg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord);
+
+	if (!(reg_eac & BIT28) &&
+	    (((reg_e94 & 0x03FF0000)>>16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000)>>16) != 0x42))
+		result |= 0x01;
+	return result;
+}
+
+static u8 phy_path_a_rx_iqk(struct adapter *adapt, bool configPathB)
+{
+	u32 reg_eac, reg_e94, reg_e9c, reg_ea4, u4tmp;
+	u8 result = 0x00;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+
+	/* 1 Get TXIMR setting */
+	/* modify RXIQK mode table */
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_WE_LUT, bRFRegOffsetMask, 0x800a0);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0000f);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf117B);
+
+	/* PA,PAD off */
+	phy_set_rf_reg(adapt, RF_PATH_A, 0xdf, bRFRegOffsetMask, 0x980);
+	phy_set_rf_reg(adapt, RF_PATH_A, 0x56, bRFRegOffsetMask, 0x51000);
+
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
+
+	/* IQK setting */
+	phy_set_bb_reg(adapt, rTx_IQK, bMaskDWord, 0x01007c00);
+	phy_set_bb_reg(adapt, rRx_IQK, bMaskDWord, 0x81004800);
+
+	/* path-A IQK setting */
+	phy_set_bb_reg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x10008c1c);
+	phy_set_bb_reg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x30008c1c);
+	phy_set_bb_reg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x82160c1f);
+	phy_set_bb_reg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160000);
+
+	/* LO calibration setting */
+	phy_set_bb_reg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+	/* One shot, path A LOK & IQK */
+	phy_set_bb_reg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	phy_set_bb_reg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	/* delay x ms */
+	mdelay(IQK_DELAY_TIME_88E);
+
+	/* Check failed */
+	reg_eac = phy_query_bb_reg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
+	reg_e94 = phy_query_bb_reg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
+	reg_e9c = phy_query_bb_reg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
+
+	if (!(reg_eac & BIT28) &&
+	    (((reg_e94 & 0x03FF0000)>>16) != 0x142) &&
+	    (((reg_e9c & 0x03FF0000)>>16) != 0x42))
+		result |= 0x01;
+	else					/* if Tx not OK, ignore Rx */
+		return result;
+
+	u4tmp = 0x80007C00 | (reg_e94&0x3FF0000)  | ((reg_e9c&0x3FF0000) >> 16);
+	phy_set_bb_reg(adapt, rTx_IQK, bMaskDWord, u4tmp);
+
+	/* 1 RX IQK */
+	/* modify RXIQK mode table */
+	ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
+		     ("Path-A Rx IQK modify RXIQK mode table 2!\n"));
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_WE_LUT, bRFRegOffsetMask, 0x800a0);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_RCK_OS, bRFRegOffsetMask, 0x30000);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_TXPA_G1, bRFRegOffsetMask, 0x0000f);
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_TXPA_G2, bRFRegOffsetMask, 0xf7ffa);
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
+
+	/* IQK setting */
+	phy_set_bb_reg(adapt, rRx_IQK, bMaskDWord, 0x01004800);
+
+	/* path-A IQK setting */
+	phy_set_bb_reg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x38008c1c);
+	phy_set_bb_reg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x18008c1c);
+	phy_set_bb_reg(adapt, rTx_IQK_PI_A, bMaskDWord, 0x82160c05);
+	phy_set_bb_reg(adapt, rRx_IQK_PI_A, bMaskDWord, 0x28160c1f);
+
+	/* LO calibration setting */
+	phy_set_bb_reg(adapt, rIQK_AGC_Rsp, bMaskDWord, 0x0046a911);
+
+	phy_set_bb_reg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf9000000);
+	phy_set_bb_reg(adapt, rIQK_AGC_Pts, bMaskDWord, 0xf8000000);
+
+	mdelay(IQK_DELAY_TIME_88E);
+
+	/*  Check failed */
+	reg_eac = phy_query_bb_reg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
+	reg_e94 = phy_query_bb_reg(adapt, rTx_Power_Before_IQK_A, bMaskDWord);
+	reg_e9c = phy_query_bb_reg(adapt, rTx_Power_After_IQK_A, bMaskDWord);
+	reg_ea4 = phy_query_bb_reg(adapt, rRx_Power_Before_IQK_A_2, bMaskDWord);
+
+	/* reload RF 0xdf */
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x00000000);
+	phy_set_rf_reg(adapt, RF_PATH_A, 0xdf, bRFRegOffsetMask, 0x180);
+
+	if (!(reg_eac & BIT27) && /* if Tx is OK, check whether Rx is OK */
+	    (((reg_ea4 & 0x03FF0000)>>16) != 0x132) &&
+	    (((reg_eac & 0x03FF0000)>>16) != 0x36))
+		result |= 0x02;
+	else
+		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
+			     ("Path A Rx IQK fail!!\n"));
+
+	return result;
+}
+
+static u8 phy_path_b_iqk(struct adapter *adapt)
+{
+	u32 regeac, regeb4, regebc, regec4, regecc;
+	u8 result = 0x00;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+
+	/* One shot, path B LOK & IQK */
+	phy_set_bb_reg(adapt, rIQK_AGC_Cont, bMaskDWord, 0x00000002);
+	phy_set_bb_reg(adapt, rIQK_AGC_Cont, bMaskDWord, 0x00000000);
+
+	mdelay(IQK_DELAY_TIME_88E);
+
+	regeac = phy_query_bb_reg(adapt, rRx_Power_After_IQK_A_2, bMaskDWord);
+	regeb4 = phy_query_bb_reg(adapt, rTx_Power_Before_IQK_B, bMaskDWord);
+	regebc = phy_query_bb_reg(adapt, rTx_Power_After_IQK_B, bMaskDWord);
+	regec4 = phy_query_bb_reg(adapt, rRx_Power_Before_IQK_B_2, bMaskDWord);
+	regecc = phy_query_bb_reg(adapt, rRx_Power_After_IQK_B_2, bMaskDWord);
+
+	if (!(regeac & BIT31) &&
+	    (((regeb4 & 0x03FF0000)>>16) != 0x142) &&
+	    (((regebc & 0x03FF0000)>>16) != 0x42))
+		result |= 0x01;
+	else
+		return result;
+
+	if (!(regeac & BIT30) &&
+	    (((regec4 & 0x03FF0000)>>16) != 0x132) &&
+	    (((regecc & 0x03FF0000)>>16) != 0x36))
+		result |= 0x02;
+	else
+		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION,
+			     ODM_DBG_LOUD,  ("Path B Rx IQK fail!!\n"));
+	return result;
+}
+
+static void patha_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8],
+			   u8 final_candidate, bool txonly)
+{
+	u32 oldval_0, x, tx0_a, reg;
+	s32 y, tx0_c;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (iqkok) {
+		oldval_0 = (phy_query_bb_reg(adapt, rOFDM0_XATxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+
+		x = result[final_candidate][0];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+
+		tx0_a = (x * oldval_0) >> 8;
+		phy_set_bb_reg(adapt, rOFDM0_XATxIQImbalance, 0x3FF, tx0_a);
+		phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT(31),
+			       ((x * oldval_0>>7) & 0x1));
+
+		y = result[final_candidate][1];
+		if ((y & 0x00000200) != 0)
+			y = y | 0xFFFFFC00;
+
+		tx0_c = (y * oldval_0) >> 8;
+		phy_set_bb_reg(adapt, rOFDM0_XCTxAFE, 0xF0000000,
+			       ((tx0_c&0x3C0)>>6));
+		phy_set_bb_reg(adapt, rOFDM0_XATxIQImbalance, 0x003F0000,
+			       (tx0_c&0x3F));
+		phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT(29),
+			       ((y * oldval_0>>7) & 0x1));
+
+		if (txonly)
+			return;
+
+		reg = result[final_candidate][2];
+		phy_set_bb_reg(adapt, rOFDM0_XARxIQImbalance, 0x3FF, reg);
+
+		reg = result[final_candidate][3] & 0x3F;
+		phy_set_bb_reg(adapt, rOFDM0_XARxIQImbalance, 0xFC00, reg);
+
+		reg = (result[final_candidate][3] >> 6) & 0xF;
+		phy_set_bb_reg(adapt, rOFDM0_RxIQExtAnta, 0xF0000000, reg);
+	}
+}
+
+static void pathb_fill_iqk(struct adapter *adapt, bool iqkok, s32 result[][8],
+			   u8 final_candidate, bool txonly)
+{
+	u32 oldval_1, x, tx1_a, reg;
+	s32 y, tx1_c;
+
+	if (final_candidate == 0xFF) {
+		return;
+	} else if (iqkok) {
+		oldval_1 = (phy_query_bb_reg(adapt, rOFDM0_XBTxIQImbalance, bMaskDWord) >> 22) & 0x3FF;
+
+		x = result[final_candidate][4];
+		if ((x & 0x00000200) != 0)
+			x = x | 0xFFFFFC00;
+		tx1_a = (x * oldval_1) >> 8;
+		phy_set_bb_reg(adapt, rOFDM0_XBTxIQImbalance, 0x3FF, tx1_a);
+
+		phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT(27),
+			       ((x * oldval_1>>7) & 0x1));
+
+		y = result[final_candidate][5];
+		if ((y & 0x00000200) != 0)
+			y = y | 0xFFFFFC00;
+
+		tx1_c = (y * oldval_1) >> 8;
+
+		phy_set_bb_reg(adapt, rOFDM0_XDTxAFE, 0xF0000000,
+			       ((tx1_c&0x3C0)>>6));
+		phy_set_bb_reg(adapt, rOFDM0_XBTxIQImbalance, 0x003F0000,
+			       (tx1_c&0x3F));
+		phy_set_bb_reg(adapt, rOFDM0_ECCAThreshold, BIT(25),
+			       ((y * oldval_1>>7) & 0x1));
+
+		if (txonly)
+			return;
+
+		reg = result[final_candidate][6];
+		phy_set_bb_reg(adapt, rOFDM0_XBRxIQImbalance, 0x3FF, reg);
+
+		reg = result[final_candidate][7] & 0x3F;
+		phy_set_bb_reg(adapt, rOFDM0_XBRxIQImbalance, 0xFC00, reg);
+
+		reg = (result[final_candidate][7] >> 6) & 0xF;
+		phy_set_bb_reg(adapt, rOFDM0_AGCRSSITable, 0x0000F000, reg);
+	}
+}
+
+static void save_adda_registers(struct adapter *adapt, u32 *addareg,
+				u32 *backup, u32 register_num)
+{
+	u32 i;
+
+	for (i = 0; i < register_num; i++) {
+		backup[i] = phy_query_bb_reg(adapt, addareg[i], bMaskDWord);
+	}
+}
+
+static void save_mac_registers(struct adapter *adapt, u32 *mac_reg,
+			       u32 *backup)
+{
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++) {
+		backup[i] = usb_read8(adapt, mac_reg[i]);
+	}
+	backup[i] = usb_read32(adapt, mac_reg[i]);
+}
+
+static void reload_adda_reg(struct adapter *adapt, u32 *adda_reg,
+			    u32 *backup, u32 regiester_num)
+{
+	u32 i;
+
+	for (i = 0; i < regiester_num; i++)
+		phy_set_bb_reg(adapt, adda_reg[i], bMaskDWord, backup[i]);
+}
+
+static void reload_mac_registers(struct adapter *adapt,
+				 u32 *mac_reg, u32 *backup)
+{
+	u32 i;
+
+	for (i = 0; i < (IQK_MAC_REG_NUM - 1); i++) {
+		usb_write8(adapt, mac_reg[i], (u8)backup[i]);
+	}
+	usb_write32(adapt, mac_reg[i], backup[i]);
+}
+
+static void path_adda_on(struct adapter *adapt, u32 *adda_reg,
+			 bool is_path_a_on, bool is2t)
+{
+	u32 path_on;
+	u32 i;
+
+	path_on = is_path_a_on ? 0x04db25a4 : 0x0b1b25a4;
+	if (!is2t) {
+		path_on = 0x0bdb25a0;
+		phy_set_bb_reg(adapt, adda_reg[0], bMaskDWord, 0x0b1b25a0);
+	} else {
+		phy_set_bb_reg(adapt, adda_reg[0], bMaskDWord, path_on);
+	}
+
+	for (i = 1; i < IQK_ADDA_REG_NUM; i++)
+		phy_set_bb_reg(adapt, adda_reg[i], bMaskDWord, path_on);
+}
+
+static void mac_setting_calibration(struct adapter *adapt, u32 *mac_reg, u32 *backup)
+{
+	u32 i = 0;
+
+	usb_write8(adapt, mac_reg[i], 0x3F);
+
+	for (i = 1; i < (IQK_MAC_REG_NUM - 1); i++) {
+		usb_write8(adapt, mac_reg[i], (u8)(backup[i]&(~BIT3)));
+	}
+	usb_write8(adapt, mac_reg[i], (u8)(backup[i]&(~BIT5)));
+}
+
+static void path_a_standby(struct adapter *adapt)
+{
+
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x0);
+	phy_set_bb_reg(adapt, 0x840, bMaskDWord, 0x00010000);
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
+}
+
+static void pi_mode_switch(struct adapter *adapt, bool pi_mode)
+{
+	u32 mode;
+
+	mode = pi_mode ? 0x01000100 : 0x01000000;
+	phy_set_bb_reg(adapt, rFPGA0_XA_HSSIParameter1, bMaskDWord, mode);
+	phy_set_bb_reg(adapt, rFPGA0_XB_HSSIParameter1, bMaskDWord, mode);
+}
+
+static bool simularity_compare(struct adapter *adapt, s32 resulta[][8],
+			       u8 c1, u8 c2)
+{
+	u32 i, j, diff, sim_bitmap, bound = 0;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+	u8 final_candidate[2] = {0xFF, 0xFF};	/* for path A and path B */
+	bool result = true;
+	bool is2t;
+	s32 tmp1 = 0, tmp2 = 0;
+
+	if ((dm_odm->RFType == ODM_2T2R) || (dm_odm->RFType == ODM_2T3R) ||
+	    (dm_odm->RFType == ODM_2T4R))
+		is2t = true;
+	else
+		is2t = false;
+
+	if (is2t)
+		bound = 8;
+	else
+		bound = 4;
+
+	sim_bitmap = 0;
+
+	for (i = 0; i < bound; i++) {
+		if ((i == 1) || (i == 3) || (i == 5) || (i == 7)) {
+			if ((resulta[c1][i] & 0x00000200) != 0)
+				tmp1 = resulta[c1][i] | 0xFFFFFC00;
+			else
+				tmp1 = resulta[c1][i];
+
+			if ((resulta[c2][i] & 0x00000200) != 0)
+				tmp2 = resulta[c2][i] | 0xFFFFFC00;
+			else
+				tmp2 = resulta[c2][i];
+		} else {
+			tmp1 = resulta[c1][i];
+			tmp2 = resulta[c2][i];
+		}
+
+		diff = (tmp1 > tmp2) ? (tmp1 - tmp2) : (tmp2 - tmp1);
+
+		if (diff > MAX_TOLERANCE) {
+			if ((i == 2 || i == 6) && !sim_bitmap) {
+				if (resulta[c1][i] + resulta[c1][i+1] == 0)
+					final_candidate[(i/4)] = c2;
+				else if (resulta[c2][i] + resulta[c2][i+1] == 0)
+					final_candidate[(i/4)] = c1;
+				else
+					sim_bitmap = sim_bitmap | (1<<i);
+			} else {
+				sim_bitmap = sim_bitmap | (1<<i);
+			}
+		}
+	}
+
+	if (sim_bitmap == 0) {
+		for (i = 0; i < (bound/4); i++) {
+			if (final_candidate[i] != 0xFF) {
+				for (j = i*4; j < (i+1)*4-2; j++)
+					resulta[3][j] = resulta[final_candidate[i]][j];
+				result = false;
+			}
+		}
+		return result;
+	} else {
+		if (!(sim_bitmap & 0x03)) {		   /* path A TX OK */
+			for (i = 0; i < 2; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+		if (!(sim_bitmap & 0x0c)) {		   /* path A RX OK */
+			for (i = 2; i < 4; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+
+		if (!(sim_bitmap & 0x30)) { /* path B TX OK */
+			for (i = 4; i < 6; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+
+		if (!(sim_bitmap & 0xc0)) { /* path B RX OK */
+			for (i = 6; i < 8; i++)
+				resulta[3][i] = resulta[c1][i];
+		}
+		return false;
+	}
+}
+
+static void phy_iq_calibrate(struct adapter *adapt, s32 result[][8],
+			     u8 t, bool is2t)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+	u32 i;
+	u8 path_a_ok, path_b_ok;
+	u32 adda_reg[IQK_ADDA_REG_NUM] = {
+					  rFPGA0_XCD_SwitchControl, rBlue_Tooth,
+					  rRx_Wait_CCA, rTx_CCK_RFON,
+					  rTx_CCK_BBON, rTx_OFDM_RFON,
+					  rTx_OFDM_BBON, rTx_To_Rx,
+					  rTx_To_Tx, rRx_CCK,
+					  rRx_OFDM, rRx_Wait_RIFS,
+					  rRx_TO_Rx, rStandby,
+					  rSleep, rPMPD_ANAEN};
+
+	u32 iqk_mac_reg[IQK_MAC_REG_NUM] = {
+					    REG_TXPAUSE, REG_BCN_CTRL,
+					    REG_BCN_CTRL_1, REG_GPIO_MUXCFG};
+
+	/* since 92C & 92D have the different define in IQK_BB_REG */
+	u32 iqk_bb_reg_92c[IQK_BB_REG_NUM] = {
+					      rOFDM0_TRxPathEnable, rOFDM0_TRMuxPar,
+					      rFPGA0_XCD_RFInterfaceSW, rConfig_AntA, rConfig_AntB,
+					      rFPGA0_XAB_RFInterfaceSW, rFPGA0_XA_RFInterfaceOE,
+					      rFPGA0_XB_RFInterfaceOE, rFPGA0_RFMOD};
+
+	u32 retry_count = 9;
+	if (*(dm_odm->mp_mode) == 1)
+		retry_count = 9;
+	else
+		retry_count = 2;
+
+	if (t == 0) {
+
+		/*  Save ADDA parameters, turn Path A ADDA on */
+		save_adda_registers(adapt, adda_reg, dm_odm->RFCalibrateInfo.ADDA_backup,
+				    IQK_ADDA_REG_NUM);
+		save_mac_registers(adapt, iqk_mac_reg,
+				   dm_odm->RFCalibrateInfo.IQK_MAC_backup);
+		save_adda_registers(adapt, iqk_bb_reg_92c,
+				    dm_odm->RFCalibrateInfo.IQK_BB_backup, IQK_BB_REG_NUM);
+	}
+
+	path_adda_on(adapt, adda_reg, true, is2t);
+	if (t == 0)
+		dm_odm->RFCalibrateInfo.bRfPiEnable = (u8)phy_query_bb_reg(adapt, rFPGA0_XA_HSSIParameter1,
+									   BIT(8));
+
+	if (!dm_odm->RFCalibrateInfo.bRfPiEnable) {
+		/*  Switch BB to PI mode to do IQ Calibration. */
+		pi_mode_switch(adapt, true);
+	}
+
+	/* BB setting */
+	phy_set_bb_reg(adapt, rFPGA0_RFMOD, BIT24, 0x00);
+	phy_set_bb_reg(adapt, rOFDM0_TRxPathEnable, bMaskDWord, 0x03a05600);
+	phy_set_bb_reg(adapt, rOFDM0_TRMuxPar, bMaskDWord, 0x000800e4);
+	phy_set_bb_reg(adapt, rFPGA0_XCD_RFInterfaceSW, bMaskDWord, 0x22204000);
+
+	phy_set_bb_reg(adapt, rFPGA0_XAB_RFInterfaceSW, BIT10, 0x01);
+	phy_set_bb_reg(adapt, rFPGA0_XAB_RFInterfaceSW, BIT26, 0x01);
+	phy_set_bb_reg(adapt, rFPGA0_XA_RFInterfaceOE, BIT10, 0x00);
+	phy_set_bb_reg(adapt, rFPGA0_XB_RFInterfaceOE, BIT10, 0x00);
+
+	if (is2t) {
+		phy_set_bb_reg(adapt, rFPGA0_XA_LSSIParameter, bMaskDWord,
+			       0x00010000);
+		phy_set_bb_reg(adapt, rFPGA0_XB_LSSIParameter, bMaskDWord,
+			       0x00010000);
+	}
+
+	/* MAC settings */
+	mac_setting_calibration(adapt, iqk_mac_reg,
+				dm_odm->RFCalibrateInfo.IQK_MAC_backup);
+
+	/* Page B init */
+	/* AP or IQK */
+	phy_set_bb_reg(adapt, rConfig_AntA, bMaskDWord, 0x0f600000);
+
+	if (is2t)
+		phy_set_bb_reg(adapt, rConfig_AntB, bMaskDWord, 0x0f600000);
+
+	/*  IQ calibration setting */
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0x80800000);
+	phy_set_bb_reg(adapt, rTx_IQK, bMaskDWord, 0x01007c00);
+	phy_set_bb_reg(adapt, rRx_IQK, bMaskDWord, 0x81004800);
+
+	for (i = 0; i < retry_count; i++) {
+		path_a_ok = phy_path_a_iqk(adapt, is2t);
+		if (path_a_ok == 0x01) {
+				result[t][0] = (phy_query_bb_reg(adapt, rTx_Power_Before_IQK_A,
+								 bMaskDWord)&0x3FF0000)>>16;
+				result[t][1] = (phy_query_bb_reg(adapt, rTx_Power_After_IQK_A,
+								 bMaskDWord)&0x3FF0000)>>16;
+			break;
+		}
+	}
+
+	for (i = 0; i < retry_count; i++) {
+		path_a_ok = phy_path_a_rx_iqk(adapt, is2t);
+		if (path_a_ok == 0x03) {
+				result[t][2] = (phy_query_bb_reg(adapt, rRx_Power_Before_IQK_A_2,
+								 bMaskDWord)&0x3FF0000)>>16;
+				result[t][3] = (phy_query_bb_reg(adapt, rRx_Power_After_IQK_A_2,
+								 bMaskDWord)&0x3FF0000)>>16;
+			break;
+		} else {
+			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
+				     ("Path A Rx IQK Fail!!\n"));
+		}
+	}
+
+	if (0x00 == path_a_ok) {
+		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
+			     ("Path A IQK failed!!\n"));
+	}
+
+	if (is2t) {
+		path_a_standby(adapt);
+
+		/*  Turn Path B ADDA on */
+		path_adda_on(adapt, adda_reg, false, is2t);
+
+		for (i = 0; i < retry_count; i++) {
+			path_b_ok = phy_path_b_iqk(adapt);
+			if (path_b_ok == 0x03) {
+				result[t][4] = (phy_query_bb_reg(adapt, rTx_Power_Before_IQK_B,
+								 bMaskDWord)&0x3FF0000)>>16;
+				result[t][5] = (phy_query_bb_reg(adapt, rTx_Power_After_IQK_B,
+								 bMaskDWord)&0x3FF0000)>>16;
+				result[t][6] = (phy_query_bb_reg(adapt, rRx_Power_Before_IQK_B_2,
+								 bMaskDWord)&0x3FF0000)>>16;
+				result[t][7] = (phy_query_bb_reg(adapt, rRx_Power_After_IQK_B_2,
+								 bMaskDWord)&0x3FF0000)>>16;
+				break;
+			} else if (i == (retry_count - 1) && path_b_ok == 0x01) {	/* Tx IQK OK */
+				result[t][4] = (phy_query_bb_reg(adapt, rTx_Power_Before_IQK_B,
+								 bMaskDWord)&0x3FF0000)>>16;
+				result[t][5] = (phy_query_bb_reg(adapt, rTx_Power_After_IQK_B,
+								 bMaskDWord)&0x3FF0000)>>16;
+			}
+		}
+
+		if (0x00 == path_b_ok) {
+			ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
+				     ("Path B IQK failed!!\n"));
+		}
+	}
+
+	/* Back to BB mode, load original value */
+	phy_set_bb_reg(adapt, rFPGA0_IQK, bMaskDWord, 0);
+
+	if (t != 0) {
+		if (!dm_odm->RFCalibrateInfo.bRfPiEnable) {
+			/* Switch back BB to SI mode after
+			 * finish IQ Calibration.
+			 */
+			pi_mode_switch(adapt, false);
+		}
+
+		/*  Reload ADDA power saving parameters */
+		reload_adda_reg(adapt, adda_reg, dm_odm->RFCalibrateInfo.ADDA_backup,
+				IQK_ADDA_REG_NUM);
+
+		/*  Reload MAC parameters */
+		reload_mac_registers(adapt, iqk_mac_reg,
+				     dm_odm->RFCalibrateInfo.IQK_MAC_backup);
+
+		reload_adda_reg(adapt, iqk_bb_reg_92c, dm_odm->RFCalibrateInfo.IQK_BB_backup,
+				IQK_BB_REG_NUM);
+
+		/*  Restore RX initial gain */
+		phy_set_bb_reg(adapt, rFPGA0_XA_LSSIParameter,
+			       bMaskDWord, 0x00032ed3);
+		if (is2t)
+			phy_set_bb_reg(adapt, rFPGA0_XB_LSSIParameter,
+				       bMaskDWord, 0x00032ed3);
+
+		/* load 0xe30 IQC default value */
+		phy_set_bb_reg(adapt, rTx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+		phy_set_bb_reg(adapt, rRx_IQK_Tone_A, bMaskDWord, 0x01008c00);
+	}
+}
+
+static void phy_lc_calibrate(struct adapter *adapt, bool is2t)
+{
+	u8 tmpreg;
+	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
+
+	/* Check continuous TX and Packet TX */
+	tmpreg = usb_read8(adapt, 0xd03);
+
+	if ((tmpreg&0x70) != 0)
+		usb_write8(adapt, 0xd03, tmpreg&0x8F);
+	else
+		usb_write8(adapt, REG_TXPAUSE, 0xFF);
+
+	if ((tmpreg&0x70) != 0) {
+		/* 1. Read original RF mode */
+		/* Path-A */
+		rf_a_mode = phy_query_rf_reg(adapt, RF_PATH_A, RF_AC,
+					     bMask12Bits);
+
+		/* Path-B */
+		if (is2t)
+			rf_b_mode = phy_query_rf_reg(adapt, RF_PATH_B, RF_AC,
+						     bMask12Bits);
+
+		/* 2. Set RF mode = standby mode */
+		/* Path-A */
+		phy_set_rf_reg(adapt, RF_PATH_A, RF_AC, bMask12Bits,
+			       (rf_a_mode&0x8FFFF)|0x10000);
+
+		/* Path-B */
+		if (is2t)
+			phy_set_rf_reg(adapt, RF_PATH_B, RF_AC, bMask12Bits,
+				       (rf_b_mode&0x8FFFF)|0x10000);
+	}
+
+	/* 3. Read RF reg18 */
+	lc_cal = phy_query_rf_reg(adapt, RF_PATH_A, RF_CHNLBW, bMask12Bits);
+
+	/* 4. Set LC calibration begin bit15 */
+	phy_set_rf_reg(adapt, RF_PATH_A, RF_CHNLBW, bMask12Bits,
+		       lc_cal|0x08000);
+
+	msleep(100);
+
+	/* Restore original situation */
+	if ((tmpreg&0x70) != 0) {
+		/* Deal with continuous TX case */
+		/* Path-A */
+		usb_write8(adapt, 0xd03, tmpreg);
+		phy_set_rf_reg(adapt, RF_PATH_A, RF_AC, bMask12Bits, rf_a_mode);
+
+		/* Path-B */
+		if (is2t)
+			phy_set_rf_reg(adapt, RF_PATH_B, RF_AC, bMask12Bits,
+				       rf_b_mode);
+	} else {
+		/* Deal with Packet TX case */
+		usb_write8(adapt, REG_TXPAUSE, 0x00);
+	}
+}
+
+void rtl88eu_phy_iq_calibrate(struct adapter *adapt, bool recovery)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+	s32 result[4][8];
+	u8 i, final, chn_index;
+	bool pathaok, pathbok;
+	s32 reg_e94, reg_e9c, reg_ea4, reg_eac, reg_eb4, reg_ebc, reg_ec4,
+	    reg_ecc;
+	bool is12simular, is13simular, is23simular;
+	bool singletone = false, carrier_sup = false;
+	u32 iqk_bb_reg_92c[IQK_BB_REG_NUM] = {
+		rOFDM0_XARxIQImbalance, rOFDM0_XBRxIQImbalance,
+		rOFDM0_ECCAThreshold, rOFDM0_AGCRSSITable,
+		rOFDM0_XATxIQImbalance, rOFDM0_XBTxIQImbalance,
+		rOFDM0_XCTxAFE, rOFDM0_XDTxAFE,
+		rOFDM0_RxIQExtAnta};
+	bool is2t;
+
+	is2t = (dm_odm->RFType == ODM_2T2R) ? true : false;
+
+	if (!(dm_odm->SupportAbility & ODM_RF_CALIBRATION))
+		return;
+
+	if (singletone || carrier_sup)
+		return;
+
+	if (recovery) {
+		ODM_RT_TRACE(dm_odm, ODM_COMP_INIT, ODM_DBG_LOUD,
+			     ("phy_iq_calibrate: Return due to recovery!\n"));
+		reload_adda_reg(adapt, iqk_bb_reg_92c,
+				dm_odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+		return;
+	}
+
+	for (i = 0; i < 8; i++) {
+		result[0][i] = 0;
+		result[1][i] = 0;
+		result[2][i] = 0;
+		if ((i == 0) || (i == 2) || (i == 4)  || (i == 6))
+			result[3][i] = 0x100;
+		else
+			result[3][i] = 0;
+	}
+	final = 0xff;
+	pathaok = false;
+	pathbok = false;
+	is12simular = false;
+	is23simular = false;
+	is13simular = false;
+
+	for (i = 0; i < 3; i++) {
+		phy_iq_calibrate(adapt, result, i, is2t);
+
+		if (i == 1) {
+			is12simular = simularity_compare(adapt, result, 0, 1);
+			if (is12simular) {
+				final = 0;
+				break;
+			}
+		}
+
+		if (i == 2) {
+			is13simular = simularity_compare(adapt, result, 0, 2);
+			if (is13simular) {
+				final = 0;
+				break;
+			}
+			is23simular = simularity_compare(adapt, result, 1, 2);
+			if (is23simular)
+				final = 1;
+			else
+				final = 3;
+		}
+	}
+
+	for (i = 0; i < 4; i++) {
+		reg_e94 = result[i][0];
+		reg_e9c = result[i][1];
+		reg_ea4 = result[i][2];
+		reg_eac = result[i][3];
+		reg_eb4 = result[i][4];
+		reg_ebc = result[i][5];
+		reg_ec4 = result[i][6];
+		reg_ecc = result[i][7];
+	}
+
+	if (final != 0xff) {
+		reg_e94 = result[final][0];
+		reg_e9c = result[final][1];
+		reg_ea4 = result[final][2];
+		reg_eac = result[final][3];
+		reg_eb4 = result[final][4];
+		reg_ebc = result[final][5];
+		dm_odm->RFCalibrateInfo.RegE94 = reg_e94;
+		dm_odm->RFCalibrateInfo.RegE9C = reg_e9c;
+		dm_odm->RFCalibrateInfo.RegEB4 = reg_eb4;
+		dm_odm->RFCalibrateInfo.RegEBC = reg_ebc;
+		reg_ec4 = result[final][6];
+		reg_ecc = result[final][7];
+		pathaok = true;
+		pathbok = true;
+	} else {
+		ODM_RT_TRACE(dm_odm, ODM_COMP_CALIBRATION, ODM_DBG_LOUD,
+			     ("IQK: FAIL use default value\n"));
+		dm_odm->RFCalibrateInfo.RegE94 = 0x100;
+		dm_odm->RFCalibrateInfo.RegEB4 = 0x100;
+		dm_odm->RFCalibrateInfo.RegE9C = 0x0;
+		dm_odm->RFCalibrateInfo.RegEBC = 0x0;
+	}
+	if (reg_e94 != 0)
+		patha_fill_iqk(adapt, pathaok, result, final,
+			       (reg_ea4 == 0));
+	if (is2t) {
+		if (reg_eb4 != 0)
+			pathb_fill_iqk(adapt, pathbok, result, final,
+				       (reg_ec4 == 0));
+	}
+
+	chn_index = get_right_chnl_for_iqk(hal_data->CurrentChannel);
+
+	if (final < 4) {
+		for (i = 0; i < IQK_Matrix_REG_NUM; i++)
+			dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[chn_index].Value[0][i] = result[final][i];
+		dm_odm->RFCalibrateInfo.IQKMatrixRegSetting[chn_index].bIQKDone = true;
+	}
+
+	save_adda_registers(adapt, iqk_bb_reg_92c,
+			    dm_odm->RFCalibrateInfo.IQK_BB_backup_recover, 9);
+}
+
+void rtl88eu_phy_lc_calibrate(struct adapter *adapt)
+{
+	bool singletone = false, carrier_sup = false;
+	u32 timeout = 2000, timecount = 0;
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct odm_dm_struct *dm_odm = &hal_data->odmpriv;
+
+	if (!(dm_odm->SupportAbility & ODM_RF_CALIBRATION))
+		return;
+	if (singletone || carrier_sup)
+		return;
+
+	while (*(dm_odm->pbScanInProcess) && timecount < timeout) {
+		mdelay(50);
+		timecount += 50;
+	}
+
+	dm_odm->RFCalibrateInfo.bLCKInProgress = true;
+
+	if (dm_odm->RFType == ODM_2T2R) {
+		phy_lc_calibrate(adapt, true);
+	} else {
+		/* For 88C 1T1R */
+		phy_lc_calibrate(adapt, false);
+	}
+
+	dm_odm->RFCalibrateInfo.bLCKInProgress = false;
+}
diff --git a/drivers/staging/rtl8188eu/hal/Hal8188EPwrSeq.c b/drivers/staging/rtl8188eu/hal/pwrseq.c
index fc23bf159345..20dce42cee1d 100644
--- a/drivers/staging/rtl8188eu/hal/Hal8188EPwrSeq.c
+++ b/drivers/staging/rtl8188eu/hal/pwrseq.c
@@ -18,68 +18,84 @@
  *
  ******************************************************************************/
 
-#include "Hal8188EPwrSeq.h"
+#include "pwrseq.h"
 #include <rtl8188e_hal.h>
 
 /*
     drivers should parse below arrays and do the corresponding actions
 */
 /* 3 Power on  Array */
-struct wl_pwr_cfg rtl8188E_power_on_flow[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS + RTL8188E_TRANS_END_STEPS] = {
+struct wl_pwr_cfg rtl8188E_power_on_flow[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS +
+					 RTL8188E_TRANS_END_STEPS] = {
 	RTL8188E_TRANS_CARDEMU_TO_ACT
 	RTL8188E_TRANS_END
 };
 
 /* 3Radio off Array */
-struct wl_pwr_cfg rtl8188E_radio_off_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS + RTL8188E_TRANS_END_STEPS] = {
+struct wl_pwr_cfg rtl8188E_radio_off_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+					  RTL8188E_TRANS_END_STEPS] = {
 	RTL8188E_TRANS_ACT_TO_CARDEMU
 	RTL8188E_TRANS_END
 };
 
 /* 3Card Disable Array */
-struct wl_pwr_cfg rtl8188E_card_disable_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS + RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS + RTL8188E_TRANS_END_STEPS] = {
-	RTL8188E_TRANS_ACT_TO_CARDEMU
-	RTL8188E_TRANS_CARDEMU_TO_CARDDIS
-	RTL8188E_TRANS_END
+struct wl_pwr_cfg rtl8188E_card_disable_flow
+	[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+	 RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+	 RTL8188E_TRANS_END_STEPS] = {
+		RTL8188E_TRANS_ACT_TO_CARDEMU
+		RTL8188E_TRANS_CARDEMU_TO_CARDDIS
+		RTL8188E_TRANS_END
 };
 
 /* 3 Card Enable Array */
-struct wl_pwr_cfg rtl8188E_card_enable_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS + RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS + RTL8188E_TRANS_END_STEPS] = {
-	RTL8188E_TRANS_CARDDIS_TO_CARDEMU
-	RTL8188E_TRANS_CARDEMU_TO_ACT
-	RTL8188E_TRANS_END
+struct wl_pwr_cfg rtl8188E_card_enable_flow
+	[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+	 RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+	 RTL8188E_TRANS_END_STEPS] = {
+		RTL8188E_TRANS_CARDDIS_TO_CARDEMU
+		RTL8188E_TRANS_CARDEMU_TO_ACT
+		RTL8188E_TRANS_END
 };
 
 /* 3Suspend Array */
-struct wl_pwr_cfg rtl8188E_suspend_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS + RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS + RTL8188E_TRANS_END_STEPS] = {
+struct wl_pwr_cfg rtl8188E_suspend_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+					RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
+					RTL8188E_TRANS_END_STEPS] = {
 	RTL8188E_TRANS_ACT_TO_CARDEMU
 	RTL8188E_TRANS_CARDEMU_TO_SUS
 	RTL8188E_TRANS_END
 };
 
 /* 3 Resume Array */
-struct wl_pwr_cfg rtl8188E_resume_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS + RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS + RTL8188E_TRANS_END_STEPS] = {
+struct wl_pwr_cfg rtl8188E_resume_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+				       RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
+				       RTL8188E_TRANS_END_STEPS] = {
 	RTL8188E_TRANS_SUS_TO_CARDEMU
 	RTL8188E_TRANS_CARDEMU_TO_ACT
 	RTL8188E_TRANS_END
 };
 
 /* 3HWPDN Array */
-struct wl_pwr_cfg rtl8188E_hwpdn_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS + RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS + RTL8188E_TRANS_END_STEPS] = {
+struct wl_pwr_cfg rtl8188E_hwpdn_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+				      RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+				      RTL8188E_TRANS_END_STEPS] = {
 	RTL8188E_TRANS_ACT_TO_CARDEMU
 	RTL8188E_TRANS_CARDEMU_TO_PDN
 	RTL8188E_TRANS_END
 };
 
 /* 3 Enter LPS */
-struct wl_pwr_cfg rtl8188E_enter_lps_flow[RTL8188E_TRANS_ACT_TO_LPS_STEPS + RTL8188E_TRANS_END_STEPS] = {
+struct wl_pwr_cfg rtl8188E_enter_lps_flow[RTL8188E_TRANS_ACT_TO_LPS_STEPS +
+					  RTL8188E_TRANS_END_STEPS] = {
 	/* FW behavior */
 	RTL8188E_TRANS_ACT_TO_LPS
 	RTL8188E_TRANS_END
 };
 
 /* 3 Leave LPS */
-struct wl_pwr_cfg rtl8188E_leave_lps_flow[RTL8188E_TRANS_LPS_TO_ACT_STEPS + RTL8188E_TRANS_END_STEPS] = {
+struct wl_pwr_cfg rtl8188E_leave_lps_flow[RTL8188E_TRANS_LPS_TO_ACT_STEPS +
+					  RTL8188E_TRANS_END_STEPS] = {
 	/* FW behavior */
 	RTL8188E_TRANS_LPS_TO_ACT
 	RTL8188E_TRANS_END
diff --git a/drivers/staging/rtl8188eu/hal/HalPwrSeqCmd.c b/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
index caca535ac17d..be0663e93f61 100644
--- a/drivers/staging/rtl8188eu/hal/HalPwrSeqCmd.c
+++ b/drivers/staging/rtl8188eu/hal/pwrseqcmd.c
@@ -15,36 +15,16 @@
  * this program; if not, write to the Free Software Foundation, Inc.,
  * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
  *
- *
  ******************************************************************************/
-/*++
-Copyright (c) Realtek Semiconductor Corp. All rights reserved.
-
-Module Name:
-	HalPwrSeqCmd.c
-
-Abstract:
-	Implement HW Power sequence configuration CMD handling routine for Realtek devices.
-
-Major Change History:
-	When       Who               What
-	---------- ---------------   -------------------------------
-	2011-10-26 Lucas            Modify to be compatible with SD4-CE driver.
-	2011-07-07 Roger            Create.
 
---*/
-
-#include <HalPwrSeqCmd.h>
+#include <pwrseqcmd.h>
 #include <usb_ops_linux.h>
 
-/*	Description: */
-/*		This routine deals with the Power Configuration CMDs parsing
- *		for RTL8723/RTL8188E Series IC.
- *	Assumption:
- *		We should follow specific format which was released from HW SD.
+/* This routine deals with the Power Configuration CMDs parsing
+ * for RTL8723/RTL8188E Series IC.
  */
-u8 HalPwrSeqCmdParsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
-		       u8 ifacetype, struct wl_pwr_cfg pwrseqcmd[])
+u8 rtl88eu_pwrseqcmdparsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
+			    u8 ifacetype, struct wl_pwr_cfg pwrseqcmd[])
 {
 	struct wl_pwr_cfg pwrcfgcmd = {0};
 	u8 poll_bit = false;
@@ -58,7 +38,9 @@ u8 HalPwrSeqCmdParsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
 		pwrcfgcmd = pwrseqcmd[aryidx];
 
 		RT_TRACE(_module_hal_init_c_ , _drv_info_,
-			 ("HalPwrSeqCmdParsing: offset(%#x) cut_msk(%#x) fab_msk(%#x) interface_msk(%#x) base(%#x) cmd(%#x) msk(%#x) value(%#x)\n",
+			 ("rtl88eu_pwrseqcmdparsing: offset(%#x) cut_msk(%#x)"
+			  "fab_msk(%#x) interface_msk(%#x) base(%#x) cmd(%#x)"
+			  "msk(%#x) value(%#x)\n",
 			 GET_PWR_CFG_OFFSET(pwrcfgcmd),
 			 GET_PWR_CFG_CUT_MASK(pwrcfgcmd),
 			 GET_PWR_CFG_FAB_MASK(pwrcfgcmd),
@@ -68,37 +50,42 @@ u8 HalPwrSeqCmdParsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
 			 GET_PWR_CFG_MASK(pwrcfgcmd),
 			 GET_PWR_CFG_VALUE(pwrcfgcmd)));
 
-		/* 2 Only Handle the command whose FAB, CUT, and Interface are matched */
+		/* Only Handle the command whose FAB, CUT, and Interface are matched */
 		if ((GET_PWR_CFG_FAB_MASK(pwrcfgcmd) & fab_vers) &&
 		    (GET_PWR_CFG_CUT_MASK(pwrcfgcmd) & cut_vers) &&
 		    (GET_PWR_CFG_INTF_MASK(pwrcfgcmd) & ifacetype)) {
 			switch (GET_PWR_CFG_CMD(pwrcfgcmd)) {
 			case PWR_CMD_READ:
-				RT_TRACE(_module_hal_init_c_ , _drv_info_, ("HalPwrSeqCmdParsing: PWR_CMD_READ\n"));
+				RT_TRACE(_module_hal_init_c_, _drv_info_,
+					 ("rtl88eu_pwrseqcmdparsing: PWR_CMD_READ\n"));
 				break;
 			case PWR_CMD_WRITE:
-				RT_TRACE(_module_hal_init_c_ , _drv_info_, ("HalPwrSeqCmdParsing: PWR_CMD_WRITE\n"));
+				RT_TRACE(_module_hal_init_c_, _drv_info_,
+					 ("rtl88eu_pwrseqcmdparsing: PWR_CMD_WRITE\n"));
 				offset = GET_PWR_CFG_OFFSET(pwrcfgcmd);
 
 				/*  Read the value from system register */
 				value = usb_read8(padapter, offset);
 
 				value &= ~(GET_PWR_CFG_MASK(pwrcfgcmd));
-				value |= (GET_PWR_CFG_VALUE(pwrcfgcmd) & GET_PWR_CFG_MASK(pwrcfgcmd));
+				value |= (GET_PWR_CFG_VALUE(pwrcfgcmd) &
+					  GET_PWR_CFG_MASK(pwrcfgcmd));
 
 				/*  Write the value back to system register */
 				usb_write8(padapter, offset, value);
 				break;
 			case PWR_CMD_POLLING:
-				RT_TRACE(_module_hal_init_c_ , _drv_info_, ("HalPwrSeqCmdParsing: PWR_CMD_POLLING\n"));
+				RT_TRACE(_module_hal_init_c_, _drv_info_,
+					 ("rtl88eu_pwrseqcmdparsing: PWR_CMD_POLLING\n"));
 
 				poll_bit = false;
 				offset = GET_PWR_CFG_OFFSET(pwrcfgcmd);
 				do {
-						value = usb_read8(padapter, offset);
-
+					value = usb_read8(padapter, offset);
 					value &= GET_PWR_CFG_MASK(pwrcfgcmd);
-					if (value == (GET_PWR_CFG_VALUE(pwrcfgcmd) & GET_PWR_CFG_MASK(pwrcfgcmd)))
+
+					if (value == (GET_PWR_CFG_VALUE(pwrcfgcmd) &
+						      GET_PWR_CFG_MASK(pwrcfgcmd)))
 						poll_bit = true;
 					else
 						udelay(10);
@@ -110,19 +97,22 @@ u8 HalPwrSeqCmdParsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
 				} while (!poll_bit);
 				break;
 			case PWR_CMD_DELAY:
-				RT_TRACE(_module_hal_init_c_ , _drv_info_, ("HalPwrSeqCmdParsing: PWR_CMD_DELAY\n"));
+				RT_TRACE(_module_hal_init_c_, _drv_info_,
+					 ("rtl88eu_pwrseqcmdparsing: PWR_CMD_DELAY\n"));
 				if (GET_PWR_CFG_VALUE(pwrcfgcmd) == PWRSEQ_DELAY_US)
 					udelay(GET_PWR_CFG_OFFSET(pwrcfgcmd));
 				else
 					udelay(GET_PWR_CFG_OFFSET(pwrcfgcmd)*1000);
 				break;
 			case PWR_CMD_END:
-				/*  When this command is parsed, end the process */
-				RT_TRACE(_module_hal_init_c_ , _drv_info_, ("HalPwrSeqCmdParsing: PWR_CMD_END\n"));
+				/* When this command is parsed, end the process */
+				RT_TRACE(_module_hal_init_c_, _drv_info_,
+					 ("rtl88eu_pwrseqcmdparsing: PWR_CMD_END\n"));
 				return true;
 				break;
 			default:
-				RT_TRACE(_module_hal_init_c_ , _drv_err_, ("HalPwrSeqCmdParsing: Unknown CMD!!\n"));
+				RT_TRACE(_module_hal_init_c_, _drv_err_,
+					 ("rtl88eu_pwrseqcmdparsing: Unknown CMD!!\n"));
 				break;
 			}
 		}
diff --git a/drivers/staging/rtl8188eu/hal/rf.c b/drivers/staging/rtl8188eu/hal/rf.c
new file mode 100644
index 000000000000..c2fac34c8132
--- /dev/null
+++ b/drivers/staging/rtl8188eu/hal/rf.c
@@ -0,0 +1,318 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ ******************************************************************************/
+
+#include <osdep_service.h>
+#include <drv_types.h>
+#include <phy.h>
+#include <rf.h>
+#include <rtl8188e_hal.h>
+
+void rtl88eu_phy_rf6052_set_bandwidth(struct adapter *adapt,
+				      enum ht_channel_width bandwidth)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+
+	switch (bandwidth) {
+	case HT_CHANNEL_WIDTH_20:
+		hal_data->RfRegChnlVal[0] = ((hal_data->RfRegChnlVal[0] &
+					      0xfffff3ff) | BIT(10) | BIT(11));
+		phy_set_rf_reg(adapt, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask,
+			       hal_data->RfRegChnlVal[0]);
+		break;
+	case HT_CHANNEL_WIDTH_40:
+		hal_data->RfRegChnlVal[0] = ((hal_data->RfRegChnlVal[0] &
+					      0xfffff3ff) | BIT(10));
+		phy_set_rf_reg(adapt, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask,
+			       hal_data->RfRegChnlVal[0]);
+		break;
+	default:
+		break;
+	}
+}
+
+void rtl88eu_phy_rf6052_set_cck_txpower(struct adapter *adapt, u8 *powerlevel)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct dm_priv *pdmpriv = &hal_data->dmpriv;
+	struct mlme_ext_priv *pmlmeext = &adapt->mlmeextpriv;
+	u32 tx_agc[2] = {0, 0}, tmpval = 0, pwrtrac_value;
+	u8 idx1, idx2;
+	u8 *ptr;
+	u8 direction;
+
+
+	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
+		tx_agc[RF_PATH_A] = 0x3f3f3f3f;
+		tx_agc[RF_PATH_B] = 0x3f3f3f3f;
+		for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
+			tx_agc[idx1] = powerlevel[idx1] |
+				      (powerlevel[idx1]<<8) |
+				      (powerlevel[idx1]<<16) |
+				      (powerlevel[idx1]<<24);
+			if (tx_agc[idx1] > 0x20 && hal_data->ExternalPA)
+				tx_agc[idx1] = 0x20;
+		}
+	} else {
+		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1) {
+			tx_agc[RF_PATH_A] = 0x10101010;
+			tx_agc[RF_PATH_B] = 0x10101010;
+		} else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level2) {
+			tx_agc[RF_PATH_A] = 0x00000000;
+			tx_agc[RF_PATH_B] = 0x00000000;
+		} else {
+			for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
+				tx_agc[idx1] = powerlevel[idx1] |
+					       (powerlevel[idx1]<<8) |
+					       (powerlevel[idx1]<<16) |
+					       (powerlevel[idx1]<<24);
+			}
+			if (hal_data->EEPROMRegulatory == 0) {
+				tmpval = hal_data->MCSTxPowerLevelOriginalOffset[0][6] +
+					 (hal_data->MCSTxPowerLevelOriginalOffset[0][7]<<8);
+				tx_agc[RF_PATH_A] += tmpval;
+
+				tmpval = hal_data->MCSTxPowerLevelOriginalOffset[0][14] +
+					 (hal_data->MCSTxPowerLevelOriginalOffset[0][15]<<24);
+				tx_agc[RF_PATH_B] += tmpval;
+			}
+		}
+	}
+	for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
+		ptr = (u8 *)(&(tx_agc[idx1]));
+		for (idx2 = 0; idx2 < 4; idx2++) {
+			if (*ptr > RF6052_MAX_TX_PWR)
+				*ptr = RF6052_MAX_TX_PWR;
+			ptr++;
+		}
+	}
+	rtl88eu_dm_txpower_track_adjust(&hal_data->odmpriv, 1, &direction,
+				        &pwrtrac_value);
+
+	if (direction == 1) {
+		/*  Increase TX power */
+		tx_agc[0] += pwrtrac_value;
+		tx_agc[1] += pwrtrac_value;
+	} else if (direction == 2) {
+		/*  Decrease TX power */
+		tx_agc[0] -=  pwrtrac_value;
+		tx_agc[1] -=  pwrtrac_value;
+	}
+
+	/*  rf-A cck tx power */
+	tmpval = tx_agc[RF_PATH_A]&0xff;
+	phy_set_bb_reg(adapt, rTxAGC_A_CCK1_Mcs32, bMaskByte1, tmpval);
+	tmpval = tx_agc[RF_PATH_A]>>8;
+	phy_set_bb_reg(adapt, rTxAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
+
+	/*  rf-B cck tx power */
+	tmpval = tx_agc[RF_PATH_B]>>24;
+	phy_set_bb_reg(adapt, rTxAGC_B_CCK11_A_CCK2_11, bMaskByte0, tmpval);
+	tmpval = tx_agc[RF_PATH_B]&0x00ffffff;
+	phy_set_bb_reg(adapt, rTxAGC_B_CCK1_55_Mcs32, 0xffffff00, tmpval);
+}
+
+/*  powerbase0 for OFDM rates */
+/*  powerbase1 for HT MCS rates */
+static void getpowerbase88e(struct adapter *adapt, u8 *pwr_level_ofdm,
+			    u8 *pwr_level_bw20, u8 *pwr_level_bw40,
+			    u8 channel,u32 *ofdmbase, u32 *mcs_base)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	u32 powerbase0, powerbase1;
+	u8 i, powerlevel[2];
+
+	for (i = 0; i < 2; i++) {
+		powerbase0 = pwr_level_ofdm[i];
+
+		powerbase0 = (powerbase0<<24) | (powerbase0<<16) |
+			     (powerbase0<<8) | powerbase0;
+		*(ofdmbase+i) = powerbase0;
+	}
+	for (i = 0; i < hal_data->NumTotalRFPath; i++) {
+		/* Check HT20 to HT40 diff */
+		if (hal_data->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+			powerlevel[i] = pwr_level_bw20[i];
+		else
+			powerlevel[i] = pwr_level_bw40[i];
+		powerbase1 = powerlevel[i];
+		powerbase1 = (powerbase1<<24) | (powerbase1<<16) |
+			     (powerbase1<<8) | powerbase1;
+		*(mcs_base+i) = powerbase1;
+	}
+}
+static void get_rx_power_val_by_reg(struct adapter *adapt, u8 channel,
+				    u8 index, u32 *powerbase0, u32 *powerbase1,
+				    u32 *out_val)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	struct dm_priv	*pdmpriv = &hal_data->dmpriv;
+	u8 i, chnlGroup = 0, pwr_diff_limit[4], customer_pwr_limit;
+	s8 pwr_diff = 0;
+	u32 write_val, customer_limit, rf;
+	u8 regulatory = hal_data->EEPROMRegulatory;
+
+	/*  Index 0 & 1= legacy OFDM, 2-5=HT_MCS rate */
+
+	for (rf = 0; rf < 2; rf++) {
+		u8 j = index + (rf ? 8 : 0);
+
+		switch (regulatory) {
+		case 0:
+			chnlGroup = 0;
+			write_val = hal_data->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf ? 8 : 0)] +
+				((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+			break;
+		case 1: /*  Realtek regulatory */
+			/*  increase power diff defined by Realtek for regulatory */
+			if (hal_data->pwrGroupCnt == 1)
+				chnlGroup = 0;
+			if (hal_data->pwrGroupCnt >= hal_data->PGMaxGroup) {
+				if (channel < 3)
+					chnlGroup = 0;
+				else if (channel < 6)
+					chnlGroup = 1;
+				else if (channel < 9)
+					chnlGroup = 2;
+				else if (channel < 12)
+					chnlGroup = 3;
+				else if (channel < 14)
+					chnlGroup = 4;
+				else if (channel == 14)
+					chnlGroup = 5;
+			}
+			write_val = hal_data->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf ? 8 : 0)] +
+					((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+			break;
+		case 2:	/*  Better regulatory */
+				/*  don't increase any power diff */
+			write_val = ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+			break;
+		case 3:	/*  Customer defined power diff. */
+				/*  increase power diff defined by customer. */
+			chnlGroup = 0;
+
+			if (index < 2)
+				pwr_diff = hal_data->TxPwrLegacyHtDiff[rf][channel-1];
+			else if (hal_data->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
+				pwr_diff = hal_data->TxPwrHt20Diff[rf][channel-1];
+
+			if (hal_data->CurrentChannelBW == HT_CHANNEL_WIDTH_40)
+				customer_pwr_limit = hal_data->PwrGroupHT40[rf][channel-1];
+			else
+				customer_pwr_limit = hal_data->PwrGroupHT20[rf][channel-1];
+
+			if (pwr_diff >= customer_pwr_limit)
+				pwr_diff = 0;
+			else
+				pwr_diff = customer_pwr_limit - pwr_diff;
+
+			for (i = 0; i < 4; i++) {
+				pwr_diff_limit[i] = (u8)((hal_data->MCSTxPowerLevelOriginalOffset[chnlGroup][j] &
+							 (0x7f << (i * 8))) >> (i * 8));
+
+				if (pwr_diff_limit[i] > pwr_diff)
+					pwr_diff_limit[i] = pwr_diff;
+			}
+			customer_limit = (pwr_diff_limit[3]<<24) |
+					 (pwr_diff_limit[2]<<16) |
+					 (pwr_diff_limit[1]<<8) |
+					 (pwr_diff_limit[0]);
+			write_val = customer_limit + ((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+			break;
+		default:
+			chnlGroup = 0;
+			write_val = hal_data->MCSTxPowerLevelOriginalOffset[chnlGroup][j] +
+					((index < 2) ? powerbase0[rf] : powerbase1[rf]);
+			break;
+		}
+/*  20100427 Joseph: Driver dynamic Tx power shall not affect Tx power. It shall be determined by power training mechanism. */
+/*  Currently, we cannot fully disable driver dynamic tx power mechanism because it is referenced by BT coexist mechanism. */
+/*  In the future, two mechanism shall be separated from each other and maintained independently. Thanks for Lanhsin's reminder. */
+		/* 92d do not need this */
+		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1)
+			write_val = 0x14141414;
+		else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level2)
+			write_val = 0x00000000;
+
+		*(out_val+rf) = write_val;
+	}
+}
+
+static void write_ofdm_pwr_reg(struct adapter *adapt, u8 index, u32 *pvalue)
+{
+	u16 regoffset_a[6] = { rTxAGC_A_Rate18_06, rTxAGC_A_Rate54_24,
+			       rTxAGC_A_Mcs03_Mcs00, rTxAGC_A_Mcs07_Mcs04,
+			       rTxAGC_A_Mcs11_Mcs08, rTxAGC_A_Mcs15_Mcs12 };
+	u16 regoffset_b[6] = { rTxAGC_B_Rate18_06, rTxAGC_B_Rate54_24,
+			       rTxAGC_B_Mcs03_Mcs00, rTxAGC_B_Mcs07_Mcs04,
+			       rTxAGC_B_Mcs11_Mcs08, rTxAGC_B_Mcs15_Mcs12 };
+	u8 i, rf, pwr_val[4];
+	u32 write_val;
+	u16 regoffset;
+
+	for (rf = 0; rf < 2; rf++) {
+		write_val = pvalue[rf];
+		for (i = 0; i < 4; i++) {
+			pwr_val[i] = (u8)((write_val & (0x7f<<(i*8)))>>(i*8));
+			if (pwr_val[i]  > RF6052_MAX_TX_PWR)
+				pwr_val[i]  = RF6052_MAX_TX_PWR;
+		}
+		write_val = (pwr_val[3]<<24) | (pwr_val[2]<<16) |
+			    (pwr_val[1]<<8) | pwr_val[0];
+
+		if (rf == 0)
+			regoffset = regoffset_a[index];
+		else
+			regoffset = regoffset_b[index];
+
+		phy_set_bb_reg(adapt, regoffset, bMaskDWord, write_val);
+	}
+}
+
+void rtl88eu_phy_rf6052_set_ofdm_txpower(struct adapter *adapt,
+					 u8 *pwr_level_ofdm,
+					 u8 *pwr_level_bw20,
+					 u8 *pwr_level_bw40, u8 channel)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	u32 write_val[2], powerbase0[2], powerbase1[2], pwrtrac_value;
+	u8 direction;
+	u8 index = 0;
+
+	getpowerbase88e(adapt, pwr_level_ofdm, pwr_level_bw20, pwr_level_bw40,
+			channel, &powerbase0[0], &powerbase1[0]);
+
+	rtl88eu_dm_txpower_track_adjust(&hal_data->odmpriv, 0, &direction,
+					&pwrtrac_value);
+
+	for (index = 0; index < 6; index++) {
+		get_rx_power_val_by_reg(adapt, channel, index,
+					&powerbase0[0], &powerbase1[0],
+					&write_val[0]);
+
+		if (direction == 1) {
+			write_val[0] += pwrtrac_value;
+			write_val[1] += pwrtrac_value;
+		} else if (direction == 2) {
+			write_val[0] -= pwrtrac_value;
+			write_val[1] -= pwrtrac_value;
+		}
+		write_ofdm_pwr_reg(adapt, index, &write_val[0]);
+	}
+}
diff --git a/drivers/staging/rtl8188eu/hal/rf_cfg.c b/drivers/staging/rtl8188eu/hal/rf_cfg.c
new file mode 100644
index 000000000000..ddc2f55fe13f
--- /dev/null
+++ b/drivers/staging/rtl8188eu/hal/rf_cfg.c
@@ -0,0 +1,320 @@
+/******************************************************************************
+*
+* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+*
+* This program is free software; you can redistribute it and/or modify it
+* under the terms of version 2 of the GNU General Public License as
+* published by the Free Software Foundation.
+*
+* This program is distributed in the hope that it will be useful, but WITHOUT
+* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+* more details.
+*
+* You should have received a copy of the GNU General Public License along with
+* this program; if not, write to the Free Software Foundation, Inc.,
+* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+*
+*
+******************************************************************************/
+
+#include "odm_precomp.h"
+
+#include <phy.h>
+
+static bool check_condition(struct adapter *adapt, const u32  condition)
+{
+	struct odm_dm_struct *odm = &GET_HAL_DATA(adapt)->odmpriv;
+	u32 _board = odm->BoardType;
+	u32 _platform = odm->SupportPlatform;
+	u32 _interface = odm->SupportInterface;
+	u32 cond = condition;
+
+	if (condition == 0xCDCDCDCD)
+		return true;
+
+	cond = condition & 0x000000FF;
+	if ((_board == cond) && cond != 0x00)
+		return false;
+
+	cond = condition & 0x0000FF00;
+	cond = cond >> 8;
+	if ((_interface & cond) == 0 && cond != 0x07)
+		return false;
+
+	cond = condition & 0x00FF0000;
+	cond = cond >> 16;
+	if ((_platform & cond) == 0 && cond != 0x0F)
+		return false;
+	return true;
+}
+
+/* RadioA_1T.TXT */
+
+static u32 Array_RadioA_1T_8188E[] = {
+		0x000, 0x00030000,
+		0x008, 0x00084000,
+		0x018, 0x00000407,
+		0x019, 0x00000012,
+		0x01E, 0x00080009,
+		0x01F, 0x00000880,
+		0x02F, 0x0001A060,
+		0x03F, 0x00000000,
+		0x042, 0x000060C0,
+		0x057, 0x000D0000,
+		0x058, 0x000BE180,
+		0x067, 0x00001552,
+		0x083, 0x00000000,
+		0x0B0, 0x000FF8FC,
+		0x0B1, 0x00054400,
+		0x0B2, 0x000CCC19,
+		0x0B4, 0x00043003,
+		0x0B6, 0x0004953E,
+		0x0B7, 0x0001C718,
+		0x0B8, 0x000060FF,
+		0x0B9, 0x00080001,
+		0x0BA, 0x00040000,
+		0x0BB, 0x00000400,
+		0x0BF, 0x000C0000,
+		0x0C2, 0x00002400,
+		0x0C3, 0x00000009,
+		0x0C4, 0x00040C91,
+		0x0C5, 0x00099999,
+		0x0C6, 0x000000A3,
+		0x0C7, 0x00088820,
+		0x0C8, 0x00076C06,
+		0x0C9, 0x00000000,
+		0x0CA, 0x00080000,
+		0x0DF, 0x00000180,
+		0x0EF, 0x000001A0,
+		0x051, 0x0006B27D,
+		0xFF0F041F, 0xABCD,
+		0x052, 0x0007E4DD,
+		0xCDCDCDCD, 0xCDCD,
+		0x052, 0x0007E49D,
+		0xFF0F041F, 0xDEAD,
+		0x053, 0x00000073,
+		0x056, 0x00051FF3,
+		0x035, 0x00000086,
+		0x035, 0x00000186,
+		0x035, 0x00000286,
+		0x036, 0x00001C25,
+		0x036, 0x00009C25,
+		0x036, 0x00011C25,
+		0x036, 0x00019C25,
+		0x0B6, 0x00048538,
+		0x018, 0x00000C07,
+		0x05A, 0x0004BD00,
+		0x019, 0x000739D0,
+		0x034, 0x0000ADF3,
+		0x034, 0x00009DF0,
+		0x034, 0x00008DED,
+		0x034, 0x00007DEA,
+		0x034, 0x00006DE7,
+		0x034, 0x000054EE,
+		0x034, 0x000044EB,
+		0x034, 0x000034E8,
+		0x034, 0x0000246B,
+		0x034, 0x00001468,
+		0x034, 0x0000006D,
+		0x000, 0x00030159,
+		0x084, 0x00068200,
+		0x086, 0x000000CE,
+		0x087, 0x00048A00,
+		0x08E, 0x00065540,
+		0x08F, 0x00088000,
+		0x0EF, 0x000020A0,
+		0x03B, 0x000F02B0,
+		0x03B, 0x000EF7B0,
+		0x03B, 0x000D4FB0,
+		0x03B, 0x000CF060,
+		0x03B, 0x000B0090,
+		0x03B, 0x000A0080,
+		0x03B, 0x00090080,
+		0x03B, 0x0008F780,
+		0x03B, 0x000722B0,
+		0x03B, 0x0006F7B0,
+		0x03B, 0x00054FB0,
+		0x03B, 0x0004F060,
+		0x03B, 0x00030090,
+		0x03B, 0x00020080,
+		0x03B, 0x00010080,
+		0x03B, 0x0000F780,
+		0x0EF, 0x000000A0,
+		0x000, 0x00010159,
+		0x018, 0x0000F407,
+		0xFFE, 0x00000000,
+		0xFFE, 0x00000000,
+		0x01F, 0x00080003,
+		0xFFE, 0x00000000,
+		0xFFE, 0x00000000,
+		0x01E, 0x00000001,
+		0x01F, 0x00080000,
+		0x000, 0x00033E60,
+};
+
+#define READ_NEXT_PAIR(v1, v2, i)	\
+do {								\
+	i += 2; v1 = array[i];			\
+	v2 = array[i+1];				\
+} while (0)
+
+#define RFREG_OFFSET_MASK 0xfffff
+#define B3WIREADDREAALENGTH 0x400
+#define B3WIREDATALENGTH 0x800
+#define BRFSI_RFENV 0x10
+
+static void rtl_rfreg_delay(struct adapter *adapt, enum rf_radio_path rfpath,u32 addr, u32 mask, u32 data)
+{
+	if (addr == 0xfe) {
+		mdelay(50);
+	} else if (addr == 0xfd) {
+		mdelay(5);
+	} else if (addr == 0xfc) {
+		mdelay(1);
+	} else if (addr == 0xfb) {
+		udelay(50);
+	} else if (addr == 0xfa) {
+		udelay(5);
+	} else if (addr == 0xf9) {
+		udelay(1);
+	} else {
+		phy_set_rf_reg(adapt, rfpath, addr, mask, data);
+		udelay(1);
+	}
+}
+
+static void rtl8188e_config_rf_reg(struct adapter *adapt,
+	u32 addr, u32 data)
+{
+	u32 content = 0x1000; /*RF Content: radio_a_txt*/
+	u32 maskforphyset = (u32)(content & 0xE000);
+
+	rtl_rfreg_delay(adapt, RF90_PATH_A, addr| maskforphyset,
+			RFREG_OFFSET_MASK,
+			data);
+}
+
+static bool rtl88e_phy_config_rf_with_headerfile(struct adapter *adapt)
+{
+	u32 i;
+	u32 array_len = sizeof(Array_RadioA_1T_8188E)/sizeof(u32);
+	u32 *array = Array_RadioA_1T_8188E;
+
+	for (i = 0; i < array_len; i += 2) {
+		u32 v1 = array[i];
+		u32 v2 = array[i+1];
+
+		if (v1 < 0xCDCDCDCD) {
+			rtl8188e_config_rf_reg(adapt, v1, v2);
+			continue;
+		} else {
+			if (!check_condition(adapt, array[i])) {
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD && v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < array_len - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+					i -= 2;
+			} else {
+				READ_NEXT_PAIR(v1, v2, i);
+				while (v2 != 0xDEAD && v2 != 0xCDEF &&
+				       v2 != 0xCDCD && i < array_len - 2) {
+						rtl8188e_config_rf_reg(adapt, v1, v2);
+						READ_NEXT_PAIR(v1, v2, i);
+				}
+
+				while (v2 != 0xDEAD && i < array_len - 2)
+					READ_NEXT_PAIR(v1, v2, i);
+			}
+		}
+	}
+	return true;
+}
+
+static bool rf6052_conf_para(struct adapter *adapt)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+	u32 u4val = 0;
+	u8 rfpath;
+	bool rtstatus = true;
+	struct bb_reg_def *pphyreg;
+
+	for (rfpath = 0; rfpath < hal_data->NumTotalRFPath; rfpath++) {
+		pphyreg = &hal_data->PHYRegDef[rfpath];
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			u4val = phy_query_bb_reg(adapt, pphyreg->rfintfs,
+						 BRFSI_RFENV);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			u4val = phy_query_bb_reg(adapt, pphyreg->rfintfs,
+						 BRFSI_RFENV << 16);
+			break;
+		}
+
+		phy_set_bb_reg(adapt, pphyreg->rfintfe, BRFSI_RFENV << 16, 0x1);
+		udelay(1);
+
+		phy_set_bb_reg(adapt, pphyreg->rfintfo, BRFSI_RFENV, 0x1);
+		udelay(1);
+
+		phy_set_bb_reg(adapt, pphyreg->rfHSSIPara2,
+			      B3WIREADDREAALENGTH, 0x0);
+		udelay(1);
+
+		phy_set_bb_reg(adapt, pphyreg->rfHSSIPara2,
+			       B3WIREDATALENGTH, 0x0);
+		udelay(1);
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+			rtstatus = rtl88e_phy_config_rf_with_headerfile(adapt);
+			break;
+		case RF90_PATH_B:
+			rtstatus = rtl88e_phy_config_rf_with_headerfile(adapt);
+			break;
+		case RF90_PATH_C:
+			break;
+		case RF90_PATH_D:
+			break;
+		}
+
+		switch (rfpath) {
+		case RF90_PATH_A:
+		case RF90_PATH_C:
+			phy_set_bb_reg(adapt, pphyreg->rfintfs,
+				       BRFSI_RFENV, u4val);
+			break;
+		case RF90_PATH_B:
+		case RF90_PATH_D:
+			phy_set_bb_reg(adapt, pphyreg->rfintfs,
+				       BRFSI_RFENV << 16, u4val);
+			break;
+		}
+
+		if (rtstatus != true)
+			return false;
+	}
+
+	return rtstatus;
+}
+
+static bool rtl88e_phy_rf6052_config(struct adapter *adapt)
+{
+	struct hal_data_8188e *hal_data = GET_HAL_DATA(adapt);
+
+	if (hal_data->rf_type == RF_1T1R)
+		hal_data->NumTotalRFPath = 1;
+	else
+		hal_data->NumTotalRFPath = 2;
+
+	return rf6052_conf_para(adapt);
+}
+
+bool rtl88eu_phy_rf_config(struct adapter *adapt)
+{
+	return rtl88e_phy_rf6052_config(adapt);
+}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
index fbf70f6a0151..d6fe5e6aa4f4 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_hal_init.c
@@ -23,7 +23,7 @@
 #include <linux/vmalloc.h>
 #include <drv_types.h>
 #include <rtw_efuse.h>
-
+#include <phy.h>
 #include <rtl8188e_hal.h>
 
 #include <rtw_iol.h>
@@ -99,216 +99,8 @@ s32 rtl8188e_iol_efuse_patch(struct adapter *padapter)
 	return result;
 }
 
-static s32 iol_ioconfig(struct adapter *padapter, u8 iocfg_bndy)
-{
-	s32 rst = _SUCCESS;
-
-	usb_write8(padapter, REG_TDECTRL+1, iocfg_bndy);
-	rst = iol_execute(padapter, CMD_IOCONFIG);
-	return rst;
-}
-
-static int rtl8188e_IOL_exec_cmds_sync(struct adapter *adapter, struct xmit_frame *xmit_frame, u32 max_wating_ms, u32 bndy_cnt)
-{
-	struct pkt_attrib *pattrib = &xmit_frame->attrib;
-	u8 i;
-	int ret = _FAIL;
-
-	if (rtw_IOL_append_END_cmd(xmit_frame) != _SUCCESS)
-		goto exit;
-	if (rtw_usb_bulk_size_boundary(adapter, TXDESC_SIZE+pattrib->last_txcmdsz)) {
-		if (rtw_IOL_append_END_cmd(xmit_frame) != _SUCCESS)
-			goto exit;
-	}
-
-	dump_mgntframe_and_wait(adapter, xmit_frame, max_wating_ms);
-
-	iol_mode_enable(adapter, 1);
-	for (i = 0; i < bndy_cnt; i++) {
-		u8 page_no = 0;
-		page_no = i*2;
-		ret = iol_ioconfig(adapter, page_no);
-		if (ret != _SUCCESS)
-			break;
-	}
-	iol_mode_enable(adapter, 0);
-exit:
-	/* restore BCN_HEAD */
-	usb_write8(adapter, REG_TDECTRL+1, 0);
-	return ret;
-}
-
-void rtw_IOL_cmd_tx_pkt_buf_dump(struct adapter *Adapter, int data_len)
-{
-	u32 fifo_data, reg_140;
-	u32 addr, rstatus, loop = 0;
-	u16 data_cnts = (data_len/8)+1;
-	u8 *pbuf = vzalloc(data_len+10);
-	DBG_88E("###### %s ######\n", __func__);
-
-	usb_write8(Adapter, REG_PKT_BUFF_ACCESS_CTRL, TXPKT_BUF_SELECT);
-	if (pbuf) {
-		for (addr = 0; addr < data_cnts; addr++) {
-			usb_write32(Adapter, 0x140, addr);
-			msleep(1);
-			loop = 0;
-			do {
-				rstatus = (reg_140 = usb_read32(Adapter, REG_PKTBUF_DBG_CTRL)&BIT24);
-				if (rstatus) {
-					fifo_data = usb_read32(Adapter, REG_PKTBUF_DBG_DATA_L);
-					memcpy(pbuf+(addr*8), &fifo_data, 4);
-
-					fifo_data = usb_read32(Adapter, REG_PKTBUF_DBG_DATA_H);
-					memcpy(pbuf+(addr*8+4), &fifo_data, 4);
-				}
-				msleep(1);
-			} while (!rstatus && (loop++ < 10));
-		}
-		rtw_IOL_cmd_buf_dump(Adapter, data_len, pbuf);
-		vfree(pbuf);
-	}
-	DBG_88E("###### %s ######\n", __func__);
-}
-
-static void _FWDownloadEnable(struct adapter *padapter, bool enable)
-{
-	u8 tmp;
-
-	if (enable) {
-		/*  MCU firmware download enable. */
-		tmp = usb_read8(padapter, REG_MCUFWDL);
-		usb_write8(padapter, REG_MCUFWDL, tmp | 0x01);
-
-		/*  8051 reset */
-		tmp = usb_read8(padapter, REG_MCUFWDL+2);
-		usb_write8(padapter, REG_MCUFWDL+2, tmp&0xf7);
-	} else {
-		/*  MCU firmware download disable. */
-		tmp = usb_read8(padapter, REG_MCUFWDL);
-		usb_write8(padapter, REG_MCUFWDL, tmp&0xfe);
-
-		/*  Reserved for fw extension. */
-		usb_write8(padapter, REG_MCUFWDL+1, 0x00);
-	}
-}
-
 #define MAX_REG_BOLCK_SIZE	196
 
-static int _BlockWrite(struct adapter *padapter, void *buffer, u32 buffSize)
-{
-	int ret = _SUCCESS;
-	u32	blockSize_p1 = 4;	/*  (Default) Phase #1 : PCI muse use 4-byte write to download FW */
-	u32	blockSize_p2 = 8;	/*  Phase #2 : Use 8-byte, if Phase#1 use big size to write FW. */
-	u32	blockSize_p3 = 1;	/*  Phase #3 : Use 1-byte, the remnant of FW image. */
-	u32	blockCount_p1 = 0, blockCount_p2 = 0, blockCount_p3 = 0;
-	u32	remainSize_p1 = 0, remainSize_p2 = 0;
-	u8 *bufferPtr	= (u8 *)buffer;
-	u32	i = 0, offset = 0;
-
-	blockSize_p1 = MAX_REG_BOLCK_SIZE;
-
-	/* 3 Phase #1 */
-	blockCount_p1 = buffSize / blockSize_p1;
-	remainSize_p1 = buffSize % blockSize_p1;
-
-	if (blockCount_p1) {
-		RT_TRACE(_module_hal_init_c_, _drv_notice_,
-			 ("_BlockWrite: [P1] buffSize(%d) blockSize_p1(%d) blockCount_p1(%d) remainSize_p1(%d)\n",
-			 buffSize, blockSize_p1, blockCount_p1, remainSize_p1));
-	}
-
-	for (i = 0; i < blockCount_p1; i++) {
-		ret = usb_writeN(padapter, (FW_8188E_START_ADDRESS + i * blockSize_p1), blockSize_p1, (bufferPtr + i * blockSize_p1));
-		if (ret == _FAIL)
-			goto exit;
-	}
-
-	/* 3 Phase #2 */
-	if (remainSize_p1) {
-		offset = blockCount_p1 * blockSize_p1;
-
-		blockCount_p2 = remainSize_p1/blockSize_p2;
-		remainSize_p2 = remainSize_p1%blockSize_p2;
-
-		if (blockCount_p2) {
-				RT_TRACE(_module_hal_init_c_, _drv_notice_,
-					 ("_BlockWrite: [P2] buffSize_p2(%d) blockSize_p2(%d) blockCount_p2(%d) remainSize_p2(%d)\n",
-					 (buffSize-offset), blockSize_p2 , blockCount_p2, remainSize_p2));
-		}
-
-		for (i = 0; i < blockCount_p2; i++) {
-			ret = usb_writeN(padapter, (FW_8188E_START_ADDRESS + offset + i*blockSize_p2), blockSize_p2, (bufferPtr + offset + i*blockSize_p2));
-
-			if (ret == _FAIL)
-				goto exit;
-		}
-	}
-
-	/* 3 Phase #3 */
-	if (remainSize_p2) {
-		offset = (blockCount_p1 * blockSize_p1) + (blockCount_p2 * blockSize_p2);
-
-		blockCount_p3 = remainSize_p2 / blockSize_p3;
-
-		RT_TRACE(_module_hal_init_c_, _drv_notice_,
-			 ("_BlockWrite: [P3] buffSize_p3(%d) blockSize_p3(%d) blockCount_p3(%d)\n",
-			 (buffSize-offset), blockSize_p3, blockCount_p3));
-
-		for (i = 0; i < blockCount_p3; i++) {
-			ret = usb_write8(padapter, (FW_8188E_START_ADDRESS + offset + i), *(bufferPtr + offset + i));
-
-			if (ret == _FAIL)
-				goto exit;
-		}
-	}
-
-exit:
-	return ret;
-}
-
-static int _PageWrite(struct adapter *padapter, u32 page, void *buffer, u32 size)
-{
-	u8 value8;
-	u8 u8Page = (u8)(page & 0x07);
-
-	value8 = (usb_read8(padapter, REG_MCUFWDL+2) & 0xF8) | u8Page;
-	usb_write8(padapter, REG_MCUFWDL+2, value8);
-
-	return _BlockWrite(padapter, buffer, size);
-}
-
-static int _WriteFW(struct adapter *padapter, void *buffer, u32 size)
-{
-	/*  Since we need dynamic decide method of dwonload fw, so we call this function to get chip version. */
-	/*  We can remove _ReadChipVersion from ReadpadapterInfo8192C later. */
-	int ret = _SUCCESS;
-	u32	pageNums, remainSize;
-	u32	page, offset;
-	u8 *bufferPtr = (u8 *)buffer;
-
-	pageNums = size / MAX_PAGE_SIZE;
-	remainSize = size % MAX_PAGE_SIZE;
-
-	for (page = 0; page < pageNums; page++) {
-		offset = page * MAX_PAGE_SIZE;
-		ret = _PageWrite(padapter, page, bufferPtr+offset, MAX_PAGE_SIZE);
-
-		if (ret == _FAIL)
-			goto exit;
-	}
-	if (remainSize) {
-		offset = pageNums * MAX_PAGE_SIZE;
-		page = pageNums;
-		ret = _PageWrite(padapter, page, bufferPtr+offset, remainSize);
-
-		if (ret == _FAIL)
-			goto exit;
-	}
-	RT_TRACE(_module_hal_init_c_, _drv_info_, ("_WriteFW Done- for Normal chip.\n"));
-exit:
-	return ret;
-}
-
 void _8051Reset88E(struct adapter *padapter)
 {
 	u8 u1bTmp;
@@ -319,167 +111,6 @@ void _8051Reset88E(struct adapter *padapter)
 	DBG_88E("=====> _8051Reset88E(): 8051 reset success .\n");
 }
 
-static s32 _FWFreeToGo(struct adapter *padapter)
-{
-	u32	counter = 0;
-	u32	value32;
-
-	/*  polling CheckSum report */
-	do {
-		value32 = usb_read32(padapter, REG_MCUFWDL);
-		if (value32 & FWDL_ChkSum_rpt)
-			break;
-	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
-
-	if (counter >= POLLING_READY_TIMEOUT_COUNT) {
-		DBG_88E("%s: chksum report fail! REG_MCUFWDL:0x%08x\n", __func__, value32);
-		return _FAIL;
-	}
-	DBG_88E("%s: Checksum report OK! REG_MCUFWDL:0x%08x\n", __func__, value32);
-
-	value32 = usb_read32(padapter, REG_MCUFWDL);
-	value32 |= MCUFWDL_RDY;
-	value32 &= ~WINTINI_RDY;
-	usb_write32(padapter, REG_MCUFWDL, value32);
-
-	_8051Reset88E(padapter);
-
-	/*  polling for FW ready */
-	counter = 0;
-	do {
-		value32 = usb_read32(padapter, REG_MCUFWDL);
-		if (value32 & WINTINI_RDY) {
-			DBG_88E("%s: Polling FW ready success!! REG_MCUFWDL:0x%08x\n", __func__, value32);
-			return _SUCCESS;
-		}
-		udelay(5);
-	} while (counter++ < POLLING_READY_TIMEOUT_COUNT);
-
-	DBG_88E("%s: Polling FW ready fail!! REG_MCUFWDL:0x%08x\n", __func__, value32);
-	return _FAIL;
-}
-
-#define IS_FW_81xxC(padapter)	(((GET_HAL_DATA(padapter))->FirmwareSignature & 0xFFF0) == 0x88C0)
-
-static int load_firmware(struct rt_firmware *pFirmware, struct device *device)
-{
-	int rtstatus = _SUCCESS;
-	const struct firmware *fw;
-	const char fw_name[] = "rtlwifi/rtl8188eufw.bin";
-
-	if (request_firmware(&fw, fw_name, device)) {
-		rtstatus = _FAIL;
-		goto exit;
-	}
-	if (!fw) {
-		pr_err("Firmware %s not available\n", fw_name);
-		rtstatus = _FAIL;
-		goto exit;
-	}
-	if (fw->size > FW_8188E_SIZE) {
-		rtstatus = _FAIL;
-		RT_TRACE(_module_hal_init_c_, _drv_err_,
-			 ("Firmware size exceed 0x%X. Check it.\n",
-			 FW_8188E_SIZE));
-		goto exit;
-	}
-
-	pFirmware->szFwBuffer = kzalloc(FW_8188E_SIZE, GFP_KERNEL);
-	if (!pFirmware->szFwBuffer) {
-		rtstatus = _FAIL;
-		goto exit;
-	}
-	memcpy(pFirmware->szFwBuffer, fw->data, fw->size);
-	pFirmware->ulFwLength = fw->size;
-	release_firmware(fw);
-
-	DBG_88E_LEVEL(_drv_info_,
-		      "+%s: !bUsedWoWLANFw, FmrmwareLen:%d+\n", __func__,
-		      pFirmware->ulFwLength);
-exit:
-	return rtstatus;
-}
-
-s32 rtl8188e_FirmwareDownload(struct adapter *padapter)
-{
-	s32	rtStatus = _SUCCESS;
-	u8 writeFW_retry = 0;
-	u32 fwdl_start_time;
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(padapter);
-	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
-	struct device *device = dvobj_to_dev(dvobj);
-	struct rt_firmware_hdr *pFwHdr = NULL;
-	u8 *pFirmwareBuf;
-	u32 FirmwareLen;
-	static int log_version;
-
-	RT_TRACE(_module_hal_init_c_, _drv_info_, ("+%s\n", __func__));
-	if (!dvobj->firmware.szFwBuffer)
-		rtStatus = load_firmware(&dvobj->firmware, device);
-	if (rtStatus == _FAIL) {
-		dvobj->firmware.szFwBuffer = NULL;
-		goto Exit;
-	}
-	pFirmwareBuf = dvobj->firmware.szFwBuffer;
-	FirmwareLen = dvobj->firmware.ulFwLength;
-
-	/*  To Check Fw header. Added by tynli. 2009.12.04. */
-	pFwHdr = (struct rt_firmware_hdr *)dvobj->firmware.szFwBuffer;
-
-	pHalData->FirmwareVersion =  le16_to_cpu(pFwHdr->Version);
-	pHalData->FirmwareSubVersion = pFwHdr->Subversion;
-	pHalData->FirmwareSignature = le16_to_cpu(pFwHdr->Signature);
-
-	if (!log_version++)
-		pr_info("%sFirmware Version %d, SubVersion %d, Signature 0x%x\n",
-			DRIVER_PREFIX, pHalData->FirmwareVersion,
-			pHalData->FirmwareSubVersion, pHalData->FirmwareSignature);
-
-	if (IS_FW_HEADER_EXIST(pFwHdr)) {
-		/*  Shift 32 bytes for FW header */
-		pFirmwareBuf = pFirmwareBuf + 32;
-		FirmwareLen = FirmwareLen - 32;
-	}
-
-	/*  Suggested by Filen. If 8051 is running in RAM code, driver should inform Fw to reset by itself, */
-	/*  or it will cause download Fw fail. 2010.02.01. by tynli. */
-	if (usb_read8(padapter, REG_MCUFWDL) & RAM_DL_SEL) { /* 8051 RAM code */
-		usb_write8(padapter, REG_MCUFWDL, 0x00);
-		_8051Reset88E(padapter);
-	}
-
-	_FWDownloadEnable(padapter, true);
-	fwdl_start_time = jiffies;
-	while (1) {
-		/* reset the FWDL chksum */
-		usb_write8(padapter, REG_MCUFWDL, usb_read8(padapter, REG_MCUFWDL) | FWDL_ChkSum_rpt);
-
-		rtStatus = _WriteFW(padapter, pFirmwareBuf, FirmwareLen);
-
-		if (rtStatus == _SUCCESS ||
-		    (rtw_get_passing_time_ms(fwdl_start_time) > 500 && writeFW_retry++ >= 3))
-			break;
-
-		DBG_88E("%s writeFW_retry:%u, time after fwdl_start_time:%ums\n",
-			__func__, writeFW_retry, rtw_get_passing_time_ms(fwdl_start_time)
-		);
-	}
-	_FWDownloadEnable(padapter, false);
-	if (_SUCCESS != rtStatus) {
-		DBG_88E("DL Firmware failed!\n");
-		goto Exit;
-	}
-
-	rtStatus = _FWFreeToGo(padapter);
-	if (_SUCCESS != rtStatus) {
-		DBG_88E("DL Firmware failed!\n");
-		goto Exit;
-	}
-	RT_TRACE(_module_hal_init_c_, _drv_info_, ("Firmware is ready to run!\n"));
-Exit:
-	return rtStatus;
-}
-
 void rtl8188e_InitializeFirmwareVars(struct adapter *padapter)
 {
 	struct hal_data_8188e *pHalData = GET_HAL_DATA(padapter);
@@ -590,8 +221,8 @@ void rtl8188e_set_hal_ops(struct hal_ops *pHalFunc)
 
 	pHalFunc->read_chip_version = &rtl8188e_read_chip_version;
 
-	pHalFunc->set_bwmode_handler = &PHY_SetBWMode8188E;
-	pHalFunc->set_channel_handler = &PHY_SwChnl8188E;
+	pHalFunc->set_bwmode_handler = &phy_set_bw_mode;
+	pHalFunc->set_channel_handler = &phy_sw_chnl;
 
 	pHalFunc->hal_dm_watchdog = &rtl8188e_HalDmWatchDog;
 
@@ -599,18 +230,14 @@ void rtl8188e_set_hal_ops(struct hal_ops *pHalFunc)
 
 	pHalFunc->AntDivBeforeLinkHandler = &AntDivBeforeLink8188E;
 	pHalFunc->AntDivCompareHandler = &AntDivCompare8188E;
-	pHalFunc->read_bbreg = &rtl8188e_PHY_QueryBBReg;
-	pHalFunc->write_bbreg = &rtl8188e_PHY_SetBBReg;
-	pHalFunc->read_rfreg = &rtl8188e_PHY_QueryRFReg;
-	pHalFunc->write_rfreg = &rtl8188e_PHY_SetRFReg;
+	pHalFunc->read_rfreg = &phy_query_rf_reg;
+	pHalFunc->write_rfreg = &phy_set_rf_reg;
 
 	pHalFunc->sreset_init_value = &sreset_init_value;
 	pHalFunc->sreset_get_wifi_status  = &sreset_get_wifi_status;
 
 	pHalFunc->SetHalODMVarHandler = &rtl8188e_SetHalODMVar;
 
-	pHalFunc->IOL_exec_cmds_sync = &rtl8188e_IOL_exec_cmds_sync;
-
 	pHalFunc->hal_notch_filter = &hal_notch_filter_8188e;
 }
 
@@ -1077,21 +704,3 @@ void Hal_ReadThermalMeter_88E(struct adapter *Adapter, u8 *PROMContent, bool Aut
 	}
 	DBG_88E("ThermalMeter = 0x%x\n", pHalData->EEPROMThermalMeter);
 }
-
-/*  This function is used only for 92C to set REG_BCN_CTRL(0x550) register. */
-/*  We just reserve the value of the register in variable pHalData->RegBcnCtrlVal and then operate */
-/*  the value of the register via atomic operation. */
-/*  This prevents from race condition when setting this register. */
-/*  The value of pHalData->RegBcnCtrlVal is initialized in HwConfigureRTL8192CE() function. */
-
-void SetBcnCtrlReg(struct adapter *padapter, u8 SetBits, u8 ClearBits)
-{
-	struct hal_data_8188e *pHalData;
-
-	pHalData = GET_HAL_DATA(padapter);
-
-	pHalData->RegBcnCtrlVal |= SetBits;
-	pHalData->RegBcnCtrlVal &= ~ClearBits;
-
-	usb_write8(padapter, REG_BCN_CTRL, (u8)pHalData->RegBcnCtrlVal);
-}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c b/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
deleted file mode 100644
index 9f016a5401d8..000000000000
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_phycfg.c
+++ /dev/null
@@ -1,936 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-#define _RTL8188E_PHYCFG_C_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-#include <rtw_iol.h>
-#include <rtl8188e_hal.h>
-
-/*---------------------------Define Local Constant---------------------------*/
-/* Channel switch:The size of command tables for switch channel*/
-#define MAX_PRECMD_CNT 16
-#define MAX_RFDEPENDCMD_CNT 16
-#define MAX_POSTCMD_CNT 16
-
-#define MAX_DOZE_WAITING_TIMES_9x 64
-
-/*---------------------------Define Local Constant---------------------------*/
-
-
-/*------------------------Define global variable-----------------------------*/
-
-/*------------------------Define local variable------------------------------*/
-
-
-/*--------------------Define export function prototype-----------------------*/
-/*  Please refer to header file */
-/*--------------------Define export function prototype-----------------------*/
-
-/*----------------------------Function Body----------------------------------*/
-/*  */
-/*  1. BB register R/W API */
-/*  */
-
-/**
-* Function:	phy_CalculateBitShift
-*
-* OverView:	Get shifted position of the BitMask
-*
-* Input:
-*			u32		BitMask,
-*
-* Output:	none
-* Return:		u32		Return the shift bit bit position of the mask
-*/
-static	u32 phy_CalculateBitShift(u32 BitMask)
-{
-	u32 i;
-
-	for (i = 0; i <= 31; i++) {
-		if (((BitMask>>i) &  0x1) == 1)
-			break;
-	}
-	return i;
-}
-
-/**
-* Function:	PHY_QueryBBReg
-*
-* OverView:	Read "sepcific bits" from BB register
-*
-* Input:
-*			struct adapter *Adapter,
-*			u32			RegAddr,	The target address to be readback
-*			u32			BitMask		The target bit position in the target address
-*								to be readback
-* Output:	None
-* Return:		u32			Data		The readback register value
-* Note:		This function is equal to "GetRegSetting" in PHY programming guide
-*/
-u32
-rtl8188e_PHY_QueryBBReg(
-		struct adapter *Adapter,
-		u32 RegAddr,
-		u32 BitMask
-	)
-{
-	u32 ReturnValue = 0, OriginalValue, BitShift;
-
-	OriginalValue = usb_read32(Adapter, RegAddr);
-	BitShift = phy_CalculateBitShift(BitMask);
-	ReturnValue = (OriginalValue & BitMask) >> BitShift;
-	return ReturnValue;
-}
-
-
-/**
-* Function:	PHY_SetBBReg
-*
-* OverView:	Write "Specific bits" to BB register (page 8~)
-*
-* Input:
-*			struct adapter *Adapter,
-*			u32			RegAddr,	The target address to be modified
-*			u32			BitMask		The target bit position in the target address
-*									to be modified
-*			u32			Data		The new register value in the target bit position
-*									of the target address
-*
-* Output:	None
-* Return:		None
-* Note:		This function is equal to "PutRegSetting" in PHY programming guide
-*/
-
-void rtl8188e_PHY_SetBBReg(struct adapter *Adapter, u32 RegAddr, u32 BitMask, u32 Data)
-{
-	u32 OriginalValue, BitShift;
-
-	if (BitMask != bMaskDWord) { /* if not "double word" write */
-		OriginalValue = usb_read32(Adapter, RegAddr);
-		BitShift = phy_CalculateBitShift(BitMask);
-		Data = ((OriginalValue & (~BitMask)) | (Data << BitShift));
-	}
-
-	usb_write32(Adapter, RegAddr, Data);
-}
-
-
-/*  */
-/*  2. RF register R/W API */
-/*  */
-/**
-* Function:	phy_RFSerialRead
-*
-* OverView:	Read regster from RF chips
-*
-* Input:
-*			struct adapter *Adapter,
-*			enum rf_radio_path eRFPath,	Radio path of A/B/C/D
-*			u32			Offset,		The target address to be read
-*
-* Output:	None
-* Return:		u32			reback value
-* Note:		Threre are three types of serial operations:
-*			1. Software serial write
-*			2. Hardware LSSI-Low Speed Serial Interface
-*			3. Hardware HSSI-High speed
-*			serial write. Driver need to implement (1) and (2).
-*			This function is equal to the combination of RF_ReadReg() and  RFLSSIRead()
-*/
-static	u32
-phy_RFSerialRead(
-		struct adapter *Adapter,
-		enum rf_radio_path eRFPath,
-		u32 Offset
-	)
-{
-	u32 retValue = 0;
-	struct hal_data_8188e				*pHalData = GET_HAL_DATA(Adapter);
-	struct bb_reg_def *pPhyReg = &pHalData->PHYRegDef[eRFPath];
-	u32 NewOffset;
-	u32 tmplong, tmplong2;
-	u8 	RfPiEnable = 0;
-	/*  */
-	/*  Make sure RF register offset is correct */
-	/*  */
-	Offset &= 0xff;
-
-	/*  */
-	/*  Switch page for 8256 RF IC */
-	/*  */
-	NewOffset = Offset;
-
-	/*  For 92S LSSI Read RFLSSIRead */
-	/*  For RF A/B write 0x824/82c(does not work in the future) */
-	/*  We must use 0x824 for RF A and B to execute read trigger */
-	tmplong = PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter2, bMaskDWord);
-	if (eRFPath == RF_PATH_A)
-		tmplong2 = tmplong;
-	else
-		tmplong2 = PHY_QueryBBReg(Adapter, pPhyReg->rfHSSIPara2, bMaskDWord);
-
-	tmplong2 = (tmplong2 & (~bLSSIReadAddress)) | (NewOffset<<23) | bLSSIReadEdge;	/* T65 RF */
-
-	PHY_SetBBReg(Adapter, rFPGA0_XA_HSSIParameter2, bMaskDWord, tmplong&(~bLSSIReadEdge));
-	udelay(10);/*  PlatformStallExecution(10); */
-
-	PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, bMaskDWord, tmplong2);
-	udelay(100);/* PlatformStallExecution(100); */
-
-	udelay(10);/* PlatformStallExecution(10); */
-
-	if (eRFPath == RF_PATH_A)
-		RfPiEnable = (u8)PHY_QueryBBReg(Adapter, rFPGA0_XA_HSSIParameter1, BIT8);
-	else if (eRFPath == RF_PATH_B)
-		RfPiEnable = (u8)PHY_QueryBBReg(Adapter, rFPGA0_XB_HSSIParameter1, BIT8);
-
-	if (RfPiEnable) {	/*  Read from BBreg8b8, 12 bits for 8190, 20bits for T65 RF */
-		retValue = PHY_QueryBBReg(Adapter, pPhyReg->rfLSSIReadBackPi, bLSSIReadBackData);
-	} else {	/* Read from BBreg8a0, 12 bits for 8190, 20 bits for T65 RF */
-		retValue = PHY_QueryBBReg(Adapter, pPhyReg->rfLSSIReadBack, bLSSIReadBackData);
-	}
-	return retValue;
-}
-
-/**
-* Function:	phy_RFSerialWrite
-*
-* OverView:	Write data to RF register (page 8~)
-*
-* Input:
-*			struct adapter *Adapter,
-*			enum rf_radio_path eRFPath,	Radio path of A/B/C/D
-*			u32			Offset,		The target address to be read
-*			u32			Data		The new register Data in the target bit position
-*									of the target to be read
-*
-* Output:	None
-* Return:		None
-* Note:		Threre are three types of serial operations:
-*			1. Software serial write
-*			2. Hardware LSSI-Low Speed Serial Interface
-*			3. Hardware HSSI-High speed
-*			serial write. Driver need to implement (1) and (2).
-*			This function is equal to the combination of RF_ReadReg() and  RFLSSIRead()
- *
- * Note:		  For RF8256 only
- *			 The total count of RTL8256(Zebra4) register is around 36 bit it only employs
- *			 4-bit RF address. RTL8256 uses "register mode control bit" (Reg00[12], Reg00[10])
- *			 to access register address bigger than 0xf. See "Appendix-4 in PHY Configuration
- *			 programming guide" for more details.
- *			 Thus, we define a sub-finction for RTL8526 register address conversion
- *		       ===========================================================
- *			 Register Mode		RegCTL[1]		RegCTL[0]		Note
- *								(Reg00[12])		(Reg00[10])
- *		       ===========================================================
- *			 Reg_Mode0				0				x			Reg 0 ~15(0x0 ~ 0xf)
- *		       ------------------------------------------------------------------
- *			 Reg_Mode1				1				0			Reg 16 ~30(0x1 ~ 0xf)
- *		       ------------------------------------------------------------------
- *			 Reg_Mode2				1				1			Reg 31 ~ 45(0x1 ~ 0xf)
- *		       ------------------------------------------------------------------
- *
- *	2008/09/02	MH	Add 92S RF definition
- *
- *
- *
-*/
-static	void
-phy_RFSerialWrite(
-		struct adapter *Adapter,
-		enum rf_radio_path eRFPath,
-		u32 Offset,
-		u32 Data
-	)
-{
-	u32 DataAndAddr = 0;
-	struct hal_data_8188e				*pHalData = GET_HAL_DATA(Adapter);
-	struct bb_reg_def *pPhyReg = &pHalData->PHYRegDef[eRFPath];
-	u32 NewOffset;
-
-
-	/*  2009/06/17 MH We can not execute IO for power save or other accident mode. */
-
-	Offset &= 0xff;
-
-	/*  */
-	/*  Switch page for 8256 RF IC */
-	/*  */
-	NewOffset = Offset;
-
-	/*  */
-	/*  Put write addr in [5:0]  and write data in [31:16] */
-	/*  */
-	DataAndAddr = ((NewOffset<<20) | (Data&0x000fffff)) & 0x0fffffff;	/*  T65 RF */
-
-	/*  */
-	/*  Write Operation */
-	/*  */
-	PHY_SetBBReg(Adapter, pPhyReg->rf3wireOffset, bMaskDWord, DataAndAddr);
-}
-
-/**
-* Function:	PHY_QueryRFReg
-*
-* OverView:	Query "Specific bits" to RF register (page 8~)
-*
-* Input:
-*			struct adapter *Adapter,
-*			enum rf_radio_path eRFPath,	Radio path of A/B/C/D
-*			u32			RegAddr,	The target address to be read
-*			u32			BitMask		The target bit position in the target address
-*									to be read
-*
-* Output:	None
-* Return:		u32			Readback value
-* Note:		This function is equal to "GetRFRegSetting" in PHY programming guide
-*/
-u32 rtl8188e_PHY_QueryRFReg(struct adapter *Adapter, enum rf_radio_path eRFPath,
-			    u32 RegAddr, u32 BitMask)
-{
-	u32 Original_Value, Readback_Value, BitShift;
-
-	Original_Value = phy_RFSerialRead(Adapter, eRFPath, RegAddr);
-
-	BitShift =  phy_CalculateBitShift(BitMask);
-	Readback_Value = (Original_Value & BitMask) >> BitShift;
-	return Readback_Value;
-}
-
-/**
-* Function:	PHY_SetRFReg
-*
-* OverView:	Write "Specific bits" to RF register (page 8~)
-*
-* Input:
-*			struct adapter *Adapter,
-*			enum rf_radio_path eRFPath,	Radio path of A/B/C/D
-*			u32			RegAddr,	The target address to be modified
-*			u32			BitMask		The target bit position in the target address
-*									to be modified
-*			u32			Data		The new register Data in the target bit position
-*									of the target address
-*
-* Output:	None
-* Return:		None
-* Note:		This function is equal to "PutRFRegSetting" in PHY programming guide
-*/
-void
-rtl8188e_PHY_SetRFReg(
-		struct adapter *Adapter,
-		enum rf_radio_path eRFPath,
-		u32 RegAddr,
-		u32 BitMask,
-		u32 Data
-	)
-{
-	u32 Original_Value, BitShift;
-
-	/*  RF data is 12 bits only */
-	if (BitMask != bRFRegOffsetMask) {
-		Original_Value = phy_RFSerialRead(Adapter, eRFPath, RegAddr);
-		BitShift =  phy_CalculateBitShift(BitMask);
-		Data = ((Original_Value & (~BitMask)) | (Data << BitShift));
-	}
-
-	phy_RFSerialWrite(Adapter, eRFPath, RegAddr, Data);
-}
-
-/*  */
-/*  3. Initial MAC/BB/RF config by reading MAC/BB/RF txt. */
-/*  */
-
-/*-----------------------------------------------------------------------------
- * Function:    PHY_MACConfig8192C
- *
- * Overview:	Condig MAC by header file or parameter file.
- *
- * Input:       NONE
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Revised History:
- *  When		Who		Remark
- *  08/12/2008	MHC		Create Version 0.
- *
- *---------------------------------------------------------------------------*/
-s32 PHY_MACConfig8188E(struct adapter *Adapter)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	int rtStatus = _SUCCESS;
-
-	/*  */
-	/*  Config MAC */
-	/*  */
-	if (HAL_STATUS_FAILURE == ODM_ConfigMACWithHeaderFile(&pHalData->odmpriv))
-		rtStatus = _FAIL;
-
-	/*  2010.07.13 AMPDU aggregation number B */
-	usb_write16(Adapter, REG_MAX_AGGR_NUM, MAX_AGGR_NUM);
-
-	return rtStatus;
-}
-
-/**
-* Function:	phy_InitBBRFRegisterDefinition
-*
-* OverView:	Initialize Register definition offset for Radio Path A/B/C/D
-*
-* Input:
-*			struct adapter *Adapter,
-*
-* Output:	None
-* Return:		None
-* Note:		The initialization value is constant and it should never be changes
-*/
-static	void
-phy_InitBBRFRegisterDefinition(
-		struct adapter *Adapter
-)
-{
-	struct hal_data_8188e		*pHalData = GET_HAL_DATA(Adapter);
-
-	/*  RF Interface Sowrtware Control */
-	pHalData->PHYRegDef[RF_PATH_A].rfintfs = rFPGA0_XAB_RFInterfaceSW; /*  16 LSBs if read 32-bit from 0x870 */
-	pHalData->PHYRegDef[RF_PATH_B].rfintfs = rFPGA0_XAB_RFInterfaceSW; /*  16 MSBs if read 32-bit from 0x870 (16-bit for 0x872) */
-	pHalData->PHYRegDef[RF_PATH_C].rfintfs = rFPGA0_XCD_RFInterfaceSW;/*  16 LSBs if read 32-bit from 0x874 */
-	pHalData->PHYRegDef[RF_PATH_D].rfintfs = rFPGA0_XCD_RFInterfaceSW;/*  16 MSBs if read 32-bit from 0x874 (16-bit for 0x876) */
-
-	/*  RF Interface Readback Value */
-	pHalData->PHYRegDef[RF_PATH_A].rfintfi = rFPGA0_XAB_RFInterfaceRB; /*  16 LSBs if read 32-bit from 0x8E0 */
-	pHalData->PHYRegDef[RF_PATH_B].rfintfi = rFPGA0_XAB_RFInterfaceRB;/*  16 MSBs if read 32-bit from 0x8E0 (16-bit for 0x8E2) */
-	pHalData->PHYRegDef[RF_PATH_C].rfintfi = rFPGA0_XCD_RFInterfaceRB;/*  16 LSBs if read 32-bit from 0x8E4 */
-	pHalData->PHYRegDef[RF_PATH_D].rfintfi = rFPGA0_XCD_RFInterfaceRB;/*  16 MSBs if read 32-bit from 0x8E4 (16-bit for 0x8E6) */
-
-	/*  RF Interface Output (and Enable) */
-	pHalData->PHYRegDef[RF_PATH_A].rfintfo = rFPGA0_XA_RFInterfaceOE; /*  16 LSBs if read 32-bit from 0x860 */
-	pHalData->PHYRegDef[RF_PATH_B].rfintfo = rFPGA0_XB_RFInterfaceOE; /*  16 LSBs if read 32-bit from 0x864 */
-
-	/*  RF Interface (Output and)  Enable */
-	pHalData->PHYRegDef[RF_PATH_A].rfintfe = rFPGA0_XA_RFInterfaceOE; /*  16 MSBs if read 32-bit from 0x860 (16-bit for 0x862) */
-	pHalData->PHYRegDef[RF_PATH_B].rfintfe = rFPGA0_XB_RFInterfaceOE; /*  16 MSBs if read 32-bit from 0x864 (16-bit for 0x866) */
-
-	/* Addr of LSSI. Wirte RF register by driver */
-	pHalData->PHYRegDef[RF_PATH_A].rf3wireOffset = rFPGA0_XA_LSSIParameter; /* LSSI Parameter */
-	pHalData->PHYRegDef[RF_PATH_B].rf3wireOffset = rFPGA0_XB_LSSIParameter;
-
-	/*  RF parameter */
-	pHalData->PHYRegDef[RF_PATH_A].rfLSSI_Select = rFPGA0_XAB_RFParameter;  /* BB Band Select */
-	pHalData->PHYRegDef[RF_PATH_B].rfLSSI_Select = rFPGA0_XAB_RFParameter;
-	pHalData->PHYRegDef[RF_PATH_C].rfLSSI_Select = rFPGA0_XCD_RFParameter;
-	pHalData->PHYRegDef[RF_PATH_D].rfLSSI_Select = rFPGA0_XCD_RFParameter;
-
-	/*  Tx AGC Gain Stage (same for all path. Should we remove this?) */
-	pHalData->PHYRegDef[RF_PATH_A].rfTxGainStage = rFPGA0_TxGainStage; /* Tx gain stage */
-	pHalData->PHYRegDef[RF_PATH_B].rfTxGainStage = rFPGA0_TxGainStage; /* Tx gain stage */
-	pHalData->PHYRegDef[RF_PATH_C].rfTxGainStage = rFPGA0_TxGainStage; /* Tx gain stage */
-	pHalData->PHYRegDef[RF_PATH_D].rfTxGainStage = rFPGA0_TxGainStage; /* Tx gain stage */
-
-	/*  Tranceiver A~D HSSI Parameter-1 */
-	pHalData->PHYRegDef[RF_PATH_A].rfHSSIPara1 = rFPGA0_XA_HSSIParameter1;  /* wire control parameter1 */
-	pHalData->PHYRegDef[RF_PATH_B].rfHSSIPara1 = rFPGA0_XB_HSSIParameter1;  /* wire control parameter1 */
-
-	/*  Tranceiver A~D HSSI Parameter-2 */
-	pHalData->PHYRegDef[RF_PATH_A].rfHSSIPara2 = rFPGA0_XA_HSSIParameter2;  /* wire control parameter2 */
-	pHalData->PHYRegDef[RF_PATH_B].rfHSSIPara2 = rFPGA0_XB_HSSIParameter2;  /* wire control parameter2 */
-
-	/*  RF switch Control */
-	pHalData->PHYRegDef[RF_PATH_A].rfSwitchControl = rFPGA0_XAB_SwitchControl; /* TR/Ant switch control */
-	pHalData->PHYRegDef[RF_PATH_B].rfSwitchControl = rFPGA0_XAB_SwitchControl;
-	pHalData->PHYRegDef[RF_PATH_C].rfSwitchControl = rFPGA0_XCD_SwitchControl;
-	pHalData->PHYRegDef[RF_PATH_D].rfSwitchControl = rFPGA0_XCD_SwitchControl;
-
-	/*  AGC control 1 */
-	pHalData->PHYRegDef[RF_PATH_A].rfAGCControl1 = rOFDM0_XAAGCCore1;
-	pHalData->PHYRegDef[RF_PATH_B].rfAGCControl1 = rOFDM0_XBAGCCore1;
-	pHalData->PHYRegDef[RF_PATH_C].rfAGCControl1 = rOFDM0_XCAGCCore1;
-	pHalData->PHYRegDef[RF_PATH_D].rfAGCControl1 = rOFDM0_XDAGCCore1;
-
-	/*  AGC control 2 */
-	pHalData->PHYRegDef[RF_PATH_A].rfAGCControl2 = rOFDM0_XAAGCCore2;
-	pHalData->PHYRegDef[RF_PATH_B].rfAGCControl2 = rOFDM0_XBAGCCore2;
-	pHalData->PHYRegDef[RF_PATH_C].rfAGCControl2 = rOFDM0_XCAGCCore2;
-	pHalData->PHYRegDef[RF_PATH_D].rfAGCControl2 = rOFDM0_XDAGCCore2;
-
-	/*  RX AFE control 1 */
-	pHalData->PHYRegDef[RF_PATH_A].rfRxIQImbalance = rOFDM0_XARxIQImbalance;
-	pHalData->PHYRegDef[RF_PATH_B].rfRxIQImbalance = rOFDM0_XBRxIQImbalance;
-	pHalData->PHYRegDef[RF_PATH_C].rfRxIQImbalance = rOFDM0_XCRxIQImbalance;
-	pHalData->PHYRegDef[RF_PATH_D].rfRxIQImbalance = rOFDM0_XDRxIQImbalance;
-
-	/*  RX AFE control 1 */
-	pHalData->PHYRegDef[RF_PATH_A].rfRxAFE = rOFDM0_XARxAFE;
-	pHalData->PHYRegDef[RF_PATH_B].rfRxAFE = rOFDM0_XBRxAFE;
-	pHalData->PHYRegDef[RF_PATH_C].rfRxAFE = rOFDM0_XCRxAFE;
-	pHalData->PHYRegDef[RF_PATH_D].rfRxAFE = rOFDM0_XDRxAFE;
-
-	/*  Tx AFE control 1 */
-	pHalData->PHYRegDef[RF_PATH_A].rfTxIQImbalance = rOFDM0_XATxIQImbalance;
-	pHalData->PHYRegDef[RF_PATH_B].rfTxIQImbalance = rOFDM0_XBTxIQImbalance;
-	pHalData->PHYRegDef[RF_PATH_C].rfTxIQImbalance = rOFDM0_XCTxIQImbalance;
-	pHalData->PHYRegDef[RF_PATH_D].rfTxIQImbalance = rOFDM0_XDTxIQImbalance;
-
-	/*  Tx AFE control 2 */
-	pHalData->PHYRegDef[RF_PATH_A].rfTxAFE = rOFDM0_XATxAFE;
-	pHalData->PHYRegDef[RF_PATH_B].rfTxAFE = rOFDM0_XBTxAFE;
-	pHalData->PHYRegDef[RF_PATH_C].rfTxAFE = rOFDM0_XCTxAFE;
-	pHalData->PHYRegDef[RF_PATH_D].rfTxAFE = rOFDM0_XDTxAFE;
-
-	/*  Tranceiver LSSI Readback SI mode */
-	pHalData->PHYRegDef[RF_PATH_A].rfLSSIReadBack = rFPGA0_XA_LSSIReadBack;
-	pHalData->PHYRegDef[RF_PATH_B].rfLSSIReadBack = rFPGA0_XB_LSSIReadBack;
-	pHalData->PHYRegDef[RF_PATH_C].rfLSSIReadBack = rFPGA0_XC_LSSIReadBack;
-	pHalData->PHYRegDef[RF_PATH_D].rfLSSIReadBack = rFPGA0_XD_LSSIReadBack;
-
-	/*  Tranceiver LSSI Readback PI mode */
-	pHalData->PHYRegDef[RF_PATH_A].rfLSSIReadBackPi = TransceiverA_HSPI_Readback;
-	pHalData->PHYRegDef[RF_PATH_B].rfLSSIReadBackPi = TransceiverB_HSPI_Readback;
-}
-
-void storePwrIndexDiffRateOffset(struct adapter *Adapter, u32 RegAddr, u32 BitMask, u32 Data)
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-
-	if (RegAddr == rTxAGC_A_Rate18_06)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][0] = Data;
-	if (RegAddr == rTxAGC_A_Rate54_24)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][1] = Data;
-	if (RegAddr == rTxAGC_A_CCK1_Mcs32)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][6] = Data;
-	if (RegAddr == rTxAGC_B_CCK11_A_CCK2_11 && BitMask == 0xffffff00)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][7] = Data;
-	if (RegAddr == rTxAGC_A_Mcs03_Mcs00)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][2] = Data;
-	if (RegAddr == rTxAGC_A_Mcs07_Mcs04)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][3] = Data;
-	if (RegAddr == rTxAGC_A_Mcs11_Mcs08)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][4] = Data;
-	if (RegAddr == rTxAGC_A_Mcs15_Mcs12) {
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][5] = Data;
-		if (pHalData->rf_type == RF_1T1R)
-			pHalData->pwrGroupCnt++;
-	}
-	if (RegAddr == rTxAGC_B_Rate18_06)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][8] = Data;
-	if (RegAddr == rTxAGC_B_Rate54_24)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][9] = Data;
-	if (RegAddr == rTxAGC_B_CCK1_55_Mcs32)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][14] = Data;
-	if (RegAddr == rTxAGC_B_CCK11_A_CCK2_11 && BitMask == 0x000000ff)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][15] = Data;
-	if (RegAddr == rTxAGC_B_Mcs03_Mcs00)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][10] = Data;
-	if (RegAddr == rTxAGC_B_Mcs07_Mcs04)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][11] = Data;
-	if (RegAddr == rTxAGC_B_Mcs11_Mcs08)
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][12] = Data;
-	if (RegAddr == rTxAGC_B_Mcs15_Mcs12) {
-		pHalData->MCSTxPowerLevelOriginalOffset[pHalData->pwrGroupCnt][13] = Data;
-		if (pHalData->rf_type != RF_1T1R)
-			pHalData->pwrGroupCnt++;
-	}
-}
-
-static	int phy_BB8188E_Config_ParaFile(struct adapter *Adapter)
-{
-	struct eeprom_priv *pEEPROM = GET_EEPROM_EFUSE_PRIV(Adapter);
-	struct hal_data_8188e		*pHalData = GET_HAL_DATA(Adapter);
-	int			rtStatus = _SUCCESS;
-
-	/*  */
-	/*  1. Read PHY_REG.TXT BB INIT!! */
-	/*  We will separate as 88C / 92C according to chip version */
-	/*  */
-	if (HAL_STATUS_FAILURE == ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_PHY_REG))
-		rtStatus = _FAIL;
-	if (rtStatus != _SUCCESS)
-		goto phy_BB8190_Config_ParaFile_Fail;
-
-	/*  2. If EEPROM or EFUSE autoload OK, We must config by PHY_REG_PG.txt */
-	if (!pEEPROM->bautoload_fail_flag) {
-		pHalData->pwrGroupCnt = 0;
-
-		if (HAL_STATUS_FAILURE == ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv, CONFIG_BB_PHY_REG_PG))
-			rtStatus = _FAIL;
-	}
-
-	if (rtStatus != _SUCCESS)
-		goto phy_BB8190_Config_ParaFile_Fail;
-
-	/*  3. BB AGC table Initialization */
-	if (HAL_STATUS_FAILURE == ODM_ConfigBBWithHeaderFile(&pHalData->odmpriv,  CONFIG_BB_AGC_TAB))
-		rtStatus = _FAIL;
-
-	if (rtStatus != _SUCCESS)
-		goto phy_BB8190_Config_ParaFile_Fail;
-
-phy_BB8190_Config_ParaFile_Fail:
-
-	return rtStatus;
-}
-
-int
-PHY_BBConfig8188E(
-		struct adapter *Adapter
-	)
-{
-	int	rtStatus = _SUCCESS;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	u32 RegVal;
-	u8 CrystalCap;
-
-	phy_InitBBRFRegisterDefinition(Adapter);
-
-
-	/*  Enable BB and RF */
-	RegVal = usb_read16(Adapter, REG_SYS_FUNC_EN);
-	usb_write16(Adapter, REG_SYS_FUNC_EN, (u16)(RegVal|BIT13|BIT0|BIT1));
-
-	/*  20090923 Joseph: Advised by Steven and Jenyu. Power sequence before init RF. */
-
-	usb_write8(Adapter, REG_RF_CTRL, RF_EN|RF_RSTB|RF_SDMRSTB);
-
-	usb_write8(Adapter, REG_SYS_FUNC_EN, FEN_USBA | FEN_USBD | FEN_BB_GLB_RSTn | FEN_BBRSTB);
-
-	/*  Config BB and AGC */
-	rtStatus = phy_BB8188E_Config_ParaFile(Adapter);
-
-	/*  write 0x24[16:11] = 0x24[22:17] = CrystalCap */
-	CrystalCap = pHalData->CrystalCap & 0x3F;
-	PHY_SetBBReg(Adapter, REG_AFE_XTAL_CTRL, 0x7ff800, (CrystalCap | (CrystalCap << 6)));
-
-	return rtStatus;
-}
-
-int PHY_RFConfig8188E(struct adapter *Adapter)
-{
-	int		rtStatus = _SUCCESS;
-
-	/*  RF config */
-	rtStatus = PHY_RF6052_Config8188E(Adapter);
-	return rtStatus;
-}
-
-static void getTxPowerIndex88E(struct adapter *Adapter, u8 channel, u8 *cckPowerLevel,
-			       u8 *ofdmPowerLevel, u8 *BW20PowerLevel,
-			       u8 *BW40PowerLevel)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	u8 index = (channel - 1);
-	u8 TxCount = 0, path_nums;
-
-	if ((RF_1T2R == pHalData->rf_type) || (RF_1T1R == pHalData->rf_type))
-		path_nums = 1;
-	else
-		path_nums = 2;
-
-	for (TxCount = 0; TxCount < path_nums; TxCount++) {
-		if (TxCount == RF_PATH_A) {
-			/*  1. CCK */
-			cckPowerLevel[TxCount]	= pHalData->Index24G_CCK_Base[TxCount][index];
-			/* 2. OFDM */
-			ofdmPowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-				pHalData->OFDM_24G_Diff[TxCount][RF_PATH_A];
-			/*  1. BW20 */
-			BW20PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-				pHalData->BW20_24G_Diff[TxCount][RF_PATH_A];
-			/* 2. BW40 */
-			BW40PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[TxCount][index];
-		} else if (TxCount == RF_PATH_B) {
-			/*  1. CCK */
-			cckPowerLevel[TxCount]	= pHalData->Index24G_CCK_Base[TxCount][index];
-			/* 2. OFDM */
-			ofdmPowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[TxCount][index];
-			/*  1. BW20 */
-			BW20PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[TxCount][RF_PATH_A]+
-			pHalData->BW20_24G_Diff[TxCount][index];
-			/* 2. BW40 */
-			BW40PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[TxCount][index];
-		} else if (TxCount == RF_PATH_C) {
-			/*  1. CCK */
-			cckPowerLevel[TxCount]	= pHalData->Index24G_CCK_Base[TxCount][index];
-			/* 2. OFDM */
-			ofdmPowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_B][index]+
-			pHalData->BW20_24G_Diff[TxCount][index];
-			/*  1. BW20 */
-			BW20PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_B][index]+
-			pHalData->BW20_24G_Diff[TxCount][index];
-			/* 2. BW40 */
-			BW40PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[TxCount][index];
-		} else if (TxCount == RF_PATH_D) {
-			/*  1. CCK */
-			cckPowerLevel[TxCount]	= pHalData->Index24G_CCK_Base[TxCount][index];
-			/* 2. OFDM */
-			ofdmPowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_B][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_C][index]+
-			pHalData->BW20_24G_Diff[TxCount][index];
-
-			/*  1. BW20 */
-			BW20PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_A][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_B][index]+
-			pHalData->BW20_24G_Diff[RF_PATH_C][index]+
-			pHalData->BW20_24G_Diff[TxCount][index];
-
-			/* 2. BW40 */
-			BW40PowerLevel[TxCount]	= pHalData->Index24G_BW40_Base[TxCount][index];
-		}
-	}
-}
-
-static void phy_PowerIndexCheck88E(struct adapter *Adapter, u8 channel, u8 *cckPowerLevel,
-				   u8 *ofdmPowerLevel, u8 *BW20PowerLevel, u8 *BW40PowerLevel)
-{
-	struct hal_data_8188e		*pHalData = GET_HAL_DATA(Adapter);
-
-	pHalData->CurrentCckTxPwrIdx = cckPowerLevel[0];
-	pHalData->CurrentOfdm24GTxPwrIdx = ofdmPowerLevel[0];
-	pHalData->CurrentBW2024GTxPwrIdx = BW20PowerLevel[0];
-	pHalData->CurrentBW4024GTxPwrIdx = BW40PowerLevel[0];
-}
-
-/*-----------------------------------------------------------------------------
- * Function:    SetTxPowerLevel8190()
- *
- * Overview:    This function is export to "HalCommon" moudule
- *			We must consider RF path later!!!!!!!
- *
- * Input:       struct adapter *Adapter
- *			u8		channel
- *
- * Output:      NONE
- *
- * Return:      NONE
- *	2008/11/04	MHC		We remove EEPROM_93C56.
- *						We need to move CCX relative code to independet file.
- *	2009/01/21	MHC		Support new EEPROM format from SD3 requirement.
- *
- *---------------------------------------------------------------------------*/
-void
-PHY_SetTxPowerLevel8188E(
-		struct adapter *Adapter,
-		u8 channel
-	)
-{
-	u8 cckPowerLevel[MAX_TX_COUNT] = {0};
-	u8 ofdmPowerLevel[MAX_TX_COUNT] = {0};/*  [0]:RF-A, [1]:RF-B */
-	u8 BW20PowerLevel[MAX_TX_COUNT] = {0};
-	u8 BW40PowerLevel[MAX_TX_COUNT] = {0};
-
-	getTxPowerIndex88E(Adapter, channel, &cckPowerLevel[0], &ofdmPowerLevel[0], &BW20PowerLevel[0], &BW40PowerLevel[0]);
-
-	phy_PowerIndexCheck88E(Adapter, channel, &cckPowerLevel[0], &ofdmPowerLevel[0], &BW20PowerLevel[0], &BW40PowerLevel[0]);
-
-	rtl8188e_PHY_RF6052SetCckTxPower(Adapter, &cckPowerLevel[0]);
-	rtl8188e_PHY_RF6052SetOFDMTxPower(Adapter, &ofdmPowerLevel[0], &BW20PowerLevel[0], &BW40PowerLevel[0], channel);
-}
-
-/*-----------------------------------------------------------------------------
- * Function:    PHY_SetBWModeCallback8192C()
- *
- * Overview:    Timer callback function for SetSetBWMode
- *
- * Input:		PRT_TIMER		pTimer
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Note:		(1) We do not take j mode into consideration now
- *			(2) Will two workitem of "switch channel" and "switch channel bandwidth" run
- *			     concurrently?
- *---------------------------------------------------------------------------*/
-static void
-_PHY_SetBWMode92C(
-		struct adapter *Adapter
-)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	u8 regBwOpMode;
-	u8 regRRSR_RSC;
-
-	if (pHalData->rf_chip == RF_PSEUDO_11N)
-		return;
-
-	/*  There is no 40MHz mode in RF_8225. */
-	if (pHalData->rf_chip == RF_8225)
-		return;
-
-	if (Adapter->bDriverStopped)
-		return;
-
-	/* 3 */
-	/* 3<1>Set MAC register */
-	/* 3 */
-
-	regBwOpMode = usb_read8(Adapter, REG_BWOPMODE);
-	regRRSR_RSC = usb_read8(Adapter, REG_RRSR+2);
-
-	switch (pHalData->CurrentChannelBW) {
-	case HT_CHANNEL_WIDTH_20:
-		regBwOpMode |= BW_OPMODE_20MHZ;
-		/*  2007/02/07 Mark by Emily because we have not verify whether this register works */
-		usb_write8(Adapter, REG_BWOPMODE, regBwOpMode);
-		break;
-	case HT_CHANNEL_WIDTH_40:
-		regBwOpMode &= ~BW_OPMODE_20MHZ;
-		/*  2007/02/07 Mark by Emily because we have not verify whether this register works */
-		usb_write8(Adapter, REG_BWOPMODE, regBwOpMode);
-		regRRSR_RSC = (regRRSR_RSC&0x90) | (pHalData->nCur40MhzPrimeSC<<5);
-		usb_write8(Adapter, REG_RRSR+2, regRRSR_RSC);
-		break;
-	default:
-		break;
-	}
-
-	/* 3  */
-	/* 3 <2>Set PHY related register */
-	/* 3 */
-	switch (pHalData->CurrentChannelBW) {
-	/* 20 MHz channel*/
-	case HT_CHANNEL_WIDTH_20:
-		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x0);
-		PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x0);
-		break;
-	/* 40 MHz channel*/
-	case HT_CHANNEL_WIDTH_40:
-		PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bRFMOD, 0x1);
-		PHY_SetBBReg(Adapter, rFPGA1_RFMOD, bRFMOD, 0x1);
-		/*  Set Control channel to upper or lower. These settings are required only for 40MHz */
-		PHY_SetBBReg(Adapter, rCCK0_System, bCCKSideBand, (pHalData->nCur40MhzPrimeSC>>1));
-		PHY_SetBBReg(Adapter, rOFDM1_LSTF, 0xC00, pHalData->nCur40MhzPrimeSC);
-		PHY_SetBBReg(Adapter, 0x818, (BIT26 | BIT27),
-			     (pHalData->nCur40MhzPrimeSC == HAL_PRIME_CHNL_OFFSET_LOWER) ? 2 : 1);
-		break;
-	default:
-		break;
-	}
-	/* Skip over setting of J-mode in BB register here. Default value is "None J mode". Emily 20070315 */
-
-	/* 3<3>Set RF related register */
-	switch (pHalData->rf_chip) {
-	case RF_8225:
-		break;
-	case RF_8256:
-		/*  Please implement this function in Hal8190PciPhy8256.c */
-		break;
-	case RF_8258:
-		/*  Please implement this function in Hal8190PciPhy8258.c */
-		break;
-	case RF_PSEUDO_11N:
-		break;
-	case RF_6052:
-		rtl8188e_PHY_RF6052SetBandwidth(Adapter, pHalData->CurrentChannelBW);
-		break;
-	default:
-		break;
-	}
-}
-
- /*-----------------------------------------------------------------------------
- * Function:   SetBWMode8190Pci()
- *
- * Overview:  This function is export to "HalCommon" moudule
- *
- * Input:		struct adapter *Adapter
- *			enum ht_channel_width Bandwidth	20M or 40M
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Note:		We do not take j mode into consideration now
- *---------------------------------------------------------------------------*/
-void PHY_SetBWMode8188E(struct adapter *Adapter, enum ht_channel_width Bandwidth,	/*  20M or 40M */
-			unsigned char	Offset)		/*  Upper, Lower, or Don't care */
-{
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	enum ht_channel_width tmpBW = pHalData->CurrentChannelBW;
-
-	pHalData->CurrentChannelBW = Bandwidth;
-
-	pHalData->nCur40MhzPrimeSC = Offset;
-
-	if ((!Adapter->bDriverStopped) && (!Adapter->bSurpriseRemoved))
-		_PHY_SetBWMode92C(Adapter);
-	else
-		pHalData->CurrentChannelBW = tmpBW;
-}
-
-static void _PHY_SwChnl8192C(struct adapter *Adapter, u8 channel)
-{
-	u8 eRFPath;
-	u32 param1, param2;
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-
-	if (Adapter->bNotifyChannelChange)
-		DBG_88E("[%s] ch = %d\n", __func__, channel);
-
-	/* s1. pre common command - CmdID_SetTxPowerLevel */
-	PHY_SetTxPowerLevel8188E(Adapter, channel);
-
-	/* s2. RF dependent command - CmdID_RF_WriteReg, param1=RF_CHNLBW, param2=channel */
-	param1 = RF_CHNLBW;
-	param2 = channel;
-	for (eRFPath = 0; eRFPath < pHalData->NumTotalRFPath; eRFPath++) {
-		pHalData->RfRegChnlVal[eRFPath] = ((pHalData->RfRegChnlVal[eRFPath] & 0xfffffc00) | param2);
-		PHY_SetRFReg(Adapter, (enum rf_radio_path)eRFPath, param1, bRFRegOffsetMask, pHalData->RfRegChnlVal[eRFPath]);
-	}
-}
-
-void PHY_SwChnl8188E(struct adapter *Adapter, u8 channel)
-{
-	/*  Call after initialization */
-	struct hal_data_8188e	*pHalData = GET_HAL_DATA(Adapter);
-	u8 tmpchannel = pHalData->CurrentChannel;
-	bool  bResult = true;
-
-	if (pHalData->rf_chip == RF_PSEUDO_11N)
-		return;		/* return immediately if it is peudo-phy */
-
-	if (channel == 0)
-		channel = 1;
-
-	pHalData->CurrentChannel = channel;
-
-	if ((!Adapter->bDriverStopped) && (!Adapter->bSurpriseRemoved)) {
-		_PHY_SwChnl8192C(Adapter, channel);
-
-		if (bResult)
-			;
-		else
-			pHalData->CurrentChannel = tmpchannel;
-
-	} else {
-		pHalData->CurrentChannel = tmpchannel;
-	}
-}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c b/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
deleted file mode 100644
index 8ce9d0e4eeff..000000000000
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_rf6052.c
+++ /dev/null
@@ -1,527 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-/******************************************************************************
- *
- *
- * Module:	rtl8192c_rf6052.c	( Source C File)
- *
- * Note:	Provide RF 6052 series relative API.
- *
- * Function:
- *
- * Export:
- *
- * Abbrev:
- *
- * History:
- * Data			Who		Remark
- *
- * 09/25/2008	MHC		Create initial version.
- * 11/05/2008	MHC		Add API for tw power setting.
- *
- *
-******************************************************************************/
-
-#define _RTL8188E_RF6052_C_
-
-#include <osdep_service.h>
-#include <drv_types.h>
-
-#include <rtl8188e_hal.h>
-
-/*-----------------------------------------------------------------------------
- * Function:    PHY_RF6052SetBandwidth()
- *
- * Overview:    This function is called by SetBWModeCallback8190Pci() only
- *
- * Input:       struct adapter *Adapter
- *			WIRELESS_BANDWIDTH_E	Bandwidth	20M or 40M
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Note:		For RF type 0222D
- *---------------------------------------------------------------------------*/
-void rtl8188e_PHY_RF6052SetBandwidth(struct adapter *Adapter,
-				     enum ht_channel_width Bandwidth)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-
-	switch (Bandwidth) {
-	case HT_CHANNEL_WIDTH_20:
-		pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff3ff) | BIT(10) | BIT(11));
-		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
-		break;
-	case HT_CHANNEL_WIDTH_40:
-		pHalData->RfRegChnlVal[0] = ((pHalData->RfRegChnlVal[0] & 0xfffff3ff) | BIT(10));
-		PHY_SetRFReg(Adapter, RF_PATH_A, RF_CHNLBW, bRFRegOffsetMask, pHalData->RfRegChnlVal[0]);
-		break;
-	default:
-		break;
-	}
-}
-
-/*-----------------------------------------------------------------------------
- * Function:	PHY_RF6052SetCckTxPower
- *
- * Overview:
- *
- * Input:       NONE
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Revised History:
- * When			Who		Remark
- * 11/05/2008	MHC		Simulate 8192series..
- *
- *---------------------------------------------------------------------------*/
-
-void
-rtl8188e_PHY_RF6052SetCckTxPower(
-		struct adapter *Adapter,
-		u8 *pPowerlevel)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	struct dm_priv *pdmpriv = &pHalData->dmpriv;
-	struct mlme_ext_priv *pmlmeext = &Adapter->mlmeextpriv;
-	u32 TxAGC[2] = {0, 0}, tmpval = 0, pwrtrac_value;
-	bool TurboScanOff = false;
-	u8 idx1, idx2;
-	u8 *ptr;
-	u8 direction;
-	/* FOR CE ,must disable turbo scan */
-	TurboScanOff = true;
-
-
-	if (pmlmeext->sitesurvey_res.state == SCAN_PROCESS) {
-		TxAGC[RF_PATH_A] = 0x3f3f3f3f;
-		TxAGC[RF_PATH_B] = 0x3f3f3f3f;
-
-		TurboScanOff = true;/* disable turbo scan */
-
-		if (TurboScanOff) {
-			for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
-				TxAGC[idx1] =
-					pPowerlevel[idx1] | (pPowerlevel[idx1]<<8) |
-					(pPowerlevel[idx1]<<16) | (pPowerlevel[idx1]<<24);
-				/*  2010/10/18 MH For external PA module. We need to limit power index to be less than 0x20. */
-				if (TxAGC[idx1] > 0x20 && pHalData->ExternalPA)
-					TxAGC[idx1] = 0x20;
-			}
-		}
-	} else {
-		/* Driver dynamic Tx power shall not affect Tx power.
-		 * It shall be determined by power training mechanism.
-i		 *  Currently, we cannot fully disable driver dynamic
-		 * tx power mechanism because it is referenced by BT
-		 * coexist mechanism.
-		 * In the future, two mechanism shall be separated from
-		 * each other and maintained independently. */
-		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1) {
-			TxAGC[RF_PATH_A] = 0x10101010;
-			TxAGC[RF_PATH_B] = 0x10101010;
-		} else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level2) {
-			TxAGC[RF_PATH_A] = 0x00000000;
-			TxAGC[RF_PATH_B] = 0x00000000;
-		} else {
-			for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
-				TxAGC[idx1] =
-					pPowerlevel[idx1] | (pPowerlevel[idx1]<<8) |
-					(pPowerlevel[idx1]<<16) | (pPowerlevel[idx1]<<24);
-			}
-			if (pHalData->EEPROMRegulatory == 0) {
-				tmpval = (pHalData->MCSTxPowerLevelOriginalOffset[0][6]) +
-						(pHalData->MCSTxPowerLevelOriginalOffset[0][7]<<8);
-				TxAGC[RF_PATH_A] += tmpval;
-
-				tmpval = (pHalData->MCSTxPowerLevelOriginalOffset[0][14]) +
-						(pHalData->MCSTxPowerLevelOriginalOffset[0][15]<<24);
-				TxAGC[RF_PATH_B] += tmpval;
-			}
-		}
-	}
-	for (idx1 = RF_PATH_A; idx1 <= RF_PATH_B; idx1++) {
-		ptr = (u8 *)(&(TxAGC[idx1]));
-		for (idx2 = 0; idx2 < 4; idx2++) {
-			if (*ptr > RF6052_MAX_TX_PWR)
-				*ptr = RF6052_MAX_TX_PWR;
-			ptr++;
-		}
-	}
-	ODM_TxPwrTrackAdjust88E(&pHalData->odmpriv, 1, &direction, &pwrtrac_value);
-
-	if (direction == 1) {
-		/*  Increase TX power */
-		TxAGC[0] += pwrtrac_value;
-		TxAGC[1] += pwrtrac_value;
-	} else if (direction == 2) {
-		/*  Decrease TX power */
-		TxAGC[0] -=  pwrtrac_value;
-		TxAGC[1] -=  pwrtrac_value;
-	}
-
-	/*  rf-A cck tx power */
-	tmpval = TxAGC[RF_PATH_A]&0xff;
-	PHY_SetBBReg(Adapter, rTxAGC_A_CCK1_Mcs32, bMaskByte1, tmpval);
-	tmpval = TxAGC[RF_PATH_A]>>8;
-	PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, 0xffffff00, tmpval);
-
-	/*  rf-B cck tx power */
-	tmpval = TxAGC[RF_PATH_B]>>24;
-	PHY_SetBBReg(Adapter, rTxAGC_B_CCK11_A_CCK2_11, bMaskByte0, tmpval);
-	tmpval = TxAGC[RF_PATH_B]&0x00ffffff;
-	PHY_SetBBReg(Adapter, rTxAGC_B_CCK1_55_Mcs32, 0xffffff00, tmpval);
-}	/* PHY_RF6052SetCckTxPower */
-
-/*  */
-/*  powerbase0 for OFDM rates */
-/*  powerbase1 for HT MCS rates */
-/*  */
-static void getpowerbase88e(struct adapter *Adapter, u8 *pPowerLevelOFDM,
-			    u8 *pPowerLevelBW20, u8 *pPowerLevelBW40, u8 Channel, u32 *OfdmBase, u32 *MCSBase)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	u32 powerBase0, powerBase1;
-	u8 i, powerlevel[2];
-
-	for (i = 0; i < 2; i++) {
-		powerBase0 = pPowerLevelOFDM[i];
-
-		powerBase0 = (powerBase0<<24) | (powerBase0<<16) | (powerBase0<<8) | powerBase0;
-		*(OfdmBase+i) = powerBase0;
-	}
-	for (i = 0; i < pHalData->NumTotalRFPath; i++) {
-		/* Check HT20 to HT40 diff */
-		if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
-			powerlevel[i] = pPowerLevelBW20[i];
-		else
-			powerlevel[i] = pPowerLevelBW40[i];
-		powerBase1 = powerlevel[i];
-		powerBase1 = (powerBase1<<24) | (powerBase1<<16) | (powerBase1<<8) | powerBase1;
-		*(MCSBase+i) = powerBase1;
-	}
-}
-static void get_rx_power_val_by_reg(struct adapter *Adapter, u8 Channel,
-				    u8 index, u32 *powerBase0, u32 *powerBase1,
-				    u32 *pOutWriteVal)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	struct dm_priv	*pdmpriv = &pHalData->dmpriv;
-	u8	i, chnlGroup = 0, pwr_diff_limit[4], customer_pwr_limit;
-	s8	pwr_diff = 0;
-	u32	writeVal, customer_limit, rf;
-	u8	Regulatory = pHalData->EEPROMRegulatory;
-
-	/*  Index 0 & 1= legacy OFDM, 2-5=HT_MCS rate */
-
-	for (rf = 0; rf < 2; rf++) {
-		switch (Regulatory) {
-		case 0:	/*  Realtek better performance */
-				/*  increase power diff defined by Realtek for large power */
-			chnlGroup = 0;
-			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf ? 8 : 0)] +
-				((index < 2) ? powerBase0[rf] : powerBase1[rf]);
-			break;
-		case 1:	/*  Realtek regulatory */
-			/*  increase power diff defined by Realtek for regulatory */
-			if (pHalData->pwrGroupCnt == 1)
-				chnlGroup = 0;
-			if (pHalData->pwrGroupCnt >= pHalData->PGMaxGroup) {
-				if (Channel < 3)			/*  Channel 1-2 */
-					chnlGroup = 0;
-				else if (Channel < 6)		/*  Channel 3-5 */
-					chnlGroup = 1;
-				else	 if (Channel < 9)		/*  Channel 6-8 */
-					chnlGroup = 2;
-				else if (Channel < 12)		/*  Channel 9-11 */
-					chnlGroup = 3;
-				else if (Channel < 14)		/*  Channel 12-13 */
-					chnlGroup = 4;
-				else if (Channel == 14)		/*  Channel 14 */
-					chnlGroup = 5;
-			}
-			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf ? 8 : 0)] +
-					((index < 2) ? powerBase0[rf] : powerBase1[rf]);
-			break;
-		case 2:	/*  Better regulatory */
-				/*  don't increase any power diff */
-			writeVal = ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
-			break;
-		case 3:	/*  Customer defined power diff. */
-				/*  increase power diff defined by customer. */
-			chnlGroup = 0;
-
-			if (index < 2)
-				pwr_diff = pHalData->TxPwrLegacyHtDiff[rf][Channel-1];
-			else if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_20)
-				pwr_diff = pHalData->TxPwrHt20Diff[rf][Channel-1];
-
-			if (pHalData->CurrentChannelBW == HT_CHANNEL_WIDTH_40)
-				customer_pwr_limit = pHalData->PwrGroupHT40[rf][Channel-1];
-			else
-				customer_pwr_limit = pHalData->PwrGroupHT20[rf][Channel-1];
-
-			if (pwr_diff >= customer_pwr_limit)
-				pwr_diff = 0;
-			else
-				pwr_diff = customer_pwr_limit - pwr_diff;
-
-			for (i = 0; i < 4; i++) {
-				pwr_diff_limit[i] = (u8)((pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf ? 8 : 0)]&(0x7f<<(i*8)))>>(i*8));
-
-				if (pwr_diff_limit[i] > pwr_diff)
-					pwr_diff_limit[i] = pwr_diff;
-			}
-			customer_limit = (pwr_diff_limit[3]<<24) | (pwr_diff_limit[2]<<16) |
-					 (pwr_diff_limit[1]<<8) | (pwr_diff_limit[0]);
-			writeVal = customer_limit + ((index < 2) ? powerBase0[rf] : powerBase1[rf]);
-			break;
-		default:
-			chnlGroup = 0;
-			writeVal = pHalData->MCSTxPowerLevelOriginalOffset[chnlGroup][index+(rf ? 8 : 0)] +
-					((index < 2) ? powerBase0[rf] : powerBase1[rf]);
-			break;
-		}
-/*  20100427 Joseph: Driver dynamic Tx power shall not affect Tx power. It shall be determined by power training mechanism. */
-/*  Currently, we cannot fully disable driver dynamic tx power mechanism because it is referenced by BT coexist mechanism. */
-/*  In the future, two mechanism shall be separated from each other and maintained independently. Thanks for Lanhsin's reminder. */
-		/* 92d do not need this */
-		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level1)
-			writeVal = 0x14141414;
-		else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_Level2)
-			writeVal = 0x00000000;
-
-		/*  20100628 Joseph: High power mode for BT-Coexist mechanism. */
-		/*  This mechanism is only applied when Driver-Highpower-Mechanism is OFF. */
-		if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_BT1)
-			writeVal = writeVal - 0x06060606;
-		else if (pdmpriv->DynamicTxHighPowerLvl == TxHighPwrLevel_BT2)
-			writeVal = writeVal;
-		*(pOutWriteVal+rf) = writeVal;
-	}
-}
-static void writeOFDMPowerReg88E(struct adapter *Adapter, u8 index, u32 *pValue)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	u16 regoffset_a[6] = {
-		rTxAGC_A_Rate18_06, rTxAGC_A_Rate54_24,
-		rTxAGC_A_Mcs03_Mcs00, rTxAGC_A_Mcs07_Mcs04,
-		rTxAGC_A_Mcs11_Mcs08, rTxAGC_A_Mcs15_Mcs12};
-	u16 regoffset_b[6] = {
-		rTxAGC_B_Rate18_06, rTxAGC_B_Rate54_24,
-		rTxAGC_B_Mcs03_Mcs00, rTxAGC_B_Mcs07_Mcs04,
-		rTxAGC_B_Mcs11_Mcs08, rTxAGC_B_Mcs15_Mcs12};
-	u8 i, rf, pwr_val[4];
-	u32 writeVal;
-	u16 regoffset;
-
-	for (rf = 0; rf < 2; rf++) {
-		writeVal = pValue[rf];
-		for (i = 0; i < 4; i++) {
-			pwr_val[i] = (u8)((writeVal & (0x7f<<(i*8)))>>(i*8));
-			if (pwr_val[i]  > RF6052_MAX_TX_PWR)
-				pwr_val[i]  = RF6052_MAX_TX_PWR;
-		}
-		writeVal = (pwr_val[3]<<24) | (pwr_val[2]<<16) | (pwr_val[1]<<8) | pwr_val[0];
-
-		if (rf == 0)
-			regoffset = regoffset_a[index];
-		else
-			regoffset = regoffset_b[index];
-
-		PHY_SetBBReg(Adapter, regoffset, bMaskDWord, writeVal);
-
-		/*  201005115 Joseph: Set Tx Power diff for Tx power training mechanism. */
-		if (((pHalData->rf_type == RF_2T2R) &&
-		     (regoffset == rTxAGC_A_Mcs15_Mcs12 || regoffset == rTxAGC_B_Mcs15_Mcs12)) ||
-		    ((pHalData->rf_type != RF_2T2R) &&
-		     (regoffset == rTxAGC_A_Mcs07_Mcs04 || regoffset == rTxAGC_B_Mcs07_Mcs04))) {
-			writeVal = pwr_val[3];
-			if (regoffset == rTxAGC_A_Mcs15_Mcs12 || regoffset == rTxAGC_A_Mcs07_Mcs04)
-				regoffset = 0xc90;
-			if (regoffset == rTxAGC_B_Mcs15_Mcs12 || regoffset == rTxAGC_B_Mcs07_Mcs04)
-				regoffset = 0xc98;
-			for (i = 0; i < 3; i++) {
-				if (i != 2)
-					writeVal = (writeVal > 8) ? (writeVal-8) : 0;
-				else
-					writeVal = (writeVal > 6) ? (writeVal-6) : 0;
-				usb_write8(Adapter, (u32)(regoffset+i), (u8)writeVal);
-			}
-		}
-	}
-}
-
-/*-----------------------------------------------------------------------------
- * Function:	PHY_RF6052SetOFDMTxPower
- *
- * Overview:	For legacy and HY OFDM, we must read EEPROM TX power index for
- *			different channel and read original value in TX power register area from
- *			0xe00. We increase offset and original value to be correct tx pwr.
- *
- * Input:       NONE
- *
- * Output:      NONE
- *
- * Return:      NONE
- *
- * Revised History:
- * When			Who		Remark
- * 11/05/2008	MHC		Simulate 8192 series method.
- * 01/06/2009	MHC		1. Prevent Path B tx power overflow or underflow dure to
- *						A/B pwr difference or legacy/HT pwr diff.
- *						2. We concern with path B legacy/HT OFDM difference.
- * 01/22/2009	MHC		Support new EPRO format from SD3.
- *
- *---------------------------------------------------------------------------*/
-
-void
-rtl8188e_PHY_RF6052SetOFDMTxPower(
-		struct adapter *Adapter,
-		u8 *pPowerLevelOFDM,
-		u8 *pPowerLevelBW20,
-		u8 *pPowerLevelBW40,
-		u8 Channel)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	u32 writeVal[2], powerBase0[2], powerBase1[2], pwrtrac_value;
-	u8 direction;
-	u8 index = 0;
-
-	getpowerbase88e(Adapter, pPowerLevelOFDM, pPowerLevelBW20, pPowerLevelBW40, Channel, &powerBase0[0], &powerBase1[0]);
-
-	/*  2012/04/23 MH According to power tracking value, we need to revise OFDM tx power. */
-	/*  This is ued to fix unstable power tracking mode. */
-	ODM_TxPwrTrackAdjust88E(&pHalData->odmpriv, 0, &direction, &pwrtrac_value);
-
-	for (index = 0; index < 6; index++) {
-		get_rx_power_val_by_reg(Adapter, Channel, index,
-					&powerBase0[0], &powerBase1[0],
-					&writeVal[0]);
-
-		if (direction == 1) {
-			writeVal[0] += pwrtrac_value;
-			writeVal[1] += pwrtrac_value;
-		} else if (direction == 2) {
-			writeVal[0] -= pwrtrac_value;
-			writeVal[1] -= pwrtrac_value;
-		}
-		writeOFDMPowerReg88E(Adapter, index, &writeVal[0]);
-	}
-}
-
-static int phy_RF6052_Config_ParaFile(struct adapter *Adapter)
-{
-	struct bb_reg_def *pPhyReg;
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	u32 u4RegValue = 0;
-	u8 eRFPath;
-	int rtStatus = _SUCCESS;
-
-	/* 3----------------------------------------------------------------- */
-	/* 3 <2> Initialize RF */
-	/* 3----------------------------------------------------------------- */
-	for (eRFPath = 0; eRFPath < pHalData->NumTotalRFPath; eRFPath++) {
-		pPhyReg = &pHalData->PHYRegDef[eRFPath];
-
-		/*----Store original RFENV control type----*/
-		switch (eRFPath) {
-		case RF_PATH_A:
-		case RF_PATH_C:
-			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV);
-			break;
-		case RF_PATH_B:
-		case RF_PATH_D:
-			u4RegValue = PHY_QueryBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV<<16);
-			break;
-		}
-		/*----Set RF_ENV enable----*/
-		PHY_SetBBReg(Adapter, pPhyReg->rfintfe, bRFSI_RFENV<<16, 0x1);
-		udelay(1);/* PlatformStallExecution(1); */
-
-		/*----Set RF_ENV output high----*/
-		PHY_SetBBReg(Adapter, pPhyReg->rfintfo, bRFSI_RFENV, 0x1);
-		udelay(1);/* PlatformStallExecution(1); */
-
-		/* Set bit number of Address and Data for RF register */
-		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireAddressLength, 0x0);	/*  Set 1 to 4 bits for 8255 */
-		udelay(1);/* PlatformStallExecution(1); */
-
-		PHY_SetBBReg(Adapter, pPhyReg->rfHSSIPara2, b3WireDataLength, 0x0);	/*  Set 0 to 12  bits for 8255 */
-		udelay(1);/* PlatformStallExecution(1); */
-
-		/*----Initialize RF fom connfiguration file----*/
-		switch (eRFPath) {
-		case RF_PATH_A:
-			if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, (enum rf_radio_path)eRFPath, (enum rf_radio_path)eRFPath))
-				rtStatus = _FAIL;
-			break;
-		case RF_PATH_B:
-		if (HAL_STATUS_FAILURE == ODM_ConfigRFWithHeaderFile(&pHalData->odmpriv, (enum rf_radio_path)eRFPath, (enum rf_radio_path)eRFPath))
-				rtStatus = _FAIL;
-			break;
-		case RF_PATH_C:
-			break;
-		case RF_PATH_D:
-			break;
-		}
-		/*----Restore RFENV control type----*/;
-		switch (eRFPath) {
-		case RF_PATH_A:
-		case RF_PATH_C:
-			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV, u4RegValue);
-			break;
-		case RF_PATH_B:
-		case RF_PATH_D:
-			PHY_SetBBReg(Adapter, pPhyReg->rfintfs, bRFSI_RFENV<<16, u4RegValue);
-			break;
-		}
-		if (rtStatus != _SUCCESS)
-			goto phy_RF6052_Config_ParaFile_Fail;
-	}
-	return rtStatus;
-
-phy_RF6052_Config_ParaFile_Fail:
-	return rtStatus;
-}
-
-int PHY_RF6052_Config8188E(struct adapter *Adapter)
-{
-	struct hal_data_8188e *pHalData = GET_HAL_DATA(Adapter);
-	int rtStatus = _SUCCESS;
-
-	/*  */
-	/*  Initialize general global value */
-	/*  */
-	/*  TODO: Extend RF_PATH_C and RF_PATH_D in the future */
-	if (pHalData->rf_type == RF_1T1R)
-		pHalData->NumTotalRFPath = 1;
-	else
-		pHalData->NumTotalRFPath = 2;
-
-	/*  */
-	/*  Config BB and RF */
-	/*  */
-	rtStatus = phy_RF6052_Config_ParaFile(Adapter);
-	return rtStatus;
-}
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c b/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c
index 7ecbcf731ea9..7a4f754d86df 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188e_xmit.c
@@ -64,6 +64,7 @@ void _dbg_dump_tx_info(struct adapter *padapter, int frame_tag,
 {
 	u8 dmp_txpkt;
 	bool dump_txdesc = false;
+
 	rtw_hal_get_def_var(padapter, HAL_DEF_DBG_DUMP_TXPKT, &(dmp_txpkt));
 
 	if (dmp_txpkt == 1) {/* dump txdesc for data frame */
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
index f25c87c63250..bc275b2a7d37 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_recv.c
@@ -41,15 +41,16 @@ int	rtl8188eu_init_recv_priv(struct adapter *padapter)
 	/* init recv_buf */
 	_rtw_init_queue(&precvpriv->free_recv_buf_queue);
 
-	precvpriv->pallocated_recv_buf = kzalloc(NR_RECVBUFF * sizeof(struct recv_buf) + 4, GFP_KERNEL);
+	precvpriv->pallocated_recv_buf =
+		kzalloc(NR_RECVBUFF * sizeof(struct recv_buf), GFP_KERNEL);
 	if (precvpriv->pallocated_recv_buf == NULL) {
 		res = _FAIL;
-		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_, ("alloc recv_buf fail!\n"));
+		RT_TRACE(_module_rtl871x_recv_c_, _drv_err_,
+				("alloc recv_buf fail!\n"));
 		goto exit;
 	}
-	memset(precvpriv->pallocated_recv_buf, 0, NR_RECVBUFF * sizeof(struct recv_buf) + 4);
 
-	precvpriv->precv_buf = (u8 *)N_BYTE_ALIGMENT((size_t)(precvpriv->pallocated_recv_buf), 4);
+	precvpriv->precv_buf = precvpriv->pallocated_recv_buf;
 
 
 	precvbuf = (struct recv_buf *)precvpriv->precv_buf;
@@ -66,20 +67,23 @@ int	rtl8188eu_init_recv_priv(struct adapter *padapter)
 	{
 		int i;
 		size_t tmpaddr = 0;
-		size_t alignment = 0;
+		size_t alignm = 0;
 		struct sk_buff *pskb = NULL;
 
 		skb_queue_head_init(&precvpriv->free_recv_skb_queue);
 
 		for (i = 0; i < NR_PREALLOC_RECV_SKB; i++) {
-			pskb = __netdev_alloc_skb(padapter->pnetdev, MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ, GFP_KERNEL);
+			pskb = __netdev_alloc_skb(padapter->pnetdev,
+					MAX_RECVBUF_SZ + RECVBUFF_ALIGN_SZ,
+					GFP_KERNEL);
 			if (pskb) {
 				pskb->dev = padapter->pnetdev;
 				tmpaddr = (size_t)pskb->data;
-				alignment = tmpaddr & (RECVBUFF_ALIGN_SZ-1);
-				skb_reserve(pskb, (RECVBUFF_ALIGN_SZ - alignment));
+				alignm = tmpaddr & (RECVBUFF_ALIGN_SZ-1);
+				skb_reserve(pskb, (RECVBUFF_ALIGN_SZ - alignm));
 
-				skb_queue_tail(&precvpriv->free_recv_skb_queue, pskb);
+				skb_queue_tail(&precvpriv->free_recv_skb_queue,
+						pskb);
 			}
 			pskb = NULL;
 		}
@@ -109,7 +113,8 @@ void rtl8188eu_free_recv_priv(struct adapter *padapter)
 
 
 	if (skb_queue_len(&precvpriv->free_recv_skb_queue))
-		DBG_88E(KERN_WARNING "free_recv_skb_queue not empty, %d\n", skb_queue_len(&precvpriv->free_recv_skb_queue));
+		DBG_88E(KERN_WARNING "free_recv_skb_queue not empty, %d\n",
+				skb_queue_len(&precvpriv->free_recv_skb_queue));
 
 	skb_queue_purge(&precvpriv->free_recv_skb_queue);
 }
diff --git a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
index 3494ac697456..be9eede6931d 100644
--- a/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
+++ b/drivers/staging/rtl8188eu/hal/rtl8188eu_xmit.c
@@ -336,7 +336,8 @@ static s32 update_txdesc(struct xmit_frame *pxmitframe, u8 *pmem, s32 sz, u8 bag
 		ptxdesc->txdw4 |= cpu_to_le32(HW_SSN);	/*  Hw set sequence number */
 	}
 
-	ODM_SetTxAntByTxInfo_88E(&haldata->odmpriv, pmem, pattrib->mac_id);
+	rtl88eu_dm_set_tx_ant_by_tx_info(&haldata->odmpriv, pmem,
+					 pattrib->mac_id);
 
 	rtl8188eu_cal_txdesc_chksum(ptxdesc);
 	_dbg_dump_tx_info(adapt, pxmitframe->frame_tag, ptxdesc);
diff --git a/drivers/staging/rtl8188eu/hal/usb_halinit.c b/drivers/staging/rtl8188eu/hal/usb_halinit.c
index e18393317bdc..caf2ca3a47e7 100644
--- a/drivers/staging/rtl8188eu/hal/usb_halinit.c
+++ b/drivers/staging/rtl8188eu/hal/usb_halinit.c
@@ -22,15 +22,14 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 #include <rtw_efuse.h>
-
+#include <fw.h>
 #include <rtl8188e_hal.h>
 #include <rtl8188e_led.h>
 #include <rtw_iol.h>
 #include <usb_hal.h>
+#include <phy.h>
 
-#define		HAL_MAC_ENABLE	1
 #define		HAL_BB_ENABLE		1
-#define		HAL_RF_ENABLE		1
 
 static void _ConfigNormalChipOutEP_8188E(struct adapter *adapt, u8 NumOutPipe)
 {
@@ -108,7 +107,9 @@ static u32 rtl8188eu_InitPowerOn(struct adapter *adapt)
 	if (haldata->bMacPwrCtrlOn)
 		return _SUCCESS;
 
-	if (!HalPwrSeqCmdParsing(adapt, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, Rtl8188E_NIC_PWR_ON_FLOW)) {
+	if (!rtl88eu_pwrseqcmdparsing(adapt, PWR_CUT_ALL_MSK,
+				      PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,
+				      Rtl8188E_NIC_PWR_ON_FLOW)) {
 		DBG_88E(KERN_ERR "%s: run power on flow fail\n", __func__);
 		return _FAIL;
 	}
@@ -614,8 +615,8 @@ static void _BeaconFunctionEnable(struct adapter *Adapter,
 /*  Set CCK and OFDM Block "ON" */
 static void _BBTurnOnBlock(struct adapter *Adapter)
 {
-	PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bCCKEn, 0x1);
-	PHY_SetBBReg(Adapter, rFPGA0_RFMOD, bOFDMEn, 0x1);
+	phy_set_bb_reg(Adapter, rFPGA0_RFMOD, bCCKEn, 0x1);
+	phy_set_bb_reg(Adapter, rFPGA0_RFMOD, bOFDMEn, 0x1);
 }
 
 enum {
@@ -632,9 +633,9 @@ static void _InitAntenna_Selection(struct adapter *Adapter)
 	DBG_88E("==>  %s ....\n", __func__);
 
 	usb_write32(Adapter, REG_LEDCFG0, usb_read32(Adapter, REG_LEDCFG0)|BIT23);
-	PHY_SetBBReg(Adapter, rFPGA0_XAB_RFParameter, BIT13, 0x01);
+	phy_set_bb_reg(Adapter, rFPGA0_XAB_RFParameter, BIT13, 0x01);
 
-	if (PHY_QueryBBReg(Adapter, rFPGA0_XA_RFInterfaceOE, 0x300) == Antenna_A)
+	if (phy_query_bb_reg(Adapter, rFPGA0_XA_RFInterfaceOE, 0x300) == Antenna_A)
 		haldata->CurAntenna = Antenna_A;
 	else
 		haldata->CurAntenna = Antenna_B;
@@ -693,14 +694,14 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	if (Adapter->pwrctrlpriv.bkeepfwalive) {
 
 		if (haldata->odmpriv.RFCalibrateInfo.bIQKInitialized) {
-			PHY_IQCalibrate_8188E(Adapter, true);
+			rtl88eu_phy_iq_calibrate(Adapter, true);
 		} else {
-			PHY_IQCalibrate_8188E(Adapter, false);
+			rtl88eu_phy_iq_calibrate(Adapter, false);
 			haldata->odmpriv.RFCalibrateInfo.bIQKInitialized = true;
 		}
 
 		ODM_TXPowerTrackingCheck(&haldata->odmpriv);
-		PHY_LCCalibrate_8188E(Adapter);
+		rtl88eu_phy_lc_calibrate(Adapter);
 
 		goto exit;
 	}
@@ -744,9 +745,9 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 		Adapter->bFWReady = false;
 		haldata->fw_ractrl = false;
 	} else {
-		status = rtl8188e_FirmwareDownload(Adapter);
+		status = rtl88eu_download_fw(Adapter);
 
-		if (status != _SUCCESS) {
+		if (status) {
 			DBG_88E("%s: Download Firmware failed!!\n", __func__);
 			Adapter->bFWReady = false;
 			haldata->fw_ractrl = false;
@@ -759,35 +760,11 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	}
 	rtl8188e_InitializeFirmwareVars(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MAC);
-#if (HAL_MAC_ENABLE == 1)
-	status = PHY_MACConfig8188E(Adapter);
-	if (status == _FAIL) {
-		DBG_88E(" ### Failed to init MAC ......\n ");
-		goto exit;
-	}
-#endif
+	rtl88eu_phy_mac_config(Adapter);
 
-	/*  */
-	/* d. Initialize BB related configurations. */
-	/*  */
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_BB);
-#if (HAL_BB_ENABLE == 1)
-	status = PHY_BBConfig8188E(Adapter);
-	if (status == _FAIL) {
-		DBG_88E(" ### Failed to init BB ......\n ");
-		goto exit;
-	}
-#endif
+	rtl88eu_phy_bb_config(Adapter);
 
-	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_RF);
-#if (HAL_RF_ENABLE == 1)
-	status = PHY_RFConfig8188E(Adapter);
-	if (status == _FAIL) {
-		DBG_88E(" ### Failed to init RF ......\n ");
-		goto exit;
-	}
-#endif
+	rtl88eu_phy_rf_config(Adapter);
 
 	HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_EFUSE_PATCH);
 	status = rtl8188e_iol_efuse_patch(Adapter);
@@ -843,8 +820,8 @@ static u32 rtl8188eu_hal_init(struct adapter *Adapter)
 	usb_write16(Adapter, REG_PKT_BE_BK_LIFE_TIME, 0x0400);	/*  unit: 256us. 256ms */
 
 	/* Keep RfRegChnlVal for later use. */
-	haldata->RfRegChnlVal[0] = PHY_QueryRFReg(Adapter, (enum rf_radio_path)0, RF_CHNLBW, bRFRegOffsetMask);
-	haldata->RfRegChnlVal[1] = PHY_QueryRFReg(Adapter, (enum rf_radio_path)1, RF_CHNLBW, bRFRegOffsetMask);
+	haldata->RfRegChnlVal[0] = phy_query_rf_reg(Adapter, (enum rf_radio_path)0, RF_CHNLBW, bRFRegOffsetMask);
+	haldata->RfRegChnlVal[1] = phy_query_rf_reg(Adapter, (enum rf_radio_path)1, RF_CHNLBW, bRFRegOffsetMask);
 
 HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_TURN_ON_BLOCK);
 	_BBTurnOnBlock(Adapter);
@@ -854,7 +831,7 @@ HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_SECURITY);
 
 HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_MISC11);
 	/*  2010/12/17 MH We need to set TX power according to EFUSE content at first. */
-	PHY_SetTxPowerLevel8188E(Adapter, haldata->CurrentChannel);
+	phy_set_tx_power_level(Adapter, haldata->CurrentChannel);
 
 /*  Move by Neo for USB SS to below setp */
 /* _RfPowerSave(Adapter); */
@@ -905,9 +882,9 @@ HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_IQK);
 		/*  2010/08/26 MH Merge from 8192CE. */
 	if (pwrctrlpriv->rf_pwrstate == rf_on) {
 		if (haldata->odmpriv.RFCalibrateInfo.bIQKInitialized) {
-				PHY_IQCalibrate_8188E(Adapter, true);
+				rtl88eu_phy_iq_calibrate(Adapter, true);
 		} else {
-			PHY_IQCalibrate_8188E(Adapter, false);
+			rtl88eu_phy_iq_calibrate(Adapter, false);
 			haldata->odmpriv.RFCalibrateInfo.bIQKInitialized = true;
 		}
 
@@ -916,7 +893,7 @@ HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_PW_TRACK);
 		ODM_TXPowerTrackingCheck(&haldata->odmpriv);
 
 HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_LCK);
-			PHY_LCCalibrate_8188E(Adapter);
+			rtl88eu_phy_lc_calibrate(Adapter);
 	}
 
 /* HAL_INIT_PROFILE_TAG(HAL_INIT_STAGES_INIT_PABIAS); */
@@ -950,7 +927,9 @@ static void CardDisableRTL8188EU(struct adapter *Adapter)
 	usb_write8(Adapter, REG_CR, 0x0);
 
 	/*  Run LPS WL RFOFF flow */
-	HalPwrSeqCmdParsing(Adapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, Rtl8188E_NIC_LPS_ENTER_FLOW);
+	rtl88eu_pwrseqcmdparsing(Adapter, PWR_CUT_ALL_MSK,
+				 PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,
+				 Rtl8188E_NIC_LPS_ENTER_FLOW);
 
 	/*  2. 0x1F[7:0] = 0		turn off RF */
 
@@ -971,7 +950,9 @@ static void CardDisableRTL8188EU(struct adapter *Adapter)
 	usb_write8(Adapter, REG_32K_CTRL, val8&(~BIT0));
 
 	/*  Card disable power action flow */
-	HalPwrSeqCmdParsing(Adapter, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, Rtl8188E_NIC_DISABLE_FLOW);
+	rtl88eu_pwrseqcmdparsing(Adapter, PWR_CUT_ALL_MSK,
+				 PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,
+				 Rtl8188E_NIC_DISABLE_FLOW);
 
 	/*  Reset MCU IO Wrapper */
 	val8 = usb_read8(Adapter, REG_RSV_CTRL+1);
@@ -1764,7 +1745,7 @@ static void SetHwReg8188EU(struct adapter *Adapter, u8 variable, u8 *val)
 			/* switch antenna to Optimum_antenna */
 			if (haldata->CurAntenna !=  Optimum_antenna) {
 				Ant = (Optimum_antenna == 2) ? MAIN_ANT : AUX_ANT;
-				ODM_UpdateRxIdleAnt_88E(&haldata->odmpriv, Ant);
+				rtl88eu_dm_update_rx_idle_ant(&haldata->odmpriv, Ant);
 
 				haldata->CurAntenna = Optimum_antenna;
 			}
diff --git a/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h b/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
index 260ea6bf9e83..20e6b40fc35c 100644
--- a/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
+++ b/drivers/staging/rtl8188eu/include/Hal8188EPhyCfg.h
@@ -198,41 +198,15 @@ struct ant_sel_cck {
 /*  */
 /*  BB and RF register read/write */
 /*  */
-u32 rtl8188e_PHY_QueryBBReg(struct adapter *adapter, u32 regaddr, u32 mask);
-void rtl8188e_PHY_SetBBReg(struct adapter *Adapter, u32 RegAddr,
-			   u32 mask, u32 data);
-u32 rtl8188e_PHY_QueryRFReg(struct adapter *adapter, enum rf_radio_path rfpath,
-			    u32 regaddr, u32 mask);
-void rtl8188e_PHY_SetRFReg(struct adapter *adapter, enum rf_radio_path rfpath,
-			   u32 regaddr, u32 mask, u32 data);
-
-/*  Initialization related function */
-/* MAC/BB/RF HAL config */
-int PHY_MACConfig8188E(struct adapter *adapter);
-int PHY_BBConfig8188E(struct adapter *adapter);
-int PHY_RFConfig8188E(struct adapter *adapter);
-
-/* RF config */
-int rtl8188e_PHY_ConfigRFWithParaFile(struct adapter *adapter, u8 *filename,
-				      enum rf_radio_path rfpath);
-int rtl8188e_PHY_ConfigRFWithHeaderFile(struct adapter *adapter,
-					enum rf_radio_path rfpath);
 
 /* Read initi reg value for tx power setting. */
 void rtl8192c_PHY_GetHWRegOriginalValue(struct adapter *adapter);
 
 /*  BB TX Power R/W */
 void PHY_GetTxPowerLevel8188E(struct adapter *adapter, u32 *powerlevel);
-void PHY_SetTxPowerLevel8188E(struct adapter *adapter, u8 channel);
 
 void PHY_ScanOperationBackup8188E(struct adapter *Adapter, u8 Operation);
 
-/*  Switch bandwidth for 8192S */
-void PHY_SetBWMode8188E(struct adapter *adapter,
-			enum ht_channel_width chnlwidth, unsigned char offset);
-
-/*  channel switch related funciton */
-void PHY_SwChnl8188E(struct adapter *adapter, u8 channel);
 /*  Call after initialization */
 void ChkFwCmdIoDone(struct adapter *adapter);
 
@@ -245,19 +219,8 @@ void PHY_EnableHostClkReq(struct adapter *adapter);
 
 bool SetAntennaConfig92C(struct adapter *adapter, u8 defaultant);
 
-void storePwrIndexDiffRateOffset(struct adapter *adapter, u32 regaddr,
-				 u32 mask, u32 data);
 /*--------------------------Exported Function prototype---------------------*/
 
-#define PHY_QueryBBReg(adapt, regaddr, mask)			\
-	 rtl8188e_PHY_QueryBBReg((adapt), (regaddr), (mask))
-#define PHY_SetBBReg(adapt, regaddr, bitmask, data)		\
-	 rtl8188e_PHY_SetBBReg((adapt), (regaddr), (bitmask), (data))
-#define PHY_QueryRFReg(adapt, rfpath, regaddr, bitmask)	\
-	rtl8188e_PHY_QueryRFReg((adapt), (rfpath), (regaddr), (bitmask))
-#define PHY_SetRFReg(adapt, rfpath, regaddr, bitmask, data)	\
-	rtl8188e_PHY_SetRFReg((adapt), (rfpath), (regaddr), (bitmask), (data))
-
 #define PHY_SetMacReg	PHY_SetBBReg
 
 #define	SIC_HW_SUPPORT			0
diff --git a/drivers/staging/rtl8188eu/include/Hal8188EPwrSeq.h b/drivers/staging/rtl8188eu/include/Hal8188EPwrSeq.h
deleted file mode 100644
index aebf1d3aac3b..000000000000
--- a/drivers/staging/rtl8188eu/include/Hal8188EPwrSeq.h
+++ /dev/null
@@ -1,176 +0,0 @@
-
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-
-#ifndef __HAL8188EPWRSEQ_H__
-#define __HAL8188EPWRSEQ_H__
-
-#include "HalPwrSeqCmd.h"
-
-/*
-	Check document WM-20110607-Paul-RTL8188E_Power_Architecture-R02.vsd
-	There are 6 HW Power States:
-	0: POFF--Power Off
-	1: PDN--Power Down
-	2: CARDEMU--Card Emulation
-	3: ACT--Active Mode
-	4: LPS--Low Power State
-	5: SUS--Suspend
-
-	The transision from different states are defined below
-	TRANS_CARDEMU_TO_ACT
-	TRANS_ACT_TO_CARDEMU
-	TRANS_CARDEMU_TO_SUS
-	TRANS_SUS_TO_CARDEMU
-	TRANS_CARDEMU_TO_PDN
-	TRANS_ACT_TO_LPS
-	TRANS_LPS_TO_ACT
-
-	TRANS_END
-
-    PWR SEQ Version: rtl8188E_PwrSeq_V09.h
-*/
-#define	RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS	10
-#define	RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS	10
-#define	RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS	10
-#define	RTL8188E_TRANS_SUS_TO_CARDEMU_STEPS	10
-#define	RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS	10
-#define	RTL8188E_TRANS_PDN_TO_CARDEMU_STEPS	10
-#define	RTL8188E_TRANS_ACT_TO_LPS_STEPS		15
-#define	RTL8188E_TRANS_LPS_TO_ACT_STEPS		15
-#define	RTL8188E_TRANS_END_STEPS		1
-
-
-#define RTL8188E_TRANS_CARDEMU_TO_ACT														\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, BIT1},/* wait till 0x04[17] = 1    power ready*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0|BIT1, 0}, /* 0x02[1:0] = 0	reset BB*/			\
-	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7}, /*0x24[23] = 2b'01 schmit trigger */	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0}, /* 0x04[15] = 0 disable HWPDN (control by DRV)*/\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4|BIT3, 0}, /*0x04[12:11] = 2b'00 disable WL suspend*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0}, /*0x04[8] = 1 polling until return 0*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT0, 0}, /*wait till 0x04[8] = 0*/	\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*LDO normal mode*/	\
-	{0x0074, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*SDIO Driving*/	\
-
-#define RTL8188E_TRANS_ACT_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
-	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},/*0x1F[7:0] = 0 turn off RF*/	\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*LDO Sleep mode*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
-
-#define RTL8188E_TRANS_CARDEMU_TO_SUS													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/				\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01enable WL suspend*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3|BIT4}, /*0x04[12:11] = 2b'11enable WL suspend for PCIe*/	\
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, BIT7}, /*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */	\
-	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*Clear SIC_EN register 0x40[12] = 1'b0 */	\
-	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*Set USB suspend enable local register  0xfe10[4]=1 */	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
-
-#define RTL8188E_TRANS_SUS_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/							\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
-
-#define RTL8188E_TRANS_CARDEMU_TO_CARDDIS													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here*/							\
-	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7}, /*0x24[23] = 2b'01 schmit trigger */	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*  0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */	\
-	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*Clear SIC_EN register 0x40[12] = 1'b0 */	\
-	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4}, /*Set USB suspend enable local register  0xfe10[4]=1 */	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
-
-#define RTL8188E_TRANS_CARDDIS_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/								\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
-
-#define RTL8188E_TRANS_CARDEMU_TO_PDN												\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here*/							\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/* 0x04[16] = 0*/\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7},/* 0x04[15] = 1*/
-
-#define RTL8188E_TRANS_PDN_TO_CARDEMU												\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here					 */ \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},/* 0x04[15] = 0*/
-
-/* This is used by driver for LPSRadioOff Procedure, not for FW LPS Step */
-#define RTL8188E_TRANS_ACT_TO_LPS														\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here				*/   \
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x7F},/*Tx Pause*/	\
-	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled,and clock are gated*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3F},/*Reset MAC TRX*/	\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/	\
-	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5},/*Respond TxOK to scheduler*/	\
-
-
-#define RTL8188E_TRANS_LPS_TO_ACT															\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, comments here				 */ \
-	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK, PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/\
-	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/\
-	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/\
-	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT7, 0}, /*Polling 0x109[7]=0  TSF in 40M*/\
-	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6|BIT7, 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1*/\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
-
-#define RTL8188E_TRANS_END															\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value },  comments here*/					\
-	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, 0, PWR_CMD_END, 0, 0}, /*  */
-
-
-extern struct wl_pwr_cfg rtl8188E_power_on_flow[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_radio_off_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_card_disable_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS+RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_card_enable_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS+RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_suspend_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS+RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_resume_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS+RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_hwpdn_flow[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS+RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_enter_lps_flow[RTL8188E_TRANS_ACT_TO_LPS_STEPS+RTL8188E_TRANS_END_STEPS];
-extern struct wl_pwr_cfg rtl8188E_leave_lps_flow[RTL8188E_TRANS_LPS_TO_ACT_STEPS+RTL8188E_TRANS_END_STEPS];
-
-#endif /* __HAL8188EPWRSEQ_H__ */
diff --git a/drivers/staging/rtl8188eu/include/HalHWImg8188E_BB.h b/drivers/staging/rtl8188eu/include/HalHWImg8188E_BB.h
deleted file mode 100644
index e57452104bfb..000000000000
--- a/drivers/staging/rtl8188eu/include/HalHWImg8188E_BB.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/******************************************************************************
-*
-* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
-*
-* This program is free software; you can redistribute it and/or modify it
-* under the terms of version 2 of the GNU General Public License as
-* published by the Free Software Foundation.
-*
-* This program is distributed in the hope that it will be useful, but WITHOUT
-* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
-* more details.
-*
-* You should have received a copy of the GNU General Public License along with
-* this program; if not, write to the Free Software Foundation, Inc.,
-* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
-*
-*
-******************************************************************************/
-
-#ifndef __INC_BB_8188E_HW_IMG_H
-#define __INC_BB_8188E_HW_IMG_H
-
-/* static bool CheckCondition(const u32 Condition, const u32 Hex); */
-
-/******************************************************************************
-*                           AGC_TAB_1T.TXT
-******************************************************************************/
-
-enum HAL_STATUS ODM_ReadAndConfig_AGC_TAB_1T_8188E(struct odm_dm_struct *odm);
-
-/******************************************************************************
-*                           PHY_REG_1T.TXT
-******************************************************************************/
-
-enum HAL_STATUS ODM_ReadAndConfig_PHY_REG_1T_8188E(struct odm_dm_struct *odm);
-
-/******************************************************************************
-*                           PHY_REG_PG.TXT
-******************************************************************************/
-
-void ODM_ReadAndConfig_PHY_REG_PG_8188E(struct odm_dm_struct *dm_odm);
-
-#endif
diff --git a/drivers/staging/rtl8188eu/include/HalHWImg8188E_MAC.h b/drivers/staging/rtl8188eu/include/HalHWImg8188E_MAC.h
deleted file mode 100644
index acf78b94fddb..000000000000
--- a/drivers/staging/rtl8188eu/include/HalHWImg8188E_MAC.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/******************************************************************************
-*
-* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
-*
-* This program is free software; you can redistribute it and/or modify it
-* under the terms of version 2 of the GNU General Public License as
-* published by the Free Software Foundation.
-*
-* This program is distributed in the hope that it will be useful, but WITHOUT
-* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
-* more details.
-*
-* You should have received a copy of the GNU General Public License along with
-* this program; if not, write to the Free Software Foundation, Inc.,
-* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
-*
-*
-******************************************************************************/
-
-#ifndef __INC_MAC_8188E_HW_IMG_H
-#define __INC_MAC_8188E_HW_IMG_H
-
-/******************************************************************************
-*                           MAC_REG.TXT
-******************************************************************************/
-
-enum HAL_STATUS ODM_ReadAndConfig_MAC_REG_8188E(struct odm_dm_struct *pDM_Odm);
-
-#endif /*  end of HWIMG_SUPPORT */
diff --git a/drivers/staging/rtl8188eu/include/HalHWImg8188E_RF.h b/drivers/staging/rtl8188eu/include/HalHWImg8188E_RF.h
deleted file mode 100644
index 8ecb40d26c70..000000000000
--- a/drivers/staging/rtl8188eu/include/HalHWImg8188E_RF.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/******************************************************************************
-*
-* Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
-*
-* This program is free software; you can redistribute it and/or modify it
-* under the terms of version 2 of the GNU General Public License as
-* published by the Free Software Foundation.
-*
-* This program is distributed in the hope that it will be useful, but WITHOUT
-* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
-* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
-* more details.
-*
-* You should have received a copy of the GNU General Public License along with
-* this program; if not, write to the Free Software Foundation, Inc.,
-* 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
-*
-*
-******************************************************************************/
-
-#ifndef __INC_RF_8188E_HW_IMG_H
-#define __INC_RF_8188E_HW_IMG_H
-
-/******************************************************************************
- *                           RadioA_1T.TXT
- ******************************************************************************/
-
-enum HAL_STATUS ODM_ReadAndConfig_RadioA_1T_8188E(struct odm_dm_struct *odm);
-
-#endif /*  end of HWIMG_SUPPORT */
diff --git a/drivers/staging/rtl8188eu/include/HalPhyRf_8188e.h b/drivers/staging/rtl8188eu/include/HalPhyRf_8188e.h
deleted file mode 100644
index 90a26c119d97..000000000000
--- a/drivers/staging/rtl8188eu/include/HalPhyRf_8188e.h
+++ /dev/null
@@ -1,61 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-
-#ifndef __HAL_PHY_RF_8188E_H__
-#define __HAL_PHY_RF_8188E_H__
-
-/*--------------------------Define Parameters-------------------------------*/
-#define	IQK_DELAY_TIME_88E		10		/* ms */
-#define	index_mapping_NUM_88E	15
-#define AVG_THERMAL_NUM_88E	4
-#define ODM_TARGET_CHNL_NUM_2G_5G	59
-
-void ODM_TxPwrTrackAdjust88E(struct odm_dm_struct *pDM_Odm,
-			     u8 Type,	/* 0 = OFDM, 1 = CCK */
-			     u8 *pDirection,/* 1 = +(incr) 2 = -(decr) */
-			     u32 *pOutWriteVal); /* Tx tracking CCK/OFDM BB
-						     * swing index adjust */
-
-
-void odm_TXPowerTrackingCallback_ThermalMeter_8188E(struct adapter *Adapter);
-
-
-/* 1 7.	IQK */
-
-void PHY_IQCalibrate_8188E(struct adapter *Adapter, bool ReCovery);
-
-/*  LC calibrate */
-void PHY_LCCalibrate_8188E(struct adapter *pAdapter);
-
-/*  AP calibrate */
-void PHY_DigitalPredistortion_8188E(struct adapter *pAdapter);
-
-void _PHY_SaveADDARegisters(struct adapter *pAdapter, u32 *ADDAReg,
-			    u32 *ADDABackup, u32 RegisterNum);
-
-void _PHY_PathADDAOn(struct adapter *pAdapter, u32 *ADDAReg,
-		     bool isPathAOn, bool is2T);
-
-void _PHY_MACSettingCalibration(struct adapter *pAdapter, u32 *MACReg,
-				u32 *MACBackup);
-
-void _PHY_PathAStandBy(struct adapter *pAdapter);
-
-#endif	/*  #ifndef __HAL_PHY_RF_8188E_H__ */
diff --git a/drivers/staging/rtl8188eu/include/HalPwrSeqCmd.h b/drivers/staging/rtl8188eu/include/HalPwrSeqCmd.h
deleted file mode 100644
index d945784ed5d9..000000000000
--- a/drivers/staging/rtl8188eu/include/HalPwrSeqCmd.h
+++ /dev/null
@@ -1,128 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-#ifndef __HALPWRSEQCMD_H__
-#define __HALPWRSEQCMD_H__
-
-#include <drv_types.h>
-
-/*---------------------------------------------*/
-/* 3 The value of cmd: 4 bits */
-/*---------------------------------------------*/
-#define PWR_CMD_READ			0x00
-     /*  offset: the read register offset */
-     /*  msk: the mask of the read value */
-     /*  value: N/A, left by 0 */
-     /*  note: dirver shall implement this function by read & msk */
-
-#define PWR_CMD_WRITE			0x01
-     /*  offset: the read register offset */
-     /*  msk: the mask of the write bits */
-     /*  value: write value */
-     /*  note: driver shall implement this cmd by read & msk after write */
-
-#define PWR_CMD_POLLING			0x02
-     /*  offset: the read register offset */
-     /*  msk: the mask of the polled value */
-     /*  value: the value to be polled, masked by the msd field. */
-     /*  note: driver shall implement this cmd by */
-     /*  do{ */
-     /*  if ( (Read(offset) & msk) == (value & msk) ) */
-     /*  break; */
-     /*  } while (not timeout); */
-
-#define PWR_CMD_DELAY			0x03
-     /*  offset: the value to delay */
-     /*  msk: N/A */
-     /*  value: the unit of delay, 0: us, 1: ms */
-
-#define PWR_CMD_END			0x04
-     /*  offset: N/A */
-     /*  msk: N/A */
-     /*  value: N/A */
-
-/*---------------------------------------------*/
-/* 3 The value of base: 4 bits */
-/*---------------------------------------------*/
-   /*  define the base address of each block */
-#define PWR_BASEADDR_MAC		0x00
-#define PWR_BASEADDR_USB		0x01
-#define PWR_BASEADDR_PCIE		0x02
-#define PWR_BASEADDR_SDIO		0x03
-
-/*---------------------------------------------*/
-/* 3 The value of interface_msk: 4 bits */
-/*---------------------------------------------*/
-#define	PWR_INTF_SDIO_MSK		BIT(0)
-#define	PWR_INTF_USB_MSK		BIT(1)
-#define	PWR_INTF_PCI_MSK		BIT(2)
-#define	PWR_INTF_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-
-/*---------------------------------------------*/
-/* 3 The value of fab_msk: 4 bits */
-/*---------------------------------------------*/
-#define	PWR_FAB_TSMC_MSK		BIT(0)
-#define	PWR_FAB_UMC_MSK			BIT(1)
-#define	PWR_FAB_ALL_MSK			(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-
-/*---------------------------------------------*/
-/* 3 The value of cut_msk: 8 bits */
-/*---------------------------------------------*/
-#define	PWR_CUT_TESTCHIP_MSK		BIT(0)
-#define	PWR_CUT_A_MSK			BIT(1)
-#define	PWR_CUT_B_MSK			BIT(2)
-#define	PWR_CUT_C_MSK			BIT(3)
-#define	PWR_CUT_D_MSK			BIT(4)
-#define	PWR_CUT_E_MSK			BIT(5)
-#define	PWR_CUT_F_MSK			BIT(6)
-#define	PWR_CUT_G_MSK			BIT(7)
-#define	PWR_CUT_ALL_MSK			0xFF
-
-
-enum pwrseq_cmd_delat_unit {
-	PWRSEQ_DELAY_US,
-	PWRSEQ_DELAY_MS,
-};
-
-struct wl_pwr_cfg {
-	u16 offset;
-	u8 cut_msk;
-	u8 fab_msk:4;
-	u8 interface_msk:4;
-	u8 base:4;
-	u8 cmd:4;
-	u8 msk;
-	u8 value;
-};
-
-#define GET_PWR_CFG_OFFSET(__PWR_CMD)		__PWR_CMD.offset
-#define GET_PWR_CFG_CUT_MASK(__PWR_CMD)		__PWR_CMD.cut_msk
-#define GET_PWR_CFG_FAB_MASK(__PWR_CMD)		__PWR_CMD.fab_msk
-#define GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
-#define GET_PWR_CFG_BASE(__PWR_CMD)		__PWR_CMD.base
-#define GET_PWR_CFG_CMD(__PWR_CMD)		__PWR_CMD.cmd
-#define GET_PWR_CFG_MASK(__PWR_CMD)		__PWR_CMD.msk
-#define GET_PWR_CFG_VALUE(__PWR_CMD)		__PWR_CMD.value
-
-
-/*	Prototype of protected function. */
-u8 HalPwrSeqCmdParsing(struct adapter *padapter, u8 CutVersion, u8 FabVersion,
-		       u8 InterfaceType, struct wl_pwr_cfg PwrCfgCmd[]);
-
-#endif
diff --git a/drivers/staging/rtl8188eu/include/drv_types.h b/drivers/staging/rtl8188eu/include/drv_types.h
index 8f42d48243f0..c81317906adc 100644
--- a/drivers/staging/rtl8188eu/include/drv_types.h
+++ b/drivers/staging/rtl8188eu/include/drv_types.h
@@ -141,15 +141,8 @@ struct registry_priv {
 
 #define MAX_CONTINUAL_URB_ERR		4
 
-struct rt_firmware {
-	u8			*szFwBuffer;
-	u32			ulFwLength;
-};
-
 struct dvobj_priv {
 	struct adapter *if1;
-	struct rt_firmware firmware;
-
 	/* For 92D, DMDP have 2 interface. */
 	u8	InterfaceNumber;
 	u8	NumInterfaces;
diff --git a/drivers/staging/rtl8821ae/rc.h b/drivers/staging/rtl8188eu/include/fw.h
index 4afa2c20adcf..7884d8f65763 100644
--- a/drivers/staging/rtl8821ae/rc.h
+++ b/drivers/staging/rtl8188eu/include/fw.h
@@ -1,6 +1,6 @@
 /******************************************************************************
  *
- * Copyright(c) 2009-2010  Realtek Corporation.
+ * Copyright(c) 2009-2013  Realtek Corporation.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of version 2 of the GNU General Public License as
@@ -22,26 +22,38 @@
  * wlanfae <wlanfae@realtek.com>
  * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
  * Hsinchu 300, Taiwan.
- *
  * Larry Finger <Larry.Finger@lwfinger.net>
  *
  *****************************************************************************/
+#include "drv_types.h"
+#include <linux/types.h>
 
-#ifndef __RTL_RC_H__
-#define __RTL_RC_H__
-
-#define B_MODE_MAX_RIX 3
-#define G_MODE_MAX_RIX 11
-#define A_MODE_MAX_RIX 7
+#ifndef __RTL92C__FW__H__
+#define __RTL92C__FW__H__
 
-/* in mac80211 mcs0-mcs15 is idx0-idx15*/
-#define N_MODE_MCS7_RIX 7
-#define N_MODE_MCS15_RIX 15
+#define FW_8192C_START_ADDRESS		0x1000
+#define FW_8192C_PAGE_SIZE			4096
+#define FW_8192C_POLLING_DELAY		5
 
-struct rtl_rate_priv {
-	u8 ht_cap;
+struct rtl92c_firmware_header {
+	__le16 signature;
+	u8 category;
+	u8 function;
+	u16 version;
+	u8 subversion;
+	u8 rsvd1;
+	u8 month;
+	u8 date;
+	u8 hour;
+	u8 minute;
+	u16 ramcodesize;
+	u16 rsvd2;
+	u32 svnindex;
+	u32 rsvd3;
+	u32 rsvd4;
+	u32 rsvd5;
 };
 
-int rtl_rate_control_register(void);
-void rtl_rate_control_unregister(void);
+int rtl88eu_download_fw(struct adapter *adapt);
+
 #endif
diff --git a/drivers/staging/rtl8188eu/include/hal_intf.h b/drivers/staging/rtl8188eu/include/hal_intf.h
index 56d5c50bb734..9191993dd3f5 100644
--- a/drivers/staging/rtl8188eu/include/hal_intf.h
+++ b/drivers/staging/rtl8188eu/include/hal_intf.h
@@ -213,11 +213,6 @@ struct hal_ops {
 			    struct xmit_frame *pxmitframe);
 	s32 (*mgnt_xmit)(struct adapter *padapter,
 			 struct xmit_frame *pmgntframe);
-
-	u32	(*read_bbreg)(struct adapter *padapter, u32 RegAddr,
-			      u32 BitMask);
-	void	(*write_bbreg)(struct adapter *padapter, u32 RegAddr,
-			       u32 BitMask, u32 Data);
 	u32	(*read_rfreg)(struct adapter *padapter,
 			      enum rf_radio_path eRFPath, u32 RegAddr,
 			      u32 BitMask);
@@ -228,10 +223,6 @@ struct hal_ops {
 	void (*sreset_init_value)(struct adapter *padapter);
 	u8 (*sreset_get_wifi_status)(struct adapter *padapter);
 
-	int (*IOL_exec_cmds_sync)(struct adapter *padapter,
-				  struct xmit_frame *frame, u32 max_wait,
-				  u32 bndy_cnt);
-
 	void (*hal_notch_filter)(struct adapter *adapter, bool enable);
 	void (*hal_reset_security_engine)(struct adapter *adapter);
 };
@@ -307,9 +298,6 @@ void	rtw_hal_clone_data(struct adapter *dst_adapt,
 
 void rtw_hal_bcn_related_reg_setting(struct adapter *padapter);
 
-u32	rtw_hal_read_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask);
-void	rtw_hal_write_bbreg(struct adapter *padapter, u32 RegAddr, u32 BitMask,
-			    u32 Data);
 u32	rtw_hal_read_rfreg(struct adapter *padapter, enum rf_radio_path eRFPath,
 			   u32 RegAddr, u32 BitMask);
 void	rtw_hal_write_rfreg(struct adapter *padapter,
@@ -331,9 +319,6 @@ void	rtw_hal_antdiv_rssi_compared(struct adapter *padapter,
 void rtw_hal_sreset_init(struct adapter *padapter);
 u8   rtw_hal_sreset_get_wifi_status(struct adapter *padapter);
 
-int rtw_hal_iol_cmd(struct adapter  *adapter, struct xmit_frame *xmit_frame,
-		    u32 max_wating_ms, u32 bndy_cnt);
-
 void rtw_hal_notch_filter(struct adapter *adapter, bool enable);
 void rtw_hal_reset_security_engine(struct adapter *adapter);
 
diff --git a/drivers/staging/rtl8188eu/include/odm.h b/drivers/staging/rtl8188eu/include/odm.h
index e83812f35ad6..525eb100cd15 100644
--- a/drivers/staging/rtl8188eu/include/odm.h
+++ b/drivers/staging/rtl8188eu/include/odm.h
@@ -164,14 +164,6 @@ struct false_alarm_stats {
 	u32	Cnt_BW_LSC;	/* Gary */
 };
 
-struct dyn_primary_cca {
-	u8		PriCCA_flag;
-	u8		intf_flag;
-	u8		intf_type;
-	u8		DupRTS_flag;
-	u8		Monitor_flag;
-};
-
 struct rx_hpc {
 	u8		RXHP_flag;
 	u8		PSD_func_trigger;
@@ -867,7 +859,6 @@ struct odm_dm_struct {
 	struct fast_ant_train DM_FatTable;
 	struct rtw_dig	DM_DigTable;
 	struct rtl_ps	DM_PSTable;
-	struct dyn_primary_cca DM_PriCCA;
 	struct rx_hpc	DM_RXHP_Table;
 	struct false_alarm_stats FalseAlmCnt;
 	struct false_alarm_stats FlaseAlmCntBuddyAdapter;
diff --git a/drivers/staging/rtl8188eu/include/odm_HWConfig.h b/drivers/staging/rtl8188eu/include/odm_HWConfig.h
index 49e7e163ba70..1de4e6399435 100644
--- a/drivers/staging/rtl8188eu/include/odm_HWConfig.h
+++ b/drivers/staging/rtl8188eu/include/odm_HWConfig.h
@@ -120,13 +120,7 @@ void ODM_MacStatusQuery(struct odm_dm_struct *pDM_Odm,
 			bool	bPacketToSelf,
 			bool	bPacketBeacon);
 
-enum HAL_STATUS ODM_ConfigRFWithHeaderFile(struct odm_dm_struct *pDM_Odm,
-					   enum rf_radio_path Content,
-					   enum rf_radio_path eRFPath);
-
 enum HAL_STATUS ODM_ConfigBBWithHeaderFile(struct odm_dm_struct *pDM_Odm,
 					   enum odm_bb_config_type ConfigType);
 
-enum HAL_STATUS ODM_ConfigMACWithHeaderFile(struct odm_dm_struct *pDM_Odm);
-
 #endif
diff --git a/drivers/staging/rtl8188eu/include/odm_RTL8188E.h b/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
index 02ac78d326d2..14dce6c4b1bc 100644
--- a/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
+++ b/drivers/staging/rtl8188eu/include/odm_RTL8188E.h
@@ -29,17 +29,17 @@
 
 void ODM_DIG_LowerBound_88E(struct odm_dm_struct *pDM_Odm);
 
-void ODM_AntennaDiversityInit_88E(struct odm_dm_struct *pDM_Odm);
+void rtl88eu_dm_antenna_div_init(struct odm_dm_struct *dm_odm);
 
-void ODM_AntennaDiversity_88E(struct odm_dm_struct *pDM_Odm);
+void rtl88eu_dm_antenna_diversity(struct odm_dm_struct *dm_odm);
 
-void ODM_SetTxAntByTxInfo_88E(struct odm_dm_struct *pDM_Odm, u8 *pDesc,
-			      u8 macId);
+void rtl88eu_dm_set_tx_ant_by_tx_info(struct odm_dm_struct *dm_odm, u8 *desc,
+				      u8 mac_id);
 
-void ODM_UpdateRxIdleAnt_88E(struct odm_dm_struct *pDM_Odm, u8 Ant);
+void rtl88eu_dm_update_rx_idle_ant(struct odm_dm_struct *dm_odm, u8 ant);
 
-void ODM_AntselStatistics_88E(struct odm_dm_struct *pDM_Odm, u8	antsel_tr_mux,
-			      u32 MacId, u8 RxPWDBAll);
+void rtl88eu_dm_ant_sel_statistics(struct odm_dm_struct *dm_odm, u8 antsel_tr_mux,
+				   u32 mac_id, u8 rx_pwdb_all);
 
 void odm_FastAntTraining(struct odm_dm_struct *pDM_Odm);
 
@@ -47,8 +47,6 @@ void odm_FastAntTrainingCallback(struct odm_dm_struct *pDM_Odm);
 
 void odm_FastAntTrainingWorkItemCallback(struct odm_dm_struct *pDM_Odm);
 
-void odm_PrimaryCCA_Init(struct odm_dm_struct *pDM_Odm);
-
 bool ODM_DynamicPrimaryCCA_DupRTS(struct odm_dm_struct *pDM_Odm);
 
 #endif
diff --git a/drivers/staging/rtl8188eu/include/odm_RegConfig8188E.h b/drivers/staging/rtl8188eu/include/odm_RegConfig8188E.h
deleted file mode 100644
index f2bf7a0d9867..000000000000
--- a/drivers/staging/rtl8188eu/include/odm_RegConfig8188E.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-#ifndef __INC_ODM_REGCONFIG_H_8188E
-#define __INC_ODM_REGCONFIG_H_8188E
-
-void odm_ConfigRFReg_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u32 Data,
-			   enum rf_radio_path  RF_PATH, u32 RegAddr);
-
-void odm_ConfigRF_RadioA_8188E(struct odm_dm_struct *pDM_Odm,
-			       u32 Addr, u32 Data);
-
-void odm_ConfigRF_RadioB_8188E(struct odm_dm_struct *pDM_Odm,
-			       u32 Addr, u32 Data);
-
-void odm_ConfigMAC_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr, u8 Data);
-
-void odm_ConfigBB_AGC_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr,
-			    u32 Bitmask, u32 Data);
-
-void odm_ConfigBB_PHY_REG_PG_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr,
-				   u32 Bitmask, u32 Data);
-
-void odm_ConfigBB_PHY_8188E(struct odm_dm_struct *pDM_Odm, u32 Addr,
-			    u32 Bitmask, u32 Data);
-
-#endif
diff --git a/drivers/staging/rtl8188eu/include/odm_RegDefine11AC.h b/drivers/staging/rtl8188eu/include/odm_RegDefine11AC.h
deleted file mode 100644
index f08775c0dd8e..000000000000
--- a/drivers/staging/rtl8188eu/include/odm_RegDefine11AC.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-
-#ifndef	__ODM_REGDEFINE11AC_H__
-#define __ODM_REGDEFINE11AC_H__
-
-/* 2 RF REG LIST */
-
-
-
-/* 2 BB REG LIST */
-/* PAGE 8 */
-/* PAGE 9 */
-#define	ODM_REG_OFDM_FA_RST_11AC		0x9A4
-/* PAGE A */
-#define	ODM_REG_CCK_CCA_11AC				0xA0A
-#define	ODM_REG_CCK_FA_RST_11AC			0xA2C
-#define	ODM_REG_CCK_FA_11AC				0xA5C
-/* PAGE C */
-#define	ODM_REG_IGI_A_11AC				0xC50
-/* PAGE E */
-#define	ODM_REG_IGI_B_11AC				0xE50
-/* PAGE F */
-#define	ODM_REG_OFDM_FA_11AC			0xF48
-
-
-/* 2 MAC REG LIST */
-
-
-
-
-/* DIG Related */
-#define	ODM_BIT_IGI_11AC				0xFFFFFFFF
-
-
-
-#endif
diff --git a/drivers/staging/rtl8188eu/include/odm_precomp.h b/drivers/staging/rtl8188eu/include/odm_precomp.h
index 0ab8254ce901..0f236da09277 100644
--- a/drivers/staging/rtl8188eu/include/odm_precomp.h
+++ b/drivers/staging/rtl8188eu/include/odm_precomp.h
@@ -37,20 +37,13 @@
 #include "odm.h"
 #include "odm_HWConfig.h"
 #include "odm_debug.h"
-#include "odm_RegDefine11AC.h"
 #include "odm_RegDefine11N.h"
 
-#include "HalPhyRf_8188e.h"/* for IQK,LCK,Power-tracking */
 #include "Hal8188ERateAdaptive.h"/* for  RA,Power training */
 #include "rtl8188e_hal.h"
 
 #include "odm_reg.h"
 
-#include "HalHWImg8188E_MAC.h"
-#include "HalHWImg8188E_RF.h"
-#include "HalHWImg8188E_BB.h"
-
-#include "odm_RegConfig8188E.h"
 #include "odm_RTL8188E.h"
 
 void odm_CmnInfoHook_Debug(struct odm_dm_struct *pDM_Odm);
diff --git a/drivers/staging/rtl8188eu/include/phy.h b/drivers/staging/rtl8188eu/include/phy.h
new file mode 100644
index 000000000000..9a9ab82a8ed3
--- /dev/null
+++ b/drivers/staging/rtl8188eu/include/phy.h
@@ -0,0 +1,30 @@
+#include <odm.h>
+
+#define IQK_DELAY_TIME_88E	    10
+#define index_mapping_NUM_88E	    15
+#define AVG_THERMAL_NUM_88E	    4
+#define ODM_TARGET_CHNL_NUM_2G_5G   59
+
+bool rtl88eu_phy_mac_config(struct adapter *adapt);
+bool rtl88eu_phy_rf_config(struct adapter *adapt);
+bool rtl88eu_phy_bb_config(struct adapter *adapt);
+
+u32 phy_query_bb_reg(struct adapter *adapt, u32 regaddr, u32 bitmask);
+void phy_set_bb_reg(struct adapter *adapt, u32 regaddr, u32 bitmask, u32 data);
+u32 phy_query_rf_reg(struct adapter *adapt, enum rf_radio_path rf_path,
+		     u32 reg_addr, u32 bit_mask);
+void phy_set_rf_reg(struct adapter *adapt, enum rf_radio_path rf_path,
+		    u32 reg_addr, u32 bit_mask, u32 data);
+
+void phy_set_tx_power_level(struct adapter *adapt, u8 channel);
+
+void phy_set_bw_mode(struct adapter *adapt, enum ht_channel_width bandwidth,
+		     unsigned char offset);
+void phy_sw_chnl(struct adapter *adapt, u8 channel);
+
+void rtl88eu_dm_txpower_track_adjust(struct odm_dm_struct *dm_odm,
+				     u8 type, u8 *dir, u32 *out_write);
+
+void rtl88eu_dm_txpower_tracking_callback_thermalmeter(struct adapter *adapt);
+void rtl88eu_phy_iq_calibrate(struct adapter *adapter, bool recovery);
+void rtl88eu_phy_lc_calibrate(struct adapter *adapter);
diff --git a/drivers/staging/rtl8188eu/include/pwrseq.h b/drivers/staging/rtl8188eu/include/pwrseq.h
new file mode 100644
index 000000000000..43db92dcb8b3
--- /dev/null
+++ b/drivers/staging/rtl8188eu/include/pwrseq.h
@@ -0,0 +1,341 @@
+
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+
+#ifndef __HAL8188EPWRSEQ_H__
+#define __HAL8188EPWRSEQ_H__
+
+#include "pwrseqcmd.h"
+
+/*
+	Check document WM-20110607-Paul-RTL8188E_Power_Architecture-R02.vsd
+	There are 6 HW Power States:
+	0: POFF--Power Off
+	1: PDN--Power Down
+	2: CARDEMU--Card Emulation
+	3: ACT--Active Mode
+	4: LPS--Low Power State
+	5: SUS--Suspend
+
+	The transision from different states are defined below
+	TRANS_CARDEMU_TO_ACT
+	TRANS_ACT_TO_CARDEMU
+	TRANS_CARDEMU_TO_SUS
+	TRANS_SUS_TO_CARDEMU
+	TRANS_CARDEMU_TO_PDN
+	TRANS_ACT_TO_LPS
+	TRANS_LPS_TO_ACT
+
+	TRANS_END
+
+    PWR SEQ Version: rtl8188E_PwrSeq_V09.h
+*/
+#define RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS	10
+#define RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS	10
+#define RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS	10
+#define RTL8188E_TRANS_SUS_TO_CARDEMU_STEPS	10
+#define RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS	10
+#define RTL8188E_TRANS_PDN_TO_CARDEMU_STEPS	10
+#define RTL8188E_TRANS_ACT_TO_LPS_STEPS		15
+#define RTL8188E_TRANS_LPS_TO_ACT_STEPS		15
+#define RTL8188E_TRANS_END_STEPS		1
+
+
+#define RTL8188E_TRANS_CARDEMU_TO_ACT					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value
+	 * },
+	 * comment here
+	 */								\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, BIT1},			\
+	/* wait till 0x04[17] = 1    power ready*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0|BIT1, 0},			\
+	/* 0x02[1:0] = 0	reset BB*/				\
+	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7},			\
+	/*0x24[23] = 2b'01 schmit trigger */				\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},			\
+	/* 0x04[15] = 0 disable HWPDN (control by DRV)*/		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4|BIT3, 0},			\
+	/*0x04[12:11] = 2b'00 disable WL suspend*/			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, BIT0},			\
+	/*0x04[8] = 1 polling until return 0*/				\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT0, 0},			\
+	/*wait till 0x04[8] = 0*/					\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0},			\
+	/*LDO normal mode*/						\
+	{0x0074, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4},			\
+	/*SDIO Driving*/
+
+#define RTL8188E_TRANS_ACT_TO_CARDEMU					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value
+	 * },
+	 * comments here
+	 */								\
+	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0},			\
+	/*0x1F[7:0] = 0 turn off RF*/					\
+	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4},			\
+	/*LDO Sleep mode*/						\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1},			\
+	/*0x04[9] = 1 turn off MAC by HW state machine*/		\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT1, 0},			\
+	/*wait till 0x04[9] = 0 polling until return 0 to disable*/
+
+#define RTL8188E_TRANS_CARDEMU_TO_SUS					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
+	PWR_CMD_WRITE, BIT3|BIT4, BIT3},				\
+	/* 0x04[12:11] = 2b'01enable WL suspend */			\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, BIT3|BIT4},		\
+	/* 0x04[12:11] = 2b'11enable WL suspend for PCIe */		\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
+	PWR_CMD_WRITE, 0xFF, BIT7},					\
+	/* 0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */\
+	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
+	PWR_CMD_WRITE, BIT4, 0},					\
+	/*Clear SIC_EN register 0x40[12] = 1'b0 */			\
+	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
+	PWR_CMD_WRITE, BIT4, BIT4},					\
+	/*Set USB suspend enable local register  0xfe10[4]=1 */		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0},			\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0},			\
+	/*wait power state to suspend*/
+
+#define RTL8188E_TRANS_SUS_TO_CARDEMU					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0},			\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1},		\
+	/*wait power state to suspend*/					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0},			\
+	/*0x04[12:11] = 2b'01enable WL suspend*/
+
+#define RTL8188E_TRANS_CARDEMU_TO_CARDDIS				\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0026, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7},			\
+	/*0x24[23] = 2b'01 schmit trigger */				\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
+	PWR_CMD_WRITE, BIT3|BIT4, BIT3},				\
+	/*0x04[12:11] = 2b'01 enable WL suspend*/			\
+	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
+	PWR_CMD_WRITE, 0xFF, 0},					\
+	/* 0x04[31:30] = 2b'10 enable enable bandgap mbias in suspend */\
+	{0x0041, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,			\
+	PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK, PWR_BASEADDR_MAC,		\
+	PWR_CMD_WRITE, BIT4, 0},					\
+	/*Clear SIC_EN register 0x40[12] = 1'b0 */			\
+	{0xfe10, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, BIT4},			\
+	/*Set USB suspend enable local register  0xfe10[4]=1 */		\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, BIT0},			\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, 0},			\
+	/*wait power state to suspend*/
+
+#define RTL8188E_TRANS_CARDDIS_TO_CARDEMU				\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, BIT0, 0},			\
+	/*Set SDIO suspend local register*/				\
+	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_POLLING, BIT1, BIT1},		\
+	/*wait power state to suspend*/					\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT3|BIT4, 0},			\
+	/*0x04[12:11] = 2b'01enable WL suspend*/
+
+#define RTL8188E_TRANS_CARDEMU_TO_PDN					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},			\
+	/* 0x04[16] = 0*/						\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, BIT7},			\
+	/* 0x04[15] = 1*/
+
+#define RTL8188E_TRANS_PDN_TO_CARDEMU					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT7, 0},			\
+	/* 0x04[15] = 0*/
+
+/* This is used by driver for LPSRadioOff Procedure, not for FW LPS Step */
+#define RTL8188E_TRANS_ACT_TO_LPS					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x7F},/*Tx Pause*/	\
+	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, 0xFF, 0},			\
+	/*Should be zero if no packet is transmitting*/			\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT0, 0},			\
+	/*CCK and OFDM are disabled,and clock are gated*/		\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0,				\
+	PWRSEQ_DELAY_US},/*Delay 1us*/					\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x3F},/*Reset MAC TRX*/	\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/\
+	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT5, BIT5},			\
+	/*Respond TxOK to scheduler*/
+
+
+#define RTL8188E_TRANS_LPS_TO_ACT					\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,	\
+	PWR_BASEADDR_SDIO, PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/	\
+	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/	\
+	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/	\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/	\
+	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT4, 0},			\
+	/* 0x08[4] = 0 switch TSF to 40M */				\
+	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_POLLING, BIT7, 0},			\
+	/* Polling 0x109[7]=0  TSF in 40M */				\
+	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT6|BIT7, 0},			\
+	/* 0x29[7:6] = 2b'00  enable BB clock */			\
+	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1, BIT1},			\
+	/* 0x101[1] = 1 */						\
+	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0xFF},			\
+	/* 0x100[7:0] = 0xFF enable WMAC TRX */				\
+	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0},		\
+	/* 0x02[1:0] = 2b'11 enable BB macro */				\
+	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,	\
+	PWR_BASEADDR_MAC, PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
+
+#define RTL8188E_TRANS_END						\
+	/* format
+	 * { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk,
+	 * value },
+	 * comments here
+	 */								\
+	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK, 0,	\
+	PWR_CMD_END, 0, 0},
+
+
+extern struct wl_pwr_cfg rtl8188E_power_on_flow
+		[RTL8188E_TRANS_CARDEMU_TO_ACT_STEPS + RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_radio_off_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS + RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_card_disable_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_card_enable_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_suspend_flow[
+		RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_resume_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_SUS_STEPS +
+		RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_hwpdn_flow
+		[RTL8188E_TRANS_ACT_TO_CARDEMU_STEPS +
+		RTL8188E_TRANS_CARDEMU_TO_PDN_STEPS + RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_enter_lps_flow
+		[RTL8188E_TRANS_ACT_TO_LPS_STEPS + RTL8188E_TRANS_END_STEPS];
+extern struct wl_pwr_cfg rtl8188E_leave_lps_flow
+		[RTL8188E_TRANS_LPS_TO_ACT_STEPS + RTL8188E_TRANS_END_STEPS];
+
+#endif /* __HAL8188EPWRSEQ_H__ */
diff --git a/drivers/staging/rtl8188eu/include/pwrseqcmd.h b/drivers/staging/rtl8188eu/include/pwrseqcmd.h
new file mode 100644
index 000000000000..980a49769157
--- /dev/null
+++ b/drivers/staging/rtl8188eu/include/pwrseqcmd.h
@@ -0,0 +1,90 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ *
+ ******************************************************************************/
+#ifndef __HALPWRSEQCMD_H__
+#define __HALPWRSEQCMD_H__
+
+#include <drv_types.h>
+
+/* The value of cmd: 4 bits */
+#define PWR_CMD_READ		0x00
+#define PWR_CMD_WRITE		0x01
+#define PWR_CMD_POLLING		0x02
+#define PWR_CMD_DELAY		0x03
+#define PWR_CMD_END		0x04
+
+/* The value of base: 4 bits */
+/*  define the base address of each block */
+#define PWR_BASEADDR_MAC	0x00
+#define PWR_BASEADDR_USB	0x01
+#define PWR_BASEADDR_PCIE	0x02
+#define PWR_BASEADDR_SDIO	0x03
+
+/* The value of interface_msk: 4 bits */
+#define PWR_INTF_SDIO_MSK	BIT(0)
+#define PWR_INTF_USB_MSK	BIT(1)
+#define PWR_INTF_PCI_MSK	BIT(2)
+#define PWR_INTF_ALL_MSK	(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+/* The value of fab_msk: 4 bits */
+#define PWR_FAB_TSMC_MSK	BIT(0)
+#define PWR_FAB_UMC_MSK		BIT(1)
+#define PWR_FAB_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
+
+/* The value of cut_msk: 8 bits */
+#define PWR_CUT_TESTCHIP_MSK	BIT(0)
+#define PWR_CUT_A_MSK		BIT(1)
+#define PWR_CUT_B_MSK		BIT(2)
+#define PWR_CUT_C_MSK		BIT(3)
+#define PWR_CUT_D_MSK		BIT(4)
+#define PWR_CUT_E_MSK		BIT(5)
+#define PWR_CUT_F_MSK		BIT(6)
+#define PWR_CUT_G_MSK		BIT(7)
+#define PWR_CUT_ALL_MSK		0xFF
+
+
+enum pwrseq_cmd_delat_unit {
+	PWRSEQ_DELAY_US,
+	PWRSEQ_DELAY_MS,
+};
+
+struct wl_pwr_cfg {
+	u16 offset;
+	u8 cut_msk;
+	u8 fab_msk:4;
+	u8 interface_msk:4;
+	u8 base:4;
+	u8 cmd:4;
+	u8 msk;
+	u8 value;
+};
+
+#define GET_PWR_CFG_OFFSET(__PWR_CMD)		__PWR_CMD.offset
+#define GET_PWR_CFG_CUT_MASK(__PWR_CMD)		__PWR_CMD.cut_msk
+#define GET_PWR_CFG_FAB_MASK(__PWR_CMD)		__PWR_CMD.fab_msk
+#define GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
+#define GET_PWR_CFG_BASE(__PWR_CMD)		__PWR_CMD.base
+#define GET_PWR_CFG_CMD(__PWR_CMD)		__PWR_CMD.cmd
+#define GET_PWR_CFG_MASK(__PWR_CMD)		__PWR_CMD.msk
+#define GET_PWR_CFG_VALUE(__PWR_CMD)		__PWR_CMD.value
+
+u8 rtl88eu_pwrseqcmdparsing(struct adapter *padapter, u8 cut_vers, u8 fab_vers,
+			    u8 ifacetype, struct wl_pwr_cfg pwrcfgCmd[]);
+
+#endif
diff --git a/drivers/staging/rtl8188eu/include/rf.h b/drivers/staging/rtl8188eu/include/rf.h
new file mode 100644
index 000000000000..98a5551f5ae7
--- /dev/null
+++ b/drivers/staging/rtl8188eu/include/rf.h
@@ -0,0 +1,11 @@
+#define RF6052_MAX_TX_PWR	0x3F
+#define RF6052_MAX_REG		0x3F
+
+void rtl88eu_phy_rf6052_set_bandwidth(struct adapter *adapt,
+				      enum ht_channel_width bandwidth);
+void rtl88eu_phy_rf6052_set_cck_txpower(struct adapter *adapt,
+				       u8 *powerlevel);
+void rtl88eu_phy_rf6052_set_ofdm_txpower(struct adapter *adapt,
+					 u8 *powerlevel_ofdm,
+					 u8 *powerlevel_bw20,
+					 u8 *powerlevel_bw40, u8 channel);
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
index fb206538392e..42ab1d288bdc 100644
--- a/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
+++ b/drivers/staging/rtl8188eu/include/rtl8188e_hal.h
@@ -25,12 +25,11 @@
 #include "rtl8188e_spec.h"
 #include "Hal8188EPhyReg.h"
 #include "Hal8188EPhyCfg.h"
-#include "rtl8188e_rf.h"
 #include "rtl8188e_dm.h"
 #include "rtl8188e_recv.h"
 #include "rtl8188e_xmit.h"
 #include "rtl8188e_cmd.h"
-#include "Hal8188EPwrSeq.h"
+#include "pwrseq.h"
 #include "rtw_efuse.h"
 #include "rtw_sreset.h"
 #include "odm_precomp.h"
@@ -70,45 +69,10 @@
 #define MAX_PAGE_SIZE			4096	/*  @ page : 4k bytes */
 
 #define IS_FW_HEADER_EXIST(_pFwHdr)				\
-	((le16_to_cpu(_pFwHdr->Signature)&0xFFF0) == 0x92C0 ||	\
-	(le16_to_cpu(_pFwHdr->Signature)&0xFFF0) == 0x88C0 ||	\
-	(le16_to_cpu(_pFwHdr->Signature)&0xFFF0) == 0x2300 ||	\
-	(le16_to_cpu(_pFwHdr->Signature)&0xFFF0) == 0x88E0)
-
-/*  This structure must be careful with byte-ordering */
-
-struct rt_firmware_hdr {
-	/*  8-byte alinment required */
-	/*  LONG WORD 0 ---- */
-	__le16		Signature;	/* 92C0: test chip; 92C,
-					 * 88C0: test chip; 88C1: MP A-cut;
-					 * 92C1: MP A-cut */
-	u8		Category;	/*  AP/NIC and USB/PCI */
-	u8		Function;	/*  Reserved for different FW function
-					 *  indcation, for further use when
-					 *  driver needs to download different
-					 *  FW for different conditions */
-	__le16		Version;	/*  FW Version */
-	u8		Subversion;	/*  FW Subversion, default 0x00 */
-	u16		Rsvd1;
-
-	/*  LONG WORD 1 ---- */
-	u8		Month;	/*  Release time Month field */
-	u8		Date;	/*  Release time Date field */
-	u8		Hour;	/*  Release time Hour field */
-	u8		Minute;	/*  Release time Minute field */
-	__le16		RamCodeSize;	/*  The size of RAM code */
-	u8		Foundry;
-	u8		Rsvd2;
-
-	/*  LONG WORD 2 ---- */
-	__le32		SvnIdx;	/*  The SVN entry index */
-	u32		Rsvd3;
-
-	/*  LONG WORD 3 ---- */
-	u32		Rsvd4;
-	u32		Rsvd5;
-};
+	((le16_to_cpu(_pFwHdr->signature)&0xFFF0) == 0x92C0 ||	\
+	(le16_to_cpu(_pFwHdr->signature)&0xFFF0) == 0x88C0 ||	\
+	(le16_to_cpu(_pFwHdr->signature)&0xFFF0) == 0x2300 ||	\
+	(le16_to_cpu(_pFwHdr->signature)&0xFFF0) == 0x88E0)
 
 #define DRIVER_EARLY_INT_TIME		0x05
 #define BCN_DMA_ATIME_INT_TIME		0x02
@@ -242,7 +206,8 @@ struct hal_data_8188e {
 	struct HAL_VERSION	VersionID;
 	enum rt_regulator_mode RegulatorMode; /*  switching regulator or LDO */
 	u16	CustomerID;
-
+	u8 *pfirmware;
+	u32 fwsize;
 	u16	FirmwareVersion;
 	u16	FirmwareVersionRev;
 	u16	FirmwareSubVersion;
@@ -419,7 +384,6 @@ struct hal_data_8188e {
 	(GET_HAL_DATA(_Adapter)->MultiFunc & RT_MULTI_FUNC_GPS)
 
 /*  rtl8188e_hal_init.c */
-s32 rtl8188e_FirmwareDownload(struct adapter *padapter);
 void _8051Reset88E(struct adapter *padapter);
 void rtl8188e_InitializeFirmwareVars(struct adapter *padapter);
 
@@ -453,12 +417,10 @@ void Hal_ReadPowerSavingMode88E(struct adapter *pAdapter, u8 *hwinfo,
 void rtl8188e_set_hal_ops(struct hal_ops *pHalFunc);
 
 /*  register */
-void SetBcnCtrlReg(struct adapter *padapter, u8 SetBits, u8 ClearBits);
 
 void rtl8188e_start_thread(struct adapter *padapter);
 void rtl8188e_stop_thread(struct adapter *padapter);
 
-void rtw_IOL_cmd_tx_pkt_buf_dump(struct adapter  *Adapter, int len);
 s32 iol_execute(struct adapter *padapter, u8 control);
 void iol_mode_enable(struct adapter *padapter, u8 enable);
 s32 rtl8188e_iol_efuse_patch(struct adapter *padapter);
diff --git a/drivers/staging/rtl8188eu/include/rtl8188e_rf.h b/drivers/staging/rtl8188eu/include/rtl8188e_rf.h
deleted file mode 100644
index 10fc356e0209..000000000000
--- a/drivers/staging/rtl8188eu/include/rtl8188e_rf.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-#ifndef __RTL8188E_RF_H__
-#define __RTL8188E_RF_H__
-
-#define		RF6052_MAX_TX_PWR		0x3F
-#define		RF6052_MAX_REG			0x3F
-#define		RF6052_MAX_PATH			2
-
-
-int	PHY_RF6052_Config8188E(struct adapter *Adapter);
-void rtl8188e_RF_ChangeTxPath(struct adapter *Adapter, u16 DataRate);
-void rtl8188e_PHY_RF6052SetBandwidth(struct adapter *Adapter,
-				     enum ht_channel_width Bandwidth);
-void	rtl8188e_PHY_RF6052SetCckTxPower(struct adapter *Adapter, u8 *level);
-void	rtl8188e_PHY_RF6052SetOFDMTxPower(struct adapter *Adapter, u8 *ofdm,
-					  u8 *pwrbw20, u8 *pwrbw40, u8 channel);
-
-#endif/* __RTL8188E_RF_H__ */
diff --git a/drivers/staging/rtl8188eu/include/rtw_efuse.h b/drivers/staging/rtl8188eu/include/rtw_efuse.h
index 720f9ea24d52..5660eed7196b 100644
--- a/drivers/staging/rtl8188eu/include/rtw_efuse.h
+++ b/drivers/staging/rtl8188eu/include/rtw_efuse.h
@@ -99,7 +99,6 @@ struct efuse_hal {
 	u8 fakeBTEfuseModifiedMap[EFUSE_BT_MAX_MAP_LEN];
 };
 
-u16 Efuse_GetCurrentSize(struct adapter *adapter);
 u8 Efuse_CalculateWordCnts(u8 word_en);
 void EFUSE_GetEfuseDefinition(struct adapter *adapt, u8 type, u8 type1,
 			      void *out);
diff --git a/drivers/staging/rtl8188eu/include/rtw_iol.h b/drivers/staging/rtl8188eu/include/rtw_iol.h
index 80bfd063dd8d..68aae7f0b02f 100644
--- a/drivers/staging/rtl8188eu/include/rtw_iol.h
+++ b/drivers/staging/rtl8188eu/include/rtw_iol.h
@@ -23,60 +23,6 @@
 #include <osdep_service.h>
 #include <drv_types.h>
 
-#define IOREG_CMD_END_LEN	4
-
-struct ioreg_cfg {
-	u8	length;
-	u8	cmd_id;
-	__le16	address;
-	__le32	data;
-	__le32  mask;
-};
-
-enum ioreg_cmd {
-	IOREG_CMD_LLT		= 0x01,
-	IOREG_CMD_REFUSE	= 0x02,
-	IOREG_CMD_EFUSE_PATH	= 0x03,
-	IOREG_CMD_WB_REG	= 0x04,
-	IOREG_CMD_WW_REG	= 0x05,
-	IOREG_CMD_WD_REG	= 0x06,
-	IOREG_CMD_W_RF		= 0x07,
-	IOREG_CMD_DELAY_US	= 0x10,
-	IOREG_CMD_DELAY_MS	= 0x11,
-	IOREG_CMD_END		= 0xFF,
-};
-
-struct xmit_frame *rtw_IOL_accquire_xmit_frame(struct adapter *adapter);
-int rtw_IOL_append_cmds(struct xmit_frame *xmit_frame, u8 *IOL_cmds,
-			u32 cmd_len);
-int rtw_IOL_append_LLT_cmd(struct xmit_frame *xmit_frame, u8 page_boundary);
-int rtw_IOL_exec_cmds_sync(struct adapter  *adapter,
-			   struct xmit_frame *xmit_frame, u32 max_wating_ms,
-			   u32 bndy_cnt);
 bool rtw_IOL_applied(struct adapter  *adapter);
-int rtw_IOL_append_DELAY_US_cmd(struct xmit_frame *xmit_frame, u16 us);
-int rtw_IOL_append_DELAY_MS_cmd(struct xmit_frame *xmit_frame, u16 ms);
-int rtw_IOL_append_END_cmd(struct xmit_frame *xmit_frame);
-
-void read_efuse_from_txpktbuf(struct adapter *adapter, int bcnhead,
-			      u8 *content, u16 *size);
-
-int _rtw_IOL_append_WB_cmd(struct xmit_frame *xmit_frame, u16 addr,
-			   u8 value, u8 mask);
-int _rtw_IOL_append_WD_cmd(struct xmit_frame *xmit_frame, u16 addr,
-			   u32 value, u32 mask);
-int _rtw_IOL_append_WRF_cmd(struct xmit_frame *xmit_frame, u8 rf_path,
-			    u16 addr, u32 value, u32 mask);
-#define rtw_IOL_append_WB_cmd(xmit_frame, addr, value, mask)		\
-	_rtw_IOL_append_WB_cmd((xmit_frame), (addr), (value) , (mask))
-#define rtw_IOL_append_WW_cmd(xmit_frame, addr, value, mask)		\
-	_rtw_IOL_append_WW_cmd((xmit_frame), (addr), (value), (mask))
-#define rtw_IOL_append_WD_cmd(xmit_frame, addr, value, mask)		\
-	_rtw_IOL_append_WD_cmd((xmit_frame), (addr), (value), (mask))
-#define rtw_IOL_append_WRF_cmd(xmit_frame, rf_path, addr, value, mask)	\
-	_rtw_IOL_append_WRF_cmd((xmit_frame), (rf_path), (addr), (value), (mask))
-
-u8 rtw_IOL_cmd_boundary_handle(struct xmit_frame *pxmit_frame);
-void  rtw_IOL_cmd_buf_dump(struct adapter  *Adapter, int buf_len, u8 *pbuf);
 
 #endif /* __RTW_IOL_H_ */
diff --git a/drivers/staging/rtl8188eu/os_dep/os_intfs.c b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
index c7a44ab33d64..08a80f759b8d 100644
--- a/drivers/staging/rtl8188eu/os_dep/os_intfs.c
+++ b/drivers/staging/rtl8188eu/os_dep/os_intfs.c
@@ -26,6 +26,7 @@
 #include <recv_osdep.h>
 #include <hal_intf.h>
 #include <rtw_ioctl.h>
+#include <rtl8188e_hal.h>
 
 #include <usb_hal.h>
 
@@ -1121,7 +1122,7 @@ int pm_netdev_open(struct net_device *pnetdev, u8 bnormal)
 int netdev_close(struct net_device *pnetdev)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(pnetdev);
-	struct dvobj_priv *dvobj = adapter_to_dvobj(padapter);
+	struct hal_data_8188e *rtlhal = GET_HAL_DATA(padapter);
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("+88eu_drv - drv_close\n"));
 
@@ -1154,8 +1155,8 @@ int netdev_close(struct net_device *pnetdev)
 		rtw_led_control(padapter, LED_CTL_POWER_OFF);
 	}
 
-	kfree(dvobj->firmware.szFwBuffer);
-	dvobj->firmware.szFwBuffer = NULL;
+	kfree(rtlhal->pfirmware);
+	rtlhal->pfirmware = NULL;
 
 	RT_TRACE(_module_os_intfs_c_, _drv_info_, ("-88eu_drv - drv_close\n"));
 	DBG_88E("-88eu_drv - drv_close, bup =%d\n", padapter->bup);
diff --git a/drivers/staging/rtl8188eu/os_dep/rtw_android.c b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
index ca2736d60b62..d9d55d12fd5f 100644
--- a/drivers/staging/rtl8188eu/os_dep/rtw_android.c
+++ b/drivers/staging/rtl8188eu/os_dep/rtw_android.c
@@ -79,7 +79,7 @@ int rtw_android_cmdstr_to_num(char *cmdstr)
 {
 	int cmd_num;
 	for (cmd_num = 0; cmd_num < ANDROID_WIFI_CMD_MAX; cmd_num++)
-		if (0 == strnicmp(cmdstr , android_wifi_cmd_str[cmd_num],
+		if (0 == strncasecmp(cmdstr , android_wifi_cmd_str[cmd_num],
 				  strlen(android_wifi_cmd_str[cmd_num])))
 			break;
 	return cmd_num;
@@ -106,23 +106,18 @@ static int rtw_android_get_link_speed(struct net_device *net, char *command,
 				      int total_len)
 {
 	struct adapter *padapter = (struct adapter *)rtw_netdev_priv(net);
-	int bytes_written;
 	u16 link_speed;
 
 	link_speed = rtw_get_cur_max_rate(padapter) / 10;
-	bytes_written = snprintf(command, total_len, "LinkSpeed %d",
+	return snprintf(command, total_len, "LinkSpeed %d",
 				 link_speed);
-	return bytes_written;
 }
 
 static int rtw_android_get_macaddr(struct net_device *net, char *command,
 				   int total_len)
 {
-	int bytes_written;
-
-	bytes_written = snprintf(command, total_len, "Macaddr = %pM",
+	return snprintf(command, total_len, "Macaddr = %pM",
 				 net->dev_addr);
-	return bytes_written;
 }
 
 static int android_set_cntry(struct net_device *net, char *command,
diff --git a/drivers/staging/rtl8188eu/os_dep/usb_intf.c b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
index b8676ac77b0c..407a318b09db 100644
--- a/drivers/staging/rtl8188eu/os_dep/usb_intf.c
+++ b/drivers/staging/rtl8188eu/os_dep/usb_intf.c
@@ -43,9 +43,11 @@ static struct usb_device_id rtw_usb_id_tbl[] = {
 	{USB_DEVICE(USB_VENDER_ID_REALTEK, 0x0179)}, /* 8188ETV */
 	/*=== Customer ID ===*/
 	/****** 8188EUS ********/
+	{USB_DEVICE(0x056e, 0x4008)}, /* Elecom WDC-150SU2M */
 	{USB_DEVICE(0x07b8, 0x8179)}, /* Abocom - Abocom */
 	{USB_DEVICE(0x2001, 0x330F)}, /* DLink DWA-125 REV D1 */
 	{USB_DEVICE(0x2001, 0x3310)}, /* Dlink DWA-123 REV D1 */
+	{USB_DEVICE(0x0df6, 0x0076)}, /* Sitecom N150 v2 */
 	{}	/* Terminating entry */
 };
 
diff --git a/drivers/staging/rtl8192e/dot11d.c b/drivers/staging/rtl8192e/dot11d.c
index bfcc935d48de..ef9da863c335 100644
--- a/drivers/staging/rtl8192e/dot11d.c
+++ b/drivers/staging/rtl8192e/dot11d.c
@@ -49,6 +49,7 @@ static struct channel_list ChannelPlan[] = {
 void dot11d_init(struct rtllib_device *ieee)
 {
 	struct rt_dot11d_info *pDot11dInfo = GET_DOT11D_INFO(ieee);
+
 	pDot11dInfo->bEnabled = false;
 
 	pDot11dInfo->State = DOT11D_STATE_NONE;
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
index 5af1c19142de..0e6bdd222e75 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8190P_rtl8256.c
@@ -79,17 +79,14 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev,
 
 		}
 	}
-	return;
 }
 
 bool PHY_RF8256_Config(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
-	bool rtStatus = true;
-	priv->NumTotalRFPath = RTL819X_TOTAL_RF_PATH;
-	rtStatus = phy_RF8256_Config_ParaFile(dev);
 
-	return rtStatus;
+	priv->NumTotalRFPath = RTL819X_TOTAL_RF_PATH;
+	return phy_RF8256_Config_ParaFile(dev);
 }
 
 bool phy_RF8256_Config_ParaFile(struct net_device *dev)
@@ -231,11 +228,11 @@ bool phy_RF8256_Config_ParaFile(struct net_device *dev)
 
 	}
 
-	RT_TRACE(COMP_PHY, "PHY Initialization Success\n") ;
+	RT_TRACE(COMP_PHY, "PHY Initialization Success\n");
 	return true;
 
 phy_RF8256_Config_ParaFile_Fail:
-	RT_TRACE(COMP_ERR, "PHY Initialization failed\n") ;
+	RT_TRACE(COMP_ERR, "PHY Initialization failed\n");
 	return false;
 }
 
@@ -302,5 +299,4 @@ void PHY_SetRF8256OFDMTxPower(struct net_device *dev, u8 powerlevel)
 		rtl8192_setBBreg(dev, RegOffset[index], 0x7f7f7f7f, writeVal);
 	}
 
-	return;
 }
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
index 79d86b9e8009..2b1430d66adb 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_cmdpkt.c
@@ -153,18 +153,14 @@ static void cmpk_handle_tx_feedback(struct net_device *dev, u8 *pmsg)
 static void cmdpkt_beacontimerinterrupt_819xusb(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
-	u16 tx_rate;
 
 	if ((priv->rtllib->current_network.mode == IEEE_A)  ||
 	    (priv->rtllib->current_network.mode == IEEE_N_5G) ||
 	    ((priv->rtllib->current_network.mode == IEEE_N_24G)  &&
-	    (!priv->rtllib->pHTInfo->bCurSuppCCK))) {
-		tx_rate = 60;
+	    (!priv->rtllib->pHTInfo->bCurSuppCCK)))
 		DMESG("send beacon frame  tx rate is 6Mbpm\n");
-	} else {
-		tx_rate = 10;
+	else
 		DMESG("send beacon frame  tx rate is 1Mbpm\n");
-	}
 }
 
 static void cmpk_handle_interrupt_status(struct net_device *dev, u8 *pmsg)
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
index 4a35f9b5602d..0ffed2d06b58 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_dev.c
@@ -62,6 +62,7 @@ static void rtl8192e_update_msr(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	u8 msr;
 	enum led_ctl_mode LedAction = LED_CTL_NO_LINK;
+
 	msr  = read_nic_byte(dev, MSR);
 	msr &= ~MSR_LINK_MASK;
 
@@ -166,6 +167,7 @@ void rtl8192e_SetHwReg(struct net_device *dev, u8 variable, u8 *val)
 	case HW_VAR_ACK_PREAMBLE:
 	{
 		u32 regTmp;
+
 		priv->short_preamble = (bool)(*(u8 *)val);
 		regTmp = priv->basic_rate;
 		if (priv->short_preamble)
@@ -299,6 +301,7 @@ void rtl8192e_SetHwReg(struct net_device *dev, u8 variable, u8 *val)
 	case HW_VAR_RF_TIMING:
 	{
 		u8 Rf_Timing = *((u8 *)val);
+
 		write_nic_byte(dev, rFPGA0_RFTiming1, Rf_Timing);
 		break;
 	}
@@ -318,6 +321,7 @@ static void rtl8192_read_eeprom_info(struct net_device *dev)
 	u16 i, usValue, IC_Version;
 	u16 EEPROMId;
 	u8 bMac_Tmp_Addr[6] = {0x00, 0xe0, 0x4c, 0x00, 0x00, 0x01};
+
 	RT_TRACE(COMP_INIT, "====> rtl8192_read_eeprom_info\n");
 
 	EEPROMId = eprom_read(dev, 0);
@@ -682,6 +686,7 @@ static void rtl8192_hwconfig(struct net_device *dev)
 	write_nic_byte(dev, BW_OPMODE, regBwOpMode);
 	{
 		u32 ratr_value = 0;
+
 		ratr_value = regRATR;
 		if (priv->rf_type == RF_1T2R)
 			ratr_value &= ~(RATE_ALL_OFDM_2SS);
@@ -807,6 +812,7 @@ start:
 	CamResetAllEntry(dev);
 	{
 		u8 SECR_value = 0x0;
+
 		SECR_value |= SCR_TxEncEnable;
 		SECR_value |= SCR_RxDecEnable;
 		SECR_value |= SCR_NoSKMC;
@@ -816,6 +822,7 @@ start:
 	write_nic_word(dev, BCN_INTERVAL, 100);
 	{
 		int i;
+
 		for (i = 0; i < QOS_QUEUE_NUM; i++)
 			write_nic_dword(dev, WDCAPARA_ADD[i], 0x005e4332);
 	}
@@ -1003,6 +1010,7 @@ void rtl8192_link_change(struct net_device *dev)
 
 	if (ieee->iw_mode == IW_MODE_INFRA || ieee->iw_mode == IW_MODE_ADHOC) {
 		u32 reg = 0;
+
 		reg = read_nic_dword(dev, RCR);
 		if (priv->rtllib->state == RTLLIB_LINKED) {
 			if (ieee->IntelPromiscuousModeInfo.bPromiscuousOn)
@@ -1175,6 +1183,7 @@ void  rtl8192_tx_fill_desc(struct net_device *dev, struct tx_desc *pdesc,
 	dma_addr_t mapping = pci_map_single(priv->pdev, skb->data, skb->len,
 			 PCI_DMA_TODEVICE);
 	struct tx_fwinfo_8190pci *pTxFwInfo = NULL;
+
 	pTxFwInfo = (struct tx_fwinfo_8190pci *)skb->data;
 	memset(pTxFwInfo, 0, sizeof(struct tx_fwinfo_8190pci));
 	pTxFwInfo->TxHT = (cb_desc->data_rate & 0x80) ? 1 : 0;
@@ -1233,6 +1242,7 @@ void  rtl8192_tx_fill_desc(struct net_device *dev, struct tx_desc *pdesc,
 	pdesc->SecType = 0x0;
 	if (cb_desc->bHwSec) {
 		static u8 tmp;
+
 		if (!tmp) {
 			RT_TRACE(COMP_DBG, "==>================hw sec\n");
 			tmp = 1;
@@ -1293,6 +1303,7 @@ void  rtl8192_tx_fill_cmd_desc(struct net_device *dev,
 		entry->CmdInit = DESC_PACKET_TYPE_INIT;
 	} else {
 		struct tx_desc * entry_tmp = (struct tx_desc *)entry;
+
 		entry_tmp->CmdInit = DESC_PACKET_TYPE_NORMAL;
 		entry_tmp->Offset = sizeof(struct tx_fwinfo_8190pci) + 8;
 		entry_tmp->PktSize = (u16)(cb_desc->pkt_size +
@@ -2031,6 +2042,7 @@ bool rtl8192_rx_query_status_desc(struct net_device *dev,
 		return false;
 	} else {
 		struct rx_fwinfo *pDrvInfo = NULL;
+
 		stats->RxDrvInfoSize = pdesc->RxDrvInfoSize;
 		stats->RxBufShift = ((pdesc->Shift)&0x03);
 		stats->Decrypted = !pdesc->SWDec;
@@ -2220,6 +2232,7 @@ rtl8192_InitializeVariables(struct net_device  *dev)
 void rtl8192_EnableInterrupt(struct net_device *dev)
 {
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
 	priv->irq_enabled = 1;
 
 	write_nic_dword(dev, INTA_MASK, priv->irq_mask[0]);
@@ -2238,6 +2251,7 @@ void rtl8192_DisableInterrupt(struct net_device *dev)
 void rtl8192_ClearInterrupt(struct net_device *dev)
 {
 	u32 tmp = 0;
+
 	tmp = read_nic_dword(dev, ISR);
 	write_nic_dword(dev, ISR, tmp);
 }
@@ -2246,6 +2260,7 @@ void rtl8192_ClearInterrupt(struct net_device *dev)
 void rtl8192_enable_rx(struct net_device *dev)
 {
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
 	write_nic_dword(dev, RDQDA, priv->rx_ring_dma[RX_MPDU_QUEUE]);
 }
 
@@ -2358,6 +2373,7 @@ bool rtl8192_GetNmodeSupportBySecCfg(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
+
 	if (ieee->rtllib_ap_sec_type &&
 	   (ieee->rtllib_ap_sec_type(priv->rtllib)&(SEC_ALG_WEP |
 				     SEC_ALG_TKIP))) {
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
index 1a95d1f87f94..2e28744b9aab 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_firmware.c
@@ -261,6 +261,7 @@ bool init_firmware(struct net_device *dev)
 				};
 				const struct firmware *fw_entry;
 				int rc;
+
 				rc = request_firmware(&fw_entry,
 						      fw_name[init_step],
 						      &priv->pdev->dev);
diff --git a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
index 88489215dc2b..8c08ef6a40a4 100644
--- a/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
+++ b/drivers/staging/rtl8192e/rtl8192e/r8192E_phy.c
@@ -56,6 +56,7 @@ static void phy_FwRFSerialWrite(struct net_device *dev,
 static u32 rtl8192_CalculateBitShift(u32 dwBitMask)
 {
 	u32 i;
+
 	for (i = 0; i <= 31; i++) {
 		if (((dwBitMask >> i) & 0x1) == 1)
 			break;
@@ -67,6 +68,7 @@ u8 rtl8192_phy_CheckIsLegalRFPath(struct net_device *dev, u32 eRFPath)
 {
 	u8 ret = 1;
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	if (priv->rf_type == RF_2T4R)
 		ret = 0;
 	else if (priv->rf_type == RF_1T2R) {
@@ -112,6 +114,7 @@ static u32 rtl8192_phy_RFSerialRead(struct net_device *dev,
 	u32 ret = 0;
 	u32 NewOffset = 0;
 	struct bb_reg_definition *pPhyReg = &priv->PHYRegDef[eRFPath];
+
 	Offset &= 0x3f;
 
 	if (priv->rf_chip == RF_8256) {
@@ -261,6 +264,7 @@ u32 rtl8192_phy_QueryRFReg(struct net_device *dev, enum rf90_radio_path eRFPath,
 {
 	u32 Original_Value, Readback_Value, BitShift;
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
 		return 0;
 	if (priv->rtllib->eRFPowerState != eRfOn && !priv->being_init_adapter)
@@ -282,9 +286,9 @@ u32 rtl8192_phy_QueryRFReg(struct net_device *dev, enum rf90_radio_path eRFPath,
 static u32 phy_FwRFSerialRead(struct net_device *dev,
 			      enum rf90_radio_path eRFPath, u32 Offset)
 {
-	u32		retValue = 0;
 	u32		Data = 0;
 	u8		time = 0;
+
 	Data |= ((Offset & 0xFF) << 12);
 	Data |= ((eRFPath & 0x3) << 20);
 	Data |= 0x80000000;
@@ -301,9 +305,7 @@ static u32 phy_FwRFSerialRead(struct net_device *dev,
 		else
 			return 0;
 	}
-	retValue = read_nic_dword(dev, RF_DATA);
-
-	return	retValue;
+	return read_nic_dword(dev, RF_DATA);
 
 }	/* phy_FwRFSerialRead */
 
@@ -601,11 +603,8 @@ static bool rtl8192_BB_Config_ParaFile(struct net_device *dev)
 }
 bool rtl8192_BBConfig(struct net_device *dev)
 {
-	bool rtStatus = true;
-
 	rtl8192_InitBBRFRegDef(dev);
-	rtStatus = rtl8192_BB_Config_ParaFile(dev);
-	return rtStatus;
+	return rtl8192_BB_Config_ParaFile(dev);
 }
 
 void rtl8192_phy_getTxPower(struct net_device *dev)
@@ -700,6 +699,7 @@ bool rtl8192_phy_RFConfig(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	bool rtStatus = true;
+
 	switch (priv->rf_chip) {
 	case RF_8225:
 		break;
@@ -1018,6 +1018,7 @@ void rtl8192_SwChnl_WorkItem(struct net_device *dev)
 u8 rtl8192_phy_SwChnl(struct net_device *dev, u8 channel)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	RT_TRACE(COMP_PHY, "=====>%s()\n", __func__);
 	if (IS_NIC_DOWN(priv)) {
 		RT_TRACE(COMP_ERR, "%s(): ERR !! driver is not up\n", __func__);
@@ -1438,6 +1439,7 @@ static bool SetRFPowerState8190(struct net_device *dev,
 			     RT_IN_PS_LEVEL(pPSC, RT_RF_OFF_LEVL_HALT_NIC)) {
 				bool rtstatus = true;
 				u32 InitilizeCount = 3;
+
 				do {
 					InitilizeCount--;
 					priv->RegRfOff = false;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
index c46c65c5542f..6c64e0899ffd 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_cam.c
@@ -53,6 +53,7 @@ void EnableHWSecurityConfig8192(struct net_device *dev)
 	u8 SECR_value = 0x0;
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
+
 	SECR_value = SCR_TxEncEnable | SCR_RxDecEnable;
 	if (((KEY_TYPE_WEP40 == ieee->pairwise_key_type) ||
 	     (KEY_TYPE_WEP104 == ieee->pairwise_key_type)) &&
@@ -84,6 +85,7 @@ void set_swcam(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_device *ieee = priv->rtllib;
+
 	RT_TRACE(COMP_DBG, "===========>%s():EntryNo is %d,KeyIndex is "
 		 "%d,KeyType is %d,is_mesh is %d\n", __func__, EntryNo,
 		 KeyIndex, KeyType, is_mesh);
@@ -106,6 +108,7 @@ void setKey(struct net_device *dev, u8 EntryNo, u8 KeyIndex, u16 KeyType,
 	u8 i;
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
 	enum rt_rf_power_state rtState;
+
 	rtState = priv->rtllib->eRFPowerState;
 	if (priv->rtllib->PowerSaveControl.bInactivePs) {
 		if (rtState == eRfOff) {
@@ -171,6 +174,7 @@ void CAM_read_entry(struct net_device *dev, u32 iIndex)
 	u8 entry_i = 0;
 	u32 ulStatus;
 	s32 i = 100;
+
 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
 		target_command = entry_i+CAM_CONTENT_COUNT*iIndex;
 		target_command = target_command | BIT31;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
index 5729cf678765..5615c80f614c 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_core.c
@@ -325,6 +325,7 @@ bool MgntActSet_RF_State(struct net_device *dev,
 	enum rt_rf_power_state rtState;
 	u16			RFWaitCounter = 0;
 	unsigned long flag;
+
 	RT_TRACE((COMP_PS | COMP_RF), "===>MgntActSet_RF_State(): "
 		 "StateToSet(%d)\n", StateToSet);
 
@@ -486,6 +487,7 @@ void rtl8192_tx_timeout(struct net_device *dev)
 void rtl8192_irq_enable(struct net_device *dev)
 {
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
 	priv->irq_enabled = 1;
 
 	priv->ops->irq_enable(dev);
@@ -598,9 +600,9 @@ static void rtl8192_qos_activate(void *data)
 	RT_TRACE(COMP_QOS, "qos active process with associate response "
 		 "received\n");
 
-	for (i = 0; i <  QOS_QUEUE_NUM; i++) {
+	for (i = 0; i <  QOS_QUEUE_NUM; i++)
 		priv->rtllib->SetHwRegHandler(dev, HW_VAR_AC_PARAM, (u8 *)(&i));
-	}
+
 
 success:
 	mutex_unlock(&priv->mutex);
@@ -667,19 +669,18 @@ static int rtl8192_handle_beacon(struct net_device *dev,
 static int rtl8192_qos_association_resp(struct r8192_priv *priv,
 	struct rtllib_network *network)
 {
-	int ret = 0;
 	unsigned long flags;
 	u32 size = sizeof(struct rtllib_qos_parameters);
 	int set_qos_param = 0;
 
 	if ((priv == NULL) || (network == NULL))
-		return ret;
+		return 0;
 
 	if (priv->rtllib->state != RTLLIB_LINKED)
-		return ret;
+		return 0;
 
 	if ((priv->rtllib->iw_mode != IW_MODE_INFRA))
-		return ret;
+		return 0;
 
 	spin_lock_irqsave(&priv->rtllib->lock, flags);
 	if (network->flags & NETWORK_HAS_QOS_PARAMETERS) {
@@ -709,7 +710,7 @@ static int rtl8192_qos_association_resp(struct r8192_priv *priv,
 		dm_init_edca_turbo(priv->rtllib->dev);
 		queue_work_rsl(priv->priv_wq, &priv->qos_activate);
 	}
-	return ret;
+	return 0;
 }
 
 static int rtl8192_handle_assoc_response(struct net_device *dev,
@@ -717,6 +718,7 @@ static int rtl8192_handle_assoc_response(struct net_device *dev,
 				 struct rtllib_network *network)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	rtl8192_qos_association_resp(priv, network);
 	return 0;
 }
@@ -762,6 +764,7 @@ void rtl8192_config_rate(struct net_device *dev, u16 *rate_config)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	struct rtllib_network *net;
 	u8 i = 0, basic_rate = 0;
+
 	net = &priv->rtllib->current_network;
 
 	for (i = 0; i < net->rates_len; i++) {
@@ -852,6 +855,7 @@ void rtl8192_config_rate(struct net_device *dev, u16 *rate_config)
 static void rtl8192_refresh_supportrate(struct r8192_priv *priv)
 {
 	struct rtllib_device *ieee = priv->rtllib;
+
 	if (ieee->mode == WIRELESS_MODE_N_24G ||
 	    ieee->mode == WIRELESS_MODE_N_5G) {
 		memcpy(ieee->Regdot11HTOperationalRateSet,
@@ -941,6 +945,7 @@ static int _rtl8192_sta_up(struct net_device *dev, bool is_silent_reset)
 	struct rt_pwr_save_ctrl *pPSC = (struct rt_pwr_save_ctrl *)
 					(&(priv->rtllib->PowerSaveControl));
 	bool init_status = true;
+
 	priv->bDriverIsGoingToUnload = false;
 	priv->bdisable_nic = false;
 
@@ -1270,6 +1275,7 @@ static short rtl8192_get_channel_map(struct net_device *dev)
 	int i;
 
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	if ((priv->rf_chip != RF_8225) && (priv->rf_chip != RF_8256)
 			&& (priv->rf_chip != RF_6052)) {
 		RT_TRACE(COMP_ERR, "%s: unknown rf chip, can't set channel "
@@ -1346,6 +1352,7 @@ short rtl8192_is_tx_queue_empty(struct net_device *dev)
 {
 	int i = 0;
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	for (i = 0; i <= MGNT_QUEUE; i++) {
 		if ((i == TXCMD_QUEUE) || (i == HCCA_QUEUE))
 			continue;
@@ -1503,7 +1510,7 @@ RESET_START:
 			RT_TRACE(COMP_ERR, "%s():the driver is not up! "
 				 "return\n", __func__);
 			up(&priv->wx_sem);
-			return ;
+			return;
 		}
 		priv->up = 0;
 
@@ -1783,6 +1790,7 @@ void	rtl819x_watchdog_wqcallback(void *data)
 void watch_dog_timer_callback(unsigned long data)
 {
 	struct r8192_priv *priv = rtllib_priv((struct net_device *)data);
+
 	queue_delayed_work_rsl(priv->priv_wq, &priv->watch_dog_wq, 0);
 	mod_timer(&priv->watch_dog_timer, jiffies +
 		  MSECS(RTLLIB_WATCH_DOG_TIME));
@@ -1794,6 +1802,7 @@ void watch_dog_timer_callback(unsigned long data)
 void rtl8192_rx_enable(struct net_device *dev)
 {
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
 	priv->ops->rx_enable(dev);
 }
 
@@ -1816,6 +1825,7 @@ static void rtl8192_free_rx_ring(struct net_device *dev)
 	     rx_queue_idx++) {
 		for (i = 0; i < priv->rxringcount; i++) {
 			struct sk_buff *skb = priv->rx_buf[rx_queue_idx][i];
+
 			if (!skb)
 				continue;
 
@@ -2080,6 +2090,7 @@ static short rtl8192_alloc_rx_desc_ring(struct net_device *dev)
 		for (i = 0; i < priv->rxringcount; i++) {
 			struct sk_buff *skb = dev_alloc_skb(priv->rxbuffersize);
 			dma_addr_t *mapping;
+
 			entry = &priv->rx_ring[rx_queue_idx][i];
 			if (!skb)
 				return 0;
@@ -2171,6 +2182,7 @@ void rtl8192_pci_resetdescring(struct net_device *dev)
 	for (rx_queue_idx = 0; rx_queue_idx < MAX_RX_QUEUE; rx_queue_idx++) {
 		if (priv->rx_ring[rx_queue_idx]) {
 			struct rx_desc *entry = NULL;
+
 			for (i = 0; i < priv->rxringcount; i++) {
 				entry = &priv->rx_ring[rx_queue_idx][i];
 				entry->OWN = 1;
@@ -2691,6 +2703,7 @@ static irqreturn_t rtl8192_interrupt(int irq, void *netdev)
 	unsigned long flags;
 	u32 inta;
 	u32 intb;
+
 	intb = 0;
 
 	if (priv->irq_enabled == 0)
@@ -3049,6 +3062,7 @@ bool NicIFDisableNIC(struct net_device *dev)
 	bool	status = true;
 	struct r8192_priv *priv = rtllib_priv(dev);
 	u8 tmp_state = 0;
+
 	RT_TRACE(COMP_PS, "=========>%s()\n", __func__);
 	priv->bdisable_nic = true;
 	tmp_state = priv->rtllib->state;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
index 2297fc20fd4e..b6ce8c3b2f8c 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_dm.c
@@ -137,6 +137,7 @@ static	void	dm_ctstoself(struct net_device *dev);
 void init_hal_dm(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	priv->DM_Type = DM_Type_ByDriver;
 
 	priv->undecorated_smoothed_pwdb = -1;
@@ -167,6 +168,7 @@ void deinit_hal_dm(struct net_device *dev)
 void hal_dm_watchdog(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	if (priv->being_init_adapter)
 		return;
 
@@ -343,6 +345,7 @@ static void dm_check_rate_adaptive(struct net_device *dev)
 		currentRATR = read_nic_dword(dev, RATR0);
 		if (targetRATR !=  currentRATR) {
 			u32 ratr_value;
+
 			ratr_value = targetRATR;
 			RT_TRACE(COMP_RATE,
 				 "currentRATR = %x, targetRATR = %x\n",
@@ -1362,6 +1365,7 @@ static void dm_InitializeTXPowerTracking_ThermalMeter(struct net_device *dev)
 void dm_initialize_txpower_tracking(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	if (priv->IC_Cut >= IC_VersionCut_D)
 		dm_InitializeTXPowerTracking_TSSI(dev);
 	else
@@ -1372,6 +1376,7 @@ static void dm_CheckTXPowerTracking_TSSI(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	static u32 tx_power_track_counter;
+
 	RT_TRACE(COMP_POWER_TRACKING, "%s()\n", __func__);
 	if (read_nic_byte(dev, 0x11e) == 1)
 		return;
@@ -1437,6 +1442,7 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 {
 	u32 TempVal;
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	TempVal = 0;
 	if (!bInCH14) {
 		TempVal = (u32)(priv->cck_txbbgain_table[(u8)(priv->CCKPresentAttentuation)].ccktxbb_valuearray[0] +
@@ -1530,6 +1536,7 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 void dm_cck_txpower_adjust(struct net_device *dev, bool  binch14)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	if (priv->IC_Cut >= IC_VersionCut_D)
 		dm_CCKTxPowerAdjust_TSSI(dev, binch14);
 	else
@@ -1702,6 +1709,7 @@ void dm_change_dynamic_initgain_thresh(struct net_device *dev,
 static void dm_dig_init(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	dm_digtable.dig_enable_flag	= true;
 	dm_digtable.Backoff_Enable_Flag = true;
 
@@ -2129,6 +2137,7 @@ static void dm_check_edca_turbo(struct net_device *dev)
 			"self_softap"
 		};
 		static int wb_tmp;
+
 		if (wb_tmp == 0) {
 			printk(KERN_INFO "%s():iot peer is %s, bssid:"
 			       " %pM\n", __func__,
@@ -2187,6 +2196,7 @@ static void dm_check_edca_turbo(struct net_device *dev)
 	} else {
 		 if (priv->bcurrent_turbo_EDCA) {
 			u8 tmp = AC0_BE;
+
 			priv->rtllib->SetHwRegHandler(dev, HW_VAR_AC_PARAM, (u8 *)(&tmp));
 			priv->bcurrent_turbo_EDCA = false;
 		}
@@ -2275,12 +2285,12 @@ void dm_CheckRfCtrlGPIO(void *data)
 
 	if (priv->bHwRadioOff && (eRfPowerStateToSet == eRfOn)) {
 		RT_TRACE(COMP_RF, "gpiochangeRF  - HW Radio ON\n");
-		printk(KERN_INFO "gpiochangeRF  - HW Radio ON\n");
+		netdev_info(dev, "gpiochangeRF  - HW Radio ON\n");
 		priv->bHwRadioOff = false;
 		bActuallySet = true;
 	} else if (!priv->bHwRadioOff && (eRfPowerStateToSet == eRfOff)) {
 		RT_TRACE(COMP_RF, "gpiochangeRF  - HW Radio OFF\n");
-		printk(KERN_INFO "gpiochangeRF  - HW Radio OFF\n");
+		netdev_info(dev, "gpiochangeRF  - HW Radio OFF\n");
 		priv->bHwRadioOff = true;
 		bActuallySet = true;
 	}
@@ -2581,6 +2591,7 @@ static void dm_init_fsync(struct net_device *dev)
 static void dm_deInit_fsync(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	del_timer_sync(&priv->fsync_timer);
 }
 
@@ -2596,6 +2607,7 @@ void dm_fsync_timer_callback(unsigned long data)
 	    priv->rtllib->bfsync_enable &&
 	    (priv->rtllib->pHTInfo->IOTAction & HT_IOT_ACT_CDD_FSYNC)) {
 		u32 rate_bitmap;
+
 		for (rate_index = 0; rate_index <= 27; rate_index++) {
 			rate_bitmap  = 1 << rate_index;
 			if (priv->rtllib->fsync_rate_bitmap &  rate_bitmap)
@@ -2690,6 +2702,7 @@ static void dm_StartHWFsync(struct net_device *dev)
 {
 	u8 rf_timing = 0x77;
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c12cf);
 	priv->rtllib->SetHwRegHandler(dev, HW_VAR_RF_TIMING,
@@ -2701,6 +2714,7 @@ static void dm_EndHWFsync(struct net_device *dev)
 {
 	u8 rf_timing = 0xaa;
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 	priv->rtllib->SetHwRegHandler(dev, HW_VAR_RF_TIMING, (u8 *)
@@ -2923,6 +2937,7 @@ static void dm_dynamic_txpower(struct net_device *dev)
 	struct r8192_priv *priv = rtllib_priv(dev);
 	unsigned int txhipower_threshhold = 0;
 	unsigned int txlowpower_threshold = 0;
+
 	if (priv->rtllib->bdynamic_txpower_enable != true) {
 		priv->bDynamicTxHighPower = false;
 		priv->bDynamicTxLowPower = false;
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
index a8c2ade4f435..4856b76292f9 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_ps.c
@@ -33,6 +33,7 @@ static void rtl8192_hw_sleep_down(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	unsigned long flags = 0;
+
 	spin_lock_irqsave(&priv->rf_ps_lock, flags);
 	if (priv->RFChangeInProgress) {
 		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
@@ -51,6 +52,7 @@ void rtl8192_hw_sleep_wq(void *data)
 	struct rtllib_device *ieee = container_of_dwork_rsl(data,
 				     struct rtllib_device, hw_sleep_wq);
 	struct net_device *dev = ieee->dev;
+
 	rtl8192_hw_sleep_down(dev);
 }
 
@@ -58,6 +60,7 @@ void rtl8192_hw_wakeup(struct net_device *dev)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
 	unsigned long flags = 0;
+
 	spin_lock_irqsave(&priv->rf_ps_lock, flags);
 	if (priv->RFChangeInProgress) {
 		spin_unlock_irqrestore(&priv->rf_ps_lock, flags);
@@ -77,8 +80,8 @@ void rtl8192_hw_wakeup_wq(void *data)
 	struct rtllib_device *ieee = container_of_dwork_rsl(data,
 				     struct rtllib_device, hw_wakeup_wq);
 	struct net_device *dev = ieee->dev;
-	rtl8192_hw_wakeup(dev);
 
+	rtl8192_hw_wakeup(dev);
 }
 
 #define MIN_SLEEP_TIME 50
@@ -179,6 +182,7 @@ void IPSLeave_wq(void *data)
 				     struct rtllib_device, ips_leave_wq);
 	struct net_device *dev = ieee->dev;
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
 	down(&priv->rtllib->ips_sem);
 	IPSLeave(dev);
 	up(&priv->rtllib->ips_sem);
@@ -188,6 +192,7 @@ void rtllib_ips_leave_wq(struct net_device *dev)
 {
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
 	enum rt_rf_power_state rtState;
+
 	rtState = priv->rtllib->eRFPowerState;
 
 	if (priv->rtllib->PowerSaveControl.bInactivePs) {
@@ -209,6 +214,7 @@ void rtllib_ips_leave_wq(struct net_device *dev)
 void rtllib_ips_leave(struct net_device *dev)
 {
 	struct r8192_priv *priv = (struct r8192_priv *)rtllib_priv(dev);
+
 	down(&priv->rtllib->ips_sem);
 	IPSLeave(dev);
 	up(&priv->rtllib->ips_sem);
diff --git a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
index 52870041fcb7..df0323f00f69 100644
--- a/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
+++ b/drivers/staging/rtl8192e/rtl8192e/rtl_wx.c
@@ -54,6 +54,7 @@ static int r8192_wx_get_rate(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	return rtllib_wx_get_rate(priv->rtllib, info, wrqu, extra);
 }
 
@@ -103,6 +104,7 @@ static int r8192_wx_get_rts(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	return rtllib_wx_get_rts(priv->rtllib, info, wrqu, extra);
 }
 
@@ -132,6 +134,7 @@ static int r8192_wx_get_power(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	return rtllib_wx_get_power(priv->rtllib, info, wrqu, extra);
 }
 
@@ -480,6 +483,7 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 
 	if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 		struct iw_scan_req *req = (struct iw_scan_req *)b;
+
 		if (req->essid_len) {
 			ieee->current_network.ssid_len = req->essid_len;
 			memcpy(ieee->current_network.ssid, req->essid,
@@ -654,6 +658,7 @@ static int r8192_wx_get_name(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	return rtllib_wx_get_name(priv->rtllib, info, wrqu, extra);
 }
 
@@ -934,6 +939,7 @@ static int r8192_wx_get_sens(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	struct r8192_priv *priv = rtllib_priv(dev);
+
 	if (priv->rf_set_sens == NULL)
 		return -1; /* we have not this support for this radio */
 	wrqu->sens.value = priv->sens;
@@ -995,6 +1001,7 @@ static int r8192_wx_set_enc_ext(struct net_device *dev,
 		struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
 		struct iw_point *encoding = &wrqu->encoding;
 		u8 idx = 0, alg = 0, group = 0;
+
 		if ((encoding->flags & IW_ENCODE_DISABLED) ||
 		     ext->alg == IW_ENCODE_ALG_NONE) {
 			ieee->pairwise_key_type = ieee->group_key_type
@@ -1309,6 +1316,7 @@ static struct iw_statistics *r8192_get_wireless_stats(struct net_device *dev)
 	int tmp_level = 0;
 	int tmp_qual = 0;
 	int tmp_noise = 0;
+
 	if (ieee->state < RTLLIB_LINKED) {
 		wstats->qual.qual = 10;
 		wstats->qual.level = 0;
diff --git a/drivers/staging/rtl8192e/rtl819x_BAProc.c b/drivers/staging/rtl8192e/rtl819x_BAProc.c
index 48a142b82d90..6da57847a533 100644
--- a/drivers/staging/rtl8192e/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_BAProc.c
@@ -196,6 +196,7 @@ static void rtllib_send_ADDBAReq(struct rtllib_device *ieee, u8 *dst,
 				 struct ba_record *pBA)
 {
 	struct sk_buff *skb = NULL;
+
 	skb = rtllib_ADDBA(ieee, dst, pBA, 0, ACT_ADDBAREQ);
 
 	if (skb) {
@@ -205,20 +206,19 @@ static void rtllib_send_ADDBAReq(struct rtllib_device *ieee, u8 *dst,
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
 			     " %s()\n", __func__);
 	}
-	return;
 }
 
 static void rtllib_send_ADDBARsp(struct rtllib_device *ieee, u8 *dst,
 				 struct ba_record *pBA, u16 StatusCode)
 {
 	struct sk_buff *skb = NULL;
+
 	skb = rtllib_ADDBA(ieee, dst, pBA, StatusCode, ACT_ADDBARSP);
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
 			     " %s()\n", __func__);
-	return;
 }
 
 static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
@@ -226,13 +226,13 @@ static void rtllib_send_DELBA(struct rtllib_device *ieee, u8 *dst,
 			      u16 ReasonCode)
 {
 	struct sk_buff *skb = NULL;
+
 	skb = rtllib_DELBA(ieee, dst, pBA, TxRxSelect, ReasonCode);
 	if (skb)
 		softmac_mgmt_xmit(skb, ieee);
 	else
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "alloc skb error in function"
 			     " %s()\n", __func__);
-	return ;
 }
 
 int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
@@ -312,6 +312,7 @@ int rtllib_rx_ADDBAReq(struct rtllib_device *ieee, struct sk_buff *skb)
 OnADDBAReq_Fail:
 	{
 		struct ba_record BA;
+
 		BA.BaParamSet = *pBaParamSet;
 		BA.BaTimeoutValue = *pBaTimeoutVal;
 		BA.DialogToken = *pDialogToken;
@@ -372,7 +373,7 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 	pAdmittedBA = &pTS->TxAdmittedBARecord;
 
 
-	if ((pAdmittedBA->bValid == true)) {
+	if (pAdmittedBA->bValid == true) {
 		RTLLIB_DEBUG(RTLLIB_DL_BA, "OnADDBARsp(): Recv ADDBA Rsp."
 			     " Drop because already admit it!\n");
 		return -1;
@@ -416,6 +417,7 @@ int rtllib_rx_ADDBARsp(struct rtllib_device *ieee, struct sk_buff *skb)
 OnADDBARsp_Reject:
 	{
 		struct ba_record BA;
+
 		BA.BaParamSet = *pBaParamSet;
 		rtllib_send_DELBA(ieee, dst, &BA, TX_DIR, ReasonCode);
 		return 0;
@@ -561,5 +563,4 @@ void RxBaInactTimeout(unsigned long data)
 	rtllib_send_DELBA(ieee, pRxTs->TsCommonInfo.Addr,
 			  &pRxTs->RxAdmittedBARecord, RX_DIR,
 			  DELBA_REASON_TIMEOUT);
-	return ;
 }
diff --git a/drivers/staging/rtl8192e/rtl819x_HTProc.c b/drivers/staging/rtl8192e/rtl819x_HTProc.c
index 8b7412980ebb..c7f45080061f 100644
--- a/drivers/staging/rtl8192e/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_HTProc.c
@@ -211,7 +211,6 @@ void HTDebugHTInfo(u8 *InfoIE, u8 *TitleString)
 		     "[%x]\n", pHTInfoEle->BasicMSC[0], pHTInfoEle->BasicMSC[1],
 		     pHTInfoEle->BasicMSC[2], pHTInfoEle->BasicMSC[3],
 		     pHTInfoEle->BasicMSC[4]);
-	return;
 }
 
 static bool IsHTHalfNmode40Bandwidth(struct rtllib_device *ieee)
@@ -340,6 +339,7 @@ static void HTIOTPeerDetermine(struct rtllib_device *ieee)
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
 	struct rtllib_network *net = &ieee->current_network;
+
 	if (net->bssht.bdRT2RTAggregation) {
 		pHTInfo->IOTPeer = HT_IOT_PEER_REALTEK;
 		if (net->bssht.RT2RT_HT_Mode & RT_HT_CAP_USE_92SE)
@@ -425,6 +425,7 @@ static u8 HTIOTActIsCCDFsync(struct rtllib_device *ieee)
 static void HTIOTActDetermineRaFunc(struct rtllib_device *ieee, bool bPeerRx2ss)
 {
 	struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
 	pHTInfo->IOTRaFunc &= HT_IOT_RAFUNC_DISABLE_ALL;
 
 	if (pHTInfo->IOTPeer == HT_IOT_PEER_RALINK && !bPeerRx2ss)
@@ -457,6 +458,7 @@ void HTConstructCapabilityElement(struct rtllib_device *ieee, u8 *posHTCap,
 
 	if ((bAssoc) && (pHT->ePeerHTSpecVer == HT_SPEC_VER_EWC)) {
 		u8	EWC11NHTCap[] = {0x00, 0x90, 0x4c, 0x33};
+
 		memcpy(posHTCap, EWC11NHTCap, sizeof(EWC11NHTCap));
 		pCapELE = (struct ht_capab_ele *)&(posHTCap[4]);
 		*len = 30 + 2;
@@ -521,7 +523,6 @@ void HTConstructCapabilityElement(struct rtllib_device *ieee, u8 *posHTCap,
 			pCapELE->MCS[1] = 0;
 		}
 	}
-	return;
 }
 
 void HTConstructInfoElement(struct rtllib_device *ieee, u8 *posHTInfo,
@@ -529,6 +530,7 @@ void HTConstructInfoElement(struct rtllib_device *ieee, u8 *posHTInfo,
 {
 	struct rt_hi_throughput *pHT = ieee->pHTInfo;
 	struct ht_info_ele *pHTInfoEle = (struct ht_info_ele *)posHTInfo;
+
 	if ((posHTInfo == NULL) || (pHTInfoEle == NULL)) {
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "posHTInfo or pHTInfoEle can't be "
 			     "null in HTConstructInfoElement()\n");
@@ -564,7 +566,6 @@ void HTConstructInfoElement(struct rtllib_device *ieee, u8 *posHTInfo,
 	} else {
 		*len = 0;
 	}
-	return;
 }
 
 void HTConstructRT2RTAggElement(struct rtllib_device *ieee, u8 *posRT2RTAgg,
@@ -588,13 +589,12 @@ void HTConstructRT2RTAggElement(struct rtllib_device *ieee, u8 *posRT2RTAgg,
 		*posRT2RTAgg |= RT_HT_CAP_USE_WOW;
 
 	*len = 6 + 2;
-
-	return;
 }
 
 static u8 HT_PickMCSRate(struct rtllib_device *ieee, u8 *pOperateMCS)
 {
 	u8 i;
+
 	if (pOperateMCS == NULL) {
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "pOperateMCS can't be null"
 			     " in HT_PickMCSRate()\n");
@@ -629,6 +629,7 @@ u8 HTGetHighestMCSRate(struct rtllib_device *ieee, u8 *pMCSRateSet,
 	u8		bitMap;
 	u8		mcsRate = 0;
 	u8		availableMcsRate[16];
+
 	if (pMCSRateSet == NULL || pMCSFilter == NULL) {
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "pMCSRateSet or pMCSFilter can't "
 			     "be null in HTGetHighestMCSRate()\n");
@@ -846,6 +847,7 @@ void HTInitializeHTInfo(struct rtllib_device *ieee)
 
 	{
 		u8 *RegHTSuppRateSets = &(ieee->RegHTSuppRateSet[0]);
+
 		RegHTSuppRateSets[0] = 0xFF;
 		RegHTSuppRateSets[1] = 0xFF;
 		RegHTSuppRateSets[4] = 0x01;
@@ -991,7 +993,6 @@ void HTUseDefaultSetting(struct rtllib_device *ieee)
 	} else {
 		pHTInfo->bCurrentHTSupport = false;
 	}
-	return;
 }
 
 u8 HTCCheck(struct rtllib_device *ieee, u8 *pFrame)
@@ -1047,7 +1048,7 @@ void HTSetConnectBwMode(struct rtllib_device *ieee,
 		Bandwidth = HT_CHANNEL_WIDTH_20;
 
 	if (pHTInfo->bSwBwInProgress) {
-		printk(KERN_INFO "%s: bSwBwInProgress!!\n", __func__);
+		pr_info("%s: bSwBwInProgress!!\n", __func__);
 		return;
 	}
 	if (Bandwidth == HT_CHANNEL_WIDTH_20_40) {
@@ -1067,7 +1068,7 @@ void HTSetConnectBwMode(struct rtllib_device *ieee,
 		pHTInfo->CurSTAExtChnlOffset = HT_EXTCHNL_OFFSET_NO_EXT;
 	}
 
-	printk(KERN_INFO "%s():pHTInfo->bCurBW40MHz:%x\n", __func__,
+	pr_info("%s():pHTInfo->bCurBW40MHz:%x\n", __func__,
 	       pHTInfo->bCurBW40MHz);
 
 	pHTInfo->bSwBwInProgress = true;
diff --git a/drivers/staging/rtl8192e/rtl819x_TSProc.c b/drivers/staging/rtl8192e/rtl819x_TSProc.c
index 52b297724fb1..294847d1a903 100644
--- a/drivers/staging/rtl8192e/rtl819x_TSProc.c
+++ b/drivers/staging/rtl8192e/rtl819x_TSProc.c
@@ -143,6 +143,7 @@ void TSInitialize(struct rtllib_device *ieee)
 	struct rx_ts_record *pRxTS  = ieee->RxTsRecord;
 	struct rx_reorder_entry *pRxReorderEntry = ieee->RxReorderEntry;
 	u8				count = 0;
+
 	RTLLIB_DEBUG(RTLLIB_DL_TS, "==========>%s()\n", __func__);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Admit_List);
 	INIT_LIST_HEAD(&ieee->Tx_TS_Pending_List);
@@ -233,6 +234,7 @@ static struct ts_common_info *SearchAdmitTRStream(struct rtllib_device *ieee,
 	bool	search_dir[4] = {0};
 	struct list_head *psearch_list;
 	struct ts_common_info *pRet = NULL;
+
 	if (ieee->iw_mode == IW_MODE_MASTER) {
 		if (TxRxSelect == TX_DIR) {
 			search_dir[DIR_DOWN] = true;
@@ -278,9 +280,8 @@ static struct ts_common_info *SearchAdmitTRStream(struct rtllib_device *ieee,
 	}
 
 	if (pRet && &pRet->List  != psearch_list)
-		return pRet ;
-	else
-		return NULL;
+		return pRet;
+	return NULL;
 }
 
 static void MakeTSEntry(struct ts_common_info *pTsCommonInfo, u8 *Addr,
@@ -310,6 +311,7 @@ bool GetTs(struct rtllib_device *ieee, struct ts_common_info **ppTS,
 	   u8 *Addr, u8 TID, enum tr_select TxRxSelect, bool bAddNewTs)
 {
 	u8	UP = 0;
+
 	if (is_multicast_ether_addr(Addr)) {
 		RTLLIB_DEBUG(RTLLIB_DL_ERR, "ERR! get TS for Broadcast or "
 			     "Multicast\n");
@@ -443,6 +445,7 @@ static void RemoveTsEntry(struct rtllib_device *ieee, struct ts_common_info *pTs
 			{
 				int i = 0;
 				struct rtllib_rxb *prxb = pRxReorderEntry->prxb;
+
 				if (unlikely(!prxb))
 					return;
 				for (i = 0; i < prxb->nr_subframes; i++)
@@ -455,6 +458,7 @@ static void RemoveTsEntry(struct rtllib_device *ieee, struct ts_common_info *pTs
 		}
 	} else {
 		struct tx_ts_record *pTxTS = (struct tx_ts_record *)pTs;
+
 		del_timer_sync(&pTxTS->TsAddBaTimer);
 	}
 }
@@ -462,6 +466,7 @@ static void RemoveTsEntry(struct rtllib_device *ieee, struct ts_common_info *pTs
 void RemovePeerTS(struct rtllib_device *ieee, u8 *Addr)
 {
 	struct ts_common_info *pTS, *pTmpTS;
+
 	printk(KERN_INFO "===========>RemovePeerTS, %pM\n", Addr);
 
 	list_for_each_entry_safe(pTS, pTmpTS, &ieee->Tx_TS_Pending_List, List) {
diff --git a/drivers/staging/rtl8192e/rtllib.h b/drivers/staging/rtl8192e/rtllib.h
index 83f5f57373a6..2d82f8993ea1 100644
--- a/drivers/staging/rtl8192e/rtllib.h
+++ b/drivers/staging/rtl8192e/rtllib.h
@@ -1456,7 +1456,7 @@ enum eap_type {
 	EAPOL_ENCAP_ASF_ALERT
 };
 
-static const char *eap_types[] = {
+static const char * const eap_types[] = {
 	[EAP_PACKET]		= "EAP-Packet",
 	[EAPOL_START]		= "EAPOL-Start",
 	[EAPOL_LOGOFF]		= "EAPOL-Logoff",
@@ -1473,6 +1473,7 @@ static inline u8 Frame_QoSTID(u8 *buf)
 {
 	struct rtllib_hdr_3addr *hdr;
 	u16 fc;
+
 	hdr = (struct rtllib_hdr_3addr *)buf;
 	fc = le16_to_cpu(hdr->frame_ctl);
 	return (u8)((union frameqos *)(buf + (((fc & RTLLIB_FCTL_TODS) &&
@@ -2513,7 +2514,7 @@ struct rtllib_device {
 	void (*AllowAllDestAddrHandler)(struct net_device *dev,
 					bool bAllowAllDA, bool WriteIntoReg);
 
-	void (*rtllib_ips_leave_wq) (struct net_device *dev);
+	void (*rtllib_ips_leave_wq)(struct net_device *dev);
 	void (*rtllib_ips_leave)(struct net_device *dev);
 	void (*LeisurePSLeave)(struct net_device *dev);
 	void (*rtllib_rfkill_poll)(struct net_device *dev);
@@ -2956,25 +2957,13 @@ extern inline int rtllib_get_scans(struct rtllib_device *ieee)
 static inline const char *escape_essid(const char *essid, u8 essid_len)
 {
 	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
-	const char *s = essid;
-	char *d = escaped;
 
 	if (rtllib_is_empty_essid(essid, essid_len)) {
 		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
 		return escaped;
 	}
 
-	essid_len = min(essid_len, (u8)IW_ESSID_MAX_SIZE);
-	while (essid_len--) {
-		if (*s == '\0') {
-			*d++ = '\\';
-			*d++ = '0';
-			s++;
-		} else {
-			*d++ = *s++;
-		}
-	}
-	*d = '\0';
+	snprintf(escaped, sizeof(escaped), "%*pEn", essid_len, essid);
 	return escaped;
 }
 
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
index 5e5c76bcdbd0..01f85db98e0d 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_ccmp.c
@@ -69,7 +69,7 @@ static void *rtllib_ccmp_init(int key_idx)
 
 	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tfm)) {
-		printk(KERN_DEBUG "rtllib_crypt_ccmp: could not allocate "
+		pr_debug("rtllib_crypt_ccmp: could not allocate "
 		       "crypto API aes\n");
 		priv->tfm = NULL;
 		goto fail;
@@ -90,6 +90,7 @@ fail:
 static void rtllib_ccmp_deinit(void *priv)
 {
 	struct rtllib_ccmp_data *_priv = priv;
+
 	if (_priv && _priv->tfm)
 		crypto_free_cipher((void *)_priv->tfm);
 	kfree(priv);
@@ -99,6 +100,7 @@ static void rtllib_ccmp_deinit(void *priv)
 static inline void xor_block(u8 *b, u8 *a, size_t len)
 {
 	int i;
+
 	for (i = 0; i < len; i++)
 		b[i] ^= a[i];
 }
@@ -276,7 +278,7 @@ static int rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	keyidx = pos[3];
 	if (!(keyidx & (1 << 5))) {
 		if (net_ratelimit()) {
-			printk(KERN_DEBUG "CCMP: received packet without ExtIV"
+			pr_debug("CCMP: received packet without ExtIV"
 			       " flag from %pM\n", hdr->addr2);
 		}
 		key->dot11RSNAStatsCCMPFormatErrors++;
@@ -284,13 +286,13 @@ static int rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	}
 	keyidx >>= 6;
 	if (key->key_idx != keyidx) {
-		printk(KERN_DEBUG "CCMP: RX tkey->key_idx=%d frame "
+		pr_debug("CCMP: RX tkey->key_idx=%d frame "
 		       "keyidx=%d priv=%p\n", key->key_idx, keyidx, priv);
 		return -6;
 	}
 	if (!key->key_set) {
 		if (net_ratelimit()) {
-			printk(KERN_DEBUG "CCMP: received packet from %pM"
+			pr_debug("CCMP: received packet from %pM"
 			       " with keyid=%d that does not have a configured"
 			       " key\n", hdr->addr2, keyidx);
 		}
@@ -339,7 +341,7 @@ static int rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 
 		if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
 			if (net_ratelimit()) {
-				printk(KERN_DEBUG "CCMP: decrypt failed: STA="
+				pr_debug("CCMP: decrypt failed: STA="
 				" %pM\n", hdr->addr2);
 			}
 			key->dot11RSNAStatsCCMPDecryptErrors++;
@@ -415,6 +417,7 @@ static int rtllib_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
 static void rtllib_ccmp_print_stats(struct seq_file *m, void *priv)
 {
 	struct rtllib_ccmp_data *ccmp = priv;
+
 	seq_printf(m,
 		   "key[%d] alg=CCMP key_set=%d "
 		   "tx_pn=%pM rx_pn=%pM "
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
index 7b5366bba353..78db2b6826f7 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_tkip.c
@@ -285,6 +285,7 @@ static void tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK,
 #ifdef __BIG_ENDIAN
 	{
 		int i;
+
 		for (i = 0; i < 6; i++)
 			PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
 	}
@@ -389,6 +390,7 @@ static int rtllib_tkip_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	u32 crc;
 	struct scatterlist sg;
 	int plen;
+
 	if (skb->len < hdr_len + 8 + 4)
 		return -1;
 
@@ -617,6 +619,7 @@ static int rtllib_michael_mic_verify(struct sk_buff *skb, int keyidx,
 
 	if (memcmp(mic, skb->data + skb->len - 8, 8) != 0) {
 		struct rtllib_hdr_4addr *hdr;
+
 		hdr = (struct rtllib_hdr_4addr *) skb->data;
 		printk(KERN_DEBUG "%s: Michael MIC verification failed for "
 		       "MSDU from %pM keyidx=%d\n",
@@ -693,6 +696,7 @@ static int rtllib_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 		/* Return the sequence number of the last transmitted frame. */
 		u16 iv16 = tkey->tx_iv16;
 		u32 iv32 = tkey->tx_iv32;
+
 		if (iv16 == 0)
 			iv32--;
 		iv16--;
@@ -711,6 +715,7 @@ static int rtllib_tkip_get_key(void *key, int len, u8 *seq, void *priv)
 static void rtllib_tkip_print_stats(struct seq_file *m, void *priv)
 {
 	struct rtllib_tkip_data *tkip = priv;
+
 	seq_printf(m,
 		   "key[%d] alg=TKIP key_set=%d "
 		   "tx_pn=%02x%02x%02x%02x%02x%02x "
diff --git a/drivers/staging/rtl8192e/rtllib_crypt_wep.c b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
index b0e5f1ff07ee..75e7b02a144a 100644
--- a/drivers/staging/rtl8192e/rtllib_crypt_wep.c
+++ b/drivers/staging/rtl8192e/rtllib_crypt_wep.c
@@ -44,14 +44,14 @@ static void *prism2_wep_init(int keyidx)
 
 	priv->tx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->tx_tfm)) {
-		printk(KERN_DEBUG "rtllib_crypt_wep: could not allocate "
+		pr_debug("rtllib_crypt_wep: could not allocate "
 		       "crypto API arc4\n");
 		priv->tx_tfm = NULL;
 		goto fail;
 	}
 	priv->rx_tfm = crypto_alloc_blkcipher("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
 	if (IS_ERR(priv->rx_tfm)) {
-		printk(KERN_DEBUG "rtllib_crypt_wep: could not allocate "
+		pr_debug("rtllib_crypt_wep: could not allocate "
 		       "crypto API arc4\n");
 		priv->rx_tfm = NULL;
 		goto fail;
@@ -105,6 +105,7 @@ static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	u32 crc;
 	u8 *icv;
 	struct scatterlist sg;
+
 	if (skb_headroom(skb) < 4 || skb_tailroom(skb) < 4 ||
 	    skb->len < hdr_len){
 		printk(KERN_ERR "Error!!! headroom=%d tailroom=%d skblen=%d"
@@ -126,6 +127,7 @@ static int prism2_wep_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	 * can be used to speedup attacks, so avoid using them. */
 	if ((wep->iv & 0xff00) == 0xff00) {
 		u8 B = (wep->iv >> 16) & 0xff;
+
 		if (B >= 3 && B < klen)
 			wep->iv += 0x0100;
 	}
@@ -177,6 +179,7 @@ static int prism2_wep_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
 	u32 crc;
 	u8 icv[4];
 	struct scatterlist sg;
+
 	if (skb->len < hdr_len + 8)
 		return -1;
 
@@ -250,6 +253,7 @@ static int prism2_wep_get_key(void *key, int len, u8 *seq, void *priv)
 static void prism2_wep_print_stats(struct seq_file *m, void *priv)
 {
 	struct prism2_wep_data *wep = priv;
+
 	seq_printf(m, "key[%d] alg=WEP len=%d\n", wep->key_idx, wep->key_len);
 }
 
diff --git a/drivers/staging/rtl8192e/rtllib_module.c b/drivers/staging/rtl8192e/rtllib_module.c
index ba95149fa42e..91e98e8e5bfc 100644
--- a/drivers/staging/rtl8192e/rtllib_module.c
+++ b/drivers/staging/rtl8192e/rtllib_module.c
@@ -217,6 +217,7 @@ static ssize_t write_debug_level(struct file *file, const char __user *buffer,
 {
 	unsigned long val;
 	int err = kstrtoul_from_user(buffer, count, 0, &val);
+
 	if (err)
 		return err;
 	rtllib_debug_level = val;
diff --git a/drivers/staging/rtl8192e/rtllib_rx.c b/drivers/staging/rtl8192e/rtllib_rx.c
index 7db3e7445428..1c2014fd8d81 100644
--- a/drivers/staging/rtl8192e/rtllib_rx.c
+++ b/drivers/staging/rtl8192e/rtllib_rx.c
@@ -52,7 +52,7 @@ static inline void rtllib_monitor_rx(struct rtllib_device *ieee,
 	skb_reset_mac_header(skb);
 	skb_pull(skb, hdr_length);
 	skb->pkt_type = PACKET_OTHERHOST;
-	skb->protocol = __constant_htons(ETH_P_80211_RAW);
+	skb->protocol = htons(ETH_P_80211_RAW);
 	memset(skb->cb, 0, sizeof(skb->cb));
 	netif_rx(skb);
 }
@@ -290,6 +290,7 @@ rtllib_rx_frame_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 
 	if (ieee->hwsec_active) {
 		struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+
 		tcb_desc->bHwSec = 1;
 
 		if (ieee->need_sw_enc)
@@ -330,6 +331,7 @@ rtllib_rx_frame_decrypt_msdu(struct rtllib_device *ieee, struct sk_buff *skb,
 		return 0;
 	if (ieee->hwsec_active) {
 		struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
+
 		tcb_desc->bHwSec = 1;
 
 		if (ieee->need_sw_enc)
@@ -389,6 +391,7 @@ static int is_duplicate_packet(struct rtllib_device *ieee,
 		struct ieee_ibss_seq *entry = NULL;
 		u8 *mac = header->addr2;
 		int index = mac[5] % IEEE_IBSS_MAC_HASH_SIZE;
+
 		list_for_each(p, &ieee->ibss_mac_hash[index]) {
 			entry = list_entry(p, struct ieee_ibss_seq, list);
 			if (!memcmp(entry->mac, mac, ETH_ALEN))
@@ -471,8 +474,10 @@ void rtllib_indicate_packets(struct rtllib_device *ieee, struct rtllib_rxb **prx
 	struct net_device_stats *stats = &ieee->stats;
 	u8 i = 0 , j = 0;
 	u16 ethertype;
+
 	for (j = 0; j < index; j++) {
 		struct rtllib_rxb *prxb = prxbIndicateArray[j];
+
 		for (i = 0; i < prxb->nr_subframes; i++) {
 			struct sk_buff *sub_skb = prxb->subframes[i];
 
@@ -572,6 +577,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 		pHTInfo->RxReorderDropCounter++;
 		{
 			int i;
+
 			for (i = 0; i < prxb->nr_subframes; i++)
 				dev_kfree_skb(prxb->subframes[i]);
 			kfree(prxb);
@@ -638,6 +644,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 				list_add_tail(&pReorderEntry->List,
 					      &ieee->RxReorder_Unused_List); {
 					int i;
+
 					for (i = 0; i < prxb->nr_subframes; i++)
 						dev_kfree_skb(prxb->subframes[i]);
 					kfree(prxb);
@@ -661,6 +668,7 @@ static void RxReorderIndicatePacket(struct rtllib_device *ieee,
 				     "dropped!!\n");
 			{
 				int i;
+
 				for (i = 0; i < prxb->nr_subframes; i++)
 					dev_kfree_skb(prxb->subframes[i]);
 				kfree(prxb);
@@ -905,15 +913,14 @@ static int rtllib_rx_check_duplicate(struct rtllib_device *ieee,
 		}
 	} else {
 		struct rx_ts_record *pRxTS = NULL;
+
 		if (GetTs(ieee, (struct ts_common_info **) &pRxTS, hdr->addr2,
 			(u8)Frame_QoSTID((u8 *)(skb->data)), RX_DIR, true)) {
 			if ((fc & (1<<11)) && (frag == pRxTS->RxLastFragNum) &&
-			    (WLAN_GET_SEQ_SEQ(sc) == pRxTS->RxLastSeqNum)) {
+			    (WLAN_GET_SEQ_SEQ(sc) == pRxTS->RxLastSeqNum))
 				return -1;
-			} else {
-				pRxTS->RxLastFragNum = frag;
-				pRxTS->RxLastSeqNum = WLAN_GET_SEQ_SEQ(sc);
-			}
+			pRxTS->RxLastFragNum = frag;
+			pRxTS->RxLastSeqNum = WLAN_GET_SEQ_SEQ(sc);
 		} else {
 			RTLLIB_DEBUG(RTLLIB_DL_ERR, "ERR!!%s(): No TS!! Skip"
 				     " the check!!\n", __func__);
@@ -1074,6 +1081,7 @@ static int rtllib_rx_decrypt(struct rtllib_device *ieee, struct sk_buff *skb,
 	if ((frag != 0 || (fc & RTLLIB_FCTL_MOREFRAGS))) {
 		int flen;
 		struct sk_buff *frag_skb = rtllib_frag_cache_get(ieee, hdr);
+
 		RTLLIB_DEBUG_FRAG("Rx Fragment received (%u)\n", frag);
 
 		if (!frag_skb) {
diff --git a/drivers/staging/rtl8192e/rtllib_softmac.c b/drivers/staging/rtl8192e/rtllib_softmac.c
index 684ceed17232..abb6729ae279 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac.c
@@ -130,6 +130,7 @@ void rtllib_TURBO_Info(struct rtllib_device *ieee, u8 **tag_p)
 static void enqueue_mgmt(struct rtllib_device *ieee, struct sk_buff *skb)
 {
 	int nh;
+
 	nh = (ieee->mgmt_queue_head + 1) % MGMT_QUEUE_NUM;
 
 /*
@@ -222,6 +223,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct rtllib_device *ieee)
 		(struct rtllib_hdr_3addr  *) skb->data;
 
 	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + 8);
+
 	spin_lock_irqsave(&ieee->lock, flags);
 
 	/* called with 2nd param 0, no mgmt lock required */
@@ -387,6 +389,7 @@ struct sk_buff *rtllib_get_beacon_(struct rtllib_device *ieee);
 static void rtllib_send_beacon(struct rtllib_device *ieee)
 {
 	struct sk_buff *skb;
+
 	if (!ieee->ieee_up)
 		return;
 	skb = rtllib_get_beacon_(ieee);
@@ -495,6 +498,7 @@ EXPORT_SYMBOL(rtllib_DisableIntelPromiscuousMode);
 static void rtllib_send_probe(struct rtllib_device *ieee, u8 is_mesh)
 {
 	struct sk_buff *skb;
+
 	skb = rtllib_probe_req(ieee);
 	if (skb) {
 		softmac_mgmt_xmit(skb, ieee);
@@ -657,6 +661,7 @@ out1:
 static void rtllib_beacons_start(struct rtllib_device *ieee)
 {
 	unsigned long flags;
+
 	spin_lock_irqsave(&ieee->beacon_lock, flags);
 
 	ieee->beacon_txing = 1;
@@ -795,6 +800,7 @@ inline struct sk_buff *rtllib_authentication_req(struct rtllib_network *beacon,
 	struct sk_buff *skb;
 	struct rtllib_authentication *auth;
 	int  len = 0;
+
 	len = sizeof(struct rtllib_authentication) + challengelen +
 		     ieee->tx_headroom + 4;
 	skb = dev_alloc_skb(len);
@@ -1043,6 +1049,7 @@ static struct sk_buff *rtllib_auth_resp(struct rtllib_device *ieee, int status,
 	struct sk_buff *skb = NULL;
 	struct rtllib_authentication *auth;
 	int len = ieee->tx_headroom + sizeof(struct rtllib_authentication) + 1;
+
 	skb = dev_alloc_skb(len);
 	if (!skb)
 		return NULL;
@@ -1139,8 +1146,8 @@ static void rtllib_resp_to_auth(struct rtllib_device *ieee, int s, u8 *dest)
 
 static void rtllib_resp_to_probe(struct rtllib_device *ieee, u8 *dest)
 {
-
 	struct sk_buff *buf = rtllib_probe_resp(ieee, dest);
+
 	if (buf)
 		softmac_mgmt_xmit(buf, ieee);
 }
@@ -1154,8 +1161,7 @@ inline int SecIsInPMKIDList(struct rtllib_device *ieee, u8 *bssid)
 		if ((ieee->PMKIDList[i].bUsed) &&
 		   (memcmp(ieee->PMKIDList[i].Bssid, bssid, ETH_ALEN) == 0))
 			break;
-		else
-			i++;
+		i++;
 	} while (i < NUM_PMKID_CACHE);
 
 	if (i == NUM_PMKID_CACHE)
@@ -1192,6 +1198,7 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	unsigned int turbo_info_len = beacon->Turbo_Enable ? 9 : 0;
 
 	int len = 0;
+
 	crypt = ieee->crypt_info.crypt[ieee->crypt_info.tx_keyidx];
 	if (crypt != NULL)
 		encrypt = ieee->host_encrypt && crypt && crypt->ops &&
@@ -1339,6 +1346,7 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 	if (beacon->BssCcxVerNumber >= 2) {
 		u8 CcxVerNumBuf[] = {0x00, 0x40, 0x96, 0x03, 0x00};
 		struct octet_string osCcxVerNum;
+
 		CcxVerNumBuf[4] = beacon->BssCcxVerNumber;
 		osCcxVerNum.Octet = CcxVerNumBuf;
 		osCcxVerNum.Length = sizeof(CcxVerNumBuf);
@@ -1418,8 +1426,8 @@ inline struct sk_buff *rtllib_association_req(struct rtllib_network *beacon,
 
 void rtllib_associate_abort(struct rtllib_device *ieee)
 {
-
 	unsigned long flags;
+
 	spin_lock_irqsave(&ieee->lock, flags);
 
 	ieee->associate_seq++;
@@ -1789,6 +1797,7 @@ static inline u16 auth_parse(struct sk_buff *skb, u8 **challenge, int *chlen)
 {
 	struct rtllib_authentication *a;
 	u8 *t;
+
 	if (skb->len <  (sizeof(struct rtllib_authentication) -
 	    sizeof(struct rtllib_info_element))) {
 		RTLLIB_DEBUG_MGMT("invalid len in auth resp: %d\n", skb->len);
@@ -1924,6 +1933,7 @@ static inline u16 assoc_parse(struct rtllib_device *ieee, struct sk_buff *skb,
 void rtllib_rx_probe_rq(struct rtllib_device *ieee, struct sk_buff *skb)
 {
 	u8 dest[ETH_ALEN];
+
 	ieee->softmac_stats.rx_probe_rq++;
 	if (probe_rq_parse(ieee, skb, dest) > 0) {
 		ieee->softmac_stats.tx_probe_rs++;
@@ -1936,6 +1946,7 @@ static inline void rtllib_rx_auth_rq(struct rtllib_device *ieee,
 {
 	u8 dest[ETH_ALEN];
 	int status;
+
 	ieee->softmac_stats.rx_auth_rq++;
 
 	status = auth_rq_parse(skb, dest);
@@ -2026,6 +2037,7 @@ static short rtllib_sta_ps_sleep(struct rtllib_device *ieee, u64 *time)
 			u8 LPSAwakeIntvl_tmp = 0;
 			u8 period = ieee->current_network.dtim_period;
 			u8 count = ieee->current_network.tim.tim_count;
+
 			if (count == 0) {
 				if (pPSC->LPSAwakeIntvl > period)
 					LPSAwakeIntvl_tmp = period +
@@ -2248,14 +2260,13 @@ inline int rtllib_rx_assoc_resp(struct rtllib_device *ieee, struct sk_buff *skb,
 							network, rx_stats)) {
 					kfree(network);
 					return 1;
-				} else {
-					memcpy(ieee->pHTInfo->PeerHTCapBuf,
-					       network->bssht.bdHTCapBuf,
-					       network->bssht.bdHTCapLen);
-					memcpy(ieee->pHTInfo->PeerHTInfoBuf,
-					       network->bssht.bdHTInfoBuf,
-					       network->bssht.bdHTInfoLen);
 				}
+				memcpy(ieee->pHTInfo->PeerHTCapBuf,
+				       network->bssht.bdHTCapBuf,
+				       network->bssht.bdHTCapLen);
+				memcpy(ieee->pHTInfo->PeerHTInfoBuf,
+				       network->bssht.bdHTInfoBuf,
+				       network->bssht.bdHTInfoLen);
 				if (ieee->handle_assoc_response != NULL)
 					ieee->handle_assoc_response(ieee->dev,
 						 (struct rtllib_assoc_response_frame *)header,
@@ -2514,6 +2525,7 @@ void rtllib_softmac_xmit(struct rtllib_txb *txb, struct rtllib_device *ieee)
 static void rtllib_resume_tx(struct rtllib_device *ieee)
 {
 	int i;
+
 	for (i = ieee->tx_pending.frag; i < ieee->tx_pending.txb->nr_frags;
 	     i++) {
 
@@ -2606,6 +2618,7 @@ void rtllib_stop_queue(struct rtllib_device *ieee)
 void rtllib_stop_all_queues(struct rtllib_device *ieee)
 {
 	unsigned int i;
+
 	for (i = 0; i < ieee->dev->num_tx_queues; i++)
 		netdev_get_tx_queue(ieee->dev, i)->trans_start = jiffies;
 
@@ -2801,6 +2814,7 @@ inline void rtllib_start_ibss(struct rtllib_device *ieee)
 void rtllib_start_bss(struct rtllib_device *ieee)
 {
 	unsigned long flags;
+
 	if (IS_DOT11D_ENABLE(ieee) && !IS_COUNTRY_IE_VALID(ieee)) {
 		if (!ieee->bGlobalDomain)
 			return;
@@ -2900,6 +2914,7 @@ struct sk_buff *rtllib_get_beacon_(struct rtllib_device *ieee)
 
 	struct sk_buff *skb;
 	struct rtllib_probe_response *b;
+
 	skb = rtllib_probe_resp(ieee, broadcast_addr);
 
 	if (!skb)
@@ -3049,6 +3064,7 @@ void rtllib_start_protocol(struct rtllib_device *ieee)
 void rtllib_softmac_init(struct rtllib_device *ieee)
 {
 	int i;
+
 	memset(&ieee->current_network, 0, sizeof(struct rtllib_network));
 
 	ieee->state = RTLLIB_NOLINK;
@@ -3513,6 +3529,7 @@ inline struct sk_buff *rtllib_disassociate_skb(struct rtllib_network *beacon,
 	struct sk_buff *skb;
 	struct rtllib_disassoc *disass;
 	int len = sizeof(struct rtllib_disassoc) + ieee->tx_headroom;
+
 	skb = dev_alloc_skb(len);
 
 	if (!skb)
diff --git a/drivers/staging/rtl8192e/rtllib_softmac_wx.c b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
index 65b650cc3d0a..835f3d78a16f 100644
--- a/drivers/staging/rtl8192e/rtllib_softmac_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_softmac_wx.c
@@ -244,6 +244,7 @@ int rtllib_wx_get_rate(struct rtllib_device *ieee,
 			     union iwreq_data *wrqu, char *extra)
 {
 	u32 tmp_rate = 0;
+
 	tmp_rate = TxCountToDataRate(ieee,
 				     ieee->softmac_stats.CurrentShowTxate);
 	wrqu->bitrate.value = tmp_rate * 500000;
@@ -334,7 +335,6 @@ void rtllib_wx_sync_scan_wq(void *data)
 	enum ht_extchnl_offset chan_offset = 0;
 	enum ht_channel_width bandwidth = 0;
 	int b40M = 0;
-	static int count;
 
 	if (!(ieee->softmac_features & IEEE_SOFTMAC_SCAN)) {
 		rtllib_start_scan_syncro(ieee, 0);
@@ -411,7 +411,6 @@ void rtllib_wx_sync_scan_wq(void *data)
 
 	rtllib_wake_all_queues(ieee);
 
-	count = 0;
 out:
 	up(&ieee->wx_sem);
 
diff --git a/drivers/staging/rtl8192e/rtllib_tx.c b/drivers/staging/rtl8192e/rtllib_tx.c
index b7dd1539bbc4..4f68ffe41475 100644
--- a/drivers/staging/rtl8192e/rtllib_tx.c
+++ b/drivers/staging/rtl8192e/rtllib_tx.c
@@ -224,6 +224,7 @@ static struct rtllib_txb *rtllib_alloc_txb(int nr_frags, int txb_size,
 {
 	struct rtllib_txb *txb;
 	int i;
+
 	txb = kmalloc(sizeof(struct rtllib_txb) + (sizeof(u8 *) * nr_frags),
 		      gfp_mask);
 	if (!txb)
@@ -353,7 +354,6 @@ FORCED_AGG_SETTING:
 		tcb_desc->ampdu_factor = 0;
 		break;
 	}
-	return;
 }
 
 static void rtllib_qurey_ShortPreambleMode(struct rtllib_device *ieee,
@@ -365,7 +365,6 @@ static void rtllib_qurey_ShortPreambleMode(struct rtllib_device *ieee,
 	else if (ieee->current_network.capability &
 		 WLAN_CAPABILITY_SHORT_PREAMBLE)
 		tcb_desc->bUseShortPreamble = true;
-	return;
 }
 
 static void rtllib_query_HTCapShortGI(struct rtllib_device *ieee,
@@ -407,7 +406,6 @@ static void rtllib_query_BandwidthMode(struct rtllib_device *ieee,
 	if (pHTInfo->bCurBW40MHz && pHTInfo->bCurTxBW40MHz &&
 	    !ieee->bandwidth_auto_switch.bforced_tx20Mhz)
 		tcb_desc->bPacketBW = true;
-	return;
 }
 
 static void rtllib_query_protectionmode(struct rtllib_device *ieee,
@@ -438,6 +436,7 @@ static void rtllib_query_protectionmode(struct rtllib_device *ieee,
 		return;
 	} else {
 		struct rt_hi_throughput *pHTInfo = ieee->pHTInfo;
+
 		while (true) {
 			if (pHTInfo->IOTAction & HT_IOT_ACT_FORCED_CTS2SELF) {
 				tcb_desc->bCTSEnable	= true;
@@ -458,6 +457,7 @@ static void rtllib_query_protectionmode(struct rtllib_device *ieee,
 			}
 			if (pHTInfo->bCurrentHTSupport  && pHTInfo->bEnableHT) {
 				u8 HTOpMode = pHTInfo->CurrentOpMode;
+
 				if ((pHTInfo->bCurBW40MHz && (HTOpMode == 2 ||
 				     HTOpMode == 3)) ||
 				     (!pHTInfo->bCurBW40MHz && HTOpMode == 3)) {
@@ -518,6 +518,7 @@ u16 rtllib_query_seqnum(struct rtllib_device *ieee, struct sk_buff *skb,
 		return 0;
 	if (IsQoSDataFrame(skb->data)) {
 		struct tx_ts_record *pTS = NULL;
+
 		if (!GetTs(ieee, (struct ts_common_info **)(&pTS), dst,
 		    skb->priority, TX_DIR, true))
 			return 0;
diff --git a/drivers/staging/rtl8192e/rtllib_wx.c b/drivers/staging/rtl8192e/rtllib_wx.c
index 13af43b90fc0..9e0f975c152f 100644
--- a/drivers/staging/rtl8192e/rtllib_wx.c
+++ b/drivers/staging/rtl8192e/rtllib_wx.c
@@ -158,6 +158,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 		struct ht_capab_ele *ht_cap = NULL;
 		bool is40M = false, isShortGI = false;
 		u8 max_mcs = 0;
+
 		if (!memcmp(network->bssht.bdHTCapBuf, EWC11NHTCap, 4))
 			ht_cap = (struct ht_capab_ele *)
 				 &network->bssht.bdHTCapBuf[4];
@@ -212,6 +213,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 	memset(&iwe, 0, sizeof(iwe));
 	if (network->wpa_ie_len) {
 		char buf[MAX_WPA_IE_LEN];
+
 		memcpy(buf, network->wpa_ie, network->wpa_ie_len);
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = network->wpa_ie_len;
@@ -220,6 +222,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 	memset(&iwe, 0, sizeof(iwe));
 	if (network->rsn_ie_len) {
 		char buf[MAX_WPA_IE_LEN];
+
 		memcpy(buf, network->rsn_ie, network->rsn_ie_len);
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = network->rsn_ie_len;
@@ -230,6 +233,7 @@ static inline char *rtl819x_translate_scan(struct rtllib_device *ieee,
 	memset(&iwe, 0, sizeof(iwe));
 	if (network->wzc_ie_len) {
 		char buf[MAX_WZC_IE_LEN];
+
 		memcpy(buf, network->wzc_ie, network->wzc_ie_len);
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = network->wzc_ie_len;
@@ -262,6 +266,7 @@ int rtllib_wx_get_scan(struct rtllib_device *ieee,
 	char *stop = ev + wrqu->data.length;
 	int i = 0;
 	int err = 0;
+
 	RTLLIB_DEBUG_WX("Getting scan\n");
 	down(&ieee->wx_sem);
 	spin_lock_irqsave(&ieee->lock, flags);
diff --git a/drivers/staging/rtl8192ee/Kconfig b/drivers/staging/rtl8192ee/Kconfig
deleted file mode 100644
index 8d77f28399d3..000000000000
--- a/drivers/staging/rtl8192ee/Kconfig
+++ /dev/null
@@ -1,14 +0,0 @@
-config R8192EE
-	tristate "Realtek RTL8192EE Wireless Network Adapter"
-	depends on PCI && WLAN && MAC80211
-	depends on m
-	select WIRELESS_EXT
-	select WEXT_PRIV
-	select EEPROM_93CX6
-	select CRYPTO
-	select FW_LOADER
-	---help---
-	This is the driver for Realtek RTL8192EE 802.11 PCIe
-	wireless network adapters.
-
-	If you choose to build it as a module, it will be called r8192ee
diff --git a/drivers/staging/rtl8192ee/Makefile b/drivers/staging/rtl8192ee/Makefile
deleted file mode 100644
index 31708620d7e7..000000000000
--- a/drivers/staging/rtl8192ee/Makefile
+++ /dev/null
@@ -1,40 +0,0 @@
-
-PCI_MAIN_OBJS	:= base.o	\
-		cam.o	\
-		core.o	\
-		debug.o	\
-		efuse.o	\
-		pci.o	\
-		ps.o	\
-		rc.o	\
-		regd.o	\
-		stats.o
-
-PCI_8192EE_HAL_OBJS:=		\
-	rtl8192ee/dm.o		\
-	rtl8192ee/fw.o		\
-	rtl8192ee/hw.o		\
-	rtl8192ee/led.o		\
-	rtl8192ee/phy.o		\
-	rtl8192ee/pwrseq.o	\
-	rtl8192ee/pwrseqcmd.o	\
-	rtl8192ee/rf.o		\
-	rtl8192ee/sw.o		\
-	rtl8192ee/table.o	\
-	rtl8192ee/trx.o
-
-
-
-BT_COEXIST_OBJS:=	btcoexist/halbtc8192e2ant.o\
-			btcoexist/halbtc8723b1ant.o\
-			btcoexist/halbtc8723b2ant.o\
-			btcoexist/halbtc8821a1ant.o\
-			btcoexist/halbtc8821a2ant.o\
-			btcoexist/halbtcoutsrc.o\
-			btcoexist/rtl_btc.o
-
-r8192ee-objs += $(PCI_MAIN_OBJS) $(PCI_8192EE_HAL_OBJS) $(BT_COEXIST_OBJS)
-
-obj-$(CONFIG_R8192EE) += r8192ee.o
-
-ccflags-y += -D__CHECK_ENDIAN__
diff --git a/drivers/staging/rtl8192ee/TODO b/drivers/staging/rtl8192ee/TODO
deleted file mode 100644
index 162092a0316b..000000000000
--- a/drivers/staging/rtl8192ee/TODO
+++ /dev/null
@@ -1,12 +0,0 @@
-TODO:
-- convert any remaining unusual variable types
-- find codes that can use %pM and %Nph formatting
-- checkpatch.pl fixes - most of the remaining ones are lines too long. Many
-  of them will require refactoring
-- merge Realtek's bugfixes and new features into the driver
-- Convert the versions of rtlwifi and btcoexist in drivers/net/wireless/rtlwifi/...
-  to work with the RTL8192EE
-- move this driver to drivers/net/wireless/rtlwifi
-
-Please send any patches to Greg Kroah-Hartman <gregkh@linux.com>,
-and Larry Finger <Larry.Finger@lwfinger.net>.
diff --git a/drivers/staging/rtl8192ee/base.c b/drivers/staging/rtl8192ee/base.c
deleted file mode 100644
index f7c3c8bf71a5..000000000000
--- a/drivers/staging/rtl8192ee/base.c
+++ /dev/null
@@ -1,1851 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include <linux/ip.h>
-#include <linux/module.h>
-#include "wifi.h"
-#include "rc.h"
-#include "base.h"
-#include "efuse.h"
-#include "cam.h"
-#include "ps.h"
-#include "regd.h"
-#include "pci.h"
-
-/*
- *NOTICE!!!: This file will be very big, we hsould
- *keep it clear under follwing roles:
- *
- *This file include follwing part, so, if you add new
- *functions into this file, please check which part it
- *should includes. or check if you should add new part
- *for this file:
- *
- *1) mac80211 init functions
- *2) tx information functions
- *3) functions called by core.c
- *4) wq & timer callback functions
- *5) frame process functions
- *6) IOT functions
- *7) sysfs functions
- *8) vif functions
- *9) ...
- */
-
-/*********************************************************
- *
- * mac80211 init functions
- *
- *********************************************************/
-static struct ieee80211_channel rtl_channeltable_2g[] = {
-	{.center_freq = 2412, .hw_value = 1,},
-	{.center_freq = 2417, .hw_value = 2,},
-	{.center_freq = 2422, .hw_value = 3,},
-	{.center_freq = 2427, .hw_value = 4,},
-	{.center_freq = 2432, .hw_value = 5,},
-	{.center_freq = 2437, .hw_value = 6,},
-	{.center_freq = 2442, .hw_value = 7,},
-	{.center_freq = 2447, .hw_value = 8,},
-	{.center_freq = 2452, .hw_value = 9,},
-	{.center_freq = 2457, .hw_value = 10,},
-	{.center_freq = 2462, .hw_value = 11,},
-	{.center_freq = 2467, .hw_value = 12,},
-	{.center_freq = 2472, .hw_value = 13,},
-	{.center_freq = 2484, .hw_value = 14,},
-};
-
-static struct ieee80211_channel rtl_channeltable_5g[] = {
-	{.center_freq = 5180, .hw_value = 36,},
-	{.center_freq = 5200, .hw_value = 40,},
-	{.center_freq = 5220, .hw_value = 44,},
-	{.center_freq = 5240, .hw_value = 48,},
-	{.center_freq = 5260, .hw_value = 52,},
-	{.center_freq = 5280, .hw_value = 56,},
-	{.center_freq = 5300, .hw_value = 60,},
-	{.center_freq = 5320, .hw_value = 64,},
-	{.center_freq = 5500, .hw_value = 100,},
-	{.center_freq = 5520, .hw_value = 104,},
-	{.center_freq = 5540, .hw_value = 108,},
-	{.center_freq = 5560, .hw_value = 112,},
-	{.center_freq = 5580, .hw_value = 116,},
-	{.center_freq = 5600, .hw_value = 120,},
-	{.center_freq = 5620, .hw_value = 124,},
-	{.center_freq = 5640, .hw_value = 128,},
-	{.center_freq = 5660, .hw_value = 132,},
-	{.center_freq = 5680, .hw_value = 136,},
-	{.center_freq = 5700, .hw_value = 140,},
-	{.center_freq = 5745, .hw_value = 149,},
-	{.center_freq = 5765, .hw_value = 153,},
-	{.center_freq = 5785, .hw_value = 157,},
-	{.center_freq = 5805, .hw_value = 161,},
-	{.center_freq = 5825, .hw_value = 165,},
-};
-
-static struct ieee80211_rate rtl_ratetable_2g[] = {
-	{.bitrate = 10, .hw_value = 0x00,},
-	{.bitrate = 20, .hw_value = 0x01,},
-	{.bitrate = 55, .hw_value = 0x02,},
-	{.bitrate = 110, .hw_value = 0x03,},
-	{.bitrate = 60, .hw_value = 0x04,},
-	{.bitrate = 90, .hw_value = 0x05,},
-	{.bitrate = 120, .hw_value = 0x06,},
-	{.bitrate = 180, .hw_value = 0x07,},
-	{.bitrate = 240, .hw_value = 0x08,},
-	{.bitrate = 360, .hw_value = 0x09,},
-	{.bitrate = 480, .hw_value = 0x0a,},
-	{.bitrate = 540, .hw_value = 0x0b,},
-};
-
-static struct ieee80211_rate rtl_ratetable_5g[] = {
-	{.bitrate = 60, .hw_value = 0x04,},
-	{.bitrate = 90, .hw_value = 0x05,},
-	{.bitrate = 120, .hw_value = 0x06,},
-	{.bitrate = 180, .hw_value = 0x07,},
-	{.bitrate = 240, .hw_value = 0x08,},
-	{.bitrate = 360, .hw_value = 0x09,},
-	{.bitrate = 480, .hw_value = 0x0a,},
-	{.bitrate = 540, .hw_value = 0x0b,},
-};
-
-static const struct ieee80211_supported_band rtl_band_2ghz = {
-	.band = IEEE80211_BAND_2GHZ,
-
-	.channels = rtl_channeltable_2g,
-	.n_channels = ARRAY_SIZE(rtl_channeltable_2g),
-
-	.bitrates = rtl_ratetable_2g,
-	.n_bitrates = ARRAY_SIZE(rtl_ratetable_2g),
-
-	.ht_cap = {0},
-};
-
-static struct ieee80211_supported_band rtl_band_5ghz = {
-	.band = IEEE80211_BAND_5GHZ,
-
-	.channels = rtl_channeltable_5g,
-	.n_channels = ARRAY_SIZE(rtl_channeltable_5g),
-
-	.bitrates = rtl_ratetable_5g,
-	.n_bitrates = ARRAY_SIZE(rtl_ratetable_5g),
-
-	.ht_cap = {0},
-};
-
-static const u8 tid_to_ac[] = {
-	2, /* IEEE80211_AC_BE */
-	3, /* IEEE80211_AC_BK */
-	3, /* IEEE80211_AC_BK */
-	2, /* IEEE80211_AC_BE */
-	1, /* IEEE80211_AC_VI */
-	1, /* IEEE80211_AC_VI */
-	0, /* IEEE80211_AC_VO */
-	0, /* IEEE80211_AC_VO */
-};
-
-u8 rtl92e_tid_to_ac(struct ieee80211_hw *hw, u8 tid)
-{
-	return tid_to_ac[tid];
-}
-
-static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
-				  struct ieee80211_sta_ht_cap *ht_cap)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	ht_cap->ht_supported = true;
-	ht_cap->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
-	    IEEE80211_HT_CAP_SGI_40 |
-	    IEEE80211_HT_CAP_SGI_20 |
-	    IEEE80211_HT_CAP_DSSSCCK40 | IEEE80211_HT_CAP_MAX_AMSDU;
-
-	if (rtlpriv->rtlhal.disable_amsdu_8k)
-		ht_cap->cap &= ~IEEE80211_HT_CAP_MAX_AMSDU;
-
-	/*
-	 *Maximum length of AMPDU that the STA can receive.
-	 *Length = 2 ^ (13 + max_ampdu_length_exp) - 1 (octets)
-	 */
-	ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
-
-	/*Minimum MPDU start spacing , */
-	ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_16;
-
-	ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
-
-	/*
-	 *hw->wiphy->bands[IEEE80211_BAND_2GHZ]
-	 *base on ant_num
-	 *rx_mask: RX mask
-	 *if rx_ant =1 rx_mask[0]=0xff;==>MCS0-MCS7
-	 *if rx_ant =2 rx_mask[1]=0xff;==>MCS8-MCS15
-	 *if rx_ant >=3 rx_mask[2]=0xff;
-	 *if BW_40 rx_mask[4]=0x01;
-	 *highest supported RX rate
-	 */
-	if (rtlpriv->dm.supp_phymode_switch) {
-		RT_TRACE(COMP_INIT, DBG_EMERG, ("Support phy mode switch\n"));
-
-		ht_cap->mcs.rx_mask[0] = 0xFF;
-		ht_cap->mcs.rx_mask[1] = 0xFF;
-		ht_cap->mcs.rx_mask[4] = 0x01;
-
-		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
-	} else {
-		if (get_rf_type(rtlphy) == RF_1T2R ||
-		    get_rf_type(rtlphy) == RF_2T2R) {
-			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T2R or 2T2R\n"));
-
-			ht_cap->mcs.rx_mask[0] = 0xFF;
-			ht_cap->mcs.rx_mask[1] = 0xFF;
-			ht_cap->mcs.rx_mask[4] = 0x01;
-
-			ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
-		} else if (get_rf_type(rtlphy) == RF_1T1R) {
-			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T1R\n"));
-
-			ht_cap->mcs.rx_mask[0] = 0xFF;
-			ht_cap->mcs.rx_mask[1] = 0x00;
-			ht_cap->mcs.rx_mask[4] = 0x01;
-
-			ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS7);
-		}
-	}
-}
-
-static void _rtl_init_hw_vht_capab(struct ieee80211_hw *hw,
-				   struct ieee80211_sta_vht_cap *vht_cap)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-		u16 mcs_map;
-		vht_cap->vht_supported = true;
-		vht_cap->cap =
-			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895 |
-			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991 |
-			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
-			IEEE80211_VHT_CAP_SHORT_GI_80 |
-			IEEE80211_VHT_CAP_TXSTBC |
-			IEEE80211_VHT_CAP_RXSTBC_1 |
-			IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
-			IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
-			IEEE80211_VHT_CAP_HTC_VHT |
-			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK |
-			IEEE80211_VHT_CAP_RX_ANTENNA_PATTERN |
-			IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN |
-			0;
-
-		mcs_map = IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
-			IEEE80211_VHT_MCS_SUPPORT_0_9 << 2 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 4 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 6 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 8 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 10 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 12 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 14;
-
-		vht_cap->vht_mcs.rx_mcs_map = cpu_to_le16(mcs_map);
-		vht_cap->vht_mcs.rx_highest =
-			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9);
-		vht_cap->vht_mcs.tx_mcs_map = cpu_to_le16(mcs_map);
-		vht_cap->vht_mcs.tx_highest =
-			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		u16 mcs_map;
-
-		vht_cap->vht_supported = true;
-		vht_cap->cap =
-			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_3895 |
-			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_7991 |
-			IEEE80211_VHT_CAP_MAX_MPDU_LENGTH_11454 |
-			IEEE80211_VHT_CAP_SHORT_GI_80 |
-			IEEE80211_VHT_CAP_TXSTBC |
-			IEEE80211_VHT_CAP_RXSTBC_1 |
-			IEEE80211_VHT_CAP_SU_BEAMFORMER_CAPABLE |
-			IEEE80211_VHT_CAP_SU_BEAMFORMEE_CAPABLE |
-			IEEE80211_VHT_CAP_HTC_VHT |
-			IEEE80211_VHT_CAP_MAX_A_MPDU_LENGTH_EXPONENT_MASK |
-			IEEE80211_VHT_CAP_RX_ANTENNA_PATTERN |
-			IEEE80211_VHT_CAP_TX_ANTENNA_PATTERN |
-			0;
-
-		mcs_map = IEEE80211_VHT_MCS_SUPPORT_0_9 << 0 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 2 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 4 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 6 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 8 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 10 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 12 |
-			IEEE80211_VHT_MCS_NOT_SUPPORTED << 14;
-
-		vht_cap->vht_mcs.rx_mcs_map = cpu_to_le16(mcs_map);
-		vht_cap->vht_mcs.rx_highest =
-			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9);
-		vht_cap->vht_mcs.tx_mcs_map = cpu_to_le16(mcs_map);
-		vht_cap->vht_mcs.tx_highest =
-			cpu_to_le16(MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9);
-	}
-}
-
-static void _rtl_init_mac80211(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct ieee80211_supported_band *sband;
-
-
-	if (rtlhal->macphymode == SINGLEMAC_SINGLEPHY &&
-	    rtlhal->bandset == BAND_ON_BOTH) {
-		/* 1: 2.4 G bands */
-		/* <1> use  mac->bands as mem for hw->wiphy->bands */
-		sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
-
-		/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
-		 * to default value(1T1R) */
-		memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]), &rtl_band_2ghz,
-		       sizeof(struct ieee80211_supported_band));
-
-		/* <3> init ht cap base on ant_num */
-		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-		/* <4> set mac->sband to wiphy->sband */
-		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
-
-		/* 2: 5 G bands */
-		/* <1> use  mac->bands as mem for hw->wiphy->bands */
-		sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
-
-		/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
-		 * to default value(1T1R) */
-		memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]), &rtl_band_5ghz,
-		       sizeof(struct ieee80211_supported_band));
-
-		/* <3> init ht cap base on ant_num */
-		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-		_rtl_init_hw_vht_capab(hw, &sband->vht_cap);
-
-		/* <4> set mac->sband to wiphy->sband */
-		hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
-	} else {
-		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-			/* <1> use  mac->bands as mem for hw->wiphy->bands */
-			sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
-
-			/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
-			 * to default value(1T1R) */
-			memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]),
-			       &rtl_band_2ghz,
-			       sizeof(struct ieee80211_supported_band));
-
-			/* <3> init ht cap base on ant_num */
-			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-			/* <4> set mac->sband to wiphy->sband */
-			hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
-		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
-			/* <1> use  mac->bands as mem for hw->wiphy->bands */
-			sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
-
-			/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
-			 * to default value(1T1R) */
-			memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]),
-			       &rtl_band_5ghz,
-			       sizeof(struct ieee80211_supported_band));
-
-			/* <3> init ht cap base on ant_num */
-			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-			_rtl_init_hw_vht_capab(hw, &sband->vht_cap);
-
-			/* <4> set mac->sband to wiphy->sband */
-			hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
-		} else {
-			RT_TRACE(COMP_INIT, DBG_EMERG,
-				 ("Err BAND %d\n", rtlhal->current_bandtype));
-		}
-	}
-	/* <5> set hw caps */
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-	    IEEE80211_HW_RX_INCLUDES_FCS |
-	    IEEE80211_HW_AMPDU_AGGREGATION |
-	    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-	    IEEE80211_HW_CONNECTION_MONITOR |
-	    /* IEEE80211_HW_SUPPORTS_CQM_RSSI | */
-	    IEEE80211_HW_MFP_CAPABLE | 0;
-
-	/* swlps or hwlps has been set in diff chip in init_sw_vars */
-	if (rtlpriv->psc.b_swctrl_lps)
-		hw->flags |= IEEE80211_HW_SUPPORTS_PS |
-			IEEE80211_HW_PS_NULLFUNC_STACK |
-			/* IEEE80211_HW_SUPPORTS_DYNAMIC_PS | */
-			0;
-/*<delete in kernel start>*/
-	hw->wiphy->interface_modes =
-		BIT(NL80211_IFTYPE_AP) |
-		BIT(NL80211_IFTYPE_STATION) |
-		BIT(NL80211_IFTYPE_ADHOC) |
-		BIT(NL80211_IFTYPE_MESH_POINT) |
-		BIT(NL80211_IFTYPE_P2P_CLIENT) |
-		BIT(NL80211_IFTYPE_P2P_GO);
-	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
-
-	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
-
-	hw->wiphy->rts_threshold = 2347;
-
-	hw->queues = AC_MAX;
-	hw->extra_tx_headroom = RTL_TX_HEADER_SIZE;
-
-	/* TODO: Correct this value for our hw */
-	/* TODO: define these hard code value */
-	/* hw->channel_change_time = 100; kernel does not use it*/
-	hw->max_listen_interval = 10;
-	hw->max_rate_tries = 4;
-	/* hw->max_rates = 1; */
-	hw->sta_data_size = sizeof(struct rtl_sta_info);
-
-/* wowlan is not supported by kernel if CONFIG_PM is not defined */
-#ifdef CONFIG_PM
-	if (rtlpriv->psc.wo_wlan_mode) {
-		if (rtlpriv->psc.wo_wlan_mode & WAKE_ON_MAGIC_PACKET)
-			rtlpriv->wowlan.flags = WIPHY_WOWLAN_MAGIC_PKT;
-		if (rtlpriv->psc.wo_wlan_mode & WAKE_ON_PATTERN_MATCH) {
-			rtlpriv->wowlan.n_patterns =
-				MAX_SUPPORT_WOL_PATTERN_NUM;
-			rtlpriv->wowlan.pattern_min_len = MIN_WOL_PATTERN_SIZE;
-			rtlpriv->wowlan.pattern_max_len = MAX_WOL_PATTERN_SIZE;
-		}
-		hw->wiphy->wowlan = &(rtlpriv->wowlan);
-	}
-#endif
-
-	/* <6> mac address */
-	if (is_valid_ether_addr(rtlefuse->dev_addr)) {
-		SET_IEEE80211_PERM_ADDR(hw, rtlefuse->dev_addr);
-	} else {
-		u8 rtlmac[] = { 0x00, 0xe0, 0x4c, 0x81, 0x92, 0x00 };
-		get_random_bytes((rtlmac + (ETH_ALEN - 1)), 1);
-		SET_IEEE80211_PERM_ADDR(hw, rtlmac);
-	}
-}
-
-static void _rtl_init_deferred_work(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	/* <1> timer */
-	init_timer(&rtlpriv->works.watchdog_timer);
-	setup_timer(&rtlpriv->works.watchdog_timer,
-		    rtl92e_watch_dog_timer_callback, (unsigned long)hw);
-	init_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer);
-	setup_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer,
-		    rtl92e_easy_concurrent_retrytimer_callback, (unsigned long)hw);
-	/* <2> work queue */
-	rtlpriv->works.hw = hw;
-	rtlpriv->works.rtl_wq = alloc_workqueue("%s", 0, 0, rtlpriv->cfg->name);
-	INIT_DELAYED_WORK(&rtlpriv->works.watchdog_wq,
-			  (void *)rtl92e_watchdog_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.ips_nic_off_wq,
-			  (void *)rtl92e_ips_nic_off_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.ps_work,
-			  (void *)rtl92e_swlps_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.ps_rfon_wq,
-			  (void *)rtl92e_swlps_rfon_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.fwevt_wq,
-			  (void *)rtl92e_fwevt_wq_callback);
-}
-
-void rtl92e_deinit_deferred_work(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	del_timer_sync(&rtlpriv->works.watchdog_timer);
-
-	cancel_delayed_work(&rtlpriv->works.watchdog_wq);
-	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
-	cancel_delayed_work(&rtlpriv->works.ps_work);
-	cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
-	cancel_delayed_work(&rtlpriv->works.fwevt_wq);
-}
-
-void rtl92e_init_rfkill(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	bool radio_state;
-	bool blocked;
-	u8 valid = 0;
-
-	/*set init state to on */
-	rtlpriv->rfkill.rfkill_state = 1;
-	wiphy_rfkill_set_hw_state(hw->wiphy, 0);
-
-	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
-
-	if (valid) {
-		pr_info("rtlwifi: wireless switch is %s\n",
-			rtlpriv->rfkill.rfkill_state ? "on" : "off");
-
-		rtlpriv->rfkill.rfkill_state = radio_state;
-
-		blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
-		wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
-	}
-
-	wiphy_rfkill_start_polling(hw->wiphy);
-}
-
-void rtl92e_deinit_rfkill(struct ieee80211_hw *hw)
-{
-	wiphy_rfkill_stop_polling(hw->wiphy);
-}
-
-#ifdef VIF_TODO
-static void rtl_init_vif(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	INIT_LIST_HEAD(&rtlpriv->vif_priv.vif_list);
-
-	rtlpriv->vif_priv.vifs = 0;
-}
-#endif
-
-int rtl92e_init_core(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
-
-	/* <1> init mac80211 */
-	_rtl_init_mac80211(hw);
-	rtlmac->hw = hw;
-	rtlmac->link_state = MAC80211_NOLINK;
-
-	/* <2> rate control register */
-	hw->rate_control_algorithm = "rtl_rc";
-
-	/*
-	 * <3> init CRDA must come after init
-	 * mac80211 hw  in _rtl_init_mac80211.
-	 */
-	if (rtl92e_regd_init(hw, rtl92e_reg_notifier)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("REGD init failed\n"));
-		return 1;
-	}
-
-	/* <4> locks */
-	mutex_init(&rtlpriv->locks.conf_mutex);
-	spin_lock_init(&rtlpriv->locks.ips_lock);
-	spin_lock_init(&rtlpriv->locks.irq_th_lock);
-	spin_lock_init(&rtlpriv->locks.h2c_lock);
-	spin_lock_init(&rtlpriv->locks.rf_ps_lock);
-	spin_lock_init(&rtlpriv->locks.rf_lock);
-	spin_lock_init(&rtlpriv->locks.lps_lock);
-	spin_lock_init(&rtlpriv->locks.waitq_lock);
-	spin_lock_init(&rtlpriv->locks.entry_list_lock);
-	spin_lock_init(&rtlpriv->locks.cck_and_rw_pagea_lock);
-	spin_lock_init(&rtlpriv->locks.check_sendpkt_lock);
-	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
-	spin_lock_init(&rtlpriv->locks.iqk_lock);
-	/* <5> init list */
-	INIT_LIST_HEAD(&rtlpriv->entry_list);
-
-	/* <6> init deferred work */
-	_rtl_init_deferred_work(hw);
-
-	/* <7> */
-#ifdef VIF_TODO
-	rtl_init_vif(hw);
-#endif
-
-	return 0;
-}
-
-void rtl92e_deinit_core(struct ieee80211_hw *hw)
-{
-}
-
-void rtl92e_init_rx_config(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *)(&mac->rx_conf));
-}
-
-/*********************************************************
- *
- * tx information functions
- *
- *********************************************************/
-static void _rtl_qurey_shortpreamble_mode(struct ieee80211_hw *hw,
-					  struct rtl_tcb_desc *tcb_desc,
-					  struct ieee80211_tx_info *info)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 rate_flag = info->control.rates[0].flags;
-
-	tcb_desc->use_shortpreamble = false;
-
-	/* 1M can only use Long Preamble. 11B spec */
-	if (tcb_desc->hw_rate == rtlpriv->cfg->maps[RTL_RC_CCK_RATE1M])
-		return;
-	else if (rate_flag & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
-		tcb_desc->use_shortpreamble = true;
-
-	return;
-}
-
-static void _rtl_query_shortgi(struct ieee80211_hw *hw,
-			       struct ieee80211_sta *sta,
-			       struct rtl_tcb_desc *tcb_desc,
-			       struct ieee80211_tx_info *info)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u8 rate_flag = info->control.rates[0].flags;
-	u8 sgi_40 = 0, sgi_20 = 0, bw_40 = 0;
-	u8 sgi_80 = 0, bw_80 = 0;
-	tcb_desc->use_shortgi = false;
-
-	if (sta == NULL)
-		return;
-
-	sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
-	sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
-	sgi_80 = sta->vht_cap.cap & IEEE80211_VHT_CAP_SHORT_GI_80;
-
-	if (!(sta->ht_cap.ht_supported) && !(sta->vht_cap.vht_supported))
-		return;
-
-	if (!sgi_40 && !sgi_20)
-		return;
-
-	if (mac->opmode == NL80211_IFTYPE_STATION) {
-		bw_40 = mac->bw_40;
-		bw_80 = mac->bw_80;
-	} else if (mac->opmode == NL80211_IFTYPE_AP ||
-		   mac->opmode == NL80211_IFTYPE_ADHOC ||
-		   mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-		bw_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40;
-		bw_80 = sta->vht_cap.vht_supported;
-	}
-
-	if (bw_80) {
-		if (sgi_80)
-			tcb_desc->use_shortgi = true;
-		else
-			tcb_desc->use_shortgi = false;
-	} else {
-		if (bw_40 && sgi_40)
-			tcb_desc->use_shortgi = true;
-		else if (!bw_40 && sgi_20)
-			tcb_desc->use_shortgi = true;
-	}
-
-	if (!(rate_flag & IEEE80211_TX_RC_SHORT_GI))
-		tcb_desc->use_shortgi = false;
-}
-
-static void _rtl_query_protection_mode(struct ieee80211_hw *hw,
-				       struct rtl_tcb_desc *tcb_desc,
-				       struct ieee80211_tx_info *info)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 rate_flag = info->control.rates[0].flags;
-
-	/* Common Settings */
-	tcb_desc->b_rts_stbc = false;
-	tcb_desc->b_cts_enable = false;
-	tcb_desc->rts_sc = 0;
-	tcb_desc->b_rts_bw = false;
-	tcb_desc->b_rts_use_shortpreamble = false;
-	tcb_desc->b_rts_use_shortgi = false;
-
-	if (rate_flag & IEEE80211_TX_RC_USE_CTS_PROTECT) {
-		/* Use CTS-to-SELF in protection mode. */
-		tcb_desc->b_rts_enable = true;
-		tcb_desc->b_cts_enable = true;
-		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
-	} else if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
-		/* Use RTS-CTS in protection mode. */
-		tcb_desc->b_rts_enable = true;
-		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
-	}
-}
-
-static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
-				   struct ieee80211_sta *sta,
-				   struct rtl_tcb_desc *tcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_sta_info *sta_entry = NULL;
-	u8 ratr_index = 7;
-
-	if (sta) {
-		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-		ratr_index = sta_entry->ratr_index;
-	}
-	if (!tcb_desc->disable_ratefallback || !tcb_desc->use_driver_rate) {
-		if (mac->opmode == NL80211_IFTYPE_STATION) {
-			tcb_desc->ratr_index = 0;
-		} else if (mac->opmode == NL80211_IFTYPE_ADHOC ||
-				mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-			if (tcb_desc->b_multicast || tcb_desc->b_broadcast) {
-				tcb_desc->hw_rate =
-				    rtlpriv->cfg->maps[RTL_RC_CCK_RATE2M];
-				tcb_desc->use_driver_rate = 1;
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
-			} else {
-				tcb_desc->ratr_index = ratr_index;
-			}
-		} else if (mac->opmode == NL80211_IFTYPE_AP) {
-			tcb_desc->ratr_index = ratr_index;
-		}
-	}
-
-	if (rtlpriv->dm.b_useramask) {
-		tcb_desc->ratr_index = ratr_index;
-		/* TODO we will differentiate adhoc and station futrue  */
-		if (mac->opmode == NL80211_IFTYPE_STATION ||
-		    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-			tcb_desc->mac_id = 0;
-
-			if (mac->mode == WIRELESS_MODE_AC_5G)
-				tcb_desc->ratr_index =
-					RATR_INX_WIRELESS_AC_5N;
-			else if (mac->mode == WIRELESS_MODE_AC_24G)
-				tcb_desc->ratr_index =
-					RATR_INX_WIRELESS_AC_24N;
-			else if (mac->mode == WIRELESS_MODE_N_24G)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_NGB;
-			else if (mac->mode == WIRELESS_MODE_N_5G)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_NG;
-			else if (mac->mode & WIRELESS_MODE_G)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_GB;
-			else if (mac->mode & WIRELESS_MODE_B)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_B;
-			else if (mac->mode & WIRELESS_MODE_A)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_G;
-
-		} else if (mac->opmode == NL80211_IFTYPE_AP ||
-			   mac->opmode == NL80211_IFTYPE_ADHOC) {
-			if (sta) {
-				if (sta->aid > 0)
-					tcb_desc->mac_id = sta->aid + 1;
-				else
-					tcb_desc->mac_id = 1;
-			} else {
-				tcb_desc->mac_id = 0;
-			}
-		}
-	}
-}
-
-static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
-				      struct ieee80211_sta *sta,
-				      struct rtl_tcb_desc *tcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	tcb_desc->packet_bw = 0;
-	if (!sta)
-		return;
-	if (mac->opmode == NL80211_IFTYPE_AP ||
-	    mac->opmode == NL80211_IFTYPE_ADHOC ||
-	    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-		if (!(sta->ht_cap.ht_supported) ||
-		    !(sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
-			return;
-	} else if (mac->opmode == NL80211_IFTYPE_STATION) {
-		if (!mac->bw_40 || !(sta->ht_cap.ht_supported))
-			return;
-	}
-	if (tcb_desc->b_multicast || tcb_desc->b_broadcast)
-		return;
-
-	/*use legency rate, shall use 20MHz */
-	if (tcb_desc->hw_rate <= rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M])
-		return;
-
-	tcb_desc->packet_bw = HT_CHANNEL_WIDTH_20_40;
-
-	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8812AE ||
-	    rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (mac->opmode == NL80211_IFTYPE_AP ||
-		    mac->opmode == NL80211_IFTYPE_ADHOC ||
-		    mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-			if (!(sta->vht_cap.vht_supported)) {
-				return;
-			} else if (mac->opmode == NL80211_IFTYPE_STATION) {
-				if (!mac->bw_80 ||
-				    !(sta->vht_cap.vht_supported))
-					return;
-			}
-		}
-		if (tcb_desc->hw_rate <=
-			rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15])
-			return;
-		tcb_desc->packet_bw = HT_CHANNEL_WIDTH_80;
-	}
-}
-
-static u8 _rtl_get_vht_highest_n_rate(struct ieee80211_hw *hw,
-				      struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 hw_rate;
-	u16 map = le16_to_cpu(sta->vht_cap.vht_mcs.tx_mcs_map);
-
-	if (get_rf_type(rtlphy) == RF_2T2R) {
-		if ((map & 0x000c) >> 2 == IEEE80211_VHT_MCS_SUPPORT_0_7)
-			hw_rate =
-			rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS7];
-		else if ((map  & 0x000c) >> 2 == IEEE80211_VHT_MCS_SUPPORT_0_8)
-			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS9];
-		else
-			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_2SS_MCS9];
-	} else {
-		if ((map  & 0x0003) == IEEE80211_VHT_MCS_SUPPORT_0_7)
-			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS7];
-		else if ((map  & 0x0003) == IEEE80211_VHT_MCS_SUPPORT_0_8)
-			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS9];
-		else
-			hw_rate = rtlpriv->cfg->maps[RTL_RC_VHT_RATE_1SS_MCS9];
-	}
-
-	return hw_rate;
-}
-
-static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw,
-				  struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 hw_rate;
-
-	if ((get_rf_type(rtlphy) == RF_2T2R) &&
-	    (sta->ht_cap.mcs.rx_mask[1] != 0))
-		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15];
-	else
-		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS7];
-
-	return hw_rate;
-}
-
-void stg_rtl_get_tcb_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	struct ieee80211_rate *txrate;
-	__le16 fc = rtl_get_fc(skb);
-
-	txrate = ieee80211_get_tx_rate(hw, info);
-	if (txrate != NULL)
-		tcb_desc->hw_rate = txrate->hw_value;
-
-	if (ieee80211_is_data(fc)) {
-		/*
-		 *we set data rate INX 0
-		 *in rtl_rc.c   if skb is special data or
-		 *mgt which need low data rate.
-		 */
-
-		/*
-		 *So tcb_desc->hw_rate is just used for
-		 *special data and mgt frames
-		 */
-		if (info->control.rates[0].idx == 0 ||
-		    ieee80211_is_nullfunc(fc)) {
-			tcb_desc->use_driver_rate = true;
-			tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
-
-			tcb_desc->disable_ratefallback = 1;
-		} else {
-			/*
-			 *because hw will nerver use hw_rate
-			 *when tcb_desc->use_driver_rate = false
-			 *so we never set highest N rate here,
-			 *and N rate will all be controled by FW
-			 *when tcb_desc->use_driver_rate = false
-			 */
-			if (sta && sta->vht_cap.vht_supported) {
-				tcb_desc->hw_rate =
-				_rtl_get_vht_highest_n_rate(hw, sta);
-			} else if (sta && (sta->ht_cap.ht_supported)) {
-				tcb_desc->hw_rate =
-					_rtl_get_highest_n_rate(hw, sta);
-			} else {
-				if (rtlmac->mode == WIRELESS_MODE_B) {
-					tcb_desc->hw_rate =
-					    rtlpriv->cfg->maps[RTL_RC_CCK_RATE11M];
-				} else {
-					tcb_desc->hw_rate =
-					    rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M];
-				}
-			}
-		}
-
-		if (is_multicast_ether_addr(ieee80211_get_DA(hdr)))
-			tcb_desc->b_multicast = 1;
-		else if (is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
-			tcb_desc->b_broadcast = 1;
-
-		_rtl_txrate_selectmode(hw, sta, tcb_desc);
-		_rtl_query_bandwidth_mode(hw, sta, tcb_desc);
-		_rtl_qurey_shortpreamble_mode(hw, tcb_desc, info);
-		_rtl_query_shortgi(hw, sta, tcb_desc, info);
-		_rtl_query_protection_mode(hw, tcb_desc, info);
-	} else {
-		tcb_desc->use_driver_rate = true;
-		tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
-		tcb_desc->disable_ratefallback = 1;
-		tcb_desc->mac_id = 0;
-		tcb_desc->packet_bw = 0;
-	}
-}
-EXPORT_SYMBOL(stg_rtl_get_tcb_desc);
-
-bool rtl92e_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	__le16 fc = rtl_get_fc(skb);
-
-	if (rtlpriv->dm.supp_phymode_switch &&
-	    mac->link_state < MAC80211_LINKED &&
-	    (ieee80211_is_auth(fc) || ieee80211_is_probe_req(fc))) {
-		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-	}
-	if (ieee80211_is_auth(fc)) {
-		RT_TRACE(COMP_SEND, DBG_DMESG, ("MAC80211_LINKING\n"));
-		rtl92e_ips_nic_on(hw);
-
-		mac->link_state = MAC80211_LINKING;
-		/* Dul mac */
-		rtlpriv->phy.b_need_iqk = true;
-	}
-	return true;
-}
-
-struct sk_buff *rtl92e_make_del_ba(struct ieee80211_hw *hw, u8 *sa,
-				   u8 *bssid, u16 tid);
-
-bool rtl92e_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	__le16 fc = rtl_get_fc(skb);
-	u8 *act = (u8 *)(((u8 *)skb->data + MAC80211_3ADDR_LEN));
-	u8 category;
-
-	if (!ieee80211_is_action(fc))
-		return true;
-
-	category = *act;
-	act++;
-	switch (category) {
-	case ACT_CAT_BA:
-		switch (*act) {
-		case ACT_ADDBAREQ:
-			if (mac->act_scanning)
-				return false;
-
-			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-				 ("%s ACT_ADDBAREQ From:%pM\n",
-				is_tx ? "Tx" : "Rx", hdr->addr2));
-			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, "req\n",
-				      skb->data, skb->len);
-			if (!is_tx) {
-				struct ieee80211_sta *sta = NULL;
-				struct rtl_sta_info *sta_entry = NULL;
-				struct ieee80211_mgmt *mgmt = (void *)skb->data;
-				u16 capab = 0, tid = 0;
-				struct rtl_tid_data *tid_data;
-				struct sk_buff *skb_delba = NULL;
-				struct ieee80211_rx_status rx_status = { 0 };
-
-				rcu_read_lock();
-				sta = rtl_find_sta(hw, hdr->addr3);
-				if (sta == NULL) {
-					RT_TRACE((COMP_SEND | COMP_RECV),
-						 DBG_TRACE, ("sta is NULL\n"));
-					rcu_read_unlock();
-					return true;
-				}
-
-				sta_entry =
-					(struct rtl_sta_info *)sta->drv_priv;
-				if (!sta_entry) {
-					rcu_read_unlock();
-					return true;
-				}
-				capab = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
-				tid = (capab &
-					IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
-				tid_data = &sta_entry->tids[tid];
-				if (tid_data->agg.rx_agg_state ==
-				    RTL_RX_AGG_START) {
-					skb_delba = rtl92e_make_del_ba(hw,
-								    hdr->addr2,
-								    hdr->addr3,
-								    tid);
-					if (skb_delba) {
-						rx_status.freq =
-							hw->conf.chandef.chan->center_freq;
-						rx_status.band =
-							hw->conf.chandef.chan->band;
-						rx_status.flag |= RX_FLAG_DECRYPTED;
-						rx_status.flag |= RX_FLAG_MACTIME_MPDU;
-						rx_status.rate_idx = 0;
-						rx_status.signal = 50 + 10;
-						memcpy(IEEE80211_SKB_RXCB(skb_delba),
-						       &rx_status, sizeof(rx_status));
-						RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG,
-							      "fake del\n",
-							      skb_delba->data, skb_delba->len);
-						ieee80211_rx_irqsafe(hw, skb_delba);
-					}
-				}
-				rcu_read_unlock();
-			}
-			break;
-		case ACT_ADDBARSP:
-			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-				 ("%s ACT_ADDBARSP From :%pM\n",
-				  is_tx ? "Tx" : "Rx", hdr->addr2));
-			break;
-		case ACT_DELBA:
-			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-				 ("ACT_ADDBADEL From :%pM\n", hdr->addr2));
-			break;
-		}
-		break;
-	default:
-		break;
-	}
-
-	return true;
-}
-
-/*should call before software enc*/
-u8 rtl92e_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb,
-			  u8 is_tx)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	__le16 fc = rtl_get_fc(skb);
-	u16 ether_type;
-	u8 mac_hdr_len = ieee80211_get_hdrlen_from_skb(skb);
-	const struct iphdr *ip;
-
-	if (!ieee80211_is_data(fc))
-		goto end;
-
-	ip = (struct iphdr *)((u8 *)skb->data + mac_hdr_len +
-			      SNAP_SIZE + PROTOC_TYPE_SIZE);
-	ether_type = be16_to_cpup((__be16 *)
-				  (skb->data + mac_hdr_len + SNAP_SIZE));
-
-	if (ETH_P_IP == ether_type) {
-		if (IPPROTO_UDP == ip->protocol) {
-			struct udphdr *udp = (struct udphdr *)((u8 *)ip +
-							       (ip->ihl << 2));
-			if (((((u8 *)udp)[1] == 68) &&
-			     (((u8 *)udp)[3] == 67)) ||
-			    ((((u8 *)udp)[1] == 67) &&
-			     (((u8 *)udp)[3] == 68))) {
-				/*
-				 * 68 : UDP BOOTP client
-				 * 67 : UDP BOOTP server
-				 */
-				RT_TRACE((COMP_SEND | COMP_RECV),
-					 DBG_DMESG, ("dhcp %s !!\n",
-						     (is_tx) ? "Tx" : "Rx"));
-
-				if (is_tx) {
-					rtlpriv->ra.is_special_data = true;
-					if (rtlpriv->cfg->ops->get_btc_status())
-						rtlpriv->btcoexist.btc_ops->btc_special_packet_notify(
-									rtlpriv, 1);
-					rtl92e_lps_leave(hw);
-					ppsc->last_delaylps_stamp_jiffies =
-									jiffies;
-				}
-
-				return true;
-			}
-		}
-	} else if (ETH_P_ARP == ether_type) {
-		if (is_tx) {
-			rtlpriv->ra.is_special_data = true;
-			if (rtlpriv->cfg->ops->get_btc_status())
-				rtlpriv->btcoexist.btc_ops->btc_special_packet_notify(
-							rtlpriv, 1);
-			rtl92e_lps_leave(hw);
-			ppsc->last_delaylps_stamp_jiffies = jiffies;
-		}
-
-		return true;
-	} else if (ETH_P_PAE == ether_type) {
-		RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-			 ("802.1X %s EAPOL pkt!!\n", (is_tx) ? "Tx" : "Rx"));
-
-		if (is_tx) {
-			rtlpriv->ra.is_special_data = true;
-			rtl92e_lps_leave(hw);
-			ppsc->last_delaylps_stamp_jiffies = jiffies;
-		}
-
-		return true;
-	} else if (0x86DD == ether_type) {
-		return true;
-	}
-
-end:
-	rtlpriv->ra.is_special_data = false;
-	return false;
-}
-
-/*********************************************************
- *
- * functions called by core.c
- *
- *********************************************************/
-int rtl92e_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-			struct ieee80211_sta *sta, u16 tid, u16 *ssn)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	if (!sta_entry)
-		return -ENXIO;
-	tid_data = &sta_entry->tids[tid];
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
-		  tid_data->seq_number));
-
-	*ssn = tid_data->seq_number;
-	tid_data->agg.agg_state = RTL_AGG_START;
-
-	ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
-	return 0;
-}
-
-int rtl92e_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		       struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	/* Comparing an array to null is not useful */
-	/*if (!sta->addr) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
-		return -EINVAL;
-	}*/
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d\n", sta->addr, tid));
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	tid_data = &sta_entry->tids[tid];
-	sta_entry->tids[tid].agg.agg_state = RTL_AGG_STOP;
-
-	ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
-	return 0;
-}
-
-int rtl92e_rx_agg_start(struct ieee80211_hw *hw,
-			struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	if (!sta_entry)
-		return -ENXIO;
-	tid_data = &sta_entry->tids[tid];
-
-	RT_TRACE(COMP_RECV, DBG_DMESG,
-		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
-		 tid_data->seq_number));
-
-	tid_data->agg.rx_agg_state = RTL_RX_AGG_START;
-	return 0;
-}
-
-int rtl92e_rx_agg_stop(struct ieee80211_hw *hw,
-		       struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	/* Comparing an array to null is not useful */
-	/*if (!sta->addr) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
-		return -EINVAL;
-	}*/
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d\n", sta->addr, tid));
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	tid_data = &sta_entry->tids[tid];
-	sta_entry->tids[tid].agg.rx_agg_state = RTL_RX_AGG_STOP;
-
-	return 0;
-}
-
-int rtl92e_tx_agg_oper(struct ieee80211_hw *hw,
-		       struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	/* Comparing an array to null is not useful */
-	/*if (!sta->addr) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
-		return -EINVAL;
-	}*/
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d\n", sta->addr, tid));
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	tid_data = &sta_entry->tids[tid];
-	sta_entry->tids[tid].agg.agg_state = RTL_AGG_OPERATIONAL;
-
-	return 0;
-}
-
-/*********************************************************
- *
- * wq & timer callback functions
- *
- *********************************************************/
-/* this function is used for roaming */
-void rtl92e_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-
-	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
-		return;
-
-	if (rtlpriv->mac80211.link_state < MAC80211_LINKED)
-		return;
-
-	/* check if this really is a beacon */
-	if (!ieee80211_is_beacon(hdr->frame_control) &&
-	    !ieee80211_is_probe_resp(hdr->frame_control))
-		return;
-
-	/* min. beacon length + FCS_LEN */
-	if (skb->len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	rtlpriv->link_info.bcn_rx_inperiod++;
-}
-
-void rtl92e_watchdog_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
-							    struct rtl_works,
-							    watchdog_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	bool b_busytraffic = false;
-	bool b_tx_busy_traffic = false;
-	bool b_rx_busy_traffic = false;
-	bool b_higher_busytraffic = false;
-	bool b_higher_busyrxtraffic = false;
-	u8 idx, tid;
-	u32 rx_cnt_inp4eriod = 0;
-	u32 tx_cnt_inp4eriod = 0;
-	u32 aver_rx_cnt_inperiod = 0;
-	u32 aver_tx_cnt_inperiod = 0;
-	u32 aver_tidtx_inperiod[MAX_TID_COUNT] = {0};
-	u32 tidtx_inp4eriod[MAX_TID_COUNT] = {0};
-	bool benter_ps = false;
-
-	if (is_hal_stop(rtlhal))
-		return;
-
-	/* <1> Determine if action frame is allowed */
-	if (mac->link_state > MAC80211_NOLINK) {
-		if (mac->cnt_after_linked < 20)
-			mac->cnt_after_linked++;
-	} else {
-		mac->cnt_after_linked = 0;
-	}
-
-	/* <2> to check if traffic busy, if
-	 * busytraffic we don't change channel */
-	if (mac->link_state >= MAC80211_LINKED) {
-		/* (1) get aver_rx_cnt_inperiod & aver_tx_cnt_inperiod */
-		for (idx = 0; idx <= 2; idx++) {
-			rtlpriv->link_info.num_rx_in4period[idx] =
-			    rtlpriv->link_info.num_rx_in4period[idx + 1];
-			rtlpriv->link_info.num_tx_in4period[idx] =
-			    rtlpriv->link_info.num_tx_in4period[idx + 1];
-		}
-		rtlpriv->link_info.num_rx_in4period[3] =
-		    rtlpriv->link_info.num_rx_inperiod;
-		rtlpriv->link_info.num_tx_in4period[3] =
-		    rtlpriv->link_info.num_tx_inperiod;
-		for (idx = 0; idx <= 3; idx++) {
-			rx_cnt_inp4eriod +=
-			    rtlpriv->link_info.num_rx_in4period[idx];
-			tx_cnt_inp4eriod +=
-			    rtlpriv->link_info.num_tx_in4period[idx];
-		}
-		aver_rx_cnt_inperiod = rx_cnt_inp4eriod / 4;
-		aver_tx_cnt_inperiod = tx_cnt_inp4eriod / 4;
-
-		/* (2) check traffic busy */
-		if (aver_rx_cnt_inperiod > 100 || aver_tx_cnt_inperiod > 100) {
-			b_busytraffic = true;
-			if (aver_rx_cnt_inperiod > aver_tx_cnt_inperiod)
-				b_rx_busy_traffic = true;
-			else
-				b_tx_busy_traffic = false;
-		}
-
-		/* Higher Tx/Rx data. */
-		if (aver_rx_cnt_inperiod > 4000 ||
-		    aver_tx_cnt_inperiod > 4000) {
-			b_higher_busytraffic = true;
-
-			/* Extremely high Rx data. */
-			if (aver_rx_cnt_inperiod > 5000)
-				b_higher_busyrxtraffic = true;
-		}
-
-		/* check every tid's tx traffic */
-		for (tid = 0; tid <= 7; tid++) {
-			for (idx = 0; idx <= 2; idx++)
-				rtlpriv->link_info.tidtx_in4period[tid][idx] =
-					rtlpriv->link_info.tidtx_in4period[tid]
-					[idx + 1];
-			rtlpriv->link_info.tidtx_in4period[tid][3] =
-				rtlpriv->link_info.tidtx_inperiod[tid];
-
-			for (idx = 0; idx <= 3; idx++)
-				tidtx_inp4eriod[tid] +=
-				   rtlpriv->link_info.tidtx_in4period[tid][idx];
-			aver_tidtx_inperiod[tid] = tidtx_inp4eriod[tid] / 4;
-			if (aver_tidtx_inperiod[tid] > 5000)
-				rtlpriv->link_info.higher_busytxtraffic[tid] =
-									true;
-			else
-				rtlpriv->link_info.higher_busytxtraffic[tid] =
-									false;
-		}
-
-		if (((rtlpriv->link_info.num_rx_inperiod +
-		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
-		    (rtlpriv->link_info.num_rx_inperiod > 2))
-			benter_ps = false;
-		else
-			benter_ps = true;
-
-		/* LeisurePS only work in infra mode. */
-		if (benter_ps)
-			rtl92e_lps_enter(hw);
-		else
-			rtl92e_lps_leave(hw);
-	}
-
-	rtlpriv->link_info.num_rx_inperiod = 0;
-	rtlpriv->link_info.num_tx_inperiod = 0;
-	for (tid = 0; tid <= 7; tid++)
-		rtlpriv->link_info.tidtx_inperiod[tid] = 0;
-
-	rtlpriv->link_info.b_busytraffic = b_busytraffic;
-	rtlpriv->link_info.b_rx_busy_traffic = b_rx_busy_traffic;
-	rtlpriv->link_info.b_tx_busy_traffic = b_tx_busy_traffic;
-	rtlpriv->link_info.b_higher_busytraffic = b_higher_busytraffic;
-	rtlpriv->link_info.b_higher_busyrxtraffic = b_higher_busyrxtraffic;
-
-	/* <3> DM */
-	rtlpriv->cfg->ops->dm_watchdog(hw);
-
-	/* <4> roaming */
-	if (mac->link_state == MAC80211_LINKED &&
-	    mac->opmode == NL80211_IFTYPE_STATION) {
-		if ((rtlpriv->link_info.bcn_rx_inperiod +
-			rtlpriv->link_info.num_rx_inperiod) == 0) {
-			rtlpriv->link_info.roam_times++;
-			RT_TRACE(COMP_ERR, DBG_DMESG,
-				 ("AP off for %d s\n",
-				  (rtlpriv->link_info.roam_times * 2)));
-
-			/* if we can't recv beacon for 10s,
-			* we should reconnect this AP */
-			if (rtlpriv->link_info.roam_times >= 5) {
-				RT_TRACE(COMP_ERR, DBG_EMERG,
-					 ("AP off, try to reconnect now\n"));
-				rtlpriv->link_info.roam_times = 0;
-				ieee80211_connection_loss(
-					rtlpriv->mac80211.vif);
-			}
-		} else {
-			rtlpriv->link_info.roam_times = 0;
-		}
-	}
-
-	if (rtlpriv->cfg->ops->get_btc_status())
-		rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
-
-	rtlpriv->link_info.bcn_rx_inperiod = 0;
-}
-
-void rtl92e_watch_dog_timer_callback(unsigned long data)
-{
-	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	queue_delayed_work(rtlpriv->works.rtl_wq,
-			   &rtlpriv->works.watchdog_wq, 0);
-
-	mod_timer(&rtlpriv->works.watchdog_timer,
-		  jiffies + MSECS(RTL_WATCH_DOG_TIME));
-}
-void rtl92e_fwevt_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-		container_of_dwork_rtl(data, struct rtl_works, fwevt_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->cfg->ops->c2h_command_handle(hw);
-}
-void rtl92e_easy_concurrent_retrytimer_callback(unsigned long data)
-{
-	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_priv *buddy_priv = rtlpriv->buddy_priv;
-
-	if (buddy_priv == NULL)
-		return;
-
-	rtlpriv->cfg->ops->dualmac_easy_concurrent(hw);
-}
-/*********************************************************
- *
- * frame process functions
- *
- *********************************************************/
-u8 *rtl92e_find_ie(u8 *data, unsigned int len, u8 ie)
-{
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	u8 *pos, *end;
-
-	pos = (u8 *)mgmt->u.beacon.variable;
-	end = data + len;
-	while (pos < end) {
-		if (pos + 2 + pos[1] > end)
-			return NULL;
-
-		if (pos[0] == ie)
-			return pos;
-
-		pos += 2 + pos[1];
-	}
-	return NULL;
-}
-
-/* when we use 2 rx ants we send IEEE80211_SMPS_OFF */
-/* when we use 1 rx ant we send IEEE80211_SMPS_STATIC */
-static struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
-					    enum ieee80211_smps_mode smps,
-					    u8 *da, u8 *bssid)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct sk_buff *skb;
-	struct ieee80211_mgmt_compat *action_frame;
-
-	/* 27 = header + category + action + smps mode */
-	skb = dev_alloc_skb(27 + hw->extra_tx_headroom);
-	if (!skb)
-		return NULL;
-
-	skb_reserve(skb, hw->extra_tx_headroom);
-	action_frame = (void *)skb_put(skb, 27);
-	memset(action_frame, 0, 27);
-	ether_addr_copy(action_frame->da, da);
-	ether_addr_copy(action_frame->sa, rtlefuse->dev_addr);
-	ether_addr_copy(action_frame->bssid, bssid);
-	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-						  IEEE80211_STYPE_ACTION);
-	action_frame->u.action.category = WLAN_CATEGORY_HT;
-	action_frame->u.action.u.ht_smps.action = WLAN_HT_ACTION_SMPS;
-	switch (smps) {
-	case IEEE80211_SMPS_AUTOMATIC:/* 0 */
-	case IEEE80211_SMPS_NUM_MODES:/* 4 */
-		WARN_ON(1);
-	/* Here will get a 'MISSING_BREAK' in Coverity Test, just ignore it.
-	 * According to Kernel Code, here is right.
-	 */
-	case IEEE80211_SMPS_OFF:/* 1 */ /*MIMO_PS_NOLIMIT*/
-		action_frame->u.action.u.ht_smps.smps_control =
-				WLAN_HT_SMPS_CONTROL_DISABLED;/* 0 */
-		break;
-	case IEEE80211_SMPS_STATIC:/* 2 */ /*MIMO_PS_STATIC*/
-		action_frame->u.action.u.ht_smps.smps_control =
-				WLAN_HT_SMPS_CONTROL_STATIC;/* 1 */
-		break;
-	case IEEE80211_SMPS_DYNAMIC:/* 3 */ /*MIMO_PS_DYNAMIC*/
-		action_frame->u.action.u.ht_smps.smps_control =
-				WLAN_HT_SMPS_CONTROL_DYNAMIC;/* 3 */
-		break;
-	}
-
-	return skb;
-}
-
-int stg_rtl_send_smps_action(struct ieee80211_hw *hw,
-			     struct ieee80211_sta *sta,
-			     enum ieee80211_smps_mode smps)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct sk_buff *skb = NULL;
-	struct rtl_tcb_desc tcb_desc;
-	u8 bssid[ETH_ALEN] = {0};
-
-	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-
-	if (rtlpriv->mac80211.act_scanning)
-		goto err_free;
-
-	if (!sta)
-		goto err_free;
-
-	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
-		goto err_free;
-
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		goto err_free;
-
-	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP)
-		ether_addr_copy(bssid, rtlpriv->efuse.dev_addr);
-	else
-		ether_addr_copy(bssid, rtlpriv->mac80211.bssid);
-
-	skb = rtl_make_smps_action(hw, smps, sta->addr, bssid);
-	/* this is a type = mgmt * stype = action frame */
-	if (skb) {
-		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-		struct rtl_sta_info *sta_entry =
-			(struct rtl_sta_info *)sta->drv_priv;
-		sta_entry->mimo_ps = smps;
-		/* rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0); */
-
-		info->control.rates[0].idx = 0;
-		info->band = hw->conf.chandef.chan->band;
-		rtlpriv->intf_ops->adapter_tx(hw, sta, skb, &tcb_desc);
-	}
-	return 1;
-
-err_free:
-	return 0;
-}
-EXPORT_SYMBOL(stg_rtl_send_smps_action);
-
-/* because mac80211 have issues when can receive del ba
- * so here we just make a fake del_ba if we receive a ba_req
- * but rx_agg was opened to let mac80211 release some ba
- * related resources, so please this del_ba for tx */
-struct sk_buff *rtl92e_make_del_ba(struct ieee80211_hw *hw,
-				   u8 *sa, u8 *bssid, u16 tid)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct sk_buff *skb;
-	struct ieee80211_mgmt *action_frame;
-	u16 params;
-
-	/* 27 = header + category + action + smps mode */
-	skb = dev_alloc_skb(34 + hw->extra_tx_headroom);
-	if (!skb)
-		return NULL;
-
-	skb_reserve(skb, hw->extra_tx_headroom);
-	action_frame = (void *)skb_put(skb, 34);
-	memset(action_frame, 0, 34);
-	ether_addr_copy(action_frame->sa, sa);
-	ether_addr_copy(action_frame->da, rtlefuse->dev_addr);
-	ether_addr_copy(action_frame->bssid, bssid);
-	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-						  IEEE80211_STYPE_ACTION);
-	action_frame->u.action.category = WLAN_CATEGORY_BACK;
-	action_frame->u.action.u.delba.action_code = WLAN_ACTION_DELBA;
-	params = (u16)(1 << 11);	/* bit 11 initiator */
-	params |= (u16)(tid << 12);	/* bit 15:12 TID number */
-
-	action_frame->u.action.u.delba.params = cpu_to_le16(params);
-	action_frame->u.action.u.delba.reason_code =
-		cpu_to_le16(WLAN_REASON_QSTA_TIMEOUT);
-
-	return skb;
-}
-
-/*********************************************************
- *
- * IOT functions
- *
- *********************************************************/
-static bool rtl_chk_vendor_ouisub(struct ieee80211_hw *hw,
-				  struct octet_string vendor_ie)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool matched = false;
-	static u8 athcap_1[] = { 0x00, 0x03, 0x7F };
-	static u8 athcap_2[] = { 0x00, 0x13, 0x74 };
-	static u8 broadcap_1[] = { 0x00, 0x10, 0x18 };
-	static u8 broadcap_2[] = { 0x00, 0x0a, 0xf7 };
-	static u8 broadcap_3[] = { 0x00, 0x05, 0xb5 };
-	static u8 racap[] = { 0x00, 0x0c, 0x43 };
-	static u8 ciscocap[] = { 0x00, 0x40, 0x96 };
-	static u8 marvcap[] = { 0x00, 0x50, 0x43 };
-
-	if (memcmp(vendor_ie.octet, athcap_1, 3) == 0 ||
-	    memcmp(vendor_ie.octet, athcap_2, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_ATH;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, broadcap_1, 3) == 0 ||
-		   memcmp(vendor_ie.octet, broadcap_2, 3) == 0 ||
-		   memcmp(vendor_ie.octet, broadcap_3, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_BROAD;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, racap, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_RAL;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, ciscocap, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_CISCO;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, marvcap, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_MARV;
-		matched = true;
-	}
-
-	return matched;
-}
-
-static bool rtl_find_221_ie(struct ieee80211_hw *hw, u8 *data, unsigned int len)
-{
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	struct octet_string vendor_ie;
-	u8 *pos, *end;
-
-	pos = (u8 *)mgmt->u.beacon.variable;
-	end = data + len;
-	while (pos < end) {
-		if (pos[0] == 221) {
-			vendor_ie.length = pos[1];
-			vendor_ie.octet = &pos[2];
-			if (rtl_chk_vendor_ouisub(hw, vendor_ie))
-				return true;
-		}
-
-		if (pos + 2 + pos[1] > end)
-			return false;
-
-		pos += 2 + pos[1];
-	}
-	return false;
-}
-
-void rtl92e_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = (void *)data;
-	u32 vendor = PEER_UNKNOWN;
-
-	static u8 ap3_1[3] = { 0x00, 0x14, 0xbf };
-	static u8 ap3_2[3] = { 0x00, 0x1a, 0x70 };
-	static u8 ap3_3[3] = { 0x00, 0x1d, 0x7e };
-	static u8 ap4_1[3] = { 0x00, 0x90, 0xcc };
-	static u8 ap4_2[3] = { 0x00, 0x0e, 0x2e };
-	static u8 ap4_3[3] = { 0x00, 0x18, 0x02 };
-	static u8 ap4_4[3] = { 0x00, 0x17, 0x3f };
-	static u8 ap4_5[3] = { 0x00, 0x1c, 0xdf };
-	static u8 ap5_1[3] = { 0x00, 0x1c, 0xf0 };
-	static u8 ap5_2[3] = { 0x00, 0x21, 0x91 };
-	static u8 ap5_3[3] = { 0x00, 0x24, 0x01 };
-	static u8 ap5_4[3] = { 0x00, 0x15, 0xe9 };
-	static u8 ap5_5[3] = { 0x00, 0x17, 0x9A };
-	static u8 ap5_6[3] = { 0x00, 0x18, 0xE7 };
-	static u8 ap6_1[3] = { 0x00, 0x17, 0x94 };
-	static u8 ap7_1[3] = { 0x00, 0x14, 0xa4 };
-
-	if (mac->opmode != NL80211_IFTYPE_STATION)
-		return;
-
-	if (mac->link_state == MAC80211_NOLINK) {
-		mac->vendor = PEER_UNKNOWN;
-		return;
-	}
-
-	if (mac->cnt_after_linked > 2)
-		return;
-
-	/* check if this really is a beacon */
-	if (!ieee80211_is_beacon(hdr->frame_control))
-		return;
-
-	/* min. beacon length + FCS_LEN */
-	if (len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	if (rtl_find_221_ie(hw, data, len))
-		vendor = mac->vendor;
-
-	if ((memcmp(mac->bssid, ap5_1, 3) == 0) ||
-	    (memcmp(mac->bssid, ap5_2, 3) == 0) ||
-	    (memcmp(mac->bssid, ap5_3, 3) == 0) ||
-	    (memcmp(mac->bssid, ap5_4, 3) == 0) ||
-	    (memcmp(mac->bssid, ap5_5, 3) == 0) ||
-	    (memcmp(mac->bssid, ap5_6, 3) == 0) ||
-		vendor == PEER_ATH) {
-		vendor = PEER_ATH;
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ath find\n"));
-	} else if ((memcmp(mac->bssid, ap4_4, 3) == 0) ||
-		   (memcmp(mac->bssid, ap4_5, 3) == 0) ||
-		   (memcmp(mac->bssid, ap4_1, 3) == 0) ||
-		   (memcmp(mac->bssid, ap4_2, 3) == 0) ||
-		   (memcmp(mac->bssid, ap4_3, 3) == 0) ||
-		vendor == PEER_RAL) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ral findn\n"));
-		vendor = PEER_RAL;
-	} else if (memcmp(mac->bssid, ap6_1, 3) == 0 ||
-		vendor == PEER_CISCO) {
-		vendor = PEER_CISCO;
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>cisco find\n"));
-	} else if ((memcmp(mac->bssid, ap3_1, 3) == 0) ||
-		(memcmp(mac->bssid, ap3_2, 3) == 0) ||
-		(memcmp(mac->bssid, ap3_3, 3) == 0) ||
-		vendor == PEER_BROAD) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>broad find\n"));
-		vendor = PEER_BROAD;
-	} else if (memcmp(mac->bssid, ap7_1, 3) == 0 ||
-		vendor == PEER_MARV) {
-		vendor = PEER_MARV;
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>marv find\n"));
-	}
-
-	mac->vendor = vendor;
-}
-
-/*********************************************************
- *
- * sysfs functions
- *
- *********************************************************/
-struct rtl_global_var global_var = {};
-
-int  rtl_core_module_init(void)
-{
-	static int here_once;
-
-	if (here_once++)
-		return 0;
-
-	if (rtl92e_rate_control_register())
-		pr_debug("rtl: Unable to register rtl_rc, use default RC !!\n");
-
-	/* init some global vars */
-	INIT_LIST_HEAD(&global_var.glb_priv_list);
-	spin_lock_init(&global_var.glb_list_lock);
-
-	return 0;
-}
-void  rtl_core_module_exit(void)
-{
-	/*RC*/
-	rtl92e_rate_control_unregister();
-}
diff --git a/drivers/staging/rtl8192ee/base.h b/drivers/staging/rtl8192ee/base.h
deleted file mode 100644
index 1d6e5a76ce3e..000000000000
--- a/drivers/staging/rtl8192ee/base.h
+++ /dev/null
@@ -1,163 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_BASE_H__
-#define __RTL_BASE_H__
-
-#include "compat.h"
-
-enum ap_peer {
-	PEER_UNKNOWN = 0,
-	PEER_RTL = 1,
-	PEER_RTL_92SE = 2,
-	PEER_BROAD = 3,
-	PEER_RAL = 4,
-	PEER_ATH = 5,
-	PEER_CISCO = 6,
-	PEER_MARV = 7,
-	PEER_AIRGO = 9,
-	PEER_MAX = 10,
-};
-
-#define RTL_DUMMY_OFFSET	0
-#define RTL_DUMMY_UNIT		8
-#define RTL_TX_DUMMY_SIZE	(RTL_DUMMY_OFFSET * RTL_DUMMY_UNIT)
-#define RTL_TX_DESC_SIZE	32
-#define RTL_TX_HEADER_SIZE	(RTL_TX_DESC_SIZE + RTL_TX_DUMMY_SIZE)
-
-#define HT_AMSDU_SIZE_4K	3839
-#define HT_AMSDU_SIZE_8K	7935
-
-#define MAX_BIT_RATE_40MHZ_MCS15	300	/* Mbps */
-#define MAX_BIT_RATE_40MHZ_MCS7		150	/* Mbps */
-
-#define MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS9	867	/* Mbps */
-#define MAX_BIT_RATE_SHORT_GI_2NSS_80MHZ_MCS7	650	/* Mbps */
-#define MAX_BIT_RATE_LONG_GI_2NSS_80MHZ_MCS9	780	/* Mbps */
-#define MAX_BIT_RATE_LONG_GI_2NSS_80MHZ_MCS7	585	/* Mbps */
-
-#define MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS9	434	/* Mbps */
-#define MAX_BIT_RATE_SHORT_GI_1NSS_80MHZ_MCS7	325	/* Mbps */
-#define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS9	390	/* Mbps */
-#define MAX_BIT_RATE_LONG_GI_1NSS_80MHZ_MCS7	293	/* Mbps */
-
-
-#define RTL_RATE_COUNT_LEGACY		12
-#define RTL_CHANNEL_COUNT		14
-
-#define FRAME_OFFSET_FRAME_CONTROL	0
-#define FRAME_OFFSET_DURATION		2
-#define FRAME_OFFSET_ADDRESS1		4
-#define FRAME_OFFSET_ADDRESS2		10
-#define FRAME_OFFSET_ADDRESS3		16
-#define FRAME_OFFSET_SEQUENCE		22
-#define FRAME_OFFSET_ADDRESS4		24
-
-#define SET_80211_HDR_FRAME_CONTROL(_hdr, _val)		\
-	WRITEEF2BYTE(_hdr, _val)
-#define SET_80211_HDR_TYPE_AND_SUBTYPE(_hdr, _val)	\
-	WRITEEF1BYTE(_hdr, _val)
-#define SET_80211_HDR_PWR_MGNT(_hdr, _val)		\
-	SET_BITS_TO_LE_2BYTE(_hdr, 12, 1, _val)
-#define SET_80211_HDR_TO_DS(_hdr, _val)			\
-	SET_BITS_TO_LE_2BYTE(_hdr, 8, 1, _val)
-
-#define SET_80211_PS_POLL_AID(_hdr, _val)		\
-	(*(u16 *)((u8 *)(_hdr) + 2) = _val)
-#define SET_80211_PS_POLL_BSSID(_hdr, _val)		\
-	memcpy(((u8 *)(_hdr)) + 4, (u8 *)(_val), ETH_ALEN)
-#define SET_80211_PS_POLL_TA(_hdr, _val)		\
-	memcpy(((u8 *)(_hdr)) + 10, (u8 *)(_val), ETH_ALEN)
-
-#define SET_80211_HDR_DURATION(_hdr, _val)	\
-	WRITEEF2BYTE((u8 *)(_hdr)+FRAME_OFFSET_DURATION, _val)
-#define SET_80211_HDR_ADDRESS1(_hdr, _val)	\
-	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS1, (u8 *)(_val))
-#define SET_80211_HDR_ADDRESS2(_hdr, _val)	\
-	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS2, (u8 *)(_val))
-#define SET_80211_HDR_ADDRESS3(_hdr, _val)	\
-	CP_MACADDR((u8 *)(_hdr)+FRAME_OFFSET_ADDRESS3, (u8 *)(_val))
-#define SET_80211_HDR_FRAGMENT_SEQUENCE(_hdr, _val)  \
-	WRITEEF2BYTE((u8 *)(_hdr)+FRAME_OFFSET_SEQUENCE, _val)
-
-#define SET_BEACON_PROBE_RSP_TIME_STAMP_LOW(__phdr, __val)	\
-	WRITEEF4BYTE(((u8 *)(__phdr)) + 24, __val)
-#define SET_BEACON_PROBE_RSP_TIME_STAMP_HIGH(__phdr, __val) \
-	WRITEEF4BYTE(((u8 *)(__phdr)) + 28, __val)
-#define SET_BEACON_PROBE_RSP_BEACON_INTERVAL(__phdr, __val) \
-	WRITEEF2BYTE(((u8 *)(__phdr)) + 32, __val)
-#define GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr)		\
-	READEF2BYTE(((u8 *)(__phdr)) + 34)
-#define SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
-	WRITEEF2BYTE(((u8 *)(__phdr)) + 34, __val)
-#define MASK_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
-	SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, \
-	(GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) & (~(__val))))
-
-int rtl92e_init_core(struct ieee80211_hw *hw);
-void rtl92e_deinit_core(struct ieee80211_hw *hw);
-void rtl92e_init_rx_config(struct ieee80211_hw *hw);
-void rtl92e_init_rfkill(struct ieee80211_hw *hw);
-void rtl92e_deinit_rfkill(struct ieee80211_hw *hw);
-
-void rtl92e_watch_dog_timer_callback(unsigned long data);
-void rtl92e_deinit_deferred_work(struct ieee80211_hw *hw);
-
-bool rtl92e_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
-bool rtl92e_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
-u8 rtl92e_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb,
-			  u8 is_tx);
-void rtl92e_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
-void rtl92e_watch_dog_timer_callback(unsigned long data);
-int rtl92e_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-			struct ieee80211_sta *sta, u16 tid, u16 *ssn);
-int rtl92e_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		       struct ieee80211_sta *sta, u16 tid);
-int rtl92e_tx_agg_oper(struct ieee80211_hw *hw,
-		       struct ieee80211_sta *sta, u16 tid);
-int rtl92e_rx_agg_start(struct ieee80211_hw *hw,
-			struct ieee80211_sta *sta, u16 tid);
-int rtl92e_rx_agg_stop(struct ieee80211_hw *hw,
-		       struct ieee80211_sta *sta, u16 tid);
-void rtl92e_watchdog_wq_callback(void *data);
-void rtl92e_fwevt_wq_callback(void *data);
-
-void stg_rtl_get_tcb_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc);
-
-int stg_rtl_send_smps_action(struct ieee80211_hw *hw,
-			     struct ieee80211_sta *sta,
-			     enum ieee80211_smps_mode smps);
-u8 *rtl92e_find_ie(u8 *data, unsigned int len, u8 ie);
-void rtl92e_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len);
-u8 rtl92e_tid_to_ac(struct ieee80211_hw *hw, u8 tid);
-void rtl92e_easy_concurrent_retrytimer_callback(unsigned long data);
-extern struct rtl_global_var global_var;
-int  rtl_core_module_init(void);
-void  rtl_core_module_exit(void);
-
-#endif
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbt_precomp.h b/drivers/staging/rtl8192ee/btcoexist/halbt_precomp.h
deleted file mode 100644
index 56e6c4651891..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbt_precomp.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- *
- ******************************************************************************/
-#ifndef	__HALBT_PRECOMP_H__
-#define __HALBT_PRECOMP_H__
-/*************************************************************
- * include files
- *************************************************************/
-#include "../wifi.h"
-#include "../efuse.h"
-#include "../base.h"
-#include "../regd.h"
-#include "../cam.h"
-#include "../ps.h"
-#include "../pci.h"
-
-#include "halbtcoutsrc.h"
-
-
-#include "halbtc8192e2ant.h"
-#include "halbtc8723b1ant.h"
-#include "halbtc8723b2ant.h"
-#include "halbtc8821a2ant.h"
-#include "halbtc8821a1ant.h"
-
-#define	MASKBYTE0			0xff
-#define	MASKBYTE1			0xff00
-#define	MASKBYTE2			0xff0000
-#define	MASKBYTE3			0xff000000
-#define	MASKHWORD			0xffff0000
-#define	MASKLWORD			0x0000ffff
-#define	MASKDWORD			0xffffffff
-#define	MASK12BITS			0xfff
-#define	MASKH4BITS			0xf0000000
-#define MASKOFDM_D			0xffc00000
-#define	MASKCCK				0x3f3f3f3f
-
-#endif	/* __HALBT_PRECOMP_H__ */
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h
deleted file mode 100644
index ece3e10e6df7..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8192e2ant.h
+++ /dev/null
@@ -1,161 +0,0 @@
-/*****************************************************************
- *   The following is for 8192E 2Ant BT Co-exist definition
- *****************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8192E_2ANT			0
-
-#define	BT_INFO_8192E_2ANT_B_FTP			BIT(7)
-#define	BT_INFO_8192E_2ANT_B_A2DP			BIT(6)
-#define	BT_INFO_8192E_2ANT_B_HID			BIT(5)
-#define	BT_INFO_8192E_2ANT_B_SCO_BUSY			BIT(4)
-#define	BT_INFO_8192E_2ANT_B_ACL_BUSY			BIT(3)
-#define	BT_INFO_8192E_2ANT_B_INQ_PAGE			BIT(2)
-#define	BT_INFO_8192E_2ANT_B_SCO_ESCO			BIT(1)
-#define	BT_INFO_8192E_2ANT_B_CONNECTION			BIT(0)
-
-#define BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT		2
-
-enum bt_info_src_8192e_2ant {
-	BT_INFO_SRC_8192E_2ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8192E_2ANT_BT_RSP			= 0x1,
-	BT_INFO_SRC_8192E_2ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8192E_2ANT_MAX
-};
-
-enum bt_8192e_2ant_bt_status {
-	BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
-	BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
-	BT_8192E_2ANT_BT_STATUS_INQ_PAGE		= 0x2,
-	BT_8192E_2ANT_BT_STATUS_ACL_BUSY		= 0x3,
-	BT_8192E_2ANT_BT_STATUS_SCO_BUSY		= 0x4,
-	BT_8192E_2ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
-	BT_8192E_2ANT_BT_STATUS_MAX
-};
-
-enum bt_8192e_2ant_coex_algo {
-	BT_8192E_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
-	BT_8192E_2ANT_COEX_ALGO_SCO			= 0x1,
-	BT_8192E_2ANT_COEX_ALGO_SCO_PAN			= 0x2,
-	BT_8192E_2ANT_COEX_ALGO_HID			= 0x3,
-	BT_8192E_2ANT_COEX_ALGO_A2DP			= 0x4,
-	BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS		= 0x5,
-	BT_8192E_2ANT_COEX_ALGO_PANEDR			= 0x6,
-	BT_8192E_2ANT_COEX_ALGO_PANHS			= 0x7,
-	BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x8,
-	BT_8192E_2ANT_COEX_ALGO_PANEDR_HID		= 0x9,
-	BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0xa,
-	BT_8192E_2ANT_COEX_ALGO_HID_A2DP		= 0xb,
-	BT_8192E_2ANT_COEX_ALGO_MAX			= 0xc
-};
-
-struct coex_dm_8192e_2ant {
-	/* fw mechanism */
-	u8 pre_dec_bt_pwr;
-	u8 cur_dec_bt_pwr;
-	u8 pre_fw_dac_swing_lvl;
-	u8 cur_fw_dac_swing_lvl;
-	bool cur_ignore_wlan_act;
-	bool pre_ignore_wlan_act;
-	u8 pre_ps_tdma;
-	u8 cur_ps_tdma;
-	u8 ps_tdma_para[5];
-	u8 ps_tdma_du_adj_type;
-	bool reset_tdma_adjust;
-	bool auto_tdma_adjust;
-	bool pre_ps_tdma_on;
-	bool cur_ps_tdma_on;
-	bool pre_bt_auto_report;
-	bool cur_bt_auto_report;
-
-	/* sw mechanism */
-	bool pre_rf_rx_lpf_shrink;
-	bool cur_rf_rx_lpf_shrink;
-	u32 bt_rf0x1e_backup;
-	bool pre_low_penalty_ra;
-	bool cur_low_penalty_ra;
-	bool pre_dac_swing_on;
-	u32 pre_dac_swing_lvl;
-	bool cur_dac_swing_on;
-	u32 cur_dac_swing_lvl;
-	bool pre_adc_back_off;
-	bool cur_adc_back_off;
-	bool pre_agc_table_en;
-	bool cur_agc_table_en;
-	u32 pre_val0x6c0;
-	u32 cur_val0x6c0;
-	u32 pre_val0x6c4;
-	u32 cur_val0x6c4;
-	u32 pre_val0x6c8;
-	u32 cur_val0x6c8;
-	u8 pre_val0x6cc;
-	u8 cur_val0x6cc;
-	bool limited_dig;
-
-	u32 backup_arfr_cnt1;	/* Auto Rate Fallback Retry cnt */
-	u32 backup_arfr_cnt2;	/* Auto Rate Fallback Retry cnt */
-	u16 backup_retrylimit;
-	u8 backup_ampdu_maxtime;
-
-	/* algorithm related */
-	u8 pre_algorithm;
-	u8 cur_algorithm;
-	u8 bt_status;
-	u8 wifi_chnl_info[3];
-
-	u8 pre_sstype;
-	u8 cur_sstype;
-
-	u32 prera_mask;
-	u32 curra_mask;
-	u8 curra_masktype;
-	u8 pre_arfrtype;
-	u8 cur_arfrtype;
-	u8 pre_retrylimit_type;
-	u8 cur_retrylimit_type;
-	u8 pre_ampdutime_type;
-	u8 cur_ampdutime_type;
-};
-
-struct coex_sta_8192e_2ant {
-	bool bt_link_exist;
-	bool sco_exist;
-	bool a2dp_exist;
-	bool hid_exist;
-	bool pan_exist;
-
-	bool under_lps;
-	bool under_ips;
-	u32 high_priority_tx;
-	u32 high_priority_rx;
-	u32 low_priority_tx;
-	u32 low_priority_rx;
-	u8 bt_rssi;
-	u8 pre_bt_rssi_state;
-	u8 pre_wifi_rssi_state[4];
-	bool c2h_bt_info_req_sent;
-	u8 bt_info_c2h[BT_INFO_SRC_8192E_2ANT_MAX][10];
-	u32 bt_info_c2h_cnt[BT_INFO_SRC_8192E_2ANT_MAX];
-	bool c2h_bt_inquiry_page;
-	u8 bt_retry_cnt;
-	u8 bt_info_ext;
-};
-
-/****************************************************************
- *    The following is interface which will notify coex module.
- ****************************************************************/
-void ex_halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type);
-void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
-					       u8 type);
-void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h
deleted file mode 100644
index bded3738f24f..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b1ant.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/**********************************************************************
- * The following is for 8723B 1ANT BT Co-exist definition
- **********************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8723B_1ANT			1
-
-#define	BT_INFO_8723B_1ANT_B_FTP			BIT(7)
-#define	BT_INFO_8723B_1ANT_B_A2DP			BIT(6)
-#define	BT_INFO_8723B_1ANT_B_HID			BIT(5)
-#define	BT_INFO_8723B_1ANT_B_SCO_BUSY			BIT(4)
-#define	BT_INFO_8723B_1ANT_B_ACL_BUSY			BIT(3)
-#define	BT_INFO_8723B_1ANT_B_INQ_PAGE			BIT(2)
-#define	BT_INFO_8723B_1ANT_B_SCO_ESCO			BIT(1)
-#define	BT_INFO_8723B_1ANT_B_CONNECTION			BIT(0)
-
-#define	BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
-		(((_BT_INFO_EXT_&BIT(0))) ? true : false)
-
-#define	BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT		2
-
-enum BT_INFO_SRC_8723B_1ANT {
-	BT_INFO_SRC_8723B_1ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8723B_1ANT_BT_RSP			= 0x1,
-	BT_INFO_SRC_8723B_1ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8723B_1ANT_MAX
-};
-
-enum BT_8723B_1ANT_BT_STATUS {
-	BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
-	BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
-	BT_8723B_1ANT_BT_STATUS_INQ_PAGE		= 0x2,
-	BT_8723B_1ANT_BT_STATUS_ACL_BUSY		= 0x3,
-	BT_8723B_1ANT_BT_STATUS_SCO_BUSY		= 0x4,
-	BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
-	BT_8723B_1ANT_BT_STATUS_MAX
-};
-
-enum BT_8723B_1ANT_WIFI_STATUS {
-	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE			= 0x0,
-	BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN		= 0x1,
-	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN			= 0x2,
-	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT			= 0x3,
-	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE			= 0x4,
-	BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY			= 0x5,
-	BT_8723B_1ANT_WIFI_STATUS_MAX
-};
-
-enum BT_8723B_1ANT_COEX_ALGO {
-	BT_8723B_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
-	BT_8723B_1ANT_COEX_ALGO_SCO			= 0x1,
-	BT_8723B_1ANT_COEX_ALGO_HID			= 0x2,
-	BT_8723B_1ANT_COEX_ALGO_A2DP			= 0x3,
-	BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
-	BT_8723B_1ANT_COEX_ALGO_PANEDR			= 0x5,
-	BT_8723B_1ANT_COEX_ALGO_PANHS			= 0x6,
-	BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
-	BT_8723B_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
-	BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
-	BT_8723B_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
-	BT_8723B_1ANT_COEX_ALGO_MAX			= 0xb,
-};
-
-struct coex_dm_8723b_1ant {
-	/* fw mechanism */
-	bool cur_ignore_wlan_act;
-	bool pre_ignore_wlan_act;
-	u8 pre_ps_tdma;
-	u8 cur_ps_tdma;
-	u8 ps_tdma_para[5];
-	u8 ps_tdma_du_adj_type;
-	bool auto_tdma_adjust;
-	bool pre_ps_tdma_on;
-	bool cur_ps_tdma_on;
-	bool pre_bt_auto_report;
-	bool cur_bt_auto_report;
-	u8 pre_lps;
-	u8 cur_lps;
-	u8 pre_rpwm;
-	u8 cur_rpwm;
-
-	/* sw mechanism */
-	bool pre_low_penalty_ra;
-	bool cur_low_penalty_ra;
-	u32 pre_val0x6c0;
-	u32 cur_val0x6c0;
-	u32 pre_val0x6c4;
-	u32 cur_val0x6c4;
-	u32 pre_val0x6c8;
-	u32 cur_val0x6c8;
-	u8 pre_val0x6cc;
-	u8 cur_val0x6cc;
-	bool limited_dig;
-
-	u32 backup_arfr_cnt1;	/* Auto Rate Fallback Retry cnt */
-	u32 backup_arfr_cnt2;	/* Auto Rate Fallback Retry cnt */
-	u16 backup_retry_limit;
-	u8 backup_ampdu_max_time;
-
-	/* algorithm related */
-	u8 pre_algorithm;
-	u8 cur_algorithm;
-	u8 bt_status;
-	u8 wifi_chnl_info[3];
-
-	u32 prera_mask;
-	u32 curra_mask;
-	u8 pre_arfr_type;
-	u8 cur_arfr_type;
-	u8 pre_retry_limit_type;
-	u8 cur_retry_limit_type;
-	u8 pre_ampdu_time_type;
-	u8 cur_ampdu_time_type;
-
-	u8 error_condition;
-};
-
-struct coex_sta_8723b_1ant {
-	bool bt_link_exist;
-	bool sco_exist;
-	bool a2dp_exist;
-	bool hid_exist;
-	bool pan_exist;
-
-	bool under_lps;
-	bool under_ips;
-	u32 special_pkt_period_cnt;
-	u32 high_priority_tx;
-	u32 high_priority_rx;
-	u32 low_priority_tx;
-	u32 low_priority_rx;
-	u8 bt_rssi;
-	u8 pre_bt_rssi_state;
-	u8 pre_wifi_rssi_state[4];
-	bool c2h_bt_info_req_sent;
-	u8 bt_info_c2h[BT_INFO_SRC_8723B_1ANT_MAX][10];
-	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_1ANT_MAX];
-	bool c2h_bt_inquiry_page;
-	u8 bt_retry_cnt;
-	u8 bt_info_ext;
-};
-
-/*************************************************************************
- * The following is interface which will notify coex module.
- *************************************************************************/
-void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type);
-void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 state);
-void ex_halbtc8723b1ant_coex_dm_reset(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.c b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.c
deleted file mode 100644
index 836206357e68..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.c
+++ /dev/null
@@ -1,3929 +0,0 @@
-/***************************************************************
- * Description:
- *
- * This file is for RTL8723B Co-exist mechanism
- *
- * History
- * 2012/11/15 Cosa first check in.
- *
- **************************************************************/
-/**************************************************************
- * include files
- **************************************************************/
-#include "halbt_precomp.h"
-/**************************************************************
- * Global variables, these are static variables
- **************************************************************/
-static struct coex_dm_8723b_2ant glcoex_dm_8723b_2ant;
-static struct coex_dm_8723b_2ant *coex_dm = &glcoex_dm_8723b_2ant;
-static struct coex_sta_8723b_2ant glcoex_sta_8723b_2ant;
-static struct coex_sta_8723b_2ant *coex_sta = &glcoex_sta_8723b_2ant;
-
-static const char *const glbt_info_src_8723b_2ant[] = {
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-static u32 glcoex_ver_date_8723b_2ant = 20131113;
-static u32 glcoex_ver_8723b_2ant = 0x3f;
-
-/**************************************************************
- * local function proto type if needed
- **************************************************************/
-/**************************************************************
- * local function start with halbtc8723b2ant_
- **************************************************************/
-static u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
-					u8 rssi_thresh1)
-{
-	s32 bt_rssi = 0;
-	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
-
-	bt_rssi = coex_sta->bt_rssi;
-
-	if (level_num == 2) {
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= rssi_thresh +
-				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state switch to High\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state stay at Low\n");
-			}
-		} else {
-			if (bt_rssi < rssi_thresh) {
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state switch to Low\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "[BTCoex], BT Rssi thresh error!!\n");
-			return coex_sta->pre_bt_rssi_state;
-		}
-
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= rssi_thresh +
-				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state switch to Medium\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state stay at Low\n");
-			}
-		} else if ((coex_sta->pre_bt_rssi_state ==
-						BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_bt_rssi_state ==
-						BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (bt_rssi >= rssi_thresh1 +
-				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state switch to High\n");
-			} else if (bt_rssi < rssi_thresh) {
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state switch to Low\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state stay at Medium\n");
-			}
-		} else {
-			if (bt_rssi < rssi_thresh1) {
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state switch to Medium\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_bt_rssi_state = bt_rssi_state;
-
-	return bt_rssi_state;
-}
-
-static u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
-					  u8 index, u8 level_num,
-					  u8 rssi_thresh, u8 rssi_thresh1)
-{
-	s32 wifi_rssi = 0;
-	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-
-	if (level_num == 2) {
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= rssi_thresh +
-					 BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state switch to High\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state stay at Low\n");
-			}
-		} else {
-			if (wifi_rssi < rssi_thresh) {
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state switch to Low\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
-				  "[BTCoex], wifi RSSI thresh error!!\n");
-			return coex_sta->pre_wifi_rssi_state[index];
-		}
-
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= rssi_thresh +
-					BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state switch to Medium\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state stay at Low\n");
-			}
-		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (wifi_rssi >= rssi_thresh1 +
-					 BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state switch to High\n");
-			} else if (wifi_rssi < rssi_thresh) {
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state switch to Low\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state stay at Medium\n");
-			}
-		} else {
-			if (wifi_rssi < rssi_thresh1) {
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state switch to Medium\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
-
-	return wifi_rssi_state;
-}
-
-static void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
-{
-	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
-	u32 reg_hp_tx = 0, reg_hp_rx = 0;
-	u32 reg_lp_tx = 0, reg_lp_rx = 0;
-
-	reg_hp_txrx = 0x770;
-	reg_lp_txrx = 0x774;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
-	reg_hp_tx = u32tmp & MASKLWORD;
-	reg_hp_rx = (u32tmp & MASKHWORD) >> 16;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
-	reg_lp_tx = u32tmp & MASKLWORD;
-	reg_lp_rx = (u32tmp & MASKHWORD) >> 16;
-
-	coex_sta->high_priority_tx = reg_hp_tx;
-	coex_sta->high_priority_rx = reg_hp_rx;
-	coex_sta->low_priority_tx = reg_lp_tx;
-	coex_sta->low_priority_rx = reg_lp_rx;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex], High Priority Tx/Rx(reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
-		  reg_hp_txrx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex], Low Priority Tx/Rx(reg 0x%x) = 0x%x(%d)/0x%x(%d)\n",
-		  reg_lp_txrx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
-
-	/* reset counter */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-}
-
-static void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
-{
-	u8 h2c_parameter[1] = {0};
-
-	coex_sta->c2h_bt_info_req_sent = true;
-
-	h2c_parameter[0] |= BIT(0);	/* trigger */
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Query Bt Info, FW write 0x61 = 0x%x\n",
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
-}
-
-static bool is_wifi_status_changed(struct btc_coexist *btcoexist)
-{
-	static bool pre_wifi_busy;
-	static bool pre_under_4way;
-	static bool pre_bt_hs_on;
-	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
-	bool wifi_connected = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
-			   &under_4way);
-
-	if (wifi_connected) {
-		if (wifi_busy != pre_wifi_busy) {
-			pre_wifi_busy = wifi_busy;
-			return true;
-		}
-
-		if (under_4way != pre_under_4way) {
-			pre_under_4way = under_4way;
-			return true;
-		}
-
-		if (bt_hs_on != pre_bt_hs_on) {
-			pre_bt_hs_on = bt_hs_on;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-static void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
-{
-	/*struct btc_stack_info *stack_info = &btcoexist->stack_info;*/
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hs_on = false;
-
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 1) /* profile from bt patch */
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-
-	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
-	bt_link_info->sco_exist = coex_sta->sco_exist;
-	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
-	bt_link_info->pan_exist = coex_sta->pan_exist;
-	bt_link_info->hid_exist = coex_sta->hid_exist;
-
-	/* work around for HS mode. */
-	if (bt_hs_on) {
-		bt_link_info->pan_exist = true;
-		bt_link_info->bt_link_exist = true;
-	}
-#else	/* profile from bt stack */
-	bt_link_info->bt_link_exist = stack_info->bt_link_exist;
-	bt_link_info->sco_exist = stack_info->sco_exist;
-	bt_link_info->a2dp_exist = stack_info->a2dp_exist;
-	bt_link_info->pan_exist = stack_info->pan_exist;
-	bt_link_info->hid_exist = stack_info->hid_exist;
-
-	/*for win-8 stack HID report error*/
-	if (!stack_info->hid_exist)
-		stack_info->hid_exist = coex_sta->hid_exist;
-	/*sync  BTInfo with BT firmware and stack*/
-	/* when stack HID report error, here we use the info from bt fw.*/
-	if (!stack_info->bt_link_exist)
-		stack_info->bt_link_exist = coex_sta->bt_link_exist;
-#endif
-	/* check if Sco only */
-	if (bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->sco_only = true;
-	else
-		bt_link_info->sco_only = false;
-
-	/* check if A2dp only */
-	if (!bt_link_info->sco_exist && bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->a2dp_only = true;
-	else
-		bt_link_info->a2dp_only = false;
-
-	/* check if Pan only */
-	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->pan_only = true;
-	else
-		bt_link_info->pan_only = false;
-
-	/* check if Hid only */
-	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && bt_link_info->hid_exist)
-		bt_link_info->hid_only = true;
-	else
-		bt_link_info->hid_only = false;
-}
-
-static u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hs_on = false;
-	u8 algorithm = BT_8723B_2ANT_COEX_ALGO_UNDEFINED;
-	u8 num_of_diff_profile = 0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-
-	if (!bt_link_info->bt_link_exist) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], No BT link exists!!!\n");
-		return algorithm;
-	}
-
-	if (bt_link_info->sco_exist)
-		num_of_diff_profile++;
-	if (bt_link_info->hid_exist)
-		num_of_diff_profile++;
-	if (bt_link_info->pan_exist)
-		num_of_diff_profile++;
-	if (bt_link_info->a2dp_exist)
-		num_of_diff_profile++;
-
-	if (num_of_diff_profile == 1) {
-		if (bt_link_info->sco_exist) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], SCO only\n");
-			algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
-		} else {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], HID only\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], A2DP only\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], PAN(HS) only\n");
-					algorithm =
-						BT_8723B_2ANT_COEX_ALGO_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], PAN(EDR) only\n");
-					algorithm =
-						BT_8723B_2ANT_COEX_ALGO_PANEDR;
-				}
-			}
-		}
-	} else if (num_of_diff_profile == 2) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], SCO + HID\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], SCO + A2DP ==> SCO\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + PAN(HS)\n");
-					algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], HID + A2DP\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + PAN(HS)\n");
-					algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], A2DP + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP;
-				}
-			}
-		}
-	} else if (num_of_diff_profile == 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], SCO + HID + A2DP ==> HID\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + HID + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + HID + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + A2DP + PAN(EDR)\n");
-					algorithm =
-					BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				}
-			}
-		}
-	} else if (num_of_diff_profile >= 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n");
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + HID + A2DP + PAN(EDR) ==>PAN(EDR)+HID\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-	}
-
-	return algorithm;
-}
-
-static bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
-{
-	bool ret = false;
-	bool bt_hs_on = false, wifi_connected = false;
-	s32 bt_hs_rssi = 0;
-	u8 bt_rssi_state;
-
-	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION,
-				&bt_hs_on))
-		return false;
-	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-				&wifi_connected))
-		return false;
-	if (!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI,
-				&bt_hs_rssi))
-		return false;
-
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	if (wifi_connected) {
-		if (bt_hs_on) {
-			if (bt_hs_rssi > 37) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-					  "[BTCoex], Need to decrease bt power for HS mode!!\n");
-				ret = true;
-			}
-		} else {
-			if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-					  "[BTCoex], Need to decrease bt power for Wifi is connected!!\n");
-				ret = true;
-			}
-		}
-	}
-
-	return ret;
-}
-
-static void set_fw_dac_swing_level(struct btc_coexist *btcoexist,
-				   u8 dac_swing_lvl)
-{
-	u8 h2c_parameter[1] = {0};
-
-	/* There are several type of dacswing
-	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
-	h2c_parameter[0] = dac_swing_lvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Set Dac Swing Level = 0x%x\n", dac_swing_lvl);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x64 = 0x%x\n", h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
-}
-
-static void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
-					      bool dec_bt_pwr)
-{
-	u8 h2c_parameter[1] = {0};
-
-	h2c_parameter[0] = 0;
-
-	if (dec_bt_pwr)
-		h2c_parameter[0] |= BIT(1);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], decrease Bt Power : %s, FW write 0x62 = 0x%x\n",
-		  (dec_bt_pwr ? "Yes!!" : "No!!"), h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
-}
-
-static void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
-				       bool force_exec, bool dec_bt_pwr)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s Dec BT power = %s\n",
-		  (force_exec ? "force to" : ""), (dec_bt_pwr ? "ON" : "OFF"));
-	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreDecBtPwr =%d, bCurDecBtPwr =%d\n",
-			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
-
-		if (coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr)
-			return;
-	}
-	halbtc8723b2ant_set_fw_dec_bt_pwr(btcoexist, coex_dm->cur_dec_bt_pwr);
-
-	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
-}
-
-static void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
-					     bool force_exec,
-					     u8 fw_dac_swing_lvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s set FW Dac Swing level = %d\n",
-		  (force_exec ? "force to" : ""), fw_dac_swing_lvl);
-	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], preFwDacSwingLvl =%d, curFwDacSwingLvl =%d\n",
-			  coex_dm->pre_fw_dac_swing_lvl,
-			  coex_dm->cur_fw_dac_swing_lvl);
-
-		if (coex_dm->pre_fw_dac_swing_lvl ==
-		   coex_dm->cur_fw_dac_swing_lvl)
-			return;
-	}
-
-	set_fw_dac_swing_level(btcoexist, coex_dm->cur_fw_dac_swing_lvl);
-	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
-}
-
-static void set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-				    bool rx_rf_shrink_on)
-{
-	if (rx_rf_shrink_on) {
-		/* Shrink RF Rx LPF corner */
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-					  0xfffff, 0xffffc);
-	} else {
-		/* Resume RF Rx LPF corner */
-		/* After initialized, we can use coex_dm->btRf0x1eBackup */
-		if (btcoexist->initilized) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-				  "[BTCoex], Resume RF Rx LPF corner!!\n");
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-						  0xfffff,
-						  coex_dm->bt_rf0x1e_backup);
-		}
-	}
-}
-
-static void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
-				      bool force_exec, bool rx_rf_shrink_on)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
-		  (force_exec ? "force to" : ""),
-		  (rx_rf_shrink_on ? "ON" : "OFF"));
-	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreRfRxLpfShrink =%d, bCurRfRxLpfShrink =%d\n",
-			  coex_dm->pre_rf_rx_lpf_shrink,
-			  coex_dm->cur_rf_rx_lpf_shrink);
-
-		if (coex_dm->pre_rf_rx_lpf_shrink ==
-		    coex_dm->cur_rf_rx_lpf_shrink)
-			return;
-	}
-	set_sw_rf_rx_lpf_corner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
-
-	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
-}
-
-static void set_sw_penalty_txrate_adaptive(
-						struct btc_coexist *btcoexist,
-						bool low_penalty_ra)
-{
-	u8 h2c_parameter[6] = {0};
-
-	h2c_parameter[0] = 0x6;	/* opCode, 0x6 = Retry_Penalty*/
-
-	if (low_penalty_ra) {
-		h2c_parameter[1] |= BIT(0);
-		/*normal rate except MCS7/6/5, OFDM54/48/36*/
-		h2c_parameter[2] = 0x00;
-		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54*/
-		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48*/
-		h2c_parameter[5] = 0xf9;  /*MCS5 or OFDM36*/
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
-		  (low_penalty_ra ? "ON!!" : "OFF!!"));
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
-}
-
-static void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
-					   bool force_exec, bool low_penalty_ra)
-{
-	/*return; */
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn LowPenaltyRA = %s\n",
-		  (force_exec ? "force to" : ""),
-		  (low_penalty_ra ? "ON" : "OFF"));
-	coex_dm->cur_low_penalty_ra = low_penalty_ra;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreLowPenaltyRa =%d, bCurLowPenaltyRa =%d\n",
-			  coex_dm->pre_low_penalty_ra,
-			  coex_dm->cur_low_penalty_ra);
-
-		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
-			return;
-	}
-	set_sw_penalty_txrate_adaptive(btcoexist, coex_dm->cur_low_penalty_ra);
-
-	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
-}
-
-static void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist *btcoexist,
-					      u32 level)
-{
-	u8 val = (u8) level;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], Write SwDacSwing = 0x%x\n", level);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
-}
-
-static void set_sw_fulltime_dac_swing(struct btc_coexist *btcoexist,
-				      bool sw_dac_swing_on,
-				      u32 sw_dac_swing_lvl)
-{
-	if (sw_dac_swing_on)
-		halbtc8723b2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
-	else
-		halbtc8723b2ant_set_dac_swing_reg(btcoexist, 0x18);
-}
-
-static void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
-				      bool force_exec, bool dac_swing_on,
-			       u32 dac_swing_lvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn DacSwing =%s, dac_swing_lvl = 0x%x\n",
-		  (force_exec ? "force to" : ""),
-		  (dac_swing_on ? "ON" : "OFF"), dac_swing_lvl);
-	coex_dm->cur_dac_swing_on = dac_swing_on;
-	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreDacSwingOn =%d, preDacSwingLvl = 0x%x, bCurDacSwingOn =%d, curDacSwingLvl = 0x%x\n",
-			  coex_dm->pre_dac_swing_on, coex_dm->pre_dac_swing_lvl,
-			  coex_dm->cur_dac_swing_on,
-			  coex_dm->cur_dac_swing_lvl);
-
-		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
-		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
-			return;
-	}
-	mdelay(30);
-	set_sw_fulltime_dac_swing(btcoexist, dac_swing_on,
-				  dac_swing_lvl);
-
-	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
-	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
-}
-
-static void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
-					  bool agc_table_en)
-{
-	u8 rssi_adjust_val = 0;
-
-	/*  BB AGC Gain Table */
-	if (agc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB Agc Table On!\n");
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6e1A0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6d1B0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6c1C0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6b1D0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6a1E0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x691F0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x68200001);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB Agc Table Off!\n");
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xaa1A0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa91B0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa81C0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa71D0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa61E0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa51F0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa4200001);
-	}
-
-	/* RF Gain */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
-	if (agc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table On!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x38fff);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x38ffe);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table Off!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x380c3);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x28ce6);
-	}
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xed, 0xfffff, 0x1);
-
-	if (agc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table On!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x38fff);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x38ffe);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table Off!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x380c3);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x28ce6);
-	}
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xed, 0xfffff, 0x0);
-
-	/* set rssiAdjustVal for wifi module. */
-	if (agc_table_en)
-		rssi_adjust_val = 8;
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
-			   &rssi_adjust_val);
-}
-
-static void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
-				      bool force_exec, bool agc_table_en)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s %s Agc Table\n",
-		  (force_exec ? "force to" : ""),
-		  (agc_table_en ? "Enable" : "Disable"));
-	coex_dm->cur_agc_table_en = agc_table_en;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAgcTableEn =%d, bCurAgcTableEn =%d\n",
-			  coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en);
-
-		if (coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en)
-			return;
-	}
-	halbtc8723b2ant_set_agc_table(btcoexist, agc_table_en);
-
-	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
-}
-
-static void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
-					   u32 val0x6c0, u32 val0x6c4,
-					   u32 val0x6c8, u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c0 = 0x%x\n", val0x6c0);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c4 = 0x%x\n", val0x6c4);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c8 = 0x%x\n", val0x6c8);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6cc = 0x%x\n", val0x6cc);
-	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
-}
-
-static void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
-				       bool force_exec, u32 val0x6c0,
-				       u32 val0x6c4, u32 val0x6c8,
-				       u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s write Coex Table 0x6c0 = 0x%x, 0x6c4 = 0x%x, 0x6c8 = 0x%x, 0x6cc = 0x%x\n",
-		  (force_exec ? "force to" : ""), val0x6c0,
-		  val0x6c4, val0x6c8, val0x6cc);
-	coex_dm->cur_val0x6c0 = val0x6c0;
-	coex_dm->cur_val0x6c4 = val0x6c4;
-	coex_dm->cur_val0x6c8 = val0x6c8;
-	coex_dm->cur_val0x6cc = val0x6cc;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], preVal0x6c0 = 0x%x, preVal0x6c4 = 0x%x, preVal0x6c8 = 0x%x, preVal0x6cc = 0x%x !!\n",
-			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4,
-			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], curVal0x6c0 = 0x%x, curVal0x6c4 = 0x%x, curVal0x6c8 = 0x%x, curVal0x6cc = 0x%x !!\n",
-			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4,
-			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
-
-		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
-		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
-		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
-		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
-			return;
-	}
-	halbtc8723b2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
-				       val0x6c8, val0x6cc);
-
-	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
-	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
-	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
-	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
-}
-
-static void coex_table_with_type(struct btc_coexist *btcoexist,
-						 bool force_exec, u8 type)
-{
-	switch (type) {
-	case 0:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x55555555, 0xffff, 0x3);
-		break;
-	case 1:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x5afa5afa, 0xffff, 0x3);
-		break;
-	case 2:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
-					   0x5a5a5a5a, 0xffff, 0x3);
-		break;
-	case 3:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xaaaaaaaa,
-					   0xaaaaaaaa, 0xffff, 0x3);
-		break;
-	case 4:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xffffffff,
-					   0xffffffff, 0xffff, 0x3);
-		break;
-	case 5:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
-					   0x5fff5fff, 0xffff, 0x3);
-		break;
-	case 6:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5a5a5a5a, 0xffff, 0x3);
-		break;
-	case 7:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5afa5afa, 0xffff, 0x3);
-		break;
-	case 8:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5aea5aea,
-					   0x5aea5aea, 0xffff, 0x3);
-		break;
-	case 9:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5aea5aea, 0xffff, 0x3);
-		break;
-	case 10:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5aff5aff, 0xffff, 0x3);
-		break;
-	case 11:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5a5f5a5f, 0xffff, 0x3);
-		break;
-	case 12:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5f5f5f5f, 0xffff, 0x3);
-		break;
-	default:
-		break;
-	}
-}
-
-static void set_fw_ignore_wlan_act(struct btc_coexist *btcoexist,
-				   bool enable)
-{
-	u8 h2c_parameter[1] = {0};
-
-	if (enable)
-		h2c_parameter[0] |= BIT(0);/* function enable*/
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63 = 0x%x\n",
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
-}
-
-static void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
-					    bool force_exec, bool enable)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn Ignore WlanAct %s\n",
-		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
-	coex_dm->cur_ignore_wlan_act = enable;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n",
-			  coex_dm->pre_ignore_wlan_act,
-			  coex_dm->cur_ignore_wlan_act);
-
-		if (coex_dm->pre_ignore_wlan_act ==
-		    coex_dm->cur_ignore_wlan_act)
-			return;
-	}
-	set_fw_ignore_wlan_act(btcoexist, enable);
-
-	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
-}
-
-static void set_fw_ps_tdma(struct btc_coexist *btcoexist, u8 byte1,
-			   u8 byte2, u8 byte3, u8 byte4, u8 byte5)
-{
-	u8 h2c_parameter[5] = {0};
-
-	h2c_parameter[0] = byte1;
-	h2c_parameter[1] = byte2;
-	h2c_parameter[2] = byte3;
-	h2c_parameter[3] = byte4;
-	h2c_parameter[4] = byte5;
-
-	coex_dm->ps_tdma_para[0] = byte1;
-	coex_dm->ps_tdma_para[1] = byte2;
-	coex_dm->ps_tdma_para[2] = byte3;
-	coex_dm->ps_tdma_para[3] = byte4;
-	coex_dm->ps_tdma_para[4] = byte5;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x60(5bytes) = 0x%x%08x\n",
-		  h2c_parameter[0],
-		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
-		  h2c_parameter[3] << 8 | h2c_parameter[4]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
-}
-
-static void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
-					  bool shrink_rx_lpf,
-					  bool low_penalty_ra,
-					  bool limited_dig,
-					  bool bt_lna_constrain)
-{
-	halbtc8723b2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
-	halbtc8723b2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
-}
-
-static void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
-					  bool agc_table_shift,
-					  bool adc_backoff,
-					  bool sw_dac_swing, u32 dac_swing_lvl)
-{
-	halbtc8723b2ant_agc_table(btcoexist, NORMAL_EXEC, agc_table_shift);
-	halbtc8723b2ant_dac_swing(btcoexist, NORMAL_EXEC, sw_dac_swing,
-				  dac_swing_lvl);
-}
-
-static void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
-					 u8 antpos_type, bool init_hwcfg,
-					 bool wifi_off)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	u32 fw_ver = 0, u32tmp = 0;
-	bool pg_ext_switch = false;
-	bool use_ext_switch = false;
-	u8 h2c_parameter[2] = {0};
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-
-	if ((fw_ver < 0xc0000) || pg_ext_switch)
-		use_ext_switch = true;
-
-	if (init_hwcfg) {
-		/* 0x4c[23] = 0, 0x4c[24] = 1  Antenna control by WL/BT */
-		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u32tmp &= ~BIT(23);
-		u32tmp |= BIT(24);
-		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
-
-		btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
-		btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
-
-		/* Force GNT_BT to low */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
-		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-
-		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
-			/* tell firmware "no antenna inverse" */
-			h2c_parameter[0] = 0;
-			h2c_parameter[1] = 1;  /* ext switch type */
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		} else {
-			/* tell firmware "antenna inverse" */
-			h2c_parameter[0] = 1;
-			h2c_parameter[1] = 1;  /* ext switch type */
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		}
-	}
-
-	/* ext switch setting */
-	if (use_ext_switch) {
-		/* fixed internal switch S1->WiFi, S0->BT */
-		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-		switch (antpos_type) {
-		case BTC_ANT_WIFI_AT_MAIN:
-			/* ext switch main at wifi */
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x1);
-			break;
-		case BTC_ANT_WIFI_AT_AUX:
-			/* ext switch aux at wifi */
-			btcoexist->btc_write_1byte_bitmask(btcoexist,
-							   0x92c, 0x3, 0x2);
-			break;
-		}
-	} else {	/* internal switch */
-		/* fixed ext switch */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c, 0x3, 0x1);
-		switch (antpos_type) {
-		case BTC_ANT_WIFI_AT_MAIN:
-			/* fixed internal switch S1->WiFi, S0->BT */
-			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-			break;
-		case BTC_ANT_WIFI_AT_AUX:
-			/* fixed internal switch S0->WiFi, S1->BT */
-			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
-			break;
-		}
-	}
-}
-
-static void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist,
-				    bool force_exec, bool turn_on, u8 type)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn %s PS TDMA, type =%d\n",
-		  (force_exec ? "force to" : ""),
-		  (turn_on ? "ON" : "OFF"), type);
-	coex_dm->cur_ps_tdma_on = turn_on;
-	coex_dm->cur_ps_tdma = type;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
-			  coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
-			  coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma);
-
-		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
-		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
-			return;
-	}
-	if (turn_on) {
-		switch (type) {
-		case 1:
-		default:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
-			break;
-		case 2:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
-			break;
-		case 3:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x1c, 0x3, 0xf1, 0x90);
-			break;
-		case 4:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x10, 0x03, 0xf1, 0x90);
-			break;
-		case 5:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
-			break;
-		case 6:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
-			break;
-		case 7:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x1c, 0x3, 0x70, 0x90);
-			break;
-		case 8:
-			set_fw_ps_tdma(btcoexist, 0xa3, 0x10, 0x3, 0x70, 0x90);
-			break;
-		case 9:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
-			break;
-		case 10:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
-			break;
-		case 11:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0xa, 0xa, 0xe1, 0x90);
-			break;
-		case 12:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
-			break;
-		case 13:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x90);
-			break;
-		case 14:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x12, 0x12, 0x60, 0x90);
-			break;
-		case 15:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0xa, 0xa, 0x60, 0x90);
-			break;
-		case 16:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x5, 0x5, 0x60, 0x90);
-			break;
-		case 17:
-			set_fw_ps_tdma(btcoexist, 0xa3, 0x2f, 0x2f, 0x60, 0x90);
-			break;
-		case 18:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x5, 0x5, 0xe1, 0x90);
-			break;
-		case 19:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x25, 0x25, 0xe1, 0x90);
-			break;
-		case 20:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x25, 0x25, 0x60, 0x90);
-			break;
-		case 21:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x15, 0x03, 0x70, 0x90);
-			break;
-		case 71:
-			set_fw_ps_tdma(btcoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x90);
-			break;
-		}
-	} else {
-		/* disable PS tdma */
-		switch (type) {
-		case 0:
-			set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
-			break;
-		case 1:
-			set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0, 0x48, 0x0);
-			break;
-		default:
-			set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0, 0x40, 0x0);
-			break;
-		}
-	}
-
-	/* update pre state */
-	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
-	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
-}
-
-static void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
-{
-	/* fw all off */
-	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	/* sw all off */
-	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-
-	/* hw all off */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-}
-
-static void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	/* force to reset coex mechanism*/
-
-	halbtc8723b2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
-	halbtc8723b2ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
-
-	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-}
-
-static void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
-{
-	bool wifi_connected = false;
-	bool low_pwr_disable = true;
-
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-			   &low_pwr_disable);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-
-	if (wifi_connected) {
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
-	} else {
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-	}
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
-	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-
-	coex_dm->need_recover_0x948 = true;
-	coex_dm->backup_0x948 = btcoexist->btc_read_2byte(btcoexist, 0x948);
-
-	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_AUX,
-				     false, false);
-}
-
-static bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
-{
-	bool common = false, wifi_connected = false;
-	bool wifi_busy = false;
-	bool bt_hs_on = false, low_pwr_disable = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-
-	if (!wifi_connected) {
-		low_pwr_disable = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-				   &low_pwr_disable);
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi non-connected idle!!\n");
-
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
-					  0x0);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-		halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false,
-					      false);
-		halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false,
-					      0x18);
-
-		common = true;
-	} else {
-		if (BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
-		    coex_dm->bt_status) {
-			low_pwr_disable = false;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Wifi connected + BT non connected-idle!!\n");
-
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x0);
-			coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 0);
-			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						1);
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 0xb);
-			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-
-			common = true;
-		} else if (BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE ==
-			   coex_dm->bt_status) {
-			low_pwr_disable = true;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			if (bt_hs_on)
-				return false;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Wifi connected + BT connected-idle!!\n");
-
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x0);
-			coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 0);
-			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						1);
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 0xb);
-			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-
-			common = true;
-		} else {
-			low_pwr_disable = true;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			if (wifi_busy) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], Wifi Connected-Busy + BT Busy!!\n");
-				common = false;
-			} else {
-				if (bt_hs_on)
-					return false;
-
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], Wifi Connected-Idle + BT Busy!!\n");
-
-				btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A,
-							  0x1, 0xfffff, 0x0);
-				coex_table_with_type(btcoexist,
-					    NORMAL_EXEC, 7);
-				halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 21);
-				halbtc8723b2ant_fw_dac_swing_lvl(btcoexist,
-								 NORMAL_EXEC,
-								 0xb);
-				if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-					halbtc8723b2ant_dec_bt_pwr(btcoexist,
-								   NORMAL_EXEC,
-								   true);
-				else
-					halbtc8723b2ant_dec_bt_pwr(btcoexist,
-								   NORMAL_EXEC,
-								   false);
-				halbtc8723b2ant_sw_mechanism1(btcoexist, false,
-							      false, false,
-							      false);
-				halbtc8723b2ant_sw_mechanism2(btcoexist, false,
-							      false, false,
-							      0x18);
-				common = true;
-			}
-		}
-	}
-
-	return common;
-}
-static void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
-						 bool sco_hid, bool tx_pause,
-						 u8 max_interval)
-{
-	static s32 up, dn, m, n, wait_count;
-	/*0: no change, +1: increase WiFi duration, -1: decrease WiFi duration*/
-	s32 result;
-	u8 retrycount = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], TdmaDurationAdjust()\n");
-
-	if (!coex_dm->auto_tdma_adjust) {
-		coex_dm->auto_tdma_adjust = true;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
-		if (sco_hid) {
-			if (tx_pause) {
-				if (max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				} else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				}
-			} else {
-				if (max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				}
-			}
-		} else {
-			if (tx_pause) {
-				if (max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				}
-			} else {
-				if (max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 1);
-					coex_dm->ps_tdma_du_adj_type = 1;
-				} else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				}
-			}
-		}
-
-		up = 0;
-		dn = 0;
-		m = 1;
-		n = 3;
-		result = 0;
-		wait_count = 0;
-	} else {
-		/*accquire the BT TRx retry count from BT_Info byte2*/
-		retrycount = coex_sta->bt_retry_cnt;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], retrycount = %d\n", retrycount);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], up =%d, dn =%d, m =%d, n =%d, wait_count =%d\n",
-			  up, dn, m, n, wait_count);
-		result = 0;
-		wait_count++;
-		 /* no retry in the last 2-second duration*/
-		if (retrycount == 0) {
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0;
-
-			if (up >= n) {
-				wait_count = 0;
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], Increase wifi duration!!\n");
-			} /* <= 3 retry in the last 2-second duration*/
-		} else if (retrycount <= 3) {
-			up--;
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2) {
-				if (wait_count <= 2)
-					m++;
-				else
-					m = 1;
-
-				if (m >= 20)
-					m = 20;
-
-				n = 3 * m;
-				up = 0;
-				dn = 0;
-				wait_count = 0;
-				result = -1;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], Decrease wifi duration for retrycounter<3!!\n");
-			}
-		} else {
-			if (wait_count == 1)
-				m++;
-			else
-				m = 1;
-
-			if (m >= 20)
-				m = 20;
-
-			n = 3 * m;
-			up = 0;
-			dn = 0;
-			wait_count = 0;
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], Decrease wifi duration for retrycounter>3!!\n");
-		}
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], max Interval = %d\n", max_interval);
-		if (max_interval == 1) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-
-				if (coex_dm->cur_ps_tdma == 71) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									      8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-						coex_dm->ps_tdma_du_adj_type =
-									      5;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-						coex_dm->ps_tdma_du_adj_type =
-									     13;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 71);
-					coex_dm->ps_tdma_du_adj_type = 71;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 71) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									      1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									      4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									      1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 71);
-						coex_dm->ps_tdma_du_adj_type =
-									     71;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-						coex_dm->ps_tdma_du_adj_type =
-									      9;
-					}
-				}
-			}
-		} else if (max_interval == 2) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									      8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									      4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					}
-				}
-			}
-		} else if (max_interval == 3) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									      8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									      4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					}
-				}
-			}
-		}
-	}
-
-	/*if current PsTdma not match with the recorded one (when scan, dhcp..),
-	 *then we have to adjust it back to the previous record one.*/
-	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
-		bool scan = false, link = false, roam = false;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], PsTdma type dismatch!!!, curPsTdma =%d, recordPsTdma =%d\n",
-			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
-
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-
-		if (!scan && !link && !roam)
-			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
-						coex_dm->ps_tdma_du_adj_type);
-		else
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n");
-	}
-}
-
-/* SCO only or SCO+PAN(HS) */
-static void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	/*for SCO quality at 11b/g mode*/
-	if (BTC_WIFI_BW_LEGACY == wifi_bw)
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-	else  /*for SCO quality & wifi performance balance at 11n mode*/
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 8);
-
-	/*for voice quality */
-	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x4);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      true, 0x4);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x4);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      true, 0x4);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /*/for HID at 11b/g mode*/
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-	else  /*for HID quality & wifi performance balance at 11n mode*/
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 9);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-	else
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
-static void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
-	u32 wifi_bw;
-	u8 ap_num = 0;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	wifi_rssi_state1 = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							   1, 2, 40, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
-
-	/* define the office environment */
-	/* driver don't know AP num in Linux, so we will never enter this if */
-	if (ap_num >= 10 && BTC_RSSI_HIGH(wifi_rssi_state1)) {
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
-					  0x0);
-		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-
-		/* sw mechanism */
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-		if (BTC_WIFI_BW_HT40 == wifi_bw) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x18);
-		}
-		return;
-	}
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, false, 1);
-	else
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 1);
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 2);
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 10);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
-	else
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*PAN(HS) only*/
-static void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*PAN(EDR)+A2DP*/
-static void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 12);
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
-							     true, 3);
-		else
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
-							     false, 3);
-	} else {
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 3);
-	}
-
-	/* sw mechanism	*/
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		if (BTC_WIFI_BW_HT40 == wifi_bw) {
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 3);
-			coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 11);
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x780);
-		} else {
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 6);
-			coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 7);
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x0);
-		}
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
-	} else {
-		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		coex_table_with_type(btcoexist, NORMAL_EXEC, 11);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
-					  0x0);
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/* HID+A2DP+PAN(EDR) */
-static void halbtc8723b2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
-							     true, 2);
-		else
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
-							     false, 3);
-	} else {
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 3);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
-	else
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
-{
-	u8 algorithm = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], RunCoexistMechanism() ===>\n");
-
-	if (btcoexist->manual_control) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n");
-		return;
-	}
-
-	if (coex_sta->under_ips) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], wifi is under IPS !!!\n");
-		return;
-	}
-
-	algorithm = halbtc8723b2ant_action_algorithm(btcoexist);
-	if (coex_sta->c2h_bt_inquiry_page &&
-	    (BT_8723B_2ANT_COEX_ALGO_PANHS != algorithm)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BT is under inquiry/page scan !!\n");
-		halbtc8723b2ant_action_bt_inquiry(btcoexist);
-		return;
-	} else {
-		if (coex_dm->need_recover_0x948) {
-			coex_dm->need_recover_0x948 = false;
-			btcoexist->btc_write_2byte(btcoexist, 0x948,
-						   coex_dm->backup_0x948);
-		}
-	}
-
-	coex_dm->cur_algorithm = algorithm;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Algorithm = %d \n",
-		  coex_dm->cur_algorithm);
-
-	if (halbtc8723b2ant_is_common_action(btcoexist)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Action 2-Ant common.\n");
-		coex_dm->auto_tdma_adjust = false;
-	} else {
-		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], preAlgorithm =%d, curAlgorithm =%d\n", coex_dm->pre_algorithm,
-				  coex_dm->cur_algorithm);
-			coex_dm->auto_tdma_adjust = false;
-		}
-		switch (coex_dm->cur_algorithm) {
-		case BT_8723B_2ANT_COEX_ALGO_SCO:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = SCO.\n");
-			halbtc8723b2ant_action_sco(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = HID.\n");
-			halbtc8723b2ant_action_hid(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = A2DP.\n");
-			halbtc8723b2ant_action_a2dp(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = A2DP+PAN(HS).\n");
-			halbtc8723b2ant_action_a2dp_pan_hs(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n");
-			halbtc8723b2ant_action_pan_edr(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = HS mode.\n");
-			halbtc8723b2ant_action_pan_hs(btcoexist);
-				break;
-		case BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n");
-			halbtc8723b2ant_action_pan_edr_a2dp(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_PANEDR_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n");
-			halbtc8723b2ant_action_pan_edr_hid(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
-			halbtc8723b2ant_action_hid_a2dp_pan_edr(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n");
-			halbtc8723b2ant_action_hid_a2dp(btcoexist);
-			break;
-		default:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n");
-			halbtc8723b2ant_coex_alloff(btcoexist);
-			break;
-		}
-		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
-	}
-}
-
-static void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
-{
-	/* set wlan_act to low */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
-	/* Force GNT_BT to High */
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
-	/* BT select s0/s1 is controlled by BT */
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
-}
-
-/*********************************************************************
- *  work around function start with wa_halbtc8723b2ant_
- *********************************************************************/
-/*********************************************************************
- *  extern function start with EXhalbtc8723b2ant_
- *********************************************************************/
-void ex92e_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
-{
-	u8 u8tmp = 0;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], 2Ant Init HW Config!!\n");
-	coex_dm->bt_rf0x1e_backup =
-		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
-
-	/* 0x790[5:0] = 0x5 */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
-	u8tmp &= 0xc0;
-	u8tmp |= 0x5;
-	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
-
-	/*Antenna config	*/
-	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN,
-				     true, false);
-
-
-
-	/* PTA parameter */
-	coex_table_with_type(btcoexist, FORCE_EXEC, 0);
-
-	/* Enable counter statistics */
-	/*0x76e[3] = 1, WLAN_Act control by PTA*/
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
-}
-
-void ex92e_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], Coex Mechanism Init!!\n");
-	halbtc8723b2ant_init_coex_dm(btcoexist);
-}
-
-void ex_halbtc8723b2ant92e_display_coex_info(struct btc_coexist *btcoexist)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	u8 *cli_buf = btcoexist->cli_buf;
-	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
-	u32 u32tmp[4];
-	bool roam = false, scan = false;
-	bool link = false, wifi_under_5g = false;
-	bool bt_hs_on = false, wifi_busy = false;
-	s32 wifi_rssi = 0, bt_hs_rssi = 0;
-	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
-	u8 wifi_dot11_chnl, wifi_hs_chnl;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-	u8 ap_num = 0;
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n ============[BT Coexist info] ============");
-	CL_PRINTF(cli_buf);
-
-	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ==========[Under Manual Control] ============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
-	}
-
-	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
-		return;
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
-		   "Ant PG number/ Ant mechanism: ",
-		   board_info->pg_ant_num, board_info->btdm_ant_num);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
-		   "BT stack/ hci ext ver",
-		   ((stack_info->profile_notified) ? "Yes" : "No"),
-		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
-		   "CoexVer/ fw_ver/ PatchVer",
-		   glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
-		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
-			   &wifi_dot11_chnl);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
-		   "Dot11 channel / HsChnl(HsMode)",
-		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
-		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
-		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
-		   "Wifi rssi/ HS rssi/ AP#", wifi_rssi, bt_hs_rssi, ap_num);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
-		   "Wifi link/ roam/ scan", link, roam, scan);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
-			   &wifi_traffic_dir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
-		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
-		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
-		   (((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
-		   ((!wifi_busy) ? "idle" :
-		   ((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
-		   "uplink" : "downlink")));
-	CL_PRINTF(cli_buf);
-
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
-		   "SCO/HID/PAN/A2DP",
-		   bt_link_info->sco_exist, bt_link_info->hid_exist,
-		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
-		   "BT Info A2DP rate",
-		   (bt_info_ext & BIT(0)) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
-
-	for (i = 0; i < BT_INFO_SRC_8723B_2ANT_MAX; i++) {
-		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-				   "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)",
-				   glbt_info_src_8723b_2ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3],
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
-				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
-		}
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
-		   "PS state, IPS/LPS",
-		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
-		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
-
-	/* Sw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s", "============[Sw mechanism] ============");
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
-		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
-		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
-		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
-		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
-		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
-	CL_PRINTF(cli_buf);
-
-	/* Fw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Fw mechanism] ============");
-	CL_PRINTF(cli_buf);
-
-	ps_tdma_case = coex_dm->cur_ps_tdma;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
-		   "PS TDMA", coex_dm->ps_tdma_para[0],
-		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
-		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
-		   ps_tdma_case, coex_dm->auto_tdma_adjust);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
-		   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
-		   coex_dm->cur_ignore_wlan_act);
-	CL_PRINTF(cli_buf);
-
-	/* Hw setting */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Hw setting] ============");
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
-		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x880);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x778/0x880[29:25]", u8tmp[0],
-		   (u32tmp[0]&0x3e000000) >> 25);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x948/ 0x67[5] / 0x765",
-		   u32tmp[0], ((u8tmp[0]&0x20) >> 5), u8tmp[1]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
-		   u32tmp[0]&0x3, u32tmp[1]&0xff, u32tmp[2]&0x3);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
-		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
-		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
-		   ((u32tmp[0]&0x01800000)>>23), u8tmp[2]&0x1);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0]&0xff, u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
-	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
-
-	fa_ofdm = ((u32tmp[0]&0xffff0000) >> 16) +
-		  ((u32tmp[1]&0xffff0000) >> 16) +
-		   (u32tmp[1] & 0xffff) +
-		   (u32tmp[2] & 0xffff) +
-		  ((u32tmp[3]&0xffff0000) >> 16) +
-		   (u32tmp[3] & 0xffff);
-	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "OFDM-CCA/OFDM-FA/CCK-FA",
-		   u32tmp[0]&0xffff, fa_ofdm, fa_cck);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
-		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
-		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x770(high-pri rx/tx)",
-		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
-		   coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 1)
-	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
-#endif
-	btcoexist->btc_disp_dbg_msg(btcoexist,
-	BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-void ex92e_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_IPS_ENTER == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS ENTER notify\n");
-		coex_sta->under_ips = true;
-		halbtc8723b2ant_wifioff_hwcfg(btcoexist);
-		halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
-		halbtc8723b2ant_coex_alloff(btcoexist);
-	} else if (BTC_IPS_LEAVE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS LEAVE notify\n");
-		coex_sta->under_ips = false;
-		ex92e_halbtc8723b2ant_init_hwconfig(btcoexist);
-		halbtc8723b2ant_init_coex_dm(btcoexist);
-		halbtc8723b2ant_query_bt_info(btcoexist);
-	}
-}
-
-void ex92e_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_LPS_ENABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS ENABLE notify\n");
-		coex_sta->under_lps = true;
-	} else if (BTC_LPS_DISABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS DISABLE notify\n");
-		coex_sta->under_lps = false;
-	}
-}
-
-void ex92e_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_SCAN_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN START notify\n");
-	else if (BTC_SCAN_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN FINISH notify\n");
-}
-
-void ex92e_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_ASSOCIATE_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT START notify\n");
-	else if (BTC_ASSOCIATE_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT FINISH notify\n");
-}
-
-void ex92e_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type)
-{
-	u8 h2c_parameter[3] = {0};
-	u32 wifi_bw;
-	u8 wifi_central_chnl;
-
-	if (BTC_MEDIA_CONNECT == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA connect notify\n");
-	else
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA disconnect notify\n");
-
-	/* only 2.4G we need to inform bt the chnl mask */
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifi_central_chnl);
-	if ((BTC_MEDIA_CONNECT == type) &&
-	    (wifi_central_chnl <= 14)) {
-		h2c_parameter[0] = 0x1;
-		h2c_parameter[1] = wifi_central_chnl;
-		btcoexist->btc_get(btcoexist,
-			BTC_GET_U4_WIFI_BW, &wifi_bw);
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			h2c_parameter[2] = 0x30;
-		else
-			h2c_parameter[2] = 0x20;
-	}
-
-	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
-	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
-	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x66 = 0x%x\n",
-		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
-		  h2c_parameter[2]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
-}
-
-void ex92e_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type)
-{
-	if (type == BTC_PACKET_DHCP)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], DHCP Packet notify\n");
-}
-
-void ex92e_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length)
-{
-	u8 btInfo = 0;
-	u8 i, rsp_source = 0;
-	bool bt_busy = false, limited_dig = false;
-	bool wifi_connected = false;
-
-	coex_sta->c2h_bt_info_req_sent = false;
-
-	rsp_source = tmpbuf[0]&0xf;
-	if (rsp_source >= BT_INFO_SRC_8723B_2ANT_MAX)
-		rsp_source = BT_INFO_SRC_8723B_2ANT_WIFI_FW;
-	coex_sta->bt_info_c2h_cnt[rsp_source]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length =%d, hex data =[",
-		  rsp_source, length);
-	for (i = 0; i < length; i++) {
-		coex_sta->bt_info_c2h[rsp_source][i] = tmpbuf[i];
-		if (i == 1)
-			btInfo = tmpbuf[i];
-		if (i == length-1)
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x]\n", tmpbuf[i]);
-		else
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x, ", tmpbuf[i]);
-	}
-
-	if (btcoexist->manual_control) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n");
-		return;
-	}
-
-	if (BT_INFO_SRC_8723B_2ANT_WIFI_FW != rsp_source) {
-		coex_sta->bt_retry_cnt =	/* [3:0]*/
-			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
-
-		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
-
-		coex_sta->bt_info_ext =
-			coex_sta->bt_info_c2h[rsp_source][4];
-
-		/* Here we need to resend some wifi info to BT
-		     because bt is reset and loss of the info.*/
-		if ((coex_sta->bt_info_ext & BIT(1))) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n");
-			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-					   &wifi_connected);
-			if (wifi_connected)
-				ex92e_halbtc8723b2ant_media_status_notify(
-							btcoexist,
-							BTC_MEDIA_CONNECT);
-			else
-				ex92e_halbtc8723b2ant_media_status_notify(
-							btcoexist,
-							BTC_MEDIA_DISCONNECT);
-		}
-
-		if ((coex_sta->bt_info_ext & BIT(3))) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n");
-			halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC,
-							false);
-		} else {
-			/* BT already NOT ignore Wlan active, do nothing here.*/
-		}
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-		if ((coex_sta->bt_info_ext & BIT(4))) {
-			/* BT auto report already enabled, do nothing*/
-		} else {
-			halbtc8723b2ant_bt_auto_report(btcoexist, FORCE_EXEC,
-						       true);
-		}
-#endif
-	}
-
-	/* check BIT(2) first ==> check if bt is under inquiry or page scan*/
-	if (btInfo & BT_INFO_8723B_2ANT_B_INQ_PAGE)
-		coex_sta->c2h_bt_inquiry_page = true;
-	else
-		coex_sta->c2h_bt_inquiry_page = false;
-
-	/* set link exist status*/
-	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
-		coex_sta->bt_link_exist = false;
-		coex_sta->pan_exist = false;
-		coex_sta->a2dp_exist = false;
-		coex_sta->hid_exist = false;
-		coex_sta->sco_exist = false;
-	} else {/*  connection exists */
-		coex_sta->bt_link_exist = true;
-		if (btInfo & BT_INFO_8723B_2ANT_B_FTP)
-			coex_sta->pan_exist = true;
-		else
-			coex_sta->pan_exist = false;
-		if (btInfo & BT_INFO_8723B_2ANT_B_A2DP)
-			coex_sta->a2dp_exist = true;
-		else
-			coex_sta->a2dp_exist = false;
-		if (btInfo & BT_INFO_8723B_2ANT_B_HID)
-			coex_sta->hid_exist = true;
-		else
-			coex_sta->hid_exist = false;
-		if (btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO)
-			coex_sta->sco_exist = true;
-		else
-			coex_sta->sco_exist = false;
-	}
-
-	halbtc8723b2ant_update_bt_link_info(btcoexist);
-
-	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
-		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT Non-Connected idle!!!\n");
-	/* connection exists but no busy */
-	} else if (btInfo == BT_INFO_8723B_2ANT_B_CONNECTION) {
-		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
-	} else if ((btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO) ||
-		   (btInfo & BT_INFO_8723B_2ANT_B_SCO_BUSY)) {
-		coex_dm->bt_status =
-			BT_8723B_2ANT_BT_STATUS_SCO_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
-	} else if (btInfo&BT_INFO_8723B_2ANT_B_ACL_BUSY) {
-		coex_dm->bt_status =
-			BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
-	} else {
-		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_MAX;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT Non-Defined state!!!\n");
-	}
-
-	if ((BT_8723B_2ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_2ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
-		bt_busy = true;
-		limited_dig = true;
-	} else {
-		bt_busy = false;
-		limited_dig = false;
-	}
-
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
-
-	coex_dm->limited_dig = limited_dig;
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
-
-	halbtc8723b2ant_run_coexist_mechanism(btcoexist);
-}
-
-void ex92e_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
-
-	halbtc8723b2ant_wifioff_hwcfg(btcoexist);
-	halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
-	ex92e_halbtc8723b2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
-}
-
-void ex92e_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	static u8 dis_ver_info_cnt;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], ========================== Periodical ===========================\n");
-
-	if (dis_ver_info_cnt <= 5) {
-		dis_ver_info_cnt += 1;
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], ****************************************************************\n");
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/%d/%d\n", board_info->pg_ant_num,
-			  board_info->btdm_ant_num, board_info->btdm_ant_pos);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], BT stack/hci ext ver = %s/%d\n",
-			  ((stack_info->profile_notified) ? "Yes" : "No"),
-			  stack_info->hci_version);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
-				   &bt_patch_ver);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], CoexVer/fw_ver/PatchVer = %d_%x/0x%x/0x%x(%d)\n",
-			  glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
-			  fw_ver, bt_patch_ver, bt_patch_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], ****************************************************************\n");
-	}
-
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-	halbtc8723b2ant_query_bt_info(btcoexist);
-	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
-	halbtc8723b2ant_monitor_bt_enable_disable(btcoexist);
-#else
-	if (is_wifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust)
-		halbtc8723b2ant_run_coexist_mechanism(btcoexist);
-#endif
-}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.h
deleted file mode 100644
index 8437e1c3ecd0..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8723b2ant.h
+++ /dev/null
@@ -1,145 +0,0 @@
-/************************************************************************
- * The following is for 8723B 2Ant BT Co-exist definition
- ************************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8723B_2ANT			1
-
-
-#define	BT_INFO_8723B_2ANT_B_FTP			BIT(7)
-#define	BT_INFO_8723B_2ANT_B_A2DP			BIT(6)
-#define	BT_INFO_8723B_2ANT_B_HID			BIT(5)
-#define	BT_INFO_8723B_2ANT_B_SCO_BUSY			BIT(4)
-#define	BT_INFO_8723B_2ANT_B_ACL_BUSY			BIT(3)
-#define	BT_INFO_8723B_2ANT_B_INQ_PAGE			BIT(2)
-#define	BT_INFO_8723B_2ANT_B_SCO_ESCO			BIT(1)
-#define	BT_INFO_8723B_2ANT_B_CONNECTION			BIT(0)
-
-#define BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT		2
-
-enum BT_INFO_SRC_8723B_2ANT {
-	BT_INFO_SRC_8723B_2ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8723B_2ANT_BT_RSP			= 0x1,
-	BT_INFO_SRC_8723B_2ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8723B_2ANT_MAX
-};
-
-enum BT_8723B_2ANT_BT_STATUS {
-	BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
-	BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
-	BT_8723B_2ANT_BT_STATUS_INQ_PAGE		= 0x2,
-	BT_8723B_2ANT_BT_STATUS_ACL_BUSY		= 0x3,
-	BT_8723B_2ANT_BT_STATUS_SCO_BUSY		= 0x4,
-	BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
-	BT_8723B_2ANT_BT_STATUS_MAX
-};
-
-enum BT_8723B_2ANT_COEX_ALGO {
-	BT_8723B_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
-	BT_8723B_2ANT_COEX_ALGO_SCO			= 0x1,
-	BT_8723B_2ANT_COEX_ALGO_HID			= 0x2,
-	BT_8723B_2ANT_COEX_ALGO_A2DP			= 0x3,
-	BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
-	BT_8723B_2ANT_COEX_ALGO_PANEDR			= 0x5,
-	BT_8723B_2ANT_COEX_ALGO_PANHS			= 0x6,
-	BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
-	BT_8723B_2ANT_COEX_ALGO_PANEDR_HID		= 0x8,
-	BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
-	BT_8723B_2ANT_COEX_ALGO_HID_A2DP		= 0xa,
-	BT_8723B_2ANT_COEX_ALGO_MAX			= 0xb,
-};
-
-struct coex_dm_8723b_2ant {
-	/* fw mechanism */
-	bool pre_dec_bt_pwr;
-	bool cur_dec_bt_pwr;
-	u8 pre_fw_dac_swing_lvl;
-	u8 cur_fw_dac_swing_lvl;
-	bool cur_ignore_wlan_act;
-	bool pre_ignore_wlan_act;
-	u8 pre_ps_tdma;
-	u8 cur_ps_tdma;
-	u8 ps_tdma_para[5];
-	u8 ps_tdma_du_adj_type;
-	bool reset_tdma_adjust;
-	bool auto_tdma_adjust;
-	bool pre_ps_tdma_on;
-	bool cur_ps_tdma_on;
-	bool pre_bt_auto_report;
-	bool cur_bt_auto_report;
-
-	/* sw mechanism */
-	bool pre_rf_rx_lpf_shrink;
-	bool cur_rf_rx_lpf_shrink;
-	u32 bt_rf0x1e_backup;
-	bool pre_low_penalty_ra;
-	bool cur_low_penalty_ra;
-	bool pre_dac_swing_on;
-	u32 pre_dac_swing_lvl;
-	bool cur_dac_swing_on;
-	u32 cur_dac_swing_lvl;
-	bool pre_adc_back_off;
-	bool cur_adc_back_off;
-	bool pre_agc_table_en;
-	bool cur_agc_table_en;
-	u32 pre_val0x6c0;
-	u32 cur_val0x6c0;
-	u32 pre_val0x6c4;
-	u32 cur_val0x6c4;
-	u32 pre_val0x6c8;
-	u32 cur_val0x6c8;
-	u8 pre_val0x6cc;
-	u8 cur_val0x6cc;
-	bool limited_dig;
-
-	/* algorithm related */
-	u8 pre_algorithm;
-	u8 cur_algorithm;
-	u8 bt_status;
-	u8 wifi_chnl_info[3];
-
-	bool need_recover_0x948;
-	u16 backup_0x948;
-};
-
-struct coex_sta_8723b_2ant {
-	bool bt_link_exist;
-	bool sco_exist;
-	bool a2dp_exist;
-	bool hid_exist;
-	bool pan_exist;
-
-	bool under_lps;
-	bool under_ips;
-	u32 high_priority_tx;
-	u32 high_priority_rx;
-	u32 low_priority_tx;
-	u32 low_priority_rx;
-	u8 bt_rssi;
-	u8 pre_bt_rssi_state;
-	u8 pre_wifi_rssi_state[4];
-	bool c2h_bt_info_req_sent;
-	u8 bt_info_c2h[BT_INFO_SRC_8723B_2ANT_MAX][10];
-	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_2ANT_MAX];
-	bool c2h_bt_inquiry_page;
-	u8 bt_retry_cnt;
-	u8 bt_info_ext;
-};
-
-/*********************************************************************
- * The following is interface which will notify coex module.
- *********************************************************************/
-void ex92e_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex92e_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex92e_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex92e_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex92e_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex92e_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist,
-					  u8 type);
-void ex92e_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
-					       u8 type);
-void ex92e_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
-						 u8 type);
-void ex92e_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
-					  u8 *tmpbuf, u8 length);
-void ex92e_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
-void ex92e_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant92e_display_coex_info(struct btc_coexist *btcoexist);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h
deleted file mode 100644
index 9b991d021a88..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a1ant.h
+++ /dev/null
@@ -1,158 +0,0 @@
-/*  */
-/*  The following is for 8821A 1ANT BT Co-exist definition */
-/*  */
-#define	BT_AUTO_REPORT_ONLY_8821A_1ANT				0
-
-#define	BT_INFO_8821A_1ANT_B_FTP				BIT(7)
-#define	BT_INFO_8821A_1ANT_B_A2DP				BIT(6)
-#define	BT_INFO_8821A_1ANT_B_HID				BIT(5)
-#define	BT_INFO_8821A_1ANT_B_SCO_BUSY				BIT(4)
-#define	BT_INFO_8821A_1ANT_B_ACL_BUSY				BIT(3)
-#define	BT_INFO_8821A_1ANT_B_INQ_PAGE				BIT(2)
-#define	BT_INFO_8821A_1ANT_B_SCO_ESCO				BIT(1)
-#define	BT_INFO_8821A_1ANT_B_CONNECTION				BIT(0)
-
-#define	BT_INFO_8821A_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
-		(((_BT_INFO_EXT_&BIT(0))) ? true : false)
-
-#define	BTC_RSSI_COEX_THRESH_TOL_8821A_1ANT		2
-
-enum BT_INFO_SRC_8821A_1ANT {
-	BT_INFO_SRC_8821A_1ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8821A_1ANT_BT_RSP			= 0x1,
-	BT_INFO_SRC_8821A_1ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8821A_1ANT_MAX
-};
-
-enum BT_8821A_1ANT_BT_STATUS {
-	BT_8821A_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
-	BT_8821A_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
-	BT_8821A_1ANT_BT_STATUS_INQ_PAGE		= 0x2,
-	BT_8821A_1ANT_BT_STATUS_ACL_BUSY		= 0x3,
-	BT_8821A_1ANT_BT_STATUS_SCO_BUSY		= 0x4,
-	BT_8821A_1ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
-	BT_8821A_1ANT_BT_STATUS_MAX
-};
-
-enum BT_8821A_1ANT_WIFI_STATUS {
-	BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE			= 0x0,
-	BT_8821A_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN		= 0x1,
-	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SCAN			= 0x2,
-	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT			= 0x3,
-	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_IDLE			= 0x4,
-	BT_8821A_1ANT_WIFI_STATUS_CONNECTED_BUSY			= 0x5,
-	BT_8821A_1ANT_WIFI_STATUS_MAX
-};
-
-enum BT_8821A_1ANT_COEX_ALGO {
-	BT_8821A_1ANT_COEX_ALGO_UNDEFINED		= 0x0,
-	BT_8821A_1ANT_COEX_ALGO_SCO			= 0x1,
-	BT_8821A_1ANT_COEX_ALGO_HID			= 0x2,
-	BT_8821A_1ANT_COEX_ALGO_A2DP			= 0x3,
-	BT_8821A_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
-	BT_8821A_1ANT_COEX_ALGO_PANEDR			= 0x5,
-	BT_8821A_1ANT_COEX_ALGO_PANHS			= 0x6,
-	BT_8821A_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
-	BT_8821A_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
-	BT_8821A_1ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
-	BT_8821A_1ANT_COEX_ALGO_HID_A2DP		= 0xa,
-	BT_8821A_1ANT_COEX_ALGO_MAX			= 0xb,
-};
-
-struct coex_dm_8821a_1ant {
-	/*  fw mechanism */
-	bool		cur_ignore_wlan_act;
-	bool		pre_ignore_wlan_act;
-	u8		pre_ps_tdma;
-	u8		cur_ps_tdma;
-	u8		ps_tdma_para[5];
-	u8		ps_tdma_du_adj_type;
-	bool		auto_tdma_adjust;
-	bool		pre_ps_tdma_on;
-	bool		cur_ps_tdma_on;
-	bool		pre_bt_auto_report;
-	bool		cur_bt_auto_report;
-	u8		pre_lps;
-	u8		cur_lps;
-	u8		pre_rpwm;
-	u8		cur_rpwm;
-
-	/*  sw mechanism */
-	bool	pre_low_penalty_ra;
-	bool		cur_low_penalty_ra;
-	u32		pre_val_0x6c0;
-	u32		cur_val_0x6c0;
-	u32		pre_val_0x6c4;
-	u32		cur_val_0x6c4;
-	u32		pre_val_0x6c8;
-	u32		cur_val_0x6c8;
-	u8		pre_val_0x6cc;
-	u8		cur_val_0x6cc;
-
-	u32		backup_arfr_cnt1; /*  Auto Rate Fallback Retry cnt */
-	u32		backup_arfr_cnt2; /*  Auto Rate Fallback Retry cnt */
-	u16		backup_retry_limit;
-	u8		backup_ampdu_max_time;
-
-	/*  algorithm related */
-	u8		pre_algorithm;
-	u8		cur_algorithm;
-	u8		bt_status;
-	u8		wifi_chnl_info[3];
-
-	u32		pre_ra_mask;
-	u32		cur_ra_mask;
-	u8		pre_arfr_type;
-	u8		cur_arfr_type;
-	u8		pre_retry_limit_type;
-	u8		cur_retry_limit_type;
-	u8		pre_ampdu_time_type;
-	u8		cur_ampdu_time_type;
-
-	u8		error_condition;
-};
-
-struct coex_sta_8821a_1ant {
-	bool		bt_link_exist;
-	bool		sco_exist;
-	bool		a2dp_exist;
-	bool		hid_exist;
-	bool		pan_exist;
-
-	bool		under_lps;
-	bool		under_ips;
-	u32		special_pkt_period_cnt;
-	u32		high_priority_tx;
-	u32		high_priority_rx;
-	u32		low_priority_tx;
-	u32		low_priority_rx;
-	u8		bt_rssi;
-	u8		pre_bt_rssi_state;
-	u8		pre_wifi_rssi_state[4];
-	bool		c2h_bt_info_req_sent;
-	u8		bt_info_c2h[BT_INFO_SRC_8821A_1ANT_MAX][10];
-	u32		bt_info_c2h_cnt[BT_INFO_SRC_8821A_1ANT_MAX];
-	bool		c2h_bt_inquiry_page;
-	u8		bt_retry_cnt;
-	u8		bt_info_ext;
-};
-
-/*  The following is interface which will notify coex module. */
-void ex_halbtc8821a1ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8821a1ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type);
-void ex_halbtc8821a1ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8821a1ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8821a1ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnpstate);
-void ex_halbtc8821a1ant_periodical(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_display_coex_info(struct btc_coexist *btcoexist);
-void ex_halbtc8821a1ant_dbg_control(struct btc_coexist *btcoexist, u8 op_code,
-				    u8 op_len, u8 *data);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h b/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h
deleted file mode 100644
index 745506b48d60..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtc8821a2ant.h
+++ /dev/null
@@ -1,179 +0,0 @@
-/*  */
-/*  The following is for 8821A 2Ant BT Co-exist definition */
-/*  */
-#define	BT_INFO_8821A_2ANT_B_FTP				BIT(7)
-#define	BT_INFO_8821A_2ANT_B_A2DP				BIT(6)
-#define	BT_INFO_8821A_2ANT_B_HID				BIT(5)
-#define	BT_INFO_8821A_2ANT_B_SCO_BUSY				BIT(4)
-#define	BT_INFO_8821A_2ANT_B_ACL_BUSY				BIT(3)
-#define	BT_INFO_8821A_2ANT_B_INQ_PAGE				BIT(2)
-#define	BT_INFO_8821A_2ANT_B_SCO_ESCO				BIT(1)
-#define	BT_INFO_8821A_2ANT_B_CONNECTION				BIT(0)
-
-#define		BTC_RSSI_COEX_THRESH_TOL_8821A_2ANT		2
-
-enum BT_INFO_SRC_8821A_2ANT {
-	BT_INFO_SRC_8821A_2ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8821A_2ANT_BT_RSP			= 0x1,
-	BT_INFO_SRC_8821A_2ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8821A_2ANT_MAX
-};
-
-enum BT_8821A_2ANT_BT_STATUS {
-	BT_8821A_2ANT_BT_STATUS_IDLE				= 0x0,
-	BT_8821A_2ANT_BT_STATUS_CONNECTED_IDLE	= 0x1,
-	BT_8821A_2ANT_BT_STATUS_NON_IDLE			= 0x2,
-	BT_8821A_2ANT_BT_STATUS_MAX
-};
-
-enum BT_8821A_2ANT_COEX_ALGO {
-	BT_8821A_2ANT_COEX_ALGO_UNDEFINED			= 0x0,
-	BT_8821A_2ANT_COEX_ALGO_SCO				= 0x1,
-	BT_8821A_2ANT_COEX_ALGO_HID				= 0x2,
-	BT_8821A_2ANT_COEX_ALGO_A2DP				= 0x3,
-	BT_8821A_2ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
-	BT_8821A_2ANT_COEX_ALGO_PANEDR			= 0x5,
-	BT_8821A_2ANT_COEX_ALGO_PANHS			= 0x6,
-	BT_8821A_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
-	BT_8821A_2ANT_COEX_ALGO_PANEDR_HID		= 0x8,
-	BT_8821A_2ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
-	BT_8821A_2ANT_COEX_ALGO_HID_A2DP			= 0xa,
-	BT_8821A_2ANT_COEX_ALGO_MAX				= 0xb,
-};
-
-struct coex_dm_8821a_2ant {
-	/*  fw mechanism */
-	bool		pre_dec_bt_pwr;
-	bool		cur_dec_bt_pwr;
-	bool		pre_bt_lna_constrain;
-	bool		cur_bt_lna_constrain;
-	u8		pre_bt_psd_mode;
-	u8		cur_bt_psd_mode;
-	u8		pre_fw_dac_swing_lvl;
-	u8		cur_fw_dac_swing_lvl;
-	bool		cur_ignore_wlan_act;
-	bool		pre_ignore_wlan_act;
-	u8		pre_ps_tdma;
-	u8		cur_ps_tdma;
-	u8		ps_tdma_para[5];
-	u8		ps_tdma_du_adj_type;
-	bool		reset_tdma_adjust;
-	bool		pre_ps_tdma_on;
-	bool		cur_ps_tdma_on;
-	bool		pre_bt_auto_report;
-	bool		cur_bt_auto_report;
-
-	/*  sw mechanism */
-	bool		pre_rf_rx_lpf_shrink;
-	bool		cur_rf_rx_lpf_shrink;
-	u32		bt_rf0x1e_backup;
-	bool	pre_low_penalty_ra;
-	bool		cur_low_penalty_ra;
-	bool		pre_dac_swing_on;
-	u32		pre_dac_swing_lvl;
-	bool		cur_dac_swing_on;
-	u32		cur_dac_swing_lvl;
-	bool		pre_adc_back_off;
-	bool		cur_adc_back_off;
-	bool	pre_agc_table_en;
-	bool		cur_agc_table_en;
-	u32		pre_val0x6c0;
-	u32		cur_val0x6c0;
-	u32		pre_val0x6c4;
-	u32		cur_val0x6c4;
-	u32		pre_val0x6c8;
-	u32		cur_val0x6c8;
-	u8		pre_val0x6cc;
-	u8		cur_val0x6cc;
-	bool		limited_dig;
-
-	/*  algorithm related */
-	u8		pre_algorithm;
-	u8		cur_algorithm;
-	u8		bt_status;
-	u8		wifi_chnl_info[3];
-};
-
-struct coex_sta_8821a_2ant {
-	bool		bt_link_exist;
-	bool		sco_exist;
-	bool		a2dp_exist;
-	bool		hid_exist;
-	bool		pan_exist;
-
-	bool		under_lps;
-	bool		under_ips;
-	u32		high_priority_tx;
-	u32		high_priority_rx;
-	u32		low_priority_tx;
-	u32		low_priority_rx;
-	u8		bt_rssi;
-	u8		pre_bt_rssi_state;
-	u8		pre_wifi_rssi_state[4];
-	bool		c2h_bt_info_req_sent;
-	u8		bt_info_c2h[BT_INFO_SRC_8821A_2ANT_MAX][10];
-	u32		bt_info_c2h_cnt[BT_INFO_SRC_8821A_2ANT_MAX];
-	bool		c2h_bt_inquiry_page;
-	u8		bt_retry_cnt;
-	u8		bt_info_ext;
-};
-
-/*  */
-/*  The following is interface which will notify coex module. */
-/*  */
-void
-ex_halbtc8821a2ant_init_hwconfig(
-	struct btc_coexist *btcoexist
-	);
-void
-ex_halbtc8821a2ant_init_coex_dm(
-	struct btc_coexist *btcoexist
-	);
-void
-ex_halbtc8821a2ant_ips_notify(
-	struct btc_coexist *btcoexist,
-	u8 type
-	);
-void
-ex_halbtc8821a2ant_lps_notify(
-	struct btc_coexist *btcoexist,
-	u8 type
-	);
-void
-ex_halbtc8821a2ant_scan_notify(
-	struct btc_coexist *btcoexist,
-	u8 type
-	);
-void
-ex_halbtc8821a2ant_connect_notify(
-	struct btc_coexist *btcoexist,
-	u8 type
-	);
-void
-ex_halbtc8821a2ant_media_status_notify(
-	struct btc_coexist *btcoexist,
-	u8 type
-	);
-void
-ex_halbtc8821a2ant_special_packet_notify(
-	struct btc_coexist *btcoexist,
-	u8 type
-	);
-void
-ex_halbtc8821a2ant_bt_info_notify(
-	struct btc_coexist *btcoexist,
-	u8 *tmp_buf,
-	u8 length
-	);
-void
-ex_halbtc8821a2ant_halt_notify(
-	struct btc_coexist *btcoexist
-	);
-void
-ex_halbtc8821a2ant_periodical(
-	struct btc_coexist *btcoexist
-	);
-void
-ex_halbtc8821a2ant_display_coex_info(
-	struct btc_coexist *btcoexist
-	);
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.c b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.c
deleted file mode 100644
index 2d9fc24b1e21..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.c
+++ /dev/null
@@ -1,1297 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- *
- ******************************************************************************/
-
-#include "halbt_precomp.h"
-
-/***********************************************
- *		Global variables
- ***********************************************/
-static const char *const bt_profile_string[] = {
-	"NONE",
-	"A2DP",
-	"PAN",
-	"HID",
-	"SCO",
-};
-
-static const char *const bt_spec_string[] = {
-	"1.0b",
-	"1.1",
-	"1.2",
-	"2.0+EDR",
-	"2.1+EDR",
-	"3.0+HS",
-	"4.0",
-};
-
-static const char *const bt_link_role_string[] = {
-	"Master",
-	"Slave",
-};
-
-static const char *const h2c_state_string[] = {
-	"successful",
-	"h2c busy",
-	"rf off",
-	"fw not read",
-};
-
-static const char *const io_state_string[] = {
-	"IO_STATUS_SUCCESS",
-	"IO_STATUS_FAIL_CANNOT_IO",
-	"IO_STATUS_FAIL_RF_OFF",
-	"IO_STATUS_FAIL_FW_READ_CLEAR_TIMEOUT",
-	"IO_STATUS_FAIL_WAIT_IO_EVENT_TIMEOUT",
-	"IO_STATUS_INVALID_LEN",
-	"IO_STATUS_IO_IDLE_QUEUE_EMPTY",
-	"IO_STATUS_IO_INSERT_WAIT_QUEUE_FAIL",
-	"IO_STATUS_UNKNOWN_FAIL",
-	"IO_STATUS_WRONG_LEVEL",
-	"IO_STATUS_H2C_STOPPED",
-};
-
-struct btc_coexist gl92e_bt_coexist;
-
-u32 btc_92edbg_type[BTC_MSG_MAX];
-static u8 btc_dbg_buf[100];
-
-/***************************************************
- *		Debug related function
- ***************************************************/
-static bool is_any_client_connect_to_ap(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	struct rtl_sta_info *drv_priv;
-	u8 cnt = 0;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC ||
-	    mac->opmode == NL80211_IFTYPE_MESH_POINT ||
-	    mac->opmode == NL80211_IFTYPE_AP) {
-		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
-		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
-			cnt++;
-		}
-		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
-	}
-	if (cnt > 0)
-		return true;
-	else
-		return false;
-}
-
-static bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
-{
-	if (!btcoexist->binded || NULL == btcoexist->adapter)
-		return false;
-
-	return true;
-}
-
-static bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
-{
-	if (rtlpriv->link_info.b_busytraffic)
-		return true;
-	else
-		return false;
-}
-
-
-static void halbtc_dbg_init(void)
-{
-	u8 i;
-
-	for (i = 0; i < BTC_MSG_MAX; i++)
-		btc_92edbg_type[i] = 0;
-
-	btc_92edbg_type[BTC_MSG_INTERFACE] = 0;
-
-	btc_92edbg_type[BTC_MSG_ALGORITHM] = 0;
-}
-
-static bool halbtc_is_bt40(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	bool is_ht40 = true;
-	enum ht_channel_width bw = rtlphy->current_chan_bw;
-
-
-	if (bw == HT_CHANNEL_WIDTH_20)
-		is_ht40 = false;
-	else if (bw == HT_CHANNEL_WIDTH_20_40)
-		is_ht40 = true;
-
-	return is_ht40;
-}
-
-static bool halbtc_legacy(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-
-	bool is_legacy = false;
-
-	if ((mac->mode == WIRELESS_MODE_B) || (mac->mode == WIRELESS_MODE_G))
-		is_legacy = true;
-
-	return is_legacy;
-}
-
-bool halbtc92e_is_wifi_uplink(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-
-	if (rtlpriv->link_info.b_tx_busy_traffic)
-		return true;
-	else
-		return false;
-}
-
-static u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv =
-		(struct rtl_priv *)btcoexist->adapter;
-	u32 wifi_bw = BTC_WIFI_BW_HT20;
-
-	if (halbtc_is_bt40(rtlpriv)) {
-		wifi_bw = BTC_WIFI_BW_HT40;
-	} else {
-		if (halbtc_legacy(rtlpriv))
-			wifi_bw = BTC_WIFI_BW_LEGACY;
-		else
-			wifi_bw = BTC_WIFI_BW_HT20;
-	}
-	return wifi_bw;
-}
-
-static u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_phy	*rtlphy = &(rtlpriv->phy);
-	u8 chnl = 1;
-
-
-	if (rtlphy->current_channel != 0)
-		chnl = rtlphy->current_channel;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "halbtc_get_wifi_central_chnl:%d\n", chnl);
-	return chnl;
-}
-
-static void halbtc_leave_lps(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv;
-	struct rtl_ps_ctl *ppsc;
-	bool ap_enable = false;
-
-	rtlpriv = btcoexist->adapter;
-	ppsc = rtl_psc(rtlpriv);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-			   &ap_enable);
-
-	if (ap_enable) {
-		pr_debug("halbtc_leave_lps()<--dont leave lps under AP mode\n");
-		return;
-	}
-
-	btcoexist->bt_info.bt_ctrl_lps = true;
-	btcoexist->bt_info.bt_lps_on = false;
-	rtl92e_lps_leave(rtlpriv->mac80211.hw);
-}
-
-static void halbtc_enter_lps(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv;
-	struct rtl_ps_ctl *ppsc;
-	bool ap_enable = false;
-
-	rtlpriv = btcoexist->adapter;
-	ppsc = rtl_psc(rtlpriv);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-			   &ap_enable);
-
-	if (ap_enable) {
-		pr_debug("halbtc_enter_lps()<--dont enter lps under AP mode\n");
-		return;
-	}
-
-	btcoexist->bt_info.bt_ctrl_lps = true;
-	btcoexist->bt_info.bt_lps_on = false;
-	rtl92e_lps_enter(rtlpriv->mac80211.hw);
-}
-
-static void halbtc_normal_lps(struct btc_coexist *btcoexist)
-{
-	if (btcoexist->bt_info.bt_ctrl_lps) {
-		btcoexist->bt_info.bt_lps_on = false;
-		btcoexist->bt_info.bt_ctrl_lps = false;
-	}
-}
-
-static void halbtc_aggregation_check(struct btc_coexist *btcoexist)
-{
-}
-
-static u32 halbtcoutsrc_get_wifi_link_status(struct btc_coexist *btcoexist)
-{
-	/*------------------------------------
-	 * return value:
-	 * [31:16] => connected port number
-	 * [15:0] => port connected bit define
-	 *------------------------------------
-	 */
-
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	u32 ret_val = 0;
-	u32 port_connected_status = 0, num_of_connected_port = 0;
-
-	if (mac->opmode == NL80211_IFTYPE_STATION &&
-	    mac->link_state >= MAC80211_LINKED) {
-		port_connected_status |= WIFI_STA_CONNECTED;
-		num_of_connected_port++;
-	}
-	/* AP & ADHOC & MESH */
-	if (is_any_client_connect_to_ap(btcoexist)) {
-		port_connected_status |= WIFI_AP_CONNECTED;
-		num_of_connected_port++;
-	}
-	/*if (BT_HsConnectionEstablished(Adapter))
-	{
-		port_connected_status |= WIFI_HS_CONNECTED;
-		num_of_connected_port++;
-	}*/
-	/* TODO:
-	 * P2P Connected Status	*/
-
-	ret_val = (num_of_connected_port << 16) | port_connected_status;
-
-	return ret_val;
-}
-
-
-static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
-{
-	return 0;
-}
-
-static s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-	s32	undecorated_smoothed_pwdb = 0;
-
-	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
-		undecorated_smoothed_pwdb =
-			rtlpriv->dm.undecorated_smoothed_pwdb;
-	else /* associated entry pwdb */
-		undecorated_smoothed_pwdb =
-			rtlpriv->dm.undecorated_smoothed_pwdb;
-	return undecorated_smoothed_pwdb;
-}
-
-static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	bool *bool_tmp = (bool *)out_buf;
-	int *s32_tmp = (int *)out_buf;
-	u32 *u32_tmp = (u32 *)out_buf;
-	u8 *u8_tmp = (u8 *)out_buf;
-	bool tmp = false;
-
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return false;
-
-
-	switch (get_type) {
-	case BTC_GET_BL_HS_OPERATION:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_HS_CONNECTING:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_CONNECTED:
-		if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
-		    rtlpriv->mac80211.link_state >= MAC80211_LINKED)
-			tmp = true;
-		if (is_any_client_connect_to_ap(btcoexist))
-			tmp = true;
-		*bool_tmp = tmp;
-		break;
-	case BTC_GET_BL_WIFI_BUSY:
-		if (halbtc_is_wifi_busy(rtlpriv))
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_SCAN:
-		if (mac->act_scanning)
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_LINK:
-		if (mac->link_state == MAC80211_LINKING)
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_ROAM:	/*TODO*/
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:	/*TODO*/
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_UNDER_5G:
-		*bool_tmp = false; /*TODO*/
-
-	case BTC_GET_BL_WIFI_DHCP:	/*TODO*/
-		break;
-	case BTC_GET_BL_WIFI_SOFTAP_IDLE:
-		*bool_tmp = true;
-		break;
-	case BTC_GET_BL_WIFI_SOFTAP_LINKING:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_IN_EARLY_SUSPEND:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
-		if (NO_ENCRYPTION == rtlpriv->sec.pairwise_enc_algorithm)
-			*bool_tmp = false;
-		else
-			*bool_tmp = true;
-		break;
-	case BTC_GET_BL_WIFI_UNDER_B_MODE:
-		if (WIRELESS_MODE_B == rtlpriv->mac80211.mode)
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_EXT_SWITCH:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_S4_WIFI_RSSI:
-		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
-		break;
-	case BTC_GET_S4_HS_RSSI:	/*TODO*/
-		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
-		break;
-	case BTC_GET_U4_WIFI_BW:
-		*u32_tmp = halbtc_get_wifi_bw(btcoexist);
-		break;
-	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
-		if (halbtc92e_is_wifi_uplink(rtlpriv))
-			*u32_tmp = BTC_WIFI_TRAFFIC_TX;
-		else
-			*u32_tmp = BTC_WIFI_TRAFFIC_RX;
-		break;
-	case BTC_GET_U4_WIFI_FW_VER:
-		*u32_tmp = (rtlhal->fw_version << 16) | rtlhal->fw_subversion;
-		break;
-	case BTC_GET_U4_WIFI_LINK_STATUS:
-		*u32_tmp = halbtcoutsrc_get_wifi_link_status(btcoexist);
-		break;
-	case BTC_GET_U4_BT_PATCH_VER:
-		*u32_tmp = halbtc_get_bt_patch_version(btcoexist);
-		break;
-	case BTC_GET_U1_WIFI_DOT11_CHNL:
-		*u8_tmp = rtlphy->current_channel;
-		break;
-	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
-		*u8_tmp = halbtc_get_wifi_central_chnl(btcoexist);
-		break;
-	case BTC_GET_U1_WIFI_HS_CHNL:
-		*u8_tmp = 1;/* BT_OperateChnl(rtlpriv); */
-		break;
-	case BTC_GET_U1_MAC_PHY_MODE:
-		*u8_tmp = BTC_MP_UNKNOWN;
-		break;
-	case BTC_GET_U1_AP_NUM:
-		/* driver don't know AP num in Linux,
-		 * So, the return value here is not right */
-		*u8_tmp = 1;/* pDefMgntInfo->NumBssDesc4Query; */
-		break;
-
-	/************* 1Ant **************/
-	case BTC_GET_U1_LPS_MODE:
-		*u8_tmp = btcoexist->pwr_mode_val[0];
-		break;
-
-	default:
-		break;
-	}
-
-	return true;
-}
-
-static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
-	bool *bool_tmp = (bool *)in_buf;
-	u8 *u8_tmp = (u8 *)in_buf;
-	u32 *u32_tmp = (u32 *)in_buf;
-
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return false;
-
-	switch (set_type) {
-	/* set some bool type variables. */
-	case BTC_SET_BL_BT_DISABLE:
-		btcoexist->bt_info.bt_disabled = *bool_tmp;
-		break;
-	case BTC_SET_BL_BT_TRAFFIC_BUSY:
-		btcoexist->bt_info.bt_busy = *bool_tmp;
-		break;
-	case BTC_SET_BL_BT_LIMITED_DIG:
-		btcoexist->bt_info.limited_dig = *bool_tmp;
-		break;
-	case BTC_SET_BL_FORCE_TO_ROAM:
-		btcoexist->bt_info.force_to_roam = *bool_tmp;
-		break;
-	case BTC_SET_BL_TO_REJ_AP_AGG_PKT:
-		btcoexist->bt_info.reject_agg_pkt = *bool_tmp;
-		break;
-	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
-		btcoexist->bt_info.b_bt_ctrl_buf_size = *bool_tmp;
-		break;
-	case BTC_SET_BL_INC_SCAN_DEV_NUM:
-		btcoexist->bt_info.increase_scan_dev_num = *bool_tmp;
-		break;
-		/* set some u1Byte type variables. */
-	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
-		btcoexist->bt_info.rssi_adjust_for_agc_table_on = *u8_tmp;
-		break;
-	case BTC_SET_U1_AGG_BUF_SIZE:
-		btcoexist->bt_info.agg_buf_size = *u8_tmp;
-		break;
-		/* the following are some action which will be triggered */
-	case BTC_SET_ACT_GET_BT_RSSI:
-		/*BTHCI_SendGetBtRssiEvent(rtlpriv);*/
-		break;
-	case BTC_SET_ACT_AGGREGATE_CTRL:
-		halbtc_aggregation_check(btcoexist);
-		break;
-
-		/* 1Ant */
-	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
-		btcoexist->bt_info.rssi_adjust_for_1ant_coex_type = *u8_tmp;
-		break;
-	case BTC_SET_U1_LPS_VAL:
-		btcoexist->bt_info.lps_val = *u8_tmp;
-		break;
-	case BTC_SET_U1_RPWM_VAL:
-		btcoexist->bt_info.rpwm_val = *u8_tmp;
-		break;
-	/* the following are some action which will be triggered  */
-	case BTC_SET_ACT_LEAVE_LPS:
-		halbtc_leave_lps(btcoexist);
-		break;
-	case BTC_SET_ACT_ENTER_LPS:
-		halbtc_enter_lps(btcoexist);
-		break;
-	case BTC_SET_ACT_NORMAL_LPS:
-		halbtc_normal_lps(btcoexist);
-		break;
-	case BTC_SET_ACT_DISABLE_LOW_POWER:
-		break;
-	case BTC_SET_ACT_UPDATE_ra_mask:
-		btcoexist->bt_info.ra_mask = *u32_tmp;
-		break;
-	case BTC_SET_ACT_SEND_MIMO_PS:
-		break;
-	case BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT:
-		btcoexist->bt_info.force_exec_pwr_cmd_cnt++;
-		break;
-	case BTC_SET_ACT_CTRL_BT_INFO: /*wait for 8812/8821*/
-		break;
-	case BTC_SET_ACT_CTRL_BT_COEX:
-		break;
-	default:
-		break;
-	}
-
-	return true;
-}
-
-static void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
-{
-}
-
-/************************************************************
- *		IO related function
- ************************************************************/
-static u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return	rtl_read_byte(rtlpriv, reg_addr);
-}
-
-
-static u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return	rtl_read_word(rtlpriv, reg_addr);
-}
-
-
-static u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return	rtl_read_dword(rtlpriv, reg_addr);
-}
-
-
-static void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_write_byte(rtlpriv, reg_addr, data);
-}
-
-static void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr,
-				       u8 bit_mask, u8 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	u8 original_value, bit_shift = 0;
-	u8 i;
-
-	if (bit_mask != MASKBYTE0) {/*if not "byte" write*/
-		original_value = rtl_read_byte(rtlpriv, reg_addr);
-		for (i = 0; i <= 7; i++) {
-			if ((bit_mask>>i)&0x1)
-				break;
-		}
-		bit_shift = i;
-		data = (original_value & (~bit_mask)) |
-			((data << bit_shift) & bit_mask);
-	}
-	rtl_write_byte(rtlpriv, reg_addr, data);
-}
-
-
-static void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_write_word(rtlpriv, reg_addr, data);
-}
-
-
-static void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
-{
-	struct btc_coexist *btcoexist =
-		(struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_write_dword(rtlpriv, reg_addr, data);
-}
-
-
-static void halbtc_set_bbreg(void *bt_context, u32 reg_addr,
-			     u32 bit_mask, u32 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_set_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask, data);
-}
-
-
-static u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return rtl_get_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask);
-}
-
-
-static void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
-			     u32 bit_mask, u32 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_set_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask, data);
-}
-
-
-static u32 halbtc_get_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
-			    u32 bit_mask)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return rtl_get_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask);
-}
-
-
-static void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
-				u32 cmd_len, u8 *cmd_buf)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtlpriv->cfg->ops->fill_h2c_cmd(rtlpriv->mac80211.hw, element_id,
-					cmd_len, cmd_buf);
-}
-
-static void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
-{
-	struct btc_coexist *btcoexist =	(struct btc_coexist *)bt_context;
-	switch (disp_type) {
-	case BTC_DBG_DISP_COEX_STATISTICS:
-		halbtc_display_coex_statistics(btcoexist);
-		break;
-	case BTC_DBG_DISP_BT_LINK_INFO:
-		halbtc_display_bt_link_info(btcoexist);
-		break;
-	case BTC_DBG_DISP_BT_FW_VER:
-		halbtc_display_bt_fw_info(btcoexist);
-		break;
-	case BTC_DBG_DISP_FW_PWR_MODE_CMD:
-		halbtc_display_fw_pwr_mode_cmd(btcoexist);
-		break;
-	default:
-		break;
-	}
-}
-
-static bool halbtc_under_ips(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
-	enum rf_pwrstate rtstate;
-
-	if (ppsc->b_inactiveps) {
-		rtstate = ppsc->rfpwr_state;
-
-		if (rtstate != ERFON &&
-		    ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
-			return true;
-	}
-
-	return false;
-}
-
-/*****************************************************************
- *         Extern functions called by other module
- *****************************************************************/
-bool exhalbtc92e_initlize_variables(struct rtl_priv *adapter)
-{
-	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
-
-	btcoexist->statistics.cnt_bind++;
-
-	halbtc_dbg_init();
-
-	if (btcoexist->binded)
-		return false;
-	else
-		btcoexist->binded = true;
-
-	btcoexist->chip_interface = BTC_INTF_UNKNOWN;
-
-	if (NULL == btcoexist->adapter)
-		btcoexist->adapter = adapter;
-
-	btcoexist->stack_info.profile_notified = false;
-
-	btcoexist->btc_read_1byte = halbtc_read_1byte;
-	btcoexist->btc_write_1byte = halbtc_write_1byte;
-	btcoexist->btc_write_1byte_bitmask = halbtc_bitmask_write_1byte;
-	btcoexist->btc_read_2byte = halbtc_read_2byte;
-	btcoexist->btc_write_2byte = halbtc_write_2byte;
-	btcoexist->btc_read_4byte = halbtc_read_4byte;
-	btcoexist->btc_write_4byte = halbtc_write_4byte;
-
-	btcoexist->btc_set_bb_reg = halbtc_set_bbreg;
-	btcoexist->btc_get_bb_reg = halbtc_get_bbreg;
-
-	btcoexist->btc_set_rf_reg = halbtc_set_rfreg;
-	btcoexist->btc_get_rf_reg = halbtc_get_rfreg;
-
-	btcoexist->btc_fill_h2c = halbtc_fill_h2c_cmd;
-	btcoexist->btc_disp_dbg_msg = halbtc_display_dbg_msg;
-
-	btcoexist->btc_get = halbtc_get;
-	btcoexist->btc_set = halbtc_set;
-
-	btcoexist->cli_buf = &btc_dbg_buf[0];
-
-	btcoexist->bt_info.b_bt_ctrl_buf_size = false;
-	btcoexist->bt_info.agg_buf_size = 5;
-
-	btcoexist->bt_info.increase_scan_dev_num = false;
-	return true;
-}
-
-void exhalbtc92e_init_hw_config(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->statistics.cnt_init_hw_config++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_init_hwconfig(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_init_hwconfig(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_init_hwconfig(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_init_hwconfig(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_init_hwconfig(btcoexist);
-	}
-}
-
-void exhalbtc92e_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->statistics.cnt_init_coex_dm++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_init_coex_dm(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_init_coex_dm(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_init_coex_dm(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_init_coex_dm(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_init_coex_dm(btcoexist);
-	}
-
-	btcoexist->initilized = true;
-}
-
-void exhalbtc92e_ips_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 ips_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_ips_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (ERFOFF == type)
-		ips_type = BTC_IPS_ENTER;
-	else
-		ips_type = BTC_IPS_LEAVE;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_ips_notify(btcoexist, ips_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_ips_notify(btcoexist, ips_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_ips_notify(btcoexist, ips_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_ips_notify(btcoexist, ips_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_ips_notify(btcoexist, ips_type);
-	}
-}
-
-void exhalbtc92e_lps_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 lps_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_lps_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (EACTIVE == type)
-		lps_type = BTC_LPS_DISABLE;
-	else
-		lps_type = BTC_LPS_ENABLE;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_lps_notify(btcoexist, lps_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_lps_notify(btcoexist, lps_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_lps_notify(btcoexist, lps_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_lps_notify(btcoexist, lps_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_lps_notify(btcoexist, lps_type);
-	}
-}
-
-void exhalbtc92e_scan_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 scan_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_scan_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (type)
-		scan_type = BTC_SCAN_START;
-	else
-		scan_type = BTC_SCAN_FINISH;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_scan_notify(btcoexist, scan_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_scan_notify(btcoexist, scan_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_scan_notify(btcoexist, scan_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_scan_notify(btcoexist, scan_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_scan_notify(btcoexist, scan_type);
-	}
-}
-
-void exhalbtc92e_connect_notify(struct btc_coexist *btcoexist, u8 action)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 asso_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_connect_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (action)
-		asso_type = BTC_ASSOCIATE_START;
-	else
-		asso_type = BTC_ASSOCIATE_FINISH;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_connect_notify(btcoexist,
-							     asso_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_connect_notify(btcoexist, asso_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_connect_notify(btcoexist, asso_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_connect_notify(btcoexist,
-							  asso_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_connect_notify(btcoexist,
-							  asso_type);
-	}
-}
-
-void exhalbtc92e_mediastatus_notify(struct btc_coexist *btcoexist,
-				    enum rt_media_status media_status)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 status;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_media_status_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (RT_MEDIA_CONNECT == media_status)
-		status = BTC_MEDIA_CONNECT;
-	else
-		status = BTC_MEDIA_DISCONNECT;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_media_status_notify(btcoexist,
-								  status);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_media_status_notify(btcoexist,
-							       status);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_media_status_notify(btcoexist, status);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_media_status_notify(btcoexist,
-							       status);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_media_status_notify(btcoexist,
-							       status);
-	}
-}
-
-void exhalbtc92e_special_packet_notify(struct btc_coexist *btcoexist,
-				       u8 pkt_type)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 packet_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_special_packet_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (PACKET_DHCP == pkt_type) {
-		packet_type = BTC_PACKET_DHCP;
-	} else if (PACKET_EAPOL == pkt_type) {
-		packet_type = BTC_PACKET_EAPOL;
-	} else if (PACKET_ARP == pkt_type) {
-		packet_type = BTC_PACKET_ARP;
-	} else {
-		packet_type = BTC_PACKET_UNKNOWN;
-		return;
-	}
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_special_packet_notify(btcoexist,
-								    packet_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_special_packet_notify(btcoexist,
-								 packet_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_special_packet_notify(btcoexist,
-								 packet_type);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_special_packet_notify(btcoexist,
-								 packet_type);
-	}
-}
-
-void exhalbtc92e_bt_info_notify(struct btc_coexist *btcoexist,
-				u8 *tmp_buf, u8 length)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_bt_info_notify++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_bt_info_notify(btcoexist,
-							     tmp_buf, length);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_bt_info_notify(btcoexist, tmp_buf,
-							  length);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		/* ex_halbtc8192e2ant_bt_info_notify(btcoexist,
-						     tmp_buf, length); */
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_bt_info_notify(btcoexist,
-							  tmp_buf, length);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_bt_info_notify(btcoexist,
-							  tmp_buf, length);
-	}
-}
-
-void exhalbtc92e_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	u8 stack_op_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_stack_operation_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if ((HCI_BT_OP_INQUIRY_START == type) ||
-	    (HCI_BT_OP_PAGING_START == type) ||
-	    (HCI_BT_OP_PAIRING_START == type))
-		stack_op_type = BTC_STACK_OP_INQ_PAGE_PAIR_START;
-	else if ((HCI_BT_OP_INQUIRY_FINISH == type) ||
-		 (HCI_BT_OP_PAGING_SUCCESS == type) ||
-		 (HCI_BT_OP_PAGING_UNSUCCESS == type) ||
-		 (HCI_BT_OP_PAIRING_FINISH == type))
-		stack_op_type = BTC_STACK_OP_INQ_PAGE_PAIR_FINISH;
-	else
-		stack_op_type = BTC_STACK_OP_NONE;
-}
-
-void exhalbtc92e_halt_notify(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->binded = false;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_halt_notify(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_halt_notify(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_halt_notify(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_halt_notify(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_halt_notify(btcoexist);
-	}
-}
-
-void exhalbtc92e_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_pnp_notify(btcoexist, pnp_state);
-	}
-}
-
-void exhalbtc_coex_dm_switch(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_coex_dm_switch++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 1) {
-			btcoexist->stop_coex_dm = true;
-			ex_halbtc8723b1ant_coex_dm_reset(btcoexist);
-			exhalbtc92e_set_ant_num(BT_COEX_ANT_TYPE_DETECTED, 2);
-			ex92e_halbtc8723b2ant_init_hwconfig(btcoexist);
-			ex92e_halbtc8723b2ant_init_coex_dm(btcoexist);
-			btcoexist->stop_coex_dm = false;
-		}
-	}
-}
-
-void exhalbtc92e_periodical(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_periodical++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex92e_halbtc8723b2ant_periodical(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_periodical(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_periodical(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_periodical(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			if (!halbtc_under_ips(btcoexist))
-				ex_halbtc8821a1ant_periodical(btcoexist);
-	}
-}
-
-void exhalbtc92e_dbg_control(struct btc_coexist *btcoexist,
-			     u8 code, u8 len, u8 *data)
-{
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_dbg_ctrl++;
-}
-
-void exhalbtc92e_stack_update_profile_info(void)
-{
-}
-
-void exhalbtc92e_update_min_bt_rssi(char bt_rssi)
-{
-	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->stack_info.min_bt_rssi = bt_rssi;
-}
-
-
-void exhalbtc92e_set_hci_version(u16 hci_version)
-{
-	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->stack_info.hci_version = hci_version;
-}
-
-void exhalbtc92e_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version)
-{
-	struct btc_coexist *btcoexist = &gl92e_bt_coexist;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->bt_info.bt_real_fw_ver = bt_patch_version;
-	btcoexist->bt_info.bt_hci_ver = bt_hci_version;
-}
-
-void exhalbtc92e_set_bt_exist(bool bt_exist)
-{
-	gl92e_bt_coexist.board_info.bt_exist = bt_exist;
-}
-
-void exhalbtc92e_set_chip_type(u8 chip_type)
-{
-	switch (chip_type) {
-	default:
-	case BT_2WIRE:
-	case BT_ISSC_3WIRE:
-	case BT_ACCEL:
-	case BT_RTL8756:
-		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_UNDEF;
-		break;
-	case BT_CSR_BC4:
-		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC4;
-		break;
-	case BT_CSR_BC8:
-		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC8;
-		break;
-	case BT_RTL8723A:
-		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723A;
-		break;
-	case BT_RTL8821A:
-		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8821;
-		break;
-	case BT_RTL8723B:
-		gl92e_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723B;
-		break;
-	}
-}
-
-void exhalbtc92e_set_ant_num(u8 type, u8 ant_num)
-{
-	if (BT_COEX_ANT_TYPE_PG == type) {
-		gl92e_bt_coexist.board_info.pg_ant_num = ant_num;
-		gl92e_bt_coexist.board_info.btdm_ant_num = ant_num;
-		/* The antenna position:
-		 * Main (default) or Aux for pgAntNum = 2 && btdmAntNum = 1.
-		 * The antenna position should be determined by
-		 * auto-detect mechanism.
-		 * The following is assumed to main,
-		 * and those must be modified
-		 * if y auto-detect mechanism is ready
-		 */
-		if ((gl92e_bt_coexist.board_info.pg_ant_num == 2) &&
-		    (gl92e_bt_coexist.board_info.btdm_ant_num == 1))
-			gl92e_bt_coexist.board_info.btdm_ant_pos =
-						       BTC_ANTENNA_AT_MAIN_PORT;
-		else
-			gl92e_bt_coexist.board_info.btdm_ant_pos =
-						       BTC_ANTENNA_AT_MAIN_PORT;
-	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
-		gl92e_bt_coexist.board_info.btdm_ant_num = ant_num;
-		gl92e_bt_coexist.board_info.btdm_ant_pos =
-						       BTC_ANTENNA_AT_MAIN_PORT;
-	} else if (BT_COEX_ANT_TYPE_DETECTED == type) {
-		gl92e_bt_coexist.board_info.btdm_ant_num = ant_num;
-		gl92e_bt_coexist.board_info.btdm_ant_pos =
-						       BTC_ANTENNA_AT_MAIN_PORT;
-	}
-}
-
-void exhalbtc92e_display_bt_coex_info(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant92e_display_coex_info(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_display_coex_info(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8821a2ant_display_coex_info(btcoexist);
-		else if (btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8821a1ant_display_coex_info(btcoexist);
-	}
-}
diff --git a/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.h b/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.h
deleted file mode 100644
index 1231b16205f2..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/halbtcoutsrc.h
+++ /dev/null
@@ -1,537 +0,0 @@
-#ifndef	__HALBTC_OUT_SRC_H__
-#define __HALBTC_OUT_SRC_H__
-
-#include	"../wifi.h"
-
-#define		NORMAL_EXEC				false
-#define		FORCE_EXEC				true
-
-#define		BTC_RF_A				RF90_PATH_A
-#define		BTC_RF_B				RF90_PATH_B
-#define		BTC_RF_C				RF90_PATH_C
-#define		BTC_RF_D				RF90_PATH_D
-
-#define		BTC_SMSP				SINGLEMAC_SINGLEPHY
-#define		BTC_DMDP				DUALMAC_DUALPHY
-#define		BTC_DMSP				DUALMAC_SINGLEPHY
-#define		BTC_MP_UNKNOWN				0xff
-
-#define		IN
-#define		OUT
-
-#define		BT_TMP_BUF_SIZE				100
-
-#define		BT_COEX_ANT_TYPE_PG			0
-#define		BT_COEX_ANT_TYPE_ANTDIV			1
-#define		BT_COEX_ANT_TYPE_DETECTED		2
-
-#define		BTC_MIMO_PS_STATIC			0
-#define		BTC_MIMO_PS_DYNAMIC			1
-
-#define		BTC_RATE_DISABLE			0
-#define		BTC_RATE_ENABLE				1
-
-/* single Antenna definition */
-#define		BTC_ANT_PATH_WIFI			0
-#define		BTC_ANT_PATH_BT				1
-#define		BTC_ANT_PATH_PTA			2
-/* dual Antenna definition */
-#define		BTC_ANT_WIFI_AT_MAIN			0
-#define		BTC_ANT_WIFI_AT_AUX			1
-/* coupler Antenna definition */
-#define		BTC_ANT_WIFI_AT_CPL_MAIN		0
-#define		BTC_ANT_WIFI_AT_CPL_AUX			1
-
-enum btc_chip_interface {
-	BTC_INTF_UNKNOWN	= 0,
-	BTC_INTF_PCI		= 1,
-	BTC_INTF_USB		= 2,
-	BTC_INTF_SDIO		= 3,
-	BTC_INTF_GSPI		= 4,
-	BTC_INTF_MAX
-};
-
-enum btc_chip_type {
-	BTC_CHIP_UNDEF		= 0,
-	BTC_CHIP_CSR_BC4	= 1,
-	BTC_CHIP_CSR_BC8	= 2,
-	BTC_CHIP_RTL8723A	= 3,
-	BTC_CHIP_RTL8821	= 4,
-	BTC_CHIP_RTL8723B	= 5,
-	BTC_CHIP_MAX
-};
-
-enum btc_msg_type {
-	BTC_MSG_INTERFACE	= 0x0,
-	BTC_MSG_ALGORITHM	= 0x1,
-	BTC_MSG_MAX
-};
-
-extern u32 btc_92edbg_type[];
-
-/* following is for BTC_MSG_INTERFACE */
-#define		INTF_INIT				BIT(0)
-#define		INTF_NOTIFY				BIT(2)
-
-/* following is for BTC_ALGORITHM */
-#define		ALGO_BT_RSSI_STATE			BIT(0)
-#define		ALGO_WIFI_RSSI_STATE			BIT(1)
-#define		ALGO_BT_MONITOR				BIT(2)
-#define		ALGO_TRACE				BIT(3)
-#define		ALGO_TRACE_FW				BIT(4)
-#define		ALGO_TRACE_FW_DETAIL			BIT(5)
-#define		ALGO_TRACE_FW_EXEC			BIT(6)
-#define		ALGO_TRACE_SW				BIT(7)
-#define		ALGO_TRACE_SW_DETAIL			BIT(8)
-#define		ALGO_TRACE_SW_EXEC			BIT(9)
-
-/* following is for wifi link status */
-#define		WIFI_STA_CONNECTED			BIT(0)
-#define		WIFI_AP_CONNECTED			BIT(1)
-#define		WIFI_HS_CONNECTED			BIT(2)
-#define		WIFI_P2P_GO_CONNECTED			BIT(3)
-#define		WIFI_P2P_GC_CONNECTED			BIT(4)
-
-
-#define	CL_SPRINTF	snprintf
-#define	CL_PRINTF(buf)	printk("%s", buf)
-
-#define	BTC_PRINT(dbgtype, dbgflag, printstr, ...)		\
-	do {							\
-		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {\
-			pr_debug(printstr, ##__VA_ARGS__);	\
-		}						\
-	} while (0)
-
-#define	BTC_PRINT_F(dbgtype, dbgflag, printstr, ...)		\
-	do {							\
-		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {\
-			pr_debug("%s: ", __func__);	\
-			pr_cont(printstr, ##__VA_ARGS__);	\
-		}						\
-	} while (0)
-
-#define	BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _ptr)	\
-	do {							\
-		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {	\
-			int __i;				\
-			u8 *__ptr = (u8 *)_ptr;			\
-			pr_debug printstr;			\
-			for (__i = 0; __i < 6; __i++)		\
-				pr_cont("%02X%s", __ptr[__i],	\
-					(__i == 5) ? "" : "-");	\
-			pr_debug("\n");				\
-		}						\
-	} while (0)
-
-#define BTC_PRINT_DATA(dbgtype, dbgflag, _titlestring, _hexdata, _hexdatalen) \
-	do {								\
-		if (unlikely(btc_92edbg_type[dbgtype] & dbgflag)) {	\
-			int __i;					\
-			u8 *__ptr = (u8 *)_hexdata;			\
-			pr_debug(_titlestring);				\
-			for (__i = 0; __i < (int)_hexdatalen; __i++) {	\
-				pr_cont("%02X%s", __ptr[__i], (((__i + 1) % 4) \
-							== 0) ? "  " : " ");\
-				if (((__i + 1) % 16) == 0)		\
-					pr_cont("\n");			\
-			}						\
-			pr_debug("\n");			\
-		}							\
-	} while (0)
-
-
-#define	BTC_RSSI_HIGH(_rssi_)	\
-	((_rssi_ == BTC_RSSI_STATE_HIGH ||	\
-	  _rssi_ == BTC_RSSI_STATE_STAY_HIGH) ? true : false)
-#define	BTC_RSSI_MEDIUM(_rssi_)	\
-	((_rssi_ == BTC_RSSI_STATE_MEDIUM ||	\
-	  _rssi_ == BTC_RSSI_STATE_STAY_MEDIUM) ? true : false)
-#define	BTC_RSSI_LOW(_rssi_)	\
-	((_rssi_ == BTC_RSSI_STATE_LOW ||	\
-	  _rssi_ == BTC_RSSI_STATE_STAY_LOW) ? true : false)
-
-
-enum btc_power_save_type {
-	BTC_PS_WIFI_NATIVE = 0,
-	BTC_PS_LPS_ON = 1,
-	BTC_PS_LPS_OFF = 2,
-	BTC_PS_LPS_MAX
-};
-
-struct btc_board_info {
-	/* The following is some board information */
-	u8 bt_chip_type;
-	u8 pg_ant_num;	/* pg ant number */
-	u8 btdm_ant_num;	/* ant number for btdm */
-	u8 btdm_ant_pos;
-	bool bt_exist;
-};
-
-enum btc_dbg_opcode {
-	BTC_DBG_SET_COEX_NORMAL = 0x0,
-	BTC_DBG_SET_COEX_WIFI_ONLY = 0x1,
-	BTC_DBG_SET_COEX_BT_ONLY = 0x2,
-	BTC_DBG_MAX
-};
-
-enum btc_rssi_state {
-	BTC_RSSI_STATE_HIGH = 0x0,
-	BTC_RSSI_STATE_MEDIUM = 0x1,
-	BTC_RSSI_STATE_LOW = 0x2,
-	BTC_RSSI_STATE_STAY_HIGH = 0x3,
-	BTC_RSSI_STATE_STAY_MEDIUM = 0x4,
-	BTC_RSSI_STATE_STAY_LOW = 0x5,
-	BTC_RSSI_MAX
-};
-
-enum btc_wifi_role {
-	BTC_ROLE_STATION = 0x0,
-	BTC_ROLE_AP = 0x1,
-	BTC_ROLE_IBSS = 0x2,
-	BTC_ROLE_HS_MODE = 0x3,
-	BTC_ROLE_MAX
-};
-
-enum btc_wifi_bw_mode {
-	BTC_WIFI_BW_LEGACY = 0x0,
-	BTC_WIFI_BW_HT20 = 0x1,
-	BTC_WIFI_BW_HT40 = 0x2,
-	BTC_WIFI_BW_MAX
-};
-
-enum btc_wifi_traffic_dir {
-	BTC_WIFI_TRAFFIC_TX = 0x0,
-	BTC_WIFI_TRAFFIC_RX = 0x1,
-	BTC_WIFI_TRAFFIC_MAX
-};
-
-enum btc_wifi_pnp {
-	BTC_WIFI_PNP_WAKE_UP = 0x0,
-	BTC_WIFI_PNP_SLEEP = 0x1,
-	BTC_WIFI_PNP_MAX
-};
-
-
-enum btc_get_type {
-	/* type bool */
-	BTC_GET_BL_HS_OPERATION,
-	BTC_GET_BL_HS_CONNECTING,
-	BTC_GET_BL_WIFI_CONNECTED,
-	BTC_GET_BL_WIFI_BUSY,
-	BTC_GET_BL_WIFI_SCAN,
-	BTC_GET_BL_WIFI_LINK,
-	BTC_GET_BL_WIFI_DHCP,
-	BTC_GET_BL_WIFI_SOFTAP_IDLE,
-	BTC_GET_BL_WIFI_SOFTAP_LINKING,
-	BTC_GET_BL_WIFI_IN_EARLY_SUSPEND,
-	BTC_GET_BL_WIFI_ROAM,
-	BTC_GET_BL_WIFI_4_WAY_PROGRESS,
-	BTC_GET_BL_WIFI_UNDER_5G,
-	BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-	BTC_GET_BL_WIFI_ENABLE_ENCRYPTION,
-	BTC_GET_BL_WIFI_UNDER_B_MODE,
-	BTC_GET_BL_EXT_SWITCH,
-
-	/* type s4Byte */
-	BTC_GET_S4_WIFI_RSSI,
-	BTC_GET_S4_HS_RSSI,
-
-	/* type u32 */
-	BTC_GET_U4_WIFI_BW,
-	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
-	BTC_GET_U4_WIFI_FW_VER,
-	BTC_GET_U4_WIFI_LINK_STATUS,
-	BTC_GET_U4_BT_PATCH_VER,
-
-	/* type u1Byte */
-	BTC_GET_U1_WIFI_DOT11_CHNL,
-	BTC_GET_U1_WIFI_CENTRAL_CHNL,
-	BTC_GET_U1_WIFI_HS_CHNL,
-	BTC_GET_U1_MAC_PHY_MODE,
-	BTC_GET_U1_AP_NUM,
-
-	/* for 1Ant */
-	BTC_GET_U1_LPS_MODE,
-	BTC_GET_BL_BT_SCO_BUSY,
-
-	/* for test mode */
-	BTC_GET_DRIVER_TEST_CFG,
-	BTC_GET_MAX
-};
-
-
-enum btc_set_type {
-	/* type bool */
-	BTC_SET_BL_BT_DISABLE,
-	BTC_SET_BL_BT_TRAFFIC_BUSY,
-	BTC_SET_BL_BT_LIMITED_DIG,
-	BTC_SET_BL_FORCE_TO_ROAM,
-	BTC_SET_BL_TO_REJ_AP_AGG_PKT,
-	BTC_SET_BL_BT_CTRL_AGG_SIZE,
-	BTC_SET_BL_INC_SCAN_DEV_NUM,
-
-	/* type u1Byte */
-	BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
-	BTC_SET_U1_AGG_BUF_SIZE,
-
-	/* type trigger some action */
-	BTC_SET_ACT_GET_BT_RSSI,
-	BTC_SET_ACT_AGGREGATE_CTRL,
-
-	/********* for 1Ant **********/
-	/* type bool */
-	BTC_SET_BL_BT_SCO_BUSY,
-	/* type u1Byte */
-	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
-	BTC_SET_U1_LPS_VAL,
-	BTC_SET_U1_RPWM_VAL,
-	BTC_SET_U1_1ANT_LPS,
-	BTC_SET_U1_1ANT_RPWM,
-	/* type trigger some action */
-	BTC_SET_ACT_LEAVE_LPS,
-	BTC_SET_ACT_ENTER_LPS,
-	BTC_SET_ACT_NORMAL_LPS,
-	BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT,
-	BTC_SET_ACT_DISABLE_LOW_POWER,
-	BTC_SET_ACT_UPDATE_ra_mask,
-	BTC_SET_ACT_SEND_MIMO_PS,
-	/* BT Coex related */
-	BTC_SET_ACT_CTRL_BT_INFO,
-	BTC_SET_ACT_CTRL_BT_COEX,
-	/***************************/
-	BTC_SET_MAX
-};
-
-enum btc_dbg_disp_type {
-	BTC_DBG_DISP_COEX_STATISTICS = 0x0,
-	BTC_DBG_DISP_BT_LINK_INFO = 0x1,
-	BTC_DBG_DISP_BT_FW_VER = 0x2,
-	BTC_DBG_DISP_FW_PWR_MODE_CMD = 0x3,
-	BTC_DBG_DISP_MAX
-};
-
-enum btc_notify_type_ips {
-	BTC_IPS_LEAVE = 0x0,
-	BTC_IPS_ENTER = 0x1,
-	BTC_IPS_MAX
-};
-
-enum btc_notify_type_lps {
-	BTC_LPS_DISABLE = 0x0,
-	BTC_LPS_ENABLE = 0x1,
-	BTC_LPS_MAX
-};
-
-enum btc_notify_type_scan {
-	BTC_SCAN_FINISH = 0x0,
-	BTC_SCAN_START = 0x1,
-	BTC_SCAN_MAX
-};
-
-enum btc_notify_type_associate {
-	BTC_ASSOCIATE_FINISH = 0x0,
-	BTC_ASSOCIATE_START = 0x1,
-	BTC_ASSOCIATE_MAX
-};
-
-enum btc_notify_type_media_status {
-	BTC_MEDIA_DISCONNECT = 0x0,
-	BTC_MEDIA_CONNECT = 0x1,
-	BTC_MEDIA_MAX
-};
-
-enum btc_notify_type_special_packet {
-	BTC_PACKET_UNKNOWN = 0x0,
-	BTC_PACKET_DHCP = 0x1,
-	BTC_PACKET_ARP = 0x2,
-	BTC_PACKET_EAPOL = 0x3,
-	BTC_PACKET_MAX
-};
-
-enum hci_ext_bt_operation {
-	HCI_BT_OP_NONE = 0x0,
-	HCI_BT_OP_INQUIRY_START = 0x1,
-	HCI_BT_OP_INQUIRY_FINISH = 0x2,
-	HCI_BT_OP_PAGING_START = 0x3,
-	HCI_BT_OP_PAGING_SUCCESS = 0x4,
-	HCI_BT_OP_PAGING_UNSUCCESS = 0x5,
-	HCI_BT_OP_PAIRING_START = 0x6,
-	HCI_BT_OP_PAIRING_FINISH = 0x7,
-	HCI_BT_OP_BT_DEV_ENABLE = 0x8,
-	HCI_BT_OP_BT_DEV_DISABLE = 0x9,
-	HCI_BT_OP_MAX
-};
-
-enum btc_notify_type_stack_operation {
-	BTC_STACK_OP_NONE = 0x0,
-	BTC_STACK_OP_INQ_PAGE_PAIR_START = 0x1,
-	BTC_STACK_OP_INQ_PAGE_PAIR_FINISH = 0x2,
-	BTC_STACK_OP_MAX
-};
-
-
-struct btc_bt_info {
-	bool bt_disabled;
-	u8 rssi_adjust_for_agc_table_on;
-	u8 rssi_adjust_for_1ant_coex_type;
-	bool bt_busy;
-	u8 agg_buf_size;
-	bool limited_dig;
-	bool reject_agg_pkt;
-	bool b_bt_ctrl_buf_size;
-	bool increase_scan_dev_num;
-	u16 bt_hci_ver;
-	u16 bt_real_fw_ver;
-	u8 bt_fw_ver;
-
-	bool bt_disable_low_pwr;
-
-	/* the following is for 1Ant solution */
-	bool bt_ctrl_lps;
-	bool bt_pwr_save_mode;
-	bool bt_lps_on;
-	bool force_to_roam;
-	u8 force_exec_pwr_cmd_cnt;
-	u8 lps_val;
-	u8 rpwm_val;
-	u32 ra_mask;
-};
-
-struct btc_stack_info {
-	bool profile_notified;
-	u16 hci_version;	/* stack hci version */
-	u8 num_of_link;
-	bool bt_link_exist;
-	bool sco_exist;
-	bool acl_exist;
-	bool a2dp_exist;
-	bool hid_exist;
-	u8 num_of_hid;
-	bool pan_exist;
-	bool unknown_acl_exist;
-	char min_bt_rssi;
-};
-
-struct btc_statistics {
-	u32 cnt_bind;
-	u32 cnt_init_hw_config;
-	u32 cnt_init_coex_dm;
-	u32 cnt_ips_notify;
-	u32 cnt_lps_notify;
-	u32 cnt_scan_notify;
-	u32 cnt_connect_notify;
-	u32 cnt_media_status_notify;
-	u32 cnt_special_packet_notify;
-	u32 cnt_bt_info_notify;
-	u32 cnt_periodical;
-	u32 cnt_coex_dm_switch;
-	u32 cnt_stack_operation_notify;
-	u32 cnt_dbg_ctrl;
-};
-
-struct btc_bt_link_info {
-	bool bt_link_exist;
-	bool sco_exist;
-	bool sco_only;
-	bool a2dp_exist;
-	bool a2dp_only;
-	bool hid_exist;
-	bool hid_only;
-	bool pan_exist;
-	bool pan_only;
-};
-
-enum btc_antenna_pos {
-	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
-	BTC_ANTENNA_AT_AUX_PORT = 0x2,
-};
-
-struct btc_coexist {
-	/* make sure only one adapter can bind the data context  */
-	bool binded;
-	/* default adapter */
-	void *adapter;
-	struct btc_board_info board_info;
-	/* some bt info referenced by non-bt module */
-	struct btc_bt_info bt_info;
-	struct btc_stack_info stack_info;
-	enum btc_chip_interface	chip_interface;
-	struct btc_bt_link_info bt_link_info;
-
-	bool initilized;
-	bool stop_coex_dm;
-	bool manual_control;
-	u8 *cli_buf;
-	struct btc_statistics statistics;
-	u8 pwr_mode_val[10];
-
-	/* function pointers io related */
-	u8 (*btc_read_1byte)(void *btc_context, u32 reg_addr);
-	void (*btc_write_1byte)(void *btc_context, u32 reg_addr, u8 data);
-	void (*btc_write_1byte_bitmask)(void *btc_context, u32 reg_addr,
-					u8 bit_mask, u8 data1b);
-	u16 (*btc_read_2byte)(void *btc_context, u32 reg_addr);
-	void (*btc_write_2byte)(void *btc_context, u32 reg_addr, u16 data);
-	u32 (*btc_read_4byte)(void *btc_context, u32 reg_addr);
-	void (*btc_write_4byte)(void *btc_context, u32 reg_addr, u32 data);
-
-	void (*btc_set_bb_reg)(void *btc_context, u32 reg_addr,
-			       u32 bit_mask, u32 data);
-	u32 (*btc_get_bb_reg)(void *btc_context, u32 reg_addr,
-			      u32 bit_mask);
-
-	void (*btc_set_rf_reg)(void *btc_context, u8 rf_path, u32 reg_addr,
-			       u32 bit_mask, u32 data);
-	u32 (*btc_get_rf_reg)(void *btc_context, u8 rf_path,
-			      u32 reg_addr, u32 bit_mask);
-
-
-	void (*btc_fill_h2c)(void *btc_context, u8 element_id,
-			     u32 cmd_len, u8 *cmd_buffer);
-
-	void (*btc_disp_dbg_msg)(void *btcoexist, u8 disp_type);
-
-	bool (*btc_get)(void *btcoexist, u8 get_type, void *out_buf);
-	bool (*btc_set)(void *btcoexist, u8 set_type, void *in_buf);
-};
-
-
-bool halbtc92e_is_wifi_uplink(struct rtl_priv *adapter);
-
-
-extern struct btc_coexist gl92e_bt_coexist;
-
-bool exhalbtc92e_initlize_variables(struct rtl_priv *adapter);
-void exhalbtc92e_init_hw_config(struct btc_coexist *btcoexist);
-void exhalbtc92e_init_coex_dm(struct btc_coexist *btcoexist);
-void exhalbtc92e_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc92e_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc92e_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc92e_connect_notify(struct btc_coexist *btcoexist, u8 action);
-void exhalbtc92e_mediastatus_notify(struct btc_coexist *btcoexist,
-				    enum rt_media_status media_status);
-void exhalbtc92e_special_packet_notify(struct btc_coexist *btcoexist,
-				       u8 pkt_type);
-void exhalbtc92e_bt_info_notify(struct btc_coexist *btcoexist, u8 *tmp_buf,
-				u8 length);
-void exhalbtc92e_stack_operation_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc92e_halt_notify(struct btc_coexist *btcoexist);
-void exhalbtc92e_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
-void exhalbtc_coex_dm_switch(struct btc_coexist *btcoexist);
-void exhalbtc92e_periodical(struct btc_coexist *btcoexist);
-void exhalbtc92e_dbg_control(struct btc_coexist *btcoexist, u8 code, u8 len,
-			     u8 *data);
-void exhalbtc92e_stack_update_profile_info(void);
-void exhalbtc92e_set_hci_version(u16 hci_version);
-void exhalbtc92e_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version);
-void exhalbtc92e_update_min_bt_rssi(char bt_rssi);
-void exhalbtc92e_set_bt_exist(bool bt_exist);
-void exhalbtc92e_set_chip_type(u8 chip_type);
-void exhalbtc92e_set_ant_num(u8 type, u8 ant_num);
-void exhalbtc92e_display_bt_coex_info(struct btc_coexist *btcoexist);
-void exhalbtc_signal_compensation(struct btc_coexist *btcoexist,
-				  u8 *rssi_wifi, u8 *rssi_bt);
-void exhalbtc_lps_leave(struct btc_coexist *btcoexist);
-void exhalbtc_low_wifi_traffic_notify(struct btc_coexist *btcoexist);
-#endif
diff --git a/drivers/staging/rtl8192ee/btcoexist/rtl_btc.c b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.c
deleted file mode 100644
index 50c012a4c174..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/rtl_btc.c
+++ /dev/null
@@ -1,194 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include <linux/vmalloc.h>
-#include <linux/module.h>
-
-#include "rtl_btc.h"
-#include "halbt_precomp.h"
-
-static struct rtl_btc_ops rtl_btc_operation = {
-	.btc_init_variables = rtl92e_btc_init_variables,
-	.btc_init_hal_vars = rtl92e_btc_init_hal_vars,
-	.btc_init_hw_config = rtl92e_btc_init_hw_config,
-	.btc_ips_notify = rtl92e_btc_ips_notify,
-	.btc_lps_notify = rtl_btc_lps_notify,
-	.btc_scan_notify = rtl92e_btc_scan_notify,
-	.btc_connect_notify = rtl92e_btc_connect_notify,
-	.btc_mediastatus_notify = rtl92e_btc_mediastatus_notify,
-	.btc_periodical = rtl92e_btc_periodical,
-	.btc_halt_notify = rtl92e_btc_halt_notify,
-	.btc_btinfo_notify = rtl92e_btc_btinfo_notify,
-	.btc_is_limited_dig = rtl92e_btc_is_limited_dig,
-	.btc_is_disable_edca_turbo = rtl92e_btc_is_disable_edca_turbo,
-	.btc_is_bt_disabled = rtl92e_btc_is_bt_disabled,
-	.btc_special_packet_notify = rtl_btc_special_packet_notify,
-};
-
-void rtl92e_btc_init_variables(struct rtl_priv *rtlpriv)
-{
-	exhalbtc92e_initlize_variables(rtlpriv);
-}
-
-void rtl92e_btc_init_hal_vars(struct rtl_priv *rtlpriv)
-{
-	u8 ant_num;
-	u8 bt_exist;
-	u8 bt_type;
-	ant_num = rtl92e_get_hwpg_ant_num(rtlpriv);
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("%s, antNum is %d\n", __func__, ant_num));
-
-	bt_exist = rtl92e_get_hwpg_bt_exist(rtlpriv);
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("%s, bt_exist is %d\n", __func__, bt_exist));
-	exhalbtc92e_set_bt_exist(bt_exist);
-
-	bt_type = rtl92e_get_hwpg_bt_type(rtlpriv);
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("%s, bt_type is %d\n", __func__, bt_type));
-	exhalbtc92e_set_chip_type(bt_type);
-
-	exhalbtc92e_set_ant_num(BT_COEX_ANT_TYPE_PG, ant_num);
-}
-
-void rtl92e_btc_init_hw_config(struct rtl_priv *rtlpriv)
-{
-	exhalbtc92e_init_hw_config(&gl92e_bt_coexist);
-	exhalbtc92e_init_coex_dm(&gl92e_bt_coexist);
-}
-
-void rtl92e_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type)
-{
-	exhalbtc92e_ips_notify(&gl92e_bt_coexist, type);
-}
-
-void rtl_btc_lps_notify(struct rtl_priv *rtlpriv, u8 type)
-{
-	exhalbtc92e_lps_notify(&gl92e_bt_coexist, type);
-}
-
-void rtl92e_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype)
-{
-	exhalbtc92e_scan_notify(&gl92e_bt_coexist, scantype);
-}
-
-void rtl92e_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action)
-{
-	exhalbtc92e_connect_notify(&gl92e_bt_coexist, action);
-}
-
-void rtl92e_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
-				   enum rt_media_status mstatus)
-{
-	exhalbtc92e_mediastatus_notify(&gl92e_bt_coexist, mstatus);
-}
-
-void rtl92e_btc_periodical(struct rtl_priv *rtlpriv)
-{
-	exhalbtc92e_periodical(&gl92e_bt_coexist);
-}
-
-void rtl92e_btc_halt_notify(void)
-{
-	exhalbtc92e_halt_notify(&gl92e_bt_coexist);
-}
-
-void rtl92e_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmp_buf, u8 length)
-{
-	exhalbtc92e_bt_info_notify(&gl92e_bt_coexist, tmp_buf, length);
-}
-
-bool rtl92e_btc_is_limited_dig(struct rtl_priv *rtlpriv)
-{
-	return gl92e_bt_coexist.bt_info.limited_dig;
-}
-
-bool rtl92e_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv)
-{
-	bool bt_change_edca = false;
-	u32 cur_edca_val;
-	u32 edca_bt_hs_uplink = 0x5ea42b, edca_bt_hs_downlink = 0x5ea42b;
-	u32 edca_hs;
-	u32 edca_addr = 0x504;
-
-	cur_edca_val = rtl_read_dword(rtlpriv, edca_addr);
-	if (halbtc92e_is_wifi_uplink(rtlpriv)) {
-		if (cur_edca_val != edca_bt_hs_uplink) {
-			edca_hs = edca_bt_hs_uplink;
-			bt_change_edca = true;
-		}
-	} else {
-		if (cur_edca_val != edca_bt_hs_downlink) {
-			edca_hs = edca_bt_hs_downlink;
-			bt_change_edca = true;
-		}
-	}
-
-	if (bt_change_edca)
-		rtl_write_dword(rtlpriv, edca_addr, edca_hs);
-
-	return true;
-}
-
-bool rtl92e_btc_is_bt_disabled(struct rtl_priv *rtlpriv)
-{
-	/* It seems 'bt_disabled' is never be initialized or set. */
-	if (gl92e_bt_coexist.bt_info.bt_disabled)
-		return true;
-	else
-		return false;
-}
-
-void rtl_btc_special_packet_notify(struct rtl_priv *rtlpriv, u8 pkt_type)
-{
-	return exhalbtc92e_special_packet_notify(&gl92e_bt_coexist, pkt_type);
-}
-
-struct rtl_btc_ops *stg_rtl_btc_get_ops_pointer(void)
-{
-	return &rtl_btc_operation;
-}
-EXPORT_SYMBOL(stg_rtl_btc_get_ops_pointer);
-
-u8 rtl92e_get_hwpg_ant_num(struct rtl_priv *rtlpriv)
-{
-	u8 num;
-
-	if (rtlpriv->btcoexist.btc_info.ant_num == ANT_X2)
-		num = 2;
-	else
-		num = 1;
-
-	return num;
-}
-u8 rtl92e_get_hwpg_bt_exist(struct rtl_priv *rtlpriv)
-{
-	return rtlpriv->btcoexist.btc_info.btcoexist;
-}
-
-u8 rtl92e_get_hwpg_bt_type(struct rtl_priv *rtlpriv)
-{
-	return rtlpriv->btcoexist.btc_info.bt_type;
-}
diff --git a/drivers/staging/rtl8192ee/btcoexist/rtl_btc.h b/drivers/staging/rtl8192ee/btcoexist/rtl_btc.h
deleted file mode 100644
index 9530eb1a6649..000000000000
--- a/drivers/staging/rtl8192ee/btcoexist/rtl_btc.h
+++ /dev/null
@@ -1,62 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_BTC_H__
-#define __RTL_BTC_H__
-
-#include "halbt_precomp.h"
-
-void rtl92e_btc_init_variables(struct rtl_priv *rtlpriv);
-void rtl92e_btc_init_hal_vars(struct rtl_priv *rtlpriv);
-void rtl92e_btc_init_hw_config(struct rtl_priv *rtlpriv);
-void rtl92e_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type);
-void rtl_btc_lps_notify(struct rtl_priv *rtlpriv, u8 type);
-void rtl92e_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype);
-void rtl92e_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action);
-void rtl92e_btc_mediastatus_notify(struct rtl_priv *rtlpriv,
-				   enum rt_media_status mstatus);
-void rtl92e_btc_periodical(struct rtl_priv *rtlpriv);
-void rtl92e_btc_halt_notify(void);
-void rtl92e_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 *tmpbuf, u8 length);
-bool rtl92e_btc_is_limited_dig(struct rtl_priv *rtlpriv);
-bool rtl92e_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv);
-bool rtl92e_btc_is_bt_disabled(struct rtl_priv *rtlpriv);
-void rtl_btc_special_packet_notify(struct rtl_priv *rtlpriv, u8 pkt_type);
-
-
-struct rtl_btc_ops *stg_rtl_btc_get_ops_pointer(void);
-
-u8 rtl92e_get_hwpg_ant_num(struct rtl_priv *rtlpriv);
-u8 rtl92e_get_hwpg_bt_exist(struct rtl_priv *rtlpriv);
-u8 rtl92e_get_hwpg_bt_type(struct rtl_priv *rtlpriv);
-enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw);
-
-
-
-
-
-
-
-
-#endif
diff --git a/drivers/staging/rtl8192ee/cam.c b/drivers/staging/rtl8192ee/cam.c
deleted file mode 100644
index e32c329da870..000000000000
--- a/drivers/staging/rtl8192ee/cam.c
+++ /dev/null
@@ -1,337 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include "wifi.h"
-#include "cam.h"
-
-void rtl92e_cam_reset_sec_info(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->sec.use_defaultkey = false;
-	rtlpriv->sec.pairwise_enc_algorithm = NO_ENCRYPTION;
-	rtlpriv->sec.group_enc_algorithm = NO_ENCRYPTION;
-	memset(rtlpriv->sec.key_buf, 0, KEY_BUF_SIZE * MAX_KEY_LEN);
-	memset(rtlpriv->sec.key_len, 0, KEY_BUF_SIZE);
-	rtlpriv->sec.pairwise_key = NULL;
-}
-
-static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
-				  u8 *mac_addr, u8 *key_cont_128, u16 us_config)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	u32 target_command;
-	u32 target_content = 0;
-	u8 entry_i;
-
-	RT_PRINT_DATA(rtlpriv, COMP_SEC, DBG_DMESG, "Key content:",
-		      key_cont_128, 16);
-
-	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-		target_command = entry_i + CAM_CONTENT_COUNT * entry_no;
-		target_command = target_command | BIT(31) | BIT(16);
-
-		if (entry_i == 0) {
-			target_content = (u32) (*(mac_addr + 0)) << 16 |
-			    (u32) (*(mac_addr + 1)) << 24 | (u32) us_config;
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
-					target_content);
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_command);
-
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE %x: %x\n",
-				  rtlpriv->cfg->maps[WCAMI], target_content));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("The Key ID is %d\n", entry_no));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE %x: %x\n",
-				  rtlpriv->cfg->maps[RWCAM], target_command));
-		} else if (entry_i == 1) {
-			target_content = (u32) (*(mac_addr + 5)) << 24 |
-			    (u32) (*(mac_addr + 4)) << 16 |
-			    (u32) (*(mac_addr + 3)) << 8 |
-			    (u32) (*(mac_addr + 2));
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
-					target_content);
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_command);
-
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A4: %x\n", target_content));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A0: %x\n", target_command));
-		} else {
-			target_content =
-			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 3)) <<
-			    24 | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 2))
-			    << 16 |
-			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 1)) << 8
-			    | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 0));
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
-					target_content);
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_command);
-			udelay(100);
-
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A4: %x\n", target_content));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A0: %x\n", target_command));
-		}
-	}
-
-	RT_TRACE(COMP_SEC, DBG_LOUD,
-		 ("after set key, usconfig:%x\n", us_config));
-}
-
-u8 stg_rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
-			     u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
-			     u32 ul_default_key, u8 *key_content)
-{
-	u32 us_config;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("EntryNo:%x, ulKeyId=%x, ulEncAlg=%x, ulUseDK=%x MacAddr %pM\n",
-		  ul_entry_idx, ul_key_id, ul_enc_alg,
-		  ul_default_key, mac_addr));
-
-	if (ul_key_id == TOTAL_CAM_ENTRY) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("ulKeyId exceed!\n"));
-		return 0;
-	}
-
-	if (ul_default_key == 1)
-		us_config = CFG_VALID | ((u16) (ul_enc_alg) << 2);
-	else
-		us_config = CFG_VALID | ((ul_enc_alg) << 2) | ul_key_id;
-
-	rtl_cam_program_entry(hw, ul_entry_idx, mac_addr,
-			      (u8 *)key_content, us_config);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG, ("end\n"));
-
-	return 1;
-}
-EXPORT_SYMBOL(stg_rtl_cam_add_one_entry);
-
-int stg_rtl_cam_delete_one_entry(struct ieee80211_hw *hw,
-				 u8 *mac_addr, u32 ul_key_id)
-{
-	u32 ul_command;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG, ("key_idx:%d\n", ul_key_id));
-
-	ul_command = ul_key_id * CAM_CONTENT_COUNT;
-	ul_command = ul_command | BIT(31) | BIT(16);
-
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], 0);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("stg_rtl_cam_delete_one_entry(): WRITE A4: %x\n", 0));
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("stg_rtl_cam_delete_one_entry(): WRITE A0: %x\n",
-		  ul_command));
-	return 0;
-}
-EXPORT_SYMBOL(stg_rtl_cam_delete_one_entry);
-
-void stg_rtl_cam_reset_all_entry(struct ieee80211_hw *hw)
-{
-	u32 ul_command;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	ul_command = BIT(31) | BIT(30);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-}
-EXPORT_SYMBOL(stg_rtl_cam_reset_all_entry);
-
-void stg_rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	u32 ul_command;
-	u32 ul_content;
-	u32 ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
-
-	switch (rtlpriv->sec.pairwise_enc_algorithm) {
-	case WEP40_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
-		break;
-	case WEP104_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
-		break;
-	case TKIP_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
-		break;
-	case AESCCMP_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
-		break;
-	default:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
-	}
-
-	ul_content = (uc_index & 3) | ((u16) (ul_enc_algo) << 2);
-
-	ul_content |= BIT(15);
-	ul_command = CAM_CONTENT_COUNT * uc_index;
-	ul_command = ul_command | BIT(31) | BIT(16);
-
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("stg_rtl_cam_mark_invalid(): WRITE A4: %x\n", ul_content));
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("stg_rtl_cam_mark_invalid(): WRITE A0: %x\n", ul_command));
-}
-EXPORT_SYMBOL(stg_rtl_cam_mark_invalid);
-
-void stg_rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	u32 ul_command;
-	u32 ul_content;
-	u32 ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
-	u8 entry_i;
-
-	switch (rtlpriv->sec.pairwise_enc_algorithm) {
-	case WEP40_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
-		break;
-	case WEP104_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
-		break;
-	case TKIP_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
-		break;
-	case AESCCMP_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
-		break;
-	default:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
-	}
-
-	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-		if (entry_i == 0) {
-			ul_content =
-			    (uc_index & 0x03) | ((u16) (ul_encalgo) << 2);
-			ul_content |= BIT(15);
-
-		} else {
-			ul_content = 0;
-		}
-
-		ul_command = CAM_CONTENT_COUNT * uc_index + entry_i;
-		ul_command = ul_command | BIT(31) | BIT(16);
-
-		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
-		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-
-		RT_TRACE(COMP_SEC, DBG_LOUD,
-			 ("stg_rtl_cam_empty_entry(): WRITE A4: %x\n",
-			  ul_content));
-		RT_TRACE(COMP_SEC, DBG_LOUD,
-			 ("stg_rtl_cam_empty_entry(): WRITE A0: %x\n",
-			  ul_command));
-	}
-}
-EXPORT_SYMBOL(stg_rtl_cam_empty_entry);
-
-u8 stg_rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> 4;
-	u8 entry_idx = 0;
-	u8 i, *addr;
-
-	if (!sta_addr) {
-		RT_TRACE(COMP_SEC, DBG_EMERG,
-			 ("sta_addr is NULL\n"));
-		return TOTAL_CAM_ENTRY;
-	}
-	/* Does STA already exist? */
-	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
-		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
-		if (memcmp(addr, sta_addr, ETH_ALEN) == 0)
-			return i;
-	}
-	/* Get a free CAM entry. */
-	for (entry_idx = 4; entry_idx < TOTAL_CAM_ENTRY; entry_idx++) {
-		if ((bitmap & BIT(0)) == 0) {
-			RT_TRACE(COMP_SEC, DBG_EMERG,
-				 ("-----hwsec_cam_bitmap: 0x%x entry_idx=%d\n",
-					  rtlpriv->sec.hwsec_cam_bitmap, entry_idx));
-			rtlpriv->sec.hwsec_cam_bitmap |= BIT(0) << entry_idx;
-			memcpy(rtlpriv->sec.hwsec_cam_sta_addr[entry_idx],
-			       sta_addr, ETH_ALEN);
-			return entry_idx;
-		}
-		bitmap = bitmap >> 1;
-	}
-	return TOTAL_CAM_ENTRY;
-}
-EXPORT_SYMBOL(stg_rtl_cam_get_free_entry);
-
-void stg_rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 bitmap;
-	u8 i, *addr;
-
-	if (NULL == sta_addr) {
-		RT_TRACE(COMP_SEC, DBG_EMERG,
-			 ("sta_addr is NULL.\n"));
-		return;
-	}
-
-	if (is_zero_ether_addr(sta_addr)) {
-		RT_TRACE(COMP_SEC, DBG_EMERG,
-			 ("sta_addr is 00:00:00:00:00:00.\n"));
-		return;
-	}
-	/* Does STA already exist? */
-	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
-		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
-		bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> i;
-		if (((bitmap & BIT(0)) == BIT(0)) &&
-		    (memcmp(addr, sta_addr, ETH_ALEN) == 0)) {
-			/* Remove from HW Security CAM */
-			memset(rtlpriv->sec.hwsec_cam_sta_addr[i], 0, ETH_ALEN);
-			rtlpriv->sec.hwsec_cam_bitmap &= ~(BIT(0) << i);
-			pr_info("&&&&&&&&&del entry %d\n", i);
-		}
-	}
-	return;
-}
-EXPORT_SYMBOL(stg_rtl_cam_del_entry);
diff --git a/drivers/staging/rtl8192ee/cam.h b/drivers/staging/rtl8192ee/cam.h
deleted file mode 100644
index b3a9464e7ed0..000000000000
--- a/drivers/staging/rtl8192ee/cam.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_CAM_H_
-#define __RTL_CAM_H_
-
-#define CAM_CONTENT_COUNT				8
-
-#define CFG_DEFAULT_KEY					BIT(5)
-#define CFG_VALID					BIT(15)
-
-#define PAIRWISE_KEYIDX					0
-#define CAM_PAIRWISE_KEY_POSITION			4
-
-#define	CAM_CONFIG_USEDK				1
-#define	CAM_CONFIG_NO_USEDK				0
-
-void stg_rtl_cam_reset_all_entry(struct ieee80211_hw *hw);
-u8 stg_rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
-			     u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
-			     u32 ul_default_key, u8 *key_content);
-int stg_rtl_cam_delete_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
-				 u32 ul_key_id);
-void stg_rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index);
-void stg_rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index);
-void rtl92e_cam_reset_sec_info(struct ieee80211_hw *hw);
-u8 stg_rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr);
-void stg_rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr);
-
-#endif
diff --git a/drivers/staging/rtl8192ee/compat.h b/drivers/staging/rtl8192ee/compat.h
deleted file mode 100644
index 72a3c13afeda..000000000000
--- a/drivers/staging/rtl8192ee/compat.h
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef __RTL_COMPAT_H__
-#define __RTL_COMPAT_H__
-
-
-#define RX_FLAG_MACTIME_MPDU RX_FLAG_MACTIME_START
-
-#define IEEE80211_KEY_FLAG_SW_MGMT IEEE80211_KEY_FLAG_SW_MGMT_TX
-
-struct ieee80211_mgmt_compat {
-	__le16 frame_control;
-	__le16 duration;
-	u8 da[6];
-	u8 sa[6];
-	u8 bssid[6];
-	__le16 seq_ctrl;
-	union {
-		struct {
-			u8 category;
-			union {
-				struct {
-					u8 action_code;
-					u8 dialog_token;
-					u8 status_code;
-					u8 variable[0];
-				} __packed wme_action;
-				struct {
-					u8 action_code;
-					u8 dialog_token;
-					__le16 capab;
-					__le16 timeout;
-					__le16 start_seq_num;
-				} __packed addba_req;
-				struct{
-					u8 action_code;
-					u8 dialog_token;
-					__le16 status;
-					__le16 capab;
-					__le16 timeout;
-				} __packed addba_resp;
-				struct {
-					u8 action_code;
-					__le16 params;
-					__le16 reason_code;
-				} __packed delba;
-				struct {
-					u8 action_code;
-					/* capab_info for open and confirm,
-					 * reason for close
-					 */
-					__le16 aux;
-					/* Followed in plink_confirm by status
-					 * code, AID and supported rates,
-					 * and directly by supported rates in
-					 * plink_open and plink_close
-					 */
-					u8 variable[0];
-				} __packed plink_action;
-				struct {
-					u8 action_code;
-					u8 variable[0];
-				} __packed mesh_action;
-				struct {
-					u8 action;
-					u8 smps_control;
-				} __packed ht_smps;
-			} u;
-		} __packed action;
-	} u;
-} __packed;
-#endif
diff --git a/drivers/staging/rtl8192ee/core.c b/drivers/staging/rtl8192ee/core.c
deleted file mode 100644
index 7f6accd59986..000000000000
--- a/drivers/staging/rtl8192ee/core.c
+++ /dev/null
@@ -1,1600 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "core.h"
-#include "cam.h"
-#include "base.h"
-#include "ps.h"
-
-#include "btcoexist/rtl_btc.h"
-
-/*mutex for start & stop is must here. */
-static int rtl_op_start(struct ieee80211_hw *hw)
-{
-	int err = 0;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (!is_hal_stop(rtlhal))
-		return 0;
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		return 0;
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	err = rtlpriv->intf_ops->adapter_start(hw);
-	if (err)
-		goto out;
-	rtl92e_watch_dog_timer_callback((unsigned long)hw);
-
-out:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-	return err;
-}
-
-static void rtl_op_stop(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool b_support_remote_wakeup = false;
-
-	if (is_hal_stop(rtlhal))
-		return;
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
-				      (u8 *)(&b_support_remote_wakeup));
-	/* here is must, because adhoc do stop and start,
-	 * but stop with RFOFF may cause something wrong,
-	 * like adhoc TP */
-	if (unlikely(ppsc->rfpwr_state == ERFOFF))
-		rtl92e_ips_nic_on(hw);
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	/* if wowlan supported, DON'T clear connected info */
-	if (!(b_support_remote_wakeup &&
-	      rtlhal->b_enter_pnp_sleep)) {
-		mac->link_state = MAC80211_NOLINK;
-		memset(mac->bssid, 0, 6);
-		mac->vendor = PEER_UNKNOWN;
-
-		/* reset sec info */
-		rtl92e_cam_reset_sec_info(hw);
-
-		rtl92e_deinit_deferred_work(hw);
-	}
-	rtlpriv->intf_ops->adapter_stop(hw);
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-
-static void rtl_op_tx(struct ieee80211_hw *hw,
-		      struct ieee80211_tx_control *control,
-		      struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_tcb_desc tcb_desc;
-	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-
-	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
-		goto err_free;
-
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		goto err_free;
-
-	if (!rtlpriv->intf_ops->waitq_insert(hw, control->sta, skb))
-		rtlpriv->intf_ops->adapter_tx(hw, control->sta, skb, &tcb_desc);
-	return;
-
-err_free:
-	dev_kfree_skb_any(skb);
-	return;
-}
-
-static int rtl_op_add_interface(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	int err = 0;
-
-	if (mac->vif) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("vif has been set!! mac->vif = 0x%p\n", mac->vif));
-		return -EOPNOTSUPP;
-	}
-
-	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
-
-	rtl92e_ips_nic_on(hw);
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	switch (ieee80211_vif_type_p2p(vif)) {
-	case NL80211_IFTYPE_P2P_CLIENT:
-		mac->p2p = P2P_ROLE_CLIENT;
-		/*fall through*/
-	case NL80211_IFTYPE_STATION:
-		if (mac->beacon_enabled == 1) {
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("NL80211_IFTYPE_STATION\n"));
-			mac->beacon_enabled = 0;
-			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
-		}
-		break;
-	case NL80211_IFTYPE_ADHOC:
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("NL80211_IFTYPE_ADHOC\n"));
-		mac->link_state = MAC80211_LINKED;
-		rtlpriv->cfg->ops->set_bcn_reg(hw);
-		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
-			mac->basic_rates = 0xfff;
-		else
-			mac->basic_rates = 0xff0;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *)(&mac->basic_rates));
-		break;
-	case NL80211_IFTYPE_P2P_GO:
-		mac->p2p = P2P_ROLE_GO;
-		/*fall through*/
-	case NL80211_IFTYPE_AP:
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("NL80211_IFTYPE_AP\n"));
-
-		mac->link_state = MAC80211_LINKED;
-		rtlpriv->cfg->ops->set_bcn_reg(hw);
-		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
-			mac->basic_rates = 0xfff;
-		else
-			mac->basic_rates = 0xff0;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-					      (u8 *)(&mac->basic_rates));
-		break;
-	case NL80211_IFTYPE_MESH_POINT:
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("NL80211_IFTYPE_MESH_POINT\n"));
-
-		mac->link_state = MAC80211_LINKED;
-		rtlpriv->cfg->ops->set_bcn_reg(hw);
-		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
-			mac->basic_rates = 0xfff;
-		else
-			mac->basic_rates = 0xff0;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *)(&mac->basic_rates));
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("operation mode %d is not support!\n", vif->type));
-		err = -EOPNOTSUPP;
-		goto out;
-	}
-
-#ifdef VIF_TODO
-	if (!rtl_set_vif_info(hw, vif))
-		goto out;
-#endif
-
-	if (mac->p2p) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("p2p role %x\n", vif->type));
-		mac->basic_rates = 0xff0;/*disable cck rate for p2p*/
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *)(&mac->basic_rates));
-	}
-	mac->vif = vif;
-	mac->opmode = vif->type;
-	rtlpriv->cfg->ops->set_network_type(hw, vif->type);
-	ether_addr_copy(mac->mac_addr, vif->addr);
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
-
-out:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-	return err;
-}
-
-static void rtl_op_remove_interface(struct ieee80211_hw *hw,
-				    struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-
-	/* Free beacon resources */
-	if ((vif->type == NL80211_IFTYPE_AP) ||
-	    (vif->type == NL80211_IFTYPE_ADHOC) ||
-	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
-		if (mac->beacon_enabled == 1) {
-			mac->beacon_enabled = 0;
-			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
-		}
-	}
-
-	/*
-	 *Note: We assume NL80211_IFTYPE_UNSPECIFIED as
-	 *NO LINK for our hardware.
-	 */
-	mac->p2p = 0;
-	mac->vif = NULL;
-	mac->link_state = MAC80211_NOLINK;
-	memset(mac->bssid, 0, 6);
-	mac->vendor = PEER_UNKNOWN;
-	mac->opmode = NL80211_IFTYPE_UNSPECIFIED;
-	rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-/*<delete in kernel start>*/
-static int rtl_op_change_interface(struct ieee80211_hw *hw,
-				   struct ieee80211_vif *vif,
-				   enum nl80211_iftype new_type, bool p2p)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int ret;
-	rtl_op_remove_interface(hw, vif);
-
-	vif->type = new_type;
-	vif->p2p = p2p;
-	ret = rtl_op_add_interface(hw, vif);
-	RT_TRACE(COMP_MAC80211, DBG_LOUD,
-		 (" p2p  %x\n", p2p));
-	return ret;
-}
-
-#ifdef CONFIG_PM
-static u16 crc16_ccitt(u8 data, u16 crc)
-{
-	u8 shift_in, data_bit, crc_bit11, crc_bit4, crc_bit15;
-	u8 i;
-	u16 result;
-
-	for (i = 0; i < 8; i++) {
-		crc_bit15 = ((crc & BIT(15)) ? 1 : 0);
-		data_bit  = (data & (BIT(0) << i) ? 1 : 0);
-		shift_in = crc_bit15 ^ data_bit;
-
-		result = crc << 1;
-		if (shift_in == 0)
-			result &= (~BIT(0));
-		else
-			result |= BIT(0);
-
-		crc_bit11 = ((crc & BIT(11)) ? 1 : 0) ^ shift_in;
-		if (crc_bit11 == 0)
-			result &= (~BIT(12));
-		else
-			result |= BIT(12);
-
-		crc_bit4 = ((crc & BIT(4)) ? 1 : 0) ^ shift_in;
-		if (crc_bit4 == 0)
-			result &= (~BIT(5));
-		else
-			result |= BIT(5);
-
-		crc = result;
-	}
-
-	return crc;
-}
-
-static u16 _calculate_wol_pattern_crc(u8 *pattern, u16 len)
-{
-	u16 crc = 0xffff;
-	u32 i;
-
-	for (i = 0; i < len; i++)
-		crc = crc16_ccitt(pattern[i], crc);
-	crc = ~crc;
-
-	return crc;
-}
-
-static void _rtl_add_wowlan_patterns(struct ieee80211_hw *hw,
-				     struct cfg80211_wowlan *wow)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = &(rtlpriv->mac80211);
-	struct cfg80211_pkt_pattern *patterns = wow->patterns;
-	struct rtl_wow_pattern rtl_pattern;
-	const u8 *pattern_os, *mask_os;
-	u8 mask[MAX_WOL_BIT_MASK_SIZE] = {0};
-	u8 content[MAX_WOL_PATTERN_SIZE] = {0};
-	u8 broadcast_addr[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
-	u8 multicast_addr1[2] = {0x33, 0x33};
-	u8 multicast_addr2[3] = {0x01, 0x00, 0x5e};
-	u8 i, mask_len;
-	u16 j, len;
-
-	for (i = 0; i < wow->n_patterns; i++) {
-		memset(&rtl_pattern, 0, sizeof(struct rtl_wow_pattern));
-		memset(mask, 0, MAX_WOL_BIT_MASK_SIZE);
-		if (patterns[i].pattern_len > MAX_WOL_PATTERN_SIZE) {
-			RT_TRACE(COMP_POWER, DBG_WARNING,
-				 ("Pattern[%d] is too long\n", i));
-			continue;
-		}
-		pattern_os = patterns[i].pattern;
-		mask_len = DIV_ROUND_UP(patterns[i].pattern_len, 8);
-		mask_os = patterns[i].mask;
-		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
-			      "pattern content\n", pattern_os,
-			      patterns[i].pattern_len);
-		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
-			      "mask content\n", mask_os, mask_len);
-		/* 1. unicast? multicast? or broadcast? */
-		if (memcmp(pattern_os, broadcast_addr, 6) == 0)
-			rtl_pattern.type = BROADCAST_PATTERN;
-		else if (memcmp(pattern_os, multicast_addr1, 2) == 0 ||
-			 memcmp(pattern_os, multicast_addr2, 3) == 0)
-			rtl_pattern.type = MULTICAST_PATTERN;
-		else if  (memcmp(pattern_os, mac->mac_addr, 6) == 0)
-			rtl_pattern.type = UNICAST_PATTERN;
-		else
-			rtl_pattern.type = UNKNOWN_TYPE;
-
-		/* 2. translate mask_from_os to mask_for_hw */
-
-/******************************************************************************
- * pattern from OS uses 'ethenet frame', like this:
-
-		   |    6   |    6   |   2  |     20    |  Variable  |	4  |
-		   |--------+--------+------+-----------+------------+-----|
-		   |    802.3 Mac Header    | IP Header | TCP Packet | FCS |
-		   |   DA   |   SA   | Type |
-
- * BUT, packet catched by our HW is in '802.11 frame', begin from LLC,
-
-	|     24 or 30      |    6   |   2  |     20    |  Variable  |  4  |
-	|-------------------+--------+------+-----------+------------+-----|
-	| 802.11 MAC Header |       LLC     | IP Header | TCP Packet | FCS |
-			    | Others | Tpye |
-
- * Therefore, we need translate mask_from_OS to mask_to_hw.
- * We should left-shift mask by 6 bits, then set the new bit[0~5] = 0,
- * because new mask[0~5] means 'SA', but our HW packet begins from LLC,
- * bit[0~5] corresponds to first 6 Bytes in LLC, they just don't match.
- ******************************************************************************/
-
-		/* Shift 6 bits */
-		for (j = 0; j < mask_len - 1; j++) {
-			mask[j] = mask_os[j] >> 6;
-			mask[j] |= (mask_os[j + 1] & 0x3F) << 2;
-		}
-		mask[j] = (mask_os[j] >> 6) & 0x3F;
-		/* Set bit 0-5 to zero */
-		mask[0] &= 0xC0;
-
-		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
-			      "mask to hw\n", mask, mask_len);
-		for (j = 0; j < (MAX_WOL_BIT_MASK_SIZE + 1) / 4; j++) {
-			rtl_pattern.mask[j] = mask[j * 4];
-			rtl_pattern.mask[j] |= (mask[j * 4 + 1] << 8);
-			rtl_pattern.mask[j] |= (mask[j * 4 + 2] << 16);
-			rtl_pattern.mask[j] |= (mask[j * 4 + 3] << 24);
-		}
-
-		/* To get the wake up pattern from the mask.
-		 * We do not count first 12 bits which means
-		 * DA[6] and SA[6] in the pattern to match HW design. */
-		len = 0;
-		for (j = 12; j < patterns[i].pattern_len; j++) {
-			if ((mask_os[j / 8] >> (j % 8)) & 0x01) {
-				content[len] = pattern_os[j];
-				len++;
-			}
-		}
-
-		RT_PRINT_DATA(rtlpriv, COMP_POWER, DBG_TRACE,
-			      "pattern to hw\n", content, len);
-		/* 3. calculate crc */
-		rtl_pattern.crc = _calculate_wol_pattern_crc(content, len);
-		RT_TRACE(COMP_POWER, DBG_TRACE,
-			 ("CRC_Remainder = 0x%x", rtl_pattern.crc));
-
-		/* 4. write crc & mask_for_hw to hw */
-		rtlpriv->cfg->ops->add_wowlan_pattern(hw, &rtl_pattern, i);
-	}
-	rtl_write_byte(rtlpriv, 0x698, wow->n_patterns);
-}
-
-static int rtl_op_suspend(struct ieee80211_hw *hw,
-			  struct cfg80211_wowlan *wow)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct timeval ts;
-
-	RT_TRACE(COMP_POWER, DBG_DMESG, ("\n"));
-	if (WARN_ON(!wow))
-		return -EINVAL;
-
-	/* to resolve s4 can not wake up*/
-	do_gettimeofday(&ts);
-	rtlhal->last_suspend_sec = ts.tv_sec;
-
-	if ((ppsc->wo_wlan_mode & WAKE_ON_PATTERN_MATCH) && wow->n_patterns)
-		_rtl_add_wowlan_patterns(hw, wow);
-
-	rtlhal->driver_is_goingto_unload = true;
-	rtlhal->b_enter_pnp_sleep = true;
-
-	rtl92e_lps_leave(hw);
-	rtl_op_stop(hw);
-	device_set_wakeup_enable(wiphy_dev(hw->wiphy), true);
-	return 0;
-}
-
-static int rtl_op_resume(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct timeval ts;
-
-	RT_TRACE(COMP_POWER, DBG_DMESG, ("\n"));
-	rtlhal->driver_is_goingto_unload = false;
-	rtlhal->b_enter_pnp_sleep = false;
-	rtlhal->b_wake_from_pnp_sleep = true;
-
-	/* to resovle s4 can not wake up*/
-	do_gettimeofday(&ts);
-	if (ts.tv_sec - rtlhal->last_suspend_sec < 5)
-		return -1;
-
-	rtl_op_start(hw);
-	device_set_wakeup_enable(wiphy_dev(hw->wiphy), false);
-	ieee80211_resume_disconnect(mac->vif);
-	rtlhal->b_wake_from_pnp_sleep = false;
-	return 0;
-}
-#endif
-
-static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct ieee80211_conf *conf = &hw->conf;
-
-	if (mac->skip_scan)
-		return 1;
-
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {	/* BIT(2) */
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("IEEE80211_CONF_CHANGE_LISTEN_INTERVAL\n"));
-	}
-
-	/*For IPS */
-	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-		if (hw->conf.flags & IEEE80211_CONF_IDLE)
-			rtl92e_ips_nic_off(hw);
-		else
-			rtl92e_ips_nic_on(hw);
-	} else {
-		/*
-		 *although rfoff may not cause by ips, but we will
-		 *check the reason in set_rf_power_state function
-		 */
-		if (unlikely(ppsc->rfpwr_state == ERFOFF))
-			rtl92e_ips_nic_on(hw);
-	}
-
-	/*For LPS */
-	if (changed & IEEE80211_CONF_CHANGE_PS) {
-		cancel_delayed_work(&rtlpriv->works.ps_work);
-		cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
-		if (conf->flags & IEEE80211_CONF_PS) {
-			rtlpriv->psc.sw_ps_enabled = true;
-			/* sleep here is must, or we may recv the beacon and
-			 * cause mac80211 into wrong ps state, this will cause
-			 * power save nullfunc send fail, and further cause
-			 * pkt loss, So sleep must quickly but not immediatly
-			 * because that will cause nullfunc send by mac80211
-			 * fail, and cause pkt loss, we have tested that 5mA
-			 * is worked very well */
-			if (!rtlpriv->psc.multi_buffered)
-				queue_delayed_work(rtlpriv->works.rtl_wq,
-						   &rtlpriv->works.ps_work,
-						   MSECS(5));
-		} else {
-			rtl92e_swlps_rf_awake(hw);
-			rtlpriv->psc.sw_ps_enabled = false;
-		}
-	}
-
-	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("IEEE80211_CONF_CHANGE_RETRY_LIMITS %x\n",
-			  hw->conf.long_frame_max_tx_count));
-		mac->retry_long = hw->conf.long_frame_max_tx_count;
-		mac->retry_short = hw->conf.long_frame_max_tx_count;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RETRY_LIMIT,
-				(u8 *)(&hw->conf.long_frame_max_tx_count));
-	}
-	if (changed & IEEE80211_CONF_CHANGE_CHANNEL &&
-	    !rtlpriv->proximity.proxim_on) {
-		struct ieee80211_channel *channel = hw->conf.chandef.chan;
-		enum nl80211_chan_width width = hw->conf.chandef.width;
-		u8 wide_chan = (u8) channel->hw_value;
-		enum nl80211_channel_type channel_type = NL80211_CHAN_NO_HT;
-
-		/* channel_type is for 20&40M */
-		if (width < NL80211_CHAN_WIDTH_80)
-			channel_type = cfg80211_get_chandef_type(&(hw->conf.chandef));
-		if (mac->act_scanning)
-			mac->n_channels++;
-
-		if (rtlpriv->dm.supp_phymode_switch &&
-		    mac->link_state < MAC80211_LINKED &&
-		    !mac->act_scanning) {
-			if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-				rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-		}
-
-		/*
-		 *because we should back channel to
-		 *current_network.chan in in scanning,
-		 *So if set_chan == current_network.chan
-		 *we should set it.
-		 *because mac80211 tell us wrong bw40
-		 *info for cisco1253 bw20, so we modify
-		 *it here based on UPPER & LOWER
-		 */
-
-		if (width >= NL80211_CHAN_WIDTH_80) {
-			if (width == NL80211_CHAN_WIDTH_80) {
-				u32 center_freq = hw->conf.chandef.center_freq1;
-				u32 primary_freq =
-				(u32)hw->conf.chandef.chan->center_freq;
-
-				rtlphy->current_chan_bw =
-					HT_CHANNEL_WIDTH_80;
-				mac->bw_80 = true;
-				mac->bw_40 = true;
-				if (center_freq > primary_freq) {
-					mac->cur_80_prime_sc =
-					PRIME_CHNL_OFFSET_LOWER;
-					if (center_freq - primary_freq == 10) {
-						mac->cur_40_prime_sc =
-						PRIME_CHNL_OFFSET_UPPER;
-
-						wide_chan += 2;
-					} else if (center_freq - primary_freq == 30) {
-						mac->cur_40_prime_sc =
-						PRIME_CHNL_OFFSET_LOWER;
-
-						wide_chan += 6;
-					}
-				} else {
-					mac->cur_80_prime_sc =
-					PRIME_CHNL_OFFSET_UPPER;
-					if (primary_freq - center_freq == 10) {
-						mac->cur_40_prime_sc =
-						PRIME_CHNL_OFFSET_LOWER;
-
-						wide_chan -= 2;
-					} else if (primary_freq - center_freq == 30) {
-						mac->cur_40_prime_sc =
-						PRIME_CHNL_OFFSET_UPPER;
-
-						wide_chan -= 6;
-					}
-				}
-			}
-		} else {
-			switch (channel_type) {
-			case NL80211_CHAN_HT20:
-			case NL80211_CHAN_NO_HT:
-				/* SC */
-				mac->cur_40_prime_sc =
-					PRIME_CHNL_OFFSET_DONT_CARE;
-				rtlphy->current_chan_bw =
-					HT_CHANNEL_WIDTH_20;
-				mac->bw_40 = false;
-				mac->bw_80 = false;
-				break;
-			case NL80211_CHAN_HT40MINUS:
-				/* SC */
-				mac->cur_40_prime_sc =
-					PRIME_CHNL_OFFSET_UPPER;
-				rtlphy->current_chan_bw =
-					HT_CHANNEL_WIDTH_20_40;
-				mac->bw_40 = true;
-				mac->bw_80 = false;
-
-				/*wide channel */
-				wide_chan -= 2;
-				break;
-			case NL80211_CHAN_HT40PLUS:
-				/* SC */
-				mac->cur_40_prime_sc =
-					PRIME_CHNL_OFFSET_LOWER;
-				rtlphy->current_chan_bw =
-					HT_CHANNEL_WIDTH_20_40;
-				mac->bw_40 = true;
-				mac->bw_80 = false;
-				/*wide channel */
-				wide_chan += 2;
-				break;
-			default:
-				mac->bw_40 = false;
-				mac->bw_80 = false;
-				RT_TRACE(COMP_ERR, DBG_EMERG,
-					 ("switch case not processed\n"));
-				break;
-			}
-		}
-
-		if (wide_chan <= 0)
-			wide_chan = 1;
-
-		/* in scanning, when before we offchannel we may send a ps=1
-		 * null to AP, and then we may send a ps = 0 null to AP quickly,
-		 * but first null have cause AP's put lots of packet to hw tx
-		 * buffer, these packet must be tx before off channel so we must
-		 * delay more time to let AP flush these packets before
-		 * offchannel, or dis-association or delete BA will happen by AP
-		 */
-		if (rtlpriv->mac80211.offchan_deley) {
-			rtlpriv->mac80211.offchan_deley = false;
-			mdelay(50);
-		}
-
-		rtlphy->current_channel = wide_chan;
-
-		rtlpriv->cfg->ops->switch_channel(hw);
-		rtlpriv->cfg->ops->set_channel_access(hw);
-		rtlpriv->cfg->ops->set_bw_mode(hw,
-			channel_type);
-	}
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-
-	return 0;
-}
-
-static void rtl_op_configure_filter(struct ieee80211_hw *hw,
-				    unsigned int changed_flags,
-				    unsigned int *new_flags, u64 multicast)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	*new_flags &= RTL_SUPPORTED_FILTERS;
-	if (0 == changed_flags)
-		return;
-
-	/*TODO: we disable broadcase now, so enable here */
-	if (changed_flags & FIF_ALLMULTI) {
-		if (*new_flags & FIF_ALLMULTI) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
-			    rtlpriv->cfg->maps[MAC_RCR_AB];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive multicast frame.\n"));
-		} else {
-			mac->rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
-					  rtlpriv->cfg->maps[MAC_RCR_AB]);
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive multicast frame.\n"));
-		}
-	}
-
-	if (changed_flags & FIF_FCSFAIL) {
-		if (*new_flags & FIF_FCSFAIL) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive FCS error frame.\n"));
-		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive FCS error frame.\n"));
-		}
-	}
-
-	/* if ssid not set to hw don't check bssid
-	 * here just used for linked scanning, & linked
-	 * and nolink check bssid is set in set network_type */
-	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
-	    (mac->link_state >= MAC80211_LINKED)) {
-		if (mac->opmode != NL80211_IFTYPE_AP &&
-		    mac->opmode != NL80211_IFTYPE_MESH_POINT) {
-			if (*new_flags & FIF_BCN_PRBRESP_PROMISC)
-				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
-			else
-				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
-		}
-	}
-
-	if (changed_flags & FIF_CONTROL) {
-		if (*new_flags & FIF_CONTROL) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
-
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive control frame.\n"));
-		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive control frame.\n"));
-		}
-	}
-
-	if (changed_flags & FIF_OTHER_BSS) {
-		if (*new_flags & FIF_OTHER_BSS) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive other BSS's frame.\n"));
-		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive other BSS's frame.\n"));
-		}
-	}
-}
-static int rtl_op_sta_add(struct ieee80211_hw *hw,
-			  struct ieee80211_vif *vif,
-			  struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_sta_info *sta_entry;
-
-	if (sta) {
-		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
-		list_add_tail(&sta_entry->list, &rtlpriv->entry_list);
-		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
-		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-			sta_entry->wireless_mode = WIRELESS_MODE_G;
-			if (sta->supp_rates[0] <= 0xf)
-				sta_entry->wireless_mode = WIRELESS_MODE_B;
-			if (sta->ht_cap.ht_supported)
-				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
-
-			if (vif->type == NL80211_IFTYPE_ADHOC)
-				sta_entry->wireless_mode = WIRELESS_MODE_G;
-		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
-			sta_entry->wireless_mode = WIRELESS_MODE_A;
-			if (sta->ht_cap.ht_supported)
-				sta_entry->wireless_mode = WIRELESS_MODE_N_5G;
-			if (sta->vht_cap.vht_supported)
-				sta_entry->wireless_mode = WIRELESS_MODE_AC_5G;
-
-			if (vif->type == NL80211_IFTYPE_ADHOC)
-				sta_entry->wireless_mode = WIRELESS_MODE_A;
-		}
-		/*disable cck rate for p2p*/
-		if (mac->p2p)
-			sta->supp_rates[0] &= 0xfffffff0;
-
-		ether_addr_copy(sta_entry->mac_addr, sta->addr);
-		RT_TRACE(COMP_MAC80211, DBG_DMESG,
-			 ("Add sta addr is %pM\n", sta->addr));
-		rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
-	}
-
-	return 0;
-}
-
-static int rtl_op_sta_remove(struct ieee80211_hw *hw,
-			     struct ieee80211_vif *vif,
-			     struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *sta_entry;
-	if (sta) {
-		RT_TRACE(COMP_MAC80211, DBG_DMESG,
-			 ("Remove sta addr is %pM\n", sta->addr));
-		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-		sta_entry->wireless_mode = 0;
-		sta_entry->ratr_index = 0;
-		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
-		list_del(&sta_entry->list);
-		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
-	}
-	return 0;
-}
-static int _rtl_get_hal_qnum(u16 queue)
-{
-	int qnum;
-
-	switch (queue) {
-	case 0:
-		qnum = AC3_VO;
-		break;
-	case 1:
-		qnum = AC2_VI;
-		break;
-	case 2:
-		qnum = AC0_BE;
-		break;
-	case 3:
-		qnum = AC1_BK;
-		break;
-	default:
-		qnum = AC0_BE;
-		break;
-	}
-	return qnum;
-}
-
-/*
- *for mac80211 VO=0, VI=1, BE=2, BK=3
- *for rtl819x  BE=0, BK=1, VI=2, VO=3
- */
-static int rtl_op_conf_tx(struct ieee80211_hw *hw,
-			  struct ieee80211_vif *vif, u16 queue,
-			  const struct ieee80211_tx_queue_params *param)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	int aci;
-
-	if (queue >= AC_MAX) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("queue number %d is incorrect!\n", queue));
-		return -EINVAL;
-	}
-
-	aci = _rtl_get_hal_qnum(queue);
-	mac->ac[aci].aifs = param->aifs;
-	mac->ac[aci].cw_min = cpu_to_le16(param->cw_min);
-	mac->ac[aci].cw_max = cpu_to_le16(param->cw_max);
-	mac->ac[aci].tx_op = cpu_to_le16(param->txop);
-	memcpy(&mac->edca_param[aci], param, sizeof(*param));
-	rtlpriv->cfg->ops->set_qos(hw, aci);
-	return 0;
-}
-
-static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
-				    struct ieee80211_vif *vif,
-				    struct ieee80211_bss_conf *bss_conf,
-				    u32 changed)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	if ((vif->type == NL80211_IFTYPE_ADHOC) ||
-	    (vif->type == NL80211_IFTYPE_AP) ||
-	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
-		if ((changed & BSS_CHANGED_BEACON) ||
-		    (changed & BSS_CHANGED_BEACON_ENABLED &&
-		     bss_conf->enable_beacon)) {
-			if (mac->beacon_enabled == 0) {
-				RT_TRACE(COMP_MAC80211, DBG_DMESG,
-					 ("BSS_CHANGED_BEACON_ENABLED\n"));
-
-				/*start hw beacon interrupt. */
-				/*rtlpriv->cfg->ops->set_bcn_reg(hw); */
-				mac->beacon_enabled = 1;
-				rtlpriv->cfg->ops->update_interrupt_mask(hw,
-						rtlpriv->cfg->maps
-						[RTL_IBSS_INT_MASKS], 0);
-
-				if (rtlpriv->cfg->ops->linked_set_reg)
-					rtlpriv->cfg->ops->linked_set_reg(hw);
-			}
-		}
-		if ((changed & BSS_CHANGED_BEACON_ENABLED) &&
-		    !bss_conf->enable_beacon) {
-			if (mac->beacon_enabled == 1) {
-				RT_TRACE(COMP_MAC80211, DBG_DMESG,
-					 ("ADHOC DISABLE BEACON\n"));
-
-				mac->beacon_enabled = 0;
-				rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-						rtlpriv->cfg->maps
-						[RTL_IBSS_INT_MASKS]);
-			}
-		}
-		if (changed & BSS_CHANGED_BEACON_INT) {
-			RT_TRACE(COMP_BEACON, DBG_TRACE,
-				 ("BSS_CHANGED_BEACON_INT\n"));
-			mac->beacon_interval = bss_conf->beacon_int;
-			rtlpriv->cfg->ops->set_bcn_intv(hw);
-		}
-	}
-
-	/*TODO: reference to enum ieee80211_bss_change */
-	if (changed & BSS_CHANGED_ASSOC) {
-		u8 mstatus;
-		if (bss_conf->assoc) {
-			struct ieee80211_sta *sta = NULL;
-			u8 keep_alive = 10;
-
-			mstatus = RT_MEDIA_CONNECT;
-			/* we should reset all sec info & cam
-			 * before set cam after linked, we should not
-			 * reset in disassoc, that will cause tkip->wep
-			 * fail because some flag will be wrong */
-			/* reset sec info */
-			rtl92e_cam_reset_sec_info(hw);
-			/* reset cam to fix wep fail issue
-			 * when change from wpa to wep */
-			stg_rtl_cam_reset_all_entry(hw);
-
-			mac->link_state = MAC80211_LINKED;
-			mac->cnt_after_linked = 0;
-			mac->assoc_id = bss_conf->aid;
-			memcpy(mac->bssid, bss_conf->bssid, 6);
-
-			if (rtlpriv->cfg->ops->linked_set_reg)
-				rtlpriv->cfg->ops->linked_set_reg(hw);
-
-			rcu_read_lock();
-			sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
-			if (!sta) {
-				pr_err("ieee80211_find_sta returned NULL\n");
-				rcu_read_unlock();
-				goto out;
-			}
-
-			if (vif->type == NL80211_IFTYPE_STATION && sta)
-				rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
-			RT_TRACE(COMP_EASY_CONCURRENT, DBG_LOUD,
-				 ("send PS STATIC frame\n"));
-			if (rtlpriv->dm.supp_phymode_switch) {
-				if (sta->ht_cap.ht_supported)
-					stg_rtl_send_smps_action(hw, sta,
-							IEEE80211_SMPS_STATIC);
-			}
-
-			if (rtlhal->current_bandtype == BAND_ON_5G) {
-				mac->mode = WIRELESS_MODE_A;
-			} else {
-				if (sta->supp_rates[0] <= 0xf)
-					mac->mode = WIRELESS_MODE_B;
-				else
-					mac->mode = WIRELESS_MODE_G;
-			}
-
-			if (sta->ht_cap.ht_supported) {
-				if (rtlhal->current_bandtype == BAND_ON_2_4G)
-					mac->mode = WIRELESS_MODE_N_24G;
-				else
-					mac->mode = WIRELESS_MODE_N_5G;
-			}
-
-			if (sta->vht_cap.vht_supported) {
-				if (rtlhal->current_bandtype == BAND_ON_5G)
-					mac->mode = WIRELESS_MODE_AC_5G;
-				else
-					mac->mode = WIRELESS_MODE_AC_24G;
-			}
-
-			rcu_read_unlock();
-
-			/* to avoid AP Disassociation caused by inactivity */
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_KEEP_ALIVE,
-						      (u8 *)(&keep_alive));
-
-			RT_TRACE(COMP_MAC80211, DBG_DMESG,
-				 ("BSS_CHANGED_ASSOC\n"));
-		} else {
-			mstatus = RT_MEDIA_DISCONNECT;
-
-			if (mac->link_state == MAC80211_LINKED)
-				rtl92e_lps_leave(hw);
-			if (ppsc->p2p_ps_info.p2p_ps_mode > P2P_PS_NONE)
-				rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-			mac->link_state = MAC80211_NOLINK;
-			memset(mac->bssid, 0, 6);
-			mac->vendor = PEER_UNKNOWN;
-			mac->mode = 0;
-
-			if (rtlpriv->dm.supp_phymode_switch) {
-				if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-					rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-			}
-			RT_TRACE(COMP_MAC80211, DBG_DMESG,
-				 ("BSS_CHANGED_UN_ASSOC\n"));
-		}
-		rtlpriv->cfg->ops->set_network_type(hw, vif->type);
-		/* For FW LPS:
-		 * To tell firmware we have connected or disconnected*/
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-					      HW_VAR_H2C_FW_JOINBSSRPT,
-					      (u8 *)(&mstatus));
-		ppsc->report_linked = (mstatus == RT_MEDIA_CONNECT) ?
-				      true : false;
-
-		if (rtlpriv->cfg->ops->get_btc_status())
-			rtlpriv->btcoexist.btc_ops->btc_mediastatus_notify(
-							rtlpriv, mstatus);
-	}
-
-	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("BSS_CHANGED_ERP_CTS_PROT\n"));
-		mac->use_cts_protect = bss_conf->use_cts_prot;
-	}
-
-	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("BSS_CHANGED_ERP_PREAMBLE use short preamble:%x\n",
-			  bss_conf->use_short_preamble));
-
-		mac->short_preamble = bss_conf->use_short_preamble;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACK_PREAMBLE,
-					      (u8 *)(&mac->short_preamble));
-	}
-
-	if (changed & BSS_CHANGED_ERP_SLOT) {
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("BSS_CHANGED_ERP_SLOT\n"));
-
-		if (bss_conf->use_short_slot)
-			mac->slot_time = RTL_SLOT_TIME_9;
-		else
-			mac->slot_time = RTL_SLOT_TIME_20;
-
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-					      (u8 *)(&mac->slot_time));
-	}
-
-	if (changed & BSS_CHANGED_HT) {
-		struct ieee80211_sta *sta = NULL;
-
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("BSS_CHANGED_HT\n"));
-
-		rcu_read_lock();
-		sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
-		if (sta) {
-			if (sta->ht_cap.ampdu_density >
-			    mac->current_ampdu_density)
-				mac->current_ampdu_density =
-				    sta->ht_cap.ampdu_density;
-			if (sta->ht_cap.ampdu_factor <
-			    mac->current_ampdu_factor)
-				mac->current_ampdu_factor =
-				    sta->ht_cap.ampdu_factor;
-		}
-		rcu_read_unlock();
-
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SHORTGI_DENSITY,
-					      (u8 *)(&mac->max_mss_density));
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_FACTOR,
-					      &mac->current_ampdu_factor);
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_MIN_SPACE,
-					      &mac->current_ampdu_density);
-	}
-
-	if (changed & BSS_CHANGED_BSSID) {
-		u32 basic_rates;
-		struct ieee80211_sta *sta = NULL;
-
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BSSID,
-					      (u8 *)bss_conf->bssid);
-
-		RT_TRACE(COMP_MAC80211, DBG_DMESG,
-			 ("bssid: %pM\n", bss_conf->bssid));
-
-		mac->vendor = PEER_UNKNOWN;
-		memcpy(mac->bssid, bss_conf->bssid, 6);
-
-		rcu_read_lock();
-		sta = ieee80211_find_sta(vif, (u8 *)bss_conf->bssid);
-		if (!sta) {
-			rcu_read_unlock();
-			goto out;
-		}
-
-		if (rtlhal->current_bandtype == BAND_ON_5G) {
-			mac->mode = WIRELESS_MODE_A;
-		} else {
-			if (sta->supp_rates[0] <= 0xf)
-				mac->mode = WIRELESS_MODE_B;
-			else
-				mac->mode = WIRELESS_MODE_G;
-		}
-
-		if (sta->ht_cap.ht_supported) {
-			if (rtlhal->current_bandtype == BAND_ON_2_4G)
-				mac->mode = WIRELESS_MODE_N_24G;
-			else
-				mac->mode = WIRELESS_MODE_N_5G;
-		}
-
-		if (sta->vht_cap.vht_supported) {
-			if (rtlhal->current_bandtype == BAND_ON_5G)
-				mac->mode = WIRELESS_MODE_AC_5G;
-			else
-				mac->mode = WIRELESS_MODE_AC_24G;
-		}
-
-		/* just station need it, because ibss & ap mode will
-		 * set in sta_add, and will be NULL here */
-		if (vif->type == NL80211_IFTYPE_STATION) {
-			struct rtl_sta_info *sta_entry;
-			sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-			sta_entry->wireless_mode = mac->mode;
-		}
-
-		if (sta->ht_cap.ht_supported) {
-			mac->ht_enable = true;
-
-			/*
-			 * for cisco 1252 bw20 it's wrong
-			 * if (ht_cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
-			 *	mac->bw_40 = true;
-			 * }
-			 * */
-		}
-
-		if (sta->vht_cap.vht_supported)
-			mac->vht_enable = true;
-
-		if (changed & BSS_CHANGED_BASIC_RATES) {
-			/* for 5G must << RATE_6M_INDEX=4,
-			 * because 5G have no cck rate*/
-			if (rtlhal->current_bandtype == BAND_ON_5G)
-				basic_rates = sta->supp_rates[1] << 4;
-			else
-				basic_rates = sta->supp_rates[0];
-
-			mac->basic_rates = basic_rates;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-					(u8 *)(&basic_rates));
-		}
-		rcu_read_unlock();
-	}
-out:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-
-static u64 rtl_op_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u64 tsf;
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *)(&tsf));
-	return tsf;
-}
-
-static void rtl_op_set_tsf(struct ieee80211_hw *hw,
-			   struct ieee80211_vif *vif, u64 tsf)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
-
-	mac->tsf = tsf;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *)(&bibss));
-}
-
-static void rtl_op_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp = 0;
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_DUAL_TSF_RST, (u8 *)(&tmp));
-}
-
-static void rtl_op_sta_notify(struct ieee80211_hw *hw,
-			      struct ieee80211_vif *vif,
-			      enum sta_notify_cmd cmd,
-			      struct ieee80211_sta *sta)
-{
-	switch (cmd) {
-	case STA_NOTIFY_SLEEP:
-		break;
-	case STA_NOTIFY_AWAKE:
-		break;
-	default:
-		break;
-	}
-}
-
-static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
-			       struct ieee80211_vif *vif,
-			       enum ieee80211_ampdu_mlme_action action,
-			       struct ieee80211_sta *sta, u16 tid, u16 *ssn,
-			       u8 buf_size)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	switch (action) {
-	case IEEE80211_AMPDU_TX_START:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_TX_START: TID:%d\n", tid));
-		return rtl92e_tx_agg_start(hw, vif, sta, tid, ssn);
-		break;
-	case IEEE80211_AMPDU_TX_STOP_CONT:
-	case IEEE80211_AMPDU_TX_STOP_FLUSH:
-	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_TX_STOP: TID:%d\n", tid));
-		return rtl92e_tx_agg_stop(hw, vif, sta, tid);
-		break;
-	case IEEE80211_AMPDU_TX_OPERATIONAL:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_TX_OPERATIONAL:TID:%d\n", tid));
-		rtl92e_tx_agg_oper(hw, sta, tid);
-		break;
-	case IEEE80211_AMPDU_RX_START:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_RX_START:TID:%d\n", tid));
-		return rtl92e_rx_agg_start(hw, sta, tid);
-		break;
-	case IEEE80211_AMPDU_RX_STOP:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_RX_STOP:TID:%d\n", tid));
-		return rtl92e_rx_agg_stop(hw, sta, tid);
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("IEEE80211_AMPDU_ERR!!!!:\n"));
-		return -EOPNOTSUPP;
-	}
-	return 0;
-}
-
-static void rtl_op_sw_scan_start(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
-	mac->act_scanning = true;
-	if (rtlpriv->link_info.b_higher_busytraffic) {
-		mac->skip_scan = true;
-		return;
-	}
-
-	if (rtlpriv->cfg->ops->get_btc_status())
-		rtlpriv->btcoexist.btc_ops->btc_scan_notify(rtlpriv, 1);
-
-	if (rtlpriv->dm.supp_phymode_switch) {
-		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-	}
-
-	if (mac->link_state == MAC80211_LINKED) {
-		rtl92e_lps_leave(hw);
-		mac->link_state = MAC80211_LINKED_SCANNING;
-	} else {
-		rtl92e_ips_nic_on(hw);
-	}
-
-	/* Dul mac */
-	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
-
-	rtlpriv->cfg->ops->led_control(hw, LED_CTL_SITE_SURVEY);
-	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_BACKUP_BAND0);
-}
-
-static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
-	mac->act_scanning = false;
-	mac->skip_scan = false;
-	if (rtlpriv->link_info.b_higher_busytraffic)
-		return;
-
-	/* p2p will use 1/6/11 to scan */
-	if (mac->n_channels == 3)
-		mac->p2p_in_use = true;
-	else
-		mac->p2p_in_use = false;
-	mac->n_channels = 0;
-	/* Dul mac */
-	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
-
-	if (mac->link_state == MAC80211_LINKED_SCANNING) {
-		mac->link_state = MAC80211_LINKED;
-		if (mac->opmode == NL80211_IFTYPE_STATION) {
-			/* fix fwlps issue */
-			rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
-		}
-	}
-
-	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_RESTORE);
-	if (rtlpriv->cfg->ops->get_btc_status())
-		rtlpriv->btcoexist.btc_ops->btc_scan_notify(rtlpriv, 0);
-}
-
-static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
-			  struct ieee80211_vif *vif, struct ieee80211_sta *sta,
-			  struct ieee80211_key_conf *key)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 key_type = NO_ENCRYPTION;
-	u8 key_idx;
-	bool group_key = false;
-	bool wep_only = false;
-	int err = 0;
-	u8 mac_addr[ETH_ALEN];
-	u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-
-	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("not open hw encryption\n"));
-		return -ENOSPC;	/*User disabled HW-crypto */
-	}
-	/* To support IBSS, use sw-crypto for GTK */
-	if (((vif->type == NL80211_IFTYPE_ADHOC) ||
-	     (vif->type == NL80211_IFTYPE_MESH_POINT)) &&
-	     !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
-		return -ENOSPC;
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("%s hardware based encryption for keyidx: %d, mac: %pM\n",
-		  cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
-		  sta ? sta->addr : bcast_addr));
-	rtlpriv->sec.being_setkey = true;
-	rtl92e_ips_nic_on(hw);
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	/* <1> get encryption alg */
-
-	switch (key->cipher) {
-	case WLAN_CIPHER_SUITE_WEP40:
-		key_type = WEP40_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP40\n"));
-		break;
-	case WLAN_CIPHER_SUITE_WEP104:
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP104\n"));
-		key_type = WEP104_ENCRYPTION;
-		break;
-	case WLAN_CIPHER_SUITE_TKIP:
-		key_type = TKIP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:TKIP\n"));
-		break;
-	case WLAN_CIPHER_SUITE_CCMP:
-		key_type = AESCCMP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CCMP\n"));
-		break;
-	case WLAN_CIPHER_SUITE_AES_CMAC:
-		/* HW don't support CMAC encryption,
-		 * use software CMAC encryption */
-		key_type = AESCMAC_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CMAC\n"));
-		RT_TRACE(COMP_SEC, DBG_DMESG,
-			 ("HW don't support CMAC encrypiton, use software CMAC encryption\n"));
-		err = -EOPNOTSUPP;
-		goto out_unlock;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("alg_err:%x!!!!:\n", key->cipher));
-		goto out_unlock;
-	}
-	if (key_type == WEP40_ENCRYPTION ||
-	    key_type == WEP104_ENCRYPTION ||
-	    vif->type == NL80211_IFTYPE_ADHOC)
-		rtlpriv->sec.use_defaultkey = true;
-
-	/* <2> get key_idx */
-	key_idx = (u8) (key->keyidx);
-	if (key_idx > 3)
-		goto out_unlock;
-	/* <3> if pairwise key enable_hw_sec */
-	group_key = !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE);
-
-	/* wep always be group key, but there are two conditions:
-	 * 1) wep only: is just for wep enc, in this condition
-	 * rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION
-	 * will be true & enable_hw_sec will be set when wep
-	 * ke setting.
-	 * 2) wep(group) + AES(pairwise): some AP like cisco
-	 * may use it, in this condition enable_hw_sec will not
-	 * be set when wep key setting */
-	/* we must reset sec_info after lingked before set key,
-	 * or some flag will be wrong*/
-	if (vif->type == NL80211_IFTYPE_AP ||
-	    vif->type == NL80211_IFTYPE_MESH_POINT) {
-		if (!group_key || key_type == WEP40_ENCRYPTION ||
-		    key_type == WEP104_ENCRYPTION) {
-			if (group_key)
-				wep_only = true;
-			rtlpriv->cfg->ops->enable_hw_sec(hw);
-		}
-	} else {
-		if ((!group_key) || (vif->type == NL80211_IFTYPE_ADHOC) ||
-		    rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION) {
-			if (rtlpriv->sec.pairwise_enc_algorithm ==
-			    NO_ENCRYPTION &&
-			   (key_type == WEP40_ENCRYPTION ||
-			    key_type == WEP104_ENCRYPTION))
-				wep_only = true;
-			rtlpriv->sec.pairwise_enc_algorithm = key_type;
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set enable_hw_sec, key_type:%x(OPEN:0 WEP40:1 TKIP:2 AES:4 WEP104:5)\n",
-				  key_type));
-			rtlpriv->cfg->ops->enable_hw_sec(hw);
-		}
-	}
-	/* <4> set key based on cmd */
-	switch (cmd) {
-	case SET_KEY:
-		if (wep_only) {
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set WEP(group/pairwise) key\n"));
-			/* Pairwise key with an assigned MAC address. */
-			rtlpriv->sec.pairwise_enc_algorithm = key_type;
-			rtlpriv->sec.group_enc_algorithm = key_type;
-			/*set local buf about wep key. */
-			memcpy(rtlpriv->sec.key_buf[key_idx],
-			       key->key, key->keylen);
-			rtlpriv->sec.key_len[key_idx] = key->keylen;
-			eth_zero_addr(mac_addr);
-		} else if (group_key) {	/* group key */
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set group key\n"));
-			/* group key */
-			rtlpriv->sec.group_enc_algorithm = key_type;
-			/*set local buf about group key. */
-			memcpy(rtlpriv->sec.key_buf[key_idx],
-			       key->key, key->keylen);
-			rtlpriv->sec.key_len[key_idx] = key->keylen;
-			ether_addr_copy(mac_addr, bcast_addr);
-		} else {	/* pairwise key */
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set pairwise key\n"));
-			if (!sta) {
-				RT_ASSERT(false,
-					  ("pairwise key without mac_addr\n"));
-
-				err = -EOPNOTSUPP;
-				goto out_unlock;
-			}
-			/* Pairwise key with an assigned MAC address. */
-			rtlpriv->sec.pairwise_enc_algorithm = key_type;
-			/*set local buf about pairwise key. */
-			memcpy(rtlpriv->sec.key_buf[PAIRWISE_KEYIDX],
-			       key->key, key->keylen);
-			rtlpriv->sec.key_len[PAIRWISE_KEYIDX] = key->keylen;
-			rtlpriv->sec.pairwise_key =
-			    rtlpriv->sec.key_buf[PAIRWISE_KEYIDX];
-			ether_addr_copy(mac_addr, sta->addr);
-		}
-		rtlpriv->cfg->ops->set_key(hw, key_idx, mac_addr,
-					   group_key, key_type, wep_only,
-					   false);
-		/* <5> tell mac80211 do something: */
-		/*must use sw generate IV, or can not work !!!!. */
-		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
-		key->hw_key_idx = key_idx;
-		if (key_type == TKIP_ENCRYPTION)
-			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
-		/*use software CCMP encryption for management frames (MFP) */
-		if (key_type == AESCCMP_ENCRYPTION)
-			key->flags |= IEEE80211_KEY_FLAG_SW_MGMT;
-		break;
-	case DISABLE_KEY:
-		RT_TRACE(COMP_SEC, DBG_DMESG,
-			 ("disable key delete one entry\n"));
-		/*set local buf about wep key. */
-		if (vif->type == NL80211_IFTYPE_AP ||
-		    vif->type == NL80211_IFTYPE_MESH_POINT) {
-			if (sta)
-				stg_rtl_cam_del_entry(hw, sta->addr);
-		}
-		memset(rtlpriv->sec.key_buf[key_idx], 0, key->keylen);
-		rtlpriv->sec.key_len[key_idx] = 0;
-		eth_zero_addr(mac_addr);
-		/*
-		 *mac80211 will delete entrys one by one,
-		 *so don't use stg_rtl_cam_reset_all_entry
-		 *or clear all entry here.
-		 */
-		stg_rtl_cam_delete_one_entry(hw, mac_addr, key_idx);
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("cmd_err:%x!!!!:\n", cmd));
-	}
-out_unlock:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-	rtlpriv->sec.being_setkey = false;
-	return err;
-}
-
-static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	bool radio_state;
-	bool blocked;
-	u8 valid = 0;
-
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		return;
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-
-	/*if Radio On return true here */
-	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
-
-	if (valid) {
-		if (unlikely(radio_state != rtlpriv->rfkill.rfkill_state)) {
-			rtlpriv->rfkill.rfkill_state = radio_state;
-
-			RT_TRACE(COMP_RF, DBG_DMESG,
-				 (KERN_INFO "wireless radio switch turned %s\n",
-				  radio_state ? "on" : "off"));
-
-			blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
-			wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
-		}
-	}
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-
-/* this function is called by mac80211 to flush tx buffer
- * before switch channle or power save, or tx buffer packet
- * maybe send after offchannel or rf sleep, this may cause
- * dis-association by AP */
-static void rtl_op_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-			 u32 queues, bool drop)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->intf_ops->flush)
-		rtlpriv->intf_ops->flush(hw, queues, drop);
-}
-
-const struct ieee80211_ops rtl92e_ops = {
-	.start = rtl_op_start,
-	.stop = rtl_op_stop,
-	.tx = rtl_op_tx,
-	.add_interface = rtl_op_add_interface,
-	.remove_interface = rtl_op_remove_interface,
-	.change_interface = rtl_op_change_interface,
-#ifdef CONFIG_PM
-	.suspend = rtl_op_suspend,
-	.resume = rtl_op_resume,
-#endif
-	.config = rtl_op_config,
-	.configure_filter = rtl_op_configure_filter,
-	.set_key = rtl_op_set_key,
-	.conf_tx = rtl_op_conf_tx,
-	.bss_info_changed = rtl_op_bss_info_changed,
-	.get_tsf = rtl_op_get_tsf,
-	.set_tsf = rtl_op_set_tsf,
-	.reset_tsf = rtl_op_reset_tsf,
-	.sta_notify = rtl_op_sta_notify,
-	.ampdu_action = rtl_op_ampdu_action,
-	.sw_scan_start = rtl_op_sw_scan_start,
-	.sw_scan_complete = rtl_op_sw_scan_complete,
-	.rfkill_poll = rtl_op_rfkill_poll,
-	.sta_add = rtl_op_sta_add,
-	.sta_remove = rtl_op_sta_remove,
-	.flush = rtl_op_flush,
-};
diff --git a/drivers/staging/rtl8192ee/core.h b/drivers/staging/rtl8192ee/core.h
deleted file mode 100644
index ef75ad5f029a..000000000000
--- a/drivers/staging/rtl8192ee/core.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * Tmis program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * Tme full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_CORE_H__
-#define __RTL_CORE_H__
-
-#define RTL_SUPPORTED_FILTERS		\
-	(FIF_PROMISC_IN_BSS | \
-	FIF_ALLMULTI | FIF_CONTROL | \
-	FIF_OTHER_BSS | \
-	FIF_FCSFAIL | \
-	FIF_BCN_PRBRESP_PROMISC)
-
-#define RTL_SUPPORTED_CTRL_FILTER	0xFF
-
-extern const struct ieee80211_ops rtl92e_ops;
-#endif
diff --git a/drivers/staging/rtl8192ee/debug.c b/drivers/staging/rtl8192ee/debug.c
deleted file mode 100644
index feec3948585d..000000000000
--- a/drivers/staging/rtl8192ee/debug.c
+++ /dev/null
@@ -1,978 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * Tmis program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "cam.h"
-
-#define GET_INODE_DATA(__node)		PDE_DATA(__node)
-
-
-void rtl92e_dbgp_flag_init(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 i;
-
-	rtlpriv->dbg.global_debuglevel = DBG_DMESG;
-
-	rtlpriv->dbg.global_debugcomponents =
-		COMP_ERR |
-		COMP_FW |
-		COMP_INIT |
-		COMP_RECV |
-		COMP_SEND |
-		COMP_MLME |
-		COMP_SCAN |
-		COMP_INTR |
-		COMP_LED |
-		COMP_SEC |
-		COMP_BEACON |
-		COMP_RATE |
-		COMP_RXDESC |
-		COMP_DIG |
-		COMP_TXAGC |
-		COMP_POWER |
-		COMP_POWER_TRACKING |
-		COMP_BB_POWERSAVING |
-		COMP_SWAS |
-		COMP_RF |
-		COMP_TURBO |
-		COMP_RATR |
-		COMP_CMD |
-		COMP_EASY_CONCURRENT |
-		COMP_EFUSE |
-		COMP_QOS | COMP_MAC80211 | COMP_REGD |
-		COMP_CHAN |
-		COMP_BT_COEXIST |
-		COMP_IQK |
-		0;
-
-	for (i = 0; i < DBGP_TYPE_MAX; i++)
-		rtlpriv->dbg.dbgp_type[i] = 0;
-
-	/*Init Debug flag enable condition */
-}
-
-static struct proc_dir_entry *proc_topdir;
-
-static int rtl_proc_get_mac_0(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x000;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_0(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_0, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_0 = {
-	.open = dl_proc_open_mac_0,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_1(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x100;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_1(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_1, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_1 = {
-	.open = dl_proc_open_mac_1,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_2(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x200;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_2(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_2, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_2 = {
-	.open = dl_proc_open_mac_2,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_3(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x300;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_3(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_3, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_3 = {
-	.open = dl_proc_open_mac_3,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_4(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x400;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_4(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_4, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_4 = {
-	.open = dl_proc_open_mac_4,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_5(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x500;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_5(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_5, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_5 = {
-	.open = dl_proc_open_mac_5,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_6(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x600;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_6(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_6, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_6 = {
-	.open = dl_proc_open_mac_6,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_7(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x700;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_7(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_7, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_7 = {
-	.open = dl_proc_open_mac_7,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_8(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0x800;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_8(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_8, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_8 = {
-	.open = dl_proc_open_bb_8,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_9(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0x900;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_9(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_9, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_9 = {
-	.open = dl_proc_open_bb_9,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_a(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xa00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_a(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_a, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_a = {
-	.open = dl_proc_open_bb_a,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_b(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xb00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_b(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_b, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_b = {
-	.open = dl_proc_open_bb_b,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_c(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xc00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_c(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_c, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_c = {
-	.open = dl_proc_open_bb_c,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_d(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xd00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_d(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_d, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_d = {
-	.open = dl_proc_open_bb_d,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_e(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xe00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_e(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_e, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_e = {
-	.open = dl_proc_open_bb_e,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_f(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xf00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_f(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_f, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_f = {
-	.open = dl_proc_open_bb_f,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_reg_rf_a(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n;
-	int max = 0x40;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n);
-		for (i = 0; i < 4 && n <= max; n += 1, i++)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_rfreg(hw, RF90_PATH_A, n, 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_rf_a(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_reg_rf_a, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_rf_a = {
-	.open = dl_proc_open_rf_a,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_reg_rf_b(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n;
-	int max = 0x40;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n);
-		for (i = 0; i < 4 && n <= max; n += 1, i++)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_rfreg(hw, RF90_PATH_B, n,
-						 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_rf_b(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_reg_rf_b, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_rf_b = {
-	.open = dl_proc_open_rf_b,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_cam_register_1(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 target_cmd = 0;
-	u32 target_val = 0;
-	u8 entry_i = 0;
-	u32 ulstatus;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	seq_puts(m,
-	    "\n#################### SECURITY CAM (0-10) ##################\n ");
-
-	for (j = 0; j < 11; j++) {
-		seq_printf(m, "\nD:  %2x > ", j);
-		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			/* polling bit, and No Write enable, and address  */
-			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
-			target_cmd = target_cmd | BIT(31);
-
-			/* Check polling bit is clear */
-			while ((i--) >= 0) {
-				ulstatus = rtl_read_dword(rtlpriv,
-						rtlpriv->cfg->maps[RWCAM]);
-				if (ulstatus & BIT(31))
-					continue;
-				else
-					break;
-			}
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_cmd);
-			target_val = rtl_read_dword(rtlpriv,
-						    rtlpriv->cfg->maps[RCAMO]);
-			seq_printf(m, "%8.8x ", target_val);
-		}
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_cam_1(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_cam_register_1,
-			   GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_cam_1 = {
-	.open = dl_proc_open_cam_1,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_cam_register_2(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 target_cmd = 0;
-	u32 target_val = 0;
-	u8 entry_i = 0;
-	u32 ulstatus;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	seq_puts(m,
-	    "\n################### SECURITY CAM (11-21) ##################\n ");
-
-	for (j = 11; j < 22; j++) {
-		seq_printf(m, "\nD:  %2x > ", j);
-		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
-			target_cmd = target_cmd | BIT(31);
-
-			while ((i--) >= 0) {
-				ulstatus = rtl_read_dword(rtlpriv,
-						rtlpriv->cfg->maps[RWCAM]);
-				if (ulstatus & BIT(31))
-					continue;
-				else
-					break;
-			}
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_cmd);
-			target_val = rtl_read_dword(rtlpriv,
-						    rtlpriv->cfg->maps[RCAMO]);
-			seq_printf(m, "%8.8x ", target_val);
-		}
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_cam_2(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_cam_register_2,
-			   GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_cam_2 = {
-	.open = dl_proc_open_cam_2,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_cam_register_3(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 target_cmd = 0;
-	u32 target_val = 0;
-	u8 entry_i = 0;
-	u32 ulstatus;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	seq_puts(m,
-	    "\n################### SECURITY CAM (22-31) ##################\n ");
-
-	for (j = 22; j < TOTAL_CAM_ENTRY; j++) {
-		seq_printf(m, "\nD:  %2x > ", j);
-		for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			target_cmd = entry_i+CAM_CONTENT_COUNT*j;
-			target_cmd = target_cmd | BIT(31);
-
-			while ((i--) >= 0) {
-				ulstatus = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM]);
-				if (ulstatus & BIT(31))
-					continue;
-				else
-					break;
-			}
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_cmd);
-			target_val = rtl_read_dword(rtlpriv,
-						    rtlpriv->cfg->maps[RCAMO]);
-			seq_printf(m, "%8.8x ", target_val);
-		}
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_cam_3(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_cam_register_3,
-			   GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_cam_3 = {
-	.open = dl_proc_open_cam_3,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-void rtl_proc_add_one(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct proc_dir_entry *entry;
-
-	snprintf(rtlpriv->dbg.proc_name, 18, "%x-%x-%x-%x-%x-%x",
-		 rtlefuse->dev_addr[0], rtlefuse->dev_addr[1],
-		 rtlefuse->dev_addr[2], rtlefuse->dev_addr[3],
-		 rtlefuse->dev_addr[4], rtlefuse->dev_addr[5]);
-
-	rtlpriv->dbg.proc_dir = proc_mkdir(rtlpriv->dbg.proc_name, proc_topdir);
-	if (!rtlpriv->dbg.proc_dir) {
-		RT_TRACE(COMP_INIT, DBG_EMERG,
-			 ("Unable to init /proc/net/%s/%s\n",
-			  rtlpriv->cfg->name,
-			  rtlpriv->dbg.proc_name));
-		return;
-	}
-
-	entry = proc_create_data("mac-0", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_0, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, DBG_EMERG,
-			 ("Unable to initialize /proc/net/%s/%s/mac-0\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-1", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_1, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-1\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-2", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_2, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-2\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-3", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_3, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-3\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-4", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_4, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-4\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-5", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_5, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-5\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-6", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_6, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-6\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-7", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_7, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-7\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-8", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_8, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-8\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-9", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_9, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-9\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-a", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_a, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-a\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-b", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_b, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-b\n",
-		      rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-c", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_c, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-c\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-d", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_d, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-d\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-e", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_e, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-e\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-f", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_f, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-f\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("rf-a", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_rf_a, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/rf-a\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("rf-b", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_rf_b, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/rf-b\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("cam-1", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_cam_1, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/cam-1\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("cam-2", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_cam_2, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/cam-2\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("cam-3", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_cam_3, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/cam-3\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-}
-
-void rtl_proc_remove_one(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->dbg.proc_dir) {
-		remove_proc_entry("mac-0", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-1", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-2", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-3", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-4", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-5", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-6", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-7", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-8", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-9", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-a", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-b", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-c", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-d", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-e", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-f", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("rf-a", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("rf-b", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("cam-1", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("cam-2", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("cam-3", rtlpriv->dbg.proc_dir);
-
-		remove_proc_entry(rtlpriv->dbg.proc_name, proc_topdir);
-
-		rtlpriv->dbg.proc_dir = NULL;
-	}
-}
-
-void rtl_proc_add_topdir(void)
-{
-	proc_topdir = proc_mkdir("rtlwifi", init_net.proc_net);
-}
-
-void rtl_proc_remove_topdir(void)
-{
-	if (proc_topdir)
-		remove_proc_entry("rtlwifi", init_net.proc_net);
-}
diff --git a/drivers/staging/rtl8192ee/debug.h b/drivers/staging/rtl8192ee/debug.h
deleted file mode 100644
index 093128d1f368..000000000000
--- a/drivers/staging/rtl8192ee/debug.h
+++ /dev/null
@@ -1,221 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * Tmis program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * Tmis program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * Tme full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_DEBUG_H__
-#define __RTL_DEBUG_H__
-
-/*--------------------------------------------------------------
-			Debug level
---------------------------------------------------------------*/
-/*
- *Fatal bug.
- *For example, Tx/Rx/IO locked up,
- *memory access violation,
- *resource allocation failed,
- *unexpected HW behavior, HW BUG
- *and so on.
- */
-#define DBG_EMERG			1
-
-/*
- *Abnormal, rare, or unexpeted cases.
- *For example, Packet/IO Ctl canceled,
- *device suprisely unremoved and so on.
- */
-#define	DBG_WARNING			2
-
-/*
- *Normal case driver developer should
- *open, we can see link status like
- *assoc/AddBA/DHCP/adapter start and
- *so on basic and useful infromations.
- */
-#define DBG_DMESG			3
-
-/*
- *Normal case with useful information
- *about current SW or HW state.
- *For example, Tx/Rx descriptor to fill,
- *Tx/Rx descriptor completed status,
- *SW protocol state change, dynamic
- *mechanism state change and so on.
- */
-#define DBG_LOUD			4
-
-/*
- *Normal case with detail execution
- *flow or information.
- */
-#define	DBG_TRACE			5
-
-/*--------------------------------------------------------------
-		Define the rt_trace components
---------------------------------------------------------------*/
-#define COMP_ERR			BIT(0)
-#define COMP_FW				BIT(1)
-#define COMP_INIT			BIT(2)	/*For init/deinit */
-#define COMP_RECV			BIT(3)	/*For Rx. */
-#define COMP_SEND			BIT(4)	/*For Tx. */
-#define COMP_MLME			BIT(5)	/*For MLME. */
-#define COMP_SCAN			BIT(6)	/*For Scan. */
-#define COMP_INTR			BIT(7)	/*For interrupt Related. */
-#define COMP_LED			BIT(8)	/*For LED. */
-#define COMP_SEC			BIT(9)	/*For sec. */
-#define COMP_BEACON			BIT(10)	/*For beacon. */
-#define COMP_RATE			BIT(11)	/*For rate. */
-#define COMP_RXDESC			BIT(12)	/*For rx desc. */
-#define COMP_DIG			BIT(13)	/*For DIG */
-#define COMP_TXAGC			BIT(14)	/*For Tx power */
-#define COMP_HIPWR			BIT(15)	/*For High Power Mechanism */
-#define COMP_POWER			BIT(16)	/*For lps/ips/aspm. */
-#define COMP_POWER_TRACKING		BIT(17)	/*For TX POWER TRACKING */
-#define COMP_BB_POWERSAVING		BIT(18)
-#define COMP_SWAS			BIT(19)	/*For SW Antenna Switch */
-#define COMP_RF				BIT(20)	/*For RF. */
-#define COMP_TURBO			BIT(21)	/*For EDCA TURBO. */
-#define COMP_RATR			BIT(22)
-#define COMP_CMD			BIT(23)
-#define COMP_EFUSE			BIT(24)
-#define COMP_QOS			BIT(25)
-#define COMP_MAC80211			BIT(26)
-#define COMP_REGD			BIT(27)
-#define COMP_CHAN			BIT(28)
-#define COMP_EASY_CONCURRENT		BIT(29)
-#define COMP_BT_COEXIST			BIT(30)
-#define COMP_IQK			BIT(31)
-
-/*--------------------------------------------------------------
-		Define the rt_print components
---------------------------------------------------------------*/
-/* Define EEPROM and EFUSE  check module bit*/
-#define EEPROM_W			BIT(0)
-#define EFUSE_PG			BIT(1)
-#define EFUSE_READ_ALL			BIT(2)
-
-/* Define init check for module bit*/
-#define	INIT_EEPROM			BIT(0)
-#define	INIT_TxPower			BIT(1)
-#define	INIT_IQK			BIT(2)
-#define	INIT_RF				BIT(3)
-
-/* Define PHY-BB/RF/MAC check module bit */
-#define	PHY_BBR				BIT(0)
-#define	PHY_BBW				BIT(1)
-#define	PHY_RFR				BIT(2)
-#define	PHY_RFW				BIT(3)
-#define	PHY_MACR			BIT(4)
-#define	PHY_MACW			BIT(5)
-#define	PHY_ALLR			BIT(6)
-#define	PHY_ALLW			BIT(7)
-#define	PHY_TXPWR			BIT(8)
-#define	PHY_PWRDIFF			BIT(9)
-
-/* Define Dynamic Mechanism check module bit --> FDM */
-#define WA_IOT				BIT(0)
-#define DM_PWDB				BIT(1)
-#define DM_MONITOR			BIT(2)
-#define DM_DIG				BIT(3)
-#define DM_EDCA_TURBO			BIT(4)
-
-enum dbgp_flag_e {
-	FQOS = 0,
-	FTX = 1,
-	FRX = 2,
-	FSEC = 3,
-	FMGNT = 4,
-	FMLME = 5,
-	FRESOURCE = 6,
-	FBEACON = 7,
-	FISR = 8,
-	FPHY = 9,
-	FMP = 10,
-	FEEPROM = 11,
-	FPWR = 12,
-	FDM = 13,
-	FDBGCtrl = 14,
-	FC2H = 15,
-	FBT = 16,
-	FINIT = 17,
-	FIOCTL = 18,
-	DBGP_TYPE_MAX
-};
-
-#define RT_ASSERT(_exp , fmt)				\
-	do { \
-		if (!(_exp))	{			\
-			pr_debug("%s:%s(): ", KBUILD_MODNAME, \
-			__func__);	\
-			pr_cont fmt;			\
-		} \
-	} while (0)
-
-#define RT_TRACE(comp, level, fmt)\
-	do { \
-		if (unlikely(((comp) & rtlpriv->dbg.global_debugcomponents) && \
-			((level) <= rtlpriv->dbg.global_debuglevel))) {\
-			pr_debug("%s-%d:%s():<%lx> ", \
-			KBUILD_MODNAME, \
-			rtlpriv->rtlhal.interfaceindex, __func__, \
-			in_interrupt());	\
-			pr_cont fmt;			\
-		} \
-	} while (0)
-
-#define RTPRINT(rtlpriv, dbgtype, dbgflag, fmt, ...)			\
-do {									\
-	if (unlikely(rtlpriv->dbg.dbgp_type[dbgtype] & dbgflag)) {	\
-		pr_debug(KBUILD_MODNAME ": " fmt,		\
-		       ##__VA_ARGS__);					\
-	}								\
-} while (0)
-
-#define RT_PRINT_DATA(rtlpriv, _comp, _level, _titlestring, _hexdata, \
-		_hexdatalen) \
-	do {\
-		if (unlikely(((_comp) & rtlpriv->dbg.global_debugcomponents) &&\
-			(_level <= rtlpriv->dbg.global_debuglevel)))	{ \
-			int __i;					\
-			u8 *ptr = (u8 *)_hexdata;			\
-			pr_debug("%s: ", KBUILD_MODNAME);	\
-			pr_cont("In process \"%s\" (pid %i):", \
-					current->comm,	\
-					current->pid); \
-			pr_cont(_titlestring);		\
-			for (__i = 0; __i < (int)_hexdatalen; __i++) {	\
-				pr_cont("%02X%s", ptr[__i], (((__i + 1) % 4) \
-							== 0) ? "  " : " ");\
-				if (((__i + 1) % 16) == 0)	\
-					pr_cont("\n");	\
-			}				\
-			pr_cont("\n");			\
-		} \
-	} while (0)
-
-void rtl92e_dbgp_flag_init(struct ieee80211_hw *hw);
-void rtl_proc_add_one(struct ieee80211_hw *hw);
-void rtl_proc_remove_one(struct ieee80211_hw *hw);
-void rtl_proc_add_topdir(void);
-void rtl_proc_remove_topdir(void);
-#endif
diff --git a/drivers/staging/rtl8192ee/efuse.c b/drivers/staging/rtl8192ee/efuse.c
deleted file mode 100644
index 3fae18369831..000000000000
--- a/drivers/staging/rtl8192ee/efuse.c
+++ /dev/null
@@ -1,1233 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * Tmis program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include "wifi.h"
-#include "efuse.h"
-
-static const u8 MAX_PGPKT_SIZE = 9;
-static const u8 PGPKT_DATA_SIZE = 8;
-static const int EFUSE_MAX_SIZE = 512;
-
-static const struct efuse_map RTL8712_SDIO_EFUSE_TABLE[] = {
-	{0, 0, 0, 2},
-	{0, 1, 0, 2},
-	{0, 2, 0, 2},
-	{1, 0, 0, 1},
-	{1, 0, 1, 1},
-	{1, 1, 0, 1},
-	{1, 1, 1, 3},
-	{1, 3, 0, 17},
-	{3, 3, 1, 48},
-	{10, 0, 0, 6},
-	{10, 3, 0, 1},
-	{10, 3, 1, 1},
-	{11, 0, 0, 28}
-};
-
-static void efuse92e_shadow_read_1byte(struct ieee80211_hw *hw, u16 offset,
-				       u8 *value);
-static void efuse92e_shadow_read_2byte(struct ieee80211_hw *hw, u16 offset,
-				       u16 *value);
-static void efuse92e_shadow_read_4byte(struct ieee80211_hw *hw, u16 offset,
-				       u32 *value);
-static void efuse92e_shadow_write_1byte(struct ieee80211_hw *hw, u16 offset,
-					u8 value);
-static void efuse92e_shadow_write_2byte(struct ieee80211_hw *hw, u16 offset,
-					u16 value);
-static void efuse92e_shadow_write_4byte(struct ieee80211_hw *hw, u16 offset,
-					u32 value);
-static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr,
-				u8 data);
-static void efuse_read_all_map(struct ieee80211_hw *hw, u8 *efuse);
-static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset,
-				u8 *data);
-static int efuse_pg_packet_write(struct ieee80211_hw *hw, u8 offset,
-				 u8 word_en, u8 *data);
-static void efuse_word_enable_data_read(u8 word_en, u8 *sourdata,
-					u8 *targetdata);
-static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
-				       u16 efuse_addr, u8 word_en, u8 *data);
-static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite,
-			       u8 pwrstate);
-static u16 efuse_get_current_size(struct ieee80211_hw *hw);
-static u8 efuse_calculate_word_cnts(u8 word_en);
-
-void efuse92e_initialize(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 bytetemp;
-	u8 temp;
-
-	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1);
-	temp = bytetemp | 0x20;
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1, temp);
-
-	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1);
-	temp = bytetemp & 0xFE;
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1, temp);
-
-	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3);
-	temp = bytetemp | 0x80;
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3, temp);
-
-	rtl_write_byte(rtlpriv, 0x2F8, 0x3);
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
-}
-
-u8 stg_efuse_read_1byte(struct ieee80211_hw *hw, u16 address)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 data;
-	u8 bytetemp;
-	u8 temp;
-	u32 k = 0;
-	const u32 efuse_real_content_len =
-		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
-
-	if (address < efuse_real_content_len) {
-		temp = address & 0xFF;
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-			       temp);
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
-		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-			       temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-		temp = bytetemp & 0x7F;
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
-			       temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-		while (!(bytetemp & 0x80)) {
-			bytetemp = rtl_read_byte(rtlpriv,
-						 rtlpriv->cfg->
-						 maps[EFUSE_CTRL] + 3);
-			k++;
-			if (k == 1000) {
-				k = 0;
-				break;
-			}
-		}
-		data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-		return data;
-	} else {
-		return 0xFF;
-	}
-}
-EXPORT_SYMBOL(stg_efuse_read_1byte);
-
-void efuse92e_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 bytetemp;
-	u8 temp;
-	u32 k = 0;
-	const u32 efuse_real_content_len =
-		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("Addr=%x Data =%x\n", address, value));
-
-	if (address < efuse_real_content_len) {
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], value);
-
-		temp = address & 0xFF;
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-			       temp);
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
-
-		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
-		rtl_write_byte(rtlpriv,
-			       rtlpriv->cfg->maps[EFUSE_CTRL] + 2, temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-		temp = bytetemp | 0x80;
-		rtl_write_byte(rtlpriv,
-			       rtlpriv->cfg->maps[EFUSE_CTRL] + 3, temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-
-		while (bytetemp & 0x80) {
-			bytetemp = rtl_read_byte(rtlpriv,
-						 rtlpriv->cfg->
-						 maps[EFUSE_CTRL] + 3);
-			k++;
-			if (k == 100) {
-				k = 0;
-				break;
-			}
-		}
-	}
-}
-
-void read92e_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 value32;
-	u8 readbyte;
-	u16 retry;
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-		       (_offset & 0xff));
-	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-		       ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
-
-	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
-		       (readbyte & 0x7f));
-
-	retry = 0;
-	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-	while (!(((value32 >> 24) & 0xff) & 0x80) && (retry < 10000)) {
-		value32 = rtl_read_dword(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL]);
-		retry++;
-	}
-
-	udelay(50);
-	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-
-	*pbuf = (u8) (value32 & 0xff);
-}
-
-void read92e_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte,
-		   u8 *pbuf)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 *efuse_tbl;
-	u8 rtemp8[1];
-	u16 efuse_addr = 0;
-	u8 offset, wren;
-	u8 u1temp = 0;
-	u16 i;
-	u16 j;
-	const u16 efuse_max_section =
-		rtlpriv->cfg->maps[EFUSE_MAX_SECTION_MAP];
-	const u32 efuse_real_content_len =
-		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
-	u16 **efuse_word;
-	u16 efuse_utilized = 0;
-	u8 efuse_usage;
-
-	if ((_offset + _size_byte) > rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]) {
-		RT_TRACE(COMP_EFUSE, DBG_LOUD,
-			 ("read92e_efuse(): Invalid offset(%#x) with read bytes(%#x)!!\n",
-			 _offset, _size_byte));
-		return;
-	}
-
-	/* allocate memory for efuse_tbl and efuse_word */
-	efuse_tbl = kmalloc(rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE] *
-			    sizeof(u8), GFP_ATOMIC);
-	if (!efuse_tbl)
-		return;
-	efuse_word = kzalloc(EFUSE_MAX_WORD_UNIT * sizeof(u16 *), GFP_ATOMIC);
-	if (!efuse_word)
-		goto out;
-	for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
-		efuse_word[i] = kmalloc(efuse_max_section * sizeof(u16),
-					GFP_ATOMIC);
-		if (!efuse_word[i])
-			goto done;
-	}
-
-	for (i = 0; i < efuse_max_section; i++)
-		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++)
-			efuse_word[j][j] = 0xFFFF;
-
-	read92e_efuse_byte(hw, efuse_addr, rtemp8);
-	if (*rtemp8 != 0xFF) {
-		efuse_utilized++;
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
-			"Addr=%d\n", efuse_addr);
-		efuse_addr++;
-	}
-
-	while ((*rtemp8 != 0xFF) && (efuse_addr < efuse_real_content_len)) {
-		/*  Check PG header for section num.  */
-		if ((*rtemp8 & 0x1F) == 0x0F) {/* extended header */
-			u1temp = ((*rtemp8 & 0xE0) >> 5);
-			read92e_efuse_byte(hw, efuse_addr, rtemp8);
-
-			if ((*rtemp8 & 0x0F) == 0x0F) {
-				efuse_addr++;
-				read92e_efuse_byte(hw, efuse_addr, rtemp8);
-
-				if (*rtemp8 != 0xFF &&
-				    (efuse_addr < efuse_real_content_len)) {
-					efuse_addr++;
-				}
-				continue;
-			} else {
-				offset = ((*rtemp8 & 0xF0) >> 1) | u1temp;
-				wren = (*rtemp8 & 0x0F);
-				efuse_addr++;
-			}
-		} else {
-			offset = ((*rtemp8 >> 4) & 0x0f);
-			wren = (*rtemp8 & 0x0f);
-		}
-
-		if (offset < efuse_max_section) {
-			RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
-				"offset-%d Worden=%x\n", offset, wren);
-
-			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
-				if (!(wren & 0x01)) {
-					RTPRINT(rtlpriv, FEEPROM,
-						EFUSE_READ_ALL, "Addr=%d\n",
-						efuse_addr);
-
-					read92e_efuse_byte(hw, efuse_addr,
-							   rtemp8);
-					efuse_addr++;
-					efuse_utilized++;
-					efuse_word[i][offset] = (*rtemp8 &
-								 0xff);
-
-					if (efuse_addr >=
-					    efuse_real_content_len)
-						break;
-
-					RTPRINT(rtlpriv, FEEPROM,
-						EFUSE_READ_ALL, "Addr=%d\n",
-						efuse_addr);
-
-					read92e_efuse_byte(hw, efuse_addr,
-							   rtemp8);
-					efuse_addr++;
-					efuse_utilized++;
-					efuse_word[i][offset] |=
-					    (((u16) *rtemp8 << 8) & 0xff00);
-
-					if (efuse_addr >=
-					    efuse_real_content_len)
-						break;
-				}
-
-				wren >>= 1;
-			}
-		}
-
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
-			"Addr=%d\n", efuse_addr);
-		read92e_efuse_byte(hw, efuse_addr, rtemp8);
-		if (*rtemp8 != 0xFF && (efuse_addr < efuse_real_content_len)) {
-			efuse_utilized++;
-			efuse_addr++;
-		}
-	}
-
-	for (i = 0; i < efuse_max_section; i++) {
-		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++) {
-			efuse_tbl[(i * 8) + (j * 2)] =
-			    (efuse_word[j][i] & 0xff);
-			efuse_tbl[(i * 8) + ((j * 2) + 1)] =
-			    ((efuse_word[j][i] >> 8) & 0xff);
-		}
-	}
-
-	for (i = 0; i < _size_byte; i++)
-		pbuf[i] = efuse_tbl[_offset + i];
-
-	rtlefuse->efuse_usedbytes = efuse_utilized;
-	efuse_usage = (u8) ((efuse_utilized * 100) / efuse_real_content_len);
-	rtlefuse->efuse_usedpercentage = efuse_usage;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_BYTES,
-				      (u8 *)&efuse_utilized);
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_USAGE,
-				      (u8 *)&efuse_usage);
-done:
-	for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++)
-		kfree(efuse_word[i]);
-	kfree(efuse_word);
-out:
-	kfree(efuse_tbl);
-}
-
-bool efuse92e_shadow_update_chk(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 section_idx, i, Base;
-	u16 words_need = 0, hdr_num = 0, totalbytes, efuse_used;
-	bool bwordchanged, bresult = true;
-
-	for (section_idx = 0; section_idx < 16; section_idx++) {
-		Base = section_idx * 8;
-		bwordchanged = false;
-
-		for (i = 0; i < 8; i = i + 2) {
-			if ((rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i] !=
-			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i]) ||
-			    (rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i + 1] !=
-			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i +
-								   1])) {
-				words_need++;
-				bwordchanged = true;
-			}
-		}
-
-		if (bwordchanged)
-			hdr_num++;
-	}
-
-	totalbytes = hdr_num + words_need * 2;
-	efuse_used = rtlefuse->efuse_usedbytes;
-
-	if ((totalbytes + efuse_used) >=
-	    (EFUSE_MAX_SIZE - rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))
-		bresult = false;
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("efuse92e_shadow_update_chk(): totalbytes(%#x), hdr_num(%#x), words_need(%#x), efuse_used(%d)\n",
-		  totalbytes, hdr_num, words_need, efuse_used));
-
-	return bresult;
-}
-
-void efuse92e_shadow_read(struct ieee80211_hw *hw, u8 type,
-		       u16 offset, u32 *value)
-{
-	if (type == 1)
-		efuse92e_shadow_read_1byte(hw, offset, (u8 *)value);
-	else if (type == 2)
-		efuse92e_shadow_read_2byte(hw, offset, (u16 *)value);
-	else if (type == 4)
-		efuse92e_shadow_read_4byte(hw, offset, (u32 *)value);
-}
-EXPORT_SYMBOL(efuse92e_shadow_read);
-
-void efuse92e_shadow_write(struct ieee80211_hw *hw, u8 type, u16 offset,
-			   u32 value)
-{
-	if (type == 1)
-		efuse92e_shadow_write_1byte(hw, offset, (u8)value);
-	else if (type == 2)
-		efuse92e_shadow_write_2byte(hw, offset, (u16)value);
-	else if (type == 4)
-		efuse92e_shadow_write_4byte(hw, offset, (u32)value);
-}
-
-bool efuse92e_shadow_update(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u16 i, offset, base;
-	u8 word_en = 0x0F;
-	u8 first_pg = false;
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
-
-	if (!efuse92e_shadow_update_chk(hw)) {
-		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
-		memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
-		       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-		       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-
-		RT_TRACE(COMP_EFUSE, DBG_LOUD,
-			 ("efuse out of capacity!!\n"));
-		return false;
-	}
-	efuse_power_switch(hw, true, true);
-
-	for (offset = 0; offset < 16; offset++) {
-		word_en = 0x0F;
-		base = offset * 8;
-
-		for (i = 0; i < 8; i++) {
-			if (first_pg) {
-				word_en &= ~(BIT(i / 2));
-
-				rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
-				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
-			} else {
-				if (rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] !=
-				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i]) {
-					word_en &= ~(BIT(i / 2));
-
-					rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
-					    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
-				}
-			}
-		}
-
-		if (word_en != 0x0F) {
-			u8 tmpdata[8];
-			memcpy(tmpdata,
-			       (&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base]),
-			       8);
-			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD,
-				      "U-efuse\n", tmpdata, 8);
-
-			if (!efuse_pg_packet_write(hw, (u8) offset, word_en,
-						   tmpdata)) {
-				RT_TRACE(COMP_ERR, DBG_WARNING,
-					 ("PG section(%#x) fail!!\n", offset));
-				break;
-			}
-		}
-	}
-
-	efuse_power_switch(hw, true, false);
-	efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
-
-	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
-	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
-	return true;
-}
-
-void stg_rtl_efuse92e_shadow_map_update(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	if (rtlefuse->autoload_failflag) {
-		memset(&rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-		       0xFF, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-	} else {
-		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
-	}
-
-	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
-	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-}
-EXPORT_SYMBOL(stg_rtl_efuse92e_shadow_map_update);
-
-void efuse92e_force_write_vendor_Id(struct ieee80211_hw *hw)
-{
-	u8 tmpdata[8] = { 0xFF, 0xFF, 0xEC, 0x10, 0xFF, 0xFF, 0xFF, 0xFF };
-
-	efuse_power_switch(hw, true, true);
-	efuse_pg_packet_write(hw, 1, 0xD, tmpdata);
-	efuse_power_switch(hw, true, false);
-}
-
-void efuse92e_re_pg_section(struct ieee80211_hw *hw, u8 section_idx)
-{
-}
-
-static void efuse92e_shadow_read_1byte(struct ieee80211_hw *hw,
-				       u16 offset, u8 *value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
-}
-
-static void efuse92e_shadow_read_2byte(struct ieee80211_hw *hw,
-				       u16 offset, u16 *value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
-}
-
-static void efuse92e_shadow_read_4byte(struct ieee80211_hw *hw,
-				       u16 offset, u32 *value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] << 16;
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] << 24;
-}
-
-static void efuse92e_shadow_write_1byte(struct ieee80211_hw *hw,
-					u16 offset, u8 value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value;
-}
-
-static void efuse92e_shadow_write_2byte(struct ieee80211_hw *hw,
-					u16 offset, u16 value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value & 0x00FF;
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] = value >> 8;
-}
-
-static void efuse92e_shadow_write_4byte(struct ieee80211_hw *hw,
-					u16 offset, u32 value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] =
-	    (u8) (value & 0x000000FF);
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] =
-	    (u8) ((value >> 8) & 0x0000FF);
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] =
-	    (u8) ((value >> 16) & 0x00FF);
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] =
-	    (u8) ((value >> 24) & 0xFF);
-}
-
-int stg_efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmpidx = 0;
-	int bresult;
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-		       (u8) (addr & 0xff));
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-		       ((u8) ((addr >> 8) & 0x03)) |
-		       (rtl_read_byte(rtlpriv,
-				      rtlpriv->cfg->maps[EFUSE_CTRL] + 2) &
-			0xFC));
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
-
-	while (!(0x80 & rtl_read_byte(rtlpriv,
-				      rtlpriv->cfg->maps[EFUSE_CTRL] + 3)) &&
-	       (tmpidx < 100)) {
-		tmpidx++;
-	}
-
-	if (tmpidx < 100) {
-		*data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-		bresult = true;
-	} else {
-		*data = 0xff;
-		bresult = false;
-	}
-	return bresult;
-}
-EXPORT_SYMBOL(stg_efuse_one_byte_read);
-
-static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr, u8 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmpidx = 0;
-	bool bresult;
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("Addr = %x Data=%x\n", addr, data));
-
-	rtl_write_byte(rtlpriv,
-		       rtlpriv->cfg->maps[EFUSE_CTRL] + 1, (u8) (addr & 0xff));
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-		       (rtl_read_byte(rtlpriv,
-			 rtlpriv->cfg->maps[EFUSE_CTRL] +
-			 2) & 0xFC) | (u8) ((addr >> 8) & 0x03));
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], data);
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0xF2);
-
-	while ((0x80 & rtl_read_byte(rtlpriv,
-				     rtlpriv->cfg->maps[EFUSE_CTRL] + 3)) &&
-	       (tmpidx < 100)) {
-		tmpidx++;
-	}
-
-	if (tmpidx < 100)
-		bresult = true;
-	else
-		bresult = false;
-
-	return bresult;
-}
-
-static void efuse_read_all_map(struct ieee80211_hw *hw, u8 *efuse)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	efuse_power_switch(hw, false, true);
-	read92e_efuse(hw, 0, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE], efuse);
-	efuse_power_switch(hw, false, false);
-}
-
-static void efuse_read_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
-				u8 efuse_data, u8 offset, u8 *tmpdata,
-				u8 *readstate)
-{
-	bool bdataempty = true;
-	u8 hoffset;
-	u8 tmpidx;
-	u8 hworden;
-	u8 word_cnts;
-
-	hoffset = (efuse_data >> 4) & 0x0F;
-	hworden = efuse_data & 0x0F;
-	word_cnts = efuse_calculate_word_cnts(hworden);
-
-	if (hoffset == offset) {
-		for (tmpidx = 0; tmpidx < word_cnts * 2; tmpidx++) {
-			if (stg_efuse_one_byte_read(hw, *efuse_addr + 1 + tmpidx,
-						    &efuse_data)) {
-				tmpdata[tmpidx] = efuse_data;
-				if (efuse_data != 0xff)
-					bdataempty = false;
-			}
-		}
-
-		if (!bdataempty) {
-			*readstate = PG_STATE_DATA;
-		} else {
-			*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
-			*readstate = PG_STATE_HEADER;
-		}
-
-	} else {
-		*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
-		*readstate = PG_STATE_HEADER;
-	}
-}
-
-static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset, u8 *data)
-{
-	u8 readstate = PG_STATE_HEADER;
-
-	bool bcontinual = true;
-
-	u8 efuse_data, word_cnts = 0;
-	u16 efuse_addr = 0;
-	u8 hworden = 0;
-	u8 tmpdata[8];
-
-	if (data == NULL)
-		return false;
-	if (offset > 15)
-		return false;
-
-	memset(data, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
-	memset(tmpdata, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
-
-	while (bcontinual && (efuse_addr < EFUSE_MAX_SIZE)) {
-		if (readstate & PG_STATE_HEADER) {
-			if (stg_efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
-			    (efuse_data != 0xFF))
-				efuse_read_data_case1(hw, &efuse_addr,
-						      efuse_data, offset,
-						      tmpdata, &readstate);
-			else
-				bcontinual = false;
-		} else if (readstate & PG_STATE_DATA) {
-			efuse_word_enable_data_read(hworden, tmpdata, data);
-			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
-			readstate = PG_STATE_HEADER;
-		}
-	}
-
-	if ((data[0] == 0xff) && (data[1] == 0xff) &&
-	    (data[2] == 0xff) && (data[3] == 0xff) &&
-	    (data[4] == 0xff) && (data[5] == 0xff) &&
-	    (data[6] == 0xff) && (data[7] == 0xff))
-		return false;
-	else
-		return true;
-}
-
-static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
-				   u8 efuse_data, u8 offset,
-				   int *bcontinual, u8 *write_state,
-				   struct pgpkt_struct *target_pkt,
-				   int *repeat_times, int *bresult, u8 word_en)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct pgpkt_struct tmp_pkt;
-	int bdataempty = true;
-	u8 originaldata[8 * sizeof(u8)];
-	u8 badworden = 0x0F;
-	u8 match_word_en, tmp_word_en;
-	u8 tmpindex;
-	u8 tmp_header = efuse_data;
-	u8 tmp_word_cnts;
-
-	tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
-	tmp_pkt.word_en = tmp_header & 0x0F;
-	tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
-
-	if (tmp_pkt.offset != target_pkt->offset) {
-		*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
-		*write_state = PG_STATE_HEADER;
-	} else {
-		for (tmpindex = 0; tmpindex < (tmp_word_cnts * 2); tmpindex++) {
-			if (stg_efuse_one_byte_read(hw,
-						(*efuse_addr + 1 + tmpindex),
-						&efuse_data) &&
-			    (efuse_data != 0xFF))
-				bdataempty = false;
-		}
-
-		if (!bdataempty) {
-			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
-			*write_state = PG_STATE_HEADER;
-		} else {
-			match_word_en = 0x0F;
-			if (!((target_pkt->word_en & BIT(0)) |
-			    (tmp_pkt.word_en & BIT(0))))
-				match_word_en &= (~BIT(0));
-
-			if (!((target_pkt->word_en & BIT(1)) |
-			    (tmp_pkt.word_en & BIT(1))))
-				match_word_en &= (~BIT(1));
-
-			if (!((target_pkt->word_en & BIT(2)) |
-			    (tmp_pkt.word_en & BIT(2))))
-				match_word_en &= (~BIT(2));
-
-			if (!((target_pkt->word_en & BIT(3)) |
-			    (tmp_pkt.word_en & BIT(3))))
-				match_word_en &= (~BIT(3));
-
-			if ((match_word_en & 0x0F) != 0x0F) {
-				badworden = efuse_word_enable_data_write(hw,
-							*efuse_addr + 1,
-							tmp_pkt.word_en,
-							target_pkt->data);
-
-				if (0x0F != (badworden & 0x0F))	{
-					u8 reorg_offset = offset;
-					u8 reorg_worden = badworden;
-					efuse_pg_packet_write(hw, reorg_offset,
-							      reorg_worden,
-							      originaldata);
-				}
-
-				tmp_word_en = 0x0F;
-				if ((target_pkt->word_en & BIT(0)) ^
-				    (match_word_en & BIT(0)))
-					tmp_word_en &= (~BIT(0));
-
-				if ((target_pkt->word_en & BIT(1)) ^
-				    (match_word_en & BIT(1)))
-					tmp_word_en &= (~BIT(1));
-
-				if ((target_pkt->word_en & BIT(2)) ^
-				    (match_word_en & BIT(2)))
-					tmp_word_en &= (~BIT(2));
-
-				if ((target_pkt->word_en & BIT(3)) ^
-				    (match_word_en & BIT(3)))
-					tmp_word_en &= (~BIT(3));
-
-				if ((tmp_word_en & 0x0F) != 0x0F) {
-					*efuse_addr = efuse_get_current_size(hw);
-					target_pkt->offset = offset;
-					target_pkt->word_en = tmp_word_en;
-				} else {
-					*bcontinual = false;
-				}
-				*write_state = PG_STATE_HEADER;
-				*repeat_times += 1;
-				if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-					*bcontinual = false;
-					*bresult = false;
-				}
-			} else {
-				*efuse_addr += (2 * tmp_word_cnts) + 1;
-				target_pkt->offset = offset;
-				target_pkt->word_en = word_en;
-				*write_state = PG_STATE_HEADER;
-			}
-		}
-	}
-	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, "efuse PG_STATE_HEADER-1\n");
-}
-
-static void efuse_write_data_case2(struct ieee80211_hw *hw, u16 *efuse_addr,
-				   int *bcontinual, u8 *write_state,
-				   struct pgpkt_struct target_pkt,
-				   int *repeat_times, int *bresult)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct pgpkt_struct tmp_pkt;
-	u8 pg_header;
-	u8 tmp_header;
-	u8 originaldata[8 * sizeof(u8)];
-	u8 tmp_word_cnts;
-	u8 badworden = 0x0F;
-
-	pg_header = ((target_pkt.offset << 4) & 0xf0) | target_pkt.word_en;
-	efuse_one_byte_write(hw, *efuse_addr, pg_header);
-	stg_efuse_one_byte_read(hw, *efuse_addr, &tmp_header);
-
-	if (tmp_header == pg_header) {
-		*write_state = PG_STATE_DATA;
-	} else if (tmp_header == 0xFF) {
-		*write_state = PG_STATE_HEADER;
-		*repeat_times += 1;
-		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-			*bcontinual = false;
-			*bresult = false;
-		}
-	} else {
-		tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
-		tmp_pkt.word_en = tmp_header & 0x0F;
-
-		tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
-
-		memset(originaldata, 0xff,  8 * sizeof(u8));
-
-		if (efuse_pg_packet_read(hw, tmp_pkt.offset, originaldata)) {
-			badworden = efuse_word_enable_data_write(hw,
-								 *efuse_addr + 1,
-								 tmp_pkt.word_en,
-								 originaldata);
-
-			if (0x0F != (badworden & 0x0F)) {
-				u8 reorg_offset = tmp_pkt.offset;
-				u8 reorg_worden = badworden;
-				efuse_pg_packet_write(hw, reorg_offset,
-						      reorg_worden,
-						      originaldata);
-				*efuse_addr = efuse_get_current_size(hw);
-			} else {
-				*efuse_addr = *efuse_addr +
-					      (tmp_word_cnts * 2) + 1;
-			}
-		} else {
-			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
-		}
-
-		*write_state = PG_STATE_HEADER;
-		*repeat_times += 1;
-		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-			*bcontinual = false;
-			*bresult = false;
-		}
-
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-			"efuse PG_STATE_HEADER-2\n");
-	}
-}
-
-static int efuse_pg_packet_write(struct ieee80211_hw *hw,
-				 u8 offset, u8 word_en, u8 *data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct pgpkt_struct target_pkt;
-	u8 write_state = PG_STATE_HEADER;
-	int bcontinual = true, bdataempty = true, bresult = true;
-	u16 efuse_addr = 0;
-	u8 efuse_data;
-	u8 target_word_cnts = 0;
-	u8 badworden = 0x0F;
-	static int repeat_times;
-
-	if (efuse_get_current_size(hw) >= (EFUSE_MAX_SIZE -
-		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-			"efuse_pg_packet_write error\n");
-		return false;
-	}
-
-	target_pkt.offset = offset;
-	target_pkt.word_en = word_en;
-
-	memset(target_pkt.data, 0xFF,  8 * sizeof(u8));
-
-	efuse_word_enable_data_read(word_en, data, target_pkt.data);
-	target_word_cnts = efuse_calculate_word_cnts(target_pkt.word_en);
-
-	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, "efuse Power ON\n");
-
-	while (bcontinual && (efuse_addr < (EFUSE_MAX_SIZE -
-	       rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))) {
-		if (write_state == PG_STATE_HEADER) {
-			bdataempty = true;
-			badworden = 0x0F;
-			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-				"efuse PG_STATE_HEADER\n");
-
-			if (stg_efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
-			    (efuse_data != 0xFF))
-				efuse_write_data_case1(hw, &efuse_addr,
-						       efuse_data, offset,
-						       &bcontinual,
-						       &write_state,
-						       &target_pkt,
-						       &repeat_times, &bresult,
-						       word_en);
-			else
-				efuse_write_data_case2(hw, &efuse_addr,
-						       &bcontinual,
-						       &write_state,
-						       target_pkt,
-						       &repeat_times,
-						       &bresult);
-
-		} else if (write_state == PG_STATE_DATA) {
-			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-				"efuse PG_STATE_DATA\n");
-			badworden = 0x0f;
-			badworden =
-			    efuse_word_enable_data_write(hw, efuse_addr + 1,
-							 target_pkt.word_en,
-							 target_pkt.data);
-
-			if ((badworden & 0x0F) == 0x0F) {
-				bcontinual = false;
-			} else {
-				efuse_addr =
-				    efuse_addr + (2 * target_word_cnts) + 1;
-
-				target_pkt.offset = offset;
-				target_pkt.word_en = badworden;
-				target_word_cnts =
-				    efuse_calculate_word_cnts(target_pkt.
-							      word_en);
-				write_state = PG_STATE_HEADER;
-				repeat_times++;
-				if (repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-					bcontinual = false;
-					bresult = false;
-				}
-				RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-					"efuse PG_STATE_HEADER-3\n");
-			}
-		}
-	}
-
-	if (efuse_addr >= (EFUSE_MAX_SIZE -
-		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
-		RT_TRACE(COMP_EFUSE, DBG_LOUD,
-			 ("efuse_addr(%#x) Out of size!!\n", efuse_addr));
-	}
-
-	return true;
-}
-
-static void efuse_word_enable_data_read(u8 word_en, u8 *sourdata,
-					u8 *targetdata)
-{
-	if (!(word_en & BIT(0))) {
-		targetdata[0] = sourdata[0];
-		targetdata[1] = sourdata[1];
-	}
-
-	if (!(word_en & BIT(1))) {
-		targetdata[2] = sourdata[2];
-		targetdata[3] = sourdata[3];
-	}
-
-	if (!(word_en & BIT(2))) {
-		targetdata[4] = sourdata[4];
-		targetdata[5] = sourdata[5];
-	}
-
-	if (!(word_en & BIT(3))) {
-		targetdata[6] = sourdata[6];
-		targetdata[7] = sourdata[7];
-	}
-}
-
-static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
-				       u16 efuse_addr, u8 word_en, u8 *data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 tmpaddr;
-	u16 start_addr = efuse_addr;
-	u8 badworden = 0x0F;
-	u8 tmpdata[8];
-
-	memset(tmpdata, 0xff, PGPKT_DATA_SIZE);
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("word_en = %x efuse_addr=%x\n", word_en, efuse_addr));
-
-	if (!(word_en & BIT(0))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[0]);
-		efuse_one_byte_write(hw, start_addr++, data[1]);
-
-		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[0]);
-		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[1]);
-		if ((data[0] != tmpdata[0]) || (data[1] != tmpdata[1]))
-			badworden &= (~BIT(0));
-	}
-
-	if (!(word_en & BIT(1))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[2]);
-		efuse_one_byte_write(hw, start_addr++, data[3]);
-
-		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[2]);
-		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[3]);
-		if ((data[2] != tmpdata[2]) || (data[3] != tmpdata[3]))
-			badworden &= (~BIT(1));
-	}
-
-	if (!(word_en & BIT(2))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[4]);
-		efuse_one_byte_write(hw, start_addr++, data[5]);
-
-		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[4]);
-		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[5]);
-		if ((data[4] != tmpdata[4]) || (data[5] != tmpdata[5]))
-			badworden &= (~BIT(2));
-	}
-
-	if (!(word_en & BIT(3))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[6]);
-		efuse_one_byte_write(hw, start_addr++, data[7]);
-
-		stg_efuse_one_byte_read(hw, tmpaddr, &tmpdata[6]);
-		stg_efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[7]);
-		if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7]))
-			badworden &= (~BIT(3));
-	}
-
-	return badworden;
-}
-
-static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite, u8 pwrstate)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 tempval;
-	u16 tmpv16;
-
-	if (pwrstate && (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)) {
-		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192CE &&
-		    rtlhal->hw_type != HARDWARE_TYPE_RTL8192DE) {
-			rtl_write_byte(rtlpriv,
-				       rtlpriv->cfg->maps[EFUSE_ACCESS], 0x69);
-		} else {
-			tmpv16 = rtl_read_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
-			if (!(tmpv16 & rtlpriv->cfg->maps[EFUSE_PWC_EV12V])) {
-				tmpv16 |= rtlpriv->cfg->maps[EFUSE_PWC_EV12V];
-				rtl_write_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_ISO_CTRL],
-					       tmpv16);
-			}
-		}
-		tmpv16 = rtl_read_word(rtlpriv,
-				       rtlpriv->cfg->maps[SYS_FUNC_EN]);
-		if (!(tmpv16 & rtlpriv->cfg->maps[EFUSE_FEN_ELDR])) {
-			tmpv16 |= rtlpriv->cfg->maps[EFUSE_FEN_ELDR];
-			rtl_write_word(rtlpriv,
-				       rtlpriv->cfg->maps[SYS_FUNC_EN], tmpv16);
-		}
-
-		tmpv16 = rtl_read_word(rtlpriv, rtlpriv->cfg->maps[SYS_CLK]);
-		if ((!(tmpv16 & rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN])) ||
-		    (!(tmpv16 & rtlpriv->cfg->maps[EFUSE_ANA8M]))) {
-			tmpv16 |= (rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN] |
-				   rtlpriv->cfg->maps[EFUSE_ANA8M]);
-			rtl_write_word(rtlpriv,
-				       rtlpriv->cfg->maps[SYS_CLK], tmpv16);
-		}
-	}
-
-	if (pwrstate) {
-		if (bwrite) {
-			tempval = rtl_read_byte(rtlpriv,
-						rtlpriv->cfg->maps[EFUSE_TEST] +
-						3);
-
-			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-				tempval &= ~(BIT(3) | BIT(4) | BIT(5) | BIT(6));
-				tempval |= (VOLTAGE_V25 << 3);
-			} else if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE) {
-				tempval &= 0x0F;
-				tempval |= (VOLTAGE_V25 << 4);
-			}
-
-			rtl_write_byte(rtlpriv,
-				       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
-				       (tempval | 0x80));
-		}
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
-			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
-				       0x03);
-		}
-	} else {
-		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192CE &&
-		    rtlhal->hw_type != HARDWARE_TYPE_RTL8192DE)
-			rtl_write_byte(rtlpriv,
-				       rtlpriv->cfg->maps[EFUSE_ACCESS], 0);
-		if (bwrite) {
-			tempval = rtl_read_byte(rtlpriv,
-						rtlpriv->cfg->maps[EFUSE_TEST] +
-						3);
-			rtl_write_byte(rtlpriv,
-				       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
-				       (tempval & 0x7F));
-		}
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
-			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
-				       0x02);
-		}
-	}
-}
-
-static u16 efuse_get_current_size(struct ieee80211_hw *hw)
-{
-	int bcontinual = true;
-	u16 efuse_addr = 0;
-	u8 hoffset, hworden;
-	u8 efuse_data, word_cnts;
-
-	while (bcontinual &&
-	       stg_efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
-	       (efuse_addr < EFUSE_MAX_SIZE)) {
-		if (efuse_data != 0xFF) {
-			hoffset = (efuse_data >> 4) & 0x0F;
-			hworden = efuse_data & 0x0F;
-			word_cnts = efuse_calculate_word_cnts(hworden);
-			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
-		} else {
-			bcontinual = false;
-		}
-	}
-
-	return efuse_addr;
-}
-
-static u8 efuse_calculate_word_cnts(u8 word_en)
-{
-	u8 word_cnts = 0;
-	if (!(word_en & BIT(0)))
-		word_cnts++;
-	if (!(word_en & BIT(1)))
-		word_cnts++;
-	if (!(word_en & BIT(2)))
-		word_cnts++;
-	if (!(word_en & BIT(3)))
-		word_cnts++;
-	return word_cnts;
-}
diff --git a/drivers/staging/rtl8192ee/efuse.h b/drivers/staging/rtl8192ee/efuse.h
deleted file mode 100644
index cc3e1116b3e4..000000000000
--- a/drivers/staging/rtl8192ee/efuse.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_EFUSE_H_
-#define __RTL_EFUSE_H_
-
-#define EFUSE_IC_ID_OFFSET		506
-
-/*
-#define EFUSE_REAL_CONTENT_LEN	512
-#define EFUSE_MAP_LEN			128
-#define EFUSE_MAX_SECTION		16
-#define EFUSE_MAX_WORD_UNIT		4
-#define EFUSE_IC_ID_OFFSET		506
-*/
-
-#define EFUSE_MAX_WORD_UNIT		4
-
-#define EFUSE_INIT_MAP			0
-#define EFUSE_MODIFY_MAP		1
-
-#define PG_STATE_HEADER			0x01
-#define PG_STATE_WORD_0			0x02
-#define PG_STATE_WORD_1			0x04
-#define PG_STATE_WORD_2			0x08
-#define PG_STATE_WORD_3			0x10
-#define PG_STATE_DATA			0x20
-
-#define PG_SWBYTE_H			0x01
-#define PG_SWBYTE_L			0x02
-
-#define _POWERON_DELAY_
-#define _PRE_EXECUTE_READ_CMD_
-
-#define EFUSE_REPEAT_THRESHOLD_		3
-#define EFUSE_ERROE_HANDLE		1
-
-struct efuse_map {
-	u8 offset;
-	u8 word_start;
-	u8 byte_start;
-	u8 byte_cnts;
-};
-
-struct pgpkt_struct {
-	u8 offset;
-	u8 word_en;
-	u8 data[8];
-};
-
-enum efuse_data_item {
-	EFUSE_CHIP_ID = 0,
-	EFUSE_LDO_SETTING,
-	EFUSE_CLK_SETTING,
-	EFUSE_SDIO_SETTING,
-	EFUSE_CCCR,
-	EFUSE_SDIO_MODE,
-	EFUSE_OCR,
-	EFUSE_F0CIS,
-	EFUSE_F1CIS,
-	EFUSE_MAC_ADDR,
-	EFUSE_EEPROM_VER,
-	EFUSE_CHAN_PLAN,
-	EFUSE_TXPW_TAB
-};
-
-enum {
-	VOLTAGE_V25 = 0x03,
-	LDOE25_SHIFT = 28,
-};
-
-struct efuse_priv {
-	u8 id[2];
-	u8 ldo_setting[2];
-	u8 clk_setting[2];
-	u8 cccr;
-	u8 sdio_mode;
-	u8 ocr[3];
-	u8 cis0[17];
-	u8 cis1[48];
-	u8 mac_addr[6];
-	u8 eeprom_verno;
-	u8 channel_plan;
-	u8 tx_power_b[14];
-	u8 tx_power_g[14];
-};
-
-void read92e_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
-void efuse92e_initialize(struct ieee80211_hw *hw);
-u8 stg_efuse_read_1byte(struct ieee80211_hw *hw, u16 address);
-int stg_efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data);
-void efuse92e_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value);
-void read92e_efuse(struct ieee80211_hw *hw, u16 _offset,
-		   u16 _size_byte, u8 *pbuf);
-void efuse92e_shadow_read(struct ieee80211_hw *hw, u8 type,
-		          u16 offset, u32 *value);
-void efuse92e_shadow_write(struct ieee80211_hw *hw, u8 type,
-			   u16 offset, u32 value);
-bool efuse92e_shadow_update(struct ieee80211_hw *hw);
-bool efuse92e_shadow_update_chk(struct ieee80211_hw *hw);
-void stg_rtl_efuse92e_shadow_map_update(struct ieee80211_hw *hw);
-void efuse92e_force_write_vendor_Id(struct ieee80211_hw *hw);
-void efuse92e_re_pg_section(struct ieee80211_hw *hw, u8 section_idx);
-
-#endif
diff --git a/drivers/staging/rtl8192ee/pci.c b/drivers/staging/rtl8192ee/pci.c
deleted file mode 100644
index 0215aef1eacc..000000000000
--- a/drivers/staging/rtl8192ee/pci.c
+++ /dev/null
@@ -1,2384 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "core.h"
-#include "wifi.h"
-#include "pci.h"
-#include "base.h"
-#include "ps.h"
-#include "efuse.h"
-
-static const u16 pcibridge_vendors[PCI_BRIDGE_VENDOR_MAX] = {
-	INTEL_VENDOR_ID,
-	ATI_VENDOR_ID,
-	AMD_VENDOR_ID,
-	SIS_VENDOR_ID
-};
-
-static const u8 ac_to_hwq[] = {
-	VO_QUEUE,
-	VI_QUEUE,
-	BE_QUEUE,
-	BK_QUEUE
-};
-
-static u8 _rtl_mac_to_hwqueue(struct ieee80211_hw *hw,
-			      struct sk_buff *skb)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	__le16 fc = rtl_get_fc(skb);
-	u8 queue_index = skb_get_queue_mapping(skb);
-
-	if (unlikely(ieee80211_is_beacon(fc)))
-		return BEACON_QUEUE;
-	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
-		return MGNT_QUEUE;
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
-		if (ieee80211_is_nullfunc(fc))
-			return HIGH_QUEUE;
-
-	return ac_to_hwq[queue_index];
-}
-
-/* Update PCI dependent default settings*/
-static void _rtl_pci_update_default_setting(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
-	u8 init_aspm;
-
-	ppsc->reg_rfps_level = 0;
-	ppsc->b_support_aspm = 0;
-
-	/*Update PCI ASPM setting */
-	ppsc->const_amdpci_aspm = rtlpci->const_amdpci_aspm;
-	switch (rtlpci->const_pci_aspm) {
-	case 0:
-		/*No ASPM */
-		break;
-
-	case 1:
-		/*ASPM dynamically enabled/disable. */
-		ppsc->reg_rfps_level |= RT_RF_LPS_LEVEL_ASPM;
-		break;
-
-	case 2:
-		/*ASPM with Clock Req dynamically enabled/disable. */
-		ppsc->reg_rfps_level |= (RT_RF_LPS_LEVEL_ASPM |
-					 RT_RF_OFF_LEVL_CLK_REQ);
-		break;
-
-	case 3:
-		/*
-		 * Always enable ASPM and Clock Req
-		 * from initialization to halt.
-		 * */
-		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM);
-		ppsc->reg_rfps_level |= (RT_RF_PS_LEVEL_ALWAYS_ASPM |
-					 RT_RF_OFF_LEVL_CLK_REQ);
-		break;
-
-	case 4:
-		/*
-		 * Always enable ASPM without Clock Req
-		 * from initialization to halt.
-		 * */
-		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM |
-					  RT_RF_OFF_LEVL_CLK_REQ);
-		ppsc->reg_rfps_level |= RT_RF_PS_LEVEL_ALWAYS_ASPM;
-		break;
-	}
-
-	ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
-
-	/*Update Radio OFF setting */
-	switch (rtlpci->const_hwsw_rfoff_d3) {
-	case 1:
-		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
-			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
-		break;
-
-	case 2:
-		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
-			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
-		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
-		break;
-
-	case 3:
-		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_PCI_D3;
-		break;
-	}
-
-	/*Set HW definition to determine if it supports ASPM. */
-	switch (rtlpci->const_support_pciaspm) {
-	case 0:{
-			/*Not support ASPM. */
-			bool b_support_aspm = false;
-			ppsc->b_support_aspm = b_support_aspm;
-			break;
-		}
-	case 1:{
-			/*Support ASPM. */
-			bool b_support_aspm = true;
-			bool b_support_backdoor = true;
-			ppsc->b_support_aspm = b_support_aspm;
-
-			/*if (priv->oem_id == RT_CID_TOSHIBA &&
-			   !priv->ndis_adapter.amd_l1_patch)
-			   b_support_backdoor = false; */
-
-			ppsc->b_support_backdoor = b_support_backdoor;
-
-			break;
-		}
-	case 2:
-		/*ASPM value set by chipset. */
-		if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL) {
-			bool b_support_aspm = true;
-			ppsc->b_support_aspm = b_support_aspm;
-		}
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process\n"));
-		break;
-	}
-
-	/* toshiba aspm issue, toshiba will set aspm selfly
-	 * so we should not set aspm in driver */
-	pci_read_config_byte(rtlpci->pdev, 0x80, &init_aspm);
-	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8192SE &&
-	    init_aspm == 0x43)
-		ppsc->b_support_aspm = false;
-}
-
-static bool _rtl_pci_platform_switch_device_pci_aspm(struct ieee80211_hw *hw,
-						     u8 value)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool bresult = false;
-
-	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)
-		value |= 0x40;
-
-	pci_write_config_byte(rtlpci->pdev, 0x80, value);
-
-	return bresult;
-}
-
-/*When we set 0x01 to enable clk request. Set 0x0 to disable clk req.*/
-static bool _rtl_pci_switch_clk_req(struct ieee80211_hw *hw, u8 value)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool bresult = false;
-
-	pci_write_config_byte(rtlpci->pdev, 0x81, value);
-	bresult = true;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
-		udelay(100);
-
-	return bresult;
-}
-
-/*Disable RTL8192SE ASPM & Disable Pci Bridge ASPM*/
-static void rtl_pci_disable_aspm(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
-	/*Retrieve original configuration settings. */
-	u8 linkctrl_reg = pcipriv->ndis_adapter.linkctrl_reg;
-	u16 pcibridge_linkctrlreg = pcipriv->ndis_adapter.
-				pcibridge_linkctrlreg;
-	u16 aspmlevel = 0;
-
-	if (!ppsc->b_support_aspm)
-		return;
-
-	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
-		RT_TRACE(COMP_POWER, DBG_TRACE,
-			 ("PCI(Bridge) UNKNOWN.\n"));
-
-		return;
-	}
-
-	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
-		RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
-		_rtl_pci_switch_clk_req(hw, 0x0);
-	}
-
-	if (1) {
-		/*for promising device will in L0 state after an I/O. */
-		u8 tmp_u1b;
-		pci_read_config_byte(rtlpci->pdev, 0x80, &tmp_u1b);
-	}
-
-	/*Set corresponding value. */
-	aspmlevel |= BIT(0) | BIT(1);
-	linkctrl_reg &= ~aspmlevel;
-	pcibridge_linkctrlreg &= ~(BIT(0) | BIT(1));
-
-	_rtl_pci_platform_switch_device_pci_aspm(hw, linkctrl_reg);
-	udelay(50);
-
-	/*4 Disable Pci Bridge ASPM */
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-				     pcicfg_addrport + (num4bytes << 2));
-	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, pcibridge_linkctrlreg);
-
-	udelay(50);
-}
-
-/*
- *Enable RTL8192SE ASPM & Enable Pci Bridge ASPM for
- *power saving We should follow the sequence to enable
- *RTL8192SE first then enable Pci Bridge ASPM
- *or the system will show bluescreen.
- */
-static void rtl_pci_enable_aspm(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
-	u16 aspmlevel;
-	u8 u_pcibridge_aspmsetting;
-	u8 u_device_aspmsetting;
-
-	if (!ppsc->b_support_aspm)
-		return;
-
-	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
-		RT_TRACE(COMP_POWER, DBG_TRACE,
-			 ("PCI(Bridge) UNKNOWN.\n"));
-		return;
-	}
-
-	/*4 Enable Pci Bridge ASPM */
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-				     pcicfg_addrport + (num4bytes << 2));
-
-	u_pcibridge_aspmsetting =
-	    pcipriv->ndis_adapter.pcibridge_linkctrlreg |
-	    rtlpci->const_hostpci_aspm_setting;
-
-	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL)
-		u_pcibridge_aspmsetting &= ~BIT(0);
-
-	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, u_pcibridge_aspmsetting);
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("PlatformEnableASPM(): Write reg[%x] = %x\n",
-		  (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10),
-		  u_pcibridge_aspmsetting));
-
-	udelay(50);
-
-	/*Get ASPM level (with/without Clock Req) */
-	aspmlevel = rtlpci->const_devicepci_aspm_setting;
-	u_device_aspmsetting = pcipriv->ndis_adapter.linkctrl_reg;
-
-	/*_rtl_pci_platform_switch_device_pci_aspm(dev,*/
-	/*(priv->ndis_adapter.linkctrl_reg | ASPMLevel)); */
-
-	u_device_aspmsetting |= aspmlevel;
-
-	_rtl_pci_platform_switch_device_pci_aspm(hw, u_device_aspmsetting);
-
-	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
-		_rtl_pci_switch_clk_req(hw, (ppsc->reg_rfps_level &
-					     RT_RF_OFF_LEVL_CLK_REQ) ? 1 : 0);
-		RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
-	}
-	udelay(100);
-}
-
-static bool rtl_pci_get_amd_l1_patch(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-
-	bool status = false;
-	u8 offset_e0;
-	unsigned offset_e4;
-
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS, pcicfg_addrport + 0xE0);
-	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, 0xA0);
-
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS, pcicfg_addrport + 0xE0);
-	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &offset_e0);
-
-	if (offset_e0 == 0xA0) {
-		rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-					     pcicfg_addrport + 0xE4);
-		rtl_pci_raw_read_port_ulong(PCI_CONF_DATA, &offset_e4);
-		if (offset_e4 & BIT(23))
-			status = true;
-	}
-
-	return status;
-}
-
-static bool rtl_pci_check_buddy_priv(struct ieee80211_hw *hw,
-				     struct rtl_priv **buddy_priv)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	bool find_buddy_priv = false;
-	struct rtl_priv *tpriv = NULL;
-	struct rtl_pci_priv *tpcipriv = NULL;
-
-	if (!list_empty(&rtlpriv->glb_var->glb_priv_list)) {
-		list_for_each_entry(tpriv, &rtlpriv->glb_var->glb_priv_list,
-				    list) {
-			if (tpriv == NULL)
-				break;
-
-			tpcipriv = (struct rtl_pci_priv *)tpriv->priv;
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				 ("pcipriv->ndis_adapter.funcnumber %x\n",
-				  pcipriv->ndis_adapter.funcnumber));
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				 ("tpcipriv->ndis_adapter.funcnumber %x\n",
-				  tpcipriv->ndis_adapter.funcnumber));
-
-			if ((pcipriv->ndis_adapter.busnumber ==
-			     tpcipriv->ndis_adapter.busnumber) &&
-			    (pcipriv->ndis_adapter.devnumber ==
-			     tpcipriv->ndis_adapter.devnumber) &&
-			    (pcipriv->ndis_adapter.funcnumber !=
-			     tpcipriv->ndis_adapter.funcnumber)) {
-				find_buddy_priv = true;
-				break;
-			}
-		}
-	}
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("find_buddy_priv %d\n", find_buddy_priv));
-
-	if (find_buddy_priv)
-		*buddy_priv = tpriv;
-
-	return find_buddy_priv;
-}
-
-static void rtl_pci_get_linkcontrol_field(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u8 capabilityoffset = pcipriv->ndis_adapter.pcibridge_pciehdr_offset;
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-	u8 linkctrl_reg;
-	u8 num4bbytes;
-
-	num4bbytes = (capabilityoffset + 0x10) / 4;
-
-	/*Read  Link Control Register */
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-				     pcicfg_addrport + (num4bbytes << 2));
-	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &linkctrl_reg);
-
-	pcipriv->ndis_adapter.pcibridge_linkctrlreg = linkctrl_reg;
-}
-
-static void rtl_pci_parse_configuration(struct pci_dev *pdev,
-					struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-
-	u8 tmp;
-	int pos;
-	u8 linkctrl_reg;
-
-	/*Link Control Register */
-	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
-	pci_read_config_byte(pdev, pos + PCI_EXP_LNKCTL, &linkctrl_reg);
-	pcipriv->ndis_adapter.linkctrl_reg = linkctrl_reg;
-
-	RT_TRACE(COMP_INIT, DBG_TRACE,
-		 ("Link Control Register =%x\n",
-		  pcipriv->ndis_adapter.linkctrl_reg));
-
-	pci_read_config_byte(pdev, 0x98, &tmp);
-	tmp |= BIT(4);
-	pci_write_config_byte(pdev, 0x98, tmp);
-
-	tmp = 0x17;
-	pci_write_config_byte(pdev, 0x70f, tmp);
-}
-
-static void rtl_pci_init_aspm(struct ieee80211_hw *hw)
-{
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	_rtl_pci_update_default_setting(hw);
-
-	if (ppsc->reg_rfps_level & RT_RF_PS_LEVEL_ALWAYS_ASPM) {
-		/*Always enable ASPM & Clock Req. */
-		rtl_pci_enable_aspm(hw);
-		RT_SET_PS_LEVEL(ppsc, RT_RF_PS_LEVEL_ALWAYS_ASPM);
-	}
-}
-
-static void _rtl_pci_io_handler_init(struct device *dev,
-				     struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->io.dev = dev;
-
-	rtlpriv->io.write8_async = pci_write8_async;
-	rtlpriv->io.write16_async = pci_write16_async;
-	rtlpriv->io.write32_async = pci_write32_async;
-
-	rtlpriv->io.read8_sync = pci_read8_sync;
-	rtlpriv->io.read16_sync = pci_read16_sync;
-	rtlpriv->io.read32_sync = pci_read32_sync;
-}
-
-static bool _rtl_pci_update_earlymode_info(struct ieee80211_hw *hw,
-					   struct sk_buff *skb,
-					   struct rtl_tcb_desc *tcb_desc,
-					   u8 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct sk_buff *next_skb;
-	u8 additionlen = FCS_LEN;
-
-	/* here open is 4, wep/tkip is 8, aes is 12*/
-	if (info->control.hw_key)
-		additionlen += info->control.hw_key->icv_len;
-
-	/* The most skb num is 6 */
-	tcb_desc->empkt_num = 0;
-	spin_lock_bh(&rtlpriv->locks.waitq_lock);
-	skb_queue_walk(&rtlpriv->mac80211.skb_waitq[tid], next_skb) {
-		struct ieee80211_tx_info *next_info;
-
-		next_info = IEEE80211_SKB_CB(next_skb);
-		if (next_info->flags & IEEE80211_TX_CTL_AMPDU) {
-			tcb_desc->empkt_len[tcb_desc->empkt_num] =
-				next_skb->len + additionlen;
-			tcb_desc->empkt_num++;
-		} else {
-			break;
-		}
-
-		if (skb_queue_is_last(&rtlpriv->mac80211.skb_waitq[tid],
-				      next_skb))
-			break;
-
-		if (tcb_desc->empkt_num >= rtlhal->max_earlymode_num)
-			break;
-	}
-	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-	return true;
-}
-
-/* just for early mode now */
-static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct sk_buff *skb = NULL;
-	struct ieee80211_tx_info *info = NULL;
-	int tid; /* should be int */
-
-	if (!rtlpriv->rtlhal.b_earlymode_enable)
-		return;
-	if (rtlpriv->dm.supp_phymode_switch &&
-	    (rtlpriv->easy_concurrent_ctl.bswitch_in_process ||
-	    (rtlpriv->buddy_priv &&
-	     rtlpriv->buddy_priv->easy_concurrent_ctl.bswitch_in_process)))
-		return;
-	/* we juse use em for BE/BK/VI/VO */
-	for (tid = 7; tid >= 0; tid--) {
-		u8 hw_queue = ac_to_hwq[rtl92e_tid_to_ac(hw, tid)];
-		struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
-		while (!mac->act_scanning &&
-		       rtlpriv->psc.rfpwr_state == ERFON) {
-			struct rtl_tcb_desc tcb_desc;
-			memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-
-			spin_lock_bh(&rtlpriv->locks.waitq_lock);
-			if (!skb_queue_empty(&mac->skb_waitq[tid]) &&
-			    (ring->entries - skb_queue_len(&ring->queue) >
-			     rtlhal->max_earlymode_num)) {
-				skb = skb_dequeue(&mac->skb_waitq[tid]);
-			} else {
-				spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-				break;
-			}
-			spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-
-			/* Some macaddr can't do early mode. like
-			 * multicast/broadcast/no_qos data */
-			info = IEEE80211_SKB_CB(skb);
-			if (info->flags & IEEE80211_TX_CTL_AMPDU)
-				_rtl_pci_update_earlymode_info(hw, skb,
-							       &tcb_desc, tid);
-
-			rtlpriv->intf_ops->adapter_tx(hw, NULL, skb, &tcb_desc);
-		}
-	}
-}
-
-static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
-
-	while (skb_queue_len(&ring->queue)) {
-		struct sk_buff *skb;
-		struct ieee80211_tx_info *info;
-		__le16 fc;
-		u8 tid;
-		u8 *entry;
-
-
-		if (rtlpriv->use_new_trx_flow)
-			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
-		else
-			entry = (u8 *)(&ring->desc[ring->idx]);
-
-		if (rtlpriv->cfg->ops->is_tx_desc_closed &&
-		    !rtlpriv->cfg->ops->is_tx_desc_closed(hw, prio, ring->idx))
-			return;
-
-		ring->idx = (ring->idx + 1) % ring->entries;
-
-		skb = __skb_dequeue(&ring->queue);
-
-		pci_unmap_single(rtlpci->pdev,
-				 rtlpriv->cfg->ops->
-					     get_desc((u8 *)entry, true,
-						      HW_DESC_TXBUFF_ADDR),
-				 skb->len, PCI_DMA_TODEVICE);
-
-		/* remove early mode header */
-		if (rtlpriv->rtlhal.b_earlymode_enable)
-			skb_pull(skb, EM_HDR_LEN);
-
-		RT_TRACE((COMP_INTR | COMP_SEND), DBG_TRACE,
-			 ("new ring->idx:%d, free: skb_queue_len:%d, free: seq:%d\n",
-			  ring->idx,
-			  skb_queue_len(&ring->queue),
-			  *(u16 *)(skb->data + 22)));
-
-		if (prio == TXCMD_QUEUE) {
-			dev_kfree_skb(skb);
-			goto tx_status_ok;
-		}
-
-		/* for sw LPS, just after NULL skb send out, we can
-		 * sure AP knows that we are sleeping, our we should not let
-		 * rf to sleep
-		 */
-		fc = rtl_get_fc(skb);
-		if (ieee80211_is_nullfunc(fc)) {
-			if (ieee80211_has_pm(fc)) {
-				rtlpriv->mac80211.offchan_deley = true;
-				rtlpriv->psc.state_inap = 1;
-			} else {
-				rtlpriv->psc.state_inap = 0;
-			}
-		}
-		if (ieee80211_is_action(fc)) {
-			struct ieee80211_mgmt_compat *action_frame =
-				(struct ieee80211_mgmt_compat *)skb->data;
-			if (action_frame->u.action.u.ht_smps.action ==
-				WLAN_HT_ACTION_SMPS) {
-				dev_kfree_skb(skb);
-				goto tx_status_ok;
-			}
-		}
-
-		/* update tid tx pkt num */
-		tid = rtl_get_tid(skb);
-		if (tid <= 7)
-			rtlpriv->link_info.tidtx_inperiod[tid]++;
-
-		info = IEEE80211_SKB_CB(skb);
-		ieee80211_tx_info_clear_status(info);
-
-		info->flags |= IEEE80211_TX_STAT_ACK;
-		/*info->status.rates[0].count = 1; */
-
-		ieee80211_tx_status_irqsafe(hw, skb);
-
-		if ((ring->entries - skb_queue_len(&ring->queue)) == 2) {
-			RT_TRACE(COMP_ERR, DBG_LOUD,
-				 ("more desc left, wake skb_queue@%d,ring->idx = %d, skb_queue_len = 0x%d\n",
-					 prio, ring->idx,
-					 skb_queue_len(&ring->queue)));
-
-			ieee80211_wake_queue(hw, skb_get_queue_mapping
-					     (skb));
-		}
-tx_status_ok:
-		skb = NULL;
-	}
-
-	if (((rtlpriv->link_info.num_rx_inperiod +
-		rtlpriv->link_info.num_tx_inperiod) > 8) ||
-		(rtlpriv->link_info.num_rx_inperiod > 2)) {
-		rtl92e_lps_leave(hw);
-	}
-}
-
-static int _rtl_pci_init_one_rxdesc(struct ieee80211_hw *hw,
-				    u8 *entry, int rxring_idx, int desc_idx)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct sk_buff *skb;
-	u32 bufferaddress;
-	u8 tmp_one = 1;
-
-	skb = dev_alloc_skb(rtlpci->rxbuffersize);
-	if (!skb)
-		return 0;
-	rtlpci->rx_ring[rxring_idx].rx_buf[desc_idx] = skb;
-
-	/* just set skb->cb to mapping addr
-	 * for pci_unmap_single use
-	 */
-	*((dma_addr_t *)skb->cb) = pci_map_single(rtlpci->pdev,
-				skb_tail_pointer(skb), rtlpci->rxbuffersize,
-				PCI_DMA_FROMDEVICE);
-	bufferaddress = *((dma_addr_t *)skb->cb);
-	if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress))
-		return 0;
-	if (rtlpriv->use_new_trx_flow) {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-					    HW_DESC_RX_PREPARE,
-					    (u8 *)&bufferaddress);
-	} else {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-					    HW_DESC_RXBUFF_ADDR,
-					    (u8 *)&bufferaddress);
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-					    HW_DESC_RXPKT_LEN,
-					    (u8 *)&rtlpci->rxbuffersize);
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-					    HW_DESC_RXOWN,
-					    (u8 *)&tmp_one);
-	}
-	return 1;
-}
-
-/* inorder to receive 8K AMSDU we have set skb to
- * 9100bytes in init rx ring, but if this packet is
- * not a AMSDU, this so big packet will be sent to
- * TCP/IP directly, this cause big packet ping fail
- * like: "ping -s 65507", so here we will realloc skb
- * based on the true size of packet, I think mac80211
- * do it will be better, but now mac80211 haven't */
-
-/* but some platform will fail when alloc skb sometimes.
- * in this condition, we will send the old skb to
- * mac80211 directly, this will not cause any other
- * issues, but only be losted by TCP/IP */
-static void _rtl_pci_rx_to_mac80211(struct ieee80211_hw *hw,
-				    struct sk_buff *skb,
-				    struct ieee80211_rx_status rx_status)
-{
-	if (unlikely(!rtl92e_action_proc(hw, skb, false))) {
-		dev_kfree_skb_any(skb);
-	} else {
-		struct sk_buff *uskb = NULL;
-		u8 *pdata;
-
-		uskb = dev_alloc_skb(skb->len + 128);
-		if (likely(uskb)) {
-			memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
-			       sizeof(rx_status));
-			pdata = (u8 *)skb_put(uskb, skb->len);
-			memcpy(pdata, skb->data, skb->len);
-			dev_kfree_skb_any(skb);
-
-			ieee80211_rx_irqsafe(hw, uskb);
-		} else {
-			ieee80211_rx_irqsafe(hw, skb);
-		}
-	}
-}
-
-/*hsisr interrupt handler*/
-static void _rtl_pci_hs_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR],
-		       rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR]) |
-		       rtlpci->sys_irq_mask);
-}
-
-static void _rtl_receive_one(struct ieee80211_hw *hw, struct sk_buff *skb,
-			     struct ieee80211_rx_status rx_status)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	__le16 fc = rtl_get_fc(skb);
-	bool unicast = false;
-
-	memcpy(IEEE80211_SKB_RXCB(skb), &rx_status, sizeof(rx_status));
-
-	if (is_broadcast_ether_addr(hdr->addr1)) {
-		;/*TODO*/
-	} else if (is_multicast_ether_addr(hdr->addr1)) {
-		;/*TODO*/
-	} else {
-		unicast = true;
-		rtlpriv->stats.rxbytesunicast += skb->len;
-	}
-
-	rtl92e_is_special_data(hw, skb, false);
-	if (ieee80211_is_data(fc)) {
-		rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
-
-		if (unicast)
-			rtlpriv->link_info.num_rx_inperiod++;
-	}
-
-	/* static bcn for roaming */
-	rtl92e_beacon_statistic(hw, skb);
-	rtl92e_p2p_info(hw, (void *)skb->data, skb->len);
-
-	/* for sw lps */
-	rtl92e_swlps_beacon(hw, (void *)skb->data, skb->len);
-	rtl92e_recognize_peer(hw, (void *)skb->data, skb->len);
-	if ((rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) &&
-	    (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G) &&
-	    (ieee80211_is_beacon(fc) ||
-	     ieee80211_is_probe_resp(fc)))
-		dev_kfree_skb_any(skb);
-	else
-		_rtl_pci_rx_to_mac80211(hw, skb, rx_status);
-}
-
-static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct ieee80211_rx_status rx_status = { 0 };
-	int rxring_idx = RTL_PCI_RX_MPDU_QUEUE;
-	unsigned int count = rtlpci->rxringcount;
-	u8 hw_queue = 0;
-	unsigned int rx_remained_cnt;
-	u8 own;
-	u8 tmp_one;
-	static int err_count;
-	struct rtl_stats stats = {
-		.signal = 0,
-		.rate = 0,
-	};
-
-	/*RX NORMAL PKT */
-	while (count--) {
-		struct ieee80211_hdr *hdr;
-		__le16 fc;
-		u16 len;
-		/*rx buffer descriptor */
-		struct rtl_rx_buffer_desc *buffer_desc = NULL;
-		/*if use new trx flow, it means wifi info */
-		struct rtl_rx_desc *pdesc = NULL;
-		/*rx pkt */
-		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[
-					rtlpci->rx_ring[rxring_idx].idx];
-
-		if (rtlpriv->use_new_trx_flow) {
-			rx_remained_cnt =
-				rtlpriv->cfg->ops->rx_desc_buff_remained_cnt(hw,
-								      hw_queue);
-			if (rx_remained_cnt < 1)
-				return;
-
-		} else {	/* rx descriptor */
-			pdesc = &rtlpci->rx_ring[rxring_idx].desc[
-				rtlpci->rx_ring[rxring_idx].idx];
-
-			own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc,
-							       false,
-							       HW_DESC_OWN);
-			if (own) /* wait data to be filled by hardware */
-				return;
-		}
-
-		/* If we get here, the data is filled already
-		 * Attention !!!
-		 * We can NOT access 'skb' before 'pci_unmap_single'
-		 */
-		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *)skb->cb),
-				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
-
-		if (rtlpriv->use_new_trx_flow) {
-			buffer_desc = &rtlpci->rx_ring[rxring_idx].buffer_desc[
-				rtlpci->rx_ring[rxring_idx].idx];
-			/*means rx wifi info*/
-			pdesc = (struct rtl_rx_desc *)skb->data;
-		}
-		memset(&rx_status , 0 , sizeof(rx_status));
-		rtlpriv->cfg->ops->query_rx_desc(hw, &stats,
-						 &rx_status, (u8 *)pdesc, skb);
-
-		if (rtlpriv->use_new_trx_flow)
-			rtlpriv->cfg->ops->rx_check_dma_ok(hw,
-							   (u8 *)buffer_desc,
-							   hw_queue);
-		len = rtlpriv->cfg->ops->get_desc((u8 *)pdesc, false,
-						  HW_DESC_RXPKT_LEN);
-
-		if (skb->end - skb->tail > len) {
-			skb_put(skb, len);
-			if (rtlpriv->use_new_trx_flow)
-				skb_reserve(skb, stats.rx_drvinfo_size +
-						 stats.rx_bufshift + 24);
-			else
-				skb_reserve(skb, stats.rx_drvinfo_size +
-						 stats.rx_bufshift);
-
-		} else {
-			if (err_count++ < 10) {
-				pr_info("skb->end (%d) - skb->tail (%d) > len (%d)\n",
-					skb->end, skb->tail, len);
-				RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_EMERG,
-					      "RX desc\n",
-					      (u8 *)pdesc, 32);
-			}
-			break;
-		}
-
-		/* handle command packet here */
-		if (rtlpriv->cfg->ops->rx_command_packet(hw, &stats, skb)) {
-				dev_kfree_skb_any(skb);
-				goto end;
-		}
-
-		/* NOTICE This can not be use for mac80211,
-		 *this is done in mac80211 code,
-		 *if you done here sec DHCP will fail
-		 *skb_trim(skb, skb->len - 4);
-		 */
-
-		hdr = rtl_get_hdr(skb);
-		fc = rtl_get_fc(skb);
-
-		if (!stats.b_crc && !stats.b_hwerror)
-			_rtl_receive_one(hw, skb, rx_status);
-		else
-			dev_kfree_skb_any(skb);
-		if (rtlpriv->use_new_trx_flow) {
-			rtlpci->rx_ring[hw_queue].next_rx_rp += 1;
-			rtlpci->rx_ring[hw_queue].next_rx_rp %=
-							RTL_PCI_MAX_RX_COUNT;
-
-
-			rx_remained_cnt--;
-			rtl_write_word(rtlpriv, 0x3B4,
-				       rtlpci->rx_ring[hw_queue].next_rx_rp);
-		}
-		if (((rtlpriv->link_info.num_rx_inperiod +
-		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
-		    (rtlpriv->link_info.num_rx_inperiod > 2)) {
-			rtl92e_lps_leave(hw);
-		}
-end:
-		if (rtlpriv->use_new_trx_flow) {
-			_rtl_pci_init_one_rxdesc(hw, (u8 *)buffer_desc,
-						 rxring_idx,
-					       rtlpci->rx_ring[rxring_idx].idx);
-		} else {
-			_rtl_pci_init_one_rxdesc(hw, (u8 *)pdesc, rxring_idx,
-						 rtlpci->rx_ring[rxring_idx].idx);
-
-			if (rtlpci->rx_ring[rxring_idx].idx ==
-			    rtlpci->rxringcount - 1)
-				rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc,
-							    false,
-							    HW_DESC_RXERO,
-							    (u8 *)&tmp_one);
-		}
-		rtlpci->rx_ring[rxring_idx].idx =
-				(rtlpci->rx_ring[rxring_idx].idx + 1) %
-				rtlpci->rxringcount;
-	}
-}
-
-static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
-{
-	struct ieee80211_hw *hw = dev_id;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	unsigned long flags;
-	u32 inta = 0;
-	u32 intb = 0;
-
-	if (rtlpci->irq_enabled == 0)
-		return IRQ_HANDLED;
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock , flags);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR], 0x0);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE], 0x0);
-
-	/*read ISR: 4/8bytes */
-	rtlpriv->cfg->ops->interrupt_recognized(hw, &inta, &intb);
-
-	/*Shared IRQ or HW disappared */
-	if (!inta || inta == 0xffff)
-		goto done;
-	/*<1> beacon related */
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_TBDOK])
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon ok interrupt!\n"));
-
-	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_TBDER]))
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon err interrupt!\n"));
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BDOK])
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon interrupt!\n"));
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BcnInt]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE,
-			 ("prepare beacon for interrupt!\n"));
-		tasklet_schedule(&rtlpriv->works.irq_prepare_bcn_tasklet);
-	}
-
-	/*<2> tx related */
-	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_TXFOVW]))
-		RT_TRACE(COMP_ERR, DBG_TRACE, ("IMR_TXFOVW!\n"));
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_MGNTDOK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("Manage ok interrupt!\n"));
-		_rtl_pci_tx_isr(hw, MGNT_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_HIGHDOK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("HIGH_QUEUE ok interrupt!\n"));
-		_rtl_pci_tx_isr(hw, HIGH_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BKDOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("BK Tx OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, BK_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BEDOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("BE TX OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, BE_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_VIDOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("VI TX OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, VI_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_VODOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("Vo TX OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, VO_QUEUE);
-	}
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
-		if (inta & rtlpriv->cfg->maps[RTL_IMR_COMDOK]) {
-			rtlpriv->link_info.num_tx_inperiod++;
-
-			RT_TRACE(COMP_INTR, DBG_TRACE,
-				 ("CMD TX OK interrupt!\n"));
-			_rtl_pci_tx_isr(hw, TXCMD_QUEUE);
-		}
-	}
-
-	/*<3> rx related */
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_ROK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("Rx ok interrupt!\n"));
-		_rtl_pci_rx_interrupt(hw);
-	}
-
-	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_RDU])) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("rx descriptor unavailable!\n"));
-		_rtl_pci_rx_interrupt(hw);
-	}
-
-	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_RXFOVW])) {
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("rx overflow !\n"));
-		_rtl_pci_rx_interrupt(hw);
-	}
-
-	/*<4> fw related*/
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723AE) {
-		if (inta & rtlpriv->cfg->maps[RTL_IMR_C2HCMD]) {
-			RT_TRACE(COMP_INTR, DBG_TRACE,
-				 ("firmware interrupt!\n"));
-			queue_delayed_work(rtlpriv->works.rtl_wq,
-					   &rtlpriv->works.fwevt_wq, 0);
-		}
-	}
-
-	/*<5> hsisr related*/
-	/* Only 8188EE & 8723BE Supported.
-	 * If Other ICs Come in, System will corrupt,
-	 * because maps[RTL_IMR_HSISR_IND] & maps[MAC_HSISR]
-	 * are not initialized*/
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE ||
-	    rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_HSISR_IND])) {
-			RT_TRACE(COMP_INTR, DBG_TRACE,
-				 ("hsisr interrupt!\n"));
-			_rtl_pci_hs_interrupt(hw);
-		}
-	}
-
-
-	if (rtlpriv->rtlhal.b_earlymode_enable)
-		tasklet_schedule(&rtlpriv->works.irq_tasklet);
-
-done:
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR],
-			rtlpci->irq_mask[0]);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE],
-			rtlpci->irq_mask[1]);
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-	return IRQ_HANDLED;
-}
-
-static void _rtl_pci_irq_tasklet(struct ieee80211_hw *hw)
-{
-	_rtl_pci_tx_chk_waitq(hw);
-}
-
-static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl8192_tx_ring *ring = NULL;
-	struct ieee80211_hdr *hdr = NULL;
-	struct ieee80211_tx_info *info = NULL;
-	struct sk_buff *pskb = NULL;
-	struct rtl_tx_desc *pdesc = NULL;
-	struct rtl_tcb_desc tcb_desc;
-	/*This is for new trx flow*/
-	struct rtl_tx_buffer_desc *pbuffer_desc = NULL;
-	u8 temp_one = 1;
-
-	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	/*NB: the beacon data buffer must be 32-bit aligned. */
-	pskb = ieee80211_beacon_get(hw, mac->vif);
-	if (pskb == NULL)
-		return;
-	hdr = rtl_get_hdr(pskb);
-	info = IEEE80211_SKB_CB(pskb);
-	pdesc = &ring->desc[0];
-	if (rtlpriv->use_new_trx_flow)
-		pbuffer_desc = &ring->buffer_desc[0];
-
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc,
-					(u8 *)pbuffer_desc, info, NULL, pskb,
-					BEACON_QUEUE, &tcb_desc);
-
-	__skb_queue_tail(&ring->queue, pskb);
-
-	rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, true, HW_DESC_OWN,
-				    (u8 *)&temp_one);
-
-	return;
-}
-
-static void _rtl_pci_init_trx_var(struct ieee80211_hw *hw)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 i;
-	u16 desc_num;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
-		desc_num = TX_DESC_NUM_92E;
-	else
-		desc_num = RT_TXDESC_NUM;
-
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
-		rtlpci->txringcount[i] = desc_num;
-	/*
-	 *we just alloc 2 desc for beacon queue,
-	 *because we just need first desc in hw beacon.
-	 */
-	rtlpci->txringcount[BEACON_QUEUE] = 2;
-
-	/*
-	 *BE queue need more descriptor for performance
-	 *consideration or, No more tx desc will happen,
-	 *and may cause mac80211 mem leakage.
-	 */
-	if (!rtl_priv(hw)->use_new_trx_flow)
-		rtlpci->txringcount[BE_QUEUE] = RT_TXDESC_NUM_BE_QUEUE;
-
-	rtlpci->rxbuffersize = 9100;	/*2048/1024; */
-	rtlpci->rxringcount = RTL_PCI_MAX_RX_COUNT;	/*64; */
-}
-
-static void _rtl_pci_init_struct(struct ieee80211_hw *hw,
-				 struct pci_dev *pdev)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	rtlpriv->rtlhal.up_first_time = true;
-	rtlpriv->rtlhal.being_init_adapter = false;
-
-	rtlhal->hw = hw;
-	rtlpci->pdev = pdev;
-
-	/*Tx/Rx related var */
-	_rtl_pci_init_trx_var(hw);
-
-	/*IBSS*/ mac->beacon_interval = 100;
-
-	/*AMPDU*/
-	mac->min_space_cfg = 0;
-	mac->max_mss_density = 0;
-	/*set sane AMPDU defaults */
-	mac->current_ampdu_density = 7;
-	mac->current_ampdu_factor = 3;
-
-	/*QOS*/
-	rtlpci->acm_method = eAcmWay2_SW;
-
-	/*task */
-	tasklet_init(&rtlpriv->works.irq_tasklet,
-		     (void (*)(unsigned long))_rtl_pci_irq_tasklet,
-		     (unsigned long)hw);
-	tasklet_init(&rtlpriv->works.irq_prepare_bcn_tasklet,
-		     (void (*)(unsigned long))_rtl_pci_prepare_bcn_tasklet,
-		     (unsigned long)hw);
-}
-
-static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
-				 unsigned int prio, unsigned int entries)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tx_buffer_desc *buffer_desc;
-	struct rtl_tx_desc *desc;
-	dma_addr_t buffer_desc_dma, desc_dma;
-	u32 nextdescaddress;
-	int i;
-
-	/* alloc tx buffer desc for new trx flow*/
-	if (rtlpriv->use_new_trx_flow) {
-		buffer_desc =
-			pci_zalloc_consistent(rtlpci->pdev,
-					      sizeof(*buffer_desc) * entries,
-					      &buffer_desc_dma);
-		if (!buffer_desc || (unsigned long)buffer_desc & 0xFF) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Cannot allocate TX ring (prio = %d)\n",
-				 prio));
-			return -ENOMEM;
-		}
-
-		rtlpci->tx_ring[prio].buffer_desc = buffer_desc;
-		rtlpci->tx_ring[prio].buffer_desc_dma = buffer_desc_dma;
-
-		rtlpci->tx_ring[prio].cur_tx_rp = 0;
-		rtlpci->tx_ring[prio].cur_tx_wp = 0;
-		rtlpci->tx_ring[prio].avl_desc = entries;
-	}
-
-	/* alloc dma for this ring */
-	desc = pci_zalloc_consistent(rtlpci->pdev, sizeof(*desc) * entries,
-				     &desc_dma);
-	if (!desc || (unsigned long)desc & 0xFF) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Cannot allocate TX ring (prio = %d)\n", prio));
-		return -ENOMEM;
-	}
-
-	rtlpci->tx_ring[prio].desc = desc;
-	rtlpci->tx_ring[prio].dma = desc_dma;
-
-	rtlpci->tx_ring[prio].idx = 0;
-	rtlpci->tx_ring[prio].entries = entries;
-	skb_queue_head_init(&rtlpci->tx_ring[prio].queue);
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("queue:%d, ring_addr:%p\n", prio, desc));
-
-	/* init every desc in this ring */
-	if (!rtlpriv->use_new_trx_flow) {
-		for (i = 0; i < entries; i++) {
-			nextdescaddress = (u32) desc_dma +
-						      ((i +	1) % entries) *
-						      sizeof(*desc);
-
-			rtlpriv->cfg->ops->set_desc(hw, (u8 *)&(desc[i]),
-						    true,
-						    HW_DESC_TX_NEXTDESC_ADDR,
-						    (u8 *)&nextdescaddress);
-		}
-	}
-	return 0;
-}
-
-static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i;
-
-	if (rtlpriv->use_new_trx_flow) {
-		struct rtl_rx_buffer_desc *entry = NULL;
-		/* alloc dma for this ring */
-		rtlpci->rx_ring[rxring_idx].buffer_desc =
-			pci_zalloc_consistent(rtlpci->pdev,
-					      sizeof(*rtlpci->rx_ring[rxring_idx].buffer_desc) * rtlpci->rxringcount,
-					      &rtlpci->rx_ring[rxring_idx].dma);
-		if (!rtlpci->rx_ring[rxring_idx].buffer_desc ||
-		    (unsigned long)rtlpci->rx_ring[rxring_idx].buffer_desc & 0xFF) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Cannot allocate RX ring\n"));
-			return -ENOMEM;
-		}
-
-		/* init every desc in this ring */
-		rtlpci->rx_ring[rxring_idx].idx = 0;
-
-		for (i = 0; i < rtlpci->rxringcount; i++) {
-			entry = &rtlpci->rx_ring[rxring_idx].buffer_desc[i];
-			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
-						      rxring_idx, i))
-				return -ENOMEM;
-		}
-	} else {
-		struct rtl_rx_desc *entry = NULL;
-		u8 tmp_one = 1;
-		/* alloc dma for this ring */
-		rtlpci->rx_ring[rxring_idx].desc =
-			pci_zalloc_consistent(rtlpci->pdev,
-					      sizeof(*rtlpci->rx_ring[rxring_idx].desc) * rtlpci->rxringcount,
-					      &rtlpci->rx_ring[rxring_idx].dma);
-		if (!rtlpci->rx_ring[rxring_idx].desc ||
-		    (unsigned long)rtlpci->rx_ring[rxring_idx].desc & 0xFF) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Cannot allocate RX ring\n"));
-			return -ENOMEM;
-		}
-
-		/* init every desc in this ring */
-		rtlpci->rx_ring[rxring_idx].idx = 0;
-		for (i = 0; i < rtlpci->rxringcount; i++) {
-			entry = &rtlpci->rx_ring[rxring_idx].desc[i];
-			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
-						      rxring_idx, i))
-				return -ENOMEM;
-		}
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry, false,
-					    HW_DESC_RXERO, (u8 *) &tmp_one);
-	}
-	return 0;
-}
-
-static void _rtl_pci_free_tx_ring(struct ieee80211_hw *hw,
-				  unsigned int prio)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
-
-	/* free every desc in this ring */
-	while (skb_queue_len(&ring->queue)) {
-		struct sk_buff *skb = __skb_dequeue(&ring->queue);
-		u8 *entry;
-
-		if (rtlpriv->use_new_trx_flow)
-			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
-		else
-			entry = (u8 *)(&ring->desc[ring->idx]);
-
-		pci_unmap_single(rtlpci->pdev,
-				 rtlpriv->cfg->ops->get_desc((u8 *)entry, true,
-				 HW_DESC_TXBUFF_ADDR),
-				 skb->len, PCI_DMA_TODEVICE);
-		kfree_skb(skb);
-		ring->idx = (ring->idx + 1) % ring->entries;
-	}
-
-	/* free dma of this ring */
-	pci_free_consistent(rtlpci->pdev,
-			    sizeof(*ring->desc) * ring->entries,
-			    ring->desc, ring->dma);
-	ring->desc = NULL;
-	if (rtlpriv->use_new_trx_flow) {
-		pci_free_consistent(rtlpci->pdev,
-				    sizeof(*ring->buffer_desc) * ring->entries,
-				    ring->buffer_desc, ring->buffer_desc_dma);
-		ring->buffer_desc = NULL;
-	}
-}
-
-static void _rtl_pci_free_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int i;
-
-	/* free every desc in this ring */
-	for (i = 0; i < rtlpci->rxringcount; i++) {
-		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[i];
-
-		if (!skb)
-			continue;
-
-		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *)skb->cb),
-				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
-		kfree_skb(skb);
-	}
-
-	/* free dma of this ring */
-	if (rtlpriv->use_new_trx_flow) {
-		pci_free_consistent(rtlpci->pdev,
-				    sizeof(*rtlpci->rx_ring[rxring_idx].
-				    buffer_desc) * rtlpci->rxringcount,
-				    rtlpci->rx_ring[rxring_idx].buffer_desc,
-				    rtlpci->rx_ring[rxring_idx].dma);
-		rtlpci->rx_ring[rxring_idx].buffer_desc = NULL;
-	} else {
-		pci_free_consistent(rtlpci->pdev,
-				    sizeof(*rtlpci->rx_ring[rxring_idx].desc) *
-				    rtlpci->rxringcount,
-				    rtlpci->rx_ring[rxring_idx].desc,
-				    rtlpci->rx_ring[rxring_idx].dma);
-		rtlpci->rx_ring[rxring_idx].desc = NULL;
-	}
-}
-
-static int _rtl_pci_init_trx_ring(struct ieee80211_hw *hw)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int ret;
-	int i, rxring_idx;
-
-	/* rxring_idx 0:RX_MPDU_QUEUE
-	 * rxring_idx 1:RX_CMD_QUEUE */
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
-		ret = _rtl_pci_init_rx_ring(hw, rxring_idx);
-		if (ret)
-			return ret;
-	}
-
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
-		ret = _rtl_pci_init_tx_ring(hw, i,
-					    rtlpci->txringcount[i]);
-		if (ret)
-			goto err_free_rings;
-	}
-
-	return 0;
-
-err_free_rings:
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
-		_rtl_pci_free_rx_ring(hw, rxring_idx);
-
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
-		if (rtlpci->tx_ring[i].desc ||
-		    rtlpci->tx_ring[i].buffer_desc)
-			_rtl_pci_free_tx_ring(hw, i);
-
-	return 1;
-}
-
-static int _rtl_pci_deinit_trx_ring(struct ieee80211_hw *hw)
-{
-	u32 i, rxring_idx;
-
-	/*free rx rings */
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
-		_rtl_pci_free_rx_ring(hw, rxring_idx);
-
-	/*free tx rings */
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
-		_rtl_pci_free_tx_ring(hw, i);
-
-	return 0;
-}
-
-int rtl92e_pci_reset_trx_ring(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int i, rxring_idx;
-	unsigned long flags;
-	u8 tmp_one = 1;
-	/* rxring_idx 0:RX_MPDU_QUEUE */
-	/* rxring_idx 1:RX_CMD_QUEUE */
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
-		/* force the rx_ring[RX_MPDU_QUEUE]
-		 * RX_CMD_QUEUE].idx to the first one
-		 * If using the new trx flow, do nothing
-		 */
-		if (!rtlpriv->use_new_trx_flow &&
-		    rtlpci->rx_ring[rxring_idx].desc) {
-			struct rtl_rx_desc *entry = NULL;
-
-			for (i = 0; i < rtlpci->rxringcount; i++) {
-				entry = &rtlpci->rx_ring[rxring_idx].desc[i];
-				rtlpriv->cfg->ops->set_desc(hw, (u8 *)entry,
-							    false,
-							    HW_DESC_RXOWN,
-							    &tmp_one);
-			}
-		}
-		rtlpci->rx_ring[rxring_idx].idx = 0;
-	}
-
-	/* after reset, release previous pending packet,
-	 * and force the  tx idx to the first one
-	 */
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
-		if (rtlpci->tx_ring[i].desc ||
-		    rtlpci->tx_ring[i].buffer_desc) {
-			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[i];
-
-			while (skb_queue_len(&ring->queue)) {
-				struct sk_buff *skb =
-					__skb_dequeue(&ring->queue);
-				u8 *entry;
-
-				if (rtlpriv->use_new_trx_flow)
-					entry = (u8 *)(&ring->buffer_desc
-								[ring->idx]);
-				else
-					entry = (u8 *)(&ring->desc[ring->idx]);
-
-				pci_unmap_single(rtlpci->pdev,
-						 rtlpriv->cfg->ops->get_desc(
-						 (u8 *)entry, true,
-						 HW_DESC_TXBUFF_ADDR),
-					skb->len, PCI_DMA_TODEVICE);
-				kfree_skb(skb);
-				ring->idx = (ring->idx + 1) % ring->entries;
-			}
-			ring->idx = 0;
-		}
-	}
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	return 0;
-}
-
-static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
-					struct ieee80211_sta *sta,
-					struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *sta_entry = NULL;
-	u8 tid = rtl_get_tid(skb);
-	__le16 fc = rtl_get_fc(skb);
-
-	if (!sta)
-		return false;
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-
-	if (!rtlpriv->rtlhal.b_earlymode_enable)
-		return false;
-	if (ieee80211_is_nullfunc(fc))
-		return false;
-	if (ieee80211_is_qos_nullfunc(fc))
-		return false;
-	if (ieee80211_is_pspoll(fc))
-		return false;
-
-	if (sta_entry->tids[tid].agg.agg_state != RTL_AGG_OPERATIONAL)
-		return false;
-	if (_rtl_mac_to_hwqueue(hw, skb) > VO_QUEUE)
-		return false;
-	if (tid > 7)
-		return false;
-	/* maybe every tid should be checked */
-	if (!rtlpriv->link_info.higher_busytxtraffic[tid])
-		return false;
-
-	spin_lock_bh(&rtlpriv->locks.waitq_lock);
-	skb_queue_tail(&rtlpriv->mac80211.skb_waitq[tid], skb);
-	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-
-	return true;
-}
-
-static int rtl_pci_tx(struct ieee80211_hw *hw,
-		      struct ieee80211_sta *sta,
-		      struct sk_buff *skb,
-		      struct rtl_tcb_desc *ptcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *sta_entry = NULL;
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	struct rtl_tx_buffer_desc *ptx_bd_desc = NULL;
-	u16 idx;
-	u8 own;
-	u8 temp_one = 1;
-	u8 hw_queue = _rtl_mac_to_hwqueue(hw, skb);
-	unsigned long flags;
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	__le16 fc = rtl_get_fc(skb);
-	u8 *pda_addr = hdr->addr1;
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	/*ssn */
-	u8 tid = 0;
-	u16 seq_number = 0;
-
-	if (ieee80211_is_mgmt(fc))
-		rtl92e_tx_mgmt_proc(hw, skb);
-
-	if (rtlpriv->psc.sw_ps_enabled) {
-		if (ieee80211_is_data(fc) && !ieee80211_is_nullfunc(fc) &&
-		    !ieee80211_has_pm(fc))
-			hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
-	}
-
-	rtl92e_action_proc(hw, skb, true);
-
-	if (is_multicast_ether_addr(pda_addr))
-		rtlpriv->stats.txbytesmulticast += skb->len;
-	else if (is_broadcast_ether_addr(pda_addr))
-		rtlpriv->stats.txbytesbroadcast += skb->len;
-	else
-		rtlpriv->stats.txbytesunicast += skb->len;
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-	ring = &rtlpci->tx_ring[hw_queue];
-	if (hw_queue != BEACON_QUEUE) {
-		if (rtlpriv->use_new_trx_flow)
-			idx = ring->cur_tx_wp;
-		else
-			idx = (ring->idx + skb_queue_len(&ring->queue)) %
-			      ring->entries;
-	} else {
-		idx = 0;
-	}
-
-	pdesc = &ring->desc[idx];
-
-	if (rtlpriv->use_new_trx_flow) {
-		ptx_bd_desc = &ring->buffer_desc[idx];
-	} else {
-		own = (u8) rtlpriv->cfg->ops->get_desc((u8 *)pdesc,
-				true, HW_DESC_OWN);
-
-		if ((own == 1) && (hw_queue != BEACON_QUEUE)) {
-			RT_TRACE(COMP_ERR, DBG_WARNING,
-				 ("No more TX desc@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%d\n",
-				  hw_queue, ring->idx, idx,
-				  skb_queue_len(&ring->queue)));
-
-			spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock,
-					       flags);
-			return skb->len;
-		}
-	}
-
-	if (ieee80211_is_data_qos(fc)) {
-		tid = rtl_get_tid(skb);
-		if (sta) {
-			sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-			seq_number = (le16_to_cpu(hdr->seq_ctrl) &
-				      IEEE80211_SCTL_SEQ) >> 4;
-			seq_number += 1;
-
-			if (!ieee80211_has_morefrags(hdr->frame_control))
-				sta_entry->tids[tid].seq_number = seq_number;
-		}
-	}
-
-	if (ieee80211_is_data(fc))
-		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
-
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *)pdesc,
-					(u8 *)ptx_bd_desc, info, sta, skb,
-					hw_queue, ptcb_desc);
-
-	__skb_queue_tail(&ring->queue, skb);
-	if (rtlpriv->use_new_trx_flow) {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, true,
-					    HW_DESC_OWN, (u8 *)&hw_queue);
-	} else {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *)pdesc, true,
-					    HW_DESC_OWN, (u8 *)&temp_one);
-	}
-
-	if ((ring->entries - skb_queue_len(&ring->queue)) < 2 &&
-	    hw_queue != BEACON_QUEUE) {
-		RT_TRACE(COMP_ERR, DBG_LOUD,
-			 ("less desc left, stop skb_queue@%d, ring->idx = %d, idx = %d, skb_queue_len = 0x%d\n",
-			  hw_queue, ring->idx, idx,
-			  skb_queue_len(&ring->queue)));
-
-		ieee80211_stop_queue(hw, skb_get_queue_mapping(skb));
-	}
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	if (rtlpriv->cfg->ops->tx_polling)
-		rtlpriv->cfg->ops->tx_polling(hw, hw_queue);
-
-	return 0;
-}
-static void rtl_pci_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u16 i = 0;
-	int queue_id;
-	struct rtl8192_tx_ring *ring;
-
-	if (mac->skip_scan)
-		return;
-
-	for (queue_id = RTL_PCI_MAX_TX_QUEUE_COUNT - 1; queue_id >= 0;) {
-		u32 queue_len;
-		if (((queues >> queue_id) & 0x1) == 0) {
-			queue_id--;
-			continue;
-		}
-		ring = &pcipriv->dev.tx_ring[queue_id];
-		queue_len = skb_queue_len(&ring->queue);
-		if (queue_len == 0 || queue_id == BEACON_QUEUE ||
-		    queue_id == TXCMD_QUEUE) {
-			queue_id--;
-			continue;
-		} else {
-			msleep(5);
-			i++;
-		}
-
-		/* we just wait 1s for all queues */
-		if (rtlpriv->psc.rfpwr_state == ERFOFF ||
-		    is_hal_stop(rtlhal) || i >= 200)
-			return;
-	}
-}
-
-static void rtl_pci_deinit(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	_rtl_pci_deinit_trx_ring(hw);
-
-	synchronize_irq(rtlpci->pdev->irq);
-	tasklet_kill(&rtlpriv->works.irq_tasklet);
-
-	flush_workqueue(rtlpriv->works.rtl_wq);
-	destroy_workqueue(rtlpriv->works.rtl_wq);
-}
-
-static int rtl_pci_init(struct ieee80211_hw *hw, struct pci_dev *pdev)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int err;
-
-	_rtl_pci_init_struct(hw, pdev);
-
-	err = _rtl_pci_init_trx_ring(hw);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("tx ring initialization failed"));
-		return err;
-	}
-
-	return 1;
-}
-
-static int rtl_pci_start(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	int err = 0;
-
-	RT_TRACE(COMP_INIT, DBG_DMESG, (" rtl_pci_start\n"));
-	rtl92e_pci_reset_trx_ring(hw);
-
-	rtlpriv->rtlhal.driver_is_goingto_unload = false;
-	if (rtlpriv->cfg->ops->get_btc_status()) {
-		rtlpriv->btcoexist.btc_ops->btc_init_variables(rtlpriv);
-		rtlpriv->btcoexist.btc_ops->btc_init_hal_vars(rtlpriv);
-	}
-
-	err = rtlpriv->cfg->ops->hw_init(hw);
-	if (err) {
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("Failed to config hardware err %x!\n" , err));
-		return err;
-	}
-
-	rtlpriv->cfg->ops->enable_interrupt(hw);
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("enable_interrupt OK\n"));
-
-	rtl92e_init_rx_config(hw);
-
-	/*should after adapter start and interrupt enable. */
-	set_hal_start(rtlhal);
-
-	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-
-	rtlpriv->rtlhal.up_first_time = false;
-
-	RT_TRACE(COMP_INIT, DBG_DMESG, ("rtl_pci_start OK\n"));
-	return 0;
-}
-
-static void rtl_pci_stop(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 RFInProgressTimeOut = 0;
-
-	if (rtlpriv->cfg->ops->get_btc_status())
-		rtlpriv->btcoexist.btc_ops->btc_halt_notify();
-
-	/*
-	 *should before disable interrrupt&adapter
-	 *and will do it immediately.
-	 */
-	set_hal_stop(rtlhal);
-
-	rtlpriv->cfg->ops->disable_interrupt(hw);
-
-	spin_lock(&rtlpriv->locks.rf_ps_lock);
-	while (ppsc->rfchange_inprogress) {
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-		if (RFInProgressTimeOut > 100) {
-			spin_lock(&rtlpriv->locks.rf_ps_lock);
-			break;
-		}
-		mdelay(1);
-		RFInProgressTimeOut++;
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-	}
-	ppsc->rfchange_inprogress = true;
-	spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-	rtlpriv->rtlhal.driver_is_goingto_unload = true;
-	rtlpriv->cfg->ops->hw_disable(hw);
-	rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
-
-	spin_lock(&rtlpriv->locks.rf_ps_lock);
-	ppsc->rfchange_inprogress = false;
-	spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-	rtl_pci_enable_aspm(hw);
-}
-
-static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
-				  struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct pci_dev *bridge_pdev = pdev->bus->self;
-	u16 venderid;
-	u16 deviceid;
-	u8 revisionid;
-	u16 irqline;
-	u8 tmp;
-
-	venderid = pdev->vendor;
-	deviceid = pdev->device;
-	pci_read_config_byte(pdev, 0x8, &revisionid);
-	pci_read_config_word(pdev, 0x3C, &irqline);
-
-	if (deviceid == RTL_PCI_8192_DID ||
-	    deviceid == RTL_PCI_0044_DID ||
-	    deviceid == RTL_PCI_0047_DID ||
-	    deviceid == RTL_PCI_8192SE_DID ||
-	    deviceid == RTL_PCI_8174_DID ||
-	    deviceid == RTL_PCI_8173_DID ||
-	    deviceid == RTL_PCI_8172_DID ||
-	    deviceid == RTL_PCI_8171_DID) {
-		switch (revisionid) {
-		case RTL_PCI_REVISION_ID_8192PCIE:
-			RT_TRACE(COMP_INIT, DBG_DMESG,
-				 ("8192E is found but not supported now-vid/did=%x/%x\n",
-				  venderid, deviceid));
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192E;
-			return false;
-			break;
-		case RTL_PCI_REVISION_ID_8192SE:
-			RT_TRACE(COMP_INIT, DBG_DMESG,
-				 ("8192SE is found - vid/did=%x/%x\n",
-				  venderid, deviceid));
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192SE;
-			break;
-		default:
-			RT_TRACE(COMP_ERR, DBG_WARNING,
-				 ("Err: Unknown device - vid/did=%x/%x\n",
-				  venderid, deviceid));
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192SE;
-			break;
-		}
-	} else if (deviceid == RTL_PCI_8723AE_DID) {
-		rtlhal->hw_type = HARDWARE_TYPE_RTL8723AE;
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("8723AE PCI-E is found - vid/did=%x/%x\n",
-			  venderid, deviceid));
-	} else if (deviceid == RTL_PCI_8192CET_DID ||
-		   deviceid == RTL_PCI_8192CE_DID ||
-		   deviceid == RTL_PCI_8191CE_DID ||
-		   deviceid == RTL_PCI_8188CE_DID) {
-		rtlhal->hw_type = HARDWARE_TYPE_RTL8192CE;
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("8192C PCI-E is found - vid/did=%x/%x\n",
-			  venderid, deviceid));
-	} else if (deviceid == RTL_PCI_8192DE_DID ||
-		   deviceid == RTL_PCI_8192DE_DID2) {
-		rtlhal->hw_type = HARDWARE_TYPE_RTL8192DE;
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("8192D PCI-E is found - vid/did=%x/%x\n",
-			  venderid, deviceid));
-	} else if (deviceid == RTL_PCI_8188EE_DID) {
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8188EE;
-			RT_TRACE(COMP_INIT , DBG_LOUD,
-				 ("Find adapter, Hardware type is 8188EE\n"));
-	} else if (deviceid == RTL_PCI_8723BE_DID) {
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8723BE;
-			RT_TRACE(COMP_INIT , DBG_LOUD,
-				 ("Find adapter, Hardware type is 8723BE\n"));
-	} else if (deviceid == RTL_PCI_8192EE_DID) {
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192EE;
-			RT_TRACE(COMP_INIT , DBG_LOUD,
-				 ("Find adapter, Hardware type is 8192EE\n"));
-	} else if (deviceid == RTL_PCI_8821AE_DID) {
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8821AE;
-			RT_TRACE(COMP_INIT , DBG_LOUD,
-				 ("Find adapter, Hardware type is 8821AE\n"));
-	} else if (deviceid == RTL_PCI_8812AE_DID) {
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8812AE;
-			RT_TRACE(COMP_INIT , DBG_LOUD,
-				 ("Find adapter, Hardware type is 8812AE\n"));
-	} else {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Err: Unknown device - vid/did=%x/%x\n",
-			  venderid, deviceid));
-
-		rtlhal->hw_type = RTL_DEFAULT_HARDWARE_TYPE;
-	}
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192DE) {
-		if (revisionid == 0 || revisionid == 1) {
-			if (revisionid == 0) {
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					 ("Find 92DE MAC0.\n"));
-				rtlhal->interfaceindex = 0;
-			} else if (revisionid == 1) {
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					 ("Find 92DE MAC1.\n"));
-				rtlhal->interfaceindex = 1;
-			}
-		} else {
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				 ("Unknown device - VendorID/DeviceID=%x/%x, Revision=%x\n",
-				  venderid, deviceid, revisionid));
-			rtlhal->interfaceindex = 0;
-		}
-	}
-
-	/* 92ee use new trx flow */
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
-		rtlpriv->use_new_trx_flow = true;
-	else
-		rtlpriv->use_new_trx_flow = false;
-
-	/*find bus info */
-	pcipriv->ndis_adapter.busnumber = pdev->bus->number;
-	pcipriv->ndis_adapter.devnumber = PCI_SLOT(pdev->devfn);
-	pcipriv->ndis_adapter.funcnumber = PCI_FUNC(pdev->devfn);
-
-	/*find bridge info */
-	pcipriv->ndis_adapter.pcibridge_vendor = PCI_BRIDGE_VENDOR_UNKNOWN;
-	/* some ARM have no bridge_pdev and will crash here
-	 * so we should check if bridge_pdev is NULL */
-	if (bridge_pdev) {
-		pcipriv->ndis_adapter.pcibridge_vendorid = bridge_pdev->vendor;
-		for (tmp = 0; tmp < PCI_BRIDGE_VENDOR_MAX; tmp++) {
-			if (bridge_pdev->vendor == pcibridge_vendors[tmp]) {
-				pcipriv->ndis_adapter.pcibridge_vendor = tmp;
-				RT_TRACE(COMP_INIT, DBG_DMESG,
-					 ("Pci Bridge Vendor is found index: %d\n",
-					  tmp));
-				break;
-			}
-		}
-	}
-
-	if (pcipriv->ndis_adapter.pcibridge_vendor !=
-	    PCI_BRIDGE_VENDOR_UNKNOWN) {
-		pcipriv->ndis_adapter.pcibridge_busnum =
-		    bridge_pdev->bus->number;
-		pcipriv->ndis_adapter.pcibridge_devnum =
-		    PCI_SLOT(bridge_pdev->devfn);
-		pcipriv->ndis_adapter.pcibridge_funcnum =
-		    PCI_FUNC(bridge_pdev->devfn);
-		pcipriv->ndis_adapter.pcicfg_addrport =
-		    (pcipriv->ndis_adapter.pcibridge_busnum << 16) |
-		    (pcipriv->ndis_adapter.pcibridge_devnum << 11) |
-		    (pcipriv->ndis_adapter.pcibridge_funcnum << 8) | (1 << 31);
-		pcipriv->ndis_adapter.pcibridge_pciehdr_offset =
-		    pci_pcie_cap(bridge_pdev);
-		pcipriv->ndis_adapter.num4bytes =
-		    (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10) / 4;
-
-		rtl_pci_get_linkcontrol_field(hw);
-
-		if (pcipriv->ndis_adapter.pcibridge_vendor ==
-		    PCI_BRIDGE_VENDOR_AMD) {
-			pcipriv->ndis_adapter.amd_l1_patch =
-			    rtl_pci_get_amd_l1_patch(hw);
-		}
-	}
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("pcidev busnumber:devnumber:funcnumber:vendor:link_ctl %d:%d:%d:%x:%x\n",
-		  pcipriv->ndis_adapter.busnumber,
-		  pcipriv->ndis_adapter.devnumber,
-		  pcipriv->ndis_adapter.funcnumber,
-		  pdev->vendor, pcipriv->ndis_adapter.linkctrl_reg));
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("pci_bridge busnumber:devnumber:funcnumber:vendor:pcie_cap:link_ctl_reg:amd %d:%d:%d:%x:%x:%x:%x\n",
-		  pcipriv->ndis_adapter.pcibridge_busnum,
-		  pcipriv->ndis_adapter.pcibridge_devnum,
-		  pcipriv->ndis_adapter.pcibridge_funcnum,
-		  pcibridge_vendors[pcipriv->ndis_adapter.pcibridge_vendor],
-		  pcipriv->ndis_adapter.pcibridge_pciehdr_offset,
-		  pcipriv->ndis_adapter.pcibridge_linkctrlreg,
-		  pcipriv->ndis_adapter.amd_l1_patch));
-
-	rtl_pci_parse_configuration(pdev, hw);
-	list_add_tail(&rtlpriv->list, &rtlpriv->glb_var->glb_priv_list);
-	return true;
-}
-
-static int rtl_pci_intr_mode_msi(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	int ret;
-	ret = pci_enable_msi(rtlpci->pdev);
-	if (ret < 0)
-		return ret;
-
-	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
-			  IRQF_SHARED, KBUILD_MODNAME, hw);
-	if (ret < 0) {
-		pci_disable_msi(rtlpci->pdev);
-		return ret;
-	}
-
-	rtlpci->using_msi = true;
-
-	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG, ("MSI Interrupt Mode!\n"));
-	return 0;
-}
-
-static int rtl_pci_intr_mode_legacy(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	int ret;
-
-	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
-			  IRQF_SHARED, KBUILD_MODNAME, hw);
-	if (ret < 0)
-		return ret;
-
-	rtlpci->using_msi = false;
-	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG,
-		 ("Pin-based Interrupt Mode!\n"));
-	return 0;
-}
-
-static int rtl_pci_intr_mode_decide(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	int ret;
-	if (rtlpci->msi_support) {
-		ret = rtl_pci_intr_mode_msi(hw);
-		if (ret < 0)
-			ret = rtl_pci_intr_mode_legacy(hw);
-	} else {
-		ret = rtl_pci_intr_mode_legacy(hw);
-	}
-	return ret;
-}
-
-/* this is used for other modules get
- * hw pointer in rtl_pci_get_hw_pointer */
-static struct ieee80211_hw *hw_export;
-
-int stg_rtl_pci_probe(struct pci_dev *pdev,
-		      const struct pci_device_id *id)
-{
-	struct ieee80211_hw *hw = NULL;
-	struct rtl_priv *rtlpriv = NULL;
-	struct rtl_pci_priv *pcipriv = NULL;
-	struct rtl_pci *rtlpci;
-	unsigned long pmem_start, pmem_len, pmem_flags;
-	int err;
-
-	err = pci_enable_device(pdev);
-	if (err) {
-		RT_ASSERT(false,
-			  ("%s : Cannot enable new PCI device\n",
-			   pci_name(pdev)));
-		return err;
-	}
-
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
-		if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
-			RT_ASSERT(false,
-				  ("Unable to obtain 32bit DMA for consistent allocations\n"));
-			pci_disable_device(pdev);
-			return -ENOMEM;
-		}
-	}
-
-	pci_set_master(pdev);
-
-	hw = ieee80211_alloc_hw(sizeof(struct rtl_pci_priv) +
-				sizeof(struct rtl_priv), &rtl92e_ops);
-	if (!hw) {
-		RT_ASSERT(false,
-			  ("%s : ieee80211 alloc failed\n", pci_name(pdev)));
-		err = -ENOMEM;
-		goto fail1;
-	}
-	hw_export = hw;
-
-	SET_IEEE80211_DEV(hw, &pdev->dev);
-	pci_set_drvdata(pdev, hw);
-
-	rtlpriv = hw->priv;
-	pcipriv = (void *)rtlpriv->priv;
-	pcipriv->dev.pdev = pdev;
-
-	/* init cfg & intf_ops */
-	rtlpriv->rtlhal.interface = INTF_PCI;
-	rtlpriv->cfg = (struct rtl_hal_cfg *)(id->driver_data);
-	rtlpriv->intf_ops = &rtl92e_pci_ops;
-	rtlpriv->glb_var = &global_var;
-
-	/*
-	 *init dbgp flags before all
-	 *other functions, because we will
-	 *use it in other funtions like
-	 *RT_TRACE/RT_PRINT/RTL_PRINT_DATA
-	 *you can not use these macro
-	 *before this
-	 */
-	rtl92e_dbgp_flag_init(hw);
-
-	/* MEM map */
-	err = pci_request_regions(pdev, KBUILD_MODNAME);
-	if (err) {
-		RT_ASSERT(false, ("Can't obtain PCI resources\n"));
-		return err;
-	}
-
-	pmem_start = pci_resource_start(pdev, rtlpriv->cfg->bar_id);
-	pmem_len = pci_resource_len(pdev, rtlpriv->cfg->bar_id);
-	pmem_flags = pci_resource_flags(pdev, rtlpriv->cfg->bar_id);
-
-	/*shared mem start */
-	rtlpriv->io.pci_mem_start =
-			(unsigned long)pci_iomap(pdev,
-			rtlpriv->cfg->bar_id, pmem_len);
-	if (rtlpriv->io.pci_mem_start == 0) {
-		RT_ASSERT(false, ("Can't map PCI mem\n"));
-		goto fail2;
-	}
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("mem mapped space: start: 0x%08lx len:%08lx flags:%08lx, after map:0x%08lx\n",
-		  pmem_start, pmem_len, pmem_flags,
-		  rtlpriv->io.pci_mem_start));
-
-	/* Disable Clk Request */
-	pci_write_config_byte(pdev, 0x81, 0);
-	/* leave D3 mode */
-	pci_write_config_byte(pdev, 0x44, 0);
-	pci_write_config_byte(pdev, 0x04, 0x06);
-	pci_write_config_byte(pdev, 0x04, 0x07);
-
-	/* The next statement is needed when built as single module */
-	rtl_core_module_init();
-
-	/* find adapter */
-	/* if chip not support, will return false */
-	if (!_rtl_pci_find_adapter(pdev, hw))
-		goto fail3;
-
-	/* Init IO handler */
-	_rtl_pci_io_handler_init(&pdev->dev, hw);
-
-	/*like read eeprom and so on */
-	rtlpriv->cfg->ops->read_eeprom_info(hw);
-
-	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Can't init_sw_vars.\n"));
-		goto fail3;
-	}
-
-	rtlpriv->cfg->ops->init_sw_leds(hw);
-
-	/*aspm */
-	rtl_pci_init_aspm(hw);
-
-	/* Init mac80211 sw */
-	err = rtl92e_init_core(hw);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Can't allocate sw for mac80211.\n"));
-		goto fail3;
-	}
-
-	/* Init PCI sw */
-	err = !rtl_pci_init(hw, pdev);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Failed to init PCI.\n"));
-		goto fail3;
-	}
-
-	err = ieee80211_register_hw(hw);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Can't register mac80211 hw.\n"));
-		goto fail3;
-	} else {
-		rtlpriv->mac80211.mac80211_registered = 1;
-	}
-	/* the wiphy must have been registed to
-	 * cfg80211 prior to regulatory_hint */
-	if (regulatory_hint(hw->wiphy, rtlpriv->regd.alpha2))
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("regulatory_hint fail\n"));
-
-	/* add for prov */
-	rtl_proc_add_one(hw);
-
-	/*init rfkill */
-	rtl92e_init_rfkill(hw);
-
-	rtlpci = rtl_pcidev(pcipriv);
-	err = rtl_pci_intr_mode_decide(hw);
-	if (err) {
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("%s: failed to register IRQ handler\n",
-			  wiphy_name(hw->wiphy)));
-		goto fail3;
-	} else {
-		rtlpci->irq_alloc = 1;
-	}
-
-	set_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
-	return 0;
-
-fail3:
-	pci_set_drvdata(pdev, NULL);
-	rtl92e_deinit_core(hw);
-	ieee80211_free_hw(hw);
-
-	if (rtlpriv->io.pci_mem_start != 0)
-		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
-
-fail2:
-	pci_release_regions(pdev);
-
-fail1:
-
-	pci_disable_device(pdev);
-
-	return -ENODEV;
-}
-EXPORT_SYMBOL(stg_rtl_pci_probe);
-
-struct ieee80211_hw *rtl_pci_get_hw_pointer(void)
-{
-	return hw_export;
-}
-EXPORT_SYMBOL(rtl_pci_get_hw_pointer);
-
-void stg_rtl_pci_disconnect(struct pci_dev *pdev)
-{
-	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	struct rtl_mac *rtlmac = rtl_mac(rtlpriv);
-
-	clear_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
-
-	/* add for prov */
-	rtl_proc_remove_one(hw);
-
-	/*ieee80211_unregister_hw will call ops_stop */
-	if (rtlmac->mac80211_registered == 1) {
-		ieee80211_unregister_hw(hw);
-		rtlmac->mac80211_registered = 0;
-	} else {
-		rtl92e_deinit_deferred_work(hw);
-		rtlpriv->intf_ops->adapter_stop(hw);
-	}
-
-	/*deinit rfkill */
-	rtl92e_deinit_rfkill(hw);
-
-	rtl_pci_deinit(hw);
-	rtl92e_deinit_core(hw);
-	rtlpriv->cfg->ops->deinit_sw_vars(hw);
-
-	if (rtlpci->irq_alloc) {
-		synchronize_irq(rtlpci->pdev->irq);
-		free_irq(rtlpci->pdev->irq, hw);
-		rtlpci->irq_alloc = 0;
-	}
-
-	if (rtlpci->using_msi)
-		pci_disable_msi(rtlpci->pdev);
-
-	list_del(&rtlpriv->list);
-	if (rtlpriv->io.pci_mem_start != 0) {
-		pci_iounmap(pdev, (void __iomem *)rtlpriv->io.pci_mem_start);
-		pci_release_regions(pdev);
-	}
-
-	pci_disable_device(pdev);
-
-	rtl_pci_disable_aspm(hw);
-
-	pci_set_drvdata(pdev, NULL);
-
-	ieee80211_free_hw(hw);
-}
-EXPORT_SYMBOL(stg_rtl_pci_disconnect);
-
-/***************************************
-kernel pci power state define:
-PCI_D0         ((pci_power_t __force) 0)
-PCI_D1         ((pci_power_t __force) 1)
-PCI_D2         ((pci_power_t __force) 2)
-PCI_D3hot      ((pci_power_t __force) 3)
-PCI_D3cold     ((pci_power_t __force) 4)
-PCI_UNKNOWN    ((pci_power_t __force) 5)
-
-This function is called when system
-goes into suspend state mac80211 will
-call rtl_mac_stop() from the mac80211
-suspend function first, So there is
-no need to call hw_disable here.
-****************************************/
-int stg_rtl_pci_suspend(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->cfg->ops->hw_suspend(hw);
-	rtl92e_deinit_rfkill(hw);
-
-	return 0;
-}
-EXPORT_SYMBOL(stg_rtl_pci_suspend);
-
-int stg_rtl_pci_resume(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->cfg->ops->hw_resume(hw);
-	rtl92e_init_rfkill(hw);
-
-	return 0;
-}
-EXPORT_SYMBOL(stg_rtl_pci_resume);
-
-struct rtl_intf_ops rtl92e_pci_ops = {
-	.read92e_efuse_byte = read92e_efuse_byte,
-	.adapter_start = rtl_pci_start,
-	.adapter_stop = rtl_pci_stop,
-	.check_buddy_priv = rtl_pci_check_buddy_priv,
-	.adapter_tx = rtl_pci_tx,
-	.flush = rtl_pci_flush,
-	.reset_trx_ring = rtl92e_pci_reset_trx_ring,
-	.waitq_insert = rtl_pci_tx_chk_waitq_insert,
-
-	.disable_aspm = rtl_pci_disable_aspm,
-	.enable_aspm = rtl_pci_enable_aspm,
-};
diff --git a/drivers/staging/rtl8192ee/pci.h b/drivers/staging/rtl8192ee/pci.h
deleted file mode 100644
index 62c23a7f450d..000000000000
--- a/drivers/staging/rtl8192ee/pci.h
+++ /dev/null
@@ -1,342 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_PCI_H__
-#define __RTL_PCI_H__
-
-#include <linux/pci.h>
-/*
-1: MSDU packet queue,
-2: Rx Command Queue
-*/
-#define RTL_PCI_RX_MPDU_QUEUE			0
-#define RTL_PCI_RX_CMD_QUEUE			1
-#define RTL_PCI_MAX_RX_QUEUE			2
-
-#define RTL_PCI_MAX_RX_COUNT			512/*64*/
-#define RTL_PCI_MAX_TX_QUEUE_COUNT		9
-
-#define RT_TXDESC_NUM				128
-#define TX_DESC_NUM_92E				512
-#define RT_TXDESC_NUM_BE_QUEUE			256
-
-#define BK_QUEUE				0
-#define BE_QUEUE				1
-#define VI_QUEUE				2
-#define VO_QUEUE				3
-#define BEACON_QUEUE				4
-#define TXCMD_QUEUE				5
-#define MGNT_QUEUE				6
-#define HIGH_QUEUE				7
-#define HCCA_QUEUE				8
-
-#define RTL_PCI_DEVICE(vend, dev, cfg)  \
-	.vendor = (vend), \
-	.device = (dev), \
-	.subvendor = PCI_ANY_ID, \
-	.subdevice = PCI_ANY_ID,\
-	.driver_data = (kernel_ulong_t)&(cfg)
-
-#define INTEL_VENDOR_ID				0x8086
-#define SIS_VENDOR_ID				0x1039
-#define ATI_VENDOR_ID				0x1002
-#define ATI_DEVICE_ID				0x7914
-#define AMD_VENDOR_ID				0x1022
-
-#define PCI_MAX_BRIDGE_NUMBER			255
-#define PCI_MAX_DEVICES				32
-#define PCI_MAX_FUNCTION			8
-
-#define PCI_CONF_ADDRESS	0x0CF8	/*PCI Configuration Space Address */
-#define PCI_CONF_DATA		0x0CFC	/*PCI Configuration Space Data */
-
-#define PCI_CLASS_BRIDGE_DEV		0x06
-#define PCI_SUBCLASS_BR_PCI_TO_PCI	0x04
-#define PCI_CAPABILITY_ID_PCI_EXPRESS	0x10
-#define PCI_CAP_ID_EXP			0x10
-
-#define U1DONTCARE			0xFF
-#define U2DONTCARE			0xFFFF
-#define U4DONTCARE			0xFFFFFFFF
-
-#define RTL_PCI_8192_DID	0x8192	/*8192 PCI-E */
-#define RTL_PCI_8192SE_DID	0x8192	/*8192 SE */
-#define RTL_PCI_8174_DID	0x8174	/*8192 SE */
-#define RTL_PCI_8173_DID	0x8173	/*8191 SE Crab */
-#define RTL_PCI_8172_DID	0x8172	/*8191 SE RE */
-#define RTL_PCI_8171_DID	0x8171	/*8191 SE Unicron */
-#define RTL_PCI_0045_DID	0x0045	/*8190 PCI for Ceraga */
-#define RTL_PCI_0046_DID	0x0046	/*8190 Cardbus for Ceraga */
-#define RTL_PCI_0044_DID	0x0044	/*8192e PCIE for Ceraga */
-#define RTL_PCI_0047_DID	0x0047	/*8192e Express Card for Ceraga */
-#define RTL_PCI_700F_DID	0x700F
-#define RTL_PCI_701F_DID	0x701F
-#define RTL_PCI_DLINK_DID	0x3304
-#define RTL_PCI_8723AE_DID	0x8723	/*8723e */
-#define RTL_PCI_8192CET_DID	0x8191	/*8192ce */
-#define RTL_PCI_8192CE_DID	0x8178	/*8192ce */
-#define RTL_PCI_8191CE_DID	0x8177	/*8192ce */
-#define RTL_PCI_8188CE_DID	0x8176	/*8192ce */
-#define RTL_PCI_8192CU_DID	0x8191	/*8192ce */
-#define RTL_PCI_8192DE_DID	0x8193	/*8192de */
-#define RTL_PCI_8192DE_DID2	0x002B	/*92DE*/
-#define RTL_PCI_8188EE_DID	0x8179  /*8188ee*/
-#define RTL_PCI_8723BE_DID	0xB723  /*8723be*/
-#define RTL_PCI_8192EE_DID	0x818B	/*8192ee*/
-#define RTL_PCI_8821AE_DID	0x8821	/*8821ae*/
-#define RTL_PCI_8812AE_DID	0x8812	/*8812ae*/
-
-/*8192 support 16 pages of IO registers*/
-#define RTL_MEM_MAPPED_IO_RANGE_8190PCI		0x1000
-#define RTL_MEM_MAPPED_IO_RANGE_8192PCIE	0x4000
-#define RTL_MEM_MAPPED_IO_RANGE_8192SE		0x4000
-#define RTL_MEM_MAPPED_IO_RANGE_8192CE		0x4000
-#define RTL_MEM_MAPPED_IO_RANGE_8192DE		0x4000
-
-#define RTL_PCI_REVISION_ID_8190PCI		0x00
-#define RTL_PCI_REVISION_ID_8192PCIE		0x01
-#define RTL_PCI_REVISION_ID_8192SE		0x10
-#define RTL_PCI_REVISION_ID_8192CE		0x1
-#define RTL_PCI_REVISION_ID_8192DE		0x0
-
-#define RTL_DEFAULT_HARDWARE_TYPE	HARDWARE_TYPE_RTL8192CE
-
-enum pci_bridge_vendor {
-	PCI_BRIDGE_VENDOR_INTEL = 0x0,	/*0b'0000,0001 */
-	PCI_BRIDGE_VENDOR_ATI,		/*0b'0000,0010*/
-	PCI_BRIDGE_VENDOR_AMD,		/*0b'0000,0100*/
-	PCI_BRIDGE_VENDOR_SIS,		/*0b'0000,1000*/
-	PCI_BRIDGE_VENDOR_UNKNOWN,	/*0b'0100,0000*/
-	PCI_BRIDGE_VENDOR_MAX,
-};
-
-struct rtl_pci_capabilities_header {
-	u8 capability_id;
-	u8 next;
-};
-
-/* In new TRX flow, Buffer_desc is new concept
-  * But TX wifi info == TX descriptor in old flow
-  * RX wifi info == RX descriptor in old flow */
-struct rtl_tx_buffer_desc {
-#if (RTL8192EE_SEG_NUM == 2)
-	u32 dword[2*(DMA_IS_64BIT + 1)*8]; /*seg = 8*/
-#elif (RTL8192EE_SEG_NUM == 1)
-	u32 dword[2*(DMA_IS_64BIT + 1)*4]; /*seg = 4*/
-#elif (RTL8192EE_SEG_NUM == 0)
-	u32 dword[2*(DMA_IS_64BIT + 1)*2]; /*seg = 2*/
-#endif
-} __packed;
-
-struct rtl_tx_desc {/*old: tx desc new: tx wifi info*/
-	u32 dword[16];
-} __packed;
-
-struct rtl_rx_buffer_desc { /*rx buffer desc*/
-	u32 dword[2];
-} __packed;
-
-struct rtl_rx_desc { /*old: rx desc new: rx wifi info*/
-	u32 dword[8];
-} __packed;
-
-struct rtl_tx_cmd_desc {
-	u32 dword[16];
-} __packed;
-
-struct rtl8192_tx_ring {
-	struct rtl_tx_desc *desc; /*tx desc / tx wifi info*/
-	dma_addr_t dma; /*tx desc dma memory / tx wifi info dma memory*/
-	unsigned int idx;
-	unsigned int entries;
-	struct sk_buff_head queue;
-	/*add for new trx flow*/
-	struct rtl_tx_buffer_desc *buffer_desc; /*tx buffer descriptor*/
-	dma_addr_t buffer_desc_dma; /*tx bufferd desc dma memory*/
-	u16 avl_desc; /* available_desc_to_write */
-	u16 cur_tx_wp; /* current_tx_write_point */
-	u16 cur_tx_rp; /* current_tx_read_point */
-};
-
-struct rtl8192_rx_ring {
-	struct rtl_rx_desc *desc;/*for old trx flow, not uesd in new trx*/
-	/*dma matches either 'desc' or 'buffer_desc'*/
-	dma_addr_t dma;
-	unsigned int idx;
-	struct sk_buff *rx_buf[RTL_PCI_MAX_RX_COUNT];
-	/*add for new trx flow*/
-	struct rtl_rx_buffer_desc *buffer_desc; /*rx buffer descriptor*/
-	u16 next_rx_rp; /* next_rx_read_point */
-};
-
-struct rtl_pci {
-	struct pci_dev *pdev;
-	bool irq_enabled;
-
-	/*Tx */
-	struct rtl8192_tx_ring tx_ring[RTL_PCI_MAX_TX_QUEUE_COUNT];
-	int txringcount[RTL_PCI_MAX_TX_QUEUE_COUNT];
-	u32 transmit_config;
-
-	/*Rx */
-	struct rtl8192_rx_ring rx_ring[RTL_PCI_MAX_RX_QUEUE];
-	int rxringcount;
-	u16 rxbuffersize;
-	u32 receive_config;
-
-	/*irq */
-	u8 irq_alloc;
-	u32 irq_mask[2];
-	u32 sys_irq_mask;
-
-	/*Bcn control register setting */
-	u32 reg_bcn_ctrl_val;
-
-	 /*ASPM*/ u8 const_pci_aspm;
-	u8 const_amdpci_aspm;
-	u8 const_hwsw_rfoff_d3;
-	u8 const_support_pciaspm;
-	/*pci-e bridge */
-	u8 const_hostpci_aspm_setting;
-	/*pci-e device */
-	u8 const_devicepci_aspm_setting;
-	/*If it supports ASPM, Offset[560h] = 0x40,
-	   otherwise Offset[560h] = 0x00. */
-	bool b_support_aspm;
-	bool b_support_backdoor;
-
-	/*QOS & EDCA */
-	enum acm_method acm_method;
-
-	u16 shortretry_limit;
-	u16 longretry_limit;
-
-	/* MSI support */
-	bool msi_support;
-	bool using_msi;
-};
-
-struct mp_adapter {
-	u8 linkctrl_reg;
-
-	u8 busnumber;
-	u8 devnumber;
-	u8 funcnumber;
-
-	u8 pcibridge_busnum;
-	u8 pcibridge_devnum;
-	u8 pcibridge_funcnum;
-
-	u8 pcibridge_vendor;
-	u16 pcibridge_vendorid;
-	u16 pcibridge_deviceid;
-
-	u32 pcicfg_addrport;
-	u8 num4bytes;
-
-	u8 pcibridge_pciehdr_offset;
-	u8 pcibridge_linkctrlreg;
-
-	bool amd_l1_patch;
-};
-
-struct rtl_pci_priv {
-	struct rtl_pci dev;
-	struct mp_adapter ndis_adapter;
-	struct rtl_led_ctl ledctl;
-	struct bt_coexist_info btcoexist;
-};
-
-#define rtl_pcipriv(hw)		(((struct rtl_pci_priv *)(rtl_priv(hw))->priv))
-#define rtl_pcidev(pcipriv)	(&((pcipriv)->dev))
-
-int rtl92e_pci_reset_trx_ring(struct ieee80211_hw *hw);
-
-extern struct rtl_intf_ops rtl92e_pci_ops;
-
-int stg_rtl_pci_probe(struct pci_dev *pdev,
-		      const struct pci_device_id *id);
-void stg_rtl_pci_disconnect(struct pci_dev *pdev);
-int stg_rtl_pci_suspend(struct device *dev);
-int stg_rtl_pci_resume(struct device *dev);
-
-static inline u8 pci_read8_sync(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return 0xff & readb((u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline u16 pci_read16_sync(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return readw((u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline u32 pci_read32_sync(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return readl((u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void pci_write8_async(struct rtl_priv *rtlpriv, u32 addr, u8 val)
-{
-	writeb(val, (u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void pci_write16_async(struct rtl_priv *rtlpriv,
-				     u32 addr, u16 val)
-{
-	writew(val, (u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void pci_write32_async(struct rtl_priv *rtlpriv,
-				     u32 addr, u32 val)
-{
-	writel(val, (u8 __iomem *)rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void rtl_pci_raw_write_port_ulong(u32 port, u32 val)
-{
-	outl(val, port);
-}
-
-static inline void rtl_pci_raw_write_port_uchar(u32 port, u8 val)
-{
-	outb(val, port);
-}
-
-static inline void rtl_pci_raw_read_port_uchar(u32 port, u8 *pval)
-{
-	*pval = inb(port);
-}
-
-static inline void rtl_pci_raw_read_port_ushort(u32 port, u16 *pval)
-{
-	*pval = inw(port);
-}
-
-static inline void rtl_pci_raw_read_port_ulong(u32 port, u32 *pval)
-{
-	*pval = inl(port);
-}
-
-#endif
diff --git a/drivers/staging/rtl8192ee/ps.c b/drivers/staging/rtl8192ee/ps.c
deleted file mode 100644
index 90c3fc2e62b6..000000000000
--- a/drivers/staging/rtl8192ee/ps.c
+++ /dev/null
@@ -1,983 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "base.h"
-#include "ps.h"
-#include "btcoexist/rtl_btc.h"
-
-bool stg_rtl_ps_enable_nic(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool init_status = true;
-
-	/*<1> reset trx ring */
-	if (rtlhal->interface == INTF_PCI)
-		rtlpriv->intf_ops->reset_trx_ring(hw);
-
-	if (is_hal_stop(rtlhal))
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("Driver is already down!\n"));
-
-	/*<2> Enable Adapter */
-	rtlpriv->cfg->ops->hw_init(hw);
-	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-	/*init_status = false; */
-
-	/*<3> Enable Interrupt */
-	rtlpriv->cfg->ops->enable_interrupt(hw);
-
-	/*<enable timer> */
-	rtl92e_watch_dog_timer_callback((unsigned long)hw);
-
-	return init_status;
-}
-EXPORT_SYMBOL(stg_rtl_ps_enable_nic);
-
-bool stg_rtl_ps_disable_nic(struct ieee80211_hw *hw)
-{
-	bool status = true;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	/*<1> Stop all timer */
-	rtl92e_deinit_deferred_work(hw);
-
-	/*<2> Disable Interrupt */
-	rtlpriv->cfg->ops->disable_interrupt(hw);
-
-	/*<3> Disable Adapter */
-	rtlpriv->cfg->ops->hw_disable(hw);
-
-	return status;
-}
-EXPORT_SYMBOL(stg_rtl_ps_disable_nic);
-
-bool stg_rtl_ps_set_rf_state(struct ieee80211_hw *hw,
-			     enum rf_pwrstate state_toset,
-			     u32 changesource, bool protect_or_not)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	enum rf_pwrstate rtstate;
-	bool b_actionallowed = false;
-	u16 rfwait_cnt = 0;
-
-	/*protect_or_not = true; */
-
-	if (protect_or_not)
-		goto no_protect;
-
-	/*
-	 *Only one thread can change
-	 *the RF state at one time, and others
-	 *should wait to be executed.
-	 */
-	while (true) {
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-		if (ppsc->rfchange_inprogress) {
-			spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-			RT_TRACE(COMP_ERR, DBG_WARNING,
-				 ("RF Change in progress! Wait to set..state_toset(%d)\n",
-				  state_toset));
-
-			/* Set RF after the previous action is done.  */
-			while (ppsc->rfchange_inprogress) {
-				rfwait_cnt++;
-				mdelay(1);
-				/*
-				 *Wait too long, return false to avoid
-				 *to be stuck here.
-				 */
-				if (rfwait_cnt > 100)
-					return false;
-			}
-		} else {
-			ppsc->rfchange_inprogress = true;
-			spin_unlock(&rtlpriv->locks.rf_ps_lock);
-			break;
-		}
-	}
-
-no_protect:
-	rtstate = ppsc->rfpwr_state;
-
-	switch (state_toset) {
-	case ERFON:
-		ppsc->rfoff_reason &= (~changesource);
-
-		if ((changesource == RF_CHANGE_BY_HW) &&
-		    (ppsc->b_hwradiooff)) {
-			ppsc->b_hwradiooff = false;
-		}
-		if (!ppsc->rfoff_reason) {
-			ppsc->rfoff_reason = 0;
-			b_actionallowed = true;
-		}
-		break;
-	case ERFOFF:
-		if ((changesource == RF_CHANGE_BY_HW) &&
-		    (!ppsc->b_hwradiooff)) {
-			ppsc->b_hwradiooff = true;
-		}
-		ppsc->rfoff_reason |= changesource;
-		b_actionallowed = true;
-		break;
-	case ERFSLEEP:
-		ppsc->rfoff_reason |= changesource;
-		b_actionallowed = true;
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case not process\n"));
-		break;
-	}
-
-	if (b_actionallowed)
-		rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
-
-	if (!protect_or_not) {
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-		ppsc->rfchange_inprogress = false;
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-	}
-
-	return b_actionallowed;
-}
-EXPORT_SYMBOL(stg_rtl_ps_set_rf_state);
-
-static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	ppsc->b_swrf_processing = true;
-
-	if (ppsc->inactive_pwrstate == ERFON && rtlhal->interface == INTF_PCI) {
-		if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
-		    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
-		    rtlhal->interface == INTF_PCI) {
-			rtlpriv->intf_ops->disable_aspm(hw);
-			RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-		}
-	}
-
-	stg_rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate,
-				RF_CHANGE_BY_IPS, false);
-
-	if (ppsc->inactive_pwrstate == ERFOFF &&
-	    rtlhal->interface == INTF_PCI) {
-		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
-		    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
-			rtlpriv->intf_ops->enable_aspm(hw);
-			RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-		}
-	}
-
-	ppsc->b_swrf_processing = false;
-}
-
-void rtl92e_ips_nic_off_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-	    container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	enum rf_pwrstate rtstate;
-
-	if (mac->opmode != NL80211_IFTYPE_STATION) {
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("not station return\n"));
-		return;
-	}
-
-	if (mac->p2p_in_use)
-		return;
-
-	if (mac->link_state > MAC80211_NOLINK)
-		return;
-
-	if (is_hal_stop(rtlhal))
-		return;
-
-	if (rtlpriv->sec.being_setkey)
-		return;
-
-	if (rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps)
-		rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps(hw);
-
-	if (ppsc->b_inactiveps) {
-		rtstate = ppsc->rfpwr_state;
-
-		/*
-		 *Do not enter IPS in the following conditions:
-		 *(1) RF is already OFF or Sleep
-		 *(2) b_swrf_processing (indicates the IPS is still under going)
-		 *(3) Connectted (only disconnected can trigger IPS)
-		 *(4) IBSS (send Beacon)
-		 *(5) AP mode (send Beacon)
-		 *(6) monitor mode (rcv packet)
-		 */
-
-		if (rtstate == ERFON &&
-		    !ppsc->b_swrf_processing &&
-		    (mac->link_state == MAC80211_NOLINK) &&
-		    !mac->act_scanning) {
-			RT_TRACE(COMP_RF, DBG_LOUD,
-				 ("IPSEnter(): Turn off RF.\n"));
-
-			ppsc->inactive_pwrstate = ERFOFF;
-			ppsc->b_in_powersavemode = true;
-
-			/* call before RF off */
-			if (rtlpriv->cfg->ops->get_btc_status())
-				rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
-									ppsc->inactive_pwrstate);
-
-			/*rtl92e_pci_reset_trx_ring(hw); */
-			_rtl_ps_inactive_ps(hw);
-		}
-	}
-}
-
-void rtl92e_ips_nic_off(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	/*
-	 *because when link with ap, mac80211 will ask us
-	 *to disable nic quickly after scan before linking,
-	 *this will cause link failed, so we delay 100ms here
-	 */
-	queue_delayed_work(rtlpriv->works.rtl_wq,
-			   &rtlpriv->works.ips_nic_off_wq, MSECS(100));
-}
-
-/* NOTICE: any opmode should exc nic_on, or disable without
- * nic_on may something wrong, like adhoc TP*/
-void rtl92e_ips_nic_on(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	enum rf_pwrstate rtstate;
-
-	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
-
-	spin_lock(&rtlpriv->locks.ips_lock);
-	if (ppsc->b_inactiveps) {
-		rtstate = ppsc->rfpwr_state;
-
-		if (rtstate != ERFON &&
-		    !ppsc->b_swrf_processing &&
-		    ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
-			ppsc->inactive_pwrstate = ERFON;
-			ppsc->b_in_powersavemode = false;
-			_rtl_ps_inactive_ps(hw);
-			/* call after RF on */
-			if (rtlpriv->cfg->ops->get_btc_status())
-				rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
-									ppsc->inactive_pwrstate);
-		}
-	}
-	spin_unlock(&rtlpriv->locks.ips_lock);
-}
-
-/*for FW LPS*/
-
-/*
- *Determine if we can set Fw into PS mode
- *in current condition.Return true if it
- *can enter PS mode.
- */
-static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u32 ps_timediff;
-
-	ps_timediff = jiffies_to_msecs(jiffies -
-				       ppsc->last_delaylps_stamp_jiffies);
-
-	if (ps_timediff < 2000) {
-		RT_TRACE(COMP_POWER, DBG_LOUD,
-			 ("Delay enter Fw LPS for DHCP, ARP, or EAPOL exchanging state\n"));
-		return false;
-	}
-
-	if (mac->link_state != MAC80211_LINKED)
-		return false;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		return false;
-
-	return true;
-}
-
-/* Change current and default preamble mode.*/
-void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool enter_fwlps;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		return;
-
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-
-	if (ppsc->dot11_psmode == rt_psmode)
-		return;
-
-	/* Update power save mode configured. */
-	ppsc->dot11_psmode = rt_psmode;
-
-	/*
-	 *<FW control LPS>
-	 *1. Enter PS mode
-	 *   Set RPWM to Fw to turn RF off and send H2C fw_pwrmode
-	 *   cmd to set Fw into PS mode.
-	 *2. Leave PS mode
-	 *   Send H2C fw_pwrmode cmd to Fw to set Fw into Active
-	 *   mode and set RPWM to turn RF on.
-	 */
-
-	if ((ppsc->b_fwctrl_lps) && ppsc->report_linked) {
-		if (ppsc->dot11_psmode == EACTIVE) {
-			RT_TRACE(COMP_RF, DBG_DMESG,
-				 ("FW LPS leave ps_mode:%x\n",
-				  FW_PS_ACTIVE_MODE));
-			enter_fwlps = false;
-			ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
-			ppsc->smart_ps = 0;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_LPS_ACTION,
-						      (u8 *)(&enter_fwlps));
-			if (ppsc->p2p_ps_info.opp_ps)
-				rtl92e_p2p_ps_cmd(hw , P2P_PS_ENABLE);
-
-			if (rtlpriv->cfg->ops->get_btc_status())
-				rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode);
-		} else {
-			if (rtl_get_fwlps_doze(hw)) {
-				RT_TRACE(COMP_RF, DBG_DMESG,
-					 ("FW LPS enter ps_mode:%x\n",
-					 ppsc->fwctrl_psmode));
-				if (rtlpriv->cfg->ops->get_btc_status())
-					rtlpriv->btcoexist.btc_ops->btc_lps_notify(rtlpriv, rt_psmode);
-				enter_fwlps = true;
-				ppsc->pwr_mode = ppsc->fwctrl_psmode;
-				ppsc->smart_ps = 2;
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-							HW_VAR_FW_LPS_ACTION,
-							(u8 *)(&enter_fwlps));
-
-			} else {
-				/* Reset the power save related parameters. */
-				ppsc->dot11_psmode = EACTIVE;
-			}
-		}
-	}
-}
-
-/*Enter the leisure power save mode.*/
-void rtl92e_lps_enter(struct ieee80211_hw *hw)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	unsigned long flag;
-
-	if (!ppsc->b_fwctrl_lps)
-		return;
-
-	if (rtlpriv->sec.being_setkey)
-		return;
-
-	if (rtlpriv->link_info.b_busytraffic)
-		return;
-
-	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
-	if (mac->cnt_after_linked < 5)
-		return;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		return;
-
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-
-	/* Idle for a while if we connect to AP a while ago. */
-	if (mac->cnt_after_linked >= 2) {
-		if (ppsc->dot11_psmode == EACTIVE) {
-			RT_TRACE(COMP_POWER, DBG_LOUD,
-				 ("Enter 802.11 power save mode...\n"));
-
-			rtl_lps_set_psmode(hw, EAUTOPS);
-		}
-	}
-
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-}
-EXPORT_SYMBOL(rtl92e_lps_enter);
-
-/*Leave the leisure power save mode.*/
-void rtl92e_lps_leave(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	unsigned long flag;
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-
-	if (ppsc->b_fwctrl_lps) {
-		if (ppsc->dot11_psmode != EACTIVE) {
-			if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
-			    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
-			    rtlhal->interface == INTF_PCI) {
-				rtlpriv->intf_ops->disable_aspm(hw);
-				RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-			}
-
-			RT_TRACE(COMP_POWER, DBG_LOUD,
-				 ("Busy Traffic,Leave 802.11 power save..\n"));
-
-			rtl_lps_set_psmode(hw, EACTIVE);
-		}
-	}
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-}
-EXPORT_SYMBOL(rtl92e_lps_leave);
-
-/* For sw LPS*/
-void rtl92e_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = (void *)data;
-	struct ieee80211_tim_ie *tim_ie;
-	u8 *tim;
-	u8 tim_len;
-	bool u_buffed;
-	bool m_buffed;
-
-	if (mac->opmode != NL80211_IFTYPE_STATION)
-		return;
-
-	if (!rtlpriv->psc.b_swctrl_lps)
-		return;
-
-	if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
-		return;
-
-	if (!rtlpriv->psc.sw_ps_enabled)
-		return;
-
-	if (rtlpriv->psc.b_fwctrl_lps)
-		return;
-
-	if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
-		return;
-
-	/* check if this really is a beacon */
-	if (!ieee80211_is_beacon(hdr->frame_control))
-		return;
-
-	/* min. beacon length + FCS_LEN */
-	if (len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	rtlpriv->psc.last_beacon = jiffies;
-
-	tim = rtl92e_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
-	if (!tim)
-		return;
-
-	if (tim[1] < sizeof(*tim_ie))
-		return;
-
-	tim_len = tim[1];
-	tim_ie = (struct ieee80211_tim_ie *)&tim[2];
-
-	if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
-		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
-
-	/* Check whenever the PHY can be turned off again. */
-
-	/* 1. What about buffered unicast traffic for our AID? */
-	u_buffed = ieee80211_check_tim(tim_ie, tim_len,
-				       rtlpriv->mac80211.assoc_id);
-
-	/* 2. Maybe the AP wants to send multicast/broadcast data? */
-	m_buffed = tim_ie->bitmap_ctrl & 0x01;
-	rtlpriv->psc.multi_buffered = m_buffed;
-
-	/* unicast will process by mac80211 through
-	 * set ~IEEE80211_CONF_PS, So we just check
-	 * multicast frames here */
-	if (!m_buffed) {/*&&) { !rtlpriv->psc.tx_doing) { */
-		/* back to low-power land. and delay is
-		 * prevent null power save frame tx fail */
-		queue_delayed_work(rtlpriv->works.rtl_wq,
-				   &rtlpriv->works.ps_work, MSECS(5));
-	} else {
-		RT_TRACE(COMP_POWER, DBG_DMESG,
-			 ("u_bufferd: %x, m_buffered: %x\n",
-			  u_buffed, m_buffed));
-	}
-}
-
-void rtl92e_swlps_rf_awake(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	unsigned long flag;
-
-	if (!rtlpriv->psc.b_swctrl_lps)
-		return;
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-
-	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
-	    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
-		rtlpriv->intf_ops->disable_aspm(hw);
-		RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-	}
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-	stg_rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS, false);
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-}
-
-void rtl92e_swlps_rfon_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-	    container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-
-	rtl92e_swlps_rf_awake(hw);
-}
-
-void rtl92e_swlps_rf_sleep(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	unsigned long flag;
-	u8 sleep_intv;
-
-	if (!rtlpriv->psc.sw_ps_enabled)
-		return;
-
-	if ((rtlpriv->sec.being_setkey) ||
-	    (mac->opmode == NL80211_IFTYPE_ADHOC))
-		return;
-
-	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
-	if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
-		return;
-
-	if (rtlpriv->link_info.b_busytraffic)
-		return;
-
-	spin_lock(&rtlpriv->locks.rf_ps_lock);
-	if (rtlpriv->psc.rfchange_inprogress) {
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-		return;
-	}
-	spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-	stg_rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS , false);
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-
-	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
-	    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
-		rtlpriv->intf_ops->enable_aspm(hw);
-		RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-	}
-
-	/* here is power save alg, when this beacon is DTIM
-	 * we will set sleep time to dtim_period * n;
-	 * when this beacon is not DTIM, we will set sleep
-	 * time to sleep_intv = rtlpriv->psc.dtim_counter or
-	 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
-
-	if (rtlpriv->psc.dtim_counter == 0) {
-		if (hw->conf.ps_dtim_period == 1)
-			sleep_intv = hw->conf.ps_dtim_period * 2;
-		else
-			sleep_intv = hw->conf.ps_dtim_period;
-	} else {
-		sleep_intv = rtlpriv->psc.dtim_counter;
-	}
-
-	if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
-		sleep_intv = MAX_SW_LPS_SLEEP_INTV;
-
-	/* this print should always be dtim_conter = 0 &
-	 * sleep  = dtim_period, that meaons, we should
-	 * awake before every dtim */
-	RT_TRACE(COMP_POWER, DBG_DMESG,
-		 ("dtim_counter:%x will sleep :%d beacon_intv\n",
-		  rtlpriv->psc.dtim_counter, sleep_intv));
-
-	/* we tested that 40ms is enough for sw & hw sw delay */
-	queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
-			   MSECS(sleep_intv*mac->vif->bss_conf.beacon_int-40));
-}
-
-
-void rtl92e_swlps_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-		container_of_dwork_rtl(data, struct rtl_works, ps_work);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool ps = false;
-
-	ps = (hw->conf.flags & IEEE80211_CONF_PS);
-
-	/* we can sleep after ps null send ok */
-	if (rtlpriv->psc.state_inap) {
-		rtl92e_swlps_rf_sleep(hw);
-
-		if (rtlpriv->psc.state && !ps) {
-			rtlpriv->psc.sleep_ms =
-				jiffies_to_msecs(jiffies -
-						 rtlpriv->psc.last_action);
-		}
-
-		if (ps)
-			rtlpriv->psc.last_slept = jiffies;
-
-		rtlpriv->psc.last_action = jiffies;
-		rtlpriv->psc.state = ps;
-	}
-}
-
-static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
-			   unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
-	u8 *pos, *end, *ie;
-	u16 noa_len;
-	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
-	u8 noa_num, index , i, noa_index = 0;
-	bool find_p2p_ie = false , find_p2p_ps_ie = false;
-	pos = (u8 *)mgmt->u.beacon.variable;
-	end = data + len;
-	ie = NULL;
-
-	while (pos + 1 < end) {
-		if (pos + 2 + pos[1] > end)
-			return;
-
-		if (pos[0] == 221 && pos[1] > 4) {
-			if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
-				ie = pos + 2+4;
-				break;
-			}
-		}
-		pos += 2 + pos[1];
-	}
-
-	if (ie == NULL)
-		return;
-	find_p2p_ie = true;
-	/*to find noa ie*/
-	while (ie + 1 < end) {
-		noa_len = READEF2BYTE((__le16 *)&ie[1]);
-		if (ie + 3 + ie[1] > end)
-			return;
-
-		if (ie[0] == 12) {
-			find_p2p_ps_ie = true;
-			if ((noa_len - 2) % 13 != 0) {
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					 ("P2P notice of absence: invalid length%d\n",
-					 noa_len));
-				return;
-			} else {
-				noa_num = (noa_len - 2) / 13;
-			}
-			noa_index = ie[3];
-			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
-			    P2P_PS_NONE || noa_index != p2pinfo->noa_index) {
-				RT_TRACE(COMP_FW, DBG_LOUD,
-					 ("update NOA ie.\n"));
-				p2pinfo->noa_index = noa_index;
-				p2pinfo->opp_ps = (ie[4] >> 7);
-				p2pinfo->ctwindow = ie[4] & 0x7F;
-				p2pinfo->noa_num = noa_num;
-				index = 5;
-				for (i = 0; i < noa_num; i++) {
-					p2pinfo->noa_count_type[i] =
-							READEF1BYTE(ie+index);
-					index += 1;
-					p2pinfo->noa_duration[i] =
-						 READEF4BYTE((__le32 *)ie+index);
-					index += 4;
-					p2pinfo->noa_interval[i] =
-						 READEF4BYTE((__le32 *)ie+index);
-					index += 4;
-					p2pinfo->noa_start_time[i] =
-						 READEF4BYTE((__le32 *)ie+index);
-					index += 4;
-				}
-
-				if (p2pinfo->opp_ps == 1) {
-					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
-					/* Driver should wait LPS
-					 * entering CTWindow*/
-					if (rtlpriv->psc.b_fw_current_inpsmode) {
-						rtl92e_p2p_ps_cmd(hw,
-							       P2P_PS_ENABLE);
-					}
-				} else if (p2pinfo->noa_num > 0) {
-					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
-					rtl92e_p2p_ps_cmd(hw, P2P_PS_ENABLE);
-				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-					rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-				}
-			}
-
-		break;
-		}
-		ie += 3 + noa_len;
-	}
-
-	if (find_p2p_ie) {
-		if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
-		    (!find_p2p_ps_ie))
-			rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-	}
-}
-
-static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
-			      unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
-	bool find_p2p_ie = false, find_p2p_ps_ie = false;
-	u8 noa_num, index, i, noa_index = 0;
-	u8 *pos, *end, *ie;
-	u16 noa_len;
-	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
-
-	pos = (u8 *)&mgmt->u.action.category;
-	end = data + len;
-	ie = NULL;
-
-	if (pos[0] == 0x7f) {
-		if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0)
-			ie = pos + 3+4;
-	}
-
-	if (ie == NULL)
-		return;
-	find_p2p_ie = true;
-
-	RT_TRACE(COMP_FW, DBG_LOUD, ("action frame find P2P IE.\n"));
-	/*to find noa ie*/
-	while (ie + 1 < end) {
-		noa_len = READEF2BYTE((__le16 *)&ie[1]);
-		if (ie + 3 + ie[1] > end)
-			return;
-
-		if (ie[0] == 12) {
-			RT_TRACE(COMP_FW, DBG_LOUD, ("find NOA IE\n"));
-			RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, "noa ie ",
-				      ie, noa_len);
-			find_p2p_ps_ie = true;
-			if ((noa_len - 2) % 13 != 0) {
-				RT_TRACE(COMP_FW, DBG_LOUD,
-					 ("P2P notice of absence: invalid length%d\n",
-					 noa_len));
-				return;
-			} else {
-				noa_num = (noa_len - 2) / 13;
-			}
-			noa_index = ie[3];
-			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode ==
-			    P2P_PS_NONE ||
-			    noa_index != p2pinfo->noa_index) {
-				p2pinfo->noa_index = noa_index;
-				p2pinfo->opp_ps = (ie[4] >> 7);
-				p2pinfo->ctwindow = ie[4] & 0x7F;
-				p2pinfo->noa_num = noa_num;
-				index = 5;
-				for (i = 0; i < noa_num; i++) {
-					p2pinfo->noa_count_type[i] =
-							READEF1BYTE(ie+index);
-					index += 1;
-					p2pinfo->noa_duration[i] =
-						 READEF4BYTE((__le32 *)ie+index);
-					index += 4;
-					p2pinfo->noa_interval[i] =
-						 READEF4BYTE((__le32 *)ie+index);
-					index += 4;
-					p2pinfo->noa_start_time[i] =
-						 READEF4BYTE((__le32 *)ie+index);
-					index += 4;
-				}
-
-				if (p2pinfo->opp_ps == 1) {
-					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
-					/* Driver should wait LPS
-					 * entering CTWindow */
-					if (rtlpriv->psc.b_fw_current_inpsmode) {
-						rtl92e_p2p_ps_cmd(hw,
-							       P2P_PS_ENABLE);
-					}
-				} else if (p2pinfo->noa_num > 0) {
-					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
-					rtl92e_p2p_ps_cmd(hw, P2P_PS_ENABLE);
-				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-					rtl92e_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-				}
-			}
-
-		break;
-		}
-		ie += 3 + noa_len;
-	}
-}
-
-void rtl92e_p2p_ps_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
-	struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
-
-	RT_TRACE(COMP_FW, DBG_LOUD, ("p2p state %x\n", p2p_ps_state));
-	switch (p2p_ps_state) {
-	case P2P_PS_DISABLE:
-		p2pinfo->p2p_ps_state = p2p_ps_state;
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-					   HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-					   (u8 *)(&p2p_ps_state));
-
-		p2pinfo->noa_index = 0;
-		p2pinfo->ctwindow = 0;
-		p2pinfo->opp_ps = 0;
-		p2pinfo->noa_num = 0;
-		p2pinfo->p2p_ps_mode = P2P_PS_NONE;
-		if (rtlps->b_fw_current_inpsmode) {
-			if (rtlps->smart_ps == 0) {
-				rtlps->smart_ps = 2;
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-					    HW_VAR_H2C_FW_PWRMODE,
-					    (u8 *)(&rtlps->pwr_mode));
-			}
-		}
-		break;
-	case P2P_PS_ENABLE:
-		if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-			p2pinfo->p2p_ps_state = p2p_ps_state;
-
-			if (p2pinfo->ctwindow > 0) {
-				if (rtlps->smart_ps != 0) {
-					rtlps->smart_ps = 0;
-					rtlpriv->cfg->ops->set_hw_reg(
-					    hw, HW_VAR_H2C_FW_PWRMODE,
-					    (u8 *)(&rtlps->pwr_mode));
-				}
-			}
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-						(u8 *)(&p2p_ps_state));
-			}
-			break;
-	case P2P_PS_SCAN:
-	case P2P_PS_SCAN_DONE:
-	case P2P_PS_ALLSTASLEEP:
-		if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-			p2pinfo->p2p_ps_state = p2p_ps_state;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-						(u8 *)(&p2p_ps_state));
-		}
-		break;
-	default:
-		break;
-	}
-	RT_TRACE(COMP_FW, DBG_LOUD, (" ctwindow %x oppps %x\n",
-				     p2pinfo->ctwindow , p2pinfo->opp_ps));
-	RT_TRACE(COMP_FW, DBG_LOUD,
-		 ("count %x duration %x index %x interval %x start time %x noa num %x\n",
-		 p2pinfo->noa_count_type[0],
-		 p2pinfo->noa_duration[0],
-		 p2pinfo->noa_index,
-		 p2pinfo->noa_interval[0],
-		 p2pinfo->noa_start_time[0],
-		 p2pinfo->noa_num));
-	RT_TRACE(COMP_FW, DBG_LOUD, ("end\n"));
-}
-
-void rtl92e_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = (void *)data;
-
-	if (!mac->p2p)
-		return;
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-	/* min. beacon length + FCS_LEN */
-	if (len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (!ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	/* check if this really is a beacon */
-	if (!(ieee80211_is_beacon(hdr->frame_control) ||
-	      ieee80211_is_probe_resp(hdr->frame_control) ||
-	      ieee80211_is_action(hdr->frame_control)))
-		return;
-
-	if (ieee80211_is_action(hdr->frame_control))
-		rtl_p2p_action_ie(hw , data , len - FCS_LEN);
-	else
-		rtl_p2p_noa_ie(hw , data , len - FCS_LEN);
-}
diff --git a/drivers/staging/rtl8192ee/ps.h b/drivers/staging/rtl8192ee/ps.h
deleted file mode 100644
index 1533661a2f43..000000000000
--- a/drivers/staging/rtl8192ee/ps.h
+++ /dev/null
@@ -1,52 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __REALTEK_RTL_PCI_PS_H__
-#define __REALTEK_RTL_PCI_PS_H__
-
-#define MAX_SW_LPS_SLEEP_INTV	5
-
-bool stg_rtl_ps_set_rf_state(struct ieee80211_hw *hw,
-			     enum rf_pwrstate state_toset, u32 changesource,
-			     bool protect_or_not);
-bool stg_rtl_ps_enable_nic(struct ieee80211_hw *hw);
-bool stg_rtl_ps_disable_nic(struct ieee80211_hw *hw);
-void rtl92e_ips_nic_off(struct ieee80211_hw *hw);
-void rtl92e_ips_nic_on(struct ieee80211_hw *hw);
-void rtl92e_ips_nic_off_wq_callback(void *data);
-void rtl92e_lps_enter(struct ieee80211_hw *hw);
-void rtl92e_lps_leave(struct ieee80211_hw *hw);
-
-void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode);
-
-void rtl92e_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len);
-void rtl92e_swlps_wq_callback(void *data);
-void rtl92e_swlps_rfon_wq_callback(void *data);
-void rtl92e_swlps_rf_awake(struct ieee80211_hw *hw);
-void rtl92e_swlps_rf_sleep(struct ieee80211_hw *hw);
-void rtl92e_p2p_ps_cmd(struct ieee80211_hw *hw , u8 p2p_ps_state);
-void rtl92e_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len);
-
-#endif
diff --git a/drivers/staging/rtl8192ee/rc.c b/drivers/staging/rtl8192ee/rc.c
deleted file mode 100644
index c4c34ddcf8cd..000000000000
--- a/drivers/staging/rtl8192ee/rc.c
+++ /dev/null
@@ -1,288 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "base.h"
-#include "rc.h"
-
-/*
- *Finds the highest rate index we can use
- *if skb is special data like DHCP/EAPOL, we set should
- *it to lowest rate CCK_1M, otherwise we set rate to
- *highest rate based on wireless mode used for iwconfig
- *show Tx rate.
- */
-static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
-				  struct ieee80211_sta *sta,
-				  struct sk_buff *skb, bool not_data)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_sta_info *sta_entry = NULL;
-	u8 wireless_mode = 0;
-
-	/*
-	 *this rate is no use for true rate, firmware
-	 *will control rate at all it just used for
-	 *1.show in iwconfig in B/G mode
-	 *2.in stg_rtl_get_tcb_desc when we check rate is
-	 *      1M we will not use FW rate but user rate.
-	 */
-
-	if (sta) {
-		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-		wireless_mode = sta_entry->wireless_mode;
-	}
-
-	if (rtl92e_is_special_data(rtlpriv->mac80211.hw, skb, true) ||
-	    not_data) {
-		return 0;
-	} else {
-		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-			if (wireless_mode == WIRELESS_MODE_B) {
-				return B_MODE_MAX_RIX;
-			} else if (wireless_mode == WIRELESS_MODE_G) {
-				return G_MODE_MAX_RIX;
-			} else if (wireless_mode == WIRELESS_MODE_N_24G) {
-				if (get_rf_type(rtlphy) != RF_2T2R)
-					return N_MODE_MCS7_RIX;
-				else
-					return N_MODE_MCS15_RIX;
-			} else if (wireless_mode == WIRELESS_MODE_AC_24G) {
-				return AC_MODE_MCS9_RIX;
-			} else {
-				return 0;
-			}
-		} else {
-			if (wireless_mode == WIRELESS_MODE_A) {
-				return A_MODE_MAX_RIX;
-			} else if (wireless_mode == WIRELESS_MODE_N_5G) {
-				if (get_rf_type(rtlphy) != RF_2T2R)
-					return N_MODE_MCS7_RIX;
-				else
-					return N_MODE_MCS15_RIX;
-			} else if (wireless_mode == WIRELESS_MODE_AC_5G) {
-				return AC_MODE_MCS9_RIX;
-			} else {
-				return 0;
-			}
-		}
-	}
-}
-
-static void _rtl_rc_rate_set_series(struct rtl_priv *rtlpriv,
-				    struct ieee80211_sta *sta,
-				    struct ieee80211_tx_rate *rate,
-				    struct ieee80211_tx_rate_control *txrc,
-				    u8 tries, char rix, int rtsctsenable,
-				    bool not_data)
-{
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	u8 sgi_20 = 0, sgi_40 = 0, sgi_80 = 0;
-
-	if (sta) {
-		sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
-		sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
-		sgi_80 = sta->vht_cap.cap & IEEE80211_VHT_CAP_SHORT_GI_80;
-	}
-	rate->count = tries;
-	rate->idx = rix >= 0x00 ? rix : 0x00;
-
-	if (!not_data) {
-		if (txrc->short_preamble)
-			rate->flags |= IEEE80211_TX_RC_USE_SHORT_PREAMBLE;
-		if (mac->opmode == NL80211_IFTYPE_AP ||
-		    mac->opmode == NL80211_IFTYPE_ADHOC) {
-			if (sta && (sta->ht_cap.cap &
-				    IEEE80211_HT_CAP_SUP_WIDTH_20_40))
-				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
-			if (sta && (sta->vht_cap.vht_supported))
-				rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
-		} else {
-			if (mac->bw_40)
-				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
-			if (mac->bw_80)
-				rate->flags |= IEEE80211_TX_RC_80_MHZ_WIDTH;
-		}
-
-		if (sgi_20 || sgi_40 || sgi_80)
-			rate->flags |= IEEE80211_TX_RC_SHORT_GI;
-		if (sta && sta->ht_cap.ht_supported)
-			rate->flags |= IEEE80211_TX_RC_MCS;
-		if (sta && sta->vht_cap.vht_supported)
-			rate->flags |= IEEE80211_TX_RC_VHT_MCS;
-	}
-}
-
-static void rtl_get_rate(void *ppriv, struct ieee80211_sta *sta,
-			 void *priv_sta,
-			 struct ieee80211_tx_rate_control *txrc)
-{
-	struct rtl_priv *rtlpriv = ppriv;
-	struct sk_buff *skb = txrc->skb;
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_tx_rate *rates = tx_info->control.rates;
-	__le16 fc = rtl_get_fc(skb);
-	u8 try_per_rate, i, rix;
-	bool not_data = !ieee80211_is_data(fc);
-
-	if (rate_control_send_low(sta, priv_sta, txrc))
-		return;
-
-	rix = _rtl_rc_get_highest_rix(rtlpriv, sta, skb, not_data);
-	try_per_rate = 1;
-	_rtl_rc_rate_set_series(rtlpriv, sta, &rates[0], txrc,
-				try_per_rate, rix, 1, not_data);
-
-	if (!not_data) {
-		for (i = 1; i < 4; i++)
-			_rtl_rc_rate_set_series(rtlpriv, sta, &rates[i],
-						txrc, i, (rix - i), 1,
-						not_data);
-	}
-}
-
-static bool _rtl_tx_aggr_check(struct rtl_priv *rtlpriv,
-			       struct rtl_sta_info *sta_entry, u16 tid)
-{
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-
-	if (mac->act_scanning)
-		return false;
-
-	if (mac->opmode == NL80211_IFTYPE_STATION &&
-	    mac->cnt_after_linked < 3)
-		return false;
-
-	if (sta_entry->tids[tid].agg.agg_state == RTL_AGG_STOP)
-		return true;
-
-	return false;
-}
-
-/*mac80211 Rate Control callbacks*/
-static void rtl_tx_status(void *ppriv,
-			  struct ieee80211_supported_band *sband,
-			  struct ieee80211_sta *sta, void *priv_sta,
-			  struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = ppriv;
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	__le16 fc = rtl_get_fc(skb);
-	struct rtl_sta_info *sta_entry;
-
-	if (!priv_sta || !ieee80211_is_data(fc))
-		return;
-
-	if (rtl92e_is_special_data(mac->hw, skb, true))
-		return;
-
-	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
-	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
-		return;
-
-	if (sta) {
-		/* Check if aggregation has to be enabled for this tid */
-		sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-		if ((sta->ht_cap.ht_supported) &&
-		    !(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
-			if (ieee80211_is_data_qos(fc)) {
-				u8 tid = rtl_get_tid(skb);
-				if (_rtl_tx_aggr_check(rtlpriv, sta_entry,
-						       tid)) {
-					sta_entry->tids[tid].agg.agg_state =
-						RTL_AGG_PROGRESS;
-					ieee80211_start_tx_ba_session(sta, tid,
-								      5000);
-				}
-			}
-		}
-	}
-}
-
-static void rtl_rate_init(void *ppriv,
-			  struct ieee80211_supported_band *sband,
-			  struct cfg80211_chan_def *chandef,
-			  struct ieee80211_sta *sta, void *priv_sta)
-{
-}
-
-static void *rtl_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	return rtlpriv;
-}
-
-static void rtl_rate_free(void *rtlpriv)
-{
-	return;
-}
-
-static void *rtl_rate_alloc_sta(void *ppriv,
-				struct ieee80211_sta *sta, gfp_t gfp)
-{
-	struct rtl_priv *rtlpriv = ppriv;
-	struct rtl_rate_priv *rate_priv;
-
-	rate_priv = kzalloc(sizeof(*rate_priv), gfp);
-	if (!rate_priv) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Unable to allocate private rc structure\n"));
-		return NULL;
-	}
-
-	rtlpriv->rate_priv = rate_priv;
-
-	return rate_priv;
-}
-
-static void rtl_rate_free_sta(void *rtlpriv,
-			      struct ieee80211_sta *sta, void *priv_sta)
-{
-	struct rtl_rate_priv *rate_priv = priv_sta;
-	kfree(rate_priv);
-}
-
-static struct rate_control_ops rtl_rate_ops = {
-	.name = "rtl_rc_92e",
-	.alloc = rtl_rate_alloc,
-	.free = rtl_rate_free,
-	.alloc_sta = rtl_rate_alloc_sta,
-	.free_sta = rtl_rate_free_sta,
-	.rate_init = rtl_rate_init,
-	.tx_status = rtl_tx_status,
-	.get_rate = rtl_get_rate,
-};
-
-int rtl92e_rate_control_register(void)
-{
-	return ieee80211_rate_control_register(&rtl_rate_ops);
-}
-
-void rtl92e_rate_control_unregister(void)
-{
-	ieee80211_rate_control_unregister(&rtl_rate_ops);
-}
diff --git a/drivers/staging/rtl8192ee/regd.c b/drivers/staging/rtl8192ee/regd.c
deleted file mode 100644
index 7272fae68ec6..000000000000
--- a/drivers/staging/rtl8192ee/regd.c
+++ /dev/null
@@ -1,448 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "regd.h"
-
-static struct country_code_to_enum_rd allcountries[] = {
-	{COUNTRY_CODE_FCC, "US"},
-	{COUNTRY_CODE_IC, "US"},
-	{COUNTRY_CODE_ETSI, "EC"},
-	{COUNTRY_CODE_SPAIN, "EC"},
-	{COUNTRY_CODE_FRANCE, "EC"},
-	{COUNTRY_CODE_MKK, "JP"},
-	{COUNTRY_CODE_MKK1, "JP"},
-	{COUNTRY_CODE_ISRAEL, "EC"},
-	{COUNTRY_CODE_TELEC, "JP"},
-	{COUNTRY_CODE_MIC, "JP"},
-	{COUNTRY_CODE_GLOBAL_DOMAIN, "JP"},
-	{COUNTRY_CODE_WORLD_WIDE_13, "EC"},
-	{COUNTRY_CODE_TELEC_NETGEAR, "EC"},
-};
-
-/*
- *Only these channels all allow active
- *scan on all world regulatory domains
- */
-#define RTL819x_2GHZ_CH01_11	\
-	REG_RULE(2412-10, 2462+10, 40, 0, 20, 0)
-
-/*
- *We enable active scan on these a case
- *by case basis by regulatory domain
- */
-#define RTL819x_2GHZ_CH12_13	\
-	REG_RULE(2467-10, 2472+10, 40, 0, 20,\
-	NL80211_RRF_PASSIVE_SCAN)
-
-#define RTL819x_2GHZ_CH14	\
-	REG_RULE(2484-10, 2484+10, 40, 0, 20, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_OFDM)
-
-/* 5G chan 36 - chan 64*/
-#define RTL819x_5GHZ_5150_5350	\
-	REG_RULE(5150-10, 5350+10, 80, 0, 30, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_IBSS)
-
-/* 5G chan 100 - chan 165*/
-#define RTL819x_5GHZ_5470_5850	\
-	REG_RULE(5470-10, 5850+10, 80, 0, 30, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_IBSS)
-
-/* 5G chan 149 - chan 165*/
-#define RTL819x_5GHZ_5725_5850	\
-	REG_RULE(5725-10, 5850+10, 80, 0, 30, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_IBSS)
-
-#define RTL819x_5GHZ_ALL	\
-	(RTL819x_5GHZ_5150_5350, RTL819x_5GHZ_5470_5850)
-
-static const struct ieee80211_regdomain rtl_regdom_11 = {
-	.n_reg_rules = 1,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_12_13 = {
-	.n_reg_rules = 2,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_no_midband = {
-	.n_reg_rules = 3,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_5GHZ_5150_5350,
-			  RTL819x_5GHZ_5725_5850,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_60_64 = {
-	.n_reg_rules = 3,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-			  RTL819x_5GHZ_5725_5850,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_14_60_64 = {
-	.n_reg_rules = 4,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-			  RTL819x_2GHZ_CH14,
-			  RTL819x_5GHZ_5725_5850,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_14 = {
-	.n_reg_rules = 3,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-			  RTL819x_2GHZ_CH14,
-		      }
-};
-
-static bool _rtl_is_radar_freq(u16 center_freq)
-{
-	return center_freq >= 5260 && center_freq <= 5700;
-}
-
-static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
-					   enum nl80211_reg_initiator initiator)
-{
-	enum ieee80211_band band;
-	struct ieee80211_supported_band *sband;
-	const struct ieee80211_reg_rule *reg_rule;
-	struct ieee80211_channel *ch;
-	unsigned int i;
-
-	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
-		if (!wiphy->bands[band])
-			continue;
-
-		sband = wiphy->bands[band];
-
-		for (i = 0; i < sband->n_channels; i++) {
-			ch = &sband->channels[i];
-			if (_rtl_is_radar_freq(ch->center_freq) ||
-			    (ch->flags & IEEE80211_CHAN_RADAR))
-				continue;
-			if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
-				reg_rule = freq_reg_info(wiphy,
-							 ch->center_freq);
-				if (IS_ERR(reg_rule))
-					continue;
-
-				/*
-				 *If 11d had a rule for this channel ensure
-				 *we enable adhoc/beaconing if it allows us to
-				 *use it. Note that we would have disabled it
-				 *by applying our static world regdomain by
-				 *default during init, prior to calling our
-				 *regulatory_hint().
-				 */
-
-				if (!(reg_rule->flags & NL80211_RRF_NO_IBSS))
-					ch->flags &= ~IEEE80211_CHAN_NO_IBSS;
-				if (!(reg_rule->flags &
-				      NL80211_RRF_PASSIVE_SCAN))
-					ch->flags &=
-					    ~IEEE80211_CHAN_PASSIVE_SCAN;
-			} else {
-				if (ch->beacon_found)
-					ch->flags &= ~(IEEE80211_CHAN_NO_IBSS |
-						   IEEE80211_CHAN_PASSIVE_SCAN);
-			}
-		}
-	}
-}
-
-/* Allows active scan scan on Ch 12 and 13 */
-static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
-					     enum nl80211_reg_initiator
-					     initiator)
-{
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *ch;
-	const struct ieee80211_reg_rule *reg_rule;
-
-	if (!wiphy->bands[IEEE80211_BAND_2GHZ])
-		return;
-	sband = wiphy->bands[IEEE80211_BAND_2GHZ];
-
-	/*
-	 *If no country IE has been received always enable active scan
-	 *on these channels. This is only done for specific regulatory SKUs
-	 */
-	if (initiator != NL80211_REGDOM_SET_BY_COUNTRY_IE) {
-		ch = &sband->channels[11];	/* CH 12 */
-		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-		ch = &sband->channels[12];	/* CH 13 */
-		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-		return;
-	}
-
-	/*
-	 *If a country IE has been recieved check its rule for this
-	 *channel first before enabling active scan. The passive scan
-	 *would have been enforced by the initial processing of our
-	 *custom regulatory domain.
-	 */
-
-	ch = &sband->channels[11];	/* CH 12 */
-	reg_rule = freq_reg_info(wiphy, ch->center_freq);
-	if (!IS_ERR(reg_rule)) {
-		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
-			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-	}
-
-	ch = &sband->channels[12];	/* CH 13 */
-	reg_rule = freq_reg_info(wiphy, ch->center_freq);
-	if (!IS_ERR(reg_rule)) {
-		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
-			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-	}
-}
-
-/*
- *Always apply Radar/DFS rules on
- *freq range 5260 MHz - 5700 MHz
- */
-static void _rtl_reg_apply_radar_flags(struct wiphy *wiphy)
-{
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *ch;
-	unsigned int i;
-
-	if (!wiphy->bands[IEEE80211_BAND_5GHZ])
-		return;
-
-	sband = wiphy->bands[IEEE80211_BAND_5GHZ];
-
-	for (i = 0; i < sband->n_channels; i++) {
-		ch = &sband->channels[i];
-		if (!_rtl_is_radar_freq(ch->center_freq))
-			continue;
-
-		/*
-		 *We always enable radar detection/DFS on this
-		 *frequency range. Additionally we also apply on
-		 *this frequency range:
-		 *- If STA mode does not yet have DFS supports disable
-		 * active scanning
-		 *- If adhoc mode does not support DFS yet then disable
-		 * adhoc in the frequency.
-		 *- If AP mode does not yet support radar detection/DFS
-		 *do not allow AP mode
-		 */
-		if (!(ch->flags & IEEE80211_CHAN_DISABLED))
-			ch->flags |= IEEE80211_CHAN_RADAR |
-			    IEEE80211_CHAN_NO_IBSS |
-			    IEEE80211_CHAN_PASSIVE_SCAN;
-	}
-}
-
-static void _rtl_reg_apply_world_flags(struct wiphy *wiphy,
-				       enum nl80211_reg_initiator initiator,
-				       struct rtl_regulatory *reg)
-{
-	_rtl_reg_apply_beaconing_flags(wiphy, initiator);
-	_rtl_reg_apply_active_scan_flags(wiphy, initiator);
-	return;
-}
-
-static void _rtl_dump_channel_map(struct wiphy *wiphy)
-{
-	enum ieee80211_band band;
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *ch;
-	unsigned int i;
-
-	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
-		if (!wiphy->bands[band])
-			continue;
-		sband = wiphy->bands[band];
-		for (i = 0; i < sband->n_channels; i++)
-			ch = &sband->channels[i];
-	}
-}
-
-static int _rtl92e_reg_notifier_apply(struct wiphy *wiphy,
-				      struct regulatory_request *request,
-				      struct rtl_regulatory *reg)
-{
-	/* We always apply this */
-	_rtl_reg_apply_radar_flags(wiphy);
-
-	switch (request->initiator) {
-	case NL80211_REGDOM_SET_BY_DRIVER:
-	case NL80211_REGDOM_SET_BY_CORE:
-	case NL80211_REGDOM_SET_BY_USER:
-		break;
-	case NL80211_REGDOM_SET_BY_COUNTRY_IE:
-		_rtl_reg_apply_world_flags(wiphy, request->initiator, reg);
-		break;
-	}
-
-	_rtl_dump_channel_map(wiphy);
-
-	return 0;
-}
-
-static const struct ieee80211_regdomain *_rtl_regdomain_select(
-						struct rtl_regulatory *reg)
-{
-	switch (reg->country_code) {
-	case COUNTRY_CODE_FCC:
-		return &rtl_regdom_no_midband;
-	case COUNTRY_CODE_IC:
-		return &rtl_regdom_11;
-	case COUNTRY_CODE_ETSI:
-	case COUNTRY_CODE_TELEC_NETGEAR:
-		return &rtl_regdom_60_64;
-	case COUNTRY_CODE_SPAIN:
-	case COUNTRY_CODE_FRANCE:
-	case COUNTRY_CODE_ISRAEL:
-	case COUNTRY_CODE_WORLD_WIDE_13:
-		return &rtl_regdom_12_13;
-	case COUNTRY_CODE_MKK:
-	case COUNTRY_CODE_MKK1:
-	case COUNTRY_CODE_TELEC:
-	case COUNTRY_CODE_MIC:
-		return &rtl_regdom_14_60_64;
-	case COUNTRY_CODE_GLOBAL_DOMAIN:
-		return &rtl_regdom_14;
-	default:
-		return &rtl_regdom_no_midband;
-	}
-}
-
-static int _rtl92e_regd_init_wiphy(struct rtl_regulatory *reg,
-				   struct wiphy *wiphy,
-				   void (*reg_notifier)(struct wiphy *wiphy,
-						        struct regulatory_request *
-						        request))
-{
-	const struct ieee80211_regdomain *regd;
-
-	wiphy->reg_notifier = reg_notifier;
-
-	wiphy->regulatory_flags |= REGULATORY_CUSTOM_REG;
-	wiphy->regulatory_flags &= ~REGULATORY_STRICT_REG;
-	wiphy->regulatory_flags &= ~REGULATORY_DISABLE_BEACON_HINTS;
-
-	regd = _rtl_regdomain_select(reg);
-	wiphy_apply_custom_regulatory(wiphy, regd);
-	_rtl_reg_apply_radar_flags(wiphy);
-	_rtl_reg_apply_world_flags(wiphy, NL80211_REGDOM_SET_BY_DRIVER, reg);
-	return 0;
-}
-
-static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(allcountries); i++) {
-		if (allcountries[i].countrycode == countrycode)
-			return &allcountries[i];
-	}
-	return NULL;
-}
-
-int rtl92e_regd_init(struct ieee80211_hw *hw,
-		     void (*reg_notifier)(struct wiphy *wiphy,
-					  struct regulatory_request *request))
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct wiphy *wiphy = hw->wiphy;
-	struct country_code_to_enum_rd *country = NULL;
-
-	if (wiphy == NULL || &rtlpriv->regd == NULL)
-		return -EINVAL;
-
-	/* init country_code from efuse channel plan */
-	rtlpriv->regd.country_code = rtlpriv->efuse.channel_plan;
-
-	RT_TRACE(COMP_REGD, DBG_TRACE,
-		 (KERN_DEBUG "rtl: EEPROM regdomain: 0x%0x\n",
-		  rtlpriv->regd.country_code));
-
-	if (rtlpriv->regd.country_code >= COUNTRY_CODE_MAX) {
-		RT_TRACE(COMP_REGD, DBG_DMESG,
-			 ("rtl: EEPROM indicates invalid contry code world wide 13 should be used\n"));
-
-		rtlpriv->regd.country_code = COUNTRY_CODE_WORLD_WIDE_13;
-	}
-
-	country = _rtl_regd_find_country(rtlpriv->regd.country_code);
-
-	if (country) {
-		rtlpriv->regd.alpha2[0] = country->iso_name[0];
-		rtlpriv->regd.alpha2[1] = country->iso_name[1];
-	} else {
-		rtlpriv->regd.alpha2[0] = '0';
-		rtlpriv->regd.alpha2[1] = '0';
-	}
-
-	RT_TRACE(COMP_REGD, DBG_TRACE,
-		 (KERN_DEBUG "rtl: Country alpha2 being used: %c%c\n",
-		  rtlpriv->regd.alpha2[0], rtlpriv->regd.alpha2[1]));
-
-	_rtl92e_regd_init_wiphy(&rtlpriv->regd, wiphy, reg_notifier);
-
-	return 0;
-}
-
-void rtl92e_reg_notifier(struct wiphy *wiphy,
-			 struct regulatory_request *request)
-{
-	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_REGD, DBG_LOUD, ("\n"));
-
-	_rtl92e_reg_notifier_apply(wiphy, request, &rtlpriv->regd);
-}
diff --git a/drivers/staging/rtl8192ee/regd.h b/drivers/staging/rtl8192ee/regd.h
deleted file mode 100644
index 1f26f0e5261f..000000000000
--- a/drivers/staging/rtl8192ee/regd.h
+++ /dev/null
@@ -1,63 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_REGD_H__
-#define __RTL_REGD_H__
-
-/* for kernel 3.14 , both value are changed to IEEE80211_CHAN_NO_IR*/
-#define IEEE80211_CHAN_NO_IBSS IEEE80211_CHAN_NO_IR
-#define IEEE80211_CHAN_PASSIVE_SCAN IEEE80211_CHAN_NO_IR
-
-struct country_code_to_enum_rd {
-	u16 countrycode;
-	const char *iso_name;
-};
-
-enum country_code_type_t {
-	COUNTRY_CODE_FCC = 0,
-	COUNTRY_CODE_IC = 1,
-	COUNTRY_CODE_ETSI = 2,
-	COUNTRY_CODE_SPAIN = 3,
-	COUNTRY_CODE_FRANCE = 4,
-	COUNTRY_CODE_MKK = 5,
-	COUNTRY_CODE_MKK1 = 6,
-	COUNTRY_CODE_ISRAEL = 7,
-	COUNTRY_CODE_TELEC = 8,
-	COUNTRY_CODE_MIC = 9,
-	COUNTRY_CODE_GLOBAL_DOMAIN = 10,
-	COUNTRY_CODE_WORLD_WIDE_13 = 11,
-	COUNTRY_CODE_TELEC_NETGEAR = 12,
-
-	/*add new channel plan above this line */
-	COUNTRY_CODE_MAX
-};
-
-int rtl92e_regd_init(struct ieee80211_hw *hw,
-		     void (*reg_notifier)(struct wiphy *wiphy,
-					  struct regulatory_request *request));
-void rtl92e_reg_notifier(struct wiphy *wiphy,
-			 struct regulatory_request *request);
-
-#endif
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c
deleted file mode 100644
index efb00f622361..000000000000
--- a/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.c
+++ /dev/null
@@ -1,139 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "pwrseq.h"
-
-
-/*
-*	Description:
-*		This routine deal with the Power Configuration CMDs
-*		 parsing for RTL8723/RTL8188E Series IC.
-*	Assumption:
-*		We should follow specific format which was released from HW SD.
-*
-*	2011.07.07, added by Roger.
-*/
-bool rtl92e_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-			       u8 fab_version, u8 interface_type,
-			       struct wlan_pwr_cfg pwrcfgcmd[])
-
-{
-	struct wlan_pwr_cfg pwr_cfg_cmd = {0};
-	bool b_polling_bit = false;
-	u32 ary_idx = 0;
-	u8 value = 0;
-	u32 offset = 0;
-	u32 polling_count = 0;
-	u32 max_polling_cnt = 5000;
-
-	do {
-		pwr_cfg_cmd = pwrcfgcmd[ary_idx];
-		RT_TRACE(COMP_INIT, DBG_TRACE,
-			 ("offset(%#x), cut_msk(%#x), fab_msk(%#x), interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
-			  GET_PWR_CFG_OFFSET(pwr_cfg_cmd),
-			  GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd),
-			  GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd),
-			  GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd),
-			  GET_PWR_CFG_BASE(pwr_cfg_cmd),
-			  GET_PWR_CFG_CMD(pwr_cfg_cmd),
-			  GET_PWR_CFG_MASK(pwr_cfg_cmd),
-			  GET_PWR_CFG_VALUE(pwr_cfg_cmd)));
-
-		if ((GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd)&fab_version) &&
-		    (GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd)&cut_version) &&
-		    (GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd)&interface_type)) {
-			switch (GET_PWR_CFG_CMD(pwr_cfg_cmd)) {
-			case PWR_CMD_READ:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					 ("PWR_CMD_READ\n"));
-				break;
-
-			case PWR_CMD_WRITE:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					 ("PWR_CMD_WRITE\n"));
-				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
-
-				/*Read the value from system register*/
-				value = rtl_read_byte(rtlpriv, offset);
-				value &= (~(GET_PWR_CFG_MASK(pwr_cfg_cmd)));
-				value |= (GET_PWR_CFG_VALUE(pwr_cfg_cmd) &
-					  GET_PWR_CFG_MASK(pwr_cfg_cmd));
-
-				/*Write value back to sytem register*/
-				rtl_write_byte(rtlpriv, offset, value);
-				break;
-
-			case PWR_CMD_POLLING:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					 ("PWR_CMD_POLLING\n"));
-				b_polling_bit = false;
-				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
-
-				do {
-					value = rtl_read_byte(rtlpriv, offset);
-
-					value &= GET_PWR_CFG_MASK(pwr_cfg_cmd);
-					if (value ==
-					    (GET_PWR_CFG_VALUE(pwr_cfg_cmd) &
-					     GET_PWR_CFG_MASK(pwr_cfg_cmd)))
-						b_polling_bit = true;
-					else
-						udelay(10);
-
-					if (polling_count++ > max_polling_cnt) {
-						RT_TRACE(COMP_INIT, DBG_LOUD,
-							 ("polling fail\n"));
-						return false;
-					}
-				} while (!b_polling_bit);
-
-				break;
-
-			case PWR_CMD_DELAY:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					 ("PWR_CMD_DELAY\n"));
-				if (GET_PWR_CFG_VALUE(pwr_cfg_cmd) ==
-				    PWRSEQ_DELAY_US)
-					udelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
-				else
-					mdelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
-				break;
-
-			case PWR_CMD_END:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					 ("PWR_CMD_END\n"));
-				return true;
-				break;
-
-			default:
-				RT_ASSERT(false, ("Unknown CMD!!\n"));
-				break;
-			}
-		}
-
-		ary_idx++;
-	} while (1);
-	return true;
-}
diff --git a/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.h b/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.h
deleted file mode 100644
index ec40ea8b8c26..000000000000
--- a/drivers/staging/rtl8192ee/rtl8192ee/pwrseqcmd.h
+++ /dev/null
@@ -1,69 +0,0 @@
-#ifndef __RTL92E_PWRSEQCMD_H__
-#define __RTL92E_PWRSEQCMD_H__
-
-#include "../wifi.h"
-/*---------------------------------------------*/
-/* The value of cmd: 4 bits */
-/*---------------------------------------------*/
-#define	PWR_CMD_READ		0x00
-#define PWR_CMD_WRITE		0x01
-#define PWR_CMD_POLLING		0x02
-#define PWR_CMD_DELAY		0x03
-#define PWR_CMD_END		0x04
-
-/* define the base address of each block */
-#define PWR_BASEADDR_MAC	0x00
-#define PWR_BASEADDR_USB	0x01
-#define PWR_BASEADDR_PCIE	0x02
-#define PWR_BASEADDR_SDIO	0x03
-
-#define	PWR_INTF_SDIO_MSK	BIT(0)
-#define	PWR_INTF_USB_MSK	BIT(1)
-#define	PWR_INTF_PCI_MSK	BIT(2)
-#define	PWR_INTF_ALL_MSK	(BIT(0) | BIT(1) | BIT(2) | BIT(3))
-
-#define	PWR_FAB_TSMC_MSK	BIT(0)
-#define	PWR_FAB_UMC_MSK		BIT(1)
-#define	PWR_FAB_ALL_MSK		(BIT(0) | BIT(1) | BIT(2) | BIT(3))
-
-#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
-#define	PWR_CUT_A_MSK		BIT(1)
-#define	PWR_CUT_B_MSK		BIT(2)
-#define	PWR_CUT_C_MSK		BIT(3)
-#define	PWR_CUT_D_MSK		BIT(4)
-#define	PWR_CUT_E_MSK		BIT(5)
-#define	PWR_CUT_F_MSK		BIT(6)
-#define	PWR_CUT_G_MSK		BIT(7)
-#define	PWR_CUT_ALL_MSK		0xFF
-
-
-enum pwrseq_delay_unit {
-	PWRSEQ_DELAY_US,
-	PWRSEQ_DELAY_MS,
-};
-
-struct wlan_pwr_cfg {
-	u16 offset;
-	u8 cut_msk;
-	u8 fab_msk:4;
-	u8 interface_msk:4;
-	u8 base:4;
-	u8 cmd:4;
-	u8 msk;
-	u8 value;
-};
-
-#define	GET_PWR_CFG_OFFSET(__PWR_CMD)		__PWR_CMD.offset
-#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)		__PWR_CMD.cut_msk
-#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)		__PWR_CMD.fab_msk
-#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
-#define	GET_PWR_CFG_BASE(__PWR_CMD)		__PWR_CMD.base
-#define	GET_PWR_CFG_CMD(__PWR_CMD)		__PWR_CMD.cmd
-#define	GET_PWR_CFG_MASK(__PWR_CMD)		__PWR_CMD.msk
-#define	GET_PWR_CFG_VALUE(__PWR_CMD)		__PWR_CMD.value
-
-bool rtl92e_hal_pwrseqcmdparsing(struct rtl_priv *rtlpriv, u8 cut_version,
-			      u8 fab_version, u8 interface_type,
-			      struct wlan_pwr_cfg pwrcfgcmd[]);
-
-#endif
diff --git a/drivers/staging/rtl8192ee/stats.c b/drivers/staging/rtl8192ee/stats.c
deleted file mode 100644
index 7ac302b1a71f..000000000000
--- a/drivers/staging/rtl8192ee/stats.c
+++ /dev/null
@@ -1,290 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include "wifi.h"
-#include "stats.h"
-
-u8 stg_rtl_query_rxpwrpercentage(char antpower)
-{
-	if ((antpower <= -100) || (antpower >= 20))
-		return 0;
-	else if (antpower >= 0)
-		return 100;
-	else
-		return 100 + antpower;
-}
-EXPORT_SYMBOL(stg_rtl_query_rxpwrpercentage);
-
-u8 stg_rtl_evm_db_to_percentage(char value)
-{
-	char ret_val;
-	ret_val = value;
-
-	if (ret_val >= 0)
-		ret_val = 0;
-	if (ret_val <= -33)
-		ret_val = -33;
-	ret_val = 0 - ret_val;
-	ret_val *= 3;
-	if (ret_val == 99)
-		ret_val = 100;
-
-	return ret_val;
-}
-EXPORT_SYMBOL(stg_rtl_evm_db_to_percentage);
-
-u8 rtl_evm_dbm_jaguar(char value)
-{
-	char ret_val;
-	ret_val = value;
-
-	/* -33dB~0dB to 33dB ~ 0dB*/
-	if (ret_val == -128)
-		ret_val = 127;
-	else if (ret_val < 0)
-		ret_val = 0 - ret_val;
-
-	ret_val  = ret_val >> 1;
-	return ret_val;
-}
-EXPORT_SYMBOL(rtl_evm_dbm_jaguar);
-
-static long rtl_translate_todbm(struct ieee80211_hw *hw,
-				u8 signal_strength_index)
-{
-	long signal_power;
-
-	signal_power = (long)((signal_strength_index + 1) >> 1);
-	signal_power -= 95;
-	return signal_power;
-}
-
-long stg_rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig)
-{
-	long retsig;
-
-	if (currsig >= 61 && currsig <= 100)
-		retsig = 90 + ((currsig - 60) / 4);
-	else if (currsig >= 41 && currsig <= 60)
-		retsig = 78 + ((currsig - 40) / 2);
-	else if (currsig >= 31 && currsig <= 40)
-		retsig = 66 + (currsig - 30);
-	else if (currsig >= 21 && currsig <= 30)
-		retsig = 54 + (currsig - 20);
-	else if (currsig >= 5 && currsig <= 20)
-		retsig = 42 + (((currsig - 5) * 2) / 3);
-	else if (currsig == 4)
-		retsig = 36;
-	else if (currsig == 3)
-		retsig = 27;
-	else if (currsig == 2)
-		retsig = 18;
-	else if (currsig == 1)
-		retsig = 9;
-	else
-		retsig = currsig;
-
-	return retsig;
-}
-EXPORT_SYMBOL(stg_rtl_signal_scale_mapping);
-
-static void rtl_process_ui_rssi(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 rfpath;
-	u32 last_rssi, tmpval;
-
-	if (!pstatus->b_packet_toself && !pstatus->b_packet_beacon)
-		return;
-
-	rtlpriv->stats.pwdb_all_cnt += pstatus->rx_pwdb_all;
-	rtlpriv->stats.rssi_calculate_cnt++;
-
-	if (rtlpriv->stats.ui_rssi.total_num++ >= PHY_RSSI_SLID_WIN_MAX) {
-		rtlpriv->stats.ui_rssi.total_num = PHY_RSSI_SLID_WIN_MAX;
-		last_rssi = rtlpriv->stats.ui_rssi.elements[
-			rtlpriv->stats.ui_rssi.index];
-		rtlpriv->stats.ui_rssi.total_val -= last_rssi;
-	}
-	rtlpriv->stats.ui_rssi.total_val += pstatus->signalstrength;
-	rtlpriv->stats.ui_rssi.elements[rtlpriv->stats.ui_rssi.index++] =
-	    pstatus->signalstrength;
-	if (rtlpriv->stats.ui_rssi.index >= PHY_RSSI_SLID_WIN_MAX)
-		rtlpriv->stats.ui_rssi.index = 0;
-	tmpval = rtlpriv->stats.ui_rssi.total_val /
-		rtlpriv->stats.ui_rssi.total_num;
-	rtlpriv->stats.signal_strength = rtl_translate_todbm(hw,
-		(u8) tmpval);
-	pstatus->rssi = rtlpriv->stats.signal_strength;
-
-	if (pstatus->b_is_cck)
-		return;
-
-	for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
-	     rfpath++) {
-		if (rtlpriv->stats.rx_rssi_percentage[rfpath] == 0) {
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    pstatus->rx_mimo_signalstrength[rfpath];
-		}
-		if (pstatus->rx_mimo_signalstrength[rfpath] >
-		    rtlpriv->stats.rx_rssi_percentage[rfpath]) {
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
-			      (RX_SMOOTH_FACTOR - 1)) +
-			     (pstatus->rx_mimo_signalstrength[rfpath])) /
-			    (RX_SMOOTH_FACTOR);
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    rtlpriv->stats.rx_rssi_percentage[rfpath] + 1;
-		} else {
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
-			      (RX_SMOOTH_FACTOR - 1)) +
-			     (pstatus->rx_mimo_signalstrength[rfpath])) /
-			    (RX_SMOOTH_FACTOR);
-		}
-		rtlpriv->stats.rx_snr_db[rfpath] = pstatus->rx_snr[rfpath];
-		rtlpriv->stats.rx_evm_dbm[rfpath] =
-					pstatus->rx_mimo_evm_dbm[rfpath];
-		rtlpriv->stats.rx_cfo_short[rfpath] =
-					pstatus->cfo_short[rfpath];
-		rtlpriv->stats.rx_cfo_tail[rfpath] = pstatus->cfo_tail[rfpath];
-	}
-}
-
-static void rtl_update_rxsignalstatistics(struct ieee80211_hw *hw,
-					  struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int weighting = 0;
-
-	if (rtlpriv->stats.recv_signal_power == 0)
-		rtlpriv->stats.recv_signal_power = pstatus->recvsignalpower;
-	if (pstatus->recvsignalpower > rtlpriv->stats.recv_signal_power)
-		weighting = 5;
-	else if (pstatus->recvsignalpower < rtlpriv->stats.recv_signal_power)
-		weighting = (-5);
-	rtlpriv->stats.recv_signal_power = (rtlpriv->stats.recv_signal_power *
-		5 + pstatus->recvsignalpower + weighting) / 6;
-}
-
-static void rtl_process_pwdb(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *drv_priv = NULL;
-	struct ieee80211_sta *sta = NULL;
-	long undecorated_smoothed_pwdb;
-
-	rcu_read_lock();
-	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
-		sta = rtl_find_sta(hw, pstatus->psaddr);
-
-	/* adhoc or ap mode */
-	if (sta) {
-		drv_priv = (struct rtl_sta_info *)sta->drv_priv;
-		undecorated_smoothed_pwdb =
-			drv_priv->rssi_stat.undecorated_smoothed_pwdb;
-	} else {
-		undecorated_smoothed_pwdb =
-			rtlpriv->dm.undecorated_smoothed_pwdb;
-	}
-
-	if (undecorated_smoothed_pwdb < 0)
-		undecorated_smoothed_pwdb = pstatus->rx_pwdb_all;
-	if (pstatus->rx_pwdb_all > (u32) undecorated_smoothed_pwdb) {
-		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
-		      (RX_SMOOTH_FACTOR - 1)) +
-		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-		undecorated_smoothed_pwdb = undecorated_smoothed_pwdb + 1;
-	} else {
-		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
-		      (RX_SMOOTH_FACTOR - 1)) +
-		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-	}
-
-	if (sta) {
-		drv_priv->rssi_stat.undecorated_smoothed_pwdb =
-			undecorated_smoothed_pwdb;
-	} else {
-		rtlpriv->dm.undecorated_smoothed_pwdb = undecorated_smoothed_pwdb;
-	}
-	rcu_read_unlock();
-
-	rtl_update_rxsignalstatistics(hw, pstatus);
-}
-
-static void rtl_process_ui_link_quality(struct ieee80211_hw *hw,
-					struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 last_evm, n_stream, tmpval;
-
-	if (pstatus->signalquality == 0)
-		return;
-
-	if (rtlpriv->stats.ui_link_quality.total_num++ >=
-	    PHY_LINKQUALITY_SLID_WIN_MAX) {
-		rtlpriv->stats.ui_link_quality.total_num =
-		    PHY_LINKQUALITY_SLID_WIN_MAX;
-		last_evm = rtlpriv->stats.ui_link_quality.elements[
-			rtlpriv->stats.ui_link_quality.index];
-		rtlpriv->stats.ui_link_quality.total_val -= last_evm;
-	}
-	rtlpriv->stats.ui_link_quality.total_val += pstatus->signalquality;
-	rtlpriv->stats.ui_link_quality.elements[
-		rtlpriv->stats.ui_link_quality.index++] =
-							pstatus->signalquality;
-	if (rtlpriv->stats.ui_link_quality.index >=
-	    PHY_LINKQUALITY_SLID_WIN_MAX)
-		rtlpriv->stats.ui_link_quality.index = 0;
-	tmpval = rtlpriv->stats.ui_link_quality.total_val /
-	    rtlpriv->stats.ui_link_quality.total_num;
-	rtlpriv->stats.signal_quality = tmpval;
-	rtlpriv->stats.last_sigstrength_inpercent = tmpval;
-	for (n_stream = 0; n_stream < 2; n_stream++) {
-		if (pstatus->rx_mimo_signalquality[n_stream] != -1) {
-			if (rtlpriv->stats.rx_evm_percentage[n_stream] == 0) {
-				rtlpriv->stats.rx_evm_percentage[n_stream] =
-				    pstatus->rx_mimo_signalquality[n_stream];
-			}
-			rtlpriv->stats.rx_evm_percentage[n_stream] =
-			    ((rtlpriv->stats.rx_evm_percentage[n_stream]
-			      * (RX_SMOOTH_FACTOR - 1)) +
-			     (pstatus->rx_mimo_signalquality[n_stream] * 1)) /
-			    (RX_SMOOTH_FACTOR);
-		}
-	}
-}
-
-void stg_rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
-			     struct rtl_stats *pstatus)
-{
-	if (!pstatus->b_packet_matchbssid)
-		return;
-
-	rtl_process_ui_rssi(hw, pstatus);
-	rtl_process_pwdb(hw, pstatus);
-	rtl_process_ui_link_quality(hw, pstatus);
-}
-EXPORT_SYMBOL(stg_rtl_process_phyinfo);
diff --git a/drivers/staging/rtl8192ee/stats.h b/drivers/staging/rtl8192ee/stats.h
deleted file mode 100644
index 072842785308..000000000000
--- a/drivers/staging/rtl8192ee/stats.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_STATS_H__
-#define __RTL_STATS_H__
-
-#define	PHY_RSSI_SLID_WIN_MAX			100
-#define	PHY_LINKQUALITY_SLID_WIN_MAX		20
-#define	PHY_BEACON_RSSI_SLID_WIN_MAX		10
-
-/* Rx smooth factor */
-#define	RX_SMOOTH_FACTOR			20
-
-u8 stg_rtl_query_rxpwrpercentage(char antpower);
-u8 stg_rtl_evm_db_to_percentage(char value);
-u8 rtl_evm_dbm_jaguar(char value);
-long stg_rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig);
-void stg_rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
-			     struct rtl_stats *pstatus);
-
-#endif
diff --git a/drivers/staging/rtl8192ee/wifi.h b/drivers/staging/rtl8192ee/wifi.h
deleted file mode 100644
index a37176af845e..000000000000
--- a/drivers/staging/rtl8192ee/wifi.h
+++ /dev/null
@@ -1,2644 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_WIFI_H__
-#define __RTL_WIFI_H__
-
-#include <linux/interrupt.h>
-#include <linux/sched.h>
-#include <linux/firmware.h>
-#include <linux/etherdevice.h>
-#include <net/mac80211.h>
-#include "debug.h"
-
-
-#define RF_CHANGE_BY_INIT		0
-#define RF_CHANGE_BY_IPS		BIT(28)
-#define RF_CHANGE_BY_PS			BIT(29)
-#define RF_CHANGE_BY_HW			BIT(30)
-#define RF_CHANGE_BY_SW			BIT(31)
-
-#define IQK_ADDA_REG_NUM		16
-#define IQK_MAC_REG_NUM			4
-#define IQK_THRESHOLD			8
-
-#define MAX_KEY_LEN			61
-#define KEY_BUF_SIZE			5
-
-/* QoS related. */
-/*aci: 0x00	Best Effort*/
-/*aci: 0x01	Background*/
-/*aci: 0x10	Video*/
-/*aci: 0x11	Voice*/
-/*Max: define total number.*/
-#define AC0_BE				0
-#define AC1_BK				1
-#define AC2_VI				2
-#define AC3_VO				3
-#define AC_MAX				4
-#define QOS_QUEUE_NUM			4
-#define RTL_MAC80211_NUM_QUEUE		5
-
-#define QBSS_LOAD_SIZE			5
-#define MAX_WMMELE_LENGTH		64
-
-#define TOTAL_CAM_ENTRY			32
-
-/*slot time for 11g. */
-#define RTL_SLOT_TIME_9			9
-#define RTL_SLOT_TIME_20		20
-
-/*related with tcp/ip. */
-/*if_ehther.h*/
-#define ETH_P_PAE			0x888E	/*Port Access Entity
-						 *(IEEE 802.1X) */
-#define ETH_P_IP			0x0800	/*Internet Protocol packet */
-#define ETH_P_ARP			0x0806	/*Address Resolution packet */
-#define SNAP_SIZE			6
-#define PROTOC_TYPE_SIZE		2
-
-/*related with 802.11 frame*/
-#define MAC80211_3ADDR_LEN		24
-#define MAC80211_4ADDR_LEN		30
-
-#define CHANNEL_MAX_NUMBER		(14 + 24 + 21)	/* 14 is the max
-							 * channel number */
-#define CHANNEL_MAX_NUMBER_2G		14
-#define CHANNEL_MAX_NUMBER_5G		54 /* Please refer to
-					    *"phy_GetChnlGroup8812A" and
-					    * "Hal_ReadTxPowerInfo8812A"*/
-
-#define MAX_REGULATION_NUM			4
-#define MAX_RF_PATH_NUM	2
-#define MAX_RATE_SECTION_NUM		6
-#define MAX_2_4G_BANDWITH_NUM		2
-#define MAX_5G_BANDWITH_NUM		4
-
-
-
-
-#define CHANNEL_MAX_NUMBER_5G_80M	7
-#define CHANNEL_GROUP_MAX		(3 + 9)	/* ch1~3, ch4~9, ch10~14
-						 * total three groups */
-#define MAX_PG_GROUP			13
-#define	CHANNEL_GROUP_MAX_2G		3
-#define	CHANNEL_GROUP_IDX_5GL		3
-#define	CHANNEL_GROUP_IDX_5GM		6
-#define	CHANNEL_GROUP_IDX_5GH		9
-#define	CHANNEL_GROUP_MAX_5G		9
-#define CHANNEL_MAX_NUMBER_2G		14
-#define AVG_THERMAL_NUM			8
-#define AVG_THERMAL_NUM_92E		4
-#define AVG_THERMAL_NUM_88E		4
-#define AVG_THERMAL_NUM_8723BE		4
-#define MAX_TID_COUNT			9
-#define MAX_NUM_RATES			264
-
-/*for 88E use*/
-/*It must always set to 4, otherwise read efuse table secquence will be wrong.*/
-#define MAX_TX_COUNT			4
-#define	MAX_RF_PATH			4
-#define	MAX_CHNL_GROUP_24G		6
-#define	MAX_CHNL_GROUP_5G		14
-
-/* BK, BE, VI, VO, HCCA, MANAGEMENT, COMMAND, HIGH, BEACON. */
-#define MAX_TX_QUEUE			9
-
-#define TX_PWR_BY_RATE_NUM_BAND		2
-#define TX_PWR_BY_RATE_NUM_RF		4
-#define TX_PWR_BY_RATE_NUM_SECTION	12
-#define MAX_BASE_NUM_IN_PHY_REG_PG_24G  6
-#define MAX_BASE_NUM_IN_PHY_REG_PG_5G	5
-
-#define DELTA_SWINGIDX_SIZE	30
-#define BAND_NUM				3
-/*Now, it's just for 8192ee
- *not OK yet, keep it 0*/
-#define DMA_IS_64BIT 0
-#define RTL8192EE_SEG_NUM		1 /* 0:2 seg, 1: 4 seg, 2: 8 seg */
-
-
-#define PACKET_NORMAL			0
-#define PACKET_DHCP			1
-#define PACKET_ARP			2
-#define PACKET_EAPOL			3
-
-#define	MAX_SUPPORT_WOL_PATTERN_NUM	16
-#define	RSVD_WOL_PATTERN_NUM		1
-#define	WKFMCAM_ADDR_NUM		6
-#define	WKFMCAM_SIZE			24
-
-#define	MAX_WOL_BIT_MASK_SIZE		16
-/* MIN LEN keeps 13 here */
-#define	MIN_WOL_PATTERN_SIZE		13
-#define	MAX_WOL_PATTERN_SIZE		128
-
-#define	WAKE_ON_MAGIC_PACKET		BIT(0)
-#define	WAKE_ON_PATTERN_MATCH		BIT(1)
-
-#define	WOL_REASON_PTK_UPDATE		BIT(0)
-#define	WOL_REASON_GTK_UPDATE		BIT(1)
-#define	WOL_REASON_DISASSOC		BIT(2)
-#define	WOL_REASON_DEAUTH		BIT(3)
-#define	WOL_REASON_AP_LOST		BIT(4)
-#define	WOL_REASON_MAGIC_PKT		BIT(5)
-#define	WOL_REASON_UNICAST_PKT		BIT(6)
-#define	WOL_REASON_PATTERN_PKT		BIT(7)
-#define	WOL_REASON_RTD3_SSID_MATCH	BIT(8)
-#define	WOL_REASON_REALWOW_V2_WAKEUPPKT	BIT(9)
-#define	WOL_REASON_REALWOW_V2_ACKLOST	BIT(10)
-
-struct txpower_info_2g {
-	u8 index_cck_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
-	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
-	/*If only one tx, only BW20 and OFDM are used.*/
-	u8 cck_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
-};
-
-struct txpower_info_5g {
-	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_5G];
-	/*If only one tx, only BW20, OFDM, BW80 and BW160 are used.*/
-	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw80_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw160_diff[MAX_RF_PATH][MAX_TX_COUNT];
-};
-
-
-/* for early mode */
-#define EM_HDR_LEN			8
-#define FCS_LEN				4
-
-#define MAX_VIRTUAL_MAC			1
-
-enum rf_tx_num {
-	RF_1TX = 0,
-	RF_2TX,
-	RF_MAX_TX_NUM,
-	RF_TX_NUM_NONIMPLEMENT,
-};
-
-enum rate_section {
-	CCK = 0,
-	OFDM,
-	HT_MCS0_MCS7,
-	HT_MCS8_MCS15,
-	VHT_1SSMCS0_1SSMCS9,
-	VHT_2SSMCS0_2SSMCS9,
-};
-
-enum intf_type {
-	INTF_PCI = 0,
-	INTF_USB = 1,
-};
-
-enum radio_path {
-	RF90_PATH_A = 0,
-	RF90_PATH_B = 1,
-	RF90_PATH_C = 2,
-	RF90_PATH_D = 3,
-};
-
-enum regulation_txpwr_lmt {
-	TXPWR_LMT_FCC = 0,
-	TXPWR_LMT_MKK = 1,
-	TXPWR_LMT_ETSI = 2,
-	TXPWR_LMT_WW = 3,
-
-	TXPWR_LMT_MAX_REGULATION_NUM = 4
-};
-
-
-enum rt_eeprom_type {
-	EEPROM_93C46,
-	EEPROM_93C56,
-	EEPROM_BOOT_EFUSE,
-};
-
-enum rtl_status {
-	RTL_STATUS_INTERFACE_START = 0,
-};
-
-enum hardware_type {
-	HARDWARE_TYPE_RTL8192E,
-	HARDWARE_TYPE_RTL8192U,
-	HARDWARE_TYPE_RTL8192SE,
-	HARDWARE_TYPE_RTL8192SU,
-	HARDWARE_TYPE_RTL8192CE,
-	HARDWARE_TYPE_RTL8192CU,
-	HARDWARE_TYPE_RTL8192DE,
-	HARDWARE_TYPE_RTL8192DU,
-	HARDWARE_TYPE_RTL8723AE,
-	HARDWARE_TYPE_RTL8188EE,
-	HARDWARE_TYPE_RTL8723BE,
-	HARDWARE_TYPE_RTL8192EE,
-	HARDWARE_TYPE_RTL8821AE,
-	HARDWARE_TYPE_RTL8812AE,
-	/* keep it last */
-	HARDWARE_TYPE_NUM
-};
-
-enum scan_operation_backup_opt {
-	SCAN_OPT_BACKUP_BAND0 = 0,
-	SCAN_OPT_BACKUP_BAND1,
-	SCAN_OPT_RESTORE,
-	SCAN_OPT_MAX
-};
-
-/*RF state.*/
-enum rf_pwrstate {
-	ERFON,
-	ERFSLEEP,
-	ERFOFF
-};
-
-struct bb_reg_def {
-	u32 rfintfs;
-	u32 rfintfi;
-	u32 rfintfo;
-	u32 rfintfe;
-	u32 rf3wire_offset;
-	u32 rflssi_select;
-	u32 rftxgain_stage;
-	u32 rfhssi_para1;
-	u32 rfhssi_para2;
-	u32 rfswitch_control;
-	u32 rfagc_control1;
-	u32 rfagc_control2;
-	u32 rfrxiq_imbalance;
-	u32 rfrx_afe;
-	u32 rftxiq_imbalance;
-	u32 rftx_afe;
-	u32 rflssi_readback;
-	u32 rflssi_readbackpi;
-};
-
-enum io_type {
-	IO_CMD_PAUSE_BAND0_DM_BY_SCAN = 0,
-	IO_CMD_PAUSE_BAND1_DM_BY_SCAN = 1,
-	IO_CMD_RESUME_DM_BY_SCAN = 2,
-};
-
-enum hw_variables {
-	HW_VAR_ETHER_ADDR,
-	HW_VAR_MULTICAST_REG,
-	HW_VAR_BASIC_RATE,
-	HW_VAR_BSSID,
-	HW_VAR_MEDIA_STATUS,
-	HW_VAR_SECURITY_CONF,
-	HW_VAR_BEACON_INTERVAL,
-	HW_VAR_ATIM_WINDOW,
-	HW_VAR_LISTEN_INTERVAL,
-	HW_VAR_CS_COUNTER,
-	HW_VAR_DEFAULTKEY0,
-	HW_VAR_DEFAULTKEY1,
-	HW_VAR_DEFAULTKEY2,
-	HW_VAR_DEFAULTKEY3,
-	HW_VAR_SIFS,
-	HW_VAR_R2T_SIFS,
-	HW_VAR_DIFS,
-	HW_VAR_EIFS,
-	HW_VAR_SLOT_TIME,
-	HW_VAR_ACK_PREAMBLE,
-	HW_VAR_CW_CONFIG,
-	HW_VAR_CW_VALUES,
-	HW_VAR_RATE_FALLBACK_CONTROL,
-	HW_VAR_CONTENTION_WINDOW,
-	HW_VAR_RETRY_COUNT,
-	HW_VAR_TR_SWITCH,
-	HW_VAR_COMMAND,
-	HW_VAR_WPA_CONFIG,
-	HW_VAR_AMPDU_MIN_SPACE,
-	HW_VAR_SHORTGI_DENSITY,
-	HW_VAR_AMPDU_FACTOR,
-	HW_VAR_MCS_RATE_AVAILABLE,
-	HW_VAR_AC_PARAM,
-	HW_VAR_ACM_CTRL,
-	HW_VAR_DIS_Req_Qsize,
-	HW_VAR_CCX_CHNL_LOAD,
-	HW_VAR_CCX_NOISE_HISTOGRAM,
-	HW_VAR_CCX_CLM_NHM,
-	HW_VAR_TxOPLimit,
-	HW_VAR_TURBO_MODE,
-	HW_VAR_RF_STATE,
-	HW_VAR_RF_OFF_BY_HW,
-	HW_VAR_BUS_SPEED,
-	HW_VAR_SET_DEV_POWER,
-
-	HW_VAR_RCR,
-	HW_VAR_RATR_0,
-	HW_VAR_RRSR,
-	HW_VAR_CPU_RST,
-	HW_VAR_CECHK_BSSID,
-	HW_VAR_LBK_MODE,
-	HW_VAR_AES_11N_FIX,
-	HW_VAR_USB_RX_AGGR,
-	HW_VAR_USER_CONTROL_TURBO_MODE,
-	HW_VAR_RETRY_LIMIT,
-	HW_VAR_INIT_TX_RATE,
-	HW_VAR_TX_RATE_REG,
-	HW_VAR_EFUSE_USAGE,
-	HW_VAR_EFUSE_BYTES,
-	HW_VAR_AUTOLOAD_STATUS,
-	HW_VAR_RF_2R_DISABLE,
-	HW_VAR_SET_RPWM,
-	HW_VAR_H2C_FW_PWRMODE,
-	HW_VAR_H2C_FW_JOINBSSRPT,
-	HW_VAR_H2C_FW_MEDIASTATUSRPT,
-	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-	HW_VAR_FW_PSMODE_STATUS,
-	HW_VAR_INIT_RTS_RATE,
-	HW_VAR_RESUME_CLK_ON,
-	HW_VAR_FW_LPS_ACTION,
-	HW_VAR_1X1_RECV_COMBINE,
-	HW_VAR_STOP_SEND_BEACON,
-	HW_VAR_TSF_TIMER,
-	HW_VAR_IO_CMD,
-
-	HW_VAR_RF_RECOVERY,
-	HW_VAR_H2C_FW_UPDATE_GTK,
-	HW_VAR_WF_MASK,
-	HW_VAR_WF_CRC,
-	HW_VAR_WF_IS_MAC_ADDR,
-	HW_VAR_H2C_FW_OFFLOAD,
-	HW_VAR_RESET_WFCRC,
-
-	HW_VAR_HANDLE_FW_C2H,
-	HW_VAR_DL_FW_RSVD_PAGE,
-	HW_VAR_AID,
-	HW_VAR_HW_SEQ_ENABLE,
-	HW_VAR_CORRECT_TSF,
-	HW_VAR_BCN_VALID,
-	HW_VAR_FWLPS_RF_ON,
-	HW_VAR_DUAL_TSF_RST,
-	HW_VAR_SWITCH_EPHY_WoWLAN,
-	HW_VAR_INT_MIGRATION,
-	HW_VAR_INT_AC,
-	HW_VAR_RF_TIMING,
-
-	HAL_DEF_WOWLAN,
-	HW_VAR_MRC,
-	HW_VAR_KEEP_ALIVE,
-	HW_VAR_NAV_UPPER,
-};
-
-enum rt_media_status {
-	RT_MEDIA_DISCONNECT = 0,
-	RT_MEDIA_CONNECT = 1
-};
-
-enum rt_oem_id {
-	RT_CID_DEFAULT = 0,
-	RT_CID_8187_ALPHA0 = 1,
-	RT_CID_8187_SERCOMM_PS = 2,
-	RT_CID_8187_HW_LED = 3,
-	RT_CID_8187_NETGEAR = 4,
-	RT_CID_WHQL = 5,
-	RT_CID_819x_CAMEO = 6,
-	RT_CID_819x_RUNTOP = 7,
-	RT_CID_819x_Senao = 8,
-	RT_CID_TOSHIBA = 9,
-	RT_CID_819x_Netcore = 10,
-	RT_CID_Nettronix = 11,
-	RT_CID_DLINK = 12,
-	RT_CID_PRONET = 13,
-	RT_CID_COREGA = 14,
-	RT_CID_819x_ALPHA = 15,
-	RT_CID_819x_Sitecom = 16,
-	RT_CID_CCX = 17,
-	RT_CID_819x_Lenovo = 18,
-	RT_CID_819x_QMI = 19,
-	RT_CID_819x_Edimax_Belkin = 20,
-	RT_CID_819x_Sercomm_Belkin = 21,
-	RT_CID_819x_CAMEO1 = 22,
-	RT_CID_819x_MSI = 23,
-	RT_CID_819x_Acer = 24,
-	RT_CID_819x_HP = 27,
-	RT_CID_819x_CLEVO = 28,
-	RT_CID_819x_Arcadyan_Belkin = 29,
-	RT_CID_819x_SAMSUNG = 30,
-	RT_CID_819x_WNC_COREGA = 31,
-	RT_CID_819x_Foxcoon = 32,
-	RT_CID_819x_DELL = 33,
-	RT_CID_819x_PRONETS = 34,
-	RT_CID_819x_Edimax_ASUS = 35,
-	RT_CID_NETGEAR = 36,
-	RT_CID_PLANEX = 37,
-	RT_CID_CC_C = 38,
-};
-
-enum hw_descs {
-	HW_DESC_OWN,
-	HW_DESC_RXOWN,
-	HW_DESC_TX_NEXTDESC_ADDR,
-	HW_DESC_TXBUFF_ADDR,
-	HW_DESC_RXBUFF_ADDR,
-	HW_DESC_RXPKT_LEN,
-	HW_DESC_RXERO,
-	HW_DESC_RX_PREPARE,
-};
-
-enum prime_sc {
-	PRIME_CHNL_OFFSET_DONT_CARE = 0,
-	PRIME_CHNL_OFFSET_LOWER = 1,
-	PRIME_CHNL_OFFSET_UPPER = 2,
-};
-
-enum rf_type {
-	RF_1T1R = 0,
-	RF_1T2R = 1,
-	RF_2T2R = 2,
-	RF_2T2R_GREEN = 3,
-};
-
-enum ht_channel_width {
-	HT_CHANNEL_WIDTH_20 = 0,
-	HT_CHANNEL_WIDTH_20_40 = 1,
-	HT_CHANNEL_WIDTH_80 = 2,
-};
-
-/* Ref: 802.11i sepc D10.0 7.3.2.25.1
-Cipher Suites Encryption Algorithms */
-enum rt_enc_alg {
-	NO_ENCRYPTION = 0,
-	WEP40_ENCRYPTION = 1,
-	TKIP_ENCRYPTION = 2,
-	RSERVED_ENCRYPTION = 3,
-	AESCCMP_ENCRYPTION = 4,
-	WEP104_ENCRYPTION = 5,
-	AESCMAC_ENCRYPTION = 6,	/*IEEE802.11w */
-};
-
-enum rtl_hal_state {
-	_HAL_STATE_STOP = 0,
-	_HAL_STATE_START = 1,
-};
-
-enum rtl_var_map {
-	/*reg map */
-	SYS_ISO_CTRL = 0,
-	SYS_FUNC_EN,
-	SYS_CLK,
-	MAC_RCR_AM,
-	MAC_RCR_AB,
-	MAC_RCR_ACRC32,
-	MAC_RCR_ACF,
-	MAC_RCR_AAP,
-	MAC_HIMR,
-	MAC_HIMRE,
-	MAC_HSISR,
-
-	/*efuse map */
-	EFUSE_TEST,
-	EFUSE_CTRL,
-	EFUSE_CLK,
-	EFUSE_CLK_CTRL,
-	EFUSE_PWC_EV12V,
-	EFUSE_FEN_ELDR,
-	EFUSE_LOADER_CLK_EN,
-	EFUSE_ANA8M,
-	EFUSE_HWSET_MAX_SIZE,
-	EFUSE_MAX_SECTION_MAP,
-	EFUSE_REAL_CONTENT_SIZE,
-	EFUSE_OOB_PROTECT_BYTES_LEN,
-	EFUSE_ACCESS,
-	/*CAM map */
-	RWCAM,
-	WCAMI,
-	RCAMO,
-	CAMDBG,
-	SECR,
-	SEC_CAM_NONE,
-	SEC_CAM_WEP40,
-	SEC_CAM_TKIP,
-	SEC_CAM_AES,
-	SEC_CAM_WEP104,
-
-	/*IMR map */
-	RTL_IMR_BCNDMAINT6,	/*Beacon DMA Interrupt 6 */
-	RTL_IMR_BCNDMAINT5,	/*Beacon DMA Interrupt 5 */
-	RTL_IMR_BCNDMAINT4,	/*Beacon DMA Interrupt 4 */
-	RTL_IMR_BCNDMAINT3,	/*Beacon DMA Interrupt 3 */
-	RTL_IMR_BCNDMAINT2,	/*Beacon DMA Interrupt 2 */
-	RTL_IMR_BCNDMAINT1,	/*Beacon DMA Interrupt 1 */
-	RTL_IMR_BCNDOK8,	/*Beacon Queue DMA OK Interrup 8 */
-	RTL_IMR_BCNDOK7,	/*Beacon Queue DMA OK Interrup 7 */
-	RTL_IMR_BCNDOK6,	/*Beacon Queue DMA OK Interrup 6 */
-	RTL_IMR_BCNDOK5,	/*Beacon Queue DMA OK Interrup 5 */
-	RTL_IMR_BCNDOK4,	/*Beacon Queue DMA OK Interrup 4 */
-	RTL_IMR_BCNDOK3,	/*Beacon Queue DMA OK Interrup 3 */
-	RTL_IMR_BCNDOK2,	/*Beacon Queue DMA OK Interrup 2 */
-	RTL_IMR_BCNDOK1,	/*Beacon Queue DMA OK Interrup 1 */
-	RTL_IMR_TIMEOUT2,	/*Timeout interrupt 2 */
-	RTL_IMR_TIMEOUT1,	/*Timeout interrupt 1 */
-	RTL_IMR_TXFOVW,		/*Transmit FIFO Overflow */
-	RTL_IMR_PSTIMEOUT,	/*Power save time out interrupt */
-	RTL_IMR_BcnInt,		/*Beacon DMA Interrupt 0 */
-	RTL_IMR_RXFOVW,		/*Receive FIFO Overflow */
-	RTL_IMR_RDU,		/*Receive Descriptor Unavailable */
-	RTL_IMR_ATIMEND,	/*For 92C,ATIM Window End Interrupt */
-	RTL_IMR_BDOK,		/*Beacon Queue DMA OK Interrup */
-	RTL_IMR_HIGHDOK,	/*High Queue DMA OK Interrupt */
-	RTL_IMR_COMDOK,		/*Command Queue DMA OK Interrupt*/
-	RTL_IMR_TBDOK,		/*Transmit Beacon OK interrup */
-	RTL_IMR_MGNTDOK,	/*Management Queue DMA OK Interrupt */
-	RTL_IMR_TBDER,		/*For 92C,Transmit Beacon Error Interrupt */
-	RTL_IMR_BKDOK,		/*AC_BK DMA OK Interrupt */
-	RTL_IMR_BEDOK,		/*AC_BE DMA OK Interrupt */
-	RTL_IMR_VIDOK,		/*AC_VI DMA OK Interrupt */
-	RTL_IMR_VODOK,		/*AC_VO DMA Interrupt */
-	RTL_IMR_ROK,		/*Receive DMA OK Interrupt */
-	RTL_IMR_HSISR_IND,  /*HSISR Interrupt*/
-	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BcnInt | RTL_IMR_TBDOK |
-				 * RTL_IMR_TBDER) */
-	RTL_IMR_C2HCMD,		/*fw interrupt*/
-
-	/*CCK Rates, TxHT = 0 */
-	RTL_RC_CCK_RATE1M,
-	RTL_RC_CCK_RATE2M,
-	RTL_RC_CCK_RATE5_5M,
-	RTL_RC_CCK_RATE11M,
-
-	/*OFDM Rates, TxHT = 0 */
-	RTL_RC_OFDM_RATE6M,
-	RTL_RC_OFDM_RATE9M,
-	RTL_RC_OFDM_RATE12M,
-	RTL_RC_OFDM_RATE18M,
-	RTL_RC_OFDM_RATE24M,
-	RTL_RC_OFDM_RATE36M,
-	RTL_RC_OFDM_RATE48M,
-	RTL_RC_OFDM_RATE54M,
-
-	RTL_RC_HT_RATEMCS7,
-	RTL_RC_HT_RATEMCS15,
-
-	RTL_RC_VHT_RATE_1SS_MCS7,
-	RTL_RC_VHT_RATE_1SS_MCS8,
-	RTL_RC_VHT_RATE_1SS_MCS9,
-	RTL_RC_VHT_RATE_2SS_MCS7,
-	RTL_RC_VHT_RATE_2SS_MCS8,
-	RTL_RC_VHT_RATE_2SS_MCS9,
-
-	/*keep it last */
-	RTL_VAR_MAP_MAX,
-};
-
-/*Firmware PS mode for control LPS.*/
-enum _fw_ps_mode {
-	FW_PS_ACTIVE_MODE = 0,
-	FW_PS_MIN_MODE = 1,
-	FW_PS_MAX_MODE = 2,
-	FW_PS_DTIM_MODE = 3,
-	FW_PS_VOIP_MODE = 4,
-	FW_PS_UAPSD_WMM_MODE = 5,
-	FW_PS_UAPSD_MODE = 6,
-	FW_PS_IBSS_MODE = 7,
-	FW_PS_WWLAN_MODE = 8,
-	FW_PS_PM_Radio_Off = 9,
-	FW_PS_PM_Card_Disable = 10,
-};
-
-enum rt_psmode {
-	EACTIVE,		/*Active/Continuous access. */
-	EMAXPS,			/*Max power save mode. */
-	EFASTPS,		/*Fast power save mode. */
-	EAUTOPS,		/*Auto power save mode. */
-};
-
-/*LED related.*/
-enum led_ctl_mode {
-	LED_CTL_POWER_ON = 1,
-	LED_CTL_LINK = 2,
-	LED_CTL_NO_LINK = 3,
-	LED_CTL_TX = 4,
-	LED_CTL_RX = 5,
-	LED_CTL_SITE_SURVEY = 6,
-	LED_CTL_POWER_OFF = 7,
-	LED_CTL_START_TO_LINK = 8,
-	LED_CTL_START_WPS = 9,
-	LED_CTL_STOP_WPS = 10,
-};
-
-enum rtl_led_pin {
-	LED_PIN_GPIO0,
-	LED_PIN_LED0,
-	LED_PIN_LED1,
-	LED_PIN_LED2
-};
-
-/*QoS related.*/
-/*acm implementation method.*/
-enum acm_method {
-	eAcmWay0_SwAndHw = 0,
-	eAcmWay1_HW = 1,
-	eAcmWay2_SW = 2,
-};
-
-enum macphy_mode {
-	SINGLEMAC_SINGLEPHY = 0,
-	DUALMAC_DUALPHY,
-	DUALMAC_SINGLEPHY,
-};
-
-enum band_type {
-	BAND_ON_2_4G = 0,
-	BAND_ON_5G,
-	BAND_ON_BOTH,
-	BANDMAX
-};
-
-/*aci/aifsn Field.
-Ref: WMM spec 2.2.2: WME Parameter Element, p.12.*/
-union aci_aifsn {
-	u8 char_data;
-
-	struct {
-		u8 aifsn:4;
-		u8 acm:1;
-		u8 aci:2;
-		u8 reserved:1;
-	} f;			/* Field */
-};
-
-/*mlme related.*/
-enum wireless_mode {
-	WIRELESS_MODE_UNKNOWN = 0x00,
-	WIRELESS_MODE_A = 0x01,
-	WIRELESS_MODE_B = 0x02,
-	WIRELESS_MODE_G = 0x04,
-	WIRELESS_MODE_AUTO = 0x08,
-	WIRELESS_MODE_N_24G = 0x10,
-	WIRELESS_MODE_N_5G = 0x20,
-	WIRELESS_MODE_AC_5G = 0x40,
-	WIRELESS_MODE_AC_24G = 0x80,
-	WIRELESS_MODE_AC_ONLY = 0x100,
-	WIRELESS_MODE_MAX = 0x800
-};
-
-enum ratr_table_mode {
-	RATR_INX_WIRELESS_NGB = 0,
-	RATR_INX_WIRELESS_NG = 1,
-	RATR_INX_WIRELESS_NB = 2,
-	RATR_INX_WIRELESS_N = 3,
-	RATR_INX_WIRELESS_GB = 4,
-	RATR_INX_WIRELESS_G = 5,
-	RATR_INX_WIRELESS_B = 6,
-	RATR_INX_WIRELESS_MC = 7,
-	RATR_INX_WIRELESS_AC_5N = 8,
-	RATR_INX_WIRELESS_AC_24N = 9,
-};
-
-enum rtl_link_state {
-	MAC80211_NOLINK = 0,
-	MAC80211_LINKING = 1,
-	MAC80211_LINKED = 2,
-	MAC80211_LINKED_SCANNING = 3,
-};
-
-enum act_category {
-	ACT_CAT_QOS = 1,
-	ACT_CAT_DLS = 2,
-	ACT_CAT_BA = 3,
-	ACT_CAT_HT = 7,
-	ACT_CAT_WMM = 17,
-};
-
-enum ba_action {
-	ACT_ADDBAREQ = 0,
-	ACT_ADDBARSP = 1,
-	ACT_DELBA = 2,
-};
-
-enum rt_polarity_ctl {
-	RT_POLARITY_LOW_ACT = 0,
-	RT_POLARITY_HIGH_ACT = 1,
-};
-
-/* After 8188E, we use V2 reason define. 88C/8723A use V1 reason. */
-enum fw_wow_reason_v2 {
-	FW_WOW_V2_PTK_UPDATE_EVENT = 0x01,
-	FW_WOW_V2_GTK_UPDATE_EVENT = 0x02,
-	FW_WOW_V2_DISASSOC_EVENT = 0x04,
-	FW_WOW_V2_DEAUTH_EVENT = 0x08,
-	FW_WOW_V2_FW_DISCONNECT_EVENT = 0x10,
-	FW_WOW_V2_MAGIC_PKT_EVENT = 0x21,
-	FW_WOW_V2_UNICAST_PKT_EVENT = 0x22,
-	FW_WOW_V2_PATTERN_PKT_EVENT = 0x23,
-	FW_WOW_V2_RTD3_SSID_MATCH_EVENT = 0x24,
-	FW_WOW_V2_REALWOW_V2_WAKEUPPKT = 0x30,
-	FW_WOW_V2_REALWOW_V2_ACKLOST = 0x31,
-	FW_WOW_V2_REASON_MAX = 0xff,
-};
-
-enum wolpattern_type {
-	UNICAST_PATTERN = 0,
-	MULTICAST_PATTERN = 1,
-	BROADCAST_PATTERN = 2,
-	DONT_CARE_DA = 3,
-	UNKNOWN_TYPE = 4,
-};
-
-struct octet_string {
-	u8 *octet;
-	u16 length;
-};
-
-struct rtl_hdr_3addr {
-	__le16 frame_ctl;
-	__le16 duration_id;
-	u8 addr1[ETH_ALEN];
-	u8 addr2[ETH_ALEN];
-	u8 addr3[ETH_ALEN];
-	__le16 seq_ctl;
-	u8 payload[0];
-} __packed;
-
-struct rtl_info_element {
-	u8 id;
-	u8 len;
-	u8 data[0];
-} __packed;
-
-struct rtl_probe_rsp {
-	struct rtl_hdr_3addr header;
-	u32 time_stamp[2];
-	__le16 beacon_interval;
-	__le16 capability;
-	/*SSID, supported rates, FH params, DS params,
-	   CF params, IBSS params, TIM (if beacon), RSN */
-	struct rtl_info_element info_element[0];
-} __packed;
-
-/*LED related.*/
-/*ledpin Identify how to implement this SW led.*/
-struct rtl_led {
-	void *hw;
-	enum rtl_led_pin ledpin;
-	bool b_ledon;
-};
-
-struct rtl_led_ctl {
-	bool bled_opendrain;
-	struct rtl_led sw_led0;
-	struct rtl_led sw_led1;
-};
-
-struct rtl_qos_parameters {
-	__le16 cw_min;
-	__le16 cw_max;
-	u8 aifs;
-	u8 flag;
-	__le16 tx_op;
-} __packed;
-
-struct rt_smooth_data {
-	u32 elements[100];	/*array to store values */
-	u32 index;		/*index to current array to store */
-	u32 total_num;		/*num of valid elements */
-	u32 total_val;		/*sum of valid elements */
-};
-
-struct rtl_ht_agg {
-	u16 txq_id;
-	u16 wait_for_ba;
-	u16 start_idx;
-	u64 bitmap;
-	u32 rate_n_flags;
-	u8 agg_state;
-	u8 rx_agg_state;
-};
-
-struct rtl_tid_data {
-	u16 seq_number;
-	struct rtl_ht_agg agg;
-};
-
-struct rssi_sta {
-	long undecorated_smoothed_pwdb;
-};
-
-struct rtl_sta_info {
-	struct list_head list;
-	u8 ratr_index;
-	u8 wireless_mode;
-	u8 mimo_ps;
-	u8 mac_addr[6];
-	struct rtl_tid_data tids[MAX_TID_COUNT];
-
-	/* just used for ap adhoc or mesh*/
-	struct rssi_sta rssi_stat;
-} __packed;
-
-struct false_alarm_statistics {
-	u32 cnt_parity_fail;
-	u32 cnt_rate_illegal;
-	u32 cnt_crc8_fail;
-	u32 cnt_mcs_fail;
-	u32 cnt_fast_fsync_fail;
-	u32 cnt_sb_search_fail;
-	u32 cnt_ofdm_fail;
-	u32 cnt_cck_fail;
-	u32 cnt_all;
-	u32 cnt_ofdm_cca;
-	u32 cnt_cck_cca;
-	u32 cnt_cca_all;
-	u32 cnt_bw_usc;
-	u32 cnt_bw_lsc;
-};
-
-struct init_gain {
-	u8 xaagccore1;
-	u8 xbagccore1;
-	u8 xcagccore1;
-	u8 xdagccore1;
-	u8 cca;
-
-};
-
-struct wireless_stats {
-	unsigned long txbytesunicast;
-	unsigned long txbytesmulticast;
-	unsigned long txbytesbroadcast;
-	unsigned long rxbytesunicast;
-
-	long rx_snr_db[4];
-	/*Correct smoothed ss in Dbm, only used
-	   in driver to report real power now. */
-	long recv_signal_power;
-	long signal_quality;
-	long last_sigstrength_inpercent;
-
-	u32 rssi_calculate_cnt;
-	u32 pwdb_all_cnt;
-
-	/*Transformed, in dbm. Beautified signal
-	   strength for UI, not correct. */
-	long signal_strength;
-
-	u8 rx_rssi_percentage[4];
-	u8 rx_evm_dbm[4];
-	u8 rx_evm_percentage[2];
-
-	u16 rx_cfo_short[4];
-	u16 rx_cfo_tail[4];
-
-	struct rt_smooth_data ui_rssi;
-	struct rt_smooth_data ui_link_quality;
-};
-
-struct rate_adaptive {
-	u8 rate_adaptive_disabled;
-	u8 ratr_state;
-	u16 reserve;
-
-	u32 high_rssi_thresh_for_ra;
-	u32 high2low_rssi_thresh_for_ra;
-	u8 low2high_rssi_thresh_for_ra;
-	u32 low_rssi_thresh_for_ra;
-	u32 upper_rssi_threshold_ratr;
-	u32 middleupper_rssi_threshold_ratr;
-	u32 middle_rssi_threshold_ratr;
-	u32 middlelow_rssi_threshold_ratr;
-	u32 low_rssi_threshold_ratr;
-	u32 ultralow_rssi_threshold_ratr;
-	u32 low_rssi_threshold_ratr_40m;
-	u32 low_rssi_threshold_ratr_20m;
-	u8 ping_rssi_enable;
-	u32 ping_rssi_ratr;
-	u32 ping_rssi_thresh_for_ra;
-	u32 last_ratr;
-	u8 pre_ratr_state;
-	u8 ldpc_thres;
-	bool use_ldpc;
-	bool lower_rts_rate;
-	bool is_special_data;
-};
-
-struct regd_pair_mapping {
-	u16 reg_dmnenum;
-	u16 reg_5ghz_ctl;
-	u16 reg_2ghz_ctl;
-};
-
-struct dynamic_primary_cca {
-	u8 pricca_flag;
-	u8 intf_flag;
-	u8 intf_type;
-	u8 dup_rts_flag;
-	u8 monitor_flag;
-	u8 ch_offset;
-	u8 mf_state;
-};
-
-struct rtl_regulatory {
-	char alpha2[2];
-	u16 country_code;
-	u16 max_power_level;
-	u32 tp_scale;
-	u16 current_rd;
-	u16 current_rd_ext;
-	int16_t power_limit;
-	struct regd_pair_mapping *regpair;
-};
-
-struct rtl_rfkill {
-	bool rfkill_state;	/*0 is off, 1 is on */
-};
-
-/*for P2P PS**/
-#define	P2P_MAX_NOA_NUM		2
-
-enum p2p_role {
-	P2P_ROLE_DISABLE = 0,
-	P2P_ROLE_DEVICE = 1,
-	P2P_ROLE_CLIENT = 2,
-	P2P_ROLE_GO = 3
-};
-
-enum p2p_ps_state {
-	P2P_PS_DISABLE = 0,
-	P2P_PS_ENABLE = 1,
-	P2P_PS_SCAN = 2,
-	P2P_PS_SCAN_DONE = 3,
-	P2P_PS_ALLSTASLEEP = 4, /* for P2P GO */
-};
-
-enum p2p_ps_mode {
-	P2P_PS_NONE = 0,
-	P2P_PS_CTWINDOW = 1,
-	P2P_PS_NOA = 2,
-	P2P_PS_MIX = 3, /* CTWindow and NoA*/
-};
-
-struct rtl_p2p_ps_info {
-	enum p2p_ps_mode p2p_ps_mode; /* indicate p2p ps mode */
-	enum p2p_ps_state p2p_ps_state; /* indicate p2p ps state */
-	u8 noa_index; /* Identifies and instance of Notice of Absence timing. */
-	/* Client traffic window. A period of time in TU after TBTT. */
-	u8 ctwindow;
-	u8 opp_ps; /* opportunistic power save. */
-	u8 noa_num; /* number of NoA descriptor in P2P IE. */
-	/* Count for owner, Type of client. */
-	u8 noa_count_type[P2P_MAX_NOA_NUM];
-	/* Max duration for owner, preferred or
-	 * min acceptable duration for client. */
-	u32 noa_duration[P2P_MAX_NOA_NUM];
-	/* Length of interval for owner, preferred or
-	 * max acceptable interval of client. */
-	u32 noa_interval[P2P_MAX_NOA_NUM];
-	/* schedule expressed in terms of the lower 4 bytes of the TSF timer. */
-	u32 noa_start_time[P2P_MAX_NOA_NUM];
-};
-
-struct p2p_ps_offload_t {
-	u8 Offload_En:1;
-	u8 role:1; /* 1: Owner, 0: Client */
-	u8 CTWindow_En:1;
-	u8 NoA0_En:1;
-	u8 NoA1_En:1;
-	u8 AllStaSleep:1;
-	u8 discovery:1;
-	u8 reserved:1;
-};
-
-#define IQK_MATRIX_REG_NUM	8
-/* Channels_2_4G_NUM + Channels_5G_20M_NUM + Channels_5G */
-#define IQK_MATRIX_SETTINGS_NUM	(14 + 24 + 21)
-struct iqk_matrix_regs {
-	bool b_iqk_done;
-	long value[1][IQK_MATRIX_REG_NUM];
-};
-
-struct rtl_phy {
-	struct bb_reg_def phyreg_def[4];	/*Radio A/B/C/D */
-	struct init_gain initgain_backup;
-	enum io_type current_io_type;
-
-	u8 rf_mode;
-	u8 rf_type;
-	u8 current_chan_bw;
-	u8 set_bwmode_inprogress;
-	u8 sw_chnl_inprogress;
-	u8 sw_chnl_stage;
-	u8 sw_chnl_step;
-	u8 current_channel;
-	u8 h2c_box_num;
-	u8 set_io_inprogress;
-	u8 lck_inprogress;
-	bool iqk_inprogress;
-
-	/* record for power tracking */
-	s32 reg_e94;
-	s32 reg_e9c;
-	s32 reg_ea4;
-	s32 reg_eac;
-	s32 reg_eb4;
-	s32 reg_ebc;
-	s32 reg_ec4;
-	s32 reg_ecc;
-	u8 rfpienable;
-	u8 reserve_0;
-	u16 reserve_1;
-	u32 reg_c04, reg_c08, reg_874;
-	u32 adda_backup[16];
-	u32 iqk_mac_backup[IQK_MAC_REG_NUM];
-	u32 iqk_bb_backup[10];
-	bool iqk_initialized;
-
-	bool rfpath_rx_enable[MAX_RF_PATH];
-	/*Jaguar*/
-	u8 reg_837;
-	/* Dul mac */
-	bool b_need_iqk;
-	struct iqk_matrix_regs iqk_matrix_regsetting[IQK_MATRIX_SETTINGS_NUM];
-
-	bool b_rfpi_enable;
-
-	u8 pwrgroup_cnt;
-	u8 bcck_high_power;
-	/* this is for 88E & 8723A */
-	u32 mcs_txpwrlevel_origoffset[MAX_PG_GROUP][16];
-	/* this is for 92EE */
-	u32 tx_power_by_rate_offset[TX_PWR_BY_RATE_NUM_BAND]
-				   [TX_PWR_BY_RATE_NUM_RF]
-				   [TX_PWR_BY_RATE_NUM_RF]
-				   [TX_PWR_BY_RATE_NUM_SECTION];
-	u8 txpwr_by_rate_base_24g[TX_PWR_BY_RATE_NUM_RF]
-				 [TX_PWR_BY_RATE_NUM_RF]
-				 [MAX_BASE_NUM_IN_PHY_REG_PG_24G];
-
-	u8 txpwr_by_rate_base_5g[TX_PWR_BY_RATE_NUM_RF]
-				[TX_PWR_BY_RATE_NUM_RF]
-				[MAX_BASE_NUM_IN_PHY_REG_PG_5G];
-	u8 default_initialgain[4];
-
-	/* the current Tx power level */
-	u8 cur_cck_txpwridx;
-	u8 cur_ofdm24g_txpwridx;
-	u8 cur_bw20_txpwridx;
-	u8 cur_bw40_txpwridx;
-
-	char txpwr_limit_2_4g[MAX_REGULATION_NUM]
-				[MAX_2_4G_BANDWITH_NUM]
-				[MAX_RATE_SECTION_NUM]
-				[CHANNEL_MAX_NUMBER_2G]
-				[MAX_RF_PATH_NUM];
-	char txpwr_limit_5g[MAX_REGULATION_NUM]
-			   [MAX_5G_BANDWITH_NUM]
-			   [MAX_RATE_SECTION_NUM]
-			   [CHANNEL_MAX_NUMBER_5G]
-			   [MAX_RF_PATH_NUM];
-
-	u32 rfreg_chnlval[2];
-	bool b_apk_done;
-	u32 reg_rf3c[2];	/* pathA / pathB  */
-
-	u32 backup_rf_0x1a;/*92ee*/
-	/* bfsync */
-	u8 framesync;
-	u32 framesync_c34;
-
-	u8 num_total_rfpath;
-	u16 rf_pathmap;
-
-	enum rt_polarity_ctl polarity_ctl;
-};
-
-#define RTL_AGG_STOP						0
-#define RTL_AGG_PROGRESS					1
-#define RTL_AGG_START						2
-#define RTL_AGG_OPERATIONAL					3
-#define RTL_RX_AGG_START					1
-#define RTL_RX_AGG_STOP						0
-
-struct rtl_priv;
-struct rtl_io {
-	struct device *dev;
-
-	/*PCI MEM map */
-	unsigned long pci_mem_end;	/*shared mem end        */
-	unsigned long pci_mem_start;	/*shared mem start */
-
-	/*PCI IO map */
-	unsigned long pci_base_addr;	/*device I/O address */
-
-	void (*write8_async)(struct rtl_priv *rtlpriv, u32 addr, u8 val);
-	void (*write16_async)(struct rtl_priv *rtlpriv, u32 addr, u16 val);
-	void (*write32_async)(struct rtl_priv *rtlpriv, u32 addr, u32 val);
-
-	u8 (*read8_sync)(struct rtl_priv *rtlpriv, u32 addr);
-	u16 (*read16_sync)(struct rtl_priv *rtlpriv, u32 addr);
-	u32 (*read32_sync)(struct rtl_priv *rtlpriv, u32 addr);
-
-};
-
-struct rtl_mac {
-	u8 mac_addr[ETH_ALEN];
-	u8 mac80211_registered;
-	u8 beacon_enabled;
-
-	u32 tx_ss_num;
-	u32 rx_ss_num;
-
-	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
-	struct ieee80211_hw *hw;
-	struct ieee80211_vif *vif;
-	enum nl80211_iftype opmode;
-
-	/*Probe Beacon management */
-	enum rtl_link_state link_state;
-
-	int n_channels;
-	int n_bitrates;
-
-	bool offchan_deley;
-	u8 p2p;	/*using p2p role*/
-	bool p2p_in_use;
-
-	/*filters */
-	u32 rx_conf;
-
-	bool act_scanning;
-	u8 cnt_after_linked;
-	bool skip_scan;
-
-	/* early mode */
-	/* skb wait queue */
-	struct sk_buff_head skb_waitq[MAX_TID_COUNT];
-
-	/*RDG*/
-	bool rdg_en;
-
-	u8 ht_stbc_cap;
-	u8 ht_cur_stbc;
-
-	/*vht support*/
-	u8 vht_enable;
-	u8 bw_80;
-	u8 vht_cur_ldpc;
-	u8 vht_cur_stbc;
-	u8 vht_stbc_cap;
-	u8 vht_ldpc_cap;
-
-	/*AP*/
-	u8 bssid[6];
-	u32 vendor;
-	u32 basic_rates; /* b/g rates */
-	u8 ht_enable;
-	u8 bw_40;
-	u8 mode;		/* wireless mode */
-	u8 slot_time;
-	u8 short_preamble;
-	u8 use_cts_protect;
-	u8 cur_40_prime_sc;
-	u8 cur_40_prime_sc_bk;
-	u8 cur_80_prime_sc;
-	u64 tsf;
-	u8 retry_short;
-	u8 retry_long;
-	u16 assoc_id;
-	bool bhiddenssid;
-
-	/*IBSS*/
-	int beacon_interval;
-
-	/*AMPDU*/
-	u8 min_space_cfg;	/*For Min spacing configurations */
-	u8 max_mss_density;
-	u8 current_ampdu_factor;
-	u8 current_ampdu_density;
-
-	/*QOS & EDCA */
-	struct ieee80211_tx_queue_params edca_param[RTL_MAC80211_NUM_QUEUE];
-	struct rtl_qos_parameters ac[AC_MAX];
-};
-
-struct rtl_hal {
-	struct ieee80211_hw *hw;
-
-	bool driver_is_goingto_unload;
-	bool up_first_time;
-	bool bfirst_init;
-	bool being_init_adapter;
-	bool b_bbrf_ready;
-	bool b_mac_func_enable;
-	bool b_pre_edcca_enable;
-
-	enum intf_type interface;
-	u16 hw_type;		/*92c or 92d or 92s and so on */
-	u8 ic_class;
-	u8 oem_id;
-	u32 version;		/*version of chip */
-	u8 state;		/*stop 0, start 1 */
-	u8 boad_type;
-
-	u8 pa_mode;
-	u8 pa_type_2g;
-	u8 pa_type_5g;
-	u8 lna_type_2g;
-	u8 lna_type_5g;
-	u8 external_pa_2g;
-	u8 external_lna_2g;
-	u8 external_pa_5g;
-	u8 external_lna_5g;
-	u8 rfe_type;
-
-	/*firmware */
-	u32 fwsize;
-	u8 *pfirmware;
-	u16 fw_version;
-	u16 fw_subversion;
-	bool b_h2c_setinprogress;
-	u8 last_hmeboxnum;
-	bool bfw_ready;
-
-	/*Reserve page start offset except beacon in TxQ. */
-	u8 fw_rsvdpage_startoffset;
-	u8 h2c_txcmd_seq;
-	u8 current_ra_rate;
-
-	/* FW Cmd IO related */
-	u16 fwcmd_iomap;
-	u32 fwcmd_ioparam;
-	bool set_fwcmd_inprogress;
-	u8 current_fwcmd_io;
-
-	bool bfw_clk_change_in_progress;
-	bool ballow_sw_to_change_hwclc;
-	u8 fw_ps_state;
-	 struct p2p_ps_offload_t p2p_ps_offload;
-	/**/
-	bool driver_going2unload;
-
-	/*AMPDU init min space*/
-	u8 minspace_cfg;	/*For Min spacing configurations */
-
-	/* Dul mac */
-	enum macphy_mode macphymode;
-	enum band_type current_bandtype;	/* 0:2.4G, 1:5G */
-	enum band_type current_bandtypebackup;
-	enum band_type bandset;
-	/* dual MAC 0--Mac0 1--Mac1 */
-	u32 interfaceindex;
-	/* just for DulMac S3S4 */
-	u8 macphyctl_reg;
-	bool b_earlymode_enable;
-	u8 max_earlymode_num;
-	/* Dul mac*/
-	bool during_mac0init_radiob;
-	bool during_mac1init_radioa;
-	bool reloadtxpowerindex;
-	/* True if IMR or IQK  have done
-	for 2.4G in scan progress */
-	bool b_load_imrandiqk_setting_for2g;
-
-	bool disable_amsdu_8k;
-	bool bmaster_of_dmsp;
-	bool bslave_of_dmsp;
-
-	u16 rx_tag;/*for 92ee*/
-	u8 rts_en;
-
-	/*for wowlan*/
-	bool wow_enable;
-	bool b_enter_pnp_sleep;
-	bool b_wake_from_pnp_sleep;
-	bool wow_enabled;
-	__kernel_time_t last_suspend_sec;
-	u32 wowlan_fwsize;
-	u8 *p_wowlan_firmware;
-
-	u8 hw_rof_enable; /*Enable GPIO[9] as WL RF HW PDn source*/
-
-	bool real_wow_v2_enable;
-	bool re_init_llt_table;
-};
-
-struct rtl_security {
-	/*default 0 */
-	bool use_sw_sec;
-
-	bool being_setkey;
-	bool use_defaultkey;
-	/*Encryption Algorithm for Unicast Packet */
-	enum rt_enc_alg pairwise_enc_algorithm;
-	/*Encryption Algorithm for Brocast/Multicast */
-	enum rt_enc_alg group_enc_algorithm;
-	/*Cam Entry Bitmap */
-	u32 hwsec_cam_bitmap;
-	u8 hwsec_cam_sta_addr[TOTAL_CAM_ENTRY][ETH_ALEN];
-	/*local Key buffer, indx 0 is for
-	   pairwise key 1-4 is for agoup key. */
-	u8 key_buf[KEY_BUF_SIZE][MAX_KEY_LEN];
-	u8 key_len[KEY_BUF_SIZE];
-
-	/*The pointer of Pairwise Key,
-	   it always points to KeyBuf[4] */
-	u8 *pairwise_key;
-};
-
-struct rtl_dig {
-	u8 dig_enable_flag;
-	u8 dig_ext_port_stage;
-
-	u32 rssi_lowthresh;
-	u32 rssi_highthresh;
-
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-
-	u8 cursta_connectstate;
-	u8 presta_connectstate;
-	u8 curmultista_connectstate;
-
-	u8 pre_igvalue;
-	u8 cur_igvalue;
-
-	char backoff_val;
-	char backoff_val_range_max;
-	char backoff_val_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-	u8 rssi_val_min;
-	u8 min_undecorated_pwdb_for_dm;
-	long last_min_undecorated_pwdb_for_dm;
-
-	u8 pre_cck_pd_state;
-	u8 cur_cck_pd_state;
-
-	u8 large_fa_hit;
-	u8 forbidden_igi;
-	u32 recover_cnt;
-
-};
-
-struct rtl_pstbl {
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-
-	u8 pre_rfstate;
-	u8 cur_rfstate;
-
-	long rssi_val_min;
-
-};
-
-#define ASSOCIATE_ENTRY_NUM	(32 + 1)
-
-struct fast_ant_trainning {
-	u8 bssid[6];
-	u8 antsel_rx_keep_0;
-	u8 antsel_rx_keep_1;
-	u8 antsel_rx_keep_2;
-	u32 ant_sum_rssi[7];
-	u32 ant_rssi_cnt[7];
-	u32 ant_ave_rssi[7];
-	u8 fat_state;
-	u32 train_idx;
-	u8 antsel_a[ASSOCIATE_ENTRY_NUM];
-	u8 antsel_b[ASSOCIATE_ENTRY_NUM];
-	u8 antsel_c[ASSOCIATE_ENTRY_NUM];
-	u32 main_ant_sum[ASSOCIATE_ENTRY_NUM];
-	u32 aux_ant_sum[ASSOCIATE_ENTRY_NUM];
-	u32 main_ant_cnt[ASSOCIATE_ENTRY_NUM];
-	u32 aux_ant_cnt[ASSOCIATE_ENTRY_NUM];
-	u8 rx_idle_ant;
-	bool b_becomelinked;
-};
-
-struct dm_phy_dbg_info {
-	char rx_snrdb[4];
-	u64 num_qry_phy_status;
-	u64 num_qry_phy_status_cck;
-	u64 num_qry_phy_status_ofdm;
-	u16 num_qry_beacon_pkt;
-	u16 num_non_be_pkt;
-	s32 rx_evm[4];
-};
-
-struct rtl_dm {
-	/*PHY status for DM */
-	long entry_min_undecoratedsmoothed_pwdb;
-	long undecorated_smoothed_pwdb;	/*out dm */
-	long entry_max_undecoratedsmoothed_pwdb;
-	bool b_dm_initialgain_enable;
-	bool bdynamic_txpower_enable;
-	bool bcurrent_turbo_edca;
-	bool bis_any_nonbepkts;	/*out dm */
-	bool bis_cur_rdlstate;
-	bool btxpower_trackinginit;
-	bool b_disable_framebursting;
-	bool b_cck_inch14;
-	bool btxpower_tracking;
-	bool b_useramask;
-	bool brfpath_rxenable[4];
-	bool binform_fw_driverctrldm;
-	bool bcurrent_mrc_switch;
-	u8 txpowercount;
-
-	u8 thermalvalue_rxgain;
-	u8 thermalvalue_iqk;
-	u8 thermalvalue_lck;
-	u8 thermalvalue;
-	u8 thermalvalue_avg[AVG_THERMAL_NUM];
-	u8 thermalvalue_avg_index;
-	bool bdone_txpower;
-	u8 last_dtp_lvl;
-	u8 dynamic_txhighpower_lvl;	/*Tx high power level */
-	u8 dm_flag;	/*Indicate if each dynamic mechanism's status. */
-	u8 dm_type;
-	u8 txpower_track_control;
-	bool binterrupt_migration;
-	bool bdisable_tx_int;
-	char ofdm_index[MAX_RF_PATH];
-	char cck_index;
-	u8 default_ofdm_index;
-	u8 default_cck_index;
-	char delta_power_index[MAX_RF_PATH];
-	char delta_power_index_last[MAX_RF_PATH];
-	char power_index_offset[MAX_RF_PATH];
-	char aboslute_ofdm_swing_idx[MAX_RF_PATH];
-	char remnant_ofdm_swing_idx[MAX_RF_PATH];
-	char remnant_cck_idx;
-	bool modify_txagc_flag_path_a;
-	bool modify_txagc_flag_path_b;
-
-	bool b_one_entry_only;
-	struct dm_phy_dbg_info dbginfo;
-	/* Dynamic ATC switch */
-
-	bool atc_status;
-	bool large_cfo_hit;
-	bool is_freeze;
-	int cfo_tail[2];
-	int cfo_ave_pre;
-	int crystal_cap;
-	u8 cfo_threshold;
-	u32 packet_count;
-	u32 packet_count_pre;
-	u8 tx_rate;
-
-
-	/*88e tx power tracking*/
-	u8 bb_swing_idx_ofdm[2];
-	u8 bb_swing_idx_ofdm_current;
-	u8 bb_swing_idx_ofdm_base[MAX_RF_PATH];
-	bool bb_swing_flag_Ofdm;
-	u8 bb_swing_idx_cck;
-	u8 bb_swing_idx_cck_current;
-	u8 bb_swing_idx_cck_base;
-	bool bb_swing_flag_cck;
-
-	char bb_swing_diff_2g;
-	char bb_swing_diff_5g;
-
-	/* DMSP */
-	bool supp_phymode_switch;
-
-	/* DulMac */
-	struct rtl_dig dm_digtable;
-	struct rtl_pstbl dm_pstable;
-	struct fast_ant_trainning fat_table;
-
-	u8 linked_interval;
-
-	u64 last_tx_ok_cnt;
-	u64 last_rx_ok_cnt;
-
-	bool cck_high_power;
-};
-
-#define	EFUSE_MAX_LOGICAL_SIZE		256
-
-struct rtl_efuse {
-	bool bautoLoad_ok;
-	bool bootfromefuse;
-	u16 max_physical_size;
-
-	u8 efuse_map[2][EFUSE_MAX_LOGICAL_SIZE];
-	u16 efuse_usedbytes;
-	u8 efuse_usedpercentage;
-	u8 autoload_failflag;
-	u8 autoload_status;
-
-	short epromtype;
-	u16 eeprom_vid;
-	u16 eeprom_did;
-	u16 eeprom_svid;
-	u16 eeprom_smid;
-	u8 eeprom_oemid;
-	u16 eeprom_channelplan;
-	u8 eeprom_version;
-
-	u8 dev_addr[6];
-	u8 board_type;
-	u8 wowlan_enable;
-	u8 antenna_div_cfg;
-	u8 antenna_div_type;
-
-	bool b_txpwr_fromeprom;
-	u8 eeprom_crystalcap;
-	u8 eeprom_tssi[2];
-	u8 eeprom_tssi_5g[3][2]; /* for 5GL/5GM/5GH band. */
-	u8 eeprom_pwrlimit_ht20[CHANNEL_GROUP_MAX];
-	u8 eeprom_pwrlimit_ht40[CHANNEL_GROUP_MAX];
-	u8 eeprom_chnlarea_txpwr_cck[2][CHANNEL_GROUP_MAX_2G];
-	u8 eeprom_chnlarea_txpwr_ht40_1s[2][CHANNEL_GROUP_MAX];
-	u8 eeprom_chnlarea_txpwr_ht40_2sdiif[2][CHANNEL_GROUP_MAX];
-
-
-	u8 internal_pa_5g[2];	/* pathA / pathB */
-	u8 eeprom_c9;
-	u8 eeprom_cc;
-
-	/*For power group */
-	u8 eeprom_pwrgroup[2][3];
-	u8 pwrgroup_ht20[2][CHANNEL_MAX_NUMBER];
-	u8 pwrgroup_ht40[2][CHANNEL_MAX_NUMBER];
-
-	u8 txpwrlevel_cck[MAX_RF_PATH][CHANNEL_MAX_NUMBER_2G];
-	/*For HT 40MHZ pwr */
-	u8 txpwrlevel_ht40_1s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	/*For HT 40MHZ pwr */
-	u8 txpwrlevel_ht40_2s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-
-	/*--------------------------------------------------------*
-	 * 8192CE\8192SE\8192DE\8723AE use the following 4 arrays,
-	 * other ICs (8188EE\8723BE\8192EE\8812AE...)
-	 * define new arrays in Windows code.
-	 * BUT, in linux code, we use the same array for all ICs.
-	 *
-	 * The Correspondance relation between two arrays is:
-	 * txpwr_cckdiff[][] == CCK_24G_Diff[][]
-	 * txpwr_ht20diff[][] == BW20_24G_Diff[][]
-	 * txpwr_ht40diff[][] == BW40_24G_Diff[][]
-	 * txpwr_legacyhtdiff[][] == OFDM_24G_Diff[][]
-	 *
-	 * Sizes of these arrays are decided by the larger ones.
-	 */
-	char txpwr_cckdiff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	char txpwr_ht20diff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	char txpwr_ht40diff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	char txpwr_legacyhtdiff[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	/*--------------------------------------------------------*/
-
-	u8 txpwr_5g_bw40base[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	u8 txpwr_5g_bw80base[MAX_RF_PATH][CHANNEL_MAX_NUMBER_5G_80M];
-	char txpwr_5g_ofdmdiff[MAX_RF_PATH][MAX_TX_COUNT];
-	char txpwr_5g_bw20diff[MAX_RF_PATH][MAX_TX_COUNT];
-	char txpwr_5g_bw40diff[MAX_RF_PATH][MAX_TX_COUNT];
-	char txpwr_5g_bw80diff[MAX_RF_PATH][MAX_TX_COUNT];
-
-	u8 txpwr_safetyflag;		/* Band edge enable flag */
-	u16 eeprom_txpowerdiff;
-	u8 legacy_httxpowerdiff;	/* Legacy to HT rate power diff */
-	u8 antenna_txpwdiff[3];
-
-	u8 eeprom_regulatory;
-	u8 eeprom_thermalmeter;
-	u8 thermalmeter[2];/*ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
-	u16 tssi_13dbm;
-	u8 crystalcap;		/* CrystalCap. */
-	u8 delta_iqk;
-	u8 delta_lck;
-
-	u8 legacy_ht_txpowerdiff;	/*Legacy to HT rate power diff */
-	bool b_apk_thermalmeterignore;
-
-	bool b1x1_recvcombine;
-	bool b1ss_support;
-
-	/*channel plan */
-	u8 channel_plan;
-};
-
-struct rtl_ps_ctl {
-	bool pwrdomain_protect;
-	bool b_in_powersavemode;
-	bool rfchange_inprogress;
-	bool b_swrf_processing;
-	bool b_hwradiooff;
-	/*
-	 * just for PCIE ASPM
-	 * If it supports ASPM, Offset[560h] = 0x40,
-	 * otherwise Offset[560h] = 0x00.
-	 * */
-	bool b_support_aspm;
-	bool b_support_backdoor;
-
-	/*for LPS */
-	enum rt_psmode dot11_psmode;	/*Power save mode configured. */
-	bool b_swctrl_lps;
-	bool b_fwctrl_lps;
-	u8 fwctrl_psmode;
-	/*For Fw control LPS mode */
-	u8 b_reg_fwctrl_lps;
-	/*Record Fw PS mode status. */
-	bool b_fw_current_inpsmode;
-	u8 reg_max_lps_awakeintvl;
-	bool report_linked;
-	bool b_low_power_enable;/*for 32k*/
-
-	/*for IPS */
-	bool b_inactiveps;
-
-	u32 rfoff_reason;
-
-	/*RF OFF Level */
-	u32 cur_ps_level;
-	u32 reg_rfps_level;
-
-	/*just for PCIE ASPM */
-	u8 const_amdpci_aspm;
-
-	enum rf_pwrstate inactive_pwrstate;
-	enum rf_pwrstate rfpwr_state;	/*cur power state */
-
-	/* for SW LPS*/
-	bool sw_ps_enabled;
-	bool state;
-	bool state_inap;
-	bool multi_buffered;
-	u16 nullfunc_seq;
-	unsigned int dtim_counter;
-	unsigned int sleep_ms;
-	unsigned long last_sleep_jiffies;
-	unsigned long last_awake_jiffies;
-	unsigned long last_delaylps_stamp_jiffies;
-	unsigned long last_dtim;
-	unsigned long last_beacon;
-	unsigned long last_action;
-	unsigned long last_slept;
-
-	/*For P2P PS */
-	struct rtl_p2p_ps_info p2p_ps_info;
-	u8 pwr_mode;
-	u8 smart_ps;
-
-	/* wake up on line */
-	u8 wo_wlan_mode;
-	u8 arp_offload_enable;
-	u8 gtk_offload_enable;
-	/* Used for WOL, indicates the reason for waking event.*/
-	u32 wakeup_reason;
-	/* Record the last waking time for comparison with setting key. */
-	u64 last_wakeup_time;
-};
-
-struct rtl_stats {
-	u8 psaddr[ETH_ALEN];
-	u32 mac_time[2];
-	s8 rssi;
-	u8 signal;
-	u8 noise;
-	u8 rate;		/* hw desc rate */
-	u8 rawdata;
-	u8 received_channel;
-	u8 control;
-	u8 mask;
-	u8 freq;
-	u16 len;
-	u64 tsf;
-	u32 beacon_time;
-	u8 nic_type;
-	u16 length;
-	u8 signalquality;	/*in 0-100 index. */
-	/*
-	 * Real power in dBm for this packet,
-	 * no beautification and aggregation.
-	 * */
-	s32 recvsignalpower;
-	s8 rxpower;		/*in dBm Translate from PWdB */
-	u8 signalstrength;	/*in 0-100 index. */
-	u16 b_hwerror:1;
-	u16 b_crc:1;
-	u16 b_icv:1;
-	u16 b_shortpreamble:1;
-	u16 antenna:1;
-	u16 decrypted:1;
-	u16 wakeup:1;
-	u32 timestamp_low;
-	u32 timestamp_high;
-	bool b_shift;
-
-	u8 rx_drvinfo_size;
-	u8 rx_bufshift;
-	bool b_isampdu;
-	bool b_isfirst_ampdu;
-	bool rx_is40Mhzpacket;
-	u8 rx_packet_bw;
-	u32 rx_pwdb_all;
-	u8 rx_mimo_signalstrength[4];	/*in 0~100 index */
-	s8 rx_mimo_signalquality[4];
-	u8 rx_mimo_evm_dbm[4];
-	u16 cfo_short[4];		/* per-path's Cfo_short */
-	u16 cfo_tail[4];
-
-	u8 rx_pwr[4]; /* per-path's pwdb */
-	u8 rx_snr[4]; /* per-path's SNR */
-	u8 bandwidth;
-	u8 bt_coex_pwr_adjust;
-	bool b_packet_matchbssid;
-	bool b_is_cck;
-	bool b_is_ht;
-	bool b_packet_toself;
-	bool b_packet_beacon;	/*for rssi */
-	char cck_adc_pwdb[4];	/*for rx path selection */
-
-	bool b_is_vht;
-	bool b_is_short_gi;
-	u8 vht_nss;
-
-	u8 packet_report_type;
-
-	u32 macid;
-	u8 wake_match;
-	u32 bt_rx_rssi_percentage;
-	u32 macid_valid_entry[2];
-};
-
-struct rt_link_detect {
-	/* count for raoming */
-	u32 bcn_rx_inperiod;
-	u32 roam_times;
-
-	u32 num_tx_in4period[4];
-	u32 num_rx_in4period[4];
-
-	u32 num_tx_inperiod;
-	u32 num_rx_inperiod;
-
-	bool b_busytraffic;
-	bool b_tx_busy_traffic;
-	bool b_rx_busy_traffic;
-	bool b_higher_busytraffic;
-	bool b_higher_busyrxtraffic;
-
-	u32 tidtx_in4period[MAX_TID_COUNT][4];
-	u32 tidtx_inperiod[MAX_TID_COUNT];
-	bool higher_busytxtraffic[MAX_TID_COUNT];
-};
-
-struct rtl_tcb_desc {
-	u8 packet_bw:2;
-	u8 b_multicast:1;
-	u8 b_broadcast:1;
-
-	u8 b_rts_stbc:1;
-	u8 b_rts_enable:1;
-	u8 b_cts_enable:1;
-	u8 b_rts_use_shortpreamble:1;
-	u8 b_rts_use_shortgi:1;
-	u8 rts_sc:1;
-	u8 b_rts_bw:1;
-	u8 rts_rate;
-
-	u8 use_shortgi:1;
-	u8 use_shortpreamble:1;
-	u8 use_driver_rate:1;
-	u8 disable_ratefallback:1;
-
-	u8 ratr_index;
-	u8 mac_id;
-	u8 hw_rate;
-
-	u8 b_last_inipkt:1;
-	u8 b_cmd_or_init:1;
-	u8 queue_index;
-
-	/* early mode */
-	u8 empkt_num;
-	/* The max value by HW */
-	u32 empkt_len[10];
-	bool btx_enable_sw_calc_duration;
-	/* used for hal construct pkt,
-	 * we may set desc when tx */
-	u8 self_desc;
-};
-
-struct rtl_wow_pattern {
-	u8 type;
-	u16 crc;
-	u32 mask[4];
-};
-
-struct proxim {
-	bool proxim_on;
-
-	void *proximity_priv;
-	int (*proxim_rx)(struct ieee80211_hw *hw, struct rtl_stats *status,
-			 struct sk_buff *skb);
-	u8  (*proxim_get_var)(struct ieee80211_hw *hw, u8 type);
-};
-
-struct rtl_hal_ops {
-	int (*init_sw_vars)(struct ieee80211_hw *hw);
-	void (*deinit_sw_vars)(struct ieee80211_hw *hw);
-	void (*read_eeprom_info)(struct ieee80211_hw *hw);
-	void (*interrupt_recognized)(struct ieee80211_hw *hw,
-				     u32 *p_inta, u32 *p_intb);
-	int (*hw_init)(struct ieee80211_hw *hw);
-	void (*hw_disable)(struct ieee80211_hw *hw);
-	void (*hw_suspend)(struct ieee80211_hw *hw);
-	void (*hw_resume)(struct ieee80211_hw *hw);
-	void (*enable_interrupt)(struct ieee80211_hw *hw);
-	void (*disable_interrupt)(struct ieee80211_hw *hw);
-	int (*set_network_type)(struct ieee80211_hw *hw,
-				enum nl80211_iftype type);
-	void (*set_chk_bssid)(struct ieee80211_hw *hw,
-			      bool check_bssid);
-	void (*set_bw_mode)(struct ieee80211_hw *hw,
-			    enum nl80211_channel_type ch_type);
-	u8 (*switch_channel)(struct ieee80211_hw *hw);
-	void (*set_qos)(struct ieee80211_hw *hw, int aci);
-	void (*set_bcn_reg)(struct ieee80211_hw *hw);
-	void (*set_bcn_intv)(struct ieee80211_hw *hw);
-	void (*update_interrupt_mask)(struct ieee80211_hw *hw,
-				      u32 add_msr, u32 rm_msr);
-	void (*get_hw_reg)(struct ieee80211_hw *hw, u8 variable, u8 *val);
-	void (*set_hw_reg)(struct ieee80211_hw *hw, u8 variable, u8 *val);
-	void (*update_rate_tbl)(struct ieee80211_hw *hw,
-				struct ieee80211_sta *sta, u8 rssi_level);
-	void (*pre_fill_tx_bd_desc)(struct ieee80211_hw *hw, u8 *tx_bd_desc,
-				    u8 *desc, u8 queue_index,
-				    struct sk_buff *skb, dma_addr_t addr);
-	u16 (*rx_desc_buff_remained_cnt)(struct ieee80211_hw *hw,
-					 u8 queue_index);
-	void (*rx_check_dma_ok)(struct ieee80211_hw *hw, u8 *header_desc,
-				u8 queue_index);
-	void (*fill_tx_desc)(struct ieee80211_hw *hw,
-			     struct ieee80211_hdr *hdr,
-			     u8 *pdesc_tx, u8 *pbd_desc,
-			     struct ieee80211_tx_info *info,
-			     struct ieee80211_sta *sta,
-			     struct sk_buff *skb, u8 hw_queue,
-			     struct rtl_tcb_desc *ptcb_desc);
-	void (*fill_tx_cmddesc)(struct ieee80211_hw *hw, u8 *pdesc,
-				bool b_firstseg, bool b_lastseg,
-				struct sk_buff *skb);
-	bool (*query_rx_desc)(struct ieee80211_hw *hw,
-			      struct rtl_stats *status,
-			      struct ieee80211_rx_status *rx_status,
-			      u8 *pdesc, struct sk_buff *skb);
-	void (*set_channel_access)(struct ieee80211_hw *hw);
-	bool (*radio_onoff_checking)(struct ieee80211_hw *hw, u8 *valid);
-	void (*dm_watchdog)(struct ieee80211_hw *hw);
-	void (*scan_operation_backup)(struct ieee80211_hw *hw, u8 operation);
-	bool (*set_rf_power_state)(struct ieee80211_hw *hw,
-				   enum rf_pwrstate rfpwr_state);
-	void (*led_control)(struct ieee80211_hw *hw,
-			    enum led_ctl_mode ledaction);
-	void (*set_desc)(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-			 u8 desc_name, u8 *val);
-	u32 (*get_desc)(u8 *pdesc, bool istx, u8 desc_name);
-	bool (*is_tx_desc_closed)(struct ieee80211_hw *hw,
-				  u8 hw_queue, u16 index);
-	void (*tx_polling)(struct ieee80211_hw *hw, u8 hw_queue);
-	void (*enable_hw_sec)(struct ieee80211_hw *hw);
-	void (*set_key)(struct ieee80211_hw *hw, u32 key_index,
-			u8 *p_macaddr, bool is_group, u8 enc_algo,
-			bool is_wepkey, bool clear_all);
-	void (*init_sw_leds)(struct ieee80211_hw *hw);
-	u32 (*get_bbreg)(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask);
-	void (*set_bbreg)(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask,
-			  u32 data);
-	u32 (*get_rfreg)(struct ieee80211_hw *hw, enum radio_path rfpath,
-			 u32 regaddr, u32 bitmask);
-	void (*set_rfreg)(struct ieee80211_hw *hw, enum radio_path rfpath,
-			  u32 regaddr, u32 bitmask, u32 data);
-	void (*allow_all_destaddr)(struct ieee80211_hw *hw,
-				   bool allow_all_da, bool write_into_reg);
-	void (*linked_set_reg)(struct ieee80211_hw *hw);
-	void (*check_switch_to_dmdp)(struct ieee80211_hw *hw);
-	void (*dualmac_easy_concurrent)(struct ieee80211_hw *hw);
-	void (*dualmac_switch_to_dmdp)(struct ieee80211_hw *hw);
-	void (*c2h_command_handle)(struct ieee80211_hw *hw);
-	void (*bt_wifi_media_status_notify)(struct ieee80211_hw *hw,
-					    bool mstate);
-	void (*bt_turn_off_bt_coexist_before_enter_lps)(struct ieee80211_hw *w);
-	void (*fill_h2c_cmd)(struct ieee80211_hw *hw, u8 element_id,
-			     u32 cmd_len, u8 *p_cmdbuffer);
-	bool (*get_btc_status)(void);
-	u32 (*rx_command_packet)(struct ieee80211_hw *hw,
-				 const struct rtl_stats *status, struct sk_buff *skb);
-	void (*add_wowlan_pattern)(struct ieee80211_hw *hw,
-				   struct rtl_wow_pattern *rtl_pattern,
-				   u8 index);
-};
-
-struct rtl_intf_ops {
-	/*com */
-	void (*read92e_efuse_byte)(struct ieee80211_hw *hw, u16 _offset,
-				   u8 *pbuf);
-	int (*adapter_start)(struct ieee80211_hw *hw);
-	void (*adapter_stop)(struct ieee80211_hw *hw);
-	bool (*check_buddy_priv)(struct ieee80211_hw *hw,
-				 struct rtl_priv **buddy_priv);
-
-	int (*adapter_tx)(struct ieee80211_hw *hw,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb,
-			  struct rtl_tcb_desc *ptcb_desc);
-	void (*flush)(struct ieee80211_hw *hw, u32 queues, bool drop);
-	int (*reset_trx_ring)(struct ieee80211_hw *hw);
-	bool (*waitq_insert)(struct ieee80211_hw *hw,
-			     struct ieee80211_sta *sta,
-			     struct sk_buff *skb);
-
-	/*pci */
-	void (*disable_aspm)(struct ieee80211_hw *hw);
-	void (*enable_aspm)(struct ieee80211_hw *hw);
-
-	/*usb */
-};
-
-struct rtl_mod_params {
-	/* default: 0 = using hardware encryption */
-	bool sw_crypto;
-
-	/* default: 0 = DBG_EMERG (0)*/
-	int debug;
-
-	/* default: 1 = using no linked power save */
-	bool b_inactiveps;
-
-	/* default: 1 = using linked sw power save */
-	bool b_swctrl_lps;
-
-	/* default: 1 = using linked fw power save */
-	bool b_fwctrl_lps;
-};
-
-struct rtl_hal_cfg {
-	u8 bar_id;
-	bool write_readback;
-	char *name;
-	char *fw_name;
-	struct rtl_hal_ops *ops;
-	struct rtl_mod_params *mod_params;
-
-	/*this map used for some registers or vars
-	   defined int HAL but used in MAIN */
-	u32 maps[RTL_VAR_MAP_MAX];
-
-};
-
-struct rtl_locks {
-	/* mutex */
-	struct mutex conf_mutex;
-
-	/*spin lock */
-	spinlock_t ips_lock;
-	spinlock_t irq_th_lock;
-	spinlock_t h2c_lock;
-	spinlock_t rf_ps_lock;
-	spinlock_t rf_lock;
-	spinlock_t lps_lock;
-	spinlock_t waitq_lock;
-	spinlock_t entry_list_lock;
-
-	/*FW clock change */
-	spinlock_t fw_ps_lock;
-
-	/*Dul mac*/
-	spinlock_t cck_and_rw_pagea_lock;
-
-	/*Easy concurrent*/
-	spinlock_t check_sendpkt_lock;
-
-	spinlock_t iqk_lock;
-};
-
-struct rtl_works {
-	struct ieee80211_hw *hw;
-
-	/*timer */
-	struct timer_list watchdog_timer;
-	struct timer_list dualmac_easyconcurrent_retrytimer;
-	struct timer_list fw_clockoff_timer;
-	struct timer_list fast_antenna_trainning_timer;
-	/*task */
-	struct tasklet_struct irq_tasklet;
-	struct tasklet_struct irq_prepare_bcn_tasklet;
-
-	/*work queue */
-	struct workqueue_struct *rtl_wq;
-	struct delayed_work watchdog_wq;
-	struct delayed_work ips_nic_off_wq;
-
-	/* For SW LPS */
-	struct delayed_work ps_work;
-	struct delayed_work ps_rfon_wq;
-	struct delayed_work fwevt_wq;
-};
-
-struct rtl_debug {
-	u32 dbgp_type[DBGP_TYPE_MAX];
-	u32 global_debuglevel;
-	u64 global_debugcomponents;
-
-	/* add for proc debug */
-	struct proc_dir_entry *proc_dir;
-	char proc_name[20];
-};
-
-#define MIMO_PS_STATIC			0
-#define MIMO_PS_DYNAMIC			1
-#define MIMO_PS_NOLIMIT			3
-
-struct rtl_dualmac_easy_concurrent_ctl {
-	enum band_type currentbandtype_backfordmdp;
-	bool bclose_bbandrf_for_dmsp;
-	bool bchange_to_dmdp;
-	bool bchange_to_dmsp;
-	bool bswitch_in_process;
-};
-
-struct rtl_dmsp_ctl {
-	bool bactivescan_for_slaveofdmsp;
-	bool bscan_for_anothermac_fordmsp;
-	bool bscan_for_itself_fordmsp;
-	bool bwritedig_for_anothermacofdmsp;
-	u32 curdigvalue_for_anothermacofdmsp;
-	bool bchangecckpdstate_for_anothermacofdmsp;
-	u8 curcckpdstate_for_anothermacofdmsp;
-	bool bchangetxhighpowerlvl_for_anothermacofdmsp;
-	u8 curtxhighlvl_for_anothermacofdmsp;
-	long rssivalmin_for_anothermacofdmsp;
-};
-
-struct rtl_global_var {
-	/* from this list we can get
-	 * other adapter's rtl_priv */
-	struct list_head glb_priv_list;
-	spinlock_t glb_list_lock;
-};
-
-struct rtl_btc_info {
-	u8 bt_type;
-	u8 btcoexist;
-	u8 ant_num;
-};
-
-struct rtl_btc_ops {
-	void (*btc_init_variables)(struct rtl_priv *rtlpriv);
-	void (*btc_init_hal_vars)(struct rtl_priv *rtlpriv);
-	void (*btc_init_hw_config)(struct rtl_priv *rtlpriv);
-	void (*btc_ips_notify)(struct rtl_priv *rtlpriv, u8 type);
-	void (*btc_lps_notify)(struct rtl_priv *rtlpriv, u8 type);
-	void (*btc_scan_notify)(struct rtl_priv *rtlpriv, u8 scantype);
-	void (*btc_connect_notify)(struct rtl_priv *rtlpriv, u8 action);
-	void (*btc_mediastatus_notify)(struct rtl_priv *rtlpriv,
-				       enum rt_media_status mstatus);
-	void (*btc_periodical)(struct rtl_priv *rtlpriv);
-	void (*btc_halt_notify)(void);
-	void (*btc_btinfo_notify)(struct rtl_priv *rtlpriv,
-				  u8 *tmp_buf, u8 length);
-	bool (*btc_is_limited_dig)(struct rtl_priv *rtlpriv);
-	bool (*btc_is_disable_edca_turbo)(struct rtl_priv *rtlpriv);
-	bool (*btc_is_bt_disabled)(struct rtl_priv *rtlpriv);
-	void (*btc_special_packet_notify)(struct rtl_priv *rtlpriv,
-					  u8 pkt_type);
-};
-
-struct rtl_bt_coexist {
-	struct rtl_btc_ops *btc_ops;
-	struct rtl_btc_info btc_info;
-};
-
-
-struct rtl_priv {
-	struct list_head list;
-	struct rtl_priv *buddy_priv;
-	struct rtl_global_var *glb_var;
-	struct rtl_dualmac_easy_concurrent_ctl easy_concurrent_ctl;
-	struct rtl_dmsp_ctl dmsp_ctl;
-	struct rtl_locks locks;
-	struct rtl_works works;
-	struct rtl_mac mac80211;
-	struct rtl_hal rtlhal;
-	struct rtl_regulatory regd;
-	struct rtl_rfkill rfkill;
-	struct rtl_io io;
-	struct rtl_phy phy;
-	struct rtl_dm dm;
-	struct rtl_security sec;
-	struct rtl_efuse efuse;
-
-	struct rtl_ps_ctl psc;
-	struct rate_adaptive ra;
-	struct dynamic_primary_cca primarycca;
-	struct wireless_stats stats;
-	struct rt_link_detect link_info;
-	struct false_alarm_statistics falsealm_cnt;
-
-	struct rtl_rate_priv *rate_priv;
-
-	struct rtl_debug dbg;
-
-	/* sta entry list for ap adhoc or mesh */
-	struct list_head entry_list;
-
-	/*
-	 *hal_cfg : for diff cards
-	 *intf_ops : for diff interrface usb/pcie
-	 */
-	struct rtl_hal_cfg *cfg;
-	struct rtl_intf_ops *intf_ops;
-
-	/*this var will be set by set_bit,
-	   and was used to indicate status of
-	   interface or hardware */
-	unsigned long status;
-
-	/* intel Proximity, should be alloc mem
-	 * in intel Proximity module and can only
-	 * be used in intel Proximity mode */
-	struct proxim proximity;
-
-	/*for bt coexist use*/
-	struct rtl_bt_coexist btcoexist;
-
-	/* seperate 92ee from other ICs,
-	  * 92ee use new trx flow. */
-	bool use_new_trx_flow;
-
-#ifdef CONFIG_PM
-	struct wiphy_wowlan_support wowlan;
-#endif
-	/*This must be the last item so
-	   that it points to the data allocated
-	   beyond  this structure like:
-	   rtl_pci_priv or rtl_usb_priv */
-	u8 priv[0];
-};
-
-#define rtl_priv(hw)		(((struct rtl_priv *)(hw)->priv))
-#define rtl_mac(rtlpriv)	(&((rtlpriv)->mac80211))
-#define rtl_hal(rtlpriv)	(&((rtlpriv)->rtlhal))
-#define rtl_efuse(rtlpriv)	(&((rtlpriv)->efuse))
-#define rtl_psc(rtlpriv)	(&((rtlpriv)->psc))
-#define rtl_sec(rtlpriv)	(&((rtlpriv)->sec))
-#define rtl_dm(rtlpriv)	(&((rtlpriv)->dm))
-/***************************************
-    Bluetooth Co-existance Related
-****************************************/
-
-enum bt_ant_num {
-	ANT_X2 = 0,
-	ANT_X1 = 1,
-};
-
-enum bt_co_type {
-	BT_2WIRE = 0,
-	BT_ISSC_3WIRE = 1,
-	BT_ACCEL = 2,
-	BT_CSR_BC4 = 3,
-	BT_CSR_BC8 = 4,
-	BT_RTL8756 = 5,
-	BT_RTL8723A = 6,
-	BT_RTL8821A = 7,
-	BT_RTL8723B = 8,
-	BT_RTL8192E = 9,
-	BT_RTL8812A = 11,
-};
-
-enum bt_total_ant_num {
-	ANT_TOTAL_X2 = 0,
-	ANT_TOTAL_X1 = 1
-};
-
-enum bt_cur_state {
-	BT_OFF = 0,
-	BT_ON = 1,
-};
-
-enum bt_service_type {
-	BT_SCO = 0,
-	BT_A2DP = 1,
-	BT_HID = 2,
-	BT_HID_IDLE = 3,
-	BT_SCAN = 4,
-	BT_IDLE = 5,
-	BT_OTHER_ACTION = 6,
-	BT_BUSY = 7,
-	BT_OTHERBUSY = 8,
-	BT_PAN = 9,
-};
-
-enum bt_radio_shared {
-	BT_RADIO_SHARED = 0,
-	BT_RADIO_INDIVIDUAL = 1,
-};
-
-struct bt_coexist_info {
-	/* EEPROM BT info. */
-	u8 eeprom_bt_coexist;
-	u8 eeprom_bt_type;
-	u8 eeprom_bt_ant_num;
-	u8 eeprom_bt_ant_isolation;
-	u8 eeprom_bt_radio_shared;
-
-	u8 bt_coexistence;
-	u8 bt_ant_num;
-	u8 bt_coexist_type;
-	u8 bt_state;
-	u8 bt_cur_state;	/* 0:on, 1:off */
-	u8 bt_ant_isolation;	/* 0:good, 1:bad */
-	u8 bt_pape_ctrl;	/* 0:SW, 1:SW/HW dynamic */
-	u8 bt_service;
-	u8 bt_radio_shared_type;
-	u8 bt_rfreg_origin_1e;
-	u8 bt_rfreg_origin_1f;
-	u8 bt_rssi_state;
-	u32 ratio_tx;
-	u32 ratio_pri;
-	u32 bt_edca_ul;
-	u32 bt_edca_dl;
-
-	bool b_init_set;
-	bool b_bt_busy_traffic;
-	bool b_bt_traffic_mode_set;
-	bool b_bt_non_traffic_mode_set;
-
-	bool b_fw_coexist_all_off;
-	bool b_sw_coexist_all_off;
-	bool b_hw_coexist_all_off;
-	u32 current_state;
-	u32 previous_state;
-	u32 current_state_h;
-	u32 previous_state_h;
-
-	u8 bt_pre_rssi_state;
-	u8 bt_pre_rssi_state1;
-
-	u8 b_reg_bt_iso;
-	u8 b_reg_bt_sco;
-	bool b_balance_on;
-	u8 bt_active_zero_cnt;
-	bool b_cur_bt_disabled;
-	bool b_pre_bt_disabled;
-
-	u8 bt_profile_case;
-	u8 bt_profile_action;
-	bool b_bt_busy;
-	bool b_hold_for_bt_operation;
-	u8 lps_counter;
-};
-
-/****************************************
-	mem access macro define start
-	Call endian free function when
-	1. Read/write packet content.
-	2. Before write integer to IO.
-	3. After read integer from IO.
-****************************************/
-/* Convert little data endian to host ordering */
-#define EF1BYTE(_val)		\
-	((u8)(_val))
-#define EF2BYTE(_val)		\
-	(le16_to_cpu(_val))
-#define EF4BYTE(_val)		\
-	(le32_to_cpu(_val))
-
-/* Read data from memory */
-#define READEF1BYTE(_ptr)	\
-	EF1BYTE(*((u8 *)(_ptr)))
-/* Read le16 data from memory and convert to host ordering */
-#define READEF2BYTE(_ptr)	\
-	EF2BYTE(*(_ptr))
-#define READEF4BYTE(_ptr)	\
-	EF4BYTE(*(_ptr))
-
-/* Write data to memory */
-#define WRITEEF1BYTE(_ptr, _val)	\
-	(*((u8 *)(_ptr))) = EF1BYTE(_val)
-/* Write le16 data to memory in host ordering */
-#define WRITEEF2BYTE(_ptr, _val)	\
-	(*((u16 *)(_ptr))) = EF2BYTE(_val)
-#define WRITEEF4BYTE(_ptr, _val)	\
-	(*((u32 *)(_ptr))) = EF2BYTE(_val)
-
-/* Create a bit mask
- * Examples:
- * BIT_LEN_MASK_32(0) => 0x00000000
- * BIT_LEN_MASK_32(1) => 0x00000001
- * BIT_LEN_MASK_32(2) => 0x00000003
- * BIT_LEN_MASK_32(32) => 0xFFFFFFFF
- */
-#define BIT_LEN_MASK_32(__bitlen)	 \
-	(0xFFFFFFFF >> (32 - (__bitlen)))
-#define BIT_LEN_MASK_16(__bitlen)	 \
-	(0xFFFF >> (16 - (__bitlen)))
-#define BIT_LEN_MASK_8(__bitlen) \
-	(0xFF >> (8 - (__bitlen)))
-
-/* Create an offset bit mask
- * Examples:
- * BIT_OFFSET_LEN_MASK_32(0, 2) => 0x00000003
- * BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000
- */
-#define BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen) \
-	(BIT_LEN_MASK_32(__bitlen) << (__bitoffset))
-#define BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen) \
-	(BIT_LEN_MASK_16(__bitlen) << (__bitoffset))
-#define BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen) \
-	(BIT_LEN_MASK_8(__bitlen) << (__bitoffset))
-
-/*Description:
- * Return 4-byte value in host byte ordering from
- * 4-byte pointer in little-endian system.
- */
-#define LE_P4BYTE_TO_HOST_4BYTE(__pstart) \
-	(EF4BYTE(*((__le32 *)(__pstart))))
-#define LE_P2BYTE_TO_HOST_2BYTE(__pstart) \
-	(EF2BYTE(*((__le16 *)(__pstart))))
-#define LE_P1BYTE_TO_HOST_1BYTE(__pstart) \
-	(EF1BYTE(*((u8 *)(__pstart))))
-
-/*Description:
-Translate subfield (continuous bits in little-endian) of 4-byte
-value to host byte ordering.*/
-#define LE_BITS_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P4BYTE_TO_HOST_4BYTE(__pstart) >> (__bitoffset))  & \
-		BIT_LEN_MASK_32(__bitlen) \
-	)
-#define LE_BITS_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P2BYTE_TO_HOST_2BYTE(__pstart) >> (__bitoffset)) & \
-		BIT_LEN_MASK_16(__bitlen) \
-	)
-#define LE_BITS_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P1BYTE_TO_HOST_1BYTE(__pstart) >> (__bitoffset)) & \
-		BIT_LEN_MASK_8(__bitlen) \
-	)
-
-/* Description:
- * Mask subfield (continuous bits in little-endian) of 4-byte value
- * and return the result in 4-byte value in host byte ordering.
- */
-#define LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		LE_P4BYTE_TO_HOST_4BYTE(__pstart)  & \
-		(~BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen)) \
-	)
-#define LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		LE_P2BYTE_TO_HOST_2BYTE(__pstart) & \
-		(~BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen)) \
-	)
-#define LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		LE_P1BYTE_TO_HOST_1BYTE(__pstart) & \
-		(~BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen)) \
-	)
-
-/* Description:
- * Set subfield of little-endian 4-byte value to specified value.
- */
-#define SET_BITS_TO_LE_4BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u32 *)(__pstart)) = \
-	( \
-		LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u32)__val) & BIT_LEN_MASK_32(__bitlen)) << (__bitoffset)) \
-	);
-#define SET_BITS_TO_LE_2BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u16 *)(__pstart)) = \
-	( \
-		LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u16)__val) & BIT_LEN_MASK_16(__bitlen)) << (__bitoffset)) \
-	);
-#define SET_BITS_TO_LE_1BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u8 *)(__pstart)) = EF1BYTE \
-	( \
-		LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u8)__val) & BIT_LEN_MASK_8(__bitlen)) << (__bitoffset)) \
-	);
-
-#define	N_BYTE_ALIGMENT(__value, __aligment) ((__aligment == 1) ? \
-	(__value) : (((__value + __aligment - 1) / __aligment) * __aligment))
-
-/****************************************
-	mem access macro define end
-****************************************/
-
-#define byte(x , n) ((x >> (8 * n)) & 0xff)
-
-#define packet_get_type(_packet) (EF1BYTE((_packet).octet[0]) & 0xFC)
-#define RTL_WATCH_DOG_TIME	2000
-#define MSECS(t)		msecs_to_jiffies(t)
-#define WLAN_FC_GET_VERS(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_VERS)
-#define WLAN_FC_GET_TYPE(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_FTYPE)
-#define WLAN_FC_GET_STYPE(fc)	(le16_to_cou(fc) & IEEE80211_FCTL_STYPE)
-#define WLAN_FC_MORE_DATA(fc)	(le16_to_cpu(fc) & IEEE80211_FCTL_MOREDATA)
-#define SEQ_TO_SN(seq)		(((seq) & IEEE80211_SCTL_SEQ) >> 4)
-#define SN_TO_SEQ(ssn)		(((ssn) << 4) & IEEE80211_SCTL_SEQ)
-#define MAX_SN			((IEEE80211_SCTL_SEQ) >> 4)
-
-#define	RT_RF_OFF_LEVL_ASPM		BIT(0)	/*PCI ASPM */
-#define	RT_RF_OFF_LEVL_CLK_REQ		BIT(1)	/*PCI clock request */
-#define	RT_RF_OFF_LEVL_PCI_D3		BIT(2)	/*PCI D3 mode */
-/*NIC halt, re-initialize hw parameters*/
-#define	RT_RF_OFF_LEVL_HALT_NIC		BIT(3)
-#define	RT_RF_OFF_LEVL_FREE_FW		BIT(4)	/*FW free, re-download the FW */
-#define	RT_RF_OFF_LEVL_FW_32K		BIT(5)	/*FW in 32k */
-/*Always enable ASPM and Clock Req in initialization.*/
-#define	RT_RF_PS_LEVEL_ALWAYS_ASPM	BIT(6)
-/* no matter RFOFF or SLEEP we set PS_ASPM_LEVL*/
-#define	RT_PS_LEVEL_ASPM		BIT(7)
-/*When LPS is on, disable 2R if no packet is received or transmittd.*/
-#define	RT_RF_LPS_DISALBE_2R		BIT(30)
-#define	RT_RF_LPS_LEVEL_ASPM		BIT(31)	/*LPS with ASPM */
-#define	RT_IN_PS_LEVEL(ppsc, _ps_flg)		\
-	((ppsc->cur_ps_level & _ps_flg) ? true : false)
-#define	RT_CLEAR_PS_LEVEL(ppsc, _ps_flg)	\
-	(ppsc->cur_ps_level &= (~(_ps_flg)))
-#define	RT_SET_PS_LEVEL(ppsc, _ps_flg)		\
-	(ppsc->cur_ps_level |= _ps_flg)
-
-#define container_of_dwork_rtl(x , y , z) \
-	container_of(container_of(x, struct delayed_work, work), y, z)
-
-#define FILL_OCTET_STRING(_os , _octet , _len)	\
-		(_os).octet = (u8 *)(_octet);		\
-		(_os).length = (_len);
-
-#define CP_MACADDR(des , src)	\
-	((des)[0] = (src)[0] , (des)[1] = (src)[1],\
-	(des)[2] = (src)[2] , (des)[3] = (src)[3],\
-	(des)[4] = (src)[4] , (des)[5] = (src)[5])
-
-#define	LDPC_HT_ENABLE_RX			BIT(0)
-#define	LDPC_HT_ENABLE_TX			BIT(1)
-#define	LDPC_HT_TEST_TX_ENABLE			BIT(2)
-#define	LDPC_HT_CAP_TX				BIT(3)
-
-#define	STBC_HT_ENABLE_RX			BIT(0)
-#define	STBC_HT_ENABLE_TX			BIT(1)
-#define	STBC_HT_TEST_TX_ENABLE			BIT(2)
-#define	STBC_HT_CAP_TX				BIT(3)
-
-
-#define	LDPC_VHT_ENABLE_RX			BIT(0)
-#define	LDPC_VHT_ENABLE_TX			BIT(1)
-#define	LDPC_VHT_TEST_TX_ENABLE			BIT(2)
-#define	LDPC_VHT_CAP_TX				BIT(3)
-
-#define	STBC_VHT_ENABLE_RX			BIT(0)
-#define	STBC_VHT_ENABLE_TX			BIT(1)
-#define	STBC_VHT_TEST_TX_ENABLE			BIT(2)
-#define	STBC_VHT_CAP_TX				BIT(3)
-
-
-static inline u8 rtl_read_byte(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return rtlpriv->io.read8_sync(rtlpriv, addr);
-}
-
-static inline u16 rtl_read_word(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return rtlpriv->io.read16_sync(rtlpriv, addr);
-}
-
-static inline u32 rtl_read_dword(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return rtlpriv->io.read32_sync(rtlpriv, addr);
-}
-
-static inline void rtl_write_byte(struct rtl_priv *rtlpriv, u32 addr, u8 val8)
-{
-	rtlpriv->io.write8_async(rtlpriv, addr, val8);
-
-	if (rtlpriv->cfg->write_readback)
-		rtlpriv->io.read8_sync(rtlpriv, addr);
-}
-
-static inline void rtl_write_word(struct rtl_priv *rtlpriv, u32 addr, u16 val16)
-{
-	rtlpriv->io.write16_async(rtlpriv, addr, val16);
-
-	if (rtlpriv->cfg->write_readback)
-		rtlpriv->io.read16_sync(rtlpriv, addr);
-}
-
-static inline void rtl_write_dword(struct rtl_priv *rtlpriv,
-				   u32 addr, u32 val32)
-{
-	rtlpriv->io.write32_async(rtlpriv, addr, val32);
-
-	if (rtlpriv->cfg->write_readback)
-		rtlpriv->io.read32_sync(rtlpriv, addr);
-}
-
-static inline u32 rtl_get_bbreg(struct ieee80211_hw *hw,
-				u32 regaddr, u32 bitmask)
-{
-	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_bbreg(hw,
-								    regaddr,
-								    bitmask);
-}
-
-static inline void rtl_set_bbreg(struct ieee80211_hw *hw, u32 regaddr,
-				 u32 bitmask, u32 data)
-{
-	struct rtl_priv *rtlpriv = hw->priv;
-
-	rtlpriv->cfg->ops->set_bbreg(hw, regaddr, bitmask, data);
-}
-
-static inline u32 rtl_get_rfreg(struct ieee80211_hw *hw,
-				enum radio_path rfpath, u32 regaddr,
-				u32 bitmask)
-{
-	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_rfreg(hw,
-								    rfpath,
-								    regaddr,
-								    bitmask);
-}
-
-static inline void rtl_set_rfreg(struct ieee80211_hw *hw,
-				 enum radio_path rfpath, u32 regaddr,
-				 u32 bitmask, u32 data)
-{
-	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_rfreg(hw,
-							     rfpath, regaddr,
-							     bitmask, data);
-}
-
-static inline bool is_hal_stop(struct rtl_hal *rtlhal)
-{
-	return _HAL_STATE_STOP == rtlhal->state;
-}
-
-static inline void set_hal_start(struct rtl_hal *rtlhal)
-{
-	rtlhal->state = _HAL_STATE_START;
-}
-
-static inline void set_hal_stop(struct rtl_hal *rtlhal)
-{
-	rtlhal->state = _HAL_STATE_STOP;
-}
-
-static inline u8 get_rf_type(struct rtl_phy *rtlphy)
-{
-	return rtlphy->rf_type;
-}
-
-static inline struct ieee80211_hdr *rtl_get_hdr(struct sk_buff *skb)
-{
-	return (struct ieee80211_hdr *)(skb->data);
-}
-
-static inline __le16 rtl_get_fc(struct sk_buff *skb)
-{
-	return rtl_get_hdr(skb)->frame_control;
-}
-
-static inline u16 rtl_get_tid_h(struct ieee80211_hdr *hdr)
-{
-	return (ieee80211_get_qos_ctl(hdr))[0] & IEEE80211_QOS_CTL_TID_MASK;
-}
-
-static inline u16 rtl_get_tid(struct sk_buff *skb)
-{
-	return rtl_get_tid_h(rtl_get_hdr(skb));
-}
-
-static inline struct ieee80211_sta *rtl_find_sta(struct ieee80211_hw *hw,
-						 u8 *mac_addr)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	return ieee80211_find_sta(mac->vif, mac_addr);
-}
-
-struct ieee80211_hw *rtl_pci_get_hw_pointer(void);
-#endif
diff --git a/drivers/staging/rtl8192u/Makefile b/drivers/staging/rtl8192u/Makefile
index eefc657ce99e..703c1505ea5f 100644
--- a/drivers/staging/rtl8192u/Makefile
+++ b/drivers/staging/rtl8192u/Makefile
@@ -6,9 +6,6 @@ ccflags-y += -O2
 ccflags-y += -DCONFIG_FORCE_HARD_FLOAT=y
 ccflags-y += -DJACKSON_NEW_8187 -DJACKSON_NEW_RX
 ccflags-y += -DTHOMAS_BEACON -DTHOMAS_TASKLET -DTHOMAS_SKB -DTHOMAS_TURBO
-#ccflags-y += -DUSB_TX_DRIVER_AGGREGATION_ENABLE
-#ccflags-y += -DUSB_RX_AGGREGATION_SUPPORT
-ccflags-y += -DUSE_ONE_PIPE
 ccflags-y += -Idrivers/staging/rtl8192u/ieee80211
 
 r8192u_usb-y := r8192U_core.o r8180_93cx6.o r8192U_wx.o		\
diff --git a/drivers/staging/rtl8192u/copying b/drivers/staging/rtl8192u/copying
index d60c31a97a54..e90dfed1a31e 100644
--- a/drivers/staging/rtl8192u/copying
+++ b/drivers/staging/rtl8192u/copying
@@ -55,7 +55,7 @@ patent must be licensed for everyone's free use or not licensed at all.
 
   The precise terms and conditions for copying, distribution and
 modification follow.
-
+
 		    GNU GENERAL PUBLIC LICENSE
    TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
 
@@ -110,7 +110,7 @@ above, provided that you also meet all of these conditions:
     License.  (Exception: if the Program itself is interactive but
     does not normally print such an announcement, your work based on
     the Program is not required to print an announcement.)
-
+
 These requirements apply to the modified work as a whole.  If
 identifiable sections of that work are not derived from the Program,
 and can be reasonably considered independent and separate works in
@@ -168,7 +168,7 @@ access to copy from a designated place, then offering equivalent
 access to copy the source code from the same place counts as
 distribution of the source code, even though third parties are not
 compelled to copy the source along with the object code.
-
+
   4. You may not copy, modify, sublicense, or distribute the Program
 except as expressly provided under this License.  Any attempt
 otherwise to copy, modify, sublicense or distribute the Program is
@@ -225,7 +225,7 @@ impose that choice.
 
 This section is intended to make thoroughly clear what is believed to
 be a consequence of the rest of this License.
-
+
   8. If the distribution and/or use of the Program is restricted in
 certain countries either by patents or by copyrighted interfaces, the
 original copyright holder who places the Program under this License
@@ -278,7 +278,7 @@ PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
 POSSIBILITY OF SUCH DAMAGES.
 
 		     END OF TERMS AND CONDITIONS
-
+
 	    How to Apply These Terms to Your New Programs
 
   If you develop a new program, and you want it to be of the greatest
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211.h b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
index 1040bab9702a..b44aa17d30a7 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211.h
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211.h
@@ -1211,7 +1211,7 @@ typedef union _frameqos {
 		u16 reserved:1;
 		u16 txop:8;
 	}field;
-}frameqos,*pframeqos;
+} frameqos, *pframeqos;
 
 /* SWEEP TABLE ENTRIES NUMBER*/
 #define MAX_SWEEP_TAB_ENTRIES		  42
@@ -1648,7 +1648,7 @@ typedef struct _bandwidth_autoswitch {
 	long	threshold_40Mhzto20Mhz;
 	bool bforced_tx20Mhz;
 	bool bautoswitch_enable;
-}bandwidth_autoswitch,*pbandwidth_autoswitch;
+} bandwidth_autoswitch, *pbandwidth_autoswitch;
 
 
 //added by amy for order
@@ -1745,7 +1745,7 @@ typedef struct _RT_POWER_SAVE_CONTROL {
 	//
 	bool				bLeisurePs;
 
-}RT_POWER_SAVE_CONTROL,*PRT_POWER_SAVE_CONTROL;
+} RT_POWER_SAVE_CONTROL, *PRT_POWER_SAVE_CONTROL;
 
 typedef u32 RT_RF_CHANGE_SOURCE;
 #define RF_CHANGE_BY_SW BIT31
@@ -2118,7 +2118,7 @@ struct ieee80211_device {
 	 * This function can't sleep.
 	 */
 	void (*softmac_data_hard_start_xmit)(struct sk_buff *skb,
-			       struct net_device *dev,int rate);
+			       struct net_device *dev, int rate);
 
 	/* stops the HW queue for DATA frames. Useful to avoid
 	 * waste time to TX data frame when we are reassociating
@@ -2133,7 +2133,7 @@ struct ieee80211_device {
 	 * This function can sleep. the driver should ensure
 	 * the radio has been swithced before return.
 	 */
-	void (*set_chan)(struct net_device *dev,short ch);
+	void (*set_chan)(struct net_device *dev, short ch);
 
 	/* These are not used if the ieee stack takes care of
 	 * scanning (IEEE_SOFTMAC_SCAN feature set).
@@ -2445,6 +2445,15 @@ extern void ieee80211_tkip_null(void);
 extern void ieee80211_wep_null(void);
 extern void ieee80211_ccmp_null(void);
 
+int ieee80211_crypto_init(void);
+void ieee80211_crypto_deinit(void);
+int ieee80211_crypto_tkip_init(void);
+void ieee80211_crypto_tkip_exit(void);
+int ieee80211_crypto_ccmp_init(void);
+void ieee80211_crypto_ccmp_exit(void);
+int ieee80211_crypto_wep_init(void);
+void ieee80211_crypto_wep_exit(void);
+
 /* ieee80211_softmac_wx.c */
 
 extern int ieee80211_wx_get_wap(struct ieee80211_device *ieee,
@@ -2485,6 +2494,10 @@ extern int ieee80211_wx_set_freq(struct ieee80211_device *ieee, struct iw_reques
 extern int ieee80211_wx_get_freq(struct ieee80211_device *ieee, struct iw_request_info *a,
 			     union iwreq_data *wrqu, char *b);
 
+/* ieee80211_module.c */
+extern int ieee80211_debug_init(void);
+extern void ieee80211_debug_exit(void);
+
 //extern void ieee80211_wx_sync_scan_wq(struct ieee80211_device *ieee);
 extern void ieee80211_wx_sync_scan_wq(struct work_struct *work);
 
@@ -2535,18 +2548,22 @@ extern u8 HTCCheck(struct ieee80211_device *ieee, u8 *pFrame);
 extern void HTResetIOTSetting(PRT_HIGH_THROUGHPUT  pHTInfo);
 extern bool IsHTHalfNmodeAPs(struct ieee80211_device *ieee);
 extern u16 HTHalfMcsToDataRate(struct ieee80211_device *ieee,  u8      nMcsRate);
-extern u16 HTMcsToDataRate( struct ieee80211_device *ieee, u8 nMcsRate);
-extern u16  TxCountToDataRate( struct ieee80211_device *ieee, u8 nDataRate);
+extern u16 HTMcsToDataRate(struct ieee80211_device *ieee, u8 nMcsRate);
+extern u16  TxCountToDataRate(struct ieee80211_device *ieee, u8 nDataRate);
 //function in BAPROC.c
-extern int ieee80211_rx_ADDBAReq( struct ieee80211_device *ieee, struct sk_buff *skb);
-extern int ieee80211_rx_ADDBARsp( struct ieee80211_device *ieee, struct sk_buff *skb);
+extern int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee,
+				 struct sk_buff *skb);
+extern int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee,
+				 struct sk_buff *skb);
 extern int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb);
-extern void TsInitAddBA( struct ieee80211_device *ieee, PTX_TS_RECORD   pTS, u8 Policy, u8 bOverwritePending);
-extern void TsInitDelBA( struct ieee80211_device *ieee, PTS_COMMON_INFO pTsCommonInfo, TR_SELECT TxRxSelect);
+extern void TsInitAddBA(struct ieee80211_device *ieee, PTX_TS_RECORD pTS,
+			u8 Policy, u8 bOverwritePending);
+extern void TsInitDelBA(struct ieee80211_device *ieee,
+			PTS_COMMON_INFO pTsCommonInfo, TR_SELECT TxRxSelect);
 extern void BaSetupTimeOut(unsigned long data);
 extern void TxBaInactTimeout(unsigned long data);
 extern void RxBaInactTimeout(unsigned long data);
-extern void ResetBaEntry( PBA_RECORD pBA);
+extern void ResetBaEntry(PBA_RECORD pBA);
 //function in TS.c
 extern bool GetTs(
 	struct ieee80211_device		*ieee,
@@ -2576,25 +2593,13 @@ static inline int ieee80211_get_scans(struct ieee80211_device *ieee)
 
 static inline const char *escape_essid(const char *essid, u8 essid_len) {
 	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
-	const char *s = essid;
-	char *d = escaped;
 
 	if (ieee80211_is_empty_essid(essid, essid_len)) {
 		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
 		return escaped;
 	}
 
-	essid_len = min(essid_len, (u8)IW_ESSID_MAX_SIZE);
-	while (essid_len--) {
-		if (*s == '\0') {
-			*d++ = '\\';
-			*d++ = '0';
-			s++;
-		} else {
-			*d++ = *s++;
-		}
-	}
-	*d = '\0';
+	snprintf(escaped, sizeof(escaped), "%*pEn", essid_len, essid);
 	return escaped;
 }
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
index af2128b956b7..143b682477e5 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_crypt_ccmp.c
@@ -57,7 +57,7 @@ struct ieee80211_ccmp_data {
 	u8 rx_b0[AES_BLOCK_LEN], rx_b[AES_BLOCK_LEN], rx_a[AES_BLOCK_LEN];
 };
 
-void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
+static void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
 			     const u8 pt[16], u8 ct[16])
 {
 	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
index 4b036a8db5a1..afbd09d7300a 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_module.c
@@ -99,7 +99,7 @@ struct net_device *alloc_ieee80211(int sizeof_priv)
 {
 	struct ieee80211_device *ieee;
 	struct net_device *dev;
-	int i,err;
+	int i, err;
 
 	IEEE80211_DEBUG_INFO("Initializing...\n");
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
index 73410ccfb1ec..73de9e9669f6 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_rx.c
@@ -65,7 +65,7 @@ static inline void ieee80211_monitor_rx(struct ieee80211_device *ieee,
 /* Called only as a tasklet (software IRQ) */
 static struct ieee80211_frag_entry *
 ieee80211_frag_cache_find(struct ieee80211_device *ieee, unsigned int seq,
-			  unsigned int frag, u8 tid,u8 *src, u8 *dst)
+			  unsigned int frag, u8 tid, u8 *src, u8 *dst)
 {
 	struct ieee80211_frag_entry *entry;
 	int i;
@@ -188,7 +188,7 @@ static int ieee80211_frag_cache_invalidate(struct ieee80211_device *ieee,
 	  tid = 0;
 	}
 
-	entry = ieee80211_frag_cache_find(ieee, seq, -1, tid,hdr->addr2,
+	entry = ieee80211_frag_cache_find(ieee, seq, -1, tid, hdr->addr2,
 					  hdr->addr1);
 
 	if (entry == NULL) {
@@ -767,7 +767,7 @@ static void RxReorderIndicatePacket(struct ieee80211_device *ieee,
 
 static u8 parse_subframe(struct sk_buff *skb,
 			 struct ieee80211_rx_stats *rx_stats,
-			 struct ieee80211_rxb *rxb,u8 *src,u8 *dst)
+			 struct ieee80211_rxb *rxb, u8 *src, u8 *dst)
 {
 	struct ieee80211_hdr_3addr  *hdr = (struct ieee80211_hdr_3addr *)skb->data;
 	u16		fc = le16_to_cpu(hdr->frame_ctl);
@@ -830,7 +830,7 @@ static u8 parse_subframe(struct sk_buff *skb,
 
 			if(skb->len<(ETHERNET_HEADER_SIZE + nSubframe_Length)) {
 				printk("%s: A-MSDU parse error!! pRfd->nTotalSubframe : %d\n",\
-						__func__,rxb->nr_subframes);
+						__func__, rxb->nr_subframes);
 				printk("%s: A-MSDU parse error!! Subframe Length: %d\n",__func__, nSubframe_Length);
 				printk("nRemain_Length is %d and nSubframe_Length is : %d\n",skb->len,nSubframe_Length);
 				printk("The Packet SeqNum is %d\n",SeqNum);
@@ -847,16 +847,18 @@ static u8 parse_subframe(struct sk_buff *skb,
 #else
 			/* Allocate new skb for releasing to upper layer */
 			sub_skb = dev_alloc_skb(nSubframe_Length + 12);
+			if (!sub_skb)
+				return 0;
 			skb_reserve(sub_skb, 12);
 			data_ptr = (u8 *)skb_put(sub_skb, nSubframe_Length);
-			memcpy(data_ptr,skb->data,nSubframe_Length);
+			memcpy(data_ptr, skb->data, nSubframe_Length);
 #endif
 			rxb->subframes[rxb->nr_subframes++] = sub_skb;
 			if(rxb->nr_subframes >= MAX_SUBFRAME_COUNT) {
 				IEEE80211_DEBUG_RX("ParseSubframe(): Too many Subframes! Packets dropped!\n");
 				break;
 			}
-			skb_pull(skb,nSubframe_Length);
+			skb_pull(skb, nSubframe_Length);
 
 			if(skb->len != 0) {
 				nPadding_Length = 4 - ((nSubframe_Length + ETHERNET_HEADER_SIZE) % 4);
@@ -868,7 +870,7 @@ static u8 parse_subframe(struct sk_buff *skb,
 					return 0;
 				}
 
-				skb_pull(skb,nPadding_Length);
+				skb_pull(skb, nPadding_Length);
 			}
 		}
 #ifdef JOHN_NOCPY
@@ -1297,7 +1299,7 @@ int ieee80211_rx(struct ieee80211_device *ieee, struct sk_buff *skb,
 	}
 	/* to parse amsdu packets */
 	/* qos data packets & reserved bit is 1 */
-	if(parse_subframe(skb,rx_stats,rxb,src,dst) == 0) {
+	if (parse_subframe(skb, rx_stats, rxb, src, dst) == 0) {
 		/* only to free rxb, and not submit the packets to upper layer */
 		for(i =0; i < rxb->nr_subframes; i++) {
 			dev_kfree_skb(rxb->subframes[i]);
@@ -2384,11 +2386,11 @@ static inline void update_network(struct ieee80211_network *dst,
 	//added by amy for LEAP
 	dst->bWithAironetIE = src->bWithAironetIE;
 	dst->bCkipSupported = src->bCkipSupported;
-	memcpy(dst->CcxRmState,src->CcxRmState,2);
+	memcpy(dst->CcxRmState, src->CcxRmState, 2);
 	dst->bCcxRmEnable = src->bCcxRmEnable;
 	dst->MBssidMask = src->MBssidMask;
 	dst->bMBssidValid = src->bMBssidValid;
-	memcpy(dst->MBssid,src->MBssid,6);
+	memcpy(dst->MBssid, src->MBssid, 6);
 	dst->bWithCcxVerNum = src->bWithCcxVerNum;
 	dst->BssCcxVerNumber = src->BssCcxVerNumber;
 
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
index c5a0a5da97da..a85bb232be97 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac.c
@@ -124,7 +124,8 @@ static void ieee80211_WMM_Info(struct ieee80211_device *ieee, u8 **tag_p)
 }
 
 #ifdef THOMAS_TURBO
-void ieee80211_TURBO_Info(struct ieee80211_device *ieee, u8 **tag_p) {
+static void ieee80211_TURBO_Info(struct ieee80211_device *ieee, u8 **tag_p)
+{
 	u8 *tag = *tag_p;
 
 	*tag++ = MFIE_TYPE_GENERIC; //0
@@ -228,7 +229,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee
 	spin_lock_irqsave(&ieee->lock, flags);
 
 	/* called with 2nd param 0, no mgmt lock required */
-	ieee80211_sta_wakeup(ieee,0);
+	ieee80211_sta_wakeup(ieee, 0);
 
 	tcb_desc->queue_index = MGNT_QUEUE;
 	tcb_desc->data_rate = MgntQuery_MgntFrameTxRate(ieee);
@@ -238,7 +239,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee
 
 	if(single){
 		if(ieee->queue_stop){
-			enqueue_mgmt(ieee,skb);
+			enqueue_mgmt(ieee, skb);
 		}else{
 			header->seq_ctl = cpu_to_le16(ieee->seq_ctrl[0]<<4);
 
@@ -277,7 +278,7 @@ inline void softmac_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *ieee
 			skb_queue_tail(&ieee->skb_waitQ[tcb_desc->queue_index], skb);
 		} else {
 			//printk("TX packet!\n");
-			ieee->softmac_hard_start_xmit(skb,ieee->dev);
+			ieee->softmac_hard_start_xmit(skb, ieee->dev);
 			//dev_kfree_skb_any(skb);//edit by thomas
 		}
 		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags);
@@ -314,7 +315,7 @@ inline void softmac_ps_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *i
 		else
 			ieee->seq_ctrl[0]++;
 
-		ieee->softmac_hard_start_xmit(skb,ieee->dev);
+		ieee->softmac_hard_start_xmit(skb, ieee->dev);
 
 	}
 	//dev_kfree_skb_any(skb);//edit by thomas
@@ -322,7 +323,7 @@ inline void softmac_ps_mgmt_xmit(struct sk_buff *skb, struct ieee80211_device *i
 
 inline struct sk_buff *ieee80211_probe_req(struct ieee80211_device *ieee)
 {
-	unsigned int len,rate_len;
+	unsigned int len, rate_len;
 	u8 *tag;
 	struct sk_buff *skb;
 	struct ieee80211_probe_request *req;
@@ -542,19 +543,19 @@ static void ieee80211_beacons_start(struct ieee80211_device *ieee)
 	ieee->beacon_txing = 1;
 	ieee80211_send_beacon(ieee);
 
-	spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
 }
 
 static void ieee80211_beacons_stop(struct ieee80211_device *ieee)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&ieee->beacon_lock,flags);
+	spin_lock_irqsave(&ieee->beacon_lock, flags);
 
 	ieee->beacon_txing = 0;
 	del_timer_sync(&ieee->beacon_timer);
 
-	spin_unlock_irqrestore(&ieee->beacon_lock,flags);
+	spin_unlock_irqrestore(&ieee->beacon_lock, flags);
 
 }
 
@@ -571,7 +572,7 @@ EXPORT_SYMBOL(ieee80211_stop_send_beacons);
 void ieee80211_start_send_beacons(struct ieee80211_device *ieee)
 {
 	if(ieee->start_send_beacons)
-		ieee->start_send_beacons(ieee->dev,ieee->basic_rate);
+		ieee->start_send_beacons(ieee->dev, ieee->basic_rate);
 	if(ieee->softmac_features & IEEE_SOFTMAC_BEACONS)
 		ieee80211_beacons_start(ieee);
 }
@@ -693,7 +694,7 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 	struct ieee80211_probe_response *beacon_buf;
 	struct sk_buff *skb = NULL;
 	int encrypt;
-	int atim_len,erp_len;
+	int atim_len, erp_len;
 	struct ieee80211_crypt_data *crypt;
 
 	char *ssid = ieee->current_network.ssid;
@@ -795,7 +796,7 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 
 	*(tag++) = MFIE_TYPE_RATES;
 	*(tag++) = rate_len-2;
-	memcpy(tag,ieee->current_network.rates,rate_len-2);
+	memcpy(tag, ieee->current_network.rates, rate_len-2);
 	tag+=rate_len-2;
 
 	*(tag++) = MFIE_TYPE_DS_SET;
@@ -820,7 +821,7 @@ static struct sk_buff *ieee80211_probe_resp(struct ieee80211_device *ieee, u8 *d
 	if(rate_ex_len){
 		*(tag++) = MFIE_TYPE_RATES_EX;
 		*(tag++) = rate_ex_len-2;
-		memcpy(tag,ieee->current_network.rates_ex,rate_ex_len-2);
+		memcpy(tag, ieee->current_network.rates_ex, rate_ex_len-2);
 		tag+=rate_ex_len-2;
 	}
 
@@ -860,7 +861,7 @@ static struct sk_buff *ieee80211_assoc_resp(struct ieee80211_device *ieee,
 	skb_reserve(skb, ieee->tx_headroom);
 
 	assoc = (struct ieee80211_assoc_response_frame *)
-		skb_put(skb,sizeof(struct ieee80211_assoc_response_frame));
+		skb_put(skb, sizeof(struct ieee80211_assoc_response_frame));
 
 	assoc->header.frame_ctl = cpu_to_le16(IEEE80211_STYPE_ASSOC_RESP);
 	memcpy(assoc->header.addr1, dest,ETH_ALEN);
@@ -877,7 +878,7 @@ static struct sk_buff *ieee80211_assoc_resp(struct ieee80211_device *ieee,
 		crypt = ieee->crypt[ieee->tx_keyidx];
 	else crypt = NULL;
 
-	encrypt = ( crypt && crypt->ops);
+	encrypt = (crypt && crypt->ops);
 
 	if (encrypt)
 		assoc->capability |= cpu_to_le16(WLAN_CAPABILITY_PRIVACY);
@@ -1123,7 +1124,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 		u8	CcxAironetBuf[30];
 		OCTET_STRING	osCcxAironetIE;
 
-		memset(CcxAironetBuf, 0,30);
+		memset(CcxAironetBuf, 0, 30);
 		osCcxAironetIE.Octet = CcxAironetBuf;
 		osCcxAironetIE.Length = sizeof(CcxAironetBuf);
 		//
@@ -1139,7 +1140,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 		tag = skb_put(skb, ckip_ie_len);
 		*tag++ = MFIE_TYPE_AIRONET;
 		*tag++ = osCcxAironetIE.Length;
-		memcpy(tag,osCcxAironetIE.Octet,osCcxAironetIE.Length);
+		memcpy(tag, osCcxAironetIE.Octet, osCcxAironetIE.Length);
 		tag += osCcxAironetIE.Length;
 	}
 
@@ -1150,10 +1151,10 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 
 		osCcxRmCap.Octet = CcxRmCapBuf;
 		osCcxRmCap.Length = sizeof(CcxRmCapBuf);
-		tag = skb_put(skb,ccxrm_ie_len);
+		tag = skb_put(skb, ccxrm_ie_len);
 		*tag++ = MFIE_TYPE_GENERIC;
 		*tag++ = osCcxRmCap.Length;
-		memcpy(tag,osCcxRmCap.Octet,osCcxRmCap.Length);
+		memcpy(tag, osCcxRmCap.Octet, osCcxRmCap.Length);
 		tag += osCcxRmCap.Length;
 	}
 
@@ -1164,10 +1165,10 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 		CcxVerNumBuf[4] = beacon->BssCcxVerNumber;
 		osCcxVerNum.Octet = CcxVerNumBuf;
 		osCcxVerNum.Length = sizeof(CcxVerNumBuf);
-		tag = skb_put(skb,cxvernum_ie_len);
+		tag = skb_put(skb, cxvernum_ie_len);
 		*tag++ = MFIE_TYPE_GENERIC;
 		*tag++ = osCcxVerNum.Length;
-		memcpy(tag,osCcxVerNum.Octet,osCcxVerNum.Length);
+		memcpy(tag, osCcxVerNum.Octet, osCcxVerNum.Length);
 		tag += osCcxVerNum.Length;
 	}
 	//HT cap element
@@ -1189,12 +1190,12 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 		memcpy(tag, ieee->wpa_ie, ieee->wpa_ie_len);
 	}
 
-	tag = skb_put(skb,wmm_info_len);
+	tag = skb_put(skb, wmm_info_len);
 	if(wmm_info_len) {
 	  ieee80211_WMM_Info(ieee, &tag);
 	}
 #ifdef THOMAS_TURBO
-	tag = skb_put(skb,turbo_info_len);
+	tag = skb_put(skb, turbo_info_len);
 	if(turbo_info_len) {
 		ieee80211_TURBO_Info(ieee, &tag);
 	}
@@ -1206,7 +1207,7 @@ inline struct sk_buff *ieee80211_association_req(struct ieee80211_network *beaco
 			tag = skb_put(skb, ht_cap_len);
 			*tag++ = MFIE_TYPE_GENERIC;
 			*tag++ = ht_cap_len - 2;
-			memcpy(tag, ht_cap_buf,ht_cap_len - 2);
+			memcpy(tag, ht_cap_buf, ht_cap_len - 2);
 			tag += ht_cap_len -2;
 		}
 
@@ -1418,7 +1419,7 @@ inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee
 	u8 tmp_ssid[IW_ESSID_MAX_SIZE+1];
 	int tmp_ssid_len = 0;
 
-	short apset,ssidset,ssidbroad,apmatch,ssidmatch;
+	short apset, ssidset, ssidbroad, apmatch, ssidmatch;
 
 	/* we are interested in new new only if we are not associated
 	 * and we are not associating / authenticating
@@ -1451,7 +1452,7 @@ inline void ieee80211_softmac_new_net(struct ieee80211_device *ieee, struct ieee
 			 * if the network does broadcast and the user does not set essid it is OK
 			 * if the network does broadcast and the user did set essid chech if essid match
 			 */
-			( apset && apmatch &&
+			(apset && apmatch &&
 				((ssidset && ssidbroad && ssidmatch) || (ssidbroad && !ssidset) || (!ssidbroad && ssidset)) ) ||
 			/* if the ap is not set, check that the user set the bssid
 			 * and the network does broadcast and that those two bssid matches
@@ -1778,10 +1779,10 @@ static short ieee80211_sta_ps_sleep(struct ieee80211_device *ieee, u32 *time_h,
 static inline void ieee80211_sta_ps(struct ieee80211_device *ieee)
 {
 
-	u32 th,tl;
+	u32 th, tl;
 	short sleep;
 
-	unsigned long flags,flags2;
+	unsigned long flags, flags2;
 
 	spin_lock_irqsave(&ieee->lock, flags);
 
@@ -1805,7 +1806,7 @@ static inline void ieee80211_sta_ps(struct ieee80211_device *ieee)
 	if(sleep == 1){
 
 		if(ieee->sta_sleep == 1)
-			ieee->enter_sleep_state(ieee->dev,th,tl);
+			ieee->enter_sleep_state(ieee->dev, th, tl);
 
 		else if(ieee->sta_sleep == 0){
 		//	printk("send null 1\n");
@@ -1818,7 +1819,7 @@ static inline void ieee80211_sta_ps(struct ieee80211_device *ieee)
 
 				ieee->ps_request_tx_ack(ieee->dev);
 
-				ieee80211_sta_ps_send_null_frame(ieee,1);
+				ieee80211_sta_ps_send_null_frame(ieee, 1);
 
 				ieee->ps_th = th;
 				ieee->ps_tl = tl;
@@ -1832,7 +1833,7 @@ static inline void ieee80211_sta_ps(struct ieee80211_device *ieee)
 //#warning CHECK_LOCK_HERE
 		spin_lock_irqsave(&ieee->mgmt_tx_lock, flags2);
 
-		ieee80211_sta_wakeup(ieee,1);
+		ieee80211_sta_wakeup(ieee, 1);
 
 		spin_unlock_irqrestore(&ieee->mgmt_tx_lock, flags2);
 	}
@@ -1867,7 +1868,7 @@ void ieee80211_sta_wakeup(struct ieee80211_device *ieee, short nl)
 
 void ieee80211_ps_tx_ack(struct ieee80211_device *ieee, short success)
 {
-	unsigned long flags,flags2;
+	unsigned long flags, flags2;
 
 	spin_lock_irqsave(&ieee->lock, flags);
 
@@ -2109,7 +2110,7 @@ ieee80211_rx_frame_softmac(struct ieee80211_device *ieee, struct sk_buff *skb,
 		}
 		break;
 	case IEEE80211_STYPE_MANAGE_ACT:
-		ieee80211_process_action(ieee,skb);
+		ieee80211_process_action(ieee, skb);
 		break;
 	default:
 		return -1;
@@ -2146,10 +2147,10 @@ void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *
 	int  i;
 	cb_desc *tcb_desc = NULL;
 
-	spin_lock_irqsave(&ieee->lock,flags);
+	spin_lock_irqsave(&ieee->lock, flags);
 
 	/* called with 2nd parm 0, no tx mgmt lock required */
-	ieee80211_sta_wakeup(ieee,0);
+	ieee80211_sta_wakeup(ieee, 0);
 
 	/* update the tx status */
 	ieee->stats.tx_bytes += txb->payload_size;
@@ -2181,7 +2182,7 @@ void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *
 		}else{
 			ieee->softmac_data_hard_start_xmit(
 					txb->fragments[i],
-					ieee->dev,ieee->rate);
+					ieee->dev, ieee->rate);
 			//ieee->stats.tx_packets++;
 			//ieee->stats.tx_bytes += txb->fragments[i]->len;
 			//ieee->dev->trans_start = jiffies;
@@ -2190,7 +2191,7 @@ void ieee80211_softmac_xmit(struct ieee80211_txb *txb, struct ieee80211_device *
 	ieee80211_txb_free(txb);
 
 //exit:
-	spin_unlock_irqrestore(&ieee->lock,flags);
+	spin_unlock_irqrestore(&ieee->lock, flags);
 
 }
 EXPORT_SYMBOL(ieee80211_softmac_xmit);
@@ -2208,7 +2209,7 @@ static void ieee80211_resume_tx(struct ieee80211_device *ieee)
 
 			ieee->softmac_data_hard_start_xmit(
 				ieee->tx_pending.txb->fragments[i],
-				ieee->dev,ieee->rate);
+				ieee->dev, ieee->rate);
 				//(i+1)<ieee->tx_pending.txb->nr_frags);
 			ieee->stats.tx_packets++;
 			ieee->dev->trans_start = jiffies;
@@ -2225,14 +2226,14 @@ void ieee80211_reset_queue(struct ieee80211_device *ieee)
 {
 	unsigned long flags;
 
-	spin_lock_irqsave(&ieee->lock,flags);
+	spin_lock_irqsave(&ieee->lock, flags);
 	init_mgmt_queue(ieee);
 	if (ieee->tx_pending.txb){
 		ieee80211_txb_free(ieee->tx_pending.txb);
 		ieee->tx_pending.txb = NULL;
 	}
 	ieee->queue_stop = 0;
-	spin_unlock_irqrestore(&ieee->lock,flags);
+	spin_unlock_irqrestore(&ieee->lock, flags);
 
 }
 EXPORT_SYMBOL(ieee80211_reset_queue);
@@ -2244,7 +2245,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 	struct sk_buff *skb;
 	struct ieee80211_hdr_3addr  *header;
 
-	spin_lock_irqsave(&ieee->lock,flags);
+	spin_lock_irqsave(&ieee->lock, flags);
 	if (! ieee->queue_stop) goto exit;
 
 	ieee->queue_stop = 0;
@@ -2274,7 +2275,7 @@ void ieee80211_wake_queue(struct ieee80211_device *ieee)
 	}
 
 exit :
-	spin_unlock_irqrestore(&ieee->lock,flags);
+	spin_unlock_irqrestore(&ieee->lock, flags);
 }
 EXPORT_SYMBOL(ieee80211_wake_queue);
 
@@ -2355,7 +2356,7 @@ static void ieee80211_start_ibss_wq(struct work_struct *work)
 	down(&ieee->wx_sem);
 
 	if (ieee->current_network.ssid_len == 0){
-		strcpy(ieee->current_network.ssid,IEEE80211_DEFAULT_TX_ESSID);
+		strcpy(ieee->current_network.ssid, IEEE80211_DEFAULT_TX_ESSID);
 		ieee->current_network.ssid_len = strlen(IEEE80211_DEFAULT_TX_ESSID);
 		ieee->ssid_set = 1;
 	}
@@ -2554,7 +2555,7 @@ exit:
 
 struct sk_buff *ieee80211_get_beacon_(struct ieee80211_device *ieee)
 {
-	u8 broadcast_addr[] = {0xff,0xff,0xff,0xff,0xff,0xff};
+	u8 broadcast_addr[] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 
 	struct sk_buff *skb;
 	struct ieee80211_probe_response *b;
@@ -2735,12 +2736,12 @@ void ieee80211_softmac_init(struct ieee80211_device *ieee)
 
 	ieee->wq = create_workqueue(DRV_NAME);
 
-	INIT_DELAYED_WORK(&ieee->start_ibss_wq,ieee80211_start_ibss_wq);
+	INIT_DELAYED_WORK(&ieee->start_ibss_wq, ieee80211_start_ibss_wq);
 	INIT_WORK(&ieee->associate_complete_wq, ieee80211_associate_complete_wq);
 	INIT_WORK(&ieee->associate_procedure_wq, ieee80211_associate_procedure_wq);
-	INIT_DELAYED_WORK(&ieee->softmac_scan_wq,ieee80211_softmac_scan_wq);
+	INIT_DELAYED_WORK(&ieee->softmac_scan_wq, ieee80211_softmac_scan_wq);
 	INIT_DELAYED_WORK(&ieee->associate_retry_wq, ieee80211_associate_retry_wq);
-	INIT_WORK(&ieee->wx_sync_scan_wq,ieee80211_wx_sync_scan_wq);
+	INIT_WORK(&ieee->wx_sync_scan_wq, ieee80211_wx_sync_scan_wq);
 
 
 	sema_init(&ieee->wx_sem, 1);
@@ -2939,10 +2940,10 @@ static int ieee80211_wpa_set_param(struct ieee80211_device *ieee, u8 name, u32 v
 		break;
 	case IEEE_PARAM_WPAX_SELECT:
 		// added for WPA2 mixed mode
-		spin_lock_irqsave(&ieee->wpax_suitlist_lock,flags);
+		spin_lock_irqsave(&ieee->wpax_suitlist_lock, flags);
 		ieee->wpax_type_set = 1;
 		ieee->wpax_type_notify = value;
-		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock,flags);
+		spin_unlock_irqrestore(&ieee->wpax_suitlist_lock, flags);
 		break;
 
 	default:
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
index 935a8f4adcf9..82ea533a0cf4 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_softmac_wx.c
@@ -181,7 +181,7 @@ EXPORT_SYMBOL(ieee80211_wx_set_wap);
 
  int ieee80211_wx_get_essid(struct ieee80211_device *ieee, struct iw_request_info *a,union iwreq_data *wrqu,char *b)
 {
-	int len,ret = 0;
+	int len, ret = 0;
 	unsigned long flags;
 
 	if (ieee->iw_mode == IW_MODE_MONITOR)
@@ -204,7 +204,7 @@ EXPORT_SYMBOL(ieee80211_wx_set_wap);
 	}
 	len = ieee->current_network.ssid_len;
 	wrqu->essid.length = len;
-	strncpy(b,ieee->current_network.ssid,len);
+	strncpy(b, ieee->current_network.ssid, len);
 	wrqu->essid.flags = 1;
 
 out:
@@ -319,7 +319,7 @@ void ieee80211_wx_sync_scan_wq(struct work_struct *work)
 
 	ieee->state = IEEE80211_LINKED_SCANNING;
 	ieee->link_change(ieee->dev);
-	ieee->InitialGainHandler(ieee->dev,IG_Backup);
+	ieee->InitialGainHandler(ieee->dev, IG_Backup);
 	if (ieee->pHTInfo->bCurrentHTSupport && ieee->pHTInfo->bEnableHT && ieee->pHTInfo->bCurBW40MHz) {
 		b40M = 1;
 		chan_offset = ieee->pHTInfo->CurSTAExtChnlOffset;
@@ -341,7 +341,7 @@ void ieee80211_wx_sync_scan_wq(struct work_struct *work)
 		ieee->set_chan(ieee->dev, chan);
 	}
 
-	ieee->InitialGainHandler(ieee->dev,IG_Restore);
+	ieee->InitialGainHandler(ieee->dev, IG_Restore);
 	ieee->state = IEEE80211_LINKED;
 	ieee->link_change(ieee->dev);
 	// To prevent the immediately calling watch_dog after scan.
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
index 7f9e655f9eb8..57bef219687b 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_tx.c
@@ -876,7 +876,7 @@ int ieee80211_xmit(struct sk_buff *skb, struct net_device *dev)
 		if (is_broadcast_ether_addr(header.addr1))
 			tcb_desc->bBroadcast = 1;
 		ieee80211_txrate_selectmode(ieee, tcb_desc);
-		if ( tcb_desc->bMulticast ||  tcb_desc->bBroadcast)
+		if (tcb_desc->bMulticast ||  tcb_desc->bBroadcast)
 			tcb_desc->data_rate = ieee->basic_rate;
 		else
 			//tcb_desc->data_rate = CURRENT_RATE(ieee->current_network.mode, ieee->rate, ieee->HTCurrentOperaRate);
diff --git a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
index 698507e00f31..68f5ede86633 100644
--- a/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
+++ b/drivers/staging/rtl8192u/ieee80211/ieee80211_wx.c
@@ -40,12 +40,12 @@ struct modes_unit {
 	int mode_size;
 };
 static struct modes_unit ieee80211_modes[] = {
-	{"a",1},
-	{"b",1},
-	{"g",1},
-	{"?",1},
-	{"N-24G",5},
-	{"N-5G",4},
+	{"a", 1},
+	{"b", 1},
+	{"g", 1},
+	{"?", 1},
+	{"N-24G", 5},
+	{"N-5G", 4},
 };
 
 #define iwe_stream_add_event_rsl iwe_stream_add_event
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
index b8cc6a537c36..51552d42d66c 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_BAProc.c
@@ -87,7 +87,7 @@ static u8 RxTsDeleteBA(struct ieee80211_device *ieee, PRX_TS_RECORD pRxTs)
  *	     PBA_RECORD		pBA //entry to be reset
  *  output:  none
 ********************************************************************************************************************/
-void ResetBaEntry( PBA_RECORD pBA)
+void ResetBaEntry(PBA_RECORD pBA)
 {
 	pBA->bValid			= false;
 	pBA->BaParamSet.shortData	= 0;
@@ -165,7 +165,7 @@ static struct sk_buff *ieee80211_ADDBA(struct ieee80211_device *ieee, u8 *Dst, P
 	if (ACT_ADDBAREQ == type)
 	{
 	// BA Start SeqCtrl
-		memcpy(tag,(u8 *)&(pBA->BaStartSeqCtrl), 2);
+		memcpy(tag, (u8 *)&(pBA->BaStartSeqCtrl), 2);
 		tag += 2;
 	}
 
@@ -331,7 +331,7 @@ static void ieee80211_send_DELBA(struct ieee80211_device *ieee, u8 *dst,
  *  return:  0(pass), other(fail)
  *  notice:  As this function need support of QOS, I comment some code out. And when qos is ready, this code need to be support.
 ********************************************************************************************************************/
-int ieee80211_rx_ADDBAReq( struct ieee80211_device *ieee, struct sk_buff *skb)
+int ieee80211_rx_ADDBAReq(struct ieee80211_device *ieee, struct sk_buff *skb)
 {
 	 struct ieee80211_hdr_3addr *req = NULL;
 	u16 rc = 0;
@@ -344,16 +344,19 @@ int ieee80211_rx_ADDBAReq( struct ieee80211_device *ieee, struct sk_buff *skb)
 
 	if (skb->len < sizeof( struct ieee80211_hdr_3addr) + 9)
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, " Invalid skb len in BAREQ(%d / %zu)\n", skb->len,	(sizeof( struct ieee80211_hdr_3addr) + 9));
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				" Invalid skb len in BAREQ(%d / %zu)\n",
+				skb->len,
+				(sizeof(struct ieee80211_hdr_3addr) + 9));
 		return -1;
 	}
 
 	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
 
-	req = ( struct ieee80211_hdr_3addr *) skb->data;
+	req = (struct ieee80211_hdr_3addr *) skb->data;
 	tag = (u8 *)req;
 	dst = (u8 *)(&req->addr2[0]);
-	tag += sizeof( struct ieee80211_hdr_3addr);
+	tag += sizeof(struct ieee80211_hdr_3addr);
 	pDialogToken = tag + 2;  //category+action
 	pBaParamSet = (PBA_PARAM_SET)(tag + 3);   //+DialogToken
 	pBaTimeoutVal = (u16 *)(tag + 5);
@@ -431,7 +434,7 @@ OnADDBAReq_Fail:
  *  return:  0(pass), other(fail)
  *  notice:  As this function need support of QOS, I comment some code out. And when qos is ready, this code need to be support.
 ********************************************************************************************************************/
-int ieee80211_rx_ADDBARsp( struct ieee80211_device *ieee, struct sk_buff *skb)
+int ieee80211_rx_ADDBARsp(struct ieee80211_device *ieee, struct sk_buff *skb)
 {
 	 struct ieee80211_hdr_3addr *rsp = NULL;
 	PBA_RECORD		pPendingBA, pAdmittedBA;
@@ -443,13 +446,16 @@ int ieee80211_rx_ADDBARsp( struct ieee80211_device *ieee, struct sk_buff *skb)
 
 	if (skb->len < sizeof( struct ieee80211_hdr_3addr) + 9)
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, " Invalid skb len in BARSP(%d / %zu)\n", skb->len,	(sizeof( struct ieee80211_hdr_3addr) + 9));
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				" Invalid skb len in BARSP(%d / %zu)\n",
+				skb->len,
+				(sizeof(struct ieee80211_hdr_3addr) + 9));
 		return -1;
 	}
-	rsp = ( struct ieee80211_hdr_3addr *)skb->data;
+	rsp = (struct ieee80211_hdr_3addr *)skb->data;
 	tag = (u8 *)rsp;
 	dst = (u8 *)(&rsp->addr2[0]);
-	tag += sizeof( struct ieee80211_hdr_3addr);
+	tag += sizeof(struct ieee80211_hdr_3addr);
 	pDialogToken = tag + 2;
 	pStatusCode = (u16 *)(tag + 3);
 	pBaParamSet = (PBA_PARAM_SET)(tag + 5);
@@ -564,7 +570,7 @@ OnADDBARsp_Reject:
  *  return:  0(pass), other(fail)
  *  notice:  As this function need support of QOS, I comment some code out. And when qos is ready, this code need to be support.
 ********************************************************************************************************************/
-int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb)
+int ieee80211_rx_DELBA(struct ieee80211_device *ieee, struct sk_buff *skb)
 {
 	 struct ieee80211_hdr_3addr *delba = NULL;
 	PDELBA_PARAM_SET	pDelBaParamSet = NULL;
@@ -573,7 +579,10 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb)
 
 	if (skb->len < sizeof( struct ieee80211_hdr_3addr) + 6)
 	{
-		IEEE80211_DEBUG(IEEE80211_DL_ERR, " Invalid skb len in DELBA(%d / %zu)\n", skb->len,	(sizeof( struct ieee80211_hdr_3addr) + 6));
+		IEEE80211_DEBUG(IEEE80211_DL_ERR,
+				" Invalid skb len in DELBA(%d / %zu)\n",
+				skb->len,
+				(sizeof(struct ieee80211_hdr_3addr) + 6));
 		return -1;
 	}
 
@@ -585,9 +594,9 @@ int ieee80211_rx_DELBA(struct ieee80211_device *ieee,struct sk_buff *skb)
 	}
 
 	IEEE80211_DEBUG_DATA(IEEE80211_DL_DATA|IEEE80211_DL_BA, skb->data, skb->len);
-	delba = ( struct ieee80211_hdr_3addr *)skb->data;
+	delba = (struct ieee80211_hdr_3addr *)skb->data;
 	dst = (u8 *)(&delba->addr2[0]);
-	delba += sizeof( struct ieee80211_hdr_3addr);
+	delba += sizeof(struct ieee80211_hdr_3addr);
 	pDelBaParamSet = (PDELBA_PARAM_SET)(delba+2);
 	pReasonCode = (u16 *)(delba+4);
 
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
index 2b8283534be4..c3aabbaac7ae 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HT.h
@@ -53,7 +53,7 @@ typedef enum _HT_MCS_RATE{
 	HT_MCS14 = 0x00004000,
 	HT_MCS15 = 0x00008000,
 	// Do not define MCS32 here although 8190 support MCS32
-}HT_MCS_RATE,*PHT_MCS_RATE;
+} HT_MCS_RATE, *PHT_MCS_RATE;
 
 //
 // Represent Channel Width in HT Capabilities
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
index 2694a085e3aa..1ea2cd392670 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_HTProc.c
@@ -278,7 +278,7 @@ u16 HTHalfMcsToDataRate(struct ieee80211_device *ieee,	u8	nMcsRate)
 }
 
 
-u16 HTMcsToDataRate( struct ieee80211_device *ieee, u8 nMcsRate)
+u16 HTMcsToDataRate(struct ieee80211_device *ieee, u8 nMcsRate)
 {
 	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
 
@@ -297,7 +297,7 @@ u16 HTMcsToDataRate( struct ieee80211_device *ieee, u8 nMcsRate)
  *  return:  tx rate
  *  notice:  quite unsure about how to use this function //wb
  * *****************************************************************************************************************/
-u16  TxCountToDataRate( struct ieee80211_device *ieee, u8 nDataRate)
+u16  TxCountToDataRate(struct ieee80211_device *ieee, u8 nDataRate)
 {
 	//PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
 	u16		CCKOFDMRate[12] = {0x02 , 0x04 , 0x0b , 0x16 , 0x0c , 0x12 , 0x18 , 0x24 , 0x30 , 0x48 , 0x60 , 0x6c};
diff --git a/drivers/staging/rtl8192u/ieee80211/rtl819x_Qos.h b/drivers/staging/rtl8192u/ieee80211/rtl819x_Qos.h
index f2d52ca08cd0..9fbce912a74b 100644
--- a/drivers/staging/rtl8192u/ieee80211/rtl819x_Qos.h
+++ b/drivers/staging/rtl8192u/ieee80211/rtl819x_Qos.h
@@ -66,7 +66,7 @@ typedef u32 QOS_MODE, *PQOS_MODE;
 typedef	enum _ACK_POLICY{
 	eAckPlc0_ACK		= 0x00,
 	eAckPlc1_NoACK		= 0x01,
-}ACK_POLICY,*PACK_POLICY;
+} ACK_POLICY, *PACK_POLICY;
 
 #define WMM_PARAM_ELEMENT_SIZE	(8+(4*AC_PARAM_SIZE))
 
@@ -276,7 +276,7 @@ typedef	union _AC_PARAM{
 typedef	enum _QOS_ELE_SUBTYPE{
 	QOSELE_TYPE_INFO	= 0x00,		// 0x00: Information element
 	QOSELE_TYPE_PARAM	= 0x01,		// 0x01: parameter element
-}QOS_ELE_SUBTYPE,*PQOS_ELE_SUBTYPE;
+} QOS_ELE_SUBTYPE, *PQOS_ELE_SUBTYPE;
 
 
 //
@@ -288,7 +288,7 @@ typedef	enum _DIRECTION_VALUE{
 	DIR_DOWN		= 1,		// 0x01	// DownLink
 	DIR_DIRECT		= 2,		// 0x10	// DirectLink
 	DIR_BI_DIR		= 3,		// 0x11	// Bi-Direction
-}DIRECTION_VALUE,*PDIRECTION_VALUE;
+} DIRECTION_VALUE, *PDIRECTION_VALUE;
 
 
 //
@@ -363,7 +363,7 @@ typedef	enum _ACM_METHOD{
 	eAcmWay0_SwAndHw		= 0,		// By SW and HW.
 	eAcmWay1_HW			= 1,		// By HW.
 	eAcmWay2_SW			= 2,		// By SW.
-}ACM_METHOD,*PACM_METHOD;
+} ACM_METHOD, *PACM_METHOD;
 
 
 typedef struct _ACM{
diff --git a/drivers/staging/rtl8192u/r8190_rtl8256.c b/drivers/staging/rtl8192u/r8190_rtl8256.c
index 43ed76806c61..45514aa97698 100644
--- a/drivers/staging/rtl8192u/r8190_rtl8256.c
+++ b/drivers/staging/rtl8192u/r8190_rtl8256.c
@@ -49,9 +49,6 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev , HT_CHANNEL_WIDTH Bandwidth)
 					rtl8192_phy_SetRFReg(dev,
 						(RF90_RADIO_PATH_E)eRFPath,
 						0x0e, bMask12Bits, 0x021);
-
-					/* cosa add for sd3's request 01/23/2008
-					 */
 					rtl8192_phy_SetRFReg(dev,
 						(RF90_RADIO_PATH_E)eRFPath,
 						0x14, bMask12Bits, 0x5ab);
@@ -61,13 +58,12 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev , HT_CHANNEL_WIDTH Bandwidth)
 				break;
 		case HT_CHANNEL_WIDTH_20_40:
 				if (priv->card_8192_version == VERSION_819xU_A || priv->card_8192_version == VERSION_819xU_B) { /* 8256 D-cut, E-cut, xiong: consider it later! */
-					rtl8192_phy_SetRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, 0x0b, bMask12Bits, 0x300); //phy para:3ba
+					rtl8192_phy_SetRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, 0x0b, bMask12Bits, 0x300); /* phy para:3ba */
 					rtl8192_phy_SetRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, 0x2c, bMask12Bits, 0x3df);
 					rtl8192_phy_SetRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, 0x0e, bMask12Bits, 0x0a1);
 
-					//cosa add for sd3's request 01/23/2008
 					if (priv->chan == 3 || priv->chan == 9)
-						//I need to set priv->chan whenever current channel changes
+						/* I need to set priv->chan whenever current channel changes */
 						rtl8192_phy_SetRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, 0x14, bMask12Bits, 0x59b);
 					else
 						rtl8192_phy_SetRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, 0x14, bMask12Bits, 0x5ab);
@@ -91,11 +87,12 @@ void PHY_SetRF8256Bandwidth(struct net_device *dev , HT_CHANNEL_WIDTH Bandwidth)
 void PHY_RF8256_Config(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	// Initialize general global value
-	//
-	// TODO: Extend RF_PATH_C and RF_PATH_D in the future
+	/* Initialize general global value
+	 *
+	 * TODO: Extend RF_PATH_C and RF_PATH_D in the future
+	 */
 	priv->NumTotalRFPath = RTL819X_TOTAL_RF_PATH;
-	// Config BB and RF
+	/* Config BB and RF */
 	phy_RF8256_Config_ParaFile(dev);
 }
 /*--------------------------------------------------------------------------
@@ -107,10 +104,6 @@ void PHY_RF8256_Config(struct net_device *dev)
 void phy_RF8256_Config_ParaFile(struct net_device *dev)
 {
 	u32	u4RegValue = 0;
-	//static s1Byte				szRadioAFile[] = RTL819X_PHY_RADIO_A;
-	//static s1Byte				szRadioBFile[] = RTL819X_PHY_RADIO_B;
-	//static s1Byte				szRadioCFile[] = RTL819X_PHY_RADIO_C;
-	//static s1Byte				szRadioDFile[] = RTL819X_PHY_RADIO_D;
 	u8	eRFPath;
 	BB_REGISTER_DEFINITION_T	*pPhyReg;
 	struct r8192_priv *priv = ieee80211_priv(dev);
@@ -119,19 +112,17 @@ void phy_RF8256_Config_ParaFile(struct net_device *dev)
 	u32	RF3_Final_Value = 0;
 	u8	ConstRetryTimes = 5, RetryTimes = 5;
 	u8 ret = 0;
-	//3//-----------------------------------------------------------------
-	//3// <2> Initialize RF
-	//3//-----------------------------------------------------------------
+	/* Initialize RF */
 	for (eRFPath = (RF90_RADIO_PATH_E)RF90_PATH_A; eRFPath < priv->NumTotalRFPath; eRFPath++) {
 		if (!rtl8192_phy_CheckIsLegalRFPath(dev, eRFPath))
 			continue;
 
 		pPhyReg = &priv->PHYRegDef[eRFPath];
 
-		// Joseph test for shorten RF config
-	//	pHalData->RfReg0Value[eRFPath] =  rtl8192_phy_QueryRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, rGlobalCtrl, bMaskDWord);
-
-		/*----Store original RFENV control type----*/
+		/* Joseph test for shorten RF config
+		 * pHalData->RfReg0Value[eRFPath] =  rtl8192_phy_QueryRFReg(dev, (RF90_RADIO_PATH_E)eRFPath, rGlobalCtrl, bMaskDWord);
+		 * ----Store original RFENV control type
+		 */
 		switch (eRFPath) {
 		case RF90_PATH_A:
 		case RF90_PATH_C:
@@ -150,13 +141,14 @@ void phy_RF8256_Config_ParaFile(struct net_device *dev)
 		rtl8192_setBBreg(dev, pPhyReg->rfintfo, bRFSI_RFENV, 0x1);
 
 		/* Set bit number of Address and Data for RF register */
-		rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, b3WireAddressLength, 0x0);	// Set 0 to 4 bits for Z-serial and set 1 to 6 bits for 8258
-		rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, b3WireDataLength, 0x0);	// Set 0 to 12 bits for Z-serial and 8258, and set 1 to 14 bits for ???
+		rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, b3WireAddressLength, 0x0);	/* Set 0 to 4 bits for Z-serial and set 1 to 6 bits for 8258 */
+		rtl8192_setBBreg(dev, pPhyReg->rfHSSIPara2, b3WireDataLength, 0x0);	/* Set 0 to 12 bits for Z-serial and 8258, and set 1 to 14 bits for ??? */
 
 		rtl8192_phy_SetRFReg(dev, (RF90_RADIO_PATH_E) eRFPath, 0x0, bMask12Bits, 0xbf);
 
-		/*----Check RF block (for FPGA platform only)----*/
-		// TODO: this function should be removed on ASIC , Emily 2007.2.2
+		/* Check RF block (for FPGA platform only)----
+		 * TODO: this function should be removed on ASIC , Emily 2007.2.2
+		 */
 		if (rtl8192_phy_checkBBAndRF(dev, HW90_BLOCK_RF, (RF90_RADIO_PATH_E)eRFPath)) {
 			RT_TRACE(COMP_ERR, "PHY_RF8256_Config():Check Radio[%d] Fail!!\n", eRFPath);
 			goto phy_RF8256_Config_ParaFile_Fail;
@@ -231,11 +223,9 @@ void PHY_SetRF8256CCKTxPower(struct net_device *dev, u8 powerlevel)
 {
 	u32	TxAGC = 0;
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	//modified by vivi, 20080109
 	TxAGC = powerlevel;
 
 	if (priv->bDynamicTxLowPower == TRUE) {
-		//cosa 05/22/2008 for scan
 		if (priv->CustomerID == RT_CID_819x_Netcore)
 			TxAGC = 0x22;
 		else
@@ -251,15 +241,15 @@ void PHY_SetRF8256CCKTxPower(struct net_device *dev, u8 powerlevel)
 void PHY_SetRF8256OFDMTxPower(struct net_device *dev, u8 powerlevel)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	//Joseph TxPower for 8192 testing
+	/* Joseph TxPower for 8192 testing */
 	u32 writeVal, powerBase0, powerBase1, writeVal_tmp;
 	u8 index = 0;
 	u16 RegOffset[6] = {0xe00, 0xe04, 0xe10, 0xe14, 0xe18, 0xe1c};
 	u8 byte0, byte1, byte2, byte3;
 
-	powerBase0 = powerlevel + priv->TxPowerDiff;	//OFDM rates
+	powerBase0 = powerlevel + priv->TxPowerDiff;	/* OFDM rates */
 	powerBase0 = (powerBase0<<24) | (powerBase0<<16) | (powerBase0<<8) | powerBase0;
-	powerBase1 = powerlevel;							//MCS rates
+	powerBase1 = powerlevel;							/* MCS rates */
 	powerBase1 = (powerBase1<<24) | (powerBase1<<16) | (powerBase1<<8) | powerBase1;
 
 	for (index = 0; index < 6; index++) {
@@ -279,7 +269,7 @@ void PHY_SetRF8256OFDMTxPower(struct net_device *dev, u8 powerlevel)
 		if (byte3 > 0x24)
 			byte3 = 0x24;
 
-		//for tx power track
+		/* for tx power track */
 		if (index == 3) {
 			writeVal_tmp = (byte3<<24) | (byte2<<16) | (byte1<<8) | byte0;
 			priv->Pwr_Track = writeVal_tmp;
diff --git a/drivers/staging/rtl8192u/r8192U_core.c b/drivers/staging/rtl8192u/r8192U_core.c
index 76403864644c..929ac29197cc 100644
--- a/drivers/staging/rtl8192u/r8192U_core.c
+++ b/drivers/staging/rtl8192u/r8192U_core.c
@@ -56,29 +56,6 @@ double __extendsfdf2(float a)
 }
 #endif
 
-#undef LOOP_TEST
-#undef DUMP_RX
-#undef DUMP_TX
-#undef DEBUG_TX_DESC2
-#undef RX_DONT_PASS_UL
-#undef DEBUG_EPROM
-#undef DEBUG_RX_VERBOSE
-#undef DUMMY_RX
-#undef DEBUG_ZERO_RX
-#undef DEBUG_RX_SKB
-#undef DEBUG_TX_FRAG
-#undef DEBUG_RX_FRAG
-#undef DEBUG_TX_FILLDESC
-#undef DEBUG_TX
-#undef DEBUG_IRQ
-#undef DEBUG_RX
-#undef DEBUG_RXALLOC
-#undef DEBUG_REGISTERS
-#undef DEBUG_RING
-#undef DEBUG_IRQ_TASKLET
-#undef DEBUG_TX_ALLOC
-#undef DEBUG_TX_DESC
-
 #define CONFIG_RTL8192_IO_MAP
 
 #include <asm/uaccess.h>
@@ -160,12 +137,12 @@ static struct usb_driver rtl8192_usb_driver = {
 };
 
 
-typedef struct _CHANNEL_LIST {
+struct CHANNEL_LIST {
 	u8	Channel[32];
 	u8	Len;
-} CHANNEL_LIST, *PCHANNEL_LIST;
+};
 
-static CHANNEL_LIST ChannelPlan[] = {
+static struct CHANNEL_LIST ChannelPlan[] = {
 	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 36, 40, 44, 48, 52, 56, 60, 64, 149, 153, 157, 161, 165}, 24},		//FCC
 	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}, 11},							//IC
 	{{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 36, 40, 44, 48, 52, 56, 60, 64}, 21},	//ETSI
@@ -665,15 +642,6 @@ static void tx_timeout(struct net_device *dev)
 	schedule_work(&priv->reset_wq);
 }
 
-
-/* this is only for debug */
-void dump_eprom(struct net_device *dev)
-{
-	int i;
-	for (i = 0; i < 63; i++)
-		RT_TRACE(COMP_EPROM, "EEPROM addr %x : %x", i, eprom_read(dev, i));
-}
-
 void rtl8192_update_msr(struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
@@ -711,13 +679,11 @@ void rtl8192_set_chan(struct net_device *dev, short ch)
 
 	/* this hack should avoid frame TX during channel setting*/
 
-#ifndef LOOP_TEST
 	//need to implement rf set channel here WB
 
 	if (priv->rf_set_chan)
 		priv->rf_set_chan(dev, priv->chan);
 	mdelay(10);
-#endif
 }
 
 static void rtl8192_rx_isr(struct urb *urb);
@@ -725,14 +691,8 @@ static void rtl8192_rx_isr(struct urb *urb);
 static u32 get_rxpacket_shiftbytes_819xusb(struct ieee80211_rx_stats *pstats)
 {
 
-#ifdef USB_RX_AGGREGATION_SUPPORT
-	if (pstats->bisrxaggrsubframe)
-		return (sizeof(rx_desc_819x_usb) + pstats->RxDrvInfoSize
-			+ pstats->RxBufShift + 8);
-	else
-#endif
-		return (sizeof(rx_desc_819x_usb) + pstats->RxDrvInfoSize
-			+ pstats->RxBufShift);
+	return (sizeof(rx_desc_819x_usb) + pstats->RxDrvInfoSize
+		+ pstats->RxBufShift);
 
 }
 static int rtl8192_rx_initiate(struct net_device *dev)
@@ -1046,194 +1006,6 @@ static int rtl8192_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 
 void rtl8192_try_wake_queue(struct net_device *dev, int pri);
 
-#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
-u16 DrvAggr_PaddingAdd(struct net_device *dev, struct sk_buff *skb)
-{
-	u16     PaddingNum =  256 - ((skb->len + TX_PACKET_DRVAGGR_SUBFRAME_SHIFT_BYTES) % 256);
-	return  PaddingNum & 0xff;
-}
-
-u8 MRateToHwRate8190Pci(u8 rate);
-u8 QueryIsShort(u8 TxHT, u8 TxRate, cb_desc *tcb_desc);
-u8 MapHwQueueToFirmwareQueue(u8 QueueID);
-struct sk_buff *DrvAggr_Aggregation(struct net_device *dev, struct ieee80211_drv_agg_txb *pSendList)
-{
-	struct ieee80211_device *ieee = netdev_priv(dev);
-	struct r8192_priv *priv = ieee80211_priv(dev);
-	cb_desc		*tcb_desc = NULL;
-	u8		i;
-	u32		TotalLength;
-	struct sk_buff	*skb;
-	struct sk_buff  *agg_skb;
-	tx_desc_819x_usb_aggr_subframe *tx_agg_desc = NULL;
-	tx_fwinfo_819x_usb	       *tx_fwinfo = NULL;
-
-	//
-	// Local variable initialization.
-	//
-	/* first skb initialization */
-	skb = pSendList->tx_agg_frames[0];
-	TotalLength = skb->len;
-
-	/* Get the total aggregation length including the padding space and
-	 * sub frame header.
-	 */
-	for (i = 1; i < pSendList->nr_drv_agg_frames; i++) {
-		TotalLength += DrvAggr_PaddingAdd(dev, skb);
-		skb = pSendList->tx_agg_frames[i];
-		TotalLength += (skb->len + TX_PACKET_DRVAGGR_SUBFRAME_SHIFT_BYTES);
-	}
-
-	/* allocate skb to contain the aggregated packets */
-	agg_skb = dev_alloc_skb(TotalLength + ieee->tx_headroom);
-	memset(agg_skb->data, 0, agg_skb->len);
-	skb_reserve(agg_skb, ieee->tx_headroom);
-
-	/* reserve info for first subframe Tx descriptor to be set in the tx function */
-	skb = pSendList->tx_agg_frames[0];
-	tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
-	tcb_desc->drv_agg_enable = 1;
-	tcb_desc->pkt_size = skb->len;
-	tcb_desc->DrvAggrNum = pSendList->nr_drv_agg_frames;
-	netdev_dbg(dev, "DrvAggNum = %d\n", tcb_desc->DrvAggrNum);
-	memcpy(agg_skb->cb, skb->cb, sizeof(skb->cb));
-	memcpy(skb_put(agg_skb, skb->len), skb->data, skb->len);
-
-	for (i = 1; i < pSendList->nr_drv_agg_frames; i++) {
-		/* push the next sub frame to be 256 byte aline */
-		skb_put(agg_skb, DrvAggr_PaddingAdd(dev, skb));
-
-		/* Subframe drv Tx descriptor and firmware info setting */
-		skb = pSendList->tx_agg_frames[i];
-		tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
-		tx_agg_desc = (tx_desc_819x_usb_aggr_subframe *)skb_tail_pointer(agg_skb);
-		tx_fwinfo = (tx_fwinfo_819x_usb *)(skb_tail_pointer(agg_skb) + sizeof(tx_desc_819x_usb_aggr_subframe));
-
-		memset(tx_fwinfo, 0, sizeof(tx_fwinfo_819x_usb));
-		/* DWORD 0 */
-		tx_fwinfo->TxHT = (tcb_desc->data_rate&0x80) ? 1 : 0;
-		tx_fwinfo->TxRate = MRateToHwRate8190Pci(tcb_desc->data_rate);
-		tx_fwinfo->EnableCPUDur = tcb_desc->bTxEnableFwCalcDur;
-		tx_fwinfo->Short = QueryIsShort(tx_fwinfo->TxHT, tx_fwinfo->TxRate, tcb_desc);
-		if (tcb_desc->bAMPDUEnable) { /* AMPDU enabled */
-			tx_fwinfo->AllowAggregation = 1;
-			/* DWORD 1 */
-			tx_fwinfo->RxMF = tcb_desc->ampdu_factor;
-			tx_fwinfo->RxAMD = tcb_desc->ampdu_density&0x07;//ampdudensity
-		} else {
-			tx_fwinfo->AllowAggregation = 0;
-			/* DWORD 1 */
-			tx_fwinfo->RxMF = 0;
-			tx_fwinfo->RxAMD = 0;
-		}
-
-		/* Protection mode related */
-		tx_fwinfo->RtsEnable = (tcb_desc->bRTSEnable) ? 1 : 0;
-		tx_fwinfo->CtsEnable = (tcb_desc->bCTSEnable) ? 1 : 0;
-		tx_fwinfo->RtsSTBC = (tcb_desc->bRTSSTBC) ? 1 : 0;
-		tx_fwinfo->RtsHT = (tcb_desc->rts_rate&0x80) ? 1 : 0;
-		tx_fwinfo->RtsRate =  MRateToHwRate8190Pci((u8)tcb_desc->rts_rate);
-		tx_fwinfo->RtsSubcarrier = (tx_fwinfo->RtsHT == 0) ? (tcb_desc->RTSSC) : 0;
-		tx_fwinfo->RtsBandwidth = (tx_fwinfo->RtsHT == 1) ? ((tcb_desc->bRTSBW) ? 1 : 0) : 0;
-		tx_fwinfo->RtsShort = (tx_fwinfo->RtsHT == 0) ? (tcb_desc->bRTSUseShortPreamble ? 1 : 0) :
-				      (tcb_desc->bRTSUseShortGI ? 1 : 0);
-
-		/* Set Bandwidth and sub-channel settings. */
-		if (priv->CurrentChannelBW == HT_CHANNEL_WIDTH_20_40) {
-			if (tcb_desc->bPacketBW) {
-				tx_fwinfo->TxBandwidth = 1;
-				tx_fwinfo->TxSubCarrier = 0;    //By SD3's Jerry suggestion, use duplicated mode
-			} else {
-				tx_fwinfo->TxBandwidth = 0;
-				tx_fwinfo->TxSubCarrier = priv->nCur40MhzPrimeSC;
-			}
-		} else {
-			tx_fwinfo->TxBandwidth = 0;
-			tx_fwinfo->TxSubCarrier = 0;
-		}
-
-		/* Fill Tx descriptor */
-		memset(tx_agg_desc, 0, sizeof(tx_desc_819x_usb_aggr_subframe));
-		/* DWORD 0 */
-		tx_agg_desc->Offset =  sizeof(tx_fwinfo_819x_usb) + 8;
-		/* already raw data, need not to subtract header length */
-		tx_agg_desc->PktSize = skb->len & 0xffff;
-
-		/*DWORD 1*/
-		tx_agg_desc->SecCAMID = 0;
-		tx_agg_desc->RATid = tcb_desc->RATRIndex;
-		tx_agg_desc->NoEnc = 1;
-		tx_agg_desc->SecType = 0x0;
-
-		if (tcb_desc->bHwSec) {
-			switch (priv->ieee80211->pairwise_key_type) {
-			case KEY_TYPE_WEP40:
-			case KEY_TYPE_WEP104:
-				tx_agg_desc->SecType = 0x1;
-				tx_agg_desc->NoEnc = 0;
-				break;
-			case KEY_TYPE_TKIP:
-				tx_agg_desc->SecType = 0x2;
-				tx_agg_desc->NoEnc = 0;
-				break;
-			case KEY_TYPE_CCMP:
-				tx_agg_desc->SecType = 0x3;
-				tx_agg_desc->NoEnc = 0;
-				break;
-			case KEY_TYPE_NA:
-				tx_agg_desc->SecType = 0x0;
-				tx_agg_desc->NoEnc = 1;
-				break;
-			}
-		}
-
-		tx_agg_desc->QueueSelect = MapHwQueueToFirmwareQueue(tcb_desc->queue_index);
-		tx_agg_desc->TxFWInfoSize =  sizeof(tx_fwinfo_819x_usb);
-
-		tx_agg_desc->DISFB = tcb_desc->bTxDisableRateFallBack;
-		tx_agg_desc->USERATE = tcb_desc->bTxUseDriverAssingedRate;
-
-		tx_agg_desc->OWN = 1;
-
-		//DWORD 2
-		/* According windows driver, it seems that there no need to fill this field */
-
-		/* to fill next packet */
-		skb_put(agg_skb, TX_PACKET_DRVAGGR_SUBFRAME_SHIFT_BYTES);
-		memcpy(skb_put(agg_skb, skb->len), skb->data, skb->len);
-	}
-
-	for (i = 0; i < pSendList->nr_drv_agg_frames; i++)
-		dev_kfree_skb_any(pSendList->tx_agg_frames[i]);
-
-	return agg_skb;
-}
-
-/* NOTE:
-	This function return a list of PTCB which is proper to be aggregate with the input TCB.
-	If no proper TCB is found to do aggregation, SendList will only contain the input TCB.
-*/
-u8 DrvAggr_GetAggregatibleList(struct net_device *dev, struct sk_buff *skb,
-			       struct ieee80211_drv_agg_txb *pSendList)
-{
-	struct ieee80211_device *ieee = netdev_priv(dev);
-	PRT_HIGH_THROUGHPUT	pHTInfo = ieee->pHTInfo;
-	u16		nMaxAggrNum = pHTInfo->UsbTxAggrNum;
-	cb_desc		*tcb_desc = (cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
-	u8		QueueID = tcb_desc->queue_index;
-
-	do {
-		pSendList->tx_agg_frames[pSendList->nr_drv_agg_frames++] = skb;
-		if (pSendList->nr_drv_agg_frames >= nMaxAggrNum)
-			break;
-
-	} while ((skb = skb_dequeue(&ieee->skb_drv_aggQ[QueueID])));
-
-	RT_TRACE(COMP_AMSDU, "DrvAggr_GetAggregatibleList, nAggrTcbNum = %d \n", pSendList->nr_drv_agg_frames);
-	return pSendList->nr_drv_agg_frames;
-}
-#endif
-
 static void rtl8192_tx_isr(struct urb *tx_urb)
 {
 	struct sk_buff *skb = (struct sk_buff *)tx_urb->context;
@@ -1285,37 +1057,6 @@ static void rtl8192_tx_isr(struct urb *tx_urb)
 
 			return; //modified by david to avoid further processing AMSDU
 		}
-#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
-		else if ((skb_queue_len(&priv->ieee80211->skb_drv_aggQ[queue_index]) != 0) &&
-			 (!(priv->ieee80211->queue_stop))) {
-			// Tx Driver Aggregation process
-			/* The driver will aggregation the packets according to the following stats
-			 * 1. check whether there's tx irq available, for it's a completion return
-			 *    function, it should contain enough tx irq;
-			 * 2. check packet type;
-			 * 3. initialize sendlist, check whether the to-be send packet no greater than 1
-			 * 4. aggregates the packets, and fill firmware info and tx desc into it, etc.
-			 * 5. check whether the packet could be sent, otherwise just insert into wait head
-			 * */
-			skb = skb_dequeue(&priv->ieee80211->skb_drv_aggQ[queue_index]);
-			if (!check_nic_enough_desc(dev, queue_index)) {
-				skb_queue_head(&(priv->ieee80211->skb_drv_aggQ[queue_index]), skb);
-				return;
-			}
-
-			/*TODO*/
-			{
-				struct ieee80211_drv_agg_txb SendList;
-
-				memset(&SendList, 0, sizeof(struct ieee80211_drv_agg_txb));
-				if (DrvAggr_GetAggregatibleList(dev, skb, &SendList) > 1) {
-					skb = DrvAggr_Aggregation(dev, &SendList);
-
-				}
-			}
-			priv->ieee80211->softmac_hard_start_xmit(skb, dev);
-		}
-#endif
 	}
 
 }
@@ -1330,83 +1071,83 @@ static void rtl8192_config_rate(struct net_device *dev, u16 *rate_config)
 	for (i = 0; i < net->rates_len; i++) {
 		basic_rate = net->rates[i]&0x7f;
 		switch (basic_rate) {
-			case MGN_1M:
-				*rate_config |= RRSR_1M;
-				break;
-			case MGN_2M:
-				*rate_config |= RRSR_2M;
-				break;
-			case MGN_5_5M:
-				*rate_config |= RRSR_5_5M;
-				break;
-			case MGN_11M:
-				*rate_config |= RRSR_11M;
-				break;
-			case MGN_6M:
-				*rate_config |= RRSR_6M;
-				break;
-			case MGN_9M:
-				*rate_config |= RRSR_9M;
-				break;
-			case MGN_12M:
-				*rate_config |= RRSR_12M;
-				break;
-			case MGN_18M:
-				*rate_config |= RRSR_18M;
-				break;
-			case MGN_24M:
-				*rate_config |= RRSR_24M;
-				break;
-			case MGN_36M:
-				*rate_config |= RRSR_36M;
-				break;
-			case MGN_48M:
-				*rate_config |= RRSR_48M;
-				break;
-			case MGN_54M:
-				*rate_config |= RRSR_54M;
-				break;
+		case MGN_1M:
+			*rate_config |= RRSR_1M;
+			break;
+		case MGN_2M:
+			*rate_config |= RRSR_2M;
+			break;
+		case MGN_5_5M:
+			*rate_config |= RRSR_5_5M;
+			break;
+		case MGN_11M:
+			*rate_config |= RRSR_11M;
+			break;
+		case MGN_6M:
+			*rate_config |= RRSR_6M;
+			break;
+		case MGN_9M:
+			*rate_config |= RRSR_9M;
+			break;
+		case MGN_12M:
+			*rate_config |= RRSR_12M;
+			break;
+		case MGN_18M:
+			*rate_config |= RRSR_18M;
+			break;
+		case MGN_24M:
+			*rate_config |= RRSR_24M;
+			break;
+		case MGN_36M:
+			*rate_config |= RRSR_36M;
+			break;
+		case MGN_48M:
+			*rate_config |= RRSR_48M;
+			break;
+		case MGN_54M:
+			*rate_config |= RRSR_54M;
+			break;
 		}
 	}
 	for (i = 0; i < net->rates_ex_len; i++) {
 		basic_rate = net->rates_ex[i]&0x7f;
 		switch (basic_rate) {
-			case MGN_1M:
-				*rate_config |= RRSR_1M;
-				break;
-			case MGN_2M:
-				*rate_config |= RRSR_2M;
-				break;
-			case MGN_5_5M:
-				*rate_config |= RRSR_5_5M;
-				break;
-			case MGN_11M:
-				*rate_config |= RRSR_11M;
-				break;
-			case MGN_6M:
-				*rate_config |= RRSR_6M;
-				break;
-			case MGN_9M:
-				*rate_config |= RRSR_9M;
-				break;
-			case MGN_12M:
-				*rate_config |= RRSR_12M;
-				break;
-			case MGN_18M:
-				*rate_config |= RRSR_18M;
-				break;
-			case MGN_24M:
-				*rate_config |= RRSR_24M;
-				break;
-			case MGN_36M:
-				*rate_config |= RRSR_36M;
-				break;
-			case MGN_48M:
-				*rate_config |= RRSR_48M;
-				break;
-			case MGN_54M:
-				*rate_config |= RRSR_54M;
-				break;
+		case MGN_1M:
+			*rate_config |= RRSR_1M;
+			break;
+		case MGN_2M:
+			*rate_config |= RRSR_2M;
+			break;
+		case MGN_5_5M:
+			*rate_config |= RRSR_5_5M;
+			break;
+		case MGN_11M:
+			*rate_config |= RRSR_11M;
+			break;
+		case MGN_6M:
+			*rate_config |= RRSR_6M;
+			break;
+		case MGN_9M:
+			*rate_config |= RRSR_9M;
+			break;
+		case MGN_12M:
+			*rate_config |= RRSR_12M;
+			break;
+		case MGN_18M:
+			*rate_config |= RRSR_18M;
+			break;
+		case MGN_24M:
+			*rate_config |= RRSR_24M;
+			break;
+		case MGN_36M:
+			*rate_config |= RRSR_36M;
+			break;
+		case MGN_48M:
+			*rate_config |= RRSR_48M;
+			break;
+		case MGN_54M:
+			*rate_config |= RRSR_54M;
+			break;
 		}
 	}
 }
@@ -1486,28 +1227,6 @@ inline u8 rtl8192_IsWirelessBMode(u16 rate)
 
 u16 N_DBPSOfRate(u16 DataRate);
 
-u16 ComputeTxTime(u16 FrameLength, u16 DataRate, u8 bManagementFrame,
-		  u8 bShortPreamble)
-{
-	u16	FrameTime;
-	u16	N_DBPS;
-	u16	Ceiling;
-
-	if (rtl8192_IsWirelessBMode(DataRate)) {
-		if (bManagementFrame || !bShortPreamble || DataRate == 10) /* long preamble */
-			FrameTime = (u16)(144+48+(FrameLength*8/(DataRate/10)));
-		else // Short preamble
-			FrameTime = (u16)(72+24+(FrameLength*8/(DataRate/10)));
-		if ((FrameLength*8 % (DataRate/10)) != 0) /* Get the Ceilling */
-			FrameTime++;
-	} else {	//802.11g DSSS-OFDM PLCP length field calculation.
-		N_DBPS = N_DBPSOfRate(DataRate);
-		Ceiling = (16 + 8*FrameLength + 6) / N_DBPS
-			+ (((16 + 8*FrameLength + 6) % N_DBPS) ? 1 : 0);
-		FrameTime = (u16)(16 + 4 + 4*Ceiling + 6);
-	}
-	return FrameTime;
-}
 
 u16 N_DBPSOfRate(u16 DataRate)
 {
@@ -1553,15 +1272,6 @@ u16 N_DBPSOfRate(u16 DataRate)
 	return N_DBPS;
 }
 
-unsigned int txqueue2outpipe(struct r8192_priv *priv, unsigned int tx_queue)
-{
-	if (tx_queue >= 9) {
-		RT_TRACE(COMP_ERR, "%s():Unknown queue ID!!!\n", __func__);
-		return 0x04;
-	}
-	return priv->txqueue_to_outpipemap[tx_queue];
-}
-
 short rtl819xU_tx_cmd(struct net_device *dev, struct sk_buff *skb)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
@@ -1591,12 +1301,7 @@ short rtl819xU_tx_cmd(struct net_device *dev, struct sk_buff *skb)
 	//----------------------------------------------------------------------------
 	// Fill up USB_OUT_CONTEXT.
 	//----------------------------------------------------------------------------
-	// Get index to out pipe from specified QueueID.
-#ifndef USE_ONE_PIPE
-	idx_pipe = txqueue2outpipe(priv, queue_index);
-#else
 	idx_pipe = 0x04;
-#endif
 	usb_fill_bulk_urb(tx_urb, priv->udev, usb_sndbulkpipe(priv->udev, idx_pipe),
 			  skb->data, skb->len, rtl8192_tx_isr, skb);
 
@@ -1666,98 +1371,98 @@ static u8 MRateToHwRate8190Pci(u8 rate)
 	u8  ret = DESC90_RATE1M;
 
 	switch (rate) {
-		case MGN_1M:
-			ret = DESC90_RATE1M;
-			break;
-		case MGN_2M:
-			ret = DESC90_RATE2M;
-			break;
-		case MGN_5_5M:
-			ret = DESC90_RATE5_5M;
-			break;
-		case MGN_11M:
-			ret = DESC90_RATE11M;
-			break;
-		case MGN_6M:
-			ret = DESC90_RATE6M;
-			break;
-		case MGN_9M:
-			ret = DESC90_RATE9M;
-			break;
-		case MGN_12M:
-			ret = DESC90_RATE12M;
-			break;
-		case MGN_18M:
-			ret = DESC90_RATE18M;
-			break;
-		case MGN_24M:
-			ret = DESC90_RATE24M;
-			break;
-		case MGN_36M:
-			ret = DESC90_RATE36M;
-			break;
-		case MGN_48M:
-			ret = DESC90_RATE48M;
-			break;
-		case MGN_54M:
-			ret = DESC90_RATE54M;
-			break;
+	case MGN_1M:
+		ret = DESC90_RATE1M;
+		break;
+	case MGN_2M:
+		ret = DESC90_RATE2M;
+		break;
+	case MGN_5_5M:
+		ret = DESC90_RATE5_5M;
+		break;
+	case MGN_11M:
+		ret = DESC90_RATE11M;
+		break;
+	case MGN_6M:
+		ret = DESC90_RATE6M;
+		break;
+	case MGN_9M:
+		ret = DESC90_RATE9M;
+		break;
+	case MGN_12M:
+		ret = DESC90_RATE12M;
+		break;
+	case MGN_18M:
+		ret = DESC90_RATE18M;
+		break;
+	case MGN_24M:
+		ret = DESC90_RATE24M;
+		break;
+	case MGN_36M:
+		ret = DESC90_RATE36M;
+		break;
+	case MGN_48M:
+		ret = DESC90_RATE48M;
+		break;
+	case MGN_54M:
+		ret = DESC90_RATE54M;
+		break;
 
-		// HT rate since here
-		case MGN_MCS0:
-			ret = DESC90_RATEMCS0;
-			break;
-		case MGN_MCS1:
-			ret = DESC90_RATEMCS1;
-			break;
-		case MGN_MCS2:
-			ret = DESC90_RATEMCS2;
-			break;
-		case MGN_MCS3:
-			ret = DESC90_RATEMCS3;
-			break;
-		case MGN_MCS4:
-			ret = DESC90_RATEMCS4;
-			break;
-		case MGN_MCS5:
-			ret = DESC90_RATEMCS5;
-			break;
-		case MGN_MCS6:
-			ret = DESC90_RATEMCS6;
-			break;
-		case MGN_MCS7:
-			ret = DESC90_RATEMCS7;
-			break;
-		case MGN_MCS8:
-			ret = DESC90_RATEMCS8;
-			break;
-		case MGN_MCS9:
-			ret = DESC90_RATEMCS9;
-			break;
-		case MGN_MCS10:
-			ret = DESC90_RATEMCS10;
-			break;
-		case MGN_MCS11:
-			ret = DESC90_RATEMCS11;
-			break;
-		case MGN_MCS12:
-			ret = DESC90_RATEMCS12;
-			break;
-		case MGN_MCS13:
-			ret = DESC90_RATEMCS13;
-			break;
-		case MGN_MCS14:
-			ret = DESC90_RATEMCS14;
-			break;
-		case MGN_MCS15:
-			ret = DESC90_RATEMCS15;
-			break;
-		case (0x80|0x20):
-			ret = DESC90_RATEMCS32;
-			break;
+	/* HT rate since here */
+	case MGN_MCS0:
+		ret = DESC90_RATEMCS0;
+		break;
+	case MGN_MCS1:
+		ret = DESC90_RATEMCS1;
+		break;
+	case MGN_MCS2:
+		ret = DESC90_RATEMCS2;
+		break;
+	case MGN_MCS3:
+		ret = DESC90_RATEMCS3;
+		break;
+	case MGN_MCS4:
+		ret = DESC90_RATEMCS4;
+		break;
+	case MGN_MCS5:
+		ret = DESC90_RATEMCS5;
+		break;
+	case MGN_MCS6:
+		ret = DESC90_RATEMCS6;
+		break;
+	case MGN_MCS7:
+		ret = DESC90_RATEMCS7;
+		break;
+	case MGN_MCS8:
+		ret = DESC90_RATEMCS8;
+		break;
+	case MGN_MCS9:
+		ret = DESC90_RATEMCS9;
+		break;
+	case MGN_MCS10:
+		ret = DESC90_RATEMCS10;
+		break;
+	case MGN_MCS11:
+		ret = DESC90_RATEMCS11;
+		break;
+	case MGN_MCS12:
+		ret = DESC90_RATEMCS12;
+		break;
+	case MGN_MCS13:
+		ret = DESC90_RATEMCS13;
+		break;
+	case MGN_MCS14:
+		ret = DESC90_RATEMCS14;
+		break;
+	case MGN_MCS15:
+		ret = DESC90_RATEMCS15;
+		break;
+	case (0x80|0x20):
+		ret = DESC90_RATEMCS32;
+		break;
 
-		default:
-			break;
+	default:
+		break;
 	}
 	return ret;
 }
@@ -1857,25 +1562,13 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
 		tx_fwinfo->TxSubCarrier = 0;
 	}
 
-#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
-	if (tcb_desc->drv_agg_enable)
-		tx_fwinfo->Tx_INFO_RSVD = (tcb_desc->DrvAggrNum & 0x1f) << 1;
-#endif
 	/* Fill Tx descriptor */
 	memset(tx_desc, 0, sizeof(tx_desc_819x_usb));
 	/* DWORD 0 */
 	tx_desc->LINIP = 0;
 	tx_desc->CmdInit = 1;
 	tx_desc->Offset =  sizeof(tx_fwinfo_819x_usb) + 8;
-
-#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
-	if (tcb_desc->drv_agg_enable)
-		tx_desc->PktSize = tcb_desc->pkt_size;
-	else
-#endif
-	{
-		tx_desc->PktSize = (skb->len - TX_PACKET_SHIFT_BYTES) & 0xffff;
-	}
+	tx_desc->PktSize = (skb->len - TX_PACKET_SHIFT_BYTES) & 0xffff;
 
 	/*DWORD 1*/
 	tx_desc->SecCAMID = 0;
@@ -1916,21 +1609,9 @@ short rtl8192_tx(struct net_device *dev, struct sk_buff *skb)
 	tx_desc->LastSeg = 1;
 	tx_desc->OWN = 1;
 
-#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
-	if (tcb_desc->drv_agg_enable) {
-		tx_desc->TxBufferSize = tcb_desc->pkt_size + sizeof(tx_fwinfo_819x_usb);
-	} else
-#endif
-	{
-		//DWORD 2
-		tx_desc->TxBufferSize = (u32)(skb->len - USB_HWDESC_HEADER_LEN);
-	}
-	/* Get index to out pipe from specified QueueID */
-#ifndef USE_ONE_PIPE
-	idx_pipe = txqueue2outpipe(priv, tcb_desc->queue_index);
-#else
+	/* DWORD 2 */
+	tx_desc->TxBufferSize = (u32)(skb->len - USB_HWDESC_HEADER_LEN);
 	idx_pipe = 0x5;
-#endif
 
 	/* To submit bulk urb */
 	usb_fill_bulk_urb(tx_urb, udev,
@@ -2230,19 +1911,18 @@ static int rtl8192_handle_beacon(struct net_device *dev,
 static int rtl8192_qos_association_resp(struct r8192_priv *priv,
 					struct ieee80211_network *network)
 {
-	int ret = 0;
 	unsigned long flags;
 	u32 size = sizeof(struct ieee80211_qos_parameters);
 	int set_qos_param = 0;
 
 	if ((priv == NULL) || (network == NULL))
-		return ret;
+		return 0;
 
 	if (priv->ieee80211->state != IEEE80211_LINKED)
-		return ret;
+		return 0;
 
 	if ((priv->ieee80211->iw_mode != IW_MODE_INFRA))
-		return ret;
+		return 0;
 
 	spin_lock_irqsave(&priv->ieee80211->lock, flags);
 	if (network->flags & NETWORK_HAS_QOS_PARAMETERS) {
@@ -2271,7 +1951,7 @@ static int rtl8192_qos_association_resp(struct r8192_priv *priv,
 		queue_work(priv->priv_wq, &priv->qos_activate);
 
 
-	return ret;
+	return 0;
 }
 
 
@@ -2869,9 +2549,6 @@ static short rtl8192_init(struct net_device *dev)
 		return -ENOMEM;
 	}
 
-#ifdef DEBUG_EPROM
-	dump_eprom(dev);
-#endif
 	return 0;
 }
 
@@ -3066,22 +2743,6 @@ static bool rtl8192_adapter_start(struct net_device *dev)
 		for (i = 0; i < QOS_QUEUE_NUM; i++)
 			write_nic_dword(dev, WDCAPARA_ADD[i], DEFAULT_EDCA);
 	}
-#ifdef USB_RX_AGGREGATION_SUPPORT
-	//3 For usb rx firmware aggregation control
-	if (priv->ResetProgress == RESET_TYPE_NORESET) {
-		u32 ulValue;
-		PRT_HIGH_THROUGHPUT	pHTInfo = priv->ieee80211->pHTInfo;
-		ulValue = (pHTInfo->UsbRxFwAggrEn<<24) | (pHTInfo->UsbRxFwAggrPageNum<<16) |
-			  (pHTInfo->UsbRxFwAggrPacketNum<<8) | (pHTInfo->UsbRxFwAggrTimeout);
-		/*
-		 * If usb rx firmware aggregation is enabled,
-		 * when anyone of three threshold conditions above is reached,
-		 * firmware will send aggregated packet to driver.
-		 */
-		write_nic_dword(dev, 0x1a8, ulValue);
-		priv->bCurrentRxAggrEnable = true;
-	}
-#endif
 
 	rtl8192_phy_configmac(dev);
 
@@ -3235,12 +2896,8 @@ static RESET_TYPE TxCheckStuck(struct net_device *dev)
 	for (QueueID = 0; QueueID <= BEACON_QUEUE; QueueID++) {
 		if (QueueID == TXCMD_QUEUE)
 			continue;
-#ifdef USB_TX_DRIVER_AGGREGATION_ENABLE
-		if ((skb_queue_len(&priv->ieee80211->skb_waitQ[QueueID]) == 0) && (skb_queue_len(&priv->ieee80211->skb_aggQ[QueueID]) == 0) && (skb_queue_len(&priv->ieee80211->skb_drv_aggQ[QueueID]) == 0))
-#else
 		if ((skb_queue_len(&priv->ieee80211->skb_waitQ[QueueID]) == 0)  && (skb_queue_len(&priv->ieee80211->skb_aggQ[QueueID]) == 0))
-#endif
-				continue;
+			continue;
 
 		bCheckFwTxCnt = true;
 	}
@@ -3532,34 +3189,6 @@ RESET_START:
 	}
 }
 
-void CAM_read_entry(struct net_device *dev, u32 iIndex)
-{
-	u32 target_command = 0;
-	u32 target_content = 0;
-	u8 entry_i = 0;
-	u32 ulStatus;
-	s32 i = 100;
-	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-		// polling bit, and No Write enable, and address
-		target_command = entry_i+CAM_CONTENT_COUNT*iIndex;
-		target_command = target_command | BIT31;
-
-		//Check polling bit is clear
-		while ((i--) >= 0) {
-			read_nic_dword(dev, RWCAM, &ulStatus);
-			if (ulStatus & BIT31)
-				continue;
-			else
-				break;
-		}
-		write_nic_dword(dev, RWCAM, target_command);
-		RT_TRACE(COMP_SEC, "CAM_read_entry(): WRITE A0: %x \n", target_command);
-		read_nic_dword(dev, RCAMO, &target_content);
-		RT_TRACE(COMP_SEC, "CAM_read_entry(): WRITE A8: %x \n", target_content);
-	}
-	printk("\n");
-}
-
 static void rtl819x_update_rxcounts(struct r8192_priv *priv, u32 *TotalRxBcnNum,
 			     u32 *TotalRxDataNum)
 {
@@ -4656,102 +4285,96 @@ UpdateReceivedRateHistogramStatistics8190(struct net_device *dev,
 		preamble_guardinterval = 0;// long
 
 	switch (stats->rate) {
-		//
-		// CCK rate
-		//
-		case MGN_1M:
-			rateIndex = 0;
-			break;
-		case MGN_2M:
-			rateIndex = 1;
-			break;
-		case MGN_5_5M:
-			rateIndex = 2;
-			break;
-		case MGN_11M:
-			rateIndex = 3;
-			break;
-		//
-		// Legacy OFDM rate
-		//
-		case MGN_6M:
-			rateIndex = 4;
-			break;
-		case MGN_9M:
-			rateIndex = 5;
-			break;
-		case MGN_12M:
-			rateIndex = 6;
-			break;
-		case MGN_18M:
-			rateIndex = 7;
-			break;
-		case MGN_24M:
-			rateIndex = 8;
-			break;
-		case MGN_36M:
-			rateIndex = 9;
-			break;
-		case MGN_48M:
-			rateIndex = 10;
-			break;
-		case MGN_54M:
-			rateIndex = 11;
-			break;
-		//
-		// 11n High throughput rate
-		//
-		case MGN_MCS0:
-			rateIndex = 12;
-			break;
-		case MGN_MCS1:
-			rateIndex = 13;
-			break;
-		case MGN_MCS2:
-			rateIndex = 14;
-			break;
-		case MGN_MCS3:
-			rateIndex = 15;
-			break;
-		case MGN_MCS4:
-			rateIndex = 16;
-			break;
-		case MGN_MCS5:
-			rateIndex = 17;
-			break;
-		case MGN_MCS6:
-			rateIndex = 18;
-			break;
-		case MGN_MCS7:
-			rateIndex = 19;
-			break;
-		case MGN_MCS8:
-			rateIndex = 20;
-			break;
-		case MGN_MCS9:
-			rateIndex = 21;
-			break;
-		case MGN_MCS10:
-			rateIndex = 22;
-			break;
-		case MGN_MCS11:
-			rateIndex = 23;
-			break;
-		case MGN_MCS12:
-			rateIndex = 24;
-			break;
-		case MGN_MCS13:
-			rateIndex = 25;
-			break;
-		case MGN_MCS14:
-			rateIndex = 26;
-			break;
-		case MGN_MCS15:
-			rateIndex = 27;
-			break;
-		default:
-			rateIndex = 28;
-			break;
+	/* CCK rate */
+	case MGN_1M:
+		rateIndex = 0;
+		break;
+	case MGN_2M:
+		rateIndex = 1;
+		break;
+	case MGN_5_5M:
+		rateIndex = 2;
+		break;
+	case MGN_11M:
+		rateIndex = 3;
+		break;
+	/* Legacy OFDM rate */
+	case MGN_6M:
+		rateIndex = 4;
+		break;
+	case MGN_9M:
+		rateIndex = 5;
+		break;
+	case MGN_12M:
+		rateIndex = 6;
+		break;
+	case MGN_18M:
+		rateIndex = 7;
+		break;
+	case MGN_24M:
+		rateIndex = 8;
+		break;
+	case MGN_36M:
+		rateIndex = 9;
+		break;
+	case MGN_48M:
+		rateIndex = 10;
+		break;
+	case MGN_54M:
+		rateIndex = 11;
+		break;
+	/* 11n High throughput rate */
+	case MGN_MCS0:
+		rateIndex = 12;
+		break;
+	case MGN_MCS1:
+		rateIndex = 13;
+		break;
+	case MGN_MCS2:
+		rateIndex = 14;
+		break;
+	case MGN_MCS3:
+		rateIndex = 15;
+		break;
+	case MGN_MCS4:
+		rateIndex = 16;
+		break;
+	case MGN_MCS5:
+		rateIndex = 17;
+		break;
+	case MGN_MCS6:
+		rateIndex = 18;
+		break;
+	case MGN_MCS7:
+		rateIndex = 19;
+		break;
+	case MGN_MCS8:
+		rateIndex = 20;
+		break;
+	case MGN_MCS9:
+		rateIndex = 21;
+		break;
+	case MGN_MCS10:
+		rateIndex = 22;
+		break;
+	case MGN_MCS11:
+		rateIndex = 23;
+		break;
+	case MGN_MCS12:
+		rateIndex = 24;
+		break;
+	case MGN_MCS13:
+		rateIndex = 25;
+		break;
+	case MGN_MCS14:
+		rateIndex = 26;
+		break;
+	case MGN_MCS15:
+		rateIndex = 27;
+		break;
+	default:
+		rateIndex = 28;
+		break;
 	}
 	priv->stats.received_preamble_GI[preamble_guardinterval][rateIndex]++;
 	priv->stats.received_rate_histogram[0][rateIndex]++; //total
@@ -4771,30 +4394,16 @@ static void query_rxdesc_status(struct sk_buff *skb,
 	//
 	//Get Rx Descriptor Information
 	//
-#ifdef USB_RX_AGGREGATION_SUPPORT
-	if (bIsRxAggrSubframe) {
-		rx_desc_819x_usb_aggr_subframe *desc = (rx_desc_819x_usb_aggr_subframe *)skb->data;
-		stats->Length = desc->Length;
-		stats->RxDrvInfoSize = desc->RxDrvInfoSize;
-		stats->RxBufShift = 0; //RxBufShift = 2 in RxDesc, but usb didn't shift bytes in fact.
-		stats->bICV = desc->ICV;
-		stats->bCRC = desc->CRC32;
-		stats->bHwError = stats->bCRC|stats->bICV;
-		stats->Decrypted = !desc->SWDec;//RTL8190 set this bit to indicate that Hw does not decrypt packet
-	} else
-#endif
-	{
-		rx_desc_819x_usb *desc = (rx_desc_819x_usb *)skb->data;
+	rx_desc_819x_usb *desc = (rx_desc_819x_usb *)skb->data;
 
-		stats->Length = desc->Length;
-		stats->RxDrvInfoSize = desc->RxDrvInfoSize;
-		stats->RxBufShift = 0;
-		stats->bICV = desc->ICV;
-		stats->bCRC = desc->CRC32;
-		stats->bHwError = stats->bCRC|stats->bICV;
-		//RTL8190 set this bit to indicate that Hw does not decrypt packet
-		stats->Decrypted = !desc->SWDec;
-	}
+	stats->Length = desc->Length;
+	stats->RxDrvInfoSize = desc->RxDrvInfoSize;
+	stats->RxBufShift = 0;
+	stats->bICV = desc->ICV;
+	stats->bCRC = desc->CRC32;
+	stats->bHwError = stats->bCRC|stats->bICV;
+	/* RTL8190 set this bit to indicate that Hw does not decrypt packet */
+	stats->Decrypted = !desc->SWDec;
 
 	if ((priv->ieee80211->pHTInfo->bCurrentHTSupport == true) && (priv->ieee80211->pairwise_key_type == KEY_TYPE_CCMP))
 		stats->bHwError = false;
@@ -4859,11 +4468,6 @@ static void query_rxdesc_status(struct sk_buff *skb,
 		skb_pull(skb, stats->RxBufShift + stats->RxDrvInfoSize);
 	}
 
-#ifdef USB_RX_AGGREGATION_SUPPORT
-	/* for the rx aggregated sub frame, the redundant space truly contained in the packet */
-	if (bIsRxAggrSubframe)
-		skb_pull(skb, 8);
-#endif
 	/* for debug 2008.5.29 */
 
 	//added by vivi, for MP, 20080108
@@ -4873,18 +4477,6 @@ static void query_rxdesc_status(struct sk_buff *skb,
 
 }
 
-u32 GetRxPacketShiftBytes819xUsb(struct ieee80211_rx_stats  *Status, bool bIsRxAggrSubframe)
-{
-#ifdef USB_RX_AGGREGATION_SUPPORT
-	if (bIsRxAggrSubframe)
-		return (sizeof(rx_desc_819x_usb) + Status->RxDrvInfoSize
-			+ Status->RxBufShift + 8);
-	else
-#endif
-		return (sizeof(rx_desc_819x_usb) + Status->RxDrvInfoSize
-			+ Status->RxBufShift);
-}
-
 static void rtl8192_rx_nomal(struct sk_buff *skb)
 {
 	rtl8192_rx_info *info = (struct rtl8192_rx_info *)skb->cb;
@@ -4899,42 +4491,13 @@ static void rtl8192_rx_nomal(struct sk_buff *skb)
 	u32 rx_pkt_len = 0;
 	struct ieee80211_hdr_1addr *ieee80211_hdr = NULL;
 	bool unicast_packet = false;
-#ifdef USB_RX_AGGREGATION_SUPPORT
-	struct sk_buff *agg_skb = NULL;
-	u32  TotalLength = 0;
-	u32  TempDWord = 0;
-	u32  PacketLength = 0;
-	u32  PacketOccupiedLendth = 0;
-	u8   TempByte = 0;
-	u32  PacketShiftBytes = 0;
-	rx_desc_819x_usb_aggr_subframe *RxDescr = NULL;
-	u8  PaddingBytes = 0;
-	//add just for testing
-	u8   testing;
-
-#endif
 
 	/* 20 is for ps-poll */
 	if ((skb->len >= (20 + sizeof(rx_desc_819x_usb))) && (skb->len < RX_URB_SIZE)) {
-#ifdef USB_RX_AGGREGATION_SUPPORT
-		TempByte = *(skb->data + sizeof(rx_desc_819x_usb));
-#endif
 		/* first packet should not contain Rx aggregation header */
 		query_rxdesc_status(skb, &stats, false);
 		/* TODO */
 		/* hardware related info */
-#ifdef USB_RX_AGGREGATION_SUPPORT
-		if (TempByte & BIT0) {
-			agg_skb = skb;
-			TotalLength = stats.Length - 4; /*sCrcLng*/
-			/* though the head pointer has passed this position  */
-			TempDWord = *(u32 *)(agg_skb->data - 4);
-			PacketLength = (u16)(TempDWord & 0x3FFF); /*sCrcLng*/
-			skb = dev_alloc_skb(PacketLength);
-			memcpy(skb_put(skb, PacketLength), agg_skb->data, PacketLength);
-			PacketShiftBytes = GetRxPacketShiftBytes819xUsb(&stats, false);
-		}
-#endif
 		/* Process the MPDU received */
 		skb_trim(skb, skb->len - 4/*sCrcLng*/);
 
@@ -4957,76 +4520,6 @@ static void rtl8192_rx_nomal(struct sk_buff *skb)
 			if (unicast_packet)
 				priv->stats.rxbytesunicast += rx_pkt_len;
 		}
-#ifdef USB_RX_AGGREGATION_SUPPORT
-		testing = 1;
-		if (TotalLength > 0) {
-			PacketOccupiedLendth = PacketLength + (PacketShiftBytes + 8);
-			if ((PacketOccupiedLendth & 0xFF) != 0)
-				PacketOccupiedLendth = (PacketOccupiedLendth & 0xFFFFFF00) + 256;
-			PacketOccupiedLendth -= 8;
-			TempDWord = PacketOccupiedLendth - PacketShiftBytes; /*- PacketLength */
-			if (agg_skb->len > TempDWord)
-				skb_pull(agg_skb, TempDWord);
-			else
-				agg_skb->len = 0;
-
-			while (agg_skb->len >= GetRxPacketShiftBytes819xUsb(&stats, true)) {
-				u8 tmpCRC = 0, tmpICV = 0;
-				RxDescr = (rx_desc_819x_usb_aggr_subframe *)(agg_skb->data);
-				tmpCRC = RxDescr->CRC32;
-				tmpICV = RxDescr->ICV;
-				memcpy(agg_skb->data, &agg_skb->data[44], 2);
-				RxDescr->CRC32 = tmpCRC;
-				RxDescr->ICV = tmpICV;
-
-				memset(&stats, 0, sizeof(struct ieee80211_rx_stats));
-				stats.signal = 0;
-				stats.noise = -98;
-				stats.rate = 0;
-				stats.freq = IEEE80211_24GHZ_BAND;
-				query_rxdesc_status(agg_skb, &stats, true);
-				PacketLength = stats.Length;
-
-				if (PacketLength > agg_skb->len)
-					break;
-				/* Process the MPDU received */
-				skb = dev_alloc_skb(PacketLength);
-				memcpy(skb_put(skb, PacketLength), agg_skb->data, PacketLength);
-				skb_trim(skb, skb->len - 4/*sCrcLng*/);
-
-				rx_pkt_len = skb->len;
-				ieee80211_hdr = (struct ieee80211_hdr_1addr *)skb->data;
-				unicast_packet = false;
-				if (is_broadcast_ether_addr(ieee80211_hdr->addr1)) {
-					//TODO
-				} else if (is_multicast_ether_addr(ieee80211_hdr->addr1)) {
-					//TODO
-				} else {
-					/* unicast packet */
-					unicast_packet = true;
-				}
-				if (!ieee80211_rx(priv->ieee80211, skb, &stats)) {
-					dev_kfree_skb_any(skb);
-				} else {
-					priv->stats.rxoktotal++;
-					if (unicast_packet)
-						priv->stats.rxbytesunicast += rx_pkt_len;
-				}
-				/* should trim the packet which has been copied to target skb */
-				skb_pull(agg_skb, PacketLength);
-				PacketShiftBytes = GetRxPacketShiftBytes819xUsb(&stats, true);
-				PacketOccupiedLendth = PacketLength + PacketShiftBytes;
-				if ((PacketOccupiedLendth & 0xFF) != 0) {
-					PaddingBytes = 256 - (PacketOccupiedLendth & 0xFF);
-					if (agg_skb->len > PaddingBytes)
-						skb_pull(agg_skb, PaddingBytes);
-					else
-						agg_skb->len = 0;
-				}
-			}
-			dev_kfree_skb(agg_skb);
-		}
-#endif
 	} else {
 		priv->stats.rxurberr++;
 		netdev_dbg(dev, "actual_length: %d\n", skb->len);
@@ -5276,18 +4769,6 @@ static void rtl8192_usb_disconnect(struct usb_interface *intf)
 	RT_TRACE(COMP_DOWN, "wlan driver removed\n");
 }
 
-/* fun with the built-in ieee80211 stack... */
-extern int ieee80211_debug_init(void);
-extern void ieee80211_debug_exit(void);
-extern int ieee80211_crypto_init(void);
-extern void ieee80211_crypto_deinit(void);
-extern int ieee80211_crypto_tkip_init(void);
-extern void ieee80211_crypto_tkip_exit(void);
-extern int ieee80211_crypto_ccmp_init(void);
-extern void ieee80211_crypto_ccmp_exit(void);
-extern int ieee80211_crypto_wep_init(void);
-extern void ieee80211_crypto_wep_exit(void);
-
 static int __init rtl8192_usb_module_init(void)
 {
 	int ret;
diff --git a/drivers/staging/rtl8192u/r8192U_dm.c b/drivers/staging/rtl8192u/r8192U_dm.c
index b4264e1353c9..b0b66fba563b 100644
--- a/drivers/staging/rtl8192u/r8192U_dm.c
+++ b/drivers/staging/rtl8192u/r8192U_dm.c
@@ -1474,40 +1474,40 @@ static void dm_CCKTxPowerAdjust_TSSI(struct net_device *dev, bool  bInCH14)
 		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[0] +
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[1]<<8) ;
 
-		rtl8192_setBBreg(dev, rCCK0_TxFilter1,bMaskHWord, TempVal);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		//Write 0xa24 ~ 0xa27
 		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[2] +
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[3]<<8) +
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[4]<<16)+
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[5]<<24);
-		rtl8192_setBBreg(dev, rCCK0_TxFilter2,bMaskDWord, TempVal);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		//Write 0xa28  0xa29
 		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[6] +
 					(priv->cck_txbbgain_table[priv->cck_present_attentuation].ccktxbb_valuearray[7]<<8) ;
 
-		rtl8192_setBBreg(dev, rCCK0_DebugPort,bMaskLWord, TempVal);
+		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 	}
 	else
 	{
 		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[0] +
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[1]<<8) ;
 
-		rtl8192_setBBreg(dev, rCCK0_TxFilter1,bMaskHWord, TempVal);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter1, bMaskHWord, TempVal);
 		//Write 0xa24 ~ 0xa27
 		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[2] +
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[3]<<8) +
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[4]<<16)+
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[5]<<24);
-		rtl8192_setBBreg(dev, rCCK0_TxFilter2,bMaskDWord, TempVal);
+		rtl8192_setBBreg(dev, rCCK0_TxFilter2, bMaskDWord, TempVal);
 		//Write 0xa28  0xa29
 		TempVal = 0;
 		TempVal =	priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[6] +
 					(priv->cck_txbbgain_ch14_table[priv->cck_present_attentuation].ccktxbb_valuearray[7]<<8) ;
 
-		rtl8192_setBBreg(dev, rCCK0_DebugPort,bMaskLWord, TempVal);
+		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
 	}
 
 
@@ -1570,7 +1570,7 @@ static void dm_CCKTxPowerAdjust_ThermalMeter(struct net_device *dev,	bool  bInCH
 					(CCKSwingTable_Ch14[priv->CCK_index][7]<<8) ;
 
 		rtl8192_setBBreg(dev, rCCK0_DebugPort, bMaskLWord, TempVal);
-		RT_TRACE(COMP_POWER_TRACKING,"CCK chnl 14, reg 0x%x = 0x%x\n",
+		RT_TRACE(COMP_POWER_TRACKING, "CCK chnl 14, reg 0x%x = 0x%x\n",
 			rCCK0_DebugPort, TempVal);
 	}
 }
@@ -1601,7 +1601,7 @@ static void dm_txpower_reset_recovery(
 	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in RFA_txPowerTrackingIndex is %x\n",priv->rfa_txpowertrackingindex);
 	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery : RF A I/Q Amplify Gain is %ld\n",priv->txbbgain_table[priv->rfa_txpowertrackingindex].txbb_iq_amplifygain);
 	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: CCK Attenuation is %d dB\n",priv->cck_present_attentuation);
-	dm_cck_txpower_adjust(dev,priv->bcck_in_ch14);
+	dm_cck_txpower_adjust(dev, priv->bcck_in_ch14);
 
 	rtl8192_setBBreg(dev, rOFDM0_XCTxIQImbalance, bMaskDWord, priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbbgain_value);
 	RT_TRACE(COMP_POWER_TRACKING, "Reset Recovery: Fill in 0xc90 is %08x\n",priv->txbbgain_table[priv->rfc_txpowertrackingindex].txbbgain_value);
@@ -2431,7 +2431,7 @@ static	void dm_cs_ratio(
 	struct net_device *dev)
 {
 	struct r8192_priv *priv = ieee80211_priv(dev);
-	static u8				initialized,force_write;
+	static u8				initialized, force_write;
 	static u32			reset_cnt;
 
 	if(dm_digtable.dig_algorithm_switch)
@@ -3134,7 +3134,7 @@ void dm_fsync_timer_callback(unsigned long data)
 			priv->bswitch_fsync = !priv->bswitch_fsync;
 			if(priv->bswitch_fsync)
 			{
-				write_nic_byte(dev,0xC36, 0x1c);
+				write_nic_byte(dev, 0xC36, 0x1c);
 				write_nic_byte(dev, 0xC3e, 0x90);
 			}
 			else
@@ -3216,7 +3216,7 @@ static void dm_StartSWFsync(struct net_device *dev)
 	u32			rateIndex;
 	u32			rateBitmap;
 
-	RT_TRACE(COMP_HALDM,"%s\n", __func__);
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	// Initial rate record to zero, start to record.
 	priv->rate_record = 0;
 	// Initialize continue diff count to zero, start to record.
@@ -3251,7 +3251,7 @@ static void dm_StartSWFsync(struct net_device *dev)
 
 static void dm_EndHWFsync(struct net_device *dev)
 {
-	RT_TRACE(COMP_HALDM,"%s\n", __func__);
+	RT_TRACE(COMP_HALDM, "%s\n", __func__);
 	write_nic_dword(dev, rOFDM0_RxDetector2, 0x465c52cd);
 	write_nic_byte(dev, 0xc3b, 0x49);
 
diff --git a/drivers/staging/rtl8192u/r8192U_hw.h b/drivers/staging/rtl8192u/r8192U_hw.h
index dd07a735b537..a93694ff0c42 100644
--- a/drivers/staging/rtl8192u/r8192U_hw.h
+++ b/drivers/staging/rtl8192u/r8192U_hw.h
@@ -24,7 +24,7 @@ typedef enum _VERSION_819xU{
 	VERSION_819xU_A, // A-cut
 	VERSION_819xU_B, // B-cut
 	VERSION_819xU_C,// C-cut
-}VERSION_819xU,*PVERSION_819xU;
+} VERSION_819xU, *PVERSION_819xU;
 //added for different RF type
 typedef enum _RT_RF_TYPE_DEF
 {
diff --git a/drivers/staging/rtl8192u/r8192U_wx.c b/drivers/staging/rtl8192u/r8192U_wx.c
index 6808e872296e..28f60d2dbe5b 100644
--- a/drivers/staging/rtl8192u/r8192U_wx.c
+++ b/drivers/staging/rtl8192u/r8192U_wx.c
@@ -22,9 +22,10 @@
 #include "r8192U_hw.h"
 
 #include "dot11d.h"
+#include "r8192U_wx.h"
 
 #define RATE_COUNT 12
-u32 rtl8180_rates[] = {1000000, 2000000, 5500000, 11000000,
+static const u32 rtl8180_rates[] = {1000000, 2000000, 5500000, 11000000,
 	6000000, 9000000, 12000000, 18000000, 24000000, 36000000, 48000000, 54000000};
 
 
@@ -344,6 +345,7 @@ static int r8192_wx_set_scan(struct net_device *dev, struct iw_request_info *a,
 		return -EAGAIN;
 	if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 		struct iw_scan_req *req = (struct iw_scan_req *)b;
+
 		if (req->essid_len) {
 			ieee->current_network.ssid_len = req->essid_len;
 			memcpy(ieee->current_network.ssid, req->essid, req->essid_len);
@@ -758,6 +760,7 @@ static int r8192_wx_set_enc_ext(struct net_device *dev,
 		struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
 		struct iw_point *encoding = &wrqu->encoding;
 		u8 idx = 0, alg = 0, group = 0;
+
 		if ((encoding->flags & IW_ENCODE_DISABLED) || ext->alg == IW_ENCODE_ALG_NONE)
 			/* none is not allowed to use hwsec WB 2008.07.01 */
 			goto end_hw_sec;
diff --git a/drivers/staging/rtl8192u/r819xU_firmware.c b/drivers/staging/rtl8192u/r819xU_firmware.c
index 04b8f5ec67e9..f66ad8a0dfe0 100644
--- a/drivers/staging/rtl8192u/r819xU_firmware.c
+++ b/drivers/staging/rtl8192u/r819xU_firmware.c
@@ -51,7 +51,7 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 	frag_threshold = pfirmware->cmdpacket_frag_thresold;
 	do {
 		if ((buffer_len - frag_offset) > frag_threshold) {
-			frag_length = frag_threshold ;
+			frag_length = frag_threshold;
 			bLastIniPkt = 0;
 
 		} else {
@@ -78,7 +78,7 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 		 * Transform from little endian to big endian
 		 * and pending  zero
 		 */
-		for(i=0 ; i < frag_length; i+=4) {
+		for (i=0; i < frag_length; i+=4) {
 			*seg_ptr++ = ((i+0)<frag_length)?code_virtual_address[i+3]:0;
 			*seg_ptr++ = ((i+1)<frag_length)?code_virtual_address[i+2]:0;
 			*seg_ptr++ = ((i+2)<frag_length)?code_virtual_address[i+1]:0;
@@ -89,17 +89,17 @@ static bool fw_download_code(struct net_device *dev, u8 *code_virtual_address,
 
 		if (!priv->ieee80211->check_nic_enough_desc(dev,tcb_desc->queue_index)||
 			(!skb_queue_empty(&priv->ieee80211->skb_waitQ[tcb_desc->queue_index]))||\
-			(priv->ieee80211->queue_stop) ) {
+			(priv->ieee80211->queue_stop)) {
 			RT_TRACE(COMP_FIRMWARE,"=====================================================> tx full!\n");
 			skb_queue_tail(&priv->ieee80211->skb_waitQ[tcb_desc->queue_index], skb);
 		} else {
-			priv->ieee80211->softmac_hard_start_xmit(skb,dev);
+			priv->ieee80211->softmac_hard_start_xmit(skb, dev);
 		}
 
 		code_virtual_address += frag_length;
 		frag_offset += frag_length;
 
-	}while(frag_offset < buffer_len);
+	} while (frag_offset < buffer_len);
 
 	return rt_status;
 
@@ -131,7 +131,7 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 		if (CPU_status&CPU_GEN_PUT_CODE_OK)
 			break;
 
-	}while(check_putcodeOK_time--);
+	} while (check_putcodeOK_time--);
 
 	if (!(CPU_status&CPU_GEN_PUT_CODE_OK)) {
 		RT_TRACE(COMP_ERR, "Download Firmware: Put code fail!\n");
@@ -151,13 +151,12 @@ static bool CPUcheck_maincodeok_turnonCPU(struct net_device *dev)
 
 		if (CPU_status&CPU_GEN_BOOT_RDY)
 			break;
-	}while(check_bootOk_time--);
+	} while (check_bootOk_time--);
 
-	if (!(CPU_status&CPU_GEN_BOOT_RDY)) {
+	if (!(CPU_status&CPU_GEN_BOOT_RDY))
 		goto CPUCheckMainCodeOKAndTurnOnCPU_Fail;
-	} else {
+	else
 		RT_TRACE(COMP_FIRMWARE, "Download Firmware: Boot ready!\n");
-	}
 
 	return rt_status;
 
@@ -181,7 +180,7 @@ static bool CPUcheck_firmware_ready(struct net_device *dev)
 		if (CPU_status&CPU_GEN_FIRM_RDY)
 			break;
 
-	}while(check_time--);
+	} while (check_time--);
 
 	if (!(CPU_status&CPU_GEN_FIRM_RDY))
 		goto CPUCheckFirmwareReady_Fail;
@@ -217,17 +216,17 @@ bool init_firmware(struct net_device *dev)
 
 	RT_TRACE(COMP_FIRMWARE, " PlatformInitFirmware()==>\n");
 
-	if (pfirmware->firmware_status == FW_STATUS_0_INIT ) {
+	if (pfirmware->firmware_status == FW_STATUS_0_INIT) {
 		/* it is called by reset */
 		rst_opt = OPT_SYSTEM_RESET;
 		starting_state = FW_INIT_STEP0_BOOT;
 		// TODO: system reset
 
-	}else if (pfirmware->firmware_status == FW_STATUS_5_READY) {
+	} else if (pfirmware->firmware_status == FW_STATUS_5_READY) {
 		/* it is called by Initialize */
 		rst_opt = OPT_FIRMWARE_RESET;
 		starting_state = FW_INIT_STEP2_DATA;
-	}else {
+	} else {
 		 RT_TRACE(COMP_FIRMWARE, "PlatformInitFirmware: undefined firmware state\n");
 	}
 
@@ -235,14 +234,14 @@ bool init_firmware(struct net_device *dev)
 	 * Download boot, main, and data image for System reset.
 	 * Download data image for firmware reset
 	 */
-	for(init_step = starting_state; init_step <= FW_INIT_STEP2_DATA; init_step++) {
+	for (init_step = starting_state; init_step <= FW_INIT_STEP2_DATA; init_step++) {
 		/*
 		 * Open image file, and map file to continuous memory if open file success.
 		 * or read image file from array. Default load from IMG file
 		 */
 		if (rst_opt == OPT_SYSTEM_RESET) {
 			rc = request_firmware(&fw_entry, fw_name[init_step],&priv->udev->dev);
-			if (rc < 0 ) {
+			if (rc < 0) {
 				RT_TRACE(COMP_ERR, "request firmware fail!\n");
 				goto download_firmware_fail;
 			}
@@ -257,13 +256,13 @@ bool init_firmware(struct net_device *dev)
 				mapped_file = pfirmware->firmware_buf;
 				file_length = fw_entry->size;
 			} else {
-				memset(pfirmware->firmware_buf,0,128);
+				memset(pfirmware->firmware_buf, 0, 128);
 				memcpy(&pfirmware->firmware_buf[128],fw_entry->data,fw_entry->size);
 				mapped_file = pfirmware->firmware_buf;
 				file_length = fw_entry->size + 128;
 			}
 			pfirmware->firmware_buf_size = file_length;
-		}else if (rst_opt == OPT_FIRMWARE_RESET ) {
+		} else if (rst_opt == OPT_FIRMWARE_RESET) {
 			/* we only need to download data.img here */
 			mapped_file = pfirmware->firmware_buf;
 			file_length = pfirmware->firmware_buf_size;
@@ -276,7 +275,7 @@ bool init_firmware(struct net_device *dev)
 		 * 3. each skb_buff packet data content will already include the firmware info
 		 *   and Tx descriptor info
 		 * */
-		rt_status = fw_download_code(dev,mapped_file,file_length);
+		rt_status = fw_download_code(dev, mapped_file, file_length);
 		if (rst_opt == OPT_SYSTEM_RESET)
 			release_firmware(fw_entry);
 
diff --git a/drivers/staging/rtl8192u/r819xU_phyreg.h b/drivers/staging/rtl8192u/r819xU_phyreg.h
index 64285d6a33f8..b855627e9816 100644
--- a/drivers/staging/rtl8192u/r819xU_phyreg.h
+++ b/drivers/staging/rtl8192u/r819xU_phyreg.h
@@ -2,10 +2,11 @@
 #define _R819XU_PHYREG_H
 
 
-#define   RF_DATA				0x1d4					// FW will write RF data in the register.
+#define   RF_DATA				0x1d4					/* FW will write RF data in the register.*/
 
-//Register   //duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF
-//page 1
+/* Register duplicate register due to connection: RF_Mode, TRxRN, NumOf L-STF
+ * page 1
+ */
 #define rPMAC_Reset				0x100
 #define rPMAC_TxStart				0x104
 #define rPMAC_TxLegacySIG			0x108
@@ -34,15 +35,16 @@
 #define rPMAC_CCKCRxRC32OK			0x188
 #define rPMAC_TxStatus				0x18c
 
-//page8
-#define rFPGA0_RFMOD				0x800  //RF mode & CCK TxSC
+/* page8 */
+#define rFPGA0_RFMOD				0x800  /* RF mode & CCK TxSC */
 #define rFPGA0_TxInfo				0x804
 #define rFPGA0_PSDFunction			0x808
 #define rFPGA0_TxGainStage			0x80c
 #define rFPGA0_RFTiming1			0x810
 #define rFPGA0_RFTiming2			0x814
-//#define rFPGA0_XC_RFTiming			0x818
-//#define rFPGA0_XD_RFTiming			0x81c
+/* #define rFPGA0_XC_RFTiming			0x818
+ * #define rFPGA0_XD_RFTiming			0x81c
+ */
 #define rFPGA0_XA_HSSIParameter1	0x820
 #define rFPGA0_XA_HSSIParameter2	0x824
 #define rFPGA0_XB_HSSIParameter1	0x828
@@ -79,51 +81,51 @@
 #define rFPGA0_XAB_RFInterfaceRB	0x8e0
 #define rFPGA0_XCD_RFInterfaceRB	0x8e4
 
-//page 9
-#define rFPGA1_RFMOD				0x900  //RF mode & OFDM TxSC
+/* page 9 */
+#define rFPGA1_RFMOD				0x900  /* RF mode & OFDM TxSC */
 #define rFPGA1_TxBlock				0x904
 #define rFPGA1_DebugSelect			0x908
 #define rFPGA1_TxInfo				0x90c
 
-//page a
+/* page a */
 #define rCCK0_System				0xa00
 #define rCCK0_AFESetting			0xa04
 #define rCCK0_CCA					0xa08
-#define rCCK0_RxAGC1				0xa0c  //AGC default value, saturation level
-#define rCCK0_RxAGC2				0xa10  //AGC & DAGC
+#define rCCK0_RxAGC1				0xa0c  /* AGC default value, saturation level */
+#define rCCK0_RxAGC2				0xa10  /* AGC & DAGC */
 #define rCCK0_RxHP				0xa14
-#define rCCK0_DSPParameter1		0xa18  //Timing recovery & Channel estimation threshold
-#define rCCK0_DSPParameter2		0xa1c  //SQ threshold
+#define rCCK0_DSPParameter1		0xa18  /* Timing recovery & Channel estimation threshold */
+#define rCCK0_DSPParameter2		0xa1c  /* SQ threshold */
 #define rCCK0_TxFilter1				0xa20
 #define rCCK0_TxFilter2				0xa24
-#define rCCK0_DebugPort				0xa28  //debug port and Tx filter3
-#define rCCK0_FalseAlarmReport		0xa2c  //0xa2d
+#define rCCK0_DebugPort				0xa28  /* debug port and Tx filter3 */
+#define rCCK0_FalseAlarmReport		0xa2c  /* 0xa2d */
 #define rCCK0_TRSSIReport			0xa50
-#define rCCK0_RxReport				0xa54  //0xa57
-#define rCCK0_FACounterLower		0xa5c  //0xa5b
-#define rCCK0_FACounterUpper		0xa58  //0xa5c
+#define rCCK0_RxReport				0xa54  /* 0xa57 */
+#define rCCK0_FACounterLower		0xa5c  /* 0xa5b */
+#define rCCK0_FACounterUpper		0xa58  /* 0xa5c */
 
-//page c
+/* page c */
 #define rOFDM0_LSTF				0xc00
 #define rOFDM0_TRxPathEnable		0xc04
 #define rOFDM0_TRMuxPar				0xc08
 #define rOFDM0_TRSWIsolation			0xc0c
-#define rOFDM0_XARxAFE				0xc10  //RxIQ DC offset, Rx digital filter, DC notch filter
-#define rOFDM0_XARxIQImbalance		0xc14  //RxIQ imblance matrix
+#define rOFDM0_XARxAFE				0xc10  /* RxIQ DC offset, Rx digital filter, DC notch filter */
+#define rOFDM0_XARxIQImbalance		0xc14  /* RxIQ imblance matrix */
 #define rOFDM0_XBRxAFE				0xc18
 #define rOFDM0_XBRxIQImbalance		0xc1c
 #define rOFDM0_XCRxAFE				0xc20
 #define rOFDM0_XCRxIQImbalance		0xc24
 #define rOFDM0_XDRxAFE				0xc28
 #define rOFDM0_XDRxIQImbalance		0xc2c
-#define rOFDM0_RxDetector1			0xc30  //PD,BW & SBD
-#define rOFDM0_RxDetector2			0xc34  //SBD & Fame Sync.
-#define rOFDM0_RxDetector3			0xc38  //Frame Sync.
-#define rOFDM0_RxDetector4			0xc3c  //PD, SBD, Frame Sync & Short-GI
-#define rOFDM0_RxDSP				0xc40  //Rx Sync Path
-#define rOFDM0_CFOandDAGC		0xc44  //CFO & DAGC
-#define rOFDM0_CCADropThreshold		0xc48 //CCA Drop threshold
-#define rOFDM0_ECCAThreshold		0xc4c // energy CCA
+#define rOFDM0_RxDetector1			0xc30  /* PD,BW & SBD */
+#define rOFDM0_RxDetector2			0xc34  /* SBD & Fame Sync.*/
+#define rOFDM0_RxDetector3			0xc38  /* Frame Sync.*/
+#define rOFDM0_RxDetector4			0xc3c  /* PD, SBD, Frame Sync & Short-GI */
+#define rOFDM0_RxDSP				0xc40  /* Rx Sync Path */
+#define rOFDM0_CFOandDAGC		0xc44  /* CFO & DAGC */
+#define rOFDM0_CCADropThreshold		0xc48 /* CCA Drop threshold */
+#define rOFDM0_ECCAThreshold		0xc4c /* energy CCA */
 #define rOFDM0_XAAGCCore1		0xc50
 #define rOFDM0_XAAGCCore2		0xc54
 #define rOFDM0_XBAGCCore1		0xc58
@@ -156,7 +158,7 @@
 #define rOFDM0_TxCoeff6				0xcb8
 
 
-//page d
+/* page d */
 #define rOFDM1_LSTF				0xd00
 #define rOFDM1_TRxPathEnable		0xd04
 #define rOFDM1_CFO				0xd08
@@ -169,9 +171,10 @@
 #define rOFDM1_PseudoNoiseStateAB 0xd50
 #define rOFDM1_PseudoNoiseStateCD 0xd54
 #define rOFDM1_RxPseudoNoiseWgt   0xd58
-#define rOFDM_PHYCounter1			0xda0  //cca, parity fail
-#define rOFDM_PHYCounter2			0xda4  //rate illegal, crc8 fail
-#define rOFDM_PHYCounter3			0xda8  //MCS not support
+#define rOFDM_PHYCounter1			0xda0  /* cca, parity fail */
+#define rOFDM_PHYCounter2			0xda4  /* rate illegal, crc8 fail */
+
+#define rOFDM_PHYCounter3			0xda8  /* MCS not support */
 #define rOFDM_ShortCFOAB			0xdac
 #define rOFDM_ShortCFOCD			0xdb0
 #define rOFDM_LongCFOAB				0xdb4
@@ -186,7 +189,7 @@
 #define rOFDM_RxEVMCSI				0xdd8
 #define rOFDM_SIGReport				0xddc
 
-//page e
+/* page e */
 #define rTxAGC_Rate18_06			0xe00
 #define rTxAGC_Rate54_24			0xe04
 #define rTxAGC_CCK_Mcs32			0xe08
@@ -196,8 +199,9 @@
 #define rTxAGC_Mcs15_Mcs12			0xe1c
 
 
-//RF
-//Zebra1
+/* RF
+ * Zebra1
+ */
 #define rZebra1_HSSIEnable		0x0
 #define rZebra1_TRxEnable1		0x1
 #define rZebra1_TRxEnable2		0x2
@@ -209,18 +213,19 @@
 #define rZebra1_RxLPF				0xb
 #define rZebra1_RxHPFCorner		0xc
 
-//Zebra4
+/* Zebra4 */
 #define rGlobalCtrl				0
 #define rRTL8256_TxLPF				19
 #define rRTL8256_RxLPF				11
 
-//RTL8258
+/* RTL8258 */
 #define rRTL8258_TxLPF				0x11
 #define rRTL8258_RxLPF				0x13
 #define rRTL8258_RSSILPF		0xa
 
-//Bit Mask
-//page-1
+/* Bit Mask
+ * page-1
+ */
 #define bBBResetB					0x100
 #define bGlobalResetB				0x200
 #define bOFDMTxStart				0x4
@@ -266,7 +271,7 @@
 #define bCCKTxStatus				0x1
 #define bOFDMTxStatus				0x2
 
-//page-8
+/* page-8 */
 #define bRFMOD						0x1
 #define bJapanMode				0x2
 #define bCCKTxSC					0x30
@@ -283,14 +288,14 @@
 #define bRFStart					0x0000f000
 #define bBBStart					0x000000f0
 #define bBBCCKStart				0x0000000f
-#define bPAEnd						0xf          //Reg0x814
+#define bPAEnd						0xf     /* Reg0x814 */
 #define bTREnd						0x0f000000
 #define bRFEnd						0x000f0000
-#define bCCAMask					0x000000f0   //T2R
+#define bCCAMask					0x000000f0   /* T2R */
 #define bR2RCCAMask				0x00000f00
 #define bHSSI_R2TDelay				0xf8000000
 #define bHSSI_T2RDelay				0xf80000
-#define bContTxHSSI				0x400     //chane gain at continue Tx
+#define bContTxHSSI				0x400     /* chane gain at continue Tx */
 #define bIGFromCCK				0x200
 #define bAGCAddress				0x3f
 #define bRxHPTx						0x7000
@@ -301,7 +306,7 @@
 #define b3WireDataLength			0x800
 #define b3WireAddressLength			0x400
 #define b3WireRFPowerDown			0x1
-//#define bHWSISelect				0x8
+/* #define bHWSISelect				0x8 */
 #define b5GPAPEPolarity				0x40000000
 #define b2GPAPEPolarity				0x80000000
 #define bRFSW_TxDefaultAnt			0x3
@@ -312,7 +317,7 @@
 #define bRFSI_3WireClock			0x2
 #define bRFSI_3WireLoad				0x4
 #define bRFSI_3WireRW				0x8
-#define bRFSI_3Wire					0xf  //3-wire total control
+#define bRFSI_3Wire					0xf  /* 3-wire total control */
 #define bRFSI_RFENV				0x10
 #define bRFSI_TRSW				0x20
 #define bRFSI_TRSWB				0x40
@@ -337,12 +342,11 @@
 #define bLSIG_Length				0x1fffe
 #define bLSIG_Parity					0x20
 #define bCCKRxPhase				0x4
-#define bLSSIReadAddress			0x3f000000   //LSSI "Read" Address
-#define bLSSIReadEdge				0x80000000   //LSSI "Read" edge signal
+#define bLSSIReadAddress			0x3f000000   /* LSSI "Read" Address */
+#define bLSSIReadEdge				0x80000000   /* LSSI "Read" edge signal */
 #define bLSSIReadBackData			0xfff
 #define bLSSIReadOKFlag				0x1000
-#define bCCKSampleRate				0x8       //0: 44MHz, 1:88MHz
-
+#define bCCKSampleRate				0x8	/* 0: 44MHz, 1:88MHz */
 #define bRegulator0Standby			0x1
 #define bRegulatorPLLStandby			0x2
 #define bRegulator1Standby			0x4
@@ -395,12 +399,12 @@
 #define bPSDSineToneScale			0x7f000000
 #define bPSDReport					0xffff
 
-//page-9
+/* page-9 */
 #define bOFDMTxSC				0x30000000
 #define bCCKTxOn					0x1
 #define bOFDMTxOn				0x2
-#define bDebugPage				0xfff  //reset debug page and also HWord, LWord
-#define bDebugItem				0xff   //reset debug page and LWord
+#define bDebugPage				0xfff  /* reset debug page and also HWord, LWord */
+#define bDebugItem				0xff   /* reset debug page and LWord */
 #define bAntL					0x10
 #define bAntNonHT					0x100
 #define bAntHT1					0x1000
@@ -408,7 +412,7 @@
 #define bAntHT1S1					0x100000
 #define bAntNonHTS1				0x1000000
 
-//page-a
+/* page-a */
 #define bCCKBBMode				0x3
 #define bCCKTxPowerSaving			0x80
 #define bCCKRxPowerSaving			0x40
@@ -429,7 +433,7 @@
 #define bCCKBistMode				0x80000000
 #define bCCKCCAMask				0x40000000
 #define bCCKTxDACPhase			0x4
-#define bCCKRxADCPhase			0x20000000   //r_rx_clk
+#define bCCKRxADCPhase			0x20000000   /* r_rx_clk */
 #define bCCKr_cp_mode0			0x0100
 #define bCCKTxDCOffset				0xf0
 #define bCCKRxDCOffset				0xf
@@ -443,12 +447,12 @@
 #define bCCKRxIG					0x7f00
 #define bCCKLNAPolarity				0x800000
 #define bCCKRx1stGain				0x7f0000
-#define bCCKRFExtend				0x20000000 //CCK Rx initial gain polarity
+#define bCCKRFExtend				0x20000000 /* CCK Rx initial gain polarity */
 #define bCCKRxAGCSatLevel			0x1f000000
 #define bCCKRxAGCSatCount			0xe0
-#define bCCKRxRFSettle				0x1f       //AGCsamp_dly
+#define bCCKRxRFSettle				0x1f       /* AGCsamp_dly */
 #define bCCKFixedRxAGC				0x8000
-//#define bCCKRxAGCFormat			0x4000   //remove to HSSI register 0x824
+/* #define bCCKRxAGCFormat			0x4000 */   /* remove to HSSI register 0x824 */
 #define bCCKAntennaPolarity			0x2000
 #define bCCKTxFilterType			0x0c00
 #define bCCKRxAGCReportType		0x0300
@@ -489,7 +493,7 @@
 #define bCCKDefaultRxPath			0xc000000
 #define bCCKOptionRxPath			0x3000000
 
-//page c
+/* page c */
 #define bNumOfSTF					0x3
 #define bShift_L					0xc0
 #define bGI_TH						0xc
@@ -591,8 +595,8 @@
 #define bRxHP_BBP1               0x7000
 #define bRxHP_BBP2               0x70000
 #define bRxHP_BBP3               0x700000
-#define bRSSI_H                  0x7f0000     //the threshold for high power
-#define bRSSI_Gen                0x7f000000   //the threshold for ant diversity
+#define bRSSI_H                  0x7f0000     /* the threshold for high power */
+#define bRSSI_Gen                0x7f000000   /* the threshold for ant diversity */
 #define bRxSettle_TRSW           0x7
 #define bRxSettle_LNA            0x38
 #define bRxSettle_RSSI           0x1c0
@@ -626,7 +630,7 @@
 #define bRxPD_Delay_TH1          0x38
 #define bRxPD_Delay_TH2          0x1c0
 #define bRxPD_DC_COUNT_MAX       0x600
-//#define bRxMF_Hold               0x3800
+/* #define bRxMF_Hold               0x3800 */
 #define bRxPD_Delay_TH           0x8000
 #define bRxProcess_Delay         0xf0000
 #define bRxSearchrange_GI2_Early 0x700000
@@ -652,7 +656,7 @@
 
 #define bExtLNAGain              0x7c00
 
-//page d
+/* page d */
 #define bSTBCEn                  0x4
 #define bAntennaMapping          0x10
 #define bNss                     0x20
@@ -662,12 +666,13 @@
 #define bOFDMContinueTx          0x10000000
 #define bOFDMSingleCarrier       0x20000000
 #define bOFDMSingleTone          0x40000000
-//#define bRxPath1                 0x01
-//#define bRxPath2                 0x02
-//#define bRxPath3                 0x04
-//#define bRxPath4                 0x08
-//#define bTxPath1                 0x10
-//#define bTxPath2                 0x20
+/* #define bRxPath1                 0x01
+ * #define bRxPath2                 0x02
+ * #define bRxPath3                 0x04
+ * #define bRxPath4                 0x08
+ * #define bTxPath1                 0x10
+ * #define bTxPath2                 0x20
+ */
 #define bHTDetect                0x100
 #define bCFOEn                   0x10000
 #define bCFOValue                0xfff00000
@@ -680,8 +685,8 @@
 #define bCounter_MCSNoSupport    0xffff
 #define bCounter_FastSync        0xffff
 #define bShortCFO                0xfff
-#define bShortCFOTLength         12   //total
-#define bShortCFOFLength         11   //fraction
+#define bShortCFOTLength         12   /* total */
+#define bShortCFOFLength         11   /* fraction */
 #define bLongCFO                 0x7ff
 #define bLongCFOTLength          11
 #define bLongCFOFLength          11
@@ -758,7 +763,7 @@
 #define bUChCfg                  0x7000000
 #define bUpdEqz                  0x8000000
 
-//page e
+/* page e */
 #define bTxAGCRate18_06			0x7f7f7f7f
 #define bTxAGCRate54_24			0x7f7f7f7f
 #define bTxAGCRateMCS32		0x7f
@@ -769,7 +774,7 @@
 #define bTxAGCRateMCS15_MCS12	0x7f7f7f7f
 
 
-//Rx Pseduo noise
+/* Rx Pseduo noise */
 #define bRxPesudoNoiseOn         0x20000000
 #define bRxPesudoNoise_A         0xff
 #define bRxPesudoNoise_B         0xff00
@@ -780,8 +785,9 @@
 #define bPesudoNoiseState_C      0xffff
 #define bPesudoNoiseState_D      0xffff0000
 
-//RF
-//Zebra1
+/* RF
+ * Zebra1
+ */
 #define bZebra1_HSSIEnable        0x8
 #define bZebra1_TRxControl        0xc00
 #define bZebra1_TRxGainSetting    0x07f
@@ -792,18 +798,18 @@
 #define bZebra1_TxLPFBW           0x400
 #define bZebra1_RxLPFBW           0x600
 
-//Zebra4
+/* Zebra4 */
 #define bRTL8256RegModeCtrl1      0x100
 #define bRTL8256RegModeCtrl0      0x40
 #define bRTL8256_TxLPFBW          0x18
 #define bRTL8256_RxLPFBW          0x600
 
-//RTL8258
+/* RTL8258 */
 #define bRTL8258_TxLPFBW          0xc
 #define bRTL8258_RxLPFBW          0xc00
 #define bRTL8258_RSSILPFBW        0xc0
 
-//byte endable for sb_write
+/* byte endable for sb_write */
 #define bByte0                    0x1
 #define bByte1                    0x2
 #define bByte2                    0x4
@@ -812,7 +818,7 @@
 #define bWord1                    0xc
 #define bDWord                    0xf
 
-//for PutRegsetting & GetRegSetting BitMask
+/* for PutRegsetting & GetRegSetting BitMask */
 #define bMaskByte0                0xff
 #define bMaskByte1                0xff00
 #define bMaskByte2                0xff0000
@@ -821,7 +827,7 @@
 #define bMaskLWord                0x0000ffff
 #define bMaskDWord                0xffffffff
 
-//for PutRFRegsetting & GetRFRegSetting BitMask
+/* for PutRFRegsetting & GetRFRegSetting BitMask */
 #define bMask12Bits               0xfff
 
 #define bEnable                   0x1
@@ -830,14 +836,14 @@
 #define LeftAntenna               0x0
 #define RightAntenna              0x1
 
-#define tCheckTxStatus            500   //500ms
-#define tUpdateRxCounter          100   //100ms
+#define tCheckTxStatus            500   /* 500ms */
+#define tUpdateRxCounter          100   /* 100ms */
 
 #define rateCCK     0
 #define rateOFDM    1
 #define rateHT      2
 
-//define Register-End
+/* define Register-End */
 #define bPMAC_End                 0x1ff
 #define bFPGAPHY0_End             0x8ff
 #define bFPGAPHY1_End             0x9ff
@@ -845,12 +851,13 @@
 #define bOFDMPHY0_End             0xcff
 #define bOFDMPHY1_End             0xdff
 
-//define max debug item in each debug page
-//#define bMaxItem_FPGA_PHY0        0x9
-//#define bMaxItem_FPGA_PHY1        0x3
-//#define bMaxItem_PHY_11B          0x16
-//#define bMaxItem_OFDM_PHY0        0x29
-//#define bMaxItem_OFDM_PHY1        0x0
+/* define max debug item in each debug page
+ * #define bMaxItem_FPGA_PHY0        0x9
+ * #define bMaxItem_FPGA_PHY1        0x3
+ * #define bMaxItem_PHY_11B          0x16
+ * #define bMaxItem_OFDM_PHY0        0x29
+ * #define bMaxItem_OFDM_PHY1        0x0
+ */
 
 #define bPMACControl              0x0
 #define bWMACControl              0x1
@@ -868,4 +875,4 @@
 #define		rRTL8256TxBBBW             19
 #define		bRTL8256TxBBBW			0x18
 
-#endif	//__INC_HAL8190PCIPHYREG_H
+#endif	/* __INC_HAL8190PCIPHYREG_H */
diff --git a/drivers/staging/rtl8712/hal_init.c b/drivers/staging/rtl8712/hal_init.c
index 0556de3b8904..1d6ade05fa18 100644
--- a/drivers/staging/rtl8712/hal_init.c
+++ b/drivers/staging/rtl8712/hal_init.c
@@ -49,6 +49,7 @@ static void rtl871x_load_fw_cb(const struct firmware *firmware, void *context)
 	if (!firmware) {
 		struct usb_device *udev = padapter->dvobjpriv.pusbdev;
 		struct usb_interface *pusb_intf = padapter->pusb_intf;
+
 		dev_err(&udev->dev, "r8712u: Firmware request failed\n");
 		usb_put_dev(udev);
 		usb_set_intfdata(pusb_intf, NULL);
@@ -388,10 +389,8 @@ uint rtl871x_hal_init(struct _adapter *padapter)
 	padapter->hw_init_completed = false;
 	if (padapter->halpriv.hal_bus_init == NULL)
 		return _FAIL;
-	else {
-		if (padapter->halpriv.hal_bus_init(padapter) != _SUCCESS)
-			return _FAIL;
-	}
+	if (padapter->halpriv.hal_bus_init(padapter) != _SUCCESS)
+		return _FAIL;
 	if (rtl8712_hal_init(padapter) == _SUCCESS)
 		padapter->hw_init_completed = true;
 	else {
diff --git a/drivers/staging/rtl8712/rtl8712_cmd.c b/drivers/staging/rtl8712/rtl8712_cmd.c
index ab0c7eb248d3..720e8a15db96 100644
--- a/drivers/staging/rtl8712/rtl8712_cmd.c
+++ b/drivers/staging/rtl8712/rtl8712_cmd.c
@@ -350,6 +350,7 @@ _next:
 			struct dvobj_priv *pdvobj = (struct dvobj_priv *)
 						    &padapter->dvobjpriv;
 			u8 blnPending = 0;
+
 			pcmdpriv->cmd_issued_cnt++;
 			cmdsz = round_up(pcmd->cmdsz, 8);
 			wr_sz = TXDESC_SIZE + 8 + cmdsz;
@@ -468,7 +469,8 @@ void r8712_event_handle(struct _adapter *padapter, uint *peventbuf)
 	pevt_priv->event_seq++;	/* update evt_seq */
 	if (pevt_priv->event_seq > 127)
 		pevt_priv->event_seq = 0;
-	peventbuf = peventbuf + 2; /* move to event content, 8 bytes alignment */
+	/* move to event content, 8 bytes alignment */
+	peventbuf = peventbuf + 2;
 	event_callback = wlanevents[evt_code].event_callback;
 	if (event_callback)
 		event_callback(padapter, (u8 *)peventbuf);
diff --git a/drivers/staging/rtl8712/rtl8712_io.c b/drivers/staging/rtl8712/rtl8712_io.c
index c7346008def0..921fcffb3065 100644
--- a/drivers/staging/rtl8712/rtl8712_io.c
+++ b/drivers/staging/rtl8712/rtl8712_io.c
@@ -39,11 +39,9 @@ u8 r8712_read8(struct _adapter *adapter, u32 addr)
 	struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue;
 	struct intf_hdl *pintfhdl = &(pio_queue->intf);
 	u8 (*_read8)(struct intf_hdl *pintfhdl, u32 addr);
-	u8 r_val;
 
 	_read8 = pintfhdl->io_ops._read8;
-	r_val = _read8(pintfhdl, addr);
-	return r_val;
+	return  _read8(pintfhdl, addr);
 }
 
 u16 r8712_read16(struct _adapter *adapter, u32 addr)
@@ -51,11 +49,9 @@ u16 r8712_read16(struct _adapter *adapter, u32 addr)
 	struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue;
 	struct intf_hdl *pintfhdl = &(pio_queue->intf);
 	u16 (*_read16)(struct intf_hdl *pintfhdl, u32 addr);
-	u16 r_val;
 
 	_read16 = pintfhdl->io_ops._read16;
-	r_val = _read16(pintfhdl, addr);
-	return r_val;
+	return _read16(pintfhdl, addr);
 }
 
 u32 r8712_read32(struct _adapter *adapter, u32 addr)
@@ -63,11 +59,9 @@ u32 r8712_read32(struct _adapter *adapter, u32 addr)
 	struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue;
 	struct intf_hdl *pintfhdl = &(pio_queue->intf);
 	u32 (*_read32)(struct intf_hdl *pintfhdl, u32 addr);
-	u32 r_val;
 
 	_read32 = pintfhdl->io_ops._read32;
-	r_val = _read32(pintfhdl, addr);
-	return r_val;
+	return _read32(pintfhdl, addr);
 }
 
 void r8712_write8(struct _adapter *adapter, u32 addr, u8 val)
@@ -84,8 +78,8 @@ void r8712_write16(struct _adapter *adapter, u32 addr, u16 val)
 {
 	struct io_queue *pio_queue = (struct io_queue *)adapter->pio_queue;
 	struct intf_hdl *pintfhdl = &(pio_queue->intf);
-
 	void (*_write16)(struct intf_hdl *pintfhdl, u32 addr, u16 val);
+
 	_write16 = pintfhdl->io_ops._write16;
 	_write16(pintfhdl, addr, val);
 }
@@ -96,6 +90,7 @@ void r8712_write32(struct _adapter *adapter, u32 addr, u32 val)
 	struct intf_hdl *pintfhdl = (struct intf_hdl *)(&(pio_queue->intf));
 
 	void (*_write32)(struct intf_hdl *pintfhdl, u32 addr, u32 val);
+
 	_write32 = pintfhdl->io_ops._write32;
 	_write32(pintfhdl, addr, val);
 }
diff --git a/drivers/staging/rtl8712/rtl8712_recv.c b/drivers/staging/rtl8712/rtl8712_recv.c
index df1e498e98b6..b27806209268 100644
--- a/drivers/staging/rtl8712/rtl8712_recv.c
+++ b/drivers/staging/rtl8712/rtl8712_recv.c
@@ -974,7 +974,7 @@ int recv_func(struct _adapter *padapter, void *pcontext)
 	prframe = (union recv_frame *)pcontext;
 	orig_prframe = prframe;
 	pattrib = &prframe->u.hdr.attrib;
-	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) {
 		if (pattrib->crc_err == 1)
 			padapter->mppriv.rx_crcerrpktcount++;
 		else
diff --git a/drivers/staging/rtl8712/rtl8712_xmit.c b/drivers/staging/rtl8712/rtl8712_xmit.c
index 4ebedb4005cf..a3093ac1204b 100644
--- a/drivers/staging/rtl8712/rtl8712_xmit.c
+++ b/drivers/staging/rtl8712/rtl8712_xmit.c
@@ -324,6 +324,7 @@ u8 r8712_append_mpdu_unit(struct xmit_buf *pxmitbuf,
 	padding_sz = (8 - (last_txcmdsz % 8));
 	if ((last_txcmdsz % 8) != 0) {
 		int i;
+
 		for (i = 0; i < padding_sz; i++)
 			*(pxmitframe->buf_addr+TXDESC_SIZE+last_txcmdsz+i) = 0;
 	}
@@ -554,6 +555,7 @@ static void update_txdesc(struct xmit_frame *pxmitframe, uint *pmem, int sz)
 		}
 		if (pattrib->pctrl == 1) { /* mp tx packets */
 			struct tx_desc *ptxdesc_mp;
+
 			ptxdesc_mp = &txdesc_mp;
 			/* offset 8 */
 			ptxdesc->txdw2 = cpu_to_le32(ptxdesc_mp->txdw2);
@@ -655,6 +657,7 @@ int r8712_xmitframe_complete(struct _adapter *padapter,
 		r8712_xmitframe_aggr_1st(pxmitbuf, pxmitframe);
 		if (p2ndxmitframe != NULL) {
 			u16 total_length;
+
 			total_length = r8712_xmitframe_aggr_next(
 				pxmitbuf, p2ndxmitframe);
 			do {
diff --git a/drivers/staging/rtl8712/rtl871x_io.c b/drivers/staging/rtl8712/rtl871x_io.c
index 8858687e87fe..d7b63aedead7 100644
--- a/drivers/staging/rtl8712/rtl871x_io.c
+++ b/drivers/staging/rtl8712/rtl871x_io.c
@@ -120,8 +120,8 @@ uint r8712_alloc_io_queue(struct _adapter *adapter)
 	INIT_LIST_HEAD(&pio_queue->pending);
 	spin_lock_init(&pio_queue->lock);
 	pio_queue->pallocated_free_ioreqs_buf = kmalloc(NUM_IOREQ *
-							(sizeof(struct io_req)) + 4,
-							GFP_ATOMIC);
+						(sizeof(struct io_req)) + 4,
+						GFP_ATOMIC);
 	if ((pio_queue->pallocated_free_ioreqs_buf) == NULL)
 		goto alloc_io_queue_fail;
 	memset(pio_queue->pallocated_free_ioreqs_buf, 0,
diff --git a/drivers/staging/rtl8712/rtl871x_io.h b/drivers/staging/rtl8712/rtl871x_io.h
index dc23395fec3b..070cc03ce374 100644
--- a/drivers/staging/rtl8712/rtl871x_io.h
+++ b/drivers/staging/rtl8712/rtl871x_io.h
@@ -166,7 +166,7 @@ struct reg_protocol_rd {
 	u32 Byte4Access:1;
 	u32 Byte2Access:1;
 	u32 Byte1Access:1;
-	u32 BurstMode:1 ;
+	u32 BurstMode:1;
 	u32 FixOrContinuous:1;
 	u32 Reserved4:16;
 	/*DW3*/
@@ -235,6 +235,7 @@ struct io_queue {
 static inline u32 _RND4(u32 sz)
 {
 	u32	val;
+
 	val = ((sz >> 2) + ((sz & 3) ? 1 : 0)) << 2;
 	return val;
 }
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl.h b/drivers/staging/rtl8712/rtl871x_ioctl.h
index 20168028d39d..8e6ef5d49fbf 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl.h
+++ b/drivers/staging/rtl8712/rtl871x_ioctl.h
@@ -69,7 +69,7 @@ struct oid_par_priv {
 struct oid_obj_priv {
 	unsigned char	dbg; /* 0: without OID debug message
 			      * 1: with OID debug message */
-	uint(*oidfuns)(struct oid_par_priv *poid_par_priv);
+	uint (*oidfuns)(struct oid_par_priv *poid_par_priv);
 };
 
 uint oid_null_function(struct oid_par_priv *poid_par_priv);
diff --git a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
index e1e95cf48302..8e42ce06e5d7 100644
--- a/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
+++ b/drivers/staging/rtl8712/rtl871x_ioctl_linux.c
@@ -241,6 +241,7 @@ static inline char *translate_scan(struct _adapter *padapter,
 	{
 		/*  check legal index */
 		u8 dsconfig = pnetwork->network.Configuration.DSConfig;
+
 		if (dsconfig >= 1 && dsconfig <= sizeof(
 		    ieee80211_wlan_frequencies) / sizeof(long))
 			iwe.u.freq.m = (s32)(ieee80211_wlan_frequencies[
@@ -287,6 +288,7 @@ static inline char *translate_scan(struct _adapter *padapter,
 		u16 wpa_len = 0, rsn_len = 0;
 		int n;
 		sint out_len = 0;
+
 		out_len = r8712_get_sec_ie(pnetwork->network.IEs,
 					   pnetwork->network.
 					   IELength, rsn_ie, &rsn_len,
@@ -425,8 +427,8 @@ static int wpa_set_encryption(struct net_device *dev, struct ieee_param *param,
 		if (wep_key_len > 0) {
 			wep_key_len = wep_key_len <= 5 ? 5 : 13;
 			pwep = kmalloc((u32)(wep_key_len +
-				       FIELD_OFFSET(struct NDIS_802_11_WEP, KeyMaterial)),
-				       GFP_ATOMIC);
+				FIELD_OFFSET(struct NDIS_802_11_WEP,
+				KeyMaterial)), GFP_ATOMIC);
 			if (pwep == NULL)
 				return -ENOMEM;
 			memset(pwep, 0, sizeof(struct NDIS_802_11_WEP));
@@ -697,6 +699,7 @@ static int r8711_wx_set_freq(struct net_device *dev,
 	  (fwrq->m <= (int) 2.487e8)) {
 		int f = fwrq->m / 100000;
 		int c = 0;
+
 		while ((c < 14) && (f != frequency_list[c]))
 			c++;
 		fwrq->e = 0;
@@ -707,6 +710,7 @@ static int r8711_wx_set_freq(struct net_device *dev,
 		rc = -EOPNOTSUPP;
 	else {
 		int channel = fwrq->m;
+
 		if ((channel < 1) || (channel > 14))
 			rc = -EINVAL;
 		else {
@@ -999,12 +1003,14 @@ static int r871x_wx_set_priv(struct net_device *dev,
 		/*Set scan type to active */
 		/*OK if successful */
 		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
 		pmlmepriv->passive_mode = 1;
 		sprintf(ext, "OK");
 	} else if (0 == strcasecmp(ext, "SCAN-PASSIVE")) {
 		/*Set scan type to passive */
 		/*OK if successful */
 		struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
+
 		pmlmepriv->passive_mode = 0;
 		sprintf(ext, "OK");
 	} else if (0 == strncmp(ext, "DCE-E", 5)) {
@@ -1179,10 +1185,12 @@ static int r8711_wx_set_scan(struct net_device *dev,
 		return 0;
 	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
 		struct iw_scan_req *req = (struct iw_scan_req *)extra;
+
 		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 			struct ndis_802_11_ssid ssid;
 			unsigned long irqL;
 			u32 len = min_t(u8, req->essid_len, IW_ESSID_MAX_SIZE);
+
 			memset((unsigned char *)&ssid, 0,
 				 sizeof(struct ndis_802_11_ssid));
 			memcpy(ssid.Ssid, req->essid, len);
@@ -1563,7 +1571,8 @@ static int r8711_wx_set_enc(struct net_device *dev,
 		authmode = Ndis802_11AuthModeOpen;
 		padapter->securitypriv.ndisauthtype = authmode;
 	} else if (erq->flags & IW_ENCODE_RESTRICTED) {
-		netdev_info(dev, "r8712u: %s: IW_ENCODE_RESTRICTED\n", __func__);
+		netdev_info(dev,
+				"r8712u: %s: IW_ENCODE_RESTRICTED\n", __func__);
 		padapter->securitypriv.ndisencryptstatus =
 				 Ndis802_11Encryption1Enabled;
 		padapter->securitypriv.AuthAlgrthm = 1; /* shared system */
@@ -2144,7 +2153,6 @@ static int wpa_set_param(struct net_device *dev, u8 name, u32 value)
 		break;
 	case IEEE_PARAM_AUTH_ALGS:
 		return wpa_set_auth_algs(dev, value);
-		break;
 	case IEEE_PARAM_IEEE_802_1X:
 		break;
 	case IEEE_PARAM_WPAX_SELECT:
diff --git a/drivers/staging/rtl8712/rtl871x_mlme.c b/drivers/staging/rtl8712/rtl871x_mlme.c
index 7b7fdec58b38..00f2e0fc4ac5 100644
--- a/drivers/staging/rtl8712/rtl871x_mlme.c
+++ b/drivers/staging/rtl8712/rtl871x_mlme.c
@@ -590,7 +590,7 @@ void r8712_surveydone_event_callback(struct _adapter *adapter, u8 *pbuf)
 	}
 
 	if (pmlmepriv->to_join == true) {
-		if ((check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true)) {
+		if (check_fwstate(pmlmepriv, WIFI_ADHOC_STATE) == true) {
 			if (check_fwstate(pmlmepriv, _FW_LINKED) == false) {
 				set_fwstate(pmlmepriv, _FW_UNDER_LINKING);
 
diff --git a/drivers/staging/rtl8712/rtl871x_mp.c b/drivers/staging/rtl8712/rtl871x_mp.c
index aae77f0ea98f..2ec660ad78af 100644
--- a/drivers/staging/rtl8712/rtl871x_mp.c
+++ b/drivers/staging/rtl8712/rtl871x_mp.c
@@ -54,8 +54,8 @@ static int init_mp_priv(struct mp_priv *pmp_priv)
 	_init_queue(&pmp_priv->free_mp_xmitqueue);
 	pmp_priv->pallocated_mp_xmitframe_buf = NULL;
 	pmp_priv->pallocated_mp_xmitframe_buf = kmalloc(NR_MP_XMITFRAME *
-							sizeof(struct mp_xmit_frame) + 4,
-							GFP_ATOMIC);
+				sizeof(struct mp_xmit_frame) + 4,
+				GFP_ATOMIC);
 	if (pmp_priv->pallocated_mp_xmitframe_buf == NULL) {
 		res = _FAIL;
 		goto _exit_init_mp_priv;
@@ -146,6 +146,7 @@ u32 r8712_bb_reg_read(struct _adapter *pAdapter, u16 offset)
 	bb_val = fw_iocmd_read(pAdapter, iocmd);
 	if (shift != 0) {
 		u32 bb_val2 = 0;
+
 		bb_val >>= (shift * 8);
 		iocmd.value += 4;
 		bb_val2 = fw_iocmd_read(pAdapter, iocmd);
@@ -186,14 +187,12 @@ u8 r8712_bb_reg_write(struct _adapter *pAdapter, u16 offset, u32 value)
 u32 r8712_rf_reg_read(struct _adapter *pAdapter, u8 path, u8 offset)
 {
 	u16 rf_addr = (path << 8) | offset;
-	u32 rf_data;
 	struct IOCMD_STRUCT iocmd;
 
 	iocmd.cmdclass	= IOCMD_CLASS_BB_RF;
 	iocmd.value	= rf_addr;
 	iocmd.index	= IOCMD_RF_READ_IDX;
-	rf_data = fw_iocmd_read(pAdapter, iocmd);
-	return rf_data;
+	return fw_iocmd_read(pAdapter, iocmd);
 }
 
 u8 r8712_rf_reg_write(struct _adapter *pAdapter, u8 path, u8 offset, u32 value)
@@ -318,6 +317,7 @@ static void SetOFDMTxPower(struct _adapter *pAdapter, u8 TxPower)
 void r8712_SetTxPower(struct _adapter *pAdapter)
 {
 	u8 TxPower = pAdapter->mppriv.curr_txpoweridx;
+
 	SetCCKTxPower(pAdapter, TxPower);
 	SetOFDMTxPower(pAdapter, TxPower);
 }
@@ -504,11 +504,8 @@ static void TriggerRFThermalMeter(struct _adapter *pAdapter)
 
 static u32 ReadRFThermalMeter(struct _adapter *pAdapter)
 {
-	u32 ThermalValue = 0;
-
 	/* 0x24: RF Reg[4:0] */
-	ThermalValue = get_rf_reg(pAdapter, RF_PATH_A, RF_T_METER, 0x1F);
-	return ThermalValue;
+	return get_rf_reg(pAdapter, RF_PATH_A, RF_T_METER, 0x1F);
 }
 
 void r8712_GetThermalMeter(struct _adapter *pAdapter, u32 *value)
@@ -549,6 +546,7 @@ void r8712_SetSingleCarrierTx(struct _adapter *pAdapter, u8 bStart)
 void r8712_SetSingleToneTx(struct _adapter *pAdapter, u8 bStart)
 {
 	u8 rfPath = pAdapter->mppriv.curr_rfpath;
+
 	switch (pAdapter->mppriv.antenna_tx) {
 	case ANTENNA_B:
 		rfPath = RF_PATH_B;
diff --git a/drivers/staging/rtl8712/rtl871x_mp.h b/drivers/staging/rtl8712/rtl871x_mp.h
index 51395d1a3c7e..75893f225dc0 100644
--- a/drivers/staging/rtl8712/rtl871x_mp.h
+++ b/drivers/staging/rtl8712/rtl871x_mp.h
@@ -208,7 +208,7 @@ enum POWER_MODE {
 #define RX_PKT_DEST_ADDR	2
 #define RX_PKT_PHY_MATCH	3
 
-#define RPTMaxCount 0x000FFFFF;
+#define RPTMaxCount 0x000FFFFF
 
 /* parameter 1 : BitMask
  *	bit 0  : OFDM PPDU
diff --git a/drivers/staging/rtl8712/rtl871x_pwrctrl.c b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
index 89ce527940fe..51dcf5555b62 100644
--- a/drivers/staging/rtl8712/rtl871x_pwrctrl.c
+++ b/drivers/staging/rtl8712/rtl871x_pwrctrl.c
@@ -157,6 +157,7 @@ static void rpwm_workitem_callback(struct work_struct *work)
 	struct _adapter *padapter = container_of(pwrpriv,
 				    struct _adapter, pwrctrlpriv);
 	u8 cpwm = pwrpriv->cpwm;
+
 	if (pwrpriv->cpwm != pwrpriv->rpwm) {
 		_enter_pwrlock(&pwrpriv->lock);
 		cpwm = r8712_read8(padapter, SDIO_HCPWM);
@@ -169,6 +170,7 @@ static void rpwm_workitem_callback(struct work_struct *work)
 static void rpwm_check_handler (void *FunctionContext)
 {
 	struct _adapter *adapter = (struct _adapter *)FunctionContext;
+
 	_rpwm_check_handler(adapter);
 }
 
diff --git a/drivers/staging/rtl8712/rtl871x_recv.c b/drivers/staging/rtl8712/rtl871x_recv.c
index a3889d18d089..9b99a71670f3 100644
--- a/drivers/staging/rtl8712/rtl871x_recv.c
+++ b/drivers/staging/rtl8712/rtl871x_recv.c
@@ -73,8 +73,8 @@ sint _r8712_init_recv_priv(struct recv_priv *precvpriv,
 	precvpriv->adapter = padapter;
 	precvpriv->free_recvframe_cnt = NR_RECVFRAME;
 	precvpriv->pallocated_frame_buf = kmalloc(NR_RECVFRAME *
-						  sizeof(union recv_frame) + RXFRAME_ALIGN_SZ,
-						  GFP_ATOMIC);
+				sizeof(union recv_frame) + RXFRAME_ALIGN_SZ,
+				GFP_ATOMIC);
 	if (precvpriv->pallocated_frame_buf == NULL)
 		return _FAIL;
 	kmemleak_not_leak(precvpriv->pallocated_frame_buf);
@@ -631,7 +631,7 @@ sint r8712_wlanhdr_to_ethhdr(union recv_frame *precvframe)
 	rmv_len = pattrib->hdrlen + pattrib->iv_len +
 		  (bsnaphdr ? SNAP_SIZE : 0);
 	len = precvframe->u.hdr.len - rmv_len;
-	if ((check_fwstate(pmlmepriv, WIFI_MP_STATE) == true)) {
+	if (check_fwstate(pmlmepriv, WIFI_MP_STATE) == true) {
 		ptr += rmv_len;
 		*ptr = 0x87;
 		*(ptr+1) = 0x12;
diff --git a/drivers/staging/rtl8712/rtl871x_security.c b/drivers/staging/rtl8712/rtl871x_security.c
index 0912f52def99..8faf22bb7c90 100644
--- a/drivers/staging/rtl8712/rtl871x_security.c
+++ b/drivers/staging/rtl8712/rtl871x_security.c
@@ -252,7 +252,6 @@ void r8712_wep_decrypt(struct _adapter  *padapter, u8 *precvframe)
 		/* calculate icv and compare the icv */
 		*((u32 *)crc) = cpu_to_le32(getcrc32(payload, length - 4));
 	}
-	return;
 }
 
 /* 3 =====TKIP related===== */
diff --git a/drivers/staging/rtl8712/rtl871x_xmit.c b/drivers/staging/rtl8712/rtl871x_xmit.c
index b985edc158b9..f49acaf04076 100644
--- a/drivers/staging/rtl8712/rtl871x_xmit.c
+++ b/drivers/staging/rtl8712/rtl871x_xmit.c
@@ -248,6 +248,7 @@ sint r8712_update_attrib(struct _adapter *padapter, _pkt *pkt,
 		 * tx these packets and let LPS awake some time
 		 * to prevent DHCP protocol fail */
 		u8 tmp[24];
+
 		_r8712_pktfile_read(&pktfile, &tmp[0], 24);
 		pattrib->dhcp_pkt = 0;
 		if (pktfile.pkt_len > 282) {/*MINIMUM_DHCP_PACKET_SIZE)*/
@@ -481,6 +482,7 @@ static sint make_wlanhdr(struct _adapter *padapter , u8 *hdr,
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct qos_priv *pqospriv = &pmlmepriv->qospriv;
 	u16 *fctrl = &pwlanhdr->frame_ctl;
+
 	memset(hdr, 0, WLANHDR_OFFSET);
 	SetFrameSubType(fctrl, pattrib->subtype);
 	if (pattrib->subtype & WIFI_DATA_TYPE) {
@@ -491,7 +493,7 @@ static sint make_wlanhdr(struct _adapter *padapter , u8 *hdr,
 				ETH_ALEN);
 			memcpy(pwlanhdr->addr2, pattrib->src, ETH_ALEN);
 			memcpy(pwlanhdr->addr3, pattrib->dst, ETH_ALEN);
-		} else if ((check_fwstate(pmlmepriv, WIFI_AP_STATE) == true)) {
+		} else if (check_fwstate(pmlmepriv, WIFI_AP_STATE) == true) {
 			/* to_ds = 0, fr_ds = 1; */
 			SetFrDs(fctrl);
 			memcpy(pwlanhdr->addr1, pattrib->dst, ETH_ALEN);
@@ -525,8 +527,8 @@ static sint make_wlanhdr(struct _adapter *padapter , u8 *hdr,
 		/* Update Seq Num will be handled by f/w */
 		{
 			struct sta_info *psta;
-
 			sint bmcst = IS_MCAST(pattrib->ra);
+
 			if (pattrib->psta)
 				psta = pattrib->psta;
 			else {
@@ -953,8 +955,8 @@ static void alloc_hwxmits(struct _adapter *padapter)
 	struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
 
 	pxmitpriv->hwxmit_entry = HWXMIT_ENTRY;
-	pxmitpriv->hwxmits = kmalloc(sizeof(struct hw_xmit) * pxmitpriv->hwxmit_entry,
-				     GFP_ATOMIC);
+	pxmitpriv->hwxmits = kmalloc_array(pxmitpriv->hwxmit_entry,
+				sizeof(struct hw_xmit), GFP_ATOMIC);
 	if (pxmitpriv->hwxmits == NULL)
 		return;
 	hwxmits = pxmitpriv->hwxmits;
diff --git a/drivers/staging/rtl8712/usb_intf.c b/drivers/staging/rtl8712/usb_intf.c
index beff69b3ff0c..a3d733b145eb 100644
--- a/drivers/staging/rtl8712/usb_intf.c
+++ b/drivers/staging/rtl8712/usb_intf.c
@@ -581,9 +581,11 @@ static int r871xu_drv_init(struct usb_interface *pusb_intf,
 			 * address by setting bit 1 of first octet.
 			 */
 			mac[0] &= 0xFE;
-			dev_info(&udev->dev, "r8712u: MAC Address from user = %pM\n", mac);
+			dev_info(&udev->dev,
+				"r8712u: MAC Address from user = %pM\n", mac);
 		} else
-			dev_info(&udev->dev, "r8712u: MAC Address from efuse = %pM\n", mac);
+			dev_info(&udev->dev,
+				"r8712u: MAC Address from efuse = %pM\n", mac);
 		memcpy(pnetdev->dev_addr, mac, ETH_ALEN);
 	}
 	/* step 6. Load the firmware asynchronously */
@@ -635,7 +637,6 @@ static void r871xu_dev_remove(struct usb_interface *pusb_intf)
 	 * Reset usb port for sitesurvey fail issue. */
 	if (udev->state != USB_STATE_NOTATTACHED)
 		usb_reset_device(udev);
-	return;
 }
 
 static int __init r8712u_drv_entry(void)
diff --git a/drivers/staging/rtl8712/usb_ops_linux.c b/drivers/staging/rtl8712/usb_ops_linux.c
index a6c2aab0a946..e89d2b07fcb9 100644
--- a/drivers/staging/rtl8712/usb_ops_linux.c
+++ b/drivers/staging/rtl8712/usb_ops_linux.c
@@ -216,6 +216,7 @@ static void r8712_usb_read_port_complete(struct urb *purb)
 						0, (unsigned char *)precvbuf);
 			} else {
 				_pkt *pskb = precvbuf->pskb;
+
 				skb_put(pskb, purb->actual_length);
 				skb_queue_tail(&precvpriv->rx_skb_queue, pskb);
 				tasklet_hi_schedule(&precvpriv->recv_tasklet);
@@ -382,7 +383,8 @@ static void usb_write_port_complete(struct urb *purb)
 	case 0:
 		break;
 	default:
-		netdev_warn(padapter->pnetdev, "r8712u: pipe error: (%d)\n", purb->status);
+		netdev_warn(padapter->pnetdev,
+				"r8712u: pipe error: (%d)\n", purb->status);
 		break;
 	}
 	/* not to consider tx fragment */
@@ -496,11 +498,8 @@ int r8712_usbctrl_vendorreq(struct intf_priv *pintfpriv, u8 request, u16 value,
 	u8 *palloc_buf, *pIo_buf;
 
 	palloc_buf = kmalloc((u32)len + 16, GFP_ATOMIC);
-	if (palloc_buf == NULL) {
-		dev_err(&udev->dev, "%s: Can't alloc memory for vendor request\n",
-			__func__);
+	if (palloc_buf == NULL)
 		return -ENOMEM;
-	}
 	pIo_buf = palloc_buf + 16 - ((addr_t)(palloc_buf) & 0x0f);
 	if (requesttype == 0x01) {
 		pipe = usb_rcvctrlpipe(udev, 0); /* read_in */
diff --git a/drivers/staging/rtl8723au/core/rtw_ap.c b/drivers/staging/rtl8723au/core/rtw_ap.c
index 4d2880abf4ee..6b4092f05da5 100644
--- a/drivers/staging/rtl8723au/core/rtw_ap.c
+++ b/drivers/staging/rtl8723au/core/rtw_ap.c
@@ -79,12 +79,12 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 
 	p = rtw_get_ie23a(pie, WLAN_EID_TIM, &tim_ielen,
 			  pnetwork_mlmeext->IELength);
-	if (p != NULL && tim_ielen>0) {
+	if (p != NULL && tim_ielen > 0) {
 		tim_ielen += 2;
 
 		premainder_ie = p+tim_ielen;
 
-		tim_ie_offset = (int)(p -pie);
+		tim_ie_offset = (int)(p - pie);
 
 		remainder_ielen = pnetwork_mlmeext->IELength - tim_ie_offset - tim_ielen;
 
@@ -125,17 +125,17 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 			memcpy(pbackup_remainder_ie, premainder_ie, remainder_ielen);
 	}
 
-	*dst_ie++= WLAN_EID_TIM;
+	*dst_ie++ = WLAN_EID_TIM;
 
 	if ((pstapriv->tim_bitmap&0xff00) && (pstapriv->tim_bitmap&0x00fc))
 		tim_ielen = 5;
 	else
 		tim_ielen = 4;
 
-	*dst_ie++= tim_ielen;
+	*dst_ie++ = tim_ielen;
 
-	*dst_ie++= 0;/* DTIM count */
-	*dst_ie++= 1;/* DTIM period */
+	*dst_ie++ = 0;/* DTIM count */
+	*dst_ie++ = 1;/* DTIM period */
 
 	if (pstapriv->tim_bitmap & BIT(0))/* for bc/mc frames */
 		*dst_ie++ = BIT(0);/* bitmap ctrl */
@@ -143,10 +143,10 @@ static void update_BCNTIM(struct rtw_adapter *padapter)
 		*dst_ie++ = 0;
 
 	if (tim_ielen == 4) {
-		*dst_ie++ = *(u8*)&tim_bitmap_le;
+		*dst_ie++ = *(u8 *)&tim_bitmap_le;
 	} else if (tim_ielen == 5) {
 		memcpy(dst_ie, &tim_bitmap_le, 2);
-		dst_ie+= 2;
+		dst_ie += 2;
 	}
 
 	/* copy remainder IE */
@@ -194,7 +194,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 	list_for_each_safe(plist, ptmp, phead) {
 		psta = container_of(plist, struct sta_info, auth_list);
 
-		if (psta->expire_to>0) {
+		if (psta->expire_to > 0) {
 			psta->expire_to--;
 			if (psta->expire_to == 0) {
 				list_del_init(&psta->auth_list);
@@ -299,7 +299,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 		int ret = _FAIL;
 
 		psta = chk_alive_list[i];
-		if (!(psta->state &_FW_LINKED))
+		if (!(psta->state & _FW_LINKED))
 			continue;
 
 		if (psta->state & WIFI_SLEEP_STATE)
@@ -335,7 +335,7 @@ void	expire_timeout_chk23a(struct rtw_adapter *padapter)
 
 	}
 
-	if (backup_oper_channel>0) /* back to the original operation channel */
+	if (backup_oper_channel > 0) /* back to the original operation channel */
 		SelectChannel23a(padapter, backup_oper_channel);
 }
 
@@ -363,7 +363,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 		return;
 
 	/* b/g mode ra_bitmap */
-	for (i = 0; i<sizeof(psta->bssrateset); i++)
+	for (i = 0; i < sizeof(psta->bssrateset); i++)
 	{
 		if (psta->bssrateset[i])
 			tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
@@ -396,7 +396,7 @@ void add_RATid23a(struct rtw_adapter *padapter, struct sta_info *psta, u8 rssi_l
 		if (tx_ra_bitmap & 0xffff000)
 			sta_band |= WIRELESS_11_24N | WIRELESS_11G | WIRELESS_11B;
 		else if (tx_ra_bitmap & 0xff0)
-			sta_band |= WIRELESS_11G |WIRELESS_11B;
+			sta_band |= WIRELESS_11G | WIRELESS_11B;
 		else
 			sta_band |= WIRELESS_11B;
 	}
@@ -463,7 +463,8 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 
 		psta->ieee8021x_blocked = 0;
 
-		memset((void*)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+		memset((void *)&psta->sta_stats, 0,
+		       sizeof(struct stainfo_stats));
 
 		/* prepare for add_RATid23a */
 		supportRateNum = rtw_get_rateset_len23a((u8*)&pcur_network->SupportedRates);
@@ -473,7 +474,7 @@ static void update_bmc_sta(struct rtw_adapter *padapter)
 		psta->bssratelen = supportRateNum;
 
 		/* b/g mode ra_bitmap */
-		for (i = 0; i<supportRateNum; i++)
+		for (i = 0; i < supportRateNum; i++)
 		{
 			if (psta->bssrateset[i])
 				tx_ra_bitmap |= rtw_get_bit_value_from_ieee_value23a(psta->bssrateset[i]&0x7f);
@@ -599,7 +600,7 @@ void update_sta_info23a_apmode23a(struct rtw_adapter *padapter, struct sta_info
 
 	/* todo: init other variables */
 
-	memset((void*)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
+	memset((void *)&psta->sta_stats, 0, sizeof(struct stainfo_stats));
 
 	spin_lock_bh(&psta->lock);
 	psta->state |= _FW_LINKED;
@@ -644,7 +645,7 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 	u32 acparm;
 	struct registry_priv *pregpriv = &padapter->registrypriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct security_priv* psecuritypriv = &padapter->securitypriv;
+	struct security_priv *psecuritypriv = &padapter->securitypriv;
 	struct wlan_bssid_ex *pnetwork = &pmlmepriv->cur_network.network;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -705,7 +706,7 @@ static void start_bss_network(struct rtw_adapter *padapter, u8 *pbuf)
 
 	/* Set Security */
 	val8 = (psecuritypriv->dot11AuthAlgrthm == dot11AuthAlgrthm_8021X) ?
-		0xcc: 0xcf;
+		0xcc : 0xcf;
 	rtl8723a_set_sec_cfg(padapter, val8);
 
 	/* Beacon Control related register */
@@ -1021,7 +1022,7 @@ int rtw_check_beacon_data23a(struct rtw_adapter *padapter,
 	pbss_network->Length = get_wlan_bssid_ex_sz(pbss_network);
 
 	/* issue beacon to start bss network */
-	start_bss_network(padapter, (u8*)pbss_network);
+	start_bss_network(padapter, (u8 *)pbss_network);
 
 	/* alloc sta_info for ap itself */
 	psta = rtw_get_stainfo23a(&padapter->stapriv, pbss_network->MacAddress);
@@ -1674,7 +1675,8 @@ u8 ap_free_sta23a(struct rtw_adapter *padapter, struct sta_info *psta, bool acti
 
 	/* clear cam entry / key */
 	/* clear_cam_entry23a(padapter, (psta->mac_id + 3)); */
-	rtw_clearstakey_cmd23a(padapter, (u8*)psta, (u8)(psta->mac_id + 3), true);
+	rtw_clearstakey_cmd23a(padapter, (u8 *)psta, (u8)(psta->mac_id + 3),
+			       true);
 
 	spin_lock_bh(&psta->lock);
 	psta->state &= ~_FW_LINKED;
@@ -1829,7 +1831,7 @@ void rtw_ap_restore_network(struct rtw_adapter *padapter)
 
 	set_channel_bwmode23a(padapter, pmlmeext->cur_channel, pmlmeext->cur_ch_offset, pmlmeext->cur_bwmode);
 
-	start_bss_network(padapter, (u8*)&mlmepriv->cur_network.network);
+	start_bss_network(padapter, (u8 *)&mlmepriv->cur_network.network);
 
 	if (padapter->securitypriv.dot11PrivacyAlgrthm ==
 	    WLAN_CIPHER_SUITE_TKIP ||
@@ -1863,7 +1865,7 @@ void rtw_ap_restore_network(struct rtw_adapter *padapter)
 	for (i = 0; i < chk_alive_num; i++) {
 		psta = chk_alive_list[i];
 
-		if (psta->state &_FW_LINKED) {
+		if (psta->state & _FW_LINKED) {
 			Update_RA_Entry23a(padapter, psta);
 			/* pairwise key */
 			rtw_setstakey_cmd23a(padapter, (unsigned char *)psta, true);
@@ -1900,7 +1902,7 @@ void start_ap_mode23a(struct rtw_adapter *padapter)
 
 	pmlmepriv->ht_op_mode = 0;
 
-	for (i = 0; i<NUM_STA; i++)
+	for (i = 0; i < NUM_STA; i++)
 		pstapriv->sta_aid[i] = NULL;
 
 	/* for ACL */
diff --git a/drivers/staging/rtl8723au/core/rtw_cmd.c b/drivers/staging/rtl8723au/core/rtw_cmd.c
index 7241a5a8731c..4eaa50297b95 100644
--- a/drivers/staging/rtl8723au/core/rtw_cmd.c
+++ b/drivers/staging/rtl8723au/core/rtw_cmd.c
@@ -677,7 +677,7 @@ int rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 	struct set_stakey_rsp *psetstakey_rsp = NULL;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
-	struct sta_info *sta = (struct sta_info*)psta;
+	struct sta_info *sta = (struct sta_info *)psta;
 	int res = _SUCCESS;
 
 	ph2c = kzalloc(sizeof(struct cmd_obj), GFP_KERNEL);
@@ -717,11 +717,11 @@ int rtw_setstakey_cmd23a(struct rtw_adapter *padapter, u8 *psta, u8 unicast_key)
 
 	if (unicast_key == true) {
 		memcpy(&psetstakey_para->key, &sta->dot118021x_UncstKey, 16);
-        } else {
+	} else {
 		int idx = psecuritypriv->dot118021XGrpKeyid;
 		memcpy(&psetstakey_para->key,
 		       &psecuritypriv->dot118021XGrpKey[idx].skey, 16);
-        }
+	}
 
 	/* jeff: set this because at least sw key is ready */
 	padapter->securitypriv.busetkipkey = 1;
@@ -1493,7 +1493,7 @@ void rtw_setstaKey_cmdrsp_callback23a(struct rtw_adapter *padapter,
 	struct sta_info *psta;
 
 	pstapriv = &padapter->stapriv;
-	psetstakey_rsp = (struct set_stakey_rsp*) (pcmd->rsp);
+	psetstakey_rsp = (struct set_stakey_rsp *) (pcmd->rsp);
 	psta = rtw_get_stainfo23a(pstapriv, psetstakey_rsp->addr);
 
 	if (!psta) {
@@ -1513,12 +1513,12 @@ void rtw_setassocsta_cmdrsp_callback23a(struct rtw_adapter *padapter,
 {
 	struct sta_priv *pstapriv = &padapter->stapriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
-	struct set_assocsta_parm* passocsta_parm;
-	struct set_assocsta_rsp* passocsta_rsp;
+	struct set_assocsta_parm *passocsta_parm;
+	struct set_assocsta_rsp *passocsta_rsp;
 	struct sta_info *psta;
 
 	passocsta_parm = (struct set_assocsta_parm *)(pcmd->parmbuf);
-	passocsta_rsp = (struct set_assocsta_rsp*) (pcmd->rsp);
+	passocsta_rsp = (struct set_assocsta_rsp *) (pcmd->rsp);
 	psta = rtw_get_stainfo23a(pstapriv, passocsta_parm->addr);
 
 	if (psta == NULL) {
diff --git a/drivers/staging/rtl8723au/core/rtw_efuse.c b/drivers/staging/rtl8723au/core/rtw_efuse.c
index fe092c5defa6..9f6ce7d071cd 100644
--- a/drivers/staging/rtl8723au/core/rtw_efuse.c
+++ b/drivers/staging/rtl8723au/core/rtw_efuse.c
@@ -156,9 +156,7 @@ ReadEFuseByte23a(struct rtw_adapter *Adapter, u16 _offset, u8 *pbuf)
 	/* Check bit 32 read-ready */
 	retry = 0;
 	value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
-	/* while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10)) */
-	while(!(((value32 >> 24) & 0xff) & 0x80)  && (retry<10000))
-	{
+	while (!((value32 >> 24) & 0x80) && retry < 10000) {
 		value32 = rtl8723au_read32(Adapter, EFUSE_CTRL);
 		retry++;
 	}
@@ -285,8 +283,7 @@ EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 				 TYPE_EFUSE_REAL_CONTENT_LEN,
 				 (void *)&contentLen);
 
-	if (Address < contentLen)	/* E-fuse 512Byte */
-	{
+	if (Address < contentLen) { /* E-fuse 512Byte */
 		/* Write E-fuse Register address bit0~7 */
 		temp = Address & 0xFF;
 		rtl8723au_write8(Adapter, EFUSE_CTRL+1, temp);
@@ -302,12 +299,10 @@ EFUSE_Read1Byte23a(struct rtw_adapter *Adapter, u16 Address)
 
 		/* Wait Write-ready (0x30[31]= 1) */
 		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
-		while(!(Bytetemp & 0x80))
-		{
+		while (!(Bytetemp & 0x80)) {
 			Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 			k++;
-			if (k == 1000)
-			{
+			if (k == 1000) {
 				k = 0;
 				break;
 			}
@@ -357,8 +352,7 @@ EFUSE_Write1Byte(
 				 TYPE_EFUSE_REAL_CONTENT_LEN,
 				 (void *)&contentLen);
 
-	if (Address < contentLen)	/* E-fuse 512Byte */
-	{
+	if (Address < contentLen) { /* E-fuse 512Byte */
 		rtl8723au_write8(Adapter, EFUSE_CTRL, Value);
 
 		/* Write E-fuse Register address bit0~7 */
@@ -377,12 +371,10 @@ EFUSE_Write1Byte(
 
 		/* Wait Write-ready (0x30[31]= 0) */
 		Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
-		while(Bytetemp & 0x80)
-		{
+		while (Bytetemp & 0x80) {
 			Bytetemp = rtl8723au_read8(Adapter, EFUSE_CTRL+3);
 			k++;
-			if (k == 100)
-			{
+			if (k == 100) {
 				k = 0;
 				break;
 			}
@@ -472,23 +464,19 @@ efuse_WordEnableDataRead23a(u8	word_en,
 			 u8	*sourdata,
 			 u8	*targetdata)
 {
-	if (!(word_en&BIT(0)))
-	{
+	if (!(word_en&BIT(0))) {
 		targetdata[0] = sourdata[0];
 		targetdata[1] = sourdata[1];
 	}
-	if (!(word_en&BIT(1)))
-	{
+	if (!(word_en&BIT(1))) {
 		targetdata[2] = sourdata[2];
 		targetdata[3] = sourdata[3];
 	}
-	if (!(word_en&BIT(2)))
-	{
+	if (!(word_en&BIT(2))) {
 		targetdata[4] = sourdata[4];
 		targetdata[5] = sourdata[5];
 	}
-	if (!(word_en&BIT(3)))
-	{
+	if (!(word_en&BIT(3))) {
 		targetdata[6] = sourdata[6];
 		targetdata[7] = sourdata[7];
 	}
@@ -743,7 +731,7 @@ EFUSE_ShadowRead23a(
 	struct rtw_adapter *	pAdapter,
 	u8		Type,
 	u16		Offset,
-	u32		*Value	)
+	u32		*Value)
 {
 	if (Type == 1)
 		efuse_ShadowRead1Byte(pAdapter, Offset, (u8 *)Value);
diff --git a/drivers/staging/rtl8723au/core/rtw_ieee80211.c b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
index 23e666244f4e..6274cb397c92 100644
--- a/drivers/staging/rtl8723au/core/rtw_ieee80211.c
+++ b/drivers/staging/rtl8723au/core/rtw_ieee80211.c
@@ -295,7 +295,7 @@ exit:
 	return ret;
 }
 
-void rtw_set_supported_rate23a(u8* SupportedRates, uint mode)
+void rtw_set_supported_rate23a(u8 *SupportedRates, uint mode)
 {
 
 
@@ -520,7 +520,7 @@ int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int
 	return ret;
 }
 
-int rtw_parse_wpa2_ie23a(const u8* rsn_ie, int rsn_ie_len, int *group_cipher,
+int rtw_parse_wpa2_ie23a(const u8 *rsn_ie, int rsn_ie_len, int *group_cipher,
 		      int *pairwise_cipher, int *is_8021x)
 {
 	int i, ret = _SUCCESS;
diff --git a/drivers/staging/rtl8723au/core/rtw_led.c b/drivers/staging/rtl8723au/core/rtw_led.c
index e21a42c5418f..989cda29a57e 100644
--- a/drivers/staging/rtl8723au/core/rtw_led.c
+++ b/drivers/staging/rtl8723au/core/rtw_led.c
@@ -1842,7 +1842,7 @@ LedControl871x23a(struct rtw_adapter *padapter, enum led_ctl_mode LedAction) {
 	if ((padapter->bSurpriseRemoved == true) ||
 	    (padapter->bDriverStopped == true) ||
 	    (padapter->hw_init_completed == false)) {
-             return;
+		return;
 	}
 
 	if (ledpriv->bRegUseLed == false)
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme.c b/drivers/staging/rtl8723au/core/rtw_mlme.c
index c475b79bbf1d..1f6006439bbb 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme.c
@@ -159,7 +159,7 @@ rtw_find_network23a(struct rtw_queue *scanned_queue, u8 *addr)
 			break;
 
 		plist = plist->next;
-        }
+	}
 
 	if (plist == phead)
 		pnetwork = NULL;
@@ -208,7 +208,7 @@ int rtw_if_up23a(struct rtw_adapter *padapter)
 	return res;
 }
 
-void rtw_generate_random_ibss23a(u8* pibss)
+void rtw_generate_random_ibss23a(u8 *pibss)
 {
 	unsigned long curtime = jiffies;
 
@@ -425,7 +425,7 @@ static void update_current_network(struct rtw_adapter *adapter,
 	if (check_fwstate(pmlmepriv, _FW_LINKED) &&
 	    is_same_network23a(&pmlmepriv->cur_network.network, pnetwork)) {
 		update_network23a(&pmlmepriv->cur_network.network,
-				  pnetwork,adapter, true);
+				  pnetwork, adapter, true);
 
 		rtw_update_protection23a(adapter,
 					 pmlmepriv->cur_network.network.IEs,
@@ -510,7 +510,7 @@ static void rtw_update_scanned_network(struct rtw_adapter *adapter,
 		    target->reserved == 1)
 			update_ie = false;
 
-		update_network23a(&pnetwork->network, target,adapter,
+		update_network23a(&pnetwork->network, target, adapter,
 				  update_ie);
 	}
 
@@ -556,7 +556,7 @@ static int rtw_is_desired_network(struct rtw_adapter *adapter,
 		/* for  correct flow of 8021X  to do.... */
 		if (desired_encmode == Ndis802_11EncryptionDisabled &&
 		    privacy != 0)
-	            bselected = false;
+			bselected = false;
 	}
 
 	if (desired_encmode != Ndis802_11EncryptionDisabled && privacy == 0) {
@@ -577,7 +577,8 @@ static int rtw_is_desired_network(struct rtw_adapter *adapter,
 /* TODO: Perry : For Power Management */
 void rtw_atimdone_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,("receive atimdone_evet\n"));
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
+		 ("receive atimdone_evet\n"));
 
 	return;
 }
@@ -591,12 +592,12 @@ void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 
 	pnetwork = survey->bss;
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 		 ("rtw_survey_event_cb23a, ssid=%s\n", pnetwork->Ssid.ssid));
 
 	len = get_wlan_bssid_ex_sz(pnetwork);
 	if (len > (sizeof(struct wlan_bssid_ex))) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 			 ("\n ****rtw_survey_event_cb23a: return a wrong "
 			  "bss ***\n"));
 		return;
@@ -610,7 +611,7 @@ void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 		   "rtw_survey_event_cb23a : WIFI_ADHOC_MASTER_STATE\n\n"); */
 		if (ether_addr_equal(pmlmepriv->cur_network.network.MacAddress,
 				     pnetwork->MacAddress)) {
-			struct wlan_network* ibss_wlan;
+			struct wlan_network *ibss_wlan;
 
 			pmlmepriv->cur_network.network.beacon_interval =
 				pnetwork->beacon_interval;
@@ -637,7 +638,7 @@ void rtw_survey_event_cb23a(struct rtw_adapter *adapter, const u8 *pbuf)
 
 	/*  lock pmlmepriv->lock when you accessing network_q */
 	if (!check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
-	        if (pnetwork->Ssid.ssid[0] == 0)
+		if (pnetwork->Ssid.ssid[0] == 0)
 			pnetwork->Ssid.ssid_len = 0;
 
 		rtw_add_network(adapter, pnetwork);
@@ -743,7 +744,7 @@ static void free_scanqueue(struct mlme_priv *pmlmepriv)
 		pnetwork = container_of(plist, struct wlan_network, list);
 		pnetwork->fixed = false;
 		_rtw_free_network23a(pmlmepriv, pnetwork);
-        }
+	}
 
 	spin_unlock_bh(&scan_queue->lock);
 }
@@ -754,11 +755,11 @@ static void free_scanqueue(struct mlme_priv *pmlmepriv)
 void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
 				 int lock_scanned_queue)
 {
-	struct wlan_network* pwlan;
+	struct wlan_network *pwlan;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	struct sta_priv *pstapriv = &adapter->stapriv;
 	struct wlan_network *tgt_network = &pmlmepriv->cur_network;
-	struct sta_info* psta;
+	struct sta_info *psta;
 
 	RT_TRACE(_module_rtl871x_mlme_c_, _drv_notice_,
 		 ("+rtw_free_assoc_resources23a\n"));
@@ -796,7 +797,7 @@ void rtw_free_assoc_resources23a(struct rtw_adapter *adapter,
 	if (pwlan)
 		pwlan->fixed = false;
 	else
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 			 ("rtw_free_assoc_resources23a : pwlan== NULL\n"));
 
 	if (check_fwstate(pmlmepriv, WIFI_ADHOC_MASTER_STATE) &&
@@ -855,7 +856,7 @@ void rtw_indicate_disconnect23a(struct rtw_adapter *padapter)
 
 	_clr_fwstate_(pmlmepriv, _FW_UNDER_LINKING|WIFI_UNDER_WPS);
 
-        /* DBG_8723A("clear wps when %s\n", __func__); */
+	/* DBG_8723A("clear wps when %s\n", __func__); */
 
 	if (padapter->mlmepriv.to_roaming > 0)
 		_clr_fwstate_(pmlmepriv, _FW_LINKED);
@@ -1094,15 +1095,15 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 	struct wlan_network *pcur_wlan, *ptarget_wlan = NULL;
 	bool the_same_macaddr;
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 		 ("joinbss event call back received with res=%d\n",
 		  pnetwork->join_res));
 
 	if (pmlmepriv->assoc_ssid.ssid_len == 0) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 			 ("@@@@@   joinbss event call back  for Any SSid\n"));
 	} else {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 			 ("@@@@@   rtw23a_joinbss_event_cb for SSid:%s\n",
 			  pmlmepriv->assoc_ssid.ssid));
 	}
@@ -1115,7 +1116,7 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 
 	pnetwork->network.Length = get_wlan_bssid_ex_sz(&pnetwork->network);
 	if (pnetwork->network.Length > sizeof(struct wlan_bssid_ex)) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 			 ("\n\n ***joinbss_evt_callback return a wrong bss "
 			  "***\n\n"));
 		return;
@@ -1123,12 +1124,12 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 
 	spin_lock_bh(&pmlmepriv->lock);
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 		 ("\n rtw23a_joinbss_event_cb !! _enter_critical\n"));
 
 	if (pnetwork->join_res > 0) {
 		spin_lock_bh(&pmlmepriv->scanned_queue.lock);
-		if (check_fwstate(pmlmepriv,_FW_UNDER_LINKING)) {
+		if (check_fwstate(pmlmepriv, _FW_UNDER_LINKING)) {
 			/* s1. find ptarget_wlan */
 			if (check_fwstate(pmlmepriv, _FW_LINKED)) {
 				if (the_same_macaddr == true) {
@@ -1172,7 +1173,7 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 							      ptarget_wlan,
 							      pnetwork);
 			else {
-				RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 					 ("Can't find ptarget_wlan when "
 					  "joinbss_event callback\n"));
 				spin_unlock_bh(&pmlmepriv->scanned_queue.lock);
@@ -1200,7 +1201,7 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 			else {
 				/* adhoc mode will rtw_indicate_connect23a
 				   when rtw_stassoc_event_callback23a */
-				RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+				RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 					 ("adhoc mode, fw_state:%x",
 					  get_fwstate(pmlmepriv)));
 			}
@@ -1208,7 +1209,7 @@ void rtw_joinbss_event_prehandle23a(struct rtw_adapter *adapter, u8 *pbuf)
 			/* s5. Cancle assoc_timer */
 			del_timer_sync(&pmlmepriv->assoc_timer);
 
-			RT_TRACE(_module_rtl871x_mlme_c_,_drv_info_,
+			RT_TRACE(_module_rtl871x_mlme_c_, _drv_info_,
 				 ("Cancle assoc_timer\n"));
 		} else {
 			RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
@@ -1256,7 +1257,7 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	struct sta_info *psta;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
-	struct stassoc_event *pstassoc = (struct stassoc_event*)pbuf;
+	struct stassoc_event *pstassoc = (struct stassoc_event *)pbuf;
 	struct wlan_network *cur_network = &pmlmepriv->cur_network;
 	struct wlan_network *ptarget_wlan;
 
@@ -1278,7 +1279,7 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	psta = rtw_get_stainfo23a(&adapter->stapriv, pstassoc->macaddr);
 	if (psta != NULL) {
 		/* the sta have been in sta_info_queue => do nothing */
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 			 ("Error: rtw_stassoc_event_callback23a: sta has "
 			  "been in sta_hash_queue\n"));
 		/* between drv has received this event before and
@@ -1289,7 +1290,7 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	psta = rtw_alloc_stainfo23a(&adapter->stapriv, pstassoc->macaddr,
 		GFP_KERNEL);
 	if (!psta) {
-		RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+		RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 			 ("Can't alloc sta_info when "
 			  "rtw_stassoc_event_callback23a\n"));
 		return;
@@ -1299,7 +1300,7 @@ void rtw_stassoc_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	psta->qos_option = 0;
 	psta->mac_id = (uint)pstassoc->cam_id;
 	/* psta->aid = (uint)pstassoc->cam_id; */
-	DBG_8723A("%s\n",__func__);
+	DBG_8723A("%s\n", __func__);
 	/* for ad-hoc mode */
 	rtl8723a_SetHalODMVar(adapter, HAL_ODM_STA_INFO, psta, true);
 
@@ -1335,7 +1336,7 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 {
 	int mac_id;
 	struct sta_info *psta;
-	struct wlan_network* pwlan;
+	struct wlan_network *pwlan;
 	struct wlan_bssid_ex *pdev_network;
 	struct mlme_priv *pmlmepriv = &adapter->mlmepriv;
 	struct stadel_event *pstadel = (struct stadel_event *)pbuf;
@@ -1351,7 +1352,7 @@ void rtw_stadel_event_callback23a(struct rtw_adapter *adapter, const u8 *pbuf)
 	DBG_8723A("%s(mac_id=%d)=" MAC_FMT "\n", __func__, mac_id,
 		  MAC_ARG(pstadel->macaddr));
 
-        if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
+	if (check_fwstate(pmlmepriv, WIFI_AP_STATE))
 		return;
 
 	mlmeext_sta_del_event_callback23a(adapter);
@@ -1768,7 +1769,7 @@ exit:
 int rtw_set_auth23a(struct rtw_adapter * adapter,
 		    struct security_priv *psecuritypriv)
 {
-	struct cmd_obj* pcmd;
+	struct cmd_obj *pcmd;
 	struct setauth_parm *psetauthparm;
 	struct cmd_priv *pcmdpriv = &adapter->cmdpriv;
 	int res = _SUCCESS;
@@ -1794,7 +1795,7 @@ int rtw_set_auth23a(struct rtw_adapter * adapter,
 	pcmd->rsp = NULL;
 	pcmd->rspsz = 0;
 
-	RT_TRACE(_module_rtl871x_mlme_c_,_drv_err_,
+	RT_TRACE(_module_rtl871x_mlme_c_, _drv_err_,
 		 ("after enqueue set_auth_cmd, auth_mode=%x\n",
 		  psecuritypriv->dot11AuthAlgrthm));
 
@@ -2048,10 +2049,10 @@ int rtw_restruct_sec_ie23a(struct rtw_adapter *adapter, u8 *in_ie, u8 *out_ie,
 	return ielength;
 }
 
-void rtw_init_registrypriv_dev_network23a(struct rtw_adapter* adapter)
+void rtw_init_registrypriv_dev_network23a(struct rtw_adapter *adapter)
 {
-	struct registry_priv* pregistrypriv = &adapter->registrypriv;
-	struct eeprom_priv* peepriv = &adapter->eeprompriv;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
+	struct eeprom_priv *peepriv = &adapter->eeprompriv;
 	struct wlan_bssid_ex    *pdev_network = &pregistrypriv->dev_network;
 	u8 *myhwaddr = myid(peepriv);
 
@@ -2063,10 +2064,10 @@ void rtw_init_registrypriv_dev_network23a(struct rtw_adapter* adapter)
 	pdev_network->beacon_interval = 100;
 }
 
-void rtw_update_registrypriv_dev_network23a(struct rtw_adapter* adapter)
+void rtw_update_registrypriv_dev_network23a(struct rtw_adapter *adapter)
 {
 	int sz = 0;
-	struct registry_priv* pregistrypriv = &adapter->registrypriv;
+	struct registry_priv *pregistrypriv = &adapter->registrypriv;
 	struct wlan_bssid_ex *pdev_network = &pregistrypriv->dev_network;
 	struct security_priv *psecuritypriv = &adapter->securitypriv;
 	struct wlan_network *cur_network = &adapter->mlmepriv.cur_network;
@@ -2187,7 +2188,7 @@ bool rtw_restructure_ht_ie23a(struct rtw_adapter *padapter, u8 *in_ie,
 
 		pframe = rtw_set_ie23a(out_ie + out_len, WLAN_EID_HT_CAPABILITY,
 				    sizeof(struct ieee80211_ht_cap),
-				    (unsigned char*)&ht_capie, pout_len);
+				    (unsigned char *)&ht_capie, pout_len);
 
 		phtpriv->ht_option = true;
 
diff --git a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
index c5fdcb89dacd..3eb77de17e3a 100644
--- a/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
+++ b/drivers/staging/rtl8723au/core/rtw_mlme_ext.c
@@ -356,7 +356,7 @@ int init_hw_mlme_ext23a(struct rtw_adapter *padapter)
 	return _SUCCESS;
 }
 
-static void init_mlme_ext_priv23a_value(struct rtw_adapter* padapter)
+static void init_mlme_ext_priv23a_value(struct rtw_adapter *padapter)
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -480,7 +480,7 @@ static void init_channel_list(struct rtw_adapter *padapter,
 	channel_list->reg_classes = cla;
 }
 
-static u8 init_channel_set(struct rtw_adapter* padapter, u8 cplan,
+static u8 init_channel_set(struct rtw_adapter *padapter, u8 cplan,
 			   struct rt_channel_info *c_set)
 {
 	u8 i, ch_size = 0;
@@ -564,10 +564,10 @@ static u8 init_channel_set(struct rtw_adapter* padapter, u8 cplan,
 	return ch_size;
 }
 
-int init_mlme_ext_priv23a(struct rtw_adapter* padapter)
+int init_mlme_ext_priv23a(struct rtw_adapter *padapter)
 {
 	int res = _SUCCESS;
-	struct registry_priv* pregistrypriv = &padapter->registrypriv;
+	struct registry_priv *pregistrypriv = &padapter->registrypriv;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -625,7 +625,7 @@ _mgt_dispatcher23a(struct rtw_adapter *padapter, struct mlme_handler *ptable,
 			return;
 
 		ptable->func(padapter, precv_frame);
-        }
+	}
 }
 
 void mgt_dispatcher23a(struct rtw_adapter *padapter,
@@ -1949,7 +1949,7 @@ OnDisassoc23a(struct rtw_adapter *padapter, struct recv_frame *precv_frame)
 
 	reason = le16_to_cpu(mgmt->u.disassoc.reason_code);
 
-        DBG_8723A("%s Reason code(%d)\n", __func__, reason);
+	DBG_8723A("%s Reason code(%d)\n", __func__, reason);
 
 #ifdef CONFIG_8723AU_AP_MODE
 	if (check_fwstate(pmlmepriv, WIFI_AP_STATE)) {
@@ -2128,7 +2128,7 @@ static int on_action_public23a(struct rtw_adapter *padapter,
 						      IEEE80211_BAND_5GHZ);
 
 	if (cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, pframe,
-			     skb->len, 0, GFP_ATOMIC))
+			     skb->len, 0))
 		return _SUCCESS;
 
 	return _FAIL;
@@ -2436,7 +2436,7 @@ void issue_beacon23a(struct rtw_adapter *padapter, int timeout_ms)
 		if (wps_ie && wps_ie[1] > 0) {
 			rtw_get_wps_attr_content23a(wps_ie, wps_ie[1],
 						    WPS_ATTR_SELECTED_REGISTRAR,
-						    (u8*)&sr);
+						    (u8 *)&sr);
 		}
 		if (sr != 0)
 			set_fwstate(pmlmepriv, WIFI_UNDER_WPS);
@@ -4313,7 +4313,7 @@ fail:
 	return NULL;
 }
 
-static void start_create_ibss(struct rtw_adapter* padapter)
+static void start_create_ibss(struct rtw_adapter *padapter)
 {
 	unsigned short caps;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
@@ -4362,7 +4362,7 @@ static void start_create_ibss(struct rtw_adapter* padapter)
 	}
 }
 
-static void start_clnt_join(struct rtw_adapter* padapter)
+static void start_clnt_join(struct rtw_adapter *padapter)
 {
 	unsigned short caps;
 	u8 val8;
@@ -4422,7 +4422,7 @@ static void start_clnt_join(struct rtw_adapter* padapter)
 	}
 }
 
-static void start_clnt_auth(struct rtw_adapter* padapter)
+static void start_clnt_auth(struct rtw_adapter *padapter)
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -4453,7 +4453,7 @@ static void start_clnt_auth(struct rtw_adapter* padapter)
 	set_link_timer(pmlmeext, REAUTH_TO);
 }
 
-static void start_clnt_assoc(struct rtw_adapter* padapter)
+static void start_clnt_assoc(struct rtw_adapter *padapter)
 {
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
@@ -4763,7 +4763,7 @@ void report_survey_event23a(struct rtw_adapter *padapter,
 	pcmd_obj->rsp = NULL;
 	pcmd_obj->rspsz  = 0;
 
-	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
 	pc2h_evt_hdr->len = sizeof(struct survey_event);
 	pc2h_evt_hdr->ID = GEN_EVT_CODE(_Survey);
 	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
@@ -4814,7 +4814,7 @@ void report_surveydone_event23a(struct rtw_adapter *padapter)
 	pcmd_obj->rsp = NULL;
 	pcmd_obj->rspsz  = 0;
 
-	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
 	pc2h_evt_hdr->len = sizeof(struct surveydone_event);
 	pc2h_evt_hdr->ID = GEN_EVT_CODE(_SurveyDone);
 	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
@@ -4858,7 +4858,7 @@ void report_join_res23a(struct rtw_adapter *padapter, int res)
 	pcmd_obj->rsp = NULL;
 	pcmd_obj->rspsz  = 0;
 
-	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
 	pc2h_evt_hdr->len = sizeof(struct joinbss_event);
 	pc2h_evt_hdr->ID = GEN_EVT_CODE(_JoinBss);
 	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
@@ -4878,7 +4878,7 @@ void report_join_res23a(struct rtw_adapter *padapter, int res)
 }
 
 void report_del_sta_event23a(struct rtw_adapter *padapter,
-			     unsigned char* MacAddr, unsigned short reason)
+			     unsigned char *MacAddr, unsigned short reason)
 {
 	struct cmd_obj *pcmd_obj;
 	u8 *pevtcmd;
@@ -4908,7 +4908,7 @@ void report_del_sta_event23a(struct rtw_adapter *padapter,
 	pcmd_obj->rsp = NULL;
 	pcmd_obj->rspsz  = 0;
 
-	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
 	pc2h_evt_hdr->len = sizeof(struct stadel_event);
 	pc2h_evt_hdr->ID = GEN_EVT_CODE(_DelSTA);
 	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
@@ -4934,7 +4934,7 @@ void report_del_sta_event23a(struct rtw_adapter *padapter,
 }
 
 void report_add_sta_event23a(struct rtw_adapter *padapter,
-			     unsigned char* MacAddr, int cam_idx)
+			     unsigned char *MacAddr, int cam_idx)
 {
 	struct cmd_obj *pcmd_obj;
 	u8 *pevtcmd;
@@ -4962,7 +4962,7 @@ void report_add_sta_event23a(struct rtw_adapter *padapter,
 	pcmd_obj->rsp = NULL;
 	pcmd_obj->rspsz  = 0;
 
-	pc2h_evt_hdr = (struct C2HEvent_Header*)(pevtcmd);
+	pc2h_evt_hdr = (struct C2HEvent_Header *)(pevtcmd);
 	pc2h_evt_hdr->len = sizeof(struct stassoc_event);
 	pc2h_evt_hdr->ID = GEN_EVT_CODE(_AddSTA);
 	pc2h_evt_hdr->seq = atomic_inc_return(&pmlmeext->event_seq);
@@ -5564,7 +5564,7 @@ int join_cmd_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 	struct ieee80211_ht_operation *pht_info;
 	u32 i;
 	u8 *p;
-        /* u32	initialgain; */
+	/* u32	initialgain; */
 	/* u32	acparm; */
 
 	/* check already connecting to AP or not */
@@ -6022,7 +6022,7 @@ int add_ba_hdl23a(struct rtw_adapter *padapter, const u8 *pbuf)
 	return H2C_SUCCESS;
 }
 
-int set_tx_beacon_cmd23a(struct rtw_adapter* padapter)
+int set_tx_beacon_cmd23a(struct rtw_adapter *padapter)
 {
 	struct cmd_obj *ph2c;
 	struct Tx_Beacon_param *ptxBeacon_parm;
diff --git a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
index d5fa26ab38a6..1b2af7381d82 100644
--- a/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
+++ b/drivers/staging/rtl8723au/core/rtw_pwrctrl.c
@@ -23,7 +23,7 @@
 #include <rtl8723a_bt_intf.h>
 #include <usb_ops_linux.h>
 
-void ips_enter23a(struct rtw_adapter * padapter)
+void ips_enter23a(struct rtw_adapter *padapter)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 
@@ -53,7 +53,7 @@ void ips_enter23a(struct rtw_adapter * padapter)
 	up(&pwrpriv->lock);
 }
 
-int ips_leave23a(struct rtw_adapter * padapter)
+int ips_leave23a(struct rtw_adapter *padapter)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct security_priv *psecuritypriv = &padapter->securitypriv;
@@ -146,7 +146,7 @@ exit:
 	return ret;
 }
 
-void rtw_ps_processor23a(struct rtw_adapter*padapter)
+void rtw_ps_processor23a(struct rtw_adapter *padapter)
 {
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -246,7 +246,7 @@ void rtw_set_rpwm23a(struct rtw_adapter *padapter, u8 pslv)
 	pwrpriv->cpwm = pslv;
 }
 
-static bool PS_RDY_CHECK(struct rtw_adapter * padapter)
+static bool PS_RDY_CHECK(struct rtw_adapter *padapter)
 {
 	unsigned long delta_time;
 	struct pwrctrl_priv *pwrpriv = &padapter->pwrctrlpriv;
diff --git a/drivers/staging/rtl8723au/core/rtw_recv.c b/drivers/staging/rtl8723au/core/rtw_recv.c
index a9f8183349ab..5bc7734d9a72 100644
--- a/drivers/staging/rtl8723au/core/rtw_recv.c
+++ b/drivers/staging/rtl8723au/core/rtw_recv.c
@@ -282,7 +282,7 @@ int recvframe_chkmic(struct rtw_adapter *adapter,
 	u32	datalen;
 	u8	miccode[8];
 	u8	bmic_err = false, brpt_micerror = true;
-	u8	*pframe, *payload,*pframemic;
+	u8	*pframe, *payload, *pframemic;
 	u8	*mickey;
 	/* u8	*iv, rxdata_key_idx = 0; */
 	struct	sta_info *stainfo;
@@ -577,7 +577,7 @@ static struct recv_frame *portctrl(struct rtw_adapter *adapter,
 				 ("########portctrl:psta->ieee8021x_blocked =="
 				  "1\n"));
 
-		        if (ether_type == eapol_type) {
+			if (ether_type == eapol_type) {
 				prtnframe = precv_frame;
 			} else {
 				/* free this frame */
@@ -1170,7 +1170,7 @@ static int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 
 				if (psta->sleepq_len>0)
 					pxmitframe->attrib.mdata = 1;
-                                else
+				else
 					pxmitframe->attrib.mdata = 0;
 
 				pxmitframe->attrib.triggered = 1;
@@ -1230,7 +1230,7 @@ static int validate_recv_ctrl_frame(struct rtw_adapter *padapter,
 	return _FAIL;
 }
 
-struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
+struct recv_frame *recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 					struct recv_frame *precv_frame);
 static int validate_recv_mgnt_frame(struct rtw_adapter *padapter,
 				    struct recv_frame *precv_frame)
@@ -1705,7 +1705,7 @@ struct recv_frame *recvframe_defrag(struct rtw_adapter *adapter,
 }
 
 /* check if need to defrag, if needed queue the frame to defrag_q */
-struct recv_frame* recvframe_chk_defrag23a(struct rtw_adapter *padapter,
+struct recv_frame *recvframe_chk_defrag23a(struct rtw_adapter *padapter,
 					struct recv_frame *precv_frame)
 {
 	u8	ismfrag;
@@ -1891,7 +1891,7 @@ int check_indicate_seq(struct recv_reorder_ctrl *preorder_ctrl, u16 seq_num)
 }
 
 static int enqueue_reorder_recvframe23a(struct recv_reorder_ctrl *preorder_ctrl,
-				        struct recv_frame *prframe)
+					struct recv_frame *prframe)
 {
 	struct rx_pkt_attrib *pattrib = &prframe->attrib;
 	struct rtw_queue *ppending_recvframe_queue;
@@ -1975,7 +1975,7 @@ int recv_indicatepkts_in_order(struct rtw_adapter *padapter,
 		}
 
 		prframe = container_of(plist, struct recv_frame, list);
-	        pattrib = &prframe->attrib;
+		pattrib = &prframe->attrib;
 		preorder_ctrl->indicate_seq = pattrib->seq_num;
 	}
 
@@ -2125,7 +2125,7 @@ int recv_indicatepkt_reorder(struct rtw_adapter *padapter,
 
 _err_exit:
 
-        spin_unlock_bh(&ppending_recvframe_queue->lock);
+	spin_unlock_bh(&ppending_recvframe_queue->lock);
 	return _FAIL;
 }
 
diff --git a/drivers/staging/rtl8723au/core/rtw_security.c b/drivers/staging/rtl8723au/core/rtw_security.c
index d6c07adc7bf4..76371ae69377 100644
--- a/drivers/staging/rtl8723au/core/rtw_security.c
+++ b/drivers/staging/rtl8723au/core/rtw_security.c
@@ -623,7 +623,7 @@ int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 	struct arc4context mycontext;
 	int			curfragnum, length;
 	u32	prwskeylen;
-	u8	*pframe, *payload,*iv,*prwskey;
+	u8	*pframe, *payload, *iv, *prwskey;
 	union pn48 dot11txpn;
 	struct	sta_info		*stainfo;
 	struct	pkt_attrib	 *pattrib = &pxmitframe->attrib;
@@ -702,7 +702,7 @@ int rtw_tkip_encrypt23a(struct rtw_adapter *padapter,
 		}
 		else{
 			RT_TRACE(_module_rtl871x_security_c_, _drv_err_, ("rtw_tkip_encrypt23a: stainfo == NULL!!!\n"));
-		                DBG_8723A("%s, psta == NUL\n", __func__);
+			DBG_8723A("%s, psta == NUL\n", __func__);
 			res = _FAIL;
 		}
 
@@ -723,7 +723,7 @@ int rtw_tkip_decrypt23a(struct rtw_adapter *padapter,
 	struct arc4context mycontext;
 	int	length;
 	u32	prwskeylen;
-	u8	*pframe, *payload,*iv,*prwskey;
+	u8	*pframe, *payload, *iv, *prwskey;
 	union pn48 dot11txpn;
 	struct	sta_info		*stainfo;
 	struct	rx_pkt_attrib *prxattrib = &precvframe->attrib;
@@ -1071,12 +1071,8 @@ static void construct_mic_header1(u8 *mic_header1, int header_length, u8 *mpdu)
 /* Builds the last MIC header block from        */
 /* header fields.                               */
 /************************************************/
-static void construct_mic_header2(
-		        u8 *mic_header2,
-		        u8 *mpdu,
-		        int a4_exists,
-		        int qc_exists
-		      )
+static void construct_mic_header2(u8 *mic_header2, u8 *mpdu, int a4_exists,
+				  int qc_exists)
 {
 	int i;
 
diff --git a/drivers/staging/rtl8723au/core/rtw_sreset.c b/drivers/staging/rtl8723au/core/rtw_sreset.c
index 9a79e11f7ffc..58ed980795a6 100644
--- a/drivers/staging/rtl8723au/core/rtw_sreset.c
+++ b/drivers/staging/rtl8723au/core/rtw_sreset.c
@@ -173,9 +173,8 @@ static void sreset_start_adapter(struct rtw_adapter *padapter)
 
 	DBG_8723A("%s(%s)\n", __func__, padapter->pnetdev->name);
 
-	if (check_fwstate(pmlmepriv, _FW_LINKED)) {
+	if (check_fwstate(pmlmepriv, _FW_LINKED))
 		sreset_restore_network_status(padapter);
-	}
 
 	/* TODO: OS and HCI independent */
 	tasklet_hi_schedule(&pxmitpriv->xmit_tasklet);
diff --git a/drivers/staging/rtl8723au/core/rtw_wlan_util.c b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
index 32f360301c77..09c44a55d4a6 100644
--- a/drivers/staging/rtl8723au/core/rtw_wlan_util.c
+++ b/drivers/staging/rtl8723au/core/rtw_wlan_util.c
@@ -352,7 +352,7 @@ static void set_bwmode(struct rtw_adapter *padapter, unsigned short bwmode,
 }
 
 void set_channel_bwmode23a(struct rtw_adapter *padapter, unsigned char channel,
-		        unsigned char channel_offset, unsigned short bwmode)
+			   unsigned char channel_offset, unsigned short bwmode)
 {
 	u8 center_ch;
 
@@ -1537,7 +1537,7 @@ void process_addba_req23a(struct rtw_adapter *padapter,
 	u16 tid, start_seq, param;
 	struct recv_reorder_ctrl *preorder_ctrl;
 	struct sta_priv *pstapriv = &padapter->stapriv;
-	struct ADDBA_request *preq = (struct ADDBA_request*)paddba_req;
+	struct ADDBA_request *preq = (struct ADDBA_request *)paddba_req;
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
diff --git a/drivers/staging/rtl8723au/core/rtw_xmit.c b/drivers/staging/rtl8723au/core/rtw_xmit.c
index d83af877ad6f..7a8038156cea 100644
--- a/drivers/staging/rtl8723au/core/rtw_xmit.c
+++ b/drivers/staging/rtl8723au/core/rtw_xmit.c
@@ -254,14 +254,14 @@ static void update_attrib_vcs_info(struct rtw_adapter *padapter, struct xmit_fra
 	struct mlme_ext_priv *pmlmeext = &padapter->mlmeextpriv;
 	struct mlme_ext_info *pmlmeinfo = &pmlmeext->mlmext_info;
 
-        if (pattrib->psta) {
+	if (pattrib->psta) {
 		psta = pattrib->psta;
 	} else {
 		DBG_8723A("%s, call rtw_get_stainfo23a()\n", __func__);
 		psta = rtw_get_stainfo23a(&padapter->stapriv, &pattrib->ra[0]);
 	}
 
-        if (psta == NULL) {
+	if (psta == NULL) {
 		DBG_8723A("%s, psta == NUL\n", __func__);
 		return;
 	}
diff --git a/drivers/staging/rtl8723au/hal/hal_com.c b/drivers/staging/rtl8723au/hal/hal_com.c
index 8f299ec89bb9..bf919f6e4128 100644
--- a/drivers/staging/rtl8723au/hal/hal_com.c
+++ b/drivers/staging/rtl8723au/hal/hal_com.c
@@ -535,6 +535,7 @@ void rtl8723a_set_bcn_func(struct rtw_adapter *padapter, u8 val)
 void rtl8723a_check_bssid(struct rtw_adapter *padapter, u8 val)
 {
 	u32 val32;
+
 	val32 = rtl8723au_read32(padapter, REG_RCR);
 	if (val)
 		val32 |= RCR_CBSSID_DATA | RCR_CBSSID_BCN;
@@ -588,7 +589,7 @@ void rtl8723a_on_rcr_am(struct rtw_adapter *padapter)
 {
 	rtl8723au_write32(padapter, REG_RCR,
 		    rtl8723au_read32(padapter, REG_RCR) | RCR_AM);
-	DBG_8723A("%s, %d, RCR = %x \n", __func__, __LINE__,
+	DBG_8723A("%s, %d, RCR = %x\n", __func__, __LINE__,
 		  rtl8723au_read32(padapter, REG_RCR));
 }
 
@@ -596,7 +597,7 @@ void rtl8723a_off_rcr_am(struct rtw_adapter *padapter)
 {
 	rtl8723au_write32(padapter, REG_RCR,
 		    rtl8723au_read32(padapter, REG_RCR) & (~RCR_AM));
-	DBG_8723A("%s, %d, RCR = %x \n", __func__, __LINE__,
+	DBG_8723A("%s, %d, RCR = %x\n", __func__, __LINE__,
 		  rtl8723au_read32(padapter, REG_RCR));
 }
 
@@ -666,12 +667,12 @@ void rtl8723a_cam_empty_entry(struct rtw_adapter *padapter, u8 ucIndex)
 		/* delay_ms(40); */
 		rtl8723au_write32(padapter, WCAMI, ulContent);
 		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("rtl8723a_cam_empty_entry(): WRITE A4: %lx \n",
+		   ("rtl8723a_cam_empty_entry(): WRITE A4: %lx\n",
 		   ulContent));*/
 		/* delay_ms(40); */
 		rtl8723au_write32(padapter, REG_CAMCMD, ulCommand);
 		/* RT_TRACE(COMP_SEC, DBG_LOUD,
-		   ("rtl8723a_cam_empty_entry(): WRITE A0: %lx \n",
+		   ("rtl8723a_cam_empty_entry(): WRITE A0: %lx\n",
 		   ulCommand));*/
 	}
 }
@@ -741,9 +742,8 @@ void rtl8723a_fifo_cleanup(struct rtw_adapter *padapter)
 			if (!v32)
 				break;
 		} while (trycnt--);
-		if (trycnt == 0) {
+		if (trycnt == 0)
 			DBG_8723A("Stop RX DMA failed......\n");
-		}
 
 		/*  RQPN Load 0 */
 		rtl8723au_write16(padapter, REG_RQPN_NPQ, 0);
diff --git a/drivers/staging/rtl8723au/hal/usb_halinit.c b/drivers/staging/rtl8723au/hal/usb_halinit.c
index b49bf33184de..a5de03d45aa9 100644
--- a/drivers/staging/rtl8723au/hal/usb_halinit.c
+++ b/drivers/staging/rtl8723au/hal/usb_halinit.c
@@ -916,7 +916,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 		break;
 	case rf_sleep:
 	case rf_off:
-		value8 = rtl8723au_read8(Adapter, REG_SPS0_CTRL) ;
+		value8 = rtl8723au_read8(Adapter, REG_SPS0_CTRL);
 		if (IS_81xxC_VENDOR_UMC_B_CUT(pHalData->VersionID))
 			value8 &= ~BIT(0);
 		else
@@ -989,7 +989,7 @@ static void phy_SsPwrSwitch92CU(struct rtw_adapter *Adapter,
 				mdelay(10);
 
 				/*  Set BB reset at first */
-				value8 = 0 ;
+				value8 = 0;
 				value8 |= (FEN_USBD | FEN_USBA |
 					   FEN_BB_GLB_RSTn);
 				/* 0x16 */
@@ -1217,26 +1217,6 @@ static void _ReadLEDSetting(struct rtw_adapter *Adapter, u8 *PROMContent,
 	pledpriv->LedStrategy = HW_LED;
 }
 
-static void Hal_EfuseParsePIDVID_8723AU(struct rtw_adapter *pAdapter,
-					u8 *hwinfo, bool AutoLoadFail)
-{
-	struct hal_data_8723a *pHalData = GET_HAL_DATA(pAdapter);
-
-	if (AutoLoadFail) {
-		pHalData->EEPROMVID = 0;
-		pHalData->EEPROMPID = 0;
-	} else {
-		/*  VID, PID */
-		pHalData->EEPROMVID =
-			le16_to_cpu(*(u16 *)&hwinfo[EEPROM_VID_8723AU]);
-		pHalData->EEPROMPID =
-			le16_to_cpu(*(u16 *)&hwinfo[EEPROM_PID_8723AU]);
-	}
-
-	MSG_8723A("EEPROM VID = 0x%4x\n", pHalData->EEPROMVID);
-	MSG_8723A("EEPROM PID = 0x%4x\n", pHalData->EEPROMPID);
-}
-
 static void Hal_EfuseParseMACAddr_8723AU(struct rtw_adapter *padapter,
 					 u8 *hwinfo, bool AutoLoadFail)
 {
@@ -1269,8 +1249,6 @@ static void readAdapterInfo(struct rtw_adapter *padapter)
 
 	Hal_InitPGData(padapter, hwinfo);
 	Hal_EfuseParseIDCode(padapter, hwinfo);
-	Hal_EfuseParsePIDVID_8723AU(padapter, hwinfo,
-				    pEEPROM->bautoload_fail_flag);
 	Hal_EfuseParseEEPROMVer(padapter, hwinfo,
 				pEEPROM->bautoload_fail_flag);
 	Hal_EfuseParseMACAddr_8723AU(padapter, hwinfo,
diff --git a/drivers/staging/rtl8723au/include/ieee80211.h b/drivers/staging/rtl8723au/include/ieee80211.h
index e9f465f0bc2c..cb23cd0349b4 100644
--- a/drivers/staging/rtl8723au/include/ieee80211.h
+++ b/drivers/staging/rtl8723au/include/ieee80211.h
@@ -159,10 +159,10 @@ enum NETWORK_TYPE
 #define P80211_OUI_LEN 3
 
 struct ieee80211_snap_hdr {
-        u8    dsap;   /* always 0xAA */
-        u8    ssap;   /* always 0xAA */
-        u8    ctrl;   /* always 0x03 */
-        u8    oui[P80211_OUI_LEN];    /* organizational universal id */
+	u8    dsap;   /* always 0xAA */
+	u8    ssap;   /* always 0xAA */
+	u8    ctrl;   /* always 0x03 */
+	u8    oui[P80211_OUI_LEN];    /* organizational universal id */
 } __attribute__ ((packed));
 
 
@@ -221,8 +221,8 @@ struct ieee80211_snap_hdr {
 #define IEEE80211_CCK_BASIC_RATES_MASK	(IEEE80211_CCK_RATE_1MB_MASK | \
 	IEEE80211_CCK_RATE_2MB_MASK)
 #define IEEE80211_CCK_DEFAULT_RATES_MASK	(IEEE80211_CCK_BASIC_RATES_MASK | \
-        IEEE80211_CCK_RATE_5MB_MASK | \
-        IEEE80211_CCK_RATE_11MB_MASK)
+	IEEE80211_CCK_RATE_5MB_MASK | \
+	IEEE80211_CCK_RATE_11MB_MASK)
 
 #define IEEE80211_OFDM_RATES_MASK		0x00000FF0
 #define IEEE80211_OFDM_BASIC_RATES_MASK	(IEEE80211_OFDM_RATE_6MB_MASK | \
@@ -235,7 +235,7 @@ struct ieee80211_snap_hdr {
 	IEEE80211_OFDM_RATE_48MB_MASK | \
 	IEEE80211_OFDM_RATE_54MB_MASK)
 #define IEEE80211_DEFAULT_RATES_MASK (IEEE80211_OFDM_DEFAULT_RATES_MASK | \
-                                IEEE80211_CCK_DEFAULT_RATES_MASK)
+				      IEEE80211_CCK_DEFAULT_RATES_MASK)
 
 #define IEEE80211_NUM_OFDM_RATES	    8
 #define IEEE80211_NUM_CCK_RATES	            4
@@ -332,7 +332,7 @@ u8 *rtw_get_ie23a(u8*pbuf, int index, int *len, int limit);
 u8 *rtw_get_ie23a_ex(u8 *in_ie, uint in_len, u8 eid, u8 *oui, u8 oui_len, u8 *ie, uint *ielen);
 int rtw_ies_remove_ie23a(u8 *ies, uint *ies_len, uint offset, u8 eid, u8 *oui, u8 oui_len);
 
-void rtw_set_supported_rate23a(u8* SupportedRates, uint mode) ;
+void rtw_set_supported_rate23a(u8 *SupportedRates, uint mode);
 
 int rtw_parse_wpa_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
 int rtw_parse_wpa2_ie23a(const u8* wpa_ie, int wpa_ie_len, int *group_cipher, int *pairwise_cipher, int *is_8021x);
diff --git a/drivers/staging/rtl8723au/include/odm.h b/drivers/staging/rtl8723au/include/odm.h
index 68ff7bbc6ad1..5a0561e092ac 100644
--- a/drivers/staging/rtl8723au/include/odm.h
+++ b/drivers/staging/rtl8723au/include/odm.h
@@ -138,7 +138,7 @@ struct dynamic_pwr_sav {
 	int		    Rssi_val_min;
 
 	u8		initialize;
-	u32		Reg874,RegC70,Reg85C,RegA74;
+	u32		Reg874, RegC70, Reg85C, RegA74;
 };
 
 struct false_alarm_stats {
diff --git a/drivers/staging/rtl8723au/include/odm_HWConfig.h b/drivers/staging/rtl8723au/include/odm_HWConfig.h
index 78021a8cf8f9..ce7abe770b5a 100644
--- a/drivers/staging/rtl8723au/include/odm_HWConfig.h
+++ b/drivers/staging/rtl8723au/include/odm_HWConfig.h
@@ -67,9 +67,9 @@
 
 struct phy_rx_agc_info {
 	#ifdef __LITTLE_ENDIAN
-		u8	gain:7,trsw:1;
+		u8	gain:7, trsw:1;
 	#else
-		u8	trsw:1,gain:7;
+		u8	trsw:1, gain:7;
 	#endif
 };
 
diff --git a/drivers/staging/rtl8723au/include/osdep_service.h b/drivers/staging/rtl8723au/include/osdep_service.h
index a5ebdb81ce38..dedb41874de5 100644
--- a/drivers/staging/rtl8723au/include/osdep_service.h
+++ b/drivers/staging/rtl8723au/include/osdep_service.h
@@ -54,7 +54,7 @@ struct rtw_queue {
 
 static inline struct list_head *get_list_head(struct rtw_queue *queue)
 {
-	return (&queue->queue);
+	return &queue->queue;
 }
 
 static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
@@ -62,7 +62,7 @@ static inline int rtw_netif_queue_stopped(struct net_device *pnetdev)
 	return (netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 0)) &&
 		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 1)) &&
 		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 2)) &&
-		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3)) );
+		netif_tx_queue_stopped(netdev_get_tx_queue(pnetdev, 3)));
 }
 
 static inline u32 CHKBIT(u32 x)
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_hal.h b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
index 3b0b06d6e41e..ee203a572cb7 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_hal.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_hal.h
@@ -280,10 +280,6 @@ struct hal_data_8723a {
 	/*  EEPROM setting. */
 	/*  */
 	u8	EEPROMVersion;
-	u16	EEPROMVID;
-	u16	EEPROMPID;
-	u16	EEPROMSVID;
-	u16	EEPROMSDID;
 	u8	EEPROMCustomerID;
 	u8	EEPROMSubCustomerID;
 	u8	EEPROMRegulatory;
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_rf.h b/drivers/staging/rtl8723au/include/rtl8723a_rf.h
index 166a45fe47b1..0432799f53cf 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_rf.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_rf.h
@@ -47,9 +47,9 @@
 void rtl8723a_phy_rf6052set_bw(struct rtw_adapter *Adapter,
 			       enum ht_channel_width Bandwidth);
 void rtl823a_phy_rf6052setccktxpower(struct rtw_adapter *Adapter,
-				      u8* pPowerlevel);
+				      u8 *pPowerlevel);
 void rtl8723a_PHY_RF6052SetOFDMTxPower(struct rtw_adapter *Adapter,
-				       u8* pPowerLevel, u8 Channel);
+				       u8 *pPowerLevel, u8 Channel);
 
 /*--------------------------Exported Function prototype---------------------*/
 
diff --git a/drivers/staging/rtl8723au/include/rtl8723a_spec.h b/drivers/staging/rtl8723au/include/rtl8723a_spec.h
index 59b545b642b4..2f186890dbb5 100644
--- a/drivers/staging/rtl8723au/include/rtl8723a_spec.h
+++ b/drivers/staging/rtl8723au/include/rtl8723a_spec.h
@@ -1367,9 +1367,9 @@ Current IOREG MAP
 #define _TXDMA_HIQ_MAP(x)		(((x)&0x3) << 14)
 #define _TXDMA_MGQ_MAP(x)		(((x)&0x3) << 12)
 #define _TXDMA_BKQ_MAP(x)		(((x)&0x3) << 10)
-#define _TXDMA_BEQ_MAP(x)		(((x)&0x3) << 8 )
-#define _TXDMA_VIQ_MAP(x)		(((x)&0x3) << 6 )
-#define _TXDMA_VOQ_MAP(x)		(((x)&0x3) << 4 )
+#define _TXDMA_BEQ_MAP(x)		(((x)&0x3) << 8)
+#define _TXDMA_VIQ_MAP(x)		(((x)&0x3) << 6)
+#define _TXDMA_VOQ_MAP(x)		(((x)&0x3) << 4)
 
 #define QUEUE_LOW			1
 #define QUEUE_NORMAL			2
diff --git a/drivers/staging/rtl8723au/include/rtw_cmd.h b/drivers/staging/rtl8723au/include/rtw_cmd.h
index c49237b04a33..ef67068a5fe3 100644
--- a/drivers/staging/rtl8723au/include/rtw_cmd.h
+++ b/drivers/staging/rtl8723au/include/rtw_cmd.h
@@ -547,14 +547,14 @@ struct setratable_parm {
 };
 
 struct getratable_parm {
-                uint rsvd;
+	uint rsvd;
 };
 
 struct getratable_rsp {
-        u8 ss_ForceUp[NumRates];
-        u8 ss_ULevel[NumRates];
-        u8 ss_DLevel[NumRates];
-        u8 count_judge[NumRates];
+	u8 ss_ForceUp[NumRates];
+	u8 ss_ULevel[NumRates];
+	u8 ss_DLevel[NumRates];
+	u8 count_judge[NumRates];
 };
 
 /* to get TX,RX retry count */
@@ -699,7 +699,7 @@ int rtw_setrfintfs_cmd(struct rtw_adapter  *padapter, u8 mode);
 int rtw_setrttbl_cmd(struct rtw_adapter  *padapter, struct setratable_parm *prate_table);
 int rtw_getrttbl_cmd(struct rtw_adapter  *padapter, struct getratable_rsp *pval);
 
-int rtw_gettssi_cmd(struct rtw_adapter  *padapter, u8 offset,u8 *pval);
+int rtw_gettssi_cmd(struct rtw_adapter  *padapter, u8 offset, u8 *pval);
 int rtw_setfwdig_cmd(struct rtw_adapter*padapter, u8 type);
 int rtw_setfwra_cmd(struct rtw_adapter*padapter, u8 type);
 
diff --git a/drivers/staging/rtl8723au/include/rtw_eeprom.h b/drivers/staging/rtl8723au/include/rtw_eeprom.h
index d008f032181b..e5121a2a64b4 100644
--- a/drivers/staging/rtl8723au/include/rtw_eeprom.h
+++ b/drivers/staging/rtl8723au/include/rtw_eeprom.h
@@ -128,7 +128,7 @@ struct eeprom_priv {
 void eeprom_write16(struct rtw_adapter *padapter, u16 reg, u16 data);
 u16 eeprom_read16(struct rtw_adapter *padapter, u16 reg);
 void read_eeprom_content(struct rtw_adapter *padapter);
-void eeprom_read_sz(struct rtw_adapter * padapter, u16 reg,u8* data, u32 sz);
+void eeprom_read_sz(struct rtw_adapter *padapter, u16 reg, u8 *data, u32 sz);
 
 void read_eeprom_content_by_attrib(struct rtw_adapter *padapter);
 
diff --git a/drivers/staging/rtl8723au/include/rtw_efuse.h b/drivers/staging/rtl8723au/include/rtw_efuse.h
index 07bdc3464443..c577e260f151 100644
--- a/drivers/staging/rtl8723au/include/rtw_efuse.h
+++ b/drivers/staging/rtl8723au/include/rtw_efuse.h
@@ -55,7 +55,7 @@ enum _EFUSE_DEF_TYPE {
 #define		EFUSE_MAX_HW_SIZE		512
 #define		EFUSE_MAX_SECTION_BASE	16
 
-#define EXT_HEADER(header) ((header & 0x1F ) == 0x0F)
+#define EXT_HEADER(header) ((header & 0x1F) == 0x0F)
 #define ALL_WORDS_DISABLED(wde)	((wde & 0x0F) == 0x0F)
 #define GET_HDR_OFFSET_2_0(header) ( (header & 0xE0) >> 5)
 
@@ -95,7 +95,8 @@ void	EFUSE_GetEfuseDefinition23a(struct rtw_adapter *pAdapter, u8 efuseType, u8
 int	efuse_OneByteRead23a(struct rtw_adapter *pAdapter, u16 addr, u8 *data);
 int	efuse_OneByteWrite23a(struct rtw_adapter *pAdapter, u16 addr, u8 data);
 
-void	Efuse_PowerSwitch23a(struct rtw_adapter *pAdapter,u8	bWrite,u8	 PwrState);
+void	Efuse_PowerSwitch23a(struct rtw_adapter *pAdapter, u8 bWrite,
+			     u8 PwrState);
 int	Efuse_PgPacketRead23a(struct rtw_adapter *pAdapter, u8 offset, u8 *data);
 int	Efuse_PgPacketWrite23a(struct rtw_adapter *pAdapter, u8 offset, u8 word_en, u8 *data);
 void	efuse_WordEnableDataRead23a(u8 word_en, u8 *sourdata, u8 *targetdata);
diff --git a/drivers/staging/rtl8723au/include/rtw_io.h b/drivers/staging/rtl8723au/include/rtw_io.h
index cc151b08d312..c8119e2c6545 100644
--- a/drivers/staging/rtl8723au/include/rtw_io.h
+++ b/drivers/staging/rtl8723au/include/rtw_io.h
@@ -223,15 +223,15 @@ struct reg_protocol_wt {
 
 };
 
-#define PlatformEFIOWrite1Byte(_a,_b,_c)		\
-	rtl8723au_write8(_a,_b,_c)
-#define PlatformEFIOWrite2Byte(_a,_b,_c)		\
-	rtl8723au_write16(_a,_b,_c)
-#define PlatformEFIOWrite4Byte(_a,_b,_c)		\
-	rtl8723au_write32(_a,_b,_c)
-
-#define PlatformEFIORead1Byte(_a,_b)		rtl8723au_read8(_a,_b)
-#define PlatformEFIORead2Byte(_a,_b)		rtl8723au_read16(_a,_b)
-#define PlatformEFIORead4Byte(_a,_b)		rtl8723au_read32(_a,_b)
+#define PlatformEFIOWrite1Byte(_a, _b, _c)		\
+	rtl8723au_write8(_a, _b, _c)
+#define PlatformEFIOWrite2Byte(_a, _b, _c)		\
+	rtl8723au_write16(_a, _b, _c)
+#define PlatformEFIOWrite4Byte(_a, _b, _c)		\
+	rtl8723au_write32(_a, _b, _c)
+
+#define PlatformEFIORead1Byte(_a, _b)		rtl8723au_read8(_a, _b)
+#define PlatformEFIORead2Byte(_a, _b)		rtl8723au_read16(_a, _b)
+#define PlatformEFIORead4Byte(_a, _b)		rtl8723au_read32(_a, _b)
 
 #endif	/* _RTL8711_IO_H_ */
diff --git a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
index 2e32c832fd21..97c3c4478f29 100644
--- a/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
+++ b/drivers/staging/rtl8723au/include/rtw_mlme_ext.h
@@ -266,7 +266,7 @@ struct mlme_handler {
 
 struct action_handler {
 	unsigned int   num;
-	char* str;
+	char *str;
 	int (*func)(struct rtw_adapter *padapter, struct recv_frame *precv_frame);
 };
 
@@ -395,7 +395,7 @@ struct p2p_channels {
 };
 
 struct p2p_oper_class_map {
-	enum hw_mode {IEEE80211G,IEEE80211A} mode;
+	enum hw_mode {IEEE80211G, IEEE80211A} mode;
 	u8 op_class;
 	u8 min_chan;
 	u8 max_chan;
@@ -446,7 +446,7 @@ struct mlme_ext_priv {
 	u8 active_keep_alive_check;
 };
 
-int init_mlme_ext_priv23a(struct rtw_adapter* padapter);
+int init_mlme_ext_priv23a(struct rtw_adapter *padapter);
 int init_hw_mlme_ext23a(struct rtw_adapter *padapter);
 void free_mlme_ext_priv23a (struct mlme_ext_priv *pmlmeext);
 void init_mlme_ext_timer23a(struct rtw_adapter *padapter);
@@ -458,7 +458,7 @@ u8 judge_network_type23a(struct rtw_adapter *padapter, unsigned char *rate,
 		      int ratelen);
 void get_rate_set23a(struct rtw_adapter *padapter, unsigned char *pbssrate,
 		  int *bssrate_len);
-void UpdateBrateTbl23a(struct rtw_adapter *padapter,u8 *mBratesOS);
+void UpdateBrateTbl23a(struct rtw_adapter *padapter, u8 *mBratesOS);
 void Update23aTblForSoftAP(u8 *bssrateset, u32 bssratelen);
 
 u8 rtw_get_oper_ch23a(struct rtw_adapter *adapter);
@@ -514,7 +514,7 @@ void update_capinfo23a(struct rtw_adapter *Adapter, u16 updateCap);
 void update_wireless_mode23a(struct rtw_adapter * padapter);
 void update_tx_basic_rate23a(struct rtw_adapter *padapter, u8 modulation);
 void update_bmc_sta_support_rate23a(struct rtw_adapter *padapter, u32 mac_id);
-int update_sta_support_rate23a(struct rtw_adapter *padapter, u8* pvar_ie,
+int update_sta_support_rate23a(struct rtw_adapter *padapter, u8 *pvar_ie,
 			    uint var_ie_len, int cam_idx);
 
 /* for sta/adhoc mode */
diff --git a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
index 93dc844a10b3..bd6953af0a03 100644
--- a/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
+++ b/drivers/staging/rtl8723au/os_dep/ioctl_cfg80211.c
@@ -279,6 +279,7 @@ static int rtw_cfg80211_inform_bss(struct rtw_adapter *padapter,
 	}
 
 	bss = cfg80211_inform_bss(wiphy, notify_channel,
+				  CFG80211_BSS_FTYPE_UNKNOWN,
 				  pnetwork->network.MacAddress,
 				  pnetwork->network.tsf,
 				  pnetwork->network.capability,
@@ -1118,7 +1119,7 @@ exit:
 	return ret;
 }
 
-int cfg80211_infrastructure_mode(struct rtw_adapter* padapter,
+static int cfg80211_infrastructure_mode(struct rtw_adapter *padapter,
 				 enum nl80211_iftype ifmode)
 {
 	struct mlme_priv *pmlmepriv = &padapter->mlmepriv;
@@ -2379,7 +2380,7 @@ void rtw_cfg80211_indicate_sta_assoc(struct rtw_adapter *padapter,
 						      IEEE80211_BAND_5GHZ);
 
 	cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, pmgmt_frame, frame_len,
-			 0, GFP_ATOMIC);
+			 0);
 #endif /* defined(RTW_USE_CFG80211_STA_EVENT) */
 }
 
@@ -2425,7 +2426,7 @@ void rtw_cfg80211_indicate_sta_disassoc(struct rtw_adapter *padapter,
 	frame_len = sizeof(struct ieee80211_hdr_3addr) + 2;
 
 	cfg80211_rx_mgmt(padapter->rtw_wdev, freq, 0, (u8 *)&mgmt, frame_len,
-			 0, GFP_ATOMIC);
+			 0);
 #endif /* defined(RTW_USE_CFG80211_STA_EVENT) */
 }
 
@@ -3168,13 +3169,13 @@ static void rtw_cfg80211_init_ht_capab(struct ieee80211_sta_ht_cap *ht_cap,
 		ht_cap->mcs.rx_mask[1] = 0x00;
 		ht_cap->mcs.rx_mask[4] = 0x01;
 
-		ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS7;
+		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS7);
 	} else if ((rf_type == RF_1T2R) || (rf_type == RF_2T2R)) {
 		ht_cap->mcs.rx_mask[0] = 0xFF;
 		ht_cap->mcs.rx_mask[1] = 0xFF;
 		ht_cap->mcs.rx_mask[4] = 0x01;
 
-		ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS15;
+		ht_cap->mcs.rx_highest = cpu_to_le16(MAX_BIT_RATE_40MHZ_MCS15);
 	} else {
 		DBG_8723A("%s, error rf_type =%d\n", __func__, rf_type);
 	}
diff --git a/drivers/staging/rtl8821ae/Kconfig b/drivers/staging/rtl8821ae/Kconfig
deleted file mode 100644
index 1a89b25647ed..000000000000
--- a/drivers/staging/rtl8821ae/Kconfig
+++ /dev/null
@@ -1,10 +0,0 @@
-config R8821AE
-	tristate "RealTek RTL8821AE Wireless LAN NIC driver"
-	depends on PCI && WLAN && MAC80211
-	depends on m
-	select WIRELESS_EXT
-	select WEXT_PRIV
-	select EEPROM_93CX6
-	select CRYPTO
-	---help---
-	  If built as a module, it will be called r8821ae.ko.
diff --git a/drivers/staging/rtl8821ae/Makefile b/drivers/staging/rtl8821ae/Makefile
deleted file mode 100644
index 8a23bd7e8842..000000000000
--- a/drivers/staging/rtl8821ae/Makefile
+++ /dev/null
@@ -1,35 +0,0 @@
-PCI_MAIN_OBJS	:= base.o	\
-		rc.o	\
-		debug.o	\
-		regd.o	\
-		efuse.o	\
-		cam.o	\
-		ps.o	\
-		core.o	\
-		stats.o	\
-		pci.o	\
-
-BT_COEXIST_OBJS:=	btcoexist/halbtc8192e2ant.o\
-			btcoexist/halbtc8723b1ant.o\
-			btcoexist/halbtc8723b2ant.o\
-			btcoexist/halbtcoutsrc.o\
-			btcoexist/rtl_btc.o	\
-
-PCI_8821AE_HAL_OBJS:=	\
-	rtl8821ae/hw.o		\
-	rtl8821ae/table.o		\
-	rtl8821ae/sw.o		\
-	rtl8821ae/trx.o		\
-	rtl8821ae/led.o		\
-	rtl8821ae/fw.o		\
-	rtl8821ae/phy.o		\
-	rtl8821ae/rf.o		\
-	rtl8821ae/dm.o		\
-	rtl8821ae/pwrseq.o	\
-	rtl8821ae/pwrseqcmd.o	\
-	rtl8821ae/hal_btc.o	\
-	rtl8821ae/hal_bt_coexist.o	\
-
-rtl8821ae-objs += $(BT_COEXIST_OBJS) $(PCI_MAIN_OBJS) $(PCI_8821AE_HAL_OBJS)
-
-obj-$(CONFIG_R8821AE) += rtl8821ae.o
diff --git a/drivers/staging/rtl8821ae/TODO b/drivers/staging/rtl8821ae/TODO
deleted file mode 100644
index 3ee7529d4ed5..000000000000
--- a/drivers/staging/rtl8821ae/TODO
+++ /dev/null
@@ -1,10 +0,0 @@
-Realtek 8821AE PCI wifi driver TODO:
-  - remove built-in btcoexist module when the "real" one gets upstream
-  - remove built-in rtlwifi code by porting driver to use the "real" one
-    in the drivers/net/ directory.
-  - fix up coding style issues
-
-Please send any patches for this driver to:
-	Greg Kroah-Hartman <gregkh@linuxfoundation.org>
-and the <devel@driverdev.osuosl.org> mailing list.
-
diff --git a/drivers/staging/rtl8821ae/base.c b/drivers/staging/rtl8821ae/base.c
deleted file mode 100644
index 4a36da06c01c..000000000000
--- a/drivers/staging/rtl8821ae/base.c
+++ /dev/null
@@ -1,1831 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include <linux/ip.h>
-#include <linux/module.h>
-#include "wifi.h"
-#include "rc.h"
-#include "base.h"
-#include "efuse.h"
-#include "cam.h"
-#include "ps.h"
-#include "regd.h"
-#include "pci.h"
-
-/*
- *NOTICE!!!: This file will be very big, we should
- *keep it clear under following roles:
- *
- *This file include following part, so, if you add new
- *functions into this file, please check which part it
- *should includes. or check if you should add new part
- *for this file:
- *
- *1) mac80211 init functions
- *2) tx information functions
- *3) functions called by core.c
- *4) wq & timer callback functions
- *5) frame process functions
- *6) IOT functions
- *7) sysfs functions
- *8) vif functions
- *9) ...
- */
-
-/*********************************************************
- *
- * mac80211 init functions
- *
- *********************************************************/
-static struct ieee80211_channel rtl_channeltable_2g[] = {
-	{.center_freq = 2412, .hw_value = 1,},
-	{.center_freq = 2417, .hw_value = 2,},
-	{.center_freq = 2422, .hw_value = 3,},
-	{.center_freq = 2427, .hw_value = 4,},
-	{.center_freq = 2432, .hw_value = 5,},
-	{.center_freq = 2437, .hw_value = 6,},
-	{.center_freq = 2442, .hw_value = 7,},
-	{.center_freq = 2447, .hw_value = 8,},
-	{.center_freq = 2452, .hw_value = 9,},
-	{.center_freq = 2457, .hw_value = 10,},
-	{.center_freq = 2462, .hw_value = 11,},
-	{.center_freq = 2467, .hw_value = 12,},
-	{.center_freq = 2472, .hw_value = 13,},
-	{.center_freq = 2484, .hw_value = 14,},
-};
-
-static struct ieee80211_channel rtl_channeltable_5g[] = {
-	{.center_freq = 5180, .hw_value = 36,},
-	{.center_freq = 5200, .hw_value = 40,},
-	{.center_freq = 5220, .hw_value = 44,},
-	{.center_freq = 5240, .hw_value = 48,},
-	{.center_freq = 5260, .hw_value = 52,},
-	{.center_freq = 5280, .hw_value = 56,},
-	{.center_freq = 5300, .hw_value = 60,},
-	{.center_freq = 5320, .hw_value = 64,},
-	{.center_freq = 5500, .hw_value = 100,},
-	{.center_freq = 5520, .hw_value = 104,},
-	{.center_freq = 5540, .hw_value = 108,},
-	{.center_freq = 5560, .hw_value = 112,},
-	{.center_freq = 5580, .hw_value = 116,},
-	{.center_freq = 5600, .hw_value = 120,},
-	{.center_freq = 5620, .hw_value = 124,},
-	{.center_freq = 5640, .hw_value = 128,},
-	{.center_freq = 5660, .hw_value = 132,},
-	{.center_freq = 5680, .hw_value = 136,},
-	{.center_freq = 5700, .hw_value = 140,},
-	{.center_freq = 5745, .hw_value = 149,},
-	{.center_freq = 5765, .hw_value = 153,},
-	{.center_freq = 5785, .hw_value = 157,},
-	{.center_freq = 5805, .hw_value = 161,},
-	{.center_freq = 5825, .hw_value = 165,},
-};
-
-static struct ieee80211_rate rtl_ratetable_2g[] = {
-	{.bitrate = 10, .hw_value = 0x00,},
-	{.bitrate = 20, .hw_value = 0x01,},
-	{.bitrate = 55, .hw_value = 0x02,},
-	{.bitrate = 110, .hw_value = 0x03,},
-	{.bitrate = 60, .hw_value = 0x04,},
-	{.bitrate = 90, .hw_value = 0x05,},
-	{.bitrate = 120, .hw_value = 0x06,},
-	{.bitrate = 180, .hw_value = 0x07,},
-	{.bitrate = 240, .hw_value = 0x08,},
-	{.bitrate = 360, .hw_value = 0x09,},
-	{.bitrate = 480, .hw_value = 0x0a,},
-	{.bitrate = 540, .hw_value = 0x0b,},
-};
-
-static struct ieee80211_rate rtl_ratetable_5g[] = {
-	{.bitrate = 60, .hw_value = 0x04,},
-	{.bitrate = 90, .hw_value = 0x05,},
-	{.bitrate = 120, .hw_value = 0x06,},
-	{.bitrate = 180, .hw_value = 0x07,},
-	{.bitrate = 240, .hw_value = 0x08,},
-	{.bitrate = 360, .hw_value = 0x09,},
-	{.bitrate = 480, .hw_value = 0x0a,},
-	{.bitrate = 540, .hw_value = 0x0b,},
-};
-
-static const struct ieee80211_supported_band rtl_band_2ghz = {
-	.band = IEEE80211_BAND_2GHZ,
-
-	.channels = rtl_channeltable_2g,
-	.n_channels = ARRAY_SIZE(rtl_channeltable_2g),
-
-	.bitrates = rtl_ratetable_2g,
-	.n_bitrates = ARRAY_SIZE(rtl_ratetable_2g),
-
-	.ht_cap = {0},
-};
-
-static struct ieee80211_supported_band rtl_band_5ghz = {
-	.band = IEEE80211_BAND_5GHZ,
-
-	.channels = rtl_channeltable_5g,
-	.n_channels = ARRAY_SIZE(rtl_channeltable_5g),
-
-	.bitrates = rtl_ratetable_5g,
-	.n_bitrates = ARRAY_SIZE(rtl_ratetable_5g),
-
-	.ht_cap = {0},
-};
-
-static const u8 tid_to_ac[] = {
-	2, /* IEEE80211_AC_BE */
-	3, /* IEEE80211_AC_BK */
-	3, /* IEEE80211_AC_BK */
-	2, /* IEEE80211_AC_BE */
-	1, /* IEEE80211_AC_VI */
-	1, /* IEEE80211_AC_VI */
-	0, /* IEEE80211_AC_VO */
-	0, /* IEEE80211_AC_VO */
-};
-
-u8 rtl_tid_to_ac(struct ieee80211_hw *hw, u8 tid)
-{
-	return tid_to_ac[tid];
-}
-
-static void _rtl_init_hw_ht_capab(struct ieee80211_hw *hw,
-				  struct ieee80211_sta_ht_cap *ht_cap)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	ht_cap->ht_supported = true;
-	ht_cap->cap = IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
-	    IEEE80211_HT_CAP_SGI_40 |
-	    IEEE80211_HT_CAP_SGI_20 |
-	    IEEE80211_HT_CAP_DSSSCCK40 | IEEE80211_HT_CAP_MAX_AMSDU;
-
-	if (rtlpriv->rtlhal.disable_amsdu_8k)
-		ht_cap->cap &= ~IEEE80211_HT_CAP_MAX_AMSDU;
-
-	/*
-	 *Maximum length of AMPDU that the STA can receive.
-	 *Length = 2 ^ (13 + max_ampdu_length_exp) - 1 (octets)
-	 */
-	ht_cap->ampdu_factor = IEEE80211_HT_MAX_AMPDU_64K;
-
-	/*Minimum MPDU start spacing , */
-	ht_cap->ampdu_density = IEEE80211_HT_MPDU_DENSITY_16;
-
-	ht_cap->mcs.tx_params = IEEE80211_HT_MCS_TX_DEFINED;
-
-	/*
-	 *hw->wiphy->bands[IEEE80211_BAND_2GHZ]
-	 *base on ant_num
-	 *rx_mask: RX mask
-	 *if rx_ant =1 rx_mask[0]=0xff;==>MCS0-MCS7
-	 *if rx_ant =2 rx_mask[1]=0xff;==>MCS8-MCS15
-	 *if rx_ant >=3 rx_mask[2]=0xff;
-	 *if BW_40 rx_mask[4]=0x01;
-	 *highest supported RX rate
-	 */
-	if (rtlpriv->dm.supp_phymode_switch) {
-		RT_TRACE(COMP_INIT, DBG_EMERG, ("Support phy mode switch\n"));
-
-		ht_cap->mcs.rx_mask[0] = 0xFF;
-		ht_cap->mcs.rx_mask[1] = 0xFF;
-		ht_cap->mcs.rx_mask[4] = 0x01;
-
-		ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS15;
-	} else {
-		if (get_rf_type(rtlphy) == RF_1T2R ||
-				get_rf_type(rtlphy) == RF_2T2R) {
-
-			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T2R or 2T2R\n"));
-
-			ht_cap->mcs.rx_mask[0] = 0xFF;
-			ht_cap->mcs.rx_mask[1] = 0xFF;
-			ht_cap->mcs.rx_mask[4] = 0x01;
-
-			ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS15;
-		} else if (get_rf_type(rtlphy) == RF_1T1R) {
-
-			RT_TRACE(COMP_INIT, DBG_DMESG, ("1T1R\n"));
-
-			ht_cap->mcs.rx_mask[0] = 0xFF;
-			ht_cap->mcs.rx_mask[1] = 0x00;
-			ht_cap->mcs.rx_mask[4] = 0x01;
-
-			ht_cap->mcs.rx_highest = MAX_BIT_RATE_40MHZ_MCS7;
-		}
-	}
-}
-
-static void _rtl_init_mac80211(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct ieee80211_supported_band *sband;
-
-
-	if (rtlhal->macphymode == SINGLEMAC_SINGLEPHY &&
-	    rtlhal->bandset == BAND_ON_BOTH) {
-		/* 1: 2.4 G bands */
-		/* <1> use  mac->bands as mem for hw->wiphy->bands */
-		sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
-
-		/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
-		 * to default value(1T1R) */
-		memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]), &rtl_band_2ghz,
-				sizeof(struct ieee80211_supported_band));
-
-		/* <3> init ht cap base on ant_num */
-		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-		/* <4> set mac->sband to wiphy->sband */
-		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
-
-		/* 2: 5 G bands */
-		/* <1> use  mac->bands as mem for hw->wiphy->bands */
-		sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
-
-		/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
-		 * to default value(1T1R) */
-		memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]), &rtl_band_5ghz,
-				sizeof(struct ieee80211_supported_band));
-
-		/* <3> init ht cap base on ant_num */
-		_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-		/* <4> set mac->sband to wiphy->sband */
-		hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
-	} else {
-		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-			/* <1> use  mac->bands as mem for hw->wiphy->bands */
-			sband = &(rtlmac->bands[IEEE80211_BAND_2GHZ]);
-
-			/* <2> set hw->wiphy->bands[IEEE80211_BAND_2GHZ]
-			 * to default value(1T1R) */
-			memcpy(&(rtlmac->bands[IEEE80211_BAND_2GHZ]),
-			       &rtl_band_2ghz,
-			       sizeof(struct ieee80211_supported_band));
-
-			/* <3> init ht cap base on ant_num */
-			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-			/* <4> set mac->sband to wiphy->sband */
-			hw->wiphy->bands[IEEE80211_BAND_2GHZ] = sband;
-		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
-			/* <1> use  mac->bands as mem for hw->wiphy->bands */
-			sband = &(rtlmac->bands[IEEE80211_BAND_5GHZ]);
-
-			/* <2> set hw->wiphy->bands[IEEE80211_BAND_5GHZ]
-			 * to default value(1T1R) */
-			memcpy(&(rtlmac->bands[IEEE80211_BAND_5GHZ]),
-			       &rtl_band_5ghz,
-			       sizeof(struct ieee80211_supported_band));
-
-			/* <3> init ht cap base on ant_num */
-			_rtl_init_hw_ht_capab(hw, &sband->ht_cap);
-
-			/* <4> set mac->sband to wiphy->sband */
-			hw->wiphy->bands[IEEE80211_BAND_5GHZ] = sband;
-		} else {
-			RT_TRACE(COMP_INIT, DBG_EMERG, ("Err BAND %d\n",
-				 rtlhal->current_bandtype));
-		}
-	}
-	/* <5> set hw caps */
-	hw->flags = IEEE80211_HW_SIGNAL_DBM |
-	    IEEE80211_HW_RX_INCLUDES_FCS |
-	    IEEE80211_HW_AMPDU_AGGREGATION |
-	    IEEE80211_HW_REPORTS_TX_ACK_STATUS |
-	    IEEE80211_HW_CONNECTION_MONITOR |
-	    /* IEEE80211_HW_SUPPORTS_CQM_RSSI | */
-	    IEEE80211_HW_MFP_CAPABLE | 0;
-
-	/* swlps or hwlps has been set in diff chip in init_sw_vars */
-	if (rtlpriv->psc.b_swctrl_lps)
-		hw->flags |= IEEE80211_HW_SUPPORTS_PS |
-			IEEE80211_HW_PS_NULLFUNC_STACK |
-			/* IEEE80211_HW_SUPPORTS_DYNAMIC_PS | */
-			0;
-	hw->wiphy->interface_modes =
-	    BIT(NL80211_IFTYPE_AP) |
-	    BIT(NL80211_IFTYPE_STATION) |
-	    BIT(NL80211_IFTYPE_ADHOC) |
-	    BIT(NL80211_IFTYPE_MESH_POINT) |
-	    BIT(NL80211_IFTYPE_P2P_CLIENT) |
-	    BIT(NL80211_IFTYPE_P2P_GO);
-
-	hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN;
-
-	hw->wiphy->flags |= WIPHY_FLAG_HAS_REMAIN_ON_CHANNEL;
-
-	hw->wiphy->rts_threshold = 2347;
-
-	hw->queues = AC_MAX;
-	hw->extra_tx_headroom = RTL_TX_HEADER_SIZE;
-
-	/* TODO: Correct this value for our hw */
-	/* TODO: define these hard code value */
-	hw->max_listen_interval = 10;
-	hw->max_rate_tries = 4;
-	/* hw->max_rates = 1; */
-	hw->sta_data_size = sizeof(struct rtl_sta_info);
-#ifdef VIF_TODO
-	hw->vif_data_size = sizeof(struct rtl_vif_info);
-#endif
-
-	/* <6> mac address */
-	if (is_valid_ether_addr(rtlefuse->dev_addr)) {
-		SET_IEEE80211_PERM_ADDR(hw, rtlefuse->dev_addr);
-	} else {
-		u8 rtlmac[] = { 0x00, 0xe0, 0x4c, 0x81, 0x92, 0x00 };
-		get_random_bytes((rtlmac + (ETH_ALEN - 1)), 1);
-		SET_IEEE80211_PERM_ADDR(hw, rtlmac);
-	}
-
-}
-
-static int _rtl_init_deferred_work(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	/* <1> timer */
-	init_timer(&rtlpriv->works.watchdog_timer);
-	setup_timer(&rtlpriv->works.watchdog_timer,
-		    rtl_watch_dog_timer_callback, (unsigned long)hw);
-	init_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer);
-	setup_timer(&rtlpriv->works.dualmac_easyconcurrent_retrytimer,
-		    rtl_easy_concurrent_retrytimer_callback, (unsigned long)hw);
-	/* <2> work queue */
-	rtlpriv->works.hw = hw;
-	rtlpriv->works.rtl_wq = alloc_workqueue("%s", 0, 0,
-						rtlpriv->cfg->name);
-	if (!rtlpriv->works.rtl_wq)
-		return -ENOMEM;
-
-	INIT_DELAYED_WORK(&rtlpriv->works.watchdog_wq,
-			  (void *)rtl_watchdog_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.ips_nic_off_wq,
-			  (void *)rtl_ips_nic_off_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.ps_work,
-			  (void *)rtl_swlps_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.ps_rfon_wq,
-			  (void *)rtl_swlps_rfon_wq_callback);
-	INIT_DELAYED_WORK(&rtlpriv->works.fwevt_wq,
-			  (void *)rtl_fwevt_wq_callback);
-
-	return 0;
-
-}
-
-void rtl_deinit_deferred_work(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	del_timer_sync(&rtlpriv->works.watchdog_timer);
-
-	cancel_delayed_work(&rtlpriv->works.watchdog_wq);
-	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
-	cancel_delayed_work(&rtlpriv->works.ps_work);
-	cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
-	cancel_delayed_work(&rtlpriv->works.fwevt_wq);
-}
-
-void rtl_init_rfkill(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	bool radio_state;
-	bool blocked;
-	u8 valid = 0;
-
-	/*set init state to on */
-	rtlpriv->rfkill.rfkill_state = 1;
-	wiphy_rfkill_set_hw_state(hw->wiphy, 0);
-
-	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
-
-	if (valid) {
-		printk(KERN_INFO "rtlwifi: wireless switch is %s\n",
-		       rtlpriv->rfkill.rfkill_state ? "on" : "off");
-
-		rtlpriv->rfkill.rfkill_state = radio_state;
-
-		blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
-		wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
-	}
-
-	wiphy_rfkill_start_polling(hw->wiphy);
-}
-
-void rtl_deinit_rfkill(struct ieee80211_hw *hw)
-{
-	wiphy_rfkill_stop_polling(hw->wiphy);
-}
-
-#ifdef VIF_TODO
-static void rtl_init_vif(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	INIT_LIST_HEAD(&rtlpriv->vif_priv.vif_list);
-
-	rtlpriv->vif_priv.vifs = 0;
-}
-#endif
-
-int rtl_init_core(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
-
-	/* <1> init mac80211 */
-	_rtl_init_mac80211(hw);
-	rtlmac->hw = hw;
-	rtlmac->link_state = MAC80211_NOLINK;
-
-	/* <2> rate control register */
-	hw->rate_control_algorithm = "rtl_rc";
-
-	/*
-	 * <3> init CRDA must come after init
-	 * mac80211 hw  in _rtl_init_mac80211.
-	 */
-	if (rtl_regd_init(hw, rtl_reg_notifier)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("REGD init failed\n"));
-		return 1;
-	}
-
-	/* <4> locks */
-	mutex_init(&rtlpriv->locks.conf_mutex);
-	spin_lock_init(&rtlpriv->locks.ips_lock);
-	spin_lock_init(&rtlpriv->locks.irq_th_lock);
-	spin_lock_init(&rtlpriv->locks.h2c_lock);
-	spin_lock_init(&rtlpriv->locks.rf_ps_lock);
-	spin_lock_init(&rtlpriv->locks.rf_lock);
-	spin_lock_init(&rtlpriv->locks.lps_lock);
-	spin_lock_init(&rtlpriv->locks.waitq_lock);
-	spin_lock_init(&rtlpriv->locks.entry_list_lock);
-	spin_lock_init(&rtlpriv->locks.cck_and_rw_pagea_lock);
-	spin_lock_init(&rtlpriv->locks.check_sendpkt_lock);
-	spin_lock_init(&rtlpriv->locks.fw_ps_lock);
-	spin_lock_init(&rtlpriv->locks.iqk_lock);
-	/* <5> init list */
-	INIT_LIST_HEAD(&rtlpriv->entry_list);
-
-	/* <6> init deferred work */
-	if (_rtl_init_deferred_work(hw))
-		return 1;
-
-	/* <7> */
-#ifdef VIF_TODO
-	rtl_init_vif(hw);
-#endif
-
-	return 0;
-}
-
-void rtl_deinit_core(struct ieee80211_hw *hw)
-{
-}
-
-void rtl_init_rx_config(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_RCR, (u8 *) (&mac->rx_conf));
-}
-
-/*********************************************************
- *
- * tx information functions
- *
- *********************************************************/
-static void _rtl_qurey_shortpreamble_mode(struct ieee80211_hw *hw,
-					  struct rtl_tcb_desc *tcb_desc,
-					  struct ieee80211_tx_info *info)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 rate_flag = info->control.rates[0].flags;
-
-	tcb_desc->use_shortpreamble = false;
-
-	/* 1M can only use Long Preamble. 11B spec */
-	if (tcb_desc->hw_rate == rtlpriv->cfg->maps[RTL_RC_CCK_RATE1M])
-		return;
-	else if (rate_flag & IEEE80211_TX_RC_USE_SHORT_PREAMBLE)
-		tcb_desc->use_shortpreamble = true;
-
-	return;
-}
-
-static void _rtl_query_shortgi(struct ieee80211_hw *hw,
-			       struct ieee80211_sta *sta,
-			       struct rtl_tcb_desc *tcb_desc,
-			       struct ieee80211_tx_info *info)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u8 rate_flag = info->control.rates[0].flags;
-	u8 sgi_40 = 0, sgi_20 = 0, bw_40 = 0;
-	tcb_desc->use_shortgi = false;
-
-	if (sta == NULL)
-		return;
-
-	sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
-	sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
-
-	if (!(sta->ht_cap.ht_supported))
-		return;
-
-	if (!sgi_40 && !sgi_20)
-		return;
-
-	if (mac->opmode == NL80211_IFTYPE_STATION)
-		bw_40 = mac->bw_40;
-	else if (mac->opmode == NL80211_IFTYPE_AP ||
-		 mac->opmode == NL80211_IFTYPE_ADHOC ||
-		 mac->opmode == NL80211_IFTYPE_MESH_POINT)
-		bw_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40;
-
-	if ((bw_40 == true) && sgi_40)
-		tcb_desc->use_shortgi = true;
-	else if ((bw_40 == false) && sgi_20)
-		tcb_desc->use_shortgi = true;
-
-	if (!(rate_flag & IEEE80211_TX_RC_SHORT_GI))
-		tcb_desc->use_shortgi = false;
-}
-
-static void _rtl_query_protection_mode(struct ieee80211_hw *hw,
-				       struct rtl_tcb_desc *tcb_desc,
-				       struct ieee80211_tx_info *info)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 rate_flag = info->control.rates[0].flags;
-
-	/* Common Settings */
-	tcb_desc->b_rts_stbc = false;
-	tcb_desc->b_cts_enable = false;
-	tcb_desc->rts_sc = 0;
-	tcb_desc->b_rts_bw = false;
-	tcb_desc->b_rts_use_shortpreamble = false;
-	tcb_desc->b_rts_use_shortgi = false;
-
-	if (rate_flag & IEEE80211_TX_RC_USE_CTS_PROTECT) {
-		/* Use CTS-to-SELF in protection mode. */
-		tcb_desc->b_rts_enable = true;
-		tcb_desc->b_cts_enable = true;
-		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
-	} else if (rate_flag & IEEE80211_TX_RC_USE_RTS_CTS) {
-		/* Use RTS-CTS in protection mode. */
-		tcb_desc->b_rts_enable = true;
-		tcb_desc->rts_rate = rtlpriv->cfg->maps[RTL_RC_OFDM_RATE24M];
-	}
-}
-
-static void _rtl_txrate_selectmode(struct ieee80211_hw *hw,
-				   struct ieee80211_sta *sta,
-				   struct rtl_tcb_desc *tcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_sta_info *sta_entry = NULL;
-	u8 ratr_index = 7;
-
-	if (sta) {
-		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-		ratr_index = sta_entry->ratr_index;
-	}
-	if (!tcb_desc->disable_ratefallback || !tcb_desc->use_driver_rate) {
-		if (mac->opmode == NL80211_IFTYPE_STATION) {
-			tcb_desc->ratr_index = 0;
-		} else if (mac->opmode == NL80211_IFTYPE_ADHOC ||
-				mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-			if (tcb_desc->b_multicast || tcb_desc->b_broadcast) {
-				tcb_desc->hw_rate =
-				    rtlpriv->cfg->maps[RTL_RC_CCK_RATE2M];
-				tcb_desc->use_driver_rate = 1;
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
-			} else {
-				tcb_desc->ratr_index = ratr_index;
-			}
-		} else if (mac->opmode == NL80211_IFTYPE_AP) {
-			tcb_desc->ratr_index = ratr_index;
-		}
-	}
-
-	if (rtlpriv->dm.b_useramask) {
-		tcb_desc->ratr_index = ratr_index;
-		/* TODO we will differentiate adhoc and station future  */
-		if (mac->opmode == NL80211_IFTYPE_STATION ||
-			mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-			tcb_desc->mac_id = 0;
-			if (mac->mode == WIRELESS_MODE_N_24G)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_NGB;
-			else if (mac->mode == WIRELESS_MODE_N_5G)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_NG;
-			else if (mac->mode & WIRELESS_MODE_G)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_GB;
-			else if (mac->mode & WIRELESS_MODE_B)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_B;
-			else if (mac->mode & WIRELESS_MODE_A)
-				tcb_desc->ratr_index = RATR_INX_WIRELESS_G;
-		} else if (mac->opmode == NL80211_IFTYPE_AP ||
-			mac->opmode == NL80211_IFTYPE_ADHOC) {
-			if (NULL != sta) {
-				if (sta->aid > 0)
-					tcb_desc->mac_id = sta->aid + 1;
-				else
-					tcb_desc->mac_id = 1;
-			} else {
-				tcb_desc->mac_id = 0;
-			}
-		}
-	}
-}
-
-static void _rtl_query_bandwidth_mode(struct ieee80211_hw *hw,
-				      struct ieee80211_sta *sta,
-				      struct rtl_tcb_desc *tcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	tcb_desc->b_packet_bw = false;
-	if (!sta)
-		return;
-	if (mac->opmode == NL80211_IFTYPE_AP ||
-		mac->opmode == NL80211_IFTYPE_ADHOC ||
-		mac->opmode == NL80211_IFTYPE_MESH_POINT) {
-		if (!(sta->ht_cap.ht_supported) ||
-			!(sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40))
-			return;
-	} else if (mac->opmode == NL80211_IFTYPE_STATION) {
-		if (!mac->bw_40 || !(sta->ht_cap.ht_supported))
-			return;
-	}
-	if (tcb_desc->b_multicast || tcb_desc->b_broadcast)
-		return;
-
-	/*use legency rate, shall use 20MHz */
-	if (tcb_desc->hw_rate <= rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M])
-		return;
-
-	tcb_desc->b_packet_bw = true;
-}
-
-static u8 _rtl_get_highest_n_rate(struct ieee80211_hw *hw,
-				  struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 hw_rate;
-
-	if ((get_rf_type(rtlphy) == RF_2T2R) && (sta->ht_cap.mcs.rx_mask[1] != 0))
-		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS15];
-	else
-		hw_rate = rtlpriv->cfg->maps[RTL_RC_HT_RATEMCS7];
-
-	return hw_rate;
-}
-
-void rtl_get_tcb_desc(struct ieee80211_hw *hw,
-		      struct ieee80211_tx_info *info,
-		      struct ieee80211_sta *sta,
-		      struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *rtlmac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	struct ieee80211_rate *txrate;
-	u16 fc = rtl_get_fc(skb);
-
-	txrate = ieee80211_get_tx_rate(hw, info);
-	if (txrate != NULL)
-		tcb_desc->hw_rate = txrate->hw_value;
-
-	if (ieee80211_is_data(fc)) {
-		/*
-		 *we set data rate INX 0
-		 *in rtl_rc.c   if skb is special data or
-		 *mgt which need low data rate.
-		 */
-
-		/*
-		 *So tcb_desc->hw_rate is just used for
-		 *special data and mgt frames
-		 */
-		if (info->control.rates[0].idx == 0 ||
-				ieee80211_is_nullfunc(fc)) {
-			tcb_desc->use_driver_rate = true;
-			tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
-
-			tcb_desc->disable_ratefallback = 1;
-		} else {
-			/*
-			 *because hw will never use hw_rate
-			 *when tcb_desc->use_driver_rate = false
-			 *so we never set highest N rate here,
-			 *and N rate will all be controlled by FW
-			 *when tcb_desc->use_driver_rate = false
-			 */
-			if (sta && (sta->ht_cap.ht_supported)) {
-				tcb_desc->hw_rate = _rtl_get_highest_n_rate(hw, sta);
-			} else {
-				if (rtlmac->mode == WIRELESS_MODE_B) {
-					tcb_desc->hw_rate =
-					    rtlpriv->cfg->maps[RTL_RC_CCK_RATE11M];
-				} else {
-					tcb_desc->hw_rate =
-					    rtlpriv->cfg->maps[RTL_RC_OFDM_RATE54M];
-				}
-			}
-		}
-
-		if (is_multicast_ether_addr(ieee80211_get_DA(hdr)))
-			tcb_desc->b_multicast = 1;
-		else if (is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
-			tcb_desc->b_broadcast = 1;
-
-		_rtl_txrate_selectmode(hw, sta, tcb_desc);
-		_rtl_query_bandwidth_mode(hw, sta, tcb_desc);
-		_rtl_qurey_shortpreamble_mode(hw, tcb_desc, info);
-		_rtl_query_shortgi(hw, sta, tcb_desc, info);
-		_rtl_query_protection_mode(hw, tcb_desc, info);
-	} else {
-		tcb_desc->use_driver_rate = true;
-		tcb_desc->ratr_index = RATR_INX_WIRELESS_MC;
-		tcb_desc->disable_ratefallback = 1;
-		tcb_desc->mac_id = 0;
-		tcb_desc->b_packet_bw = false;
-	}
-}
-/* EXPORT_SYMBOL(rtl_get_tcb_desc); */
-
-bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 fc = rtl_get_fc(skb);
-
-	if (rtlpriv->dm.supp_phymode_switch &&
-		mac->link_state < MAC80211_LINKED &&
-		(ieee80211_is_auth(fc) || ieee80211_is_probe_req(fc))) {
-		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-	}
-	if (ieee80211_is_auth(fc)) {
-		RT_TRACE(COMP_SEND, DBG_DMESG, ("MAC80211_LINKING\n"));
-		rtl_ips_nic_on(hw);
-
-		mac->link_state = MAC80211_LINKING;
-		/* Dul mac */
-		rtlpriv->phy.b_need_iqk = true;
-
-	}
-
-	return true;
-}
-
-struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw, u8 *sa,
-				u8 *bssid, u16 tid);
-bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 fc = rtl_get_fc(skb);
-	u8 *act = (u8 *) (((u8 *) skb->data + MAC80211_3ADDR_LEN));
-	u8 category;
-
-	if (!ieee80211_is_action(fc))
-		return true;
-
-	category = *act;
-	act++;
-	switch (category) {
-	case ACT_CAT_BA:
-		switch (*act) {
-		case ACT_ADDBAREQ:
-			if (mac->act_scanning)
-				return false;
-
-			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-				 ("%s ACT_ADDBAREQ From :%pM\n",
-				  is_tx ? "Tx" : "Rx", hdr->addr2));
-			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("req\n"),
-			skb->data, skb->len);
-			if (!is_tx) {
-				struct ieee80211_sta *sta = NULL;
-				struct rtl_sta_info *sta_entry = NULL;
-				struct ieee80211_mgmt *mgmt = (void *)skb->data;
-				u16 capab = 0, tid = 0;
-				struct rtl_tid_data *tid_data;
-				struct sk_buff *skb_delba = NULL;
-				struct ieee80211_rx_status rx_status = { 0 };
-
-				rcu_read_lock();
-				sta = rtl_find_sta(hw, hdr->addr3);
-				if (sta == NULL) {
-					RT_TRACE((COMP_SEND | COMP_RECV),
-						 DBG_EMERG, ("sta is NULL\n"));
-					rcu_read_unlock();
-					return true;
-				}
-
-				sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-				if (!sta_entry) {
-					rcu_read_unlock();
-					return true;
-				}
-				capab = le16_to_cpu(mgmt->u.action.u.addba_req.capab);
-				tid = (capab & IEEE80211_ADDBA_PARAM_TID_MASK) >> 2;
-				tid_data = &sta_entry->tids[tid];
-				if (tid_data->agg.rx_agg_state ==
-				    RTL_RX_AGG_START) {
-					skb_delba = rtl_make_del_ba(hw,
-								    hdr->addr2,
-								    hdr->addr3,
-								    tid);
-					if (skb_delba) {
-						rx_status.freq = hw->conf.chandef.chan->center_freq;
-						rx_status.band = hw->conf.chandef.chan->band;
-						rx_status.flag |= RX_FLAG_DECRYPTED;
-						rx_status.flag |= RX_FLAG_MACTIME_MPDU;
-						rx_status.rate_idx = 0;
-						rx_status.signal = 50 + 10;
-						memcpy(IEEE80211_SKB_RXCB(skb_delba), &rx_status,
-								sizeof(rx_status));
-						RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG,
-								("fake del\n"), skb_delba->data,
-								skb_delba->len);
-						ieee80211_rx_irqsafe(hw, skb_delba);
-					}
-				}
-				rcu_read_unlock();
-			}
-			break;
-		case ACT_ADDBARSP:
-			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-				 ("%s ACT_ADDBARSP From :%pM\n",
-				  is_tx ? "Tx" : "Rx", hdr->addr2));
-			break;
-		case ACT_DELBA:
-			RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-				 ("ACT_ADDBADEL From :%pM\n", hdr->addr2));
-			break;
-		}
-		break;
-	default:
-		break;
-	}
-
-	return true;
-}
-
-/*should call before software enc*/
-u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u16 fc = rtl_get_fc(skb);
-	u16 ether_type;
-	u8 mac_hdr_len = ieee80211_get_hdrlen_from_skb(skb);
-	const struct iphdr *ip;
-
-	if (!ieee80211_is_data(fc))
-		goto end;
-
-
-	ip = (struct iphdr *)((u8 *) skb->data + mac_hdr_len +
-			      SNAP_SIZE + PROTOC_TYPE_SIZE);
-	ether_type = *(u16 *) ((u8 *) skb->data + mac_hdr_len + SNAP_SIZE);
-	ether_type = ntohs(ether_type);
-
-	if (ETH_P_IP == ether_type) {
-		if (IPPROTO_UDP == ip->protocol) {
-			struct udphdr *udp = (struct udphdr *)((u8 *) ip +
-							       (ip->ihl << 2));
-			if (((((u8 *) udp)[1] == 68) &&
-			     (((u8 *) udp)[3] == 67)) ||
-			    ((((u8 *) udp)[1] == 67) &&
-			     (((u8 *) udp)[3] == 68))) {
-				/*
-				 * 68 : UDP BOOTP client
-				 * 67 : UDP BOOTP server
-				 */
-				RT_TRACE((COMP_SEND | COMP_RECV),
-					 DBG_DMESG, ("dhcp %s !!\n",
-						     (is_tx) ? "Tx" : "Rx"));
-
-				if (is_tx) {
-					rtlpriv->ra.is_special_data = true;
-					rtl_lps_leave(hw);
-					ppsc->last_delaylps_stamp_jiffies =
-									jiffies;
-				}
-
-				return true;
-			}
-		}
-	} else if (ETH_P_ARP == ether_type) {
-		if (is_tx) {
-			rtlpriv->ra.is_special_data = true;
-			rtl_lps_leave(hw);
-			ppsc->last_delaylps_stamp_jiffies = jiffies;
-		}
-
-		return true;
-	} else if (ETH_P_PAE == ether_type) {
-		RT_TRACE((COMP_SEND | COMP_RECV), DBG_DMESG,
-			 ("802.1X %s EAPOL pkt!!\n", (is_tx) ? "Tx" : "Rx"));
-
-		if (is_tx) {
-			rtlpriv->ra.is_special_data = true;
-			rtl_lps_leave(hw);
-			ppsc->last_delaylps_stamp_jiffies = jiffies;
-		}
-
-		return true;
-	} else if (0x86DD == ether_type) {
-		return true;
-	}
-
-end:
-	rtlpriv->ra.is_special_data = false;
-	return false;
-}
-
-/*********************************************************
- *
- * functions called by core.c
- *
- *********************************************************/
-int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		     struct ieee80211_sta *sta, u16 tid, u16 *ssn)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	if (!sta_entry)
-		return -ENXIO;
-	tid_data = &sta_entry->tids[tid];
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
-		  tid_data->seq_number));
-
-	*ssn = tid_data->seq_number;
-	tid_data->agg.agg_state = RTL_AGG_START;
-
-	ieee80211_start_tx_ba_cb_irqsafe(vif, sta->addr, tid);
-	return 0;
-}
-
-int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-		    struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	if (!sta->addr) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
-		return -EINVAL;
-	}
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d\n", sta->addr, tid));
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	tid_data = &sta_entry->tids[tid];
-	sta_entry->tids[tid].agg.agg_state = RTL_AGG_STOP;
-
-	ieee80211_stop_tx_ba_cb_irqsafe(vif, sta->addr, tid);
-	return 0;
-}
-
-int rtl_rx_agg_start(struct ieee80211_hw *hw,
-		     struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	if (!sta_entry)
-		return -ENXIO;
-	tid_data = &sta_entry->tids[tid];
-
-	RT_TRACE(COMP_RECV, DBG_DMESG,
-		 ("on ra = %pM tid = %d seq:%d\n", sta->addr, tid,
-		 tid_data->seq_number));
-
-	tid_data->agg.rx_agg_state = RTL_RX_AGG_START;
-	return 0;
-}
-
-int rtl_rx_agg_stop(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	if (!sta->addr) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
-		return -EINVAL;
-	}
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d\n", sta->addr, tid));
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	tid_data = &sta_entry->tids[tid];
-	sta_entry->tids[tid].agg.rx_agg_state = RTL_RX_AGG_STOP;
-
-	return 0;
-}
-int rtl_tx_agg_oper(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u16 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tid_data *tid_data;
-	struct rtl_sta_info *sta_entry = NULL;
-
-	if (sta == NULL)
-		return -EINVAL;
-
-	if (!sta->addr) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("ra = NULL\n"));
-		return -EINVAL;
-	}
-
-	RT_TRACE(COMP_SEND, DBG_DMESG,
-		 ("on ra = %pM tid = %d\n", sta->addr, tid));
-
-	if (unlikely(tid >= MAX_TID_COUNT))
-		return -EINVAL;
-
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-	tid_data = &sta_entry->tids[tid];
-	sta_entry->tids[tid].agg.agg_state = RTL_AGG_OPERATIONAL;
-
-	return 0;
-}
-
-/*********************************************************
- *
- * wq & timer callback functions
- *
- *********************************************************/
-/* this function is used for roaming */
-void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
-
-	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
-		return;
-
-	if (rtlpriv->mac80211.link_state < MAC80211_LINKED)
-		return;
-
-	/* check if this really is a beacon */
-	if (!ieee80211_is_beacon(hdr->frame_control) &&
-		!ieee80211_is_probe_resp(hdr->frame_control))
-		return;
-
-	/* min. beacon length + FCS_LEN */
-	if (skb->len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	rtlpriv->link_info.bcn_rx_inperiod++;
-}
-
-void rtl_watchdog_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks = container_of_dwork_rtl(data,
-							    struct rtl_works,
-							    watchdog_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	bool b_busytraffic = false;
-	bool b_tx_busy_traffic = false;
-	bool b_rx_busy_traffic = false;
-	bool b_higher_busytraffic = false;
-	bool b_higher_busyrxtraffic = false;
-	u8 idx, tid;
-	u32 rx_cnt_inp4eriod = 0;
-	u32 tx_cnt_inp4eriod = 0;
-	u32 aver_rx_cnt_inperiod = 0;
-	u32 aver_tx_cnt_inperiod = 0;
-	u32 aver_tidtx_inperiod[MAX_TID_COUNT] = {0};
-	u32 tidtx_inp4eriod[MAX_TID_COUNT] = {0};
-	bool benter_ps = false;
-
-	if (is_hal_stop(rtlhal))
-		return;
-
-	/* <1> Determine if action frame is allowed */
-	if (mac->link_state > MAC80211_NOLINK) {
-		if (mac->cnt_after_linked < 20)
-			mac->cnt_after_linked++;
-	} else {
-		mac->cnt_after_linked = 0;
-	}
-
-	/* <2> to check if traffic busy, if
-	 * busytraffic we don't change channel */
-	if (mac->link_state >= MAC80211_LINKED) {
-
-		/* (1) get aver_rx_cnt_inperiod & aver_tx_cnt_inperiod */
-		for (idx = 0; idx <= 2; idx++) {
-			rtlpriv->link_info.num_rx_in4period[idx] =
-			    rtlpriv->link_info.num_rx_in4period[idx + 1];
-			rtlpriv->link_info.num_tx_in4period[idx] =
-			    rtlpriv->link_info.num_tx_in4period[idx + 1];
-		}
-		rtlpriv->link_info.num_rx_in4period[3] =
-		    rtlpriv->link_info.num_rx_inperiod;
-		rtlpriv->link_info.num_tx_in4period[3] =
-		    rtlpriv->link_info.num_tx_inperiod;
-		for (idx = 0; idx <= 3; idx++) {
-			rx_cnt_inp4eriod +=
-			    rtlpriv->link_info.num_rx_in4period[idx];
-			tx_cnt_inp4eriod +=
-			    rtlpriv->link_info.num_tx_in4period[idx];
-		}
-		aver_rx_cnt_inperiod = rx_cnt_inp4eriod / 4;
-		aver_tx_cnt_inperiod = tx_cnt_inp4eriod / 4;
-
-		/* (2) check traffic busy */
-		if (aver_rx_cnt_inperiod > 100 || aver_tx_cnt_inperiod > 100) {
-			b_busytraffic = true;
-			if (aver_rx_cnt_inperiod > aver_tx_cnt_inperiod)
-				b_rx_busy_traffic = true;
-			else
-				b_tx_busy_traffic = false;
-		}
-
-		/* Higher Tx/Rx data. */
-		if (aver_rx_cnt_inperiod > 4000 ||
-			aver_tx_cnt_inperiod > 4000) {
-			b_higher_busytraffic = true;
-
-			/* Extremely high Rx data. */
-			if (aver_rx_cnt_inperiod > 5000)
-				b_higher_busyrxtraffic = true;
-		}
-
-		/* check every tid's tx traffic */
-		for (tid = 0; tid <= 7; tid++) {
-			for (idx = 0; idx <= 2; idx++)
-				rtlpriv->link_info.tidtx_in4period[tid][idx] =
-					rtlpriv->link_info.tidtx_in4period[tid]
-					[idx + 1];
-			rtlpriv->link_info.tidtx_in4period[tid][3] =
-				rtlpriv->link_info.tidtx_inperiod[tid];
-
-			for (idx = 0; idx <= 3; idx++)
-				tidtx_inp4eriod[tid] +=
-				   rtlpriv->link_info.tidtx_in4period[tid][idx];
-			aver_tidtx_inperiod[tid] = tidtx_inp4eriod[tid] / 4;
-			if (aver_tidtx_inperiod[tid] > 5000)
-				rtlpriv->link_info.higher_busytxtraffic[tid] =
-									true;
-			else
-				rtlpriv->link_info.higher_busytxtraffic[tid] =
-									false;
-		}
-
-		if (((rtlpriv->link_info.num_rx_inperiod +
-		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
-		    (rtlpriv->link_info.num_rx_inperiod > 2))
-			benter_ps = false;
-		else
-			benter_ps = true;
-
-		/* LeisurePS only work in infra mode. */
-		if (benter_ps)
-			rtl_lps_enter(hw);
-		else
-			rtl_lps_leave(hw);
-	}
-
-	rtlpriv->link_info.num_rx_inperiod = 0;
-	rtlpriv->link_info.num_tx_inperiod = 0;
-	for (tid = 0; tid <= 7; tid++)
-		rtlpriv->link_info.tidtx_inperiod[tid] = 0;
-
-	rtlpriv->link_info.b_busytraffic = b_busytraffic;
-	rtlpriv->link_info.b_rx_busy_traffic = b_rx_busy_traffic;
-	rtlpriv->link_info.b_tx_busy_traffic = b_tx_busy_traffic;
-	rtlpriv->link_info.b_higher_busytraffic = b_higher_busytraffic;
-	rtlpriv->link_info.b_higher_busyrxtraffic = b_higher_busyrxtraffic;
-
-	/* <3> DM */
-	rtlpriv->cfg->ops->dm_watchdog(hw);
-
-	/* <4> roaming */
-	if (mac->link_state == MAC80211_LINKED &&
-			mac->opmode == NL80211_IFTYPE_STATION) {
-		if ((rtlpriv->link_info.bcn_rx_inperiod +
-			rtlpriv->link_info.num_rx_inperiod) == 0) {
-			rtlpriv->link_info.roam_times++;
-			RT_TRACE(COMP_ERR, DBG_DMESG, ("AP off for %d s\n",
-				(rtlpriv->link_info.roam_times * 2)));
-
-			/* if we can't recv beacon for 10s,
-			 * we should reconnect this AP */
-			if (rtlpriv->link_info.roam_times >= 5) {
-				RT_TRACE(COMP_ERR, DBG_EMERG,
-					 ("AP off, try to reconnect now\n"));
-				rtlpriv->link_info.roam_times = 0;
-				ieee80211_connection_loss(rtlpriv->mac80211.vif);
-			}
-		} else {
-			rtlpriv->link_info.roam_times = 0;
-		}
-	}
-	rtlpriv->link_info.bcn_rx_inperiod = 0;
-}
-
-void rtl_watch_dog_timer_callback(unsigned long data)
-{
-	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	queue_delayed_work(rtlpriv->works.rtl_wq,
-			   &rtlpriv->works.watchdog_wq, 0);
-
-	mod_timer(&rtlpriv->works.watchdog_timer,
-		  jiffies + MSECS(RTL_WATCH_DOG_TIME));
-}
-void rtl_fwevt_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-		container_of_dwork_rtl(data, struct rtl_works, fwevt_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->cfg->ops->c2h_command_handle(hw);
-}
-void rtl_easy_concurrent_retrytimer_callback(unsigned long data)
-{
-	struct ieee80211_hw *hw = (struct ieee80211_hw *)data;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_priv *buddy_priv = rtlpriv->buddy_priv;
-
-	if (buddy_priv == NULL)
-		return;
-
-	rtlpriv->cfg->ops->dualmac_easy_concurrent(hw);
-}
-/*********************************************************
- *
- * frame process functions
- *
- *********************************************************/
-u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie)
-{
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	u8 *pos, *end;
-
-	pos = (u8 *)mgmt->u.beacon.variable;
-	end = data + len;
-	while (pos < end) {
-		if (pos + 2 + pos[1] > end)
-			return NULL;
-
-		if (pos[0] == ie)
-			return pos;
-
-		pos += 2 + pos[1];
-	}
-	return NULL;
-}
-
-/* when we use 2 rx ants we send IEEE80211_SMPS_OFF */
-/* when we use 1 rx ant we send IEEE80211_SMPS_STATIC */
-struct sk_buff *rtl_make_smps_action(struct ieee80211_hw *hw,
-				     enum ieee80211_smps_mode smps,
-				     u8 *da, u8 *bssid)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct sk_buff *skb;
-	struct ieee80211_mgmt_compat *action_frame;
-
-	/* 27 = header + category + action + smps mode */
-	skb = dev_alloc_skb(27 + hw->extra_tx_headroom);
-	if (!skb)
-		return NULL;
-
-	skb_reserve(skb, hw->extra_tx_headroom);
-	action_frame = (void *)skb_put(skb, 27);
-	memset(action_frame, 0, 27);
-	memcpy(action_frame->da, da, ETH_ALEN);
-	memcpy(action_frame->sa, rtlefuse->dev_addr, ETH_ALEN);
-	memcpy(action_frame->bssid, bssid, ETH_ALEN);
-	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-						  IEEE80211_STYPE_ACTION);
-	action_frame->u.action.category = WLAN_CATEGORY_HT;
-	action_frame->u.action.u.ht_smps.action = WLAN_HT_ACTION_SMPS;
-	switch (smps) {
-	case IEEE80211_SMPS_AUTOMATIC:/* 0 */
-	case IEEE80211_SMPS_NUM_MODES:/* 4 */
-		WARN_ON(1);
-	case IEEE80211_SMPS_OFF:/* 1 */ /*MIMO_PS_NOLIMIT*/
-		action_frame->u.action.u.ht_smps.smps_control =
-				WLAN_HT_SMPS_CONTROL_DISABLED;/* 0 */
-		break;
-	case IEEE80211_SMPS_STATIC:/* 2 */ /*MIMO_PS_STATIC*/
-		action_frame->u.action.u.ht_smps.smps_control =
-				WLAN_HT_SMPS_CONTROL_STATIC;/* 1 */
-		break;
-	case IEEE80211_SMPS_DYNAMIC:/* 3 */ /*MIMO_PS_DYNAMIC*/
-		action_frame->u.action.u.ht_smps.smps_control =
-				WLAN_HT_SMPS_CONTROL_DYNAMIC;/* 3 */
-		break;
-	}
-
-	return skb;
-}
-
-int rtl_send_smps_action(struct ieee80211_hw *hw,
-			 struct ieee80211_sta *sta,
-			 enum ieee80211_smps_mode smps)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct sk_buff *skb = NULL;
-	struct rtl_tcb_desc tcb_desc;
-	u8 bssid[ETH_ALEN] = {0};
-
-	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-
-	if (rtlpriv->mac80211.act_scanning)
-		goto err_free;
-
-	if (!sta)
-		goto err_free;
-
-	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
-		goto err_free;
-
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		goto err_free;
-
-	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP)
-		memcpy(bssid, rtlpriv->efuse.dev_addr, ETH_ALEN);
-	else
-		memcpy(bssid, rtlpriv->mac80211.bssid, ETH_ALEN);
-
-	skb = rtl_make_smps_action(hw, smps, sta->addr, bssid);
-	/* this is a type = mgmt * stype = action frame */
-	if (skb) {
-		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-		struct rtl_sta_info *sta_entry =
-			(struct rtl_sta_info *) sta->drv_priv;
-		sta_entry->mimo_ps = smps;
-		/* rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0); */
-
-		info->control.rates[0].idx = 0;
-		info->band = hw->conf.chandef.chan->band;
-		rtlpriv->intf_ops->adapter_tx(hw, sta, skb, &tcb_desc);
-	}
-	return 1;
-
-err_free:
-	return 0;
-}
-/* EXPORT_SYMBOL(rtl_send_smps_action); */
-
-/* because mac80211 have issues when can receive del ba
- * so here we just make a fake del_ba if we receive a ba_req
- * but rx_agg was opened to let mac80211 release some ba
- * related resources, so please this del_ba for tx */
-struct sk_buff *rtl_make_del_ba(struct ieee80211_hw *hw,
-				u8 *sa, u8 *bssid, u16 tid)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct sk_buff *skb;
-	struct ieee80211_mgmt *action_frame;
-	u16 params;
-
-	/* 27 = header + category + action + smps mode */
-	skb = dev_alloc_skb(34 + hw->extra_tx_headroom);
-	if (!skb)
-		return NULL;
-
-	skb_reserve(skb, hw->extra_tx_headroom);
-	action_frame = (void *)skb_put(skb, 34);
-	memset(action_frame, 0, 34);
-	memcpy(action_frame->sa, sa, ETH_ALEN);
-	memcpy(action_frame->da, rtlefuse->dev_addr, ETH_ALEN);
-	memcpy(action_frame->bssid, bssid, ETH_ALEN);
-	action_frame->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
-						  IEEE80211_STYPE_ACTION);
-	action_frame->u.action.category = WLAN_CATEGORY_BACK;
-	action_frame->u.action.u.delba.action_code = WLAN_ACTION_DELBA;
-	params = (u16)(1 << 11);	/* bit 11 initiator */
-	params |= (u16)(tid << 12);	/* bit 15:12 TID number */
-
-	action_frame->u.action.u.delba.params = cpu_to_le16(params);
-	action_frame->u.action.u.delba.reason_code =
-		cpu_to_le16(WLAN_REASON_QSTA_TIMEOUT);
-
-	return skb;
-}
-
-/*********************************************************
- *
- * IOT functions
- *
- *********************************************************/
-static bool rtl_chk_vendor_ouisub(struct ieee80211_hw *hw,
-				  struct octet_string vendor_ie)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool matched = false;
-	static u8 athcap_1[] = { 0x00, 0x03, 0x7F };
-	static u8 athcap_2[] = { 0x00, 0x13, 0x74 };
-	static u8 broadcap_1[] = { 0x00, 0x10, 0x18 };
-	static u8 broadcap_2[] = { 0x00, 0x0a, 0xf7 };
-	static u8 broadcap_3[] = { 0x00, 0x05, 0xb5 };
-	static u8 racap[] = { 0x00, 0x0c, 0x43 };
-	static u8 ciscocap[] = { 0x00, 0x40, 0x96 };
-	static u8 marvcap[] = { 0x00, 0x50, 0x43 };
-
-	if (memcmp(vendor_ie.octet, athcap_1, 3) == 0 ||
-		memcmp(vendor_ie.octet, athcap_2, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_ATH;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, broadcap_1, 3) == 0 ||
-		memcmp(vendor_ie.octet, broadcap_2, 3) == 0 ||
-		memcmp(vendor_ie.octet, broadcap_3, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_BROAD;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, racap, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_RAL;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, ciscocap, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_CISCO;
-		matched = true;
-	} else if (memcmp(vendor_ie.octet, marvcap, 3) == 0) {
-		rtlpriv->mac80211.vendor = PEER_MARV;
-		matched = true;
-	}
-
-	return matched;
-}
-
-bool rtl_find_221_ie(struct ieee80211_hw *hw, u8 *data,
-		unsigned int len)
-{
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	struct octet_string vendor_ie;
-	u8 *pos, *end;
-
-	pos = (u8 *)mgmt->u.beacon.variable;
-	end = data + len;
-	while (pos < end) {
-		if (pos[0] == 221) {
-			vendor_ie.length = pos[1];
-			vendor_ie.octet = &pos[2];
-			if (rtl_chk_vendor_ouisub(hw, vendor_ie))
-				return true;
-		}
-
-		if (pos + 2 + pos[1] > end)
-			return false;
-
-		pos += 2 + pos[1];
-	}
-	return false;
-}
-
-void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = (void *)data;
-	u32 vendor = PEER_UNKNOWN;
-
-	static u8 ap3_1[3] = { 0x00, 0x14, 0xbf };
-	static u8 ap3_2[3] = { 0x00, 0x1a, 0x70 };
-	static u8 ap3_3[3] = { 0x00, 0x1d, 0x7e };
-	static u8 ap4_1[3] = { 0x00, 0x90, 0xcc };
-	static u8 ap4_2[3] = { 0x00, 0x0e, 0x2e };
-	static u8 ap4_3[3] = { 0x00, 0x18, 0x02 };
-	static u8 ap4_4[3] = { 0x00, 0x17, 0x3f };
-	static u8 ap4_5[3] = { 0x00, 0x1c, 0xdf };
-	static u8 ap5_1[3] = { 0x00, 0x1c, 0xf0 };
-	static u8 ap5_2[3] = { 0x00, 0x21, 0x91 };
-	static u8 ap5_3[3] = { 0x00, 0x24, 0x01 };
-	static u8 ap5_4[3] = { 0x00, 0x15, 0xe9 };
-	static u8 ap5_5[3] = { 0x00, 0x17, 0x9A };
-	static u8 ap5_6[3] = { 0x00, 0x18, 0xE7 };
-	static u8 ap6_1[3] = { 0x00, 0x17, 0x94 };
-	static u8 ap7_1[3] = { 0x00, 0x14, 0xa4 };
-
-	if (mac->opmode != NL80211_IFTYPE_STATION)
-		return;
-
-	if (mac->link_state == MAC80211_NOLINK) {
-		mac->vendor = PEER_UNKNOWN;
-		return;
-	}
-
-	if (mac->cnt_after_linked > 2)
-		return;
-
-	/* check if this really is a beacon */
-	if (!ieee80211_is_beacon(hdr->frame_control))
-		return;
-
-	/* min. beacon length + FCS_LEN */
-	if (len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	if (rtl_find_221_ie(hw, data, len))
-		vendor = mac->vendor;
-
-	if ((memcmp(mac->bssid, ap5_1, 3) == 0) ||
-		(memcmp(mac->bssid, ap5_2, 3) == 0) ||
-		(memcmp(mac->bssid, ap5_3, 3) == 0) ||
-		(memcmp(mac->bssid, ap5_4, 3) == 0) ||
-		(memcmp(mac->bssid, ap5_5, 3) == 0) ||
-		(memcmp(mac->bssid, ap5_6, 3) == 0) ||
-		vendor == PEER_ATH) {
-		vendor = PEER_ATH;
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ath find\n"));
-	} else if ((memcmp(mac->bssid, ap4_4, 3) == 0) ||
-		(memcmp(mac->bssid, ap4_5, 3) == 0) ||
-		(memcmp(mac->bssid, ap4_1, 3) == 0) ||
-		(memcmp(mac->bssid, ap4_2, 3) == 0) ||
-		(memcmp(mac->bssid, ap4_3, 3) == 0) ||
-		vendor == PEER_RAL) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>ral find\n"));
-		vendor = PEER_RAL;
-	} else if (memcmp(mac->bssid, ap6_1, 3) == 0 ||
-		vendor == PEER_CISCO) {
-		vendor = PEER_CISCO;
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>cisco find\n"));
-	} else if ((memcmp(mac->bssid, ap3_1, 3) == 0) ||
-		(memcmp(mac->bssid, ap3_2, 3) == 0) ||
-		(memcmp(mac->bssid, ap3_3, 3) == 0) ||
-		vendor == PEER_BROAD) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>broad find\n"));
-		vendor = PEER_BROAD;
-	} else if (memcmp(mac->bssid, ap7_1, 3) == 0 ||
-		vendor == PEER_MARV) {
-		vendor = PEER_MARV;
-		RT_TRACE(COMP_MAC80211, DBG_LOUD, ("=>marv find\n"));
-	}
-
-	mac->vendor = vendor;
-}
-
-/*********************************************************
- *
- * sysfs functions
- *
- *********************************************************/
-static ssize_t rtl_show_debug_level(struct device *d,
-				    struct device_attribute *attr, char *buf)
-{
-	struct ieee80211_hw *hw = dev_get_drvdata(d);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	return sprintf(buf, "0x%08X\n", rtlpriv->dbg.global_debuglevel);
-}
-
-static ssize_t rtl_store_debug_level(struct device *d,
-				     struct device_attribute *attr,
-				     const char *buf, size_t count)
-{
-	struct ieee80211_hw *hw = dev_get_drvdata(d);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	unsigned long val;
-	int ret;
-
-	ret = kstrtoul(buf, 0, &val);
-	if (ret) {
-		printk(KERN_DEBUG "%s is not in hex or decimal form.\n", buf);
-	} else {
-		rtlpriv->dbg.global_debuglevel = val;
-		printk(KERN_DEBUG "debuglevel:%x\n",
-			rtlpriv->dbg.global_debuglevel);
-	}
-
-	return strnlen(buf, count);
-}
-
-static DEVICE_ATTR(debug_level, S_IWUSR | S_IRUGO,
-		   rtl_show_debug_level, rtl_store_debug_level);
-
-static struct attribute *rtl_sysfs_entries[] = {
-
-	&dev_attr_debug_level.attr,
-
-	NULL
-};
-
-/*
- * "name" is folder name witch will be
- * put in device directory like :
- * sys/devices/pci0000:00/0000:00:1c.4/
- * 0000:06:00.0/rtl_sysfs
- */
-struct attribute_group rtl_attribute_group = {
-	.name = "rtlsysfs",
-	.attrs = rtl_sysfs_entries,
-};
-
-#ifdef VIF_TODO
-/*********************************************************
- *
- * vif functions
- *
- *********************************************************/
-static inline struct ieee80211_vif *
-rtl_get_vif(struct rtl_vif_info *vif_priv)
-{
-	return container_of((void *)vif_priv, struct ieee80211_vif, drv_priv);
-}
-
-/* Protected by ar->mutex or RCU */
-struct ieee80211_vif *rtl_get_main_vif(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_vif_info *cvif;
-
-	list_for_each_entry_rcu(cvif, &rtlpriv->vif_priv.vif_list, list) {
-		if (cvif->active)
-			return rtl_get_vif(cvif);
-	}
-
-	return NULL;
-}
-
-static inline bool is_main_vif(struct ieee80211_hw *hw,
-			       struct ieee80211_vif *vif)
-{
-	bool ret;
-
-	rcu_read_lock();
-	ret = (rtl_get_main_vif(hw) == vif);
-	rcu_read_unlock();
-	return ret;
-}
-
-bool rtl_set_vif_info(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-{
-	struct rtl_vif_info *vif_info = (void *) vif->drv_priv;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int vif_id = -1;
-
-	if (rtlpriv->vif_priv.vifs >= MAX_VIRTUAL_MAC) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("vif number can not bigger than %d, now vifs is:%d\n",
-			  MAX_VIRTUAL_MAC, rtlpriv->vif_priv.vifs));
-		return false;
-	}
-
-	rcu_read_lock();
-	vif_id = bitmap_find_free_region(&rtlpriv->vif_priv.vif_bitmap,
-					 MAX_VIRTUAL_MAC, 0);
-	RT_TRACE(COMP_MAC80211, DBG_DMESG,
-		 ("%s vid_id:%d\n", __func__, vif_id));
-
-	if (vif_id < 0) {
-		rcu_read_unlock();
-		return false;
-	}
-
-	BUG_ON(rtlpriv->vif_priv.vif[vif_id].id != vif_id);
-	vif_info->active = true;
-	vif_info->id = vif_id;
-	vif_info->enable_beacon = false;
-	rtlpriv->vif_priv.vifs++;
-	if (rtlpriv->vif_priv.vifs > 1) {
-		rtlpriv->psc.b_inactiveps = false;
-		rtlpriv->psc.b_swctrl_lps = false;
-		rtlpriv->psc.b_fwctrl_lps = false;
-	}
-
-	list_add_tail_rcu(&vif_info->list, &rtlpriv->vif_priv.vif_list);
-	rcu_assign_pointer(rtlpriv->vif_priv.vif[vif_id].vif, vif);
-
-	RT_TRACE(COMP_MAC80211, DBG_DMESG, ("vifaddress:%p %p %p\n",
-		 rtlpriv->vif_priv.vif[vif_id].vif, vif, rtl_get_main_vif(hw)));
-
-	rcu_read_unlock();
-
-	return true;
-}
-#endif
-
-
-#if 0
-MODULE_AUTHOR("lizhaoming	<chaoming_li@realsil.com.cn>");
-MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
-MODULE_AUTHOR("Larry Finger	<Larry.FInger@lwfinger.net>");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Realtek 802.11n PCI wireless core");
-#endif
-struct rtl_global_var global_var = {};
-
-int rtl_core_module_init(void)
-{
-	if (rtl_rate_control_register())
-		printk(KERN_DEBUG "rtl: Unable to register rtl_rc, use default RC !!\n");
-
-	/* add proc for debug */
-	rtl_proc_add_topdir();
-
-	/* init some global vars */
-	INIT_LIST_HEAD(&global_var.glb_priv_list);
-	spin_lock_init(&global_var.glb_list_lock);
-
-	return 0;
-}
-
-void rtl_core_module_exit(void)
-{
-	/*RC*/
-	rtl_rate_control_unregister();
-
-	/* add proc for debug */
-	rtl_proc_remove_topdir();
-}
-
-#if 0
-module_init(rtl_core_module_init);
-module_exit(rtl_core_module_exit);
-#endif
diff --git a/drivers/staging/rtl8821ae/base.h b/drivers/staging/rtl8821ae/base.h
deleted file mode 100644
index 629d14f42f0b..000000000000
--- a/drivers/staging/rtl8821ae/base.h
+++ /dev/null
@@ -1,159 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_BASE_H__
-#define __RTL_BASE_H__
-
-#include "compat.h"
-
-enum ap_peer {
-	PEER_UNKNOWN = 0,
-	PEER_RTL = 1,
-	PEER_RTL_92SE = 2,
-	PEER_BROAD = 3,
-	PEER_RAL = 4,
-	PEER_ATH = 5,
-	PEER_CISCO = 6,
-	PEER_MARV = 7,
-	PEER_AIRGO = 9,
-	PEER_MAX = 10,
-} ;
-
-#define RTL_DUMMY_OFFSET	0
-#define RTL_DUMMY_UNIT		8
-#define RTL_TX_DUMMY_SIZE	(RTL_DUMMY_OFFSET * RTL_DUMMY_UNIT)
-#define RTL_TX_DESC_SIZE	32
-#define RTL_TX_HEADER_SIZE	(RTL_TX_DESC_SIZE + RTL_TX_DUMMY_SIZE)
-
-#define HT_AMSDU_SIZE_4K 	3839
-#define HT_AMSDU_SIZE_8K 	7935
-
-#define MAX_BIT_RATE_40MHZ_MCS15 	300	/* Mbps */
-#define MAX_BIT_RATE_40MHZ_MCS7 	150	/* Mbps */
-
-#define RTL_RATE_COUNT_LEGACY		12
-#define RTL_CHANNEL_COUNT		14
-
-#define FRAME_OFFSET_FRAME_CONTROL	0
-#define FRAME_OFFSET_DURATION		2
-#define FRAME_OFFSET_ADDRESS1		4
-#define FRAME_OFFSET_ADDRESS2		10
-#define FRAME_OFFSET_ADDRESS3		16
-#define FRAME_OFFSET_SEQUENCE		22
-#define FRAME_OFFSET_ADDRESS4		24
-
-#define SET_80211_HDR_FRAME_CONTROL(_hdr, _val)		\
-	WRITEEF2BYTE(_hdr, _val)
-#define SET_80211_HDR_TYPE_AND_SUBTYPE(_hdr, _val)	\
-	WRITEEF1BYTE(_hdr, _val)
-#define SET_80211_HDR_PWR_MGNT(_hdr, _val)		\
-	SET_BITS_TO_LE_2BYTE(_hdr, 12, 1, _val)
-#define SET_80211_HDR_TO_DS(_hdr, _val)			\
-	SET_BITS_TO_LE_2BYTE(_hdr, 8, 1, _val)
-
-#define SET_80211_PS_POLL_AID(_hdr, _val)		\
-	WRITEEF2BYTE(((u8*)(_hdr))+2, _val)
-#define SET_80211_PS_POLL_BSSID(_hdr, _val)		\
-	CP_MACADDR(((u8*)(_hdr))+4, (u8*)(_val))
-#define SET_80211_PS_POLL_TA(_hdr, _val)		\
-	CP_MACADDR(((u8*)(_hdr))+10, (u8*)(_val))
-
-#define SET_80211_HDR_DURATION(_hdr, _val)	\
-	WRITEEF2BYTE((u8*)(_hdr)+FRAME_OFFSET_DURATION, _val)
-#define SET_80211_HDR_ADDRESS1(_hdr, _val)	\
-	CP_MACADDR((u8*)(_hdr)+FRAME_OFFSET_ADDRESS1, (u8*)(_val))
-#define SET_80211_HDR_ADDRESS2(_hdr, _val) 	\
-	CP_MACADDR((u8*)(_hdr)+FRAME_OFFSET_ADDRESS2, (u8*)(_val))
-#define SET_80211_HDR_ADDRESS3(_hdr, _val) 	\
-	CP_MACADDR((u8*)(_hdr)+FRAME_OFFSET_ADDRESS3, (u8*)(_val))
-#define SET_80211_HDR_FRAGMENT_SEQUENCE(_hdr, _val)  \
-	WRITEEF2BYTE((u8*)(_hdr)+FRAME_OFFSET_SEQUENCE, _val)
-
-#define SET_BEACON_PROBE_RSP_TIME_STAMP_LOW(__phdr, __val) 	\
-	WRITEEF4BYTE(((u8*)(__phdr)) + 24, __val)
-#define SET_BEACON_PROBE_RSP_TIME_STAMP_HIGH(__phdr, __val) \
-	WRITEEF4BYTE(((u8*)(__phdr)) + 28, __val)
-#define SET_BEACON_PROBE_RSP_BEACON_INTERVAL(__phdr, __val) \
-	WRITEEF2BYTE(((u8*)(__phdr)) + 32, __val)
-#define GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) 		\
-	READEF2BYTE(((u8*)(__phdr)) + 34)
-#define SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
-	WRITEEF2BYTE(((u8*)(__phdr)) + 34, __val)
-#define MASK_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, __val) \
-	SET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr, \
-	(GET_BEACON_PROBE_RSP_CAPABILITY_INFO(__phdr) & (~(__val))))
-
-int rtl_init_core(struct ieee80211_hw *hw);
-void rtl_deinit_core(struct ieee80211_hw *hw);
-void rtl_init_rx_config(struct ieee80211_hw *hw);
-void rtl_init_rfkill(struct ieee80211_hw *hw);
-void rtl_deinit_rfkill(struct ieee80211_hw *hw);
-
-void rtl_watch_dog_timer_callback(unsigned long data);
-void rtl_deinit_deferred_work(struct ieee80211_hw *hw);
-
-bool rtl_action_proc(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
-bool rtl_tx_mgmt_proc(struct ieee80211_hw *hw, struct sk_buff *skb);
-u8 rtl_is_special_data(struct ieee80211_hw *hw, struct sk_buff *skb, u8 is_tx);
-
-void rtl_beacon_statistic(struct ieee80211_hw *hw, struct sk_buff *skb);
-void rtl_watch_dog_timer_callback(unsigned long data);
-int rtl_tx_agg_start(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-	struct ieee80211_sta *sta, u16 tid, u16 * ssn);
-int rtl_tx_agg_stop(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
-	struct ieee80211_sta *sta, u16 tid);
-int rtl_tx_agg_oper(struct ieee80211_hw *hw,
-		    struct ieee80211_sta *sta, u16 tid);
-int rtl_rx_agg_start(struct ieee80211_hw *hw,
-		     struct ieee80211_sta *sta, u16 tid);
-int rtl_rx_agg_stop(struct ieee80211_hw *hw,
-		    struct ieee80211_sta *sta, u16 tid);
-void rtl_watchdog_wq_callback(void *data);
-void rtl_fwevt_wq_callback(void *data);
-
-void rtl_get_tcb_desc(struct ieee80211_hw *hw,
-		      struct ieee80211_tx_info *info,
-		      struct ieee80211_sta *sta,
-		      struct sk_buff *skb, struct rtl_tcb_desc *tcb_desc);
-
-int rtl_send_smps_action(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta,
-		enum ieee80211_smps_mode smps);
-u8 *rtl_find_ie(u8 *data, unsigned int len, u8 ie);
-void rtl_recognize_peer(struct ieee80211_hw *hw, u8 *data, unsigned int len);
-u8 rtl_tid_to_ac(struct ieee80211_hw *hw, u8 tid);
-extern struct attribute_group rtl_attribute_group;
-void rtl_easy_concurrent_retrytimer_callback(unsigned long data);
-extern struct rtl_global_var global_var;
-
-#ifdef VIF_TODO
-struct ieee80211_vif *rtl_get_main_vif(struct ieee80211_hw *hw);
-bool rtl_set_vif_info(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
-#endif
-#endif
diff --git a/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.c b/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.c
deleted file mode 100644
index cf8c38292cd8..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.c
+++ /dev/null
@@ -1,3976 +0,0 @@
-//============================================================
-// Description:
-//
-// This file is for 8812a1ant Co-exist mechanism
-//
-// History
-// 2012/11/15 Cosa first check in.
-//
-//============================================================
-
-//============================================================
-// include files
-//============================================================
-#include "halbt_precomp.h"
-#if 1
-//============================================================
-// Global variables, these are static variables
-//============================================================
-static COEX_DM_8812A_1ANT		GLCoexDm8812a1Ant;
-static PCOEX_DM_8812A_1ANT 	coex_dm=&GLCoexDm8812a1Ant;
-static COEX_STA_8812A_1ANT		GLCoexSta8812a1Ant;
-static PCOEX_STA_8812A_1ANT	coex_sta=&GLCoexSta8812a1Ant;
-
-const char *const GLBtInfoSrc8812a1Ant[]={
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-//============================================================
-// local function proto type if needed
-//============================================================
-//============================================================
-// local function start with halbtc8812a1ant_
-//============================================================
-#if 0
-void
-halbtc8812a1ant_Reg0x550Bit3(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			bSet
-	)
-{
-	u1Byte	u1tmp=0;
-	
-	u1tmp = btcoexist->btc_read_1byte(btcoexist, 0x550);
-	if(bSet)
-	{
-		u1tmp |= BIT3;
-	}
-	else
-	{
-		u1tmp &= ~BIT3;
-	}
-	btcoexist->btc_write_1byte(btcoexist, 0x550, u1tmp);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], set 0x550[3]=%d\n", (bSet? 1:0)));
-}
-#endif
-u1Byte
-halbtc8812a1ant_BtRssiState(
-	u1Byte			level_num,
-	u1Byte			rssi_thresh,
-	u1Byte			rssi_thresh1
-	)
-{
-	s4Byte			bt_rssi=0;
-	u1Byte			bt_rssi_state;
-
-	bt_rssi = coex_sta->bt_rssi;
-
-	if(level_num == 2)
-	{			
-		if( (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-			(coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
-			{
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
-			}
-			else
-			{
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
-			}
-		}
-		else
-		{
-			if(bt_rssi < rssi_thresh)
-			{
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
-			}
-			else
-			{
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
-			}
-		}
-	}
-	else if(level_num == 3)
-	{
-		if(rssi_thresh > rssi_thresh1)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
-			return coex_sta->pre_bt_rssi_state;
-		}
-		
-		if( (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-			(coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(bt_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
-			{
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
-			}
-			else
-			{
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
-			}
-		}
-		else if( (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_MEDIUM) ||
-			(coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_MEDIUM))
-		{
-			if(bt_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
-			{
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
-			}
-			else if(bt_rssi < rssi_thresh)
-			{
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
-			}
-			else
-			{
-				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
-			}
-		}
-		else
-		{
-			if(bt_rssi < rssi_thresh1)
-			{
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
-			}
-			else
-			{
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
-			}
-		}
-	}
-		
-	coex_sta->pre_bt_rssi_state = bt_rssi_state;
-
-	return bt_rssi_state;
-}
-
-u1Byte
-halbtc8812a1ant_WifiRssiState(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			index,
-	 	u1Byte			level_num,
-	 	u1Byte			rssi_thresh,
-	 	u1Byte			rssi_thresh1
-	)
-{
-	s4Byte			wifi_rssi=0;
-	u1Byte			wifi_rssi_state;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-	
-	if(level_num == 2)
-	{
-		if( (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_LOW) ||
-			(coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(wifi_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
-			}
-			else
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
-			}
-		}
-		else
-		{
-			if(wifi_rssi < rssi_thresh)
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
-			}
-			else
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
-			}
-		}
-	}
-	else if(level_num == 3)
-	{
-		if(rssi_thresh > rssi_thresh1)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
-			return coex_sta->pre_wifi_rssi_state[index];
-		}
-		
-		if( (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_LOW) ||
-			(coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(wifi_rssi >= (rssi_thresh+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
-			}
-			else
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
-			}
-		}
-		else if( (coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_MEDIUM) ||
-			(coex_sta->pre_wifi_rssi_state[index] == BTC_RSSI_STATE_STAY_MEDIUM))
-		{
-			if(wifi_rssi >= (rssi_thresh1+BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT))
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
-			}
-			else if(wifi_rssi < rssi_thresh)
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
-			}
-			else
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
-			}
-		}
-		else
-		{
-			if(wifi_rssi < rssi_thresh1)
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
-			}
-			else
-			{
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
-			}
-		}
-	}
-		
-	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
-
-	return wifi_rssi_state;
-}
-
-void
-halbtc8812a1ant_MonitorBtEnableDisable(
-	  	PBTC_COEXIST		btcoexist
-	)
-{
-	static BOOLEAN	pre_bt_disabled=false;
-	static u4Byte		bt_disable_cnt=0;
-	BOOLEAN			bt_active=true, bt_disable_by68=false, bt_disabled=false;
-	u4Byte			u4_tmp=0;
-
-	// This function check if bt is disabled
-
-	if(	coex_sta->high_priority_tx == 0 &&
-		coex_sta->high_priority_rx == 0 &&
-		coex_sta->low_priority_tx == 0 &&
-		coex_sta->low_priority_rx == 0)
-	{
-		bt_active = false;
-	}
-	if(	coex_sta->high_priority_tx == 0xffff &&
-		coex_sta->high_priority_rx == 0xffff &&
-		coex_sta->low_priority_tx == 0xffff &&
-		coex_sta->low_priority_rx == 0xffff)
-	{
-		bt_active = false;
-	}
-	if(bt_active)
-	{
-		bt_disable_cnt = 0;
-		bt_disabled = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE, &bt_disabled);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
-	}
-	else
-	{
-		bt_disable_cnt++;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
-				bt_disable_cnt));
-		if(bt_disable_cnt >= 2 ||bt_disable_by68)
-		{
-			bt_disabled = true;
-			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE, &bt_disabled);
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
-		}
-	}
-	if(pre_bt_disabled != bt_disabled)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
-			(pre_bt_disabled ? "disabled":"enabled"), 
-			(bt_disabled ? "disabled":"enabled")));
-		pre_bt_disabled = bt_disabled;
-		if(!bt_disabled)
-		{
-		}
-		else
-		{
-		}
-	}
-}
-
-void
-halbtc8812a1ant_MonitorBtCtr(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u4Byte 			reg_hp_tx_rx, reg_lp_tx_rx, u4_tmp;
-	u4Byte			reg_hp_tx=0, reg_hp_rx=0, reg_lp_tx=0, reg_lp_rx=0;
-	u1Byte			u1_tmp;
-	
-	reg_hp_tx_rx = 0x770;
-	reg_lp_tx_rx = 0x774;
-
-	u4_tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_tx_rx);
-	reg_hp_tx = u4_tmp & bMaskLWord;
-	reg_hp_rx = (u4_tmp & bMaskHWord)>>16;
-
-	u4_tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_tx_rx);
-	reg_lp_tx = u4_tmp & bMaskLWord;
-	reg_lp_rx = (u4_tmp & bMaskHWord)>>16;
-		
-	coex_sta->high_priority_tx = reg_hp_tx;
-	coex_sta->high_priority_rx = reg_hp_rx;
-	coex_sta->low_priority_tx = reg_lp_tx;
-	coex_sta->low_priority_rx = reg_lp_rx;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n", 
-		reg_hp_tx_rx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx));
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n", 
-		reg_lp_tx_rx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx));
-
-	// reset counter
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-}
-
-void
-halbtc8812a1ant_QueryBtInfo(
-	 	PBTC_COEXIST		btcoexist
-	)
-{	
-	u1Byte	dataLen=3;
-	u1Byte	buf[5] = {0};
-	static	u4Byte	btInfoCnt=0;
-
-	if(!btInfoCnt ||
-		(coex_sta->bt_info_c2h_cnt[BT_INFO_SRC_8812A_1ANT_BT_RSP]-btInfoCnt)>2)
-	{
-		buf[0] = dataLen;
-		buf[1] = 0x1;	// polling enable, 1=enable, 0=disable
-		buf[2] = 0x2;	// polling time in seconds
-		buf[3] = 0x1;	// auto report enable, 1=enable, 0=disable
-			
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_INFO, (PVOID)&buf[0]);
-	}
-	btInfoCnt = coex_sta->bt_info_c2h_cnt[BT_INFO_SRC_8812A_1ANT_BT_RSP];
-}
-u1Byte
-halbtc8812a1ant_ActionAlgorithm(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
-	BOOLEAN				bt_hs_on=false;
-	u1Byte				algorithm=BT_8812A_1ANT_COEX_ALGO_UNDEFINED;
-	u1Byte				num_of_diff_profile=0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	
-	if(!stack_info->bt_link_exist)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No profile exists!!!\n"));
-		return algorithm;
-	}
-
-	if(stack_info->sco_exist)
-		num_of_diff_profile++;
-	if(stack_info->hid_exist)
-		num_of_diff_profile++;
-	if(stack_info->pan_exist)
-		num_of_diff_profile++;
-	if(stack_info->a2dp_exist)
-		num_of_diff_profile++;
-	
-	if(num_of_diff_profile == 1)
-	{
-		if(stack_info->sco_exist)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
-			algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
-		}
-		else
-		{
-			if(stack_info->hid_exist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
-				algorithm = BT_8812A_1ANT_COEX_ALGO_HID;
-			}
-			else if(stack_info->a2dp_exist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
-				algorithm = BT_8812A_1ANT_COEX_ALGO_A2DP;
-			}
-			else if(stack_info->pan_exist)
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANHS;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR;
-				}
-			}
-		}
-	}
-	else if(num_of_diff_profile == 2)
-	{
-		if(stack_info->sco_exist)
-		{
-			if(stack_info->hid_exist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
-				algorithm = BT_8812A_1ANT_COEX_ALGO_HID;
-			}
-			else if(stack_info->a2dp_exist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
-				algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
-			}
-			else if(stack_info->pan_exist)
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-		else
-		{
-			if( stack_info->hid_exist &&
-				stack_info->a2dp_exist )
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
-				algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
-			}
-			else if( stack_info->hid_exist &&
-				stack_info->pan_exist )
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-			else if( stack_info->pan_exist &&
-				stack_info->a2dp_exist )
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_A2DP_PANHS;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_A2DP;
-				}
-			}
-		}
-	}
-	else if(num_of_diff_profile == 3)
-	{
-		if(stack_info->sco_exist)
-		{
-			if( stack_info->hid_exist &&
-				stack_info->a2dp_exist )
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP ==> HID\n"));
-				algorithm = BT_8812A_1ANT_COEX_ALGO_HID;
-			}
-			else if( stack_info->hid_exist &&
-				stack_info->pan_exist )
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(HS)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(EDR)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-			else if( stack_info->pan_exist &&
-				stack_info->a2dp_exist )
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_SCO;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-		else
-		{
-			if( stack_info->hid_exist &&
-				stack_info->pan_exist &&
-				stack_info->a2dp_exist )
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				}
-			}
-		}
-	}
-	else if(num_of_diff_profile >= 3)
-	{
-		if(stack_info->sco_exist)
-		{
-			if( stack_info->hid_exist &&
-				stack_info->pan_exist &&
-				stack_info->a2dp_exist )
-			{
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
-
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n"));
-					algorithm = BT_8812A_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-	}
-
-	return algorithm;
-}
-
-BOOLEAN
-halbtc8812a1ant_NeedToDecBtPwr(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	BOOLEAN		ret=false;
-	BOOLEAN		bt_hs_on=false, wifi_connected=false;
-	s4Byte		bt_hs_rssi=0;
-
-	if(!btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on))
-		return false;
-	if(!btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected))
-		return false;
-	if(!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
-		return false;
-
-	if(wifi_connected)
-	{
-		if(bt_hs_on)
-		{
-			if(bt_hs_rssi > 37)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for HS mode!!\n"));
-				ret = true;
-			}
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for Wifi is connected!!\n"));
-			ret = true;
-		}
-	}
-	
-	return ret;
-}
-
-void
-halbtc8812a1ant_SetFwDacSwingLevel(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			dac_swing_lvl
-	)
-{
-	u1Byte			h2c_parameter[1] ={0};
-
-	// There are several type of dacswing
-	// 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
-	h2c_parameter[0] = dac_swing_lvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl));
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x64=0x%x\n", h2c_parameter[0]));
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
-}
-
-void
-halbtc8812a1ant_SetFwDecBtPwr(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			dec_bt_pwr
-	)
-{
-	u1Byte	dataLen=3;
-	u1Byte	buf[5] = {0};
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], decrease Bt Power : %s\n", 
-			(dec_bt_pwr? "Yes!!":"No!!")));
-
-	buf[0] = dataLen;
-	buf[1] = 0x3;		// OP_Code
-	buf[2] = 0x1;		// OP_Code_Length
-	if(dec_bt_pwr)
-		buf[3] = 0x1;	// OP_Code_Content
-	else
-		buf[3] = 0x0;
-		
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
-}
-
-void
-halbtc8812a1ant_DecBtPwr(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			dec_bt_pwr
-	)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s Dec BT power = %s\n",  
-		(force_exec? "force to":""), ((dec_bt_pwr)? "ON":"OFF")));
-	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_dec_bt_pwr=%d, cur_dec_bt_pwr=%d\n", 
-			coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr));
-
-		if(coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr) 
-			return;
-	}
-	halbtc8812a1ant_SetFwDecBtPwr(btcoexist, coex_dm->cur_dec_bt_pwr);
-
-	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
-}
-
-void
-halbtc8812a1ant_SetFwBtLnaConstrain(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			bt_lna_cons_on
-	)
-{
-	u1Byte	dataLen=3;
-	u1Byte	buf[5] = {0};
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT LNA Constrain: %s\n", 
-		(bt_lna_cons_on? "ON!!":"OFF!!")));
-
-	buf[0] = dataLen;
-	buf[1] = 0x2;		// OP_Code
-	buf[2] = 0x1;		// OP_Code_Length
-	if(bt_lna_cons_on)
-		buf[3] = 0x1;	// OP_Code_Content
-	else
-		buf[3] = 0x0;
-		
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
-}
-
-void
-halbtc8812a1ant_SetBtLnaConstrain(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			bt_lna_cons_on
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Constrain = %s\n",  
-		(force_exec? "force":""), ((bt_lna_cons_on)? "ON":"OFF")));
-	coex_dm->bCurBtLnaConstrain = bt_lna_cons_on;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtLnaConstrain=%d, bCurBtLnaConstrain=%d\n", 
-			coex_dm->bPreBtLnaConstrain, coex_dm->bCurBtLnaConstrain));
-
-		if(coex_dm->bPreBtLnaConstrain == coex_dm->bCurBtLnaConstrain) 
-			return;
-	}
-	halbtc8812a1ant_SetFwBtLnaConstrain(btcoexist, coex_dm->bCurBtLnaConstrain);
-
-	coex_dm->bPreBtLnaConstrain = coex_dm->bCurBtLnaConstrain;
-}
-
-void
-halbtc8812a1ant_SetFwBtPsdMode(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			bt_psd_mode
-	)
-{
-	u1Byte	dataLen=3;
-	u1Byte	buf[5] = {0};
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT PSD mode=0x%x\n", 
-		bt_psd_mode));
-
-	buf[0] = dataLen;
-	buf[1] = 0x4;			// OP_Code
-	buf[2] = 0x1;			// OP_Code_Length
-	buf[3] = bt_psd_mode;	// OP_Code_Content
-		
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
-}
-
-
-void
-halbtc8812a1ant_SetBtPsdMode(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	u1Byte			bt_psd_mode
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT PSD mode = 0x%x\n",  
-		(force_exec? "force":""), bt_psd_mode));
-	coex_dm->bCurBtPsdMode = bt_psd_mode;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtPsdMode=0x%x, bCurBtPsdMode=0x%x\n", 
-			coex_dm->bPreBtPsdMode, coex_dm->bCurBtPsdMode));
-
-		if(coex_dm->bPreBtPsdMode == coex_dm->bCurBtPsdMode) 
-			return;
-	}
-	halbtc8812a1ant_SetFwBtPsdMode(btcoexist, coex_dm->bCurBtPsdMode);
-
-	coex_dm->bPreBtPsdMode = coex_dm->bCurBtPsdMode;
-}
-
-
-void
-halbtc8812a1ant_SetBtAutoReport(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			enable_auto_report
-	)
-{
-#if 0
-	u1Byte			h2c_parameter[1] ={0};
-	
-	h2c_parameter[0] = 0;
-
-	if(enable_auto_report)
-	{
-		h2c_parameter[0] |= BIT0;
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n", 
-		(enable_auto_report? "Enabled!!":"Disabled!!"), h2c_parameter[0]));
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);	
-#else
-
-#endif
-}
-
-void
-halbtc8812a1ant_BtAutoReport(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			enable_auto_report
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Auto report = %s\n",  
-		(force_exec? "force to":""), ((enable_auto_report)? "Enabled":"Disabled")));
-	coex_dm->cur_bt_auto_report = enable_auto_report;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_bt_auto_report=%d, cur_bt_auto_report=%d\n", 
-			coex_dm->pre_bt_auto_report, coex_dm->cur_bt_auto_report));
-
-		if(coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report) 
-			return;
-	}
-	halbtc8812a1ant_SetBtAutoReport(btcoexist, coex_dm->cur_bt_auto_report);
-
-	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
-}
-
-void
-halbtc8812a1ant_FwDacSwingLvl(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	u1Byte			fw_dac_swing_lvl
-	)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set FW Dac Swing level = %d\n",  
-		(force_exec? "force to":""), fw_dac_swing_lvl));
-	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_fw_dac_swing_lvl=%d, cur_fw_dac_swing_lvl=%d\n", 
-			coex_dm->pre_fw_dac_swing_lvl, coex_dm->cur_fw_dac_swing_lvl));
-
-		if(coex_dm->pre_fw_dac_swing_lvl == coex_dm->cur_fw_dac_swing_lvl) 
-			return;
-	}
-
-	halbtc8812a1ant_SetFwDacSwingLevel(btcoexist, coex_dm->cur_fw_dac_swing_lvl);
-
-	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
-}
-
-void
-halbtc8812a1ant_SetSwRfRxLpfCorner(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			rx_rf_shrink_on
-	)
-{
-	if(rx_rf_shrink_on)
-	{
-		//Shrink RF Rx LPF corner
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
-	}
-	else
-	{
-		//Resume RF Rx LPF corner
-		// After initialized, we can use coex_dm->bt_rf0x1e_backup
-		if(btcoexist->bInitilized)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff, coex_dm->bt_rf0x1e_backup);
-		}
-	}
-}
-
-void
-halbtc8812a1ant_RfShrink(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			rx_rf_shrink_on
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
-		(force_exec? "force to":""), ((rx_rf_shrink_on)? "ON":"OFF")));
-	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_rf_rx_lpf_shrink=%d, cur_rf_rx_lpf_shrink=%d\n", 
-			coex_dm->pre_rf_rx_lpf_shrink, coex_dm->cur_rf_rx_lpf_shrink));
-
-		if(coex_dm->pre_rf_rx_lpf_shrink == coex_dm->cur_rf_rx_lpf_shrink) 
-			return;
-	}
-	halbtc8812a1ant_SetSwRfRxLpfCorner(btcoexist, coex_dm->cur_rf_rx_lpf_shrink);
-
-	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
-}
-
-void
-halbtc8812a1ant_SetSwPenaltyTxRateAdaptive(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			low_penalty_ra
-	)
-{
-	u1Byte	u1_tmp;
-
-	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x4fd);
-	u1_tmp |= BIT0;
-	if(low_penalty_ra)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
-		u1_tmp &= ~BIT2;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
-		u1_tmp |= BIT2;
-	}
-
-	btcoexist->btc_write_1byte(btcoexist, 0x4fd, u1_tmp);
-}
-
-void
-halbtc8812a1ant_LowPenaltyRa(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			low_penalty_ra
-	)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
-		(force_exec? "force to":""), ((low_penalty_ra)? "ON":"OFF")));
-	coex_dm->cur_low_penalty_ra = low_penalty_ra;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_low_penalty_ra=%d, cur_low_penalty_ra=%d\n", 
-			coex_dm->pre_low_penalty_ra, coex_dm->cur_low_penalty_ra));
-
-		if(coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra) 
-			return;
-	}
-	halbtc8812a1ant_SetSwPenaltyTxRateAdaptive(btcoexist, coex_dm->cur_low_penalty_ra);
-
-	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
-}
-
-void
-halbtc8812a1ant_SetDacSwingReg(
-	 	PBTC_COEXIST		btcoexist,
-	 	u4Byte			level
-	)
-{
-	u1Byte	val=(u1Byte)level;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Write SwDacSwing = 0x%x\n", level));
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc5b, 0x3e, val);
-}
-
-void
-halbtc8812a1ant_SetSwFullTimeDacSwing(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			sw_dac_swing_on,
-	 	u4Byte			sw_dac_swing_lvl
-	)
-{
-	if(sw_dac_swing_on)
-	{
-		halbtc8812a1ant_SetDacSwingReg(btcoexist, sw_dac_swing_lvl);
-	}
-	else
-	{
-		halbtc8812a1ant_SetDacSwingReg(btcoexist, 0x18);
-	}
-}
-
-
-void
-halbtc8812a1ant_DacSwing(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			dac_swing_on,
-	 	u4Byte			dac_swing_lvl
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn DacSwing=%s, dac_swing_lvl=0x%x\n",  
-		(force_exec? "force to":""), ((dac_swing_on)? "ON":"OFF"), dac_swing_lvl));
-	coex_dm->cur_dac_swing_on = dac_swing_on;
-	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_dac_swing_on=%d, pre_dac_swing_lvl=0x%x, cur_dac_swing_on=%d, cur_dac_swing_lvl=0x%x\n", 
-			coex_dm->pre_dac_swing_on, coex_dm->pre_dac_swing_lvl,
-			coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl));
-
-		if( (coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
-			(coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl) )
-			return;
-	}
-	delay_ms(30);
-	halbtc8812a1ant_SetSwFullTimeDacSwing(btcoexist, dac_swing_on, dac_swing_lvl);
-
-	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
-	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
-}
-
-void
-halbtc8812a1ant_SetAdcBackOff(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			adc_back_off
-	)
-{
-	if(adc_back_off)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level On!\n"));
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x3);
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level Off!\n"));
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x8db, 0x60, 0x1);
-	}
-}
-
-void
-halbtc8812a1ant_AdcBackOff(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			adc_back_off
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn AdcBackOff = %s\n",  
-		(force_exec? "force to":""), ((adc_back_off)? "ON":"OFF")));
-	coex_dm->cur_adc_back_off = adc_back_off;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_adc_back_off=%d, cur_adc_back_off=%d\n", 
-			coex_dm->pre_adc_back_off, coex_dm->cur_adc_back_off));
-
-		if(coex_dm->pre_adc_back_off == coex_dm->cur_adc_back_off) 
-			return;
-	}
-	halbtc8812a1ant_SetAdcBackOff(btcoexist, coex_dm->cur_adc_back_off);
-
-	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
-}
-
-void
-halbtc8812a1ant_SetAgcTable(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			agc_table_en
-	)
-{
-	u1Byte		rssi_adjust_val=0;
-	
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
-	if(agc_table_en)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x3fa58);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x37a58);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x2fa58);
-		rssi_adjust_val = 8;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x39258);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x31258);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b, 0xfffff, 0x29258);
-	}
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
-
-	// set rssi_adjust_val for wifi module.
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssi_adjust_val);
-}
-
-
-void
-halbtc8812a1ant_AgcTable(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			agc_table_en
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s %s Agc Table\n",  
-		(force_exec? "force to":""), ((agc_table_en)? "Enable":"Disable")));
-	coex_dm->cur_agc_table_en = agc_table_en;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_agc_table_en=%d, cur_agc_table_en=%d\n", 
-			coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en));
-
-		if(coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en) 
-			return;
-	}
-	halbtc8812a1ant_SetAgcTable(btcoexist, agc_table_en);
-
-	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
-}
-
-void
-halbtc8812a1ant_SetCoexTable(
-	 	PBTC_COEXIST	btcoexist,
-	 	u4Byte		val0x6c0,
-	 	u4Byte		val0x6c4,
-	 	u4Byte		val0x6c8,
-	 	u1Byte		val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
-	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c4=0x%x\n", val0x6c4));
-	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
-	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
-	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
-}
-
-void
-halbtc8812a1ant_CoexTable(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	u4Byte			val0x6c0,
-	 	u4Byte			val0x6c4,
-	 	u4Byte			val0x6c8,
-	 	u1Byte			val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
-		(force_exec? "force to":""), val0x6c0, val0x6c4, val0x6c8, val0x6cc));
-	coex_dm->cur_val0x6c0 = val0x6c0;
-	coex_dm->cur_val0x6c4 = val0x6c4;
-	coex_dm->cur_val0x6c8 = val0x6c8;
-	coex_dm->cur_val0x6cc = val0x6cc;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], pre_val0x6c0=0x%x, pre_val0x6c4=0x%x, pre_val0x6c8=0x%x, pre_val0x6cc=0x%x !!\n", 
-			coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4, coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], cur_val0x6c0=0x%x, cur_val0x6c4=0x%x, cur_val0x6c8=0x%x, cur_val0x6cc=0x%x !!\n", 
-			coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4, coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc));
-	
-		if( (coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
-			(coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
-			(coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
-			(coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc) )
-			return;
-	}
-	halbtc8812a1ant_SetCoexTable(btcoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc);
-
-	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
-	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
-	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
-	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
-}
-
-void
-halbtc8812a1ant_SetFwIgnoreWlanAct(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			enable
-	)
-{
-	u1Byte	dataLen=3;
-	u1Byte	buf[5] = {0};
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], %s BT Ignore Wlan_Act\n",
-		(enable? "Enable":"Disable")));
-
-	buf[0] = dataLen;
-	buf[1] = 0x1;			// OP_Code
-	buf[2] = 0x1;			// OP_Code_Length
-	if(enable)
-		buf[3] = 0x1; 		// OP_Code_Content
-	else
-		buf[3] = 0x0;
-		
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);	
-}
-
-void
-halbtc8812a1ant_IgnoreWlanAct(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			enable
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
-		(force_exec? "force to":""), (enable? "ON":"OFF")));
-	coex_dm->cur_ignore_wlan_act = enable;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_ignore_wlan_act = %d, cur_ignore_wlan_act = %d!!\n", 
-			coex_dm->pre_ignore_wlan_act, coex_dm->cur_ignore_wlan_act));
-
-		if(coex_dm->pre_ignore_wlan_act == coex_dm->cur_ignore_wlan_act)
-			return;
-	}
-	halbtc8812a1ant_SetFwIgnoreWlanAct(btcoexist, enable);
-
-	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
-}
-
-void
-halbtc8812a1ant_SetFwPstdma(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			byte1,
-	 	u1Byte			byte2,
-	 	u1Byte			byte3,
-	 	u1Byte			byte4,
-	 	u1Byte			byte5
-	)
-{
-	u1Byte			h2c_parameter[5] ={0};
-
-	h2c_parameter[0] = byte1;	
-	h2c_parameter[1] = byte2;	
-	h2c_parameter[2] = byte3;
-	h2c_parameter[3] = byte4;
-	h2c_parameter[4] = byte5;
-
-	coex_dm->ps_tdma_para[0] = byte1;
-	coex_dm->ps_tdma_para[1] = byte2;
-	coex_dm->ps_tdma_para[2] = byte3;
-	coex_dm->ps_tdma_para[3] = byte4;
-	coex_dm->ps_tdma_para[4] = byte5;
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n", 
-		h2c_parameter[0], 
-		h2c_parameter[1]<<24|h2c_parameter[2]<<16|h2c_parameter[3]<<8|h2c_parameter[4]));
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
-}
-
-void
-halbtc8812a1ant_SetLpsRpwm(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			lps_val,
-	 	u1Byte			rpwm_val
-	)
-{
-	u1Byte	lps=lps_val;
-	u1Byte	rpwm=rpwm_val;
-	
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_LPS, &lps);
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
-	
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT, NULL);
-}
-
-void
-halbtc8812a1ant_LpsRpwm(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	u1Byte			lps_val,
-	 	u1Byte			rpwm_val
-	)
-{
-	BOOLEAN	bForceExecPwrCmd=false;
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set lps/rpwm=0x%x/0x%x \n", 
-		(force_exec? "force to":""), lps_val, rpwm_val));
-	coex_dm->cur_lps = lps_val;
-	coex_dm->cur_rpwm = rpwm_val;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_lps/cur_lps=0x%x/0x%x, pre_rpwm/cur_rpwm=0x%x/0x%x!!\n", 
-			coex_dm->pre_lps, coex_dm->cur_lps, coex_dm->pre_rpwm, coex_dm->cur_rpwm));
-
-		if( (coex_dm->pre_lps == coex_dm->cur_lps) &&
-			(coex_dm->pre_rpwm == coex_dm->cur_rpwm) )
-		{
-			return;
-		}
-	}
-	halbtc8812a1ant_SetLpsRpwm(btcoexist, lps_val, rpwm_val);
-
-	coex_dm->pre_lps = coex_dm->cur_lps;
-	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
-}
-
-void
-halbtc8812a1ant_SwMechanism1(
-	 	PBTC_COEXIST	btcoexist,	
-	 	BOOLEAN		shrink_rx_lpf,
-	 	BOOLEAN 	low_penalty_ra,
-	 	BOOLEAN		limited_dig, 
-	 	BOOLEAN		bt_lna_constrain
-	) 
-{
-	//halbtc8812a1ant_RfShrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
-	//halbtc8812a1ant_LowPenaltyRa(btcoexist, NORMAL_EXEC, low_penalty_ra);
-
-	//no limited DIG
-	//halbtc8812a1ant_SetBtLnaConstrain(btcoexist, NORMAL_EXEC, bt_lna_constrain);
-}
-
-void
-halbtc8812a1ant_SwMechanism2(
-	 	PBTC_COEXIST	btcoexist,	
-	 	BOOLEAN		agc_table_shift,
-	 	BOOLEAN 	adc_back_off,
-	 	BOOLEAN		sw_dac_swing,
-	 	u4Byte		dac_swing_lvl
-	)
-{
-	//halbtc8812a1ant_AgcTable(btcoexist, NORMAL_EXEC, agc_table_shift);
-	//halbtc8812a1ant_AdcBackOff(btcoexist, NORMAL_EXEC, adc_back_off);
-	//halbtc8812a1ant_DacSwing(btcoexist, NORMAL_EXEC, sw_dac_swing, dac_swing_lvl);
-}
-
-void
-halbtc8812a1ant_PsTdma(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			force_exec,
-	 	BOOLEAN			turn_on,
-	 	u1Byte			type
-	)
-{
-	BOOLEAN			bTurnOnByCnt=false;
-	u1Byte			psTdmaTypeByCnt=0, rssi_adjust_val=0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
-		(force_exec? "force to":""), (turn_on? "ON":"OFF"), type));
-	coex_dm->cur_ps_tdma_on = turn_on;
-	coex_dm->cur_ps_tdma = type;
-
-	if(!force_exec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_ps_tdma_on = %d, cur_ps_tdma_on = %d!!\n", 
-			coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], pre_ps_tdma = %d, cur_ps_tdma = %d!!\n", 
-			coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma));
-
-		if( (coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
-			(coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma) )
-			return;
-	}
-	if(turn_on)
-	{
-		switch(type)
-		{
-			default:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x58);
-				break;
-			case 1:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x48);
-				rssi_adjust_val = 11;
-				break;
-			case 2:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x12, 0x12, 0x0, 0x48);
-				rssi_adjust_val = 14;
-				break;
-			case 3:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x40);
-				break;
-			case 4:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
-				rssi_adjust_val = 17;
-				break;
-			case 5:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x61, 0x15, 0x3, 0x31, 0x0);
-				break;
-			case 6:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xa, 0x3, 0x0, 0x0);
-				break;
-			case 7:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
-				break;
-			case 8:	
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
-				break;
-			case 9:	
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0xa, 0xa, 0x0, 0x48);
-				rssi_adjust_val = 18;
-				break;
-			case 10:	
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
-				break;
-			case 11:	
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x5, 0x5, 0x0, 0x48);
-				rssi_adjust_val = 20;
-				break;
-			case 12:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xeb, 0xa, 0x3, 0x31, 0x18);
-				break;
-
-			case 15:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0xa, 0x3, 0x8, 0x0);
-				break;
-			case 16:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x15, 0x3, 0x10, 0x0);
-				rssi_adjust_val = 18;
-				break;
-
-			case 18:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
-				rssi_adjust_val = 14;
-				break;			
-				
-			case 20:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0x25, 0x25, 0x0, 0x0);
-				break;
-			case 21:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x20, 0x3, 0x10, 0x40);
-				break;
-			case 22:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x13, 0x8, 0x8, 0x0, 0x40);
-				break;
-			case 23:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x18);
-				rssi_adjust_val = 22;
-				break;
-			case 24:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x15, 0x3, 0x31, 0x18);
-				rssi_adjust_val = 22;
-				break;
-			case 25:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
-				rssi_adjust_val = 22;
-				break;
-			case 26:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
-				rssi_adjust_val = 22;
-				break;
-			case 27:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x25, 0x3, 0x31, 0x98);
-				rssi_adjust_val = 22;
-				break;
-			case 28:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x69, 0x25, 0x3, 0x31, 0x0);
-				break;
-			case 29:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xab, 0x1a, 0x1a, 0x1, 0x8);
-				break;
-			case 30:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
-				break;
-			case 31:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0, 0x58);
-				break;
-			case 32:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xab, 0xa, 0x3, 0x31, 0x88);
-				break;
-			case 33:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xa3, 0x25, 0x3, 0x30, 0x88);
-				break;
-			case 34:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x8);
-				break;
-			case 35:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xe3, 0x1a, 0x1a, 0x0, 0x8);
-				break;
-			case 36:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0xd3, 0x12, 0x3, 0x14, 0x58);
-				break;
-		}
-	}
-	else
-	{
-		// disable PS tdma
-		switch(type)
-		{
-			case 8:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
-				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x4);
-				break;
-			case 0:
-			default:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
-				delay_ms(5);
-				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
-				break;
-			case 9:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
-				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x4);
-				break;
-			case 10:
-				halbtc8812a1ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
-				delay_ms(5);
-				btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
-				break;
-		}
-	}
-	rssi_adjust_val =0;
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssi_adjust_val);
-
-	// update pre state
-	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
-	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
-}
-
-void
-halbtc8812a1ant_CoexAllOff(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	// fw all off
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	// sw all off
-	halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
-	halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-
-
-	// hw all off
-	halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-}
-
-void
-halbtc8812a1ant_WifiParaAdjust(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			enable
-	)
-{
-	if(enable)
-	{
-		halbtc8812a1ant_LowPenaltyRa(btcoexist, NORMAL_EXEC, true);
-	}
-	else
-	{
-		halbtc8812a1ant_LowPenaltyRa(btcoexist, NORMAL_EXEC, false);
-	}
-}
-
-BOOLEAN
-halbtc8812a1ant_IsCommonAction(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	BOOLEAN			common=false, wifi_connected=false, wifi_busy=false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-
-	//halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-
-	if(!wifi_connected && 
-		BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n"));
-		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-		
- 		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
-		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-
-		common = true;
-	}
-	else if(wifi_connected && 
-		(BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT non connected-idle!!\n"));
-		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-
-      		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
-		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-
-		common = true;
-	}
-	else if(!wifi_connected && 
-		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT connected-idle!!\n"));
-		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
-		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-
-		common = true;
-	}
-	else if(wifi_connected && 
-		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
-		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-		halbtc8812a1ant_SwMechanism1(btcoexist,true,true,true,true);
-		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-
-		common = true;
-	}
-	else if(!wifi_connected && 
-		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE != coex_dm->bt_status) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT Busy!!\n"));
-		halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-		halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
-		halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		
-		common = true;
-	}
-	else
-	{
-		halbtc8812a1ant_SwMechanism1(btcoexist,true,true,true,true);
-		
-		common = false;
-	}
-	
-	return common;
-}
-
-
-void
-halbtc8812a1ant_TdmaDurationAdjustForAcl(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	static s4Byte		up,dn,m,n,wait_count;
-	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
-	u1Byte			retry_count=0, bt_info_ext;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], halbtc8812a1ant_TdmaDurationAdjustForAcl()\n"));
-	if(coex_dm->reset_tdma_adjust)
-	{
-		coex_dm->reset_tdma_adjust = false;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
-
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-		coex_dm->ps_tdma_du_adj_type = 2;
-		//============
-		up = 0;
-		dn = 0;
-		m = 1;
-		n= 3;
-		result = 0;
-		wait_count = 0;
-	}
-	else
-	{
-		//acquire the BT TRx retry count from BT_Info byte2
-		retry_count = coex_sta->bt_retry_cnt;
-		bt_info_ext = coex_sta->bt_info_ext;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retry_count = %d\n", retry_count));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], up=%d, dn=%d, m=%d, n=%d, wait_count=%d\n", 
-			up, dn, m, n, wait_count));
-		result = 0;
-		wait_count++; 
-		  
-		if(retry_count == 0)  // no retry in the last 2-second duration
-		{
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0;				 
-
-			if(up >= n)	// Google translated: if consecutive n-2 seconds retry count is 0, width-modulated WiFi duration
-			{
-				wait_count = 0; 
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
-			}
-		}
-		else if (retry_count <= 3)	// <=3 retry in the last 2-second duration
-		{
-			up--; 
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2)	// Google translated: if 2 consecutive two seconds retry count <3, then tune narrow WiFi duration
-			{
-				if (wait_count <= 2)
-					m++; // Google translated: Avoid been back and forth in the two level
-				else
-					m = 1;
-
-				if ( m >= 20) // Google translated: m max = 20 'Max 120 seconds recheck whether to adjust WiFi duration.
-					m = 20;
-
-				n = 3*m;
-				up = 0;
-				dn = 0;
-				wait_count = 0;
-				result = -1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
-			}
-		}
-		else  // Google translated: retry count> 3, as long as a second retry count> 3, then tune narrow WiFi duration
-		{
-			if (wait_count == 1)
-				m++; // Google translated: Avoid been back and forth in the two level 
-			else
-				m = 1;
-
-			if ( m >= 20) // Google translated: m max = 20 'Max 120 seconds recheck whether to adjust WiFi duration.
-				m = 20;
-
-			n = 3*m;
-			up = 0;
-			dn = 0;
-			wait_count = 0; 
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
-		}
-
-		if(result == -1)
-		{
-			if( (BT_INFO_8812A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
-				((coex_dm->cur_ps_tdma == 1) ||(coex_dm->cur_ps_tdma == 2)) )
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			}
-			else if(coex_dm->cur_ps_tdma == 1)
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
-			}
-			else if(coex_dm->cur_ps_tdma == 2)
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			}
-			else if(coex_dm->cur_ps_tdma == 9)
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 11);
-				coex_dm->ps_tdma_du_adj_type = 11;
-			}
-		}
-		else if(result == 1)
-		{
-			if( (BT_INFO_8812A_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
-				((coex_dm->cur_ps_tdma == 1) ||(coex_dm->cur_ps_tdma == 2)) )
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			}
-			else if(coex_dm->cur_ps_tdma == 11)
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			}
-			else if(coex_dm->cur_ps_tdma == 9)
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
-			}
-			else if(coex_dm->cur_ps_tdma == 2)
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 1);
-				coex_dm->ps_tdma_du_adj_type = 1;
-			}
-		}
-
-		if( coex_dm->cur_ps_tdma != 1 &&
-			coex_dm->cur_ps_tdma != 2 &&
-			coex_dm->cur_ps_tdma != 9 &&
-			coex_dm->cur_ps_tdma != 11 )
-		{
-			// recover to previous adjust type
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, coex_dm->ps_tdma_du_adj_type);
-		}
-	}
-}
-
-u1Byte
-halbtc8812a1ant_PsTdmaTypeByWifiRssi(
-	 	s4Byte	wifi_rssi,
-	 	s4Byte	pre_wifi_rssi,
-	 	u1Byte	wifi_rssi_thresh
-	)
-{
-	u1Byte	ps_tdma_type=0;
-	
-	if(wifi_rssi > pre_wifi_rssi)
-	{
-		if(wifi_rssi > (wifi_rssi_thresh+5))
-		{
-			ps_tdma_type = 26;
-		}
-		else
-		{
-			ps_tdma_type = 25;
-		}
-	}
-	else
-	{
-		if(wifi_rssi > wifi_rssi_thresh)
-		{
-			ps_tdma_type = 26;
-		}
-		else
-		{
-			ps_tdma_type = 25;
-		}
-	}
-
-	return ps_tdma_type;
-}
-
-void
-halbtc8812a1ant_PsTdmaCheckForPowerSaveState(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			new_ps_state
-	)
-{
-	u1Byte	lps_mode=0x0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_LPS_MODE, &lps_mode);
-	
-	if(lps_mode)	// already under LPS state
-	{
-		if(new_ps_state)		
-		{
-			// keep state under LPS, do nothing.
-		}
-		else
-		{
-			// will leave LPS state, turn off psTdma first
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
-		}
-	}
-	else						// NO PS state
-	{
-		if(new_ps_state)
-		{
-			// will enter LPS state, turn off psTdma first
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
-		}
-		else
-		{
-			// keep state under NO PS state, do nothing.
-		}
-	}
-}
-
-// SCO only or SCO+PAN(HS)
-void
-halbtc8812a1ant_ActionSco(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte	wifi_rssi_state;
-	u4Byte	wifi_bw;
-
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 4);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-	
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			  halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			  halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}		
-	}
-}
-
-
-void
-halbtc8812a1ant_ActionHid(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte	wifi_rssi_state, bt_rssi_state;	
-	u4Byte	wifi_bw;
-
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,false,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}		
-	}
-}
-
-//A2DP only / PAN(EDR) only/ A2DP+PAN(HS)
-void
-halbtc8812a1ant_ActionA2dp(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state;
-	u4Byte		wifi_bw;
-
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}		
-	}
-}
-
-void
-halbtc8812a1ant_ActionA2dpPanHs(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u4Byte		wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}		
-	}
-}
-
-void
-halbtc8812a1ant_ActionPanEdr(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state;
-	u4Byte		wifi_bw;
-
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}
-	}
-}
-
-
-//PAN(HS) only
-void
-halbtc8812a1ant_ActionPanHs(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state;
-	u4Byte		wifi_bw;
-
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// fw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-		}
-		else
-		{
-			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-		}
-
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// fw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-		}
-		else
-		{
-			halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-		}
-
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}
-	}
-}
-
-//PAN(EDR)+A2DP
-void
-halbtc8812a1ant_ActionPanEdrA2dp(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u4Byte		wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}
-	}
-}
-
-void
-halbtc8812a1ant_ActionPanEdrHid(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state;
-	u4Byte		wifi_bw;
-
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{		
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}
-	}
-}
-
-// HID+A2DP+PAN(EDR)
-void
-halbtc8812a1ant_ActionHidA2dpPanEdr(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u4Byte		wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, NORMAL_EXEC, 6);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{	
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}
-	}
-}
-
-void
-halbtc8812a1ant_ActionHidA2dp(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u1Byte		wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u4Byte		wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8812a1ant_WifiRssiState(btcoexist, 0, 2, 25, 0);
-	bt_rssi_state = halbtc8812a1ant_BtRssiState(2, 50, 0);
-
-	if(halbtc8812a1ant_NeedToDecBtPwr(btcoexist))
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8812a1ant_DecBtPwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 == wifi_bw)
-	{		
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,true,false,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,true,true,false,0x18);
-		}
-		else
-		{
-			halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-		}
-	}
-}
-
-void
-halbtc8812a1ant_ActionHs(
-	 	PBTC_COEXIST		btcoexist,
-	 	BOOLEAN			hs_connecting
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action for HS, hs_connecting=%d!!!\n", hs_connecting));
-	halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-
-	if(hs_connecting)
-	{
-		halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0xaaaaaaaa, 0xaaaaaaaa, 0xffff, 0x3);
-	}
-	else
-	{
-		if((coex_sta->high_priority_tx+coex_sta->high_priority_rx+
-			coex_sta->low_priority_tx+coex_sta->low_priority_rx)<=1200)
-			halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0xaaaaaaaa, 0xaaaaaaaa, 0xffff, 0x3);
-		else
-			halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0xffffffff, 0xffffffff, 0xffff, 0x3);	
-	}
-}
-
-
-void
-halbtc8812a1ant_ActionWifiNotConnected(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	BOOLEAN		hs_connecting=false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
-	
-	halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-
-	if(hs_connecting)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HS is connecting!!!\n"));
-		halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
-	}
-	else
-	{
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-	}
-}
-
-void
-halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
-	BOOLEAN			hs_connecting=false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
-	halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-
-	if(hs_connecting)
-	{
-		halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
-	}
-	else if(btcoexist->bt_info.bt_disabled)
-	{
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-	}
-	else if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
-{
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-	}
-	else if( (BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ||
-		(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
-	{
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 28);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-	}
-	else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
-	{
-		if(stack_info->hid_exist)
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 35);
-		else
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 29);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-	}
-	else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-		(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
-		{
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
-	}
-	else 
-	{
-		//error condition, should not reach here, record error number for debugging.
-		coex_dm->error_condition = 1;
-	}
-}
-
-void
-halbtc8812a1ant_ActionWifiConnectedScan(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ActionConnectedScan()===>\n"));
-
-	if(btcoexist->bt_info.bt_disabled)
-	{
-		halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-	}
-	else
-	{
-		halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-		halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
-		// power save must executed before psTdma.
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-
-		// psTdma
-		if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ActionConnectedScan(), bt is under inquiry/page scan\n"));
-			if(stack_info->sco_exist)
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-			}
-			else
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-			}
-		}
-		else if( (BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ||
-			(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
-		{
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 5);
-			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-		}
-		else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
-		{
-			if(stack_info->hid_exist)
-		{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 34);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-			}
-			else
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 4);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-			}
-		}
-		else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-			(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
-		{
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 33);
-			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-		}
-		else 
-		{
-			//error condition, should not reach here
-			coex_dm->error_condition = 2;
-		}
-	}
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ActionConnectedScan()<===\n"));
-}
-
-void
-halbtc8812a1ant_ActionWifiConnectedSpecialPacket(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
-
-	halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-
-	if(btcoexist->bt_info.bt_disabled)
-	{	
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-	}
-	else
-	{
-		if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
-		{
-			if(stack_info->sco_exist)
-		{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-			}
-			else
-			{
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-			}
-		}
-		else if( (BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) ||
-			(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status) )
-		{
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 28);
-			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-		}
-		else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
-		{
-			if(stack_info->hid_exist)
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 35);
-			else
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 29);
-			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-		}
-		else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-			(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
-		{
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
-		}
-		else 
-		{
-			//error condition, should not reach here
-			coex_dm->error_condition = 3;
-		}
-	}
-}
-
-void
-halbtc8812a1ant_ActionWifiConnected(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_STACK_INFO	stack_info=&btcoexist->stack_info;
-	BOOLEAN 	wifi_connected=false, wifi_busy=false, bt_hs_on=false;
-	BOOLEAN		scan=false, link=false, roam=false;
-	BOOLEAN		hs_connecting=false, under4way=false;
-	u4Byte		wifi_bw;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect()===>\n"));
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
-	if(!wifi_connected)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi not connected<===\n"));
-		return;
-	}
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &under4way);
-	if(under4way)
-	{
-		halbtc8812a1ant_ActionWifiConnectedSpecialPacket(btcoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n"));
-		return;
-	}
-	
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	if(scan || link || roam)
-	{
-		halbtc8812a1ant_ActionWifiConnectedScan(btcoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n"));
-		return;
-	}
-	
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);	
-	if(!wifi_busy)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi associated-idle!!!\n"));
-		if(btcoexist->bt_info.bt_disabled)
-		{
-			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-			halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-		}
-		else
-		{
-			if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], bt is under inquiry/page scan!!!\n"));
-				if(stack_info->sco_exist)
-				{
-					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
-					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-				}
-				else
-				{
-					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
-					// power save must executed before psTdma.
-					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
-					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-				}
-			}
-			else if(BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)
-			{
-				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-				halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x26, 0x0);
-				// power save must executed before psTdma.
-				btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-			}
-			else if(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)
-			{
-				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-				halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x26, 0x0);
-				// power save must executed before psTdma.
-				btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-			}
-			else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
-			{
-				if(stack_info->hid_exist && stack_info->numOfLink==1)
-				{
-					// hid only
-					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-					// power save must executed before psTdma.
-					btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-					
-					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5fff5fff, 0x5fff5fff, 0xffff, 0x3);
-					coex_dm->reset_tdma_adjust = true;
-				}
-				else
-				{				
-					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
-					// power save must executed before psTdma.
-					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-
-					if(stack_info->hid_exist)
-					{
-						if(stack_info->a2dp_exist)
-						{
-							// hid+a2dp
-							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-						else if(stack_info->pan_exist)
-						{
-							if(bt_hs_on)
-							{
-								// hid+hs
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							}
-							else
-							{
-								// hid+pan
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							}
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-						else
-						{
-							coex_dm->error_condition = 4;
-						}
-						coex_dm->reset_tdma_adjust = true;
-					}
-					else if(stack_info->a2dp_exist)
-					{
-						if(stack_info->pan_exist)
-						{
-							if(bt_hs_on)
-							{
-								// a2dp+hs
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							}
-							else
-							{
-								// a2dp+pan
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 36);
-							}
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-							coex_dm->reset_tdma_adjust = true;
-						}
-						else
-						{
-							// a2dp only
-							halbtc8812a1ant_TdmaDurationAdjustForAcl(btcoexist);
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-					}
-					else if(stack_info->pan_exist)
-					{
-						// pan only
-						if(bt_hs_on)
-						{
-							coex_dm->error_condition = 5;
-						}
-						else
-						{
-							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-						coex_dm->reset_tdma_adjust = true;
-					}
-					else
-					{
-						// temp state, do nothing!!!
-						//DbgPrint("error 6, coex_dm->bt_status=%d\n", coex_dm->bt_status);
-						//DbgPrint("error 6, stack_info->numOfLink=%d, stack_info->hid_exist=%d, stack_info->a2dp_exist=%d, stack_info->pan_exist=%d, stack_info->sco_exist=%d\n", 
-							//stack_info->numOfLink, stack_info->hid_exist, stack_info->a2dp_exist, stack_info->pan_exist, stack_info->sco_exist);
-						//coex_dm->error_condition = 6;
-					}
-				}
-			}
-			else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-				(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
-			{
-				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-				// power save must executed before psTdma.
-				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
-			}
-			else 
-			{
-				//error condition, should not reach here
-				coex_dm->error_condition = 7;
-			}
-		}
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi busy!!!\n"));
-		if(btcoexist->bt_info.bt_disabled)
-		{
-			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-			halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-		}
-		else
-		{
-			if(bt_hs_on)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HS is under progress!!!\n"));
-				//DbgPrint("coex_dm->bt_status = 0x%x\n", coex_dm->bt_status);
-				halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
-			}
-			else if(BT_8812A_1ANT_BT_STATUS_INQ_PAGE == coex_dm->bt_status)
-			{
-				if(stack_info->sco_exist)
-				{
-					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 32);
-					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-				}
-				else
-				{
-					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
-					// power save must executed before psTdma.
-					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 30);
-					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-				}
-			}
-			else if(BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)
-			{
-				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-				// power save must executed before psTdma.
-				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 5);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5a5a5a5a, 0x5a5a5a5a, 0xffff, 0x3);
-			}
-			else if(BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)
-			{
-				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-				// power save must executed before psTdma.
-				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-				if(bt_hs_on)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HS is under progress!!!\n"));
-					halbtc8812a1ant_ActionHs(btcoexist, hs_connecting);
-				}
-				else
-				{
-					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 5);
-					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5a5a5a5a, 0x5a5a5a5a, 0xffff, 0x3);
-				}
-			}
-			else if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status)
-			{
-				if(stack_info->hid_exist && stack_info->numOfLink==1)
-				{
-					// hid only
-					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-					// power save must executed before psTdma.
-					btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-					
-					halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-					halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5fff5fff, 0x5fff5fff, 0xffff, 0x3);
-					coex_dm->reset_tdma_adjust = true;
-				}
-				else
-				{
-					halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-					halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
-					// power save must executed before psTdma.
-					btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-
-					if(stack_info->hid_exist)
-					{
-						if(stack_info->a2dp_exist)
-						{
-							// hid+a2dp
-							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-						else if(stack_info->pan_exist)
-						{
-							if(bt_hs_on)
-							{
-								// hid+hs
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							}
-							else
-							{
-								// hid+pan
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							}
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-						else
-						{
-							coex_dm->error_condition = 8;
-						}
-						coex_dm->reset_tdma_adjust = true;
-					}
-					else if(stack_info->a2dp_exist)
-					{
-						if(stack_info->pan_exist)
-						{
-							if(bt_hs_on)
-							{
-								// a2dp+hs
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							}
-							else
-							{
-								// a2dp+pan
-								halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 36);
-							}
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-							coex_dm->reset_tdma_adjust = true;
-						}
-						else
-						{
-							// a2dp only
-							halbtc8812a1ant_TdmaDurationAdjustForAcl(btcoexist);
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-					}
-					else if(stack_info->pan_exist)
-					{
-						// pan only
-						if(bt_hs_on)
-						{
-							coex_dm->error_condition = 9;
-						}
-						else
-						{
-							halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, true, 2);
-							halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x5afa5afa, 0xffff, 0x3);
-						}
-						coex_dm->reset_tdma_adjust = true;
-					}
-					else
-					{
-						//DbgPrint("error 10, stack_info->numOfLink=%d, stack_info->hid_exist=%d, stack_info->a2dp_exist=%d, stack_info->pan_exist=%d, stack_info->sco_exist=%d\n", 
-							//stack_info->numOfLink, stack_info->hid_exist, stack_info->a2dp_exist, stack_info->pan_exist, stack_info->sco_exist);
-						coex_dm->error_condition = 10;
-					}
-				}
-			}
-			else if( (BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-				(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
-			{
-				halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-				// power save must executed before psTdma.
-				btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-
-				halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 8);
-				halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x5aea5aea, 0x5aea5aea, 0xffff, 0x3);
-			}
-			else 
-			{
-				//DbgPrint("error 11, coex_dm->bt_status=%d\n", coex_dm->bt_status);
-				//DbgPrint("error 11, stack_info->numOfLink=%d, stack_info->hid_exist=%d, stack_info->a2dp_exist=%d, stack_info->pan_exist=%d, stack_info->sco_exist=%d\n", 
-					//stack_info->numOfLink, stack_info->hid_exist, stack_info->a2dp_exist, stack_info->pan_exist, stack_info->sco_exist);
-				//error condition, should not reach here
-				coex_dm->error_condition = 11;
-			}
-		}
-	}
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect()<===\n"));
-}
-
-void
-halbtc8812a1ant_RunSwCoexistMechanism(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
-	BOOLEAN				wifi_under5g=false, wifi_busy=false, wifi_connected=false;
-	u1Byte				bt_info_original=0, bt_retry_cnt=0;
-	u1Byte				algorithm=0;
-
-	return;
-	if(stack_info->bProfileNotified)
-	{
-		algorithm = halbtc8812a1ant_ActionAlgorithm(btcoexist);
-		coex_dm->cur_algorithm = algorithm;		
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Algorithm = %d \n", coex_dm->cur_algorithm));
-
-		if(halbtc8812a1ant_IsCommonAction(btcoexist))
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action common.\n"));
-		}
-		else
-		{
-			switch(coex_dm->cur_algorithm)
-			{
-				case BT_8812A_1ANT_COEX_ALGO_SCO:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = SCO.\n"));
-					halbtc8812a1ant_ActionSco(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_HID:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID.\n"));
-					halbtc8812a1ant_ActionHid(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_A2DP:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP.\n"));
-					halbtc8812a1ant_ActionA2dp(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_A2DP_PANHS:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP+PAN(HS).\n"));
-					halbtc8812a1ant_ActionA2dpPanHs(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_PANEDR:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR).\n"));
-					halbtc8812a1ant_ActionPanEdr(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_PANHS:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HS mode.\n"));
-					halbtc8812a1ant_ActionPanHs(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_PANEDR_A2DP:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN+A2DP.\n"));
-					halbtc8812a1ant_ActionPanEdrA2dp(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_PANEDR_HID:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR)+HID.\n"));
-					halbtc8812a1ant_ActionPanEdrHid(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP+PAN.\n"));
-					halbtc8812a1ant_ActionHidA2dpPanEdr(btcoexist);
-					break;
-				case BT_8812A_1ANT_COEX_ALGO_HID_A2DP:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP.\n"));
-					halbtc8812a1ant_ActionHidA2dp(btcoexist);
-					break;
-				default:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = coexist All Off!!\n"));
-					halbtc8812a1ant_CoexAllOff(btcoexist);
-					break;
-			}
-			coex_dm->pre_algorithm = coex_dm->cur_algorithm;
-		}
-	}
-}
-
-void
-halbtc8812a1ant_RunCoexistMechanism(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
-	BOOLEAN				wifi_under5g=false, wifi_busy=false, wifi_connected=false;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism()===>\n"));
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
-
-	if(wifi_under5g)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for 5G <===\n"));
-		return;
-	}
-
-	if(btcoexist->manual_control)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
-		return;
-	}
-
-	if(btcoexist->stop_coex_dm)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n"));
-		return;
-	}
-
-	halbtc8812a1ant_RunSwCoexistMechanism(btcoexist);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
-	if(btcoexist->bt_info.bt_disabled)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], bt is disabled!!!\n"));
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-		if(wifi_busy)
-		{			
-			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-		}
-		else
-		{
-			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-			halbtc8812a1ant_LpsRpwm(btcoexist, NORMAL_EXEC, 0x0, 0x4);
-			// power save must executed before psTdma.
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-		}
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-	}
-	else if(coex_sta->under_ips)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 0);
-		halbtc8812a1ant_CoexTable(btcoexist, NORMAL_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-		halbtc8812a1ant_WifiParaAdjust(btcoexist, false);
-	}
-	else if(!wifi_connected)
-	{
-		BOOLEAN	scan=false, link=false, roam=false;
-		
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is non connected-idle !!!\n"));
-
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-
-		if(scan || link || roam)
-			halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(btcoexist);
-		else
-			halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
-	}
-	else	// wifi LPS/Busy
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is NOT under IPS!!!\n"));
-		halbtc8812a1ant_WifiParaAdjust(btcoexist, true);
-		halbtc8812a1ant_ActionWifiConnected(btcoexist);
-	}
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism()<===\n"));
-}
-
-void
-halbtc8812a1ant_InitCoexDm(
-	 	PBTC_COEXIST		btcoexist
-	)
-{	
-	BOOLEAN		wifi_connected=false;
-	// force to reset coex mechanism
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
-	if(!wifi_connected) // non-connected scan
-	{
-		halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
-	}
-	else	// wifi is connected
-	{
-		halbtc8812a1ant_ActionWifiConnected(btcoexist);
-	}
-
-	halbtc8812a1ant_FwDacSwingLvl(btcoexist, FORCE_EXEC, 6);
-	halbtc8812a1ant_DecBtPwr(btcoexist, FORCE_EXEC, false);
-
-	// sw all off
-	halbtc8812a1ant_SwMechanism1(btcoexist,false,false,false,false);
-	halbtc8812a1ant_SwMechanism2(btcoexist,false,false,false,0x18);
-
-	halbtc8812a1ant_CoexTable(btcoexist, FORCE_EXEC, 0x55555555, 0x55555555, 0xffff, 0x3);
-}
-
-//============================================================
-// work around function start with wa_halbtc8812a1ant_
-//============================================================
-//============================================================
-// extern function start with EXhalbtc8812a1ant_
-//============================================================
-void
-EXhalbtc8812a1ant_InitHwConfig(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	u4Byte	u4_tmp=0;
-	u2Byte	u2Tmp=0;
-	u1Byte	u1_tmp=0;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 1Ant Init HW Config!!\n"));
-
-	// backup rf 0x1e value
-	coex_dm->bt_rf0x1e_backup = 
-		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
-	
-	//ant sw control to BT
-	 btcoexist->btc_write_4byte(btcoexist, 0x900, 0x00000400);
-	 btcoexist->btc_write_1byte(btcoexist, 0x76d, 0x1);
-	 btcoexist->btc_write_1byte(btcoexist, 0xcb3, 0x77);
-	 btcoexist->btc_write_1byte(btcoexist, 0xcb7, 0x40);	
-
-	// 0x790[5:0]=0x5
-	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
-	u1_tmp &= 0xc0;
-	u1_tmp |= 0x5;
-	btcoexist->btc_write_1byte(btcoexist, 0x790, u1_tmp);
-
-	// PTA parameter
-	btcoexist->btc_write_1byte(btcoexist, 0x6cc, 0x0);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c8, 0xffff);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c4, 0x55555555);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c0, 0x55555555);
-
-	// coex parameters
-	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x1);
-
-	// enable counter statistics
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
-
-	// enable PTA
-	btcoexist->btc_write_1byte(btcoexist, 0x40, 0x20);
-
-	// bt clock related
-	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x4);
-	u1_tmp |= BIT7;
-	btcoexist->btc_write_1byte(btcoexist, 0x4, u1_tmp);
-
-	// bt clock related
-	u1_tmp = btcoexist->btc_read_1byte(btcoexist, 0x7);
-	u1_tmp |= BIT1;
-	btcoexist->btc_write_1byte(btcoexist, 0x7, u1_tmp);
-}
-
-void
-EXhalbtc8812a1ant_InitCoexDm(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
-
-	btcoexist->stop_coex_dm = false;
-
-	halbtc8812a1ant_InitCoexDm(btcoexist);
-}
-
-void
-EXhalbtc8812a1ant_DisplayCoexInfo(
-	 	PBTC_COEXIST		btcoexist
-	)
-{
-	PBTC_BOARD_INFO		board_info=&btcoexist->boardInfo;
-	PBTC_STACK_INFO		stack_info=&btcoexist->stack_info;
-	pu1Byte				cli_buf=btcoexist->cli_buf;
-	u1Byte				u1_tmp[4], i, bt_info_ext, psTdmaCase=0;
-	u4Byte				u4_tmp[4];
-	BOOLEAN				roam=false, scan=false, link=false, wifi_under5g=false;
-	BOOLEAN				bt_hs_on=false, wifi_busy=false;
-	s4Byte				wifi_rssi=0, bt_hs_rssi=0;
-	u4Byte				wifi_bw, wifiTrafficDir;
-	u1Byte				wifiDot11Chnl, wifiHsChnl;
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cli_buf);
-
-	if(btcoexist->manual_control)
-	{
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ============[Under Manual Control]============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
-	}
-	if(btcoexist->stop_coex_dm)
-	{
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ============[Coex is STOPPED]============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
-	}
-
-	if(!board_info->bBtExist)
-	{
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
-		return;
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
-		board_info->pgAntNum, board_info->btdmAntNum);
-	CL_PRINTF(cli_buf);	
-	
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
-		((stack_info->bProfileNotified)? "Yes":"No"), stack_info->hciVersion);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_FW_VER);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
-		wifiDot11Chnl, wifiHsChnl, bt_hs_on);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
-		coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
-		coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
-		wifi_rssi, bt_hs_rssi);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi link/ roam/ scan", \
-		link, roam, scan);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
-		(wifi_under5g? "5G":"2.4G"),
-		((BTC_WIFI_BW_LEGACY==wifi_bw)? "Legacy": (((BTC_WIFI_BW_HT40==wifi_bw)? "HT40":"HT20"))),
-		((!wifi_busy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
-		((coex_sta->c2h_bt_inquiry_page)?("inquiry/page scan"):((BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)? "non-connected idle":
-		(  (BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)? "connected-idle":"busy"))),
-		coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-	CL_PRINTF(cli_buf);
-	
-	if(stack_info->bProfileNotified)
-	{			
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
-			stack_info->sco_exist, stack_info->hid_exist, stack_info->pan_exist, stack_info->a2dp_exist);
-		CL_PRINTF(cli_buf);	
-
-		btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
-	}
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
-		(bt_info_ext&BIT0)? "Basic rate":"EDR rate");
-	CL_PRINTF(cli_buf);	
-
-	for(i=0; i<BT_INFO_SRC_8812A_1ANT_MAX; i++)
-	{
-		if(coex_sta->bt_info_c2h_cnt[i])
-		{				
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8812a1Ant[i], \
-				coex_sta->bt_info_c2h[i][0], coex_sta->bt_info_c2h[i][1],
-				coex_sta->bt_info_c2h[i][2], coex_sta->bt_info_c2h[i][3],
-				coex_sta->bt_info_c2h[i][4], coex_sta->bt_info_c2h[i][5],
-				coex_sta->bt_info_c2h[i][6], coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
-		}
-	}
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s, (0x%x/0x%x)", "PS state, IPS/LPS, (lps/rpwm)", \
-		((coex_sta->under_ips? "IPS ON":"IPS OFF")),
-		((coex_sta->under_lps? "LPS ON":"LPS OFF")), 
-		btcoexist->bt_info.lps1Ant, 
-		btcoexist->bt_info.rpwm1Ant);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
-
-	if(!btcoexist->manual_control)
-	{
-		// Sw mechanism	
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
-		CL_PRINTF(cli_buf);
-	
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d/ %d ", "SM1[ShRf/ LpRA/ LimDig/ btLna]", \
-			coex_dm->cur_rf_rx_lpf_shrink, coex_dm->cur_low_penalty_ra, coex_dm->limited_dig, coex_dm->bCurBtLnaConstrain);
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ", "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
-			coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off, coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
-		CL_PRINTF(cli_buf);
-	
-		// Fw mechanism		
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
-		CL_PRINTF(cli_buf);	
-
-		psTdmaCase = coex_dm->cur_ps_tdma;
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d", "PS TDMA", \
-			coex_dm->ps_tdma_para[0], coex_dm->ps_tdma_para[1],
-			coex_dm->ps_tdma_para[2], coex_dm->ps_tdma_para[3],
-			coex_dm->ps_tdma_para[4], psTdmaCase);
-		CL_PRINTF(cli_buf);
-
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Latest error condition(should be 0)", \
-			coex_dm->error_condition);
-		CL_PRINTF(cli_buf);
-		
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct", \
-			coex_dm->cur_dec_bt_pwr, coex_dm->cur_ignore_wlan_act);
-		CL_PRINTF(cli_buf);
-	}
-
-	// Hw setting		
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
-	CL_PRINTF(cli_buf);	
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
-		coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
-
-	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778", \
-		u1_tmp[0]);
-	CL_PRINTF(cli_buf);
-	
-	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x92c);
-	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x930);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x92c/ 0x930", \
-		(u1_tmp[0]), u4_tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
-	u1_tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x4f);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x40/ 0x4f", \
-		u1_tmp[0], u1_tmp[1]);
-	CL_PRINTF(cli_buf);
-
-	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
-	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
-		u4_tmp[0], u1_tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
-		u4_tmp[0]);
-	CL_PRINTF(cli_buf);
-
-#if 0
-	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xf48);
-	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xf4c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0xf48/ 0xf4c (FA cnt)", \
-		u4_tmp[0], u4_tmp[1]);
-	CL_PRINTF(cli_buf);
-#endif
-
-	u4_tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
-	u4_tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
-	u4_tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
-	u1_tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
-		u4_tmp[0], u4_tmp[1], u4_tmp[2], u1_tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770(hp rx[31:16]/tx[15:0])", \
-		coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
-		coex_sta->low_priority_rx, coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
-	
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-
-void
-EXhalbtc8812a1ant_IpsNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			type
-	)
-{
-	u4Byte	u4_tmp=0;
-
-	if(btcoexist->manual_control ||	btcoexist->stop_coex_dm)
-		return;
-
-	if(BTC_IPS_ENTER == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
-		coex_sta->under_ips = true;
-
-		// 0x4c[23]=1
-		u4_tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u4_tmp |= BIT23;
-		btcoexist->btc_write_4byte(btcoexist, 0x4c, u4_tmp);
-		
-		halbtc8812a1ant_CoexAllOff(btcoexist);
-	}
-	else if(BTC_IPS_LEAVE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
-		coex_sta->under_ips = false;
-		//halbtc8812a1ant_InitCoexDm(btcoexist);
-	}
-}
-
-void
-EXhalbtc8812a1ant_LpsNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			type
-	)
-{
-	if(btcoexist->manual_control || btcoexist->stop_coex_dm)
-		return;
-
-	if(BTC_LPS_ENABLE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
-		coex_sta->under_lps = true;
-	}
-	else if(BTC_IPS_LEAVE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
-		coex_sta->under_lps = false;
-	}
-}
-
-void
-EXhalbtc8812a1ant_ScanNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			type
-	)
-{
-	PBTC_STACK_INFO 	stack_info=&btcoexist->stack_info;
-	BOOLEAN 		wifi_connected=false;	
-
-	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
-		return;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
-	if(BTC_SCAN_START == type)
-	{	
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
-		if(!wifi_connected)	// non-connected scan
-		{
-			//set 0x550[3]=1 before PsTdma
-			//halbtc8812a1ant_Reg0x550Bit3(btcoexist, true);
-			halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(btcoexist);
-		}
-		else	// wifi is connected
-		{
-			halbtc8812a1ant_ActionWifiConnectedScan(btcoexist);
-		}
-	}
-	else if(BTC_SCAN_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
-		if(!wifi_connected)	// non-connected scan
-		{
-			//halbtc8812a1ant_Reg0x550Bit3(btcoexist, false);
-			halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
-		}
-		else
-		{
-			halbtc8812a1ant_ActionWifiConnected(btcoexist);
-		}
-	}
-}
-
-void
-EXhalbtc8812a1ant_ConnectNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u1Byte			type
-	)
-{
-	PBTC_STACK_INFO 	stack_info=&btcoexist->stack_info;
-	BOOLEAN			wifi_connected=false;	
-
-	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
-		return;
-
-	if(BTC_ASSOCIATE_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
-		if(btcoexist->bt_info.bt_disabled)
-		{			
-			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);
-		}
-		else
-		{
-			halbtc8812a1ant_ActionWifiNotConnectedAssoAuthScan(btcoexist);
-		}
-	}
-	else if(BTC_ASSOCIATE_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
-		
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
-		if(!wifi_connected) // non-connected scan
-		{
-			//halbtc8812a1ant_Reg0x550Bit3(btcoexist, false);
-			halbtc8812a1ant_ActionWifiNotConnected(btcoexist);
-		}
-		else
-		{
-			halbtc8812a1ant_ActionWifiConnected(btcoexist);
-		}
-	}
-}
-
-void
-EXhalbtc8812a1ant_MediaStatusNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u1Byte				type
-	)
-{
-	u1Byte			dataLen=5;
-	u1Byte			buf[6] = {0};
-	u1Byte			h2c_parameter[3] ={0};
-	BOOLEAN			wifi_under5g=false;
-	u4Byte			wifi_bw;
-	u1Byte			wifi_central_chnl;
-
-	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
-		return;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
-
-	// only 2.4G we need to inform bt the chnl mask
-	if(!wifi_under5g)
-	{
-		if(BTC_MEDIA_CONNECT == type)
-		{
-			h2c_parameter[0] = 0x1;
-		}
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-		btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifi_central_chnl);
-		h2c_parameter[1] = wifi_central_chnl;
-		if(BTC_WIFI_BW_HT40 == wifi_bw)
-			h2c_parameter[2] = 0x30;
-		else
-			h2c_parameter[2] = 0x20;
-	}
-		
-	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
-	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
-	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
-	
-	buf[0] = dataLen;
-	buf[1] = 0x5;				// OP_Code
-	buf[2] = 0x3;				// OP_Code_Length
-	buf[3] = h2c_parameter[0]; 	// OP_Code_Content
-	buf[4] = h2c_parameter[1];
-	buf[5] = h2c_parameter[2];
-		
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_CTRL_BT_COEX, (PVOID)&buf[0]);		
-}
-
-void
-EXhalbtc8812a1ant_SpecialPacketNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u1Byte				type
-	)
-{
-	BOOLEAN 	bSecurityLink=false;
-
-	if(btcoexist->manual_control ||btcoexist->stop_coex_dm)
-		return;
-
-	//if(type == BTC_PACKET_DHCP)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], special Packet(%d) notify\n", type));
-		if(btcoexist->bt_info.bt_disabled)
-		{
-			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);	
-		}
-		else
-		{
-			halbtc8812a1ant_ActionWifiConnectedSpecialPacket(btcoexist);
-		}
-	}
-}
-
-void
-EXhalbtc8812a1ant_BtInfoNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	pu1Byte			tmp_buf,
-	 	u1Byte			length
-	)
-{
-	u1Byte			bt_info=0;
-	u1Byte			i, rsp_source=0;
-	static u4Byte		set_bt_lna_cnt=0, set_bt_psd_mode=0;
-	BOOLEAN			bt_busy=false, limited_dig=false;
-	BOOLEAN			wifi_connected=false;
-	BOOLEAN			bRejApAggPkt=false;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify()===>\n"));
-
-
-	rsp_source = tmp_buf[0]&0xf;
-	if(rsp_source >= BT_INFO_SRC_8812A_1ANT_MAX)
-		rsp_source = BT_INFO_SRC_8812A_1ANT_WIFI_FW;
-	coex_sta->bt_info_c2h_cnt[rsp_source]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rsp_source, length));
-	for(i=0; i<length; i++)
-	{
-		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
-		if(i == 1)
-			bt_info = tmp_buf[i];
-		if(i == length-1)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%2x]\n", tmp_buf[i]));
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%2x, ", tmp_buf[i]));
-		}
-	}
-
-	if(btcoexist->manual_control)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n"));
-		return;
-	}
-	if(btcoexist->stop_coex_dm)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Coex STOPPED!!<===\n"));
-		return;
-	}
-
-	if(BT_INFO_SRC_8812A_1ANT_WIFI_FW != rsp_source)
-	{
-		coex_sta->bt_retry_cnt =
-			coex_sta->bt_info_c2h[rsp_source][2];
-
-		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rsp_source][3]*2+10;
-
-		coex_sta->bt_info_ext = 
-			coex_sta->bt_info_c2h[rsp_source][4];
-
-		// Here we need to resend some wifi info to BT
-		// because bt is reset and loss of the info.
-		if( (coex_sta->bt_info_ext & BIT1) )
-		{			
-			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED, &wifi_connected);
-			if(wifi_connected)
-			{
-				EXhalbtc8812a1ant_MediaStatusNotify(btcoexist, BTC_MEDIA_CONNECT);
-			}
-			else
-			{
-				EXhalbtc8812a1ant_MediaStatusNotify(btcoexist, BTC_MEDIA_DISCONNECT);
-			}
-
-			set_bt_psd_mode = 0;
-		}
-
-		// test-chip bt patch doesn't support, temporary remove.
-		// need to add back when mp-chip. 12/20/2012
-#if 0		
-		if(set_bt_psd_mode <= 3)
-		{

-			halbtc8812a1ant_SetBtPsdMode(btcoexist, FORCE_EXEC, 0xd);
-			set_bt_psd_mode++;
-		}
-		
-		if(coex_dm->bCurBtLnaConstrain)
-		{
-			if( (coex_sta->bt_info_ext & BIT2) )
-			{
-			}
-			else
-			{
-				if(set_bt_lna_cnt <= 3)
-				{
-					halbtc8812a1ant_SetBtLnaConstrain(btcoexist, FORCE_EXEC, true);
-					set_bt_lna_cnt++;
-				}
-			}
-		}
-		else
-		{
-			set_bt_lna_cnt = 0;
-		}
-#endif
-		// test-chip bt patch only rsp the status for BT_RSP, 
-		// so temporary we consider the following only under BT_RSP
-		if(BT_INFO_SRC_8812A_1ANT_BT_RSP == rsp_source)
-		{
-			if( (coex_sta->bt_info_ext & BIT3) )
-			{
-			#if 0// temp disable because bt patch report the wrong value.
-				halbtc8812a1ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, false);
-			#endif
-			}
-			else
-			{
-				// BT already NOT ignore Wlan active, do nothing here.
-			}
-
-			if( (coex_sta->bt_info_ext & BIT4) )
-			{
-				// BT auto report already enabled, do nothing
-			}
-			else
-			{
-				halbtc8812a1ant_BtAutoReport(btcoexist, FORCE_EXEC, true);
-			}
-		}
-	}
-		
-	// check BIT2 first ==> check if bt is under inquiry or page scan
-	if(bt_info & BT_INFO_8812A_1ANT_B_INQ_PAGE)
-	{
-		coex_sta->c2h_bt_inquiry_page = true;
-		coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_INQ_PAGE;
-	}
-	else
-	{
-		coex_sta->c2h_bt_inquiry_page = false;
-		if(!(bt_info&BT_INFO_8812A_1ANT_B_CONNECTION))
-		{
-			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-connected idle!!!\n"));
-		}
-		else if(bt_info == BT_INFO_8812A_1ANT_B_CONNECTION)	// connection exists but no busy
-		{
-			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt connected-idle!!!\n"));
-		}		
-		else if((bt_info&BT_INFO_8812A_1ANT_B_SCO_ESCO) ||
-			(bt_info&BT_INFO_8812A_1ANT_B_SCO_BUSY))
-		{
-			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_SCO_BUSY;
-			bRejApAggPkt = true;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt sco busy!!!\n"));
-		}
-		else if(bt_info&BT_INFO_8812A_1ANT_B_ACL_BUSY)
-		{
-			if(BT_8812A_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
-				coex_dm->reset_tdma_adjust = true;
-			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_ACL_BUSY;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt acl busy!!!\n"));
-		}
-#if 0
-		else if(bt_info&BT_INFO_8812A_1ANT_B_SCO_ESCO)
-		{
-			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt acl/sco busy!!!\n"));
-		}
-#endif
-		else
-		{
-			//DbgPrint("error, undefined bt_info=0x%x\n", bt_info);
-			coex_dm->bt_status = BT_8812A_1ANT_BT_STATUS_MAX;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-defined state!!!\n"));
-		}
-
-		// send delete BA to disable aggregation
-		//btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejApAggPkt);
-	}
-
-	if( (BT_8812A_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
-		(BT_8812A_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-		(BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status) )
-	{
-		bt_busy = true;
-	}
-	else
-	{
-		bt_busy = false;
-	}
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
-
-	if(bt_busy)
-	{
-		limited_dig = true;
-	}
-	else
-	{
-		limited_dig = false;
-	}
-	coex_dm->limited_dig = limited_dig;
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
-
-	halbtc8812a1ant_RunCoexistMechanism(btcoexist);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify()<===\n"));
-}
-
-void
-EXhalbtc8812a1ant_StackOperationNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u1Byte				type
-	)
-{
-	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
-	}
-	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
-	}
-}
-
-void
-EXhalbtc8812a1ant_HaltNotify(
-	 	PBTC_COEXIST			btcoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
-
-	halbtc8812a1ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, true);
-	halbtc8812a1ant_PsTdma(btcoexist, FORCE_EXEC, false, 0);
-	btcoexist->btc_write_1byte(btcoexist, 0x4f, 0xf);
-	halbtc8812a1ant_WifiParaAdjust(btcoexist, false);
-}
-
-void
-EXhalbtc8812a1ant_PnpNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u1Byte				pnpState
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
-
-	if(BTC_WIFI_PNP_SLEEP == pnpState)
-	{
-		btcoexist->stop_coex_dm = true;
-		halbtc8812a1ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, true);
-		halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-		halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);	
-	}
-	else if(BTC_WIFI_PNP_WAKE_UP == pnpState)
-	{
-		
-	}
-}
-
-void
-EXhalbtc8812a1ant_Periodical(
-	 	PBTC_COEXIST			btcoexist
-	)
-{
-	BOOLEAN			wifi_under5g=false;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Periodical()===>\n"));
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], 1Ant Periodical!!\n"));
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under5g);
-
-	if(wifi_under5g)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Periodical(), return for 5G<===\n"));
-		halbtc8812a1ant_CoexAllOff(btcoexist);
-		return;
-	}
-
-	halbtc8812a1ant_QueryBtInfo(btcoexist);
-	halbtc8812a1ant_MonitorBtCtr(btcoexist);
-	halbtc8812a1ant_MonitorBtEnableDisable(btcoexist);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Periodical()<===\n"));
-}
-
-void
-EXhalbtc8812a1ant_DbgControl(
-	 	PBTC_COEXIST			btcoexist,
-	 	u1Byte				opCode,
-	 	u1Byte				opLen,
-	 	pu1Byte				pData
-	)
-{
-	switch(opCode)
-	{
-		case BTC_DBG_SET_COEX_NORMAL:
-			btcoexist->manual_control = false;
-			halbtc8812a1ant_InitCoexDm(btcoexist);
-			break;
-		case BTC_DBG_SET_COEX_WIFI_ONLY:
-			btcoexist->manual_control = true;
-			halbtc8812a1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-			halbtc8812a1ant_PsTdma(btcoexist, NORMAL_EXEC, false, 9);	
-			break;
-		case BTC_DBG_SET_COEX_BT_ONLY:
-			// todo
-			break;
-		default:
-			break;
-	}
-}
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.h b/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.h
deleted file mode 100644
index 37bdab5ae9f1..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/HalBtc8812a1Ant.h
+++ /dev/null
@@ -1,205 +0,0 @@
-//===========================================
-// The following is for 8812A_1ANT BT Co-exist definition
-//===========================================
-#define	BT_INFO_8812A_1ANT_B_FTP						BIT7
-#define	BT_INFO_8812A_1ANT_B_A2DP					BIT6
-#define	BT_INFO_8812A_1ANT_B_HID						BIT5
-#define	BT_INFO_8812A_1ANT_B_SCO_BUSY				BIT4
-#define	BT_INFO_8812A_1ANT_B_ACL_BUSY				BIT3
-#define	BT_INFO_8812A_1ANT_B_INQ_PAGE				BIT2
-#define	BT_INFO_8812A_1ANT_B_SCO_ESCO				BIT1
-#define	BT_INFO_8812A_1ANT_B_CONNECTION				BIT0
-
-#define	BT_INFO_8812A_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
-		(((_BT_INFO_EXT_&BIT0))? true:false)
-
-#define	BTC_RSSI_COEX_THRESH_TOL_8812A_1ANT		2
-
-#define  
-#define OUT
-
-typedef enum _BT_INFO_SRC_8812A_1ANT{
-	BT_INFO_SRC_8812A_1ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8812A_1ANT_BT_RSP				= 0x1,
-	BT_INFO_SRC_8812A_1ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8812A_1ANT_MAX
-}BT_INFO_SRC_8812A_1ANT,*PBT_INFO_SRC_8812A_1ANT;
-
-typedef enum _BT_8812A_1ANT_BT_STATUS{
-	BT_8812A_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
-	BT_8812A_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
-	BT_8812A_1ANT_BT_STATUS_INQ_PAGE				= 0x2,
-	BT_8812A_1ANT_BT_STATUS_ACL_BUSY				= 0x3,
-	BT_8812A_1ANT_BT_STATUS_SCO_BUSY				= 0x4,
-	BT_8812A_1ANT_BT_STATUS_ACL_SCO_BUSY			= 0x5,
-	BT_8812A_1ANT_BT_STATUS_MAX
-}BT_8812A_1ANT_BT_STATUS,*PBT_8812A_1ANT_BT_STATUS;
-
-typedef enum _BT_8812A_1ANT_COEX_ALGO{
-	BT_8812A_1ANT_COEX_ALGO_UNDEFINED			= 0x0,
-	BT_8812A_1ANT_COEX_ALGO_SCO				= 0x1,
-	BT_8812A_1ANT_COEX_ALGO_HID				= 0x2,
-	BT_8812A_1ANT_COEX_ALGO_A2DP				= 0x3,
-	BT_8812A_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
-	BT_8812A_1ANT_COEX_ALGO_PANEDR			= 0x5,
-	BT_8812A_1ANT_COEX_ALGO_PANHS			= 0x6,
-	BT_8812A_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
-	BT_8812A_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
-	BT_8812A_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
-	BT_8812A_1ANT_COEX_ALGO_HID_A2DP			= 0xa,
-	BT_8812A_1ANT_COEX_ALGO_MAX				= 0xb,
-}BT_8812A_1ANT_COEX_ALGO,*PBT_8812A_1ANT_COEX_ALGO;
-
-typedef struct _COEX_DM_8812A_1ANT{
-	// fw mechanism
-	bool		pre_dec_bt_pwr;
-	bool		cur_dec_bt_pwr;
-	bool		bPreBtLnaConstrain;
-	bool		bCurBtLnaConstrain;
-	u8		bPreBtPsdMode;
-	u8		bCurBtPsdMode;
-	u8		pre_fw_dac_swing_lvl;
-	u8		cur_fw_dac_swing_lvl;
-	bool		cur_ignore_wlan_act;
-	bool		pre_ignore_wlan_act;
-	u8		pre_ps_tdma;
-	u8		cur_ps_tdma;
-	u8		ps_tdma_para[5];
-	u8		ps_tdma_du_adj_type;
-	bool		reset_tdma_adjust;
-	bool		pre_ps_tdma_on;
-	bool		cur_ps_tdma_on;
-	bool		pre_bt_auto_report;
-	bool		cur_bt_auto_report;
-	u8		pre_lps;
-	u8		cur_lps;
-	u8		pre_rpwm;
-	u8		cur_rpwm;
-
-	// sw mechanism
-	bool		pre_rf_rx_lpf_shrink;
-	bool		cur_rf_rx_lpf_shrink;
-	u32		bt_rf0x1e_backup;
-	bool 	pre_low_penalty_ra;
-	bool		cur_low_penalty_ra;
-	bool		pre_dac_swing_on;
-	u32		pre_dac_swing_lvl;
-	bool		cur_dac_swing_on;
-	u32		cur_dac_swing_lvl;
-	bool		pre_adc_back_off;
-	bool		cur_adc_back_off;
-	bool 	pre_agc_table_en;
-	bool		cur_agc_table_en;
-	u32		pre_val0x6c0;
-	u32		cur_val0x6c0;
-	u32		pre_val0x6c4;
-	u32		cur_val0x6c4;
-	u32		pre_val0x6c8;
-	u32		cur_val0x6c8;
-	u8		pre_val0x6cc;
-	u8		cur_val0x6cc;
-	bool		limited_dig;
-
-	// algorithm related
-	u8		pre_algorithm;
-	u8		cur_algorithm;
-	u8		bt_status;
-	u8		wifi_chnl_info[3];
-
-	u8		error_condition;
-} COEX_DM_8812A_1ANT, *PCOEX_DM_8812A_1ANT;
-
-typedef struct _COEX_STA_8812A_1ANT{
-	bool					under_lps;
-	bool					under_ips;
-	u32					high_priority_tx;
-	u32					high_priority_rx;
-	u32					low_priority_tx;
-	u32					low_priority_rx;
-	u8					bt_rssi;
-	u8					pre_bt_rssi_state;
-	u8					pre_wifi_rssi_state[4];
-	bool					c2h_bt_info_req_sent;
-	u8					bt_info_c2h[BT_INFO_SRC_8812A_1ANT_MAX][10];
-	u32					bt_info_c2h_cnt[BT_INFO_SRC_8812A_1ANT_MAX];
-	bool					c2h_bt_inquiry_page;
-	u8					bt_retry_cnt;
-	u8					bt_info_ext;
-}COEX_STA_8812A_1ANT, *PCOEX_STA_8812A_1ANT;
-
-//===========================================
-// The following is interface which will notify coex module.
-//===========================================
-void
-EXhalbtc8812a1ant_InitHwConfig(
-	 	PBTC_COEXIST		btcoexist
-	);
-void
-EXhalbtc8812a1ant_InitCoexDm(
-	 	PBTC_COEXIST		btcoexist
-	);
-void
-EXhalbtc8812a1ant_IpsNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u8			type
-	);
-void
-EXhalbtc8812a1ant_LpsNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u8			type
-	);
-void
-EXhalbtc8812a1ant_ScanNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u8			type
-	);
-void
-EXhalbtc8812a1ant_ConnectNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u8			type
-	);
-void
-EXhalbtc8812a1ant_MediaStatusNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u8				type
-	);
-void
-EXhalbtc8812a1ant_SpecialPacketNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u8				type
-	);
-void
-EXhalbtc8812a1ant_BtInfoNotify(
-	 	PBTC_COEXIST		btcoexist,
-	 	u8			*tmp_buf,
-	 	u8			length
-	);
-void
-EXhalbtc8812a1ant_StackOperationNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u8				type
-	);
-void
-EXhalbtc8812a1ant_HaltNotify(
-	 	PBTC_COEXIST			btcoexist
-	);
-void
-EXhalbtc8812a1ant_PnpNotify(
-	 	PBTC_COEXIST			btcoexist,
-	 	u8				pnpState
-	);
-void
-EXhalbtc8812a1ant_Periodical(
-	 	PBTC_COEXIST			btcoexist
-	);
-void
-EXhalbtc8812a1ant_DisplayCoexInfo(
-	 	PBTC_COEXIST		btcoexist
-	);
-void
-EXhalbtc8812a1ant_DbgControl(
-	 	PBTC_COEXIST			btcoexist,
-	 	u8				opCode,
-	 	u8				opLen,
-	 	u8 				*pData
-	);
diff --git a/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.c b/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.c
deleted file mode 100644
index 8e4293a35872..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.c
+++ /dev/null
@@ -1,1614 +0,0 @@
-//============================================================
-// Description:
-//
-// This file is for RTL8723A Co-exist mechanism
-//
-// History
-// 2012/08/22 Cosa first check in.
-// 2012/11/14 Cosa Revise for 8723A 1Ant out sourcing.
-//
-//============================================================
-
-//============================================================
-// include files
-//============================================================
-#include "Mp_Precomp.h"
-#if(BT_30_SUPPORT == 1)
-//============================================================
-// Global variables, these are static variables
-//============================================================
-static COEX_DM_8723A_1ANT	GLCoexDm8723a1Ant;
-static PCOEX_DM_8723A_1ANT 	pCoexDm=&GLCoexDm8723a1Ant;
-static COEX_STA_8723A_1ANT	GLCoexSta8723a1Ant;
-static PCOEX_STA_8723A_1ANT	pCoexSta=&GLCoexSta8723a1Ant;
-
-const char *const GLBtInfoSrc8723a1Ant[]={
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-//============================================================
-// local function proto type if needed
-//============================================================
-//============================================================
-// local function start with halbtc8723a1ant_
-//============================================================
-VOID
-halbtc8723a1ant_Reg0x550Bit3(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bSet
-	)
-{
-	u1Byte	u1tmp=0;
-	
-	u1tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x550);
-	if(bSet)
-	{
-		u1tmp |= BIT3;
-	}
-	else
-	{
-		u1tmp &= ~BIT3;
-	}
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x550, u1tmp);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], set 0x550[3]=%d\n", (bSet? 1:0)));
-}
-
-VOID
-halbtc8723a1ant_NotifyFwScan(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			scanType
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-	
-	if(BTC_SCAN_START == scanType)
-		H2C_Parameter[0] = 0x1;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Notify FW for wifi scan, write 0x3b=0x%x\n", 
-		H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x3b, 1, H2C_Parameter);
-}
-
-VOID
-halbtc8723a1ant_QueryBtInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-
-	pCoexSta->bC2hBtInfoReqSent = true;
-
-	H2C_Parameter[0] |= BIT0;	// trigger
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Query Bt Info, FW write 0x38=0x%x\n", 
-		H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x38, 1, H2C_Parameter);
-}
-
-VOID
-halbtc8723a1ant_SetSwRfRxLpfCorner(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bRxRfShrinkOn
-	)
-{
-	if(bRxRfShrinkOn)
-	{
-		//Shrink RF Rx LPF corner
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
-	}
-	else
-	{
-		//Resume RF Rx LPF corner
-		// After initialized, we can use pCoexDm->btRf0x1eBackup
-		if(pBtCoexist->initilized)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
-			pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
-		}
-	}
-}
-
-VOID
-halbtc8723a1ant_RfShrink(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bRxRfShrinkOn
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
-		(bForceExec? "force to":""), ((bRxRfShrinkOn)? "ON":"OFF")));
-	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreRfRxLpfShrink=%d, bCurRfRxLpfShrink=%d\n", 
-			pCoexDm->bPreRfRxLpfShrink, pCoexDm->bCurRfRxLpfShrink));
-
-		if(pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink) 
-			return;
-	}
-	halbtc8723a1ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
-
-	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
-}
-
-VOID
-halbtc8723a1ant_SetSwPenaltyTxRateAdaptive(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bLowPenaltyRa
-	)
-{
-	u1Byte	tmpU1;
-
-	tmpU1 = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4fd);
-	tmpU1 |= BIT0;
-	if(bLowPenaltyRa)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
-		tmpU1 &= ~BIT2;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
-		tmpU1 |= BIT2;
-	}
-
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4fd, tmpU1);
-}
-
-VOID
-halbtc8723a1ant_LowPenaltyRa(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bLowPenaltyRa
-	)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
-		(bForceExec? "force to":""), ((bLowPenaltyRa)? "ON":"OFF")));
-	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreLowPenaltyRa=%d, bCurLowPenaltyRa=%d\n", 
-			pCoexDm->bPreLowPenaltyRa, pCoexDm->bCurLowPenaltyRa));
-
-		if(pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa) 
-			return;
-	}
-	halbtc8723a1ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
-
-	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
-}
-
-VOID
-halbtc8723a1ant_SetCoexTable(
-	IN	PBTC_COEXIST	pBtCoexist,
-	IN	u4Byte		val0x6c0,
-	IN	u4Byte		val0x6c8,
-	IN	u1Byte		val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, val0x6cc);
-}
-
-VOID
-halbtc8723a1ant_CoexTable(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	u4Byte			val0x6c0,
-	IN	u4Byte			val0x6c8,
-	IN	u1Byte			val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
-		(bForceExec? "force to":""), val0x6c0, val0x6c8, val0x6cc));
-	pCoexDm->curVal0x6c0 = val0x6c0;
-	pCoexDm->curVal0x6c8 = val0x6c8;
-	pCoexDm->curVal0x6cc = val0x6cc;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], preVal0x6c0=0x%x, preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n", 
-			pCoexDm->preVal0x6c0, pCoexDm->preVal0x6c8, pCoexDm->preVal0x6cc));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], curVal0x6c0=0x%x, curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n", 
-			pCoexDm->curVal0x6c0, pCoexDm->curVal0x6c8, pCoexDm->curVal0x6cc));
-	
-		if( (pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
-			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
-			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc) )
-			return;
-	}
-	halbtc8723a1ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c8, val0x6cc);
-
-	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
-	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
-	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
-}
-
-VOID
-halbtc8723a1ant_SetFwIgnoreWlanAct(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bEnable
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-		
-	if(bEnable)
-	{
-		H2C_Parameter[0] |= BIT0;		// function enable
-	}
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x25=0x%x\n", 
-		H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x25, 1, H2C_Parameter);	
-}
-
-VOID
-halbtc8723a1ant_IgnoreWlanAct(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bEnable
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
-		(bForceExec? "force to":""), (bEnable? "ON":"OFF")));
-	pCoexDm->bCurIgnoreWlanAct = bEnable;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n", 
-			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
-
-		if(pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
-			return;
-	}
-	halbtc8723a1ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
-
-	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
-}
-
-VOID
-halbtc8723a1ant_SetFwPstdma(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type,
-	IN	u1Byte			byte1,
-	IN	u1Byte			byte2,
-	IN	u1Byte			byte3,
-	IN	u1Byte			byte4,
-	IN	u1Byte			byte5
-	)
-{
-	u1Byte			H2C_Parameter[5] ={0};
-	u1Byte			realByte1=byte1, realByte5=byte5;
-	BOOLEAN			bApEnable=FALSE;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, &bApEnable);
-
-	// byte1[1:0] != 0 means enable pstdma
-	// for 2Ant bt coexist, if byte1 != 0 means enable pstdma
-	if(byte1)
-	{
-		if(bApEnable)
-		{
-			if(type != 5 && type != 12)
-			{
-				BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], FW for 1Ant AP mode\n"));
-				realByte1 &= ~BIT4;
-				realByte1 |= BIT5;
-
-				realByte5 |= BIT5;
-				realByte5 &= ~BIT6;
-			}
-		}
-	}
-	H2C_Parameter[0] = realByte1;	
-	H2C_Parameter[1] = byte2;	
-	H2C_Parameter[2] = byte3;
-	H2C_Parameter[3] = byte4;
-	H2C_Parameter[4] = realByte5;
-
-	pCoexDm->psTdmaPara[0] = realByte1;
-	pCoexDm->psTdmaPara[1] = byte2;
-	pCoexDm->psTdmaPara[2] = byte3;
-	pCoexDm->psTdmaPara[3] = byte4;
-	pCoexDm->psTdmaPara[4] = realByte5;
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x3a(5bytes)=0x%x%08x\n", 
-		H2C_Parameter[0], 
-		H2C_Parameter[1]<<24|H2C_Parameter[2]<<16|H2C_Parameter[3]<<8|H2C_Parameter[4]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x3a, 5, H2C_Parameter);
-}
-
-VOID
-halbtc8723a1ant_PsTdma(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bTurnOn,
-	IN	u1Byte			type
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
-		(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type));		
-	pCoexDm->bCurPsTdmaOn = bTurnOn;
-	pCoexDm->curPsTdma = type;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n", 
-			pCoexDm->bPrePsTdmaOn, pCoexDm->bCurPsTdmaOn));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n", 
-			pCoexDm->prePsTdma, pCoexDm->curPsTdma));
-
-		if( (pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
-			(pCoexDm->prePsTdma == pCoexDm->curPsTdma) )
-			return;
-	}	
-	if(pCoexDm->bCurPsTdmaOn)
-	{
-		switch(pCoexDm->curPsTdma)
-		{
-			case 1:
-			default:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x1a, 0x1a, 0x0, 0x40);
-				break;
-			case 2:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x12, 0x12, 0x0, 0x40);
-				break;
-			case 3:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x3f, 0x3, 0x10, 0x40);
-				break;
-			case 4:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x15, 0x3, 0x10, 0x0);
-				break;
-			case 5:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0xa9, 0x15, 0x3, 0x35, 0xc0);
-				break;
-			
-			case 8: 
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x25, 0x3, 0x10, 0x0);
-				break;
-			case 9: 
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0xa, 0xa, 0x0, 0x40);
-				break;
-			case 10:	
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0xa, 0xa, 0x0, 0x40);
-				break;
-			case 11:	
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x5, 0x5, 0x0, 0x40);
-				break;
-			case 12:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0xa9, 0xa, 0x3, 0x15, 0xc0);
-				break;
-	
-			case 18:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x25, 0x3, 0x10, 0x0);
-				break;			
-
-			case 20:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x2a, 0x2a, 0x0, 0x0);
-				break;
-			case 21:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x20, 0x3, 0x10, 0x40);
-				break;
-			case 22:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x1a, 0x1a, 0x2, 0x40);
-				break;
-			case 23:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x12, 0x12, 0x2, 0x40);
-				break;
-			case 24:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0xa, 0xa, 0x2, 0x40);
-				break;
-			case 25:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x5, 0x5, 0x2, 0x40);
-				break;
-			case 26:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x93, 0x25, 0x3, 0x10, 0x0);
-				break;
-			case 27:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x13, 0x5, 0x5, 0x2, 0x40);
-				break;
-			case 28:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x3, 0x2f, 0x2f, 0x0, 0x0);
-				break;
-
-		}
-	}
-	else
-	{
-		// disable PS tdma
-		switch(pCoexDm->curPsTdma)
-		{
-			case 8:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x8, 0x0, 0x0, 0x0, 0x0);		
-				break;
-			case 0:
-			default:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x0, 0x0, 0x0, 0x0, 0x0);
-				pBtCoexist->btc_write_2byte(pBtCoexist, 0x860, 0x210);
-				break;
-			case 9:
-				halbtc8723a1ant_SetFwPstdma(pBtCoexist, type, 0x0, 0x0, 0x0, 0x0, 0x0);
-				pBtCoexist->btc_write_2byte(pBtCoexist, 0x860, 0x110);
-				break;
-
-		}
-	}
-
-	// update pre state
-	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
-	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
-}
-
-
-VOID
-halbtc8723a1ant_CoexAllOff(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	// fw all off
-	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-
-	// sw all off
-	halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
-
-	// hw all off
-	halbtc8723a1ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-}
-
-VOID
-halbtc8723a1ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	// force to reset coex mechanism
-	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
-}
-
-VOID
-halbtc8723a1ant_BtEnableAction(
-	IN 	PBTC_COEXIST		pBtCoexist
-	)
-{
-	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
-}
-
-VOID
-halbtc8723a1ant_MonitorBtCtr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u4Byte 			regHPTxRx, regLPTxRx, u4Tmp;
-	u4Byte			regHPTx=0, regHPRx=0, regLPTx=0, regLPRx=0;
-	u1Byte			u1Tmp;
-	
-	regHPTxRx = 0x770;
-	regLPTxRx = 0x774;
-
-	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regHPTxRx);
-	regHPTx = u4Tmp & MASKLWORD;
-	regHPRx = (u4Tmp & MASKHWORD)>>16;
-
-	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regLPTxRx);
-	regLPTx = u4Tmp & MASKLWORD;
-	regLPRx = (u4Tmp & MASKHWORD)>>16;
-		
-	pCoexSta->highPriorityTx = regHPTx;
-	pCoexSta->highPriorityRx = regHPRx;
-	pCoexSta->lowPriorityTx = regLPTx;
-	pCoexSta->lowPriorityRx = regLPRx;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
-		regHPTxRx, regHPTx, regHPTx, regHPRx, regHPRx));
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
-		regLPTxRx, regLPTx, regLPTx, regLPRx, regLPRx));
-
-	// reset counter
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0xc);
-}
-
-VOID
-halbtc8723a1ant_MonitorBtEnableDisable(
-	IN 	PBTC_COEXIST		pBtCoexist
-	)
-{
-	static BOOLEAN	bPreBtDisabled=FALSE;
-	static u4Byte	btDisableCnt=0;
-	BOOLEAN			bBtActive=true, bBtDisabled=FALSE;
-
-	// This function check if bt is disabled
-	
-	if(	pCoexSta->highPriorityTx == 0 &&
-		pCoexSta->highPriorityRx == 0 &&
-		pCoexSta->lowPriorityTx == 0 &&
-		pCoexSta->lowPriorityRx == 0)
-	{
-		bBtActive = FALSE;
-	}
-	if(	pCoexSta->highPriorityTx == 0xffff &&
-		pCoexSta->highPriorityRx == 0xffff &&
-		pCoexSta->lowPriorityTx == 0xffff &&
-		pCoexSta->lowPriorityRx == 0xffff)
-	{
-		bBtActive = FALSE;
-	}
-	if(bBtActive)
-	{
-		btDisableCnt = 0;
-		bBtDisabled = FALSE;
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
-	}
-	else
-	{
-		btDisableCnt++;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
-				btDisableCnt));
-		if(btDisableCnt >= 2)
-		{
-			bBtDisabled = true;
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
-		}
-	}
-	if(bPreBtDisabled != bBtDisabled)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
-			(bPreBtDisabled ? "disabled":"enabled"), 
-			(bBtDisabled ? "disabled":"enabled")));
-		bPreBtDisabled = bBtDisabled;
-		if(!bBtDisabled)
-		{
-			halbtc8723a1ant_BtEnableAction(pBtCoexist);
-		}
-		else
-		{
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
-		}
-	}
-}
-
-VOID
-halbtc8723a1ant_TdmaDurationAdjust(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	static s4Byte		up,dn,m,n,WaitCount;
-	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
-	u1Byte			retryCount=0;
-	u1Byte			btState;
-	BOOLEAN			bScan=FALSE, bLink=FALSE, bRoam=FALSE;
-	u4Byte			wifiBw;
-	
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	btState = pCoexDm->btStatus;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], TdmaDurationAdjust()\n"));
-	if(pCoexDm->psTdmaGlobalCnt != pCoexDm->psTdmaMonitorCnt)
-	{
-		pCoexDm->psTdmaMonitorCnt = 0;
-		pCoexDm->psTdmaGlobalCnt = 0;
-	}
-	if(pCoexDm->psTdmaMonitorCnt == 0)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], first run BT A2DP + WiFi busy state!!\n"));
-		if(btState == BT_STATE_8723A_1ANT_ACL_ONLY_BUSY)
-		{
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-			pCoexDm->psTdmaDuAdjType = 1;
-		}
-		else
-		{
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
-			pCoexDm->psTdmaDuAdjType = 22;
-		}
-		//============
-		up = 0;
-		dn = 0;
-		m = 1;
-		n= 3;
-		result = 0;
-		WaitCount = 0;
-	}
-	else
-	{
-		//acquire the BT TRx retry count from BT_Info byte2
-		retryCount = pCoexSta->btRetryCnt;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], retryCount = %d\n", retryCount));
-		result = 0;
-		WaitCount++; 
-		  
-		if(retryCount == 0)  // no retry in the last 2-second duration
-		{
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0; 			 
-
-			if(up >= n) // if ³sÄò n ­Ó2¬í retry count¬°0, «h½Õ¼eWiFi duration
-			{
-				WaitCount = 0; 
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Increase wifi duration!!\n"));
-			}
-		}
-		else if (retryCount <= 3)	// <=3 retry in the last 2-second duration
-		{
-			up--; 
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2)	// if ³sÄò 2 ­Ó2¬í retry count< 3, «h½Õ¯¶WiFi duration
-			{
-				if (WaitCount <= 2)
-					m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
-				else
-					m = 1;
-
-				if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
-					m = 20;
-
-				n = 3*m;
-				up = 0;
-				dn = 0;
-				WaitCount = 0;	
-				result = -1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
-			}
-		}
-		else  //retry count > 3, ¥u­n1¦¸ retry count > 3, «h½Õ¯¶WiFi duration
-		{
-			if (WaitCount == 1)
-				m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
-			else
-				m = 1;
-
-			if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
-				m = 20;
-
-			n = 3*m;
-			up = 0;
-			dn = 0;
-			WaitCount = 0; 
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
-		}
-		
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT TxRx counter H+L <= 1200\n"));
-			if(btState != BT_STATE_8723A_1ANT_ACL_ONLY_BUSY)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], NOT ACL only busy!\n"));
-				if(BTC_WIFI_BW_HT40 != wifiBw)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], 20MHz\n"));
-					if(result == -1)
-					{
-						if(pCoexDm->curPsTdma == 22)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
-							pCoexDm->psTdmaDuAdjType = 23;
-						}
-						else if(pCoexDm->curPsTdma == 23)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
-							pCoexDm->psTdmaDuAdjType = 24;
-						}
-						else if(pCoexDm->curPsTdma == 24)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 25);
-							pCoexDm->psTdmaDuAdjType = 25;
-						}
-					} 
-					else if (result == 1)
-					{
-						if(pCoexDm->curPsTdma == 25)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
-							pCoexDm->psTdmaDuAdjType = 24;
-						}
-						else if(pCoexDm->curPsTdma == 24)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
-							pCoexDm->psTdmaDuAdjType = 23;
-						}
-						else if(pCoexDm->curPsTdma == 23)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 22);
-							pCoexDm->psTdmaDuAdjType = 22;
-						}
-					}
-					// error handle, if not in the following state,
-					// set psTdma again.
-					if( (pCoexDm->psTdmaDuAdjType != 22) &&
-						(pCoexDm->psTdmaDuAdjType != 23) &&
-						(pCoexDm->psTdmaDuAdjType != 24) &&
-						(pCoexDm->psTdmaDuAdjType != 25) )
-					{
-						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], duration case out of handle!!\n"));
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
-						pCoexDm->psTdmaDuAdjType = 23;
-					}
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], 40MHz\n"));
-					if(result == -1)
-					{
-						if(pCoexDm->curPsTdma == 23)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
-							pCoexDm->psTdmaDuAdjType = 24;
-						}
-						else if(pCoexDm->curPsTdma == 24)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 25);
-							pCoexDm->psTdmaDuAdjType = 25;
-						}
-						else if(pCoexDm->curPsTdma == 25)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 27);
-							pCoexDm->psTdmaDuAdjType = 27;
-						}
-					} 
-					else if (result == 1)
-					{
-						if(pCoexDm->curPsTdma == 27)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 25);
-							pCoexDm->psTdmaDuAdjType = 25;
-						}
-						else if(pCoexDm->curPsTdma == 25)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
-							pCoexDm->psTdmaDuAdjType = 24;
-						}
-						else if(pCoexDm->curPsTdma == 24)
-						{
-							halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 23);
-							pCoexDm->psTdmaDuAdjType = 23;
-						}
-					}
-					// error handle, if not in the following state,
-					// set psTdma again.
-					if( (pCoexDm->psTdmaDuAdjType != 23) &&
-						(pCoexDm->psTdmaDuAdjType != 24) &&
-						(pCoexDm->psTdmaDuAdjType != 25) &&
-						(pCoexDm->psTdmaDuAdjType != 27) )
-					{
-						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], duration case out of handle!!\n"));
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 24);
-						pCoexDm->psTdmaDuAdjType = 24;
-					}
-				}
-			}
-			else
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ACL only busy\n"));
-				if (result == -1)
-				{
-					if(pCoexDm->curPsTdma == 1)
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-						pCoexDm->psTdmaDuAdjType = 9;
-					}
-					else if(pCoexDm->curPsTdma == 9)
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-				}
-				else if (result == 1)
-				{
-					if(pCoexDm->curPsTdma == 11)
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-						pCoexDm->psTdmaDuAdjType = 9;
-					}
-					else if(pCoexDm->curPsTdma == 9)
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-						pCoexDm->psTdmaDuAdjType = 1;
-					}
-				}
-
-				// error handle, if not in the following state,
-				// set psTdma again.
-				if( (pCoexDm->psTdmaDuAdjType != 1) &&
-					(pCoexDm->psTdmaDuAdjType != 2) &&
-					(pCoexDm->psTdmaDuAdjType != 9) &&
-					(pCoexDm->psTdmaDuAdjType != 11) )
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], duration case out of handle!!\n"));
-					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-					pCoexDm->psTdmaDuAdjType = 2;
-				}
-			}
-		}
-	}
-
-	// if current PsTdma not match with the recorded one (when scan, dhcp...), 
-	// then we have to adjust it back to the previous record one.
-	if(pCoexDm->curPsTdma != pCoexDm->psTdmaDuAdjType)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PsTdma type dismatch!!!, curPsTdma=%d, recordPsTdma=%d\n", 
-			pCoexDm->curPsTdma, pCoexDm->psTdmaDuAdjType));
-
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-
-		if( !bScan && !bLink &&	!bRoam)
-		{
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n"));
-		}
-	}
-	pCoexDm->psTdmaMonitorCnt++;
-}
-
-
-VOID
-halbtc8723a1ant_CoexForWifiConnect(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN		bWifiConnected=FALSE, bWifiBusy=FALSE;
-	u1Byte		btState, btInfoOriginal=0;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-
-	btState = pCoexDm->btStatus;
-	btInfoOriginal = pCoexSta->btInfoC2h[BT_INFO_SRC_8723A_1ANT_BT_RSP][0];
-
-	if(bWifiConnected)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi connected!!\n"));
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
-		
-		if( !bWifiBusy &&
-			((BT_STATE_8723A_1ANT_NO_CONNECTION == btState) ||
-			(BT_STATE_8723A_1ANT_CONNECT_IDLE == btState)) )
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], [Wifi is idle] or [Bt is non connected idle or Bt is connected idle]!!\n"));
-
-			if(BT_STATE_8723A_1ANT_NO_CONNECTION == btState)
-				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);
-			else if(BT_STATE_8723A_1ANT_CONNECT_IDLE == btState)
-				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-
-			pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0xc0);
-		}
-		else
-		{
-			if( (BT_STATE_8723A_1ANT_SCO_ONLY_BUSY == btState) ||
-				(BT_STATE_8723A_1ANT_ACL_SCO_BUSY == btState) ||
-				(BT_STATE_8723A_1ANT_HID_BUSY == btState) ||
-				(BT_STATE_8723A_1ANT_HID_SCO_BUSY == btState) )
-			{
-				pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0x60);
-			}
-			else
-			{
-				pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0xc0);
-			}
-			switch(btState)
-			{
-				case BT_STATE_8723A_1ANT_NO_CONNECTION:
-					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
-					break;
-				case BT_STATE_8723A_1ANT_CONNECT_IDLE:
-					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-					break;
-				case BT_STATE_8723A_1ANT_INQ_OR_PAG:
-					halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-					break;
-				case BT_STATE_8723A_1ANT_SCO_ONLY_BUSY:
-				case BT_STATE_8723A_1ANT_ACL_SCO_BUSY:
-				case BT_STATE_8723A_1ANT_HID_BUSY:
-				case BT_STATE_8723A_1ANT_HID_SCO_BUSY:
-					halbtc8723a1ant_TdmaDurationAdjust(pBtCoexist);
-					break;
-				case BT_STATE_8723A_1ANT_ACL_ONLY_BUSY:
-					if (btInfoOriginal&BT_INFO_8723A_1ANT_B_A2DP)
-					{
-						halbtc8723a1ant_TdmaDurationAdjust(pBtCoexist);
-					}
-					else if(btInfoOriginal&BT_INFO_8723A_1ANT_B_FTP)
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-					}
-					else if( (btInfoOriginal&BT_INFO_8723A_1ANT_B_A2DP) &&
-							(btInfoOriginal&BT_INFO_8723A_1ANT_B_FTP) )
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-					}
-					else
-					{
-						halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-					}
-					break;
-				default:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], error!!!, undefined case in halbtc8723a1ant_CoexForWifiConnect()!!\n"));
-					break;
-			}
-		}
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is disconnected!!\n"));
-	}
-
-	pCoexDm->psTdmaGlobalCnt++;
-}
-
-//============================================================
-// work around function start with wa_halbtc8723a1ant_
-//============================================================
-VOID
-wa_halbtc8723a1ant_MonitorC2h(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	u1Byte	tmp1b=0x0;
-	u4Byte	curC2hTotalCnt=0x0;
-	static u4Byte	preC2hTotalCnt=0x0, sameCntPollingTime=0x0;
-
-	curC2hTotalCnt+=pCoexSta->btInfoC2hCnt[BT_INFO_SRC_8723A_1ANT_BT_RSP];
-
-	if(curC2hTotalCnt == preC2hTotalCnt)
-	{
-		sameCntPollingTime++;
-	}
-	else
-	{
-		preC2hTotalCnt = curC2hTotalCnt;
-		sameCntPollingTime = 0;
-	}
-
-	if(sameCntPollingTime >= 2)
-	{
-		tmp1b = pBtCoexist->btc_read_1byte(pBtCoexist, 0x1af);
-		if(tmp1b != 0x0)
-		{
-			pCoexSta->c2hHangDetectCnt++;
-			pBtCoexist->btc_write_1byte(pBtCoexist, 0x1af, 0x0);
-		}
-	}
-}
-
-//============================================================
-// extern function start with EXhalbtc8723a1ant_
-//============================================================
-VOID
-EXhalbtc8723a1ant_InitHwConfig(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 1Ant Init HW Config!!\n"));
-
-	// backup rf 0x1e value
-	pCoexDm->btRf0x1eBackup = 
-		pBtCoexist->btc_get_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
-
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x40, 0x20);
-
-	// enable counter statistics
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0x4);
-	
-	// coex table
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, 0x0);			// 1-Ant coex
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, 0xffff);		// wifi break table
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c4, 0x55555555);	//coex table
-
-	// antenna switch control parameter
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x858, 0xaaaaaaaa);
-	
-	pBtCoexist->btc_write_2byte(pBtCoexist, 0x860, 0x210);	//set antenna at wifi side if ANTSW is software control
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x870, 0x300);	//SPDT(connected with TRSW) control by hardware PTA
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x874, 0x22804000);	//ANTSW keep by GNT_BT
-
-	// coexistence parameters
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x778, 0x1);	// enable RTK mode PTA
-}
-
-VOID
-EXhalbtc8723a1ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
-	
-	halbtc8723a1ant_InitCoexDm(pBtCoexist);
-}
-
-VOID
-EXhalbtc8723a1ant_DisplayCoexInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
-	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
-	pu1Byte				cliBuf=pBtCoexist->cli_buf;
-	u1Byte				u1Tmp[4], i, btInfoExt, psTdmaCase=0;
-	u4Byte				u4Tmp[4];
-	BOOLEAN				bRoam=FALSE, bScan=FALSE, bLink=FALSE, bWifiUnder5G=FALSE;
-	BOOLEAN				bBtHsOn=FALSE, bWifiBusy=FALSE;
-	s4Byte				wifiRssi=0, btHsRssi=0;
-	u4Byte				wifiBw, wifiTrafficDir;
-	u1Byte				wifiDot11Chnl, wifiHsChnl;
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cliBuf);
-
-	if(!pBoardInfo->bt_exist)
-	{
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cliBuf);
-		return;
-	}
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
-		pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num);
-	CL_PRINTF(cliBuf);	
-	
-	if(pBtCoexist->manual_control)
-	{
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "[Action Manual control]!!");
-		CL_PRINTF(cliBuf);
-	}
-	
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
-		((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
-		wifiDot11Chnl, wifiHsChnl, bBtHsOn);
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
-		pCoexDm->wifiChnlInfo[0], pCoexDm->wifiChnlInfo[1],
-		pCoexDm->wifiChnlInfo[2]);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
-		wifiRssi, btHsRssi);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
-		bLink, bRoam, bScan);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
-		(bWifiUnder5G? "5G":"2.4G"),
-		((BTC_WIFI_BW_LEGACY==wifiBw)? "Legacy": (((BTC_WIFI_BW_HT40==wifiBw)? "HT40":"HT20"))),
-		((!bWifiBusy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
-		((pCoexSta->bC2hBtInquiryPage)?("inquiry/page scan"):((BT_8723A_1ANT_BT_STATUS_IDLE == pCoexDm->btStatus)? "idle":(  (BT_8723A_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)? "connected-idle":"busy"))),
-		pCoexSta->btRssi, pCoexSta->btRetryCnt);
-	CL_PRINTF(cliBuf);
-	
-	if(pStackInfo->bProfileNotified)
-	{			
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
-			pStackInfo->bScoExist, pStackInfo->bHidExist, pStackInfo->bPanExist, pStackInfo->bA2dpExist);
-		CL_PRINTF(cliBuf);	
-
-		pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
-	}
-
-	btInfoExt = pCoexSta->btInfoExt;
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
-		(btInfoExt&BIT0)? "Basic rate":"EDR rate");
-	CL_PRINTF(cliBuf);	
-
-	for(i=0; i<BT_INFO_SRC_8723A_1ANT_MAX; i++)
-	{
-		if(pCoexSta->btInfoC2hCnt[i])
-		{				
-			CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723a1Ant[i], \
-				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
-				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
-				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
-				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]);
-			CL_PRINTF(cliBuf);
-		}
-	}
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "write 0x1af=0x0 num", \
-		pCoexSta->c2hHangDetectCnt);
-	CL_PRINTF(cliBuf);
-	
-	// Sw mechanism	
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
-	CL_PRINTF(cliBuf);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d", "SM1[ShRf/ LpRA/ LimDig]", \
-		pCoexDm->bCurRfRxLpfShrink, pCoexDm->bCurLowPenaltyRa, pCoexDm->limited_dig);
-	CL_PRINTF(cliBuf);
-
-	// Fw mechanism		
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
-	CL_PRINTF(cliBuf);	
-	
-	if(!pBtCoexist->manual_control)
-	{
-		psTdmaCase = pCoexDm->curPsTdma;
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d", "PS TDMA", \
-			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
-			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
-			pCoexDm->psTdmaPara[4], psTdmaCase);
-		CL_PRINTF(cliBuf);
-	
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d ", "IgnWlanAct", \
-			pCoexDm->bCurIgnoreWlanAct);
-		CL_PRINTF(cliBuf);
-	}
-
-	// Hw setting		
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
-	CL_PRINTF(cliBuf);	
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
-		pCoexDm->btRf0x1eBackup);
-	CL_PRINTF(cliBuf);
-
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x778);
-	u1Tmp[1] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x783);
-	u1Tmp[2] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x796);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/ 0x783/ 0x796", \
-		u1Tmp[0], u1Tmp[1], u1Tmp[2]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x880);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x880", \
-		u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x40);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x40", \
-		u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x550);
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x522);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
-		u4Tmp[0], u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x484);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x484(rate adaptive)", \
-		u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc50);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
-		u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda0);
-	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda4);
-	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda8);
-	u4Tmp[3] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xdac);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0xda0/0xda4/0xda8/0xdac(FA cnt)", \
-		u4Tmp[0], u4Tmp[1], u4Tmp[2], u4Tmp[3]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c0);
-	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c4);
-	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c8);
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x6cc);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
-		u4Tmp[0], u4Tmp[1], u4Tmp[2], u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770 (hp rx[31:16]/tx[15:0])", \
-		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx);
-	CL_PRINTF(cliBuf);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
-		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx);
-	CL_PRINTF(cliBuf);
-
-	// Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x41b);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x41b (mgntQ hang chk == 0xf)", \
-		u1Tmp[0]);
-	CL_PRINTF(cliBuf);	
-
-	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-
-VOID
-EXhalbtc8723a1ant_IpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	if(BTC_IPS_ENTER == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
-		halbtc8723a1ant_CoexAllOff(pBtCoexist);
-	}
-	else if(BTC_IPS_LEAVE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
-		//halbtc8723a1ant_InitCoexDm(pBtCoexist);
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_LpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	if(BTC_LPS_ENABLE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
-	}
-	else if(BTC_LPS_DISABLE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
-		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_ScanNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	BOOLEAN		bWifiConnected=FALSE;
-	
-	halbtc8723a1ant_NotifyFwScan(pBtCoexist, type);
-
-	if(pBtCoexist->btInfo.bBtDisabled)
-	{
-		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9); 
-	}
-	else
-	{
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-		if(BTC_SCAN_START == type)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
-			if(!bWifiConnected)	// non-connected scan
-			{
-				//set 0x550[3]=1 before PsTdma
-				halbtc8723a1ant_Reg0x550Bit3(pBtCoexist, true);
-			}
-
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-		}
-		else if(BTC_SCAN_FINISH == type)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
-			if(!bWifiConnected)	// non-connected scan
-			{
-				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0); 
-			}
-			else
-			{
-				halbtc8723a1ant_CoexForWifiConnect(pBtCoexist);
-			}
-		}
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_ConnectNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	BOOLEAN		bWifiConnected=FALSE;
-		
-	if(pBtCoexist->btInfo.bBtDisabled)
-	{
-		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9); 
-	}
-	else
-	{
-		if(BTC_ASSOCIATE_START == type)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
-			//set 0x550[3]=1 before PsTdma
-			halbtc8723a1ant_Reg0x550Bit3(pBtCoexist, true);
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);	// extend wifi slot	
-		}
-		else if(BTC_ASSOCIATE_FINISH == type)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
-			pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-			if(!bWifiConnected)	// non-connected scan
-			{
-				halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-			}
-			else
-			{
-				halbtc8723a1ant_CoexForWifiConnect(pBtCoexist);
-			}
-		}
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_MediaStatusNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	if(BTC_MEDIA_CONNECT == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_SpecialPacketNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	if(type == BTC_PACKET_DHCP)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], DHCP Packet notify\n"));
-		if(pBtCoexist->btInfo.bBtDisabled)
-		{
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
-		}
-		else
-		{
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 18);
-		}		
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_BtInfoNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	pu1Byte			tmpBuf,
-	IN	u1Byte			length
-	)
-{
-	u1Byte			btInfo=0;
-	u1Byte			i, rspSource=0;
-	BOOLEAN			bBtHsOn=FALSE, bBtBusy=FALSE, bForceLps=FALSE;
-
-	pCoexSta->bC2hBtInfoReqSent = FALSE;
-	
-	rspSource = BT_INFO_SRC_8723A_1ANT_BT_RSP;
-	pCoexSta->btInfoC2hCnt[rspSource]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rspSource, length));
-	for(i=0; i<length; i++)
-	{
-		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
-		if(i == 0)
-			btInfo = tmpBuf[i];
-		if(i == length-1)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
-		}
-	}
-
-	if(BT_INFO_SRC_8723A_1ANT_WIFI_FW != rspSource)
-	{
-		pCoexSta->btRetryCnt =
-			pCoexSta->btInfoC2h[rspSource][1];
-
-		pCoexSta->btRssi =
-			pCoexSta->btInfoC2h[rspSource][2]*2+10;
-
-		pCoexSta->btInfoExt = 
-			pCoexSta->btInfoC2h[rspSource][3];
-	}
-		
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	// check BIT2 first ==> check if bt is under inquiry or page scan
-	if(btInfo & BT_INFO_8723A_1ANT_B_INQ_PAGE)
-	{
-		pCoexSta->bC2hBtInquiryPage = true;
-	}
-	else
-	{
-		pCoexSta->bC2hBtInquiryPage = FALSE;
-	}
-	btInfo &= ~BIT2;
-	if(!(btInfo & BIT0))
-	{
-		pCoexDm->btStatus = BT_STATE_8723A_1ANT_NO_CONNECTION;
-		bForceLps = FALSE;
-	}
-	else
-	{
-		bForceLps = true;
-		if(btInfo == 0x1)
-		{
-			pCoexDm->btStatus = BT_STATE_8723A_1ANT_CONNECT_IDLE;
-		}
-		else if(btInfo == 0x9)
-		{
-			pCoexDm->btStatus = BT_STATE_8723A_1ANT_ACL_ONLY_BUSY;
-			bBtBusy = true;
-		}
-		else if(btInfo == 0x13)
-		{
-			pCoexDm->btStatus = BT_STATE_8723A_1ANT_SCO_ONLY_BUSY;
-			bBtBusy = true;
-		}
-		else if(btInfo == 0x1b)
-		{
-			pCoexDm->btStatus = BT_STATE_8723A_1ANT_ACL_SCO_BUSY;
-			bBtBusy = true;
-		}
-		else if(btInfo == 0x29)
-		{
-			pCoexDm->btStatus = BT_STATE_8723A_1ANT_HID_BUSY;
-			bBtBusy = true;
-		}
-		else if(btInfo == 0x3b)
-		{
-			pCoexDm->btStatus = BT_STATE_8723A_1ANT_HID_SCO_BUSY;
-			bBtBusy = true;
-		}
-	}
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &bBtBusy);
-	if(bForceLps)
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-	else
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
-
-	if( (BT_STATE_8723A_1ANT_NO_CONNECTION == pCoexDm->btStatus) ||
-		(BT_STATE_8723A_1ANT_CONNECT_IDLE == pCoexDm->btStatus) )
-	{
-		if(pCoexSta->bC2hBtInquiryPage)
-			pCoexDm->btStatus = BT_STATE_8723A_1ANT_INQ_OR_PAG;
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_StackOperationNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
-	}
-	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
-	}
-}
-
-VOID
-EXhalbtc8723a1ant_HaltNotify(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	halbtc8723a1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
-	
-	halbtc8723a1ant_LowPenaltyRa(pBtCoexist, FORCE_EXEC, FALSE);
-	halbtc8723a1ant_RfShrink(pBtCoexist, FORCE_EXEC, FALSE);
-
-	halbtc8723a1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
-	EXhalbtc8723a1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
-}
-
-VOID
-EXhalbtc8723a1ant_Periodical(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	BOOLEAN		bScan=FALSE, bLink=FALSE, bRoam=FALSE, bWifiConnected=FALSE;
-	
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], 1Ant Periodical!!\n"));
-	
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-
-	// work around for c2h hang
-	wa_halbtc8723a1ant_MonitorC2h(pBtCoexist);	
-
-	halbtc8723a1ant_QueryBtInfo(pBtCoexist);
-	halbtc8723a1ant_MonitorBtCtr(pBtCoexist);
-	halbtc8723a1ant_MonitorBtEnableDisable(pBtCoexist);
-
-	
-	if(bScan)
-		return;
-	if(bLink)
-		return;
-
-	if(bWifiConnected)
-	{
-		if(pBtCoexist->btInfo.bBtDisabled)
-		{
-			halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);
-			
-			halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
-		}
-		else
-		{
-			halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a1ant_CoexForWifiConnect(pBtCoexist);
-		}
-	}
-	else
-	{
-		halbtc8723a1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-
-		halbtc8723a1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a1ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
-	}
-}
-
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.h b/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.h
deleted file mode 100644
index 60992f59eb37..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/habtc8723a1ant.h
+++ /dev/null
@@ -1,176 +0,0 @@
-//===========================================
-// The following is for 8723A 1Ant BT Co-exist definition
-//===========================================
-#define	BT_INFO_8723A_1ANT_B_FTP						BIT7
-#define	BT_INFO_8723A_1ANT_B_A2DP					BIT6
-#define	BT_INFO_8723A_1ANT_B_HID						BIT5
-#define	BT_INFO_8723A_1ANT_B_SCO_BUSY				BIT4
-#define	BT_INFO_8723A_1ANT_B_ACL_BUSY				BIT3
-#define	BT_INFO_8723A_1ANT_B_INQ_PAGE				BIT2
-#define	BT_INFO_8723A_1ANT_B_SCO_ESCO				BIT1
-#define	BT_INFO_8723A_1ANT_B_CONNECTION				BIT0
-
-typedef enum _BT_STATE_8723A_1ANT{
-	BT_STATE_8723A_1ANT_DISABLED				= 0,
-	BT_STATE_8723A_1ANT_NO_CONNECTION		= 1,
-	BT_STATE_8723A_1ANT_CONNECT_IDLE		= 2,
-	BT_STATE_8723A_1ANT_INQ_OR_PAG			= 3,
-	BT_STATE_8723A_1ANT_ACL_ONLY_BUSY		= 4,
-	BT_STATE_8723A_1ANT_SCO_ONLY_BUSY		= 5,
-	BT_STATE_8723A_1ANT_ACL_SCO_BUSY			= 6,
-	BT_STATE_8723A_1ANT_HID_BUSY				= 7,
-	BT_STATE_8723A_1ANT_HID_SCO_BUSY			= 8,
-	BT_STATE_8723A_1ANT_MAX
-}BT_STATE_8723A_1ANT, *PBT_STATE_8723A_1ANT;
-
-#define		BTC_RSSI_COEX_THRESH_TOL_8723A_1ANT		2
-
-typedef enum _BT_INFO_SRC_8723A_1ANT{
-	BT_INFO_SRC_8723A_1ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8723A_1ANT_BT_RSP				= 0x1,
-	BT_INFO_SRC_8723A_1ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8723A_1ANT_MAX
-}BT_INFO_SRC_8723A_1ANT,*PBT_INFO_SRC_8723A_1ANT;
-
-typedef enum _BT_8723A_1ANT_BT_STATUS{
-	BT_8723A_1ANT_BT_STATUS_IDLE				= 0x0,
-	BT_8723A_1ANT_BT_STATUS_CONNECTED_IDLE	= 0x1,
-	BT_8723A_1ANT_BT_STATUS_NON_IDLE			= 0x2,
-	BT_8723A_1ANT_BT_STATUS_MAX
-}BT_8723A_1ANT_BT_STATUS,*PBT_8723A_1ANT_BT_STATUS;
-
-typedef enum _BT_8723A_1ANT_COEX_ALGO{
-	BT_8723A_1ANT_COEX_ALGO_UNDEFINED			= 0x0,
-	BT_8723A_1ANT_COEX_ALGO_SCO				= 0x1,
-	BT_8723A_1ANT_COEX_ALGO_HID				= 0x2,
-	BT_8723A_1ANT_COEX_ALGO_A2DP				= 0x3,
-	BT_8723A_1ANT_COEX_ALGO_PANEDR			= 0x4,
-	BT_8723A_1ANT_COEX_ALGO_PANHS			= 0x5,
-	BT_8723A_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x6,
-	BT_8723A_1ANT_COEX_ALGO_PANEDR_HID		= 0x7,
-	BT_8723A_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x8,
-	BT_8723A_1ANT_COEX_ALGO_HID_A2DP			= 0x9,
-	BT_8723A_1ANT_COEX_ALGO_MAX
-}BT_8723A_1ANT_COEX_ALGO,*PBT_8723A_1ANT_COEX_ALGO;
-
-typedef struct _COEX_DM_8723A_1ANT{
-	// fw mechanism
-	BOOLEAN		bCurIgnoreWlanAct;
-	BOOLEAN		bPreIgnoreWlanAct;
-	u1Byte		prePsTdma;
-	u1Byte		curPsTdma;
-	u1Byte		psTdmaPara[5];
-	u1Byte		psTdmaDuAdjType;
-	u4Byte		psTdmaMonitorCnt;
-	u4Byte		psTdmaGlobalCnt;
-	BOOLEAN		bResetTdmaAdjust;
-	BOOLEAN		bPrePsTdmaOn;
-	BOOLEAN		bCurPsTdmaOn;
-
-	// sw mechanism
-	BOOLEAN		bPreRfRxLpfShrink;
-	BOOLEAN		bCurRfRxLpfShrink;
-	u4Byte		btRf0x1eBackup;
-	BOOLEAN 	bPreLowPenaltyRa;
-	BOOLEAN		bCurLowPenaltyRa;
-	u4Byte		preVal0x6c0;
-	u4Byte		curVal0x6c0;
-	u4Byte		preVal0x6c8;
-	u4Byte		curVal0x6c8;
-	u1Byte		preVal0x6cc;
-	u1Byte		curVal0x6cc;
-	BOOLEAN		limited_dig;
-
-	// algorithm related
-	u1Byte		preAlgorithm;
-	u1Byte		curAlgorithm;
-	u1Byte		btStatus;
-	u1Byte		wifiChnlInfo[3];
-} COEX_DM_8723A_1ANT, *PCOEX_DM_8723A_1ANT;
-
-typedef struct _COEX_STA_8723A_1ANT{
-	u4Byte					highPriorityTx;
-	u4Byte					highPriorityRx;
-	u4Byte					lowPriorityTx;
-	u4Byte					lowPriorityRx;
-	u1Byte					btRssi;
-	u1Byte					preBtRssiState;
-	u1Byte					preBtRssiState1;
-	u1Byte					preWifiRssiState[4];
-	BOOLEAN					bC2hBtInfoReqSent;
-	u1Byte					btInfoC2h[BT_INFO_SRC_8723A_1ANT_MAX][10];
-	u4Byte					btInfoC2hCnt[BT_INFO_SRC_8723A_1ANT_MAX];
-	BOOLEAN					bC2hBtInquiryPage;
-	u1Byte					btRetryCnt;
-	u1Byte					btInfoExt;
-	//BOOLEAN					bHoldForStackOperation;
-	//u1Byte					bHoldPeriodCnt;
-	// this is for c2h hang work-around
-	u4Byte					c2hHangDetectCnt;
-}COEX_STA_8723A_1ANT, *PCOEX_STA_8723A_1ANT;
-
-//===========================================
-// The following is interface which will notify coex module.
-//===========================================
-VOID
-EXhalbtc8723a1ant_InitHwConfig(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-VOID
-EXhalbtc8723a1ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-VOID
-EXhalbtc8723a1ant_IpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a1ant_LpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a1ant_ScanNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a1ant_ConnectNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a1ant_MediaStatusNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8723a1ant_SpecialPacketNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8723a1ant_BtInfoNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	pu1Byte			tmpBuf,
-	IN	u1Byte			length
-	);
-VOID
-EXhalbtc8723a1ant_StackOperationNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8723a1ant_HaltNotify(
-	IN	PBTC_COEXIST			pBtCoexist
-	);
-VOID
-EXhalbtc8723a1ant_Periodical(
-	IN	PBTC_COEXIST			pBtCoexist
-	);
-VOID
-EXhalbtc8723a1ant_DisplayCoexInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbt_precomp.h b/drivers/staging/rtl8821ae/btcoexist/halbt_precomp.h
deleted file mode 100644
index d538ba3d0a2e..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbt_precomp.h
+++ /dev/null
@@ -1,99 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2011 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-#ifndef	__HALBT_PRECOMP_H__
-#define __HALBT_PRECOMP_H__
-/*************************************************************
- * include files
- *************************************************************/
-#include "../wifi.h"
-#include "../efuse.h"
-#include "../base.h"
-#include "../regd.h"
-#include "../cam.h"
-#include "../ps.h"
-#include "../pci.h"
-#include "../rtl8821ae/reg.h"
-#include "../rtl8821ae/def.h"
-#include "../rtl8821ae/phy.h"
-#include "../rtl8821ae/dm.h"
-#include "../rtl8821ae/fw.h"
-#include "../rtl8821ae/led.h"
-#include "../rtl8821ae/hw.h"
-#include "../rtl8821ae/pwrseqcmd.h"
-#include "../rtl8821ae/pwrseq.h"
-
-#include "halbtcoutsrc.h"
-
-
-#include "halbtc8192e2ant.h"
-#include "halbtc8723b1ant.h"
-#include "halbtc8723b2ant.h"
-
-
-
-#define GetDefaultAdapter(padapter)	padapter
-
-
-#define BIT0	0x00000001
-#define BIT1	0x00000002
-#define BIT2	0x00000004
-#define BIT3	0x00000008
-#define BIT4	0x00000010
-#define BIT5	0x00000020
-#define BIT6	0x00000040
-#define BIT7	0x00000080
-#define BIT8	0x00000100
-#define BIT9	0x00000200
-#define BIT10	0x00000400
-#define BIT11	0x00000800
-#define BIT12	0x00001000
-#define BIT13	0x00002000
-#define BIT14	0x00004000
-#define BIT15	0x00008000
-#define BIT16	0x00010000
-#define BIT17	0x00020000
-#define BIT18	0x00040000
-#define BIT19	0x00080000
-#define BIT20	0x00100000
-#define BIT21	0x00200000
-#define BIT22	0x00400000
-#define BIT23	0x00800000
-#define BIT24	0x01000000
-#define BIT25	0x02000000
-#define BIT26	0x04000000
-#define BIT27	0x08000000
-#define BIT28	0x10000000
-#define BIT29	0x20000000
-#define BIT30	0x40000000
-#define BIT31	0x80000000
-
-#define	MASKBYTE0                	0xff
-#define	MASKBYTE1                	0xff00
-#define	MASKBYTE2                	0xff0000
-#define	MASKBYTE3                	0xff000000
-#define	MASKHWORD                	0xffff0000
-#define	MASKLWORD                	0x0000ffff
-#define	MASKDWORD			0xffffffff
-#define	MASK12BITS			0xfff
-#define	MASKH4BITS			0xf0000000
-#define MASKOFDM_D			0xffc00000
-#define	MASKCCK				0x3f3f3f3f
-
-#endif	/* __HALBT_PRECOMP_H__ */
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.c
deleted file mode 100644
index 1b04530d46bc..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.c
+++ /dev/null
@@ -1,3891 +0,0 @@
-//============================================================
-// Description:
-//
-// This file is for 8192e1ant Co-exist mechanism
-//
-// History
-// 2012/11/15 Cosa first check in.
-//
-//============================================================
-
-//============================================================
-// include files
-//============================================================
-#include "Mp_Precomp.h"
-#if(BT_30_SUPPORT == 1)
-//============================================================
-// Global variables, these are static variables
-//============================================================
-static COEX_DM_8192E_1ANT		GLCoexDm8192e1Ant;
-static PCOEX_DM_8192E_1ANT 	pCoexDm=&GLCoexDm8192e1Ant;
-static COEX_STA_8192E_1ANT		GLCoexSta8192e1Ant;
-static PCOEX_STA_8192E_1ANT	pCoexSta=&GLCoexSta8192e1Ant;
-
-const char *const GLBtInfoSrc8192e1Ant[]={
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-u4Byte	GLCoexVerDate8192e1Ant=20130729;
-u4Byte	GLCoexVer8192e1Ant=0x10;
-
-//============================================================
-// local function proto type if needed
-//============================================================
-//============================================================
-// local function start with halbtc8192e1ant_
-//============================================================
-u1Byte
-halbtc8192e1ant_BtRssiState(
-	u1Byte			levelNum,
-	u1Byte			rssiThresh,
-	u1Byte			rssiThresh1
-	)
-{
-	s4Byte			btRssi=0;
-	u1Byte			btRssiState=pCoexSta->preBtRssiState;
-
-	btRssi = pCoexSta->btRssi;
-
-	if(levelNum == 2)
-	{			
-		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
-			{
-				btRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
-			}
-		}
-		else
-		{
-			if(btRssi < rssiThresh)
-			{
-				btRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
-			}
-		}
-	}
-	else if(levelNum == 3)
-	{
-		if(rssiThresh > rssiThresh1)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
-			return pCoexSta->preBtRssiState;
-		}
-		
-		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
-			{
-				btRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
-			}
-		}
-		else if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
-			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM))
-		{
-			if(btRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
-			{
-				btRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
-			}
-			else if(btRssi < rssiThresh)
-			{
-				btRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
-			}
-		}
-		else
-		{
-			if(btRssi < rssiThresh1)
-			{
-				btRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
-			}
-		}
-	}
-		
-	pCoexSta->preBtRssiState = btRssiState;
-
-	return btRssiState;
-}
-
-u1Byte
-halbtc8192e1ant_WifiRssiState(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			index,
-	IN	u1Byte			levelNum,
-	IN	u1Byte			rssiThresh,
-	IN	u1Byte			rssiThresh1
-	)
-{
-	s4Byte			wifiRssi=0;
-	u1Byte			wifiRssiState=pCoexSta->preWifiRssiState[index];
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
-	
-	if(levelNum == 2)
-	{
-		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
-			{
-				wifiRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
-			}
-		}
-		else
-		{
-			if(wifiRssi < rssiThresh)
-			{
-				wifiRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
-			}
-		}
-	}
-	else if(levelNum == 3)
-	{
-		if(rssiThresh > rssiThresh1)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
-			return pCoexSta->preWifiRssiState[index];
-		}
-		
-		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
-			{
-				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
-			}
-		}
-		else if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_MEDIUM) ||
-			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_MEDIUM))
-		{
-			if(wifiRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT))
-			{
-				wifiRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
-			}
-			else if(wifiRssi < rssiThresh)
-			{
-				wifiRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
-			}
-		}
-		else
-		{
-			if(wifiRssi < rssiThresh1)
-			{
-				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
-			}
-		}
-	}
-		
-	pCoexSta->preWifiRssiState[index] = wifiRssiState;
-
-	return wifiRssiState;
-}
-
-VOID
-halbtc8192e1ant_Updatera_mask(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN				bForceExec,
-	IN	u1Byte				type,
-	IN	u4Byte				rateMask
-	)
-{
-	if(BTC_RATE_DISABLE == type)
-	{
-		pCoexDm->curra_mask |= rateMask;		// disable rate
-	}
-	else if(BTC_RATE_ENABLE == type)
-	{
-		pCoexDm->curra_mask &= ~rateMask;	// enable rate
-	}
-	
-	if( bForceExec || (pCoexDm->prera_mask != pCoexDm->curra_mask))
-	{
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_UPDATE_ra_mask, &pCoexDm->curra_mask);
-	}
-	pCoexDm->prera_mask = pCoexDm->curra_mask;
-}
-
-VOID
-halbtc8192e1ant_MonitorBtCtr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u4Byte 			regHPTxRx, regLPTxRx, u4Tmp;
-	u4Byte			regHPTx=0, regHPRx=0, regLPTx=0, regLPRx=0;
-	u1Byte			u1Tmp;
-	
-	regHPTxRx = 0x770;
-	regLPTxRx = 0x774;
-
-	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regHPTxRx);
-	regHPTx = u4Tmp & MASKLWORD;
-	regHPRx = (u4Tmp & MASKHWORD)>>16;
-
-	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regLPTxRx);
-	regLPTx = u4Tmp & MASKLWORD;
-	regLPRx = (u4Tmp & MASKHWORD)>>16;
-		
-	pCoexSta->highPriorityTx = regHPTx;
-	pCoexSta->highPriorityRx = regHPRx;
-	pCoexSta->lowPriorityTx = regLPTx;
-	pCoexSta->lowPriorityRx = regLPRx;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
-		regHPTxRx, regHPTx, regHPTx, regHPRx, regHPRx));
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
-		regLPTxRx, regLPTx, regLPTx, regLPRx, regLPRx));
-
-	// reset counter
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0xc);
-}
-
-VOID
-halbtc8192e1ant_QueryBtInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-
-	pCoexSta->bC2hBtInfoReqSent = true;
-
-	H2C_Parameter[0] |= BIT0;	// trigger
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Query Bt Info, FW write 0x61=0x%x\n", 
-		H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x61, 1, H2C_Parameter);
-}
-
-BOOLEAN
-halbtc8192e1ant_IsWifiStatusChanged(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	static BOOLEAN	bPreWifiBusy=FALSE, bPreUnder4way=FALSE, bPreBtHsOn=FALSE;
-	BOOLEAN	bWifiBusy=FALSE, bUnder4way=FALSE, bBtHsOn=FALSE;
-	BOOLEAN	bWifiConnected=FALSE;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
-
-	if(bWifiConnected)
-	{
-		if(bWifiBusy != bPreWifiBusy)
-		{
-			bPreWifiBusy = bWifiBusy;
-			return true;
-		}
-		if(bUnder4way != bPreUnder4way)
-		{
-			bPreUnder4way = bUnder4way;
-			return true;
-		}
-		if(bBtHsOn != bPreBtHsOn)
-		{
-			bPreBtHsOn = bBtHsOn;
-			return true;
-		}
-	}
-
-	return FALSE;
-}
-
-VOID
-halbtc8192e1ant_UpdateBtLinkInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
-
-	pBtLinkInfo->bBtLinkExist = pCoexSta->bBtLinkExist;
-	pBtLinkInfo->bScoExist = pCoexSta->bScoExist;
-	pBtLinkInfo->bA2dpExist = pCoexSta->bA2dpExist;
-	pBtLinkInfo->bPanExist = pCoexSta->bPanExist;
-	pBtLinkInfo->bHidExist = pCoexSta->bHidExist;
-
-	// check if Sco only
-	if( pBtLinkInfo->bScoExist &&
-		!pBtLinkInfo->bA2dpExist &&
-		!pBtLinkInfo->bPanExist &&
-		!pBtLinkInfo->bHidExist )
-		pBtLinkInfo->bScoOnly = true;
-	else
-		pBtLinkInfo->bScoOnly = FALSE;
-
-	// check if A2dp only
-	if( !pBtLinkInfo->bScoExist &&
-		pBtLinkInfo->bA2dpExist &&
-		!pBtLinkInfo->bPanExist &&
-		!pBtLinkInfo->bHidExist )
-		pBtLinkInfo->bA2dpOnly = true;
-	else
-		pBtLinkInfo->bA2dpOnly = FALSE;
-
-	// check if Pan only
-	if( !pBtLinkInfo->bScoExist &&
-		!pBtLinkInfo->bA2dpExist &&
-		pBtLinkInfo->bPanExist &&
-		!pBtLinkInfo->bHidExist )
-		pBtLinkInfo->bPanOnly = true;
-	else
-		pBtLinkInfo->bPanOnly = FALSE;
-	
-	// check if Hid only
-	if( !pBtLinkInfo->bScoExist &&
-		!pBtLinkInfo->bA2dpExist &&
-		!pBtLinkInfo->bPanExist &&
-		pBtLinkInfo->bHidExist )
-		pBtLinkInfo->bHidOnly = true;
-	else
-		pBtLinkInfo->bHidOnly = FALSE;
-}
-
-u1Byte
-halbtc8192e1ant_ActionAlgorithm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
-	BOOLEAN				bBtHsOn=FALSE;
-	u1Byte				algorithm=BT_8192E_1ANT_COEX_ALGO_UNDEFINED;
-	u1Byte				numOfDiffProfile=0;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-		
-	if(!pBtLinkInfo->bBtLinkExist)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No BT link exists!!!\n"));
-		return algorithm;
-	}
-
-	if(pBtLinkInfo->bScoExist)
-		numOfDiffProfile++;
-	if(pBtLinkInfo->bHidExist)
-		numOfDiffProfile++;
-	if(pBtLinkInfo->bPanExist)
-		numOfDiffProfile++;
-	if(pBtLinkInfo->bA2dpExist)
-		numOfDiffProfile++;
-	
-	if(numOfDiffProfile == 1)
-	{
-		if(pBtLinkInfo->bScoExist)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
-			algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
-		}
-		else
-		{
-			if(pBtLinkInfo->bHidExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
-				algorithm = BT_8192E_1ANT_COEX_ALGO_HID;
-			}
-			else if(pBtLinkInfo->bA2dpExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
-				algorithm = BT_8192E_1ANT_COEX_ALGO_A2DP;
-			}
-			else if(pBtLinkInfo->bPanExist)
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANHS;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR;
-				}
-			}
-		}
-	}
-	else if(numOfDiffProfile == 2)
-	{
-		if(pBtLinkInfo->bScoExist)
-		{
-			if(pBtLinkInfo->bHidExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
-				algorithm = BT_8192E_1ANT_COEX_ALGO_HID;
-			}
-			else if(pBtLinkInfo->bA2dpExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
-				algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
-			}
-			else if(pBtLinkInfo->bPanExist)
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-		else
-		{
-			if( pBtLinkInfo->bHidExist &&
-				pBtLinkInfo->bA2dpExist )
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
-				algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
-			}
-			else if( pBtLinkInfo->bHidExist &&
-				pBtLinkInfo->bPanExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-			else if( pBtLinkInfo->bPanExist &&
-				pBtLinkInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_A2DP_PANHS;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_A2DP;
-				}
-			}
-		}
-	}
-	else if(numOfDiffProfile == 3)
-	{
-		if(pBtLinkInfo->bScoExist)
-		{
-			if( pBtLinkInfo->bHidExist &&
-				pBtLinkInfo->bA2dpExist )
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP ==> HID\n"));
-				algorithm = BT_8192E_1ANT_COEX_ALGO_HID;
-			}
-			else if( pBtLinkInfo->bHidExist &&
-				pBtLinkInfo->bPanExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(HS)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(EDR)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-			else if( pBtLinkInfo->bPanExist &&
-				pBtLinkInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_SCO;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-		else
-		{
-			if( pBtLinkInfo->bHidExist &&
-				pBtLinkInfo->bPanExist &&
-				pBtLinkInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				}
-			}
-		}
-	}
-	else if(numOfDiffProfile >= 3)
-	{
-		if(pBtLinkInfo->bScoExist)
-		{
-			if( pBtLinkInfo->bHidExist &&
-				pBtLinkInfo->bPanExist &&
-				pBtLinkInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
-
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n"));
-					algorithm = BT_8192E_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-	}
-
-	return algorithm;
-}
-
-VOID
-halbtc8192e1ant_SetFwDacSwingLevel(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			dacSwingLvl
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-
-	// There are several type of dacswing
-	// 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
-	H2C_Parameter[0] = dacSwingLvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Set Dac Swing Level=0x%x\n", dacSwingLvl));
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x64=0x%x\n", H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x64, 1, H2C_Parameter);
-}
-
-VOID
-halbtc8192e1ant_SetFwDecBtPwr(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte				decBtPwrLvl
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-	
-	H2C_Parameter[0] = decBtPwrLvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], decrease Bt Power level = %d, FW write 0x62=0x%x\n", 
-		decBtPwrLvl, H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x62, 1, H2C_Parameter);
-}
-
-VOID
-halbtc8192e1ant_DecBtPwr(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN				bForceExec,
-	IN	u1Byte				decBtPwrLvl
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s Dec BT power level = %d\n",  
-		(bForceExec? "force to":""), decBtPwrLvl));
-	pCoexDm->curBtDecPwrLvl = decBtPwrLvl;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], BtDecPwrLvl=%d, curBtDecPwrLvl=%d\n", 
-			pCoexDm->preBtDecPwrLvl, pCoexDm->curBtDecPwrLvl));
-
-		if(pCoexDm->preBtDecPwrLvl == pCoexDm->curBtDecPwrLvl) 
-			return;
-	}
-	halbtc8192e1ant_SetFwDecBtPwr(pBtCoexist, pCoexDm->curBtDecPwrLvl);
-
-	pCoexDm->preBtDecPwrLvl = pCoexDm->curBtDecPwrLvl;
-}
-
-VOID
-halbtc8192e1ant_SetFwBtLnaConstrain(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bBtLnaConsOn
-	)
-{
-	u1Byte			H2C_Parameter[2] ={0};
-	
-	H2C_Parameter[0] = 0x3;	// opCode, 0x3=BT_SET_LNA_CONSTRAIN
-
-	if(bBtLnaConsOn)
-	{
-		H2C_Parameter[1] |= BIT0;
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT LNA Constrain: %s, FW write 0x69=0x%x\n", 
-		(bBtLnaConsOn? "ON!!":"OFF!!"), 
-		H2C_Parameter[0]<<8|H2C_Parameter[1]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x69, 2, H2C_Parameter);	
-}
-
-VOID
-halbtc8192e1ant_SetBtLnaConstrain(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bBtLnaConsOn
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Constrain = %s\n",  
-		(bForceExec? "force":""), ((bBtLnaConsOn)? "ON":"OFF")));
-	pCoexDm->bCurBtLnaConstrain = bBtLnaConsOn;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtLnaConstrain=%d, bCurBtLnaConstrain=%d\n", 
-			pCoexDm->bPreBtLnaConstrain, pCoexDm->bCurBtLnaConstrain));
-
-		if(pCoexDm->bPreBtLnaConstrain == pCoexDm->bCurBtLnaConstrain) 
-			return;
-	}
-	halbtc8192e1ant_SetFwBtLnaConstrain(pBtCoexist, pCoexDm->bCurBtLnaConstrain);
-
-	pCoexDm->bPreBtLnaConstrain = pCoexDm->bCurBtLnaConstrain;
-}
-
-VOID
-halbtc8192e1ant_SetFwBtPsdMode(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			btPsdMode
-	)
-{
-	u1Byte			H2C_Parameter[2] ={0};
-	
-	H2C_Parameter[0] = 0x2;	// opCode, 0x2=BT_SET_PSD_MODE
-
-	H2C_Parameter[1] = btPsdMode;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set BT PSD mode=0x%x, FW write 0x69=0x%x\n", 
-		H2C_Parameter[1], 
-		H2C_Parameter[0]<<8|H2C_Parameter[1]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x69, 2, H2C_Parameter);	
-}
-
-
-VOID
-halbtc8192e1ant_SetBtPsdMode(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	u1Byte			btPsdMode
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT PSD mode = 0x%x\n",  
-		(bForceExec? "force":""), btPsdMode));
-	pCoexDm->bCurBtPsdMode = btPsdMode;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtPsdMode=0x%x, bCurBtPsdMode=0x%x\n", 
-			pCoexDm->bPreBtPsdMode, pCoexDm->bCurBtPsdMode));
-
-		if(pCoexDm->bPreBtPsdMode == pCoexDm->bCurBtPsdMode) 
-			return;
-	}
-	halbtc8192e1ant_SetFwBtPsdMode(pBtCoexist, pCoexDm->bCurBtPsdMode);
-
-	pCoexDm->bPreBtPsdMode = pCoexDm->bCurBtPsdMode;
-}
-
-
-VOID
-halbtc8192e1ant_SetBtAutoReport(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bEnableAutoReport
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-	
-	H2C_Parameter[0] = 0;
-
-	if(bEnableAutoReport)
-	{
-		H2C_Parameter[0] |= BIT0;
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n", 
-		(bEnableAutoReport? "Enabled!!":"Disabled!!"), H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x68, 1, H2C_Parameter);	
-}
-
-VOID
-halbtc8192e1ant_BtAutoReport(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bEnableAutoReport
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s BT Auto report = %s\n",  
-		(bForceExec? "force to":""), ((bEnableAutoReport)? "Enabled":"Disabled")));
-	pCoexDm->bCurBtAutoReport = bEnableAutoReport;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreBtAutoReport=%d, bCurBtAutoReport=%d\n", 
-			pCoexDm->bPreBtAutoReport, pCoexDm->bCurBtAutoReport));
-
-		if(pCoexDm->bPreBtAutoReport == pCoexDm->bCurBtAutoReport) 
-			return;
-	}
-	halbtc8192e1ant_SetBtAutoReport(pBtCoexist, pCoexDm->bCurBtAutoReport);
-
-	pCoexDm->bPreBtAutoReport = pCoexDm->bCurBtAutoReport;
-}
-
-VOID
-halbtc8192e1ant_FwDacSwingLvl(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	u1Byte			fwDacSwingLvl
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set FW Dac Swing level = %d\n",  
-		(bForceExec? "force to":""), fwDacSwingLvl));
-	pCoexDm->curFwDacSwingLvl = fwDacSwingLvl;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], preFwDacSwingLvl=%d, curFwDacSwingLvl=%d\n", 
-			pCoexDm->preFwDacSwingLvl, pCoexDm->curFwDacSwingLvl));
-
-		if(pCoexDm->preFwDacSwingLvl == pCoexDm->curFwDacSwingLvl) 
-			return;
-	}
-
-	halbtc8192e1ant_SetFwDacSwingLevel(pBtCoexist, pCoexDm->curFwDacSwingLvl);
-
-	pCoexDm->preFwDacSwingLvl = pCoexDm->curFwDacSwingLvl;
-}
-
-VOID
-halbtc8192e1ant_SetSwRfRxLpfCorner(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bRxRfShrinkOn
-	)
-{
-	if(bRxRfShrinkOn)
-	{
-		//Shrink RF Rx LPF corner
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
-	}
-	else
-	{
-		//Resume RF Rx LPF corner
-		// After initialized, we can use pCoexDm->btRf0x1eBackup
-		if(pBtCoexist->initilized)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
-			pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
-		}
-	}
-}
-
-VOID
-halbtc8192e1ant_RfShrink(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bRxRfShrinkOn
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
-		(bForceExec? "force to":""), ((bRxRfShrinkOn)? "ON":"OFF")));
-	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreRfRxLpfShrink=%d, bCurRfRxLpfShrink=%d\n", 
-			pCoexDm->bPreRfRxLpfShrink, pCoexDm->bCurRfRxLpfShrink));
-
-		if(pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink) 
-			return;
-	}
-	halbtc8192e1ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
-
-	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
-}
-
-VOID
-halbtc8192e1ant_SetSwPenaltyTxRateAdaptive(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bLowPenaltyRa
-	)
-{
-	u1Byte	tmpU1;
-
-	tmpU1 = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4fd);
-	tmpU1 |= BIT0;
-	if(bLowPenaltyRa)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
-		tmpU1 &= ~BIT2;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
-		tmpU1 |= BIT2;
-	}
-
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4fd, tmpU1);
-}
-
-VOID
-halbtc8192e1ant_LowPenaltyRa(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bLowPenaltyRa
-	)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
-		(bForceExec? "force to":""), ((bLowPenaltyRa)? "ON":"OFF")));
-	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreLowPenaltyRa=%d, bCurLowPenaltyRa=%d\n", 
-			pCoexDm->bPreLowPenaltyRa, pCoexDm->bCurLowPenaltyRa));
-
-		if(pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa) 
-			return;
-	}
-	halbtc8192e1ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
-
-	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
-}
-
-VOID
-halbtc8192e1ant_SetDacSwingReg(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u4Byte			level
-	)
-{
-	u1Byte	val=(u1Byte)level;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Write SwDacSwing = 0x%x\n", level));
-	pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0x883, 0x3e, val);
-}
-
-VOID
-halbtc8192e1ant_SetSwFullTimeDacSwing(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bSwDacSwingOn,
-	IN	u4Byte			swDacSwingLvl
-	)
-{
-	if(bSwDacSwingOn)
-	{
-		halbtc8192e1ant_SetDacSwingReg(pBtCoexist, swDacSwingLvl);
-	}
-	else
-	{
-		halbtc8192e1ant_SetDacSwingReg(pBtCoexist, 0x18);
-	}
-}
-
-
-VOID
-halbtc8192e1ant_DacSwing(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bDacSwingOn,
-	IN	u4Byte			dacSwingLvl
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn DacSwing=%s, dacSwingLvl=0x%x\n",  
-		(bForceExec? "force to":""), ((bDacSwingOn)? "ON":"OFF"), dacSwingLvl));
-	pCoexDm->bCurDacSwingOn = bDacSwingOn;
-	pCoexDm->curDacSwingLvl = dacSwingLvl;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x, bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n", 
-			pCoexDm->bPreDacSwingOn, pCoexDm->preDacSwingLvl,
-			pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl));
-
-		if( (pCoexDm->bPreDacSwingOn == pCoexDm->bCurDacSwingOn) &&
-			(pCoexDm->preDacSwingLvl == pCoexDm->curDacSwingLvl) )
-			return;
-	}
-	mdelay(30);
-	halbtc8192e1ant_SetSwFullTimeDacSwing(pBtCoexist, bDacSwingOn, dacSwingLvl);
-
-	pCoexDm->bPreDacSwingOn = pCoexDm->bCurDacSwingOn;
-	pCoexDm->preDacSwingLvl = pCoexDm->curDacSwingLvl;
-}
-
-VOID
-halbtc8192e1ant_SetAdcBackOff(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bAdcBackOff
-	)
-{
-	if(bAdcBackOff)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level On!\n"));
-		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0x8db, 0x60, 0x3);
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level Off!\n"));
-		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0x8db, 0x60, 0x1);
-	}
-}
-
-VOID
-halbtc8192e1ant_AdcBackOff(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bAdcBackOff
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn AdcBackOff = %s\n",  
-		(bForceExec? "force to":""), ((bAdcBackOff)? "ON":"OFF")));
-	pCoexDm->bCurAdcBackOff = bAdcBackOff;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n", 
-			pCoexDm->bPreAdcBackOff, pCoexDm->bCurAdcBackOff));
-
-		if(pCoexDm->bPreAdcBackOff == pCoexDm->bCurAdcBackOff) 
-			return;
-	}
-	halbtc8192e1ant_SetAdcBackOff(pBtCoexist, pCoexDm->bCurAdcBackOff);
-
-	pCoexDm->bPreAdcBackOff = pCoexDm->bCurAdcBackOff;
-}
-
-VOID
-halbtc8192e1ant_SetAgcTable(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bAgcTableEn
-	)
-{
-	u1Byte		rssiAdjustVal=0;
-	
-	pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
-	if(bAgcTableEn)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x3fa58);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x37a58);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x2fa58);
-		rssiAdjustVal = 8;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x39258);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x31258);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x3b, 0xfffff, 0x29258);
-	}
-	pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
-
-	// set rssiAdjustVal for wifi module.
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssiAdjustVal);
-}
-
-
-VOID
-halbtc8192e1ant_AgcTable(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bAgcTableEn
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s %s Agc Table\n",  
-		(bForceExec? "force to":""), ((bAgcTableEn)? "Enable":"Disable")));
-	pCoexDm->bCurAgcTableEn = bAgcTableEn;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n", 
-			pCoexDm->bPreAgcTableEn, pCoexDm->bCurAgcTableEn));
-
-		if(pCoexDm->bPreAgcTableEn == pCoexDm->bCurAgcTableEn) 
-			return;
-	}
-	halbtc8192e1ant_SetAgcTable(pBtCoexist, bAgcTableEn);
-
-	pCoexDm->bPreAgcTableEn = pCoexDm->bCurAgcTableEn;
-}
-
-VOID
-halbtc8192e1ant_SetCoexTable(
-	IN	PBTC_COEXIST	pBtCoexist,
-	IN	u4Byte		val0x6c0,
-	IN	u4Byte		val0x6c4,
-	IN	u4Byte		val0x6c8,
-	IN	u1Byte		val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c4=0x%x\n", val0x6c4));
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c4, val0x6c4);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, val0x6cc);
-}
-
-VOID
-halbtc8192e1ant_CoexTable(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	u4Byte			val0x6c0,
-	IN	u4Byte			val0x6c4,
-	IN	u4Byte			val0x6c8,
-	IN	u1Byte			val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
-		(bForceExec? "force to":""), val0x6c0, val0x6c4, val0x6c8, val0x6cc));
-	pCoexDm->curVal0x6c0 = val0x6c0;
-	pCoexDm->curVal0x6c4 = val0x6c4;
-	pCoexDm->curVal0x6c8 = val0x6c8;
-	pCoexDm->curVal0x6cc = val0x6cc;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], preVal0x6c0=0x%x, preVal0x6c4=0x%x, preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n", 
-			pCoexDm->preVal0x6c0, pCoexDm->preVal0x6c4, pCoexDm->preVal0x6c8, pCoexDm->preVal0x6cc));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], curVal0x6c0=0x%x, curVal0x6c4=0x%x, curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n", 
-			pCoexDm->curVal0x6c0, pCoexDm->curVal0x6c4, pCoexDm->curVal0x6c8, pCoexDm->curVal0x6cc));
-	
-		if( (pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
-			(pCoexDm->preVal0x6c4 == pCoexDm->curVal0x6c4) &&
-			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
-			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc) )
-			return;
-	}
-	halbtc8192e1ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c4, val0x6c8, val0x6cc);
-
-	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
-	pCoexDm->preVal0x6c4 = pCoexDm->curVal0x6c4;
-	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
-	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
-}
-
-VOID
-halbtc8192e1ant_CoexTableWithType(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN				bForceExec,
-	IN	u1Byte				type
-	)
-{
-	switch(type)
-	{
-		case 0:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x55555555, 0xffffff, 0x3);
-			break;
-		case 1:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55555555, 0x5a5a5a5a, 0xffffff, 0x3);
-			break;
-		case 2:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x5a5a5a5a, 0x5a5a5a5a, 0xffffff, 0x3);
-			break;
-		case 3:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0xaaaaaaaa, 0xaaaaaaaa, 0xffffff, 0x3);
-			break;
-		case 4:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0xffffffff, 0xffffffff, 0xffffff, 0x3);
-			break;
-		case 5:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x5fff5fff, 0x5fff5fff, 0xffffff, 0x3);
-			break;
-		case 6:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5a5a5a5a, 0xffffff, 0x3);
-			break;
-		case 7:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0xddffddff, 0xddffddff, 0xffffff, 0x3);
-			break;
-		case 8:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x55ff55ff, 0x5afa5afa, 0xffffff, 0x3);
-			break;		
-		case 9:
-			halbtc8192e1ant_CoexTable(pBtCoexist, bForceExec, 0x5f5f5f5f, 0x5f5f5f5f, 0xffffff, 0x3);
-			break;
-		default:
-			break;
-	}
-}
-
-VOID
-halbtc8192e1ant_SetFwIgnoreWlanAct(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bEnable
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-		
-	if(bEnable)
-	{
-		H2C_Parameter[0] |= BIT0;		// function enable
-	}
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x63=0x%x\n", 
-		H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x63, 1, H2C_Parameter);
-}
-
-VOID
-halbtc8192e1ant_IgnoreWlanAct(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bEnable
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
-		(bForceExec? "force to":""), (bEnable? "ON":"OFF")));
-	pCoexDm->bCurIgnoreWlanAct = bEnable;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n", 
-			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
-
-		if(pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
-			return;
-	}
-	halbtc8192e1ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
-
-	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
-}
-
-VOID
-halbtc8192e1ant_SetFwPstdma(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			byte1,
-	IN	u1Byte			byte2,
-	IN	u1Byte			byte3,
-	IN	u1Byte			byte4,
-	IN	u1Byte			byte5
-	)
-{
-	u1Byte			H2C_Parameter[5] ={0};
-
-	H2C_Parameter[0] = byte1;	
-	H2C_Parameter[1] = byte2;	
-	H2C_Parameter[2] = byte3;
-	H2C_Parameter[3] = byte4;
-	H2C_Parameter[4] = byte5;
-
-	pCoexDm->psTdmaPara[0] = byte1;
-	pCoexDm->psTdmaPara[1] = byte2;
-	pCoexDm->psTdmaPara[2] = byte3;
-	pCoexDm->psTdmaPara[3] = byte4;
-	pCoexDm->psTdmaPara[4] = byte5;
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n", 
-		H2C_Parameter[0], 
-		H2C_Parameter[1]<<24|H2C_Parameter[2]<<16|H2C_Parameter[3]<<8|H2C_Parameter[4]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x60, 5, H2C_Parameter);
-}
-
-VOID
-halbtc8192e1ant_SetLpsRpwm(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			lpsVal,
-	IN	u1Byte			rpwmVal
-	)
-{
-	u1Byte	lps=lpsVal;
-	u1Byte	rpwm=rpwmVal;
-	
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_1ANT_LPS, &lps);
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
-}
-
-VOID
-halbtc8192e1ant_LpsRpwm(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	u1Byte			lpsVal,
-	IN	u1Byte			rpwmVal
-	)
-{
-	BOOLEAN	bForceExecPwrCmd=FALSE;
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set lps/rpwm=0x%x/0x%x \n", 
-		(bForceExec? "force to":""), lpsVal, rpwmVal));
-	pCoexDm->curLps = lpsVal;
-	pCoexDm->curRpwm = rpwmVal;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], preLps/curLps=0x%x/0x%x, preRpwm/curRpwm=0x%x/0x%x!!\n", 
-			pCoexDm->preLps, pCoexDm->curLps, pCoexDm->preRpwm, pCoexDm->curRpwm));
-
-		if( (pCoexDm->preLps == pCoexDm->curLps) &&
-			(pCoexDm->preRpwm == pCoexDm->curRpwm) )
-		{
-			return;
-		}
-	}
-	halbtc8192e1ant_SetLpsRpwm(pBtCoexist, lpsVal, rpwmVal);
-
-	pCoexDm->preLps = pCoexDm->curLps;
-	pCoexDm->preRpwm = pCoexDm->curRpwm;
-}
-
-VOID
-halbtc8192e1ant_SwMechanism1(
-	IN	PBTC_COEXIST	pBtCoexist,	
-	IN	BOOLEAN		bShrinkRxLPF,
-	IN	BOOLEAN 	bLowPenaltyRA,
-	IN	BOOLEAN		limited_dig, 
-	IN	BOOLEAN		bBTLNAConstrain
-	) 
-{
-	//halbtc8192e1ant_RfShrink(pBtCoexist, NORMAL_EXEC, bShrinkRxLPF);
-	//halbtc8192e1ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, bLowPenaltyRA);
-
-	//no limited DIG
-	//halbtc8192e1ant_SetBtLnaConstrain(pBtCoexist, NORMAL_EXEC, bBTLNAConstrain);
-}
-
-VOID
-halbtc8192e1ant_SwMechanism2(
-	IN	PBTC_COEXIST	pBtCoexist,	
-	IN	BOOLEAN		bAGCTableShift,
-	IN	BOOLEAN 	bADCBackOff,
-	IN	BOOLEAN		bSWDACSwing,
-	IN	u4Byte		dacSwingLvl
-	)
-{
-	//halbtc8192e1ant_AgcTable(pBtCoexist, NORMAL_EXEC, bAGCTableShift);
-	//halbtc8192e1ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, bADCBackOff);
-	//halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, bSWDACSwing, dacSwingLvl);
-}
-
-VOID
-halbtc8192e1ant_PsTdma(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bTurnOn,
-	IN	u1Byte			type
-	)
-{
-	BOOLEAN			bTurnOnByCnt=FALSE;
-	u1Byte			psTdmaTypeByCnt=0, rssiAdjustVal=0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
-		(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type));
-	pCoexDm->bCurPsTdmaOn = bTurnOn;
-	pCoexDm->curPsTdma = type;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n", 
-			pCoexDm->bPrePsTdmaOn, pCoexDm->bCurPsTdmaOn));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n", 
-			pCoexDm->prePsTdma, pCoexDm->curPsTdma));
-
-		if( (pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
-			(pCoexDm->prePsTdma == pCoexDm->curPsTdma) )
-			return;
-	}
-	if(bTurnOn)
-	{
-		switch(type)
-		{
-			default:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x2c, 0x03, 0x10, 0x50);
-				break;
-			case 1:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x2c, 0x03, 0x10, 0x50);
-				rssiAdjustVal = 11;
-				break;
-			case 2:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x25, 0x03, 0x10, 0x50);
-				rssiAdjustVal = 14;
-				break;
-			case 3:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x40);
-				break;
-			case 4:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
-				rssiAdjustVal = 17;
-				break;
-			case 5:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x61, 0x15, 0x3, 0x31, 0x0);
-				break;
-			case 6:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0x3, 0x0, 0x0);
-				break;
-			case 7:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xc, 0x5, 0x0, 0x0);
-				break;
-			case 8:	
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
-				break;
-			case 9:	
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x1e, 0x03, 0x10, 0x50);
-				rssiAdjustVal = 18;
-				break;
-			case 10:	
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0xa, 0x0, 0x40);
-				break;
-			case 11:	
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x12, 0x03, 0x10, 0x50);
-				rssiAdjustVal = 20;
-				break;
-			case 12:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xeb, 0xa, 0x3, 0x31, 0x18);
-				break;
-
-			case 15:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0xa, 0x3, 0x8, 0x0);
-				break;
-			case 16:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x15, 0x3, 0x10, 0x0);
-				rssiAdjustVal = 18;
-				break;
-
-			case 18:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x25, 0x3, 0x10, 0x0);
-				rssiAdjustVal = 14;
-				break;			
-				
-			case 20:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0x25, 0x25, 0x0, 0x0);
-				break;
-			case 21:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x20, 0x3, 0x10, 0x40);
-				break;
-			case 22:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x13, 0x8, 0x8, 0x0, 0x40);
-				break;
-			case 23:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x18);
-				rssiAdjustVal = 22;
-				break;
-			case 24:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x15, 0x3, 0x31, 0x18);
-				rssiAdjustVal = 22;
-				break;
-			case 25:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
-				rssiAdjustVal = 22;
-				break;
-			case 26:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0x3, 0x31, 0x18);
-				rssiAdjustVal = 22;
-				break;
-			case 27:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x3, 0x31, 0x98);
-				rssiAdjustVal = 22;
-				break;
-			case 28:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x69, 0x25, 0x3, 0x31, 0x0);
-				break;
-			case 29:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xab, 0x1a, 0x1a, 0x1, 0x10);
-				break;
-			case 30:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x93, 0x15, 0x3, 0x14, 0x0);
-				break;
-			case 31:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xd3, 0x1a, 0x1a, 0, 0x58);
-				break;
-			case 32:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xab, 0xa, 0x3, 0x31, 0x90);
-				break;
-			case 33:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xa3, 0x25, 0x3, 0x30, 0x90);
-				break;
-			case 34:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xd3, 0x1a, 0x1a, 0x0, 0x10);
-				break;
-			case 35:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x0, 0x10);
-				break;
-			case 36:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xd3, 0x12, 0x3, 0x14, 0x50);
-				break;
-			case 37:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x53, 0x25, 0x3, 0x10, 0x50);
-				break;
-			case 38:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x90);
-				break;
-		}
-	}
-	else
-	{
-		// disable PS tdma
-		switch(type)
-		{
-			case 8:		//0x778 = 1, ant2PTA
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x8, 0x0, 0x0, 0x0, 0x0);
-				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x4);
-				break;
-			case 0:		//0x778 = 1, ant2BT
-			default:
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
-				mdelay(5);
-				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x20);
-				break;
-			case 9:		//0x778 = 1, ant2WIFI
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
-				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x4);
-				break;
-			case 10:	//0x778 = 3, ant2BT
-				halbtc8192e1ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
-				mdelay(5);
-				pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x20);
-				break;
-		}
-	}
-	rssiAdjustVal =0;
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE, &rssiAdjustVal);
-
-	// update pre state
-	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
-	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
-}
-
-VOID
-halbtc8192e1ant_SetSwitchSsType(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte				ssType
-	)
-{
-	u1Byte	mimoPs=BTC_MIMO_PS_DYNAMIC;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], REAL set SS Type = %d\n", ssType));
-
-	if(ssType == 1)
-	{
-		halbtc8192e1ant_Updatera_mask(pBtCoexist, FORCE_EXEC, BTC_RATE_DISABLE, 0xfff00000);	// disable 2ss
-		halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
-		// switch ofdm path
-		pBtCoexist->btc_write_1byte(pBtCoexist, 0xc04, 0x11);
-		pBtCoexist->btc_write_1byte(pBtCoexist, 0xd04, 0x1);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0x90c, 0x81111111);
-		// switch cck patch
-		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0xe77, 0x4, 0x1);
-		pBtCoexist->btc_write_1byte(pBtCoexist, 0xa07, 0x81);
-		mimoPs=BTC_MIMO_PS_STATIC;
-	}
-	else if(ssType == 2)
-	{
-		halbtc8192e1ant_Updatera_mask(pBtCoexist, FORCE_EXEC, BTC_RATE_ENABLE, 0xfff00000);	// enable 2ss
-		halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 8);
-		pBtCoexist->btc_write_1byte(pBtCoexist, 0xc04, 0x33);
-		pBtCoexist->btc_write_1byte(pBtCoexist, 0xd04, 0x3);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0x90c, 0x81121313);
-		pBtCoexist->btc_write_1byte_bitmask(pBtCoexist, 0xe77, 0x4, 0x0);
-		pBtCoexist->btc_write_1byte(pBtCoexist, 0xa07, 0x41);
-		mimoPs=BTC_MIMO_PS_DYNAMIC;
-	}
-	
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_SEND_MIMO_PS, &mimoPs);	// set rx 1ss or 2ss
-}
-
-VOID
-halbtc8192e1ant_SwitchSsType(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN				bForceExec,
-	IN	u1Byte				newSsType
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], %s Switch SS Type = %d\n",  
-		(bForceExec? "force to":""), newSsType));
-	pCoexDm->curSsType = newSsType;
-
-	if(!bForceExec)
-	{
-		if(pCoexDm->preSsType == pCoexDm->curSsType) 
-			return;
-	}
-	halbtc8192e1ant_SetSwitchSsType(pBtCoexist, pCoexDm->curSsType);
-
-	pCoexDm->preSsType = pCoexDm->curSsType;
-}
-
-VOID
-halbtc8192e1ant_CoexAllOff(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-
-	// sw all off
-	halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
-	halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-
-
-	// hw all off
-	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-}
-
-BOOLEAN
-halbtc8192e1ant_IsCommonAction(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN			bCommon=FALSE, bWifiConnected=FALSE, bWifiBusy=FALSE;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
-
-	if(!bWifiConnected && 
-		BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT non connected-idle!!\n"));		
- 		halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
-		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-
-		bCommon = true;
-	}
-	else if(bWifiConnected && 
-		(BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT non connected-idle!!\n"));
-      	halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
-		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-
-		bCommon = true;
-	}
-	else if(!bWifiConnected && 
-		(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT connected-idle!!\n"));
-		halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
-		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-
-		bCommon = true;
-	}
-	else if(bWifiConnected && 
-		(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi connected + BT connected-idle!!\n"));
-		halbtc8192e1ant_SwMechanism1(pBtCoexist,true,true,true,true);
-		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-
-		bCommon = true;
-	}
-	else if(!bWifiConnected && 
-		(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non connected-idle + BT Busy!!\n"));
-		halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
-		halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		
-		bCommon = true;
-	}
-	else
-	{
-		halbtc8192e1ant_SwMechanism1(pBtCoexist,true,true,true,true);
-		
-		bCommon = FALSE;
-	}
-	
-	return bCommon;
-}
-
-
-VOID
-halbtc8192e1ant_TdmaDurationAdjustForAcl(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte				wifiStatus
-	)
-{
-	static s4Byte		up,dn,m,n,WaitCount;
-	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
-	u1Byte			retryCount=0, btInfoExt;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], TdmaDurationAdjustForAcl()\n"));
-
-	if( (BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifiStatus) ||
-		(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifiStatus) ||
-		(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifiStatus) )
-	{
-		if( pCoexDm->curPsTdma != 1 &&
-			pCoexDm->curPsTdma != 2 &&
-			pCoexDm->curPsTdma != 3 &&
-			pCoexDm->curPsTdma != 9 )
-		{
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-			pCoexDm->psTdmaDuAdjType = 9;
-
-			up = 0;
-			dn = 0;
-			m = 1;
-			n= 3;
-			result = 0;
-			WaitCount = 0;
-		}		
-		return;
-	}
-
-	if(!pCoexDm->bAutoTdmaAdjust)
-	{
-		pCoexDm->bAutoTdmaAdjust = true;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
-
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-		pCoexDm->psTdmaDuAdjType = 2;
-		//============
-		up = 0;
-		dn = 0;
-		m = 1;
-		n= 3;
-		result = 0;
-		WaitCount = 0;
-	}
-	else
-	{
-		//acquire the BT TRx retry count from BT_Info byte2
-		retryCount = pCoexSta->btRetryCnt;
-		btInfoExt = pCoexSta->btInfoExt;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], up=%d, dn=%d, m=%d, n=%d, WaitCount=%d\n", 
-			up, dn, m, n, WaitCount));
-		result = 0;
-		WaitCount++; 
-		  
-		if(retryCount == 0)  // no retry in the last 2-second duration
-		{
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0;				 
-
-			if(up >= n)	// if ³sÄò n ­Ó2¬í retry count¬°0, «h½Õ¼eWiFi duration
-			{
-				WaitCount = 0; 
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
-			}
-		}
-		else if (retryCount <= 3)	// <=3 retry in the last 2-second duration
-		{
-			up--; 
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2)	// if ³sÄò 2 ­Ó2¬í retry count< 3, «h½Õ¯¶WiFi duration
-			{
-				if (WaitCount <= 2)
-					m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
-				else
-					m = 1;
-
-				if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
-					m = 20;
-
-				n = 3*m;
-				up = 0;
-				dn = 0;
-				WaitCount = 0;
-				result = -1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
-			}
-		}
-		else  //retry count > 3, ¥u­n1¦¸ retry count > 3, «h½Õ¯¶WiFi duration
-		{
-			if (WaitCount == 1)
-				m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
-			else
-				m = 1;
-
-			if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
-				m = 20;
-
-			n = 3*m;
-			up = 0;
-			dn = 0;
-			WaitCount = 0; 
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
-		}
-
-		if(result == -1)
-		{
-			if( (BT_INFO_8192E_1ANT_A2DP_BASIC_RATE(btInfoExt)) &&
-				((pCoexDm->curPsTdma == 1) ||(pCoexDm->curPsTdma == 2)) )
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-				pCoexDm->psTdmaDuAdjType = 9;
-			}
-			else if(pCoexDm->curPsTdma == 1)
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-				pCoexDm->psTdmaDuAdjType = 2;
-			}
-			else if(pCoexDm->curPsTdma == 2)
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-				pCoexDm->psTdmaDuAdjType = 9;
-			}
-			else if(pCoexDm->curPsTdma == 9)
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-				pCoexDm->psTdmaDuAdjType = 11;
-			}
-		}
-		else if(result == 1)
-		{
-			if( (BT_INFO_8192E_1ANT_A2DP_BASIC_RATE(btInfoExt)) &&
-				((pCoexDm->curPsTdma == 1) ||(pCoexDm->curPsTdma == 2)) )
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-				pCoexDm->psTdmaDuAdjType = 9;
-			}
-			else if(pCoexDm->curPsTdma == 11)
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-				pCoexDm->psTdmaDuAdjType = 9;
-			}
-			else if(pCoexDm->curPsTdma == 9)
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-				pCoexDm->psTdmaDuAdjType = 2;
-			}
-			else if(pCoexDm->curPsTdma == 2)
-			{
-				halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-				pCoexDm->psTdmaDuAdjType = 1;
-			}
-		}
-
-		if( pCoexDm->curPsTdma != 1 &&
-			pCoexDm->curPsTdma != 2 &&
-			pCoexDm->curPsTdma != 9 &&
-			pCoexDm->curPsTdma != 11 )
-		{
-			// recover to previous adjust type
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
-		}
-	}
-}
-
-u1Byte
-halbtc8192e1ant_PsTdmaTypeByWifiRssi(
-	IN	s4Byte	wifiRssi,
-	IN	s4Byte	preWifiRssi,
-	IN	u1Byte	wifiRssiThresh
-	)
-{
-	u1Byte	psTdmaType=0;
-	
-	if(wifiRssi > preWifiRssi)
-	{
-		if(wifiRssi > (wifiRssiThresh+5))
-		{
-			psTdmaType = 26;
-		}
-		else
-		{
-			psTdmaType = 25;
-		}
-	}
-	else
-	{
-		if(wifiRssi > wifiRssiThresh)
-		{
-			psTdmaType = 26;
-		}
-		else
-		{
-			psTdmaType = 25;
-		}
-	}
-
-	return psTdmaType;
-}
-
-VOID
-halbtc8192e1ant_PsTdmaCheckForPowerSaveState(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bNewPsState
-	)
-{
-	u1Byte	lpsMode=0x0;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_LPS_MODE, &lpsMode);
-	
-	if(lpsMode)	// already under LPS state
-	{
-		if(bNewPsState)		
-		{
-			// keep state under LPS, do nothing.
-		}
-		else
-		{
-			// will leave LPS state, turn off psTdma first
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		}
-	}
-	else						// NO PS state
-	{
-		if(bNewPsState)
-		{
-			// will enter LPS state, turn off psTdma first
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		}
-		else
-		{
-			// keep state under NO PS state, do nothing.
-		}
-	}
-}
-
-VOID
-halbtc8192e1ant_PowerSaveState(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte				psType,
-	IN	u1Byte				lpsVal,
-	IN	u1Byte				rpwmVal
-	)
-{
-	BOOLEAN		bLowPwrDisable=FALSE;
-
-	switch(psType)
-	{
-		case BTC_PS_WIFI_NATIVE:
-			// recover to original 32k low power setting
-			bLowPwrDisable = FALSE;
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
-			break;
-		case BTC_PS_LPS_ON:
-			halbtc8192e1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, true);
-			halbtc8192e1ant_LpsRpwm(pBtCoexist, NORMAL_EXEC, lpsVal, rpwmVal);
-			// when coex force to enter LPS, do not enter 32k low power.
-			bLowPwrDisable = true;
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
-			// power save must executed before psTdma.
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-			break;
-		case BTC_PS_LPS_OFF:
-			halbtc8192e1ant_PsTdmaCheckForPowerSaveState(pBtCoexist, FALSE);
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-			break;
-		default:
-			break;
-	}
-}
-
-
-VOID
-halbtc8192e1ant_ActionWifiOnly(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-	halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
-}
-
-VOID
-halbtc8192e1ant_MonitorBtEnableDisable(
-	IN 	PBTC_COEXIST		pBtCoexist
-	)
-{
-	static BOOLEAN	bPreBtDisabled=FALSE;
-	static u4Byte		btDisableCnt=0;
-	BOOLEAN			bBtActive=true, bBtDisabled=FALSE;
-
-	// This function check if bt is disabled
-
-	if(	pCoexSta->highPriorityTx == 0 &&
-		pCoexSta->highPriorityRx == 0 &&
-		pCoexSta->lowPriorityTx == 0 &&
-		pCoexSta->lowPriorityRx == 0)
-	{
-		bBtActive = FALSE;
-	}
-	if(	pCoexSta->highPriorityTx == 0xffff &&
-		pCoexSta->highPriorityRx == 0xffff &&
-		pCoexSta->lowPriorityTx == 0xffff &&
-		pCoexSta->lowPriorityRx == 0xffff)
-	{
-		bBtActive = FALSE;
-	}
-	if(bBtActive)
-	{
-		btDisableCnt = 0;
-		bBtDisabled = FALSE;
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
-	}
-	else
-	{
-		btDisableCnt++;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
-				btDisableCnt));
-		if(btDisableCnt >= 2)
-		{
-			bBtDisabled = true;
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
-			halbtc8192e1ant_ActionWifiOnly(pBtCoexist);
-		}
-	}
-	if(bPreBtDisabled != bBtDisabled)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
-			(bPreBtDisabled ? "disabled":"enabled"), 
-			(bBtDisabled ? "disabled":"enabled")));
-		bPreBtDisabled = bBtDisabled;
-		if(!bBtDisabled)
-		{
-		}
-		else
-		{
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
-		}
-	}
-}
-
-//=============================================
-//
-//	Software Coex Mechanism start
-//
-//=============================================
-
-// SCO only or SCO+PAN(HS)
-VOID
-halbtc8192e1ant_ActionSco(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte	wifiRssiState;
-	u4Byte	wifiBw;
-
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-	
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			  halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			  halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}		
-	}
-}
-
-
-VOID
-halbtc8192e1ant_ActionHid(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte	wifiRssiState;	
-	u4Byte	wifiBw;
-
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,FALSE,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}		
-	}
-}
-
-//A2DP only / PAN(EDR) only/ A2DP+PAN(HS)
-VOID
-halbtc8192e1ant_ActionA2dp(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState;
-	u4Byte		wifiBw;
-
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}		
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionA2dpPanHs(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}		
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionPanEdr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState;
-	u4Byte		wifiBw;
-
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}
-	}
-}
-
-
-//PAN(HS) only
-VOID
-halbtc8192e1ant_ActionPanHs(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState;
-	u4Byte		wifiBw;
-
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}
-	}
-}
-
-//PAN(EDR)+A2DP
-VOID
-halbtc8192e1ant_ActionPanEdrA2dp(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionPanEdrHid(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState;
-	u4Byte		wifiBw;
-
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{		
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}
-	}
-}
-
-// HID+A2DP+PAN(EDR)
-VOID
-halbtc8192e1ant_ActionHidA2dpPanEdr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{	
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionHidA2dp(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-	wifiRssiState = halbtc8192e1ant_WifiRssiState(pBtCoexist, 0, 2, 25, 0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{		
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,true,FALSE,0x18);
-		}
-	}
-	else
-	{
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,true,true,FALSE,0x18);
-		}
-		else
-		{
-			halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-		}
-	}
-}
-
-//=============================================
-//
-//	Non-Software Coex Mechanism start
-//
-//=============================================
-VOID
-halbtc8192e1ant_ActionBtInquiry(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
-	BOOLEAN				bWifiConnected=FALSE, bBtHsOn=FALSE;
-	
-	// Note:
-	// Do not do DacSwing here, use original setting.
-	
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	if(bBtHsOn)
-		return;
-
-	if(!bWifiConnected)
-	{
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-		
-		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-	}
-	else if( (pBtLinkInfo->bScoExist) ||
-			(pBtLinkInfo->bHidOnly) )
-	{
-		// SCO/HID-only busy
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-		
-		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 32);
-		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 1);
-	}
-	else
-	{
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
-
-		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 30);
-		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionBtScoHidOnlyBusy(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte				wifiStatus
-	)
-{
-	PBTC_BT_LINK_INFO pBtLinkInfo=&pBtCoexist->bt_link_info;
-	u1Byte		btRssiState=BTC_RSSI_STATE_HIGH;
-
-	if(BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifiStatus)
-	{		
-		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-	
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-	}
-	else
-	{
-		if(pBtLinkInfo->bHidOnly)
-		{
-			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
-		}
-		else
-		{
-			// dec bt power for diff level
-			btRssiState = halbtc8192e1ant_BtRssiState(3, 34, 42);
-			if( (btRssiState == BTC_RSSI_STATE_LOW) ||
-				(btRssiState == BTC_RSSI_STATE_STAY_LOW) )
-			{
-				halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-			}
-			else if( (btRssiState == BTC_RSSI_STATE_MEDIUM) ||
-					(btRssiState == BTC_RSSI_STATE_STAY_MEDIUM) )
-			{
-				halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 2);
-			}
-			else if( (btRssiState == BTC_RSSI_STATE_HIGH) ||
-					(btRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-			{
-				halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 6);
-			}
-
-			// sw dacSwing
-			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, true, 0xc);
-
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 7);			
-		}
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionHs(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_BT_LINK_INFO pBtLinkInfo=&pBtCoexist->bt_link_info;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action for HS!!!\n"));
-
-	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-
-	if(BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
-	{
-		// error, should not be here
-		pCoexDm->errorCondition = 1;
-	}
-	else if(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
-	{
-		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, true, 6);
-
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 10);
-		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-	}
-	else if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && 
-			!pBtCoexist->bt_link_info.bHidOnly)
-	{
-		if(pCoexDm->curSsType == 1)
-		{
-			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, true, 6);
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 10);
-			//halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 38);
-			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-		}
-	}
-	else
-	{
-		halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
-			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY);
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionWifiConnectedBtAclBusy(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte				wifiStatus
-	)
-{
-	PBTC_BT_LINK_INFO pBtLinkInfo=&pBtCoexist->bt_link_info;
-
-	if(pBtLinkInfo->bHidOnly)
-	{
-		halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist, wifiStatus);
-		pCoexDm->bAutoTdmaAdjust = FALSE;
-		return;
-	}
-	
-	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-	halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-
-	if( (pBtLinkInfo->bA2dpOnly) ||
-		(pBtLinkInfo->bHidExist&&pBtLinkInfo->bA2dpExist) )
-	{
-		halbtc8192e1ant_TdmaDurationAdjustForAcl(pBtCoexist, wifiStatus);
-	}
-	else if( (pBtLinkInfo->bPanOnly) ||
-			(pBtLinkInfo->bHidExist&&pBtLinkInfo->bPanExist) )
-	{
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-		pCoexDm->bAutoTdmaAdjust = FALSE;
-	}
-	else
-	{
-		if( (BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN == wifiStatus) ||
-			(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifiStatus) ||
-			(BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifiStatus) )
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-		else
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-		pCoexDm->bAutoTdmaAdjust = FALSE;
-	}
-		
-	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 1);
-}
-
-
-VOID
-halbtc8192e1ant_ActionWifiNotConnected(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	// power save state
-	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-
-	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-	halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-
-	halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);	
-}
-
-VOID
-halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-
-	halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-	halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-
-	halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-	halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 0);
-}
-
-VOID
-halbtc8192e1ant_ActionWifiConnectedScan(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	// power save state
-	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && !pBtCoexist->bt_link_info.bHidOnly)
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
-	else
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-
-	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
-	{
-		halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist,
-			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN);
-	}
-	else if( (BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
-			(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
-	{
-		halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
-			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN);
-	}
-	else
-	{		
-		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
-	}
-}
-
-
-VOID
-halbtc8192e1ant_ActionWifiConnectedSpecialPacket(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	// power save state
-	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && !pBtCoexist->bt_link_info.bHidOnly)
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
-	else
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-
-	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
-	{
-		halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist,
-			BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT);
-	}
-	else
-	{		
-		halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-		halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-		halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
-	}
-}
-
-VOID
-halbtc8192e1ant_ActionWifiConnected(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN 	bWifiConnected=FALSE, bWifiBusy=FALSE;
-	BOOLEAN 	bScan=FALSE, bLink=FALSE, bRoam=FALSE;
-	BOOLEAN 	bUnder4way=FALSE;
-	u4Byte		wifiBw;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect()===>\n"));
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-	if(!bWifiConnected)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi not connected<===\n"));
-		return;
-	}
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS, &bUnder4way);
-	if(bUnder4way)
-	{
-		halbtc8192e1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under 4way<===\n"));
-		return;
-	}
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-	if(bScan || bLink || bRoam)
-	{
-		halbtc8192e1ant_ActionWifiConnectedScan(pBtCoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], CoexForWifiConnect(), return for wifi is under scan<===\n"));
-		return;
-	}
-
-	// power save state
-	if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus && !pBtCoexist->bt_link_info.bHidOnly)
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_LPS_ON, 0x50, 0x0);
-	else
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);	
-	if(!bWifiBusy)
-	{
-		if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
-		{
-			halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist, 
-				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_IDLE);
-		}
-		else if( (BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
-			(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
-		{
-			halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
-				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_IDLE);
-		}
-		else
-		{				
-			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
-		}
-	}
-	else
-	{
-		if(BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)
-		{				
-			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
-			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
-		}
-		else if(BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)
-		{				
-			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);			
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
-			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
-		}
-		else if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus)
-		{
-			halbtc8192e1ant_ActionWifiConnectedBtAclBusy(pBtCoexist,
-				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY);
-		}
-		else if( (BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
-			(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
-		{
-			halbtc8192e1ant_ActionBtScoHidOnlyBusy(pBtCoexist,
-				BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY);
-		}
-		else 
-		{				
-			halbtc8192e1ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, 0);
-			halbtc8192e1ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 8);
-			halbtc8192e1ant_CoexTableWithType(pBtCoexist, NORMAL_EXEC, 2);
-		}
-	}
-}
-
-VOID
-halbtc8192e1ant_RunSwCoexistMechanism(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN				bWifiUnder5G=FALSE, bWifiBusy=FALSE, bWifiConnected=FALSE;
-	u1Byte				btInfoOriginal=0, btRetryCnt=0;
-	u1Byte				algorithm=0;
-
-	return;
-
-	algorithm = halbtc8192e1ant_ActionAlgorithm(pBtCoexist);
-	pCoexDm->curAlgorithm = algorithm;
-
-	if(halbtc8192e1ant_IsCommonAction(pBtCoexist))
-	{
-	}
-	else
-	{
-		switch(pCoexDm->curAlgorithm)
-		{
-			case BT_8192E_1ANT_COEX_ALGO_SCO:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = SCO.\n"));
-				halbtc8192e1ant_ActionSco(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_HID:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID.\n"));
-				halbtc8192e1ant_ActionHid(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_A2DP:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP.\n"));
-				halbtc8192e1ant_ActionA2dp(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_A2DP_PANHS:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = A2DP+PAN(HS).\n"));
-				halbtc8192e1ant_ActionA2dpPanHs(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_PANEDR:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR).\n"));
-				halbtc8192e1ant_ActionPanEdr(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_PANHS:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HS mode.\n"));
-				halbtc8192e1ant_ActionPanHs(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_PANEDR_A2DP:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN+A2DP.\n"));
-				halbtc8192e1ant_ActionPanEdrA2dp(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_PANEDR_HID:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = PAN(EDR)+HID.\n"));
-				halbtc8192e1ant_ActionPanEdrHid(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP+PAN.\n"));
-				halbtc8192e1ant_ActionHidA2dpPanEdr(pBtCoexist);
-				break;
-			case BT_8192E_1ANT_COEX_ALGO_HID_A2DP:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = HID+A2DP.\n"));
-				halbtc8192e1ant_ActionHidA2dp(pBtCoexist);
-				break;
-			default:
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action algorithm = coexist All Off!!\n"));
-				halbtc8192e1ant_CoexAllOff(pBtCoexist);
-				break;
-		}
-		pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
-	}
-}
-
-VOID
-halbtc8192e1ant_RunCoexistMechanism(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
-	BOOLEAN	bWifiConnected=FALSE, bBtHsOn=FALSE;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism()===>\n"));
-
-	if(pBtCoexist->manual_control)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Manual CTRL <===\n"));
-		return;
-	}
-
-	if(pBtCoexist->bStopCoexDm)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], RunCoexistMechanism(), return for Stop Coex DM <===\n"));
-		return;
-	}
-
-	if(pCoexSta->bUnderIps)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is under IPS !!!\n"));
-		return;
-	}
-	
-	halbtc8192e1ant_RunSwCoexistMechanism(pBtCoexist);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	if(pCoexSta->bC2hBtInquiryPage)
-	{
-		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
-		return;
-	}
-
-	// 1ss or 2ss
-	if(pBtLinkInfo->bScoExist)
-	{
-		halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 1);
-	}
-	else if(bBtHsOn)
-	{
-		if(pBtLinkInfo->bHidOnly)
-			halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 2);
-		else
-			halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 1);
-	}
-	else
-		halbtc8192e1ant_SwitchSsType(pBtCoexist, NORMAL_EXEC, 2);
-	
-	if(bBtHsOn)
-	{
-		halbtc8192e1ant_ActionHs(pBtCoexist);
-		return;
-	}
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-	if(!bWifiConnected)
-	{
-		BOOLEAN	bScan=FALSE, bLink=FALSE, bRoam=FALSE;
-		
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], wifi is non connected-idle !!!\n"));
-
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-
-		if(bScan || bLink || bRoam)
-			halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(pBtCoexist);
-		else
-			halbtc8192e1ant_ActionWifiNotConnected(pBtCoexist);
-	}
-	else
-	{
-		halbtc8192e1ant_ActionWifiConnected(pBtCoexist);
-	}
-}
-
-VOID
-halbtc8192e1ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{	
-	// force to reset coex mechanism
-	halbtc8192e1ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 6);
-	halbtc8192e1ant_DecBtPwr(pBtCoexist, FORCE_EXEC, 0);
-
-	// sw all off
-	halbtc8192e1ant_SwMechanism1(pBtCoexist,FALSE,FALSE,FALSE,FALSE);
-	halbtc8192e1ant_SwMechanism2(pBtCoexist,FALSE,FALSE,FALSE,0x18);
-
-	halbtc8192e1ant_SwitchSsType(pBtCoexist, FORCE_EXEC, 2);
-
-	halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 8);
-	halbtc8192e1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
-}
-
-//============================================================
-// work around function start with wa_halbtc8192e1ant_
-//============================================================
-//============================================================
-// extern function start with EXhalbtc8192e1ant_
-//============================================================
-VOID
-EXhalbtc8192e1ant_InitHwConfig(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u4Byte	u4Tmp=0;
-	u16	u2Tmp=0;
-	u1Byte	u1Tmp=0;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 1Ant Init HW Config!!\n"));
-
-	// backup rf 0x1e value
-	pCoexDm->btRf0x1eBackup = 
-		pBtCoexist->btc_get_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
-
-	// antenna sw ctrl to bt
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4f, 0x6);
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x944, 0x24);
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x930, 0x700700);
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x92c, 0x20);
-	if(pBtCoexist->chipInterface == BTC_INTF_USB)
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0x64, 0x30430004);
-	else
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0x64, 0x30030004);
-
-	halbtc8192e1ant_CoexTableWithType(pBtCoexist, FORCE_EXEC, 0);
-
-	// antenna switch control parameter
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x858, 0x55555555);
-
-	// coex parameters
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x778, 0x1);
-	// 0x790[5:0]=0x5
-	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x790);
-	u1Tmp &= 0xc0;
-	u1Tmp |= 0x5;
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x790, u1Tmp);
-
-	// enable counter statistics
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0x4);
-
-	// enable PTA
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x40, 0x20);
-	// enable mailbox interface
-	u2Tmp = pBtCoexist->btc_read_2byte(pBtCoexist, 0x40);
-	u2Tmp |= BIT9;
-	pBtCoexist->btc_write_2byte(pBtCoexist, 0x40, u2Tmp);
-
-	// enable PTA I2C mailbox 
-	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x101);
-	u1Tmp |= BIT4;
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x101, u1Tmp);
-
-	// enable bt clock when wifi is disabled.
-	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x93);
-	u1Tmp |= BIT0;
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x93, u1Tmp);
-	// enable bt clock when suspend.
-	u1Tmp = pBtCoexist->btc_read_1byte(pBtCoexist, 0x7);
-	u1Tmp |= BIT0;
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x7, u1Tmp);
-}
-
-VOID
-EXhalbtc8192e1ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
-
-	pBtCoexist->bStopCoexDm = FALSE;
-	
-	halbtc8192e1ant_InitCoexDm(pBtCoexist);
-}
-
-VOID
-EXhalbtc8192e1ant_DisplayCoexInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
-	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
-	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
-	pu1Byte				cliBuf=pBtCoexist->cli_buf;
-	u1Byte				u1Tmp[4], i, btInfoExt, psTdmaCase=0;
-	u4Byte				u4Tmp[4];
-	BOOLEAN				bRoam=FALSE, bScan=FALSE, bLink=FALSE, bWifiUnder5G=FALSE;
-	BOOLEAN				bBtHsOn=FALSE, bWifiBusy=FALSE;
-	s4Byte				wifiRssi=0, btHsRssi=0;
-	u4Byte				wifiBw, wifiTrafficDir;
-	u1Byte				wifiDot11Chnl, wifiHsChnl;
-	u4Byte				fwVer=0, btPatchVer=0;
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cliBuf);
-
-	if(pBtCoexist->manual_control)
-	{
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[Under Manual Control]============");
-		CL_PRINTF(cliBuf);
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
-		CL_PRINTF(cliBuf);
-	}	
-	if(pBtCoexist->bStopCoexDm)
-	{
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[Coex is STOPPED]============");
-		CL_PRINTF(cliBuf);
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ==========================================");
-		CL_PRINTF(cliBuf);
-	}
-
-	if(!pBoardInfo->bt_exist)
-	{
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cliBuf);
-		return;
-	}
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
-		pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num);
-	CL_PRINTF(cliBuf);	
-	
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
-		((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)", "CoexVer/ FwVer/ PatchVer", \
-		GLCoexVerDate8192e1Ant, GLCoexVer8192e1Ant, fwVer, btPatchVer, btPatchVer);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
-		wifiDot11Chnl, wifiHsChnl, bBtHsOn);
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
-		pCoexDm->wifiChnlInfo[0], pCoexDm->wifiChnlInfo[1],
-		pCoexDm->wifiChnlInfo[2]);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
-		wifiRssi, btHsRssi);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
-		bLink, bRoam, bScan);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
-		(bWifiUnder5G? "5G":"2.4G"),
-		((BTC_WIFI_BW_LEGACY==wifiBw)? "Legacy": (((BTC_WIFI_BW_HT40==wifiBw)? "HT40":"HT20"))),
-		((!bWifiBusy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
-	CL_PRINTF(cliBuf);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
-		((pBtCoexist->btInfo.bBtDisabled)? ("disabled"):	((pCoexSta->bC2hBtInquiryPage)?("inquiry/page scan"):((BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE == pCoexDm->btStatus)? "non-connected idle":
-		(  (BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)? "connected-idle":"busy")))),
-		pCoexSta->btRssi, pCoexSta->btRetryCnt);
-	CL_PRINTF(cliBuf);
-	
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
-		pBtLinkInfo->bScoExist, pBtLinkInfo->bHidExist, pBtLinkInfo->bPanExist, pBtLinkInfo->bA2dpExist);
-	CL_PRINTF(cliBuf);
-	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);	
-
-	btInfoExt = pCoexSta->btInfoExt;
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
-		(btInfoExt&BIT0)? "Basic rate":"EDR rate");
-	CL_PRINTF(cliBuf);	
-
-	for(i=0; i<BT_INFO_SRC_8192E_1ANT_MAX; i++)
-	{
-		if(pCoexSta->btInfoC2hCnt[i])
-		{				
-			CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8192e1Ant[i], \
-				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
-				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
-				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
-				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]);
-			CL_PRINTF(cliBuf);
-		}
-	}
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s, (0x%x/0x%x)", "PS state, IPS/LPS, (lps/rpwm)", \
-		((pCoexSta->bUnderIps? "IPS ON":"IPS OFF")),
-		((pCoexSta->bUnderLps? "LPS ON":"LPS OFF")), 
-		pBtCoexist->btInfo.lps1Ant, 
-		pBtCoexist->btInfo.rpwm_1ant);
-	CL_PRINTF(cliBuf);
-	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "SS Type", \
-		pCoexDm->curSsType);
-	CL_PRINTF(cliBuf);
-
-	if(!pBtCoexist->manual_control)
-	{
-		// Sw mechanism	
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
-		CL_PRINTF(cliBuf);
-	
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d/ %d ", "SM1[ShRf/ LpRA/ LimDig/ btLna]", \
-			pCoexDm->bCurRfRxLpfShrink, pCoexDm->bCurLowPenaltyRa, pCoexDm->limited_dig, pCoexDm->bCurBtLnaConstrain);
-		CL_PRINTF(cliBuf);
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ", "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
-			pCoexDm->bCurAgcTableEn, pCoexDm->bCurAdcBackOff, pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl);
-		CL_PRINTF(cliBuf);
-
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/ %s/ %d ", "DelBA/ BtCtrlAgg/ AggSize", \
-			(pBtCoexist->btInfo.reject_agg_pkt? "Yes":"No"), (pBtCoexist->btInfo.b_bt_ctrl_agg_buf_size? "Yes":"No"),
-				pBtCoexist->btInfo.aggBufSize);
-		CL_PRINTF(cliBuf);
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Rate Mask", \
-				pBtCoexist->btInfo.ra_mask);
-		CL_PRINTF(cliBuf);
-	
-		// Fw mechanism		
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
-		CL_PRINTF(cliBuf);	
-
-		psTdmaCase = pCoexDm->curPsTdma;
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)", "PS TDMA", \
-			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
-			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
-			pCoexDm->psTdmaPara[4], psTdmaCase, pCoexDm->bAutoTdmaAdjust);
-		CL_PRINTF(cliBuf);
-
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Latest error condition(should be 0)", \
-			pCoexDm->errorCondition);
-		CL_PRINTF(cliBuf);
-		
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "DecBtPwrLvl/ IgnWlanAct", \
-			pCoexDm->curBtDecPwrLvl, pCoexDm->bCurIgnoreWlanAct);
-		CL_PRINTF(cliBuf);
-	}
-
-	// Hw setting		
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
-	CL_PRINTF(cliBuf);	
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
-		pCoexDm->btRf0x1eBackup);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc04);
-	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xd04);
-	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x90c);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0xc04/ 0xd04/ 0x90c", \
-		u4Tmp[0], u4Tmp[1], u4Tmp[2]);
-	CL_PRINTF(cliBuf);
-
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x778);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778", \
-		u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-	
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x92c);
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x930);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x92c/ 0x930", \
-		(u1Tmp[0]), u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x40);
-	u1Tmp[1] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4f);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x40/ 0x4f", \
-		u1Tmp[0], u1Tmp[1]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x550);
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x522);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
-		u4Tmp[0], u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc50);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
-		u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c0);
-	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c4);
-	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c8);
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x6cc);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
-		u4Tmp[0], u4Tmp[1], u4Tmp[2], u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770(hp rx[31:16]/tx[15:0])", \
-		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx);
-	CL_PRINTF(cliBuf);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
-		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx);
-	CL_PRINTF(cliBuf);
-#if(BT_AUTO_REPORT_ONLY_8192E_1ANT == 1)
-	halbtc8192e1ant_MonitorBtCtr(pBtCoexist);
-#endif
-	
-	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-
-VOID
-EXhalbtc8192e1ant_IpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	u4Byte	u4Tmp=0;
-
-	if(pBtCoexist->manual_control ||	pBtCoexist->bStopCoexDm)
-		return;
-
-	if(BTC_IPS_ENTER == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
-		pCoexSta->bUnderIps = true;
-		halbtc8192e1ant_CoexAllOff(pBtCoexist);
-	}
-	else if(BTC_IPS_LEAVE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
-		pCoexSta->bUnderIps = FALSE;
-	}
-}
-
-VOID
-EXhalbtc8192e1ant_LpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	if(pBtCoexist->manual_control || pBtCoexist->bStopCoexDm)
-		return;
-
-	if(BTC_LPS_ENABLE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
-		pCoexSta->bUnderLps = true;
-	}
-	else if(BTC_LPS_DISABLE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
-		pCoexSta->bUnderLps = FALSE;
-	}
-}
-
-VOID
-EXhalbtc8192e1ant_ScanNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	BOOLEAN 		bWifiConnected=FALSE, bBtHsOn=FALSE;	
-
-	if(pBtCoexist->manual_control ||
-		pBtCoexist->bStopCoexDm ||
-		pBtCoexist->btInfo.bBtDisabled )
-		return;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	if(pCoexSta->bC2hBtInquiryPage)
-	{
-		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
-		return;
-	}
-	else if(bBtHsOn)
-	{
-		halbtc8192e1ant_ActionHs(pBtCoexist);
-		return;
-	}
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-	if(BTC_SCAN_START == type)
-	{	
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
-		if(!bWifiConnected)	// non-connected scan
-		{
-			halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(pBtCoexist);
-		}
-		else	// wifi is connected
-		{
-			halbtc8192e1ant_ActionWifiConnectedScan(pBtCoexist);
-		}
-	}
-	else if(BTC_SCAN_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
-		if(!bWifiConnected)	// non-connected scan
-		{
-			halbtc8192e1ant_ActionWifiNotConnected(pBtCoexist);
-		}
-		else
-		{
-			halbtc8192e1ant_ActionWifiConnected(pBtCoexist);
-		}
-	}
-}
-
-VOID
-EXhalbtc8192e1ant_ConnectNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	BOOLEAN	bWifiConnected=FALSE, bBtHsOn=FALSE;
-
-	if(pBtCoexist->manual_control ||
-		pBtCoexist->bStopCoexDm ||
-		pBtCoexist->btInfo.bBtDisabled )
-		return;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	if(pCoexSta->bC2hBtInquiryPage)
-	{
-		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
-		return;
-	}
-	else if(bBtHsOn)
-	{
-		halbtc8192e1ant_ActionHs(pBtCoexist);
-		return;
-	}
-
-	if(BTC_ASSOCIATE_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
-		halbtc8192e1ant_ActionWifiNotConnectedAssoAuthScan(pBtCoexist);
-	}
-	else if(BTC_ASSOCIATE_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
-		
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-		if(!bWifiConnected) // non-connected scan
-		{
-			halbtc8192e1ant_ActionWifiNotConnected(pBtCoexist);
-		}
-		else
-		{
-			halbtc8192e1ant_ActionWifiConnected(pBtCoexist);
-		}
-	}
-}
-
-VOID
-EXhalbtc8192e1ant_MediaStatusNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	u1Byte			H2C_Parameter[3] ={0};
-	u4Byte			wifiBw;
-	u1Byte			wifiCentralChnl;
-
-	if(pBtCoexist->manual_control ||
-		pBtCoexist->bStopCoexDm ||
-		pBtCoexist->btInfo.bBtDisabled )
-		return;
-
-	if(BTC_MEDIA_CONNECT == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
-	}
-
-	// only 2.4G we need to inform bt the chnl mask
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
-	if( (BTC_MEDIA_CONNECT == type) &&
-		(wifiCentralChnl <= 14) )
-	{
-		H2C_Parameter[0] = 0x1;
-		H2C_Parameter[1] = wifiCentralChnl;
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-		if(BTC_WIFI_BW_HT40 == wifiBw)
-			H2C_Parameter[2] = 0x30;
-		else
-			H2C_Parameter[2] = 0x20;
-	}
-		
-	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
-	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
-	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x66=0x%x\n", 
-		H2C_Parameter[0]<<16|H2C_Parameter[1]<<8|H2C_Parameter[2]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x66, 3, H2C_Parameter);
-}
-
-VOID
-EXhalbtc8192e1ant_SpecialPacketNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	BOOLEAN	bBtHsOn=FALSE;
-
-	if(pBtCoexist->manual_control ||
-		pBtCoexist->bStopCoexDm ||
-		pBtCoexist->btInfo.bBtDisabled )
-		return;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	if(pCoexSta->bC2hBtInquiryPage)
-	{
-		halbtc8192e1ant_ActionBtInquiry(pBtCoexist);
-		return;
-	}
-	else if(bBtHsOn)
-	{
-		halbtc8192e1ant_ActionHs(pBtCoexist);
-		return;
-	}
-
-	if( BTC_PACKET_DHCP == type ||
-		BTC_PACKET_EAPOL == type )
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], special Packet(%d) notify\n", type));
-		halbtc8192e1ant_ActionWifiConnectedSpecialPacket(pBtCoexist);
-	}
-}
-
-VOID
-EXhalbtc8192e1ant_BtInfoNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	pu1Byte			tmpBuf,
-	IN	u1Byte			length
-	)
-{
-	PBTC_BT_LINK_INFO	pBtLinkInfo=&pBtCoexist->bt_link_info;
-	u1Byte				btInfo=0;
-	u1Byte				i, rspSource=0;
-	static u4Byte		setBtPsdMode=0;
-	BOOLEAN				bBtBusy=FALSE, limited_dig=FALSE;
-	BOOLEAN				bWifiConnected=FALSE;
-	BOOLEAN				b_bt_ctrl_agg_buf_size=FALSE;
-
-	pCoexSta->bC2hBtInfoReqSent = FALSE;
-
-	rspSource = tmpBuf[0]&0xf;
-	if(rspSource >= BT_INFO_SRC_8192E_1ANT_MAX)
-		rspSource = BT_INFO_SRC_8192E_1ANT_WIFI_FW;
-	pCoexSta->btInfoC2hCnt[rspSource]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rspSource, length));
-	for(i=0; i<length; i++)
-	{
-		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
-		if(i == 1)
-			btInfo = tmpBuf[i];
-		if(i == length-1)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
-		}
-	}
-
-	if(pBtCoexist->btInfo.bBtDisabled)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for BT is disabled <===\n"));
-		return;
-	}	
-
-	if(pBtCoexist->manual_control)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Manual CTRL<===\n"));
-		return;
-	}
-	if(pBtCoexist->bStopCoexDm)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), return for Coex STOPPED!!<===\n"));
-		return;
-	}
-
-	if(BT_INFO_SRC_8192E_1ANT_WIFI_FW != rspSource)
-	{
-		pCoexSta->btRetryCnt =	// [3:0]
-			pCoexSta->btInfoC2h[rspSource][2]&0xf;
-
-		pCoexSta->btRssi =
-			pCoexSta->btInfoC2h[rspSource][3]*2+10;
-
-		pCoexSta->btInfoExt = 
-			pCoexSta->btInfoC2h[rspSource][4];
-
-		// Here we need to resend some wifi info to BT
-		// because bt is reset and loss of the info.
-		if( (pCoexSta->btInfoExt & BIT1) )
-		{			
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit1 check, send wifi BW&Chnl to BT!!\n"));
-			pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-			if(bWifiConnected)
-			{
-				EXhalbtc8192e1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_CONNECT);
-			}
-			else
-			{
-				EXhalbtc8192e1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
-			}
-
-			setBtPsdMode = 0;
-		}
-
-		// test-chip bt patch only rsp the status for BT_RSP, 
-		// so temporary we consider the following only under BT_RSP
-		if(BT_INFO_SRC_8192E_1ANT_BT_RSP == rspSource)
-		{
-			if( (pCoexSta->btInfoExt & BIT3) )
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit3 check, set BT NOT to ignore Wlan active!!\n"));
-				halbtc8192e1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
-			}
-			else
-			{
-				// BT already NOT ignore Wlan active, do nothing here.
-			}
-#if(BT_AUTO_REPORT_ONLY_8192E_1ANT == 0)
-			if( (pCoexSta->btInfoExt & BIT4) )
-			{
-				// BT auto report already enabled, do nothing
-			}
-			else
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BT ext info bit4 check, set BT to enable Auto Report!!\n"));
-				halbtc8192e1ant_BtAutoReport(pBtCoexist, FORCE_EXEC, true);
-			}
-#endif
-		}
-	}
-		
-	// check BIT2 first ==> check if bt is under inquiry or page scan
-	if(btInfo & BT_INFO_8192E_1ANT_B_INQ_PAGE)
-		pCoexSta->bC2hBtInquiryPage = true;
-	else
-		pCoexSta->bC2hBtInquiryPage = FALSE;
-
-	// set link exist status
-	if(!(btInfo&BT_INFO_8192E_1ANT_B_CONNECTION))
-	{
-		pCoexSta->bBtLinkExist = FALSE;
-		pCoexSta->bPanExist = FALSE;
-		pCoexSta->bA2dpExist = FALSE;
-		pCoexSta->bHidExist = FALSE;
-		pCoexSta->bScoExist = FALSE;
-	}
-	else // connection exists 
-	{
-		pCoexSta->bBtLinkExist = true;
-		if(btInfo & BT_INFO_8192E_1ANT_B_FTP)
-			pCoexSta->bPanExist = true;
-		else
-			pCoexSta->bPanExist = FALSE;
-		if(btInfo & BT_INFO_8192E_1ANT_B_A2DP)
-			pCoexSta->bA2dpExist = true;
-		else
-			pCoexSta->bA2dpExist = FALSE;
-		if(btInfo & BT_INFO_8192E_1ANT_B_HID)
-			pCoexSta->bHidExist = true;
-		else
-			pCoexSta->bHidExist = FALSE;
-		if(btInfo & BT_INFO_8192E_1ANT_B_SCO_ESCO)
-			pCoexSta->bScoExist = true;
-		else
-			pCoexSta->bScoExist = FALSE;
-	}
-
-	halbtc8192e1ant_UpdateBtLinkInfo(pBtCoexist);
-
-	if(!(btInfo&BT_INFO_8192E_1ANT_B_CONNECTION))
-	{
-		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-connected idle!!!\n"));
-	}
-	else if(btInfo == BT_INFO_8192E_1ANT_B_CONNECTION)	// connection exists but no busy
-	{
-		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt connected-idle!!!\n"));
-	}		
-	else if((btInfo&BT_INFO_8192E_1ANT_B_SCO_ESCO) ||
-		(btInfo&BT_INFO_8192E_1ANT_B_SCO_BUSY))
-	{
-		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_SCO_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt sco busy!!!\n"));
-	}
-	else if( (btInfo&BT_INFO_8192E_1ANT_B_ACL_BUSY) ||
-			(btInfo&BT_INFO_8192E_1ANT_B_A2DP) ||
-			(btInfo&BT_INFO_8192E_1ANT_B_FTP) )
-	{
-		if(BT_8192E_1ANT_BT_STATUS_ACL_BUSY != pCoexDm->btStatus)
-			pCoexDm->bAutoTdmaAdjust = FALSE;
-		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_ACL_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt acl busy!!!\n"));
-	}
-	else
-	{
-		pCoexDm->btStatus = BT_8192E_1ANT_BT_STATUS_MAX;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], BtInfoNotify(), bt non-defined state!!!\n"));
-	}
-
-	// ra mask check
-	if(pBtLinkInfo->bScoExist || pBtLinkInfo->bHidExist)
-	{
-		halbtc8192e1ant_Updatera_mask(pBtCoexist, NORMAL_EXEC, BTC_RATE_DISABLE, 0x00000003);	// disable tx cck 1M/2M
-	}
-	else
-	{
-		halbtc8192e1ant_Updatera_mask(pBtCoexist, NORMAL_EXEC, BTC_RATE_ENABLE, 0x00000003);	// enable tx cck 1M/2M
-	}
-	
-	if( (BT_8192E_1ANT_BT_STATUS_ACL_BUSY == pCoexDm->btStatus) ||
-		(BT_8192E_1ANT_BT_STATUS_SCO_BUSY == pCoexDm->btStatus) ||
-		(BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY == pCoexDm->btStatus) )
-	{
-		bBtBusy = true;
-		limited_dig = true;
-		if(pBtLinkInfo->bHidExist)
-			b_bt_ctrl_agg_buf_size = true;
-	}
-	else
-	{
-		bBtBusy = FALSE;
-		limited_dig = FALSE;
-	}
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
-
-	//============================================
-	//	Aggregation related setting
-	//============================================
-	// if sco, reject AddBA
-	//pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT, &bRejApAggPkt);
-
-	// decide BT control aggregation buf size or not
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE, &b_bt_ctrl_agg_buf_size);
-	// real update aggregation setting
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
-	//============================================
-
-	pCoexDm->limited_dig = limited_dig;
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
-
-	halbtc8192e1ant_RunCoexistMechanism(pBtCoexist);
-}
-
-VOID
-EXhalbtc8192e1ant_StackOperationNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
-	}
-	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
-	}
-}
-
-VOID
-EXhalbtc8192e1ant_HaltNotify(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
-
-	pBtCoexist->bStopCoexDm = true;
-	halbtc8192e1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
-
-	halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-	
-	halbtc8192e1ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4f, 0xf);
-
-	EXhalbtc8192e1ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
-}
-
-VOID
-EXhalbtc8192e1ant_PnpNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				pnpState
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Pnp notify\n"));
-
-	if(BTC_WIFI_PNP_SLEEP == pnpState)
-	{
-		pBtCoexist->bStopCoexDm = true;
-		halbtc8192e1ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
-		halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-		halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
-	}
-	else if(BTC_WIFI_PNP_WAKE_UP == pnpState)
-	{
-		
-	}
-}
-
-VOID
-EXhalbtc8192e1ant_Periodical(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	static u1Byte		disVerInfoCnt=0;
-	u4Byte				fwVer=0, btPatchVer=0;
-	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
-	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], ==========================Periodical===========================\n"));
-
-	if(disVerInfoCnt <= 5)
-	{
-		disVerInfoCnt += 1;
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n", \
-			pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num, pBoardInfo->btdm_ant_pos));
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], BT stack/ hci ext ver = %s / %d\n", \
-			((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion));
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_BT_PATCH_VER, &btPatchVer);
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_FW_VER, &fwVer);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n", \
-			GLCoexVerDate8192e1Ant, GLCoexVer8192e1Ant, fwVer, btPatchVer, btPatchVer));
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], ****************************************************************\n"));
-	}
-#if(BT_AUTO_REPORT_ONLY_8192E_1ANT == 0)
-	halbtc8192e1ant_QueryBtInfo(pBtCoexist);
-	halbtc8192e1ant_MonitorBtCtr(pBtCoexist);
-	halbtc8192e1ant_MonitorBtEnableDisable(pBtCoexist);
-#else
-	if( halbtc8192e1ant_IsWifiStatusChanged(pBtCoexist) ||
-		pCoexDm->bAutoTdmaAdjust)
-	{
-		halbtc8192e1ant_RunCoexistMechanism(pBtCoexist);
-	}
-#endif
-}
-
-VOID
-EXhalbtc8192e1ant_DbgControl(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				opCode,
-	IN	u1Byte				opLen,
-	IN	pu1Byte				pData
-	)
-{
-	switch(opCode)
-	{
-		case BTC_DBG_SET_COEX_NORMAL:
-			pBtCoexist->manual_control = FALSE;
-			halbtc8192e1ant_InitCoexDm(pBtCoexist);
-			break;
-		case BTC_DBG_SET_COEX_WIFI_ONLY:
-			pBtCoexist->manual_control = true;
-			halbtc8192e1ant_PowerSaveState(pBtCoexist, BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-			halbtc8192e1ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 9);	
-			break;
-		case BTC_DBG_SET_COEX_BT_ONLY:
-			// todo
-			break;
-		default:
-			break;
-	}
-}
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.h
deleted file mode 100644
index a759b758faef..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e1ant.h
+++ /dev/null
@@ -1,226 +0,0 @@
-//===========================================
-// The following is for 8192E_1ANT BT Co-exist definition
-//===========================================
-#define	BT_AUTO_REPORT_ONLY_8192E_1ANT				0
-
-#define	BT_INFO_8192E_1ANT_B_FTP						BIT7
-#define	BT_INFO_8192E_1ANT_B_A2DP					BIT6
-#define	BT_INFO_8192E_1ANT_B_HID						BIT5
-#define	BT_INFO_8192E_1ANT_B_SCO_BUSY				BIT4
-#define	BT_INFO_8192E_1ANT_B_ACL_BUSY				BIT3
-#define	BT_INFO_8192E_1ANT_B_INQ_PAGE				BIT2
-#define	BT_INFO_8192E_1ANT_B_SCO_ESCO				BIT1
-#define	BT_INFO_8192E_1ANT_B_CONNECTION				BIT0
-
-#define	BT_INFO_8192E_1ANT_A2DP_BASIC_RATE(_BT_INFO_EXT_)	\
-		(((_BT_INFO_EXT_&BIT0))? true:FALSE)
-
-#define	BTC_RSSI_COEX_THRESH_TOL_8192E_1ANT		2
-
-typedef enum _BT_INFO_SRC_8192E_1ANT{
-	BT_INFO_SRC_8192E_1ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8192E_1ANT_BT_RSP				= 0x1,
-	BT_INFO_SRC_8192E_1ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8192E_1ANT_MAX
-}BT_INFO_SRC_8192E_1ANT,*PBT_INFO_SRC_8192E_1ANT;
-
-typedef enum _BT_8192E_1ANT_BT_STATUS{
-	BT_8192E_1ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
-	BT_8192E_1ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
-	BT_8192E_1ANT_BT_STATUS_INQ_PAGE				= 0x2,
-	BT_8192E_1ANT_BT_STATUS_ACL_BUSY				= 0x3,
-	BT_8192E_1ANT_BT_STATUS_SCO_BUSY				= 0x4,
-	BT_8192E_1ANT_BT_STATUS_ACL_SCO_BUSY			= 0x5,
-	BT_8192E_1ANT_BT_STATUS_MAX
-}BT_8192E_1ANT_BT_STATUS,*PBT_8192E_1ANT_BT_STATUS;
-
-typedef enum _BT_8192E_1ANT_WIFI_STATUS{
-	BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_IDLE				= 0x0,
-	BT_8192E_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN		= 0x1,
-	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SCAN					= 0x2,
-	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT				= 0x3,
-	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_IDLE					= 0x4,
-	BT_8192E_1ANT_WIFI_STATUS_CONNECTED_BUSY					= 0x5,
-	BT_8192E_1ANT_WIFI_STATUS_MAX
-}BT_8192E_1ANT_WIFI_STATUS,*PBT_8192E_1ANT_WIFI_STATUS;
-
-typedef enum _BT_8192E_1ANT_COEX_ALGO{
-	BT_8192E_1ANT_COEX_ALGO_UNDEFINED			= 0x0,
-	BT_8192E_1ANT_COEX_ALGO_SCO				= 0x1,
-	BT_8192E_1ANT_COEX_ALGO_HID				= 0x2,
-	BT_8192E_1ANT_COEX_ALGO_A2DP				= 0x3,
-	BT_8192E_1ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
-	BT_8192E_1ANT_COEX_ALGO_PANEDR			= 0x5,
-	BT_8192E_1ANT_COEX_ALGO_PANHS			= 0x6,
-	BT_8192E_1ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
-	BT_8192E_1ANT_COEX_ALGO_PANEDR_HID		= 0x8,
-	BT_8192E_1ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x9,
-	BT_8192E_1ANT_COEX_ALGO_HID_A2DP			= 0xa,
-	BT_8192E_1ANT_COEX_ALGO_MAX				= 0xb,
-}BT_8192E_1ANT_COEX_ALGO,*PBT_8192E_1ANT_COEX_ALGO;
-
-typedef struct _COEX_DM_8192E_1ANT{
-	// fw mechanism
-	u1Byte		preBtDecPwrLvl;
-	u1Byte		curBtDecPwrLvl;
-	BOOLEAN		bPreBtLnaConstrain;
-	BOOLEAN		bCurBtLnaConstrain;
-	u1Byte		bPreBtPsdMode;
-	u1Byte		bCurBtPsdMode;
-	u1Byte		preFwDacSwingLvl;
-	u1Byte		curFwDacSwingLvl;
-	BOOLEAN		bCurIgnoreWlanAct;
-	BOOLEAN		bPreIgnoreWlanAct;
-	u1Byte		prePsTdma;
-	u1Byte		curPsTdma;
-	u1Byte		psTdmaPara[5];
-	u1Byte		psTdmaDuAdjType;
-	BOOLEAN		bAutoTdmaAdjust;
-	BOOLEAN		bPrePsTdmaOn;
-	BOOLEAN		bCurPsTdmaOn;
-	BOOLEAN		bPreBtAutoReport;
-	BOOLEAN		bCurBtAutoReport;
-	u1Byte		preLps;
-	u1Byte		curLps;
-	u1Byte		preRpwm;
-	u1Byte		curRpwm;
-
-	// sw mechanism
-	BOOLEAN		bPreRfRxLpfShrink;
-	BOOLEAN		bCurRfRxLpfShrink;
-	u4Byte		btRf0x1eBackup;
-	BOOLEAN 	bPreLowPenaltyRa;
-	BOOLEAN		bCurLowPenaltyRa;
-	BOOLEAN		bPreDacSwingOn;
-	u4Byte		preDacSwingLvl;
-	BOOLEAN		bCurDacSwingOn;
-	u4Byte		curDacSwingLvl;
-	BOOLEAN		bPreAdcBackOff;
-	BOOLEAN		bCurAdcBackOff;
-	BOOLEAN 	bPreAgcTableEn;
-	BOOLEAN		bCurAgcTableEn;
-	u4Byte		preVal0x6c0;
-	u4Byte		curVal0x6c0;
-	u4Byte		preVal0x6c4;
-	u4Byte		curVal0x6c4;
-	u4Byte		preVal0x6c8;
-	u4Byte		curVal0x6c8;
-	u1Byte		preVal0x6cc;
-	u1Byte		curVal0x6cc;
-	BOOLEAN		limited_dig;
-
-	// algorithm related
-	u1Byte		preAlgorithm;
-	u1Byte		curAlgorithm;
-	u1Byte		btStatus;
-	u1Byte		wifiChnlInfo[3];
-
-	u1Byte		preSsType;
-	u1Byte		curSsType;
-
-	u4Byte		prera_mask;
-	u4Byte		curra_mask;
-
-	u1Byte		errorCondition;
-} COEX_DM_8192E_1ANT, *PCOEX_DM_8192E_1ANT;
-
-typedef struct _COEX_STA_8192E_1ANT{
-	BOOLEAN					bBtLinkExist;
-	BOOLEAN					bScoExist;
-	BOOLEAN					bA2dpExist;
-	BOOLEAN					bHidExist;
-	BOOLEAN					bPanExist;
-
-	BOOLEAN					bUnderLps;
-	BOOLEAN					bUnderIps;
-	u4Byte					highPriorityTx;
-	u4Byte					highPriorityRx;
-	u4Byte					lowPriorityTx;
-	u4Byte					lowPriorityRx;
-	u1Byte					btRssi;
-	u1Byte					preBtRssiState;
-	u1Byte					preWifiRssiState[4];
-	BOOLEAN					bC2hBtInfoReqSent;
-	u1Byte					btInfoC2h[BT_INFO_SRC_8192E_1ANT_MAX][10];
-	u4Byte					btInfoC2hCnt[BT_INFO_SRC_8192E_1ANT_MAX];
-	BOOLEAN					bC2hBtInquiryPage;
-	u1Byte					btRetryCnt;
-	u1Byte					btInfoExt;
-}COEX_STA_8192E_1ANT, *PCOEX_STA_8192E_1ANT;
-
-//===========================================
-// The following is interface which will notify coex module.
-//===========================================
-VOID
-EXhalbtc8192e1ant_InitHwConfig(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-VOID
-EXhalbtc8192e1ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-VOID
-EXhalbtc8192e1ant_IpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8192e1ant_LpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8192e1ant_ScanNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8192e1ant_ConnectNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8192e1ant_MediaStatusNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8192e1ant_SpecialPacketNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8192e1ant_BtInfoNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	pu1Byte			tmpBuf,
-	IN	u1Byte			length
-	);
-VOID
-EXhalbtc8192e1ant_StackOperationNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8192e1ant_HaltNotify(
-	IN	PBTC_COEXIST			pBtCoexist
-	);
-VOID
-EXhalbtc8192e1ant_PnpNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				pnpState
-	);
-VOID
-EXhalbtc8192e1ant_Periodical(
-	IN	PBTC_COEXIST			pBtCoexist
-	);
-VOID
-EXhalbtc8192e1ant_DisplayCoexInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-VOID
-EXhalbtc8192e1ant_DbgControl(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				opCode,
-	IN	u1Byte				opLen,
-	IN	pu1Byte 			pData
-	);
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
deleted file mode 100644
index d2381794ecc9..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8192e2ant.c
+++ /dev/null
@@ -1,4118 +0,0 @@
-/**************************************************************
- * Description:
- *
- * This file is for RTL8192E Co-exist mechanism
- *
- * History
- * 2012/11/15 Cosa first check in.
- *
- **************************************************************/
-
-/**************************************************************
- *   include files
- **************************************************************/
-#include "halbt_precomp.h"
-
-/**************************************************************
- *   Global variables, these are static variables
- **************************************************************/
-static struct coex_dm_8192e_2ant glcoex_dm_8192e_2ant;
-static struct coex_dm_8192e_2ant *coex_dm = &glcoex_dm_8192e_2ant;
-static struct coex_sta_8192e_2ant glcoex_sta_8192e_2ant;
-static struct coex_sta_8192e_2ant *coex_sta = &glcoex_sta_8192e_2ant;
-
-static const char *const GLBtInfoSrc8192e2Ant[] = {
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-static u32 glcoex_ver_date_8192e_2ant = 20130902;
-static u32 glcoex_ver_8192e_2ant = 0x34;
-
-/**************************************************************
- *   local function proto type if needed
- **************************************************************/
-/**************************************************************
- *   local function start with halbtc8192e2ant_
- **************************************************************/
-static u8 halbtc8192e2ant_btrssi_state(u8 level_num, u8 rssi_thresh,
-				       u8 rssi_thresh1)
-{
-	int btrssi = coex_sta->bt_rssi;
-	u8 btrssi_state = coex_sta->pre_bt_rssi_state;
-
-	if (level_num == 2) {
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "BT Rssi pre state=LOW\n");
-			if (btrssi >= (rssi_thresh +
-				       BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
-				btrssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state switch to High\n");
-			} else {
-				btrssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state stay at Low\n");
-			}
-		} else {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "BT Rssi pre state=HIGH\n");
-			if (btrssi < rssi_thresh) {
-				btrssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state switch to Low\n");
-			} else {
-				btrssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "BT Rssi thresh error!!\n");
-			return coex_sta->pre_bt_rssi_state;
-		}
-
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "BT Rssi pre state=LOW\n");
-			if (btrssi >= (rssi_thresh +
-				      BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
-				btrssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state switch to Medium\n");
-			} else {
-				btrssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state stay at Low\n");
-			}
-		} else if ((coex_sta->pre_bt_rssi_state ==
-			    BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_bt_rssi_state ==
-			    BTC_RSSI_STATE_STAY_MEDIUM)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "[BTCoex], BT Rssi pre state=MEDIUM\n");
-			if (btrssi >= (rssi_thresh1 +
-				       BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
-				btrssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state switch to High\n");
-			} else if (btrssi < rssi_thresh) {
-				btrssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state switch to Low\n");
-			} else {
-				btrssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state stay at Medium\n");
-			}
-		} else {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "BT Rssi pre state=HIGH\n");
-			if (btrssi < rssi_thresh1) {
-				btrssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state switch to Medium\n");
-			} else {
-				btrssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "BT Rssi state stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_bt_rssi_state = btrssi_state;
-
-	return btrssi_state;
-}
-
-static u8 halbtc8192e2ant_wifirssi_state(struct btc_coexist *btcoexist,
-					 u8 index, u8 level_num,
-					 u8 rssi_thresh, u8 rssi_thresh1)
-{
-	int wifirssi = 0;
-	u8 wifirssi_state = coex_sta->pre_wifi_rssi_state[index];
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifirssi);
-
-	if (level_num == 2) {
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-		     BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-		     BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifirssi >= (rssi_thresh +
-					 BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
-				wifirssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state switch to High\n");
-			} else {
-				wifirssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state stay at Low\n");
-			}
-		} else {
-			if (wifirssi < rssi_thresh) {
-				wifirssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state switch to Low\n");
-			} else {
-				wifirssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
-				  "wifi RSSI thresh error!!\n");
-			return coex_sta->pre_wifi_rssi_state[index];
-		}
-
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-		     BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-		     BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifirssi >= (rssi_thresh +
-					 BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
-				wifirssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state switch to Medium\n");
-			} else {
-				wifirssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state stay at Low\n");
-			}
-		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
-			    BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_wifi_rssi_state[index] ==
-			    BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (wifirssi >= (rssi_thresh1 +
-					 BTC_RSSI_COEX_THRESH_TOL_8192E_2ANT)) {
-				wifirssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state switch to High\n");
-			} else if (wifirssi < rssi_thresh) {
-				wifirssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state switch to Low\n");
-			} else {
-				wifirssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state stay at Medium\n");
-			}
-		} else {
-			if (wifirssi < rssi_thresh1) {
-				wifirssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state switch to Medium\n");
-			} else {
-				wifirssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "wifi RSSI state stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_wifi_rssi_state[index] = wifirssi_state;
-
-	return wifirssi_state;
-}
-
-static void halbtc8192e2ant_monitor_bt_enable_disable(struct btc_coexist *btcoexist)
-{
-	static bool pre_bt_disabled;
-	static u32 bt_disable_cnt;
-	bool bt_active = true, bt_disabled = false;
-
-	/* This function check if bt is disabled */
-
-	if (coex_sta->high_priority_tx == 0 &&
-	    coex_sta->high_priority_rx == 0 &&
-	    coex_sta->low_priority_tx == 0 &&
-	    coex_sta->low_priority_rx == 0)
-		bt_active = false;
-
-	if (coex_sta->high_priority_tx == 0xffff &&
-	    coex_sta->high_priority_rx == 0xffff &&
-	    coex_sta->low_priority_tx == 0xffff &&
-	    coex_sta->low_priority_rx == 0xffff)
-		bt_active = false;
-
-	if (bt_active) {
-		bt_disable_cnt = 0;
-		bt_disabled = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
-				   &bt_disabled);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], BT is enabled !!\n");
-	} else {
-		bt_disable_cnt++;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], bt all counters=0, %d times!!\n",
-			  bt_disable_cnt);
-		if (bt_disable_cnt >= 2) {
-			bt_disabled = true;
-			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
-					   &bt_disabled);
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-				  "[BTCoex], BT is disabled !!\n");
-		}
-	}
-	if (pre_bt_disabled != bt_disabled) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], BT is from %s to %s!!\n",
-			  (pre_bt_disabled ? "disabled":"enabled"),
-			  (bt_disabled ? "disabled":"enabled"));
-		pre_bt_disabled = bt_disabled;
-	}
-}
-
-static u32 halbtc8192e2ant_decidera_mask(struct btc_coexist *btcoexist,
-				  u8 sstype, u32 ra_masktype)
-{
-	u32 disra_mask = 0x0;
-
-	switch (ra_masktype) {
-	case 0: /* normal mode */
-		if (sstype == 2)
-			disra_mask = 0x0;	/* enable 2ss */
-		else
-			disra_mask = 0xfff00000;/* disable 2ss */
-		break;
-	case 1: /* disable cck 1/2 */
-		if (sstype == 2)
-			disra_mask = 0x00000003;/* enable 2ss */
-		else
-			disra_mask = 0xfff00003;/* disable 2ss */
-		break;
-	case 2: /* disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4 */
-		if (sstype == 2)
-			disra_mask = 0x0001f1f7;/* enable 2ss */
-		else
-			disra_mask = 0xfff1f1f7;/* disable 2ss */
-		break;
-	default:
-		break;
-	}
-
-	return disra_mask;
-}
-
-static void halbtc8192e2ant_Updatera_mask(struct btc_coexist *btcoexist,
-					  bool force_exec, u32 dis_ratemask)
-{
-	coex_dm->curra_mask = dis_ratemask;
-
-	if (force_exec || (coex_dm->prera_mask != coex_dm->curra_mask))
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_UPDATE_ra_mask,
-				   &coex_dm->curra_mask);
-	coex_dm->prera_mask = coex_dm->curra_mask;
-}
-
-static void halbtc8192e2ant_autorate_fallback_retry(struct btc_coexist *btcoexist,
-						    bool force_exec, u8 type)
-{
-	bool wifi_under_bmode = false;
-
-	coex_dm->cur_arfrtype = type;
-
-	if (force_exec || (coex_dm->pre_arfrtype != coex_dm->cur_arfrtype)) {
-		switch (coex_dm->cur_arfrtype) {
-		case 0:	/* normal mode */
-			btcoexist->btc_write_4byte(btcoexist, 0x430,
-						   coex_dm->backup_arfr_cnt1);
-			btcoexist->btc_write_4byte(btcoexist, 0x434,
-						   coex_dm->backup_arfr_cnt2);
-			break;
-		case 1:
-			btcoexist->btc_get(btcoexist,
-					   BTC_GET_BL_WIFI_UNDER_B_MODE,
-					   &wifi_under_bmode);
-			if (wifi_under_bmode) {
-				btcoexist->btc_write_4byte(btcoexist, 0x430,
-							   0x0);
-				btcoexist->btc_write_4byte(btcoexist, 0x434,
-							   0x01010101);
-			} else {
-				btcoexist->btc_write_4byte(btcoexist, 0x430,
-							   0x0);
-				btcoexist->btc_write_4byte(btcoexist, 0x434,
-							   0x04030201);
-			}
-			break;
-		default:
-			break;
-		}
-	}
-
-	coex_dm->pre_arfrtype = coex_dm->cur_arfrtype;
-}
-
-static void halbtc8192e2ant_retrylimit(struct btc_coexist *btcoexist,
-				       bool force_exec, u8 type)
-{
-	coex_dm->cur_retrylimit_type = type;
-
-	if (force_exec || (coex_dm->pre_retrylimit_type !=
-			   coex_dm->cur_retrylimit_type)) {
-		switch (coex_dm->cur_retrylimit_type) {
-		case 0:	/* normal mode */
-			btcoexist->btc_write_2byte(btcoexist, 0x42a,
-					    coex_dm->backup_retrylimit);
-			break;
-		case 1:	/* retry limit=8 */
-			btcoexist->btc_write_2byte(btcoexist, 0x42a,
-						   0x0808);
-			break;
-		default:
-			break;
-		}
-	}
-
-	coex_dm->pre_retrylimit_type = coex_dm->cur_retrylimit_type;
-}
-
-static void halbtc8192e2ant_ampdu_maxtime(struct btc_coexist *btcoexist,
-					  bool force_exec, u8 type)
-{
-	coex_dm->cur_ampdutime_type = type;
-
-	if (force_exec || (coex_dm->pre_ampdutime_type !=
-			   coex_dm->cur_ampdutime_type)) {
-		switch (coex_dm->cur_ampdutime_type) {
-		case 0:	/* normal mode */
-			btcoexist->btc_write_1byte(btcoexist, 0x456,
-						coex_dm->backup_ampdu_maxtime);
-			break;
-		case 1:	/* AMPDU timw = 0x38 * 32us */
-			btcoexist->btc_write_1byte(btcoexist, 0x456, 0x38);
-			break;
-		default:
-			break;
-		}
-	}
-
-	coex_dm->pre_ampdutime_type = coex_dm->cur_ampdutime_type;
-}
-
-static void halbtc8192e2ant_limited_tx(struct btc_coexist *btcoexist,
-				       bool force_exec, u8 ra_masktype,
-				       u8 arfr_type, u8 retrylimit_type,
-				       u8 ampdutime_type)
-{
-	u32 disra_mask = 0x0;
-
-	coex_dm->curra_masktype = ra_masktype;
-	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist,
-						   coex_dm->cur_sstype,
-						   ra_masktype);
-	halbtc8192e2ant_Updatera_mask(btcoexist, force_exec, disra_mask);
-
-	halbtc8192e2ant_autorate_fallback_retry(btcoexist, force_exec,
-						arfr_type);
-	halbtc8192e2ant_retrylimit(btcoexist, force_exec, retrylimit_type);
-	halbtc8192e2ant_ampdu_maxtime(btcoexist, force_exec, ampdutime_type);
-}
-
-static void halbtc8192e2ant_limited_rx(struct btc_coexist *btcoexist,
-				       bool force_exec, bool rej_ap_agg_pkt,
-				       bool b_bt_ctrl_agg_buf_size,
-				       u8 agg_buf_size)
-{
-	bool reject_rx_agg = rej_ap_agg_pkt;
-	bool bt_ctrl_rx_agg_size = b_bt_ctrl_agg_buf_size;
-	u8 rx_agg_size = agg_buf_size;
-
-	/*********************************************
-	 *	Rx Aggregation related setting
-	 *********************************************/
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT,
-			   &reject_rx_agg);
-	/* decide BT control aggregation buf size or not */
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE,
-			   &bt_ctrl_rx_agg_size);
-	/* aggregation buf size, only work
-	 * when BT control Rx aggregation size. */
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rx_agg_size);
-	/* real update aggregation setting */
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
-
-
-}
-
-static void halbtc8192e2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
-{
-	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
-	u32 reg_hp_tx = 0, reg_hp_rx = 0, reg_lp_tx = 0, reg_lp_rx = 0;
-
-	reg_hp_txrx = 0x770;
-	reg_lp_txrx = 0x774;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
-	reg_hp_tx = u32tmp & MASKLWORD;
-	reg_hp_rx = (u32tmp & MASKHWORD)>>16;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
-	reg_lp_tx = u32tmp & MASKLWORD;
-	reg_lp_rx = (u32tmp & MASKHWORD)>>16;
-
-	coex_sta->high_priority_tx = reg_hp_tx;
-	coex_sta->high_priority_rx = reg_hp_rx;
-	coex_sta->low_priority_tx = reg_lp_tx;
-	coex_sta->low_priority_rx = reg_lp_rx;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex] High Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n",
-		  reg_hp_txrx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex] Low Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n",
-		  reg_lp_txrx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
-
-	/* reset counter */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-}
-
-static void halbtc8192e2ant_querybt_info(struct btc_coexist *btcoexist)
-{
-	u8 h2c_parameter[1] = {0};
-
-	coex_sta->c2h_bt_info_req_sent = true;
-
-	h2c_parameter[0] |= BIT0;	/* trigger */
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Query Bt Info, FW write 0x61=0x%x\n",
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
-}
-
-static void halbtc8192e2ant_update_btlink_info(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hson = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
-
-	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
-	bt_link_info->sco_exist = coex_sta->sco_exist;
-	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
-	bt_link_info->pan_exist = coex_sta->pan_exist;
-	bt_link_info->hid_exist = coex_sta->hid_exist;
-
-	/* work around for HS mode. */
-	if (bt_hson) {
-		bt_link_info->pan_exist = true;
-		bt_link_info->bt_link_exist = true;
-	}
-
-	/* check if Sco only */
-	if (bt_link_info->sco_exist &&
-	    !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist &&
-	    !bt_link_info->hid_exist)
-		bt_link_info->sco_only = true;
-	else
-		bt_link_info->sco_only = false;
-
-	/* check if A2dp only */
-	if (!bt_link_info->sco_exist &&
-	    bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist &&
-	    !bt_link_info->hid_exist)
-		bt_link_info->a2dp_only = true;
-	else
-		bt_link_info->a2dp_only = false;
-
-	/* check if Pan only */
-	if (!bt_link_info->sco_exist &&
-	    !bt_link_info->a2dp_exist &&
-	    bt_link_info->pan_exist &&
-	    !bt_link_info->hid_exist)
-		bt_link_info->pan_only = true;
-	else
-		bt_link_info->pan_only = false;
-
-	/* check if Hid only */
-	if (!bt_link_info->sco_exist &&
-	    !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist &&
-	    bt_link_info->hid_exist)
-		bt_link_info->hid_only = true;
-	else
-		bt_link_info->hid_only = false;
-}
-
-static u8 halbtc8192e2ant_action_algorithm(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	bool bt_hson = false;
-	u8 algorithm = BT_8192E_2ANT_COEX_ALGO_UNDEFINED;
-	u8 numOfDiffProfile = 0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
-
-	if (!bt_link_info->bt_link_exist) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "No BT link exists!!!\n");
-		return algorithm;
-	}
-
-	if (bt_link_info->sco_exist)
-		numOfDiffProfile++;
-	if (bt_link_info->hid_exist)
-		numOfDiffProfile++;
-	if (bt_link_info->pan_exist)
-		numOfDiffProfile++;
-	if (bt_link_info->a2dp_exist)
-		numOfDiffProfile++;
-
-	if (numOfDiffProfile == 1) {
-		if (bt_link_info->sco_exist) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "SCO only\n");
-			algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
-		} else {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "HID only\n");
-				algorithm = BT_8192E_2ANT_COEX_ALGO_HID;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "A2DP only\n");
-				algorithm = BT_8192E_2ANT_COEX_ALGO_A2DP;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "PAN(HS) only\n");
-					algorithm =
-						BT_8192E_2ANT_COEX_ALGO_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "PAN(EDR) only\n");
-					algorithm =
-						BT_8192E_2ANT_COEX_ALGO_PANEDR;
-				}
-			}
-		}
-	} else if (numOfDiffProfile == 2) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "SCO + HID\n");
-				algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "SCO + A2DP ==> SCO\n");
-				algorithm = BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "SCO + PAN(HS)\n");
-					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "SCO + PAN(EDR)\n");
-					algorithm =
-						BT_8192E_2ANT_COEX_ALGO_SCO_PAN;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (stack_info->num_of_hid >= 2) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "HID*2 + A2DP\n");
-					algorithm =
-					BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "HID + A2DP\n");
-					algorithm =
-					    BT_8192E_2ANT_COEX_ALGO_HID_A2DP;
-				}
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "HID + PAN(HS)\n");
-					algorithm = BT_8192E_2ANT_COEX_ALGO_HID;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "HID + PAN(EDR)\n");
-					algorithm =
-					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "A2DP + PAN(EDR)\n");
-					algorithm =
-					    BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP;
-				}
-			}
-		}
-	} else if (numOfDiffProfile == 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "SCO + HID + A2DP ==> HID\n");
-				algorithm = BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "SCO + HID + PAN(HS)\n");
-					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "SCO + HID + PAN(EDR)\n");
-					algorithm =
-						BT_8192E_2ANT_COEX_ALGO_SCO_PAN;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "SCO + A2DP + PAN(HS)\n");
-					algorithm = BT_8192E_2ANT_COEX_ALGO_SCO;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "SCO + A2DP + PAN(EDR)\n");
-					algorithm =
-					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "HID + A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8192E_2ANT_COEX_ALGO_HID_A2DP;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "HID + A2DP + PAN(EDR)\n");
-					algorithm =
-					BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				}
-			}
-		}
-	} else if (numOfDiffProfile >= 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hson) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "ErrorSCO+HID+A2DP+PAN(HS)\n");
-
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "SCO+HID+A2DP+PAN(EDR)\n");
-					algorithm =
-					    BT_8192E_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-	}
-
-	return algorithm;
-}
-
-static void halbtc8192e2ant_setfw_dac_swinglevel(struct btc_coexist *btcoexist,
-						 u8 dac_swinglvl)
-{
-	u8 h2c_parameter[1] = {0};
-
-	/* There are several type of dacswing
-	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
-	h2c_parameter[0] = dac_swinglvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Set Dac Swing Level=0x%x\n", dac_swinglvl);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x64=0x%x\n", h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
-}
-
-static void halbtc8192e2ant_set_fwdec_btpwr(struct btc_coexist *btcoexist,
-					    u8 dec_btpwr_lvl)
-{
-	u8 h2c_parameter[1] = {0};
-
-	h2c_parameter[0] = dec_btpwr_lvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex] decrease Bt Power level = %d, FW write 0x62=0x%x\n",
-		  dec_btpwr_lvl, h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
-}
-
-static void halbtc8192e2ant_dec_btpwr(struct btc_coexist *btcoexist,
-				      bool force_exec, u8 dec_btpwr_lvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s Dec BT power level = %d\n",
-		  (force_exec ? "force to":""), dec_btpwr_lvl);
-	coex_dm->cur_dec_bt_pwr = dec_btpwr_lvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], preBtDecPwrLvl=%d, curBtDecPwrLvl=%d\n",
-			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
-	}
-	halbtc8192e2ant_set_fwdec_btpwr(btcoexist, coex_dm->cur_dec_bt_pwr);
-
-	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
-}
-
-static void halbtc8192e2ant_set_bt_autoreport(struct btc_coexist *btcoexist,
-					      bool enable_autoreport)
-{
-	u8 h2c_parameter[1] = {0};
-
-	h2c_parameter[0] = 0;
-
-	if (enable_autoreport)
-		h2c_parameter[0] |= BIT0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n",
-		  (enable_autoreport ? "Enabled!!":"Disabled!!"),
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
-}
-
-static void halbtc8192e2ant_bt_autoreport(struct btc_coexist *btcoexist,
-					  bool force_exec,
-					  bool enable_autoreport)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s BT Auto report = %s\n",
-		  (force_exec ? "force to":""),
-		  ((enable_autoreport) ? "Enabled":"Disabled"));
-	coex_dm->cur_bt_auto_report = enable_autoreport;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex] bPreBtAutoReport=%d, bCurBtAutoReport=%d\n",
-			  coex_dm->pre_bt_auto_report,
-			  coex_dm->cur_bt_auto_report);
-
-		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
-			return;
-	}
-	halbtc8192e2ant_set_bt_autoreport(btcoexist,
-					  coex_dm->cur_bt_auto_report);
-
-	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
-}
-
-static void halbtc8192e2ant_fw_dac_swinglvl(struct btc_coexist *btcoexist,
-					    bool force_exec, u8 fw_dac_swinglvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s set FW Dac Swing level = %d\n",
-		  (force_exec ? "force to":""), fw_dac_swinglvl);
-	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swinglvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex] preFwDacSwingLvl=%d, curFwDacSwingLvl=%d\n",
-			  coex_dm->pre_fw_dac_swing_lvl,
-			  coex_dm->cur_fw_dac_swing_lvl);
-
-		if (coex_dm->pre_fw_dac_swing_lvl ==
-		    coex_dm->cur_fw_dac_swing_lvl)
-			return;
-	}
-
-	halbtc8192e2ant_setfw_dac_swinglevel(btcoexist,
-					     coex_dm->cur_fw_dac_swing_lvl);
-
-	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
-}
-
-static void halbtc8192e2ant_set_sw_rf_rx_lpf_corner(struct btc_coexist *btcoexist,
-						    bool rx_rf_shrink_on)
-{
-	if (rx_rf_shrink_on) {
-		/* Shrink RF Rx LPF corner */
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-					  0xfffff, 0xffffc);
-	} else {
-		/* Resume RF Rx LPF corner
-		 * After initialized, we can use coex_dm->btRf0x1eBackup */
-		if (btcoexist->initilized) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-				  "[BTCoex], Resume RF Rx LPF corner!!\n");
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-						  0xfffff,
-						  coex_dm->bt_rf0x1e_backup);
-		}
-	}
-}
-
-static void halbtc8192e2ant_rf_shrink(struct btc_coexist *btcoexist,
-				      bool force_exec, bool rx_rf_shrink_on)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
-		  (force_exec ? "force to":""), ((rx_rf_shrink_on) ? "ON":"OFF"));
-	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex]bPreRfRxLpfShrink=%d,bCurRfRxLpfShrink=%d\n",
-			  coex_dm->pre_rf_rx_lpf_shrink,
-			  coex_dm->cur_rf_rx_lpf_shrink);
-
-		if (coex_dm->pre_rf_rx_lpf_shrink ==
-		    coex_dm->cur_rf_rx_lpf_shrink)
-			return;
-	}
-	halbtc8192e2ant_set_sw_rf_rx_lpf_corner(btcoexist,
-						coex_dm->cur_rf_rx_lpf_shrink);
-
-	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
-}
-
-static void halbtc8192e2ant_set_dac_swingreg(struct btc_coexist *btcoexist,
-					     u32 level)
-{
-	u8 val = (u8)level;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], Write SwDacSwing = 0x%x\n", level);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
-}
-
-static void halbtc8192e2ant_setsw_fulltime_dacswing(struct btc_coexist *btcoexist,
-						    bool sw_dac_swingon,
-						    u32 sw_dac_swinglvl)
-{
-	if (sw_dac_swingon)
-		halbtc8192e2ant_set_dac_swingreg(btcoexist, sw_dac_swinglvl);
-	else
-		halbtc8192e2ant_set_dac_swingreg(btcoexist, 0x18);
-}
-
-static void halbtc8192e2ant_DacSwing(struct btc_coexist *btcoexist,
-				     bool force_exec, bool dac_swingon,
-				     u32 dac_swinglvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn DacSwing=%s, dac_swinglvl=0x%x\n",
-		  (force_exec ? "force to":""),
-		  ((dac_swingon) ? "ON":"OFF"), dac_swinglvl);
-	coex_dm->cur_dac_swing_on = dac_swingon;
-	coex_dm->cur_dac_swing_lvl = dac_swinglvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x, ",
-			  coex_dm->pre_dac_swing_on,
-			  coex_dm->pre_dac_swing_lvl);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n",
-			  coex_dm->cur_dac_swing_on,
-			  coex_dm->cur_dac_swing_lvl);
-
-		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
-		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
-			return;
-	}
-	mdelay(30);
-	halbtc8192e2ant_setsw_fulltime_dacswing(btcoexist, dac_swingon,
-						dac_swinglvl);
-
-	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
-	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
-}
-
-static void halbtc8192e2ant_set_agc_table(struct btc_coexist *btcoexist,
-					  bool agc_table_en)
-{
-
-	/* BB AGC Gain Table */
-	if (agc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB Agc Table On!\n");
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x0a1A0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x091B0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x081C0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x071D0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x061E0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x051F0001);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB Agc Table Off!\n");
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xaa1A0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa91B0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa81C0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa71D0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa61E0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa51F0001);
-	}
-}
-
-static void halbtc8192e2ant_AgcTable(struct btc_coexist *btcoexist,
-				     bool force_exec, bool agc_table_en)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s %s Agc Table\n",
-		  (force_exec ? "force to":""),
-		  ((agc_table_en) ? "Enable":"Disable"));
-	coex_dm->cur_agc_table_en = agc_table_en;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n",
-			  coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en);
-
-		if (coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en)
-			return;
-	}
-	halbtc8192e2ant_set_agc_table(btcoexist, agc_table_en);
-
-	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
-}
-
-static void halbtc8192e2ant_set_coex_table(struct btc_coexist *btcoexist,
-					   u32 val0x6c0, u32 val0x6c4,
-					   u32 val0x6c8, u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c4=0x%x\n", val0x6c4);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc);
-	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
-}
-
-static void halbtc8192e2ant_coex_table(struct btc_coexist *btcoexist,
-				       bool force_exec,
-				       u32 val0x6c0, u32 val0x6c4,
-				       u32 val0x6c8, u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s write Coex Table 0x6c0=0x%x, ",
-		  (force_exec ? "force to":""), val0x6c0);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n",
-		  val0x6c4, val0x6c8, val0x6cc);
-	coex_dm->cur_val0x6c0 = val0x6c0;
-	coex_dm->cur_val0x6c4 = val0x6c4;
-	coex_dm->cur_val0x6c8 = val0x6c8;
-	coex_dm->cur_val0x6cc = val0x6cc;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], preVal0x6c0=0x%x, preVal0x6c4=0x%x, ",
-			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n",
-			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], curVal0x6c0=0x%x, curVal0x6c4=0x%x,\n",
-			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n",
-			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
-
-		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
-		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
-		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
-		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
-			return;
-	}
-	halbtc8192e2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
-				       val0x6c8, val0x6cc);
-
-	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
-	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
-	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
-	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
-}
-
-static void halbtc8192e2ant_coex_table_with_type(struct btc_coexist *btcoexist,
-						 bool force_exec, u8 type)
-{
-	switch (type) {
-	case 0:
-		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x5a5a5a5a, 0xffffff, 0x3);
-		break;
-	case 1:
-		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
-					   0x5a5a5a5a, 0xffffff, 0x3);
-		break;
-	case 2:
-		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x5ffb5ffb, 0xffffff, 0x3);
-		break;
-	case 3:
-		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0xdfffdfff,
-					   0x5fdb5fdb, 0xffffff, 0x3);
-		break;
-	case 4:
-		halbtc8192e2ant_coex_table(btcoexist, force_exec, 0xdfffdfff,
-					   0x5ffb5ffb, 0xffffff, 0x3);
-		break;
-	default:
-		break;
-	}
-}
-
-static void halbtc8192e2ant_set_fw_ignore_wlanact(struct btc_coexist *btcoexist,
-						  bool enable)
-{
-	u8 h2c_parameter[1] = {0};
-
-	if (enable)
-		h2c_parameter[0] |= BIT0; /* function enable */
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex]set FW for BT Ignore Wlan_Act, FW write 0x63=0x%x\n",
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
-}
-
-static void halbtc8192e2ant_IgnoreWlanAct(struct btc_coexist *btcoexist,
-					  bool force_exec, bool enable)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn Ignore WlanAct %s\n",
-		  (force_exec ? "force to":""), (enable ? "ON":"OFF"));
-	coex_dm->cur_ignore_wlan_act = enable;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreIgnoreWlanAct = %d ",
-			  coex_dm->pre_ignore_wlan_act);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "bCurIgnoreWlanAct = %d!!\n",
-			  coex_dm->cur_ignore_wlan_act);
-
-		if (coex_dm->pre_ignore_wlan_act ==
-		    coex_dm->cur_ignore_wlan_act)
-			return;
-	}
-	halbtc8192e2ant_set_fw_ignore_wlanact(btcoexist, enable);
-
-	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
-}
-
-static void halbtc8192e2ant_SetFwPstdma(struct btc_coexist *btcoexist, u8 byte1,
-					u8 byte2, u8 byte3, u8 byte4, u8 byte5)
-{
-	u8 h2c_parameter[5] = {0};
-
-	h2c_parameter[0] = byte1;
-	h2c_parameter[1] = byte2;
-	h2c_parameter[2] = byte3;
-	h2c_parameter[3] = byte4;
-	h2c_parameter[4] = byte5;
-
-	coex_dm->ps_tdma_para[0] = byte1;
-	coex_dm->ps_tdma_para[1] = byte2;
-	coex_dm->ps_tdma_para[2] = byte3;
-	coex_dm->ps_tdma_para[3] = byte4;
-	coex_dm->ps_tdma_para[4] = byte5;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n",
-		  h2c_parameter[0],
-		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
-		  h2c_parameter[3] << 8 | h2c_parameter[4]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
-}
-
-static void halbtc8192e2ant_sw_mechanism1(struct btc_coexist *btcoexist,
-					  bool shrink_rx_lpf,
-					  bool low_penalty_ra,
-					  bool limited_dig,
-					  bool btlan_constrain)
-{
-	halbtc8192e2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
-}
-
-static void halbtc8192e2ant_sw_mechanism2(struct btc_coexist *btcoexist,
-					  bool agc_table_shift,
-					  bool adc_backoff,
-					  bool sw_dac_swing,
-					  u32 dac_swinglvl)
-{
-	halbtc8192e2ant_AgcTable(btcoexist, NORMAL_EXEC, agc_table_shift);
-	halbtc8192e2ant_DacSwing(btcoexist, NORMAL_EXEC, sw_dac_swing,
-				 dac_swinglvl);
-}
-
-static void halbtc8192e2ant_ps_tdma(struct btc_coexist *btcoexist,
-				    bool force_exec, bool turn_on, u8 type)
-{
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn %s PS TDMA, type=%d\n",
-		  (force_exec ? "force to":""), (turn_on ? "ON":"OFF"), type);
-	coex_dm->cur_ps_tdma_on = turn_on;
-	coex_dm->cur_ps_tdma = type;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
-			  coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
-			  coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma);
-
-		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
-		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
-			return;
-	}
-	if (turn_on) {
-		switch (type) {
-		case 1:
-		default:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
-						    0x1a, 0xe1, 0x90);
-			break;
-		case 2:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
-						    0x12, 0xe1, 0x90);
-			break;
-		case 3:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
-						    0x3, 0xf1, 0x90);
-			break;
-		case 4:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x10,
-						    0x3, 0xf1, 0x90);
-			break;
-		case 5:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
-						    0x1a, 0x60, 0x90);
-			break;
-		case 6:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
-						     0x12, 0x60, 0x90);
-			break;
-		case 7:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
-						    0x3, 0x70, 0x90);
-			break;
-		case 8:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xa3, 0x10,
-						    0x3, 0x70, 0x90);
-			break;
-		case 9:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
-						    0x1a, 0xe1, 0x10);
-			break;
-		case 10:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
-						    0x12, 0xe1, 0x10);
-			break;
-		case 11:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
-						    0x3, 0xf1, 0x10);
-			break;
-		case 12:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x10,
-						    0x3, 0xf1, 0x10);
-			break;
-		case 13:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
-						    0x1a, 0xe0, 0x10);
-			break;
-		case 14:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
-						    0x12, 0xe0, 0x10);
-			break;
-		case 15:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1c,
-						    0x3, 0xf0, 0x10);
-			break;
-		case 16:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x12,
-						    0x3, 0xf0, 0x10);
-			break;
-		case 17:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0x61, 0x20,
-						    0x03, 0x10, 0x10);
-			break;
-		case 18:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x5,
-						    0x5, 0xe1, 0x90);
-			break;
-		case 19:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x25,
-						    0x25, 0xe1, 0x90);
-			break;
-		case 20:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x25,
-						    0x25, 0x60, 0x90);
-			break;
-		case 21:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x15,
-						    0x03, 0x70, 0x90);
-			break;
-		case 71:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0xe3, 0x1a,
-						    0x1a, 0xe1, 0x90);
-			break;
-		}
-	} else {
-		/* disable PS tdma */
-		switch (type) {
-		default:
-		case 0:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0x8, 0x0, 0x0,
-						    0x0, 0x0);
-			btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x4);
-			break;
-		case 1:
-			halbtc8192e2ant_SetFwPstdma(btcoexist, 0x0, 0x0, 0x0,
-						    0x8, 0x0);
-			mdelay(5);
-			btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
-			break;
-		}
-	}
-
-	/* update pre state */
-	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
-	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
-}
-
-static void halbtc8192e2ant_set_switch_sstype(struct btc_coexist *btcoexist,
-					      u8 sstype)
-{
-	u8 mimops = BTC_MIMO_PS_DYNAMIC;
-	u32 disra_mask = 0x0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], REAL set SS Type = %d\n", sstype);
-
-	disra_mask = halbtc8192e2ant_decidera_mask(btcoexist, sstype,
-						   coex_dm->curra_masktype);
-	halbtc8192e2ant_Updatera_mask(btcoexist, FORCE_EXEC, disra_mask);
-
-	if (sstype == 1) {
-		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
-		/* switch ofdm path */
-		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x11);
-		btcoexist->btc_write_1byte(btcoexist, 0xd04, 0x1);
-		btcoexist->btc_write_4byte(btcoexist, 0x90c, 0x81111111);
-		/* switch cck patch */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x1);
-		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x81);
-		mimops = BTC_MIMO_PS_STATIC;
-	} else if (sstype == 2) {
-		halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
-		btcoexist->btc_write_1byte(btcoexist, 0xc04, 0x33);
-		btcoexist->btc_write_1byte(btcoexist, 0xd04, 0x3);
-		btcoexist->btc_write_4byte(btcoexist, 0x90c, 0x81121313);
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xe77, 0x4, 0x0);
-		btcoexist->btc_write_1byte(btcoexist, 0xa07, 0x41);
-		mimops = BTC_MIMO_PS_DYNAMIC;
-	}
-	/* set rx 1ss or 2ss */
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_SEND_MIMO_PS, &mimops);
-}
-
-static void halbtc8192e2ant_switch_sstype(struct btc_coexist *btcoexist,
-				   bool force_exec, u8 new_sstype)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], %s Switch SS Type = %d\n",
-		  (force_exec ? "force to":""), new_sstype);
-	coex_dm->cur_sstype = new_sstype;
-
-	if (!force_exec) {
-		if (coex_dm->pre_sstype == coex_dm->cur_sstype)
-			return;
-	}
-	halbtc8192e2ant_set_switch_sstype(btcoexist, coex_dm->cur_sstype);
-
-	coex_dm->pre_sstype = coex_dm->cur_sstype;
-}
-
-static void halbtc8192e2ant_coex_alloff(struct btc_coexist *btcoexist)
-{
-	/* fw all off */
-	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-
-	/* sw all off */
-	halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-
-	/* hw all off */
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-}
-
-static void halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	/* force to reset coex mechanism */
-
-	halbtc8192e2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, FORCE_EXEC, 6);
-	halbtc8192e2ant_dec_btpwr(btcoexist, FORCE_EXEC, 0);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
-	halbtc8192e2ant_switch_sstype(btcoexist, FORCE_EXEC, 2);
-
-	halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-}
-
-static void halbtc8192e2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
-{
-	bool low_pwr_disable = true;
-
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-			   &low_pwr_disable);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-
-	halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-}
-
-static bool halbtc8192e2ant_is_common_action(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool common = false, wifi_connected = false, wifi_busy = false;
-	bool bt_hson = false, low_pwr_disable = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-
-	if (bt_link_info->sco_exist || bt_link_info->hid_exist)
-		halbtc8192e2ant_limited_tx(btcoexist, NORMAL_EXEC, 1, 0, 0, 0);
-	else
-		halbtc8192e2ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
-
-	if (!wifi_connected) {
-		low_pwr_disable = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-				   &low_pwr_disable);
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi non-connected idle!!\n");
-
-		if ((BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
-		     coex_dm->bt_status) ||
-		    (BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
-		     coex_dm->bt_status)) {
-			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
-						      2);
-			halbtc8192e2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						0);
-		} else {
-			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
-						      1);
-			halbtc8192e2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 0);
-			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						1);
-		}
-
-		halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-
-		halbtc8192e2ant_sw_mechanism1(btcoexist, false, false, false,
-					      false);
-		halbtc8192e2ant_sw_mechanism2(btcoexist, false, false, false,
-					      0x18);
-
-		common = true;
-	} else {
-		if (BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
-		    coex_dm->bt_status) {
-			low_pwr_disable = false;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Wifi connected + BT non connected-idle!!\n");
-
-			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
-						      2);
-			halbtc8192e2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						0);
-			halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC,
-							6);
-			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-
-			common = true;
-		} else if (BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
-			   coex_dm->bt_status) {
-			low_pwr_disable = true;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			if (bt_hson)
-				return false;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Wifi connected + BT connected-idle!!\n");
-
-			halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC,
-						      2);
-			halbtc8192e2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						0);
-			halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC,
-							6);
-			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-
-			common = true;
-		} else {
-			low_pwr_disable = true;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			if (wifi_busy) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "Wifi Connected-Busy + BT Busy!!\n");
-				common = false;
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "Wifi Connected-Idle + BT Busy!!\n");
-
-				halbtc8192e2ant_switch_sstype(btcoexist,
-							      NORMAL_EXEC, 1);
-				halbtc8192e2ant_coex_table_with_type(btcoexist,
-								    NORMAL_EXEC,
-								    2);
-				halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 21);
-				halbtc8192e2ant_fw_dac_swinglvl(btcoexist,
-								NORMAL_EXEC, 6);
-				halbtc8192e2ant_dec_btpwr(btcoexist,
-							  NORMAL_EXEC, 0);
-				halbtc8192e2ant_sw_mechanism1(btcoexist, false,
-							      false, false,
-							      false);
-				halbtc8192e2ant_sw_mechanism2(btcoexist, false,
-							      false, false,
-							      0x18);
-				common = true;
-			}
-		}
-	}
-	return common;
-}
-
-static void halbtc8192e2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
-						 bool sco_hid, bool tx_pause,
-						 u8 max_interval)
-{
-	static int up, dn, m, n, wait_cnt;
-	/* 0: no change, +1: increase WiFi duration,
-	 * -1: decrease WiFi duration */
-	int result;
-	u8 retry_cnt = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], TdmaDurationAdjust()\n");
-
-	if (!coex_dm->auto_tdma_adjust) {
-		coex_dm->auto_tdma_adjust = true;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
-		if (sco_hid) {
-			if (tx_pause) {
-				if (max_interval == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				} else if (max_interval == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (max_interval == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				}
-			} else {
-				if (max_interval == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (max_interval == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (max_interval == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				}
-			}
-		} else {
-			if (tx_pause) {
-				if (max_interval == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (max_interval == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (max_interval == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				}
-			} else {
-				if (max_interval == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 1);
-					coex_dm->ps_tdma_du_adj_type = 1;
-				} else if (max_interval == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (max_interval == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				}
-			}
-		}
-
-		up = 0;
-		dn = 0;
-		m = 1;
-		n = 3;
-		result = 0;
-		wait_cnt = 0;
-	} else {
-		/* acquire the BT TRx retry count from BT_Info byte2 */
-		retry_cnt = coex_sta->bt_retry_cnt;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], retry_cnt = %d\n", retry_cnt);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], up=%d, dn=%d, m=%d, n=%d, wait_cnt=%d\n",
-			  up, dn, m, n, wait_cnt);
-		result = 0;
-		wait_cnt++;
-		/* no retry in the last 2-second duration */
-		if (retry_cnt == 0) {
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0;
-
-			if (up >= n) {
-				wait_cnt = 0;
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex]Increase wifi duration!!\n");
-			}
-		} else if (retry_cnt <= 3) {
-			up--;
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2) {
-				if (wait_cnt <= 2)
-					m++;
-				else
-					m = 1;
-
-				if (m >= 20)
-					m = 20;
-
-				n = 3 * m;
-				up = 0;
-				dn = 0;
-				wait_cnt = 0;
-				result = -1;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "Reduce wifi duration for retry<3\n");
-			}
-		} else {
-			if (wait_cnt == 1)
-				m++;
-			else
-				m = 1;
-
-			if (m >= 20)
-				m = 20;
-
-			n = 3*m;
-			up = 0;
-			dn = 0;
-			wait_cnt = 0;
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "Decrease wifi duration for retryCounter>3!!\n");
-		}
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], max Interval = %d\n", max_interval);
-		if (max_interval == 1) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-
-				if (coex_dm->cur_ps_tdma == 71) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									     8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 5);
-						coex_dm->ps_tdma_du_adj_type =
-									     5;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 13);
-						coex_dm->ps_tdma_du_adj_type =
-									     13;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 71);
-					coex_dm->ps_tdma_du_adj_type = 71;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 71) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									     1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									     4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									     1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 71);
-						coex_dm->ps_tdma_du_adj_type =
-									     71;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 9);
-						coex_dm->ps_tdma_du_adj_type =
-									     9;
-					}
-				}
-			}
-		} else if (max_interval == 2) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									     8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									     6;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									     4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									     2;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					}
-				}
-			}
-		} else if (max_interval == 3) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									     8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									     7;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8192e2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									     4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									     3;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8192e2ant_ps_tdma(
-								    btcoexist,
-								    NORMAL_EXEC,
-								    true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					}
-				}
-			}
-		}
-	}
-
-	/* if current PsTdma not match with
-	 * the recorded one (when scan, dhcp...),
-	 * then we have to adjust it back to the previous record one. */
-	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
-		bool scan = false, link = false, roam = false;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], PsTdma type dismatch!!!, ");
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "curPsTdma=%d, recordPsTdma=%d\n",
-			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
-
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-
-		if (!scan && !link && !roam)
-			halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true,
-						coex_dm->ps_tdma_du_adj_type);
-		else
-			BTC_PRINT(BTC_MSG_ALGORITHM,
-				  ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n");
-	}
-}
-
-/* SCO only or SCO+PAN(HS) */
-static void halbtc8192e2ant_action_sco(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
-
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-	}
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x6);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x6);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x6);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x6);
-		}
-	}
-}
-
-static void halbtc8192e2ant_action_sco_pan(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_STAY_LOW;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 4);
-
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
-	}
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x6);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x6);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x6);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x6);
-		}
-	}
-}
-
-static void halbtc8192e2ant_action_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/* A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
-static void halbtc8192e2ant_action_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-	bool long_dist = false;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW ||
-	     btrssi_state == BTC_RSSI_STATE_STAY_LOW) &&
-	    (wifirssi_state == BTC_RSSI_STATE_LOW ||
-	     wifirssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], A2dp, wifi/bt rssi both LOW!!\n");
-		long_dist = true;
-	}
-	if (long_dist) {
-		halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, true,
-					   0x4);
-	} else {
-		halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-		halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false,
-					   0x8);
-	}
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (long_dist)
-		halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-	else
-		halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-
-
-	if (long_dist) {
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 17);
-		coex_dm->auto_tdma_adjust = false;
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-	} else {
-		if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-		    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
-							     true, 1);
-			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-			   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
-							     false, 1);
-			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-			   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_tdma_duration_adjust(btcoexist, false,
-							     false, 1);
-			halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		}
-	}
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8192e2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, true, 2);
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     2);
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     2);
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-	}
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x6);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      true, 0x6);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x6);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      true, 0x6);
-		}
-	}
-}
-
-static void halbtc8192e2ant_action_pan_edr(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
-	}
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/* PAN(HS) only */
-static void halbtc8192e2ant_action_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-	}
-	halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/* PAN(EDR)+A2DP */
-static void halbtc8192e2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, true, 3);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     3);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, false, false,
-						     3);
-	}
-
-	/* sw mechanism	*/
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8192e2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 14);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 10);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/* HID+A2DP+PAN(EDR) */
-static void halbtc8192e2ant_action_hid_a2dp_pan_edr(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	halbtc8192e2ant_fw_dac_swinglvl(btcoexist, NORMAL_EXEC, 6);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	   (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, true, 3);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 3);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 3);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8192e2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifirssi_state, btrssi_state = BTC_RSSI_STATE_HIGH;
-	u32 wifi_bw;
-
-	wifirssi_state = halbtc8192e2ant_wifirssi_state(btcoexist, 0, 2, 15, 0);
-	btrssi_state = halbtc8192e2ant_btrssi_state(3, 34, 42);
-
-	halbtc8192e2ant_switch_sstype(btcoexist, NORMAL_EXEC, 1);
-	halbtc8192e2ant_limited_rx(btcoexist, NORMAL_EXEC, false, false, 0x8);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 3);
-
-	if ((btrssi_state == BTC_RSSI_STATE_LOW) ||
-	    (btrssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 0);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, true, 2);
-	} else if ((btrssi_state == BTC_RSSI_STATE_MEDIUM) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_MEDIUM)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 2);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 2);
-	} else if ((btrssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (btrssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8192e2ant_dec_btpwr(btcoexist, NORMAL_EXEC, 4);
-		halbtc8192e2ant_tdma_duration_adjust(btcoexist, true, false, 2);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifirssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifirssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8192e2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8192e2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8192e2ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
-{
-	u8 algorithm = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], RunCoexistMechanism()===>\n");
-
-	if (btcoexist->manual_control) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], return for Manual CTRL <===\n");
-		return;
-	}
-
-	if (coex_sta->under_ips) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], wifi is under IPS !!!\n");
-		return;
-	}
-
-	algorithm = halbtc8192e2ant_action_algorithm(btcoexist);
-	if (coex_sta->c2h_bt_inquiry_page &&
-	    (BT_8192E_2ANT_COEX_ALGO_PANHS != algorithm)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BT is under inquiry/page scan !!\n");
-		halbtc8192e2ant_action_bt_inquiry(btcoexist);
-		return;
-	}
-
-	coex_dm->cur_algorithm = algorithm;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], Algorithm = %d\n", coex_dm->cur_algorithm);
-
-	if (halbtc8192e2ant_is_common_action(btcoexist)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Action 2-Ant common.\n");
-		coex_dm->auto_tdma_adjust = false;
-	} else {
-		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex] preAlgorithm=%d, curAlgorithm=%d\n",
-				  coex_dm->pre_algorithm,
-				  coex_dm->cur_algorithm);
-			coex_dm->auto_tdma_adjust = false;
-		}
-		switch (coex_dm->cur_algorithm) {
-		case BT_8192E_2ANT_COEX_ALGO_SCO:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = SCO.\n");
-			halbtc8192e2ant_action_sco(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_SCO_PAN:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = SCO+PAN(EDR).\n");
-			halbtc8192e2ant_action_sco_pan(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = HID.\n");
-			halbtc8192e2ant_action_hid(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = A2DP.\n");
-			halbtc8192e2ant_action_a2dp(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_A2DP_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = A2DP+PAN(HS).\n");
-			halbtc8192e2ant_action_a2dp_pan_hs(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = PAN(EDR).\n");
-			halbtc8192e2ant_action_pan_edr(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = HS mode.\n");
-			halbtc8192e2ant_action_pan_hs(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_PANEDR_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = PAN+A2DP.\n");
-			halbtc8192e2ant_action_pan_edr_a2dp(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_PANEDR_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = PAN(EDR)+HID.\n");
-			halbtc8192e2ant_action_pan_edr_hid(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = HID+A2DP+PAN.\n");
-			halbtc8192e2ant_action_hid_a2dp_pan_edr(btcoexist);
-			break;
-		case BT_8192E_2ANT_COEX_ALGO_HID_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = HID+A2DP.\n");
-			halbtc8192e2ant_action_hid_a2dp(btcoexist);
-			break;
-		default:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "Action 2-Ant, algorithm = unknown!!\n");
-			/* halbtc8192e2ant_coex_alloff(btcoexist); */
-			break;
-		}
-		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
-	}
-}
-
-static void halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist,
-					  bool backup)
-{
-	u16 u16tmp = 0;
-	u8 u8tmp = 0;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], 2Ant Init HW Config!!\n");
-
-	if (backup) {
-		/* backup rf 0x1e value */
-		coex_dm->bt_rf0x1e_backup =
-			btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A,
-						  0x1e, 0xfffff);
-
-		coex_dm->backup_arfr_cnt1 = btcoexist->btc_read_4byte(btcoexist,
-								      0x430);
-		coex_dm->backup_arfr_cnt2 = btcoexist->btc_read_4byte(btcoexist,
-								     0x434);
-		coex_dm->backup_retrylimit = btcoexist->btc_read_2byte(
-								    btcoexist,
-								    0x42a);
-		coex_dm->backup_ampdu_maxtime = btcoexist->btc_read_1byte(
-								    btcoexist,
-								    0x456);
-	}
-
-	/* antenna sw ctrl to bt */
-	btcoexist->btc_write_1byte(btcoexist, 0x4f, 0x6);
-	btcoexist->btc_write_1byte(btcoexist, 0x944, 0x24);
-	btcoexist->btc_write_4byte(btcoexist, 0x930, 0x700700);
-	btcoexist->btc_write_1byte(btcoexist, 0x92c, 0x20);
-	if (btcoexist->chip_interface == BTC_INTF_USB)
-		btcoexist->btc_write_4byte(btcoexist, 0x64, 0x30430004);
-	else
-		btcoexist->btc_write_4byte(btcoexist, 0x64, 0x30030004);
-
-	halbtc8192e2ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
-
-	/* antenna switch control parameter */
-	btcoexist->btc_write_4byte(btcoexist, 0x858, 0x55555555);
-
-	/* coex parameters */
-	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
-	/* 0x790[5:0]=0x5 */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
-	u8tmp &= 0xc0;
-	u8tmp |= 0x5;
-	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
-
-	/* enable counter statistics */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
-
-	/* enable PTA */
-	btcoexist->btc_write_1byte(btcoexist, 0x40, 0x20);
-	/* enable mailbox interface */
-	u16tmp = btcoexist->btc_read_2byte(btcoexist, 0x40);
-	u16tmp |= BIT9;
-	btcoexist->btc_write_2byte(btcoexist, 0x40, u16tmp);
-
-	/* enable PTA I2C mailbox  */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x101);
-	u8tmp |= BIT4;
-	btcoexist->btc_write_1byte(btcoexist, 0x101, u8tmp);
-
-	/* enable bt clock when wifi is disabled. */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x93);
-	u8tmp |= BIT0;
-	btcoexist->btc_write_1byte(btcoexist, 0x93, u8tmp);
-	/* enable bt clock when suspend. */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x7);
-	u8tmp |= BIT0;
-	btcoexist->btc_write_1byte(btcoexist, 0x7, u8tmp);
-}
-
-/*************************************************************
- *   work around function start with wa_halbtc8192e2ant_
- *************************************************************/
-
-/************************************************************
- *   extern function start with EXhalbtc8192e2ant_
- ************************************************************/
-
-void ex_halbtc8192e2ant_init_hwconfig(struct btc_coexist *btcoexist)
-{
-	halbtc8192e2ant_init_hwconfig(btcoexist, true);
-}
-
-void ex_halbtc8192e2ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], Coex Mechanism Init!!\n");
-	halbtc8192e2ant_init_coex_dm(btcoexist);
-}
-
-void ex_halbtc8192e2ant_display_coex_info(struct btc_coexist *btcoexist)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	u8 *cli_buf = btcoexist->cli_buf;
-	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case = 0;
-	u16 u16tmp[4];
-	u32 u32tmp[4];
-	bool roam = false, scan = false, link = false, wifi_under_5g = false;
-	bool bt_hson = false, wifi_busy = false;
-	int wifirssi = 0, bt_hs_rssi = 0;
-	u32 wifi_bw, wifi_traffic_dir;
-	u8 wifi_dot11_chnl, wifi_hs_chnl;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cli_buf);
-
-	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ===========[Under Manual Control]===========");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
-	}
-
-	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
-		return;
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:",
-		   board_info->pg_ant_num, board_info->btdm_ant_num);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
-		   "BT stack/ hci ext ver",
-		   ((stack_info->profile_notified) ? "Yes" : "No"),
-		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %d_%d/ 0x%x/ 0x%x(%d)",
-		   "CoexVer/ FwVer/ PatchVer",
-		   glcoex_ver_date_8192e_2ant, glcoex_ver_8192e_2ant,
-		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hson);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
-			   &wifi_dot11_chnl);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
-		   "Dot11 channel / HsMode(HsChnl)",
-		   wifi_dot11_chnl, bt_hson, wifi_hs_chnl);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
-		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
-		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifirssi);
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "Wifi rssi/ HS rssi", wifirssi, bt_hs_rssi);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
-		   "Wifi link/ roam/ scan", link, roam, scan);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
-			   &wifi_traffic_dir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
-		   "Wifi status", (wifi_under_5g ? "5G" : "2.4G"),
-		   ((BTC_WIFI_BW_LEGACY == wifi_bw) ? "Legacy" :
-			(((BTC_WIFI_BW_HT40 == wifi_bw) ? "HT40" : "HT20"))),
-		   ((!wifi_busy) ? "idle" :
-			((BTC_WIFI_TRAFFIC_TX == wifi_traffic_dir) ?
-				"uplink" : "downlink")));
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
-		   "BT [status/ rssi/ retryCnt]",
-		   ((btcoexist->bt_info.bt_disabled) ? ("disabled") :
-		    ((coex_sta->c2h_bt_inquiry_page) ?
-		     ("inquiry/page scan") :
-		      ((BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
-			coex_dm->bt_status) ? "non-connected idle" :
-			 ((BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE ==
-			   coex_dm->bt_status) ? "connected-idle" : "busy")))),
-		   coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
-		   "SCO/HID/PAN/A2DP", stack_info->sco_exist,
-		   stack_info->hid_exist, stack_info->pan_exist,
-		   stack_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
-		   "BT Info A2DP rate",
-		   (bt_info_ext&BIT0) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
-
-	for (i = 0; i < BT_INFO_SRC_8192E_2ANT_MAX; i++) {
-		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-				   "\r\n %-35s = %02x %02x %02x %02x ",
-				   GLBtInfoSrc8192e2Ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3]);
-			CL_PRINTF(cli_buf);
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-				   "%02x %02x %02x(%d)",
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
-				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
-		}
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
-		   "PS state, IPS/LPS",
-		   ((coex_sta->under_ips ? "IPS ON" : "IPS OFF")),
-		   ((coex_sta->under_lps ? "LPS ON" : "LPS OFF")));
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "SS Type",
-		   coex_dm->cur_sstype);
-	CL_PRINTF(cli_buf);
-
-	/* Sw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Sw mechanism]============");
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
-		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
-		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
-		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
-		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
-		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ", "Rate Mask",
-		   btcoexist->bt_info.ra_mask);
-	CL_PRINTF(cli_buf);
-
-	/* Fw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Fw mechanism]============");
-	CL_PRINTF(cli_buf);
-
-	ps_tdma_case = coex_dm->cur_ps_tdma;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
-		   "PS TDMA", coex_dm->ps_tdma_para[0],
-		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
-		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
-		   ps_tdma_case, coex_dm->auto_tdma_adjust);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
-		   "DecBtPwr/ IgnWlanAct",
-		   coex_dm->cur_dec_bt_pwr, coex_dm->cur_ignore_wlan_act);
-	CL_PRINTF(cli_buf);
-
-	/* Hw setting */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Hw setting]============");
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
-		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
-		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
-		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retrylimit,
-		   coex_dm->backup_ampdu_maxtime);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
-	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
-		   "0x430/0x434/0x42a/0x456",
-		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc04);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xd04);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x90c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0xc04/ 0xd04/ 0x90c", u32tmp[0], u32tmp[1], u32tmp[2]);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x778",
-		   u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x92c);
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x930);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x92c/ 0x930", (u8tmp[0]), u32tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x40);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x4f);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x40/ 0x4f", u8tmp[0], u8tmp[1]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)",
-		   u32tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
-		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)",
-		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x770(hp rx[31:16]/tx[15:0])",
-		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x774(lp rx[31:16]/tx[15:0])",
-		   coex_sta->low_priority_rx, coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-
-void ex_halbtc8192e2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_IPS_ENTER == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS ENTER notify\n");
-		coex_sta->under_ips = true;
-		halbtc8192e2ant_coex_alloff(btcoexist);
-	} else if (BTC_IPS_LEAVE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS LEAVE notify\n");
-		coex_sta->under_ips = false;
-	}
-}
-
-void ex_halbtc8192e2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_LPS_ENABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS ENABLE notify\n");
-		coex_sta->under_lps = true;
-	} else if (BTC_LPS_DISABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS DISABLE notify\n");
-		coex_sta->under_lps = false;
-	}
-}
-
-void ex_halbtc8192e2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_SCAN_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN START notify\n");
-	else if (BTC_SCAN_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN FINISH notify\n");
-}
-
-void ex_halbtc8192e2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_ASSOCIATE_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT START notify\n");
-	else if (BTC_ASSOCIATE_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT FINISH notify\n");
-}
-
-void ex_halbtc8192e2ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type)
-{
-	u8 h2c_parameter[3] = {0};
-	u32 wifi_bw;
-	u8 wifi_center_chnl;
-
-	if (btcoexist->manual_control ||
-	    btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
-		return;
-
-	if (BTC_MEDIA_CONNECT == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA connect notify\n");
-	else
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA disconnect notify\n");
-
-	/* only 2.4G we need to inform bt the chnl mask */
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
-			   &wifi_center_chnl);
-	if ((BTC_MEDIA_CONNECT == type) &&
-	    (wifi_center_chnl <= 14)) {
-		h2c_parameter[0] = 0x1;
-		h2c_parameter[1] = wifi_center_chnl;
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			h2c_parameter[2] = 0x30;
-		else
-			h2c_parameter[2] = 0x20;
-	}
-
-	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
-	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
-	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x66=0x%x\n",
-		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
-		  h2c_parameter[2]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
-}
-
-void ex_halbtc8192e2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type)
-{
-	if (type == BTC_PACKET_DHCP)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], DHCP Packet notify\n");
-}
-
-void ex_halbtc8192e2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmp_buf, u8 length)
-{
-	u8 bt_info = 0;
-	u8 i, rspSource = 0;
-	bool bt_busy = false, limited_dig = false;
-	bool wifi_connected = false;
-
-	coex_sta->c2h_bt_info_req_sent = false;
-
-	rspSource = tmp_buf[0] & 0xf;
-	if (rspSource >= BT_INFO_SRC_8192E_2ANT_MAX)
-		rspSource = BT_INFO_SRC_8192E_2ANT_WIFI_FW;
-	coex_sta->bt_info_c2h_cnt[rspSource]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length=%d, hex data=[",
-		  rspSource, length);
-	for (i = 0; i < length; i++) {
-		coex_sta->bt_info_c2h[rspSource][i] = tmp_buf[i];
-		if (i == 1)
-			bt_info = tmp_buf[i];
-		if (i == length-1)
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x]\n", tmp_buf[i]);
-		else
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x, ", tmp_buf[i]);
-	}
-
-	if (BT_INFO_SRC_8192E_2ANT_WIFI_FW != rspSource) {
-		coex_sta->bt_retry_cnt =	/* [3:0] */
-			coex_sta->bt_info_c2h[rspSource][2] & 0xf;
-
-		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rspSource][3] * 2 + 10;
-
-		coex_sta->bt_info_ext =
-			coex_sta->bt_info_c2h[rspSource][4];
-
-		/* Here we need to resend some wifi info to BT
-		 * because bt is reset and loss of the info. */
-		if ((coex_sta->bt_info_ext & BIT1)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "bit1, send wifi BW&Chnl to BT!!\n");
-			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-					   &wifi_connected);
-			if (wifi_connected)
-				ex_halbtc8192e2ant_media_status_notify(
-							btcoexist,
-							BTC_MEDIA_CONNECT);
-			else
-				ex_halbtc8192e2ant_media_status_notify(
-							btcoexist,
-							BTC_MEDIA_DISCONNECT);
-		}
-
-		if ((coex_sta->bt_info_ext & BIT3)) {
-			if (!btcoexist->manual_control &&
-			    !btcoexist->stop_coex_dm) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "bit3, BT NOT ignore Wlan active!\n");
-				halbtc8192e2ant_IgnoreWlanAct(btcoexist,
-							      FORCE_EXEC,
-							      false);
-			}
-		} else {
-			/* BT already NOT ignore Wlan active,
-			 * do nothing here. */
-		}
-
-		if ((coex_sta->bt_info_ext & BIT4)) {
-			/* BT auto report already enabled, do nothing */
-		} else {
-			halbtc8192e2ant_bt_autoreport(btcoexist, FORCE_EXEC,
-						      true);
-		}
-	}
-
-	/* check BIT2 first ==> check if bt is under inquiry or page scan */
-	if (bt_info & BT_INFO_8192E_2ANT_B_INQ_PAGE)
-		coex_sta->c2h_bt_inquiry_page = true;
-	else
-		coex_sta->c2h_bt_inquiry_page = false;
-
-	/* set link exist status */
-	if (!(bt_info&BT_INFO_8192E_2ANT_B_CONNECTION)) {
-		coex_sta->bt_link_exist = false;
-		coex_sta->pan_exist = false;
-		coex_sta->a2dp_exist = false;
-		coex_sta->hid_exist = false;
-		coex_sta->sco_exist = false;
-	} else {/* connection exists */
-		coex_sta->bt_link_exist = true;
-		if (bt_info & BT_INFO_8192E_2ANT_B_FTP)
-			coex_sta->pan_exist = true;
-		else
-			coex_sta->pan_exist = false;
-		if (bt_info & BT_INFO_8192E_2ANT_B_A2DP)
-			coex_sta->a2dp_exist = true;
-		else
-			coex_sta->a2dp_exist = false;
-		if (bt_info & BT_INFO_8192E_2ANT_B_HID)
-			coex_sta->hid_exist = true;
-		else
-			coex_sta->hid_exist = false;
-		if (bt_info & BT_INFO_8192E_2ANT_B_SCO_ESCO)
-			coex_sta->sco_exist = true;
-		else
-			coex_sta->sco_exist = false;
-	}
-
-	halbtc8192e2ant_update_btlink_info(btcoexist);
-
-	if (!(bt_info&BT_INFO_8192E_2ANT_B_CONNECTION)) {
-		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BT Non-Connected idle!!!\n");
-	} else if (bt_info == BT_INFO_8192E_2ANT_B_CONNECTION) {
-		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], bt_infoNotify(), BT Connected-idle!!!\n");
-	} else if ((bt_info&BT_INFO_8192E_2ANT_B_SCO_ESCO) ||
-		   (bt_info&BT_INFO_8192E_2ANT_B_SCO_BUSY)) {
-		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_SCO_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], bt_infoNotify(), BT SCO busy!!!\n");
-	} else if (bt_info&BT_INFO_8192E_2ANT_B_ACL_BUSY) {
-		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_ACL_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], bt_infoNotify(), BT ACL busy!!!\n");
-	} else {
-		coex_dm->bt_status = BT_8192E_2ANT_BT_STATUS_MAX;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex]bt_infoNotify(), BT Non-Defined state!!!\n");
-	}
-
-	if ((BT_8192E_2ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
-	    (BT_8192E_2ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8192E_2ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
-		bt_busy = true;
-		limited_dig = true;
-	} else {
-		bt_busy = false;
-		limited_dig = false;
-	}
-
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
-
-	coex_dm->limited_dig = limited_dig;
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
-
-	halbtc8192e2ant_run_coexist_mechanism(btcoexist);
-}
-
-void ex_halbtc8192e2ant_stack_operation_notify(struct btc_coexist *btcoexist,
-					       u8 type)
-{
-	if (BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex] StackOP Inquiry/page/pair start notify\n");
-	else if (BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex] StackOP Inquiry/page/pair finish notify\n");
-}
-
-void ex_halbtc8192e2ant_halt_notify(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
-
-	halbtc8192e2ant_IgnoreWlanAct(btcoexist, FORCE_EXEC, true);
-	ex_halbtc8192e2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
-}
-
-void ex_halbtc8192e2ant_periodical(struct btc_coexist *btcoexist)
-{
-	static u8 dis_ver_info_cnt;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "=======================Periodical=======================\n");
-	if (dis_ver_info_cnt <= 5) {
-		dis_ver_info_cnt += 1;
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "************************************************\n");
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "Ant PG Num/ Ant Mech/ Ant Pos = %d/ %d/ %d\n",
-			  board_info->pg_ant_num, board_info->btdm_ant_num,
-			  board_info->btdm_ant_pos);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "BT stack/ hci ext ver = %s / %d\n",
-			  ((stack_info->profile_notified) ? "Yes" : "No"),
-			  stack_info->hci_version);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
-				   &bt_patch_ver);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "CoexVer/ FwVer/ PatchVer = %d_%x/ 0x%x/ 0x%x(%d)\n",
-			  glcoex_ver_date_8192e_2ant, glcoex_ver_8192e_2ant,
-			  fw_ver, bt_patch_ver, bt_patch_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "************************************************\n");
-	}
-
-	halbtc8192e2ant_querybt_info(btcoexist);
-	halbtc8192e2ant_monitor_bt_ctr(btcoexist);
-	halbtc8192e2ant_monitor_bt_enable_disable(btcoexist);
-}
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.c
deleted file mode 100644
index 3f5c4fd2e73f..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.c
+++ /dev/null
@@ -1,3780 +0,0 @@
-//============================================================
-// Description:
-//
-// This file is for RTL8723A Co-exist mechanism
-//
-// History
-// 2012/08/22 Cosa first check in.
-// 2012/11/14 Cosa Revise for 8723A 2Ant out sourcing.
-//
-//============================================================
-
-//============================================================
-// include files
-//============================================================
-#include "Mp_Precomp.h"
-#if(BT_30_SUPPORT == 1)
-//============================================================
-// Global variables, these are static variables
-//============================================================
-static COEX_DM_8723A_2ANT	GLCoexDm8723a2Ant;
-static PCOEX_DM_8723A_2ANT 	pCoexDm=&GLCoexDm8723a2Ant;
-static COEX_STA_8723A_2ANT	GLCoexSta8723a2Ant;
-static PCOEX_STA_8723A_2ANT	pCoexSta=&GLCoexSta8723a2Ant;
-
-const char *const GLBtInfoSrc8723a2Ant[]={
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-//============================================================
-// local function proto type if needed
-//============================================================
-//============================================================
-// local function start with halbtc8723a2ant_
-//============================================================
-BOOLEAN
-halbtc8723a2ant_IsWifiIdle(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN		bWifiConnected=FALSE, bScan=FALSE, bLink=FALSE, bRoam=FALSE;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-
-	if(bWifiConnected)
-		return FALSE;
-	if(bScan)
-		return FALSE;
-	if(bLink)
-		return FALSE;
-	if(bRoam)
-		return FALSE;
-
-	return true;
-}
-
-u1Byte
-halbtc8723a2ant_BtRssiState(
-	u1Byte			levelNum,
-	u1Byte			rssiThresh,
-	u1Byte			rssiThresh1
-	)
-{
-	s4Byte			btRssi=0;
-	u1Byte			btRssiState=pCoexSta->preBtRssiState;
-
-	btRssi = pCoexSta->btRssi;
-
-	if(levelNum == 2)
-	{			
-		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
-			{
-				btRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
-			}
-		}
-		else
-		{
-			if(btRssi < rssiThresh)
-			{
-				btRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
-			}
-		}
-	}
-	else if(levelNum == 3)
-	{
-		if(rssiThresh > rssiThresh1)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi thresh error!!\n"));
-			return pCoexSta->preBtRssiState;
-		}
-		
-		if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(btRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
-			{
-				btRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Low\n"));
-			}
-		}
-		else if( (pCoexSta->preBtRssiState == BTC_RSSI_STATE_MEDIUM) ||
-			(pCoexSta->preBtRssiState == BTC_RSSI_STATE_STAY_MEDIUM))
-		{
-			if(btRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
-			{
-				btRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to High\n"));
-			}
-			else if(btRssi < rssiThresh)
-			{
-				btRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Low\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at Medium\n"));
-			}
-		}
-		else
-		{
-			if(btRssi < rssiThresh1)
-			{
-				btRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state switch to Medium\n"));
-			}
-			else
-			{
-				btRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE, ("[BTCoex], BT Rssi state stay at High\n"));
-			}
-		}
-	}
-		
-	pCoexSta->preBtRssiState = btRssiState;
-
-	return btRssiState;
-}
-
-u1Byte
-halbtc8723a2ant_WifiRssiState(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			index,
-	IN	u1Byte			levelNum,
-	IN	u1Byte			rssiThresh,
-	IN	u1Byte			rssiThresh1
-	)
-{
-	s4Byte			wifiRssi=0;
-	u1Byte			wifiRssiState=pCoexSta->preWifiRssiState[index];
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
-	
-	if(levelNum == 2)
-	{
-		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
-			{
-				wifiRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
-			}
-		}
-		else
-		{
-			if(wifiRssi < rssiThresh)
-			{
-				wifiRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
-			}
-		}
-	}
-	else if(levelNum == 3)
-	{
-		if(rssiThresh > rssiThresh1)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI thresh error!!\n"));
-			return pCoexSta->preWifiRssiState[index];
-		}
-		
-		if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_LOW) ||
-			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_LOW))
-		{
-			if(wifiRssi >= (rssiThresh+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
-			{
-				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Low\n"));
-			}
-		}
-		else if( (pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_MEDIUM) ||
-			(pCoexSta->preWifiRssiState[index] == BTC_RSSI_STATE_STAY_MEDIUM))
-		{
-			if(wifiRssi >= (rssiThresh1+BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT))
-			{
-				wifiRssiState = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to High\n"));
-			}
-			else if(wifiRssi < rssiThresh)
-			{
-				wifiRssiState = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Low\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at Medium\n"));
-			}
-		}
-		else
-		{
-			if(wifiRssi < rssiThresh1)
-			{
-				wifiRssiState = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state switch to Medium\n"));
-			}
-			else
-			{
-				wifiRssiState = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE, ("[BTCoex], wifi RSSI state stay at High\n"));
-			}
-		}
-	}
-		
-	pCoexSta->preWifiRssiState[index] = wifiRssiState;
-
-	return wifiRssiState;
-}
-
-VOID
-halbtc8723a2ant_IndicateWifiChnlBwInfo(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	u1Byte			H2C_Parameter[3] ={0};
-	u4Byte			wifiBw;
-	u1Byte			wifiCentralChnl;
-	
-	// only 2.4G we need to inform bt the chnl mask
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifiCentralChnl);
-	if( (BTC_MEDIA_CONNECT == type) &&
-		(wifiCentralChnl <= 14) )
-	{
-		H2C_Parameter[0] = 0x1;
-		H2C_Parameter[1] = wifiCentralChnl;
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-		if(BTC_WIFI_BW_HT40 == wifiBw)
-			H2C_Parameter[2] = 0x30;
-		else
-			H2C_Parameter[2] = 0x20;
-	}
-		
-	pCoexDm->wifiChnlInfo[0] = H2C_Parameter[0];
-	pCoexDm->wifiChnlInfo[1] = H2C_Parameter[1];
-	pCoexDm->wifiChnlInfo[2] = H2C_Parameter[2];
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x19=0x%x\n", 
-		H2C_Parameter[0]<<16|H2C_Parameter[1]<<8|H2C_Parameter[2]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x19, 3, H2C_Parameter);
-}
-
-VOID
-halbtc8723a2ant_QueryBtInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-
-	pCoexSta->bC2hBtInfoReqSent = true;
-
-	H2C_Parameter[0] |= BIT0;	// trigger
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Query Bt Info, FW write 0x38=0x%x\n", 
-		H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x38, 1, H2C_Parameter);
-}
-u1Byte
-halbtc8723a2ant_ActionAlgorithm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
-	BOOLEAN				bBtHsOn=FALSE, bBtBusy=FALSE, limited_dig=FALSE;
-	u1Byte				algorithm=BT_8723A_2ANT_COEX_ALGO_UNDEFINED;
-	u1Byte				numOfDiffProfile=0;
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-
-	//======================
-	// here we get BT status first
-	//======================
-	pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_IDLE;
-	
-	if((pStackInfo->bScoExist) ||(bBtHsOn) ||(pStackInfo->bHidExist))
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO or HID or HS exists, set BT non-idle !!!\n"));
-		pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
-	}
-	else
-	{
-		// A2dp profile
-		if( (pBtCoexist->stack_info.numOfLink == 1) &&
-			(pStackInfo->bA2dpExist) )
-		{		
-			if( (pCoexSta->lowPriorityTx+	pCoexSta->lowPriorityRx) < 100)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP, low priority tx+rx < 100, set BT connected-idle!!!\n"));
-				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
-			}
-			else
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP, low priority tx+rx >= 100, set BT non-idle!!!\n"));
-				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
-			}
-		}
-		// Pan profile
-		if( (pBtCoexist->stack_info.numOfLink == 1) &&
-			(pStackInfo->bPanExist) )
-		{		
-			if((pCoexSta->lowPriorityTx+	pCoexSta->lowPriorityRx) < 600)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN, low priority tx+rx < 600, set BT connected-idle!!!\n"));
-				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
-			}
-			else
-			{
-				if(pCoexSta->lowPriorityTx)
-				{
-					if((pCoexSta->lowPriorityRx /pCoexSta->lowPriorityTx)>9 )
-					{
-						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN, low priority rx/tx > 9, set BT connected-idle!!!\n"));
-						pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
-					}
-				}
-			}
-			if(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN, set BT non-idle!!!\n"));
-				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
-			}
-		}
-		// Pan+A2dp profile
-		if( (pBtCoexist->stack_info.numOfLink == 2) &&
-			(pStackInfo->bA2dpExist) &&
-			(pStackInfo->bPanExist) )
-		{
-			if((pCoexSta->lowPriorityTx+	pCoexSta->lowPriorityRx) < 600)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN+A2DP, low priority tx+rx < 600, set BT connected-idle!!!\n"));
-				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
-			}
-			else
-			{
-				if(pCoexSta->lowPriorityTx)
-				{
-					if((pCoexSta->lowPriorityRx /pCoexSta->lowPriorityTx)>9 )
-					{
-						BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN+A2DP, low priority rx/tx > 9, set BT connected-idle!!!\n"));
-						pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE;
-					}
-				}
-			}
-			if(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE != pCoexDm->btStatus)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN+A2DP, set BT non-idle!!!\n"));
-				pCoexDm->btStatus = BT_8723A_2ANT_BT_STATUS_NON_IDLE;
-			}
-		}
-	}
-	if(BT_8723A_2ANT_BT_STATUS_IDLE != pCoexDm->btStatus)
-	{
-		bBtBusy = true;
-		limited_dig = true;
-	}
-	else
-	{
-		bBtBusy = FALSE;
-		limited_dig = FALSE;
-	}
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bBtBusy);
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
-	//======================
-
-	if(!pStackInfo->bBtLinkExist)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], No profile exists!!!\n"));
-		return algorithm;
-	}
-
-	if(pStackInfo->bScoExist)
-		numOfDiffProfile++;
-	if(pStackInfo->bHidExist)
-		numOfDiffProfile++;
-	if(pStackInfo->bPanExist)
-		numOfDiffProfile++;
-	if(pStackInfo->bA2dpExist)
-		numOfDiffProfile++;
-	
-	if(numOfDiffProfile == 1)
-	{
-		if(pStackInfo->bScoExist)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO only\n"));
-			algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
-		}
-		else
-		{
-			if(pStackInfo->bHidExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID only\n"));
-				algorithm = BT_8723A_2ANT_COEX_ALGO_HID;
-			}
-			else if(pStackInfo->bA2dpExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP only\n"));
-				algorithm = BT_8723A_2ANT_COEX_ALGO_A2DP;
-			}
-			else if(pStackInfo->bPanExist)
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(HS) only\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANHS;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], PAN(EDR) only\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR;
-				}
-			}
-		}
-	}
-	else if(numOfDiffProfile == 2)
-	{
-		if(pStackInfo->bScoExist)
-		{
-			if(pStackInfo->bHidExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID\n"));
-				algorithm = BT_8723A_2ANT_COEX_ALGO_HID;
-			}
-			else if(pStackInfo->bA2dpExist)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP ==> SCO\n"));
-				algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
-			}
-			else if(pStackInfo->bPanExist)
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(HS)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + PAN(EDR)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-		else
-		{
-			if( pStackInfo->bHidExist &&
-				pStackInfo->bA2dpExist )
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP\n"));
-				algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
-			}
-			else if( pStackInfo->bHidExist &&
-				pStackInfo->bPanExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(HS)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + PAN(EDR)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-			else if( pStackInfo->bPanExist &&
-				pStackInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(HS)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], A2DP + PAN(EDR)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_A2DP;
-				}
-			}
-		}
-	}
-	else if(numOfDiffProfile == 3)
-	{
-		if(pStackInfo->bScoExist)
-		{
-			if( pStackInfo->bHidExist &&
-				pStackInfo->bA2dpExist )
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP ==> HID\n"));
-				algorithm = BT_8723A_2ANT_COEX_ALGO_HID;
-			}
-			else if( pStackInfo->bHidExist &&
-				pStackInfo->bPanExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(HS)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + PAN(EDR)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-			else if( pStackInfo->bPanExist &&
-				pStackInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(HS)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_SCO;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + A2DP + PAN(EDR) ==> HID\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-		else
-		{
-			if( pStackInfo->bHidExist &&
-				pStackInfo->bPanExist &&
-				pStackInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(HS)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP;
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], HID + A2DP + PAN(EDR)\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				}
-			}
-		}
-	}
-	else if(numOfDiffProfile >= 3)
-	{
-		if(pStackInfo->bScoExist)
-		{
-			if( pStackInfo->bHidExist &&
-				pStackInfo->bPanExist &&
-				pStackInfo->bA2dpExist )
-			{
-				if(bBtHsOn)
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Error!!! SCO + HID + A2DP + PAN(HS)\n"));
-
-				}
-				else
-				{
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], SCO + HID + A2DP + PAN(EDR)==>PAN(EDR)+HID\n"));
-					algorithm = BT_8723A_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-	}
-
-	return algorithm;
-}
-
-BOOLEAN
-halbtc8723a2ant_NeedToDecBtPwr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN		bRet=FALSE;
-	BOOLEAN		bBtHsOn=FALSE, bWifiConnected=FALSE;
-	s4Byte		btHsRssi=0;
-
-	if(!pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn))
-		return FALSE;
-	if(!pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected))
-		return FALSE;
-	if(!pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi))
-		return FALSE;
-
-	if(bWifiConnected)
-	{
-		if(bBtHsOn)
-		{
-			if(btHsRssi > 37)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for HS mode!!\n"));
-				bRet = true;
-			}
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], Need to decrease bt power for Wifi is connected!!\n"));
-			bRet = true;
-		}
-	}
-	
-	return bRet;
-}
-
-VOID
-halbtc8723a2ant_SetFwDacSwingLevel(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			dacSwingLvl
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-
-	// There are several type of dacswing
-	// 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6
-	H2C_Parameter[0] = dacSwingLvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], Set Dac Swing Level=0x%x\n", dacSwingLvl));
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x29=0x%x\n", H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x29, 1, H2C_Parameter);
-}
-
-VOID
-halbtc8723a2ant_SetFwDecBtPwr(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bDecBtPwr
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-	
-	H2C_Parameter[0] = 0;
-
-	if(bDecBtPwr)
-	{
-		H2C_Parameter[0] |= BIT1;
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], decrease Bt Power : %s, FW write 0x21=0x%x\n", 
-		(bDecBtPwr? "Yes!!":"No!!"), H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x21, 1, H2C_Parameter);	
-}
-
-VOID
-halbtc8723a2ant_DecBtPwr(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bDecBtPwr
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s Dec BT power = %s\n",  
-		(bForceExec? "force to":""), ((bDecBtPwr)? "ON":"OFF")));
-	pCoexDm->bCurDecBtPwr = bDecBtPwr;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreDecBtPwr=%d, bCurDecBtPwr=%d\n", 
-			pCoexDm->bPreDecBtPwr, pCoexDm->bCurDecBtPwr));
-
-		if(pCoexDm->bPreDecBtPwr == pCoexDm->bCurDecBtPwr) 
-			return;
-	}
-	halbtc8723a2ant_SetFwDecBtPwr(pBtCoexist, pCoexDm->bCurDecBtPwr);
-
-	pCoexDm->bPreDecBtPwr = pCoexDm->bCurDecBtPwr;
-}
-
-VOID
-halbtc8723a2ant_FwDacSwingLvl(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	u1Byte			fwDacSwingLvl
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s set FW Dac Swing level = %d\n",  
-		(bForceExec? "force to":""), fwDacSwingLvl));
-	pCoexDm->curFwDacSwingLvl = fwDacSwingLvl;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], preFwDacSwingLvl=%d, curFwDacSwingLvl=%d\n", 
-			pCoexDm->preFwDacSwingLvl, pCoexDm->curFwDacSwingLvl));
-
-		if(pCoexDm->preFwDacSwingLvl == pCoexDm->curFwDacSwingLvl) 
-			return;
-	}
-
-	halbtc8723a2ant_SetFwDacSwingLevel(pBtCoexist, pCoexDm->curFwDacSwingLvl);
-
-	pCoexDm->preFwDacSwingLvl = pCoexDm->curFwDacSwingLvl;
-}
-
-VOID
-halbtc8723a2ant_SetSwRfRxLpfCorner(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bRxRfShrinkOn
-	)
-{
-	if(bRxRfShrinkOn)
-	{
-		//Shrink RF Rx LPF corner
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Shrink RF Rx LPF corner!!\n"));
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, 0xf0ff7);
-	}
-	else
-	{
-		//Resume RF Rx LPF corner
-		// After initialized, we can use pCoexDm->btRf0x1eBackup
-		if(pBtCoexist->initilized)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Resume RF Rx LPF corner!!\n"));
-			pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff, pCoexDm->btRf0x1eBackup);
-		}
-	}
-}
-
-VOID
-halbtc8723a2ant_RfShrink(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bRxRfShrinkOn
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn Rx RF Shrink = %s\n",  
-		(bForceExec? "force to":""), ((bRxRfShrinkOn)? "ON":"OFF")));
-	pCoexDm->bCurRfRxLpfShrink = bRxRfShrinkOn;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreRfRxLpfShrink=%d, bCurRfRxLpfShrink=%d\n", 
-			pCoexDm->bPreRfRxLpfShrink, pCoexDm->bCurRfRxLpfShrink));
-
-		if(pCoexDm->bPreRfRxLpfShrink == pCoexDm->bCurRfRxLpfShrink) 
-			return;
-	}
-	halbtc8723a2ant_SetSwRfRxLpfCorner(pBtCoexist, pCoexDm->bCurRfRxLpfShrink);
-
-	pCoexDm->bPreRfRxLpfShrink = pCoexDm->bCurRfRxLpfShrink;
-}
-
-VOID
-halbtc8723a2ant_SetSwPenaltyTxRateAdaptive(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bLowPenaltyRa
-	)
-{
-	u1Byte	tmpU1;
-
-	tmpU1 = pBtCoexist->btc_read_1byte(pBtCoexist, 0x4fd);
-	tmpU1 |= BIT0;
-	if(bLowPenaltyRa)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set low penalty!!\n"));
-		tmpU1 &= ~BIT2;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Tx rate adaptive, set normal!!\n"));
-		tmpU1 |= BIT2;
-	}
-
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x4fd, tmpU1);
-}
-
-VOID
-halbtc8723a2ant_LowPenaltyRa(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bLowPenaltyRa
-	)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn LowPenaltyRA = %s\n",  
-		(bForceExec? "force to":""), ((bLowPenaltyRa)? "ON":"OFF")));
-	pCoexDm->bCurLowPenaltyRa = bLowPenaltyRa;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreLowPenaltyRa=%d, bCurLowPenaltyRa=%d\n", 
-			pCoexDm->bPreLowPenaltyRa, pCoexDm->bCurLowPenaltyRa));
-
-		if(pCoexDm->bPreLowPenaltyRa == pCoexDm->bCurLowPenaltyRa) 
-			return;
-	}
-	halbtc8723a2ant_SetSwPenaltyTxRateAdaptive(pBtCoexist, pCoexDm->bCurLowPenaltyRa);
-
-	pCoexDm->bPreLowPenaltyRa = pCoexDm->bCurLowPenaltyRa;
-}
-
-VOID
-halbtc8723a2ant_SetSwFullTimeDacSwing(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bSwDacSwingOn,
-	IN	u4Byte			swDacSwingLvl
-	)
-{
-	if(bSwDacSwingOn)
-	{
-		pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, swDacSwingLvl);
-	}
-	else
-	{
-		pBtCoexist->btc_setBbReg(pBtCoexist, 0x880, 0xff000000, 0xc0);
-	}
-}
-
-
-VOID
-halbtc8723a2ant_DacSwing(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bDacSwingOn,
-	IN	u4Byte			dacSwingLvl
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn DacSwing=%s, dacSwingLvl=0x%x\n",  
-		(bForceExec? "force to":""), ((bDacSwingOn)? "ON":"OFF"), dacSwingLvl));
-	pCoexDm->bCurDacSwingOn = bDacSwingOn;
-	pCoexDm->curDacSwingLvl = dacSwingLvl;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x, bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n", 
-			pCoexDm->bPreDacSwingOn, pCoexDm->preDacSwingLvl,
-			pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl));
-
-		if( (pCoexDm->bPreDacSwingOn == pCoexDm->bCurDacSwingOn) &&
-			(pCoexDm->preDacSwingLvl == pCoexDm->curDacSwingLvl) )
-			return;
-	}
-	mdelay(30);
-	halbtc8723a2ant_SetSwFullTimeDacSwing(pBtCoexist, bDacSwingOn, dacSwingLvl);
-
-	pCoexDm->bPreDacSwingOn = pCoexDm->bCurDacSwingOn;
-	pCoexDm->preDacSwingLvl = pCoexDm->curDacSwingLvl;
-}
-
-VOID
-halbtc8723a2ant_SetAdcBackOff(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bAdcBackOff
-	)
-{
-	if(bAdcBackOff)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level On!\n"));
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc04,0x3a07611);
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], BB BackOff Level Off!\n"));		
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc04,0x3a05611);
-	}
-}
-
-VOID
-halbtc8723a2ant_AdcBackOff(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bAdcBackOff
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s turn AdcBackOff = %s\n",  
-		(bForceExec? "force to":""), ((bAdcBackOff)? "ON":"OFF")));
-	pCoexDm->bCurAdcBackOff = bAdcBackOff;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n", 
-			pCoexDm->bPreAdcBackOff, pCoexDm->bCurAdcBackOff));
-
-		if(pCoexDm->bPreAdcBackOff == pCoexDm->bCurAdcBackOff) 
-			return;
-	}
-	halbtc8723a2ant_SetAdcBackOff(pBtCoexist, pCoexDm->bCurAdcBackOff);
-
-	pCoexDm->bPreAdcBackOff = pCoexDm->bCurAdcBackOff;
-}
-
-VOID
-halbtc8723a2ant_SetAgcTable(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bAgcTableEn
-	)
-{
-	u1Byte		rssiAdjustVal=0;
-
-	if(bAgcTableEn)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table On!\n"));
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4e1c0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4d1d0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4c1e0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4b1f0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x4a200001);
-		
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0xdc000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x90000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x51000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x12000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1a, 0xfffff, 0x00355);
-		
-		rssiAdjustVal = 6;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], Agc Table Off!\n"));
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x641c0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x631d0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x621e0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x611f0001);
-		pBtCoexist->btc_write_4byte(pBtCoexist, 0xc78,0x60200001);
-
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x32000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0x71000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0xb0000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x12, 0xfffff, 0xfc000);
-		pBtCoexist->btc_set_rf_reg(pBtCoexist, BTC_RF_A, 0x1a, 0xfffff, 0x30355);
-	}
-
-	// set rssiAdjustVal for wifi module.
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON, &rssiAdjustVal);
-}
-
-
-VOID
-halbtc8723a2ant_AgcTable(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bAgcTableEn
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s %s Agc Table\n",  
-		(bForceExec? "force to":""), ((bAgcTableEn)? "Enable":"Disable")));
-	pCoexDm->bCurAgcTableEn = bAgcTableEn;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n", 
-			pCoexDm->bPreAgcTableEn, pCoexDm->bCurAgcTableEn));
-
-		if(pCoexDm->bPreAgcTableEn == pCoexDm->bCurAgcTableEn) 
-			return;
-	}
-	halbtc8723a2ant_SetAgcTable(pBtCoexist, bAgcTableEn);
-
-	pCoexDm->bPreAgcTableEn = pCoexDm->bCurAgcTableEn;
-}
-
-VOID
-halbtc8723a2ant_SetCoexTable(
-	IN	PBTC_COEXIST	pBtCoexist,
-	IN	u4Byte		val0x6c0,
-	IN	u4Byte		val0x6c8,
-	IN	u1Byte		val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0));
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8));
-	pBtCoexist->btc_write_4byte(pBtCoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC, ("[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc));
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x6cc, val0x6cc);
-}
-
-VOID
-halbtc8723a2ant_CoexTable(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	u4Byte			val0x6c0,
-	IN	u4Byte			val0x6c8,
-	IN	u1Byte			val0x6cc
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW, ("[BTCoex], %s write Coex Table 0x6c0=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n", 
-		(bForceExec? "force to":""), val0x6c0, val0x6c8, val0x6cc));
-	pCoexDm->curVal0x6c0 = val0x6c0;
-	pCoexDm->curVal0x6c8 = val0x6c8;
-	pCoexDm->curVal0x6cc = val0x6cc;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], preVal0x6c0=0x%x, preVal0x6c8=0x%x, preVal0x6cc=0x%x !!\n", 
-			pCoexDm->preVal0x6c0, pCoexDm->preVal0x6c8, pCoexDm->preVal0x6cc));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL, ("[BTCoex], curVal0x6c0=0x%x, curVal0x6c8=0x%x, curVal0x6cc=0x%x !!\n", 
-			pCoexDm->curVal0x6c0, pCoexDm->curVal0x6c8, pCoexDm->curVal0x6cc));
-	
-		if( (pCoexDm->preVal0x6c0 == pCoexDm->curVal0x6c0) &&
-			(pCoexDm->preVal0x6c8 == pCoexDm->curVal0x6c8) &&
-			(pCoexDm->preVal0x6cc == pCoexDm->curVal0x6cc) )
-			return;
-	}
-	halbtc8723a2ant_SetCoexTable(pBtCoexist, val0x6c0, val0x6c8, val0x6cc);
-
-	pCoexDm->preVal0x6c0 = pCoexDm->curVal0x6c0;
-	pCoexDm->preVal0x6c8 = pCoexDm->curVal0x6c8;
-	pCoexDm->preVal0x6cc = pCoexDm->curVal0x6cc;
-}
-
-VOID
-halbtc8723a2ant_SetFwIgnoreWlanAct(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bEnable
-	)
-{
-	u1Byte			H2C_Parameter[1] ={0};
-		
-	if(bEnable)
-	{
-		H2C_Parameter[0] |= BIT0;		// function enable
-	}
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], set FW for BT Ignore Wlan_Act, FW write 0x25=0x%x\n", 
-		H2C_Parameter[0]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x25, 1, H2C_Parameter);	
-}
-
-VOID
-halbtc8723a2ant_IgnoreWlanAct(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bEnable
-	)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn Ignore WlanAct %s\n", 
-		(bForceExec? "force to":""), (bEnable? "ON":"OFF")));
-	pCoexDm->bCurIgnoreWlanAct = bEnable;
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPreIgnoreWlanAct = %d, bCurIgnoreWlanAct = %d!!\n", 
-			pCoexDm->bPreIgnoreWlanAct, pCoexDm->bCurIgnoreWlanAct));
-
-		if(pCoexDm->bPreIgnoreWlanAct == pCoexDm->bCurIgnoreWlanAct)
-			return;
-	}
-	halbtc8723a2ant_SetFwIgnoreWlanAct(pBtCoexist, bEnable);
-
-	pCoexDm->bPreIgnoreWlanAct = pCoexDm->bCurIgnoreWlanAct;
-}
-
-VOID
-halbtc8723a2ant_SetFwPstdma(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			byte1,
-	IN	u1Byte			byte2,
-	IN	u1Byte			byte3,
-	IN	u1Byte			byte4,
-	IN	u1Byte			byte5
-	)
-{
-	u1Byte			H2C_Parameter[5] ={0};
-
-	H2C_Parameter[0] = byte1;	
-	H2C_Parameter[1] = byte2;	
-	H2C_Parameter[2] = byte3;
-	H2C_Parameter[3] = byte4;
-	H2C_Parameter[4] = byte5;
-
-	pCoexDm->psTdmaPara[0] = byte1;
-	pCoexDm->psTdmaPara[1] = byte2;
-	pCoexDm->psTdmaPara[2] = byte3;
-	pCoexDm->psTdmaPara[3] = byte4;
-	pCoexDm->psTdmaPara[4] = byte5;
-	
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC, ("[BTCoex], FW write 0x3a(5bytes)=0x%x%08x\n", 
-		H2C_Parameter[0], 
-		H2C_Parameter[1]<<24|H2C_Parameter[2]<<16|H2C_Parameter[3]<<8|H2C_Parameter[4]));
-
-	pBtCoexist->btc_fill_h2c(pBtCoexist, 0x3a, 5, H2C_Parameter);
-}
-
-VOID
-halbtc8723a2ant_PsTdma(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bForceExec,
-	IN	BOOLEAN			bTurnOn,
-	IN	u1Byte			type
-	)
-{
-	u4Byte	btTxRxCnt=0;
-
-	btTxRxCnt = pCoexSta->highPriorityTx+pCoexSta->highPriorityRx+
-				pCoexSta->lowPriorityTx+pCoexSta->lowPriorityRx;
-
-	if(btTxRxCnt > 3000)
-	{		
-		pCoexDm->bCurPsTdmaOn = true;
-		pCoexDm->curPsTdma = 8;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], turn ON PS TDMA, type=%d for BT tx/rx counters=%d(>3000)\n", 
-			pCoexDm->curPsTdma, btTxRxCnt));
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], %s turn %s PS TDMA, type=%d\n", 
-			(bForceExec? "force to":""), (bTurnOn? "ON":"OFF"), type));		
-		pCoexDm->bCurPsTdmaOn = bTurnOn;
-		pCoexDm->curPsTdma = type;
-	}
-
-	if(!bForceExec)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n", 
-			pCoexDm->bPrePsTdmaOn, pCoexDm->bCurPsTdmaOn));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n", 
-			pCoexDm->prePsTdma, pCoexDm->curPsTdma));
-
-		if( (pCoexDm->bPrePsTdmaOn == pCoexDm->bCurPsTdmaOn) &&
-			(pCoexDm->prePsTdma == pCoexDm->curPsTdma) )
-			return;
-	}	
-	if(pCoexDm->bCurPsTdmaOn)
-	{
-		switch(pCoexDm->curPsTdma)
-		{
-			case 1:
-			default:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x98);
-				break;
-			case 2:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x98);
-				break;
-			case 3:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0xe1, 0x98);
-				break;
-			case 4:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x5, 0x5, 0xe1, 0x80);
-				break;
-			case 5:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x98);
-				break;
-			case 6:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x98);
-				break;
-			case 7:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0x60, 0x98);
-				break;
-			case 8: 
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x5, 0x5, 0x60, 0x80);
-				break;
-			case 9: 
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0xe1, 0x98);
-				break;
-			case 10:	
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0xe1, 0x98);
-				break;
-			case 11:	
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0xe1, 0x98);
-				break;
-			case 12:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x98);
-				break;
-			case 13:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x1a, 0x1a, 0x60, 0x98);
-				break;
-			case 14:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x12, 0x12, 0x60, 0x98);
-				break;
-			case 15:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0xa, 0xa, 0x60, 0x98);
-				break;
-			case 16:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0x60, 0x98);
-				break;
-			case 17:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xa3, 0x2f, 0x2f, 0x60, 0x80);
-				break;
-			case 18:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x5, 0x5, 0xe1, 0x98);
-				break;			
-			case 19:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0xe1, 0x98);
-				break;
-			case 20:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0xe3, 0x25, 0x25, 0x60, 0x98);
-				break;
-		}
-	}
-	else
-	{
-		// disable PS tdma
-		switch(pCoexDm->curPsTdma)
-		{
-			case 0:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
-				break;
-			case 1:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x0, 0x0);
-				break;
-			default:
-				halbtc8723a2ant_SetFwPstdma(pBtCoexist, 0x0, 0x0, 0x0, 0x8, 0x0);
-				break;
-		}
-	}
-
-	// update pre state
-	pCoexDm->bPrePsTdmaOn = pCoexDm->bCurPsTdmaOn;
-	pCoexDm->prePsTdma = pCoexDm->curPsTdma;
-}
-
-
-VOID
-halbtc8723a2ant_CoexAllOff(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	// fw all off
-	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-	halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
-	halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-
-	// sw all off
-	halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-
-	// hw all off
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-}
-
-VOID
-halbtc8723a2ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	// force to reset coex mechanism
-	halbtc8723a2ant_CoexTable(pBtCoexist, FORCE_EXEC, 0x55555555, 0xffff, 0x3);
-	halbtc8723a2ant_PsTdma(pBtCoexist, FORCE_EXEC, FALSE, 0);
-	halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, FORCE_EXEC, 0x20);
-	halbtc8723a2ant_DecBtPwr(pBtCoexist, FORCE_EXEC, FALSE);
-	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
-
-	halbtc8723a2ant_AgcTable(pBtCoexist, FORCE_EXEC, FALSE);
-	halbtc8723a2ant_AdcBackOff(pBtCoexist, FORCE_EXEC, FALSE);
-	halbtc8723a2ant_LowPenaltyRa(pBtCoexist, FORCE_EXEC, FALSE);
-	halbtc8723a2ant_RfShrink(pBtCoexist, FORCE_EXEC, FALSE);
-	halbtc8723a2ant_DacSwing(pBtCoexist, FORCE_EXEC, FALSE, 0xc0);
-}
-
-VOID
-halbtc8723a2ant_BtInquiryPage(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN	bLowPwrDisable=true;
-	
-	pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
-
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-}
-
-VOID
-halbtc8723a2ant_BtEnableAction(
-	IN 	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BOOLEAN		bWifiConnected=FALSE;
-	
-	// Here we need to resend some wifi info to BT
-	// because bt is reset and loss of the info.						
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-	if(bWifiConnected)
-	{
-		halbtc8723a2ant_IndicateWifiChnlBwInfo(pBtCoexist, BTC_MEDIA_CONNECT);
-	}
-	else
-	{
-		halbtc8723a2ant_IndicateWifiChnlBwInfo(pBtCoexist, BTC_MEDIA_DISCONNECT);
-	}
-
-	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, FALSE);
-}
-
-VOID
-halbtc8723a2ant_MonitorBtCtr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u4Byte 			regHPTxRx, regLPTxRx, u4Tmp;
-	u4Byte			regHPTx=0, regHPRx=0, regLPTx=0, regLPRx=0;
-	u1Byte			u1Tmp;
-	
-	regHPTxRx = 0x770;
-	regLPTxRx = 0x774;
-
-	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regHPTxRx);
-	regHPTx = u4Tmp & MASKLWORD;
-	regHPRx = (u4Tmp & MASKHWORD)>>16;
-
-	u4Tmp = pBtCoexist->btc_read_4byte(pBtCoexist, regLPTxRx);
-	regLPTx = u4Tmp & MASKLWORD;
-	regLPRx = (u4Tmp & MASKHWORD)>>16;
-		
-	pCoexSta->highPriorityTx = regHPTx;
-	pCoexSta->highPriorityRx = regHPRx;
-	pCoexSta->lowPriorityTx = regLPTx;
-	pCoexSta->lowPriorityRx = regLPRx;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], High Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
-		regHPTxRx, regHPTx, regHPTx, regHPRx, regHPRx));
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], Low Priority Tx/Rx (reg 0x%x)=0x%x(%d)/0x%x(%d)\n", 
-		regLPTxRx, regLPTx, regLPTx, regLPRx, regLPRx));
-
-	// reset counter
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0xc);
-}
-
-VOID
-halbtc8723a2ant_MonitorBtEnableDisable(
-	IN 	PBTC_COEXIST		pBtCoexist
-	)
-{
-	static BOOLEAN	bPreBtDisabled=FALSE;
-	static u4Byte	btDisableCnt=0;
-	BOOLEAN			bBtActive=true, bBtDisabled=FALSE;
-
-	// This function check if bt is disabled
-
-	if(	pCoexSta->highPriorityTx == 0 &&
-		pCoexSta->highPriorityRx == 0 &&
-		pCoexSta->lowPriorityTx == 0 &&
-		pCoexSta->lowPriorityRx == 0)
-	{
-		bBtActive = FALSE;
-	}
-	if(	pCoexSta->highPriorityTx == 0xffff &&
-		pCoexSta->highPriorityRx == 0xffff &&
-		pCoexSta->lowPriorityTx == 0xffff &&
-		pCoexSta->lowPriorityRx == 0xffff)
-	{
-		bBtActive = FALSE;
-	}
-	if(bBtActive)
-	{
-		btDisableCnt = 0;
-		bBtDisabled = FALSE;
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is enabled !!\n"));
-	}
-	else
-	{
-		btDisableCnt++;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], bt all counters=0, %d times!!\n", 
-				btDisableCnt));
-		if(btDisableCnt >= 2)
-		{
-			bBtDisabled = true;
-			pBtCoexist->btc_set(pBtCoexist, BTC_SET_BL_BT_DISABLE, &bBtDisabled);
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is disabled !!\n"));
-		}
-	}
-	if(bPreBtDisabled != bBtDisabled)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR, ("[BTCoex], BT is from %s to %s!!\n", 
-			(bPreBtDisabled ? "disabled":"enabled"), 
-			(bBtDisabled ? "disabled":"enabled")));
-		bPreBtDisabled = bBtDisabled;
-		if(!bBtDisabled)
-		{
-			halbtc8723a2ant_BtEnableAction(pBtCoexist);
-		}
-	}
-}
-
-BOOLEAN
-halbtc8723a2ant_IsCommonAction(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
-	BOOLEAN			bCommon=FALSE, bWifiConnected=FALSE;
-	BOOLEAN			bLowPwrDisable=FALSE;
-
-	if(!pStackInfo->bBtLinkExist)
-	{
-		bLowPwrDisable = FALSE;
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
-	}
-	else
-	{
-		bLowPwrDisable = true;
-		pBtCoexist->btc_set(pBtCoexist, BTC_SET_ACT_DISABLE_LOW_POWER, &bLowPwrDisable);
-	}
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_CONNECTED, &bWifiConnected);
-
-	if(halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
-		BT_8723A_2ANT_BT_STATUS_IDLE == pCoexDm->btStatus)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi idle + Bt idle!!\n"));
-			
-		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-
-		bCommon = true;
-	}
-	else if(!halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
-			(BT_8723A_2ANT_BT_STATUS_IDLE == pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-idle + BT idle!!\n"));
-
-		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-
-		bCommon = true;
-	}
-	else if(halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
-		(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi idle + Bt connected idle!!\n"));
-		
-		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-
-		bCommon = true;
-	}
-	else if(!halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
-		(BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-idle + Bt connected idle!!\n"));
-
-		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-
-		bCommon = true;
-	}
-	else if(halbtc8723a2ant_IsWifiIdle(pBtCoexist) && 
-			(BT_8723A_2ANT_BT_STATUS_NON_IDLE == pCoexDm->btStatus) )
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi idle + BT non-idle!!\n"));
-		
-		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		
-		bCommon = true;
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Wifi non-idle + BT non-idle!!\n"));
-		halbtc8723a2ant_LowPenaltyRa(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_RfShrink(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_FwDacSwingLvl(pBtCoexist, NORMAL_EXEC, 0x20);
-		
-		bCommon = FALSE;
-	}
-	
-	return bCommon;
-}
-VOID
-halbtc8723a2ant_TdmaDurationAdjust(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	BOOLEAN			bScoHid,
-	IN	BOOLEAN			bTxPause,
-	IN	u1Byte			maxInterval
-	)
-{
-	static s4Byte		up,dn,m,n,WaitCount;
-	s4Byte			result;   //0: no change, +1: increase WiFi duration, -1: decrease WiFi duration
-	u1Byte			retryCount=0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW, ("[BTCoex], TdmaDurationAdjust()\n"));
-
-	if(pCoexDm->bResetTdmaAdjust)
-	{
-		pCoexDm->bResetTdmaAdjust = FALSE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], first run TdmaDurationAdjust()!!\n"));
-		{
-			if(bScoHid)
-			{
-				if(bTxPause)
-				{
-					if(maxInterval == 1)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
-						pCoexDm->psTdmaDuAdjType = 13;	
-					}
-					else if(maxInterval == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-						pCoexDm->psTdmaDuAdjType = 14;	
-					}
-					else if(maxInterval == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;	
-					}
-					else
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-				}
-				else
-				{
-					if(maxInterval == 1)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-						pCoexDm->psTdmaDuAdjType = 9;	
-					}
-					else if(maxInterval == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-						pCoexDm->psTdmaDuAdjType = 10;	
-					}
-					else if(maxInterval == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-				}
-			}
-			else
-			{
-				if(bTxPause)
-				{
-					if(maxInterval == 1)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
-						pCoexDm->psTdmaDuAdjType = 5;	
-					}
-					else if(maxInterval == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-						pCoexDm->psTdmaDuAdjType = 6;	
-					}
-					else if(maxInterval == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-				}
-				else
-				{
-					if(maxInterval == 1)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-						pCoexDm->psTdmaDuAdjType = 1;	
-					}
-					else if(maxInterval == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;	
-					}
-					else if(maxInterval == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-				}
-			}
-		}
-		//============
-		up = 0;
-		dn = 0;
-		m = 1;
-		n= 3;
-		result = 0;
-		WaitCount = 0;
-	}
-	else
-	{
-		//acquire the BT TRx retry count from BT_Info byte2
-		retryCount = pCoexSta->btRetryCnt;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], retryCount = %d\n", retryCount));
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], up=%d, dn=%d, m=%d, n=%d, WaitCount=%d\n", 
-			up, dn, m, n, WaitCount));
-		result = 0;
-		WaitCount++; 
-		  
-		if(retryCount == 0)  // no retry in the last 2-second duration
-		{
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0;				 
-
-			if(up >= n)	// if ³sÄò n ­Ó2¬í retry count¬°0, «h½Õ¼eWiFi duration
-			{
-				WaitCount = 0; 
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Increase wifi duration!!\n"));
-			}
-		}
-		else if (retryCount <= 3)	// <=3 retry in the last 2-second duration
-		{
-			up--; 
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2)	// if ³sÄò 2 ­Ó2¬í retry count< 3, «h½Õ¯¶WiFi duration
-			{
-				if (WaitCount <= 2)
-					m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
-				else
-					m = 1;
-
-				if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
-					m = 20;
-
-				n = 3*m;
-				up = 0;
-				dn = 0;
-				WaitCount = 0;
-				result = -1; 
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter<3!!\n"));
-			}
-		}
-		else  //retry count > 3, ¥u­n1¦¸ retry count > 3, «h½Õ¯¶WiFi duration
-		{
-			if (WaitCount == 1)
-				m++; // ÁקK¤@ª½¦b¨â­Ólevel¤¤¨Ó¦^
-			else
-				m = 1;
-
-			if ( m >= 20) //m ³Ì¤j­È = 20 ' ³Ì¤j120¬í recheck¬O§_½Õ¾ã WiFi duration.
-				m = 20;
-
-			n = 3*m;
-			up = 0;
-			dn = 0;
-			WaitCount = 0; 
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], Decrease wifi duration for retryCounter>3!!\n"));
-		}
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], max Interval = %d\n", maxInterval));
-		if(maxInterval == 1)
-		{
-			if(bTxPause)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
-
-				if(pCoexDm->curPsTdma == 1)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
-					pCoexDm->psTdmaDuAdjType = 5;
-				}
-				else if(pCoexDm->curPsTdma == 2)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-					pCoexDm->psTdmaDuAdjType = 6;
-				}
-				else if(pCoexDm->curPsTdma == 3)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-					pCoexDm->psTdmaDuAdjType = 7;
-				}
-				else if(pCoexDm->curPsTdma == 4)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-					pCoexDm->psTdmaDuAdjType = 8;
-				}
-				if(pCoexDm->curPsTdma == 9)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
-					pCoexDm->psTdmaDuAdjType = 13;
-				}
-				else if(pCoexDm->curPsTdma == 10)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-					pCoexDm->psTdmaDuAdjType = 14;
-				}
-				else if(pCoexDm->curPsTdma == 11)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-					pCoexDm->psTdmaDuAdjType = 15;
-				}
-				else if(pCoexDm->curPsTdma == 12)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-					pCoexDm->psTdmaDuAdjType = 16;
-				}
-				
-				if(result == -1)
-				{					
-					if(pCoexDm->curPsTdma == 5)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-						pCoexDm->psTdmaDuAdjType = 6;
-					}
-					else if(pCoexDm->curPsTdma == 6)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 7)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-						pCoexDm->psTdmaDuAdjType = 8;
-					}
-					else if(pCoexDm->curPsTdma == 13)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-						pCoexDm->psTdmaDuAdjType = 14;
-					}
-					else if(pCoexDm->curPsTdma == 14)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 15)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-						pCoexDm->psTdmaDuAdjType = 16;
-					}
-				} 
-				else if (result == 1)
-				{
-					if(pCoexDm->curPsTdma == 8)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 7)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-						pCoexDm->psTdmaDuAdjType = 6;
-					}
-					else if(pCoexDm->curPsTdma == 6)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 5);
-						pCoexDm->psTdmaDuAdjType = 5;
-					}
-					else if(pCoexDm->curPsTdma == 16)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 15)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-						pCoexDm->psTdmaDuAdjType = 14;
-					}
-					else if(pCoexDm->curPsTdma == 14)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
-						pCoexDm->psTdmaDuAdjType = 13;
-					}
-				}
-			}
-			else
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
-				if(pCoexDm->curPsTdma == 5)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-					pCoexDm->psTdmaDuAdjType = 1;
-				}
-				else if(pCoexDm->curPsTdma == 6)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-					pCoexDm->psTdmaDuAdjType = 2;
-				}
-				else if(pCoexDm->curPsTdma == 7)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-					pCoexDm->psTdmaDuAdjType = 3;
-				}
-				else if(pCoexDm->curPsTdma == 8)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-					pCoexDm->psTdmaDuAdjType = 4;
-				}
-				if(pCoexDm->curPsTdma == 13)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-					pCoexDm->psTdmaDuAdjType = 9;
-				}
-				else if(pCoexDm->curPsTdma == 14)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-					pCoexDm->psTdmaDuAdjType = 10;
-				}
-				else if(pCoexDm->curPsTdma == 15)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-					pCoexDm->psTdmaDuAdjType = 11;
-				}
-				else if(pCoexDm->curPsTdma == 16)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-					pCoexDm->psTdmaDuAdjType = 12;
-				}
-				
-				if(result == -1)
-				{
-					if(pCoexDm->curPsTdma == 1)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-						pCoexDm->psTdmaDuAdjType = 4;
-					}
-					else if(pCoexDm->curPsTdma == 9)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-						pCoexDm->psTdmaDuAdjType = 10;
-					}
-					else if(pCoexDm->curPsTdma == 10)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 11)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-						pCoexDm->psTdmaDuAdjType = 12;
-					}
-				} 
-				else if (result == 1)
-				{
-					if(pCoexDm->curPsTdma == 4)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 1);
-						pCoexDm->psTdmaDuAdjType = 1;
-					}
-					else if(pCoexDm->curPsTdma == 12)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 11)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-						pCoexDm->psTdmaDuAdjType = 10;
-					}
-					else if(pCoexDm->curPsTdma == 10)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-						pCoexDm->psTdmaDuAdjType = 9;
-					}
-				}
-			}
-		}
-		else if(maxInterval == 2)
-		{
-			if(bTxPause)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
-				if(pCoexDm->curPsTdma == 1)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-					pCoexDm->psTdmaDuAdjType = 6;
-				}
-				else if(pCoexDm->curPsTdma == 2)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-					pCoexDm->psTdmaDuAdjType = 6;
-				}
-				else if(pCoexDm->curPsTdma == 3)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-					pCoexDm->psTdmaDuAdjType = 7;
-				}
-				else if(pCoexDm->curPsTdma == 4)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-					pCoexDm->psTdmaDuAdjType = 8;
-				}
-				if(pCoexDm->curPsTdma == 9)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-					pCoexDm->psTdmaDuAdjType = 14;
-				}
-				else if(pCoexDm->curPsTdma == 10)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-					pCoexDm->psTdmaDuAdjType = 14;
-				}
-				else if(pCoexDm->curPsTdma == 11)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-					pCoexDm->psTdmaDuAdjType = 15;
-				}
-				else if(pCoexDm->curPsTdma == 12)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-					pCoexDm->psTdmaDuAdjType = 16;
-				}
-				if(result == -1)
-				{
-					if(pCoexDm->curPsTdma == 5) 
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-						pCoexDm->psTdmaDuAdjType = 6;
-					}
-					else if(pCoexDm->curPsTdma == 6)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 7)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-						pCoexDm->psTdmaDuAdjType = 8;
-					}
-					else if(pCoexDm->curPsTdma == 13)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-						pCoexDm->psTdmaDuAdjType = 14;
-					}
-					else if(pCoexDm->curPsTdma == 14)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 15)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-						pCoexDm->psTdmaDuAdjType = 16;
-					}
-				} 
-				else if (result == 1)
-				{
-					if(pCoexDm->curPsTdma == 8)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 7)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-						pCoexDm->psTdmaDuAdjType = 6;
-					}
-					else if(pCoexDm->curPsTdma == 6)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-						pCoexDm->psTdmaDuAdjType = 6;
-					}					
-					else if(pCoexDm->curPsTdma == 16)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 15)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-						pCoexDm->psTdmaDuAdjType = 14;
-					}
-					else if(pCoexDm->curPsTdma == 14)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-						pCoexDm->psTdmaDuAdjType = 14;
-					}
-				}
-			}
-			else
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
-				if(pCoexDm->curPsTdma == 5)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-					pCoexDm->psTdmaDuAdjType = 2;
-				}
-				else if(pCoexDm->curPsTdma == 6)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-					pCoexDm->psTdmaDuAdjType = 2;
-				}
-				else if(pCoexDm->curPsTdma == 7)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-					pCoexDm->psTdmaDuAdjType = 3;
-				}
-				else if(pCoexDm->curPsTdma == 8)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-					pCoexDm->psTdmaDuAdjType = 4;
-				}
-				if(pCoexDm->curPsTdma == 13)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-					pCoexDm->psTdmaDuAdjType = 10;
-				}
-				else if(pCoexDm->curPsTdma == 14)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-					pCoexDm->psTdmaDuAdjType = 10;
-				}
-				else if(pCoexDm->curPsTdma == 15)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-					pCoexDm->psTdmaDuAdjType = 11;
-				}
-				else if(pCoexDm->curPsTdma == 16)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-					pCoexDm->psTdmaDuAdjType = 12;
-				}
-				if(result == -1)
-				{
-					if(pCoexDm->curPsTdma == 1)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-						pCoexDm->psTdmaDuAdjType = 4;
-					}
-					else if(pCoexDm->curPsTdma == 9)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-						pCoexDm->psTdmaDuAdjType = 10;
-					}
-					else if(pCoexDm->curPsTdma == 10)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 11)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-						pCoexDm->psTdmaDuAdjType = 12;
-					}
-				} 
-				else if (result == 1)
-				{
-					if(pCoexDm->curPsTdma == 4)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-						pCoexDm->psTdmaDuAdjType = 2;
-					}
-					else if(pCoexDm->curPsTdma == 12)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 11)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-						pCoexDm->psTdmaDuAdjType = 10;
-					}
-					else if(pCoexDm->curPsTdma == 10)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-						pCoexDm->psTdmaDuAdjType = 10;
-					}
-				}
-			}
-		}
-		else if(maxInterval == 3)
-		{
-			if(bTxPause)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 1\n"));
-				if(pCoexDm->curPsTdma == 1)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-					pCoexDm->psTdmaDuAdjType = 7;
-				}
-				else if(pCoexDm->curPsTdma == 2)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-					pCoexDm->psTdmaDuAdjType = 7;
-				}
-				else if(pCoexDm->curPsTdma == 3)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-					pCoexDm->psTdmaDuAdjType = 7;
-				}
-				else if(pCoexDm->curPsTdma == 4)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-					pCoexDm->psTdmaDuAdjType = 8;
-				}
-				if(pCoexDm->curPsTdma == 9)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-					pCoexDm->psTdmaDuAdjType = 15;
-				}
-				else if(pCoexDm->curPsTdma == 10)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-					pCoexDm->psTdmaDuAdjType = 15;
-				}
-				else if(pCoexDm->curPsTdma == 11)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-					pCoexDm->psTdmaDuAdjType = 15;
-				}
-				else if(pCoexDm->curPsTdma == 12)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-					pCoexDm->psTdmaDuAdjType = 16;
-				}
-				if(result == -1)
-				{
-					if(pCoexDm->curPsTdma == 5) 
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 6)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 7)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-						pCoexDm->psTdmaDuAdjType = 8;
-					}
-					else if(pCoexDm->curPsTdma == 13)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 14)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 15)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-						pCoexDm->psTdmaDuAdjType = 16;
-					}
-				} 
-				else if (result == 1)
-				{
-					if(pCoexDm->curPsTdma == 8)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 7)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}
-					else if(pCoexDm->curPsTdma == 6)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 7);
-						pCoexDm->psTdmaDuAdjType = 7;
-					}					
-					else if(pCoexDm->curPsTdma == 16)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 15)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-					else if(pCoexDm->curPsTdma == 14)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-						pCoexDm->psTdmaDuAdjType = 15;
-					}
-				}
-			}
-			else
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], TxPause = 0\n"));
-				if(pCoexDm->curPsTdma == 5)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-					pCoexDm->psTdmaDuAdjType = 3;
-				}
-				else if(pCoexDm->curPsTdma == 6)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-					pCoexDm->psTdmaDuAdjType = 3;
-				}
-				else if(pCoexDm->curPsTdma == 7)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-					pCoexDm->psTdmaDuAdjType = 3;
-				}
-				else if(pCoexDm->curPsTdma == 8)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-					pCoexDm->psTdmaDuAdjType = 4;
-				}
-				if(pCoexDm->curPsTdma == 13)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-					pCoexDm->psTdmaDuAdjType = 11;
-				}
-				else if(pCoexDm->curPsTdma == 14)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-					pCoexDm->psTdmaDuAdjType = 11;
-				}
-				else if(pCoexDm->curPsTdma == 15)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-					pCoexDm->psTdmaDuAdjType = 11;
-				}
-				else if(pCoexDm->curPsTdma == 16)
-				{
-					halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-					pCoexDm->psTdmaDuAdjType = 12;
-				}
-				if(result == -1)
-				{
-					if(pCoexDm->curPsTdma == 1)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-						pCoexDm->psTdmaDuAdjType = 4;
-					}
-					else if(pCoexDm->curPsTdma == 9)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 10)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 11)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-						pCoexDm->psTdmaDuAdjType = 12;
-					}
-				} 
-				else if (result == 1)
-				{
-					if(pCoexDm->curPsTdma == 4)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 3)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 2)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 3);
-						pCoexDm->psTdmaDuAdjType = 3;
-					}
-					else if(pCoexDm->curPsTdma == 12)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 11)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-					else if(pCoexDm->curPsTdma == 10)
-					{
-						halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-						pCoexDm->psTdmaDuAdjType = 11;
-					}
-				}
-			}
-		}
-	}
-
-	// if current PsTdma not match with the recorded one (when scan, dhcp...), 
-	// then we have to adjust it back to the previous record one.
-	if(pCoexDm->curPsTdma != pCoexDm->psTdmaDuAdjType)
-	{
-		BOOLEAN	bScan=FALSE, bLink=FALSE, bRoam=FALSE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], PsTdma type dismatch!!!, curPsTdma=%d, recordPsTdma=%d\n", 
-			pCoexDm->curPsTdma, pCoexDm->psTdmaDuAdjType));
-
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-		pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-		
-		if( !bScan && !bLink && !bRoam)
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, pCoexDm->psTdmaDuAdjType);
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL, ("[BTCoex], roaming/link/scan is under progress, will adjust next time!!!\n"));
-		}
-	}
-}
-
-// SCO only or SCO+PAN(HS)
-VOID
-halbtc8723a2ant_ActionSco(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte	wifiRssiState, wifiRssiState1;
-	u4Byte	wifiBw;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-	
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-		}
-
-		// sw mechanism		
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 11);
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 15);
-		}
-		
-		// sw mechanism
-		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}		
-	}
-}
-
-
-VOID
-halbtc8723a2ant_ActionHid(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte	wifiRssiState, wifiRssiState1;
-	u4Byte	wifiBw;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
-		}
-
-		// sw mechanism
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
-
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 9);
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 13);
-		}
-
-		// sw mechanism
-		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}		
-	}
-}
-
-//A2DP only / PAN(EDR) only/ A2DP+PAN(HS)
-VOID
-halbtc8723a2ant_ActionA2dp(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-	
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 3);
-			}
-			else
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 1);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 3);
-			}
-			else
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 1);
-			}
-		}
-
-		// sw mechanism		
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 3);
-			}
-			else
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, FALSE, 1);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp rate, 1:basic /0:edr
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 3);
-			}
-			else
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, FALSE, true, 1);
-			}
-		}
-		
-		// sw mechanism
-		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}		
-	}
-}
-
-VOID
-halbtc8723a2ant_ActionPanEdr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
-	u4Byte		wifiBw;
-	
-	btInfoExt = pCoexSta->btInfoExt;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-			
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-		}
-
-		// sw mechanism
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-		}
-
-		// sw mechanism
-		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-	}
-}
-
-
-//PAN(HS) only
-VOID
-halbtc8723a2ant_ActionPanHs(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState;
-	u4Byte		wifiBw;
-
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-		}
-		else
-		{
-			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-		}
-		halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-
-		// sw mechanism		
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		}
-		else
-		{
-			halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, FALSE, 0);
-		}
-
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-	}
-}
-
-//PAN(EDR)+A2DP
-VOID
-halbtc8723a2ant_ActionPanEdrA2dp(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-			}
-		}
-
-		// sw mechanism
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 47, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 4);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 2);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 8);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 6);
-			}
-		}
-
-		// sw mechanism
-		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-	}
-}
-
-VOID
-halbtc8723a2ant_ActionPanEdrHid(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, wifiRssiState1;
-	u4Byte		wifiBw;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10); 
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14); 
-		}
-
-		// sw mechanism
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-		}
-		else
-		{
-			halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-		}
-		
-		// sw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-	}
-}
-
-// HID+A2DP+PAN(EDR)
-VOID
-halbtc8723a2ant_ActionHidA2dpPanEdr(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-			
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-			}
-		}
-		
-		// sw mechanism
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 12);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 10);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 16);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_PsTdma(pBtCoexist, NORMAL_EXEC, true, 14);
-			}
-		}
-
-		// sw mechanism
-		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-	}
-}
-
-VOID
-halbtc8723a2ant_ActionHidA2dp(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u1Byte		wifiRssiState, wifiRssiState1, btInfoExt;
-	u4Byte		wifiBw;
-
-	btInfoExt = pCoexSta->btInfoExt;
-
-	if(halbtc8723a2ant_NeedToDecBtPwr(pBtCoexist))
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, true);
-	else	
-		halbtc8723a2ant_DecBtPwr(pBtCoexist, NORMAL_EXEC, FALSE);
-	halbtc8723a2ant_CoexTable(pBtCoexist, NORMAL_EXEC, 0x55555555, 0xffff, 0x3);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	if(BTC_WIFI_BW_HT40 == wifiBw)
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 37, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 3);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 1);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 3);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 1);
-			}
-		}
-		
-		// sw mechanism
-		halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-		halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-		halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-	}
-	else
-	{
-		wifiRssiState = halbtc8723a2ant_WifiRssiState(pBtCoexist, 0, 2, 27, 0);
-		wifiRssiState1 = halbtc8723a2ant_WifiRssiState(pBtCoexist, 1, 2, 47, 0);
-		
-		// fw mechanism
-		if( (wifiRssiState == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 3);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, FALSE, 1);
-			}
-		}
-		else
-		{
-			if(btInfoExt&BIT0)	//a2dp basic rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 3);
-			}
-			else				//a2dp edr rate
-			{
-				halbtc8723a2ant_TdmaDurationAdjust(pBtCoexist, true, true, 1);
-			}
-		}
-
-		// sw mechanism
-		if( (wifiRssiState1 == BTC_RSSI_STATE_HIGH) ||
-			(wifiRssiState1 == BTC_RSSI_STATE_STAY_HIGH) )
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, true);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-		else
-		{
-			halbtc8723a2ant_AgcTable(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_AdcBackOff(pBtCoexist, NORMAL_EXEC, FALSE);
-			halbtc8723a2ant_DacSwing(pBtCoexist, NORMAL_EXEC, FALSE, 0xc0);
-		}
-	}
-}
-
-VOID
-halbtc8723a2ant_RunCoexistMechanism(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
-	u1Byte				btInfoOriginal=0, btRetryCnt=0;
-	u1Byte				algorithm=0;
-
-	if(pBtCoexist->manual_control)
-	{
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Manual control!!!\n"));
-		return;
-	}
-
-	if(pStackInfo->bProfileNotified)
-	{
-		if(pCoexSta->bHoldForStackOperation)
-		{
-			// if bt inquiry/page/pair, do not execute.
-			return;
-		}
-		
-		algorithm = halbtc8723a2ant_ActionAlgorithm(pBtCoexist);
-		if(pCoexSta->bHoldPeriodCnt && (BT_8723A_2ANT_COEX_ALGO_PANHS!=algorithm))
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex],Hold BT inquiry/page scan setting (cnt = %d)!!\n", 
-				pCoexSta->bHoldPeriodCnt));
-			if(pCoexSta->bHoldPeriodCnt >= 6)
-			{
-				pCoexSta->bHoldPeriodCnt = 0;
-				// next time the coexist parameters should be reset again.
-			}
-			else
-				pCoexSta->bHoldPeriodCnt++;
-			return;
-		}
-
-		pCoexDm->curAlgorithm = algorithm;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Algorithm = %d \n", pCoexDm->curAlgorithm));
-		if(halbtc8723a2ant_IsCommonAction(pBtCoexist))
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant common.\n"));
-			pCoexDm->bResetTdmaAdjust = true;
-		}
-		else
-		{
-			if(pCoexDm->curAlgorithm != pCoexDm->preAlgorithm)
-			{
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], preAlgorithm=%d, curAlgorithm=%d\n", 
-					pCoexDm->preAlgorithm, pCoexDm->curAlgorithm));
-				pCoexDm->bResetTdmaAdjust = true;
-			}
-			switch(pCoexDm->curAlgorithm)
-			{
-				case BT_8723A_2ANT_COEX_ALGO_SCO:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = SCO.\n"));
-					halbtc8723a2ant_ActionSco(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_HID:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID.\n"));
-					halbtc8723a2ant_ActionHid(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_A2DP:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = A2DP.\n"));
-					halbtc8723a2ant_ActionA2dp(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_PANEDR:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR).\n"));
-					halbtc8723a2ant_ActionPanEdr(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_PANHS:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HS mode.\n"));
-					halbtc8723a2ant_ActionPanHs(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_PANEDR_A2DP:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN+A2DP.\n"));
-					halbtc8723a2ant_ActionPanEdrA2dp(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_PANEDR_HID:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = PAN(EDR)+HID.\n"));
-					halbtc8723a2ant_ActionPanEdrHid(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP+PAN.\n"));
-					halbtc8723a2ant_ActionHidA2dpPanEdr(pBtCoexist);
-					break;
-				case BT_8723A_2ANT_COEX_ALGO_HID_A2DP:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = HID+A2DP.\n"));
-					halbtc8723a2ant_ActionHidA2dp(pBtCoexist);
-					break;
-				default:
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, ("[BTCoex], Action 2-Ant, algorithm = coexist All Off!!\n"));
-					halbtc8723a2ant_CoexAllOff(pBtCoexist);
-					break;
-			}
-			pCoexDm->preAlgorithm = pCoexDm->curAlgorithm;
-		}
-	}
-}
-
-//============================================================
-// work around function start with wa_halbtc8723a2ant_
-//============================================================
-VOID
-wa_halbtc8723a2ant_MonitorC2h(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	u1Byte	tmp1b=0x0;
-	u4Byte	curC2hTotalCnt=0x0;
-	static u4Byte	preC2hTotalCnt=0x0, sameCntPollingTime=0x0;
-
-	curC2hTotalCnt+=pCoexSta->btInfoC2hCnt[BT_INFO_SRC_8723A_2ANT_BT_RSP];
-
-	if(curC2hTotalCnt == preC2hTotalCnt)
-	{
-		sameCntPollingTime++;
-	}
-	else
-	{
-		preC2hTotalCnt = curC2hTotalCnt;
-		sameCntPollingTime = 0;
-	}
-
-	if(sameCntPollingTime >= 2)
-	{
-		tmp1b = pBtCoexist->btc_read_1byte(pBtCoexist, 0x1af);
-		if(tmp1b != 0x0)
-		{
-			pCoexSta->c2hHangDetectCnt++;
-			pBtCoexist->btc_write_1byte(pBtCoexist, 0x1af, 0x0);
-		}
-	}
-}
-
-//============================================================
-// extern function start with EXhalbtc8723a2ant_
-//============================================================
-VOID
-EXhalbtc8723a2ant_InitHwConfig(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	u4Byte	u4Tmp=0;
-	u1Byte	u1Tmp=0;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], 2Ant Init HW Config!!\n"));
-
-	// backup rf 0x1e value
-	pCoexDm->btRf0x1eBackup = 
-		pBtCoexist->btc_get_rf_reg(pBtCoexist, BTC_RF_A, 0x1e, 0xfffff);
-
-	// Enable counter statistics
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x76e, 0x4);
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x778, 0x3);
-	pBtCoexist->btc_write_1byte(pBtCoexist, 0x40, 0x20);
-}
-
-VOID
-EXhalbtc8723a2ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT, ("[BTCoex], Coex Mechanism Init!!\n"));
-	
-	halbtc8723a2ant_InitCoexDm(pBtCoexist);
-}
-
-VOID
-EXhalbtc8723a2ant_DisplayCoexInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	)
-{
-	struct btc_board_info *		pBoardInfo=&pBtCoexist->board_info;
-	PBTC_STACK_INFO		pStackInfo=&pBtCoexist->stack_info;
-	pu1Byte				cliBuf=pBtCoexist->cli_buf;
-	u1Byte				u1Tmp[4], i, btInfoExt, psTdmaCase=0;
-	u4Byte				u4Tmp[4];
-	BOOLEAN				bRoam=FALSE, bScan=FALSE, bLink=FALSE, bWifiUnder5G=FALSE;
-	BOOLEAN				bBtHsOn=FALSE, bWifiBusy=FALSE;
-	s4Byte				wifiRssi=0, btHsRssi=0;
-	u4Byte				wifiBw, wifiTrafficDir;
-	u1Byte				wifiDot11Chnl, wifiHsChnl;
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cliBuf);
-
-	if(!pBoardInfo->bt_exist)
-	{
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cliBuf);
-		return;
-	}
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "Ant PG number/ Ant mechanism:", \
-		pBoardInfo->pg_ant_num, pBoardInfo->btdm_ant_num);
-	CL_PRINTF(cliBuf);	
-	
-	if(pBtCoexist->manual_control)
-	{
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "[Action Manual control]!!");
-		CL_PRINTF(cliBuf);
-	}
-	
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d", "BT stack/ hci ext ver", \
-		((pStackInfo->bProfileNotified)? "Yes":"No"), pStackInfo->hciVersion);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_DOT11_CHNL, &wifiDot11Chnl);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifiHsChnl);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)", "Dot11 channel / HsChnl(HsMode)", \
-		wifiDot11Chnl, wifiHsChnl, bBtHsOn);
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ", "H2C Wifi inform bt chnl Info", \
-		pCoexDm->wifiChnlInfo[0], pCoexDm->wifiChnlInfo[1],
-		pCoexDm->wifiChnlInfo[2]);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_WIFI_RSSI, &wifiRssi);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_S4_HS_RSSI, &btHsRssi);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "Wifi rssi/ HS rssi", \
-		wifiRssi, btHsRssi);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_SCAN, &bScan);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_LINK, &bLink);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_ROAM, &bRoam);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ", "Wifi bLink/ bRoam/ bScan", \
-		bLink, bRoam, bScan);
-	CL_PRINTF(cliBuf);
-
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_UNDER_5G, &bWifiUnder5G);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_BW, &wifiBw);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_WIFI_BUSY, &bWifiBusy);
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION, &wifiTrafficDir);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ", "Wifi status", \
-		(bWifiUnder5G? "5G":"2.4G"),
-		((BTC_WIFI_BW_LEGACY==wifiBw)? "Legacy": (((BTC_WIFI_BW_HT40==wifiBw)? "HT40":"HT20"))),
-		((!bWifiBusy)? "idle": ((BTC_WIFI_TRAFFIC_TX==wifiTrafficDir)? "uplink":"downlink")));
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ", "BT [status/ rssi/ retryCnt]", \
-		((pCoexSta->bC2hBtInquiryPage)?("inquiry/page scan"):((BT_8723A_2ANT_BT_STATUS_IDLE == pCoexDm->btStatus)? "idle":(  (BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE == pCoexDm->btStatus)? "connected-idle":"busy"))),
-		pCoexSta->btRssi, pCoexSta->btRetryCnt);
-	CL_PRINTF(cliBuf);
-	
-	if(pStackInfo->bProfileNotified)
-	{			
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d", "SCO/HID/PAN/A2DP", \
-			pStackInfo->bScoExist, pStackInfo->bHidExist, pStackInfo->bPanExist, pStackInfo->bA2dpExist);
-		CL_PRINTF(cliBuf);	
-
-		pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_BT_LINK_INFO);
-	}
-
-	btInfoExt = pCoexSta->btInfoExt;
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s", "BT Info A2DP rate", \
-		(btInfoExt&BIT0)? "Basic rate":"EDR rate");
-	CL_PRINTF(cliBuf);	
-
-	for(i=0; i<BT_INFO_SRC_8723A_2ANT_MAX; i++)
-	{
-		if(pCoexSta->btInfoC2hCnt[i])
-		{				
-			CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x %02x %02x(%d)", GLBtInfoSrc8723a2Ant[i], \
-				pCoexSta->btInfoC2h[i][0], pCoexSta->btInfoC2h[i][1],
-				pCoexSta->btInfoC2h[i][2], pCoexSta->btInfoC2h[i][3],
-				pCoexSta->btInfoC2h[i][4], pCoexSta->btInfoC2h[i][5],
-				pCoexSta->btInfoC2h[i][6], pCoexSta->btInfoC2hCnt[i]);
-			CL_PRINTF(cliBuf);
-		}
-	}
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d", "write 0x1af=0x0 num", \
-		pCoexSta->c2hHangDetectCnt);
-	CL_PRINTF(cliBuf);
-	
-	// Sw mechanism	
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Sw mechanism]============");
-	CL_PRINTF(cliBuf);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d", "SM1[ShRf/ LpRA/ LimDig]", \
-		pCoexDm->bCurRfRxLpfShrink, pCoexDm->bCurLowPenaltyRa, pCoexDm->limited_dig);
-	CL_PRINTF(cliBuf);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ", "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
-		pCoexDm->bCurAgcTableEn, pCoexDm->bCurAdcBackOff, pCoexDm->bCurDacSwingOn, pCoexDm->curDacSwingLvl);
-	CL_PRINTF(cliBuf);
-
-	// Fw mechanism		
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Fw mechanism]============");
-	CL_PRINTF(cliBuf);	
-	
-	if(!pBtCoexist->manual_control)
-	{
-		psTdmaCase = pCoexDm->curPsTdma;
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x %02x %02x case-%d", "PS TDMA", \
-			pCoexDm->psTdmaPara[0], pCoexDm->psTdmaPara[1],
-			pCoexDm->psTdmaPara[2], pCoexDm->psTdmaPara[3],
-			pCoexDm->psTdmaPara[4], psTdmaCase);
-		CL_PRINTF(cliBuf);
-	
-		CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ", "DecBtPwr/ IgnWlanAct", \
-			pCoexDm->bCurDecBtPwr, pCoexDm->bCurIgnoreWlanAct);
-		CL_PRINTF(cliBuf);
-	}
-
-	// Hw setting		
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s", "============[Hw setting]============");
-	CL_PRINTF(cliBuf);	
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "RF-A, 0x1e initVal", \
-		pCoexDm->btRf0x1eBackup);
-	CL_PRINTF(cliBuf);
-
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x778);
-	u1Tmp[1] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x783);
-	u1Tmp[2] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x796);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x", "0x778/ 0x783/ 0x796", \
-		u1Tmp[0], u1Tmp[1], u1Tmp[2]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x880);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x880", \
-		u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x40);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x40", \
-		u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x550);
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x522);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x", "0x550(bcn ctrl)/0x522", \
-		u4Tmp[0], u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x484);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x484(rate adaptive)", \
-		u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xc50);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0xc50(dig)", \
-		u4Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda0);
-	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda4);
-	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xda8);
-	u4Tmp[3] = pBtCoexist->btc_read_4byte(pBtCoexist, 0xdac);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0xda0/0xda4/0xda8/0xdac(FA cnt)", \
-		u4Tmp[0], u4Tmp[1], u4Tmp[2], u4Tmp[3]);
-	CL_PRINTF(cliBuf);
-
-	u4Tmp[0] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c0);
-	u4Tmp[1] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c4);
-	u4Tmp[2] = pBtCoexist->btc_read_4byte(pBtCoexist, 0x6c8);
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x6cc);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x", "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
-		u4Tmp[0], u4Tmp[1], u4Tmp[2], u1Tmp[0]);
-	CL_PRINTF(cliBuf);
-
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x770 (hp rx[31:16]/tx[15:0])", \
-		pCoexSta->highPriorityRx, pCoexSta->highPriorityTx);
-	CL_PRINTF(cliBuf);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d", "0x774(lp rx[31:16]/tx[15:0])", \
-		pCoexSta->lowPriorityRx, pCoexSta->lowPriorityTx);
-	CL_PRINTF(cliBuf);
-
-	// Tx mgnt queue hang or not, 0x41b should = 0xf, ex: 0xd ==>hang
-	u1Tmp[0] = pBtCoexist->btc_read_1byte(pBtCoexist, 0x41b);
-	CL_SPRINTF(cliBuf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x", "0x41b (mgntQ hang chk == 0xf)", \
-		u1Tmp[0]);
-	CL_PRINTF(cliBuf);	
-
-	pBtCoexist->btc_disp_dbg_msg(pBtCoexist, BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-
-VOID
-EXhalbtc8723a2ant_IpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	if(BTC_IPS_ENTER == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS ENTER notify\n"));
-		halbtc8723a2ant_CoexAllOff(pBtCoexist);
-	}
-	else if(BTC_IPS_LEAVE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], IPS LEAVE notify\n"));
-		//halbtc8723a2ant_InitCoexDm(pBtCoexist);
-	}
-}
-
-VOID
-EXhalbtc8723a2ant_LpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	if(BTC_LPS_ENABLE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS ENABLE notify\n"));
-	}
-	else if(BTC_LPS_DISABLE == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], LPS DISABLE notify\n"));
-	}
-}
-
-VOID
-EXhalbtc8723a2ant_ScanNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	if(BTC_SCAN_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN START notify\n"));
-	}
-	else if(BTC_SCAN_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], SCAN FINISH notify\n"));
-	}
-}
-
-VOID
-EXhalbtc8723a2ant_ConnectNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	)
-{
-	if(BTC_ASSOCIATE_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT START notify\n"));
-	}
-	else if(BTC_ASSOCIATE_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], CONNECT FINISH notify\n"));
-	}
-}
-
-VOID
-EXhalbtc8723a2ant_MediaStatusNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	if(BTC_MEDIA_CONNECT == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA connect notify\n"));
-	}
-	else
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], MEDIA disconnect notify\n"));
-	}
-
-	halbtc8723a2ant_IndicateWifiChnlBwInfo(pBtCoexist, type);
-}
-
-VOID
-EXhalbtc8723a2ant_SpecialPacketNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	if(type == BTC_PACKET_DHCP)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], DHCP Packet notify\n"));
-	}
-}
-
-VOID
-EXhalbtc8723a2ant_BtInfoNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	pu1Byte			tmpBuf,
-	IN	u1Byte			length
-	)
-{
-	u1Byte			btInfo=0;
-	u1Byte			i, rspSource=0;
-	BOOLEAN			bBtBusy=FALSE, limited_dig=FALSE;
-	BOOLEAN			bWifiConnected=FALSE, bBtHsOn=FALSE;
-
-	pCoexSta->bC2hBtInfoReqSent = FALSE;
-	
-	rspSource = BT_INFO_SRC_8723A_2ANT_BT_RSP;
-	pCoexSta->btInfoC2hCnt[rspSource]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Bt info[%d], length=%d, hex data=[", rspSource, length));
-	for(i=0; i<length; i++)
-	{
-		pCoexSta->btInfoC2h[rspSource][i] = tmpBuf[i];
-		if(i == 0)
-			btInfo = tmpBuf[i];
-		if(i == length-1)
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x]\n", tmpBuf[i]));
-		}
-		else
-		{
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("0x%02x, ", tmpBuf[i]));
-		}
-	}
-
-	if(BT_INFO_SRC_8723A_2ANT_WIFI_FW != rspSource)
-	{
-		pCoexSta->btRetryCnt =
-			pCoexSta->btInfoC2h[rspSource][1];
-
-		pCoexSta->btRssi =
-			pCoexSta->btInfoC2h[rspSource][2]*2+10;
-
-		pCoexSta->btInfoExt = 
-			pCoexSta->btInfoC2h[rspSource][3];
-	}
-		
-	pBtCoexist->btc_get(pBtCoexist, BTC_GET_BL_HS_OPERATION, &bBtHsOn);
-	// check BIT2 first ==> check if bt is under inquiry or page scan
-	if(btInfo & BT_INFO_8723A_2ANT_B_INQ_PAGE)
-	{
-		pCoexSta->bC2hBtInquiryPage = true;
-	}
-	else
-	{
-		pCoexSta->bC2hBtInquiryPage = FALSE;
-	}
-}
-
-VOID
-EXhalbtc8723a2ant_StackOperationNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	)
-{
-	if(BTC_STACK_OP_INQ_PAGE_PAIR_START == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair start notify\n"));
-		pCoexSta->bHoldForStackOperation = true;
-		pCoexSta->bHoldPeriodCnt = 1;
-		halbtc8723a2ant_BtInquiryPage(pBtCoexist);
-	}
-	else if(BTC_STACK_OP_INQ_PAGE_PAIR_FINISH == type)
-	{
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], StackOP Inquiry/page/pair finish notify\n"));
-		pCoexSta->bHoldForStackOperation = FALSE;
-	}
-}
-
-VOID
-EXhalbtc8723a2ant_HaltNotify(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], Halt notify\n"));
-
-	halbtc8723a2ant_IgnoreWlanAct(pBtCoexist, FORCE_EXEC, true);
-	EXhalbtc8723a2ant_MediaStatusNotify(pBtCoexist, BTC_MEDIA_DISCONNECT);
-}
-
-VOID
-EXhalbtc8723a2ant_Periodical(
-	IN	PBTC_COEXIST			pBtCoexist
-	)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, ("[BTCoex], 2Ant Periodical!!\n"));
-
-	// work around for c2h hang
-	wa_halbtc8723a2ant_MonitorC2h(pBtCoexist);
-	
-	halbtc8723a2ant_QueryBtInfo(pBtCoexist);
-	halbtc8723a2ant_MonitorBtCtr(pBtCoexist);
-	halbtc8723a2ant_MonitorBtEnableDisable(pBtCoexist);
-
-	halbtc8723a2ant_RunCoexistMechanism(pBtCoexist);
-}
-
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.h
deleted file mode 100644
index c07d3738aadc..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723a2ant.h
+++ /dev/null
@@ -1,179 +0,0 @@
-//===========================================
-// The following is for 8723A 2Ant BT Co-exist definition
-//===========================================
-#define	BT_INFO_8723A_2ANT_B_FTP						BIT7
-#define	BT_INFO_8723A_2ANT_B_A2DP					BIT6
-#define	BT_INFO_8723A_2ANT_B_HID						BIT5
-#define	BT_INFO_8723A_2ANT_B_SCO_BUSY				BIT4
-#define	BT_INFO_8723A_2ANT_B_ACL_BUSY				BIT3
-#define	BT_INFO_8723A_2ANT_B_INQ_PAGE				BIT2
-#define	BT_INFO_8723A_2ANT_B_SCO_ESCO				BIT1
-#define	BT_INFO_8723A_2ANT_B_CONNECTION				BIT0
-
-#define		BTC_RSSI_COEX_THRESH_TOL_8723A_2ANT		2
-
-typedef enum _BT_INFO_SRC_8723A_2ANT{
-	BT_INFO_SRC_8723A_2ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8723A_2ANT_BT_RSP				= 0x1,
-	BT_INFO_SRC_8723A_2ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8723A_2ANT_MAX
-}BT_INFO_SRC_8723A_2ANT,*PBT_INFO_SRC_8723A_2ANT;
-
-typedef enum _BT_8723A_2ANT_BT_STATUS{
-	BT_8723A_2ANT_BT_STATUS_IDLE				= 0x0,
-	BT_8723A_2ANT_BT_STATUS_CONNECTED_IDLE	= 0x1,
-	BT_8723A_2ANT_BT_STATUS_NON_IDLE			= 0x2,
-	BT_8723A_2ANT_BT_STATUS_MAX
-}BT_8723A_2ANT_BT_STATUS,*PBT_8723A_2ANT_BT_STATUS;
-
-typedef enum _BT_8723A_2ANT_COEX_ALGO{
-	BT_8723A_2ANT_COEX_ALGO_UNDEFINED			= 0x0,
-	BT_8723A_2ANT_COEX_ALGO_SCO				= 0x1,
-	BT_8723A_2ANT_COEX_ALGO_HID				= 0x2,
-	BT_8723A_2ANT_COEX_ALGO_A2DP				= 0x3,
-	BT_8723A_2ANT_COEX_ALGO_PANEDR			= 0x4,
-	BT_8723A_2ANT_COEX_ALGO_PANHS			= 0x5,
-	BT_8723A_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x6,
-	BT_8723A_2ANT_COEX_ALGO_PANEDR_HID		= 0x7,
-	BT_8723A_2ANT_COEX_ALGO_HID_A2DP_PANEDR	= 0x8,
-	BT_8723A_2ANT_COEX_ALGO_HID_A2DP			= 0x9,
-	BT_8723A_2ANT_COEX_ALGO_MAX
-}BT_8723A_2ANT_COEX_ALGO,*PBT_8723A_2ANT_COEX_ALGO;
-
-typedef struct _COEX_DM_8723A_2ANT{
-	// fw mechanism
-	BOOLEAN		bPreDecBtPwr;
-	BOOLEAN		bCurDecBtPwr;
-	//BOOLEAN		bPreBtLnaConstrain;
-	//BOOLEAN		bCurBtLnaConstrain;
-	//u1Byte		bPreBtPsdMode;
-	//u1Byte		bCurBtPsdMode;
-	u1Byte		preFwDacSwingLvl;
-	u1Byte		curFwDacSwingLvl;
-	BOOLEAN		bCurIgnoreWlanAct;
-	BOOLEAN		bPreIgnoreWlanAct;
-	u1Byte		prePsTdma;
-	u1Byte		curPsTdma;
-	u1Byte		psTdmaPara[5];
-	u1Byte		psTdmaDuAdjType;
-	BOOLEAN		bResetTdmaAdjust;
-	BOOLEAN		bPrePsTdmaOn;
-	BOOLEAN		bCurPsTdmaOn;
-	//BOOLEAN		bPreBtAutoReport;
-	//BOOLEAN		bCurBtAutoReport;
-
-	// sw mechanism
-	BOOLEAN		bPreRfRxLpfShrink;
-	BOOLEAN		bCurRfRxLpfShrink;
-	u4Byte		btRf0x1eBackup;
-	BOOLEAN 	bPreLowPenaltyRa;
-	BOOLEAN		bCurLowPenaltyRa;
-	BOOLEAN		bPreDacSwingOn;
-	u4Byte		preDacSwingLvl;
-	BOOLEAN		bCurDacSwingOn;
-	u4Byte		curDacSwingLvl;
-	BOOLEAN		bPreAdcBackOff;
-	BOOLEAN		bCurAdcBackOff;
-	BOOLEAN 	bPreAgcTableEn;
-	BOOLEAN		bCurAgcTableEn;
-	u4Byte		preVal0x6c0;
-	u4Byte		curVal0x6c0;
-	u4Byte		preVal0x6c8;
-	u4Byte		curVal0x6c8;
-	u1Byte		preVal0x6cc;
-	u1Byte		curVal0x6cc;
-	BOOLEAN		limited_dig;
-
-	// algorithm related
-	u1Byte		preAlgorithm;
-	u1Byte		curAlgorithm;
-	u1Byte		btStatus;
-	u1Byte		wifiChnlInfo[3];
-} COEX_DM_8723A_2ANT, *PCOEX_DM_8723A_2ANT;
-
-typedef struct _COEX_STA_8723A_2ANT{
-	u4Byte					highPriorityTx;
-	u4Byte					highPriorityRx;
-	u4Byte					lowPriorityTx;
-	u4Byte					lowPriorityRx;
-	u1Byte					btRssi;
-	u1Byte					preBtRssiState;
-	u1Byte					preBtRssiState1;
-	u1Byte					preWifiRssiState[4];
-	BOOLEAN					bC2hBtInfoReqSent;
-	u1Byte					btInfoC2h[BT_INFO_SRC_8723A_2ANT_MAX][10];
-	u4Byte					btInfoC2hCnt[BT_INFO_SRC_8723A_2ANT_MAX];
-	BOOLEAN					bC2hBtInquiryPage;
-	u1Byte					btRetryCnt;
-	u1Byte					btInfoExt;
-	BOOLEAN					bHoldForStackOperation;
-	u1Byte					bHoldPeriodCnt;
-	// this is for c2h hang work-around
-	u4Byte					c2hHangDetectCnt;
-}COEX_STA_8723A_2ANT, *PCOEX_STA_8723A_2ANT;
-
-//===========================================
-// The following is interface which will notify coex module.
-//===========================================
-VOID
-EXhalbtc8723a2ant_InitHwConfig(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-VOID
-EXhalbtc8723a2ant_InitCoexDm(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-VOID
-EXhalbtc8723a2ant_IpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a2ant_LpsNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a2ant_ScanNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a2ant_ConnectNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	u1Byte			type
-	);
-VOID
-EXhalbtc8723a2ant_MediaStatusNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8723a2ant_SpecialPacketNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8723a2ant_HaltNotify(
-	IN	PBTC_COEXIST			pBtCoexist
-	);
-VOID
-EXhalbtc8723a2ant_Periodical(
-	IN	PBTC_COEXIST			pBtCoexist
-	);
-VOID
-EXhalbtc8723a2ant_BtInfoNotify(
-	IN	PBTC_COEXIST		pBtCoexist,
-	IN	pu1Byte			tmpBuf,
-	IN	u1Byte			length
-	);
-VOID
-EXhalbtc8723a2ant_StackOperationNotify(
-	IN	PBTC_COEXIST			pBtCoexist,
-	IN	u1Byte				type
-	);
-VOID
-EXhalbtc8723a2ant_DisplayCoexInfo(
-	IN	PBTC_COEXIST		pBtCoexist
-	);
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
deleted file mode 100644
index c9693944c137..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b1ant.c
+++ /dev/null
@@ -1,3892 +0,0 @@
-/***************************************************************
- * Description:
- *
- * This file is for RTL8723B Co-exist mechanism
- *
- * History
- * 2012/11/15 Cosa first check in.
- *
- ***************************************************************/
-
-
-/***************************************************************
- * include files
- ***************************************************************/
-#include "halbt_precomp.h"
-#if 1
-/***************************************************************
- * Global variables, these are static variables
- ***************************************************************/
-static struct coex_dm_8723b_1ant glcoex_dm_8723b_1ant;
-static struct coex_dm_8723b_1ant *coex_dm = &glcoex_dm_8723b_1ant;
-static struct coex_sta_8723b_1ant glcoex_sta_8723b_1ant;
-static struct coex_sta_8723b_1ant *coex_sta = &glcoex_sta_8723b_1ant;
-
-static const char *const GLBtInfoSrc8723b1Ant[] = {
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-static u32 glcoex_ver_date_8723b_1ant = 20130906;
-static u32 glcoex_ver_8723b_1ant = 0x45;
-
-/***************************************************************
- * local function proto type if needed
- ***************************************************************/
-/***************************************************************
- * local function start with halbtc8723b1ant_
- ***************************************************************/
-static u8 halbtc8723b1ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
-					u8 rssi_thresh1)
-{
-	s32 bt_rssi=0;
-	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
-
-	bt_rssi = coex_sta->bt_rssi;
-
-	if (level_num == 2){
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= rssi_thresh +
-					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to High\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at Low\n");
-			}
-		} else {
-			if (bt_rssi < rssi_thresh) {
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Low\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "[BTCoex], BT Rssi thresh error!!\n");
-			return coex_sta->pre_bt_rssi_state;
-		}
-
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= rssi_thresh +
-					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Medium\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at Low\n");
-			}
-		} else if ((coex_sta->pre_bt_rssi_state ==
-					BTC_RSSI_STATE_MEDIUM) ||
-			  (coex_sta->pre_bt_rssi_state ==
-			  		BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (bt_rssi >= rssi_thresh1 +
-					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to High\n");
-			} else if (bt_rssi < rssi_thresh) {
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Low\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at Medium\n");
-			}
-		} else {
-			if (bt_rssi < rssi_thresh1) {
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Medium\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_bt_rssi_state = bt_rssi_state;
-
-	return bt_rssi_state;
-}
-
-static u8 halbtc8723b1ant_wifi_rssi_state(struct btc_coexist *btcoexist,
-					u8 index, u8 level_num,
-					u8 rssi_thresh, u8 rssi_thresh1)
-{
-	s32 wifi_rssi=0;
-	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
-
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-
-	if (level_num == 2) {
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-					BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-		    			BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= rssi_thresh +
-					BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to High\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at Low\n");
-			}
-		} else {
-			if (wifi_rssi < rssi_thresh) {
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Low\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
-				  "[BTCoex], wifi RSSI thresh error!!\n");
-			return coex_sta->pre_wifi_rssi_state[index];
-		}
-
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-		    				BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= rssi_thresh +
-					 BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Medium\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at Low\n");
-			}
-		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (wifi_rssi >= rssi_thresh1 +
-					 BTC_RSSI_COEX_THRESH_TOL_8723B_1ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to High\n");
-			} else if (wifi_rssi < rssi_thresh) {
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Low\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at Medium\n");
-			}
-		} else {
-			if (wifi_rssi < rssi_thresh1) {
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Medium\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
-
-	return wifi_rssi_state;
-}
-
-static void halbtc8723b1ant_updatera_mask(struct btc_coexist *btcoexist,
-					bool force_exec, u32 dis_rate_mask)
-{
-	coex_dm->curra_mask = dis_rate_mask;
-
-	if (force_exec || (coex_dm->prera_mask != coex_dm->curra_mask))
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_UPDATE_ra_mask,
-				   &coex_dm->curra_mask);
-
-	coex_dm->prera_mask = coex_dm->curra_mask;
-}
-
-static void halbtc8723b1ant_auto_rate_fallback_retry(
-						struct btc_coexist *btcoexist,
-						bool force_exec, u8 type)
-{
-	bool wifi_under_bmode = false;
-
-	coex_dm->cur_arfr_type = type;
-
-	if (force_exec || (coex_dm->pre_arfr_type != coex_dm->cur_arfr_type)) {
-		switch (coex_dm->cur_arfr_type) {
-		case 0:	/* normal mode */
-			btcoexist->btc_write_4byte(btcoexist, 0x430,
-						   coex_dm->backup_arfr_cnt1);
-			btcoexist->btc_write_4byte(btcoexist, 0x434,
-						   coex_dm->backup_arfr_cnt2);
-			break;
-		case 1:
-			btcoexist->btc_get(btcoexist,
-					   BTC_GET_BL_WIFI_UNDER_B_MODE,
-					   &wifi_under_bmode);
-			if (wifi_under_bmode) {
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x430, 0x0);
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x434, 0x01010101);
-			} else {
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x430, 0x0);
-				btcoexist->btc_write_4byte(btcoexist,
-							   0x434, 0x04030201);
-			}
-			break;
-		default:
-			break;
-		}
-	}
-
-	coex_dm->pre_arfr_type = coex_dm->cur_arfr_type;
-}
-
-static void halbtc8723b1ant_retry_limit(struct btc_coexist *btcoexist,
-					bool force_exec, u8 type)
-{
-	coex_dm->cur_retry_limit_type = type;
-
-	if (force_exec || (coex_dm->pre_retry_limit_type !=
-			   coex_dm->cur_retry_limit_type)) {
-
-		switch (coex_dm->cur_retry_limit_type) {
-		case 0:	/* normal mode */
-			btcoexist->btc_write_2byte(btcoexist, 0x42a,
-						   coex_dm->backup_retry_limit);
-			break;
-		case 1:	/* retry limit=8 */
-			btcoexist->btc_write_2byte(btcoexist, 0x42a, 0x0808);
-			break;
-		default:
-			break;
-		}
-	}
-
-	coex_dm->pre_retry_limit_type = coex_dm->cur_retry_limit_type;
-}
-
-static void halbtc8723b1ant_ampdu_maxtime(struct btc_coexist *btcoexist,
-					bool force_exec, u8 type)
-{
-	coex_dm->cur_ampdu_time_type = type;
-
-	if (force_exec || (coex_dm->pre_ampdu_time_type !=
-		coex_dm->cur_ampdu_time_type)) {
-		switch (coex_dm->cur_ampdu_time_type) {
-			case 0:	/* normal mode */
-				btcoexist->btc_write_1byte(btcoexist, 0x456,
-						coex_dm->backup_ampdu_max_time);
-				break;
-			case 1:	/* AMPDU timw = 0x38 * 32us */
-				btcoexist->btc_write_1byte(btcoexist,
-							   0x456, 0x38);
-				break;
-			default:
-				break;
-		}
-	}
-
-	coex_dm->pre_ampdu_time_type = coex_dm->cur_ampdu_time_type;
-}
-
-static void halbtc8723b1ant_limited_tx(struct btc_coexist *btcoexist,
-				bool force_exec, u8 ra_maskType, u8 arfr_type,
-				u8 retry_limit_type, u8 ampdu_time_type)
-{
-	switch (ra_maskType) {
-	case 0:	/* normal mode */
-		halbtc8723b1ant_updatera_mask(btcoexist, force_exec, 0x0);
-		break;
-	case 1:	/* disable cck 1/2 */
-		halbtc8723b1ant_updatera_mask(btcoexist, force_exec,
-					      0x00000003);
-		break;
-	/* disable cck 1/2/5.5, ofdm 6/9/12/18/24, mcs 0/1/2/3/4*/
-	case 2:
-		halbtc8723b1ant_updatera_mask(btcoexist, force_exec,
-					      0x0001f1f7);
-		break;
-	default:
-		break;
-	}
-
-	halbtc8723b1ant_auto_rate_fallback_retry(btcoexist, force_exec,
-						 arfr_type);
-	halbtc8723b1ant_retry_limit(btcoexist, force_exec, retry_limit_type);
-	halbtc8723b1ant_ampdu_maxtime(btcoexist, force_exec, ampdu_time_type);
-}
-
-static void halbtc8723b1ant_limited_rx(struct btc_coexist *btcoexist,
-				bool force_exec, bool rej_ap_agg_pkt,
-				bool b_bt_ctrl_agg_buf_size, u8 agg_buf_size)
-{
-	bool reject_rx_agg = rej_ap_agg_pkt;
-	bool bt_ctrl_rx_agg_size = b_bt_ctrl_agg_buf_size;
-	u8 rxAggSize = agg_buf_size;
-
-	/**********************************************
-	 *	Rx Aggregation related setting
-	 **********************************************/
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_TO_REJ_AP_AGG_PKT,
-			   &reject_rx_agg);
-	/* decide BT control aggregation buf size or not  */
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_CTRL_AGG_SIZE,
-			   &bt_ctrl_rx_agg_size);
-	/* aggregation buf size, only work
-	 *when BT control Rx aggregation size.  */
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_AGG_BUF_SIZE, &rxAggSize);
-	/* real update aggregation setting  */
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_AGGREGATE_CTRL, NULL);
-}
-
-static void halbtc8723b1ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
-{
-	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
-	u32 reg_hp_tx = 0, reg_hp_rx = 0;
-	u32 reg_lp_tx = 0, reg_lp_rx = 0;
-
-	reg_hp_txrx = 0x770;
-	reg_lp_txrx = 0x774;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
-	reg_hp_tx = u32tmp & MASKLWORD;
-	reg_hp_rx = (u32tmp & MASKHWORD) >> 16;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
-	reg_lp_tx = u32tmp & MASKLWORD;
-	reg_lp_rx = (u32tmp & MASKHWORD) >> 16;
-
-	coex_sta->high_priority_tx = reg_hp_tx;
-	coex_sta->high_priority_rx = reg_hp_rx;
-	coex_sta->low_priority_tx = reg_lp_tx;
-	coex_sta->low_priority_rx = reg_lp_rx;
-
-	/* reset counter */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-}
-
-static void halbtc8723b1ant_query_bt_info(struct btc_coexist *btcoexist)
-{
-	u8 h2c_parameter[1] = {0};
-
-	coex_sta->c2h_bt_info_req_sent = true;
-
-	h2c_parameter[0] |= BIT0;	/* trigger*/
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Query Bt Info, FW write 0x61=0x%x\n",
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
-}
-
-static bool halbtc8723b1ant_is_wifi_status_changed(
-						struct btc_coexist *btcoexist)
-{
-	static bool pre_wifi_busy = false;
-	static bool pre_under_4way = false, pre_bt_hs_on = false;
-	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
-	bool wifi_connected = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
- 	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
-			   &under_4way);
-
-	if (wifi_connected) {
-		if (wifi_busy != pre_wifi_busy) {
-			pre_wifi_busy = wifi_busy;
-			return true;
-		}
-		if (under_4way != pre_under_4way) {
-			pre_under_4way = under_4way;
-			return true;
-		}
-		if (bt_hs_on != pre_bt_hs_on) {
-			pre_bt_hs_on = bt_hs_on;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-static void halbtc8723b1ant_update_bt_link_info(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hs_on = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-
-	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
-	bt_link_info->sco_exist = coex_sta->sco_exist;
-	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
-	bt_link_info->pan_exist = coex_sta->pan_exist;
-	bt_link_info->hid_exist = coex_sta->hid_exist;
-
-	/* work around for HS mode. */
-	if (bt_hs_on) {
-		bt_link_info->pan_exist = true;
-		bt_link_info->bt_link_exist = true;
-	}
-
-	/* check if Sco only */
-	if (bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->sco_only = true;
-	else
-		bt_link_info->sco_only = false;
-
-	/* check if A2dp only */
-	if (!bt_link_info->sco_exist && bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->a2dp_only = true;
-	else
-		bt_link_info->a2dp_only = false;
-
-	/* check if Pan only */
-	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->pan_only = true;
-	else
-		bt_link_info->pan_only = false;
-
-	/* check if Hid only */
-	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && bt_link_info->hid_exist )
-		bt_link_info->hid_only = true;
-	else
-		bt_link_info->hid_only = false;
-}
-
-static u8 halbtc8723b1ant_action_algorithm(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hs_on = false;
-	u8 algorithm = BT_8723B_1ANT_COEX_ALGO_UNDEFINED;
-	u8 numOfDiffProfile = 0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-
-	if (!bt_link_info->bt_link_exist) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], No BT link exists!!!\n");
-		return algorithm;
-	}
-
-	if (bt_link_info->sco_exist)
-		numOfDiffProfile++;
-	if (bt_link_info->hid_exist)
-		numOfDiffProfile++;
-	if (bt_link_info->pan_exist)
-		numOfDiffProfile++;
-	if (bt_link_info->a2dp_exist)
-		numOfDiffProfile++;
-
-	if (numOfDiffProfile == 1) {
-		if (bt_link_info->sco_exist) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], BT Profile = SCO only\n");
-			algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
-		} else {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], BT Profile = HID only\n");
-				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], BT Profile = A2DP only\n");
-				algorithm = BT_8723B_1ANT_COEX_ALGO_A2DP;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "PAN(HS) only\n");
-					algorithm =
-						BT_8723B_1ANT_COEX_ALGO_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "PAN(EDR) only\n");
-					algorithm =
-						BT_8723B_1ANT_COEX_ALGO_PANEDR;
-				}
-			}
-		}
-	} else if (numOfDiffProfile == 2) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], BT Profile = SCO + HID\n");
-				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], BT Profile = "
-					  "SCO + A2DP ==> SCO\n");
-				algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile "
-						  "= SCO + PAN(HS)\n");
-					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile "
-						  "= SCO + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], BT Profile = "
-					  "HID + A2DP\n");
-				algorithm = BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "HID + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "HID + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "A2DP + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP;
-				}
-			}
-		}
-	} else if (numOfDiffProfile == 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], BT Profile = "
-					  "SCO + HID + A2DP ==> HID\n");
-				algorithm = BT_8723B_1ANT_COEX_ALGO_HID;
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "SCO + HID + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "SCO + HID + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "SCO + A2DP + PAN(HS)\n");
-					algorithm = BT_8723B_1ANT_COEX_ALGO_SCO;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = SCO + "
-						  "A2DP + PAN(EDR) ==> HID\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "HID + A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "HID + A2DP + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				}
-			}
-		}
-	} else if (numOfDiffProfile >= 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], Error!!! "
-						  "BT Profile = SCO + "
-						  "HID + A2DP + PAN(HS)\n");
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], BT Profile = "
-						  "SCO + HID + A2DP + PAN(EDR)"
-						  "==>PAN(EDR)+HID\n");
-					algorithm =
-					    BT_8723B_1ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-	}
-
-	return algorithm;
-}
-
-static bool halbtc8723b1ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
-{
-	bool ret = false;
-	bool bt_hs_on = false, wifi_connected = false;
-	s32 bt_hs_rssi = 0;
-	u8 bt_rssi_state;
-
-	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on))
-		return false;
-	if (!btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-				&wifi_connected))
-		return false;
-	if (!btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
-		return false;
-
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 35, 0);
-
-	if (wifi_connected) {
-		if (bt_hs_on) {
-			if (bt_hs_rssi > 37)
-				ret = true;
-		} else {
-			if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-				ret = true;
-		}
-	}
-
-	return ret;
-}
-
-static void halbtc8723b1ant_set_fw_dac_swing_level(
-						struct btc_coexist *btcoexist,
-						u8 dac_swing_lvl)
-{
-	u8 h2c_parameter[1] = {0};
-
-	/* There are several type of dacswing
-	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
-	h2c_parameter[0] = dac_swing_lvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x64=0x%x\n", h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
-}
-
-static void halbtc8723b1ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
-				       bool dec_bt_pwr)
-{
-	u8 h2c_parameter[1] = {0};
-
-	h2c_parameter[0] = 0;
-
-	if (dec_bt_pwr)
-		h2c_parameter[0] |= BIT1;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], decrease Bt Power : %s, FW write 0x62=0x%x\n",
-		  (dec_bt_pwr? "Yes!!":"No!!"),h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
-}
-
-static void halbtc8723b1ant_dec_bt_pwr(struct btc_coexist *btcoexist,
-				bool force_exec, bool dec_bt_pwr)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s Dec BT power = %s\n",
-		  (force_exec ? "force to" : ""), (dec_bt_pwr ? "ON" : "OFF"));
-	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreDecBtPwr=%d, bCurDecBtPwr=%d\n",
-			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
-
-		if (coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr)
-			return;
-	}
-	halbtc8723b1ant_set_fw_dec_bt_pwr(btcoexist, coex_dm->cur_dec_bt_pwr);
-
-	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
-}
-
-#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
-static void halbtc8723b1ant_set_bt_auto_report(struct btc_coexist *btcoexist,
-					bool enable_auto_report)
-{
-	u8 h2c_parameter[1] = {0};
-
-	h2c_parameter[0] = 0;
-
-	if (enable_auto_report)
-		h2c_parameter[0] |= BIT0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n",
-		  (enable_auto_report? "Enabled!!":"Disabled!!"),
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
-}
-
-static void halbtc8723b1ant_bt_auto_report(struct btc_coexist *btcoexist,
-				    bool force_exec, bool enable_auto_report)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s BT Auto report = %s\n",
-		  (force_exec? "force to":""),
-		  ((enable_auto_report)? "Enabled":"Disabled"));
-	coex_dm->cur_bt_auto_report = enable_auto_report;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreBtAutoReport=%d, "
-			  "bCurBtAutoReport=%d\n",
-			  coex_dm->pre_bt_auto_report,
-			  coex_dm->cur_bt_auto_report);
-
-		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
-			return;
-	}
-	halbtc8723b1ant_set_bt_auto_report(btcoexist,
-					   coex_dm->cur_bt_auto_report);
-
-	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
-}
-#endif
-
-static void halbtc8723b1ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
-				      bool force_exec, u8 fw_dac_swing_lvl)
-{
-	return;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s set FW Dac Swing level = %d\n",
-		  (force_exec? "force to":""), fw_dac_swing_lvl);
-	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], preFwDacSwingLvl=%d, "
-			  "curFwDacSwingLvl=%d\n",
-			  coex_dm->pre_fw_dac_swing_lvl,
-			  coex_dm->cur_fw_dac_swing_lvl);
-
-		if (coex_dm->pre_fw_dac_swing_lvl ==
-		    coex_dm->cur_fw_dac_swing_lvl)
-			return;
-	}
-
-	halbtc8723b1ant_set_fw_dac_swing_level(btcoexist,
-					       coex_dm->cur_fw_dac_swing_lvl);
-
-	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
-}
-
-static void halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(
-					struct btc_coexist *btcoexist,
-					bool low_penalty_ra)
-{
-	u8 h2c_parameter[6] = {0};
-
-	h2c_parameter[0] = 0x6;	/* opCode, 0x6= Retry_Penalty */
-
-	if (low_penalty_ra) {
-		h2c_parameter[1] |= BIT0;
-		/*normal rate except MCS7/6/5, OFDM54/48/36 */
-		h2c_parameter[2] = 0x00;
-		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54 */
-		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48 */
-		h2c_parameter[5] = 0xf9;  /*MCS5 or OFDM36 */
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
-		  (low_penalty_ra ? "ON!!" : "OFF!!"));
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
-}
-
-static void halbtc8723b1ant_low_penalty_ra(struct btc_coexist *btcoexist,
-				    bool force_exec, bool low_penalty_ra)
-{
-	coex_dm->cur_low_penalty_ra = low_penalty_ra;
-
-	if (!force_exec) {
-		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
-			return;
-	}
-	halbtc8723b1ant_set_sw_penalty_tx_rate_adaptive(btcoexist,
-						coex_dm->cur_low_penalty_ra);
-
-	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
-}
-
-static void halbtc8723b1ant_set_coex_table(struct btc_coexist *btcoexist,
-				    u32 val0x6c0, u32 val0x6c4,
-				    u32 val0x6c8, u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c4=0x%x\n", val0x6c4);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc);
-	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
-}
-
-static void halbtc8723b1ant_coex_table(struct btc_coexist *btcoexist,
- 				bool force_exec, u32 val0x6c0,
- 				u32 val0x6c4, u32 val0x6c8,
- 				u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s write Coex Table 0x6c0=0x%x,"
-		  " 0x6c4=0x%x, 0x6cc=0x%x\n", (force_exec ? "force to" : ""),
-		  val0x6c0, val0x6c4, val0x6cc);
-	coex_dm->cur_val0x6c0 = val0x6c0;
-	coex_dm->cur_val0x6c4 = val0x6c4;
-	coex_dm->cur_val0x6c8 = val0x6c8;
-	coex_dm->cur_val0x6cc = val0x6cc;
-
-	if (!force_exec) {
-		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
-		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
-		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
-		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
-			return;
-	}
-	halbtc8723b1ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
-				       val0x6c8, val0x6cc);
-
-	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
-	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
-	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
-	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
-}
-
-static void halbtc8723b1ant_coex_table_with_type(struct btc_coexist *btcoexist,
-					  bool force_exec, u8 type)
-{
-	switch (type) {
-	case 0:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x55555555, 0xffffff, 0x3);
-		break;
-	case 1:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x5a5a5a5a, 0xffffff, 0x3);
-		break;
-	case 2:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
-					   0x5a5a5a5a, 0xffffff, 0x3);
-		break;
-	case 3:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0xaaaaaaaa, 0xffffff, 0x3);
-		break;
-	case 4:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x5aaa5aaa, 0xffffff, 0x3);
-		break;
-	case 5:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
-					   0xaaaa5a5a, 0xffffff, 0x3);
-		break;
-	case 6:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0xaaaa5a5a, 0xffffff, 0x3);
-		break;
-	case 7:
-		halbtc8723b1ant_coex_table(btcoexist, force_exec, 0x5afa5afa,
-					   0x5afa5afa, 0xffffff, 0x3);
-		break;
-	default:
-		break;
-	}
-}
-
-static void halbtc8723b1ant_SetFwIgnoreWlanAct(struct btc_coexist *btcoexist,
-					bool enable)
-{
-	u8 h2c_parameter[1] = {0};
-
-	if (enable)
-		h2c_parameter[0] |= BIT0;	/* function enable */
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], set FW for BT Ignore Wlan_Act,"
-		  " FW write 0x63=0x%x\n", h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
-}
-
-static void halbtc8723b1ant_ignore_wlan_act(struct btc_coexist *btcoexist,
-				     bool force_exec, bool enable)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn Ignore WlanAct %s\n",
-		  (force_exec ? "force to" : ""), (enable ? "ON" : "OFF"));
-	coex_dm->cur_ignore_wlan_act = enable;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreIgnoreWlanAct = %d, "
-			  "bCurIgnoreWlanAct = %d!!\n",
-			  coex_dm->pre_ignore_wlan_act,
-			  coex_dm->cur_ignore_wlan_act);
-
-		if (coex_dm->pre_ignore_wlan_act ==
-		    coex_dm->cur_ignore_wlan_act)
-			return;
-	}
-	halbtc8723b1ant_SetFwIgnoreWlanAct(btcoexist, enable);
-
-	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
-}
-
-static void halbtc8723b1ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
-				    u8 byte1, u8 byte2, u8 byte3,
-				    u8 byte4, u8 byte5)
-{
-	u8 h2c_parameter[5] = {0};
-	u8 real_byte1 = byte1, real_byte5 = byte5;
-	bool ap_enable = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-			   &ap_enable);
-
-	if (ap_enable) {
-		if ((byte1 & BIT4) && !(byte1 & BIT5)) {
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "[BTCoex], FW for 1Ant AP mode\n");
-			real_byte1 &= ~BIT4;
-			real_byte1 |= BIT5;
-
-			real_byte5 |= BIT5;
-			real_byte5 &= ~BIT6;
-		}
-	}
-
-	h2c_parameter[0] = real_byte1;
-	h2c_parameter[1] = byte2;
-	h2c_parameter[2] = byte3;
-	h2c_parameter[3] = byte4;
-	h2c_parameter[4] = real_byte5;
-
-	coex_dm->ps_tdma_para[0] = real_byte1;
-	coex_dm->ps_tdma_para[1] = byte2;
-	coex_dm->ps_tdma_para[2] = byte3;
-	coex_dm->ps_tdma_para[3] = byte4;
-	coex_dm->ps_tdma_para[4] = real_byte5;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], PS-TDMA H2C cmd =0x%x%08x\n",
-		  h2c_parameter[0],
-		  h2c_parameter[1] << 24 |
-		  h2c_parameter[2] << 16 |
-		  h2c_parameter[3] << 8 |
-		  h2c_parameter[4]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
-}
-
-static void halbtc8723b1ant_SetLpsRpwm(struct btc_coexist *btcoexist,
-				u8 lps_val, u8 rpwm_val)
-{
-	u8 lps = lps_val;
-	u8 rpwm = rpwm_val;
-
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_LPS, &lps);
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_1ANT_RPWM, &rpwm);
-}
-
-static void halbtc8723b1ant_LpsRpwm(struct btc_coexist *btcoexist,
-				bool force_exec, u8 lps_val, u8 rpwm_val)
-{
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s set lps/rpwm=0x%x/0x%x \n",
-		  (force_exec ? "force to" : ""), lps_val, rpwm_val);
-	coex_dm->cur_lps = lps_val;
-	coex_dm->cur_rpwm = rpwm_val;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], LPS-RxBeaconMode=0x%x , LPS-RPWM=0x%x!!\n",
-			  coex_dm->cur_lps, coex_dm->cur_rpwm);
-
-		if ((coex_dm->pre_lps == coex_dm->cur_lps) &&
-		    (coex_dm->pre_rpwm == coex_dm->cur_rpwm)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], LPS-RPWM_Last=0x%x"
-				  " , LPS-RPWM_Now=0x%x!!\n",
-				  coex_dm->pre_rpwm, coex_dm->cur_rpwm);
-
-			return;
-		}
-	}
-	halbtc8723b1ant_SetLpsRpwm(btcoexist, lps_val, rpwm_val);
-
-	coex_dm->pre_lps = coex_dm->cur_lps;
-	coex_dm->pre_rpwm = coex_dm->cur_rpwm;
-}
-
-static void halbtc8723b1ant_sw_mechanism1(struct btc_coexist *btcoexist,
-				   bool shrink_rx_lpf, bool low_penalty_ra,
-				   bool limited_dig, bool bt_lna_constrain)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex], SM1[ShRf/ LpRA/ LimDig/ btLna] = %d %d %d %d\n",
-		  shrink_rx_lpf, low_penalty_ra, limited_dig, bt_lna_constrain);
-
-	halbtc8723b1ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
-}
-
-static void halbtc8723b1ant_sw_mechanism2(struct btc_coexist *btcoexist,
-				   bool agc_table_shift, bool adc_backoff,
-				   bool sw_dac_swing, u32 dac_swing_lvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex], SM2[AgcT/ AdcB/ SwDacSwing(lvl)] = %d %d %d\n",
-		  agc_table_shift, adc_backoff, sw_dac_swing);
-}
-
-static void halbtc8723b1ant_SetAntPath(struct btc_coexist *btcoexist,
-				u8 ant_pos_type, bool init_hw_cfg,
-				bool wifi_off)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	u32 fw_ver = 0, u32tmp = 0;
-	bool pg_ext_switch = false;
-	bool use_ext_switch = false;
-	u8 h2c_parameter[2] = {0};
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
-	/* [31:16]=fw ver, [15:0]=fw sub ver */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-
-
-	if ((fw_ver < 0xc0000) || pg_ext_switch)
-		use_ext_switch = true;
-
-	if (init_hw_cfg){
-		/*BT select s0/s1 is controlled by WiFi */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
-
-		/*Force GNT_BT to Normal */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
-	} else if (wifi_off) {
-		/*Force GNT_BT to High */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
-		/*BT select s0/s1 is controlled by BT */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
-
-		/* 0x4c[24:23]=00, Set Antenna control by BT_RFE_CTRL
-		 * BT Vendor 0xac=0xf002 */
-		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u32tmp &= ~BIT23;
-		u32tmp &= ~BIT24;
-		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
-	}
-
-	if (use_ext_switch) {
-		if (init_hw_cfg) {
-			/* 0x4c[23]=0, 0x4c[24]=1  Antenna control by WL/BT */
-			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-			u32tmp &= ~BIT23;
-			u32tmp |= BIT24;
-			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
-
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT) {
-				/* Main Ant to  BT for IPS case 0x4c[23]=1 */
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x1);
-
-				/*tell firmware "no antenna inverse"*/
-				h2c_parameter[0] = 0;
-				h2c_parameter[1] = 1;  /*ext switch type*/
-				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-							h2c_parameter);
-			} else {
-				/*Aux Ant to  BT for IPS case 0x4c[23]=1 */
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x0);
-
-				/*tell firmware "antenna inverse"*/
-				h2c_parameter[0] = 1;
-				h2c_parameter[1] = 1;  /*ext switch type*/
-				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-							h2c_parameter);
-			}
-		}
-
-		/* fixed internal switch first*/
-		/* fixed internal switch S1->WiFi, S0->BT*/
-		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
- 			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-		else/* fixed internal switch S0->WiFi, S1->BT*/
-			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
-
-		/* ext switch setting */
-		switch (ant_pos_type) {
-		case BTC_ANT_PATH_WIFI:
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x1);
-			else
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x92c, 0x3,
-								   0x2);
-			break;
-		case BTC_ANT_PATH_BT:
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-							    	   0x92c, 0x3,
-							    	   0x2);
-			else
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-							    	   0x92c, 0x3,
-							    	   0x1);
-			break;
-		default:
-		case BTC_ANT_PATH_PTA:
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-							    	   0x92c, 0x3,
-							    	   0x1);
-			else
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-							    	   0x92c, 0x3,
-							    	   0x2);
-			break;
-		}
-
-	} else {
-		if (init_hw_cfg) {
-			/* 0x4c[23]=1, 0x4c[24]=0  Antenna control by 0x64*/
-			u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-			u32tmp |= BIT23;
-			u32tmp &= ~BIT24;
-			btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
-
-			if (board_info->btdm_ant_pos ==
-			    BTC_ANTENNA_AT_MAIN_PORT) {
-				/*Main Ant to  WiFi for IPS case 0x4c[23]=1*/
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x0);
-
-				/*tell firmware "no antenna inverse"*/
-				h2c_parameter[0] = 0;
-				h2c_parameter[1] = 0;  /*internal switch type*/
-				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-							h2c_parameter);
-			} else {
-				/*Aux Ant to  BT for IPS case 0x4c[23]=1*/
-				btcoexist->btc_write_1byte_bitmask(btcoexist,
-								   0x64, 0x1,
-								   0x1);
-
-				/*tell firmware "antenna inverse"*/
-				h2c_parameter[0] = 1;
-				h2c_parameter[1] = 0;  /*internal switch type*/
-				btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-							h2c_parameter);
-			}
-		}
-
-		/* fixed external switch first*/
-		/*Main->WiFi, Aux->BT*/
-		if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x1);
-		else/*Main->BT, Aux->WiFi */
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x2);
-
-		/* internal switch setting*/
-		switch (ant_pos_type) {
-		case BTC_ANT_PATH_WIFI:
-			if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x0);
-			else
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x280);
-			break;
-		case BTC_ANT_PATH_BT:
-			if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x280);
-			else
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x0);
-			break;
-		default:
-		case BTC_ANT_PATH_PTA:
-			if(board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT)
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x200);
-			else
-				btcoexist->btc_write_2byte(btcoexist, 0x948,
-							   0x80);
-			break;
-		}
-	}
-}
-
-static void halbtc8723b1ant_ps_tdma(struct btc_coexist *btcoexist,
-					bool force_exec, bool turn_on, u8 type)
-{
-	bool wifi_busy = false;
-	u8 rssi_adjust_val = 0;
-
-	coex_dm->cur_ps_tdma_on = turn_on;
-	coex_dm->cur_ps_tdma = type;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-
-	if (!force_exec) {
-		if (coex_dm->cur_ps_tdma_on)
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], ******** TDMA(on, %d) *********\n",
-				  coex_dm->cur_ps_tdma);
-		else
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], ******** TDMA(off, %d) ********\n",
-				  coex_dm->cur_ps_tdma);
-
-
-		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
-		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
-			return;
-	}
-	if (turn_on) {
-		switch (type) {
-		default:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1a,
-						       0x1a, 0x0, 0x50);
-			break;
-		case 1:
-			if (wifi_busy)
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
-							       0x3a, 0x03,
-							       0x10, 0x50);
-			else
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist,0x51,
-							       0x3a, 0x03,
-							       0x10, 0x51);
-
-			rssi_adjust_val = 11;
-			break;
-		case 2:
-			if (wifi_busy)
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
-							       0x2b, 0x03,
-							       0x10, 0x50);
-			else
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
-							       0x2b, 0x03,
-							       0x10, 0x51);
-			rssi_adjust_val = 14;
-			break;
-		case 3:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x1d,
-						       0x1d, 0x0, 0x52);
-			break;
-		case 4:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
-						       0x3, 0x14, 0x0);
-			rssi_adjust_val = 17;
-			break;
-		case 5:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x15,
-						       0x3, 0x11, 0x10);
-			break;
-		case 6:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x20,
-						       0x3, 0x11, 0x13);
-			break;
-		case 7:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xc,
-						       0x5, 0x0, 0x0);
-			break;
-		case 8:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
-						       0x3, 0x10, 0x0);
-			break;
-		case 9:
-			if(wifi_busy)
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
-							       0x21, 0x3,
-							       0x10, 0x50);
-			else
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
-							       0x21, 0x3,
-							       0x10, 0x50);
-			rssi_adjust_val = 18;
-			break;
-		case 10:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
-						       0xa, 0x0, 0x40);
-			break;
-		case 11:
-			if (wifi_busy)
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
-							       0x15, 0x03,
-							       0x10, 0x50);
-			else
-				halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51,
-							       0x15, 0x03,
-							       0x10, 0x50);
-			rssi_adjust_val = 20;
-			break;
-		case 12:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x0a,
-						       0x0a, 0x0, 0x50);
-			break;
-		case 13:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x15,
-						       0x15, 0x0, 0x50);
-			break;
-		case 14:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x21,
-						       0x3, 0x10, 0x52);
-			break;
-		case 15:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x13, 0xa,
-						       0x3, 0x8, 0x0);
-			break;
-		case 16:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x15,
-						       0x3, 0x10, 0x0);
-			rssi_adjust_val = 18;
-			break;
-		case 18:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x93, 0x25,
-						       0x3, 0x10, 0x0);
-			rssi_adjust_val = 14;
-			break;
-		case 20:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x35,
-						       0x03, 0x11, 0x10);
-			break;
-		case 21:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x15,
-						       0x03, 0x11, 0x10);
-			break;
-		case 22:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0x25,
-						       0x03, 0x11, 0x10);
-			break;
-		case 23:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
-						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
-			break;
-		case 24:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x15,
-						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
-			break;
-		case 25:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
-						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
-			break;
-		case 26:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
-						       0x3, 0x31, 0x18);
-			rssi_adjust_val = 22;
-			break;
-		case 27:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
-						       0x3, 0x31, 0x98);
-			rssi_adjust_val = 22;
-			break;
-		case 28:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x69, 0x25,
-						       0x3, 0x31, 0x0);
-			break;
-		case 29:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xab, 0x1a,
-						       0x1a, 0x1, 0x10);
-			break;
-		case 30:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x51, 0x14,
-						       0x3, 0x10, 0x50);
-			break;
-		case 31:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x1a,
-						       0x1a, 0, 0x58);
-			break;
-		case 32:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x61, 0xa,
-						       0x3, 0x10, 0x0);
-			break;
-		case 33:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x25,
-						       0x3, 0x30, 0x90);
-			break;
-		case 34:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x53, 0x1a,
-						       0x1a, 0x0, 0x10);
-			break;
-		case 35:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x63, 0x1a,
-						       0x1a, 0x0, 0x10);
-			break;
-		case 36:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0xd3, 0x12,
-						       0x3, 0x14, 0x50);
-			break;
-		/* SoftAP only with no sta associated,BT disable ,
-		 * TDMA mode for power saving
-		 * here softap mode screen off will cost 70-80mA for phone */
-		case 40:
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x23, 0x18,
-						       0x00, 0x10, 0x24);
-			break;
-		}
-	} else {
-		switch (type) {
-		case 8: /*PTA Control */
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x8, 0x0,
-						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_PTA,
-						   false, false);
-			break;
-		case 0:
-		default:  /*Software control, Antenna at BT side */
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
-						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT,
-						   false, false);
-			break;
-		case 9:   /*Software control, Antenna at WiFi side */
-			halbtc8723b1ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0,
-						       0x0, 0x0, 0x0);
-			halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_WIFI,
-						   false, false);
-			break;
-		}
-	}
-	rssi_adjust_val = 0;
-	btcoexist->btc_set(btcoexist,
-			   BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
-			   &rssi_adjust_val);
-
-	/* update pre state */
-	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
-	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
-}
-
-static bool halbtc8723b1ant_is_common_action(struct btc_coexist *btcoexist)
-{
-	bool commom = false, wifi_connected = false;
-	bool wifi_busy = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-
-	if (!wifi_connected &&
-	    BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi non connected-idle + "
-			  "BT non connected-idle!!\n");
-		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
- 		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-					      false, false);
-		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-					      false, 0x18);
-
-		commom = true;
-	} else if (wifi_connected &&
-		   (BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE ==
-		    coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi connected + "
-			  "BT non connected-idle!!\n");
-		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-      		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-					      false, false);
-		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-					      false, 0x18);
-
-		commom = true;
-	} else if (!wifi_connected &&
-		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE ==
-		    coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi non connected-idle + "
-			  "BT connected-idle!!\n");
-		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-					      false, false);
-		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-					      false, 0x18);
-
-		commom = true;
-	} else if (wifi_connected &&
-		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE ==
-		    coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi connected + BT connected-idle!!\n");
-		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-					      false, false);
-		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-					      false, 0x18);
-
-		commom = true;
-	} else if (!wifi_connected &&
-		   (BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE !=
-		    coex_dm->bt_status)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			("[BTCoex], Wifi non connected-idle + BT Busy!!\n"));
-		halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-		halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-					      false, false);
-		halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-					      false, 0x18);
-
-		commom = true;
-	} else {
-		if (wifi_busy)
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Wifi Connected-Busy"
-				  " + BT Busy!!\n");
-		else
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Wifi Connected-Idle"
-				  " + BT Busy!!\n");
-
-		commom = false;
-	}
-
-	return commom;
-}
-
-
-static void halbtc8723b1ant_tdma_duration_adjust_for_acl(
-						struct btc_coexist *btcoexist,
-						u8 wifi_status)
-{
-	static s32 up, dn, m, n, wait_count;
-	/* 0: no change, +1: increase WiFi duration,
-	 * -1: decrease WiFi duration */
-	s32 result;
-	u8 retry_count = 0, bt_info_ext;
-	static bool pre_wifi_busy = false;
-	bool wifi_busy = false;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], TdmaDurationAdjustForAcl()\n");
-
-	if (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY == wifi_status)
-		wifi_busy = true;
-	else
-		wifi_busy = false;
-
-	if ((BT_8723B_1ANT_WIFI_STATUS_NON_CONNECTED_ASSO_AUTH_SCAN ==
-							 wifi_status) ||
-	    (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN == wifi_status) ||
-	    (BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SPECIAL_PKT == wifi_status)) {
-		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
-		    coex_dm->cur_ps_tdma != 3 && coex_dm->cur_ps_tdma != 9) {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 9);
-			coex_dm->ps_tdma_du_adj_type = 9;
-
-			up = 0;
-			dn = 0;
-			m = 1;
-			n = 3;
-			result = 0;
-			wait_count = 0;
-		}
-		return;
-	}
-
-	if (!coex_dm->auto_tdma_adjust) {
-		coex_dm->auto_tdma_adjust = true;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
-
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 2);
-		coex_dm->ps_tdma_du_adj_type = 2;
-
-		up = 0;
-		dn = 0;
-		m = 1;
-		n = 3;
-		result = 0;
-		wait_count = 0;
-	} else {
-		/*acquire the BT TRx retry count from BT_Info byte2 */
-		retry_count = coex_sta->bt_retry_cnt;
-		bt_info_ext = coex_sta->bt_info_ext;
-		result = 0;
-		wait_count++;
-		/* no retry in the last 2-second duration */
-		if (retry_count == 0) {
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0;
-
-			if (up >= n) {
-				wait_count = 0;
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], Increase wifi "
-					  "duration!!\n");
-			}
-		} else if (retry_count <= 3) {
-			up--;
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2) {
-				if (wait_count <= 2)
-					m++;
-				else
-					m = 1;
-
-				if (m >= 20)
-					m = 20;
-
-				n = 3 * m;
-				up = 0;
-				dn = 0;
-				wait_count = 0;
-				result = -1;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], Decrease wifi duration"
-					  " for retryCounter<3!!\n");
-			}
-		} else {
-			if (wait_count == 1)
-				m++;
-			else
-				m = 1;
-
-			if (m >= 20)
-				m = 20;
-
-			n = 3 * m;
-			up = 0;
-			dn = 0;
-			wait_count = 0;
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], Decrease wifi duration"
-				  " for retryCounter>3!!\n");
-		}
-
-		if (result == -1) {
-			if ((BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
-			    ((coex_dm->cur_ps_tdma == 1) ||
-			     (coex_dm->cur_ps_tdma == 2))) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			} else if (coex_dm->cur_ps_tdma == 1) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
-			} else if (coex_dm->cur_ps_tdma == 2) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			} else if (coex_dm->cur_ps_tdma == 9) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 11);
-				coex_dm->ps_tdma_du_adj_type = 11;
-			}
-		} else if(result == 1) {
-			if ((BT_INFO_8723B_1ANT_A2DP_BASIC_RATE(bt_info_ext)) &&
-			    ((coex_dm->cur_ps_tdma == 1) ||
-			     (coex_dm->cur_ps_tdma == 2))) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			} else if (coex_dm->cur_ps_tdma == 11) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 9);
-				coex_dm->ps_tdma_du_adj_type = 9;
-			} else if (coex_dm->cur_ps_tdma == 9) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 2);
-				coex_dm->ps_tdma_du_adj_type = 2;
-			} else if (coex_dm->cur_ps_tdma == 2) {
-				halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 1);
-				coex_dm->ps_tdma_du_adj_type = 1;
-			}
-		} else {	  /*no change */
-			/*if busy / idle change */
-			if (wifi_busy != pre_wifi_busy) {
-				pre_wifi_busy = wifi_busy;
-				halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC,
-						        true,
-						        coex_dm->cur_ps_tdma);
-			}
-
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex],********* TDMA(on, %d) ********\n",
-				  coex_dm->cur_ps_tdma);
-		}
-
-		if (coex_dm->cur_ps_tdma != 1 && coex_dm->cur_ps_tdma != 2 &&
-		    coex_dm->cur_ps_tdma != 9 && coex_dm->cur_ps_tdma != 11) {
-			/* recover to previous adjust type */
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
-						coex_dm->ps_tdma_du_adj_type);
-		}
-	}
-}
-
-static void halbtc8723b1ant_PsTdmaCheckForPowerSaveState(
-			struct btc_coexist *btcoexist, bool new_ps_state)
-{
-	u8 lps_mode = 0x0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_LPS_MODE, &lps_mode);
-
-	if (lps_mode) {	/* already under LPS state */
-		if (new_ps_state) {
-			/* keep state under LPS, do nothing. */
-		} else {
-			/* will leave LPS state, turn off psTdma first */
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						false, 0);
-		}
-	} else {	/* NO PS state */
-		if (new_ps_state) {
-			/* will enter LPS state, turn off psTdma first */
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						false, 0);
-		} else {
-			/* keep state under NO PS state, do nothing. */
-		}
-	}
-}
-
-static void halbtc8723b1ant_power_save_state(struct btc_coexist *btcoexist,
-				      u8 ps_type, u8 lps_val,
-				      u8 rpwm_val)
-{
-	bool low_pwr_disable = false;
-
-	switch (ps_type) {
-	case BTC_PS_WIFI_NATIVE:
-		/* recover to original 32k low power setting */
-		low_pwr_disable = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-				   &low_pwr_disable);
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS, NULL);
-		break;
-	case BTC_PS_LPS_ON:
-		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(btcoexist, true);
-		halbtc8723b1ant_LpsRpwm(btcoexist, NORMAL_EXEC, lps_val,
-					rpwm_val);
-		/* when coex force to enter LPS, do not enter 32k low power. */
-		low_pwr_disable = true;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-				   &low_pwr_disable);
-		/* power save must executed before psTdma.	 */
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_ENTER_LPS, NULL);
-		break;
-	case BTC_PS_LPS_OFF:
-		halbtc8723b1ant_PsTdmaCheckForPowerSaveState(btcoexist, false);
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS, NULL);
-		break;
-	default:
-		break;
-	}
-}
-
-#if (BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
-static void halbtc8723b1ant_action_wifi_only(struct btc_coexist *btcoexist)
-{
-	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 9);
-}
-
-static void halbtc8723b1ant_monitor_bt_enable_disable(
-						struct btc_coexist *btcoexist)
-{
-	static bool pre_bt_disabled = false;
-	static u32 bt_disable_cnt = 0;
-	bool bt_active = true, bt_disabled = false;
-
-	/* This function check if bt is disabled */
-
-	if (coex_sta->high_priority_tx == 0 &&
-	    coex_sta->high_priority_rx == 0 &&
-	    coex_sta->low_priority_tx == 0 &&
-	    coex_sta->low_priority_rx == 0)
-		bt_active = false;
-
-	if (coex_sta->high_priority_tx == 0xffff &&
-	    coex_sta->high_priority_rx == 0xffff &&
-	    coex_sta->low_priority_tx == 0xffff &&
-	    coex_sta->low_priority_rx == 0xffff)
-		bt_active = false;
-
-	if (bt_active) {
-		bt_disable_cnt = 0;
-		bt_disabled = false;
- 		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
-				   &bt_disabled);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], BT is enabled !!\n");
-	} else {
-		bt_disable_cnt++;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], bt all counters=0, %d times!!\n",
-			  bt_disable_cnt);
-		if (bt_disable_cnt >= 2) {
-			bt_disabled = true;
-			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
-					   &bt_disabled);
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-				  "[BTCoex], BT is disabled !!\n");
-			halbtc8723b1ant_action_wifi_only(btcoexist);
-		}
-	}
-	if (pre_bt_disabled != bt_disabled) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], BT is from %s to %s!!\n",
-			  (pre_bt_disabled ? "disabled" : "enabled"),
-			  (bt_disabled ? "disabled" : "enabled"));
-		pre_bt_disabled = bt_disabled;
-		if (!bt_disabled) {
-		} else {
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_LEAVE_LPS,
-					   NULL);
-			btcoexist->btc_set(btcoexist, BTC_SET_ACT_NORMAL_LPS,
-					   NULL);
-		}
-	}
-}
-#endif
-
-/***************************************************
- *
- *	Software Coex Mechanism start
- *
- ***************************************************/
-/* SCO only or SCO+PAN(HS) */
-static void halbtc8723b1ant_action_sco(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state =
-		halbtc8723b1ant_wifi_rssi_state(btcoexist, 0, 2, 25, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-			  			      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-			  			      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-			  			      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-			  			      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-
-static void halbtc8723b1ant_action_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS) */
-static void halbtc8723b1ant_action_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b1ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
- 			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b1ant_action_pan_edr(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-
-/* PAN(HS) only */
-static void halbtc8723b1ant_action_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* fw mechanism */
-		if((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-		else
-			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* fw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-		else
-			halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*PAN(EDR)+A2DP */
-static void halbtc8723b1ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b1ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/* HID+A2DP+PAN(EDR) */
-static void halbtc8723b1ant_action_hid_a2dp_pan_edr(
-						struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		   (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b1ant_action_hid_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state, bt_info_ext;
-	u32 wifi_bw;
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	wifi_rssi_state = halbtc8723b1ant_wifi_rssi_state(btcoexist,
-							  0, 2, 25, 0);
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 50, 0);
-
-	if (halbtc8723b1ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-	else
-		halbtc8723b1ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			(wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		/* sw mechanism */
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b1ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b1ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*****************************************************
- *
- *	Non-Software Coex Mechanism start
- *
- *****************************************************/
-static void halbtc8723b1ant_action_hs(struct btc_coexist *btcoexist)
-{
-	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 2);
-}
-
-static void halbtc8723b1ant_action_bt_inquiry(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool wifi_connected = false, ap_enable = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-			   &ap_enable);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-
-	if (!wifi_connected) {
-		halbtc8723b1ant_power_save_state(btcoexist,
-						 BTC_PS_WIFI_NATIVE, 0x0, 0x0);
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	} else if (bt_link_info->sco_exist || bt_link_info->hid_only) {
-		/* SCO/HID-only busy */
-		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-						 0x0, 0x0);
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 32);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	} else {
-		if (ap_enable)
-			halbtc8723b1ant_power_save_state(btcoexist,
-							 BTC_PS_WIFI_NATIVE,
-							 0x0, 0x0);
-		else
-			halbtc8723b1ant_power_save_state(btcoexist,
-							 BTC_PS_LPS_ON,
-							 0x50, 0x4);
-
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 30);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	}
-}
-
-static void halbtc8723b1ant_action_bt_sco_hid_only_busy(
-						struct btc_coexist *btcoexist,
-						u8 wifi_status)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool wifi_connected = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-
-	/* tdma and coex table */
-
-	if (bt_link_info->sco_exist) {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-	} else { /* HID */
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 6);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 5);
-	}
-}
-
-static void halbtc8723b1ant_action_wifi_connected_bt_acl_busy(
-					struct btc_coexist *btcoexist,
-					u8 wifi_status)
-{
-	u8 bt_rssi_state;
-
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bt_rssi_state = halbtc8723b1ant_bt_rssi_state(2, 28, 0);
-
-	if (bt_link_info->hid_only) {  /*HID */
-		halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
-							    wifi_status);
-		coex_dm->auto_tdma_adjust = false;
-		return;
-	} else if (bt_link_info->a2dp_only) { /*A2DP */
-		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
- 			 halbtc8723b1ant_tdma_duration_adjust_for_acl(btcoexist,
-			 					   wifi_status);
-		} else { /*for low BT RSSI */
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 11);
-			coex_dm->auto_tdma_adjust = false;
-		}
-
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	} else if (bt_link_info->hid_exist &&
-			bt_link_info->a2dp_exist) { /*HID+A2DP */
-		if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 14);
-			coex_dm->auto_tdma_adjust = false;
-		} else { /*for low BT RSSI*/
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 14);
-			coex_dm->auto_tdma_adjust = false;
-		}
-
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
-	 /*PAN(OPP,FTP), HID+PAN(OPP,FTP) */
-	} else if (bt_link_info->pan_only ||
-		   (bt_link_info->hid_exist && bt_link_info->pan_exist)) {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 6);
-		coex_dm->auto_tdma_adjust = false;
-	 /*A2DP+PAN(OPP,FTP), HID+A2DP+PAN(OPP,FTP)*/
-	} else if ((bt_link_info->a2dp_exist && bt_link_info->pan_exist) ||
-		   (bt_link_info->hid_exist && bt_link_info->a2dp_exist &&
-		    bt_link_info->pan_exist)) {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-		coex_dm->auto_tdma_adjust = false;
-	} else {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 11);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-		coex_dm->auto_tdma_adjust = false;
-	}
-}
-
-static void halbtc8723b1ant_action_wifi_not_connected(
-						struct btc_coexist *btcoexist)
-{
-	/* power save state */
-	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-					 0x0, 0x0);
-
-	/* tdma and coex table */
-	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-}
-
-static void halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(
-						struct btc_coexist *btcoexist)
-{
-	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-					 0x0, 0x0);
-
-	halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 22);
-	halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-}
-
-static void halbtc8723b1ant_ActionWifiConnectedScan(
-						struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-
-	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-					 0x0, 0x0);
-
-	/* tdma and coex table */
-	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-		if (bt_link_info->a2dp_exist &&
-		    bt_link_info->pan_exist) {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 22);
-			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-	 	} else {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-		}
-	} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-		   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-		    coex_dm->bt_status)) {
-		halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
-				BT_8723B_1ANT_WIFI_STATUS_CONNECTED_SCAN);
-	} else {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	}
-}
-
-static void halbtc8723b1ant_action_wifi_connected_special_packet(
-						struct btc_coexist *btcoexist)
-{
-	bool hs_connecting = false;
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_CONNECTING, &hs_connecting);
-
-	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-					 0x0, 0x0);
-
-	/* tdma and coex table */
-	if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-		if (bt_link_info->a2dp_exist && bt_link_info->pan_exist) {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 22);
-			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-	 	} else {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						true, 20);
-			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 1);
-	 	}
-	} else {
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 20);
-		halbtc8723b1ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 1);
-	}
-}
-
-static void halbtc8723b1ant_action_wifi_connected(struct btc_coexist *btcoexist)
-{
-	bool wifi_busy = false;
-	bool scan = false, link = false, roam = false;
-	bool under_4way = false, ap_enable = false;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], CoexForWifiConnect()===>\n");
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
-			   &under_4way);
-	if (under_4way) {
-		halbtc8723b1ant_action_wifi_connected_special_packet(btcoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], CoexForWifiConnect(), "
-			  "return for wifi is under 4way<===\n");
-		return;
-	}
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-
-	if (scan || link || roam) {
-		halbtc8723b1ant_ActionWifiConnectedScan(btcoexist);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], CoexForWifiConnect(), "
-			  "return for wifi is under scan<===\n");
-		return;
-	}
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-			   &ap_enable);
-	/* power save state */
-	if (!ap_enable &&
-	    BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status &&
-	    !btcoexist->bt_link_info.hid_only)
-		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_LPS_ON,
-						 0x50, 0x4);
-	else
-		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-						 0x0, 0x0);
-
-	/* tdma and coex table */
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	if (!wifi_busy) {
-		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
-				      BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
-		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
-						coex_dm->bt_status) ||
-			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-			   			coex_dm->bt_status)) {
-			halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
-				     BT_8723B_1ANT_WIFI_STATUS_CONNECTED_IDLE);
-		} else {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC,
-						false, 8);
-			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 2);
-		}
-	} else {
-		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) {
-			halbtc8723b1ant_action_wifi_connected_bt_acl_busy(btcoexist,
-				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
-		} else if ((BT_8723B_1ANT_BT_STATUS_SCO_BUSY ==
-						coex_dm->bt_status) ||
-			   (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY ==
-			   			coex_dm->bt_status)) {
-			halbtc8723b1ant_action_bt_sco_hid_only_busy(btcoexist,
-				    BT_8723B_1ANT_WIFI_STATUS_CONNECTED_BUSY);
-		} else {
-			halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 8);
-			halbtc8723b1ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 2);
-		}
-	}
-}
-
-static void halbtc8723b1ant_run_sw_coexist_mechanism(
-						struct btc_coexist *btcoexist)
-{
-	u8 algorithm = 0;
-
-	algorithm = halbtc8723b1ant_action_algorithm(btcoexist);
-	coex_dm->cur_algorithm = algorithm;
-
-	if (halbtc8723b1ant_is_common_action(btcoexist)) {
-	} else {
-		switch (coex_dm->cur_algorithm) {
-		case BT_8723B_1ANT_COEX_ALGO_SCO:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = SCO.\n");
-			halbtc8723b1ant_action_sco(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = HID.\n");
-			halbtc8723b1ant_action_hid(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = A2DP.\n");
-			halbtc8723b1ant_action_a2dp(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_A2DP_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = "
-				  "A2DP+PAN(HS).\n");
-			halbtc8723b1ant_action_a2dp_pan_hs(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = PAN(EDR).\n");
-			halbtc8723b1ant_action_pan_edr(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = HS mode.\n");
-			halbtc8723b1ant_action_pan_hs(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_PANEDR_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = PAN+A2DP.\n");
-			halbtc8723b1ant_action_pan_edr_a2dp(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_PANEDR_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = "
-				  "PAN(EDR)+HID.\n");
-			halbtc8723b1ant_action_pan_edr_hid(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = "
-				  "HID+A2DP+PAN.\n");
-			halbtc8723b1ant_action_hid_a2dp_pan_edr(btcoexist);
-			break;
-		case BT_8723B_1ANT_COEX_ALGO_HID_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = HID+A2DP.\n");
-			halbtc8723b1ant_action_hid_a2dp(btcoexist);
-			break;
-		default:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action algorithm = "
-				  "coexist All Off!!\n");
-			break;
-		}
-		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
-	}
-}
-
-static void halbtc8723b1ant_run_coexist_mechanism(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool wifi_connected = false, bt_hs_on = false;
-	bool limited_dig = false, bIncreaseScanDevNum = false;
-	bool b_bt_ctrl_agg_buf_size = false;
-	u8 agg_buf_size = 5;
-	u8 wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], RunCoexistMechanism()===>\n");
-
-	if (btcoexist->manual_control) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], RunCoexistMechanism(), "
-			  "return for Manual CTRL <===\n");
-		return;
-	}
-
-	if (btcoexist->stop_coex_dm) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], RunCoexistMechanism(), "
-			  "return for Stop Coex DM <===\n");
-		return;
-	}
-
-	if (coex_sta->under_ips) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], wifi is under IPS !!!\n");
-		return;
-	}
-
-	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
-		limited_dig = true;
-		bIncreaseScanDevNum = true;
-	}
-
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_INC_SCAN_DEV_NUM,
-			   &bIncreaseScanDevNum);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-
-	if (!bt_link_info->sco_exist && !bt_link_info->hid_exist) {
-		halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC, 0, 0, 0, 0);
-	} else {
-		if (wifi_connected) {
-			wifi_rssi_state =
-				halbtc8723b1ant_wifi_rssi_state(btcoexist,
-								1, 2, 30, 0);
-			if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-				halbtc8723b1ant_limited_tx(btcoexist,
-							   NORMAL_EXEC,
-							   1, 1, 1, 1);
-			} else {
-				halbtc8723b1ant_limited_tx(btcoexist,
-							   NORMAL_EXEC,
-							   1, 1, 1, 1);
-			}
-		} else {
-			halbtc8723b1ant_limited_tx(btcoexist, NORMAL_EXEC,
-						   0, 0, 0, 0);
-		}
-	}
-
-	if (bt_link_info->sco_exist) {
-		b_bt_ctrl_agg_buf_size = true;
-		agg_buf_size = 0x3;
-	} else if (bt_link_info->hid_exist) {
-		b_bt_ctrl_agg_buf_size = true;
-		agg_buf_size = 0x5;
- 	} else if (bt_link_info->a2dp_exist || bt_link_info->pan_exist) {
-		b_bt_ctrl_agg_buf_size = true;
-		agg_buf_size = 0x8;
-	}
-	halbtc8723b1ant_limited_rx(btcoexist, NORMAL_EXEC, false,
-				   b_bt_ctrl_agg_buf_size, agg_buf_size);
-
-	halbtc8723b1ant_run_sw_coexist_mechanism(btcoexist);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-
-	if (coex_sta->c2h_bt_inquiry_page) {
-		halbtc8723b1ant_action_bt_inquiry(btcoexist);
-		return;
-	} else if (bt_hs_on) {
-		halbtc8723b1ant_action_hs(btcoexist);
-		return;
-	}
-
-
-	if (!wifi_connected) {
-		bool scan = false, link = false, roam = false;
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], wifi is non connected-idle !!!\n");
-
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-
-		if (scan || link || roam)
-			halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(btcoexist);
-		else
-			halbtc8723b1ant_action_wifi_not_connected(btcoexist);
-	} else { /* wifi LPS/Busy */
-		halbtc8723b1ant_action_wifi_connected(btcoexist);
-	}
-}
-
-static void halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	/* force to reset coex mechanism */
-	halbtc8723b1ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
-	halbtc8723b1ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
-
-	/* sw all off */
-	halbtc8723b1ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b1ant_sw_mechanism2(btcoexist,false, false, false, 0x18);
-
-	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 8);
-	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
-}
-
-static void halbtc8723b1ant_init_hw_config(struct btc_coexist *btcoexist,
-					bool backup)
-{
-	u32 u32tmp = 0;
-	u8 u8tmp = 0;
-	u32 cnt_bt_cal_chk = 0;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], 1Ant Init HW Config!!\n");
-
-	if (backup) {/* backup rf 0x1e value */
-		coex_dm->bt_rf0x1e_backup =
-			btcoexist->btc_get_rf_reg(btcoexist,
-						  BTC_RF_A, 0x1e, 0xfffff);
-
-		coex_dm->backup_arfr_cnt1 =
-			btcoexist->btc_read_4byte(btcoexist, 0x430);
-		coex_dm->backup_arfr_cnt2 =
-			btcoexist->btc_read_4byte(btcoexist, 0x434);
-		coex_dm->backup_retry_limit =
-			btcoexist->btc_read_2byte(btcoexist, 0x42a);
-		coex_dm->backup_ampdu_max_time =
-			btcoexist->btc_read_1byte(btcoexist, 0x456);
-	}
-
-	/* WiFi goto standby while GNT_BT 0-->1 */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x780);
-	/* BT goto standby while GNT_BT 1-->0 */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x2, 0xfffff, 0x500);
-
-	btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
-	btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
-
-
-	/* BT calibration check */
-	while (cnt_bt_cal_chk <= 20) {
-		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x49d);
-		cnt_bt_cal_chk++;
-		if (u32tmp & BIT0) {
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-				  "[BTCoex], ########### BT "
-				  "calibration(cnt=%d) ###########\n",
-				  cnt_bt_cal_chk);
-			mdelay(50);
-		} else {
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-				  "[BTCoex], ********** BT NOT "
-				  "calibration (cnt=%d)**********\n",
-				  cnt_bt_cal_chk);
-			break;
-		}
-	}
-
-	/* 0x790[5:0]=0x5 */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
-	u8tmp &= 0xc0;
-	u8tmp |= 0x5;
-	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
-
-	/* Enable counter statistics */
-	/*0x76e[3] =1, WLAN_Act control by PTA */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x1);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
-
-	/*Antenna config */
-	halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_PTA, true, false);
-	/* PTA parameter */
-	halbtc8723b1ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
-
-}
-
-static void halbtc8723b1ant_wifi_off_hw_cfg(struct btc_coexist *btcoexist)
-{
-	/* set wlan_act to low */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0);
-}
-
-/**************************************************************
- * work around function start with wa_halbtc8723b1ant_
- **************************************************************/
-/**************************************************************
- * extern function start with EXhalbtc8723b1ant_
- **************************************************************/
-
-void ex_halbtc8723b1ant_init_hwconfig(struct btc_coexist *btcoexist)
-{
-	halbtc8723b1ant_init_hw_config(btcoexist, true);
-}
-
-void ex_halbtc8723b1ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], Coex Mechanism Init!!\n");
-
-	btcoexist->stop_coex_dm = false;
-
-	halbtc8723b1ant_init_coex_dm(btcoexist);
-
-	halbtc8723b1ant_query_bt_info(btcoexist);
-}
-
-void ex_halbtc8723b1ant_display_coex_info(struct btc_coexist *btcoexist)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	u8 *cli_buf = btcoexist->cli_buf;
-	u8 u8tmp[4], i, bt_info_ext, psTdmaCase=0;
-	u16 u16tmp[4];
-	u32 u32tmp[4];
-	bool roam = false, scan = false;
-	bool link = false, wifi_under_5g = false;
-	bool bt_hs_on = false, wifi_busy = false;
-	s32 wifi_rssi =0, bt_hs_rssi = 0;
-	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
-	u8 wifi_dot11_chnl, wifi_hs_chnl;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cli_buf);
-
-	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ============[Under Manual Control]==========");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
-	}
-	if (btcoexist->stop_coex_dm) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ============[Coex is STOPPED]============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
-	}
-
-	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
-		return;
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
-		   "Ant PG Num/ Ant Mech/ Ant Pos:", \
-		   board_info->pg_ant_num, board_info->btdm_ant_num,
-		   board_info->btdm_ant_pos);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
-		   "BT stack/ hci ext ver", \
-		   ((stack_info->profile_notified)? "Yes":"No"),
-		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
-		   "CoexVer/ FwVer/ PatchVer", \
-		   glcoex_ver_date_8723b_1ant, glcoex_ver_8723b_1ant,
-		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
-			   &wifi_dot11_chnl);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
-		   "Dot11 channel / HsChnl(HsMode)", \
-		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
-		   "H2C Wifi inform bt chnl Info", \
-		   coex_dm->wifi_chnl_info[0], coex_dm->wifi_chnl_info[1],
-		   coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "Wifi rssi/ HS rssi", wifi_rssi, bt_hs_rssi);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
-		"Wifi link/ roam/ scan", link, roam, scan);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist,BTC_GET_BL_WIFI_UNDER_5G,
-			   &wifi_under_5g);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
-			   &wifi_traffic_dir);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
-		   "Wifi status", (wifi_under_5g? "5G":"2.4G"),
-		   ((BTC_WIFI_BW_LEGACY==wifi_bw)? "Legacy":
-			(((BTC_WIFI_BW_HT40==wifi_bw)? "HT40":"HT20"))),
-		   ((!wifi_busy)? "idle":
-			((BTC_WIFI_TRAFFIC_TX==wifi_traffic_dir)?
-				"uplink":"downlink")));
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = [%s/ %d/ %d] ",
-		"BT [status/ rssi/ retryCnt]",
-		((btcoexist->bt_info.bt_disabled)? ("disabled"):
-		  ((coex_sta->c2h_bt_inquiry_page)?("inquiry/page scan"):
-		    ((BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE == coex_dm->bt_status)?
-		      "non-connected idle":
-			((BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE == coex_dm->bt_status)?
-			  "connected-idle":"busy")))),
-			    coex_sta->bt_rssi, coex_sta->bt_retry_cnt);
-	CL_PRINTF(cli_buf);
-
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
-		"SCO/HID/PAN/A2DP", bt_link_info->sco_exist,
-		bt_link_info->hid_exist, bt_link_info->pan_exist,
-		bt_link_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
-		   "BT Info A2DP rate",
-		   (bt_info_ext & BIT0) ? "Basic rate" : "EDR rate");
-	CL_PRINTF(cli_buf);
-
-	for (i = 0; i < BT_INFO_SRC_8723B_1ANT_MAX; i++) {
-		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-				   "\r\n %-35s = %02x %02x %02x "
-				   "%02x %02x %02x %02x(%d)",
-				   GLBtInfoSrc8723b1Ant[i],
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3],
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
-				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
-		}
-	}
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %s/%s, (0x%x/0x%x)",
-		   "PS state, IPS/LPS, (lps/rpwm)", \
-		   ((coex_sta->under_ips? "IPS ON":"IPS OFF")),
-		   ((coex_sta->under_lps? "LPS ON":"LPS OFF")),
-		   btcoexist->bt_info.lps_1ant,
-		   btcoexist->bt_info.rpwm_1ant);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
-
-	if (!btcoexist->manual_control) {
-		/* Sw mechanism	*/
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-			   "============[Sw mechanism]============");
-		CL_PRINTF(cli_buf);
-
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
- 			   "SM1[ShRf/ LpRA/ LimDig]", \
- 			   coex_dm->cur_rf_rx_lpf_shrink,
-			   coex_dm->cur_low_penalty_ra,
-			   btcoexist->bt_info.limited_dig);
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n %-35s = %d/ %d/ %d(0x%x) ",
-			   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]", \
-			   coex_dm->cur_agc_table_en,
-			   coex_dm->cur_adc_backoff,
-			   coex_dm->cur_dac_swing_on,
-			   coex_dm->cur_dac_swing_lvl);
-		CL_PRINTF(cli_buf);
-
-
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
-			   "Rate Mask", btcoexist->bt_info.ra_mask);
-		CL_PRINTF(cli_buf);
-
-		/* Fw mechanism	*/
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-			   "============[Fw mechanism]============");
-		CL_PRINTF(cli_buf);
-
-		psTdmaCase = coex_dm->cur_ps_tdma;
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n %-35s = %02x %02x %02x %02x %02x "
-			   "case-%d (auto:%d)",
-			   "PS TDMA", coex_dm->ps_tdma_para[0],
-			   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
-			   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
-			   psTdmaCase, coex_dm->auto_tdma_adjust);
-		CL_PRINTF(cli_buf);
-
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x ",
-			   "Latest error condition(should be 0)", \
-			   coex_dm->error_condition);
-		CL_PRINTF(cli_buf);
-
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
-			   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
-			   coex_dm->cur_ignore_wlan_act);
-		CL_PRINTF(cli_buf);
-	}
-
-	/* Hw setting */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Hw setting]============");
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
-		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
-		   "backup ARFR1/ARFR2/RL/AMaxTime", coex_dm->backup_arfr_cnt1,
-		   coex_dm->backup_arfr_cnt2, coex_dm->backup_retry_limit,
-		   coex_dm->backup_ampdu_max_time);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x430);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x434);
-	u16tmp[0] = btcoexist->btc_read_2byte(btcoexist, 0x42a);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x456);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/0x%x/0x%x/0x%x",
-		   "0x430/0x434/0x42a/0x456",
-		   u32tmp[0], u32tmp[1], u16tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6cc);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x880);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x778/0x6cc/0x880[29:25]", u8tmp[0], u32tmp[0],
-		   (u32tmp[1] & 0x3e000000) >> 25);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x948/ 0x67[5] / 0x765",
-		   u32tmp[0], ((u8tmp[0] & 0x20)>> 5), u8tmp[1]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
-		   u32tmp[0] & 0x3, u32tmp[1] & 0xff, u32tmp[2] & 0x3);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
-		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
-		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
-		   ((u32tmp[0] & 0x01800000) >> 23), u8tmp[2] & 0x1);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0] & 0xff, u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
-	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
-
-	fa_ofdm = ((u32tmp[0] & 0xffff0000) >> 16) +
-		  ((u32tmp[1] & 0xffff0000) >> 16) +
-		   (u32tmp[1] & 0xffff) +
-		   (u32tmp[2] & 0xffff) + \
-		  ((u32tmp[3] & 0xffff0000) >> 16) +
-		   (u32tmp[3] & 0xffff) ;
-	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "OFDM-CCA/OFDM-FA/CCK-FA",
-		   u32tmp[0] & 0xffff, fa_ofdm, fa_cck);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x6c0/0x6c4/0x6c8(coexTable)",
-		   u32tmp[0], u32tmp[1], u32tmp[2]);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x770(high-pri rx/tx)", coex_sta->high_priority_rx,
-		   coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
-		   coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
-#if(BT_AUTO_REPORT_ONLY_8723B_1ANT == 1)
-	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
-#endif
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-
-void ex_halbtc8723b1ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
-{
-
-	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
-		return;
-
-	if (BTC_IPS_ENTER == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS ENTER notify\n");
-		coex_sta->under_ips = true;
-
-		halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT,
-					   false, true);
-                /* set PTA control */
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
-		halbtc8723b1ant_coex_table_with_type(btcoexist,
-			NORMAL_EXEC, 0);
-	} else if (BTC_IPS_LEAVE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS LEAVE notify\n");
-		coex_sta->under_ips = false;
-
-		halbtc8723b1ant_run_coexist_mechanism(btcoexist);
-	}
-}
-
-void ex_halbtc8723b1ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (btcoexist->manual_control || btcoexist->stop_coex_dm)
-		return;
-
-	if (BTC_LPS_ENABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS ENABLE notify\n");
-		coex_sta->under_lps = true;
-	} else if (BTC_LPS_DISABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS DISABLE notify\n");
-		coex_sta->under_lps = false;
-	}
-}
-
-void ex_halbtc8723b1ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	bool wifi_connected = false, bt_hs_on = false;
-
-	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
-		return;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-
-	halbtc8723b1ant_query_bt_info(btcoexist);
-
-	if (coex_sta->c2h_bt_inquiry_page) {
-		halbtc8723b1ant_action_bt_inquiry(btcoexist);
-		return;
-	} else if (bt_hs_on) {
-		halbtc8723b1ant_action_hs(btcoexist);
-		return;
-	}
-
-	if (BTC_SCAN_START == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN START notify\n");
-		if (!wifi_connected)	/* non-connected scan */
-			halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(btcoexist);
-		else	/* wifi is connected */
-			halbtc8723b1ant_ActionWifiConnectedScan(btcoexist);
-	} else if (BTC_SCAN_FINISH == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN FINISH notify\n");
-		if (!wifi_connected)	/* non-connected scan */
-			halbtc8723b1ant_action_wifi_not_connected(btcoexist);
-		else
-			halbtc8723b1ant_action_wifi_connected(btcoexist);
-	}
-}
-
-void ex_halbtc8723b1ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	bool wifi_connected = false, bt_hs_on = false;
-
-	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
-		return;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	if (coex_sta->c2h_bt_inquiry_page) {
-		halbtc8723b1ant_action_bt_inquiry(btcoexist);
-		return;
-	} else if (bt_hs_on) {
-		halbtc8723b1ant_action_hs(btcoexist);
-		return;
-	}
-
-	if (BTC_ASSOCIATE_START == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT START notify\n");
-		halbtc8723b1ant_action_wifi_not_connected_asso_auth_scan(btcoexist);
-	} else if (BTC_ASSOCIATE_FINISH == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT FINISH notify\n");
-
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-				   &wifi_connected);
-		if (!wifi_connected) /* non-connected scan */
-			halbtc8723b1ant_action_wifi_not_connected(btcoexist);
-		else
-			halbtc8723b1ant_action_wifi_connected(btcoexist);
-	}
-}
-
-void ex_halbtc8723b1ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type)
-{
-	u8 h2c_parameter[3] ={0};
-	u32 wifi_bw;
-	u8 wifiCentralChnl;
-
-	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled )
-		return;
-
-	if (BTC_MEDIA_CONNECT == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA connect notify\n");
-	else
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA disconnect notify\n");
-
-	/* only 2.4G we need to inform bt the chnl mask */
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_CENTRAL_CHNL,
-			   &wifiCentralChnl);
-
-	if ((BTC_MEDIA_CONNECT == type) &&
-	    (wifiCentralChnl <= 14)) {
-		h2c_parameter[0] = 0x0;
-		h2c_parameter[1] = wifiCentralChnl;
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			h2c_parameter[2] = 0x30;
-		else
-			h2c_parameter[2] = 0x20;
-	}
-
-	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
-	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
-	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x66=0x%x\n",
-		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
-		  h2c_parameter[2]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
-}
-
-void ex_halbtc8723b1ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type)
-{
-	bool bt_hs_on = false;
-
-	if (btcoexist->manual_control || btcoexist->stop_coex_dm ||
-	    btcoexist->bt_info.bt_disabled)
-		return;
-
-	coex_sta->special_pkt_period_cnt = 0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	if (coex_sta->c2h_bt_inquiry_page) {
-		halbtc8723b1ant_action_bt_inquiry(btcoexist);
-		return;
-	} else if (bt_hs_on) {
-		halbtc8723b1ant_action_hs(btcoexist);
-		return;
-	}
-
-	if (BTC_PACKET_DHCP == type ||
-		BTC_PACKET_EAPOL == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], special Packet(%d) notify\n", type);
-		halbtc8723b1ant_action_wifi_connected_special_packet(btcoexist);
-	}
-}
-
-void ex_halbtc8723b1ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmp_buf, u8 length)
-{
-	u8 bt_info = 0;
-	u8 i, rsp_source = 0;
-	bool wifi_connected = false;
-	bool bt_busy = false;
-
-	coex_sta->c2h_bt_info_req_sent = false;
-
-	rsp_source = tmp_buf[0] & 0xf;
-	if (rsp_source >= BT_INFO_SRC_8723B_1ANT_MAX)
-		rsp_source = BT_INFO_SRC_8723B_1ANT_WIFI_FW;
-	coex_sta->bt_info_c2h_cnt[rsp_source]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length=%d, hex data=[",
-		  rsp_source, length);
-	for (i=0; i<length; i++) {
-		coex_sta->bt_info_c2h[rsp_source][i] = tmp_buf[i];
-		if (i == 1)
-			bt_info = tmp_buf[i];
-		if (i == length - 1)
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x]\n", tmp_buf[i]);
-		else
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x, ", tmp_buf[i]);
-	}
-
-	if (BT_INFO_SRC_8723B_1ANT_WIFI_FW != rsp_source) {
-		coex_sta->bt_retry_cnt =	/* [3:0] */
-			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
-
-		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
-
-		coex_sta->bt_info_ext =
-			coex_sta->bt_info_c2h[rsp_source][4];
-
-		/* Here we need to resend some wifi info to BT
-		 * because bt is reset and loss of the info.*/
-		if(coex_sta->bt_info_ext & BIT1)
-		{
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], BT ext info bit1 check, "
-				  "send wifi BW&Chnl to BT!!\n");
-			btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-					   &wifi_connected);
-			if(wifi_connected)
-				ex_halbtc8723b1ant_media_status_notify(btcoexist,
-							     BTC_MEDIA_CONNECT);
-			else
-				ex_halbtc8723b1ant_media_status_notify(btcoexist,
-							  BTC_MEDIA_DISCONNECT);
-		}
-
-		if (coex_sta->bt_info_ext & BIT3) {
-			if (!btcoexist->manual_control &&
-			    !btcoexist->stop_coex_dm) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], BT ext info bit3 check, "
-					  "set BT NOT ignore Wlan active!!\n");
-				halbtc8723b1ant_ignore_wlan_act(btcoexist,
-								FORCE_EXEC,
-								false);
-			}
-		} else {
-			/* BT already NOT ignore Wlan active, do nothing here.*/
-		}
-#if(BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
-		if (coex_sta->bt_info_ext & BIT4) {
-			/* BT auto report already enabled, do nothing */
-		} else {
-			halbtc8723b1ant_bt_auto_report(btcoexist, FORCE_EXEC,
-						       true);
-		}
-#endif
-	}
-
-	/* check BIT2 first ==> check if bt is under inquiry or page scan */
-	if (bt_info & BT_INFO_8723B_1ANT_B_INQ_PAGE)
-		coex_sta->c2h_bt_inquiry_page = true;
-	else
-		coex_sta->c2h_bt_inquiry_page = false;
-
-	/* set link exist status */
-	if (!(bt_info & BT_INFO_8723B_1ANT_B_CONNECTION)) {
-		coex_sta->bt_link_exist = false;
-		coex_sta->pan_exist = false;
-		coex_sta->a2dp_exist = false;
-		coex_sta->hid_exist = false;
-		coex_sta->sco_exist = false;
-	} else { /* connection exists */
-		coex_sta->bt_link_exist = true;
-		if (bt_info & BT_INFO_8723B_1ANT_B_FTP)
-			coex_sta->pan_exist = true;
-		else
-			coex_sta->pan_exist = false;
-		if (bt_info & BT_INFO_8723B_1ANT_B_A2DP)
-			coex_sta->a2dp_exist = true;
-		else
-			coex_sta->a2dp_exist = false;
-		if (bt_info & BT_INFO_8723B_1ANT_B_HID)
-			coex_sta->hid_exist = true;
-		else
-			coex_sta->hid_exist = false;
-		if (bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO)
-			coex_sta->sco_exist = true;
-		else
-			coex_sta->sco_exist = false;
-	}
-
-	halbtc8723b1ant_update_bt_link_info(btcoexist);
-
-	if (!(bt_info&BT_INFO_8723B_1ANT_B_CONNECTION)) {
-		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_NON_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), "
-			  "BT Non-Connected idle!!!\n");
-	/* connection exists but no busy */
-	} else if (bt_info == BT_INFO_8723B_1ANT_B_CONNECTION) {
-		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
-	} else if ((bt_info & BT_INFO_8723B_1ANT_B_SCO_ESCO) ||
-		(bt_info & BT_INFO_8723B_1ANT_B_SCO_BUSY)) {
-		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_SCO_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), "
-			  "BT SCO busy!!!\n");
-	} else if (bt_info & BT_INFO_8723B_1ANT_B_ACL_BUSY) {
-		if (BT_8723B_1ANT_BT_STATUS_ACL_BUSY != coex_dm->bt_status)
-			coex_dm->auto_tdma_adjust = false;
-
-		coex_dm->bt_status = BT_8723B_1ANT_BT_STATUS_ACL_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
-	} else {
-		coex_dm->bt_status =
-			BT_8723B_1ANT_BT_STATUS_MAX;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT Non-Defined state!!\n");
-	}
-
-	if ((BT_8723B_1ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_1ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_1ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status))
-		bt_busy = true;
-	else
-		bt_busy = false;
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
-
-	halbtc8723b1ant_run_coexist_mechanism(btcoexist);
-}
-
-void ex_halbtc8723b1ant_halt_notify(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
-
-	btcoexist->stop_coex_dm = true;
-
-	halbtc8723b1ant_SetAntPath(btcoexist, BTC_ANT_PATH_BT, false, true);
-
-	halbtc8723b1ant_wifi_off_hw_cfg(btcoexist);
-	halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
-
-	halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-					 0x0, 0x0);
-	halbtc8723b1ant_ps_tdma(btcoexist, FORCE_EXEC, false, 0);
-
-	ex_halbtc8723b1ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
-}
-
-void ex_halbtc8723b1ant_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Pnp notify\n");
-
-	if (BTC_WIFI_PNP_SLEEP == pnp_state) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], Pnp notify to SLEEP\n");
-		btcoexist->stop_coex_dm = true;
-		halbtc8723b1ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
-		halbtc8723b1ant_power_save_state(btcoexist, BTC_PS_WIFI_NATIVE,
-						 0x0, 0x0);
-		halbtc8723b1ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 9);
-	} else if (BTC_WIFI_PNP_WAKE_UP == pnp_state) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], Pnp notify to WAKE UP\n");
-		btcoexist->stop_coex_dm = false;
-		halbtc8723b1ant_init_hw_config(btcoexist, false);
-		halbtc8723b1ant_init_coex_dm(btcoexist);
-		halbtc8723b1ant_query_bt_info(btcoexist);
-	}
-}
-
-void ex_halbtc8723b1ant_periodical(struct btc_coexist *btcoexist)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	static u8 dis_ver_info_cnt = 0;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], =========================="
-		  "Periodical===========================\n");
-
-	if (dis_ver_info_cnt <= 5) {
-		dis_ver_info_cnt += 1;
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], *************************"
-			  "***************************************\n");
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], Ant PG Num/ Ant Mech/ "
-			  "Ant Pos = %d/ %d/ %d\n", \
-			  board_info->pg_ant_num, board_info->btdm_ant_num,
-			  board_info->btdm_ant_pos);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n", \
-			  ((stack_info->profile_notified)? "Yes":"No"),
-			  stack_info->hci_version);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
-				   &bt_patch_ver);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], CoexVer/ FwVer/ PatchVer "
-			  "= %d_%x/ 0x%x/ 0x%x(%d)\n", \
-			  glcoex_ver_date_8723b_1ant,
-			  glcoex_ver_8723b_1ant, fw_ver,
-			  bt_patch_ver, bt_patch_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], *****************************"
-			  "***********************************\n");
-	}
-
-#if(BT_AUTO_REPORT_ONLY_8723B_1ANT == 0)
-	halbtc8723b1ant_query_bt_info(btcoexist);
-	halbtc8723b1ant_monitor_bt_ctr(btcoexist);
-	halbtc8723b1ant_monitor_bt_enable_disable(btcoexist);
-#else
-	if (halbtc8723b1ant_is_wifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust) {
-		if (coex_sta->special_pkt_period_cnt > 2)
-			halbtc8723b1ant_run_coexist_mechanism(btcoexist);
-	}
-
-	coex_sta->special_pkt_period_cnt++;
-#endif
-}
-
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
deleted file mode 100644
index da3f62da4e40..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.c
+++ /dev/null
@@ -1,4200 +0,0 @@
-/***************************************************************
- * Description:
- *
- * This file is for RTL8723B Co-exist mechanism
- *
- * History
- * 2012/11/15 Cosa first check in.
- *
- **************************************************************/
-/**************************************************************
- * include files
- **************************************************************/
-#include "halbt_precomp.h"
-#if 1
-/**************************************************************
- * Global variables, these are static variables
- **************************************************************/
-static struct coex_dm_8723b_2ant glcoex_dm_8723b_2ant;
-static struct coex_dm_8723b_2ant *coex_dm = &glcoex_dm_8723b_2ant;
-static struct coex_sta_8723b_2ant glcoex_sta_8723b_2ant;
-static struct coex_sta_8723b_2ant *coex_sta = &glcoex_sta_8723b_2ant;
-
-static const char *const glbt_info_src_8723b_2ant[] = {
-	"BT Info[wifi fw]",
-	"BT Info[bt rsp]",
-	"BT Info[bt auto report]",
-};
-
-static u32 glcoex_ver_date_8723b_2ant = 20131113;
-static u32 glcoex_ver_8723b_2ant = 0x3f;
-
-/**************************************************************
- * local function proto type if needed
- **************************************************************/
-/**************************************************************
- * local function start with halbtc8723b2ant_
- **************************************************************/
-static u8 halbtc8723b2ant_bt_rssi_state(u8 level_num, u8 rssi_thresh,
-					u8 rssi_thresh1)
-{
-	s32 bt_rssi = 0;
-	u8 bt_rssi_state = coex_sta->pre_bt_rssi_state;
-
-	bt_rssi = coex_sta->bt_rssi;
-
-	if (level_num == 2) {
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= rssi_thresh +
-				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to High\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at Low\n");
-			}
-		} else {
-			if (bt_rssi < rssi_thresh) {
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Low\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-				  "[BTCoex], BT Rssi thresh error!!\n");
-			return coex_sta->pre_bt_rssi_state;
-		}
-
-		if ((coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_bt_rssi_state == BTC_RSSI_STATE_STAY_LOW)) {
-			if (bt_rssi >= rssi_thresh +
-				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Medium\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at Low\n");
-			}
-		} else if ((coex_sta->pre_bt_rssi_state ==
-						BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_bt_rssi_state ==
-			   			BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (bt_rssi >= rssi_thresh1 +
-				       BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				bt_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to High\n");
-			} else if (bt_rssi < rssi_thresh) {
-				bt_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Low\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at Medium\n");
-			}
-		} else {
-			if (bt_rssi < rssi_thresh1) {
-				bt_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "switch to Medium\n");
-			} else {
-				bt_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_RSSI_STATE,
-					  "[BTCoex], BT Rssi state "
-					  "stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_bt_rssi_state = bt_rssi_state;
-
-	return bt_rssi_state;
-}
-
-static u8 halbtc8723b2ant_wifi_rssi_state(struct btc_coexist *btcoexist,
-				   u8 index, u8 level_num,
-				   u8 rssi_thresh, u8 rssi_thresh1)
-{
-	s32 wifi_rssi=0;
-	u8 wifi_rssi_state = coex_sta->pre_wifi_rssi_state[index];
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-
-	if (level_num == 2) {
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-		    				BTC_RSSI_STATE_STAY_LOW)) {
-			if (wifi_rssi >= rssi_thresh +
-					 BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to High\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at Low\n");
-			}
-		} else {
-			if (wifi_rssi < rssi_thresh) {
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Low\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at High\n");
-			}
-		}
-	} else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_WIFI_RSSI_STATE,
-				  "[BTCoex], wifi RSSI thresh error!!\n");
-			return coex_sta->pre_wifi_rssi_state[index];
-		}
-
-		if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_LOW) ||
-		    (coex_sta->pre_wifi_rssi_state[index] ==
-		    				BTC_RSSI_STATE_STAY_LOW)) {
-			if(wifi_rssi >= rssi_thresh +
-					BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Medium\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at Low\n");
-			}
-		} else if ((coex_sta->pre_wifi_rssi_state[index] ==
-						BTC_RSSI_STATE_MEDIUM) ||
-			   (coex_sta->pre_wifi_rssi_state[index] ==
-			   			BTC_RSSI_STATE_STAY_MEDIUM)) {
-			if (wifi_rssi >= rssi_thresh1 +
-					 BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT) {
-				wifi_rssi_state = BTC_RSSI_STATE_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to High\n");
-			} else if (wifi_rssi < rssi_thresh) {
-				wifi_rssi_state = BTC_RSSI_STATE_LOW;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Low\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at Medium\n");
-			}
-		} else {
-			if (wifi_rssi < rssi_thresh1) {
-				wifi_rssi_state = BTC_RSSI_STATE_MEDIUM;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "switch to Medium\n");
-			} else {
-				wifi_rssi_state = BTC_RSSI_STATE_STAY_HIGH;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_WIFI_RSSI_STATE,
-					  "[BTCoex], wifi RSSI state "
-					  "stay at High\n");
-			}
-		}
-	}
-
-	coex_sta->pre_wifi_rssi_state[index] = wifi_rssi_state;
-
-	return wifi_rssi_state;
-}
-
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-static void halbtc8723b2ant_monitor_bt_enable_disable(
-						struct btc_coexist *btcoexist)
-{
-	static bool pre_bt_disabled = false;
-	static u32 bt_disable_cnt = 0;
-	bool bt_active = true, bt_disabled = false;
-
-	/* This function check if bt is disabled */
-	if (coex_sta->high_priority_tx == 0 &&
-	    coex_sta->high_priority_rx == 0 &&
-	    coex_sta->low_priority_tx == 0 &&
-	    coex_sta->low_priority_rx == 0)
-		bt_active = false;
-
-	if (coex_sta->high_priority_tx == 0xffff &&
-	    coex_sta->high_priority_rx == 0xffff &&
-	    coex_sta->low_priority_tx == 0xffff &&
-	    coex_sta->low_priority_rx == 0xffff)
-		bt_active = true;
-
-	if (bt_active) {
-		bt_disable_cnt = 0;
-		bt_disabled = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
-				   &bt_disabled);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], BT is enabled !!\n");
-	} else {
-		bt_disable_cnt++;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], bt all counters=0, %d times!!\n",
-			  bt_disable_cnt);
-		if (bt_disable_cnt >= 2) {
-			bt_disabled = true;
-			btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_DISABLE,
-					   &bt_disabled);
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-				  "[BTCoex], BT is disabled !!\n");
-		}
-	}
-
-	if (pre_bt_disabled != bt_disabled) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-			  "[BTCoex], BT is from %s to %s!!\n",
-			  (pre_bt_disabled ? "disabled":"enabled"),
-			  (bt_disabled ? "disabled":"enabled"));
-
-		pre_bt_disabled = bt_disabled;
-		if (!bt_disabled) {
-		} else {
-		}
-	}
-}
-#endif
-
-static void halbtc8723b2ant_monitor_bt_ctr(struct btc_coexist *btcoexist)
-{
-	u32 reg_hp_txrx, reg_lp_txrx, u32tmp;
-	u32 reg_hp_tx = 0, reg_hp_rx = 0;
-	u32 reg_lp_tx = 0, reg_lp_rx = 0;
-
-	reg_hp_txrx = 0x770;
-	reg_lp_txrx = 0x774;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_hp_txrx);
-	reg_hp_tx = u32tmp & MASKLWORD;
-	reg_hp_rx = (u32tmp & MASKHWORD) >> 16;
-
-	u32tmp = btcoexist->btc_read_4byte(btcoexist, reg_lp_txrx);
-	reg_lp_tx = u32tmp & MASKLWORD;
-	reg_lp_rx = (u32tmp & MASKHWORD) >> 16;
-
-	coex_sta->high_priority_tx = reg_hp_tx;
-	coex_sta->high_priority_rx = reg_hp_rx;
-	coex_sta->low_priority_tx = reg_lp_tx;
-	coex_sta->low_priority_rx = reg_lp_rx;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex], High Priority Tx/Rx(reg 0x%x)=0x%x(%d)/0x%x(%d)\n",
-		  reg_hp_txrx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_BT_MONITOR,
-		  "[BTCoex], Low Priority Tx/Rx(reg 0x%x)=0x%x(%d)/0x%x(%d)\n",
-		  reg_lp_txrx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx);
-
-	/* reset counter */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-}
-
-static void halbtc8723b2ant_query_bt_info(struct btc_coexist *btcoexist)
-{
-	u8 h2c_parameter[1] ={0};
-
-	coex_sta->c2h_bt_info_req_sent = true;
-
-	h2c_parameter[0] |= BIT0;	/* trigger */
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Query Bt Info, FW write 0x61=0x%x\n",
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x61, 1, h2c_parameter);
-}
-
-static bool halbtc8723b2ant_is_wifi_status_changed(
-						struct btc_coexist *btcoexist)
-{
-	static bool pre_wifi_busy = false;
-	static bool pre_under_4way = false;
-	static bool pre_bt_hs_on = false;
-	bool wifi_busy = false, under_4way = false, bt_hs_on = false;
-	bool wifi_connected = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_4_WAY_PROGRESS,
-			   &under_4way);
-
-	if (wifi_connected) {
-		if (wifi_busy != pre_wifi_busy) {
-			pre_wifi_busy = wifi_busy;
-			return true;
-		}
-
-		if (under_4way != pre_under_4way) {
-			pre_under_4way = under_4way;
-			return true;
-		}
-
-		if (bt_hs_on != pre_bt_hs_on) {
-			pre_bt_hs_on = bt_hs_on;
-			return true;
-		}
-	}
-
-	return false;
-}
-
-static void halbtc8723b2ant_update_bt_link_info(struct btc_coexist *btcoexist)
-{
-	/*struct btc_stack_info *stack_info = &btcoexist->stack_info;*/
-	struct btc_bt_link_info *bt_link_info = &btcoexist->bt_link_info;
-	bool bt_hs_on = false;
-
-#if(BT_AUTO_REPORT_ONLY_8723B_2ANT == 1) /* profile from bt patch */
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-
-	bt_link_info->bt_link_exist = coex_sta->bt_link_exist;
-	bt_link_info->sco_exist = coex_sta->sco_exist;
-	bt_link_info->a2dp_exist = coex_sta->a2dp_exist;
-	bt_link_info->pan_exist = coex_sta->pan_exist;
-	bt_link_info->hid_exist = coex_sta->hid_exist;
-
-	/* work around for HS mode. */
-	if (bt_hs_on) {
-		bt_link_info->pan_exist = true;
-		bt_link_info->bt_link_exist = true;
-	}
-#else	/* profile from bt stack */
-	bt_link_info->bt_link_exist = stack_info->bt_link_exist;
-	bt_link_info->sco_exist = stack_info->sco_exist;
-	bt_link_info->a2dp_exist = stack_info->a2dp_exist;
-	bt_link_info->pan_exist = stack_info->pan_exist;
-	bt_link_info->hid_exist = stack_info->hid_exist;
-
-	/*for win-8 stack HID report error*/
-	if (!stack_info->hid_exist)
-		stack_info->hid_exist = coex_sta->hid_exist;
-	/*sync  BTInfo with BT firmware and stack*/
-	/* when stack HID report error, here we use the info from bt fw.*/
-	if (!stack_info->bt_link_exist)
-		stack_info->bt_link_exist = coex_sta->bt_link_exist;
-#endif
-	/* check if Sco only */
-	if (bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->sco_only = true;
-	else
-		bt_link_info->sco_only = false;
-
-	/* check if A2dp only */
-	if (!bt_link_info->sco_exist && bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->a2dp_only = true;
-	else
-		bt_link_info->a2dp_only = false;
-
-	/* check if Pan only */
-	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    bt_link_info->pan_exist && !bt_link_info->hid_exist)
-		bt_link_info->pan_only = true;
-	else
-		bt_link_info->pan_only = false;
-
-	/* check if Hid only */
-	if (!bt_link_info->sco_exist && !bt_link_info->a2dp_exist &&
-	    !bt_link_info->pan_exist && bt_link_info->hid_exist)
-		bt_link_info->hid_only = true;
-	else
-		bt_link_info->hid_only = false;
-}
-
-static u8 halbtc8723b2ant_action_algorithm(struct btc_coexist *btcoexist)
-{
-	struct btc_bt_link_info *bt_link_info=&btcoexist->bt_link_info;
-	bool bt_hs_on = false;
-	u8 algorithm = BT_8723B_2ANT_COEX_ALGO_UNDEFINED;
-	u8 num_of_diff_profile = 0;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-
-	if (!bt_link_info->bt_link_exist) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], No BT link exists!!!\n");
-		return algorithm;
-	}
-
-	if (bt_link_info->sco_exist)
-		num_of_diff_profile++;
-	if (bt_link_info->hid_exist)
-		num_of_diff_profile++;
-	if (bt_link_info->pan_exist)
-		num_of_diff_profile++;
-	if (bt_link_info->a2dp_exist)
-		num_of_diff_profile++;
-
-	if (num_of_diff_profile == 1) {
-		if (bt_link_info->sco_exist) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], SCO only\n");
-			algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
-		} else {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], HID only\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], A2DP only\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_A2DP;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], PAN(HS) only\n");
-					algorithm =
-						BT_8723B_2ANT_COEX_ALGO_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], PAN(EDR) only\n");
-					algorithm =
-						BT_8723B_2ANT_COEX_ALGO_PANEDR;
-				}
-			}
-		}
-	} else if (num_of_diff_profile == 2) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], SCO + HID\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], SCO + A2DP ==> SCO\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + PAN(HS)\n");
-					algorithm = BT_8723B_2ANT_COEX_ALGO_SCO;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], HID + A2DP\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + PAN(HS)\n");
-					algorithm = BT_8723B_2ANT_COEX_ALGO_HID;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], A2DP + PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex],A2DP + PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP;
-				}
-			}
-		}
-	} else if (num_of_diff_profile == 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->a2dp_exist) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], SCO + HID + A2DP"
-					  " ==> HID\n");
-				algorithm = BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-			} else if (bt_link_info->hid_exist &&
-				   bt_link_info->pan_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + HID + "
-						  "PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + HID + "
-						  "PAN(EDR)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			} else if (bt_link_info->pan_exist &&
-				   bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + A2DP + "
-						  "PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + A2DP + "
-						  "PAN(EDR) ==> HID\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		} else {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + A2DP + "
-						  "PAN(HS)\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_HID_A2DP;
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], HID + A2DP + "
-						  "PAN(EDR)\n");
-					algorithm =
-					BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR;
-				}
-			}
-		}
-	} else if (num_of_diff_profile >= 3) {
-		if (bt_link_info->sco_exist) {
-			if (bt_link_info->hid_exist &&
-			    bt_link_info->pan_exist &&
-			    bt_link_info->a2dp_exist) {
-				if (bt_hs_on) {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], Error!!! SCO + HID"
-						  " + A2DP + PAN(HS)\n");
-				} else {
-					BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-						  "[BTCoex], SCO + HID + A2DP +"
-						  " PAN(EDR)==>PAN(EDR)+HID\n");
-					algorithm =
-					    BT_8723B_2ANT_COEX_ALGO_PANEDR_HID;
-				}
-			}
-		}
-	}
-
-	return algorithm;
-}
-
-static bool halbtc8723b2ant_need_to_dec_bt_pwr(struct btc_coexist *btcoexist)
-{
-	bool bRet = false;
-	bool bt_hs_on = false, wifi_connected = false;
-	s32 bt_hs_rssi=0;
-	u8 bt_rssi_state;
-
-	if (!btcoexist->btc_get(btcoexist,
-			BTC_GET_BL_HS_OPERATION, &bt_hs_on))
-		return false;
-	if (!btcoexist->btc_get(btcoexist,
-			BTC_GET_BL_WIFI_CONNECTED, &wifi_connected))
-		return false;
-	if (!btcoexist->btc_get(btcoexist,
-			BTC_GET_S4_HS_RSSI, &bt_hs_rssi))
-		return false;
-
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	if (wifi_connected) {
-		if (bt_hs_on) {
-			if (bt_hs_rssi > 37) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-					  "[BTCoex], Need to decrease bt "
-					  "power for HS mode!!\n");
-				bRet = true;
-			}
-		} else {
-			if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-			    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-					  "[BTCoex], Need to decrease bt "
-					  "power for Wifi is connected!!\n");
-				bRet = true;
-			}
-		}
-	}
-
-	return bRet;
-}
-
-static void halbtc8723b2ant_set_fw_dac_swing_level(
-						struct btc_coexist *btcoexist,
-						u8 dac_swing_lvl)
-{
-	u8 h2c_parameter[1] ={0};
-
-	/* There are several type of dacswing
-	 * 0x18/ 0x10/ 0xc/ 0x8/ 0x4/ 0x6 */
-	h2c_parameter[0] = dac_swing_lvl;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl);
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x64=0x%x\n", h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x64, 1, h2c_parameter);
-}
-
-static void halbtc8723b2ant_set_fw_dec_bt_pwr(struct btc_coexist *btcoexist,
-				       bool dec_bt_pwr)
-{
-	u8 h2c_parameter[1] = {0};
-
-	h2c_parameter[0] = 0;
-
-	if (dec_bt_pwr)
-		h2c_parameter[0] |= BIT1;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], decrease Bt Power : %s, FW write 0x62=0x%x\n",
-		  (dec_bt_pwr? "Yes!!":"No!!"), h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x62, 1, h2c_parameter);
-}
-
-static void halbtc8723b2ant_dec_bt_pwr(struct btc_coexist *btcoexist,
-					bool force_exec, bool dec_bt_pwr)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s Dec BT power = %s\n",
-		  (force_exec? "force to":""), (dec_bt_pwr? "ON":"OFF"));
-	coex_dm->cur_dec_bt_pwr = dec_bt_pwr;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreDecBtPwr=%d, bCurDecBtPwr=%d\n",
-			  coex_dm->pre_dec_bt_pwr, coex_dm->cur_dec_bt_pwr);
-
-		if (coex_dm->pre_dec_bt_pwr == coex_dm->cur_dec_bt_pwr)
-			return;
-	}
-	halbtc8723b2ant_set_fw_dec_bt_pwr(btcoexist, coex_dm->cur_dec_bt_pwr);
-
-	coex_dm->pre_dec_bt_pwr = coex_dm->cur_dec_bt_pwr;
-}
-
-#if (BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-static void halbtc8723b2ant_set_bt_auto_report(struct btc_coexist *btcoexist,
-					bool enable_auto_report)
-{
-	u8 h2c_parameter[1] = {0};
-	h2c_parameter[0] = 0;
-
-	if (enable_auto_report)
-		h2c_parameter[0] |= BIT0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], BT FW auto report : %s, FW write 0x68=0x%x\n",
-		  (enable_auto_report? "Enabled!!":"Disabled!!"),
-		  h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x68, 1, h2c_parameter);
-}
-
-static void halbtc8723b2ant_bt_auto_report(struct btc_coexist *btcoexist,
-				    bool force_exec, bool enable_auto_report)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s BT Auto report = %s\n",
-		  (force_exec? "force to":""),
-		  ((enable_auto_report)? "Enabled":"Disabled"));
-	coex_dm->cur_bt_auto_report = enable_auto_report;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreBtAutoReport=%d, "
-			  "bCurBtAutoReport=%d\n",
-			  coex_dm->pre_bt_auto_report,
-			  coex_dm->cur_bt_auto_report);
-
-		if (coex_dm->pre_bt_auto_report == coex_dm->cur_bt_auto_report)
-			return;
-	}
-	halbtc8723b2ant_set_bt_auto_report(btcoexist,
-					   coex_dm->cur_bt_auto_report);
-
-	coex_dm->pre_bt_auto_report = coex_dm->cur_bt_auto_report;
-}
-#endif
-
-static void halbtc8723b2ant_fw_dac_swing_lvl(struct btc_coexist *btcoexist,
-				      bool force_exec, u8 fw_dac_swing_lvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s set FW Dac Swing level = %d\n",
-		  (force_exec? "force to":""), fw_dac_swing_lvl);
-	coex_dm->cur_fw_dac_swing_lvl = fw_dac_swing_lvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], preFwDacSwingLvl=%d, "
-			  "curFwDacSwingLvl=%d\n",
-			  coex_dm->pre_fw_dac_swing_lvl,
-			  coex_dm->cur_fw_dac_swing_lvl);
-
-		if(coex_dm->pre_fw_dac_swing_lvl ==
-		   coex_dm->cur_fw_dac_swing_lvl)
-			return;
-	}
-
-	halbtc8723b2ant_set_fw_dac_swing_level(btcoexist,
-					       coex_dm->cur_fw_dac_swing_lvl);
-	coex_dm->pre_fw_dac_swing_lvl = coex_dm->cur_fw_dac_swing_lvl;
-}
-
-static void halbtc8723b2ant_set_sw_rf_rx_lpf_corner(
-						struct btc_coexist *btcoexist,
-						bool rx_rf_shrink_on)
-{
-	if (rx_rf_shrink_on) {
-		/* Shrink RF Rx LPF corner */
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Shrink RF Rx LPF corner!!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-					  0xfffff, 0xffffc);
-	} else {
-		/* Resume RF Rx LPF corner */
-		/* After initialized, we can use coex_dm->btRf0x1eBackup */
-		if (btcoexist->initilized) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-				  "[BTCoex], Resume RF Rx LPF corner!!\n");
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1e,
-						  0xfffff,
-						  coex_dm->bt_rf0x1e_backup);
-		}
-	}
-}
-
-static void halbtc8723b2ant_rf_shrink(struct btc_coexist *btcoexist,
-			       bool force_exec, bool rx_rf_shrink_on)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn Rx RF Shrink = %s\n",
-		  (force_exec? "force to":""), (rx_rf_shrink_on? "ON":"OFF"));
-	coex_dm->cur_rf_rx_lpf_shrink = rx_rf_shrink_on;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreRfRxLpfShrink=%d, "
-			  "bCurRfRxLpfShrink=%d\n",
-			  coex_dm->pre_rf_rx_lpf_shrink,
-			  coex_dm->cur_rf_rx_lpf_shrink);
-
-		if (coex_dm->pre_rf_rx_lpf_shrink ==
-		    coex_dm->cur_rf_rx_lpf_shrink)
-			return;
-	}
-	halbtc8723b2ant_set_sw_rf_rx_lpf_corner(btcoexist,
-						coex_dm->cur_rf_rx_lpf_shrink);
-
-	coex_dm->pre_rf_rx_lpf_shrink = coex_dm->cur_rf_rx_lpf_shrink;
-}
-
-static void halbtc8723b2ant_set_sw_penalty_txrate_adaptive(
-						struct btc_coexist *btcoexist,
-						bool low_penalty_ra)
-{
-	u8 h2c_parameter[6] ={0};
-
-	h2c_parameter[0] = 0x6;	/* opCode, 0x6= Retry_Penalty*/
-
-	if (low_penalty_ra) {
-		h2c_parameter[1] |= BIT0;
-		/*normal rate except MCS7/6/5, OFDM54/48/36*/
-		h2c_parameter[2] = 0x00;
-		h2c_parameter[3] = 0xf7;  /*MCS7 or OFDM54*/
-		h2c_parameter[4] = 0xf8;  /*MCS6 or OFDM48*/
-		h2c_parameter[5] = 0xf9;  /*MCS5 or OFDM36*/
-	}
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], set WiFi Low-Penalty Retry: %s",
-		  (low_penalty_ra? "ON!!":"OFF!!"));
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x69, 6, h2c_parameter);
-}
-
-static void halbtc8723b2ant_low_penalty_ra(struct btc_coexist *btcoexist,
-				    bool force_exec, bool low_penalty_ra)
-{
-	/*return; */
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn LowPenaltyRA = %s\n",
-		  (force_exec? "force to":""), (low_penalty_ra? "ON":"OFF"));
-	coex_dm->cur_low_penalty_ra = low_penalty_ra;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreLowPenaltyRa=%d, "
-			  "bCurLowPenaltyRa=%d\n",
-			  coex_dm->pre_low_penalty_ra,
-			  coex_dm->cur_low_penalty_ra);
-
-		if (coex_dm->pre_low_penalty_ra == coex_dm->cur_low_penalty_ra)
-			return;
-	}
-	halbtc8723b2ant_set_sw_penalty_txrate_adaptive(btcoexist,
-						coex_dm->cur_low_penalty_ra);
-
-	coex_dm->pre_low_penalty_ra = coex_dm->cur_low_penalty_ra;
-}
-
-static void halbtc8723b2ant_set_dac_swing_reg(struct btc_coexist *btcoexist,
-				       u32 level)
-{
-	u8 val = (u8) level;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], Write SwDacSwing = 0x%x\n", level);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x883, 0x3e, val);
-}
-
-static void halbtc8723b2ant_set_sw_fulltime_dac_swing(
-						struct btc_coexist *btcoexist,
-						bool sw_dac_swing_on,
-						u32 sw_dac_swing_lvl)
-{
-	if(sw_dac_swing_on)
-		halbtc8723b2ant_set_dac_swing_reg(btcoexist, sw_dac_swing_lvl);
-	else
-		halbtc8723b2ant_set_dac_swing_reg(btcoexist, 0x18);
-}
-
-
-static void halbtc8723b2ant_dac_swing(struct btc_coexist *btcoexist,
-			       bool force_exec, bool dac_swing_on,
-			       u32 dac_swing_lvl)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn DacSwing=%s, dac_swing_lvl=0x%x\n",
-		  (force_exec? "force to":""),
-		  (dac_swing_on? "ON":"OFF"), dac_swing_lvl);
-	coex_dm->cur_dac_swing_on = dac_swing_on;
-	coex_dm->cur_dac_swing_lvl = dac_swing_lvl;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreDacSwingOn=%d, preDacSwingLvl=0x%x,"
-			  " bCurDacSwingOn=%d, curDacSwingLvl=0x%x\n",
-			  coex_dm->pre_dac_swing_on, coex_dm->pre_dac_swing_lvl,
-			  coex_dm->cur_dac_swing_on,
-			  coex_dm->cur_dac_swing_lvl);
-
-		if ((coex_dm->pre_dac_swing_on == coex_dm->cur_dac_swing_on) &&
-		    (coex_dm->pre_dac_swing_lvl == coex_dm->cur_dac_swing_lvl))
-			return;
-	}
-	mdelay(30);
-	halbtc8723b2ant_set_sw_fulltime_dac_swing(btcoexist, dac_swing_on,
-						  dac_swing_lvl);
-
-	coex_dm->pre_dac_swing_on = coex_dm->cur_dac_swing_on;
-	coex_dm->pre_dac_swing_lvl = coex_dm->cur_dac_swing_lvl;
-}
-
-#if 0
-static void halbtc8723b2ant_set_adc_backoff(struct btc_coexist *btcoexist,
-				     bool adc_backoff)
-{
-	if (adc_backoff) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB BackOff Level On!\n");
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc05, 0x30, 0x3);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB BackOff Level Off!\n");
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0xc05, 0x30, 0x1);
-	}
-}
-
-static void halbtc8723b2ant_adc_backoff(struct btc_coexist *btcoexist,
-				 bool force_exec, bool adc_backoff)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s turn AdcBackOff = %s\n",
-		  (force_exec? "force to":""), (adc_backoff? "ON":"OFF"));
-	coex_dm->cur_adc_back_off = adc_backoff;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAdcBackOff=%d, bCurAdcBackOff=%d\n",
-			  coex_dm->pre_adc_back_off,
-			  coex_dm->cur_adc_back_off);
-
-		if (coex_dm->pre_adc_back_off == coex_dm->cur_adc_back_off)
-			return;
-	}
-	halbtc8723b2ant_set_adc_backoff(btcoexist, coex_dm->cur_adc_back_off);
-
-	coex_dm->pre_adc_back_off = coex_dm->cur_adc_back_off;
-}
-#endif
-
-static void halbtc8723b2ant_set_agc_table(struct btc_coexist *btcoexist,
-				   bool agc_table_en)
-{
-	u8 rssi_adjust_val = 0;
-
-	/*  BB AGC Gain Table */
-	if (agc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB Agc Table On!\n");
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6e1A0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6d1B0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6c1C0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6b1D0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x6a1E0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x691F0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0x68200001);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], BB Agc Table Off!\n");
-	 	btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xaa1A0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa91B0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa81C0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa71D0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa61E0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa51F0001);
-		btcoexist->btc_write_4byte(btcoexist, 0xc78, 0xa4200001);
-	}
-
-
-	/* RF Gain */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x02000);
-	if (agc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table On!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x38fff);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x38ffe);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table Off!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x380c3);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x3b,
-					  0xfffff, 0x28ce6);
-	}
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xef, 0xfffff, 0x0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xed, 0xfffff, 0x1);
-
-	if (agc_table_en) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table On!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x38fff);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x38ffe);
-	} else {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-			  "[BTCoex], Agc Table Off!\n");
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x380c3);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x40,
-					  0xfffff, 0x28ce6);
-	}
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0xed, 0xfffff, 0x0);
-
-	/* set rssiAdjustVal for wifi module. */
-	if (agc_table_en)
-		rssi_adjust_val = 8;
-	btcoexist->btc_set(btcoexist, BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
-			   &rssi_adjust_val);
-}
-
-static void halbtc8723b2ant_agc_table(struct btc_coexist *btcoexist,
-			       bool force_exec, bool agc_table_en)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s %s Agc Table\n",
-		  (force_exec? "force to":""),
-		  (agc_table_en? "Enable":"Disable"));
-	coex_dm->cur_agc_table_en = agc_table_en;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], bPreAgcTableEn=%d, bCurAgcTableEn=%d\n",
-			  coex_dm->pre_agc_table_en, coex_dm->cur_agc_table_en);
-
-		if (coex_dm->pre_agc_table_en == coex_dm->cur_agc_table_en)
-			return;
-	}
-	halbtc8723b2ant_set_agc_table(btcoexist, agc_table_en);
-
-	coex_dm->pre_agc_table_en = coex_dm->cur_agc_table_en;
-}
-
-static void halbtc8723b2ant_set_coex_table(struct btc_coexist *btcoexist,
-				    u32 val0x6c0, u32 val0x6c4,
-				    u32 val0x6c8, u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c0=0x%x\n", val0x6c0);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c0, val0x6c0);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c4=0x%x\n", val0x6c4);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c4, val0x6c4);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6c8=0x%x\n", val0x6c8);
-	btcoexist->btc_write_4byte(btcoexist, 0x6c8, val0x6c8);
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_EXEC,
-		  "[BTCoex], set coex table, set 0x6cc=0x%x\n", val0x6cc);
-	btcoexist->btc_write_1byte(btcoexist, 0x6cc, val0x6cc);
-}
-
-static void halbtc8723b2ant_coex_table(struct btc_coexist *btcoexist,
-				bool force_exec, u32 val0x6c0,
-				u32 val0x6c4, u32 val0x6c8,
-				u8 val0x6cc)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW,
-		  "[BTCoex], %s write Coex Table 0x6c0=0x%x,"
-		  " 0x6c4=0x%x, 0x6c8=0x%x, 0x6cc=0x%x\n",
-		  (force_exec? "force to":""), val0x6c0,
-		  val0x6c4, val0x6c8, val0x6cc);
-	coex_dm->cur_val0x6c0 = val0x6c0;
-	coex_dm->cur_val0x6c4 = val0x6c4;
-	coex_dm->cur_val0x6c8 = val0x6c8;
-	coex_dm->cur_val0x6cc = val0x6cc;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], preVal0x6c0=0x%x, "
-			  "preVal0x6c4=0x%x, preVal0x6c8=0x%x, "
-			  "preVal0x6cc=0x%x !!\n",
-			  coex_dm->pre_val0x6c0, coex_dm->pre_val0x6c4,
-			  coex_dm->pre_val0x6c8, coex_dm->pre_val0x6cc);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_SW_DETAIL,
-			  "[BTCoex], curVal0x6c0=0x%x, "
-			  "curVal0x6c4=0x%x, curVal0x6c8=0x%x, "
-			  "curVal0x6cc=0x%x !!\n",
-			  coex_dm->cur_val0x6c0, coex_dm->cur_val0x6c4,
-			  coex_dm->cur_val0x6c8, coex_dm->cur_val0x6cc);
-
-		if ((coex_dm->pre_val0x6c0 == coex_dm->cur_val0x6c0) &&
-		    (coex_dm->pre_val0x6c4 == coex_dm->cur_val0x6c4) &&
-		    (coex_dm->pre_val0x6c8 == coex_dm->cur_val0x6c8) &&
-		    (coex_dm->pre_val0x6cc == coex_dm->cur_val0x6cc))
-			return;
-	}
-	halbtc8723b2ant_set_coex_table(btcoexist, val0x6c0, val0x6c4,
-				       val0x6c8, val0x6cc);
-
-	coex_dm->pre_val0x6c0 = coex_dm->cur_val0x6c0;
-	coex_dm->pre_val0x6c4 = coex_dm->cur_val0x6c4;
-	coex_dm->pre_val0x6c8 = coex_dm->cur_val0x6c8;
-	coex_dm->pre_val0x6cc = coex_dm->cur_val0x6cc;
-}
-
-static void halbtc8723b2ant_coex_table_with_type(struct btc_coexist *btcoexist,
-					  bool force_exec, u8 type)
-{
-	switch (type) {
-	case 0:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x55555555, 0xffff, 0x3);
-		break;
-	case 1:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55555555,
-					   0x5afa5afa, 0xffff, 0x3);
-		break;
-	case 2:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5a5a5a5a,
-					   0x5a5a5a5a, 0xffff, 0x3);
-		break;
-	case 3:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xaaaaaaaa,
-					   0xaaaaaaaa, 0xffff, 0x3);
-		break;
-	case 4:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0xffffffff,
-					   0xffffffff, 0xffff, 0x3);
-		break;
-	case 5:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5fff5fff,
-					   0x5fff5fff, 0xffff, 0x3);
-		break;
-	case 6:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5a5a5a5a, 0xffff, 0x3);
-		break;
-	case 7:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5afa5afa, 0xffff, 0x3);
-		break;
-	case 8:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x5aea5aea,
-					   0x5aea5aea, 0xffff, 0x3);
-		break;
-	case 9:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5aea5aea, 0xffff, 0x3);
-		break;
-	case 10:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5aff5aff, 0xffff, 0x3);
-		break;
-	case 11:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5a5f5a5f, 0xffff, 0x3);
-		break;
-	case 12:
-		halbtc8723b2ant_coex_table(btcoexist, force_exec, 0x55ff55ff,
-					   0x5f5f5f5f, 0xffff, 0x3);
-		break;
-	default:
-		break;
-	}
-}
-
-static void halbtc8723b2ant_set_fw_ignore_wlan_act(
-						struct btc_coexist *btcoexist,
-						bool enable)
-{
-	u8 h2c_parameter[1] ={0};
-
-	if (enable)
-		h2c_parameter[0] |= BIT0;/* function enable*/
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], set FW for BT Ignore Wlan_Act, "
-		  "FW write 0x63=0x%x\n", h2c_parameter[0]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x63, 1, h2c_parameter);
-}
-
-static void halbtc8723b2ant_ignore_wlan_act(struct btc_coexist *btcoexist,
-				     bool force_exec, bool enable)
-{
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn Ignore WlanAct %s\n",
-		  (force_exec? "force to":""), (enable? "ON":"OFF"));
-	coex_dm->cur_ignore_wlan_act = enable;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPreIgnoreWlanAct = %d, "
-			  "bCurIgnoreWlanAct = %d!!\n",
-			  coex_dm->pre_ignore_wlan_act,
-			  coex_dm->cur_ignore_wlan_act);
-
-		if (coex_dm->pre_ignore_wlan_act ==
-		    coex_dm->cur_ignore_wlan_act)
-			return;
-	}
-	halbtc8723b2ant_set_fw_ignore_wlan_act(btcoexist, enable);
-
-	coex_dm->pre_ignore_wlan_act = coex_dm->cur_ignore_wlan_act;
-}
-
-static void halbtc8723b2ant_set_fw_ps_tdma(struct btc_coexist *btcoexist,
-			u8 byte1, u8 byte2, u8 byte3, u8 byte4, u8 byte5)
-{
-	u8 h2c_parameter[5] ={0};
-
-	h2c_parameter[0] = byte1;
-	h2c_parameter[1] = byte2;
-	h2c_parameter[2] = byte3;
-	h2c_parameter[3] = byte4;
-	h2c_parameter[4] = byte5;
-
-	coex_dm->ps_tdma_para[0] = byte1;
-	coex_dm->ps_tdma_para[1] = byte2;
-	coex_dm->ps_tdma_para[2] = byte3;
-	coex_dm->ps_tdma_para[3] = byte4;
-	coex_dm->ps_tdma_para[4] = byte5;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x60(5bytes)=0x%x%08x\n",
-		  h2c_parameter[0],
-		  h2c_parameter[1] << 24 | h2c_parameter[2] << 16 |
-		  h2c_parameter[3] << 8 | h2c_parameter[4]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x60, 5, h2c_parameter);
-}
-
-static void halbtc8723b2ant_sw_mechanism1(struct btc_coexist *btcoexist,
-				   bool shrink_rx_lpf, bool low_penalty_ra,
-				   bool limited_dig, bool bt_lna_constrain)
-{
-	/*
-	u32	wifi_bw;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if(BTC_WIFI_BW_HT40 != wifi_bw)  //only shrink RF Rx LPF for HT40
-	{
-		if (shrink_rx_lpf)
-			shrink_rx_lpf = false;
-	}
-	*/
-
-	halbtc8723b2ant_rf_shrink(btcoexist, NORMAL_EXEC, shrink_rx_lpf);
-	halbtc8723b2ant_low_penalty_ra(btcoexist, NORMAL_EXEC, low_penalty_ra);
-}
-
-static void halbtc8723b2ant_sw_mechanism2(struct btc_coexist *btcoexist,
-				   bool agc_table_shift, bool adc_backoff,
-				   bool sw_dac_swing, u32 dac_swing_lvl)
-{
-	halbtc8723b2ant_agc_table(btcoexist, NORMAL_EXEC, agc_table_shift);
-	/*halbtc8723b2ant_adc_backoff(btcoexist, NORMAL_EXEC, adc_backoff);*/
-	halbtc8723b2ant_dac_swing(btcoexist, NORMAL_EXEC, sw_dac_swing,
-				  dac_swing_lvl);
-}
-
-static void halbtc8723b2ant_set_ant_path(struct btc_coexist *btcoexist,
-				  u8 antpos_type, bool init_hwcfg,
-				  bool wifi_off)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	u32 fw_ver = 0, u32tmp=0;
-	bool pg_ext_switch = false;
-	bool use_ext_switch = false;
-	u8 h2c_parameter[2] ={0};
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_EXT_SWITCH, &pg_ext_switch);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-
-	if ((fw_ver<0xc0000) || pg_ext_switch)
-		use_ext_switch = true;
-
-	if (init_hwcfg) {
-		/* 0x4c[23]=0, 0x4c[24]=1  Antenna control by WL/BT */
-		u32tmp = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-		u32tmp &= ~BIT23;
-		u32tmp |= BIT24;
-		btcoexist->btc_write_4byte(btcoexist, 0x4c, u32tmp);
-
-		btcoexist->btc_write_1byte(btcoexist, 0x974, 0xff);
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x944, 0x3, 0x3);
-		btcoexist->btc_write_1byte(btcoexist, 0x930, 0x77);
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x1);
-
-		/* Force GNT_BT to low */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x0);
-		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-
-		if (board_info->btdm_ant_pos == BTC_ANTENNA_AT_MAIN_PORT) {
-			/* tell firmware "no antenna inverse" */
-			h2c_parameter[0] = 0;
-			h2c_parameter[1] = 1;  /* ext switch type */
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		} else {
-			/* tell firmware "antenna inverse" */
-			h2c_parameter[0] = 1;
-			h2c_parameter[1] = 1;  /* ext switch type */
-			btcoexist->btc_fill_h2c(btcoexist, 0x65, 2,
-						h2c_parameter);
-		}
-	}
-
-	/* ext switch setting */
-	if (use_ext_switch) {
-		 /* fixed internal switch S1->WiFi, S0->BT */
-		btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-		switch (antpos_type) {
-		case BTC_ANT_WIFI_AT_MAIN:
-			/* ext switch main at wifi */
-			btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c,
-							   0x3, 0x1);
-			break;
-		case BTC_ANT_WIFI_AT_AUX:
-			/* ext switch aux at wifi */
-			btcoexist->btc_write_1byte_bitmask(btcoexist,
-							   0x92c, 0x3, 0x2);
-			break;
-		}
-	} else {	/* internal switch */
-		/* fixed ext switch */
-		btcoexist->btc_write_1byte_bitmask(btcoexist, 0x92c, 0x3, 0x1);
-		switch (antpos_type) {
-		case BTC_ANT_WIFI_AT_MAIN:
-			/* fixed internal switch S1->WiFi, S0->BT */
-			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x0);
-			break;
-		case BTC_ANT_WIFI_AT_AUX:
-			/* fixed internal switch S0->WiFi, S1->BT */
-			btcoexist->btc_write_2byte(btcoexist, 0x948, 0x280);
-			break;
-		}
-	}
-}
-
-
-static void halbtc8723b2ant_ps_tdma(struct btc_coexist *btcoexist,
-					bool force_exec,
-					bool turn_on, u8 type)
-{
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], %s turn %s PS TDMA, type=%d\n",
-		  (force_exec? "force to":""), (turn_on? "ON":"OFF"), type);
-	coex_dm->cur_ps_tdma_on = turn_on;
-	coex_dm->cur_ps_tdma = type;
-
-	if (!force_exec) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], bPrePsTdmaOn = %d, bCurPsTdmaOn = %d!!\n",
-			  coex_dm->pre_ps_tdma_on, coex_dm->cur_ps_tdma_on);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], prePsTdma = %d, curPsTdma = %d!!\n",
-			  coex_dm->pre_ps_tdma, coex_dm->cur_ps_tdma);
-
-		if ((coex_dm->pre_ps_tdma_on == coex_dm->cur_ps_tdma_on) &&
-		    (coex_dm->pre_ps_tdma == coex_dm->cur_ps_tdma))
-			return;
-	}
-	if (turn_on) {
-		switch (type) {
-		case 1:
-		default:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
-						       0x1a, 0xe1, 0x90);
-			break;
-		case 2:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
-						       0x12, 0xe1, 0x90);
-			break;
-		case 3:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
-						       0x3, 0xf1, 0x90);
-			break;
-		case 4:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x10,
-						       0x03, 0xf1, 0x90);
-			break;
-		case 5:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
-						       0x1a, 0x60, 0x90);
-			break;
-		case 6:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
-						       0x12, 0x60, 0x90);
-			break;
-		case 7:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1c,
-						       0x3, 0x70, 0x90);
-			break;
-		case 8:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x10,
-						       0x3, 0x70, 0x90);
-			break;
-		case 9:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
-						       0x1a, 0xe1, 0x90);
-			break;
-		case 10:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
-						       0x12, 0xe1, 0x90);
-			break;
-		case 11:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
-						       0xa, 0xe1, 0x90);
-			break;
-		case 12:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
-						       0x5, 0xe1, 0x90);
-			break;
-		case 13:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
-						       0x1a, 0x60, 0x90);
-			break;
-		case 14:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x12,
-						       0x12, 0x60, 0x90);
-			break;
-		case 15:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0xa,
-						       0xa, 0x60, 0x90);
-			break;
-		case 16:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
-						       0x5, 0x60, 0x90);
-			break;
-		case 17:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xa3, 0x2f,
-						       0x2f, 0x60, 0x90);
-			break;
-		case 18:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x5,
-						       0x5, 0xe1, 0x90);
-			break;
-		case 19:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
-						       0x25, 0xe1, 0x90);
-			break;
-		case 20:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x25,
-						       0x25, 0x60, 0x90);
-			break;
-		case 21:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x15,
-						       0x03, 0x70, 0x90);
-			break;
-		case 71:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0xe3, 0x1a,
-						       0x1a, 0xe1, 0x90);
-			break;
-		}
-	} else {
-		/* disable PS tdma */
-		switch (type) {
-		case 0:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
-						       0x40, 0x0);
-			break;
-		case 1:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
-						       0x48, 0x0);
-			break;
-		default:
-			halbtc8723b2ant_set_fw_ps_tdma(btcoexist, 0x0, 0x0, 0x0,
-						       0x40, 0x0);
-			break;
-		}
-	}
-
-	/* update pre state */
-	coex_dm->pre_ps_tdma_on = coex_dm->cur_ps_tdma_on;
-	coex_dm->pre_ps_tdma = coex_dm->cur_ps_tdma;
-}
-
-static void halbtc8723b2ant_coex_alloff(struct btc_coexist *btcoexist)
-{
-	/* fw all off */
-	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	/* sw all off */
-	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-
-	/* hw all off */
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-}
-
-static void halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	/* force to reset coex mechanism*/
-
-	halbtc8723b2ant_ps_tdma(btcoexist, FORCE_EXEC, false, 1);
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
-	halbtc8723b2ant_dec_bt_pwr(btcoexist, FORCE_EXEC, false);
-
-	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-}
-
-static void halbtc8723b2ant_action_bt_inquiry(struct btc_coexist *btcoexist)
-{
-	bool wifi_connected = false;
-	bool low_pwr_disable = true;
-
-	btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-			   &low_pwr_disable);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-
-	if (wifi_connected) {
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 3);
-	} else {
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-	}
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, FORCE_EXEC, 6);
-	halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false, false);
-	halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false, 0x18);
-
-	coex_dm->need_recover_0x948 = true;
-	coex_dm->backup_0x948 = btcoexist->btc_read_2byte(btcoexist, 0x948);
-
-	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_AUX,
-				     false, false);
-}
-
-static bool halbtc8723b2ant_is_common_action(struct btc_coexist *btcoexist)
-{
-	bool bCommon = false, wifi_connected = false;
-	bool wifi_busy = false;
-	bool bt_hs_on = false, low_pwr_disable = false;
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_CONNECTED,
-			   &wifi_connected);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-
-	if (!wifi_connected) {
-		low_pwr_disable = false;
-		btcoexist->btc_set(btcoexist, BTC_SET_ACT_DISABLE_LOW_POWER,
-				   &low_pwr_disable);
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Wifi non-connected idle!!\n");
-
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
-					  0x0);
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
- 		halbtc8723b2ant_sw_mechanism1(btcoexist, false, false, false,
-					      false);
-		halbtc8723b2ant_sw_mechanism2(btcoexist, false, false, false,
-					      0x18);
-
-		bCommon = true;
-	} else {
-		if (BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE ==
-		    coex_dm->bt_status) {
-			low_pwr_disable = false;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Wifi connected + "
-				  "BT non connected-idle!!\n");
-
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x0);
-			halbtc8723b2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 0);
-			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						1);
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 0xb);
-			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-
-	      		halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-
-			bCommon = true;
-		} else if (BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE ==
-			   coex_dm->bt_status) {
-			low_pwr_disable = true;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			if(bt_hs_on)
-				return false;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Wifi connected + "
-				  "BT connected-idle!!\n");
-
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x0);
-			halbtc8723b2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 0);
-			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false,
-						1);
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 0xb);
-			halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC,
-						   false);
-
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-
-			bCommon = true;
-		} else {
-			low_pwr_disable = true;
-			btcoexist->btc_set(btcoexist,
-					   BTC_SET_ACT_DISABLE_LOW_POWER,
-					   &low_pwr_disable);
-
-			if (wifi_busy) {
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], Wifi Connected-Busy + "
-					  "BT Busy!!\n");
-				bCommon = false;
-			} else {
-				if(bt_hs_on)
-					return false;
-
-				BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-					  "[BTCoex], Wifi Connected-Idle + "
-					  "BT Busy!!\n");
-
-				btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A,
-							  0x1, 0xfffff, 0x0);
-				halbtc8723b2ant_coex_table_with_type(btcoexist,
-								    NORMAL_EXEC,
-								    7);
-				halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC,
-							true, 21);
-				halbtc8723b2ant_fw_dac_swing_lvl(btcoexist,
-								 NORMAL_EXEC,
-								 0xb);
-				if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-					halbtc8723b2ant_dec_bt_pwr(btcoexist,
-								   NORMAL_EXEC,
-								   true);
-				else
-					halbtc8723b2ant_dec_bt_pwr(btcoexist,
-								   NORMAL_EXEC,
-								   false);
-				halbtc8723b2ant_sw_mechanism1(btcoexist, false,
-							      false, false,
-							      false);
-				halbtc8723b2ant_sw_mechanism2(btcoexist, false,
-							      false, false,
-							      0x18);
-				bCommon = true;
-			}
-		}
-	}
-
-	return bCommon;
-}
-
-static void halbtc8723b2ant_tdma_duration_adjust(struct btc_coexist *btcoexist,
-					  bool sco_hid, bool tx_pause,
-					  u8 max_interval)
-{
-	static s32 up, dn, m, n, wait_count;
-	/*0: no change, +1: increase WiFi duration, -1: decrease WiFi duration*/
-	s32 result;
-	u8 retryCount=0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW,
-		  "[BTCoex], TdmaDurationAdjust()\n");
-
-	if (!coex_dm->auto_tdma_adjust) {
-		coex_dm->auto_tdma_adjust = true;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], first run TdmaDurationAdjust()!!\n");
-		if (sco_hid) {
-			if (tx_pause) {
-				if (max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				}else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				}
-			} else {
-				if(max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				}
-			}
-		} else {
-			if (tx_pause) {
-				if (max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				}
-			} else {
-				if (max_interval == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 1);
-					coex_dm->ps_tdma_du_adj_type = 1;
-				} else if (max_interval == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (max_interval == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				}
-			}
-		}
-
-		up = 0;
-		dn = 0;
-		m = 1;
-		n= 3;
-		result = 0;
-		wait_count = 0;
-	} else {
-		/*acquire the BT TRx retry count from BT_Info byte2*/
-		retryCount = coex_sta->bt_retry_cnt;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], retryCount = %d\n", retryCount);
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], up=%d, dn=%d, m=%d, n=%d, wait_count=%d\n",
-			  up, dn, m, n, wait_count);
-		result = 0;
-		wait_count++;
-		 /* no retry in the last 2-second duration*/
-		if (retryCount == 0) {
-			up++;
-			dn--;
-
-			if (dn <= 0)
-				dn = 0;
-
-			if (up >= n) {
-				wait_count = 0;
-				n = 3;
-				up = 0;
-				dn = 0;
-				result = 1;
- 				BTC_PRINT(BTC_MSG_ALGORITHM,
- 					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], Increase wifi "
-					  "duration!!\n");
-			}/* <=3 retry in the last 2-second duration*/
-		} else if (retryCount <= 3) {
-			up--;
-			dn++;
-
-			if (up <= 0)
-				up = 0;
-
-			if (dn == 2) {
-				if (wait_count <= 2)
-					m++;
-				else
-					m = 1;
-
-				if (m >= 20)
-					m = 20;
-
-				n = 3 * m;
-				up = 0;
-				dn = 0;
-				wait_count = 0;
-				result = -1;
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], Decrease wifi duration "
-					  "for retryCounter<3!!\n");
-			}
-		} else {
-			if (wait_count == 1)
-				m++;
-			else
-				m = 1;
-
-			if (m >= 20)
-				m = 20;
-
-			n = 3 * m;
-			up = 0;
-			dn = 0;
-			wait_count = 0;
-			result = -1;
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], Decrease wifi duration "
-				  "for retryCounter>3!!\n");
-		}
-
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], max Interval = %d\n", max_interval);
-		if (max_interval == 1) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-
-				if (coex_dm->cur_ps_tdma == 71) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-					coex_dm->ps_tdma_du_adj_type = 5;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-					coex_dm->ps_tdma_du_adj_type = 13;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									      8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if(coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if(coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if(coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 5);
-						coex_dm->ps_tdma_du_adj_type =
-									      5;
-					} else if(coex_dm->cur_ps_tdma == 16) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if(coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if(coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 13);
-						coex_dm->ps_tdma_du_adj_type =
-									     13;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 71);
-					coex_dm->ps_tdma_du_adj_type = 71;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-					coex_dm->ps_tdma_du_adj_type = 9;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if(coex_dm->cur_ps_tdma == 16) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 71) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									      1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if(coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									      4;
-					} else if(coex_dm->cur_ps_tdma == 9) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
- 					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 1);
-						coex_dm->ps_tdma_du_adj_type =
-									      1;
-					} else if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 71);
-						coex_dm->ps_tdma_du_adj_type =
-									     71;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 9);
-						coex_dm->ps_tdma_du_adj_type =
-									      9;
-					}
-				}
-			}
-		} else if(max_interval == 2) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-					coex_dm->ps_tdma_du_adj_type = 6;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-					coex_dm->ps_tdma_du_adj_type = 14;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									      8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 6);
-						coex_dm->ps_tdma_du_adj_type =
-									      6;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 14);
-						coex_dm->ps_tdma_du_adj_type =
-									     14;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-					coex_dm->ps_tdma_du_adj_type = 2;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 14){
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-					coex_dm->ps_tdma_du_adj_type = 10;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									      4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 2);
-						coex_dm->ps_tdma_du_adj_type =
-									      2;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 10);
-						coex_dm->ps_tdma_du_adj_type =
-									     10;
-					}
-				}
-			}
-		} else if (max_interval == 3) {
-			if (tx_pause) {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 1\n");
-				if (coex_dm->cur_ps_tdma == 1) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 2) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 3) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-					coex_dm->ps_tdma_du_adj_type = 7;
-				} else if (coex_dm->cur_ps_tdma == 4) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-					coex_dm->ps_tdma_du_adj_type = 8;
-				}
-				if (coex_dm->cur_ps_tdma == 9) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 10) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 11) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-					coex_dm->ps_tdma_du_adj_type = 15;
-				} else if (coex_dm->cur_ps_tdma == 12) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-					coex_dm->ps_tdma_du_adj_type = 16;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 5) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 8);
-						coex_dm->ps_tdma_du_adj_type =
-									      8;
-					} else if (coex_dm->cur_ps_tdma == 13) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 16);
-						coex_dm->ps_tdma_du_adj_type =
-									     16;
-					}
-				}  else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 8) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 7) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 6) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 7);
-						coex_dm->ps_tdma_du_adj_type =
-									      7;
-					} else if (coex_dm->cur_ps_tdma == 16) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 15) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					} else if (coex_dm->cur_ps_tdma == 14) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 15);
-						coex_dm->ps_tdma_du_adj_type =
-									     15;
-					}
-				}
-			} else {
-				BTC_PRINT(BTC_MSG_ALGORITHM,
-					  ALGO_TRACE_FW_DETAIL,
-					  "[BTCoex], TxPause = 0\n");
-				if (coex_dm->cur_ps_tdma == 5) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 6) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 7) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-					coex_dm->ps_tdma_du_adj_type = 3;
-				} else if (coex_dm->cur_ps_tdma == 8) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-					coex_dm->ps_tdma_du_adj_type = 4;
-				}
-				if (coex_dm->cur_ps_tdma == 13) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 14) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 15) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-					coex_dm->ps_tdma_du_adj_type = 11;
-				} else if (coex_dm->cur_ps_tdma == 16) {
-					halbtc8723b2ant_ps_tdma(btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-					coex_dm->ps_tdma_du_adj_type = 12;
-				}
-				if (result == -1) {
-					if (coex_dm->cur_ps_tdma == 1) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 4);
-						coex_dm->ps_tdma_du_adj_type =
-									      4;
-					} else if (coex_dm->cur_ps_tdma == 9) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 12);
-						coex_dm->ps_tdma_du_adj_type =
-									     12;
-					}
-				} else if (result == 1) {
-					if (coex_dm->cur_ps_tdma == 4) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 3) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 2) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 3);
-						coex_dm->ps_tdma_du_adj_type =
-									      3;
-					} else if (coex_dm->cur_ps_tdma == 12) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 11) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					} else if (coex_dm->cur_ps_tdma == 10) {
-						halbtc8723b2ant_ps_tdma(
-								btcoexist,
-								NORMAL_EXEC,
-								true, 11);
-						coex_dm->ps_tdma_du_adj_type =
-									     11;
-					}
-				}
-			}
-		}
-	}
-
-	/*if current PsTdma not match with the recorded one (when scan, dhcp..),
-	 *then we have to adjust it back to the previous record one.*/
-	if (coex_dm->cur_ps_tdma != coex_dm->ps_tdma_du_adj_type) {
-		bool scan = false, link = false, roam = false;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-			  "[BTCoex], PsTdma type dismatch!!!, "
-			  "curPsTdma=%d, recordPsTdma=%d\n",
-			  coex_dm->cur_ps_tdma, coex_dm->ps_tdma_du_adj_type);
-
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-		btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-
-		if (!scan && !link && !roam)
-			halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true,
-						coex_dm->ps_tdma_du_adj_type);
-		else
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_DETAIL,
-				  "[BTCoex], roaming/link/scan is under"
-				  " progress, will adjust next time!!!\n");
-	}
-}
-
-/* SCO only or SCO+PAN(HS) */
-static void halbtc8723b2ant_action_sco(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 4);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	/*for SCO quality at 11b/g mode*/
-	if (BTC_WIFI_BW_LEGACY == wifi_bw)
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 2);
-	else  /*for SCO quality & wifi performance balance at 11n mode*/
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 8);
-
-	/*for voice quality */
-	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 0);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x4);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      true, 0x4);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x4);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      true, 0x4);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (BTC_WIFI_BW_LEGACY == wifi_bw) /*/for HID at 11b/g mode*/
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-	else  /*for HID quality & wifi performance balance at 11n mode*/
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 9);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 9);
-	else
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 13);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
- 			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
- 			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*A2DP only / PAN(EDR) only/ A2DP+PAN(HS)*/
-static void halbtc8723b2ant_action_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, wifi_rssi_state1, bt_rssi_state;
-	u32 wifi_bw;
-	u8 ap_num = 0;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	wifi_rssi_state1 = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							   1, 2, 40, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
-
-	/* define the office environment */
-	/* driver don't know AP num in Linux, so we will never enter this if */
-	if (ap_num >= 10 && BTC_RSSI_HIGH(wifi_rssi_state1)) {
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
-					  0x0);
-		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 0);
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-
-		/* sw mechanism */
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-		if (BTC_WIFI_BW_HT40 == wifi_bw) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      true, 0x18);
-		}
-		return;
-	}
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist,false, false, 1);
-	else
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist,false, true, 1);
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
- 			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_a2dp_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 2);
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
- 			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false,0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_pan_edr(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 10);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 1);
-	else
-		halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, true, 5);
-
-	/* sw mechanism */
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*PAN(HS) only*/
-static void halbtc8723b2ant_action_pan_hs(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH) )
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	halbtc8723b2ant_ps_tdma(btcoexist, NORMAL_EXEC, false, 1);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/*PAN(EDR)+A2DP*/
-static void halbtc8723b2ant_action_pan_edr_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		halbtc8723b2ant_coex_table_with_type(btcoexist,NORMAL_EXEC, 12);
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
-							     true, 3);
-		else
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, false,
-							     false, 3);
-	} else {
-		halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, false, true, 3);
-	}
-
-	/* sw mechanism	*/
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, false,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_pan_edr_hid(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		if (BTC_WIFI_BW_HT40 == wifi_bw) {
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 3);
-			halbtc8723b2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 11);
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x780);
-		} else {
-			halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC,
-							 6);
-			halbtc8723b2ant_coex_table_with_type(btcoexist,
-							     NORMAL_EXEC, 7);
-			btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1,
-						  0xfffff, 0x0);
-		}
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
-	} else {
-		halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-		halbtc8723b2ant_coex_table_with_type(btcoexist,NORMAL_EXEC, 11);
-		btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff,
-					  0x0);
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
- 			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)){
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-/* HID+A2DP+PAN(EDR) */
-static void halbtc8723b2ant_action_hid_a2dp_pan_edr(
-						struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
-							     true, 2);
-		else
-			halbtc8723b2ant_tdma_duration_adjust(btcoexist, true,
-							     false, 3);
-	} else {
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 3);
-	}
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_action_hid_a2dp(struct btc_coexist *btcoexist)
-{
-	u8 wifi_rssi_state, bt_rssi_state;
-	u32 wifi_bw;
-
-	wifi_rssi_state = halbtc8723b2ant_wifi_rssi_state(btcoexist,
-							  0, 2, 15, 0);
-	bt_rssi_state = halbtc8723b2ant_bt_rssi_state(2, 29, 0);
-
-	btcoexist->btc_set_rf_reg(btcoexist, BTC_RF_A, 0x1, 0xfffff, 0x0);
-
-	halbtc8723b2ant_fw_dac_swing_lvl(btcoexist, NORMAL_EXEC, 6);
-
-	if (halbtc8723b2ant_need_to_dec_bt_pwr(btcoexist))
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, true);
-	else
-		halbtc8723b2ant_dec_bt_pwr(btcoexist, NORMAL_EXEC, false);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-
-	halbtc8723b2ant_coex_table_with_type(btcoexist, NORMAL_EXEC, 7);
-
-	if ((bt_rssi_state == BTC_RSSI_STATE_HIGH) ||
-	    (bt_rssi_state == BTC_RSSI_STATE_STAY_HIGH))
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, false, 2);
-	else
-		halbtc8723b2ant_tdma_duration_adjust(btcoexist, true, true, 2);
-
-	/* sw mechanism */
-	if (BTC_WIFI_BW_HT40 == wifi_bw) {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, true, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	} else {
-		if ((wifi_rssi_state == BTC_RSSI_STATE_HIGH) ||
-		    (wifi_rssi_state == BTC_RSSI_STATE_STAY_HIGH)) {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, true, false,
-						      false, 0x18);
-		} else {
-			halbtc8723b2ant_sw_mechanism1(btcoexist, false, true,
-						      false, false);
-			halbtc8723b2ant_sw_mechanism2(btcoexist, false, false,
-						      false, 0x18);
-		}
-	}
-}
-
-static void halbtc8723b2ant_run_coexist_mechanism(
-						struct btc_coexist *btcoexist)
-{
-	u8 algorithm = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], RunCoexistMechanism()===>\n");
-
-	if (btcoexist->manual_control) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], RunCoexistMechanism(), "
-			  "return for Manual CTRL <===\n");
-		return;
-	}
-
-	if (coex_sta->under_ips) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], wifi is under IPS !!!\n");
-		return;
-	}
-
-	algorithm = halbtc8723b2ant_action_algorithm(btcoexist);
-	if (coex_sta->c2h_bt_inquiry_page &&
-	    (BT_8723B_2ANT_COEX_ALGO_PANHS != algorithm)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BT is under inquiry/page scan !!\n");
-		halbtc8723b2ant_action_bt_inquiry(btcoexist);
-		return;
-	} else {
-		if (coex_dm->need_recover_0x948) {
-			coex_dm->need_recover_0x948 = false;
-			btcoexist->btc_write_2byte(btcoexist, 0x948,
-						   coex_dm->backup_0x948);
-		}
-	}
-
-	coex_dm->cur_algorithm = algorithm;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, "[BTCoex], Algorithm = %d \n",
-		  coex_dm->cur_algorithm);
-
-	if (halbtc8723b2ant_is_common_action(btcoexist)) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], Action 2-Ant common.\n");
-		coex_dm->auto_tdma_adjust = false;
-	} else {
-		if (coex_dm->cur_algorithm != coex_dm->pre_algorithm) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], preAlgorithm=%d, "
-				  "curAlgorithm=%d\n", coex_dm->pre_algorithm,
-				  coex_dm->cur_algorithm);
-			coex_dm->auto_tdma_adjust = false;
-		}
-		switch (coex_dm->cur_algorithm) {
-		case BT_8723B_2ANT_COEX_ALGO_SCO:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = SCO.\n");
-			halbtc8723b2ant_action_sco(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, algorithm = HID.\n");
-			halbtc8723b2ant_action_hid(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = A2DP.\n");
-			halbtc8723b2ant_action_a2dp(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = A2DP+PAN(HS).\n");
-			halbtc8723b2ant_action_a2dp_pan_hs(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = PAN(EDR).\n");
-			halbtc8723b2ant_action_pan_edr(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_PANHS:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = HS mode.\n");
-			halbtc8723b2ant_action_pan_hs(btcoexist);
-				break;
-		case BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = PAN+A2DP.\n");
-			halbtc8723b2ant_action_pan_edr_a2dp(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_PANEDR_HID:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = PAN(EDR)+HID.\n");
-			halbtc8723b2ant_action_pan_edr_hid(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = HID+A2DP+PAN.\n");
-			halbtc8723b2ant_action_hid_a2dp_pan_edr(btcoexist);
-			break;
-		case BT_8723B_2ANT_COEX_ALGO_HID_A2DP:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = HID+A2DP.\n");
-			halbtc8723b2ant_action_hid_a2dp(btcoexist);
-			break;
-		default:
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], Action 2-Ant, "
-				  "algorithm = coexist All Off!!\n");
-			halbtc8723b2ant_coex_alloff(btcoexist);
-			break;
-		}
-		coex_dm->pre_algorithm = coex_dm->cur_algorithm;
-	}
-}
-
-static void halbtc8723b2ant_wifioff_hwcfg(struct btc_coexist *btcoexist)
-{
-	/* set wlan_act to low */
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0x4);
-	/* Force GNT_BT to High */
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x765, 0x18, 0x3);
-	/* BT select s0/s1 is controlled by BT */
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x67, 0x20, 0x0);
-}
-
-/*********************************************************************
- *  work around function start with wa_halbtc8723b2ant_
- *********************************************************************/
-/*********************************************************************
- *  extern function start with EXhalbtc8723b2ant_
- *********************************************************************/
-void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist)
-{
-	u8 u8tmp = 0;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], 2Ant Init HW Config!!\n");
-	coex_dm->bt_rf0x1e_backup =
-		btcoexist->btc_get_rf_reg(btcoexist, BTC_RF_A, 0x1e, 0xfffff);
-
-	/* 0x790[5:0]=0x5 */
-	u8tmp = btcoexist->btc_read_1byte(btcoexist, 0x790);
-	u8tmp &= 0xc0;
-	u8tmp |= 0x5;
-	btcoexist->btc_write_1byte(btcoexist, 0x790, u8tmp);
-
-
-	/*Antenna config	*/
-	halbtc8723b2ant_set_ant_path(btcoexist, BTC_ANT_WIFI_AT_MAIN,
-				     true, false);
-
-
-
-
-	/* PTA parameter */
-	halbtc8723b2ant_coex_table_with_type(btcoexist, FORCE_EXEC, 0);
-
-	/* Enable counter statistics */
-	/*0x76e[3] =1, WLAN_Act control by PTA*/
-	btcoexist->btc_write_1byte(btcoexist, 0x76e, 0xc);
-	btcoexist->btc_write_1byte(btcoexist, 0x778, 0x3);
-	btcoexist->btc_write_1byte_bitmask(btcoexist, 0x40, 0x20, 0x1);
-}
-
-void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-		  "[BTCoex], Coex Mechanism Init!!\n");
-	halbtc8723b2ant_init_coex_dm(btcoexist);
-}
-
-void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	struct btc_bt_link_info* bt_link_info = &btcoexist->bt_link_info;
-	u8 *cli_buf = btcoexist->cli_buf;
-	u8 u8tmp[4], i, bt_info_ext, ps_tdma_case=0;
-	u32 u32tmp[4];
-	bool roam = false, scan = false;
-	bool link = false, wifi_under_5g = false;
-	bool bt_hs_on = false, wifi_busy = false;
-	s32 wifi_rssi = 0, bt_hs_rssi = 0;
-	u32 wifi_bw, wifi_traffic_dir, fa_ofdm, fa_cck;
-	u8 wifi_dot11_chnl, wifi_hs_chnl;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-	u8 ap_num = 0;
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n ============[BT Coexist info]============");
-	CL_PRINTF(cli_buf);
-
-	if (btcoexist->manual_control) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ==========[Under Manual Control]============");
-		CL_PRINTF(cli_buf);
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-			   "\r\n ==========================================");
-		CL_PRINTF(cli_buf);
-	}
-
-	if (!board_info->bt_exist) {
-		CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n BT not exists !!!");
-		CL_PRINTF(cli_buf);
-		return;
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
-		   "Ant PG number/ Ant mechanism:",
-		   board_info->pg_ant_num, board_info->btdm_ant_num);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %d",
-		   "BT stack/ hci ext ver",
-		   ((stack_info->profile_notified)? "Yes":"No"),
-		   stack_info->hci_version);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER, &bt_patch_ver);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %d_%x/ 0x%x/ 0x%x(%d)",
-		   "CoexVer/ fw_ver/ PatchVer",
-		   glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
-		   fw_ver, bt_patch_ver, bt_patch_ver);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_HS_OPERATION, &bt_hs_on);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_DOT11_CHNL,
-			   &wifi_dot11_chnl);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_WIFI_HS_CHNL, &wifi_hs_chnl);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d(%d)",
-		   "Dot11 channel / HsChnl(HsMode)",
-		   wifi_dot11_chnl, wifi_hs_chnl, bt_hs_on);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %02x %02x %02x ",
-		   "H2C Wifi inform bt chnl Info", coex_dm->wifi_chnl_info[0],
-		   coex_dm->wifi_chnl_info[1], coex_dm->wifi_chnl_info[2]);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_WIFI_RSSI, &wifi_rssi);
-	btcoexist->btc_get(btcoexist, BTC_GET_S4_HS_RSSI, &bt_hs_rssi);
-	btcoexist->btc_get(btcoexist, BTC_GET_U1_AP_NUM, &ap_num);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d",
-		   "Wifi rssi/ HS rssi/ AP#", wifi_rssi, bt_hs_rssi, ap_num);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_SCAN, &scan);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_LINK, &link);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_ROAM, &roam);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
-		   "Wifi link/ roam/ scan", link, roam, scan);
-	CL_PRINTF(cli_buf);
-
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_UNDER_5G, &wifi_under_5g);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_BW, &wifi_bw);
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_BUSY, &wifi_busy);
-	btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
-			   &wifi_traffic_dir);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s / %s/ %s ",
-		   "Wifi status", (wifi_under_5g? "5G":"2.4G"),
-		   ((BTC_WIFI_BW_LEGACY == wifi_bw)? "Legacy":
-		   (((BTC_WIFI_BW_HT40 == wifi_bw)? "HT40":"HT20"))),
-		   ((!wifi_busy)? "idle":
-		   ((BTC_WIFI_TRAFFIC_TX ==wifi_traffic_dir)?\
-		   "uplink":"downlink")));
-	CL_PRINTF(cli_buf);
-
-
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d / %d / %d / %d",
-		   "SCO/HID/PAN/A2DP",
-		   bt_link_info->sco_exist, bt_link_info->hid_exist,
-		   bt_link_info->pan_exist, bt_link_info->a2dp_exist);
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_BT_LINK_INFO);
-
-	bt_info_ext = coex_sta->bt_info_ext;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s",
-		   "BT Info A2DP rate",
-		   (bt_info_ext&BIT0)? "Basic rate":"EDR rate");
-	CL_PRINTF(cli_buf);
-
-	for (i=0; i<BT_INFO_SRC_8723B_2ANT_MAX; i++) {
-		if (coex_sta->bt_info_c2h_cnt[i]) {
-			CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-				   "\r\n %-35s = %02x %02x %02x "
-				   "%02x %02x %02x %02x(%d)",
-				   glbt_info_src_8723b_2ant[i], \
-				   coex_sta->bt_info_c2h[i][0],
-				   coex_sta->bt_info_c2h[i][1],
-				   coex_sta->bt_info_c2h[i][2],
-				   coex_sta->bt_info_c2h[i][3],
-				   coex_sta->bt_info_c2h[i][4],
-				   coex_sta->bt_info_c2h[i][5],
-				   coex_sta->bt_info_c2h[i][6],
-				   coex_sta->bt_info_c2h_cnt[i]);
-			CL_PRINTF(cli_buf);
-		}
-	}
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %s/%s",
-		   "PS state, IPS/LPS",
-		   ((coex_sta->under_ips? "IPS ON":"IPS OFF")),
-		   ((coex_sta->under_lps? "LPS ON":"LPS OFF")));
-	CL_PRINTF(cli_buf);
-	btcoexist->btc_disp_dbg_msg(btcoexist, BTC_DBG_DISP_FW_PWR_MODE_CMD);
-
-	/* Sw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s", "============[Sw mechanism]============");
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d ",
- 		   "SM1[ShRf/ LpRA/ LimDig]", coex_dm->cur_rf_rx_lpf_shrink,
-		   coex_dm->cur_low_penalty_ra, coex_dm->limited_dig);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d/ %d(0x%x) ",
-		   "SM2[AgcT/ AdcB/ SwDacSwing(lvl)]",
-		   coex_dm->cur_agc_table_en, coex_dm->cur_adc_back_off,
-		   coex_dm->cur_dac_swing_on, coex_dm->cur_dac_swing_lvl);
-	CL_PRINTF(cli_buf);
-
-	/* Fw mechanism	*/
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Fw mechanism]============");
-	CL_PRINTF(cli_buf);
-
-	ps_tdma_case = coex_dm->cur_ps_tdma;
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = %02x %02x %02x %02x %02x case-%d (auto:%d)",
-		   "PS TDMA", coex_dm->ps_tdma_para[0],
-		   coex_dm->ps_tdma_para[1], coex_dm->ps_tdma_para[2],
-		   coex_dm->ps_tdma_para[3], coex_dm->ps_tdma_para[4],
-		   ps_tdma_case, coex_dm->auto_tdma_adjust);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d ",
-		   "DecBtPwr/ IgnWlanAct", coex_dm->cur_dec_bt_pwr,
-		   coex_dm->cur_ignore_wlan_act);
-	CL_PRINTF(cli_buf);
-
-	/* Hw setting */
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s",
-		   "============[Hw setting]============");
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x",
-		   "RF-A, 0x1e initVal", coex_dm->bt_rf0x1e_backup);
-	CL_PRINTF(cli_buf);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x778);
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x880);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x778/0x880[29:25]", u8tmp[0],
-		   (u32tmp[0]&0x3e000000) >> 25);
-	CL_PRINTF(cli_buf);
-
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x948);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x67);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x765);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x948/ 0x67[5] / 0x765",
-		   u32tmp[0], ((u8tmp[0]&0x20)>> 5), u8tmp[1]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x92c);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x930);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x944);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "0x92c[1:0]/ 0x930[7:0]/0x944[1:0]",
-		   u32tmp[0]&0x3, u32tmp[1]&0xff, u32tmp[2]&0x3);
-	CL_PRINTF(cli_buf);
-
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x39);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0x40);
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x4c);
-	u8tmp[2] = btcoexist->btc_read_1byte(btcoexist, 0x64);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
-		   "0x38[11]/0x40/0x4c[24:23]/0x64[0]",
-		   ((u8tmp[0] & 0x8)>>3), u8tmp[1],
-		   ((u32tmp[0]&0x01800000)>>23), u8tmp[2]&0x1);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x550);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x522);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0x550(bcn ctrl)/0x522", u32tmp[0], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xc50);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x49c);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x",
-		   "0xc50(dig)/0x49c(null-drop)", u32tmp[0]&0xff, u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0xda0);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0xda4);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0xda8);
-	u32tmp[3] = btcoexist->btc_read_4byte(btcoexist, 0xcf0);
-
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0xa5b);
-	u8tmp[1] = btcoexist->btc_read_1byte(btcoexist, 0xa5c);
-
-	fa_ofdm = ((u32tmp[0]&0xffff0000) >> 16) +
-		  ((u32tmp[1]&0xffff0000) >> 16) +
-		   (u32tmp[1] & 0xffff) +
-		   (u32tmp[2] & 0xffff) +
-		  ((u32tmp[3]&0xffff0000) >> 16) +
-		   (u32tmp[3] & 0xffff) ;
-	fa_cck = (u8tmp[0] << 8) + u8tmp[1];
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = 0x%x/ 0x%x/ 0x%x",
-		   "OFDM-CCA/OFDM-FA/CCK-FA", \
-		   u32tmp[0]&0xffff, fa_ofdm, fa_cck);
-	CL_PRINTF(cli_buf);
-
-	u32tmp[0] = btcoexist->btc_read_4byte(btcoexist, 0x6c0);
-	u32tmp[1] = btcoexist->btc_read_4byte(btcoexist, 0x6c4);
-	u32tmp[2] = btcoexist->btc_read_4byte(btcoexist, 0x6c8);
-	u8tmp[0] = btcoexist->btc_read_1byte(btcoexist, 0x6cc);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE,
-		   "\r\n %-35s = 0x%x/ 0x%x/ 0x%x/ 0x%x",
-		   "0x6c0/0x6c4/0x6c8/0x6cc(coexTable)", \
-		   u32tmp[0], u32tmp[1], u32tmp[2], u8tmp[0]);
-	CL_PRINTF(cli_buf);
-
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x770(high-pri rx/tx)",
-		   coex_sta->high_priority_rx, coex_sta->high_priority_tx);
-	CL_PRINTF(cli_buf);
-	CL_SPRINTF(cli_buf, BT_TMP_BUF_SIZE, "\r\n %-35s = %d/ %d",
-		   "0x774(low-pri rx/tx)", coex_sta->low_priority_rx,
-		   coex_sta->low_priority_tx);
-	CL_PRINTF(cli_buf);
-#if(BT_AUTO_REPORT_ONLY_8723B_2ANT == 1)
-	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
-#endif
-	btcoexist->btc_disp_dbg_msg(btcoexist,
-	BTC_DBG_DISP_COEX_STATISTICS);
-}
-
-
-void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_IPS_ENTER == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS ENTER notify\n");
-		coex_sta->under_ips = true;
-		halbtc8723b2ant_wifioff_hwcfg(btcoexist);
-		halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
-		halbtc8723b2ant_coex_alloff(btcoexist);
-	} else if (BTC_IPS_LEAVE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], IPS LEAVE notify\n");
-		coex_sta->under_ips = false;
-		ex_halbtc8723b2ant_init_hwconfig(btcoexist);
-		halbtc8723b2ant_init_coex_dm(btcoexist);
-		halbtc8723b2ant_query_bt_info(btcoexist);
-	}
-}
-
-void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_LPS_ENABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS ENABLE notify\n");
-		coex_sta->under_lps = true;
-	} else if (BTC_LPS_DISABLE == type) {
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], LPS DISABLE notify\n");
-		coex_sta->under_lps = false;
-	}
-}
-
-void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_SCAN_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN START notify\n");
-	else if (BTC_SCAN_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], SCAN FINISH notify\n");
-}
-
-void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	if (BTC_ASSOCIATE_START == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT START notify\n");
-	else if (BTC_ASSOCIATE_FINISH == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], CONNECT FINISH notify\n");
-}
-
-void ex_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type)
-{
-	u8 h2c_parameter[3] ={0};
-	u32 wifi_bw;
-	u8 wifi_central_chnl;
-
-	if (BTC_MEDIA_CONNECT == type)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA connect notify\n");
-	else
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], MEDIA disconnect notify\n");
-
-	/* only 2.4G we need to inform bt the chnl mask */
-	btcoexist->btc_get(btcoexist,
-		BTC_GET_U1_WIFI_CENTRAL_CHNL, &wifi_central_chnl);
-	if ((BTC_MEDIA_CONNECT == type) &&
-	    (wifi_central_chnl <= 14)) {
-		h2c_parameter[0] = 0x1;
-		h2c_parameter[1] = wifi_central_chnl;
-		btcoexist->btc_get(btcoexist,
-			BTC_GET_U4_WIFI_BW, &wifi_bw);
-		if (BTC_WIFI_BW_HT40 == wifi_bw)
-			h2c_parameter[2] = 0x30;
-		else
-			h2c_parameter[2] = 0x20;
-	}
-
-	coex_dm->wifi_chnl_info[0] = h2c_parameter[0];
-	coex_dm->wifi_chnl_info[1] = h2c_parameter[1];
-	coex_dm->wifi_chnl_info[2] = h2c_parameter[2];
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE_FW_EXEC,
-		  "[BTCoex], FW write 0x66=0x%x\n",
-		  h2c_parameter[0] << 16 | h2c_parameter[1] << 8 |
-		  h2c_parameter[2]);
-
-	btcoexist->btc_fill_h2c(btcoexist, 0x66, 3, h2c_parameter);
-}
-
-void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type)
-{
-	if (type == BTC_PACKET_DHCP)
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-			  "[BTCoex], DHCP Packet notify\n");
-}
-
-void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length)
-{
-	u8 btInfo = 0;
-	u8 i, rsp_source = 0;
-	bool bt_busy = false, limited_dig = false;
-	bool wifi_connected = false;
-
-	coex_sta->c2h_bt_info_req_sent = false;
-
-	rsp_source = tmpbuf[0]&0xf;
-	if(rsp_source >= BT_INFO_SRC_8723B_2ANT_MAX)
-		rsp_source = BT_INFO_SRC_8723B_2ANT_WIFI_FW;
-	coex_sta->bt_info_c2h_cnt[rsp_source]++;
-
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-		  "[BTCoex], Bt info[%d], length=%d, hex data=[",
-		  rsp_source, length);
-	for (i = 0; i < length; i++) {
-		coex_sta->bt_info_c2h[rsp_source][i] = tmpbuf[i];
-		if (i == 1)
-			btInfo = tmpbuf[i];
-		if (i == length-1)
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x]\n", tmpbuf[i]);
-		else
-			BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY,
-				  "0x%02x, ", tmpbuf[i]);
-	}
-
-	if (btcoexist->manual_control) {
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), "
-			  "return for Manual CTRL<===\n");
-		return;
-	}
-
-	if (BT_INFO_SRC_8723B_2ANT_WIFI_FW != rsp_source) {
-		coex_sta->bt_retry_cnt =	/* [3:0]*/
-			coex_sta->bt_info_c2h[rsp_source][2] & 0xf;
-
-		coex_sta->bt_rssi =
-			coex_sta->bt_info_c2h[rsp_source][3] * 2 + 10;
-
-		coex_sta->bt_info_ext =
-			coex_sta->bt_info_c2h[rsp_source][4];
-
-		/* Here we need to resend some wifi info to BT
-		     because bt is reset and loss of the info.*/
-		if ((coex_sta->bt_info_ext & BIT1)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], BT ext info bit1 check,"
-				  " send wifi BW&Chnl to BT!!\n");
-			btcoexist->btc_get(btcoexist,BTC_GET_BL_WIFI_CONNECTED,
-					   &wifi_connected);
-			if (wifi_connected)
-				ex_halbtc8723b2ant_media_status_notify(
-							btcoexist,
-							BTC_MEDIA_CONNECT);
-			else
-				ex_halbtc8723b2ant_media_status_notify(
-							btcoexist,
-							BTC_MEDIA_DISCONNECT);
-		}
-
-		if ((coex_sta->bt_info_ext & BIT3)) {
-			BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-				  "[BTCoex], BT ext info bit3 check, "
-				  "set BT NOT to ignore Wlan active!!\n");
-			halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC,
-							false);
-		} else {
-			/* BT already NOT ignore Wlan active, do nothing here.*/
-		}
-#if(BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-		if ((coex_sta->bt_info_ext & BIT4)) {
-			/* BT auto report already enabled, do nothing*/
-		} else {
-			halbtc8723b2ant_bt_auto_report(btcoexist, FORCE_EXEC,
-						       true);
-		}
-#endif
-	}
-
-	/* check BIT2 first ==> check if bt is under inquiry or page scan*/
-	if (btInfo & BT_INFO_8723B_2ANT_B_INQ_PAGE)
-		coex_sta->c2h_bt_inquiry_page = true;
-	else
-		coex_sta->c2h_bt_inquiry_page = false;
-
-	/* set link exist status*/
-	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
-		coex_sta->bt_link_exist = false;
-		coex_sta->pan_exist = false;
-		coex_sta->a2dp_exist = false;
-		coex_sta->hid_exist = false;
-		coex_sta->sco_exist = false;
-	} else {// connection exists
-		coex_sta->bt_link_exist = true;
-		if (btInfo & BT_INFO_8723B_2ANT_B_FTP)
-			coex_sta->pan_exist = true;
-		else
-			coex_sta->pan_exist = false;
-		if (btInfo & BT_INFO_8723B_2ANT_B_A2DP)
-			coex_sta->a2dp_exist = true;
-		else
-			coex_sta->a2dp_exist = false;
-		if (btInfo & BT_INFO_8723B_2ANT_B_HID)
-			coex_sta->hid_exist = true;
-		else
-			coex_sta->hid_exist = false;
-		if (btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO)
-			coex_sta->sco_exist = true;
-		else
-			coex_sta->sco_exist = false;
-	}
-
-	halbtc8723b2ant_update_bt_link_info(btcoexist);
-
-	if (!(btInfo & BT_INFO_8723B_2ANT_B_CONNECTION)) {
-		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), "
-			  "BT Non-Connected idle!!!\n");
-	/* connection exists but no busy */
-	} else if (btInfo == BT_INFO_8723B_2ANT_B_CONNECTION) {
-		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT Connected-idle!!!\n");
-	} else if ((btInfo & BT_INFO_8723B_2ANT_B_SCO_ESCO) ||
-		   (btInfo & BT_INFO_8723B_2ANT_B_SCO_BUSY)) {
-		coex_dm->bt_status =
-			BT_8723B_2ANT_BT_STATUS_SCO_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT SCO busy!!!\n");
-	} else if (btInfo&BT_INFO_8723B_2ANT_B_ACL_BUSY) {
-		coex_dm->bt_status =
-			BT_8723B_2ANT_BT_STATUS_ACL_BUSY;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), BT ACL busy!!!\n");
-	} else {
-		coex_dm->bt_status = BT_8723B_2ANT_BT_STATUS_MAX;
-		BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-			  "[BTCoex], BtInfoNotify(), "
-			  "BT Non-Defined state!!!\n");
-	}
-
-	if ((BT_8723B_2ANT_BT_STATUS_ACL_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_2ANT_BT_STATUS_SCO_BUSY == coex_dm->bt_status) ||
-	    (BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY == coex_dm->bt_status)) {
-		bt_busy = true;
-		limited_dig = true;
-	} else {
-		bt_busy = false;
-		limited_dig = false;
-	}
-
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_TRAFFIC_BUSY, &bt_busy);
-
-	coex_dm->limited_dig = limited_dig;
-	btcoexist->btc_set(btcoexist, BTC_SET_BL_BT_LIMITED_DIG, &limited_dig);
-
-	halbtc8723b2ant_run_coexist_mechanism(btcoexist);
-}
-
-void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist)
-{
-	BTC_PRINT(BTC_MSG_INTERFACE, INTF_NOTIFY, "[BTCoex], Halt notify\n");
-
-	halbtc8723b2ant_wifioff_hwcfg(btcoexist);
-	halbtc8723b2ant_ignore_wlan_act(btcoexist, FORCE_EXEC, true);
-	ex_halbtc8723b2ant_media_status_notify(btcoexist, BTC_MEDIA_DISCONNECT);
-}
-
-void ex_halbtc8723b2ant_periodical(struct btc_coexist *btcoexist)
-{
-	struct btc_board_info *board_info = &btcoexist->board_info;
-	struct btc_stack_info *stack_info = &btcoexist->stack_info;
-	static u8 dis_ver_info_cnt = 0;
-	u32 fw_ver = 0, bt_patch_ver = 0;
-
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE,
-		  "[BTCoex], =========================="
-		  "Periodical===========================\n");
-
-	if (dis_ver_info_cnt <= 5) {
-		dis_ver_info_cnt += 1;
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], ****************************"
-			  "************************************\n");
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], Ant PG Num/ Ant Mech/ "
-			  "Ant Pos = %d/ %d/ %d\n", board_info->pg_ant_num,
-			  board_info->btdm_ant_num, board_info->btdm_ant_pos);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], BT stack/ hci ext ver = %s / %d\n",
-			  ((stack_info->profile_notified)? "Yes":"No"),
-			  stack_info->hci_version);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_BT_PATCH_VER,
-				   &bt_patch_ver);
-		btcoexist->btc_get(btcoexist, BTC_GET_U4_WIFI_FW_VER, &fw_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], CoexVer/ fw_ver/ PatchVer = "
-			  "%d_%x/ 0x%x/ 0x%x(%d)\n",
-			  glcoex_ver_date_8723b_2ant, glcoex_ver_8723b_2ant,
-			  fw_ver, bt_patch_ver, bt_patch_ver);
-		BTC_PRINT(BTC_MSG_INTERFACE, INTF_INIT,
-			  "[BTCoex], *****************************"
-			  "***********************************\n");
-	}
-
-#if(BT_AUTO_REPORT_ONLY_8723B_2ANT == 0)
-	halbtc8723b2ant_query_bt_info(btcoexist);
-	halbtc8723b2ant_monitor_bt_ctr(btcoexist);
-	halbtc8723b2ant_monitor_bt_enable_disable(btcoexist);
-#else
-	if (halbtc8723b2ant_is_wifi_status_changed(btcoexist) ||
-	    coex_dm->auto_tdma_adjust)
-		halbtc8723b2ant_run_coexist_mechanism(btcoexist);
-#endif
-}
-
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.h b/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.h
deleted file mode 100644
index fa3784aa70cd..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtc8723b2ant.h
+++ /dev/null
@@ -1,145 +0,0 @@
-/************************************************************************
- * The following is for 8723B 2Ant BT Co-exist definition
- ************************************************************************/
-#define	BT_AUTO_REPORT_ONLY_8723B_2ANT			1
-
-
-#define	BT_INFO_8723B_2ANT_B_FTP			BIT7
-#define	BT_INFO_8723B_2ANT_B_A2DP			BIT6
-#define	BT_INFO_8723B_2ANT_B_HID			BIT5
-#define	BT_INFO_8723B_2ANT_B_SCO_BUSY			BIT4
-#define	BT_INFO_8723B_2ANT_B_ACL_BUSY			BIT3
-#define	BT_INFO_8723B_2ANT_B_INQ_PAGE			BIT2
-#define	BT_INFO_8723B_2ANT_B_SCO_ESCO			BIT1
-#define	BT_INFO_8723B_2ANT_B_CONNECTION			BIT0
-
-#define BTC_RSSI_COEX_THRESH_TOL_8723B_2ANT		2
-
-typedef enum _BT_INFO_SRC_8723B_2ANT{
-	BT_INFO_SRC_8723B_2ANT_WIFI_FW			= 0x0,
-	BT_INFO_SRC_8723B_2ANT_BT_RSP			= 0x1,
-	BT_INFO_SRC_8723B_2ANT_BT_ACTIVE_SEND		= 0x2,
-	BT_INFO_SRC_8723B_2ANT_MAX
-}BT_INFO_SRC_8723B_2ANT,*PBT_INFO_SRC_8723B_2ANT;
-
-typedef enum _BT_8723B_2ANT_BT_STATUS{
-	BT_8723B_2ANT_BT_STATUS_NON_CONNECTED_IDLE	= 0x0,
-	BT_8723B_2ANT_BT_STATUS_CONNECTED_IDLE		= 0x1,
-	BT_8723B_2ANT_BT_STATUS_INQ_PAGE		= 0x2,
-	BT_8723B_2ANT_BT_STATUS_ACL_BUSY		= 0x3,
-	BT_8723B_2ANT_BT_STATUS_SCO_BUSY		= 0x4,
-	BT_8723B_2ANT_BT_STATUS_ACL_SCO_BUSY		= 0x5,
-	BT_8723B_2ANT_BT_STATUS_MAX
-}BT_8723B_2ANT_BT_STATUS,*PBT_8723B_2ANT_BT_STATUS;
-
-typedef enum _BT_8723B_2ANT_COEX_ALGO{
-	BT_8723B_2ANT_COEX_ALGO_UNDEFINED		= 0x0,
-	BT_8723B_2ANT_COEX_ALGO_SCO			= 0x1,
-	BT_8723B_2ANT_COEX_ALGO_HID			= 0x2,
-	BT_8723B_2ANT_COEX_ALGO_A2DP			= 0x3,
-	BT_8723B_2ANT_COEX_ALGO_A2DP_PANHS		= 0x4,
-	BT_8723B_2ANT_COEX_ALGO_PANEDR			= 0x5,
-	BT_8723B_2ANT_COEX_ALGO_PANHS			= 0x6,
-	BT_8723B_2ANT_COEX_ALGO_PANEDR_A2DP		= 0x7,
-	BT_8723B_2ANT_COEX_ALGO_PANEDR_HID		= 0x8,
-	BT_8723B_2ANT_COEX_ALGO_HID_A2DP_PANEDR		= 0x9,
-	BT_8723B_2ANT_COEX_ALGO_HID_A2DP		= 0xa,
-	BT_8723B_2ANT_COEX_ALGO_MAX			= 0xb,
-}BT_8723B_2ANT_COEX_ALGO,*PBT_8723B_2ANT_COEX_ALGO;
-
-struct coex_dm_8723b_2ant{
-	/* fw mechanism */
-	bool pre_dec_bt_pwr;
-	bool cur_dec_bt_pwr;
-	u8 pre_fw_dac_swing_lvl;
-	u8 cur_fw_dac_swing_lvl;
-	bool cur_ignore_wlan_act;
-	bool pre_ignore_wlan_act;
-	u8 pre_ps_tdma;
-	u8 cur_ps_tdma;
-	u8 ps_tdma_para[5];
-	u8 ps_tdma_du_adj_type;
-	bool reset_tdma_adjust;
-	bool auto_tdma_adjust;
-	bool pre_ps_tdma_on;
-	bool cur_ps_tdma_on;
-	bool pre_bt_auto_report;
-	bool cur_bt_auto_report;
-
-	/* sw mechanism */
-	bool pre_rf_rx_lpf_shrink;
-	bool cur_rf_rx_lpf_shrink;
-	u32 bt_rf0x1e_backup;
-	bool pre_low_penalty_ra;
-	bool cur_low_penalty_ra;
-	bool pre_dac_swing_on;
-	u32 pre_dac_swing_lvl;
-	bool cur_dac_swing_on;
-	u32 cur_dac_swing_lvl;
-	bool pre_adc_back_off;
-	bool cur_adc_back_off;
-	bool pre_agc_table_en;
-	bool cur_agc_table_en;
-	u32 pre_val0x6c0;
-	u32 cur_val0x6c0;
-	u32 pre_val0x6c4;
-	u32 cur_val0x6c4;
-	u32 pre_val0x6c8;
-	u32 cur_val0x6c8;
-	u8 pre_val0x6cc;
-	u8 cur_val0x6cc;
-	bool limited_dig;
-
-	/* algorithm related */
-	u8 pre_algorithm;
-	u8 cur_algorithm;
-	u8 bt_status;
-	u8 wifi_chnl_info[3];
-
-	bool need_recover_0x948;
-	u16 backup_0x948;
-};
-
-struct coex_sta_8723b_2ant{
-	bool bt_link_exist;
-	bool sco_exist;
-	bool a2dp_exist;
-	bool hid_exist;
-	bool pan_exist;
-
-	bool under_lps;
-	bool under_ips;
-	u32 high_priority_tx;
-	u32 high_priority_rx;
-	u32 low_priority_tx;
-	u32 low_priority_rx;
-	u8 bt_rssi;
-	u8 pre_bt_rssi_state;
-	u8 pre_wifi_rssi_state[4];
-	bool c2h_bt_info_req_sent;
-	u8 bt_info_c2h[BT_INFO_SRC_8723B_2ANT_MAX][10];
-	u32 bt_info_c2h_cnt[BT_INFO_SRC_8723B_2ANT_MAX];
-	bool c2h_bt_inquiry_page;
-	u8 bt_retry_cnt;
-	u8 bt_info_ext;
-};
-
-/*********************************************************************
- * The following is interface which will notify coex module.
- *********************************************************************/
-void ex_halbtc8723b2ant_init_hwconfig(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant_init_coex_dm(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_connect_notify(struct btc_coexist *btcoexist, u8 type);
-void ex_halbtc8723b2ant_media_status_notify(struct btc_coexist *btcoexist,
-					    u8 type);
-void ex_halbtc8723b2ant_special_packet_notify(struct btc_coexist *btcoexist,
-					      u8 type);
-void ex_halbtc8723b2ant_bt_info_notify(struct btc_coexist *btcoexist,
-				       u8 *tmpbuf, u8 length);
-void ex_halbtc8723b2ant_halt_notify(struct btc_coexist *btcoexist);
-void ex_halbtc8723b2ant_periodical(struct btc_coexist * btcoexist);
-void ex_halbtc8723b2ant_display_coex_info(struct btc_coexist *btcoexist);
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
deleted file mode 100644
index 01f5a74028a0..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.c
+++ /dev/null
@@ -1,1130 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2007 - 2013 Realtek Corporation. All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- *
- ******************************************************************************/
-
-#include "halbt_precomp.h"
-
-/*#if(BT_30_SUPPORT == 1)*/
-#if 1
-/***********************************************
- *		Global variables
- ***********************************************/
-static const char *const bt_profile_string[] = {
-	"NONE",
-	"A2DP",
-	"PAN",
-	"HID",
-	"SCO",
-};
-
-static const char *const bt_spec_string[] = {
-	"1.0b",
-	"1.1",
-	"1.2",
-	"2.0+EDR",
-	"2.1+EDR",
-	"3.0+HS",
-	"4.0",
-};
-
-static const char *const bt_link_role_string[] = {
-	"Master",
-	"Slave",
-};
-
-static const char *const h2c_state_string[] = {
-	"successful",
-	"h2c busy",
-	"rf off",
-	"fw not read",
-};
-
-static const char *const io_state_string[] = {
-	"IO_STATUS_SUCCESS",
-	"IO_STATUS_FAIL_CANNOT_IO",
-	"IO_STATUS_FAIL_RF_OFF",
-	"IO_STATUS_FAIL_FW_READ_CLEAR_TIMEOUT",
-	"IO_STATUS_FAIL_WAIT_IO_EVENT_TIMEOUT",
-	"IO_STATUS_INVALID_LEN",
-	"IO_STATUS_IO_IDLE_QUEUE_EMPTY",
-	"IO_STATUS_IO_INSERT_WAIT_QUEUE_FAIL",
-	"IO_STATUS_UNKNOWN_FAIL",
-	"IO_STATUS_WRONG_LEVEL",
-	"IO_STATUS_H2C_STOPPED",
-};
-
-struct btc_coexist gl_bt_coexist;
-
-u32 btc_dbg_type[BTC_MSG_MAX];
-static u8 btc_dbg_buf[100];
-
-/***************************************************
- *		Debug related function
- ***************************************************/
-static bool halbtc_is_bt_coexist_available(struct btc_coexist *btcoexist)
-{
-	if (!btcoexist->binded || NULL == btcoexist->adapter)
-		return false;
-	
-	return true;
-}
-
-static bool halbtc_is_wifi_busy(struct rtl_priv *rtlpriv)
-{
-
-	if (rtlpriv->link_info.b_busytraffic)
-		return true;
-	else
-		return false;
-}
-
-
-static void halbtc_dbg_init(void)
-{
-	u8 i;
-
-	for (i = 0; i < BTC_MSG_MAX; i++)
-		btc_dbg_type[i] = 0;
-
-	btc_dbg_type[BTC_MSG_INTERFACE]			= 	\
-//			INTF_INIT				|
-//			INTF_NOTIFY				|
-			0;
-	
-	btc_dbg_type[BTC_MSG_ALGORITHM]			= 	\
-//			ALGO_BT_RSSI_STATE			|
-//			ALGO_WIFI_RSSI_STATE			|
-//			ALGO_BT_MONITOR				|
-//			ALGO_TRACE				|
-//			ALGO_TRACE_FW				|
-//			ALGO_TRACE_FW_DETAIL			|
-//			ALGO_TRACE_FW_EXEC			|
-//			ALGO_TRACE_SW				|
-//			ALGO_TRACE_SW_DETAIL			|
-//			ALGO_TRACE_SW_EXEC			|
-			0;
-}
-
-static bool halbtc_is_bt40(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	bool is_ht40 = true;
-	enum ht_channel_width bw = rtlphy->current_chan_bw;
-	
-	
-	if (bw == HT_CHANNEL_WIDTH_20)
-		is_ht40 = false;
-	else if (bw == HT_CHANNEL_WIDTH_20_40)
-		is_ht40 = true;
-
-	return is_ht40;
-}
-
-static bool halbtc_legacy(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	
-	bool is_legacy = false;
-
-	if ((mac->mode == WIRELESS_MODE_B) || (mac->mode == WIRELESS_MODE_B))
-		is_legacy = true;
-
-	return is_legacy;
-}
-
-bool halbtc_is_wifi_uplink(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-	
-	if (rtlpriv->link_info.b_tx_busy_traffic)
-		return true;
-	else
-		return false;
-}
-
-static u32 halbtc_get_wifi_bw(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = 
-		(struct rtl_priv *)btcoexist->adapter;
-	u32 wifi_bw = BTC_WIFI_BW_HT20;
-	
-	if (halbtc_is_bt40(rtlpriv)){
-		wifi_bw = BTC_WIFI_BW_HT40;	
-	} else {
-		if(halbtc_legacy(rtlpriv))
-			wifi_bw = BTC_WIFI_BW_LEGACY;
-		else
-			wifi_bw = BTC_WIFI_BW_HT20;
-	}
-	return wifi_bw;
-}
-
-static u8 halbtc_get_wifi_central_chnl(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_phy 	*rtlphy = &(rtlpriv->phy);
-	u8 chnl = 1;
-	
-
-	if (rtlphy->current_channel != 0)
-		chnl = rtlphy->current_channel;
-	BTC_PRINT(BTC_MSG_ALGORITHM, ALGO_TRACE, 
-		  "halbtc_get_wifi_central_chnl:%d\n",chnl);
-	return chnl;
-}
-
-static void halbtc_leave_lps(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv;
-	struct rtl_ps_ctl *ppsc;
-	bool ap_enable = false;
-	
-	rtlpriv = btcoexist->adapter;
-	ppsc = rtl_psc(rtlpriv);
-	
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-			   &ap_enable);
-	
-	if (ap_enable) {
-		printk("halbtc_leave_lps()<--dont leave lps under AP mode\n");
-		return;
-	}
-
-	btcoexist->bt_info.bt_ctrl_lps = true;
-	btcoexist->bt_info.bt_lps_on = false;
-}
-
-static void halbtc_enter_lps(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv;
-	struct rtl_ps_ctl *ppsc;
-	bool ap_enable = false;
-	
-	rtlpriv = btcoexist->adapter;
-	ppsc = rtl_psc(rtlpriv);
-	
-	btcoexist->btc_get(btcoexist, BTC_GET_BL_WIFI_AP_MODE_ENABLE, 
-			   &ap_enable);
-	
-	if (ap_enable) {
-		printk("halbtc_enter_lps()<--dont enter lps under AP mode\n");
-		return;
-	}
-	
-	btcoexist->bt_info.bt_ctrl_lps = true;
-	btcoexist->bt_info.bt_lps_on = false;
-}
-
-static void halbtc_normal_lps(struct btc_coexist *btcoexist)
-{
-	if (btcoexist->bt_info.bt_ctrl_lps) {
-		btcoexist->bt_info.bt_lps_on = false;
-		btcoexist->bt_info.bt_ctrl_lps = false;
-	}
-		
-}
-
-static void halbtc_leave_low_power(void)
-{
-}
-
-static void halbtc_nomal_low_power(void)
-{
-}
-
-static void halbtc_disable_low_power(void)
-{
-}
-
-static void halbtc_aggregation_check(void)
-{
-}
-
-
-static u32 halbtc_get_bt_patch_version(struct btc_coexist *btcoexist)
-{
-	return 0;
-}
-
-static s32 halbtc_get_wifi_rssi(struct rtl_priv *adapter)
-{
-	struct rtl_priv *rtlpriv = adapter;
-	s32	undecorated_smoothed_pwdb = 0;
-	
-	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
-		undecorated_smoothed_pwdb = 
-			rtlpriv->dm.undecorated_smoothed_pwdb;
-	else /* associated entry pwdb */
-		undecorated_smoothed_pwdb =
-			rtlpriv->dm.undecorated_smoothed_pwdb;
-	return undecorated_smoothed_pwdb;
-}
-
-static bool halbtc_get(void *void_btcoexist, u8 get_type, void *out_buf)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	bool *bool_tmp = (bool*)out_buf;
-	int *s32_tmp = (int*)out_buf;
-	u32 *u32_tmp = (u32*)out_buf;
-	u8 *u8_tmp = (u8*)out_buf;
-	bool tmp = false;
-
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return false;
-	
-
-	switch (get_type){
-	case BTC_GET_BL_HS_OPERATION:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_HS_CONNECTING:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_CONNECTED:
-		if (rtlpriv->mac80211.link_state >= MAC80211_LINKED)
-			tmp = true;
-				
-		*bool_tmp = tmp;
-		break;
-	case BTC_GET_BL_WIFI_BUSY:
-		if(halbtc_is_wifi_busy(rtlpriv))
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_SCAN:
-		if (mac->act_scanning == true)
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_LINK:
-		if (mac->link_state == MAC80211_LINKING)
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_ROAM:	/*TODO*/
-		if (mac->link_state == MAC80211_LINKING)
-			*bool_tmp = true;
-		else
-			*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_4_WAY_PROGRESS:	/*TODO*/
-			*bool_tmp = false;
-
-		break;
-	case BTC_GET_BL_WIFI_UNDER_5G:
-		*bool_tmp = false; /*TODO*/
-		
-	case BTC_GET_BL_WIFI_DHCP:	/*TODO*/
-		break;
-	case BTC_GET_BL_WIFI_SOFTAP_IDLE:
-		*bool_tmp = true;
-		break;
-	case BTC_GET_BL_WIFI_SOFTAP_LINKING:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_IN_EARLY_SUSPEND:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_AP_MODE_ENABLE:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_BL_WIFI_ENABLE_ENCRYPTION:
-		if (NO_ENCRYPTION == rtlpriv->sec.pairwise_enc_algorithm)
-			*bool_tmp = false;
-		else
-			*bool_tmp = true;
-		break;
-	case BTC_GET_BL_WIFI_UNDER_B_MODE:
-		*bool_tmp = false; /*TODO*/
-		break;
-	case BTC_GET_BL_EXT_SWITCH:
-		*bool_tmp = false;
-		break;
-	case BTC_GET_S4_WIFI_RSSI:
-		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
-		break;
-	case BTC_GET_S4_HS_RSSI:	/*TODO*/
-		*s32_tmp = halbtc_get_wifi_rssi(rtlpriv);
-		break;
-	case BTC_GET_U4_WIFI_BW:
-		*u32_tmp = halbtc_get_wifi_bw(btcoexist);
-		break;
-	case BTC_GET_U4_WIFI_TRAFFIC_DIRECTION:
-		if (halbtc_is_wifi_uplink(rtlpriv))
-			*u32_tmp = BTC_WIFI_TRAFFIC_TX;
-		else
-			*u32_tmp = BTC_WIFI_TRAFFIC_RX;
-		break;
-	case BTC_GET_U4_WIFI_FW_VER:
-		*u32_tmp = rtlhal->fw_version;
-		break;
-	case BTC_GET_U4_BT_PATCH_VER:
-		*u32_tmp = halbtc_get_bt_patch_version(btcoexist);
-		break;
-	case BTC_GET_U1_WIFI_DOT11_CHNL:
-		*u8_tmp = rtlphy->current_channel;
-		break;
-	case BTC_GET_U1_WIFI_CENTRAL_CHNL:
-		*u8_tmp = halbtc_get_wifi_central_chnl(btcoexist);
-		break;
-	case BTC_GET_U1_WIFI_HS_CHNL:
-		*u8_tmp = 1;/* BT_OperateChnl(rtlpriv); */
-		break;
-	case BTC_GET_U1_MAC_PHY_MODE:
-		*u8_tmp = BTC_MP_UNKNOWN;
-		break;
-	case BTC_GET_U1_AP_NUM:
-		/* driver don't know AP num in Linux, 
-		 * So, the return value here is not right */
-		*u8_tmp = 1;/* pDefMgntInfo->NumBssDesc4Query; */
-		break;
-
-	/************* 1Ant **************/
-	case BTC_GET_U1_LPS_MODE:
-		*u8_tmp = btcoexist->pwr_mode_val[0];
-		break;
-					
-	default:
-		break;
-	}
-	
-	return true;
-}
-
-static bool halbtc_set(void *void_btcoexist, u8 set_type, void *in_buf)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)void_btcoexist;
-	bool *bool_tmp = (bool *)in_buf;
-	u8 *u8_tmp = (u8 *)in_buf;
-	u32 *u32_tmp = (u32 *)in_buf;
-	
-	
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return false;
-
-	switch (set_type) {
-	/* set some bool type variables. */
-	case BTC_SET_BL_BT_DISABLE:
-		btcoexist->bt_info.bt_disabled = *bool_tmp;
-		break;
-	case BTC_SET_BL_BT_TRAFFIC_BUSY:
-		btcoexist->bt_info.bt_busy = *bool_tmp;
-		break;
-	case BTC_SET_BL_BT_LIMITED_DIG:
-		btcoexist->bt_info.limited_dig = *bool_tmp;
-		break;
-	case BTC_SET_BL_FORCE_TO_ROAM:
-		btcoexist->bt_info.force_to_roam = *bool_tmp;
-		break;
-	case BTC_SET_BL_TO_REJ_AP_AGG_PKT:
-		btcoexist->bt_info.reject_agg_pkt = *bool_tmp;
-		break;
-	case BTC_SET_BL_BT_CTRL_AGG_SIZE:
-		btcoexist->bt_info.b_bt_ctrl_buf_size = *bool_tmp;
-		break;
-	case BTC_SET_BL_INC_SCAN_DEV_NUM:
-		btcoexist->bt_info.increase_scan_dev_num = *bool_tmp;
-		break;
-		/* set some u1Byte type variables. */
-	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON:
-		btcoexist->bt_info.rssi_adjust_for_agc_table_on = *u8_tmp;
-		break;
-	case BTC_SET_U1_AGG_BUF_SIZE:
-		btcoexist->bt_info.agg_buf_size = *u8_tmp;
-		break;
-		/* the following are some action which will be triggered */
-	case BTC_SET_ACT_GET_BT_RSSI:
-		/*BTHCI_SendGetBtRssiEvent(rtlpriv);*/
-		break;
-	case BTC_SET_ACT_AGGREGATE_CTRL:
-		halbtc_aggregation_check();
-		break;
-
-		/* 1Ant */
-	case BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE:
-		btcoexist->bt_info.rssi_adjust_for_1ant_coex_type = *u8_tmp;
-		break;
-	case BTC_SET_UI_SCAN_SIG_COMPENSATION:
-	/*	rtlpriv->mlmepriv.scan_compensation = *u8_tmp;  */
-		break;
-	case BTC_SET_U1_1ANT_LPS:
-		btcoexist->bt_info.lps_1ant = *u8_tmp;
-		break;
-	case BTC_SET_U1_1ANT_RPWM:
-		btcoexist->bt_info.rpwm_1ant = *u8_tmp;
-		break;
-	/* the following are some action which will be triggered  */
-	case BTC_SET_ACT_LEAVE_LPS:
-		halbtc_leave_lps(btcoexist);
-		break;
-	case BTC_SET_ACT_ENTER_LPS:
-		halbtc_enter_lps(btcoexist);
-		break;
-	case BTC_SET_ACT_NORMAL_LPS:
-		halbtc_normal_lps(btcoexist);
-		break;
-	case BTC_SET_ACT_DISABLE_LOW_POWER:
-		halbtc_disable_low_power();
-		break;
-	case BTC_SET_ACT_UPDATE_ra_mask:
-		btcoexist->bt_info.ra_mask = *u32_tmp;
-		break;
-	case BTC_SET_ACT_SEND_MIMO_PS:
-		break;
-	case BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT:
-		btcoexist->bt_info.force_exec_pwr_cmd_cnt++;
-		break;
-	case BTC_SET_ACT_CTRL_BT_INFO: /*wait for 8812/8821*/
-		break;
-	case BTC_SET_ACT_CTRL_BT_COEX:
-		break;
-	default:
-		break;
-	}
-	
-	return true;
-}
-
-static void halbtc_display_coex_statistics(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_bt_link_info(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_bt_fw_info(struct btc_coexist *btcoexist)
-{
-}
-
-static void halbtc_display_fw_pwr_mode_cmd(struct btc_coexist *btcoexist)
-{
-}
-
-/************************************************************
- *		IO related function
- ************************************************************/
-static u8 halbtc_read_1byte(void *bt_context, u32 reg_addr)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	
-	return	rtl_read_byte(rtlpriv, reg_addr);
-}
-
-
-static u16 halbtc_read_2byte(void *bt_context, u32 reg_addr)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return	rtl_read_word(rtlpriv, reg_addr);
-}
-
-
-static u32 halbtc_read_4byte(void *bt_context, u32 reg_addr)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	
-	return	rtl_read_dword(rtlpriv, reg_addr);
-}
-
-
-static void halbtc_write_1byte(void *bt_context, u32 reg_addr, u8 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_write_byte(rtlpriv, reg_addr, data);
-}
-
-static void halbtc_bitmask_write_1byte(void *bt_context, u32 reg_addr,
-				u8 bit_mask, u8 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	u8 original_value, bit_shift = 0;
-	u8 i;
-
-	if (bit_mask != MASKDWORD) {/*if not "double word" write*/
-		original_value = rtl_read_byte(rtlpriv, reg_addr);	
-		for (i=0; i<=7; i++) {
-			if((bit_mask>>i)&0x1)
-				break;
-		}
-		bit_shift = i;
-		data = (original_value & (~bit_mask)) | 
-			((data << bit_shift) & bit_mask);
-	}
-	rtl_write_byte(rtlpriv, reg_addr, data);
-}
-
-
-static void halbtc_write_2byte(void *bt_context, u32 reg_addr, u16 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_write_word(rtlpriv, reg_addr, data);
-}
-
-
-static void halbtc_write_4byte(void *bt_context, u32 reg_addr, u32 data)
-{
-	struct btc_coexist *btcoexist = 
-		(struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_write_dword(rtlpriv, reg_addr, data);
-}
-
-static void halbtc_set_bbreg(void *bt_context, u32 reg_addr,
-				u32 bit_mask, u32 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_set_bbreg(rtlpriv->mac80211.hw, reg_addr, bit_mask, data);
-}
-
-
-static u32 halbtc_get_bbreg(void *bt_context, u32 reg_addr, u32 bit_mask)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return rtl_get_bbreg(rtlpriv->mac80211.hw,reg_addr, bit_mask);
-}
-
-
-static void halbtc_set_rfreg(void *bt_context, u8 rf_path, u32 reg_addr,
-		      u32 bit_mask, u32 data)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	rtl_set_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask, data);
-}
-
-
-static u32 halbtc_get_rfreg(void *bt_context, u8 rf_path,
-			u32 reg_addr, u32 bit_mask)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-
-	return rtl_get_rfreg(rtlpriv->mac80211.hw, rf_path, reg_addr, bit_mask);
-}
-
-
-static void halbtc_fill_h2c_cmd(void *bt_context, u8 element_id,
-			 u32 cmd_len, u8 *cmd_buf)
-{
-	struct btc_coexist *btcoexist = (struct btc_coexist *)bt_context;
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	
-	rtlpriv->cfg->ops->fill_h2c_cmd(rtlpriv->mac80211.hw, element_id, 
-					cmd_len, cmd_buf);
-}
-
-static void halbtc_display_dbg_msg(void *bt_context, u8 disp_type)
-{
-	struct btc_coexist *btcoexist =	(struct btc_coexist *)bt_context;
-	switch (disp_type) {
-	case BTC_DBG_DISP_COEX_STATISTICS:
-		halbtc_display_coex_statistics(btcoexist);
-		break;
-	case BTC_DBG_DISP_BT_LINK_INFO:
-		halbtc_display_bt_link_info(btcoexist);
-		break;
-	case BTC_DBG_DISP_BT_FW_VER:
-		halbtc_display_bt_fw_info(btcoexist);
-		break;
-	case BTC_DBG_DISP_FW_PWR_MODE_CMD:
-		halbtc_display_fw_pwr_mode_cmd(btcoexist);
-		break;
-	default:
-		break;
-	}
-}
-
-/*****************************************************************
- *         Extern functions called by other module
- *****************************************************************/
-bool exhalbtc_initlize_variables(struct rtl_priv *adapter)
-{
-	struct btc_coexist *btcoexist = &gl_bt_coexist;
-
-	btcoexist->statistics.cnt_bind++;
-	
-	halbtc_dbg_init();
-
-	if (btcoexist->binded)
-		return false;
-	else
-		btcoexist->binded = true;
-
-	btcoexist->chip_interface = BTC_INTF_UNKNOWN;
-
-	if (NULL == btcoexist->adapter)
-		btcoexist->adapter = adapter;
-
-	btcoexist->stack_info.profile_notified = false;
-
-	btcoexist->btc_read_1byte = halbtc_read_1byte;
-	btcoexist->btc_write_1byte = halbtc_write_1byte;
-	btcoexist->btc_write_1byte_bitmask = halbtc_bitmask_write_1byte;
-	btcoexist->btc_read_2byte = halbtc_read_2byte;
-	btcoexist->btc_write_2byte = halbtc_write_2byte;
-	btcoexist->btc_read_4byte = halbtc_read_4byte;
-	btcoexist->btc_write_4byte = halbtc_write_4byte;
-
-	btcoexist->btc_set_bb_reg = halbtc_set_bbreg;
-	btcoexist->btc_get_bb_reg = halbtc_get_bbreg;
-
-	btcoexist->btc_set_rf_reg = halbtc_set_rfreg;
-	btcoexist->btc_get_rf_reg = halbtc_get_rfreg;
-
-	btcoexist->btc_fill_h2c = halbtc_fill_h2c_cmd;
-	btcoexist->btc_disp_dbg_msg = halbtc_display_dbg_msg;
-	
-	btcoexist->btc_get = halbtc_get;
-	btcoexist->btc_set = halbtc_set;
-
-	btcoexist->cli_buf = &btc_dbg_buf[0];
-
-	btcoexist->bt_info.b_bt_ctrl_buf_size = false;
-	btcoexist->bt_info.agg_buf_size = 5;
-
-	btcoexist->bt_info.increase_scan_dev_num = false;
-	return true;
-}
-
-void exhalbtc_init_hw_config(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->statistics.cnt_init_hw_config++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_init_hwconfig(btcoexist);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_init_hwconfig(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_init_hwconfig(btcoexist);
-	}
-	
-}
-
-void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->statistics.cnt_init_coex_dm++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_init_coex_dm(btcoexist);		
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_init_coex_dm(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_init_coex_dm(btcoexist);
-	}
-	
-	btcoexist->initilized = true;
-}
-
-void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 ips_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_ips_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (ERFOFF == type)
-		ips_type = BTC_IPS_ENTER;
-	else
-		ips_type = BTC_IPS_LEAVE;
-
-	halbtc_leave_low_power();
-	
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_ips_notify(btcoexist, ips_type);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_ips_notify(btcoexist, ips_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_ips_notify(btcoexist, ips_type);
-	}
-
-	halbtc_nomal_low_power();
-}
-
-void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 lps_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_lps_notify++;
-	if (btcoexist->manual_control)
-		return;
-	
-	if (EACTIVE == type)
-		lps_type = BTC_LPS_DISABLE;
-	else
-		lps_type = BTC_LPS_ENABLE;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_lps_notify(btcoexist, lps_type);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_lps_notify(btcoexist, lps_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_lps_notify(btcoexist, lps_type);
-	}
-}
-
-void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 scan_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_scan_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (type)
-		scan_type = BTC_SCAN_START;
-	else
-		scan_type = BTC_SCAN_FINISH;
-
-	halbtc_leave_low_power();
-	
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_scan_notify(btcoexist, scan_type);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_scan_notify(btcoexist, scan_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_scan_notify(btcoexist, scan_type);
-	}
-
-	halbtc_nomal_low_power();
-}
-
-void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 asso_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_connect_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (action)
-		asso_type = BTC_ASSOCIATE_START;
-	else
-		asso_type = BTC_ASSOCIATE_FINISH;
-
-	halbtc_leave_low_power();
-	
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_connect_notify(btcoexist, asso_type);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_connect_notify(btcoexist, asso_type);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_connect_notify(btcoexist, asso_type);
-	}
-
-	halbtc_nomal_low_power();
-}
-
-void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist, 
-				 enum rt_media_status media_status)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 status;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_media_status_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	if (RT_MEDIA_CONNECT == media_status)
-		status = BTC_MEDIA_CONNECT;
-	else
-		status = BTC_MEDIA_DISCONNECT;
-
-	halbtc_leave_low_power();
-	
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_media_status_notify(btcoexist, status);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_media_status_notify(btcoexist, status);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_media_status_notify(btcoexist, status);
-	}
-
-	halbtc_nomal_low_power();
-}
-
-void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type)
-{
-	u8 packet_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_special_packet_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	/*if(PACKET_DHCP == pkt_type)*/
-		packet_type = BTC_PACKET_DHCP;
-	/*else if(PACKET_EAPOL == pkt_type)
-		packet_type = BTC_PACKET_EAPOL;
-	else
-		packet_type = BTC_PACKET_UNKNOWN;*/
-
-	halbtc_leave_low_power();
-
-	if (btcoexist->board_info.btdm_ant_num == 2)
-		ex_halbtc8723b2ant_special_packet_notify(btcoexist,
-							 packet_type);
-	else if (btcoexist->board_info.btdm_ant_num == 1)
-		ex_halbtc8723b1ant_special_packet_notify(btcoexist,
-							 packet_type);
-
-	halbtc_nomal_low_power();
-}
-
-void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist, 
-			     u8 *tmp_buf, u8 length)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_bt_info_notify++;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_bt_info_notify(btcoexist, tmp_buf, length);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_bt_info_notify(btcoexist, tmp_buf, length);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-	//	ex_halbtc8192e2ant_bt_info_notify(btcoexist, tmp_buf, length);
-	}
-}
-
-void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type)
-{
-	u8 stack_op_type;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_stack_operation_notify++;
-	if (btcoexist->manual_control)
-		return;
-
-	stack_op_type = BTC_STACK_OP_NONE;
-}
-
-void exhalbtc_halt_notify(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_halt_notify(btcoexist);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_halt_notify(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_halt_notify(btcoexist);
-	}
-}
-
-void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state)
-{
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-}
-
-void exhalbtc_periodical(struct btc_coexist *btcoexist)
-{
-	struct rtl_priv *rtlpriv = btcoexist->adapter;
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_periodical++;
-
-	halbtc_leave_low_power();
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (btcoexist->board_info.btdm_ant_num == 2)
-			ex_halbtc8723b2ant_periodical(btcoexist);
-		else if(btcoexist->board_info.btdm_ant_num == 1)
-			ex_halbtc8723b1ant_periodical(btcoexist);
-	} else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE) {
-		ex_halbtc8192e2ant_periodical(btcoexist);
-	}
-
-	halbtc_nomal_low_power();
-}
-
-void exhalbtc_dbg_control(struct btc_coexist *btcoexist, 
-			  u8 code, u8 len, u8 *data)
-{
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	btcoexist->statistics.cnt_dbg_ctrl++;
-}
-
-void exhalbtc_stack_update_profile_info(void)
-{
-}
-
-void exhalbtc_update_min_bt_rssi(char bt_rssi)
-{
-	struct btc_coexist *btcoexist = &gl_bt_coexist;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->stack_info.min_bt_rssi = bt_rssi;
-}
-
-
-void exhalbtc_set_hci_version(u16 hci_version)
-{
-	struct btc_coexist *btcoexist = &gl_bt_coexist;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->stack_info.hci_version = hci_version;
-}
-
-void exhalbtc_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version) 
-{
-	struct btc_coexist *btcoexist = &gl_bt_coexist;
-
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-
-	btcoexist->bt_info.bt_real_fw_ver = bt_patch_version;
-	btcoexist->bt_info.bt_hci_ver = bt_hci_version;
-}
-
-void exhalbtc_set_bt_exist(bool bt_exist)
-{
-	gl_bt_coexist.board_info.bt_exist = bt_exist;
-}
-
-void exhalbtc_set_chip_type(u8 chip_type)
-{
-	switch (chip_type) {
-	default:
-	case BT_2WIRE:
-	case BT_ISSC_3WIRE:
-	case BT_ACCEL:
-	case BT_RTL8756:
-		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_UNDEF;
-		break;
-	case BT_CSR_BC4:
-		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC4;
-		break;
-	case BT_CSR_BC8:
-		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_CSR_BC8;
-		break;
-	case BT_RTL8723A:
-		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723A;
-		break;
-	case BT_RTL8821A:
-		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8821;
-		break;
-	case BT_RTL8723B:
-		gl_bt_coexist.board_info.bt_chip_type = BTC_CHIP_RTL8723B;
-		break;
-	}
-}
-
-void exhalbtc_set_ant_num(u8 type, u8 ant_num)
-{
-	if (BT_COEX_ANT_TYPE_PG == type) {
-		gl_bt_coexist.board_info.pg_ant_num = ant_num;
-		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
-	} else if (BT_COEX_ANT_TYPE_ANTDIV == type) {
-		gl_bt_coexist.board_info.btdm_ant_num = ant_num;
-	}
-}
-
-void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist)
-{
-	if (!halbtc_is_bt_coexist_available(btcoexist))
-		return;
-	
-	if (btcoexist->board_info.btdm_ant_num == 2)
-		ex_halbtc8723b2ant_display_coex_info(btcoexist);
-	else if (btcoexist->board_info.btdm_ant_num == 1)
-		ex_halbtc8723b1ant_display_coex_info(btcoexist);
-}
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.h b/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.h
deleted file mode 100644
index fd233cc85a5b..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/halbtcoutsrc.h
+++ /dev/null
@@ -1,549 +0,0 @@
-#ifndef	__HALBTC_OUT_SRC_H__
-#define __HALBTC_OUT_SRC_H__
-
-#include	"../wifi.h"
-
-#define		NORMAL_EXEC				false
-#define		FORCE_EXEC				true
-
-#define		BTC_RF_A				RF90_PATH_A
-#define		BTC_RF_B				RF90_PATH_B
-#define		BTC_RF_C				RF90_PATH_C
-#define		BTC_RF_D				RF90_PATH_D
-
-#define		BTC_SMSP				SINGLEMAC_SINGLEPHY
-#define		BTC_DMDP				DUALMAC_DUALPHY
-#define		BTC_DMSP				DUALMAC_SINGLEPHY
-#define		BTC_MP_UNKNOWN				0xff
-
-#define 	IN
-#define 	OUT
-
-#define 	BT_TMP_BUF_SIZE 			100
-
-#define		BT_COEX_ANT_TYPE_PG			0
-#define		BT_COEX_ANT_TYPE_ANTDIV			1
-#define		BT_COEX_ANT_TYPE_DETECTED		2
-
-#define		BTC_MIMO_PS_STATIC			0
-#define		BTC_MIMO_PS_DYNAMIC			1
-
-#define		BTC_RATE_DISABLE			0
-#define		BTC_RATE_ENABLE				1
-
-/* single Antenna definition */
-#define		BTC_ANT_PATH_WIFI			0
-#define		BTC_ANT_PATH_BT				1
-#define		BTC_ANT_PATH_PTA			2
-/* dual Antenna definition */
-#define		BTC_ANT_WIFI_AT_MAIN			0
-#define		BTC_ANT_WIFI_AT_AUX			1
-/* coupler Antenna definition */
-#define		BTC_ANT_WIFI_AT_CPL_MAIN		0
-#define		BTC_ANT_WIFI_AT_CPL_AUX			1
-
-enum btc_chip_interface{
-	BTC_INTF_UNKNOWN	= 0,
-	BTC_INTF_PCI		= 1,
-	BTC_INTF_USB		= 2,
-	BTC_INTF_SDIO		= 3,
-	BTC_INTF_GSPI		= 4,
-	BTC_INTF_MAX
-};
-
-enum btc_chip_type{
-	BTC_CHIP_UNDEF		= 0,
-	BTC_CHIP_CSR_BC4	= 1,
-	BTC_CHIP_CSR_BC8	= 2,
-	BTC_CHIP_RTL8723A 	= 3,
-	BTC_CHIP_RTL8821	= 4,
-	BTC_CHIP_RTL8723B 	= 5,
-	BTC_CHIP_MAX
-};
-
-enum btc_msg_type{
-	BTC_MSG_INTERFACE	= 0x0,
-	BTC_MSG_ALGORITHM	= 0x1,
-	BTC_MSG_MAX
-};
-
-extern u32 btc_dbg_type[];
-
-/* following is for BTC_MSG_INTERFACE */
-#define		INTF_INIT				BIT0
-#define		INTF_NOTIFY				BIT2
-
-/* following is for BTC_ALGORITHM */
-#define		ALGO_BT_RSSI_STATE			BIT0
-#define		ALGO_WIFI_RSSI_STATE			BIT1
-#define		ALGO_BT_MONITOR				BIT2
-#define		ALGO_TRACE				BIT3
-#define		ALGO_TRACE_FW				BIT4
-#define		ALGO_TRACE_FW_DETAIL			BIT5
-#define		ALGO_TRACE_FW_EXEC			BIT6
-#define		ALGO_TRACE_SW				BIT7
-#define		ALGO_TRACE_SW_DETAIL			BIT8
-#define		ALGO_TRACE_SW_EXEC			BIT9
-
-
-
-#define	CL_SPRINTF	snprintf
-#define	CL_PRINTF(buf)	printk("%s", buf)
-
-#define	BTC_PRINT(dbgtype, dbgflag, printstr, ...)		\
-	do { 							\
-		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {\
-			printk(printstr, ##__VA_ARGS__);	\
-		}						\
-	} while(0)
-
-#define	BTC_PRINT_F(dbgtype, dbgflag, printstr, ...)		\
-	do {							\
-		if (unlikely(btc_dbg_type[dbgtype] & dbgflag)) {\
-			printk(KERN_DEBUG "%s: ", __func__);	\
-			printk(printstr, ##__VA_ARGS__);	\
-		}						\
-	} while(0)
-
-#define	BTC_PRINT_ADDR(dbgtype, dbgflag, printstr, _ptr)	\
-	do { 							\
-		if(unlikely(btc_dbg_type[dbgtype] & dbgflag)) {	\
-			int __i;				\
-			u8* __ptr = (u8*)_Ptr;			\
-			printk printstr;			\
-			for( __i = 0; __i < 6; __i++ )		\
-				printk("%02X%s", __ptr[__i], (__i==5)?"":"-");\
-			printk(KERN_DEBUG "\n");		\
-		}\
-	} while(0)
-
-#define BTC_PRINT_DATA(dbgtype, dbgflag, _titlestring, _hexdata, _hexdatalen) \
-	do {								\
-		if(unlikely(btc_dbg_type[dbgtype] & dbgflag) )	{ 	\
-			int __i;					\
-			u8 *__ptr = (u8*)_hexdata;			\
-			printk(_titlestring);				\
-			for( __i = 0; __i < (int)_hexdatalen; __i++ ) {	\
-				printk("%02X%s", __ptr[__i], (((__i + 1) % 4) \
-							== 0)?"  ":" ");\
-				if (((__i + 1) % 16) == 0)		\
-					printk("\n");			\
-			}						\
-			printk(KERN_DEBUG "\n");			\
-		} 							\
-	} while(0)
-
-
-#define	BTC_RSSI_HIGH(_rssi_) \
-	((_rssi_==BTC_RSSI_STATE_HIGH || _rssi_==BTC_RSSI_STATE_STAY_HIGH) ? \
-	true : false)
-
-#define	BTC_RSSI_MEDIUM(_rssi_)	\
-	((_rssi_==BTC_RSSI_STATE_MEDIUM || _rssi_==BTC_RSSI_STATE_STAY_MEDIUM) \
-	? true : false)
-
-#define	BTC_RSSI_LOW(_rssi_) \
-	((_rssi_==BTC_RSSI_STATE_LOW || _rssi_==BTC_RSSI_STATE_STAY_LOW) ? \
-	true : false)
-
-
-enum btc_power_save_type {
-	BTC_PS_WIFI_NATIVE = 0,
-	BTC_PS_LPS_ON = 1,
-	BTC_PS_LPS_OFF = 2,
-	BTC_PS_LPS_MAX
-};
-
-struct btc_board_info {
-	/* The following is some board information */
-	u8 bt_chip_type;
-	u8 pg_ant_num;	/* pg ant number */
-	u8 btdm_ant_num;	/* ant number for btdm */
-	u8 btdm_ant_pos;
-	bool bt_exist;
-};
-
-enum btc_dbg_opcode{
-	BTC_DBG_SET_COEX_NORMAL = 0x0,
-	BTC_DBG_SET_COEX_WIFI_ONLY = 0x1,
-	BTC_DBG_SET_COEX_BT_ONLY = 0x2,
-	BTC_DBG_MAX
-};
-
-enum btc_rssi_state{
-	BTC_RSSI_STATE_HIGH = 0x0,
-	BTC_RSSI_STATE_MEDIUM = 0x1,
-	BTC_RSSI_STATE_LOW = 0x2,
-	BTC_RSSI_STATE_STAY_HIGH = 0x3,
-	BTC_RSSI_STATE_STAY_MEDIUM = 0x4,
-	BTC_RSSI_STATE_STAY_LOW = 0x5,
-	BTC_RSSI_MAX
-};
-
-enum btc_wifi_role{
-	BTC_ROLE_STATION = 0x0,
-	BTC_ROLE_AP = 0x1,
-	BTC_ROLE_IBSS = 0x2,
-	BTC_ROLE_HS_MODE = 0x3,
-	BTC_ROLE_MAX
-};
-
-enum btc_wifi_bw_mode{
-	BTC_WIFI_BW_LEGACY = 0x0,
-	BTC_WIFI_BW_HT20 = 0x1,
-	BTC_WIFI_BW_HT40 = 0x2,
-	BTC_WIFI_BW_MAX
-};
-
-enum btc_wifi_traffic_dir{
-	BTC_WIFI_TRAFFIC_TX = 0x0,
-	BTC_WIFI_TRAFFIC_RX = 0x1,
-	BTC_WIFI_TRAFFIC_MAX
-};
-
-enum btc_wifi_pnp{
-	BTC_WIFI_PNP_WAKE_UP = 0x0,
-	BTC_WIFI_PNP_SLEEP = 0x1,
-	BTC_WIFI_PNP_MAX
-};
-
-
-enum btc_get_type{
-	/* type bool */
-	BTC_GET_BL_HS_OPERATION,
-	BTC_GET_BL_HS_CONNECTING,
-	BTC_GET_BL_WIFI_CONNECTED,
-	BTC_GET_BL_WIFI_BUSY,
-	BTC_GET_BL_WIFI_SCAN,
-	BTC_GET_BL_WIFI_LINK,
-	BTC_GET_BL_WIFI_DHCP,
-	BTC_GET_BL_WIFI_SOFTAP_IDLE,
-	BTC_GET_BL_WIFI_SOFTAP_LINKING,
-	BTC_GET_BL_WIFI_IN_EARLY_SUSPEND,
-	BTC_GET_BL_WIFI_ROAM,
-	BTC_GET_BL_WIFI_4_WAY_PROGRESS,
-	BTC_GET_BL_WIFI_UNDER_5G,
-	BTC_GET_BL_WIFI_AP_MODE_ENABLE,
-	BTC_GET_BL_WIFI_ENABLE_ENCRYPTION,
-	BTC_GET_BL_WIFI_UNDER_B_MODE,
-	BTC_GET_BL_EXT_SWITCH,
-
-	/* type s4Byte */
-	BTC_GET_S4_WIFI_RSSI,
-	BTC_GET_S4_HS_RSSI,
-
-	/* type u32 */
-	BTC_GET_U4_WIFI_BW,
-	BTC_GET_U4_WIFI_TRAFFIC_DIRECTION,
-	BTC_GET_U4_WIFI_FW_VER,
-	BTC_GET_U4_BT_PATCH_VER,
-
-	/* type u1Byte */
-	BTC_GET_U1_WIFI_DOT11_CHNL,
-	BTC_GET_U1_WIFI_CENTRAL_CHNL,
-	BTC_GET_U1_WIFI_HS_CHNL,
-	BTC_GET_U1_MAC_PHY_MODE,
-	BTC_GET_U1_AP_NUM,
-
-	/* for 1Ant */
-	BTC_GET_U1_LPS_MODE,
-	BTC_GET_BL_BT_SCO_BUSY,
-
-	/* for test mode */
-	BTC_GET_DRIVER_TEST_CFG,
-#if 0
-	BTC_GET_U1_LPS,
-	BTC_GET_U1_RPWM,
-#endif
-	BTC_GET_MAX
-};
-
-
-enum btc_set_type{
-	/* type bool */
-	BTC_SET_BL_BT_DISABLE,
-	BTC_SET_BL_BT_TRAFFIC_BUSY,
-	BTC_SET_BL_BT_LIMITED_DIG,
-	BTC_SET_BL_FORCE_TO_ROAM,
-	BTC_SET_BL_TO_REJ_AP_AGG_PKT,
-	BTC_SET_BL_BT_CTRL_AGG_SIZE,
-	BTC_SET_BL_INC_SCAN_DEV_NUM,
-
-	/* type u1Byte */
-	BTC_SET_U1_RSSI_ADJ_VAL_FOR_AGC_TABLE_ON,
-	BTC_SET_U1_RSSI_ADJ_VAL_FOR_1ANT_COEX_TYPE,
-	BTC_SET_UI_SCAN_SIG_COMPENSATION,
-	BTC_SET_U1_AGG_BUF_SIZE,
-
-	/* type trigger some action */
-	BTC_SET_ACT_GET_BT_RSSI,
-	BTC_SET_ACT_AGGREGATE_CTRL,
-
-	/********* for 1Ant **********/
-	/* type bool */
-	BTC_SET_BL_BT_SCO_BUSY,
-	/* type u1Byte */
-	BTC_SET_U1_1ANT_LPS,
-	BTC_SET_U1_1ANT_RPWM,
-	/* type trigger some action */
-	BTC_SET_ACT_LEAVE_LPS,
-	BTC_SET_ACT_ENTER_LPS,
-	BTC_SET_ACT_NORMAL_LPS,
-	BTC_SET_ACT_INC_FORCE_EXEC_PWR_CMD_CNT,
-	BTC_SET_ACT_DISABLE_LOW_POWER,
-	BTC_SET_ACT_UPDATE_ra_mask,
-	BTC_SET_ACT_SEND_MIMO_PS,
-	/* BT Coex related */
-	BTC_SET_ACT_CTRL_BT_INFO,
-	BTC_SET_ACT_CTRL_BT_COEX,
-	/***************************/
-	BTC_SET_MAX
-};
-
-enum btc_dbg_disp_type{
-	BTC_DBG_DISP_COEX_STATISTICS = 0x0,
-	BTC_DBG_DISP_BT_LINK_INFO = 0x1,
-	BTC_DBG_DISP_BT_FW_VER = 0x2,
-	BTC_DBG_DISP_FW_PWR_MODE_CMD = 0x3,
-	BTC_DBG_DISP_MAX
-};
-
-enum btc_notify_type_ips{
-	BTC_IPS_LEAVE = 0x0,
-	BTC_IPS_ENTER = 0x1,
-	BTC_IPS_MAX
-};
-
-enum btc_notify_type_lps{
-	BTC_LPS_DISABLE = 0x0,
-	BTC_LPS_ENABLE = 0x1,
-	BTC_LPS_MAX
-};
-
-enum btc_notify_type_scan{
-	BTC_SCAN_FINISH = 0x0,
-	BTC_SCAN_START = 0x1,
-	BTC_SCAN_MAX
-};
-
-enum btc_notify_type_associate{
-	BTC_ASSOCIATE_FINISH = 0x0,
-	BTC_ASSOCIATE_START = 0x1,
-	BTC_ASSOCIATE_MAX
-};
-
-enum btc_notify_type_media_status{
-	BTC_MEDIA_DISCONNECT = 0x0,
-	BTC_MEDIA_CONNECT = 0x1,
-	BTC_MEDIA_MAX
-};
-
-enum btc_notify_type_special_packet{
-	BTC_PACKET_UNKNOWN = 0x0,
-	BTC_PACKET_DHCP = 0x1,
-	BTC_PACKET_ARP = 0x2,
-	BTC_PACKET_EAPOL = 0x3,
-	BTC_PACKET_MAX
-};
-
-enum btc_notify_type_stack_operation{
-	BTC_STACK_OP_NONE = 0x0,
-	BTC_STACK_OP_INQ_PAGE_PAIR_START = 0x1,
-	BTC_STACK_OP_INQ_PAGE_PAIR_FINISH = 0x2,
-	BTC_STACK_OP_MAX
-};
-
-
-typedef u8 (*bfp_btc_r1)(void *btc_context, u32 reg_addr);
-
-typedef u16 (*bfp_btc_r2)(void *btc_context, u32 reg_addr);
-
-typedef u32 (*bfp_btc_r4)(void *btc_context, u32 reg_addr);
-
-typedef void (*bfp_btc_w1)(void *btc_context, u32 reg_addr, u8 data);
-
-typedef void (*bfp_btc_w1_bit_mak)(void *btc_context, u32 reg_addr,
-				   u8 bit_mask, u8 data1b);
-
-typedef void (*bfp_btc_w2)(void *btc_context, u32 reg_addr, u16 data);
-
-typedef void (*bfp_btc_w4)(void *btc_context, u32 reg_addr, u32 data);
-
-typedef void (*bfp_btc_wr_1byte_bit_mask)(void *btc_context, u32 reg_addr,
-					  u8 bit_mask, u8 data);
-
-typedef void (*bfp_btc_set_bb_reg)(void *btc_context, u32 reg_addr,
-				   u32 bit_mask, u32 data);
-
-typedef u32 (*bfp_btc_get_bb_reg)(void *btc_context, u32 reg_addr,
-				  u32 bit_mask);
-
-typedef void (*bfp_btc_set_rf_reg)(void *btc_context, u8 rf_path, u32 reg_addr,
-				   u32 bit_mask, u32 data);
-
-typedef u32 (*bfp_btc_get_rf_reg)(void *btc_context, u8 rf_path,
-				  u32 reg_addr, u32 bit_mask);
-
-typedef void (*bfp_btc_fill_h2c)(void *btc_context, u8 element_id,
-				 u32 cmd_len, u8 *cmd_buffer);
-
-typedef	bool (*bfp_btc_get)(void *btcoexist, u8 get_type, void *out_buf);
-
-typedef	bool (*bfp_btc_set)(void *btcoexist, u8 set_type, void *in_buf);
-
-typedef void (*bfp_btc_disp_dbg_msg)(void *btcoexist, u8 disp_type);
-
-struct btc_bt_info {
-	bool bt_disabled;
-	u8 rssi_adjust_for_agc_table_on;
-	u8 rssi_adjust_for_1ant_coex_type;
-	bool bt_busy;
-	u8 agg_buf_size;
-	bool limited_dig;
-	bool reject_agg_pkt;
-	bool b_bt_ctrl_buf_size;
-	bool increase_scan_dev_num;
-	u16 bt_hci_ver;
-	u16 bt_real_fw_ver;
-	u8 bt_fw_ver;
-
-	/* the following is for 1Ant solution */
-	bool bt_ctrl_lps;
-	bool bt_pwr_save_mode;
-	bool bt_lps_on;
-	bool force_to_roam;
-	u8 force_exec_pwr_cmd_cnt;
-	u8 lps_1ant;
-	u8 rpwm_1ant;
-	u32 ra_mask;
-};
-
-struct btc_stack_info {
-	bool profile_notified;
-	u16 hci_version;	/* stack hci version */
-	u8 num_of_link;
-	bool bt_link_exist;
-	bool sco_exist;
-	bool acl_exist;
-	bool a2dp_exist;
-	bool hid_exist;
-	u8 num_of_hid;
-	bool pan_exist;
-	bool unknown_acl_exist;
-	char min_bt_rssi;
-};
-
-struct btc_statistics {
-	u32 cnt_bind;
-	u32 cnt_init_hw_config;
-	u32 cnt_init_coex_dm;
-	u32 cnt_ips_notify;
-	u32 cnt_lps_notify;
-	u32 cnt_scan_notify;
-	u32 cnt_connect_notify;
-	u32 cnt_media_status_notify;
-	u32 cnt_special_packet_notify;
-	u32 cnt_bt_info_notify;
-	u32 cnt_periodical;
-	u32 cnt_stack_operation_notify;
-	u32 cnt_dbg_ctrl;
-};
-
-struct btc_bt_link_info {
-	bool bt_link_exist;
-	bool sco_exist;
-	bool sco_only;
-	bool a2dp_exist;
-	bool a2dp_only;
-	bool hid_exist;
-	bool hid_only;
-	bool pan_exist;
-	bool pan_only;
-};
-
-enum btc_antenna_pos {
-	BTC_ANTENNA_AT_MAIN_PORT = 0x1,
-	BTC_ANTENNA_AT_AUX_PORT = 0x2,
-};
-
-struct btc_coexist {
-	/* make sure only one adapter can bind the data context  */
-	bool binded;
-	/* default adapter */
-	void *adapter;
-	struct btc_board_info board_info;
-	/* some bt info referenced by non-bt module */
-	struct btc_bt_info bt_info;
-	struct btc_stack_info stack_info;
-	enum btc_chip_interface	chip_interface;
-	struct btc_bt_link_info bt_link_info;
-
-	bool initilized;
-	bool stop_coex_dm;
-	bool manual_control;
-	u8 *cli_buf;
-	struct btc_statistics statistics;
-	u8 pwr_mode_val[10];
-
-	/* function pointers
-	 * io related */
-	bfp_btc_r1 btc_read_1byte;
-	bfp_btc_w1 btc_write_1byte;
-	bfp_btc_w1_bit_mak btc_write_1byte_bitmask;
-	bfp_btc_r2 btc_read_2byte;
-	bfp_btc_w2 btc_write_2byte;
-	bfp_btc_r4 btc_read_4byte;
-	bfp_btc_w4 btc_write_4byte;
-
-	bfp_btc_set_bb_reg btc_set_bb_reg;
-	bfp_btc_get_bb_reg btc_get_bb_reg;
-
-
-	bfp_btc_set_rf_reg btc_set_rf_reg;
-	bfp_btc_get_rf_reg btc_get_rf_reg;
-
-
-	bfp_btc_fill_h2c btc_fill_h2c;
-
-	bfp_btc_disp_dbg_msg btc_disp_dbg_msg;
-
-	bfp_btc_get btc_get;
-	bfp_btc_set btc_set;
-};
-
-bool halbtc_is_wifi_uplink(struct rtl_priv *adapter);
-
-
-extern struct btc_coexist gl_bt_coexist;
-
-bool exhalbtc_initlize_variables(struct rtl_priv* adapter);
-void exhalbtc_init_hw_config(struct btc_coexist *btcoexist);
-void exhalbtc_init_coex_dm(struct btc_coexist *btcoexist);
-void exhalbtc_ips_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc_lps_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc_scan_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc_connect_notify(struct btc_coexist *btcoexist, u8 action);
-void exhalbtc_mediastatus_notify(struct btc_coexist *btcoexist,
-				 enum rt_media_status media_status);
-void exhalbtc_special_packet_notify(struct btc_coexist *btcoexist, u8 pkt_type);
-void exhalbtc_bt_info_notify(struct btc_coexist *btcoexist, u8 *tmp_buf,
-			     u8 length);
-void exhalbtc_stack_operation_notify(struct btc_coexist *btcoexist, u8 type);
-void exhalbtc_halt_notify(struct btc_coexist *btcoexist);
-void exhalbtc_pnp_notify(struct btc_coexist *btcoexist, u8 pnp_state);
-void exhalbtc_periodical(struct btc_coexist *btcoexist);
-void exhalbtc_dbg_control(struct btc_coexist *btcoexist, u8 code, u8 len,
-			  u8 *data);
-void exhalbtc_stack_update_profile_info(void);
-void exhalbtc_set_hci_version(u16 hci_version);
-void exhalbtc_set_bt_patch_version(u16 bt_hci_version, u16 bt_patch_version);
-void exhalbtc_update_min_bt_rssi(char bt_rssi);
-void exhalbtc_set_bt_exist(bool bt_exist);
-void exhalbtc_set_chip_type(u8 chip_type);
-void exhalbtc_set_ant_num(u8 type, u8 ant_num);
-void exhalbtc_display_bt_coex_info(struct btc_coexist *btcoexist);
-void exhalbtc_signal_compensation(struct btc_coexist *btcoexist,
-				  u8 *rssi_wifi, u8 *rssi_bt);
-void exhalbtc_lps_leave(struct btc_coexist *btcoexist);
-void exhalbtc_low_wifi_traffic_notify(struct btc_coexist *btcoexist);
-#endif
diff --git a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
deleted file mode 100644
index 6be0acafba5d..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.c
+++ /dev/null
@@ -1,236 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include <linux/vmalloc.h>
-#include <linux/module.h>
-
-#include "rtl_btc.h"
-#include "halbt_precomp.h"
-
-static struct rtl_btc_ops rtl_btc_operation = {
-	.btc_init_variables = rtl_btc_init_variables,
-	.btc_init_hal_vars = rtl_btc_init_hal_vars,
-	.btc_init_hw_config = rtl_btc_init_hw_config,
-	.btc_ips_notify = rtl_btc_ips_notify,
-	.btc_scan_notify = rtl_btc_scan_notify,
-	.btc_connect_notify = rtl_btc_connect_notify,
-	.btc_mediastatus_notify = rtl_btc_mediastatus_notify,
-	.btc_periodical = rtl_btc_periodical,
-	.btc_halt_notify = rtl_btc_halt_notify,
-	.btc_btinfo_notify = rtl_btc_btinfo_notify,
-	.btc_is_limited_dig = rtl_btc_is_limited_dig,
-	.btc_is_disable_edca_turbo = rtl_btc_is_disable_edca_turbo,
-	.btc_is_bt_disabled = rtl_btc_is_bt_disabled,
-};
-
-
-void rtl_btc_init_variables(struct rtl_priv *rtlpriv)
-{
-
-	exhalbtc_initlize_variables(rtlpriv);
-}
-
-void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv)
-{
-	u8 ant_num;
-	u8 bt_exist;
-	u8 bt_type;
-	ant_num = rtl_get_hwpg_ant_num(rtlpriv);
-	RT_TRACE(COMP_INIT, DBG_DMESG, ("%s, antNum is %d\n", __func__, ant_num));
-
-	bt_exist = rtl_get_hwpg_bt_exist(rtlpriv);
-	RT_TRACE(COMP_INIT, DBG_DMESG, ("%s, bt_exist is %d\n", __func__, bt_exist));
-	exhalbtc_set_bt_exist(bt_exist);
-
-	bt_type = rtl_get_hwpg_bt_type(rtlpriv);
-	RT_TRACE(COMP_INIT, DBG_DMESG, ("%s, bt_type is %d\n", __func__, bt_type));
-	exhalbtc_set_chip_type(bt_type);
-
-	exhalbtc_set_ant_num(BT_COEX_ANT_TYPE_PG, ant_num);
-
-}
-
-
-void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv)
-{
-	exhalbtc_init_hw_config(&gl_bt_coexist);
-	exhalbtc_init_coex_dm(&gl_bt_coexist);
-}
-
-
-void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type)
-{
-	exhalbtc_ips_notify(&gl_bt_coexist, type);
-}
-
-
-void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype)
-{
-	exhalbtc_scan_notify(&gl_bt_coexist, scantype);
-}
-
-
-void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action)
-{
-	exhalbtc_connect_notify(&gl_bt_coexist, action);
-}
-
-
-void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv, enum rt_media_status mstatus)
-{
-	exhalbtc_mediastatus_notify(&gl_bt_coexist, mstatus);
-}
-
-void rtl_btc_periodical(struct rtl_priv *rtlpriv)
-{
-//	rtl_bt_dm_monitor();
-	exhalbtc_periodical(&gl_bt_coexist);
-}
-
-void rtl_btc_halt_notify(void)
-{
-	exhalbtc_halt_notify(&gl_bt_coexist);
-}
-
-void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 * tmp_buf, u8 length)
-{
-	exhalbtc_bt_info_notify(&gl_bt_coexist, tmp_buf, length);
-}
-
-bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv)
-{
-	return gl_bt_coexist.bt_info.limited_dig;
-}
-
-bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv)
-{
-	bool bt_change_edca = false;
-	u32 cur_edca_val;
-	u32 edca_bt_hs_uplink = 0x5ea42b, edca_bt_hs_downlink = 0x5ea42b;
-	u32 edca_hs;
-	u32 edca_addr = 0x504;
-
-	cur_edca_val = rtl_read_dword(rtlpriv, edca_addr);
-	if (halbtc_is_wifi_uplink(rtlpriv)){
-		if (cur_edca_val != edca_bt_hs_uplink){
-			edca_hs = edca_bt_hs_uplink;
-			bt_change_edca = true;
-		}
-	}else{
-		if (cur_edca_val != edca_bt_hs_downlink){
-			edca_hs = edca_bt_hs_downlink;
-			bt_change_edca = true;
-		}
-	}
-
-	if(bt_change_edca)
-		rtl_write_dword(rtlpriv, edca_addr, edca_hs);
-
-	return true;
-}
-
-bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv)
-{
-	if (gl_bt_coexist.bt_info.bt_disabled)
-		return true;
-	else
-		return false;
-}
-
-struct rtl_btc_ops *rtl_btc_get_ops_pointer(void)
-{
-	return &rtl_btc_operation;
-}
-//EXPORT_SYMBOL(rtl_btc_get_ops_pointer);
-
-u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv)
-{
-	u8 num;
-
-	if (rtlpriv->btcoexist.btc_info.ant_num == ANT_X2)
-		num = 2;
-	else
-		num = 1;
-
-	return num;
-}
-
-#if 0
-enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw)
-{
-    struct rtl_priv *rtlpriv = rtl_priv(hw);
-    struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-    enum rt_media_status    m_status = RT_MEDIA_DISCONNECT;
-
-    u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
-
-    if(bibss || rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-            m_status = RT_MEDIA_CONNECT;
-    }
-
-    return m_status;
-}
-#endif
-
-u8 rtl_get_hwpg_bt_exist(struct rtl_priv *rtlpriv)
-{
-	return rtlpriv->btcoexist.btc_info.btcoexist;
-}
-
-u8 rtl_get_hwpg_bt_type(struct rtl_priv *rtlpriv)
-{
-	return rtlpriv->btcoexist.btc_info.bt_type;
-}
-
-
-#if 0
-
-MODULE_AUTHOR("Page He	<page_he@realsil.com.cn>");
-MODULE_AUTHOR("Realtek WlanFAE	<wlanfae@realtek.com>");
-MODULE_AUTHOR("Larry Finger	<Larry.FInger@lwfinger.net>");
-MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("Realtek 802.11n PCI wireless core");
-
-static int __init rtl_btcoexist_module_init(void)
-{
-
-	//printk("%s, rtlpriv->btc_ops.btc_init_variables addr is %p\n", __func__, rtlpriv->btc_ops.btc_init_variables);
-
-	return 0;
-}
-
-static void __exit rtl_btcoexist_module_exit(void)
-{
-	return;
-}
-
-module_init(rtl_btcoexist_module_init);
-module_exit(rtl_btcoexist_module_exit);
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.h b/drivers/staging/rtl8821ae/btcoexist/rtl_btc.h
deleted file mode 100644
index 452fbf1e6d1e..000000000000
--- a/drivers/staging/rtl8821ae/btcoexist/rtl_btc.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_BTC_H__
-#define __RTL_BTC_H__
-
-#include "halbt_precomp.h"
-
-
-
-void rtl_btc_init_variables(struct rtl_priv *rtlpriv);
-void rtl_btc_init_hal_vars(struct rtl_priv *rtlpriv);
-void rtl_btc_init_hw_config(struct rtl_priv *rtlpriv);
-void rtl_btc_ips_notify(struct rtl_priv *rtlpriv, u8 type);
-void rtl_btc_scan_notify(struct rtl_priv *rtlpriv, u8 scantype);
-void rtl_btc_connect_notify(struct rtl_priv *rtlpriv, u8 action);
-void rtl_btc_mediastatus_notify(struct rtl_priv *rtlpriv, enum rt_media_status mstatus);
-void rtl_btc_periodical(struct rtl_priv *rtlpriv);
-void rtl_btc_halt_notify(void);
-void rtl_btc_btinfo_notify(struct rtl_priv *rtlpriv, u8 * tmpBuf, u8 length);
-bool rtl_btc_is_limited_dig(struct rtl_priv *rtlpriv);
-bool rtl_btc_is_disable_edca_turbo(struct rtl_priv *rtlpriv);
-bool rtl_btc_is_bt_disabled(struct rtl_priv *rtlpriv);
-
-
-//extern struct rtl_btc_ops rtl_btc_operation;
-extern struct rtl_btc_ops *rtl_btc_get_ops_pointer(void);
-
-u8 rtl_get_hwpg_ant_num(struct rtl_priv *rtlpriv);
-u8 rtl_get_hwpg_bt_exist(struct rtl_priv *rtlpriv);
-u8 rtl_get_hwpg_bt_type(struct rtl_priv *rtlpriv);
-//enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw);
-
-
-
-
-
-
-
-
-#endif
diff --git a/drivers/staging/rtl8821ae/cam.c b/drivers/staging/rtl8821ae/cam.c
deleted file mode 100644
index 6185ea42deb1..000000000000
--- a/drivers/staging/rtl8821ae/cam.c
+++ /dev/null
@@ -1,345 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include "wifi.h"
-#include "cam.h"
-#include <linux/export.h>
-
-void rtl_cam_reset_sec_info(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->sec.use_defaultkey = false;
-	rtlpriv->sec.pairwise_enc_algorithm = NO_ENCRYPTION;
-	rtlpriv->sec.group_enc_algorithm = NO_ENCRYPTION;
-	memset(rtlpriv->sec.key_buf, 0, KEY_BUF_SIZE * MAX_KEY_LEN);
-	memset(rtlpriv->sec.key_len, 0, KEY_BUF_SIZE);
-	rtlpriv->sec.pairwise_key = NULL;
-}
-
-static void rtl_cam_program_entry(struct ieee80211_hw *hw, u32 entry_no,
-			   u8 *mac_addr, u8 *key_cont_128, u16 us_config)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	u32 target_command;
-	u32 target_content = 0;
-	u8 entry_i;
-
-	RT_PRINT_DATA(rtlpriv, COMP_SEC, DBG_DMESG, "Key content :",
-			key_cont_128, 16);
-
-	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-		target_command = entry_i + CAM_CONTENT_COUNT * entry_no;
-		target_command = target_command | BIT(31) | BIT(16);
-
-		if (entry_i == 0) {
-			target_content = (u32) (*(mac_addr + 0)) << 16 |
-			    (u32) (*(mac_addr + 1)) << 24 | (u32) us_config;
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
-					target_content);
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_command);
-
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE %x: %x \n",
-				  rtlpriv->cfg->maps[WCAMI], target_content));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("The Key ID is %d\n", entry_no));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE %x: %x \n",
-				  rtlpriv->cfg->maps[RWCAM], target_command));
-
-		} else if (entry_i == 1) {
-
-			target_content = (u32) (*(mac_addr + 5)) << 24 |
-			    (u32) (*(mac_addr + 4)) << 16 |
-			    (u32) (*(mac_addr + 3)) << 8 |
-			    (u32) (*(mac_addr + 2));
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
-					target_content);
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_command);
-
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A4: %x \n", target_content));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A0: %x \n", target_command));
-
-		} else {
-
-			target_content =
-			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 3)) <<
-			    24 | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 2))
-			    << 16 |
-			    (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 1)) << 8
-			    | (u32) (*(key_cont_128 + (entry_i * 4 - 8) + 0));
-
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI],
-					target_content);
-			rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_command);
-			udelay(100);
-
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A4: %x \n", target_content));
-			RT_TRACE(COMP_SEC, DBG_LOUD,
-				 ("WRITE A0: %x \n", target_command));
-		}
-	}
-
-	RT_TRACE(COMP_SEC, DBG_LOUD,
-		 ("after set key, usconfig:%x\n", us_config));
-}
-
-u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
-			 u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
-			 u32 ul_default_key, u8 *key_content)
-{
-	u32 us_config;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("EntryNo:%x, ulKeyId=%x, ulEncAlg=%x, "
-		  "ulUseDK=%x MacAddr %pM\n",
-		  ul_entry_idx, ul_key_id, ul_enc_alg,
-		  ul_default_key, mac_addr));
-
-	if (ul_key_id == TOTAL_CAM_ENTRY) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("ulKeyId exceed!\n"));
-		return 0;
-	}
-
-	if (ul_default_key == 1) {
-		us_config = CFG_VALID | ((u16) (ul_enc_alg) << 2);
-	} else {
-		us_config = CFG_VALID | ((ul_enc_alg) << 2) | ul_key_id;
-	}
-
-	rtl_cam_program_entry(hw, ul_entry_idx, mac_addr,
-			      (u8 *) key_content, us_config);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG, ("end \n"));
-
-	return 1;
-
-}
-
-int rtl_cam_delete_one_entry(struct ieee80211_hw *hw,
-			     u8 *mac_addr, u32 ul_key_id)
-{
-	u32 ul_command;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG, ("key_idx:%d\n", ul_key_id));
-
-	ul_command = ul_key_id * CAM_CONTENT_COUNT;
-	ul_command = ul_command | BIT(31) | BIT(16);
-
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], 0);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("rtl_cam_delete_one_entry(): WRITE A4: %x \n", 0));
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("rtl_cam_delete_one_entry(): WRITE A0: %x \n", ul_command));
-
-	return 0;
-
-}
-
-void rtl_cam_reset_all_entry(struct ieee80211_hw *hw)
-{
-	u32 ul_command;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	ul_command = BIT(31) | BIT(30);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-}
-
-void rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	u32 ul_command;
-	u32 ul_content;
-	u32 ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
-
-	switch (rtlpriv->sec.pairwise_enc_algorithm) {
-	case WEP40_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
-		break;
-	case WEP104_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
-		break;
-	case TKIP_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
-		break;
-	case AESCCMP_ENCRYPTION:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
-		break;
-	default:
-		ul_enc_algo = rtlpriv->cfg->maps[SEC_CAM_AES];
-	}
-
-	ul_content = (uc_index & 3) | ((u16) (ul_enc_algo) << 2);
-
-	ul_content |= BIT(15);
-	ul_command = CAM_CONTENT_COUNT * uc_index;
-	ul_command = ul_command | BIT(31) | BIT(16);
-
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("rtl_cam_mark_invalid(): WRITE A4: %x \n", ul_content));
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("rtl_cam_mark_invalid(): WRITE A0: %x \n", ul_command));
-}
-
-void rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	u32 ul_command;
-	u32 ul_content;
-	u32 ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
-	u8 entry_i;
-
-	switch (rtlpriv->sec.pairwise_enc_algorithm) {
-	case WEP40_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP40];
-		break;
-	case WEP104_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_WEP104];
-		break;
-	case TKIP_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_TKIP];
-		break;
-	case AESCCMP_ENCRYPTION:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
-		break;
-	default:
-		ul_encalgo = rtlpriv->cfg->maps[SEC_CAM_AES];
-	}
-
-	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-
-		if (entry_i == 0) {
-			ul_content =
-			    (uc_index & 0x03) | ((u16) (ul_encalgo) << 2);
-			ul_content |= BIT(15);
-
-		} else {
-			ul_content = 0;
-		}
-
-		ul_command = CAM_CONTENT_COUNT * uc_index + entry_i;
-		ul_command = ul_command | BIT(31) | BIT(16);
-
-		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[WCAMI], ul_content);
-		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM], ul_command);
-
-		RT_TRACE(COMP_SEC, DBG_LOUD,
-			 ("rtl_cam_empty_entry(): WRITE A4: %x \n",
-			  ul_content));
-		RT_TRACE(COMP_SEC, DBG_LOUD,
-			 ("rtl_cam_empty_entry(): WRITE A0: %x \n",
-			  ul_command));
-	}
-
-}
-
-u8 rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> 4;
-	u8 entry_idx = 0;
-	u8 i, *addr;
-
-	if (NULL == sta_addr) {
-		RT_TRACE(COMP_SEC, DBG_EMERG,
-			("sta_addr is NULL.\n"));
-		return TOTAL_CAM_ENTRY;
-	}
-	/* Does STA already exist? */
-	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
-		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
-		if(memcmp(addr, sta_addr, ETH_ALEN) == 0)
-			return i;
-	}
-	/* Get a free CAM entry. */
-	for (entry_idx = 4; entry_idx < TOTAL_CAM_ENTRY; entry_idx++) {
-		if ((bitmap & BIT(0)) == 0) {
-			RT_TRACE(COMP_SEC, DBG_EMERG,
-				("-----hwsec_cam_bitmap: 0x%x entry_idx=%d\n",
-				 rtlpriv->sec.hwsec_cam_bitmap, entry_idx));
-			rtlpriv->sec.hwsec_cam_bitmap |= BIT(0) << entry_idx;
-			memcpy(rtlpriv->sec.hwsec_cam_sta_addr[entry_idx],
-			       sta_addr, ETH_ALEN);
-			return entry_idx;
-		}
-		bitmap = bitmap >>1;
-	}
-	return TOTAL_CAM_ENTRY;
-}
-
-void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 bitmap;
-	u8 i, *addr;
-
-	if (NULL == sta_addr) {
-		RT_TRACE(COMP_SEC, DBG_EMERG,
-			("sta_addr is NULL.\n"));
-	}
-
-	if ((sta_addr[0]|sta_addr[1]|sta_addr[2]|sta_addr[3]|\
-				sta_addr[4]|sta_addr[5]) == 0) {
-		RT_TRACE(COMP_SEC, DBG_EMERG,
-			("sta_addr is 00:00:00:00:00:00.\n"));
-		return;
-	}
-	/* Does STA already exist? */
-	for (i = 4; i < TOTAL_CAM_ENTRY; i++) {
-		addr = rtlpriv->sec.hwsec_cam_sta_addr[i];
-		bitmap = (rtlpriv->sec.hwsec_cam_bitmap) >> i;
-		if (((bitmap & BIT(0)) == BIT(0)) &&
-		    (memcmp(addr, sta_addr, ETH_ALEN) == 0)) {
-			/* Remove from HW Security CAM */
-			memset(rtlpriv->sec.hwsec_cam_sta_addr[i], 0, ETH_ALEN);
-			rtlpriv->sec.hwsec_cam_bitmap &= ~(BIT(0) << i);
-			printk("&&&&&&&&&del entry %d\n",i);
-		}
-	}
-	return;
-}
diff --git a/drivers/staging/rtl8821ae/cam.h b/drivers/staging/rtl8821ae/cam.h
deleted file mode 100644
index 326fa6784ae5..000000000000
--- a/drivers/staging/rtl8821ae/cam.h
+++ /dev/null
@@ -1,56 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_CAM_H_
-#define __RTL_CAM_H_
-
-#define CAM_CONTENT_COUNT 				8
-
-#define CFG_DEFAULT_KEY  				BIT(5)
-#define CFG_VALID        				BIT(15)
-
-#define PAIRWISE_KEYIDX					0
-#define CAM_PAIRWISE_KEY_POSITION			4
-
-#define	CAM_CONFIG_USEDK				1
-#define	CAM_CONFIG_NO_USEDK				0
-
-extern void rtl_cam_reset_all_entry(struct ieee80211_hw *hw);
-extern u8 rtl_cam_add_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
-				u32 ul_key_id, u32 ul_entry_idx, u32 ul_enc_alg,
-				u32 ul_default_key, u8 *key_content);
-int rtl_cam_delete_one_entry(struct ieee80211_hw *hw, u8 *mac_addr,
-			     u32 ul_key_id);
-void rtl_cam_mark_invalid(struct ieee80211_hw *hw, u8 uc_index);
-void rtl_cam_empty_entry(struct ieee80211_hw *hw, u8 uc_index);
-void rtl_cam_reset_sec_info(struct ieee80211_hw *hw);
-u8 rtl_cam_get_free_entry(struct ieee80211_hw *hw, u8 *sta_addr);
-void rtl_cam_del_entry(struct ieee80211_hw *hw, u8 *sta_addr);
-
-#endif
diff --git a/drivers/staging/rtl8821ae/compat.h b/drivers/staging/rtl8821ae/compat.h
deleted file mode 100644
index ffb5f8b07bd0..000000000000
--- a/drivers/staging/rtl8821ae/compat.h
+++ /dev/null
@@ -1,68 +0,0 @@
-#ifndef __RTL_COMPAT_H__
-#define __RTL_COMPAT_H__
-
-#define RX_FLAG_MACTIME_MPDU RX_FLAG_MACTIME_START
-#define IEEE80211_KEY_FLAG_SW_MGMT IEEE80211_KEY_FLAG_SW_MGMT_TX
-
-struct ieee80211_mgmt_compat {
-	__le16 frame_control;
-	__le16 duration;
-	u8 da[6];
-	u8 sa[6];
-	u8 bssid[6];
-	__le16 seq_ctrl;
-	union {
-		struct {
-			u8 category;
-			union {
-				struct {
-					u8 action_code;
-					u8 dialog_token;
-					u8 status_code;
-					u8 variable[0];
-				} __attribute__ ((packed)) wme_action;
-				struct{
-					u8 action_code;
-					u8 dialog_token;
-					__le16 capab;
-					__le16 timeout;
-					__le16 start_seq_num;
-				} __attribute__((packed)) addba_req;
-				struct{
-					u8 action_code;
-					u8 dialog_token;
-					__le16 status;
-					__le16 capab;
-					__le16 timeout;
-				} __attribute__((packed)) addba_resp;
-				struct{
-					u8 action_code;
-					__le16 params;
-					__le16 reason_code;
-				} __attribute__((packed)) delba;
-				struct{
-					u8 action_code;
-					/* capab_info for open and confirm,
-					 * reason for close
-					 */
-					__le16 aux;
-					/* Followed in plink_confirm by status
-					 * code, AID and supported rates,
-					 * and directly by supported rates in
-					 * plink_open and plink_close
-					 */
-					u8 variable[0];
-				} __attribute__((packed)) plink_action;
-				struct{
-					u8 action_code;
-					u8 variable[0];
-				} __attribute__((packed)) mesh_action;
-				struct {
-					u8 action;
-					u8 smps_control;
-				} __attribute__ ((packed)) ht_smps;
-			} u;
-		} __attribute__ ((packed)) action;
-	} u;
-} __attribute__ ((packed));
-#endif
diff --git a/drivers/staging/rtl8821ae/core.c b/drivers/staging/rtl8821ae/core.c
deleted file mode 100644
index 046be2ce9c1a..000000000000
--- a/drivers/staging/rtl8821ae/core.c
+++ /dev/null
@@ -1,1314 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "core.h"
-#include "cam.h"
-#include "base.h"
-#include "ps.h"
-
-#include "btcoexist/rtl_btc.h"
-
-/*mutex for start & stop is must here. */
-static int rtl_op_start(struct ieee80211_hw *hw)
-{
-	int err = 0;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (!is_hal_stop(rtlhal))
-		return 0;
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		return 0;
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	err = rtlpriv->intf_ops->adapter_start(hw);
-	if (err)
-		goto out;
-	rtl_watch_dog_timer_callback((unsigned long)hw);
-
-out:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-	return err;
-}
-
-static void rtl_op_stop(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	if (is_hal_stop(rtlhal))
-		return;
-
-	/* here is must, because adhoc do stop and start,
-	 * but stop with RFOFF may cause something wrong,
-	 * like adhoc TP */
-	if (unlikely(ppsc->rfpwr_state == ERFOFF))
-		rtl_ips_nic_on(hw);
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-
-	mac->link_state = MAC80211_NOLINK;
-	memset(mac->bssid, 0, 6);
-	mac->vendor = PEER_UNKNOWN;
-
-	/*reset sec info */
-	rtl_cam_reset_sec_info(hw);
-
-	rtl_deinit_deferred_work(hw);
-	rtlpriv->intf_ops->adapter_stop(hw);
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-
-static void rtl_op_tx(struct ieee80211_hw *hw,
-		      struct ieee80211_tx_control *control,
-		      struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_tcb_desc tcb_desc;
-	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-
-	if (unlikely(is_hal_stop(rtlhal) || ppsc->rfpwr_state != ERFON))
-		goto err_free;
-
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		goto err_free;
-
-	if (!rtlpriv->intf_ops->waitq_insert(hw, control->sta, skb))
-	        rtlpriv->intf_ops->adapter_tx(hw, control->sta, skb, &tcb_desc);
-	return;
-
-err_free:
-	dev_kfree_skb_any(skb);
-	return;
-}
-
-static int rtl_op_add_interface(struct ieee80211_hw *hw,
-		struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	int err = 0;
-
-	if (mac->vif) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("vif has been set!! mac->vif = 0x%p\n", mac->vif));
-		return -EOPNOTSUPP;
-	}
-
-	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
-
-	rtl_ips_nic_on(hw);
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	switch (ieee80211_vif_type_p2p(vif)) {
-	case NL80211_IFTYPE_P2P_CLIENT:
-		mac->p2p = P2P_ROLE_CLIENT;
-		/*fall through*/
-	case NL80211_IFTYPE_STATION:
-		if (mac->beacon_enabled == 1) {
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("NL80211_IFTYPE_STATION \n"));
-			mac->beacon_enabled = 0;
-			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
-		}
-		break;
-	case NL80211_IFTYPE_ADHOC:
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("NL80211_IFTYPE_ADHOC \n"));
-
-		mac->link_state = MAC80211_LINKED;
-		rtlpriv->cfg->ops->set_bcn_reg(hw);
-		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
-			mac->basic_rates = 0xfff;
-		else
-			mac->basic_rates = 0xff0;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *) (&mac->basic_rates));
-
-		break;
-	case NL80211_IFTYPE_P2P_GO:
-		mac->p2p = P2P_ROLE_GO;
-		/*fall through*/
-	case NL80211_IFTYPE_AP:
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("NL80211_IFTYPE_AP \n"));
-
-		mac->link_state = MAC80211_LINKED;
-		rtlpriv->cfg->ops->set_bcn_reg(hw);
-		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
-			mac->basic_rates = 0xfff;
-		else
-			mac->basic_rates = 0xff0;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-					      (u8 *) (&mac->basic_rates));
-		break;
-	case NL80211_IFTYPE_MESH_POINT:
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("NL80211_IFTYPE_MESH_POINT \n"));
-
-		mac->link_state = MAC80211_LINKED;
-		rtlpriv->cfg->ops->set_bcn_reg(hw);
-		if (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)
-			mac->basic_rates = 0xfff;
-		else
-			mac->basic_rates = 0xff0;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *) (&mac->basic_rates));
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("operation mode %d is not support!\n", vif->type));
-		err = -EOPNOTSUPP;
-		goto out;
-	}
-
-#ifdef VIF_TODO
-	if (!rtl_set_vif_info(hw, vif))
-		goto out;
-#endif
-
-	if (mac->p2p) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("p2p role %x \n",vif->type));
-		mac->basic_rates = 0xff0;/*disable cck rate for p2p*/
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-				(u8 *) (&mac->basic_rates));
-	}
-	mac->vif = vif;
-	mac->opmode = vif->type;
-	rtlpriv->cfg->ops->set_network_type(hw, vif->type);
-	memcpy(mac->mac_addr, vif->addr, ETH_ALEN);
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
-
-out:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-	return err;
-}
-
-static void rtl_op_remove_interface(struct ieee80211_hw *hw,
-		struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-
-	/* Free beacon resources */
-	if ((vif->type == NL80211_IFTYPE_AP) ||
-	    (vif->type == NL80211_IFTYPE_ADHOC) ||
-	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
-		if (mac->beacon_enabled == 1) {
-			mac->beacon_enabled = 0;
-			rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-					rtlpriv->cfg->maps[RTL_IBSS_INT_MASKS]);
-		}
-	}
-
-	/*
-	 *Note: We assume NL80211_IFTYPE_UNSPECIFIED as
-	 *NO LINK for our hardware.
-	 */
-	mac->p2p = 0;
-	mac->vif = NULL;
-	mac->link_state = MAC80211_NOLINK;
-	memset(mac->bssid, 0, 6);
-	mac->vendor = PEER_UNKNOWN;
-	mac->opmode = NL80211_IFTYPE_UNSPECIFIED;
-	rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-
-static int rtl_op_change_interface(struct ieee80211_hw *hw,
-				   struct ieee80211_vif *vif,
-				   enum nl80211_iftype new_type, bool p2p)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int ret;
-	rtl_op_remove_interface(hw, vif);
-
-	vif->type = new_type;
-	vif->p2p = p2p;
-	ret = rtl_op_add_interface(hw, vif);
-	RT_TRACE(COMP_MAC80211, DBG_LOUD,
-		 (" p2p  %x\n",p2p));
-	return ret;
-}
-
-static int rtl_op_config(struct ieee80211_hw *hw, u32 changed)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct ieee80211_conf *conf = &hw->conf;
-
-	if (mac->skip_scan)
-		return 1;
-
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	if (changed & IEEE80211_CONF_CHANGE_LISTEN_INTERVAL) {	/* BIT(2) */
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("IEEE80211_CONF_CHANGE_LISTEN_INTERVAL\n"));
-	}
-
-	/*For IPS */
-	if (changed & IEEE80211_CONF_CHANGE_IDLE) {
-		if (hw->conf.flags & IEEE80211_CONF_IDLE)
-			rtl_ips_nic_off(hw);
-		else
-			rtl_ips_nic_on(hw);
-	} else {
-		/*
-		 *although rfoff may not cause by ips, but we will
-		 *check the reason in set_rf_power_state function
-		 */
-		if (unlikely(ppsc->rfpwr_state == ERFOFF))
-			rtl_ips_nic_on(hw);
-	}
-
-	/*For LPS */
-	if (changed & IEEE80211_CONF_CHANGE_PS) {
-		cancel_delayed_work(&rtlpriv->works.ps_work);
-		cancel_delayed_work(&rtlpriv->works.ps_rfon_wq);
-		if (conf->flags & IEEE80211_CONF_PS) {
-			rtlpriv->psc.sw_ps_enabled = true;
-			/* sleep here is must, or we may recv the beacon and
-			 * cause mac80211 into wrong ps state, this will cause
-			 * power save nullfunc send fail, and further cause
-			 * pkt loss, So sleep must quickly but not immediately
-			 * because that will cause nullfunc send by mac80211
-			 * fail, and cause pkt loss, we have tested that 5mA
-			 * is worked very well */
-			if (!rtlpriv->psc.multi_buffered)
-				queue_delayed_work(rtlpriv->works.rtl_wq,
-						   &rtlpriv->works.ps_work,
-						   MSECS(5));
-		} else {
-			rtl_swlps_rf_awake(hw);
-			rtlpriv->psc.sw_ps_enabled = false;
-		}
-	}
-
-	if (changed & IEEE80211_CONF_CHANGE_RETRY_LIMITS) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("IEEE80211_CONF_CHANGE_RETRY_LIMITS %x\n",
-			  hw->conf.long_frame_max_tx_count));
-		mac->retry_long = hw->conf.long_frame_max_tx_count;
-		mac->retry_short = hw->conf.long_frame_max_tx_count;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RETRY_LIMIT,
-				(u8 *) (&hw->conf.long_frame_max_tx_count));
-	}
-
-	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
-		struct ieee80211_channel *channel = hw->conf.chandef.chan;
-		enum nl80211_channel_type channel_type =
-				cfg80211_get_chandef_type(&(hw->conf.chandef));
-		u8 wide_chan = (u8) channel->hw_value;
-
-		if (mac->act_scanning)
-			mac->n_channels++;
-
-		if (rtlpriv->dm.supp_phymode_switch &&
-			mac->link_state < MAC80211_LINKED &&
-			!mac->act_scanning) {
-			if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-				rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-		}
-
-		/*
-		 *because we should back channel to
-		 *current_network.chan in in scanning,
-		 *So if set_chan == current_network.chan
-		 *we should set it.
-		 *because mac80211 tell us wrong bw40
-		 *info for cisco1253 bw20, so we modify
-		 *it here based on UPPER & LOWER
-		 */
-		switch (channel_type) {
-			case NL80211_CHAN_HT20:
-			case NL80211_CHAN_NO_HT:
-				/* SC */
-				mac->cur_40_prime_sc =
-					PRIME_CHNL_OFFSET_DONT_CARE;
-				rtlphy->current_chan_bw = HT_CHANNEL_WIDTH_20;
-				mac->bw_40 = false;
-				break;
-			case NL80211_CHAN_HT40MINUS:
-				/* SC */
-				mac->cur_40_prime_sc = PRIME_CHNL_OFFSET_UPPER;
-				rtlphy->current_chan_bw =
-					HT_CHANNEL_WIDTH_20_40;
-				mac->bw_40 = true;
-
-				/*wide channel */
-				wide_chan -= 2;
-
-				break;
-			case NL80211_CHAN_HT40PLUS:
-				/* SC */
-				mac->cur_40_prime_sc = PRIME_CHNL_OFFSET_LOWER;
-				rtlphy->current_chan_bw =
-					HT_CHANNEL_WIDTH_20_40;
-				mac->bw_40 = true;
-
-				/*wide channel */
-				wide_chan += 2;
-
-				break;
-			default:
-				mac->bw_40 = false;
-				RT_TRACE(COMP_ERR, DBG_EMERG,
-						("switch case not processed \n"));
-				break;
-		}
-
-		if (wide_chan <= 0)
-			wide_chan = 1;
-
-		/* in scanning, when before we offchannel we may send a ps=1
-		 * null to AP, and then we may send a ps = 0 null to AP quickly,
-		 * but first null have cause AP's put lots of packet to hw tx
-		 * buffer, these packet must be tx before off channel so we must
-		 * delay more time to let AP flush these packets before
-		 * offchannel, or dis-association or delete BA will happen by AP
-		 */
-		if (rtlpriv->mac80211.offchan_deley) {
-			rtlpriv->mac80211.offchan_deley = false;
-			mdelay(50);
-		}
-
-		rtlphy->current_channel = wide_chan;
-
-		rtlpriv->cfg->ops->switch_channel(hw);
-		rtlpriv->cfg->ops->set_channel_access(hw);
-		rtlpriv->cfg->ops->set_bw_mode(hw,
-			channel_type);
-	}
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-
-	return 0;
-}
-
-static void rtl_op_configure_filter(struct ieee80211_hw *hw,
-				    unsigned int changed_flags,
-				    unsigned int *new_flags, u64 multicast)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	*new_flags &= RTL_SUPPORTED_FILTERS;
-	if (0 == changed_flags)
-		return;
-
-	/*TODO: we disable broadcase now, so enable here */
-	if (changed_flags & FIF_ALLMULTI) {
-		if (*new_flags & FIF_ALLMULTI) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AM] |
-			    rtlpriv->cfg->maps[MAC_RCR_AB];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive multicast frame.\n"));
-		} else {
-			mac->rx_conf &= ~(rtlpriv->cfg->maps[MAC_RCR_AM] |
-					  rtlpriv->cfg->maps[MAC_RCR_AB]);
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive multicast frame.\n"));
-		}
-	}
-
-	if (changed_flags & FIF_FCSFAIL) {
-		if (*new_flags & FIF_FCSFAIL) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACRC32];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive FCS error frame.\n"));
-		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACRC32];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive FCS error frame.\n"));
-		}
-	}
-
-	/* if ssid not set to hw don't check bssid
-	 * here just used for linked scanning, & linked
-	 * and nolink check bssid is set in set network_type */
-	if ((changed_flags & FIF_BCN_PRBRESP_PROMISC) &&
-		(mac->link_state >= MAC80211_LINKED)) {
-		if (mac->opmode != NL80211_IFTYPE_AP &&
-			mac->opmode != NL80211_IFTYPE_MESH_POINT) {
-			if (*new_flags & FIF_BCN_PRBRESP_PROMISC) {
-				rtlpriv->cfg->ops->set_chk_bssid(hw, false);
-			} else {
-				rtlpriv->cfg->ops->set_chk_bssid(hw, true);
-			}
-		}
-	}
-
-	if (changed_flags & FIF_CONTROL) {
-		if (*new_flags & FIF_CONTROL) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_ACF];
-
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive control frame.\n"));
-		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_ACF];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive control frame.\n"));
-		}
-	}
-
-	if (changed_flags & FIF_OTHER_BSS) {
-		if (*new_flags & FIF_OTHER_BSS) {
-			mac->rx_conf |= rtlpriv->cfg->maps[MAC_RCR_AAP];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Enable receive other BSS's frame.\n"));
-		} else {
-			mac->rx_conf &= ~rtlpriv->cfg->maps[MAC_RCR_AAP];
-			RT_TRACE(COMP_MAC80211, DBG_LOUD,
-				 ("Disable receive other BSS's frame.\n"));
-		}
-	}
-}
-static int rtl_op_sta_add(struct ieee80211_hw *hw,
-			 struct ieee80211_vif *vif,
-			 struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal= rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_sta_info *sta_entry;
-
-	if (sta) {
-		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
-		list_add_tail(&sta_entry->list, &rtlpriv->entry_list);
-		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
-		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-			sta_entry->wireless_mode = WIRELESS_MODE_G;
-			if (sta->supp_rates[0] <= 0xf)
-				sta_entry->wireless_mode = WIRELESS_MODE_B;
-			if (sta->ht_cap.ht_supported == true)
-				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
-
-			if (vif->type == NL80211_IFTYPE_ADHOC)
-				sta_entry->wireless_mode = WIRELESS_MODE_G;
-		} else if (rtlhal->current_bandtype == BAND_ON_5G) {
-			sta_entry->wireless_mode = WIRELESS_MODE_A;
-			if (sta->ht_cap.ht_supported == true)
-				sta_entry->wireless_mode = WIRELESS_MODE_N_24G;
-
-			if (vif->type == NL80211_IFTYPE_ADHOC)
-				sta_entry->wireless_mode = WIRELESS_MODE_A;
-		}
-		/*disable cck rate for p2p*/
-		if (mac->p2p)
-			sta->supp_rates[0] &= 0xfffffff0;
-
-		memcpy(sta_entry->mac_addr, sta->addr, ETH_ALEN);
-		RT_TRACE(COMP_MAC80211, DBG_DMESG,
-			("Add sta addr is %pM\n",sta->addr));
-		rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
-	}
-
-	return 0;
-}
-
-static int rtl_op_sta_remove(struct ieee80211_hw *hw,
-				struct ieee80211_vif *vif,
-				struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *sta_entry;
-	if (sta) {
-		RT_TRACE(COMP_MAC80211, DBG_DMESG,
-			("Remove sta addr is %pM\n",sta->addr));
-		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-		sta_entry->wireless_mode = 0;
-		sta_entry->ratr_index = 0;
-		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
-		list_del(&sta_entry->list);
-		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
-	}
-	return 0;
-}
-static int _rtl_get_hal_qnum(u16 queue)
-{
-	int qnum;
-
-	switch (queue) {
-	case 0:
-		qnum = AC3_VO;
-		break;
-	case 1:
-		qnum = AC2_VI;
-		break;
-	case 2:
-		qnum = AC0_BE;
-		break;
-	case 3:
-		qnum = AC1_BK;
-		break;
-	default:
-		qnum = AC0_BE;
-		break;
-	}
-	return qnum;
-}
-
-/*
- *for mac80211 VO=0, VI=1, BE=2, BK=3
- *for rtl819x  BE=0, BK=1, VI=2, VO=3
- */
-static int rtl_op_conf_tx(struct ieee80211_hw *hw,
-			  struct ieee80211_vif *vif, u16 queue,
-			  const struct ieee80211_tx_queue_params *param)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	int aci;
-
-	if (queue >= AC_MAX) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("queue number %d is incorrect!\n", queue));
-		return -EINVAL;
-	}
-
-	aci = _rtl_get_hal_qnum(queue);
-	mac->ac[aci].aifs = param->aifs;
-	mac->ac[aci].cw_min = param->cw_min;
-	mac->ac[aci].cw_max = param->cw_max;
-	mac->ac[aci].tx_op = param->txop;
-	memcpy(&mac->edca_param[aci], param, sizeof(*param));
-	rtlpriv->cfg->ops->set_qos(hw, aci);
-	return 0;
-}
-
-static void rtl_op_bss_info_changed(struct ieee80211_hw *hw,
-				    struct ieee80211_vif *vif,
-				    struct ieee80211_bss_conf *bss_conf,
-				    u32 changed)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	if ((vif->type == NL80211_IFTYPE_ADHOC) ||
-	    (vif->type == NL80211_IFTYPE_AP) ||
-	    (vif->type == NL80211_IFTYPE_MESH_POINT)) {
-		if ((changed & BSS_CHANGED_BEACON) ||
-		    (changed & BSS_CHANGED_BEACON_ENABLED &&
-		     bss_conf->enable_beacon)) {
-			if (mac->beacon_enabled == 0) {
-				RT_TRACE(COMP_MAC80211, DBG_DMESG,
-					 ("BSS_CHANGED_BEACON_ENABLED \n"));
-
-				/*start hw beacon interrupt. */
-				/*rtlpriv->cfg->ops->set_bcn_reg(hw); */
-				mac->beacon_enabled = 1;
-				rtlpriv->cfg->ops->update_interrupt_mask(hw,
-						rtlpriv->cfg->maps
-						[RTL_IBSS_INT_MASKS], 0);
-
-				if (rtlpriv->cfg->ops->linked_set_reg)
-					rtlpriv->cfg->ops->linked_set_reg(hw);
-			}
-		}
-		if ((changed & BSS_CHANGED_BEACON_ENABLED &&
-			!bss_conf->enable_beacon)){
-			if (mac->beacon_enabled == 1) {
-				RT_TRACE(COMP_MAC80211, DBG_DMESG,
-					 ("ADHOC DISABLE BEACON\n"));
-
-				mac->beacon_enabled = 0;
-				rtlpriv->cfg->ops->update_interrupt_mask(hw, 0,
-						rtlpriv->cfg->maps
-						[RTL_IBSS_INT_MASKS]);
-			}
-		}
-		if (changed & BSS_CHANGED_BEACON_INT) {
-			RT_TRACE(COMP_BEACON, DBG_TRACE,
-				 ("BSS_CHANGED_BEACON_INT\n"));
-			mac->beacon_interval = bss_conf->beacon_int;
-			rtlpriv->cfg->ops->set_bcn_intv(hw);
-		}
-	}
-
-	/*TODO: reference to enum ieee80211_bss_change */
-	if (changed & BSS_CHANGED_ASSOC) {
-		if (bss_conf->assoc) {
-			struct ieee80211_sta *sta = NULL;
-			/* we should reset all sec info & cam
-			 * before set cam after linked, we should not
-			 * reset in disassoc, that will cause tkip->wep
-			 * fail because some flag will be wrong */
-			/* reset sec info */
-			rtl_cam_reset_sec_info(hw);
-			/* reset cam to fix wep fail issue
-			 * when change from wpa to wep */
-			rtl_cam_reset_all_entry(hw);
-
-			mac->link_state = MAC80211_LINKED;
-			mac->cnt_after_linked = 0;
-			mac->assoc_id = bss_conf->aid;
-			memcpy(mac->bssid, bss_conf->bssid, 6);
-
-			if (rtlpriv->cfg->ops->linked_set_reg)
-				rtlpriv->cfg->ops->linked_set_reg(hw);
-
-			rcu_read_lock();
-			sta = ieee80211_find_sta(vif, (u8*)bss_conf->bssid);
-
-			if (vif->type == NL80211_IFTYPE_STATION && sta)
-				rtlpriv->cfg->ops->update_rate_tbl(hw, sta, 0);
-			RT_TRACE(COMP_EASY_CONCURRENT, DBG_LOUD,
-					("send PS STATIC frame \n"));
-			if (rtlpriv->dm.supp_phymode_switch) {
-				if (sta->ht_cap.ht_supported)
-					rtl_send_smps_action(hw, sta,
-							IEEE80211_SMPS_STATIC);
-			}
-			rcu_read_unlock();
-
-			RT_TRACE(COMP_MAC80211, DBG_DMESG,
-				 ("BSS_CHANGED_ASSOC\n"));
-		} else {
-			if (mac->link_state == MAC80211_LINKED)
-				rtl_lps_leave(hw);
-			if (ppsc->p2p_ps_info.p2p_ps_mode> P2P_PS_NONE)
-				rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-			mac->link_state = MAC80211_NOLINK;
-			memset(mac->bssid, 0, 6);
-			mac->vendor = PEER_UNKNOWN;
-
-			if (rtlpriv->dm.supp_phymode_switch) {
-				if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-					rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-			}
-			RT_TRACE(COMP_MAC80211, DBG_DMESG,
-				 ("BSS_CHANGED_UN_ASSOC\n"));
-		}
-	}
-
-	if (changed & BSS_CHANGED_ERP_CTS_PROT) {
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("BSS_CHANGED_ERP_CTS_PROT\n"));
-		mac->use_cts_protect = bss_conf->use_cts_prot;
-	}
-
-	if (changed & BSS_CHANGED_ERP_PREAMBLE) {
-		RT_TRACE(COMP_MAC80211, DBG_LOUD,
-			 ("BSS_CHANGED_ERP_PREAMBLE use short preamble:%x \n",
-			  bss_conf->use_short_preamble));
-
-		mac->short_preamble = bss_conf->use_short_preamble;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ACK_PREAMBLE,
-					      (u8 *) (&mac->short_preamble));
-	}
-
-	if (changed & BSS_CHANGED_ERP_SLOT) {
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("BSS_CHANGED_ERP_SLOT\n"));
-
-		if (bss_conf->use_short_slot)
-			mac->slot_time = RTL_SLOT_TIME_9;
-		else
-			mac->slot_time = RTL_SLOT_TIME_20;
-
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-					      (u8 *) (&mac->slot_time));
-	}
-
-	if (changed & BSS_CHANGED_HT) {
-		struct ieee80211_sta *sta = NULL;
-
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("BSS_CHANGED_HT\n"));
-
-		rcu_read_lock();
-		sta = ieee80211_find_sta(vif, (u8*)bss_conf->bssid);
-		if (sta) {
-			if (sta->ht_cap.ampdu_density >
-			    mac->current_ampdu_density)
-				mac->current_ampdu_density =
-				    sta->ht_cap.ampdu_density;
-			if (sta->ht_cap.ampdu_factor <
-			    mac->current_ampdu_factor)
-				mac->current_ampdu_factor =
-				    sta->ht_cap.ampdu_factor;
-		}
-		rcu_read_unlock();
-
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SHORTGI_DENSITY,
-					      (u8 *) (&mac->max_mss_density));
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_FACTOR,
-					      &mac->current_ampdu_factor);
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AMPDU_MIN_SPACE,
-					      &mac->current_ampdu_density);
-	}
-
-	if (changed & BSS_CHANGED_BSSID) {
-		u32 basic_rates;
-		struct ieee80211_sta *sta = NULL;
-
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BSSID,
-					      (u8 *) bss_conf->bssid);
-
-		RT_TRACE(COMP_MAC80211, DBG_DMESG,
-			 ("bssid: %pM\n", bss_conf->bssid));
-
-		mac->vendor = PEER_UNKNOWN;
-		memcpy(mac->bssid, bss_conf->bssid, 6);
-		rtlpriv->cfg->ops->set_network_type(hw, vif->type);
-
-		rcu_read_lock();
-		sta = ieee80211_find_sta(vif, (u8*)bss_conf->bssid);
-		if (!sta) {
-			rcu_read_unlock();
-			goto out;
-		}
-
-		if (rtlhal->current_bandtype == BAND_ON_5G) {
-			mac->mode = WIRELESS_MODE_A;
-		} else {
-			if (sta->supp_rates[0] <= 0xf)
-				mac->mode = WIRELESS_MODE_B;
-			else
-				mac->mode = WIRELESS_MODE_G;
-		}
-
-		if (sta->ht_cap.ht_supported) {
-			if (rtlhal->current_bandtype == BAND_ON_2_4G)
-				mac->mode = WIRELESS_MODE_N_24G;
-			else
-				mac->mode = WIRELESS_MODE_N_5G;
-		}
-
-		/* just station need it, because ibss & ap mode will
-		 * set in sta_add, and will be NULL here */
-		if (vif->type == NL80211_IFTYPE_STATION) {
-			struct rtl_sta_info *sta_entry;
-			sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-			sta_entry->wireless_mode = mac->mode;
-		}
-
-		if (sta->ht_cap.ht_supported) {
-			mac->ht_enable = true;
-
-			/*
-			 * for cisco 1252 bw20 it's wrong
-			 * if (ht_cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40) {
-			 * 	mac->bw_40 = true;
-			 * }
-			 * */
-		}
-
-		if (changed & BSS_CHANGED_BASIC_RATES) {
-			/* for 5G must << RATE_6M_INDEX=4,
-			 * because 5G have no cck rate*/
-			if (rtlhal->current_bandtype == BAND_ON_5G)
-				basic_rates = sta->supp_rates[1] << 4;
-			else
-				basic_rates = sta->supp_rates[0];
-
-			mac->basic_rates = basic_rates;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_BASIC_RATE,
-					(u8 *) (&basic_rates));
-		}
-		rcu_read_unlock();
-	}
-
-	/*
-	 * For FW LPS and Keep Alive:
-	 * To tell firmware we have connected
-	 * to an AP. For 92SE/CE power save v2.
-	 */
-	if (changed & BSS_CHANGED_ASSOC) {
-		if (bss_conf->assoc) {
-			u8 keep_alive = 10;
-			u8 mstatus = RT_MEDIA_CONNECT;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_KEEP_ALIVE,
-						      (u8 *) (&keep_alive));
-
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_H2C_FW_JOINBSSRPT,
-						      (u8 *) (&mstatus));
-			ppsc->report_linked = true;
-
-		} else {
-
-			u8 mstatus = RT_MEDIA_DISCONNECT;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_H2C_FW_JOINBSSRPT,
-						      (u8 *) (&mstatus));
-			ppsc->report_linked = false;
-
-		}
-
-		if (rtlpriv->cfg->ops->get_btc_status()){
-			rtlpriv->btcoexist.btc_ops->btc_mediastatus_notify(
-						rtlpriv, ppsc->report_linked);
-		}
-	}
-
-out:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-
-static u64 rtl_op_get_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u64 tsf;
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *) (&tsf));
-	return tsf;
-}
-
-static void rtl_op_set_tsf(struct ieee80211_hw *hw,
-			   struct ieee80211_vif *vif, u64 tsf)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
-
-	mac->tsf = tsf;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_CORRECT_TSF, (u8 *) (&bibss));
-}
-
-static void rtl_op_reset_tsf(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp = 0;
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_DUAL_TSF_RST, (u8 *) (&tmp));
-}
-
-static void rtl_op_sta_notify(struct ieee80211_hw *hw,
-			      struct ieee80211_vif *vif,
-			      enum sta_notify_cmd cmd,
-			      struct ieee80211_sta *sta)
-{
-	switch (cmd) {
-	case STA_NOTIFY_SLEEP:
-		break;
-	case STA_NOTIFY_AWAKE:
-		break;
-	default:
-		break;
-	}
-}
-
-static int rtl_op_ampdu_action(struct ieee80211_hw *hw,
-			       struct ieee80211_vif *vif,
-			       enum ieee80211_ampdu_mlme_action action,
-			       struct ieee80211_sta *sta, u16 tid, u16 * ssn
-			       ,u8 buf_size
-			       )
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	switch (action) {
-	case IEEE80211_AMPDU_TX_START:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_TX_START: TID:%d\n", tid));
-		return rtl_tx_agg_start(hw, vif, sta, tid, ssn);
-		break;
-	case IEEE80211_AMPDU_TX_STOP_CONT:
-	case IEEE80211_AMPDU_TX_STOP_FLUSH:
-	case IEEE80211_AMPDU_TX_STOP_FLUSH_CONT:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_TX_STOP: TID:%d\n", tid));
-		return rtl_tx_agg_stop(hw, vif, sta, tid);
-		break;
-	case IEEE80211_AMPDU_TX_OPERATIONAL:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_TX_OPERATIONAL:TID:%d\n", tid));
-		rtl_tx_agg_oper(hw, sta, tid);
-		break;
-	case IEEE80211_AMPDU_RX_START:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_RX_START:TID:%d\n", tid));
-		return rtl_rx_agg_start(hw, sta, tid);
-		break;
-	case IEEE80211_AMPDU_RX_STOP:
-		RT_TRACE(COMP_MAC80211, DBG_TRACE,
-			 ("IEEE80211_AMPDU_RX_STOP:TID:%d\n", tid));
-		return rtl_rx_agg_stop(hw, sta, tid);
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("IEEE80211_AMPDU_ERR!!!!:\n"));
-		return -EOPNOTSUPP;
-	}
-	return 0;
-}
-
-static void rtl_op_sw_scan_start(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
-	mac->act_scanning = true;
-	/*rtlpriv->btcops->btc_scan_notify(rtlpriv, 0); */
-	if (rtlpriv->link_info.b_higher_busytraffic) {
-		mac->skip_scan = true;
-		return;
-	}
-
-	if (rtlpriv->dm.supp_phymode_switch) {
-		if (rtlpriv->cfg->ops->check_switch_to_dmdp)
-			rtlpriv->cfg->ops->check_switch_to_dmdp(hw);
-	}
-
-	if (mac->link_state == MAC80211_LINKED) {
-		rtl_lps_leave(hw);
-		mac->link_state = MAC80211_LINKED_SCANNING;
-	} else {
-		rtl_ips_nic_on(hw);
-	}
-
-	/* Dul mac */
-	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
-
-	rtlpriv->cfg->ops->led_control(hw, LED_CTL_SITE_SURVEY);
-
-	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_BACKUP_BAND0);
-
-}
-
-static void rtl_op_sw_scan_complete(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	RT_TRACE(COMP_MAC80211, DBG_LOUD, ("\n"));
-	mac->act_scanning = false;
-	mac->skip_scan = false;
-	if (rtlpriv->link_info.b_higher_busytraffic) {
-		return;
-	}
-
-	/* p2p will use 1/6/11 to scan */
-	if (mac->n_channels == 3)
-		mac->p2p_in_use = true;
-	else
-		mac->p2p_in_use = false;
-	mac->n_channels = 0;
-	/* Dul mac */
-	rtlpriv->rtlhal.b_load_imrandiqk_setting_for2g = false;
-
-	if (mac->link_state == MAC80211_LINKED_SCANNING) {
-		mac->link_state = MAC80211_LINKED;
-		if (mac->opmode == NL80211_IFTYPE_STATION) {
-			/* fix fwlps issue */
-			rtlpriv->cfg->ops->set_network_type(hw, mac->opmode);
-		}
-	}
-
-	rtlpriv->cfg->ops->scan_operation_backup(hw, SCAN_OPT_RESTORE);
-	/* rtlpriv->btcops->btc_scan_notify(rtlpriv, 1); */
-}
-
-static int rtl_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
-			  struct ieee80211_vif *vif, struct ieee80211_sta *sta,
-			  struct ieee80211_key_conf *key)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 key_type = NO_ENCRYPTION;
-	u8 key_idx;
-	bool group_key = false;
-	bool wep_only = false;
-	int err = 0;
-	u8 mac_addr[ETH_ALEN];
-	u8 bcast_addr[ETH_ALEN] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
-	u8 zero_addr[ETH_ALEN] = { 0 };
-
-	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("not open hw encryption\n"));
-		return -ENOSPC;	/*User disabled HW-crypto */
-	}
-	/* To support IBSS, use sw-crypto for GTK */
-	if(((vif->type == NL80211_IFTYPE_ADHOC) ||
-	    (vif->type == NL80211_IFTYPE_MESH_POINT)) &&
-	   !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE))
-		return -ENOSPC;
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("%s hardware based encryption for keyidx: %d, mac: %pM\n",
-		  cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
-		  sta ? sta->addr : bcast_addr));
-	rtlpriv->sec.being_setkey = true;
-	rtl_ips_nic_on(hw);
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-	/* <1> get encryption alg */
-
-	switch (key->cipher) {
-	case WLAN_CIPHER_SUITE_WEP40:
-		key_type = WEP40_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP40\n"));
-		break;
-	case WLAN_CIPHER_SUITE_WEP104:
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:WEP104\n"));
-		key_type = WEP104_ENCRYPTION;
-		break;
-	case WLAN_CIPHER_SUITE_TKIP:
-		key_type = TKIP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:TKIP\n"));
-		break;
-	case WLAN_CIPHER_SUITE_CCMP:
-		key_type = AESCCMP_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CCMP\n"));
-		break;
-	case WLAN_CIPHER_SUITE_AES_CMAC:
-		/* HW don't support CMAC encryption,
-		 * use software CMAC encryption */
-		key_type = AESCMAC_ENCRYPTION;
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("alg:CMAC\n"));
-		RT_TRACE(COMP_SEC, DBG_DMESG,
-				("HW don't support CMAC encryption, "
-				"use software CMAC encryption\n"));
-		err = -EOPNOTSUPP;
-		goto out_unlock;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("alg_err:%x!!!!:\n", key->cipher));
-		goto out_unlock;
-	}
-	if(key_type == WEP40_ENCRYPTION ||
-			key_type == WEP104_ENCRYPTION ||
-			vif->type == NL80211_IFTYPE_ADHOC)
-		rtlpriv->sec.use_defaultkey = true;
-
-	/* <2> get key_idx */
-	key_idx = (u8) (key->keyidx);
-	if (key_idx > 3)
-		goto out_unlock;
-	/* <3> if pairwise key enable_hw_sec */
-	group_key = !(key->flags & IEEE80211_KEY_FLAG_PAIRWISE);
-
-	/* wep always be group key, but there are two conditions:
-	 * 1) wep only: is just for wep enc, in this condition
-	 * rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION
-	 * will be true & enable_hw_sec will be set when wep
-	 * ke setting.
-	 * 2) wep(group) + AES(pairwise): some AP like cisco
-	 * may use it, in this condition enable_hw_sec will not
-	 * be set when wep key setting */
-	/* we must reset sec_info after lingked before set key,
-	 * or some flag will be wrong*/
-	if (vif->type == NL80211_IFTYPE_AP ||
-		vif->type == NL80211_IFTYPE_MESH_POINT) {
-		if (!group_key || key_type == WEP40_ENCRYPTION ||
-			key_type == WEP104_ENCRYPTION) {
-			if (group_key) {
-				wep_only = true;
-			}
-			rtlpriv->cfg->ops->enable_hw_sec(hw);
-		}
-	} else {
-		if ((!group_key) || (vif->type == NL80211_IFTYPE_ADHOC) ||
-	    	    rtlpriv->sec.pairwise_enc_algorithm == NO_ENCRYPTION) {
-			if (rtlpriv->sec.pairwise_enc_algorithm ==
-			    NO_ENCRYPTION &&
-		    	   (key_type == WEP40_ENCRYPTION ||
-		    	    key_type == WEP104_ENCRYPTION))
-				wep_only = true;
-			rtlpriv->sec.pairwise_enc_algorithm = key_type;
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set enable_hw_sec, key_type:%x(OPEN:0 WEP40:"
-				  "1 TKIP:2 AES:4 WEP104:5)\n", key_type));
-			rtlpriv->cfg->ops->enable_hw_sec(hw);
-		}
-	}
-	/* <4> set key based on cmd */
-	switch (cmd) {
-	case SET_KEY:
-		if (wep_only) {
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set WEP(group/pairwise) key\n"));
-			/* Pairwise key with an assigned MAC address. */
-			rtlpriv->sec.pairwise_enc_algorithm = key_type;
-			rtlpriv->sec.group_enc_algorithm = key_type;
-			/*set local buf about wep key. */
-			memcpy(rtlpriv->sec.key_buf[key_idx],
-			       key->key, key->keylen);
-			rtlpriv->sec.key_len[key_idx] = key->keylen;
-			memcpy(mac_addr, zero_addr, ETH_ALEN);
-		} else if (group_key) {	/* group key */
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set group key\n"));
-			/* group key */
-			rtlpriv->sec.group_enc_algorithm = key_type;
-			/*set local buf about group key. */
-			memcpy(rtlpriv->sec.key_buf[key_idx],
-			       key->key, key->keylen);
-			rtlpriv->sec.key_len[key_idx] = key->keylen;
-			memcpy(mac_addr, bcast_addr, ETH_ALEN);
-		} else {	/* pairwise key */
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("set pairwise key\n"));
-			if (!sta) {
-				RT_ASSERT(false, ("pairwise key withnot"
-						  "mac_addr\n"));
-
-				err = -EOPNOTSUPP;
-				goto out_unlock;
-			}
-			/* Pairwise key with an assigned MAC address. */
-			rtlpriv->sec.pairwise_enc_algorithm = key_type;
-			/*set local buf about pairwise key. */
-			memcpy(rtlpriv->sec.key_buf[PAIRWISE_KEYIDX],
-			       key->key, key->keylen);
-			rtlpriv->sec.key_len[PAIRWISE_KEYIDX] = key->keylen;
-			rtlpriv->sec.pairwise_key =
-			    rtlpriv->sec.key_buf[PAIRWISE_KEYIDX];
-			memcpy(mac_addr, sta->addr, ETH_ALEN);
-		}
-		rtlpriv->cfg->ops->set_key(hw, key_idx, mac_addr,
-					   group_key, key_type, wep_only,
-					   false);
-		/* <5> tell mac80211 do something: */
-		/*must use sw generate IV, or can not work !!!!. */
-		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
-		key->hw_key_idx = key_idx;
-		if (key_type == TKIP_ENCRYPTION)
-			key->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
-		/*use software CCMP encryption for management frames (MFP) */
-		if (key_type == AESCCMP_ENCRYPTION)
-			key->flags |= IEEE80211_KEY_FLAG_SW_MGMT;
-		break;
-	case DISABLE_KEY:
-		RT_TRACE(COMP_SEC, DBG_DMESG,
-			 ("disable key delete one entry\n"));
-		/*set local buf about wep key. */
-		if (vif->type == NL80211_IFTYPE_AP ||
-			vif->type == NL80211_IFTYPE_MESH_POINT) {
-			if (sta)
-				rtl_cam_del_entry(hw, sta->addr);
-		}
-		memset(rtlpriv->sec.key_buf[key_idx], 0, key->keylen);
-		rtlpriv->sec.key_len[key_idx] = 0;
-		memcpy(mac_addr, zero_addr, ETH_ALEN);
-		/*
-		 *mac80211 will delete entrys one by one,
-		 *so don't use rtl_cam_reset_all_entry
-		 *or clear all entry here.
-		 */
-		rtl_cam_delete_one_entry(hw, mac_addr, key_idx);
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("cmd_err:%x!!!!:\n", cmd));
-	}
-out_unlock:
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-	rtlpriv->sec.being_setkey = false;
-	return err;
-}
-
-static void rtl_op_rfkill_poll(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	bool radio_state;
-	bool blocked;
-	u8 valid = 0;
-
-	if (!test_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status))
-		return;
-
-	mutex_lock(&rtlpriv->locks.conf_mutex);
-
-	/*if Radio On return true here */
-	radio_state = rtlpriv->cfg->ops->radio_onoff_checking(hw, &valid);
-
-	if (valid) {
-		if (unlikely(radio_state != rtlpriv->rfkill.rfkill_state)) {
-			rtlpriv->rfkill.rfkill_state = radio_state;
-
-			RT_TRACE(COMP_RF, DBG_DMESG,
-				 (KERN_INFO "wireless radio switch turned %s\n",
-				  radio_state ? "on" : "off"));
-
-			blocked = (rtlpriv->rfkill.rfkill_state == 1) ? 0 : 1;
-			wiphy_rfkill_set_hw_state(hw->wiphy, blocked);
-		}
-	}
-
-	mutex_unlock(&rtlpriv->locks.conf_mutex);
-}
-
-/* this function is called by mac80211 to flush tx buffer
- * before switch channel or power save, or tx buffer packet
- * maybe send after offchannel or rf sleep, this may cause
- * dis-association by AP */
-static void rtl_op_flush(struct ieee80211_hw *hw,
-			 struct ieee80211_vif *vif,
-			 u32 queues, bool drop)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->intf_ops->flush)
-		rtlpriv->intf_ops->flush(hw, queues, drop);
-}
-
-const struct ieee80211_ops rtl_ops = {
-	.start = rtl_op_start,
-	.stop = rtl_op_stop,
-	.tx = rtl_op_tx,
-	.add_interface = rtl_op_add_interface,
-	.remove_interface = rtl_op_remove_interface,
-	.change_interface = rtl_op_change_interface,
-	.config = rtl_op_config,
-	.configure_filter = rtl_op_configure_filter,
-	.set_key = rtl_op_set_key,
-	.conf_tx = rtl_op_conf_tx,
-	.bss_info_changed = rtl_op_bss_info_changed,
-	.get_tsf = rtl_op_get_tsf,
-	.set_tsf = rtl_op_set_tsf,
-	.reset_tsf = rtl_op_reset_tsf,
-	.sta_notify = rtl_op_sta_notify,
-	.ampdu_action = rtl_op_ampdu_action,
-	.sw_scan_start = rtl_op_sw_scan_start,
-	.sw_scan_complete = rtl_op_sw_scan_complete,
-	.rfkill_poll = rtl_op_rfkill_poll,
-	.sta_add = rtl_op_sta_add,
-	.sta_remove = rtl_op_sta_remove,
-	.flush = rtl_op_flush,
-};
diff --git a/drivers/staging/rtl8821ae/core.h b/drivers/staging/rtl8821ae/core.h
deleted file mode 100644
index f0c74e9239fd..000000000000
--- a/drivers/staging/rtl8821ae/core.h
+++ /dev/null
@@ -1,43 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_CORE_H__
-#define __RTL_CORE_H__
-
-#define RTL_SUPPORTED_FILTERS		\
-	(FIF_PROMISC_IN_BSS | \
-	FIF_ALLMULTI | FIF_CONTROL | \
-	FIF_OTHER_BSS | \
-	FIF_FCSFAIL | \
-	FIF_BCN_PRBRESP_PROMISC)
-
-#define RTL_SUPPORTED_CTRL_FILTER	0xFF
-
-extern const struct ieee80211_ops rtl_ops;
-#endif
diff --git a/drivers/staging/rtl8821ae/debug.c b/drivers/staging/rtl8821ae/debug.c
deleted file mode 100644
index 8aefbf10a45e..000000000000
--- a/drivers/staging/rtl8821ae/debug.c
+++ /dev/null
@@ -1,983 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "cam.h"
-
-#define GET_INODE_DATA(__node)		PDE_DATA(__node)
-
-void rtl_dbgp_flag_init(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 i;
-
-	rtlpriv->dbg.global_debuglevel = DBG_DMESG;
-
-	rtlpriv->dbg.global_debugcomponents =
-		COMP_ERR |
-		COMP_FW |
-		COMP_INIT |
-		COMP_RECV |
-		COMP_SEND |
-	  	COMP_MLME |
-	  	COMP_SCAN |
-	  	COMP_INTR |
-	  	COMP_LED |
-	  	COMP_SEC |
-	  	COMP_BEACON |
-	  	COMP_RATE |
-	  	COMP_RXDESC |
-	  	COMP_DIG |
-	  	COMP_TXAGC |
-		COMP_POWER |
-	  	COMP_POWER_TRACKING |
-	  	COMP_BB_POWERSAVING |
-	  	COMP_SWAS |
-	  	COMP_RF |
-	  	COMP_TURBO |
-	  	COMP_RATR |
-	  	COMP_CMD |
-	  	COMP_EASY_CONCURRENT |
-	  	COMP_EFUSE |
-	  	COMP_QOS | COMP_MAC80211 | COMP_REGD |
-		COMP_CHAN |
-		COMP_BT_COEXIST |
-		COMP_IQK |
-		0;
-
-	for (i = 0; i < DBGP_TYPE_MAX; i++)
-		rtlpriv->dbg.dbgp_type[i] = 0;
-
-	/*Init Debug flag enable condition */
-}
-
-struct proc_dir_entry *proc_topdir;
-static int rtl_proc_get_mac_0(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x000;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_0(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_0, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_0 = {
-	.open = dl_proc_open_mac_0,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_1(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x100;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_1(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_1, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_1 = {
-	.open = dl_proc_open_mac_1,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_2(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x200;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_2(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_2, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_2 = {
-	.open = dl_proc_open_mac_2,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_3(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x300;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_3(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_3, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_3 = {
-	.open = dl_proc_open_mac_3,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_4(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x400;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_4(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_4, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_4 = {
-	.open = dl_proc_open_mac_4,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_5(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x500;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_5(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_5, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_5 = {
-	.open = dl_proc_open_mac_5,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_6(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x600;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_6(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_6, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_6 = {
-	.open = dl_proc_open_mac_6,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_mac_7(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i, n, page;
-	int max = 0xff;
-	page = 0x700;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_read_dword(rtlpriv, (page | n)));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_mac_7(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_mac_7, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_mac_7 = {
-	.open = dl_proc_open_mac_7,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_8(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0x800;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_8(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_8, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_8 = {
-	.open = dl_proc_open_bb_8,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_9(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0x900;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_9(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_9, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_9 = {
-	.open = dl_proc_open_bb_9,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_a(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xa00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_a(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_a, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_a = {
-	.open = dl_proc_open_bb_a,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_b(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xb00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_b(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_b, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_b = {
-	.open = dl_proc_open_bb_b,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_c(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xc00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_c(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_c, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_c = {
-	.open = dl_proc_open_bb_c,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_d(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xd00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_d(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_d, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_d = {
-	.open = dl_proc_open_bb_d,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_e(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xe00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_e(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_e, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_e = {
-	.open = dl_proc_open_bb_e,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_bb_f(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n, page;
-	int max = 0xff;
-	page = 0xf00;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n + page);
-		for (i = 0; i < 4 && n <= max; i++, n += 4)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_bbreg(hw, (page | n), 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_bb_f(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_bb_f, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_bb_f = {
-	.open = dl_proc_open_bb_f,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_reg_rf_a(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n;
-	int max = 0x40;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n);
-		for (i = 0; i < 4 && n <= max; n += 1, i++)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_rfreg(hw, RF90_PATH_A, n, 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_rf_a(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_reg_rf_a, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_rf_a = {
-	.open = dl_proc_open_rf_a,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_reg_rf_b(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	int i, n;
-	int max = 0x40;
-
-	for (n = 0; n <= max; ) {
-		seq_printf(m, "\n%8.8x  ", n);
-		for (i = 0; i < 4 && n <= max; n += 1, i++)
-			seq_printf(m, "%8.8x    ",
-				   rtl_get_rfreg(hw, RF90_PATH_B, n,
-				   		 0xffffffff));
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_rf_b(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_reg_rf_b, GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_rf_b = {
-	.open = dl_proc_open_rf_b,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_cam_register_1(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 target_cmd = 0;
-	u32 target_val=0;
-	u8 entry_i=0;
-	u32 ulstatus;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	seq_puts(m,
-	    "\n#################### SECURITY CAM (0-10) ##################\n ");
-
-	for (j = 0; j < 11; j++) {
-		seq_printf(m, "\nD:  %2x > ", j);
-	 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-	   		/* polling bit, and No Write enable, and address  */
-			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
-			target_cmd = target_cmd | BIT(31);
-
-			/* Check polling bit is clear */
-			while ((i--) >= 0) {
-				ulstatus = rtl_read_dword(rtlpriv,
-						rtlpriv->cfg->maps[RWCAM]);
-				if (ulstatus & BIT(31)) {
-					continue;
-				} else {
-					break;
-				}
-			}
-
-	  		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_cmd);
-	  	 	target_val = rtl_read_dword(rtlpriv,
-						    rtlpriv->cfg->maps[RCAMO]);
-			seq_printf(m, "%8.8x ", target_val);
-	 	}
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_cam_1(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_cam_register_1,
-			   GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_cam_1 = {
-	.open = dl_proc_open_cam_1,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_cam_register_2(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 target_cmd = 0;
-	u32 target_val = 0;
-	u8 entry_i = 0;
-	u32 ulstatus;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	seq_puts(m,
-	    "\n################### SECURITY CAM (11-21) ##################\n ");
-
-	for (j = 11; j < 22; j++) {
-		seq_printf(m, "\nD:  %2x > ", j);
-	 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			target_cmd = entry_i + CAM_CONTENT_COUNT * j;
-			target_cmd = target_cmd | BIT(31);
-
-			while ((i--) >= 0) {
-				ulstatus = rtl_read_dword(rtlpriv,
-						rtlpriv->cfg->maps[RWCAM]);
-				if (ulstatus & BIT(31)) {
-					continue;
-				} else {
-					break;
-				}
-			}
-
-	  		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_cmd);
-	  	 	target_val = rtl_read_dword(rtlpriv,
-						    rtlpriv->cfg->maps[RCAMO]);
-			seq_printf(m, "%8.8x ", target_val);
-	 	}
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_cam_2(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_cam_register_2,
-			   GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_cam_2 = {
-	.open = dl_proc_open_cam_2,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-static int rtl_proc_get_cam_register_3(struct seq_file *m, void *v)
-{
-	struct ieee80211_hw *hw = m->private;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 target_cmd = 0;
-	u32 target_val = 0;
-	u8 entry_i = 0;
-	u32 ulstatus;
-	int i = 100, j = 0;
-
-	/* This dump the current register page */
-	seq_puts(m,
-	    "\n################### SECURITY CAM (22-31) ##################\n ");
-
-	for (j = 22; j < TOTAL_CAM_ENTRY; j++) {
-		seq_printf(m, "\nD:  %2x > ", j);
-	 	for (entry_i = 0; entry_i < CAM_CONTENT_COUNT; entry_i++) {
-			target_cmd = entry_i+CAM_CONTENT_COUNT*j;
-			target_cmd = target_cmd | BIT(31);
-
-			while ((i--) >= 0) {
-				ulstatus = rtl_read_dword(rtlpriv,
-						rtlpriv->cfg->maps[RWCAM]);
-				if (ulstatus & BIT(31)) {
-					continue;
-				} else {
-					break;
-				}
-			}
-
-	  		rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[RWCAM],
-					target_cmd);
-	  	 	target_val = rtl_read_dword(rtlpriv,
-						    rtlpriv->cfg->maps[RCAMO]);
-			seq_printf(m, "%8.8x ", target_val);
-	 	}
-	}
-	seq_puts(m, "\n");
-	return 0;
-}
-
-static int dl_proc_open_cam_3(struct inode *inode, struct file *file)
-{
-	return single_open(file, rtl_proc_get_cam_register_3,
-			   GET_INODE_DATA(inode));
-}
-
-static const struct file_operations file_ops_cam_3 = {
-	.open = dl_proc_open_cam_3,
-	.read = seq_read,
-	.llseek = seq_lseek,
-	.release = seq_release,
-};
-
-void rtl_proc_add_one(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct proc_dir_entry *entry;
-
-	snprintf(rtlpriv->dbg.proc_name, 18, "%x-%x-%x-%x-%x-%x",
-		rtlefuse->dev_addr[0], rtlefuse->dev_addr[1],
-		rtlefuse->dev_addr[2], rtlefuse->dev_addr[3],
-		rtlefuse->dev_addr[4], rtlefuse->dev_addr[5]);
-
-	rtlpriv->dbg.proc_dir = proc_mkdir(rtlpriv->dbg.proc_name, proc_topdir);
-	if (!rtlpriv->dbg.proc_dir) {
-		RT_TRACE(COMP_INIT, DBG_EMERG, ("Unable to init "
-			"/proc/net/%s/%s\n", rtlpriv->cfg->name,
-			rtlpriv->dbg.proc_name));
-		return;
-	}
-
-	entry = proc_create_data("mac-0", S_IFREG | S_IRUGO,
-				  rtlpriv->dbg.proc_dir, &file_ops_mac_0, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, DBG_EMERG,
-			 ("Unable to initialize /proc/net/%s/%s/mac-0\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-1", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_1, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-1\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-2", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_2, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-2\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-3", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_3, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-3\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-4", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_4, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-4\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-5", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_5, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-5\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-6", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_6, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-6\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("mac-7", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_mac_7, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/mac-7\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-8", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_8, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-8\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-9", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_9, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-9\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-a", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_a, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-a\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-b", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_b, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-b\n",
-		      rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-c", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_c, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-c\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-d", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_d, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-d\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-e", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_e, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-e\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("bb-f", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_bb_f, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/bb-f\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("rf-a", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_rf_a, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/rf-a\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("rf-b", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_rf_b, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/rf-b\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("cam-1", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_cam_1, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/cam-1\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("cam-2", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_cam_2, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/cam-2\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-
-	entry = proc_create_data("cam-3", S_IFREG | S_IRUGO,
-				 rtlpriv->dbg.proc_dir, &file_ops_cam_3, hw);
-	if (!entry)
-		RT_TRACE(COMP_INIT, COMP_ERR,
-			 ("Unable to initialize /proc/net/%s/%s/cam-3\n",
-			  rtlpriv->cfg->name, rtlpriv->dbg.proc_name));
-}
-
-void rtl_proc_remove_one(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->dbg.proc_dir) {
-		remove_proc_entry("mac-0", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-1", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-2", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-3", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-4", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-5", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-6", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("mac-7", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-8", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-9", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-a", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-b", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-c", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-d", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-e", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("bb-f", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("rf-a", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("rf-b", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("cam-1", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("cam-2", rtlpriv->dbg.proc_dir);
-		remove_proc_entry("cam-3", rtlpriv->dbg.proc_dir);
-
-		remove_proc_entry(rtlpriv->dbg.proc_name, proc_topdir);
-
-		rtlpriv->dbg.proc_dir = NULL;
-	}
-}
-
-void rtl_proc_add_topdir(void)
-{
-	proc_topdir = proc_mkdir("rtlwifi", init_net.proc_net);
-}
-
-void rtl_proc_remove_topdir(void)
-{
-	if (proc_topdir)
-		remove_proc_entry("rtlwifi", init_net.proc_net);
-}
diff --git a/drivers/staging/rtl8821ae/debug.h b/drivers/staging/rtl8821ae/debug.h
deleted file mode 100644
index 6c0a553e98b7..000000000000
--- a/drivers/staging/rtl8821ae/debug.h
+++ /dev/null
@@ -1,227 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_DEBUG_H__
-#define __RTL_DEBUG_H__
-
-/*--------------------------------------------------------------
-			Debug level
---------------------------------------------------------------*/
-/*
- *Fatal bug.
- *For example, Tx/Rx/IO locked up,
- *memory access violation,
- *resource allocation failed,
- *unexpected HW behavior, HW BUG
- *and so on.
- */
-#define DBG_EMERG			0
-
-/*
- *Abnormal, rare, or unexpected cases.
- *For example, Packet/IO Ctl canceled,
- *device suprisely unremoved and so on.
- */
-#define	DBG_WARNING			2
-
-/*
- *Normal case driver developer should
- *open, we can see link status like
- *assoc/AddBA/DHCP/adapter start and
- *so on basic and useful informations.
- */
-#define DBG_DMESG			3
-
-/*
- *Normal case with useful information
- *about current SW or HW state.
- *For example, Tx/Rx descriptor to fill,
- *Tx/Rx descriptor completed status,
- *SW protocol state change, dynamic
- *mechanism state change and so on.
- */
-#define DBG_LOUD			4
-
-/*
- *Normal case with detail execution
- *flow or information.
- */
-#define	DBG_TRACE			5
-
-/*--------------------------------------------------------------
-		Define the rt_trace components
---------------------------------------------------------------*/
-#define COMP_ERR			BIT(0)
-#define COMP_FW				BIT(1)
-#define COMP_INIT			BIT(2)	/*For init/deinit */
-#define COMP_RECV			BIT(3)	/*For Rx. */
-#define COMP_SEND			BIT(4)	/*For Tx. */
-#define COMP_MLME			BIT(5)	/*For MLME. */
-#define COMP_SCAN			BIT(6)	/*For Scan. */
-#define COMP_INTR			BIT(7)	/*For interrupt Related. */
-#define COMP_LED			BIT(8)	/*For LED. */
-#define COMP_SEC			BIT(9)	/*For sec. */
-#define COMP_BEACON			BIT(10)	/*For beacon. */
-#define COMP_RATE			BIT(11)	/*For rate. */
-#define COMP_RXDESC			BIT(12)	/*For rx desc. */
-#define COMP_DIG			BIT(13)	/*For DIG */
-#define COMP_TXAGC			BIT(14)	/*For Tx power */
-#define COMP_HIPWR			BIT(15)	/*For High Power Mechanism */
-#define COMP_POWER			BIT(16)	/*For lps/ips/aspm. */
-#define COMP_POWER_TRACKING	BIT(17)	/*For TX POWER TRACKING */
-#define COMP_BB_POWERSAVING	BIT(18)
-#define COMP_SWAS			BIT(19)	/*For SW Antenna Switch */
-#define COMP_RF				BIT(20)	/*For RF. */
-#define COMP_TURBO			BIT(21)	/*For EDCA TURBO. */
-#define COMP_RATR			BIT(22)
-#define COMP_CMD			BIT(23)
-#define COMP_EFUSE			BIT(24)
-#define COMP_QOS			BIT(25)
-#define COMP_MAC80211		BIT(26)
-#define COMP_REGD			BIT(27)
-#define COMP_CHAN			BIT(28)
-#define COMP_EASY_CONCURRENT 		BIT(29)
-#define COMP_BT_COEXIST			BIT(30)
-#define COMP_IQK			BIT(31)
-
-/*--------------------------------------------------------------
-		Define the rt_print components
---------------------------------------------------------------*/
-/* Define EEPROM and EFUSE  check module bit*/
-#define EEPROM_W			BIT(0)
-#define EFUSE_PG			BIT(1)
-#define EFUSE_READ_ALL		BIT(2)
-
-/* Define init check for module bit*/
-#define	INIT_EEPROM			BIT(0)
-#define	INIT_TxPower		BIT(1)
-#define	INIT_IQK			BIT(2)
-#define	INIT_RF				BIT(3)
-
-/* Define PHY-BB/RF/MAC check module bit */
-#define	PHY_BBR				BIT(0)
-#define	PHY_BBW				BIT(1)
-#define	PHY_RFR				BIT(2)
-#define	PHY_RFW				BIT(3)
-#define	PHY_MACR			BIT(4)
-#define	PHY_MACW			BIT(5)
-#define	PHY_ALLR			BIT(6)
-#define	PHY_ALLW			BIT(7)
-#define	PHY_TXPWR			BIT(8)
-#define	PHY_PWRDIFF			BIT(9)
-
-/* Define Dynamic Mechanism check module bit --> FDM */
-#define WA_IOT				BIT(0)
-#define DM_PWDB				BIT(1)
-#define DM_MONITOR			BIT(2)
-#define DM_DIG				BIT(3)
-#define DM_EDCA_TURBO		BIT(4)
-
-enum dbgp_flag_e {
-	FQOS = 0,
-	FTX = 1,
-	FRX = 2,
-	FSEC = 3,
-	FMGNT = 4,
-	FMLME = 5,
-	FRESOURCE = 6,
-	FBEACON = 7,
-	FISR = 8,
-	FPHY = 9,
-	FMP = 10,
-	FEEPROM = 11,
-	FPWR = 12,
-	FDM = 13,
-	FDBGCtrl = 14,
-	FC2H = 15,
-	FBT = 16,
-	FINIT = 17,
-	FIOCTL = 18,
-	DBGP_TYPE_MAX
-};
-
-#define RT_ASSERT(_exp,fmt)				\
-	do { \
-		if(!(_exp))	{			\
-			printk(KERN_DEBUG "%s:%s(): ", KBUILD_MODNAME, \
-			__func__);	\
-			printk fmt;			\
-		} \
-	} while(0);
-
-#define	RT_DISP(dbgtype, dbgflag, printstr)
-
-#define RT_TRACE(comp, level, fmt)\
-	do { \
-		if(unlikely(((comp) & rtlpriv->dbg.global_debugcomponents) && \
-			((level) <= rtlpriv->dbg.global_debuglevel))) {\
-			printk(KERN_DEBUG "%s-%d:%s():<%lx-%x> ", \
-			KBUILD_MODNAME, \
-			rtlpriv->rtlhal.interfaceindex, __func__, \
-			in_interrupt(), in_atomic());	\
-			printk fmt; 			\
-		}\
-	} while(0);
-
-#define RTPRINT(rtlpriv, dbgtype, dbgflag, printstr)	\
-	do {						\
-		if (unlikely(rtlpriv->dbg.dbgp_type[dbgtype] & dbgflag)) { \
-			printk(KERN_DEBUG "%s: ", KBUILD_MODNAME);	\
-			printk printstr;		\
-		}					\
-	} while(0);
-
-#define RT_PRINT_DATA(rtlpriv, _comp, _level, _titlestring, _hexdata, \
-		_hexdatalen) \
-	do {\
-		if(unlikely(((_comp) & rtlpriv->dbg.global_debugcomponents ) &&\
-			(_level <= rtlpriv->dbg.global_debuglevel )))	{ \
-			int __i;					\
-			u8*	ptr = (u8*)_hexdata;			\
-			printk(KERN_DEBUG "%s: ", KBUILD_MODNAME);	\
-			printk(KERN_DEBUG "In process \"%s\" (pid %i):", \
-					current->comm, 	\
-					current->pid); \
-			printk(_titlestring);		\
-			for( __i=0; __i<(int)_hexdatalen; __i++ ) {	\
-				printk("%02X%s", ptr[__i], (((__i + 1) % 4) \
-							== 0)?"  ":" ");\
-				if (((__i + 1) % 16) == 0)	\
-					printk("\n");	\
-			}				\
-			printk(KERN_DEBUG "\n");			\
-		} \
-	} while(0);
-
-void rtl_dbgp_flag_init(struct ieee80211_hw *hw);
-void rtl_proc_add_one(struct ieee80211_hw *hw);
-void rtl_proc_remove_one(struct ieee80211_hw *hw);
-void rtl_proc_add_topdir(void);
-void rtl_proc_remove_topdir(void);
-#endif
diff --git a/drivers/staging/rtl8821ae/efuse.c b/drivers/staging/rtl8821ae/efuse.c
deleted file mode 100644
index 206012cf3601..000000000000
--- a/drivers/staging/rtl8821ae/efuse.c
+++ /dev/null
@@ -1,1283 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include "wifi.h"
-#include "efuse.h"
-#include "btcoexist/halbt_precomp.h"
-#include <linux/export.h>
-
-static const u8 MAX_PGPKT_SIZE = 9;
-static const u8 PGPKT_DATA_SIZE = 8;
-static const int EFUSE_MAX_SIZE = 512;
-
-static const struct efuse_map RTL8712_SDIO_EFUSE_TABLE[] = {
-	{0, 0, 0, 2},
-	{0, 1, 0, 2},
-	{0, 2, 0, 2},
-	{1, 0, 0, 1},
-	{1, 0, 1, 1},
-	{1, 1, 0, 1},
-	{1, 1, 1, 3},
-	{1, 3, 0, 17},
-	{3, 3, 1, 48},
-	{10, 0, 0, 6},
-	{10, 3, 0, 1},
-	{10, 3, 1, 1},
-	{11, 0, 0, 28}
-};
-
-static void efuse_shadow_read_1byte(struct ieee80211_hw *hw, u16 offset,
-				    u8 * value);
-static void efuse_shadow_read_2byte(struct ieee80211_hw *hw, u16 offset,
-				    u16 * value);
-static void efuse_shadow_read_4byte(struct ieee80211_hw *hw, u16 offset,
-				    u32 * value);
-static void efuse_shadow_write_1byte(struct ieee80211_hw *hw, u16 offset,
-				     u8 value);
-static void efuse_shadow_write_2byte(struct ieee80211_hw *hw, u16 offset,
-				     u16 value);
-static void efuse_shadow_write_4byte(struct ieee80211_hw *hw, u16 offset,
-				     u32 value);
-static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr,
-				u8 data);
-static void efuse_read_all_map(struct ieee80211_hw *hw, u8 * efuse);
-static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset,
-				u8 *data);
-static int efuse_pg_packet_write(struct ieee80211_hw *hw, u8 offset,
-				 u8 word_en, u8 * data);
-static void efuse_word_enable_data_read(u8 word_en, u8 * sourdata,
-					u8 * targetdata);
-static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
-				       u16 efuse_addr, u8 word_en, u8 * data);
-static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite,
-					u8 pwrstate);
-static u16 efuse_get_current_size(struct ieee80211_hw *hw);
-static u8 efuse_calculate_word_cnts(u8 word_en);
-
-void efuse_initialize(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 bytetemp;
-	u8 temp;
-
-	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1);
-	temp = bytetemp | 0x20;
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN] + 1, temp);
-
-	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1);
-	temp = bytetemp & 0xFE;
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_ISO_CTRL] + 1, temp);
-
-	bytetemp = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3);
-	temp = bytetemp | 0x80;
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_TEST] + 3, temp);
-
-	rtl_write_byte(rtlpriv, 0x2F8, 0x3);
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
-
-}
-
-u8 efuse_read_1byte(struct ieee80211_hw *hw, u16 address)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 data;
-	u8 bytetemp;
-	u8 temp;
-	u32 k = 0;
-	const u32 efuse_real_content_len =
-		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
-
-	if (address < efuse_real_content_len) {
-		temp = address & 0xFF;
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-			       temp);
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
-		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-			       temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-		temp = bytetemp & 0x7F;
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
-			       temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-		while (!(bytetemp & 0x80)) {
-			bytetemp = rtl_read_byte(rtlpriv,
-						 rtlpriv->cfg->
-						 maps[EFUSE_CTRL] + 3);
-			k++;
-			if (k == 1000) {
-				k = 0;
-				break;
-			}
-		}
-		data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-		return data;
-	} else
-		return 0xFF;
-
-}
-/* EXPORT_SYMBOL(efuse_read_1byte); */
-
-void efuse_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 bytetemp;
-	u8 temp;
-	u32 k = 0;
-	const u32 efuse_real_content_len =
-		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("Addr=%x Data =%x\n", address, value));
-
-	if (address < efuse_real_content_len) {
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], value);
-
-		temp = address & 0xFF;
-		rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-			       temp);
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
-
-		temp = ((address >> 8) & 0x03) | (bytetemp & 0xFC);
-		rtl_write_byte(rtlpriv,
-			       rtlpriv->cfg->maps[EFUSE_CTRL] + 2, temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-		temp = bytetemp | 0x80;
-		rtl_write_byte(rtlpriv,
-			       rtlpriv->cfg->maps[EFUSE_CTRL] + 3, temp);
-
-		bytetemp = rtl_read_byte(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-
-		while (bytetemp & 0x80) {
-			bytetemp = rtl_read_byte(rtlpriv,
-						 rtlpriv->cfg->
-						 maps[EFUSE_CTRL] + 3);
-			k++;
-			if (k == 100) {
-				k = 0;
-				break;
-			}
-		}
-	}
-
-}
-
-void read_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 value32;
-	u8 readbyte;
-	u16 retry;
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-		       (_offset & 0xff));
-	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2);
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-		       ((_offset >> 8) & 0x03) | (readbyte & 0xfc));
-
-	readbyte = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3);
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3,
-		       (readbyte & 0x7f));
-
-	retry = 0;
-	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-	while (!(((value32 >> 24) & 0xff) & 0x80) && (retry < 10000)) {
-		value32 = rtl_read_dword(rtlpriv,
-					 rtlpriv->cfg->maps[EFUSE_CTRL]);
-		retry++;
-	}
-
-	udelay(50);
-	value32 = rtl_read_dword(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-
-	*pbuf = (u8) (value32 & 0xff);
-}
-
-void read_efuse(struct ieee80211_hw *hw, u16 _offset, u16 _size_byte, u8 *pbuf)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 efuse_tbl[rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]];
-	u8 rtemp8[1];
-	u16 efuse_addr = 0;
-	u8 offset, wren;
-	u8 u1temp = 0;
-	u16 i;
-	u16 j;
-	const u16 efuse_max_section =
-		rtlpriv->cfg->maps[EFUSE_MAX_SECTION_MAP];
-	const u32 efuse_real_content_len =
-		rtlpriv->cfg->maps[EFUSE_REAL_CONTENT_SIZE];
-	u16 efuse_word[efuse_max_section][EFUSE_MAX_WORD_UNIT];
-	u16 efuse_utilized = 0;
-	u8 efuse_usage;
-
-	if ((_offset + _size_byte) > rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]) {
-		RT_TRACE(COMP_EFUSE, DBG_LOUD,
-			 ("read_efuse(): Invalid offset(%#x) with read "
-			  "bytes(%#x)!!\n", _offset, _size_byte));
-		return;
-	}
-
-	for (i = 0; i < efuse_max_section; i++)
-		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++)
-			efuse_word[i][j] = 0xFFFF;
-
-	read_efuse_byte(hw, efuse_addr, rtemp8);
-	if (*rtemp8 != 0xFF) {
-		efuse_utilized++;
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
-			("Addr=%d\n", efuse_addr));
-		efuse_addr++;
-	}
-
-	while ((*rtemp8 != 0xFF) && (efuse_addr < efuse_real_content_len)) {
-		/*  Check PG header for section num.  */
-		if((*rtemp8 & 0x1F ) == 0x0F) {/* extended header */
-			u1temp =( (*rtemp8 & 0xE0) >> 5);
-			read_efuse_byte(hw, efuse_addr, rtemp8);
-
-			if((*rtemp8 & 0x0F) == 0x0F) {
-				efuse_addr++;
-				read_efuse_byte(hw, efuse_addr, rtemp8);
-
-				if (*rtemp8 != 0xFF &&
-				    (efuse_addr < efuse_real_content_len)) {
-					efuse_addr++;
-				}
-				continue;
-			} else {
-				offset = ((*rtemp8 & 0xF0) >> 1) | u1temp;
-				wren = (*rtemp8 & 0x0F);
-				efuse_addr++;
-			}
-		} else {
-			offset = ((*rtemp8 >> 4) & 0x0f);
-			wren = (*rtemp8 & 0x0f);
-		}
-
-		if (offset < efuse_max_section) {
-			RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
-				("offset-%d Worden=%x\n", offset, wren));
-
-			for (i = 0; i < EFUSE_MAX_WORD_UNIT; i++) {
-				if (!(wren & 0x01)) {
-					RTPRINT(rtlpriv, FEEPROM,
-						EFUSE_READ_ALL, ("Addr=%d\n",
-								 efuse_addr));
-
-					read_efuse_byte(hw, efuse_addr, rtemp8);
-					efuse_addr++;
-					efuse_utilized++;
-					efuse_word[offset][i] = (*rtemp8 &
-								 0xff);
-
-					if (efuse_addr >=
-					    efuse_real_content_len)
-						break;
-
-					RTPRINT(rtlpriv, FEEPROM,
-						EFUSE_READ_ALL, ("Addr=%d\n",
-								 efuse_addr));
-
-					read_efuse_byte(hw, efuse_addr, rtemp8);
-					efuse_addr++;
-					efuse_utilized++;
-					efuse_word[offset][i] |=
-					    (((u16) * rtemp8 << 8) & 0xff00);
-
-					if (efuse_addr >= efuse_real_content_len)
-						break;
-				}
-
-				wren >>= 1;
-			}
-		}
-
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_READ_ALL,
-			("Addr=%d\n", efuse_addr));
-		read_efuse_byte(hw, efuse_addr, rtemp8);
-		if (*rtemp8 != 0xFF && (efuse_addr < efuse_real_content_len)) {
-			efuse_utilized++;
-			efuse_addr++;
-		}
-	}
-
-	for (i = 0; i < efuse_max_section; i++) {
-		for (j = 0; j < EFUSE_MAX_WORD_UNIT; j++) {
-			efuse_tbl[(i * 8) + (j * 2)] =
-			    (efuse_word[i][j] & 0xff);
-			efuse_tbl[(i * 8) + ((j * 2) + 1)] =
-			    ((efuse_word[i][j] >> 8) & 0xff);
-		}
-	}
-
-	for (i = 0; i < _size_byte; i++)
-		pbuf[i] = efuse_tbl[_offset + i];
-
-	rtlefuse->efuse_usedbytes = efuse_utilized;
-	efuse_usage = (u8) ((efuse_utilized * 100) / efuse_real_content_len);
-	rtlefuse->efuse_usedpercentage = efuse_usage;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_BYTES,
-				      (u8 *) & efuse_utilized);
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_EFUSE_USAGE,
-				      (u8 *) & efuse_usage);
-}
-
-bool efuse_shadow_update_chk(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 section_idx, i, Base;
-	u16 words_need = 0, hdr_num = 0, totalbytes, efuse_used;
-	bool bwordchanged, bresult = true;
-
-	for (section_idx = 0; section_idx < 16; section_idx++) {
-		Base = section_idx * 8;
-		bwordchanged = false;
-
-		for (i = 0; i < 8; i = i + 2) {
-			if ((rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i] !=
-			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i]) ||
-			    (rtlefuse->efuse_map[EFUSE_INIT_MAP][Base + i + 1] !=
-			     rtlefuse->efuse_map[EFUSE_MODIFY_MAP][Base + i +
-								   1])) {
-				words_need++;
-				bwordchanged = true;
-			}
-		}
-
-		if (bwordchanged == true)
-			hdr_num++;
-	}
-
-	totalbytes = hdr_num + words_need * 2;
-	efuse_used = rtlefuse->efuse_usedbytes;
-
-	if ((totalbytes + efuse_used) >= (EFUSE_MAX_SIZE -
-		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))
-		bresult = false;
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("efuse_shadow_update_chk(): totalbytes(%#x), "
-		  "hdr_num(%#x), words_need(%#x), efuse_used(%d)\n",
-		  totalbytes, hdr_num, words_need, efuse_used));
-
-	return bresult;
-}
-
-void efuse_shadow_read(struct ieee80211_hw *hw, u8 type,
-		       u16 offset, u32 *value)
-{
-	if (type == 1)
-		efuse_shadow_read_1byte(hw, offset, (u8 *) value);
-	else if (type == 2)
-		efuse_shadow_read_2byte(hw, offset, (u16 *) value);
-	else if (type == 4)
-		efuse_shadow_read_4byte(hw, offset, (u32 *) value);
-
-}
-
-void efuse_shadow_write(struct ieee80211_hw *hw, u8 type, u16 offset,
-				u32 value)
-{
-	if (type == 1)
-		efuse_shadow_write_1byte(hw, offset, (u8) value);
-	else if (type == 2)
-		efuse_shadow_write_2byte(hw, offset, (u16) value);
-	else if (type == 4)
-		efuse_shadow_write_4byte(hw, offset, (u32) value);
-
-}
-
-bool efuse_shadow_update(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u16 i, offset, base;
-	u8 word_en = 0x0F;
-	u8 first_pg = false;
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
-
-	if (!efuse_shadow_update_chk(hw)) {
-		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
-		memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
-		       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-		       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-
-		RT_TRACE(COMP_EFUSE, DBG_LOUD,
-			 ("efuse out of capacity!!\n"));
-		return false;
-	}
-	efuse_power_switch(hw, true, true);
-
-	for (offset = 0; offset < 16; offset++) {
-
-		word_en = 0x0F;
-		base = offset * 8;
-
-		for (i = 0; i < 8; i++) {
-			if (first_pg == true) {
-
-				word_en &= ~(BIT(i / 2));
-
-				rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
-				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
-			} else {
-
-				if (rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] !=
-				    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i]) {
-					word_en &= ~(BIT(i / 2));
-
-					rtlefuse->efuse_map[EFUSE_INIT_MAP][base + i] =
-					    rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base + i];
-				}
-			}
-		}
-
-		if (word_en != 0x0F) {
-			u8 tmpdata[8];
-			memcpy(tmpdata, (&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][base]), 8);
-			RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_LOUD,
-				      ("U-efuse\n"), tmpdata, 8);
-
-			if (!efuse_pg_packet_write(hw, (u8) offset, word_en,
-						   tmpdata)) {
-				RT_TRACE(COMP_ERR, DBG_WARNING,
-					 ("PG section(%#x) fail!!\n", offset));
-				break;
-			}
-		}
-
-	}
-
-	efuse_power_switch(hw, true, false);
-	efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
-
-	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
-	       &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-	       rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD, ("\n"));
-	return true;
-}
-
-void rtl_efuse_shadow_map_update(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	if (rtlefuse->autoload_failflag == true) {
-		memset((&rtlefuse->efuse_map[EFUSE_INIT_MAP][0]),
-			0xFF, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-	} else {
-		efuse_read_all_map(hw, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0]);
-	}
-
-	memcpy(&rtlefuse->efuse_map[EFUSE_MODIFY_MAP][0],
-			&rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-			rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE]);
-
-}
-/* EXPORT_SYMBOL(rtl_efuse_shadow_map_update); */
-
-void efuse_force_write_vendor_Id(struct ieee80211_hw *hw)
-{
-	u8 tmpdata[8] = { 0xFF, 0xFF, 0xEC, 0x10, 0xFF, 0xFF, 0xFF, 0xFF };
-
-	efuse_power_switch(hw, true, true);
-
-	efuse_pg_packet_write(hw, 1, 0xD, tmpdata);
-
-	efuse_power_switch(hw, true, false);
-
-}
-
-void efuse_re_pg_section(struct ieee80211_hw *hw, u8 section_idx)
-{
-}
-
-static void efuse_shadow_read_1byte(struct ieee80211_hw *hw,
-				    u16 offset, u8 *value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
-}
-
-static void efuse_shadow_read_2byte(struct ieee80211_hw *hw,
-				    u16 offset, u16 *value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
-
-}
-
-static void efuse_shadow_read_4byte(struct ieee80211_hw *hw,
-				    u16 offset, u32 *value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	*value = rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset];
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] << 8;
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] << 16;
-	*value |= rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] << 24;
-}
-
-static void efuse_shadow_write_1byte(struct ieee80211_hw *hw,
-				     u16 offset, u8 value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value;
-}
-
-static void efuse_shadow_write_2byte(struct ieee80211_hw *hw,
-				     u16 offset, u16 value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] = value & 0x00FF;
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] = value >> 8;
-
-}
-
-static void efuse_shadow_write_4byte(struct ieee80211_hw *hw,
-				     u16 offset, u32 value)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset] =
-	    (u8) (value & 0x000000FF);
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 1] =
-	    (u8) ((value >> 8) & 0x0000FF);
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 2] =
-	    (u8) ((value >> 16) & 0x00FF);
-	rtlefuse->efuse_map[EFUSE_MODIFY_MAP][offset + 3] =
-	    (u8) ((value >> 24) & 0xFF);
-
-}
-
-int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmpidx = 0;
-	int bresult;
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 1,
-		       (u8) (addr & 0xff));
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-		       ((u8) ((addr >> 8) & 0x03)) |
-		       (rtl_read_byte(rtlpriv,
-				      rtlpriv->cfg->maps[EFUSE_CTRL] + 2) &
-			0xFC));
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0x72);
-
-	while (!(0x80 & rtl_read_byte(rtlpriv,
-				      rtlpriv->cfg->maps[EFUSE_CTRL] + 3))
-	       && (tmpidx < 100)) {
-		tmpidx++;
-	}
-
-	if (tmpidx < 100) {
-		*data = rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL]);
-		bresult = true;
-	} else {
-		*data = 0xff;
-		bresult = false;
-	}
-	return bresult;
-}
-/* EXPORT_SYMBOL(efuse_one_byte_read); */
-
-static int efuse_one_byte_write(struct ieee80211_hw *hw, u16 addr, u8 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmpidx = 0;
-	bool bresult;
-
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("Addr = %x Data=%x\n", addr, data));
-
-	rtl_write_byte(rtlpriv,
-		       rtlpriv->cfg->maps[EFUSE_CTRL] + 1, (u8) (addr & 0xff));
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 2,
-		       (rtl_read_byte(rtlpriv,
-			 rtlpriv->cfg->maps[EFUSE_CTRL] +
-			 2) & 0xFC) | (u8) ((addr >> 8) & 0x03));
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL], data);
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CTRL] + 3, 0xF2);
-
-	while ((0x80 & rtl_read_byte(rtlpriv,
-				     rtlpriv->cfg->maps[EFUSE_CTRL] + 3))
-	       && (tmpidx < 100)) {
-		tmpidx++;
-	}
-
-	if (tmpidx < 100)
-		bresult = true;
-	else
-		bresult = false;
-
-	return bresult;
-}
-
-static void efuse_read_all_map(struct ieee80211_hw *hw, u8 * efuse)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	efuse_power_switch(hw, false, true);
-	read_efuse(hw, 0, rtlpriv->cfg->maps[EFUSE_HWSET_MAX_SIZE], efuse);
-	efuse_power_switch(hw, false, false);
-}
-
-static void efuse_read_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
-				u8 efuse_data, u8 offset, u8 *tmpdata,
-				u8 *readstate)
-{
-	bool bdataempty = true;
-	u8 hoffset;
-	u8 tmpidx;
-	u8 hworden;
-	u8 word_cnts;
-
-	hoffset = (efuse_data >> 4) & 0x0F;
-	hworden = efuse_data & 0x0F;
-	word_cnts = efuse_calculate_word_cnts(hworden);
-
-	if (hoffset == offset) {
-		for (tmpidx = 0; tmpidx < word_cnts * 2; tmpidx++) {
-			if (efuse_one_byte_read(hw, *efuse_addr + 1 + tmpidx,
-						&efuse_data)) {
-				tmpdata[tmpidx] = efuse_data;
-				if (efuse_data != 0xff)
-					bdataempty = true;
-			}
-		}
-
-		if (bdataempty == true) {
-			*readstate = PG_STATE_DATA;
-		} else {
-			*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
-			*readstate = PG_STATE_HEADER;
-		}
-
-	} else {
-		*efuse_addr = *efuse_addr + (word_cnts * 2) + 1;
-		*readstate = PG_STATE_HEADER;
-	}
-}
-
-static int efuse_pg_packet_read(struct ieee80211_hw *hw, u8 offset, u8 *data)
-{
-	u8 readstate = PG_STATE_HEADER;
-
-	bool bcontinual = true;
-
-	u8 efuse_data, word_cnts = 0;
-	u16 efuse_addr = 0;
-	u8 hworden = 0;
-	u8 tmpdata[8];
-
-	if (data == NULL)
-		return false;
-	if (offset > 15)
-		return false;
-
-	memset(data, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
-	memset(tmpdata, 0xff, PGPKT_DATA_SIZE * sizeof(u8));
-
-	while (bcontinual && (efuse_addr < EFUSE_MAX_SIZE)) {
-		if (readstate & PG_STATE_HEADER) {
-			if (efuse_one_byte_read(hw, efuse_addr, &efuse_data)
-			    && (efuse_data != 0xFF))
-				efuse_read_data_case1(hw, &efuse_addr, efuse_data, offset,
-							tmpdata, &readstate);
-			else
-				bcontinual = false;
-		} else if (readstate & PG_STATE_DATA) {
-			efuse_word_enable_data_read(hworden, tmpdata, data);
-			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
-			readstate = PG_STATE_HEADER;
-		}
-
-	}
-
-	if ((data[0] == 0xff) && (data[1] == 0xff) &&
-	    (data[2] == 0xff) && (data[3] == 0xff) &&
-	    (data[4] == 0xff) && (data[5] == 0xff) &&
-	    (data[6] == 0xff) && (data[7] == 0xff))
-		return false;
-	else
-		return true;
-
-}
-
-static void efuse_write_data_case1(struct ieee80211_hw *hw, u16 *efuse_addr,
-				u8 efuse_data, u8 offset, int *bcontinual,
-				u8 *write_state, struct pgpkt_struct *target_pkt,
-				int *repeat_times, int *bresult, u8 word_en)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct pgpkt_struct tmp_pkt;
-	int bdataempty = true;
-	u8 originaldata[8 * sizeof(u8)];
-	u8 badworden = 0x0F;
-	u8 match_word_en, tmp_word_en;
-	u8 tmpindex;
-	u8 tmp_header = efuse_data;
-	u8 tmp_word_cnts;
-
-	tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
-	tmp_pkt.word_en = tmp_header & 0x0F;
-	tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
-
-	if (tmp_pkt.offset != target_pkt->offset) {
-		*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
-		*write_state = PG_STATE_HEADER;
-	} else {
-		for (tmpindex = 0; tmpindex < (tmp_word_cnts * 2); tmpindex++) {
-			if (efuse_one_byte_read(hw,
-						(*efuse_addr + 1 + tmpindex),
-						&efuse_data) && (efuse_data != 0xFF))
-				bdataempty = false;
-		}
-
-		if (bdataempty == false) {
-			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
-			*write_state = PG_STATE_HEADER;
-		} else {
-			match_word_en = 0x0F;
-			if (!((target_pkt->word_en & BIT(0)) |
-			    (tmp_pkt.word_en & BIT(0))))
-				match_word_en &= (~BIT(0));
-
-			if (!((target_pkt->word_en & BIT(1)) |
-			    (tmp_pkt.word_en & BIT(1))))
-				match_word_en &= (~BIT(1));
-
-			if (!((target_pkt->word_en & BIT(2)) |
-			    (tmp_pkt.word_en & BIT(2))))
-				match_word_en &= (~BIT(2));
-
-			if (!((target_pkt->word_en & BIT(3)) |
-			    (tmp_pkt.word_en & BIT(3))))
-				match_word_en &= (~BIT(3));
-
-			if ((match_word_en & 0x0F) != 0x0F) {
-				badworden = efuse_word_enable_data_write(hw,
-							*efuse_addr + 1,
-							tmp_pkt.word_en,
-							target_pkt->data);
-
-				if (0x0F != (badworden & 0x0F))	{
-					u8 reorg_offset = offset;
-					u8 reorg_worden = badworden;
-					efuse_pg_packet_write(hw, reorg_offset,
-							      reorg_worden,
-							      originaldata);
-				}
-
-				tmp_word_en = 0x0F;
-				if ((target_pkt->word_en & BIT(0)) ^
-				    (match_word_en & BIT(0)))
-					tmp_word_en &= (~BIT(0));
-
-				if ((target_pkt->word_en & BIT(1)) ^
-				    (match_word_en & BIT(1)))
-					tmp_word_en &= (~BIT(1));
-
-				if ((target_pkt->word_en & BIT(2)) ^
-				    (match_word_en & BIT(2)))
-					tmp_word_en &= (~BIT(2));
-
-				if ((target_pkt->word_en & BIT(3)) ^
-				    (match_word_en & BIT(3)))
-					tmp_word_en &= (~BIT(3));
-
-				if ((tmp_word_en & 0x0F) != 0x0F) {
-					*efuse_addr = efuse_get_current_size(hw);
-					target_pkt->offset = offset;
-					target_pkt->word_en = tmp_word_en;
-				} else {
-					*bcontinual = false;
-				}
-				*write_state = PG_STATE_HEADER;
-				*repeat_times += 1;
-				if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-					*bcontinual = false;
-					*bresult = false;
-				}
-			} else {
-				*efuse_addr += (2 * tmp_word_cnts) + 1;
-				target_pkt->offset = offset;
-				target_pkt->word_en = word_en;
-				*write_state = PG_STATE_HEADER;
-			}
-		}
-	}
-	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, ("efuse PG_STATE_HEADER-1\n"));
-}
-
-static void efuse_write_data_case2(struct ieee80211_hw *hw, u16 *efuse_addr,
-				   int *bcontinual, u8 *write_state,
-				   struct pgpkt_struct target_pkt,
-				   int *repeat_times, int *bresult)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct pgpkt_struct tmp_pkt;
-	u8 pg_header;
-	u8 tmp_header;
-	u8 originaldata[8 * sizeof(u8)];
-	u8 tmp_word_cnts;
-	u8 badworden = 0x0F;
-
-	pg_header = ((target_pkt.offset << 4) & 0xf0) | target_pkt.word_en;
-	efuse_one_byte_write(hw, *efuse_addr, pg_header);
-	efuse_one_byte_read(hw, *efuse_addr, &tmp_header);
-
-	if (tmp_header == pg_header) {
-		*write_state = PG_STATE_DATA;
-	} else if (tmp_header == 0xFF) {
-		*write_state = PG_STATE_HEADER;
-		*repeat_times += 1;
-		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-			*bcontinual = false;
-			*bresult = false;
-		}
-	} else {
-		tmp_pkt.offset = (tmp_header >> 4) & 0x0F;
-		tmp_pkt.word_en = tmp_header & 0x0F;
-
-		tmp_word_cnts = efuse_calculate_word_cnts(tmp_pkt.word_en);
-
-		memset(originaldata, 0xff,  8 * sizeof(u8));
-
-		if (efuse_pg_packet_read(hw, tmp_pkt.offset, originaldata)) {
-			badworden = efuse_word_enable_data_write(hw,
-								*efuse_addr + 1,
-								tmp_pkt.word_en,
-								originaldata);
-
-			if (0x0F != (badworden & 0x0F)) {
-				u8 reorg_offset = tmp_pkt.offset;
-				u8 reorg_worden = badworden;
-				efuse_pg_packet_write(hw, reorg_offset,
-						      reorg_worden,
-						      originaldata);
-				*efuse_addr = efuse_get_current_size(hw);
-			} else {
-				*efuse_addr = *efuse_addr +
-					      (tmp_word_cnts * 2) + 1;
-			}
-		} else {
-			*efuse_addr = *efuse_addr + (tmp_word_cnts * 2) + 1;
-		}
-
-		*write_state = PG_STATE_HEADER;
-		*repeat_times += 1;
-		if (*repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-			*bcontinual = false;
-			*bresult = false;
-		}
-
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-			("efuse PG_STATE_HEADER-2\n"));
-	}
-}
-
-static int efuse_pg_packet_write(struct ieee80211_hw *hw,
-				 u8 offset, u8 word_en, u8 *data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct pgpkt_struct target_pkt;
-	u8 write_state = PG_STATE_HEADER;
-	int bcontinual = true, bdataempty = true, bresult = true;
-	u16 efuse_addr = 0;
-	u8 efuse_data;
-	u8 target_word_cnts = 0;
-	u8 badworden = 0x0F;
-	static int repeat_times = 0;
-
-	if (efuse_get_current_size(hw) >= (EFUSE_MAX_SIZE -
-		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
-		RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-			("efuse_pg_packet_write error \n"));
-		return false;
-	}
-
-	target_pkt.offset = offset;
-	target_pkt.word_en = word_en;
-
-	memset(target_pkt.data, 0xFF,  8 * sizeof(u8));
-
-	efuse_word_enable_data_read(word_en, data, target_pkt.data);
-	target_word_cnts = efuse_calculate_word_cnts(target_pkt.word_en);
-
-	RTPRINT(rtlpriv, FEEPROM, EFUSE_PG, ("efuse Power ON\n"));
-
-	while (bcontinual && (efuse_addr < (EFUSE_MAX_SIZE -
-		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN]))) {
-
-		if (write_state == PG_STATE_HEADER) {
-			bdataempty = true;
-			badworden = 0x0F;
-			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-				("efuse PG_STATE_HEADER\n"));
-
-			if (efuse_one_byte_read(hw, efuse_addr, &efuse_data) &&
-			    (efuse_data != 0xFF))
-				efuse_write_data_case1(hw, &efuse_addr,
-						       efuse_data, offset,
-						       &bcontinual,
-						       &write_state,
-						       &target_pkt,
-						       &repeat_times, &bresult,
-						       word_en);
-			else
-				efuse_write_data_case2(hw, &efuse_addr,
-						       &bcontinual,
-						       &write_state,
-						       target_pkt,
-						       &repeat_times,
-						       &bresult);
-
-		} else if (write_state == PG_STATE_DATA) {
-			RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-				("efuse PG_STATE_DATA\n"));
-			badworden = 0x0f;
-			badworden =
-			    efuse_word_enable_data_write(hw, efuse_addr + 1,
-							 target_pkt.word_en,
-							 target_pkt.data);
-
-			if ((badworden & 0x0F) == 0x0F) {
-				bcontinual = false;
-			} else {
-				efuse_addr =
-				    efuse_addr + (2 * target_word_cnts) + 1;
-
-				target_pkt.offset = offset;
-				target_pkt.word_en = badworden;
-				target_word_cnts =
-				    efuse_calculate_word_cnts(target_pkt.
-							      word_en);
-				write_state = PG_STATE_HEADER;
-				repeat_times++;
-				if (repeat_times > EFUSE_REPEAT_THRESHOLD_) {
-					bcontinual = false;
-					bresult = false;
-				}
-				RTPRINT(rtlpriv, FEEPROM, EFUSE_PG,
-					("efuse PG_STATE_HEADER-3\n"));
-			}
-		}
-	}
-
-	if (efuse_addr >= (EFUSE_MAX_SIZE -
-		rtlpriv->cfg->maps[EFUSE_OOB_PROTECT_BYTES_LEN])) {
-		RT_TRACE(COMP_EFUSE, DBG_LOUD,
-			 ("efuse_addr(%#x) Out of size!!\n", efuse_addr));
-	}
-
-	return true;
-}
-
-static void efuse_word_enable_data_read(u8 word_en, u8 * sourdata,
-					u8 *targetdata)
-{
-	if (!(word_en & BIT(0))) {
-		targetdata[0] = sourdata[0];
-		targetdata[1] = sourdata[1];
-	}
-
-	if (!(word_en & BIT(1))) {
-		targetdata[2] = sourdata[2];
-		targetdata[3] = sourdata[3];
-	}
-
-	if (!(word_en & BIT(2))) {
-		targetdata[4] = sourdata[4];
-		targetdata[5] = sourdata[5];
-	}
-
-	if (!(word_en & BIT(3))) {
-		targetdata[6] = sourdata[6];
-		targetdata[7] = sourdata[7];
-	}
-}
-
-static u8 efuse_word_enable_data_write(struct ieee80211_hw *hw,
-				       u16 efuse_addr, u8 word_en, u8 *data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u16 tmpaddr;
-	u16 start_addr = efuse_addr;
-	u8 badworden = 0x0F;
-	u8 tmpdata[8];
-
-	memset(tmpdata, 0xff, PGPKT_DATA_SIZE);
-	RT_TRACE(COMP_EFUSE, DBG_LOUD,
-		 ("word_en = %x efuse_addr=%x\n", word_en, efuse_addr));
-
-	if (!(word_en & BIT(0))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[0]);
-		efuse_one_byte_write(hw, start_addr++, data[1]);
-
-		efuse_one_byte_read(hw, tmpaddr, &tmpdata[0]);
-		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[1]);
-		if ((data[0] != tmpdata[0]) || (data[1] != tmpdata[1]))
-			badworden &= (~BIT(0));
-	}
-
-	if (!(word_en & BIT(1))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[2]);
-		efuse_one_byte_write(hw, start_addr++, data[3]);
-
-		efuse_one_byte_read(hw, tmpaddr, &tmpdata[2]);
-		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[3]);
-		if ((data[2] != tmpdata[2]) || (data[3] != tmpdata[3]))
-			badworden &= (~BIT(1));
-	}
-
-	if (!(word_en & BIT(2))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[4]);
-		efuse_one_byte_write(hw, start_addr++, data[5]);
-
-		efuse_one_byte_read(hw, tmpaddr, &tmpdata[4]);
-		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[5]);
-		if ((data[4] != tmpdata[4]) || (data[5] != tmpdata[5]))
-			badworden &= (~BIT(2));
-	}
-
-	if (!(word_en & BIT(3))) {
-		tmpaddr = start_addr;
-		efuse_one_byte_write(hw, start_addr++, data[6]);
-		efuse_one_byte_write(hw, start_addr++, data[7]);
-
-		efuse_one_byte_read(hw, tmpaddr, &tmpdata[6]);
-		efuse_one_byte_read(hw, tmpaddr + 1, &tmpdata[7]);
-		if ((data[6] != tmpdata[6]) || (data[7] != tmpdata[7]))
-			badworden &= (~BIT(3));
-	}
-
-	return badworden;
-}
-
-static void efuse_power_switch(struct ieee80211_hw *hw, u8 bwrite, u8 pwrstate)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 tempval;
-	u16 tmpV16;
-
-	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-	{
-		if (pwrstate == true)
-		{
-			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS], 0x69);
-
-			/* 1.2V Power: From VDDON with Power Cut(0x0000h[15]), default valid */
-			tmpV16 = rtl_read_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
-
-			printk("SYS_ISO_CTRL=%04x.\n",tmpV16);
-			if( ! (tmpV16 & PWC_EV12V ) ){
-				tmpV16 |= PWC_EV12V ;
-				 /* PlatformEFIOWrite2Byte(pAdapter,REG_SYS_ISO_CTRL,tmpV16); */
-			}
-			/* Reset: 0x0000h[28], default valid */
- 			tmpV16 = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[SYS_FUNC_EN]);
-			printk("SYS_FUNC_EN=%04x.\n",tmpV16);
-			if( !(tmpV16 & FEN_ELDR) ){
-				tmpV16 |= FEN_ELDR ;
-				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_FUNC_EN], tmpV16);
-			}
-
-			/* Clock: Gated(0x0008h[5]) 8M(0x0008h[1]) clock from ANA, default valid */
-			tmpV16 = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[SYS_CLK] );
-			printk("SYS_CLK=%04x.\n",tmpV16);
-			if( (!(tmpV16 & LOADER_CLK_EN) )  ||(!(tmpV16 & ANA8M) ) )
-			{
-				tmpV16 |= (LOADER_CLK_EN |ANA8M ) ;
-				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[SYS_CLK], tmpV16);
-			}
-
-			if(bwrite == true)
-			{
-				/* Enable LDO 2.5V before read/write action */
-				tempval = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3);
-				printk("EFUSE_TEST=%04x.\n",tmpV16);
-				tempval &= ~(BIT(3) | BIT(4) |BIT(5) | BIT(6));
-				tempval |= (VOLTAGE_V25 << 3);
-				tempval |= BIT(7);
-				rtl_write_byte(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3, tempval);
-			}
-		}
-		else
-		{
-			rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS], 0x00);
-			if(bwrite == true){
-				/* Disable LDO 2.5V after read/write action */
-				tempval = rtl_read_word(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3);
-				rtl_write_byte(rtlpriv,  rtlpriv->cfg->maps[EFUSE_TEST] + 3, (tempval & 0x7F));
-			}
-		}
-	}
-	else
-	{
-		if (pwrstate == true && (rtlhal->hw_type !=
-			HARDWARE_TYPE_RTL8192SE)) {
-
-			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
-				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS],
-						0x69);
-
-			tmpV16 = rtl_read_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_ISO_CTRL]);
-			if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_PWC_EV12V])) {
-				tmpV16 |= rtlpriv->cfg->maps[EFUSE_PWC_EV12V];
-				rtl_write_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_ISO_CTRL],
-					       tmpV16);
-			}
-
-			tmpV16 = rtl_read_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_FUNC_EN]);
-			if (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_FEN_ELDR])) {
-				tmpV16 |= rtlpriv->cfg->maps[EFUSE_FEN_ELDR];
-				rtl_write_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_FUNC_EN], tmpV16);
-			}
-
-			tmpV16 = rtl_read_word(rtlpriv, rtlpriv->cfg->maps[SYS_CLK]);
-			if ((!(tmpV16 & rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN])) ||
-			    (!(tmpV16 & rtlpriv->cfg->maps[EFUSE_ANA8M]))) {
-				tmpV16 |= (rtlpriv->cfg->maps[EFUSE_LOADER_CLK_EN] |
-					   rtlpriv->cfg->maps[EFUSE_ANA8M]);
-				rtl_write_word(rtlpriv,
-					       rtlpriv->cfg->maps[SYS_CLK], tmpV16);
-			}
-		}
-
-		if (pwrstate == true) {
-			if (bwrite == true) {
-				tempval = rtl_read_byte(rtlpriv,
-							rtlpriv->cfg->maps[EFUSE_TEST] +
-							3);
-
-				if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE) {
-					tempval &= 0x0F;
-					tempval |= (VOLTAGE_V25 << 4);
-				}
-
-				rtl_write_byte(rtlpriv,
-					       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
-					       (tempval | 0x80));
-			}
-
-			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
-				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
-							0x03);
-			}
-
-		} else {
-			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE)
-				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_ACCESS], 0);
-
-			if (bwrite == true) {
-				tempval = rtl_read_byte(rtlpriv,
-							rtlpriv->cfg->maps[EFUSE_TEST] +
-							3);
-				rtl_write_byte(rtlpriv,
-					       rtlpriv->cfg->maps[EFUSE_TEST] + 3,
-					       (tempval & 0x7F));
-			}
-
-			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
-				rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[EFUSE_CLK],
-							0x02);
-			}
-
-		}
-	}
-
-}
-
-static u16 efuse_get_current_size(struct ieee80211_hw *hw)
-{
-	int bcontinual = true;
-	u16 efuse_addr = 0;
-	u8 hoffset, hworden;
-	u8 efuse_data, word_cnts;
-
-	while (bcontinual && efuse_one_byte_read(hw, efuse_addr, &efuse_data)
-	       && (efuse_addr < EFUSE_MAX_SIZE)) {
-		if (efuse_data != 0xFF) {
-			hoffset = (efuse_data >> 4) & 0x0F;
-			hworden = efuse_data & 0x0F;
-			word_cnts = efuse_calculate_word_cnts(hworden);
-			efuse_addr = efuse_addr + (word_cnts * 2) + 1;
-		} else {
-			bcontinual = false;
-		}
-	}
-
-	return efuse_addr;
-}
-
-static u8 efuse_calculate_word_cnts(u8 word_en)
-{
-	u8 word_cnts = 0;
-	if (!(word_en & BIT(0)))
-		word_cnts++;
-	if (!(word_en & BIT(1)))
-		word_cnts++;
-	if (!(word_en & BIT(2)))
-		word_cnts++;
-	if (!(word_en & BIT(3)))
-		word_cnts++;
-	return word_cnts;
-}
-
diff --git a/drivers/staging/rtl8821ae/efuse.h b/drivers/staging/rtl8821ae/efuse.h
deleted file mode 100644
index a9fcbe05cf9a..000000000000
--- a/drivers/staging/rtl8821ae/efuse.h
+++ /dev/null
@@ -1,130 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_EFUSE_H_
-#define __RTL_EFUSE_H_
-
-#define EFUSE_IC_ID_OFFSET		506
-
-/*
-#define EFUSE_REAL_CONTENT_LEN	512
-#define EFUSE_MAP_LEN			128
-#define EFUSE_MAX_SECTION		16
-#define EFUSE_MAX_WORD_UNIT		4
-#define EFUSE_IC_ID_OFFSET		506
-*/
-
-#define EFUSE_MAX_WORD_UNIT		4
-
-#define EFUSE_INIT_MAP			0
-#define EFUSE_MODIFY_MAP		1
-
-#define PG_STATE_HEADER 		0x01
-#define PG_STATE_WORD_0			0x02
-#define PG_STATE_WORD_1			0x04
-#define PG_STATE_WORD_2			0x08
-#define PG_STATE_WORD_3			0x10
-#define PG_STATE_DATA			0x20
-
-#define PG_SWBYTE_H			0x01
-#define PG_SWBYTE_L			0x02
-
-#define _POWERON_DELAY_
-#define _PRE_EXECUTE_READ_CMD_
-
-#define EFUSE_REPEAT_THRESHOLD_		3
-#define EFUSE_ERROE_HANDLE		1
-
-struct efuse_map {
-	u8 offset;
-	u8 word_start;
-	u8 byte_start;
-	u8 byte_cnts;
-};
-
-struct pgpkt_struct {
-	u8 offset;
-	u8 word_en;
-	u8 data[8];
-};
-
-enum efuse_data_item {
-	EFUSE_CHIP_ID = 0,
-	EFUSE_LDO_SETTING,
-	EFUSE_CLK_SETTING,
-	EFUSE_SDIO_SETTING,
-	EFUSE_CCCR,
-	EFUSE_SDIO_MODE,
-	EFUSE_OCR,
-	EFUSE_F0CIS,
-	EFUSE_F1CIS,
-	EFUSE_MAC_ADDR,
-	EFUSE_EEPROM_VER,
-	EFUSE_CHAN_PLAN,
-	EFUSE_TXPW_TAB
-};
-
-enum {
-	VOLTAGE_V25 = 0x03,
-	LDOE25_SHIFT = 28,
-};
-
-struct efuse_priv {
-	u8 id[2];
-	u8 ldo_setting[2];
-	u8 clk_setting[2];
-	u8 cccr;
-	u8 sdio_mode;
-	u8 ocr[3];
-	u8 cis0[17];
-	u8 cis1[48];
-	u8 mac_addr[6];
-	u8 eeprom_verno;
-	u8 channel_plan;
-	u8 tx_power_b[14];
-	u8 tx_power_g[14];
-};
-
-extern void read_efuse_byte(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
-extern void efuse_initialize(struct ieee80211_hw *hw);
-extern u8 efuse_read_1byte(struct ieee80211_hw *hw, u16 address);
-extern int efuse_one_byte_read(struct ieee80211_hw *hw, u16 addr, u8 *data);
-extern void efuse_write_1byte(struct ieee80211_hw *hw, u16 address, u8 value);
-extern void read_efuse(struct ieee80211_hw *hw, u16 _offset,
-		       u16 _size_byte, u8 * pbuf);
-extern void efuse_shadow_read(struct ieee80211_hw *hw, u8 type,
-			      u16 offset, u32 * value);
-extern void efuse_shadow_write(struct ieee80211_hw *hw, u8 type,
-			       u16 offset, u32 value);
-extern bool efuse_shadow_update(struct ieee80211_hw *hw);
-extern bool efuse_shadow_update_chk(struct ieee80211_hw *hw);
-extern void rtl_efuse_shadow_map_update(struct ieee80211_hw *hw);
-extern void efuse_force_write_vendor_Id(struct ieee80211_hw *hw);
-extern void efuse_re_pg_section(struct ieee80211_hw *hw, u8 section_idx);
-#endif
diff --git a/drivers/staging/rtl8821ae/pci.c b/drivers/staging/rtl8821ae/pci.c
deleted file mode 100644
index 26d7b2fc852a..000000000000
--- a/drivers/staging/rtl8821ae/pci.c
+++ /dev/null
@@ -1,2417 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "core.h"
-#include "wifi.h"
-#include "pci.h"
-#include "base.h"
-#include "ps.h"
-#include "efuse.h"
-#include <linux/export.h>
-
-static const u16 pcibridge_vendors[PCI_BRIDGE_VENDOR_MAX] = {
-	INTEL_VENDOR_ID,
-	ATI_VENDOR_ID,
-	AMD_VENDOR_ID,
-	SIS_VENDOR_ID
-};
-
-static const u8 ac_to_hwq[] = {
-	VO_QUEUE,
-	VI_QUEUE,
-	BE_QUEUE,
-	BK_QUEUE
-};
-
-u8 _rtl_mac_to_hwqueue(struct ieee80211_hw *hw,
-		struct sk_buff *skb)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u16 fc = rtl_get_fc(skb);
-	u8 queue_index = skb_get_queue_mapping(skb);
-
-	if (unlikely(ieee80211_is_beacon(fc)))
-		return BEACON_QUEUE;
-	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
-		return MGNT_QUEUE;
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
-		if (ieee80211_is_nullfunc(fc))
-			return HIGH_QUEUE;
-
-	return ac_to_hwq[queue_index];
-}
-
-/* Update PCI dependent default settings*/
-static void _rtl_pci_update_default_setting(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
-	u8 init_aspm;
-
-	ppsc->reg_rfps_level = 0;
-	ppsc->b_support_aspm = 0;
-
-	/*Update PCI ASPM setting */
-	ppsc->const_amdpci_aspm = rtlpci->const_amdpci_aspm;
-	switch (rtlpci->const_pci_aspm) {
-	case 0:
-		/*No ASPM */
-		break;
-
-	case 1:
-		/*ASPM dynamically enabled/disable. */
-		ppsc->reg_rfps_level |= RT_RF_LPS_LEVEL_ASPM;
-		break;
-
-	case 2:
-		/*ASPM with Clock Req dynamically enabled/disable. */
-		ppsc->reg_rfps_level |= (RT_RF_LPS_LEVEL_ASPM |
-					 RT_RF_OFF_LEVL_CLK_REQ);
-		break;
-
-	case 3:
-		/*
-		 * Always enable ASPM and Clock Req
-		 * from initialization to halt.
-		 * */
-		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM);
-		ppsc->reg_rfps_level |= (RT_RF_PS_LEVEL_ALWAYS_ASPM |
-					 RT_RF_OFF_LEVL_CLK_REQ);
-		break;
-
-	case 4:
-		/*
-		 * Always enable ASPM without Clock Req
-		 * from initialization to halt.
-		 * */
-		ppsc->reg_rfps_level &= ~(RT_RF_LPS_LEVEL_ASPM |
-					  RT_RF_OFF_LEVL_CLK_REQ);
-		ppsc->reg_rfps_level |= RT_RF_PS_LEVEL_ALWAYS_ASPM;
-		break;
-	}
-
-	ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
-
-	/*Update Radio OFF setting */
-	switch (rtlpci->const_hwsw_rfoff_d3) {
-	case 1:
-		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
-			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
-		break;
-
-	case 2:
-		if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM)
-			ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_ASPM;
-		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_HALT_NIC;
-		break;
-
-	case 3:
-		ppsc->reg_rfps_level |= RT_RF_OFF_LEVL_PCI_D3;
-		break;
-	}
-
-	/*Set HW definition to determine if it supports ASPM. */
-	switch (rtlpci->const_support_pciaspm) {
-	case 0:{
-			/*Not support ASPM. */
-			bool b_support_aspm = false;
-			ppsc->b_support_aspm = b_support_aspm;
-			break;
-		}
-	case 1:{
-			/*Support ASPM. */
-			bool b_support_aspm = true;
-			bool b_support_backdoor = true;
-			ppsc->b_support_aspm = b_support_aspm;
-
-			/*if(priv->oem_id == RT_CID_TOSHIBA &&
-			   !priv->ndis_adapter.amd_l1_patch)
-			   b_support_backdoor = false; */
-
-			ppsc->b_support_backdoor = b_support_backdoor;
-
-			break;
-		}
-	case 2:
-		/*ASPM value set by chipset. */
-		if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL) {
-			bool b_support_aspm = true;
-			ppsc->b_support_aspm = b_support_aspm;
-		}
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		break;
-	}
-
-	/* toshiba aspm issue, toshiba will set aspm selfly
-	 * so we should not set aspm in driver */
-	pci_read_config_byte(rtlpci->pdev, 0x80, &init_aspm);
-	if (rtlpriv->rtlhal.hw_type == HARDWARE_TYPE_RTL8192SE &&
-		init_aspm == 0x43)
-		ppsc->b_support_aspm = false;
-}
-
-static bool _rtl_pci_platform_switch_device_pci_aspm(struct ieee80211_hw *hw,
-						     u8 value)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool bresult = false;
-
-	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8192SE)
-		value |= 0x40;
-
-	pci_write_config_byte(rtlpci->pdev, 0x80, value);
-
-	return bresult;
-}
-
-/*When we set 0x01 to enable clk request. Set 0x0 to disable clk req.*/
-static bool _rtl_pci_switch_clk_req(struct ieee80211_hw *hw, u8 value)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool bresult = false;
-
-	pci_write_config_byte(rtlpci->pdev, 0x81, value);
-	bresult = true;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE)
-		udelay(100);
-
-	return bresult;
-}
-
-/*Disable RTL8192SE ASPM & Disable Pci Bridge ASPM*/
-static void rtl_pci_disable_aspm(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
-	/*Retrieve original configuration settings. */
-	u8 linkctrl_reg = pcipriv->ndis_adapter.linkctrl_reg;
-	u16 pcibridge_linkctrlreg = pcipriv->ndis_adapter.
-				pcibridge_linkctrlreg;
-	u16 aspmlevel = 0;
-
-	if (!ppsc->b_support_aspm)
-		return;
-
-	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
-		RT_TRACE(COMP_POWER, DBG_TRACE,
-			 ("PCI(Bridge) UNKNOWN.\n"));
-
-		return;
-	}
-
-	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
-		RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
-		_rtl_pci_switch_clk_req(hw, 0x0);
-	}
-
-	if (1) {
-		/*for promising device will in L0 state after an I/O. */
-		u8 tmp_u1b;
-		pci_read_config_byte(rtlpci->pdev, 0x80, &tmp_u1b);
-	}
-
-	/*Set corresponding value. */
-	aspmlevel |= BIT(0) | BIT(1);
-	linkctrl_reg &= ~aspmlevel;
-	pcibridge_linkctrlreg &= ~(BIT(0) | BIT(1));
-
-	_rtl_pci_platform_switch_device_pci_aspm(hw, linkctrl_reg);
-	udelay(50);
-
-	/*4 Disable Pci Bridge ASPM */
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-				     pcicfg_addrport + (num4bytes << 2));
-	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, pcibridge_linkctrlreg);
-
-	udelay(50);
-
-}
-
-/*
- *Enable RTL8192SE ASPM & Enable Pci Bridge ASPM for
- *power saving We should follow the sequence to enable
- *RTL8192SE first then enable Pci Bridge ASPM
- *or the system will show bluescreen.
- */
-static void rtl_pci_enable_aspm(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 pcibridge_vendor = pcipriv->ndis_adapter.pcibridge_vendor;
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-	u8 num4bytes = pcipriv->ndis_adapter.num4bytes;
-	u16 aspmlevel;
-	u8 u_pcibridge_aspmsetting;
-	u8 u_device_aspmsetting;
-
-	if (!ppsc->b_support_aspm)
-		return;
-
-	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_UNKNOWN) {
-		RT_TRACE(COMP_POWER, DBG_TRACE,
-			 ("PCI(Bridge) UNKNOWN.\n"));
-		return;
-	}
-
-	/*4 Enable Pci Bridge ASPM */
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-				     pcicfg_addrport + (num4bytes << 2));
-
-	u_pcibridge_aspmsetting =
-	    pcipriv->ndis_adapter.pcibridge_linkctrlreg |
-	    rtlpci->const_hostpci_aspm_setting;
-
-	if (pcibridge_vendor == PCI_BRIDGE_VENDOR_INTEL)
-		u_pcibridge_aspmsetting &= ~BIT(0);
-
-	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, u_pcibridge_aspmsetting);
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("PlatformEnableASPM(): Write reg[%x] = %x\n",
-		  (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10),
-		  u_pcibridge_aspmsetting));
-
-	udelay(50);
-
-	/*Get ASPM level (with/without Clock Req) */
-	aspmlevel = rtlpci->const_devicepci_aspm_setting;
-	u_device_aspmsetting = pcipriv->ndis_adapter.linkctrl_reg;
-
-	/*_rtl_pci_platform_switch_device_pci_aspm(dev,*/
-	/*(priv->ndis_adapter.linkctrl_reg | ASPMLevel)); */
-
-	u_device_aspmsetting |= aspmlevel;
-
-	_rtl_pci_platform_switch_device_pci_aspm(hw, u_device_aspmsetting);
-
-	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_CLK_REQ) {
-		_rtl_pci_switch_clk_req(hw, (ppsc->reg_rfps_level &
-					     RT_RF_OFF_LEVL_CLK_REQ) ? 1 : 0);
-		RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_CLK_REQ);
-	}
-	udelay(100);
-}
-
-static bool rtl_pci_get_amd_l1_patch(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-
-	bool status = false;
-	u8 offset_e0;
-	unsigned offset_e4;
-
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-			pcicfg_addrport + 0xE0);
-	rtl_pci_raw_write_port_uchar(PCI_CONF_DATA, 0xA0);
-
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-			pcicfg_addrport + 0xE0);
-	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &offset_e0);
-
-	if (offset_e0 == 0xA0) {
-		rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-					     pcicfg_addrport + 0xE4);
-		rtl_pci_raw_read_port_ulong(PCI_CONF_DATA, &offset_e4);
-		if (offset_e4 & BIT(23))
-			status = true;
-	}
-
-	return status;
-}
-
-bool rtl_pci_check_buddy_priv(struct ieee80211_hw *hw,
-        		      struct rtl_priv **buddy_priv)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	bool b_find_buddy_priv = false;
-	struct rtl_priv *temp_priv = NULL;
-	struct rtl_pci_priv *temp_pcipriv = NULL;
-
-	if (!list_empty(&rtlpriv->glb_var->glb_priv_list)) {
-		list_for_each_entry(temp_priv, &rtlpriv->glb_var->glb_priv_list,
-			list) {
-			if (temp_priv) {
-				temp_pcipriv =
-					(struct rtl_pci_priv *)temp_priv->priv;
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					(("pcipriv->ndis_adapter.funcnumber %x \n"),
-					pcipriv->ndis_adapter.funcnumber));
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					(("temp_pcipriv->ndis_adapter.funcnumber %x \n"),
-					temp_pcipriv->ndis_adapter.funcnumber));
-
-				if ((pcipriv->ndis_adapter.busnumber ==
-					temp_pcipriv->ndis_adapter.busnumber) &&
-				    (pcipriv->ndis_adapter.devnumber ==
-				    temp_pcipriv->ndis_adapter.devnumber) &&
-				    (pcipriv->ndis_adapter.funcnumber !=
-				    temp_pcipriv->ndis_adapter.funcnumber)) {
-					b_find_buddy_priv = true;
-					break;
-				}
-			}
-		}
-	}
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		(("b_find_buddy_priv %d \n"), b_find_buddy_priv));
-
-	if (b_find_buddy_priv)
-		*buddy_priv = temp_priv;
-
-	return b_find_buddy_priv;
-}
-
-void rtl_pci_get_linkcontrol_field(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u8 capabilityoffset = pcipriv->ndis_adapter.pcibridge_pciehdr_offset;
-	u32 pcicfg_addrport = pcipriv->ndis_adapter.pcicfg_addrport;
-	u8 linkctrl_reg;
-	u8 num4bbytes;
-
-	num4bbytes = (capabilityoffset + 0x10) / 4;
-
-	/*Read  Link Control Register */
-	rtl_pci_raw_write_port_ulong(PCI_CONF_ADDRESS,
-				     pcicfg_addrport + (num4bbytes << 2));
-	rtl_pci_raw_read_port_uchar(PCI_CONF_DATA, &linkctrl_reg);
-
-	pcipriv->ndis_adapter.pcibridge_linkctrlreg = linkctrl_reg;
-}
-
-static void rtl_pci_parse_configuration(struct pci_dev *pdev,
-					struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-
-	u8 tmp;
-	int pos;
-	u8 linkctrl_reg;
-
-	/*Link Control Register */
-	pos = pci_find_capability(pdev, PCI_CAP_ID_EXP);
-	pci_read_config_byte(pdev, pos + PCI_EXP_LNKCTL, &linkctrl_reg);
-	pcipriv->ndis_adapter.linkctrl_reg = linkctrl_reg;
-
-	RT_TRACE(COMP_INIT, DBG_TRACE,
-		 ("Link Control Register =%x\n",
-		  pcipriv->ndis_adapter.linkctrl_reg));
-
-	pci_read_config_byte(pdev, 0x98, &tmp);
-	tmp |= BIT(4);
-	pci_write_config_byte(pdev, 0x98, tmp);
-
-	tmp = 0x17;
-	pci_write_config_byte(pdev, 0x70f, tmp);
-}
-
-static void rtl_pci_init_aspm(struct ieee80211_hw *hw)
-{
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	_rtl_pci_update_default_setting(hw);
-
-	if (ppsc->reg_rfps_level & RT_RF_PS_LEVEL_ALWAYS_ASPM) {
-		/*Always enable ASPM & Clock Req. */
-		rtl_pci_enable_aspm(hw);
-		RT_SET_PS_LEVEL(ppsc, RT_RF_PS_LEVEL_ALWAYS_ASPM);
-	}
-
-}
-
-static void _rtl_pci_io_handler_init(struct device *dev,
-				     struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->io.dev = dev;
-
-	rtlpriv->io.write8_async = pci_write8_async;
-	rtlpriv->io.write16_async = pci_write16_async;
-	rtlpriv->io.write32_async = pci_write32_async;
-
-	rtlpriv->io.read8_sync = pci_read8_sync;
-	rtlpriv->io.read16_sync = pci_read16_sync;
-	rtlpriv->io.read32_sync = pci_read32_sync;
-
-}
-
-static bool _rtl_pci_update_earlymode_info(struct ieee80211_hw *hw,
-					   struct sk_buff *skb,
-					   struct rtl_tcb_desc *tcb_desc,
-					   u8 tid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 additionlen = FCS_LEN;
-	struct sk_buff *next_skb;
-
-	/* here open is 4, wep/tkip is 8, aes is 12*/
-	if (info->control.hw_key)
-		additionlen += info->control.hw_key->icv_len;
-
-	/* The most skb num is 6 */
-	tcb_desc->empkt_num = 0;
-	spin_lock_bh(&rtlpriv->locks.waitq_lock);
-	skb_queue_walk(&rtlpriv->mac80211.skb_waitq[tid], next_skb) {
-		struct ieee80211_tx_info *next_info =
-					IEEE80211_SKB_CB(next_skb);
-		if (next_info->flags & IEEE80211_TX_CTL_AMPDU) {
-			tcb_desc->empkt_len[tcb_desc->empkt_num] =
-				next_skb->len + additionlen;
-			tcb_desc->empkt_num++;
-		} else {
-			break;
-		}
-
-		if (skb_queue_is_last(&rtlpriv->mac80211.skb_waitq[tid],
-				      next_skb))
-			break;
-
-		if (tcb_desc->empkt_num >= rtlhal->max_earlymode_num)
-			break;
-	}
-	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-	return true;
-}
-
-/* just for early mode now */
-static void _rtl_pci_tx_chk_waitq(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct sk_buff *skb = NULL;
-	struct ieee80211_tx_info *info = NULL;
-	int tid; /* should be int */
-
-	if (!rtlpriv->rtlhal.b_earlymode_enable)
-		return;
-	if (rtlpriv->dm.supp_phymode_switch &&
-		(rtlpriv->easy_concurrent_ctl.bswitch_in_process ||
-		(rtlpriv->buddy_priv &&
-		 rtlpriv->buddy_priv->easy_concurrent_ctl.bswitch_in_process)))
-		return;
-	/* we just use em for BE/BK/VI/VO */
-	for (tid = 7; tid >= 0; tid--) {
-		u8 hw_queue = ac_to_hwq[rtl_tid_to_ac(hw, tid)];
-		struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
-		while (!mac->act_scanning &&
-		       rtlpriv->psc.rfpwr_state == ERFON) {
-			struct rtl_tcb_desc tcb_desc;
-			memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-
-			spin_lock_bh(&rtlpriv->locks.waitq_lock);
-			if (!skb_queue_empty(&mac->skb_waitq[tid]) &&
-			    (ring->entries - skb_queue_len(&ring->queue) >
-			     rtlhal->max_earlymode_num)) {
-				skb = skb_dequeue(&mac->skb_waitq[tid]);
-			} else {
-				spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-				break;
-			}
-			spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-
-			/* Some macaddr can't do early mode. like
-			 * multicast/broadcast/no_qos data */
-			info = IEEE80211_SKB_CB(skb);
-			if (info->flags & IEEE80211_TX_CTL_AMPDU)
-				_rtl_pci_update_earlymode_info(hw, skb,
-							       &tcb_desc, tid);
-
-			rtlpriv->intf_ops->adapter_tx(hw, NULL, skb, &tcb_desc);
-		}
-	}
-}
-
-static void _rtl_pci_tx_isr(struct ieee80211_hw *hw, int prio)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
-
-	while (skb_queue_len(&ring->queue)) {
-		struct sk_buff *skb;
-		struct ieee80211_tx_info *info;
-		u16 fc;
-		u8 tid;
-		u8 *entry;
-
-
-		if (rtlpriv->use_new_trx_flow)
-			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
-		else
-			entry = (u8 *)(&ring->desc[ring->idx]);
-
-		if (!rtlpriv->cfg->ops->is_tx_desc_closed(hw, prio, ring->idx))
-			return;
-
-		ring->idx = (ring->idx + 1) % ring->entries;
-
-		skb = __skb_dequeue(&ring->queue);
-
-		pci_unmap_single(rtlpci->pdev,
-				 le32_to_cpu(rtlpriv->cfg->ops->
-					     get_desc((u8 *) entry, true,
-						      HW_DESC_TXBUFF_ADDR)),
-				 skb->len, PCI_DMA_TODEVICE);
-
-		/* remove early mode header */
-		if(rtlpriv->rtlhal.b_earlymode_enable)
-			skb_pull(skb, EM_HDR_LEN);
-
-		RT_TRACE((COMP_INTR | COMP_SEND), DBG_TRACE,
-			 ("new ring->idx:%d, "
-			  "free: skb_queue_len:%d, free: seq:%d\n",
-			  ring->idx,
-			  skb_queue_len(&ring->queue),
-			  *(u16 *) (skb->data + 22)));
-
-		if(prio == TXCMD_QUEUE) {
-			dev_kfree_skb(skb);
-			goto tx_status_ok;
-
-		}
-
-		/* for sw LPS, just after NULL skb send out, we can
-		 * sure AP known we are slept, our we should not let
-		 * rf to sleep*/
-		fc = rtl_get_fc(skb);
-		if (ieee80211_is_nullfunc(fc)) {
-			if(ieee80211_has_pm(fc)) {
-				rtlpriv->mac80211.offchan_deley = true;
-				rtlpriv->psc.state_inap = 1;
-			} else {
-				rtlpriv->psc.state_inap = 0;
-			}
-		}
-		if (ieee80211_is_action(fc)) {
-			struct ieee80211_mgmt_compat *action_frame =
-				(struct ieee80211_mgmt_compat *)skb->data;
-			if (action_frame->u.action.u.ht_smps.action ==
-				WLAN_HT_ACTION_SMPS) {
-				dev_kfree_skb(skb);
-				goto tx_status_ok;
-			}
-		}
-
-		/* update tid tx pkt num */
-		tid = rtl_get_tid(skb);
-		if (tid <= 7)
-			rtlpriv->link_info.tidtx_inperiod[tid]++;
-
-		info = IEEE80211_SKB_CB(skb);
-		ieee80211_tx_info_clear_status(info);
-
-		info->flags |= IEEE80211_TX_STAT_ACK;
-		/*info->status.rates[0].count = 1; */
-
-		ieee80211_tx_status_irqsafe(hw, skb);
-
-		if ((ring->entries - skb_queue_len(&ring->queue))
-				== 2) {
-
-			RT_TRACE(COMP_ERR, DBG_LOUD,
-					("more desc left, wake"
-					 "skb_queue@%d,ring->idx = %d,"
-					 "skb_queue_len = 0x%d\n",
-					 prio, ring->idx,
-					 skb_queue_len(&ring->queue)));
-
-			ieee80211_wake_queue(hw,
-					skb_get_queue_mapping
-					(skb));
-		}
-tx_status_ok:
-		skb = NULL;
-	}
-
-	if (((rtlpriv->link_info.num_rx_inperiod +
-		rtlpriv->link_info.num_tx_inperiod) > 8) ||
-		(rtlpriv->link_info.num_rx_inperiod > 2)) {
-		rtl_lps_leave(hw);
-	}
-}
-
-static int _rtl_pci_init_one_rxdesc(struct ieee80211_hw *hw,
-	u8 *entry, int rxring_idx, int desc_idx)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u32 bufferaddress;
-	u8 tmp_one = 1;
-	struct sk_buff *skb;
-
-	skb = dev_alloc_skb(rtlpci->rxbuffersize);
-	if (!skb)
-		return 0;
-	rtlpci->rx_ring[rxring_idx].rx_buf[desc_idx] = skb;
-
-	/* just set skb->cb to mapping addr
-	 * for pci_unmap_single use */
-	*((dma_addr_t *) skb->cb) = pci_map_single(rtlpci->pdev,
-				skb_tail_pointer(skb), rtlpci->rxbuffersize,
-				PCI_DMA_FROMDEVICE);
-	bufferaddress = cpu_to_le32(*((dma_addr_t *) skb->cb));
-	if (pci_dma_mapping_error(rtlpci->pdev, bufferaddress))
-		return 0;
-	if (rtlpriv->use_new_trx_flow) {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false,
-					    HW_DESC_RX_PREPARE,
-					    (u8 *) & bufferaddress);
-	} else {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false,
-					    HW_DESC_RXBUFF_ADDR,
-					    (u8 *) & bufferaddress);
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false,
-					    HW_DESC_RXPKT_LEN,
-					    (u8 *) & rtlpci->rxbuffersize);
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false,
-					    HW_DESC_RXOWN,
-					    (u8 *) & tmp_one);
-	}
-
-	return 1;
-}
-
-/* In order to receive 8K AMSDU we have set skb to
- * 9100bytes in init rx ring, but if this packet is
- * not a AMSDU, this so big packet will be sent to
- * TCP/IP directly, this cause big packet ping fail
- * like: "ping -s 65507", so here we will realloc skb
- * based on the true size of packet, I think mac80211
- * do it will be better, but now mac80211 haven't */
-
-/* but some platform will fail when alloc skb sometimes.
- * in this condition, we will send the old skb to
- * mac80211 directly, this will not cause any other
- * issues, but only be lost by TCP/IP */
-static void _rtl_pci_rx_to_mac80211(struct ieee80211_hw *hw,
-	struct sk_buff *skb, struct ieee80211_rx_status rx_status)
-{
-	if (unlikely(!rtl_action_proc(hw, skb, false))) {
-		dev_kfree_skb_any(skb);
-	} else {
-		struct sk_buff *uskb = NULL;
-		u8 *pdata;
-
-		uskb = dev_alloc_skb(skb->len + 128);
-		if (likely(uskb)) {
-			memcpy(IEEE80211_SKB_RXCB(uskb), &rx_status,
-					sizeof(rx_status));
-			pdata = (u8 *)skb_put(uskb, skb->len);
-			memcpy(pdata, skb->data, skb->len);
-			dev_kfree_skb_any(skb);
-
-			ieee80211_rx_irqsafe(hw, uskb);
-		} else {
-			ieee80211_rx_irqsafe(hw, skb);
-		}
-	}
-}
-
-/*hsisr interrupt handler*/
-static void _rtl_pci_hs_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	rtl_write_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR],
-		       rtl_read_byte(rtlpriv, rtlpriv->cfg->maps[MAC_HSISR]) |
-		       rtlpci->sys_irq_mask);
-
-
-}
-static void _rtl_pci_rx_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int rxring_idx = RTL_PCI_RX_MPDU_QUEUE;
-
-	struct ieee80211_rx_status rx_status = { 0 };
-	unsigned int count = rtlpci->rxringcount;
-	bool unicast = false;
-	u8 hw_queue = 0;
-	unsigned int rx_remained_cnt;
-	u8 own;
-	u8 tmp_one;
-
-	struct rtl_stats status = {
-		.signal = 0,
-		.noise = -98,
-		.rate = 0,
-	};
-
-	/*RX NORMAL PKT */
-	while (count--) {
-		struct ieee80211_hdr *hdr;
-		u16 fc;
-		u16 len;
-		/*rx buffer descriptor */
-		struct rtl_rx_buffer_desc *buffer_desc = NULL;
-		/*if use new trx flow, it means wifi info */
-		struct rtl_rx_desc *pdesc = NULL;
-		/*rx pkt */
-		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[
-					rtlpci->rx_ring[rxring_idx].idx];
-
-		if (rtlpriv->use_new_trx_flow) {
-			rx_remained_cnt =
-				rtlpriv->cfg->ops->rx_desc_buff_remained_cnt(hw,
-								      hw_queue);
-			if (rx_remained_cnt < 1)
-				return;
-
-		} else {	/* rx descriptor */
-			pdesc = &rtlpci->rx_ring[rxring_idx].desc[
-				rtlpci->rx_ring[rxring_idx].idx];
-
-			own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc,
-							       false,
-							       HW_DESC_OWN);
-			if (own) /* wait data to be filled by hardware */
-				return;
-		}
-
-		/* Get here means: data is filled already*/
-		/* AAAAAAttention !!!
-		 * We can NOT access 'skb' before 'pci_unmap_single' */
-		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *) skb->cb),
-			 	 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
-
-		if (rtlpriv->use_new_trx_flow) {
-			buffer_desc = &rtlpci->rx_ring[rxring_idx].buffer_desc[
-				rtlpci->rx_ring[rxring_idx].idx];
-			/*means rx wifi info*/
-			pdesc = (struct rtl_rx_desc *)skb->data;
-		}
-
-		rtlpriv->cfg->ops->query_rx_desc(hw, &status,
-						 &rx_status, (u8 *) pdesc, skb);
-
-		if (rtlpriv->use_new_trx_flow)
-			rtlpriv->cfg->ops->rx_check_dma_ok(hw,
-							   (u8 *)buffer_desc,
-							   hw_queue);
-
-
-		len = rtlpriv->cfg->ops->get_desc((u8 *)pdesc, false,
-						  HW_DESC_RXPKT_LEN);
-
-		if (skb->end - skb->tail > len) {
-			skb_put(skb, len);
-			if (rtlpriv->use_new_trx_flow)
-				skb_reserve(skb, status.rx_drvinfo_size +
-						 status.rx_bufshift + 24);
-			else
-				skb_reserve(skb, status.rx_drvinfo_size +
-						 status.rx_bufshift);
-
-		} else {
-			printk("skb->end - skb->tail = %d, len is %d\n",
-			       skb->end - skb->tail, len);
-			break;
-		}
-
-		rtlpriv->cfg->ops->rx_command_packet_handler(hw, &status, skb);
-
-		/*
-		 *NOTICE This can not be use for mac80211,
-		 *this is done in mac80211 code,
-		 *if you done here sec DHCP will fail
-		 *skb_trim(skb, skb->len - 4);
-		 */
-
-		hdr = rtl_get_hdr(skb);
-		fc = rtl_get_fc(skb);
-
-		if (!status.b_crc && !status.b_hwerror) {
-			memcpy(IEEE80211_SKB_RXCB(skb), &rx_status,
-			       sizeof(rx_status));
-
-			if (is_broadcast_ether_addr(hdr->addr1)) {
-				;/*TODO*/
-			} else if (is_multicast_ether_addr(hdr->addr1)) {
-			 	;/*TODO*/
-			} else {
-				unicast = true;
-				rtlpriv->stats.rxbytesunicast += skb->len;
-			}
-
-			rtl_is_special_data(hw, skb, false);
-
-			if (ieee80211_is_data(fc)) {
-				rtlpriv->cfg->ops->led_control(hw, LED_CTL_RX);
-
-				if (unicast)
-					rtlpriv->link_info.num_rx_inperiod++;
-			}
-
-			/* static bcn for roaming */
-			rtl_beacon_statistic(hw, skb);
-			rtl_p2p_info(hw, (void*)skb->data, skb->len);
-			/* for sw lps */
-			rtl_swlps_beacon(hw, (void*)skb->data, skb->len);
-			rtl_recognize_peer(hw, (void*)skb->data, skb->len);
-			if ((rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP) &&
-			    (rtlpriv->rtlhal.current_bandtype == BAND_ON_2_4G)&&
-			    (ieee80211_is_beacon(fc) ||
-			     ieee80211_is_probe_resp(fc))) {
-				dev_kfree_skb_any(skb);
-			} else {
-				_rtl_pci_rx_to_mac80211(hw, skb, rx_status);
-			}
-		} else {
-			dev_kfree_skb_any(skb);
-		}
-		if (rtlpriv->use_new_trx_flow) {
-			rtlpci->rx_ring[hw_queue].next_rx_rp += 1;
-			rtlpci->rx_ring[hw_queue].next_rx_rp %=
-							RTL_PCI_MAX_RX_COUNT;
-
-
-			rx_remained_cnt--;
-			if (1/*rx_remained_cnt == 0*/) {
-				rtl_write_word(rtlpriv, 0x3B4,
-					rtlpci->rx_ring[hw_queue].next_rx_rp);
-			}
-		}
-		if (((rtlpriv->link_info.num_rx_inperiod +
-		      rtlpriv->link_info.num_tx_inperiod) > 8) ||
-		    (rtlpriv->link_info.num_rx_inperiod > 2)) {
-			rtl_lps_leave(hw);
-		}
-
-		if (rtlpriv->use_new_trx_flow) {
-			_rtl_pci_init_one_rxdesc(hw, (u8 *)buffer_desc,
-						 rxring_idx,
-					       rtlpci->rx_ring[rxring_idx].idx);
-		} else {
-			_rtl_pci_init_one_rxdesc(hw, (u8 *)pdesc, rxring_idx,
-					       rtlpci->rx_ring[rxring_idx].idx);
-
-			if (rtlpci->rx_ring[rxring_idx].idx ==
-			    rtlpci->rxringcount - 1)
-				rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc,
-							    false,
-							    HW_DESC_RXERO,
-							    (u8 *) & tmp_one);
-		}
-		rtlpci->rx_ring[rxring_idx].idx =
-				(rtlpci->rx_ring[rxring_idx].idx + 1) %
-				rtlpci->rxringcount;
-	}
-}
-
-static irqreturn_t _rtl_pci_interrupt(int irq, void *dev_id)
-{
-	struct ieee80211_hw *hw = dev_id;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	unsigned long flags;
-	u32 inta = 0;
-	u32 intb = 0;
-
-
-
-	if (rtlpci->irq_enabled == 0)
-		return IRQ_HANDLED;
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock,flags);
-
-
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR], 0x0);
-
-
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE], 0x0);
-
-
-	/*read ISR: 4/8bytes */
-	rtlpriv->cfg->ops->interrupt_recognized(hw, &inta, &intb);
-
-
-	/*Shared IRQ or HW disappeared */
-	if (!inta || inta == 0xffff)
-		goto done;
-	/*<1> beacon related */
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_TBDOK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon ok interrupt!\n"));
-	}
-
-	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_TBDER])) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon err interrupt!\n"));
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BDOK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("beacon interrupt!\n"));
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BcnInt]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE,
-			 ("prepare beacon for interrupt!\n"));
-		tasklet_schedule(&rtlpriv->works.irq_prepare_bcn_tasklet);
-	}
-
-
-	/*<2> tx related */
-	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_TXFOVW]))
-		RT_TRACE(COMP_ERR, DBG_TRACE, ("IMR_TXFOVW!\n"));
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_MGNTDOK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("Manage ok interrupt!\n"));
-		_rtl_pci_tx_isr(hw, MGNT_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_HIGHDOK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("HIGH_QUEUE ok interrupt!\n"));
-		_rtl_pci_tx_isr(hw, HIGH_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BKDOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("BK Tx OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, BK_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_BEDOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("BE TX OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, BE_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_VIDOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("VI TX OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, VI_QUEUE);
-	}
-
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_VODOK]) {
-		rtlpriv->link_info.num_tx_inperiod++;
-
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("Vo TX OK interrupt!\n"));
-		_rtl_pci_tx_isr(hw, VO_QUEUE);
-	}
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192SE) {
-		if (inta & rtlpriv->cfg->maps[RTL_IMR_COMDOK]) {
-			rtlpriv->link_info.num_tx_inperiod++;
-
-			RT_TRACE(COMP_INTR, DBG_TRACE,
-				 ("CMD TX OK interrupt!\n"));
-			_rtl_pci_tx_isr(hw, TXCMD_QUEUE);
-		}
-	}
-
-	/*<3> rx related */
-	if (inta & rtlpriv->cfg->maps[RTL_IMR_ROK]) {
-		RT_TRACE(COMP_INTR, DBG_TRACE, ("Rx ok interrupt!\n"));
-
-		_rtl_pci_rx_interrupt(hw);
-
-	}
-
-	if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_RDU])) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("rx descriptor unavailable!\n"));
-		rtl_write_byte(rtlpriv, 0xb4, BIT(1) );
-		_rtl_pci_rx_interrupt(hw);
-	}
-
-	if (unlikely(intb & rtlpriv->cfg->maps[RTL_IMR_RXFOVW])) {
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("rx overflow !\n"));
-		_rtl_pci_rx_interrupt(hw);
-	}
-
-	/*<4> fw related*/
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8723AE) {
-		if (inta & rtlpriv->cfg->maps[RTL_IMR_C2HCMD]) {
-			RT_TRACE(COMP_INTR, DBG_TRACE,
-				 ("firmware interrupt!\n"));
-			queue_delayed_work(rtlpriv->works.rtl_wq,
-					   &rtlpriv->works.fwevt_wq, 0);
-		}
-	}
-
-	/*<5> hsisr related*/
-	/* Only 8188EE & 8723BE Supported.
-	 * If Other ICs Come in, System will corrupt,
-	 * because maps[RTL_IMR_HSISR_IND] & maps[MAC_HSISR]
-	 * are not initialized*/
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8188EE ||
-	    rtlhal->hw_type == HARDWARE_TYPE_RTL8723BE) {
-		if (unlikely(inta & rtlpriv->cfg->maps[RTL_IMR_HSISR_IND])) {
-			RT_TRACE(COMP_INTR, DBG_TRACE,
-					 ("hsisr interrupt!\n"));
-			_rtl_pci_hs_interrupt(hw);
-		}
-	}
-
-
-	if(rtlpriv->rtlhal.b_earlymode_enable)
-		tasklet_schedule(&rtlpriv->works.irq_tasklet);
-
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMR],
-			rtlpci->irq_mask[0]);
-	rtl_write_dword(rtlpriv, rtlpriv->cfg->maps[MAC_HIMRE],
-			rtlpci->irq_mask[1]);
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	return IRQ_HANDLED;
-
-done:
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-	return IRQ_HANDLED;
-}
-
-static void _rtl_pci_irq_tasklet(struct ieee80211_hw *hw)
-{
-	_rtl_pci_tx_chk_waitq(hw);
-}
-
-static void _rtl_pci_prepare_bcn_tasklet(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl8192_tx_ring *ring = NULL;
-	struct ieee80211_hdr *hdr = NULL;
-	struct ieee80211_tx_info *info = NULL;
-	struct sk_buff *pskb = NULL;
-	struct rtl_tx_desc *pdesc = NULL;
-	struct rtl_tcb_desc tcb_desc;
-	/*This is for new trx flow*/
-	struct rtl_tx_buffer_desc *pbuffer_desc = NULL;
-	u8 temp_one = 1;
-
-	memset(&tcb_desc, 0, sizeof(struct rtl_tcb_desc));
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	/*NB: the beacon data buffer must be 32-bit aligned. */
-	pskb = ieee80211_beacon_get(hw, mac->vif);
-	if (pskb == NULL)
-		return;
-	hdr = rtl_get_hdr(pskb);
-	info = IEEE80211_SKB_CB(pskb);
-	pdesc = &ring->desc[0];
-	if (rtlpriv->use_new_trx_flow)
-		pbuffer_desc = &ring->buffer_desc[0];
-
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
-					(u8 *)pbuffer_desc, info, NULL, pskb,
-					BEACON_QUEUE, &tcb_desc);
-
-	__skb_queue_tail(&ring->queue, pskb);
-
-	rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc, true, HW_DESC_OWN,
-				    (u8 *) & temp_one);
-
-	return;
-}
-
-static void _rtl_pci_init_trx_var(struct ieee80211_hw *hw)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 i;
-	u16 desc_num;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
-		desc_num = TX_DESC_NUM_92E;
-	else
-		desc_num = RT_TXDESC_NUM;
-
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
-		rtlpci->txringcount[i] = desc_num;
-	}
-	/*
-	 *we just alloc 2 desc for beacon queue,
-	 *because we just need first desc in hw beacon.
-	 */
-	rtlpci->txringcount[BEACON_QUEUE] = 2;
-
-	/*
-	 *BE queue need more descriptor for performance
-	 *consideration or, No more tx desc will happen,
-	 *and may cause mac80211 mem leakage.
-	 */
-	if (rtl_priv(hw)->use_new_trx_flow == false)
-		rtlpci->txringcount[BE_QUEUE] = RT_TXDESC_NUM_BE_QUEUE;
-
-	rtlpci->rxbuffersize = 9100;	/*2048/1024; */
-	rtlpci->rxringcount = RTL_PCI_MAX_RX_COUNT;	/*64; */
-}
-
-static void _rtl_pci_init_struct(struct ieee80211_hw *hw,
-		struct pci_dev *pdev)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	rtlpriv->rtlhal.up_first_time = true;
-	rtlpriv->rtlhal.being_init_adapter = false;
-
-	rtlhal->hw = hw;
-	rtlpci->pdev = pdev;
-
-	/*Tx/Rx related var */
-	_rtl_pci_init_trx_var(hw);
-
-	/*IBSS*/ mac->beacon_interval = 100;
-
-	/*AMPDU*/
-	mac->min_space_cfg = 0;
-	mac->max_mss_density = 0;
-	/*set sane AMPDU defaults */
-	mac->current_ampdu_density = 7;
-	mac->current_ampdu_factor = 3;
-
-	/*QOS*/
-	rtlpci->acm_method = eAcmWay2_SW;
-
-	/*task */
-	tasklet_init(&rtlpriv->works.irq_tasklet,
-		     (void (*)(unsigned long))_rtl_pci_irq_tasklet,
-		     (unsigned long)hw);
-	tasklet_init(&rtlpriv->works.irq_prepare_bcn_tasklet,
-		     (void (*)(unsigned long))_rtl_pci_prepare_bcn_tasklet,
-		     (unsigned long)hw);
-}
-
-static int _rtl_pci_init_tx_ring(struct ieee80211_hw *hw,
-				 unsigned int prio, unsigned int entries)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_tx_buffer_desc *buffer_desc;
-	struct rtl_tx_desc *desc;
-	dma_addr_t buffer_desc_dma, desc_dma;
-	u32 nextdescaddress;
-	int i;
-
-	/* alloc tx buffer desc for new trx flow*/
-	if (rtlpriv->use_new_trx_flow) {
-		buffer_desc =
-			pci_zalloc_consistent(rtlpci->pdev,
-					      sizeof(*buffer_desc) * entries,
-					      &buffer_desc_dma);
-
-		if (!buffer_desc || (unsigned long)buffer_desc & 0xFF) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Cannot allocate TX ring (prio = %d)\n",
-				 prio));
-			return -ENOMEM;
-		}
-
-		rtlpci->tx_ring[prio].buffer_desc = buffer_desc;
-		rtlpci->tx_ring[prio].buffer_desc_dma = buffer_desc_dma;
-
-		rtlpci->tx_ring[prio].cur_tx_rp = 0;
-		rtlpci->tx_ring[prio].cur_tx_wp = 0;
-		rtlpci->tx_ring[prio].avl_desc = entries;
-
-	}
-
-	/* alloc dma for this ring */
-	desc = pci_zalloc_consistent(rtlpci->pdev, sizeof(*desc) * entries,
-				     &desc_dma);
-
-	if (!desc || (unsigned long)desc & 0xFF) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Cannot allocate TX ring (prio = %d)\n", prio));
-		return -ENOMEM;
-	}
-
-	rtlpci->tx_ring[prio].desc = desc;
-	rtlpci->tx_ring[prio].dma = desc_dma;
-
-	rtlpci->tx_ring[prio].idx = 0;
-	rtlpci->tx_ring[prio].entries = entries;
-	skb_queue_head_init(&rtlpci->tx_ring[prio].queue);
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("queue:%d, ring_addr:%p\n", prio, desc));
-
-	/* init every desc in this ring */
-	if (rtlpriv->use_new_trx_flow == false) {
-		for (i = 0; i < entries; i++) {
-			nextdescaddress = cpu_to_le32((u32) desc_dma +
-						      ((i +	1) % entries) *
-						      sizeof(*desc));
-
-			rtlpriv->cfg->ops->set_desc(hw, (u8 *) & (desc[i]),
-						    true,
-						    HW_DESC_TX_NEXTDESC_ADDR,
-						    (u8 *) & nextdescaddress);
-		}
-	}
-	return 0;
-}
-
-static int _rtl_pci_init_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	int i;
-
-	if (rtlpriv->use_new_trx_flow) {
-		struct rtl_rx_buffer_desc *entry = NULL;
-		/* alloc dma for this ring */
-		rtlpci->rx_ring[rxring_idx].buffer_desc =
-			pci_zalloc_consistent(rtlpci->pdev,
-					      sizeof(*rtlpci->rx_ring[rxring_idx].buffer_desc) * rtlpci->rxringcount,
-					      &rtlpci->rx_ring[rxring_idx].dma);
-		if (!rtlpci->rx_ring[rxring_idx].buffer_desc ||
-		    (unsigned long)rtlpci->rx_ring[rxring_idx].buffer_desc & 0xFF) {
-			RT_TRACE(COMP_ERR, DBG_EMERG, ("Cannot allocate RX ring\n"));
-			return -ENOMEM;
-		}
-
-		/* init every desc in this ring */
-		rtlpci->rx_ring[rxring_idx].idx = 0;
-		for (i = 0; i < rtlpci->rxringcount; i++) {
-			entry = &rtlpci->rx_ring[rxring_idx].buffer_desc[i];
-			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
-						      rxring_idx, i))
-				return -ENOMEM;
-		}
-	} else {
-		struct rtl_rx_desc *entry = NULL;
-		u8 tmp_one = 1;
-		/* alloc dma for this ring */
-		rtlpci->rx_ring[rxring_idx].desc =
-			pci_zalloc_consistent(rtlpci->pdev,
-					      sizeof(*rtlpci->rx_ring[rxring_idx].desc) * rtlpci->rxringcount,
-					      &rtlpci->rx_ring[rxring_idx].dma);
-		if (!rtlpci->rx_ring[rxring_idx].desc ||
-		    (unsigned long)rtlpci->rx_ring[rxring_idx].desc & 0xFF) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Cannot allocate RX ring\n"));
-			return -ENOMEM;
-		}
-
-		/* init every desc in this ring */
-		rtlpci->rx_ring[rxring_idx].idx = 0;
-		for (i = 0; i < rtlpci->rxringcount; i++) {
-			entry = &rtlpci->rx_ring[rxring_idx].desc[i];
-			if (!_rtl_pci_init_one_rxdesc(hw, (u8 *)entry,
-						      rxring_idx, i))
-				return -ENOMEM;
-		}
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry, false,
-					    HW_DESC_RXERO, (u8 *) & tmp_one);
-	}
-	return 0;
-}
-
-static void _rtl_pci_free_tx_ring(struct ieee80211_hw *hw,
-				  unsigned int prio)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[prio];
-
-	/* free every desc in this ring */
-	while (skb_queue_len(&ring->queue)) {
-		u8 *entry;
-		struct sk_buff *skb = __skb_dequeue(&ring->queue);
-		if (rtlpriv->use_new_trx_flow)
-			entry = (u8 *)(&ring->buffer_desc[ring->idx]);
-		else
-			entry = (u8 *)(&ring->desc[ring->idx]);
-
-		pci_unmap_single(rtlpci->pdev,
-				 le32_to_cpu(rtlpriv->cfg->ops->get_desc(
-				 (u8 *) entry, true, HW_DESC_TXBUFF_ADDR)),
-				 skb->len, PCI_DMA_TODEVICE);
-		kfree_skb(skb);
-		ring->idx = (ring->idx + 1) % ring->entries;
-	}
-
-	/* free dma of this ring */
-	pci_free_consistent(rtlpci->pdev,
-			    sizeof(*ring->desc) * ring->entries,
-			    ring->desc, ring->dma);
-	ring->desc = NULL;
-	if (rtlpriv->use_new_trx_flow) {
-		pci_free_consistent(rtlpci->pdev,
-				    sizeof(*ring->buffer_desc) * ring->entries,
-				    ring->buffer_desc, ring->buffer_desc_dma);
-		ring->buffer_desc = NULL;
-	}
-}
-
-static void _rtl_pci_free_rx_ring(struct ieee80211_hw *hw, int rxring_idx)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int i;
-
-	/* free every desc in this ring */
-	for (i = 0; i < rtlpci->rxringcount; i++) {
-		struct sk_buff *skb = rtlpci->rx_ring[rxring_idx].rx_buf[i];
-		if (!skb)
-			continue;
-
-		pci_unmap_single(rtlpci->pdev, *((dma_addr_t *) skb->cb),
-				 rtlpci->rxbuffersize, PCI_DMA_FROMDEVICE);
-		kfree_skb(skb);
-	}
-
-	/* free dma of this ring */
-	if (rtlpriv->use_new_trx_flow) {
-		pci_free_consistent(rtlpci->pdev,
-				    sizeof(*rtlpci->rx_ring[rxring_idx].
-				           buffer_desc) * rtlpci->rxringcount,
-				    rtlpci->rx_ring[rxring_idx].buffer_desc,
-				    rtlpci->rx_ring[rxring_idx].dma);
-		rtlpci->rx_ring[rxring_idx].buffer_desc = NULL;
-	} else {
-		pci_free_consistent(rtlpci->pdev,
-				    sizeof(*rtlpci->rx_ring[rxring_idx].desc) *
-				    rtlpci->rxringcount,
-				    rtlpci->rx_ring[rxring_idx].desc,
-				    rtlpci->rx_ring[rxring_idx].dma);
-		rtlpci->rx_ring[rxring_idx].desc = NULL;
-	}
-}
-
-static int _rtl_pci_init_trx_ring(struct ieee80211_hw *hw)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int ret;
-	int i, rxring_idx;
-
-	/* rxring_idx 0:RX_MPDU_QUEUE
-	 * rxring_idx 1:RX_CMD_QUEUE */
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
-		ret = _rtl_pci_init_rx_ring(hw, rxring_idx);
-		if (ret)
-			return ret;
-	}
-
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
-		ret = _rtl_pci_init_tx_ring(hw, i, rtlpci->txringcount[i]);
-		if (ret)
-			goto err_free_rings;
-	}
-
-	return 0;
-
-err_free_rings:
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
-		_rtl_pci_free_rx_ring(hw, rxring_idx);
-
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
-		if (rtlpci->tx_ring[i].desc ||
-		    rtlpci->tx_ring[i].buffer_desc)
-			_rtl_pci_free_tx_ring(hw, i);
-
-	return 1;
-}
-
-static int _rtl_pci_deinit_trx_ring(struct ieee80211_hw *hw)
-{
-	u32 i, rxring_idx;
-
-	/*free rx rings */
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++)
-		_rtl_pci_free_rx_ring(hw, rxring_idx);
-
-	/*free tx rings */
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++)
-		_rtl_pci_free_tx_ring(hw, i);
-
-	return 0;
-}
-
-int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	int i, rxring_idx;
-	unsigned long flags;
-	u8 tmp_one = 1;
-	/* rxring_idx 0:RX_MPDU_QUEUE */
-	/* rxring_idx 1:RX_CMD_QUEUE */
-	for (rxring_idx = 0; rxring_idx < RTL_PCI_MAX_RX_QUEUE; rxring_idx++) {
-		/* force the rx_ring[RX_MPDU_QUEUE/
-		 * RX_CMD_QUEUE].idx to the first one */
-		/*new trx flow, do nothing*/
-		if ((rtlpriv->use_new_trx_flow == false) &&
-		     rtlpci->rx_ring[rxring_idx].desc) {
-			struct rtl_rx_desc *entry = NULL;
-
-			for (i = 0; i < rtlpci->rxringcount; i++) {
-				entry = &rtlpci->rx_ring[rxring_idx].desc[i];
-				rtlpriv->cfg->ops->set_desc(hw, (u8 *) entry,
-							    false,
-							    HW_DESC_RXOWN,
-							    (u8 *) & tmp_one);
-			}
-		}
-		rtlpci->rx_ring[rxring_idx].idx = 0;	}
-
-	/* after reset, release previous pending packet,
-	 * and force the  tx idx to the first one */
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-	for (i = 0; i < RTL_PCI_MAX_TX_QUEUE_COUNT; i++) {
-		if (rtlpci->tx_ring[i].desc ||
-			rtlpci->tx_ring[i].buffer_desc) {
-			struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[i];
-
-			while (skb_queue_len(&ring->queue)) {
-				u8 *entry;
-				struct sk_buff *skb =
-					__skb_dequeue(&ring->queue);
-				if (rtlpriv->use_new_trx_flow)
-					entry = (u8 *)(&ring->buffer_desc
-								[ring->idx]);
-				else
-					entry = (u8 *)(&ring->desc[ring->idx]);
-
-				pci_unmap_single(rtlpci->pdev,
-					le32_to_cpu(rtlpriv->cfg->ops->get_desc(
-							(u8 *)entry, true,
-							HW_DESC_TXBUFF_ADDR)),
-					skb->len, PCI_DMA_TODEVICE);
-				kfree_skb(skb);
-				ring->idx = (ring->idx + 1) % ring->entries;
-			}
-			ring->idx = 0;
-		}
-	}
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	return 0;
-}
-
-static bool rtl_pci_tx_chk_waitq_insert(struct ieee80211_hw *hw,
-					struct ieee80211_sta *sta,
-					struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *sta_entry = NULL;
-	u8 tid = rtl_get_tid(skb);
-	u16 fc = rtl_get_fc(skb);
-
-	if(!sta)
-		return false;
-	sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-
-	if (!rtlpriv->rtlhal.b_earlymode_enable)
-		return false;
-	if (ieee80211_is_nullfunc(fc))
-		return false;
-	if (ieee80211_is_qos_nullfunc(fc))
-		return false;
-	if (ieee80211_is_pspoll(fc)) {
-		return false;
-	}
-
-	if (sta_entry->tids[tid].agg.agg_state != RTL_AGG_OPERATIONAL)
-		return false;
-	if (_rtl_mac_to_hwqueue(hw, skb) > VO_QUEUE)
-		return false;
-	if (tid > 7)
-		return false;
-	/* maybe every tid should be checked */
-	if (!rtlpriv->link_info.higher_busytxtraffic[tid])
-		return false;
-
-	spin_lock_bh(&rtlpriv->locks.waitq_lock);
-	skb_queue_tail(&rtlpriv->mac80211.skb_waitq[tid], skb);
-	spin_unlock_bh(&rtlpriv->locks.waitq_lock);
-
-	return true;
-}
-
-static int rtl_pci_tx(struct ieee80211_hw *hw,
-		      struct ieee80211_sta *sta,
-		      struct sk_buff *skb,
-		      struct rtl_tcb_desc *ptcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *sta_entry = NULL;
-	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	struct rtl_tx_buffer_desc *ptx_bd_desc = NULL;
-	u16 idx;
-	u8 own;
-	u8 temp_one = 1;
-	u8 hw_queue = _rtl_mac_to_hwqueue(hw, skb);
-	unsigned long flags;
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	u16 fc = rtl_get_fc(skb);
-	u8 *pda_addr = hdr->addr1;
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	/*ssn */
-	u8 tid = 0;
-	u16 seq_number = 0;
-
-
-	if (ieee80211_is_mgmt(fc))
-		rtl_tx_mgmt_proc(hw, skb);
-
-	if (rtlpriv->psc.sw_ps_enabled) {
-		if (ieee80211_is_data(fc) && !ieee80211_is_nullfunc(fc) &&
-		    !ieee80211_has_pm(fc))
-			hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_PM);
-	}
-
-	rtl_action_proc(hw, skb, true);
-
-	if (is_multicast_ether_addr(pda_addr))
-		rtlpriv->stats.txbytesmulticast += skb->len;
-	else if (is_broadcast_ether_addr(pda_addr))
-		rtlpriv->stats.txbytesbroadcast += skb->len;
-	else
-		rtlpriv->stats.txbytesunicast += skb->len;
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-	ring = &rtlpci->tx_ring[hw_queue];
-	if (hw_queue != BEACON_QUEUE) {
-		if (rtlpriv->use_new_trx_flow)
-			idx = ring->cur_tx_wp;
-		else
-			idx = (ring->idx + skb_queue_len(&ring->queue)) %
-			      ring->entries;
-	} else {
-		idx = 0;
-	}
-
-	pdesc = &ring->desc[idx];
-
-	if (rtlpriv->use_new_trx_flow) {
-		ptx_bd_desc = &ring->buffer_desc[idx];
-	} else {
-		own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc,
-				true, HW_DESC_OWN);
-
-		if ((own == 1) && (hw_queue != BEACON_QUEUE)) {
-			RT_TRACE(COMP_ERR, DBG_WARNING,
-				 ("No more TX desc@%d, ring->idx = %d,"
-				  "idx = %d, skb_queue_len = 0x%d\n",
-				  hw_queue, ring->idx, idx,
-				  skb_queue_len(&ring->queue)));
-
-			spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock,
-					       flags);
-			return skb->len;
-		}
-	}
-
-	if (ieee80211_is_data_qos(fc)) {
-		tid = rtl_get_tid(skb);
-		if (sta) {
-			sta_entry = (struct rtl_sta_info *)sta->drv_priv;
-			seq_number = (le16_to_cpu(hdr->seq_ctrl) &
-				      IEEE80211_SCTL_SEQ) >> 4;
-			seq_number += 1;
-
-			if (!ieee80211_has_morefrags(hdr->frame_control))
-				sta_entry->tids[tid].seq_number = seq_number;
-		}
-	}
-
-	if (ieee80211_is_data(fc))
-		rtlpriv->cfg->ops->led_control(hw, LED_CTL_TX);
-
-	rtlpriv->cfg->ops->fill_tx_desc(hw, hdr, (u8 *) pdesc,
-					(u8 *)ptx_bd_desc, info, sta, skb,
-					hw_queue, ptcb_desc);
-
-	__skb_queue_tail(&ring->queue, skb);
-	if (rtlpriv->use_new_trx_flow) {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc, true,
-					    HW_DESC_OWN, (u8 *) & hw_queue);
-	} else {
-		rtlpriv->cfg->ops->set_desc(hw, (u8 *) pdesc, true,
-					    HW_DESC_OWN, (u8 *) & temp_one);
-	}
-
-	if ((ring->entries - skb_queue_len(&ring->queue)) < 2 &&
-	    hw_queue != BEACON_QUEUE) {
-
-		RT_TRACE(COMP_ERR, DBG_LOUD,
-			 ("less desc left, stop skb_queue@%d, "
-			  "ring->idx = %d,"
-			  "idx = %d, skb_queue_len = 0x%d\n",
-			  hw_queue, ring->idx, idx,
-			  skb_queue_len(&ring->queue)));
-
-		ieee80211_stop_queue(hw, skb_get_queue_mapping(skb));
-	}
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	rtlpriv->cfg->ops->tx_polling(hw, hw_queue);
-
-	return 0;
-}
-static void rtl_pci_flush(struct ieee80211_hw *hw, u32 queues, bool drop)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u16 i = 0;
-	int queue_id;
-	struct rtl8192_tx_ring *ring;
-
-	if (mac->skip_scan)
-		return;
-
-	for (queue_id = RTL_PCI_MAX_TX_QUEUE_COUNT - 1; queue_id >= 0;) {
-		u32 queue_len;
-		if (((queues >> queue_id) & 0x1) == 0) {
-			queue_id--;
-			continue;
-		}
-		ring = &pcipriv->dev.tx_ring[queue_id];
-		queue_len = skb_queue_len(&ring->queue);
-		if (queue_len == 0 || queue_id == BEACON_QUEUE ||
-			queue_id == TXCMD_QUEUE) {
-			queue_id--;
-			continue;
-		} else {
-			msleep(5);
-			i++;
-		}
-
-		/* we just wait 1s for all queues */
-		if (rtlpriv->psc.rfpwr_state == ERFOFF ||
-			is_hal_stop(rtlhal) || i >= 200)
-			return;
-	}
-}
-
-void rtl_pci_deinit(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	_rtl_pci_deinit_trx_ring(hw);
-
-	synchronize_irq(rtlpci->pdev->irq);
-	tasklet_kill(&rtlpriv->works.irq_tasklet);
-
-	flush_workqueue(rtlpriv->works.rtl_wq);
-	destroy_workqueue(rtlpriv->works.rtl_wq);
-
-}
-
-int rtl_pci_init(struct ieee80211_hw *hw, struct pci_dev *pdev)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int err;
-
-	_rtl_pci_init_struct(hw, pdev);
-
-	err = _rtl_pci_init_trx_ring(hw);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("tx ring initialization failed"));
-		return err;
-	}
-
-	return 1;
-}
-
-int rtl_pci_start(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	int err = 0;
-	RT_TRACE(COMP_INIT, DBG_DMESG, (" rtl_pci_start \n"));
-	rtl_pci_reset_trx_ring(hw);
-
-	rtlpriv->rtlhal.driver_is_goingto_unload = false;
-	err = rtlpriv->cfg->ops->hw_init(hw);
-	if (err) {
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("Failed to config hardware err %x!\n",err));
-		return err;
-	}
-
-	rtlpriv->cfg->ops->enable_interrupt(hw);
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("enable_interrupt OK\n"));
-
-	rtl_init_rx_config(hw);
-
-	/*should after adapter start and interrupt enable. */
-	set_hal_start(rtlhal);
-
-	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-
-	rtlpriv->rtlhal.up_first_time = false;
-
-	RT_TRACE(COMP_INIT, DBG_DMESG, ("rtl_pci_start OK\n"));
-	return 0;
-}
-
-void rtl_pci_stop(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 RFInProgressTimeOut = 0;
-
-	/*
-	 *should before disable interrupt&adapter
-	 *and will do it immediately.
-	 */
-	set_hal_stop(rtlhal);
-
-	rtlpriv->cfg->ops->disable_interrupt(hw);
-
-	spin_lock(&rtlpriv->locks.rf_ps_lock);
-	while (ppsc->rfchange_inprogress) {
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-		if (RFInProgressTimeOut > 100) {
-			spin_lock(&rtlpriv->locks.rf_ps_lock);
-			break;
-		}
-		mdelay(1);
-		RFInProgressTimeOut++;
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-	}
-	ppsc->rfchange_inprogress = true;
-	spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-	rtlpriv->rtlhal.driver_is_goingto_unload = true;
-	rtlpriv->cfg->ops->hw_disable(hw);
-	rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
-
-	spin_lock(&rtlpriv->locks.rf_ps_lock);
-	ppsc->rfchange_inprogress = false;
-	spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-	rtl_pci_enable_aspm(hw);
-}
-
-static bool _rtl_pci_find_adapter(struct pci_dev *pdev,
-				  struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct pci_dev *bridge_pdev = pdev->bus->self;
-	u16 venderid;
-	u16 deviceid;
-	u8 revisionid;
-	u16 irqline;
-	u8 tmp;
-
-	venderid = pdev->vendor;
-	deviceid = pdev->device;
-	pci_read_config_byte(pdev, 0x8, &revisionid);
-	pci_read_config_word(pdev, 0x3C, &irqline);
-
-	if (deviceid == RTL_PCI_8192_DID ||
-	    deviceid == RTL_PCI_0044_DID ||
-	    deviceid == RTL_PCI_0047_DID ||
-	    deviceid == RTL_PCI_8192SE_DID ||
-	    deviceid == RTL_PCI_8174_DID ||
-	    deviceid == RTL_PCI_8173_DID ||
-	    deviceid == RTL_PCI_8172_DID ||
-	    deviceid == RTL_PCI_8171_DID) {
-		switch (revisionid) {
-		case RTL_PCI_REVISION_ID_8192PCIE:
-			RT_TRACE(COMP_INIT, DBG_DMESG,
-				 ("8192E is found but not supported now-"
-				  "vid/did=%x/%x\n", venderid, deviceid));
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192E;
-			return false;
-			break;
-		case RTL_PCI_REVISION_ID_8192SE:
-			RT_TRACE(COMP_INIT, DBG_DMESG,
-				 ("8192SE is found - "
-				  "vid/did=%x/%x\n", venderid, deviceid));
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192SE;
-			break;
-		default:
-			RT_TRACE(COMP_ERR, DBG_WARNING,
-				 ("Err: Unknown device - "
-				  "vid/did=%x/%x\n", venderid, deviceid));
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192SE;
-			break;
-
-		}
-	}else if(deviceid == RTL_PCI_8723AE_DID) {
-		rtlhal->hw_type = HARDWARE_TYPE_RTL8723AE;
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("8723AE PCI-E is found - "
-			  "vid/did=%x/%x\n", venderid, deviceid));
-	} else if (deviceid == RTL_PCI_8192CET_DID ||
-		   deviceid == RTL_PCI_8192CE_DID ||
-		   deviceid == RTL_PCI_8191CE_DID ||
-		   deviceid == RTL_PCI_8188CE_DID) {
-		rtlhal->hw_type = HARDWARE_TYPE_RTL8192CE;
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("8192C PCI-E is found - "
-			  "vid/did=%x/%x\n", venderid, deviceid));
-	} else if (deviceid == RTL_PCI_8192DE_DID ||
-		   deviceid == RTL_PCI_8192DE_DID2) {
-		rtlhal->hw_type = HARDWARE_TYPE_RTL8192DE;
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("8192D PCI-E is found - "
-			  "vid/did=%x/%x\n", venderid, deviceid));
-	}else if(deviceid == RTL_PCI_8188EE_DID){
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8188EE;
-			RT_TRACE(COMP_INIT,DBG_LOUD,
-				 ("Find adapter, Hardware type is 8188EE\n"));
-	}else if (deviceid == RTL_PCI_8723BE_DID){
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8723BE;
-			RT_TRACE(COMP_INIT,DBG_LOUD,
-				 ("Find adapter, Hardware type is 8723BE\n"));
-	}else if (deviceid == RTL_PCI_8192EE_DID){
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8192EE;
-			RT_TRACE(COMP_INIT,DBG_LOUD,
-				 ("Find adapter, Hardware type is 8192EE\n"));
-	}else if (deviceid == RTL_PCI_8821AE_DID) {
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8821AE;
-			RT_TRACE(COMP_INIT,DBG_LOUD,
-				("Find adapter, Hardware type is 8821AE\n"));
-	}else if (deviceid == RTL_PCI_8812AE_DID) {
-			rtlhal->hw_type = HARDWARE_TYPE_RTL8812AE;
-			RT_TRACE(COMP_INIT,DBG_LOUD,
-				("Find adapter, Hardware type is 8812AE\n"));
-	}else {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Err: Unknown device -"
-			  " vid/did=%x/%x\n", venderid, deviceid));
-
-		rtlhal->hw_type = RTL_DEFAULT_HARDWARE_TYPE;
-	}
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192DE) {
-		if (revisionid == 0 || revisionid == 1) {
-			if (revisionid == 0) {
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					 ("Find 92DE MAC0.\n"));
-				rtlhal->interfaceindex = 0;
-			} else if (revisionid == 1) {
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					 ("Find 92DE MAC1.\n"));
-				rtlhal->interfaceindex = 1;
-			}
-		} else {
-			RT_TRACE(COMP_INIT, DBG_LOUD, ("Unknown device - "
-				 "VendorID/DeviceID=%x/%x, Revision=%x\n",
-				 venderid, deviceid, revisionid));
-			rtlhal->interfaceindex = 0;
-		}
-	}
-
-	/* 92ee use new trx flow */
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8192EE)
-		rtlpriv->use_new_trx_flow = true;
-	else
-		rtlpriv->use_new_trx_flow = false;
-
-	/*find bus info */
-	pcipriv->ndis_adapter.busnumber = pdev->bus->number;
-	pcipriv->ndis_adapter.devnumber = PCI_SLOT(pdev->devfn);
-	pcipriv->ndis_adapter.funcnumber = PCI_FUNC(pdev->devfn);
-
-	/*find bridge info */
-	pcipriv->ndis_adapter.pcibridge_vendor = PCI_BRIDGE_VENDOR_UNKNOWN;
-	/* some ARM have no bridge_pdev and will crash here
-	 * so we should check if bridge_pdev is NULL */
-	if (bridge_pdev) {
-		pcipriv->ndis_adapter.pcibridge_vendorid = bridge_pdev->vendor;
-		for (tmp = 0; tmp < PCI_BRIDGE_VENDOR_MAX; tmp++) {
-			if (bridge_pdev->vendor == pcibridge_vendors[tmp]) {
-				pcipriv->ndis_adapter.pcibridge_vendor = tmp;
-				RT_TRACE(COMP_INIT, DBG_DMESG,
-					 ("Pci Bridge Vendor is found index: %d\n",
-					  tmp));
-				break;
-			}
-		}
-	}
-
-	if (pcipriv->ndis_adapter.pcibridge_vendor !=
-	    PCI_BRIDGE_VENDOR_UNKNOWN) {
-		pcipriv->ndis_adapter.pcibridge_busnum =
-		    bridge_pdev->bus->number;
-		pcipriv->ndis_adapter.pcibridge_devnum =
-		    PCI_SLOT(bridge_pdev->devfn);
-		pcipriv->ndis_adapter.pcibridge_funcnum =
-		    PCI_FUNC(bridge_pdev->devfn);
-		pcipriv->ndis_adapter.pcicfg_addrport =
-		    (pcipriv->ndis_adapter.pcibridge_busnum << 16) |
-		    (pcipriv->ndis_adapter.pcibridge_devnum << 11) |
-		    (pcipriv->ndis_adapter.pcibridge_funcnum << 8) | (1 << 31);
-		pcipriv->ndis_adapter.pcibridge_pciehdr_offset =
-		    pci_pcie_cap(bridge_pdev);
-		pcipriv->ndis_adapter.num4bytes =
-		    (pcipriv->ndis_adapter.pcibridge_pciehdr_offset + 0x10) / 4;
-
-		rtl_pci_get_linkcontrol_field(hw);
-
-		if (pcipriv->ndis_adapter.pcibridge_vendor ==
-		    PCI_BRIDGE_VENDOR_AMD) {
-			pcipriv->ndis_adapter.amd_l1_patch =
-			    rtl_pci_get_amd_l1_patch(hw);
-		}
-	}
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("pcidev busnumber:devnumber:funcnumber:"
-		  "vendor:link_ctl %d:%d:%d:%x:%x\n",
-		  pcipriv->ndis_adapter.busnumber,
-		  pcipriv->ndis_adapter.devnumber,
-		  pcipriv->ndis_adapter.funcnumber,
-		  pdev->vendor, pcipriv->ndis_adapter.linkctrl_reg));
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("pci_bridge busnumber:devnumber:funcnumber:vendor:"
-		  "pcie_cap:link_ctl_reg:amd %d:%d:%d:%x:%x:%x:%x\n",
-		  pcipriv->ndis_adapter.pcibridge_busnum,
-		  pcipriv->ndis_adapter.pcibridge_devnum,
-		  pcipriv->ndis_adapter.pcibridge_funcnum,
-		  pcibridge_vendors[pcipriv->ndis_adapter.pcibridge_vendor],
-		  pcipriv->ndis_adapter.pcibridge_pciehdr_offset,
-		  pcipriv->ndis_adapter.pcibridge_linkctrlreg,
-		  pcipriv->ndis_adapter.amd_l1_patch));
-
-	rtl_pci_parse_configuration(pdev, hw);
-	list_add_tail(&rtlpriv->list, &rtlpriv->glb_var->glb_priv_list);
-	return true;
-}
-
-static int rtl_pci_intr_mode_msi(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	int ret;
-	ret = pci_enable_msi(rtlpci->pdev);
-	if (ret < 0)
-		return ret;
-
-	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
-			  IRQF_SHARED, KBUILD_MODNAME, hw);
-	if (ret < 0) {
-		pci_disable_msi(rtlpci->pdev);
-		return ret;
-	}
-
-	rtlpci->using_msi = true;
-
-	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG, ("MSI Interrupt Mode!\n"));
-	return 0;
-}
-
-static int rtl_pci_intr_mode_legacy(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	int ret;
-
-	ret = request_irq(rtlpci->pdev->irq, &_rtl_pci_interrupt,
-			  IRQF_SHARED, KBUILD_MODNAME, hw);
-	if (ret < 0) {
-		return ret;
-	}
-
-	rtlpci->using_msi = false;
-	RT_TRACE(COMP_INIT|COMP_INTR, DBG_DMESG,
-		 ("Pin-based Interrupt Mode!\n"));
-	return 0;
-}
-
-static int rtl_pci_intr_mode_decide(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	int ret;
-	if (rtlpci->msi_support == true) {
-		ret = rtl_pci_intr_mode_msi(hw);
-		if (ret < 0)
-			ret = rtl_pci_intr_mode_legacy(hw);
-	} else {
-		ret = rtl_pci_intr_mode_legacy(hw);
-	}
-	return ret;
-}
-
-/* this is used for other modules get
- * hw pointer in rtl_pci_get_hw_pointer */
-struct ieee80211_hw *hw_export = NULL;
-
-int rtl_pci_probe(struct pci_dev *pdev,
-                  const struct pci_device_id *id)
-{
-	struct ieee80211_hw *hw = NULL;
-
-	struct rtl_priv *rtlpriv = NULL;
-	struct rtl_pci_priv *pcipriv = NULL;
-	struct rtl_pci *rtlpci;
-	unsigned long pmem_start, pmem_len, pmem_flags;
-	int err;
-
-
-	err = pci_enable_device(pdev);
-	if (err) {
-		RT_ASSERT(false,
-			  ("%s : Cannot enable new PCI device\n",
-			   pci_name(pdev)));
-		return err;
-	}
-
-	if (!pci_set_dma_mask(pdev, DMA_BIT_MASK(32))) {
-		if (pci_set_consistent_dma_mask(pdev, DMA_BIT_MASK(32))) {
-			RT_ASSERT(false, ("Unable to obtain 32bit DMA "
-					  "for consistent allocations\n"));
-			pci_disable_device(pdev);
-			return -ENOMEM;
-		}
-	}
-
-	pci_set_master(pdev);
-
-	hw = ieee80211_alloc_hw(sizeof(struct rtl_pci_priv) +
-				sizeof(struct rtl_priv), &rtl_ops);
-	if (!hw) {
-		RT_ASSERT(false,
-			  ("%s : ieee80211 alloc failed\n", pci_name(pdev)));
-		err = -ENOMEM;
-		goto fail1;
-	}
-	hw_export = hw;
-
-	SET_IEEE80211_DEV(hw, &pdev->dev);
-	pci_set_drvdata(pdev, hw);
-
-	rtlpriv = hw->priv;
-	pcipriv = (void *)rtlpriv->priv;
-	pcipriv->dev.pdev = pdev;
-
-	/* init cfg & intf_ops */
-	rtlpriv->rtlhal.interface = INTF_PCI;
-	rtlpriv->cfg = (struct rtl_hal_cfg *)(id->driver_data);
-	rtlpriv->intf_ops = &rtl_pci_ops;
-	rtlpriv->glb_var = &global_var;
-
-	/*
-	 *init dbgp flags before all
-	 *other functions, because we will
-	 *use it in other functions like
-	 *RT_TRACE/RT_PRINT/RTL_PRINT_DATA
-	 *you can not use these macro
-	 *before this
-	 */
-	rtl_dbgp_flag_init(hw);
-
-	/* MEM map */
-	err = pci_request_regions(pdev, KBUILD_MODNAME);
-	if (err) {
-		RT_ASSERT(false, ("Can't obtain PCI resources\n"));
-		return err;
-	}
-
-	pmem_start = pci_resource_start(pdev, rtlpriv->cfg->bar_id);
-	pmem_len = pci_resource_len(pdev, rtlpriv->cfg->bar_id);
-	pmem_flags = pci_resource_flags(pdev, rtlpriv->cfg->bar_id);
-
-	/*shared mem start */
-	rtlpriv->io.pci_mem_start =
-			pci_iomap(pdev,
-			rtlpriv->cfg->bar_id, pmem_len);
-	if (rtlpriv->io.pci_mem_start == NULL) {
-		RT_ASSERT(false, ("Can't map PCI mem\n"));
-		goto fail2;
-	}
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("mem mapped space: start: 0x%08lx len:%08lx "
-		  "flags:%08lx, after map:0x%p\n",
-		  pmem_start, pmem_len, pmem_flags,
-		  rtlpriv->io.pci_mem_start));
-
-	/* Disable Clk Request */
-	pci_write_config_byte(pdev, 0x81, 0);
-	/* leave D3 mode */
-	pci_write_config_byte(pdev, 0x44, 0);
-	pci_write_config_byte(pdev, 0x04, 0x06);
-	pci_write_config_byte(pdev, 0x04, 0x07);
-
-	/* find adapter */
-	/* if chip not support, will return false */
-	if(!_rtl_pci_find_adapter(pdev, hw))
-		goto fail3;
-
-	/* Init IO handler */
-	_rtl_pci_io_handler_init(&pdev->dev, hw);
-
-	/*like read eeprom and so on */
-	rtlpriv->cfg->ops->read_eeprom_info(hw);
-
-	if (rtlpriv->cfg->ops->init_sw_vars(hw)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Can't init_sw_vars.\n"));
-		goto fail3;
-	}
-
-	rtlpriv->cfg->ops->init_sw_leds(hw);
-
-	/*aspm */
-	rtl_pci_init_aspm(hw);
-
-	/* Init mac80211 sw */
-	err = rtl_init_core(hw);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Can't allocate sw for mac80211.\n"));
-		goto fail3;
-	}
-
-	/* Init PCI sw */
-	err = !rtl_pci_init(hw, pdev);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Failed to init PCI.\n"));
-		goto fail3;
-	}
-
-	err = ieee80211_register_hw(hw);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Can't register mac80211 hw.\n"));
-		goto fail3;
-	} else {
-		rtlpriv->mac80211.mac80211_registered = 1;
-	}
-	/* the wiphy must have been registed to
-	 * cfg80211 prior to regulatory_hint */
-	if (regulatory_hint(hw->wiphy, rtlpriv->regd.alpha2)) {
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("regulatory_hint fail\n"));
-	}
-
-	err = sysfs_create_group(&pdev->dev.kobj, &rtl_attribute_group);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("failed to create sysfs device attributes\n"));
-		goto fail3;
-	}
-	/* add for prov */
-	rtl_proc_add_one(hw);
-
-	/*init rfkill */
-	rtl_init_rfkill(hw);
-
-	rtlpci = rtl_pcidev(pcipriv);
-
-	err = rtl_pci_intr_mode_decide(hw);
-	if (err) {
-		RT_TRACE(COMP_INIT, DBG_DMESG,
-			 ("%s: failed to register IRQ handler\n",
-			  wiphy_name(hw->wiphy)));
-		goto fail3;
-	} else {
-		rtlpci->irq_alloc = 1;
-	}
-
-	set_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
-	return 0;
-
-fail3:
-	pci_set_drvdata(pdev, NULL);
-	rtl_deinit_core(hw);
-	ieee80211_free_hw(hw);
-
-	if (rtlpriv->io.pci_mem_start != NULL)
-		pci_iounmap(pdev, rtlpriv->io.pci_mem_start);
-
-fail2:
-	pci_release_regions(pdev);
-
-fail1:
-
-	pci_disable_device(pdev);
-
-	return -ENODEV;
-
-}
-/* EXPORT_SYMBOL(rtl_pci_probe); */
-
-struct ieee80211_hw *rtl_pci_get_hw_pointer(void)
-{
-	return hw_export;
-}
-/* EXPORT_SYMBOL(rtl_pci_get_hw_pointer); */
-
-void rtl_pci_disconnect(struct pci_dev *pdev)
-{
-	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(pcipriv);
-	struct rtl_mac *rtlmac = rtl_mac(rtlpriv);
-
-	clear_bit(RTL_STATUS_INTERFACE_START, &rtlpriv->status);
-
-	sysfs_remove_group(&pdev->dev.kobj, &rtl_attribute_group);
-
-	/* add for prov */
-	rtl_proc_remove_one(hw);
-
-
-	/*ieee80211_unregister_hw will call ops_stop */
-	if (rtlmac->mac80211_registered == 1) {
-		ieee80211_unregister_hw(hw);
-		rtlmac->mac80211_registered = 0;
-	} else {
-		rtl_deinit_deferred_work(hw);
-		rtlpriv->intf_ops->adapter_stop(hw);
-	}
-
-	/*deinit rfkill */
-	rtl_deinit_rfkill(hw);
-
-	rtl_pci_deinit(hw);
-	rtl_deinit_core(hw);
-	rtlpriv->cfg->ops->deinit_sw_vars(hw);
-
-	if (rtlpci->irq_alloc) {
-		synchronize_irq(rtlpci->pdev->irq);
-		free_irq(rtlpci->pdev->irq, hw);
-		rtlpci->irq_alloc = 0;
-	}
-
-	if (rtlpci->using_msi == true)
-		pci_disable_msi(rtlpci->pdev);
-
-	list_del(&rtlpriv->list);
-	if (rtlpriv->io.pci_mem_start != NULL) {
-		pci_iounmap(pdev, rtlpriv->io.pci_mem_start);
-		pci_release_regions(pdev);
-	}
-
-	pci_disable_device(pdev);
-
-	rtl_pci_disable_aspm(hw);
-
-	pci_set_drvdata(pdev, NULL);
-
-	ieee80211_free_hw(hw);
-}
-/* EXPORT_SYMBOL(rtl_pci_disconnect); */
-
-/***************************************
-kernel pci power state define:
-PCI_D0         ((pci_power_t __force) 0)
-PCI_D1         ((pci_power_t __force) 1)
-PCI_D2         ((pci_power_t __force) 2)
-PCI_D3hot      ((pci_power_t __force) 3)
-PCI_D3cold     ((pci_power_t __force) 4)
-PCI_UNKNOWN    ((pci_power_t __force) 5)
-
-This function is called when system
-goes into suspend state mac80211 will
-call rtl_mac_stop() from the mac80211
-suspend function first, So there is
-no need to call hw_disable here.
-****************************************/
-int rtl_pci_suspend(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->cfg->ops->hw_suspend(hw);
-	rtl_deinit_rfkill(hw);
-
-	return 0;
-}
-/* EXPORT_SYMBOL(rtl_pci_suspend); */
-
-int rtl_pci_resume(struct device *dev)
-{
-	struct pci_dev *pdev = to_pci_dev(dev);
-	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->cfg->ops->hw_resume(hw);
-	rtl_init_rfkill(hw);
-
-	return 0;
-}
-/* EXPORT_SYMBOL(rtl_pci_resume); */
-
-struct rtl_intf_ops rtl_pci_ops = {
-	.read_efuse_byte = read_efuse_byte,
-	.adapter_start = rtl_pci_start,
-	.adapter_stop = rtl_pci_stop,
-	.check_buddy_priv = rtl_pci_check_buddy_priv,
-	.adapter_tx = rtl_pci_tx,
-	.flush = rtl_pci_flush,
-	.reset_trx_ring = rtl_pci_reset_trx_ring,
-	.waitq_insert = rtl_pci_tx_chk_waitq_insert,
-
-	.disable_aspm = rtl_pci_disable_aspm,
-	.enable_aspm = rtl_pci_enable_aspm,
-};
diff --git a/drivers/staging/rtl8821ae/pci.h b/drivers/staging/rtl8821ae/pci.h
deleted file mode 100644
index 3f16ec90296a..000000000000
--- a/drivers/staging/rtl8821ae/pci.h
+++ /dev/null
@@ -1,348 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_PCI_H__
-#define __RTL_PCI_H__
-
-#include <linux/pci.h>
-/*
-1: MSDU packet queue,
-2: Rx Command Queue
-*/
-#define RTL_PCI_RX_MPDU_QUEUE			0
-#define RTL_PCI_RX_CMD_QUEUE			1
-#define RTL_PCI_MAX_RX_QUEUE			2
-
-#define RTL_PCI_MAX_RX_COUNT			512//64
-#define RTL_PCI_MAX_TX_QUEUE_COUNT		9
-
-#define RT_TXDESC_NUM				128
-#define TX_DESC_NUM_92E				512
-#define RT_TXDESC_NUM_BE_QUEUE			256
-
-#define BK_QUEUE				0
-#define BE_QUEUE				1
-#define VI_QUEUE				2
-#define VO_QUEUE				3
-#define BEACON_QUEUE				4
-#define TXCMD_QUEUE				5
-#define MGNT_QUEUE				6
-#define HIGH_QUEUE				7
-#define HCCA_QUEUE				8
-
-#define RTL_PCI_DEVICE(vend, dev, cfg)  \
-	.vendor = (vend), \
-	.device = (dev), \
-	.subvendor = PCI_ANY_ID, \
-	.subdevice = PCI_ANY_ID,\
-	.driver_data = (kernel_ulong_t)&(cfg)
-
-#define INTEL_VENDOR_ID				0x8086
-#define SIS_VENDOR_ID				0x1039
-#define ATI_VENDOR_ID				0x1002
-#define ATI_DEVICE_ID				0x7914
-#define AMD_VENDOR_ID				0x1022
-
-#define PCI_MAX_BRIDGE_NUMBER			255
-#define PCI_MAX_DEVICES				32
-#define PCI_MAX_FUNCTION			8
-
-#define PCI_CONF_ADDRESS   	0x0CF8	/*PCI Configuration Space Address */
-#define PCI_CONF_DATA		0x0CFC	/*PCI Configuration Space Data */
-
-#define PCI_CLASS_BRIDGE_DEV		0x06
-#define PCI_SUBCLASS_BR_PCI_TO_PCI	0x04
-#define PCI_CAPABILITY_ID_PCI_EXPRESS	0x10
-#define PCI_CAP_ID_EXP			0x10
-
-#define U1DONTCARE 			0xFF
-#define U2DONTCARE 			0xFFFF
-#define U4DONTCARE 			0xFFFFFFFF
-
-#define RTL_PCI_8192_DID	0x8192	/*8192 PCI-E */
-#define RTL_PCI_8192SE_DID	0x8192	/*8192 SE */
-#define RTL_PCI_8174_DID	0x8174	/*8192 SE */
-#define RTL_PCI_8173_DID	0x8173	/*8191 SE Crab */
-#define RTL_PCI_8172_DID	0x8172	/*8191 SE RE */
-#define RTL_PCI_8171_DID	0x8171	/*8191 SE Unicron */
-#define RTL_PCI_0045_DID	0x0045	/*8190 PCI for Ceraga */
-#define RTL_PCI_0046_DID	0x0046	/*8190 Cardbus for Ceraga */
-#define RTL_PCI_0044_DID	0x0044	/*8192e PCIE for Ceraga */
-#define RTL_PCI_0047_DID	0x0047	/*8192e Express Card for Ceraga */
-#define RTL_PCI_700F_DID	0x700F
-#define RTL_PCI_701F_DID	0x701F
-#define RTL_PCI_DLINK_DID	0x3304
-#define RTL_PCI_8723AE_DID	0x8723	/*8723e */
-#define RTL_PCI_8192CET_DID	0x8191	/*8192ce */
-#define RTL_PCI_8192CE_DID	0x8178	/*8192ce */
-#define RTL_PCI_8191CE_DID	0x8177	/*8192ce */
-#define RTL_PCI_8188CE_DID	0x8176	/*8192ce */
-#define RTL_PCI_8192CU_DID	0x8191	/*8192ce */
-#define RTL_PCI_8192DE_DID	0x8193	/*8192de */
-#define RTL_PCI_8192DE_DID2	0x002B	/*92DE*/
-#define RTL_PCI_8188EE_DID	0x8179  /*8188ee*/
-#define RTL_PCI_8723BE_DID	0xB723  /*8723be*/
-#define RTL_PCI_8192EE_DID	0x818B	/*8192ee*/
-#define RTL_PCI_8821AE_DID	0x8821	/*8821ae*/
-#define RTL_PCI_8812AE_DID	0x8812	/*8812ae*/
-
-/*8192 support 16 pages of IO registers*/
-#define RTL_MEM_MAPPED_IO_RANGE_8190PCI 	0x1000
-#define RTL_MEM_MAPPED_IO_RANGE_8192PCIE	0x4000
-#define RTL_MEM_MAPPED_IO_RANGE_8192SE		0x4000
-#define RTL_MEM_MAPPED_IO_RANGE_8192CE		0x4000
-#define RTL_MEM_MAPPED_IO_RANGE_8192DE		0x4000
-
-#define RTL_PCI_REVISION_ID_8190PCI		0x00
-#define RTL_PCI_REVISION_ID_8192PCIE		0x01
-#define RTL_PCI_REVISION_ID_8192SE		0x10
-#define RTL_PCI_REVISION_ID_8192CE		0x1
-#define RTL_PCI_REVISION_ID_8192DE		0x0
-
-#define PCI_VENDOR_ID_REALTEK		0x10ec
-
-#define RTL_DEFAULT_HARDWARE_TYPE	HARDWARE_TYPE_RTL8192CE
-
-enum pci_bridge_vendor {
-	PCI_BRIDGE_VENDOR_INTEL = 0x0,	/*0b'0000,0001 */
-	PCI_BRIDGE_VENDOR_ATI,		/*0b'0000,0010*/
-	PCI_BRIDGE_VENDOR_AMD,		/*0b'0000,0100*/
-	PCI_BRIDGE_VENDOR_SIS,		/*0b'0000,1000*/
-	PCI_BRIDGE_VENDOR_UNKNOWN,	/*0b'0100,0000*/
-	PCI_BRIDGE_VENDOR_MAX,	
-};
-
-struct rtl_pci_capabilities_header {
-    u8 capability_id;
-    u8 next;
-};
-
-/* In new TRX flow, Buffer_desc is new concept 
-  * But TX wifi info == TX descriptor in old flow
-  * RX wifi info == RX descriptor in old flow */
-struct rtl_tx_buffer_desc {
-#if (RTL8192EE_SEG_NUM == 2)
-	u32 dword[2*(DMA_IS_64BIT + 1)*8]; /* seg = 8 */
-#elif (RTL8192EE_SEG_NUM == 1)
-	u32 dword[2*(DMA_IS_64BIT + 1)*4]; /* seg = 4 */
-#elif (RTL8192EE_SEG_NUM == 0)
-	u32 dword[2*(DMA_IS_64BIT + 1)*2]; /* seg = 2 */
-#endif
-} __packed;
-
-struct rtl_tx_desc {/*old: tx desc*//*new: tx wifi info*/
-	u32 dword[16];
-} __packed;
-
-struct rtl_rx_buffer_desc { /*rx buffer desc*/
-	u32 dword[2];
-} __packed;
-
-struct rtl_rx_desc { /*old: rx desc*//*new: rx wifi info*/
-	u32 dword[8];
-} __packed;
-
-struct rtl_tx_cmd_desc {
-	u32 dword[16];
-} __packed;
-
-struct rtl8192_tx_ring {
-	struct rtl_tx_desc *desc; /*tx desc / tx wifi info*/
-	dma_addr_t dma; /*tx desc dma memory / tx wifi info dma memory*/
-	unsigned int idx;
-	unsigned int entries;
-	struct sk_buff_head queue;
-	/*add for new trx flow*/
-	struct rtl_tx_buffer_desc *buffer_desc; /*tx buffer descriptor*/
-	dma_addr_t buffer_desc_dma; /*tx bufferd desc dma memory*/
-	u16 avl_desc; /* available_desc_to_write */
-	u16 cur_tx_wp; /* current_tx_write_point */
-	u16 cur_tx_rp; /* current_tx_read_point */
-};
-
-struct rtl8192_rx_ring {
-	struct rtl_rx_desc *desc;/*for old trx flow, not used in new trx*/
-	/*dma matches either 'desc' or 'buffer_desc'*/
-	dma_addr_t dma;
-	unsigned int idx;
-	struct sk_buff *rx_buf[RTL_PCI_MAX_RX_COUNT];
-	/*add for new trx flow*/
-	struct rtl_rx_buffer_desc *buffer_desc; /*rx buffer descriptor*/
-	u16 next_rx_rp; /* next_rx_read_point */
-};
-
-struct rtl_pci {
-	struct pci_dev *pdev;
-	bool irq_enabled;
-
-	/*Tx */
-	struct rtl8192_tx_ring tx_ring[RTL_PCI_MAX_TX_QUEUE_COUNT];
-	int txringcount[RTL_PCI_MAX_TX_QUEUE_COUNT];
-	u32 transmit_config;
-
-	/*Rx */
-	struct rtl8192_rx_ring rx_ring[RTL_PCI_MAX_RX_QUEUE];
-	int rxringcount;
-	u16 rxbuffersize;
-	u32 receive_config;
-
-	/*irq */
-	u8 irq_alloc;
-	u32 irq_mask[2];
-	u32 sys_irq_mask;
-
-	/*Bcn control register setting */
-	u32 reg_bcn_ctrl_val;
-
-	 /*ASPM*/ u8 const_pci_aspm;
-	u8 const_amdpci_aspm;
-	u8 const_hwsw_rfoff_d3;
-	u8 const_support_pciaspm;
-	/*pci-e bridge */
-	u8 const_hostpci_aspm_setting;
-	/*pci-e device */
-	u8 const_devicepci_aspm_setting;
-	/*If it supports ASPM, Offset[560h] = 0x40,
-	   otherwise Offset[560h] = 0x00. */
-	bool b_support_aspm;
-	bool b_support_backdoor;
-
-	/*QOS & EDCA */
-	enum acm_method acm_method;
-
-	u16 shortretry_limit;
-	u16 longretry_limit;
-	
-	/* MSI support */
-	bool msi_support;
-	bool using_msi;
-};
-
-struct mp_adapter {
-	u8 linkctrl_reg;
-
-	u8 busnumber;
-	u8 devnumber;
-	u8 funcnumber;
-
-	u8 pcibridge_busnum;
-	u8 pcibridge_devnum;
-	u8 pcibridge_funcnum;
-
-	u8 pcibridge_vendor;
-	u16 pcibridge_vendorid;
-	u16 pcibridge_deviceid;
-
-	u32 pcicfg_addrport;
-	u8 num4bytes;
-
-	u8 pcibridge_pciehdr_offset;
-	u8 pcibridge_linkctrlreg;
-
-	bool amd_l1_patch;
-};
-
-struct rtl_pci_priv {
-	struct rtl_pci dev;
-	struct mp_adapter ndis_adapter;
-	struct rtl_led_ctl ledctl;
-	struct bt_coexist_info btcoexist;
-};
-
-#define rtl_pcipriv(hw)		(((struct rtl_pci_priv *)(rtl_priv(hw))->priv))
-#define rtl_pcidev(pcipriv)	(&((pcipriv)->dev))
-
-int rtl_pci_reset_trx_ring(struct ieee80211_hw *hw);
-
-extern struct rtl_intf_ops rtl_pci_ops;
-
-int rtl_pci_probe(struct pci_dev *pdev,
-                  const struct pci_device_id *id);
-void rtl_pci_disconnect(struct pci_dev *pdev);
-int rtl_pci_suspend(struct device *dev);
-int rtl_pci_resume(struct device *dev);
-
-static inline u8 pci_read8_sync(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return 0xff & readb((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline u16 pci_read16_sync(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return readw((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline u32 pci_read32_sync(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return readl((u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void pci_write8_async(struct rtl_priv *rtlpriv, u32 addr, u8 val)
-{
-	writeb(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void pci_write16_async(struct rtl_priv *rtlpriv,
-				     u32 addr, u16 val)
-{
-	writew(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void pci_write32_async(struct rtl_priv *rtlpriv,
-				     u32 addr, u32 val)
-{
-	writel(val, (u8 __iomem *) rtlpriv->io.pci_mem_start + addr);
-}
-
-static inline void rtl_pci_raw_write_port_ulong(u32 port, u32 val)
-{
-	outl(val, port);
-}
-
-static inline void rtl_pci_raw_write_port_uchar(u32 port, u8 val)
-{
-	outb(val, port);
-}
-
-static inline void rtl_pci_raw_read_port_uchar(u32 port, u8 * pval)
-{
-	*pval = inb(port);
-}
-
-static inline void rtl_pci_raw_read_port_ushort(u32 port, u16 * pval)
-{
-	*pval = inw(port);
-}
-
-static inline void rtl_pci_raw_read_port_ulong(u32 port, u32 * pval)
-{
-	*pval = inl(port);
-}
-
-#endif
diff --git a/drivers/staging/rtl8821ae/ps.c b/drivers/staging/rtl8821ae/ps.c
deleted file mode 100644
index db9a02fdb640..000000000000
--- a/drivers/staging/rtl8821ae/ps.c
+++ /dev/null
@@ -1,1001 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "base.h"
-#include "ps.h"
-#include <linux/export.h>
-#include "btcoexist/rtl_btc.h"
-
-bool rtl_ps_enable_nic(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool init_status = true;
-
-	/*<1> reset trx ring */
-	if (rtlhal->interface == INTF_PCI)
-		rtlpriv->intf_ops->reset_trx_ring(hw);
-
-	if (is_hal_stop(rtlhal))
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("Driver is already down!\n"));
-
-	/*<2> Enable Adapter */
-	rtlpriv->cfg->ops->hw_init(hw);
-	RT_CLEAR_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-	/*init_status = false; */
-
-	/*<3> Enable Interrupt */
-	rtlpriv->cfg->ops->enable_interrupt(hw);
-
-	/*<enable timer> */
-	rtl_watch_dog_timer_callback((unsigned long)hw);
-
-	return init_status;
-}
-//EXPORT_SYMBOL(rtl_ps_enable_nic);
-
-bool rtl_ps_disable_nic(struct ieee80211_hw *hw)
-{
-	bool status = true;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	/*<1> Stop all timer */
-	rtl_deinit_deferred_work(hw);
-
-	/*<2> Disable Interrupt */
-	rtlpriv->cfg->ops->disable_interrupt(hw);
-
-	/*<3> Disable Adapter */
-	rtlpriv->cfg->ops->hw_disable(hw);
-
-	return status;
-}
-//EXPORT_SYMBOL(rtl_ps_disable_nic);
-
-bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
-			 enum rf_pwrstate state_toset,
-			 u32 changesource, bool protect_or_not)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	enum rf_pwrstate rtstate;
-	bool b_actionallowed = false;
-	u16 rfwait_cnt = 0;
-
-	/*protect_or_not = true; */
-
-	if (protect_or_not)
-		goto no_protect;
-
-	/*
-	 *Only one thread can change
-	 *the RF state at one time, and others
-	 *should wait to be executed.
-	 */
-	while (true) {
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-		if (ppsc->rfchange_inprogress) {
-			spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-			RT_TRACE(COMP_ERR, DBG_WARNING,
-				 ("RF Change in progress!"
-				  "Wait to set..state_toset(%d).\n",
-				  state_toset));
-
-			/* Set RF after the previous action is done.  */
-			while (ppsc->rfchange_inprogress) {
-				rfwait_cnt++;
-				mdelay(1);
-				/*
-				 *Wait too long, return false to avoid
-				 *to be stuck here.
-				 */
-				if (rfwait_cnt > 100)
-					return false;
-			}
-		} else {
-			ppsc->rfchange_inprogress = true;
-			spin_unlock(&rtlpriv->locks.rf_ps_lock);
-			break;
-		}
-	}
-
-no_protect:
-	rtstate = ppsc->rfpwr_state;
-
-	switch (state_toset) {
-	case ERFON:
-		ppsc->rfoff_reason &= (~changesource);
-
-		if ((changesource == RF_CHANGE_BY_HW) &&
-		    (ppsc->b_hwradiooff == true)) {
-			ppsc->b_hwradiooff = false;
-		}
-
-		if (!ppsc->rfoff_reason) {
-			ppsc->rfoff_reason = 0;
-			b_actionallowed = true;
-		}
-
-		break;
-
-	case ERFOFF:
-
-		if ((changesource == RF_CHANGE_BY_HW) &&
-		    (ppsc->b_hwradiooff == false)) {
-			ppsc->b_hwradiooff = true;
-		}
-
-		ppsc->rfoff_reason |= changesource;
-		b_actionallowed = true;
-		break;
-
-	case ERFSLEEP:
-		ppsc->rfoff_reason |= changesource;
-		b_actionallowed = true;
-		break;
-
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case not process \n"));
-		break;
-	}
-
-	if (b_actionallowed)
-		rtlpriv->cfg->ops->set_rf_power_state(hw, state_toset);
-
-	if (!protect_or_not) {
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-		ppsc->rfchange_inprogress = false;
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-	}
-
-	return b_actionallowed;
-}
-//EXPORT_SYMBOL(rtl_ps_set_rf_state);
-
-static void _rtl_ps_inactive_ps(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	ppsc->b_swrf_processing = true;
-
-	if (ppsc->inactive_pwrstate == ERFON && rtlhal->interface == INTF_PCI) {
-		if ((ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM) &&
-		    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
-		    rtlhal->interface == INTF_PCI) {
-			rtlpriv->intf_ops->disable_aspm(hw);
-			RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-		}
-	}
-
-	if (rtlpriv->cfg->ops->get_btc_status()){
-		rtlpriv->btcoexist.btc_ops->btc_ips_notify(rtlpriv,
-						ppsc->inactive_pwrstate);
-	}
-	rtl_ps_set_rf_state(hw, ppsc->inactive_pwrstate,
-			    RF_CHANGE_BY_IPS, false);
-
-	if (ppsc->inactive_pwrstate == ERFOFF &&
-	    rtlhal->interface == INTF_PCI) {
-		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
-		    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
-			rtlpriv->intf_ops->enable_aspm(hw);
-			RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-		}
-	}
-
-	ppsc->b_swrf_processing = false;
-}
-
-void rtl_ips_nic_off_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-	    container_of_dwork_rtl(data, struct rtl_works, ips_nic_off_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	enum rf_pwrstate rtstate;
-
-	if (mac->opmode != NL80211_IFTYPE_STATION) {
-		RT_TRACE(COMP_ERR, DBG_WARNING, ("not station return\n"));
-		return;
-	}
-
-	if (mac->p2p_in_use)
-		return;
-
-	if (mac->link_state > MAC80211_NOLINK)
-		return;
-
-	if (is_hal_stop(rtlhal))
-		return;
-
-	if (rtlpriv->sec.being_setkey)
-		return;
-
-	if(rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps)
-		rtlpriv->cfg->ops->bt_turn_off_bt_coexist_before_enter_lps(hw);
-
-	if (ppsc->b_inactiveps) {
-		rtstate = ppsc->rfpwr_state;
-
-		/*
-		 *Do not enter IPS in the following conditions:
-		 *(1) RF is already OFF or Sleep
-		 *(2) b_swrf_processing (indicates the IPS is still under going)
-		 *(3) Connected (only disconnected can trigger IPS)
-		 *(4) IBSS (send Beacon)
-		 *(5) AP mode (send Beacon)
-		 *(6) monitor mode (rcv packet)
-		 */
-
-		if (rtstate == ERFON &&
-		    !ppsc->b_swrf_processing &&
-		    (mac->link_state == MAC80211_NOLINK) &&
-		    !mac->act_scanning) {
-			RT_TRACE(COMP_RF, DBG_LOUD,
-				 ("IPSEnter(): Turn off RF.\n"));
-
-			ppsc->inactive_pwrstate = ERFOFF;
-			ppsc->b_in_powersavemode = true;
-
-			/*rtl_pci_reset_trx_ring(hw); */
-			_rtl_ps_inactive_ps(hw);
-		}
-	}
-}
-
-void rtl_ips_nic_off(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	/*
-	 *because when link with ap, mac80211 will ask us
-	 *to disable nic quickly after scan before linking,
-	 *this will cause link failed, so we delay 100ms here
-	 */
-	queue_delayed_work(rtlpriv->works.rtl_wq,
-			   &rtlpriv->works.ips_nic_off_wq, MSECS(100));
-}
-
-/* NOTICE: any opmode should exc nic_on, or disable without
- * nic_on may something wrong, like adhoc TP*/
-void rtl_ips_nic_on(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	enum rf_pwrstate rtstate;
-
-	cancel_delayed_work(&rtlpriv->works.ips_nic_off_wq);
-
-	spin_lock(&rtlpriv->locks.ips_lock);
-	if (ppsc->b_inactiveps) {
-		rtstate = ppsc->rfpwr_state;
-
-		if (rtstate != ERFON &&
-		    !ppsc->b_swrf_processing &&
-		    ppsc->rfoff_reason <= RF_CHANGE_BY_IPS) {
-
-			ppsc->inactive_pwrstate = ERFON;
-			ppsc->b_in_powersavemode = false;
-			_rtl_ps_inactive_ps(hw);
-		}
-	}
-	spin_unlock(&rtlpriv->locks.ips_lock);
-}
-
-/*for FW LPS*/
-
-/*
- *Determine if we can set Fw into PS mode
- *in current condition.Return true if it
- *can enter PS mode.
- */
-static bool rtl_get_fwlps_doze(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u32 ps_timediff;
-
-	ps_timediff = jiffies_to_msecs(jiffies -
-				       ppsc->last_delaylps_stamp_jiffies);
-
-	if (ps_timediff < 2000) {
-		RT_TRACE(COMP_POWER, DBG_LOUD,
-			 ("Delay enter Fw LPS for DHCP, ARP,"
-			  " or EAPOL exchanging state.\n"));
-		return false;
-	}
-
-	if (mac->link_state != MAC80211_LINKED)
-		return false;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		return false;
-
-	return true;
-}
-
-/* Change current and default preamble mode.*/
-void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool enter_fwlps;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		return;
-
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-
-	if (ppsc->dot11_psmode == rt_psmode)
-		return;
-
-	/* Update power save mode configured. */
-	ppsc->dot11_psmode = rt_psmode;
-
-	/*
-	 *<FW control LPS>
-	 *1. Enter PS mode
-	 *   Set RPWM to Fw to turn RF off and send H2C fw_pwrmode
-	 *   cmd to set Fw into PS mode.
-	 *2. Leave PS mode
-	 *   Send H2C fw_pwrmode cmd to Fw to set Fw into Active
-	 *   mode and set RPWM to turn RF on.
-	 */
-
-	if ((ppsc->b_fwctrl_lps) && ppsc->report_linked) {
-		if (ppsc->dot11_psmode == EACTIVE) {
-			RT_TRACE(COMP_RF, DBG_DMESG,
-				 ("FW LPS leave ps_mode:%x\n",
-				  FW_PS_ACTIVE_MODE));
-			enter_fwlps = false;
-			ppsc->pwr_mode = FW_PS_ACTIVE_MODE;
-			ppsc->smart_ps = 0;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_LPS_ACTION,
-						      (u8 *)(&enter_fwlps));
-			if (ppsc->p2p_ps_info.opp_ps)
-				rtl_p2p_ps_cmd(hw,P2P_PS_ENABLE);
-
-		} else {
-			if (rtl_get_fwlps_doze(hw)) {
-				RT_TRACE(COMP_RF, DBG_DMESG,
-					 ("FW LPS enter ps_mode:%x\n",
-					 ppsc->fwctrl_psmode));
-				enter_fwlps = true;
-				ppsc->pwr_mode = ppsc->fwctrl_psmode;
-				ppsc->smart_ps = 2;
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-							HW_VAR_FW_LPS_ACTION,
-							(u8 *)(&enter_fwlps));
-
-			} else {
-				/* Reset the power save related parameters. */
-				ppsc->dot11_psmode = EACTIVE;
-			}
-		}
-	}
-}
-
-/*Enter the leisure power save mode.*/
-void rtl_lps_enter(struct ieee80211_hw *hw)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	unsigned long flag;
-
-	if (!ppsc->b_fwctrl_lps)
-		return;
-
-	if (rtlpriv->sec.being_setkey)
-		return;
-
-	if (rtlpriv->link_info.b_busytraffic)
-		return;
-
-	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
-	if (mac->cnt_after_linked < 5)
-		return;
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		return;
-
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-
-	/* Idle for a while if we connect to AP a while ago. */
-	if (mac->cnt_after_linked >= 2) {
-		if (ppsc->dot11_psmode == EACTIVE) {
-			RT_TRACE(COMP_POWER, DBG_LOUD,
-				 ("Enter 802.11 power save mode...\n"));
-
-			rtl_lps_set_psmode(hw, EAUTOPS);
-		}
-	}
-
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-}
-
-/*Leave the leisure power save mode.*/
-void rtl_lps_leave(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	unsigned long flag;
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-
-	if (ppsc->b_fwctrl_lps) {
-		if (ppsc->dot11_psmode != EACTIVE) {
-
-			/*FIX ME */
-			rtlpriv->cfg->ops->enable_interrupt(hw);
-
-			if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
-			    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM) &&
-			    rtlhal->interface == INTF_PCI) {
-				rtlpriv->intf_ops->disable_aspm(hw);
-				RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-			}
-
-			RT_TRACE(COMP_POWER, DBG_LOUD,
-				 ("Busy Traffic,Leave 802.11 power save..\n"));
-
-			rtl_lps_set_psmode(hw, EACTIVE);
-		}
-	}
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-}
-
-/* For sw LPS*/
-void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = (void *) data;
-	struct ieee80211_tim_ie *tim_ie;
-	u8 *tim;
-	u8 tim_len;
-	bool u_buffed;
-	bool m_buffed;
-
-	if (mac->opmode != NL80211_IFTYPE_STATION)
-		return;
-
-	if (!rtlpriv->psc.b_swctrl_lps)
-		return;
-
-	if (rtlpriv->mac80211.link_state != MAC80211_LINKED)
-		return;
-
-	if (!rtlpriv->psc.sw_ps_enabled)
-		return;
-
-	if (rtlpriv->psc.b_fwctrl_lps)
-		return;
-
-	if (likely(!(hw->conf.flags & IEEE80211_CONF_PS)))
-		return;
-
-	/* check if this really is a beacon */
-	if (!ieee80211_is_beacon(hdr->frame_control))
-		return;
-
-	/* min. beacon length + FCS_LEN */
-	if (len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	rtlpriv->psc.last_beacon = jiffies;
-
-	tim = rtl_find_ie(data, len - FCS_LEN, WLAN_EID_TIM);
-	if (!tim)
-		return;
-
-	if (tim[1] < sizeof(*tim_ie))
-		return;
-
-	tim_len = tim[1];
-	tim_ie = (struct ieee80211_tim_ie *) &tim[2];
-
-	if (!WARN_ON_ONCE(!hw->conf.ps_dtim_period))
-		rtlpriv->psc.dtim_counter = tim_ie->dtim_count;
-
-	/* Check whenever the PHY can be turned off again. */
-
-	/* 1. What about buffered unicast traffic for our AID? */
-	u_buffed = ieee80211_check_tim(tim_ie, tim_len,
-				       rtlpriv->mac80211.assoc_id);
-
-	/* 2. Maybe the AP wants to send multicast/broadcast data? */
-	m_buffed = tim_ie->bitmap_ctrl & 0x01;
-	rtlpriv->psc.multi_buffered = m_buffed;
-
-	/* unicast will process by mac80211 through
-	 * set ~IEEE80211_CONF_PS, So we just check
-	 * multicast frames here */
-	if (!m_buffed ) {//&&) {// !rtlpriv->psc.tx_doing) {
-		/* back to low-power land. and delay is
-		 * prevent null power save frame tx fail */
-		queue_delayed_work(rtlpriv->works.rtl_wq,
-				   &rtlpriv->works.ps_work, MSECS(5));
-	} else {
-		RT_TRACE(COMP_POWER, DBG_DMESG,
-			 ("u_bufferd: %x, m_buffered: %x\n",
-			  u_buffed, m_buffed));
-	}
-}
-
-void rtl_swlps_rf_awake(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	unsigned long flag;
-
-	if (!rtlpriv->psc.b_swctrl_lps)
-		return;
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-
-	if (ppsc->reg_rfps_level & RT_RF_LPS_LEVEL_ASPM &&
-	    RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
-		rtlpriv->intf_ops->disable_aspm(hw);
-		RT_CLEAR_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-	}
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-	rtl_ps_set_rf_state(hw, ERFON, RF_CHANGE_BY_PS, false);
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-}
-
-void rtl_swlps_rfon_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-	    container_of_dwork_rtl(data, struct rtl_works, ps_rfon_wq);
-	struct ieee80211_hw *hw = rtlworks->hw;
-
-	rtl_swlps_rf_awake(hw);
-}
-
-void rtl_swlps_rf_sleep(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	unsigned long flag;
-	u8 sleep_intv;
-
-	if (!rtlpriv->psc.sw_ps_enabled)
-		return;
-
-	if ((rtlpriv->sec.being_setkey) ||
-	    (mac->opmode == NL80211_IFTYPE_ADHOC))
-		return;
-
-	/*sleep after linked 10s, to let DHCP and 4-way handshake ok enough!! */
-	if ((mac->link_state != MAC80211_LINKED) || (mac->cnt_after_linked < 5))
-		return;
-
-	if (rtlpriv->link_info.b_busytraffic)
-		return;
-
-	spin_lock(&rtlpriv->locks.rf_ps_lock);
-	if (rtlpriv->psc.rfchange_inprogress) {
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-		return;
-	}
-	spin_unlock(&rtlpriv->locks.rf_ps_lock);
-
-	spin_lock_irqsave(&rtlpriv->locks.lps_lock, flag);
-	rtl_ps_set_rf_state(hw, ERFSLEEP, RF_CHANGE_BY_PS,false);
-	spin_unlock_irqrestore(&rtlpriv->locks.lps_lock, flag);
-
-	if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_ASPM &&
-	    !RT_IN_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM)) {
-		rtlpriv->intf_ops->enable_aspm(hw);
-		RT_SET_PS_LEVEL(ppsc, RT_PS_LEVEL_ASPM);
-	}
-
-	/* here is power save alg, when this beacon is DTIM
-	 * we will set sleep time to dtim_period * n;
-	 * when this beacon is not DTIM, we will set sleep
-	 * time to sleep_intv = rtlpriv->psc.dtim_counter or
-	 * MAX_SW_LPS_SLEEP_INTV(default set to 5) */
-
-	if (rtlpriv->psc.dtim_counter == 0) {
-		if (hw->conf.ps_dtim_period == 1)
-			sleep_intv = hw->conf.ps_dtim_period * 2;
-		else
-			sleep_intv = hw->conf.ps_dtim_period;
-	} else {
-		sleep_intv = rtlpriv->psc.dtim_counter;
-	}
-
-	if (sleep_intv > MAX_SW_LPS_SLEEP_INTV)
-		sleep_intv = MAX_SW_LPS_SLEEP_INTV;
-
-	/* this print should always be dtim_conter = 0 &
-	 * sleep  = dtim_period, that meaons, we should
-	 * awake before every dtim */
-	RT_TRACE(COMP_POWER, DBG_DMESG,
-		 ("dtim_counter:%x will sleep :%d beacon_intv\n",
-		  rtlpriv->psc.dtim_counter, sleep_intv));
-
-	/* we tested that 40ms is enough for sw & hw sw delay */
-	queue_delayed_work(rtlpriv->works.rtl_wq, &rtlpriv->works.ps_rfon_wq,
-			MSECS(sleep_intv * mac->vif->bss_conf.beacon_int - 40));
-}
-
-
-void rtl_swlps_wq_callback(void *data)
-{
-	struct rtl_works *rtlworks =
-		container_of_dwork_rtl(data, struct rtl_works, ps_work);
-	struct ieee80211_hw *hw = rtlworks->hw;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool ps = false;
-
-	ps = (hw->conf.flags & IEEE80211_CONF_PS);
-
-	/* we can sleep after ps null send ok */
-	if (rtlpriv->psc.state_inap) {
-		rtl_swlps_rf_sleep(hw);
-
-		if (rtlpriv->psc.state && !ps) {
-			rtlpriv->psc.sleep_ms =
-				jiffies_to_msecs(jiffies -
-						 rtlpriv->psc.last_action);
-		}
-
-		if (ps)
-			rtlpriv->psc.last_slept = jiffies;
-
-		rtlpriv->psc.last_action = jiffies;
-		rtlpriv->psc.state = ps;
-	}
-}
-
-
-static void rtl_p2p_noa_ie(struct ieee80211_hw *hw, void *data,
-			   unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
-	u8 *pos, *end, *ie;
-	u16 noa_len;
-	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
-	u8 noa_num, index,i, noa_index = 0;
-	bool find_p2p_ie = false , find_p2p_ps_ie = false;
-	pos = (u8 *)mgmt->u.beacon.variable;
-	end = data + len;
-	ie = NULL;
-
-	while (pos + 1 < end) {
-
-		if (pos + 2 + pos[1] > end)
-			return;
-
-		if (pos[0] == 221 && pos[1] > 4) {
-			if (memcmp(&pos[2], p2p_oui_ie_type, 4) == 0) {
-				ie = pos + 2+4;
-				break;
-			}
-		}
-		pos += 2 + pos[1];
-	}
-
-	if (ie == NULL)
-		return;
-	find_p2p_ie = true;
-	/*to find noa ie*/
-	while (ie + 1 < end) {
-		noa_len = READEF2BYTE(&ie[1]);
-		if (ie + 3 + ie[1] > end)
-			return;
-
-		if (ie[0] == 12) {
-			find_p2p_ps_ie = true;
-			if ( (noa_len - 2) % 13 != 0){
-				RT_TRACE(COMP_INIT, DBG_LOUD,
-					 ("P2P notice of absence: "
-					  "invalid length.%d\n",noa_len));
-				return;
-			} else {
-				noa_num = (noa_len - 2) / 13;
-			}
-			noa_index = ie[3];
-			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == P2P_PS_NONE
-			    || noa_index != p2pinfo->noa_index) {
-				RT_TRACE(COMP_FW, DBG_LOUD,
-					 ("update NOA ie.\n"));
-				p2pinfo->noa_index = noa_index;
-				p2pinfo->opp_ps= (ie[4] >> 7);
-				p2pinfo->ctwindow = ie[4] & 0x7F;
-				p2pinfo->noa_num = noa_num;
-				index = 5;
-				for (i = 0; i< noa_num; i++){
-					p2pinfo->noa_count_type[i] =
-							READEF1BYTE(ie+index);
-					index += 1;
-					p2pinfo->noa_duration[i] =
-							READEF4BYTE(ie+index);
-					index += 4;
-					p2pinfo->noa_interval[i] =
-							READEF4BYTE(ie+index);
-					index += 4;
-					p2pinfo->noa_start_time[i] =
-							READEF4BYTE(ie+index);
-					index += 4;
-				}
-
-				if (p2pinfo->opp_ps == 1) {
-					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
-					/* Driver should wait LPS
-					 * entering CTWindow*/
-					if (rtlpriv->psc.b_fw_current_inpsmode){
-						rtl_p2p_ps_cmd(hw,
-							       P2P_PS_ENABLE);
-					}
-				} else if (p2pinfo->noa_num > 0) {
-					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
-					rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
-				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-				}
-			}
-
-		break;
-		}
-		ie += 3 + noa_len;
-	}
-
-	if (find_p2p_ie == true) {
-		if ((p2pinfo->p2p_ps_mode > P2P_PS_NONE) &&
-		    (find_p2p_ps_ie == false))
-			rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-	}
-}
-
-static void rtl_p2p_action_ie(struct ieee80211_hw *hw, void *data,
-			      unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct ieee80211_mgmt *mgmt = (void *)data;
-	struct rtl_p2p_ps_info *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
-	bool find_p2p_ie = false , find_p2p_ps_ie = false;
-	u8 noa_num, index,i, noa_index = 0;
-	u8 *pos, *end, *ie;
-	u16 noa_len;
-	static u8 p2p_oui_ie_type[4] = {0x50, 0x6f, 0x9a, 0x09};
-
-	pos = (u8 *) &mgmt->u.action.category;
-	end = data + len;
-	ie = NULL;
-
-	if (pos[0] == 0x7f ) {
-		if (memcmp(&pos[1], p2p_oui_ie_type, 4) == 0) {
-			ie = pos + 3+4;
-		}
-	}
-
-	if (ie == NULL)
-		return;
-	find_p2p_ie = true;
-
-	RT_TRACE(COMP_FW, DBG_LOUD, ("action frame find P2P IE.\n"));
-	/*to find noa ie*/
-	while (ie + 1 < end) {
-		noa_len = READEF2BYTE(&ie[1]);
-		if (ie + 3 + ie[1] > end)
-			return;
-
-		if (ie[0] == 12) {
-			RT_TRACE(COMP_FW, DBG_LOUD, ("find NOA IE.\n"));
-			RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD, ("noa ie "),
-				      ie, noa_len);
-			find_p2p_ps_ie = true;
-			if ( (noa_len - 2) % 13 != 0){
-				RT_TRACE(COMP_FW, DBG_LOUD,
-					 ("P2P notice of absence: "
-					  "invalid length.%d\n",noa_len));
-				return;
-			} else {
-				noa_num = (noa_len - 2) / 13;
-			}
-			noa_index = ie[3];
-			if (rtlpriv->psc.p2p_ps_info.p2p_ps_mode == P2P_PS_NONE
-			    || noa_index != p2pinfo->noa_index) {
-				p2pinfo->noa_index = noa_index;
-				p2pinfo->opp_ps= (ie[4] >> 7);
-				p2pinfo->ctwindow = ie[4] & 0x7F;
-				p2pinfo->noa_num = noa_num;
-				index = 5;
-				for (i = 0; i< noa_num; i++){
-					p2pinfo->noa_count_type[i] =
-							READEF1BYTE(ie+index);
-					index += 1;
-					p2pinfo->noa_duration[i] =
-							READEF4BYTE(ie+index);
-					index += 4;
-					p2pinfo->noa_interval[i] =
-							READEF4BYTE(ie+index);
-					index += 4;
-					p2pinfo->noa_start_time[i] =
-							READEF4BYTE(ie+index);
-					index += 4;
-				}
-
-				if (p2pinfo->opp_ps == 1) {
-					p2pinfo->p2p_ps_mode = P2P_PS_CTWINDOW;
-					/* Driver should wait LPS
-					 * entering CTWindow */
-					if (rtlpriv->psc.b_fw_current_inpsmode){
-						rtl_p2p_ps_cmd(hw,
-							       P2P_PS_ENABLE);
-					}
-				} else if (p2pinfo->noa_num > 0) {
-					p2pinfo->p2p_ps_mode = P2P_PS_NOA;
-					rtl_p2p_ps_cmd(hw, P2P_PS_ENABLE);
-				} else if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-					rtl_p2p_ps_cmd(hw, P2P_PS_DISABLE);
-				}
-			}
-
-		break;
-		}
-		ie += 3 + noa_len;
-	}
-
-
-}
-
-
-void rtl_p2p_ps_cmd(struct ieee80211_hw *hw,u8 p2p_ps_state)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
-	struct rtl_p2p_ps_info  *p2pinfo = &(rtlpriv->psc.p2p_ps_info);
-
-	RT_TRACE(COMP_FW, DBG_LOUD, (" p2p state %x\n",p2p_ps_state));
-	switch (p2p_ps_state) {
-		case P2P_PS_DISABLE:
-			p2pinfo->p2p_ps_state = p2p_ps_state;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						   HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-						   (u8 *)(&p2p_ps_state));
-
-			p2pinfo->noa_index = 0;
-			p2pinfo->ctwindow = 0;
-			p2pinfo->opp_ps = 0;
-			p2pinfo->noa_num = 0;
-			p2pinfo->p2p_ps_mode = P2P_PS_NONE;
-			if (rtlps->b_fw_current_inpsmode == true) {
-				if (rtlps->smart_ps == 0) {
-					rtlps->smart_ps = 2;
-					rtlpriv->cfg->ops->set_hw_reg(hw,
-						    HW_VAR_H2C_FW_PWRMODE,
-						    (u8 *)(&rtlps->pwr_mode));
-				}
-
-			}
-			break;
-		case P2P_PS_ENABLE:
-			if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-				p2pinfo->p2p_ps_state = p2p_ps_state;
-
-				if (p2pinfo->ctwindow > 0) {
-					if (rtlps->smart_ps != 0){
-						rtlps->smart_ps = 0;
-						rtlpriv->cfg->ops->set_hw_reg(
-						    hw, HW_VAR_H2C_FW_PWRMODE,
-						    (u8 *)(&rtlps->pwr_mode));
-					}
-				}
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-						(u8 *)(&p2p_ps_state));
-
-			}
-			break;
-		case P2P_PS_SCAN:
-		case P2P_PS_SCAN_DONE:
-		case P2P_PS_ALLSTASLEEP:
-			if (p2pinfo->p2p_ps_mode > P2P_PS_NONE) {
-				p2pinfo->p2p_ps_state = p2p_ps_state;
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-						HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-						(u8 *)(&p2p_ps_state));
-			}
-			break;
-		default:
-			break;
-
-	}
-	RT_TRACE(COMP_FW, DBG_LOUD, (" ctwindow %x oppps %x \n",
-		 		     p2pinfo->ctwindow,p2pinfo->opp_ps));
-	RT_TRACE(COMP_FW, DBG_LOUD, ("count %x duration %x index %x interval %x"
-				     " start time %x noa num %x\n",
-				     p2pinfo->noa_count_type[0],
-				     p2pinfo->noa_duration[0],
-				     p2pinfo->noa_index,
-				     p2pinfo->noa_interval[0],
-				     p2pinfo->noa_start_time[0],
-				     p2pinfo->noa_num));
-	RT_TRACE(COMP_FW, DBG_LOUD, ("end\n"));
-}
-
-void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct ieee80211_hdr *hdr = (void *) data;
-
-	if (!mac->p2p)
-		return;
-	if (mac->link_state != MAC80211_LINKED)
-		return;
-	/* min. beacon length + FCS_LEN */
-	if (len <= 40 + FCS_LEN)
-		return;
-
-	/* and only beacons from the associated BSSID, please */
-	if (ether_addr_equal(hdr->addr3, rtlpriv->mac80211.bssid))
-		return;
-
-	/* check if this really is a beacon */
-	if (!(ieee80211_is_beacon(hdr->frame_control) ||
-	      ieee80211_is_probe_resp(hdr->frame_control) ||
-	      ieee80211_is_action(hdr->frame_control)))
-		return;
-
-	if (ieee80211_is_action(hdr->frame_control)) {
-		rtl_p2p_action_ie(hw,data,len - FCS_LEN);
-	} else {
-		rtl_p2p_noa_ie(hw,data,len - FCS_LEN);
-	}
-
-}
diff --git a/drivers/staging/rtl8821ae/ps.h b/drivers/staging/rtl8821ae/ps.h
deleted file mode 100644
index 374ed77c4126..000000000000
--- a/drivers/staging/rtl8821ae/ps.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __REALTEK_RTL_PCI_PS_H__
-#define __REALTEK_RTL_PCI_PS_H__
-
-#define MAX_SW_LPS_SLEEP_INTV	5
-
-bool rtl_ps_set_rf_state(struct ieee80211_hw *hw,
-			 enum rf_pwrstate state_toset, u32 changesource,
-			 bool protect_or_not);
-bool rtl_ps_enable_nic(struct ieee80211_hw *hw);
-bool rtl_ps_disable_nic(struct ieee80211_hw *hw);
-void rtl_ips_nic_off(struct ieee80211_hw *hw);
-void rtl_ips_nic_on(struct ieee80211_hw *hw);
-void rtl_ips_nic_off_wq_callback(void *data);
-void rtl_lps_enter(struct ieee80211_hw *hw);
-void rtl_lps_leave(struct ieee80211_hw *hw);
-
-void rtl_lps_set_psmode(struct ieee80211_hw *hw, u8 rt_psmode);
-
-void rtl_swlps_beacon(struct ieee80211_hw *hw, void *data, unsigned int len);
-void rtl_swlps_wq_callback(void *data);
-void rtl_swlps_rfon_wq_callback(void *data);
-void rtl_swlps_rf_awake(struct ieee80211_hw *hw);
-void rtl_swlps_rf_sleep(struct ieee80211_hw *hw);
-void rtl_p2p_ps_cmd(struct ieee80211_hw *hw,u8 p2p_ps_state);
-void rtl_p2p_info(struct ieee80211_hw *hw, void *data, unsigned int len);
-#endif
diff --git a/drivers/staging/rtl8821ae/rc.c b/drivers/staging/rtl8821ae/rc.c
deleted file mode 100644
index 0b4f32185082..000000000000
--- a/drivers/staging/rtl8821ae/rc.c
+++ /dev/null
@@ -1,290 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "base.h"
-#include "rc.h"
-
-/*
- *Finds the highest rate index we can use
- *if skb is special data like DHCP/EAPOL, we set should
- *it to lowest rate CCK_1M, otherwise we set rate to
- *highest rate based on wireless mode used for iwconfig
- *show Tx rate.
- */
-static u8 _rtl_rc_get_highest_rix(struct rtl_priv *rtlpriv,
-				  struct ieee80211_sta *sta,
-				  struct sk_buff *skb, bool not_data)
-{
-	struct rtl_mac *rtlmac = rtl_mac(rtlpriv);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_sta_info *sta_entry = NULL;
-	u8 wireless_mode = 0;
-
-	/*
-	 *this rate is no use for true rate, firmware
-	 *will control rate at all it just used for
-	 *1.show in iwconfig in B/G mode
-	 *2.in rtl_get_tcb_desc when we check rate is
-	 *      1M we will not use FW rate but user rate.
-	 */
-	if (rtlmac->opmode == NL80211_IFTYPE_AP ||
-	    rtlmac->opmode == NL80211_IFTYPE_ADHOC ||
-	    rtlmac->opmode == NL80211_IFTYPE_MESH_POINT) {
-		if (sta) {
-			sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-			wireless_mode = sta_entry->wireless_mode;
-		} else {
-			return 0;
-		}
-	} else {
-		wireless_mode = rtlmac->mode;
-	}
-
-	if (rtl_is_special_data(rtlpriv->mac80211.hw, skb, true) || not_data) {
-		return 0;
-	} else {
-		if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-			if (wireless_mode == WIRELESS_MODE_B) {
-				return B_MODE_MAX_RIX;
-			} else if (wireless_mode == WIRELESS_MODE_G) {
-				return G_MODE_MAX_RIX;
-			} else {
-				if (get_rf_type(rtlphy) != RF_2T2R)
-					return N_MODE_MCS7_RIX;
-				else
-					return N_MODE_MCS15_RIX;
-			}
-		} else {
-			if (wireless_mode == WIRELESS_MODE_A) {
-				return A_MODE_MAX_RIX;
-			} else {
-				if (get_rf_type(rtlphy) != RF_2T2R)
-					return N_MODE_MCS7_RIX;
-				else
-					return N_MODE_MCS15_RIX;
-			}
-		}
-	}
-}
-
-static void _rtl_rc_rate_set_series(struct rtl_priv *rtlpriv,
-				    struct ieee80211_sta *sta,
-				    struct ieee80211_tx_rate *rate,
-				    struct ieee80211_tx_rate_control *txrc,
-				    u8 tries, char rix, int rtsctsenable,
-				    bool not_data)
-{
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	u8 sgi_20 = 0, sgi_40 = 0;
-
-	if (sta) {
-		sgi_20 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20;
-		sgi_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40;
-	}
-	rate->count = tries;
-	rate->idx = rix >= 0x00 ? rix : 0x00;
-
-	if (!not_data) {
-		if (txrc->short_preamble)
-			rate->flags |= IEEE80211_TX_RC_USE_SHORT_PREAMBLE;
-		if (mac->opmode == NL80211_IFTYPE_AP ||
-			mac->opmode == NL80211_IFTYPE_ADHOC) {
-			if (sta && (sta->ht_cap.cap &
-				    IEEE80211_HT_CAP_SUP_WIDTH_20_40))
-				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
-		} else {
-			if (mac->bw_40)
-				rate->flags |= IEEE80211_TX_RC_40_MHZ_WIDTH;
-		}
-		if (sgi_20 || sgi_40)
-			rate->flags |= IEEE80211_TX_RC_SHORT_GI;
-		if (sta && sta->ht_cap.ht_supported)
-			rate->flags |= IEEE80211_TX_RC_MCS;
-	}
-}
-
-static void rtl_get_rate(void *ppriv, struct ieee80211_sta *sta,
-			 void *priv_sta,
-			 struct ieee80211_tx_rate_control *txrc)
-{
-	struct rtl_priv *rtlpriv = ppriv;
-	struct sk_buff *skb = txrc->skb;
-	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
-	struct ieee80211_tx_rate *rates = tx_info->control.rates;
-	__le16 fc = rtl_get_fc(skb);
-	u8 try_per_rate, i, rix;
-	bool not_data = !ieee80211_is_data(fc);
-
-	if (rate_control_send_low(sta, priv_sta, txrc))
-		return;
-
-	rix = _rtl_rc_get_highest_rix(rtlpriv, sta, skb, not_data);
-	try_per_rate = 1;
-	_rtl_rc_rate_set_series(rtlpriv, sta, &rates[0], txrc,
-				try_per_rate, rix, 1, not_data);
-
-	if (!not_data) {
-		for (i = 1; i < 4; i++)
-			_rtl_rc_rate_set_series(rtlpriv, sta, &rates[i],
-						txrc, i, (rix - i), 1,
-						not_data);
-	}
-}
-
-static bool _rtl_tx_aggr_check(struct rtl_priv *rtlpriv,
-			       struct rtl_sta_info *sta_entry, u16 tid)
-{
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-
-	if (mac->act_scanning)
-		return false;
-
-	if (mac->opmode == NL80211_IFTYPE_STATION &&
-	    mac->cnt_after_linked < 3)
-		return false;
-
-	if (sta_entry->tids[tid].agg.agg_state == RTL_AGG_STOP)
-		return true;
-
-	return false;
-}
-
-/*mac80211 Rate Control callbacks*/
-static void rtl_tx_status(void *ppriv,
-			  struct ieee80211_supported_band *sband,
-			  struct ieee80211_sta *sta, void *priv_sta,
-			  struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = ppriv;
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	struct ieee80211_hdr *hdr = rtl_get_hdr(skb);
-	__le16 fc = rtl_get_fc(skb);
-	struct rtl_sta_info *sta_entry;
-
-	if (!priv_sta || !ieee80211_is_data(fc))
-		return;
-
-	if (rtl_is_special_data(mac->hw, skb, true))
-		return;
-
-	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
-	    is_broadcast_ether_addr(ieee80211_get_DA(hdr)))
-		return;
-
-	if (sta) {
-		/* Check if aggregation has to be enabled for this tid */
-		sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-		if ((sta->ht_cap.ht_supported == true) &&
-				!(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
-			if (ieee80211_is_data_qos(fc)) {
-				u8 tid = rtl_get_tid(skb);
-				if (_rtl_tx_aggr_check(rtlpriv, sta_entry,
-						       tid)) {
-					sta_entry->tids[tid].agg.agg_state =
-						RTL_AGG_PROGRESS;
-					ieee80211_start_tx_ba_session(sta, tid,
-								      5000);
-				}
-			}
-		}
-	}
-}
-
-static void rtl_rate_init(void *ppriv,
-			  struct ieee80211_supported_band *sband,
-			  struct cfg80211_chan_def *chandef,
-			  struct ieee80211_sta *sta, void *priv_sta)
-{
-}
-static void rtl_rate_update(void *ppriv,
-			    struct ieee80211_supported_band *sband,
-			    struct cfg80211_chan_def *chandef,
-			    struct ieee80211_sta *sta, void *priv_sta,
-			    u32 changed)
-{
-}
-static void *rtl_rate_alloc(struct ieee80211_hw *hw, struct dentry *debugfsdir)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	return rtlpriv;
-}
-
-static void rtl_rate_free(void *rtlpriv)
-{
-	return;
-}
-
-static void *rtl_rate_alloc_sta(void *ppriv,
-				struct ieee80211_sta *sta, gfp_t gfp)
-{
-	struct rtl_priv *rtlpriv = ppriv;
-	struct rtl_rate_priv *rate_priv;
-
-	rate_priv = kzalloc(sizeof(struct rtl_rate_priv), gfp);
-	if (!rate_priv) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Unable to allocate private rc structure\n"));
-		return NULL;
-	}
-
-	rtlpriv->rate_priv = rate_priv;
-
-	return rate_priv;
-}
-
-static void rtl_rate_free_sta(void *rtlpriv,
-			      struct ieee80211_sta *sta, void *priv_sta)
-{
-	struct rtl_rate_priv *rate_priv = priv_sta;
-	kfree(rate_priv);
-}
-
-static struct rate_control_ops rtl_rate_ops = {
-	.name = "rtl_rc_21ae",
-	.alloc = rtl_rate_alloc,
-	.free = rtl_rate_free,
-	.alloc_sta = rtl_rate_alloc_sta,
-	.free_sta = rtl_rate_free_sta,
-	.rate_init = rtl_rate_init,
-	.rate_update = rtl_rate_update,
-	.tx_status = rtl_tx_status,
-	.get_rate = rtl_get_rate,
-};
-
-int rtl_rate_control_register(void)
-{
-	return ieee80211_rate_control_register(&rtl_rate_ops);
-}
-
-void rtl_rate_control_unregister(void)
-{
-	ieee80211_rate_control_unregister(&rtl_rate_ops);
-}
diff --git a/drivers/staging/rtl8821ae/regd.c b/drivers/staging/rtl8821ae/regd.c
deleted file mode 100644
index 2efa5f31822b..000000000000
--- a/drivers/staging/rtl8821ae/regd.c
+++ /dev/null
@@ -1,451 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "wifi.h"
-#include "regd.h"
-
-static struct country_code_to_enum_rd allCountries[] = {
-	{COUNTRY_CODE_FCC, "US"},
-	{COUNTRY_CODE_IC, "US"},
-	{COUNTRY_CODE_ETSI, "EC"},
-	{COUNTRY_CODE_SPAIN, "EC"},
-	{COUNTRY_CODE_FRANCE, "EC"},
-	{COUNTRY_CODE_MKK, "JP"},
-	{COUNTRY_CODE_MKK1, "JP"},
-	{COUNTRY_CODE_ISRAEL, "EC"},
-	{COUNTRY_CODE_TELEC, "JP"},
-	{COUNTRY_CODE_MIC, "JP"},
-	{COUNTRY_CODE_GLOBAL_DOMAIN, "JP"},
-	{COUNTRY_CODE_WORLD_WIDE_13, "EC"},
-	{COUNTRY_CODE_TELEC_NETGEAR, "EC"},
-};
-
-/*
- *Only these channels all allow active
- *scan on all world regulatory domains
- */
-#define RTL819x_2GHZ_CH01_11	\
-	REG_RULE(2412-10, 2462+10, 40, 0, 20, 0)
-
-/*
- *We enable active scan on these a case
- *by case basis by regulatory domain
- */
-#define RTL819x_2GHZ_CH12_13	\
-	REG_RULE(2467-10, 2472+10, 40, 0, 20,\
-	NL80211_RRF_PASSIVE_SCAN)
-
-#define RTL819x_2GHZ_CH14	\
-	REG_RULE(2484-10, 2484+10, 40, 0, 20, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_OFDM)
-
-/* 5G chan 36 - chan 64*/
-#define RTL819x_5GHZ_5150_5350	\
-	REG_RULE(5150-10, 5350+10, 40, 0, 30, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_IBSS)
-
-/* 5G chan 100 - chan 165*/
-#define RTL819x_5GHZ_5470_5850	\
-	REG_RULE(5470-10, 5850+10, 40, 0, 30, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_IBSS)
-
-/* 5G chan 149 - chan 165*/
-#define RTL819x_5GHZ_5725_5850	\
-	REG_RULE(5725-10, 5850+10, 40, 0, 30, \
-	NL80211_RRF_PASSIVE_SCAN | \
-	NL80211_RRF_NO_IBSS)
-
-#define RTL819x_5GHZ_ALL	\
-	RTL819x_5GHZ_5150_5350, RTL819x_5GHZ_5470_5850
-
-static const struct ieee80211_regdomain rtl_regdom_11 = {
-	.n_reg_rules = 1,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_12_13 = {
-	.n_reg_rules = 2,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_no_midband = {
-	.n_reg_rules = 3,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_5GHZ_5150_5350,
-			  RTL819x_5GHZ_5725_5850,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_60_64 = {
-	.n_reg_rules = 3,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-			  RTL819x_5GHZ_5725_5850,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_14_60_64 = {
-	.n_reg_rules = 4,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-			  RTL819x_2GHZ_CH14,
-			  RTL819x_5GHZ_5725_5850,
-		      }
-};
-
-static const struct ieee80211_regdomain rtl_regdom_14 = {
-	.n_reg_rules = 3,
-	.alpha2 = "99",
-	.reg_rules = {
-		      RTL819x_2GHZ_CH01_11,
-			  RTL819x_2GHZ_CH12_13,
-			  RTL819x_2GHZ_CH14,
-		      }
-};
-
-static bool _rtl_is_radar_freq(u16 center_freq)
-{
-	return (center_freq >= 5260 && center_freq <= 5700);
-}
-
-static void _rtl_reg_apply_beaconing_flags(struct wiphy *wiphy,
-					   enum nl80211_reg_initiator initiator)
-{
-	enum ieee80211_band band;
-	struct ieee80211_supported_band *sband;
-	const struct ieee80211_reg_rule *reg_rule;
-	struct ieee80211_channel *ch;
-	unsigned int i;
-	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
-
-		if (!wiphy->bands[band])
-			continue;
-
-		sband = wiphy->bands[band];
-
-		for (i = 0; i < sband->n_channels; i++) {
-			ch = &sband->channels[i];
-			if (_rtl_is_radar_freq(ch->center_freq) ||
-			    (ch->flags & IEEE80211_CHAN_RADAR))
-				continue;
-			if (initiator == NL80211_REGDOM_SET_BY_COUNTRY_IE) {
-				reg_rule = freq_reg_info(wiphy, ch->center_freq);
-				if (IS_ERR(reg_rule))
-					continue;
-
-				/*
-				 *If 11d had a rule for this channel ensure
-				 *we enable adhoc/beaconing if it allows us to
-				 *use it. Note that we would have disabled it
-				 *by applying our static world regdomain by
-				 *default during init, prior to calling our
-				 *regulatory_hint().
-				 */
-
-				if (!(reg_rule->flags & NL80211_RRF_NO_IBSS))
-					ch->flags &= ~IEEE80211_CHAN_NO_IBSS;
-				if (!(reg_rule->flags &
-				      NL80211_RRF_PASSIVE_SCAN))
-					ch->flags &=
-					    ~IEEE80211_CHAN_PASSIVE_SCAN;
-			} else {
-				if (ch->beacon_found)
-					ch->flags &= ~(IEEE80211_CHAN_NO_IBSS |
-						   IEEE80211_CHAN_PASSIVE_SCAN);
-			}
-		}
-	}
-}
-
-/* Allows active scan scan on Ch 12 and 13 */
-static void _rtl_reg_apply_active_scan_flags(struct wiphy *wiphy,
-					     enum nl80211_reg_initiator
-					     initiator)
-{
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *ch;
-	const struct ieee80211_reg_rule *reg_rule;
-
-	if (!wiphy->bands[IEEE80211_BAND_2GHZ])
-		return;
-	sband = wiphy->bands[IEEE80211_BAND_2GHZ];
-
-	/*
-	 *If no country IE has been received always enable active scan
-	 *on these channels. This is only done for specific regulatory SKUs
-	 */
-	if (initiator != NL80211_REGDOM_SET_BY_COUNTRY_IE) {
-		ch = &sband->channels[11];	/* CH 12 */
-		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-		ch = &sband->channels[12];	/* CH 13 */
-		if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-			ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-		return;
-	}
-
-	/*
-	 *If a country IE has been received check its rule for this
-	 *channel first before enabling active scan. The passive scan
-	 *would have been enforced by the initial processing of our
-	 *custom regulatory domain.
-	 */
-
-	ch = &sband->channels[11];	/* CH 12 */
-	reg_rule = freq_reg_info(wiphy, ch->center_freq);
-	if (!IS_ERR(reg_rule)) {
-		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
-			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-	}
-
-	ch = &sband->channels[12];	/* CH 13 */
-	reg_rule = freq_reg_info(wiphy, ch->center_freq);
-	if (!IS_ERR(reg_rule)) {
-		if (!(reg_rule->flags & NL80211_RRF_PASSIVE_SCAN))
-			if (ch->flags & IEEE80211_CHAN_PASSIVE_SCAN)
-				ch->flags &= ~IEEE80211_CHAN_PASSIVE_SCAN;
-	}
-}
-
-/*
- *Always apply Radar/DFS rules on
- *freq range 5260 MHz - 5700 MHz
- */
-static void _rtl_reg_apply_radar_flags(struct wiphy *wiphy)
-{
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *ch;
-	unsigned int i;
-
-	if (!wiphy->bands[IEEE80211_BAND_5GHZ])
-		return;
-
-	sband = wiphy->bands[IEEE80211_BAND_5GHZ];
-
-	for (i = 0; i < sband->n_channels; i++) {
-		ch = &sband->channels[i];
-		if (!_rtl_is_radar_freq(ch->center_freq))
-			continue;
-
-		/*
-		 *We always enable radar detection/DFS on this
-		 *frequency range. Additionally we also apply on
-		 *this frequency range:
-		 *- If STA mode does not yet have DFS supports disable
-		 * active scanning
-		 *- If adhoc mode does not support DFS yet then disable
-		 * adhoc in the frequency.
-		 *- If AP mode does not yet support radar detection/DFS
-		 *do not allow AP mode
-		 */
-		if (!(ch->flags & IEEE80211_CHAN_DISABLED))
-			ch->flags |= IEEE80211_CHAN_RADAR |
-			    IEEE80211_CHAN_NO_IBSS |
-			    IEEE80211_CHAN_PASSIVE_SCAN;
-	}
-}
-
-static void _rtl_reg_apply_world_flags(struct wiphy *wiphy,
-				       enum nl80211_reg_initiator initiator,
-				       struct rtl_regulatory *reg)
-{
-	_rtl_reg_apply_beaconing_flags(wiphy, initiator);
-	_rtl_reg_apply_active_scan_flags(wiphy, initiator);
-	return;
-}
-
-static void _rtl_dump_channel_map(struct wiphy *wiphy)
-{
-	enum ieee80211_band band;
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *ch;
-	unsigned int i;
-
-	for (band = 0; band < IEEE80211_NUM_BANDS; band++) {
-		if (!wiphy->bands[band])
-			continue;
-		sband = wiphy->bands[band];
-		for (i = 0; i < sband->n_channels; i++)
-			ch = &sband->channels[i];
-	}
-}
-
-static int _rtl_reg_notifier_apply(struct wiphy *wiphy,
-				   struct regulatory_request *request,
-				   struct rtl_regulatory *reg)
-{
-	/* We always apply this */
-	_rtl_reg_apply_radar_flags(wiphy);
-
-	switch (request->initiator) {
-	case NL80211_REGDOM_SET_BY_DRIVER:
-	case NL80211_REGDOM_SET_BY_CORE:
-	case NL80211_REGDOM_SET_BY_USER:
-		break;
-	case NL80211_REGDOM_SET_BY_COUNTRY_IE:
-		_rtl_reg_apply_world_flags(wiphy, request->initiator, reg);
-		break;
-	}
-
-	_rtl_dump_channel_map(wiphy);
-
-	return 0;
-}
-
-static const struct ieee80211_regdomain *_rtl_regdomain_select(
-						struct rtl_regulatory *reg)
-{
-	switch (reg->country_code) {
-	case COUNTRY_CODE_FCC:
-		return &rtl_regdom_no_midband;
-	case COUNTRY_CODE_IC:
-		return &rtl_regdom_11;
-	case COUNTRY_CODE_ETSI:
-	case COUNTRY_CODE_TELEC_NETGEAR:
-		return &rtl_regdom_60_64;
-	case COUNTRY_CODE_SPAIN:
-	case COUNTRY_CODE_FRANCE:
-	case COUNTRY_CODE_ISRAEL:
-	case COUNTRY_CODE_WORLD_WIDE_13:
-		return &rtl_regdom_12_13;
-	case COUNTRY_CODE_MKK:
-	case COUNTRY_CODE_MKK1:
-	case COUNTRY_CODE_TELEC:
-	case COUNTRY_CODE_MIC:
-		return &rtl_regdom_14_60_64;
-	case COUNTRY_CODE_GLOBAL_DOMAIN:
-		return &rtl_regdom_14;
-	default:
-		return &rtl_regdom_no_midband;
-	}
-}
-
-static int _rtl_regd_init_wiphy(struct rtl_regulatory *reg,
-				struct wiphy *wiphy,
-				void (*reg_notifier) (struct wiphy * wiphy,
-						     struct regulatory_request *
-						     request))
-{
-	const struct ieee80211_regdomain *regd;
-
-	wiphy->reg_notifier = reg_notifier;
-
-	wiphy->flags |= WIPHY_FLAG_CUSTOM_REGULATORY;
-	wiphy->flags &= ~WIPHY_FLAG_STRICT_REGULATORY;
-	wiphy->flags &= ~WIPHY_FLAG_DISABLE_BEACON_HINTS;
-
-	regd = _rtl_regdomain_select(reg);
-	wiphy_apply_custom_regulatory(wiphy, regd);
-	_rtl_reg_apply_radar_flags(wiphy);
-	_rtl_reg_apply_world_flags(wiphy, NL80211_REGDOM_SET_BY_DRIVER, reg);
-	return 0;
-}
-
-static struct country_code_to_enum_rd *_rtl_regd_find_country(u16 countrycode)
-{
-	int i;
-
-	for (i = 0; i < ARRAY_SIZE(allCountries); i++) {
-		if (allCountries[i].countrycode == countrycode)
-			return &allCountries[i];
-	}
-	return NULL;
-}
-
-int rtl_regd_init(struct ieee80211_hw *hw,
-		  void (*reg_notifier) (struct wiphy *wiphy,
-				        struct regulatory_request *request))
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct wiphy *wiphy = hw->wiphy;
-	struct country_code_to_enum_rd *country = NULL;
-
-	if (wiphy == NULL || &rtlpriv->regd == NULL)
-		return -EINVAL;
-
-	/* init country_code from efuse channel plan */
-	rtlpriv->regd.country_code = rtlpriv->efuse.channel_plan;
-
-	RT_TRACE(COMP_REGD, DBG_TRACE,
-		 (KERN_DEBUG "rtl: EEPROM regdomain: 0x%0x\n",
-		  rtlpriv->regd.country_code));
-
-	if (rtlpriv->regd.country_code >= COUNTRY_CODE_MAX) {
-		RT_TRACE(COMP_REGD, DBG_DMESG,
-			 (KERN_DEBUG "rtl: EEPROM indicates invalid country code"
-			  "world wide 13 should be used\n"));
-
-		rtlpriv->regd.country_code = COUNTRY_CODE_WORLD_WIDE_13;
-	}
-
-	country = _rtl_regd_find_country(rtlpriv->regd.country_code);
-
-	if (country) {
-		rtlpriv->regd.alpha2[0] = country->iso_name[0];
-		rtlpriv->regd.alpha2[1] = country->iso_name[1];
-	} else {
-		rtlpriv->regd.alpha2[0] = '0';
-		rtlpriv->regd.alpha2[1] = '0';
-	}
-
-	RT_TRACE(COMP_REGD, DBG_TRACE,
-		 (KERN_DEBUG "rtl: Country alpha2 being used: %c%c\n",
-		  rtlpriv->regd.alpha2[0], rtlpriv->regd.alpha2[1]));
-
-	_rtl_regd_init_wiphy(&rtlpriv->regd, wiphy, reg_notifier);
-
-	return 0;
-}
-
-void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request)
-{
-	struct ieee80211_hw *hw = wiphy_to_ieee80211_hw(wiphy);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_REGD, DBG_LOUD, ("\n"));
-
-	_rtl_reg_notifier_apply(wiphy, request, &rtlpriv->regd);
-}
diff --git a/drivers/staging/rtl8821ae/regd.h b/drivers/staging/rtl8821ae/regd.h
deleted file mode 100644
index 1cfcb97b0ab1..000000000000
--- a/drivers/staging/rtl8821ae/regd.h
+++ /dev/null
@@ -1,67 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_REGD_H__
-#define __RTL_REGD_H__
-
-#define IEEE80211_CHAN_NO_IBSS		(1 << 2)
-#define IEEE80211_CHAN_PASSIVE_SCAN	(1 << 1)
-#define WIPHY_FLAG_CUSTOM_REGULATORY	BIT(0)
-#define WIPHY_FLAG_STRICT_REGULATORY	BIT(1)
-#define WIPHY_FLAG_DISABLE_BEACON_HINTS	BIT(2)
-
-struct country_code_to_enum_rd {
-	u16 countrycode;
-	const char *iso_name;
-};
-
-enum country_code_type_t {
-	COUNTRY_CODE_FCC = 0,
-	COUNTRY_CODE_IC = 1,
-	COUNTRY_CODE_ETSI = 2,
-	COUNTRY_CODE_SPAIN = 3,
-	COUNTRY_CODE_FRANCE = 4,
-	COUNTRY_CODE_MKK = 5,
-	COUNTRY_CODE_MKK1 = 6,
-	COUNTRY_CODE_ISRAEL = 7,
-	COUNTRY_CODE_TELEC = 8,
-	COUNTRY_CODE_MIC = 9,
-	COUNTRY_CODE_GLOBAL_DOMAIN = 10,
-	COUNTRY_CODE_WORLD_WIDE_13 = 11,
-	COUNTRY_CODE_TELEC_NETGEAR = 12,
-
-	/*add new channel plan above this line */
-	COUNTRY_CODE_MAX
-};
-
-int rtl_regd_init(struct ieee80211_hw *hw,
-		  void (*reg_notifier) (struct wiphy *wiphy,
-				        struct regulatory_request *request));
-void rtl_reg_notifier(struct wiphy *wiphy, struct regulatory_request *request);
-#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/btc.h b/drivers/staging/rtl8821ae/rtl8821ae/btc.h
deleted file mode 100644
index 74ac189e3a88..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/btc.h
+++ /dev/null
@@ -1,87 +0,0 @@
-
-/******************************************************************************
- **
- ** Copyright(c) 2009-2010  Realtek Corporation.
- **
- ** This program is free software; you can redistribute it and/or modify it
- ** under the terms of version 2 of the GNU General Public License as
- ** published by the Free Software Foundation.
- **
- ** This program is distributed in the hope that it will be useful, but WITHOUT
- ** ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- ** FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- ** more details.
- **
- ** You should have received a copy of the GNU General Public License along with
- ** this program; if not, write to the Free Software Foundation, Inc.,
- ** 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- **
- ** The full GNU General Public License is included in this distribution in the
- ** file called LICENSE.
- **
- ** Contact Information:
- ** wlanfae <wlanfae@realtek.com>
- ** Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- ** Hsinchu 300, Taiwan.
- ** Larry Finger <Larry.Finger@lwfinger.net>
- **
- ******************************************************************************/
-
-#ifndef __RTL8821AE_BTC_H__
-#define __RTL8821AE_BTC_H__
-
-#include "../wifi.h"
-#include "hal_bt_coexist.h"
-
-struct bt_coexist_c2h_info {
-	u8 no_parse_c2h;
-	u8 has_c2h;
-};
-
-struct btdm_8821ae {
-	bool b_all_off;
-	bool b_agc_table_en;
-	bool b_adc_back_off_on;
-	bool b2_ant_hid_en;
-	bool b_low_penalty_rate_adaptive;
-	bool b_rf_rx_lpf_shrink;
-	bool b_reject_aggre_pkt;
-	bool b_tra_tdma_on;
-	u8 tra_tdma_nav;
-	u8 tra_tdma_ant;
-	bool b_tdma_on;
-	u8 tdma_ant;
-	u8 tdma_nav;
-	u8 tdma_dac_swing;
-	u8 fw_dac_swing_lvl;
-	bool b_ps_tdma_on;
-	u8 ps_tdma_byte[5];
-	bool b_pta_on;
-	u32 val_0x6c0;
-	u32 val_0x6c8;
-	u32 val_0x6cc;
-	bool b_sw_dac_swing_on;
-	u32 sw_dac_swing_lvl;
-	u32 wlan_act_hi;
-	u32 wlan_act_lo;
-	u32 bt_retry_index;
-	bool b_dec_bt_pwr;
-	bool b_ignore_wlan_act;
-};
-
-struct bt_coexist_8821ae {
-	u32 high_priority_tx;
-	u32 high_priority_rx;
-	u32 low_priority_tx;
-	u32 low_priority_rx;
-	u8 c2h_bt_info;
-	bool b_c2h_bt_info_req_sent;
-	bool b_c2h_bt_inquiry_page;
-	u32 bt_inq_page_start_time;
-	u8 bt_retry_cnt;
-	u8 c2h_bt_info_original;
-	u8 bt_inquiry_page_cnt;
-	struct btdm_8821ae btdm;
-};
-
-#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/dm.c b/drivers/staging/rtl8821ae/rtl8821ae/dm.c
deleted file mode 100644
index e0efcd281dfe..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/dm.c
+++ /dev/null
@@ -1,3045 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "../wifi.h"
-#include "../base.h"
-#include "../pci.h"
-#include "reg.h"
-#include "def.h"
-#include "phy.h"
-#include "dm.h"
-#include "fw.h"
-#include "trx.h"
-#include "../btcoexist/rtl_btc.h"
-
-struct dig_t dm_digtable;
-static struct ps_t dm_pstable;
-
-static const u32 rtl8812ae_txscaling_table[TXSCALE_TABLE_SIZE] =
-{
-	0x081, // 0,  -12.0dB
-	0x088, // 1,  -11.5dB
-	0x090, // 2,  -11.0dB
-	0x099, // 3,  -10.5dB
-	0x0A2, // 4,  -10.0dB
-	0x0AC, // 5,  -9.5dB
-	0x0B6, // 6,  -9.0dB
-	0x0C0, // 7,  -8.5dB
-	0x0CC, // 8,  -8.0dB
-	0x0D8, // 9,  -7.5dB
-	0x0E5, // 10, -7.0dB
-	0x0F2, // 11, -6.5dB
-	0x101, // 12, -6.0dB
-	0x110, // 13, -5.5dB
-	0x120, // 14, -5.0dB
-	0x131, // 15, -4.5dB
-	0x143, // 16, -4.0dB
-	0x156, // 17, -3.5dB
-	0x16A, // 18, -3.0dB
-	0x180, // 19, -2.5dB
-	0x197, // 20, -2.0dB
-	0x1AF, // 21, -1.5dB
-	0x1C8, // 22, -1.0dB
-	0x1E3, // 23, -0.5dB
-	0x200, // 24, +0  dB
-	0x21E, // 25, +0.5dB
-	0x23E, // 26, +1.0dB
-	0x261, // 27, +1.5dB
-	0x285, // 28, +2.0dB
-	0x2AB, // 29, +2.5dB
-	0x2D3, // 30, +3.0dB
-	0x2FE, // 31, +3.5dB
-	0x32B, // 32, +4.0dB
-	0x35C, // 33, +4.5dB
-	0x38E, // 34, +5.0dB
-	0x3C4, // 35, +5.5dB
-	0x3FE  // 36, +6.0dB
-};
-
-static const u32 rtl8821ae_txscaling_table[TXSCALE_TABLE_SIZE] = {
-	0x081, // 0,  -12.0dB
-	0x088, // 1,  -11.5dB
-	0x090, // 2,  -11.0dB
-	0x099, // 3,  -10.5dB
-	0x0A2, // 4,  -10.0dB
-	0x0AC, // 5,  -9.5dB
-	0x0B6, // 6,  -9.0dB
-	0x0C0, // 7,  -8.5dB
-	0x0CC, // 8,  -8.0dB
-	0x0D8, // 9,  -7.5dB
-	0x0E5, // 10, -7.0dB
-	0x0F2, // 11, -6.5dB
-	0x101, // 12, -6.0dB
-	0x110, // 13, -5.5dB
-	0x120, // 14, -5.0dB
-	0x131, // 15, -4.5dB
-	0x143, // 16, -4.0dB
-	0x156, // 17, -3.5dB
-	0x16A, // 18, -3.0dB
-	0x180, // 19, -2.5dB
-	0x197, // 20, -2.0dB
-	0x1AF, // 21, -1.5dB
-	0x1C8, // 22, -1.0dB
-	0x1E3, // 23, -0.5dB
-	0x200, // 24, +0  dB
-	0x21E, // 25, +0.5dB
-	0x23E, // 26, +1.0dB
-	0x261, // 27, +1.5dB
-	0x285, // 28, +2.0dB
-	0x2AB, // 29, +2.5dB
-	0x2D3, // 30, +3.0dB
-	0x2FE, // 31, +3.5dB
-	0x32B, // 32, +4.0dB
-	0x35C, // 33, +4.5dB
-	0x38E, // 34, +5.0dB
-	0x3C4, // 35, +5.5dB
-	0x3FE  // 36, +6.0dB
-};
-
-static const u32 ofdmswing_table[] = {
-	0x0b40002d, // 0,  -15.0dB
-	0x0c000030, // 1,  -14.5dB
-	0x0cc00033, // 2,  -14.0dB
-	0x0d800036, // 3,  -13.5dB
-	0x0e400039, // 4,  -13.0dB
-	0x0f00003c, // 5,  -12.5dB
-	0x10000040, // 6,  -12.0dB
-	0x11000044, // 7,  -11.5dB
-	0x12000048, // 8,  -11.0dB
-	0x1300004c, // 9,  -10.5dB
-	0x14400051, // 10, -10.0dB
-	0x15800056, // 11, -9.5dB
-	0x16c0005b, // 12, -9.0dB
-	0x18000060, // 13, -8.5dB
-	0x19800066, // 14, -8.0dB
-	0x1b00006c, // 15, -7.5dB
-	0x1c800072, // 16, -7.0dB
-	0x1e400079, // 17, -6.5dB
-	0x20000080, // 18, -6.0dB
-	0x22000088, // 19, -5.5dB
-	0x24000090, // 20, -5.0dB
-	0x26000098, // 21, -4.5dB
-	0x288000a2, // 22, -4.0dB
-	0x2ac000ab, // 23, -3.5dB
-	0x2d4000b5, // 24, -3.0dB
-	0x300000c0, // 25, -2.5dB
-	0x32c000cb, // 26, -2.0dB
-	0x35c000d7, // 27, -1.5dB
-	0x390000e4, // 28, -1.0dB
-	0x3c8000f2, // 29, -0.5dB
-	0x40000100, // 30, +0dB
-	0x43c0010f, // 31, +0.5dB
-	0x47c0011f, // 32, +1.0dB
-	0x4c000130, // 33, +1.5dB
-	0x50800142, // 34, +2.0dB
-	0x55400155, // 35, +2.5dB
-	0x5a400169, // 36, +3.0dB
-	0x5fc0017f, // 37, +3.5dB
-	0x65400195, // 38, +4.0dB
-	0x6b8001ae, // 39, +4.5dB
-	0x71c001c7, // 40, +5.0dB
-	0x788001e2, // 41, +5.5dB
-	0x7f8001fe  // 42, +6.0dB
-};
-
-static const u8 cckswing_table_ch1ch13[CCK_TABLE_SIZE][8] = {
-	{0x09, 0x08, 0x07, 0x06, 0x04, 0x03, 0x01, 0x01},	//  0, -16.0dB
-	{0x09, 0x09, 0x08, 0x06, 0x05, 0x03, 0x01, 0x01},	//  1, -15.5dB
-	{0x0a, 0x09, 0x08, 0x07, 0x05, 0x03, 0x02, 0x01},	//  2, -15.0dB
-	{0x0a, 0x0a, 0x09, 0x07, 0x05, 0x03, 0x02, 0x01},	//  3, -14.5dB
-	{0x0b, 0x0a, 0x09, 0x08, 0x06, 0x04, 0x02, 0x01},	//  4, -14.0dB
-	{0x0b, 0x0b, 0x0a, 0x08, 0x06, 0x04, 0x02, 0x01},	//  5, -13.5dB
-	{0x0c, 0x0c, 0x0a, 0x09, 0x06, 0x04, 0x02, 0x01},	//  6, -13.0dB
-	{0x0d, 0x0c, 0x0b, 0x09, 0x07, 0x04, 0x02, 0x01},	//  7, -12.5dB
-	{0x0d, 0x0d, 0x0c, 0x0a, 0x07, 0x05, 0x02, 0x01},	//  8, -12.0dB
-	{0x0e, 0x0e, 0x0c, 0x0a, 0x08, 0x05, 0x02, 0x01},	//  9, -11.5dB
-	{0x0f, 0x0f, 0x0d, 0x0b, 0x08, 0x05, 0x03, 0x01},	// 10, -11.0dB
-	{0x10, 0x10, 0x0e, 0x0b, 0x08, 0x05, 0x03, 0x01},	// 11, -10.5dB
-	{0x11, 0x11, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},	// 12, -10.0dB
-	{0x12, 0x12, 0x0f, 0x0c, 0x09, 0x06, 0x03, 0x01},	// 13, -9.5dB
-	{0x13, 0x13, 0x10, 0x0d, 0x0a, 0x06, 0x03, 0x01},	// 14, -9.0dB
-	{0x14, 0x14, 0x11, 0x0e, 0x0b, 0x07, 0x03, 0x02},	// 15, -8.5dB
-	{0x16, 0x15, 0x12, 0x0f, 0x0b, 0x07, 0x04, 0x01},	// 16, -8.0dB
-	{0x17, 0x16, 0x13, 0x10, 0x0c, 0x08, 0x04, 0x02},	// 17, -7.5dB
-	{0x18, 0x17, 0x15, 0x11, 0x0c, 0x08, 0x04, 0x02},	// 18, -7.0dB
-	{0x1a, 0x19, 0x16, 0x12, 0x0d, 0x09, 0x04, 0x02},	// 19, -6.5dB
-    {0x1b, 0x1a, 0x17, 0x13, 0x0e, 0x09, 0x04, 0x02},	// 20, -6.0dB
-	{0x1d, 0x1c, 0x18, 0x14, 0x0f, 0x0a, 0x05, 0x02},	// 21, -5.5dB
-	{0x1f, 0x1e, 0x1a, 0x15, 0x10, 0x0a, 0x05, 0x02},	// 22, -5.0dB
-	{0x20, 0x20, 0x1b, 0x16, 0x11, 0x08, 0x05, 0x02},	// 23, -4.5dB
-	{0x22, 0x21, 0x1d, 0x18, 0x11, 0x0b, 0x06, 0x02},	// 24, -4.0dB
-	{0x24, 0x23, 0x1f, 0x19, 0x13, 0x0c, 0x06, 0x03},	// 25, -3.5dB
-	{0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03},	// 26, -3.0dB
-	{0x28, 0x28, 0x22, 0x1c, 0x15, 0x0d, 0x07, 0x03},	// 27, -2.5dB
-	{0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03},	// 28, -2.0dB
-	{0x2d, 0x2d, 0x27, 0x1f, 0x18, 0x0f, 0x08, 0x03},	// 29, -1.5dB
-	{0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03},	// 30, -1.0dB
-	{0x33, 0x32, 0x2b, 0x23, 0x1a, 0x11, 0x08, 0x04},	// 31, -0.5dB
-	{0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04} 	// 32, +0dB
-};
-
-static const u8 cckswing_table_ch14[CCK_TABLE_SIZE][8]= {
-	{0x09, 0x08, 0x07, 0x04, 0x00, 0x00, 0x00, 0x00},	//  0, -16.0dB
-	{0x09, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},	//  1, -15.5dB
-	{0x0a, 0x09, 0x08, 0x05, 0x00, 0x00, 0x00, 0x00},	//  2, -15.0dB
-	{0x0a, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},	//  3, -14.5dB
-	{0x0b, 0x0a, 0x09, 0x05, 0x00, 0x00, 0x00, 0x00},	//  4, -14.0dB
-	{0x0b, 0x0b, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},	//  5, -13.5dB
-	{0x0c, 0x0c, 0x0a, 0x06, 0x00, 0x00, 0x00, 0x00},	//  6, -13.0dB
-	{0x0d, 0x0c, 0x0b, 0x06, 0x00, 0x00, 0x00, 0x00},	//  7, -12.5dB
-	{0x0d, 0x0d, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},	//  8, -12.0dB
-	{0x0e, 0x0e, 0x0c, 0x07, 0x00, 0x00, 0x00, 0x00},	//  9, -11.5dB
-	{0x0f, 0x0f, 0x0d, 0x08, 0x00, 0x00, 0x00, 0x00},	// 10, -11.0dB
-	{0x10, 0x10, 0x0e, 0x08, 0x00, 0x00, 0x00, 0x00},	// 11, -10.5dB
-	{0x11, 0x11, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},	// 12, -10.0dB
-	{0x12, 0x12, 0x0f, 0x09, 0x00, 0x00, 0x00, 0x00},	// 13, -9.5dB
-	{0x13, 0x13, 0x10, 0x0a, 0x00, 0x00, 0x00, 0x00},	// 14, -9.0dB
-	{0x14, 0x14, 0x11, 0x0a, 0x00, 0x00, 0x00, 0x00},	// 15, -8.5dB
-	{0x16, 0x15, 0x12, 0x0b, 0x00, 0x00, 0x00, 0x00},	// 16, -8.0dB
-	{0x17, 0x16, 0x13, 0x0b, 0x00, 0x00, 0x00, 0x00},	// 17, -7.5dB
-	{0x18, 0x17, 0x15, 0x0c, 0x00, 0x00, 0x00, 0x00},	// 18, -7.0dB
-	{0x1a, 0x19, 0x16, 0x0d, 0x00, 0x00, 0x00, 0x00},	// 19, -6.5dB
-	{0x1b, 0x1a, 0x17, 0x0e, 0x00, 0x00, 0x00, 0x00},	// 20, -6.0dB
-	{0x1d, 0x1c, 0x18, 0x0e, 0x00, 0x00, 0x00, 0x00},	// 21, -5.5dB
-	{0x1f, 0x1e, 0x1a, 0x0f, 0x00, 0x00, 0x00, 0x00},	// 22, -5.0dB
-	{0x20, 0x20, 0x1b, 0x10, 0x00, 0x00, 0x00, 0x00},	// 23, -4.5dB
-	{0x22, 0x21, 0x1d, 0x11, 0x00, 0x00, 0x00, 0x00},	// 24, -4.0dB
-	{0x24, 0x23, 0x1f, 0x12, 0x00, 0x00, 0x00, 0x00},	// 25, -3.5dB
-	{0x26, 0x25, 0x21, 0x13, 0x00, 0x00, 0x00, 0x00},	// 26, -3.0dB
-	{0x28, 0x28, 0x24, 0x14, 0x00, 0x00, 0x00, 0x00},	// 27, -2.5dB
-	{0x2b, 0x2a, 0x25, 0x15, 0x00, 0x00, 0x00, 0x00},	// 28, -2.0dB
-	{0x2d, 0x2d, 0x17, 0x17, 0x00, 0x00, 0x00, 0x00},	// 29, -1.5dB
-	{0x30, 0x2f, 0x29, 0x18, 0x00, 0x00, 0x00, 0x00},	// 30, -1.0dB
-	{0x33, 0x32, 0x2b, 0x19, 0x00, 0x00, 0x00, 0x00},	// 31, -0.5dB
-	{0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00} 	// 32, +0dB
-};
-
-static const u32 edca_setting_dl[PEER_MAX] = {
-	0xa44f, 	/* 0 UNKNOWN */
- 	0x5ea44f,	/* 1 REALTEK_90 */
-	0x5e4322,	/* 2 REALTEK_92SE */
-	0x5ea42b,		/* 3 BROAD	*/
-	0xa44f,		/* 4 RAL */
-	0xa630,		/* 5 ATH */
-	0x5ea630,		/* 6 CISCO */
-	0x5ea42b,		/* 7 MARVELL */
-};
-
-static const u32 edca_setting_ul[PEER_MAX] = {
-	0x5e4322, 	/* 0 UNKNOWN */
-	0xa44f,		/* 1 REALTEK_90 */
-	0x5ea44f,	/* 2 REALTEK_92SE */
-	0x5ea32b, 	/* 3 BROAD */
-	0x5ea422,	/* 4 RAL */
-	0x5ea322, 	/* 5 ATH */
-	0x3ea430,	/* 6 CISCO */
-	0x5ea44f,	/* 7 MARV */
-};
-
-static u8 rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack[] =
-	{0, 0, 0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 4,  4,  4,  4,  4,  4,  5,  5,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9};
-static u8 rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack[] =
-	{0, 0, 0, 2, 2, 3, 3, 4, 4, 4, 4, 5, 5,  6,  6,  7,  7,  7,  7,  8,  8,  9,  9, 10, 10, 10, 11, 11, 11, 11};
-
-
-u8 rtl8812ae_delta_swing_table_idx_24gb_n_txpwrtrack[]    =
-	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  9,  9,  9,  9, 10, 10, 10, 10, 11, 11};
-u8 rtl8812ae_delta_swing_table_idx_24gb_p_txpwrtrack[]    =
-	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
-u8 rtl8812ae_delta_swing_table_idx_24ga_n_txpwrtrack[]    =
-	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  8,  9,  9,  9, 10, 10, 10, 10, 11, 11};
-u8 rtl8812ae_delta_swing_table_idx_24ga_p_txpwrtrack[]    =
-	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
-u8 rtl8812ae_delta_swing_table_idx_24gcckb_n_txpwrtrack[] =
-	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  9,  9,  9,  9, 10, 10, 10, 10, 11, 11};
-u8 rtl8812ae_delta_swing_table_idx_24gcckb_p_txpwrtrack[] =
-	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
-u8 rtl8812ae_delta_swing_table_idx_24gccka_n_txpwrtrack[] =
-	{0, 1, 1, 1, 2, 2, 2, 3, 3,  3,  4,  4,  5,  5,  5,  6,  6,  6,  7,  8,  8,  9,  9,  9, 10, 10, 10, 10, 11, 11};
-u8 rtl8812ae_delta_swing_table_idx_24gccka_p_txpwrtrack[] =
-	{0, 0, 1, 1, 2, 2, 2, 2, 3,  3,  3,  4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  8,  9,  9,  9,  9,  9,  9};
-
-u8 rtl8812ae_delta_swing_table_idx_5gb_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 1, 1, 2, 2, 2, 3, 3, 4,  4,  5,  5,  6,  6,  6,  7,  7,  7,  8,  8,  9,  9,  9, 10, 10, 11, 11, 12, 12, 13},
-	{0, 1, 1, 2, 2, 2, 3, 3, 4,  4,  4,  5,  5,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 12, 12, 12, 13, 13},
-	{0, 1, 1, 2, 3, 3, 4, 4, 5,  6,  6,  7,  8,  9, 10, 11, 12, 12, 13, 14, 14, 14, 15, 16, 17, 17, 17, 18, 18, 18},
-};
-u8 rtl8812ae_delta_swing_table_idx_5gb_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 1, 1, 2, 2, 3, 3, 4, 4,  5,  5,  6,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
-	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  5,  5,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
-	{0, 1, 1, 2, 2, 3, 3, 4, 4,  5,  5,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
-};
-u8 rtl8812ae_delta_swing_table_idx_5ga_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  5,  5,  6,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 12, 12, 12, 13, 13, 13},
-	{0, 1, 1, 2, 2, 2, 3, 3, 4,  4,  5,  5,  6,  6,  7,  8,  9,  9, 10, 10, 11, 11, 11, 12, 12, 12, 12, 12, 13, 13},
-	{0, 1, 1, 2, 2, 3, 3, 4, 5,  6,  7,  8,  8,  9, 10, 11, 12, 13, 14, 14, 15, 15, 15, 16, 16, 16, 17, 17, 18, 18},
-};
-u8 rtl8812ae_delta_swing_table_idx_5ga_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  4,  5,  5,  6,  7,  7,  8,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11},
-	{0, 1, 1, 2, 2, 3, 3, 4, 4,  4,  5,  5,  6,  6,  7,  7,  8,  9,  9, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
-	{0, 1, 1, 2, 3, 3, 4, 4, 5,  6,  6,  7,  7,  8,  9,  9, 10, 10, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11, 11},
-};
-
-u8 rtl8821ae_delta_swing_table_idx_24gb_n_txpwrtrack[] =
-	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
-u8 rtl8821ae_delta_swing_table_idx_24gb_p_txpwrtrack[]  =
-	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
-u8 rtl8821ae_delta_swing_table_idx_24ga_n_txpwrtrack[]  =
-	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
-u8 rtl8821ae_delta_swing_table_idx_24ga_p_txpwrtrack[] =
-	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
-u8 rtl8821ae_delta_swing_table_idx_24gcckb_n_txpwrtrack[] =
-	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
-u8 rtl8821ae_delta_swing_table_idx_24gcckb_p_txpwrtrack[] =
-	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
-u8 rtl8821ae_delta_swing_table_idx_24gccka_n_txpwrtrack[] =
-	{0, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5, 5, 6, 6, 6, 7, 7, 7, 8, 8, 8, 9, 9, 9, 10, 10, 10};
-u8 rtl8821ae_delta_swing_table_idx_24gccka_p_txpwrtrack[] =
-	{0, 0, 1, 1, 2, 2, 3, 3, 4, 4, 5, 5, 6, 6, 7, 7, 8, 8, 9, 9, 10, 10, 11, 11, 12, 12, 12, 12, 12, 12};
-
-u8 rtl8821ae_delta_swing_table_idx_5gb_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-};
-
-u8 rtl8821ae_delta_swing_table_idx_5gb_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-};
-
-u8 rtl8821ae_delta_swing_table_idx_5ga_n_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-};
-
-u8 rtl8821ae_delta_swing_table_idx_5ga_p_txpwrtrack[][DELTA_SWINGIDX_SIZE] = {
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-	{0, 0, 1, 2, 3, 3, 4, 5, 6, 6, 7, 8, 9, 9, 10, 11, 12, 12, 13, 14, 15, 15, 16, 16, 16, 16, 16, 16, 16, 16},
-};
-
-void rtl8812ae_dm_read_and_config_txpower_track(
- 	struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("===> rtl8821ae_dm_read_and_config_txpower_track\n"));
-
-
-	memcpy(rtldm->delta_swing_table_idx_24ga_p,
-		rtl8812ae_delta_swing_table_idx_24ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24ga_n,
-		rtl8812ae_delta_swing_table_idx_24ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gb_p,
-		rtl8812ae_delta_swing_table_idx_24gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gb_n,
-		rtl8812ae_delta_swing_table_idx_24gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-
-	memcpy(rtldm->delta_swing_table_idx_24gccka_p,
-		rtl8812ae_delta_swing_table_idx_24gccka_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gccka_n,
-		rtl8812ae_delta_swing_table_idx_24gccka_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gcckb_p,
-		rtl8812ae_delta_swing_table_idx_24gcckb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gcckb_n,
-		rtl8812ae_delta_swing_table_idx_24gcckb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-
-	memcpy(rtldm->delta_swing_table_idx_5ga_p,
-		rtl8812ae_delta_swing_table_idx_5ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-	memcpy(rtldm->delta_swing_table_idx_5ga_n,
-		rtl8812ae_delta_swing_table_idx_5ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-	memcpy(rtldm->delta_swing_table_idx_5gb_p,
-		rtl8812ae_delta_swing_table_idx_5gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-	memcpy(rtldm->delta_swing_table_idx_5gb_n,
-		rtl8812ae_delta_swing_table_idx_5gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-}
-
-void rtl8821ae_dm_read_and_config_txpower_track(
- 	struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("===> rtl8821ae_dm_read_and_config_txpower_track\n"));
-
-
-	memcpy(rtldm->delta_swing_table_idx_24ga_p,
-		rtl8821ae_delta_swing_table_idx_24ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24ga_n,
-		rtl8821ae_delta_swing_table_idx_24ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gb_p,
-		rtl8821ae_delta_swing_table_idx_24gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gb_n,
-		rtl8821ae_delta_swing_table_idx_24gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-
-	memcpy(rtldm->delta_swing_table_idx_24gccka_p,
-		rtl8821ae_delta_swing_table_idx_24gccka_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gccka_n,
-		rtl8821ae_delta_swing_table_idx_24gccka_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gcckb_p,
-		rtl8821ae_delta_swing_table_idx_24gcckb_p_txpwrtrack, DELTA_SWINGIDX_SIZE);
-	memcpy(rtldm->delta_swing_table_idx_24gcckb_n,
-		rtl8821ae_delta_swing_table_idx_24gcckb_n_txpwrtrack, DELTA_SWINGIDX_SIZE);
-
-	memcpy(rtldm->delta_swing_table_idx_5ga_p,
-		rtl8821ae_delta_swing_table_idx_5ga_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-	memcpy(rtldm->delta_swing_table_idx_5ga_n,
-		rtl8821ae_delta_swing_table_idx_5ga_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-	memcpy(rtldm->delta_swing_table_idx_5gb_p,
-		rtl8821ae_delta_swing_table_idx_5gb_p_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-	memcpy(rtldm->delta_swing_table_idx_5gb_n,
-		rtl8821ae_delta_swing_table_idx_5gb_n_txpwrtrack, DELTA_SWINGIDX_SIZE*3);
-}
-
-
-
-#define 	CALCULATE_SWINGTALBE_OFFSET(_offset, _direction, _size, _deltaThermal) \
-					do {\
-						for(_offset = 0; _offset < _size; _offset++)\
-						{\
-							if(_deltaThermal < thermal_threshold[_direction][_offset])\
-							{\
-								if(_offset != 0)\
-									_offset--;\
-								break;\
-							}\
-						}			\
-						if(_offset >= _size)\
-							_offset = _size-1;\
-					} while(0)
-
-
-void rtl8821ae_dm_txpower_track_adjust(struct ieee80211_hw *hw,
-												   u8 type,u8 *pdirection,
-												   u32 *poutwrite_val)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	u8 pwr_val = 0;
-
-	if (type == 0){
-		if (rtlpriv->dm.bb_swing_idx_ofdm[RF90_PATH_A] <=
-			rtlpriv->dm.bb_swing_idx_ofdm_base[RF90_PATH_A]) {
-			*pdirection = 1;
-			pwr_val = rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A] - rtldm->bb_swing_idx_ofdm[RF90_PATH_A];
-		} else {
-			*pdirection = 2;
-			pwr_val = rtldm->bb_swing_idx_ofdm[RF90_PATH_A] - rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A];
-		}
-	} else if (type ==1) {
-		if (rtldm->bb_swing_idx_cck <= rtldm->bb_swing_idx_cck_base) {
-			*pdirection = 1;
-			pwr_val = rtldm->bb_swing_idx_cck_base - rtldm->bb_swing_idx_cck;
-		} else {
-			*pdirection = 2;
-			pwr_val = rtldm->bb_swing_idx_cck - rtldm->bb_swing_idx_cck_base;
-		}
-	}
-
-	if (pwr_val >= TXPWRTRACK_MAX_IDX && (*pdirection == 1))
-		pwr_val = TXPWRTRACK_MAX_IDX;
-
-	*poutwrite_val = pwr_val |(pwr_val << 8)|(pwr_val << 16) | (pwr_val << 24);
-}
-
-void rtl8821ae_dm_clear_txpower_tracking_state(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
-	u8 p = 0;
-	rtldm->bb_swing_idx_cck_base = rtldm->default_cck_index;
-	rtldm->bb_swing_idx_cck = rtldm->default_cck_index;
-	rtldm->cck_index = 0;
-
-	for (p = RF90_PATH_A; p < MAX_RF_PATH; ++p) {
-		rtldm->bb_swing_idx_ofdm_base[p] = rtldm->default_ofdm_index;
-		rtldm->bb_swing_idx_ofdm[p] = rtldm->default_ofdm_index;
-		rtldm->ofdm_index[p] = rtldm->default_ofdm_index;
-
-		rtldm->power_index_offset[p] = 0;
-		rtldm->delta_power_index[p] = 0;
-		rtldm->delta_power_index_last[p] = 0;
-
-		rtldm->aboslute_ofdm_swing_idx[p] = 0;    /*Initial Mix mode power tracking*/
-		rtldm->remnant_ofdm_swing_idx[p] = 0;
-	}
-
-	rtldm->modify_txagc_flag_path_a = false;       /*Initial at Modify Tx Scaling Mode*/
-	rtldm->modify_txagc_flag_path_b = false;       /*Initial at Modify Tx Scaling Mode*/
-	rtldm->remnant_cck_idx = 0;
-	rtldm->thermalvalue = rtlefuse->eeprom_thermalmeter;
-	rtldm->thermalvalue_iqk = rtlefuse->eeprom_thermalmeter;
-	rtldm->thermalvalue_lck = rtlefuse->eeprom_thermalmeter;
-}
-
-u8  rtl8821ae_dm_get_swing_index(struct ieee80211_hw *hw)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 i = 0;
-	u32  bb_swing;
-
-	bb_swing =rtl8821ae_phy_query_bb_reg(hw, rtlhal->current_bandtype, RF90_PATH_A);
-
-	for (i = 0; i < TXSCALE_TABLE_SIZE; ++i)
-		if ( bb_swing == rtl8821ae_txscaling_table[i])
-			break;
-
-	return i;
-}
-
-void rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(
-				struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtlpriv);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 default_swing_index  = 0;
-	u8 p = 0;
-
-	rtlpriv->dm.txpower_track_control = true;
-	rtldm->thermalvalue = rtlefuse->eeprom_thermalmeter;
-	rtldm->thermalvalue_iqk = rtlefuse->eeprom_thermalmeter;
-	rtldm->thermalvalue_lck = rtlefuse->eeprom_thermalmeter;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-		rtl8812ae_dm_read_and_config_txpower_track(hw);
-	else
-		rtl8821ae_dm_read_and_config_txpower_track(hw);
-
-	default_swing_index = rtl8821ae_dm_get_swing_index(hw);
-
-	rtldm->default_ofdm_index = (default_swing_index == TXSCALE_TABLE_SIZE) ? 24 : default_swing_index;
-	rtldm->default_cck_index = 24;
-
-	rtldm->bb_swing_idx_cck_base = rtldm->default_cck_index;
-	rtldm->cck_index = rtldm->default_cck_index;
-
-	for (p = RF90_PATH_A; p < MAX_RF_PATH; ++p)
-	{
-		rtldm->bb_swing_idx_ofdm_base[p] = rtldm->default_ofdm_index;
-	   	rtldm->ofdm_index[p] = rtldm->default_ofdm_index;
-		rtldm->delta_power_index[p] = 0;
-		rtldm->power_index_offset[p] = 0;
-		rtldm->delta_power_index_last[p] = 0;
-	}
-}
-
-static void rtl8821ae_dm_init_dynamic_bb_powersaving(struct ieee80211_hw *hw)
-{
-	dm_pstable.pre_ccastate = CCA_MAX;
-	dm_pstable.cur_ccasate = CCA_MAX;
-	dm_pstable.pre_rfstate = RF_MAX;
-	dm_pstable.cur_rfstate = RF_MAX;
-	dm_pstable.rssi_val_min = 0;
-	dm_pstable.initialize = 0;
-}
-
-
-static void rtl8821ae_dm_diginit(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	//dm_digtable.dig_enable_flag = true;
-	dm_digtable.cur_igvalue = rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, 0x7f);
-	/*dm_digtable.pre_igvalue = 0;
-	dm_digtable.cursta_connectctate = DIG_STA_DISCONNECT;
-	dm_digtable.presta_connectstate = DIG_STA_DISCONNECT;
-	dm_digtable.curmultista_connectstate = DIG_MULTISTA_DISCONNECT;*/
-	dm_digtable.rssi_lowthresh = DM_DIG_THRESH_LOW;
-	dm_digtable.rssi_highthresh = DM_DIG_THRESH_HIGH;
-	dm_digtable.fa_lowthresh = DM_FALSEALARM_THRESH_LOW;
-	dm_digtable.fa_highthresh = DM_FALSEALARM_THRESH_HIGH;
-	dm_digtable.rx_gain_range_max = DM_DIG_MAX;
-	dm_digtable.rx_gain_range_min = DM_DIG_MIN;
-	dm_digtable.backoff_val = DM_DIG_BACKOFF_DEFAULT;
-	dm_digtable.backoff_val_range_max = DM_DIG_BACKOFF_MAX;
-	dm_digtable.backoff_val_range_min = DM_DIG_BACKOFF_MIN;
-	dm_digtable.pre_cck_cca_thres = 0xff;
-	dm_digtable.cur_cck_cca_thres = 0x83;
-	dm_digtable.forbidden_igi = DM_DIG_MIN;
-	dm_digtable.large_fa_hit = 0;
-	dm_digtable.recover_cnt = 0;
-	dm_digtable.dig_dynamic_min_0 = DM_DIG_MIN;
-	dm_digtable.dig_dynamic_min_1 = DM_DIG_MIN;
-	dm_digtable.b_media_connect_0 = false;
-	dm_digtable.b_media_connect_1 = false;
-	rtlpriv->dm.b_dm_initialgain_enable = true;
-	dm_digtable.bt30_cur_igi = 0x32;
-}
-
-static void rtl8821ae_dm_init_dynamic_txpower(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.bdynamic_txpower_enable = false;
-
-	rtlpriv->dm.last_dtp_lvl = TXHIGHPWRLEVEL_NORMAL;
-	rtlpriv->dm.dynamic_txhighpower_lvl = TXHIGHPWRLEVEL_NORMAL;
-}
-
-
-void rtl8821ae_dm_init_edca_turbo(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	rtlpriv->dm.bcurrent_turbo_edca = false;
-	rtlpriv->dm.bis_any_nonbepkts = false;
-	rtlpriv->dm.bis_cur_rdlstate = false;
-}
-
-
-void rtl8821ae_dm_init_rate_adaptive_mask(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
-
-	p_ra->ratr_state = DM_RATR_STA_INIT;
-	p_ra->pre_ratr_state = DM_RATR_STA_INIT;
-
-	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	if (rtlpriv->dm.dm_type == DM_TYPE_BYDRIVER)
-		rtlpriv->dm.b_useramask = true;
-	else
-		rtlpriv->dm.b_useramask = false;
-
-	p_ra->high_rssi_thresh_for_ra = 50;
-	p_ra->low_rssi_thresh_for_ra = 20;
-}
-
-
-static void rtl8821ae_dm_init_txpower_tracking(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.btxpower_tracking = true;
-	rtlpriv->dm.btxpower_trackinginit = false;
-	rtlpriv->dm.txpowercount = 0;
-	rtlpriv->dm.txpower_track_control = true;
-	rtlpriv->dm.thermalvalue = 0;
-
-	rtlpriv->dm.ofdm_index[0] = 30;
-	rtlpriv->dm.cck_index = 20;
-
-	rtlpriv->dm.bb_swing_idx_cck_base = rtlpriv->dm.cck_index;
-
-
-	rtlpriv->dm.bb_swing_idx_ofdm[RF90_PATH_A] = rtlpriv->dm.ofdm_index[0];
-	rtlpriv->dm.bb_swing_idx_ofdm[RF90_PATH_B] = rtlpriv->dm.ofdm_index[0];
-	rtlpriv->dm.delta_power_index[0] = 0;
-	rtlpriv->dm.delta_power_index_last[0] = 0;
-	rtlpriv->dm.power_index_offset[0] = 0;
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		 ("  rtlpriv->dm.btxpower_tracking = %d\n",
-		  rtlpriv->dm.btxpower_tracking));
-}
-
-
-void rtl8821ae_dm_init_dynamic_atc_switch(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpriv->dm.crystal_cap = rtlpriv->efuse.crystalcap;
-
-	rtlpriv->dm.atc_status = rtl_get_bbreg(hw, ROFDM1_CFOTRACKING, BIT(11));
-	rtlpriv->dm.cfo_threshold = CFO_THRESHOLD_XTAL;
-}
-
-
-void rtl8821ae_dm_init(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	spin_lock(&rtlpriv->locks.iqk_lock);
-	rtlphy->b_iqk_in_progress = false;
-	spin_unlock(&rtlpriv->locks.iqk_lock);
-
-	rtlpriv->dm.dm_type = DM_TYPE_BYDRIVER;
-	rtl8821ae_dm_diginit(hw);
-	rtl8821ae_dm_init_rate_adaptive_mask(hw);
-	rtl8812ae_dm_path_diversity_init(hw);
-	rtl8821ae_dm_init_edca_turbo(hw);
-	rtl8821ae_dm_initialize_txpower_tracking_thermalmeter(hw);
-#if 1
-	rtl8821ae_dm_init_dynamic_bb_powersaving(hw);
-	rtl8821ae_dm_init_dynamic_txpower(hw);
-	rtl8821ae_dm_init_txpower_tracking(hw);
-#endif
-	rtl8821ae_dm_init_dynamic_atc_switch(hw);
-}
-
-void rtl8821ae_dm_find_minimum_rssi(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dig *rtl_dm_dig = &(rtlpriv->dm.dm_digtable);
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-
-	/* Determine the minimum RSSI  */
-	if ((mac->link_state < MAC80211_LINKED) &&
-	    (rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb == 0)) {
-		rtl_dm_dig->min_undecorated_pwdb_for_dm = 0;
-		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-			 ("Not connected to any \n"));
-	}
-	if (mac->link_state >= MAC80211_LINKED) {
-		if (mac->opmode == NL80211_IFTYPE_AP ||
-			mac->opmode == NL80211_IFTYPE_ADHOC) {
-			rtl_dm_dig->min_undecorated_pwdb_for_dm =
-			    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-				 ("AP Client PWDB = 0x%lx \n",
-				  rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb));
-		} else {
-			rtl_dm_dig->min_undecorated_pwdb_for_dm =
-			    rtlpriv->dm.undecorated_smoothed_pwdb;
-			RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-				 ("STA Default Port PWDB = 0x%x \n",
-				  rtl_dm_dig->min_undecorated_pwdb_for_dm));
-		}
-	} else {
-		rtl_dm_dig->min_undecorated_pwdb_for_dm =
-		    rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-		RT_TRACE(COMP_BB_POWERSAVING, DBG_LOUD,
-			 ("AP Ext Port or disconnect PWDB = 0x%x \n",
-			  rtl_dm_dig->min_undecorated_pwdb_for_dm));
-	}
-	RT_TRACE(COMP_DIG, DBG_LOUD, ("MinUndecoratedPWDBForDM =%d\n",
-			rtl_dm_dig->min_undecorated_pwdb_for_dm));
-}
-
-#if 0
-void  rtl8812ae_dm_rssi_dump_to_register(
-	struct ieee80211_hw *hw
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtl_write_byte(rtlpriv, RA_RSSI_DUMP, Adapter->RxStats.RxRSSIPercentage[0]);
-	rtl_write_byte(rtlpriv, RB_RSSI_DUMP, Adapter->RxStats.RxRSSIPercentage[1]);
-
-	/* Rx EVM*/
-	rtl_write_byte(rtlpriv, RS1_RX_EVM_DUMP, Adapter->RxStats.RxEVMdbm[0]);
-	rtl_write_byte(rtlpriv, RS2_RX_EVM_DUMP, Adapter->RxStats.RxEVMdbm[1]);
-
-	/*Rx SNR*/
-	rtl_write_byte(rtlpriv, RA_RX_SNR_DUMP, (u1Byte)(Adapter->RxStats.RxSNRdB[0]));
-	rtl_write_byte(rtlpriv, RB_RX_SNR_DUMP, (u1Byte)(Adapter->RxStats.RxSNRdB[1]));
-
-	/*Rx Cfo_Short*/
-	rtl_write_word(rtlpriv, RA_CFO_SHORT_DUMP, Adapter->RxStats.RxCfoShort[0]);
-	rtl_write_word(rtlpriv, RB_CFO_SHORT_DUMP, Adapter->RxStats.RxCfoShort[1]);
-
-	/*Rx Cfo_Tail*/
-	rtl_write_word(rtlpriv, RA_CFO_LONG_DUMP, Adapter->RxStats.RxCfoTail[0]);
-	rtl_write_word(rtlpriv, RB_CFO_LONG_DUMP, Adapter->RxStats.RxCfoTail[1]);
-
-}
-#endif
-
-static void rtl8821ae_dm_check_rssi_monitor(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *drv_priv;
-	u8 h2c_parameter[3] = { 0 };
-	long tmp_entry_max_pwdb = 0, tmp_entry_min_pwdb = 0xff;
-
-
-	/* AP & ADHOC & MESH */
-	spin_lock_bh(&rtlpriv->locks.entry_list_lock);
-	list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
-		if(drv_priv->rssi_stat.undecorated_smoothed_pwdb < tmp_entry_min_pwdb)
-			tmp_entry_min_pwdb = drv_priv->rssi_stat.undecorated_smoothed_pwdb;
-		if(drv_priv->rssi_stat.undecorated_smoothed_pwdb > tmp_entry_max_pwdb)
-			tmp_entry_max_pwdb = drv_priv->rssi_stat.undecorated_smoothed_pwdb;
-
-		/*h2c_parameter[2] = (u8) (rtlpriv->dm.undecorated_smoothed_pwdb & 0xFF);
-		h2c_parameter[1] = 0x20;
-		h2c_parameter[0] =  drv_priv->rssi_stat;
-		rtl8821ae_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);*/
-	}
-	spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
-
-	/* If associated entry is found */
-	if (tmp_entry_max_pwdb != 0) {
-		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb = tmp_entry_max_pwdb;
-		RTPRINT(rtlpriv, FDM, DM_PWDB, ("EntryMaxPWDB = 0x%lx(%ld)\n",
-			tmp_entry_max_pwdb, tmp_entry_max_pwdb));
-	} else {
-		rtlpriv->dm.entry_max_undecoratedsmoothed_pwdb = 0;
-	}
-	/* If associated entry is found */
-	if (tmp_entry_min_pwdb != 0xff) {
-		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb = tmp_entry_min_pwdb;
-		RTPRINT(rtlpriv, FDM, DM_PWDB, ("EntryMinPWDB = 0x%lx(%ld)\n",
-					tmp_entry_min_pwdb, tmp_entry_min_pwdb));
-	} else {
-		rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb = 0;
-	}
-	/* Indicate Rx signal strength to FW. */
-	if (rtlpriv->dm.b_useramask) {
-		h2c_parameter[2] = (u8) (rtlpriv->dm.undecorated_smoothed_pwdb & 0xFF);
-		h2c_parameter[1] = 0x20;
-		h2c_parameter[0] = 0;
-		rtl8821ae_fill_h2c_cmd(hw, H2C_RSSI_REPORT, 3, h2c_parameter);
-	} else {
-		rtl_write_byte(rtlpriv, 0x4fe, rtlpriv->dm.undecorated_smoothed_pwdb);
-	}
-	rtl8821ae_dm_find_minimum_rssi(hw);
-	dm_digtable.rssi_val_min = rtlpriv->dm.dm_digtable.min_undecorated_pwdb_for_dm;
-}
-
-void rtl8821ae_dm_write_cck_cca_thres(struct ieee80211_hw *hw, u8 current_cca)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (dm_digtable.cur_cck_cca_thres != current_cca)
-		rtl_write_byte(rtlpriv, DM_REG_CCK_CCA_11AC, current_cca);
-
-	dm_digtable.pre_cck_cca_thres = dm_digtable.cur_cck_cca_thres;
-	dm_digtable.cur_cck_cca_thres = current_cca;
-}
-
-void rtl8821ae_dm_write_dig(struct ieee80211_hw *hw, u8 current_igi)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	if(dm_digtable.stop_dig)
-		return;
-
-	if (dm_digtable.cur_igvalue != current_igi){
-		rtl_set_bbreg(hw, DM_REG_IGI_A_11AC, DM_BIT_IGI_11AC, current_igi);
-		if (rtlpriv->phy.rf_type != RF_1T1R)
-			rtl_set_bbreg(hw, DM_REG_IGI_B_11AC, DM_BIT_IGI_11AC, current_igi);
-	}
-	//dm_digtable.pre_igvalue = dm_digtable.cur_igvalue;
-	dm_digtable.cur_igvalue = current_igi;
-}
-
-static void rtl8821ae_dm_dig(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 dig_dynamic_min;
-	u8 dig_max_of_min;
-	bool first_connect, first_disconnect;
-	u8 dm_dig_max, dm_dig_min, offset;
-	u8 current_igi =dm_digtable.cur_igvalue;
-
-
-	RT_TRACE(COMP_DIG, DBG_LOUD,("rtl8821ae_dm_dig()==>\n"));
-
-
-	if (mac->act_scanning == true) {
-		RT_TRACE(COMP_DIG, DBG_LOUD,("rtl8821ae_dm_dig() Return: In Scan Progress \n"));
-	    	return;
-	}
-
-	/*add by Neil Chen to avoid PSD is processing*/
-	dig_dynamic_min = dm_digtable.dig_dynamic_min_0;
-	first_connect = (mac->link_state >= MAC80211_LINKED) &&
-			(dm_digtable.b_media_connect_0 == false);
-	first_disconnect = (mac->link_state < MAC80211_LINKED) &&
-			(dm_digtable.b_media_connect_0 == true);
-
-	/*1 Boundary Decision*/
-
-
-	dm_dig_max = 0x5A;
-
-	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8821AE)
-		dm_dig_min = DM_DIG_MIN;
-	else
-		dm_dig_min = 0x1C;
-
-	dig_max_of_min = DM_DIG_MAX_AP;
-
-	if (mac->link_state >= MAC80211_LINKED) {
-		if (rtlhal->hw_type != HARDWARE_TYPE_RTL8821AE)
-			offset = 20;
-		else
-			offset = 10;
-
-		if ((dm_digtable.rssi_val_min + offset) > dm_dig_max)
-			dm_digtable.rx_gain_range_max = dm_dig_max;
-		else if ((dm_digtable.rssi_val_min + offset) < dm_dig_min)
-			dm_digtable.rx_gain_range_max = dm_dig_min;
-		else
-			dm_digtable.rx_gain_range_max = dm_digtable.rssi_val_min + offset;
-
-		if(rtlpriv->dm.b_one_entry_only){
-			offset = 0;
-
-			if (dm_digtable.rssi_val_min - offset < dm_dig_min)
-				dig_dynamic_min = dm_dig_min;
-			else if (dm_digtable.rssi_val_min - offset > dig_max_of_min)
-				dig_dynamic_min = dig_max_of_min;
-			else
-				dig_dynamic_min = dm_digtable.rssi_val_min - offset;
-
-			RT_TRACE(COMP_DIG, DBG_LOUD,
-				("rtl8821ae_dm_dig() : bOneEntryOnly=TRUE,  dig_dynamic_min=0x%x\n",
-				dig_dynamic_min));
-			RT_TRACE(COMP_DIG, DBG_LOUD,
-				("rtl8821ae_dm_dig() : dm_digtable.rssi_val_min=%d",dm_digtable.
-				rssi_val_min));
-		} else {
-			dig_dynamic_min = dm_dig_min;
-		}
-	} else {
-		dm_digtable.rx_gain_range_max = dm_dig_max;
-		dig_dynamic_min = dm_dig_min;
-		RT_TRACE(COMP_DIG, DBG_LOUD,
-			("rtl8821ae_dm_dig() : No Link\n"));
-	}
-
-	if (rtlpriv->falsealm_cnt.cnt_all > 10000) {
-		RT_TRACE(COMP_DIG, DBG_LOUD,
-			("rtl8821ae_dm_dig(): Abnormally false alarm case. \n"));
-
-		if (dm_digtable.large_fa_hit != 3)
-		        dm_digtable.large_fa_hit++;
-		if (dm_digtable.forbidden_igi < current_igi) {
-			dm_digtable.forbidden_igi = current_igi;
-			dm_digtable.large_fa_hit = 1;
-		}
-
-		if (dm_digtable.large_fa_hit >= 3) {
-			if((dm_digtable.forbidden_igi + 1) > dm_digtable.rx_gain_range_max)
-				dm_digtable.rx_gain_range_min = dm_digtable.rx_gain_range_max;
-			else
-				dm_digtable.rx_gain_range_min = (dm_digtable.forbidden_igi + 1);
-			dm_digtable.recover_cnt = 3600;
-		}
-
-	} else {
-		/*Recovery mechanism for IGI lower bound*/
-		if (dm_digtable.recover_cnt != 0)
-			dm_digtable.recover_cnt --;
-		else {
-			if (dm_digtable.large_fa_hit < 3) {
-				if ((dm_digtable.forbidden_igi -1) < dig_dynamic_min) {
-					dm_digtable.forbidden_igi = dig_dynamic_min;
-					dm_digtable.rx_gain_range_min = dig_dynamic_min;
-					RT_TRACE(COMP_DIG, DBG_LOUD,
-						("rtl8821ae_dm_dig(): Normal Case: At Lower Bound\n"));
-				} else {
-					dm_digtable.forbidden_igi --;
-					dm_digtable.rx_gain_range_min = (dm_digtable.forbidden_igi + 1);
-					RT_TRACE(COMP_DIG, DBG_LOUD,
-						("rtl8821ae_dm_dig(): Normal Case: Approach Lower Bound\n"));
-				}
-			} else {
-				dm_digtable.large_fa_hit = 0;
-			}
-		}
-	}
-	RT_TRACE(COMP_DIG, DBG_LOUD,
-		("rtl8821ae_dm_dig(): pDM_DigTable->LargeFAHit=%d\n",
-		dm_digtable.large_fa_hit));
-
-	if (rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 10)
-		dm_digtable.rx_gain_range_min = dm_dig_min;
-
-	if (dm_digtable.rx_gain_range_min > dm_digtable.rx_gain_range_max)
-		dm_digtable.rx_gain_range_min = dm_digtable.rx_gain_range_max;
-
-	/*Adjust initial gain by false alarm*/
-	if (mac->link_state >= MAC80211_LINKED) {
-		RT_TRACE(COMP_DIG, DBG_LOUD,
-			("rtl8821ae_dm_dig(): DIG AfterLink\n"));
-		if (first_connect) {
-			if (dm_digtable.rssi_val_min <= dig_max_of_min)
-				current_igi = dm_digtable.rssi_val_min;
-			else
-				current_igi = dig_max_of_min;
-			RT_TRACE(COMP_DIG, DBG_LOUD,
-				("rtl8821ae_dm_dig: First Connect\n"));
-		} else {
-			if(rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH2)
-				current_igi = current_igi + 4;
-			else if (rtlpriv->falsealm_cnt.cnt_all > DM_DIG_FA_TH1)
-				current_igi = current_igi + 2;
-			else if(rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH0)
-				current_igi = current_igi - 2;
-
-			if((rtlpriv->dm.dbginfo.num_qry_beacon_pkt < 10)
-				&&(rtlpriv->falsealm_cnt.cnt_all < DM_DIG_FA_TH1)) {
-				current_igi = dm_digtable.rx_gain_range_min;
-				RT_TRACE(COMP_DIG, DBG_LOUD,
-					("rtl8821ae_dm_dig(): Beacon is less than 10 and FA is less than 768, IGI GOES TO 0x1E!!!!!!!!!!!!\n"));
-			}
-		}
-	}  else{
-		RT_TRACE(COMP_DIG, DBG_LOUD,
-			("rtl8821ae_dm_dig(): DIG BeforeLink\n"));
-		if (first_disconnect){
-			current_igi = dm_digtable.rx_gain_range_min;
-			RT_TRACE(COMP_DIG, DBG_LOUD,
-				("rtl8821ae_dm_dig(): First DisConnect \n"));
-		} else {
-			/*2012.03.30 LukeLee: enable DIG before link but with very high thresholds*/
-	       	if (rtlpriv->falsealm_cnt.cnt_all > 2000)
-				current_igi = current_igi + 4;
-			else if (rtlpriv->falsealm_cnt.cnt_all > 600)
-				current_igi = current_igi + 2;
-			else if(rtlpriv->falsealm_cnt.cnt_all < 300)
-				current_igi = current_igi - 2;
-			if (current_igi >= 0x3e)
-				current_igi = 0x3e;
-			RT_TRACE(COMP_DIG, DBG_LOUD,("rtl8821ae_dm_dig(): England DIG \n"));
-		}
-	}
-	RT_TRACE(COMP_DIG, DBG_LOUD,
-		("rtl8821ae_dm_dig(): DIG End Adjust IGI\n"));
-	/* Check initial gain by upper/lower bound*/
-
-	if (current_igi > dm_digtable.rx_gain_range_max)
-		current_igi = dm_digtable.rx_gain_range_max;
-	if (current_igi < dm_digtable.rx_gain_range_min)
-		current_igi = dm_digtable.rx_gain_range_min;
-
-	RT_TRACE(COMP_DIG, DBG_LOUD,
-		("rtl8821ae_dm_dig(): rx_gain_range_max=0x%x, rx_gain_range_min=0x%x\n",
-		dm_digtable.rx_gain_range_max, dm_digtable.rx_gain_range_min));
-	RT_TRACE(COMP_DIG, DBG_LOUD,
-		("rtl8821ae_dm_dig(): TotalFA=%d\n", rtlpriv->falsealm_cnt.cnt_all));
-	RT_TRACE(COMP_DIG, DBG_LOUD,
-		("rtl8821ae_dm_dig(): CurIGValue=0x%x\n", current_igi));
-
-	rtl8821ae_dm_write_dig(hw, current_igi);
-	dm_digtable.b_media_connect_0= ((mac->link_state >= MAC80211_LINKED) ? true :false);
-	dm_digtable.dig_dynamic_min_0 = dig_dynamic_min;
-}
-
-static void rtl8821ae_dm_common_info_self_update(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 cnt = 0;
-	struct rtl_sta_info *drv_priv;
-
-	rtlpriv->dm.b_one_entry_only = false;
-
-	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_STATION &&
-		rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-		rtlpriv->dm.b_one_entry_only = true;
-		return;
-	}
-
-	if (rtlpriv->mac80211.opmode == NL80211_IFTYPE_AP ||
-		rtlpriv->mac80211.opmode == NL80211_IFTYPE_ADHOC ||
-		rtlpriv->mac80211.opmode == NL80211_IFTYPE_MESH_POINT) {
-		spin_lock_bh(&rtlpriv->locks.entry_list_lock);
-		list_for_each_entry(drv_priv, &rtlpriv->entry_list, list) {
-			cnt ++;
-		}
-		spin_unlock_bh(&rtlpriv->locks.entry_list_lock);
-
-		if (cnt == 1)
-			rtlpriv->dm.b_one_entry_only = true;
-	}
-}
-
-
-static void rtl8821ae_dm_false_alarm_counter_statistics(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct false_alarm_statistics *falsealm_cnt = &(rtlpriv->falsealm_cnt);
-	u32 cck_enable =0;
-
-	/*read OFDM FA counter*/
-	falsealm_cnt->cnt_ofdm_fail = rtl_get_bbreg(hw, ODM_REG_OFDM_FA_11AC, BMASKLWORD);
-	falsealm_cnt->cnt_cck_fail = rtl_get_bbreg(hw, ODM_REG_CCK_FA_11AC, BMASKLWORD);
-
-	cck_enable =  rtl_get_bbreg(hw, ODM_REG_BB_RX_PATH_11AC, BIT(28));
-	if (cck_enable)  /*if(pDM_Odm->pBandType == ODM_BAND_2_4G)*/
-		falsealm_cnt->cnt_all = falsealm_cnt->cnt_ofdm_fail + falsealm_cnt->cnt_cck_fail;
-	else
-		falsealm_cnt->cnt_all = falsealm_cnt->cnt_ofdm_fail;
-
-	/*reset OFDM FA counter*/
-	rtl_set_bbreg(hw, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 1);
-	rtl_set_bbreg(hw, ODM_REG_OFDM_FA_RST_11AC, BIT(17), 0);
-	/* reset CCK FA counter*/
-	rtl_set_bbreg(hw,  ODM_REG_CCK_FA_RST_11AC, BIT(15), 0);
-	rtl_set_bbreg(hw,  ODM_REG_CCK_FA_RST_11AC, BIT(15), 1);
-
-	RT_TRACE(COMP_DIG, DBG_LOUD, ("Cnt_Cck_fail=%d\n",
-			falsealm_cnt->cnt_cck_fail));
-	RT_TRACE(COMP_DIG, DBG_LOUD, ("cnt_ofdm_fail=%d\n",
-			falsealm_cnt->cnt_ofdm_fail));
-	RT_TRACE(COMP_DIG, DBG_LOUD, ("Total False Alarm=%d\n",
-			falsealm_cnt->cnt_all));
-}
-
-void rtl8812ae_dm_check_txpower_tracking_thermalmeter(
-		struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger = 0;
-
-	if (!rtlpriv->dm.btxpower_tracking)
-		return;
-
-	if (!tm_trigger) {
-		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E, BIT(17)|BIT(16), 0x03);
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			 ("Trigger 8812 Thermal Meter!!\n"));
-		tm_trigger = 1;
-		return;
-	} else {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			 ("Schedule TxPowerTracking direct call!!\n"));
-		rtl8812ae_dm_txpower_tracking_callback_thermalmeter(hw);
-		tm_trigger = 0;
-	}
-}
-
-static void rtl8821ae_dm_iq_calibrate(struct ieee80211_hw *hw)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (mac->link_state >= MAC80211_LINKED) {
-		/*if ((*rtldm->p_channel != rtldm->pre_channel )
-			&& (!mac->act_scanning)) {
-			rtldm->pre_channel = *rtldm->p_channel;
-			rtldm->linked_interval = 0;
-		}*/
-
-		if(rtldm->linked_interval < 3)
-			rtldm->linked_interval ++;
-
-		if(rtldm->linked_interval == 2)
-		{
-			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-				rtl8812ae_phy_iq_calibrate(hw, false);
-			else
-				rtl8821ae_phy_iq_calibrate(hw, false);
-		}
-	} else {
-		rtldm->linked_interval = 0;
-	}
-}
-
-
-void rtl8812ae_get_delta_swing_table(
-	struct ieee80211_hw *hw,
-	u8 **temperature_up_a,
-	u8 **temperature_down_a,
-	u8 **temperature_up_b,
-	u8 **temperature_down_b
-	)
-{
-   	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	u8 channel = rtlphy->current_channel;
-	u8 rate = rtldm->tx_rate;
-
-
-	if ( 1 <= channel && channel <= 14) {
-		if (RX_HAL_IS_CCK_RATE(rate)) {
-		        *temperature_up_a = rtldm->delta_swing_table_idx_24gccka_p;
-		        *temperature_down_a = rtldm->delta_swing_table_idx_24gccka_n;
-		        *temperature_up_b = rtldm->delta_swing_table_idx_24gcckb_p;
-		        *temperature_down_b = rtldm->delta_swing_table_idx_24gcckb_n;
-		} else {
-		        *temperature_up_a = rtldm->delta_swing_table_idx_24ga_p;
-		        *temperature_down_a = rtldm->delta_swing_table_idx_24ga_n;
-		        *temperature_up_b = rtldm->delta_swing_table_idx_24gb_p;
-		        *temperature_down_b = rtldm->delta_swing_table_idx_24gb_n;
-		}
- 	} else if ( 36 <= channel && channel <= 64) {
-	        *temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[0];
-	        *temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[0];
-	        *temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[0];
-	        *temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[0];
-    	} else if ( 100 <= channel && channel <= 140) {
-		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[1];
-		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[1];
-		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[1];
-		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[1];
-    	} else if ( 149 <= channel && channel <= 173) {
-		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[2];
-		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[2];
-		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[2];
-		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[2];
-    	} else {
-	    *temperature_up_a = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
-	    *temperature_down_a =(u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
-	    *temperature_up_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
-	    *temperature_down_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
-    	}
-
-	return;
-}
-
-void rtl8812ae_phy_lccalibrate(
-	struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("===> rtl8812ae_phy_lccalibrate\n"));
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("<=== rtl8812ae_phy_lccalibrate\n"));
-
-}
-
-void rtl8812ae_dm_update_init_rate(
-	struct ieee80211_hw *hw,
-	u8 rate
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 p = 0;
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("Get C2H Command! Rate=0x%x\n", rate));
-
-	rtldm->tx_rate = rate;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE){
-		rtl8821ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, RF90_PATH_A, 0);
-	}
-	else
-	{
-		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
-		{
-			rtl8812ae_dm_txpwr_track_set_pwr(hw, BBSWING, p, 0);
-		}
-	}
-
-}
-
-u8 rtl8812ae_hw_rate_to_mrate(
-	struct ieee80211_hw *hw,
-	u8 rate
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 ret_rate = MGN_1M;
-
-
-	switch(rate)
-	{
-		case DESC_RATE1M:		ret_rate = MGN_1M;		break;
-		case DESC_RATE2M:		ret_rate = MGN_2M;		break;
-		case DESC_RATE5_5M:		ret_rate = MGN_5_5M;		break;
-		case DESC_RATE11M:		ret_rate = MGN_11M;		break;
-		case DESC_RATE6M:		ret_rate = MGN_6M;		break;
-		case DESC_RATE9M:		ret_rate = MGN_9M;		break;
-		case DESC_RATE12M:		ret_rate = MGN_12M;		break;
-		case DESC_RATE18M:		ret_rate = MGN_18M;		break;
-		case DESC_RATE24M:		ret_rate = MGN_24M;		break;
-		case DESC_RATE36M:		ret_rate = MGN_36M;		break;
-		case DESC_RATE48M:		ret_rate = MGN_48M;		break;
-		case DESC_RATE54M:		ret_rate = MGN_54M;		break;
-		case DESC_RATEMCS0:	ret_rate = MGN_MCS0;		break;
-		case DESC_RATEMCS1:	ret_rate = MGN_MCS1;		break;
-		case DESC_RATEMCS2:	ret_rate = MGN_MCS2;		break;
-		case DESC_RATEMCS3:	ret_rate = MGN_MCS3;		break;
-		case DESC_RATEMCS4:	ret_rate = MGN_MCS4;		break;
-		case DESC_RATEMCS5:	ret_rate = MGN_MCS5;		break;
-		case DESC_RATEMCS6:	ret_rate = MGN_MCS6;		break;
-		case DESC_RATEMCS7:	ret_rate = MGN_MCS7;		break;
-		case DESC_RATEMCS8:	ret_rate = MGN_MCS8;		break;
-		case DESC_RATEMCS9:	ret_rate = MGN_MCS9;		break;
-		case DESC_RATEMCS10:	ret_rate = MGN_MCS10;	break;
-		case DESC_RATEMCS11:	ret_rate = MGN_MCS11;	break;
-		case DESC_RATEMCS12:	ret_rate = MGN_MCS12;	break;
-		case DESC_RATEMCS13:	ret_rate = MGN_MCS13;	break;
-		case DESC_RATEMCS14:	ret_rate = MGN_MCS14;	break;
-		case DESC_RATEMCS15:	ret_rate = MGN_MCS15;	break;
-		case DESC_RATEVHT1SS_MCS0:	ret_rate = MGN_VHT1SS_MCS0;		break;
-		case DESC_RATEVHT1SS_MCS1:	ret_rate = MGN_VHT1SS_MCS1;		break;
-		case DESC_RATEVHT1SS_MCS2:	ret_rate = MGN_VHT1SS_MCS2;		break;
-		case DESC_RATEVHT1SS_MCS3:	ret_rate = MGN_VHT1SS_MCS3;		break;
-		case DESC_RATEVHT1SS_MCS4:	ret_rate = MGN_VHT1SS_MCS4;		break;
-		case DESC_RATEVHT1SS_MCS5:	ret_rate = MGN_VHT1SS_MCS5;		break;
-		case DESC_RATEVHT1SS_MCS6:	ret_rate = MGN_VHT1SS_MCS6;		break;
-		case DESC_RATEVHT1SS_MCS7:	ret_rate = MGN_VHT1SS_MCS7;		break;
-		case DESC_RATEVHT1SS_MCS8:	ret_rate = MGN_VHT1SS_MCS8;		break;
-		case DESC_RATEVHT1SS_MCS9:	ret_rate = MGN_VHT1SS_MCS9;		break;
-		case DESC_RATEVHT2SS_MCS0:	ret_rate = MGN_VHT2SS_MCS0;		break;
-		case DESC_RATEVHT2SS_MCS1:	ret_rate = MGN_VHT2SS_MCS1;		break;
-		case DESC_RATEVHT2SS_MCS2:	ret_rate = MGN_VHT2SS_MCS2;		break;
-		case DESC_RATEVHT2SS_MCS3:	ret_rate = MGN_VHT2SS_MCS3;		break;
-		case DESC_RATEVHT2SS_MCS4:	ret_rate = MGN_VHT2SS_MCS4;		break;
-		case DESC_RATEVHT2SS_MCS5:	ret_rate = MGN_VHT2SS_MCS5;		break;
-		case DESC_RATEVHT2SS_MCS6:	ret_rate = MGN_VHT2SS_MCS6;		break;
-		case DESC_RATEVHT2SS_MCS7:	ret_rate = MGN_VHT2SS_MCS7;		break;
-		case DESC_RATEVHT2SS_MCS8:	ret_rate = MGN_VHT2SS_MCS8;		break;
-		case DESC_RATEVHT2SS_MCS9:	ret_rate = MGN_VHT2SS_MCS9;		break;
-
-		default:
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("HwRateToMRate8812(): Non supported Rate [%x]!!!\n",rate ));
-			break;
-	}
-	return ret_rate;
-}
-
-/*-----------------------------------------------------------------------------
- * Function:	odm_TxPwrTrackSetPwr88E()
- *
- * Overview:	88E change all channel tx power according to flag.
- *				OFDM & CCK are all different.
- *
- * Input:		NONE
- *
- * Output:		NONE
- *
- * Return:		NONE
- *
- * Revised History:
- *	When		Who		Remark
- *	04/23/2012	MHC		Create Version 0.
- *
- *---------------------------------------------------------------------------*/
-void rtl8812ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
-	enum pwr_track_control_method method, u8 rf_path, u8 channel_mapped_index)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 final_bb_swing_idx[2];
-	u8 pwr_tracking_limit = 26; /*+1.0dB*/
-	u8 tx_rate = 0xFF;
-	s8 final_ofdm_swing_index = 0;
-
-	if(rtldm->tx_rate != 0xFF)
-		tx_rate = rtl8812ae_hw_rate_to_mrate(hw, rtldm->tx_rate);
-
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
-
-	if(tx_rate != 0xFF) { /*20130429 Mimic Modify High Rate BBSwing Limit.*/
-		/*CCK*/
-		if((tx_rate >= MGN_1M) && (tx_rate <= MGN_11M))
-			pwr_tracking_limit = 32; /*+4dB*/
-		/*OFDM*/
-		else if((tx_rate >= MGN_6M) && (tx_rate <= MGN_48M))
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if(tx_rate == MGN_54M)
-			pwr_tracking_limit = 28; /*+2dB*/
-		/*HT*/
-		else if((tx_rate >= MGN_MCS0) && (tx_rate <= MGN_MCS2)) /*QPSK/BPSK*/
-			pwr_tracking_limit = 34; /*+5dB*/
-		else if((tx_rate >= MGN_MCS3) && (tx_rate <= MGN_MCS4)) /*16QAM*/
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if((tx_rate >= MGN_MCS5) && (tx_rate <= MGN_MCS7)) /*64QAM*/
-			pwr_tracking_limit = 28; /*+2dB*/
-
-		else if((tx_rate >= MGN_MCS8) && (tx_rate <= MGN_MCS10)) /*QPSK/BPSK*/
-			pwr_tracking_limit = 34; /*+5dB*/
-		else if((tx_rate >= MGN_MCS11) && (tx_rate <= MGN_MCS12)) /*16QAM*/
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if((tx_rate >= MGN_MCS13) && (tx_rate <= MGN_MCS15)) /*64QAM*/
-			pwr_tracking_limit = 28; /*+2dB*/
-
-		/*2 VHT*/
-		else if((tx_rate >= MGN_VHT1SS_MCS0) && (tx_rate <= MGN_VHT1SS_MCS2)) /*QPSK/BPSK*/
-			pwr_tracking_limit = 34; /*+5dB*/
-		else if((tx_rate >= MGN_VHT1SS_MCS3) && (tx_rate <= MGN_VHT1SS_MCS4)) /*16QAM*/
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if((tx_rate >= MGN_VHT1SS_MCS5)&&(tx_rate <= MGN_VHT1SS_MCS6)) /*64QAM*/
-			pwr_tracking_limit = 28; /*+2dB*/
-		else if(tx_rate == MGN_VHT1SS_MCS7) /*64QAM*/
-			pwr_tracking_limit = 26; /*+1dB*/
-		else if(tx_rate == MGN_VHT1SS_MCS8) /*256QAM*/
-			pwr_tracking_limit = 24; /*+0dB*/
-		else if(tx_rate == MGN_VHT1SS_MCS9) /*256QAM*/
-			pwr_tracking_limit = 22; /*-1dB*/
-
-		else if((tx_rate >= MGN_VHT2SS_MCS0)&&(tx_rate <= MGN_VHT2SS_MCS2)) /*QPSK/BPSK*/
-			pwr_tracking_limit = 34; /*+5dB*/
-		else if((tx_rate >= MGN_VHT2SS_MCS3)&&(tx_rate <= MGN_VHT2SS_MCS4)) /*16QAM*/
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if((tx_rate >= MGN_VHT2SS_MCS5)&&(tx_rate <= MGN_VHT2SS_MCS6)) /*64QAM*/
-			pwr_tracking_limit = 28; /*+2dB*/
-		else if(tx_rate == MGN_VHT2SS_MCS7) /*64QAM*/
-			pwr_tracking_limit = 26; /*+1dB*/
-		else if(tx_rate == MGN_VHT2SS_MCS8) /*256QAM*/
-			pwr_tracking_limit = 24; /*+0dB*/
-		else if(tx_rate == MGN_VHT2SS_MCS9) /*256QAM*/
-			pwr_tracking_limit = 22; /*-1dB*/
-		else
-			pwr_tracking_limit = 24;
-	}
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("TxRate=0x%x, PwrTrackingLimit=%d\n", tx_rate, pwr_tracking_limit));
-
-
-	if (method == BBSWING) {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
-
-		if (rf_path == RF90_PATH_A) {
-			final_bb_swing_idx[RF90_PATH_A] =
-				(rtldm->ofdm_index[RF90_PATH_A] > pwr_tracking_limit) ?
-				pwr_tracking_limit : rtldm->ofdm_index[RF90_PATH_A];
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_A]=%d, \
-				pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_A]=%d\n",
-				rtldm->ofdm_index[RF90_PATH_A], final_bb_swing_idx[RF90_PATH_A]));
-
-			rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_bb_swing_idx[RF90_PATH_A]]);
-		} else {
-			final_bb_swing_idx[RF90_PATH_B] =
-				rtldm->ofdm_index[RF90_PATH_B] > pwr_tracking_limit ? \
-				pwr_tracking_limit : rtldm->ofdm_index[RF90_PATH_B];
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_B]=%d, \
-				pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_B]=%d\n",
-				rtldm->ofdm_index[RF90_PATH_B], final_bb_swing_idx[RF90_PATH_B]));
-
-			rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_bb_swing_idx[RF90_PATH_B]]);
-		}
-	} else if (method == MIX_MODE) {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("pDM_Odm->DefaultOfdmIndex=%d, \
-			pDM_Odm->Aboslute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
-			rtldm->default_ofdm_index, rtldm->aboslute_ofdm_swing_idx[rf_path],
-			rf_path ));
-
-
-		final_ofdm_swing_index = rtldm->default_ofdm_index + rtldm->aboslute_ofdm_swing_idx[rf_path];
-
-		if (rf_path == RF90_PATH_A) {
-			if(final_ofdm_swing_index > pwr_tracking_limit) {     /*BBSwing higher then Limit*/
-
-				rtldm->remnant_cck_idx = final_ofdm_swing_index - pwr_tracking_limit;
-				/* CCK Follow the same compensate value as Path A*/
-				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index - pwr_tracking_limit;
-
-				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[pwr_tracking_limit]);
-
-				rtldm->modify_txagc_flag_path_a = true;
-
-				/*Set TxAGC Page C{};*/
-				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_A Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d \n",
-					pwr_tracking_limit, rtldm->remnant_ofdm_swing_idx[rf_path]));
-			} else if (final_ofdm_swing_index < 0) {
-				rtldm->remnant_cck_idx = final_ofdm_swing_index;
-				/* CCK Follow the same compensate value as Path A*/
-				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index;
-
-				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[0]);
-
-				rtldm->modify_txagc_flag_path_a = true;
-
-				/*Set TxAGC Page C{};*/
-				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d \n",
-					 rtldm->remnant_ofdm_swing_idx[rf_path]));
-			} else {
-				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_ofdm_swing_index]);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_A Compensate with BBSwing , Final_OFDM_Swing_Index = %d \n",
-					final_ofdm_swing_index));
-
-				if(rtldm->modify_txagc_flag_path_a) { /*If TxAGC has changed, reset TxAGC again*/
-					rtldm->remnant_cck_idx = 0;
-					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
-
-					/*Set TxAGC Page C{};*/
-					rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
-
-					rtldm->modify_txagc_flag_path_a = false;
-
-					RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-						("******Path_A pDM_Odm->Modify_TxAGC_Flag = FALSE \n"));
-				}
-			}
-		}
-
-		if (rf_path == RF90_PATH_B) {
-			if(final_ofdm_swing_index > pwr_tracking_limit) {    /*BBSwing higher then Limit*/
-				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index - pwr_tracking_limit;
-
-				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[pwr_tracking_limit]);
-
-				rtldm->modify_txagc_flag_path_b = true;
-
-				/*Set TxAGC Page E{};*/
-				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_B);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_B Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d \n",
-					pwr_tracking_limit, rtldm->remnant_ofdm_swing_idx[rf_path]));
-			} else if (final_ofdm_swing_index < 0) {
-				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index;
-
-				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[0]);
-
-				rtldm->modify_txagc_flag_path_b = true;
-
-				/*Set TxAGC Page E{};*/
-				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_B);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_B Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d \n",
-					rtldm->remnant_ofdm_swing_idx[rf_path] ));
-			} else {
-				rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_ofdm_swing_index]);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_B Compensate with BBSwing , Final_OFDM_Swing_Index = %d \n",
-					final_ofdm_swing_index));
-
-				if(rtldm->modify_txagc_flag_path_b) { /*If TxAGC has changed, reset TxAGC again*/
-					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
-
-					/*Set TxAGC Page E{};*/
-					rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_B);
-
-					rtldm->modify_txagc_flag_path_b = false;
-
-					RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-						("******Path_B dm_Odm->Modify_TxAGC_Flag = FALSE \n"));
-				}
-			}
-		}
-
-	} else {
-		return;
-	}
-}
-
-void rtl8812ae_dm_txpower_tracking_callback_thermalmeter
-	(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	u8 thermal_value = 0, delta, delta_lck, delta_iqk, p = 0, i = 0;
-	u8 thermal_value_avg_count = 0;
-	u32 thermal_value_avg = 0;
-
-	u8 ofdm_min_index = 6;  /*OFDM BB Swing should be less than +3.0dB, which is required by Arthur*/
-	u8 index_for_channel = 0; /* GetRightChnlPlaceforIQK(pHalData->CurrentChannel)*/
-
-	/* 1. The following TWO tables decide the final index of OFDM/CCK swing table.*/
-	u8 *delta_swing_table_idx_tup_a;
-	u8 *delta_swing_table_idx_tdown_a;
-	u8 *delta_swing_table_idx_tup_b;
-	u8 *delta_swing_table_idx_tdown_b;
-
-	/*2. Initilization ( 7 steps in total )*/
-	rtl8812ae_get_delta_swing_table(hw, (u8**)&delta_swing_table_idx_tup_a,
-									(u8**)&delta_swing_table_idx_tdown_a,
-									  (u8**)&delta_swing_table_idx_tup_b,
-									  (u8**)&delta_swing_table_idx_tdown_b);
-
-	rtldm->btxpower_trackinginit = true;
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("===>rtl8812ae_dm_txpower_tracking_callback_thermalmeter, \
-		 \n pDM_Odm->BbSwingIdxCckBase: %d, pDM_Odm->BbSwingIdxOfdmBase[A]:\
-		 %d, pDM_Odm->DefaultOfdmIndex: %d\n",
-		rtldm->bb_swing_idx_cck_base,
-		rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A],
-		rtldm->default_ofdm_index));
-
-	thermal_value = (u8)rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER_8812A, 0xfc00);	/*0x42: RF Reg[15:10] 88E*/
-	if( ! rtldm->txpower_track_control || rtlefuse->eeprom_thermalmeter == 0 ||
-		rtlefuse->eeprom_thermalmeter == 0xFF)
-        	return;
-
-
-	/* 3. Initialize ThermalValues of RFCalibrateInfo*/
-
-	if(rtlhal->reloadtxpowerindex)
-	{
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("reload ofdm index for band switch\n"));
-	}
-
-	/*4. Calculate average thermal meter*/
-	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermal_value;
-	rtldm->thermalvalue_avg_index++;
-	if(rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_8812A)
-		/*Average times =  c.AverageThermalNum*/
-		rtldm->thermalvalue_avg_index = 0;
-
-	for(i = 0; i < AVG_THERMAL_NUM_8812A; i++)
-	{
-		if(rtldm->thermalvalue_avg[i])
-		{
-			thermal_value_avg += rtldm->thermalvalue_avg[i];
-			thermal_value_avg_count++;
-		}
-	}
-
-	if(thermal_value_avg_count) /*Calculate Average ThermalValue after average enough times*/
-	{
-		thermal_value = (u8)(thermal_value_avg / thermal_value_avg_count);
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
-			thermal_value, rtlefuse->eeprom_thermalmeter));
-	}
-
-	/*5. Calculate delta, delta_LCK, delta_IQK.*/
-	/*"delta" here is used to determine whether thermal value changes or not.*/
-	delta = (thermal_value > rtldm->thermalvalue) ? \
-		(thermal_value - rtldm->thermalvalue): \
-		(rtldm->thermalvalue - thermal_value);
-	delta_lck = (thermal_value > rtldm->thermalvalue_lck) ? \
-		(thermal_value - rtldm->thermalvalue_lck) : \
-		(rtldm->thermalvalue_lck - thermal_value);
-	delta_iqk = (thermal_value > rtldm->thermalvalue_iqk) ? \
-		(thermal_value - rtldm->thermalvalue_iqk) : \
-		(rtldm->thermalvalue_iqk - thermal_value);
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
-		delta, delta_lck, delta_iqk));
-
-	/* 6. If necessary, do LCK.	*/
-
-	if (delta_lck >= IQK_THRESHOLD) /*Delta temperature is equal to or larger than 20 centigrade.*/
-	{
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("delta_LCK(%d) >= Threshold_IQK(%d)\n",
-			delta_lck, IQK_THRESHOLD));
-		rtldm->thermalvalue_lck = thermal_value;
-		rtl8812ae_phy_lccalibrate(hw);
-	}
-
-	/*7. If necessary, move the index of swing table to adjust Tx power.*/
-
-	if (delta > 0 && rtldm->txpower_track_control)
-	{
-		/*"delta" here is used to record the absolute value of difference.*/
-	    	delta = thermal_value > rtlefuse->eeprom_thermalmeter ? \
-		    	(thermal_value - rtlefuse->eeprom_thermalmeter) : \
-		    	(rtlefuse->eeprom_thermalmeter - thermal_value);
-
-		if (delta >= TXPWR_TRACK_TABLE_SIZE)
-			delta = TXPWR_TRACK_TABLE_SIZE - 1;
-
-		/*7.1 The Final Power Index = BaseIndex + PowerIndexOffset*/
-
-		if(thermal_value > rtlefuse->eeprom_thermalmeter) {
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			    		("delta_swing_table_idx_tup_a[%d] = %d\n",
-			    		delta, delta_swing_table_idx_tup_a[delta]));
-			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
-			rtldm->delta_power_index[RF90_PATH_A] = delta_swing_table_idx_tup_a[delta];
-
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  delta_swing_table_idx_tup_a[delta];
-			/*Record delta swing for mix mode power tracking*/
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
-
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-	                                     ("delta_swing_table_idx_tup_b[%d] = %d\n",
-	                                     delta, delta_swing_table_idx_tup_b[delta]));
-			rtldm->delta_power_index_last[RF90_PATH_B] = rtldm->delta_power_index[RF90_PATH_B];
-			rtldm->delta_power_index[RF90_PATH_B] = delta_swing_table_idx_tup_b[delta];
-
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B] =  delta_swing_table_idx_tup_b[delta];
-			/*Record delta swing for mix mode power tracking*/
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B]));
-
-        	} else {
-        		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-               	 	("delta_swing_table_idx_tdown_a[%d] = %d\n",
-               	 	delta, delta_swing_table_idx_tdown_a[delta]));
-
-			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
-			rtldm->delta_power_index[RF90_PATH_A] = -1 * delta_swing_table_idx_tdown_a[delta];
-
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  -1 * delta_swing_table_idx_tdown_a[delta];
-			/* Record delta swing for mix mode power tracking*/
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
-
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-                		("deltaSwingTableIdx_TDOWN_B[%d] = %d\n",
-                		delta, delta_swing_table_idx_tdown_b[delta]));
-
-			rtldm->delta_power_index_last[RF90_PATH_B] = rtldm->delta_power_index[RF90_PATH_B];
-			rtldm->delta_power_index[RF90_PATH_B] = -1 * delta_swing_table_idx_tdown_b[delta];
-
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B] =  -1 * delta_swing_table_idx_tdown_b[delta];
-			/*Record delta swing for mix mode power tracking*/
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_B] = %d\n",
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_B]));
- 		}
-
-		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
-        	{
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		                ("\n\n================================ [Path-%c] \
-		                Calculating PowerIndexOffset ================================\n",
-		                (p == RF90_PATH_A ? 'A' : 'B')));
-
-		    	if (rtldm->delta_power_index[p] == rtldm->delta_power_index_last[p])
-				/*If Thermal value changes but lookup table value still the same*/
-		    		rtldm->power_index_offset[p] = 0;
-		    	else
-		    		rtldm->power_index_offset[p] =
-		    			rtldm->delta_power_index[p] - rtldm->delta_power_index_last[p];
-				/*Power Index Diff between 2 times Power Tracking*/
-
-		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		    		("[Path-%c] PowerIndexOffset(%d) = DeltaPowerIndex(%d) - DeltaPowerIndexLast(%d)\n",
-                		(p == RF90_PATH_A ? 'A' : 'B'),
-                		rtldm->power_index_offset[p],
-                		rtldm->delta_power_index[p] ,
-                		rtldm->delta_power_index_last[p]));
-
-	    		rtldm->ofdm_index[p] =
-					rtldm->bb_swing_idx_ofdm_base[p] + rtldm->power_index_offset[p];
-		    	rtldm->cck_index =
-					rtldm->bb_swing_idx_cck_base + rtldm->power_index_offset[p];
-
-		    	rtldm->bb_swing_idx_cck = rtldm->cck_index;
-		   	rtldm->bb_swing_idx_ofdm[p] = rtldm->ofdm_index[p];
-
-	           	/*************Print BB Swing Base and Index Offset*************/
-
-		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		    		("The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
-		    		rtldm->bb_swing_idx_cck,
-		    		rtldm->bb_swing_idx_cck_base,
-		    		rtldm->power_index_offset[p]));
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
-		   		rtldm->bb_swing_idx_ofdm[p],
-		   		(p == RF90_PATH_A ? 'A' : 'B'),
-		   		rtldm->bb_swing_idx_ofdm_base[p],
-		   		rtldm->power_index_offset[p]));
-
-		    	/*7.1 Handle boundary conditions of index.*/
-
-
-			if(rtldm->ofdm_index[p] > TXSCALE_TABLE_SIZE -1)
-			{
-				rtldm->ofdm_index[p] = TXSCALE_TABLE_SIZE -1;
-			}
-			else if (rtldm->ofdm_index[p] < ofdm_min_index)
-			{
-				rtldm->ofdm_index[p] = ofdm_min_index;
-			}
-		}
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-            		("\n\n======================================================\
-            		==================================================\n"));
-		if(rtldm->cck_index > TXSCALE_TABLE_SIZE -1)
-			rtldm->cck_index = TXSCALE_TABLE_SIZE -1;
-		else if (rtldm->cck_index < 0)
-			rtldm->cck_index = 0;
-	} else {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("The thermal meter is unchanged or TxPowerTracking OFF(%d): \
-			ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
-			rtldm->txpower_track_control,
-			thermal_value,
-			rtldm->thermalvalue));
-
-	    	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
-		    	rtldm->power_index_offset[p] = 0;
-	}
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("TxPowerTracking: [CCK] Swing Current Index: %d, Swing Base Index: %d\n",
-		rtldm->cck_index, rtldm->bb_swing_idx_cck_base));       /*Print Swing base & current*/
-	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
-	{
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("TxPowerTracking: [OFDM] Swing Current Index: %d, Swing Base Index[%c]: %d\n",
-			rtldm->ofdm_index[p],
-			(p == RF90_PATH_A ? 'A' : 'B'),
-			rtldm->bb_swing_idx_ofdm_base[p]));
-	}
-
-	if ((rtldm->power_index_offset[RF90_PATH_A] != 0 ||
-		rtldm->power_index_offset[RF90_PATH_B] != 0 ) &&
-     	 	rtldm->txpower_track_control)
-	{
-		/*7.2 Configure the Swing Table to adjust Tx Power.*/
-		/*Always TRUE after Tx Power is adjusted by power tracking.*/
-		/*
-		  2012/04/23 MH According to Luke's suggestion, we can not write BB digital
-		  to increase TX power. Otherwise, EVM will be bad.
-
-		  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E.
-		*/
-		if (thermal_value > rtldm->thermalvalue)
-		{
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature Increasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
-				rtldm->power_index_offset[RF90_PATH_A],
-				delta, thermal_value,
-				rtlefuse->eeprom_thermalmeter,
-				rtldm->thermalvalue));
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature Increasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
-				rtldm->power_index_offset[RF90_PATH_B],
-				delta, thermal_value,
-				rtlefuse->eeprom_thermalmeter,
-				rtldm->thermalvalue));
-
-		} else if (thermal_value < rtldm->thermalvalue) { /*Low temperature*/
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
-				rtldm->power_index_offset[RF90_PATH_A],
-				delta, thermal_value,
-				rtlefuse->eeprom_thermalmeter,
-				rtldm->thermalvalue));
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature Decreasing(B): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
-				rtldm->power_index_offset[RF90_PATH_B],
-				delta, thermal_value,
-				rtlefuse->eeprom_thermalmeter,
-				rtldm->thermalvalue));
-		}
-
-		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature(%d) higher than PG value(%d)\n",
-				thermal_value, rtlefuse->eeprom_thermalmeter));
-
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("**********Enter POWER Tracking MIX_MODE**********\n"));
-			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
-					rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, 0);
-
-		} else {
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature(%d) lower than PG value(%d)\n",
-				thermal_value, rtlefuse->eeprom_thermalmeter));
-
-
-	            	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("**********Enter POWER Tracking MIX_MODE**********\n"));
-			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
-				rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, index_for_channel);
-
-		}
-
-		rtldm->bb_swing_idx_cck_base = rtldm->bb_swing_idx_cck;   /*Record last time Power Tracking result as base.*/
-		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8812A; p++)
-				rtldm->bb_swing_idx_ofdm_base[p] = rtldm->bb_swing_idx_ofdm[p];
-
-	 		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("pDM_Odm->RFCalibrateInfo.ThermalValue = %d ThermalValue= %d\n",
-					rtldm->thermalvalue, thermal_value));
-
-		rtldm->thermalvalue = thermal_value;         /*Record last Power Tracking Thermal Value*/
-
-	}
-	/*Delta temperature is equal to or larger than 20 centigrade (When threshold is 8).*/
-	if ((delta_iqk >= IQK_THRESHOLD)) {
-
-		if ( !rtlphy->b_iqk_in_progress) {
-
-			spin_lock(&rtlpriv->locks.iqk_lock);
-			rtlphy->b_iqk_in_progress = true;
-			spin_unlock(&rtlpriv->locks.iqk_lock);
-
-			rtl8812ae_do_iqk(hw, delta_iqk, thermal_value, 8);
-
-			spin_lock(&rtlpriv->locks.iqk_lock);
-			rtlphy->b_iqk_in_progress = false;
-			spin_unlock(&rtlpriv->locks.iqk_lock);
-		}
-	}
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("<===rtl8812ae_dm_txpower_tracking_callback_thermalmeter\n"));
-}
-
-
-void rtl8821ae_get_delta_swing_table(
-	struct ieee80211_hw *hw,
-	u8 **temperature_up_a,
-	u8 **temperature_down_a,
-	u8 **temperature_up_b,
-	u8 **temperature_down_b
-	)
-{
-   	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	u8 channel = rtlphy->current_channel;
-	u8 rate = rtldm->tx_rate;
-
-
-	if ( 1 <= channel && channel <= 14) {
-		if (RX_HAL_IS_CCK_RATE(rate)) {
-		        *temperature_up_a = rtldm->delta_swing_table_idx_24gccka_p;
-		        *temperature_down_a = rtldm->delta_swing_table_idx_24gccka_n;
-		        *temperature_up_b = rtldm->delta_swing_table_idx_24gcckb_p;
-		        *temperature_down_b = rtldm->delta_swing_table_idx_24gcckb_n;
-		} else {
-		        *temperature_up_a = rtldm->delta_swing_table_idx_24ga_p;
-		        *temperature_down_a = rtldm->delta_swing_table_idx_24ga_n;
-		        *temperature_up_b = rtldm->delta_swing_table_idx_24gb_p;
-		        *temperature_down_b = rtldm->delta_swing_table_idx_24gb_n;
-		}
- 	} else if ( 36 <= channel && channel <= 64) {
-	        *temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[0];
-	        *temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[0];
-	        *temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[0];
-	        *temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[0];
-    	} else if ( 100 <= channel && channel <= 140) {
-		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[1];
-		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[1];
-		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[1];
-		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[1];
-    	} else if ( 149 <= channel && channel <= 173) {
-		*temperature_up_a = rtldm->delta_swing_table_idx_5ga_p[2];
-		*temperature_down_a = rtldm->delta_swing_table_idx_5ga_n[2];
-		*temperature_up_b = rtldm->delta_swing_table_idx_5gb_p[2];
-		*temperature_down_b = rtldm->delta_swing_table_idx_5gb_n[2];
-    	} else {
-	    *temperature_up_a = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
-	    *temperature_down_a =(u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
-	    *temperature_up_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_p_txpwrtrack;
-	    *temperature_down_b = (u8*)rtl8818e_delta_swing_table_idx_24gb_n_txpwrtrack;
-    	}
-
-	return;
-}
-
-void rtl8821ae_phy_lccalibrate(
-	struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("===> rtl8812ae_phy_lccalibrate\n"));
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("<=== rtl8812ae_phy_lccalibrate\n"));
-
-}
-
-/*-----------------------------------------------------------------------------
- * Function:	odm_TxPwrTrackSetPwr88E()
- *
- * Overview:	88E change all channel tx power according to flag.
- *				OFDM & CCK are all different.
- *
- * Input:		NONE
- *
- * Output:		NONE
- *
- * Return:		NONE
- *
- * Revised History:
- *	When		Who		Remark
- *	04/23/2012	MHC		Create Version 0.
- *
- *---------------------------------------------------------------------------*/
-void rtl8821ae_dm_txpwr_track_set_pwr(struct ieee80211_hw *hw,
-	enum pwr_track_control_method method, u8 rf_path, u8 channel_mapped_index)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u32 final_bb_swing_idx[1];
-	u8 pwr_tracking_limit = 26; /*+1.0dB*/
-	u8 tx_rate = 0xFF;
-	s8 final_ofdm_swing_index = 0;
-
-	if(rtldm->tx_rate != 0xFF)
-		tx_rate = rtl8812ae_hw_rate_to_mrate(hw, rtldm->tx_rate);
-
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
-
-	if(tx_rate != 0xFF) { /*20130429 Mimic Modify High Rate BBSwing Limit.*/
-		/*CCK*/
-		if((tx_rate >= MGN_1M) && (tx_rate <= MGN_11M))
-			pwr_tracking_limit = 32; /*+4dB*/
-		/*OFDM*/
-		else if((tx_rate >= MGN_6M) && (tx_rate <= MGN_48M))
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if(tx_rate == MGN_54M)
-			pwr_tracking_limit = 28; /*+2dB*/
-		/*HT*/
-		else if((tx_rate >= MGN_MCS0) && (tx_rate <= MGN_MCS2)) /*QPSK/BPSK*/
-			pwr_tracking_limit = 34; /*+5dB*/
-		else if((tx_rate >= MGN_MCS3) && (tx_rate <= MGN_MCS4)) /*16QAM*/
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if((tx_rate >= MGN_MCS5) && (tx_rate <= MGN_MCS7)) /*64QAM*/
-			pwr_tracking_limit = 28; /*+2dB*/
-#if 0
-		else if((tx_rate >= MGN_MCS8) && (tx_rate <= MGN_MCS10)) /*QPSK/BPSK*/
-			pwr_tracking_limit = 34; /*+5dB*/
-		else if((tx_rate >= MGN_MCS11) && (tx_rate <= MGN_MCS12)) /*16QAM*/
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if((tx_rate >= MGN_MCS13) && (tx_rate <= MGN_MCS15)) /*64QAM*/
-			pwr_tracking_limit = 28; /*+2dB*/
-#endif
-		/*2 VHT*/
-		else if((tx_rate >= MGN_VHT1SS_MCS0) && (tx_rate <= MGN_VHT1SS_MCS2)) /*QPSK/BPSK*/
-			pwr_tracking_limit = 34; /*+5dB*/
-		else if((tx_rate >= MGN_VHT1SS_MCS3) && (tx_rate <= MGN_VHT1SS_MCS4)) /*16QAM*/
-			pwr_tracking_limit = 30; /*+3dB*/
-		else if((tx_rate >= MGN_VHT1SS_MCS5)&&(tx_rate <= MGN_VHT1SS_MCS6)) /*64QAM*/
-			pwr_tracking_limit = 28; /*+2dB*/
-		else if(tx_rate == MGN_VHT1SS_MCS7) /*64QAM*/
-			pwr_tracking_limit = 26; /*+1dB*/
-		else if(tx_rate == MGN_VHT1SS_MCS8) /*256QAM*/
-			pwr_tracking_limit = 24; /*+0dB*/
-		else if(tx_rate == MGN_VHT1SS_MCS9) /*256QAM*/
-			pwr_tracking_limit = 22; /*-1dB*/
-		else
-			pwr_tracking_limit = 24;
-	}
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("TxRate=0x%x, PwrTrackingLimit=%d\n", tx_rate, pwr_tracking_limit));
-
-
-	if (method == BBSWING) {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("===>rtl8812ae_dm_txpwr_track_set_pwr\n"));
-
-		if (rf_path == RF90_PATH_A) {
-			final_bb_swing_idx[RF90_PATH_A] =
-				(rtldm->ofdm_index[RF90_PATH_A] > pwr_tracking_limit) ?
-				pwr_tracking_limit : rtldm->ofdm_index[RF90_PATH_A];
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("pDM_Odm->RFCalibrateInfo.OFDM_index[ODM_RF_PATH_A]=%d, \
-				pDM_Odm->RealBbSwingIdx[ODM_RF_PATH_A]=%d\n",
-				rtldm->ofdm_index[RF90_PATH_A], final_bb_swing_idx[RF90_PATH_A]));
-
-			rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_bb_swing_idx[RF90_PATH_A]]);
-		}
-	} else if (method == MIX_MODE) {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("pDM_Odm->DefaultOfdmIndex=%d, \
-			pDM_Odm->Aboslute_OFDMSwingIdx[RFPath]=%d, RF_Path = %d\n",
-			rtldm->default_ofdm_index, rtldm->aboslute_ofdm_swing_idx[rf_path],
-			rf_path ));
-
-
-		final_ofdm_swing_index = rtldm->default_ofdm_index + rtldm->aboslute_ofdm_swing_idx[rf_path];
-
-		if (rf_path == RF90_PATH_A) {
-			if(final_ofdm_swing_index > pwr_tracking_limit) {     /*BBSwing higher then Limit*/
-
-				rtldm->remnant_cck_idx = final_ofdm_swing_index - pwr_tracking_limit;
-				/* CCK Follow the same compensate value as Path A*/
-				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index - pwr_tracking_limit;
-
-				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[pwr_tracking_limit]);
-
-				rtldm->modify_txagc_flag_path_a = true;
-
-				/*Set TxAGC Page C{};*/
-				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_A Over BBSwing Limit , PwrTrackingLimit = %d , Remnant TxAGC Value = %d \n",
-					pwr_tracking_limit, rtldm->remnant_ofdm_swing_idx[rf_path]));
-			} else if (final_ofdm_swing_index < 0) {
-				rtldm->remnant_cck_idx = final_ofdm_swing_index;
-				/* CCK Follow the same compensate value as Path A*/
-				rtldm->remnant_ofdm_swing_idx[rf_path] = final_ofdm_swing_index;
-
-				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[0]);
-
-				rtldm->modify_txagc_flag_path_a = true;
-
-				/*Set TxAGC Page C{};*/
-				rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_A Lower then BBSwing lower bound  0 , Remnant TxAGC Value = %d \n",
-					 rtldm->remnant_ofdm_swing_idx[rf_path]));
-			} else {
-				rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000, rtl8812ae_txscaling_table[final_ofdm_swing_index]);
-
-				RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("******Path_A Compensate with BBSwing , Final_OFDM_Swing_Index = %d \n",
-					final_ofdm_swing_index));
-
-				if(rtldm->modify_txagc_flag_path_a) { /*If TxAGC has changed, reset TxAGC again*/
-					rtldm->remnant_cck_idx = 0;
-					rtldm->remnant_ofdm_swing_idx[rf_path] = 0;
-
-					/*Set TxAGC Page C{};*/
-					rtl8821ae_phy_set_txpower_level_by_path(hw, rtlphy->current_channel, RF90_PATH_A);
-
-					rtldm->modify_txagc_flag_path_a = false;
-
-					RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-						("******Path_A pDM_Odm->Modify_TxAGC_Flag = FALSE \n"));
-				}
-			}
-		}
-
-	} else {
-		return;
-	}
-}
-
-
-void rtl8821ae_dm_txpower_tracking_callback_thermalmeter
-	(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	u8 thermal_value = 0, delta, delta_lck, delta_iqk, p = 0, i = 0;
-	u8 thermal_value_avg_count = 0;
-	u32 thermal_value_avg = 0;
-
-	u8 ofdm_min_index = 6;  /*OFDM BB Swing should be less than +3.0dB, which is required by Arthur*/
-	u8 index_for_channel = 0; /* GetRightChnlPlaceforIQK(pHalData->CurrentChannel)*/
-
-	/* 1. The following TWO tables decide the final index of OFDM/CCK swing table.*/
-	u8 *delta_swing_table_idx_tup_a;
-	u8 *delta_swing_table_idx_tdown_a;
-	u8 *delta_swing_table_idx_tup_b;
-	u8 *delta_swing_table_idx_tdown_b;
-
-	/*2. Initialization ( 7 steps in total )*/
-	rtl8821ae_get_delta_swing_table(hw, (u8**)&delta_swing_table_idx_tup_a,
-									(u8**)&delta_swing_table_idx_tdown_a,
-									  (u8**)&delta_swing_table_idx_tup_b,
-									  (u8**)&delta_swing_table_idx_tdown_b);
-
-	rtldm->btxpower_trackinginit = true;
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("===>rtl8812ae_dm_txpower_tracking_callback_thermalmeter, \
-		 \n pDM_Odm->BbSwingIdxCckBase: %d, pDM_Odm->BbSwingIdxOfdmBase[A]:\
-		 %d, pDM_Odm->DefaultOfdmIndex: %d\n",
-		rtldm->bb_swing_idx_cck_base,
-		rtldm->bb_swing_idx_ofdm_base[RF90_PATH_A],
-		rtldm->default_ofdm_index));
-
-	thermal_value = (u8)rtl_get_rfreg(hw, RF90_PATH_A, RF_T_METER_8812A, 0xfc00);	/*0x42: RF Reg[15:10] 88E*/
-	if( ! rtldm->txpower_track_control || rtlefuse->eeprom_thermalmeter == 0 ||
-		rtlefuse->eeprom_thermalmeter == 0xFF)
-        	return;
-
-
-	/* 3. Initialize ThermalValues of RFCalibrateInfo*/
-
-	if(rtlhal->reloadtxpowerindex)
-	{
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("reload ofdm index for band switch\n"));
-	}
-
-	/*4. Calculate average thermal meter*/
-	rtldm->thermalvalue_avg[rtldm->thermalvalue_avg_index] = thermal_value;
-	rtldm->thermalvalue_avg_index++;
-	if(rtldm->thermalvalue_avg_index == AVG_THERMAL_NUM_8812A)
-		/*Average times =  c.AverageThermalNum*/
-		rtldm->thermalvalue_avg_index = 0;
-
-	for(i = 0; i < AVG_THERMAL_NUM_8812A; i++)
-	{
-		if(rtldm->thermalvalue_avg[i])
-		{
-			thermal_value_avg += rtldm->thermalvalue_avg[i];
-			thermal_value_avg_count++;
-		}
-	}
-
-	if(thermal_value_avg_count) /*Calculate Average ThermalValue after average enough times*/
-	{
-		thermal_value = (u8)(thermal_value_avg / thermal_value_avg_count);
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("AVG Thermal Meter = 0x%X, EFUSE Thermal Base = 0x%X\n",
-			thermal_value, rtlefuse->eeprom_thermalmeter));
-	}
-
-	/*5. Calculate delta, delta_LCK, delta_IQK.*/
-	/*"delta" here is used to determine whether thermal value changes or not.*/
-	delta = (thermal_value > rtldm->thermalvalue) ? \
-		(thermal_value - rtldm->thermalvalue): \
-		(rtldm->thermalvalue - thermal_value);
-	delta_lck = (thermal_value > rtldm->thermalvalue_lck) ? \
-		(thermal_value - rtldm->thermalvalue_lck) : \
-		(rtldm->thermalvalue_lck - thermal_value);
-	delta_iqk = (thermal_value > rtldm->thermalvalue_iqk) ? \
-		(thermal_value - rtldm->thermalvalue_iqk) : \
-		(rtldm->thermalvalue_iqk - thermal_value);
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("(delta, delta_LCK, delta_IQK) = (%d, %d, %d)\n",
-		delta, delta_lck, delta_iqk));
-
-	/* 6. If necessary, do LCK.	*/
-
-	if (delta_lck >= IQK_THRESHOLD) /*Delta temperature is equal to or larger than 20 centigrade.*/
-	{
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("delta_LCK(%d) >= Threshold_IQK(%d)\n",
-			delta_lck, IQK_THRESHOLD));
-		rtldm->thermalvalue_lck = thermal_value;
-		rtl8821ae_phy_lccalibrate(hw);
-	}
-
-	/*7. If necessary, move the index of swing table to adjust Tx power.*/
-
-	if (delta > 0 && rtldm->txpower_track_control)
-	{
-		/*"delta" here is used to record the absolute value of difference.*/
-	    	delta = thermal_value > rtlefuse->eeprom_thermalmeter ? \
-		    	(thermal_value - rtlefuse->eeprom_thermalmeter) : \
-		    	(rtlefuse->eeprom_thermalmeter - thermal_value);
-
-		if (delta >= TXSCALE_TABLE_SIZE)
-			delta = TXSCALE_TABLE_SIZE - 1;
-
-		/*7.1 The Final Power Index = BaseIndex + PowerIndexOffset*/
-
-		if(thermal_value > rtlefuse->eeprom_thermalmeter) {
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			    		("delta_swing_table_idx_tup_a[%d] = %d\n",
-			    		delta, delta_swing_table_idx_tup_a[delta]));
-			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
-			rtldm->delta_power_index[RF90_PATH_A] = delta_swing_table_idx_tup_a[delta];
-
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  delta_swing_table_idx_tup_a[delta];
-			/*Record delta swing for mix mode power tracking*/
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("******Temp is higher and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
-
-        	} else {
-        		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-               	 	("delta_swing_table_idx_tdown_a[%d] = %d\n",
-               	 	delta, delta_swing_table_idx_tdown_a[delta]));
-
-			rtldm->delta_power_index_last[RF90_PATH_A] = rtldm->delta_power_index[RF90_PATH_A];
-			rtldm->delta_power_index[RF90_PATH_A] = -1 * delta_swing_table_idx_tdown_a[delta];
-
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A] =  -1 * delta_swing_table_idx_tdown_a[delta];
-			/* Record delta swing for mix mode power tracking*/
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("******Temp is lower and pDM_Odm->Aboslute_OFDMSwingIdx[ODM_RF_PATH_A] = %d\n",
-			rtldm->aboslute_ofdm_swing_idx[RF90_PATH_A]));
- 		}
-
-		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
-        	{
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		                ("\n\n================================ [Path-%c] \
-		                Calculating PowerIndexOffset ================================\n",
-		                (p == RF90_PATH_A ? 'A' : 'B')));
-
-		    	if (rtldm->delta_power_index[p] == rtldm->delta_power_index_last[p])
-				/*If Thermal value changes but lookup table value still the same*/
-		    		rtldm->power_index_offset[p] = 0;
-		    	else
-		    		rtldm->power_index_offset[p] =
-		    			rtldm->delta_power_index[p] - rtldm->delta_power_index_last[p];
-				/*Power Index Diff between 2 times Power Tracking*/
-
-		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		    		("[Path-%c] PowerIndexOffset(%d) = DeltaPowerIndex(%d) - DeltaPowerIndexLast(%d)\n",
-                		(p == RF90_PATH_A ? 'A' : 'B'),
-                		rtldm->power_index_offset[p],
-                		rtldm->delta_power_index[p] ,
-                		rtldm->delta_power_index_last[p]));
-
-	    		rtldm->ofdm_index[p] =
-					rtldm->bb_swing_idx_ofdm_base[p] + rtldm->power_index_offset[p];
-		    	rtldm->cck_index =
-					rtldm->bb_swing_idx_cck_base + rtldm->power_index_offset[p];
-
-		    	rtldm->bb_swing_idx_cck = rtldm->cck_index;
-		   	rtldm->bb_swing_idx_ofdm[p] = rtldm->ofdm_index[p];
-
-	           	/*************Print BB Swing Base and Index Offset*************/
-
-		    	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		    		("The 'CCK' final index(%d) = BaseIndex(%d) + PowerIndexOffset(%d)\n",
-		    		rtldm->bb_swing_idx_cck,
-		    		rtldm->bb_swing_idx_cck_base,
-		    		rtldm->power_index_offset[p]));
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("The 'OFDM' final index(%d) = BaseIndex[%c](%d) + PowerIndexOffset(%d)\n",
-		   		rtldm->bb_swing_idx_ofdm[p],
-		   		(p == RF90_PATH_A ? 'A' : 'B'),
-		   		rtldm->bb_swing_idx_ofdm_base[p],
-		   		rtldm->power_index_offset[p]));
-
-		    	/*7.1 Handle boundary conditions of index.*/
-
-
-			if(rtldm->ofdm_index[p] > TXSCALE_TABLE_SIZE -1)
-			{
-				rtldm->ofdm_index[p] = TXSCALE_TABLE_SIZE -1;
-			}
-			else if (rtldm->ofdm_index[p] < ofdm_min_index)
-			{
-				rtldm->ofdm_index[p] = ofdm_min_index;
-			}
-		}
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-            		("\n\n======================================================\
-            		==================================================\n"));
-		if(rtldm->cck_index > TXSCALE_TABLE_SIZE -1)
-			rtldm->cck_index = TXSCALE_TABLE_SIZE -1;
-		else if (rtldm->cck_index < 0)
-			rtldm->cck_index = 0;
-	} else {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("The thermal meter is unchanged or TxPowerTracking OFF(%d): \
-			ThermalValue: %d , pDM_Odm->RFCalibrateInfo.ThermalValue: %d\n",
-			rtldm->txpower_track_control,
-			thermal_value,
-			rtldm->thermalvalue));
-
-	    	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
-		    	rtldm->power_index_offset[p] = 0;
-	}
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("TxPowerTracking: [CCK] Swing Current Index: %d, Swing Base Index: %d\n",
-		rtldm->cck_index, rtldm->bb_swing_idx_cck_base));       /*Print Swing base & current*/
-	for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
-	{
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			("TxPowerTracking: [OFDM] Swing Current Index: %d, Swing Base Index[%c]: %d\n",
-			rtldm->ofdm_index[p],
-			(p == RF90_PATH_A ? 'A' : 'B'),
-			rtldm->bb_swing_idx_ofdm_base[p]));
-	}
-
-	if ((rtldm->power_index_offset[RF90_PATH_A] != 0 ||
-		rtldm->power_index_offset[RF90_PATH_B] != 0 ) &&
-     	 	rtldm->txpower_track_control)
-	{
-		/*7.2 Configure the Swing Table to adjust Tx Power.*/
-		/*Always TRUE after Tx Power is adjusted by power tracking.*/
-		/*
-		  2012/04/23 MH According to Luke's suggestion, we can not write BB digital
-		  to increase TX power. Otherwise, EVM will be bad.
-
-		  2012/04/25 MH Add for tx power tracking to set tx power in tx agc for 88E.
-		*/
-		if (thermal_value > rtldm->thermalvalue)
-		{
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature Increasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
-				rtldm->power_index_offset[RF90_PATH_A],
-				delta, thermal_value,
-				rtlefuse->eeprom_thermalmeter,
-				rtldm->thermalvalue));
-		} else if (thermal_value < rtldm->thermalvalue) { /*Low temperature*/
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature Decreasing(A): delta_pi: %d , delta_t: %d, Now_t: %d, EFUSE_t: %d, Last_t: %d\n",
-				rtldm->power_index_offset[RF90_PATH_A],
-				delta, thermal_value,
-				rtlefuse->eeprom_thermalmeter,
-				rtldm->thermalvalue));
-		}
-
-		if (thermal_value > rtlefuse->eeprom_thermalmeter) {
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature(%d) higher than PG value(%d)\n",
-				thermal_value, rtlefuse->eeprom_thermalmeter));
-
-
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("**********Enter POWER Tracking MIX_MODE**********\n"));
-			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
-					rtl8821ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, index_for_channel);
-
-		} else {
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("Temperature(%d) lower than PG value(%d)\n",
-				thermal_value, rtlefuse->eeprom_thermalmeter));
-
-
-	            	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-				("**********Enter POWER Tracking MIX_MODE**********\n"));
-			for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
-				rtl8812ae_dm_txpwr_track_set_pwr(hw, MIX_MODE, p, index_for_channel);
-
-		}
-
-		rtldm->bb_swing_idx_cck_base = rtldm->bb_swing_idx_cck;   /*Record last time Power Tracking result as base.*/
-		for (p = RF90_PATH_A; p < MAX_PATH_NUM_8821A; p++)
-				rtldm->bb_swing_idx_ofdm_base[p] = rtldm->bb_swing_idx_ofdm[p];
-
-	 		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-					("pDM_Odm->RFCalibrateInfo.ThermalValue = %d ThermalValue= %d\n",
-					rtldm->thermalvalue, thermal_value));
-
-		rtldm->thermalvalue = thermal_value;         /*Record last Power Tracking Thermal Value*/
-
-	}
-	/*Delta temperature is equal to or larger than 20 centigrade (When threshold is 8).*/
-	if ((delta_iqk >= IQK_THRESHOLD)) {
-
-		if ( !rtlphy->b_iqk_in_progress) {
-
-			spin_lock(&rtlpriv->locks.iqk_lock);
-			rtlphy->b_iqk_in_progress = true;
-			spin_unlock(&rtlpriv->locks.iqk_lock);
-
-			rtl8821ae_do_iqk(hw, delta_iqk, thermal_value, 8);
-
-			spin_lock(&rtlpriv->locks.iqk_lock);
-			rtlphy->b_iqk_in_progress = false;
-			spin_unlock(&rtlpriv->locks.iqk_lock);
-		}
-	}
-
-	RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-		("<===rtl8812ae_dm_txpower_tracking_callback_thermalmeter\n"));
-}
-
-
-void rtl8821ae_dm_check_txpower_tracking_thermalmeter(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	static u8 tm_trigger = 0;
-
-	//if (!rtlpriv->dm.btxpower_tracking)
-	//	return;
-
-	if (!tm_trigger) {
-		rtl_set_rfreg(hw, RF90_PATH_A, RF_T_METER_88E, BIT(17)|BIT(16),
-			      0x03);
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			 ("Trigger 8821ae Thermal Meter!!\n"));
-		tm_trigger = 1;
-		return;
-	} else {
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD,
-			 ("Schedule TxPowerTracking !!\n"));
-
-		rtl8821ae_dm_txpower_tracking_callback_thermalmeter(hw);
-		tm_trigger = 0;
-	}
-}
-
-
-void rtl8821ae_dm_refresh_rate_adaptive_mask(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rate_adaptive *p_ra = &(rtlpriv->ra);
-	u32 low_rssithresh_for_ra = p_ra->low2high_rssi_thresh_for_ra;
-	u32 high_rssithresh_for_ra = p_ra->high_rssi_thresh_for_ra;
-	u8 go_up_gap = 5;
-	struct ieee80211_sta *sta = NULL;
-
-	if (is_hal_stop(rtlhal)) {
-		RT_TRACE(COMP_RATE, DBG_LOUD,
-			 ("driver is going to unload\n"));
-		return;
-	}
-
-	if (!rtlpriv->dm.b_useramask) {
-		RT_TRACE(COMP_RATE, DBG_LOUD,
-			 ("driver does not control rate adaptive mask\n"));
-		return;
-	}
-
-	if (mac->link_state == MAC80211_LINKED &&
-		mac->opmode == NL80211_IFTYPE_STATION) {
-
-		switch (p_ra->pre_ratr_state) {
-			case DM_RATR_STA_MIDDLE:
-				high_rssithresh_for_ra += go_up_gap;
-				break;
-			case DM_RATR_STA_LOW:
-				high_rssithresh_for_ra += go_up_gap;
-				low_rssithresh_for_ra += go_up_gap;
-				break;
-			default:
-				break;
-		}
-
-		if (rtlpriv->dm.undecorated_smoothed_pwdb >
-		    (long)high_rssithresh_for_ra)
-			p_ra->ratr_state = DM_RATR_STA_HIGH;
-		else if (rtlpriv->dm.undecorated_smoothed_pwdb >
-			 (long)low_rssithresh_for_ra)
-			p_ra->ratr_state = DM_RATR_STA_MIDDLE;
-		else
-			p_ra->ratr_state = DM_RATR_STA_LOW;
-
-		if (p_ra->pre_ratr_state != p_ra->ratr_state ) {
-			RT_TRACE(COMP_RATE, DBG_LOUD,
-				 ("RSSI = %ld\n",
-				  rtlpriv->dm.undecorated_smoothed_pwdb));
-			RT_TRACE(COMP_RATE, DBG_LOUD,
-				 ("RSSI_LEVEL = %d\n", p_ra->ratr_state));
-			RT_TRACE(COMP_RATE, DBG_LOUD,
-				 ("PreState = %d, CurState = %d\n",
-				  p_ra->pre_ratr_state, p_ra->ratr_state));
-
-			rcu_read_lock();
-			sta = rtl_find_sta(hw, mac->bssid);
-			if (sta)
-			rtlpriv->cfg->ops->update_rate_tbl(hw, sta, p_ra->ratr_state);
-			rcu_read_unlock();
-
-			p_ra->pre_ratr_state = p_ra->ratr_state;
-		}
-	}
-}
-
-bool rtl8821ae_dm_is_edca_turbo_disable(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->btcoexist.btc_ops->btc_is_disable_edca_turbo(rtlpriv))
-		return true;
-	if (rtlpriv->mac80211.mode == WIRELESS_MODE_B)
-		return true;
-
-	return false;
-}
-
-void rtl8821ae_dm_edca_choose_traffic_idx(
-	struct ieee80211_hw *hw, u64 cur_tx_bytes, u64 cur_rx_bytes, bool b_bias_on_rx,
-	bool *pb_is_cur_rdl_state)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if(b_bias_on_rx)
-	{
-		if (cur_tx_bytes > (cur_rx_bytes*4)) {
-			*pb_is_cur_rdl_state = false;
-			RT_TRACE(COMP_TURBO, DBG_LOUD,
-				("Uplink Traffic\n "));
-		} else {
-			*pb_is_cur_rdl_state = true;
-			RT_TRACE(COMP_TURBO, DBG_LOUD,
-				("Balance Traffic\n"));
-		}
-	} else {
-		if (cur_rx_bytes > (cur_tx_bytes*4)) {
-			*pb_is_cur_rdl_state = true;
-			RT_TRACE(COMP_TURBO, DBG_LOUD,
-				("Downlink	Traffic\n"));
-		} else {
-			*pb_is_cur_rdl_state = false;
-			RT_TRACE(COMP_TURBO, DBG_LOUD,
-				("Balance Traffic\n"));
-		}
-	}
-	return ;
-}
-
-static void rtl8821ae_dm_check_edca_turbo(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_dm *rtldm =  rtl_dm(rtl_priv(hw));
-
-	/*Keep past Tx/Rx packet count for RT-to-RT EDCA turbo.*/
-	unsigned long cur_tx_ok_cnt = 0;
-	unsigned long cur_rx_ok_cnt = 0;
-	u32 edca_be_ul = 0x5ea42b;
-	u32 edca_be_dl = 0x5ea42b;
-	u32 edca_be = 0x5ea42b;
-	u8 iot_peer = 0;
-	bool *pb_is_cur_rdl_state = NULL;
-	bool b_last_is_cur_rdl_state = false;
-	bool b_bias_on_rx = false;
-	bool b_edca_turbo_on = false;
-
-	RT_TRACE(COMP_TURBO, DBG_LOUD,
-		("rtl8821ae_dm_check_edca_turbo=====>"));
-	RT_TRACE(COMP_TURBO, DBG_LOUD,
-		("Original BE PARAM: 0x%x\n",
-		rtl_read_dword(rtlpriv, DM_REG_EDCA_BE_11N)));
-
-	/*===============================
-	list parameter for different platform
-	===============================*/
-	b_last_is_cur_rdl_state = rtlpriv->dm.bis_cur_rdlstate;
-	pb_is_cur_rdl_state = &( rtlpriv->dm.bis_cur_rdlstate);
-
-	cur_tx_ok_cnt = rtlpriv->stats.txbytesunicast - rtldm->last_tx_ok_cnt;
-	cur_rx_ok_cnt = rtlpriv->stats.rxbytesunicast - rtldm->last_rx_ok_cnt;
-
-	rtldm->last_tx_ok_cnt = rtlpriv->stats.txbytesunicast;
-	rtldm->last_rx_ok_cnt = rtlpriv->stats.rxbytesunicast;
-
-	iot_peer = rtlpriv->mac80211.vendor;
-	b_bias_on_rx = (iot_peer == PEER_RAL || iot_peer == PEER_ATH) ?
-		       true : false;
-	b_edca_turbo_on = ((!rtlpriv->dm.bis_any_nonbepkts) &&
-			   (!rtlpriv->dm.b_disable_framebursting)) ?
-			   true : false;
-
-	/*if (rtl8821ae_dm_is_edca_turbo_disable(hw))
-		goto dm_CheckEdcaTurbo_EXIT;*/
-
-	if ((iot_peer == PEER_CISCO) && (mac->mode == WIRELESS_MODE_N_24G))
-	{
-		edca_be_dl = edca_setting_dl[iot_peer];
-		edca_be_ul = edca_setting_ul[iot_peer];
-	}
-
-	RT_TRACE(COMP_TURBO, DBG_LOUD,
-		("bIsAnyNonBEPkts : 0x%x  bDisableFrameBursting : 0x%x  \n",
-		rtlpriv->dm.bis_any_nonbepkts, rtlpriv->dm.b_disable_framebursting));
-
-	RT_TRACE(COMP_TURBO, DBG_LOUD,
-			("bEdcaTurboOn : 0x%x bBiasOnRx : 0x%x\n",
-			b_edca_turbo_on, b_bias_on_rx));
-
-	if (b_edca_turbo_on) {
-		RT_TRACE(COMP_TURBO, DBG_LOUD,
-			("curTxOkCnt : 0x%lx \n",cur_tx_ok_cnt));
-		RT_TRACE(COMP_TURBO, DBG_LOUD,
-			("curRxOkCnt : 0x%lx \n",cur_rx_ok_cnt));
-		if(b_bias_on_rx)
-			rtl8821ae_dm_edca_choose_traffic_idx(hw, cur_tx_ok_cnt,
-				cur_rx_ok_cnt, true, pb_is_cur_rdl_state);
-		else
-			rtl8821ae_dm_edca_choose_traffic_idx(hw, cur_tx_ok_cnt,
-				cur_rx_ok_cnt, false, pb_is_cur_rdl_state);
-
-		edca_be = ((*pb_is_cur_rdl_state) == true) ? edca_be_dl : edca_be_ul;
-
-		rtl_write_dword(rtlpriv, DM_REG_EDCA_BE_11N, edca_be);
-
-		RT_TRACE(COMP_TURBO, DBG_LOUD,
-			("EDCA Turbo on: EDCA_BE:0x%x\n", edca_be));
-
-		rtlpriv->dm.bcurrent_turbo_edca = true;
-
-		RT_TRACE(COMP_TURBO, DBG_LOUD,
-			("EDCA_BE_DL : 0x%x  EDCA_BE_UL : 0x%x  EDCA_BE : 0x%x  \n",
-			edca_be_dl, edca_be_ul, edca_be));
-	} else {
-		if (rtlpriv->dm.bcurrent_turbo_edca) {
-			u8 tmp = AC0_BE;
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AC_PARAM,
-						      (u8 *) (&tmp));
-		}
-		rtlpriv->dm.bcurrent_turbo_edca = false;
-	}
-
-/* dm_CheckEdcaTurbo_EXIT: */
-	rtlpriv->dm.bis_any_nonbepkts = false;
-	rtldm->last_tx_ok_cnt = rtlpriv->stats.txbytesunicast;
-	rtldm->last_rx_ok_cnt = rtlpriv->stats.rxbytesunicast;
-}
-
-static void rtl8821ae_dm_cck_packet_detection_thresh(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 cur_cck_cca_thresh;
-
-	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-		/*dm_digtable.rssi_val_min = rtl8821ae_dm_initial_gain_min_pwdb(hw);*/
-		if (dm_digtable.rssi_val_min > 25)
-			cur_cck_cca_thresh = 0xcd;
-		else if ((dm_digtable.rssi_val_min <= 25) && (dm_digtable.rssi_val_min > 10))
-			cur_cck_cca_thresh = 0x83;
-		else {
-			if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
-				cur_cck_cca_thresh = 0x83;
-			else
-				cur_cck_cca_thresh = 0x40;
-		}
-
-	} else {
-		if (rtlpriv->falsealm_cnt.cnt_cck_fail > 1000)
-			cur_cck_cca_thresh = 0x83;
-		else
-			cur_cck_cca_thresh = 0x40;
-	}
-
-	if (dm_digtable.cur_cck_cca_thres != cur_cck_cca_thresh) {
-		rtl_set_bbreg(hw, RCCK0_CCA, MASKBYTE2, cur_cck_cca_thresh);
-	}
-
-	dm_digtable.pre_cck_cca_thres = dm_digtable.cur_cck_cca_thres;
-	dm_digtable.cur_cck_cca_thres = cur_cck_cca_thresh;
-	RT_TRACE(COMP_DIG, DBG_TRACE,
-		 ("CCK cca thresh hold =%x\n", dm_digtable.cur_cck_cca_thres));
-
-}
-
-void rtl8821ae_dm_dynamic_edcca(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool b_fw_current_in_ps_mode = false;
-
-	rtlpriv->cfg->ops->get_hw_reg(hw,HW_VAR_FW_PSMODE_STATUS, \
-		(u8*)(&b_fw_current_in_ps_mode));
-	if (b_fw_current_in_ps_mode)
-		return;
-}
-
-void rtl8812ae_dm_update_txpath(struct ieee80211_hw *hw, u8 path)
-{
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtldm->resp_tx_path != path) {
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("Need to Update Tx Path\n"));
-		if (path == RF90_PATH_A) {
-			/*Tx by Reg*/
-			rtl_set_bbreg(hw, 0x80c, 0xFFF0, 0x111);
-			 /*Resp Tx by Txinfo*/
-			rtl_set_bbreg(hw, 0x6d8, BIT(7) | BIT(6), 1);
-		} else {
-			/*Tx by Reg*/
-			rtl_set_bbreg(hw, 0x80c, 0xFFF0, 0x222);
-			 /*Resp Tx by Txinfo*/
-			rtl_set_bbreg(hw, 0x6d8, BIT(7) |BIT(6), 2);
-		}
-	}
-	rtldm->resp_tx_path = path;
-	RT_TRACE(COMP_DIG, DBG_LOUD, \
-		("Path=%s\n",(path == RF90_PATH_A) ?  \
-		"RF90_PATH_A":"RF90_PATH_A"));
-}
-
-void rtl8812ae_dm_path_diversity_init(struct ieee80211_hw *hw)
-{
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-
-	//rtl_set_bbreg(hw, 0x80c , BIT(29), 1); /*Tx path from Reg*/
-	rtl_set_bbreg(hw, 0x80c , 0xFFF0, 0x111); /*Tx by Reg*/
-	rtl_set_bbreg(hw, 0x6d8 , BIT(7) | BIT(6), 1); /*Resp Tx by Txinfo*/
-	rtl8812ae_dm_update_txpath(hw, RF90_PATH_A);
-
-	rtldm->path_sel = 1; /* TxInfo default at path-A*/
-}
-
-void rtl812ae_dm_set_txpath_by_txinfo(struct ieee80211_hw *hw,
-	u8 *pdesc)
-{
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-
-	SET_TX_DESC_TX_ANT(pdesc, rtldm->path_sel);
-}
-
-void rtl8812ae_dm_path_statistics(struct ieee80211_hw *hw,
-	u32 rssi_a, u32 rssi_b)
-{
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-
-	rtldm->patha_sum += rssi_a;
-	rtldm->patha_cnt ++;
-
-	rtldm->pathb_sum += rssi_b;
-	rtldm->pathb_cnt ++;
-}
-
-void rtl8812ae_dm_path_diversity(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u32	rssi_avg_a = 0;
-	u32 rssi_avg_b = 0;
-	u32 local_min_rssi = 0;
-	u32 min_rssi = 0xFF;
-	u8 tx_resp_path=0, target_path;
-	struct ieee80211_sta *sta = NULL;
-
-	sta = rtl_find_sta(hw, mac->bssid);
-	if (sta) {
-		/*Caculate RSSI per Path*/
-		rssi_avg_a = (rtldm->patha_cnt != 0) ? \
-			(rtldm->patha_sum / rtldm->patha_cnt) : 0;
-		rssi_avg_b = (rtldm->pathb_cnt != 0) ? \
-			(rtldm->pathb_sum / rtldm->pathb_cnt) : 0;
-
-		target_path = (rssi_avg_a == rssi_avg_b) ? rtldm->resp_tx_path : \
-			((rssi_avg_a>=rssi_avg_b) ? RF90_PATH_A : RF90_PATH_B);
-
-		RT_TRACE(COMP_DIG, DBG_TRACE, \
-			("assoc_id=%d, PathA_Sum=%d, PathA_Cnt=%d\n", \
-			mac->assoc_id, rtldm->patha_sum, rtldm->patha_cnt));
-		RT_TRACE(COMP_DIG, DBG_TRACE, \
-			("assoc_id=%d, PathB_Sum=%d, PathB_Cnt=%d\n", \
-			mac->assoc_id, rtldm->pathb_sum, rtldm->pathb_cnt));
-		RT_TRACE(COMP_DIG, DBG_TRACE, \
-			("assoc_id=%d, RssiAvgA= %d, RssiAvgB= %d\n", \
-			mac->assoc_id, rssi_avg_a, rssi_avg_b));
-
-		/*Select Resp Tx Path*/
-		local_min_rssi = (rssi_avg_a > rssi_avg_b) ?  rssi_avg_b : rssi_avg_a;
-		if(local_min_rssi  < min_rssi)
-		{
-			min_rssi = local_min_rssi;
-			tx_resp_path = target_path;
-		}
-
-		/*Select Tx DESC*/
-		if(target_path == RF90_PATH_A)
-			rtldm->path_sel = 1;
-		else
-			rtldm->path_sel = 2;
-
-		RT_TRACE(COMP_DIG, DBG_TRACE, \
-			("Tx from TxInfo, TargetPath=%s\n", \
-			(target_path==RF90_PATH_A) ? \
-			"ODM_RF_PATH_A":"ODM_RF_PATH_B"));
-		RT_TRACE(COMP_DIG, DBG_TRACE, \
-			("pDM_PathDiv->PathSel= %d\n", \
-			rtldm->path_sel));
-	}
-	rtldm->patha_cnt = 0;
-	rtldm->patha_sum = 0;
-	rtldm->pathb_cnt = 0;
-	rtldm->pathb_sum = 0;
-}
-
-void rtl8821ae_dm_dynamic_atc_switch(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	u8 crystal_cap;
-	u32 packet_count;
-	int cfo_khz_a,cfo_khz_b,cfo_ave = 0, adjust_xtal = 0;
-	int cfo_ave_diff;
-
-	if (rtlpriv->mac80211.link_state < MAC80211_LINKED){
-		/*1.Enable ATC*/
-		if (rtldm->atc_status == ATC_STATUS_OFF)
-		{
-			rtl_set_bbreg(hw, RFC_AREA, BIT(14), ATC_STATUS_ON);
-			rtldm->atc_status = ATC_STATUS_ON;
-		}
-
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("rtl8821ae_dm_dynamic_atc_switch(): No link!!\n"));
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("rtl8821ae_dm_dynamic_atc_switch(): atc_status = %d\n", \
-			rtldm->atc_status));
-
-		if (rtldm->crystal_cap != rtlpriv->efuse.crystalcap)
-		{
-			rtldm->crystal_cap = rtlpriv->efuse.crystalcap;
-			crystal_cap = rtldm->crystal_cap & 0x3f;
-			crystal_cap = crystal_cap & 0x3f;
-			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, \
-				0x7ff80000, (crystal_cap | (crystal_cap << 6)));
-		}
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("rtl8821ae_dm_dynamic_atc_switch(): crystal_cap = 0x%x\n", \
-			rtldm->crystal_cap));
-	}else{
-		/*1. Calculate CFO for path-A & path-B*/
-		cfo_khz_a = (int)(rtldm->cfo_tail[0] * 3125) / 1280;
-		cfo_khz_b = (int)(rtldm->cfo_tail[1] * 3125) / 1280;
-		packet_count = rtldm->packet_count;
-
-		/*2.No new packet*/
-		if (packet_count == rtldm->packet_count_pre) {
-			RT_TRACE(COMP_DIG, DBG_LOUD, \
-				("rtl8821ae_dm_dynamic_atc_switch(): packet counter doesn't change\n"));
-			return;
-		}
-
-		rtldm->packet_count_pre = packet_count;
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("rtl8821ae_dm_dynamic_atc_switch(): packet counter = %d\n", \
-			rtldm->packet_count));
-
-		/*3.Average CFO*/
-		if (rtlpriv->phy.rf_type == RF_1T1R)
-			cfo_ave = cfo_khz_a;
-		else
-			cfo_ave = (cfo_khz_a + cfo_khz_b) >> 1;
-
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("rtl8821ae_dm_dynamic_atc_switch():"
-			"cfo_khz_a = %dkHz, cfo_khz_b = %dkHz, cfo_ave = %dkHz\n",
-			cfo_khz_a, cfo_khz_b, cfo_ave));
-
-		/*4.Avoid abnormal large CFO*/
-		cfo_ave_diff = (rtldm->cfo_ave_pre >= cfo_ave)?
-						(rtldm->cfo_ave_pre - cfo_ave):
-						(cfo_ave - rtldm->cfo_ave_pre);
-
-		if (cfo_ave_diff > 20 && rtldm->large_cfo_hit == 0){
-			RT_TRACE(COMP_DIG, DBG_LOUD, \
-				("rtl8821ae_dm_dynamic_atc_switch(): first large CFO hit\n"));
-			rtldm->large_cfo_hit = 1;
-			return;
-		}
-		else
-			rtldm->large_cfo_hit = 0;
-
-		rtldm->cfo_ave_pre = cfo_ave;
-
-		/*CFO tracking by adjusting Xtal cap.*/
-
-		/*1.Dynamic Xtal threshold*/
-		if (cfo_ave >= -rtldm->cfo_threshold &&
-			cfo_ave <= rtldm->cfo_threshold &&
-			rtldm->is_freeze == 0){
-			if (rtldm->cfo_threshold == CFO_THRESHOLD_XTAL){
-				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL + 10;
-				rtldm->is_freeze = 1;
-			}
-			else
-				rtldm->cfo_threshold = CFO_THRESHOLD_XTAL;
-		}
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("rtl8821ae_dm_dynamic_atc_switch(): Dynamic threshold = %d\n", \
-			rtldm->cfo_threshold));
-
-		/* 2.Calculate Xtal offset*/
-		if (cfo_ave > rtldm->cfo_threshold && rtldm->crystal_cap < 0x3f)
-			adjust_xtal = ((cfo_ave - CFO_THRESHOLD_XTAL) >> 2) + 1;
-		else if ((cfo_ave < -rtlpriv->dm.cfo_threshold) && rtlpriv->dm.crystal_cap > 0)
-			adjust_xtal = ((cfo_ave + CFO_THRESHOLD_XTAL) >> 2) - 1;
-		RT_TRACE(COMP_DIG, DBG_LOUD, \
-			("rtl8821ae_dm_dynamic_atc_switch(): "
-			"Crystal cap = 0x%x, Crystal cap offset = %d\n",
-			rtldm->crystal_cap, adjust_xtal));
-
-		/*3.Adjust Crystal Cap.*/
-		if (adjust_xtal != 0){
-			rtldm->is_freeze = 0;
-			rtldm->crystal_cap += adjust_xtal;
-
-			if (rtldm->crystal_cap > 0x3f)
-				rtldm->crystal_cap = 0x3f;
-			else if (rtldm->crystal_cap < 0)
-				rtldm->crystal_cap = 0;
-
-			crystal_cap = rtldm->crystal_cap & 0x3f;
-			crystal_cap = crystal_cap & 0x3f;
-			rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, \
-				0x7ff80000, (crystal_cap | (crystal_cap << 6)));
-			RT_TRACE(COMP_DIG, DBG_LOUD, \
-				("rtl8821ae_dm_dynamic_atc_switch(): New crystal cap = 0x%x \n", \
-				rtldm->crystal_cap));
-		}
-	}
-
-}
-
-void rtl8821ae_dm_watchdog(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_fw_current_inpsmode = false;
-	bool b_fw_ps_awake = true;
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				      (u8 *) (&b_fw_current_inpsmode));
-
-	rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
-				      (u8 *) (&b_fw_ps_awake));
-
-	if(ppsc->p2p_ps_info.p2p_ps_mode)
-		b_fw_ps_awake = false;
-
-	if((ppsc->rfpwr_state == ERFON) &&
-		((!b_fw_current_inpsmode) && b_fw_ps_awake) &&
-		(!ppsc->rfchange_inprogress)) {
-		rtl8821ae_dm_common_info_self_update(hw);
-		rtl8821ae_dm_false_alarm_counter_statistics(hw);
-		rtl8821ae_dm_check_rssi_monitor(hw);
-		rtl8821ae_dm_dig(hw);
-		rtl8821ae_dm_dynamic_edcca(hw);
-		rtl8821ae_dm_cck_packet_detection_thresh(hw);
-		rtl8821ae_dm_refresh_rate_adaptive_mask(hw);
-		rtl8821ae_dm_check_edca_turbo(hw);
-		rtl8821ae_dm_dynamic_atc_switch(hw);
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-			rtl8812ae_dm_check_txpower_tracking_thermalmeter(hw);
-		else
-			rtl8821ae_dm_check_txpower_tracking_thermalmeter(hw);
-		rtl8821ae_dm_iq_calibrate(hw);
-		if (rtlpriv->cfg->ops->get_btc_status()){
-			rtlpriv->btcoexist.btc_ops->btc_periodical(rtlpriv);
-		}
-	}
-
-	rtlpriv->dm.dbginfo.num_qry_beacon_pkt = 0;
-}
-
-void rtl8821ae_dm_set_tx_ant_by_tx_info(struct ieee80211_hw *hw,
-												   u8 *pdesc, u32 mac_id)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	struct fast_ant_trainning *pfat_table= &(rtldm->fat_table);
-
-	if (rtlhal->hw_type != HARDWARE_TYPE_RTL8812AE)
-		return;
-
-	if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
-		(rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV)){
-		SET_TX_DESC_TX_ANT(pdesc, pfat_table->antsel_a[mac_id]);
-	}
-}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/fw.c b/drivers/staging/rtl8821ae/rtl8821ae/fw.c
deleted file mode 100644
index 46eb4125d18f..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/fw.c
+++ /dev/null
@@ -1,1349 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "../wifi.h"
-#include "../pci.h"
-#include "../base.h"
-#include "reg.h"
-#include "def.h"
-#include "fw.h"
-#include "dm.h"
-
-static void _rtl8821ae_enable_fw_download(struct ieee80211_hw *hw, bool enable)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp;
-
-	if (enable) {
-		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x05);
-
-		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL + 2);
-		rtl_write_byte(rtlpriv, REG_MCUFWDL + 2, tmp & 0xf7);
-
-		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
-		//printk("0x80=%02x.\n",tmp);
-	} else {
-		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
-		rtl_write_byte(rtlpriv, REG_MCUFWDL, tmp & 0xfe);
-		tmp = rtl_read_byte(rtlpriv, REG_MCUFWDL);
-		//printk("0x80=%02x.\n",tmp);
-	}
-
-}
-
-static void _rtl8821ae_fw_block_write(struct ieee80211_hw *hw,
-				   const u8 *buffer, u32 size)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 blockSize = sizeof(u32);
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 *pu4BytePtr = (u32 *) buffer;
-	u32 i, offset, blockCount, remainSize;
-
-	blockCount = size / blockSize;
-	remainSize = size % blockSize;
-
-	for (i = 0; i < blockCount; i++) {
-		offset = i * blockSize;
-		rtl_write_dword(rtlpriv, (FW_8821AE_START_ADDRESS + offset),
-				*(pu4BytePtr + i));
-	}
-
-	if (remainSize) {
-		offset = blockCount * blockSize;
-		bufferPtr += offset;
-		for (i = 0; i < remainSize; i++) {
-			rtl_write_byte(rtlpriv, (FW_8821AE_START_ADDRESS +
-						 offset + i), *(bufferPtr + i));
-		}
-	}
-}
-
-static void _rtl8821ae_fw_page_write(struct ieee80211_hw *hw,
-				  u32 page, const u8 *buffer, u32 size)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 value8;
-	u8 u8page = (u8) (page & 0x07);
-
-	value8 = (rtl_read_byte(rtlpriv, REG_MCUFWDL + 2) & 0xF8) | u8page;
-
-	rtl_write_byte(rtlpriv, (REG_MCUFWDL + 2), value8);
-	_rtl8821ae_fw_block_write(hw, buffer, size);
-}
-
-static void _rtl8821ae_fill_dummy(u8 *pfwbuf, u32 *pfwlen)
-{
-	u32 fwlen = *pfwlen;
-	u8 remain = (u8) (fwlen % 4);
-
-	remain = (remain == 0) ? 0 : (4 - remain);
-
-	while (remain > 0) {
-		pfwbuf[fwlen] = 0;
-		fwlen++;
-		remain--;
-	}
-
-	*pfwlen = fwlen;
-}
-
-static void _rtl8821ae_write_fw(struct ieee80211_hw *hw,
-			     					  enum version_8821ae version,
-			     					  u8 *buffer, u32 size)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 *bufferPtr = (u8 *) buffer;
-	u32 pageNums, remainSize;
-	u32 page, offset;
-
-	RT_TRACE(COMP_FW, DBG_LOUD, ("FW size is %d bytes,\n", size));
-
-	_rtl8821ae_fill_dummy(bufferPtr, &size);
-
-	pageNums = size / FW_8821AE_PAGE_SIZE;
-	remainSize = size % FW_8821AE_PAGE_SIZE;
-
-	if (pageNums > 8) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Page numbers should not greater then 8\n"));
-	}
-
-	for (page = 0; page < pageNums; page++) {
-		offset = page * FW_8821AE_PAGE_SIZE;
-		_rtl8821ae_fw_page_write(hw, page, (bufferPtr + offset),
-				      FW_8821AE_PAGE_SIZE);
-	}
-
-	if (remainSize) {
-		offset = pageNums * FW_8821AE_PAGE_SIZE;
-		page = pageNums;
-		_rtl8821ae_fw_page_write(hw, page, (bufferPtr + offset),
-				      remainSize);
-	}
-
-}
-
-static int _rtl8821ae_fw_free_to_go(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int err = -EIO;
-	u32 counter = 0;
-	u32 value32;
-
-	do {
-		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
-	} while ((counter++ < FW_8821AE_POLLING_TIMEOUT_COUNT) &&
-		 (!(value32 & FWDL_CHKSUM_RPT)));
-
-	if (counter >= FW_8821AE_POLLING_TIMEOUT_COUNT) {
-		RT_TRACE(COMP_ERR, DBG_LOUD,
-			 ("chksum report fail ! REG_MCUFWDL:0x%08x .\n",
-			  value32));
-		goto exit;
-	}
-
-	RT_TRACE(COMP_FW, DBG_EMERG,
-		 ("Checksum report OK ! REG_MCUFWDL:0x%08x .\n", value32));
-
-	value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
-	value32 |= MCUFWDL_RDY;
-	value32 &= ~WINTINI_RDY;
-	rtl_write_dword(rtlpriv, REG_MCUFWDL, value32);
-
-	rtl8821ae_firmware_selfreset(hw);
-
-	counter = 0;
-	do {
-		value32 = rtl_read_dword(rtlpriv, REG_MCUFWDL);
-		if (value32 & WINTINI_RDY) {
-			RT_TRACE(COMP_FW, DBG_LOUD,
-				 ("Polling FW ready success!! REG_MCUFWDL:0x%08x .\n",
-				  value32));
-			err = 0;
-			goto exit;
-		}
-
-		udelay(FW_8821AE_POLLING_DELAY);
-
-	} while (counter++ < FW_8821AE_POLLING_TIMEOUT_COUNT);
-
-	RT_TRACE(COMP_ERR, DBG_EMERG,
-		 ("Polling FW ready fail!! REG_MCUFWDL:0x%08x .\n", value32));
-
-exit:
-	return err;
-}
-
-int rtl8821ae_download_fw(struct ieee80211_hw *hw,
-	bool buse_wake_on_wlan_fw
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl8821a_firmware_header *pfwheader;
-	u8 *pfwdata;
-	u32 fwsize;
-	int err;
-	enum version_8821ae version = rtlhal->version;
-
-	if(!rtlhal->pfirmware)
-		return 1;
-
-	pfwheader = (struct rtl8821a_firmware_header *)rtlhal->pfirmware;
-	pfwdata = (u8 *) rtlhal->pfirmware;
-	fwsize = rtlhal->fwsize;
-	RT_TRACE(COMP_FW, DBG_DMESG,
-		 ("normal Firmware SIZE %d \n",fwsize));
-
-	if (IS_FW_HEADER_EXIST_8812(pfwheader) || IS_FW_HEADER_EXIST_8821(pfwheader)) {
-		RT_TRACE(COMP_FW, DBG_DMESG,
-			 ("Firmware Version(%d), Signature(%#x),Size(%d)\n",
-			  pfwheader->version, pfwheader->signature,
-			  (int)sizeof(struct rtl8821a_firmware_header)));
-
-		pfwdata = pfwdata + sizeof(struct rtl8821a_firmware_header);
-		fwsize = fwsize - sizeof(struct rtl8821a_firmware_header);
-	}
-
-	if(rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)){
-		rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
-		rtl8821ae_firmware_selfreset(hw);
-	}
-	_rtl8821ae_enable_fw_download(hw, true);
-	_rtl8821ae_write_fw(hw, version, pfwdata, fwsize);
-	_rtl8821ae_enable_fw_download(hw, false);
-
-	err = _rtl8821ae_fw_free_to_go(hw);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("Firmware is not ready to run!\n"));
-	} else {
-		RT_TRACE(COMP_FW, DBG_LOUD,
-			 ("Firmware is ready to run!\n"));
-	}
-
-	return 0;
-}
-
-static bool _rtl8821ae_check_fw_read_last_h2c(struct ieee80211_hw *hw, u8 boxnum)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 val_hmetfr;
-	bool result = false;
-
-	val_hmetfr = rtl_read_byte(rtlpriv, REG_HMETFR);
-	if (((val_hmetfr >> boxnum) & BIT(0)) == 0)
-		result = true;
-	return result;
-}
-
-static void _rtl8821ae_fill_h2c_command(struct ieee80211_hw *hw,
-			      u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 boxnum =0;
-	u16 box_reg = 0, box_extreg = 0;
-	u8 u1b_tmp = 0;
-	bool isfw_read = false;
-	u8 buf_index = 0;
-	bool bwrite_sucess = false;
-	u8 wait_h2c_limmit = 100;
-	/*u8 wait_writeh2c_limmit = 100;*/
-	u8 boxcontent[4], boxextcontent[4];
-	u32 h2c_waitcounter = 0;
-	unsigned long flag =0;
-	u8 idx =0;
-
-	RT_TRACE(COMP_CMD, DBG_LOUD, ("come in\n"));
-
-	while (true) {
-		spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
-		if (rtlhal->b_h2c_setinprogress) {
-			RT_TRACE(COMP_CMD, DBG_LOUD,
-				 ("H2C set in progress! Wait to set.."
-				  "element_id(%d).\n", element_id));
-
-			while (rtlhal->b_h2c_setinprogress) {
-				spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock,
-						       flag);
-				h2c_waitcounter++;
-				RT_TRACE(COMP_CMD, DBG_LOUD,
-					 ("Wait 100 us (%d times)...\n",
-					  h2c_waitcounter));
-				udelay(100);
-
-				if (h2c_waitcounter > 1000)
-					return;
-				spin_lock_irqsave(&rtlpriv->locks.h2c_lock,
-						  flag);
-			}
-			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
-		} else {
-			rtlhal->b_h2c_setinprogress = true;
-			spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
-			break;
-		}
-	}
-
-	while (!bwrite_sucess) {
-	/*cosa remove this because never reach this.*/
-#if 0
-		wait_writeh2c_limmit--;
-		if (wait_writeh2c_limmit == 0) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Write H2C fail because no trigger "
-				  "for FW INT!\n"));
-			break;
-		}
-#endif
-
-		boxnum = rtlhal->last_hmeboxnum;
-		switch (boxnum) {
-		case 0:
-			box_reg = REG_HMEBOX_0;
-			box_extreg = REG_HMEBOX_EXT_0;
-			break;
-		case 1:
-			box_reg = REG_HMEBOX_1;
-			box_extreg = REG_HMEBOX_EXT_1;
-			break;
-		case 2:
-			box_reg = REG_HMEBOX_2;
-			box_extreg = REG_HMEBOX_EXT_2;
-			break;
-		case 3:
-			box_reg = REG_HMEBOX_3;
-			box_extreg = REG_HMEBOX_EXT_3;
-			break;
-		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("switch case not process \n"));
-			break;
-		}
-
-		isfw_read = false;
-		u1b_tmp = rtl_read_byte(rtlpriv, REG_CR);
-
-		if (u1b_tmp != 0xEA)
-			isfw_read = true;
-		else {
-			if( rtl_read_byte(rtlpriv, REG_TXDMA_STATUS) == 0xEA ||
-				rtl_read_byte(rtlpriv, REG_TXPKT_EMPTY) == 0xEA)
-				rtl_write_byte(rtlpriv, REG_SYS_CFG1 + 3, 0xFF);
-		}
-
-		if (isfw_read == true) {
-			wait_h2c_limmit = 100;
-			isfw_read = _rtl8821ae_check_fw_read_last_h2c(hw, boxnum);
-			while (!isfw_read) {
-				/*wait until Fw read*/
-				wait_h2c_limmit--;
-				if (wait_h2c_limmit == 0) {
-					RT_TRACE(COMP_CMD, DBG_LOUD,
-						 ("Waiting too long for FW read "
-						  "clear HMEBox(%d)!\n", boxnum));
-					break;
-				}
-
-				udelay(10);
-
-				isfw_read = _rtl8821ae_check_fw_read_last_h2c(hw, boxnum);
-				u1b_tmp = rtl_read_byte(rtlpriv, 0x130);
-				RT_TRACE(COMP_CMD, DBG_LOUD,
-					 ("Waiting for FW read clear HMEBox(%d)!!! "
-					  "0x130 = %2x\n", boxnum, u1b_tmp));
-			}
-		}
-
-		if (!isfw_read) {
-			RT_TRACE(COMP_CMD, DBG_LOUD,
-				 ("Write H2C register BOX[%d] fail!!!!! "
-				  "Fw do not read. \n", boxnum));
-			break;
-		}
-
-		memset(boxcontent, 0, sizeof(boxcontent));
-		memset(boxextcontent, 0, sizeof(boxextcontent));
-		boxcontent[0] = element_id;
-		RT_TRACE(COMP_CMD, DBG_LOUD,
-			 ("Write element_id box_reg(%4x) = %2x \n",
-			  box_reg, element_id));
-
-		switch (cmd_len) {
-		case 1:
-		case 2:
-		case 3:
-			/*boxcontent[0] &= ~(BIT(7));*/
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + buf_index, cmd_len);
-
-			for (idx = 0; idx < 4; idx++) {
-				rtl_write_byte(rtlpriv, box_reg + idx,
-					       boxcontent[idx]);
-			}
-			break;
-		case 4:
-		case 5:
-		case 6:
-		case 7:
-			/*boxcontent[0] |= (BIT(7));*/
-			memcpy((u8 *) (boxextcontent),
-			       p_cmdbuffer + buf_index+3, cmd_len-3);
-			memcpy((u8 *) (boxcontent) + 1,
-			       p_cmdbuffer + buf_index, 3);
-
-			for (idx = 0; idx < 4; idx++) {
-				rtl_write_byte(rtlpriv, box_extreg + idx,
-					       boxextcontent[idx]);
-			}
-
-			for (idx = 0; idx < 4; idx++) {
-				rtl_write_byte(rtlpriv, box_reg + idx,
-					       boxcontent[idx]);
-			}
-			break;
-		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("switch case not process \n"));
-			break;
-		}
-
-		bwrite_sucess = true;
-
-		rtlhal->last_hmeboxnum = boxnum + 1;
-		if (rtlhal->last_hmeboxnum == 4)
-			rtlhal->last_hmeboxnum = 0;
-
-		RT_TRACE(COMP_CMD, DBG_LOUD,
-			 ("pHalData->last_hmeboxnum  = %d\n",
-			  rtlhal->last_hmeboxnum));
-	}
-
-	spin_lock_irqsave(&rtlpriv->locks.h2c_lock, flag);
-	rtlhal->b_h2c_setinprogress = false;
-	spin_unlock_irqrestore(&rtlpriv->locks.h2c_lock, flag);
-
-	RT_TRACE(COMP_CMD, DBG_LOUD, ("go out\n"));
-}
-
-void rtl8821ae_fill_h2c_cmd(struct ieee80211_hw *hw,
-			 u8 element_id, u32 cmd_len, u8 *p_cmdbuffer)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u32 tmp_cmdbuf[2];
-
-	if (rtlhal->bfw_ready == false) {
-		RT_ASSERT(false, ("return H2C cmd because of Fw "
-				  "download fail!!!\n"));
-		return;
-	}
-
-	memset(tmp_cmdbuf, 0, 8);
-	memcpy(tmp_cmdbuf, p_cmdbuffer, cmd_len);
-	_rtl8821ae_fill_h2c_command(hw, element_id, cmd_len, (u8 *) & tmp_cmdbuf);
-
-	return;
-}
-
-void rtl8821ae_firmware_selfreset(struct ieee80211_hw *hw)
-{
-	u8 u1b_tmp;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-	{
-		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
-		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(3))));
-	}else {
-		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
-		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp & (~BIT(0))));
-	}
-
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
-	udelay(50);
-
-	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-	{
-		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
-		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp | BIT(3)));
-	}else {
-		u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL+1);
-		rtl_write_byte(rtlpriv, REG_RSV_CTRL+1, (u1b_tmp | BIT(0)));
-	}
-
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp | BIT(2)));
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("  _8051Reset8812ae(): 8051 reset success .\n"));
-
-}
-
-void rtl8821ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 u1_h2c_set_pwrmode[H2C_8821AE_PWEMODE_LENGTH] = { 0 };
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	u8 rlbm,power_state = 0;
-	RT_TRACE(COMP_POWER, DBG_LOUD, ("FW LPS mode = %d\n", mode));
-
-	SET_H2CCMD_PWRMODE_PARM_MODE(u1_h2c_set_pwrmode, ((mode) ? 1 : 0));
-	rlbm = 0;/*YJ,temp,120316. FW now not support RLBM=2.*/
-	SET_H2CCMD_PWRMODE_PARM_RLBM(u1_h2c_set_pwrmode, rlbm);
-	SET_H2CCMD_PWRMODE_PARM_SMART_PS(u1_h2c_set_pwrmode, (rtlpriv->mac80211.p2p) ? ppsc->smart_ps : 1);
-	SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(u1_h2c_set_pwrmode, ppsc->reg_max_lps_awakeintvl);
-	SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(u1_h2c_set_pwrmode, 0);
-	if(mode == FW_PS_ACTIVE_MODE)
-	{
-		power_state |= FW_PWR_STATE_ACTIVE;
-	}
-	else
-	{
-		power_state |= FW_PWR_STATE_RF_OFF;
-	}
-	SET_H2CCMD_PWRMODE_PARM_PWR_STATE(u1_h2c_set_pwrmode, power_state);
-
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl92c_set_fw_pwrmode(): u1_h2c_set_pwrmode \n",
-		      u1_h2c_set_pwrmode, H2C_8821AE_PWEMODE_LENGTH);
-	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_SETPWRMODE, H2C_8821AE_PWEMODE_LENGTH, u1_h2c_set_pwrmode);
-
-}
-
-void rtl8821ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus)
-{
-	u8 u1_joinbssrpt_parm[1] = { 0 };
-
-	SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(u1_joinbssrpt_parm, mstatus);
-
-	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_JOINBSSRPT, 1, u1_joinbssrpt_parm);
-}
-
-void rtl8821ae_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,  u8 ap_offload_enable)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u8 u1_apoffload_parm[H2C_8821AE_AP_OFFLOAD_LENGTH] = { 0 };
-
-	SET_H2CCMD_AP_OFFLOAD_ON(u1_apoffload_parm, ap_offload_enable);
-	SET_H2CCMD_AP_OFFLOAD_HIDDEN(u1_apoffload_parm, mac->bhiddenssid);
-	SET_H2CCMD_AP_OFFLOAD_DENYANY(u1_apoffload_parm, 0);
-
-	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_AP_OFFLOAD, H2C_8821AE_AP_OFFLOAD_LENGTH, u1_apoffload_parm);
-
-}
-
-static bool _rtl8821ae_cmd_send_packet(struct ieee80211_hw *hw,
-				struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	struct rtl_tx_desc *pdesc;
-	u8 own;
-	unsigned long flags;
-	struct sk_buff *pskb = NULL;
-
-	ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	pskb = __skb_dequeue(&ring->queue);
-	if (pskb)
-		kfree_skb(pskb);
-
-	spin_lock_irqsave(&rtlpriv->locks.irq_th_lock, flags);
-
-	pdesc = &ring->desc[0];
-	own = (u8) rtlpriv->cfg->ops->get_desc((u8 *) pdesc, true, HW_DESC_OWN);
-
-	rtlpriv->cfg->ops->fill_tx_cmddesc(hw, (u8 *) pdesc, 1, 1, skb);
-
-	__skb_queue_tail(&ring->queue, skb);
-
-	spin_unlock_irqrestore(&rtlpriv->locks.irq_th_lock, flags);
-
-	rtlpriv->cfg->ops->tx_polling(hw, BEACON_QUEUE);
-
-	return true;
-}
-
-#define BEACON_PG		0 /* ->1 */
-#define PSPOLL_PG		2
-#define NULL_PG			3
-#define PROBERSP_PG		4 /* ->5 */
-
-#define BEACON_PG_8812		0
-#define PSPOLL_PG_8812		1
-#define NULL_PG_8812			2
-#define PROBERSP_PG_8812		3
-
-#define BEACON_PG_8821		0
-#define PSPOLL_PG_8821		1
-#define NULL_PG_8821			2
-#define PROBERSP_PG_8821		3
-
-#define TOTAL_RESERVED_PKT_LEN_8812	2048
-#define TOTAL_RESERVED_PKT_LEN_8821	1024
-
-
-static u8 reserved_page_packet_8821[TOTAL_RESERVED_PKT_LEN_8821] = {
-	/* page 0 */
-	0x80, 0x00, 0x00, 0x00,  0xff, 0xff, 0xff, 0xff,
-	0xff, 0xff, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
-	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x20, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x64, 0x00, 0x20, 0x04,  0x00, 0x06, 0x64, 0x6c,
-	0x69, 0x6e, 0x6b, 0x31,  0x01, 0x08, 0x82, 0x84,
-	0x8b, 0x96, 0x0c, 0x18,  0x30, 0x48, 0x03, 0x01,
-	0x0b, 0x06, 0x02, 0x00,  0x00, 0x2a, 0x01, 0x8b,
-	0x32, 0x04, 0x12, 0x24,  0x60, 0x6c, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x10, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x81, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	/* page 1 */
-	0xa4, 0x10, 0x01, 0xc0,  0x40, 0x16, 0x9f, 0x23,
-	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x18, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	/* page 2 */
-	0x48, 0x01, 0x00, 0x00,  0x40, 0x16, 0x9f, 0x23,
-	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
-	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x1a, 0x00, 0x28, 0x8c,  0x00, 0x12, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	/* page 3 */
-	0xc8, 0x01, 0x00, 0x00,  0x40, 0x16, 0x9f, 0x23,
-	0xd4, 0x46, 0x00, 0xe0,  0x4c, 0x02, 0xe2, 0x64,
-	0x40, 0x16, 0x9f, 0x23,  0xd4, 0x46, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-};
-
-
-static u8 reserved_page_packet_8812[TOTAL_RESERVED_PKT_LEN_8812] = {
- 	0x80, 0x00, 0x00, 0x00,  0xFF, 0xFF, 0xFF, 0xFF,
-	0xFF, 0xFF, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
-	0xE0, 0x46, 0x9A, 0x57,  0x71, 0x30, 0x20, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x64, 0x00, 0x30, 0x04,  0x00, 0x0C, 0x4E, 0x45,
-	0x54, 0x47, 0x45, 0x41,  0x52, 0x5F, 0x31, 0x35,
-	0x30, 0x4E, 0x01, 0x08,  0x82, 0x84, 0x8B, 0x96,
-	0x0C, 0x12, 0x18, 0x24,  0x03, 0x01, 0x03, 0x06,
-	0x02, 0x00, 0x00, 0x2A,  0x01, 0x8A, 0x32, 0x04,
-	0x30, 0x48, 0x60, 0x6C,  0xDD, 0x18, 0x00, 0x50,
-	0xF2, 0x01, 0x01, 0x00,  0x00, 0x50, 0xF2, 0x02,
-	0x01, 0x00, 0x00, 0x50,  0xF2, 0x02, 0x01, 0x00,
-	0x00, 0x50, 0xF2, 0x02,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x10, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x81, 0x00, 0x00,
-	0x04, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-
-	0xA4, 0x10, 0x02, 0xC0,  0xE0, 0x46, 0x9A, 0x57,
-	0x71, 0x30, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x18, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
-	0x04, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-
-	0x48, 0x01, 0x00, 0x00,  0xE0, 0x46, 0x9A, 0x57,
-	0x71, 0x30, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
-	0xE0, 0x46, 0x9A, 0x57,  0x71, 0x30, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x1A, 0x00, 0x28, 0x8C,  0x00, 0x12, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x01, 0x00, 0x00,
-	0x04, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x80, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-
-	0xC8, 0x01, 0x00, 0x00,  0xE0, 0x46, 0x9A, 0x57,
-	0x71, 0x30, 0x00, 0xE0,  0x4C, 0x02, 0x53, 0xE5,
-	0xE0, 0x46, 0x9A, 0x57,  0x71, 0x30, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-	0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00,
-};
-
-void rtl8812ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct sk_buff *skb = NULL;
-
-	u32 totalpacketlen;
-	bool rtstatus;
-	u8 u1RsvdPageLoc[5] = { 0 };
-	bool b_dlok = false;
-
-	u8* beacon;
-	u8* p_pspoll;
-	u8* nullfunc;
-	u8* p_probersp;
-	/*---------------------------------------------------------
-				(1) beacon
-	---------------------------------------------------------*/
-	beacon = &reserved_page_packet_8812[BEACON_PG_8812 * 512];
-	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
-
-	/*-------------------------------------------------------
-				(2) ps-poll
-	--------------------------------------------------------*/
-	p_pspoll = &reserved_page_packet_8812[PSPOLL_PG_8812 * 512];
-	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
-	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
-	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
-
-	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG_8812);
-
-	/*--------------------------------------------------------
-				(3) null data
-	---------------------------------------------------------*/
-	nullfunc = &reserved_page_packet_8812[NULL_PG_8812* 512];
-	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
-	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
-
-	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG_8812);
-
-	/*---------------------------------------------------------
-				(4) probe response
-	----------------------------------------------------------*/
-	p_probersp = &reserved_page_packet_8812[PROBERSP_PG_8812 * 512];
-	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
-	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
-
-	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG_8812);
-
-	totalpacketlen = TOTAL_RESERVED_PKT_LEN_8812;
-
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
-		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
-		      &reserved_page_packet_8812[0], totalpacketlen);
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
-		      u1RsvdPageLoc, 3);
-
-
-	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *) skb_put(skb, totalpacketlen),
-	       &reserved_page_packet_8812, totalpacketlen);
-
-	rtstatus = _rtl8821ae_cmd_send_packet(hw, skb);
-
-	if (rtstatus)
-		b_dlok = true;
-
-	if (b_dlok) {
-		RT_TRACE(COMP_POWER, DBG_LOUD,
-			 ("Set RSVD page location to Fw.\n"));
-		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-				"H2C_RSVDPAGE:\n",
-				u1RsvdPageLoc, 3);
-		rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RSVDPAGE,
-				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
-	} else
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Set RSVD page location to Fw FAIL!!!!!!.\n"));
-}
-
-void rtl8821ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct sk_buff *skb = NULL;
-
-	u32 totalpacketlen;
-	bool rtstatus;
-	u8 u1RsvdPageLoc[5] = { 0 };
-	bool b_dlok = false;
-
-	u8* beacon;
-	u8* p_pspoll;
-	u8* nullfunc;
-	u8* p_probersp;
-	/*---------------------------------------------------------
-				(1) beacon
-	---------------------------------------------------------*/
-	beacon = &reserved_page_packet_8821[BEACON_PG_8821 * 256];
-	SET_80211_HDR_ADDRESS2(beacon, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(beacon, mac->bssid);
-
-	/*-------------------------------------------------------
-				(2) ps-poll
-	--------------------------------------------------------*/
-	p_pspoll = &reserved_page_packet_8821[PSPOLL_PG_8821 * 256];
-	SET_80211_PS_POLL_AID(p_pspoll, (mac->assoc_id | 0xc000));
-	SET_80211_PS_POLL_BSSID(p_pspoll, mac->bssid);
-	SET_80211_PS_POLL_TA(p_pspoll, mac->mac_addr);
-
-	SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(u1RsvdPageLoc, PSPOLL_PG_8821);
-
-	/*--------------------------------------------------------
-				(3) null data
-	---------------------------------------------------------*/
-	nullfunc = &reserved_page_packet_8821[NULL_PG_8821 * 256];
-	SET_80211_HDR_ADDRESS1(nullfunc, mac->bssid);
-	SET_80211_HDR_ADDRESS2(nullfunc, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(nullfunc, mac->bssid);
-
-	SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(u1RsvdPageLoc, NULL_PG_8821);
-
-	/*---------------------------------------------------------
-				(4) probe response
-	----------------------------------------------------------*/
-	p_probersp = &reserved_page_packet_8821[PROBERSP_PG_8821 * 256];
-	SET_80211_HDR_ADDRESS1(p_probersp, mac->bssid);
-	SET_80211_HDR_ADDRESS2(p_probersp, mac->mac_addr);
-	SET_80211_HDR_ADDRESS3(p_probersp, mac->bssid);
-
-	SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(u1RsvdPageLoc, PROBERSP_PG_8821);
-
-	totalpacketlen = TOTAL_RESERVED_PKT_LEN_8821;
-
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
-		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
-		      &reserved_page_packet_8821[0], totalpacketlen);
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-		      "rtl8821ae_set_fw_rsvdpagepkt(): HW_VAR_SET_TX_CMD: ALL \n",
-		      u1RsvdPageLoc, 3);
-
-
-	skb = dev_alloc_skb(totalpacketlen);
-	memcpy((u8 *) skb_put(skb, totalpacketlen),
-	       &reserved_page_packet_8821, totalpacketlen);
-
-	rtstatus = _rtl8821ae_cmd_send_packet(hw, skb);
-
-	if (rtstatus)
-		b_dlok = true;
-
-	if (b_dlok) {
-		RT_TRACE(COMP_POWER, DBG_LOUD,
-			 ("Set RSVD page location to Fw.\n"));
-		RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_DMESG,
-				"H2C_RSVDPAGE:\n",
-				u1RsvdPageLoc, 3);
-		rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RSVDPAGE,
-				    sizeof(u1RsvdPageLoc), u1RsvdPageLoc);
-	} else
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Set RSVD page location to Fw FAIL!!!!!!.\n"));
-}
-
-/*Should check FW support p2p or not.*/
-void rtl8821ae_set_p2p_ctw_period_cmd(struct ieee80211_hw *hw, u8 ctwindow)
-{
-	u8 u1_ctwindow_period[1] ={ ctwindow};
-
-	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_P2P_PS_CTW_CMD, 1, u1_ctwindow_period);
-
-}
-
-void rtl8821ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *rtlps = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_p2p_ps_info *p2pinfo = &(rtlps->p2p_ps_info);
-	struct p2p_ps_offload_t *p2p_ps_offload = &rtlhal->p2p_ps_offload;
-	u8	i;
-	u16	ctwindow;
-	u32	start_time, tsf_low;
-
-	switch(p2p_ps_state)
-	{
-		case P2P_PS_DISABLE:
-			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_DISABLE \n"));
-			memset(p2p_ps_offload, 0, 1);
-			break;
-		case P2P_PS_ENABLE:
-			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_ENABLE \n"));
-			/* update CTWindow value. */
-			if( p2pinfo->ctwindow > 0 )
-			{
-				p2p_ps_offload->CTWindow_En = 1;
-				ctwindow = p2pinfo->ctwindow;
-				rtl8821ae_set_p2p_ctw_period_cmd(hw, ctwindow);
-			}
-
-			/* hw only support 2 set of NoA */
-			for( i=0 ; i<p2pinfo->noa_num ; i++)
-			{
-				/* To control the register setting for which NOA*/
-				rtl_write_byte(rtlpriv, 0x5cf, (i << 4));
-				if(i == 0)
-					p2p_ps_offload->NoA0_En = 1;
-				else
-					p2p_ps_offload->NoA1_En = 1;
-
-				/* config P2P NoA Descriptor Register */
-				rtl_write_dword(rtlpriv, 0x5E0, p2pinfo->noa_duration[i]);
-				rtl_write_dword(rtlpriv, 0x5E4, p2pinfo->noa_interval[i]);
-
-				/*Get Current TSF value */
-				tsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
-
-				start_time = p2pinfo->noa_start_time[i];
-				if(p2pinfo->noa_count_type[i] != 1)
-				{
-					while( start_time <= (tsf_low+(50*1024) ) ) {
-						start_time += p2pinfo->noa_interval[i];
-						if(p2pinfo->noa_count_type[i] != 255)
-							p2pinfo->noa_count_type[i]--;
-					}
-				}
-				rtl_write_dword(rtlpriv, 0x5E8, start_time);
-				rtl_write_dword(rtlpriv, 0x5EC, p2pinfo->noa_count_type[i] );
-
-			}
-
-			if( (p2pinfo->opp_ps == 1) || (p2pinfo->noa_num > 0) )
-			{
-				/* rst p2p circuit */
-				rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, BIT(4));
-
-				p2p_ps_offload->Offload_En = 1;
-
-				if(P2P_ROLE_GO == rtlpriv->mac80211.p2p)
-				{
-					p2p_ps_offload->role= 1;
-					p2p_ps_offload->AllStaSleep = 0;
-				}
-				else
-				{
-					p2p_ps_offload->role= 0;
-				}
-
-				p2p_ps_offload->discovery = 0;
-			}
-			break;
-		case P2P_PS_SCAN:
-			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_SCAN \n"));
-			p2p_ps_offload->discovery = 1;
-			break;
-		case P2P_PS_SCAN_DONE:
-			RT_TRACE(COMP_FW, DBG_LOUD,("P2P_PS_SCAN_DONE \n"));
-			p2p_ps_offload->discovery = 0;
-			p2pinfo->p2p_ps_state = P2P_PS_ENABLE;
-			break;
-		default:
-			break;
-	}
-
-	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_P2P_PS_OFFLOAD, 1, (u8 *)p2p_ps_offload);
-
-}
-
-void rtl8812ae_c2h_ra_report_handler(
-	struct ieee80211_hw *hw,
-	u8 *cmd_buf,
-	u8 cmd_len
-)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 rate = cmd_buf[0] & 0x3F;
-
-	rtlhal->current_ra_rate= rtl8812ae_hw_rate_to_mrate(hw, rate);
-
-	rtl8812ae_dm_update_init_rate(hw, rate);
-}
-
-
-void _rtl8812ae_c2h_content_parsing(
-	struct ieee80211_hw *hw,
-	u8 c2h_cmd_id,
-	u8 c2h_cmd_len,
-	u8 *tmp_buf
-)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	switch (c2h_cmd_id) {
-	case C2H_8812_DBG:
-		RT_TRACE(COMP_FW, DBG_LOUD,("[C2H], C2H_8812_DBG!!\n"));
-		break;
-
-	case C2H_8812_RA_RPT:
-		rtl8812ae_c2h_ra_report_handler(hw, tmp_buf, c2h_cmd_len);
-		break;
-
-	default:
-		break;
-	}
-
-}
-
-void rtl8812ae_c2h_packet_handler(
-	struct ieee80211_hw *hw,
-	u8 *buffer,
-	u8 length
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 c2h_cmd_id=0, c2h_cmd_seq=0, c2h_cmd_len=0;
-	u8 *tmp_buf=NULL;
-
-	c2h_cmd_id = buffer[0];
-	c2h_cmd_seq = buffer[1];
-	c2h_cmd_len = length -2;
-	tmp_buf = buffer + 2;
-
-	RT_TRACE(COMP_FW, DBG_LOUD,
-		("[C2H packet], c2hCmdId=0x%x, c2hCmdSeq=0x%x, c2hCmdLen=%d\n",
-		c2h_cmd_id, c2h_cmd_seq, c2h_cmd_len));
-
-	RT_PRINT_DATA(rtlpriv, COMP_FW, DBG_LOUD,
-		"[C2H packet], Content Hex:\n", tmp_buf, c2h_cmd_len);
-	_rtl8812ae_c2h_content_parsing(hw, c2h_cmd_id, c2h_cmd_len, tmp_buf);
-}
-
-
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/fw.h b/drivers/staging/rtl8821ae/rtl8821ae/fw.h
deleted file mode 100644
index 30eec880026c..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/fw.h
+++ /dev/null
@@ -1,321 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8821AE__FW__H__
-#define __RTL8821AE__FW__H__
-
-#define FW_8821AE_SIZE					0x8000
-#define FW_8821AE_START_ADDRESS			0x1000
-#define FW_8821AE_END_ADDRESS			0x5FFF
-#define FW_8821AE_PAGE_SIZE				4096
-#define FW_8821AE_POLLING_DELAY			5
-#define FW_8821AE_POLLING_TIMEOUT_COUNT	6000
-
-#define IS_FW_HEADER_EXIST_8812(_pfwhdr)	\
-	((_pfwhdr->signature&0xFFF0) == 0x9500 )
-
-#define IS_FW_HEADER_EXIST_8821(_pfwhdr)	\
-	((_pfwhdr->signature&0xFFF0) == 0x2100 )
-
-#define USE_OLD_WOWLAN_DEBUG_FW 0
-
-#define H2C_8821AE_RSVDPAGE_LOC_LEN		5
-#define H2C_8821AE_PWEMODE_LENGTH			5
-#define H2C_8821AE_JOINBSSRPT_LENGTH		1
-#define H2C_8821AE_AP_OFFLOAD_LENGTH		3
-#define H2C_8821AE_WOWLAN_LENGTH			3
-#define H2C_8821AE_KEEP_ALIVE_CTRL_LENGTH	3
-#if(USE_OLD_WOWLAN_DEBUG_FW == 0)
-#define H2C_8821AE_REMOTE_WAKE_CTRL_LEN	1
-#else
-#define H2C_8821AE_REMOTE_WAKE_CTRL_LEN	3
-#endif
-#define H2C_8821AE_AOAC_GLOBAL_INFO_LEN	2
-#define H2C_8821AE_AOAC_RSVDPAGE_LOC_LEN	7
-
-
-/* Fw PS state for RPWM.
-*BIT[2:0] = HW state
-*BIT[3] = Protocol PS state,   1: register active state , 0: register sleep state
-*BIT[4] = sub-state
-*/
-#define	FW_PS_GO_ON			BIT(0)
-#define	FW_PS_TX_NULL			BIT(1)
-#define	FW_PS_RF_ON			BIT(2)
-#define	FW_PS_REGISTER_ACTIVE	BIT(3)
-
-#define	FW_PS_DPS    		BIT(0)
-#define	FW_PS_LCLK   		(FW_PS_DPS)
-#define	FW_PS_RF_OFF   		BIT(1)
-#define	FW_PS_ALL_ON   		BIT(2)
-#define	FW_PS_ST_ACTIVE  	BIT(3)
-#define	FW_PS_ISR_ENABLE    	BIT(4)
-#define	FW_PS_IMR_ENABLE	BIT(5)
-
-
-#define	FW_PS_ACK    		BIT(6)
-#define	FW_PS_TOGGLE   		BIT(7)
-
- /* 8821AE RPWM value*/
- /* BIT[0] = 1: 32k, 0: 40M*/
-#define	FW_PS_CLOCK_OFF		BIT(0)		/* 32k*/
-#define	FW_PS_CLOCK_ON		0		/*40M*/
-
-#define	FW_PS_STATE_MASK  		(0x0F)
-#define	FW_PS_STATE_HW_MASK 	(0x07)
-#define	FW_PS_STATE_INT_MASK 	(0x3F)	/*ISR_ENABLE, IMR_ENABLE, and PS mode should be inherited.*/
-
-#define	FW_PS_STATE(x)   		(FW_PS_STATE_MASK & (x))
-#define	FW_PS_STATE_HW(x) 		(FW_PS_STATE_HW_MASK & (x))
-#define	FW_PS_STATE_INT(x)   	(FW_PS_STATE_INT_MASK & (x))
-#define	FW_PS_ISR_VAL(x)		((x) & 0x70)
-#define	FW_PS_IMR_MASK(x)   	((x) & 0xDF)
-#define	FW_PS_KEEP_IMR(x)		((x) & 0x20)
-
-
-#define	FW_PS_STATE_S0		(FW_PS_DPS)
-#define	FW_PS_STATE_S1		(FW_PS_LCLK)
-#define	FW_PS_STATE_S2		(FW_PS_RF_OFF)
-#define	FW_PS_STATE_S3		(FW_PS_ALL_ON)
-#define	FW_PS_STATE_S4		((FW_PS_ST_ACTIVE) | (FW_PS_ALL_ON))
-
-#define	FW_PS_STATE_ALL_ON_8821AE	(FW_PS_CLOCK_ON) /* ((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))*/
-#define	FW_PS_STATE_RF_ON_8821AE	(FW_PS_CLOCK_ON) /* (FW_PS_RF_ON)*/
-#define	FW_PS_STATE_RF_OFF_8821AE	(FW_PS_CLOCK_ON) /* 0x0*/
-#define	FW_PS_STATE_RF_OFF_LOW_PWR_8821AE	(FW_PS_CLOCK_OFF) /* (FW_PS_STATE_RF_OFF)*/
-
-#define	FW_PS_STATE_ALL_ON_92C	(FW_PS_STATE_S4)
-#define	FW_PS_STATE_RF_ON_92C		(FW_PS_STATE_S3)
-#define	FW_PS_STATE_RF_OFF_92C	(FW_PS_STATE_S2)
-#define	FW_PS_STATE_RF_OFF_LOW_PWR_92C	(FW_PS_STATE_S1)
-
-
-/* For 8821AE H2C PwrMode Cmd ID 5.*/
-#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
-#define	FW_PWR_STATE_RF_OFF	0
-
-#define	FW_PS_IS_ACK(x)  		((x) & FW_PS_ACK )
-#define	FW_PS_IS_CLK_ON(x) 		((x) & (FW_PS_RF_OFF |FW_PS_ALL_ON ))
-#define	FW_PS_IS_RF_ON(x)  		((x) & (FW_PS_ALL_ON))
-#define	FW_PS_IS_ACTIVE(x)  		((x) & (FW_PS_ST_ACTIVE))
-#define	FW_PS_IS_CPWM_INT(x)	((x) & 0x40)
-
-#define	FW_CLR_PS_STATE(x) 		((x) = ((x) & (0xF0)))
-
-#define	IS_IN_LOW_POWER_STATE_8821AE(FwPSState)		\
-			(FW_PS_STATE(FwPSState) == FW_PS_CLOCK_OFF)
-
-#define	FW_PWR_STATE_ACTIVE	((FW_PS_RF_ON) | (FW_PS_REGISTER_ACTIVE))
-#define	FW_PWR_STATE_RF_OFF	0
-
-struct rtl8821a_firmware_header {
-	u16 signature;
-	u8 category;
-	u8 function;
-	u16 version;
-	u8 subversion;
-	u8 rsvd1;
-	u8 month;
-	u8 date;
-	u8 hour;
-	u8 minute;
-	u16 ramcodeSize;
-	u16 rsvd2;
-	u32 svnindex;
-	u32 rsvd3;
-	u32 rsvd4;
-	u32 rsvd5;
-};
-
-enum rtl8812_c2h_evt{
-	C2H_8812_DBG = 0,
-	C2H_8812_LB = 1,
-	C2H_8812_TXBF = 2,
-	C2H_8812_TX_REPORT = 3,
-	C2H_8812_BT_INFO = 9,
-	C2H_8812_BT_MP = 11,
-	C2H_8812_RA_RPT=12,
-
-	C2H_8812_FW_SWCHNL = 0x10,
-	C2H_8812_IQK_FINISH = 0x11,
-	MAX_8812_C2HEVENT
-};
-
-enum rtl8821a_h2c_cmd {
-	H2C_8821AE_RSVDPAGE = 0,
-	H2C_8821AE_JOINBSSRPT = 1,
-	H2C_8821AE_SCAN = 2,
-	H2C_8821AE_KEEP_ALIVE_CTRL = 3,
-	H2C_8821AE_DISCONNECT_DECISION = 4,
-#if(USE_OLD_WOWLAN_DEBUG_FW == 1)
-	H2C_8821AE_WO_WLAN = 5,
-#endif
-	H2C_8821AE_INIT_OFFLOAD = 6,
-#if(USE_OLD_WOWLAN_DEBUG_FW == 1)
-	H2C_8821AE_REMOTE_WAKE_CTRL = 7,
-#endif
-	H2C_8821AE_AP_OFFLOAD = 8,
-	H2C_8821AE_BCN_RSVDPAGE = 9,
-	H2C_8821AE_PROBERSP_RSVDPAGE = 10,
-
-	H2C_8821AE_SETPWRMODE = 0x20,
-	H2C_8821AE_PS_TUNING_PARA = 0x21,
-	H2C_8821AE_PS_TUNING_PARA2 = 0x22,
-	H2C_8821AE_PS_LPS_PARA = 0x23,
-	H2C_8821AE_P2P_PS_OFFLOAD = 024,
-
-#if(USE_OLD_WOWLAN_DEBUG_FW == 0)
-	H2C_8821AE_WO_WLAN = 0x80,
-	H2C_8821AE_REMOTE_WAKE_CTRL = 0x81,
-	H2C_8821AE_AOAC_GLOBAL_INFO = 0x82,
-	H2C_8821AE_AOAC_RSVDPAGE = 0x83,
-#endif
-	H2C_RSSI_REPORT = 0x42,
-	H2C_8821AE_RA_MASK = 0x40,
-	H2C_8821AE_SELECTIVE_SUSPEND_ROF_CMD,
-	H2C_8821AE_P2P_PS_MODE,
-	H2C_8821AE_PSD_RESULT,
-	/*Not defined CTW CMD for P2P yet*/
-	H2C_8821AE_P2P_PS_CTW_CMD,
-	MAX_8821AE_H2CCMD
-};
-
-#define pagenum_128(_len)		(u32)(((_len)>>7) + ((_len)&0x7F ? 1:0))
-
-#define SET_8821AE_H2CCMD_WOWLAN_FUNC_ENABLE(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_PATTERN_MATCH_ENABLE(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_MAGIC_PKT_ENABLE(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_UNICAST_PKT_ENABLE(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 3, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_ALL_PKT_DROP(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 4, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_GPIO_ACTIVE(__pH2CCmd, __Value)				\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 5, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_REKEY_WAKE_UP(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 6, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_DISCONNECT_WAKE_UP(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 7, 1, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_GPIONUM(__pH2CCmd, __Value)					\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
-#define SET_8821AE_H2CCMD_WOWLAN_GPIO_DURATION(__pH2CCmd, __Value)			\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
-
-
-#define SET_H2CCMD_PWRMODE_PARM_MODE(__ph2ccmd, __val)			\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_H2CCMD_PWRMODE_PARM_RLBM(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 4, __Value)
-#define SET_H2CCMD_PWRMODE_PARM_SMART_PS(__pH2CCmd, __Value)	\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 4, 4, __Value)
-#define SET_H2CCMD_PWRMODE_PARM_AWAKE_INTERVAL(__pH2CCmd, __Value)	\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
-#define SET_H2CCMD_PWRMODE_PARM_ALL_QUEUE_UAPSD(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __Value)
-#define SET_H2CCMD_PWRMODE_PARM_PWR_STATE(__pH2CCmd, __Value)	\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+4, 0, 8, __Value)
-#define GET_8821AE_H2CCMD_PWRMODE_PARM_MODE(__pH2CCmd)		\
-	LE_BITS_TO_1BYTE(__pH2CCmd, 0, 8)
-
-#define SET_H2CCMD_JOINBSSRPT_PARM_OPMODE(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_H2CCMD_RSVDPAGE_LOC_PROBE_RSP(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE(__ph2ccmd, 0, 8, __val)
-#define SET_H2CCMD_RSVDPAGE_LOC_PSPOLL(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+1, 0, 8, __val)
-#define SET_H2CCMD_RSVDPAGE_LOC_NULL_DATA(__ph2ccmd, __val)		\
-	SET_BITS_TO_LE_1BYTE((__ph2ccmd)+2, 0, 8, __val)
-
-/* AP_OFFLOAD */
-#define SET_H2CCMD_AP_OFFLOAD_ON(__pH2CCmd, __Value)	\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
-#define SET_H2CCMD_AP_OFFLOAD_HIDDEN(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
-#define SET_H2CCMD_AP_OFFLOAD_DENYANY(__pH2CCmd, __Value)	\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
-#define SET_H2CCMD_AP_OFFLOAD_WAKEUP_EVT_RPT(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __Value)
-
-/* Keep Alive Control*/
-#define SET_8821AE_H2CCMD_KEEP_ALIVE_ENABLE(__pH2CCmd, __Value)				\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
-#define SET_8821AE_H2CCMD_KEEP_ALIVE_ACCPEPT_USER_DEFINED(__pH2CCmd, __Value)	\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
-#define SET_8821AE_H2CCMD_KEEP_ALIVE_PERIOD(__pH2CCmd, __Value)				\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
-
-/*REMOTE_WAKE_CTRL */
-#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_EN(__pH2CCmd, __Value)				\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 1, __Value)
-#if(USE_OLD_WOWLAN_DEBUG_FW == 0)
-#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_ARP_OFFLOAD_EN(__pH2CCmd, __Value)				\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 1, 1, __Value)
-#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_NDP_OFFLOAD_EN(__pH2CCmd, __Value)				\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 2, 1, __Value)
-#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_GTK_OFFLOAD_EN(__pH2CCmd, __Value)				\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 3, 1, __Value)
-#else
-#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_PAIRWISE_ENC_ALG(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
-#define SET_8821AE_H2CCMD_REMOTE_WAKE_CTRL_GROUP_ENC_ALG(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
-#endif
-
-/* GTK_OFFLOAD */
-#define SET_8821AE_H2CCMD_AOAC_GLOBAL_INFO_PAIRWISE_ENC_ALG(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE(__pH2CCmd, 0, 8, __Value)
-#define SET_8821AE_H2CCMD_AOAC_GLOBAL_INFO_GROUP_ENC_ALG(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
-
-/* AOAC_RSVDPAGE_LOC */
-#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_REMOTE_WAKE_CTRL_INFO(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd), 0, 8, __Value)
-#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_ARP_RSP(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+1, 0, 8, __Value)
-#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_NEIGHBOR_ADV(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+2, 0, 8, __Value)
-#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_RSP(__pH2CCmd, __Value)		\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+3, 0, 8, __Value)
-#define SET_8821AE_H2CCMD_AOAC_RSVDPAGE_LOC_GTK_INFO(__pH2CCmd, __Value)			\
-	SET_BITS_TO_LE_1BYTE((__pH2CCmd)+4, 0, 8, __Value)
-
-int rtl8821ae_download_fw(struct ieee80211_hw *hw,
-				bool buse_wake_on_wlan_fw);
-void rtl8821ae_fill_h2c_cmd(struct ieee80211_hw *hw, u8 element_id,
-			 u32 cmd_len, u8 *p_cmdbuffer);
-void rtl8821ae_firmware_selfreset(struct ieee80211_hw *hw);
-void rtl8821ae_set_fw_pwrmode_cmd(struct ieee80211_hw *hw, u8 mode);
-void rtl8821ae_set_fw_joinbss_report_cmd(struct ieee80211_hw *hw, u8 mstatus);
-void rtl8821ae_set_fw_ap_off_load_cmd(struct ieee80211_hw *hw,  u8 ap_offload_enable);
-void rtl8821ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
-void rtl8812ae_set_fw_rsvdpagepkt(struct ieee80211_hw *hw, bool b_dl_finished);
-void rtl8821ae_set_p2p_ps_offload_cmd(struct ieee80211_hw *hw, u8 p2p_ps_state);
-void rtl8812ae_c2h_packet_handler(struct ieee80211_hw *hw, u8 *buffer, u8 length);
-#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.c b/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.c
deleted file mode 100644
index 8bee772d766f..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.c
+++ /dev/null
@@ -1,519 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "hal_bt_coexist.h"
-#include "../pci.h"
-#include "dm.h"
-#include "fw.h"
-#include "phy.h"
-#include "reg.h"
-#include "hal_btc.h"
-
-static bool bt_operation_on = false;
-
-void rtl8821ae_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw, bool b_reject)
-{
-#if 0
-	struct rtl_priv rtlpriv = rtl_priv(hw);
-	PRX_TS_RECORD			pRxTs = NULL;
-
-	if(b_reject){
-		// Do not allow receiving A-MPDU aggregation.
-		if (rtlpriv->mac80211.vendor == PEER_CISCO) {
-				if (pHTInfo->bAcceptAddbaReq) {
-					RTPRINT(FBT, BT_TRACE, ("BT_Disallow AMPDU \n"));
-					pHTInfo->bAcceptAddbaReq = FALSE;
-					if(GetTs(Adapter, (PTS_COMMON_INFO*)(&pRxTs), pMgntInfo->Bssid, 0, RX_DIR, FALSE))
-						TsInitDelBA(Adapter, (PTS_COMMON_INFO)pRxTs, RX_DIR);
-				}
-			} else {
-				if (!pHTInfo->bAcceptAddbaReq) {
-					RTPRINT(FBT, BT_TRACE, ("BT_Allow AMPDU BT Idle\n"));
-					pHTInfo->bAcceptAddbaReq = TRUE;
-				}
-			}
-		} else {
-			if(rtlpriv->mac80211.vendor == PEER_CISCO) {
-				if (!pHTInfo->bAcceptAddbaReq) {
-					RTPRINT(FBT, BT_TRACE, ("BT_Allow AMPDU \n"));
-					pHTInfo->bAcceptAddbaReq = TRUE;
-				}
-			}
-		}
-#endif
-}
-
-void _rtl8821ae_dm_bt_check_wifi_state(struct ieee80211_hw *hw)
-{
-struct rtl_priv *rtlpriv = rtl_priv(hw);
-struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-if (rtlpriv->link_info.b_busytraffic) {
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_IDLE;
-
-	if(rtlpriv->link_info.b_tx_busy_traffic) {
-		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_UPLINK;
-	} else {
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_UPLINK;
-	}
-
-	if(rtlpriv->link_info.b_rx_busy_traffic) {
-		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_DOWNLINK;
-	} else {
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_DOWNLINK;
-	}
-} else {
-	rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_IDLE;
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_UPLINK;
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_DOWNLINK;
-}
-
-if (rtlpriv->mac80211.mode == WIRELESS_MODE_G
-	|| rtlpriv->mac80211.mode == WIRELESS_MODE_B) {
-	rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_LEGACY;
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT20;
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT40;
-} else {
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_LEGACY;
-	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_HT40;
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT20;
-	} else {
-		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_HT20;
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_HT40;
-	}
-}
-
-if (bt_operation_on) {
-	rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BT30;
-} else {
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT30;
-}
-}
-
-
-u8 rtl8821ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
-						u8	level_num, u8	rssi_thresh, u8 rssi_thresh1)
-
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long undecoratedsmoothed_pwdb = 0;
-	u8 bt_rssi_state = 0;
-
-	undecoratedsmoothed_pwdb =  rtl8821ae_dm_bt_get_rx_ss(hw);
-
-	if(level_num == 2) {
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-
-		if( (rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
-			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)) {
-			if(undecoratedsmoothed_pwdb >= (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
-				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to High\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at Low\n"));
-			}
-		} else {
-			if(undecoratedsmoothed_pwdb < rssi_thresh) {
-				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to Low\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at High\n"));
-			}
-		}
-	} else if(level_num == 3) {
-		if(rssi_thresh > rssi_thresh1) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 thresh error!!\n"));
-			return rtlpcipriv->btcoexist.bt_pre_rssi_state;
-		}
-
-		if( (rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
-			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)) {
-			if(undecoratedsmoothed_pwdb >= (rssi_thresh+BT_FW_COEX_THRESH_TOL)) {
-				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to Medium\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at Low\n"));
-			}
-		} else if( (rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_MEDIUM) ||
-			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_MEDIUM)) {
-			if(undecoratedsmoothed_pwdb >= (rssi_thresh1 + BT_FW_COEX_THRESH_TOL)) {
-				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to High\n"));
-			} else if(undecoratedsmoothed_pwdb < rssi_thresh) {
-				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state switch to Low\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_MEDIUM;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at Medium\n"));
-			}
-		} else {
-			if(undecoratedsmoothed_pwdb < rssi_thresh1) {
-				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_WIFI_RSSI_1_MEDIUM;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_HIGH;
-				rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_1_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,("[DM][BT], RSSI_1 state switch to Medium\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[DM][BT], RSSI_1 state stay at High\n"));
-			}
-		}
-	}
-
-	rtlpcipriv->btcoexist.bt_pre_rssi_state1 = bt_rssi_state;
-
-	return bt_rssi_state;
-}
-
-u8 rtl8821ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
-						u8	level_num, u8	rssi_thresh, u8 rssi_thresh1)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long undecoratedsmoothed_pwdb = 0;
-	u8 bt_rssi_state = 0;
-
-	undecoratedsmoothed_pwdb = rtl8821ae_dm_bt_get_rx_ss(hw);
-
-	if (level_num == 2) {
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-
-		if ((rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
-			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)){
-			if (undecoratedsmoothed_pwdb
-				>= (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
-				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->btcoexist.current_state
-					|= BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state switch to High\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state stay at Low\n"));
-			}
-		} else {
-			if (undecoratedsmoothed_pwdb < rssi_thresh) {
-				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->btcoexist.current_state
-					|= BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state switch to Low\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state stay at High\n"));
-			}
-		}
-	}
-	else if (level_num == 3) {
-		if (rssi_thresh > rssi_thresh1) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-				("[DM][BT], RSSI thresh error!!\n"));
-			return rtlpcipriv->btcoexist.bt_pre_rssi_state;
-		}
-		if ((rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_LOW) ||
-			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_LOW)) {
-			if(undecoratedsmoothed_pwdb
-				>= (rssi_thresh + BT_FW_COEX_THRESH_TOL)) {
-				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->btcoexist.current_state
-					|= BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state switch to Medium\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state stay at Low\n"));
-			}
-		} else if ((rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_MEDIUM) ||
-			(rtlpcipriv->btcoexist.bt_pre_rssi_state == BT_RSSI_STATE_STAY_MEDIUM)) {
-			if (undecoratedsmoothed_pwdb
-				>= (rssi_thresh1 + BT_FW_COEX_THRESH_TOL)) {
-				bt_rssi_state = BT_RSSI_STATE_HIGH;
-				rtlpcipriv->btcoexist.current_state
-					|= BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state switch to High\n"));
-			} else if(undecoratedsmoothed_pwdb < rssi_thresh)
-			{
-				bt_rssi_state = BT_RSSI_STATE_LOW;
-				rtlpcipriv->btcoexist.current_state
-					|= BT_COEX_STATE_WIFI_RSSI_LOW;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state switch to Low\n"));
-			} else {
-				bt_rssi_state = BT_RSSI_STATE_STAY_MEDIUM;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state stay at Medium\n"));
-			}
-		} else {
-			if(undecoratedsmoothed_pwdb < rssi_thresh1) {
-				bt_rssi_state = BT_RSSI_STATE_MEDIUM;
-				rtlpcipriv->btcoexist.current_state
-					|= BT_COEX_STATE_WIFI_RSSI_MEDIUM;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_HIGH;
-				rtlpcipriv->btcoexist.current_state
-					&= ~BT_COEX_STATE_WIFI_RSSI_LOW;
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-					("[DM][BT], RSSI state switch to Medium\n"));
-			} else {
-			bt_rssi_state = BT_RSSI_STATE_STAY_HIGH;
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-				("[DM][BT], RSSI state stay at High\n"));
-			}
-		}
-	}
-
-	rtlpcipriv->btcoexist.bt_pre_rssi_state = bt_rssi_state;
-	return bt_rssi_state;
-}
-long rtl8821ae_dm_bt_get_rx_ss(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	long undecoratedsmoothed_pwdb = 0;
-
-	if (rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-		undecoratedsmoothed_pwdb = GET_UNDECORATED_AVERAGE_RSSI(rtlpriv);
-	} else {
-		undecoratedsmoothed_pwdb
-			= rtlpriv->dm.entry_min_undecoratedsmoothed_pwdb;
-	}
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("rtl8821ae_dm_bt_get_rx_ss() = %ld\n", undecoratedsmoothed_pwdb));
-
-	return undecoratedsmoothed_pwdb;
-}
-
-void rtl8821ae_dm_bt_balance(struct ieee80211_hw *hw,
-							bool b_balance_on, u8 ms0, u8 ms1)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[3] ={0};
-
-	if (b_balance_on) {
-		h2c_parameter[2] = 1;
-		h2c_parameter[1] = ms1;
-		h2c_parameter[0] = ms0;
-		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
-	} else {
-		h2c_parameter[2] = 0;
-		h2c_parameter[1] = 0;
-		h2c_parameter[0] = 0;
-	}
-	rtlpcipriv->btcoexist.b_balance_on = b_balance_on;
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[DM][BT], Balance=[%s:%dms:%dms], write 0xc=0x%x\n",
-		b_balance_on?"ON":"OFF", ms0, ms1,
-		h2c_parameter[0]<<16 | h2c_parameter[1]<<8 | h2c_parameter[2]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0xc, 3, h2c_parameter);
-}
-
-
-void rtl8821ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-
-	if (type == BT_AGCTABLE_OFF) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]AGCTable Off!\n"));
-		rtl_write_dword(rtlpriv, 0xc78,0x641c0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x631d0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x621e0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x611f0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x60200001);
-
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0x32000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0x71000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0xb0000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0xfc000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_G1, 0xfffff, 0x30355);
-	} else if (type == BT_AGCTABLE_ON) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]AGCTable On!\n"));
-		rtl_write_dword(rtlpriv, 0xc78,0x4e1c0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x4d1d0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x4c1e0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x4b1f0001);
-		rtl_write_dword(rtlpriv, 0xc78,0x4a200001);
-
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0xdc000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0x90000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0x51000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_AGC_HP, 0xfffff, 0x12000);
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A,
-					RF_RX_G1, 0xfffff, 0x00355);
-
-		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
-	}
-}
-
-void rtl8821ae_dm_bt_bb_back_off_level(struct ieee80211_hw *hw, u8 type)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-
-	if (type == BT_BB_BACKOFF_OFF) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]BBBackOffLevel Off!\n"));
-		rtl_write_dword(rtlpriv, 0xc04,0x3a05611);
-	} else if (type == BT_BB_BACKOFF_ON) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BT]BBBackOffLevel On!\n"));
-		rtl_write_dword(rtlpriv, 0xc04,0x3a07611);
-		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
-	}
-}
-
-void rtl8821ae_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("rtl8821ae_dm_bt_fw_coex_all_off()\n"));
-
-	if(rtlpcipriv->btcoexist.b_fw_coexist_all_off)
-		return;
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("rtl8821ae_dm_bt_fw_coex_all_off(), real Do\n"));
-	rtl8821ae_dm_bt_fw_coex_all_off_8723a(hw);
-	rtlpcipriv->btcoexist.b_fw_coexist_all_off = true;
-}
-
-void rtl8821ae_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("rtl8821ae_dm_bt_sw_coex_all_off()\n"));
-
-	if(rtlpcipriv->btcoexist.b_sw_coexist_all_off)
-		return;
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("rtl8821ae_dm_bt_sw_coex_all_off(), real Do\n"));
-	rtl8821ae_dm_bt_sw_coex_all_off_8723a(hw);
-	rtlpcipriv->btcoexist.b_sw_coexist_all_off = true;
-}
-
-void rtl8821ae_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("rtl8821ae_dm_bt_hw_coex_all_off()\n"));
-
-	if(rtlpcipriv->btcoexist.b_hw_coexist_all_off)
-		return;
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("rtl8821ae_dm_bt_hw_coex_all_off(), real Do\n"));
-
-	rtl8821ae_dm_bt_hw_coex_all_off_8723a(hw);
-
-	rtlpcipriv->btcoexist.b_hw_coexist_all_off = true;
-}
-
-void rtl8821ae_btdm_coex_all_off(struct ieee80211_hw *hw)
-{
-	rtl8821ae_dm_bt_fw_coex_all_off(hw);
-	rtl8821ae_dm_bt_sw_coex_all_off(hw);
-	rtl8821ae_dm_bt_hw_coex_all_off(hw);
-}
-
-bool rtl8821ae_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-
-	if((rtlpcipriv->btcoexist.previous_state
-		== rtlpcipriv->btcoexist.current_state)
-		&& (rtlpcipriv->btcoexist.previous_state_h
-		== rtlpcipriv->btcoexist.current_state_h))
-		return false;
-	else
-		return true;
-}
-
-bool rtl8821ae_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->link_info.b_tx_busy_traffic)
-		return true;
-	else
-		return false;
-}
-
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.h b/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.h
deleted file mode 100644
index b365f82f481c..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/hal_bt_coexist.h
+++ /dev/null
@@ -1,169 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8821AE_HAL_BT_COEXIST_H__
-#define __RTL8821AE_HAL_BT_COEXIST_H__
-
-#include "../wifi.h"
-
-/* The reg define is for 8723 */
-#define	REG_HIGH_PRIORITY_TXRX			0x770
-#define	REG_LOW_PRIORITY_TXRX			0x774
-
-#define BT_FW_COEX_THRESH_TOL			6
-#define BT_FW_COEX_THRESH_20			20
-#define BT_FW_COEX_THRESH_23			23
-#define BT_FW_COEX_THRESH_25			25
-#define BT_FW_COEX_THRESH_30			30
-#define BT_FW_COEX_THRESH_35			35
-#define BT_FW_COEX_THRESH_40			40
-#define BT_FW_COEX_THRESH_45			45
-#define BT_FW_COEX_THRESH_47			47
-#define BT_FW_COEX_THRESH_50			50
-#define BT_FW_COEX_THRESH_55			55
-
-#define BT_COEX_STATE_BT30				BIT(0)
-#define BT_COEX_STATE_WIFI_HT20			BIT(1)
-#define BT_COEX_STATE_WIFI_HT40			BIT(2)
-#define BT_COEX_STATE_WIFI_LEGACY		BIT(3)
-
-#define BT_COEX_STATE_WIFI_RSSI_LOW		BIT(4)
-#define BT_COEX_STATE_WIFI_RSSI_MEDIUM	BIT(5)
-#define BT_COEX_STATE_WIFI_RSSI_HIGH	BIT(6)
-#define BT_COEX_STATE_DEC_BT_POWER		BIT(7)
-
-#define BT_COEX_STATE_WIFI_IDLE			BIT(8)
-#define BT_COEX_STATE_WIFI_UPLINK		BIT(9)
-#define BT_COEX_STATE_WIFI_DOWNLINK		BIT(10)
-
-#define BT_COEX_STATE_BT_INQ_PAGE			BIT(11)
-#define BT_COEX_STATE_BT_IDLE			BIT(12)
-#define BT_COEX_STATE_BT_UPLINK			BIT(13)
-#define BT_COEX_STATE_BT_DOWNLINK		BIT(14)
-
-#define BT_COEX_STATE_HOLD_FOR_BT_OPERATION	BIT(15)
-#define BT_COEX_STATE_BT_RSSI_LOW		BIT(19)
-
-#define BT_COEX_STATE_PROFILE_HID		BIT(20)
-#define BT_COEX_STATE_PROFILE_A2DP		BIT(21)
-#define BT_COEX_STATE_PROFILE_PAN		BIT(22)
-#define BT_COEX_STATE_PROFILE_SCO		BIT(23)
-
-#define BT_COEX_STATE_WIFI_RSSI_1_LOW		BIT(24)
-#define BT_COEX_STATE_WIFI_RSSI_1_MEDIUM	BIT(25)
-#define BT_COEX_STATE_WIFI_RSSI_1_HIGH		BIT(26)
-
-#define BT_COEX_STATE_BTINFO_COMMON			BIT(30)
-#define BT_COEX_STATE_BTINFO_B_HID_SCOESCO	BIT(31)
-#define BT_COEX_STATE_BTINFO_B_FTP_A2DP		BIT(29)
-
-#define BT_COEX_STATE_BT_CNT_LEVEL_0		BIT(0)
-#define BT_COEX_STATE_BT_CNT_LEVEL_1		BIT(1)
-#define BT_COEX_STATE_BT_CNT_LEVEL_2		BIT(2)
-#define BT_COEX_STATE_BT_CNT_LEVEL_3		BIT(3)
-
-#define BT_RSSI_STATE_HIGH			0
-#define BT_RSSI_STATE_MEDIUM		1
-#define BT_RSSI_STATE_LOW			2
-#define BT_RSSI_STATE_STAY_HIGH		3
-#define BT_RSSI_STATE_STAY_MEDIUM	4
-#define BT_RSSI_STATE_STAY_LOW		5
-
-#define	BT_AGCTABLE_OFF				0
-#define	BT_AGCTABLE_ON				1
-#define	BT_BB_BACKOFF_OFF			0
-#define	BT_BB_BACKOFF_ON			1
-#define	BT_FW_NAV_OFF				0
-#define	BT_FW_NAV_ON				1
-
-#define	BT_COEX_MECH_NONE			0
-#define	BT_COEX_MECH_SCO			1
-#define	BT_COEX_MECH_HID			2
-#define	BT_COEX_MECH_A2DP			3
-#define	BT_COEX_MECH_PAN			4
-#define	BT_COEX_MECH_HID_A2DP		5
-#define	BT_COEX_MECH_HID_PAN		6
-#define	BT_COEX_MECH_PAN_A2DP		7
-#define	BT_COEX_MECH_HID_SCO_ESCO	8
-#define	BT_COEX_MECH_FTP_A2DP		9
-#define	BT_COEX_MECH_COMMON			10
-#define	BT_COEX_MECH_MAX			11
-
-#define	BT_DBG_PROFILE_NONE			0
-#define	BT_DBG_PROFILE_SCO			1
-#define	BT_DBG_PROFILE_HID			2
-#define	BT_DBG_PROFILE_A2DP			3
-#define	BT_DBG_PROFILE_PAN			4
-#define	BT_DBG_PROFILE_HID_A2DP		5
-#define	BT_DBG_PROFILE_HID_PAN		6
-#define	BT_DBG_PROFILE_PAN_A2DP		7
-#define	BT_DBG_PROFILE_MAX			9
-
-#define	BTINFO_B_FTP					BIT(7)
-#define	BTINFO_B_A2DP					BIT(6)
-#define	BTINFO_B_HID					BIT(5)
-#define	BTINFO_B_SCO_BUSY				BIT(4)
-#define	BTINFO_B_ACL_BUSY				BIT(3)
-#define	BTINFO_B_INQ_PAGE				BIT(2)
-#define	BTINFO_B_SCO_ESCO				BIT(1)
-#define	BTINFO_B_CONNECTION				BIT(0)
-
-
-void rtl8821ae_btdm_coex_all_off(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_fw_coex_all_off(struct ieee80211_hw *hw);
-
-void rtl8821ae_dm_bt_sw_coex_all_off(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_hw_coex_all_off(struct ieee80211_hw *hw);
-long rtl8821ae_dm_bt_get_rx_ss(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_balance(struct ieee80211_hw *hw,
-			bool b_balance_on, u8 ms0, u8 ms1);
-void rtl8821ae_dm_bt_agc_table(struct ieee80211_hw *hw, u8 type);
-void rtl8821ae_dm_bt_bb_back_off_level(struct ieee80211_hw *hw, u8 type);
-u8 rtl8821ae_dm_bt_check_coex_rssi_state(struct ieee80211_hw *hw,
-						u8	level_num, u8	rssi_thresh, u8 rssi_thresh1);
-u8 rtl8821ae_dm_bt_check_coex_rssi_state1(struct ieee80211_hw *hw,
-		                        u8  level_num, u8   rssi_thresh, u8 rssi_thresh1);
-void _rtl8821ae_dm_bt_check_wifi_state(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_reject_ap_aggregated_packet(struct ieee80211_hw *hw,
-			bool b_reject);
-
-#if 0
-VOID
-BTDM_PWDBMonitor(
-	 	PADAPTER	Adapter
-	);
-
-BOOLEAN
-BTDM_DIGByBTRSSI(
-	 		PADAPTER	Adapter
-	);
-#endif
-bool rtl8821ae_dm_bt_is_coexist_state_changed(struct ieee80211_hw *hw);
-bool rtl8821ae_dm_bt_is_wifi_up_link(struct ieee80211_hw *hw);
-#endif
-
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.c b/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.c
deleted file mode 100644
index f885ca77344b..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.c
+++ /dev/null
@@ -1,2054 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include "hal_btc.h"
-#include "../pci.h"
-#include "phy.h"
-#include "fw.h"
-#include "reg.h"
-#include "def.h"
-#include "../btcoexist/rtl_btc.h"
-
-static struct bt_coexist_8821ae hal_coex_8821ae;
-
-void rtl8821ae_dm_bt_turn_off_bt_coexist_before_enter_lps(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-    struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	if(!rtlpcipriv->btcoexist.bt_coexistence)
-		return;
-
-	if(ppsc->b_inactiveps) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,("[BT][DM], Before enter IPS, turn off all Coexist DM\n"));
-		rtlpcipriv->btcoexist.current_state = 0;
-		rtlpcipriv->btcoexist.previous_state = 0;
-		rtlpcipriv->btcoexist.current_state_h = 0;
-		rtlpcipriv->btcoexist.previous_state_h = 0;
-		rtl8821ae_btdm_coex_all_off(hw);
-	}
-}
-
-
-enum rt_media_status mgnt_link_status_query(struct ieee80211_hw *hw)
-{
-    struct rtl_priv *rtlpriv = rtl_priv(hw);
-    struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-    enum rt_media_status    m_status = RT_MEDIA_DISCONNECT;
-
-    u8 bibss = (mac->opmode == NL80211_IFTYPE_ADHOC) ? 1 : 0;
-
-    if(bibss || rtlpriv->mac80211.link_state >= MAC80211_LINKED) {
-            m_status = RT_MEDIA_CONNECT;
-    }
-
-    return m_status;
-}
-
-void rtl_8821ae_bt_wifi_media_status_notify(struct ieee80211_hw *hw, bool mstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 h2c_parameter[3] ={0};
-	u8 chnl;
-
-	if(!rtlpcipriv->btcoexist.bt_coexistence)
-		return;
-
-	if(RT_MEDIA_CONNECT == mstatus)
-		h2c_parameter[0] = 0x1; // 0: disconnected, 1:connected
-	else
-		h2c_parameter[0] = 0x0;
-
-	if(mgnt_link_status_query(hw))	{
-		chnl = rtlphy->current_channel;
-		h2c_parameter[1] = chnl;
-	}
-
-	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40){
-		h2c_parameter[2] = 0x30;
-	} else {
-		h2c_parameter[2] = 0x20;
-	}
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,("[BTCoex], FW write 0x19=0x%x\n",
-		h2c_parameter[0]<<16|h2c_parameter[1]<<8|h2c_parameter[2]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x19, 3, h2c_parameter);
-
-}
-
-
-bool rtl8821ae_dm_bt_is_wifi_busy(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	if(rtlpriv->link_info.b_busytraffic ||
-		rtlpriv->link_info.b_rx_busy_traffic ||
-		rtlpriv->link_info.b_tx_busy_traffic)
-		return true;
-	else
-		return false;
-}
-void rtl8821ae_dm_bt_set_fw_3a(struct ieee80211_hw *hw,
-						u8 byte1, u8 byte2, u8 byte3, u8 byte4, u8 byte5)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[5] ={0};
-	h2c_parameter[0] = byte1;
-	h2c_parameter[1] = byte2;
-	h2c_parameter[2] = byte3;
-	h2c_parameter[3] = byte4;
-	h2c_parameter[4] = byte5;
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], FW write 0x3a(4bytes)=0x%x%8x\n",
-		h2c_parameter[0], h2c_parameter[1]<<24 | h2c_parameter[2]<<16 | h2c_parameter[3]<<8 | h2c_parameter[4]));
-	rtl8821ae_fill_h2c_cmd(hw, 0x3a, 5, h2c_parameter);
-}
-
-bool rtl8821ae_dm_bt_need_to_dec_bt_pwr(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (mgnt_link_status_query(hw) == RT_MEDIA_CONNECT) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Need to decrease bt power\n"));
-		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_DEC_BT_POWER;
-			return true;
-	}
-
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_DEC_BT_POWER;
-	return false;
-}
-
-
-bool rtl8821ae_dm_bt_is_same_coexist_state(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-
-	if ((rtlpcipriv->btcoexist.previous_state
-		== rtlpcipriv->btcoexist.current_state)
-		&&(rtlpcipriv->btcoexist.previous_state_h
-		== rtlpcipriv->btcoexist.current_state_h)) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-				("[DM][BT], Coexist state do not change!!\n"));
-		return true;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-				("[DM][BT], Coexist state changed!!\n"));
-		return false;
-	}
-}
-
-void rtl8821ae_dm_bt_set_coex_table(struct ieee80211_hw *hw,
-						u32 val_0x6c0, u32 val_0x6c8, u32 val_0x6cc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("set coex table, set 0x6c0=0x%x\n", val_0x6c0));
-	rtl_write_dword(rtlpriv, 0x6c0, val_0x6c0);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("set coex table, set 0x6c8=0x%x\n", val_0x6c8));
-	rtl_write_dword(rtlpriv, 0x6c8, val_0x6c8);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("set coex table, set 0x6cc=0x%x\n", val_0x6cc));
-	rtl_write_byte(rtlpriv, 0x6cc, val_0x6cc);
-}
-
-void rtl8821ae_dm_bt_set_hw_pta_mode(struct ieee80211_hw *hw, bool b_mode)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (BT_PTA_MODE_ON == b_mode) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("PTA mode on, "));
-		/*  Enable GPIO 0/1/2/3/8 pins for bt */
-		rtl_write_byte(rtlpriv, 0x40, 0x20);
-		rtlpcipriv->btcoexist.b_hw_coexist_all_off = false;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("PTA mode off\n"));
-		rtl_write_byte(rtlpriv, 0x40, 0x0);
-	}
-}
-
-void rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(struct ieee80211_hw *hw, u8 type)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (BT_RF_RX_LPF_CORNER_SHRINK == type) {
-		/* Shrink RF Rx LPF corner, 0x1e[7:4]=1111 ==> [11:4] by Jenyu */
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Shrink RF Rx LPF corner!!\n"));
-		/* PHY_SetRFReg(Adapter, (RF_RADIO_PATH_E)PathA, 0x1e, 0xf0, 0xf); */
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e, 0xfffff, 0xf0ff7);
-		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
-	} else if(BT_RF_RX_LPF_CORNER_RESUME == type) {
-		/*Resume RF Rx LPF corner*/
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Resume RF Rx LPF corner!!\n"));
-		/* PHY_SetRFReg(Adapter, (RF_RADIO_PATH_E)PathA, 0x1e, 0xf0,
-		 * pHalData->btcoexist.BtRfRegOrigin1E); */
-		rtl8821ae_phy_set_rf_reg(hw, RF90_PATH_A, 0x1e, 0xfffff,
-			rtlpcipriv->btcoexist.bt_rfreg_origin_1e);
-	}
-}
-
-void rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(struct ieee80211_hw *hw,
-																	u8 ra_type)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u8 tmp_u1;
-
-	tmp_u1 = rtl_read_byte(rtlpriv, 0x4fd);
-	tmp_u1 |= BIT(0);
-	if (BT_TX_RATE_ADAPTIVE_LOW_PENALTY == ra_type) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Tx rate adaptive, set low penalty!!\n"));
-		tmp_u1 &= ~BIT(2);
-		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
-	} else if(BT_TX_RATE_ADAPTIVE_NORMAL == ra_type) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Tx rate adaptive, set normal!!\n"));
-		tmp_u1 |= BIT(2);
-	}
-
-	rtl_write_byte(rtlpriv, 0x4fd, tmp_u1);
-}
-
-void rtl8821ae_dm_bt_btdm_structure_reload(struct ieee80211_hw *hw,
-													struct btdm_8821ae	*p_btdm)
-{
-	p_btdm->b_all_off = false;
-	p_btdm->b_agc_table_en = false;
-	p_btdm->b_adc_back_off_on = false;
-	p_btdm->b2_ant_hid_en = false;
-	p_btdm->b_low_penalty_rate_adaptive = false;
-	p_btdm->b_rf_rx_lpf_shrink = false;
-	p_btdm->b_reject_aggre_pkt= false;
-
-	p_btdm->b_tdma_on = false;
-	p_btdm->tdma_ant = TDMA_2ANT;
-	p_btdm->tdma_nav = TDMA_NAV_OFF;
-	p_btdm->tdma_dac_swing = TDMA_DAC_SWING_OFF;
-	p_btdm->fw_dac_swing_lvl = 0x20;
-
-	p_btdm->b_tra_tdma_on = false;
-	p_btdm->tra_tdma_ant = TDMA_2ANT;
-	p_btdm->tra_tdma_nav = TDMA_NAV_OFF;
-	p_btdm->b_ignore_wlan_act = false;
-
-	p_btdm->b_ps_tdma_on = false;
-	p_btdm->ps_tdma_byte[0] = 0x0;
-	p_btdm->ps_tdma_byte[1] = 0x0;
-	p_btdm->ps_tdma_byte[2] = 0x0;
-	p_btdm->ps_tdma_byte[3] = 0x8;
-	p_btdm->ps_tdma_byte[4] = 0x0;
-
-	p_btdm->b_pta_on = true;
-	p_btdm->val_0x6c0 = 0x5a5aaaaa;
-	p_btdm->val_0x6c8 = 0xcc;
-	p_btdm->val_0x6cc = 0x3;
-
-	p_btdm->b_sw_dac_swing_on = false;
-	p_btdm->sw_dac_swing_lvl = 0xc0;
-	p_btdm->wlan_act_hi = 0x20;
-	p_btdm->wlan_act_lo = 0x10;
-	p_btdm->bt_retry_index = 2;
-
-	p_btdm->b_dec_bt_pwr = false;
-}
-
-void rtl8821ae_dm_bt_btdm_structure_reload_all_off(struct ieee80211_hw *hw,
-													struct btdm_8821ae	*p_btdm)
-{
-	rtl8821ae_dm_bt_btdm_structure_reload(hw, p_btdm);
-	p_btdm->b_all_off = true;
-	p_btdm->b_pta_on = false;
-	p_btdm->wlan_act_hi = 0x10;
-}
-
-bool rtl8821ae_dm_bt_is_2_ant_common_action(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct btdm_8821ae btdm8821ae;
-	bool b_common = false;
-
-	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
-
-	if(!rtl8821ae_dm_bt_is_wifi_busy(hw)
-		&& !rtlpcipriv->btcoexist.b_bt_busy) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("Wifi idle + Bt idle, bt coex mechanism always off!!\n"));
-		rtl8821ae_dm_bt_btdm_structure_reload_all_off(hw, &btdm8821ae);
-		b_common = true;
-	} else if (rtl8821ae_dm_bt_is_wifi_busy(hw)
-		&& !rtlpcipriv->btcoexist.b_bt_busy) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("Wifi non-idle + Bt disabled/idle!!\n"));
-		btdm8821ae.b_low_penalty_rate_adaptive = true;
-		btdm8821ae.b_rf_rx_lpf_shrink = false;
-		btdm8821ae.b_reject_aggre_pkt = false;
-
-		/* sw mechanism */
-		btdm8821ae.b_agc_table_en = false;
-		btdm8821ae.b_adc_back_off_on = false;
-		btdm8821ae.b_sw_dac_swing_on = false;
-
-		btdm8821ae.b_pta_on = true;
-		btdm8821ae.val_0x6c0 = 0x5a5aaaaa;
-		btdm8821ae.val_0x6c8 = 0xcccc;
-		btdm8821ae.val_0x6cc = 0x3;
-
-		btdm8821ae.b_tdma_on = false;
-		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
-		btdm8821ae.b2_ant_hid_en = false;
-
-		b_common = true;
-	}else if (rtlpcipriv->btcoexist.b_bt_busy) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("Bt non-idle!\n"));
-		if(mgnt_link_status_query(hw) == RT_MEDIA_CONNECT){
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi connection exist\n"))
-			b_common = false;
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-				("No Wifi connection!\n"));
-			btdm8821ae.b_rf_rx_lpf_shrink = true;
-			btdm8821ae.b_low_penalty_rate_adaptive = false;
-			btdm8821ae.b_reject_aggre_pkt = false;
-
-			/* sw mechanism */
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = false;
-			btdm8821ae.b_sw_dac_swing_on = false;
-
-			btdm8821ae.b_pta_on = true;
-			btdm8821ae.val_0x6c0 = 0x55555555;
-			btdm8821ae.val_0x6c8 = 0x0000ffff;
-			btdm8821ae.val_0x6cc = 0x3;
-
-			btdm8821ae.b_tdma_on = false;
-			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
-			btdm8821ae.b2_ant_hid_en = false;
-
-			b_common = true;
-		}
-	}
-
-	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
-		btdm8821ae.b_dec_bt_pwr = true;
-	}
-
-	if(b_common)
-		 rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_COMMON;
-
-	if (b_common && rtl8821ae_dm_bt_is_coexist_state_changed(hw))
-		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
-
-	return b_common;
-}
-
-void rtl8821ae_dm_bt_set_sw_full_time_dac_swing(
-		struct ieee80211_hw * hw, bool b_sw_dac_swing_on, u32 sw_dac_swing_lvl)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (b_sw_dac_swing_on) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], SwDacSwing = 0x%x\n", sw_dac_swing_lvl));
-		rtl8821ae_phy_set_bb_reg(hw, 0x880, 0xff000000, sw_dac_swing_lvl);
-		rtlpcipriv->btcoexist.b_sw_coexist_all_off = false;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], SwDacSwing Off!\n"));
-		rtl8821ae_phy_set_bb_reg(hw, 0x880, 0xff000000, 0xc0);
-	}
-}
-
-void rtl8821ae_dm_bt_set_fw_dec_bt_pwr(
-		struct ieee80211_hw *hw, bool b_dec_bt_pwr)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[1] ={0};
-
-	h2c_parameter[0] = 0;
-
-	if (b_dec_bt_pwr) {
-		h2c_parameter[0] |= BIT(1);
-		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
-	}
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], decrease Bt Power : %s, write 0x21=0x%x\n",
-		(b_dec_bt_pwr? "Yes!!":"No!!"), h2c_parameter[0]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x21, 1, h2c_parameter);
-}
-
-
-void rtl8821ae_dm_bt_set_fw_2_ant_hid(struct ieee80211_hw *hw,
-									bool b_enable, bool b_dac_swing_on)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[1] ={0};
-
-	if (b_enable) {
-		h2c_parameter[0] |= BIT(0);
-		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
-	}
-	if (b_dac_swing_on) {
-		h2c_parameter[0] |= BIT(1); /* Dac Swing default enable */
-	}
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], turn 2-Ant+HID mode %s, DACSwing:%s, write 0x15=0x%x\n",
-		(b_enable ? "ON!!":"OFF!!"), (b_dac_swing_on ? "ON":"OFF"),
-		h2c_parameter[0]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x15, 1, h2c_parameter);
-}
-
-void rtl8821ae_dm_bt_set_fw_tdma_ctrl(struct ieee80211_hw *hw,
-				bool b_enable, u8 ant_num, u8 nav_en, u8 dac_swing_en)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u8 h2c_parameter[1] ={0};
-	u8 h2c_parameter1[1] = {0};
-
-	h2c_parameter[0] = 0;
-	h2c_parameter1[0] = 0;
-
-	if(b_enable) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], set BT PTA update manager to trigger update!!\n"));
-		h2c_parameter1[0] |= BIT(0);
-
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], turn TDMA mode ON!!\n"));
-		h2c_parameter[0] |= BIT(0);		/* function enable */
-		if (TDMA_1ANT == ant_num) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_1ANT\n"));
-			h2c_parameter[0] |= BIT(1);
-		} else if(TDMA_2ANT == ant_num) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_2ANT\n"));
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], Unknown Ant\n"));
-		}
-
-		if (TDMA_NAV_OFF == nav_en) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_NAV_OFF\n"));
-		} else if (TDMA_NAV_ON == nav_en) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TDMA_NAV_ON\n"));
-			h2c_parameter[0] |= BIT(2);
-		}
-
-		if (TDMA_DAC_SWING_OFF == dac_swing_en) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-				("[BTCoex], TDMA_DAC_SWING_OFF\n"));
-		} else if(TDMA_DAC_SWING_ON == dac_swing_en) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-				("[BTCoex], TDMA_DAC_SWING_ON\n"));
-			h2c_parameter[0] |= BIT(4);
-		}
-		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], set BT PTA update manager to no update!!\n"));
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], turn TDMA mode OFF!!\n"));
-	}
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], FW2AntTDMA, write 0x26=0x%x\n", h2c_parameter1[0]));
-	rtl8821ae_fill_h2c_cmd(hw, 0x26, 1, h2c_parameter1);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], FW2AntTDMA, write 0x14=0x%x\n", h2c_parameter[0]));
-	rtl8821ae_fill_h2c_cmd(hw, 0x14, 1, h2c_parameter);
-
-	if (!b_enable) {
-		/* delay_ms(2);
-		 * PlatformEFIOWrite1Byte(Adapter, 0x778, 0x1); */
-	}
-}
-
-
-void rtl8821ae_dm_bt_set_fw_ignore_wlan_act( struct ieee80211_hw *hw, bool b_enable)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u8 h2c_parameter[1] ={0};
-
-	if (b_enable) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], BT Ignore Wlan_Act !!\n"));
-		h2c_parameter[0] |= BIT(0);		// function enable
-		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], BT don't ignore Wlan_Act !!\n"));
-	}
-
-    RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], set FW for BT Ignore Wlan_Act, write 0x25=0x%x\n",
-		h2c_parameter[0]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x25, 1, h2c_parameter);
-}
-
-
-void rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(struct ieee80211_hw *hw,
-		bool b_enable, u8 ant_num, u8 nav_en
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	//struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	u8 h2c_parameter[2] ={0};
-
-
-	if (b_enable) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], turn TTDMA mode ON!!\n"));
-		h2c_parameter[0] |= BIT(0);		// function enable
-		if (TDMA_1ANT == ant_num) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_1ANT\n"));
-			h2c_parameter[0] |= BIT(1);
-		} else if (TDMA_2ANT == ant_num) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_2ANT\n"));
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], Unknown Ant\n"));
-		}
-
-		if (TDMA_NAV_OFF == nav_en) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_NAV_OFF\n"));
-		} else if (TDMA_NAV_ON == nav_en) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], TTDMA_NAV_ON\n"));
-			h2c_parameter[1] |= BIT(0);
-		}
-
-		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], turn TTDMA mode OFF!!\n"));
-	}
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], FW Traditional TDMA, write 0x33=0x%x\n",
-		h2c_parameter[0] << 8| h2c_parameter[1]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x33, 2, h2c_parameter);
-}
-
-
-void rtl8821ae_dm_bt_set_fw_dac_swing_level(struct ieee80211_hw *hw,
-													u8 dac_swing_lvl)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[1] ={0};
-	h2c_parameter[0] = dac_swing_lvl;
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], Set Dac Swing Level=0x%x\n", dac_swing_lvl));
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], write 0x29=0x%x\n", h2c_parameter[0]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x29, 1, h2c_parameter);
-}
-
-void rtl8821ae_dm_bt_set_fw_bt_hid_info(struct ieee80211_hw *hw, bool b_enable)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[1] ={0};
-	h2c_parameter[0] = 0;
-
-	if(b_enable){
-		h2c_parameter[0] |= BIT(0);
-		rtlpcipriv->btcoexist.b_fw_coexist_all_off = false;
-	}
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], Set BT HID information=0x%x\n", b_enable));
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], write 0x24=0x%x\n", h2c_parameter[0]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x24, 1, h2c_parameter);
-}
-
-void rtl8821ae_dm_bt_set_fw_bt_retry_index(struct ieee80211_hw *hw,
-													u8 retry_index)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[1] ={0};
-	h2c_parameter[0] = retry_index;
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], Set BT Retry Index=%d\n", retry_index));
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], write 0x23=0x%x\n", h2c_parameter[0]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x23, 1, h2c_parameter);
-}
-
-void rtl8821ae_dm_bt_set_fw_wlan_act(struct ieee80211_hw *hw,
-											u8 wlan_act_hi, u8 wlan_act_lo)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter_hi[1] ={0};
-	u8 h2c_parameter_lo[1] ={0};
-	h2c_parameter_hi[0] = wlan_act_hi;
-	h2c_parameter_lo[0] = wlan_act_lo;
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], Set WLAN_ACT Hi:Lo=0x%x/0x%x\n", wlan_act_hi, wlan_act_lo));
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], write 0x22=0x%x\n", h2c_parameter_hi[0]));
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("[BTCoex], write 0x11=0x%x\n", h2c_parameter_lo[0]));
-
-	/* WLAN_ACT = High duration, unit:ms */
-	rtl8821ae_fill_h2c_cmd(hw, 0x22, 1, h2c_parameter_hi);
-	/*  WLAN_ACT = Low duration, unit:3*625us */
-	rtl8821ae_fill_h2c_cmd(hw, 0x11, 1, h2c_parameter_lo);
-}
-
-void rtl8821ae_dm_bt_set_bt_dm(struct ieee80211_hw *hw, struct btdm_8821ae *p_btdm)
-{
-	struct rtl_pci_priv	*rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv	*rtlpriv = rtl_priv(hw);
-	struct btdm_8821ae *p_btdm_8821ae = &hal_coex_8821ae.btdm;
-	u8 i;
-
-	bool b_fw_current_inpsmode = false;
-    bool b_fw_ps_awake = true;
-
-    rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-			                      (u8 *) (&b_fw_current_inpsmode));
-    rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_FWLPS_RF_ON,
-			                      (u8 *) (&b_fw_ps_awake));
-
-	// check new setting is different with the old one,
-	// if all the same, don't do the setting again.
-	if (memcmp(p_btdm_8821ae, p_btdm, sizeof(struct btdm_8821ae)) == 0) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], the same coexist setting, return!!\n"));
-		return;
-	} else {	//save the new coexist setting
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], UPDATE TO NEW COEX SETTING!!\n"));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bAllOff=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_all_off, p_btdm->b_all_off));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new b_agc_table_en=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_agc_table_en, p_btdm->b_agc_table_en));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new b_adc_back_off_on=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_adc_back_off_on, p_btdm->b_adc_back_off_on));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new b2_ant_hid_en=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b2_ant_hid_en, p_btdm->b2_ant_hid_en));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bLowPenaltyRateAdaptive=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_low_penalty_rate_adaptive,
-			p_btdm->b_low_penalty_rate_adaptive));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bRfRxLpfShrink=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_rf_rx_lpf_shrink, p_btdm->b_rf_rx_lpf_shrink));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bRejectAggrePkt=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_reject_aggre_pkt, p_btdm->b_reject_aggre_pkt));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new b_tdma_on=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_tdma_on, p_btdm->b_tdma_on));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new tdmaAnt=0x%x/ 0x%x \n",
-			p_btdm_8821ae->tdma_ant, p_btdm->tdma_ant));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new tdmaNav=0x%x/ 0x%x \n",
-			p_btdm_8821ae->tdma_nav, p_btdm->tdma_nav));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new tdma_dac_swing=0x%x/ 0x%x \n",
-			p_btdm_8821ae->tdma_dac_swing, p_btdm->tdma_dac_swing));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new fw_dac_swing_lvl=0x%x/ 0x%x \n",
-			p_btdm_8821ae->fw_dac_swing_lvl, p_btdm->fw_dac_swing_lvl));
-
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bTraTdmaOn=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_tra_tdma_on, p_btdm->b_tra_tdma_on));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new traTdmaAnt=0x%x/ 0x%x \n",
-			p_btdm_8821ae->tra_tdma_ant, p_btdm->tra_tdma_ant));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new traTdmaNav=0x%x/ 0x%x \n",
-			p_btdm_8821ae->tra_tdma_nav, p_btdm->tra_tdma_nav));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bPsTdmaOn=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_ps_tdma_on, p_btdm->b_ps_tdma_on));
-		for(i=0; i<5; i++)
-		{
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-				("[BTCoex], original/new psTdmaByte[i]=0x%x/ 0x%x \n",
-				p_btdm_8821ae->ps_tdma_byte[i], p_btdm->ps_tdma_byte[i]));
-		}
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bIgnoreWlanAct=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_ignore_wlan_act, p_btdm->b_ignore_wlan_act));
-
-
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new bPtaOn=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_pta_on, p_btdm->b_pta_on));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new val_0x6c0=0x%x/ 0x%x \n",
-			p_btdm_8821ae->val_0x6c0, p_btdm->val_0x6c0));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new val_0x6c8=0x%x/ 0x%x \n",
-			p_btdm_8821ae->val_0x6c8, p_btdm->val_0x6c8));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new val_0x6cc=0x%x/ 0x%x \n",
-			p_btdm_8821ae->val_0x6cc, p_btdm->val_0x6cc));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new b_sw_dac_swing_on=0x%x/ 0x%x \n",
-			p_btdm_8821ae->b_sw_dac_swing_on, p_btdm->b_sw_dac_swing_on));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new sw_dac_swing_lvl=0x%x/ 0x%x \n",
-			p_btdm_8821ae->sw_dac_swing_lvl, p_btdm->sw_dac_swing_lvl));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new wlanActHi=0x%x/ 0x%x \n",
-			p_btdm_8821ae->wlan_act_hi, p_btdm->wlan_act_hi));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new wlanActLo=0x%x/ 0x%x \n",
-			p_btdm_8821ae->wlan_act_lo, p_btdm->wlan_act_lo));
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], original/new btRetryIndex=0x%x/ 0x%x \n",
-			p_btdm_8821ae->bt_retry_index, p_btdm->bt_retry_index));
-
-		memcpy(p_btdm_8821ae, p_btdm, sizeof(struct btdm_8821ae));
-	}
-	/*
-	 * Here we only consider when Bt Operation
-	 * inquiry/paging/pairing is ON
-	 * we only need to turn off TDMA */
-
-	if (rtlpcipriv->btcoexist.b_hold_for_bt_operation) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], set to ignore wlanAct for BT OP!!\n"));
-		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, true);
-		return;
-	}
-
-	if (p_btdm->b_all_off) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("[BTCoex], disable all coexist mechanism !!\n"));
-		rtl8821ae_btdm_coex_all_off(hw);
-		return;
-	}
-
-	rtl8821ae_dm_bt_reject_ap_aggregated_packet(hw, p_btdm->b_reject_aggre_pkt);
-
-	if(p_btdm->b_low_penalty_rate_adaptive)
-		rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(hw,
-			BT_TX_RATE_ADAPTIVE_LOW_PENALTY);
-	else
-		rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(hw,
-			BT_TX_RATE_ADAPTIVE_NORMAL);
-
-	if(p_btdm->b_rf_rx_lpf_shrink)
-		rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_SHRINK);
-	else
-		rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_RESUME);
-
-	if(p_btdm->b_agc_table_en)
-		rtl8821ae_dm_bt_agc_table(hw, BT_AGCTABLE_ON);
-	else
-		rtl8821ae_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
-
-	if(p_btdm->b_adc_back_off_on)
-		rtl8821ae_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_ON);
-	else
-		rtl8821ae_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_OFF);
-
-	rtl8821ae_dm_bt_set_fw_bt_retry_index(hw, p_btdm->bt_retry_index);
-
-	rtl8821ae_dm_bt_set_fw_dac_swing_level(hw, p_btdm->fw_dac_swing_lvl);
-	rtl8821ae_dm_bt_set_fw_wlan_act(hw, p_btdm->wlan_act_hi, p_btdm->wlan_act_lo);
-
-	rtl8821ae_dm_bt_set_coex_table(hw, p_btdm->val_0x6c0,
-		p_btdm->val_0x6c8, p_btdm->val_0x6cc);
-	rtl8821ae_dm_bt_set_hw_pta_mode(hw, p_btdm->b_pta_on);
-
-	/*
-	 * Note: There is a constraint between TDMA and 2AntHID
-	 * Only one of 2AntHid and tdma can be turn on
-	 * We should turn off those mechanisms should be turned off first
-	 * and then turn on those mechanisms should be turned on.
-	*/
-#if 1
-	if(p_btdm->b2_ant_hid_en) {
-		// turn off tdma
-		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on,
-							p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
-		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant,
-							p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
-
-		// turn off Pstdma
-		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
-		rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0); 	// Antenna control by PTA, 0x870 = 0x300.
-
-		// turn on 2AntHid
-		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, true);
-		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, true, true);
-	} else if(p_btdm->b_tdma_on) {
-		// turn off 2AntHid
-		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
-		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
-
-		// turn off pstdma
-		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
-		rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0); 	// Antenna control by PTA, 0x870 = 0x300.
-
-		// turn on tdma
-		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
-		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, true, p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
-	} else if(p_btdm->b_ps_tdma_on) {
-		// turn off 2AntHid
-		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
-		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
-
-		// turn off tdma
-		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
-		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
-
-		// turn on pstdma
-		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
-		rtl8821ae_dm_bt_set_fw_3a(hw,
-			p_btdm->ps_tdma_byte[0],
-			p_btdm->ps_tdma_byte[1],
-			p_btdm->ps_tdma_byte[2],
-			p_btdm->ps_tdma_byte[3],
-			p_btdm->ps_tdma_byte[4]);
-	} else {
-		// turn off 2AntHid
-		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
-		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
-
-		// turn off tdma
-		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
-		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
-
-		// turn off pstdma
-		rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, p_btdm->b_ignore_wlan_act);
-		rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0); 	// Antenna control by PTA, 0x870 = 0x300.
-	}
-#else
-	if (p_btdm->b_tdma_on) {
-		if(p_btdm->b_ps_tdma_on) {
-		} else {
-			rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
-		}
-		/* Turn off 2AntHID first then turn tdma ON */
-		rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
-		rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
-		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
-		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, true,
-			p_btdm->tdma_ant, p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
-	} else {
-		/* Turn off tdma first then turn 2AntHID ON if need */
-		rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, p_btdm->b_tra_tdma_on, p_btdm->tra_tdma_ant, p_btdm->tra_tdma_nav);
-		rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, p_btdm->tdma_ant,
-			p_btdm->tdma_nav, p_btdm->tdma_dac_swing);
-		if (p_btdm->b2_ant_hid_en) {
-			rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, true);
-			rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, true, true);
-		} else {
-			rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
-			rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
-		}
-		if(p_btdm->b_ps_tdma_on) {
-			rtl8821ae_dm_bt_set_fw_3a(hw, p_btdm->ps_tdma_byte[0], p_btdm->ps_tdma_byte[1],
-				p_btdm->ps_tdma_byte[2], p_btdm->ps_tdma_byte[3], p_btdm->ps_tdma_byte[4]);
-		} else {
-			rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
-		}
-	}
-#endif
-
-	/*
-	 * Note:
-	 * We should add delay for making sure sw DacSwing can be set successfully.
-	 * because of that rtl8821ae_dm_bt_set_fw_2_ant_hid() and rtl8821ae_dm_bt_set_fw_tdma_ctrl()
-	 * will overwrite the reg 0x880.
-	*/
-	mdelay(30);
-	rtl8821ae_dm_bt_set_sw_full_time_dac_swing(hw,
-		p_btdm->b_sw_dac_swing_on, p_btdm->sw_dac_swing_lvl);
-	rtl8821ae_dm_bt_set_fw_dec_bt_pwr(hw, p_btdm->b_dec_bt_pwr);
-}
-
-void rtl8821ae_dm_bt_bt_state_update_2_ant_hid(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], HID busy!!\n"));
-	rtlpcipriv->btcoexist.b_bt_busy = true;
-	rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT_IDLE;
-}
-
-void rtl8821ae_dm_bt_bt_state_update_2_ant_pan(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	bool b_idle = false;
-
-	if (hal_coex_8821ae.low_priority_tx >=
-		hal_coex_8821ae.low_priority_rx) {
-		if((hal_coex_8821ae.low_priority_tx/
-			hal_coex_8821ae.low_priority_rx) > 10) {
-			b_idle = true;
-		}
-	} else {
-		if((hal_coex_8821ae.low_priority_rx/
-			hal_coex_8821ae.low_priority_tx) > 10) {
-			b_idle = true;
-		}
-	}
-
-	if(!b_idle) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], PAN busy!!\n"));
-		rtlpcipriv->btcoexist.b_bt_busy = true;
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT_IDLE;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], PAN idle!!\n"));
-	}
-}
-
-void rtl8821ae_dm_bt_2_ant_sco_action(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct btdm_8821ae btdm8821ae;
-	u8 bt_rssi_state;
-
-	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
-	btdm8821ae.b_rf_rx_lpf_shrink = true;
-	btdm8821ae.b_low_penalty_rate_adaptive = true;
-	btdm8821ae.b_reject_aggre_pkt = false;
-
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
-		/*  coex table */
-		btdm8821ae.val_0x6c0 = 0x5a5aaaaa;
-		btdm8821ae.val_0x6c8 = 0xcc;
-		btdm8821ae.val_0x6cc = 0x3;
-		/* sw mechanism */
-		btdm8821ae.b_agc_table_en = false;
-		btdm8821ae.b_adc_back_off_on = true;
-		btdm8821ae.b_sw_dac_swing_on = false;
-		/* fw mechanism */
-		btdm8821ae.b_tdma_on = false;
-		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
-		bt_rssi_state
-			= rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, BT_FW_COEX_THRESH_47, 0);
-
-		/* coex table */
-		btdm8821ae.val_0x6c0 = 0x5a5aaaaa;
-		btdm8821ae.val_0x6c8 = 0xcc;
-		btdm8821ae.val_0x6cc = 0x3;
-		/* sw mechanism */
-		if ((bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
-			btdm8821ae.b_agc_table_en = true;
-			btdm8821ae.b_adc_back_off_on = true;
-			btdm8821ae.b_sw_dac_swing_on = false;
-		} else {
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = false;
-			btdm8821ae.b_sw_dac_swing_on = false;
-		}
-		/* fw mechanism */
-		btdm8821ae.b_tdma_on = false;
-		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
-	}
-
-	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
-		btdm8821ae.b_dec_bt_pwr = true;
-	}
-
-	if(rtl8821ae_dm_bt_is_coexist_state_changed(hw))
-		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
-}
-
-void rtl8821ae_dm_bt_2_ant_hid_action(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct btdm_8821ae btdm8821ae;
-	u8 bt_rssi_state;
-
-	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
-
-	btdm8821ae.b_rf_rx_lpf_shrink = true;
-	btdm8821ae.b_low_penalty_rate_adaptive = true;
-	btdm8821ae.b_reject_aggre_pkt = false;
-
-	// coex table
-	btdm8821ae.val_0x6c0 = 0x55555555;
-	btdm8821ae.val_0x6c8 = 0xffff;
-	btdm8821ae.val_0x6cc = 0x3;
-	btdm8821ae.b_ignore_wlan_act = true;
-
-	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
-		// sw mechanism
-		btdm8821ae.b_agc_table_en = false;
-		btdm8821ae.b_adc_back_off_on = false;
-		btdm8821ae.b_sw_dac_swing_on = false;
-
-		// fw mechanism
-		btdm8821ae.b_ps_tdma_on = true;
-		btdm8821ae.ps_tdma_byte[0] = 0xa3;
-		btdm8821ae.ps_tdma_byte[1] = 0xf;
-		btdm8821ae.ps_tdma_byte[2] = 0xf;
-		btdm8821ae.ps_tdma_byte[3] = 0x0;
-		btdm8821ae.ps_tdma_byte[4] = 0x80;
-
-		btdm8821ae.b_tra_tdma_on = false;
-		btdm8821ae.b_tdma_on = false;
-		btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
-		btdm8821ae.b2_ant_hid_en = false;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
-		bt_rssi_state =
-			rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
-
-		if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
-			// sw mechanism
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = false;
-			btdm8821ae.b_sw_dac_swing_on = true;
-			btdm8821ae.sw_dac_swing_lvl = 0x20;
-
-			// fw mechanism
-			btdm8821ae.b_ps_tdma_on = false;
-			btdm8821ae.b_tdma_on = false;
-			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
-			btdm8821ae.b2_ant_hid_en = false;
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
-			// sw mechanism
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = false;
-			btdm8821ae.b_sw_dac_swing_on = false;
-
-			// fw mechanism
-			btdm8821ae.b_ps_tdma_on = false;
-			btdm8821ae.b_tdma_on = false;
-			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_OFF;
-			btdm8821ae.b2_ant_hid_en = true;
-			btdm8821ae.fw_dac_swing_lvl = 0x20;
-		}
-	}
-
-	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
-		btdm8821ae.b_dec_bt_pwr = true;
-	}
-
-	if (rtl8821ae_dm_bt_is_coexist_state_changed(hw)) {
-		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
-	}
-}
-
-
-void rtl8821ae_dm_bt_2_ant_2_dp_action_no_profile(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct btdm_8821ae btdm8821ae;
-	u8 bt_rssi_state;
-
-	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
-
-	btdm8821ae.b_rf_rx_lpf_shrink = true;
-	btdm8821ae.b_low_penalty_rate_adaptive = true;
-	btdm8821ae.b_reject_aggre_pkt = false;
-
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("HT40\n"));
-		if (rtl8821ae_dm_bt_is_wifi_up_link(hw)) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Uplink\n"));
-			/* coex table */
-			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
-			btdm8821ae.val_0x6c8 = 0xcccc;
-			btdm8821ae.val_0x6cc = 0x3;
-			// sw mechanism
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = true;
-			btdm8821ae.b_sw_dac_swing_on = false;
-			// fw mechanism
-			btdm8821ae.b_tra_tdma_on = true;
-			btdm8821ae.b_tdma_on = true;
-			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
-			btdm8821ae.b2_ant_hid_en = false;
-			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
-			//if(btSpec >= BT_SPEC_2_1_EDR)
-			{
-				btdm8821ae.wlan_act_hi = 0x10;
-				btdm8821ae.wlan_act_lo = 0x10;
-			}
-			//else
-			//{
-				//btdm8821ae.wlanActHi = 0x20;
-				//btdm8821ae.wlanActLo = 0x20;
-			//}
-			btdm8821ae.bt_retry_index = 2;
-			btdm8821ae.fw_dac_swing_lvl = 0x18;
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Downlink\n"));
-			// coex table
-			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
-			btdm8821ae.val_0x6c8 = 0xcc;
-			btdm8821ae.val_0x6cc = 0x3;
-			// sw mechanism
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = true;
-			btdm8821ae.b_sw_dac_swing_on = false;
-			// fw mechanism
-			btdm8821ae.b_tra_tdma_on = true;
-			btdm8821ae.b_tdma_on = true;
-			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
-			btdm8821ae.b2_ant_hid_en = false;
-			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
-			//if(btSpec >= BT_SPEC_2_1_EDR)
-			{
-				btdm8821ae.wlan_act_hi = 0x10;
-				btdm8821ae.wlan_act_lo = 0x10;
-			}
-			//else
-			//{
-			//	btdm8821ae.wlanActHi = 0x20;
-			//	btdm8821ae.wlanActLo = 0x20;
-			//}
-			btdm8821ae.bt_retry_index = 2;
-			btdm8821ae.fw_dac_swing_lvl = 0x40;
-		}
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("HT20 or Legacy\n"));
-		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, BT_FW_COEX_THRESH_47, 0);
-
-		if(rtl8821ae_dm_bt_is_wifi_up_link(hw))
-		{
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Uplink\n"));
-			// coex table
-			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
-			btdm8821ae.val_0x6c8 = 0xcccc;
-			btdm8821ae.val_0x6cc = 0x3;
-			// sw mechanism
-			if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-				(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) )
-			{
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi high \n"));
-				btdm8821ae.b_agc_table_en = true;
-				btdm8821ae.b_adc_back_off_on = true;
-				btdm8821ae.b_sw_dac_swing_on = false;
-			} else {
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi low \n"));
-				btdm8821ae.b_agc_table_en = false;
-				btdm8821ae.b_adc_back_off_on = false;
-				btdm8821ae.b_sw_dac_swing_on = false;
-			}
-			// fw mechanism
-			btdm8821ae.b_tra_tdma_on = true;
-			btdm8821ae.b_tdma_on = true;
-			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
-			btdm8821ae.b2_ant_hid_en = false;
-			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
-			//if(btSpec >= BT_SPEC_2_1_EDR)
-			{
-				btdm8821ae.wlan_act_hi = 0x10;
-				btdm8821ae.wlan_act_lo = 0x10;
-			}
-			//else
-			//{
-				//btdm8821ae.wlanActHi = 0x20;
-				//btdm8821ae.wlanActLo = 0x20;
-			//}
-			btdm8821ae.bt_retry_index = 2;
-			btdm8821ae.fw_dac_swing_lvl = 0x18;
-		}
-		else
-		{
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi Downlink\n"));
-			// coex table
-			btdm8821ae.val_0x6c0 = 0x5a5a5a5a;
-			btdm8821ae.val_0x6c8 = 0xcc;
-			btdm8821ae.val_0x6cc = 0x3;
-			// sw mechanism
-			if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-				(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) )
-			{
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi high \n"));
-				btdm8821ae.b_agc_table_en = true;
-				btdm8821ae.b_adc_back_off_on = true;
-				btdm8821ae.b_sw_dac_swing_on = false;
-			}
-			else
-			{
-				RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("Wifi rssi low \n"));
-				btdm8821ae.b_agc_table_en = false;
-				btdm8821ae.b_adc_back_off_on = false;
-				btdm8821ae.b_sw_dac_swing_on = false;
-			}
-			// fw mechanism
-			btdm8821ae.b_tra_tdma_on = true;
-			btdm8821ae.b_tdma_on = true;
-			btdm8821ae.tdma_dac_swing = TDMA_DAC_SWING_ON;
-			btdm8821ae.b2_ant_hid_en = false;
-			//btSpec = BTHCI_GetBTCoreSpecByProf(Adapter, BT_PROFILE_A2DP);
-			//if(btSpec >= BT_SPEC_2_1_EDR)
-			{
-				btdm8821ae.wlan_act_hi = 0x10;
-				btdm8821ae.wlan_act_lo = 0x10;
-			}
-			//else
-			//{
-				//btdm8821ae.wlanActHi = 0x20;
-				//btdm8821ae.wlanActLo = 0x20;
-			//}
-			btdm8821ae.bt_retry_index = 2;
-			btdm8821ae.fw_dac_swing_lvl = 0x40;
-		}
-	}
-
-	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
-		btdm8821ae.b_dec_bt_pwr = true;
-	}
-
-	if (rtl8821ae_dm_bt_is_coexist_state_changed(hw)) {
-		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
-	}
-}
-
-
-//============================================================
-// extern function start with BTDM_
-//============================================================
-u32 rtl8821ae_dm_bt_tx_rx_couter_h(struct ieee80211_hw *hw)
-{
-	u32	counters=0;
-
-	counters = hal_coex_8821ae.high_priority_tx + hal_coex_8821ae.high_priority_rx ;
-	return counters;
-}
-
-u32 rtl8821ae_dm_bt_tx_rx_couter_l(struct ieee80211_hw *hw)
-{
-	u32 counters=0;
-
-	counters = hal_coex_8821ae.low_priority_tx + hal_coex_8821ae.low_priority_rx ;
-	return counters;
-}
-
-u8 rtl8821ae_dm_bt_bt_tx_rx_counter_level(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u32	bt_tx_rx_cnt = 0;
-	u8	bt_tx_rx_cnt_lvl = 0;
-
-	bt_tx_rx_cnt = rtl8821ae_dm_bt_tx_rx_couter_h(hw)
-				+ rtl8821ae_dm_bt_tx_rx_couter_l(hw);
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-		("[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt));
-
-	rtlpcipriv->btcoexist.current_state_h &= ~\
-		 (BT_COEX_STATE_BT_CNT_LEVEL_0 | BT_COEX_STATE_BT_CNT_LEVEL_1|
-		  BT_COEX_STATE_BT_CNT_LEVEL_2);
-
-	if (bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_3) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], BT TxRx Counters at level 3\n"));
-		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_3;
-		rtlpcipriv->btcoexist.current_state_h |= BT_COEX_STATE_BT_CNT_LEVEL_3;
-	} else if(bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_2) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], BT TxRx Counters at level 2\n"));
-		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_2;
-		rtlpcipriv->btcoexist.current_state_h |= BT_COEX_STATE_BT_CNT_LEVEL_2;
-	} else if(bt_tx_rx_cnt >= BT_TXRX_CNT_THRES_1) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], BT TxRx Counters at level 1\n"));
-		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_1;
-		rtlpcipriv->btcoexist.current_state_h  |= BT_COEX_STATE_BT_CNT_LEVEL_1;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], BT TxRx Counters at level 0\n"));
-		bt_tx_rx_cnt_lvl = BT_TXRX_CNT_LEVEL_0;
-		rtlpcipriv->btcoexist.current_state_h |= BT_COEX_STATE_BT_CNT_LEVEL_0;
-	}
-	return bt_tx_rx_cnt_lvl;
-}
-
-
-void rtl8821ae_dm_bt_2_ant_hid_sco_esco(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct btdm_8821ae btdm8821ae;
-
-	u8 bt_rssi_state, bt_rssi_state1;
-	u8	bt_tx_rx_cnt_lvl = 0;
-
-	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
-
-
-	btdm8821ae.b_rf_rx_lpf_shrink = true;
-	btdm8821ae.b_low_penalty_rate_adaptive = true;
-	btdm8821ae.b_reject_aggre_pkt = false;
-
-	bt_tx_rx_cnt_lvl = rtl8821ae_dm_bt_bt_tx_rx_counter_level(hw);
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt_lvl));
-
-	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
-	{
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
-		// coex table
-		btdm8821ae.val_0x6c0 = 0x55555555;
-		btdm8821ae.val_0x6c8 = 0xffff;
-		btdm8821ae.val_0x6cc = 0x3;
-
-		// sw mechanism
-		btdm8821ae.b_agc_table_en = false;
-		btdm8821ae.b_adc_back_off_on = false;
-		btdm8821ae.b_sw_dac_swing_on = false;
-
-		// fw mechanism
-		btdm8821ae.b_ps_tdma_on = true;
-		if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
-			btdm8821ae.ps_tdma_byte[0] = 0xa3;
-			btdm8821ae.ps_tdma_byte[1] = 0x5;
-			btdm8821ae.ps_tdma_byte[2] = 0x5;
-			btdm8821ae.ps_tdma_byte[3] = 0x2;
-			btdm8821ae.ps_tdma_byte[4] = 0x80;
-		} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
-			btdm8821ae.ps_tdma_byte[0] = 0xa3;
-			btdm8821ae.ps_tdma_byte[1] = 0xa;
-			btdm8821ae.ps_tdma_byte[2] = 0xa;
-			btdm8821ae.ps_tdma_byte[3] = 0x2;
-			btdm8821ae.ps_tdma_byte[4] = 0x80;
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
-			btdm8821ae.ps_tdma_byte[0] = 0xa3;
-			btdm8821ae.ps_tdma_byte[1] = 0xf;
-			btdm8821ae.ps_tdma_byte[2] = 0xf;
-			btdm8821ae.ps_tdma_byte[3] = 0x2;
-			btdm8821ae.ps_tdma_byte[4] = 0x80;
-		}
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
-		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
-		bt_rssi_state1 = rtl8821ae_dm_bt_check_coex_rssi_state1(hw, 2, 27, 0);
-
-		// coex table
-		btdm8821ae.val_0x6c0 = 0x55555555;
-		btdm8821ae.val_0x6c8 = 0xffff;
-		btdm8821ae.val_0x6cc = 0x3;
-
-		// sw mechanism
-		if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
-			btdm8821ae.b_agc_table_en = true;
-			btdm8821ae.b_adc_back_off_on = true;
-			btdm8821ae.b_sw_dac_swing_on = false;
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = false;
-			btdm8821ae.b_sw_dac_swing_on = false;
-		}
-
-		// fw mechanism
-		btdm8821ae.b_ps_tdma_on = true;
-		if( (bt_rssi_state1 == BT_RSSI_STATE_HIGH) ||
-			(bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH) ) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,("Wifi rssi-1 high \n"));
-			// only rssi high we need to do this,
-			// when rssi low, the value will modified by fw
-			rtl_write_byte(rtlpriv, 0x883, 0x40);
-			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0x5;
-				btdm8821ae.ps_tdma_byte[2] = 0x5;
-				btdm8821ae.ps_tdma_byte[3] = 0x83;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xa;
-				btdm8821ae.ps_tdma_byte[2] = 0xa;
-				btdm8821ae.ps_tdma_byte[3] = 0x83;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xf;
-				btdm8821ae.ps_tdma_byte[2] = 0xf;
-				btdm8821ae.ps_tdma_byte[3] = 0x83;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			}
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi-1 low \n"));
-			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2)
-			{
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0x5;
-				btdm8821ae.ps_tdma_byte[2] = 0x5;
-				btdm8821ae.ps_tdma_byte[3] = 0x2;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xa;
-				btdm8821ae.ps_tdma_byte[2] = 0xa;
-				btdm8821ae.ps_tdma_byte[3] = 0x2;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xf;
-				btdm8821ae.ps_tdma_byte[2] = 0xf;
-				btdm8821ae.ps_tdma_byte[3] = 0x2;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			}
-		}
-	}
-
-	if (rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
-		btdm8821ae.b_dec_bt_pwr = true;
-	}
-
-	// Always ignore WlanAct if bHid|bSCOBusy|bSCOeSCO
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-		("[BTCoex], BT btInqPageStartTime = 0x%x, btTxRxCntLvl = %d\n",
-		hal_coex_8821ae.bt_inq_page_start_time, bt_tx_rx_cnt_lvl));
-	if( (hal_coex_8821ae.bt_inq_page_start_time) ||
-		(BT_TXRX_CNT_LEVEL_3 == bt_tx_rx_cnt_lvl) ) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], Set BT inquiry / page scan 0x3a setting\n"));
-		btdm8821ae.b_ps_tdma_on = true;
-		btdm8821ae.ps_tdma_byte[0] = 0xa3;
-		btdm8821ae.ps_tdma_byte[1] = 0x5;
-		btdm8821ae.ps_tdma_byte[2] = 0x5;
-		btdm8821ae.ps_tdma_byte[3] = 0x2;
-		btdm8821ae.ps_tdma_byte[4] = 0x80;
-	}
-
-	if(rtl8821ae_dm_bt_is_coexist_state_changed(hw)) {
-		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
-	}
-}
-
-void rtl8821ae_dm_bt_2_ant_ftp_a2dp(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct btdm_8821ae btdm8821ae;
-
-	u8 bt_rssi_state, bt_rssi_state1;
-	u32 bt_tx_rx_cnt_lvl = 0;
-
-	rtl8821ae_dm_bt_btdm_structure_reload(hw, &btdm8821ae);
-
-	btdm8821ae.b_rf_rx_lpf_shrink = true;
-	btdm8821ae.b_low_penalty_rate_adaptive = true;
-	btdm8821ae.b_reject_aggre_pkt = false;
-
-	bt_tx_rx_cnt_lvl = rtl8821ae_dm_bt_bt_tx_rx_counter_level(hw);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters = %d\n", bt_tx_rx_cnt_lvl));
-
-	if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40)
-	{
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT40\n"));
-		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 37, 0);
-
-		// coex table
-		btdm8821ae.val_0x6c0 = 0x55555555;
-		btdm8821ae.val_0x6c8 = 0xffff;
-		btdm8821ae.val_0x6cc = 0x3;
-
-		// sw mechanism
-		btdm8821ae.b_agc_table_en = false;
-		btdm8821ae.b_adc_back_off_on = true;
-		btdm8821ae.b_sw_dac_swing_on = false;
-
-		// fw mechanism
-		btdm8821ae.b_ps_tdma_on = true;
-		if ((bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
-			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0x5;
-				btdm8821ae.ps_tdma_byte[2] = 0x5;
-				btdm8821ae.ps_tdma_byte[3] = 0x81;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xa;
-				btdm8821ae.ps_tdma_byte[2] = 0xa;
-				btdm8821ae.ps_tdma_byte[3] = 0x81;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xf;
-				btdm8821ae.ps_tdma_byte[2] = 0xf;
-				btdm8821ae.ps_tdma_byte[3] = 0x81;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			}
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
-			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0x5;
-				btdm8821ae.ps_tdma_byte[2] = 0x5;
-				btdm8821ae.ps_tdma_byte[3] = 0x0;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xa;
-				btdm8821ae.ps_tdma_byte[2] = 0xa;
-				btdm8821ae.ps_tdma_byte[3] = 0x0;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xf;
-				btdm8821ae.ps_tdma_byte[2] = 0xf;
-				btdm8821ae.ps_tdma_byte[3] = 0x0;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			}
-		}
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("HT20 or Legacy\n"));
-		bt_rssi_state = rtl8821ae_dm_bt_check_coex_rssi_state(hw, 2, 47, 0);
-		bt_rssi_state1 = rtl8821ae_dm_bt_check_coex_rssi_state1(hw, 2, 27, 0);
-
-		// coex table
-		btdm8821ae.val_0x6c0 = 0x55555555;
-		btdm8821ae.val_0x6c8 = 0xffff;
-		btdm8821ae.val_0x6cc = 0x3;
-
-		// sw mechanism
-		if( (bt_rssi_state == BT_RSSI_STATE_HIGH) ||
-			(bt_rssi_state == BT_RSSI_STATE_STAY_HIGH) ) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi high \n"));
-			btdm8821ae.b_agc_table_en = true;
-			btdm8821ae.b_adc_back_off_on = true;
-			btdm8821ae.b_sw_dac_swing_on = false;
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi low \n"));
-			btdm8821ae.b_agc_table_en = false;
-			btdm8821ae.b_adc_back_off_on = false;
-			btdm8821ae.b_sw_dac_swing_on = false;
-		}
-
-		// fw mechanism
-		btdm8821ae.b_ps_tdma_on = true;
-		if( (bt_rssi_state1 == BT_RSSI_STATE_HIGH) ||
-			(bt_rssi_state1 == BT_RSSI_STATE_STAY_HIGH) ) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi-1 high \n"));
-			// only rssi high we need to do this,
-			// when rssi low, the value will modified by fw
-			rtl_write_byte(rtlpriv, 0x883, 0x40);
-			if (bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0x5;
-				btdm8821ae.ps_tdma_byte[2] = 0x5;
-				btdm8821ae.ps_tdma_byte[3] = 0x81;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xa;
-				btdm8821ae.ps_tdma_byte[2] = 0xa;
-				btdm8821ae.ps_tdma_byte[3] = 0x81;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xf;
-				btdm8821ae.ps_tdma_byte[2] = 0xf;
-				btdm8821ae.ps_tdma_byte[3] = 0x81;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			}
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Wifi rssi-1 low \n"));
-			if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_2) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0x5;
-				btdm8821ae.ps_tdma_byte[2] = 0x5;
-				btdm8821ae.ps_tdma_byte[3] = 0x0;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else if(bt_tx_rx_cnt_lvl == BT_TXRX_CNT_LEVEL_1) {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters >= 1200 && < 1400\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xa;
-				btdm8821ae.ps_tdma_byte[2] = 0xa;
-				btdm8821ae.ps_tdma_byte[3] = 0x0;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			} else {
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BT TxRx Counters < 1200\n"));
-				btdm8821ae.ps_tdma_byte[0] = 0xa3;
-				btdm8821ae.ps_tdma_byte[1] = 0xf;
-				btdm8821ae.ps_tdma_byte[2] = 0xf;
-				btdm8821ae.ps_tdma_byte[3] = 0x0;
-				btdm8821ae.ps_tdma_byte[4] = 0x80;
-			}
-		}
-	}
-
-	if(rtl8821ae_dm_bt_need_to_dec_bt_pwr(hw)) {
-		btdm8821ae.b_dec_bt_pwr = true;
-	}
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-	       ("[BTCoex], BT btInqPageStartTime = 0x%x, btTxRxCntLvl = %d\n",
-	        hal_coex_8821ae.bt_inq_page_start_time, bt_tx_rx_cnt_lvl));
-
-	if( (hal_coex_8821ae.bt_inq_page_start_time) ||
-		(BT_TXRX_CNT_LEVEL_3 == bt_tx_rx_cnt_lvl) )
-	{
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-				("[BTCoex], Set BT inquiry / page scan 0x3a setting\n"));
-		btdm8821ae.b_ps_tdma_on = true;
-		btdm8821ae.ps_tdma_byte[0] = 0xa3;
-		btdm8821ae.ps_tdma_byte[1] = 0x5;
-		btdm8821ae.ps_tdma_byte[2] = 0x5;
-		btdm8821ae.ps_tdma_byte[3] = 0x83;
-		btdm8821ae.ps_tdma_byte[4] = 0x80;
-	}
-
-	if(rtl8821ae_dm_bt_is_coexist_state_changed(hw)){
-		rtl8821ae_dm_bt_set_bt_dm(hw, &btdm8821ae);
-	}
-}
-
-void rtl8821ae_dm_bt_inq_page_monitor(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 cur_time;
-	cur_time = jiffies;
-	if (hal_coex_8821ae.b_c2h_bt_inquiry_page) {
-		//pHalData->btcoexist.halCoex8821ae.btInquiryPageCnt++;
-		// bt inquiry or page is started.
-		if(hal_coex_8821ae.bt_inq_page_start_time == 0){
-			rtlpcipriv->btcoexist.current_state  |= BT_COEX_STATE_BT_INQ_PAGE;
-			hal_coex_8821ae.bt_inq_page_start_time = cur_time;
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-				("[BTCoex], BT Inquiry/page is started at time : 0x%x \n",
-				hal_coex_8821ae.bt_inq_page_start_time));
-		}
-	}
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-		("[BTCoex], BT Inquiry/page started time : 0x%x, cur_time : 0x%x \n",
-		hal_coex_8821ae.bt_inq_page_start_time, cur_time));
-
-	if (hal_coex_8821ae.bt_inq_page_start_time) {
-		if ((((long)cur_time - (long)hal_coex_8821ae.bt_inq_page_start_time) / HZ) >= 10) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,  ("[BTCoex], BT Inquiry/page >= 10sec!!!"));
-			hal_coex_8821ae.bt_inq_page_start_time = 0;
-			rtlpcipriv->btcoexist.current_state &=~ BT_COEX_STATE_BT_INQ_PAGE;
-		}
-	}
-}
-
-void rtl8821ae_dm_bt_reset_action_profile_state(struct ieee80211_hw *hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-
-	rtlpcipriv->btcoexist.current_state &= ~\
-		(BT_COEX_STATE_PROFILE_HID | BT_COEX_STATE_PROFILE_A2DP|
-		BT_COEX_STATE_PROFILE_PAN | BT_COEX_STATE_PROFILE_SCO);
-
-	rtlpcipriv->btcoexist.current_state &= ~\
-		(BT_COEX_STATE_BTINFO_COMMON | BT_COEX_STATE_BTINFO_B_HID_SCOESCO|
-		BT_COEX_STATE_BTINFO_B_FTP_A2DP);
-}
-
-void _rtl8821ae_dm_bt_coexist_2_ant(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u8 bt_retry_cnt;
-	u8 bt_info_original;
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex] Get bt info by fw!!\n"));
-
-	_rtl8821ae_dm_bt_check_wifi_state(hw);
-
-	if (hal_coex_8821ae.b_c2h_bt_info_req_sent) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-				("[BTCoex] c2h for bt_info not rcvd yet!!\n"));
-	}
-
-	bt_retry_cnt = hal_coex_8821ae.bt_retry_cnt;
-	bt_info_original = hal_coex_8821ae.c2h_bt_info_original;
-
-	// when bt inquiry or page scan, we have to set h2c 0x25
-	// ignore wlanact for continuous 4x2secs
-	rtl8821ae_dm_bt_inq_page_monitor(hw);
-	rtl8821ae_dm_bt_reset_action_profile_state(hw);
-
-	if(rtl8821ae_dm_bt_is_2_ant_common_action(hw)) {
-		rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_COMMON;
-		rtlpcipriv->btcoexist.bt_profile_action= BT_COEX_MECH_COMMON;
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("Action 2-Ant common.\n"));
-	} else {
-		if( (bt_info_original & BTINFO_B_HID) ||
-			(bt_info_original & BTINFO_B_SCO_BUSY) ||
-			(bt_info_original & BTINFO_B_SCO_ESCO) ) {
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_B_HID_SCOESCO;
-				rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_HID_SCO_ESCO;
-				rtlpcipriv->btcoexist.bt_profile_action = BT_COEX_MECH_HID_SCO_ESCO;
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BTInfo: bHid|bSCOBusy|bSCOeSCO\n"));
-				rtl8821ae_dm_bt_2_ant_hid_sco_esco(hw);
-		} else if( (bt_info_original & BTINFO_B_FTP) ||
-				(bt_info_original & BTINFO_B_A2DP) ) {
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_B_FTP_A2DP;
-				rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_FTP_A2DP;
-				rtlpcipriv->btcoexist.bt_profile_action = BT_COEX_MECH_FTP_A2DP;
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("BTInfo: bFTP|bA2DP\n"));
-				rtl8821ae_dm_bt_2_ant_ftp_a2dp(hw);
-		} else {
-				rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BTINFO_B_HID_SCOESCO;
-				rtlpcipriv->btcoexist.bt_profile_case = BT_COEX_MECH_NONE;
-				rtlpcipriv->btcoexist.bt_profile_action= BT_COEX_MECH_NONE;
-				RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], BTInfo: undefined case!!!!\n"));
-				rtl8821ae_dm_bt_2_ant_hid_sco_esco(hw);
-		}
-	}
-}
-
-void _rtl8821ae_dm_bt_coexist_1_ant(struct ieee80211_hw *hw)
-{
-	return;
-}
-
-void rtl8821ae_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw)
-{
-	rtl8821ae_dm_bt_set_coex_table(hw, 0x5a5aaaaa, 0xcc, 0x3);
-	rtl8821ae_dm_bt_set_hw_pta_mode(hw, true);
-}
-
-void rtl8821ae_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw)
-{
-	rtl8821ae_dm_bt_set_fw_ignore_wlan_act(hw, false);
-	rtl8821ae_dm_bt_set_fw_3a(hw, 0x0, 0x0, 0x0, 0x8, 0x0);
-	rtl8821ae_dm_bt_set_fw_2_ant_hid(hw, false, false);
-	rtl8821ae_dm_bt_set_fw_tra_tdma_ctrl(hw, false, TDMA_2ANT, TDMA_NAV_OFF);
-	rtl8821ae_dm_bt_set_fw_tdma_ctrl(hw, false, TDMA_2ANT,
-				TDMA_NAV_OFF, TDMA_DAC_SWING_OFF);
-	rtl8821ae_dm_bt_set_fw_dac_swing_level(hw, 0);
-	rtl8821ae_dm_bt_set_fw_bt_hid_info(hw, false);
-	rtl8821ae_dm_bt_set_fw_bt_retry_index(hw, 2);
-	rtl8821ae_dm_bt_set_fw_wlan_act(hw, 0x10, 0x10);
-	rtl8821ae_dm_bt_set_fw_dec_bt_pwr(hw, false);
-}
-
-void rtl8821ae_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw)
-{
-	rtl8821ae_dm_bt_agc_table(hw, BT_AGCTABLE_OFF);
-	rtl8821ae_dm_bt_bb_back_off_level(hw, BT_BB_BACKOFF_OFF);
-	rtl8821ae_dm_bt_reject_ap_aggregated_packet(hw, false);
-
-	rtl8821ae_dm_bt_set_sw_penalty_tx_rate_adaptive(hw,
-							BT_TX_RATE_ADAPTIVE_NORMAL);
-	rtl8821ae_dm_bt_set_sw_rf_rx_lpf_corner(hw, BT_RF_RX_LPF_CORNER_RESUME);
-	rtl8821ae_dm_bt_set_sw_full_time_dac_swing(hw, false, 0xc0);
-}
-
-void rtl8821ae_dm_bt_query_bt_information(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 h2c_parameter[1] = {0};
-
-	hal_coex_8821ae.b_c2h_bt_info_req_sent = true;
-
-	h2c_parameter[0] |=  BIT(0);
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-		("Query Bt information, write 0x38=0x%x\n", h2c_parameter[0]));
-
-	rtl8821ae_fill_h2c_cmd(hw, 0x38, 1, h2c_parameter);
-}
-
-void rtl8821ae_dm_bt_bt_hw_counters_monitor(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u32 reg_hp_tx_rx, reg_lp_tx_rx, u32_tmp;
-	u32 reg_hp_tx=0, reg_hp_rx=0, reg_lp_tx=0, reg_lp_rx=0;
-
-	reg_hp_tx_rx = REG_HIGH_PRIORITY_TXRX;
-	reg_lp_tx_rx = REG_LOW_PRIORITY_TXRX;
-
-	u32_tmp = rtl_read_dword(rtlpriv, reg_hp_tx_rx);
-	reg_hp_tx = u32_tmp & MASKLWORD;
-	reg_hp_rx = (u32_tmp & MASKHWORD)>>16;
-
-	u32_tmp = rtl_read_dword(rtlpriv, reg_lp_tx_rx);
-	reg_lp_tx = u32_tmp & MASKLWORD;
-	reg_lp_rx = (u32_tmp & MASKHWORD)>>16;
-
-	if(rtlpcipriv->btcoexist.lps_counter > 1) {
-		reg_hp_tx %= rtlpcipriv->btcoexist.lps_counter;
-		reg_hp_rx %= rtlpcipriv->btcoexist.lps_counter;
-		reg_lp_tx %= rtlpcipriv->btcoexist.lps_counter;
-		reg_lp_rx %= rtlpcipriv->btcoexist.lps_counter;
-	}
-
-	hal_coex_8821ae.high_priority_tx = reg_hp_tx;
-	hal_coex_8821ae.high_priority_rx = reg_hp_rx;
-	hal_coex_8821ae.low_priority_tx = reg_lp_tx;
-	hal_coex_8821ae.low_priority_rx = reg_lp_rx;
-
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-		("High Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
-		reg_hp_tx_rx, reg_hp_tx, reg_hp_tx, reg_hp_rx, reg_hp_rx));
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-		("Low Priority Tx/Rx (reg 0x%x)=%x(%d)/%x(%d)\n",
-		reg_lp_tx_rx, reg_lp_tx, reg_lp_tx, reg_lp_rx, reg_lp_rx));
-	rtlpcipriv->btcoexist.lps_counter = 0;
-	//rtl_write_byte(rtlpriv, 0x76e, 0xc);
-}
-
-void rtl8821ae_dm_bt_bt_enable_disable_check(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	bool bt_alife = true;
-
-	if (hal_coex_8821ae.high_priority_tx == 0 &&
-		hal_coex_8821ae.high_priority_rx == 0 &&
-		hal_coex_8821ae.low_priority_tx == 0 &&
-		hal_coex_8821ae.low_priority_rx == 0) {
-		bt_alife = false;
-	}
-	if (hal_coex_8821ae.high_priority_tx == 0xeaea &&
-		hal_coex_8821ae.high_priority_rx == 0xeaea &&
-		hal_coex_8821ae.low_priority_tx == 0xeaea &&
-		hal_coex_8821ae.low_priority_rx == 0xeaea) {
-		bt_alife = false;
-	}
-	if (hal_coex_8821ae.high_priority_tx == 0xffff &&
-		hal_coex_8821ae.high_priority_rx == 0xffff &&
-		hal_coex_8821ae.low_priority_tx == 0xffff &&
-		hal_coex_8821ae.low_priority_rx == 0xffff) {
-		bt_alife = false;
-	}
-	if (bt_alife) {
-		rtlpcipriv->btcoexist.bt_active_zero_cnt = 0;
-		rtlpcipriv->btcoexist.b_cur_bt_disabled = false;
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("8821AE BT is enabled !!\n"));
-	} else {
-		rtlpcipriv->btcoexist.bt_active_zero_cnt++;
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE,
-			("8821AE bt all counters=0, %d times!!\n",
-			rtlpcipriv->btcoexist.bt_active_zero_cnt));
-		if (rtlpcipriv->btcoexist.bt_active_zero_cnt >= 2) {
-			rtlpcipriv->btcoexist.b_cur_bt_disabled = true;
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("8821AE BT is disabled !!\n"));
-		}
-	}
-	if (rtlpcipriv->btcoexist.b_pre_bt_disabled !=
-		rtlpcipriv->btcoexist.b_cur_bt_disabled) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("8821AE BT is from %s to %s!!\n",
-			(rtlpcipriv->btcoexist.b_pre_bt_disabled ? "disabled":"enabled"),
-			(rtlpcipriv->btcoexist.b_cur_bt_disabled ? "disabled":"enabled")));
-		rtlpcipriv->btcoexist.b_pre_bt_disabled
-			= rtlpcipriv->btcoexist.b_cur_bt_disabled;
-	}
-}
-
-
-void rtl8821ae_dm_bt_coexist(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-
-	rtl8821ae_dm_bt_query_bt_information(hw);
-	rtl8821ae_dm_bt_bt_hw_counters_monitor(hw);
-	rtl8821ae_dm_bt_bt_enable_disable_check(hw);
-
-	if (rtlpcipriv->btcoexist.bt_ant_num == ANT_X2) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTCoex], 2 Ant mechanism\n"));
-		_rtl8821ae_dm_bt_coexist_2_ant(hw);
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("[BTCoex], 1 Ant mechanism\n"));
-		_rtl8821ae_dm_bt_coexist_1_ant(hw);
-	}
-
-	if (!rtl8821ae_dm_bt_is_same_coexist_state(hw)) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-			("[BTCoex], Coexist State[bitMap] change from 0x%x%8x to 0x%x%8x\n",
-			rtlpcipriv->btcoexist.previous_state_h,
-			rtlpcipriv->btcoexist.previous_state,
-			rtlpcipriv->btcoexist.current_state_h,
-			rtlpcipriv->btcoexist.current_state));
-		rtlpcipriv->btcoexist.previous_state
-			= rtlpcipriv->btcoexist.current_state;
-		rtlpcipriv->btcoexist.previous_state_h
-			= rtlpcipriv->btcoexist.current_state_h;
-	}
-}
-
-void rtl8821ae_dm_bt_parse_bt_info(struct ieee80211_hw *hw, u8 * tmp_buf, u8 len)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	u8 bt_info;
-	u8 i;
-
-	hal_coex_8821ae.b_c2h_bt_info_req_sent = false;
-	hal_coex_8821ae.bt_retry_cnt = 0;
-	for (i = 0; i < len; i++) {
-		if (i == 0) {
-			hal_coex_8821ae.c2h_bt_info_original = tmp_buf[i];
-		} else if (i == 1) {
-			hal_coex_8821ae.bt_retry_cnt = tmp_buf[i];
-		}
-		if(i == len-1) {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("0x%2x]", tmp_buf[i]));
-		} else {
-			RT_TRACE(COMP_BT_COEXIST, DBG_TRACE, ("0x%2x, ", tmp_buf[i]));
-		}
-	}
-	RT_TRACE(COMP_BT_COEXIST, DBG_DMESG,
-		("BT info bt_info (Data)= 0x%x\n",hal_coex_8821ae.c2h_bt_info_original));
-	bt_info = hal_coex_8821ae.c2h_bt_info_original;
-
-	if(bt_info & BIT(2)){
-		hal_coex_8821ae.b_c2h_bt_inquiry_page = true;
-	} else {
-		hal_coex_8821ae.b_c2h_bt_inquiry_page = false;
-	}
-
-	if (bt_info & BTINFO_B_CONNECTION) {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTC2H], BTInfo: bConnect=true\n"));
-		rtlpcipriv->btcoexist.b_bt_busy = true;
-		rtlpcipriv->btcoexist.current_state &= ~BT_COEX_STATE_BT_IDLE;
-	} else {
-		RT_TRACE(COMP_BT_COEXIST, DBG_DMESG, ("[BTC2H], BTInfo: bConnect=false\n"));
-		rtlpcipriv->btcoexist.b_bt_busy = false;
-		rtlpcipriv->btcoexist.current_state |= BT_COEX_STATE_BT_IDLE;
-	}
-}
-void rtl_8821ae_c2h_command_handle(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct c2h_evt_hdr c2h_event;
-	u8 * ptmp_buf = NULL;
-	u8 index = 0;
-	u8 u1b_tmp = 0;
-	memset(&c2h_event, 0, sizeof(c2h_event));
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL);
-	RT_TRACE(COMP_FW, DBG_DMESG,
-		("&&&&&&: REG_C2HEVT_MSG_NORMAL is 0x%x\n", u1b_tmp));
-	c2h_event.cmd_id = u1b_tmp & 0xF;
-	c2h_event.cmd_len = (u1b_tmp & 0xF0) >> 4;
-	c2h_event.cmd_seq = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL + 1);
-	RT_TRACE(COMP_FW, DBG_DMESG, ("cmd_id: %d, cmd_len: %d, cmd_seq: %d\n",
-		c2h_event.cmd_id , c2h_event.cmd_len, c2h_event.cmd_seq));
-	u1b_tmp = rtl_read_byte(rtlpriv, 0x01AF);
-	if (u1b_tmp == C2H_EVT_HOST_CLOSE) {
-		return;
-	} else if (u1b_tmp != C2H_EVT_FW_CLOSE) {
-		rtl_write_byte(rtlpriv, 0x1AF, 0x00);
-		return;
-	}
-	ptmp_buf = kmalloc(c2h_event.cmd_len, GFP_KERNEL);
-	if(ptmp_buf == NULL) {
-		RT_TRACE(COMP_FW, DBG_TRACE, ("malloc cmd buf failed\n"));
-		return;
-	}
-
-	/* Read the content */
-	for (index = 0; index < c2h_event.cmd_len; index ++) {
-		ptmp_buf[index] = rtl_read_byte(rtlpriv, REG_C2HEVT_MSG_NORMAL + 2+ index);
-	}
-
-	switch(c2h_event.cmd_id) {
-		case C2H_BT_RSSI:
-			break;
-
-	case C2H_BT_OP_MODE:
-			break;
-
-	case BT_INFO:
-		RT_TRACE(COMP_FW, DBG_TRACE,
-			("BT info Byte[0] (ID) is 0x%x\n", c2h_event.cmd_id));
-		RT_TRACE(COMP_FW, DBG_TRACE,
-			("BT info Byte[1] (Seq) is 0x%x\n", c2h_event.cmd_seq));
-		RT_TRACE(COMP_FW, DBG_TRACE,
-			("BT info Byte[2] (Data)= 0x%x\n", ptmp_buf[0]));
-
-		if (rtlpriv->cfg->ops->get_btc_status()){
-			rtlpriv->btcoexist.btc_ops->btc_btinfo_notify(rtlpriv, ptmp_buf, c2h_event.cmd_len);
-		}
-		break;
-	default:
-		break;
-	}
-
-	kfree(ptmp_buf);
-
-	rtl_write_byte(rtlpriv, 0x01AF, C2H_EVT_HOST_CLOSE);
-}
-
-
-
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.h b/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.h
deleted file mode 100644
index a94474faca49..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/hal_btc.h
+++ /dev/null
@@ -1,160 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8821AE_HAL_BTC_H__
-#define __RTL8821AE_HAL_BTC_H__
-
-#include "../wifi.h"
-#include "btc.h"
-#include "hal_bt_coexist.h"
-
-#define	BT_TXRX_CNT_THRES_1				1200
-#define	BT_TXRX_CNT_THRES_2				1400
-#define	BT_TXRX_CNT_THRES_3				3000
-#define	BT_TXRX_CNT_LEVEL_0				0	// < 1200
-#define	BT_TXRX_CNT_LEVEL_1				1	// >= 1200 && < 1400
-#define	BT_TXRX_CNT_LEVEL_2				2	// >= 1400
-#define	BT_TXRX_CNT_LEVEL_3				3
-
-
-
-#define	BT_COEX_DISABLE		0
-#define	BT_Q_PKT_OFF		0
-#define	BT_Q_PKT_ON			1
-
-#define	BT_TX_PWR_OFF		0
-#define	BT_TX_PWR_ON		1
-
-/* TDMA mode definition */
-#define	TDMA_2ANT			0
-#define	TDMA_1ANT			1
-#define	TDMA_NAV_OFF		0
-#define	TDMA_NAV_ON			1
-#define	TDMA_DAC_SWING_OFF	0
-#define	TDMA_DAC_SWING_ON	1
-
-/* PTA mode related definition */
-#define	BT_PTA_MODE_OFF		0
-#define	BT_PTA_MODE_ON		1
-
-/* Penalty Tx Rate Adaptive */
-#define	BT_TX_RATE_ADAPTIVE_NORMAL	0
-#define	BT_TX_RATE_ADAPTIVE_LOW_PENALTY	1
-
-/* RF Corner */
-#define	BT_RF_RX_LPF_CORNER_RESUME	0
-#define	BT_RF_RX_LPF_CORNER_SHRINK	1
-
-#define C2H_EVT_HOST_CLOSE		0x00
-#define C2H_EVT_FW_CLOSE		0xFF
-
-enum bt_traffic_mode {
-	BT_MOTOR_EXT_BE = 0x00,
-	BT_MOTOR_EXT_GUL = 0x01,
-	BT_MOTOR_EXT_GUB = 0x02,
-	BT_MOTOR_EXT_GULB = 0x03
-};
-
-enum bt_traffic_mode_profile {
-	BT_PROFILE_NONE,
-	BT_PROFILE_A2DP,
-	BT_PROFILE_PAN,
-	BT_PROFILE_HID,
-	BT_PROFILE_SCO
-};
-
-enum hci_ext_bt_operation {
-	HCI_BT_OP_NONE = 0x0,
-	HCI_BT_OP_INQUIRE_START	= 0x1,
-	HCI_BT_OP_INQUIRE_FINISH = 0x2,
-	HCI_BT_OP_PAGING_START = 0x3,
-	HCI_BT_OP_PAGING_SUCCESS = 0x4,
-	HCI_BT_OP_PAGING_UNSUCCESS = 0x5,
-	HCI_BT_OP_PAIRING_START = 0x6,
-	HCI_BT_OP_PAIRING_FINISH = 0x7,
-	HCI_BT_OP_BT_DEV_ENABLE = 0x8,
-	HCI_BT_OP_BT_DEV_DISABLE = 0x9,
-	HCI_BT_OP_MAX,
-};
-
-enum bt_spec {
-	BT_SPEC_1_0_b = 0x00,
-	BT_SPEC_1_1 = 0x01,
-	BT_SPEC_1_2 = 0x02,
-	BT_SPEC_2_0_EDR = 0x03,
-	BT_SPEC_2_1_EDR = 0x04,
-	BT_SPEC_3_0_HS = 0x05,
-	BT_SPEC_4_0 = 0x06
-};
-
-struct c2h_evt_hdr {
-        u8 cmd_id;
-        u8 cmd_len;
-        u8 cmd_seq;
-};
-
-enum bt_state{
-	BT_INFO_STATE_DISABLED = 0,
-	BT_INFO_STATE_NO_CONNECTION = 1,
-	BT_INFO_STATE_CONNECT_IDLE = 2,
-	BT_INFO_STATE_INQ_OR_PAG = 3,
-	BT_INFO_STATE_ACL_ONLY_BUSY = 4,
-	BT_INFO_STATE_SCO_ONLY_BUSY = 5,
-	BT_INFO_STATE_ACL_SCO_BUSY = 6,
-	BT_INFO_STATE_HID_BUSY = 7,
-	BT_INFO_STATE_HID_SCO_BUSY = 8,
-	BT_INFO_STATE_MAX = 7
-};
-
-enum rtl8723be_c2h_evt {
-	C2H_DBG = 0,
-	C2H_TSF = 1,
-	C2H_AP_RPT_RSP = 2,
-	C2H_CCX_TX_RPT = 3,	// The FW notify the report of the specific tx packet.
-	C2H_BT_RSSI = 4,
-	C2H_BT_OP_MODE = 5,
-	C2H_HW_INFO_EXCH = 10,
-	C2H_C2H_H2C_TEST = 11,
-	BT_INFO = 9,
-	MAX_C2HEVENT
-};
-
-
-
-void rtl8821ae_dm_bt_fw_coex_all_off_8723a(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_sw_coex_all_off_8723a(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_hw_coex_all_off_8723a(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_coexist(struct ieee80211_hw *hw);
-void rtl8821ae_dm_bt_set_bt_dm(struct ieee80211_hw *hw, struct btdm_8821ae *p_btdm);
-void rtl_8821ae_c2h_command_handle(struct ieee80211_hw * hw);
-void rtl_8821ae_bt_wifi_media_status_notify(struct ieee80211_hw * hw, bool mstatus);
-void rtl8821ae_dm_bt_turn_off_bt_coexist_before_enter_lps(struct ieee80211_hw *hw);
-
-
-
-#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/hw.c b/drivers/staging/rtl8821ae/rtl8821ae/hw.c
deleted file mode 100644
index 1aa16612e248..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/hw.c
+++ /dev/null
@@ -1,3347 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "../wifi.h"
-#include "../efuse.h"
-#include "../base.h"
-#include "../regd.h"
-#include "../cam.h"
-#include "../ps.h"
-#include "../pci.h"
-#include "reg.h"
-#include "def.h"
-#include "phy.h"
-#include "dm.h"
-#include "fw.h"
-#include "led.h"
-#include "hw.h"
-#include "pwrseqcmd.h"
-#include "pwrseq.h"
-#include "btc.h"
-#include "../btcoexist/rtl_btc.h"
-
-#define LLT_CONFIG	5
-
-static void _rtl8821ae_return_beacon_queue_skb(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[BEACON_QUEUE];
-
-	while (skb_queue_len(&ring->queue)) {
-		struct rtl_tx_desc *entry = &ring->desc[ring->idx];
-		struct sk_buff *skb = __skb_dequeue(&ring->queue);
-
-		pci_unmap_single(rtlpci->pdev,
-				 le32_to_cpu(rtlpriv->cfg->ops->get_desc(
-				 (u8 *) entry, true, HW_DESC_TXBUFF_ADDR)),
-				 skb->len, PCI_DMA_TODEVICE);
-		kfree_skb(skb);
-		ring->idx = (ring->idx + 1) % ring->entries;
-	}
-
-}
-
-static void _rtl8821ae_set_bcn_ctrl_reg(struct ieee80211_hw *hw,
-				      u8 set_bits, u8 clear_bits)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtlpci->reg_bcn_ctrl_val |= set_bits;
-	rtlpci->reg_bcn_ctrl_val &= ~clear_bits;
-
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
-}
-
-void _rtl8821ae_stop_tx_beacon(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp1byte;
-
-	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
-	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte & (~BIT(6)));
-	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0x64);
-	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
-	tmp1byte &= ~(BIT(0));
-	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
-}
-
-void _rtl8821ae_resume_tx_beacon(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp1byte;
-
-	tmp1byte = rtl_read_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2);
-	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2, tmp1byte | BIT(6));
-	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1, 0xff);
-	tmp1byte = rtl_read_byte(rtlpriv, REG_TBTT_PROHIBIT + 2);
-	tmp1byte |= BIT(0);
-	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 2, tmp1byte);
-}
-
-static void _rtl8821ae_enable_bcn_sub_func(struct ieee80211_hw *hw)
-{
-	_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(1));
-}
-
-static void _rtl8821ae_disable_bcn_sub_func(struct ieee80211_hw *hw)
-{
-	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(1), 0);
-}
-
-static void _rtl8821ae_set_fw_clock_on(struct ieee80211_hw *hw,
-	u8 rpwm_val, bool b_need_turn_off_ckk)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_support_remote_wake_up;
-	u32 count = 0,isr_regaddr,content;
-	bool b_schedule_timer = b_need_turn_off_ckk;
-	rtlpriv->cfg->ops->get_hw_reg(hw, HAL_DEF_WOWLAN,
-					(u8 *) (&b_support_remote_wake_up));
-
-	if (!rtlhal->bfw_ready)
-		return;
-	if (!rtlpriv->psc.b_fw_current_inpsmode)
-		return;
-
-	while (1) {
-		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		if (rtlhal->bfw_clk_change_in_progress) {
-			while (rtlhal->bfw_clk_change_in_progress) {
-				spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-				count++;
-				udelay(100);
-				if (count > 1000)
-					return;
-				spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-			}
-			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-		} else {
-			rtlhal->bfw_clk_change_in_progress = false;
-			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-			break;
-		}
-	}
-
-	if (IS_IN_LOW_POWER_STATE_8821AE(rtlhal->fw_ps_state)) {
-		rtlpriv->cfg->ops->get_hw_reg(hw, HW_VAR_SET_RPWM,
-					(u8 *) (&rpwm_val));
-		if (FW_PS_IS_ACK(rpwm_val)) {
-			isr_regaddr = REG_HISR;
-			content = rtl_read_dword(rtlpriv, isr_regaddr);
-			while (!(content & IMR_CPWM) && (count < 500)) {
-				udelay(50);
-				count++;
-				content = rtl_read_dword(rtlpriv, isr_regaddr);
-			}
-
-			if (content & IMR_CPWM) {
-			rtl_write_word(rtlpriv,isr_regaddr, 0x0100);
-			rtlhal->fw_ps_state = FW_PS_STATE_RF_ON_8821AE;
-			RT_TRACE(COMP_POWER, DBG_LOUD, ("Receive CPWM INT!!! Set pHalData->FwPSState = %X\n", rtlhal->fw_ps_state));
-			}
-		}
-
-		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		rtlhal->bfw_clk_change_in_progress = false;
-		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-		if (b_schedule_timer) {
-			mod_timer(&rtlpriv->works.fw_clockoff_timer,
-				  jiffies + MSECS(10));
-		}
-
-	} else  {
-		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		rtlhal->bfw_clk_change_in_progress = false;
-		spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-	}
-
-
-}
-
-static void _rtl8821ae_set_fw_clock_off(struct ieee80211_hw *hw,
-	u8 rpwm_val)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring;
-	enum rf_pwrstate rtstate;
-	bool b_schedule_timer = false;
-	u8 queue;
-
-	if (!rtlhal->bfw_ready)
-		return;
-	if (!rtlpriv->psc.b_fw_current_inpsmode)
-		return;
-	if (!rtlhal->ballow_sw_to_change_hwclc)
-		return;
-	rtlpriv->cfg->ops->get_hw_reg(hw,HW_VAR_RF_STATE,(u8 *)(&rtstate));
-	if (rtstate == ERFOFF ||rtlpriv->psc.inactive_pwrstate ==ERFOFF)
-		return;
-
-	for (queue = 0; queue < RTL_PCI_MAX_TX_QUEUE_COUNT; queue++) {
-		ring = &rtlpci->tx_ring[queue];
-		if (skb_queue_len(&ring->queue)) {
-			b_schedule_timer = true;
-			break;
-		}
-	}
-
-	if (b_schedule_timer) {
-		mod_timer(&rtlpriv->works.fw_clockoff_timer,
-					  jiffies + MSECS(10));
-		return;
-	}
-
-	if (FW_PS_STATE(rtlhal->fw_ps_state) != FW_PS_STATE_RF_OFF_LOW_PWR_8821AE) {
-		spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-		if (!rtlhal->bfw_clk_change_in_progress) {
-			rtlhal->bfw_clk_change_in_progress = true;
-			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-			rtlhal->fw_ps_state = FW_PS_STATE(rpwm_val);
-			rtl_write_word(rtlpriv, REG_HISR, 0x0100);
-			rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-				(u8 *) (&rpwm_val));
-			spin_lock_bh(&rtlpriv->locks.fw_ps_lock);
-			rtlhal->bfw_clk_change_in_progress = false;
-			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-		} else {
-			spin_unlock_bh(&rtlpriv->locks.fw_ps_lock);
-			mod_timer(&rtlpriv->works.fw_clockoff_timer,
-					  jiffies + MSECS(10));
-		}
-	}
-
-}
-
-static void _rtl8821ae_set_fw_ps_rf_on(struct ieee80211_hw *hw)
-{
-	u8 rpwm_val = 0;
-	rpwm_val |= (FW_PS_STATE_RF_OFF_8821AE | FW_PS_ACK);
-	_rtl8821ae_set_fw_clock_on(hw, rpwm_val, true);
-}
-
-static void _rtl8821ae_fwlps_leave(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_fw_current_inps = false;
-	u8 rpwm_val = 0,fw_pwrmode = FW_PS_ACTIVE_MODE;
-
-	if (ppsc->b_low_power_enable){
-		rpwm_val = (FW_PS_STATE_ALL_ON_8821AE|FW_PS_ACK);/* RF on */
-		_rtl8821ae_set_fw_clock_on(hw, rpwm_val, false);
-		rtlhal->ballow_sw_to_change_hwclc = false;
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-				(u8 *) (&fw_pwrmode));
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				(u8 *) (&b_fw_current_inps));
-	} else {
-		rpwm_val = FW_PS_STATE_ALL_ON_8821AE;	/* RF on */
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SET_RPWM,
-				(u8 *) (&rpwm_val));
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_H2C_FW_PWRMODE,
-				(u8 *) (&fw_pwrmode));
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_FW_PSMODE_STATUS,
-				(u8 *) (&b_fw_current_inps));
-	}
-
-}
-
-static void _rtl8821ae_fwlps_enter(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_fw_current_inps = true;
-	u8 rpwm_val;
-
-	if (ppsc->b_low_power_enable){
-		rpwm_val = FW_PS_STATE_RF_OFF_LOW_PWR_8821AE;	/* RF off */
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-				HW_VAR_FW_PSMODE_STATUS,
-				(u8 *) (&b_fw_current_inps));
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-				HW_VAR_H2C_FW_PWRMODE,
-				(u8 *) (&ppsc->fwctrl_psmode));
-		rtlhal->ballow_sw_to_change_hwclc = true;
-		_rtl8821ae_set_fw_clock_off(hw, rpwm_val);
-
-
-	} else {
-		rpwm_val = FW_PS_STATE_RF_OFF_8821AE;	/* RF off */
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-				HW_VAR_FW_PSMODE_STATUS,
-				(u8 *) (&b_fw_current_inps));
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-				HW_VAR_H2C_FW_PWRMODE,
-				(u8 *) (&ppsc->fwctrl_psmode));
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-				HW_VAR_SET_RPWM,
-				(u8 *) (&rpwm_val));
-	}
-
-}
-
-void rtl8821ae_get_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-
-	switch (variable) {
-	case HW_VAR_ETHER_ADDR:
-		*((u32 *)(val)) = rtl_read_dword(rtlpriv, REG_MACID);
-		*((u16 *)(val+4)) = rtl_read_word(rtlpriv, REG_MACID + 4);
-		break;
-	case HW_VAR_BSSID:
-		*((u32 *)(val)) = rtl_read_dword(rtlpriv, REG_BSSID);
-		*((u16 *)(val+4)) = rtl_read_word(rtlpriv, REG_BSSID+4);
-		break;
-	case HW_VAR_MEDIA_STATUS:
-		val[0] = rtl_read_byte(rtlpriv, REG_CR+2) & 0x3;
-		break;
-	case HW_VAR_SLOT_TIME:
-		*((u8 *)(val)) = mac->slot_time;
-		break;
-	case HW_VAR_BEACON_INTERVAL:
-		*((u16 *)(val)) = rtl_read_word(rtlpriv, REG_BCN_INTERVAL);
-		break;
-	case HW_VAR_ATIM_WINDOW:
-		*((u16 *)(val)) =  rtl_read_word(rtlpriv, REG_ATIMWND);
-		break;
-	case HW_VAR_RCR:
-		*((u32 *) (val)) = rtlpci->receive_config;
-		break;
-	case HW_VAR_RF_STATE:
-		*((enum rf_pwrstate *)(val)) = ppsc->rfpwr_state;
-		break;
-	case HW_VAR_FWLPS_RF_ON:{
-			enum rf_pwrstate rfState;
-			u32 val_rcr;
-
-			rtlpriv->cfg->ops->get_hw_reg(hw,
-						      HW_VAR_RF_STATE,
-						      (u8 *) (&rfState));
-			if (rfState == ERFOFF) {
-				*((bool *) (val)) = true;
-			} else {
-				val_rcr = rtl_read_dword(rtlpriv, REG_RCR);
-				val_rcr &= 0x00070000;
-				if (val_rcr)
-					*((bool *) (val)) = false;
-				else
-					*((bool *) (val)) = true;
-			}
-			break;
-		}
-	case HW_VAR_FW_PSMODE_STATUS:
-		*((bool *) (val)) = ppsc->b_fw_current_inpsmode;
-		break;
-	case HW_VAR_CORRECT_TSF:{
-			u64 tsf;
-			u32 *ptsf_low = (u32 *) & tsf;
-			u32 *ptsf_high = ((u32 *) & tsf) + 1;
-
-			*ptsf_high = rtl_read_dword(rtlpriv, (REG_TSFTR + 4));
-			*ptsf_low = rtl_read_dword(rtlpriv, REG_TSFTR);
-
-			*((u64 *) (val)) = tsf;
-
-			break;
-		}
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process %x\n",variable));
-		break;
-	}
-}
-
-
-void rtl8821ae_set_hw_reg(struct ieee80211_hw *hw, u8 variable, u8 *val)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 idx;
-
-	switch (variable) {
-	case HW_VAR_ETHER_ADDR:{
-			for (idx = 0; idx < ETH_ALEN; idx++) {
-				rtl_write_byte(rtlpriv, (REG_MACID + idx),
-					       val[idx]);
-			}
-			break;
-		}
-	case HW_VAR_BASIC_RATE:{
-			u16 b_rate_cfg = ((u16 *) val)[0];
-			u8 rate_index = 0;
-			b_rate_cfg = b_rate_cfg & 0x15f;
-			b_rate_cfg |= 0x01;
-			rtl_write_byte(rtlpriv, REG_RRSR, b_rate_cfg & 0xff);
-			rtl_write_byte(rtlpriv, REG_RRSR + 1,
-				       (b_rate_cfg >> 8) & 0xff);
-			while (b_rate_cfg > 0x1) {
-				b_rate_cfg = (b_rate_cfg >> 1);
-				rate_index++;
-			}
-			rtl_write_byte(rtlpriv, REG_INIRTS_RATE_SEL,
-				       rate_index);
-			break;
-		}
-	case HW_VAR_BSSID:{
-			for (idx = 0; idx < ETH_ALEN; idx++) {
-				rtl_write_byte(rtlpriv, (REG_BSSID + idx),
-					       val[idx]);
-			}
-			break;
-		}
-	case HW_VAR_SIFS:{
-			rtl_write_byte(rtlpriv, REG_SIFS_CTX + 1, val[0]);
-			rtl_write_byte(rtlpriv, REG_SIFS_TRX + 1, val[1]);
-
-			rtl_write_byte(rtlpriv, REG_SPEC_SIFS + 1, val[0]);
-			rtl_write_byte(rtlpriv, REG_MAC_SPEC_SIFS + 1, val[0]);
-
-			if (!mac->ht_enable)
-				rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
-					       0x0e0e);
-			else
-				rtl_write_word(rtlpriv, REG_RESP_SIFS_OFDM,
-					       *((u16 *) val));
-			break;
-		}
-	case HW_VAR_SLOT_TIME:{
-			u8 e_aci;
-
-			RT_TRACE(COMP_MLME, DBG_LOUD,
-				 ("HW_VAR_SLOT_TIME %x\n", val[0]));
-
-			rtl_write_byte(rtlpriv, REG_SLOT, val[0]);
-
-			for (e_aci = 0; e_aci < AC_MAX; e_aci++) {
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-							      HW_VAR_AC_PARAM,
-							      (u8 *) (&e_aci));
-			}
-			break;
-		}
-	case HW_VAR_ACK_PREAMBLE:{
-			u8 reg_tmp;
-			u8 short_preamble = (bool) (*(u8 *) val);
-			reg_tmp = rtl_read_byte(rtlpriv, REG_TRXPTCL_CTL+2);
-			if (short_preamble){
-				reg_tmp |= BIT(1);
-				rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
-			} else {
-				reg_tmp &= (~BIT(1));
-				rtl_write_byte(rtlpriv, REG_TRXPTCL_CTL + 2, reg_tmp);
-			}
-			break;
-		}
-	case HW_VAR_WPA_CONFIG:
-		rtl_write_byte(rtlpriv, REG_SECCFG, *((u8 *) val));
-		break;
-	case HW_VAR_AMPDU_MIN_SPACE:{
-			u8 min_spacing_to_set;
-			u8 sec_min_space;
-
-			min_spacing_to_set = *((u8 *) val);
-			if (min_spacing_to_set <= 7) {
-				sec_min_space = 0;
-
-				if (min_spacing_to_set < sec_min_space)
-					min_spacing_to_set = sec_min_space;
-
-				mac->min_space_cfg = ((mac->min_space_cfg &
-						       0xf8) |
-						      min_spacing_to_set);
-
-				*val = min_spacing_to_set;
-
-				RT_TRACE(COMP_MLME, DBG_LOUD,
-					 ("Set HW_VAR_AMPDU_MIN_SPACE: %#x\n",
-					  mac->min_space_cfg));
-
-				rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
-					       mac->min_space_cfg);
-			}
-			break;
-		}
-	case HW_VAR_SHORTGI_DENSITY:{
-			u8 density_to_set;
-
-			density_to_set = *((u8 *) val);
-			mac->min_space_cfg |= (density_to_set << 3);
-
-			RT_TRACE(COMP_MLME, DBG_LOUD,
-				 ("Set HW_VAR_SHORTGI_DENSITY: %#x\n",
-				  mac->min_space_cfg));
-
-			rtl_write_byte(rtlpriv, REG_AMPDU_MIN_SPACE,
-				       mac->min_space_cfg);
-
-			break;
-		}
-	case HW_VAR_AMPDU_FACTOR:{
-			u32	ampdu_len =  (*((u8 *)val));
-			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-				if(ampdu_len < VHT_AGG_SIZE_128K)
-					ampdu_len = (0x2000 << (*((u8 *)val))) -1;
-				else
-					ampdu_len = 0x1ffff;
-			} else if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-				if(ampdu_len < HT_AGG_SIZE_64K)
-					ampdu_len = (0x2000 << (*((u8 *)val))) -1;
-				else
-					ampdu_len = 0xffff;
-			}
-			ampdu_len |= BIT(31);
-
-			rtl_write_dword(rtlpriv,
-				REG_AMPDU_MAX_LENGTH_8812, ampdu_len);
-			break;
-		}
-	case HW_VAR_AC_PARAM:{
-			u8 e_aci = *((u8 *) val);
-			rtl8821ae_dm_init_edca_turbo(hw);
-
-			if (rtlpci->acm_method != eAcmWay2_SW)
-				rtlpriv->cfg->ops->set_hw_reg(hw,
-							      HW_VAR_ACM_CTRL,
-							      (u8 *) (&e_aci));
-			break;
-		}
-	case HW_VAR_ACM_CTRL:{
-			u8 e_aci = *((u8 *) val);
-			union aci_aifsn *p_aci_aifsn =
-			    (union aci_aifsn *)(&(mac->ac[0].aifs));
-			u8 acm = p_aci_aifsn->f.acm;
-			u8 acm_ctrl = rtl_read_byte(rtlpriv, REG_ACMHWCTRL);
-
-			acm_ctrl =
-			    acm_ctrl | ((rtlpci->acm_method == 2) ? 0x0 : 0x1);
-
-			if (acm) {
-				switch (e_aci) {
-				case AC0_BE:
-					acm_ctrl |= AcmHw_BeqEn;
-					break;
-				case AC2_VI:
-					acm_ctrl |= AcmHw_ViqEn;
-					break;
-				case AC3_VO:
-					acm_ctrl |= AcmHw_VoqEn;
-					break;
-				default:
-					RT_TRACE(COMP_ERR, DBG_WARNING,
-						 ("HW_VAR_ACM_CTRL acm set "
-						  "failed: eACI is %d\n", acm));
-					break;
-				}
-			} else {
-				switch (e_aci) {
-				case AC0_BE:
-					acm_ctrl &= (~AcmHw_BeqEn);
-					break;
-				case AC2_VI:
-					acm_ctrl &= (~AcmHw_ViqEn);
-					break;
-				case AC3_VO:
-					acm_ctrl &= (~AcmHw_BeqEn);
-					break;
-				default:
-					RT_TRACE(COMP_ERR, DBG_EMERG,
-						 ("switch case not process \n"));
-					break;
-				}
-			}
-
-			RT_TRACE(COMP_QOS, DBG_TRACE,
-				 ("SetHwReg8190pci(): [HW_VAR_ACM_CTRL] "
-				  "Write 0x%X\n", acm_ctrl));
-			rtl_write_byte(rtlpriv, REG_ACMHWCTRL, acm_ctrl);
-			break;
-		}
-	case HW_VAR_RCR:{
-			rtl_write_dword(rtlpriv, REG_RCR, ((u32 *) (val))[0]);
-			rtlpci->receive_config = ((u32 *) (val))[0];
-			break;
-		}
-	case HW_VAR_RETRY_LIMIT:{
-			u8 retry_limit = ((u8 *) (val))[0];
-
-			rtl_write_word(rtlpriv, REG_RL,
-				       retry_limit << RETRY_LIMIT_SHORT_SHIFT |
-				       retry_limit << RETRY_LIMIT_LONG_SHIFT);
-			break;
-		}
-	case HW_VAR_DUAL_TSF_RST:
-		rtl_write_byte(rtlpriv, REG_DUAL_TSF_RST, (BIT(0) | BIT(1)));
-		break;
-	case HW_VAR_EFUSE_BYTES:
-		rtlefuse->efuse_usedbytes = *((u16 *) val);
-		break;
-	case HW_VAR_EFUSE_USAGE:
-		rtlefuse->efuse_usedpercentage = *((u8 *) val);
-		break;
-	case HW_VAR_IO_CMD:
-		rtl8821ae_phy_set_io_cmd(hw, (*(enum io_type *)val));
-		break;
-	case HW_VAR_SET_RPWM:{
-			u8 rpwm_val;
-
-			rpwm_val = rtl_read_byte(rtlpriv, REG_PCIE_HRPWM);
-			udelay(1);
-
-			if (rpwm_val & BIT(7)) {
-				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
-					       (*(u8 *) val));
-			} else {
-				rtl_write_byte(rtlpriv, REG_PCIE_HRPWM,
-					       ((*(u8 *) val) | BIT(7)));
-			}
-
-			break;
-		}
-	case HW_VAR_H2C_FW_PWRMODE:{
-			rtl8821ae_set_fw_pwrmode_cmd(hw, (*(u8 *) val));
-			break;
-		}
-	case HW_VAR_FW_PSMODE_STATUS:
-		ppsc->b_fw_current_inpsmode = *((bool *) val);
-		break;
-
-	case HW_VAR_RESUME_CLK_ON:
-		_rtl8821ae_set_fw_ps_rf_on(hw);
-		break;
-
-	case HW_VAR_FW_LPS_ACTION:{
-		bool b_enter_fwlps = *((bool *) val);
-
-		if (b_enter_fwlps)
-			_rtl8821ae_fwlps_enter(hw);
-		 else
-			_rtl8821ae_fwlps_leave(hw);
-
-		 break;
-		}
-
-	case HW_VAR_H2C_FW_JOINBSSRPT:{
-			u8 mstatus = (*(u8 *) val);
-			u8 tmp_regcr, tmp_reg422,bcnvalid_reg;
-			u8 count = 0, dlbcn_count = 0;
-			bool b_recover = false;
-
-			if (mstatus == RT_MEDIA_CONNECT) {
-				rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_AID,
-							      NULL);
-
-				tmp_regcr = rtl_read_byte(rtlpriv, REG_CR + 1);
-				rtl_write_byte(rtlpriv, REG_CR + 1,
-					       (tmp_regcr | BIT(0)));
-
-				_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
-				_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
-
-				tmp_reg422 =
-				    rtl_read_byte(rtlpriv,
-						  REG_FWHW_TXQ_CTRL + 2);
-				rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 2,
-					       tmp_reg422 & (~BIT(6)));
-				if (tmp_reg422 & BIT(6))
-					b_recover = true;
-
-				do {
-					bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
-					rtl_write_byte(rtlpriv, REG_TDECTRL+2,(bcnvalid_reg | BIT(0)));
-					_rtl8821ae_return_beacon_queue_skb(hw);
-
-					if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-						rtl8812ae_set_fw_rsvdpagepkt(hw, 0);
-					else
-						rtl8821ae_set_fw_rsvdpagepkt(hw, 0);
-					bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
-					count = 0;
-					while (!(bcnvalid_reg & BIT(0)) && count <20){
-						count++;
-						udelay(10);
-						bcnvalid_reg = rtl_read_byte(rtlpriv, REG_TDECTRL+2);
-					}
-					dlbcn_count++;
-				} while (!(bcnvalid_reg & BIT(0)) && dlbcn_count <5);
-
-				if (bcnvalid_reg & BIT(0))
-					rtl_write_byte(rtlpriv, REG_TDECTRL+2, BIT(0));
-
-				_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
-				_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
-
-				if (b_recover) {
-					rtl_write_byte(rtlpriv,
-						       REG_FWHW_TXQ_CTRL + 2,
-						       tmp_reg422);
-				}
-
-				rtl_write_byte(rtlpriv, REG_CR + 1,
-					       (tmp_regcr & ~(BIT(0))));
-			}
-			rtl8821ae_set_fw_joinbss_report_cmd(hw, (*(u8 *) val));
-
-			break;
-		}
-	case HW_VAR_H2C_FW_P2P_PS_OFFLOAD:{
-		rtl8821ae_set_p2p_ps_offload_cmd(hw, (*(u8 *) val));
-		break;
-	}
-
-	case HW_VAR_AID:{
-			u16 u2btmp;
-			u2btmp = rtl_read_word(rtlpriv, REG_BCN_PSR_RPT);
-			u2btmp &= 0xC000;
-			rtl_write_word(rtlpriv, REG_BCN_PSR_RPT, (u2btmp |
-						mac->assoc_id));
-
-			break;
-		}
-	case HW_VAR_CORRECT_TSF:{
-			u8 btype_ibss = ((u8 *) (val))[0];
-
-			if (btype_ibss == true)
-				_rtl8821ae_stop_tx_beacon(hw);
-
-			_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(3));
-
-			rtl_write_dword(rtlpriv, REG_TSFTR,
-					(u32) (mac->tsf & 0xffffffff));
-			rtl_write_dword(rtlpriv, REG_TSFTR + 4,
-					(u32) ((mac->tsf >> 32) & 0xffffffff));
-
-			_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
-
-			if (btype_ibss == true)
-				_rtl8821ae_resume_tx_beacon(hw);
-
-			break;
-
-		}
-	case HW_VAR_NAV_UPPER: {
-			u32	us_nav_upper = ((u32)*val);
-
-			if(us_nav_upper > HAL_92C_NAV_UPPER_UNIT * 0xFF)
-			{
-				RT_TRACE(COMP_INIT , DBG_WARNING,
-					("The setting value (0x%08X us) of NAV_UPPER"
-					 " is larger than (%d * 0xFF)!!!\n",
-					 us_nav_upper, HAL_92C_NAV_UPPER_UNIT));
-				break;
-			}
-			rtl_write_byte(rtlpriv, REG_NAV_UPPER,
-				((u8)((us_nav_upper + HAL_92C_NAV_UPPER_UNIT - 1) / HAL_92C_NAV_UPPER_UNIT)));
-			break;
-		}
-	case HW_VAR_KEEP_ALIVE: {
-			u8 array[2];
-			array[0] = 0xff;
-			array[1] = *((u8 *)val);
-			rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_KEEP_ALIVE_CTRL, 2, array);
-		}
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case "
-							"not process %x\n",variable));
-		break;
-	}
-}
-
-static bool _rtl8821ae_llt_write(struct ieee80211_hw *hw, u32 address, u32 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	bool status = true;
-	long count = 0;
-	u32 value = _LLT_INIT_ADDR(address) |
-	    _LLT_INIT_DATA(data) | _LLT_OP(_LLT_WRITE_ACCESS);
-
-	rtl_write_dword(rtlpriv, REG_LLT_INIT, value);
-
-	do {
-		value = rtl_read_dword(rtlpriv, REG_LLT_INIT);
-		if (_LLT_NO_ACTIVE == _LLT_OP_VALUE(value))
-			break;
-
-		if (count > POLLING_LLT_THRESHOLD) {
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Failed to polling write LLT done at "
-				  "address %d!\n", address));
-			status = false;
-			break;
-		}
-	} while (++count);
-
-	return status;
-}
-
-static bool _rtl8821ae_llt_table_init(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	unsigned short i;
-	u8 txpktbuf_bndy;
-	u8 maxPage;
-	bool status;
-
-	maxPage = 255;
-	txpktbuf_bndy = 0xF8;
-
-
-	rtl_write_byte(rtlpriv, REG_TRXFF_BNDY, txpktbuf_bndy);
-	rtl_write_word(rtlpriv, REG_TRXFF_BNDY + 2, MAX_RX_DMA_BUFFER_SIZE - 1);
-
-	rtl_write_byte(rtlpriv, REG_TDECTRL + 1, txpktbuf_bndy);
-
-	rtl_write_byte(rtlpriv, REG_TXPKTBUF_BCNQ_BDNY, txpktbuf_bndy);
-	rtl_write_byte(rtlpriv, REG_TXPKTBUF_MGQ_BDNY, txpktbuf_bndy);
-
-	rtl_write_byte(rtlpriv, REG_PBP, 0x31);
-	rtl_write_byte(rtlpriv, REG_RX_DRVINFO_SZ, 0x4);
-
-	for (i = 0; i < (txpktbuf_bndy - 1); i++) {
-		status = _rtl8821ae_llt_write(hw, i, i + 1);
-		if (true != status)
-			return status;
-	}
-
-	status = _rtl8821ae_llt_write(hw, (txpktbuf_bndy - 1), 0xFF);
-	if (true != status)
-		return status;
-
-	for (i = txpktbuf_bndy; i < maxPage; i++) {
-		status = _rtl8821ae_llt_write(hw, i, (i + 1));
-		if (true != status)
-			return status;
-	}
-
-	status = _rtl8821ae_llt_write(hw, maxPage, txpktbuf_bndy);
-	if (true != status)
-		return status;
-
-	rtl_write_dword(rtlpriv, REG_RQPN, 0x80e70808);
-	rtl_write_byte(rtlpriv, REG_RQPN_NPQ, 0x00);
-
-	return true;
-}
-
-static void _rtl8821ae_gen_refresh_led_state(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_led *pLed0 = &(pcipriv->ledctl.sw_led0);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	if (rtlpriv->rtlhal.up_first_time)
-		return;
-
-	if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS)
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-			rtl8812ae_sw_led_on(hw, pLed0);
-		else
-			rtl8821ae_sw_led_on(hw, pLed0);
-	else if (ppsc->rfoff_reason == RF_CHANGE_BY_INIT)
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-			rtl8812ae_sw_led_on(hw, pLed0);
-		else
-			rtl8821ae_sw_led_on(hw, pLed0);
-	else
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-			rtl8812ae_sw_led_off(hw, pLed0);
-		else
-			rtl8821ae_sw_led_off(hw, pLed0);
-}
-
-static bool _rtl8821ae_init_mac(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	u8 bytetmp = 0;
-	u16 wordtmp = 0;
-	bool b_mac_func_enable = rtlhal->b_mac_func_enable;
-
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x00);
-
-	/*Auto Power Down to CHIP-off State*/
-	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO + 1) & (~BIT(7));
-	rtl_write_byte(rtlpriv, REG_APS_FSMCO + 1, bytetmp);
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-		/* HW Power on sequence*/
-		if(!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-			PWR_INTF_PCI_MSK, RTL8812_NIC_ENABLE_FLOW)) {
-				RT_TRACE(COMP_INIT,DBG_LOUD,("init 8812 MAC Fail as power on failure\n"));
-				return false;
-		}
-	} else {
-		/* HW Power on sequence */
-		if (!rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_A_MSK, PWR_FAB_ALL_MSK,
-			PWR_INTF_PCI_MSK, RTL8821A_NIC_ENABLE_FLOW)){
-			RT_TRACE(COMP_INIT,DBG_LOUD,("init 8821 MAC Fail as power on failure\n"));
-			return false;
-		}
-	}
-
-	bytetmp = rtl_read_byte(rtlpriv, REG_APS_FSMCO) | BIT(4);
-	rtl_write_byte(rtlpriv, REG_APS_FSMCO, bytetmp);
-
-	bytetmp = rtl_read_byte(rtlpriv, REG_CR);
-	bytetmp = 0xff;
-	rtl_write_byte(rtlpriv, REG_CR, bytetmp);
-	mdelay(2);
-
-	bytetmp |= 0x7f;
-	rtl_write_byte(rtlpriv, REG_HWSEQ_CTRL, bytetmp);
-	mdelay(2);
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		bytetmp = rtl_read_byte(rtlpriv, REG_SYS_CFG + 3);
-		if (bytetmp & BIT(0)) {
-			bytetmp = rtl_read_byte(rtlpriv, 0x7c);
-			bytetmp |= BIT(6);
-			rtl_write_byte(rtlpriv, 0x7c, bytetmp);
-		}
-	}
-
-	bytetmp = rtl_read_byte(rtlpriv, REG_GPIO_MUXCFG + 1);
-	bytetmp &= ~BIT(4);
-	rtl_write_byte(rtlpriv, REG_GPIO_MUXCFG + 1, bytetmp);
-
-	rtl_write_word(rtlpriv, REG_CR, 0x2ff);
-
-	if (!b_mac_func_enable) {
-		if (!_rtl8821ae_llt_table_init(hw))
-			return false;
-	}
-
-	rtl_write_dword(rtlpriv, REG_HISR, 0xffffffff);
-	rtl_write_dword(rtlpriv, REG_HISRE, 0xffffffff);
-
-	/* Enable FW Beamformer Interrupt */
-	bytetmp = rtl_read_byte(rtlpriv, REG_FWIMR + 3);
-	rtl_write_byte(rtlpriv, REG_FWIMR + 3, bytetmp | BIT(6));
-
-	wordtmp = rtl_read_word(rtlpriv, REG_TRXDMA_CTRL);
-	wordtmp &= 0xf;
-	wordtmp |= 0xF5B1;
-	rtl_write_word(rtlpriv, REG_TRXDMA_CTRL, wordtmp);
-
-	rtl_write_byte(rtlpriv, REG_FWHW_TXQ_CTRL + 1, 0x1F);
-	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
-	rtl_write_word(rtlpriv, REG_RXFLTMAP2, 0xFFFF);
-	/*low address*/
-	rtl_write_dword(rtlpriv, REG_BCNQ_DESA,
-			rtlpci->tx_ring[BEACON_QUEUE].dma & DMA_BIT_MASK(32));
-	rtl_write_dword(rtlpriv, REG_MGQ_DESA,
-			rtlpci->tx_ring[MGNT_QUEUE].dma & DMA_BIT_MASK(32));
-	rtl_write_dword(rtlpriv, REG_VOQ_DESA,
-			rtlpci->tx_ring[VO_QUEUE].dma & DMA_BIT_MASK(32));
-	rtl_write_dword(rtlpriv, REG_VIQ_DESA,
-			rtlpci->tx_ring[VI_QUEUE].dma & DMA_BIT_MASK(32));
-	rtl_write_dword(rtlpriv, REG_BEQ_DESA,
-			rtlpci->tx_ring[BE_QUEUE].dma & DMA_BIT_MASK(32));
-	rtl_write_dword(rtlpriv, REG_BKQ_DESA,
-			rtlpci->tx_ring[BK_QUEUE].dma & DMA_BIT_MASK(32));
-	rtl_write_dword(rtlpriv, REG_HQ_DESA,
-			rtlpci->tx_ring[HIGH_QUEUE].dma & DMA_BIT_MASK(32));
-	rtl_write_dword(rtlpriv, REG_RX_DESA,
-			rtlpci->rx_ring[RX_MPDU_QUEUE].dma & DMA_BIT_MASK(32));
-
-	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 3, 0x77);
-
-	rtl_write_dword(rtlpriv, REG_INT_MIG, 0);
-
-	rtl_write_byte(rtlpriv, REG_SECONDARY_CCA_CTRL, 0x3);
-	_rtl8821ae_gen_refresh_led_state(hw);
-
-	return true;
-}
-
-static void _rtl8821ae_hw_configure(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u32 reg_rrsr;
-
-	reg_rrsr = RATE_ALL_CCK | RATE_ALL_OFDM_AG;
-
-	rtl_write_dword(rtlpriv, REG_RRSR, reg_rrsr);
-	/* ARFB table 9 for 11ac 5G 2SS */
-	rtl_write_dword(rtlpriv, REG_ARFR0 + 4, 0xfffff000);
-	/* ARFB table 10 for 11ac 5G 1SS */
-	rtl_write_dword(rtlpriv, REG_ARFR1 + 4, 0x003ff000);
-	/* ARFB table 11 for 11ac 24G 1SS */
-	rtl_write_dword(rtlpriv, REG_ARFR2, 0x00000015);
-	rtl_write_dword(rtlpriv, REG_ARFR2 + 4, 0x003ff000);
-	/* ARFB table 12 for 11ac 24G 1SS */
-	rtl_write_dword(rtlpriv, REG_ARFR3, 0x00000015);
-	rtl_write_dword(rtlpriv, REG_ARFR3 + 4, 0xffcff000);
-	/* 0x420[7] = 0 , enable retry AMPDU in new AMPD not signal MPDU. */
-	rtl_write_word(rtlpriv, REG_FWHW_TXQ_CTRL, 0x1F00);
-	rtl_write_byte(rtlpriv, REG_AMPDU_MAX_TIME, 0x70);
-
-	/*Set retry limit*/
-	rtl_write_word(rtlpriv, REG_RL, 0x0707);
-
-
-	/* Set Data / Response auto rate fallack retry count*/
-	rtl_write_dword(rtlpriv, REG_DARFRC, 0x01000000);
-	rtl_write_dword(rtlpriv, REG_DARFRC + 4, 0x07060504);
-	rtl_write_dword(rtlpriv, REG_RARFRC, 0x01000000);
-	rtl_write_dword(rtlpriv, REG_RARFRC + 4, 0x07060504);
-
-	rtlpci->reg_bcn_ctrl_val = 0x1d;
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, rtlpci->reg_bcn_ctrl_val);
-
-	/* TBTT prohibit hold time. Suggested by designer TimChen. */
-	rtl_write_byte(rtlpriv, REG_TBTT_PROHIBIT + 1,0xff); // 8 ms
-
-	/* AGGR_BK_TIME Reg51A 0x16 */
-	rtl_write_word(rtlpriv, REG_NAV_PROT_LEN, 0x0040);
-
-	/*For Rx TP. Suggested by SD1 Richard. Added by tynli. 2010.04.12.*/
-	rtl_write_dword(rtlpriv, REG_FAST_EDCA_CTRL, 0x03086666);
-
-	rtl_write_byte(rtlpriv, REG_HT_SINGLE_AMPDU, 0x80);
-	rtl_write_byte(rtlpriv, REG_RX_PKT_LIMIT, 0x20);
-	rtl_write_word(rtlpriv, REG_MAX_AGGR_NUM, 0x1F1F);
-}
-
-static u16 _rtl8821ae_mdio_read(struct rtl_priv *rtlpriv, u8 addr)
-{
-	u16 ret = 0;
-	u8 tmp = 0, count = 0;
-
-	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(6));
-	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6) ;
-	count = 0;
-	while (tmp && count < 20) {
-		udelay(10);
-		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(6);
-		count++;
-	}
-	if (0 == tmp)
-		ret = rtl_read_word(rtlpriv, REG_MDIO_RDATA);
-
-	return ret;
-}
-
-void _rtl8821ae_mdio_write(struct rtl_priv *rtlpriv, u8 addr, u16 data)
-{
-	u8 tmp = 0, count = 0;
-
-	rtl_write_word(rtlpriv, REG_MDIO_WDATA, data);
-	rtl_write_byte(rtlpriv, REG_MDIO_CTL, addr | BIT(5));
-	tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5) ;
-	count = 0;
-	while (tmp && count < 20) {
-		udelay(10);
-		tmp = rtl_read_byte(rtlpriv, REG_MDIO_CTL) & BIT(5);
-		count++;
-	}
-}
-
-static u8 _rtl8821ae_dbi_read(struct rtl_priv *rtlpriv, u16 addr)
-{
-	u16 read_addr = addr & 0xfffc;
-	u8 tmp = 0, count = 0, ret = 0;
-
-	rtl_write_word(rtlpriv, REG_DBI_ADDR, read_addr);
-	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x2);
-	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
-	count = 0;
-	while (tmp && count < 20) {
-		udelay(10);
-		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
-		count++;
-	}
-	if (0 == tmp) {
-		read_addr = REG_DBI_RDATA + addr % 4;
-		ret = rtl_read_word(rtlpriv, read_addr);
-	}
-	return ret;
-}
-
-void _rtl8821ae_dbi_write(struct rtl_priv *rtlpriv, u16 addr, u8 data)
-{
-	u8 tmp = 0, count = 0;
-	u16 wrtie_addr, remainder = addr % 4;
-
-	wrtie_addr = REG_DBI_WDATA + remainder;
-	rtl_write_byte(rtlpriv, wrtie_addr, data);
-
-	wrtie_addr = (addr & 0xfffc) | (BIT(0) << (remainder + 12));
-	rtl_write_word(rtlpriv, REG_DBI_ADDR, wrtie_addr);
-
-	rtl_write_byte(rtlpriv, REG_DBI_FLAG, 0x1);
-
-	tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
-	count = 0;
-	while (tmp && count < 20) {
-		udelay(10);
-		tmp = rtl_read_byte(rtlpriv, REG_DBI_FLAG);
-		count++;
-	}
-
-}
-
-static void _rtl8821ae_enable_aspm_back_door(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 tmp;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		if (_rtl8821ae_mdio_read(rtlpriv, 0x04) != 0x8544)
-			_rtl8821ae_mdio_write(rtlpriv, 0x04, 0x8544);
-
-		if (_rtl8821ae_mdio_read(rtlpriv, 0x0b) != 0x0070)
-			_rtl8821ae_mdio_write(rtlpriv, 0x0b, 0x0070);
-	}
-
-	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x70f);
-	_rtl8821ae_dbi_write(rtlpriv, 0x70f, tmp | BIT(7));
-
-	tmp = _rtl8821ae_dbi_read(rtlpriv, 0x719);
-	_rtl8821ae_dbi_write(rtlpriv, 0x719, tmp | BIT(3) | BIT(4));
-
-	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-	{
-		tmp  = _rtl8821ae_dbi_read(rtlpriv, 0x718);
-		_rtl8821ae_dbi_write(rtlpriv, 0x718, tmp|BIT(4));
-	}
-}
-
-void rtl8821ae_enable_hw_security_config(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 sec_reg_value;
-	u8 tmp;
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("PairwiseEncAlgorithm = %d GroupEncAlgorithm = %d\n",
-		  rtlpriv->sec.pairwise_enc_algorithm,
-		  rtlpriv->sec.group_enc_algorithm));
-
-	if (rtlpriv->cfg->mod_params->sw_crypto || rtlpriv->sec.use_sw_sec) {
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("not open hw encryption\n"));
-		return;
-	}
-
-	sec_reg_value = SCR_TxEncEnable | SCR_RxDecEnable;
-
-	if (rtlpriv->sec.use_defaultkey) {
-		sec_reg_value |= SCR_TxUseDK;
-		sec_reg_value |= SCR_RxUseDK;
-	}
-
-	sec_reg_value |= (SCR_RXBCUSEDK | SCR_TXBCUSEDK);
-
-	tmp = rtl_read_byte(rtlpriv, REG_CR + 1);
-	rtl_write_byte(rtlpriv, REG_CR + 1, tmp | BIT(1));
-
-	RT_TRACE(COMP_SEC, DBG_DMESG,
-		 ("The SECR-value %x \n", sec_reg_value));
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_WPA_CONFIG, &sec_reg_value);
-
-}
-
-#if 0
-bool _rtl8821ae_check_pcie_dma_hang(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp;
-	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL+3);
-	if (!(tmp&BIT(2))) {
-		rtl_write_byte(rtlpriv, REG_DBI_CTRL+3, tmp|BIT(2));
-		mdelay(100);
-	}
-
-	tmp = rtl_read_byte(rtlpriv, REG_DBI_CTRL+3);
-	if (tmp&BIT(0) || tmp&BIT(1)) {
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			("rtl8821ae_check_pcie_dma_hang(): TRUE! Reset PCIE DMA!\n"));
-		return true;
-	} else {
-		return false;
-	}
-}
-
-void _rtl8821ae_reset_pcie_interface_dma(struct ieee80211_hw *hw,
-													bool mac_power_on, bool watch_dog)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 tmp;
-	bool release_mac_rx_pause;
-	u8 backup_pcie_dma_pause;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("_rtl8821ae_reset_pcie_interface_dma()\n"));
-
-	tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL);
-	tmp &= ~BIT(1);
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL, tmp);
-	tmp = rtl_read_byte(rtlpriv, REG_PMC_DBG_CTRL2);
-	tmp |= BIT2;
-	rtl_write_byte(rtlpriv, REG_PMC_DBG_CTRL2, tmp);
-
-	tmp = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
-	if (tmp & BIT(2)) {
-		release_mac_rx_pause = false;
-	} else {
-		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, tmp | BIT(2));
-		release_mac_rx_pause = true;
-	}
-	backup_pcie_dma_pause = rtl_read_byte(rtlpriv, REG_PCIE_CTRL_REG+1);
-	if (backup_pcie_dma_pause != 0xFF)
-		rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG+1, 0xFF);
-
-	if (mac_power_on)
-		rtl_write_byte(rtlpriv, REG_CR, 0);
-
-	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
-	tmp &= ~BIT(0);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, tmp);
-
-	tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
-	tmp |= ~BIT(0);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, tmp);
-
-	if (mac_power_on)
-		rtl_write_byte(rtlpriv, REG_CR, 0xFF);
-
-	tmp = rtl_read_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL+2);
-	tmp |= BIT1;
-	rtl_write_byte(rtlpriv, REG_MAC_PHY_CTRL_NORMAL+2, tmp);
-
-	if (watch_dog) {
-		u32 rqpn = 0;
-		u32 rqpn_npq = 0;
-		u8 tx_page_boundary = _RQPN_Init_8812E(Adapter, &rqpn_npq, &rqpn);
-
-		if(LLT_table_init_8812(Adapter, TX_PAGE_BOUNDARY, RQPN, RQPN_NPQ) == RT_STATUS_FAILURE)
-			return false;
-
-			PlatformAcquireSpinLock(Adapter, RT_RX_SPINLOCK);
-			PlatformAcquireSpinLock(Adapter, RT_TX_SPINLOCK);
-
-			// <1> Reset Tx descriptor
-			Adapter->HalFunc.ResetTxDescHandler(Adapter,Adapter->NumTxDesc);
-
-			// <2> Reset Rx descriptor
-			Adapter->HalFunc.ResetRxDescHandler(Adapter,Adapter->NumRxDesc);
-
-			// <3> Reset RFDs
-			FreeRFDs( Adapter, TRUE);
-
-			// <4> Reset TCBs
-			FreeTCBs( Adapter, TRUE);
-
-			// We should set all Rx desc own bit to 1 to prevent from RDU after enable Rx DMA. 2013.02.18, by tynli.
-			PrepareAllRxDescBuffer(Adapter);
-
-			PlatformReleaseSpinLock(Adapter, RT_TX_SPINLOCK);
-			PlatformReleaseSpinLock(Adapter, RT_RX_SPINLOCK);
-
-			//
-			// Initialize TRx DMA address.
-			//
-			// Because set 0x100 to 0x0 will cause the Rx descriptor address 0x340 be cleared to zero on 88EE,
-			// we should re-initialize Rx desc. address before enable DMA. 2012.11.07. by tynli.
-			InitTRxDescHwAddress8812AE(Adapter);
-		}
-
-		// In MAC power on state, BB and RF maybe in ON state, if we release TRx DMA here
-		// it will cause packets to be started to Tx/Rx, so we release Tx/Rx DMA later.
-		if(!bInMACPowerOn || bInWatchDog)
-		{
-			// 8. release TRX DMA
-			//write 0x284 bit[18] = 1'b0
-			//write 0x301 = 0x00
-			if(bReleaseMACRxPause)
-			{
-				u1Tmp = PlatformEFIORead1Byte(Adapter, REG_RXDMA_CONTROL);
-				PlatformEFIOWrite1Byte(Adapter, REG_RXDMA_CONTROL, (u1Tmp&~BIT2));
-			}
-			PlatformEFIOWrite1Byte(Adapter, 	REG_PCIE_CTRL_REG+1, BackUpPcieDMAPause);
-		}
-
-		if(IS_HARDWARE_TYPE_8821E(Adapter))
-		{
-			//9. lock system register
-			//	 write 0xCC bit[2] = 1'b0
-			u1Tmp = PlatformEFIORead1Byte(Adapter, REG_PMC_DBG_CTRL2_8723B);
-			u1Tmp &= ~(BIT2);
-			PlatformEFIOWrite1Byte(Adapter, REG_PMC_DBG_CTRL2_8723B, u1Tmp);
-		}
-
-		return RT_STATUS_SUCCESS;
-}
-#endif
-
-// Static MacID Mapping (cf. Used in MacIdDoStaticMapping) ----------
-#define MAC_ID_STATIC_FOR_DEFAULT_PORT				0
-#define MAC_ID_STATIC_FOR_BROADCAST_MULTICAST		1
-#define MAC_ID_STATIC_FOR_BT_CLIENT_START				2
-#define MAC_ID_STATIC_FOR_BT_CLIENT_END				3
-// -----------------------------------------------------------
-
-void rtl8821ae_macid_initialize_mediastatus(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8	media_rpt[4] = {RT_MEDIA_CONNECT, 1, \
-		MAC_ID_STATIC_FOR_BROADCAST_MULTICAST, \
-		MAC_ID_STATIC_FOR_BT_CLIENT_END};
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, \
-		HW_VAR_H2C_FW_MEDIASTATUSRPT, media_rpt);
-
-	RT_TRACE(COMP_INIT,DBG_LOUD, \
-		("Initialize MacId media status: from %d to %d\n", \
-		MAC_ID_STATIC_FOR_BROADCAST_MULTICAST, \
-		MAC_ID_STATIC_FOR_BT_CLIENT_END));
-}
-
-int rtl8821ae_hw_init(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool rtstatus = true;
-	int err;
-	u8 tmp_u1b;
-	u32 nav_upper = WIFI_NAV_UPPER_US;
-
-	rtlpriv->rtlhal.being_init_adapter = true;
-	rtlpriv->intf_ops->disable_aspm(hw);
-
-	/*YP wowlan not considered*/
-
-	tmp_u1b = rtl_read_byte(rtlpriv, REG_CR);
-	if (tmp_u1b!=0 && tmp_u1b != 0xEA) {
-		rtlhal->b_mac_func_enable = true;
-		RT_TRACE(COMP_INIT,DBG_LOUD,(" MAC has already power on.\n"));
-	} else {
-		rtlhal->b_mac_func_enable = false;
-		rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_8821AE;
-	}
-
-/*	if (_rtl8821ae_check_pcie_dma_hang(hw)) {
-		_rtl8821ae_reset_pcie_interface_dma(hw,rtlhal->b_mac_func_enable,false);
-		rtlhal->b_mac_func_enable = false;
-	} */
-
-	rtstatus = _rtl8821ae_init_mac(hw);
-	if (rtstatus != true) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Init MAC failed\n"));
-		err = 1;
-		return err;
-	}
-
-	tmp_u1b = rtl_read_byte(rtlpriv, REG_SYS_CFG);
-	tmp_u1b &= 0x7F;
-	rtl_write_byte(rtlpriv, REG_SYS_CFG, tmp_u1b);
-
-	err = rtl8821ae_download_fw(hw, false);
-	if (err) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("Failed to download FW. Init HW "
-			  "without FW now..\n"));
-		err = 1;
-		rtlhal->bfw_ready = false;
-		return err;
-	} else {
-		rtlhal->bfw_ready = true;
-	}
-	rtlhal->fw_ps_state = FW_PS_STATE_ALL_ON_8821AE;
-	rtlhal->bfw_clk_change_in_progress = false;
-	rtlhal->ballow_sw_to_change_hwclc = false;
-	rtlhal->last_hmeboxnum = 0;
-
-	/*SIC_Init(Adapter);
-	if(pHalData->AMPDUBurstMode)
-		PlatformEFIOWrite1Byte(Adapter,REG_AMPDU_BURST_MODE_8812,  0x7F);*/
-
-	rtl8821ae_phy_mac_config(hw);
-	/* because last function modify RCR, so we update
-	 * rcr var here, or TP will unstable for receive_config
-	 * is wrong, RX RCR_ACRC32 will cause TP unstable & Rx
-	 * RCR_APP_ICV will cause mac80211 unassoc for cisco 1252
-	rtlpci->receive_config = rtl_read_dword(rtlpriv, REG_RCR);
-	rtlpci->receive_config &= ~(RCR_ACRC32 | RCR_AICV);
-	rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);*/
-	rtl8821ae_phy_bb_config(hw);
-
-	rtl8821ae_phy_rf_config(hw);
-
-	_rtl8821ae_hw_configure(hw);
-
-	rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_2_4G);
-
-	/*set wireless mode*/
-
-	rtlhal->b_mac_func_enable = true;
-
-	rtl_cam_reset_all_entry(hw);
-
-	rtl8821ae_enable_hw_security_config(hw);
-
-	ppsc->rfpwr_state = ERFON;
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_ETHER_ADDR, mac->mac_addr);
-	_rtl8821ae_enable_aspm_back_door(hw);
-	rtlpriv->intf_ops->enable_aspm(hw);
-
-	//rtl8821ae_bt_hw_init(hw);
-	rtlpriv->rtlhal.being_init_adapter = false;
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_NAV_UPPER, (u8 *)&nav_upper);
-
-	//rtl8821ae_dm_check_txpower_tracking(hw);
-	//rtl8821ae_phy_lc_calibrate(hw);
-
-	/* Release Rx DMA*/
-	tmp_u1b = rtl_read_byte(rtlpriv, REG_RXDMA_CONTROL);
-	if (tmp_u1b & BIT(2)) {
-		/* Release Rx DMA if needed*/
-		tmp_u1b &= ~BIT(2);
-		rtl_write_byte(rtlpriv, REG_RXDMA_CONTROL, tmp_u1b);
-	}
-
-	/* Release Tx/Rx PCIE DMA if*/
-	rtl_write_byte(rtlpriv, REG_PCIE_CTRL_REG + 1, 0);
-
-	rtl8821ae_dm_init(hw);
-	rtl8821ae_macid_initialize_mediastatus(hw);
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("rtl8821ae_hw_init() <====\n"));
-	return err;
-}
-
-static enum version_8821ae _rtl8821ae_read_chip_version(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	enum version_8821ae version = VERSION_UNKNOWN;
-	u32 value32;
-
-	value32 = rtl_read_dword(rtlpriv, REG_SYS_CFG1);
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("ReadChipVersion8812A 0xF0 = 0x%x \n", value32));
-
-
-
-	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-		rtlphy->rf_type = RF_2T2R;
-	else if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-		rtlphy->rf_type = RF_1T1R;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("RF_Type is %x!!\n", rtlphy->rf_type));
-
-
-	if (value32 & TRP_VAUX_EN)
-	{
-		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-		{
-			if(rtlphy->rf_type == RF_2T2R)
-				version = VERSION_TEST_CHIP_2T2R_8812;
-			else
-				version = VERSION_TEST_CHIP_1T1R_8812;
-		}
-		else
-			version = VERSION_TEST_CHIP_8821;
-	} else {
-		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-		{
-			u32	rtl_id = ((value32 & CHIP_VER_RTL_MASK) >> 12) +1 ;
-
-			if(rtlphy->rf_type == RF_2T2R)
-				version = (enum version_8821ae)(CHIP_8812 | NORMAL_CHIP | RF_TYPE_2T2R);
-			else
-				version = (enum version_8821ae)(CHIP_8812 | NORMAL_CHIP);
-
-			version = (enum version_8821ae)(version| (rtl_id << 12));
-		}
-		else if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-		{
-			u32	rtl_id = value32 & CHIP_VER_RTL_MASK;
-
-			version = (enum version_8821ae)(CHIP_8821 | NORMAL_CHIP | rtl_id);
-		}
-	}
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("Chip RF Type: %s\n", (rtlphy->rf_type == RF_2T2R) ?
-		  "RF_2T2R" : "RF_1T1R"));
-
-	if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-	{
-		/*WL_HWROF_EN.*/
-		value32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
-		rtlphy->hw_rof_enable= ((value32 & WL_HWROF_EN) ? 1 : 0);
-	}
-
-	switch(version)
-	{
-		case VERSION_TEST_CHIP_1T1R_8812:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_TEST_CHIP_1T1R_8812.\n"));
-			break;
-		case VERSION_TEST_CHIP_2T2R_8812:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_TEST_CHIP_2T2R_8812.\n"));
-			break;
-		case VERSION_NORMAL_TSMC_CHIP_1T1R_8812:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_1T1R_8812.\n"));
-			break;
-		case VERSION_NORMAL_TSMC_CHIP_2T2R_8812:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_2T2R_8812.\n"));
-			break;
-		case VERSION_NORMAL_TSMC_CHIP_1T1R_8812_C_CUT:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_1T1R_8812 C CUT.\n"));
-			break;
-		case VERSION_NORMAL_TSMC_CHIP_2T2R_8812_C_CUT:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_2T2R_8812 C CUT.\n"));
-			break;
-		case VERSION_TEST_CHIP_8821:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_TEST_CHIP_8821.\n"));
-			break;
-		case VERSION_NORMAL_TSMC_CHIP_8821:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_8821 A CUT.\n"));
-			break;
-		case VERSION_NORMAL_TSMC_CHIP_8821_B_CUT:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: VERSION_NORMAL_TSMC_CHIP_8821 B CUT.\n"));
-			break;
-		default:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Chip Version ID: Unknown (0x%X).\n", version));
-			break;
-	}
-
-	return version;
-}
-
-static int _rtl8821ae_set_media_status(struct ieee80211_hw *hw,
-				     enum nl80211_iftype type)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 bt_msr = rtl_read_byte(rtlpriv, MSR);
-	enum led_ctl_mode ledaction = LED_CTL_NO_LINK;
-	bt_msr &= 0xfc;
-
-	rtl_write_dword(rtlpriv, REG_BCN_CTRL, 0);
-	RT_TRACE(COMP_BEACON, DBG_LOUD,
-		("clear 0x550 when set HW_VAR_MEDIA_STATUS\n"));
-
-	if (type == NL80211_IFTYPE_UNSPECIFIED ||
-	    type == NL80211_IFTYPE_STATION) {
-		_rtl8821ae_stop_tx_beacon(hw);
-		_rtl8821ae_enable_bcn_sub_func(hw);
-	} else if (type == NL80211_IFTYPE_ADHOC ||
-		type == NL80211_IFTYPE_AP) {
-		_rtl8821ae_resume_tx_beacon(hw);
-		_rtl8821ae_disable_bcn_sub_func(hw);
-	} else {
-		RT_TRACE(COMP_ERR, DBG_WARNING,("Set HW_VAR_MEDIA_STATUS: "
-			  "No such media status(%x).\n", type));
-	}
-
-	switch (type) {
-	case NL80211_IFTYPE_UNSPECIFIED:
-		bt_msr |= MSR_NOLINK;
-		ledaction = LED_CTL_LINK;
-		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to NO LINK!\n"));
-		break;
-	case NL80211_IFTYPE_ADHOC:
-		bt_msr |= MSR_ADHOC;
-		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to Ad Hoc!\n"));
-		break;
-	case NL80211_IFTYPE_STATION:
-		bt_msr |= MSR_INFRA;
-		ledaction = LED_CTL_LINK;
-		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to STA!\n"));
-		break;
-	case NL80211_IFTYPE_AP:
-		bt_msr |= MSR_AP;
-		RT_TRACE(COMP_INIT, DBG_TRACE, ("Set Network type to AP!\n"));
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Network type %d not support!\n", type));
-		return 1;
-		break;
-
-	}
-
-	rtl_write_byte(rtlpriv, (MSR), bt_msr);
-	rtlpriv->cfg->ops->led_control(hw, ledaction);
-	if ((bt_msr & MSR_MASK) == MSR_AP)
-		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x00);
-	else
-		rtl_write_byte(rtlpriv, REG_BCNTCFG + 1, 0x66);
-
-	return 0;
-}
-
-void rtl8821ae_set_check_bssid(struct ieee80211_hw *hw, bool check_bssid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u32 reg_rcr = rtlpci->receive_config;
-
-	if (rtlpriv->psc.rfpwr_state != ERFON)
-		return;
-
-	if (check_bssid == true) {
-		reg_rcr |= (RCR_CBSSID_DATA | RCR_CBSSID_BCN);
-		rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_RCR,
-					      (u8 *) (&reg_rcr));
-		_rtl8821ae_set_bcn_ctrl_reg(hw, 0, BIT(4));
-	} else if (check_bssid == false) {
-		reg_rcr &= (~(RCR_CBSSID_DATA | RCR_CBSSID_BCN));
-		_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(4), 0);
-		rtlpriv->cfg->ops->set_hw_reg(hw,
-			HW_VAR_RCR, (u8 *) (&reg_rcr));
-	}
-
-}
-
-int rtl8821ae_set_network_type(struct ieee80211_hw *hw, enum nl80211_iftype type)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("rtl8821ae_set_network_type!\n"));
-
-	if (_rtl8821ae_set_media_status(hw, type))
-		return -EOPNOTSUPP;
-
-	if (rtlpriv->mac80211.link_state == MAC80211_LINKED) {
-		if (type != NL80211_IFTYPE_AP)
-			rtl8821ae_set_check_bssid(hw, true);
-	} else {
-		rtl8821ae_set_check_bssid(hw, false);
-	}
-
-	return 0;
-}
-
-/* don't set REG_EDCA_BE_PARAM here because mac80211 will send pkt when scan */
-void rtl8821ae_set_qos(struct ieee80211_hw *hw, int aci)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	rtl8821ae_dm_init_edca_turbo(hw);
-	switch (aci) {
-	case AC1_BK:
-		rtl_write_dword(rtlpriv, REG_EDCA_BK_PARAM, 0xa44f);
-		break;
-	case AC0_BE:
-		/* rtl_write_dword(rtlpriv, REG_EDCA_BE_PARAM, u4b_ac_param); */
-		break;
-	case AC2_VI:
-		rtl_write_dword(rtlpriv, REG_EDCA_VI_PARAM, 0x5e4322);
-		break;
-	case AC3_VO:
-		rtl_write_dword(rtlpriv, REG_EDCA_VO_PARAM, 0x2f3222);
-		break;
-	default:
-		RT_ASSERT(false, ("invalid aci: %d !\n", aci));
-		break;
-	}
-}
-
-void rtl8821ae_enable_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	rtl_write_dword(rtlpriv, REG_HIMR, rtlpci->irq_mask[0] & 0xFFFFFFFF);
-	rtl_write_dword(rtlpriv, REG_HIMRE, rtlpci->irq_mask[1] & 0xFFFFFFFF);
-	rtlpci->irq_enabled = true;
-	/* there are some C2H CMDs have been sent before system interrupt is enabled, e.g., C2H, CPWM.
-	*So we need to clear all C2H events that FW has notified, otherwise FW won't schedule any commands anymore.
-	*/
-	//rtl_write_byte(rtlpriv, REG_C2HEVT_CLEAR, 0);
-	/*enable system interrupt*/
-	rtl_write_dword(rtlpriv, REG_HSIMR, rtlpci->sys_irq_mask & 0xFFFFFFFF);
-}
-
-void rtl8821ae_disable_interrupt(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	rtl_write_dword(rtlpriv, REG_HIMR, IMR_DISABLED);
-	rtl_write_dword(rtlpriv, REG_HIMRE, IMR_DISABLED);
-	rtlpci->irq_enabled = false;
-	synchronize_irq(rtlpci->pdev->irq);
-}
-
-static void _rtl8821ae_poweroff_adapter(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 u1b_tmp;
-
-	rtlhal->b_mac_func_enable = false;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-	/* Combo (PCIe + USB) Card and PCIe-MF Card */
-	/* 1. Run LPS WL RFOFF flow */
-		//RT_TRACE(COMP_INIT, DBG_LOUD, ("=====>CardDisableRTL8812E,RTL8821A_NIC_LPS_ENTER_FLOW\n"));
-		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-			PWR_INTF_PCI_MSK, RTL8821A_NIC_LPS_ENTER_FLOW);
-	}
-	/* 2. 0x1F[7:0] = 0 */
-	/* turn off RF */
-	//rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x00);
-	if ((rtl_read_byte(rtlpriv, REG_MCUFWDL) & BIT(7)) &&
-		rtlhal->bfw_ready ) {
-		rtl8821ae_firmware_selfreset(hw);
-	}
-
-	/* Reset MCU. Suggested by Filen. */
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN+1);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN+1, (u1b_tmp & (~BIT(2))));
-
-	/* g.	MCUFWDL 0x80[1:0]=0	 */
-	/* reset MCU ready status */
-	rtl_write_byte(rtlpriv, REG_MCUFWDL, 0x00);
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-		/* HW card disable configuration. */
-		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-			PWR_INTF_PCI_MSK, RTL8821A_NIC_DISABLE_FLOW);
-	} else {
-		/* HW card disable configuration. */
-		rtl_hal_pwrseqcmdparsing(rtlpriv, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK,
-			PWR_INTF_PCI_MSK, RTL8812_NIC_DISABLE_FLOW);
-	}
-
-	/* Reset MCU IO Wrapper */
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, (u1b_tmp & (~BIT(0))));
-	u1b_tmp = rtl_read_byte(rtlpriv, REG_RSV_CTRL + 1);
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL + 1, u1b_tmp | BIT(0));
-
-	/* 7. RSV_CTRL 0x1C[7:0] = 0x0E */
-	/* lock ISO/CLK/Power control register */
-	rtl_write_byte(rtlpriv, REG_RSV_CTRL, 0x0e);
-}
-
-void rtl8821ae_card_disable(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	enum nl80211_iftype opmode;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("rtl8821ae_card_disable.\n"));
-
-	mac->link_state = MAC80211_NOLINK;
-	opmode = NL80211_IFTYPE_UNSPECIFIED;
-	_rtl8821ae_set_media_status(hw, opmode);
-	if (rtlpriv->rtlhal.driver_is_goingto_unload ||
-	    ppsc->rfoff_reason > RF_CHANGE_BY_PS)
-		rtlpriv->cfg->ops->led_control(hw, LED_CTL_POWER_OFF);
-	RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-	_rtl8821ae_poweroff_adapter(hw);
-
-	/* after power off we should do iqk again */
-	rtlpriv->phy.iqk_initialized = false;
-}
-
-void rtl8821ae_interrupt_recognized(struct ieee80211_hw *hw,
-				  u32 *p_inta, u32 *p_intb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	*p_inta = rtl_read_dword(rtlpriv, ISR) & rtlpci->irq_mask[0];
-	rtl_write_dword(rtlpriv, ISR, *p_inta);
-
-
-	*p_intb = rtl_read_dword(rtlpriv, REG_HISRE) & rtlpci->irq_mask[1];
-	rtl_write_dword(rtlpriv, REG_HISRE, *p_intb);
-
-}
-
-
-void rtl8821ae_set_beacon_related_registers(struct ieee80211_hw *hw)
-{
-
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u16 bcn_interval, atim_window;
-
-	bcn_interval = mac->beacon_interval;
-	atim_window = 2;	/*FIX MERGE */
-	rtl8821ae_disable_interrupt(hw);
-	rtl_write_word(rtlpriv, REG_ATIMWND, atim_window);
-	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
-	rtl_write_word(rtlpriv, REG_BCNTCFG, 0x660f);
-	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_CCK, 0x18);
-	rtl_write_byte(rtlpriv, REG_RXTSF_OFFSET_OFDM, 0x18);
-	rtl_write_byte(rtlpriv, 0x606, 0x30);
-	rtlpci->reg_bcn_ctrl_val |= BIT(3);
-	rtl_write_byte(rtlpriv, REG_BCN_CTRL, (u8) rtlpci->reg_bcn_ctrl_val);
-	rtl8821ae_enable_interrupt(hw);
-}
-
-void rtl8821ae_set_beacon_interval(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u16 bcn_interval = mac->beacon_interval;
-
-	RT_TRACE(COMP_BEACON, DBG_DMESG,
-		 ("beacon_interval:%d\n", bcn_interval));
-	rtl8821ae_disable_interrupt(hw);
-	rtl_write_word(rtlpriv, REG_BCN_INTERVAL, bcn_interval);
-	rtl8821ae_enable_interrupt(hw);
-}
-
-void rtl8821ae_update_interrupt_mask(struct ieee80211_hw *hw,
-				   u32 add_msr, u32 rm_msr)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	RT_TRACE(COMP_INTR, DBG_LOUD,
-		 ("add_msr:%x, rm_msr:%x\n", add_msr, rm_msr));
-
-	if (add_msr)
-		rtlpci->irq_mask[0] |= add_msr;
-	if (rm_msr)
-		rtlpci->irq_mask[0] &= (~rm_msr);
-	rtl8821ae_disable_interrupt(hw);
-	rtl8821ae_enable_interrupt(hw);
-}
-
-static u8 _rtl8821ae_get_chnl_group(u8 chnl)
-{
-	u8 group = 0;
-
-	if (chnl <= 14) {
-		if (1 <= chnl && chnl <= 2 )
-			group = 0;
-        else if (3 <= chnl && chnl <= 5 )
-			group = 1;
-        else if (6 <= chnl && chnl <= 8 )
-			group = 2;
-        else if (9 <= chnl && chnl <= 11)
-			group = 3;
-        else /*if (12 <= chnl && chnl <= 14)*/
-			group = 4;
-	} else {
-		if (36 <= chnl && chnl <= 42)
-			group = 0;
-        else if (44 <= chnl && chnl <= 48)
-			group = 1;
-        else if (50 <= chnl && chnl <= 58)
-			group = 2;
-        else if (60 <= chnl && chnl <= 64)
-			group = 3;
-        else if (100 <= chnl && chnl <= 106)
-			group = 4;
-        else if (108 <= chnl && chnl <= 114)
-			group = 5;
-        else if (116 <= chnl && chnl <= 122)
-			group = 6;
-        else if (124 <= chnl && chnl <= 130)
-			group = 7;
-        else if (132 <= chnl && chnl <= 138)
-			group = 8;
-        else if (140 <= chnl && chnl <= 144)
-			group = 9;
-        else if (149 <= chnl && chnl <= 155)
-			group = 10;
-        else if (157 <= chnl && chnl <= 161)
-			group = 11;
-        else if (165 <= chnl && chnl <= 171)
-			group = 12;
-        else if (173 <= chnl && chnl <= 177)
-			group = 13;
-		else
-			/*RT_TRACE(COMP_EFUSE,DBG_LOUD,
-				("5G, Channel %d in Group not found \n",chnl));*/
-			RT_ASSERT(!COMP_EFUSE,
-				("5G, Channel %d in Group not found \n",chnl));
-	}
-	return group;
-}
-
-static void _rtl8821ae_read_power_value_fromprom(struct ieee80211_hw *hw,
-	struct txpower_info_2g *pwrinfo24g,
-	struct txpower_info_5g *pwrinfo5g,
-	bool autoload_fail,
-	u8 *hwinfo)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 rfPath, eeAddr=EEPROM_TX_PWR_INX, group,TxCount=0;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("hal_ReadPowerValueFromPROM8821ae(): PROMContent[0x%x]=0x%x\n", (eeAddr+1), hwinfo[eeAddr+1]));
-	if (0xFF == hwinfo[eeAddr+1])  /*YJ,add,120316*/
-		autoload_fail = true;
-
-	if (autoload_fail)
-	{
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("auto load fail : Use Default value!\n"));
-		for (rfPath = 0 ; rfPath < MAX_RF_PATH ; rfPath++) {
-			/*2.4G default value*/
-			for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
-				pwrinfo24g->index_cck_base[rfPath][group] =	0x2D;
-				pwrinfo24g->index_bw40_base[rfPath][group] = 0x2D;
-			}
-			for (TxCount = 0;TxCount < MAX_TX_COUNT;TxCount++) {
-				if (TxCount == 0) {
-					pwrinfo24g->bw20_diff[rfPath][0] = 0x02;
-					pwrinfo24g->ofdm_diff[rfPath][0] = 0x04;
-				} else {
-					pwrinfo24g->bw20_diff[rfPath][TxCount] = 0xFE;
-					pwrinfo24g->bw40_diff[rfPath][TxCount] = 0xFE;
-					pwrinfo24g->cck_diff[rfPath][TxCount] =	0xFE;
-					pwrinfo24g->ofdm_diff[rfPath][TxCount] = 0xFE;
-				}
-			}
-			/*5G default value*/
-			for (group = 0 ; group < MAX_CHNL_GROUP_5G; group++)
-				pwrinfo5g->index_bw40_base[rfPath][group] = 0x2A;
-
-			for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
-				if (TxCount == 0) {
-					pwrinfo5g->ofdm_diff[rfPath][0] = 0x04;
-					pwrinfo5g->bw20_diff[rfPath][0] = 0x00;
-					pwrinfo5g->bw80_diff[rfPath][0] = 0xFE;
-					pwrinfo5g->bw160_diff[rfPath][0] = 0xFE;
-				} else {
-					pwrinfo5g->ofdm_diff[rfPath][0] = 0xFE;
-					pwrinfo5g->bw20_diff[rfPath][0] = 0xFE;
-					pwrinfo5g->bw40_diff[rfPath][0] = 0xFE;
-					pwrinfo5g->bw80_diff[rfPath][0] = 0xFE;
-					pwrinfo5g->bw160_diff[rfPath][0] = 0xFE;
-				}
-			}
-		}
-		return;
-	}
-
-	rtl_priv(hw)->efuse.b_txpwr_fromeprom = true;
-
-	for (rfPath = 0 ; rfPath < MAX_RF_PATH ; rfPath++) {
-		/*2.4G default value*/
-		for (group = 0 ; group < MAX_CHNL_GROUP_24G; group++) {
-			pwrinfo24g->index_cck_base[rfPath][group] = hwinfo[eeAddr++];
-			if (pwrinfo24g->index_cck_base[rfPath][group] == 0xFF)
-				pwrinfo24g->index_cck_base[rfPath][group] = 0x2D;
-		}
-		for (group = 0 ; group < MAX_CHNL_GROUP_24G - 1; group++) {
-			pwrinfo24g->index_bw40_base[rfPath][group] = hwinfo[eeAddr++];
-			if (pwrinfo24g->index_bw40_base[rfPath][group] == 0xFF)
-				pwrinfo24g->index_bw40_base[rfPath][group] = 0x2D;
-		}
-		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount ++) {
-			if (TxCount == 0) {
-				pwrinfo24g->bw40_diff[rfPath][TxCount] = 0;
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo24g->bw20_diff[rfPath][TxCount] = 0x02;
-				} else {
-					pwrinfo24g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0xf0) >> 4;
-					if (pwrinfo24g->bw20_diff[rfPath][TxCount] & BIT(3))	/*bit sign number to 8 bit sign number*/
-						pwrinfo24g->bw20_diff[rfPath][TxCount] |= 0xF0;
-				}
-
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo24g->ofdm_diff[rfPath][TxCount] = 0x04;
-				} else {
-					pwrinfo24g->ofdm_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
-					if(pwrinfo24g->ofdm_diff[rfPath][TxCount] & BIT(3))		/*bit sign number to 8 bit sign number*/
-						pwrinfo24g->ofdm_diff[rfPath][TxCount] |= 0xF0;
-				}
-				pwrinfo24g->cck_diff[rfPath][TxCount] = 0;
-				eeAddr++;
-			} else {
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo24g->bw40_diff[rfPath][TxCount] = 0xFE;
-				} else {
-					pwrinfo24g->bw40_diff[rfPath][TxCount] = (hwinfo[eeAddr]&0xf0) >> 4;
-					if (pwrinfo24g->bw40_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo24g->bw40_diff[rfPath][TxCount] |= 0xF0;
-				}
-
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo24g->bw20_diff[rfPath][TxCount] = 0xFE;
-				} else {
-					pwrinfo24g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
-					if (pwrinfo24g->bw20_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo24g->bw20_diff[rfPath][TxCount] |= 0xF0;
-				}
-
-				eeAddr++;
-
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo24g->ofdm_diff[rfPath][TxCount] = 0xFE;
-				} else {
-					pwrinfo24g->ofdm_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0xf0) >> 4;
-					if(pwrinfo24g->ofdm_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo24g->ofdm_diff[rfPath][TxCount] |= 0xF0;
-				}
-
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo24g->cck_diff[rfPath][TxCount] = 0xFE;
-				} else {
-					pwrinfo24g->cck_diff[rfPath][TxCount] =	(hwinfo[eeAddr] & 0x0f);
-					if(pwrinfo24g->cck_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo24g->cck_diff[rfPath][TxCount] |= 0xF0;
-				}
-				eeAddr++;
-			}
-		}
-
-		/*5G default value*/
-		for (group = 0 ; group < MAX_CHNL_GROUP_5G; group ++) {
-			pwrinfo5g->index_bw40_base[rfPath][group] = hwinfo[eeAddr++];
-			if (pwrinfo5g->index_bw40_base[rfPath][group] == 0xFF)
-				pwrinfo5g->index_bw40_base[rfPath][group] = 0xFE;
-		}
-
-		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
-			if (TxCount == 0) {
-				pwrinfo5g->bw40_diff[rfPath][TxCount] = 0;
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo5g->bw20_diff[rfPath][TxCount] = 0x0;
-				} else {
-					pwrinfo5g->bw20_diff[rfPath][0] = (hwinfo[eeAddr] & 0xf0) >> 4;
-					if(pwrinfo5g->bw20_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo5g->bw20_diff[rfPath][TxCount] |= 0xF0;
-				}
-
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo5g->ofdm_diff[rfPath][TxCount] = 0x4;
-				} else {
-					pwrinfo5g->ofdm_diff[rfPath][0] = (hwinfo[eeAddr] & 0x0f);
-					if(pwrinfo5g->ofdm_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo5g->ofdm_diff[rfPath][TxCount] |= 0xF0;
-				}
-				eeAddr++;
-			} else {
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo5g->bw40_diff[rfPath][TxCount] = 0xFE;
-				} else {
-					pwrinfo5g->bw40_diff[rfPath][TxCount]= (hwinfo[eeAddr] & 0xf0) >> 4;
-					if(pwrinfo5g->bw40_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo5g->bw40_diff[rfPath][TxCount] |= 0xF0;
-				}
-
-				if (hwinfo[eeAddr] == 0xFF) {
-					pwrinfo5g->bw20_diff[rfPath][TxCount] = 0xFE;
-				} else {
-					pwrinfo5g->bw20_diff[rfPath][TxCount] = (hwinfo[eeAddr] & 0x0f);
-					if(pwrinfo5g->bw20_diff[rfPath][TxCount] & BIT(3))
-						pwrinfo5g->bw20_diff[rfPath][TxCount] |= 0xF0;
-				}
-				eeAddr++;
-			}
-		}
-
-		if (hwinfo[eeAddr] == 0xFF) {
-			pwrinfo5g->ofdm_diff[rfPath][1] = 0xFE;
-			pwrinfo5g->ofdm_diff[rfPath][2] = 0xFE;
-		} else {
-			pwrinfo5g->ofdm_diff[rfPath][1] =	(hwinfo[eeAddr] & 0xf0) >> 4;
-			pwrinfo5g->ofdm_diff[rfPath][2] =	(hwinfo[eeAddr] & 0x0f);
-		}
-		eeAddr++;
-		if (hwinfo[eeAddr] == 0xFF)
-			pwrinfo5g->ofdm_diff[rfPath][3] = 0xFE;
-		else
-			pwrinfo5g->ofdm_diff[rfPath][3] = (hwinfo[eeAddr] & 0x0f);
-
-		eeAddr++;
-
-		for (TxCount = 1; TxCount < MAX_TX_COUNT; TxCount++) {
-			if (pwrinfo5g->ofdm_diff[rfPath][TxCount] == 0xFF)
-				pwrinfo5g->ofdm_diff[rfPath][TxCount] = 0xFE;
-			else if(pwrinfo5g->ofdm_diff[rfPath][TxCount] & BIT(3))
-				pwrinfo5g->ofdm_diff[rfPath][TxCount] |= 0xF0;
-		}
-		for (TxCount = 0; TxCount < MAX_TX_COUNT; TxCount++) {
-			if (hwinfo[eeAddr] == 0xFF) {
-				pwrinfo5g->bw80_diff[rfPath][TxCount] = 0xFE;
-			} else {
-				pwrinfo5g->bw80_diff[rfPath][TxCount] =	(hwinfo[eeAddr] & 0xf0) >> 4;
-				if(pwrinfo5g->bw80_diff[rfPath][TxCount] & BIT(3))		//4bit sign number to 8 bit sign number
-					pwrinfo5g->bw80_diff[rfPath][TxCount] |= 0xF0;
-			}
-
-			if (hwinfo[eeAddr] == 0xFF) {
-				pwrinfo5g->bw160_diff[rfPath][TxCount] = 0xFE;
-			} else {
-				pwrinfo5g->bw160_diff[rfPath][TxCount]=	(hwinfo[eeAddr] & 0x0f);
-				if(pwrinfo5g->bw160_diff[rfPath][TxCount] & BIT(3))		//4bit sign number to 8 bit sign number
-					pwrinfo5g->bw160_diff[rfPath][TxCount] |= 0xF0;
-			}
-			eeAddr++;
-		}
-	}
-}
-
-static void _rtl8812ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
-						 bool autoload_fail,
-						 u8 *hwinfo)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct txpower_info_2g pwrinfo24g;
-	struct txpower_info_5g pwrinfo5g;
-	u8 channel5g[CHANNEL_MAX_NUMBER_5G] =
-				 {36,38,40,42,44,46,48,50,52,54,56,58,60,62,64,100,102,104,106,108,110,112,
-                          114,116,118,120,122,124,126,128,130,132,134,136,138,140,142,144,149,151,
-                          153,155,157,159,161,163,165,167,168,169,171,173,175,177};
-	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
-	u8 rf_path, index;
-	u8 i;
-
-	_rtl8821ae_read_power_value_fromprom(hw, &pwrinfo24g, &pwrinfo5g, autoload_fail, hwinfo);
-
-	for (rf_path = 0; rf_path < 2; rf_path++) {
-		for (i = 0; i < CHANNEL_MAX_NUMBER_2G; i++) {
-			index = _rtl8821ae_get_chnl_group(i + 1);
-
-			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
-				rtlefuse->txpwrlevel_cck[rf_path][i] =
-					pwrinfo24g.index_cck_base[rf_path][5];
-				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
-					pwrinfo24g.index_bw40_base[rf_path][index];
-			} else {
-				rtlefuse->txpwrlevel_cck[rf_path][i] =
-					pwrinfo24g.index_cck_base[rf_path][index];
-				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] =
-					pwrinfo24g.index_bw40_base[rf_path][index];
-			}
-		}
-
-		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
-			index = _rtl8821ae_get_chnl_group(channel5g[i]);
-			rtlefuse->txpwr_5g_bw40base[rf_path][i] = pwrinfo5g.index_bw40_base[rf_path][index];
-		}
-		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
-			u8 upper, lower;
-			index = _rtl8821ae_get_chnl_group(channel5g_80m[i]);
-			upper = pwrinfo5g.index_bw40_base[rf_path][index];
-			lower = pwrinfo5g.index_bw40_base[rf_path][index + 1];
-
-			rtlefuse->txpwr_5g_bw80base[rf_path][i] = (upper + lower) / 2;
-		}
-		for (i = 0; i < MAX_TX_COUNT; i++) {
-			rtlefuse->txpwr_cckdiff[rf_path][i] = pwrinfo24g.cck_diff[rf_path][i];
-			rtlefuse->txpwr_legacyhtdiff[rf_path][i] = pwrinfo24g.ofdm_diff[rf_path][i];
-			rtlefuse->txpwr_ht20diff[rf_path][i] = pwrinfo24g.bw20_diff[rf_path][i];
-			rtlefuse->txpwr_ht40diff[rf_path][i] = pwrinfo24g.bw40_diff[rf_path][i];
-
-			rtlefuse->txpwr_5g_ofdmdiff[rf_path][i] = pwrinfo5g.ofdm_diff[rf_path][i];
-			rtlefuse->txpwr_5g_bw20diff[rf_path][i] = pwrinfo5g.bw20_diff[rf_path][i];
-			rtlefuse->txpwr_5g_bw40diff[rf_path][i] = pwrinfo5g.bw40_diff[rf_path][i];
-			rtlefuse->txpwr_5g_bw80diff[rf_path][i] = pwrinfo5g.bw80_diff[rf_path][i];
-		}
-	}
-
-	if (!autoload_fail){
-		rtlefuse->eeprom_regulatory =
-			hwinfo[EEPROM_RF_BOARD_OPTION] & 0x07;/*bit0~2*/
-		if (hwinfo[EEPROM_RF_BOARD_OPTION] == 0xFF)
-			rtlefuse->eeprom_regulatory = 0;
-	} else {
-		rtlefuse->eeprom_regulatory = 0;
-	}
-
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
-	("eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory ));
-}
-
-static void _rtl8821ae_read_txpower_info_from_hwpg(struct ieee80211_hw *hw,
-						 bool autoload_fail,
-						 u8 *hwinfo)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct txpower_info_2g pwrinfo24g;
-	struct txpower_info_5g pwrinfo5g;
-	u8 channel5g[CHANNEL_MAX_NUMBER_5G] =
-				{36,38,40,42,44,46,48,50,52,54,56,58,60,62,64,100,102,104,106,108,110,112,
-                 114,116,118,120,122,124,126,128,130,132,134,136,138,140,142,144,149,151,
-                 153,155,157,159,161,163,165,167,168,169,171,173,175,177};
-	u8 channel5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
-	u8 rf_path, index;
-	u8 i;
-
-	_rtl8821ae_read_power_value_fromprom(hw, &pwrinfo24g, &pwrinfo5g, autoload_fail, hwinfo);
-
-	for (rf_path = 0; rf_path < 2; rf_path++) {
-		for (i = 0; i < CHANNEL_MAX_NUMBER_2G; i++) {
-			index = _rtl8821ae_get_chnl_group(i + 1);
-
-			if (i == CHANNEL_MAX_NUMBER_2G - 1) {
-				rtlefuse->txpwrlevel_cck[rf_path][i] = pwrinfo24g.index_cck_base[rf_path][5];
-				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] = pwrinfo24g.index_bw40_base[rf_path][index];
-			} else {
-				rtlefuse->txpwrlevel_cck[rf_path][i] = pwrinfo24g.index_cck_base[rf_path][index];
-				rtlefuse->txpwrlevel_ht40_1s[rf_path][i] = pwrinfo24g.index_bw40_base[rf_path][index];
-			}
-		}
-
-		for (i = 0; i < CHANNEL_MAX_NUMBER_5G; i++) {
-			index = _rtl8821ae_get_chnl_group(channel5g[i]);
-			rtlefuse->txpwr_5g_bw40base[rf_path][i] = pwrinfo5g.index_bw40_base[rf_path][index];
-		}
-		for (i = 0; i < CHANNEL_MAX_NUMBER_5G_80M; i++) {
-			u8 upper, lower;
-			index = _rtl8821ae_get_chnl_group(channel5g_80m[i]);
-			upper = pwrinfo5g.index_bw40_base[rf_path][index];
-			lower = pwrinfo5g.index_bw40_base[rf_path][index + 1];
-
-			rtlefuse->txpwr_5g_bw80base[rf_path][i] = (upper + lower) / 2;
-		}
-		for (i = 0; i < MAX_TX_COUNT; i++) {
-			rtlefuse->txpwr_cckdiff[rf_path][i] = pwrinfo24g.cck_diff[rf_path][i];
-			rtlefuse->txpwr_legacyhtdiff[rf_path][i] = pwrinfo24g.ofdm_diff[rf_path][i];
-			rtlefuse->txpwr_ht20diff[rf_path][i] = pwrinfo24g.bw20_diff[rf_path][i];
-			rtlefuse->txpwr_ht40diff[rf_path][i] = pwrinfo24g.bw40_diff[rf_path][i];
-
-			rtlefuse->txpwr_5g_ofdmdiff[rf_path][i] = pwrinfo5g.ofdm_diff[rf_path][i];
-			rtlefuse->txpwr_5g_bw20diff[rf_path][i] = pwrinfo5g.bw20_diff[rf_path][i];
-			rtlefuse->txpwr_5g_bw40diff[rf_path][i] = pwrinfo5g.bw40_diff[rf_path][i];
-			rtlefuse->txpwr_5g_bw80diff[rf_path][i] = pwrinfo5g.bw80_diff[rf_path][i];
-		}
-	}
-
-	if (!autoload_fail){
-		rtlefuse->eeprom_regulatory = hwinfo[EEPROM_RF_BOARD_OPTION] & 0x07;/*bit0~2*/
-		if (hwinfo[EEPROM_RF_BOARD_OPTION] == 0xFF)
-			rtlefuse->eeprom_regulatory = 0;
-	} else {
-		rtlefuse->eeprom_regulatory = 0;
-	}
-
-	RTPRINT(rtlpriv, FINIT, INIT_TxPower,
-	("eeprom_regulatory = 0x%x\n", rtlefuse->eeprom_regulatory ));
-}
-
-static void _rtl8812ae_read_adapter_info(struct ieee80211_hw *hw, bool b_pseudo_test )
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u16 i, usvalue;
-	u8 hwinfo[HWSET_MAX_SIZE];
-	u16 eeprom_id;
-
-	if (b_pseudo_test) {
-		/* need add */
-	}
-
-	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
-		rtl_efuse_shadow_map_update(hw);
-		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-		       HWSET_MAX_SIZE);
-	} else if (rtlefuse->epromtype == EEPROM_93C46) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("RTL819X Not boot from eeprom, check it !!"));
-	}
-
-	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP \n"),
-		      hwinfo, HWSET_MAX_SIZE);
-
-	eeprom_id = *((u16 *) & hwinfo[0]);
-	if (eeprom_id != RTL_EEPROM_ID) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("EEPROM ID(%#x) is invalid!!\n", eeprom_id));
-		rtlefuse->autoload_failflag = true;
-	} else {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
-		rtlefuse->autoload_failflag = false;
-	}
-
-	if (rtlefuse->autoload_failflag == true) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("RTL8812AE autoload_failflag, check it !!"));
-		return;
-	}
-
-	rtlefuse->eeprom_version = *(u8 *) & hwinfo[EEPROM_VERSION];
-	if (rtlefuse->eeprom_version == 0xff)
-			rtlefuse->eeprom_version = 0;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM version: 0x%2x\n", rtlefuse->eeprom_version));
-
-	rtlefuse->eeprom_vid = *(u16 *) &hwinfo[EEPROM_VID];
-	rtlefuse->eeprom_did = *(u16 *) &hwinfo[EEPROM_DID];
-	rtlefuse->eeprom_svid = *(u16 *) &hwinfo[EEPROM_SVID];
-	rtlefuse->eeprom_smid = *(u16 *) &hwinfo[EEPROM_SMID];
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROMId = 0x%4x\n", eeprom_id));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid));
-
-	/*customer ID*/
-	rtlefuse->eeprom_oemid = *(u8 *) & hwinfo[EEPROM_CUSTOMER_ID];
-	if (rtlefuse->eeprom_oemid == 0xFF)
-		rtlefuse->eeprom_oemid = 0;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid));
-
-	for (i = 0; i < 6; i += 2) {
-		usvalue = *(u16 *) & hwinfo[EEPROM_MAC_ADDR + i];
-		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
-	}
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("dev_addr: %pM\n", rtlefuse->dev_addr));
-
-	_rtl8812ae_read_txpower_info_from_hwpg(hw,
-		rtlefuse->autoload_failflag, hwinfo);
-
-	/*board type*/
-	rtlefuse->board_type = (((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) & 0xE0 ) >> 5);
-	if ((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) == 0xff )
-		rtlefuse->board_type = 0;
-	rtlhal->boad_type = rtlefuse->board_type;
-
-	rtl8812ae_read_bt_coexist_info_from_hwpg(hw,
-			rtlefuse->autoload_failflag, hwinfo);
-
-	rtlefuse->eeprom_channelplan = *(u8 *) & hwinfo[EEPROM_CHANNELPLAN];
-	if (rtlefuse->eeprom_channelplan == 0xff)
-		rtlefuse->eeprom_channelplan = 0x7F;
-
-	/* set channel plan to world wide 13 */
-	//rtlefuse->channel_plan = (u8) rtlefuse->eeprom_channelplan;
-
-	/*parse xtal*/
-	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_8821AE];
-	if ( rtlefuse->crystalcap == 0xFF )
-		rtlefuse->crystalcap = 0x20;
-
-	rtlefuse->eeprom_thermalmeter = *(u8 *) & hwinfo[EEPROM_THERMAL_METER];
-	if ((rtlefuse->eeprom_thermalmeter == 0xff) ||rtlefuse->autoload_failflag )
-	{
-		rtlefuse->b_apk_thermalmeterignore = true;
-		rtlefuse->eeprom_thermalmeter = 0xff;
-	}
-
-	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter));
-
-	if (rtlefuse->autoload_failflag == false) {
-		rtlefuse->antenna_div_cfg = *(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION] & 0x18 >> 3;
-		if (*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION] == 0xff)
-			rtlefuse->antenna_div_cfg = 0x00;
-		/*if (BT_1ant())
-			rtlefuse->antenna_div_cfg = 0;*/
-		rtlefuse->antenna_div_type = *(u8 *) & hwinfo[EEPROM_RF_ANTENNA_OPT_88E];
-		if (rtlefuse->antenna_div_type == 0xFF)
-		{
-			rtlefuse->antenna_div_type = FIXED_HW_ANTDIV;
-		}
-	} else {
-		rtlefuse->antenna_div_cfg = 0;
-		rtlefuse->antenna_div_type = 0;
-	}
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		("SWAS: bHwAntDiv = %x, TRxAntDivType = %x\n",
-                rtlefuse->antenna_div_cfg, rtlefuse->antenna_div_type));
-
-	/*Hal_ReadPAType_8821A()*/
-	/*Hal_EfuseParseRateIndicationOption8821A()*/
-	/*Hal_ReadEfusePCIeCap8821AE()*/
-
-	pcipriv->ledctl.bled_opendrain = true;
-
-	if (rtlhal->oem_id == RT_CID_DEFAULT) {
-		switch (rtlefuse->eeprom_oemid) {
-		case RT_CID_DEFAULT:
-			break;
-		case EEPROM_CID_TOSHIBA:
-			rtlhal->oem_id = RT_CID_TOSHIBA;
-			break;
-		case EEPROM_CID_CCX:
-			rtlhal->oem_id = RT_CID_CCX;
-			break;
-		case EEPROM_CID_QMI:
-			rtlhal->oem_id = RT_CID_819x_QMI;
-			break;
-		case EEPROM_CID_WHQL:
-			break;
-		default:
-			break;
-
-		}
-	}
-}
-
-static void _rtl8821ae_read_adapter_info(struct ieee80211_hw *hw, bool b_pseudo_test )
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	u16 i, usvalue;
-	u8 hwinfo[HWSET_MAX_SIZE];
-	u16 eeprom_id;
-
-	if (b_pseudo_test) {
-		/* need add */
-	}
-
-	if (rtlefuse->epromtype == EEPROM_BOOT_EFUSE) {
-		rtl_efuse_shadow_map_update(hw);
-		memcpy(hwinfo, &rtlefuse->efuse_map[EFUSE_INIT_MAP][0],
-		       HWSET_MAX_SIZE);
-	} else if (rtlefuse->epromtype == EEPROM_93C46) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("RTL819X Not boot from eeprom, check it !!"));
-	}
-
-	RT_PRINT_DATA(rtlpriv, COMP_INIT, DBG_DMESG, ("MAP \n"),
-		      hwinfo, HWSET_MAX_SIZE);
-
-	eeprom_id = *((u16 *) & hwinfo[0]);
-	if (eeprom_id != RTL_EEPROM_ID) {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("EEPROM ID(%#x) is invalid!!\n", eeprom_id));
-		rtlefuse->autoload_failflag = true;
-	} else {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
-		rtlefuse->autoload_failflag = false;
-	}
-
-	if (rtlefuse->autoload_failflag == true) {
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("RTL8812AE autoload_failflag, check it !!"));
-		return;
-	}
-
-	rtlefuse->eeprom_version = *(u8 *) & hwinfo[EEPROM_VERSION];
-	if (rtlefuse->eeprom_version == 0xff)
-			rtlefuse->eeprom_version = 0;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM version: 0x%2x\n", rtlefuse->eeprom_version));
-
-	rtlefuse->eeprom_vid = *(u16 *) &hwinfo[EEPROM_VID];
-	rtlefuse->eeprom_did = *(u16 *) &hwinfo[EEPROM_DID];
-	rtlefuse->eeprom_svid = *(u16 *) &hwinfo[EEPROM_SVID];
-	rtlefuse->eeprom_smid = *(u16 *) &hwinfo[EEPROM_SMID];
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROMId = 0x%4x\n", eeprom_id));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM VID = 0x%4x\n", rtlefuse->eeprom_vid));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM DID = 0x%4x\n", rtlefuse->eeprom_did));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM SVID = 0x%4x\n", rtlefuse->eeprom_svid));
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM SMID = 0x%4x\n", rtlefuse->eeprom_smid));
-
-	/*customer ID*/
-	rtlefuse->eeprom_oemid = *(u8 *) & hwinfo[EEPROM_CUSTOMER_ID];
-	if (rtlefuse->eeprom_oemid == 0xFF)
-		rtlefuse->eeprom_oemid = 0;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("EEPROM Customer ID: 0x%2x\n", rtlefuse->eeprom_oemid));
-
-	for (i = 0; i < 6; i += 2) {
-		usvalue = *(u16 *) & hwinfo[EEPROM_MAC_ADDR + i];
-		*((u16 *) (&rtlefuse->dev_addr[i])) = usvalue;
-	}
-
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("dev_addr: %pM\n", rtlefuse->dev_addr));
-
-	_rtl8821ae_read_txpower_info_from_hwpg(hw,
-		rtlefuse->autoload_failflag, hwinfo);
-
-	/*board type*/
-	rtlefuse->board_type = (((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) & 0xE0 ) >> 5);
-	if ((*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION]) == 0xff )
-		rtlefuse->board_type = 0;
-	rtlhal->boad_type = rtlefuse->board_type;
-
-	rtl8821ae_read_bt_coexist_info_from_hwpg(hw,
-			rtlefuse->autoload_failflag, hwinfo);
-
-	rtlefuse->eeprom_channelplan = *(u8 *) & hwinfo[EEPROM_CHANNELPLAN];
-	if (rtlefuse->eeprom_channelplan == 0xff)
-		rtlefuse->eeprom_channelplan = 0x7F;
-
-	/* set channel plan to world wide 13 */
-	//rtlefuse->channel_plan = (u8) rtlefuse->eeprom_channelplan;
-
-	/*parse xtal*/
-	rtlefuse->crystalcap = hwinfo[EEPROM_XTAL_8821AE];
-	if ( rtlefuse->crystalcap == 0xFF )
-		rtlefuse->crystalcap = 0x20;
-
-	rtlefuse->eeprom_thermalmeter = *(u8 *) & hwinfo[EEPROM_THERMAL_METER];
-	if ((rtlefuse->eeprom_thermalmeter == 0xff) ||rtlefuse->autoload_failflag )
-	{
-		rtlefuse->b_apk_thermalmeterignore = true;
-		rtlefuse->eeprom_thermalmeter = 0x18;
-	}
-
-	rtlefuse->thermalmeter[0] = rtlefuse->eeprom_thermalmeter;
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("thermalmeter = 0x%x\n", rtlefuse->eeprom_thermalmeter));
-
-	if (rtlefuse->autoload_failflag == false) {
-		rtlefuse->antenna_div_cfg = (*(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION] & BIT(3))?true:false;
-		/*if (BT_1ant())
-			rtlefuse->antenna_div_cfg = 0;*/
-
-		rtlefuse->antenna_div_type = CG_TRX_HW_ANTDIV;
-	} else {
-		rtlefuse->antenna_div_cfg = 0;
-		rtlefuse->antenna_div_type = 0;
-	}
-
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		("SWAS: bHwAntDiv = %x, TRxAntDivType = %x\n",
-                rtlefuse->antenna_div_cfg, rtlefuse->antenna_div_type));
-
-	pcipriv->ledctl.bled_opendrain = true;
-
-	if (rtlhal->oem_id == RT_CID_DEFAULT) {
-		switch (rtlefuse->eeprom_oemid) {
-		case RT_CID_DEFAULT:
-			break;
-		case EEPROM_CID_TOSHIBA:
-			rtlhal->oem_id = RT_CID_TOSHIBA;
-			break;
-		case EEPROM_CID_CCX:
-			rtlhal->oem_id = RT_CID_CCX;
-			break;
-		case EEPROM_CID_QMI:
-			rtlhal->oem_id = RT_CID_819x_QMI;
-			break;
-		case EEPROM_CID_WHQL:
-			break;
-		default:
-			break;
-		}
-	}
-}
-
-
-/*static void _rtl8821ae_hal_customized_behavior(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	pcipriv->ledctl.bled_opendrain = true;
-	switch (rtlhal->oem_id) {
-	case RT_CID_819x_HP:
-		pcipriv->ledctl.bled_opendrain = true;
-		break;
-	case RT_CID_819x_Lenovo:
-	case RT_CID_DEFAULT:
-	case RT_CID_TOSHIBA:
-	case RT_CID_CCX:
-	case RT_CID_819x_Acer:
-	case RT_CID_WHQL:
-	default:
-		break;
-	}
-	RT_TRACE(COMP_INIT, DBG_DMESG,
-		 ("RT Customized ID: 0x%02X\n", rtlhal->oem_id));
-}*/
-
-void rtl8821ae_read_eeprom_info(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 tmp_u1b;
-
-	rtlhal->version = _rtl8821ae_read_chip_version(hw);
-
-	if (get_rf_type(rtlphy) == RF_1T1R)
-		rtlpriv->dm.brfpath_rxenable[0] = true;
-	else
-		rtlpriv->dm.brfpath_rxenable[0] =
-		    rtlpriv->dm.brfpath_rxenable[1] = true;
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("VersionID = 0x%4x\n",
-						rtlhal->version));
-
-	tmp_u1b = rtl_read_byte(rtlpriv, REG_9346CR);
-	if (tmp_u1b & BIT(4)) {
-		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EEPROM\n"));
-		rtlefuse->epromtype = EEPROM_93C46;
-	} else {
-		RT_TRACE(COMP_INIT, DBG_DMESG, ("Boot from EFUSE\n"));
-		rtlefuse->epromtype = EEPROM_BOOT_EFUSE;
-	}
-
-	if (tmp_u1b & BIT(5)) {
-		RT_TRACE(COMP_INIT, DBG_LOUD, ("Autoload OK\n"));
-		rtlefuse->autoload_failflag = false;
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-			_rtl8812ae_read_adapter_info(hw, false);
-		else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-			_rtl8821ae_read_adapter_info(hw, false);
-	} else {
-			RT_TRACE(COMP_ERR, DBG_EMERG, ("Autoload ERR!!\n"));
-	}
-	/*hal_ReadRFType_8812A()*/
-	//_rtl8821ae_hal_customized_behavior(hw);
-}
-
-static void rtl8821ae_update_hal_rate_table(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u32 ratr_value;
-	u8 ratr_index = 0;
-	u8 b_nmode = mac->ht_enable;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
-	u16 shortgi_rate;
-	u32 tmp_ratr_value;
-	u8 b_curtxbw_40mhz = mac->bw_40;
-	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
-				1 : 0;
-	u8 b_curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
-				1 : 0;
-	enum wireless_mode wirelessmode = mac->mode;
-
-	if (rtlhal->current_bandtype == BAND_ON_5G)
-		ratr_value = sta->supp_rates[1] << 4;
-	else
-		ratr_value = sta->supp_rates[0];
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		ratr_value = 0xfff;
-	ratr_value |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
-			sta->ht_cap.mcs.rx_mask[0] << 12);
-	switch (wirelessmode) {
-	case WIRELESS_MODE_B:
-		if (ratr_value & 0x0000000c)
-			ratr_value &= 0x0000000d;
-		else
-			ratr_value &= 0x0000000f;
-		break;
-	case WIRELESS_MODE_G:
-		ratr_value &= 0x00000FF5;
-		break;
-	case WIRELESS_MODE_N_24G:
-	case WIRELESS_MODE_N_5G:
-		b_nmode = 1;
-		if (mimo_ps == IEEE80211_SMPS_STATIC) {
-			ratr_value &= 0x0007F005;
-		} else {
-			u32 ratr_mask;
-
-			if (get_rf_type(rtlphy) == RF_1T2R ||
-			    get_rf_type(rtlphy) == RF_1T1R)
-				ratr_mask = 0x000ff005;
-			else
-				ratr_mask = 0x0f0ff005;
-
-			ratr_value &= ratr_mask;
-		}
-		break;
-	default:
-		if (rtlphy->rf_type == RF_1T2R)
-			ratr_value &= 0x000ff0ff;
-		else
-			ratr_value &= 0x0f0ff0ff;
-
-		break;
-	}
-
-	if ( (rtlpcipriv->btcoexist.bt_coexistence) &&
-	     (rtlpcipriv->btcoexist.bt_coexist_type == BT_CSR_BC4) &&
-	     (rtlpcipriv->btcoexist.bt_cur_state) &&
-	     (rtlpcipriv->btcoexist.bt_ant_isolation) &&
-	     ((rtlpcipriv->btcoexist.bt_service == BT_SCO)||
-	     (rtlpcipriv->btcoexist.bt_service == BT_BUSY)) )
-		ratr_value &= 0x0fffcfc0;
-	else
-		ratr_value &= 0x0FFFFFFF;
-
-	if (b_nmode && ((b_curtxbw_40mhz &&
-			 b_curshortgi_40mhz) || (!b_curtxbw_40mhz &&
-						 b_curshortgi_20mhz))) {
-
-		ratr_value |= 0x10000000;
-		tmp_ratr_value = (ratr_value >> 12);
-
-		for (shortgi_rate = 15; shortgi_rate > 0; shortgi_rate--) {
-			if ((1 << shortgi_rate) & tmp_ratr_value)
-				break;
-		}
-
-		shortgi_rate = (shortgi_rate << 12) | (shortgi_rate << 8) |
-		    (shortgi_rate << 4) | (shortgi_rate);
-	}
-
-	rtl_write_dword(rtlpriv, REG_ARFR0 + ratr_index * 4, ratr_value);
-
-	RT_TRACE(COMP_RATR, DBG_DMESG,
-		 ("%x\n", rtl_read_dword(rtlpriv, REG_ARFR0)));
-}
-
-
-static u8 _rtl8821ae_mrate_idx_to_arfr_id(
-	struct ieee80211_hw *hw, u8 rate_index,
-	enum wireless_mode wirelessmode)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 ret = 0;
-	switch(rate_index){
-		case RATR_INX_WIRELESS_NGB:
-			if(rtlphy->rf_type == RF_1T1R)
-				ret = 1;
-			else
-				ret = 0;
-			;break;
-		case RATR_INX_WIRELESS_N:
-		case RATR_INX_WIRELESS_NG:
-			if(rtlphy->rf_type == RF_1T1R)
-				ret = 5;
-			else
-				ret = 4;
-			;break;
-		case RATR_INX_WIRELESS_NB:
-			if(rtlphy->rf_type == RF_1T1R)
-				ret = 3;
-			else
-				ret = 2;
-			;break;
-		case RATR_INX_WIRELESS_GB:
-			ret = 6;
-			break;
-		case RATR_INX_WIRELESS_G:
-			ret = 7;
-			break;
-		case RATR_INX_WIRELESS_B:
-			ret = 8;
-			break;
-		case RATR_INX_WIRELESS_MC:
-			if ((wirelessmode == WIRELESS_MODE_B)
-				|| (wirelessmode == WIRELESS_MODE_G)
-				|| (wirelessmode == WIRELESS_MODE_N_24G)
-				|| (wirelessmode == WIRELESS_MODE_AC_24G))
-				ret = 6;
-			else
-				ret = 7;
-		case RATR_INX_WIRELESS_AC_5N:
-			if(rtlphy->rf_type == RF_1T1R)
-				ret = 10;
-			else
-				ret = 9;
-			break;
-		case RATR_INX_WIRELESS_AC_24N:
-			if(rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
-			{
-				if(rtlphy->rf_type == RF_1T1R)
-					ret = 10;
-				else
-					ret = 9;
-			} else {
-				if(rtlphy->rf_type == RF_1T1R)
-					ret = 11;
-				else
-					ret = 12;
-			}
-			break;
-		default:
-			ret = 0;break;
-	}
-	return ret;
-}
-
-static void rtl8821ae_update_hal_rate_mask(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u8 rssi_level)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_sta_info * sta_entry = NULL;
-	u32 ratr_bitmap;
-	u8 ratr_index;
-	u8 b_curtxbw_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40)
-				? 1 : 0;
-	u8 b_curshortgi_40mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_40) ?
-				1 : 0;
-	u8 b_curshortgi_20mhz = (sta->ht_cap.cap & IEEE80211_HT_CAP_SGI_20) ?
-				1 : 0;
-	enum wireless_mode wirelessmode = 0;
-	bool b_shortgi = false;
-	u8 rate_mask[7];
-	u8 macid = 0;
-	u8 mimo_ps = IEEE80211_SMPS_OFF;
-
-	sta_entry = (struct rtl_sta_info *) sta->drv_priv;
-	wirelessmode = sta_entry->wireless_mode;
-	if (mac->opmode == NL80211_IFTYPE_STATION ||
-		mac->opmode == NL80211_IFTYPE_MESH_POINT)
-		b_curtxbw_40mhz = mac->bw_40;
-	else if (mac->opmode == NL80211_IFTYPE_AP ||
-		mac->opmode == NL80211_IFTYPE_ADHOC)
-		macid = sta->aid + 1;
-
-	ratr_bitmap = sta->supp_rates[0];
-
-	if (mac->opmode == NL80211_IFTYPE_ADHOC)
-		ratr_bitmap = 0xfff;
-
-	ratr_bitmap |= (sta->ht_cap.mcs.rx_mask[1] << 20 |
-			sta->ht_cap.mcs.rx_mask[0] << 12);
-/*mac id owner*/
-	switch (wirelessmode) {
-	case WIRELESS_MODE_B:
-		ratr_index = RATR_INX_WIRELESS_B;
-		if (ratr_bitmap & 0x0000000c)
-			ratr_bitmap &= 0x0000000d;
-		else
-			ratr_bitmap &= 0x0000000f;
-		break;
-	case WIRELESS_MODE_G:
-		ratr_index = RATR_INX_WIRELESS_GB;
-
-		if (rssi_level == 1)
-			ratr_bitmap &= 0x00000f00;
-		else if (rssi_level == 2)
-			ratr_bitmap &= 0x00000ff0;
-		else
-			ratr_bitmap &= 0x00000ff5;
-		break;
-	case WIRELESS_MODE_A:
-		ratr_index = RATR_INX_WIRELESS_G;
-		ratr_bitmap &= 0x00000ff0;
-		break;
-	case WIRELESS_MODE_N_24G:
-	case WIRELESS_MODE_N_5G:
-		if (wirelessmode == WIRELESS_MODE_N_24G)
-			ratr_index = RATR_INX_WIRELESS_NGB;
-		else
-			ratr_index = RATR_INX_WIRELESS_NG;
-
-		if (mimo_ps == IEEE80211_SMPS_STATIC  || mimo_ps == IEEE80211_SMPS_DYNAMIC) {
-			if (rssi_level == 1)
-				ratr_bitmap &= 0x00070000;
-			else if (rssi_level == 2)
-				ratr_bitmap &= 0x0007f000;
-			else
-				ratr_bitmap &= 0x0007f005;
-		} else {
-			if ( rtlphy->rf_type == RF_1T1R) {
-				if (b_curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x000f0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x000ff000;
-					else
-						ratr_bitmap &= 0x000ff005;
-				}
-			} else {
-				if (b_curtxbw_40mhz) {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0fff0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0ffff000;
-					else
-						ratr_bitmap &= 0x0ffff015;
-				} else {
-					if (rssi_level == 1)
-						ratr_bitmap &= 0x0fff0000;
-					else if (rssi_level == 2)
-						ratr_bitmap &= 0x0ffff000;
-					else
-						ratr_bitmap &= 0x0ffff005;
-				}
-			}
-		}
-		if ((b_curtxbw_40mhz && b_curshortgi_40mhz) ||
-		    (!b_curtxbw_40mhz && b_curshortgi_20mhz)) {
-
-			if (macid == 0)
-				b_shortgi = true;
-			else if (macid == 1)
-				b_shortgi = false;
-		}
-		break;
-
-	case WIRELESS_MODE_AC_24G:
-		ratr_index = RATR_INX_WIRELESS_AC_24N;
-		if(rssi_level == 1)
-			ratr_bitmap &= 0xfc3f0000;
-		else if(rssi_level == 2)
-			ratr_bitmap &= 0xfffff000;
-		else
-			ratr_bitmap &= 0xffffffff;
-		break;
-
-	case WIRELESS_MODE_AC_5G:
-		ratr_index = RATR_INX_WIRELESS_AC_5N;
-
-		if (rtlphy->rf_type == RF_1T1R)
-		{
-			if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-			{
-				if(rssi_level == 1)				/*add by Gary for ac-series*/
-					ratr_bitmap &= 0x003f8000;
-				else if (rssi_level == 2)
-					ratr_bitmap &= 0x003ff000;
-				else
-					ratr_bitmap &= 0x003ff010;
-			}
-			else
-				ratr_bitmap &= 0x000ff010;
-		}
-		else
-		{
-			if(rssi_level == 1)				/* add by Gary for ac-series*/
-				ratr_bitmap &= 0xfe3f8000;       /*VHT 2SS MCS3~9*/
-			else if (rssi_level == 2)
-				ratr_bitmap &= 0xfffff000;       /*VHT 2SS MCS0~9*/
-			else
-				ratr_bitmap &= 0xfffff010;       /*All*/
-		}
-		break;
-
-	default:
-		ratr_index = RATR_INX_WIRELESS_NGB;
-
-		if (rtlphy->rf_type == RF_1T2R)
-			ratr_bitmap &= 0x000ff0ff;
-		else
-			ratr_bitmap &= 0x0f0ff0ff;
-		break;
-
-	}
-
-	sta_entry->ratr_index = ratr_index;
-
-	RT_TRACE(COMP_RATR, DBG_DMESG,
-		 ("ratr_bitmap :%x\n", ratr_bitmap));
-	*(u32 *) & rate_mask = EF4BYTE((ratr_bitmap & 0x0fffffff) |
-				       (ratr_index << 28));
-	rate_mask[0] = macid;
-	rate_mask[1] = _rtl8821ae_mrate_idx_to_arfr_id(hw, ratr_index, wirelessmode) | (b_shortgi ? 0x80 : 0x00);
-	rate_mask[2] = b_curtxbw_40mhz;
-	/* if (prox_priv->proxim_modeinfo->power_output > 0)
-		rate_mask[2] |= BIT(6); */
-
-	rate_mask[3] = (u8)(ratr_bitmap & 0x000000ff);
-	rate_mask[4] = (u8)((ratr_bitmap & 0x0000ff00) >>8);
-	rate_mask[5] = (u8)((ratr_bitmap & 0x00ff0000) >> 16);
-	rate_mask[6] = (u8)((ratr_bitmap & 0xff000000) >> 24);
-
-	RT_TRACE(COMP_RATR, DBG_DMESG, ("Rate_index:%x, "
-						 "ratr_val:%x, %x:%x:%x:%x:%x:%x:%x\n",
-						 ratr_index, ratr_bitmap,
-						 rate_mask[0], rate_mask[1],
-						 rate_mask[2], rate_mask[3],
-						 rate_mask[4], rate_mask[5],
-						 rate_mask[6]));
-	rtl8821ae_fill_h2c_cmd(hw, H2C_8821AE_RA_MASK, 7, rate_mask);
-	_rtl8821ae_set_bcn_ctrl_reg(hw, BIT(3), 0);
-}
-
-void rtl8821ae_update_hal_rate_tbl(struct ieee80211_hw *hw,
-		struct ieee80211_sta *sta, u8 rssi_level)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	if (rtlpriv->dm.b_useramask)
-		rtl8821ae_update_hal_rate_mask(hw, sta, rssi_level);
-	else
-		/*RT_TRACE(COMP_RATR,DBG_LOUD,("rtl8821ae_update_hal_rate_tbl(): Error! 8821ae FW RA Only"));*/
-		rtl8821ae_update_hal_rate_table(hw, sta);
-}
-
-void rtl8821ae_update_channel_access_setting(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	u16 sifs_timer;
-
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SLOT_TIME,
-				      (u8 *) & mac->slot_time);
-	if (!mac->ht_enable)
-		sifs_timer = 0x0a0a;
-	else
-		sifs_timer = 0x0e0e;
-	rtlpriv->cfg->ops->set_hw_reg(hw, HW_VAR_SIFS, (u8 *) & sifs_timer);
-}
-
-bool rtl8821ae_gpio_radio_on_off_checking(struct ieee80211_hw *hw, u8 * valid)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	enum rf_pwrstate e_rfpowerstate_toset, cur_rfstate;
-	u8 u1tmp = 0;
-	bool b_actuallyset = false;
-
-	if (rtlpriv->rtlhal.being_init_adapter)
-		return false;
-
-	if (ppsc->b_swrf_processing)
-		return false;
-
-	spin_lock(&rtlpriv->locks.rf_ps_lock);
-	if (ppsc->rfchange_inprogress) {
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-		return false;
-	} else {
-		ppsc->rfchange_inprogress = true;
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-	}
-
-	cur_rfstate = ppsc->rfpwr_state;
-
-	rtl_write_byte(rtlpriv, REG_GPIO_IO_SEL_2,
-					rtl_read_byte(rtlpriv, REG_GPIO_IO_SEL_2) & ~(BIT(1)));
-
-	u1tmp = rtl_read_byte(rtlpriv, REG_GPIO_PIN_CTRL_2);
-
-	if (rtlphy->polarity_ctl) {
-		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFOFF : ERFON;
-	} else {
-		e_rfpowerstate_toset = (u1tmp & BIT(1)) ? ERFON : ERFOFF;
-	}
-
-	if ((ppsc->b_hwradiooff == true) && (e_rfpowerstate_toset == ERFON)) {
-		RT_TRACE(COMP_RF, DBG_DMESG,
-			 ("GPIOChangeRF  - HW Radio ON, RF ON\n"));
-
-		e_rfpowerstate_toset = ERFON;
-		ppsc->b_hwradiooff = false;
-		b_actuallyset = true;
-	} else if ((ppsc->b_hwradiooff == false)
-		   && (e_rfpowerstate_toset == ERFOFF)) {
-		RT_TRACE(COMP_RF, DBG_DMESG,
-			 ("GPIOChangeRF  - HW Radio OFF, RF OFF\n"));
-
-		e_rfpowerstate_toset = ERFOFF;
-		ppsc->b_hwradiooff = true;
-		b_actuallyset = true;
-	}
-
-	if (b_actuallyset) {
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-		ppsc->rfchange_inprogress = false;
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-	} else {
-		if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC)
-			RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-
-		spin_lock(&rtlpriv->locks.rf_ps_lock);
-		ppsc->rfchange_inprogress = false;
-		spin_unlock(&rtlpriv->locks.rf_ps_lock);
-	}
-
-	*valid = 1;
-	return !ppsc->b_hwradiooff;
-
-}
-
-void rtl8821ae_set_key(struct ieee80211_hw *hw, u32 key_index,
-		     u8 *p_macaddr, bool is_group, u8 enc_algo,
-		     bool is_wepkey, bool clear_all)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 *macaddr = p_macaddr;
-	u32 entry_id = 0;
-	bool is_pairwise = false;
-
-	static u8 cam_const_addr[4][6] = {
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x01},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x02},
-		{0x00, 0x00, 0x00, 0x00, 0x00, 0x03}
-	};
-	static u8 cam_const_broad[] = {
-		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
-	};
-
-	if (clear_all) {
-		u8 idx = 0;
-		u8 cam_offset = 0;
-		u8 clear_number = 5;
-
-		RT_TRACE(COMP_SEC, DBG_DMESG, ("clear_all\n"));
-
-		for (idx = 0; idx < clear_number; idx++) {
-			rtl_cam_mark_invalid(hw, cam_offset + idx);
-			rtl_cam_empty_entry(hw, cam_offset + idx);
-
-			if (idx < 5) {
-				memset(rtlpriv->sec.key_buf[idx], 0,
-				       MAX_KEY_LEN);
-				rtlpriv->sec.key_len[idx] = 0;
-			}
-		}
-
-	} else {
-		switch (enc_algo) {
-		case WEP40_ENCRYPTION:
-			enc_algo = CAM_WEP40;
-			break;
-		case WEP104_ENCRYPTION:
-			enc_algo = CAM_WEP104;
-			break;
-		case TKIP_ENCRYPTION:
-			enc_algo = CAM_TKIP;
-			break;
-		case AESCCMP_ENCRYPTION:
-			enc_algo = CAM_AES;
-			break;
-		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG, ("switch case "
-								"not process \n"));
-			enc_algo = CAM_TKIP;
-			break;
-		}
-
-		if (is_wepkey || rtlpriv->sec.use_defaultkey) {
-			macaddr = cam_const_addr[key_index];
-			entry_id = key_index;
-		} else {
-			if (is_group) {
-				macaddr = cam_const_broad;
-				entry_id = key_index;
-			} else {
-				if (mac->opmode == NL80211_IFTYPE_AP) {
-					entry_id = rtl_cam_get_free_entry(hw, p_macaddr);
-					if (entry_id >=  TOTAL_CAM_ENTRY) {
-						RT_TRACE(COMP_SEC, DBG_EMERG,
-								("Can not find free hw security cam entry\n"));
-						return;
-					}
-				} else {
-					entry_id = CAM_PAIRWISE_KEY_POSITION;
-				}
-
-				key_index = PAIRWISE_KEYIDX;
-				is_pairwise = true;
-			}
-		}
-
-		if (rtlpriv->sec.key_len[key_index] == 0) {
-			RT_TRACE(COMP_SEC, DBG_DMESG,
-				 ("delete one entry, entry_id is %d\n",entry_id));
-			if (mac->opmode == NL80211_IFTYPE_AP)
-				rtl_cam_del_entry(hw, p_macaddr);
-			rtl_cam_delete_one_entry(hw, p_macaddr, entry_id);
-		} else {
-			RT_TRACE(COMP_SEC, DBG_DMESG, ("add one entry\n"));
-			if (is_pairwise) {
-				RT_TRACE(COMP_SEC, DBG_DMESG, ("set Pairwiase key\n"));
-
-				rtl_cam_add_one_entry(hw, macaddr, key_index,
-						      entry_id, enc_algo,
-						      CAM_CONFIG_NO_USEDK,
-						      rtlpriv->sec.key_buf[key_index]);
-			} else {
-				RT_TRACE(COMP_SEC, DBG_DMESG, ("set group key\n"));
-
-				if (mac->opmode == NL80211_IFTYPE_ADHOC) {
-					rtl_cam_add_one_entry(hw,
-							rtlefuse->dev_addr,
-							PAIRWISE_KEYIDX,
-							CAM_PAIRWISE_KEY_POSITION,
-							enc_algo,
-							CAM_CONFIG_NO_USEDK,
-							rtlpriv->sec.key_buf
-							[entry_id]);
-				}
-
-				rtl_cam_add_one_entry(hw, macaddr, key_index,
-						entry_id, enc_algo,
-						CAM_CONFIG_NO_USEDK,
-						rtlpriv->sec.key_buf[entry_id]);
-			}
-
-		}
-	}
-}
-
-
-void rtl8812ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-					      bool auto_load_fail, u8 *hwinfo)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 value;
-
-	if (!auto_load_fail) {
-		value = *(u8 *) & hwinfo[EEPROM_RF_BOARD_OPTION];
-		if (((value & 0xe0) >> 5) == 0x1)
-			rtlpriv->btcoexist.btc_info.btcoexist = 1;
-		else
-			rtlpriv->btcoexist.btc_info.btcoexist = 0;
-		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8812A;
-
-		value = hwinfo[EEPROM_RF_BT_SETTING];
-		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
-	} else {
-		rtlpriv->btcoexist.btc_info.btcoexist = 0;
-		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8812A;
-		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
-	}
-	/*move BT_InitHalVars() to init_sw_vars*/
-}
-
-void rtl8821ae_read_bt_coexist_info_from_hwpg(struct ieee80211_hw *hw,
-					      bool auto_load_fail, u8 *hwinfo)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u8 value;
-	u32 tmpu_32;
-
-	if (!auto_load_fail) {
-		tmpu_32 = rtl_read_dword(rtlpriv, REG_MULTI_FUNC_CTRL);
-		if(tmpu_32 & BIT(18))
-			rtlpriv->btcoexist.btc_info.btcoexist = 1;
-		else
-			rtlpriv->btcoexist.btc_info.btcoexist = 0;
-		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8821A;
-
-		value = hwinfo[EEPROM_RF_BT_SETTING];
-		rtlpriv->btcoexist.btc_info.ant_num = (value & 0x1);
-	} else {
-		rtlpriv->btcoexist.btc_info.btcoexist = 0;
-		rtlpriv->btcoexist.btc_info.bt_type = BT_RTL8821A;
-		rtlpriv->btcoexist.btc_info.ant_num = ANT_X2;
-	}
-	/*move BT_InitHalVars() to init_sw_vars*/
-}
-
-void rtl8821ae_bt_reg_init(struct ieee80211_hw* hw)
-{
-	struct rtl_pci_priv *rtlpcipriv = rtl_pcipriv(hw);
-
-	/* 0:Low, 1:High, 2:From Efuse. */
-	rtlpcipriv->btcoexist.b_reg_bt_iso = 2;
-	/* 0:Idle, 1:None-SCO, 2:SCO, 3:From Counter. */
-	rtlpcipriv->btcoexist.b_reg_bt_sco= 3;
-	/* 0:Disable BT control A-MPDU, 1:Enable BT control A-MPDU. */
-	rtlpcipriv->btcoexist.b_reg_bt_sco= 0;
-}
-
-
-void rtl8821ae_bt_hw_init(struct ieee80211_hw* hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (rtlpriv->cfg->ops->get_btc_status()){
-		rtlpriv->btcoexist.btc_ops->btc_init_hw_config(rtlpriv);
-	}
-}
-
-void rtl8821ae_suspend(struct ieee80211_hw *hw)
-{
-}
-
-void rtl8821ae_resume(struct ieee80211_hw *hw)
-{
-}
-
-/* Turn on AAP (RCR:bit 0) for promicuous mode. */
-void rtl8821ae_allow_all_destaddr(struct ieee80211_hw *hw,
-	bool allow_all_da, bool write_into_reg)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-
-	if (allow_all_da) /* Set BIT0 */
-		rtlpci->receive_config |= RCR_AAP;
-	else /* Clear BIT0 */
-		rtlpci->receive_config &= ~RCR_AAP;
-
-	if(write_into_reg)
-		rtl_write_dword(rtlpriv, REG_RCR, rtlpci->receive_config);
-
-
-	RT_TRACE(COMP_TURBO | COMP_INIT, DBG_LOUD,
-		("receive_config=0x%08X, write_into_reg=%d\n",
-		rtlpci->receive_config, write_into_reg ));
-}
-
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/phy.c b/drivers/staging/rtl8821ae/rtl8821ae/phy.c
deleted file mode 100644
index 1dd33016b4b9..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/phy.c
+++ /dev/null
@@ -1,5525 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "../wifi.h"
-#include "../pci.h"
-#include "../ps.h"
-#include "reg.h"
-#include "def.h"
-#include "phy.h"
-#include "rf.h"
-#include "dm.h"
-#include "table.h"
-#include "trx.h"
-#include "../btcoexist/halbt_precomp.h"
-#include "hw.h"
-
-#define READ_NEXT_PAIR(array_table,v1, v2, i) do { i += 2; v1 = array_table[i]; v2 = array_table[i+1]; } while(0)
-
-static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
-				      enum radio_path rfpath, u32 offset);
-static void _rtl8821ae_phy_rf_serial_write(struct ieee80211_hw *hw,
-					enum radio_path rfpath, u32 offset,
-					u32 data);
-static u32 _rtl8821ae_phy_calculate_bit_shift(u32 bitmask);
-static bool _rtl8821ae_phy_bb8821a_config_parafile(struct ieee80211_hw *hw);
-static bool _rtl8812ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
-static bool _rtl8821ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw);
-static bool _rtl8821ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						u8 configtype);
-static bool _rtl8812ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						u8 configtype);
-static bool _rtl8821ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
-						  u8 configtype);
-static bool _rtl8812ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
-						u8 configtype);
-static void _rtl8821ae_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw);
-
-static long _rtl8821ae_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
-					 enum wireless_mode wirelessmode,
-					 u8 txpwridx);
-static void rtl8821ae_phy_set_rf_on(struct ieee80211_hw *hw);
-static void rtl8821ae_phy_set_io(struct ieee80211_hw *hw);
-
-void rtl8812ae_fixspur(
-	struct ieee80211_hw *hw,
-	enum ht_channel_width band_width,
-	u8 channel
-)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	/*C cut Item12 ADC FIFO CLOCK*/
-	if(IS_VENDOR_8812A_C_CUT(rtlhal->version))
-	{
-		if(band_width == HT_CHANNEL_WIDTH_20_40 && channel == 11)
-			rtl_set_bbreg(hw, RRFMOD, 0xC00, 0x3)	;
-			/* 0x8AC[11:10] = 2'b11*/
-		else
-			rtl_set_bbreg(hw, RRFMOD, 0xC00, 0x2);
-			/* 0x8AC[11:10] = 2'b10*/
-
-
-		/* <20120914, Kordan> A workaround to resolve
-		2480Mhz spur by setting ADC clock as 160M. (Asked by Binson)*/
-		if (band_width == HT_CHANNEL_WIDTH_20 &&
-			(channel == 13 || channel == 14)) {
-			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x3);
-			/*0x8AC[9:8] = 2'b11*/
-			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);
-			/* 0x8C4[30] = 1*/
-		} else if (band_width == HT_CHANNEL_WIDTH_20_40 &&
-			channel == 11) {
-			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);
-			/*0x8C4[30] = 1*/
-		} else if (band_width != HT_CHANNEL_WIDTH_80) {
-			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x2);
-			/*0x8AC[9:8] = 2'b10*/
-			rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
-			/*0x8C4[30] = 0*/
-		}
-	}
-	else if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-	{
-		/* <20120914, Kordan> A workaround to resolve
-		2480Mhz spur by setting ADC clock as 160M. (Asked by Binson)*/
-		if (band_width == HT_CHANNEL_WIDTH_20 &&
-			(channel == 13 || channel == 14))
-			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x3);
-			/*0x8AC[9:8] = 11*/
-		else if (channel  <= 14) /*2.4G only*/
-			rtl_set_bbreg(hw, RRFMOD, 0x300, 0x2);
-			/*0x8AC[9:8] = 10*/
-	}
-
-}
-
-u32 rtl8821ae_phy_query_bb_reg(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 returnvalue, originalvalue, bitshift;
-
-	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), "
-					       "bitmask(%#x)\n", regaddr,
-					       bitmask));
-	originalvalue = rtl_read_dword(rtlpriv, regaddr);
-	bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
-	returnvalue = (originalvalue & bitmask) >> bitshift;
-
-	RT_TRACE(COMP_RF, DBG_TRACE, ("BBR MASK=0x%x "
-					       "Addr[0x%x]=0x%x\n", bitmask,
-					       regaddr, originalvalue));
-
-	return returnvalue;
-
-}
-
-void rtl8821ae_phy_set_bb_reg(struct ieee80211_hw *hw,
-			   u32 regaddr, u32 bitmask, u32 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 originalvalue, bitshift;
-
-	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
-					       " data(%#x)\n", regaddr, bitmask,
-					       data));
-
-	if (bitmask != MASKDWORD) {
-		originalvalue = rtl_read_dword(rtlpriv, regaddr);
-		bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
-		data = ((originalvalue & (~bitmask)) | ((data << bitshift) & bitmask));
-	}
-
-	rtl_write_dword(rtlpriv, regaddr, data);
-
-	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), bitmask(%#x),"
-					       " data(%#x)\n", regaddr, bitmask,
-					       data));
-
-}
-
-u32 rtl8821ae_phy_query_rf_reg(struct ieee80211_hw *hw,
-			    enum radio_path rfpath, u32 regaddr, u32 bitmask)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 original_value, readback_value, bitshift;
-	unsigned long flags;
-
-	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), "
-					       "rfpath(%#x), bitmask(%#x)\n",
-					       regaddr, rfpath, bitmask));
-
-	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
-
-
-	original_value = _rtl8821ae_phy_rf_serial_read(hw,rfpath, regaddr);
-	bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
-	readback_value = (original_value & bitmask) >> bitshift;
-
-	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
-
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), rfpath(%#x), "
-		  "bitmask(%#x), original_value(%#x)\n",
-		  regaddr, rfpath, bitmask, original_value));
-
-	return readback_value;
-}
-
-void rtl8821ae_phy_set_rf_reg(struct ieee80211_hw *hw,
-			   enum radio_path rfpath,
-			   u32 regaddr, u32 bitmask, u32 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 original_value, bitshift;
-	unsigned long flags;
-
-	RT_TRACE(COMP_RF, DBG_TRACE,
-		 ("regaddr(%#x), bitmask(%#x), data(%#x), rfpath(%#x)\n",
-		  regaddr, bitmask, data, rfpath));
-
-	spin_lock_irqsave(&rtlpriv->locks.rf_lock, flags);
-
-	if (bitmask != RFREG_OFFSET_MASK) {
-			original_value = _rtl8821ae_phy_rf_serial_read(hw,
-								    rfpath,
-								    regaddr);
-			bitshift = _rtl8821ae_phy_calculate_bit_shift(bitmask);
-			data =
-			    ((original_value & (~bitmask)) |
-			     (data << bitshift));
-		}
-
-	_rtl8821ae_phy_rf_serial_write(hw, rfpath, regaddr, data);
-
-
-	spin_unlock_irqrestore(&rtlpriv->locks.rf_lock, flags);
-
-	RT_TRACE(COMP_RF, DBG_TRACE, ("regaddr(%#x), "
-					       "bitmask(%#x), data(%#x), rfpath(%#x)\n",
-					       regaddr, bitmask, data, rfpath));
-
-}
-
-static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
-				      enum radio_path rfpath, u32 offset)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool b_is_pi_mode =false;
-	u32 retvalue = 0;
-
-	/* 2009/06/17 MH We can not execute IO for power save or other accident mode.*/
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("return all one\n"));
-		return 0xFFFFFFFF;
-	}
-
-	/* <20120809, Kordan> CCA OFF(when entering), asked by James to avoid reading the wrong value.
-	    <20120828, Kordan> Toggling CCA would affect RF 0x0, skip it!*/
-	if (offset != 0x0 &&
-		!((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-		|| (IS_VENDOR_8812A_C_CUT(rtlhal->version))))
-		rtl_set_bbreg(hw, RCCAONSEC, 0x8, 1);
-
-	offset &= 0xff;
-
-	if (rfpath == RF90_PATH_A)
-		b_is_pi_mode = (bool) rtl_get_bbreg(hw, 0xC00, 0x4);
-	else if (rfpath == RF90_PATH_B)
-		b_is_pi_mode = (bool) rtl_get_bbreg(hw, 0xE00, 0x4);
-
-	rtl_set_bbreg(hw, RHSSIREAD_8821AE, 0xff, offset);
-
-	if ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-		|| (IS_VENDOR_8812A_C_CUT(rtlhal->version)))
-		udelay(20);
-
-	if (b_is_pi_mode)
-	{
-		if (rfpath == RF90_PATH_A) {
-			retvalue = rtl_get_bbreg(hw, RA_PIREAD_8821A, BLSSIREADBACKDATA);
-		}
-		else if (rfpath == RF90_PATH_B){
-			retvalue = rtl_get_bbreg(hw, RB_PIREAD_8821A, BLSSIREADBACKDATA);
-		}
-	}
-	else
-	{
-		if (rfpath == RF90_PATH_A) {
-			retvalue = rtl_get_bbreg(hw, RA_SIREAD_8821A, BLSSIREADBACKDATA);
-		}
-		else if (rfpath == RF90_PATH_B){
-			retvalue = rtl_get_bbreg(hw, RB_SIREAD_8821A, BLSSIREADBACKDATA);
-		}
-	}
-
-	/*<20120809, Kordan> CCA ON(when exiting), asked by James to avoid reading the wrong value.
-	    <20120828, Kordan> Toggling CCA would affect RF 0x0, skip it!*/
-	if (offset != 0x0 &&  ! ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-		|| (IS_VENDOR_8812A_C_CUT(rtlhal->version))))
-		rtl_set_bbreg(hw, RCCAONSEC, 0x8, 0);
-	return retvalue;
-}
-
-#if 0
-static u32 _rtl8821ae_phy_rf_serial_read(struct ieee80211_hw *hw,
-				      enum radio_path rfpath, u32 offset)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
-	u32 newoffset;
-	u32 tmplong, tmplong2;
-	u8 rfpi_enable = 0;
-	u32 retvalue;
-
-	offset &= 0xff;
-	newoffset = offset;
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("return all one\n"));
-		return 0xFFFFFFFF;
-	}
-	tmplong = rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD);
-	if (rfpath == RF90_PATH_A)
-		tmplong2 = tmplong;
-	else
-		tmplong2 = rtl_get_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD);
-	tmplong2 = (tmplong2 & (~BLSSIREADADDRESS)) |
-	    (newoffset << 23) | BLSSIREADEDGE;
-	rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
-		      tmplong & (~BLSSIREADEDGE));
-	mdelay(1);
-	rtl_set_bbreg(hw, pphyreg->rfhssi_para2, MASKDWORD, tmplong2);
-	mdelay(1);
-	/*rtl_set_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2, MASKDWORD,
-		      tmplong | BLSSIREADEDGE);*/
-	mdelay(1);
-	if (rfpath == RF90_PATH_A)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER1,
-						 BIT(8));
-	else if (rfpath == RF90_PATH_B)
-		rfpi_enable = (u8) rtl_get_bbreg(hw, RFPGA0_XB_HSSIPARAMETER1,
-						 BIT(8));
-	if (rfpi_enable)
-		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readbackpi,
-					 BLSSIREADBACKDATA);
-	else
-		retvalue = rtl_get_bbreg(hw, pphyreg->rflssi_readback,
-					 BLSSIREADBACKDATA);
-	RT_TRACE(COMP_RF, DBG_TRACE, ("RFR-%d Addr[0x%x]=0x%x\n",
-					       rfpath, pphyreg->rflssi_readback,
-					       retvalue));
-	return retvalue;
-}
-#endif
-
-static void _rtl8821ae_phy_rf_serial_write(struct ieee80211_hw *hw,
-					enum radio_path rfpath, u32 offset,
-					u32 data)
-{
-	u32 data_and_addr;
-	u32 newoffset;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct bb_reg_def *pphyreg = &rtlphy->phyreg_def[rfpath];
-
-	if (RT_CANNOT_IO(hw)) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("stop\n"));
-		return;
-	}
-	offset &= 0xff;
-	newoffset = offset;
-	data_and_addr = ((newoffset << 20) | (data & 0x000fffff)) & 0x0fffffff;
-	rtl_set_bbreg(hw, pphyreg->rf3wire_offset, MASKDWORD, data_and_addr);
-	RT_TRACE(COMP_RF, DBG_TRACE, ("RFW-%d Addr[0x%x]=0x%x\n",
-					       rfpath, pphyreg->rf3wire_offset,
-					       data_and_addr));
-}
-
-static u32 _rtl8821ae_phy_calculate_bit_shift(u32 bitmask)
-{
-	u32 i;
-
-	for (i = 0; i <= 31; i++) {
-		if (((bitmask >> i) & 0x1) == 1)
-			break;
-	}
-	return i;
-}
-
-bool rtl8821ae_phy_mac_config(struct ieee80211_hw *hw)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool rtstatus = 0;
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-		rtstatus = _rtl8812ae_phy_config_mac_with_headerfile(hw);
-	else
-		rtstatus = _rtl8821ae_phy_config_mac_with_headerfile(hw);
-
-	return rtstatus;
-}
-
-bool rtl8821ae_phy_bb_config(struct ieee80211_hw *hw)
-{
-	bool rtstatus = true;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 regval;
-	u8 crystal_cap;
-	//u32 tmp;
-
-	_rtl8821ae_phy_init_bb_rf_register_definition(hw);
-
-	regval = rtl_read_byte(rtlpriv, REG_SYS_FUNC_EN);
-	regval |= regval | FEN_PCIEA;
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, regval);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN,
-				regval | FEN_BB_GLB_RSTN | FEN_BBRSTB);
-
-	rtl_write_byte(rtlpriv, REG_RF_CTRL, 0x7);/*RF_EN | RF_RSTB | RF_SDMRSTB*/
-	rtl_write_byte(rtlpriv, REG_OPT_CTRL + 2, 0x7);/*RF_EN | RF_RSTB | RF_SDMRSTB*/
-
-	rtstatus = _rtl8821ae_phy_bb8821a_config_parafile(hw);
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-	{
-		crystal_cap = rtlefuse->crystalcap & 0x3F;
-		rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0x7FF80000, (crystal_cap | (crystal_cap << 6)));
-	}else{
-		crystal_cap = rtlefuse->crystalcap & 0x3F;
-		rtl_set_bbreg(hw, REG_MAC_PHY_CTRL, 0xFFF000, (crystal_cap | (crystal_cap << 6)));
-	}
-	rtlphy->reg_837 = rtl_read_byte(rtlpriv, 0x837);
-
-	return rtstatus;
-}
-
-bool rtl8821ae_phy_rf_config(struct ieee80211_hw *hw)
-{
-	return rtl8821ae_phy_rf6052_config(hw);
-}
-
-
-u32 phy_get_tx_bb_swing_8812A(
-	struct ieee80211_hw *hw,
-	u8 	band,
-	u8 	rf_path
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-
-	char bb_swing_2g = (char) ((-1 * 0xFF) & 0xFF);
-	char bb_swing_5g = (char) ((-1 * 0xFF) & 0xFF);
-	u32  out = 0x200;
-	const char auto_temp = -1;
-
-	RT_TRACE(COMP_SCAN, DBG_LOUD,
-		("===> PHY_GetTxBBSwing_8812A, bbSwing_2G: %d, bbSwing_5G: %d\n",
-		(int)bb_swing_2g, (int)bb_swing_5g));
-
-    if ( rtlefuse->autoload_failflag) {
-        	if ( band == BAND_ON_2_4G ) {
-			rtldm->bb_swing_diff_2g = bb_swing_2g;
-			if      (bb_swing_2g == 0)  out = 0x200; //  0 dB
-			else if (bb_swing_2g == -3) out = 0x16A; // -3 dB
-			else if (bb_swing_2g == -6) out = 0x101; // -6 dB
-			else if (bb_swing_2g == -9) out = 0x0B6; // -9 dB
-			else {
-			        rtldm->bb_swing_diff_2g = 0;
-			        out = 0x200;
-			}
-
-		} else if ( band == BAND_ON_5G ) {
-			rtldm->bb_swing_diff_5g = bb_swing_5g;
-			if      (bb_swing_5g == 0)  out = 0x200; //  0 dB
-			else if (bb_swing_5g == -3) out = 0x16A; // -3 dB
-			else if (bb_swing_5g == -6) out = 0x101; // -6 dB
-			else if (bb_swing_5g == -9) out = 0x0B6; // -9 dB
-			else {
-				if ( rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-					rtldm->bb_swing_diff_5g = -3;
-					out = 0x16A;
-				} else  {
-					rtldm->bb_swing_diff_5g = 0;
-					out = 0x200;
-				}
-			}
-		} else  {
-				rtldm->bb_swing_diff_2g = -3;
-				rtldm->bb_swing_diff_5g = -3;
-				out = 0x16A; // -3 dB
-		}
-	}
-	else
-	{
-	    u32 swing = 0, swing_a = 0, swing_b = 0;
-
-	    if (band == BAND_ON_2_4G)
-	    {
-			if (0xFF == auto_temp)
-			{
-				efuse_shadow_read(hw, 1, 0xC6, (u32 *)&swing);
-				swing = (swing == 0xFF) ? 0x00 : swing;
-			}
-			else if (bb_swing_2g ==  0) swing = 0x00; //  0 dB
-			else if (bb_swing_2g == -3) swing = 0x05; // -3 dB
-			else if (bb_swing_2g == -6) swing = 0x0A; // -6 dB
-			else if (bb_swing_2g == -9) swing = 0xFF; // -9 dB
-			else swing = 0x00;
-		}
-		else
-		{
-			if (0xFF == auto_temp)
-			{
-				efuse_shadow_read(hw, 1, 0xC7, (u32 *)&swing);
-				swing = (swing == 0xFF) ? 0x00 : swing;
-			}
-			else if (bb_swing_5g ==  0) swing = 0x00; //  0 dB
-			else if (bb_swing_5g == -3) swing = 0x05; // -3 dB
-			else if (bb_swing_5g == -6) swing = 0x0A; // -6 dB
-			else if (bb_swing_5g == -9) swing = 0xFF; // -9 dB
-			else swing = 0x00;
-		}
-
-		swing_a = (swing & 0x3) >> 0; // 0xC6/C7[1:0]
-		swing_b = (swing & 0xC) >> 2; // 0xC6/C7[3:2]
-		RT_TRACE(COMP_SCAN, DBG_LOUD,
-		("===> PHY_GetTxBBSwing_8812A, swingA: 0x%X, swingB: 0x%X\n",
-		swing_a, swing_b));
-
-		//3 Path-A
-		if (swing_a == 0x0) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = 0;
-			else
-				rtldm->bb_swing_diff_5g = 0;
-			out = 0x200; // 0 dB
-		} else if (swing_a == 0x1) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = -3;
-			else
-				rtldm->bb_swing_diff_5g = -3;
-			out = 0x16A; // -3 dB
-		} else if (swing_a == 0x2) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = -6;
-			else
-				rtldm->bb_swing_diff_5g = -6;
-			out = 0x101; // -6 dB
-		} else if (swing_a == 0x3) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = -9;
-			else
-				rtldm->bb_swing_diff_5g = -9;
-			out = 0x0B6; // -9 dB
-		}
-
-			//3 Path-B
-		if (swing_b == 0x0) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = 0;
-			else
-				rtldm->bb_swing_diff_5g = 0;
-			out = 0x200; // 0 dB
-		} else if (swing_b == 0x1) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = -3;
-			else
-				rtldm->bb_swing_diff_5g = -3;
-			out = 0x16A; // -3 dB
-		} else if (swing_b == 0x2) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = -6;
-			else
-				rtldm->bb_swing_diff_5g = -6;
-			out = 0x101; // -6 dB
-		} else if (swing_b == 0x3) {
-			if (band == BAND_ON_2_4G)
-				rtldm->bb_swing_diff_2g = -9;
-			else
-				rtldm->bb_swing_diff_5g = -9;
-			out = 0x0B6; // -9 dB
-		}
-	}
-
-	RT_TRACE(COMP_SCAN, DBG_LOUD,
-		("<=== PHY_GetTxBBSwing_8812A, out = 0x%X\n", out));
-	 return out;
-}
-void rtl8821ae_phy_switch_wirelessband(struct ieee80211_hw *hw, u8 band)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_dm *rtldm = rtl_dm(rtlpriv);
-	u8 current_band = rtlhal->current_bandtype;
-	u32 txpath, rxpath;
-	//u8 i, value8;
-	char bb_diff_between_band;
-
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("\n"));
-	txpath = rtl8821ae_phy_query_bb_reg(hw, RTXPATH, 0xf0);
-	rxpath = rtl8821ae_phy_query_bb_reg(hw, RCCK_RX, 0x0f000000);
-	rtlhal->current_bandtype = (enum band_type) band;
-	/* reconfig BB/RF according to wireless mode */
-	if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-		/* BB & RF Config */
-		RT_TRACE(COMP_CMD, DBG_DMESG, ("2.4G\n"));
-		rtl_set_bbreg(hw, ROFDMCCKEN, BOFDMEN|BCCKEN, 0x03);
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-			/* 0xCB0[15:12] = 0x7 (LNA_On)*/
-			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF000, 0x7);
-			/* 0xCB0[7:4] = 0x7 (PAPE_A)*/
-			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF0, 0x7);
-		}
-
-		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-			rtl_set_bbreg(hw, 0x830, 0xE, 0x4);	/*0x830[3:1] = 0x4*/
-			rtl_set_bbreg(hw, 0x834, 0x3, 0x1);	/*0x834[1:0] = 0x1*/
-		}
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE)
-			rtl_set_bbreg(hw, RA_TXSCALE, 0xF00, 0); // 0xC1C[11:8] = 0
-		else
-			rtl_set_bbreg(hw, 0x82c, 0x3, 0);	// 0x82C[1:0] = 2b'00
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-			rtl_set_bbreg(hw, RA_RFE_PINMUX, BMASKDWORD, 0x77777777);
-			rtl_set_bbreg(hw, RB_RFE_PINMUX, BMASKDWORD, 0x77777777);
-			rtl_set_bbreg(hw, RA_RFE_INV, 0x3ff00000, 0x000);
-			rtl_set_bbreg(hw, RB_RFE_INV, 0x3ff00000, 0x000);
-		}
-
-		rtl_set_bbreg(hw, RTXPATH, 0xf0, txpath);
-		rtl_set_bbreg(hw, RCCK_RX, 0x0f000000, rxpath);
-
-		rtl_write_byte(rtlpriv, REG_CCK_CHECK, 0x0);
-	} else {/* 5G band */
-		u16 count, reg_41a;
-		RT_TRACE(COMP_CMD, DBG_DMESG, ("5G\n"));
-
-		if(rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-			/*0xCB0[15:12] = 0x5 (LNA_On)*/
-			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF000, 0x5);
-			/*0xCB0[7:4] = 0x4 (PAPE_A)*/
-			rtl_set_bbreg(hw, RA_RFE_PINMUX, 0xF0, 0x4);
-		}
-		/*CCK_CHECK_en*/
-		rtl_write_byte(rtlpriv, REG_CCK_CHECK, 0x80);
-
-		count = 0;
-		reg_41a = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
-		RT_TRACE(COMP_SCAN, DBG_LOUD, ("Reg41A value %d", reg_41a));
-		reg_41a &= 0x30;
-		while ((reg_41a!= 0x30) && (count < 50)) {
-			udelay(50);
-			RT_TRACE(COMP_SCAN, DBG_LOUD, ("Delay 50us \n"));
-
-			reg_41a = rtl_read_word(rtlpriv, REG_TXPKT_EMPTY);
-			reg_41a &= 0x30;
-			count++;
-			RT_TRACE(COMP_SCAN, DBG_LOUD, ("Reg41A value %d", reg_41a));
-		}
-		if (count != 0)
-			RT_TRACE(COMP_MLME, DBG_LOUD,
-			("PHY_SwitchWirelessBand8812(): Switch to 5G Band. "
-			"Count = %d reg41A=0x%x\n", count, reg_41a));
-
-		// 2012/02/01, Sinda add registry to switch workaround without long-run verification for scan issue.
-		rtl_set_bbreg(hw, ROFDMCCKEN, BOFDMEN|BCCKEN, 0x03);
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-			rtl_set_bbreg(hw, 0x830, 0xE, 0x3);	/*0x830[3:1] = 0x3*/
-			rtl_set_bbreg(hw, 0x834, 0x3, 0x2);	/*0x834[1:0] = 0x2*/
-		}
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-			/* AGC table select */
-			rtl_set_bbreg(hw, RA_TXSCALE, 0xF00, 1); /* 0xC1C[11:8] = 1*/
-		} else
-			rtl_set_bbreg(hw, 0x82c, 0x3, 1);	// 0x82C[1:0] = 2'b00
-
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE) {
-			rtl_set_bbreg(hw, RA_RFE_PINMUX, BMASKDWORD, 0x77337777);
-			rtl_set_bbreg(hw, RB_RFE_PINMUX, BMASKDWORD, 0x77337777);
-			rtl_set_bbreg(hw, RA_RFE_INV, 0x3ff00000, 0x010);
-			rtl_set_bbreg(hw, RB_RFE_INV, 0x3ff00000, 0x010);
-		}
-
-		rtl_set_bbreg(hw, RTXPATH, 0xf0, txpath);
-		rtl_set_bbreg(hw, RCCK_RX, 0x0f000000, rxpath);
-
-		RT_TRACE(COMP_SCAN, DBG_LOUD,
-			("==>PHY_SwitchWirelessBand8812() BAND_ON_5G settings OFDM index 0x%x\n",
-			rtlpriv->dm.ofdm_index[RF90_PATH_A]));
-	}
-
-	if ((rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) ||
-			(rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)) {
-		rtl_set_bbreg(hw, RA_TXSCALE, 0xFFE00000,
-						 phy_get_tx_bb_swing_8812A(hw, band, RF90_PATH_A)); // 0xC1C[31:21]
-		rtl_set_bbreg(hw, RB_TXSCALE, 0xFFE00000,
-						 phy_get_tx_bb_swing_8812A(hw, band, RF90_PATH_B)); // 0xE1C[31:21]
-
-		/* <20121005, Kordan> When TxPowerTrack is ON, we should take care of the change of BB swing.
-		    That is, reset all info to trigger Tx power tracking.*/
-		if (band != current_band) {
-			bb_diff_between_band = (rtldm->bb_swing_diff_2g - rtldm->bb_swing_diff_5g);
-			bb_diff_between_band = (band == BAND_ON_2_4G) ? bb_diff_between_band : (-1 * bb_diff_between_band);
-			rtldm->default_ofdm_index += bb_diff_between_band * 2;
-		}
-		rtl8821ae_dm_clear_txpower_tracking_state(hw);
-	}
-
-	RT_TRACE(COMP_SCAN, DBG_TRACE,
-		("<==rtl8821ae_phy_switch_wirelessband():Switch Band OK.\n"));
-	return;
-}
-
-static bool _rtl8821ae_check_condition(struct ieee80211_hw *hw,
-	const u32  Condition
-	)
-{
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u32 _board = rtlefuse->board_type; /*need efuse define*/
-	u32 _interface = rtlhal->interface;
-	u32 _platform = 0x08;/*SupportPlatform */
-	u32 cond = Condition;
-
-	if ( Condition == 0xCDCDCDCD )
-		return true;
-
-	cond = Condition & 0xFF;
-	if ( (_board != cond) == 0 && cond != 0xFF)
-		return false;
-
-	cond = Condition & 0xFF00;
-	cond = cond >> 8;
-	if ( (_interface & cond) == 0 && cond != 0x07)
-		return false;
-
-	cond = Condition & 0xFF0000;
-	cond = cond >> 16;
-	if ( (_platform & cond) == 0 && cond != 0x0F)
-		return false;
-	return true;
-}
-
-static void _rtl8821ae_config_rf_reg(struct ieee80211_hw *hw,
-	u32 addr,
-	u32 data,
-	enum radio_path rfpath,
-	u32 regaddr
-	)
-{
-	if ( addr == 0xfe || addr == 0xffe) {
-		mdelay(50);
-	} else {
-		rtl_set_rfreg(hw, rfpath, regaddr, RFREG_OFFSET_MASK, data);
-		udelay(1);
-	}
-}
-
-static void _rtl8821ae_config_rf_radio_a(struct ieee80211_hw *hw,
-	u32 addr, u32 data)
-{
-	u32 content = 0x1000; /*RF Content: radio_a_txt*/
-	u32 maskforphyset = (u32)(content & 0xE000);
-
-	_rtl8821ae_config_rf_reg(hw, addr, data, RF90_PATH_A, addr | maskforphyset);
-
-}
-
-static void _rtl8821ae_config_rf_radio_b(struct ieee80211_hw *hw,
-	u32 addr, u32 data)
-{
-	u32 content = 0x1001; /*RF Content: radio_b_txt*/
-	u32 maskforphyset = (u32)(content & 0xE000);
-
-	_rtl8821ae_config_rf_reg(hw, addr, data, RF90_PATH_B, addr | maskforphyset);
-
-}
-
-static void _rtl8812ae_config_bb_reg(struct ieee80211_hw *hw,
-	u32 addr, u32 data)
-{
-	if ( addr == 0xfe) {
-		mdelay(50);
-	} else if ( addr == 0xfd)
-		mdelay(5);
-	else if ( addr == 0xfc)
-		mdelay(1);
-	else if ( addr == 0xfb)
-		udelay(50);
-	else if ( addr == 0xfa)
-		udelay(5);
-	else if ( addr == 0xf9)
-		udelay(1);
-	else {
-		rtl_set_bbreg(hw, addr, MASKDWORD,data);
-	}
-	udelay(1);
-}
-
-static void _rtl8821ae_config_bb_reg(struct ieee80211_hw *hw,
-	u32 addr, u32 data)
-{
-	if ( addr == 0xfe) {
-		mdelay(50);
-	} else if ( addr == 0xfd)
-		mdelay(5);
-	else if ( addr == 0xfc)
-		mdelay(1);
-	else if ( addr == 0xfb)
-		udelay(50);
-	else if ( addr == 0xfa)
-		udelay(5);
-	else if ( addr == 0xf9)
-		udelay(1);
-
-	rtl_set_bbreg(hw, addr, MASKDWORD,data);
-	udelay(1);
-}
-
-static void _rtl8821ae_phy_init_tx_power_by_rate(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	u8 band, rfpath, txnum, rate_section;
-
-	for ( band = BAND_ON_2_4G; band <= BAND_ON_5G; ++band )
-		for ( rfpath = 0; rfpath < TX_PWR_BY_RATE_NUM_RF; ++rfpath )
-			for ( txnum = 0; txnum < TX_PWR_BY_RATE_NUM_RF; ++txnum )
-				for ( rate_section = 0; rate_section < TX_PWR_BY_RATE_NUM_SECTION; ++rate_section )
-					rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] = 0;
-}
-
-void _rtl8821ae_phy_set_txpower_by_rate_base(struct ieee80211_hw *hw,
-														   u8 band, u8 path,
-														   u8 rate_section,
-														   u8 txnum, u8 value)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (path > RF90_PATH_D) {
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			("Invalid Rf Path %d in phy_SetTxPowerByRatBase()\n", path));
-		return;
-	}
-
-	if (band == BAND_ON_2_4G) {
-		switch (rate_section) {
-		case CCK:
-			rtlphy->txpwr_by_rate_base_24g[path][txnum][0] = value;
-			break;
-		case OFDM:
-			rtlphy->txpwr_by_rate_base_24g[path][txnum][1] = value;
-			break;
-		case HT_MCS0_MCS7:
-			rtlphy->txpwr_by_rate_base_24g[path][txnum][2] = value;
-			break;
-		case HT_MCS8_MCS15:
-			rtlphy->txpwr_by_rate_base_24g[path][txnum][3] = value;
-			break;
-		case VHT_1SSMCS0_1SSMCS9:
-			rtlphy->txpwr_by_rate_base_24g[path][txnum][4] = value;
-			break;
-		case VHT_2SSMCS0_2SSMCS9:
-			rtlphy->txpwr_by_rate_base_24g[path][txnum][5] = value;
-			break;
-		default:
-			RT_TRACE(COMP_INIT, DBG_LOUD, ( "Invalid RateSection %d in Band 2.4G, Rf Path %d, %dTx in PHY_SetTxPowerByRateBase()\n",
-					 rate_section, path, txnum ) );
-			break;
-		};
-	} else if (band == BAND_ON_5G) {
-		switch (rate_section) {
-		case OFDM:
-			rtlphy->txpwr_by_rate_base_5g[path][txnum][0] = value;
-			break;
-		case HT_MCS0_MCS7:
-			rtlphy->txpwr_by_rate_base_5g[path][txnum][1] = value;
-			break;
-		case HT_MCS8_MCS15:
-			rtlphy->txpwr_by_rate_base_5g[path][txnum][2] = value;
-			break;
-		case VHT_1SSMCS0_1SSMCS9:
-			rtlphy->txpwr_by_rate_base_5g[path][txnum][3] = value;
-			break;
-		case VHT_2SSMCS0_2SSMCS9:
-			rtlphy->txpwr_by_rate_base_5g[path][txnum][4] = value;
-			break;
-		default:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Invalid RateSection %d in Band 5G, Rf Path %d, "
-				"%dTx in PHY_SetTxPowerByRateBase()\n",
-				rate_section, path, txnum));
-			break;
-		};
-	} else {
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			("Invalid Band %d in PHY_SetTxPowerByRateBase()\n", band));
-	}
-
-}
-
-u8 _rtl8821ae_phy_get_txpower_by_rate_base(struct ieee80211_hw *hw,
-														 u8 band, u8 path,
-														 u8 txnum, u8 rate_section)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 value = 0;
-
-	if (path > RF90_PATH_D) {
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			("Invalid Rf Path %d in PHY_GetTxPowerByRateBase()\n", path));
-		return 0;
-	}
-
-	if (band == BAND_ON_2_4G) {
-		switch (rate_section) {
-		case CCK:
-			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][0];
-			break;
-		case OFDM:
-			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][1];
-			break;
-		case HT_MCS0_MCS7:
-			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][2];
-			break;
-		case HT_MCS8_MCS15:
-			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][3];
-			break;
-		case VHT_1SSMCS0_1SSMCS9:
-			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][4];
-			break;
-		case VHT_2SSMCS0_2SSMCS9:
-			value = rtlphy->txpwr_by_rate_base_24g[path][txnum][5];
-			break;
-		default:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Invalid RateSection %d in Band 2.4G, Rf Path %d,"
-				" %dTx in PHY_GetTxPowerByRateBase()\n",
-				rate_section, path, txnum));
-			break;
-		};
-	} else if (band == BAND_ON_5G) {
-		switch (rate_section) {
-		case OFDM:
-			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][0];
-			break;
-		case HT_MCS0_MCS7:
-			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][1];
-			break;
-		case HT_MCS8_MCS15:
-			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][2];
-			break;
-		case VHT_1SSMCS0_1SSMCS9:
-			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][3];
-			break;
-		case VHT_2SSMCS0_2SSMCS9:
-			value = rtlphy->txpwr_by_rate_base_5g[path][txnum][4];
-			break;
-		default:
-			RT_TRACE(COMP_INIT, DBG_LOUD,
-				("Invalid RateSection %d in Band 5G, Rf Path %d,"
-				" %dTx in PHY_GetTxPowerByRateBase()\n",
-				rate_section, path, txnum));
-			break;
-		};
-	} else {
-		RT_TRACE(COMP_INIT, DBG_LOUD,
-			("Invalid Band %d in PHY_GetTxPowerByRateBase()\n", band));
-	}
-
-	return value;
-
-}
-void _rtl8821ae_phy_store_txpower_by_rate_base(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u16 rawValue = 0;
-	u8 base = 0, path = 0;
-
-	for (path = RF90_PATH_A; path <= RF90_PATH_B; ++path) {
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][0] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, CCK, RF_1TX, base);
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][2] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, OFDM, RF_1TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][4] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS0_MCS7, RF_1TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][6] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, HT_MCS8_MCS15, RF_2TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_1TX][8] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][path][RF_2TX][11] >> 8) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_2_4G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][2] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, OFDM, RF_1TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][4] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, HT_MCS0_MCS7, RF_1TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_2TX][6] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, HT_MCS8_MCS15, RF_2TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_1TX][8] >> 24) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, VHT_1SSMCS0_1SSMCS9, RF_1TX, base );
-
-		rawValue = (u16) (rtlphy->tx_power_by_rate_offset[BAND_ON_5G][path][RF_2TX][11] >> 8) & 0xFF;
-		base = (rawValue >> 4) * 10 + (rawValue & 0xF);
-		_rtl8821ae_phy_set_txpower_by_rate_base(hw, BAND_ON_5G, path, VHT_2SSMCS0_2SSMCS9, RF_2TX, base );
-	}
-}
-
-void _phy_convert_txpower_dbm_to_relative_value(u32 *data, u8 start,
-																u8 end, u8 base_val)
-{
-	char i = 0;
-	u8 temp_value = 0;
-	u32 temp_data = 0;
-
-	for (i = 3; i >= 0; --i)
-	{
-		if (i >= start && i <= end) {
-			// Get the exact value
-			temp_value = (u8) (*data >> (i * 8)) & 0xF;
-			temp_value += ((u8) ((*data >> (i * 8 + 4)) & 0xF)) * 10;
-
-			// Change the value to a relative value
-			temp_value = (temp_value > base_val) ? temp_value - base_val : base_val - temp_value;
-		} else {
-			temp_value = (u8) (*data >> (i * 8)) & 0xFF;
-		}
-		temp_data <<= 8;
-		temp_data |= temp_value;
-	}
-	*data = temp_data;
-}
-
-void _rtl8821ae_phy_convert_txpower_dbm_to_relative_value(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 base = 0, rfPath = 0;
-
-	for (rfPath = RF90_PATH_A; rfPath <= RF90_PATH_B; ++rfPath) {
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, CCK);
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G CCK 1TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][0] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, OFDM );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G OFDM 1TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][1] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][2] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, HT_MCS0_MCS7 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G HTMCS0-7 1TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][3] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][4] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_2TX, HT_MCS8_MCS15 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G HTMCS8-15 2TX: %d\n", base ) );
-
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][5] ),
-			0, 3, base );
-
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][6] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_1TX, VHT_1SSMCS0_1SSMCS9 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G VHT1SSMCS0-9 1TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][7] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][8] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][9] ),
-			0, 1, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_2_4G, rfPath, RF_2TX, VHT_2SSMCS0_2SSMCS9 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 2.4G VHT2SSMCS0-9 2TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_1TX][9] ),
-			2, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][10] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_2_4G][rfPath][RF_2TX][11] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, OFDM );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G OFDM 1TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][1] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][2] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, HT_MCS0_MCS7 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G HTMCS0-7 1TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][3] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][4] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_2TX, HT_MCS8_MCS15 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G HTMCS8-15 2TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][5] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][6] ),
-			0, 3, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_1TX, VHT_1SSMCS0_1SSMCS9 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G VHT1SSMCS0-9 1TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][7] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][8] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][9] ),
-			0, 1, base );
-
-		base = _rtl8821ae_phy_get_txpower_by_rate_base(hw, BAND_ON_5G, rfPath, RF_2TX, VHT_2SSMCS0_2SSMCS9 );
-		RT_DISP( FPHY, PHY_TXPWR, ( "base of 5G VHT2SSMCS0-9 2TX: %d\n", base ) );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_1TX][9] ),
-			2, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][10] ),
-			0, 3, base );
-		_phy_convert_txpower_dbm_to_relative_value(
-			&(rtlphy->tx_power_by_rate_offset[BAND_ON_5G][rfPath][RF_2TX][11] ),
-			0, 3, base );
-	}
-
-	RT_TRACE(COMP_POWER, DBG_TRACE,
-		("<===_rtl8821ae_phy_convert_txpower_dbm_to_relative_value()\n"));
-
-}
-
-void _rtl8821ae_phy_txpower_by_rate_configuration(struct ieee80211_hw *hw)
-{
-	_rtl8821ae_phy_store_txpower_by_rate_base(hw);
-	_rtl8821ae_phy_convert_txpower_dbm_to_relative_value(hw);
-}
-
-static bool _rtl8821ae_phy_bb8821a_config_parafile(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	bool rtstatus;
-
-	/*TX POWER LIMIT
-	    PHY_InitTxPowerLimit
-	    PHY_ConfigRFWithCustomPowerLimitTableParaFile*/
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-		rtstatus = _rtl8812ae_phy_config_bb_with_headerfile(hw,
-								BASEBAND_CONFIG_PHY_REG);
-	else{
-		rtstatus = _rtl8821ae_phy_config_bb_with_headerfile(hw,
-								BASEBAND_CONFIG_PHY_REG);
-	}
-	if (rtstatus != true) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("Write BB Reg Fail!!"));
-		return false;
-	}
-	_rtl8821ae_phy_init_tx_power_by_rate(hw);
-	if (rtlefuse->autoload_failflag == false) {
-		//rtlphy->pwrgroup_cnt = 0;
-		if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-			rtstatus = _rtl8812ae_phy_config_bb_with_pgheaderfile(hw,
-								   BASEBAND_CONFIG_PHY_REG);
-		else{
-			rtstatus = _rtl8821ae_phy_config_bb_with_pgheaderfile(hw,
-								   BASEBAND_CONFIG_PHY_REG);
-		}
-	}
-	if (rtstatus != true) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("BB_PG Reg Fail!!"));
-		return false;
-	}
-
-	_rtl8821ae_phy_txpower_by_rate_configuration(hw);
-
-	if (rtlhal->hw_type == HARDWARE_TYPE_RTL8812AE)
-		rtstatus = _rtl8812ae_phy_config_bb_with_headerfile(hw,
-							 BASEBAND_CONFIG_AGC_TAB);
-	else
-		rtstatus = _rtl8821ae_phy_config_bb_with_headerfile(hw,
-							 BASEBAND_CONFIG_AGC_TAB);
-
-	if (rtstatus != true) {
-		RT_TRACE(COMP_ERR, DBG_EMERG, ("AGC Table Fail\n"));
-		return false;
-	}
-	rtlphy->bcck_high_power = (bool) (rtl_get_bbreg(hw,
-							RFPGA0_XA_HSSIPARAMETER2,
-							0x200));
-	return true;
-}
-
-static bool _rtl8812ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i, v1, v2;
-	u32 arraylength;
-	u32 *ptrarray;
-
-	RT_TRACE(COMP_INIT, DBG_TRACE, ("Read rtl8812AE_MAC_REG_Array\n"));
-	arraylength = RTL8812AEMAC_1T_ARRAYLEN;
-	ptrarray = RTL8812AE_MAC_REG_ARRAY;
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("Img:RTL8812AE_MAC_REG_ARRAY LEN %d\n",arraylength));
-	for (i = 0; i < arraylength; i += 2) {
-		v1 = ptrarray[i];
-		v2 = (u8) ptrarray[i + 1];
-		if (v1<0xCDCDCDCD) {
-			rtl_write_byte(rtlpriv, v1, (u8) v2);
-		} else {
-			if (!_rtl8821ae_check_condition(hw,v1)) {
-				/*Discard the following (offset, data) pairs*/
-				READ_NEXT_PAIR(ptrarray, v1, v2, i);
-				while (v2 != 0xDEAD &&
-					   v2 != 0xCDEF &&
-					   v2 != 0xCDCD && i < arraylength -2)
-					READ_NEXT_PAIR(ptrarray, v1, v2, i);
-
-				i -= 2; /* prevent from for-loop += 2*/
-			} else {/*Configure matched pairs and skip to end of if-else.*/
-				READ_NEXT_PAIR(ptrarray, v1, v2, i);
-				while (v2 != 0xDEAD &&
-					   v2 != 0xCDEF &&
-					   v2 != 0xCDCD && i < arraylength -2) {
-					rtl_write_byte(rtlpriv,v1,v2);
-					READ_NEXT_PAIR(ptrarray, v1, v2, i);
-				}
-
-				while (v2 != 0xDEAD && i < arraylength -2)
-					READ_NEXT_PAIR(ptrarray, v1, v2, i);
-			}
-		}
-	}
-	return true;
-}
-
-static bool _rtl8821ae_phy_config_mac_with_headerfile(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i, v1, v2;
-	u32 arraylength;
-	u32 *ptrarray;
-
-	RT_TRACE(COMP_INIT, DBG_TRACE, ("Read rtl8821AE_MAC_REG_Array\n"));
-	arraylength = RTL8821AEMAC_1T_ARRAYLEN;
-	ptrarray = RTL8821AE_MAC_REG_ARRAY;
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("Img:RTL8821AE_MAC_REG_ARRAY LEN %d\n",arraylength));
-	for (i = 0; i < arraylength; i += 2) {
-		v1 = ptrarray[i];
-		v2 = (u8) ptrarray[i + 1];
-		if (v1<0xCDCDCDCD) {
-			rtl_write_byte(rtlpriv, v1, (u8) v2);
-			continue;
-		} else {
-			if (!_rtl8821ae_check_condition(hw,v1)) {
-				/*Discard the following (offset, data) pairs*/
-				READ_NEXT_PAIR(ptrarray, v1, v2, i);
-				while (v2 != 0xDEAD &&
-					   v2 != 0xCDEF &&
-					   v2 != 0xCDCD && i < arraylength -2)
-					READ_NEXT_PAIR(ptrarray, v1, v2, i);
-
-				i -= 2; /* prevent from for-loop += 2*/
-			} else {/*Configure matched pairs and skip to end of if-else.*/
-				READ_NEXT_PAIR(ptrarray, v1, v2, i);
-				while (v2 != 0xDEAD &&
-					   v2 != 0xCDEF &&
-					   v2 != 0xCDCD && i < arraylength -2) {
-					rtl_write_byte(rtlpriv,v1,v2);
-					READ_NEXT_PAIR(ptrarray, v1, v2, i);
-				}
-
-				while (v2 != 0xDEAD && i < arraylength -2)
-					READ_NEXT_PAIR(ptrarray, v1, v2, i);
-			}
-		}
-	}
-	return true;
-}
-
-static bool _rtl8812ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						  u8 configtype)
-{
-	int i;
-	u32 *array_table;
-	u16 arraylen;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 v1 = 0, v2 = 0;
-
-	if (configtype == BASEBAND_CONFIG_PHY_REG) {
-		arraylen = RTL8812AEPHY_REG_1TARRAYLEN;
-		array_table = RTL8812AE_PHY_REG_ARRAY;
-
-		for (i = 0; i < arraylen; i += 2) {
-			v1 = array_table[i];
-			v2 = array_table[i+1];
-			if (v1<0xCDCDCDCD) {
-				_rtl8812ae_config_bb_reg(hw, v1, v2);
-				continue;
-			} else {/*This line is the start line of branch.*/
-				if (!_rtl8821ae_check_condition(hw,v1)) {
-					/*Discard the following (offset, data) pairs*/
-					READ_NEXT_PAIR(array_table,v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < arraylen -2)
-					    READ_NEXT_PAIR(array_table,v1, v2, i);
-
-					i -= 2; /* prevent from for-loop += 2*/
-				} else {/*Configure matched pairs and skip to end of if-else.*/
-					READ_NEXT_PAIR(array_table,v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < arraylen -2) {
-						_rtl8812ae_config_bb_reg(hw,v1,v2);
-						READ_NEXT_PAIR(array_table,v1, v2, i);
-					}
-
-					while (v2 != 0xDEAD && i < arraylen -2)
-						READ_NEXT_PAIR(array_table,v1, v2, i);
-				}
-			}
-		}
-	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
-			arraylen = RTL8812AEAGCTAB_1TARRAYLEN;
-			array_table = RTL8812AE_AGC_TAB_ARRAY;
-
-			for (i = 0; i < arraylen; i = i + 2) {
-				v1 = array_table[i];
-				v2 = array_table[i+1];
-				if (v1 < 0xCDCDCDCD) {
-					rtl_set_bbreg(hw, v1, MASKDWORD, v2);
-					udelay(1);
-					continue;
-			    } else {/*This line is the start line of branch.*/
-					if (!_rtl8821ae_check_condition(hw,v1)) {
-						/*Discard the following (offset, data) pairs*/
-						READ_NEXT_PAIR(array_table,v1, v2, i);
-						while (v2 != 0xDEAD &&
-						       v2 != 0xCDEF &&
-						       v2 != 0xCDCD && i < arraylen -2)
-						    READ_NEXT_PAIR(array_table,v1, v2, i);
-
-						i -= 2; /* prevent from for-loop += 2*/
-					}else{/*Configure matched pairs and skip to end of if-else.*/
-						READ_NEXT_PAIR(array_table,v1, v2, i);
-						while (v2 != 0xDEAD &&
-						       v2 != 0xCDEF &&
-						       v2 != 0xCDCD && i < arraylen -2)
-						{
-							rtl_set_bbreg(hw, v1, MASKDWORD, v2);
-							udelay(1);
-							READ_NEXT_PAIR(array_table,v1, v2, i);
-						}
-
-						while (v2 != 0xDEAD && i < arraylen -2)
-							READ_NEXT_PAIR(array_table,v1, v2, i);
-					}
-				}
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-				 ("The agctab_array_table[0] is "
-				  "%x Rtl818EEPHY_REGArray[1] is %x \n",
-				  array_table[i],
-				  array_table[i + 1]));
-		}
-	}
-	return true;
-}
-
-static bool _rtl8821ae_phy_config_bb_with_headerfile(struct ieee80211_hw *hw,
-						  u8 configtype)
-{
-	int i;
-	u32 *array_table;
-	u16 arraylen;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 v1 = 0, v2 = 0;
-
-	if (configtype == BASEBAND_CONFIG_PHY_REG) {
-		arraylen = RTL8821AEPHY_REG_1TARRAYLEN;
-		array_table = RTL8821AE_PHY_REG_ARRAY;
-
-		for (i = 0; i < arraylen; i += 2) {
-			v1 = array_table[i];
-			v2 = array_table[i+1];
-			if (v1<0xCDCDCDCD) {
-				_rtl8821ae_config_bb_reg(hw, v1, v2);
-				continue;
-			} else {/*This line is the start line of branch.*/
-				if (!_rtl8821ae_check_condition(hw,v1)) {
-					/*Discard the following (offset, data) pairs*/
-					READ_NEXT_PAIR(array_table, v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < arraylen -2)
-					    READ_NEXT_PAIR(array_table, v1, v2, i);
-
-					i -= 2; /* prevent from for-loop += 2*/
-				} else {/*Configure matched pairs and skip to end of if-else.*/
-					READ_NEXT_PAIR(array_table, v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < arraylen -2) {
-						_rtl8821ae_config_bb_reg(hw,v1,v2);
-						READ_NEXT_PAIR(array_table, v1, v2, i);
-					}
-
-					while (v2 != 0xDEAD && i < arraylen -2)
-						READ_NEXT_PAIR(array_table, v1, v2, i);
-				}
-			}
-		}
-	} else if (configtype == BASEBAND_CONFIG_AGC_TAB) {
-			arraylen = RTL8821AEAGCTAB_1TARRAYLEN;
-			array_table = RTL8821AE_AGC_TAB_ARRAY;
-
-			for (i = 0; i < arraylen; i = i + 2) {
-				v1 = array_table[i];
-				v2 = array_table[i+1];
-				if (v1 < 0xCDCDCDCD) {
-					rtl_set_bbreg(hw, v1, MASKDWORD, v2);
-					udelay(1);
-					continue;
-			    } else {/*This line is the start line of branch.*/
-					if (!_rtl8821ae_check_condition(hw,v1)) {
-						/*Discard the following (offset, data) pairs*/
-						READ_NEXT_PAIR(array_table, v1, v2, i);
-						while (v2 != 0xDEAD &&
-						       v2 != 0xCDEF &&
-						       v2 != 0xCDCD && i < arraylen -2)
-						    READ_NEXT_PAIR(array_table, v1, v2, i);
-
-						i -= 2; /* prevent from for-loop += 2*/
-					}else{/*Configure matched pairs and skip to end of if-else.*/
-						READ_NEXT_PAIR(array_table, v1, v2, i);
-						while (v2 != 0xDEAD &&
-						       v2 != 0xCDEF &&
-						       v2 != 0xCDCD && i < arraylen -2)
-						{
-							rtl_set_bbreg(hw, v1, MASKDWORD, v2);
-							udelay(1);
-							READ_NEXT_PAIR(array_table, v1, v2, i);
-						}
-
-						while (v2 != 0xDEAD && i < arraylen -2)
-							READ_NEXT_PAIR(array_table, v1, v2, i);
-					}
-				}
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-				 ("The agctab_array_table[0] is "
-				  "%x Rtl818EEPHY_REGArray[1] is %x \n",
-				  array_table[i],
-				  array_table[i + 1]));
-		}
-	}
-	return true;
-}
-
-static u8 _rtl8821ae_get_rate_selection_index(u32 regaddr)
-{
-	u8 index = 0;
-
-	regaddr &= 0xFFF;
-	if (regaddr >= 0xC20 && regaddr <= 0xC4C)
-		index = (u8) ((regaddr - 0xC20) / 4);
-	else if (regaddr >= 0xE20 && regaddr <= 0xE4C)
-		index = (u8) ((regaddr - 0xE20) / 4);
-	else
-		RT_ASSERT(!COMP_INIT,
-			("Invalid RegAddr 0x%x in"
-			 "PHY_GetRateSectionIndexOfTxPowerByRate()\n",regaddr));
-
-	return index;
-}
-
-static void _rtl8821ae_store_tx_power_by_rate(struct ieee80211_hw *hw,
-						   								 u32 band, u32 rfpath,
-						   								 u32 txnum, u32 regaddr,
-						   								 u32 bitmask, u32 data)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 rate_section = _rtl8821ae_get_rate_selection_index(regaddr);
-
-	if (band != BAND_ON_2_4G && band != BAND_ON_5G)
-		RT_TRACE(COMP_INIT, DBG_WARNING, ("Invalid Band %d\n", band));
-
-	if (rfpath > MAX_RF_PATH)
-		RT_TRACE(COMP_INIT, DBG_WARNING, ("Invalid RfPath %d\n", rfpath));
-
-	if (txnum > MAX_RF_PATH)
-		RT_TRACE(COMP_INIT, DBG_WARNING, ("Invalid TxNum %d\n", txnum ) );
-
-	rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section] = data;
-	RT_TRACE(COMP_INIT, DBG_WARNING,( "pHalData->TxPwrByRateOffset[Band %d][RfPath %d][TxNum %d][RateSection %d] = 0x%x\n",
-			band, rfpath, txnum, rate_section, rtlphy->tx_power_by_rate_offset[band][rfpath][txnum][rate_section]));
-
-}
-
-static bool _rtl8812ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
-						   											 u8 configtype)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i;
-	u32 *phy_regarray_table_pg;
-	u16 phy_regarray_pg_len;
-	u32 v1, v2, v3, v4, v5, v6;
-
-	phy_regarray_pg_len = RTL8812AEPHY_REG_ARRAY_PGLEN;
-	phy_regarray_table_pg = RTL8812AE_PHY_REG_ARRAY_PG;
-
-	if (configtype == BASEBAND_CONFIG_PHY_REG) {
-		for (i = 0; i < phy_regarray_pg_len; i += 6) {
-			v1 = phy_regarray_table_pg[i];
-			v2 = phy_regarray_table_pg[i+1];
-			v3 = phy_regarray_table_pg[i+2];
-			v4 = phy_regarray_table_pg[i+3];
-			v5 = phy_regarray_table_pg[i+4];
-			v6 = phy_regarray_table_pg[i+5];
-
-			if (v1<0xCDCDCDCD) {
-				if ( (v4 == 0xfe) || (v4 == 0xffe))
-					mdelay(50);
-				else
-					/*_rtl8821ae_store_pwrIndex_diffrate_offset*/
-					_rtl8821ae_store_tx_power_by_rate(hw, v1, v2, v3, v4, v5, v6);
-				continue;
-			} else {
-				if (!_rtl8821ae_check_condition(hw,v1)) { /*don't need the hw_body*/
-			    	i += 2; /* skip the pair of expression*/
-			        v1 = phy_regarray_table_pg[i];
-			        v2 = phy_regarray_table_pg[i+1];
-			        v3 = phy_regarray_table_pg[i+2];
-			        while (v2 != 0xDEAD) {
-			            i += 3;
-			            v1 = phy_regarray_table_pg[i];
-			            v2 = phy_regarray_table_pg[i+1];
-			            v3 = phy_regarray_table_pg[i+2];
-			        }
-			    }
-			}
-		}
-	} else {
-
-		RT_TRACE(COMP_SEND, DBG_TRACE,
-			 ("configtype != BaseBand_Config_PHY_REG\n"));
-	}
-	return true;
-}
-
-static bool _rtl8821ae_phy_config_bb_with_pgheaderfile(struct ieee80211_hw *hw,
-						   											 u8 configtype)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int i;
-	u32 *phy_regarray_table_pg;
-	u16 phy_regarray_pg_len;
-	u32 v1, v2, v3, v4, v5, v6;
-
-	phy_regarray_pg_len = RTL8821AEPHY_REG_ARRAY_PGLEN;
-	phy_regarray_table_pg = RTL8821AE_PHY_REG_ARRAY_PG;
-
-	if (configtype == BASEBAND_CONFIG_PHY_REG) {
-		for (i = 0; i < phy_regarray_pg_len; i += 6) {
-			v1 = phy_regarray_table_pg[i];
-			v2 = phy_regarray_table_pg[i+1];
-			v3 = phy_regarray_table_pg[i+2];
-			v4 = phy_regarray_table_pg[i+3];
-			v5 = phy_regarray_table_pg[i+4];
-			v6 = phy_regarray_table_pg[i+5];
-
-			if (v1<0xCDCDCDCD) {
-				if (v4 == 0xfe)
-					mdelay(50);
-				else if (v4 == 0xfd)
-					mdelay(5);
-				else if (v4 == 0xfc)
-					mdelay(1);
-				else if (v4 == 0xfb)
-					udelay(50);
-				else if (v4 == 0xfa)
-					udelay(5);
-				else if (v4 == 0xf9)
-					udelay(1);
-
-				/*_rtl8821ae_store_pwrIndex_diffrate_offset*/
-				_rtl8821ae_store_tx_power_by_rate(hw, v1, v2, v3, v4, v5, v6);
-				continue;
-			} else {
-				if (!_rtl8821ae_check_condition(hw,v1)) { /*don't need the hw_body*/
-			    	i += 2; /* skip the pair of expression*/
-			        v1 = phy_regarray_table_pg[i];
-			        v2 = phy_regarray_table_pg[i+1];
-			        v3 = phy_regarray_table_pg[i+2];
-			        while (v2 != 0xDEAD) {
-			            i += 3;
-			            v1 = phy_regarray_table_pg[i];
-			            v2 = phy_regarray_table_pg[i+1];
-			            v3 = phy_regarray_table_pg[i+2];
-			        }
-			    }
-			}
-		}
-	} else {
-
-		RT_TRACE(COMP_SEND, DBG_TRACE,
-			 ("configtype != BaseBand_Config_PHY_REG\n"));
-	}
-	return true;
-}
-
-bool rtl8812ae_phy_config_rf_with_headerfile(struct ieee80211_hw * hw,
-					  									 enum radio_path rfpath)
-{
-	#define READ_NEXT_RF_PAIR_8812(radioa_array_table,v1, v2, i) do { i += 2; v1 = radioa_array_table[i]; v2 = radioa_array_table[i+1]; } while(0)
-
-	int i;
-	bool rtstatus = true;
-	u32 *radioa_array_table_a, *radioa_array_table_b;
-	u16 radioa_arraylen_a, radioa_arraylen_b;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 v1 = 0, v2 = 0;
-
-	radioa_arraylen_a = RTL8812AE_RADIOA_1TARRAYLEN;
-	radioa_array_table_a= RTL8812AE_RADIOA_ARRAY;
-	radioa_arraylen_b= RTL8812AE_RADIOB_1TARRAYLEN;
-	radioa_array_table_b = RTL8812AE_RADIOB_ARRAY;
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("Radio_A:RTL8821AE_RADIOA_ARRAY %d\n",radioa_arraylen_a));
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
-	rtstatus = true;
-	switch (rfpath) {
-	case RF90_PATH_A:
-		for (i = 0; i < radioa_arraylen_a; i = i + 2) {
-			v1 = radioa_array_table_a[i];
-			v2 = radioa_array_table_a[i+1];
-			if (v1<0xcdcdcdcd) {
-				_rtl8821ae_config_rf_radio_a(hw,v1,v2);
-				continue;
-			}else{/*This line is the start line of branch.*/
-				if(!_rtl8821ae_check_condition(hw,v1)){
-					/*Discard the following (offset, data) pairs*/
-					READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < radioa_arraylen_a-2)
-					    READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
-
-					i -= 2; /* prevent from for-loop += 2*/
-				} else {/*Configure matched pairs and skip to end of if-else.*/
-					READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < radioa_arraylen_a -2) {
-						_rtl8821ae_config_rf_radio_a(hw,v1,v2);
-						READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
-					}
-
-					while (v2 != 0xDEAD && i < radioa_arraylen_a-2)
-						READ_NEXT_RF_PAIR_8812(radioa_array_table_a,v1, v2, i);
-				}
-			}
-		}
-		break;
-	case RF90_PATH_B:
-		for (i = 0; i < radioa_arraylen_b; i = i + 2) {
-			v1 = radioa_array_table_b[i];
-			v2 = radioa_array_table_b[i+1];
-			if (v1<0xcdcdcdcd) {
-				_rtl8821ae_config_rf_radio_b(hw,v1,v2);
-				continue;
-			}else{/*This line is the start line of branch.*/
-				if(!_rtl8821ae_check_condition(hw,v1)){
-					/*Discard the following (offset, data) pairs*/
-					READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < radioa_arraylen_b-2)
-					    READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
-
-					i -= 2; /* prevent from for-loop += 2*/
-				} else {/*Configure matched pairs and skip to end of if-else.*/
-					READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < radioa_arraylen_b-2) {
-						_rtl8821ae_config_rf_radio_b(hw,v1,v2);
-						READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
-					}
-
-					while (v2 != 0xDEAD && i < radioa_arraylen_b-2)
-						READ_NEXT_RF_PAIR_8812(radioa_array_table_b,v1, v2, i);
-				}
-			}
-		}
-		break;
-	case RF90_PATH_C:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		break;
-	case RF90_PATH_D:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		break;
-	}
-	return true;
-}
-
-
-bool rtl8821ae_phy_config_rf_with_headerfile(struct ieee80211_hw * hw,
-					  									 enum radio_path rfpath)
-{
-	#define READ_NEXT_RF_PAIR(v1, v2, i) do { i += 2; v1 = radioa_array_table[i]; v2 = radioa_array_table[i+1]; } while(0)
-
-	int i;
-	bool rtstatus = true;
-	u32 *radioa_array_table;
-	u16 radioa_arraylen;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	//struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u32 v1 = 0, v2 = 0;
-
-	radioa_arraylen = RTL8821AE_RADIOA_1TARRAYLEN;
-	radioa_array_table = RTL8821AE_RADIOA_ARRAY;
-	RT_TRACE(COMP_INIT, DBG_LOUD,
-		 ("Radio_A:RTL8821AE_RADIOA_ARRAY %d\n",radioa_arraylen));
-	RT_TRACE(COMP_INIT, DBG_LOUD, ("Radio No %x\n", rfpath));
-	rtstatus = true;
-	switch (rfpath) {
-	case RF90_PATH_A:
-		for (i = 0; i < radioa_arraylen; i = i + 2) {
-			v1 = radioa_array_table[i];
-			v2 = radioa_array_table[i+1];
-			if (v1<0xcdcdcdcd) {
-				_rtl8821ae_config_rf_radio_a(hw,v1,v2);
-			}else{/*This line is the start line of branch.*/
-				if(!_rtl8821ae_check_condition(hw,v1)){
-					/*Discard the following (offset, data) pairs*/
-					READ_NEXT_RF_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < radioa_arraylen -2)
-					    READ_NEXT_RF_PAIR(v1, v2, i);
-
-					i -= 2; /* prevent from for-loop += 2*/
-				} else {/*Configure matched pairs and skip to end of if-else.*/
-					READ_NEXT_RF_PAIR(v1, v2, i);
-					while (v2 != 0xDEAD &&
-					       v2 != 0xCDEF &&
-					       v2 != 0xCDCD && i < radioa_arraylen -2) {
-						_rtl8821ae_config_rf_radio_a(hw,v1,v2);
-						READ_NEXT_RF_PAIR(v1, v2, i);
-					}
-
-					while (v2 != 0xDEAD && i < radioa_arraylen -2)
-						READ_NEXT_RF_PAIR(v1, v2, i);
-				}
-			}
-		}
-		break;
-
-	case RF90_PATH_B:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		break;
-	case RF90_PATH_C:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		break;
-	case RF90_PATH_D:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		break;
-	}
-	return true;
-}
-
-void rtl8821ae_phy_get_hw_reg_originalvalue(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	rtlphy->default_initialgain[0] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XAAGCCORE1, MASKBYTE0);
-	rtlphy->default_initialgain[1] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XBAGCCORE1, MASKBYTE0);
-	rtlphy->default_initialgain[2] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XCAGCCORE1, MASKBYTE0);
-	rtlphy->default_initialgain[3] =
-	    (u8) rtl_get_bbreg(hw, ROFDM0_XDAGCCORE1, MASKBYTE0);
-
-	RT_TRACE(COMP_INIT, DBG_TRACE,
-		 ("Default initial gain (c50=0x%x, "
-		  "c58=0x%x, c60=0x%x, c68=0x%x \n",
-		  rtlphy->default_initialgain[0],
-		  rtlphy->default_initialgain[1],
-		  rtlphy->default_initialgain[2],
-		  rtlphy->default_initialgain[3]));
-
-	rtlphy->framesync = (u8) rtl_get_bbreg(hw,
-					       ROFDM0_RXDETECTOR3, MASKBYTE0);
-	rtlphy->framesync_c34 = rtl_get_bbreg(hw,
-					      ROFDM0_RXDETECTOR2, MASKDWORD);
-
-	RT_TRACE(COMP_INIT, DBG_TRACE,
-		 ("Default framesync (0x%x) = 0x%x \n",
-		  ROFDM0_RXDETECTOR3, rtlphy->framesync));
-}
-
-static void _rtl8821ae_phy_init_bb_rf_register_definition(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfs = RFPGA0_XAB_RFINTERFACESW;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfo = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfo = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfintfe = RFPGA0_XA_RFINTERFACEOE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfintfe = RFPGA0_XB_RFINTERFACEOE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rf3wire_offset = RA_LSSIWRITE_8821A;
-	rtlphy->phyreg_def[RF90_PATH_B].rf3wire_offset = RB_LSSIWRITE_8821A;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rfhssi_para2 = RHSSIREAD_8821AE;
-	rtlphy->phyreg_def[RF90_PATH_B].rfhssi_para2 = RHSSIREAD_8821AE;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readback = RA_SIREAD_8821A;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readback = RB_SIREAD_8821A;
-
-	rtlphy->phyreg_def[RF90_PATH_A].rflssi_readbackpi = RA_PIREAD_8821A;
-	rtlphy->phyreg_def[RF90_PATH_B].rflssi_readbackpi = RB_PIREAD_8821A;
-}
-
-void rtl8821ae_phy_get_txpower_level(struct ieee80211_hw *hw, long *powerlevel)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 txpwr_level;
-	long txpwr_dbm;
-
-	txpwr_level = rtlphy->cur_cck_txpwridx;
-	txpwr_dbm = _rtl8821ae_phy_txpwr_idx_to_dbm(hw,
-						 WIRELESS_MODE_B, txpwr_level);
-	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (_rtl8821ae_phy_txpwr_idx_to_dbm(hw,
-					 WIRELESS_MODE_G,
-					 txpwr_level) > txpwr_dbm)
-		txpwr_dbm =
-		    _rtl8821ae_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_G,
-						 txpwr_level);
-	txpwr_level = rtlphy->cur_ofdm24g_txpwridx;
-	if (_rtl8821ae_phy_txpwr_idx_to_dbm(hw,
-					 WIRELESS_MODE_N_24G,
-					 txpwr_level) > txpwr_dbm)
-		txpwr_dbm =
-		    _rtl8821ae_phy_txpwr_idx_to_dbm(hw, WIRELESS_MODE_N_24G,
-						 txpwr_level);
-	*powerlevel = txpwr_dbm;
-}
-
-static bool _rtl8821ae_phy_get_chnl_index(u8 channel, u8 *chnl_index)
-{
-	u8 channel_5g[CHANNEL_MAX_NUMBER_5G] =
-				 {36,38,40,42,44,46,48,50,52,54,56,58,60,62,64,100,102,104,106,108,110,112,
-				114,116,118,120,122,124,126,128,130,132,134,136,138,140,142,144,149,151,
-				153,155,157,159,161,163,165,167,168,169,171,173,175,177};
-	u8 i = 0;
-	bool in_24g = true;
-
-	if (channel <= 14) {
-		in_24g = true;
-		*chnl_index = channel - 1;
-	} else {
-		in_24g = false;
-
-		for (i = 0; i < sizeof(channel_5g) / sizeof(u8); ++i) {
-			if (channel_5g[i] == channel) {
-				*chnl_index = i;
-				return in_24g;
-			}
-		}
-	}
-	return in_24g;
-}
-
-static char _rtl8821ae_phy_get_ratesection_intxpower_byrate(u8 path, u8 rate)
-{
-	char rate_section = 0;
-	switch (rate) {
-	case DESC_RATE1M:
-	case DESC_RATE2M:
-	case DESC_RATE5_5M:
-	case DESC_RATE11M:
-		rate_section = 0;
-		break;
-
-	case DESC_RATE6M:
-	case DESC_RATE9M:
-	case DESC_RATE12M:
-	case DESC_RATE18M:
-		rate_section = 1;
-		break;
-
-	case DESC_RATE24M:
-	case DESC_RATE36M:
-	case DESC_RATE48M:
-	case DESC_RATE54M:
-		rate_section = 2;
-		break;
-
-	case DESC_RATEMCS0:
-	case DESC_RATEMCS1:
-	case DESC_RATEMCS2:
-	case DESC_RATEMCS3:
-		rate_section = 3;
-		break;
-
-	case DESC_RATEMCS4:
-	case DESC_RATEMCS5:
-	case DESC_RATEMCS6:
-	case DESC_RATEMCS7:
-		rate_section = 4;
-		break;
-
-	case DESC_RATEMCS8:
-	case DESC_RATEMCS9:
-	case DESC_RATEMCS10:
-	case DESC_RATEMCS11:
-		rate_section = 5;
-		break;
-
-	case DESC_RATEMCS12:
-	case DESC_RATEMCS13:
-	case DESC_RATEMCS14:
-	case DESC_RATEMCS15:
-		rate_section = 6;
-		break;
-
-	case DESC_RATEVHT1SS_MCS0:
-	case DESC_RATEVHT1SS_MCS1:
-	case DESC_RATEVHT1SS_MCS2:
-	case DESC_RATEVHT1SS_MCS3:
-		rate_section = 7;
-		break;
-
-	case DESC_RATEVHT1SS_MCS4:
-	case DESC_RATEVHT1SS_MCS5:
-	case DESC_RATEVHT1SS_MCS6:
-	case DESC_RATEVHT1SS_MCS7:
-		rate_section = 8;
-		break;
-
-	case DESC_RATEVHT1SS_MCS8:
-	case DESC_RATEVHT1SS_MCS9:
-	case DESC_RATEVHT2SS_MCS0:
-	case DESC_RATEVHT2SS_MCS1:
-		rate_section = 9;
-		break;
-
-	case DESC_RATEVHT2SS_MCS2:
-	case DESC_RATEVHT2SS_MCS3:
-	case DESC_RATEVHT2SS_MCS4:
-	case DESC_RATEVHT2SS_MCS5:
-		rate_section = 10;
-		break;
-
-	case DESC_RATEVHT2SS_MCS6:
-	case DESC_RATEVHT2SS_MCS7:
-	case DESC_RATEVHT2SS_MCS8:
-	case DESC_RATEVHT2SS_MCS9:
-		rate_section = 11;
-		break;
-
-	default:
-		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
-		break;
-	}
-
-	return rate_section;
-}
-
-static char _rtl8821ae_phy_get_txpower_by_rate(struct ieee80211_hw *hw,
-														   u8 band, u8 path, u8 rate)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 shift = 0, rate_section, tx_num;
-	char tx_pwr_diff = 0;
-
-	rate_section = _rtl8821ae_phy_get_ratesection_intxpower_byrate(path, rate);
-	tx_num = RF_TX_NUM_NONIMPLEMENT;
-
-	if (tx_num == RF_TX_NUM_NONIMPLEMENT) {
-		if ((rate >= DESC_RATEMCS8 && rate <= DESC_RATEMCS15 ) ||
-			(rate >= DESC_RATEVHT2SS_MCS2 && rate <= DESC_RATEVHT2SS_MCS9))
-			 tx_num = RF_2TX;
-		else
-			tx_num = RF_1TX;
-	}
-
-	switch (rate) {
-	case DESC_RATE1M:	shift = 0;		break;
-	case DESC_RATE2M:	shift = 8;		break;
-	case DESC_RATE5_5M:	shift = 16;		break;
-	case DESC_RATE11M:	shift = 24;		break;
-
-	case DESC_RATE6M:	shift = 0;		break;
-	case DESC_RATE9M:	shift = 8;      break;
-	case DESC_RATE12M:	shift = 16;     break;
-	case DESC_RATE18M:	shift = 24;     break;
-
-	case DESC_RATE24M:	shift = 0;    	break;
-	case DESC_RATE36M:	shift = 8;      break;
-	case DESC_RATE48M:	shift = 16;     break;
-	case DESC_RATE54M:	shift = 24;     break;
-
-	case DESC_RATEMCS0:	shift = 0; 		break;
-	case DESC_RATEMCS1:	shift = 8;      break;
-	case DESC_RATEMCS2:	shift = 16;     break;
-	case DESC_RATEMCS3:	shift = 24;     break;
-
-	case DESC_RATEMCS4:	shift = 0; 		break;
-	case DESC_RATEMCS5:	shift = 8;      break;
-	case DESC_RATEMCS6:	shift = 16;     break;
-	case DESC_RATEMCS7:	shift = 24;     break;
-
-	case DESC_RATEMCS8:	shift = 0; 		break;
-	case DESC_RATEMCS9:	shift = 8;      break;
-	case DESC_RATEMCS10:	shift = 16;     break;
-	case DESC_RATEMCS11:	shift = 24;     break;
-
-	case DESC_RATEMCS12:	shift = 0; 		break;
-	case DESC_RATEMCS13:	shift = 8;      break;
-	case DESC_RATEMCS14:	shift = 16;     break;
-	case DESC_RATEMCS15:	shift = 24;     break;
-
-	case DESC_RATEVHT1SS_MCS0:	shift = 0; 		break;
-	case DESC_RATEVHT1SS_MCS1:	shift = 8;      break;
-	case DESC_RATEVHT1SS_MCS2:	shift = 16;     break;
-	case DESC_RATEVHT1SS_MCS3:	shift = 24;     break;
-
-	case DESC_RATEVHT1SS_MCS4:	shift = 0; 		break;
-	case DESC_RATEVHT1SS_MCS5:	shift = 8;      break;
-	case DESC_RATEVHT1SS_MCS6:	shift = 16;     break;
-	case DESC_RATEVHT1SS_MCS7:	shift = 24;     break;
-
-	case DESC_RATEVHT1SS_MCS8:	shift = 0; 		break;
-	case DESC_RATEVHT1SS_MCS9:	shift = 8;      break;
-	case DESC_RATEVHT2SS_MCS0:	shift = 16;     break;
-	case DESC_RATEVHT2SS_MCS1:	shift = 24;     break;
-
-	case DESC_RATEVHT2SS_MCS2:	shift = 0; 		break;
-	case DESC_RATEVHT2SS_MCS3:	shift = 8;      break;
-	case DESC_RATEVHT2SS_MCS4:	shift = 16;     break;
-	case DESC_RATEVHT2SS_MCS5:	shift = 24;     break;
-
-	case DESC_RATEVHT2SS_MCS6:	shift = 0; 		break;
-	case DESC_RATEVHT2SS_MCS7:	shift = 8;      break;
-	case DESC_RATEVHT2SS_MCS8:	shift = 16;     break;
-	case DESC_RATEVHT2SS_MCS9:	shift = 24;     break;
-
-	default:
-		RT_ASSERT(true, ("Rate_Section is Illegal\n"));
-		break;
-	}
-
-	tx_pwr_diff = (u8) (rtlphy->tx_power_by_rate_offset[band][path][tx_num][rate_section] >> shift) & 0xff;
-
-	return	tx_pwr_diff;
-}
-
-static u8 _rtl8821ae_get_txpower_index(struct ieee80211_hw *hw, u8 path,
-				      							 u8 rate, u8 bandwidth, u8 channel)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	u8 index = (channel - 1);
-	u8 txpower = 0;
-	bool in_24g = false;
-	char powerdiff_byrate = 0;
-
-	if (((rtlhal->current_bandtype == BAND_ON_2_4G) && (channel > 14 || channel < 1)) ||
-		((rtlhal->current_bandtype == BAND_ON_5G) && (channel <= 14))) {
-		index = 0;
-		RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("Illegal channel!!\n"));
-	}
-
-	in_24g = _rtl8821ae_phy_get_chnl_index(channel, &index);
-	if (in_24g) {
-		if (RX_HAL_IS_CCK_RATE(rate))
-			txpower = rtlefuse->txpwrlevel_cck[path][index];
-		else if ( DESC_RATE6M <= rate )
-			txpower = rtlefuse->txpwrlevel_ht40_1s[path][index];
-		else
-			RT_TRACE(COMP_POWER_TRACKING, DBG_LOUD, ("invalid rate\n"));
-
-		if (DESC_RATE6M <= rate && rate <= DESC_RATE54M && !RX_HAL_IS_CCK_RATE(rate))
-			txpower += rtlefuse->txpwr_legacyhtdiff[path][TX_1S];
-
-		if (bandwidth == HT_CHANNEL_WIDTH_20) {
-			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-				txpower += rtlefuse->txpwr_ht20diff[path][TX_1S];
-			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-				txpower += rtlefuse->txpwr_ht20diff[path][TX_2S];
-		} else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
-			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-				txpower += rtlefuse->txpwr_ht40diff[path][TX_1S];
-			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-				txpower += rtlefuse->txpwr_ht40diff[path][TX_2S];
-		} else if (bandwidth == HT_CHANNEL_WIDTH_80) {
-			if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-				txpower += rtlefuse->txpwr_ht40diff[path][TX_1S];
-			if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-				txpower += rtlefuse->txpwr_ht40diff[path][TX_2S];
-		}
-
-	} else {
-		if (DESC_RATE6M <= rate)
-			txpower = rtlefuse->txpwr_5g_bw40base[path][index];
-	    else
-	        RT_TRACE(COMP_POWER_TRACKING, DBG_WARNING,("INVALID Rate.\n"));
-
-        if (DESC_RATE6M <= rate && rate <= DESC_RATE54M && !RX_HAL_IS_CCK_RATE(rate))
-	    	txpower += rtlefuse->txpwr_5g_ofdmdiff[path][TX_1S];
-
-	    if (bandwidth == HT_CHANNEL_WIDTH_20) {
-	        if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-	    	    txpower += rtlefuse->txpwr_5g_bw20diff[path][TX_1S];
-	        if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-	    	    txpower += rtlefuse->txpwr_5g_bw20diff[path][TX_2S];
-	    } else if (bandwidth == HT_CHANNEL_WIDTH_20_40) {
-	        if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-	    	    txpower += rtlefuse->txpwr_5g_bw40diff[path][TX_1S];
-	        if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-	    	    txpower += rtlefuse->txpwr_5g_bw40diff[path][TX_2S];
-	    } else if (bandwidth == HT_CHANNEL_WIDTH_80) {
-			u8 channel_5g_80m[CHANNEL_MAX_NUMBER_5G_80M] = {42, 58, 106, 122, 138, 155, 171};
-			u8 i = 0;
-			for (i = 0; i < sizeof(channel_5g_80m) / sizeof(u8); ++i)
-				if (channel_5g_80m[i] == channel)
-					index = i;
-
-	        if ((DESC_RATEMCS0 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT1SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-	    	    txpower = rtlefuse->txpwr_5g_bw80base[path][index]
-	    	    		+ rtlefuse->txpwr_5g_bw80diff[path][TX_1S];
-	        if ((DESC_RATEMCS8 <= rate && rate <= DESC_RATEMCS15) ||
-				(DESC_RATEVHT2SS_MCS0 <= rate && rate <= DESC_RATEVHT2SS_MCS9))
-	    	    txpower = rtlefuse->txpwr_5g_bw80base[path][index]
-	    	    		+ rtlefuse->txpwr_5g_bw80diff[path][TX_1S]
-	    	    		+ rtlefuse->txpwr_5g_bw80diff[path][TX_2S];
-	    }
-	}
-	if (rtlefuse->eeprom_regulatory != 2)
-		powerdiff_byrate = _rtl8821ae_phy_get_txpower_by_rate(hw,
-									(u8)(!in_24g), path, rate);
-
-	if (rate == DESC_RATEVHT1SS_MCS8 || rate == DESC_RATEVHT1SS_MCS9 ||
-		rate == DESC_RATEVHT2SS_MCS8 || rate == DESC_RATEVHT2SS_MCS9)
-		txpower -= powerdiff_byrate;
-	else
-		txpower += powerdiff_byrate;
-
-	if (rate > DESC_RATE11M)
-		txpower += rtlpriv->dm.remnant_ofdm_swing_idx[path];
-	else
-		txpower += rtlpriv->dm.remnant_cck_idx;
-
-	if (txpower > MAX_POWER_INDEX)
-		txpower = MAX_POWER_INDEX;
-
-	return txpower;
-}
-
-static void _rtl8821ae_phy_set_txpower_index(struct ieee80211_hw *hw,
-												u8 power_index, u8 path, u8 rate)
-{
-	struct rtl_priv* rtlpriv = rtl_priv(hw);
-
-	if (path == RF90_PATH_A) {
-        switch (rate) {
-        case DESC_RATE1M:
-		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE0, power_index);
-		break;
-        case DESC_RATE2M:
-		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE1, power_index);
-		break;
-        case DESC_RATE5_5M:
-		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE2, power_index);
-		break;
-        case DESC_RATE11M:
-		rtl_set_bbreg(hw, RTXAGC_A_CCK11_CCK1, MASKBYTE3, power_index);
-		break;
-
-        case DESC_RATE6M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE0, power_index);
-		break;
-        case DESC_RATE9M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE1, power_index);
-		break;
-        case DESC_RATE12M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE2, power_index);
-		break;
-        case DESC_RATE18M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM18_OFDM6, MASKBYTE3, power_index);
-		break;
-
-        case DESC_RATE24M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE0, power_index);
-		break;
-        case DESC_RATE36M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE1, power_index);
-		break;
-        case DESC_RATE48M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE2, power_index);
-		break;
-        case DESC_RATE54M:
-		rtl_set_bbreg(hw, RTXAGC_A_OFDM54_OFDM24, MASKBYTE3, power_index);
-		break;
-
-        case DESC_RATEMCS0:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE0, power_index);
-		break;
-        case DESC_RATEMCS1:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE1, power_index);
-		break;
-        case DESC_RATEMCS2:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE2, power_index);
-		break;
-        case DESC_RATEMCS3:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS03_MCS00, MASKBYTE3, power_index);
-		break;
-
-        case DESC_RATEMCS4:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE0, power_index);
-		break;
-        case DESC_RATEMCS5:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE1, power_index);
-		break;
-        case DESC_RATEMCS6:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE2, power_index);
-		break;
-        case DESC_RATEMCS7:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS07_MCS04, MASKBYTE3, power_index);
-		break;
-
-        case DESC_RATEMCS8:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE0, power_index);
-		break;
-        case DESC_RATEMCS9:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE1, power_index);
-		break;
-        case DESC_RATEMCS10:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE2, power_index);
-		break;
-        case DESC_RATEMCS11:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS11_MCS08, MASKBYTE3, power_index);
-		break;
-
-        case DESC_RATEMCS12:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE0, power_index);
-		break;
-        case DESC_RATEMCS13:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE1, power_index);
-		break;
-        case DESC_RATEMCS14:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE2, power_index);
-		break;
-        case DESC_RATEMCS15:
-		rtl_set_bbreg(hw, RTXAGC_A_MCS15_MCS12, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT1SS_MCS0:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS1:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS2:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS3:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX3_NSS1INDEX0, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT1SS_MCS4:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS5:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS6:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS7:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS1INDEX7_NSS1INDEX4, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT1SS_MCS8:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS9:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS0:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS1:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX1_NSS1INDEX8, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT2SS_MCS2:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS3:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS4:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS5:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX5_NSS2INDEX2, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT2SS_MCS6:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS7:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS8:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS9:
-		rtl_set_bbreg(hw, RTXAGC_A_NSS2INDEX9_NSS2INDEX6, MASKBYTE3, power_index);
-		break;
-
-        default:
-			RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
-            break;
-        }
-    } else if (path == RF90_PATH_B) {
-	switch (rate) {
-	case DESC_RATE1M:
-		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE0, power_index);
-		break;
-	case DESC_RATE2M:
-		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE1, power_index);
-		break;
-	case DESC_RATE5_5M:
-		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE2, power_index);
-		break;
-	case DESC_RATE11M:
-		rtl_set_bbreg(hw, RTXAGC_B_CCK11_CCK1, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATE6M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE0, power_index);
-		break;
-	case DESC_RATE9M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE1, power_index);
-		break;
-	case DESC_RATE12M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE2, power_index);
-		break;
-	case DESC_RATE18M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM18_OFDM6, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATE24M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE0, power_index);
-		break;
-	case DESC_RATE36M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE1, power_index);
-		break;
-	case DESC_RATE48M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE2, power_index);
-		break;
-	case DESC_RATE54M:
-		rtl_set_bbreg(hw, RTXAGC_B_OFDM54_OFDM24, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEMCS0:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEMCS1:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEMCS2:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEMCS3:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS03_MCS00, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEMCS4:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEMCS5:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEMCS6:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEMCS7:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS07_MCS04, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEMCS8:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEMCS9:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEMCS10:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEMCS11:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS11_MCS08, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEMCS12:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEMCS13:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEMCS14:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEMCS15:
-		rtl_set_bbreg(hw, RTXAGC_B_MCS15_MCS12, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT1SS_MCS0:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS1:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS2:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS3:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX3_NSS1INDEX0, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT1SS_MCS4:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS5:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS6:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS7:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS1INDEX7_NSS1INDEX4, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT1SS_MCS8:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT1SS_MCS9:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS0:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS1:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX1_NSS1INDEX8, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT2SS_MCS2:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS3:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS4:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS5:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX5_NSS2INDEX2, MASKBYTE3, power_index);
-		break;
-
-	case DESC_RATEVHT2SS_MCS6:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE0, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS7:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE1, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS8:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE2, power_index);
-		break;
-	case DESC_RATEVHT2SS_MCS9:
-		rtl_set_bbreg(hw, RTXAGC_B_NSS2INDEX9_NSS2INDEX6, MASKBYTE3, power_index);
-		break;
-
-       default:
-		RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid Rate!!\n"));
-            	break;
-        }
-    } else {
-		RT_TRACE(COMP_POWER, DBG_LOUD, ("Invalid RFPath!!\n"));
-    }
-}
-
-void _rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw,
-													u8 *array, u8 path, u8 channel,
-													u8 size)
-{
-	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
-	u8 i;
-	u8 power_index;
-	for (i = 0; i < size; i ++) {
-		power_index = _rtl8821ae_get_txpower_index(hw, path, array[i],
-									rtlphy->current_chan_bw, channel);
-		_rtl8821ae_phy_set_txpower_index(hw, power_index, path, array[i]);
-	}
-}
-
-static void _rtl8821ae_phy_txpower_training_by_path(struct ieee80211_hw *hw,
-														u8 bw, u8 channel, u8 path)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	u8 i;
-	u32 power_level, data, offset;
-
-	if(path >= rtlphy->num_total_rfpath)
-		return;
-
-	data = 0;
-	if (path == RF90_PATH_A) {
-		power_level =
-			_rtl8821ae_get_txpower_index(hw, RF90_PATH_A,
-			DESC_RATEMCS7, bw, channel);
-		offset =  RA_TXPWRTRAING;
-	} else {
-		power_level =
-			_rtl8821ae_get_txpower_index(hw, RF90_PATH_A,
-			DESC_RATEMCS7, bw, channel);
-		offset =  RB_TXPWRTRAING;
-	}
-
-	for (i = 0; i < 3; i++) {
-		if (i == 0)
-			power_level = power_level - 10;
-		else if (i == 1)
-			power_level = power_level - 8;
-		else
-			power_level = power_level - 6;
-
-		data |= (((power_level > 2) ? (power_level) : 2) << (i * 8));
-	}
-	rtl_set_bbreg(hw, offset, 0xffffff, data);
-}
-
-void rtl8821ae_phy_set_txpower_level_by_path(struct ieee80211_hw *hw, u8 channel, u8 path)
-{
-	//struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
-	u8 cck_rates[]  = {DESC_RATE1M, DESC_RATE2M, DESC_RATE5_5M, DESC_RATE11M};
-	u8 ofdm_rates[]  = {DESC_RATE6M, DESC_RATE9M, DESC_RATE12M, DESC_RATE18M,
-						DESC_RATE24M, DESC_RATE36M, DESC_RATE48M, DESC_RATE54M};
-	u8 ht_rates_1t[]  = {DESC_RATEMCS0, DESC_RATEMCS1, DESC_RATEMCS2, DESC_RATEMCS3,
-						 DESC_RATEMCS4, DESC_RATEMCS5, DESC_RATEMCS6, DESC_RATEMCS7};
-	u8 ht_rates_2t[]  = {DESC_RATEMCS8, DESC_RATEMCS9, DESC_RATEMCS10, DESC_RATEMCS11,
-						 DESC_RATEMCS12, DESC_RATEMCS13, DESC_RATEMCS14, DESC_RATEMCS15};
-	u8 vht_rates_1t[]  = {DESC_RATEVHT1SS_MCS0, DESC_RATEVHT1SS_MCS1, DESC_RATEVHT1SS_MCS2,
-						DESC_RATEVHT1SS_MCS3, DESC_RATEVHT1SS_MCS4,
-						  DESC_RATEVHT1SS_MCS5, DESC_RATEVHT1SS_MCS6, DESC_RATEVHT1SS_MCS7,
-						  DESC_RATEVHT1SS_MCS8, DESC_RATEVHT1SS_MCS9};
-	u8 vht_rates_2t[]  = {DESC_RATEVHT2SS_MCS0, DESC_RATEVHT2SS_MCS1, DESC_RATEVHT2SS_MCS2,
-						DESC_RATEVHT2SS_MCS3, DESC_RATEVHT2SS_MCS4,
-						  DESC_RATEVHT2SS_MCS5, DESC_RATEVHT2SS_MCS6, DESC_RATEVHT2SS_MCS7,
-						  DESC_RATEVHT2SS_MCS8, DESC_RATEVHT2SS_MCS9};
-	//u8 i,size;
-	//u8 power_index;
-
-	if (rtlhal->current_bandtype == BAND_ON_2_4G)
-		_rtl8821ae_phy_set_txpower_level_by_path(hw,cck_rates,path,channel,
-		                                         sizeof(cck_rates) / sizeof(u8));
-
-	_rtl8821ae_phy_set_txpower_level_by_path(hw,ofdm_rates,path,channel,
-		                                     sizeof(ofdm_rates) / sizeof(u8));
-	_rtl8821ae_phy_set_txpower_level_by_path(hw,ht_rates_1t,path,channel,
-		                                     sizeof(ht_rates_1t) / sizeof(u8));
-	_rtl8821ae_phy_set_txpower_level_by_path(hw,vht_rates_1t,path,channel,
-		                                     sizeof(vht_rates_1t) / sizeof(u8));
-
-	if (rtlphy->num_total_rfpath >= 2) {
-		_rtl8821ae_phy_set_txpower_level_by_path(hw,ht_rates_2t,path,channel,
-			                                     sizeof(ht_rates_2t) / sizeof(u8));
-		_rtl8821ae_phy_set_txpower_level_by_path(hw,vht_rates_2t,path,channel,
-			                                     sizeof(vht_rates_2t) / sizeof(u8));
-	}
-
-	_rtl8821ae_phy_txpower_training_by_path(hw, rtlphy->current_chan_bw, channel, path);
-}
-/*just in case, write txpower in DW, to reduce time*/
-#if 0
-void _rtl8821ae_phy_get_txpower_index_by_rate_array(struct ieee80211_hw *hw, u8 channel,
-											u8 *rate, u8 path, u8 bw, u8 *power_index, u8 size)
-{
-	u8 i;
-	for (i = 0; i < size; i++)
-		power_index[i] = _rtl8821ae_get_txpower_index(hw, path, rate[i], bw, channel);
-}
-
-void rtl8821ae_phy_set_txpower_level_by_path2(struct ieee80211_hw *hw, u8 channel, u8 path)
-{
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtl_priv(hw)->phy);
-	u8 cck_rates[]  = {DESC_RATE1M, DESC_RATE2M, DESC_RATE5_5M, DESC_RATE11M};
-	u8 ofdm_rates[]  = {DESC_RATE6M, DESC_RATE9M, DESC_RATE12M, DESC_RATE18M,
-						DESC_RATE24M, DESC_RATE36M, DESC_RATE48M, DESC_RATE54M};
-	u8 ht_rates_1t[]  = {DESC_RATEMCS0, DESC_RATEMCS1, DESC_RATEMCS2, DESC_RATEMCS3,
-						 DESC_RATEMCS4, DESC_RATEMCS5, DESC_RATEMCS6, DESC_RATEMCS7};
-	u8 ht_rates_2t[]  = {DESC_RATEMCS8, DESC_RATEMCS9, DESC_RATEMCS10, DESC_RATEMCS11,
-						 DESC_RATEMCS12, DESC_RATEMCS13, DESC_RATEMCS14, DESC_RATEMCS15};
-	u8 vht_rates_1t[]  = {DESC_RATEVHT1SS_MCS0, DESC_RATEVHT1SS_MCS1, DESC_RATEVHT1SS_MCS2, DESC_RATEVHT1SS_MCS3, DESC_RATEVHT1SS_MCS4,
-						  DESC_RATEVHT1SS_MCS5, DESC_RATEVHT1SS_MCS6, DESC_RATEVHT1SS_MCS7, DESC_RATEVHT1SS_MCS8, DESC_RATEVHT1SS_MCS9};
-	u8 vht_rates_2t[]  = {DESC_RATEVHT2SS_MCS0, DESC_RATEVHT2SS_MCS1, DESC_RATEVHT2SS_MCS2, DESC_RATEVHT2SS_MCS3, DESC_RATEVHT2SS_MCS4,
-						  DESC_RATEVHT2SS_MCS5, DESC_RATEVHT2SS_MCS6, DESC_RATEVHT2SS_MCS7, DESC_RATEVHT2SS_MCS8, DESC_RATEVHT2SS_MCS9};
-	u8 i, j;
-	u8 pwridx[48] = {0};
-	u8 cs = sizeof(cck_rates) / sizeof(u8);
-	u8 os = sizeof(ofdm_rates) / sizeof(u8);
-	u8 h1s = sizeof(ht_rates_1t) / sizeof(u8);
-	u8 h2s = sizeof(ht_rates_2t) / sizeof(u8);
-	u8 v1s = sizeof(vht_rates_1t) / sizeof(u8);
-	u8 v2s = sizeof(vht_rates_2t) / sizeof(u8);
-
-	u8 len, start;
-	u32 reg_addr, power_index;
-	u8 bw = rtlphy->current_chan_bw;
-
-	_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
-		ofdm_rates, path, bw, &pwridx[cs], os);
-
-	_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
-		ht_rates_1t, path, bw, &pwridx[cs+os], h1s);
-
-	_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
-		vht_rates_1t, path, bw, &pwridx[cs+os+h1s+h2s], v1s);
-
-
-	if (rtlhal->current_bandtype == BAND_ON_2_4G) {
-		_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
-			cck_rates, path, bw, pwridx, cs);
-
-		start = 0;
-	} else {
-		start = cs;
-	}
-
-	reg_addr = (path == 0) ? RTXAGC_A_CCK11_CCK1 : RTXAGC_B_CCK11_CCK1;
-	reg_addr += start;
-
-	len = cs + os + h1s + h2s + v1s;
-	if (rtlphy->num_total_rfpath >= 2) {
-		_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
-			ht_rates_2t, path, bw, &pwridx[cs+os+h1s], h2s);
-
-		_rtl8821ae_phy_get_txpower_index_by_rate_array(hw, channel,
-			vht_rates_2t, path, bw, &pwridx[cs+os+h1s+h2s+v1s], v2s);
-
-		len += v2s;
-	}
-	for (i = start; i < len; i += 4) {
-		power_index = 0;
-		for (j = 0; j < 4; j++)
-			power_index |= (pwridx[i+j] << (j*8));
-		rtl_set_bbreg(hw, reg_addr + i, MASKDWORD, power_index);
-	}
-
-	_rtl8821ae_phy_txpower_training_by_path(hw, rtlphy->current_chan_bw, channel, path);
-}
-#endif
-
-void rtl8821ae_phy_set_txpower_level(struct ieee80211_hw *hw, u8 channel)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 path = 0;
-
-	for (path = RF90_PATH_A; path < rtlphy->num_total_rfpath; ++path )
-		rtl8821ae_phy_set_txpower_level_by_path(hw, channel, path);
-}
-
-static long _rtl8821ae_phy_txpwr_idx_to_dbm(struct ieee80211_hw *hw,
-					 enum wireless_mode wirelessmode,
-					 u8 txpwridx)
-{
-	long offset;
-	long pwrout_dbm;
-
-	switch (wirelessmode) {
-	case WIRELESS_MODE_B:
-		offset = -7;
-		break;
-	case WIRELESS_MODE_G:
-	case WIRELESS_MODE_N_24G:
-		offset = -8;
-		break;
-	default:
-		offset = -8;
-		break;
-	}
-	pwrout_dbm = txpwridx / 2 + offset;
-	return pwrout_dbm;
-}
-
-void rtl8821ae_phy_scan_operation_backup(struct ieee80211_hw *hw, u8 operation)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	enum io_type iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
-
-	if (!is_hal_stop(rtlhal)) {
-		switch (operation) {
-		case SCAN_OPT_BACKUP_BAND0:
-			iotype = IO_CMD_PAUSE_BAND0_DM_BY_SCAN;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_IO_CMD,
-						      (u8 *) & iotype);
-
-			break;
-		case SCAN_OPT_BACKUP_BAND1:
-			iotype = IO_CMD_PAUSE_BAND1_DM_BY_SCAN;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_IO_CMD,
-						      (u8 *) & iotype);
-
-			break;
-		case SCAN_OPT_RESTORE:
-			iotype = IO_CMD_RESUME_DM_BY_SCAN;
-			rtlpriv->cfg->ops->set_hw_reg(hw,
-						      HW_VAR_IO_CMD,
-						      (u8 *) & iotype);
-			break;
-		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("Unknown Scan Backup operation.\n"));
-			break;
-		}
-	}
-}
-
-static void _rtl8821ae_phy_set_reg_bw(struct rtl_priv * rtlpriv, u8 bw)
-{
-	u16 reg_rf_mode_bw, tmp = 0;
-	reg_rf_mode_bw = rtl_read_word(rtlpriv, REG_TRXPTCL_CTL);
-	switch (bw) {
-	case HT_CHANNEL_WIDTH_20:
-		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, reg_rf_mode_bw & 0xFE7F);
-		break;
-	case HT_CHANNEL_WIDTH_20_40:
-		tmp = reg_rf_mode_bw | BIT(7);
-		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, tmp & 0xFEFF);
-		break;
-	case HT_CHANNEL_WIDTH_80:
-		tmp = reg_rf_mode_bw | BIT(8);
-		rtl_write_word(rtlpriv, REG_TRXPTCL_CTL, tmp & 0xFF7F);
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_WARNING,("unknown Bandwidth: 0x%x\n",bw));
-		break;
-	}
-}
-
-static u8 _rtl8821ae_phy_get_secondary_chnl(struct rtl_priv * rtlpriv)
-{
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_mac *mac = rtl_mac(rtlpriv);
-	u8 sc_set_40 = 0, sc_set_20 =0;
-
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80) {
-		if(mac->cur_80_prime_sc == PRIME_CHNL_OFFSET_LOWER)
-			sc_set_40 = VHT_DATA_SC_40_LOWER_OF_80MHZ;
-		else if(mac->cur_80_prime_sc == PRIME_CHNL_OFFSET_UPPER)
-			sc_set_40 = VHT_DATA_SC_40_UPPER_OF_80MHZ;
-		else
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				("SCMapping: Not Correct Primary40MHz Setting \n"));
-
-		if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER) &&
-			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_LOWER))
-			sc_set_20 = VHT_DATA_SC_20_LOWEST_OF_80MHZ;
-		else if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER) &&
-			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_LOWER))
-			sc_set_20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
-		else if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER) &&
-			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_UPPER))
-			sc_set_20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
-		else if((mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER) &&
-			(mac->cur_80_prime_sc == HAL_PRIME_CHNL_OFFSET_UPPER))
-			sc_set_20 = VHT_DATA_SC_20_UPPERST_OF_80MHZ;
-		else
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				("SCMapping: Not Correct Primary40MHz Setting \n"));
-	} else if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40) {
-		if (mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_UPPER)
-			sc_set_20 = VHT_DATA_SC_20_UPPER_OF_80MHZ;
-		else if (mac->cur_40_prime_sc == PRIME_CHNL_OFFSET_LOWER)
-			sc_set_20 = VHT_DATA_SC_20_LOWER_OF_80MHZ;
-		else
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("SCMapping: Not Correct Primary40MHz Setting \n"));
-	}
-	return ((sc_set_40 << 4) | sc_set_20);
-}
-
-void rtl8821ae_phy_set_bw_mode_callback(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 sub_chnl = 0;
-	u8 l1pk_val = 0;
-
-	RT_TRACE(COMP_SCAN, DBG_TRACE,
-		 ("Switch to %s bandwidth\n",
-		  (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20 ?
-		  "20MHz" :
-		  (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_20_40 ?
-		  "40MHz" : "80MHz"))))
-
-
-
-	_rtl8821ae_phy_set_reg_bw(rtlpriv, rtlphy->current_chan_bw);
-	sub_chnl = _rtl8821ae_phy_get_secondary_chnl(rtlpriv);
-	rtl_write_byte(rtlpriv, 0x0483, sub_chnl);
-
-	switch (rtlphy->current_chan_bw) {
-	case HT_CHANNEL_WIDTH_20:
-		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300200);
-		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
-
-		if(rtlphy->rf_type == RF_2T2R)
-			rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, 7);
-		else
-			rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, 8);
-		break;
-	case HT_CHANNEL_WIDTH_20_40:
-		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300201);
-		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 0);
-		rtl_set_bbreg(hw, RRFMOD, 0x3C, sub_chnl);
-		rtl_set_bbreg(hw, RCCAONSEC, 0xf0000000, sub_chnl);
-
-		if(rtlphy->reg_837 & BIT(2))
-			l1pk_val = 6;
-		else
-		{
-			if(rtlphy->rf_type == RF_2T2R)
-				l1pk_val = 7;
-			else
-				l1pk_val = 8;
-		}
-		rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, l1pk_val);	// 0x848[25:22] = 0x6
-
-		if(sub_chnl == VHT_DATA_SC_20_UPPER_OF_80MHZ)
-			rtl_set_bbreg(hw, RCCK_SYSTEM, BCCK_SYSTEM, 1);
-		else
-			rtl_set_bbreg(hw, RCCK_SYSTEM, BCCK_SYSTEM, 0);
-		break;
-
-	case HT_CHANNEL_WIDTH_80:
-		rtl_set_bbreg(hw, RRFMOD, 0x003003C3, 0x00300202); // 0x8ac[21,20,9:6,1,0]=8'b11100010
-		rtl_set_bbreg(hw, RADC_BUF_CLK, BIT(30), 1);			// 0x8c4[30] = 1
-		rtl_set_bbreg(hw, RRFMOD, 0x3C, sub_chnl);
-		rtl_set_bbreg(hw, RCCAONSEC, 0xf0000000, sub_chnl);
-
-		if(rtlphy->reg_837 & BIT(2))
-			l1pk_val = 5;
-		else
-		{
-			if(rtlphy->rf_type == RF_2T2R)
-				l1pk_val = 6;
-			else
-				l1pk_val = 7;
-		}
-		rtl_set_bbreg(hw, RL1PEAKTH, 0x03C00000, l1pk_val);
-
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("unknown bandwidth: %#X\n", rtlphy->current_chan_bw));
-		break;
-	}
-
-	rtl8812ae_fixspur(hw, rtlphy->current_chan_bw, rtlphy->current_channel);
-
-	rtl8821ae_phy_rf6052_set_bandwidth(hw, rtlphy->current_chan_bw);
-	rtlphy->set_bwmode_inprogress = false;
-
-	RT_TRACE(COMP_SCAN, DBG_LOUD, (" \n"));
-}
-
-void rtl8821ae_phy_set_bw_mode(struct ieee80211_hw *hw,
-			    enum nl80211_channel_type ch_type)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u8 tmp_bw = rtlphy->current_chan_bw;
-
-	if (rtlphy->set_bwmode_inprogress)
-		return;
-	rtlphy->set_bwmode_inprogress = true;
-	if ((!is_hal_stop(rtlhal)) && !(RT_CANNOT_IO(hw))) {
-		rtl8821ae_phy_set_bw_mode_callback(hw);
-	} else {
-		RT_TRACE(COMP_ERR, DBG_WARNING,
-			 ("FALSE driver sleep or unload\n"));
-		rtlphy->set_bwmode_inprogress = false;
-		rtlphy->current_chan_bw = tmp_bw;
-	}
-}
-
-void rtl8821ae_phy_sw_chnl_callback(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 channel = rtlphy->current_channel;
-	u8 path;
-	u32 data;
-
-	RT_TRACE(COMP_SCAN, DBG_TRACE,
-		 ("switch to channel%d\n", rtlphy->current_channel));
-	if (is_hal_stop(rtlhal))
-		return;
-
-	if (36 <= channel && channel <= 48)
-		data = 0x494;
-	else if (50 <= channel && channel <= 64)
-		data = 0x453;
-	else if (100 <= channel && channel <= 116)
-		data = 0x452;
-	else if (118 <= channel)
-		data = 0x412;
-	else
-		data = 0x96a;
-	rtl_set_bbreg(hw, RFC_AREA, 0x1ffe0000, data);
-
-
-	for(path = RF90_PATH_A; path < rtlphy->num_total_rfpath; path++)
-	{
-		if (36 <= channel && channel <= 64)
-			data = 0x101;
-		else if (100 <= channel && channel <= 140)
-			data = 0x301;
-		else if (140 < channel)
-			data = 0x501;
-		else
-			data = 0x000;
-		rtl8821ae_phy_set_rf_reg(hw, path, RF_CHNLBW,
-			BIT(18)|BIT(17)|BIT(16)|BIT(9)|BIT(8), data);
-
-		rtl8821ae_phy_set_rf_reg(hw, path, RF_CHNLBW,
-			BMASKBYTE0, channel);
-
-		if (channel > 14) {
-			if (rtlhal->hw_type == HARDWARE_TYPE_RTL8821AE) {
-				if (36 <= channel && channel <= 64)
-					data = 0x114E9;
-				else if (100 <= channel && channel <= 140)
-					data = 0x110E9;
-				else
-					data = 0x110E9;
-				rtl8821ae_phy_set_rf_reg(hw, path, RF_APK,
-					BRFREGOFFSETMASK, data);
-			}
-		}
-	}
-	RT_TRACE(COMP_SCAN, DBG_TRACE, ("\n"));
-}
-
-u8 rtl8821ae_phy_sw_chnl(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	u32 timeout = 1000, timecount = 0;
-	u8 channel = rtlphy->current_channel;
-
-	if (rtlphy->sw_chnl_inprogress)
-		return 0;
-	if (rtlphy->set_bwmode_inprogress)
-		return 0;
-
-	if ((is_hal_stop(rtlhal)) || (RT_CANNOT_IO(hw))) {
-		RT_TRACE(COMP_CHAN, DBG_LOUD,
-			 ("sw_chnl_inprogress false driver sleep or unload\n"));
-		return 0;
-	}
-	while (rtlphy->lck_inprogress && timecount < timeout) {
-		mdelay(50);
-		timecount += 50;
-	}
-
-	if (rtlphy->current_channel > 14 && rtlhal->current_bandtype != BAND_ON_5G)
-		rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_5G);
-	else if (rtlphy->current_channel <= 14 && rtlhal->current_bandtype != BAND_ON_2_4G)
-		rtl8821ae_phy_switch_wirelessband(hw, BAND_ON_2_4G);
-
-	rtlphy->sw_chnl_inprogress = true;
-	if (channel == 0)
-		channel = 1;
-
-	RT_TRACE(COMP_SCAN, DBG_TRACE,
-		 ("switch to channel%d, band type is %d\n", rtlphy->current_channel, rtlhal->current_bandtype));
-
-	rtl8821ae_phy_sw_chnl_callback(hw);
-
-	rtl8821ae_dm_clear_txpower_tracking_state(hw);
-	rtl8821ae_phy_set_txpower_level(hw, rtlphy->current_channel);
-
-	RT_TRACE(COMP_SCAN, DBG_TRACE, ("\n"));
-	rtlphy->sw_chnl_inprogress = false;
-	return 1;
-}
-
-#if 0
-static u8 _rtl8821ae_phy_path_b_iqk(struct ieee80211_hw *hw)
-{
-	u32 reg_eac, reg_eb4, reg_ebc, reg_ec4, reg_ecc;
-	u8 result = 0x00;
-
-	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000002);
-	rtl_set_bbreg(hw, 0xe60, MASKDWORD, 0x00000000);
-	mdelay(IQK_DELAY_TIME);
-	reg_eac = rtl_get_bbreg(hw, 0xeac, MASKDWORD);
-	reg_eb4 = rtl_get_bbreg(hw, 0xeb4, MASKDWORD);
-	reg_ebc = rtl_get_bbreg(hw, 0xebc, MASKDWORD);
-	reg_ec4 = rtl_get_bbreg(hw, 0xec4, MASKDWORD);
-	reg_ecc = rtl_get_bbreg(hw, 0xecc, MASKDWORD);
-
-	if (!(reg_eac & BIT(31)) &&
-	    (((reg_eb4 & 0x03FF0000) >> 16) != 0x142) &&
-	    (((reg_ebc & 0x03FF0000) >> 16) != 0x42))
-		result |= 0x01;
-	else
-		return result;
-	if (!(reg_eac & BIT(30)) &&
-	    (((reg_ec4 & 0x03FF0000) >> 16) != 0x132) &&
-	    (((reg_ecc & 0x03FF0000) >> 16) != 0x36))
-		result |= 0x02;
-	return result;
-}
-
-static u8 _rtl8821ae_phy_path_a_rx_iqk(struct ieee80211_hw *hw, bool config_pathb)
-{
-	u32 reg_eac, reg_e94, reg_e9c, reg_ea4,u32temp;
-	u8 result = 0x00;
-
-	/*Get TXIMR Setting*/
-	/*Modify RX IQK mode table*/
-	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf117b);
-	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
-
-	/*IQK Setting*/
-	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, 0x01007c00);
-	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x81004800);
-
-	/*path a IQK setting*/
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x10008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x30008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160804);
-	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x28160000);
-
-	/*LO calibration Setting*/
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
-	/*one shot,path A LOK & iqk*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf9000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
-
-	mdelay(IQK_DELAY_TIME);
-
-	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
-	reg_e94 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_A, MASKDWORD);
-	reg_e9c = rtl_get_bbreg(hw, RTx_Power_After_IQK_A, MASKDWORD);
-
-
-	if (!(reg_eac & BIT(28)) &&
-	    (((reg_e94 & 0x03FF0000) >> 16) != 0x142) &&
-	    (((reg_e9c & 0x03FF0000) >> 16) != 0x42))
-		result |= 0x01;
-	else
-		return result;
-
-	u32temp = 0x80007C00 | (reg_e94&0x3FF0000)  | ((reg_e9c&0x3FF0000) >> 16);
-	rtl_set_bbreg(hw, RTx_IQK, MASKDWORD, u32temp);
-	/*RX IQK*/
-	/*Modify RX IQK mode table*/
-	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x00000000);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_WE_LUT, RFREG_OFFSET_MASK, 0x800a0);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_RCK_OS, RFREG_OFFSET_MASK, 0x30000);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G1, RFREG_OFFSET_MASK, 0x0000f);
-	rtl_set_rfreg(hw, RF90_PATH_A, RF_TXPA_G2, RFREG_OFFSET_MASK, 0xf7ffa);
-	rtl_set_bbreg(hw, RFPGA0_IQK, MASKDWORD, 0x80800000);
-
-	/*IQK Setting*/
-	rtl_set_bbreg(hw, RRx_IQK, MASKDWORD, 0x01004800);
-
-	/*path a IQK setting*/
-	rtl_set_bbreg(hw, RTx_IQK_Tone_A, MASKDWORD, 0x30008c1c);
-	rtl_set_bbreg(hw, RRx_IQK_Tone_A, MASKDWORD, 0x10008c1c);
-	rtl_set_bbreg(hw, RTx_IQK_PI_A, MASKDWORD, 0x82160c05);
-	rtl_set_bbreg(hw, RRx_IQK_PI_A, MASKDWORD, 0x28160c05);
-
-	/*LO calibration Setting*/
-	rtl_set_bbreg(hw, RIQK_AGC_Rsp, MASKDWORD, 0x0046a911);
-	/*one shot,path A LOK & iqk*/
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf9000000);
-	rtl_set_bbreg(hw, RIQK_AGC_Pts, MASKDWORD, 0xf8000000);
-
-	mdelay(IQK_DELAY_TIME);
-
-	reg_eac = rtl_get_bbreg(hw, RRx_Power_After_IQK_A_2, MASKDWORD);
-	reg_e94 = rtl_get_bbreg(hw, RTx_Power_Before_IQK_A, MASKDWORD);
-	reg_e9c = rtl_get_bbreg(hw, RTx_Power_After_IQK_A, MASKDWORD);
-	reg_ea4 = rtl_get_bbreg(hw, RRx_Power_Before_IQK_A_2, MASKDWORD);
-
-	if (!(reg_eac & BIT(27)) &&
-	    (((reg_ea4 & 0x03FF0000) >> 16) != 0x132) &&
-	    (((reg_eac & 0x03FF0000) >> 16) != 0x36))
-		result |= 0x02;
-	return result;
-}
-#endif
-
-u8 _rtl8812ae_get_right_chnl_place_for_iqk(u8 chnl)
-{
-	u8 channel_all[TARGET_CHNL_NUM_2G_5G_8812] =
-		{1,2,3,4,5,6,7,8,9,10,11,12,13,14,36,38,40,42,\
-		44,46,48,50,52,54,56,58,60,62,64,100,\
-		102,104,106,108,110,112,114,116,118,\
-		120,122,124,126,128,130,132,134,136,\
-		138,140,149,151,153,155,157,159,161,\
-		163,165};
-	u8 place = chnl;
-
-	if(chnl > 14)
-	{
-		for(place = 14; place<sizeof(channel_all); place++)
-		{
-			if(channel_all[place] == chnl)
-			{
-				return place-13;
-			}
-		}
-	}
-
-	return 0;
-}
-
-void _rtl8812ae_iqk_rx_fill_iqc(
-	struct ieee80211_hw *hw,
-	enum radio_path path,
-	u32 rx_x,
-	u32 rx_y
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	switch (path) {
-	case RF90_PATH_A:
-		{
-			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-			if (rx_x >> 1 ==0x112 || rx_y >> 1 == 0x3ee){
-				rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x100);
-				rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0);
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("RX_X = %x;;RX_Y = %x ====>fill to IQC\n",
-					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
-			}
-			else{
-				rtl_set_bbreg(hw, 0xc10, 0x000003ff, rx_x >> 1);
-				rtl_set_bbreg(hw, 0xc10, 0x03ff0000, rx_y >> 1);
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("RX_X = %x;;RX_Y = %x ====>fill to IQC\n",
-					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("0xc10 = %x ====>fill to IQC\n",
-					rtl_read_dword(rtlpriv, 0xc10)));
-			}
-		}
-		break;
-	case RF90_PATH_B:
-		{
-			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-			if (rx_x >> 1 ==0x112 || rx_y >> 1 == 0x3ee){
-				rtl_set_bbreg(hw, 0xe10, 0x000003ff, 0x100);
-				rtl_set_bbreg(hw, 0xe10, 0x03ff0000, 0);
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("RX_X = %x;;RX_Y = %x ====>fill to IQC\n",
-					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
-			}
-			else{
-				rtl_set_bbreg(hw, 0xe10, 0x000003ff, rx_x >> 1);
-				rtl_set_bbreg(hw, 0xe10, 0x03ff0000, rx_y >> 1);
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("RX_X = %x;;RX_Y = %x====>fill to IQC\n ",
-					rx_x >> 1 & 0x000003ff, rx_y >> 1 & 0x000003ff));
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("0xe10 = %x====>fill to IQC\n",
-					rtl_read_dword(rtlpriv, 0xe10)));
-			}
-		}
-		break;
-	default:
-		break;
-	};
-}
-
-void _rtl8812ae_iqk_tx_fill_iqc(
-	struct ieee80211_hw *hw,
-	enum radio_path path,
-	u32 tx_x,
-	u32 tx_y
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	switch (path) {
-	case RF90_PATH_A:
-		{
-			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
-			rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
-			rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
-			rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
-			rtl_set_bbreg(hw, 0xccc, 0x000007ff, tx_y);
-			rtl_set_bbreg(hw, 0xcd4, 0x000007ff, tx_x);
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("TX_X = %x;;TX_Y = %x =====> fill to IQC\n",
-				tx_x & 0x000007ff, tx_y & 0x000007ff));
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("0xcd4 = %x;;0xccc = %x ====>fill to IQC\n",
-				rtl_get_bbreg(hw, 0xcd4, 0x000007ff),
-				rtl_get_bbreg(hw, 0xccc, 0x000007ff)));
-		}
-		break;
-	case RF90_PATH_B:
-		{
-			rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
-			rtl_write_dword(rtlpriv, 0xe90, 0x00000080);
-			rtl_write_dword(rtlpriv, 0xec4, 0x20040000);
-			rtl_write_dword(rtlpriv, 0xec8, 0x20000000);
-			rtl_set_bbreg(hw, 0xecc, 0x000007ff, tx_y);
-			rtl_set_bbreg(hw, 0xed4, 0x000007ff, tx_x);
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("TX_X = %x;;TX_Y = %x =====> fill to IQC\n",
-				tx_x&0x000007ff, tx_y&0x000007ff));
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("0xed4 = %x;;0xecc = %x ====>fill to IQC\n",
-				rtl_get_bbreg(hw, 0xed4, 0x000007ff),
-				rtl_get_bbreg(hw, 0xecc, 0x000007ff)));
-		}
-		break;
-	default:
-		break;
-	};
-}
-
-void _rtl8812ae_iqk_backup_macbb(
-	struct ieee80211_hw *hw,
-	u32 *macbb_backup,
-	u32 *backup_macbb_reg,
-	u32 mac_bb_num
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	 /*save MACBB default value*/
-	for (i = 0; i < mac_bb_num; i++) {
-		macbb_backup[i] =rtl_read_dword(rtlpriv,backup_macbb_reg[i]);
-	}
-
-	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupMacBB Success!!!!\n"));
-}
-
-void _rtl8812ae_iqk_backup_afe(
-	struct ieee80211_hw *hw,
-	u32 *afe_backup,
-	u32 *backup_afe_REG,
-	u32 afe_num
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	/*Save AFE Parameters */
-    	for (i = 0; i < afe_num; i++){
-        	afe_backup[i] = rtl_read_dword(rtlpriv, backup_afe_REG[i]);
-    	}
-    	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupAFE Success!!!!\n"));
-}
-
-void _rtl8812ae_iqk_backup_rf(
-	struct ieee80211_hw *hw,
-	u32 *rfa_backup,
-	u32 *rfb_backup,
-	u32 *backup_rf_reg,
-	u32 rf_num
-	)
-{
-
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	/*Save RF Parameters*/
-    	for (i = 0; i < rf_num; i++){
-        	rfa_backup[i] = rtl_get_rfreg(hw, RF90_PATH_A, backup_rf_reg[i], BMASKDWORD);
-        	rfb_backup[i] = rtl_get_rfreg(hw, RF90_PATH_B, backup_rf_reg[i], BMASKDWORD);
-    	}
-	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupRF Success!!!!\n"));
-}
-
-void _rtl8812ae_iqk_configure_mac(
-	struct ieee80211_hw *hw
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	/* ========MAC register setting========*/
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	rtl_write_byte(rtlpriv, 0x522, 0x3f);
-	rtl_set_bbreg(hw, 0x550, BIT(11) | BIT(3), 0x0);
-	rtl_write_byte(rtlpriv, 0x808, 0x00);		/*RX ante off*/
-	rtl_set_bbreg(hw, 0x838, 0xf, 0xc);		/*CCA off*/
-}
-
-#define cal_num 10
-
-void _rtl8812ae_iqk_tx(
-	struct ieee80211_hw *hw,
-	u8 chnl_idx
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	u8 delay_count;
-	u8 cal0_retry, cal1_retry;
-	u8 tx0_average = 0, tx1_average = 0, rx0_average = 0, rx1_average = 0;
-    	int tx0_x = 0, tx0_y = 0, rx0_x = 0, rx0_y = 0;
-	int tx_x0[cal_num], tx_y0[cal_num], rx_x0[cal_num], rx_y0[cal_num];
-	int tx1_x = 0, tx1_y = 0, rx1_x = 0, rx1_y = 0;
-	int tx_x1[cal_num], tx_y1[cal_num], rx_x1[cal_num], rx_y1[cal_num];
-	bool tx0iqkok= false, rx0iqkok = false, tx0_fail = true, rx0_fail;
-	bool iqk0_ready = false, tx0_finish = false, rx0_finish = false;
-	bool tx1iqkok = false, rx1iqkok = false, tx1_fail = true, rx1_fail;
-	bool iqk1_ready = false, tx1_finish = false, rx1_finish = false, vdf_enable = false;
-	int i, tx_dt[3] = {0}, rx_dt[3] = {0}, ii, dx = 0, dy = 0;
-
-	RT_TRACE(COMP_IQK, DBG_LOUD,
-			("BandWidth = %d.\n",
-			rtlphy->current_chan_bw));
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80){
-		vdf_enable = true;
-	}
-	vdf_enable = false;
-
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	/*========Path-A AFE all on========*/
-	/*Port 0 DAC/ADC on*/
-	rtl_write_dword(rtlpriv, 0xc60, 0x77777777);
-	rtl_write_dword(rtlpriv, 0xc64, 0x77777777);
-
-	/* Port 1 DAC/ADC off*/
-	rtl_write_dword(rtlpriv, 0xe60, 0x77777777);
-	rtl_write_dword(rtlpriv, 0xe64, 0x77777777);
-
-	rtl_write_dword(rtlpriv, 0xc68, 0x19791979);
-	rtl_write_dword(rtlpriv, 0xe68, 0x19791979);
-	rtl_set_bbreg(hw,0xc00, 0xf, 0x4);/*hardware 3-wire off*/
-	rtl_set_bbreg(hw,0xe00, 0xf, 0x4);/*hardware 3-wire off*/
-
-	/*DAC/ADC sampling rate (160 MHz)*/
-	rtl_set_bbreg(hw, 0xc5c, BIT(26) | BIT(25) | BIT(24), 0x7);
-	rtl_set_bbreg(hw, 0xe5c, BIT(26) | BIT(25) | BIT(24), 0x7);
-	rtl_set_bbreg(hw, 0x8c4, BIT(30), 0x1);
-
-        /*====== Path A TX IQK RF Setting ======*/
-	rtl_set_bbreg(hw,0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-	rtl_set_rfreg(hw,RF90_PATH_A, 0xef, BRFREGOFFSETMASK, 0x80002);
-	rtl_set_rfreg(hw,RF90_PATH_A, 0x30, BRFREGOFFSETMASK, 0x20000);
-	rtl_set_rfreg(hw,RF90_PATH_A, 0x31, BRFREGOFFSETMASK, 0x3fffd);
-	rtl_set_rfreg(hw,RF90_PATH_A, 0x32, BRFREGOFFSETMASK, 0xfe83f);
-	rtl_set_rfreg(hw,RF90_PATH_A, 0x65, BRFREGOFFSETMASK, 0x931d5);
-	rtl_set_rfreg(hw,RF90_PATH_A, 0x8f, BRFREGOFFSETMASK, 0x8a001);
-	/*====== Path A TX IQK RF Setting ======*/
-	rtl_set_rfreg(hw,RF90_PATH_B, 0xef, BRFREGOFFSETMASK, 0x80002);
-	rtl_set_rfreg(hw,RF90_PATH_B, 0x30, BRFREGOFFSETMASK, 0x20000);
-	rtl_set_rfreg(hw,RF90_PATH_B, 0x31, BRFREGOFFSETMASK, 0x3fffd);
-	rtl_set_rfreg(hw,RF90_PATH_B, 0x32, BRFREGOFFSETMASK, 0xfe83f);
-	rtl_set_rfreg(hw,RF90_PATH_B, 0x65, BRFREGOFFSETMASK, 0x931d5);
-	rtl_set_rfreg(hw,RF90_PATH_B, 0x8f, BRFREGOFFSETMASK, 0x8a001);
-	rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-	rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
-	rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
-	rtl_set_bbreg(hw, 0xe94, BIT(0), 0x1);
-	rtl_write_dword(rtlpriv, 0x978, 0x29002000);/* TX (X,Y)*/
-	rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);/* RX (X,Y)*/
-	rtl_write_dword(rtlpriv, 0x984, 0x00462910);/*[0]:AGC_en, [15]:idac_K_Mask*/
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1*/
-
-	/*ExternalPA_5G == 0*/
-	rtl_write_dword(rtlpriv, 0xc88, 0x821403f1);
-	rtl_write_dword(rtlpriv, 0xe88, 0x821403f1);
-
-	if (rtlhal->current_bandtype){
-		rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
-		rtl_write_dword(rtlpriv, 0xe8c, 0x68163e96);
-	}
-	else{
-		rtl_write_dword(rtlpriv, 0xc8c, 0x28163e96);
-		rtl_write_dword(rtlpriv, 0xe8c, 0x28163e96);
-	}
-
-	if (vdf_enable){}
-	else{
-		rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
-		rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
-		rtl_write_dword(rtlpriv, 0xce8, 0x00000000);
-		rtl_write_dword(rtlpriv, 0xe80, 0x18008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
-		rtl_write_dword(rtlpriv, 0xe84, 0x38008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
-		rtl_write_dword(rtlpriv, 0xee8, 0x00000000);
-
-		cal0_retry = 0;
-		cal1_retry = 0;
-		while(1){
-			/*one shot*/
-			rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);/* cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
-			rtl_write_dword(rtlpriv, 0xeb8, 0x00100000);/* cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
-			rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-			rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-			mdelay(10); /*Delay 25ms*/
-			rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-			rtl_write_dword(rtlpriv, 0xeb8, 0x00000000);
-			delay_count = 0;
-			while (1){
-				if (!tx0_finish)
-					iqk0_ready = (bool) rtl_get_bbreg(hw, 0xd00, BIT(10));
-				if (!tx1_finish)
-					iqk1_ready = (bool) rtl_get_bbreg(hw, 0xd40, BIT(10));
-				if ((iqk0_ready && iqk1_ready) || (delay_count>20))
-					break;
-				else{
-				mdelay(1);
-				delay_count++;
-				}
-			}
-			RT_TRACE(COMP_IQK, DBG_LOUD, ("TX delay_count = %d\n", delay_count));
-			if (delay_count < 20){							// If 20ms No Result, then cal_retry++
-				/* ============TXIQK Check==============*/
-				tx0_fail = (bool) rtl_get_bbreg(hw, 0xd00, BIT(12));
-				tx1_fail = (bool) rtl_get_bbreg(hw, 0xd40, BIT(12));
-				if (!(tx0_fail || tx0_finish)){
-					rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
-					tx_x0[tx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
-					rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
-					tx_y0[tx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
-					tx0iqkok = true;
-					RT_TRACE(COMP_IQK, DBG_LOUD,
-						("TX_X0[%d] = %x ;; TX_Y0[%d] = %x\n",
-						tx0_average, (tx_x0[tx0_average]) >> 21 & 0x000007ff,
-						tx0_average, (tx_y0[tx0_average]) >> 21 & 0x000007ff));
-
-					tx0_average++;
-			}
-			else{
-				tx0iqkok = false;
-				cal0_retry++;
-				if (cal0_retry == 10)
-					break;
-				}
-			if (!(tx1_fail || tx1_finish)){
-				rtl_write_dword(rtlpriv, 0xeb8, 0x02000000);
-				tx_x1[tx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
-				rtl_write_dword(rtlpriv, 0xeb8, 0x04000000);
-				tx_y1[tx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
-				tx1iqkok= true;
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("TX_X1[%d] = %x ;; TX_Y1[%d] = %x\n",
-					tx1_average, (tx_x1[tx1_average]) >> 21 & 0x000007ff,
-					tx1_average, (tx_y1[tx1_average]) >> 21 & 0x000007ff));
-
-				tx1_average++;
-				}
-			else{
-				tx1iqkok = false;
-				cal1_retry++;
-				if (cal1_retry == 10)
-					break;
-				}
-			}
-			else{
-				tx0iqkok = false;
-				tx1iqkok = false;
-				cal0_retry++;
-				cal1_retry++;
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("Delay 20ms TX IQK Not Ready!!!!!\n"));
-				if (cal0_retry == 10)
-					break;
-			}
-			if (tx0_average >= 2){
-				for (i = 0; i < tx0_average; i++){
-					for (ii = i+1; ii <tx0_average; ii++){
-						dx = (tx_x0[i] >> 21) - (tx_x0[ii] >> 21);
-						if (dx < 4 && dx > -4){
-							dy = (tx_y0[i]>>21) - (tx_y0[ii]>>21);
-							if (dy < 4 && dy > -4){
-								tx0_x = ((tx_x0[i] >> 21) + (tx_x0[ii] >> 21)) / 2;
-								tx0_y = ((tx_y0[i] >> 21) + (tx_y0[ii] >> 21)) / 2;
-								tx_x0[0] = tx_x0[i];
-								tx_y0[1] = tx_y0[ii];
-								RT_TRACE(COMP_IQK, DBG_LOUD,
-									("TX0_X = %x;;TX0_Y = %x\n",
-									tx0_x & 0x000007ff, tx0_y & 0x000007ff));
-								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
-									&& vdf_enable) {
-									tx_dt[0] = (tx_dt[i] + tx_dt[ii]) / 2;
-								}
-								tx0_finish = true;
-							}
-						}
-					}
-				}
-			}
-			if (tx1_average >= 2){
-				for (i = 0; i < tx1_average; i++){
-					for (ii = i+1; ii < tx1_average; ii++){
-						dx = (tx_x1[i] >> 21) - (tx_x1[ii] >> 21);
-						if (dx < 4 && dx > -4){
-							dy = (tx_y1[i] >> 21) - (tx_y1[ii] >> 21);
-							if (dy < 4 && dy > -4){
-								tx1_x = ((tx_x1[i] >> 21) + (tx_x1[ii] >> 21)) / 2;
-								tx1_y = ((tx_y1[i] >> 21) + (tx_y1[ii] >> 21)) / 2;
-								tx_x1[0] = tx_x1[i];
-								tx_y1[1] = tx_y1[ii];
-								RT_TRACE(COMP_IQK, DBG_LOUD,
-									("TX1_X = %x;;TX1_Y = %x\n",
-									tx1_x & 0x000007ff, tx1_y & 0x000007ff));
-								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
-									&& vdf_enable) {
-									tx_dt[0] = (tx_dt[i] +  tx_dt[ii]) / 2;
-								}
-								tx1_finish = true;
-							}
-						}
-					}
-				}
-			}
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("TX0_Average = %d, TX1_Average = %d\n",
-				tx0_average, tx1_average));
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("TX0_finish = %d, TX1_finish = %d\n",
-				tx0_finish, tx1_finish));
-			if (tx0_finish && tx1_finish)
-				break;
-			if ((cal0_retry + tx0_average) >= 10
-				|| (cal1_retry + tx1_average) >= 10 )
-				break;
-		}
-		RT_TRACE(COMP_IQK, DBG_LOUD,
-			("TXA_cal_retry = %d\n", cal0_retry));
-		RT_TRACE(COMP_IQK, DBG_LOUD,
-			("TXB_cal_retry = %d\n", cal1_retry));
-
-	}
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
-	rtl_set_rfreg(hw, RF90_PATH_A, 0x58, 0x7fe00,
-		rtl_get_rfreg(hw, RF90_PATH_A, 0x8, 0xffc00)); /*Load LOK*/
-	rtl_set_rfreg(hw, RF90_PATH_B, 0x58, 0x7fe00,
-		rtl_get_rfreg(hw, RF90_PATH_B, 0x8, 0xffc00)); /* Load LOK*/
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
-
-
-	if (vdf_enable) {}
-	else{
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-		if (tx0_finish) {
-			/*====== Path A RX IQK RF Setting======*/
-			rtl_set_rfreg(hw, RF90_PATH_A, 0xef, BRFREGOFFSETMASK, 0x80000);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x18, 0x00c00, 0x3);     /* BW 20M*/
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x30, BRFREGOFFSETMASK, 0x30000);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x31, BRFREGOFFSETMASK, 0x3f7ff);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x32, BRFREGOFFSETMASK, 0xfe7bf);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x8f, BRFREGOFFSETMASK, 0x88001);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0x65, BRFREGOFFSETMASK, 0x931d6);
-			rtl_set_rfreg(hw, RF90_PATH_A, 0xef, BRFREGOFFSETMASK, 0x00000);
-		}
-		if (tx1_finish){
-			/*====== Path B RX IQK RF Setting======*/
-			rtl_set_rfreg(hw, RF90_PATH_B, 0xef, BRFREGOFFSETMASK, 0x80000);
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x30, BRFREGOFFSETMASK, 0x30000);
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x31, BRFREGOFFSETMASK, 0x3f7ff);
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x32, BRFREGOFFSETMASK, 0xfe7bf);
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x8f, BRFREGOFFSETMASK, 0x88001);
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x65, BRFREGOFFSETMASK, 0x931d1);
-			rtl_set_rfreg(hw, RF90_PATH_B, 0xef, BRFREGOFFSETMASK, 0x00000);
-		}
-		rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
-		rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
-		rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-		rtl_write_dword(rtlpriv, 0x984, 0x0046a890);
-
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
-		if (tx0_finish) {
-			rtl_write_dword(rtlpriv, 0xc80, 0x38008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
-			rtl_write_dword(rtlpriv, 0xc84, 0x18008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
-			rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
-			rtl_write_dword(rtlpriv, 0xc8c, 0x28160cc0);
-		}
-		if (tx1_finish){
-			rtl_write_dword(rtlpriv, 0xe80, 0x38008c10);/*TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16*/
-			rtl_write_dword(rtlpriv, 0xe84, 0x18008c10);/*RX_Tone_idx[9:0], RxK_Mask[29]*/
-			rtl_write_dword(rtlpriv, 0xe88, 0x02140119);
-			rtl_write_dword(rtlpriv, 0xe8c, 0x28160ca0);
-		}
-              cal0_retry = 0;
-		cal1_retry = 0;
-		while(1){
-		    /* one shot*/
-		    	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-		    	if (tx0_finish){
-				rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0[rx0_average % 2]) >> 21 & 0x000007ff);
-				rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0[rx0_average % 2]) >> 21 & 0x000007ff);
-				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1*/
-				rtl_write_dword(rtlpriv, 0xcb8, 0x00300000);/*cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
-				rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);
-				mdelay(5); /*Delay 10ms*/
-		    	}
-			if (tx1_finish){
-				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-				rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x1[rx1_average % 2]) >> 21 & 0x000007ff);
-				rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y1[rx1_average % 2]) >> 21 & 0x000007ff);
-				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 1 --> Page C1*/
-				rtl_write_dword(rtlpriv, 0xeb8, 0x00300000);/*cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
-				rtl_write_dword(rtlpriv, 0xeb8, 0x00100000);/* cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module*/
-			}
-			mdelay(10); /*Delay 10ms*/
-			rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-			rtl_write_dword(rtlpriv, 0xeb8, 0x00000000);
-			delay_count = 0;
-			while (1){
-				if (!rx0_finish && tx0_finish)
-					iqk0_ready = (bool) rtl_get_bbreg(hw, 0xd00, BIT(10));
-				if (!rx1_finish && tx1_finish)
-					iqk1_ready = (bool) rtl_get_bbreg(hw, 0xd40, BIT(10));
-				if ((iqk0_ready && iqk1_ready)||(delay_count>20))
-					break;
-				else{
-					mdelay(1);
-					delay_count++;
-				}
-			}
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("RX delay_count = %d\n", delay_count));
-			if (delay_count < 20){	// If 20ms No Result, then cal_retry++
-				// ============RXIQK Check==============
-				rx0_fail = (bool) rtl_get_bbreg(hw, 0xd00, BIT(11));
-				rx1_fail = (bool) rtl_get_bbreg(hw, 0xd40, BIT(11));
-				if (!(rx0_fail || rx0_finish) && tx0_finish){
-					rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
-					rx_x0[rx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
-					rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
-					rx_y0[rx0_average] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000) << 21;
-					rx0iqkok= true;
-					RT_TRACE(COMP_IQK, DBG_LOUD,
-						("RX_X0[%d] = %x ;; RX_Y0[%d] = %x\n",
-						rx0_average, (rx_x0[rx0_average]) >> 21 & 0x000007ff,
-						rx0_average, (rx_y0[rx0_average]) >> 21 & 0x000007ff));
-
-					rx0_average++;
-				}
-				else{
-					RT_TRACE(COMP_IQK, DBG_LOUD,
-						("1. RXA_cal_retry = %d\n", cal0_retry));
-					rx0iqkok = false;
-					cal0_retry++;
-					if (cal0_retry == 10)
-					break;
-				}
-				if (!(rx1_fail || rx1_finish) && tx1_finish){
-                            		rtl_write_dword(rtlpriv, 0xeb8, 0x06000000);
-                            		rx_x1[rx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
-                            		rtl_write_dword(rtlpriv, 0xeb8, 0x08000000);
-                            		rx_y1[rx1_average] = rtl_get_bbreg(hw, 0xd40, 0x07ff0000) << 21;
-                            		rx1iqkok = true;
-					RT_TRACE(COMP_IQK, DBG_LOUD,
-						("RX_X1[%d] = %x ;; RX_Y1[%d] = %x\n",
-						rx1_average, (rx_x1[rx1_average]) >> 21 & 0x000007ff,
-						rx1_average, (rx_y1[rx1_average]) >> 21 & 0x000007ff));
-
-					rx1_average++;
-                        	}
-                        	else{
-                            		rx1iqkok= false;
-                            		cal1_retry++;
-                            		if (cal1_retry == 10)
-                                		break;
-                    		}
-
-			}
-			else{
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("2. RXA_cal_retry = %d\n", cal0_retry));
-				rx0iqkok = false;
-				rx1iqkok = false;
-				cal0_retry++;
-				cal1_retry++;
-				RT_TRACE(COMP_IQK, DBG_LOUD,
-					("Delay 20ms RX IQK Not Ready!!!!!\n"));
-			    if (cal0_retry == 10)
-			        break;
-			}
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("3. RXA_cal_retry = %d\n", cal0_retry));
-			if (rx0_average >= 2){
-				for (i = 0; i < rx0_average; i++){
-					for (ii = i+1; ii < rx0_average; ii++){
-					dx = (rx_x0[i] >> 21) - (rx_x0[ii] >> 21);
-						if (dx < 4 && dx > -4){
-						dy = (rx_y0[i] >> 21) - (rx_y0[ii] >> 21);
-							if (dy < 4 && dy > -4){
-								rx0_x = ((rx_x0[i]>>21) + (rx_x0[ii] >> 21)) / 2;
-								rx0_y = ((rx_y0[i]>>21) + (rx_y0[ii] >> 21)) / 2;
-								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
-									&& vdf_enable) {
-									rx_dt[0] = (rx_dt[i] + rx_dt[ii]) / 2;
-								}
-								rx0_finish = true;
-								break;
-							}
-						}
-					}
-				}
-			}
-			if (rx1_average >= 2){
-				for (i = 0; i < rx1_average; i++){
-					for (ii = i+1; ii < rx1_average; ii++){
-					dx = (rx_x1[i] >> 21) - (rx_x1[ii] >> 21);
-						if (dx < 4 && dx > -4){
-						dy = (rx_y1[i] >> 21) - (rx_y1[ii] >> 21);
-							if (dy < 4 && dy > -4){
-								rx1_x = ((rx_x1[i] >> 21) + (rx_x1[ii] >> 21)) / 2;
-								rx1_y = ((rx_y1[i] >> 21) + (rx_y1[ii] >> 21)) / 2;
-								if ((rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80)
-									&& vdf_enable) {
-									rx_dt[0] = (rx_dt[i] + rx_dt[ii]) / 2;
-								}
-								rx1_finish = true;
-								break;
-							}
-						}
-					}
-				}
-			}
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("RX0_Average = %d, RX1_Average = %d\n",
-				rx0_average, rx1_average));
-			RT_TRACE(COMP_IQK, DBG_LOUD,
-				("RX0_finish = %d, RX1_finish = %d\n",
-				rx0_finish, rx1_finish));
-			if ((rx0_finish|| !tx0_finish) && (rx1_finish || !tx1_finish) )
-				break;
-			if ((cal0_retry + rx0_average) >= 10
-				|| (cal1_retry + rx1_average) >= 10
-				|| rx0_average == 3
-				|| rx1_average == 3)
-				break;
-		}
-		RT_TRACE(COMP_IQK, DBG_LOUD,
-			("RXA_cal_retry = %d\n", cal0_retry));
-		RT_TRACE(COMP_IQK, DBG_LOUD,
-			("RXB_cal_retry = %d\n", cal1_retry));
-	}
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
-	switch (rtlphy->current_chan_bw)
-	{
-	case HT_CHANNEL_WIDTH_20_40:
-		{
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x18, 0x00c00, 0x1);
-		}
-		break;
-	case HT_CHANNEL_WIDTH_80:
-		{
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x18, 0x00c00, 0x0);
-		}
-		break;
-	default:
-		break;
-
-	}
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 0 --> Page C*/
-	/*FillIQK Result*/
-	RT_TRACE(COMP_IQK, DBG_LOUD,
-		("========Path_A =======\n"));
-
-	if (tx0_finish){
-		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_A, tx0_x, tx0_y);
-	}
-	else{
-		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_A, 0x200, 0x0);
-	}
-
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
-		|| vdf_enable){
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 0 --> Page C*/
-		rtl_set_bbreg(hw, 0xce8, 0x3fff0000, tx_dt[0] & 0x00003fff);
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	}
-
-	if (rx0_finish == 1){
-		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_A, rx0_x, rx0_y);
-	}
-	else{
-		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_A, 0x200, 0x0);
-	}
-
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
-		|| vdf_enable){
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /*[31] = 0 --> Page C*/
-		rtl_set_bbreg(hw, 0xce8, 0x00003fff, rx_dt[0] & 0x00003fff);
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
-	}
-
-	RT_TRACE(COMP_IQK, DBG_LOUD,
-		("========Path_B =======\n"));
-
-	if (tx1_finish){
-		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_B, tx1_x, tx1_y);
-	}
-	else{
-		_rtl8812ae_iqk_tx_fill_iqc(hw, RF90_PATH_B, 0x200, 0x0);
-	}
-
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
-		|| vdf_enable){
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 0 --> Page C*/
-		rtl_set_bbreg(hw, 0xee8, 0x3fff0000, tx_dt[0] & 0x00003fff);
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
-	}
-
-	if (rx1_finish == 1){
-		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_B, rx1_x, rx1_y);
-	}
-	else{
-		_rtl8812ae_iqk_rx_fill_iqc(hw, RF90_PATH_B, 0x200, 0x0);
-	}
-
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80
-		|| vdf_enable){
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 0 --> Page C*/
-		rtl_set_bbreg(hw, 0xee8, 0x00003fff, rx_dt[0] & 0x00003fff);
-		rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
-	}
-}
-
-void _rtl8812ae_iqk_restore_rf(
-	struct ieee80211_hw *hw,
-	enum radio_path path,
-	u32 *backup_rf_reg,
-	u32 *rf_backup,
-	u32 rf_reg_num
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-    	for (i = 0; i < rf_reg_num; i++)
-        	rtl_set_rfreg(hw, path, backup_rf_reg[i], BRFREGOFFSETMASK, rf_backup[i]);
-
-	rtl_set_rfreg(hw, path, 0xef, BRFREGOFFSETMASK, 0x0);
-
-	switch(path){
-	case RF90_PATH_A:
-       {
-       	RT_TRACE(COMP_IQK, DBG_LOUD,
-			("RestoreRF Path A Success!!!!\n"));
-       }
-		break;
-	case RF90_PATH_B:
-       {
-       	RT_TRACE(COMP_IQK, DBG_LOUD,
-			("RestoreRF Path B Success!!!!\n"));
-       }
-		break;
-	default:
-		break;
-	}
-}
-
-void _rtl8812ae_iqk_restore_afe(
-	struct ieee80211_hw *hw,
-	u32 *afe_backup,
-	u32 *backup_afe_reg,
-	u32 afe_num
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	/*Reload AFE Parameters */
-    	for (i = 0; i < afe_num; i++){
-        	rtl_write_dword(rtlpriv, backup_afe_reg[i], afe_backup[i]);
-    	}
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); /* [31] = 1 --> Page C1*/
-	rtl_write_dword(rtlpriv, 0xc80, 0x0);
-	rtl_write_dword(rtlpriv, 0xc84, 0x0);
-	rtl_write_dword(rtlpriv, 0xc88, 0x0);
-	rtl_write_dword(rtlpriv, 0xc8c, 0x3c000000);
-	rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
-	rtl_write_dword(rtlpriv, 0xc94, 0x00000000);
-	rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
-	rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
-	rtl_write_dword(rtlpriv, 0xcb8, 0x0);
-	rtl_write_dword(rtlpriv, 0xe80, 0x0);
-	rtl_write_dword(rtlpriv, 0xe84, 0x0);
-	rtl_write_dword(rtlpriv, 0xe88, 0x0);
-	rtl_write_dword(rtlpriv, 0xe8c, 0x3c000000);
-	rtl_write_dword(rtlpriv, 0xe90, 0x00000080);
-	rtl_write_dword(rtlpriv, 0xe94, 0x00000000);
-	rtl_write_dword(rtlpriv, 0xec4, 0x20040000);
-	rtl_write_dword(rtlpriv, 0xec8, 0x20000000);
-	rtl_write_dword(rtlpriv, 0xeb8, 0x0);
-	RT_TRACE(COMP_IQK, DBG_LOUD,
-		("RestoreAFE Success!!!!\n"));
-}
-
-void _rtl8812ae_iqk_restore_macbb(
-	struct ieee80211_hw *hw,
-	u32 *macbb_backup,
-	u32 *backup_macbb_reg,
-	u32 macbb_num
-	)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /* [31] = 0 --> Page C*/
-	//Reload MacBB Parameters
-    	for (i = 0; i < macbb_num; i++){
-        	rtl_write_dword(rtlpriv, backup_macbb_reg[i], macbb_backup[i]);
-    	}
-    	RT_TRACE(COMP_IQK, DBG_LOUD,
-			("RestoreMacBB Success!!!!\n"));
-}
-
-#define MACBB_REG_NUM 10
-#define AFE_REG_NUM 14
-#define RF_REG_NUM 3
-
-static void _rtl8812ae_phy_iq_calibrate(
-		struct ieee80211_hw *hw,
-		u8 channel)
-{
-	u32	macbb_backup[MACBB_REG_NUM];
-	u32 afe_backup[AFE_REG_NUM];
-	u32 rfa_backup[RF_REG_NUM];
-	u32 rfb_backup[RF_REG_NUM];
-	u32 	backup_macbb_reg[MACBB_REG_NUM] = {0xb00, 0x520, 0x550,
-											0x808, 0x90c, 0xc00, 0xe00,
-											0x8c4,0x838,  0x82c};
-	u32	backup_afe_reg[AFE_REG_NUM] = {0xc5c, 0xc60, 0xc64, 0xc68,
-										0xcb8, 0xcb0, 0xcb4,0xe5c,
-										0xe60, 0xe64, 0xe68, 0xeb8,
-										0xeb0, 0xeb4};
-	u32 	backup_rf_reg[RF_REG_NUM] = {0x65, 0x8f, 0x0};
-	u8 	chnl_idx = _rtl8812ae_get_right_chnl_place_for_iqk(channel);
-
-	_rtl8812ae_iqk_backup_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
-	_rtl8812ae_iqk_backup_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
-	_rtl8812ae_iqk_backup_rf(hw, rfa_backup, rfb_backup, backup_rf_reg, RF_REG_NUM);
-
-	_rtl8812ae_iqk_configure_mac(hw);
-	_rtl8812ae_iqk_tx(hw, chnl_idx);
-	_rtl8812ae_iqk_restore_rf(hw, RF90_PATH_A, backup_rf_reg, rfa_backup, RF_REG_NUM);
-	_rtl8812ae_iqk_restore_rf(hw, RF90_PATH_A, backup_rf_reg, rfb_backup, RF_REG_NUM); // PATH_A ?
-
-	_rtl8812ae_iqk_restore_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
-	_rtl8812ae_iqk_restore_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
-}
-
-
-void _rtl8821ae_iqk_backup_macbb(
-		struct ieee80211_hw *hw,
-		u32 *macbb_backup,
-		u32 *backup_macbb_reg,
-		u32 mac_bb_num
-		)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	/*save MACBB default value*/
-	for (i = 0; i < mac_bb_num; i++) {
-		macbb_backup[i] =rtl_read_dword(rtlpriv,backup_macbb_reg[i]);
-	}
-
-	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupMacBB Success!!!!\n"));
-}
-
-void _rtl8821ae_iqk_backup_afe(
-		struct ieee80211_hw *hw,
-		u32 *afe_backup,
-		u32 *backup_afe_REG,
-		u32 afe_num
-		)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	/*Save AFE Parameters */
-	for (i = 0; i < afe_num; i++){
-		afe_backup[i] = rtl_read_dword(rtlpriv, backup_afe_REG[i]);
-	}
-	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupAFE Success!!!!\n"));
-}
-
-void _rtl8821ae_iqk_backup_rf(
-		struct ieee80211_hw *hw,
-		u32 *rfa_backup,
-		u32 *rfb_backup,
-		u32 *backup_rf_reg,
-		u32 rf_num
-		)
-{
-
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 i;
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	/*Save RF Parameters*/
-	for (i = 0; i < rf_num; i++){
-		rfa_backup[i] = rtl_get_rfreg(hw, RF90_PATH_A, backup_rf_reg[i], BMASKDWORD);
-		rfb_backup[i] = rtl_get_rfreg(hw, RF90_PATH_B, backup_rf_reg[i], BMASKDWORD);
-	}
-	RT_TRACE(COMP_IQK, DBG_LOUD, ("BackupRF Success!!!!\n"));
-}
-
-void _rtl8821ae_iqk_configure_mac(
-		struct ieee80211_hw *hw
-		)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	/* ========MAC register setting========*/
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-	rtl_write_byte(rtlpriv, 0x522, 0x3f);
-	rtl_set_bbreg(hw, 0x550, BIT(11) | BIT(3), 0x0);
-	rtl_write_byte(rtlpriv, 0x808, 0x00);		/*RX ante off*/
-	rtl_set_bbreg(hw, 0x838, 0xf, 0xc);		/*CCA off*/
-}
-
-
-void _rtl8821ae_iqk_tx_fill_iqc(
-		struct ieee80211_hw *hw,
-		enum radio_path path,
-		u32 tx_x,
-		u32 tx_y
-		)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	switch (path) {
-		case RF90_PATH_A:
-			{
-				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-				rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
-				rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
-				rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
-				rtl_set_bbreg(hw, 0xccc, 0x000007ff, tx_y);
-				rtl_set_bbreg(hw, 0xcd4, 0x000007ff, tx_x);
-				RT_TRACE(COMP_IQK, DBG_LOUD, ("TX_X = %x;;TX_Y = %x =====> fill to IQC\n", tx_x, tx_y));
-				RT_TRACE(COMP_IQK, DBG_LOUD, ("0xcd4 = %x;;0xccc = %x ====>fill to IQC\n", rtl_get_bbreg(hw, 0xcd4, 0x000007ff), rtl_get_bbreg(hw, 0xccc, 0x000007ff)));
-			}
-			break;
-		default:
-			break;
-	};
-}
-
-
-void _rtl8821ae_iqk_rx_fill_iqc(
-		struct ieee80211_hw *hw,
-		enum radio_path path,
-		u32 rx_x,
-		u32 rx_y
-		)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	switch (path) {
-		case RF90_PATH_A:
-			{
-				rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-				rtl_set_bbreg(hw, 0xc10, 0x000003ff, rx_x>>1);
-				rtl_set_bbreg(hw, 0xc10, 0x03ff0000, rx_y>>1);
-				RT_TRACE(COMP_IQK, DBG_LOUD, ("rx_x = %x;;rx_y = %x ====>fill to IQC\n", rx_x>>1, rx_y>>1));
-				RT_TRACE(COMP_IQK, DBG_LOUD, ("0xc10 = %x ====>fill to IQC\n", rtl_read_dword(rtlpriv, 0xc10)));
-			}
-			break;
-		default:
-			break;
-	};
-}
-
-
-
-#define cal_num 10
-
-void _rtl8821ae_iqk_tx(
-		struct ieee80211_hw *hw,
-		enum radio_path path
-		)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-
-	u32 	tx_fail, rx_fail, delay_count, iqk_ready, cal_retry, cal = 0, temp_reg65;
-	int	tx_x = 0, tx_y = 0, rx_x = 0, rx_y = 0, tx_average = 0, rx_average = 0;
-	int	tx_x0[cal_num], tx_y0[cal_num], tx_x0_rxk[cal_num], tx_y0_rxk[cal_num], rx_x0[cal_num], rx_y0[cal_num];
-	bool 	tx0iqkok = false, rx0iqkok = false;
-	bool  	vdf_enable = false;
-	int	i, k, vdf_y[3], vdf_x[3], tx_dt[3], rx_dt[3], ii, dx = 0, dy = 0, tx_finish = 0, rx_finish = 0;
-
-
-	RT_TRACE(COMP_IQK, DBG_LOUD,
-			("BandWidth = %d.\n",
-			 rtlphy->current_chan_bw));
-	if (rtlphy->current_chan_bw == HT_CHANNEL_WIDTH_80){
-		vdf_enable = true;
-	}
-
-	while (cal < cal_num) {
-		switch (path) {
-			case RF90_PATH_A:
-				{
-					temp_reg65 = rtl_get_rfreg(hw, path, 0x65, 0xffffffff);
-					//Path-A LOK
-					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); /*[31] = 0 --> Page C*/
-					/*========Path-A AFE all on========*/
-					/*Port 0 DAC/ADC on*/
-					rtl_write_dword(rtlpriv, 0xc60, 0x77777777);
-					rtl_write_dword(rtlpriv, 0xc64, 0x77777777);
-					rtl_write_dword(rtlpriv, 0xc68, 0x19791979);
-					rtl_write_dword(rtlpriv, 0xc6c, 0x19791979);
-					rtl_write_dword(rtlpriv, 0xc70, 0x19791979);
-					rtl_write_dword(rtlpriv, 0xc74, 0x19791979);
-					rtl_write_dword(rtlpriv, 0xc78, 0x19791979);
-					rtl_write_dword(rtlpriv, 0xc7c, 0x19791979);
-					rtl_write_dword(rtlpriv, 0xc80, 0x19791979);
-					rtl_write_dword(rtlpriv, 0xc84, 0x19791979);
-
-					rtl_set_bbreg(hw, 0xc00, 0xf, 0x4); /*hardware 3-wire off*/
-
-					// LOK Setting
-					//====== LOK ======
-					/*DAC/ADC sampling rate (160 MHz)*/
-					rtl_set_bbreg(hw, 0xc5c, BIT(26) | BIT(25) | BIT(24), 0x7);
-
-					// 2. LoK RF Setting (at BW = 20M)
-					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80002);
-					rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x3);     // BW 20M
-					rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x20000);
-					rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0003f);
-					rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xf3fc3);
-					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d5);
-					rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
-					rtl_set_bbreg(hw, 0xcb8, 0xf, 0xd);
-					rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-					rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
-					rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
-					rtl_write_dword(rtlpriv, 0x978, 0x29002000);// TX (X,Y)
-					rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);// RX (X,Y)
-					rtl_write_dword(rtlpriv, 0x984, 0x00462910);// [0]:AGC_en, [15]:idac_K_Mask
-
-					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-					rtl_write_dword(rtlpriv, 0xc88, 0x821403f4);
-
-					if (rtlhal->current_bandtype)
-						rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
-					else
-						rtl_write_dword(rtlpriv, 0xc8c, 0x28163e96);
-
-					rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-					rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
-					rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
-					rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-					rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-					mdelay(10); //Delay 10ms
-					rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-
-					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-					rtl_set_rfreg(hw, path, 0x58, 0x7fe00, rtl_get_rfreg(hw, path, 0x8, 0xffc00)); // Load LOK
-
-					switch (rtlphy->current_chan_bw)
-					{
-						case 1:
-							{
-								rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x1);
-							}
-							break;
-						case 2:
-							{
-								rtl_set_rfreg(hw, path, 0x18, 0x00c00, 0x0);
-							}
-							break;
-						default:
-							break;
-
-					}
-
-					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-
-					// 3. TX RF Setting
-					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
-					rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x20000);
-					rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0003f);
-					rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xf3fc3);
-					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d5);
-					rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
-					rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
-					//ODM_SetBBReg(pDM_Odm, 0xcb8, 0xf, 0xd);
-					rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-					rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
-					rtl_set_bbreg(hw, 0xc94, BIT(0), 0x1);
-					rtl_write_dword(rtlpriv, 0x978, 0x29002000);// TX (X,Y)
-					rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);// RX (X,Y)
-					rtl_write_dword(rtlpriv, 0x984, 0x0046a910);// [0]:AGC_en, [15]:idac_K_Mask
-
-					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-					rtl_write_dword(rtlpriv, 0xc88, 0x821403f1);
-					if (rtlhal->current_bandtype)
-						rtl_write_dword(rtlpriv, 0xc8c, 0x40163e96);
-					else
-						rtl_write_dword(rtlpriv, 0xc8c, 0x00163e96);
-
-					if (vdf_enable == 1){
-						RT_TRACE(COMP_IQK, DBG_LOUD, ("VDF_enable\n"));
-						for (k = 0;k <= 2; k++){
-							switch (k){
-								case 0:
-									{
-										rtl_write_dword(rtlpriv, 0xc80, 0x18008c38);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-										rtl_write_dword(rtlpriv, 0xc84, 0x38008c38);// RX_Tone_idx[9:0], RxK_Mask[29]
-										rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);
-									}
-									break;
-								case 1:
-									{
-										rtl_set_bbreg(hw, 0xc80, BIT(28), 0x0);
-										rtl_set_bbreg(hw, 0xc84, BIT(28), 0x0);
-										rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);
-									}
-									break;
-								case 2:
-									{
-										RT_TRACE(COMP_IQK, DBG_LOUD, ("vdf_y[1] = %x;;;vdf_y[0] = %x\n", vdf_y[1]>>21 & 0x00007ff, vdf_y[0]>>21 & 0x00007ff));
-										RT_TRACE(COMP_IQK, DBG_LOUD, ("vdf_x[1] = %x;;;vdf_x[0] = %x\n", vdf_x[1]>>21 & 0x00007ff, vdf_x[0]>>21 & 0x00007ff));
-										tx_dt[cal] = (vdf_y[1]>>20)-(vdf_y[0]>>20);
-										tx_dt[cal] = ((16*tx_dt[cal])*10000/15708);
-										tx_dt[cal] = (tx_dt[cal] >> 1 )+(tx_dt[cal] & BIT(0));
-										rtl_write_dword(rtlpriv, 0xc80, 0x18008c20);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-										rtl_write_dword(rtlpriv, 0xc84, 0x38008c20);// RX_Tone_idx[9:0], RxK_Mask[29]
-										rtl_set_bbreg(hw, 0xce8, BIT(31), 0x1);
-										rtl_set_bbreg(hw, 0xce8, 0x3fff0000, tx_dt[cal] & 0x00003fff);
-									}
-									break;
-								default:
-									break;
-							}
-							rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
-							cal_retry = 0;
-							while(1){
-								// one shot
-								rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-								rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-								mdelay(10); //Delay 10ms
-								rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-								delay_count = 0;
-								while (1){
-									iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
-									if ((~iqk_ready) || (delay_count>20)){
-										break;
-									}
-									else{
-										mdelay(1);
-										delay_count++;
-									}
-								}
-
-								if (delay_count < 20){							// If 20ms No Result, then cal_retry++
-									// ============TXIQK Check==============
-									tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
-
-									if (~tx_fail){
-										rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
-										vdf_x[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-										rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
-										vdf_y[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-										tx0iqkok = true;
-										break;
-									}
-									else{
-										rtl_set_bbreg(hw, 0xccc, 0x000007ff, 0x0);
-										rtl_set_bbreg(hw, 0xcd4, 0x000007ff, 0x200);
-										tx0iqkok = false;
-										cal_retry++;
-										if (cal_retry == 10) {
-											break;
-										}
-									}
-								}
-								else{
-									tx0iqkok = false;
-									cal_retry++;
-									if (cal_retry == 10){
-										break;
-									}
-								}
-							}
-						}
-						if (k == 3){
-							tx_x0[cal] = vdf_x[k-1] ;
-							tx_y0[cal] = vdf_y[k-1];
-						}
-					}
-
-					else {
-						rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-						rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
-						rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
-						cal_retry = 0;
-						while(1){
-							// one shot
-							rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-							rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-							mdelay(10); //Delay 10ms
-							rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-							delay_count = 0;
-							while (1){
-								iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
-								if ((~iqk_ready) || (delay_count>20)) {
-									break;
-								}
-								else{
-									mdelay(1);
-									delay_count++;
-								}
-							}
-
-							if (delay_count < 20){							// If 20ms No Result, then cal_retry++
-								// ============TXIQK Check==============
-								tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
-
-								if (~tx_fail){
-									rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
-									tx_x0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-									rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
-									tx_y0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-									tx0iqkok = true;
-									break;
-								}
-								else{
-									rtl_set_bbreg(hw, 0xccc, 0x000007ff, 0x0);
-									rtl_set_bbreg(hw, 0xcd4, 0x000007ff, 0x200);
-									tx0iqkok = false;
-									cal_retry++;
-									if (cal_retry == 10) {
-										break;
-									}
-								}
-							}
-							else{
-								tx0iqkok = false;
-								cal_retry++;
-								if (cal_retry == 10)
-									break;
-							}
-						}
-					}
-
-
-					if (tx0iqkok == false)
-						break;				// TXK fail, Don't do RXK
-
-					if (vdf_enable == 1){
-						rtl_set_bbreg(hw, 0xce8, BIT(31), 0x0);    // TX VDF Disable
-						RT_TRACE(COMP_IQK, DBG_LOUD, ("RXVDF Start\n"));
-						for (k = 0;k <= 2; k++){
-							//====== RX mode TXK (RXK Step 1) ======
-							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-							// 1. TX RF Setting
-							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
-							rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
-							rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x00029);
-							rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xd7ffb);
-							rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
-							rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
-							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
-
-							rtl_set_bbreg(hw, 0xcb8, 0xf, 0xd);
-							rtl_write_dword(rtlpriv, 0x978, 0x29002000);// TX (X,Y)
-							rtl_write_dword(rtlpriv, 0x97c, 0xa9002000);// RX (X,Y)
-							rtl_write_dword(rtlpriv, 0x984, 0x0046a910);// [0]:AGC_en, [15]:idac_K_Mask
-							rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-							rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
-							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-							switch (k){
-								case 0:
-									{
-										rtl_write_dword(rtlpriv, 0xc80, 0x18008c38);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-										rtl_write_dword(rtlpriv, 0xc84, 0x38008c38);// RX_Tone_idx[9:0], RxK_Mask[29]
-										rtl_set_bbreg(hw, 0xce8, BIT(30), 0x0);
-									}
-									break;
-								case 1:
-									{
-										rtl_write_dword(rtlpriv, 0xc80, 0x08008c38);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-										rtl_write_dword(rtlpriv, 0xc84, 0x28008c38);// RX_Tone_idx[9:0], RxK_Mask[29]
-										rtl_set_bbreg(hw, 0xce8, BIT(30), 0x0);
-									}
-									break;
-								case 2:
-									{
-										RT_TRACE(COMP_IQK, DBG_LOUD, ("VDF_Y[1] = %x;;;VDF_Y[0] = %x\n", vdf_y[1]>>21 & 0x00007ff, vdf_y[0]>>21 & 0x00007ff));
-										RT_TRACE(COMP_IQK, DBG_LOUD, ("VDF_X[1] = %x;;;VDF_X[0] = %x\n", vdf_x[1]>>21 & 0x00007ff, vdf_x[0]>>21 & 0x00007ff));
-										rx_dt[cal] = (vdf_y[1]>>20)-(vdf_y[0]>>20);
-										RT_TRACE(COMP_IQK, DBG_LOUD, ("Rx_dt = %d\n", rx_dt[cal]));
-										rx_dt[cal] = ((16*rx_dt[cal])*10000/13823);
-										rx_dt[cal] = (rx_dt[cal] >> 1 )+(rx_dt[cal] & BIT(0));
-										rtl_write_dword(rtlpriv, 0xc80, 0x18008c20);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-										rtl_write_dword(rtlpriv, 0xc84, 0x38008c20);// RX_Tone_idx[9:0], RxK_Mask[29]
-										rtl_set_bbreg(hw, 0xce8, 0x00003fff, rx_dt[cal] & 0x00003fff);
-									}
-									break;
-								default:
-									break;
-							}
-							rtl_write_dword(rtlpriv, 0xc88, 0x821603e0);
-							rtl_write_dword(rtlpriv, 0xc8c, 0x68163e96);
-							rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
-							cal_retry = 0;
-							while(1){
-								// one shot
-								rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-								rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-								mdelay(10); //Delay 10ms
-								rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-								delay_count = 0;
-								while (1){
-									iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
-									if ((~iqk_ready)||(delay_count>20)){
-										break;
-									}
-									else{
-										mdelay(1);
-										delay_count++;
-									}
-								}
-
-								if (delay_count < 20){							// If 20ms No Result, then cal_retry++
-									// ============TXIQK Check==============
-									tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
-
-									if (~tx_fail){
-										rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
-										tx_x0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-										rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
-										tx_y0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-										tx0iqkok = true;
-										break;
-									}
-									else{
-										tx0iqkok = false;
-										cal_retry++;
-										if (cal_retry == 10)
-											break;
-									}
-								}
-								else{
-									tx0iqkok = false;
-									cal_retry++;
-									if (cal_retry == 10)
-										break;
-								}
-							}
-
-							if (tx0iqkok == false){   //If RX mode TXK fail, then take TXK Result
-								tx_x0_rxk[cal] = tx_x0[cal];
-								tx_y0_rxk[cal] = tx_y0[cal];
-								tx0iqkok = true;
-								RT_TRACE(COMP_IQK, DBG_LOUD, ("RXK Step 1 fail\n"));
-							}
-
-
-							//====== RX IQK ======
-							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-							// 1. RX RF Setting
-							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
-							rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
-							rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0002f);
-							rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xfffbb);
-							rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x88001);
-							rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d8);
-							rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
-
-							rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0_rxk[cal])>>21&0x000007ff);
-							rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0_rxk[cal])>>21&0x000007ff);
-							rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
-							rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
-							rtl_set_bbreg(hw, 0xcb8, 0xF, 0xe);
-							rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-							rtl_write_dword(rtlpriv, 0x984, 0x0046a911);
-
-							rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-							rtl_set_bbreg(hw, 0xc80, BIT(29), 0x1);
-							rtl_set_bbreg(hw, 0xc84, BIT(29), 0x0);
-							rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
-
-							rtl_write_dword(rtlpriv, 0xc8c, 0x28160d00); /* pDM_Odm->SupportInterface == 1 */
-
-							if (k==2){
-								rtl_set_bbreg(hw, 0xce8, BIT(30), 0x1);  //RX VDF Enable
-							}
-							rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
-
-							cal_retry = 0;
-							while(1){
-								// one shot
-								rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-								rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-								mdelay(10); //Delay 10ms
-								rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-								delay_count = 0;
-								while (1){
-									iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
-									if ((~iqk_ready)||(delay_count>20)){
-										break;
-									}
-									else{
-										mdelay(1);
-										delay_count++;
-									}
-								}
-
-								if (delay_count < 20){	// If 20ms No Result, then cal_retry++
-									// ============RXIQK Check==============
-									rx_fail = rtl_get_bbreg(hw, 0xd00, BIT(11));
-									if (rx_fail == 0){
-										rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
-										vdf_x[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-										rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
-										vdf_y[k] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-										rx0iqkok = true;
-										break;
-									}
-									else{
-										rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x200>>1);
-										rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0x0>>1);
-										rx0iqkok = false;
-										cal_retry++;
-										if (cal_retry == 10)
-											break;
-
-									}
-								}
-								else{
-									rx0iqkok = false;
-									cal_retry++;
-									if (cal_retry == 10)
-										break;
-								}
-							}
-
-						}
-						if (k == 3){
-							rx_x0[cal] = vdf_x[k-1] ;
-							rx_y0[cal] = vdf_y[k-1];
-						}
-						rtl_set_bbreg(hw, 0xce8, BIT(31), 0x1);    // TX VDF Enable
-					}
-
-					else{
-						//====== RX mode TXK (RXK Step 1) ======
-						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-						// 1. TX RF Setting
-						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
-						rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
-						rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x00029);
-						rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xd7ffb);
-						rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
-						rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x8a001);
-						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
-						rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-						rtl_write_dword(rtlpriv, 0xb00, 0x03000100);
-						rtl_write_dword(rtlpriv, 0x984, 0x0046a910);// [0]:AGC_en, [15]:idac_K_Mask
-
-						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-						rtl_write_dword(rtlpriv, 0xc80, 0x18008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-						rtl_write_dword(rtlpriv, 0xc84, 0x38008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
-						rtl_write_dword(rtlpriv, 0xc88, 0x821603e0);
-						//ODM_Write4Byte(pDM_Odm, 0xc8c, 0x68163e96);
-						rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
-						cal_retry = 0;
-						while(1){
-							// one shot
-							rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-							rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-							mdelay(10); //Delay 10ms
-							rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-							delay_count = 0;
-							while (1){
-								iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
-								if ((~iqk_ready)||(delay_count>20)){
-									break;
-								}
-								else{
-									mdelay(1);
-									delay_count++;
-								}
-							}
-
-							if (delay_count < 20){							// If 20ms No Result, then cal_retry++
-								// ============TXIQK Check==============
-								tx_fail = rtl_get_bbreg(hw, 0xd00, BIT(12));
-
-								if (~tx_fail){
-									rtl_write_dword(rtlpriv, 0xcb8, 0x02000000);
-									tx_x0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-									rtl_write_dword(rtlpriv, 0xcb8, 0x04000000);
-									tx_y0_rxk[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-									tx0iqkok = true;
-									break;
-								}
-								else{
-									tx0iqkok = false;
-									cal_retry++;
-									if (cal_retry == 10)
-										break;
-								}
-							}
-							else{
-								tx0iqkok = false;
-								cal_retry++;
-								if (cal_retry == 10)
-									break;
-							}
-						}
-
-
-						if (tx0iqkok == false){   //If RX mode TXK fail, then take TXK Result
-							tx_x0_rxk[cal] = tx_x0[cal];
-							tx_y0_rxk[cal] = tx_y0[cal];
-							tx0iqkok = true;
-							RT_TRACE(COMP_IQK, DBG_LOUD, ("1"));
-						}
-
-
-						//====== RX IQK ======
-						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-						// 1. RX RF Setting
-						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x80000);
-						rtl_set_rfreg(hw, path, 0x30, RFREG_OFFSET_MASK, 0x30000);
-						rtl_set_rfreg(hw, path, 0x31, RFREG_OFFSET_MASK, 0x0002f);
-						rtl_set_rfreg(hw, path, 0x32, RFREG_OFFSET_MASK, 0xfffbb);
-						rtl_set_rfreg(hw, path, 0x8f, RFREG_OFFSET_MASK, 0x88001);
-						rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, 0x931d8);
-						rtl_set_rfreg(hw, path, 0xef, RFREG_OFFSET_MASK, 0x00000);
-
-						rtl_set_bbreg(hw, 0x978, 0x03FF8000, (tx_x0_rxk[cal])>>21&0x000007ff);
-						rtl_set_bbreg(hw, 0x978, 0x000007FF, (tx_y0_rxk[cal])>>21&0x000007ff);
-						rtl_set_bbreg(hw, 0x978, BIT(31), 0x1);
-						rtl_set_bbreg(hw, 0x97c, BIT(31), 0x0);
-						//ODM_SetBBReg(pDM_Odm, 0xcb8, 0xF, 0xe);
-						rtl_write_dword(rtlpriv, 0x90c, 0x00008000);
-						rtl_write_dword(rtlpriv, 0x984, 0x0046a911);
-
-						rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-						rtl_write_dword(rtlpriv, 0xc80, 0x38008c10);// TX_Tone_idx[9:0], TxK_Mask[29] TX_Tone = 16
-						rtl_write_dword(rtlpriv, 0xc84, 0x18008c10);// RX_Tone_idx[9:0], RxK_Mask[29]
-						rtl_write_dword(rtlpriv, 0xc88, 0x02140119);
-
-						rtl_write_dword(rtlpriv, 0xc8c, 0x28160d00); /*pDM_Odm->SupportInterface == 1*/
-
-						rtl_write_dword(rtlpriv, 0xcb8, 0x00100000);// cb8[20] ±N SI/PI ¨Ï¥ÎÅv¤Áµ¹ iqk_dpk module
-
-						cal_retry = 0;
-						while(1){
-							// one shot
-							rtl_write_dword(rtlpriv, 0x980, 0xfa000000);
-							rtl_write_dword(rtlpriv, 0x980, 0xf8000000);
-
-							mdelay(10); //Delay 10ms
-							rtl_write_dword(rtlpriv, 0xcb8, 0x00000000);
-							delay_count = 0;
-							while (1){
-								iqk_ready = rtl_get_bbreg(hw, 0xd00, BIT(10));
-								if ((~iqk_ready)||(delay_count>20)){
-									break;
-								}
-								else{
-									mdelay(1);
-									delay_count++;
-								}
-							}
-
-							if (delay_count < 20){	// If 20ms No Result, then cal_retry++
-								// ============RXIQK Check==============
-								rx_fail = rtl_get_bbreg(hw, 0xd00, BIT(11));
-								if (rx_fail == 0){
-									/*
-									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x05000000);
-									   reg1 = ODM_GetBBReg(pDM_Odm, 0xd00, 0xffffffff);
-									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x06000000);
-									   reg2 = ODM_GetBBReg(pDM_Odm, 0xd00, 0x0000001f);
-									   DbgPrint("reg1 = %d, reg2 = %d", reg1, reg2);
-									   Image_Power = (reg2<<32)+reg1;
-									   DbgPrint("Before PW = %d\n", Image_Power);
-									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x07000000);
-									   reg1 = ODM_GetBBReg(pDM_Odm, 0xd00, 0xffffffff);
-									   ODM_Write4Byte(pDM_Odm, 0xcb8, 0x08000000);
-									   reg2 = ODM_GetBBReg(pDM_Odm, 0xd00, 0x0000001f);
-									   Image_Power = (reg2<<32)+reg1;
-									   DbgPrint("After PW = %d\n", Image_Power);
-									   */
-
-									rtl_write_dword(rtlpriv, 0xcb8, 0x06000000);
-									rx_x0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-									rtl_write_dword(rtlpriv, 0xcb8, 0x08000000);
-									rx_y0[cal] = rtl_get_bbreg(hw, 0xd00, 0x07ff0000)<<21;
-									rx0iqkok = true;
-									break;
-								}
-								else{
-									rtl_set_bbreg(hw, 0xc10, 0x000003ff, 0x200>>1);
-									rtl_set_bbreg(hw, 0xc10, 0x03ff0000, 0x0>>1);
-									rx0iqkok = false;
-									cal_retry++;
-									if (cal_retry == 10)
-										break;
-
-								}
-							}
-							else{
-								rx0iqkok = false;
-								cal_retry++;
-								if (cal_retry == 10)
-									break;
-							}
-						}
-					}
-
-					if (tx0iqkok)
-						tx_average++;
-					if (rx0iqkok)
-						rx_average++;
-					rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-					rtl_set_rfreg(hw, path, 0x65, RFREG_OFFSET_MASK, temp_reg65);
-				}
-				break;
-			default:
-				break;
-		}
-		cal++;
-	}
-
-	// FillIQK Result
-	switch (path){
-		case RF90_PATH_A:
-			{
-				RT_TRACE(COMP_IQK, DBG_LOUD, ("========Path_A =======\n"));
-				if (tx_average == 0)
-					break;
-
-				for (i = 0; i < tx_average; i++){
-					RT_TRACE(COMP_IQK, DBG_LOUD, (" TX_X0_RXK[%d] = %x ;; TX_Y0_RXK[%d] = %x\n", i, (tx_x0_rxk[i])>>21&0x000007ff, i, (tx_y0_rxk[i])>>21&0x000007ff));
-					RT_TRACE(COMP_IQK, DBG_LOUD, ("TX_X0[%d] = %x ;; TX_Y0[%d] = %x\n", i, (tx_x0[i])>>21&0x000007ff, i, (tx_y0[i])>>21&0x000007ff));
-				}
-				for (i = 0; i < tx_average; i++){
-					for (ii = i+1; ii <tx_average; ii++){
-						dx = (tx_x0[i]>>21) - (tx_x0[ii]>>21);
-						if (dx < 3 && dx > -3){
-							dy = (tx_y0[i]>>21) - (tx_y0[ii]>>21);
-							if (dy < 3 && dy > -3){
-								tx_x = ((tx_x0[i]>>21) + (tx_x0[ii]>>21))/2;
-								tx_y = ((tx_y0[i]>>21) + (tx_y0[ii]>>21))/2;
-								tx_finish = 1;
-								break;
-							}
-						}
-					}
-					if (tx_finish == 1)
-						break;
-				}
-
-				if (tx_finish == 1){
-					_rtl8821ae_iqk_tx_fill_iqc(hw, path, tx_x, tx_y); // ?
-				}
-				else{
-					_rtl8821ae_iqk_tx_fill_iqc(hw, path, 0x200, 0x0);
-				}
-
-				if (rx_average == 0)
-					break;
-
-				for (i = 0; i < rx_average; i++){
-					RT_TRACE(COMP_IQK, DBG_LOUD, ("RX_X0[%d] = %x ;; RX_Y0[%d] = %x\n", i, (rx_x0[i])>>21&0x000007ff, i, (rx_y0[i])>>21&0x000007ff));
-				}
-				for (i = 0; i < rx_average; i++){
-					for (ii = i+1; ii <rx_average; ii++){
-						dx = (rx_x0[i]>>21) - (rx_x0[ii]>>21);
-						if (dx < 4 && dx > -4){
-							dy = (rx_y0[i]>>21) - (rx_y0[ii]>>21);
-							if (dy < 4 && dy > -4){
-								rx_x = ((rx_x0[i]>>21) + (rx_x0[ii]>>21))/2;
-								rx_y = ((rx_y0[i]>>21) + (rx_y0[ii]>>21))/2;
-								rx_finish = 1;
-								break;
-							}
-						}
-					}
-					if (rx_finish == 1)
-						break;
-				}
-
-				if (rx_finish == 1){
-					_rtl8821ae_iqk_rx_fill_iqc(hw, path, rx_x, rx_y);
-				}
-				else{
-					_rtl8821ae_iqk_rx_fill_iqc(hw, path, 0x200, 0x0);
-				}
-			}
-			break;
-		default:
-			break;
-	}
-}
-
-void _rtl8821ae_iqk_restore_rf(
-		struct ieee80211_hw *hw,
-		enum radio_path path,
-		u32*			backup_rf_reg,
-		u32* 			rf_backup,
-		u32			rf_reg_num
-		)
-{
-	u32 i;
-	struct rtl_priv* rtlpriv = rtl_priv(hw);
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-	for (i = 0; i < RF_REG_NUM; i++)
-		rtl_set_rfreg(hw, path, backup_rf_reg[i], RFREG_OFFSET_MASK, rf_backup[i]);
-
-	switch(path){
-		case RF90_PATH_A:
-			{
-				RT_TRACE(COMP_IQK, DBG_LOUD, ("RestoreRF Path A Success!!!!\n"));
-			}
-			break;
-		default:
-			break;
-	}
-}
-
-void _rtl8821ae_iqk_restore_afe(
-		struct ieee80211_hw *hw,
-		u32*		afe_backup,
-		u32*		backup_afe_reg,
-		u32		afe_num
-		)
-{
-	u32 i;
-	struct rtl_priv* rtlpriv = rtl_priv(hw);
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-	//Reload AFE Parameters
-	for (i = 0; i < afe_num; i++){
-		rtl_write_dword(rtlpriv, backup_afe_reg[i], afe_backup[i]);
-	}
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x1); // [31] = 1 --> Page C1
-	rtl_write_dword(rtlpriv, 0xc80, 0x0);
-	rtl_write_dword(rtlpriv, 0xc84, 0x0);
-	rtl_write_dword(rtlpriv, 0xc88, 0x0);
-	rtl_write_dword(rtlpriv, 0xc8c, 0x3c000000);
-	rtl_write_dword(rtlpriv, 0xc90, 0x00000080);
-	rtl_write_dword(rtlpriv, 0xc94, 0x00000000);
-	rtl_write_dword(rtlpriv, 0xcc4, 0x20040000);
-	rtl_write_dword(rtlpriv, 0xcc8, 0x20000000);
-	rtl_write_dword(rtlpriv, 0xcb8, 0x0);
-	RT_TRACE(COMP_IQK, DBG_LOUD, ("RestoreAFE Success!!!!\n"));
-}
-
-void _rtl8821ae_iqk_restore_macbb(
-		struct ieee80211_hw *hw,
-		u32*		macbb_backup,
-		u32*		backup_macbb_reg,
-		u32		macbb_num
-		)
-{
-	u32 i;
-	struct rtl_priv* rtlpriv = rtl_priv(hw);
-
-	rtl_set_bbreg(hw, 0x82c, BIT(31), 0x0); // [31] = 0 --> Page C
-	//Reload MacBB Parameters
-	for (i = 0; i < macbb_num; i++){
-		rtl_write_dword(rtlpriv, backup_macbb_reg[i], macbb_backup[i]);
-	}
-	RT_TRACE(COMP_IQK, DBG_LOUD, ("RestoreMacBB Success!!!!\n"));
-}
-
-
-#undef MACBB_REG_NUM
-#undef AFE_REG_NUM
-#undef RF_REG_NUM
-
-#define MACBB_REG_NUM 11
-#define AFE_REG_NUM 12
-#define RF_REG_NUM 3
-
-static void _rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw)
-{
-	u32	macbb_backup[MACBB_REG_NUM];
-	u32 afe_backup[AFE_REG_NUM];
-	u32 rfa_backup[RF_REG_NUM];
-	u32 rfb_backup[RF_REG_NUM];
-	u32 	backup_macbb_reg[MACBB_REG_NUM] = {0xb00, 0x520, 0x550, 0x808, 0x90c, 0xc00, 0xc50,
-							0xe00, 0xe50, 0x838, 0x82c};
-	u32	backup_afe_reg[AFE_REG_NUM] = {0xc5c, 0xc60, 0xc64, 0xc68, 0xc6c, 0xc70, 0xc74,
-							0xc78, 0xc7c, 0xc80, 0xc84, 0xcb8};
-	u32 	backup_rf_reg[RF_REG_NUM] = {0x65, 0x8f, 0x0};
-
-	_rtl8821ae_iqk_backup_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
-	_rtl8821ae_iqk_backup_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
-	_rtl8821ae_iqk_backup_rf(hw, rfa_backup, rfb_backup, backup_rf_reg, RF_REG_NUM);
-
-	_rtl8821ae_iqk_configure_mac(hw);
-	_rtl8821ae_iqk_tx(hw, RF90_PATH_A);
-	_rtl8821ae_iqk_restore_rf(hw, RF90_PATH_A, backup_rf_reg, rfa_backup, RF_REG_NUM);
-
-	_rtl8821ae_iqk_restore_afe(hw, afe_backup, backup_afe_reg, AFE_REG_NUM);
-	_rtl8821ae_iqk_restore_macbb(hw, macbb_backup, backup_macbb_reg, MACBB_REG_NUM);
-}
-
-static void _rtl8821ae_phy_lc_calibrate(struct ieee80211_hw *hw, bool is2t)
-{
-	u8 tmpreg;
-	u32 rf_a_mode = 0, rf_b_mode = 0, lc_cal;
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	tmpreg = rtl_read_byte(rtlpriv, 0xd03);
-
-	if ((tmpreg & 0x70) != 0)
-		rtl_write_byte(rtlpriv, 0xd03, tmpreg & 0x8F);
-	else
-		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
-
-	if ((tmpreg & 0x70) != 0) {
-		rf_a_mode = rtl_get_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS);
-
-		if (is2t)
-			rf_b_mode = rtl_get_rfreg(hw, RF90_PATH_B, 0x00,
-						  MASK12BITS);
-
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS,
-			      (rf_a_mode & 0x8FFFF) | 0x10000);
-
-		if (is2t)
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS,
-				      (rf_b_mode & 0x8FFFF) | 0x10000);
-	}
-	lc_cal = rtl_get_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS);
-
-	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdfbe0);
-	/* rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, lc_cal | 0x08000); */
-	rtl_set_rfreg(hw, RF90_PATH_A, 0x18, MASK12BITS, 0x8c0a);
-
-	mdelay(100);
-
-	rtl_set_rfreg(hw, RF90_PATH_A, 0xb0, RFREG_OFFSET_MASK, 0xdffe0);
-
-	if ((tmpreg & 0x70) != 0) {
-		rtl_write_byte(rtlpriv, 0xd03, tmpreg);
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, MASK12BITS, rf_a_mode);
-
-		if (is2t)
-			rtl_set_rfreg(hw, RF90_PATH_B, 0x00, MASK12BITS, rf_b_mode);
-	} else {
-		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
-	}
-RT_TRACE(COMP_INIT,DBG_LOUD,("\n"));
-
-}
-
-static void _rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool main)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	//struct rtl_hal *rtlhal = rtl_hal(rtl_priv(hw));
-	//struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	RT_TRACE(COMP_INIT,DBG_LOUD,("\n"));
-
-	if (main)
-		rtl_set_bbreg(hw, RA_RFE_PINMUX + 4, BIT(29) | BIT(28), 0x1);
-	else
-		rtl_set_bbreg(hw, RA_RFE_PINMUX + 4, BIT(29) | BIT(28), 0x2);
-}
-
-#undef IQK_ADDA_REG_NUM
-#undef IQK_DELAY_TIME
-
-void rtl8812ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (!rtlphy->b_iqk_in_progress)
-	{
-		spin_lock(&rtlpriv->locks.iqk_lock);
-		rtlphy->b_iqk_in_progress = true;
-		spin_unlock(&rtlpriv->locks.iqk_lock);
-
-		_rtl8812ae_phy_iq_calibrate(hw, rtlphy->current_channel);
-
-		spin_lock(&rtlpriv->locks.iqk_lock);
-		rtlphy->b_iqk_in_progress = false;
-		spin_unlock(&rtlpriv->locks.iqk_lock);
-	}
-}
-
-void rtl8812ae_reset_iqk_result(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 i;
-
-	RT_TRACE(COMP_IQK, DBG_LOUD,
-		("rtl8812ae_dm_reset_iqk_result:: settings regs %d default regs %d\n",
-		(int)(sizeof(rtlphy->iqk_matrix_regsetting) /
-		sizeof(struct iqk_matrix_regs)),
-		IQK_MATRIX_SETTINGS_NUM));
-
-	for(i = 0; i < IQK_MATRIX_SETTINGS_NUM; i++) {
-		{
-			rtlphy->iqk_matrix_regsetting[i].value[0][0] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][2] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][4] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][6] = 0x100;
-
-			rtlphy->iqk_matrix_regsetting[i].value[0][1] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][3] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][5] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][7] = 0x0;
-
-			rtlphy->iqk_matrix_regsetting[i].b_iqk_done = false;
-
-		}
-	}
-}
-
-void rtl8812ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
-	u8 thermal_value, u8 threshold)
-{
-	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
-
-	rtl8812ae_reset_iqk_result(hw);
-
-	rtldm->thermalvalue_iqk= thermal_value;
-	rtl8812ae_phy_iq_calibrate(hw, false);
-}
-
-void rtl8821ae_phy_iq_calibrate(struct ieee80211_hw *hw, bool b_recovery)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (!rtlphy->b_iqk_in_progress)
-	{
-		spin_lock(&rtlpriv->locks.iqk_lock);
-		rtlphy->b_iqk_in_progress = true;
-		spin_unlock(&rtlpriv->locks.iqk_lock);
-
-		_rtl8821ae_phy_iq_calibrate(hw);
-
-		spin_lock(&rtlpriv->locks.iqk_lock);
-		rtlphy->b_iqk_in_progress = false;
-		spin_unlock(&rtlpriv->locks.iqk_lock);
-	}
-}
-
-void rtl8821ae_reset_iqk_result(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 i;
-
-	RT_TRACE(COMP_IQK, DBG_LOUD,
-		("rtl8812ae_dm_reset_iqk_result:: settings regs %d default regs %d\n",
-		(int)(sizeof(rtlphy->iqk_matrix_regsetting) /
-		sizeof(struct iqk_matrix_regs)),
-		IQK_MATRIX_SETTINGS_NUM));
-
-	for(i = 0; i < IQK_MATRIX_SETTINGS_NUM; i++) {
-		{
-			rtlphy->iqk_matrix_regsetting[i].value[0][0] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][2] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][4] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][6] = 0x100;
-
-			rtlphy->iqk_matrix_regsetting[i].value[0][1] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][3] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][5] =
-				rtlphy->iqk_matrix_regsetting[i].value[0][7] = 0x0;
-
-			rtlphy->iqk_matrix_regsetting[i].b_iqk_done = false;
-
-		}
-	}
-}
-
-void rtl8821ae_do_iqk(struct ieee80211_hw *hw,u8 delta_thermal_index,
-	u8 thermal_value, u8 threshold)
-{
-	struct rtl_dm	*rtldm = rtl_dm(rtl_priv(hw));
-
-	rtl8821ae_reset_iqk_result(hw);
-
-	rtldm->thermalvalue_iqk= thermal_value;
-	rtl8821ae_phy_iq_calibrate(hw, false);
-}
-
-void rtl8821ae_phy_lc_calibrate(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	struct rtl_hal *rtlhal = &(rtlpriv->rtlhal);
-	u32 timeout = 2000, timecount = 0;
-
-
-	while (rtlpriv->mac80211.act_scanning && timecount < timeout) {
-		udelay(50);
-		timecount += 50;
-	}
-
-	rtlphy->lck_inprogress = true;
-	RTPRINT(rtlpriv, FINIT, INIT_IQK,
-		("LCK:Start!!! currentband %x delay %d ms\n",
-		 rtlhal->current_bandtype, timecount));
-
-	_rtl8821ae_phy_lc_calibrate(hw, false);
-
-	rtlphy->lck_inprogress = false;
-}
-
-void rtl8821ae_phy_ap_calibrate(struct ieee80211_hw *hw, char delta)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	if (rtlphy->b_apk_done)
-		return;
-
-	return;
-}
-
-void rtl8821ae_phy_set_rfpath_switch(struct ieee80211_hw *hw, bool bmain)
-{
-	_rtl8821ae_phy_set_rfpath_switch(hw, bmain);
-}
-
-bool rtl8821ae_phy_set_io_cmd(struct ieee80211_hw *hw, enum io_type iotype)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	bool b_postprocessing = false;
-
-	RT_TRACE(COMP_CMD, DBG_TRACE,
-		 ("-->IO Cmd(%#x), set_io_inprogress(%d)\n",
-		  iotype, rtlphy->set_io_inprogress));
-	do {
-		switch (iotype) {
-		case IO_CMD_RESUME_DM_BY_SCAN:
-			RT_TRACE(COMP_CMD, DBG_TRACE,
-				 ("[IO CMD] Resume DM after scan.\n"));
-			b_postprocessing = true;
-			break;
-		case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
-		case IO_CMD_PAUSE_BAND1_DM_BY_SCAN:
-			RT_TRACE(COMP_CMD, DBG_TRACE,
-				 ("[IO CMD] Pause DM before scan.\n"));
-			b_postprocessing = true;
-			break;
-		default:
-			RT_TRACE(COMP_ERR, DBG_EMERG,
-				 ("switch case not process \n"));
-			break;
-		}
-	} while (false);
-	if (b_postprocessing && !rtlphy->set_io_inprogress) {
-		rtlphy->set_io_inprogress = true;
-		rtlphy->current_io_type = iotype;
-	} else {
-		return false;
-	}
-	rtl8821ae_phy_set_io(hw);
-	RT_TRACE(COMP_CMD, DBG_TRACE, ("IO Type(%#x)\n", iotype));
-	return true;
-}
-
-static void rtl8821ae_phy_set_io(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-
-	RT_TRACE(COMP_CMD, DBG_TRACE,
-		 ("--->Cmd(%#x), set_io_inprogress(%d)\n",
-		  rtlphy->current_io_type, rtlphy->set_io_inprogress));
-	switch (rtlphy->current_io_type) {
-	case IO_CMD_RESUME_DM_BY_SCAN:
-		if (rtlpriv->mac80211.opmode== NL80211_IFTYPE_ADHOC)
-			_rtl8821ae_resume_tx_beacon(hw);
-		rtl8821ae_dm_write_dig(hw, rtlphy->initgain_backup.xaagccore1);
-		rtl8821ae_dm_write_cck_cca_thres(hw, rtlphy->initgain_backup.cca);
-		break;
-	case IO_CMD_PAUSE_BAND0_DM_BY_SCAN:
-		if (rtlpriv->mac80211.opmode== NL80211_IFTYPE_ADHOC)
-			_rtl8821ae_stop_tx_beacon(hw);
-		rtlphy->initgain_backup.xaagccore1 = dm_digtable.cur_igvalue;
-		rtl8821ae_dm_write_dig(hw, 0x17);
-		rtlphy->initgain_backup.cca = dm_digtable.cur_cck_cca_thres;
-		rtl8821ae_dm_write_cck_cca_thres(hw, 0x40);
-		break;
-	case IO_CMD_PAUSE_BAND1_DM_BY_SCAN:
-		break;
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		break;
-	}
-	rtlphy->set_io_inprogress = false;
-	RT_TRACE(COMP_CMD, DBG_TRACE,
-		 ("(%#x)\n", rtlphy->current_io_type));
-}
-
-static void rtl8821ae_phy_set_rf_on(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x2b);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
-	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
-}
-
-#if 0
-static void _rtl8821ae_phy_set_rf_sleep(struct ieee80211_hw *hw)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	rtl_write_byte(rtlpriv, REG_TXPAUSE, 0xFF);
-	rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
-	/*rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
-	u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
-	while (u4b_tmp != 0 && delay > 0) {
-		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x0);
-		rtl_set_rfreg(hw, RF90_PATH_A, 0x00, RFREG_OFFSET_MASK, 0x00);
-		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x40);
-		u4b_tmp = rtl_get_rfreg(hw, RF90_PATH_A, 0, RFREG_OFFSET_MASK);
-		delay--;
-	}
-	if (delay == 0) {
-		rtl_write_byte(rtlpriv, REG_APSD_CTRL, 0x00);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
-		rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE3);
-		rtl_write_byte(rtlpriv, REG_TXPAUSE, 0x00);
-		RT_TRACE(COMP_POWER, DBG_TRACE,
-			 ("Switch RF timeout !!!.\n"));
-		return;
-	}*/
-	rtl_write_byte(rtlpriv, REG_SYS_FUNC_EN, 0xE2);
-	rtl_write_byte(rtlpriv, REG_SPS0_CTRL, 0x22);
-}
-#endif
-
-static bool _rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
-					    enum rf_pwrstate rfpwr_state)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci_priv *pcipriv = rtl_pcipriv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-	bool bresult = true;
-	u8 i, queue_id;
-	struct rtl8192_tx_ring *ring = NULL;
-
-	switch (rfpwr_state) {
-	case ERFON:{
-			if ((ppsc->rfpwr_state == ERFOFF) &&
-			    RT_IN_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC)) {
-				bool rtstatus = false;
-				u32 InitializeCount = 0;
-				do {
-					InitializeCount++;
-					RT_TRACE(COMP_RF, DBG_DMESG,
-						 ("IPS Set eRf nic enable\n"));
-					rtstatus = rtl_ps_enable_nic(hw);
-				} while ((rtstatus != true)
-					 && (InitializeCount < 10));
-				RT_CLEAR_PS_LEVEL(ppsc,
-						  RT_RF_OFF_LEVL_HALT_NIC);
-			} else {
-				RT_TRACE(COMP_RF, DBG_DMESG,
-					 ("Set ERFON sleeped:%d ms\n",
-					  jiffies_to_msecs(jiffies -
-							   ppsc->
-							   last_sleep_jiffies)));
-				ppsc->last_awake_jiffies = jiffies;
-				rtl8821ae_phy_set_rf_on(hw);
-			}
-			if (mac->link_state == MAC80211_LINKED) {
-				rtlpriv->cfg->ops->led_control(hw,
-							       LED_CTL_LINK);
-			} else {
-				rtlpriv->cfg->ops->led_control(hw,
-							       LED_CTL_NO_LINK);
-			}
-			break;
-		}
-	case ERFOFF:{
-			for (queue_id = 0, i = 0;
-			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
-				ring = &pcipriv->dev.tx_ring[queue_id];
-				if (skb_queue_len(&ring->queue) == 0) {
-					queue_id++;
-					continue;
-				} else {
-					RT_TRACE(COMP_ERR, DBG_WARNING,
-						 ("eRf Off/Sleep: %d times "
-						  "TcbBusyQueue[%d] =%d before "
-						  "doze!\n", (i + 1), queue_id,
-						  skb_queue_len(&ring->queue)));
-
-					udelay(10);
-					i++;
-				}
-				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
-					RT_TRACE(COMP_ERR, DBG_WARNING,
-						 ("\n ERFSLEEP: %d times "
-						  "TcbBusyQueue[%d] = %d !\n",
-						  MAX_DOZE_WAITING_TIMES_9x,
-						  queue_id,
-						  skb_queue_len(&ring->queue)));
-					break;
-				}
-			}
-
-			if (ppsc->reg_rfps_level & RT_RF_OFF_LEVL_HALT_NIC) {
-				RT_TRACE(COMP_RF, DBG_DMESG,
-					 ("IPS Set eRf nic disable\n"));
-				rtl_ps_disable_nic(hw);
-				RT_SET_PS_LEVEL(ppsc, RT_RF_OFF_LEVL_HALT_NIC);
-			} else {
-				if (ppsc->rfoff_reason == RF_CHANGE_BY_IPS) {
-					rtlpriv->cfg->ops->led_control(hw,
-								       LED_CTL_NO_LINK);
-				} else {
-					rtlpriv->cfg->ops->led_control(hw,
-								       LED_CTL_POWER_OFF);
-				}
-			}
-			break;
-		}
-	/*case ERFSLEEP:{
-			if (ppsc->rfpwr_state == ERFOFF)
-				break;
-			for (queue_id = 0, i = 0;
-			     queue_id < RTL_PCI_MAX_TX_QUEUE_COUNT;) {
-				ring = &pcipriv->dev.tx_ring[queue_id];
-				if (skb_queue_len(&ring->queue) == 0) {
-					queue_id++;
-					continue;
-				} else {
-					RT_TRACE(COMP_ERR, DBG_WARNING,
-						 ("eRf Off/Sleep: %d times "
-						  "TcbBusyQueue[%d] =%d before "
-						  "doze!\n", (i + 1), queue_id,
-						  skb_queue_len(&ring->queue)));
-
-					udelay(10);
-					i++;
-				}
-				if (i >= MAX_DOZE_WAITING_TIMES_9x) {
-					RT_TRACE(COMP_ERR, DBG_WARNING,
-						 ("\n ERFSLEEP: %d times "
-						  "TcbBusyQueue[%d] = %d !\n",
-						  MAX_DOZE_WAITING_TIMES_9x,
-						  queue_id,
-						  skb_queue_len(&ring->queue)));
-					break;
-				}
-			}
-			RT_TRACE(COMP_RF, DBG_DMESG,
-				 ("Set ERFSLEEP awaked:%d ms\n",
-				  jiffies_to_msecs(jiffies -
-						   ppsc->last_awake_jiffies)));
-			ppsc->last_sleep_jiffies = jiffies;
-			_rtl8821ae_phy_set_rf_sleep(hw);
-			break;
-		}*/
-	default:
-		RT_TRACE(COMP_ERR, DBG_EMERG,
-			 ("switch case not process \n"));
-		bresult = false;
-		break;
-	}
-	if (bresult)
-		ppsc->rfpwr_state = rfpwr_state;
-	return bresult;
-}
-
-bool rtl8821ae_phy_set_rf_power_state(struct ieee80211_hw *hw,
-				   enum rf_pwrstate rfpwr_state)
-{
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtl_priv(hw));
-
-	bool bresult = false;
-
-	if (rfpwr_state == ppsc->rfpwr_state)
-		return bresult;
-	bresult = _rtl8821ae_phy_set_rf_power_state(hw, rfpwr_state);
-	return bresult;
-}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.h b/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.h
deleted file mode 100644
index 480a6bb6d76b..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/pwrseq.h
+++ /dev/null
@@ -1,413 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8821AE_PWRSEQ_H__
-#define __RTL8821AE_PWRSEQ_H__
-
-#include "pwrseqcmd.h"
-#include "../btcoexist/halbt_precomp.h"
-
-#define	RTL8812_TRANS_CARDEMU_TO_ACT_STEPS	15
-#define	RTL8812_TRANS_ACT_TO_CARDEMU_STEPS	15
-#define	RTL8812_TRANS_CARDEMU_TO_SUS_STEPS	15
-#define	RTL8812_TRANS_SUS_TO_CARDEMU_STEPS	15
-#define	RTL8812_TRANS_CARDEMU_TO_PDN_STEPS	25
-#define	RTL8812_TRANS_PDN_TO_CARDEMU_STEPS	15
-#define	RTL8812_TRANS_ACT_TO_LPS_STEPS	15
-#define	RTL8812_TRANS_LPS_TO_ACT_STEPS	15
-#define	RTL8812_TRANS_END_STEPS	1
-
-
-#define RTL8812_TRANS_CARDEMU_TO_ACT 														\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, 0},/* disable SW LPS 0x04[10]=0*/	\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, BIT1},/* wait till 0x04[17] = 1    power ready*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* disable HWPDN 0x04[15]=0*/ \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, 0},/* disable WL suspend*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/* polling until return 0*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT0, 0},/**/
-
-#define RTL8812_TRANS_ACT_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0c00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xc00[7:0] = 4	turn off 3-wire */	\
-	{0x0e00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xe00[7:0] = 4	turn off 3-wire */	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, /* 0x2[0] = 0	 RESET BB, CLOSE RF */	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},  /* Whole BB is reset*/			\
-	/*{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},//0x1F[7:0] = 0 turn off RF*/	\
-	/*{0x004E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},//0x4C[23] = 0x4E[7] = 0, switch DPDT_SEL_P output from register 0x65[2] */	\
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x2A}, /* 0x07[7:0] = 0x28 sps pwm mode 0x2a for BT coex*/	\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x02, 0},/*0x8[1] = 0 ANA clk =500k */	\
-	/*{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0|BIT1, 0}, //  0x02[1:0] = 0	reset BB */	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/
-
-#define RTL8812_TRANS_CARDEMU_TO_SUS													\
-	/* format */								\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xcc},\
-	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xEC},\
-	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x07},/* gpio11 input mode, gpio10~8 output mode */	\
-	{0x0045, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio 0~7 output same value as input ?? */	\
-	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xff},/* gpio0~7 output mode */	\
-	{0x0047, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/* 0x47[7:0] = 00 gpio mode */	\
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/* suspend option all off */	\
-	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, BIT7},/*0x14[7] = 1 turn on ZCD */	\
-	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, BIT0},/* 0x15[0] =1 turn on ZCD */	\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, BIT4},/*0x23[4] = 1 hpon LDO sleep mode */	\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x02, 0},/*0x8[1] = 0 ANA clk =500k */	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, BIT3}, /*0x04[11] = 2b'11 enable WL suspend for PCIe*/
-
-#define RTL8812_TRANS_SUS_TO_CARDEMU													\
-	/* format */								\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, 0}, /*0x04[11] = 2b'01enable WL suspend*/   \
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, 0},/*0x23[4] = 0 hpon LDO sleep mode leave */	\
-	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, 0},/* 0x15[0] =0 turn off ZCD */	\
-	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, 0},/*0x14[7] = 0 turn off ZCD */	\
-	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio0~7 input mode */	\
-	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio11 input mode, gpio10~8 input mode */
-
-#define RTL8812_TRANS_CARDEMU_TO_CARDDIS													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	/**{0x0194, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, //0x194[0]=0 , disable 32K clock*/	\
-	/**{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x94}, //0x93=0x94 , 90[30] =0 enable 500k ANA clock .switch clock from 12M to 500K , 90 [26] =0 disable EEprom loader clock*/	\
-	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, 0}, /*0x03[2] = 0, reset 8051*/	\
-	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x05}, /*0x80=05h if reload fw, fill the default value of host_CPU handshake field*/	\
-	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xcc},\
-	{0x0042, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xF0, 0xEC},\
-	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x07},/* gpio11 input mode, gpio10~8 output mode */	\
-	{0x0045, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio 0~7 output same value as input ?? */	\
-	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xff},/* gpio0~7 output mode */	\
-	{0x0047, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/* 0x47[7:0] = 00 gpio mode */	\
-	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, BIT7},/*0x14[7] = 1 turn on ZCD */	\
-	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, BIT0},/* 0x15[0] =1 turn on ZCD */	\
-	{0x0012, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, 0},/*0x12[0] = 0 force PFM mode */	\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, BIT4},/*0x23[4] = 1 hpon LDO sleep mode */	\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x02, 0},/*0x8[1] = 0 ANA clk =500k */	\
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07=0x20 , SOP option to disable BG/MB*/	\
-	{0x001f, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0}, /*0x01f[1]=0 , disable RFC_0  control  REG_RF_CTRL_8812 */	\
-	{0x0076, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0}, /*0x076[1]=0 , disable RFC_1  control REG_OPT_CTRL_8812 +2 */	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, BIT3}, /*0x04[11] = 2b'01 enable WL suspend*/
-
-#define RTL8812_TRANS_CARDDIS_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/                       \
-	{0x0012, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/*0x12[0] = 1 force PWM mode */	\
-	{0x0014, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x80, 0},/*0x14[7] = 0 turn off ZCD */	\
-	{0x0015, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x01, 0},/* 0x15[0] =0 turn off ZCD */	\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0x10, 0},/*0x23[4] = 0 hpon LDO leave sleep mode */	\
-	{0x0046, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio0~7 input mode */	\
-	{0x0043, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/* gpio11 input mode, gpio10~8 input mode */ \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, 0}, /*0x04[10] = 0, enable SW LPS PCIE only*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3, 0}, /*0x04[11] = 2b'01enable WL suspend*/	\
-	{0x0003, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, BIT2}, /*0x03[2] = 1, enable 8051*/	\
-	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/*PCIe DMA start*/
-
-
-#define RTL8812_TRANS_CARDEMU_TO_PDN												\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, BIT7},/* 0x04[15] = 1*/
-
-#define RTL8812_TRANS_PDN_TO_CARDEMU												\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* 0x04[15] = 0*/
-
-#define RTL8812_TRANS_ACT_TO_LPS														\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x7F},/*Tx Pause*/		\
-	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x0c00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xc00[7:0] = 4	turn off 3-wire */	\
-	{0x0e00, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x04}, /* 0xe00[7:0] = 4	turn off 3-wire */	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled,and clock are gated,and RF closed*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},  /* Whole BB is reset*/			\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x03},/*Reset MAC TRX*/			\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/		\
-	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, BIT5},/*Respond TxOK to scheduler*/
-
-
-#define RTL8812_TRANS_LPS_TO_ACT															\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/	\
-	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/	\
-	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/	\
-	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/	\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/	\
-	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT7, 0}, /*Polling 0x109[7]=0  TSF in 40M*/			\
-	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6|BIT7, 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/	\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1*/					\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/	\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
-
-#define RTL8812_TRANS_END																\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/		\
-	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,0,PWR_CMD_END, 0, 0}, //
-
-
-extern struct wlan_pwr_cfg  rtl8812_power_on_flow[RTL8812_TRANS_CARDEMU_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_radio_off_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_card_disable_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_card_enable_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_suspend_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_resume_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_SUS_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_hwpdn_flow[RTL8812_TRANS_ACT_TO_CARDEMU_STEPS+RTL8812_TRANS_CARDEMU_TO_PDN_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_enter_lps_flow[RTL8812_TRANS_ACT_TO_LPS_STEPS+RTL8812_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg  rtl8812_leave_lps_flow[RTL8812_TRANS_LPS_TO_ACT_STEPS+RTL8812_TRANS_END_STEPS];
-
-/*
-	Check document WM-20130516-JackieLau-RTL8821A_Power_Architecture-R10.vsd
-	There are 6 HW Power States:
-	0: POFF--Power Off
-	1: PDN--Power Down
-	2: CARDEMU--Card Emulation
-	3: ACT--Active Mode
-	4: LPS--Low Power State
-	5: SUS--Suspend
-
-	The transition from different states are defined below
-	TRANS_CARDEMU_TO_ACT
-	TRANS_ACT_TO_CARDEMU
-	TRANS_CARDEMU_TO_SUS
-	TRANS_SUS_TO_CARDEMU
-	TRANS_CARDEMU_TO_PDN
-	TRANS_ACT_TO_LPS
-	TRANS_LPS_TO_ACT
-
-	TRANS_END
-*/
-#define	RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS	25
-#define	RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS	15
-#define	RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS	15
-#define	RTL8821A_TRANS_SUS_TO_CARDEMU_STEPS	15
-#define RTL8821A_TRANS_CARDDIS_TO_CARDEMU_STEPS	15
-#define	RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS	15
-#define	RTL8821A_TRANS_PDN_TO_CARDEMU_STEPS	15
-#define	RTL8821A_TRANS_ACT_TO_LPS_STEPS	15
-#define	RTL8821A_TRANS_LPS_TO_ACT_STEPS	15
-#define	RTL8821A_TRANS_END_STEPS	1
-
-
-#define RTL8821A_TRANS_CARDEMU_TO_ACT 														\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0}, /*0x20[0] = 1b'1 enable LDOA12 MACRO block for all interface*/   \
-	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*0x67[0] = 0 to disable BT_GPS_SEL pins*/	\
-	{0x0001, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 1, PWRSEQ_DELAY_MS},/*Delay 1ms*/   \
-	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, 0}, /*0x00[5] = 1b'0 release analog Ips to digital ,1:isolation*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, (BIT4|BIT3|BIT2), 0},/* disable SW LPS 0x04[10]=0 and WLSUS_EN 0x04[12:11]=0*/	\
-	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0 , BIT0},/* Disable USB suspend */	\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, BIT1},/* wait till 0x04[17] = 1    power ready*/	\
-	{0x0075, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0 , 0},/* Enable USB suspend */	\
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/* release WLON reset  0x04[16]=1*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* disable HWPDN 0x04[15]=0*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, (BIT4|BIT3), 0},/* disable WL suspend*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/* polling until return 0*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT0, 0},/**/	\
-	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/*0x4C[24] = 0x4F[0] = 1, switch DPDT_SEL_P output from WL BB */\
-	{0x0067, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, (BIT5|BIT4), (BIT5|BIT4)},/*0x66[13] = 0x67[5] = 1, switch for PAPE_G/PAPE_A from WL BB ; 0x66[12] = 0x67[4] = 1, switch LNAON from WL BB */\
-	{0x0025, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6, 0},/*anapar_mac<118> , 0x25[6]=0 by wlan single function*/\
-	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1},/*Enable falling edge triggering interrupt*/\
-	{0x0063, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1},/*Enable GPIO9 interrupt mode*/\
-	{0x0062, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*Enable GPIO9 input mode*/\
-	{0x0058, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, BIT0},/*Enable HSISR GPIO[C:0] interrupt*/\
-	{0x005A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1},/*Enable HSISR GPIO9 interrupt*/\
-	{0x007A, PWR_CUT_TESTCHIP_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x3A},/*0x7A = 0x3A start BT*/\
-	{0x002E, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF , 0x82 },/* 0x2C[23:12]=0x820 ; XTAL trim */ \
-	{0x0010, PWR_CUT_A_MSK , PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6 , BIT6 },/* 0x10[6]=1 ; MP·s¼W¹ï©ó0x2Cªº±±¨îÅv¡A¶·§â0x10[6]³]¬°1¤~¯àÅýWLAN±±¨î */ \
-
-
-#define RTL8821A_TRANS_ACT_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x001F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/*0x1F[7:0] = 0 turn off RF*/	\
-	{0x004F, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/*0x4C[24] = 0x4F[0] = 0, switch DPDT_SEL_P output from register 0x65[2] */\
-	{0x0049, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*Enable rising edge triggering interrupt*/ \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*0x04[9] = 1 turn off MAC by HW state machine*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT1, 0}, /*wait till 0x04[9] = 0 polling until return 0 to disable*/	\
-	{0x0000, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, BIT5}, /*0x00[5] = 1b'1 analog Ips to digital ,1:isolation*/   \
-	{0x0020, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, /*0x20[0] = 1b'0 disable LDOA12 MACRO block*/   \
-
-
-#define RTL8821A_TRANS_CARDEMU_TO_SUS													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4|BIT3, (BIT4|BIT3)}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SDIO SOP option to disable BG/MB/ACK/SWR*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, BIT3|BIT4}, /*0x04[12:11] = 2b'11 enable WL suspend for PCIe*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
-
-#define RTL8821A_TRANS_SUS_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/
-
-#define RTL8821A_TRANS_CARDEMU_TO_CARDDIS													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07=0x20 , SOP option to disable BG/MB*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK|PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, BIT3}, /*0x04[12:11] = 2b'01 enable WL suspend*/	\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT2, BIT2}, /*0x04[10] = 1, enable SW LPS*/	\
-        {0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 1}, /*0x48[16] = 1 to enable GPIO9 as EXT WAKEUP*/   \
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, BIT0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, 0}, /*wait power state to suspend*/
-
-#define RTL8821A_TRANS_CARDDIS_TO_CARDEMU													\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3 | BIT7, 0}, /*clear suspend enable and power down enable*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, BIT0, 0}, /*Set SDIO suspend local register*/	\
-	{0x0086, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_POLLING, BIT1, BIT1}, /*wait power state to suspend*/\
-	{0x004A, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0}, /*0x48[16] = 0 to disable GPIO9 as EXT WAKEUP*/   \
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT3|BIT4, 0}, /*0x04[12:11] = 2b'01enable WL suspend*/\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*0x23[4] = 1b'0 12H LDO enter normal mode*/   \
-	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0},/*PCIe DMA start*/
-
-
-#define RTL8821A_TRANS_CARDEMU_TO_PDN												\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0023, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, BIT4}, /*0x23[4] = 1b'1 12H LDO enter sleep mode*/   \
-	{0x0007, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK|PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x20}, /*0x07[7:0] = 0x20 SOP option to disable BG/MB/ACK/SWR*/   \
-	{0x0006, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/* 0x04[16] = 0*/\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, BIT7},/* 0x04[15] = 1*/
-
-#define RTL8821A_TRANS_PDN_TO_CARDEMU												\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0005, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT7, 0},/* 0x04[15] = 0*/
-
-#define RTL8821A_TRANS_ACT_TO_LPS														\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0301, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF},/*PCIe DMA stop*/	\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF},/*Tx Pause*/	\
-	{0x05F8, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05F9, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05FA, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x05FB, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, 0xFF, 0},/*Should be zero if no packet is transmitting*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT0, 0},/*CCK and OFDM are disabled,and clock are gated*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_US},/*Delay 1us*/	\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*Whole BB is reset*/	\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x03},/*Reset MAC TRX*/	\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, 0},/*check if removed later*/	\
-	{0x0093, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x00},/*When driver enter Sus/ Disable, enable LOP for BT*/	\
-	{0x0553, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT5, BIT5},/*Respond TxOK to scheduler*/	\
-
-
-#define RTL8821A_TRANS_LPS_TO_ACT															\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0x0080, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_SDIO_MSK,PWR_BASEADDR_SDIO,PWR_CMD_WRITE, 0xFF, 0x84}, /*SDIO RPWM*/\
-	{0xFE58, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_USB_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*USB RPWM*/\
-	{0x0361, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_PCI_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0x84}, /*PCIe RPWM*/\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_DELAY, 0, PWRSEQ_DELAY_MS}, /*Delay*/\
-	{0x0008, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT4, 0}, /*.	0x08[4] = 0		 switch TSF to 40M*/\
-	{0x0109, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_POLLING, BIT7, 0}, /*Polling 0x109[7]=0  TSF in 40M*/\
-	{0x0029, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT6|BIT7, 0}, /*.	0x29[7:6] = 2b'00	 enable BB clock*/\
-	{0x0101, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1, BIT1}, /*.	0x101[1] = 1*/\
-	{0x0100, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0xFF}, /*.	0x100[7:0] = 0xFF	 enable WMAC TRX*/\
-	{0x0002, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, BIT1|BIT0, BIT1|BIT0}, /*.	0x02[1:0] = 2b'11	 enable BB macro*/\
-	{0x0522, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,PWR_BASEADDR_MAC,PWR_CMD_WRITE, 0xFF, 0}, /*.	0x522 = 0*/
-
-#define RTL8821A_TRANS_END															\
-	/* format */																\
-	/* { offset, cut_msk, fab_msk|interface_msk, base|cmd, msk, value }, // comments here*/								\
-	{0xFFFF, PWR_CUT_ALL_MSK, PWR_FAB_ALL_MSK, PWR_INTF_ALL_MSK,0,PWR_CMD_END, 0, 0}, //
-
-extern struct wlan_pwr_cfg rtl8821A_power_on_flow[RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_radio_off_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_card_disable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-						+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_card_enable_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-						+ RTL8821A_TRANS_CARDEMU_TO_ACT_STEPS/*RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS*/
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_suspend_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-						+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_resume_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-						+ RTL8821A_TRANS_CARDEMU_TO_SUS_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_hwpdn_flow[RTL8821A_TRANS_ACT_TO_CARDEMU_STEPS
-						+ RTL8821A_TRANS_CARDEMU_TO_PDN_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_enter_lps_flow[RTL8821A_TRANS_ACT_TO_LPS_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-extern struct wlan_pwr_cfg rtl8821A_leave_lps_flow[RTL8821A_TRANS_LPS_TO_ACT_STEPS
-						+ RTL8821A_TRANS_END_STEPS];
-
-/*RTL8812 Power Configuration CMDs for PCIe interface*/
-#define RTL8812_NIC_PWR_ON_FLOW				rtl8812_power_on_flow
-#define RTL8812_NIC_RF_OFF_FLOW				rtl8812_radio_off_flow
-#define RTL8812_NIC_DISABLE_FLOW 			rtl8812_card_disable_flow
-#define RTL8812_NIC_ENABLE_FLOW				rtl8812_card_enable_flow
-#define RTL8812_NIC_SUSPEND_FLOW			rtl8812_suspend_flow
-#define RTL8812_NIC_RESUME_FLOW				rtl8812_resume_flow
-#define RTL8812_NIC_PDN_FLOW					rtl8812_hwpdn_flow
-#define RTL8812_NIC_LPS_ENTER_FLOW			rtl8812_enter_lps_flow
-#define RTL8812_NIC_LPS_LEAVE_FLOW			rtl8812_leave_lps_flow
-
-/* RTL8821 Power Configuration CMDs for PCIe interface */
-#define RTL8821A_NIC_PWR_ON_FLOW			rtl8821A_power_on_flow
-#define RTL8821A_NIC_RF_OFF_FLOW			rtl8821A_radio_off_flow
-#define RTL8821A_NIC_DISABLE_FLOW			rtl8821A_card_disable_flow
-#define RTL8821A_NIC_ENABLE_FLOW			rtl8821A_card_enable_flow
-#define RTL8821A_NIC_SUSPEND_FLOW			rtl8821A_suspend_flow
-#define RTL8821A_NIC_RESUME_FLOW			rtl8821A_resume_flow
-#define RTL8821A_NIC_PDN_FLOW				rtl8821A_hwpdn_flow
-#define RTL8821A_NIC_LPS_ENTER_FLOW			rtl8821A_enter_lps_flow
-#define RTL8821A_NIC_LPS_LEAVE_FLOW			rtl8821A_leave_lps_flow
-
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.c b/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.c
deleted file mode 100644
index ff1887187770..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.c
+++ /dev/null
@@ -1,140 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "pwrseq.h"
-
-
-/*
-*	Description:
-*		This routine deal with the Power Configuration CMDs
-*		 parsing for RTL8723/RTL8188E Series IC.
-*	Assumption:
-*		We should follow specific format which was released from HW SD.
-*
-*	2011.07.07, added by Roger.
-*/
-bool rtl_hal_pwrseqcmdparsing (struct rtl_priv* rtlpriv, u8 cut_version,
-										u8 fab_version, u8 interface_type,
-										struct wlan_pwr_cfg	pwrcfgcmd[])
-
-{
-	struct wlan_pwr_cfg pwr_cfg_cmd = {0};
-	bool polling_bit = false;
-	u32 ary_idx=0;
-	u8 value = 0;
-	u32 offset = 0;
-	u32 polling_count = 0;
-	u32 max_polling_cnt = 5000;
-
-	do {
-		pwr_cfg_cmd = pwrcfgcmd[ary_idx];
-		RT_TRACE(COMP_INIT, DBG_TRACE,
-			("rtl_hal_pwrseqcmdparsing(): offset(%#x),cut_msk(%#x), fab_msk(%#x),"
-			"interface_msk(%#x), base(%#x), cmd(%#x), msk(%#x), value(%#x)\n",
-			GET_PWR_CFG_OFFSET(pwr_cfg_cmd), GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd),
-			GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd), GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd),
-			GET_PWR_CFG_BASE(pwr_cfg_cmd), GET_PWR_CFG_CMD(pwr_cfg_cmd),
-			GET_PWR_CFG_MASK(pwr_cfg_cmd), GET_PWR_CFG_VALUE(pwr_cfg_cmd)));
-
-		if ((GET_PWR_CFG_FAB_MASK(pwr_cfg_cmd)&fab_version) &&
-			(GET_PWR_CFG_CUT_MASK(pwr_cfg_cmd)&cut_version) &&
-			(GET_PWR_CFG_INTF_MASK(pwr_cfg_cmd)&interface_type)) {
-			switch (GET_PWR_CFG_CMD(pwr_cfg_cmd)) {
-			case PWR_CMD_READ:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_READ\n"));
-				break;
-
-			case PWR_CMD_WRITE: {
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_WRITE\n"));
-				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
-
-					/*Read the value from system register*/
-					value = rtl_read_byte(rtlpriv, offset);
-					value = value & (~(GET_PWR_CFG_MASK(pwr_cfg_cmd)));
-					value = value | (GET_PWR_CFG_VALUE(pwr_cfg_cmd)
-							& GET_PWR_CFG_MASK(pwr_cfg_cmd));
-
-					/*Write the value back to system register*/
-					rtl_write_byte(rtlpriv, offset, value);
-				}
-				break;
-
-			case PWR_CMD_POLLING:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_POLLING\n"));
-				polling_bit = false;
-				offset = GET_PWR_CFG_OFFSET(pwr_cfg_cmd);
-
-				do {
-					value = rtl_read_byte(rtlpriv, offset);
-
-					value = value & GET_PWR_CFG_MASK(pwr_cfg_cmd);
-					if (value == (GET_PWR_CFG_VALUE(pwr_cfg_cmd)
-							& GET_PWR_CFG_MASK(pwr_cfg_cmd)))
-						polling_bit=true;
-					else
-						udelay(10);
-
-					if (polling_count++ > max_polling_cnt) {
-						return false;
-					}
-				} while (!polling_bit);
-
-				break;
-
-			case PWR_CMD_DELAY:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_DELAY\n"));
-				if (GET_PWR_CFG_VALUE(pwr_cfg_cmd) == PWRSEQ_DELAY_US)
-					udelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
-				else
-					mdelay(GET_PWR_CFG_OFFSET(pwr_cfg_cmd));
-				break;
-
-			case PWR_CMD_END:
-				RT_TRACE(COMP_INIT, DBG_TRACE,
-					("rtl_hal_pwrseqcmdparsing(): PWR_CMD_END\n"));
-				return true;
-				break;
-
-			default:
-				RT_ASSERT(false,
-					("rtl_hal_pwrseqcmdparsing(): Unknown CMD!!\n"));
-				break;
-			}
-
-		}
-
-		ary_idx++;
-	} while (1);
-
-	return true;
-}
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.h b/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.h
deleted file mode 100644
index 571e7e50d5b5..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/pwrseqcmd.h
+++ /dev/null
@@ -1,71 +0,0 @@
-#ifndef __RTL8821AE_PWRSEQCMD_H__
-#define __RTL8821AE_PWRSEQCMD_H__
-
-#include "../wifi.h"
-/*---------------------------------------------*/
-/*The value of cmd: 4 bits */
-/*---------------------------------------------*/
-#define	PWR_CMD_READ 		0x00
-#define	PWR_CMD_WRITE	0x01
-#define	PWR_CMD_POLLING	0x02
-#define	PWR_CMD_DELAY	0x03
-#define	PWR_CMD_END		0x04
-
-/* define the base address of each block */
-#define	PWR_BASEADDR_MAC	0x00
-#define	PWR_BASEADDR_USB	0x01
-#define	PWR_BASEADDR_PCIE	0x02
-#define	PWR_BASEADDR_SDIO	0x03
-
-#define	PWR_INTF_SDIO_MSK	BIT(0)
-#define	PWR_INTF_USB_MSK	BIT(1)
-#define	PWR_INTF_PCI_MSK	BIT(2)
-#define	PWR_INTF_ALL_MSK	(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-
-#define	PWR_FAB_TSMC_MSK	BIT(0)
-#define	PWR_FAB_UMC_MSK		BIT(1)
-#define	PWR_FAB_ALL_MSK		(BIT(0)|BIT(1)|BIT(2)|BIT(3))
-
-#define	PWR_CUT_TESTCHIP_MSK	BIT(0)
-#define	PWR_CUT_A_MSK		BIT(1)
-#define	PWR_CUT_B_MSK		BIT(2)
-#define	PWR_CUT_C_MSK		BIT(3)
-#define	PWR_CUT_D_MSK		BIT(4)
-#define	PWR_CUT_E_MSK		BIT(5)
-#define	PWR_CUT_F_MSK		BIT(6)
-#define	PWR_CUT_G_MSK		BIT(7)
-#define	PWR_CUT_ALL_MSK		0xFF
-
-
-enum pwrseq_delay_unit {
-   PWRSEQ_DELAY_US,
-   PWRSEQ_DELAY_MS,
-};
-
-struct wlan_pwr_cfg {
-	u16 offset;
-	u8 cut_msk;
-	u8 fab_msk:4;
-	u8 interface_msk:4;
-	u8 base:4;
-	u8 cmd:4;
-	u8 msk;
-	u8 value;
-
-};
-
-#define	GET_PWR_CFG_OFFSET(__PWR_CMD)	__PWR_CMD.offset
-#define	GET_PWR_CFG_CUT_MASK(__PWR_CMD)	__PWR_CMD.cut_msk
-#define	GET_PWR_CFG_FAB_MASK(__PWR_CMD)	__PWR_CMD.fab_msk
-#define	GET_PWR_CFG_INTF_MASK(__PWR_CMD)	__PWR_CMD.interface_msk
-#define	GET_PWR_CFG_BASE(__PWR_CMD)	__PWR_CMD.base
-#define	GET_PWR_CFG_CMD(__PWR_CMD)	__PWR_CMD.cmd
-#define	GET_PWR_CFG_MASK(__PWR_CMD)	__PWR_CMD.msk
-#define	GET_PWR_CFG_VALUE(__PWR_CMD)	__PWR_CMD.value
-
-bool rtl_hal_pwrseqcmdparsing(struct rtl_priv * rtlpriv, u8 cut_version,
-									   u8 fab_version, u8 interface_type,
-									   struct wlan_pwr_cfg pwrcfgcmd[]);
-
-#endif
-
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/reg.h b/drivers/staging/rtl8821ae/rtl8821ae/reg.h
deleted file mode 100644
index 4cb3ca95f773..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/reg.h
+++ /dev/null
@@ -1,2428 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8821AE_REG_H__
-#define __RTL8821AE_REG_H__
-
-#define TXPKT_BUF_SELECT				0x69
-#define RXPKT_BUF_SELECT				0xA5
-#define DISABLE_TRXPKT_BUF_ACCESS			0x0
-
-#define REG_SYS_ISO_CTRL			0x0000
-#define REG_SYS_FUNC_EN				0x0002
-#define REG_APS_FSMCO				0x0004
-#define REG_SYS_CLKR				0x0008
-#define REG_9346CR					0x000A
-#define REG_EE_VPD					0x000C
-#define REG_AFE_MISC				0x0010
-#define REG_SPS0_CTRL				0x0011
-#define REG_SPS_OCP_CFG				0x0018
-#define REG_RSV_CTRL				0x001C
-#define REG_RF_CTRL					0x001F
-#define REG_LDOA15_CTRL				0x0020
-#define REG_LDOV12D_CTRL			0x0021
-#define REG_LDOHCI12_CTRL			0x0022
-#define REG_LPLDO_CTRL				0x0023
-#define REG_AFE_XTAL_CTRL			0x0024
-#define REG_AFE_LDO_CTRL                	0x0027 /* 1.5v for 8188EE test chip, 1.4v for MP chip */
-#define REG_AFE_PLL_CTRL			0x0028
-#define REG_MAC_PHY_CTRL			0x002c
-#define REG_EFUSE_CTRL				0x0030
-#define REG_EFUSE_TEST				0x0034
-#define REG_PWR_DATA				0x0038
-#define REG_CAL_TIMER				0x003C
-#define REG_ACLK_MON				0x003E
-#define REG_GPIO_MUXCFG			0x0040
-#define REG_GPIO_IO_SEL				0x0042
-#define REG_MAC_PINMUX_CFG		0x0043
-#define REG_GPIO_PIN_CTRL			0x0044
-#define REG_GPIO_INTM				0x0048
-#define REG_LEDCFG0					0x004C
-#define REG_LEDCFG1					0x004D
-#define REG_LEDCFG2					0x004E
-#define REG_LEDCFG3					0x004F
-#define REG_FSIMR					0x0050
-#define REG_FSISR					0x0054
-#define REG_HSIMR					0x0058
-#define REG_HSISR					0x005c
-#define REG_GPIO_PIN_CTRL_2		0x0060
-#define REG_GPIO_IO_SEL_2			0x0062
-#define REG_MULTI_FUNC_CTRL			0x0068
-#define REG_GPIO_OUTPUT			0x006c
-#define REG_OPT_CTRL			0x0074
-#define REG_AFE_XTAL_CTRL_EXT		0x0078
-#define REG_XCK_OUT_CTRL			0x007c
-#define REG_MCUFWDL				0x0080
-#define REG_WOL_EVENT				0x0081
-#define REG_MCUTSTCFG				0x0084
-
-
-#define REG_HIMR					0x00B0
-#define REG_HISR					0x00B4
-#define REG_HIMRE					0x00B8
-#define REG_HISRE					0x00BC
-
-#define REG_PMC_DBG_CTRL2			0x00CC
-
-#define REG_EFUSE_ACCESS			0x00CF
-
-#define REG_BIST_SCAN				0x00D0
-#define REG_BIST_RPT				0x00D4
-#define REG_BIST_ROM_RPT			0x00D8
-#define REG_USB_SIE_INTF			0x00E0
-#define REG_PCIE_MIO_INTF			0x00E4
-#define REG_PCIE_MIO_INTD			0x00E8
-#define REG_HPON_FSM				0x00EC
-#define REG_SYS_CFG					0x00F0
-#define REG_GPIO_OUTSTS				0x00F4
-#define REG_SYS_CFG1				0x00FC
-#define REG_ROM_VERSION				0x00FD
-
-#define REG_CR						0x0100
-#define REG_PBP						0x0104
-#define REG_PKT_BUFF_ACCESS_CTRL			0x0106
-#define REG_TRXDMA_CTRL				0x010C
-#define REG_TRXFF_BNDY				0x0114
-#define REG_TRXFF_STATUS			0x0118
-#define REG_RXFF_PTR				0x011C
-
-#define REG_CPWM					0x012F
-#define REG_FWIMR					0x0130
-#define REG_FWISR					0x0134
-#define REG_PKTBUF_DBG_CTRL			0x0140
-#define REG_PKTBUF_DBG_DATA_L		0x0144
-#define REG_PKTBUF_DBG_DATA_H		0x0148
-#define REG_RXPKTBUF_CTRL				(REG_PKTBUF_DBG_CTRL+2)
-
-#define REG_TC0_CTRL				0x0150
-#define REG_TC1_CTRL				0x0154
-#define REG_TC2_CTRL				0x0158
-#define REG_TC3_CTRL				0x015C
-#define REG_TC4_CTRL				0x0160
-#define REG_TCUNIT_BASE				0x0164
-#define REG_MBIST_START				0x0174
-#define REG_MBIST_DONE				0x0178
-#define REG_MBIST_FAIL				0x017C
-#define REG_32K_CTRL					0x0194
-#define REG_C2HEVT_MSG_NORMAL		0x01A0
-#define REG_C2HEVT_CLEAR			0x01AF
-#define REG_C2HEVT_MSG_TEST			0x01B8
-#define REG_MCUTST_1				0x01c0
-#define REG_FMETHR					0x01C8
-#define REG_HMETFR					0x01CC
-#define REG_HMEBOX_0				0x01D0
-#define REG_HMEBOX_1				0x01D4
-#define REG_HMEBOX_2				0x01D8
-#define REG_HMEBOX_3				0x01DC
-
-#define REG_LLT_INIT				0x01E0
-#define REG_BB_ACCEESS_CTRL			0x01E8
-#define REG_BB_ACCESS_DATA			0x01EC
-
-#define REG_HMEBOX_EXT_0			0x01F0
-#define REG_HMEBOX_EXT_1			0x01F4
-#define REG_HMEBOX_EXT_2			0x01F8
-#define REG_HMEBOX_EXT_3			0x01FC
-
-#define REG_RQPN					0x0200
-#define REG_FIFOPAGE				0x0204
-#define REG_TDECTRL					0x0208
-#define REG_TXDMA_OFFSET_CHK		0x020C
-#define REG_TXDMA_STATUS			0x0210
-#define REG_RQPN_NPQ				0x0214
-
-#define REG_RXDMA_AGG_PG_TH		0x0280
-#define REG_FW_UPD_RDPTR			0x0284 /* FW shall update this register before FW write RXPKT_RELEASE_POLL to 1 */
-#define REG_RXDMA_CONTROL			0x0286 /* Control the RX DMA.*/
-#define REG_RXPKT_NUM				0x0287 /* The number of packets in RXPKTBUF.	*/
-
-#define	REG_PCIE_CTRL_REG			0x0300
-#define	REG_INT_MIG					0x0304
-#define	REG_BCNQ_DESA				0x0308
-#define	REG_HQ_DESA					0x0310
-#define	REG_MGQ_DESA				0x0318
-#define	REG_VOQ_DESA				0x0320
-#define	REG_VIQ_DESA				0x0328
-#define	REG_BEQ_DESA				0x0330
-#define	REG_BKQ_DESA				0x0338
-#define	REG_RX_DESA					0x0340
-
-#define	REG_DBI_WDATA				0x0348
-#define	REG_DBI_RDATA				0x034C
-#define	REG_DBI_ADDR				0x0350
-#define	REG_DBI_FLAG				0x0352
-#define	REG_MDIO_WDATA				0x0354
-#define	REG_MDIO_RDATA				0x0356
-#define	REG_MDIO_CTL				0x0358
-#define	REG_DBG_SEL					0x0360
-#define	REG_PCIE_HRPWM				0x0361
-#define	REG_PCIE_HCPWM				0x0363
-#define	REG_UART_CTRL				0x0364
-#define	REG_WATCH_DOG				0x0368
-#define	REG_UART_TX_DESA			0x0370
-#define	REG_UART_RX_DESA			0x0378
-
-
-#define	REG_HDAQ_DESA_NODEF			0x0000
-#define	REG_CMDQ_DESA_NODEF			0x0000
-
-#define REG_VOQ_INFORMATION			0x0400
-#define REG_VIQ_INFORMATION			0x0404
-#define REG_BEQ_INFORMATION			0x0408
-#define REG_BKQ_INFORMATION			0x040C
-#define REG_MGQ_INFORMATION			0x0410
-#define REG_HGQ_INFORMATION			0x0414
-#define REG_BCNQ_INFORMATION			0x0418
-#define REG_TXPKT_EMPTY				0x041A
-
-
-#define REG_CPU_MGQ_INFORMATION		0x041C
-#define REG_FWHW_TXQ_CTRL			0x0420
-#define REG_HWSEQ_CTRL				0x0423
-#define REG_TXPKTBUF_BCNQ_BDNY		0x0424
-#define REG_TXPKTBUF_MGQ_BDNY		0x0425
-#define REG_MULTI_BCNQ_EN			0x0426
-#define REG_MULTI_BCNQ_OFFSET		0x0427
-#define REG_SPEC_SIFS				0x0428
-#define REG_RL						0x042A
-#define REG_DARFRC					0x0430
-#define REG_RARFRC					0x0438
-#define REG_RRSR					0x0440
-#define REG_ARFR0					0x0444
-#define REG_ARFR1					0x044C
-#define REG_CCK_CHECK				0x0454
-#define REG_AMPDU_MAX_TIME			0x0456
-#define REG_AGGLEN_LMT				0x0458
-#define REG_AMPDU_MIN_SPACE			0x045C
-#define REG_TXPKTBUF_WMAC_LBK_BF_HD	0x045D
-#define REG_FAST_EDCA_CTRL			0x0460
-#define REG_RD_RESP_PKT_TH			0x0463
-#define REG_INIRTS_RATE_SEL			0x0480
-#define REG_INIDATA_RATE_SEL		0x0484
-#define REG_ARFR2					0x048C
-#define REG_ARFR3					0x0494
-#define REG_POWER_STATUS			0x04A4
-#define REG_POWER_STAGE1			0x04B4
-#define REG_POWER_STAGE2			0x04B8
-#define REG_PKT_LIFE_TIME			0x04C0
-#define REG_STBC_SETTING			0x04C4
-#define REG_HT_SINGLE_AMPDU			0x04C7
-#define REG_PROT_MODE_CTRL			0x04C8
-#define REG_MAX_AGGR_NUM			0x04CA
-#define REG_BAR_MODE_CTRL			0x04CC
-#define REG_RA_TRY_RATE_AGG_LMT		0x04CF
-#define REG_EARLY_MODE_CONTROL			0x04D0
-#define REG_NQOS_SEQ				0x04DC
-#define REG_QOS_SEQ					0x04DE
-#define REG_NEED_CPU_HANDLE			0x04E0
-#define REG_PKT_LOSE_RPT			0x04E1
-#define REG_PTCL_ERR_STATUS			0x04E2
-#define REG_TX_RPT_CTRL				0x04EC
-#define REG_TX_RPT_TIME				0x04F0
-#define REG_DUMMY					0x04FC
-
-#define REG_EDCA_VO_PARAM			0x0500
-#define REG_EDCA_VI_PARAM			0x0504
-#define REG_EDCA_BE_PARAM			0x0508
-#define REG_EDCA_BK_PARAM			0x050C
-#define REG_BCNTCFG					0x0510
-#define REG_PIFS					0x0512
-#define REG_RDG_PIFS				0x0513
-#define REG_SIFS_CTX				0x0514
-#define REG_SIFS_TRX				0x0516
-#define REG_AGGR_BREAK_TIME			0x051A
-#define REG_SLOT					0x051B
-#define REG_TX_PTCL_CTRL			0x0520
-#define REG_TXPAUSE					0x0522
-#define REG_DIS_TXREQ_CLR			0x0523
-#define REG_RD_CTRL					0x0524
-#define REG_TBTT_PROHIBIT			0x0540
-#define REG_RD_NAV_NXT				0x0544
-#define REG_NAV_PROT_LEN			0x0546
-#define REG_BCN_CTRL				0x0550
-#define REG_USTIME_TSF				0x0551
-#define REG_MBID_NUM				0x0552
-#define REG_DUAL_TSF_RST			0x0553
-#define REG_BCN_INTERVAL			0x0554
-#define REG_MBSSID_BCN_SPACE		0x0554
-#define REG_DRVERLYINT				0x0558
-#define REG_BCNDMATIM				0x0559
-#define REG_ATIMWND					0x055A
-#define REG_BCN_MAX_ERR				0x055D
-#define REG_RXTSF_OFFSET_CCK		0x055E
-#define REG_RXTSF_OFFSET_OFDM		0x055F
-#define REG_TSFTR					0x0560
-#define REG_INIT_TSFTR				0x0564
-#define REG_SECONDARY_CCA_CTRL		0x0577
-#define REG_PSTIMER					0x0580
-#define REG_TIMER0					0x0584
-#define REG_TIMER1					0x0588
-#define REG_ACMHWCTRL				0x05C0
-#define REG_ACMRSTCTRL				0x05C1
-#define REG_ACMAVG					0x05C2
-#define REG_VO_ADMTIME				0x05C4
-#define REG_VI_ADMTIME				0x05C6
-#define REG_BE_ADMTIME				0x05C8
-#define REG_EDCA_RANDOM_GEN			0x05CC
-#define REG_NOA_DESC_SEL			0x05CF
-#define REG_NOA_DESC_DURATION		0x05E0
-#define REG_NOA_DESC_INTERVAL		0x05E4
-#define REG_NOA_DESC_START			0x05E8
-#define REG_NOA_DESC_COUNT			0x05EC
-#define REG_SCH_TX_CMD				0x05F8
-
-#define REG_APSD_CTRL				0x0600
-#define REG_BWOPMODE				0x0603
-#define REG_TCR						0x0604
-#define REG_RCR						0x0608
-#define REG_RX_PKT_LIMIT			0x060C
-#define REG_RX_DLK_TIME				0x060D
-#define REG_RX_DRVINFO_SZ			0x060F
-
-#define REG_MACID					0x0610
-#define REG_BSSID					0x0618
-#define REG_MAR						0x0620
-#define REG_MBIDCAMCFG				0x0628
-
-#define REG_USTIME_EDCA				0x0638
-#define REG_MAC_SPEC_SIFS			0x063A
-#define REG_RESP_SIFS_CCK			0x063C
-#define REG_RESP_SIFS_OFDM			0x063E
-#define REG_ACKTO					0x0640
-#define REG_CTS2TO					0x0641
-#define REG_EIFS					0x0642
-
-#define REG_NAV_CTRL				0x0650
-#define REG_NAV_UPPER				0x0652
-#define REG_BACAMCMD				0x0654
-#define REG_BACAMCONTENT			0x0658
-#define REG_LBDLY					0x0660
-#define REG_FWDLY					0x0661
-#define REG_RXERR_RPT				0x0664
-#define REG_TRXPTCL_CTL				0x0668
-
-#define REG_CAMCMD					0x0670
-#define REG_CAMWRITE				0x0674
-#define REG_CAMREAD					0x0678
-#define REG_CAMDBG					0x067C
-#define REG_SECCFG					0x0680
-
-#define REG_WOW_CTRL				0x0690
-#define REG_PSSTATUS				0x0691
-#define REG_PS_RX_INFO				0x0692
-#define REG_UAPSD_TID				0x0693
-#define REG_LPNAV_CTRL				0x0694
-#define REG_WKFMCAM_NUM				0x0698
-#define REG_WKFMCAM_RWD				0x069C
-#define REG_RXFLTMAP0				0x06A0
-#define REG_RXFLTMAP1				0x06A2
-#define REG_RXFLTMAP2				0x06A4
-#define REG_BCN_PSR_RPT				0x06A8
-#define REG_CALB32K_CTRL			0x06AC
-#define REG_PKT_MON_CTRL			0x06B4
-#define REG_BT_COEX_TABLE			0x06C0
-#define REG_WMAC_RESP_TXINFO		0x06D8
-
-#define REG_USB_INFO				0xFE17
-#define REG_USB_SPECIAL_OPTION		0xFE55
-#define REG_USB_DMA_AGG_TO			0xFE5B
-#define REG_USB_AGG_TO				0xFE5C
-#define REG_USB_AGG_TH				0xFE5D
-
-#define REG_TEST_USB_TXQS			0xFE48
-#define REG_TEST_SIE_VID			0xFE60
-#define REG_TEST_SIE_PID			0xFE62
-#define REG_TEST_SIE_OPTIONAL		0xFE64
-#define REG_TEST_SIE_CHIRP_K		0xFE65
-#define REG_TEST_SIE_PHY			0xFE66
-#define REG_TEST_SIE_MAC_ADDR		0xFE70
-#define REG_TEST_SIE_STRING			0xFE80
-
-#define REG_NORMAL_SIE_VID			0xFE60
-#define REG_NORMAL_SIE_PID			0xFE62
-#define REG_NORMAL_SIE_OPTIONAL		0xFE64
-#define REG_NORMAL_SIE_EP			0xFE65
-#define REG_NORMAL_SIE_PHY			0xFE68
-#define REG_NORMAL_SIE_MAC_ADDR		0xFE70
-#define REG_NORMAL_SIE_STRING		0xFE80
-
-#define	CR9346				REG_9346CR
-#define	MSR				(REG_CR + 2)
-#define	ISR				REG_HISR
-#define	TSFR				REG_TSFTR
-
-#define	MACIDR0				REG_MACID
-#define	MACIDR4				(REG_MACID + 4)
-
-#define PBP				REG_PBP
-
-#define	IDR0				MACIDR0
-#define	IDR4				MACIDR4
-
-#define	UNUSED_REGISTER			0x1BF
-#define	DCAM				UNUSED_REGISTER
-#define	PSR				UNUSED_REGISTER
-#define BBADDR				UNUSED_REGISTER
-#define	PHYDATAR			UNUSED_REGISTER
-
-#define	INVALID_BBRF_VALUE		0x12345678
-
-#define	MAX_MSS_DENSITY_2T 		0x13
-#define	MAX_MSS_DENSITY_1T 		0x0A
-
-#define	CMDEEPROM_EN			BIT(5)
-#define	CMDEEPROM_SEL			BIT(4)
-#define	CMD9346CR_9356SEL		BIT(4)
-#define	AUTOLOAD_EEPROM			(CMDEEPROM_EN|CMDEEPROM_SEL)
-#define	AUTOLOAD_EFUSE			CMDEEPROM_EN
-
-#define	GPIOSEL_GPIO			0
-#define	GPIOSEL_ENBT			BIT(5)
-
-#define	GPIO_IN				REG_GPIO_PIN_CTRL
-#define	GPIO_OUT			(REG_GPIO_PIN_CTRL+1)
-#define	GPIO_IO_SEL			(REG_GPIO_PIN_CTRL+2)
-#define	GPIO_MOD			(REG_GPIO_PIN_CTRL+3)
-
-/*      8723/8188E Host System Interrupt Mask Register (offset 0x58, 32 byte) */
-#define	HSIMR_GPIO12_0_INT_EN			BIT(0)
-#define	HSIMR_SPS_OCP_INT_EN			BIT(5)
-#define	HSIMR_RON_INT_EN			BIT(6)
-#define	HSIMR_PDN_INT_EN			BIT(7)
-#define	HSIMR_GPIO9_INT_EN			BIT(25)
-
-
-/*
-*       8723/8188E Host System Interrupt Status Register (offset 0x5C, 32 byte)
-*/
-#define	HSISR_GPIO12_0_INT			BIT(0)
-#define	HSISR_SPS_OCP_INT			BIT(5)
-#define	HSISR_RON_INT_EN			BIT(6)
-#define	HSISR_PDNINT				BIT(7)
-#define	HSISR_GPIO9_INT				BIT(25)
-
-#define	MSR_NOLINK					0x00
-#define	MSR_ADHOC					0x01
-#define	MSR_INFRA					0x02
-#define	MSR_AP						0x03
-#define	MSR_MASK					0x03
-
-#define	RRSR_RSC_OFFSET				21
-#define	RRSR_SHORT_OFFSET			23
-#define	RRSR_RSC_BW_40M				0x600000
-#define	RRSR_RSC_UPSUBCHNL			0x400000
-#define	RRSR_RSC_LOWSUBCHNL			0x200000
-#define	RRSR_SHORT					0x800000
-#define	RRSR_1M						BIT(0)
-#define	RRSR_2M						BIT(1)
-#define	RRSR_5_5M					BIT(2)
-#define	RRSR_11M					BIT(3)
-#define	RRSR_6M						BIT(4)
-#define	RRSR_9M						BIT(5)
-#define	RRSR_12M					BIT(6)
-#define	RRSR_18M					BIT(7)
-#define	RRSR_24M					BIT(8)
-#define	RRSR_36M					BIT(9)
-#define	RRSR_48M					BIT(10)
-#define	RRSR_54M					BIT(11)
-#define	RRSR_MCS0					BIT(12)
-#define	RRSR_MCS1					BIT(13)
-#define	RRSR_MCS2					BIT(14)
-#define	RRSR_MCS3					BIT(15)
-#define	RRSR_MCS4					BIT(16)
-#define	RRSR_MCS5					BIT(17)
-#define	RRSR_MCS6					BIT(18)
-#define	RRSR_MCS7					BIT(19)
-#define	BRSR_ACKSHORTPMB			BIT(23)
-
-#define	RATR_1M						0x00000001
-#define	RATR_2M						0x00000002
-#define	RATR_55M					0x00000004
-#define	RATR_11M					0x00000008
-#define	RATR_6M						0x00000010
-#define	RATR_9M						0x00000020
-#define	RATR_12M					0x00000040
-#define	RATR_18M					0x00000080
-#define	RATR_24M					0x00000100
-#define	RATR_36M					0x00000200
-#define	RATR_48M					0x00000400
-#define	RATR_54M					0x00000800
-#define	RATR_MCS0					0x00001000
-#define	RATR_MCS1					0x00002000
-#define	RATR_MCS2					0x00004000
-#define	RATR_MCS3					0x00008000
-#define	RATR_MCS4					0x00010000
-#define	RATR_MCS5					0x00020000
-#define	RATR_MCS6					0x00040000
-#define	RATR_MCS7					0x00080000
-#define	RATR_MCS8					0x00100000
-#define	RATR_MCS9					0x00200000
-#define	RATR_MCS10					0x00400000
-#define	RATR_MCS11					0x00800000
-#define	RATR_MCS12					0x01000000
-#define	RATR_MCS13					0x02000000
-#define	RATR_MCS14					0x04000000
-#define	RATR_MCS15					0x08000000
-
-#define RATE_1M						BIT(0)
-#define RATE_2M						BIT(1)
-#define RATE_5_5M					BIT(2)
-#define RATE_11M					BIT(3)
-#define RATE_6M						BIT(4)
-#define RATE_9M						BIT(5)
-#define RATE_12M					BIT(6)
-#define RATE_18M					BIT(7)
-#define RATE_24M					BIT(8)
-#define RATE_36M					BIT(9)
-#define RATE_48M					BIT(10)
-#define RATE_54M					BIT(11)
-#define RATE_MCS0					BIT(12)
-#define RATE_MCS1					BIT(13)
-#define RATE_MCS2					BIT(14)
-#define RATE_MCS3					BIT(15)
-#define RATE_MCS4					BIT(16)
-#define RATE_MCS5					BIT(17)
-#define RATE_MCS6					BIT(18)
-#define RATE_MCS7					BIT(19)
-#define RATE_MCS8					BIT(20)
-#define RATE_MCS9					BIT(21)
-#define RATE_MCS10					BIT(22)
-#define RATE_MCS11					BIT(23)
-#define RATE_MCS12					BIT(24)
-#define RATE_MCS13					BIT(25)
-#define RATE_MCS14					BIT(26)
-#define RATE_MCS15					BIT(27)
-
-#define	RATE_ALL_CCK		(RATR_1M | RATR_2M | RATR_55M | RATR_11M)
-#define	RATE_ALL_OFDM_AG	(RATR_6M | RATR_9M | RATR_12M | RATR_18M |\
-							RATR_24M| RATR_36M | RATR_48M | RATR_54M)
-#define	RATE_ALL_OFDM_1SS	(RATR_MCS0 | RATR_MCS1 | RATR_MCS2 |\
-							RATR_MCS3 | RATR_MCS4 | RATR_MCS5 |\
-							RATR_MCS6 | RATR_MCS7)
-#define	RATE_ALL_OFDM_2SS	(RATR_MCS8 | RATR_MCS9 | RATR_MCS10 |\
-							RATR_MCS11| RATR_MCS12 | RATR_MCS13 |\
-							RATR_MCS14 | RATR_MCS15)
-
-#define	BW_OPMODE_20MHZ				BIT(2)
-#define	BW_OPMODE_5G				BIT(1)
-#define	BW_OPMODE_11J				BIT(0)
-
-#define	CAM_VALID					BIT(15)
-#define	CAM_NOTVALID				0x0000
-#define	CAM_USEDK					BIT(5)
-
-#define	CAM_NONE					0x0
-#define	CAM_WEP40					0x01
-#define	CAM_TKIP					0x02
-#define	CAM_AES						0x04
-#define	CAM_WEP104					0x05
-
-#define	TOTAL_CAM_ENTRY				32
-#define	HALF_CAM_ENTRY				16
-
-#define	CAM_WRITE					BIT(16)
-#define	CAM_READ					0x00000000
-#define	CAM_POLLINIG				BIT(31)
-
-#define	SCR_USEDK					0x01
-#define	SCR_TXSEC_ENABLE			0x02
-#define	SCR_RXSEC_ENABLE			0x04
-
-#define	WOW_PMEN					BIT(0)
-#define	WOW_WOMEN					BIT(1)
-#define	WOW_MAGIC					BIT(2)
-#define	WOW_UWF						BIT(3)
-
-/*********************************************
-*       8188 IMR/ISR bits
-**********************************************/
-#define	IMR_DISABLED			0x0
-/* IMR DW0(0x0060-0063) Bit 0-31 */
-#define	IMR_TXCCK				BIT(30)		/* TXRPT interrupt when CCX bit of the packet is set	*/
-#define	IMR_PSTIMEOUT			BIT(29)		/* Power Save Time Out Interrupt */
-#define	IMR_GTINT4				BIT(28)		/* When GTIMER4 expires, this bit is set to 1	*/
-#define	IMR_GTINT3				BIT(27)		/* When GTIMER3 expires, this bit is set to 1	*/
-#define	IMR_TBDER				BIT(26)		/* Transmit Beacon0 Error			*/
-#define	IMR_TBDOK				BIT(25)		/* Transmit Beacon0 OK			*/
-#define	IMR_TSF_BIT32_TOGGLE		BIT(24)		/* TSF Timer BIT32 toggle indication interrupt		*/
-#define	IMR_BCNDMAINT0			BIT(20)		/* Beacon DMA Interrupt 0			*/
-#define	IMR_BCNDOK0				BIT(16)		/* Beacon Queue DMA OK0			*/
-#define	IMR_HSISR_IND_ON_INT		BIT(15)		/* HSISR Indicator (HSIMR & HSISR is true, this bit is set to 1)		*/
-#define	IMR_BCNDMAINT_E			BIT(14)		/* Beacon DMA Interrupt Extension for Win7			*/
-#define	IMR_ATIMEND				BIT(12)		/* CTWidnow End or ATIM Window End */
-#define	IMR_HISR1_IND_INT			BIT(11)		/* HISR1 Indicator (HISR1 & HIMR1 is true, this bit is set to 1)*/
-#define	IMR_C2HCMD				BIT(10)		/* CPU to Host Command INT Status, Write 1 clear	*/
-#define	IMR_CPWM2			BIT(9)			/* CPU power Mode exchange INT Status, Write 1 clear	*/
-#define	IMR_CPWM				BIT(8)			/* CPU power Mode exchange INT Status, Write 1 clear	*/
-#define	IMR_HIGHDOK				BIT(7)			/* High Queue DMA OK	*/
-#define	IMR_MGNTDOK				BIT(6)			/* Management Queue DMA OK	*/
-#define	IMR_BKDOK				BIT(5)			/* AC_BK DMA OK		*/
-#define	IMR_BEDOK				BIT(4)		/* AC_BE DMA OK	*/
-#define	IMR_VIDOK				BIT(3)			/* AC_VI DMA OK	*/
-#define	IMR_VODOK				BIT(2)			/* AC_VO DMA OK	*/
-#define	IMR_RDU				BIT(1)			/* Rx Descriptor Unavailable	*/
-#define	IMR_ROK				BIT(0)			/* Receive DMA OK */
-
-/* IMR DW1(0x00B4-00B7) Bit 0-31 */
-#define	IMR_BCNDMAINT7			BIT(27)		/* Beacon DMA Interrupt 7 	*/
-#define	IMR_BCNDMAINT6			BIT(26)		/* Beacon DMA Interrupt 6		*/
-#define	IMR_BCNDMAINT5			BIT(25)		/* Beacon DMA Interrupt 5		*/
-#define	IMR_BCNDMAINT4			BIT(24)		/* Beacon DMA Interrupt 4		*/
-#define	IMR_BCNDMAINT3			BIT(23)		/* Beacon DMA Interrupt 3		*/
-#define	IMR_BCNDMAINT2			BIT(22)		/* Beacon DMA Interrupt 2		*/
-#define	IMR_BCNDMAINT1			BIT(21)		/* Beacon DMA Interrupt 1		*/
-#define	IMR_BCNDOK7				BIT(20)		/* Beacon Queue DMA OK Interrupt 7 */
-#define	IMR_BCNDOK6				BIT(19)		/* Beacon Queue DMA OK Interrupt 6 */
-#define	IMR_BCNDOK5				BIT(18)		/* Beacon Queue DMA OK Interrupt 5 */
-#define	IMR_BCNDOK4				BIT(17)		/* Beacon Queue DMA OK Interrupt 4 */
-#define	IMR_BCNDOK3				BIT(16)		/* Beacon Queue DMA OK Interrupt 3 */
-#define	IMR_BCNDOK2				BIT(15)		/* Beacon Queue DMA OK Interrupt 2 */
-#define	IMR_BCNDOK1				BIT(14)		/* Beacon Queue DMA OK Interrupt 1 */
-#define	IMR_ATIMEND_E		BIT(13)		/* ATIM Window End Extension for Win7 */
-#define	IMR_TXERR				BIT(11)		/* Tx Error Flag Interrupt Status, write 1 clear. */
-#define	IMR_RXERR				BIT(10)		/* Rx Error Flag INT Status, Write 1 clear */
-#define	IMR_TXFOVW				BIT(9)			/* Transmit FIFO Overflow */
-#define	IMR_RXFOVW				BIT(8)			/* Receive FIFO Overflow */
-
-
-#define	HWSET_MAX_SIZE				512
-#define   EFUSE_MAX_SECTION			64
-#define   EFUSE_REAL_CONTENT_LEN			256
-#define 	EFUSE_OOB_PROTECT_BYTES    	18 	/* PG data exclude header, dummy 7 bytes from CP test and reserved 1byte.*/
-
-
-#define	EEPROM_DEFAULT_TSSI					0x0
-#define EEPROM_DEFAULT_TXPOWERDIFF			0x0
-#define EEPROM_DEFAULT_CRYSTALCAP			0x5
-#define EEPROM_DEFAULT_BOARDTYPE			0x02
-#define EEPROM_DEFAULT_TXPOWER				0x1010
-#define	EEPROM_DEFAULT_HT2T_TXPWR			0x10
-
-#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
-#define	EEPROM_DEFAULT_THERMALMETER			0x18
-#define	EEPROM_DEFAULT_ANTTXPOWERDIFF		0x0
-#define	EEPROM_DEFAULT_TXPWDIFF_CRYSTALCAP	0x5
-#define	EEPROM_DEFAULT_TXPOWERLEVEL			0x22
-#define	EEPROM_DEFAULT_HT40_2SDIFF			0x0
-#define EEPROM_DEFAULT_HT20_DIFF			2
-#define	EEPROM_DEFAULT_LEGACYHTTXPOWERDIFF	0x3
-#define EEPROM_DEFAULT_HT40_PWRMAXOFFSET	0
-#define EEPROM_DEFAULT_HT20_PWRMAXOFFSET	0
-
-#define RF_OPTION1							0x79
-#define RF_OPTION2							0x7A
-#define RF_OPTION3							0x7B
-#define RF_OPTION4							0xC3
-
-#define EEPROM_DEFAULT_PID					0x1234
-#define EEPROM_DEFAULT_VID					0x5678
-#define EEPROM_DEFAULT_CUSTOMERID			0xAB
-#define EEPROM_DEFAULT_SUBCUSTOMERID		0xCD
-#define EEPROM_DEFAULT_VERSION				0
-
-#define	EEPROM_CHANNEL_PLAN_FCC				0x0
-#define	EEPROM_CHANNEL_PLAN_IC				0x1
-#define	EEPROM_CHANNEL_PLAN_ETSI			0x2
-#define	EEPROM_CHANNEL_PLAN_SPAIN			0x3
-#define	EEPROM_CHANNEL_PLAN_FRANCE			0x4
-#define	EEPROM_CHANNEL_PLAN_MKK				0x5
-#define	EEPROM_CHANNEL_PLAN_MKK1			0x6
-#define	EEPROM_CHANNEL_PLAN_ISRAEL			0x7
-#define	EEPROM_CHANNEL_PLAN_TELEC			0x8
-#define	EEPROM_CHANNEL_PLAN_GLOBAL_DOMAIN	0x9
-#define	EEPROM_CHANNEL_PLAN_WORLD_WIDE_13	0xA
-#define	EEPROM_CHANNEL_PLAN_NCC				0xB
-#define	EEPROM_CHANNEL_PLAN_BY_HW_MASK		0x80
-
-#define EEPROM_CID_DEFAULT					0x0
-#define EEPROM_CID_TOSHIBA					0x4
-#define	EEPROM_CID_CCX						0x10
-#define	EEPROM_CID_QMI						0x0D
-#define EEPROM_CID_WHQL 					0xFE
-
-#define	RTL_EEPROM_ID					0x8129
-
-#define EEPROM_HPON							0x02
-#define EEPROM_CLK							0x06
-#define EEPROM_TESTR						0x08
-
-
-#define EEPROM_TXPOWERCCK			0x10
-#define	EEPROM_TXPOWERHT40_1S		0x16
-#define EEPROM_TXPOWERHT20DIFF		0x1B
-#define EEPROM_TXPOWER_OFDMDIFF		0x1B
-
-
-
-#define	EEPROM_TX_PWR_INX				0x10
-
-#define	EEPROM_CHANNELPLAN					0xB8
-#define	EEPROM_XTAL_8821AE					0xB9
-#define	EEPROM_THERMAL_METER				0xBA
-#define	EEPROM_IQK_LCK_88E					0xBB
-
-#define	EEPROM_RF_BOARD_OPTION			0xC1
-#define	EEPROM_RF_FEATURE_OPTION_88E		0xC2
-#define	EEPROM_RF_BT_SETTING				0xC3
-#define	EEPROM_VERSION					0xC4
-#define	EEPROM_CUSTOMER_ID					0xC5
-#define	EEPROM_RF_ANTENNA_OPT_88E			0xC9
-
-#define	EEPROM_MAC_ADDR					0xD0
-#define EEPROM_VID							0xD6
-#define EEPROM_DID							0xD8
-#define EEPROM_SVID							0xDA
-#define EEPROM_SMID						0xDC
-
-#define	STOPBECON					BIT(6)
-#define	STOPHIGHT					BIT(5)
-#define	STOPMGT						BIT(4)
-#define	STOPVO						BIT(3)
-#define	STOPVI						BIT(2)
-#define	STOPBE						BIT(1)
-#define	STOPBK						BIT(0)
-
-#define	RCR_APPFCS					BIT(31)
-#define	RCR_APP_MIC					BIT(30)
-#define	RCR_APP_ICV					BIT(29)
-#define	RCR_APP_PHYST_RXFF			BIT(28)
-#define	RCR_APP_BA_SSN				BIT(27)
-#define	RCR_NONQOS_VHT				BIT(26)
-#define	RCR_ENMBID					BIT(24)
-#define	RCR_LSIGEN					BIT(23)
-#define	RCR_MFBEN					BIT(22)
-#define	RCR_HTC_LOC_CTRL			BIT(14)
-#define	RCR_AMF						BIT(13)
-#define	RCR_ACF						BIT(12)
-#define	RCR_ADF						BIT(11)
-#define	RCR_AICV					BIT(9)
-#define	RCR_ACRC32					BIT(8)
-#define	RCR_CBSSID_BCN				BIT(7)
-#define	RCR_CBSSID_DATA				BIT(6)
-#define	RCR_CBSSID					RCR_CBSSID_DATA
-#define	RCR_APWRMGT					BIT(5)
-#define	RCR_ADD3					BIT(4)
-#define	RCR_AB						BIT(3)
-#define	RCR_AM						BIT(2)
-#define	RCR_APM						BIT(1)
-#define	RCR_AAP						BIT(0)
-#define	RCR_MXDMA_OFFSET			8
-#define	RCR_FIFO_OFFSET				13
-
-#define RSV_CTRL					0x001C
-#define RD_CTRL						0x0524
-
-#define REG_USB_INFO				0xFE17
-#define REG_USB_SPECIAL_OPTION		0xFE55
-#define REG_USB_DMA_AGG_TO			0xFE5B
-#define REG_USB_AGG_TO				0xFE5C
-#define REG_USB_AGG_TH				0xFE5D
-
-#define REG_USB_VID					0xFE60
-#define REG_USB_PID					0xFE62
-#define REG_USB_OPTIONAL			0xFE64
-#define REG_USB_CHIRP_K				0xFE65
-#define REG_USB_PHY					0xFE66
-#define REG_USB_MAC_ADDR			0xFE70
-#define REG_USB_HRPWM				0xFE58
-#define REG_USB_HCPWM				0xFE57
-
-#define SW18_FPWM					BIT(3)
-
-#define ISO_MD2PP					BIT(0)
-#define ISO_UA2USB					BIT(1)
-#define ISO_UD2CORE					BIT(2)
-#define ISO_PA2PCIE					BIT(3)
-#define ISO_PD2CORE					BIT(4)
-#define ISO_IP2MAC					BIT(5)
-#define ISO_DIOP					BIT(6)
-#define ISO_DIOE					BIT(7)
-#define ISO_EB2CORE					BIT(8)
-#define ISO_DIOR					BIT(9)
-
-#define PWC_EV25V					BIT(14)
-#define PWC_EV12V					BIT(15)
-
-#define FEN_BBRSTB					BIT(0)
-#define FEN_BB_GLB_RSTN				BIT(1)
-#define FEN_USBA					BIT(2)
-#define FEN_UPLL					BIT(3)
-#define FEN_USBD					BIT(4)
-#define FEN_DIO_PCIE				BIT(5)
-#define FEN_PCIEA					BIT(6)
-#define FEN_PPLL					BIT(7)
-#define FEN_PCIED					BIT(8)
-#define FEN_DIOE					BIT(9)
-#define FEN_CPUEN					BIT(10)
-#define FEN_DCORE					BIT(11)
-#define FEN_ELDR					BIT(12)
-#define FEN_DIO_RF					BIT(13)
-#define FEN_HWPDN					BIT(14)
-#define FEN_MREGEN					BIT(15)
-
-#define PFM_LDALL					BIT(0)
-#define PFM_ALDN					BIT(1)
-#define PFM_LDKP					BIT(2)
-#define PFM_WOWL					BIT(3)
-#define EnPDN						BIT(4)
-#define PDN_PL						BIT(5)
-#define APFM_ONMAC					BIT(8)
-#define APFM_OFF					BIT(9)
-#define APFM_RSM					BIT(10)
-#define AFSM_HSUS					BIT(11)
-#define AFSM_PCIE					BIT(12)
-#define APDM_MAC					BIT(13)
-#define APDM_HOST					BIT(14)
-#define APDM_HPDN					BIT(15)
-#define RDY_MACON					BIT(16)
-#define SUS_HOST					BIT(17)
-#define ROP_ALD						BIT(20)
-#define ROP_PWR						BIT(21)
-#define ROP_SPS						BIT(22)
-#define SOP_MRST					BIT(25)
-#define SOP_FUSE					BIT(26)
-#define SOP_ABG						BIT(27)
-#define SOP_AMB						BIT(28)
-#define SOP_RCK						BIT(29)
-#define SOP_A8M						BIT(30)
-#define XOP_BTCK					BIT(31)
-
-#define ANAD16V_EN					BIT(0)
-#define ANA8M						BIT(1)
-#define MACSLP						BIT(4)
-#define LOADER_CLK_EN				BIT(5)
-#define _80M_SSC_DIS				BIT(7)
-#define _80M_SSC_EN_HO				BIT(8)
-#define PHY_SSC_RSTB				BIT(9)
-#define SEC_CLK_EN					BIT(10)
-#define MAC_CLK_EN					BIT(11)
-#define SYS_CLK_EN					BIT(12)
-#define RING_CLK_EN					BIT(13)
-
-#define	BOOT_FROM_EEPROM			BIT(4)
-#define	EEPROM_EN					BIT(5)
-
-#define AFE_BGEN					BIT(0)
-#define AFE_MBEN					BIT(1)
-#define MAC_ID_EN					BIT(7)
-
-#define WLOCK_ALL					BIT(0)
-#define WLOCK_00					BIT(1)
-#define WLOCK_04					BIT(2)
-#define WLOCK_08					BIT(3)
-#define WLOCK_40					BIT(4)
-#define R_DIS_PRST_0				BIT(5)
-#define R_DIS_PRST_1				BIT(6)
-#define LOCK_ALL_EN					BIT(7)
-
-#define RF_EN						BIT(0)
-#define RF_RSTB						BIT(1)
-#define RF_SDMRSTB					BIT(2)
-
-#define LDA15_EN					BIT(0)
-#define LDA15_STBY					BIT(1)
-#define LDA15_OBUF					BIT(2)
-#define LDA15_REG_VOS				BIT(3)
-#define _LDA15_VOADJ(x)				(((x) & 0x7) << 4)
-
-#define LDV12_EN					BIT(0)
-#define LDV12_SDBY					BIT(1)
-#define LPLDO_HSM					BIT(2)
-#define LPLDO_LSM_DIS				BIT(3)
-#define _LDV12_VADJ(x)				(((x) & 0xF) << 4)
-
-#define XTAL_EN						BIT(0)
-#define XTAL_BSEL					BIT(1)
-#define _XTAL_BOSC(x)				(((x) & 0x3) << 2)
-#define _XTAL_CADJ(x)				(((x) & 0xF) << 4)
-#define XTAL_GATE_USB				BIT(8)
-#define _XTAL_USB_DRV(x)			(((x) & 0x3) << 9)
-#define XTAL_GATE_AFE				BIT(11)
-#define _XTAL_AFE_DRV(x)			(((x) & 0x3) << 12)
-#define XTAL_RF_GATE				BIT(14)
-#define _XTAL_RF_DRV(x)				(((x) & 0x3) << 15)
-#define XTAL_GATE_DIG				BIT(17)
-#define _XTAL_DIG_DRV(x)			(((x) & 0x3) << 18)
-#define XTAL_BT_GATE				BIT(20)
-#define _XTAL_BT_DRV(x)				(((x) & 0x3) << 21)
-#define _XTAL_GPIO(x)				(((x) & 0x7) << 23)
-
-#define CKDLY_AFE					BIT(26)
-#define CKDLY_USB					BIT(27)
-#define CKDLY_DIG					BIT(28)
-#define CKDLY_BT					BIT(29)
-
-#define APLL_EN						BIT(0)
-#define APLL_320_EN					BIT(1)
-#define APLL_FREF_SEL				BIT(2)
-#define APLL_EDGE_SEL				BIT(3)
-#define APLL_WDOGB					BIT(4)
-#define APLL_LPFEN					BIT(5)
-
-#define APLL_REF_CLK_13MHZ			0x1
-#define APLL_REF_CLK_19_2MHZ		0x2
-#define APLL_REF_CLK_20MHZ			0x3
-#define APLL_REF_CLK_25MHZ			0x4
-#define APLL_REF_CLK_26MHZ			0x5
-#define APLL_REF_CLK_38_4MHZ		0x6
-#define APLL_REF_CLK_40MHZ			0x7
-
-#define APLL_320EN					BIT(14)
-#define APLL_80EN					BIT(15)
-#define APLL_1MEN					BIT(24)
-
-#define ALD_EN						BIT(18)
-#define EF_PD						BIT(19)
-#define EF_FLAG						BIT(31)
-
-#define EF_TRPT						BIT(7)
-#define LDOE25_EN					BIT(31)
-
-#define RSM_EN						BIT(0)
-#define Timer_EN					BIT(4)
-
-#define TRSW0EN						BIT(2)
-#define TRSW1EN						BIT(3)
-#define EROM_EN						BIT(4)
-#define EnBT						BIT(5)
-#define EnUart						BIT(8)
-#define Uart_910					BIT(9)
-#define EnPMAC						BIT(10)
-#define SIC_SWRST					BIT(11)
-#define EnSIC						BIT(12)
-#define SIC_23						BIT(13)
-#define EnHDP						BIT(14)
-#define SIC_LBK						BIT(15)
-
-#define LED0PL 						BIT(4)
-#define LED1PL 						BIT(12)
-#define LED0DIS						BIT(7)
-
-#define MCUFWDL_EN					BIT(0)
-#define MCUFWDL_RDY					BIT(1)
-#define FWDL_CHKSUM_RPT				BIT(2)
-#define MACINI_RDY					BIT(3)
-#define BBINI_RDY					BIT(4)
-#define RFINI_RDY					BIT(5)
-#define WINTINI_RDY					BIT(6)
-#define CPRST						BIT(23)
-
-#define XCLK_VLD					BIT(0)
-#define ACLK_VLD					BIT(1)
-#define UCLK_VLD					BIT(2)
-#define PCLK_VLD					BIT(3)
-#define PCIRSTB						BIT(4)
-#define V15_VLD						BIT(5)
-#define TRP_B15V_EN					BIT(7)
-#define SIC_IDLE					BIT(8)
-#define BD_MAC2						BIT(9)
-#define BD_MAC1						BIT(10)
-#define IC_MACPHY_MODE				BIT(11)
-#define VENDOR_ID					BIT(19)
-#define PAD_HWPD_IDN				BIT(22)
-#define TRP_VAUX_EN					BIT(23)
-#define TRP_BT_EN					BIT(24)
-#define BD_PKG_SEL					BIT(25)
-#define BD_HCI_SEL					BIT(26)
-#define TYPE_ID						BIT(27)
-
-#define CHIP_VER_RTL_MASK			0xF000
-#define CHIP_VER_RTL_SHIFT			12
-
-#define REG_LBMODE					(REG_CR + 3)
-
-#define HCI_TXDMA_EN				BIT(0)
-#define HCI_RXDMA_EN				BIT(1)
-#define TXDMA_EN					BIT(2)
-#define RXDMA_EN					BIT(3)
-#define PROTOCOL_EN					BIT(4)
-#define SCHEDULE_EN					BIT(5)
-#define MACTXEN						BIT(6)
-#define MACRXEN						BIT(7)
-#define ENSWBCN						BIT(8)
-#define ENSEC						BIT(9)
-
-#define _NETTYPE(x)					(((x) & 0x3) << 16)
-#define MASK_NETTYPE				0x30000
-#define NT_NO_LINK					0x0
-#define NT_LINK_AD_HOC				0x1
-#define NT_LINK_AP					0x2
-#define NT_AS_AP					0x3
-
-#define _LBMODE(x)					(((x) & 0xF) << 24)
-#define MASK_LBMODE					0xF000000
-#define LOOPBACK_NORMAL				0x0
-#define LOOPBACK_IMMEDIATELY		0xB
-#define LOOPBACK_MAC_DELAY			0x3
-#define LOOPBACK_PHY				0x1
-#define LOOPBACK_DMA				0x7
-
-#define GET_RX_PAGE_SIZE(value)		((value) & 0xF)
-#define GET_TX_PAGE_SIZE(value)		(((value) & 0xF0) >> 4)
-#define _PSRX_MASK					0xF
-#define _PSTX_MASK					0xF0
-#define _PSRX(x)					(x)
-#define _PSTX(x)					((x) << 4)
-
-#define PBP_64						0x0
-#define PBP_128						0x1
-#define PBP_256						0x2
-#define PBP_512						0x3
-#define PBP_1024					0x4
-
-#define RXDMA_ARBBW_EN				BIT(0)
-#define RXSHFT_EN					BIT(1)
-#define RXDMA_AGG_EN				BIT(2)
-#define QS_VO_QUEUE					BIT(8)
-#define QS_VI_QUEUE					BIT(9)
-#define QS_BE_QUEUE					BIT(10)
-#define QS_BK_QUEUE					BIT(11)
-#define QS_MANAGER_QUEUE			BIT(12)
-#define QS_HIGH_QUEUE				BIT(13)
-
-#define HQSEL_VOQ					BIT(0)
-#define HQSEL_VIQ					BIT(1)
-#define HQSEL_BEQ					BIT(2)
-#define HQSEL_BKQ					BIT(3)
-#define HQSEL_MGTQ					BIT(4)
-#define HQSEL_HIQ					BIT(5)
-
-#define _TXDMA_HIQ_MAP(x) 	 		(((x)&0x3) << 14)
-#define _TXDMA_MGQ_MAP(x) 	 		(((x)&0x3) << 12)
-#define _TXDMA_BKQ_MAP(x) 	 		(((x)&0x3) << 10)
-#define _TXDMA_BEQ_MAP(x) 	 		(((x)&0x3) << 8 )
-#define _TXDMA_VIQ_MAP(x) 	 		(((x)&0x3) << 6 )
-#define _TXDMA_VOQ_MAP(x) 	 		(((x)&0x3) << 4 )
-
-#define QUEUE_LOW					1
-#define QUEUE_NORMAL				2
-#define QUEUE_HIGH					3
-
-#define _LLT_NO_ACTIVE				0x0
-#define _LLT_WRITE_ACCESS			0x1
-#define _LLT_READ_ACCESS			0x2
-
-#define _LLT_INIT_DATA(x)			((x) & 0xFF)
-#define _LLT_INIT_ADDR(x)			(((x) & 0xFF) << 8)
-#define _LLT_OP(x)					(((x) & 0x3) << 30)
-#define _LLT_OP_VALUE(x)			(((x) >> 30) & 0x3)
-
-#define BB_WRITE_READ_MASK			(BIT(31) | BIT(30))
-#define BB_WRITE_EN					BIT(30)
-#define BB_READ_EN					BIT(31)
-
-#define _HPQ(x)			((x) & 0xFF)
-#define _LPQ(x)			(((x) & 0xFF) << 8)
-#define _PUBQ(x)		(((x) & 0xFF) << 16)
-#define _NPQ(x)			((x) & 0xFF)
-
-#define HPQ_PUBLIC_DIS		BIT(24)
-#define LPQ_PUBLIC_DIS		BIT(25)
-#define LD_RQPN			BIT(31)
-
-#define BCN_VALID		BIT(16)
-#define BCN_HEAD(x)		(((x) & 0xFF) << 8)
-#define	BCN_HEAD_MASK		0xFF00
-
-#define BLK_DESC_NUM_SHIFT			4
-#define BLK_DESC_NUM_MASK			0xF
-
-#define DROP_DATA_EN				BIT(9)
-
-#define EN_AMPDU_RTY_NEW			BIT(7)
-
-#define _INIRTSMCS_SEL(x)			((x) & 0x3F)
-
-#define _SPEC_SIFS_CCK(x)			((x) & 0xFF)
-#define _SPEC_SIFS_OFDM(x)			(((x) & 0xFF) << 8)
-
-#define RATE_REG_BITMAP_ALL			0xFFFFF
-
-#define _RRSC_BITMAP(x)				((x) & 0xFFFFF)
-
-#define _RRSR_RSC(x)				(((x) & 0x3) << 21)
-#define RRSR_RSC_RESERVED			0x0
-#define RRSR_RSC_UPPER_SUBCHANNEL	0x1
-#define RRSR_RSC_LOWER_SUBCHANNEL	0x2
-#define RRSR_RSC_DUPLICATE_MODE		0x3
-
-#define USE_SHORT_G1				BIT(20)
-
-#define _AGGLMT_MCS0(x)				((x) & 0xF)
-#define _AGGLMT_MCS1(x)				(((x) & 0xF) << 4)
-#define _AGGLMT_MCS2(x)				(((x) & 0xF) << 8)
-#define _AGGLMT_MCS3(x)				(((x) & 0xF) << 12)
-#define _AGGLMT_MCS4(x)				(((x) & 0xF) << 16)
-#define _AGGLMT_MCS5(x)				(((x) & 0xF) << 20)
-#define _AGGLMT_MCS6(x)				(((x) & 0xF) << 24)
-#define _AGGLMT_MCS7(x)				(((x) & 0xF) << 28)
-
-#define	RETRY_LIMIT_SHORT_SHIFT		8
-#define	RETRY_LIMIT_LONG_SHIFT		0
-
-#define _DARF_RC1(x)				((x) & 0x1F)
-#define _DARF_RC2(x)				(((x) & 0x1F) << 8)
-#define _DARF_RC3(x)				(((x) & 0x1F) << 16)
-#define _DARF_RC4(x)				(((x) & 0x1F) << 24)
-#define _DARF_RC5(x)				((x) & 0x1F)
-#define _DARF_RC6(x)				(((x) & 0x1F) << 8)
-#define _DARF_RC7(x)				(((x) & 0x1F) << 16)
-#define _DARF_RC8(x)				(((x) & 0x1F) << 24)
-
-#define _RARF_RC1(x)				((x) & 0x1F)
-#define _RARF_RC2(x)				(((x) & 0x1F) << 8)
-#define _RARF_RC3(x)				(((x) & 0x1F) << 16)
-#define _RARF_RC4(x)				(((x) & 0x1F) << 24)
-#define _RARF_RC5(x)				((x) & 0x1F)
-#define _RARF_RC6(x)				(((x) & 0x1F) << 8)
-#define _RARF_RC7(x)				(((x) & 0x1F) << 16)
-#define _RARF_RC8(x)				(((x) & 0x1F) << 24)
-
-#define AC_PARAM_TXOP_LIMIT_OFFSET	16
-#define AC_PARAM_ECW_MAX_OFFSET		12
-#define AC_PARAM_ECW_MIN_OFFSET		8
-#define AC_PARAM_AIFS_OFFSET		0
-
-#define _AIFS(x)					(x)
-#define _ECW_MAX_MIN(x)				((x) << 8)
-#define _TXOP_LIMIT(x)				((x) << 16)
-
-#define _BCNIFS(x)					((x) & 0xFF)
-#define _BCNECW(x)					((((x) & 0xF))<< 8)
-
-#define _LRL(x)						((x) & 0x3F)
-#define _SRL(x)						(((x) & 0x3F) << 8)
-
-#define _SIFS_CCK_CTX(x)			((x) & 0xFF)
-#define _SIFS_CCK_TRX(x)			(((x) & 0xFF) << 8);
-
-#define _SIFS_OFDM_CTX(x)			((x) & 0xFF)
-#define _SIFS_OFDM_TRX(x)			(((x) & 0xFF) << 8);
-
-#define _TBTT_PROHIBIT_HOLD(x)		(((x) & 0xFF) << 8)
-
-#define DIS_EDCA_CNT_DWN			BIT(11)
-
-#define EN_MBSSID					BIT(1)
-#define EN_TXBCN_RPT				BIT(2)
-#define	EN_BCN_FUNCTION				BIT(3)
-
-#define TSFTR_RST					BIT(0)
-#define TSFTR1_RST					BIT(1)
-
-#define STOP_BCNQ					BIT(6)
-
-#define	DIS_TSF_UDT0_NORMAL_CHIP	BIT(4)
-#define	DIS_TSF_UDT0_TEST_CHIP		BIT(5)
-
-#define	AcmHw_HwEn					BIT(0)
-#define	AcmHw_BeqEn					BIT(1)
-#define	AcmHw_ViqEn					BIT(2)
-#define	AcmHw_VoqEn					BIT(3)
-#define	AcmHw_BeqStatus				BIT(4)
-#define	AcmHw_ViqStatus				BIT(5)
-#define	AcmHw_VoqStatus				BIT(6)
-
-#define APSDOFF						BIT(6)
-#define APSDOFF_STATUS				BIT(7)
-
-#define BW_20MHZ					BIT(2)
-
-#define RATE_BITMAP_ALL				0xFFFFF
-
-#define RATE_RRSR_CCK_ONLY_1M		0xFFFF1
-
-#define TSFRST						BIT(0)
-#define DIS_GCLK					BIT(1)
-#define PAD_SEL						BIT(2)
-#define PWR_ST						BIT(6)
-#define PWRBIT_OW_EN				BIT(7)
-#define ACRC						BIT(8)
-#define CFENDFORM					BIT(9)
-#define ICV							BIT(10)
-
-#define AAP							BIT(0)
-#define APM							BIT(1)
-#define AM							BIT(2)
-#define AB							BIT(3)
-#define ADD3						BIT(4)
-#define APWRMGT						BIT(5)
-#define CBSSID						BIT(6)
-#define CBSSID_DATA					BIT(6)
-#define CBSSID_BCN					BIT(7)
-#define ACRC32						BIT(8)
-#define AICV						BIT(9)
-#define ADF							BIT(11)
-#define ACF							BIT(12)
-#define AMF							BIT(13)
-#define HTC_LOC_CTRL				BIT(14)
-#define UC_DATA_EN					BIT(16)
-#define BM_DATA_EN					BIT(17)
-#define MFBEN						BIT(22)
-#define LSIGEN						BIT(23)
-#define EnMBID						BIT(24)
-#define APP_BASSN					BIT(27)
-#define APP_PHYSTS					BIT(28)
-#define APP_ICV						BIT(29)
-#define APP_MIC						BIT(30)
-#define APP_FCS						BIT(31)
-
-#define _MIN_SPACE(x)				((x) & 0x7)
-#define _SHORT_GI_PADDING(x)		(((x) & 0x1F) << 3)
-
-#define RXERR_TYPE_OFDM_PPDU		0
-#define RXERR_TYPE_OFDM_FALSE_ALARM	1
-#define	RXERR_TYPE_OFDM_MPDU_OK		2
-#define RXERR_TYPE_OFDM_MPDU_FAIL	3
-#define RXERR_TYPE_CCK_PPDU			4
-#define RXERR_TYPE_CCK_FALSE_ALARM	5
-#define RXERR_TYPE_CCK_MPDU_OK		6
-#define RXERR_TYPE_CCK_MPDU_FAIL	7
-#define RXERR_TYPE_HT_PPDU			8
-#define RXERR_TYPE_HT_FALSE_ALARM	9
-#define RXERR_TYPE_HT_MPDU_TOTAL	10
-#define RXERR_TYPE_HT_MPDU_OK		11
-#define RXERR_TYPE_HT_MPDU_FAIL		12
-#define RXERR_TYPE_RX_FULL_DROP		15
-
-#define RXERR_COUNTER_MASK			0xFFFFF
-#define RXERR_RPT_RST				BIT(27)
-#define _RXERR_RPT_SEL(type)		((type) << 28)
-
-#define	SCR_TxUseDK					BIT(0)
-#define	SCR_RxUseDK					BIT(1)
-#define	SCR_TxEncEnable				BIT(2)
-#define	SCR_RxDecEnable				BIT(3)
-#define	SCR_SKByA2					BIT(4)
-#define	SCR_NoSKMC					BIT(5)
-#define SCR_TXBCUSEDK				BIT(6)
-#define SCR_RXBCUSEDK				BIT(7)
-
-#define XCLK_VLD					BIT(0)
-#define ACLK_VLD					BIT(1)
-#define UCLK_VLD					BIT(2)
-#define PCLK_VLD					BIT(3)
-#define PCIRSTB						BIT(4)
-#define V15_VLD						BIT(5)
-#define TRP_B15V_EN					BIT(7)
-#define SIC_IDLE					BIT(8)
-#define BD_MAC2						BIT(9)
-#define BD_MAC1						BIT(10)
-#define IC_MACPHY_MODE				BIT(11)
-#define BT_FUNC						BIT(16)
-#define VENDOR_ID					BIT(19)
-#define PAD_HWPD_IDN				BIT(22)
-#define TRP_VAUX_EN					BIT(23)
-#define TRP_BT_EN					BIT(24)
-#define BD_PKG_SEL					BIT(25)
-#define BD_HCI_SEL					BIT(26)
-#define TYPE_ID						BIT(27)
-
-#define USB_IS_HIGH_SPEED			0
-#define USB_IS_FULL_SPEED			1
-#define USB_SPEED_MASK				BIT(5)
-
-#define USB_NORMAL_SIE_EP_MASK		0xF
-#define USB_NORMAL_SIE_EP_SHIFT		4
-
-#define USB_TEST_EP_MASK			0x30
-#define USB_TEST_EP_SHIFT			4
-
-#define USB_AGG_EN					BIT(3)
-
-#define MAC_ADDR_LEN				6
-#define LAST_ENTRY_OF_TX_PKT_BUFFER	175/*255    88e*/
-
-#define POLLING_LLT_THRESHOLD		20
-#define POLLING_READY_TIMEOUT_COUNT		3000
-
-#define	MAX_MSS_DENSITY_2T			0x13
-#define	MAX_MSS_DENSITY_1T			0x0A
-
-#define EPROM_CMD_OPERATING_MODE_MASK 	((1<<7)|(1<<6))
-#define EPROM_CMD_CONFIG 			0x3
-#define EPROM_CMD_LOAD 				1
-
-#define	HWSET_MAX_SIZE_92S			HWSET_MAX_SIZE
-
-#define	HAL_8192C_HW_GPIO_WPS_BIT	BIT(2)
-
-#define RA_LSSIWRITE_8821A			0xc90
-#define RB_LSSIWRITE_8821A			0xe90
-
-#define	RA_PIREAD_8821A		0xd04
-#define	RB_PIREAD_8821A		0xd44
-#define	RA_SIREAD_8821A		0xd08
-#define	RB_SIREAD_8821A		0xd48
-
-#define	RPMAC_RESET					0x100
-#define	RPMAC_TXSTART				0x104
-#define	RPMAC_TXLEGACYSIG			0x108
-#define	RPMAC_TXHTSIG1				0x10c
-#define	RPMAC_TXHTSIG2				0x110
-#define	RPMAC_PHYDEBUG				0x114
-#define	RPMAC_TXPACKETNUM			0x118
-#define	RPMAC_TXIDLE				0x11c
-#define	RPMAC_TXMACHEADER0			0x120
-#define	RPMAC_TXMACHEADER1			0x124
-#define	RPMAC_TXMACHEADER2			0x128
-#define	RPMAC_TXMACHEADER3			0x12c
-#define	RPMAC_TXMACHEADER4			0x130
-#define	RPMAC_TXMACHEADER5			0x134
-#define	RPMAC_TXDADATYPE			0x138
-#define	RPMAC_TXRANDOMSEED			0x13c
-#define	RPMAC_CCKPLCPPREAMBLE		0x140
-#define	RPMAC_CCKPLCPHEADER			0x144
-#define	RPMAC_CCKCRC16				0x148
-#define	RPMAC_OFDMRXCRC32OK			0x170
-#define	RPMAC_OFDMRXCRC32Er			0x174
-#define	RPMAC_OFDMRXPARITYER		0x178
-#define	RPMAC_OFDMRXCRC8ER			0x17c
-#define	RPMAC_CCKCRXRC16ER			0x180
-#define	RPMAC_CCKCRXRC32ER			0x184
-#define	RPMAC_CCKCRXRC32OK			0x188
-#define	RPMAC_TXSTATUS				0x18c
-
-#define	RFPGA0_RFMOD				0x800
-
-#define	RFPGA0_TXINFO				0x804
-#define	RFPGA0_PSDFUNCTION			0x808
-
-#define	RFPGA0_TXGAINSTAGE			0x80c
-
-#define	RFPGA0_RFTIMING1			0x810
-#define	RFPGA0_RFTIMING2			0x814
-
-#define	RFPGA0_XA_HSSIPARAMETER1	0x820
-#define	RFPGA0_XA_HSSIPARAMETER2	0x824
-#define	RFPGA0_XB_HSSIPARAMETER1	0x828
-#define	RFPGA0_XB_HSSIPARAMETER2	0x82c
-#define 	RCCAONSEC					0x838
-
-#define	RFPGA0_XA_LSSIPARAMETER		0x840
-#define	RFPGA0_XB_LSSIPARAMETER		0x844
-#define	RL1PEAKTH					0x848
-
-#define	RFPGA0_RFWAKEUPPARAMETER	0x850
-#define	RFPGA0_RFSLEEPUPPARAMETER	0x854
-
-#define	RFPGA0_XAB_SWITCHCONTROL	0x858
-#define	RFPGA0_XCD_SWITCHCONTROL	0x85c
-
-#define	RFPGA0_XA_RFINTERFACEOE		0x860
-#define RFC_AREA					0x860
-#define	RFPGA0_XB_RFINTERFACEOE		0x864
-
-#define	RFPGA0_XAB_RFINTERFACESW	0x870
-#define	RFPGA0_XCD_RFINTERFACESW	0x874
-
-#define	rFPGA0_XAB_RFPARAMETER		0x878
-#define	rFPGA0_XCD_RFPARAMETER		0x87c
-
-#define	RFPGA0_ANALOGPARAMETER1		0x880
-#define	RFPGA0_ANALOGPARAMETER2		0x884
-#define	RFPGA0_ANALOGPARAMETER3		0x888
-#define	RFPGA0_ANALOGPARAMETER4		0x88c
-
-#define	RFPGA0_XA_LSSIREADBACK		0x8a0
-#define	RFPGA0_XB_LSSIREADBACK		0x8a4
-#define	RFPGA0_XC_LSSIREADBACK		0x8a8
-//#define	RFPGA0_XD_LSSIREADBACK		0x8ac
-#define RRFMOD						0x8ac
-#define	RHSSIREAD_8821AE			0x8b0
-
-#define	RFPGA0_PSDREPORT			0x8b4
-#define	TRANSCEIVEA_HSPI_READBACK	0x8b8
-#define	TRANSCEIVEB_HSPI_READBACK	0x8bc
-//#define	REG_SC_CNT					0x8c4
-#define RADC_BUF_CLK				0x8c4
-#define	RFPGA0_XAB_RFINTERFACERB	0x8e0
-#define	RFPGA0_XCD_RFINTERFACERB	0x8e4
-
-#define	RFPGA1_RFMOD				0x900
-
-#define	RFPGA1_TXBLOCK				0x904
-#define	RFPGA1_DEBUGSELECT			0x908
-#define	RFPGA1_TXINFO				0x90c
-
-#define	RCCK_SYSTEM					0xa00
-#define	BCCK_SYSTEM					0x10
-
-
-#define	RCCK0_AFESETTING			0xa04
-#define	RCCK0_CCA					0xa08
-
-#define	RCCK0_RXAGC1				0xa0c
-#define	RCCK0_RXAGC2				0xa10
-
-#define	RCCK0_RXHP					0xa14
-
-#define	RCCK0_DSPPARAMETER1			0xa18
-#define	RCCK0_DSPPARAMETER2			0xa1c
-
-#define	RCCK0_TXFILTER1				0xa20
-#define	RCCK0_TXFILTER2				0xa24
-#define	RCCK0_DEBUGPORT				0xa28
-#define	RCCK0_FALSEALARMREPORT		0xa2c
-#define	RCCK0_TRSSIREPORT         	0xa50
-#define	RCCK0_RXREPORT            	0xa54
-#define	RCCK0_FACOUNTERLOWER      	0xa5c
-#define	RCCK0_FACOUNTERUPPER      	0xa58
-#define	RCCK0_CCA_CNT			0xa60
-
-
-/* PageB(0xB00) */
-#define	rPdp_AntA      					0xb00
-#define	rPdp_AntA_4    				0xb04
-#define	rPdp_AntA_8    				0xb08
-#define	rPdp_AntA_C    				0xb0c
-#define	rPdp_AntA_10    				0xb10
-#define	rPdp_AntA_14    				0xb14
-#define	rPdp_AntA_18    				0xb18
-#define	rPdp_AntA_1C    				0xb1c
-#define	rPdp_AntA_20    				0xb20
-#define	rPdp_AntA_24    				0xb24
-
-#define	rConfig_Pmpd_AntA 			0xb28
-#define	rConfig_ram64x16				0xb2c
-
-#define	rBndA						0xb30
-#define	rHssiPar						0xb34
-
-#define	rConfig_AntA 					0xb68
-#define	rConfig_AntB 					0xb6c
-
-#define	rPdp_AntB 					0xb70
-#define	rPdp_AntB_4 					0xb74
-#define	rPdp_AntB_8 					0xb78
-#define	rPdp_AntB_C 					0xb7c
-#define	rPdp_AntB_10 					0xb80
-#define	rPdp_AntB_14 					0xb84
-#define	rPdp_AntB_18 					0xb88
-#define	rPdp_AntB_1C 					0xb8c
-#define	rPdp_AntB_20 					0xb90
-#define	rPdp_AntB_24 					0xb94
-
-#define	rConfig_Pmpd_AntB			0xb98
-
-#define	rBndB						0xba0
-
-#define	rAPK							0xbd8
-#define	rPm_Rx0_AntA				0xbdc
-#define	rPm_Rx1_AntA				0xbe0
-#define	rPm_Rx2_AntA				0xbe4
-#define	rPm_Rx3_AntA				0xbe8
-#define	rPm_Rx0_AntB				0xbec
-#define	rPm_Rx1_AntB				0xbf0
-#define	rPm_Rx2_AntB				0xbf4
-#define	rPm_Rx3_AntB				0xbf8
-
-/*RSSI Dump*/
-#define		RA_RSSI_DUMP		0xBF0
-#define		RB_RSSI_DUMP		0xBF1
-#define		RS1_RX_EVM_DUMP	0xBF4
-#define		RS2_RX_EVM_DUMP	0xBF5
-#define		RA_RX_SNR_DUMP		0xBF6
-#define		RB_RX_SNR_DUMP		0xBF7
-#define		RA_CFO_SHORT_DUMP	0xBF8
-#define		RB_CFO_SHORT_DUMP	0xBFA
-#define		RA_CFO_LONG_DUMP	0xBEC
-#define		RB_CFO_LONG_DUMP	0xBEE
-
-/*Page C*/
-#define	ROFDM0_LSTF					0xc00
-
-#define	ROFDM0_TRXPATHENABLE		0xc04
-#define	ROFDM0_TRMUXPAR				0xc08
-#define	ROFDM0_TRSWISOLATION		0xc0c
-
-#define	ROFDM0_XARXAFE				0xc10
-#define	ROFDM0_XARXIQIMBALANCE    	0xc14
-#define	ROFDM0_XBRXAFE            	0xc18
-#define	ROFDM0_XBRXIQIMBALANCE    	0xc1c
-#define	ROFDM0_XCRXAFE            	0xc20
-#define	ROFDM0_XCRXIQIMBANLANCE    	0xc24
-#define	ROFDM0_XDRXAFE            	0xc28
-#define	ROFDM0_XDRXIQIMBALANCE    	0xc2c
-
-#define	ROFDM0_RXDETECTOR1			0xc30
-#define	ROFDM0_RXDETECTOR2			0xc34
-#define	ROFDM0_RXDETECTOR3			0xc38
-#define	ROFDM0_RXDETECTOR4			0xc3c
-
-#define	ROFDM0_RXDSP				0xc40
-#define	ROFDM0_CFOANDDAGC			0xc44
-#define	ROFDM0_CCADROPTHRESHOLD		0xc48
-#define	ROFDM0_ECCATHRESHOLD		0xc4c
-
-#define	ROFDM0_XAAGCCORE1			0xc50
-#define	ROFDM0_XAAGCCORE2			0xc54
-#define	ROFDM0_XBAGCCORE1			0xc58
-#define	ROFDM0_XBAGCCORE2			0xc5c
-#define	ROFDM0_XCAGCCORE1			0xc60
-#define	ROFDM0_XCAGCCORE2			0xc64
-#define	ROFDM0_XDAGCCORE1			0xc68
-#define	ROFDM0_XDAGCCORE2			0xc6c
-
-#define	ROFDM0_AGCPARAMETER1		0xc70
-#define	ROFDM0_AGCPARAMETER2		0xc74
-#define	ROFDM0_AGCRSSITABLE			0xc78
-#define	ROFDM0_HTSTFAGC				0xc7c
-
-#define	ROFDM0_XATXIQIMBALANCE		0xc80
-#define	ROFDM0_XATXAFE				0xc84
-#define	ROFDM0_XBTXIQIMBALANCE		0xc88
-#define	ROFDM0_XBTXAFE				0xc8c
-#define	ROFDM0_XCTXIQIMBALANCE		0xc90
-#define	ROFDM0_XCTXAFE            	0xc94
-#define	ROFDM0_XDTXIQIMBALANCE		0xc98
-#define	ROFDM0_XDTXAFE				0xc9c
-
-#define ROFDM0_RXIQEXTANTA			0xca0
-#define	ROFDM0_TXCOEFF1				0xca4
-#define	ROFDM0_TXCOEFF2				0xca8
-#define	ROFDM0_TXCOEFF3				0xcac
-#define	ROFDM0_TXCOEFF4				0xcb0
-#define	ROFDM0_TXCOEFF5				0xcb4
-#define	ROFDM0_TXCOEFF6				0xcb8
-
-/*Path_A RFE control */
-#define	RA_RFE_CTRL_8812				0xcb8
-/*Path_B RFE control*/
-#define	RB_RFE_CTRL_8812				0xeb8
-
-#define	ROFDM0_RXHPPARAMETER		0xce0
-#define	ROFDM0_TXPSEUDONOISEWGT		0xce4
-#define	ROFDM0_FRAMESYNC			0xcf0
-#define	ROFDM0_DFSREPORT			0xcf4
-
-
-#define	ROFDM1_LSTF					0xd00
-#define	ROFDM1_TRXPATHENABLE		0xd04
-
-#define	ROFDM1_CF0					0xd08
-#define	ROFDM1_CSI1					0xd10
-#define	ROFDM1_SBD					0xd14
-#define	ROFDM1_CSI2					0xd18
-#define	ROFDM1_CFOTRACKING			0xd2c
-#define	ROFDM1_TRXMESAURE1			0xd34
-#define	ROFDM1_INTFDET				0xd3c
-#define	ROFDM1_PSEUDONOISESTATEAB	0xd50
-#define	ROFDM1_PSEUDONOISESTATECD	0xd54
-#define	ROFDM1_RXPSEUDONOISEWGT		0xd58
-
-#define	ROFDM_PHYCOUNTER1			0xda0
-#define	ROFDM_PHYCOUNTER2			0xda4
-#define	ROFDM_PHYCOUNTER3			0xda8
-
-#define	ROFDM_SHORTCFOAB			0xdac
-#define	ROFDM_SHORTCFOCD			0xdb0
-#define	ROFDM_LONGCFOAB				0xdb4
-#define	ROFDM_LONGCFOCD				0xdb8
-#define	ROFDM_TAILCF0AB				0xdbc
-#define	ROFDM_TAILCF0CD				0xdc0
-#define	ROFDM_PWMEASURE1          	0xdc4
-#define	ROFDM_PWMEASURE2          	0xdc8
-#define	ROFDM_BWREPORT				0xdcc
-#define	ROFDM_AGCREPORT				0xdd0
-#define	ROFDM_RXSNR					0xdd4
-#define	ROFDM_RXEVMCSI				0xdd8
-#define	ROFDM_SIGREPORT				0xddc
-
-#define RTXAGC_A_CCK11_CCK1			0xc20
-#define RTXAGC_A_OFDM18_OFDM6		0xc24
-#define RTXAGC_A_OFDM54_OFDM24		0xc28
-#define RTXAGC_A_MCS03_MCS00			0xc2c
-#define RTXAGC_A_MCS07_MCS04			0xc30
-#define RTXAGC_A_MCS11_MCS08			0xc34
-#define RTXAGC_A_MCS15_MCS12		0xc38
-#define RTXAGC_A_NSS1INDEX3_NSS1INDEX0	0xc3c
-#define	RTXAGC_A_NSS1INDEX7_NSS1INDEX4	0xc40
-#define	RTXAGC_A_NSS2INDEX1_NSS1INDEX8	0xc44
-#define	RTXAGC_A_NSS2INDEX5_NSS2INDEX2	0xc48
-#define	RTXAGC_A_NSS2INDEX9_NSS2INDEX6	0xc4c
-#define	RTXAGC_B_CCK11_CCK1			0xe20
-#define	RTXAGC_B_OFDM18_OFDM6		0xe24
-#define	RTXAGC_B_OFDM54_OFDM24		0xe28
-#define	RTXAGC_B_MCS03_MCS00			0xe2c
-#define	RTXAGC_B_MCS07_MCS04			0xe30
-#define	RTXAGC_B_MCS11_MCS08			0xe34
-#define	RTXAGC_B_MCS15_MCS12		0xe38
-#define	RTXAGC_B_NSS1INDEX3_NSS1INDEX0	0xe3c
-#define	RTXAGC_B_NSS1INDEX7_NSS1INDEX4	0xe40
-#define	RTXAGC_B_NSS2INDEX1_NSS1INDEX8	0xe44
-#define	RTXAGC_B_NSS2INDEX5_NSS2INDEX2	0xe48
-#define	RTXAGC_B_NSS2INDEX9_NSS2INDEX6	0xe4c
-
-#define	RA_TXPWRTRAING		0xc54
-#define	RB_TXPWRTRAING		0xe54
-
-
-#define	RFPGA0_IQK					0xe28
-#define	RTx_IQK_Tone_A				0xe30
-#define	RRx_IQK_Tone_A				0xe34
-#define	RTx_IQK_PI_A					0xe38
-#define	RRx_IQK_PI_A					0xe3c
-
-#define	RTx_IQK 						0xe40
-#define	RRx_IQK						0xe44
-#define	RIQK_AGC_Pts					0xe48
-#define	RIQK_AGC_Rsp					0xe4c
-#define	RTx_IQK_Tone_B				0xe50
-#define	RRx_IQK_Tone_B				0xe54
-#define	RTx_IQK_PI_B					0xe58
-#define	RRx_IQK_PI_B					0xe5c
-#define	RIQK_AGC_Cont				0xe60
-
-#define	RBlue_Tooth					0xe6c
-#define	RRx_Wait_CCA					0xe70
-#define	RTx_CCK_RFON					0xe74
-#define	RTx_CCK_BBON				0xe78
-#define	RTx_OFDM_RFON				0xe7c
-#define	RTx_OFDM_BBON				0xe80
-#define	RTx_To_Rx					0xe84
-#define	RTx_To_Tx					0xe88
-#define	RRx_CCK						0xe8c
-
-#define	RTx_Power_Before_IQK_A		0xe94
-#define	RTx_Power_After_IQK_A			0xe9c
-
-#define	RRx_Power_Before_IQK_A		0xea0
-#define	RRx_Power_Before_IQK_A_2		0xea4
-#define	RRx_Power_After_IQK_A			0xea8
-#define	RRx_Power_After_IQK_A_2		0xeac
-
-#define	RTx_Power_Before_IQK_B		0xeb4
-#define	RTx_Power_After_IQK_B			0xebc
-
-#define	RRx_Power_Before_IQK_B		0xec0
-#define	RRx_Power_Before_IQK_B_2		0xec4
-#define	RRx_Power_After_IQK_B			0xec8
-#define	RRx_Power_After_IQK_B_2		0xecc
-
-#define	RRx_OFDM					0xed0
-#define	RRx_Wait_RIFS 				0xed4
-#define	RRx_TO_Rx 					0xed8
-#define	RStandby 						0xedc
-#define	RSleep 						0xee0
-#define	RPMPD_ANAEN				0xeec
-
-#define	RZEBRA1_HSSIENABLE			0x0
-#define	RZEBRA1_TRXENABLE1			0x1
-#define	RZEBRA1_TRXENABLE2			0x2
-#define	RZEBRA1_AGC					0x4
-#define	RZEBRA1_CHARGEPUMP			0x5
-#define	RZEBRA1_CHANNEL				0x7
-
-#define	RZEBRA1_TXGAIN				0x8
-#define	RZEBRA1_TXLPF				0x9
-#define	RZEBRA1_RXLPF				0xb
-#define	RZEBRA1_RXHPFCORNER			0xc
-
-#define	RGLOBALCTRL					0
-#define	RRTL8256_TXLPF				19
-#define	RRTL8256_RXLPF				11
-#define	RRTL8258_TXLPF				0x11
-#define	RRTL8258_RXLPF				0x13
-#define	RRTL8258_RSSILPF			0xa
-
-#define	RF_AC						0x00
-
-#define	RF_IQADJ_G1					0x01
-#define	RF_IQADJ_G2					0x02
-#define	RF_POW_TRSW					0x05
-
-#define	RF_GAIN_RX					0x06
-#define	RF_GAIN_TX					0x07
-
-#define	RF_TXM_IDAC					0x08
-#define	RF_BS_IQGEN					0x0F
-
-#define	RF_MODE1					0x10
-#define	RF_MODE2					0x11
-
-#define	RF_RX_AGC_HP				0x12
-#define	RF_TX_AGC					0x13
-#define	RF_BIAS						0x14
-#define	RF_IPA						0x15
-#define	RF_POW_ABILITY				0x17
-#define	RF_MODE_AG					0x18
-#define	RRFCHANNEL					0x18
-#define	RF_CHNLBW					0x18
-#define	RF_TOP						0x19
-
-#define	RF_RX_G1					0x1A
-#define	RF_RX_G2					0x1B
-
-#define	RF_RX_BB2					0x1C
-#define	RF_RX_BB1					0x1D
-
-#define	RF_RCK1						0x1E
-#define	RF_RCK2						0x1F
-
-#define	RF_TX_G1					0x20
-#define	RF_TX_G2					0x21
-#define	RF_TX_G3					0x22
-
-#define	RF_TX_BB1					0x23
-#define	RF_T_METER					0x24
-#define	RF_T_METER_88E				0x42
-#define  RF_T_METER_8812A 		0x42
-
-#define	RF_SYN_G1					0x25
-#define	RF_SYN_G2					0x26
-#define	RF_SYN_G3					0x27
-#define	RF_SYN_G4					0x28
-#define	RF_SYN_G5					0x29
-#define	RF_SYN_G6					0x2A
-#define	RF_SYN_G7					0x2B
-#define	RF_SYN_G8					0x2C
-
-#define	RF_RCK_OS					0x30
-#define	RF_TXPA_G1					0x31
-#define	RF_TXPA_G2					0x32
-#define	RF_TXPA_G3					0x33
-
-#define	RF_TX_BIAS_A					0x35
-#define	RF_TX_BIAS_D					0x36
-#define	RF_LOBF_9					0x38
-#define	RF_RXRF_A3					0x3C
-#define	RF_TRSW						0x3F
-
-#define	RF_TXRF_A2					0x41
-#define	RF_TXPA_G4					0x46
-#define	RF_TXPA_A4					0x4B
-
-#define RF_APK						0x63
-
-#define	RF_WE_LUT					0xEF
-
-#define	BBBRESETB					0x100
-#define	BGLOBALRESETB				0x200
-#define	BOFDMTXSTART				0x4
-#define	BCCKTXSTART					0x8
-#define	BCRC32DEBUG					0x100
-#define	BPMACLOOPBACK				0x10
-#define	BTXLSIG						0xffffff
-#define	BOFDMTXRATE					0xf
-#define	BOFDMTXRESERVED				0x10
-#define	BOFDMTXLENGTH				0x1ffe0
-#define	BOFDMTXPARITY				0x20000
-#define	BTXHTSIG1					0xffffff
-#define	BTXHTMCSRATE				0x7f
-#define	BTXHTBW						0x80
-#define	BTXHTLENGTH					0xffff00
-#define	BTXHTSIG2					0xffffff
-#define	BTXHTSMOOTHING				0x1
-#define	BTXHTSOUNDING				0x2
-#define	BTXHTRESERVED				0x4
-#define	BTXHTAGGREATION				0x8
-#define	BTXHTSTBC					0x30
-#define	BTXHTADVANCECODING			0x40
-#define	BTXHTSHORTGI				0x80
-#define	BTXHTNUMBERHT_LTF			0x300
-#define	BTXHTCRC8					0x3fc00
-#define	BCOUNTERRESET				0x10000
-#define	BNUMOFOFDMTX				0xffff
-#define	BNUMOFCCKTX					0xffff0000
-#define	BTXIDLEINTERVAL				0xffff
-#define	BOFDMSERVICE				0xffff0000
-#define	BTXMACHEADER				0xffffffff
-#define	BTXDATAINIT					0xff
-#define	BTXHTMODE					0x100
-#define	BTXDATATYPE					0x30000
-#define	BTXRANDOMSEED				0xffffffff
-#define	BCCKTXPREAMBLE				0x1
-#define	BCCKTXSFD					0xffff0000
-#define	BCCKTXSIG					0xff
-#define	BCCKTXSERVICE				0xff00
-#define	BCCKLENGTHEXT				0x8000
-#define	BCCKTXLENGHT				0xffff0000
-#define	BCCKTXCRC16					0xffff
-#define	BCCKTXSTATUS				0x1
-#define	BOFDMTXSTATUS				0x2
-#define IS_BB_REG_OFFSET_92S(_Offset)	\
-	((_Offset >= 0x800) && (_Offset <= 0xfff))
-
-#define	BRFMOD						0x1
-#define	BJAPANMODE					0x2
-#define	BCCKTXSC					0x30
-/* Block & Path enable*/
-#define ROFDMCCKEN					0x808
-#define	BCCKEN						0x10000000
-#define	BOFDMEN						0x20000000
-#define	RRXPATH						0x808	/* Rx antenna*/
-#define	BRXPATH						0xff
-#define	RTXPATH						0x80c	/* Tx antenna*/
-#define	BTXPATH						0x0fffffff
-#define	RCCK_RX						0xa04	/* for cck rx path selection*/
-#define	BCCK_RX						0x0c000000
-#define	RVHTLEN_USE_LSIG			0x8c3	/* Use LSIG for VHT length*/
-
-
-#define	BOFDMRXADCPHASE           	0x10000
-#define	BOFDMTXDACPHASE           	0x40000
-#define	BXATXAGC                  	0x3f
-
-#define	BXBTXAGC                  	0xf00
-#define	BXCTXAGC                  	0xf000
-#define	BXDTXAGC                  	0xf0000
-
-#define	BPASTART                  	0xf0000000
-#define	BTRSTART                  	0x00f00000
-#define	BRFSTART                  	0x0000f000
-#define	BBBSTART                  	0x000000f0
-#define	BBBCCKSTART               	0x0000000f
-#define	BPAEND                    	0xf
-#define	BTREND                    	0x0f000000
-#define	BRFEND                    	0x000f0000
-#define	BCCAMASK                  	0x000000f0
-#define	BR2RCCAMASK               	0x00000f00
-#define	BHSSI_R2TDELAY            	0xf8000000
-#define	BHSSI_T2RDELAY            	0xf80000
-#define	BCONTXHSSI               	0x400
-#define	BIGFROMCCK                	0x200
-#define	BAGCADDRESS               	0x3f
-#define	BRXHPTX                   	0x7000
-#define	BRXHP2RX                  	0x38000
-#define	BRXHPCCKINI               	0xc0000
-#define	BAGCTXCODE                	0xc00000
-#define	BAGCRXCODE                	0x300000
-
-#define	B3WIREDATALENGTH          	0x800
-#define	B3WIREADDREAALENGTH       	0x400
-
-#define	B3WIRERFPOWERDOWN         	0x1
-#define	B5GPAPEPOLARITY           	0x40000000
-#define	B2GPAPEPOLARITY           	0x80000000
-#define	BRFSW_TXDEFAULTANT        	0x3
-#define	BRFSW_TXOPTIONANT         	0x30
-#define	BRFSW_RXDEFAULTANT        	0x300
-#define	BRFSW_RXOPTIONANT         	0x3000
-#define	BRFSI_3WIREDATA           	0x1
-#define	BRFSI_3WIRECLOCK          	0x2
-#define	BRFSI_3WIRELOAD           	0x4
-#define	BRFSI_3WIRERW             	0x8
-#define	BRFSI_3WIRE               	0xf
-
-#define	BRFSI_RFENV               	0x10
-
-#define	BRFSI_TRSW                	0x20
-#define	BRFSI_TRSWB               	0x40
-#define	BRFSI_ANTSW               	0x100
-#define	BRFSI_ANTSWB              	0x200
-#define	BRFSI_PAPE                	0x400
-#define	BRFSI_PAPE5G              	0x800
-#define	BBANDSELECT               	0x1
-#define	BHTSIG2_GI                	0x80
-#define	BHTSIG2_SMOOTHING         	0x01
-#define	BHTSIG2_SOUNDING          	0x02
-#define	BHTSIG2_AGGREATON         	0x08
-#define	BHTSIG2_STBC              	0x30
-#define	BHTSIG2_ADVCODING         	0x40
-#define	BHTSIG2_NUMOFHTLTF        	0x300
-#define	BHTSIG2_CRC8              	0x3fc
-#define	BHTSIG1_MCS               	0x7f
-#define	BHTSIG1_BANDWIDTH         	0x80
-#define	BHTSIG1_HTLENGTH          	0xffff
-#define	BLSIG_RATE                	0xf
-#define	BLSIG_RESERVED            	0x10
-#define	BLSIG_LENGTH              	0x1fffe
-#define	BLSIG_PARITY              	0x20
-#define	BCCKRXPHASE               	0x4
-
-#define	BLSSIREADADDRESS          	0x7f800000
-#define	BLSSIREADEDGE             	0x80000000
-
-#define	BLSSIREADBACKDATA         	0xfffff
-
-#define	BLSSIREADOKFLAG           	0x1000
-#define	BCCKSAMPLERATE            	0x8
-#define	BREGULATOR0STANDBY        	0x1
-#define	BREGULATORPLLSTANDBY      	0x2
-#define	BREGULATOR1STANDBY        	0x4
-#define	BPLLPOWERUP               	0x8
-#define	BDPLLPOWERUP              	0x10
-#define	BDA10POWERUP              	0x20
-#define	BAD7POWERUP               	0x200
-#define	BDA6POWERUP               	0x2000
-#define	BXTALPOWERUP              	0x4000
-#define	B40MDCLKPOWERUP           	0x8000
-#define	BDA6DEBUGMODE             	0x20000
-#define	BDA6SWING                 	0x380000
-
-#define	BADCLKPHASE               	0x4000000
-#define	B80MCLKDELAY              	0x18000000
-#define	BAFEWATCHDOGENABLE        	0x20000000
-
-#define	BXTALCAP01                	0xc0000000
-#define	BXTALCAP23                	0x3
-#define	BXTALCAP92X					0x0f000000
-#define BXTALCAP                	0x0f000000
-
-#define	BINTDIFCLKENABLE          	0x400
-#define	BEXTSIGCLKENABLE         	0x800
-#define	BBANDGAP_MBIAS_POWERUP      0x10000
-#define	BAD11SH_GAIN               	0xc0000
-#define	BAD11NPUT_RANGE           	0x700000
-#define	BAD110P_CURRENT            	0x3800000
-#define	BLPATH_LOOPBACK            	0x4000000
-#define	BQPATH_LOOPBACK            	0x8000000
-#define	BAFE_LOOPBACK              	0x10000000
-#define	BDA10_SWING                	0x7e0
-#define	BDA10_REVERSE              	0x800
-#define	BDA_CLK_SOURCE              0x1000
-#define	BDA7INPUT_RANGE            	0x6000
-#define	BDA7_GAIN                  	0x38000
-#define	BDA7OUTPUT_CM_MODE          0x40000
-#define	BDA7INPUT_CM_MODE           0x380000
-#define	BDA7CURRENT               	0xc00000
-#define	BREGULATOR_ADJUST          	0x7000000
-#define	BAD11POWERUP_ATTX          	0x1
-#define	BDA10PS_ATTX               	0x10
-#define	BAD11POWERUP_ATRX          	0x100
-#define	BDA10PS_ATRX               	0x1000
-#define	BCCKRX_AGC_FORMAT           0x200
-#define	BPSDFFT_SAMPLE_POINT       	0xc000
-#define	BPSD_AVERAGE_NUM            0x3000
-#define	BIQPATH_CONTROL            	0xc00
-#define	BPSD_FREQ                  	0x3ff
-#define	BPSD_ANTENNA_PATH           0x30
-#define	BPSD_IQ_SWITCH              0x40
-#define	BPSD_RX_TRIGGER             0x400000
-#define	BPSD_TX_TRIGGER             0x80000000
-#define	BPSD_SINE_TONE_SCALE        0x7f000000
-#define	BPSD_REPORT                	0xffff
-
-#define	BOFDM_TXSC                	0x30000000
-#define	BCCK_TXON                  	0x1
-#define	BOFDM_TXON                 	0x2
-#define	BDEBUG_PAGE                	0xfff
-#define	BDEBUG_ITEM                	0xff
-#define	BANTL              	       	0x10
-#define	BANT_NONHT           	    0x100
-#define	BANT_HT1               		0x1000
-#define	BANT_HT2                   	0x10000
-#define	BANT_HT1S1                 	0x100000
-#define	BANT_NONHTS1               	0x1000000
-
-#define	BCCK_BBMODE                	0x3
-#define	BCCK_TXPOWERSAVING         	0x80
-#define	BCCK_RXPOWERSAVING         	0x40
-
-#define	BCCK_SIDEBAND              	0x10
-
-#define	BCCK_SCRAMBLE              	0x8
-#define	BCCK_ANTDIVERSITY    		0x8000
-#define	BCCK_CARRIER_RECOVERY   	0x4000
-#define	BCCK_TXRATE           		0x3000
-#define	BCCK_DCCANCEL             	0x0800
-#define	BCCK_ISICANCEL             	0x0400
-#define	BCCK_MATCH_FILTER           0x0200
-#define	BCCK_EQUALIZER             	0x0100
-#define	BCCK_PREAMBLE_DETECT       	0x800000
-#define	BCCK_FAST_FALSECCA          0x400000
-#define	BCCK_CH_ESTSTART            0x300000
-#define	BCCK_CCA_COUNT              0x080000
-#define	BCCK_CS_LIM                	0x070000
-#define	BCCK_BIST_MODE              0x80000000
-#define	BCCK_CCAMASK             	0x40000000
-#define	BCCK_TX_DAC_PHASE         	0x4
-#define	BCCK_RX_ADC_PHASE         	0x20000000
-#define	BCCKR_CP_MODE         	   	0x0100
-#define	BCCK_TXDC_OFFSET           	0xf0
-#define	BCCK_RXDC_OFFSET           	0xf
-#define	BCCK_CCA_MODE              	0xc000
-#define	BCCK_FALSECS_LIM           	0x3f00
-#define	BCCK_CS_RATIO              	0xc00000
-#define	BCCK_CORGBIT_SEL           	0x300000
-#define	BCCK_PD_LIM                	0x0f0000
-#define	BCCK_NEWCCA                	0x80000000
-#define	BCCK_RXHP_OF_IG             0x8000
-#define	BCCK_RXIG                  	0x7f00
-#define	BCCK_LNA_POLARITY           0x800000
-#define	BCCK_RX1ST_BAIN             0x7f0000
-#define	BCCK_RF_EXTEND              0x20000000
-#define	BCCK_RXAGC_SATLEVEL        	0x1f000000
-#define	BCCK_RXAGC_SATCOUNT       	0xe0
-#define	bCCKRxRFSettle            	0x1f
-#define	BCCK_FIXED_RXAGC           	0x8000
-#define	BCCK_ANTENNA_POLARITY      	0x2000
-#define	BCCK_TXFILTER_TYPE          0x0c00
-#define	BCCK_RXAGC_REPORTTYPE   	0x0300
-#define	BCCK_RXDAGC_EN              0x80000000
-#define	BCCK_RXDAGC_PERIOD        	0x20000000
-#define	BCCK_RXDAGC_SATLEVEL     	0x1f000000
-#define	BCCK_TIMING_RECOVERY       	0x800000
-#define	BCCK_TXC0                  	0x3f0000
-#define	BCCK_TXC1                  	0x3f000000
-#define	BCCK_TXC2                  	0x3f
-#define	BCCK_TXC3                  	0x3f00
-#define	BCCK_TXC4                  	0x3f0000
-#define	BCCK_TXC5                  	0x3f000000
-#define	BCCK_TXC6                  	0x3f
-#define	BCCK_TXC7                  	0x3f00
-#define	BCCK_DEBUGPORT             	0xff0000
-#define	BCCK_DAC_DEBUG              0x0f000000
-#define	BCCK_FALSEALARM_ENABLE      0x8000
-#define	BCCK_FALSEALARM_READ        0x4000
-#define	BCCK_TRSSI                 	0x7f
-#define	BCCK_RXAGC_REPORT           0xfe
-#define	BCCK_RXREPORT_ANTSEL       	0x80000000
-#define	BCCK_RXREPORT_MFOFF        	0x40000000
-#define	BCCK_RXREPORT_SQLOSS     	0x20000000
-#define	BCCK_RXREPORT_PKTLOSS      	0x10000000
-#define	BCCK_RXREPORT_LOCKEDBIT    	0x08000000
-#define	BCCK_RXREPORT_RATEERROR    	0x04000000
-#define	BCCK_RXREPORT_RXRATE       	0x03000000
-#define	BCCK_RXFA_COUNTER_LOWER     0xff
-#define	BCCK_RXFA_COUNTER_UPPER     0xff000000
-#define	BCCK_RXHPAGC_START          0xe000
-#define	BCCK_RXHPAGC_FINAL          0x1c00
-#define	BCCK_RXFALSEALARM_ENABLE    0x8000
-#define	BCCK_FACOUNTER_FREEZE       0x4000
-#define	BCCK_TXPATH_SEL             0x10000000
-#define	BCCK_DEFAULT_RXPATH         0xc000000
-#define	BCCK_OPTION_RXPATH          0x3000000
-
-#define	BNUM_OFSTF                	0x3
-#define	BSHIFT_L                 	0xc0
-#define	BGI_TH                   	0xc
-#define	BRXPATH_A                 	0x1
-#define	BRXPATH_B                 	0x2
-#define	BRXPATH_C                 	0x4
-#define	BRXPATH_D                 	0x8
-#define	BTXPATH_A                 	0x1
-#define	BTXPATH_B                 	0x2
-#define	BTXPATH_C                 	0x4
-#define	BTXPATH_D                 	0x8
-#define	BTRSSI_FREQ               	0x200
-#define	BADC_BACKOFF              	0x3000
-#define	BDFIR_BACKOFF             	0xc000
-#define	BTRSSI_LATCH_PHASE         	0x10000
-#define	BRX_LDC_OFFSET             	0xff
-#define	BRX_QDC_OFFSET             	0xff00
-#define	BRX_DFIR_MODE              	0x1800000
-#define	BRX_DCNF_TYPE              	0xe000000
-#define	BRXIQIMB_A               	0x3ff
-#define	BRXIQIMB_B               	0xfc00
-#define	BRXIQIMB_C               	0x3f0000
-#define	BRXIQIMB_D               	0xffc00000
-#define	BDC_DC_NOTCH             	0x60000
-#define	BRXNB_NOTCH              	0x1f000000
-#define	BPD_TH                   	0xf
-#define	BPD_TH_OPT2              	0xc000
-#define	BPWED_TH                 	0x700
-#define	BIFMF_WIN_L              	0x800
-#define	BPD_OPTION               	0x1000
-#define	BMF_WIN_L                	0xe000
-#define	BBW_SEARCH_L             	0x30000
-#define	BWIN_ENH_L               	0xc0000
-#define	BBW_TH                   	0x700000
-#define	BED_TH2                  	0x3800000
-#define	BBW_OPTION               	0x4000000
-#define	BRADIO_TH                	0x18000000
-#define	BWINDOW_L                	0xe0000000
-#define	BSBD_OPTION              	0x1
-#define	BFRAME_TH                	0x1c
-#define	BFS_OPTION               	0x60
-#define	BDC_SLOPE_CHECK          	0x80
-#define	BFGUARD_COUNTER_DC_L     	0xe00
-#define	BFRAME_WEIGHT_SHORT      	0x7000
-#define	BSUB_TUNE                	0xe00000
-#define	BFRAME_DC_LENGTH         	0xe000000
-#define	BSBD_START_OFFSET        	0x30000000
-#define	BFRAME_TH_2              	0x7
-#define	BFRAME_GI2_TH            	0x38
-#define	BGI2_SYNC_EN             	0x40
-#define	BSARCH_SHORT_EARLY       	0x300
-#define	BSARCH_SHORT_LATE        	0xc00
-#define	BSARCH_GI2_LATE          	0x70000
-#define	BCFOANTSUM               	0x1
-#define	BCFOACC                  	0x2
-#define	BCFOSTARTOFFSET          	0xc
-#define	BCFOLOOPBACK             	0x70
-#define	BCFOSUMWEIGHT            	0x80
-#define	BDAGCENABLE              	0x10000
-#define	BTXIQIMB_A               	0x3ff
-#define	BTXIQIMB_b               	0xfc00
-#define	BTXIQIMB_C               	0x3f0000
-#define	BTXIQIMB_D               	0xffc00000
-#define	BTXIDCOFFSET             	0xff
-#define	BTXIQDCOFFSET             	0xff00
-#define	BTXDFIRMODE              	0x10000
-#define	BTXPESUDO_NOISEON         	0x4000000
-#define	BTXPESUDO_NOISE_A         	0xff
-#define	BTXPESUDO_NOISE_B         	0xff00
-#define	BTXPESUDO_NOISE_C         	0xff0000
-#define	BTXPESUDO_NOISE_D         	0xff000000
-#define	BCCA_DROPOPTION           	0x20000
-#define	BCCA_DROPTHRES            	0xfff00000
-#define	BEDCCA_H                 	0xf
-#define	BEDCCA_L                 	0xf0
-#define	BLAMBDA_ED               	0x300
-#define	BRX_INITIALGAIN           	0x7f
-#define	BRX_ANTDIV_EN              	0x80
-#define	BRX_AGC_ADDRESS_FOR_LNA     0x7f00
-#define	BRX_HIGHPOWER_FLOW         	0x8000
-#define	BRX_AGC_FREEZE_THRES        0xc0000
-#define	BRX_FREEZESTEP_AGC1       	0x300000
-#define	BRX_FREEZESTEP_AGC2       	0xc00000
-#define	BRX_FREEZESTEP_AGC3       	0x3000000
-#define	BRX_FREEZESTEP_AGC0       	0xc000000
-#define	BRXRSSI_CMP_EN           	0x10000000
-#define	BRXQUICK_AGCEN            	0x20000000
-#define	BRXAGC_FREEZE_THRES_MODE    0x40000000
-#define	BRX_OVERFLOW_CHECKTYPE     	0x80000000
-#define	BRX_AGCSHIFT              	0x7f
-#define	BTRSW_TRI_ONLY           	0x80
-#define	BPOWER_THRES              	0x300
-#define	BRXAGC_EN                 	0x1
-#define	BRXAGC_TOGETHER_EN         	0x2
-#define	BRXAGC_MIN                	0x4
-#define	BRXHP_INI                	0x7
-#define	BRXHP_TRLNA              	0x70
-#define	BRXHP_RSSI               	0x700
-#define	BRXHP_BBP1               	0x7000
-#define	BRXHP_BBP2               	0x70000
-#define	BRXHP_BBP3               	0x700000
-#define	BRSSI_H                  	0x7f0000
-#define	BRSSI_GEN                	0x7f000000
-#define	BRXSETTLE_TRSW           	0x7
-#define	BRXSETTLE_LNA            	0x38
-#define	BRXSETTLE_RSSI           	0x1c0
-#define	BRXSETTLE_BBP            	0xe00
-#define	BRXSETTLE_RXHP           	0x7000
-#define	BRXSETTLE_ANTSW_RSSI     	0x38000
-#define	BRXSETTLE_ANTSW          	0xc0000
-#define	BRXPROCESS_TIME_DAGC      	0x300000
-#define	BRXSETTLE_HSSI           	0x400000
-#define	BRXPROCESS_TIME_BBPPW     	0x800000
-#define	BRXANTENNA_POWER_SHIFT     	0x3000000
-#define	BRSSI_TABLE_SELECT         	0xc000000
-#define	BRXHP_FINAL              	0x7000000
-#define	BRXHPSETTLE_BBP          	0x7
-#define	BRXHTSETTLE_HSSI         	0x8
-#define	BRXHTSETTLE_RXHP         	0x70
-#define	BRXHTSETTLE_BBPPW        	0x80
-#define	BRXHTSETTLE_IDLE         	0x300
-#define	BRXHTSETTLE_RESERVED     	0x1c00
-#define	BRXHT_RXHP_EN              	0x8000
-#define	BRXAGC_FREEZE_THRES      	0x30000
-#define	BRXAGC_TOGETHEREN       	0x40000
-#define	BRXHTAGC_MIN              	0x80000
-#define	BRXHTAGC_EN               	0x100000
-#define	BRXHTDAGC_EN              	0x200000
-#define	BRXHT_RXHP_BBP            	0x1c00000
-#define	BRXHT_RXHP_FINAL          	0xe0000000
-#define	BRXPW_RADIO_TH             	0x3
-#define	BRXPW_RADIO_EN             	0x4
-#define	BRXMF_HOLD                	0x3800
-#define	BRXPD_DELAY_TH1          	0x38
-#define	BRXPD_DELAY_TH2          	0x1c0
-#define	BRXPD_DC_COUNT_MAX       	0x600
-#define	BRXPD_DELAY_TH           	0x8000
-#define	BRXPROCESS_DELAY         	0xf0000
-#define	BRXSEARCHRANGE_GI2_EARLY 	0x700000
-#define	BRXFRAME_FUARD_COUNTER_L 	0x3800000
-#define	BRXSGI_GUARD_L           	0xc000000
-#define	BRXSGI_SEARCH_L          	0x30000000
-#define	BRXSGI_TH                	0xc0000000
-#define	BDFSCNT0                 	0xff
-#define	BDFSCNT1                 	0xff00
-#define	BDFSFLAG                 	0xf0000
-#define	BMF_WEIGHT_SUM             	0x300000
-#define	BMINIDX_TH                	0x7f000000
-#define	BDAFORMAT                	0x40000
-#define	BTXCH_EMU_ENABLE           	0x01000000
-#define	BTRSW_ISOLATION_A         	0x7f
-#define	BTRSW_ISOLATION_B         	0x7f00
-#define	BTRSW_ISOLATION_C         	0x7f0000
-#define	BTRSW_ISOLATION_D         	0x7f000000
-#define	BEXT_LNA_GAIN              	0x7c00
-
-#define	BSTBC_EN                  	0x4
-#define	BANTENNA_MAPPING          	0x10
-#define	BNSS                     	0x20
-#define	BCFO_ANTSUM_ID              0x200
-#define	BPHY_COUNTER_RESET         	0x8000000
-#define	BCFO_REPORT_GET            	0x4000000
-#define	BOFDM_CONTINUE_TX          	0x10000000
-#define	BOFDM_SINGLE_CARRIER       	0x20000000
-#define	BOFDM_SINGLE_TONE          	0x40000000
-#define	BHT_DETECT                	0x100
-#define	BCFOEN                   	0x10000
-#define	BCFOVALUE                	0xfff00000
-#define	BSIGTONE_RE              	0x3f
-#define	BSIGTONE_IM              	0x7f00
-#define	BCOUNTER_CCA             	0xffff
-#define	BCOUNTER_PARITYFAIL      	0xffff0000
-#define	BCOUNTER_RATEILLEGAL     	0xffff
-#define	BCOUNTER_CRC8FAIL        	0xffff0000
-#define	BCOUNTER_MCSNOSUPPORT    	0xffff
-#define	BCOUNTER_FASTSYNC        	0xffff
-#define	BSHORTCFO                	0xfff
-#define	BSHORTCFOT_LENGTH         	12
-#define	BSHORTCFOF_LENGTH         	11
-#define	BLONGCFO                 	0x7ff
-#define	BLONGCFOT_LENGTH          	11
-#define	BLONGCFOF_LENGTH          	11
-#define	BTAILCFO                 	0x1fff
-#define	BTAILCFOT_LENGTH          	13
-#define	BTAILCFOF_LENGTH          	12
-#define	BNOISE_EN_PWDB             	0xffff
-#define	BCC_POWER_DB             	0xffff0000
-#define	BMOISE_PWDB              	0xffff
-#define	BPOWERMEAST_LENGTH        	10
-#define	BPOWERMEASF_LENGTH        	3
-#define	BRX_HT_BW                	0x1
-#define	BRXSC                    	0x6
-#define	BRX_HT                   	0x8
-#define	BNB_INTF_DET_ON          	0x1
-#define	BINTF_WIN_LEN_CFG        	0x30
-#define	BNB_INTF_TH_CFG          	0x1c0
-#define	BRFGAIN                  	0x3f
-#define	BTABLESEL                	0x40
-#define	BTRSW                    	0x80
-#define	BRXSNR_A                 	0xff
-#define	BRXSNR_B                 	0xff00
-#define	BRXSNR_C                 	0xff0000
-#define	BRXSNR_D                 	0xff000000
-#define	BSNR_EVMT_LENGTH           	8
-#define	BSNR_EVMF_LENGTH           	1
-#define	BCSI1ST                  	0xff
-#define	BCSI2ND                  	0xff00
-#define	BRXEVM1ST                	0xff0000
-#define	BRXEVM2ND                	0xff000000
-#define	BSIGEVM                  	0xff
-#define	BPWDB                    	0xff00
-#define	BSGIEN                   	0x10000
-
-#define	BSFACTOR_QMA1             	0xf
-#define	BSFACTOR_QMA2             	0xf0
-#define	BSFACTOR_QMA3             	0xf00
-#define	BSFACTOR_QMA4             	0xf000
-#define	BSFACTOR_QMA5             	0xf0000
-#define	BSFACTOR_QMA6             	0xf0000
-#define	BSFACTOR_QMA7             	0xf00000
-#define	BSFACTOR_QMA8             	0xf000000
-#define	BSFACTOR_QMA9             	0xf0000000
-#define	BCSI_SCHEME               	0x100000
-
-#define	BNOISE_LVL_TOP_SET          0x3
-#define	BCHSMOOTH                	0x4
-#define	BCHSMOOTH_CFG1            	0x38
-#define	BCHSMOOTH_CFG2            	0x1c0
-#define	BCHSMOOTH_CFG3            	0xe00
-#define	BCHSMOOTH_CFG4            	0x7000
-#define	BMRCMODE                 	0x800000
-#define	BTHEVMCFG                	0x7000000
-
-#define	BLOOP_FIT_TYPE             	0x1
-#define	BUPD_CFO                  	0x40
-#define	BUPD_CFO_OFFDATA           	0x80
-#define	BADV_UPD_CFO               	0x100
-#define	BADV_TIME_CTRL             	0x800
-#define	BUPD_CLKO                 	0x1000
-#define	BFC                      	0x6000
-#define	BTRACKING_MODE            	0x8000
-#define	BPHCMP_ENABLE             	0x10000
-#define	BUPD_CLKO_LTF              	0x20000
-#define	BCOM_CH_CFO                	0x40000
-#define	BCSI_ESTI_MODE             	0x80000
-#define	BADV_UPD_EQZ               	0x100000
-#define	BUCHCFG                  	0x7000000
-#define	BUPDEQZ                  	0x8000000
-
-#define	BRX_PESUDO_NOISE_ON         0x20000000
-#define	BRX_PESUDO_NOISE_A         	0xff
-#define	BRX_PESUDO_NOISE_B         	0xff00
-#define	BRX_PESUDO_NOISE_C         	0xff0000
-#define	BRX_PESUDO_NOISE_D         	0xff000000
-#define	BRX_PESUDO_NOISESTATE_A     0xffff
-#define	BRX_PESUDO_NOISESTATE_B     0xffff0000
-#define	BRX_PESUDO_NOISESTATE_C     0xffff
-#define	BRX_PESUDO_NOISESTATE_D     0xffff0000
-
-#define	BZEBRA1_HSSIENABLE        	0x8
-#define	BZEBRA1_TRXCONTROL        	0xc00
-#define	BZEBRA1_TRXGAINSETTING    	0x07f
-#define	BZEBRA1_RXCOUNTER          	0xc00
-#define	BZEBRA1_TXCHANGEPUMP      	0x38
-#define	BZEBRA1_RXCHANGEPUMP      	0x7
-#define	BZEBRA1_CHANNEL_NUM        	0xf80
-#define	BZEBRA1_TXLPFBW           	0x400
-#define	BZEBRA1_RXLPFBW           	0x600
-
-#define	BRTL8256REG_MODE_CTRL1      0x100
-#define	BRTL8256REG_MODE_CTRL0      0x40
-#define	BRTL8256REG_TXLPFBW         0x18
-#define	BRTL8256REG_RXLPFBW         0x600
-
-#define	BRTL8258_TXLPFBW          	0xc
-#define	BRTL8258_RXLPFBW          	0xc00
-#define	BRTL8258_RSSILPFBW        	0xc0
-
-#define	BBYTE0                    	0x1
-#define	BBYTE1                    	0x2
-#define	BBYTE2                    	0x4
-#define	BBYTE3                    	0x8
-#define	BWORD0                    	0x3
-#define	BWORD1                    	0xc
-#define	BWORD                    	0xf
-
-#define	MASKBYTE0                	0xff
-#define	MASKBYTE1                	0xff00
-#define	MASKBYTE2                	0xff0000
-#define	MASKBYTE3                	0xff000000
-#define	MASKHWORD                	0xffff0000
-#define	MASKLWORD                	0x0000ffff
-#define	MASKDWORD					0xffffffff
-#define	MASK12BITS					0xfff
-#define	MASKH4BITS					0xf0000000
-#define MASKOFDM_D					0xffc00000
-#define	MASKCCK						0x3f3f3f3f
-
-#define	MASK4BITS              		0x0f
-#define	MASK20BITS             		0xfffff
-#define RFREG_OFFSET_MASK			0xfffff
-
-#define	BENABLE                   	0x1
-#define	BDISABLE                  	0x0
-
-#define	LEFT_ANTENNA               	0x0
-#define	RIGHT_ANTENNA              	0x1
-
-#define	TCHECK_TXSTATUS            	500
-#define	TUPDATE_RXCOUNTER          	100
-
-#define	REG_UN_used_register		0x01bf
-
-/* WOL bit information */
-#define	HAL92C_WOL_PTK_UPDATE_EVENT		BIT(0)
-#define	HAL92C_WOL_GTK_UPDATE_EVENT		BIT(1)
-#define	HAL92C_WOL_DISASSOC_EVENT		BIT(2)
-#define	HAL92C_WOL_DEAUTH_EVENT			BIT(3)
-#define	HAL92C_WOL_FW_DISCONNECT_EVENT	BIT(4)
-
-#define		WOL_REASON_PTK_UPDATE		BIT(0)
-#define		WOL_REASON_GTK_UPDATE		BIT(1)
-#define		WOL_REASON_DISASSOC			BIT(2)
-#define		WOL_REASON_DEAUTH			BIT(3)
-#define		WOL_REASON_FW_DISCONNECT	BIT(4)
-
-#define		RA_RFE_PINMUX	0xcb0  /* Path_A RFE control pinmux*/
-#define		RB_RFE_PINMUX	0xeb0 /* Path_B RFE control pinmux*/
-
-#define		RA_RFE_INV 0xcb4
-#define		RB_RFE_INV 0xeb4
-
-/* RXIQC */
-#define		RA_RXIQC_AB    	0xc10  /*RxIQ imbalance matrix coeff. A & B*/
-#define		RA_RXIQC_CD    	0xc14  /*RxIQ imbalance matrix coeff. C & D*/
-#define	 	RA_TXSCALE 		0xc1c  /* Pah_A TX scaling factor*/
-#define		RB_TXSCALE 		0xe1c  /* Path_B TX scaling factor*/
-#define		RB_RXIQC_AB    	0xe10  /*RxIQ imbalance matrix coeff. A & B*/
-#define		RB_RXIQC_CD    	0xe14  /*RxIQ imbalance matrix coeff. C & D*/
-#define		RXIQC_AC		0x02ff  /*bit mask for IQC matrix element A & C*/
-#define		RXIQC_BD		0x02ff0000 /*bit mask for IQC matrix element A & C*/
-
-/* 2 EFUSE_TEST (For RTL8723 partially) */
-#define EFUSE_SEL(x)					(((x) & 0x3) << 8)
-#define EFUSE_SEL_MASK				0x300
-#define EFUSE_WIFI_SEL_0				0x0
-
-/*REG_MULTI_FUNC_CTRL(For RTL8723 Only)*/
-#define	WL_HWPDN_EN				BIT(0)	/* Enable GPIO[9] as WiFi HW PDn source*/
-#define	WL_HWPDN_SL				BIT(1)	/* WiFi HW PDn polarity control*/
-#define	WL_FUNC_EN				BIT(2)	// WiFi function enable
-#define	WL_HWROF_EN				BIT(3)	// Enable GPIO[9] as WiFi RF HW PDn source
-#define	BT_HWPDN_EN				BIT(16)	// Enable GPIO[11] as BT HW PDn source
-#define	BT_HWPDN_SL				BIT(17)	// BT HW PDn polarity control
-#define	BT_FUNC_EN				BIT(18)	// BT function enable
-#define	BT_HWROF_EN				BIT(19)	// Enable GPIO[11] as BT/GPS RF HW PDn source
-#define	GPS_HWPDN_EN				BIT(20)	// Enable GPIO[10] as GPS HW PDn source
-#define	GPS_HWPDN_SL				BIT(21)	// GPS HW PDn polarity control
-#define	GPS_FUNC_EN				BIT(22)	// GPS function enable
-
-
-#define	BMASKBYTE0                	0xff
-#define	BMASKBYTE1                	0xff00
-#define	BMASKBYTE2                	0xff0000
-#define	BMASKBYTE3                	0xff000000
-#define	BMASKHWORD                	0xffff0000
-#define	BMASKLWORD                	0x0000ffff
-#define	BMASKDWORD                	0xffffffff
-#define	BMASK12BITS				  	0xfff
-#define	BMASKH4BITS				  	0xf0000000
-#define BMASKOFDM_D					0xffc00000
-#define	BMASKCCK				  	0x3f3f3f3f
-
-#define BRFREGOFFSETMASK			0xfffff
-
-#define	ODM_REG_CCK_RPT_FORMAT_11AC	0x804
-#define	ODM_REG_BB_RX_PATH_11AC			0x808
-/*PAGE 9*/
-#define	ODM_REG_OFDM_FA_RST_11AC		0x9A4
-/*PAGE A*/
-#define	ODM_REG_CCK_CCA_11AC			0xA0A
-#define	ODM_REG_CCK_FA_RST_11AC			0xA2C
-#define	ODM_REG_CCK_FA_11AC				0xA5C
-/*PAGE C*/
-#define	ODM_REG_IGI_A_11AC				0xC50
-/*PAGE E*/
-#define	ODM_REG_IGI_B_11AC				0xE50
-/*PAGE F*/
-#define	ODM_REG_OFDM_FA_11AC			0xF48
-
-
-//2 MAC REG LIST
-
-
-
-
-//DIG Related
-#define	ODM_BIT_IGI_11AC					0xFFFFFFFF
-#define	ODM_BIT_CCK_RPT_FORMAT_11AC		BIT16
-#define	ODM_BIT_BB_RX_PATH_11AC			0xF
-
-typedef enum AGGRE_SIZE{
-	HT_AGG_SIZE_8K = 0,
-	HT_AGG_SIZE_16K = 1,
-	HT_AGG_SIZE_32K = 2,
-	HT_AGG_SIZE_64K = 3,
-	VHT_AGG_SIZE_128K = 4,
-	VHT_AGG_SIZE_256K = 5,
-	VHT_AGG_SIZE_512K = 6,
-	VHT_AGG_SIZE_1024K = 7,
-}AGGRE_SIZE_E, *PAGGRE_SIZE_E;
-
-#define REG_AMPDU_MAX_LENGTH_8812	0x0458
-
-#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/rf.h b/drivers/staging/rtl8821ae/rtl8821ae/rf.h
deleted file mode 100644
index b665c0ff1b7d..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/rf.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL8821AE_RF_H__
-#define __RTL8821AE_RF_H__
-
-#define RF6052_MAX_TX_PWR		0x3F
-#define RF6052_MAX_REG			0x3F
-
-extern void rtl8821ae_phy_rf6052_set_bandwidth(struct ieee80211_hw *hw,
-					    									u8 bandwidth);
-extern void rtl8821ae_phy_rf6052_set_cck_txpower(struct ieee80211_hw *hw,
-					      									  u8 *ppowerlevel);
-extern void rtl8821ae_phy_rf6052_set_ofdm_txpower(struct ieee80211_hw *hw,
-					      									   u8 *ppowerlevel_ofdm,
-					      									   u8 *ppowerlevel_bw20,
-					      									   u8 *ppowerlevel_bw40,
-					      									   u8 channel);
-extern bool rtl8821ae_phy_rf6052_config(struct ieee80211_hw *hw);
-#endif
diff --git a/drivers/staging/rtl8821ae/rtl8821ae/trx.c b/drivers/staging/rtl8821ae/rtl8821ae/trx.c
deleted file mode 100644
index b4943e8b7f80..000000000000
--- a/drivers/staging/rtl8821ae/rtl8821ae/trx.c
+++ /dev/null
@@ -1,1005 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#include "../wifi.h"
-#include "../pci.h"
-#include "../base.h"
-#include "../stats.h"
-#include "reg.h"
-#include "def.h"
-#include "phy.h"
-#include "trx.h"
-#include "led.h"
-#include "dm.h"
-
-static u8 _rtl8821ae_map_hwqueue_to_fwqueue(struct sk_buff *skb, u8 hw_queue)
-{
-	u16 fc = rtl_get_fc(skb);
-
-	if (unlikely(ieee80211_is_beacon(fc)))
-		return QSLT_BEACON;
-	if (ieee80211_is_mgmt(fc) || ieee80211_is_ctl(fc))
-		return QSLT_MGNT;
-
-	return skb->priority;
-}
-
-/* mac80211's rate_idx is like this:
- *
- * 2.4G band:rx_status->band == IEEE80211_BAND_2GHZ
- *
- * B/G rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC_RATE1M-->DESC_RATE54M ==> idx is 0-->11,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
- *
- * 5G band:rx_status->band == IEEE80211_BAND_5GHZ
- * A rate:
- * (rx_status->flag & RX_FLAG_HT) = 0,
- * DESC_RATE6M-->DESC_RATE54M ==> idx is 0-->7,
- *
- * N rate:
- * (rx_status->flag & RX_FLAG_HT) = 1,
- * DESC_RATEMCS0-->DESC_RATEMCS15 ==> idx is 0-->15
- */
-static int _rtl8821ae_rate_mapping(struct ieee80211_hw *hw,
-	bool isht, u8 desc_rate)
-{
-	int rate_idx;
-
-	if (false == isht) {
-		if (IEEE80211_BAND_2GHZ == hw->conf.chandef.chan->band) {
-			switch (desc_rate) {
-			case DESC_RATE1M:
-				rate_idx = 0;
-				break;
-			case DESC_RATE2M:
-				rate_idx = 1;
-				break;
-			case DESC_RATE5_5M:
-				rate_idx = 2;
-				break;
-			case DESC_RATE11M:
-				rate_idx = 3;
-				break;
-			case DESC_RATE6M:
-				rate_idx = 4;
-				break;
-			case DESC_RATE9M:
-				rate_idx = 5;
-				break;
-			case DESC_RATE12M:
-				rate_idx = 6;
-				break;
-			case DESC_RATE18M:
-				rate_idx = 7;
-				break;
-			case DESC_RATE24M:
-				rate_idx = 8;
-				break;
-			case DESC_RATE36M:
-				rate_idx = 9;
-				break;
-			case DESC_RATE48M:
-				rate_idx = 10;
-				break;
-			case DESC_RATE54M:
-				rate_idx = 11;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		} else {
-			switch (desc_rate) {
-			case DESC_RATE6M:
-				rate_idx = 0;
-				break;
-			case DESC_RATE9M:
-				rate_idx = 1;
-				break;
-			case DESC_RATE12M:
-				rate_idx = 2;
-				break;
-			case DESC_RATE18M:
-				rate_idx = 3;
-				break;
-			case DESC_RATE24M:
-				rate_idx = 4;
-				break;
-			case DESC_RATE36M:
-				rate_idx = 5;
-				break;
-			case DESC_RATE48M:
-				rate_idx = 6;
-				break;
-			case DESC_RATE54M:
-				rate_idx = 7;
-				break;
-			default:
-				rate_idx = 0;
-				break;
-			}
-		}
-	} else {
-		switch(desc_rate) {
-		case DESC_RATEMCS0:
-			rate_idx = 0;
-			break;
-		case DESC_RATEMCS1:
-			rate_idx = 1;
-			break;
-		case DESC_RATEMCS2:
-			rate_idx = 2;
-			break;
-		case DESC_RATEMCS3:
-			rate_idx = 3;
-			break;
-		case DESC_RATEMCS4:
-			rate_idx = 4;
-			break;
-		case DESC_RATEMCS5:
-			rate_idx = 5;
-			break;
-		case DESC_RATEMCS6:
-			rate_idx = 6;
-			break;
-		case DESC_RATEMCS7:
-			rate_idx = 7;
-			break;
-		case DESC_RATEMCS8:
-			rate_idx = 8;
-			break;
-		case DESC_RATEMCS9:
-			rate_idx = 9;
-			break;
-		case DESC_RATEMCS10:
-			rate_idx = 10;
-			break;
-		case DESC_RATEMCS11:
-			rate_idx = 11;
-			break;
-		case DESC_RATEMCS12:
-			rate_idx = 12;
-			break;
-		case DESC_RATEMCS13:
-			rate_idx = 13;
-			break;
-		case DESC_RATEMCS14:
-			rate_idx = 14;
-			break;
-		case DESC_RATEMCS15:
-			rate_idx = 15;
-			break;
-		default:
-			rate_idx = 0;
-			break;
-		}
-	}
-	return rate_idx;
-}
-
-static void _rtl8821ae_query_rxphystatus(struct ieee80211_hw *hw,
-			struct rtl_stats *pstatus, u8 *pdesc,
-			struct rx_fwinfo_8821ae *p_drvinfo, bool bpacket_match_bssid,
-			bool bpacket_toself, bool b_packet_beacon)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_ps_ctl *ppsc = rtl_psc(rtlpriv);
-	struct phy_sts_cck_8821ae_t *cck_buf;
-	struct phy_status_rpt *p_phystRpt = (struct phy_status_rpt *)p_drvinfo;
-	struct rtl_dm *rtldm = rtl_dm(rtl_priv(hw));
-	char rx_pwr_all = 0, rx_pwr[4];
-	u8 rf_rx_num = 0, evm, pwdb_all;
-	u8 i, max_spatial_stream;
-	u32 rssi, total_rssi = 0;
-	bool b_is_cck = pstatus->b_is_cck;
-	u8 lan_idx,vga_idx;
-
-	/* Record it for next packet processing */
-	pstatus->b_packet_matchbssid = bpacket_match_bssid;
-	pstatus->b_packet_toself = bpacket_toself;
-	pstatus->b_packet_beacon = b_packet_beacon;
-	pstatus->rx_mimo_signalquality[0] = -1;
-	pstatus->rx_mimo_signalquality[1] = -1;
-
-	if (b_is_cck) {
-		u8 cck_highpwr;
-		u8 cck_agc_rpt;
-		/* CCK Driver info Structure is not the same as OFDM packet. */
-		cck_buf = (struct phy_sts_cck_8821ae_t *)p_drvinfo;
-		cck_agc_rpt = cck_buf->cck_agc_rpt;
-
-		/* (1)Hardware does not provide RSSI for CCK */
-		/* (2)PWDB, Average PWDB calculated by
-		 * hardware (for rate adaptive) */
-		if (ppsc->rfpwr_state == ERFON)
-			cck_highpwr = (u8) rtl_get_bbreg(hw, RFPGA0_XA_HSSIPARAMETER2,
-							 BIT(9));
-		else
-			cck_highpwr = false;
-
-		lan_idx = ((cck_agc_rpt & 0xE0) >> 5);
-		vga_idx = (cck_agc_rpt & 0x1f);
-		switch (lan_idx) {
-			case 7:
-				if(vga_idx <= 27)
-					rx_pwr_all = -100 + 2*(27-vga_idx); /*VGA_idx = 27~2*/
-				else
-					rx_pwr_all = -100;
-				break;
-			case 6:
-				rx_pwr_all = -48 + 2*(2-vga_idx); /*VGA_idx = 2~0*/
-				break;
-			case 5:
-				rx_pwr_all = -42 + 2*(7-vga_idx); /*VGA_idx = 7~5*/
-				break;
-			case 4:
-				rx_pwr_all = -36 + 2*(7-vga_idx); /*VGA_idx = 7~4*/
-				break;
-			case 3:
-				rx_pwr_all = -24 + 2*(7-vga_idx); /*VGA_idx = 7~0*/
-				break;
-			case 2:
-				if(cck_highpwr)
-					rx_pwr_all = -12 + 2*(5-vga_idx); /*VGA_idx = 5~0*/
-				else
-					rx_pwr_all = -6+ 2*(5-vga_idx);
-				break;
-			case 1:
-				rx_pwr_all = 8-2*vga_idx;
-				break;
-			case 0:
-				rx_pwr_all = 14-2*vga_idx;
-				break;
-			default:
-				break;
-		}
-		rx_pwr_all += 6;
-		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
-		/* CCK gain is smaller than OFDM/MCS gain,  */
-		/* so we add gain diff by experiences,
-		 * the val is 6 */
-		pwdb_all += 6;
-		if(pwdb_all > 100)
-			pwdb_all = 100;
-		/* modify the offset to make the same
-		 * gain index with OFDM. */
-		if(pwdb_all > 34 && pwdb_all <= 42)
-			pwdb_all -= 2;
-		else if(pwdb_all > 26 && pwdb_all <= 34)
-			pwdb_all -= 6;
-		else if(pwdb_all > 14 && pwdb_all <= 26)
-			pwdb_all -= 8;
-		else if(pwdb_all > 4 && pwdb_all <= 14)
-			pwdb_all -= 4;
-		if (cck_highpwr == false){
-			if (pwdb_all >= 80)
-				pwdb_all =((pwdb_all-80)<<1)+((pwdb_all-80)>>1)+80;
-			else if((pwdb_all <= 78) && (pwdb_all >= 20))
-				pwdb_all += 3;
-			if(pwdb_all>100)
-				pwdb_all = 100;
-		}
-
-		pstatus->rx_pwdb_all = pwdb_all;
-		pstatus->recvsignalpower = rx_pwr_all;
-
-		/* (3) Get Signal Quality (EVM) */
-		if (bpacket_match_bssid) {
-			u8 sq;
-
-			if (pstatus->rx_pwdb_all > 40)
-				sq = 100;
-			else {
-				sq = cck_buf->sq_rpt;
-				if (sq > 64)
-					sq = 0;
-				else if (sq < 20)
-					sq = 100;
-				else
-					sq = ((64 - sq) * 100) / 44;
-			}
-
-			pstatus->signalquality = sq;
-			pstatus->rx_mimo_signalquality[0] = sq;
-			pstatus->rx_mimo_signalquality[1] = -1;
-		}
-	} else {
-		rtlpriv->dm.brfpath_rxenable[0] =
-		    rtlpriv->dm.brfpath_rxenable[1] = true;
-
-		/* (1)Get RSSI for HT rate */
-		for (i = RF90_PATH_A; i < RF6052_MAX_PATH; i++) {
-
-			/* we will judge RF RX path now. */
-			if (rtlpriv->dm.brfpath_rxenable[i])
-				rf_rx_num++;
-
-			rx_pwr[i] = ((p_drvinfo->gain_trsw[i] & 0x3f) * 2) - 110;
-
-			/* Translate DBM to percentage. */
-			rssi = rtl_query_rxpwrpercentage(rx_pwr[i]);
-			total_rssi += rssi;
-
-			/* Get Rx snr value in DB */
-			rtlpriv->stats.rx_snr_db[i] = (long)(p_drvinfo->rxsnr[i] / 2);
-
-			/* Record Signal Strength for next packet */
-			if (bpacket_match_bssid)
-				pstatus->rx_mimo_signalstrength[i] = (u8) rssi;
-		}
-
-		/* (2)PWDB, Average PWDB calculated by
-		 * hardware (for rate adaptive) */
-		rx_pwr_all = ((p_drvinfo->pwdb_all >> 1) & 0x7f) - 110;
-
-		pwdb_all = rtl_query_rxpwrpercentage(rx_pwr_all);
-		pstatus->rx_pwdb_all = pwdb_all;
-		pstatus->rxpower = rx_pwr_all;
-		pstatus->recvsignalpower = rx_pwr_all;
-
-		/* (3)EVM of HT rate */
-		if (pstatus->b_is_ht && pstatus->rate >= DESC_RATEMCS8 &&
-		    pstatus->rate <= DESC_RATEMCS15)
-			max_spatial_stream = 2;
-		else
-			max_spatial_stream = 1;
-
-		for (i = 0; i < max_spatial_stream; i++) {
-			evm = rtl_evm_db_to_percentage(p_drvinfo->rxevm[i]);
-
-			if (bpacket_match_bssid) {
-				/* Fill value in RFD, Get the first
-				 * spatial stream only */
-				if (i == 0)
-					pstatus->signalquality = (u8) (evm & 0xff);
-				pstatus->rx_mimo_signalquality[i] = (u8) (evm & 0xff);
-			}
-		}
-	}
-
-	/* UI BSS List signal strength(in percentage),
-	 * make it good looking, from 0~100. */
-	if (b_is_cck)
-		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
-			pwdb_all));
-	else if (rf_rx_num != 0)
-		pstatus->signalstrength = (u8)(rtl_signal_scale_mapping(hw,
-			total_rssi /= rf_rx_num));
-	/*HW antenna diversity*/
-	rtldm->fat_table.antsel_rx_keep_0 = p_phystRpt->ant_sel;
-	rtldm->fat_table.antsel_rx_keep_1 = p_phystRpt->ant_sel_b;
-	rtldm->fat_table.antsel_rx_keep_2 = p_phystRpt->antsel_rx_keep_2;
-
-}
-#if 0
-static void _rtl8821ae_smart_antenna(struct ieee80211_hw *hw,
-	struct rtl_stats *pstatus)
-{
-	struct rtl_dm *rtldm= rtl_dm(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse =rtl_efuse(rtl_priv(hw));
-	u8 antsel_tr_mux;
-	struct fast_ant_trainning *pfat_table = &(rtldm->fat_table);
-
-	if (rtlefuse->antenna_div_type == CG_TRX_SMART_ANTDIV) {
-		if (pfat_table->fat_state == FAT_TRAINING_STATE) {
-			if (pstatus->b_packet_toself) {
-				antsel_tr_mux = (pfat_table->antsel_rx_keep_2 << 2) |
-					(pfat_table->antsel_rx_keep_1 << 1) | pfat_table->antsel_rx_keep_0;
-				pfat_table->ant_sum_rssi[antsel_tr_mux] += pstatus->rx_pwdb_all;
-				pfat_table->ant_rssi_cnt[antsel_tr_mux]++;
-			}
-		}
-	} else if ((rtlefuse->antenna_div_type == CG_TRX_HW_ANTDIV) ||
-	(rtlefuse->antenna_div_type == CGCS_RX_HW_ANTDIV)) {
-		if (pstatus->b_packet_toself || pstatus->b_packet_matchbssid) {
-			antsel_tr_mux = (pfat_table->antsel_rx_keep_2 << 2) |
-					(pfat_table->antsel_rx_keep_1 << 1) | pfat_table->antsel_rx_keep_0;
-			rtl8821ae_dm_ant_sel_statistics(hw, antsel_tr_mux, 0, pstatus->rx_pwdb_all);
-		}
-
-	}
-}
-#endif
-static void _rtl8821ae_translate_rx_signal_stuff(struct ieee80211_hw *hw,
-		struct sk_buff *skb, struct rtl_stats *pstatus,
-		u8 *pdesc, struct rx_fwinfo_8821ae *p_drvinfo)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_efuse *rtlefuse = rtl_efuse(rtl_priv(hw));
-	struct ieee80211_hdr *hdr;
-	u8 *tmp_buf;
-	u8 *praddr;
-	u8 *psaddr;
-	u16 fc, type;
-	bool b_packet_matchbssid, b_packet_toself, b_packet_beacon;
-
-	tmp_buf = skb->data + pstatus->rx_drvinfo_size + pstatus->rx_bufshift;
-
-	hdr = (struct ieee80211_hdr *)tmp_buf;
-	fc = le16_to_cpu(hdr->frame_control);
-	type = WLAN_FC_GET_TYPE(fc);
-	praddr = hdr->addr1;
-	psaddr = ieee80211_get_SA(hdr);
-	memcpy(pstatus->psaddr, psaddr, ETH_ALEN);
-
-	b_packet_matchbssid = ((IEEE80211_FTYPE_CTL != type) &&
-	     (!ether_addr_equal(mac->bssid, (fc & IEEE80211_FCTL_TODS) ?
-				  hdr->addr1 : (fc & IEEE80211_FCTL_FROMDS) ?
-				  hdr->addr2 : hdr->addr3)) && (!pstatus->b_hwerror) &&
-				  (!pstatus->b_crc) && (!pstatus->b_icv));
-
-	b_packet_toself = b_packet_matchbssid &&
-	    (!ether_addr_equal(praddr, rtlefuse->dev_addr));
-
-	if (ieee80211_is_beacon(fc))
-		b_packet_beacon = true;
-	else
-		b_packet_beacon = false;
-
-	if (b_packet_beacon && b_packet_matchbssid)
-		rtl_priv(hw)->dm.dbginfo.num_qry_beacon_pkt++;
-
-	_rtl8821ae_query_rxphystatus(hw, pstatus, pdesc, p_drvinfo,
-				   b_packet_matchbssid, b_packet_toself,
-				   b_packet_beacon);
-	/*_rtl8821ae_smart_antenna(hw, pstatus); */
-	rtl_process_phyinfo(hw, tmp_buf, pstatus);
-}
-
-static void _rtl8821ae_insert_emcontent(struct rtl_tcb_desc *ptcb_desc,
-				      u8 *virtualaddress)
-{
-	u32 dwtmp = 0;
-	memset(virtualaddress, 0, 8);
-
-	SET_EARLYMODE_PKTNUM(virtualaddress, ptcb_desc->empkt_num);
-	if (ptcb_desc->empkt_num == 1)
-		dwtmp = ptcb_desc->empkt_len[0];
-	else {
-		dwtmp = ptcb_desc->empkt_len[0];
-		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
-		dwtmp += ptcb_desc->empkt_len[1];
-	}
-	SET_EARLYMODE_LEN0(virtualaddress, dwtmp);
-
-	if (ptcb_desc->empkt_num <= 3)
-		dwtmp = ptcb_desc->empkt_len[2];
-	else {
-		dwtmp = ptcb_desc->empkt_len[2];
-		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
-		dwtmp += ptcb_desc->empkt_len[3];
-	}
-	SET_EARLYMODE_LEN1(virtualaddress, dwtmp);
-	if (ptcb_desc->empkt_num <= 5)
-		dwtmp = ptcb_desc->empkt_len[4];
-	else {
-		dwtmp = ptcb_desc->empkt_len[4];
-		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
-		dwtmp += ptcb_desc->empkt_len[5];
-	}
-	SET_EARLYMODE_LEN2_1(virtualaddress, dwtmp & 0xF);
-	SET_EARLYMODE_LEN2_2(virtualaddress, dwtmp >> 4);
-	if (ptcb_desc->empkt_num <= 7)
-		dwtmp = ptcb_desc->empkt_len[6];
-	else {
-		dwtmp = ptcb_desc->empkt_len[6];
-		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
-		dwtmp += ptcb_desc->empkt_len[7];
-	}
-	SET_EARLYMODE_LEN3(virtualaddress, dwtmp);
-	if (ptcb_desc->empkt_num <= 9)
-		dwtmp = ptcb_desc->empkt_len[8];
-	else {
-		dwtmp = ptcb_desc->empkt_len[8];
-		dwtmp += ((dwtmp%4)?(4-dwtmp%4):0)+4;
-		dwtmp += ptcb_desc->empkt_len[9];
-	}
-	SET_EARLYMODE_LEN4(virtualaddress, dwtmp);
-}
-
-bool rtl8821ae_rx_query_desc(struct ieee80211_hw *hw,
-			   struct rtl_stats *status,
-			   struct ieee80211_rx_status *rx_status,
-			   u8 *pdesc, struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rx_fwinfo_8821ae *p_drvinfo;
-	struct ieee80211_hdr *hdr;
-
-	u32 phystatus = GET_RX_DESC_PHYST(pdesc);
-
-	status->length = (u16) GET_RX_DESC_PKT_LEN(pdesc);
-	status->rx_drvinfo_size = (u8) GET_RX_DESC_DRV_INFO_SIZE(pdesc) *
-	    RX_DRV_INFO_SIZE_UNIT;
-	status->rx_bufshift = (u8) (GET_RX_DESC_SHIFT(pdesc) & 0x03);
-	status->b_icv = (u16) GET_RX_DESC_ICV(pdesc);
-	status->b_crc = (u16) GET_RX_DESC_CRC32(pdesc);
-	status->b_hwerror = (status->b_crc | status->b_icv);
-	status->decrypted = !GET_RX_DESC_SWDEC(pdesc);
-	status->rate = (u8) GET_RX_DESC_RXMCS(pdesc);
-	status->b_shortpreamble = (u16) GET_RX_DESC_SPLCP(pdesc);
-	status->b_isampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	status->b_isfirst_ampdu = (bool) (GET_RX_DESC_PAGGR(pdesc) == 1);
-	status->timestamp_low = GET_RX_DESC_TSFL(pdesc);
-	status->rx_is40Mhzpacket = (bool) GET_RX_DESC_BW(pdesc);
-	status->macid = GET_RX_DESC_MACID(pdesc);
-	status->b_is_ht = (bool)GET_RX_DESC_RXHT(pdesc);
-
-	status->b_is_cck = RX_HAL_IS_CCK_RATE(status->rate);
-
-	if (GET_RX_STATUS_DESC_RPT_SEL(pdesc))
-		status->packet_report_type = C2H_PACKET;
-	else
-		status->packet_report_type = NORMAL_RX;
-
-	if (GET_RX_STATUS_DESC_PATTERN_MATCH(pdesc))
-		status->wake_match = BIT(2);
-	else if (GET_RX_STATUS_DESC_MAGIC_MATCH(pdesc))
-		status->wake_match = BIT(1);
-	else if (GET_RX_STATUS_DESC_UNICAST_MATCH(pdesc))
-		status->wake_match = BIT(0);
-	else
-		status->wake_match = 0;
-
-	if (status->wake_match)
-		RT_TRACE(COMP_RXDESC,DBG_LOUD,
-		("GGGGGGGGGGGGGet Wakeup Packet!! WakeMatch=%d\n",status->wake_match ));
-	rx_status->freq = hw->conf.chandef.chan->center_freq;
-	rx_status->band = hw->conf.chandef.chan->band;
-
-	hdr = (struct ieee80211_hdr *)(skb->data + status->rx_drvinfo_size
-			+ status->rx_bufshift);
-
-	if (status->b_crc)
-		rx_status->flag |= RX_FLAG_FAILED_FCS_CRC;
-
-	if (status->rx_is40Mhzpacket)
-		rx_status->flag |= RX_FLAG_40MHZ;
-
-	if (status->b_is_ht)
-		rx_status->flag |= RX_FLAG_HT;
-
-	rx_status->flag |= RX_FLAG_MACTIME_MPDU;
-
-	/* hw will set status->decrypted true, if it finds the
-	 * frame is open data frame or mgmt frame. */
-	/* So hw will not decryption robust management frame
-	 * for IEEE80211w but still set status->decrypted
-	 * true, so here we should set it back to undecrypted
-	 * for IEEE80211w frame, and mac80211 sw will help
-	 * to decrypt it */
-	if (status->decrypted) {
-		if (!hdr) {
-			WARN_ON_ONCE(true);
-			pr_err("decrypted is true but hdr NULL, from skb %p\n",
-				rtl_get_hdr(skb));
-				return false;
-		}
-
-		if ((_ieee80211_is_robust_mgmt_frame(hdr)) &&
-			(ieee80211_has_protected(hdr->frame_control)))
-			rx_status->flag &= ~RX_FLAG_DECRYPTED;
-		else
-			rx_status->flag |= RX_FLAG_DECRYPTED;
-	}
-
-	/* rate_idx: index of data rate into band's
-	 * supported rates or MCS index if HT rates
-	 * are use (RX_FLAG_HT)*/
-	/* Notice: this is diff with windows define */
-	rx_status->rate_idx = _rtl8821ae_rate_mapping(hw,
-				status->b_is_ht, status->rate);
-
-	rx_status->mactime = status->timestamp_low;
-	if (phystatus == true) {
-		p_drvinfo = (struct rx_fwinfo_8821ae *)(skb->data +
-						     status->rx_bufshift);
-
-		_rtl8821ae_translate_rx_signal_stuff(hw,
-						   skb, status, pdesc,
-						   p_drvinfo);
-	}
-
-	/*rx_status->qual = status->signal; */
-	rx_status->signal = status->recvsignalpower + 10;
-	/*rx_status->noise = -status->noise; */
-	if (status->packet_report_type == TX_REPORT2){
-		status->macid_valid_entry[0] = GET_RX_RPT2_DESC_MACID_VALID_1(pdesc);
-		status->macid_valid_entry[1] = GET_RX_RPT2_DESC_MACID_VALID_2(pdesc);
-	}
-	return true;
-}
-
-void rtl8821ae_tx_fill_desc(struct ieee80211_hw *hw,
-			  struct ieee80211_hdr *hdr, u8 *pdesc_tx, u8 *txbd,
-			  struct ieee80211_tx_info *info,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb,
-			  u8 hw_queue, struct rtl_tcb_desc *ptcb_desc)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl_hal *rtlhal = rtl_hal(rtlpriv);
-	u8 *pdesc = (u8 *) pdesc_tx;
-	u16 seq_number;
-	u16 fc = le16_to_cpu(hdr->frame_control);
-	unsigned int buf_len = 0;
-	unsigned int skb_len = skb->len;
-	u8 fw_qsel = _rtl8821ae_map_hwqueue_to_fwqueue(skb, hw_queue);
-	bool b_firstseg = ((hdr->seq_ctrl &
-			    cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0);
-	bool b_lastseg = ((hdr->frame_control &
-			   cpu_to_le16(IEEE80211_FCTL_MOREFRAGS)) == 0);
-	dma_addr_t mapping;
-	u8 bw_40 = 0;
-	u8 short_gi = 0;
-
-	if (mac->opmode == NL80211_IFTYPE_STATION) {
-		bw_40 = mac->bw_40;
-	} else if (mac->opmode == NL80211_IFTYPE_AP ||
-		mac->opmode == NL80211_IFTYPE_ADHOC) {
-		if (sta)
-			bw_40 = sta->ht_cap.cap & IEEE80211_HT_CAP_SUP_WIDTH_20_40;
-	}
-	seq_number = (le16_to_cpu(hdr->seq_ctrl) & IEEE80211_SCTL_SEQ) >> 4;
-	rtl_get_tcb_desc(hw, info, sta, skb, ptcb_desc);
-	/* reserve 8 byte for AMPDU early mode */
-	if (rtlhal->b_earlymode_enable) {
-		skb_push(skb, EM_HDR_LEN);
-		memset(skb->data, 0, EM_HDR_LEN);
-	}
-	buf_len = skb->len;
-	mapping = pci_map_single(rtlpci->pdev, skb->data, skb->len,
-				 PCI_DMA_TODEVICE);
-	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
-		RT_TRACE(COMP_SEND, DBG_TRACE,
-			 ("DMA mapping error"));
-		return;
-	}
-	CLEAR_PCI_TX_DESC_CONTENT(pdesc, sizeof(struct tx_desc_8821ae));
-	if (ieee80211_is_nullfunc(fc) || ieee80211_is_ctl(fc)) {
-		b_firstseg = true;
-		b_lastseg = true;
-	}
-	if (b_firstseg) {
-		if (rtlhal->b_earlymode_enable) {
-			SET_TX_DESC_PKT_OFFSET(pdesc, 1);
-			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN + EM_HDR_LEN);
-			if (ptcb_desc->empkt_num) {
-				RT_TRACE(COMP_SEND, DBG_TRACE,
-					 ("Insert 8 byte.pTcb->EMPktNum:%d\n",
-					  ptcb_desc->empkt_num));
-				_rtl8821ae_insert_emcontent(ptcb_desc, (u8 *)(skb->data));
-			}
-		} else {
-			SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
-		}
-
-		/* ptcb_desc->use_driver_rate = true; */
-		SET_TX_DESC_TX_RATE(pdesc, ptcb_desc->hw_rate);
-		if (ptcb_desc->hw_rate > DESC_RATEMCS0) {
-			short_gi = (ptcb_desc->use_shortgi) ? 1 : 0;
-		} else {
-			short_gi = (ptcb_desc->use_shortpreamble) ? 1 :0;
-		}
-		SET_TX_DESC_DATA_SHORTGI(pdesc, short_gi);
-
-		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
-			SET_TX_DESC_AGG_ENABLE(pdesc, 1);
-			SET_TX_DESC_MAX_AGG_NUM(pdesc, 0x14);
-		}
-		SET_TX_DESC_SEQ(pdesc, seq_number);
-		SET_TX_DESC_RTS_ENABLE(pdesc, ((ptcb_desc->b_rts_enable &&
-						!ptcb_desc->b_cts_enable) ? 1 : 0));
-		SET_TX_DESC_HW_RTS_ENABLE(pdesc,0);
-		SET_TX_DESC_CTS2SELF(pdesc, ((ptcb_desc->b_cts_enable) ? 1 : 0));
-	/*	SET_TX_DESC_RTS_STBC(pdesc, ((ptcb_desc->b_rts_stbc) ? 1 : 0));*/
-
-		SET_TX_DESC_RTS_RATE(pdesc, ptcb_desc->rts_rate);
-	/*	SET_TX_DESC_RTS_BW(pdesc, 0);*/
-		SET_TX_DESC_RTS_SC(pdesc, ptcb_desc->rts_sc);
-		SET_TX_DESC_RTS_SHORT(pdesc, ((ptcb_desc->rts_rate <= DESC_RATE54M) ?
-			(ptcb_desc->b_rts_use_shortpreamble ? 1 : 0) :
-			(ptcb_desc->b_rts_use_shortgi ? 1 : 0)));
-
-		if(ptcb_desc->btx_enable_sw_calc_duration)
-			SET_TX_DESC_NAV_USE_HDR(pdesc, 1);
-
-		if (bw_40) {
-			if (ptcb_desc->b_packet_bw) {
-				SET_TX_DESC_DATA_BW(pdesc, 1);
-				SET_TX_DESC_TX_SUB_CARRIER(pdesc, 3);
-			} else {
-				SET_TX_DESC_DATA_BW(pdesc, 0);
-				SET_TX_DESC_TX_SUB_CARRIER(pdesc, mac->cur_40_prime_sc);
-			}
-		} else {
-			SET_TX_DESC_DATA_BW(pdesc, 0);
-			SET_TX_DESC_TX_SUB_CARRIER(pdesc, 0);
-		}
-
-		SET_TX_DESC_LINIP(pdesc, 0);
-		SET_TX_DESC_PKT_SIZE(pdesc, (u16) skb_len);
-		if (sta) {
-			u8 ampdu_density = sta->ht_cap.ampdu_density;
-			SET_TX_DESC_AMPDU_DENSITY(pdesc, ampdu_density);
-		}
-		if (info->control.hw_key) {
-			struct ieee80211_key_conf *keyconf = info->control.hw_key;
-			switch (keyconf->cipher) {
-			case WLAN_CIPHER_SUITE_WEP40:
-			case WLAN_CIPHER_SUITE_WEP104:
-			case WLAN_CIPHER_SUITE_TKIP:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x1);
-				break;
-			case WLAN_CIPHER_SUITE_CCMP:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x3);
-				break;
-			default:
-				SET_TX_DESC_SEC_TYPE(pdesc, 0x0);
-				break;
-
-			}
-		}
-
-		SET_TX_DESC_QUEUE_SEL(pdesc, fw_qsel);
-		SET_TX_DESC_DATA_RATE_FB_LIMIT(pdesc, 0x1F);
-		SET_TX_DESC_RTS_RATE_FB_LIMIT(pdesc, 0xF);
-		SET_TX_DESC_DISABLE_FB(pdesc, ptcb_desc->disable_ratefallback ? 1 : 0);
-		SET_TX_DESC_USE_RATE(pdesc, ptcb_desc->use_driver_rate ? 1 : 0);
-
-#if 0
-		SET_TX_DESC_USE_RATE(pdesc, 1);
-		SET_TX_DESC_TX_RATE(pdesc, 0x04);
-
-		SET_TX_DESC_RETRY_LIMIT_ENABLE(pdesc, 1);
-		SET_TX_DESC_DATA_RETRY_LIMIT(pdesc, 0x3f);
-#endif
-
-		/*SET_TX_DESC_PWR_STATUS(pdesc, pwr_status);*/
-		/* Set TxRate and RTSRate in TxDesc  */
-		/* This prevent Tx initial rate of new-coming packets */
-		/* from being overwritten by retried  packet rate.*/
-		if (!ptcb_desc->use_driver_rate) {
-			/*SET_TX_DESC_RTS_RATE(pdesc, 0x08); */
-			/* SET_TX_DESC_TX_RATE(pdesc, 0x0b); */
-		}
-		if (ieee80211_is_data_qos(fc)) {
-			if (mac->rdg_en) {
-				RT_TRACE(COMP_SEND, DBG_TRACE,
-					("Enable RDG function.\n"));
-				SET_TX_DESC_RDG_ENABLE(pdesc, 1);
-				SET_TX_DESC_HTC(pdesc, 1);
-			}
-		}
-	}
-
-	SET_TX_DESC_FIRST_SEG(pdesc, (b_firstseg ? 1 : 0));
-	SET_TX_DESC_LAST_SEG(pdesc, (b_lastseg ? 1 : 0));
-	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) buf_len);
-	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, cpu_to_le32(mapping));
-	//if (rtlpriv->dm.b_useramask) {
-	if(1){
-		SET_TX_DESC_RATE_ID(pdesc, ptcb_desc->ratr_index);
-		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
-	} else {
-		SET_TX_DESC_RATE_ID(pdesc, 0xC + ptcb_desc->ratr_index);
-		SET_TX_DESC_MACID(pdesc, ptcb_desc->mac_id);
-	}
-/*	if (ieee80211_is_data_qos(fc))
-		SET_TX_DESC_QOS(pdesc, 1);
-*/
-	if (!ieee80211_is_data_qos(fc))  {
-		SET_TX_DESC_HWSEQ_EN(pdesc, 1);
-		SET_TX_DESC_HWSEQ_SEL(pdesc, 0);
-	}
-	SET_TX_DESC_MORE_FRAG(pdesc, (b_lastseg ? 0 : 1));
-	if (is_multicast_ether_addr(ieee80211_get_DA(hdr)) ||
-	    is_broadcast_ether_addr(ieee80211_get_DA(hdr))) {
-		SET_TX_DESC_BMC(pdesc, 1);
-	}
-
-	rtl8821ae_dm_set_tx_ant_by_tx_info(hw,pdesc,ptcb_desc->mac_id);
-	RT_TRACE(COMP_SEND, DBG_TRACE, ("\n"));
-}
-
-void rtl8821ae_tx_fill_cmddesc(struct ieee80211_hw *hw,
-			     u8 *pdesc, bool b_firstseg,
-			     bool b_lastseg, struct sk_buff *skb)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	u8 fw_queue = QSLT_BEACON;
-
-	dma_addr_t mapping = pci_map_single(rtlpci->pdev,
-					    skb->data, skb->len,
-					    PCI_DMA_TODEVICE);
-
-	if (pci_dma_mapping_error(rtlpci->pdev, mapping)) {
-		RT_TRACE(COMP_SEND, DBG_TRACE,
-			 ("DMA mapping error"));
-		return;
-	}
-	CLEAR_PCI_TX_DESC_CONTENT(pdesc, TX_DESC_SIZE);
-
-	SET_TX_DESC_FIRST_SEG(pdesc, 1);
-	SET_TX_DESC_LAST_SEG(pdesc, 1);
-
-	SET_TX_DESC_PKT_SIZE((u8 *) pdesc, (u16) (skb->len));
-
-	SET_TX_DESC_OFFSET(pdesc, USB_HWDESC_HEADER_LEN);
-
-	SET_TX_DESC_USE_RATE(pdesc, 1);
-	SET_TX_DESC_TX_RATE(pdesc, DESC_RATE1M);
-	SET_TX_DESC_DISABLE_FB(pdesc, 1);
-
-	SET_TX_DESC_DATA_BW(pdesc, 0);
-
-	SET_TX_DESC_HWSEQ_EN(pdesc, 1);
-
-	SET_TX_DESC_QUEUE_SEL(pdesc, fw_queue);
-/*
-	if(IsCtrlNDPA(VirtualAddress) || IsMgntNDPA(VirtualAddress))
-	{
-		SET_TX_DESC_DATA_RETRY_LIMIT_8812(pDesc, 5);
-		SET_TX_DESC_RETRY_LIMIT_ENABLE_8812(pDesc, 1);
-
-		if(IsMgntNDPA(VirtualAddress))
-		{
-			SET_TX_DESC_NDPA_8812(pDesc, 1);
-			SET_TX_DESC_RTS_SC_8812(pDesc, SCMapping_8812(Adapter, pTcb));
-		}
-		else
-		{
-			SET_TX_DESC_NDPA_8812(pDesc, 2);
-			SET_TX_DESC_RTS_SC_8812(pDesc, SCMapping_8812(Adapter, pTcb));
-		}
-	}*/
-
-	SET_TX_DESC_TX_BUFFER_SIZE(pdesc, (u16) (skb->len));
-
-	SET_TX_DESC_TX_BUFFER_ADDRESS(pdesc, cpu_to_le32(mapping));
-
-	SET_TX_DESC_MACID(pdesc, 0);
-
-	SET_TX_DESC_OWN(pdesc, 1);
-
-	RT_PRINT_DATA(rtlpriv, COMP_CMD, DBG_LOUD,
-		      "H2C Tx Cmd Content\n",
-		      pdesc, TX_DESC_SIZE);
-}
-
-void rtl8821ae_set_desc(struct ieee80211_hw * hw, u8 *pdesc, bool istx, u8 desc_name, u8 *val)
-{
-	if (istx == true) {
-		switch (desc_name) {
-		case HW_DESC_OWN:
-			SET_TX_DESC_OWN(pdesc, 1);
-			break;
-		case HW_DESC_TX_NEXTDESC_ADDR:
-			SET_TX_DESC_NEXT_DESC_ADDRESS(pdesc, *(u32 *) val);
-			break;
-		default:
-			RT_ASSERT(false, ("ERR txdesc :%d"
-					  " not process\n", desc_name));
-			break;
-		}
-	} else {
-		switch (desc_name) {
-		case HW_DESC_RXOWN:
-			SET_RX_DESC_OWN(pdesc, 1);
-			break;
-		case HW_DESC_RXBUFF_ADDR:
-			SET_RX_DESC_BUFF_ADDR(pdesc, *(u32 *) val);
-			break;
-		case HW_DESC_RXPKT_LEN:
-			SET_RX_DESC_PKT_LEN(pdesc, *(u32 *) val);
-			break;
-		case HW_DESC_RXERO:
-			SET_RX_DESC_EOR(pdesc, 1);
-			break;
-		default:
-			RT_ASSERT(false, ("ERR rxdesc :%d "
-					  "not process\n", desc_name));
-			break;
-		}
-	}
-}
-
-u32 rtl8821ae_get_desc(u8 *pdesc, bool istx, u8 desc_name)
-{
-	u32 ret = 0;
-
-	if (istx == true) {
-		switch (desc_name) {
-		case HW_DESC_OWN:
-			ret = GET_TX_DESC_OWN(pdesc);
-			break;
-		case HW_DESC_TXBUFF_ADDR:
-			ret = GET_TX_DESC_TX_BUFFER_ADDRESS(pdesc);
-			break;
-		default:
-			RT_ASSERT(false, ("ERR txdesc :%d "
-					  "not process\n", desc_name));
-			break;
-		}
-	} else {
-		switch (desc_name) {
-		case HW_DESC_OWN:
-			ret = GET_RX_DESC_OWN(pdesc);
-			break;
-		case HW_DESC_RXPKT_LEN:
-			ret = GET_RX_DESC_PKT_LEN(pdesc);
-			break;
-		default:
-			RT_ASSERT(false, ("ERR rxdesc :%d "
-					  "not process\n", desc_name));
-			break;
-		}
-	}
-	return ret;
-}
-
-bool rtl8821ae_is_tx_desc_closed(struct ieee80211_hw *hw,
-				 u8 hw_queue, u16 index)
-{
-	struct rtl_pci *rtlpci = rtl_pcidev(rtl_pcipriv(hw));
-	struct rtl8192_tx_ring *ring = &rtlpci->tx_ring[hw_queue];
-	u8 *entry = (u8 *)(&ring->desc[ring->idx]);
-	u8 own = (u8) rtl8821ae_get_desc(entry, true, HW_DESC_OWN);
-
-	/*
-	 *beacon packet will only use the first
-	 *descriptor defautly,and the own may not
-	 *be cleared by the hardware
-	 */
-	if (own)
-		return false;
-	else
-		return true;
-}
-
-
-void rtl8821ae_tx_polling(struct ieee80211_hw *hw, u8 hw_queue)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-
-	if (hw_queue == BEACON_QUEUE) {
-		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG, BIT(4));
-	} else {
-		rtl_write_word(rtlpriv, REG_PCIE_CTRL_REG,
-			       BIT(0) << (hw_queue));
-	}
-}
diff --git a/drivers/staging/rtl8821ae/stats.c b/drivers/staging/rtl8821ae/stats.c
deleted file mode 100644
index 4d383d11574e..000000000000
--- a/drivers/staging/rtl8821ae/stats.c
+++ /dev/null
@@ -1,281 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-#include "wifi.h"
-#include "stats.h"
-#include <linux/export.h>
-
-u8 rtl_query_rxpwrpercentage(char antpower)
-{
-	if ((antpower <= -100) || (antpower >= 20))
-		return 0;
-	else if (antpower >= 0)
-		return 100;
-	else
-		return (100 + antpower);
-}
-//EXPORT_SYMBOL(rtl_query_rxpwrpercentage);
-
-u8 rtl_evm_db_to_percentage(char value)
-{
-	char ret_val;
-	ret_val = value;
-
-	if (ret_val >= 0)
-		ret_val = 0;
-	if (ret_val <= -33)
-		ret_val = -33;
-	ret_val = 0 - ret_val;
-	ret_val *= 3;
-	if (ret_val == 99)
-		ret_val = 100;
-
-	return ret_val;
-}
-//EXPORT_SYMBOL(rtl_evm_db_to_percentage);
-
-long rtl_translate_todbm(struct ieee80211_hw *hw,
-			 u8 signal_strength_index)
-{
-	long signal_power;
-
-	signal_power = (long)((signal_strength_index + 1) >> 1);
-	signal_power -= 95;
-	return signal_power;
-}
-
-long rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig)
-{
-	long retsig;
-
-	if (currsig >= 61 && currsig <= 100)
-		retsig = 90 + ((currsig - 60) / 4);
-	else if (currsig >= 41 && currsig <= 60)
-		retsig = 78 + ((currsig - 40) / 2);
-	else if (currsig >= 31 && currsig <= 40)
-		retsig = 66 + (currsig - 30);
-	else if (currsig >= 21 && currsig <= 30)
-		retsig = 54 + (currsig - 20);
-	else if (currsig >= 5 && currsig <= 20)
-		retsig = 42 + (((currsig - 5) * 2) / 3);
-	else if (currsig == 4)
-		retsig = 36;
-	else if (currsig == 3)
-		retsig = 27;
-	else if (currsig == 2)
-		retsig = 18;
-	else if (currsig == 1)
-		retsig = 9;
-	else
-		retsig = currsig;
-
-	return retsig;
-}
-//EXPORT_SYMBOL(rtl_signal_scale_mapping);
-
-void rtl_process_ui_rssi(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_phy *rtlphy = &(rtlpriv->phy);
-	u8 rfpath;
-	u32 last_rssi, tmpval;
-
-	if (!pstatus->b_packet_toself && !pstatus->b_packet_beacon)
-		return;
-
-	rtlpriv->stats.pwdb_all_cnt += pstatus->rx_pwdb_all;
-	rtlpriv->stats.rssi_calculate_cnt++;
-
-	if (rtlpriv->stats.ui_rssi.total_num++ >= PHY_RSSI_SLID_WIN_MAX) {
-		rtlpriv->stats.ui_rssi.total_num = PHY_RSSI_SLID_WIN_MAX;
-		last_rssi = rtlpriv->stats.ui_rssi.elements[
-			rtlpriv->stats.ui_rssi.index];
-		rtlpriv->stats.ui_rssi.total_val -= last_rssi;
-	}
-	rtlpriv->stats.ui_rssi.total_val += pstatus->signalstrength;
-	rtlpriv->stats.ui_rssi.elements[rtlpriv->stats.ui_rssi.index++] =
-	    pstatus->signalstrength;
-	if (rtlpriv->stats.ui_rssi.index >= PHY_RSSI_SLID_WIN_MAX)
-		rtlpriv->stats.ui_rssi.index = 0;
-	tmpval = rtlpriv->stats.ui_rssi.total_val /
-		rtlpriv->stats.ui_rssi.total_num;
-	rtlpriv->stats.signal_strength = rtl_translate_todbm(hw,
-		(u8) tmpval);
-	pstatus->rssi = rtlpriv->stats.signal_strength;
-
-	if (pstatus->b_is_cck)
-		return;
-
-	for (rfpath = RF90_PATH_A; rfpath < rtlphy->num_total_rfpath;
-	     rfpath++) {
-		if (rtlpriv->stats.rx_rssi_percentage[rfpath] == 0) {
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    pstatus->rx_mimo_signalstrength[rfpath];
-
-		}
-		if (pstatus->rx_mimo_signalstrength[rfpath] >
-		    rtlpriv->stats.rx_rssi_percentage[rfpath]) {
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
-			      (RX_SMOOTH_FACTOR - 1)) +
-			     (pstatus->rx_mimo_signalstrength[rfpath])) /
-			    (RX_SMOOTH_FACTOR);
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    rtlpriv->stats.rx_rssi_percentage[rfpath] + 1;
-		} else {
-			rtlpriv->stats.rx_rssi_percentage[rfpath] =
-			    ((rtlpriv->stats.rx_rssi_percentage[rfpath] *
-			      (RX_SMOOTH_FACTOR - 1)) +
-			     (pstatus->rx_mimo_signalstrength[rfpath])) /
-			    (RX_SMOOTH_FACTOR);
-		}
-		rtlpriv->stats.rx_snr_db[rfpath] = pstatus->rx_snr[rfpath];
-		rtlpriv->stats.rx_evm_dbm[rfpath] =
-					pstatus->rx_mimo_evm_dbm[rfpath];
-		rtlpriv->stats.rx_cfo_short[rfpath] =
-					pstatus->cfo_short[rfpath];
-		rtlpriv->stats.rx_cfo_tail[rfpath] = pstatus->cfo_tail[rfpath];
-	}
-}
-
-static void rtl_update_rxsignalstatistics(struct ieee80211_hw *hw,
-					  struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	int weighting = 0;
-
-	if (rtlpriv->stats.recv_signal_power == 0)
-		rtlpriv->stats.recv_signal_power = pstatus->recvsignalpower;
-	if (pstatus->recvsignalpower > rtlpriv->stats.recv_signal_power)
-		weighting = 5;
-	else if (pstatus->recvsignalpower < rtlpriv->stats.recv_signal_power)
-		weighting = (-5);
-	rtlpriv->stats.recv_signal_power = (rtlpriv->stats.recv_signal_power *
-		5 + pstatus->recvsignalpower + weighting) / 6;
-}
-
-static void rtl_process_pwdb(struct ieee80211_hw *hw, struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	struct rtl_sta_info *drv_priv = NULL;
-	struct ieee80211_sta *sta = NULL;
-	long undecorated_smoothed_pwdb;
-
-	rcu_read_lock();
-	if (rtlpriv->mac80211.opmode != NL80211_IFTYPE_STATION)
-		sta = rtl_find_sta(hw, pstatus->psaddr);
-
-	/* adhoc or ap mode */
-	if (sta) {
-		drv_priv = (struct rtl_sta_info *) sta->drv_priv;
-		undecorated_smoothed_pwdb =
-			drv_priv->rssi_stat.undecorated_smoothed_pwdb;
-	} else {
-		undecorated_smoothed_pwdb =
-			rtlpriv->dm.undecorated_smoothed_pwdb;
-	}
-
-	if (undecorated_smoothed_pwdb < 0)
-		undecorated_smoothed_pwdb = pstatus->rx_pwdb_all;
-	if (pstatus->rx_pwdb_all > (u32) undecorated_smoothed_pwdb) {
-		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
-		      (RX_SMOOTH_FACTOR - 1)) +
-		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-		undecorated_smoothed_pwdb = undecorated_smoothed_pwdb + 1;
-	} else {
-		undecorated_smoothed_pwdb = (((undecorated_smoothed_pwdb) *
-		      (RX_SMOOTH_FACTOR - 1)) +
-		     (pstatus->rx_pwdb_all)) / (RX_SMOOTH_FACTOR);
-	}
-
-	if(sta) {
-		drv_priv->rssi_stat.undecorated_smoothed_pwdb =
-			undecorated_smoothed_pwdb;
-	} else {
-		rtlpriv->dm.undecorated_smoothed_pwdb = undecorated_smoothed_pwdb;
-	}
-	rcu_read_unlock();
-
-	rtl_update_rxsignalstatistics(hw, pstatus);
-}
-
-static void rtl_process_ui_link_quality(struct ieee80211_hw *hw,
-					struct rtl_stats *pstatus)
-{
-	struct rtl_priv *rtlpriv = rtl_priv(hw);
-	u32 last_evm, n_stream, tmpval;
-
-	if (pstatus->signalquality == 0)
-		return;
-
-	if (rtlpriv->stats.ui_link_quality.total_num++ >=
-	    PHY_LINKQUALITY_SLID_WIN_MAX) {
-		rtlpriv->stats.ui_link_quality.total_num =
-		    PHY_LINKQUALITY_SLID_WIN_MAX;
-		last_evm = rtlpriv->stats.ui_link_quality.elements[
-			rtlpriv->stats.ui_link_quality.index];
-		rtlpriv->stats.ui_link_quality.total_val -= last_evm;
-	}
-	rtlpriv->stats.ui_link_quality.total_val += pstatus->signalquality;
-	rtlpriv->stats.ui_link_quality.elements[
-		rtlpriv->stats.ui_link_quality.index++] =
-							pstatus->signalquality;
-	if (rtlpriv->stats.ui_link_quality.index >=
-	    PHY_LINKQUALITY_SLID_WIN_MAX)
-		rtlpriv->stats.ui_link_quality.index = 0;
-	tmpval = rtlpriv->stats.ui_link_quality.total_val /
-	    rtlpriv->stats.ui_link_quality.total_num;
-	rtlpriv->stats.signal_quality = tmpval;
-	rtlpriv->stats.last_sigstrength_inpercent = tmpval;
-	for (n_stream = 0; n_stream < 2; n_stream++) {
-		if (pstatus->rx_mimo_signalquality[n_stream] != -1) {
-			if (rtlpriv->stats.rx_evm_percentage[n_stream] == 0) {
-				rtlpriv->stats.rx_evm_percentage[n_stream] =
-				    pstatus->rx_mimo_signalquality[n_stream];
-			}
-			rtlpriv->stats.rx_evm_percentage[n_stream] =
-			    ((rtlpriv->stats.rx_evm_percentage[n_stream]
-			      * (RX_SMOOTH_FACTOR - 1)) +
-			     (pstatus->rx_mimo_signalquality[n_stream] * 1)) /
-			    (RX_SMOOTH_FACTOR);
-		}
-	}
-}
-
-void rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
-			 struct rtl_stats *pstatus)
-{
-
-	if (!pstatus->b_packet_matchbssid)
-		return;
-
-	rtl_process_ui_rssi(hw, pstatus);
-	rtl_process_pwdb(hw, pstatus);
-	rtl_process_ui_link_quality(hw, pstatus);
-}
-//EXPORT_SYMBOL(rtl_process_phyinfo);
diff --git a/drivers/staging/rtl8821ae/stats.h b/drivers/staging/rtl8821ae/stats.h
deleted file mode 100644
index d69d0cfd7e14..000000000000
--- a/drivers/staging/rtl8821ae/stats.h
+++ /dev/null
@@ -1,46 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_STATS_H__
-#define __RTL_STATS_H__
-
-#define	PHY_RSSI_SLID_WIN_MAX			100
-#define	PHY_LINKQUALITY_SLID_WIN_MAX		20
-#define	PHY_BEACON_RSSI_SLID_WIN_MAX		10
-
-/* Rx smooth factor */
-#define	RX_SMOOTH_FACTOR			20
-
-u8 rtl_query_rxpwrpercentage(char antpower);
-u8 rtl_evm_db_to_percentage(char value);
-long rtl_signal_scale_mapping(struct ieee80211_hw *hw, long currsig);
-void rtl_process_phyinfo(struct ieee80211_hw *hw, u8 *buffer,
-			 struct rtl_stats *pstatus);
-
-#endif
diff --git a/drivers/staging/rtl8821ae/wifi.h b/drivers/staging/rtl8821ae/wifi.h
deleted file mode 100644
index 218cd44fbc64..000000000000
--- a/drivers/staging/rtl8821ae/wifi.h
+++ /dev/null
@@ -1,2534 +0,0 @@
-/******************************************************************************
- *
- * Copyright(c) 2009-2010  Realtek Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms of version 2 of the GNU General Public License as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- *
- * You should have received a copy of the GNU General Public License along with
- * this program; if not, write to the Free Software Foundation, Inc.,
- * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
- *
- * The full GNU General Public License is included in this distribution in the
- * file called LICENSE.
- *
- * Contact Information:
- * wlanfae <wlanfae@realtek.com>
- * Realtek Corporation, No. 2, Innovation Road II, Hsinchu Science Park,
- * Hsinchu 300, Taiwan.
- *
- * Larry Finger <Larry.Finger@lwfinger.net>
- *
- *****************************************************************************/
-
-#ifndef __RTL_WIFI_H__
-#define __RTL_WIFI_H__
-
-#include <linux/interrupt.h>
-#include <linux/sched.h>
-#include <linux/firmware.h>
-#include <linux/version.h>
-#include <linux/etherdevice.h>
-#include <net/mac80211.h>
-#include "debug.h"
-
-
-#define RF_CHANGE_BY_INIT		0
-#define RF_CHANGE_BY_IPS		BIT(28)
-#define RF_CHANGE_BY_PS			BIT(29)
-#define RF_CHANGE_BY_HW			BIT(30)
-#define RF_CHANGE_BY_SW			BIT(31)
-
-#define IQK_ADDA_REG_NUM		16
-#define IQK_MAC_REG_NUM			4
-#define IQK_THRESHOLD			8
-
-#define MAX_KEY_LEN			61
-#define KEY_BUF_SIZE			5
-
-/* QoS related. */
-/*aci: 0x00	Best Effort*/
-/*aci: 0x01	Background*/
-/*aci: 0x10	Video*/
-/*aci: 0x11	Voice*/
-/*Max: define total number.*/
-#define AC0_BE				0
-#define AC1_BK				1
-#define AC2_VI				2
-#define AC3_VO				3
-#define AC_MAX				4
-#define QOS_QUEUE_NUM			4
-#define RTL_MAC80211_NUM_QUEUE		5
-
-#define QBSS_LOAD_SIZE			5
-#define MAX_WMMELE_LENGTH		64
-
-#define TOTAL_CAM_ENTRY			32
-
-/*slot time for 11g. */
-#define RTL_SLOT_TIME_9			9
-#define RTL_SLOT_TIME_20		20
-
-/*related with tcp/ip. */
-/*if_ether.h*/
-#define ETH_P_PAE			0x888E	/*Port Access Entity
-						 *(IEEE 802.1X) */
-#define ETH_P_IP			0x0800	/*Internet Protocol packet */
-#define ETH_P_ARP			0x0806	/*Address Resolution packet */
-#define SNAP_SIZE			6
-#define PROTOC_TYPE_SIZE		2
-
-/*related with 802.11 frame*/
-#define MAC80211_3ADDR_LEN		24
-#define MAC80211_4ADDR_LEN		30
-
-#define CHANNEL_MAX_NUMBER		(14 + 24 + 21)	/* 14 is the max
-							 * channel number */
-#define CHANNEL_MAX_NUMBER_2G		14
-#define CHANNEL_MAX_NUMBER_5G		54 /* Please refer to
-					    *"phy_GetChnlGroup8812A" and
-					    * "Hal_ReadTxPowerInfo8812A"*/
-#define CHANNEL_MAX_NUMBER_5G_80M	7
-#define CHANNEL_GROUP_MAX		(3 + 9)	/* ch1~3, ch4~9, ch10~14
-						 * total three groups */
-#define MAX_PG_GROUP			13
-#define	CHANNEL_GROUP_MAX_2G		3
-#define	CHANNEL_GROUP_IDX_5GL		3
-#define	CHANNEL_GROUP_IDX_5GM		6
-#define	CHANNEL_GROUP_IDX_5GH		9
-#define	CHANNEL_GROUP_MAX_5G		9
-#define CHANNEL_MAX_NUMBER_2G		14
-#define AVG_THERMAL_NUM			8
-#define AVG_THERMAL_NUM_92E		4
-#define AVG_THERMAL_NUM_88E		4
-#define AVG_THERMAL_NUM_8723BE		4
-#define MAX_TID_COUNT			9
-#define MAX_NUM_RATES			264
-
-/*for 88E use*/
-/*It must always set to 4, otherwise read efuse table sequence will be wrong.*/
-#define MAX_TX_COUNT			4
-#define	MAX_RF_PATH			4
-#define	MAX_CHNL_GROUP_24G		6
-#define	MAX_CHNL_GROUP_5G		14
-
-/* BK, BE, VI, VO, HCCA, MANAGEMENT, COMMAND, HIGH, BEACON. */
-#define MAX_TX_QUEUE			9
-
-#define TX_PWR_BY_RATE_NUM_BAND		2
-#define TX_PWR_BY_RATE_NUM_RF		4
-#define TX_PWR_BY_RATE_NUM_SECTION	12
-#define MAX_BASE_NUM_IN_PHY_REG_PG_24G  6
-#define MAX_BASE_NUM_IN_PHY_REG_PG_5G	5
-
-#define DELTA_SWINGIDX_SIZE		30
-#define BAND_NUM			3
-/*Now, it's just for 8192ee
- *not OK yet, keep it 0*/
-#define DMA_IS_64BIT			0
-#define RTL8192EE_SEG_NUM		1 /* 0:2 seg, 1: 4 seg, 2: 8 seg */
-
-struct txpower_info_2g {
-	u8 index_cck_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
-	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_24G];
-	/*If only one tx, only BW20 and OFDM are used.*/
-	u8 cck_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
-};
-
-struct txpower_info_5g {
-	u8 index_bw40_base[MAX_RF_PATH][MAX_CHNL_GROUP_5G];
-	/*If only one tx, only BW20, OFDM, BW80 and BW160 are used.*/
-	u8 ofdm_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw20_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw40_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw80_diff[MAX_RF_PATH][MAX_TX_COUNT];
-	u8 bw160_diff[MAX_RF_PATH][MAX_TX_COUNT];
-};
-
-
-/* for early mode */
-#define EM_HDR_LEN			8
-#define FCS_LEN				4
-
-#define MAX_VIRTUAL_MAC			1
-
-enum rf_tx_num {
-	RF_1TX = 0,
-	RF_2TX,
-	RF_MAX_TX_NUM,
-	RF_TX_NUM_NONIMPLEMENT,
-};
-
-enum rate_section {
-	CCK = 0,
-	OFDM,
-	HT_MCS0_MCS7,
-	HT_MCS8_MCS15,
-	VHT_1SSMCS0_1SSMCS9,
-	VHT_2SSMCS0_2SSMCS9,
-};
-
-enum intf_type {
-	INTF_PCI = 0,
-	INTF_USB = 1,
-};
-
-enum radio_path {
-	RF90_PATH_A = 0,
-	RF90_PATH_B = 1,
-	RF90_PATH_C = 2,
-	RF90_PATH_D = 3,
-};
-
-enum rt_eeprom_type {
-	EEPROM_93C46,
-	EEPROM_93C56,
-	EEPROM_BOOT_EFUSE,
-};
-
-enum rtl_status {
-	RTL_STATUS_INTERFACE_START = 0,
-};
-
-enum hardware_type {
-	HARDWARE_TYPE_RTL8192E,
-	HARDWARE_TYPE_RTL8192U,
-	HARDWARE_TYPE_RTL8192SE,
-	HARDWARE_TYPE_RTL8192SU,
-	HARDWARE_TYPE_RTL8192CE,
-	HARDWARE_TYPE_RTL8192CU,
-	HARDWARE_TYPE_RTL8192DE,
-	HARDWARE_TYPE_RTL8192DU,
-	HARDWARE_TYPE_RTL8723AE,
-	HARDWARE_TYPE_RTL8188EE,
-	HARDWARE_TYPE_RTL8723BE,
-	HARDWARE_TYPE_RTL8192EE,
-	HARDWARE_TYPE_RTL8821AE,
-	HARDWARE_TYPE_RTL8812AE,
-	/* keep it last */
-	HARDWARE_TYPE_NUM
-};
-
-enum scan_operation_backup_opt {
-	SCAN_OPT_BACKUP_BAND0 = 0,
-	SCAN_OPT_BACKUP_BAND1,
-	SCAN_OPT_RESTORE,
-	SCAN_OPT_MAX
-};
-
-/*RF state.*/
-enum rf_pwrstate {
-	ERFON,
-	ERFSLEEP,
-	ERFOFF
-};
-
-struct bb_reg_def {
-	u32 rfintfs;
-	u32 rfintfi;
-	u32 rfintfo;
-	u32 rfintfe;
-	u32 rf3wire_offset;
-	u32 rflssi_select;
-	u32 rftxgain_stage;
-	u32 rfhssi_para1;
-	u32 rfhssi_para2;
-	u32 rfswitch_control;
-	u32 rfagc_control1;
-	u32 rfagc_control2;
-	u32 rfrxiq_imbalance;
-	u32 rfrx_afe;
-	u32 rftxiq_imbalance;
-	u32 rftx_afe;
-	u32 rflssi_readback;
-	u32 rflssi_readbackpi;
-};
-
-enum io_type {
-	IO_CMD_PAUSE_BAND0_DM_BY_SCAN = 0,
-	IO_CMD_PAUSE_BAND1_DM_BY_SCAN = 1,
-	IO_CMD_RESUME_DM_BY_SCAN = 2,
-};
-
-enum hw_variables {
-	HW_VAR_ETHER_ADDR,
-	HW_VAR_MULTICAST_REG,
-	HW_VAR_BASIC_RATE,
-	HW_VAR_BSSID,
-	HW_VAR_MEDIA_STATUS,
-	HW_VAR_SECURITY_CONF,
-	HW_VAR_BEACON_INTERVAL,
-	HW_VAR_ATIM_WINDOW,
-	HW_VAR_LISTEN_INTERVAL,
-	HW_VAR_CS_COUNTER,
-	HW_VAR_DEFAULTKEY0,
-	HW_VAR_DEFAULTKEY1,
-	HW_VAR_DEFAULTKEY2,
-	HW_VAR_DEFAULTKEY3,
-	HW_VAR_SIFS,
-	HW_VAR_DIFS,
-	HW_VAR_EIFS,
-	HW_VAR_SLOT_TIME,
-	HW_VAR_ACK_PREAMBLE,
-	HW_VAR_CW_CONFIG,
-	HW_VAR_CW_VALUES,
-	HW_VAR_RATE_FALLBACK_CONTROL,
-	HW_VAR_CONTENTION_WINDOW,
-	HW_VAR_RETRY_COUNT,
-	HW_VAR_TR_SWITCH,
-	HW_VAR_COMMAND,
-	HW_VAR_WPA_CONFIG,
-	HW_VAR_AMPDU_MIN_SPACE,
-	HW_VAR_SHORTGI_DENSITY,
-	HW_VAR_AMPDU_FACTOR,
-	HW_VAR_MCS_RATE_AVAILABLE,
-	HW_VAR_AC_PARAM,
-	HW_VAR_ACM_CTRL,
-	HW_VAR_DIS_Req_Qsize,
-	HW_VAR_CCX_CHNL_LOAD,
-	HW_VAR_CCX_NOISE_HISTOGRAM,
-	HW_VAR_CCX_CLM_NHM,
-	HW_VAR_TxOPLimit,
-	HW_VAR_TURBO_MODE,
-	HW_VAR_RF_STATE,
-	HW_VAR_RF_OFF_BY_HW,
-	HW_VAR_BUS_SPEED,
-	HW_VAR_SET_DEV_POWER,
-
-	HW_VAR_RCR,
-	HW_VAR_RATR_0,
-	HW_VAR_RRSR,
-	HW_VAR_CPU_RST,
-	HW_VAR_CECHK_BSSID,
-	HW_VAR_LBK_MODE,
-	HW_VAR_AES_11N_FIX,
-	HW_VAR_USB_RX_AGGR,
-	HW_VAR_USER_CONTROL_TURBO_MODE,
-	HW_VAR_RETRY_LIMIT,
-	HW_VAR_INIT_TX_RATE,
-	HW_VAR_TX_RATE_REG,
-	HW_VAR_EFUSE_USAGE,
-	HW_VAR_EFUSE_BYTES,
-	HW_VAR_AUTOLOAD_STATUS,
-	HW_VAR_RF_2R_DISABLE,
-	HW_VAR_SET_RPWM,
-	HW_VAR_H2C_FW_PWRMODE,
-	HW_VAR_H2C_FW_JOINBSSRPT,
-	HW_VAR_H2C_FW_MEDIASTATUSRPT,
-	HW_VAR_H2C_FW_P2P_PS_OFFLOAD,
-	HW_VAR_FW_PSMODE_STATUS,
-	HW_VAR_RESUME_CLK_ON,
-	HW_VAR_FW_LPS_ACTION,
-	HW_VAR_1X1_RECV_COMBINE,
-	HW_VAR_STOP_SEND_BEACON,
-	HW_VAR_TSF_TIMER,
-	HW_VAR_IO_CMD,
-
-	HW_VAR_RF_RECOVERY,
-	HW_VAR_H2C_FW_UPDATE_GTK,
-	HW_VAR_WF_MASK,
-	HW_VAR_WF_CRC,
-	HW_VAR_WF_IS_MAC_ADDR,
-	HW_VAR_H2C_FW_OFFLOAD,
-	HW_VAR_RESET_WFCRC,
-
-	HW_VAR_HANDLE_FW_C2H,
-	HW_VAR_DL_FW_RSVD_PAGE,
-	HW_VAR_AID,
-	HW_VAR_HW_SEQ_ENABLE,
-	HW_VAR_CORRECT_TSF,
-	HW_VAR_BCN_VALID,
-	HW_VAR_FWLPS_RF_ON,
-	HW_VAR_DUAL_TSF_RST,
-	HW_VAR_SWITCH_EPHY_WoWLAN,
-	HW_VAR_INT_MIGRATION,
-	HW_VAR_INT_AC,
-	HW_VAR_RF_TIMING,
-
-	HAL_DEF_WOWLAN,
-	HW_VAR_MRC,
-	HW_VAR_KEEP_ALIVE,
-	HW_VAR_NAV_UPPER,
-};
-
-enum rt_media_status {
-	RT_MEDIA_DISCONNECT = 0,
-	RT_MEDIA_CONNECT = 1
-};
-
-enum rt_oem_id {
-	RT_CID_DEFAULT = 0,
-	RT_CID_8187_ALPHA0 = 1,
-	RT_CID_8187_SERCOMM_PS = 2,
-	RT_CID_8187_HW_LED = 3,
-	RT_CID_8187_NETGEAR = 4,
-	RT_CID_WHQL = 5,
-	RT_CID_819x_CAMEO = 6,
-	RT_CID_819x_RUNTOP = 7,
-	RT_CID_819x_Senao = 8,
-	RT_CID_TOSHIBA = 9,
-	RT_CID_819x_Netcore = 10,
-	RT_CID_Nettronix = 11,
-	RT_CID_DLINK = 12,
-	RT_CID_PRONET = 13,
-	RT_CID_COREGA = 14,
-	RT_CID_819x_ALPHA = 15,
-	RT_CID_819x_Sitecom = 16,
-	RT_CID_CCX = 17,
-	RT_CID_819x_Lenovo = 18,
-	RT_CID_819x_QMI = 19,
-	RT_CID_819x_Edimax_Belkin = 20,
-	RT_CID_819x_Sercomm_Belkin = 21,
-	RT_CID_819x_CAMEO1 = 22,
-	RT_CID_819x_MSI = 23,
-	RT_CID_819x_Acer = 24,
-	RT_CID_819x_HP = 27,
-	RT_CID_819x_CLEVO = 28,
-	RT_CID_819x_Arcadyan_Belkin = 29,
-	RT_CID_819x_SAMSUNG = 30,
-	RT_CID_819x_WNC_COREGA = 31,
-	RT_CID_819x_Foxcoon = 32,
-	RT_CID_819x_DELL = 33,
-	RT_CID_819x_PRONETS = 34,
-	RT_CID_819x_Edimax_ASUS = 35,
-	RT_CID_NETGEAR = 36,
-	RT_CID_PLANEX = 37,
-	RT_CID_CC_C = 38,
-};
-
-enum hw_descs {
-	HW_DESC_OWN,
-	HW_DESC_RXOWN,
-	HW_DESC_TX_NEXTDESC_ADDR,
-	HW_DESC_TXBUFF_ADDR,
-	HW_DESC_RXBUFF_ADDR,
-	HW_DESC_RXPKT_LEN,
-	HW_DESC_RXERO,
-	HW_DESC_RX_PREPARE,
-};
-
-enum prime_sc {
-	PRIME_CHNL_OFFSET_DONT_CARE = 0,
-	PRIME_CHNL_OFFSET_LOWER = 1,
-	PRIME_CHNL_OFFSET_UPPER = 2,
-};
-
-enum rf_type {
-	RF_1T1R = 0,
-	RF_1T2R = 1,
-	RF_2T2R = 2,
-	RF_2T2R_GREEN = 3,
-};
-
-enum ht_channel_width {
-	HT_CHANNEL_WIDTH_20 = 0,
-	HT_CHANNEL_WIDTH_20_40 = 1,
-	HT_CHANNEL_WIDTH_80 = 2,
-};
-
-/* Ref: 802.11i spec D10.0 7.3.2.25.1
-Cipher Suites Encryption Algorithms */
-enum rt_enc_alg {
-	NO_ENCRYPTION = 0,
-	WEP40_ENCRYPTION = 1,
-	TKIP_ENCRYPTION = 2,
-	RSERVED_ENCRYPTION = 3,
-	AESCCMP_ENCRYPTION = 4,
-	WEP104_ENCRYPTION = 5,
-	AESCMAC_ENCRYPTION = 6,	/*IEEE802.11w */
-};
-
-enum rtl_hal_state {
-	_HAL_STATE_STOP = 0,
-	_HAL_STATE_START = 1,
-};
-
-enum rtl_var_map {
-	/*reg map */
-	SYS_ISO_CTRL = 0,
-	SYS_FUNC_EN,
-	SYS_CLK,
-	MAC_RCR_AM,
-	MAC_RCR_AB,
-	MAC_RCR_ACRC32,
-	MAC_RCR_ACF,
-	MAC_RCR_AAP,
-	MAC_HIMR,
-	MAC_HIMRE,
-	MAC_HSISR,
-
-	/*efuse map */
-	EFUSE_TEST,
-	EFUSE_CTRL,
-	EFUSE_CLK,
-	EFUSE_CLK_CTRL,
-	EFUSE_PWC_EV12V,
-	EFUSE_FEN_ELDR,
-	EFUSE_LOADER_CLK_EN,
-	EFUSE_ANA8M,
-	EFUSE_HWSET_MAX_SIZE,
-	EFUSE_MAX_SECTION_MAP,
-	EFUSE_REAL_CONTENT_SIZE,
-	EFUSE_OOB_PROTECT_BYTES_LEN,
-	EFUSE_ACCESS,
-	/*CAM map */
-	RWCAM,
-	WCAMI,
-	RCAMO,
-	CAMDBG,
-	SECR,
-	SEC_CAM_NONE,
-	SEC_CAM_WEP40,
-	SEC_CAM_TKIP,
-	SEC_CAM_AES,
-	SEC_CAM_WEP104,
-
-	/*IMR map */
-	RTL_IMR_BCNDMAINT6,	/*Beacon DMA Interrupt 6 */
-	RTL_IMR_BCNDMAINT5,	/*Beacon DMA Interrupt 5 */
-	RTL_IMR_BCNDMAINT4,	/*Beacon DMA Interrupt 4 */
-	RTL_IMR_BCNDMAINT3,	/*Beacon DMA Interrupt 3 */
-	RTL_IMR_BCNDMAINT2,	/*Beacon DMA Interrupt 2 */
-	RTL_IMR_BCNDMAINT1,	/*Beacon DMA Interrupt 1 */
-	RTL_IMR_BCNDOK8,	/*Beacon Queue DMA OK Interrupt 8 */
-	RTL_IMR_BCNDOK7,	/*Beacon Queue DMA OK Interrupt 7 */
-	RTL_IMR_BCNDOK6,	/*Beacon Queue DMA OK Interrupt 6 */
-	RTL_IMR_BCNDOK5,	/*Beacon Queue DMA OK Interrupt 5 */
-	RTL_IMR_BCNDOK4,	/*Beacon Queue DMA OK Interrupt 4 */
-	RTL_IMR_BCNDOK3,	/*Beacon Queue DMA OK Interrupt 3 */
-	RTL_IMR_BCNDOK2,	/*Beacon Queue DMA OK Interrupt 2 */
-	RTL_IMR_BCNDOK1,	/*Beacon Queue DMA OK Interrupt 1 */
-	RTL_IMR_TIMEOUT2,	/*Timeout interrupt 2 */
-	RTL_IMR_TIMEOUT1,	/*Timeout interrupt 1 */
-	RTL_IMR_TXFOVW,		/*Transmit FIFO Overflow */
-	RTL_IMR_PSTIMEOUT,	/*Power save time out interrupt */
-	RTL_IMR_BcnInt,		/*Beacon DMA Interrupt 0 */
-	RTL_IMR_RXFOVW,		/*Receive FIFO Overflow */
-	RTL_IMR_RDU,		/*Receive Descriptor Unavailable */
-	RTL_IMR_ATIMEND,	/*For 92C,ATIM Window End Interrupt */
-	RTL_IMR_BDOK,		/*Beacon Queue DMA OK Interrupt */
-	RTL_IMR_HIGHDOK,	/*High Queue DMA OK Interrupt */
-	RTL_IMR_COMDOK,		/*Command Queue DMA OK Interrupt*/
-	RTL_IMR_TBDOK,		/*Transmit Beacon OK interrupt */
-	RTL_IMR_MGNTDOK,	/*Management Queue DMA OK Interrupt */
-	RTL_IMR_TBDER,		/*For 92C,Transmit Beacon Error Interrupt */
-	RTL_IMR_BKDOK,		/*AC_BK DMA OK Interrupt */
-	RTL_IMR_BEDOK,		/*AC_BE DMA OK Interrupt */
-	RTL_IMR_VIDOK,		/*AC_VI DMA OK Interrupt */
-	RTL_IMR_VODOK,		/*AC_VO DMA Interrupt */
-	RTL_IMR_ROK,		/*Receive DMA OK Interrupt */
-	RTL_IMR_HSISR_IND,  /*HSISR Interrupt*/
-	RTL_IBSS_INT_MASKS,	/*(RTL_IMR_BcnInt | RTL_IMR_TBDOK |
-				 * RTL_IMR_TBDER) */
-	RTL_IMR_C2HCMD,		/*fw interrupt*/
-
-	/*CCK Rates, TxHT = 0 */
-	RTL_RC_CCK_RATE1M,
-	RTL_RC_CCK_RATE2M,
-	RTL_RC_CCK_RATE5_5M,
-	RTL_RC_CCK_RATE11M,
-
-	/*OFDM Rates, TxHT = 0 */
-	RTL_RC_OFDM_RATE6M,
-	RTL_RC_OFDM_RATE9M,
-	RTL_RC_OFDM_RATE12M,
-	RTL_RC_OFDM_RATE18M,
-	RTL_RC_OFDM_RATE24M,
-	RTL_RC_OFDM_RATE36M,
-	RTL_RC_OFDM_RATE48M,
-	RTL_RC_OFDM_RATE54M,
-
-	RTL_RC_HT_RATEMCS7,
-	RTL_RC_HT_RATEMCS15,
-
-	/*keep it last */
-	RTL_VAR_MAP_MAX,
-};
-
-/*Firmware PS mode for control LPS.*/
-enum _fw_ps_mode {
-	FW_PS_ACTIVE_MODE = 0,
-	FW_PS_MIN_MODE = 1,
-	FW_PS_MAX_MODE = 2,
-	FW_PS_DTIM_MODE = 3,
-	FW_PS_VOIP_MODE = 4,
-	FW_PS_UAPSD_WMM_MODE = 5,
-	FW_PS_UAPSD_MODE = 6,
-	FW_PS_IBSS_MODE = 7,
-	FW_PS_WWLAN_MODE = 8,
-	FW_PS_PM_Radio_Off = 9,
-	FW_PS_PM_Card_Disable = 10,
-};
-
-enum rt_psmode {
-	EACTIVE,		/*Active/Continuous access. */
-	EMAXPS,			/*Max power save mode. */
-	EFASTPS,		/*Fast power save mode. */
-	EAUTOPS,		/*Auto power save mode. */
-};
-
-/*LED related.*/
-enum led_ctl_mode {
-	LED_CTL_POWER_ON = 1,
-	LED_CTL_LINK = 2,
-	LED_CTL_NO_LINK = 3,
-	LED_CTL_TX = 4,
-	LED_CTL_RX = 5,
-	LED_CTL_SITE_SURVEY = 6,
-	LED_CTL_POWER_OFF = 7,
-	LED_CTL_START_TO_LINK = 8,
-	LED_CTL_START_WPS = 9,
-	LED_CTL_STOP_WPS = 10,
-};
-
-enum rtl_led_pin {
-	LED_PIN_GPIO0,
-	LED_PIN_LED0,
-	LED_PIN_LED1,
-	LED_PIN_LED2
-};
-
-/*QoS related.*/
-/*acm implementation method.*/
-enum acm_method {
-	eAcmWay0_SwAndHw = 0,
-	eAcmWay1_HW = 1,
-	eAcmWay2_SW = 2,
-};
-
-enum macphy_mode {
-	SINGLEMAC_SINGLEPHY = 0,
-	DUALMAC_DUALPHY,
-	DUALMAC_SINGLEPHY,
-};
-
-enum band_type {
-	BAND_ON_2_4G = 0,
-	BAND_ON_5G,
-	BAND_ON_BOTH,
-	BANDMAX
-};
-
-/*aci/aifsn Field.
-Ref: WMM spec 2.2.2: WME Parameter Element, p.12.*/
-union aci_aifsn {
-	u8 char_data;
-
-	struct {
-		u8 aifsn:4;
-		u8 acm:1;
-		u8 aci:2;
-		u8 reserved:1;
-	} f;			/* Field */
-};
-
-/*mlme related.*/
-enum wireless_mode {
-	WIRELESS_MODE_UNKNOWN = 0x00,
-	WIRELESS_MODE_A = 0x01,
-	WIRELESS_MODE_B = 0x02,
-	WIRELESS_MODE_G = 0x04,
-	WIRELESS_MODE_AUTO = 0x08,
-	WIRELESS_MODE_N_24G = 0x10,
-	WIRELESS_MODE_N_5G = 0x20,
-	WIRELESS_MODE_AC_5G = 0x40,
-	WIRELESS_MODE_AC_24G  = 0x80
-};
-
-enum ratr_table_mode {
-	RATR_INX_WIRELESS_NGB = 0,		/* BGN 40 Mhz 2SS 1SS */
-	RATR_INX_WIRELESS_NG = 1,		/* GN or N */
-	RATR_INX_WIRELESS_NB = 2,		/* BGN 20 Mhz 2SS 1SS  or BN */
-	RATR_INX_WIRELESS_N = 3,
-	RATR_INX_WIRELESS_GB = 4,
-	RATR_INX_WIRELESS_G = 5,
-	RATR_INX_WIRELESS_B = 6,
-	RATR_INX_WIRELESS_MC = 7,
-	RATR_INX_WIRELESS_AC_5N = 8,
-	RATR_INX_WIRELESS_AC_24N = 9,
-};
-
-enum rtl_link_state {
-	MAC80211_NOLINK = 0,
-	MAC80211_LINKING = 1,
-	MAC80211_LINKED = 2,
-	MAC80211_LINKED_SCANNING = 3,
-};
-
-enum act_category {
-	ACT_CAT_QOS = 1,
-	ACT_CAT_DLS = 2,
-	ACT_CAT_BA = 3,
-	ACT_CAT_HT = 7,
-	ACT_CAT_WMM = 17,
-};
-
-enum ba_action {
-	ACT_ADDBAREQ = 0,
-	ACT_ADDBARSP = 1,
-	ACT_DELBA = 2,
-};
-
-enum rt_polarity_ctl {
-	RT_POLARITY_LOW_ACT = 0,
-	RT_POLARITY_HIGH_ACT = 1,
-};
-
-
-struct octet_string {
-	u8 *octet;
-	u16 length;
-};
-
-struct rtl_hdr_3addr {
-	__le16 frame_ctl;
-	__le16 duration_id;
-	u8 addr1[ETH_ALEN];
-	u8 addr2[ETH_ALEN];
-	u8 addr3[ETH_ALEN];
-	__le16 seq_ctl;
-	u8 payload[0];
-} __packed;
-
-struct rtl_info_element {
-	u8 id;
-	u8 len;
-	u8 data[0];
-} __packed;
-
-struct rtl_probe_rsp {
-	struct rtl_hdr_3addr header;
-	u32 time_stamp[2];
-	__le16 beacon_interval;
-	__le16 capability;
-	/*SSID, supported rates, FH params, DS params,
-	   CF params, IBSS params, TIM (if beacon), RSN */
-	struct rtl_info_element info_element[0];
-} __packed;
-
-/*LED related.*/
-/*ledpin Identify how to implement this SW led.*/
-struct rtl_led {
-	void *hw;
-	enum rtl_led_pin ledpin;
-	bool b_ledon;
-};
-
-struct rtl_led_ctl {
-	bool bled_opendrain;
-	struct rtl_led sw_led0;
-	struct rtl_led sw_led1;
-};
-
-struct rtl_qos_parameters {
-	__le16 cw_min;
-	__le16 cw_max;
-	u8 aifs;
-	u8 flag;
-	__le16 tx_op;
-} __packed;
-
-struct rt_smooth_data {
-	u32 elements[100];	/*array to store values */
-	u32 index;		/*index to current array to store */
-	u32 total_num;		/*num of valid elements */
-	u32 total_val;		/*sum of valid elements */
-};
-
-struct rtl_ht_agg {
-	u16 txq_id;
-	u16 wait_for_ba;
-	u16 start_idx;
-	u64 bitmap;
-	u32 rate_n_flags;
-	u8 agg_state;
-	u8 rx_agg_state;
-};
-
-struct rtl_tid_data {
-	u16 seq_number;
-	struct rtl_ht_agg agg;
-};
-
-struct rssi_sta {
-	long undecorated_smoothed_pwdb;
-};
-
-struct rtl_sta_info {
-	struct list_head list;
-	u8 ratr_index;
-	u8 wireless_mode;
-	u8 mimo_ps;
-	u8 mac_addr[6];
-	struct rtl_tid_data tids[MAX_TID_COUNT];
-
-	/* just used for ap adhoc or mesh*/
-	struct rssi_sta rssi_stat;
-} __packed;
-
-#ifdef VIF_TODO
-struct rtl_vif {
-	unsigned int id;
-	/* struct ieee80211_vif __rcu *vif; */
-	struct ieee80211_vif *vif;
-};
-
-struct rtl_vif_info {
-	struct list_head list;
-	bool active;
-	unsigned int id;
-	struct sk_buff *beacon;
-	bool enable_beacon;
-};
-
-struct vif_priv {
-	struct list_head vif_list;
-
-	/* interface mode settings */
-	unsigned long vif_bitmap;
-	unsigned int vifs;
-	struct rtl_vif vif[MAX_VIRTUAL_MAC];
-
-	/* beaconing */
-	spinlock_t beacon_lock;
-	unsigned int global_pretbtt;
-	unsigned int global_beacon_int;
-	/* struct rtl_vif_info __rcu *beacon_iter; */
-	struct rtl_vif_info *beacon_iter;
-	unsigned int beacon_enabled;
-};
-#endif
-
-struct false_alarm_statistics {
-	u32 cnt_parity_fail;
-	u32 cnt_rate_illegal;
-	u32 cnt_crc8_fail;
-	u32 cnt_mcs_fail;
-	u32 cnt_fast_fsync_fail;
-	u32 cnt_sb_search_fail;
-	u32 cnt_ofdm_fail;
-	u32 cnt_cck_fail;
-	u32 cnt_all;
-	u32 cnt_ofdm_cca;
-	u32 cnt_cck_cca;
-	u32 cnt_cca_all;
-	u32 cnt_bw_usc;
-	u32 cnt_bw_lsc;
-};
-
-struct init_gain {
-	u8 xaagccore1;
-	u8 xbagccore1;
-	u8 xcagccore1;
-	u8 xdagccore1;
-	u8 cca;
-
-};
-
-struct wireless_stats {
-	unsigned long txbytesunicast;
-	unsigned long txbytesmulticast;
-	unsigned long txbytesbroadcast;
-	unsigned long rxbytesunicast;
-
-	long rx_snr_db[4];
-	/*Correct smoothed ss in Dbm, only used
-	   in driver to report real power now. */
-	long recv_signal_power;
-	long signal_quality;
-	long last_sigstrength_inpercent;
-
-	u32 rssi_calculate_cnt;
-	u32 pwdb_all_cnt;
-
-	/*Transformed, in dbm. Beautified signal
-	   strength for UI, not correct. */
-	long signal_strength;
-
-	u8 rx_rssi_percentage[4];
-	u8 rx_evm_dbm[4];
-	u8 rx_evm_percentage[2];
-
-	u16 rx_cfo_short[4];
-	u16 rx_cfo_tail[4];
-
-	struct rt_smooth_data ui_rssi;
-	struct rt_smooth_data ui_link_quality;
-};
-
-struct rate_adaptive {
-	u8 rate_adaptive_disabled;
-	u8 ratr_state;
-	u16 reserve;
-
-	u32 high_rssi_thresh_for_ra;
-	u32 high2low_rssi_thresh_for_ra;
-	u8 low2high_rssi_thresh_for_ra;
-	u32 low_rssi_thresh_for_ra;
-	u32 upper_rssi_threshold_ratr;
-	u32 middleupper_rssi_threshold_ratr;
-	u32 middle_rssi_threshold_ratr;
-	u32 middlelow_rssi_threshold_ratr;
-	u32 low_rssi_threshold_ratr;
-	u32 ultralow_rssi_threshold_ratr;
-	u32 low_rssi_threshold_ratr_40m;
-	u32 low_rssi_threshold_ratr_20m;
-	u8 ping_rssi_enable;
-	u32 ping_rssi_ratr;
-	u32 ping_rssi_thresh_for_ra;
-	u32 last_ratr;
-	u8 pre_ratr_state;
-	u8 ldpc_thres;
-	bool use_ldpc;
-	bool lower_rts_rate;
-	bool is_special_data;
-};
-
-struct regd_pair_mapping {
-	u16 reg_dmnenum;
-	u16 reg_5ghz_ctl;
-	u16 reg_2ghz_ctl;
-};
-
-struct dynamic_primary_cca {
-	u8 pricca_flag;
-	u8 intf_flag;
-	u8 intf_type;
-	u8 dup_rts_flag;
-	u8 monitor_flag;
-	u8 ch_offset;
-	u8 mf_state;
-};
-
-struct rtl_regulatory {
-	char alpha2[2];
-	u16 country_code;
-	u16 max_power_level;
-	u32 tp_scale;
-	u16 current_rd;
-	u16 current_rd_ext;
-	int16_t power_limit;
-	struct regd_pair_mapping *regpair;
-};
-
-struct rtl_rfkill {
-	bool rfkill_state;	/*0 is off, 1 is on */
-};
-
-/*for P2P PS**/
-#define	P2P_MAX_NOA_NUM		2
-
-enum p2p_role {
-	P2P_ROLE_DISABLE = 0,
-	P2P_ROLE_DEVICE = 1,
-	P2P_ROLE_CLIENT = 2,
-	P2P_ROLE_GO = 3
-};
-
-enum p2p_ps_state {
-	P2P_PS_DISABLE = 0,
-	P2P_PS_ENABLE = 1,
-	P2P_PS_SCAN = 2,
-	P2P_PS_SCAN_DONE = 3,
-	P2P_PS_ALLSTASLEEP = 4, /* for P2P GO */
-};
-
-enum p2p_ps_mode {
-	P2P_PS_NONE = 0,
-	P2P_PS_CTWINDOW = 1,
-	P2P_PS_NOA = 2,
-	P2P_PS_MIX = 3, /* CTWindow and NoA */
-};
-
-struct rtl_p2p_ps_info {
-	enum p2p_ps_mode p2p_ps_mode; /* indicate p2p ps mode */
-	enum p2p_ps_state p2p_ps_state; /* indicate p2p ps state */
-	u8 noa_index; /* Identifies and instance of Notice of Absence timing. */
-	/* Client traffic window. A period of time in TU after TBTT. */
-	u8 ctwindow;
-	u8 opp_ps; /* opportunistic power save. */
-	u8 noa_num; /* number of NoA descriptor in P2P IE. */
-	/* Count for owner, Type of client. */
-	u8 noa_count_type[P2P_MAX_NOA_NUM];
-	/* Max duration for owner, preferred or
-	 * min acceptable duration for client. */
-	u32 noa_duration[P2P_MAX_NOA_NUM];
-	/* Length of interval for owner, preferred or
-	 * max acceptable interval of client. */
-	u32 noa_interval[P2P_MAX_NOA_NUM];
-	/* schedule expressed in terms of the lower 4 bytes of the TSF timer. */
-	u32 noa_start_time[P2P_MAX_NOA_NUM];
-};
-
-struct p2p_ps_offload_t {
-	u8 Offload_En:1;
-	u8 role:1; /* 1: Owner, 0: Client */
-	u8 CTWindow_En:1;
-	u8 NoA0_En:1;
-	u8 NoA1_En:1;
-	u8 AllStaSleep:1;
-	u8 discovery:1;
-	u8 reserved:1;
-};
-
-#define IQK_MATRIX_REG_NUM	8
-#define IQK_MATRIX_SETTINGS_NUM	 (14+24+21) /* Channels_2_4G_NUM + Channels_5G_20M_NUM + Channels_5G */
-struct iqk_matrix_regs {
-	bool b_iqk_done;
-	long value[1][IQK_MATRIX_REG_NUM];
-};
-
-struct rtl_phy {
-	struct bb_reg_def phyreg_def[4];	/*Radio A/B/C/D */
-	struct init_gain initgain_backup;
-	enum io_type current_io_type;
-
-	u8 rf_mode;
-	u8 rf_type;
-	u8 current_chan_bw;
-	u8 set_bwmode_inprogress;
-	u8 sw_chnl_inprogress;
-	u8 sw_chnl_stage;
-	u8 sw_chnl_step;
-	u8 current_channel;
-	u8 h2c_box_num;
-	u8 set_io_inprogress;
-	u8 lck_inprogress;
-
-	/* record for power tracking */
-	s32 reg_e94;
-	s32 reg_e9c;
-	s32 reg_ea4;
-	s32 reg_eac;
-	s32 reg_eb4;
-	s32 reg_ebc;
-	s32 reg_ec4;
-	s32 reg_ecc;
-	u8 rfpienable;
-	u8 reserve_0;
-	u16 reserve_1;
-	u32 reg_c04, reg_c08, reg_874;
-	u32 adda_backup[16];
-	u32 iqk_mac_backup[IQK_MAC_REG_NUM];
-	u32 iqk_bb_backup[10];
-	bool iqk_initialized;
-
-	bool rfpath_rx_enable[MAX_RF_PATH];
-	/*Jaguar*/
-	u8 reg_837;
-	/* Dul mac */
-	bool b_need_iqk;
-	struct iqk_matrix_regs iqk_matrix_regsetting[IQK_MATRIX_SETTINGS_NUM];
-
-	bool b_rfpi_enable;
-
-	bool b_iqk_in_progress;
-
-	u8 pwrgroup_cnt;
-	u8 bcck_high_power;
-	/* this is for 88E & 8723A */
-	u32 mcs_txpwrlevel_origoffset[MAX_PG_GROUP][16];
-	/* this is for 92EE */
-	u32 tx_power_by_rate_offset[TX_PWR_BY_RATE_NUM_BAND]
-				   [TX_PWR_BY_RATE_NUM_RF]
-				   [TX_PWR_BY_RATE_NUM_RF]
-				   [TX_PWR_BY_RATE_NUM_SECTION];
-	u8 txpwr_by_rate_base_24g[TX_PWR_BY_RATE_NUM_RF]
-				 [TX_PWR_BY_RATE_NUM_RF]
-				 [MAX_BASE_NUM_IN_PHY_REG_PG_24G];
-
-	u8 txpwr_by_rate_base_5g[TX_PWR_BY_RATE_NUM_RF]
-				[TX_PWR_BY_RATE_NUM_RF]
-				[MAX_BASE_NUM_IN_PHY_REG_PG_5G];
-	u8 default_initialgain[4];
-
-	/* the current Tx power level */
-	u8 cur_cck_txpwridx;
-	u8 cur_ofdm24g_txpwridx;
-	u8 cur_bw20_txpwridx;
-	u8 cur_bw40_txpwridx;
-
-	u32 rfreg_chnlval[2];
-	bool b_apk_done;
-	u32 reg_rf3c[2];	/* pathA / pathB  */
-
-	u32 backup_rf_0x1a;/*92ee*/
-	/* bfsync */
-	u8 framesync;
-	u32 framesync_c34;
-
-	u8 num_total_rfpath;
-	u16 rf_pathmap;
-
-	u8 hw_rof_enable; /*Enable GPIO[9] as WL RF HW PDn source*/
-
-	enum rt_polarity_ctl polarity_ctl;
-};
-
-#define RTL_AGG_STOP						0
-#define RTL_AGG_PROGRESS					1
-#define RTL_AGG_START						2
-#define RTL_AGG_OPERATIONAL					3
-#define RTL_RX_AGG_START					1
-#define RTL_RX_AGG_STOP						0
-
-struct rtl_priv;
-struct rtl_io {
-	struct device *dev;
-
-	/*PCI MEM map */
-	void __iomem *pci_mem_end;	/*shared mem end        */
-	void __iomem *pci_mem_start;	/*shared mem start */
-
-	/*PCI IO map */
-	unsigned long pci_base_addr;	/*device I/O address */
-
-	void (*write8_async)(struct rtl_priv *rtlpriv, u32 addr, u8 val);
-	void (*write16_async)(struct rtl_priv *rtlpriv, u32 addr, u16 val);
-	void (*write32_async)(struct rtl_priv *rtlpriv, u32 addr, u32 val);
-
-	u8 (*read8_sync)(struct rtl_priv *rtlpriv, u32 addr);
-	u16 (*read16_sync)(struct rtl_priv *rtlpriv, u32 addr);
-	u32 (*read32_sync)(struct rtl_priv *rtlpriv, u32 addr);
-
-};
-
-struct rtl_mac {
-	u8 mac_addr[ETH_ALEN];
-	u8 mac80211_registered;
-	u8 beacon_enabled;
-
-	u32 tx_ss_num;
-	u32 rx_ss_num;
-
-	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
-	struct ieee80211_hw *hw;
-	struct ieee80211_vif *vif;
-	enum nl80211_iftype opmode;
-
-	/*Probe Beacon management */
-	enum rtl_link_state link_state;
-
-	int n_channels;
-	int n_bitrates;
-
-	bool offchan_deley;
-	u8 p2p;	/*using p2p role*/
-	bool p2p_in_use;
-
-	/*filters */
-	u32 rx_conf;
-
-	bool act_scanning;
-	u8 cnt_after_linked;
-	bool skip_scan;
-
-	/* early mode */
-	/* skb wait queue */
-	struct sk_buff_head skb_waitq[MAX_TID_COUNT];
-
-	/*RDG*/
-	bool rdg_en;
-
-	/*AP*/
-	u8 bssid[6];
-	u32 vendor;
-	u32 basic_rates; /* b/g rates */
-	u8 ht_enable;
-	u8 bw_40;
-	u8 mode;		/* wireless mode */
-	u8 slot_time;
-	u8 short_preamble;
-	u8 use_cts_protect;
-	u8 cur_40_prime_sc;
-	u8 cur_40_prime_sc_bk;
-	u8 cur_80_prime_sc;
-	u64 tsf;
-	u8 retry_short;
-	u8 retry_long;
-	u16 assoc_id;
-	bool bhiddenssid;
-
-	/*IBSS*/
-	int beacon_interval;
-
-	/*AMPDU*/
-	u8 min_space_cfg;	/*For Min spacing configurations */
-	u8 max_mss_density;
-	u8 current_ampdu_factor;
-	u8 current_ampdu_density;
-
-	/*QOS & EDCA */
-	struct ieee80211_tx_queue_params edca_param[RTL_MAC80211_NUM_QUEUE];
-	struct rtl_qos_parameters ac[AC_MAX];
-};
-
-struct rtl_hal {
-	struct ieee80211_hw *hw;
-
-	bool driver_is_goingto_unload;
-	bool up_first_time;
-	bool bfirst_init;
-	bool being_init_adapter;
-	bool b_bbrf_ready;
-	bool b_mac_func_enable;
-	bool b_pre_edcca_enable;
-
-	enum intf_type interface;
-	u16 hw_type;		/*92c or 92d or 92s and so on */
-	u8 ic_class;
-	u8 oem_id;
-	u32 version;		/*version of chip */
-	u8 state;		/*stop 0, start 1 */
-	u8 boad_type;
-
-	/*firmware */
-	u32 fwsize;
-	u8 *pfirmware;
-	u16 fw_version;
-	u16 fw_subversion;
-	bool b_h2c_setinprogress;
-	u8 last_hmeboxnum;
-	bool bfw_ready;
-
-	/*Reserve page start offset except beacon in TxQ. */
-	u8 fw_rsvdpage_startoffset;
-	u8 h2c_txcmd_seq;
-	u8 current_ra_rate;
-
-	/* FW Cmd IO related */
-	u16 fwcmd_iomap;
-	u32 fwcmd_ioparam;
-	bool set_fwcmd_inprogress;
-	u8 current_fwcmd_io;
-
-	bool bfw_clk_change_in_progress;
-	bool ballow_sw_to_change_hwclc;
-	u8 fw_ps_state;
-	 struct p2p_ps_offload_t p2p_ps_offload;
-	/**/
-	bool driver_going2unload;
-
-	/*AMPDU init min space*/
-	u8 minspace_cfg;	/*For Min spacing configurations */
-
-	/* Dul mac */
-	enum macphy_mode macphymode;
-	enum band_type current_bandtype;	/* 0:2.4G, 1:5G */
-	enum band_type current_bandtypebackup;
-	enum band_type bandset;
-	/* dual MAC 0--Mac0 1--Mac1 */
-	u32 interfaceindex;
-	/* just for DulMac S3S4 */
-	u8 macphyctl_reg;
-	bool b_earlymode_enable;
-	u8 max_earlymode_num;
-	/* Dul mac*/
-	bool during_mac0init_radiob;
-	bool during_mac1init_radioa;
-	bool reloadtxpowerindex;
-	/* True if IMR or IQK  have done
-	for 2.4G in scan progress */
-	bool b_load_imrandiqk_setting_for2g;
-
-	bool disable_amsdu_8k;
-	bool bmaster_of_dmsp;
-	bool bslave_of_dmsp;
-
-	u16 rx_tag;/*for 92ee*/
-	u8 rts_en;
-};
-
-struct rtl_security {
-	/*default 0 */
-	bool use_sw_sec;
-
-	bool being_setkey;
-	bool use_defaultkey;
-	/*Encryption Algorithm for Unicast Packet */
-	enum rt_enc_alg pairwise_enc_algorithm;
-	/*Encryption Algorithm for Broadcast/Multicast */
-	enum rt_enc_alg group_enc_algorithm;
-	/*Cam Entry Bitmap */
-	u32 hwsec_cam_bitmap;
-	u8 hwsec_cam_sta_addr[TOTAL_CAM_ENTRY][ETH_ALEN];
-	/*local Key buffer, indx 0 is for
-	   pairwise key 1-4 is for agoup key. */
-	u8 key_buf[KEY_BUF_SIZE][MAX_KEY_LEN];
-	u8 key_len[KEY_BUF_SIZE];
-
-	/*The pointer of Pairwise Key,
-	   it always points to KeyBuf[4] */
-	u8 *pairwise_key;
-};
-
-struct rtl_dig {
-	u8 dig_enable_flag;
-	u8 dig_ext_port_stage;
-
-	u32 rssi_lowthresh;
-	u32 rssi_highthresh;
-
-	u32 fa_lowthresh;
-	u32 fa_highthresh;
-
-	u8 cursta_connectstate;
-	u8 presta_connectstate;
-	u8 curmultista_connectstate;
-
-	u8 pre_igvalue;
-	u8 cur_igvalue;
-
-	char backoff_val;
-	char backoff_val_range_max;
-	char backoff_val_range_min;
-	u8 rx_gain_range_max;
-	u8 rx_gain_range_min;
-	u8 rssi_val_min;
-	u8 min_undecorated_pwdb_for_dm;
-	long last_min_undecorated_pwdb_for_dm;
-
-	u8 pre_cck_pd_state;
-	u8 cur_cck_pd_state;
-
-	u8 large_fa_hit;
-	u8 forbidden_igi;
-	u32 recover_cnt;
-
-};
-
-struct rtl_pstbl {
-	u8 pre_ccastate;
-	u8 cur_ccasate;
-
-	u8 pre_rfstate;
-	u8 cur_rfstate;
-
-	long rssi_val_min;
-
-};
-
-#define ASSOCIATE_ENTRY_NUM	(32+1)
-
-struct fast_ant_trainning {
-	u8 bssid[6];
-	u8 antsel_rx_keep_0;
-	u8 antsel_rx_keep_1;
-	u8 antsel_rx_keep_2;
-	u32 ant_sum_rssi[7];
-	u32 ant_rssi_cnt[7];
-	u32 ant_ave_rssi[7];
-	u8 fat_state;
-	u32 train_idx;
-	u8 antsel_a[ASSOCIATE_ENTRY_NUM];
-	u8 antsel_b[ASSOCIATE_ENTRY_NUM];
-	u8 antsel_c[ASSOCIATE_ENTRY_NUM];
-	u32 main_ant_sum[ASSOCIATE_ENTRY_NUM];
-	u32 aux_ant_sum[ASSOCIATE_ENTRY_NUM];
-	u32 main_ant_cnt[ASSOCIATE_ENTRY_NUM];
-	u32 aux_ant_cnt[ASSOCIATE_ENTRY_NUM];
-	u8 rx_idle_ant;
-	bool b_becomelinked;
-};
-
-struct dm_phy_dbg_info {
-	char rx_snrdb[4];
-	u64 num_qry_phy_status;
-	u64 num_qry_phy_status_cck;
-	u64 num_qry_phy_status_ofdm;
-	u16 num_qry_beacon_pkt;
-	u16 num_non_be_pkt;
-	s32 rx_evm[4];
-};
-
-struct rtl_dm {
-	/*PHY status for DM */
-	long entry_min_undecoratedsmoothed_pwdb;
-	long undecorated_smoothed_pwdb;	/*out dm */
-	long entry_max_undecoratedsmoothed_pwdb;
-	bool b_dm_initialgain_enable;
-	bool bdynamic_txpower_enable;
-	bool bcurrent_turbo_edca;
-	bool bis_any_nonbepkts;	/*out dm */
-	bool bis_cur_rdlstate;
-	bool btxpower_trackinginit;
-	bool b_disable_framebursting;
-	bool b_cck_inch14;
-	bool btxpower_tracking;
-	bool b_useramask;
-	bool brfpath_rxenable[4];
-	bool binform_fw_driverctrldm;
-	bool bcurrent_mrc_switch;
-	u8 txpowercount;
-
-	u8 thermalvalue_rxgain;
-	u8 thermalvalue_iqk;
-	u8 thermalvalue_lck;
-	u8 thermalvalue;
-	u8 thermalvalue_avg[AVG_THERMAL_NUM];
-	u8 thermalvalue_avg_index;
-	bool bdone_txpower;
-	u8 last_dtp_lvl;
-	u8 dynamic_txhighpower_lvl;	/*Tx high power level */
-	u8 dm_flag;	/*Indicate if each dynamic mechanism's status. */
-	u8 dm_type;
-	u8 txpower_track_control;
-	bool binterrupt_migration;
-	bool bdisable_tx_int;
-	char ofdm_index[MAX_RF_PATH];
-	u8 default_ofdm_index;
-	u8 default_cck_index;
-	char cck_index;
-	char delta_power_index[MAX_RF_PATH];
-	char delta_power_index_last[MAX_RF_PATH];
-	char power_index_offset[MAX_RF_PATH];
-	char aboslute_ofdm_swing_idx[MAX_RF_PATH];
-	char remnant_ofdm_swing_idx[MAX_RF_PATH];
-	char remnant_cck_idx;
-	bool modify_txagc_flag_path_a;
-	bool modify_txagc_flag_path_b;
-
-	bool b_one_entry_only;
-	struct dm_phy_dbg_info dbginfo;
-	/* Dynamic ATC switch */
-
-	bool atc_status;
-	bool large_cfo_hit;
-	bool is_freeze;
-	int cfo_tail[2];
-	int cfo_ave_pre;
-	int crystal_cap;
-	u8 cfo_threshold;
-	u32 packet_count;
-	u32 packet_count_pre;
-	u8 tx_rate;
-
-
-	/*88e tx power tracking*/
-	u8 bb_swing_idx_ofdm[MAX_RF_PATH];
-	u8 bb_swing_idx_ofdm_current;
-	u8 bb_swing_idx_ofdm_base[MAX_RF_PATH];
-	bool bb_swing_flag_Ofdm;
-	u8 bb_swing_idx_cck;
-	u8 bb_swing_idx_cck_current;
-	u8 bb_swing_idx_cck_base;
-	bool bb_swing_flag_cck;
-
-	char bb_swing_diff_2g;
-	char bb_swing_diff_5g;
-
-	u8 delta_swing_table_idx_24gccka_p[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24gccka_n[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24gcckb_p[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24gcckb_n[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24ga_p[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24ga_n[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24gb_p[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24gb_n[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_5ga_p[BAND_NUM][DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_5ga_n[BAND_NUM][DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_5gb_p[BAND_NUM][DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_5gb_n[BAND_NUM][DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24ga_p_8188e[DELTA_SWINGIDX_SIZE];
-	u8 delta_swing_table_idx_24ga_n_8188e[DELTA_SWINGIDX_SIZE];
-
-
-	/* DMSP */
-	bool supp_phymode_switch;
-
-	/* DulMac */
-	struct rtl_dig dm_digtable;
-	struct rtl_pstbl dm_pstable;
-	struct fast_ant_trainning fat_table;
-
-	u8	resp_tx_path;
-	u8	path_sel;
-	u32	patha_sum;
-	u32	pathb_sum;
-	u32	patha_cnt;
-	u32	pathb_cnt;
-
-	u8 pre_channel;
-	u8 *p_channel;
-	u8 linked_interval;
-
-	u64 last_tx_ok_cnt;
-	u64 last_rx_ok_cnt;
-};
-
-#define	EFUSE_MAX_LOGICAL_SIZE		256
-
-struct rtl_efuse {
-	bool bautoLoad_ok;
-	bool bootfromefuse;
-	u16 max_physical_size;
-
-	u8 efuse_map[2][EFUSE_MAX_LOGICAL_SIZE];
-	u16 efuse_usedbytes;
-	u8 efuse_usedpercentage;
-#ifdef EFUSE_REPG_WORKAROUND
-	bool efuse_re_pg_sec1flag;
-	u8 efuse_re_pg_data[8];
-#endif
-
-	u8 autoload_failflag;
-	u8 autoload_status;
-
-	short epromtype;
-	u16 eeprom_vid;
-	u16 eeprom_did;
-	u16 eeprom_svid;
-	u16 eeprom_smid;
-	u8 eeprom_oemid;
-	u16 eeprom_channelplan;
-	u8 eeprom_version;
-
-	u8 dev_addr[6];
-	u8 board_type;
-	u8 wowlan_enable;
-	u8 antenna_div_cfg;
-	u8 antenna_div_type;
-
-	bool b_txpwr_fromeprom;
-	u8 eeprom_crystalcap;
-	u8 eeprom_tssi[2];
-	u8 eeprom_tssi_5g[3][2]; /* for 5GL/5GM/5GH band. */
-	u8 eeprom_pwrlimit_ht20[CHANNEL_GROUP_MAX];
-	u8 eeprom_pwrlimit_ht40[CHANNEL_GROUP_MAX];
-	u8 eeprom_chnlarea_txpwr_cck[2][CHANNEL_GROUP_MAX_2G];
-	u8 eeprom_chnlarea_txpwr_ht40_1s[2][CHANNEL_GROUP_MAX];
-	u8 eeprom_chnlarea_txpwr_ht40_2sdiif[2][CHANNEL_GROUP_MAX];
-
-
-	u8 internal_pa_5g[2];	/* pathA / pathB */
-	u8 eeprom_c9;
-	u8 eeprom_cc;
-
-	/*For power group */
-	u8 eeprom_pwrgroup[2][3];
-	u8 pwrgroup_ht20[2][CHANNEL_MAX_NUMBER];
-	u8 pwrgroup_ht40[2][CHANNEL_MAX_NUMBER];
-
-	u8 txpwrlevel_cck[MAX_RF_PATH][CHANNEL_MAX_NUMBER_2G];
-	/*For HT 40MHZ pwr */
-	u8 txpwrlevel_ht40_1s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	/*For HT 40MHZ pwr */
-	u8 txpwrlevel_ht40_2s[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	char txpwr_cckdiff[MAX_RF_PATH][MAX_TX_COUNT]; /*CCK_24G_Diff*/
-	/*HT 20<->40 Pwr diff */
-	char txpwr_ht20diff[MAX_RF_PATH][MAX_TX_COUNT];	/*BW20_24G_Diff*/
-	char txpwr_ht40diff[MAX_RF_PATH][MAX_TX_COUNT];/*BW40_24G_Diff*/
-	/*For HT<->legacy pwr diff */
-	char txpwr_legacyhtdiff[MAX_RF_PATH][MAX_TX_COUNT];/*OFDM_24G_Diff*/
-
-	u8 txpwr_5g_bw40base[MAX_RF_PATH][CHANNEL_MAX_NUMBER];
-	u8 txpwr_5g_bw80base[MAX_RF_PATH][CHANNEL_MAX_NUMBER_5G_80M];
-	char txpwr_5g_ofdmdiff[MAX_RF_PATH][MAX_TX_COUNT];
-	char txpwr_5g_bw20diff[MAX_RF_PATH][MAX_TX_COUNT];
-	char txpwr_5g_bw40diff[MAX_RF_PATH][MAX_TX_COUNT];
-	char txpwr_5g_bw80diff[MAX_RF_PATH][MAX_TX_COUNT];
-
-	u8 txpwr_safetyflag;		/* Band edge enable flag */
-	u16 eeprom_txpowerdiff;
-	u8 legacy_httxpowerdiff;	/* Legacy to HT rate power diff */
-	u8 antenna_txpwdiff[3];
-
-	u8 eeprom_regulatory;
-	u8 eeprom_thermalmeter;
-	u8 thermalmeter[2];/*ThermalMeter, index 0 for RFIC0, and 1 for RFIC1 */
-	u16 tssi_13dbm;
-	u8 crystalcap;		/* CrystalCap. */
-	u8 delta_iqk;
-	u8 delta_lck;
-
-	u8 legacy_ht_txpowerdiff;	/*Legacy to HT rate power diff */
-	bool b_apk_thermalmeterignore;
-
-	bool b1x1_recvcombine;
-	bool b1ss_support;
-
-	/*channel plan */
-	u8 channel_plan;
-};
-
-struct rtl_ps_ctl {
-	bool pwrdomain_protect;
-	bool b_in_powersavemode;
-	bool rfchange_inprogress;
-	bool b_swrf_processing;
-	bool b_hwradiooff;
-	/*
-	 * just for PCIE ASPM
-	 * If it supports ASPM, Offset[560h] = 0x40,
-	 * otherwise Offset[560h] = 0x00.
-	 * */
-	bool b_support_aspm;
-	bool b_support_backdoor;
-
-	/*for LPS */
-	enum rt_psmode dot11_psmode;	/*Power save mode configured. */
-	bool b_swctrl_lps;
-	bool b_fwctrl_lps;
-	u8 fwctrl_psmode;
-	/*For Fw control LPS mode */
-	u8 b_reg_fwctrl_lps;
-	/*Record Fw PS mode status. */
-	bool b_fw_current_inpsmode;
-	u8 reg_max_lps_awakeintvl;
-	bool report_linked;
-	bool b_low_power_enable;/*for 32k*/
-
-	/*for IPS */
-	bool b_inactiveps;
-
-	u32 rfoff_reason;
-
-	/*RF OFF Level */
-	u32 cur_ps_level;
-	u32 reg_rfps_level;
-
-	/*just for PCIE ASPM */
-	u8 const_amdpci_aspm;
-
-	enum rf_pwrstate inactive_pwrstate;
-	enum rf_pwrstate rfpwr_state;	/*cur power state */
-
-	/* for SW LPS*/
-	bool sw_ps_enabled;
-	bool state;
-	bool state_inap;
-	bool multi_buffered;
-	u16 nullfunc_seq;
-	unsigned int dtim_counter;
-	unsigned int sleep_ms;
-	unsigned long last_sleep_jiffies;
-	unsigned long last_awake_jiffies;
-	unsigned long last_delaylps_stamp_jiffies;
-	unsigned long last_dtim;
-	unsigned long last_beacon;
-	unsigned long last_action;
-	unsigned long last_slept;
-
-	/*For P2P PS */
-	struct rtl_p2p_ps_info p2p_ps_info;
-	u8 pwr_mode;
-	u8 smart_ps;
-};
-
-struct rtl_stats {
-	u8 psaddr[ETH_ALEN];
-	u32 mac_time[2];
-	s8 rssi;
-	u8 signal;
-	u8 noise;
-	u8 rate;		/* hw desc rate */
-	u8 rawdata;
-	u8 received_channel;
-	u8 control;
-	u8 mask;
-	u8 freq;
-	u16 len;
-	u64 tsf;
-	u32 beacon_time;
-	u8 nic_type;
-	u16 length;
-	u8 signalquality;	/*in 0-100 index. */
-	/*
-	 * Real power in dBm for this packet,
-	 * no beautification and aggregation.
-	 * */
-	s32 recvsignalpower;
-	s8 rxpower;		/*in dBm Translate from PWdB */
-	u8 signalstrength;	/*in 0-100 index. */
-	u16 b_hwerror:1;
-	u16 b_crc:1;
-	u16 b_icv:1;
-	u16 b_shortpreamble:1;
-	u16 antenna:1;
-	u16 decrypted:1;
-	u16 wakeup:1;
-	u32 timestamp_low;
-	u32 timestamp_high;
-	bool b_shift;
-
-	u8 rx_drvinfo_size;
-	u8 rx_bufshift;
-	bool b_isampdu;
-	bool b_isfirst_ampdu;
-	bool rx_is40Mhzpacket;
-	u32 rx_pwdb_all;
-	u8 rx_mimo_signalstrength[4];	/*in 0~100 index */
-	s8 rx_mimo_signalquality[4];
-	u8 rx_mimo_evm_dbm[4];
-	u16 cfo_short[4]; 		/* per-path's Cfo_short */
-	u16 cfo_tail[4];
-	u8 rx_pwr[4]; /* per-path's pwdb */
-	u8 rx_snr[4]; /* per-path's SNR */
-	u8 bandwidth;
-	u8 bt_coex_pwr_adjust;
-	bool b_packet_matchbssid;
-	bool b_is_cck;
-	bool b_is_ht;
-	bool b_packet_toself;
-	bool b_packet_beacon;	/*for rssi */
-	char cck_adc_pwdb[4];	/*for rx path selection */
-
-	u8 packet_report_type;
-
-	u32 macid;
-	u8 wake_match;
-	u32 bt_rx_rssi_percentage;
-	u32 macid_valid_entry[2];
-};
-
-struct rt_link_detect {
-	/* count for roaming */
-	u32 bcn_rx_inperiod;
-	u32 roam_times;
-
-	u32 num_tx_in4period[4];
-	u32 num_rx_in4period[4];
-
-	u32 num_tx_inperiod;
-	u32 num_rx_inperiod;
-
-	bool b_busytraffic;
-	bool b_tx_busy_traffic;
-	bool b_rx_busy_traffic;
-	bool b_higher_busytraffic;
-	bool b_higher_busyrxtraffic;
-
-	u32 tidtx_in4period[MAX_TID_COUNT][4];
-	u32 tidtx_inperiod[MAX_TID_COUNT];
-	bool higher_busytxtraffic[MAX_TID_COUNT];
-};
-
-struct rtl_tcb_desc {
-	u8 b_packet_bw:1;
-	u8 b_multicast:1;
-	u8 b_broadcast:1;
-
-	u8 b_rts_stbc:1;
-	u8 b_rts_enable:1;
-	u8 b_cts_enable:1;
-	u8 b_rts_use_shortpreamble:1;
-	u8 b_rts_use_shortgi:1;
-	u8 rts_sc:1;
-	u8 b_rts_bw:1;
-	u8 rts_rate;
-
-	u8 use_shortgi:1;
-	u8 use_shortpreamble:1;
-	u8 use_driver_rate:1;
-	u8 disable_ratefallback:1;
-
-	u8 ratr_index;
-	u8 mac_id;
-	u8 hw_rate;
-
-	u8 b_last_inipkt:1;
-	u8 b_cmd_or_init:1;
-	u8 queue_index;
-
-	/* early mode */
-	u8 empkt_num;
-	/* The max value by HW */
-	u32 empkt_len[10];
-	bool btx_enable_sw_calc_duration;
-	/* used for hal construct pkt,
-	 * we may set desc when tx */
-	u8 self_desc;
-};
-
-struct proxim {
-	bool proxim_on;
-
-	void *proximity_priv;
-	int (*proxim_rx)(struct ieee80211_hw *hw, struct rtl_stats *status,
-			 struct sk_buff *skb);
-	u8 (*proxim_get_var)(struct ieee80211_hw *hw, u8 type);
-};
-
-struct rtl_hal_ops {
-	int (*init_sw_vars)(struct ieee80211_hw *hw);
-	void (*deinit_sw_vars)(struct ieee80211_hw *hw);
-	void (*read_eeprom_info)(struct ieee80211_hw *hw);
-	void (*interrupt_recognized)(struct ieee80211_hw *hw,
-				     u32 *p_inta, u32 *p_intb);
-	int (*hw_init)(struct ieee80211_hw *hw);
-	void (*hw_disable)(struct ieee80211_hw *hw);
-	void (*hw_suspend)(struct ieee80211_hw *hw);
-	void (*hw_resume)(struct ieee80211_hw *hw);
-	void (*enable_interrupt)(struct ieee80211_hw *hw);
-	void (*disable_interrupt)(struct ieee80211_hw *hw);
-	int (*set_network_type)(struct ieee80211_hw *hw,
-				enum nl80211_iftype type);
-	void (*set_chk_bssid)(struct ieee80211_hw *hw,
-			      bool check_bssid);
-	void (*set_bw_mode)(struct ieee80211_hw *hw,
-			    enum nl80211_channel_type ch_type);
-	u8 (*switch_channel)(struct ieee80211_hw *hw);
-	void (*set_qos)(struct ieee80211_hw *hw, int aci);
-	void (*set_bcn_reg)(struct ieee80211_hw *hw);
-	void (*set_bcn_intv)(struct ieee80211_hw *hw);
-	void (*update_interrupt_mask)(struct ieee80211_hw *hw,
-				      u32 add_msr, u32 rm_msr);
-	void (*get_hw_reg)(struct ieee80211_hw *hw, u8 variable, u8 *val);
-	void (*set_hw_reg)(struct ieee80211_hw *hw, u8 variable, u8 *val);
-	void (*update_rate_tbl)(struct ieee80211_hw *hw,
-				struct ieee80211_sta *sta, u8 rssi_level);
-	void (*pre_fill_tx_bd_desc)(struct ieee80211_hw *hw, u8 *tx_bd_desc,
-				    u8 *desc, u8 queue_index,
-				    struct sk_buff *skb, dma_addr_t addr);
-	u16 (*rx_desc_buff_remained_cnt)(struct ieee80211_hw *hw,
-					 u8 queue_index);
-	void (*rx_check_dma_ok)(struct ieee80211_hw *hw, u8 *header_desc,
-				u8 queue_index);
-	void (*fill_tx_desc)(struct ieee80211_hw *hw,
-			     struct ieee80211_hdr *hdr,
-			     u8 *pdesc_tx, u8 *pbd_desc,
-			     struct ieee80211_tx_info *info,
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,7,0))
-/*<delete in kernel end>*/
-			      struct ieee80211_sta *sta,
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
-			      struct sk_buff *skb, u8 hw_queue,
-			      struct rtl_tcb_desc *ptcb_desc);
-	void (*fill_tx_cmddesc)(struct ieee80211_hw *hw, u8 *pdesc,
-				bool b_firstseg, bool b_lastseg,
-				struct sk_buff *skb);
-	bool (*query_rx_desc)(struct ieee80211_hw *hw,
-			      struct rtl_stats *status,
-			      struct ieee80211_rx_status *rx_status,
-			      u8 *pdesc, struct sk_buff *skb);
-	void (*set_channel_access)(struct ieee80211_hw *hw);
-	bool (*radio_onoff_checking)(struct ieee80211_hw *hw, u8 *valid);
-	void (*dm_watchdog)(struct ieee80211_hw *hw);
-	void (*scan_operation_backup)(struct ieee80211_hw *hw, u8 operation);
-	bool (*set_rf_power_state)(struct ieee80211_hw *hw,
-				   enum rf_pwrstate rfpwr_state);
-	void (*led_control)(struct ieee80211_hw *hw,
-			    enum led_ctl_mode ledaction);
-	void (*set_desc)(struct ieee80211_hw *hw, u8 *pdesc, bool istx,
-			 u8 desc_name, u8 *val);
-	u32 (*get_desc)(u8 *pdesc, bool istx, u8 desc_name);
-	bool (*is_tx_desc_closed)(struct ieee80211_hw *hw,
-				  u8 hw_queue, u16 index);
-	void (*tx_polling)(struct ieee80211_hw *hw, u8 hw_queue);
-	void (*enable_hw_sec)(struct ieee80211_hw *hw);
-	void (*set_key)(struct ieee80211_hw *hw, u32 key_index,
-			u8 *p_macaddr, bool is_group, u8 enc_algo,
-			bool is_wepkey, bool clear_all);
-	void (*init_sw_leds)(struct ieee80211_hw *hw);
-	u32 (*get_bbreg)(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask);
-	void (*set_bbreg)(struct ieee80211_hw *hw, u32 regaddr, u32 bitmask,
-			  u32 data);
-	u32 (*get_rfreg)(struct ieee80211_hw *hw, enum radio_path rfpath,
-			 u32 regaddr, u32 bitmask);
-	void (*set_rfreg)(struct ieee80211_hw *hw, enum radio_path rfpath,
-			  u32 regaddr, u32 bitmask, u32 data);
-	void (*allow_all_destaddr)(struct ieee80211_hw *hw,
-				   bool allow_all_da, bool write_into_reg);
-	void (*linked_set_reg)(struct ieee80211_hw *hw);
-	void (*check_switch_to_dmdp)(struct ieee80211_hw *hw);
-	void (*dualmac_easy_concurrent)(struct ieee80211_hw *hw);
-	void (*dualmac_switch_to_dmdp)(struct ieee80211_hw *hw);
-	void (*c2h_command_handle)(struct ieee80211_hw *hw);
-	void (*bt_wifi_media_status_notify)(struct ieee80211_hw *hw,
-					    bool mstate);
-	void (*bt_turn_off_bt_coexist_before_enter_lps)(struct ieee80211_hw *hw);
-	void (*fill_h2c_cmd)(struct ieee80211_hw *hw, u8 element_id,
-			     u32 cmd_len, u8 *p_cmdbuffer);
-	bool (*get_btc_status)(void);
-	u32 (*rx_command_packet_handler)(struct ieee80211_hw *hw,
-					 const struct rtl_stats *status,
-					 struct sk_buff *skb);
-};
-
-struct rtl_intf_ops {
-	/*com */
-	void (*read_efuse_byte)(struct ieee80211_hw *hw, u16 _offset, u8 *pbuf);
-	int (*adapter_start)(struct ieee80211_hw *hw);
-	void (*adapter_stop)(struct ieee80211_hw *hw);
-	bool (*check_buddy_priv)(struct ieee80211_hw *hw,
-				 struct rtl_priv **buddy_priv);
-
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	int (*adapter_tx)(struct ieee80211_hw *hw, struct sk_buff *skb,
-			  struct rtl_tcb_desc *ptcb_desc);
-#else
-/*<delete in kernel end>*/
-	int (*adapter_tx)(struct ieee80211_hw *hw,
-			  struct ieee80211_sta *sta,
-			  struct sk_buff *skb,
-			  struct rtl_tcb_desc *ptcb_desc);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
-#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3,10,0))
-	void (*flush)(struct ieee80211_hw *hw, u32 queues, bool drop);
-#else
-	void (*flush)(struct ieee80211_hw *hw, bool drop);
-#endif
-	int (*reset_trx_ring)(struct ieee80211_hw *hw);
-/*<delete in kernel start>*/
-#if (LINUX_VERSION_CODE < KERNEL_VERSION(3,7,0))
-	bool (*waitq_insert)(struct ieee80211_hw *hw, struct sk_buff *skb);
-#else
-/*<delete in kernel end>*/
-	bool (*waitq_insert)(struct ieee80211_hw *hw,
-			     struct ieee80211_sta *sta,
-			     struct sk_buff *skb);
-/*<delete in kernel start>*/
-#endif
-/*<delete in kernel end>*/
-
-	/*pci */
-	void (*disable_aspm)(struct ieee80211_hw *hw);
-	void (*enable_aspm)(struct ieee80211_hw *hw);
-
-	/*usb */
-};
-
-struct rtl_mod_params {
-	/* default: 0 = using hardware encryption */
-	bool sw_crypto;
-
-	/* default: 1 = using no linked power save */
-	bool b_inactiveps;
-
-	/* default: 1 = using linked sw power save */
-	bool b_swctrl_lps;
-
-	/* default: 1 = using linked fw power save */
-	bool b_fwctrl_lps;
-};
-
-struct rtl_hal_cfg {
-	u8 bar_id;
-	bool write_readback;
-	char *name;
-	char *fw_name;
-	struct rtl_hal_ops *ops;
-	struct rtl_mod_params *mod_params;
-
-	/*this map used for some registers or vars
-	   defined int HAL but used in MAIN */
-	u32 maps[RTL_VAR_MAP_MAX];
-
-};
-
-struct rtl_locks {
-	/* mutex */
-	struct mutex conf_mutex;
-
-	/*spin lock */
-	spinlock_t ips_lock;
-	spinlock_t irq_th_lock;
-	spinlock_t h2c_lock;
-	spinlock_t rf_ps_lock;
-	spinlock_t rf_lock;
-	spinlock_t lps_lock;
-	spinlock_t waitq_lock;
-	spinlock_t entry_list_lock;
-
-	/*FW clock change */
-	spinlock_t fw_ps_lock;
-
-	/*Dul mac*/
-	spinlock_t cck_and_rw_pagea_lock;
-
-	/*Easy concurrent*/
-	spinlock_t check_sendpkt_lock;
-
-	spinlock_t iqk_lock;
-};
-
-struct rtl_works {
-	struct ieee80211_hw *hw;
-
-	/*timer */
-	struct timer_list watchdog_timer;
-	struct timer_list dualmac_easyconcurrent_retrytimer;
-	struct timer_list fw_clockoff_timer;
-	struct timer_list fast_antenna_trainning_timer;
-	/*task */
-	struct tasklet_struct irq_tasklet;
-	struct tasklet_struct irq_prepare_bcn_tasklet;
-
-	/*work queue */
-	struct workqueue_struct *rtl_wq;
-	struct delayed_work watchdog_wq;
-	struct delayed_work ips_nic_off_wq;
-
-	/* For SW LPS */
-	struct delayed_work ps_work;
-	struct delayed_work ps_rfon_wq;
-	struct delayed_work fwevt_wq;
-};
-
-struct rtl_debug {
-	u32 dbgp_type[DBGP_TYPE_MAX];
-	u32 global_debuglevel;
-	u64 global_debugcomponents;
-
-	/* add for proc debug */
-	struct proc_dir_entry *proc_dir;
-	char proc_name[20];
-};
-
-#define MIMO_PS_STATIC			0
-#define MIMO_PS_DYNAMIC			1
-#define MIMO_PS_NOLIMIT			3
-
-struct rtl_dualmac_easy_concurrent_ctl {
-	enum band_type currentbandtype_backfordmdp;
-	bool bclose_bbandrf_for_dmsp;
-	bool bchange_to_dmdp;
-	bool bchange_to_dmsp;
-	bool bswitch_in_process;
-};
-
-struct rtl_dmsp_ctl {
-	bool bactivescan_for_slaveofdmsp;
-	bool bscan_for_anothermac_fordmsp;
-	bool bscan_for_itself_fordmsp;
-	bool bwritedig_for_anothermacofdmsp;
-	u32 curdigvalue_for_anothermacofdmsp;
-	bool bchangecckpdstate_for_anothermacofdmsp;
-	u8 curcckpdstate_for_anothermacofdmsp;
-	bool bchangetxhighpowerlvl_for_anothermacofdmsp;
-	u8 curtxhighlvl_for_anothermacofdmsp;
-	long rssivalmin_for_anothermacofdmsp;
-};
-
-struct rtl_global_var {
-	/* from this list we can get
-	 * other adapter's rtl_priv */
-	struct list_head glb_priv_list;
-	spinlock_t glb_list_lock;
-};
-
-struct rtl_btc_info {
-	u8 bt_type;
-	u8 btcoexist;
-	u8 ant_num;
-};
-
-struct rtl_btc_ops {
-	void (*btc_init_variables)(struct rtl_priv *rtlpriv);
-	void (*btc_init_hal_vars)(struct rtl_priv *rtlpriv);
-	void (*btc_init_hw_config)(struct rtl_priv *rtlpriv);
-	void (*btc_ips_notify)(struct rtl_priv *rtlpriv, u8 type);
-	void (*btc_scan_notify)(struct rtl_priv *rtlpriv, u8 scantype);
-	void (*btc_connect_notify)(struct rtl_priv *rtlpriv, u8 action);
-	void (*btc_mediastatus_notify)(struct rtl_priv *rtlpriv,
-				       enum rt_media_status mstatus);
-	void (*btc_periodical)(struct rtl_priv *rtlpriv);
-	void (*btc_halt_notify)(void);
-	void (*btc_btinfo_notify)(struct rtl_priv *rtlpriv,
-				  u8 *tmp_buf, u8 length);
-	bool (*btc_is_limited_dig)(struct rtl_priv *rtlpriv);
-	bool (*btc_is_disable_edca_turbo)(struct rtl_priv *rtlpriv);
-	bool (*btc_is_bt_disabled)(struct rtl_priv *rtlpriv);
-};
-
-struct rtl_bt_coexist {
-	struct rtl_btc_ops *btc_ops;
-	struct rtl_btc_info btc_info;
-};
-
-
-struct rtl_priv {
-	struct list_head list;
-#ifdef VIF_TODO
-	struct vif_priv vif_priv;
-#endif
-	struct rtl_priv *buddy_priv;
-	struct rtl_global_var *glb_var;
-	struct rtl_dualmac_easy_concurrent_ctl easy_concurrent_ctl;
-	struct rtl_dmsp_ctl dmsp_ctl;
-	struct rtl_locks locks;
-	struct rtl_works works;
-	struct rtl_mac mac80211;
-	struct rtl_hal rtlhal;
-	struct rtl_regulatory regd;
-	struct rtl_rfkill rfkill;
-	struct rtl_io io;
-	struct rtl_phy phy;
-	struct rtl_dm dm;
-	struct rtl_security sec;
-	struct rtl_efuse efuse;
-
-	struct rtl_ps_ctl psc;
-	struct rate_adaptive ra;
-	struct dynamic_primary_cca primarycca;
-	struct wireless_stats stats;
-	struct rt_link_detect link_info;
-	struct false_alarm_statistics falsealm_cnt;
-
-	struct rtl_rate_priv *rate_priv;
-
-	struct rtl_debug dbg;
-
-	/* sta entry list for ap adhoc or mesh */
-	struct list_head entry_list;
-
-	/*
-	 *hal_cfg : for diff cards
-	 *intf_ops : for diff interface usb/pcie
-	 */
-	struct rtl_hal_cfg *cfg;
-	struct rtl_intf_ops *intf_ops;
-
-	/*this var will be set by set_bit,
-	   and was used to indicate status of
-	   interface or hardware */
-	unsigned long status;
-
-	/* intel Proximity, should be alloc mem
-	 * in intel Proximity module and can only
-	 * be used in intel Proximity mode */
-	struct proxim proximity;
-
-	/*for bt coexist use*/
-	struct rtl_bt_coexist btcoexist;
-
-	/* separate 92ee from other ICs,
-	  * 92ee use new trx flow. */
-	bool use_new_trx_flow;
-	/*This must be the last item so
-	   that it points to the data allocated
-	   beyond  this structure like:
-	   rtl_pci_priv or rtl_usb_priv */
-	u8 priv[0];
-};
-
-#define rtl_priv(hw)		(((struct rtl_priv *)(hw)->priv))
-#define rtl_mac(rtlpriv)	(&((rtlpriv)->mac80211))
-#define rtl_hal(rtlpriv)	(&((rtlpriv)->rtlhal))
-#define rtl_efuse(rtlpriv)	(&((rtlpriv)->efuse))
-#define rtl_psc(rtlpriv)	(&((rtlpriv)->psc))
-#define rtl_sec(rtlpriv)	(&((rtlpriv)->sec))
-#define rtl_dm(rtlpriv)		(&((rtlpriv)->dm))
-/***************************************
-    Bluetooth Co-existance Related
-****************************************/
-
-enum bt_ant_num {
-	ANT_X2 = 0,
-	ANT_X1 = 1,
-};
-
-enum bt_co_type {
-	BT_2WIRE = 0,
-	BT_ISSC_3WIRE = 1,
-	BT_ACCEL = 2,
-	BT_CSR_BC4 = 3,
-	BT_CSR_BC8 = 4,
-	BT_RTL8756 = 5,
-	BT_RTL8723A = 6,
-	BT_RTL8821A = 7,
-	BT_RTL8723B = 8,
-	BT_RTL8192E = 9,
-	BT_RTL8812A = 11,
-};
-
-enum bt_total_ant_num {
-	ANT_TOTAL_X2 = 0,
-	ANT_TOTAL_X1 = 1
-};
-
-enum bt_cur_state {
-	BT_OFF = 0,
-	BT_ON = 1,
-};
-
-enum bt_service_type {
-	BT_SCO = 0,
-	BT_A2DP = 1,
-	BT_HID = 2,
-	BT_HID_IDLE = 3,
-	BT_SCAN = 4,
-	BT_IDLE = 5,
-	BT_OTHER_ACTION = 6,
-	BT_BUSY = 7,
-	BT_OTHERBUSY = 8,
-	BT_PAN = 9,
-};
-
-enum bt_radio_shared {
-	BT_RADIO_SHARED = 0,
-	BT_RADIO_INDIVIDUAL = 1,
-};
-
-struct bt_coexist_info {
-
-	/* EEPROM BT info. */
-	u8 eeprom_bt_coexist;
-	u8 eeprom_bt_type;
-	u8 eeprom_bt_ant_num;
-	u8 eeprom_bt_ant_isolation;
-	u8 eeprom_bt_radio_shared;
-
-	u8 bt_coexistence;
-	u8 bt_ant_num;
-	u8 bt_coexist_type;
-	u8 bt_state;
-	u8 bt_cur_state;	/* 0:on, 1:off */
-	u8 bt_ant_isolation;	/* 0:good, 1:bad */
-	u8 bt_pape_ctrl;	/* 0:SW, 1:SW/HW dynamic */
-	u8 bt_service;
-	u8 bt_radio_shared_type;
-	u8 bt_rfreg_origin_1e;
-	u8 bt_rfreg_origin_1f;
-	u8 bt_rssi_state;
-	u32 ratio_tx;
-	u32 ratio_pri;
-	u32 bt_edca_ul;
-	u32 bt_edca_dl;
-
-	bool b_init_set;
-	bool b_bt_busy_traffic;
-	bool b_bt_traffic_mode_set;
-	bool b_bt_non_traffic_mode_set;
-
-	bool b_fw_coexist_all_off;
-	bool b_sw_coexist_all_off;
-	bool b_hw_coexist_all_off;
-	u32 current_state;
-	u32 previous_state;
-	u32 current_state_h;
-	u32 previous_state_h;
-
-	u8 bt_pre_rssi_state;
-	u8 bt_pre_rssi_state1;
-
-	u8 b_reg_bt_iso;
-	u8 b_reg_bt_sco;
-	bool b_balance_on;
-	u8 bt_active_zero_cnt;
-	bool b_cur_bt_disabled;
-	bool b_pre_bt_disabled;
-
-	u8 bt_profile_case;
-	u8 bt_profile_action;
-	bool b_bt_busy;
-	bool b_hold_for_bt_operation;
-	u8 lps_counter;
-};
-
-
-/****************************************
-	mem access macro define start
-	Call endian free function when
-	1. Read/write packet content.
-	2. Before write integer to IO.
-	3. After read integer from IO.
-****************************************/
-/* Convert little data endian to host */
-#define EF1BYTE(_val)		\
-	((u8)(_val))
-#define EF2BYTE(_val)		\
-	(le16_to_cpu(_val))
-#define EF4BYTE(_val)		\
-	(le32_to_cpu(_val))
-
-/* Read data from memory */
-#define READEF1BYTE(_ptr)	\
-	EF1BYTE(*((u8 *)(_ptr)))
-#define READEF2BYTE(_ptr)	\
-	EF2BYTE(*((u16 *)(_ptr)))
-#define READEF4BYTE(_ptr)	\
-	EF4BYTE(*((u32 *)(_ptr)))
-
-/* Write data to memory */
-#define WRITEEF1BYTE(_ptr, _val)	\
-	((*((u8 *)(_ptr))) = EF1BYTE(_val))
-#define WRITEEF2BYTE(_ptr, _val)	\
-	((*((u16 *)(_ptr))) = EF2BYTE(_val))
-#define WRITEEF4BYTE(_ptr, _val)	\
-	((*((u32 *)(_ptr))) = EF4BYTE(_val))
-
-/*Example:
-BIT_LEN_MASK_32(0) => 0x00000000
-BIT_LEN_MASK_32(1) => 0x00000001
-BIT_LEN_MASK_32(2) => 0x00000003
-BIT_LEN_MASK_32(32) => 0xFFFFFFFF*/
-#define BIT_LEN_MASK_32(__bitlen)	 \
-	(0xFFFFFFFF >> (32 - (__bitlen)))
-#define BIT_LEN_MASK_16(__bitlen)	 \
-	(0xFFFF >> (16 - (__bitlen)))
-#define BIT_LEN_MASK_8(__bitlen) \
-	(0xFF >> (8 - (__bitlen)))
-
-/*Example:
-BIT_OFFSET_LEN_MASK_32(0, 2) => 0x00000003
-BIT_OFFSET_LEN_MASK_32(16, 2) => 0x00030000*/
-#define BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen) \
-	(BIT_LEN_MASK_32(__bitlen) << (__bitoffset))
-#define BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen) \
-	(BIT_LEN_MASK_16(__bitlen) << (__bitoffset))
-#define BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen) \
-	(BIT_LEN_MASK_8(__bitlen) << (__bitoffset))
-
-/*Description:
-Return 4-byte value in host byte ordering from
-4-byte pointer in little-endian system.*/
-#define LE_P4BYTE_TO_HOST_4BYTE(__pstart) \
-	(EF4BYTE(*((u32 *)(__pstart))))
-#define LE_P2BYTE_TO_HOST_2BYTE(__pstart) \
-	(EF2BYTE(*((u16 *)(__pstart))))
-#define LE_P1BYTE_TO_HOST_1BYTE(__pstart) \
-	(EF1BYTE(*((u8 *)(__pstart))))
-
-/*Description:
-Translate subfield (continuous bits in little-endian) of 4-byte
-value to host byte ordering.*/
-#define LE_BITS_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P4BYTE_TO_HOST_4BYTE(__pstart) >> (__bitoffset))  & \
-		BIT_LEN_MASK_32(__bitlen) \
-	)
-#define LE_BITS_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P2BYTE_TO_HOST_2BYTE(__pstart) >> (__bitoffset)) & \
-		BIT_LEN_MASK_16(__bitlen) \
-	)
-#define LE_BITS_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		(LE_P1BYTE_TO_HOST_1BYTE(__pstart) >> (__bitoffset)) & \
-		BIT_LEN_MASK_8(__bitlen) \
-	)
-
-/*Description:
-Mask subfield (continuous bits in little-endian) of 4-byte value
-and return the result in 4-byte value in host byte ordering.*/
-#define LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		LE_P4BYTE_TO_HOST_4BYTE(__pstart)  & \
-		(~BIT_OFFSET_LEN_MASK_32(__bitoffset, __bitlen)) \
-	)
-#define LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		LE_P2BYTE_TO_HOST_2BYTE(__pstart) & \
-		(~BIT_OFFSET_LEN_MASK_16(__bitoffset, __bitlen)) \
-	)
-#define LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) \
-	( \
-		LE_P1BYTE_TO_HOST_1BYTE(__pstart) & \
-		(~BIT_OFFSET_LEN_MASK_8(__bitoffset, __bitlen)) \
-	)
-
-/*Description:
-Set subfield of little-endian 4-byte value to specified value.	*/
-#define SET_BITS_TO_LE_4BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u32 *)(__pstart)) = EF4BYTE \
-	( \
-		LE_BITS_CLEARED_TO_4BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u32)__val) & BIT_LEN_MASK_32(__bitlen)) << (__bitoffset))\
-       );
-#define SET_BITS_TO_LE_2BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u16 *)(__pstart)) = EF2BYTE \
-	( \
-		LE_BITS_CLEARED_TO_2BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u16)__val) & BIT_LEN_MASK_16(__bitlen)) << (__bitoffset))\
-       );
-#define SET_BITS_TO_LE_1BYTE(__pstart, __bitoffset, __bitlen, __val) \
-	*((u8 *)(__pstart)) = EF1BYTE \
-	( \
-		LE_BITS_CLEARED_TO_1BYTE(__pstart, __bitoffset, __bitlen) | \
-		((((u8)__val) & BIT_LEN_MASK_8(__bitlen)) << (__bitoffset)) \
-       );
-
-#define	N_BYTE_ALIGMENT(__value, __aligment) ((__aligment == 1) ? \
-	(__value) : (((__value + __aligment - 1) / __aligment) * __aligment))
-
-/****************************************
-	mem access macro define end
-****************************************/
-
-#define byte(x, n) ((x >> (8 * n)) & 0xff)
-
-#define packet_get_type(_packet) (EF1BYTE((_packet).octet[0]) & 0xFC)
-#define RTL_WATCH_DOG_TIME	2000
-#define MSECS(t)		msecs_to_jiffies(t)
-#define WLAN_FC_GET_VERS(fc)	((fc) & IEEE80211_FCTL_VERS)
-#define WLAN_FC_GET_TYPE(fc)	((fc) & IEEE80211_FCTL_FTYPE)
-#define WLAN_FC_GET_STYPE(fc)	((fc) & IEEE80211_FCTL_STYPE)
-#define WLAN_FC_MORE_DATA(fc)	((fc) & IEEE80211_FCTL_MOREDATA)
-#define SEQ_TO_SN(seq)		(((seq) & IEEE80211_SCTL_SEQ) >> 4)
-#define SN_TO_SEQ(ssn)		(((ssn) << 4) & IEEE80211_SCTL_SEQ)
-#define MAX_SN			((IEEE80211_SCTL_SEQ) >> 4)
-
-#define	RT_RF_OFF_LEVL_ASPM		BIT(0)	/*PCI ASPM */
-#define	RT_RF_OFF_LEVL_CLK_REQ		BIT(1)	/*PCI clock request */
-#define	RT_RF_OFF_LEVL_PCI_D3		BIT(2)	/*PCI D3 mode */
-/*NIC halt, re-initialize hw parameters*/
-#define	RT_RF_OFF_LEVL_HALT_NIC		BIT(3)
-#define	RT_RF_OFF_LEVL_FREE_FW		BIT(4)	/*FW free, re-download the FW */
-#define	RT_RF_OFF_LEVL_FW_32K		BIT(5)	/*FW in 32k */
-/*Always enable ASPM and Clock Req in initialization.*/
-#define	RT_RF_PS_LEVEL_ALWAYS_ASPM	BIT(6)
-/* no matter RFOFF or SLEEP we set PS_ASPM_LEVL*/
-#define	RT_PS_LEVEL_ASPM		BIT(7)
-/*When LPS is on, disable 2R if no packet is received or transmitted.*/
-#define	RT_RF_LPS_DISALBE_2R		BIT(30)
-#define	RT_RF_LPS_LEVEL_ASPM		BIT(31)	/*LPS with ASPM */
-#define	RT_IN_PS_LEVEL(ppsc, _ps_flg)		\
-	((ppsc->cur_ps_level & _ps_flg) ? true : false)
-#define	RT_CLEAR_PS_LEVEL(ppsc, _ps_flg)	\
-	(ppsc->cur_ps_level &= (~(_ps_flg)))
-#define	RT_SET_PS_LEVEL(ppsc, _ps_flg)		\
-	(ppsc->cur_ps_level |= _ps_flg)
-
-#define container_of_dwork_rtl(x,y,z) \
-	container_of(container_of(x, struct delayed_work, work), y, z)
-
-#define FILL_OCTET_STRING(_os,_octet,_len)	\
-		(_os).octet = (u8 *)(_octet);		\
-		(_os).length = (_len);
-
-#define CP_MACADDR(des,src)	\
-	((des)[0] = (src)[0],(des)[1] = (src)[1],\
-	(des)[2] = (src)[2],(des)[3] = (src)[3],\
-	(des)[4] = (src)[4],(des)[5] = (src)[5])
-
-static inline u8 rtl_read_byte(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return rtlpriv->io.read8_sync(rtlpriv, addr);
-}
-
-static inline u16 rtl_read_word(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return rtlpriv->io.read16_sync(rtlpriv, addr);
-}
-
-static inline u32 rtl_read_dword(struct rtl_priv *rtlpriv, u32 addr)
-{
-	return rtlpriv->io.read32_sync(rtlpriv, addr);
-}
-
-static inline void rtl_write_byte(struct rtl_priv *rtlpriv, u32 addr, u8 val8)
-{
-	rtlpriv->io.write8_async(rtlpriv, addr, val8);
-
-	if (rtlpriv->cfg->write_readback)
-		rtlpriv->io.read8_sync(rtlpriv, addr);
-}
-
-static inline void rtl_write_word(struct rtl_priv *rtlpriv, u32 addr, u16 val16)
-{
-	rtlpriv->io.write16_async(rtlpriv, addr, val16);
-
-	if (rtlpriv->cfg->write_readback)
-		rtlpriv->io.read16_sync(rtlpriv, addr);
-}
-
-static inline void rtl_write_dword(struct rtl_priv *rtlpriv,
-				   u32 addr, u32 val32)
-{
-	rtlpriv->io.write32_async(rtlpriv, addr, val32);
-
-	if (rtlpriv->cfg->write_readback)
-		rtlpriv->io.read32_sync(rtlpriv, addr);
-}
-
-static inline u32 rtl_get_bbreg(struct ieee80211_hw *hw,
-				u32 regaddr, u32 bitmask)
-{
-	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_bbreg(hw,
-								    regaddr,
-								    bitmask);
-}
-
-static inline void rtl_set_bbreg(struct ieee80211_hw *hw, u32 regaddr,
-				 u32 bitmask, u32 data)
-{
-	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_bbreg(hw,
-							     regaddr, bitmask,
-							     data);
-
-}
-
-static inline u32 rtl_get_rfreg(struct ieee80211_hw *hw,
-				enum radio_path rfpath, u32 regaddr,
-				u32 bitmask)
-{
-	return ((struct rtl_priv *)(hw)->priv)->cfg->ops->get_rfreg(hw,
-								    rfpath,
-								    regaddr,
-								    bitmask);
-}
-
-static inline void rtl_set_rfreg(struct ieee80211_hw *hw,
-				 enum radio_path rfpath, u32 regaddr,
-				 u32 bitmask, u32 data)
-{
-	((struct rtl_priv *)(hw)->priv)->cfg->ops->set_rfreg(hw,
-							     rfpath, regaddr,
-							     bitmask, data);
-}
-
-static inline bool is_hal_stop(struct rtl_hal *rtlhal)
-{
-	return (_HAL_STATE_STOP == rtlhal->state);
-}
-
-static inline void set_hal_start(struct rtl_hal *rtlhal)
-{
-	rtlhal->state = _HAL_STATE_START;
-}
-
-static inline void set_hal_stop(struct rtl_hal *rtlhal)
-{
-	rtlhal->state = _HAL_STATE_STOP;
-}
-
-static inline u8 get_rf_type(struct rtl_phy *rtlphy)
-{
-	return rtlphy->rf_type;
-}
-
-static inline struct ieee80211_hdr *rtl_get_hdr(struct sk_buff *skb)
-{
-	return (struct ieee80211_hdr *)(skb->data);
-}
-
-static inline u16 rtl_get_fc(struct sk_buff *skb)
-{
-	return le16_to_cpu(rtl_get_hdr(skb)->frame_control);
-}
-
-static inline u16 rtl_get_tid_h(struct ieee80211_hdr *hdr)
-{
-	return (ieee80211_get_qos_ctl(hdr))[0] & IEEE80211_QOS_CTL_TID_MASK;
-}
-
-static inline u16 rtl_get_tid(struct sk_buff *skb)
-{
-	return rtl_get_tid_h(rtl_get_hdr(skb));
-}
-
-static inline struct ieee80211_sta *rtl_find_sta(struct ieee80211_hw *hw,
-						 u8 *mac_addr)
-{
-	struct rtl_mac *mac = rtl_mac(rtl_priv(hw));
-	return ieee80211_find_sta(mac->vif, mac_addr);
-}
-
-struct ieee80211_hw *rtl_pci_get_hw_pointer(void);
-#endif
diff --git a/drivers/staging/rts5208/Kconfig b/drivers/staging/rts5208/Kconfig
index 055655cecaf7..05c990f654a4 100644
--- a/drivers/staging/rts5208/Kconfig
+++ b/drivers/staging/rts5208/Kconfig
@@ -6,10 +6,3 @@ config RTS5208
 	  PCI-E card reader rts5208/rts5288.
 
 	  If this driver is compiled as a module, it will be named rts5208.
-
-config RTS5208_DEBUG
-	bool "Realtek PCI-E Card Reader RTS5208/5288 verbose debug"
-	depends on RTS5208
-	help
-	  Say Y here in order to have the rts5208 code generate
-	  verbose debugging messages.
diff --git a/drivers/staging/rts5208/general.c b/drivers/staging/rts5208/general.c
index eada934288b2..79d245877264 100644
--- a/drivers/staging/rts5208/general.c
+++ b/drivers/staging/rts5208/general.c
@@ -25,6 +25,7 @@
 int bit1cnt_long(u32 data)
 {
 	int i, cnt = 0;
+
 	for (i = 0; i < 32; i++) {
 		if (data & 0x01)
 			cnt++;
diff --git a/drivers/staging/rts5208/ms.c b/drivers/staging/rts5208/ms.c
index 390b1f83ebc2..228e48339b9e 100644
--- a/drivers/staging/rts5208/ms.c
+++ b/drivers/staging/rts5208/ms.c
@@ -305,7 +305,8 @@ static int ms_read_bytes(struct rtsx_chip *chip,
 
 	if ((tpc == PRO_READ_SHORT_DATA) && (data_len == 8)) {
 		dev_dbg(rtsx_dev(chip), "Read format progress:\n");
-		RTSX_DUMP(ptr, cnt);
+		print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, ptr,
+				     cnt);
 	}
 
 	return STATUS_SUCCESS;
@@ -861,8 +862,8 @@ static int ms_read_attribute_info(struct rtsx_chip *chip)
 					0, 0, buf, 64 * 512);
 		if (retval == STATUS_SUCCESS)
 			break;
-		else
-			rtsx_clear_ms_error(chip);
+
+		rtsx_clear_ms_error(chip);
 	}
 	if (retval != STATUS_SUCCESS) {
 		kfree(buf);
@@ -1913,7 +1914,7 @@ RE_SEARCH:
 	ptr = rtsx_get_cmd_data(chip);
 
 	dev_dbg(rtsx_dev(chip), "Boot block data:\n");
-	RTSX_DUMP(ptr, 16);
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 16, ptr);
 
 	/* Block ID error
 	 * HEADER_ID0, HEADER_ID1
@@ -2712,6 +2713,7 @@ static int mspro_read_format_progress(struct rtsx_chip *chip,
 		ms_card->progress = 0;
 	} else {
 		u64 ulltmp = (u64)cur_progress * (u64)65535;
+
 		do_div(ulltmp, total_progress);
 		ms_card->progress = (u16)ulltmp;
 	}
@@ -2775,8 +2777,6 @@ void mspro_polling_format_status(struct rtsx_chip *chip)
 				break;
 		}
 	}
-
-	return;
 }
 
 int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
diff --git a/drivers/staging/rts5208/rtsx.c b/drivers/staging/rts5208/rtsx.c
index e7a6ba2002ae..2d2527c3aea2 100644
--- a/drivers/staging/rts5208/rtsx.c
+++ b/drivers/staging/rts5208/rtsx.c
@@ -313,7 +313,7 @@ int rtsx_read_pci_cfg_byte(u8 bus, u8 dev, u8 func, u8 offset, u8 *val)
  */
 static int rtsx_suspend(struct pci_dev *pci, pm_message_t state)
 {
-	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
 	struct rtsx_chip *chip;
 
 	if (!dev)
@@ -348,7 +348,7 @@ static int rtsx_suspend(struct pci_dev *pci, pm_message_t state)
 
 static int rtsx_resume(struct pci_dev *pci)
 {
-	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
 	struct rtsx_chip *chip;
 
 	if (!dev)
@@ -394,7 +394,7 @@ static int rtsx_resume(struct pci_dev *pci)
 
 static void rtsx_shutdown(struct pci_dev *pci)
 {
-	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
 	struct rtsx_chip *chip;
 
 	if (!dev)
@@ -414,8 +414,6 @@ static void rtsx_shutdown(struct pci_dev *pci)
 		pci_disable_msi(pci);
 
 	pci_disable_device(pci);
-
-	return;
 }
 
 static int rtsx_control_thread(void *__dev)
@@ -598,8 +596,7 @@ static irqreturn_t rtsx_interrupt(int irq, void *dev_id)
 		spin_unlock(&dev->reg_lock);
 		if (chip->int_reg == 0xFFFFFFFF)
 			return IRQ_HANDLED;
-		else
-			return IRQ_NONE;
+		return IRQ_NONE;
 	}
 
 	status = chip->int_reg;
@@ -1017,7 +1014,7 @@ errout:
 
 static void rtsx_remove(struct pci_dev *pci)
 {
-	struct rtsx_dev *dev = (struct rtsx_dev *)pci_get_drvdata(pci);
+	struct rtsx_dev *dev = pci_get_drvdata(pci);
 
 	dev_info(&pci->dev, "rtsx_remove() called\n");
 
diff --git a/drivers/staging/rts5208/rtsx_card.h b/drivers/staging/rts5208/rtsx_card.h
index b19239e106f9..75cf5889bda4 100644
--- a/drivers/staging/rts5208/rtsx_card.h
+++ b/drivers/staging/rts5208/rtsx_card.h
@@ -1037,8 +1037,8 @@ static inline u32 get_card_size(struct rtsx_chip *chip, unsigned int lun)
 	if ((get_lun_card(chip, lun) == SD_CARD) &&
 		(sd_card->sd_lock_status & SD_LOCKED))
 		return 0;
-	else
-		return chip->capacity[lun];
+
+	return chip->capacity[lun];
 #else
 	return chip->capacity[lun];
 #endif
diff --git a/drivers/staging/rts5208/rtsx_chip.c b/drivers/staging/rts5208/rtsx_chip.c
index fe98309b7de6..a7ade8b4e7f2 100644
--- a/drivers/staging/rts5208/rtsx_chip.c
+++ b/drivers/staging/rts5208/rtsx_chip.c
@@ -380,6 +380,7 @@ int rtsx_reset_chip(struct rtsx_chip *chip)
 
 			if (chip->ic_version >= IC_VER_D) {
 				u16 reg;
+
 				retval = rtsx_read_phy_register(chip, 0x00,
 								&reg);
 				if (retval != STATUS_SUCCESS)
@@ -505,6 +506,7 @@ static inline int check_sd_speed_prior(u32 sd_speed_prior)
 
 	for (i = 0; i < 4; i++) {
 		u8 tmp = (u8)(sd_speed_prior >> (i*8));
+
 		if ((tmp < 0x01) || (tmp > 0x04)) {
 			fake_para = 1;
 			break;
@@ -520,6 +522,7 @@ static inline int check_sd_current_prior(u32 sd_current_prior)
 
 	for (i = 0; i < 4; i++) {
 		u8 tmp = (u8)(sd_current_prior >> (i*8));
+
 		if (tmp > 0x03) {
 			fake_para = 1;
 			break;
@@ -876,6 +879,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 
 	if (chip->polling_config) {
 		u8 val;
+
 		rtsx_read_config_byte(chip, 0, &val);
 	}
 
@@ -901,6 +905,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 	if (sd_card->sd_erase_status) {
 		if (chip->card_exist & SD_CARD) {
 			u8 val;
+
 			rtsx_read_register(chip, 0xFD30, &val);
 			if (val & 0x02) {
 				sd_card->sd_erase_status = SD_NOT_ERASE;
@@ -923,6 +928,7 @@ void rtsx_polling_func(struct rtsx_chip *chip)
 		} else {
 			if (CHK_SDIO_EXIST(chip) && !CHK_SDIO_IGNORED(chip)) {
 				u32 val;
+
 				rtsx_read_cfg_dw(chip, 1, 0x04, &val);
 				if (val & 0x07)
 					ss_allowed = 0;
@@ -1140,6 +1146,7 @@ void rtsx_stop_cmd(struct rtsx_chip *chip, int card)
 	for (i = 0; i <= 8; i++) {
 		int addr = RTSX_HCBAR + i * 4;
 		u32 reg;
+
 		reg = rtsx_readl(chip, addr);
 		dev_dbg(rtsx_dev(chip), "BAR (0x%02x): 0x%08x\n", addr, reg);
 	}
@@ -1149,6 +1156,7 @@ void rtsx_stop_cmd(struct rtsx_chip *chip, int card)
 	for (i = 0; i < 16; i++) {
 		u16 addr = 0xFE20 + (u16)i;
 		u8 val;
+
 		rtsx_read_register(chip, addr, &val);
 		dev_dbg(rtsx_dev(chip), "0x%04X: 0x%02x\n", addr, val);
 	}
@@ -1309,8 +1317,10 @@ int rtsx_write_cfg_seq(struct rtsx_chip *chip, u8 func, u16 addr, u8 *buf,
 		}
 	}
 
-	RTSX_DUMP(mask, dw_len * 4);
-	RTSX_DUMP(data, dw_len * 4);
+	print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, mask,
+			     dw_len * 4);
+	print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, data,
+			     dw_len * 4);
 
 	for (i = 0; i < dw_len; i++) {
 		retval = rtsx_write_cfg_dw(chip, func, aligned_addr + i * 4,
@@ -1597,6 +1607,7 @@ void rtsx_enter_ss(struct rtsx_chip *chip)
 	} else {
 		if (!chip->phy_debug_mode) {
 			u32 tmp;
+
 			tmp = rtsx_readl(chip, RTSX_BIER);
 			tmp |= CARD_INT;
 			rtsx_writel(chip, RTSX_BIER, tmp);
@@ -1802,6 +1813,7 @@ void rtsx_enable_aspm(struct rtsx_chip *chip)
 
 			if (CHK_SDIO_EXIST(chip)) {
 				u16 val = chip->aspm_l0s_l1_en | 0x0100;
+
 				if (CHECK_PID(chip, 0x5288))
 					rtsx_write_cfg_dw(chip, 2, 0xC0,
 							0xFFFF, val);
@@ -1811,8 +1823,6 @@ void rtsx_enable_aspm(struct rtsx_chip *chip)
 			}
 		}
 	}
-
-	return;
 }
 
 void rtsx_disable_aspm(struct rtsx_chip *chip)
@@ -1836,8 +1846,6 @@ void rtsx_disable_aspm(struct rtsx_chip *chip)
 			wait_timeout(1);
 		}
 	}
-
-	return;
 }
 
 int rtsx_read_ppbuf(struct rtsx_chip *chip, u8 *buf, int buf_len)
diff --git a/drivers/staging/rts5208/rtsx_scsi.c b/drivers/staging/rts5208/rtsx_scsi.c
index 5f5f512714e5..bbbf7968a0b6 100644
--- a/drivers/staging/rts5208/rtsx_scsi.c
+++ b/drivers/staging/rts5208/rtsx_scsi.c
@@ -39,7 +39,7 @@ void scsi_show_command(struct rtsx_chip *chip)
 {
 	struct scsi_cmnd *srb = chip->srb;
 	char *what = NULL;
-	int i, unknown_cmd = 0;
+	int unknown_cmd = 0, len;
 
 	switch (srb->cmnd[0]) {
 	case TEST_UNIT_READY:
@@ -319,9 +319,8 @@ void scsi_show_command(struct rtsx_chip *chip)
 			what, srb->cmd_len);
 
 	if (unknown_cmd) {
-		for (i = 0; i < srb->cmd_len && i < 16; i++)
-			dev_dbg(rtsx_dev(chip), " %02x", srb->cmnd[i]);
-		dev_dbg(rtsx_dev(chip), "\n");
+		len = min_t(unsigned short, srb->cmd_len, 16);
+		dev_dbg(rtsx_dev(chip), "%*ph\n", len, srb->cmnd);
 	}
 }
 
@@ -441,6 +440,7 @@ static int test_unit_ready(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 #ifdef SUPPORT_SD_LOCK
 	if (get_lun_card(chip, SCSI_LUN(srb)) == SD_CARD) {
 		struct sd_info *sd_card = &(chip->sd_card);
+
 		if (sd_card->sd_lock_notify) {
 			sd_card->sd_lock_notify = 0;
 			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_CHANGE);
@@ -586,10 +586,9 @@ static int start_stop_unit(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 	case LOAD_MEDIUM:
 		if (check_card_ready(chip, lun)) {
 			return TRANSPORT_GOOD;
-		} else {
-			set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
-			TRACE_RET(chip, TRANSPORT_FAILED);
 		}
+		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
+		TRACE_RET(chip, TRANSPORT_FAILED);
 
 		break;
 	}
@@ -747,6 +746,7 @@ static void ms_mode_sense(struct rtsx_chip *chip, u8 cmd,
 	if (data_size > sys_info_offset) {
 		/* 96 Bytes Attribute Data */
 		int len = data_size - sys_info_offset;
+
 		len = (len < 96) ? len : 96;
 
 		memcpy(buf + sys_info_offset, ms_card->raw_sys_info, len);
@@ -1569,6 +1569,7 @@ static int get_variable(struct scsi_cmnd *srb, struct rtsx_chip *chip)
 		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
 	} else if (srb->cmnd[3] == 2) {
 		u8 tmp = chip->blink_led;
+
 		rtsx_stor_set_xfer_buf(&tmp, 1, srb);
 	} else {
 		set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
diff --git a/drivers/staging/rts5208/sd.c b/drivers/staging/rts5208/sd.c
index c79bea808698..c28a92773f05 100644
--- a/drivers/staging/rts5208/sd.c
+++ b/drivers/staging/rts5208/sd.c
@@ -427,7 +427,7 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 	memcpy(sd_card->raw_csd, rsp + 1, 15);
 
 	dev_dbg(rtsx_dev(chip), "CSD Response:\n");
-	RTSX_DUMP(sd_card->raw_csd, 16);
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 16, sd_card->raw_csd);
 
 	csd_ver = (rsp[1] & 0xc0) >> 6;
 	dev_dbg(rtsx_dev(chip), "csd_ver = %d\n", csd_ver);
@@ -476,6 +476,7 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 		if ((!CHK_SD_HCXC(sd_card)) || (csd_ver == 0)) {
 			u8 blk_size, c_size_mult;
 			u16 c_size;
+
 			blk_size = rsp[6] & 0x0F;
 			c_size =  ((u16)(rsp[7] & 0x03) << 10)
 					+ ((u16)rsp[8] << 2)
@@ -487,6 +488,7 @@ static int sd_check_csd(struct rtsx_chip *chip, char check_wp)
 				<< (blk_size - 9);
 		} else {
 			u32 total_sector = 0;
+
 			total_sector = (((u32)rsp[8] & 0x3f) << 16) |
 				((u32)rsp[9] << 8) | (u32)rsp[10];
 			sd_card->capacity = (total_sector + 1) << 10;
@@ -812,12 +814,10 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 			PHASE_NOT_RESET);
 		RTSX_WRITE_REG(chip, CLK_CTL, CHANGE_CLK, 0);
 	} else {
-#ifdef CONFIG_RTS5208_DEBUG
 		rtsx_read_register(chip, SD_VP_CTL, &val);
 		dev_dbg(rtsx_dev(chip), "SD_VP_CTL: 0x%x\n", val);
 		rtsx_read_register(chip, SD_DCMPS_CTL, &val);
 		dev_dbg(rtsx_dev(chip), "SD_DCMPS_CTL: 0x%x\n", val);
-#endif
 
 		if (ddr_rx) {
 			RTSX_WRITE_REG(chip, SD_VP_CTL, PHASE_CHANGE,
@@ -863,12 +863,11 @@ static int sd_change_phase(struct rtsx_chip *chip, u8 sample_point, u8 tune_dir)
 	return STATUS_SUCCESS;
 
 Fail:
-#ifdef CONFIG_RTS5208_DEBUG
 	rtsx_read_register(chip, SD_VP_CTL, &val);
 	dev_dbg(rtsx_dev(chip), "SD_VP_CTL: 0x%x\n", val);
 	rtsx_read_register(chip, SD_DCMPS_CTL, &val);
 	dev_dbg(rtsx_dev(chip), "SD_DCMPS_CTL: 0x%x\n", val);
-#endif
+
 	rtsx_write_register(chip, SD_DCMPS_CTL, DCMPS_CHANGE, 0);
 	rtsx_write_register(chip, SD_VP_CTL, PHASE_CHANGE, 0);
 	wait_timeout(10);
@@ -1060,7 +1059,7 @@ static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
 		TRACE_RET(chip, STATUS_FAIL);
 	}
 
-	RTSX_DUMP(buf, 64);
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 64, buf);
 
 	if (func_group == NO_ARGUMENT) {
 		sd_card->func_group1_mask = buf[0x0D];
@@ -1081,6 +1080,7 @@ static int sd_check_switch_mode(struct rtsx_chip *chip, u8 mode,
 		 * acceptable; bit[511:496] = 0x0000 means some error happened.
 		 */
 		u16 cc = ((u16)buf[0] << 8) | buf[1];
+
 		dev_dbg(rtsx_dev(chip), "Maximum current consumption: %dmA\n",
 			cc);
 		if ((cc == 0) || (cc > 800))
@@ -1553,6 +1553,7 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map,
 			new_block = 1;
 			if (cont_path_cnt) {
 				int idx = cont_path_cnt - 1;
+
 				path[idx].len = path[idx].end -
 					path[idx].start + 1;
 				path[idx].mid = path[idx].start +
@@ -1566,6 +1567,7 @@ static u8 sd_search_final_phase(struct rtsx_chip *chip, u32 phase_map,
 		goto Search_Finish;
 	} else {
 		int idx = cont_path_cnt - 1;
+
 		path[idx].len = path[idx].end - path[idx].start + 1;
 		path[idx].mid = path[idx].start + path[idx].len / 2;
 	}
@@ -2119,7 +2121,7 @@ static int sd_check_wp_state(struct rtsx_chip *chip)
 	}
 
 	dev_dbg(rtsx_dev(chip), "ACMD13:\n");
-	RTSX_DUMP(buf, 64);
+	dev_dbg(rtsx_dev(chip), "%*ph\n", 64, buf);
 
 	sd_card_type = ((u16)buf[2] << 8) | buf[3];
 	dev_dbg(rtsx_dev(chip), "sd_card_type = 0x%04x\n", sd_card_type);
@@ -2183,6 +2185,7 @@ Switch_Fail:
 						SD_RSP_TYPE_R4, rsp, 5);
 			if (retval == STATUS_SUCCESS) {
 				int func_num = (rsp[1] >> 4) & 0x07;
+
 				if (func_num) {
 					dev_dbg(rtsx_dev(chip), "SD_IO card (Function number: %d)!\n",
 						func_num);
diff --git a/drivers/staging/rts5208/trace.h b/drivers/staging/rts5208/trace.h
index fbb304a54acc..a9ab4077b283 100644
--- a/drivers/staging/rts5208/trace.h
+++ b/drivers/staging/rts5208/trace.h
@@ -84,12 +84,4 @@ static inline char *filename(char *path)
 #define TRACE_GOTO(chip, label)	goto label
 #endif
 
-#ifdef CONFIG_RTS5208_DEBUG
-#define RTSX_DUMP(buf, buf_len)					\
-	print_hex_dump(KERN_DEBUG, KBUILD_MODNAME ": ",		\
-		       DUMP_PREFIX_NONE, 16, 1, (buf), (buf_len), false)
-#else
-#define RTSX_DUMP(buf, buf_len)
-#endif
-
 #endif  /* __REALTEK_RTSX_TRACE_H */
diff --git a/drivers/staging/skein/skein_block.c b/drivers/staging/skein/skein_block.c
index 04ce1d005479..616364faf92e 100644
--- a/drivers/staging/skein/skein_block.c
+++ b/drivers/staging/skein/skein_block.c
@@ -34,7 +34,7 @@
 
 #ifdef SKEIN_DEBUG
 #define debug_save_tweak(ctx) { \
-                        ctx->h.tweak[0] = ts[0]; ctx->h.tweak[1] = ts[1]; }
+			ctx->h.tweak[0] = ts[0]; ctx->h.tweak[1] = ts[1]; }
 #else
 #define debug_save_tweak(ctx)
 #endif
diff --git a/drivers/staging/slicoss/slicoss.c b/drivers/staging/slicoss/slicoss.c
index f35fa3dfe22c..56ca3b6c1444 100644
--- a/drivers/staging/slicoss/slicoss.c
+++ b/drivers/staging/slicoss/slicoss.c
@@ -1982,7 +1982,6 @@ static void slic_rcv_handle_error(struct adapter *adapter,
 				adapter->if_events.uflow802++;
 		}
 	}
-	return;
 }
 
 #define TCP_OFFLOAD_FRAME_PUSHFLAG  0x10000000
@@ -2455,6 +2454,7 @@ static void slic_entry_remove(struct pci_dev *pcidev)
 	adapter->allocated = 0;
 	if (!card->adapters_allocated) {
 		struct sliccard *curr_card = slic_global.slic_card;
+
 		if (curr_card == card) {
 			slic_global.slic_card = card->next;
 		} else {
@@ -2551,6 +2551,7 @@ static int slic_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	case SIOCSLICTRACEDUMP:
 		{
 			u32 value;
+
 			DBG_IOCTL("slic_ioctl  SIOCSLIC_TRACE_DUMP\n");
 
 			if (copy_from_user(data, rq->ifr_data, 28)) {
diff --git a/drivers/staging/speakup/buffers.c b/drivers/staging/speakup/buffers.c
index 382973e8b80f..d45c8afb041d 100644
--- a/drivers/staging/speakup/buffers.c
+++ b/drivers/staging/speakup/buffers.c
@@ -101,6 +101,5 @@ EXPORT_SYMBOL_GPL(synth_buffer_peek);
 void synth_buffer_clear(void)
 {
 	buff_in = buff_out = synth_buffer;
-	return;
 }
 EXPORT_SYMBOL_GPL(synth_buffer_clear);
diff --git a/drivers/staging/speakup/i18n.h b/drivers/staging/speakup/i18n.h
index dd338f4218de..16a0871373d9 100644
--- a/drivers/staging/speakup/i18n.h
+++ b/drivers/staging/speakup/i18n.h
@@ -106,19 +106,23 @@ enum msg_index_t {
 	MSG_KEYNAMES_START,
 	MSG_KEYNAME_ESC = MSG_KEYNAMES_START,
 	MSG_KEYNAME_1, MSG_KEYNAME_2, MSG_KEYNAME_3, MSG_KEYNAME_4,
-	MSG_KEYNAME_5, MSG_KEYNAME_6, MSG_KEYNAME_7, MSG_KEYNAME_8, MSG_KEYNAME_9,
+	MSG_KEYNAME_5, MSG_KEYNAME_6, MSG_KEYNAME_7, MSG_KEYNAME_8,
+	MSG_KEYNAME_9,
 	MSG_KEYNAME_0, MSG_KEYNAME_DASH, MSG_KEYNAME_EQUAL, MSG_KEYNAME_BS,
 	MSG_KEYNAME_TAB,
-	MSG_KEYNAME_Q, MSG_KEYNAME_W, MSG_KEYNAME_E, MSG_KEYNAME_R, MSG_KEYNAME_T,
-	MSG_KEYNAME_Y, MSG_KEYNAME_U, MSG_KEYNAME_I, MSG_KEYNAME_O, MSG_KEYNAME_P,
+	MSG_KEYNAME_Q, MSG_KEYNAME_W, MSG_KEYNAME_E, MSG_KEYNAME_R,
+	MSG_KEYNAME_T, MSG_KEYNAME_Y, MSG_KEYNAME_U, MSG_KEYNAME_I,
+	MSG_KEYNAME_O, MSG_KEYNAME_P,
 	MSG_KEYNAME_LEFTBRACE, MSG_KEYNAME_RIGHTBRACE, MSG_KEYNAME_ENTER,
 	MSG_KEYNAME_LEFTCTRL, MSG_KEYNAME_A,
-	MSG_KEYNAME_S, MSG_KEYNAME_D, MSG_KEYNAME_F, MSG_KEYNAME_G, MSG_KEYNAME_H,
-	MSG_KEYNAME_J, MSG_KEYNAME_K, MSG_KEYNAME_L, MSG_KEYNAME_SEMICOLON,
+	MSG_KEYNAME_S, MSG_KEYNAME_D, MSG_KEYNAME_F, MSG_KEYNAME_G,
+	MSG_KEYNAME_H, MSG_KEYNAME_J, MSG_KEYNAME_K, MSG_KEYNAME_L,
+	MSG_KEYNAME_SEMICOLON,
 	MSG_KEYNAME_SINGLEQUOTE, MSG_KEYNAME_GRAVE,
-	MSG_KEYNAME_LEFTSHFT, MSG_KEYNAME_BACKSLASH, MSG_KEYNAME_Z, MSG_KEYNAME_X,
-	MSG_KEYNAME_C, MSG_KEYNAME_V, MSG_KEYNAME_B, MSG_KEYNAME_N, MSG_KEYNAME_M,
-	MSG_KEYNAME_COMMA, MSG_KEYNAME_DOT, MSG_KEYNAME_SLASH, MSG_KEYNAME_RIGHTSHFT,
+	MSG_KEYNAME_LEFTSHFT, MSG_KEYNAME_BACKSLASH, MSG_KEYNAME_Z,
+	MSG_KEYNAME_X, MSG_KEYNAME_C, MSG_KEYNAME_V, MSG_KEYNAME_B,
+	MSG_KEYNAME_N, MSG_KEYNAME_M, MSG_KEYNAME_COMMA, MSG_KEYNAME_DOT,
+	MSG_KEYNAME_SLASH, MSG_KEYNAME_RIGHTSHFT,
 	MSG_KEYNAME_KPSTAR,
 	MSG_KEYNAME_LEFTALT, MSG_KEYNAME_SPACE, MSG_KEYNAME_CAPSLOCK,
 	MSG_KEYNAME_F1, MSG_KEYNAME_F2,
@@ -144,8 +148,9 @@ enum msg_index_t {
 	MSG_KEYNAME_INS,
 	MSG_KEYNAME_DEL, MSG_KEYNAME_MACRO, MSG_KEYNAME_MUTE,
 	MSG_KEYNAME_VOLDOWN, MSG_KEYNAME_VOLUP,
-	MSG_KEYNAME_POWER, MSG_KEYNAME_KPEQUAL, MSG_KEYNAME_KPPLUSDASH, MSG_KEYNAME_PAUSE, MSG_KEYNAME_F21,
-	MSG_KEYNAME_F22, MSG_KEYNAME_F23, MSG_KEYNAME_F24, MSG_KEYNAME_KPCOMMA, MSG_KEYNAME_LEFTMETA,
+	MSG_KEYNAME_POWER, MSG_KEYNAME_KPEQUAL, MSG_KEYNAME_KPPLUSDASH,
+	MSG_KEYNAME_PAUSE, MSG_KEYNAME_F21, MSG_KEYNAME_F22, MSG_KEYNAME_F23,
+	MSG_KEYNAME_F24, MSG_KEYNAME_KPCOMMA, MSG_KEYNAME_LEFTMETA,
 	MSG_KEYNAME_RIGHTMETA, MSG_KEYNAME_COMPOSE, MSG_KEYNAME_STOP,
 	MSG_KEYNAME_AGAIN, MSG_KEYNAME_PROPS,
 	MSG_KEYNAME_UNDO, MSG_KEYNAME_FRONT, MSG_KEYNAME_COPY, MSG_KEYNAME_OPEN,
@@ -180,7 +185,8 @@ enum msg_index_t {
 	MSG_FUNCNAME_CHAR_PHONETIC, MSG_FUNCNAME_CHAR_PREVIOUS,
 	MSG_FUNCNAME_CURSOR_PARK, MSG_FUNCNAME_CUT,
 	MSG_FUNCNAME_EDIT_DELIM, MSG_FUNCNAME_EDIT_EXNUM,
-	MSG_FUNCNAME_EDIT_MOST, MSG_FUNCNAME_EDIT_REPEATS, MSG_FUNCNAME_EDIT_SOME,
+	MSG_FUNCNAME_EDIT_MOST, MSG_FUNCNAME_EDIT_REPEATS,
+	MSG_FUNCNAME_EDIT_SOME,
 	MSG_FUNCNAME_GOTO, MSG_FUNCNAME_GOTO_BOTTOM, MSG_FUNCNAME_GOTO_LEFT,
 	MSG_FUNCNAME_GOTO_RIGHT, MSG_FUNCNAME_GOTO_TOP, MSG_FUNCNAME_HELP,
 	MSG_FUNCNAME_LINE_SAY_CURRENT, MSG_FUNCNAME_LINE_SAY_NEXT,
diff --git a/drivers/staging/speakup/keyhelp.c b/drivers/staging/speakup/keyhelp.c
index 5091625a4901..94756742136f 100644
--- a/drivers/staging/speakup/keyhelp.c
+++ b/drivers/staging/speakup/keyhelp.c
@@ -64,6 +64,7 @@ static void build_key_data(void)
 	u_char *kp, counters[MAXFUNCS], ch, ch1;
 	u_short *p_key = key_data, key;
 	int i, offset = 1;
+
 	nstates = (int)(state_tbl[-1]);
 	memset(counters, 0, sizeof(counters));
 	memset(key_offsets, 0, sizeof(key_offsets));
@@ -112,6 +113,7 @@ static void build_key_data(void)
 static void say_key(int key)
 {
 	int i, state = key >> 8;
+
 	key &= 0xff;
 	for (i = 0; i < 6; i++) {
 		if (state & masks[i])
@@ -127,9 +129,11 @@ static int help_init(void)
 	char start = SPACE;
 	int i;
 	int num_funcs = MSG_FUNCNAMES_END - MSG_FUNCNAMES_START + 1;
-state_tbl = spk_our_keys[0]+SHIFT_TBL_SIZE+2;
+
+	state_tbl = spk_our_keys[0]+SHIFT_TBL_SIZE+2;
 	for (i = 0; i < num_funcs; i++) {
 		char *cur_funcname = spk_msg_get(MSG_FUNCNAMES_START + i);
+
 		if (start == *cur_funcname)
 			continue;
 		start = *cur_funcname;
@@ -144,6 +148,7 @@ int spk_handle_help(struct vc_data *vc, u_char type, u_char ch, u_short key)
 	char *name;
 	u_char func, *kp;
 	u_short *p_keys, val;
+
 	if (letter_offsets[0] == -1)
 		help_init();
 	if (type == KT_LATIN) {
diff --git a/drivers/staging/speakup/kobjects.c b/drivers/staging/speakup/kobjects.c
index 2ef7f6f201a8..bcc7f62654f4 100644
--- a/drivers/staging/speakup/kobjects.c
+++ b/drivers/staging/speakup/kobjects.c
@@ -232,6 +232,7 @@ static ssize_t keymap_show(struct kobject *kobj, struct kobj_attribute *attr,
 	u_char *cp1;
 	u_char ch;
 	unsigned long flags;
+
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	cp1 = spk_key_buf + SHIFT_TBL_SIZE;
 	num_keys = (int)(*cp1);
@@ -905,9 +906,11 @@ static struct kobj_attribute spell_delay_attribute =
 static struct kobj_attribute announcements_attribute =
 	__ATTR(announcements, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute characters_attribute =
-	__ATTR(characters, S_IWUSR|S_IRUGO, chars_chartab_show, chars_chartab_store);
+	__ATTR(characters, S_IWUSR|S_IRUGO, chars_chartab_show,
+	       chars_chartab_store);
 static struct kobj_attribute chartab_attribute =
-	__ATTR(chartab, S_IWUSR|S_IRUGO, chars_chartab_show, chars_chartab_store);
+	__ATTR(chartab, S_IWUSR|S_IRUGO, chars_chartab_show,
+	       chars_chartab_store);
 static struct kobj_attribute ctl_keys_attribute =
 	__ATTR(ctl_keys, S_IWUSR|S_IRUGO, message_show, message_store);
 static struct kobj_attribute colors_attribute =
diff --git a/drivers/staging/speakup/main.c b/drivers/staging/speakup/main.c
index 0cd3cdba8644..3f30a1b6e72c 100644
--- a/drivers/staging/speakup/main.c
+++ b/drivers/staging/speakup/main.c
@@ -78,7 +78,8 @@ int spk_key_echo, spk_say_word_ctl;
 int spk_say_ctrl, spk_bell_pos;
 short spk_punc_mask;
 int spk_punc_level, spk_reading_punc;
-char spk_str_caps_start[MAXVARLEN + 1] = "\0", spk_str_caps_stop[MAXVARLEN + 1] = "\0";
+char spk_str_caps_start[MAXVARLEN + 1] = "\0";
+char spk_str_caps_stop[MAXVARLEN + 1] = "\0";
 const struct st_bits_data spk_punc_info[] = {
 	{"none", "", 0},
 	{"some", "/$%&@", SOME},
@@ -282,6 +283,7 @@ static void bleep(u_short val)
 	};
 	short freq;
 	int time = spk_bleep_time;
+
 	freq = vals[val % 12];
 	if (val > 11)
 		freq *= (1 << (val / 12));
@@ -305,6 +307,7 @@ static void speakup_shut_up(struct vc_data *vc)
 static void speech_kill(struct vc_data *vc)
 {
 	char val = synth->is_alive(synth);
+
 	if (val == 0)
 		return;
 
@@ -393,6 +396,7 @@ static void say_attributes(struct vc_data *vc)
 {
 	int fg = spk_attr & 0x0f;
 	int bg = spk_attr >> 4;
+
 	if (fg > 8) {
 		synth_printf("%s ", spk_msg_get(MSG_BRIGHT));
 		fg -= 8;
@@ -426,6 +430,7 @@ static void speak_char(u_char ch)
 {
 	char *cp = spk_characters[ch];
 	struct var_t *direct = spk_get_var(DIRECT);
+
 	if (direct && direct->u.n.value) {
 		if (IS_CHAR(ch, B_CAP)) {
 			spk_pitch_shift++;
@@ -459,6 +464,7 @@ static void speak_char(u_char ch)
 static u16 get_char(struct vc_data *vc, u16 *pos, u_char *attribs)
 {
 	u16 ch = ' ';
+
 	if (vc && pos) {
 		u16 w = scr_readw(pos);
 		u16 c = w & 0xff;
@@ -475,6 +481,7 @@ static u16 get_char(struct vc_data *vc, u16 *pos, u_char *attribs)
 static void say_char(struct vc_data *vc)
 {
 	u_short ch;
+
 	spk_old_attr = spk_attr;
 	ch = get_char(vc, (u_short *) spk_pos, &spk_attr);
 	if (spk_attr != spk_old_attr) {
@@ -489,6 +496,7 @@ static void say_char(struct vc_data *vc)
 static void say_phonetic_char(struct vc_data *vc)
 {
 	u_short ch;
+
 	spk_old_attr = spk_attr;
 	ch = get_char(vc, (u_short *) spk_pos, &spk_attr);
 	if (isascii(ch) && isalpha(ch)) {
@@ -539,6 +547,7 @@ static u_long get_word(struct vc_data *vc)
 	char ch;
 	u_short attr_ch;
 	u_char temp;
+
 	spk_old_attr = spk_attr;
 	ch = (char)get_char(vc, (u_short *) tmp_pos, &temp);
 
@@ -582,6 +591,7 @@ static void say_word(struct vc_data *vc)
 {
 	u_long cnt = get_word(vc);
 	u_short saved_punc_mask = spk_punc_mask;
+
 	if (cnt == 0)
 		return;
 	spk_punc_mask = PUNC;
@@ -595,6 +605,7 @@ static void say_prev_word(struct vc_data *vc)
 	u_char temp;
 	char ch;
 	u_short edge_said = 0, last_state = 0, state = 0;
+
 	spk_parked |= 0x01;
 
 	if (spk_x == 0) {
@@ -647,8 +658,8 @@ static void say_next_word(struct vc_data *vc)
 	u_char temp;
 	char ch;
 	u_short edge_said = 0, last_state = 2, state = 0;
-	spk_parked |= 0x01;
 
+	spk_parked |= 0x01;
 	if (spk_x == vc->vc_cols - 1 && spk_y == vc->vc_rows - 1) {
 		announce_edge(vc, edge_bottom);
 		return;
@@ -688,6 +699,7 @@ static void spell_word(struct vc_data *vc)
 	char *cp = buf, *str_cap = spk_str_caps_stop;
 	char *cp1, *last_cap = spk_str_caps_stop;
 	u_char ch;
+
 	if (!get_word(vc))
 		return;
 	while ((ch = (u_char) *cp)) {
@@ -746,6 +758,7 @@ static void say_line(struct vc_data *vc)
 	int i = get_line(vc);
 	char *cp;
 	u_short saved_punc_mask = spk_punc_mask;
+
 	if (i == 0) {
 		synth_printf("%s\n", spk_msg_get(MSG_BLANK));
 		return;
@@ -792,6 +805,7 @@ static int say_from_to(struct vc_data *vc, u_long from, u_long to,
 	int i = 0;
 	u_char tmp;
 	u_short saved_punc_mask = spk_punc_mask;
+
 	spk_old_attr = spk_attr;
 	spk_attr = get_attributes((u_short *) from);
 	while (from < to) {
@@ -820,6 +834,7 @@ static void say_line_from_to(struct vc_data *vc, u_long from, u_long to,
 {
 	u_long start = vc->vc_origin + (spk_y * vc->vc_size_row);
 	u_long end = start + (to * 2);
+
 	start += from * 2;
 	if (say_from_to(vc, start, end, read_punc) <= 0)
 		if (cursor_track != read_all_mode)
@@ -903,6 +918,7 @@ static int get_sentence_buf(struct vc_data *vc, int read_punc)
 static void say_screen_from_to(struct vc_data *vc, u_long from, u_long to)
 {
 	u_long start = vc->vc_origin, end;
+
 	if (from > 0)
 		start += from * vc->vc_size_row;
 	if (to > vc->vc_rows)
@@ -922,6 +938,7 @@ static void say_screen(struct vc_data *vc)
 static void speakup_win_say(struct vc_data *vc)
 {
 	u_long start, end, from, to;
+
 	if (win_start < 2) {
 		synth_printf("%s\n", spk_msg_get(MSG_NO_WINDOW));
 		return;
@@ -972,6 +989,7 @@ static void say_first_char(struct vc_data *vc)
 {
 	int i, len = get_line(vc);
 	u_char ch;
+
 	spk_parked |= 0x01;
 	if (len == 0) {
 		synth_printf("%s\n", spk_msg_get(MSG_BLANK));
@@ -991,6 +1009,7 @@ static void say_last_char(struct vc_data *vc)
 {
 	int len = get_line(vc);
 	u_char ch;
+
 	spk_parked |= 0x01;
 	if (len == 0) {
 		synth_printf("%s\n", spk_msg_get(MSG_BLANK));
@@ -1015,6 +1034,7 @@ static void say_char_num(struct vc_data *vc)
 {
 	u_char tmp;
 	u_short ch = get_char(vc, (u_short *) spk_pos, &tmp);
+
 	ch &= 0xff;
 	synth_printf(spk_msg_get(MSG_CHAR_INFO), ch, ch);
 }
@@ -1049,6 +1069,7 @@ static void spkup_write(const char *in_buf, int count)
 	static u_char ch = '\0', old_ch = '\0';
 	static u_short char_type, last_type;
 	int in_count = count;
+
 	spk_keydown = 0;
 	while (count--) {
 		if (cursor_track == read_all_mode) {
@@ -1126,6 +1147,7 @@ static DEFINE_TIMER(cursor_timer, cursor_done, 0, 0);
 static void do_handle_shift(struct vc_data *vc, u_char value, char up_flag)
 {
 	unsigned long flags;
+
 	if (synth == NULL || up_flag || spk_killed)
 		return;
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
@@ -1156,6 +1178,7 @@ static void do_handle_shift(struct vc_data *vc, u_char value, char up_flag)
 static void do_handle_latin(struct vc_data *vc, u_char value, char up_flag)
 {
 	unsigned long flags;
+
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	if (up_flag) {
 		spk_lastkey = spk_keydown = 0;
@@ -1181,6 +1204,7 @@ int spk_set_key_info(const u_char *key_info, u_char *k_buffer)
 	const u_char *cp = key_info;
 	u_char *cp1 = k_buffer;
 	u_char ch, version, num_keys;
+
 	version = *cp++;
 	if (version != KEY_MAP_VER)
 		return -1;
@@ -1263,6 +1287,7 @@ static const struct st_bits_data *pb_edit;
 static int edit_bits(struct vc_data *vc, u_char type, u_char ch, u_short key)
 {
 	short mask = pb_edit->mask, ch_type = spk_chartab[ch];
+
 	if (type != KT_LATIN || (ch_type & B_NUM) || ch < SPACE)
 		return -1;
 	if (ch == SPACE) {
@@ -1448,6 +1473,7 @@ static void handle_cursor_read_all(struct vc_data *vc, int command)
 static int pre_handle_cursor(struct vc_data *vc, u_char value, char up_flag)
 {
 	unsigned long flags;
+
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	if (cursor_track == read_all_mode) {
 		spk_parked &= 0xfe;
@@ -1531,6 +1557,7 @@ static void reset_highlight_buffers(struct vc_data *vc)
 {
 	int i;
 	int vc_num = vc->vc_num;
+
 	for (i = 0; i < 8; i++)
 		speakup_console[vc_num]->ht.highsize[i] = 0;
 }
@@ -1549,6 +1576,7 @@ static int count_highlight_color(struct vc_data *vc)
 	for (i = 0; i < vc->vc_rows; i++) {
 		u16 *end = start + vc->vc_cols * 2;
 		u16 *ptr;
+
 		for (ptr = start; ptr < end; ptr++) {
 			ch = get_attributes(ptr);
 			bg = (ch & 0x70) >> 4;
@@ -1593,6 +1621,7 @@ static int speak_highlight(struct vc_data *vc)
 {
 	int hc, d;
 	int vc_num = vc->vc_num;
+
 	if (count_highlight_color(vc) == 1)
 		return 0;
 	hc = get_highlight_color(vc);
@@ -1617,6 +1646,7 @@ static void cursor_done(u_long data)
 {
 	struct vc_data *vc = vc_cons[cursor_con].d;
 	unsigned long flags;
+
 	del_timer(&cursor_timer);
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	if (cursor_con != fg_console) {
@@ -1656,6 +1686,7 @@ out:
 static void speakup_bs(struct vc_data *vc)
 {
 	unsigned long flags;
+
 	if (!speakup_console[vc->vc_num])
 		return;
 	if (!spin_trylock_irqsave(&speakup_info.spinlock, flags))
@@ -1679,6 +1710,7 @@ static void speakup_bs(struct vc_data *vc)
 static void speakup_con_write(struct vc_data *vc, const char *str, int len)
 {
 	unsigned long flags;
+
 	if ((vc->vc_num != fg_console) || spk_shut_up || synth == NULL)
 		return;
 	if (!spin_trylock_irqsave(&speakup_info.spinlock, flags))
@@ -1707,6 +1739,7 @@ static void speakup_con_write(struct vc_data *vc, const char *str, int len)
 static void speakup_con_update(struct vc_data *vc)
 {
 	unsigned long flags;
+
 	if (speakup_console[vc->vc_num] == NULL || spk_parked)
 		return;
 	if (!spin_trylock_irqsave(&speakup_info.spinlock, flags))
@@ -1721,6 +1754,7 @@ static void do_handle_spec(struct vc_data *vc, u_char value, char up_flag)
 	unsigned long flags;
 	int on_off = 2;
 	char *label;
+
 	if (synth == NULL || up_flag || spk_killed)
 		return;
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
@@ -1762,6 +1796,7 @@ static int inc_dec_var(u_char value)
 	char *pn;
 	int var_id = (int)value - VAR_START;
 	int how = (var_id & 1) ? E_INC : E_DEC;
+
 	var_id = var_id / 2 + FIRST_SET_VAR;
 	p_header = spk_get_var_header(var_id);
 	if (p_header == NULL)
@@ -1788,6 +1823,7 @@ static int inc_dec_var(u_char value)
 static void speakup_win_set(struct vc_data *vc)
 {
 	char info[40];
+
 	if (win_start > 1) {
 		synth_printf("%s\n", spk_msg_get(MSG_WINDOW_ALREADY_SET));
 		return;
@@ -1842,6 +1878,7 @@ static void speakup_win_enable(struct vc_data *vc)
 static void speakup_bits(struct vc_data *vc)
 {
 	int val = this_speakup_key - (FIRST_EDIT_BITS - 1);
+
 	if (spk_special_handler != NULL || val < 1 || val > 6) {
 		synth_printf("%s\n", spk_msg_get(MSG_ERROR));
 		return;
@@ -1939,7 +1976,6 @@ static void speakup_goto(struct vc_data *vc)
 	}
 	synth_printf("%s\n", spk_msg_get(MSG_GOTO));
 	spk_special_handler = handle_goto;
-	return;
 }
 
 static void speakup_help(struct vc_data *vc)
@@ -2011,6 +2047,7 @@ speakup_key(struct vc_data *vc, int shift_state, int keycode, u_short keysym,
 	u_char type = KTYP(keysym), value = KVAL(keysym), new_key = 0;
 	u_char shift_info, offset;
 	int ret = 0;
+
 	if (synth == NULL)
 		return 0;
 
@@ -2161,6 +2198,7 @@ static int keyboard_notifier_call(struct notifier_block *nb,
 	case KBD_POST_KEYSYM:{
 			unsigned char type = KTYP(param->value) - 0xf0;
 			unsigned char val = KVAL(param->value);
+
 			switch (type) {
 			case KT_SHIFT:
 				do_handle_shift(vc, val, up);
@@ -2187,6 +2225,7 @@ static int vt_notifier_call(struct notifier_block *nb,
 {
 	struct vt_notifier_param *param = _param;
 	struct vc_data *vc = param->vc;
+
 	switch (code) {
 	case VT_ALLOCATE:
 		if (vc->vc_mode == KD_TEXT)
@@ -2200,6 +2239,7 @@ static int vt_notifier_call(struct notifier_block *nb,
 			speakup_bs(vc);
 		else if (param->c < 0x100) {
 			char d = param->c;
+
 			speakup_con_write(vc, &d, 1);
 		}
 		break;
diff --git a/drivers/staging/speakup/selection.c b/drivers/staging/speakup/selection.c
index ca04d3669acc..507fc9a1776e 100644
--- a/drivers/staging/speakup/selection.c
+++ b/drivers/staging/speakup/selection.c
@@ -66,6 +66,7 @@ int speakup_set_selection(struct tty_struct *tty)
 	if (ps > pe) {
 		/* make sel_start <= sel_end */
 		int tmp = ps;
+
 		ps = pe;
 		pe = tmp;
 	}
diff --git a/drivers/staging/speakup/serialio.c b/drivers/staging/speakup/serialio.c
index c62d74c47906..1d9d51bdf517 100644
--- a/drivers/staging/speakup/serialio.c
+++ b/drivers/staging/speakup/serialio.c
@@ -83,6 +83,7 @@ static irqreturn_t synth_readbuf_handler(int irq, void *dev_id)
 /*printk(KERN_ERR "in irq\n"); */
 /*pr_warn("in IRQ\n"); */
 	int c;
+
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	while (inb_p(speakup_info.port_tts + UART_LSR) & UART_LSR_DR) {
 
@@ -137,6 +138,7 @@ void spk_stop_serial_interrupt(void)
 int spk_wait_for_xmitr(void)
 {
 	int tmout = SPK_XMITR_TIMEOUT;
+
 	if ((synth->alive) && (timeouts >= NUM_DISABLE_TIMEOUTS)) {
 		pr_warn("%s: too many timeouts, deactivating speakup\n",
 			synth->long_name);
diff --git a/drivers/staging/speakup/serialio.h b/drivers/staging/speakup/serialio.h
index 0a937732a190..317bb8432a04 100644
--- a/drivers/staging/speakup/serialio.h
+++ b/drivers/staging/speakup/serialio.h
@@ -4,7 +4,7 @@
 #include <linux/serial.h>	/* for rs_table, serial constants */
 #include <linux/serial_reg.h>	/* for more serial constants */
 #ifndef __sparc__
-#include <asm/serial.h>
+#include <linux/serial.h>
 #endif
 
 /*
diff --git a/drivers/staging/speakup/speakup_acntpc.c b/drivers/staging/speakup/speakup_acntpc.c
index 31f952b9049b..f0bfd9e99191 100644
--- a/drivers/staging/speakup/speakup_acntpc.c
+++ b/drivers/staging/speakup/speakup_acntpc.c
@@ -152,8 +152,10 @@ static inline bool synth_full(void)
 static const char *synth_immediate(struct spk_synth *synth, const char *buf)
 {
 	u_char ch;
+
 	while ((ch = *buf)) {
 		int timeout = SPK_XMITR_TIMEOUT;
+
 		if (ch == '\n')
 			ch = PROCSPEECH;
 		if (synth_full())
@@ -257,6 +259,7 @@ static int synth_probe(struct spk_synth *synth)
 {
 	unsigned int port_val = 0;
 	int i = 0;
+
 	pr_info("Probing for %s.\n", synth->long_name);
 	if (port_forced) {
 		speakup_info.port_tts = port_forced;
diff --git a/drivers/staging/speakup/speakup_audptr.c b/drivers/staging/speakup/speakup_audptr.c
index 362f9747e48d..5cbaec8335fe 100644
--- a/drivers/staging/speakup/speakup_audptr.c
+++ b/drivers/staging/speakup/speakup_audptr.c
@@ -132,6 +132,7 @@ static struct spk_synth synth_audptr = {
 static void synth_flush(struct spk_synth *synth)
 {
 	int timeout = SPK_XMITR_TIMEOUT;
+
 	while (spk_serial_tx_busy()) {
 		if (!--timeout)
 			break;
@@ -145,6 +146,7 @@ static void synth_version(struct spk_synth *synth)
 {
 	unsigned char test = 0;
 	char synth_id[40] = "";
+
 	spk_synth_immediate(synth, "\x05[Q]");
 	synth_id[test] = spk_serial_in();
 	if (synth_id[test] == 'A') {
diff --git a/drivers/staging/speakup/speakup_decext.c b/drivers/staging/speakup/speakup_decext.c
index 67b7de1d8c75..555029036dc2 100644
--- a/drivers/staging/speakup/speakup_decext.c
+++ b/drivers/staging/speakup/speakup_decext.c
@@ -39,6 +39,7 @@ static unsigned char last_char;
 static inline u_char get_last_char(void)
 {
 	u_char avail = inb_p(speakup_info.port_tts + UART_LSR) & UART_LSR_DR;
+
 	if (avail)
 		last_char = inb_p(speakup_info.port_tts + UART_RX);
 	return last_char;
@@ -203,7 +204,7 @@ static void do_catch_up(struct spk_synth *synth)
 		else if (ch <= SPACE) {
 			if (!in_escape && strchr(",.!?;:", last))
 				spk_serial_out(PROCSPEECH);
-			if (jiffies >= jiff_max) {
+			if (time_after_eq(jiffies, jiff_max)) {
 				if (!in_escape)
 					spk_serial_out(PROCSPEECH);
 				spin_lock_irqsave(&speakup_info.spinlock, flags);
diff --git a/drivers/staging/speakup/speakup_decpc.c b/drivers/staging/speakup/speakup_decpc.c
index 67678d8888c2..7c9c432e48d9 100644
--- a/drivers/staging/speakup/speakup_decpc.c
+++ b/drivers/staging/speakup/speakup_decpc.c
@@ -260,6 +260,7 @@ static void dt_sendcmd(u_int cmd)
 static int dt_waitbit(int bit)
 {
 	int timeout = 100;
+
 	while (--timeout > 0) {
 		if ((dt_getstatus() & bit) == bit)
 			return 1;
@@ -271,6 +272,7 @@ static int dt_waitbit(int bit)
 static int dt_wait_dma(void)
 {
 	int timeout = 100, state = dma_state;
+
 	if (!dt_waitbit(STAT_dma_ready))
 		return 0;
 	while (--timeout > 0) {
@@ -285,6 +287,7 @@ static int dt_wait_dma(void)
 static int dt_ctrl(u_int cmd)
 {
 	int timeout = 10;
+
 	if (!dt_waitbit(STAT_cmd_ready))
 		return -1;
 	outb_p(0, speakup_info.port_tts+2);
@@ -304,6 +307,7 @@ static int dt_ctrl(u_int cmd)
 static void synth_flush(struct spk_synth *synth)
 {
 	int timeout = 10;
+
 	if (is_flushing)
 		return;
 	is_flushing = 4;
@@ -346,6 +350,7 @@ static int dt_sendchar(char ch)
 static int testkernel(void)
 {
 	int status = 0;
+
 	if (dt_getstatus() == 0xffff) {
 		status = -1;
 		goto oops;
@@ -415,7 +420,7 @@ static void do_catch_up(struct spk_synth *synth)
 		else if (ch <= SPACE) {
 			if (!in_escape && strchr(",.!?;:", last))
 				dt_sendchar(PROCSPEECH);
-			if (jiffies >= jiff_max) {
+			if (time_after_eq(jiffies, jiff_max)) {
 				if (!in_escape)
 					dt_sendchar(PROCSPEECH);
 				spin_lock_irqsave(&speakup_info.spinlock, flags);
@@ -437,6 +442,7 @@ static void do_catch_up(struct spk_synth *synth)
 static const char *synth_immediate(struct spk_synth *synth, const char *buf)
 {
 	u_char ch;
+
 	while ((ch = *buf)) {
 		if (ch == '\n')
 			ch = PROCSPEECH;
@@ -450,6 +456,7 @@ static const char *synth_immediate(struct spk_synth *synth, const char *buf)
 static int synth_probe(struct spk_synth *synth)
 {
 	int i = 0, failed = 0;
+
 	pr_info("Probing for %s.\n", synth->long_name);
 	for (i = 0; synth_portlist[i]; i++) {
 		if (synth_request_region(synth_portlist[i], SYNTH_IO_EXTENT)) {
diff --git a/drivers/staging/speakup/speakup_dectlk.c b/drivers/staging/speakup/speakup_dectlk.c
index c07c9670eef4..69e7c2b41b94 100644
--- a/drivers/staging/speakup/speakup_dectlk.c
+++ b/drivers/staging/speakup/speakup_dectlk.c
@@ -169,6 +169,7 @@ static u_char lastind;
 static unsigned char get_index(void)
 {
 	u_char rv;
+
 	rv = lastind;
 	lastind = 0;
 	return rv;
@@ -180,6 +181,7 @@ static void read_buff_add(u_char c)
 
 	if (c == 0x01) {
 		unsigned long flags;
+
 		spin_lock_irqsave(&flush_lock, flags);
 		is_flushing = 0;
 		wake_up_interruptible(&flush);
diff --git a/drivers/staging/speakup/speakup_dtlk.c b/drivers/staging/speakup/speakup_dtlk.c
index 98d1f497e4e0..d7d515273896 100644
--- a/drivers/staging/speakup/speakup_dtlk.c
+++ b/drivers/staging/speakup/speakup_dtlk.c
@@ -174,6 +174,7 @@ static inline bool synth_full(void)
 static void spk_out(const char ch)
 {
 	int timeout = SPK_XMITR_TIMEOUT;
+
 	while (!synth_writable()) {
 		if (!--timeout)
 			break;
@@ -246,6 +247,7 @@ static void do_catch_up(struct spk_synth *synth)
 static const char *synth_immediate(struct spk_synth *synth, const char *buf)
 {
 	u_char ch;
+
 	while ((ch = (u_char)*buf)) {
 		if (synth_full())
 			return buf;
@@ -267,6 +269,7 @@ static void synth_flush(struct spk_synth *synth)
 static char synth_read_tts(void)
 {
 	u_char ch;
+
 	while (!synth_readable())
 		cpu_relax();
 	ch = synth_status & 0x7f;
@@ -283,6 +286,7 @@ static struct synth_settings *synth_interrogate(struct spk_synth *synth)
 	static char buf[sizeof(struct synth_settings) + 1];
 	int total, i;
 	static struct synth_settings status;
+
 	synth_immediate(synth, "\x18\x01?");
 	for (total = 0, i = 0; i < 50; i++) {
 		buf[total] = synth_read_tts();
@@ -324,6 +328,7 @@ static int synth_probe(struct spk_synth *synth)
 		unsigned int port_val = 0;
 	int i = 0;
 	struct synth_settings *sp;
+
 	pr_info("Probing for DoubleTalk.\n");
 	if (port_forced) {
 		speakup_info.port_tts = port_forced;
diff --git a/drivers/staging/speakup/speakup_keypc.c b/drivers/staging/speakup/speakup_keypc.c
index 9d30c1945f92..4ed38898a17a 100644
--- a/drivers/staging/speakup/speakup_keypc.c
+++ b/drivers/staging/speakup/speakup_keypc.c
@@ -143,6 +143,7 @@ static inline bool synth_full(void)
 static char *oops(void)
 {
 	int s1, s2, s3, s4;
+
 	s1 = inb_p(synth_port);
 	s2 = inb_p(synth_port+1);
 	s3 = inb_p(synth_port+2);
@@ -155,6 +156,7 @@ static const char *synth_immediate(struct spk_synth *synth, const char *buf)
 {
 	u_char ch;
 	int timeout;
+
 	while ((ch = *buf)) {
 		if (ch == '\n')
 			ch = PROCSPEECH;
@@ -264,6 +266,7 @@ static int synth_probe(struct spk_synth *synth)
 {
 	unsigned int port_val = 0;
 	int i = 0;
+
 	pr_info("Probing for %s.\n", synth->long_name);
 	if (port_forced) {
 		synth_port = port_forced;
diff --git a/drivers/staging/speakup/speakup_ltlk.c b/drivers/staging/speakup/speakup_ltlk.c
index d6de72295d33..377a6e2b8b93 100644
--- a/drivers/staging/speakup/speakup_ltlk.c
+++ b/drivers/staging/speakup/speakup_ltlk.c
@@ -141,6 +141,7 @@ static void synth_interrogate(struct spk_synth *synth)
 {
 	unsigned char *t, i;
 	unsigned char buf[50], rom_v[20];
+
 	spk_synth_immediate(synth, "\x18\x01?");
 	for (i = 0; i < 50; i++) {
 		buf[i] = spk_serial_in();
diff --git a/drivers/staging/speakup/speakup_soft.c b/drivers/staging/speakup/speakup_soft.c
index 9ed726509261..e6e93fbd183a 100644
--- a/drivers/staging/speakup/speakup_soft.c
+++ b/drivers/staging/speakup/speakup_soft.c
@@ -192,6 +192,7 @@ static int softsynth_open(struct inode *inode, struct file *fp)
 static int softsynth_close(struct inode *inode, struct file *fp)
 {
 	unsigned long flags;
+
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
 	synth_soft.alive = 0;
 	init_pos = 0;
@@ -283,6 +284,7 @@ static unsigned int softsynth_poll(struct file *fp,
 {
 	unsigned long flags;
 	int ret = 0;
+
 	poll_wait(fp, &speakup_event, wait);
 
 	spin_lock_irqsave(&speakup_info.spinlock, flags);
@@ -295,6 +297,7 @@ static unsigned int softsynth_poll(struct file *fp,
 static unsigned char get_index(void)
 {
 	int rv;
+
 	rv = last_index;
 	last_index = 0;
 	return rv;
diff --git a/drivers/staging/speakup/speakup_spkout.c b/drivers/staging/speakup/speakup_spkout.c
index 77f2dc2c3d97..bccddf8b89fd 100644
--- a/drivers/staging/speakup/speakup_spkout.c
+++ b/drivers/staging/speakup/speakup_spkout.c
@@ -131,6 +131,7 @@ static struct spk_synth synth_spkout = {
 static void synth_flush(struct spk_synth *synth)
 {
 	int timeout = SPK_XMITR_TIMEOUT;
+
 	while (spk_serial_tx_busy()) {
 		if (!--timeout)
 			break;
diff --git a/drivers/staging/speakup/spk_types.h b/drivers/staging/speakup/spk_types.h
index 3ac552c1236e..8c565c94b8b2 100644
--- a/drivers/staging/speakup/spk_types.h
+++ b/drivers/staging/speakup/spk_types.h
@@ -39,10 +39,10 @@ enum var_id_t {
 	DELIM, REPEATS, EXNUMBER,
 	DELAY, TRIGGER, JIFFY, FULL, /* all timers must be together */
 	BLEEP_TIME, CURSOR_TIME, BELL_POS,
-SAY_CONTROL, SAY_WORD_CTL, NO_INTERRUPT, KEY_ECHO,
+	SAY_CONTROL, SAY_WORD_CTL, NO_INTERRUPT, KEY_ECHO,
 	SPELL_DELAY, PUNC_LEVEL, READING_PUNC,
 	ATTRIB_BLEEP, BLEEPS,
- RATE, PITCH, VOL, TONE, PUNCT, VOICE, FREQUENCY, LANG, DIRECT,
+	RATE, PITCH, VOL, TONE, PUNCT, VOICE, FREQUENCY, LANG, DIRECT,
 	CAPS_START, CAPS_STOP, CHARTAB,
 	MAXVARS
 };
diff --git a/drivers/staging/speakup/spkguide.txt b/drivers/staging/speakup/spkguide.txt
index f3210571e396..b699de3c649f 100644
--- a/drivers/staging/speakup/spkguide.txt
+++ b/drivers/staging/speakup/spkguide.txt
@@ -88,7 +88,7 @@ NOT * support the internal Tripletalk!
 
 Speakup does support two other synthesizers, but because they work in
 conjunction with other software, they must be loaded as modules after
-their related software is loaded, and so are not available at boot up. 
+their related software is loaded, and so are not available at boot up.
 These are as follows:
 
 decpc -- DecTalk PC (not available at boot up)
@@ -106,7 +106,7 @@ the bootup messages.
 
 Once you have booted the system, and if necessary, have supplied the
 proper bootup parameter for your synthesizer, Speakup will begin
-talking as soon as the kernel is loaded.  In fact, it will talk a lot! 
+talking as soon as the kernel is loaded.  In fact, it will talk a lot!
 It will speak all the boot up messages that the kernel prints on the
 screen during the boot process.  This is because Speakup is not a
 separate screen reader, but is actually built into the operating
@@ -115,7 +115,7 @@ screen using the kernel, and must get their keyboard input through the
 kernel, they are automatically handled properly by Speakup.  There are a
 few exceptions, but we'll come to those later.
 
-Note:  In this guide I will refer to the numeric keypad as the keypad. 
+Note:  In this guide I will refer to the numeric keypad as the keypad.
 This is done because the speakupmap.map file referred to later in this
 manual uses the term keypad instead of numeric keypad.  Also I'm lazy
 and would rather only type one word.  So keypad it is.  Got it?  Good.
@@ -143,7 +143,7 @@ that way, until you press another key.
 You can check to see if the boot process has completed by pressing the 8
 key on the keypad, which reads the current line.  This also has the
 effect of starting Speakup talking again, so you can press keypad enter
-to silence it again if the boot process has not completed.  
+to silence it again if the boot process has not completed.
 
 When the boot process is complete, you will arrive at a "login" prompt.
 At this point, you'll need to type in your user id and password, as
@@ -159,7 +159,7 @@ which require root privileges.
 
 When you are running a program or command, Speakup will automatically
 speak new text as it arrives on the screen.  You can at any time silence
-the speech with keypad enter, or use any of the Speakup review keys. 
+the speech with keypad enter, or use any of the Speakup review keys.
 
 Here are some basic Speakup review keys, and a short description of what
 they do.
@@ -215,12 +215,12 @@ example, if you press the keypad 8 key, you will hear:
 
 "Keypad 8 is line, say current."
 
-You'll notice that some commands do not have keys assigned to them. 
+You'll notice that some commands do not have keys assigned to them.
 This is because they are very infrequently used commands, and are also
 accessible through the sys system.  We'll discuss the sys system later
 in this manual.
 
-You'll also notice that some commands have two keys assigned to them. 
+You'll also notice that some commands have two keys assigned to them.
 This is because Speakup has a built in set of alternative key bindings
 for laptop users.  The alternate speakup key is the caps lock key.  You
 can press and hold the caps lock key, while pressing an alternate
@@ -292,12 +292,12 @@ spk key_kp5 = spell_phonetic
 spk key_kp6 = say_to_right
  key_kpplus = say_screen
 spk key_kpplus = say_win
- key_kp1 = say_prev_char 
+ key_kp1 = say_prev_char
 spk key_kp1 = right_edge
  key_kp2 = say_char
 spk key_kp2 = say_to_bottom
 double key_kp2 = say_phonetic_char
- key_kp3 = say_next_char  
+ key_kp3 = say_next_char
 spk  key_kp3 = bottom_edge
  key_kp0 = spk_key
  key_kpdot = say_position
@@ -354,7 +354,7 @@ As a convenience, run as root
 
 ln -s /sys/accessibility/speakup /speakup
 
-to directly access speakup parameters from /speakup. 
+to directly access speakup parameters from /speakup.
 You can see these entries by typing the command:
 
 ls -1 /speakup/*
@@ -497,7 +497,7 @@ only, is owned by root, and belongs to the root group.  Doing a cat of
 this:
 
 cat /speakup/version
-Speakup v-2.00 CVS: Thu Oct 21 10:38:21 EDT 2004 
+Speakup v-2.00 CVS: Thu Oct 21 10:38:21 EDT 2004
 synth dtlk version 1.1
 
 The display shows the Speakup version number, along with the version
@@ -517,7 +517,7 @@ volume is set at.
 All the entries in the Speakup sys system are readable, some are
 writable by root only, and some are writable by everyone.  Unless you
 know what you are doing, you should probably leave the ones that are
-writable by root only alone.  Most of the names are self explanatory. 
+writable by root only alone.  Most of the names are self explanatory.
 Vol for controlling volume, pitch for pitch, rate for controlling speaking
 rate, etc.  If you find one you aren't sure about, you can post a query
 on the Speakup list.
@@ -527,7 +527,7 @@ on the Speakup list.
 It is possible to change to a different synthesizer while speakup is
 running.  In other words, it is not necessary to reboot the system
 in order to use a different synthesizer.  You can simply echo the
-synthesizer keyword to the /speakup/synth sys entry. 
+synthesizer keyword to the /speakup/synth sys entry.
 Depending on your situation, you may wish to echo none to the synth
 sys entry, to disable speech while one synthesizer is disconnected and
 a second one is connected in its place.  Then echo the keyword for the
@@ -632,7 +632,7 @@ Espeakup is a connector between Speakup and the eSpeak software synthesizer.
 Espeakup may already be available as a package for your distribution
 of Linux.  If it is not packaged, you need to install it manually.
 You can find it in the contrib/ subdirectory of the Speakup sources.
-The filename is espeakup-$VERSION.tar.bz2, where $VERSION 
+The filename is espeakup-$VERSION.tar.bz2, where $VERSION
 depends on the current release of Espeakup.  The Speakup 3.1.2 source
 ships with version 0.71 of Espeakup.
 The README file included with the Espeakup sources describes the process
@@ -660,7 +660,7 @@ daemons are started by the bootup scripts.
 These procedures are not described in this document.
 
 If you built Espeakup manually, the "make install" step placed the binary
-under /usr/bin.  
+under /usr/bin.
 Run the following command as root:
 /usr/bin/espeakup
 Espeakup should start speaking.
@@ -684,7 +684,7 @@ distribution may also have a precompiled Speech Dispatcher package.
 
 Once you've installed, configured, and tested Speech Dispatcher with your
 chosen software synthesizer, you still need one more piece of software
-in order to make things work.  You need a package called speechd-up. 
+in order to make things work.  You need a package called speechd-up.
 You get it from the free(b)soft web site mentioned above.  After you've
 compiled and installed speechd-up, you are almost ready to begin using
 your software synthesizer.
@@ -813,7 +813,7 @@ press the keypad slash key once.  Speakup will say, "mark".  Next,
 position the reading cursor on the rightmost character of the above
 url. Press the keypad slash key once again to actually cut the text
 from the screen.  Speakup will say, "cut".  Although we call this
-cutting, Speakup does not actually delete the cut text from the screen. 
+cutting, Speakup does not actually delete the cut text from the screen.
 It makes a copy of the text in a special buffer for later pasting.
 
 Now that you have the url cut from the screen, you can paste it into
@@ -960,7 +960,7 @@ you reboot, or until you load another keymap.
 
 One final warning.  If you try to load a partial map, you will quickly
 find that all the mappings you didn't include in your file got deleted
-from the working map.  Be extremely careful, and always make a backup! 
+from the working map.  Be extremely careful, and always make a backup!
 You have been warned!
 
 14.  Internationalizing Speakup
diff --git a/drivers/staging/speakup/synth.c b/drivers/staging/speakup/synth.c
index 172cf62b1aaf..f3aa4239dc68 100644
--- a/drivers/staging/speakup/synth.c
+++ b/drivers/staging/speakup/synth.c
@@ -122,7 +122,7 @@ void spk_do_catch_up(struct spk_synth *synth)
 			schedule_timeout(msecs_to_jiffies(full_time_val));
 			continue;
 		}
-		if ((jiffies >= jiff_max) && (ch == SPACE)) {
+		if (time_after_eq(jiffies, jiff_max) && (ch == SPACE)) {
 			spin_lock_irqsave(&speakup_info.spinlock, flags);
 			jiffy_delta_val = jiffy_delta->u.n.value;
 			delay_time_val = delay_time->u.n.value;
@@ -148,6 +148,7 @@ EXPORT_SYMBOL_GPL(spk_do_catch_up);
 const char *spk_synth_immediate(struct spk_synth *synth, const char *buff)
 {
 	u_char ch;
+
 	while ((ch = *buff)) {
 		if (ch == '\n')
 			ch = synth->procspeech;
@@ -259,6 +260,7 @@ static int sentence_count;
 void spk_reset_index_count(int sc)
 {
 	static int first = 1;
+
 	if (first)
 		first = 0;
 	else
@@ -277,6 +279,7 @@ int synth_supports_indexing(void)
 void synth_insert_next_index(int sent_num)
 {
 	int out;
+
 	if (synth->alive) {
 		if (sent_num == 0) {
 			synth->indexing.currindex++;
@@ -295,6 +298,7 @@ void synth_insert_next_index(int sent_num)
 void spk_get_index_count(int *linecount, int *sentcount)
 {
 	int ind = synth->get_index();
+
 	if (ind) {
 		sentence_count = ind % 10;
 
@@ -315,6 +319,7 @@ static struct resource synth_res;
 int synth_request_region(unsigned long start, unsigned long n)
 {
 	struct resource *parent = &ioport_resource;
+
 	memset(&synth_res, 0, sizeof(synth_res));
 	synth_res.name = synth->name;
 	synth_res.start = start;
@@ -437,6 +442,7 @@ int synth_add(struct spk_synth *in_synth)
 {
 	int i;
 	int status = 0;
+
 	mutex_lock(&spk_mutex);
 	for (i = 0; i < MAXSYNTHS && synths[i] != NULL; i++)
 		/* synth_remove() is responsible for rotating the array down */
@@ -461,6 +467,7 @@ EXPORT_SYMBOL_GPL(synth_add);
 void synth_remove(struct spk_synth *in_synth)
 {
 	int i;
+
 	mutex_lock(&spk_mutex);
 	if (synth == in_synth)
 		synth_release();
diff --git a/drivers/staging/speakup/thread.c b/drivers/staging/speakup/thread.c
index 4397c8e898c7..d95efb702fe4 100644
--- a/drivers/staging/speakup/thread.c
+++ b/drivers/staging/speakup/thread.c
@@ -21,6 +21,7 @@ int speakup_thread(void *data)
 	mutex_lock(&spk_mutex);
 	while (1) {
 		DEFINE_WAIT(wait);
+
 		while (1) {
 			spin_lock_irqsave(&speakup_info.spinlock, flags);
 			our_sound = spk_unprocessed_sound;
diff --git a/drivers/staging/speakup/varhandlers.c b/drivers/staging/speakup/varhandlers.c
index 445a3fda380e..1b0d1c08741f 100644
--- a/drivers/staging/speakup/varhandlers.c
+++ b/drivers/staging/speakup/varhandlers.c
@@ -112,12 +112,12 @@ void speakup_register_var(struct var_t *var)
 	default:
 		break;
 	}
-	return;
 }
 
 void speakup_unregister_var(enum var_id_t var_id)
 {
 	struct st_var_header *p_header;
+
 	BUG_ON(var_id < 0 || var_id >= MAXVARS);
 	p_header = var_ptrs[var_id];
 	p_header->data = NULL;
@@ -126,6 +126,7 @@ void speakup_unregister_var(enum var_id_t var_id)
 struct st_var_header *spk_get_var_header(enum var_id_t var_id)
 {
 	struct st_var_header *p_header;
+
 	if (var_id < 0 || var_id >= MAXVARS)
 		return NULL;
 	p_header = var_ptrs[var_id];
@@ -224,6 +225,7 @@ int spk_set_num_var(int input, struct st_var_header *var, int how)
 		return ret;
 	if (synth->synth_adjust != NULL) {
 		int status = synth->synth_adjust(var);
+
 		return (status != 0) ? status : ret;
 	}
 	if (!var_data->u.n.synth_fmt)
@@ -272,6 +274,7 @@ int spk_set_mask_bits(const char *input, const int which, const int how)
 {
 	u_char *cp;
 	short mask = spk_punc_info[which].mask;
+
 	if (how&1) {
 		for (cp = (u_char *)spk_punc_info[3].value; *cp; cp++)
 			spk_chartab[*cp] &= ~mask;
@@ -308,6 +311,7 @@ int spk_set_mask_bits(const char *input, const int which, const int how)
 char *spk_strlwr(char *s)
 {
 	char *p;
+
 	if (s == NULL)
 		return NULL;
 
@@ -319,6 +323,7 @@ char *spk_strlwr(char *s)
 char *spk_s2uchar(char *start, char *dest)
 {
 	int val = 0;
+
 	val = simple_strtoul(skip_spaces(start), &start, 10);
 	if (*start == ',')
 		start++;
diff --git a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
index 28b393071026..f92ae1d24f9f 100644
--- a/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
+++ b/drivers/staging/ste_rmi4/synaptics_i2c_rmi4.c
@@ -415,6 +415,7 @@ static int synaptics_rmi4_report_device(struct synaptics_rmi4_data *pdata,
 	int touch = 0;
 	struct	i2c_client *client = pdata->i2c_client;
 	static int num_error_reports;
+
 	if (rfi->fn_number != SYNAPTICS_RMI4_TOUCHPAD_FUNC_NUM) {
 		num_error_reports++;
 		if (num_error_reports < MAX_ERROR_REPORT)
@@ -485,6 +486,7 @@ static irqreturn_t synaptics_rmi4_irq(int irq, void *data)
 {
 	struct synaptics_rmi4_data *pdata = data;
 	int touch_count;
+
 	do {
 		touch_count = synaptics_rmi4_sensor_report(pdata);
 		if (touch_count)
diff --git a/drivers/staging/unisys/channels/channel.c b/drivers/staging/unisys/channels/channel.c
index b9bf8e81677c..b4bdee4b575b 100644
--- a/drivers/staging/unisys/channels/channel.c
+++ b/drivers/staging/unisys/channels/channel.c
@@ -73,9 +73,9 @@ visor_signal_insert(CHANNEL_HEADER __iomem *pChannel, u32 Queue, void *pSignal)
 	 */
 	psignal = (char __iomem *)pqhdr + readq(&pqhdr->oSignalBase) +
 		(head * readl(&pqhdr->SignalSize));
-	MEMCPY_TOIO(psignal, pSignal, readl(&pqhdr->SignalSize));
+	memcpy_toio(psignal, pSignal, readl(&pqhdr->SignalSize));
 
-	VolatileBarrier();
+	mb(); /* channel synch */
 	writel(head, &pqhdr->Head);
 
 	writeq(readq(&pqhdr->NumSignalsSent) + 1, &pqhdr->NumSignalsSent);
@@ -126,9 +126,9 @@ visor_signal_remove(CHANNEL_HEADER __iomem *pChannel, u32 Queue, void *pSignal)
 	/* copy signal from tail location to the area pointed to by pSignal */
 	psource = (char __iomem *) pqhdr + readq(&pqhdr->oSignalBase) +
 		(tail * readl(&pqhdr->SignalSize));
-	MEMCPY_FROMIO(pSignal, psource, readl(&pqhdr->SignalSize));
+	memcpy_fromio(pSignal, psource, readl(&pqhdr->SignalSize));
 
-	VolatileBarrier();
+	mb(); /* channel synch */
 	writel(tail, &pqhdr->Tail);
 
 	writeq(readq(&pqhdr->NumSignalsReceived) + 1,
@@ -183,10 +183,10 @@ SignalRemoveAll(pCHANNEL_HEADER pChannel, u32 Queue, void *pSignal)
 		psource =
 		    (char *) pqhdr + pqhdr->oSignalBase +
 		    (tail * pqhdr->SignalSize);
-		MEMCPY((char *) pSignal + (pqhdr->SignalSize * signalCount),
+		memcpy((char *) pSignal + (pqhdr->SignalSize * signalCount),
 		       psource, pqhdr->SignalSize);
 
-		VolatileBarrier();
+		mb(); /* channel synch */
 		pqhdr->Tail = tail;
 
 		signalCount++;
diff --git a/drivers/staging/unisys/channels/chanstub.c b/drivers/staging/unisys/channels/chanstub.c
index 7f36d9adac55..d54c5d635a94 100644
--- a/drivers/staging/unisys/channels/chanstub.c
+++ b/drivers/staging/unisys/channels/chanstub.c
@@ -25,7 +25,7 @@
 
 #include "channel.h"
 #include "chanstub.h"
-#include "timskmodutils.h"
+#include "timskmod.h"
 #include "version.h"
 
 static __init int
@@ -47,6 +47,7 @@ SignalInsert_withLock(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
 {
 	unsigned char result;
 	unsigned long flags;
+
 	spin_lock_irqsave(lock, flags);
 	result = visor_signal_insert(pChannel, Queue, pSignal);
 	spin_unlock_irqrestore(lock, flags);
@@ -58,6 +59,7 @@ SignalRemove_withLock(CHANNEL_HEADER __iomem *pChannel, u32 Queue,
 		      void *pSignal, spinlock_t *lock)
 {
 	unsigned char result;
+
 	spin_lock(lock);
 	result = visor_signal_remove(pChannel, Queue, pSignal);
 	spin_unlock(lock);
diff --git a/drivers/staging/unisys/common-spar/include/channels/channel.h b/drivers/staging/unisys/common-spar/include/channels/channel.h
index 15a8d6b35dac..c25dfbf7f6bc 100644
--- a/drivers/staging/unisys/common-spar/include/channels/channel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/channel.h
@@ -16,6 +16,8 @@
 #ifndef __CHANNEL_H__
 #define __CHANNEL_H__
 
+#include <linux/types.h>
+#include <linux/io.h>
 #include <linux/uuid.h>
 
 /*
@@ -30,8 +32,6 @@
  */
 #define __SUPERVISOR_CHANNEL_H__
 
-#include "commontypes.h"
-
 #define SIGNATURE_16(A, B) ((A) | (B<<8))
 #define SIGNATURE_32(A, B, C, D) \
 	(SIGNATURE_16(A, B) | (SIGNATURE_16(C, D) << 16))
@@ -50,6 +50,37 @@
 
 #define ULTRA_CHANNEL_PROTOCOL_SIGNATURE  SIGNATURE_32('E', 'C', 'N', 'L')
 
+#define CHANNEL_GUID_MISMATCH(chType, chName, field, expected, actual, fil, \
+			      lin, logCtx)				\
+	do {								\
+		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=%pUL actual=%pUL @%s:%d\n", \
+		       chName, &chType, field,	\
+		       &expected, &actual, \
+		       fil, lin);					\
+	} while (0)
+#define CHANNEL_U32_MISMATCH(chType, chName, field, expected, actual, fil, \
+			     lin, logCtx)				\
+	do {								\
+		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d\n", \
+		       chName, &chType, field,	\
+		       (unsigned long)expected, (unsigned long)actual,	\
+		       fil, lin);					\
+	} while (0)
+
+#define CHANNEL_U64_MISMATCH(chType, chName, field, expected, actual, fil, \
+			     lin, logCtx)				\
+	do {								\
+		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d\n", \
+		       chName, &chType, field,	\
+		       (unsigned long long)expected,			\
+		       (unsigned long long)actual,			\
+		       fil, lin);					\
+	} while (0)
+
+#define UltraLogEvent(logCtx, EventId, Severity, SubsystemMask, pFunctionName, \
+		      LineNumber, Str, args...)				\
+	pr_info(Str, ## args)
+
 typedef enum {
 	CHANNELSRV_UNINITIALIZED = 0,	/* channel is in an undefined state */
 	CHANNELSRV_READY = 1	/* channel has been initialized by server */
@@ -157,7 +188,7 @@ ULTRA_CHANNELCLI_STRING(u32 v)
 			      PathName_Last_N_Nodes(__FILE__, 4), __LINE__); \
 		writel(newstate, &((CHANNEL_HEADER __iomem *) \
 				   (pChan))->CliStateOS);		\
-		MEMORYBARRIER;						\
+		mb(); /* required for channel synch */			\
 	} while (0)
 
 #define ULTRA_CHANNEL_CLIENT_ACQUIRE_OS(pChan, chanId, logCtx)	\
@@ -316,17 +347,21 @@ ULTRA_check_channel_client(void __iomem *pChannel,
 			   u64 expectedSignature,
 			   char *fileName, int lineNumber, void *logCtx)
 {
-	if (uuid_le_cmp(expectedTypeGuid, NULL_UUID_LE) != 0)
+	if (uuid_le_cmp(expectedTypeGuid, NULL_UUID_LE) != 0) {
+		uuid_le guid;
+
+		memcpy_fromio(&guid,
+			      &((CHANNEL_HEADER __iomem *)(pChannel))->Type,
+			      sizeof(guid));
 		/* caller wants us to verify type GUID */
-		if (uuid_le_cmp((((CHANNEL_HEADER __iomem *)(pChannel))->Type),
-			   expectedTypeGuid) != 0) {
+		if (uuid_le_cmp(guid, expectedTypeGuid) != 0) {
 			CHANNEL_GUID_MISMATCH(expectedTypeGuid, channelName,
 					      "type", expectedTypeGuid,
-					      ((CHANNEL_HEADER __iomem *)
-					       (pChannel))->Type, fileName,
+					      guid, fileName,
 					      lineNumber, logCtx);
 			return 0;
 		}
+	}
 	if (expectedMinBytes > 0)	/* caller wants us to verify
 					 * channel size */
 		if (readq(&((CHANNEL_HEADER __iomem *)
@@ -399,6 +434,7 @@ PathName_Last_N_Nodes(u8 *s, unsigned int n)
 {
 	u8 *p = s;
 	unsigned int node_count = 0;
+
 	while (*p != '\0') {
 		if ((*p == '/') || (*p == '\\'))
 			node_count++;
@@ -458,7 +494,7 @@ ULTRA_channel_client_acquire_os(void __iomem *pChannel, u8 *chanId,
 			      CHANNELCLI_OWNED,
 			      PathName_Last_N_Nodes((u8 *) file, 4), line);
 		writel(CHANNELCLI_OWNED, &pChan->CliStateOS);
-		MEMORYBARRIER;
+		mb(); /* required for channel synch */
 	}
 	if (readl(&pChan->CliStateOS) == CHANNELCLI_OWNED) {
 		if (readb(&pChan->CliErrorOS) != 0) {
@@ -502,7 +538,7 @@ ULTRA_channel_client_acquire_os(void __iomem *pChannel, u8 *chanId,
 		return 0;
 	}
 	writel(CHANNELCLI_BUSY, &pChan->CliStateOS);
-	MEMORYBARRIER;
+	mb(); /* required for channel synch */
 	if (readl(&pChan->CliStateBoot) == CHANNELCLI_BUSY) {
 		if ((readb(&pChan->CliErrorOS)
 		     & ULTRA_CLIERROROS_THROTTLEMSG_BUSY) == 0) {
@@ -521,7 +557,7 @@ ULTRA_channel_client_acquire_os(void __iomem *pChannel, u8 *chanId,
 		}
 		/* reset busy */
 		writel(CHANNELCLI_ATTACHED, &pChan->CliStateOS);
-		MEMORYBARRIER;
+		mb(); /* required for channel synch */
 		return 0;
 	}
 	if (readb(&pChan->CliErrorOS) != 0) {
@@ -542,6 +578,7 @@ ULTRA_channel_client_release_os(void __iomem *pChannel, u8 *chanId,
 				void *logCtx, char *file, int line, char *func)
 {
 	CHANNEL_HEADER __iomem *pChan = pChannel;
+
 	if (readb(&pChan->CliErrorOS) != 0) {
 		/* we are in an error msg throttling state; come out of it */
 		UltraLogEvent(logCtx, CHANNELSTATE_DIAG_EVENTID_TRANSITOK,
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlframework.h b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
index b0a49e0c37a2..fd4726e754ea 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlframework.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlframework.h
@@ -25,7 +25,7 @@
 #ifndef _CONTROL_FRAMEWORK_H_
 #define _CONTROL_FRAMEWORK_H_
 
-#include "commontypes.h"
+#include <linux/types.h>
 #include "channel.h"
 
 #define ULTRA_MEMORY_COUNT_Ki 1024
diff --git a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
index 153f57ce908f..d08c198e0de3 100644
--- a/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/controlvmchannel.h
@@ -17,9 +17,11 @@
 #define __CONTROLVMCHANNEL_H__
 
 #include <linux/uuid.h>
-#include "commontypes.h"
 #include "channel.h"
 #include "controlframework.h"
+
+typedef u64 GUEST_PHYSICAL_ADDRESS;
+
 enum { INVALID_GUEST_FIRMWARE, SAMPLE_GUEST_FIRMWARE,
 	    TIANO32_GUEST_FIRMWARE, TIANO64_GUEST_FIRMWARE
 };
diff --git a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
index c01649a985c7..9912e51b89b5 100644
--- a/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/diagchannel.h
@@ -33,7 +33,7 @@
 #ifndef _DIAG_CHANNEL_H_
 #define _DIAG_CHANNEL_H_
 
-#include "commontypes.h"
+#include <linux/uuid.h>
 #include "channel.h"
 
 /* {EEA7A573-DB82-447c-8716-EFBEAAAE4858} */
diff --git a/drivers/staging/unisys/common-spar/include/channels/iochannel.h b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
index 24e11858e0ee..b1dd73d1f42c 100644
--- a/drivers/staging/unisys/common-spar/include/channels/iochannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/iochannel.h
@@ -31,7 +31,6 @@
 
 #include <linux/uuid.h>
 
-#include "commontypes.h"
 #include "vmcallinterface.h"
 
 #define _ULTRA_CONTROLVM_CHANNEL_INLINE_
@@ -711,24 +710,24 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 /* ///////////// END PRAGMA PACK PUSH 1 /////////////////////////// */
 
 /* define offsets to members of struct uiscmdrsp */
-#define OFFSET_CMDTYPE OFFSETOF(struct uiscmdrsp, cmdtype)
-#define OFFSET_SCSI OFFSETOF(struct uiscmdrsp, scsi)
-#define OFFSET_NET OFFSETOF(struct uiscmdrsp, net)
-#define OFFSET_SCSITASKMGMT OFFSETOF(struct uiscmdrsp, scsitaskmgmt)
-#define OFFSET_NEXT OFFSETOF(struct uiscmdrsp, next)
+#define OFFSET_CMDTYPE offsetof(struct uiscmdrsp, cmdtype)
+#define OFFSET_SCSI offsetof(struct uiscmdrsp, scsi)
+#define OFFSET_NET offsetof(struct uiscmdrsp, net)
+#define OFFSET_SCSITASKMGMT offsetof(struct uiscmdrsp, scsitaskmgmt)
+#define OFFSET_NEXT offsetof(struct uiscmdrsp, next)
 
 /* define offsets to members of struct uiscmdrsp_net */
-#define OFFSET_TYPE OFFSETOF(struct uiscmdrsp_net, type)
-#define OFFSET_BUF OFFSETOF(struct uiscmdrsp_net, buf)
-#define OFFSET_XMT OFFSETOF(struct uiscmdrsp_net, xmt)
-#define OFFSET_XMT_DONE_RESULT OFFSETOF(struct uiscmdrsp_net, xmtdone)
-#define OFFSET_RCVPOST OFFSETOF(struct uiscmdrsp_net, rcvpost)
-#define OFFSET_RCV_DONE_LEN OFFSETOF(struct uiscmdrsp_net, rcv)
-#define OFFSET_ENBDIS OFFSETOF(struct uiscmdrsp_net, enbdis)
+#define OFFSET_TYPE offsetof(struct uiscmdrsp_net, type)
+#define OFFSET_BUF offsetof(struct uiscmdrsp_net, buf)
+#define OFFSET_XMT offsetof(struct uiscmdrsp_net, xmt)
+#define OFFSET_XMT_DONE_RESULT offsetof(struct uiscmdrsp_net, xmtdone)
+#define OFFSET_RCVPOST offsetof(struct uiscmdrsp_net, rcvpost)
+#define OFFSET_RCV_DONE_LEN offsetof(struct uiscmdrsp_net, rcv)
+#define OFFSET_ENBDIS offsetof(struct uiscmdrsp_net, enbdis)
 
 /* define offsets to members of struct net_pkt_rcvpost */
-#define OFFSET_TOTALLEN OFFSETOF(struct net_pkt_rcvpost, totallen)
-#define	OFFSET_FRAG OFFSETOF(struct net_pkt_rcvpost, frag)
+#define OFFSET_TOTALLEN offsetof(struct net_pkt_rcvpost, totallen)
+#define	OFFSET_FRAG offsetof(struct net_pkt_rcvpost, frag)
 
 /*
 * INLINE functions for initializing and accessing I/O data channels
@@ -753,7 +752,7 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 	do {							\
 		x->cmdQ.Size = QSIZEFROMBYTES(x->ChannelHeader.Size);	\
 		x->cmdQ.oSignalBase = SIZEOF_PROTOCOL -			\
-			OFFSETOF(ULTRA_IO_CHANNEL_PROTOCOL, cmdQ);	\
+			offsetof(ULTRA_IO_CHANNEL_PROTOCOL, cmdQ);	\
 		x->cmdQ.SignalSize = SIZEOF_CMDRSP;			\
 		x->cmdQ.MaxSignalSlots =				\
 			QSLOTSFROMBYTES(x->ChannelHeader.Size);		\
@@ -761,21 +760,21 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 		x->rspQ.Size = QSIZEFROMBYTES(x->ChannelHeader.Size);	\
 		x->rspQ.oSignalBase =					\
 			(SIZEOF_PROTOCOL + x->cmdQ.Size) -		\
-			OFFSETOF(ULTRA_IO_CHANNEL_PROTOCOL, rspQ);	\
+			offsetof(ULTRA_IO_CHANNEL_PROTOCOL, rspQ);	\
 		x->rspQ.SignalSize = SIZEOF_CMDRSP;			\
 		x->rspQ.MaxSignalSlots =				\
 			QSLOTSFROMBYTES(x->ChannelHeader.Size);		\
 		x->rspQ.MaxSignals = x->rspQ.MaxSignalSlots - 1;	\
 		x->ChannelHeader.oChannelSpace =			\
-			OFFSETOF(ULTRA_IO_CHANNEL_PROTOCOL, cmdQ);	\
+			offsetof(ULTRA_IO_CHANNEL_PROTOCOL, cmdQ);	\
 	} while (0)
 
 #define INIT_CLIENTSTRING(chan, type, clientStr, clientStrLen)	\
 	do {								\
 		if (clientStr) {					\
 			chan->ChannelHeader.oClientString =		\
-				OFFSETOF(type, clientString);		\
-			MEMCPY(chan->clientString, clientStr,		\
+				offsetof(type, clientString);		\
+			memcpy(chan->clientString, clientStr,		\
 			       MINNUM(clientStrLen,			\
 				      (u32) (MAX_CLIENTSTRING_LEN - 1))); \
 			chan->clientString[MINNUM(clientStrLen,		\
@@ -791,11 +790,11 @@ typedef struct _ULTRA_IO_CHANNEL_PROTOCOL {
 
 #define ULTRA_IO_CHANNEL_SERVER_READY(x, chanId, logCtx) \
 	ULTRA_CHANNEL_SERVER_TRANSITION(x, chanId, SrvState, CHANNELSRV_READY, \
-					logCtx);
+					logCtx)
 
 #define ULTRA_IO_CHANNEL_SERVER_NOTREADY(x, chanId, logCtx)	\
 	ULTRA_CHANNEL_SERVER_TRANSITION(x, chanId, SrvState, \
-					CHANNELSRV_UNINITIALIZED, logCtx);
+					CHANNELSRV_UNINITIALIZED, logCtx)
 
 static inline int ULTRA_VHBA_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 					      struct vhba_wwnn *wwnn,
@@ -846,7 +845,7 @@ static inline int ULTRA_VNIC_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 	x->ChannelHeader.Size = COVER(bytes, 4096);
 	x->ChannelHeader.Type = UltraVnicChannelProtocolGuid;
 	x->ChannelHeader.ZoneGuid = NULL_UUID_LE;
-	MEMCPY(x->vnic.macaddr, macaddr, MAX_MACADDR_LEN);
+	memcpy(x->vnic.macaddr, macaddr, MAX_MACADDR_LEN);
 	x->vnic.num_rcv_bufs = num_rcv_bufs;
 	x->vnic.mtu = mtu;
 	x->vnic.zoneGuid = zoneGuid;
@@ -882,7 +881,7 @@ static inline int ULTRA_VNIC_init_channel(ULTRA_IO_CHANNEL_PROTOCOL *x,
 /* returns next non-zero index on success or zero on failure (i.e. out of
  * room)
  */
-static INLINE  u16
+static inline  u16
 add_physinfo_entries(u32 inp_pfn,	/* input - specifies the pfn to be used
 					 * to add entries */
 		     u16 inp_off,	/* input - specifies the off to be used
diff --git a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
index 8facb51143ae..1231c454176f 100644
--- a/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
+++ b/drivers/staging/unisys/common-spar/include/channels/vbuschannel.h
@@ -24,7 +24,6 @@
  *  the client devices and client drivers for the server end to see.
  */
 #include <linux/uuid.h>
-#include "commontypes.h"
 #include "vbusdeviceinfo.h"
 #include "channel.h"
 
diff --git a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h b/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
index db77d6f626a1..f74f5d8c2820 100644
--- a/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
+++ b/drivers/staging/unisys/common-spar/include/controlvmcompletionstatus.h
@@ -62,14 +62,16 @@
 							 * DEVICE_CREATE,
 							 * DEVICE_DESTROY */
 /* Unable to invoke VIRTPCI callback */
-#define CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR 605 /* BUS_CREATE,
-								* BUS_DESTROY,
-								* DEVICE_CREATE,
-								* DEVICE_DESTROY */
+#define CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR 605
+							/* BUS_CREATE,
+							 * BUS_DESTROY,
+							 * DEVICE_CREATE,
+							 * DEVICE_DESTROY */
 /* VIRTPCI Callback returned error */
-#define CONTROLVM_RESP_ERROR_GENERIC_DRIVER_CALLBACK_ERROR 606 /* SWITCH_ATTACHEXTPORT,
-								* SWITCH_DETACHEXTPORT
-								* DEVICE_CONFIGURE */
+#define CONTROLVM_RESP_ERROR_GENERIC_DRIVER_CALLBACK_ERROR 606
+							/* SWITCH_ATTACHEXTPORT,
+							 * SWITCH_DETACHEXTPORT
+							 * DEVICE_CONFIGURE */
 
 /* generic device callback returned error */
 /* Bus Related------------------------------------------------------[700-799] */
diff --git a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h b/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
index fe9598c941a6..57dd93e0cc83 100644
--- a/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
+++ b/drivers/staging/unisys/common-spar/include/iovmcall_gnuc.h
@@ -19,16 +19,14 @@ __unisys_vmcall_gnuc(unsigned long tuple, unsigned long reg_ebx,
 		     unsigned long reg_ecx)
 {
 	unsigned long result = 0;
-
 	unsigned int cpuid_eax, cpuid_ebx, cpuid_ecx, cpuid_edx;
+
 	cpuid(0x00000001, &cpuid_eax, &cpuid_ebx, &cpuid_ecx, &cpuid_edx);
-	if (cpuid_ecx & 0x80000000) {
-	      __asm__ __volatile__(".byte 0x00f, 0x001, 0x0c1" : "=a"(result) :
-				   "a"(tuple), "b"(reg_ebx), "c"(reg_ecx)
-				      );
-	} else {
-		result = -1;
-	}
+	if (!(cpuid_ecx & 0x80000000))
+		return -1;
+
+	__asm__ __volatile__(".byte 0x00f, 0x001, 0x0c1" : "=a"(result) :
+		"a"(tuple), "b"(reg_ebx), "c"(reg_ecx));
 	return result;
 }
 
@@ -39,15 +37,13 @@ __unisys_extended_vmcall_gnuc(unsigned long long tuple,
 			      unsigned long long reg_edx)
 {
 	unsigned long result = 0;
-
 	unsigned int cpuid_eax, cpuid_ebx, cpuid_ecx, cpuid_edx;
+
 	cpuid(0x00000001, &cpuid_eax, &cpuid_ebx, &cpuid_ecx, &cpuid_edx);
-	if (cpuid_ecx & 0x80000000) {
-	      __asm__ __volatile__(".byte 0x00f, 0x001, 0x0c1" : "=a"(result) :
-				   "a"(tuple), "b"(reg_ebx), "c"(reg_ecx),
-				   "d"(reg_edx));
-	} else {
-		result = -1;
-	}
+	if (!(cpuid_ecx & 0x80000000))
+		return -1;
+
+	__asm__ __volatile__(".byte 0x00f, 0x001, 0x0c1" : "=a"(result) :
+		"a"(tuple), "b"(reg_ebx), "c"(reg_ecx), "d"(reg_edx));
 	return result;
-	}
+}
diff --git a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
index 5e0d98cd422e..3bbdc2bb7ebf 100644
--- a/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
+++ b/drivers/staging/unisys/common-spar/include/vbusdeviceinfo.h
@@ -16,7 +16,7 @@
 #ifndef __VBUSDEVICEINFO_H__
 #define __VBUSDEVICEINFO_H__
 
-#include "commontypes.h"
+#include <linux/types.h>
 
 #pragma pack(push, 1)		/* both GCC and VC now allow this pragma */
 
@@ -54,6 +54,7 @@ vbuschannel_sanitize_buffer(char *p, int remain, char *src, int srcmax)
 {
 	int chars = 0;
 	int nonprintable_streak = 0;
+
 	while (srcmax > 0) {
 		if ((*src >= ' ') && (*src < 0x7f)) {
 			if (nonprintable_streak) {
diff --git a/drivers/staging/unisys/include/commontypes.h b/drivers/staging/unisys/include/commontypes.h
deleted file mode 100644
index 4311e9f6200f..000000000000
--- a/drivers/staging/unisys/include/commontypes.h
+++ /dev/null
@@ -1,72 +0,0 @@
-/* Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef _COMMONTYPES_H_
-#define _COMMONTYPES_H_
-
-/* define the following to prevent include nesting in kernel header files of
- * similar abbreviated content */
-#define _SUPERVISOR_COMMONTYPES_H_
-
-#include <linux/types.h>
-#include <linux/version.h>
-#include <linux/io.h>
-#include <linux/uuid.h>
-
-typedef u64 GUEST_PHYSICAL_ADDRESS;
-
-#define INLINE inline
-#define OFFSETOF offsetof
-
-#define MEMORYBARRIER mb()
-#define MEMCPY(dest, src, len) memcpy(dest, src, len)
-#define MEMCPY_TOIO(dest, src, len) memcpy_toio(dest, src, len)
-#define MEMCPY_FROMIO(dest, src, len) memcpy_fromio(dest, src, len)
-
-#define CHANNEL_GUID_MISMATCH(chType, chName, field, expected, actual, fil, \
-			      lin, logCtx)				\
-	do {								\
-		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=%pUL actual=%pUL @%s:%d\n", \
-		       chName, &chType, field,	\
-		       &expected, &actual, \
-		       fil, lin);					\
-	} while (0)
-#define CHANNEL_U32_MISMATCH(chType, chName, field, expected, actual, fil, \
-			     lin, logCtx)				\
-	do {								\
-		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8lx actual=0x%-8.8lx @%s:%d\n", \
-		       chName, &chType, field,	\
-		       (unsigned long)expected, (unsigned long)actual,	\
-		       fil, lin);					\
-	} while (0)
-
-#define CHANNEL_U64_MISMATCH(chType, chName, field, expected, actual, fil, \
-			     lin, logCtx)				\
-	do {								\
-		pr_err("Channel mismatch on channel=%s(%pUL) field=%s expected=0x%-8.8Lx actual=0x%-8.8Lx @%s:%d\n", \
-		       chName, &chType, field,	\
-		       (unsigned long long)expected,			\
-		       (unsigned long long)actual,			\
-		       fil, lin);					\
-	} while (0)
-
-#define UltraLogEvent(logCtx, EventId, Severity, SubsystemMask, pFunctionName, \
-		      LineNumber, Str, args...)				\
-	pr_info(Str, ## args)
-
-#define VolatileBarrier() MEMORYBARRIER
-
-#endif
-
diff --git a/drivers/staging/unisys/include/guestlinuxdebug.h b/drivers/staging/unisys/include/guestlinuxdebug.h
index 8728e4c7bf6b..957a627d0527 100644
--- a/drivers/staging/unisys/include/guestlinuxdebug.h
+++ b/drivers/staging/unisys/include/guestlinuxdebug.h
@@ -21,10 +21,9 @@
 * regarding adding additional structure and functionality to linux
 * ISSUE_IO_VMCALL_POSTCODE_SEVERITY */
 
-
 /******* INFO ON ISSUE_POSTCODE_LINUX() BELOW *******/
 #include "vmcallinterface.h"
-typedef enum {		/* POSTCODE driver identifier tuples */
+enum driver_pc {		/* POSTCODE driver identifier tuples */
 	/* visorchipset driver files */
 	VISOR_CHIPSET_PC = 0xA0,
 	VISOR_CHIPSET_PC_controlvm_c = 0xA1,
@@ -59,9 +58,9 @@ typedef enum {		/* POSTCODE driver identifier tuples */
 	UISLIB_PC_uisqueue_c = 0xD2,
 	UISLIB_PC_uisthread_c = 0xD3,
 	UISLIB_PC_uisutils_c = 0xD4,
-} DRIVER_PC;
+};
 
-typedef enum {			/* POSTCODE event identifier tuples */
+enum event_pc {			/* POSTCODE event identifier tuples */
 	ATTACH_PORT_ENTRY_PC = 0x001,
 	ATTACH_PORT_FAILURE_PC = 0x002,
 	ATTACH_PORT_SUCCESS_PC = 0x003,
@@ -129,7 +128,7 @@ typedef enum {			/* POSTCODE event identifier tuples */
 	SAVE_MSG_BUS_FAILURE_PC = 0x0D9,
 	SAVE_MSG_DEV_FAILURE_PC = 0x0DA,
 	CALLHOME_INIT_FAILURE_PC = 0x0DB
-} EVENT_PC;
+};
 
 #ifdef __GNUC__
 
@@ -173,7 +172,6 @@ do {									\
 #define POSTCODE_LINUX_3(EVENT_PC, pc32bit, severity)			\
 	POSTCODE_LINUX_A(CURRENT_FILE_PC, EVENT_PC, pc32bit, severity)
 
-
 #define POSTCODE_LINUX_4(EVENT_PC, pc16bit1, pc16bit2, severity)	\
 	POSTCODE_LINUX_B(CURRENT_FILE_PC, EVENT_PC, pc16bit1,		\
 			 pc16bit2, severity)
diff --git a/drivers/staging/unisys/include/periodic_work.h b/drivers/staging/unisys/include/periodic_work.h
index 418ba634e1a2..26ec10bdfe65 100644
--- a/drivers/staging/unisys/include/periodic_work.h
+++ b/drivers/staging/unisys/include/periodic_work.h
@@ -20,21 +20,19 @@
 
 #include "timskmod.h"
 
-
-
 /* PERIODIC_WORK an opaque structure to users.
  * Fields are declared only in the implementation .c files.
  */
-typedef struct PERIODIC_WORK_Tag PERIODIC_WORK;
+struct periodic_work;
 
-PERIODIC_WORK *visor_periodic_work_create(ulong jiffy_interval,
-					  struct workqueue_struct *workqueue,
-					  void (*workfunc)(void *),
-					  void *workfuncarg,
-					  const char *devnam);
-void            visor_periodic_work_destroy(PERIODIC_WORK *periodic_work);
-BOOL            visor_periodic_work_nextperiod(PERIODIC_WORK *periodic_work);
-BOOL            visor_periodic_work_start(PERIODIC_WORK *periodic_work);
-BOOL            visor_periodic_work_stop(PERIODIC_WORK *periodic_work);
+struct periodic_work *visor_periodic_work_create(ulong jiffy_interval,
+					struct workqueue_struct *workqueue,
+					void (*workfunc)(void *),
+					void *workfuncarg,
+					const char *devnam);
+void visor_periodic_work_destroy(struct periodic_work *pw);
+BOOL visor_periodic_work_nextperiod(struct periodic_work *pw);
+BOOL visor_periodic_work_start(struct periodic_work *pw);
+BOOL visor_periodic_work_stop(struct periodic_work *pw);
 
 #endif
diff --git a/drivers/staging/unisys/include/timskmod.h b/drivers/staging/unisys/include/timskmod.h
index ecf1a6fac7a1..b14494ff6c1b 100644
--- a/drivers/staging/unisys/include/timskmod.h
+++ b/drivers/staging/unisys/include/timskmod.h
@@ -62,8 +62,6 @@
 #if !defined SUCCESS
 #define SUCCESS 0
 #endif
-#define FAILURE (-1)
-#define DRIVERNAMEMAX 50
 #define MIN(a, b)     (((a) < (b)) ? (a) : (b))
 #define MAX(a, b)     (((a) > (b)) ? (a) : (b))
 #define STRUCTSEQUAL(x, y) (memcmp(&x, &y, sizeof(x)) == 0)
@@ -71,57 +69,6 @@
 #define HOSTADDRESS unsigned long long
 #endif
 
-typedef long VMMIO;  /**< Virtual MMIO address (returned from ioremap), which
-    *   is a virtual address pointer to a memory-mapped region.
-    *   These are declared as "long" instead of u32* to force you to
-    *   use readb()/writeb()/memcpy_fromio()/etc to access them.
-    *   (On x86 we could probably get away with treating them as
-    *   pointers.)
-    */
-typedef long VMMIO8; /**< #VMMIO pointing to  8-bit data */
-typedef long VMMIO16;/**< #VMMIO pointing to 16-bit data */
-typedef long VMMIO32;/**< #VMMIO pointing to 32-bit data */
-
-#define LOCKSEM(sem)                   down_interruptible(sem)
-#define LOCKSEM_UNINTERRUPTIBLE(sem)   down(sem)
-#define UNLOCKSEM(sem)                 up(sem)
-
-/** lock read/write semaphore for reading.
-    Note that all read/write semaphores are of the "uninterruptible" variety.
-    @param sem (rw_semaphore *) points to semaphore to lock
- */
-#define LOCKREADSEM(sem)               down_read(sem)
-
-/** unlock read/write semaphore for reading.
-    Note that all read/write semaphores are of the "uninterruptible" variety.
-    @param sem (rw_semaphore *) points to semaphore to unlock
- */
-#define UNLOCKREADSEM(sem)             up_read(sem)
-
-/** lock read/write semaphore for writing.
-    Note that all read/write semaphores are of the "uninterruptible" variety.
-    @param sem (rw_semaphore *) points to semaphore to lock
- */
-#define LOCKWRITESEM(sem)              down_write(sem)
-
-/** unlock read/write semaphore for writing.
-    Note that all read/write semaphores are of the "uninterruptible" variety.
-    @param sem (rw_semaphore *) points to semaphore to unlock
- */
-#define UNLOCKWRITESEM(sem)            up_write(sem)
-
-#ifdef ENABLE_RETURN_TRACE
-#define RETTRACE(x)                                            \
-	do {						       \
-		if (1) {				       \
-			INFODRV("RET 0x%lx in %s",	       \
-				(ulong)(x), __func__);     \
-		}					   \
-	} while (0)
-#else
-#define RETTRACE(x)
-#endif
-
 /** Try to evaulate the provided expression, and do a RETINT(x) iff
  *  the expression evaluates to < 0.
  *  @param x the expression to try
@@ -167,7 +114,6 @@ typedef long VMMIO32;/**< #VMMIO pointing to 32-bit data */
 #define INFODEVX(devno, fmt, args...)     LOGINFDEVX(devno, fmt, ## args)
 #define DEBUGDEV(devname, fmt, args...)   DBGINFDEV(devname, fmt, ## args)
 
-
 /* @} */
 
 /** Verifies the consistency of your PRIVATEDEVICEDATA structure using
@@ -192,94 +138,6 @@ typedef long VMMIO32;/**< #VMMIO pointing to 32-bit data */
 	 ((fd)->sig1 == sizeof(PRIVATEFILEDATA)) &&    \
 	 ((fd)->sig2 == fd))
 
-/** Locks dd->lockDev if you havn't already locked it */
-#define LOCKDEV(dd)                                                    \
-	{                                                              \
-		if (!lockedDev) {				       \
-			spin_lock(&dd->lockDev);		       \
-			lockedDev = TRUE;			       \
-		}						       \
-	}
-
-/** Unlocks dd->lockDev if you previously locked it */
-#define UNLOCKDEV(dd)                                                  \
-	{                                                              \
-		if (lockedDev) {				       \
-			spin_unlock(&dd->lockDev);		       \
-			lockedDev = FALSE;			       \
-		}						       \
-	}
-
-/** Locks dd->lockDevISR if you havn't already locked it */
-#define LOCKDEVISR(dd)                                                 \
-	{                                                              \
-		if (!lockedDevISR) {				       \
-			spin_lock_irqsave(&dd->lockDevISR, flags);     \
-			lockedDevISR = TRUE;			       \
-		}						       \
-	}
-
-/** Unlocks dd->lockDevISR if you previously locked it */
-#define UNLOCKDEVISR(dd)						\
-	{								\
-		if (lockedDevISR) {					\
-			spin_unlock_irqrestore(&dd->lockDevISR, flags); \
-			lockedDevISR = FALSE;				\
-		}							\
-	}
-
-/** Locks LockGlobalISR if you havn't already locked it */
-#define LOCKGLOBALISR                                                  \
-	{                                                              \
-		if (!lockedGlobalISR) {				       \
-			spin_lock_irqsave(&LockGlobalISR, flags);      \
-			lockedGlobalISR = TRUE;			       \
-		}						       \
-	}
-
-/** Unlocks LockGlobalISR if you previously locked it */
-#define UNLOCKGLOBALISR                                                \
-	{                                                              \
-		if (lockedGlobalISR) {				       \
-			spin_unlock_irqrestore(&LockGlobalISR, flags); \
-			lockedGlobalISR = FALSE;		       \
-		}						       \
-	}
-
-/** Locks LockGlobal if you havn't already locked it */
-#define LOCKGLOBAL                                                     \
-	{                                                              \
-		if (!lockedGlobal) {				       \
-			spin_lock(&LockGlobal);			       \
-			lockedGlobal = TRUE;			       \
-		}						       \
-	}
-
-/** Unlocks LockGlobal if you previously locked it */
-#define UNLOCKGLOBAL                                                   \
-	{                                                              \
-		if (lockedGlobal) {				       \
-			spin_unlock(&LockGlobal);		       \
-			lockedGlobal = FALSE;			       \
-		}						       \
-	}
-
-/** Use this at the beginning of functions where you intend to
- *  use #LOCKDEV/#UNLOCKDEV, #LOCKDEVISR/#UNLOCKDEVISR,
- *  #LOCKGLOBAL/#UNLOCKGLOBAL, #LOCKGLOBALISR/#UNLOCKGLOBALISR.
- *
- *  Note that __attribute__((unused)) is how you tell GNU C to suppress
- *  any warning messages about the variable being unused.
- */
-#define LOCKPREAMBLE							\
-	ulong flags __attribute__((unused)) = 0;			\
-	BOOL lockedDev __attribute__((unused)) = FALSE;			\
-	BOOL lockedDevISR __attribute__((unused)) = FALSE;		\
-	BOOL lockedGlobal __attribute__((unused)) = FALSE;		\
-	BOOL lockedGlobalISR __attribute__((unused)) = FALSE
-
-
-
 /** Sleep for an indicated number of seconds (for use in kernel mode).
  *  @param x the number of seconds to sleep.
  */
@@ -297,13 +155,14 @@ typedef long VMMIO32;/**< #VMMIO pointing to 32-bit data */
 	} while (0)
 
 #ifndef max
-#define max(a, b) (((a) > (b)) ? (a):(b))
+#define max(a, b) (((a) > (b)) ? (a) : (b))
 #endif
 
 static inline struct cdev *cdev_alloc_init(struct module *owner,
 					   const struct file_operations *fops)
 {
 	struct cdev *cdev = NULL;
+
 	cdev = cdev_alloc();
 	if (!cdev)
 		return NULL;
@@ -319,6 +178,6 @@ static inline struct cdev *cdev_alloc_init(struct module *owner,
 	return cdev;
 }
 
-#include "timskmodutils.h"
+extern int unisys_spar_platform;
 
 #endif
diff --git a/drivers/staging/unisys/include/timskmodutils.h b/drivers/staging/unisys/include/timskmodutils.h
deleted file mode 100644
index c316c94ea194..000000000000
--- a/drivers/staging/unisys/include/timskmodutils.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/* timskmodutils.h
- *
- * Copyright (C) 2010 - 2013 UNISYS CORPORATION
- * All rights reserved.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or (at
- * your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE, GOOD TITLE or
- * NON INFRINGEMENT.  See the GNU General Public License for more
- * details.
- */
-
-#ifndef __TIMSKMODUTILS_H__
-#define __TIMSKMODUTILS_H__
-
-#include "timskmod.h"
-
-void *kmalloc_kernel(size_t siz);
-void  myprintk(const char *myDrvName, const char *devname,
-		const char *template, ...);
-
-/*--------------------------------*
- *---  GENERAL MESSAGEQ STUFF  ---*
- *--------------------------------*/
-
-struct MessageQEntry;
-
-/** the data structure used to hold an arbitrary data item that you want
- *  to place on a #MESSAGEQ.  Declare and initialize as follows:
- *
- *  This structure should be considered opaque; the client using it should
- *  never access the fields directly.
- *  Refer to these functions for more info:
- *
- *  @ingroup messageq
- */
-typedef struct MessageQEntry {
-	void *data;
-	struct MessageQEntry *qNext;
-	struct MessageQEntry *qPrev;
-} MESSAGEQENTRY;
-
-/** the data structure used to hold a FIFO queue of #MESSAGEQENTRY<b></b>s.
- *  Declare and initialize as follows:
- *  @code
- *      MESSAGEQ myQueue;
- *  @endcode
- *  This structure should be considered opaque; the client using it should
- *  never access the fields directly.
- *  Refer to these functions for more info:
- *
- *  @ingroup messageq
- */
-typedef struct MessageQ {
-	MESSAGEQENTRY *qHead;
-	MESSAGEQENTRY *qTail;
-	struct semaphore nQEntries;
-	spinlock_t       queueLock;
-} MESSAGEQ;
-
-char *cyclesToSeconds(u64 cycles, u64 cyclesPerSecond,
-		      char *buf, size_t bufsize);
-char *cyclesToIterationSeconds(u64 cycles, u64 cyclesPerSecond,
-			       u64 iterations, char *buf, size_t bufsize);
-char *cyclesToSomethingsPerSecond(u64 cycles, u64 cyclesPerSecond,
-				  u64 somethings, char *buf, size_t bufsize);
-struct seq_file *visor_seq_file_new_buffer(void *buf, size_t buf_size);
-void visor_seq_file_done_buffer(struct seq_file *m);
-
-extern int unisys_spar_platform;
-
-#endif
diff --git a/drivers/staging/unisys/include/uisqueue.h b/drivers/staging/unisys/include/uisqueue.h
index a2abfa8c82fd..5178270b98d1 100644
--- a/drivers/staging/unisys/include/uisqueue.h
+++ b/drivers/staging/unisys/include/uisqueue.h
@@ -48,13 +48,13 @@ struct uisqueue_info {
 	u64 non_empty_wakeup_cnt;
 
 	struct {
-		SIGNAL_QUEUE_HEADER Reserved1;	/*  */
-		SIGNAL_QUEUE_HEADER Reserved2;	/*  */
+		SIGNAL_QUEUE_HEADER reserved1;	/*  */
+		SIGNAL_QUEUE_HEADER reserved2;	/*  */
 	} safe_uis_queue;
 	unsigned int (*send_int_if_needed)(struct uisqueue_info *info,
 					   unsigned int whichcqueue,
-					   unsigned char issueInterruptIfEmpty,
-					   u64 interruptHandle,
+					   unsigned char issue_irq_if_empty,
+					   u64 irq_handle,
 					   unsigned char io_termination);
 };
 
@@ -76,25 +76,19 @@ struct uisqueue_info {
 		".previous\n"                   \
 		"661:\n\tlock; "
 
-unsigned long long uisqueue_InterlockedOr(unsigned long long __iomem *Target,
-					  unsigned long long Set);
-unsigned long long uisqueue_InterlockedAnd(unsigned long long __iomem *Target,
-					   unsigned long long Set);
-
-unsigned int uisqueue_send_int_if_needed(struct uisqueue_info *pqueueinfo,
-					 unsigned int whichqueue,
-					 unsigned char issueInterruptIfEmpty,
-					 u64 interruptHandle,
-					 unsigned char io_termination);
+unsigned long long uisqueue_interlocked_or(unsigned long long __iomem *tgt,
+					   unsigned long long set);
+unsigned long long uisqueue_interlocked_and(unsigned long long __iomem *tgt,
+					    unsigned long long set);
 
 int uisqueue_put_cmdrsp_with_lock_client(struct uisqueue_info *queueinfo,
 					 struct uiscmdrsp *cmdrsp,
 					 unsigned int queue,
 					 void *insertlock,
-					 unsigned char issueInterruptIfEmpty,
-					 u64 interruptHandle,
+					 unsigned char issue_irq_if_empty,
+					 u64 irq_handle,
 					 char oktowait,
-					 u8 *channelId);
+					 u8 *channel_id);
 
 /* uisqueue_get_cmdrsp gets the cmdrsp entry at the head of the queue
  * and copies it to the area pointed by cmdrsp param.
@@ -135,17 +129,17 @@ struct extport_info {
 
 struct device_info {
 	void __iomem *chanptr;
-	u64 channelAddr;
-	u64 channelBytes;
-	uuid_le channelTypeGuid;
-	uuid_le devInstGuid;
+	u64 channel_addr;
+	u64 channel_bytes;
+	uuid_le channel_uuid;
+	uuid_le instance_uuid;
 	struct InterruptInfo intr;
 	struct switch_info *swtch;
 	char devid[30];		/* "vbus<busno>:dev<devno>" */
 	u16 polling;
 	struct semaphore interrupt_callback_lock;
-	u32 busNo;
-	u32 devNo;
+	u32 bus_no;
+	u32 dev_no;
 	int (*interrupt)(void *);
 	void *interrupt_context;
 	void *private_data;
@@ -176,8 +170,8 @@ struct bus_info {
 				 * by IOVM; 0 otherwise... */
 };
 
-#define DEDICATED_SWITCH(pSwitch) ((pSwitch->extPortCount == 1) &&	\
-				   (pSwitch->intPortCount == 1))
+#define DEDICATED_SWITCH(s) ((s->extPortCount == 1) &&	\
+			     (s->intPortCount == 1))
 
 struct sn_list_entry {
 	struct uisscsi_dest pdest;	/* scsi bus, target, lun for
@@ -189,7 +183,7 @@ struct sn_list_entry {
 	struct sn_list_entry *next;
 };
 
-struct networkPolicy {
+struct network_policy {
 	u32 promiscuous:1;
 	u32 macassign:1;
 	u32 peerforwarding:1;
@@ -229,10 +223,10 @@ typedef enum {
 
 struct add_virt_iopart {
 	void *chanptr;		/* pointer to data channel */
-	u64 guestHandle;	/* used to convert guest physical
+	u64 guest_handle;	/* used to convert guest physical
 				 * address to real physical address
 				 * for DMA, for ex. */
-	u64 recvBusInterruptHandle;	/* used to register to receive
+	u64 recv_bus_irq_handle;	/* used to register to receive
 					 * bus level interrupts. */
 	struct InterruptInfo intr;	/* contains recv & send
 					 * interrupt info */
@@ -247,12 +241,12 @@ struct add_virt_iopart {
 					 * switch to which the vnic is
 					 * connected */
 
-	u8 useG2GCopy;		/* Used to determine if a virtual HBA
+	u8 use_g2g_copy;	/* Used to determine if a virtual HBA
 				 * needs to use G2G copy. */
-	u8 Filler[7];
+	u8 filler[7];
 
-	u32 busNo;
-	u32 devNo;
+	u32 bus_no;
+	u32 dev_no;
 	char *params;
 	ulong params_bytes;
 
@@ -265,21 +259,21 @@ struct add_vdisk_iopart {
 	struct uisscsi_dest pdest;    /* scsi bus, target, lun for phys disk */
 	u8 sernum[MAX_SERIAL_NUM];    /* serial num of physical disk */
 	u32 serlen;		      /* length of serial num */
-	u32 busNo;
-	u32 devNo;
+	u32 bus_no;
+	u32 dev_no;
 };
 
 struct del_vdisk_iopart {
 	void *chanptr;		     /* pointer to data channel */
 	struct uisscsi_dest vdest;   /* scsi bus, target, lun for virt disk */
-	u32 busNo;
-	u32 devNo;
+	u32 bus_no;
+	u32 dev_no;
 };
 
 struct del_virt_iopart {
 	void *chanptr;		     /* pointer to data channel */
-	u32 busNo;
-	u32 devNo;
+	u32 bus_no;
+	u32 dev_no;
 };
 
 struct det_virt_iopart {	     /* detach internal port */
@@ -322,7 +316,7 @@ struct io_msgs {
 		struct det_virt_iopart det_intport;
 		struct add_switch_iopart add_switch;
 		struct del_switch_iopart del_switch;
-		struct extport_info *extPort;	/* for attach or detach
+		struct extport_info *ext_port;	/* for attach or detach
 						 * pnic/generic delete all
 						 * vhbas/allvnics need no
 						 * parameters */
@@ -362,7 +356,7 @@ struct add_vbus_guestpart {
 };
 
 struct del_vbus_guestpart {
-	u32 busNo;		/* bus number to be deleted */
+	u32 bus_no;		/* bus number to be deleted */
 	/* once we start using the bus's channel, add can dump busNo
 	* into the channel header and then delete will need only one
 	* parameter, chanptr. */
@@ -370,9 +364,9 @@ struct del_vbus_guestpart {
 
 struct add_virt_guestpart {
 	void __iomem *chanptr;		/* pointer to data channel */
-	u32 busNo;		/* bus number for the operation */
-	u32 deviceNo;		/* number of device on the bus */
-	uuid_le devInstGuid;	/* instance guid for device */
+	u32 bus_no;		/* bus number for the operation */
+	u32 device_no;		/* number of device on the bus */
+	uuid_le instance_uuid;	/* instance guid for device */
 	struct InterruptInfo intr;	/* recv/send interrupt info */
 	/* recvInterruptHandle contains info needed in order to
 	 * register to receive interrupts on the data channel.
@@ -395,8 +389,8 @@ struct del_virt_guestpart {
 };
 
 struct init_chipset_guestpart {
-	u32 busCount;		/* indicates the max number of busses */
-	u32 switchCount;	/* indicates the max number of switches */
+	u32 bus_count;		/* indicates the max number of busses */
+	u32 switch_count;	/* indicates the max number of switches */
 };
 
 struct guest_msgs {
@@ -423,19 +417,4 @@ struct guest_msgs {
 
 };
 
-#ifndef __xg
-#define __xg(x) ((volatile long *)(x))
-#endif
-
-/*
-*  Below code is a copy of Linux kernel's cmpxchg function located at
-*  this place
-*  http://tcsxeon:8080/source/xref/00trunk-AppOS-linux/include/asm-x86/cmpxchg_64.h#84
-*  Reason for creating our own version of cmpxchg along with
-*  UISLIB_LOCK_PREFIX is to make the operation atomic even for non SMP
-*  guests.
-*/
-
-#define uislibcmpxchg64(p, o, n, s) cmpxchg(p, o, n)
-
 #endif				/* __UISQUEUE_H__ */
diff --git a/drivers/staging/unisys/include/uisthread.h b/drivers/staging/unisys/include/uisthread.h
index 26837771b43f..aa86ade7cb13 100644
--- a/drivers/staging/unisys/include/uisthread.h
+++ b/drivers/staging/unisys/include/uisthread.h
@@ -19,11 +19,9 @@
 /* Unisys thread utilities header                                            */
 /*****************************************************************************/
 
-
 #ifndef __UISTHREAD_H__
 #define __UISTHREAD_H__
 
-
 #include "linux/completion.h"
 
 struct uisthread_info {
@@ -33,7 +31,6 @@ struct uisthread_info {
 	struct completion has_stopped;
 };
 
-
 /* returns 0 for failure, 1 for success */
 int uisthread_start(
 	struct uisthread_info *thrinfo,
diff --git a/drivers/staging/unisys/include/uisutils.h b/drivers/staging/unisys/include/uisutils.h
index a1c193c5827e..74e7cf65502c 100644
--- a/drivers/staging/unisys/include/uisutils.h
+++ b/drivers/staging/unisys/include/uisutils.h
@@ -84,6 +84,7 @@ static inline void __iomem *
 dbg_ioremap_cache(u64 addr, unsigned long size, char *file, int line)
 {
 	void __iomem *new;
+
 	new = ioremap_cache(addr, size);
 	return new;
 }
@@ -94,6 +95,7 @@ static inline void *
 dbg_ioremap(u64 addr, unsigned long size, char *file, int line)
 {
 	void *new;
+
 	new = ioremap(addr, size);
 	return new;
 }
@@ -112,7 +114,7 @@ int uisutil_add_proc_line_ex(int *total, char **buffer, int *buffer_remaining,
 			     char *format, ...);
 
 int uisctrl_register_req_handler(int type, void *fptr,
-				 ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo);
+				 ULTRA_VBUS_DEVICEINFO *chipset_driver_info);
 int uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
 				    const char *switch_type_name,
 				    int (*fptr)(struct io_msgs *),
@@ -220,7 +222,7 @@ unsigned int uisutil_copy_fragsinfo_from_skb(unsigned char *calling_ctx,
 					     struct phys_info frags[]);
 
 static inline unsigned int
-Issue_VMCALL_IO_CONTROLVM_ADDR(u64 *ControlAddress, u32 *ControlBytes)
+issue_vmcall_io_controlvm_addr(u64 *control_addr, u32 *control_bytes)
 {
 	VMCALL_IO_CONTROLVM_ADDR_PARAMS params;
 	int result = VMCALL_SUCCESS;
@@ -229,13 +231,13 @@ Issue_VMCALL_IO_CONTROLVM_ADDR(u64 *ControlAddress, u32 *ControlBytes)
 	physaddr = virt_to_phys(&params);
 	ISSUE_IO_VMCALL(VMCALL_IO_CONTROLVM_ADDR, physaddr, result);
 	if (VMCALL_SUCCESSFUL(result)) {
-		*ControlAddress = params.ChannelAddress;
-		*ControlBytes = params.ChannelBytes;
+		*control_addr = params.ChannelAddress;
+		*control_bytes = params.ChannelBytes;
 	}
 	return result;
 }
 
-static inline unsigned int Issue_VMCALL_IO_DIAG_ADDR(u64 *DiagChannelAddress)
+static inline unsigned int issue_vmcall_io_diag_addr(u64 *diag_channel_addr)
 {
 	VMCALL_IO_DIAG_ADDR_PARAMS params;
 	int result = VMCALL_SUCCESS;
@@ -244,12 +246,11 @@ static inline unsigned int Issue_VMCALL_IO_DIAG_ADDR(u64 *DiagChannelAddress)
 	physaddr = virt_to_phys(&params);
 	ISSUE_IO_VMCALL(VMCALL_IO_DIAG_ADDR, physaddr, result);
 	if (VMCALL_SUCCESSFUL(result))
-		*DiagChannelAddress = params.ChannelAddress;
+		*diag_channel_addr = params.ChannelAddress;
 	return result;
 }
 
-static inline unsigned int
-Issue_VMCALL_IO_VISORSERIAL_ADDR(u64 *DiagChannelAddress)
+static inline unsigned int issue_vmcall_io_visorserial_addr(u64 *channel_addr)
 {
 	VMCALL_IO_VISORSERIAL_ADDR_PARAMS params;
 	int result = VMCALL_SUCCESS;
@@ -258,11 +259,11 @@ Issue_VMCALL_IO_VISORSERIAL_ADDR(u64 *DiagChannelAddress)
 	physaddr = virt_to_phys(&params);
 	ISSUE_IO_VMCALL(VMCALL_IO_VISORSERIAL_ADDR, physaddr, result);
 	if (VMCALL_SUCCESSFUL(result))
-		*DiagChannelAddress = params.ChannelAddress;
+		*channel_addr = params.ChannelAddress;
 	return result;
 }
 
-static inline s64 Issue_VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET(void)
+static inline s64 issue_vmcall_query_guest_virtual_time_offset(void)
 {
 	u64 result = VMCALL_SUCCESS;
 	u64 physaddr = 0;
@@ -272,7 +273,7 @@ static inline s64 Issue_VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET(void)
 	return result;
 }
 
-static inline s64 Issue_VMCALL_MEASUREMENT_DO_NOTHING(void)
+static inline s64 issue_vmcall_measurement_do_nothing(void)
 {
 	u64 result = VMCALL_SUCCESS;
 	u64 physaddr = 0;
@@ -289,7 +290,7 @@ struct log_info_t {
 	unsigned long long min_delta[64];
 };
 
-static inline int Issue_VMCALL_UPDATE_PHYSICAL_TIME(u64 adjustment)
+static inline int issue_vmcall_update_physical_time(u64 adjustment)
 {
 	int result = VMCALL_SUCCESS;
 
@@ -297,21 +298,20 @@ static inline int Issue_VMCALL_UPDATE_PHYSICAL_TIME(u64 adjustment)
 	return result;
 }
 
-static inline unsigned int
-Issue_VMCALL_CHANNEL_MISMATCH(const char *ChannelName,
-			      const char *ItemName,
-			      u32 SourceLineNumber, const char *path_n_fn)
+static inline unsigned int issue_vmcall_channel_mismatch(const char *chname,
+			      const char *item_name, u32 line_no,
+			      const char *path_n_fn)
 {
 	VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS params;
 	int result = VMCALL_SUCCESS;
 	u64 physaddr;
 	char *last_slash = NULL;
 
-	strlcpy(params.ChannelName, ChannelName,
+	strlcpy(params.ChannelName, chname,
 		lengthof(VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS, ChannelName));
-	strlcpy(params.ItemName, ItemName,
+	strlcpy(params.ItemName, item_name,
 		lengthof(VMCALL_CHANNEL_VERSION_MISMATCH_PARAMS, ItemName));
-	params.SourceLineNumber = SourceLineNumber;
+	params.SourceLineNumber = line_no;
 
 	last_slash = strrchr(path_n_fn, '/');
 	if (last_slash != NULL) {
@@ -330,7 +330,7 @@ Issue_VMCALL_CHANNEL_MISMATCH(const char *ChannelName,
 	return result;
 }
 
-static inline unsigned int Issue_VMCALL_FATAL_BYE_BYE(void)
+static inline unsigned int issue_vmcall_fatal(void)
 {
 	int result = VMCALL_SUCCESS;
 	u64 physaddr = 0;
@@ -347,10 +347,10 @@ void uislib_cache_free(struct kmem_cache *cur_pool, void *p, char *fn, int ln);
 #define UISCACHEFREE(cur_pool, p) \
 	uislib_cache_free(cur_pool, p, __FILE__, __LINE__)
 
-void uislib_enable_channel_interrupts(u32 busNo, u32 devNo,
+void uislib_enable_channel_interrupts(u32 bus_no, u32 dev_no,
 				      int (*interrupt)(void *),
 				      void *interrupt_context);
-void uislib_disable_channel_interrupts(u32 busNo, u32 devNo);
-void uislib_force_channel_interrupt(u32 busNo, u32 devNo);
+void uislib_disable_channel_interrupts(u32 bus_no, u32 dev_no);
+void uislib_force_channel_interrupt(u32 bus_no, u32 dev_no);
 
 #endif /* __UISUTILS__H__ */
diff --git a/drivers/staging/unisys/include/uniklog.h b/drivers/staging/unisys/include/uniklog.h
index 6178cc46f7c1..ecd1bdb6d097 100644
--- a/drivers/staging/unisys/include/uniklog.h
+++ b/drivers/staging/unisys/include/uniklog.h
@@ -23,7 +23,6 @@
 #ifndef __UNIKLOG_H__
 #define __UNIKLOG_H__
 
-
 #include <linux/printk.h>
 
 /*
@@ -122,7 +121,6 @@
 		}							\
 	} while (0)
 
-
 /*
  * # LOGERR
  *
diff --git a/drivers/staging/unisys/include/vbushelper.h b/drivers/staging/unisys/include/vbushelper.h
index ed943759634a..1bde549ec0df 100644
--- a/drivers/staging/unisys/include/vbushelper.h
+++ b/drivers/staging/unisys/include/vbushelper.h
@@ -1,6 +1,6 @@
 /* vbushelper.h
  *
- * Copyright © 2011 - 2013 UNISYS CORPORATION
+ * Copyright (C) 2011 - 2013 UNISYS CORPORATION
  * All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
@@ -25,20 +25,22 @@
 
 #define TARGET_HOSTNAME "linuxguest"
 
-static inline void
-BusDeviceInfo_Init(ULTRA_VBUS_DEVICEINFO *pBusDeviceInfo,
-		   const char *deviceType, const char *driverName,
-		   const char *ver, const char *verTag)
+static inline void bus_device_info_init(
+		ULTRA_VBUS_DEVICEINFO * bus_device_info_ptr,
+		const char *dev_type, const char *drv_name,
+		const char *ver, const char *ver_tag)
 {
-	memset(pBusDeviceInfo, 0, sizeof(ULTRA_VBUS_DEVICEINFO));
-	snprintf(pBusDeviceInfo->devType, sizeof(pBusDeviceInfo->devType),
-		 "%s", (deviceType) ? deviceType : "unknownType");
-	snprintf(pBusDeviceInfo->drvName, sizeof(pBusDeviceInfo->drvName),
-		 "%s", (driverName) ? driverName : "unknownDriver");
-	snprintf(pBusDeviceInfo->infoStrings,
-		 sizeof(pBusDeviceInfo->infoStrings), "%s\t%s\t%s",
+	memset(bus_device_info_ptr, 0, sizeof(ULTRA_VBUS_DEVICEINFO));
+	snprintf(bus_device_info_ptr->devType,
+		 sizeof(bus_device_info_ptr->devType),
+		 "%s", (dev_type) ? dev_type : "unknownType");
+	snprintf(bus_device_info_ptr->drvName,
+		 sizeof(bus_device_info_ptr->drvName),
+		 "%s", (drv_name) ? drv_name : "unknownDriver");
+	snprintf(bus_device_info_ptr->infoStrings,
+		 sizeof(bus_device_info_ptr->infoStrings), "%s\t%s\t%s",
 		 (ver) ? ver : "unknownVer",
-		 (verTag) ? verTag : "unknownVerTag",
+		 (ver_tag) ? ver_tag : "unknownVerTag",
 		 TARGET_HOSTNAME);
 }
 
diff --git a/drivers/staging/unisys/uislib/Kconfig b/drivers/staging/unisys/uislib/Kconfig
index 8d87d9c81c66..6b134e267904 100644
--- a/drivers/staging/unisys/uislib/Kconfig
+++ b/drivers/staging/unisys/uislib/Kconfig
@@ -4,7 +4,7 @@
 
 config UNISYS_UISLIB
 	tristate "Unisys uislib driver"
-	depends on UNISYSSPAR && UNISYS_VISORCHIPSET && UNISYS_CHANNELSTUB
+	depends on UNISYSSPAR && UNISYS_VISORCHIPSET && UNISYS_CHANNELSTUB && HAS_IOMEM
 	---help---
 	If you say Y here, you will enable the Unisys uislib driver.
 
diff --git a/drivers/staging/unisys/uislib/uislib.c b/drivers/staging/unisys/uislib/uislib.c
index 63c91cd6fdcc..706f1c0c2c6e 100644
--- a/drivers/staging/unisys/uislib/uislib.c
+++ b/drivers/staging/unisys/uislib/uislib.c
@@ -25,7 +25,8 @@
 #include <linux/module.h>
 #include <linux/debugfs.h>
 
-#include "commontypes.h"
+#include <linux/types.h>
+#include <linux/uuid.h>
 
 #include <linux/version.h>
 #include "uniklog.h"
@@ -133,6 +134,7 @@ init_vbus_channel(u64 channelAddr, u32 channelBytes)
 {
 	void __iomem *rc = NULL;
 	void __iomem *pChan = uislib_ioremap_cache(channelAddr, channelBytes);
+
 	if (!pChan) {
 		LOGERR("CONTROLVM_BUS_CREATE error: ioremap_cache of channelAddr:%Lx for channelBytes:%llu failed",
 		     (unsigned long long) channelAddr,
@@ -229,6 +231,7 @@ create_bus(CONTROLVM_MESSAGE *msg, char *buf)
 	/* the msg is bound for virtpci; send guest_msgs struct to callback */
 	if (!msg->hdr.Flags.server) {
 		struct guest_msgs cmd;
+
 		cmd.msgtype = GUEST_ADD_VBUS;
 		cmd.add_vbus.busNo = busNo;
 		cmd.add_vbus.chanptr = bus->pBusChannel;
@@ -273,52 +276,19 @@ destroy_bus(CONTROLVM_MESSAGE *msg, char *buf)
 {
 	int i;
 	struct bus_info *bus, *prev = NULL;
+	struct guest_msgs cmd;
 	u32 busNo;
 
 	busNo = msg->cmd.destroyBus.busNo;
 
-	/* find and delete the bus */
 	read_lock(&BusListLock);
-	for (bus = BusListHead; bus; prev = bus, bus = bus->next) {
-		if (bus->busNo == busNo) {
-			/* found the bus - ensure that all device
-			 * slots are NULL
-			 */
-			for (i = 0; i < bus->deviceCount; i++) {
-				if (bus->device[i] != NULL) {
-					LOGERR("CONTROLVM_BUS_DESTROY Failed: device %i attached to bus %d.",
-					     i, busNo);
-					read_unlock(&BusListLock);
-					return CONTROLVM_RESP_ERROR_BUS_DEVICE_ATTACHED;
-				}
-			}
-			read_unlock(&BusListLock);
-			/* the msg is bound for virtpci; send
-			 * guest_msgs struct to callback
-			 */
-			if (!msg->hdr.Flags.server) {
-				struct guest_msgs cmd;
-				cmd.msgtype = GUEST_DEL_VBUS;
-				cmd.del_vbus.busNo = busNo;
-				if (!VirtControlChanFunc) {
-					LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci callback not registered.");
-					return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-				}
-				if (!VirtControlChanFunc(&cmd)) {
-					LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci GUEST_DEL_VBUS returned error.");
-					return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-				}
-			}
-			/* remove the bus from the list */
-			write_lock(&BusListLock);
-			if (prev)	/* not at head */
-				prev->next = bus->next;
-			else
-				BusListHead = bus->next;
-			BusListCount--;
-			write_unlock(&BusListLock);
+
+	bus = BusListHead;
+	while (bus) {
+		if (bus->busNo == busNo)
 			break;
-		}
+		prev = bus;
+		bus = bus->next;
 	}
 
 	if (!bus) {
@@ -327,6 +297,44 @@ destroy_bus(CONTROLVM_MESSAGE *msg, char *buf)
 		read_unlock(&BusListLock);
 		return CONTROLVM_RESP_ERROR_ALREADY_DONE;
 	}
+
+	/* verify that this bus has no devices. */
+	for (i = 0; i < bus->deviceCount; i++) {
+		if (bus->device[i] != NULL) {
+			LOGERR("CONTROLVM_BUS_DESTROY Failed: device %i attached to bus %d.",
+			     i, busNo);
+			read_unlock(&BusListLock);
+			return CONTROLVM_RESP_ERROR_BUS_DEVICE_ATTACHED;
+		}
+	}
+	read_unlock(&BusListLock);
+
+	if (msg->hdr.Flags.server)
+		goto remove;
+
+	/* client messages require us to call the virtpci callback associated
+	   with this bus. */
+	cmd.msgtype = GUEST_DEL_VBUS;
+	cmd.del_vbus.bus_no = busNo;
+	if (!VirtControlChanFunc) {
+		LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci callback not registered.");
+		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
+	}
+	if (!VirtControlChanFunc(&cmd)) {
+		LOGERR("CONTROLVM_BUS_DESTROY Failed: virtpci GUEST_DEL_VBUS returned error.");
+		return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
+	}
+
+	/* finally, remove the bus from the list */
+remove:
+	write_lock(&BusListLock);
+	if (prev)	/* not at head */
+		prev->next = bus->next;
+	else
+		BusListHead = bus->next;
+	BusListCount--;
+	write_unlock(&BusListLock);
+
 	if (bus->pBusChannel) {
 		uislib_iounmap(bus->pBusChannel);
 		bus->pBusChannel = NULL;
@@ -360,18 +368,18 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 		return CONTROLVM_RESP_ERROR_KMALLOC_FAILED;
 	}
 
-	dev->channelTypeGuid = msg->cmd.createDevice.dataTypeGuid;
+	dev->channel_uuid = msg->cmd.createDevice.dataTypeGuid;
 	dev->intr = msg->cmd.createDevice.intr;
-	dev->channelAddr = msg->cmd.createDevice.channelAddr;
-	dev->busNo = busNo;
-	dev->devNo = devNo;
+	dev->channel_addr = msg->cmd.createDevice.channelAddr;
+	dev->bus_no = busNo;
+	dev->dev_no = devNo;
 	sema_init(&dev->interrupt_callback_lock, 1);	/* unlocked */
 	sprintf(dev->devid, "vbus%u:dev%u", (unsigned) busNo, (unsigned) devNo);
 	/* map the channel memory for the device. */
 	if (msg->hdr.Flags.testMessage)
-		dev->chanptr = (void __iomem *)__va(dev->channelAddr);
+		dev->chanptr = (void __iomem *)__va(dev->channel_addr);
 	else {
-		pReqHandler = ReqHandlerFind(dev->channelTypeGuid);
+		pReqHandler = ReqHandlerFind(dev->channel_uuid);
 		if (pReqHandler)
 			/* generic service handler registered for this
 			 * channel
@@ -387,11 +395,11 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 			goto Away;
 		}
 		dev->chanptr =
-		    uislib_ioremap_cache(dev->channelAddr,
+		    uislib_ioremap_cache(dev->channel_addr,
 					 msg->cmd.createDevice.channelBytes);
 		if (!dev->chanptr) {
 			LOGERR("CONTROLVM_DEVICE_CREATE Failed: ioremap_cache of channelAddr:%Lx for channelBytes:%llu failed",
-			     dev->channelAddr,
+			     dev->channel_addr,
 			     msg->cmd.createDevice.channelBytes);
 			result = CONTROLVM_RESP_ERROR_IOREMAP_FAILED;
 			POSTCODE_LINUX_4(DEVICE_CREATE_FAILURE_PC, devNo, busNo,
@@ -399,8 +407,8 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 			goto Away;
 		}
 	}
-	dev->devInstGuid = msg->cmd.createDevice.devInstGuid;
-	dev->channelBytes = msg->cmd.createDevice.channelBytes;
+	dev->instance_uuid = msg->cmd.createDevice.devInstGuid;
+	dev->channel_bytes = msg->cmd.createDevice.channelBytes;
 
 	read_lock(&BusListLock);
 	for (bus = BusListHead; bus; bus = bus->next) {
@@ -433,7 +441,8 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 			 */
 			if (!msg->hdr.Flags.server) {
 				struct guest_msgs cmd;
-				if (!uuid_le_cmp(dev->channelTypeGuid,
+
+				if (!uuid_le_cmp(dev->channel_uuid,
 				     UltraVhbaChannelProtocolGuid)) {
 					wait_for_valid_guid(&((CHANNEL_HEADER
 							      __iomem *) (dev->
@@ -452,13 +461,13 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 					}
 					cmd.msgtype = GUEST_ADD_VHBA;
 					cmd.add_vhba.chanptr = dev->chanptr;
-					cmd.add_vhba.busNo = busNo;
-					cmd.add_vhba.deviceNo = devNo;
-					cmd.add_vhba.devInstGuid =
-					    dev->devInstGuid;
+					cmd.add_vhba.bus_no = busNo;
+					cmd.add_vhba.device_no = devNo;
+					cmd.add_vhba.instance_uuid =
+					    dev->instance_uuid;
 					cmd.add_vhba.intr = dev->intr;
 				} else
-				    if (!uuid_le_cmp(dev->channelTypeGuid,
+				    if (!uuid_le_cmp(dev->channel_uuid,
 					 UltraVnicChannelProtocolGuid)) {
 					wait_for_valid_guid(&((CHANNEL_HEADER
 							      __iomem *) (dev->
@@ -477,10 +486,10 @@ create_device(CONTROLVM_MESSAGE *msg, char *buf)
 					}
 					cmd.msgtype = GUEST_ADD_VNIC;
 					cmd.add_vnic.chanptr = dev->chanptr;
-					cmd.add_vnic.busNo = busNo;
-					cmd.add_vnic.deviceNo = devNo;
-					cmd.add_vnic.devInstGuid =
-					    dev->devInstGuid;
+					cmd.add_vnic.bus_no = busNo;
+					cmd.add_vnic.device_no = devNo;
+					cmd.add_vnic.instance_uuid =
+					    dev->instance_uuid;
 					cmd.add_vhba.intr = dev->intr;
 				} else {
 					LOGERR("CONTROLVM_DEVICE_CREATE Failed: unknown channelTypeGuid.\n");
@@ -539,6 +548,7 @@ pause_device(CONTROLVM_MESSAGE *msg)
 	struct bus_info *bus;
 	struct device_info *dev;
 	struct guest_msgs cmd;
+	int retval = CONTROLVM_RESP_SUCCESS;
 
 	busNo = msg->cmd.deviceChangeState.busNo;
 	devNo = msg->cmd.deviceChangeState.devNo;
@@ -550,58 +560,53 @@ pause_device(CONTROLVM_MESSAGE *msg)
 			if (devNo >= bus->deviceCount) {
 				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: device(%d) >= deviceCount(%d).",
 				     devNo, bus->deviceCount);
-				read_unlock(&BusListLock);
-				return CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-			}
-			/* make sure this device exists */
-			dev = bus->device[devNo];
-			if (!dev) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: device %d does not exist.",
-				     devNo);
-				read_unlock(&BusListLock);
-				return CONTROLVM_RESP_ERROR_ALREADY_DONE;
-			}
-			read_unlock(&BusListLock);
-			/* the msg is bound for virtpci; send
-			 * guest_msgs struct to callback
-			 */
-			if (!uuid_le_cmp(dev->channelTypeGuid,
-					UltraVhbaChannelProtocolGuid)) {
-				cmd.msgtype = GUEST_PAUSE_VHBA;
-				cmd.pause_vhba.chanptr = dev->chanptr;
-			} else
-			    if (!uuid_le_cmp(dev->channelTypeGuid,
-					    UltraVnicChannelProtocolGuid)) {
-				cmd.msgtype = GUEST_PAUSE_VNIC;
-				cmd.pause_vnic.chanptr = dev->chanptr;
+				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: unknown channelTypeGuid.\n");
-				return
-				    CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
-			}
-
-			if (!VirtControlChanFunc) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
-				return
-				    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-			}
-
-			if (!VirtControlChanFunc(&cmd)) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: virtpci GUEST_PAUSE_[VHBA||VNIC] returned error.");
-				return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
+				/* make sure this device exists */
+				dev = bus->device[devNo];
+				if (!dev) {
+					LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: device %d does not exist.",
+					     devNo);
+					retval =
+					  CONTROLVM_RESP_ERROR_ALREADY_DONE;
+				}
 			}
 			break;
 		}
 	}
-
 	if (!bus) {
 		LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: bus %d does not exist",
 		     busNo);
-		read_unlock(&BusListLock);
-		return CONTROLVM_RESP_ERROR_BUS_INVALID;
+		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
 	}
-
-	return CONTROLVM_RESP_SUCCESS;
+	read_unlock(&BusListLock);
+	if (retval == CONTROLVM_RESP_SUCCESS) {
+		/* the msg is bound for virtpci; send
+		 * guest_msgs struct to callback
+		 */
+		if (!uuid_le_cmp(dev->channel_uuid,
+				UltraVhbaChannelProtocolGuid)) {
+			cmd.msgtype = GUEST_PAUSE_VHBA;
+			cmd.pause_vhba.chanptr = dev->chanptr;
+		} else if (!uuid_le_cmp(dev->channel_uuid,
+					UltraVnicChannelProtocolGuid)) {
+			cmd.msgtype = GUEST_PAUSE_VNIC;
+			cmd.pause_vnic.chanptr = dev->chanptr;
+		} else {
+			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: unknown channelTypeGuid.\n");
+			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
+		}
+		if (!VirtControlChanFunc) {
+			LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
+			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
+		}
+		if (!VirtControlChanFunc(&cmd)) {
+			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:pause Failed: virtpci GUEST_PAUSE_[VHBA||VNIC] returned error.");
+			return
+			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
+		}
+	}
+	return retval;
 }
 
 static int
@@ -611,6 +616,7 @@ resume_device(CONTROLVM_MESSAGE *msg)
 	struct bus_info *bus;
 	struct device_info *dev;
 	struct guest_msgs cmd;
+	int retval = CONTROLVM_RESP_SUCCESS;
 
 	busNo = msg->cmd.deviceChangeState.busNo;
 	devNo = msg->cmd.deviceChangeState.devNo;
@@ -622,45 +628,16 @@ resume_device(CONTROLVM_MESSAGE *msg)
 			if (devNo >= bus->deviceCount) {
 				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: device(%d) >= deviceCount(%d).",
 				     devNo, bus->deviceCount);
-				read_unlock(&BusListLock);
-				return CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-			}
-			/* make sure this device exists */
-			dev = bus->device[devNo];
-			if (!dev) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: device %d does not exist.",
-				     devNo);
-				read_unlock(&BusListLock);
-				return CONTROLVM_RESP_ERROR_ALREADY_DONE;
-			}
-			read_unlock(&BusListLock);
-			/* the msg is bound for virtpci; send
-			 * guest_msgs struct to callback
-			 */
-			if (!uuid_le_cmp(dev->channelTypeGuid,
-					UltraVhbaChannelProtocolGuid)) {
-				cmd.msgtype = GUEST_RESUME_VHBA;
-				cmd.resume_vhba.chanptr = dev->chanptr;
-			} else
-			    if (!uuid_le_cmp(dev->channelTypeGuid,
-					    UltraVnicChannelProtocolGuid)) {
-				cmd.msgtype = GUEST_RESUME_VNIC;
-				cmd.resume_vnic.chanptr = dev->chanptr;
+				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: unknown channelTypeGuid.\n");
-				return
-				    CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
-			}
-
-			if (!VirtControlChanFunc) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
-				return
-				    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-			}
-
-			if (!VirtControlChanFunc(&cmd)) {
-				LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: virtpci GUEST_RESUME_[VHBA||VNIC] returned error.");
-				return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
+				/* make sure this device exists */
+				dev = bus->device[devNo];
+				if (!dev) {
+					LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: device %d does not exist.",
+					     devNo);
+					retval =
+					  CONTROLVM_RESP_ERROR_ALREADY_DONE;
+				}
 			}
 			break;
 		}
@@ -669,11 +646,36 @@ resume_device(CONTROLVM_MESSAGE *msg)
 	if (!bus) {
 		LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: bus %d does not exist",
 		     busNo);
-		read_unlock(&BusListLock);
-		return CONTROLVM_RESP_ERROR_BUS_INVALID;
+		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
 	}
-
-	return CONTROLVM_RESP_SUCCESS;
+	read_unlock(&BusListLock);
+	/* the msg is bound for virtpci; send
+	 * guest_msgs struct to callback
+	 */
+	if (retval == CONTROLVM_RESP_SUCCESS) {
+		if (!uuid_le_cmp(dev->channel_uuid,
+				 UltraVhbaChannelProtocolGuid)) {
+			cmd.msgtype = GUEST_RESUME_VHBA;
+			cmd.resume_vhba.chanptr = dev->chanptr;
+		} else if (!uuid_le_cmp(dev->channel_uuid,
+					UltraVnicChannelProtocolGuid)) {
+			cmd.msgtype = GUEST_RESUME_VNIC;
+			cmd.resume_vnic.chanptr = dev->chanptr;
+		} else {
+			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: unknown channelTypeGuid.\n");
+			return CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
+		}
+		if (!VirtControlChanFunc) {
+			LOGERR("CONTROLVM_DEVICE_CHANGESTATE Failed: virtpci callback not registered.");
+			return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
+		}
+		if (!VirtControlChanFunc(&cmd)) {
+			LOGERR("CONTROLVM_DEVICE_CHANGESTATE:resume Failed: virtpci GUEST_RESUME_[VHBA||VNIC] returned error.");
+			return
+			  CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
+		}
+	}
+	return retval;
 }
 
 static int
@@ -683,6 +685,7 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 	struct bus_info *bus;
 	struct device_info *dev;
 	struct guest_msgs cmd;
+	int retval = CONTROLVM_RESP_SUCCESS;
 
 	busNo = msg->cmd.destroyDevice.busNo;
 	devNo = msg->cmd.destroyDevice.devNo;
@@ -694,63 +697,18 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 			/* make sure the device number is valid */
 			if (devNo >= bus->deviceCount) {
 				LOGERR("CONTROLVM_DEVICE_DESTORY Failed: device(%d) >= deviceCount(%d).",
-				     devNo, bus->deviceCount);
-				read_unlock(&BusListLock);
-				return CONTROLVM_RESP_ERROR_DEVICE_INVALID;
-			}
-			/* make sure this device exists */
-			dev = bus->device[devNo];
-			if (!dev) {
-				LOGERR("CONTROLVM_DEVICE_DESTROY Failed: device %d does not exist.",
-				     devNo);
-				read_unlock(&BusListLock);
-				return CONTROLVM_RESP_ERROR_ALREADY_DONE;
-			}
-			read_unlock(&BusListLock);
-			/* the msg is bound for virtpci; send
-			 * guest_msgs struct to callback
-			 */
-			if (!uuid_le_cmp(dev->channelTypeGuid,
-					UltraVhbaChannelProtocolGuid)) {
-				cmd.msgtype = GUEST_DEL_VHBA;
-				cmd.del_vhba.chanptr = dev->chanptr;
-			} else
-			    if (!uuid_le_cmp(dev->channelTypeGuid,
-					    UltraVnicChannelProtocolGuid)) {
-				cmd.msgtype = GUEST_DEL_VNIC;
-				cmd.del_vnic.chanptr = dev->chanptr;
+				       devNo, bus->deviceCount);
+				retval = CONTROLVM_RESP_ERROR_DEVICE_INVALID;
 			} else {
-				LOGERR("CONTROLVM_DEVICE_DESTROY Failed: unknown channelTypeGuid.\n");
-				return
-				    CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
-			}
-
-			if (!VirtControlChanFunc) {
-				LOGERR("CONTROLVM_DEVICE_DESTORY Failed: virtpci callback not registered.");
-				return
-				    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
-			}
-
-			if (!VirtControlChanFunc(&cmd)) {
-				LOGERR("CONTROLVM_DEVICE_DESTROY Failed: virtpci GUEST_DEL_[VHBA||VNIC] returned error.");
-				return CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
-			}
-/* you must disable channel interrupts BEFORE you unmap the channel,
- * because if you unmap first, there may still be some activity going
- * on which accesses the channel and you will get a "unable to handle
- * kernel paging request"
- */
-			if (dev->polling) {
-				LOGINF("calling uislib_disable_channel_interrupts");
-				uislib_disable_channel_interrupts(busNo, devNo);
-			}
-			/* unmap the channel memory for the device. */
-			if (!msg->hdr.Flags.testMessage) {
-				LOGINF("destroy_device, doing iounmap");
-				uislib_iounmap(dev->chanptr);
+				/* make sure this device exists */
+				dev = bus->device[devNo];
+				if (!dev) {
+					LOGERR("CONTROLVM_DEVICE_DESTROY Failed: device %d does not exist.",
+					       devNo);
+					retval =
+					     CONTROLVM_RESP_ERROR_ALREADY_DONE;
+				}
 			}
-			kfree(dev);
-			bus->device[devNo] = NULL;
 			break;
 		}
 	}
@@ -758,11 +716,54 @@ destroy_device(CONTROLVM_MESSAGE *msg, char *buf)
 	if (!bus) {
 		LOGERR("CONTROLVM_DEVICE_DESTROY Failed: bus %d does not exist",
 		       busNo);
-		read_unlock(&BusListLock);
-		return CONTROLVM_RESP_ERROR_BUS_INVALID;
+		retval = CONTROLVM_RESP_ERROR_BUS_INVALID;
 	}
-
-	return CONTROLVM_RESP_SUCCESS;
+	read_unlock(&BusListLock);
+	if (retval == CONTROLVM_RESP_SUCCESS) {
+		/* the msg is bound for virtpci; send
+		 * guest_msgs struct to callback
+		 */
+		if (!uuid_le_cmp(dev->channel_uuid,
+				 UltraVhbaChannelProtocolGuid)) {
+			cmd.msgtype = GUEST_DEL_VHBA;
+			cmd.del_vhba.chanptr = dev->chanptr;
+		} else if (!uuid_le_cmp(dev->channel_uuid,
+					UltraVnicChannelProtocolGuid)) {
+			cmd.msgtype = GUEST_DEL_VNIC;
+			cmd.del_vnic.chanptr = dev->chanptr;
+		} else {
+			LOGERR("CONTROLVM_DEVICE_DESTROY Failed: unknown channelTypeGuid.\n");
+			return
+			    CONTROLVM_RESP_ERROR_CHANNEL_TYPE_UNKNOWN;
+		}
+		if (!VirtControlChanFunc) {
+			LOGERR("CONTROLVM_DEVICE_DESTORY Failed: virtpci callback not registered.");
+			return
+			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_FAILURE;
+		}
+		if (!VirtControlChanFunc(&cmd)) {
+			LOGERR("CONTROLVM_DEVICE_DESTROY Failed: virtpci GUEST_DEL_[VHBA||VNIC] returned error.");
+			return
+			    CONTROLVM_RESP_ERROR_VIRTPCI_DRIVER_CALLBACK_ERROR;
+		}
+/* you must disable channel interrupts BEFORE you unmap the channel,
+ * because if you unmap first, there may still be some activity going
+ * on which accesses the channel and you will get a "unable to handle
+ * kernel paging request"
+ */
+		if (dev->polling) {
+			LOGINF("calling uislib_disable_channel_interrupts");
+			uislib_disable_channel_interrupts(busNo, devNo);
+		}
+		/* unmap the channel memory for the device. */
+		if (!msg->hdr.Flags.testMessage) {
+			LOGINF("destroy_device, doing iounmap");
+			uislib_iounmap(dev->chanptr);
+		}
+		kfree(dev);
+		bus->device[devNo] = NULL;
+	}
+	return retval;
 }
 
 static int
@@ -1164,6 +1165,7 @@ uislib_cache_alloc(struct kmem_cache *cur_pool, char *fn, int ln)
 	* invoke oom killer), which will probably cripple the system.
 	*/
 	void *p = kmem_cache_alloc(cur_pool, GFP_ATOMIC | __GFP_NORETRY);
+
 	if (p == NULL) {
 		LOGERR("uislib_malloc failed to alloc uiscmdrsp @%s:%d",
 		       fn, ln);
@@ -1339,6 +1341,7 @@ Process_Incoming(void *v)
 	unsigned long long cur_cycles, old_cycles, idle_cycles, delta_cycles;
 	struct list_head *new_tail = NULL;
 	int i;
+
 	UIS_DAEMONIZE("dev_incoming");
 	for (i = 0; i < 16; i++) {
 		old_cycles = get_cycles();
@@ -1361,18 +1364,19 @@ Process_Incoming(void *v)
 		struct device_info *dev = NULL;
 
 		/* poll each channel for input */
-		LOCKSEM_UNINTERRUPTIBLE(&Lock_Polling_Device_Channels);
+		down(&Lock_Polling_Device_Channels);
 		new_tail = NULL;
 		list_for_each_safe(lelt, tmp, &List_Polling_Device_Channels) {
 			int rc = 0;
+
 			dev = list_entry(lelt, struct device_info,
 					 list_polling_device_channels);
-			LOCKSEM_UNINTERRUPTIBLE(&dev->interrupt_callback_lock);
+			down(&dev->interrupt_callback_lock);
 			if (dev->interrupt)
 				rc = dev->interrupt(dev->interrupt_context);
 			else
 				continue;
-			UNLOCKSEM(&dev->interrupt_callback_lock);
+			up(&dev->interrupt_callback_lock);
 			if (rc) {
 				/* dev->interrupt returned, but there
 				* is still more work to do.
@@ -1399,7 +1403,7 @@ Process_Incoming(void *v)
 			tot_moved_to_tail_cnt++;
 			list_move_tail(new_tail, &List_Polling_Device_Channels);
 		}
-		UNLOCKSEM(&Lock_Polling_Device_Channels);
+		up(&Lock_Polling_Device_Channels);
 		cur_cycles = get_cycles();
 		delta_cycles = cur_cycles - old_cycles;
 		old_cycles = cur_cycles;
@@ -1458,25 +1462,26 @@ Initialize_incoming_thread(void)
  *                        function.
  */
 void
-uislib_enable_channel_interrupts(u32 busNo, u32 devNo,
+uislib_enable_channel_interrupts(u32 bus_no, u32 dev_no,
 				 int (*interrupt)(void *),
 				 void *interrupt_context)
 {
 	struct device_info *dev;
-	dev = find_dev(busNo, devNo);
+
+	dev = find_dev(bus_no, dev_no);
 	if (!dev) {
-		LOGERR("%s busNo=%d, devNo=%d", __func__, (int) (busNo),
-		       (int) (devNo));
+		LOGERR("%s busNo=%d, devNo=%d", __func__, (int) (bus_no),
+		       (int) (dev_no));
 		return;
 	}
-	LOCKSEM_UNINTERRUPTIBLE(&Lock_Polling_Device_Channels);
+	down(&Lock_Polling_Device_Channels);
 	Initialize_incoming_thread();
 	dev->interrupt = interrupt;
 	dev->interrupt_context = interrupt_context;
 	dev->polling = TRUE;
 	list_add_tail(&(dev->list_polling_device_channels),
 		      &List_Polling_Device_Channels);
-	UNLOCKSEM(&Lock_Polling_Device_Channels);
+	up(&Lock_Polling_Device_Channels);
 }
 EXPORT_SYMBOL_GPL(uislib_enable_channel_interrupts);
 
@@ -1484,20 +1489,21 @@ EXPORT_SYMBOL_GPL(uislib_enable_channel_interrupts);
  *  Process_Incoming().
  */
 void
-uislib_disable_channel_interrupts(u32 busNo, u32 devNo)
+uislib_disable_channel_interrupts(u32 bus_no, u32 dev_no)
 {
 	struct device_info *dev;
-	dev = find_dev(busNo, devNo);
+
+	dev = find_dev(bus_no, dev_no);
 	if (!dev) {
-		LOGERR("%s busNo=%d, devNo=%d", __func__, (int) (busNo),
-		       (int) (devNo));
+		LOGERR("%s busNo=%d, devNo=%d", __func__, (int) (bus_no),
+		       (int) (dev_no));
 		return;
 	}
-	LOCKSEM_UNINTERRUPTIBLE(&Lock_Polling_Device_Channels);
+	down(&Lock_Polling_Device_Channels);
 	list_del(&dev->list_polling_device_channels);
 	dev->polling = FALSE;
 	dev->interrupt = NULL;
-	UNLOCKSEM(&Lock_Polling_Device_Channels);
+	up(&Lock_Polling_Device_Channels);
 }
 EXPORT_SYMBOL_GPL(uislib_disable_channel_interrupts);
 
@@ -1517,7 +1523,7 @@ static DECLARE_WORK(Work_wakeup_polling_device_channels,
  *  your device might have more requests.
  */
 void
-uislib_force_channel_interrupt(u32 busNo, u32 devNo)
+uislib_force_channel_interrupt(u32 bus_no, u32 dev_no)
 {
 	if (en_smart_wakeup == 0)
 		return;
@@ -1612,7 +1618,6 @@ uislib_mod_exit(void)
 	debugfs_remove(dir_debugfs);
 
 	DBGINF("goodbye.\n");
-	return;
 }
 
 module_init(uislib_mod_init);
diff --git a/drivers/staging/unisys/uislib/uisqueue.c b/drivers/staging/unisys/uislib/uisqueue.c
index 84eafca5e45c..44208841bd5a 100644
--- a/drivers/staging/unisys/uislib/uisqueue.c
+++ b/drivers/staging/unisys/uislib/uisqueue.c
@@ -34,42 +34,40 @@
 /* Exported functions                                */
 /*****************************************************/
 unsigned long long
-uisqueue_InterlockedOr(unsigned long long __iomem *Target,
-		       unsigned long long Set)
+uisqueue_interlocked_or(unsigned long long __iomem *tgt,
+		       unsigned long long set)
 {
 	unsigned long long i;
 	unsigned long long j;
 
-	j = readq(Target);
+	j = readq(tgt);
 	do {
 		i = j;
-		j = uislibcmpxchg64((__force unsigned long long *)Target,
-				    i, i | Set, sizeof(*(Target)));
+		j = cmpxchg((__force unsigned long long *)tgt, i, i | set);
 
 	} while (i != j);
 
 	return j;
 }
-EXPORT_SYMBOL_GPL(uisqueue_InterlockedOr);
+EXPORT_SYMBOL_GPL(uisqueue_interlocked_or);
 
 unsigned long long
-uisqueue_InterlockedAnd(unsigned long long __iomem *Target,
-			unsigned long long Set)
+uisqueue_interlocked_and(unsigned long long __iomem *tgt,
+			unsigned long long set)
 {
 	unsigned long long i;
 	unsigned long long j;
 
-	j = readq(Target);
+	j = readq(tgt);
 	do {
 		i = j;
-		j = uislibcmpxchg64((__force unsigned long long *)Target,
-				    i, i & Set, sizeof(*(Target)));
+		j = cmpxchg((__force unsigned long long *)tgt, i, i & set);
 
 	} while (i != j);
 
 	return j;
 }
-EXPORT_SYMBOL_GPL(uisqueue_InterlockedAnd);
+EXPORT_SYMBOL_GPL(uisqueue_interlocked_and);
 
 static u8
 do_locked_client_insert(struct uisqueue_info *queueinfo,
@@ -80,41 +78,18 @@ do_locked_client_insert(struct uisqueue_info *queueinfo,
 			u64 interruptHandle, u8 *channelId)
 {
 	unsigned long flags;
-	unsigned char queueWasEmpty;
-	unsigned int locked = 0;
-	unsigned int acquired = 0;
 	u8 rc = 0;
 
 	spin_lock_irqsave(lock, flags);
-	locked = 1;
-
 	if (!ULTRA_CHANNEL_CLIENT_ACQUIRE_OS(queueinfo->chan, channelId, NULL))
-		goto Away;
-
-	acquired = 1;
-
-	queueWasEmpty = visor_signalqueue_empty(queueinfo->chan, whichqueue);
-	if (!visor_signal_insert(queueinfo->chan, whichqueue, pSignal))
-		goto Away;
-	ULTRA_CHANNEL_CLIENT_RELEASE_OS(queueinfo->chan, channelId, NULL);
-	acquired = 0;
-	spin_unlock_irqrestore(lock, flags);
-	locked = 0;
-
-	queueinfo->packets_sent++;
-
-	rc = 1;
-Away:
-	if (acquired) {
-		ULTRA_CHANNEL_CLIENT_RELEASE_OS(queueinfo->chan, channelId,
-						NULL);
-		acquired = 0;
+		goto unlock;
+	if (visor_signal_insert(queueinfo->chan, whichqueue, pSignal)) {
+		queueinfo->packets_sent++;
+		rc = 1;
 	}
-	if (locked) {
-		spin_unlock_irqrestore((spinlock_t *) lock, flags);
-		locked = 0;
-	}
-
+	ULTRA_CHANNEL_CLIENT_RELEASE_OS(queueinfo->chan, channelId, NULL);
+unlock:
+	spin_unlock_irqrestore((spinlock_t *)lock, flags);
 	return rc;
 }
 
@@ -123,14 +98,14 @@ uisqueue_put_cmdrsp_with_lock_client(struct uisqueue_info *queueinfo,
 				     struct uiscmdrsp *cmdrsp,
 				     unsigned int whichqueue,
 				     void *insertlock,
-				     unsigned char issueInterruptIfEmpty,
-				     u64 interruptHandle,
-				     char oktowait, u8 *channelId)
+				     unsigned char issue_irq_if_empty,
+				     u64 irq_handle,
+				     char oktowait, u8 *channel_id)
 {
 	while (!do_locked_client_insert(queueinfo, whichqueue, cmdrsp,
 					(spinlock_t *) insertlock,
-					issueInterruptIfEmpty,
-					interruptHandle, channelId)) {
+					issue_irq_if_empty,
+					irq_handle, channel_id)) {
 		if (oktowait != OK_TO_WAIT) {
 			LOGERR("****FAILED visor_signal_insert failed; cannot wait; insert aborted\n");
 			return 0;	/* failed to queue */
diff --git a/drivers/staging/unisys/uislib/uisthread.c b/drivers/staging/unisys/uislib/uisthread.c
index c93ab04c3cfc..c0fc812f751e 100644
--- a/drivers/staging/unisys/uislib/uisthread.c
+++ b/drivers/staging/unisys/uislib/uisthread.c
@@ -62,6 +62,7 @@ uisthread_stop(struct uisthread_info *thrinfo)
 {
 	int ret;
 	int stopped = 0;
+
 	if (thrinfo->id == 0)
 		return;		/* thread not running */
 
diff --git a/drivers/staging/unisys/uislib/uisutils.c b/drivers/staging/unisys/uislib/uisutils.c
index ee26e009b400..8ff6d26ff00b 100644
--- a/drivers/staging/unisys/uislib/uisutils.c
+++ b/drivers/staging/unisys/uislib/uisutils.c
@@ -17,7 +17,8 @@
 
 #include <linux/string.h>
 #include <linux/slab.h>
-#include <commontypes.h>
+#include <linux/types.h>
+#include <linux/uuid.h>
 #include <linux/spinlock.h>
 #include <linux/list.h>
 #include "uniklog.h"
@@ -74,7 +75,7 @@ EXPORT_SYMBOL_GPL(uisutil_add_proc_line_ex);
 
 int
 uisctrl_register_req_handler(int type, void *fptr,
-			     ULTRA_VBUS_DEVICEINFO *chipset_DriverInfo)
+			     ULTRA_VBUS_DEVICEINFO *chipset_driver_info)
 {
 	LOGINF("type = %d, fptr = 0x%p.\n", type, fptr);
 
@@ -95,8 +96,8 @@ uisctrl_register_req_handler(int type, void *fptr,
 		LOGERR("invalid type %d.\n", type);
 		return 0;
 	}
-	if (chipset_DriverInfo)
-		BusDeviceInfo_Init(chipset_DriverInfo, "chipset", "uislib",
+	if (chipset_driver_info)
+		bus_device_info_init(chipset_driver_info, "chipset", "uislib",
 				   VERSION, NULL);
 
 	return 1;
@@ -117,6 +118,7 @@ uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
 {
 	ReqHandlerInfo_t *pReqHandlerInfo;
 	int rc = 0;		/* assume failure */
+
 	LOGINF("type=%pUL, controlfunc=0x%p.\n",
 	       &switchTypeGuid, controlfunc);
 	if (!controlfunc) {
@@ -148,7 +150,7 @@ uisctrl_register_req_handler_ex(uuid_le switchTypeGuid,
 Away:
 	if (rc) {
 		if (chipset_DriverInfo)
-			BusDeviceInfo_Init(chipset_DriverInfo, "chipset",
+			bus_device_info_init(chipset_DriverInfo, "chipset",
 					   "uislib", VERSION, NULL);
 	} else
 		LOGERR("failed to register type %pUL.\n", &switchTypeGuid);
@@ -161,6 +163,7 @@ int
 uisctrl_unregister_req_handler_ex(uuid_le switchTypeGuid)
 {
 	int rc = 0;		/* assume failure */
+
 	LOGINF("type=%pUL.\n", &switchTypeGuid);
 	if (ReqHandlerDel(switchTypeGuid) < 0) {
 		LOGERR("failed to remove %pUL from server list\n",
@@ -226,38 +229,41 @@ uisutil_copy_fragsinfo_from_skb(unsigned char *calling_ctx, void *skb_in,
 		offset += size;
 		count++;
 	}
-	if (numfrags) {
-		if ((count + numfrags) > frags_max) {
-			LOGERR("**** FAILED %s frags array too small: max:%d count+nr_frags:%d\n",
-			     calling_ctx, frags_max, count + numfrags);
-			return -1;	/* failure */
-		}
+	if (!numfrags)
+		goto dolist;
 
-		for (ii = 0; ii < numfrags; ii++) {
-			count = add_physinfo_entries(page_to_pfn(skb_frag_page(&skb_shinfo(skb)->frags[ii])),	/* pfn */
-						     skb_shinfo(skb)->frags[ii].
-						     page_offset,
-						     skb_shinfo(skb)->frags[ii].
-						     size, count, frags_max,
-						     frags);
-			if (count == 0) {
-				LOGERR("**** FAILED to add physinfo entries\n");
-				return -1;	/* failure */
-			}
+	if ((count + numfrags) > frags_max) {
+		LOGERR("**** FAILED %s frags array too small: max:%d count+nr_frags:%d\n",
+		     calling_ctx, frags_max, count + numfrags);
+		return -1;	/* failure */
+	}
+
+	for (ii = 0; ii < numfrags; ii++) {
+		count = add_physinfo_entries(page_to_pfn(
+				skb_frag_page(&skb_shinfo(skb)->frags[ii])),
+					skb_shinfo(skb)->frags[ii].
+					page_offset,
+					skb_shinfo(skb)->frags[ii].
+					size, count, frags_max,
+					frags);
+		if (count == 0) {
+			LOGERR("**** FAILED to add physinfo entries\n");
+			return -1;	/* failure */
 		}
 	}
-	if (skb_shinfo(skb)->frag_list) {
+
+dolist: if (skb_shinfo(skb)->frag_list) {
 		struct sk_buff *skbinlist;
 		int c;
+
 		for (skbinlist = skb_shinfo(skb)->frag_list; skbinlist;
 		     skbinlist = skbinlist->next) {
 
 			c = uisutil_copy_fragsinfo_from_skb("recursive",
-							    skbinlist,
-							    skbinlist->len -
-							    skbinlist->data_len,
-							    frags_max - count,
-							    &frags[count]);
+				skbinlist,
+				skbinlist->len - skbinlist->data_len,
+				frags_max - count,
+				&frags[count]);
 			if (c == -1) {
 				LOGERR("**** FAILED recursive call failed\n");
 				return -1;
@@ -306,6 +312,7 @@ ReqHandlerFind(uuid_le switchTypeGuid)
 {
 	struct list_head *lelt, *tmp;
 	ReqHandlerInfo_t *entry = NULL;
+
 	spin_lock(&ReqHandlerInfo_list_lock);
 	list_for_each_safe(lelt, tmp, &ReqHandlerInfo_list) {
 		entry = list_entry(lelt, ReqHandlerInfo_t, list_link);
@@ -324,6 +331,7 @@ ReqHandlerDel(uuid_le switchTypeGuid)
 	struct list_head *lelt, *tmp;
 	ReqHandlerInfo_t *entry = NULL;
 	int rc = -1;
+
 	spin_lock(&ReqHandlerInfo_list_lock);
 	list_for_each_safe(lelt, tmp, &ReqHandlerInfo_list) {
 		entry = list_entry(lelt, ReqHandlerInfo_t, list_link);
diff --git a/drivers/staging/unisys/virthba/virthba.c b/drivers/staging/unisys/virthba/virthba.c
index 049eeab08bac..938e2c82c1ab 100644
--- a/drivers/staging/unisys/virthba/virthba.c
+++ b/drivers/staging/unisys/virthba/virthba.c
@@ -436,7 +436,7 @@ virthba_ISR(int irq, void *dev_id)
 		0)) {
 		virthbainfo->interrupts_disabled++;
 		mask = ~ULTRA_CHANNEL_ENABLE_INTS;
-		rc1 = uisqueue_InterlockedAnd(virthbainfo->flags_addr, mask);
+		rc1 = uisqueue_interlocked_and(virthbainfo->flags_addr, mask);
 	}
 	if (visor_signalqueue_empty(pChannelHeader, IOCHAN_FROM_IOPART)) {
 		virthbainfo->interrupts_notme++;
@@ -517,7 +517,7 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 	    (unsigned short) (virtpcidev->scsi.max.max_io_size / PAGE_SIZE);
 	if (scsihost->sg_tablesize > MAX_PHYS_INFO)
 		scsihost->sg_tablesize = MAX_PHYS_INFO;
-	LOGINF("scsihost->max_channel=%u, max_id=%u, max_lun=%u, cmd_per_lun=%u, max_sectors=%hu, sg_tablesize=%hu\n",
+	LOGINF("scsihost->max_channel=%u, max_id=%u, max_lun=%llu, cmd_per_lun=%u, max_sectors=%hu, sg_tablesize=%hu\n",
 	     scsihost->max_channel, scsihost->max_id, scsihost->max_lun,
 	     scsihost->cmd_per_lun, scsihost->max_sectors,
 	     scsihost->sg_tablesize);
@@ -627,9 +627,9 @@ virthba_probe(struct virtpci_dev *virtpcidev, const struct pci_device_id *id)
 		       virthbainfo->interrupt_vector);
 		mask = ~(ULTRA_IO_CHANNEL_IS_POLLING |
 			 ULTRA_IO_DRIVER_DISABLES_INTS);
-		uisqueue_InterlockedAnd(Features_addr, mask);
+		uisqueue_interlocked_and(Features_addr, mask);
 		mask = ULTRA_IO_DRIVER_ENABLES_INTS;
-		uisqueue_InterlockedOr(Features_addr, mask);
+		uisqueue_interlocked_or(Features_addr, mask);
 		rsltq_wait_usecs = 4000000;
 	}
 
@@ -746,7 +746,7 @@ forward_taskmgmt_command(TASK_MGMT_TYPES tasktype, struct scsi_device *scsidev)
 	int notifyresult = 0xffff;
 	wait_queue_head_t notifyevent;
 
-	LOGINF("TaskMgmt:%d %d:%d:%d\n", tasktype,
+	LOGINF("TaskMgmt:%d %d:%d:%llu\n", tasktype,
 	       scsidev->channel, scsidev->id, scsidev->lun);
 
 	if (virthbainfo->serverdown || virthbainfo->serverchangingstate) {
@@ -1105,7 +1105,6 @@ virthba_slave_destroy(struct scsi_device *scsidev)
 			return;
 		}
 	}
-	return;
 }
 
 /*****************************************************/
@@ -1139,7 +1138,7 @@ do_scsi_linuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 
 		if (atomic_read(&vdisk->error_count) < VIRTHBA_ERROR_COUNT) {
 			atomic_inc(&vdisk->error_count);
-			LOGERR("SCSICMD ****FAILED scsicmd:0x%p op:0x%x <%d:%d:%d:%d> 0x%x-0x%x-0x%x-0x%x-0x%x.\n",
+			LOGERR("SCSICMD ****FAILED scsicmd:0x%p op:0x%x <%d:%d:%d:%llu> 0x%x-0x%x-0x%x-0x%x-0x%x.\n",
 			       scsicmd, cmdrsp->scsi.cmnd[0],
 			       scsidev->host->host_no, scsidev->id,
 			       scsidev->channel, scsidev->lun,
@@ -1148,7 +1147,7 @@ do_scsi_linuxstat(struct uiscmdrsp *cmdrsp, struct scsi_cmnd *scsicmd)
 			       sd->AdditionalSenseCodeQualifier);
 			if (atomic_read(&vdisk->error_count) ==
 			    VIRTHBA_ERROR_COUNT) {
-				LOGERR("Throtling SCSICMD errors disk <%d:%d:%d:%d>\n",
+				LOGERR("Throtling SCSICMD errors disk <%d:%d:%d:%llu>\n",
 				     scsidev->host->host_no, scsidev->id,
 				     scsidev->channel, scsidev->lun);
 			}
@@ -1354,7 +1353,7 @@ process_incoming_rsps(void *v)
 		atomic_set(&virthbainfo->interrupt_rcvd, 0);
 		/* drain queue */
 		drain_queue(virthbainfo, dc, cmdrsp);
-		rc1 = uisqueue_InterlockedOr(virthbainfo->flags_addr, mask);
+		rc1 = uisqueue_interlocked_or(virthbainfo->flags_addr, mask);
 		if (dc->threadinfo.should_stop)
 			break;
 	}
@@ -1458,16 +1457,16 @@ static ssize_t enable_ints_write(struct file *file,
 			if (new_value == 1) {
 				mask = ~(ULTRA_IO_CHANNEL_IS_POLLING |
 					 ULTRA_IO_DRIVER_DISABLES_INTS);
-				uisqueue_InterlockedAnd(Features_addr, mask);
+				uisqueue_interlocked_and(Features_addr, mask);
 				mask = ULTRA_IO_DRIVER_ENABLES_INTS;
-				uisqueue_InterlockedOr(Features_addr, mask);
+				uisqueue_interlocked_or(Features_addr, mask);
 				rsltq_wait_usecs = 4000000;
 			} else {
 				mask = ~(ULTRA_IO_DRIVER_ENABLES_INTS |
 					 ULTRA_IO_DRIVER_DISABLES_INTS);
-				uisqueue_InterlockedAnd(Features_addr, mask);
+				uisqueue_interlocked_and(Features_addr, mask);
 				mask = ULTRA_IO_CHANNEL_IS_POLLING;
-				uisqueue_InterlockedOr(Features_addr, mask);
+				uisqueue_interlocked_or(Features_addr, mask);
 				rsltq_wait_usecs = 4000;
 			}
 		}
diff --git a/drivers/staging/unisys/virtpci/virtpci.c b/drivers/staging/unisys/virtpci/virtpci.c
index d9443a968ddf..ee9f8260cd15 100644
--- a/drivers/staging/unisys/virtpci/virtpci.c
+++ b/drivers/staging/unisys/virtpci/virtpci.c
@@ -24,9 +24,11 @@
 #include "uniklog.h"
 #include "diagnostics/appos_subsystems.h"
 #include "uisutils.h"
-#include "commontypes.h"
 #include "vbuschannel.h"
 #include "vbushelper.h"
+#include <linux/types.h>
+#include <linux/io.h>
+#include <linux/uuid.h>
 #include <linux/module.h>
 #include <linux/init.h>
 #include <linux/pci.h>
@@ -39,7 +41,7 @@
 #include <linux/debugfs.h>
 #include "version.h"
 #include "guestlinuxdebug.h"
-#include "timskmodutils.h"
+#include "timskmod.h"
 
 struct driver_private {
 	struct kobject kobj;
@@ -172,6 +174,7 @@ static inline
 int WAIT_FOR_IO_CHANNEL(ULTRA_IO_CHANNEL_PROTOCOL __iomem  *chanptr)
 {
 	int count = 120;
+
 	while (count > 0) {
 
 		if (ULTRA_CHANNEL_SERVER_READY(&chanptr->ChannelHeader))
@@ -187,6 +190,7 @@ static int write_vbus_chpInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
 			      ULTRA_VBUS_DEVICEINFO *info)
 {
 	int off;
+
 	if (!chan) {
 		LOGERR("vbus channel not present");
 		return -1;
@@ -205,6 +209,7 @@ static int write_vbus_busInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
 			      ULTRA_VBUS_DEVICEINFO *info)
 {
 	int off;
+
 	if (!chan) {
 		LOGERR("vbus channel not present");
 		return -1;
@@ -226,6 +231,7 @@ write_vbus_devInfo(ULTRA_VBUS_CHANNEL_PROTOCOL *chan,
 		   ULTRA_VBUS_DEVICEINFO *info, int devix)
 {
 	int off;
+
 	if (!chan) {
 		LOGERR("vbus channel not present");
 		return -1;
@@ -249,6 +255,7 @@ static int add_vbus(struct add_vbus_guestpart *addparams)
 {
 	int ret;
 	struct device *vbus;
+
 	vbus = kzalloc(sizeof(struct device), GFP_ATOMIC);
 
 	POSTCODE_LINUX_2(VPCI_CREATE_ENTRY_PC, POSTCODE_SEVERITY_INFO);
@@ -326,7 +333,7 @@ static int add_vhba(struct add_virt_guestpart *addparams)
 
 	GET_SCSIADAPINFO_FROM_CHANPTR(addparams->chanptr);
 
-	GET_BUS_DEV(addparams->busNo);
+	GET_BUS_DEV(addparams->bus_no);
 
 	LOGINF("Adding vhba wwnn:%x:%x config:%d-%d-%d-%d chanptr:%p\n",
 	       scsi.wwnn.wwnn1, scsi.wwnn.wwnn2,
@@ -383,7 +390,7 @@ add_vnic(struct add_virt_guestpart *addparams)
 
 	GET_NETADAPINFO_FROM_CHANPTR(addparams->chanptr);
 
-	GET_BUS_DEV(addparams->busNo);
+	GET_BUS_DEV(addparams->bus_no);
 
 	LOGINF("Adding vnic macaddr:%02x:%02x:%02x:%02x:%02x:%02x rcvbufs:%d mtu:%d chanptr:%p%pUL\n",
 	     net.mac_addr[0], net.mac_addr[1], net.mac_addr[2], net.mac_addr[3],
@@ -410,12 +417,12 @@ delete_vbus(struct del_vbus_guestpart *delparams)
 	struct device *vbus;
 	unsigned char busid[BUS_ID_SIZE];
 
-	GET_BUS_DEV(delparams->busNo);
+	GET_BUS_DEV(delparams->bus_no);
 	/* ensure that bus has no devices? -- TBD */
 	LOGINF("Deleting %s\n", BUS_ID(vbus));
 	if (delete_vbus_device(vbus, NULL))
 		return 0;	/* failure */
-	LOGINF("Deleted vbus %d\n", delparams->busNo);
+	LOGINF("Deleted vbus %d\n", delparams->bus_no);
 	return 1;
 }
 
@@ -613,7 +620,7 @@ static int delete_all_virt(VIRTPCI_DEV_TYPE devtype, struct del_vbus_guestpart *
 	unsigned char busid[BUS_ID_SIZE];
 	struct device *vbus;
 
-	GET_BUS_DEV(delparams->busNo);
+	GET_BUS_DEV(delparams->bus_no);
 
 	if ((devtype != VIRTHBA_TYPE) && (devtype != VIRTNIC_TYPE)) {
 		LOGERR("**** FAILED to delete all devices; devtype:%d not vhba:%d or vnic:%d\n",
@@ -785,7 +792,7 @@ static void fix_vbus_devInfo(struct device *dev, int devNo, int devType,
 		stype = "unknown";
 		break;
 	}
-	BusDeviceInfo_Init(&devInfo, stype,
+	bus_device_info_init(&devInfo, stype,
 			   virtpcidrv->name,
 			   virtpcidrv->version,
 			   virtpcidrv->vertag);
@@ -854,6 +861,7 @@ static int virtpci_device_remove(struct device *dev_)
 	*/
 	struct virtpci_dev *virtpcidev = device_to_virtpci_dev(dev_);
 	struct virtpci_driver *virtpcidrv = virtpcidev->mydriver;
+
 	LOGINF("In virtpci_device_remove bus_id:%s dev_:%p virtpcidev:%p dev->driver:%p drivername:%s\n",
 	       BUS_ID(dev_), dev_, virtpcidev, dev_->driver,
 	       dev_->driver->name);	/* VERBOSE/DEBUG */
@@ -931,8 +939,8 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 		virtpcidev->net = *net;
 	}
 	virtpcidev->vendor = PCI_VENDOR_ID_UNISYS;
-	virtpcidev->busNo = addparams->busNo;
-	virtpcidev->deviceNo = addparams->deviceNo;
+	virtpcidev->busNo = addparams->bus_no;
+	virtpcidev->deviceNo = addparams->device_no;
 
 	virtpcidev->queueinfo.chan = addparams->chanptr;
 	virtpcidev->queueinfo.send_int_if_needed = NULL;
@@ -949,7 +957,7 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 	virtpcidev->generic_dev.release = virtpci_device_release;
 
 	dev_set_name(&virtpcidev->generic_dev, "%x:%x",
-		     addparams->busNo, addparams->deviceNo);
+		     addparams->bus_no, addparams->device_no);
 
 	/* add the vhba/vnic to virtpci device list - but check for
 	 * duplicate wwnn/macaddr first
@@ -1047,7 +1055,8 @@ static int virtpci_device_add(struct device *parentbus, int devtype,
 
 	LOGINF("Added %s:%d:%d &virtpcidev->generic_dev:%p\n",
 	       (devtype == VIRTHBA_TYPE) ? "virthba" : "virtnic",
-	       addparams->busNo, addparams->deviceNo, &virtpcidev->generic_dev);
+	       addparams->bus_no, addparams->device_no,
+	       &virtpcidev->generic_dev);
 	POSTCODE_LINUX_2(VPCI_CREATE_EXIT_PC, POSTCODE_SEVERITY_INFO);
 	return 1;
 }
@@ -1306,6 +1315,7 @@ static ssize_t virtpci_driver_attr_show(struct kobject *kobj,
 
 	struct driver_private *dprivate = to_driver(kobj);
 	struct device_driver *driver;
+
 	if (dprivate != NULL)
 		driver = dprivate->driver;
 	else
@@ -1328,6 +1338,7 @@ static ssize_t virtpci_driver_attr_store(struct kobject *kobj,
 
 	struct driver_private *dprivate = to_driver(kobj);
 	struct device_driver *driver;
+
 	if (dprivate != NULL)
 		driver = dprivate->driver;
 	else
@@ -1504,7 +1515,7 @@ static int __init virtpci_mod_init(void)
 		return ret;
 	}
 	DBGINF("bus_register successful\n");
-	BusDeviceInfo_Init(&Bus_DriverInfo, "clientbus", "virtpci",
+	bus_device_info_init(&Bus_DriverInfo, "clientbus", "virtpci",
 			   VERSION, NULL);
 
 	/* create a root bus used to parent all the virtpci buses. */
diff --git a/drivers/staging/unisys/visorchannel/visorchannel.h b/drivers/staging/unisys/visorchannel/visorchannel.h
index aa17a842381b..9a4d7b6755d1 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel.h
+++ b/drivers/staging/unisys/visorchannel/visorchannel.h
@@ -20,7 +20,6 @@
 
 #include <linux/uuid.h>
 
-#include "commontypes.h"
 #include "memregion.h"
 #include "channel.h"
 #ifndef HOSTADDRESS
diff --git a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
index 62ec9280cb3a..01a44c553500 100644
--- a/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
+++ b/drivers/staging/unisys/visorchannel/visorchannel_funcs.c
@@ -257,6 +257,7 @@ visorchannel_clear(VISORCHANNEL *channel, ulong offset, u8 ch, ulong nbytes)
 	while (nbytes > 0) {
 		ulong thisbytes = bufsize;
 		int x = -1;
+
 		if (nbytes < thisbytes)
 			thisbytes = nbytes;
 		x = visor_memregion_write(channel->memregion, offset + written,
@@ -424,7 +425,7 @@ visorchannel_signalremove(VISORCHANNEL *channel, u32 queue, void *msg)
 	/* For each data field in SIGNAL_QUEUE_HEADER that was modified,
 	 * update host memory.
 	 */
-	MEMORYBARRIER;
+	mb(); /* required for channel synch */
 	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, Tail)) {
 		ERRDRV("visor_memregion_write of Tail failed: (status=%d)\n",
 		       rc);
@@ -477,7 +478,7 @@ visorchannel_signalinsert(VISORCHANNEL *channel, u32 queue, void *msg)
 	/* For each data field in SIGNAL_QUEUE_HEADER that was modified,
 	 * update host memory.
 	 */
-	MEMORYBARRIER;
+	mb(); /* required for channel synch */
 	if (!SIG_WRITE_FIELD(channel, queue, &sig_hdr, Head)) {
 		ERRDRV("visor_memregion_write of Head failed: (status=%d)\n",
 		       rc);
@@ -520,6 +521,7 @@ int
 visorchannel_signalqueue_max_slots(VISORCHANNEL *channel, u32 queue)
 {
 	SIGNAL_QUEUE_HEADER sig_hdr;
+
 	if (!sig_read_header(channel, queue, &sig_hdr))
 		return 0;
 	return (int) sig_hdr.MaxSignals;
@@ -612,6 +614,7 @@ visorchannel_debug(VISORCHANNEL *channel, int nQueues,
 	else
 		for (i = 0; i < nQueues; i++) {
 			SIGNAL_QUEUE_HEADER q;
+
 			errcode = visorchannel_read(channel,
 						    off + phdr->oChannelSpace +
 						    (i * sizeof(q)),
diff --git a/drivers/staging/unisys/visorchipset/Kconfig b/drivers/staging/unisys/visorchipset/Kconfig
index 7ca2fbca9d57..e86836f84243 100644
--- a/drivers/staging/unisys/visorchipset/Kconfig
+++ b/drivers/staging/unisys/visorchipset/Kconfig
@@ -4,7 +4,7 @@
 
 config UNISYS_VISORCHIPSET
 	tristate "Unisys visorchipset driver"
-	depends on UNISYSSPAR && UNISYS_VISORUTIL && UNISYS_VISORCHANNEL
+	depends on UNISYSSPAR && UNISYS_VISORUTIL && UNISYS_VISORCHANNEL && HAS_IOMEM
 	---help---
 	If you say Y here, you will enable the Unisys visorchipset driver.
 
diff --git a/drivers/staging/unisys/visorchipset/file.c b/drivers/staging/unisys/visorchipset/file.c
index bf2e546d76bf..3321764069de 100644
--- a/drivers/staging/unisys/visorchipset/file.c
+++ b/drivers/staging/unisys/visorchipset/file.c
@@ -192,11 +192,12 @@ visorchipset_ioctl(struct inode *inode, struct file *file,
 	int rc = SUCCESS;
 	s64 adjustment;
 	s64 vrtc_offset;
+
 	DBGINF("entered visorchipset_ioctl, cmd=%d", cmd);
 	switch (cmd) {
 	case VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET:
 		/* get the physical rtc offset */
-		vrtc_offset = Issue_VMCALL_QUERY_GUEST_VIRTUAL_TIME_OFFSET();
+		vrtc_offset = issue_vmcall_query_guest_virtual_time_offset();
 		if (copy_to_user
 		    ((void __user *)arg, &vrtc_offset, sizeof(vrtc_offset))) {
 			rc = -EFAULT;
@@ -213,7 +214,7 @@ visorchipset_ioctl(struct inode *inode, struct file *file,
 		}
 		DBGINF("insde visorchipset_ioctl, cmd=%d, adjustment=%lld", cmd,
 		       adjustment);
-		rc = Issue_VMCALL_UPDATE_PHYSICAL_TIME(adjustment);
+		rc = issue_vmcall_update_physical_time(adjustment);
 		break;
 	default:
 		LOGERR("visorchipset_ioctl received invalid command");
diff --git a/drivers/staging/unisys/visorchipset/parser.c b/drivers/staging/unisys/visorchipset/parser.c
index 86fa2949dc4e..661aaae9b154 100644
--- a/drivers/staging/unisys/visorchipset/parser.c
+++ b/drivers/staging/unisys/visorchipset/parser.c
@@ -84,6 +84,7 @@ parser_init_guts(u64 addr, u32 bytes, BOOL isLocal,
 	ctx->byte_stream = FALSE;
 	if (isLocal) {
 		void *p;
+
 		if (addr > virt_to_phys(high_memory - 1)) {
 			ERRDRV("%s - bad local address (0x%-16.16Lx for %lu)",
 			       __func__,
@@ -257,6 +258,7 @@ static int
 string_length_no_trail(char *s, int len)
 {
 	int i = len - 1;
+
 	while (i >= 0) {
 		if (!isspace(s[i]))
 			return i + 1;
diff --git a/drivers/staging/unisys/visorchipset/visorchipset_main.c b/drivers/staging/unisys/visorchipset/visorchipset_main.c
index fe3c0127d255..e5df39554a1a 100644
--- a/drivers/staging/unisys/visorchipset/visorchipset_main.c
+++ b/drivers/staging/unisys/visorchipset/visorchipset_main.c
@@ -58,8 +58,7 @@ NONULLSTR(char *s)
 {
 	if (s)
 		return s;
-	else
-		return "";
+	return "";
 }
 
 static int serverregistered;
@@ -379,8 +378,7 @@ static ssize_t toolaction_store(struct device *dev,
 
 	if (ret)
 		return ret;
-	else
-		return count;
+	return count;
 }
 
 static ssize_t boottotool_show(struct device *dev,
@@ -416,8 +414,7 @@ static ssize_t boottotool_store(struct device *dev,
 
 	if (ret)
 		return ret;
-	else
-		return count;
+	return count;
 }
 
 static ssize_t error_show(struct device *dev, struct device_attribute *attr,
@@ -446,8 +443,7 @@ static ssize_t error_store(struct device *dev, struct device_attribute *attr,
 			&error, sizeof(u32));
 	if (ret)
 		return ret;
-	else
-		return count;
+	return count;
 }
 
 static ssize_t textid_show(struct device *dev, struct device_attribute *attr,
@@ -476,8 +472,7 @@ static ssize_t textid_store(struct device *dev, struct device_attribute *attr,
 			&textId, sizeof(u32));
 	if (ret)
 		return ret;
-	else
-		return count;
+	return count;
 }
 
 
@@ -509,8 +504,7 @@ static ssize_t remaining_steps_store(struct device *dev,
 			&remainingSteps, sizeof(u16));
 	if (ret)
 		return ret;
-	else
-		return count;
+	return count;
 }
 
 #if 0
@@ -565,6 +559,7 @@ static void
 devInfo_clear(void *v)
 {
 	VISORCHIPSET_DEVICE_INFO *p = (VISORCHIPSET_DEVICE_INFO *) (v);
+
 	p->state.created = 0;
 	memset(p, 0, sizeof(VISORCHIPSET_DEVICE_INFO));
 }
@@ -594,7 +589,7 @@ visorchipset_register_busdev_server(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
 				    VISORCHIPSET_BUSDEV_RESPONDERS *responders,
 				    ULTRA_VBUS_DEVICEINFO *driverInfo)
 {
-	LOCKSEM_UNINTERRUPTIBLE(&NotifierLock);
+	down(&NotifierLock);
 	if (notifiers == NULL) {
 		memset(&BusDev_Server_Notifiers, 0,
 		       sizeof(BusDev_Server_Notifiers));
@@ -606,10 +601,10 @@ visorchipset_register_busdev_server(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
 	if (responders)
 		*responders = BusDev_Responders;
 	if (driverInfo)
-		BusDeviceInfo_Init(driverInfo, "chipset", "visorchipset",
+		bus_device_info_init(driverInfo, "chipset", "visorchipset",
 				   VERSION, NULL);
 
-	UNLOCKSEM(&NotifierLock);
+	up(&NotifierLock);
 }
 EXPORT_SYMBOL_GPL(visorchipset_register_busdev_server);
 
@@ -618,7 +613,7 @@ visorchipset_register_busdev_client(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
 				    VISORCHIPSET_BUSDEV_RESPONDERS *responders,
 				    ULTRA_VBUS_DEVICEINFO *driverInfo)
 {
-	LOCKSEM_UNINTERRUPTIBLE(&NotifierLock);
+	down(&NotifierLock);
 	if (notifiers == NULL) {
 		memset(&BusDev_Client_Notifiers, 0,
 		       sizeof(BusDev_Client_Notifiers));
@@ -630,9 +625,9 @@ visorchipset_register_busdev_client(VISORCHIPSET_BUSDEV_NOTIFIERS *notifiers,
 	if (responders)
 		*responders = BusDev_Responders;
 	if (driverInfo)
-		BusDeviceInfo_Init(driverInfo, "chipset(bolts)", "visorchipset",
+		bus_device_info_init(driverInfo, "chipset(bolts)", "visorchipset",
 				   VERSION, NULL);
-	UNLOCKSEM(&NotifierLock);
+	up(&NotifierLock);
 }
 EXPORT_SYMBOL_GPL(visorchipset_register_busdev_client);
 
@@ -707,6 +702,7 @@ static void
 controlvm_respond(CONTROLVM_MESSAGE_HEADER *msgHdr, int response)
 {
 	CONTROLVM_MESSAGE outmsg;
+
 	controlvm_init_response(&outmsg, msgHdr, response);
 	/* For DiagPool channel DEVICE_CHANGESTATE, we need to send
 	* back the deviceChangeState structure in the packet. */
@@ -733,6 +729,7 @@ controlvm_respond_chipset_init(CONTROLVM_MESSAGE_HEADER *msgHdr, int response,
 			       ULTRA_CHIPSET_FEATURE features)
 {
 	CONTROLVM_MESSAGE outmsg;
+
 	controlvm_init_response(&outmsg, msgHdr, response);
 	outmsg.cmd.initChipset.features = features;
 	if (!visorchannel_signalinsert(ControlVm_channel,
@@ -747,6 +744,7 @@ controlvm_respond_physdev_changestate(CONTROLVM_MESSAGE_HEADER *msgHdr,
 				      int response, ULTRA_SEGMENT_STATE state)
 {
 	CONTROLVM_MESSAGE outmsg;
+
 	controlvm_init_response(&outmsg, msgHdr, response);
 	outmsg.cmd.deviceChangeState.state = state;
 	outmsg.cmd.deviceChangeState.flags.physicalDevice = 1;
@@ -944,7 +942,7 @@ bus_epilog(u32 busNo,
 	} else
 		pBusInfo->pendingMsgHdr.Id = CONTROLVM_INVALID;
 
-	LOCKSEM_UNINTERRUPTIBLE(&NotifierLock);
+	down(&NotifierLock);
 	if (response == CONTROLVM_RESP_SUCCESS) {
 		switch (cmd) {
 		case CONTROLVM_BUS_CREATE:
@@ -989,7 +987,7 @@ bus_epilog(u32 busNo,
 		;
 	else
 		bus_responder(cmd, busNo, response);
-	UNLOCKSEM(&NotifierLock);
+	up(&NotifierLock);
 }
 
 static void
@@ -1021,7 +1019,7 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 	} else
 		pDevInfo->pendingMsgHdr.Id = CONTROLVM_INVALID;
 
-	LOCKSEM_UNINTERRUPTIBLE(&NotifierLock);
+	down(&NotifierLock);
 	if (response >= 0) {
 		switch (cmd) {
 		case CONTROLVM_DEVICE_CREATE:
@@ -1087,7 +1085,7 @@ device_epilog(u32 busNo, u32 devNo, ULTRA_SEGMENT_STATE state, u32 cmd,
 		;
 	else
 		device_responder(cmd, busNo, devNo, response);
-	UNLOCKSEM(&NotifierLock);
+	up(&NotifierLock);
 }
 
 static void
@@ -1429,6 +1427,7 @@ initialize_controlvm_payload(void)
 	HOSTADDRESS phys_addr = visorchannel_get_physaddr(ControlVm_channel);
 	u64 payloadOffset = 0;
 	u32 payloadBytes = 0;
+
 	if (visorchannel_read(ControlVm_channel,
 			      offsetof(ULTRA_CONTROLVM_CHANNEL_PROTOCOL,
 				       RequestPayloadOffset),
@@ -1468,6 +1467,7 @@ visorchipset_chipset_selftest(void)
 {
 	char env_selftest[20];
 	char *envp[] = { env_selftest, NULL };
+
 	sprintf(env_selftest, "SPARSP_SELFTEST=%d", 1);
 	kobject_uevent_env(&Visorchipset_platform_device.dev.kobj, KOBJ_CHANGE,
 			   envp);
@@ -1490,6 +1490,7 @@ static void
 chipset_ready(CONTROLVM_MESSAGE_HEADER *msgHdr)
 {
 	int rc = visorchipset_chipset_ready();
+
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
 	if (msgHdr->Flags.responseExpected && !visorchipset_holdchipsetready)
@@ -1507,6 +1508,7 @@ static void
 chipset_selftest(CONTROLVM_MESSAGE_HEADER *msgHdr)
 {
 	int rc = visorchipset_chipset_selftest();
+
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
 	if (msgHdr->Flags.responseExpected)
@@ -1517,6 +1519,7 @@ static void
 chipset_notready(CONTROLVM_MESSAGE_HEADER *msgHdr)
 {
 	int rc = visorchipset_chipset_notready();
+
 	if (rc != CONTROLVM_RESP_SUCCESS)
 		rc = -rc;
 	if (msgHdr->Flags.responseExpected)
@@ -1535,8 +1538,8 @@ read_controlvm_event(CONTROLVM_MESSAGE *msg)
 		if (msg->hdr.Flags.testMessage == 1) {
 			LOGERR("ignoring bad CONTROLVM_QUEUE_EVENT msg with controlvm_msg_id=0x%x because Flags.testMessage is nonsensical (=1)", msg->hdr.Id);
 			return FALSE;
-		} else
-			return TRUE;
+		}
+		return TRUE;
 	}
 	return FALSE;
 }
@@ -1564,6 +1567,7 @@ static int
 parahotplug_next_id(void)
 {
 	static atomic_t id = ATOMIC_INIT(0);
+
 	return atomic_inc_return(&id);
 }
 
@@ -1788,6 +1792,7 @@ handle_command(CONTROLVM_MESSAGE inmsg, HOSTADDRESS channel_addr)
 	 */
 	if (parametersAddr != 0 && parametersBytes != 0) {
 		BOOL retry = FALSE;
+
 		parser_ctx =
 		    parser_init_byteStream(parametersAddr, parametersBytes,
 					   isLocalAddr, &retry);
@@ -1906,7 +1911,7 @@ static HOSTADDRESS controlvm_get_channel_address(void)
 	u64 addr = 0;
 	u32 size = 0;
 
-	if (!VMCALL_SUCCESSFUL(Issue_VMCALL_IO_CONTROLVM_ADDR(&addr, &size))) {
+	if (!VMCALL_SUCCESSFUL(issue_vmcall_io_controlvm_addr(&addr, &size))) {
 		ERRDRV("%s - vmcall to determine controlvm channel addr failed",
 		       __func__);
 		return 0;
@@ -2184,6 +2189,7 @@ BOOL
 visorchipset_get_bus_info(ulong busNo, VISORCHIPSET_BUS_INFO *busInfo)
 {
 	void *p = findbus(&BusInfoList, busNo);
+
 	if (!p) {
 		LOGERR("(%lu) failed", busNo);
 		return FALSE;
@@ -2197,6 +2203,7 @@ BOOL
 visorchipset_set_bus_context(ulong busNo, void *context)
 {
 	VISORCHIPSET_BUS_INFO *p = findbus(&BusInfoList, busNo);
+
 	if (!p) {
 		LOGERR("(%lu) failed", busNo);
 		return FALSE;
@@ -2211,6 +2218,7 @@ visorchipset_get_device_info(ulong busNo, ulong devNo,
 			     VISORCHIPSET_DEVICE_INFO *devInfo)
 {
 	void *p = finddevice(&DevInfoList, busNo, devNo);
+
 	if (!p) {
 		LOGERR("(%lu,%lu) failed", busNo, devNo);
 		return FALSE;
@@ -2224,6 +2232,7 @@ BOOL
 visorchipset_set_device_context(ulong busNo, ulong devNo, void *context)
 {
 	VISORCHIPSET_DEVICE_INFO *p = finddevice(&DevInfoList, busNo, devNo);
+
 	if (!p) {
 		LOGERR("(%lu,%lu) failed", busNo, devNo);
 		return FALSE;
@@ -2290,8 +2299,8 @@ static ssize_t chipsetready_store(struct device *dev,
 	} else if (strcmp(msgtype, "MODULES_LOADED") == 0) {
 		chipset_events[1] = 1;
 		return count;
-	} else
-		return -EINVAL;
+	}
+	return -EINVAL;
 }
 
 /* The parahotplug/devicedisabled interface gets called by our support script
@@ -2468,6 +2477,7 @@ static void
 visorchipset_exit(void)
 {
 	char s[99];
+
 	POSTCODE_LINUX_2(DRIVER_EXIT_PC, POSTCODE_SEVERITY_INFO);
 
 	if (visorchipset_disable_controlvm) {
diff --git a/drivers/staging/unisys/visorutil/Kconfig b/drivers/staging/unisys/visorutil/Kconfig
index 4ff61a7c506b..74b474eac252 100644
--- a/drivers/staging/unisys/visorutil/Kconfig
+++ b/drivers/staging/unisys/visorutil/Kconfig
@@ -4,7 +4,7 @@
 
 config UNISYS_VISORUTIL
 	tristate "Unisys visorutil driver"
-	depends on UNISYSSPAR
+	depends on UNISYSSPAR && HAS_IOMEM
 	---help---
 	If you say Y here, you will enable the Unisys visorutil driver.
 
diff --git a/drivers/staging/unisys/visorutil/periodic_work.c b/drivers/staging/unisys/visorutil/periodic_work.c
index 38a60ce8c27c..3dd1c04d0e14 100644
--- a/drivers/staging/unisys/visorutil/periodic_work.c
+++ b/drivers/staging/unisys/visorutil/periodic_work.c
@@ -27,7 +27,7 @@
 
 
 
-struct PERIODIC_WORK_Tag {
+struct periodic_work {
 	rwlock_t lock;
 	struct delayed_work work;
 	void (*workfunc)(void *);
@@ -43,42 +43,41 @@ struct PERIODIC_WORK_Tag {
 
 static void periodic_work_func(struct work_struct *work)
 {
-	PERIODIC_WORK *periodic_work =
-		container_of(work, struct PERIODIC_WORK_Tag, work.work);
-	(*periodic_work->workfunc)(periodic_work->workfuncarg);
+	struct periodic_work *pw;
+
+	pw = container_of(work, struct periodic_work, work.work);
+	(*pw->workfunc)(pw->workfuncarg);
 }
 
 
 
-PERIODIC_WORK *visor_periodic_work_create(ulong jiffy_interval,
-					  struct workqueue_struct *workqueue,
-					  void (*workfunc)(void *),
-					  void *workfuncarg,
-					  const char *devnam)
+struct periodic_work *visor_periodic_work_create(ulong jiffy_interval,
+					struct workqueue_struct *workqueue,
+					void (*workfunc)(void *),
+					void *workfuncarg,
+					const char *devnam)
 {
-	PERIODIC_WORK *periodic_work = kzalloc(sizeof(PERIODIC_WORK),
-					       GFP_KERNEL | __GFP_NORETRY);
-	if (periodic_work == NULL) {
-		ERRDRV("periodic_work allocation failed ");
+	struct periodic_work *pw;
+
+	pw = kzalloc(sizeof(*pw), GFP_KERNEL | __GFP_NORETRY);
+	if (!pw)
 		return NULL;
-	}
-	rwlock_init(&periodic_work->lock);
-	periodic_work->jiffy_interval = jiffy_interval;
-	periodic_work->workqueue = workqueue;
-	periodic_work->workfunc = workfunc;
-	periodic_work->workfuncarg = workfuncarg;
-	periodic_work->devnam = devnam;
-	return periodic_work;
+
+	rwlock_init(&pw->lock);
+	pw->jiffy_interval = jiffy_interval;
+	pw->workqueue = workqueue;
+	pw->workfunc = workfunc;
+	pw->workfuncarg = workfuncarg;
+	pw->devnam = devnam;
+	return pw;
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_create);
 
 
 
-void visor_periodic_work_destroy(PERIODIC_WORK *periodic_work)
+void visor_periodic_work_destroy(struct periodic_work *pw)
 {
-	if (periodic_work == NULL)
-		return;
-	kfree(periodic_work);
+	kfree(pw);
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_destroy);
 
@@ -89,27 +88,26 @@ EXPORT_SYMBOL_GPL(visor_periodic_work_destroy);
  *  If this function returns FALSE, there was a failure and the
  *  periodic work is no longer scheduled
  */
-BOOL visor_periodic_work_nextperiod(PERIODIC_WORK *periodic_work)
+BOOL visor_periodic_work_nextperiod(struct periodic_work *pw)
 {
 	BOOL rc = FALSE;
 
-	write_lock(&periodic_work->lock);
-	if (periodic_work->want_to_stop) {
-		periodic_work->is_scheduled = FALSE;
-		periodic_work->want_to_stop = FALSE;
+	write_lock(&pw->lock);
+	if (pw->want_to_stop) {
+		pw->is_scheduled = FALSE;
+		pw->want_to_stop = FALSE;
 		rc = TRUE;  /* yes, TRUE; see visor_periodic_work_stop() */
-		goto Away;
-	} else if (queue_delayed_work(periodic_work->workqueue,
-				      &periodic_work->work,
-				      periodic_work->jiffy_interval) < 0) {
-		ERRDEV(periodic_work->devnam, "queue_delayed_work failed!");
-		periodic_work->is_scheduled = FALSE;
+		goto unlock;
+	} else if (queue_delayed_work(pw->workqueue, &pw->work,
+				      pw->jiffy_interval) < 0) {
+		ERRDEV(pw->devnam, "queue_delayed_work failed!");
+		pw->is_scheduled = FALSE;
 		rc = FALSE;
-		goto Away;
+		goto unlock;
 	}
 	rc = TRUE;
-Away:
-	write_unlock(&periodic_work->lock);
+unlock:
+	write_unlock(&pw->lock);
 	return rc;
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_nextperiod);
@@ -120,34 +118,32 @@ EXPORT_SYMBOL_GPL(visor_periodic_work_nextperiod);
  *  If this function returns FALSE, then no work was started
  *  (either because it was already started, or because of a failure).
  */
-BOOL visor_periodic_work_start(PERIODIC_WORK *periodic_work)
+BOOL visor_periodic_work_start(struct periodic_work *pw)
 {
 	BOOL rc = FALSE;
 
-	write_lock(&periodic_work->lock);
-	if (periodic_work->is_scheduled) {
+	write_lock(&pw->lock);
+	if (pw->is_scheduled) {
 		rc = FALSE;
-		goto Away;
+		goto unlock;
 	}
-	if (periodic_work->want_to_stop) {
-		ERRDEV(periodic_work->devnam,
+	if (pw->want_to_stop) {
+		ERRDEV(pw->devnam,
 		       "dev_start_periodic_work failed!");
 		rc = FALSE;
-		goto Away;
+		goto unlock;
 	}
-	INIT_DELAYED_WORK(&periodic_work->work, &periodic_work_func);
-	if (queue_delayed_work(periodic_work->workqueue,
-			       &periodic_work->work,
-			       periodic_work->jiffy_interval) < 0) {
-		ERRDEV(periodic_work->devnam,
-		       "%s queue_delayed_work failed!", __func__);
+	INIT_DELAYED_WORK(&pw->work, &periodic_work_func);
+	if (queue_delayed_work(pw->workqueue, &pw->work,
+			       pw->jiffy_interval) < 0) {
+		ERRDEV(pw->devnam, "%s queue_delayed_work failed!", __func__);
 		rc = FALSE;
-		goto Away;
+		goto unlock;
 	}
-	periodic_work->is_scheduled = TRUE;
+	pw->is_scheduled = TRUE;
 	rc = TRUE;
-Away:
-	write_unlock(&periodic_work->lock);
+unlock:
+	write_unlock(&pw->lock);
 	return rc;
 
 }
@@ -190,21 +186,20 @@ EXPORT_SYMBOL_GPL(visor_periodic_work_start);
  *     this deadlock, you will get hung up in an infinite loop saying
  *     "waiting for delayed work...".
  */
-BOOL visor_periodic_work_stop(PERIODIC_WORK *periodic_work)
+BOOL visor_periodic_work_stop(struct periodic_work *pw)
 {
 	BOOL stopped_something = FALSE;
 
-	write_lock(&periodic_work->lock);
-	stopped_something = periodic_work->is_scheduled &&
-		(!periodic_work->want_to_stop);
-	while (periodic_work->is_scheduled) {
-		periodic_work->want_to_stop = TRUE;
-		if (cancel_delayed_work(&periodic_work->work)) {
+	write_lock(&pw->lock);
+	stopped_something = pw->is_scheduled && (!pw->want_to_stop);
+	while (pw->is_scheduled) {
+		pw->want_to_stop = TRUE;
+		if (cancel_delayed_work(&pw->work)) {
 			/* We get here if the delayed work was pending as
 			 * delayed work, but was NOT run.
 			 */
-			ASSERT(periodic_work->is_scheduled);
-			periodic_work->is_scheduled = FALSE;
+			ASSERT(pw->is_scheduled);
+			pw->is_scheduled = FALSE;
 		} else {
 			/* If we get here, either the delayed work:
 			 * - was run, OR,
@@ -216,9 +211,9 @@ BOOL visor_periodic_work_stop(PERIODIC_WORK *periodic_work)
 			 * explains the loop...
 			 */
 		}
-		if (periodic_work->is_scheduled) {
-			write_unlock(&periodic_work->lock);
-			WARNDEV(periodic_work->devnam,
+		if (pw->is_scheduled) {
+			write_unlock(&pw->lock);
+			WARNDEV(pw->devnam,
 				"waiting for delayed work...");
 			/* We rely on the delayed work function running here,
 			 * and eventually calling
@@ -227,11 +222,11 @@ BOOL visor_periodic_work_stop(PERIODIC_WORK *periodic_work)
 			 * subsequently clear is_scheduled.
 			 */
 			SLEEPJIFFIES(10);
-			write_lock(&periodic_work->lock);
+			write_lock(&pw->lock);
 		} else
-			periodic_work->want_to_stop = FALSE;
+			pw->want_to_stop = FALSE;
 	}
-	write_unlock(&periodic_work->lock);
+	write_unlock(&pw->lock);
 	return stopped_something;
 }
 EXPORT_SYMBOL_GPL(visor_periodic_work_stop);
diff --git a/drivers/staging/unisys/visorutil/procobjecttree.c b/drivers/staging/unisys/visorutil/procobjecttree.c
index 5c8c95c51519..c476036f7382 100644
--- a/drivers/staging/unisys/visorutil/procobjecttree.c
+++ b/drivers/staging/unisys/visorutil/procobjecttree.c
@@ -250,9 +250,8 @@ MYPROCOBJECT *visor_proc_CreateObject(MYPROCTYPE *type,
 		}
 		strcpy(obj->name, name);
 		obj->procDir = createProcDir(obj->name, type->procDir);
-		if (obj->procDir == NULL) {
+		if (obj->procDir == NULL)
 			goto Away;
-		}
 	}
 	obj->procDirPropertyContexts =
 		kzalloc((type->nProperties + 1) * sizeof(PROCDIRENTRYCONTEXT),
diff --git a/drivers/staging/unisys/visorutil/visorkmodutils.c b/drivers/staging/unisys/visorutil/visorkmodutils.c
index 10d77cb6ee97..d6815f9e1337 100644
--- a/drivers/staging/unisys/visorutil/visorkmodutils.c
+++ b/drivers/staging/unisys/visorutil/visorkmodutils.c
@@ -46,46 +46,6 @@ void *kmalloc_kernel(size_t siz)
 	return kmalloc(siz, GFP_KERNEL | __GFP_NORETRY);
 }
 
-/*  Use these handy-dandy seq_file_xxx functions if you want to call some
- *  functions that write stuff into a seq_file, but you actually just want
- *  to dump that output into a buffer.  Use them as follows:
- *  - call visor_seq_file_new_buffer to create the seq_file (you supply the buf)
- *  - call whatever functions you want that take a seq_file as an argument
- *    (the buf you supplied will get the output data)
- *  - call visor_seq_file_done_buffer to dispose of your seq_file
- */
-struct seq_file *visor_seq_file_new_buffer(void *buf, size_t buf_size)
-{
-	struct seq_file *rc = NULL;
-	struct seq_file *m = kmalloc_kernel(sizeof(struct seq_file));
-
-	if (m == NULL) {
-		rc = NULL;
-		goto Away;
-	}
-	memset(m, 0, sizeof(struct seq_file));
-	m->buf = buf;
-	m->size = buf_size;
-	rc = m;
-Away:
-	if (rc == NULL) {
-		visor_seq_file_done_buffer(m);
-		m = NULL;
-	}
-	return rc;
-}
-EXPORT_SYMBOL_GPL(visor_seq_file_new_buffer);
-
-
-
-void visor_seq_file_done_buffer(struct seq_file *m)
-{
-	if (!m)
-		return;
-	kfree(m);
-}
-EXPORT_SYMBOL_GPL(visor_seq_file_done_buffer);
-
 static __init uint32_t
 visorutil_spar_detect(void)
 {
diff --git a/drivers/staging/usbip/uapi/usbip.h b/drivers/staging/usbip/uapi/usbip.h
deleted file mode 100644
index fa5db30ede36..000000000000
--- a/drivers/staging/usbip/uapi/usbip.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/*
- *	usbip.h
- *
- *	USBIP uapi defines and function prototypes etc.
-*/
-
-#ifndef _UAPI_LINUX_USBIP_H
-#define _UAPI_LINUX_USBIP_H
-
-/* usbip device status - exported in usbip device sysfs status */
-enum usbip_device_status {
-	/* sdev is available. */
-	SDEV_ST_AVAILABLE = 0x01,
-	/* sdev is now used. */
-	SDEV_ST_USED,
-	/* sdev is unusable because of a fatal error. */
-	SDEV_ST_ERROR,
-
-	/* vdev does not connect a remote device. */
-	VDEV_ST_NULL,
-	/* vdev is used, but the USB address is not assigned yet */
-	VDEV_ST_NOTASSIGNED,
-	VDEV_ST_USED,
-	VDEV_ST_ERROR
-};
-#endif /* _UAPI_LINUX_USBIP_H */
diff --git a/drivers/staging/usbip/userspace/.gitignore b/drivers/staging/usbip/userspace/.gitignore
deleted file mode 100644
index 9aad9e30a8ba..000000000000
--- a/drivers/staging/usbip/userspace/.gitignore
+++ /dev/null
@@ -1,28 +0,0 @@
-Makefile
-Makefile.in
-aclocal.m4
-autom4te.cache/
-config.guess
-config.h
-config.h.in
-config.log
-config.status
-config.sub
-configure
-depcomp
-install-sh
-libsrc/Makefile
-libsrc/Makefile.in
-libtool
-ltmain.sh
-missing
-src/Makefile
-src/Makefile.in
-stamp-h1
-libsrc/libusbip.la
-libsrc/libusbip_la-names.lo
-libsrc/libusbip_la-usbip_common.lo
-libsrc/libusbip_la-usbip_host_driver.lo
-libsrc/libusbip_la-vhci_driver.lo
-src/usbip
-src/usbipd
diff --git a/drivers/staging/usbip/userspace/AUTHORS b/drivers/staging/usbip/userspace/AUTHORS
deleted file mode 100644
index a27ea8d03aec..000000000000
--- a/drivers/staging/usbip/userspace/AUTHORS
+++ /dev/null
@@ -1,3 +0,0 @@
-Takahiro Hirofuchi
-Robert Leibl
-matt mooney <mfm@muteddisk.com>
diff --git a/drivers/staging/usbip/userspace/COPYING b/drivers/staging/usbip/userspace/COPYING
deleted file mode 100644
index c5611e48a8e1..000000000000
--- a/drivers/staging/usbip/userspace/COPYING
+++ /dev/null
@@ -1,340 +0,0 @@
-		    GNU GENERAL PUBLIC LICENSE
-		       Version 2, June 1991
-
- Copyright (C) 1989, 1991 Free Software Foundation, Inc.
-     51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-			    Preamble
-
-  The licenses for most software are designed to take away your
-freedom to share and change it.  By contrast, the GNU General Public
-License is intended to guarantee your freedom to share and change free
-software--to make sure the software is free for all its users.  This
-General Public License applies to most of the Free Software
-Foundation's software and to any other program whose authors commit to
-using it.  (Some other Free Software Foundation software is covered by
-the GNU Library General Public License instead.)  You can apply it to
-your programs, too.
-
-  When we speak of free software, we are referring to freedom, not
-price.  Our General Public Licenses are designed to make sure that you
-have the freedom to distribute copies of free software (and charge for
-this service if you wish), that you receive source code or can get it
-if you want it, that you can change the software or use pieces of it
-in new free programs; and that you know you can do these things.
-
-  To protect your rights, we need to make restrictions that forbid
-anyone to deny you these rights or to ask you to surrender the rights.
-These restrictions translate to certain responsibilities for you if you
-distribute copies of the software, or if you modify it.
-
-  For example, if you distribute copies of such a program, whether
-gratis or for a fee, you must give the recipients all the rights that
-you have.  You must make sure that they, too, receive or can get the
-source code.  And you must show them these terms so they know their
-rights.
-
-  We protect your rights with two steps: (1) copyright the software, and
-(2) offer you this license which gives you legal permission to copy,
-distribute and/or modify the software.
-
-  Also, for each author's protection and ours, we want to make certain
-that everyone understands that there is no warranty for this free
-software.  If the software is modified by someone else and passed on, we
-want its recipients to know that what they have is not the original, so
-that any problems introduced by others will not reflect on the original
-authors' reputations.
-
-  Finally, any free program is threatened constantly by software
-patents.  We wish to avoid the danger that redistributors of a free
-program will individually obtain patent licenses, in effect making the
-program proprietary.  To prevent this, we have made it clear that any
-patent must be licensed for everyone's free use or not licensed at all.
-
-  The precise terms and conditions for copying, distribution and
-modification follow.
-
-		    GNU GENERAL PUBLIC LICENSE
-   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
-
-  0. This License applies to any program or other work which contains
-a notice placed by the copyright holder saying it may be distributed
-under the terms of this General Public License.  The "Program", below,
-refers to any such program or work, and a "work based on the Program"
-means either the Program or any derivative work under copyright law:
-that is to say, a work containing the Program or a portion of it,
-either verbatim or with modifications and/or translated into another
-language.  (Hereinafter, translation is included without limitation in
-the term "modification".)  Each licensee is addressed as "you".
-
-Activities other than copying, distribution and modification are not
-covered by this License; they are outside its scope.  The act of
-running the Program is not restricted, and the output from the Program
-is covered only if its contents constitute a work based on the
-Program (independent of having been made by running the Program).
-Whether that is true depends on what the Program does.
-
-  1. You may copy and distribute verbatim copies of the Program's
-source code as you receive it, in any medium, provided that you
-conspicuously and appropriately publish on each copy an appropriate
-copyright notice and disclaimer of warranty; keep intact all the
-notices that refer to this License and to the absence of any warranty;
-and give any other recipients of the Program a copy of this License
-along with the Program.
-
-You may charge a fee for the physical act of transferring a copy, and
-you may at your option offer warranty protection in exchange for a fee.
-
-  2. You may modify your copy or copies of the Program or any portion
-of it, thus forming a work based on the Program, and copy and
-distribute such modifications or work under the terms of Section 1
-above, provided that you also meet all of these conditions:
-
-    a) You must cause the modified files to carry prominent notices
-    stating that you changed the files and the date of any change.
-
-    b) You must cause any work that you distribute or publish, that in
-    whole or in part contains or is derived from the Program or any
-    part thereof, to be licensed as a whole at no charge to all third
-    parties under the terms of this License.
-
-    c) If the modified program normally reads commands interactively
-    when run, you must cause it, when started running for such
-    interactive use in the most ordinary way, to print or display an
-    announcement including an appropriate copyright notice and a
-    notice that there is no warranty (or else, saying that you provide
-    a warranty) and that users may redistribute the program under
-    these conditions, and telling the user how to view a copy of this
-    License.  (Exception: if the Program itself is interactive but
-    does not normally print such an announcement, your work based on
-    the Program is not required to print an announcement.)
-
-These requirements apply to the modified work as a whole.  If
-identifiable sections of that work are not derived from the Program,
-and can be reasonably considered independent and separate works in
-themselves, then this License, and its terms, do not apply to those
-sections when you distribute them as separate works.  But when you
-distribute the same sections as part of a whole which is a work based
-on the Program, the distribution of the whole must be on the terms of
-this License, whose permissions for other licensees extend to the
-entire whole, and thus to each and every part regardless of who wrote it.
-
-Thus, it is not the intent of this section to claim rights or contest
-your rights to work written entirely by you; rather, the intent is to
-exercise the right to control the distribution of derivative or
-collective works based on the Program.
-
-In addition, mere aggregation of another work not based on the Program
-with the Program (or with a work based on the Program) on a volume of
-a storage or distribution medium does not bring the other work under
-the scope of this License.
-
-  3. You may copy and distribute the Program (or a work based on it,
-under Section 2) in object code or executable form under the terms of
-Sections 1 and 2 above provided that you also do one of the following:
-
-    a) Accompany it with the complete corresponding machine-readable
-    source code, which must be distributed under the terms of Sections
-    1 and 2 above on a medium customarily used for software interchange; or,
-
-    b) Accompany it with a written offer, valid for at least three
-    years, to give any third party, for a charge no more than your
-    cost of physically performing source distribution, a complete
-    machine-readable copy of the corresponding source code, to be
-    distributed under the terms of Sections 1 and 2 above on a medium
-    customarily used for software interchange; or,
-
-    c) Accompany it with the information you received as to the offer
-    to distribute corresponding source code.  (This alternative is
-    allowed only for noncommercial distribution and only if you
-    received the program in object code or executable form with such
-    an offer, in accord with Subsection b above.)
-
-The source code for a work means the preferred form of the work for
-making modifications to it.  For an executable work, complete source
-code means all the source code for all modules it contains, plus any
-associated interface definition files, plus the scripts used to
-control compilation and installation of the executable.  However, as a
-special exception, the source code distributed need not include
-anything that is normally distributed (in either source or binary
-form) with the major components (compiler, kernel, and so on) of the
-operating system on which the executable runs, unless that component
-itself accompanies the executable.
-
-If distribution of executable or object code is made by offering
-access to copy from a designated place, then offering equivalent
-access to copy the source code from the same place counts as
-distribution of the source code, even though third parties are not
-compelled to copy the source along with the object code.
-
-  4. You may not copy, modify, sublicense, or distribute the Program
-except as expressly provided under this License.  Any attempt
-otherwise to copy, modify, sublicense or distribute the Program is
-void, and will automatically terminate your rights under this License.
-However, parties who have received copies, or rights, from you under
-this License will not have their licenses terminated so long as such
-parties remain in full compliance.
-
-  5. You are not required to accept this License, since you have not
-signed it.  However, nothing else grants you permission to modify or
-distribute the Program or its derivative works.  These actions are
-prohibited by law if you do not accept this License.  Therefore, by
-modifying or distributing the Program (or any work based on the
-Program), you indicate your acceptance of this License to do so, and
-all its terms and conditions for copying, distributing or modifying
-the Program or works based on it.
-
-  6. Each time you redistribute the Program (or any work based on the
-Program), the recipient automatically receives a license from the
-original licensor to copy, distribute or modify the Program subject to
-these terms and conditions.  You may not impose any further
-restrictions on the recipients' exercise of the rights granted herein.
-You are not responsible for enforcing compliance by third parties to
-this License.
-
-  7. If, as a consequence of a court judgment or allegation of patent
-infringement or for any other reason (not limited to patent issues),
-conditions are imposed on you (whether by court order, agreement or
-otherwise) that contradict the conditions of this License, they do not
-excuse you from the conditions of this License.  If you cannot
-distribute so as to satisfy simultaneously your obligations under this
-License and any other pertinent obligations, then as a consequence you
-may not distribute the Program at all.  For example, if a patent
-license would not permit royalty-free redistribution of the Program by
-all those who receive copies directly or indirectly through you, then
-the only way you could satisfy both it and this License would be to
-refrain entirely from distribution of the Program.
-
-If any portion of this section is held invalid or unenforceable under
-any particular circumstance, the balance of the section is intended to
-apply and the section as a whole is intended to apply in other
-circumstances.
-
-It is not the purpose of this section to induce you to infringe any
-patents or other property right claims or to contest validity of any
-such claims; this section has the sole purpose of protecting the
-integrity of the free software distribution system, which is
-implemented by public license practices.  Many people have made
-generous contributions to the wide range of software distributed
-through that system in reliance on consistent application of that
-system; it is up to the author/donor to decide if he or she is willing
-to distribute software through any other system and a licensee cannot
-impose that choice.
-
-This section is intended to make thoroughly clear what is believed to
-be a consequence of the rest of this License.
-
-  8. If the distribution and/or use of the Program is restricted in
-certain countries either by patents or by copyrighted interfaces, the
-original copyright holder who places the Program under this License
-may add an explicit geographical distribution limitation excluding
-those countries, so that distribution is permitted only in or among
-countries not thus excluded.  In such case, this License incorporates
-the limitation as if written in the body of this License.
-
-  9. The Free Software Foundation may publish revised and/or new versions
-of the General Public License from time to time.  Such new versions will
-be similar in spirit to the present version, but may differ in detail to
-address new problems or concerns.
-
-Each version is given a distinguishing version number.  If the Program
-specifies a version number of this License which applies to it and "any
-later version", you have the option of following the terms and conditions
-either of that version or of any later version published by the Free
-Software Foundation.  If the Program does not specify a version number of
-this License, you may choose any version ever published by the Free Software
-Foundation.
-
-  10. If you wish to incorporate parts of the Program into other free
-programs whose distribution conditions are different, write to the author
-to ask for permission.  For software which is copyrighted by the Free
-Software Foundation, write to the Free Software Foundation; we sometimes
-make exceptions for this.  Our decision will be guided by the two goals
-of preserving the free status of all derivatives of our free software and
-of promoting the sharing and reuse of software generally.
-
-			    NO WARRANTY
-
-  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
-FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
-OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
-PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
-OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
-MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
-TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
-PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
-REPAIR OR CORRECTION.
-
-  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
-WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
-REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
-INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
-OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
-TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
-YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
-PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
-POSSIBILITY OF SUCH DAMAGES.
-
-		     END OF TERMS AND CONDITIONS
-
-	    How to Apply These Terms to Your New Programs
-
-  If you develop a new program, and you want it to be of the greatest
-possible use to the public, the best way to achieve this is to make it
-free software which everyone can redistribute and change under these terms.
-
-  To do so, attach the following notices to the program.  It is safest
-to attach them to the start of each source file to most effectively
-convey the exclusion of warranty; and each file should have at least
-the "copyright" line and a pointer to where the full notice is found.
-
-    <one line to give the program's name and a brief idea of what it does.>
-    Copyright (C) <year>  <name of author>
-
-    This program is free software; you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation; either version 2 of the License, or
-    (at your option) any later version.
-
-    This program is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program; if not, write to the Free Software
-    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-
-
-Also add information on how to contact you by electronic and paper mail.
-
-If the program is interactive, make it output a short notice like this
-when it starts in an interactive mode:
-
-    Gnomovision version 69, Copyright (C) year  name of author
-    Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
-    This is free software, and you are welcome to redistribute it
-    under certain conditions; type `show c' for details.
-
-The hypothetical commands `show w' and `show c' should show the appropriate
-parts of the General Public License.  Of course, the commands you use may
-be called something other than `show w' and `show c'; they could even be
-mouse-clicks or menu items--whatever suits your program.
-
-You should also get your employer (if you work as a programmer) or your
-school, if any, to sign a "copyright disclaimer" for the program, if
-necessary.  Here is a sample; alter the names:
-
-  Yoyodyne, Inc., hereby disclaims all copyright interest in the program
-  `Gnomovision' (which makes passes at compilers) written by James Hacker.
-
-  <signature of Ty Coon>, 1 April 1989
-  Ty Coon, President of Vice
-
-This General Public License does not permit incorporating your program into
-proprietary programs.  If your program is a subroutine library, you may
-consider it more useful to permit linking proprietary applications with the
-library.  If this is what you want to do, use the GNU Library General
-Public License instead of this License.
diff --git a/drivers/staging/usbip/userspace/INSTALL b/drivers/staging/usbip/userspace/INSTALL
deleted file mode 100644
index d3c5b40a9409..000000000000
--- a/drivers/staging/usbip/userspace/INSTALL
+++ /dev/null
@@ -1,237 +0,0 @@
-Installation Instructions
-*************************
-
-Copyright (C) 1994, 1995, 1996, 1999, 2000, 2001, 2002, 2004, 2005,
-2006, 2007 Free Software Foundation, Inc.
-
-This file is free documentation; the Free Software Foundation gives
-unlimited permission to copy, distribute and modify it.
-
-Basic Installation
-==================
-
-Briefly, the shell commands `./configure; make; make install' should
-configure, build, and install this package.  The following
-more-detailed instructions are generic; see the `README' file for
-instructions specific to this package.
-
-   The `configure' shell script attempts to guess correct values for
-various system-dependent variables used during compilation.  It uses
-those values to create a `Makefile' in each directory of the package.
-It may also create one or more `.h' files containing system-dependent
-definitions.  Finally, it creates a shell script `config.status' that
-you can run in the future to recreate the current configuration, and a
-file `config.log' containing compiler output (useful mainly for
-debugging `configure').
-
-   It can also use an optional file (typically called `config.cache'
-and enabled with `--cache-file=config.cache' or simply `-C') that saves
-the results of its tests to speed up reconfiguring.  Caching is
-disabled by default to prevent problems with accidental use of stale
-cache files.
-
-   If you need to do unusual things to compile the package, please try
-to figure out how `configure' could check whether to do them, and mail
-diffs or instructions to the address given in the `README' so they can
-be considered for the next release.  If you are using the cache, and at
-some point `config.cache' contains results you don't want to keep, you
-may remove or edit it.
-
-   The file `configure.ac' (or `configure.in') is used to create
-`configure' by a program called `autoconf'.  You need `configure.ac' if
-you want to change it or regenerate `configure' using a newer version
-of `autoconf'.
-
-The simplest way to compile this package is:
-
-  1. `cd' to the directory containing the package's source code and type
-     `./configure' to configure the package for your system.
-
-     Running `configure' might take a while.  While running, it prints
-     some messages telling which features it is checking for.
-
-  2. Type `make' to compile the package.
-
-  3. Optionally, type `make check' to run any self-tests that come with
-     the package.
-
-  4. Type `make install' to install the programs and any data files and
-     documentation.
-
-  5. You can remove the program binaries and object files from the
-     source code directory by typing `make clean'.  To also remove the
-     files that `configure' created (so you can compile the package for
-     a different kind of computer), type `make distclean'.  There is
-     also a `make maintainer-clean' target, but that is intended mainly
-     for the package's developers.  If you use it, you may have to get
-     all sorts of other programs in order to regenerate files that came
-     with the distribution.
-
-  6. Often, you can also type `make uninstall' to remove the installed
-     files again.
-
-Compilers and Options
-=====================
-
-Some systems require unusual options for compilation or linking that the
-`configure' script does not know about.  Run `./configure --help' for
-details on some of the pertinent environment variables.
-
-   You can give `configure' initial values for configuration parameters
-by setting variables in the command line or in the environment.  Here
-is an example:
-
-     ./configure CC=c99 CFLAGS=-g LIBS=-lposix
-
-   *Note Defining Variables::, for more details.
-
-Compiling For Multiple Architectures
-====================================
-
-You can compile the package for more than one kind of computer at the
-same time, by placing the object files for each architecture in their
-own directory.  To do this, you can use GNU `make'.  `cd' to the
-directory where you want the object files and executables to go and run
-the `configure' script.  `configure' automatically checks for the
-source code in the directory that `configure' is in and in `..'.
-
-   With a non-GNU `make', it is safer to compile the package for one
-architecture at a time in the source code directory.  After you have
-installed the package for one architecture, use `make distclean' before
-reconfiguring for another architecture.
-
-Installation Names
-==================
-
-By default, `make install' installs the package's commands under
-`/usr/local/bin', include files under `/usr/local/include', etc.  You
-can specify an installation prefix other than `/usr/local' by giving
-`configure' the option `--prefix=PREFIX'.
-
-   You can specify separate installation prefixes for
-architecture-specific files and architecture-independent files.  If you
-pass the option `--exec-prefix=PREFIX' to `configure', the package uses
-PREFIX as the prefix for installing programs and libraries.
-Documentation and other data files still use the regular prefix.
-
-   In addition, if you use an unusual directory layout you can give
-options like `--bindir=DIR' to specify different values for particular
-kinds of files.  Run `configure --help' for a list of the directories
-you can set and what kinds of files go in them.
-
-   If the package supports it, you can cause programs to be installed
-with an extra prefix or suffix on their names by giving `configure' the
-option `--program-prefix=PREFIX' or `--program-suffix=SUFFIX'.
-
-Optional Features
-=================
-
-Some packages pay attention to `--enable-FEATURE' options to
-`configure', where FEATURE indicates an optional part of the package.
-They may also pay attention to `--with-PACKAGE' options, where PACKAGE
-is something like `gnu-as' or `x' (for the X Window System).  The
-`README' should mention any `--enable-' and `--with-' options that the
-package recognizes.
-
-   For packages that use the X Window System, `configure' can usually
-find the X include and library files automatically, but if it doesn't,
-you can use the `configure' options `--x-includes=DIR' and
-`--x-libraries=DIR' to specify their locations.
-
-Specifying the System Type
-==========================
-
-There may be some features `configure' cannot figure out automatically,
-but needs to determine by the type of machine the package will run on.
-Usually, assuming the package is built to be run on the _same_
-architectures, `configure' can figure that out, but if it prints a
-message saying it cannot guess the machine type, give it the
-`--build=TYPE' option.  TYPE can either be a short name for the system
-type, such as `sun4', or a canonical name which has the form:
-
-     CPU-COMPANY-SYSTEM
-
-where SYSTEM can have one of these forms:
-
-     OS KERNEL-OS
-
-   See the file `config.sub' for the possible values of each field.  If
-`config.sub' isn't included in this package, then this package doesn't
-need to know the machine type.
-
-   If you are _building_ compiler tools for cross-compiling, you should
-use the option `--target=TYPE' to select the type of system they will
-produce code for.
-
-   If you want to _use_ a cross compiler, that generates code for a
-platform different from the build platform, you should specify the
-"host" platform (i.e., that on which the generated programs will
-eventually be run) with `--host=TYPE'.
-
-Sharing Defaults
-================
-
-If you want to set default values for `configure' scripts to share, you
-can create a site shell script called `config.site' that gives default
-values for variables like `CC', `cache_file', and `prefix'.
-`configure' looks for `PREFIX/share/config.site' if it exists, then
-`PREFIX/etc/config.site' if it exists.  Or, you can set the
-`CONFIG_SITE' environment variable to the location of the site script.
-A warning: not all `configure' scripts look for a site script.
-
-Defining Variables
-==================
-
-Variables not defined in a site shell script can be set in the
-environment passed to `configure'.  However, some packages may run
-configure again during the build, and the customized values of these
-variables may be lost.  In order to avoid this problem, you should set
-them in the `configure' command line, using `VAR=value'.  For example:
-
-     ./configure CC=/usr/local2/bin/gcc
-
-causes the specified `gcc' to be used as the C compiler (unless it is
-overridden in the site shell script).
-
-Unfortunately, this technique does not work for `CONFIG_SHELL' due to
-an Autoconf bug.  Until the bug is fixed you can use this workaround:
-
-     CONFIG_SHELL=/bin/bash /bin/bash ./configure CONFIG_SHELL=/bin/bash
-
-`configure' Invocation
-======================
-
-`configure' recognizes the following options to control how it operates.
-
-`--help'
-`-h'
-     Print a summary of the options to `configure', and exit.
-
-`--version'
-`-V'
-     Print the version of Autoconf used to generate the `configure'
-     script, and exit.
-
-`--cache-file=FILE'
-     Enable the cache: use and save the results of the tests in FILE,
-     traditionally `config.cache'.  FILE defaults to `/dev/null' to
-     disable caching.
-
-`--config-cache'
-`-C'
-     Alias for `--cache-file=config.cache'.
-
-`--quiet'
-`--silent'
-`-q'
-     Do not print messages saying which checks are being made.  To
-     suppress all normal output, redirect it to `/dev/null' (any error
-     messages will still be shown).
-
-`--srcdir=DIR'
-     Look for the package's source code in directory DIR.  Usually
-     `configure' can determine that directory automatically.
-
-`configure' also accepts some other, not widely useful, options.  Run
-`configure --help' for more details.
-
diff --git a/drivers/staging/usbip/userspace/Makefile.am b/drivers/staging/usbip/userspace/Makefile.am
deleted file mode 100644
index 66f8bf038c9f..000000000000
--- a/drivers/staging/usbip/userspace/Makefile.am
+++ /dev/null
@@ -1,6 +0,0 @@
-SUBDIRS := libsrc src
-includedir = @includedir@/usbip
-include_HEADERS := $(addprefix libsrc/, \
-		     usbip_common.h vhci_driver.h usbip_host_driver.h)
-
-dist_man_MANS := $(addprefix doc/, usbip.8 usbipd.8)
diff --git a/drivers/staging/usbip/userspace/README b/drivers/staging/usbip/userspace/README
deleted file mode 100644
index 831f49fea3ce..000000000000
--- a/drivers/staging/usbip/userspace/README
+++ /dev/null
@@ -1,202 +0,0 @@
-#
-# README for usbip-utils
-#
-# Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
-#               2005-2008 Takahiro Hirofuchi
-
-
-[Requirements]
-    - USB/IP device drivers
-	Found in the staging directory of the Linux kernel.
-
-    - libudev >= 2.0
-	libudev library
-
-    - libwrap0-dev
-	tcp wrapper library
-
-    - gcc >= 4.0
-
-    - libtool, automake >= 1.9, autoconf >= 2.5.0, pkg-config
-
-[Optional]
-    - hwdata
-        Contains USB device identification data.
-
-
-[Install]
-    0. Generate configuration scripts.
-	$ ./autogen.sh
-
-    1. Compile & install the userspace utilities.
-	$ ./configure [--with-tcp-wrappers=no] [--with-usbids-dir=<dir>]
-	$ make install
-
-    2. Compile & install USB/IP drivers.
-
-
-[Usage]
-    server:# (Physically attach your USB device.)
-
-    server:# insmod usbip-core.ko
-    server:# insmod usbip-host.ko
-
-    server:# usbipd -D
-	- Start usbip daemon.
-
-    server:# usbip list -l
-	- List driver assignments for USB devices.
-
-    server:# usbip bind --busid 1-2
-	- Bind usbip-host.ko to the device with busid 1-2.
-	- The USB device 1-2 is now exportable to other hosts!
-	- Use `usbip unbind --busid 1-2' to stop exporting the device.
-
-    client:# insmod usbip-core.ko
-    client:# insmod vhci-hcd.ko
-
-    client:# usbip list --remote <host>
-	- List exported USB devices on the <host>.
-
-    client:# usbip attach --remote <host> --busid 1-2
-	- Connect the remote USB device.
-
-    client:# usbip port
-	- Show virtual port status.
-
-    client:# usbip detach --port <port>
-	- Detach the USB device.
-
-
-[Example]
----------------------------
-	SERVER SIDE
----------------------------
-Physically attach your USB devices to this host.
-
-    trois:# insmod path/to/usbip-core.ko
-    trois:# insmod path/to/usbip-host.ko
-    trois:# usbipd -D
-
-In another terminal, let's look up what USB devices are physically
-attached to this host.
-
-    trois:# usbip list -l
-    Local USB devices
-    =================
-     - busid 1-1 (05a9:a511)
-	     1-1:1.0 -> ov511
-
-     - busid 3-2 (0711:0902)
-	     3-2:1.0 -> none
-
-     - busid 3-3.1 (08bb:2702)
-	     3-3.1:1.0 -> snd-usb-audio
-	     3-3.1:1.1 -> snd-usb-audio
-
-     - busid 3-3.2 (04bb:0206)
-	     3-3.2:1.0 -> usb-storage
-
-     - busid 3-3 (0409:0058)
-	     3-3:1.0 -> hub
-
-     - busid 4-1 (046d:08b2)
-	     4-1:1.0 -> none
-	     4-1:1.1 -> none
-	     4-1:1.2 -> none
-
-     - busid 5-2 (058f:9254)
-	     5-2:1.0 -> hub
-
-A USB storage device of busid 3-3.2 is now bound to the usb-storage
-driver. To export this device, we first mark the device as
-"exportable"; the device is bound to the usbip-host driver. Please
-remember you can not export a USB hub.
-
-Mark the device of busid 3-3.2 as exportable:
-
-    trois:# usbip --debug bind --busid 3-3.2
-    ...
-    usbip debug: usbip_bind.c:162:[unbind_other] 3-3.2:1.0 -> usb-storage
-    ...
-    bind device on busid 3-3.2: complete
-
-    trois:# usbip list -l
-    Local USB devices
-    =================
-    ...
-
-     - busid 3-3.2 (04bb:0206)
-	     3-3.2:1.0 -> usbip-host
-    ...
-
----------------------------
-	CLIENT SIDE
----------------------------
-First, let's list available remote devices that are marked as
-exportable on the host.
-
-    deux:# insmod path/to/usbip-core.ko
-    deux:# insmod path/to/vhci-hcd.ko
-
-    deux:# usbip list --remote 10.0.0.3
-    Exportable USB devices
-    ======================
-     - 10.0.0.3
-	    1-1: Prolific Technology, Inc. : unknown product (067b:3507)
-	       : /sys/devices/pci0000:00/0000:00:1f.2/usb1/1-1
-	       : (Defined at Interface level) / unknown subclass / unknown protocol (00/00/00)
-	       :  0 - Mass Storage / SCSI / Bulk (Zip) (08/06/50)
-
-	1-2.2.1: Apple Computer, Inc. : unknown product (05ac:0203)
-	       : /sys/devices/pci0000:00/0000:00:1f.2/usb1/1-2/1-2.2/1-2.2.1
-	       : (Defined at Interface level) / unknown subclass / unknown protocol (00/00/00)
-	       :  0 - Human Interface Devices / Boot Interface Subclass / Keyboard (03/01/01)
-
-	1-2.2.3: OmniVision Technologies, Inc. : OV511+ WebCam (05a9:a511)
-	       : /sys/devices/pci0000:00/0000:00:1f.2/usb1/1-2/1-2.2/1-2.2.3
-	       : (Defined at Interface level) / unknown subclass / unknown protocol (00/00/00)
-	       :  0 - Vendor Specific Class / unknown subclass / unknown protocol (ff/00/00)
-
-	    3-1: Logitech, Inc. : QuickCam Pro 4000 (046d:08b2)
-	       : /sys/devices/pci0000:00/0000:00:1e.0/0000:02:0a.0/usb3/3-1
-	       : (Defined at Interface level) / unknown subclass / unknown protocol (00/00/00)
-	       :  0 - Data / unknown subclass / unknown protocol (0a/ff/00)
-	       :  1 - Audio / Control Device / unknown protocol (01/01/00)
-	       :  2 - Audio / Streaming / unknown protocol (01/02/00)
-
-Attach a remote USB device:
-
-    deux:# usbip attach --remote 10.0.0.3 --busid 1-1
-    port 0 attached
-
-Show the devices attached to this client:
-
-    deux:# usbip port
-    Port 00: <Port in Use> at Full Speed(12Mbps)
-	   Prolific Technology, Inc. : unknown product (067b:3507)
-	   6-1 -> usbip://10.0.0.3:3240/1-1  (remote bus/dev 001/004)
-	   6-1:1.0 used by usb-storage
-			  /sys/class/scsi_device/0:0:0:0/device
-			  /sys/class/scsi_host/host0/device
-			  /sys/block/sda/device
-
-Detach the imported device:
-
-    deux:# usbip detach --port 0
-    port 0 detached
-
-
-[Checklist]
-    - See 'Debug Tips' on the project wiki.
-	- http://usbip.wiki.sourceforge.net/how-to-debug-usbip
-    - usbip-host.ko must be bound to the target device.
-	- See /proc/bus/usb/devices and find "Driver=..." lines of the device.
-    - Shutdown firewall.
-	- usbip now uses TCP port 3240.
-    - Disable SELinux.
-    - Check the kernel and daemon messages.
-
-
-[Contact]
-    Mailing List: linux-usb@vger.kernel.org
diff --git a/drivers/staging/usbip/userspace/autogen.sh b/drivers/staging/usbip/userspace/autogen.sh
deleted file mode 100755
index e1112d3fcbf6..000000000000
--- a/drivers/staging/usbip/userspace/autogen.sh
+++ /dev/null
@@ -1,9 +0,0 @@
-#!/bin/sh -x
-
-#aclocal
-#autoheader
-#libtoolize --copy --force
-#automake-1.9 -acf
-#autoconf
-
-autoreconf -i -f -v
diff --git a/drivers/staging/usbip/userspace/cleanup.sh b/drivers/staging/usbip/userspace/cleanup.sh
deleted file mode 100755
index 955c3ccb729a..000000000000
--- a/drivers/staging/usbip/userspace/cleanup.sh
+++ /dev/null
@@ -1,12 +0,0 @@
-#!/bin/sh
-
-if [ -r Makefile ]; then
-	make distclean
-fi
-
-FILES="aclocal.m4 autom4te.cache compile config.guess config.h.in config.log \
-       config.status config.sub configure cscope.out depcomp install-sh      \
-       libsrc/Makefile libsrc/Makefile.in libtool ltmain.sh Makefile         \
-       Makefile.in missing src/Makefile src/Makefile.in"
-
-rm -vRf $FILES
diff --git a/drivers/staging/usbip/userspace/configure.ac b/drivers/staging/usbip/userspace/configure.ac
deleted file mode 100644
index 607d05c5ccfd..000000000000
--- a/drivers/staging/usbip/userspace/configure.ac
+++ /dev/null
@@ -1,111 +0,0 @@
-dnl Process this file with autoconf to produce a configure script.
-
-AC_PREREQ(2.59)
-AC_INIT([usbip-utils], [2.0], [linux-usb@vger.kernel.org])
-AC_DEFINE([USBIP_VERSION], [0x00000111], [binary-coded decimal version number])
-
-CURRENT=0
-REVISION=1
-AGE=0
-AC_SUBST([LIBUSBIP_VERSION], [$CURRENT:$REVISION:$AGE], [library version])
-
-AC_CONFIG_SRCDIR([src/usbipd.c])
-AC_CONFIG_HEADERS([config.h])
-
-AM_INIT_AUTOMAKE([foreign])
-LT_INIT
-
-# Silent build for automake >= 1.11
-m4_ifdef([AM_SILENT_RULES], [AM_SILENT_RULES([yes])])
-
-AC_SUBST([EXTRA_CFLAGS], ["-Wall -Werror -Wextra -std=gnu99"])
-
-# Checks for programs.
-AC_PROG_CC
-AC_PROG_INSTALL
-AC_PROG_MAKE_SET
-
-# Checks for header files.
-AC_HEADER_DIRENT
-AC_HEADER_STDC
-AC_CHECK_HEADERS([arpa/inet.h fcntl.h netdb.h netinet/in.h stdint.h stdlib.h dnl
-		  string.h sys/socket.h syslog.h unistd.h])
-
-# Checks for typedefs, structures, and compiler characteristics.
-AC_TYPE_INT32_T
-AC_TYPE_SIZE_T
-AC_TYPE_SSIZE_T
-AC_TYPE_UINT16_T
-AC_TYPE_UINT32_T
-AC_TYPE_UINT8_T
-
-# Checks for library functions.
-AC_FUNC_REALLOC
-AC_CHECK_FUNCS([memset mkdir regcomp socket strchr strerror strstr dnl
-		strtoul])
-
-AC_CHECK_HEADER([libudev.h],
-		[AC_CHECK_LIB([udev], [udev_new],
-			      [LIBS="$LIBS -ludev"],
-			      [AC_MSG_ERROR([Missing udev library!])])],
-		[AC_MSG_ERROR([Missing /usr/include/libudev.h])])
-
-# Checks for libwrap library.
-AC_MSG_CHECKING([whether to use the libwrap (TCP wrappers) library])
-AC_ARG_WITH([tcp-wrappers],
-	    [AS_HELP_STRING([--with-tcp-wrappers],
-			    [use the libwrap (TCP wrappers) library])],
-	    dnl [ACTION-IF-GIVEN]
-	    [if test "$withval" = "yes"; then
-		     AC_MSG_RESULT([yes])
-		     AC_MSG_CHECKING([for hosts_access in -lwrap])
-		     saved_LIBS="$LIBS"
-		     LIBS="-lwrap $saved_LIBS"
-		     AC_TRY_LINK(
-		       [int hosts_access(); int allow_severity, deny_severity;],
-		       [hosts_access()],
-		       [AC_MSG_RESULT([yes]);
-			AC_DEFINE([HAVE_LIBWRAP], [1],
-				  [use tcp wrapper]) wrap_LIB="-lwrap"],
-		       [AC_MSG_RESULT([not found]); exit 1])
-	     else
-		     AC_MSG_RESULT([no]);
-	     fi],
-	    dnl [ACTION-IF-NOT-GIVEN]
-	    [AC_MSG_RESULT([(default)])
-	     AC_MSG_CHECKING([for hosts_access in -lwrap])
-	     saved_LIBS="$LIBS"
-	     LIBS="-lwrap $saved_LIBS"
-	     AC_TRY_LINK(
-	       [int hosts_access(); int allow_severity, deny_severity;],
-	       [hosts_access()],
-	       [AC_MSG_RESULT([yes]);
-		AC_DEFINE([HAVE_LIBWRAP], [1], [use tcp wrapper])],
-	       [AC_MSG_RESULT([no]); LIBS="$saved_LIBS"])])
-
-# Sets directory containing usb.ids.
-AC_ARG_WITH([usbids-dir],
-	    [AS_HELP_STRING([--with-usbids-dir=DIR],
-	       [where usb.ids is found (default /usr/share/hwdata/)])],
-	    [USBIDS_DIR=$withval], [USBIDS_DIR="/usr/share/hwdata/"])
-AC_SUBST([USBIDS_DIR])
-
-# use _FORTIFY_SOURCE
-AC_MSG_CHECKING([whether to use fortify])
-AC_ARG_WITH([fortify],
-	    [AS_HELP_STRING([--with-fortify],
-			    [use _FORTIFY_SROUCE option when compiling)])],
-			    dnl [ACTION-IF-GIVEN]
-			    [if test "$withval" = "yes"; then
-				AC_MSG_RESULT([yes])
-				CFLAGS="$CFLAGS -D_FORTIFY_SOURCE -O"
-			     else
-			     	AC_MSG_RESULT([no])
-				CFLAGS="$CFLAGS -U_FORTIFY_SOURCE"
-			     fi
-			    ],
-			    dnl [ACTION-IF-NOT-GIVEN]
-			    [AC_MSG_RESULT([default])])
-
-AC_CONFIG_FILES([Makefile libsrc/Makefile src/Makefile])
-AC_OUTPUT
diff --git a/drivers/staging/usbip/userspace/doc/usbip.8 b/drivers/staging/usbip/userspace/doc/usbip.8
deleted file mode 100644
index a6097be25d28..000000000000
--- a/drivers/staging/usbip/userspace/doc/usbip.8
+++ /dev/null
@@ -1,95 +0,0 @@
-.TH USBIP "8" "February 2009" "usbip" "System Administration Utilities"
-.SH NAME
-usbip \- manage USB/IP devices
-.SH SYNOPSIS
-.B usbip
-[\fIoptions\fR] <\fIcommand\fR> <\fIargs\fR>
-
-.SH DESCRIPTION
-On a USB/IP server, devices can be listed, bound, and unbound using
-this program.  On a USB/IP client, devices exported by USB/IP servers
-can be listed, attached and detached.
-
-.SH OPTIONS
-.HP
-\fB\-\-debug\fR
-.IP
-Print debugging information.
-.PP
-
-.HP
-\fB\-\-log\fR
-.IP
-Log to syslog.
-.PP
-
-.HP
-\fB\-\-tcp-port PORT\fR
-.IP
-Connect to PORT on remote host (used for attach and list --remote).
-.PP
-
-.SH COMMANDS
-.HP
-\fBversion\fR
-.IP
-Show version and exit.
-.PP
-
-.HP
-\fBhelp\fR [\fIcommand\fR]
-.IP
-Print the program help message, or help on a specific command, and
-then exit.
-.PP
-
-.HP
-\fBattach\fR \-\-remote=<\fIhost\fR> \-\-busid=<\fIbus_id\fR>
-.IP
-Attach a remote USB device.
-.PP
-
-.HP
-\fBdetach\fR \-\-port=<\fIport\fR>
-.IP
-Detach an imported USB device.
-.PP
-
-.HP
-\fBbind\fR \-\-busid=<\fIbusid\fR>
-.IP
-Make a device exportable.
-.PP
-
-.HP
-\fBunbind\fR \-\-busid=<\fIbusid\fR>
-.IP
-Stop exporting a device so it can be used by a local driver.
-.PP
-
-.HP
-\fBlist\fR \-\-remote=<\fIhost\fR>
-.IP
-List USB devices exported by a remote host.
-.PP
-
-.HP
-\fBlist\fR \-\-local
-.IP
-List local USB devices.
-.PP
-
-
-.SH EXAMPLES
-
-    client:# usbip list --remote=server
-        - List exportable usb devices on the server.
-
-    client:# usbip attach --remote=server --busid=1-2
-        - Connect the remote USB device.
-
-    client:# usbip detach --port=0
-        - Detach the usb device.
-
-.SH "SEE ALSO"
-\fBusbipd\fP\fB(8)\fB\fP
diff --git a/drivers/staging/usbip/userspace/doc/usbipd.8 b/drivers/staging/usbip/userspace/doc/usbipd.8
deleted file mode 100644
index ac4635db3f03..000000000000
--- a/drivers/staging/usbip/userspace/doc/usbipd.8
+++ /dev/null
@@ -1,91 +0,0 @@
-.TH USBIP "8" "February 2009" "usbip" "System Administration Utilities"
-.SH NAME
-usbipd \- USB/IP server daemon
-.SH SYNOPSIS
-.B usbipd
-[\fIoptions\fR]
-
-.SH DESCRIPTION
-.B usbipd
-provides USB/IP clients access to exported USB devices.
-
-Devices have to explicitly be exported using
-.B usbip bind
-before usbipd makes them available to other hosts.
-
-The daemon accepts connections from USB/IP clients
-on TCP port 3240 by default.
-
-.SH OPTIONS
-.HP
-\fB\-4\fR, \fB\-\-ipv4\fR
-.IP
-Bind to IPv4. Default is both.
-.PP
-
-.HP
-\fB\-6\fR, \fB\-\-ipv6\fR
-.IP
-Bind to IPv6. Default is both.
-.PP
-
-.HP
-\fB\-D\fR, \fB\-\-daemon\fR
-.IP
-Run as a daemon process.
-.PP
-
-.HP
-\fB\-d\fR, \fB\-\-debug\fR
-.IP
-Print debugging information.
-.PP
-
-.HP
-\fB\-PFILE\fR, \fB\-\-pid FILE\fR
-.IP
-Write process id to FILE.
-.br
-If no FILE specified, use /var/run/usbipd.pid
-.PP
-
-\fB\-tPORT\fR, \fB\-\-tcp\-port PORT\fR
-.IP
-Listen on TCP/IP port PORT.
-.PP
-
-\fB\-h\fR, \fB\-\-help\fR
-.IP
-Print the program help message and exit.
-.PP
-
-.HP
-\fB\-v\fR, \fB\-\-version\fR
-.IP
-Show version.
-.PP
-
-.SH LIMITATIONS
-
-.B usbipd
-offers no authentication or authorization for USB/IP. Any
-USB/IP client can connect and use exported devices.
-
-.SH EXAMPLES
-
-    server:# modprobe usbip
-
-    server:# usbipd -D
-        - Start usbip daemon.
-
-    server:# usbip list --local
-        - List driver assignments for usb devices.
-
-    server:# usbip bind --busid=1-2
-        - Bind usbip-host.ko to the device of busid 1-2.
-        - A usb device 1-2 is now exportable to other hosts!
-        - Use 'usbip unbind --busid=1-2' when you want to shutdown exporting and use the device locally.
-
-.SH "SEE ALSO"
-\fBusbip\fP\fB(8)\fB\fP
-
diff --git a/drivers/staging/usbip/userspace/libsrc/Makefile.am b/drivers/staging/usbip/userspace/libsrc/Makefile.am
deleted file mode 100644
index 7c8f8a4d54e4..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/Makefile.am
+++ /dev/null
@@ -1,8 +0,0 @@
-libusbip_la_CPPFLAGS = -DUSBIDS_FILE='"@USBIDS_DIR@/usb.ids"'
-libusbip_la_CFLAGS   = @EXTRA_CFLAGS@
-libusbip_la_LDFLAGS  = -version-info @LIBUSBIP_VERSION@
-
-lib_LTLIBRARIES := libusbip.la
-libusbip_la_SOURCES := names.c names.h usbip_host_driver.c usbip_host_driver.h \
-		       usbip_common.c usbip_common.h vhci_driver.c vhci_driver.h \
-		       sysfs_utils.c sysfs_utils.h
diff --git a/drivers/staging/usbip/userspace/libsrc/list.h b/drivers/staging/usbip/userspace/libsrc/list.h
deleted file mode 100644
index 8d0c936e184f..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/list.h
+++ /dev/null
@@ -1,136 +0,0 @@
-#ifndef _LIST_H
-#define _LIST_H
-
-/* Stripped down implementation of linked list taken
- * from the Linux Kernel.
- */
-
-/*
- * Simple doubly linked list implementation.
- *
- * Some of the internal functions ("__xxx") are useful when
- * manipulating whole lists rather than single entries, as
- * sometimes we already know the next/prev entries and we can
- * generate better code by using them directly rather than
- * using the generic single-entry routines.
- */
-
-struct list_head {
-	struct list_head *next, *prev;
-};
-
-#define LIST_HEAD_INIT(name) { &(name), &(name) }
-
-#define LIST_HEAD(name) \
-	struct list_head name = LIST_HEAD_INIT(name)
-
-static inline void INIT_LIST_HEAD(struct list_head *list)
-{
-	list->next = list;
-	list->prev = list;
-}
-
-/*
- * Insert a new entry between two known consecutive entries.
- *
- * This is only for internal list manipulation where we know
- * the prev/next entries already!
- */
-static inline void __list_add(struct list_head *new,
-			      struct list_head *prev,
-			      struct list_head *next)
-{
-	next->prev = new;
-	new->next = next;
-	new->prev = prev;
-	prev->next = new;
-}
-
-/**
- * list_add - add a new entry
- * @new: new entry to be added
- * @head: list head to add it after
- *
- * Insert a new entry after the specified head.
- * This is good for implementing stacks.
- */
-static inline void list_add(struct list_head *new, struct list_head *head)
-{
-	__list_add(new, head, head->next);
-}
-
-/*
- * Delete a list entry by making the prev/next entries
- * point to each other.
- *
- * This is only for internal list manipulation where we know
- * the prev/next entries already!
- */
-static inline void __list_del(struct list_head * prev, struct list_head * next)
-{
-	next->prev = prev;
-	prev->next = next;
-}
-
-#define POISON_POINTER_DELTA 0
-#define LIST_POISON1  ((void *) 0x00100100 + POISON_POINTER_DELTA)
-#define LIST_POISON2  ((void *) 0x00200200 + POISON_POINTER_DELTA)
-
-/**
- * list_del - deletes entry from list.
- * @entry: the element to delete from the list.
- * Note: list_empty() on entry does not return true after this, the entry is
- * in an undefined state.
- */
-static inline void __list_del_entry(struct list_head *entry)
-{
-	__list_del(entry->prev, entry->next);
-}
-
-static inline void list_del(struct list_head *entry)
-{
-	__list_del(entry->prev, entry->next);
-	entry->next = LIST_POISON1;
-	entry->prev = LIST_POISON2;
-}
-
-/**
- * list_entry - get the struct for this entry
- * @ptr:	the &struct list_head pointer.
- * @type:	the type of the struct this is embedded in.
- * @member:	the name of the list_struct within the struct.
- */
-#define list_entry(ptr, type, member) \
-	container_of(ptr, type, member)
-/**
- * list_for_each	-	iterate over a list
- * @pos:	the &struct list_head to use as a loop cursor.
- * @head:	the head for your list.
- */
-#define list_for_each(pos, head) \
-	for (pos = (head)->next; pos != (head); pos = pos->next)
-
-/**
- * list_for_each_safe - iterate over a list safe against removal of list entry
- * @pos:	the &struct list_head to use as a loop cursor.
- * @n:		another &struct list_head to use as temporary storage
- * @head:	the head for your list.
- */
-#define list_for_each_safe(pos, n, head) \
-	for (pos = (head)->next, n = pos->next; pos != (head); \
-		pos = n, n = pos->next)
-
-#define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
-
-/**
- * container_of - cast a member of a structure out to the containing structure
- * @ptr:	the pointer to the member.
- * @type:	the type of the container struct this is embedded in.
- * @member:	the name of the member within the struct.
- *
- */
-#define container_of(ptr, type, member) ({			\
-	const typeof( ((type *)0)->member ) *__mptr = (ptr);	\
-	(type *)( (char *)__mptr - offsetof(type,member) );})
-
-#endif
diff --git a/drivers/staging/usbip/userspace/libsrc/names.c b/drivers/staging/usbip/userspace/libsrc/names.c
deleted file mode 100644
index 81ff8522405c..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/names.c
+++ /dev/null
@@ -1,504 +0,0 @@
-/*
- *      names.c  --  USB name database manipulation routines
- *
- *      Copyright (C) 1999, 2000  Thomas Sailer (sailer@ife.ee.ethz.ch)
- *
- *      This program is free software; you can redistribute it and/or modify
- *      it under the terms of the GNU General Public License as published by
- *      the Free Software Foundation; either version 2 of the License, or
- *      (at your option) any later version.
- *
- *      This program is distributed in the hope that it will be useful,
- *      but WITHOUT ANY WARRANTY; without even the implied warranty of
- *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *      GNU General Public License for more details.
- *
- *      You should have received a copy of the GNU General Public License
- *      along with this program; if not, write to the Free Software
- *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *
- *
- *
- *	Copyright (C) 2005 Takahiro Hirofuchi
- *		- names_deinit() is added.
- *
- */
-
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <dirent.h>
-#include <string.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <unistd.h>
-#include <stdio.h>
-#include <ctype.h>
-
-#include "names.h"
-#include "usbip_common.h"
-
-struct vendor {
-	struct vendor *next;
-	u_int16_t vendorid;
-	char name[1];
-};
-
-struct product {
-	struct product *next;
-	u_int16_t vendorid, productid;
-	char name[1];
-};
-
-struct class {
-	struct class *next;
-	u_int8_t classid;
-	char name[1];
-};
-
-struct subclass {
-	struct subclass *next;
-	u_int8_t classid, subclassid;
-	char name[1];
-};
-
-struct protocol {
-	struct protocol *next;
-	u_int8_t classid, subclassid, protocolid;
-	char name[1];
-};
-
-struct genericstrtable {
-	struct genericstrtable *next;
-	unsigned int num;
-	char name[1];
-};
-
-
-#define HASH1  0x10
-#define HASH2  0x02
-#define HASHSZ 16
-
-static unsigned int hashnum(unsigned int num)
-{
-	unsigned int mask1 = HASH1 << 27, mask2 = HASH2 << 27;
-
-	for (; mask1 >= HASH1; mask1 >>= 1, mask2 >>= 1)
-		if (num & mask1)
-			num ^= mask2;
-	return num & (HASHSZ-1);
-}
-
-
-static struct vendor *vendors[HASHSZ] = { NULL, };
-static struct product *products[HASHSZ] = { NULL, };
-static struct class *classes[HASHSZ] = { NULL, };
-static struct subclass *subclasses[HASHSZ] = { NULL, };
-static struct protocol *protocols[HASHSZ] = { NULL, };
-
-const char *names_vendor(u_int16_t vendorid)
-{
-	struct vendor *v;
-
-	v = vendors[hashnum(vendorid)];
-	for (; v; v = v->next)
-		if (v->vendorid == vendorid)
-			return v->name;
-	return NULL;
-}
-
-const char *names_product(u_int16_t vendorid, u_int16_t productid)
-{
-	struct product *p;
-
-	p = products[hashnum((vendorid << 16) | productid)];
-	for (; p; p = p->next)
-		if (p->vendorid == vendorid && p->productid == productid)
-			return p->name;
-	return NULL;
-}
-
-const char *names_class(u_int8_t classid)
-{
-	struct class *c;
-
-	c = classes[hashnum(classid)];
-	for (; c; c = c->next)
-		if (c->classid == classid)
-			return c->name;
-	return NULL;
-}
-
-const char *names_subclass(u_int8_t classid, u_int8_t subclassid)
-{
-	struct subclass *s;
-
-	s = subclasses[hashnum((classid << 8) | subclassid)];
-	for (; s; s = s->next)
-		if (s->classid == classid && s->subclassid == subclassid)
-			return s->name;
-	return NULL;
-}
-
-const char *names_protocol(u_int8_t classid, u_int8_t subclassid,
-			   u_int8_t protocolid)
-{
-	struct protocol *p;
-
-	p = protocols[hashnum((classid << 16) | (subclassid << 8)
-			      | protocolid)];
-	for (; p; p = p->next)
-		if (p->classid == classid && p->subclassid == subclassid &&
-		    p->protocolid == protocolid)
-			return p->name;
-	return NULL;
-}
-
-/* add a cleanup function by takahiro */
-struct pool {
-	struct pool *next;
-	void *mem;
-};
-
-static struct pool *pool_head;
-
-static void *my_malloc(size_t size)
-{
-	struct pool *p;
-
-	p = calloc(1, sizeof(struct pool));
-	if (!p)
-		return NULL;
-
-	p->mem = calloc(1, size);
-	if (!p->mem) {
-		free(p);
-		return NULL;
-	}
-
-	p->next = pool_head;
-	pool_head = p;
-
-	return p->mem;
-}
-
-void names_free(void)
-{
-	struct pool *pool;
-
-	if (!pool_head)
-		return;
-
-	for (pool = pool_head; pool != NULL; ) {
-		struct pool *tmp;
-
-		if (pool->mem)
-			free(pool->mem);
-
-		tmp = pool;
-		pool = pool->next;
-		free(tmp);
-	}
-}
-
-static int new_vendor(const char *name, u_int16_t vendorid)
-{
-	struct vendor *v;
-	unsigned int h = hashnum(vendorid);
-
-	v = vendors[h];
-	for (; v; v = v->next)
-		if (v->vendorid == vendorid)
-			return -1;
-	v = my_malloc(sizeof(struct vendor) + strlen(name));
-	if (!v)
-		return -1;
-	strcpy(v->name, name);
-	v->vendorid = vendorid;
-	v->next = vendors[h];
-	vendors[h] = v;
-	return 0;
-}
-
-static int new_product(const char *name, u_int16_t vendorid,
-		       u_int16_t productid)
-{
-	struct product *p;
-	unsigned int h = hashnum((vendorid << 16) | productid);
-
-	p = products[h];
-	for (; p; p = p->next)
-		if (p->vendorid == vendorid && p->productid == productid)
-			return -1;
-	p = my_malloc(sizeof(struct product) + strlen(name));
-	if (!p)
-		return -1;
-	strcpy(p->name, name);
-	p->vendorid = vendorid;
-	p->productid = productid;
-	p->next = products[h];
-	products[h] = p;
-	return 0;
-}
-
-static int new_class(const char *name, u_int8_t classid)
-{
-	struct class *c;
-	unsigned int h = hashnum(classid);
-
-	c = classes[h];
-	for (; c; c = c->next)
-		if (c->classid == classid)
-			return -1;
-	c = my_malloc(sizeof(struct class) + strlen(name));
-	if (!c)
-		return -1;
-	strcpy(c->name, name);
-	c->classid = classid;
-	c->next = classes[h];
-	classes[h] = c;
-	return 0;
-}
-
-static int new_subclass(const char *name, u_int8_t classid, u_int8_t subclassid)
-{
-	struct subclass *s;
-	unsigned int h = hashnum((classid << 8) | subclassid);
-
-	s = subclasses[h];
-	for (; s; s = s->next)
-		if (s->classid == classid && s->subclassid == subclassid)
-			return -1;
-	s = my_malloc(sizeof(struct subclass) + strlen(name));
-	if (!s)
-		return -1;
-	strcpy(s->name, name);
-	s->classid = classid;
-	s->subclassid = subclassid;
-	s->next = subclasses[h];
-	subclasses[h] = s;
-	return 0;
-}
-
-static int new_protocol(const char *name, u_int8_t classid, u_int8_t subclassid,
-			u_int8_t protocolid)
-{
-	struct protocol *p;
-	unsigned int h = hashnum((classid << 16) | (subclassid << 8)
-				 | protocolid);
-
-	p = protocols[h];
-	for (; p; p = p->next)
-		if (p->classid == classid && p->subclassid == subclassid
-		    && p->protocolid == protocolid)
-			return -1;
-	p = my_malloc(sizeof(struct protocol) + strlen(name));
-	if (!p)
-		return -1;
-	strcpy(p->name, name);
-	p->classid = classid;
-	p->subclassid = subclassid;
-	p->protocolid = protocolid;
-	p->next = protocols[h];
-	protocols[h] = p;
-	return 0;
-}
-
-static void parse(FILE *f)
-{
-	char buf[512], *cp;
-	unsigned int linectr = 0;
-	int lastvendor = -1;
-	int lastclass = -1;
-	int lastsubclass = -1;
-	int lasthut = -1;
-	int lastlang = -1;
-	unsigned int u;
-
-	while (fgets(buf, sizeof(buf), f)) {
-		linectr++;
-		/* remove line ends */
-		cp = strchr(buf, '\r');
-		if (cp)
-			*cp = 0;
-		cp = strchr(buf, '\n');
-		if (cp)
-			*cp = 0;
-		if (buf[0] == '#' || !buf[0])
-			continue;
-		cp = buf;
-		if (buf[0] == 'P' && buf[1] == 'H' && buf[2] == 'Y' &&
-		    buf[3] == 'S' && buf[4] == 'D' &&
-		    buf[5] == 'E' && buf[6] == 'S' && /*isspace(buf[7])*/
-		    buf[7] == ' ') {
-			continue;
-		}
-		if (buf[0] == 'P' && buf[1] == 'H' &&
-		    buf[2] == 'Y' && /*isspace(buf[3])*/ buf[3] == ' ') {
-			continue;
-		}
-		if (buf[0] == 'B' && buf[1] == 'I' && buf[2] == 'A' &&
-		    buf[3] == 'S' && /*isspace(buf[4])*/ buf[4] == ' ') {
-			continue;
-		}
-		if (buf[0] == 'L' && /*isspace(buf[1])*/ buf[1] == ' ') {
-			lasthut = lastclass = lastvendor = lastsubclass = -1;
-			/*
-			 * set 1 as pseudo-id to indicate that the parser is
-			 * in a `L' section.
-			 */
-			lastlang = 1;
-			continue;
-		}
-		if (buf[0] == 'C' && /*isspace(buf[1])*/ buf[1] == ' ') {
-			/* class spec */
-			cp = buf+2;
-			while (isspace(*cp))
-				cp++;
-			if (!isxdigit(*cp)) {
-				err("Invalid class spec at line %u", linectr);
-				continue;
-			}
-			u = strtoul(cp, &cp, 16);
-			while (isspace(*cp))
-				cp++;
-			if (!*cp) {
-				err("Invalid class spec at line %u", linectr);
-				continue;
-			}
-			if (new_class(cp, u))
-				err("Duplicate class spec at line %u class %04x %s",
-				    linectr, u, cp);
-			dbg("line %5u class %02x %s", linectr, u, cp);
-			lasthut = lastlang = lastvendor = lastsubclass = -1;
-			lastclass = u;
-			continue;
-		}
-		if (buf[0] == 'A' && buf[1] == 'T' && isspace(buf[2])) {
-			/* audio terminal type spec */
-			continue;
-		}
-		if (buf[0] == 'H' && buf[1] == 'C' && buf[2] == 'C'
-		    && isspace(buf[3])) {
-			/* HID Descriptor bCountryCode */
-			continue;
-		}
-		if (isxdigit(*cp)) {
-			/* vendor */
-			u = strtoul(cp, &cp, 16);
-			while (isspace(*cp))
-				cp++;
-			if (!*cp) {
-				err("Invalid vendor spec at line %u", linectr);
-				continue;
-			}
-			if (new_vendor(cp, u))
-				err("Duplicate vendor spec at line %u vendor %04x %s",
-				    linectr, u, cp);
-			dbg("line %5u vendor %04x %s", linectr, u, cp);
-			lastvendor = u;
-			lasthut = lastlang = lastclass = lastsubclass = -1;
-			continue;
-		}
-		if (buf[0] == '\t' && isxdigit(buf[1])) {
-			/* product or subclass spec */
-			u = strtoul(buf+1, &cp, 16);
-			while (isspace(*cp))
-				cp++;
-			if (!*cp) {
-				err("Invalid product/subclass spec at line %u",
-				    linectr);
-				continue;
-			}
-			if (lastvendor != -1) {
-				if (new_product(cp, lastvendor, u))
-					err("Duplicate product spec at line %u product %04x:%04x %s",
-					    linectr, lastvendor, u, cp);
-				dbg("line %5u product %04x:%04x %s", linectr,
-				    lastvendor, u, cp);
-				continue;
-			}
-			if (lastclass != -1) {
-				if (new_subclass(cp, lastclass, u))
-					err("Duplicate subclass spec at line %u class %02x:%02x %s",
-					    linectr, lastclass, u, cp);
-				dbg("line %5u subclass %02x:%02x %s", linectr,
-				    lastclass, u, cp);
-				lastsubclass = u;
-				continue;
-			}
-			if (lasthut != -1) {
-				/* do not store hut */
-				continue;
-			}
-			if (lastlang != -1) {
-				/* do not store langid */
-				continue;
-			}
-			err("Product/Subclass spec without prior Vendor/Class spec at line %u",
-			    linectr);
-			continue;
-		}
-		if (buf[0] == '\t' && buf[1] == '\t' && isxdigit(buf[2])) {
-			/* protocol spec */
-			u = strtoul(buf+2, &cp, 16);
-			while (isspace(*cp))
-				cp++;
-			if (!*cp) {
-				err("Invalid protocol spec at line %u",
-				    linectr);
-				continue;
-			}
-			if (lastclass != -1 && lastsubclass != -1) {
-				if (new_protocol(cp, lastclass, lastsubclass,
-						 u))
-					err("Duplicate protocol spec at line %u class %02x:%02x:%02x %s",
-					    linectr, lastclass, lastsubclass,
-					    u, cp);
-				dbg("line %5u protocol %02x:%02x:%02x %s",
-				    linectr, lastclass, lastsubclass, u, cp);
-				continue;
-			}
-			err("Protocol spec without prior Class and Subclass spec at line %u",
-			    linectr);
-			continue;
-		}
-		if (buf[0] == 'H' && buf[1] == 'I' &&
-		    buf[2] == 'D' && /*isspace(buf[3])*/ buf[3] == ' ') {
-			continue;
-		}
-		if (buf[0] == 'H' && buf[1] == 'U' &&
-		    buf[2] == 'T' && /*isspace(buf[3])*/ buf[3] == ' ') {
-			lastlang = lastclass = lastvendor = lastsubclass = -1;
-			/*
-			 * set 1 as pseudo-id to indicate that the parser is
-			 * in a `HUT' section.
-			 */
-			lasthut = 1;
-			continue;
-		}
-		if (buf[0] == 'R' && buf[1] == ' ')
-			continue;
-
-		if (buf[0] == 'V' && buf[1] == 'T')
-			continue;
-
-		err("Unknown line at line %u", linectr);
-	}
-}
-
-
-int names_init(char *n)
-{
-	FILE *f;
-
-	f = fopen(n, "r");
-	if (!f)
-		return errno;
-
-	parse(f);
-	fclose(f);
-	return 0;
-}
diff --git a/drivers/staging/usbip/userspace/libsrc/names.h b/drivers/staging/usbip/userspace/libsrc/names.h
deleted file mode 100644
index 680926512de2..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/names.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- *      names.h  --  USB name database manipulation routines
- *
- *      Copyright (C) 1999, 2000  Thomas Sailer (sailer@ife.ee.ethz.ch)
- *
- *      This program is free software; you can redistribute it and/or modify
- *      it under the terms of the GNU General Public License as published by
- *      the Free Software Foundation; either version 2 of the License, or
- *      (at your option) any later version.
- *
- *      This program is distributed in the hope that it will be useful,
- *      but WITHOUT ANY WARRANTY; without even the implied warranty of
- *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- *      GNU General Public License for more details.
- *
- *      You should have received a copy of the GNU General Public License
- *      along with this program; if not, write to the Free Software
- *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
- *
- *
- *
- *	Copyright (C) 2005 Takahiro Hirofuchi
- *	       - names_free() is added.
- */
-
-#ifndef _NAMES_H
-#define _NAMES_H
-
-#include <sys/types.h>
-
-/* used by usbip_common.c */
-extern const char *names_vendor(u_int16_t vendorid);
-extern const char *names_product(u_int16_t vendorid, u_int16_t productid);
-extern const char *names_class(u_int8_t classid);
-extern const char *names_subclass(u_int8_t classid, u_int8_t subclassid);
-extern const char *names_protocol(u_int8_t classid, u_int8_t subclassid,
-				  u_int8_t protocolid);
-extern int  names_init(char *n);
-extern void names_free(void);
-
-#endif /* _NAMES_H */
diff --git a/drivers/staging/usbip/userspace/libsrc/sysfs_utils.c b/drivers/staging/usbip/userspace/libsrc/sysfs_utils.c
deleted file mode 100644
index 36ac88ece0b8..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/sysfs_utils.c
+++ /dev/null
@@ -1,31 +0,0 @@
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <errno.h>
-
-#include "sysfs_utils.h"
-#include "usbip_common.h"
-
-int write_sysfs_attribute(const char *attr_path, const char *new_value,
-			  size_t len)
-{
-	int fd;
-	int length;
-
-	fd = open(attr_path, O_WRONLY);
-	if (fd < 0) {
-		dbg("error opening attribute %s", attr_path);
-		return -1;
-	}
-
-	length = write(fd, new_value, len);
-	if (length < 0) {
-		dbg("error writing to attribute %s", attr_path);
-		close(fd);
-		return -1;
-	}
-
-	close(fd);
-
-	return 0;
-}
diff --git a/drivers/staging/usbip/userspace/libsrc/sysfs_utils.h b/drivers/staging/usbip/userspace/libsrc/sysfs_utils.h
deleted file mode 100644
index 32ac1d105d18..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/sysfs_utils.h
+++ /dev/null
@@ -1,8 +0,0 @@
-
-#ifndef __SYSFS_UTILS_H
-#define __SYSFS_UTILS_H
-
-int write_sysfs_attribute(const char *attr_path, const char *new_value,
-			  size_t len);
-
-#endif
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.c b/drivers/staging/usbip/userspace/libsrc/usbip_common.c
deleted file mode 100644
index ac73710473de..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.c
+++ /dev/null
@@ -1,285 +0,0 @@
-/*
- * Copyright (C) 2005-2007 Takahiro Hirofuchi
- */
-
-#include <libudev.h>
-#include "usbip_common.h"
-#include "names.h"
-
-#undef  PROGNAME
-#define PROGNAME "libusbip"
-
-int usbip_use_syslog;
-int usbip_use_stderr;
-int usbip_use_debug;
-
-extern struct udev *udev_context;
-
-struct speed_string {
-	int num;
-	char *speed;
-	char *desc;
-};
-
-static const struct speed_string speed_strings[] = {
-	{ USB_SPEED_UNKNOWN, "unknown", "Unknown Speed"},
-	{ USB_SPEED_LOW,  "1.5", "Low Speed(1.5Mbps)"  },
-	{ USB_SPEED_FULL, "12",  "Full Speed(12Mbps)" },
-	{ USB_SPEED_HIGH, "480", "High Speed(480Mbps)" },
-	{ USB_SPEED_WIRELESS, "53.3-480", "Wireless"},
-	{ USB_SPEED_SUPER, "5000", "Super Speed(5000Mbps)" },
-	{ 0, NULL, NULL }
-};
-
-struct portst_string {
-	int num;
-	char *desc;
-};
-
-static struct portst_string portst_strings[] = {
-	{ SDEV_ST_AVAILABLE,	"Device Available" },
-	{ SDEV_ST_USED,		"Device in Use" },
-	{ SDEV_ST_ERROR,	"Device Error"},
-	{ VDEV_ST_NULL,		"Port Available"},
-	{ VDEV_ST_NOTASSIGNED,	"Port Initializing"},
-	{ VDEV_ST_USED,		"Port in Use"},
-	{ VDEV_ST_ERROR,	"Port Error"},
-	{ 0, NULL}
-};
-
-const char *usbip_status_string(int32_t status)
-{
-	for (int i = 0; portst_strings[i].desc != NULL; i++)
-		if (portst_strings[i].num == status)
-			return portst_strings[i].desc;
-
-	return "Unknown Status";
-}
-
-const char *usbip_speed_string(int num)
-{
-	for (int i = 0; speed_strings[i].speed != NULL; i++)
-		if (speed_strings[i].num == num)
-			return speed_strings[i].desc;
-
-	return "Unknown Speed";
-}
-
-
-#define DBG_UDEV_INTEGER(name)\
-	dbg("%-20s = %x", to_string(name), (int) udev->name)
-
-#define DBG_UINF_INTEGER(name)\
-	dbg("%-20s = %x", to_string(name), (int) uinf->name)
-
-void dump_usb_interface(struct usbip_usb_interface *uinf)
-{
-	char buff[100];
-
-	usbip_names_get_class(buff, sizeof(buff),
-			uinf->bInterfaceClass,
-			uinf->bInterfaceSubClass,
-			uinf->bInterfaceProtocol);
-	dbg("%-20s = %s", "Interface(C/SC/P)", buff);
-}
-
-void dump_usb_device(struct usbip_usb_device *udev)
-{
-	char buff[100];
-
-	dbg("%-20s = %s", "path",  udev->path);
-	dbg("%-20s = %s", "busid", udev->busid);
-
-	usbip_names_get_class(buff, sizeof(buff),
-			udev->bDeviceClass,
-			udev->bDeviceSubClass,
-			udev->bDeviceProtocol);
-	dbg("%-20s = %s", "Device(C/SC/P)", buff);
-
-	DBG_UDEV_INTEGER(bcdDevice);
-
-	usbip_names_get_product(buff, sizeof(buff),
-			udev->idVendor,
-			udev->idProduct);
-	dbg("%-20s = %s", "Vendor/Product", buff);
-
-	DBG_UDEV_INTEGER(bNumConfigurations);
-	DBG_UDEV_INTEGER(bNumInterfaces);
-
-	dbg("%-20s = %s", "speed",
-			usbip_speed_string(udev->speed));
-
-	DBG_UDEV_INTEGER(busnum);
-	DBG_UDEV_INTEGER(devnum);
-}
-
-
-int read_attr_value(struct udev_device *dev, const char *name,
-		    const char *format)
-{
-	const char *attr;
-	int num = 0;
-	int ret;
-
-	attr = udev_device_get_sysattr_value(dev, name);
-	if (!attr) {
-		err("udev_device_get_sysattr_value failed");
-		goto err;
-	}
-
-	/* The client chooses the device configuration
-	 * when attaching it so right after being bound
-	 * to usbip-host on the server the device will
-	 * have no configuration.
-	 * Therefore, attributes such as bConfigurationValue
-	 * and bNumInterfaces will not exist and sscanf will
-	 * fail. Check for these cases and don't treat them
-	 * as errors.
-	 */
-
-	ret = sscanf(attr, format, &num);
-	if (ret < 1) {
-		if (strcmp(name, "bConfigurationValue") &&
-				strcmp(name, "bNumInterfaces")) {
-			err("sscanf failed for attribute %s", name);
-			goto err;
-		}
-	}
-
-err:
-
-	return num;
-}
-
-
-int read_attr_speed(struct udev_device *dev)
-{
-	const char *speed;
-
-	speed = udev_device_get_sysattr_value(dev, "speed");
-	if (!speed) {
-		err("udev_device_get_sysattr_value failed");
-		goto err;
-	}
-
-	for (int i = 0; speed_strings[i].speed != NULL; i++) {
-		if (!strcmp(speed, speed_strings[i].speed))
-			return speed_strings[i].num;
-	}
-
-err:
-
-	return USB_SPEED_UNKNOWN;
-}
-
-#define READ_ATTR(object, type, dev, name, format)			      \
-	do {								      \
-		(object)->name = (type) read_attr_value(dev, to_string(name), \
-							format);	      \
-	} while (0)
-
-
-int read_usb_device(struct udev_device *sdev, struct usbip_usb_device *udev)
-{
-	uint32_t busnum, devnum;
-	const char *path, *name;
-
-	READ_ATTR(udev, uint8_t,  sdev, bDeviceClass,		"%02x\n");
-	READ_ATTR(udev, uint8_t,  sdev, bDeviceSubClass,	"%02x\n");
-	READ_ATTR(udev, uint8_t,  sdev, bDeviceProtocol,	"%02x\n");
-
-	READ_ATTR(udev, uint16_t, sdev, idVendor,		"%04x\n");
-	READ_ATTR(udev, uint16_t, sdev, idProduct,		"%04x\n");
-	READ_ATTR(udev, uint16_t, sdev, bcdDevice,		"%04x\n");
-
-	READ_ATTR(udev, uint8_t,  sdev, bConfigurationValue,	"%02x\n");
-	READ_ATTR(udev, uint8_t,  sdev, bNumConfigurations,	"%02x\n");
-	READ_ATTR(udev, uint8_t,  sdev, bNumInterfaces,		"%02x\n");
-
-	READ_ATTR(udev, uint8_t,  sdev, devnum,			"%d\n");
-	udev->speed = read_attr_speed(sdev);
-
-	path = udev_device_get_syspath(sdev);
-	name = udev_device_get_sysname(sdev);
-
-	strncpy(udev->path,  path,  SYSFS_PATH_MAX);
-	strncpy(udev->busid, name, SYSFS_BUS_ID_SIZE);
-
-	sscanf(name, "%u-%u", &busnum, &devnum);
-	udev->busnum = busnum;
-
-	return 0;
-}
-
-int read_usb_interface(struct usbip_usb_device *udev, int i,
-		       struct usbip_usb_interface *uinf)
-{
-	char busid[SYSFS_BUS_ID_SIZE];
-	struct udev_device *sif;
-
-	sprintf(busid, "%s:%d.%d", udev->busid, udev->bConfigurationValue, i);
-
-	sif = udev_device_new_from_subsystem_sysname(udev_context, "usb", busid);
-	if (!sif) {
-		err("udev_device_new_from_subsystem_sysname %s failed", busid);
-		return -1;
-	}
-
-	READ_ATTR(uinf, uint8_t,  sif, bInterfaceClass,		"%02x\n");
-	READ_ATTR(uinf, uint8_t,  sif, bInterfaceSubClass,	"%02x\n");
-	READ_ATTR(uinf, uint8_t,  sif, bInterfaceProtocol,	"%02x\n");
-
-	return 0;
-}
-
-int usbip_names_init(char *f)
-{
-	return names_init(f);
-}
-
-void usbip_names_free(void)
-{
-	names_free();
-}
-
-void usbip_names_get_product(char *buff, size_t size, uint16_t vendor,
-			     uint16_t product)
-{
-	const char *prod, *vend;
-
-	prod = names_product(vendor, product);
-	if (!prod)
-		prod = "unknown product";
-
-
-	vend = names_vendor(vendor);
-	if (!vend)
-		vend = "unknown vendor";
-
-	snprintf(buff, size, "%s : %s (%04x:%04x)", vend, prod, vendor, product);
-}
-
-void usbip_names_get_class(char *buff, size_t size, uint8_t class,
-			   uint8_t subclass, uint8_t protocol)
-{
-	const char *c, *s, *p;
-
-	if (class == 0 && subclass == 0 && protocol == 0) {
-		snprintf(buff, size, "(Defined at Interface level) (%02x/%02x/%02x)", class, subclass, protocol);
-		return;
-	}
-
-	p = names_protocol(class, subclass, protocol);
-	if (!p)
-		p = "unknown protocol";
-
-	s = names_subclass(class, subclass);
-	if (!s)
-		s = "unknown subclass";
-
-	c = names_class(class);
-	if (!c)
-		c = "unknown class";
-
-	snprintf(buff, size, "%s / %s / %s (%02x/%02x/%02x)", c, s, p, class, subclass, protocol);
-}
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_common.h b/drivers/staging/usbip/userspace/libsrc/usbip_common.h
deleted file mode 100644
index 5a0e95edf4df..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/usbip_common.h
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- * Copyright (C) 2005-2007 Takahiro Hirofuchi
- */
-
-#ifndef __USBIP_COMMON_H
-#define __USBIP_COMMON_H
-
-#include <libudev.h>
-
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <syslog.h>
-#include <unistd.h>
-#include <linux/usb/ch9.h>
-#include "../../uapi/usbip.h"
-
-#ifndef USBIDS_FILE
-#define USBIDS_FILE "/usr/share/hwdata/usb.ids"
-#endif
-
-#ifndef VHCI_STATE_PATH
-#define VHCI_STATE_PATH "/var/run/vhci_hcd"
-#endif
-
-/* kernel module names */
-#define USBIP_CORE_MOD_NAME	"usbip-core"
-#define USBIP_HOST_DRV_NAME	"usbip-host"
-#define USBIP_VHCI_DRV_NAME	"vhci_hcd"
-
-/* sysfs constants */
-#define SYSFS_MNT_PATH         "/sys"
-#define SYSFS_BUS_NAME         "bus"
-#define SYSFS_BUS_TYPE         "usb"
-#define SYSFS_DRIVERS_NAME     "drivers"
-
-#define SYSFS_PATH_MAX		256
-#define SYSFS_BUS_ID_SIZE	32
-
-extern int usbip_use_syslog;
-extern int usbip_use_stderr;
-extern int usbip_use_debug ;
-
-#define PROGNAME "usbip"
-
-#define pr_fmt(fmt)	"%s: %s: " fmt "\n", PROGNAME
-#define dbg_fmt(fmt)	pr_fmt("%s:%d:[%s] " fmt), "debug",	\
-		        __FILE__, __LINE__, __func__
-
-#define err(fmt, args...)						\
-	do {								\
-		if (usbip_use_syslog) {					\
-			syslog(LOG_ERR, pr_fmt(fmt), "error", ##args);	\
-		}							\
-		if (usbip_use_stderr) {					\
-			fprintf(stderr, pr_fmt(fmt), "error", ##args);	\
-		}							\
-	} while (0)
-
-#define info(fmt, args...)						\
-	do {								\
-		if (usbip_use_syslog) {					\
-			syslog(LOG_INFO, pr_fmt(fmt), "info", ##args);	\
-		}							\
-		if (usbip_use_stderr) {					\
-			fprintf(stderr, pr_fmt(fmt), "info", ##args);	\
-		}							\
-	} while (0)
-
-#define dbg(fmt, args...)						\
-	do {								\
-	if (usbip_use_debug) {						\
-		if (usbip_use_syslog) {					\
-			syslog(LOG_DEBUG, dbg_fmt(fmt), ##args);	\
-		}							\
-		if (usbip_use_stderr) {					\
-			fprintf(stderr, dbg_fmt(fmt), ##args);		\
-		}							\
-	}								\
-	} while (0)
-
-#define BUG()						\
-	do {						\
-		err("sorry, it's a bug!");		\
-		abort();				\
-	} while (0)
-
-struct usbip_usb_interface {
-	uint8_t bInterfaceClass;
-	uint8_t bInterfaceSubClass;
-	uint8_t bInterfaceProtocol;
-	uint8_t padding;	/* alignment */
-} __attribute__((packed));
-
-struct usbip_usb_device {
-	char path[SYSFS_PATH_MAX];
-	char busid[SYSFS_BUS_ID_SIZE];
-
-	uint32_t busnum;
-	uint32_t devnum;
-	uint32_t speed;
-
-	uint16_t idVendor;
-	uint16_t idProduct;
-	uint16_t bcdDevice;
-
-	uint8_t bDeviceClass;
-	uint8_t bDeviceSubClass;
-	uint8_t bDeviceProtocol;
-	uint8_t bConfigurationValue;
-	uint8_t bNumConfigurations;
-	uint8_t bNumInterfaces;
-} __attribute__((packed));
-
-#define to_string(s)	#s
-
-void dump_usb_interface(struct usbip_usb_interface *);
-void dump_usb_device(struct usbip_usb_device *);
-int read_usb_device(struct udev_device *sdev, struct usbip_usb_device *udev);
-int read_attr_value(struct udev_device *dev, const char *name,
-		    const char *format);
-int read_usb_interface(struct usbip_usb_device *udev, int i,
-		       struct usbip_usb_interface *uinf);
-
-const char *usbip_speed_string(int num);
-const char *usbip_status_string(int32_t status);
-
-int usbip_names_init(char *);
-void usbip_names_free(void);
-void usbip_names_get_product(char *buff, size_t size, uint16_t vendor,
-			     uint16_t product);
-void usbip_names_get_class(char *buff, size_t size, uint8_t class,
-			   uint8_t subclass, uint8_t protocol);
-
-#endif /* __USBIP_COMMON_H */
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.c b/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.c
deleted file mode 100644
index bef08d5c44e8..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.c
+++ /dev/null
@@ -1,280 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-
-#include <errno.h>
-#include <unistd.h>
-
-#include <libudev.h>
-
-#include "usbip_common.h"
-#include "usbip_host_driver.h"
-#include "list.h"
-#include "sysfs_utils.h"
-
-#undef  PROGNAME
-#define PROGNAME "libusbip"
-
-struct usbip_host_driver *host_driver;
-struct udev *udev_context;
-
-static int32_t read_attr_usbip_status(struct usbip_usb_device *udev)
-{
-	char status_attr_path[SYSFS_PATH_MAX];
-	int fd;
-	int length;
-	char status;
-	int value = 0;
-
-	snprintf(status_attr_path, SYSFS_PATH_MAX, "%s/usbip_status",
-		 udev->path);
-
-	fd = open(status_attr_path, O_RDONLY);
-	if (fd < 0) {
-		err("error opening attribute %s", status_attr_path);
-		return -1;
-	}
-
-	length = read(fd, &status, 1);
-	if (length < 0) {
-		err("error reading attribute %s", status_attr_path);
-		close(fd);
-		return -1;
-	}
-
-	value = atoi(&status);
-
-	return value;
-}
-
-static
-struct usbip_exported_device *usbip_exported_device_new(const char *sdevpath)
-{
-	struct usbip_exported_device *edev = NULL;
-	struct usbip_exported_device *edev_old;
-	size_t size;
-	int i;
-
-	edev = calloc(1, sizeof(struct usbip_exported_device));
-
-	edev->sudev = udev_device_new_from_syspath(udev_context, sdevpath);
-	if (!edev->sudev) {
-		err("udev_device_new_from_syspath: %s", sdevpath);
-		goto err;
-	}
-
-	read_usb_device(edev->sudev, &edev->udev);
-
-	edev->status = read_attr_usbip_status(&edev->udev);
-	if (edev->status < 0)
-		goto err;
-
-	/* reallocate buffer to include usb interface data */
-	size = sizeof(struct usbip_exported_device) +
-		edev->udev.bNumInterfaces * sizeof(struct usbip_usb_interface);
-
-	edev_old = edev;
-	edev = realloc(edev, size);
-	if (!edev) {
-		edev = edev_old;
-		dbg("realloc failed");
-		goto err;
-	}
-
-	for (i = 0; i < edev->udev.bNumInterfaces; i++)
-		read_usb_interface(&edev->udev, i, &edev->uinf[i]);
-
-	return edev;
-err:
-	if (edev->sudev)
-		udev_device_unref(edev->sudev);
-	if (edev)
-		free(edev);
-
-	return NULL;
-}
-
-static int refresh_exported_devices(void)
-{
-	struct usbip_exported_device *edev;
-	struct udev_enumerate *enumerate;
-	struct udev_list_entry *devices, *dev_list_entry;
-	struct udev_device *dev;
-	const char *path;
-	const char *driver;
-
-	enumerate = udev_enumerate_new(udev_context);
-	udev_enumerate_add_match_subsystem(enumerate, "usb");
-	udev_enumerate_scan_devices(enumerate);
-
-	devices = udev_enumerate_get_list_entry(enumerate);
-
-	udev_list_entry_foreach(dev_list_entry, devices) {
-		path = udev_list_entry_get_name(dev_list_entry);
-		dev = udev_device_new_from_syspath(udev_context, path);
-		if (dev == NULL)
-			continue;
-
-		/* Check whether device uses usbip-host driver. */
-		driver = udev_device_get_driver(dev);
-		if (driver != NULL && !strcmp(driver, USBIP_HOST_DRV_NAME)) {
-			edev = usbip_exported_device_new(path);
-			if (!edev) {
-				dbg("usbip_exported_device_new failed");
-				continue;
-			}
-
-			list_add(&edev->node, &host_driver->edev_list);
-			host_driver->ndevs++;
-		}
-	}
-
-	return 0;
-}
-
-static void usbip_exported_device_destroy(void)
-{
-	struct list_head *i, *tmp;
-	struct usbip_exported_device *edev;
-
-	list_for_each_safe(i, tmp, &host_driver->edev_list) {
-		edev = list_entry(i, struct usbip_exported_device, node);
-		list_del(i);
-		free(edev);
-	}
-}
-
-int usbip_host_driver_open(void)
-{
-	int rc;
-
-	udev_context = udev_new();
-	if (!udev_context) {
-		err("udev_new failed");
-		return -1;
-	}
-
-	host_driver = calloc(1, sizeof(*host_driver));
-
-	host_driver->ndevs = 0;
-	INIT_LIST_HEAD(&host_driver->edev_list);
-
-	rc = refresh_exported_devices();
-	if (rc < 0)
-		goto err_free_host_driver;
-
-	return 0;
-
-err_free_host_driver:
-	free(host_driver);
-	host_driver = NULL;
-
-	udev_unref(udev_context);
-
-	return -1;
-}
-
-void usbip_host_driver_close(void)
-{
-	if (!host_driver)
-		return;
-
-	usbip_exported_device_destroy();
-
-	free(host_driver);
-	host_driver = NULL;
-
-	udev_unref(udev_context);
-}
-
-int usbip_host_refresh_device_list(void)
-{
-	int rc;
-
-	usbip_exported_device_destroy();
-
-	host_driver->ndevs = 0;
-	INIT_LIST_HEAD(&host_driver->edev_list);
-
-	rc = refresh_exported_devices();
-	if (rc < 0)
-		return -1;
-
-	return 0;
-}
-
-int usbip_host_export_device(struct usbip_exported_device *edev, int sockfd)
-{
-	char attr_name[] = "usbip_sockfd";
-	char sockfd_attr_path[SYSFS_PATH_MAX];
-	char sockfd_buff[30];
-	int ret;
-
-	if (edev->status != SDEV_ST_AVAILABLE) {
-		dbg("device not available: %s", edev->udev.busid);
-		switch (edev->status) {
-		case SDEV_ST_ERROR:
-			dbg("status SDEV_ST_ERROR");
-			break;
-		case SDEV_ST_USED:
-			dbg("status SDEV_ST_USED");
-			break;
-		default:
-			dbg("status unknown: 0x%x", edev->status);
-		}
-		return -1;
-	}
-
-	/* only the first interface is true */
-	snprintf(sockfd_attr_path, sizeof(sockfd_attr_path), "%s/%s",
-		 edev->udev.path, attr_name);
-
-	snprintf(sockfd_buff, sizeof(sockfd_buff), "%d\n", sockfd);
-
-	ret = write_sysfs_attribute(sockfd_attr_path, sockfd_buff,
-				    strlen(sockfd_buff));
-	if (ret < 0) {
-		err("write_sysfs_attribute failed: sockfd %s to %s",
-		    sockfd_buff, sockfd_attr_path);
-		return ret;
-	}
-
-	info("connect: %s", edev->udev.busid);
-
-	return ret;
-}
-
-struct usbip_exported_device *usbip_host_get_device(int num)
-{
-	struct list_head *i;
-	struct usbip_exported_device *edev;
-	int cnt = 0;
-
-	list_for_each(i, &host_driver->edev_list) {
-		edev = list_entry(i, struct usbip_exported_device, node);
-		if (num == cnt)
-			return edev;
-		else
-			cnt++;
-	}
-
-	return NULL;
-}
diff --git a/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.h b/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.h
deleted file mode 100644
index 2a31f855c616..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/usbip_host_driver.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __USBIP_HOST_DRIVER_H
-#define __USBIP_HOST_DRIVER_H
-
-#include <stdint.h>
-#include "usbip_common.h"
-#include "list.h"
-
-struct usbip_host_driver {
-	int ndevs;
-	/* list of exported device */
-	struct list_head edev_list;
-};
-
-struct usbip_exported_device {
-	struct udev_device *sudev;
-	int32_t status;
-	struct usbip_usb_device udev;
-	struct list_head node;
-	struct usbip_usb_interface uinf[];
-};
-
-extern struct usbip_host_driver *host_driver;
-
-int usbip_host_driver_open(void);
-void usbip_host_driver_close(void);
-
-int usbip_host_refresh_device_list(void);
-int usbip_host_export_device(struct usbip_exported_device *edev, int sockfd);
-struct usbip_exported_device *usbip_host_get_device(int num);
-
-#endif /* __USBIP_HOST_DRIVER_H */
diff --git a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c b/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
deleted file mode 100644
index ad9204773533..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/vhci_driver.c
+++ /dev/null
@@ -1,411 +0,0 @@
-/*
- * Copyright (C) 2005-2007 Takahiro Hirofuchi
- */
-
-#include "usbip_common.h"
-#include "vhci_driver.h"
-#include <limits.h>
-#include <netdb.h>
-#include <libudev.h>
-#include "sysfs_utils.h"
-
-#undef  PROGNAME
-#define PROGNAME "libusbip"
-
-struct usbip_vhci_driver *vhci_driver;
-struct udev *udev_context;
-
-static struct usbip_imported_device *
-imported_device_init(struct usbip_imported_device *idev, char *busid)
-{
-	struct udev_device *sudev;
-
-	sudev = udev_device_new_from_subsystem_sysname(udev_context,
-						       "usb", busid);
-	if (!sudev) {
-		dbg("udev_device_new_from_subsystem_sysname failed: %s", busid);
-		goto err;
-	}
-	read_usb_device(sudev, &idev->udev);
-	udev_device_unref(sudev);
-
-	return idev;
-
-err:
-	return NULL;
-}
-
-
-
-static int parse_status(const char *value)
-{
-	int ret = 0;
-	char *c;
-
-
-	for (int i = 0; i < vhci_driver->nports; i++)
-		memset(&vhci_driver->idev[i], 0, sizeof(vhci_driver->idev[i]));
-
-
-	/* skip a header line */
-	c = strchr(value, '\n');
-	if (!c)
-		return -1;
-	c++;
-
-	while (*c != '\0') {
-		int port, status, speed, devid;
-		unsigned long socket;
-		char lbusid[SYSFS_BUS_ID_SIZE];
-
-		ret = sscanf(c, "%d %d %d %x %lx %31s\n",
-				&port, &status, &speed,
-				&devid, &socket, lbusid);
-
-		if (ret < 5) {
-			dbg("sscanf failed: %d", ret);
-			BUG();
-		}
-
-		dbg("port %d status %d speed %d devid %x",
-				port, status, speed, devid);
-		dbg("socket %lx lbusid %s", socket, lbusid);
-
-
-		/* if a device is connected, look at it */
-		{
-			struct usbip_imported_device *idev = &vhci_driver->idev[port];
-
-			idev->port	= port;
-			idev->status	= status;
-
-			idev->devid	= devid;
-
-			idev->busnum	= (devid >> 16);
-			idev->devnum	= (devid & 0x0000ffff);
-
-			if (idev->status != VDEV_ST_NULL
-			    && idev->status != VDEV_ST_NOTASSIGNED) {
-				idev = imported_device_init(idev, lbusid);
-				if (!idev) {
-					dbg("imported_device_init failed");
-					return -1;
-				}
-			}
-		}
-
-
-		/* go to the next line */
-		c = strchr(c, '\n');
-		if (!c)
-			break;
-		c++;
-	}
-
-	dbg("exit");
-
-	return 0;
-}
-
-static int refresh_imported_device_list(void)
-{
-	const char *attr_status;
-
-	attr_status = udev_device_get_sysattr_value(vhci_driver->hc_device,
-					       "status");
-	if (!attr_status) {
-		err("udev_device_get_sysattr_value failed");
-		return -1;
-	}
-
-	return parse_status(attr_status);
-}
-
-static int get_nports(void)
-{
-	char *c;
-	int nports = 0;
-	const char *attr_status;
-
-	attr_status = udev_device_get_sysattr_value(vhci_driver->hc_device,
-					       "status");
-	if (!attr_status) {
-		err("udev_device_get_sysattr_value failed");
-		return -1;
-	}
-
-	/* skip a header line */
-	c = strchr(attr_status, '\n');
-	if (!c)
-		return 0;
-	c++;
-
-	while (*c != '\0') {
-		/* go to the next line */
-		c = strchr(c, '\n');
-		if (!c)
-			return nports;
-		c++;
-		nports += 1;
-	}
-
-	return nports;
-}
-
-/*
- * Read the given port's record.
- *
- * To avoid buffer overflow we will read the entire line and
- * validate each part's size. The initial buffer is padded by 4 to
- * accommodate the 2 spaces, 1 newline and an additional character
- * which is needed to properly validate the 3rd part without it being
- * truncated to an acceptable length.
- */
-static int read_record(int rhport, char *host, unsigned long host_len,
-		char *port, unsigned long port_len, char *busid)
-{
-	int part;
-	FILE *file;
-	char path[PATH_MAX+1];
-	char *buffer, *start, *end;
-	char delim[] = {' ', ' ', '\n'};
-	int max_len[] = {(int)host_len, (int)port_len, SYSFS_BUS_ID_SIZE};
-	size_t buffer_len = host_len + port_len + SYSFS_BUS_ID_SIZE + 4;
-
-	buffer = malloc(buffer_len);
-	if (!buffer)
-		return -1;
-
-	snprintf(path, PATH_MAX, VHCI_STATE_PATH"/port%d", rhport);
-
-	file = fopen(path, "r");
-	if (!file) {
-		err("fopen");
-		free(buffer);
-		return -1;
-	}
-
-	if (fgets(buffer, buffer_len, file) == NULL) {
-		err("fgets");
-		free(buffer);
-		fclose(file);
-		return -1;
-	}
-	fclose(file);
-
-	/* validate the length of each of the 3 parts */
-	start = buffer;
-	for (part = 0; part < 3; part++) {
-		end = strchr(start, delim[part]);
-		if (end == NULL || (end - start) > max_len[part]) {
-			free(buffer);
-			return -1;
-		}
-		start = end + 1;
-	}
-
-	if (sscanf(buffer, "%s %s %s\n", host, port, busid) != 3) {
-		err("sscanf");
-		free(buffer);
-		return -1;
-	}
-
-	free(buffer);
-
-	return 0;
-}
-
-/* ---------------------------------------------------------------------- */
-
-int usbip_vhci_driver_open(void)
-{
-	udev_context = udev_new();
-	if (!udev_context) {
-		err("udev_new failed");
-		return -1;
-	}
-
-	vhci_driver = calloc(1, sizeof(struct usbip_vhci_driver));
-
-	/* will be freed in usbip_driver_close() */
-	vhci_driver->hc_device =
-		udev_device_new_from_subsystem_sysname(udev_context,
-						       USBIP_VHCI_BUS_TYPE,
-						       USBIP_VHCI_DRV_NAME);
-	if (!vhci_driver->hc_device) {
-		err("udev_device_new_from_subsystem_sysname failed");
-		goto err;
-	}
-
-	vhci_driver->nports = get_nports();
-
-	dbg("available ports: %d", vhci_driver->nports);
-
-	if (refresh_imported_device_list())
-		goto err;
-
-	return 0;
-
-err:
-	udev_device_unref(vhci_driver->hc_device);
-
-	if (vhci_driver)
-		free(vhci_driver);
-
-	vhci_driver = NULL;
-
-	udev_unref(udev_context);
-
-	return -1;
-}
-
-
-void usbip_vhci_driver_close(void)
-{
-	if (!vhci_driver)
-		return;
-
-	udev_device_unref(vhci_driver->hc_device);
-
-	free(vhci_driver);
-
-	vhci_driver = NULL;
-
-	udev_unref(udev_context);
-}
-
-
-int usbip_vhci_refresh_device_list(void)
-{
-
-	if (refresh_imported_device_list())
-		goto err;
-
-	return 0;
-err:
-	dbg("failed to refresh device list");
-	return -1;
-}
-
-
-int usbip_vhci_get_free_port(void)
-{
-	for (int i = 0; i < vhci_driver->nports; i++) {
-		if (vhci_driver->idev[i].status == VDEV_ST_NULL)
-			return i;
-	}
-
-	return -1;
-}
-
-int usbip_vhci_attach_device2(uint8_t port, int sockfd, uint32_t devid,
-		uint32_t speed) {
-	char buff[200]; /* what size should be ? */
-	char attach_attr_path[SYSFS_PATH_MAX];
-	char attr_attach[] = "attach";
-	const char *path;
-	int ret;
-
-	snprintf(buff, sizeof(buff), "%u %d %u %u",
-			port, sockfd, devid, speed);
-	dbg("writing: %s", buff);
-
-	path = udev_device_get_syspath(vhci_driver->hc_device);
-	snprintf(attach_attr_path, sizeof(attach_attr_path), "%s/%s",
-		 path, attr_attach);
-	dbg("attach attribute path: %s", attach_attr_path);
-
-	ret = write_sysfs_attribute(attach_attr_path, buff, strlen(buff));
-	if (ret < 0) {
-		dbg("write_sysfs_attribute failed");
-		return -1;
-	}
-
-	dbg("attached port: %d", port);
-
-	return 0;
-}
-
-static unsigned long get_devid(uint8_t busnum, uint8_t devnum)
-{
-	return (busnum << 16) | devnum;
-}
-
-/* will be removed */
-int usbip_vhci_attach_device(uint8_t port, int sockfd, uint8_t busnum,
-		uint8_t devnum, uint32_t speed)
-{
-	int devid = get_devid(busnum, devnum);
-
-	return usbip_vhci_attach_device2(port, sockfd, devid, speed);
-}
-
-int usbip_vhci_detach_device(uint8_t port)
-{
-	char detach_attr_path[SYSFS_PATH_MAX];
-	char attr_detach[] = "detach";
-	char buff[200]; /* what size should be ? */
-	const char *path;
-	int ret;
-
-	snprintf(buff, sizeof(buff), "%u", port);
-	dbg("writing: %s", buff);
-
-	path = udev_device_get_syspath(vhci_driver->hc_device);
-	snprintf(detach_attr_path, sizeof(detach_attr_path), "%s/%s",
-		 path, attr_detach);
-	dbg("detach attribute path: %s", detach_attr_path);
-
-	ret = write_sysfs_attribute(detach_attr_path, buff, strlen(buff));
-	if (ret < 0) {
-		dbg("write_sysfs_attribute failed");
-		return -1;
-	}
-
-	dbg("detached port: %d", port);
-
-	return 0;
-}
-
-int usbip_vhci_imported_device_dump(struct usbip_imported_device *idev)
-{
-	char product_name[100];
-	char host[NI_MAXHOST] = "unknown host";
-	char serv[NI_MAXSERV] = "unknown port";
-	char remote_busid[SYSFS_BUS_ID_SIZE];
-	int ret;
-	int read_record_error = 0;
-
-	if (idev->status == VDEV_ST_NULL || idev->status == VDEV_ST_NOTASSIGNED)
-		return 0;
-
-	ret = read_record(idev->port, host, sizeof(host), serv, sizeof(serv),
-			  remote_busid);
-	if (ret) {
-		err("read_record");
-		read_record_error = 1;
-	}
-
-	printf("Port %02d: <%s> at %s\n", idev->port,
-	       usbip_status_string(idev->status),
-	       usbip_speed_string(idev->udev.speed));
-
-	usbip_names_get_product(product_name, sizeof(product_name),
-				idev->udev.idVendor, idev->udev.idProduct);
-
-	printf("       %s\n",  product_name);
-
-	if (!read_record_error) {
-		printf("%10s -> usbip://%s:%s/%s\n", idev->udev.busid,
-		       host, serv, remote_busid);
-		printf("%10s -> remote bus/dev %03d/%03d\n", " ",
-		       idev->busnum, idev->devnum);
-	} else {
-		printf("%10s -> unknown host, remote port and remote busid\n",
-		       idev->udev.busid);
-		printf("%10s -> remote bus/dev %03d/%03d\n", " ",
-		       idev->busnum, idev->devnum);
-	}
-
-	return 0;
-}
diff --git a/drivers/staging/usbip/userspace/libsrc/vhci_driver.h b/drivers/staging/usbip/userspace/libsrc/vhci_driver.h
deleted file mode 100644
index fa2316cf2cac..000000000000
--- a/drivers/staging/usbip/userspace/libsrc/vhci_driver.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*
- * Copyright (C) 2005-2007 Takahiro Hirofuchi
- */
-
-#ifndef __VHCI_DRIVER_H
-#define __VHCI_DRIVER_H
-
-#include <libudev.h>
-#include <stdint.h>
-
-#include "usbip_common.h"
-
-#define USBIP_VHCI_BUS_TYPE "platform"
-#define MAXNPORT 128
-
-struct usbip_imported_device {
-	uint8_t port;
-	uint32_t status;
-
-	uint32_t devid;
-
-	uint8_t busnum;
-	uint8_t devnum;
-
-	/* usbip_class_device list */
-	struct usbip_usb_device udev;
-};
-
-struct usbip_vhci_driver {
-
-	/* /sys/devices/platform/vhci_hcd */
-	struct udev_device *hc_device;
-
-	int nports;
-	struct usbip_imported_device idev[MAXNPORT];
-};
-
-
-extern struct usbip_vhci_driver *vhci_driver;
-
-int usbip_vhci_driver_open(void);
-void usbip_vhci_driver_close(void);
-
-int  usbip_vhci_refresh_device_list(void);
-
-
-int usbip_vhci_get_free_port(void);
-int usbip_vhci_attach_device2(uint8_t port, int sockfd, uint32_t devid,
-		uint32_t speed);
-
-/* will be removed */
-int usbip_vhci_attach_device(uint8_t port, int sockfd, uint8_t busnum,
-		uint8_t devnum, uint32_t speed);
-
-int usbip_vhci_detach_device(uint8_t port);
-
-int usbip_vhci_imported_device_dump(struct usbip_imported_device *idev);
-
-#endif /* __VHCI_DRIVER_H */
diff --git a/drivers/staging/usbip/userspace/src/Makefile.am b/drivers/staging/usbip/userspace/src/Makefile.am
deleted file mode 100644
index e81a4ebadeff..000000000000
--- a/drivers/staging/usbip/userspace/src/Makefile.am
+++ /dev/null
@@ -1,11 +0,0 @@
-AM_CPPFLAGS = -I$(top_srcdir)/libsrc -DUSBIDS_FILE='"@USBIDS_DIR@/usb.ids"'
-AM_CFLAGS   = @EXTRA_CFLAGS@
-LDADD       = $(top_builddir)/libsrc/libusbip.la
-
-sbin_PROGRAMS := usbip usbipd
-
-usbip_SOURCES := usbip.h utils.h usbip.c utils.c usbip_network.c \
-		 usbip_attach.c usbip_detach.c usbip_list.c \
-		 usbip_bind.c usbip_unbind.c usbip_port.c
-
-usbipd_SOURCES := usbip_network.h usbipd.c usbip_network.c
diff --git a/drivers/staging/usbip/userspace/src/usbip.c b/drivers/staging/usbip/userspace/src/usbip.c
deleted file mode 100644
index d7599d943529..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip.c
+++ /dev/null
@@ -1,201 +0,0 @@
-/*
- * command structure borrowed from udev
- * (git://git.kernel.org/pub/scm/linux/hotplug/udev.git)
- *
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-
-#include <getopt.h>
-#include <syslog.h>
-
-#include "usbip_common.h"
-#include "usbip_network.h"
-#include "usbip.h"
-
-static int usbip_help(int argc, char *argv[]);
-static int usbip_version(int argc, char *argv[]);
-
-static const char usbip_version_string[] = PACKAGE_STRING;
-
-static const char usbip_usage_string[] =
-	"usbip [--debug] [--log] [--tcp-port PORT] [version]\n"
-	"             [help] <command> <args>\n";
-
-static void usbip_usage(void)
-{
-	printf("usage: %s", usbip_usage_string);
-}
-
-struct command {
-	const char *name;
-	int (*fn)(int argc, char *argv[]);
-	const char *help;
-	void (*usage)(void);
-};
-
-static const struct command cmds[] = {
-	{
-		.name  = "help",
-		.fn    = usbip_help,
-		.help  = NULL,
-		.usage = NULL
-	},
-	{
-		.name  = "version",
-		.fn    = usbip_version,
-		.help  = NULL,
-		.usage = NULL
-	},
-	{
-		.name  = "attach",
-		.fn    = usbip_attach,
-		.help  = "Attach a remote USB device",
-		.usage = usbip_attach_usage
-	},
-	{
-		.name  = "detach",
-		.fn    = usbip_detach,
-		.help  = "Detach a remote USB device",
-		.usage = usbip_detach_usage
-	},
-	{
-		.name  = "list",
-		.fn    = usbip_list,
-		.help  = "List exportable or local USB devices",
-		.usage = usbip_list_usage
-	},
-	{
-		.name  = "bind",
-		.fn    = usbip_bind,
-		.help  = "Bind device to " USBIP_HOST_DRV_NAME ".ko",
-		.usage = usbip_bind_usage
-	},
-	{
-		.name  = "unbind",
-		.fn    = usbip_unbind,
-		.help  = "Unbind device from " USBIP_HOST_DRV_NAME ".ko",
-		.usage = usbip_unbind_usage
-	},
-	{
-		.name  = "port",
-		.fn    = usbip_port_show,
-		.help  = "Show imported USB devices",
-		.usage = NULL
-	},
-	{ NULL, NULL, NULL, NULL }
-};
-
-static int usbip_help(int argc, char *argv[])
-{
-	const struct command *cmd;
-	int i;
-	int ret = 0;
-
-	if (argc > 1 && argv++) {
-		for (i = 0; cmds[i].name != NULL; i++)
-			if (!strcmp(cmds[i].name, argv[0]) && cmds[i].usage) {
-				cmds[i].usage();
-				goto done;
-			}
-		ret = -1;
-	}
-
-	usbip_usage();
-	printf("\n");
-	for (cmd = cmds; cmd->name != NULL; cmd++)
-		if (cmd->help != NULL)
-			printf("  %-10s %s\n", cmd->name, cmd->help);
-	printf("\n");
-done:
-	return ret;
-}
-
-static int usbip_version(int argc, char *argv[])
-{
-	(void) argc;
-	(void) argv;
-
-	printf(PROGNAME " (%s)\n", usbip_version_string);
-	return 0;
-}
-
-static int run_command(const struct command *cmd, int argc, char *argv[])
-{
-	dbg("running command: `%s'", cmd->name);
-	return cmd->fn(argc, argv);
-}
-
-int main(int argc, char *argv[])
-{
-	static const struct option opts[] = {
-		{ "debug",    no_argument,       NULL, 'd' },
-		{ "log",      no_argument,       NULL, 'l' },
-		{ "tcp-port", required_argument, NULL, 't' },
-		{ NULL,       0,                 NULL,  0  }
-	};
-
-	char *cmd;
-	int opt;
-	int i, rc = -1;
-
-	usbip_use_stderr = 1;
-	opterr = 0;
-	for (;;) {
-		opt = getopt_long(argc, argv, "+dlt:", opts, NULL);
-
-		if (opt == -1)
-			break;
-
-		switch (opt) {
-		case 'd':
-			usbip_use_debug = 1;
-			break;
-		case 'l':
-			usbip_use_syslog = 1;
-			openlog("", LOG_PID, LOG_USER);
-			break;
-		case 't':
-			usbip_setup_port_number(optarg);
-			break;
-		case '?':
-			printf("usbip: invalid option\n");
-		default:
-			usbip_usage();
-			goto out;
-		}
-	}
-
-	cmd = argv[optind];
-	if (cmd) {
-		for (i = 0; cmds[i].name != NULL; i++)
-			if (!strcmp(cmds[i].name, cmd)) {
-				argc -= optind;
-				argv += optind;
-				optind = 0;
-				rc = run_command(&cmds[i], argc, argv);
-				goto out;
-			}
-	}
-
-	/* invalid command */
-	usbip_help(0, NULL);
-out:
-	return (rc > -1 ? EXIT_SUCCESS : EXIT_FAILURE);
-}
diff --git a/drivers/staging/usbip/userspace/src/usbip.h b/drivers/staging/usbip/userspace/src/usbip.h
deleted file mode 100644
index 84fe66a9d8ad..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip.h
+++ /dev/null
@@ -1,40 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __USBIP_H
-#define __USBIP_H
-
-#ifdef HAVE_CONFIG_H
-#include "../config.h"
-#endif
-
-/* usbip commands */
-int usbip_attach(int argc, char *argv[]);
-int usbip_detach(int argc, char *argv[]);
-int usbip_list(int argc, char *argv[]);
-int usbip_bind(int argc, char *argv[]);
-int usbip_unbind(int argc, char *argv[]);
-int usbip_port_show(int argc, char *argv[]);
-
-void usbip_attach_usage(void);
-void usbip_detach_usage(void);
-void usbip_list_usage(void);
-void usbip_bind_usage(void);
-void usbip_unbind_usage(void);
-
-#endif /* __USBIP_H */
diff --git a/drivers/staging/usbip/userspace/src/usbip_attach.c b/drivers/staging/usbip/userspace/src/usbip_attach.c
deleted file mode 100644
index d58a14dfc094..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_attach.c
+++ /dev/null
@@ -1,241 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <sys/stat.h>
-
-#include <limits.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <string.h>
-
-#include <fcntl.h>
-#include <getopt.h>
-#include <unistd.h>
-#include <errno.h>
-
-#include "vhci_driver.h"
-#include "usbip_common.h"
-#include "usbip_network.h"
-#include "usbip.h"
-
-static const char usbip_attach_usage_string[] =
-	"usbip attach <args>\n"
-	"    -r, --remote=<host>      The machine with exported USB devices\n"
-	"    -b, --busid=<busid>    Busid of the device on <host>\n";
-
-void usbip_attach_usage(void)
-{
-	printf("usage: %s", usbip_attach_usage_string);
-}
-
-#define MAX_BUFF 100
-static int record_connection(char *host, char *port, char *busid, int rhport)
-{
-	int fd;
-	char path[PATH_MAX+1];
-	char buff[MAX_BUFF+1];
-	int ret;
-
-	ret = mkdir(VHCI_STATE_PATH, 0700);
-	if (ret < 0) {
-		/* if VHCI_STATE_PATH exists, then it better be a directory */
-		if (errno == EEXIST) {
-			struct stat s;
-
-			ret = stat(VHCI_STATE_PATH, &s);
-			if (ret < 0)
-				return -1;
-			if (!(s.st_mode & S_IFDIR))
-				return -1;
-		} else
-			return -1;
-	}
-
-	snprintf(path, PATH_MAX, VHCI_STATE_PATH"/port%d", rhport);
-
-	fd = open(path, O_WRONLY|O_CREAT|O_TRUNC, S_IRWXU);
-	if (fd < 0)
-		return -1;
-
-	snprintf(buff, MAX_BUFF, "%s %s %s\n",
-			host, port, busid);
-
-	ret = write(fd, buff, strlen(buff));
-	if (ret != (ssize_t) strlen(buff)) {
-		close(fd);
-		return -1;
-	}
-
-	close(fd);
-
-	return 0;
-}
-
-static int import_device(int sockfd, struct usbip_usb_device *udev)
-{
-	int rc;
-	int port;
-
-	rc = usbip_vhci_driver_open();
-	if (rc < 0) {
-		err("open vhci_driver");
-		return -1;
-	}
-
-	port = usbip_vhci_get_free_port();
-	if (port < 0) {
-		err("no free port");
-		usbip_vhci_driver_close();
-		return -1;
-	}
-
-	rc = usbip_vhci_attach_device(port, sockfd, udev->busnum,
-				      udev->devnum, udev->speed);
-	if (rc < 0) {
-		err("import device");
-		usbip_vhci_driver_close();
-		return -1;
-	}
-
-	usbip_vhci_driver_close();
-
-	return port;
-}
-
-static int query_import_device(int sockfd, char *busid)
-{
-	int rc;
-	struct op_import_request request;
-	struct op_import_reply   reply;
-	uint16_t code = OP_REP_IMPORT;
-
-	memset(&request, 0, sizeof(request));
-	memset(&reply, 0, sizeof(reply));
-
-	/* send a request */
-	rc = usbip_net_send_op_common(sockfd, OP_REQ_IMPORT, 0);
-	if (rc < 0) {
-		err("send op_common");
-		return -1;
-	}
-
-	strncpy(request.busid, busid, SYSFS_BUS_ID_SIZE-1);
-
-	PACK_OP_IMPORT_REQUEST(0, &request);
-
-	rc = usbip_net_send(sockfd, (void *) &request, sizeof(request));
-	if (rc < 0) {
-		err("send op_import_request");
-		return -1;
-	}
-
-	/* receive a reply */
-	rc = usbip_net_recv_op_common(sockfd, &code);
-	if (rc < 0) {
-		err("recv op_common");
-		return -1;
-	}
-
-	rc = usbip_net_recv(sockfd, (void *) &reply, sizeof(reply));
-	if (rc < 0) {
-		err("recv op_import_reply");
-		return -1;
-	}
-
-	PACK_OP_IMPORT_REPLY(0, &reply);
-
-	/* check the reply */
-	if (strncmp(reply.udev.busid, busid, SYSFS_BUS_ID_SIZE)) {
-		err("recv different busid %s", reply.udev.busid);
-		return -1;
-	}
-
-	/* import a device */
-	return import_device(sockfd, &reply.udev);
-}
-
-static int attach_device(char *host, char *busid)
-{
-	int sockfd;
-	int rc;
-	int rhport;
-
-	sockfd = usbip_net_tcp_connect(host, usbip_port_string);
-	if (sockfd < 0) {
-		err("tcp connect");
-		return -1;
-	}
-
-	rhport = query_import_device(sockfd, busid);
-	if (rhport < 0) {
-		err("query");
-		return -1;
-	}
-
-	close(sockfd);
-
-	rc = record_connection(host, usbip_port_string, busid, rhport);
-	if (rc < 0) {
-		err("record connection");
-		return -1;
-	}
-
-	return 0;
-}
-
-int usbip_attach(int argc, char *argv[])
-{
-	static const struct option opts[] = {
-		{ "remote", required_argument, NULL, 'r' },
-		{ "busid",  required_argument, NULL, 'b' },
-		{ NULL, 0,  NULL, 0 }
-	};
-	char *host = NULL;
-	char *busid = NULL;
-	int opt;
-	int ret = -1;
-
-	for (;;) {
-		opt = getopt_long(argc, argv, "r:b:", opts, NULL);
-
-		if (opt == -1)
-			break;
-
-		switch (opt) {
-		case 'r':
-			host = optarg;
-			break;
-		case 'b':
-			busid = optarg;
-			break;
-		default:
-			goto err_out;
-		}
-	}
-
-	if (!host || !busid)
-		goto err_out;
-
-	ret = attach_device(host, busid);
-	goto out;
-
-err_out:
-	usbip_attach_usage();
-out:
-	return ret;
-}
diff --git a/drivers/staging/usbip/userspace/src/usbip_bind.c b/drivers/staging/usbip/userspace/src/usbip_bind.c
deleted file mode 100644
index fa46141ae68b..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_bind.c
+++ /dev/null
@@ -1,214 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <libudev.h>
-
-#include <errno.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <getopt.h>
-
-#include "usbip_common.h"
-#include "utils.h"
-#include "usbip.h"
-#include "sysfs_utils.h"
-
-enum unbind_status {
-	UNBIND_ST_OK,
-	UNBIND_ST_USBIP_HOST,
-	UNBIND_ST_FAILED
-};
-
-static const char usbip_bind_usage_string[] =
-	"usbip bind <args>\n"
-	"    -b, --busid=<busid>    Bind " USBIP_HOST_DRV_NAME ".ko to device "
-	"on <busid>\n";
-
-void usbip_bind_usage(void)
-{
-	printf("usage: %s", usbip_bind_usage_string);
-}
-
-/* call at unbound state */
-static int bind_usbip(char *busid)
-{
-	char attr_name[] = "bind";
-	char bind_attr_path[SYSFS_PATH_MAX];
-	int rc = -1;
-
-	snprintf(bind_attr_path, sizeof(bind_attr_path), "%s/%s/%s/%s/%s/%s",
-		 SYSFS_MNT_PATH, SYSFS_BUS_NAME, SYSFS_BUS_TYPE,
-		 SYSFS_DRIVERS_NAME, USBIP_HOST_DRV_NAME, attr_name);
-
-	rc = write_sysfs_attribute(bind_attr_path, busid, strlen(busid));
-	if (rc < 0) {
-		err("error binding device %s to driver: %s", busid,
-		    strerror(errno));
-		return -1;
-	}
-
-	return 0;
-}
-
-/* buggy driver may cause dead lock */
-static int unbind_other(char *busid)
-{
-	enum unbind_status status = UNBIND_ST_OK;
-
-	char attr_name[] = "unbind";
-	char unbind_attr_path[SYSFS_PATH_MAX];
-	int rc = -1;
-
-	struct udev *udev;
-	struct udev_device *dev;
-	const char *driver;
-	const char *bDevClass;
-
-	/* Create libudev context. */
-	udev = udev_new();
-
-	/* Get the device. */
-	dev = udev_device_new_from_subsystem_sysname(udev, "usb", busid);
-	if (!dev) {
-		dbg("unable to find device with bus ID %s", busid);
-		goto err_close_busid_dev;
-	}
-
-	/* Check what kind of device it is. */
-	bDevClass  = udev_device_get_sysattr_value(dev, "bDeviceClass");
-	if (!bDevClass) {
-		dbg("unable to get bDevClass device attribute");
-		goto err_close_busid_dev;
-	}
-
-	if (!strncmp(bDevClass, "09", strlen(bDevClass))) {
-		dbg("skip unbinding of hub");
-		goto err_close_busid_dev;
-	}
-
-	/* Get the device driver. */
-	driver = udev_device_get_driver(dev);
-	if (!driver) {
-		/* No driver bound to this device. */
-		goto out;
-	}
-
-	if (!strncmp(USBIP_HOST_DRV_NAME, driver,
-				strlen(USBIP_HOST_DRV_NAME))) {
-		/* Already bound to usbip-host. */
-		status = UNBIND_ST_USBIP_HOST;
-		goto out;
-	}
-
-	/* Unbind device from driver. */
-	snprintf(unbind_attr_path, sizeof(unbind_attr_path), "%s/%s/%s/%s/%s/%s",
-		 SYSFS_MNT_PATH, SYSFS_BUS_NAME, SYSFS_BUS_TYPE,
-		 SYSFS_DRIVERS_NAME, driver, attr_name);
-
-	rc = write_sysfs_attribute(unbind_attr_path, busid, strlen(busid));
-	if (rc < 0) {
-		err("error unbinding device %s from driver", busid);
-		goto err_close_busid_dev;
-	}
-
-	goto out;
-
-err_close_busid_dev:
-	status = UNBIND_ST_FAILED;
-out:
-	udev_device_unref(dev);
-	udev_unref(udev);
-
-	return status;
-}
-
-static int bind_device(char *busid)
-{
-	int rc;
-	struct udev *udev;
-	struct udev_device *dev;
-
-	/* Check whether the device with this bus ID exists. */
-	udev = udev_new();
-	dev = udev_device_new_from_subsystem_sysname(udev, "usb", busid);
-	if (!dev) {
-		err("device with the specified bus ID does not exist");
-		return -1;
-	}
-	udev_unref(udev);
-
-	rc = unbind_other(busid);
-	if (rc == UNBIND_ST_FAILED) {
-		err("could not unbind driver from device on busid %s", busid);
-		return -1;
-	} else if (rc == UNBIND_ST_USBIP_HOST) {
-		err("device on busid %s is already bound to %s", busid,
-		    USBIP_HOST_DRV_NAME);
-		return -1;
-	}
-
-	rc = modify_match_busid(busid, 1);
-	if (rc < 0) {
-		err("unable to bind device on %s", busid);
-		return -1;
-	}
-
-	rc = bind_usbip(busid);
-	if (rc < 0) {
-		err("could not bind device to %s", USBIP_HOST_DRV_NAME);
-		modify_match_busid(busid, 0);
-		return -1;
-	}
-
-	info("bind device on busid %s: complete", busid);
-
-	return 0;
-}
-
-int usbip_bind(int argc, char *argv[])
-{
-	static const struct option opts[] = {
-		{ "busid", required_argument, NULL, 'b' },
-		{ NULL,    0,                 NULL,  0  }
-	};
-
-	int opt;
-	int ret = -1;
-
-	for (;;) {
-		opt = getopt_long(argc, argv, "b:", opts, NULL);
-
-		if (opt == -1)
-			break;
-
-		switch (opt) {
-		case 'b':
-			ret = bind_device(optarg);
-			goto out;
-		default:
-			goto err_out;
-		}
-	}
-
-err_out:
-	usbip_bind_usage();
-out:
-	return ret;
-}
diff --git a/drivers/staging/usbip/userspace/src/usbip_detach.c b/drivers/staging/usbip/userspace/src/usbip_detach.c
deleted file mode 100644
index 05c6d15856eb..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_detach.c
+++ /dev/null
@@ -1,110 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <ctype.h>
-#include <limits.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <getopt.h>
-#include <unistd.h>
-
-#include "vhci_driver.h"
-#include "usbip_common.h"
-#include "usbip_network.h"
-#include "usbip.h"
-
-static const char usbip_detach_usage_string[] =
-	"usbip detach <args>\n"
-	"    -p, --port=<port>    " USBIP_VHCI_DRV_NAME
-	" port the device is on\n";
-
-void usbip_detach_usage(void)
-{
-	printf("usage: %s", usbip_detach_usage_string);
-}
-
-static int detach_port(char *port)
-{
-	int ret;
-	uint8_t portnum;
-	char path[PATH_MAX+1];
-
-	for (unsigned int i = 0; i < strlen(port); i++)
-		if (!isdigit(port[i])) {
-			err("invalid port %s", port);
-			return -1;
-		}
-
-	/* check max port */
-
-	portnum = atoi(port);
-
-	/* remove the port state file */
-
-	snprintf(path, PATH_MAX, VHCI_STATE_PATH"/port%d", portnum);
-
-	remove(path);
-	rmdir(VHCI_STATE_PATH);
-
-	ret = usbip_vhci_driver_open();
-	if (ret < 0) {
-		err("open vhci_driver");
-		return -1;
-	}
-
-	ret = usbip_vhci_detach_device(portnum);
-	if (ret < 0)
-		return -1;
-
-	usbip_vhci_driver_close();
-
-	return ret;
-}
-
-int usbip_detach(int argc, char *argv[])
-{
-	static const struct option opts[] = {
-		{ "port", required_argument, NULL, 'p' },
-		{ NULL, 0, NULL, 0 }
-	};
-	int opt;
-	int ret = -1;
-
-	for (;;) {
-		opt = getopt_long(argc, argv, "p:", opts, NULL);
-
-		if (opt == -1)
-			break;
-
-		switch (opt) {
-		case 'p':
-			ret = detach_port(optarg);
-			goto out;
-		default:
-			goto err_out;
-		}
-	}
-
-err_out:
-	usbip_detach_usage();
-out:
-	return ret;
-}
diff --git a/drivers/staging/usbip/userspace/src/usbip_list.c b/drivers/staging/usbip/userspace/src/usbip_list.c
deleted file mode 100644
index d5ce34a410e7..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_list.c
+++ /dev/null
@@ -1,283 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <sys/types.h>
-#include <libudev.h>
-
-#include <errno.h>
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include <getopt.h>
-#include <netdb.h>
-#include <unistd.h>
-
-#include "usbip_common.h"
-#include "usbip_network.h"
-#include "usbip.h"
-
-static const char usbip_list_usage_string[] =
-	"usbip list [-p|--parsable] <args>\n"
-	"    -p, --parsable         Parsable list format\n"
-	"    -r, --remote=<host>    List the exportable USB devices on <host>\n"
-	"    -l, --local            List the local USB devices\n";
-
-void usbip_list_usage(void)
-{
-	printf("usage: %s", usbip_list_usage_string);
-}
-
-static int get_exported_devices(char *host, int sockfd)
-{
-	char product_name[100];
-	char class_name[100];
-	struct op_devlist_reply reply;
-	uint16_t code = OP_REP_DEVLIST;
-	struct usbip_usb_device udev;
-	struct usbip_usb_interface uintf;
-	unsigned int i;
-	int rc, j;
-
-	rc = usbip_net_send_op_common(sockfd, OP_REQ_DEVLIST, 0);
-	if (rc < 0) {
-		dbg("usbip_net_send_op_common failed");
-		return -1;
-	}
-
-	rc = usbip_net_recv_op_common(sockfd, &code);
-	if (rc < 0) {
-		dbg("usbip_net_recv_op_common failed");
-		return -1;
-	}
-
-	memset(&reply, 0, sizeof(reply));
-	rc = usbip_net_recv(sockfd, &reply, sizeof(reply));
-	if (rc < 0) {
-		dbg("usbip_net_recv_op_devlist failed");
-		return -1;
-	}
-	PACK_OP_DEVLIST_REPLY(0, &reply);
-	dbg("exportable devices: %d\n", reply.ndev);
-
-	if (reply.ndev == 0) {
-		info("no exportable devices found on %s", host);
-		return 0;
-	}
-
-	printf("Exportable USB devices\n");
-	printf("======================\n");
-	printf(" - %s\n", host);
-
-	for (i = 0; i < reply.ndev; i++) {
-		memset(&udev, 0, sizeof(udev));
-		rc = usbip_net_recv(sockfd, &udev, sizeof(udev));
-		if (rc < 0) {
-			dbg("usbip_net_recv failed: usbip_usb_device[%d]", i);
-			return -1;
-		}
-		usbip_net_pack_usb_device(0, &udev);
-
-		usbip_names_get_product(product_name, sizeof(product_name),
-					udev.idVendor, udev.idProduct);
-		usbip_names_get_class(class_name, sizeof(class_name),
-				      udev.bDeviceClass, udev.bDeviceSubClass,
-				      udev.bDeviceProtocol);
-		printf("%11s: %s\n", udev.busid, product_name);
-		printf("%11s: %s\n", "", udev.path);
-		printf("%11s: %s\n", "", class_name);
-
-		for (j = 0; j < udev.bNumInterfaces; j++) {
-			rc = usbip_net_recv(sockfd, &uintf, sizeof(uintf));
-			if (rc < 0) {
-				err("usbip_net_recv failed: usbip_usb_intf[%d]",
-						j);
-
-				return -1;
-			}
-			usbip_net_pack_usb_interface(0, &uintf);
-
-			usbip_names_get_class(class_name, sizeof(class_name),
-					uintf.bInterfaceClass,
-					uintf.bInterfaceSubClass,
-					uintf.bInterfaceProtocol);
-			printf("%11s: %2d - %s\n", "", j, class_name);
-		}
-
-		printf("\n");
-	}
-
-	return 0;
-}
-
-static int list_exported_devices(char *host)
-{
-	int rc;
-	int sockfd;
-
-	sockfd = usbip_net_tcp_connect(host, usbip_port_string);
-	if (sockfd < 0) {
-		err("could not connect to %s:%s: %s", host,
-		    usbip_port_string, gai_strerror(sockfd));
-		return -1;
-	}
-	dbg("connected to %s:%s", host, usbip_port_string);
-
-	rc = get_exported_devices(host, sockfd);
-	if (rc < 0) {
-		err("failed to get device list from %s", host);
-		return -1;
-	}
-
-	close(sockfd);
-
-	return 0;
-}
-
-static void print_device(const char *busid, const char *vendor,
-			 const char *product, bool parsable)
-{
-	if (parsable)
-		printf("busid=%s#usbid=%.4s:%.4s#", busid, vendor, product);
-	else
-		printf(" - busid %s (%.4s:%.4s)\n", busid, vendor, product);
-}
-
-static void print_product_name(char *product_name, bool parsable)
-{
-	if (!parsable)
-		printf("   %s\n", product_name);
-}
-
-static int list_devices(bool parsable)
-{
-	struct udev *udev;
-	struct udev_enumerate *enumerate;
-	struct udev_list_entry *devices, *dev_list_entry;
-	struct udev_device *dev;
-	const char *path;
-	const char *idVendor;
-	const char *idProduct;
-	const char *bConfValue;
-	const char *bNumIntfs;
-	const char *busid;
-	char product_name[128];
-	int ret = -1;
-
-	/* Create libudev context. */
-	udev = udev_new();
-
-	/* Create libudev device enumeration. */
-	enumerate = udev_enumerate_new(udev);
-
-	/* Take only USB devices that are not hubs and do not have
-	 * the bInterfaceNumber attribute, i.e. are not interfaces.
-	 */
-	udev_enumerate_add_match_subsystem(enumerate, "usb");
-	udev_enumerate_add_nomatch_sysattr(enumerate, "bDeviceClass", "09");
-	udev_enumerate_add_nomatch_sysattr(enumerate, "bInterfaceNumber", NULL);
-	udev_enumerate_scan_devices(enumerate);
-
-	devices = udev_enumerate_get_list_entry(enumerate);
-
-	/* Show information about each device. */
-	udev_list_entry_foreach(dev_list_entry, devices) {
-		path = udev_list_entry_get_name(dev_list_entry);
-		dev = udev_device_new_from_syspath(udev, path);
-
-		/* Get device information. */
-		idVendor = udev_device_get_sysattr_value(dev, "idVendor");
-		idProduct = udev_device_get_sysattr_value(dev, "idProduct");
-		bConfValue = udev_device_get_sysattr_value(dev, "bConfigurationValue");
-		bNumIntfs = udev_device_get_sysattr_value(dev, "bNumInterfaces");
-		busid = udev_device_get_sysname(dev);
-		if (!idVendor || !idProduct || !bConfValue || !bNumIntfs) {
-			err("problem getting device attributes: %s",
-			    strerror(errno));
-			goto err_out;
-		}
-
-		/* Get product name. */
-		usbip_names_get_product(product_name, sizeof(product_name),
-					strtol(idVendor, NULL, 16),
-					strtol(idProduct, NULL, 16));
-
-		/* Print information. */
-		print_device(busid, idVendor, idProduct, parsable);
-		print_product_name(product_name, parsable);
-
-		printf("\n");
-
-		udev_device_unref(dev);
-	}
-
-	ret = 0;
-
-err_out:
-	udev_enumerate_unref(enumerate);
-	udev_unref(udev);
-
-	return ret;
-}
-
-int usbip_list(int argc, char *argv[])
-{
-	static const struct option opts[] = {
-		{ "parsable", no_argument,       NULL, 'p' },
-		{ "remote",   required_argument, NULL, 'r' },
-		{ "local",    no_argument,       NULL, 'l' },
-		{ NULL,       0,                 NULL,  0  }
-	};
-
-	bool parsable = false;
-	int opt;
-	int ret = -1;
-
-	if (usbip_names_init(USBIDS_FILE))
-		err("failed to open %s", USBIDS_FILE);
-
-	for (;;) {
-		opt = getopt_long(argc, argv, "pr:l", opts, NULL);
-
-		if (opt == -1)
-			break;
-
-		switch (opt) {
-		case 'p':
-			parsable = true;
-			break;
-		case 'r':
-			ret = list_exported_devices(optarg);
-			goto out;
-		case 'l':
-			ret = list_devices(parsable);
-			goto out;
-		default:
-			goto err_out;
-		}
-	}
-
-err_out:
-	usbip_list_usage();
-out:
-	usbip_names_free();
-
-	return ret;
-}
diff --git a/drivers/staging/usbip/userspace/src/usbip_network.c b/drivers/staging/usbip/userspace/src/usbip_network.c
deleted file mode 100644
index b4c37e76a6e0..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_network.c
+++ /dev/null
@@ -1,303 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <sys/socket.h>
-
-#include <string.h>
-
-#include <arpa/inet.h>
-#include <netdb.h>
-#include <netinet/tcp.h>
-#include <unistd.h>
-
-#ifdef HAVE_LIBWRAP
-#include <tcpd.h>
-#endif
-
-#include "usbip_common.h"
-#include "usbip_network.h"
-
-int usbip_port = 3240;
-char *usbip_port_string = "3240";
-
-void usbip_setup_port_number(char *arg)
-{
-	dbg("parsing port arg '%s'", arg);
-	char *end;
-	unsigned long int port = strtoul(arg, &end, 10);
-
-	if (end == arg) {
-		err("port: could not parse '%s' as a decimal integer", arg);
-		return;
-	}
-
-	if (*end != '\0') {
-		err("port: garbage at end of '%s'", arg);
-		return;
-	}
-
-	if (port > UINT16_MAX) {
-		err("port: %s too high (max=%d)",
-		    arg, UINT16_MAX);
-		return;
-	}
-
-	usbip_port = port;
-	usbip_port_string = arg;
-	info("using port %d (\"%s\")", usbip_port, usbip_port_string);
-}
-
-void usbip_net_pack_uint32_t(int pack, uint32_t *num)
-{
-	uint32_t i;
-
-	if (pack)
-		i = htonl(*num);
-	else
-		i = ntohl(*num);
-
-	*num = i;
-}
-
-void usbip_net_pack_uint16_t(int pack, uint16_t *num)
-{
-	uint16_t i;
-
-	if (pack)
-		i = htons(*num);
-	else
-		i = ntohs(*num);
-
-	*num = i;
-}
-
-void usbip_net_pack_usb_device(int pack, struct usbip_usb_device *udev)
-{
-	usbip_net_pack_uint32_t(pack, &udev->busnum);
-	usbip_net_pack_uint32_t(pack, &udev->devnum);
-	usbip_net_pack_uint32_t(pack, &udev->speed);
-
-	usbip_net_pack_uint16_t(pack, &udev->idVendor);
-	usbip_net_pack_uint16_t(pack, &udev->idProduct);
-	usbip_net_pack_uint16_t(pack, &udev->bcdDevice);
-}
-
-void usbip_net_pack_usb_interface(int pack __attribute__((unused)),
-				  struct usbip_usb_interface *udev
-				  __attribute__((unused)))
-{
-	/* uint8_t members need nothing */
-}
-
-static ssize_t usbip_net_xmit(int sockfd, void *buff, size_t bufflen,
-			      int sending)
-{
-	ssize_t nbytes;
-	ssize_t total = 0;
-
-	if (!bufflen)
-		return 0;
-
-	do {
-		if (sending)
-			nbytes = send(sockfd, buff, bufflen, 0);
-		else
-			nbytes = recv(sockfd, buff, bufflen, MSG_WAITALL);
-
-		if (nbytes <= 0)
-			return -1;
-
-		buff	 = (void *)((intptr_t) buff + nbytes);
-		bufflen	-= nbytes;
-		total	+= nbytes;
-
-	} while (bufflen > 0);
-
-	return total;
-}
-
-ssize_t usbip_net_recv(int sockfd, void *buff, size_t bufflen)
-{
-	return usbip_net_xmit(sockfd, buff, bufflen, 0);
-}
-
-ssize_t usbip_net_send(int sockfd, void *buff, size_t bufflen)
-{
-	return usbip_net_xmit(sockfd, buff, bufflen, 1);
-}
-
-int usbip_net_send_op_common(int sockfd, uint32_t code, uint32_t status)
-{
-	struct op_common op_common;
-	int rc;
-
-	memset(&op_common, 0, sizeof(op_common));
-
-	op_common.version = USBIP_VERSION;
-	op_common.code    = code;
-	op_common.status  = status;
-
-	PACK_OP_COMMON(1, &op_common);
-
-	rc = usbip_net_send(sockfd, &op_common, sizeof(op_common));
-	if (rc < 0) {
-		dbg("usbip_net_send failed: %d", rc);
-		return -1;
-	}
-
-	return 0;
-}
-
-int usbip_net_recv_op_common(int sockfd, uint16_t *code)
-{
-	struct op_common op_common;
-	int rc;
-
-	memset(&op_common, 0, sizeof(op_common));
-
-	rc = usbip_net_recv(sockfd, &op_common, sizeof(op_common));
-	if (rc < 0) {
-		dbg("usbip_net_recv failed: %d", rc);
-		goto err;
-	}
-
-	PACK_OP_COMMON(0, &op_common);
-
-	if (op_common.version != USBIP_VERSION) {
-		dbg("version mismatch: %d %d", op_common.version,
-		    USBIP_VERSION);
-		goto err;
-	}
-
-	switch (*code) {
-	case OP_UNSPEC:
-		break;
-	default:
-		if (op_common.code != *code) {
-			dbg("unexpected pdu %#0x for %#0x", op_common.code,
-			    *code);
-			goto err;
-		}
-	}
-
-	if (op_common.status != ST_OK) {
-		dbg("request failed at peer: %d", op_common.status);
-		goto err;
-	}
-
-	*code = op_common.code;
-
-	return 0;
-err:
-	return -1;
-}
-
-int usbip_net_set_reuseaddr(int sockfd)
-{
-	const int val = 1;
-	int ret;
-
-	ret = setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(val));
-	if (ret < 0)
-		dbg("setsockopt: SO_REUSEADDR");
-
-	return ret;
-}
-
-int usbip_net_set_nodelay(int sockfd)
-{
-	const int val = 1;
-	int ret;
-
-	ret = setsockopt(sockfd, IPPROTO_TCP, TCP_NODELAY, &val, sizeof(val));
-	if (ret < 0)
-		dbg("setsockopt: TCP_NODELAY");
-
-	return ret;
-}
-
-int usbip_net_set_keepalive(int sockfd)
-{
-	const int val = 1;
-	int ret;
-
-	ret = setsockopt(sockfd, SOL_SOCKET, SO_KEEPALIVE, &val, sizeof(val));
-	if (ret < 0)
-		dbg("setsockopt: SO_KEEPALIVE");
-
-	return ret;
-}
-
-int usbip_net_set_v6only(int sockfd)
-{
-	const int val = 1;
-	int ret;
-
-	ret = setsockopt(sockfd, IPPROTO_IPV6, IPV6_V6ONLY, &val, sizeof(val));
-	if (ret < 0)
-		dbg("setsockopt: IPV6_V6ONLY");
-
-	return ret;
-}
-
-/*
- * IPv6 Ready
- */
-int usbip_net_tcp_connect(char *hostname, char *service)
-{
-	struct addrinfo hints, *res, *rp;
-	int sockfd;
-	int ret;
-
-	memset(&hints, 0, sizeof(hints));
-	hints.ai_family = AF_UNSPEC;
-	hints.ai_socktype = SOCK_STREAM;
-
-	/* get all possible addresses */
-	ret = getaddrinfo(hostname, service, &hints, &res);
-	if (ret < 0) {
-		dbg("getaddrinfo: %s service %s: %s", hostname, service,
-		    gai_strerror(ret));
-		return ret;
-	}
-
-	/* try the addresses */
-	for (rp = res; rp; rp = rp->ai_next) {
-		sockfd = socket(rp->ai_family, rp->ai_socktype,
-				rp->ai_protocol);
-		if (sockfd < 0)
-			continue;
-
-		/* should set TCP_NODELAY for usbip */
-		usbip_net_set_nodelay(sockfd);
-		/* TODO: write code for heartbeat */
-		usbip_net_set_keepalive(sockfd);
-
-		if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) == 0)
-			break;
-
-		close(sockfd);
-	}
-
-	freeaddrinfo(res);
-
-	if (!rp)
-		return EAI_SYSTEM;
-
-	return sockfd;
-}
diff --git a/drivers/staging/usbip/userspace/src/usbip_network.h b/drivers/staging/usbip/userspace/src/usbip_network.h
deleted file mode 100644
index c1e875cf1078..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_network.h
+++ /dev/null
@@ -1,185 +0,0 @@
-/*
- * Copyright (C) 2005-2007 Takahiro Hirofuchi
- */
-
-#ifndef __USBIP_NETWORK_H
-#define __USBIP_NETWORK_H
-
-#ifdef HAVE_CONFIG_H
-#include "../config.h"
-#endif
-
-#include <sys/types.h>
-
-#include <stdint.h>
-
-extern int usbip_port;
-extern char *usbip_port_string;
-void usbip_setup_port_number(char *arg);
-
-/* ---------------------------------------------------------------------- */
-/* Common header for all the kinds of PDUs. */
-struct op_common {
-	uint16_t version;
-
-#define OP_REQUEST	(0x80 << 8)
-#define OP_REPLY	(0x00 << 8)
-	uint16_t code;
-
-	/* add more error code */
-#define ST_OK	0x00
-#define ST_NA	0x01
-	uint32_t status; /* op_code status (for reply) */
-
-} __attribute__((packed));
-
-#define PACK_OP_COMMON(pack, op_common)  do {\
-	usbip_net_pack_uint16_t(pack, &(op_common)->version);\
-	usbip_net_pack_uint16_t(pack, &(op_common)->code);\
-	usbip_net_pack_uint32_t(pack, &(op_common)->status);\
-} while (0)
-
-/* ---------------------------------------------------------------------- */
-/* Dummy Code */
-#define OP_UNSPEC	0x00
-#define OP_REQ_UNSPEC	OP_UNSPEC
-#define OP_REP_UNSPEC	OP_UNSPEC
-
-/* ---------------------------------------------------------------------- */
-/* Retrieve USB device information. (still not used) */
-#define OP_DEVINFO	0x02
-#define OP_REQ_DEVINFO	(OP_REQUEST | OP_DEVINFO)
-#define OP_REP_DEVINFO	(OP_REPLY   | OP_DEVINFO)
-
-struct op_devinfo_request {
-	char busid[SYSFS_BUS_ID_SIZE];
-} __attribute__((packed));
-
-struct op_devinfo_reply {
-	struct usbip_usb_device udev;
-	struct usbip_usb_interface uinf[];
-} __attribute__((packed));
-
-/* ---------------------------------------------------------------------- */
-/* Import a remote USB device. */
-#define OP_IMPORT	0x03
-#define OP_REQ_IMPORT	(OP_REQUEST | OP_IMPORT)
-#define OP_REP_IMPORT   (OP_REPLY   | OP_IMPORT)
-
-struct op_import_request {
-	char busid[SYSFS_BUS_ID_SIZE];
-} __attribute__((packed));
-
-struct op_import_reply {
-	struct usbip_usb_device udev;
-//	struct usbip_usb_interface uinf[];
-} __attribute__((packed));
-
-#define PACK_OP_IMPORT_REQUEST(pack, request)  do {\
-} while (0)
-
-#define PACK_OP_IMPORT_REPLY(pack, reply)  do {\
-	usbip_net_pack_usb_device(pack, &(reply)->udev);\
-} while (0)
-
-/* ---------------------------------------------------------------------- */
-/* Export a USB device to a remote host. */
-#define OP_EXPORT	0x06
-#define OP_REQ_EXPORT	(OP_REQUEST | OP_EXPORT)
-#define OP_REP_EXPORT	(OP_REPLY   | OP_EXPORT)
-
-struct op_export_request {
-	struct usbip_usb_device udev;
-} __attribute__((packed));
-
-struct op_export_reply {
-	int returncode;
-} __attribute__((packed));
-
-
-#define PACK_OP_EXPORT_REQUEST(pack, request)  do {\
-	usbip_net_pack_usb_device(pack, &(request)->udev);\
-} while (0)
-
-#define PACK_OP_EXPORT_REPLY(pack, reply)  do {\
-} while (0)
-
-/* ---------------------------------------------------------------------- */
-/* un-Export a USB device from a remote host. */
-#define OP_UNEXPORT	0x07
-#define OP_REQ_UNEXPORT	(OP_REQUEST | OP_UNEXPORT)
-#define OP_REP_UNEXPORT	(OP_REPLY   | OP_UNEXPORT)
-
-struct op_unexport_request {
-	struct usbip_usb_device udev;
-} __attribute__((packed));
-
-struct op_unexport_reply {
-	int returncode;
-} __attribute__((packed));
-
-#define PACK_OP_UNEXPORT_REQUEST(pack, request)  do {\
-	usbip_net_pack_usb_device(pack, &(request)->udev);\
-} while (0)
-
-#define PACK_OP_UNEXPORT_REPLY(pack, reply)  do {\
-} while (0)
-
-/* ---------------------------------------------------------------------- */
-/* Negotiate IPSec encryption key. (still not used) */
-#define OP_CRYPKEY	0x04
-#define OP_REQ_CRYPKEY	(OP_REQUEST | OP_CRYPKEY)
-#define OP_REP_CRYPKEY	(OP_REPLY   | OP_CRYPKEY)
-
-struct op_crypkey_request {
-	/* 128bit key */
-	uint32_t key[4];
-} __attribute__((packed));
-
-struct op_crypkey_reply {
-	uint32_t __reserved;
-} __attribute__((packed));
-
-
-/* ---------------------------------------------------------------------- */
-/* Retrieve the list of exported USB devices. */
-#define OP_DEVLIST	0x05
-#define OP_REQ_DEVLIST	(OP_REQUEST | OP_DEVLIST)
-#define OP_REP_DEVLIST	(OP_REPLY   | OP_DEVLIST)
-
-struct op_devlist_request {
-} __attribute__((packed));
-
-struct op_devlist_reply {
-	uint32_t ndev;
-	/* followed by reply_extra[] */
-} __attribute__((packed));
-
-struct op_devlist_reply_extra {
-	struct usbip_usb_device    udev;
-	struct usbip_usb_interface uinf[];
-} __attribute__((packed));
-
-#define PACK_OP_DEVLIST_REQUEST(pack, request)  do {\
-} while (0)
-
-#define PACK_OP_DEVLIST_REPLY(pack, reply)  do {\
-	usbip_net_pack_uint32_t(pack, &(reply)->ndev);\
-} while (0)
-
-void usbip_net_pack_uint32_t(int pack, uint32_t *num);
-void usbip_net_pack_uint16_t(int pack, uint16_t *num);
-void usbip_net_pack_usb_device(int pack, struct usbip_usb_device *udev);
-void usbip_net_pack_usb_interface(int pack, struct usbip_usb_interface *uinf);
-
-ssize_t usbip_net_recv(int sockfd, void *buff, size_t bufflen);
-ssize_t usbip_net_send(int sockfd, void *buff, size_t bufflen);
-int usbip_net_send_op_common(int sockfd, uint32_t code, uint32_t status);
-int usbip_net_recv_op_common(int sockfd, uint16_t *code);
-int usbip_net_set_reuseaddr(int sockfd);
-int usbip_net_set_nodelay(int sockfd);
-int usbip_net_set_keepalive(int sockfd);
-int usbip_net_set_v6only(int sockfd);
-int usbip_net_tcp_connect(char *hostname, char *port);
-
-#endif /* __USBIP_NETWORK_H */
diff --git a/drivers/staging/usbip/userspace/src/usbip_port.c b/drivers/staging/usbip/userspace/src/usbip_port.c
deleted file mode 100644
index a2e884fd9226..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_port.c
+++ /dev/null
@@ -1,57 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- */
-
-#include "vhci_driver.h"
-#include "usbip_common.h"
-
-static int list_imported_devices(void)
-{
-	int i;
-	struct usbip_imported_device *idev;
-	int ret;
-
-	ret = usbip_vhci_driver_open();
-	if (ret < 0) {
-		err("open vhci_driver");
-		return -1;
-	}
-
-	printf("Imported USB devices\n");
-	printf("====================\n");
-
-	for (i = 0; i < vhci_driver->nports; i++) {
-		idev = &vhci_driver->idev[i];
-
-		if (usbip_vhci_imported_device_dump(idev) < 0)
-			ret = -1;
-	}
-
-	usbip_vhci_driver_close();
-
-	return ret;
-
-}
-
-int usbip_port_show(__attribute__((unused)) int argc,
-		    __attribute__((unused)) char *argv[])
-{
-	int ret;
-
-	ret = list_imported_devices();
-	if (ret < 0)
-		err("list imported devices");
-
-	return ret;
-}
diff --git a/drivers/staging/usbip/userspace/src/usbip_unbind.c b/drivers/staging/usbip/userspace/src/usbip_unbind.c
deleted file mode 100644
index a4a496c9cbaf..000000000000
--- a/drivers/staging/usbip/userspace/src/usbip_unbind.c
+++ /dev/null
@@ -1,141 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <libudev.h>
-
-#include <errno.h>
-#include <stdio.h>
-#include <string.h>
-
-#include <getopt.h>
-
-#include "usbip_common.h"
-#include "utils.h"
-#include "usbip.h"
-#include "sysfs_utils.h"
-
-static const char usbip_unbind_usage_string[] =
-	"usbip unbind <args>\n"
-	"    -b, --busid=<busid>    Unbind " USBIP_HOST_DRV_NAME ".ko from "
-	"device on <busid>\n";
-
-void usbip_unbind_usage(void)
-{
-	printf("usage: %s", usbip_unbind_usage_string);
-}
-
-static int unbind_device(char *busid)
-{
-	char bus_type[] = "usb";
-	int rc, ret = -1;
-
-	char unbind_attr_name[] = "unbind";
-	char unbind_attr_path[SYSFS_PATH_MAX];
-	char rebind_attr_name[] = "rebind";
-	char rebind_attr_path[SYSFS_PATH_MAX];
-
-	struct udev *udev;
-	struct udev_device *dev;
-	const char *driver;
-
-	/* Create libudev context. */
-	udev = udev_new();
-
-	/* Check whether the device with this bus ID exists. */
-	dev = udev_device_new_from_subsystem_sysname(udev, "usb", busid);
-	if (!dev) {
-		err("device with the specified bus ID does not exist");
-		goto err_close_udev;
-	}
-
-	/* Check whether the device is using usbip-host driver. */
-	driver = udev_device_get_driver(dev);
-	if (!driver || strcmp(driver, "usbip-host")) {
-		err("device is not bound to usbip-host driver");
-		goto err_close_udev;
-	}
-
-	/* Unbind device from driver. */
-	snprintf(unbind_attr_path, sizeof(unbind_attr_path), "%s/%s/%s/%s/%s/%s",
-		 SYSFS_MNT_PATH, SYSFS_BUS_NAME, bus_type, SYSFS_DRIVERS_NAME,
-		 USBIP_HOST_DRV_NAME, unbind_attr_name);
-
-	rc = write_sysfs_attribute(unbind_attr_path, busid, strlen(busid));
-	if (rc < 0) {
-		err("error unbinding device %s from driver", busid);
-		goto err_close_udev;
-	}
-
-	/* Notify driver of unbind. */
-	rc = modify_match_busid(busid, 0);
-	if (rc < 0) {
-		err("unable to unbind device on %s", busid);
-		goto err_close_udev;
-	}
-
-	/* Trigger new probing. */
-	snprintf(rebind_attr_path, sizeof(unbind_attr_path), "%s/%s/%s/%s/%s/%s",
-			SYSFS_MNT_PATH, SYSFS_BUS_NAME, bus_type, SYSFS_DRIVERS_NAME,
-			USBIP_HOST_DRV_NAME, rebind_attr_name);
-
-	rc = write_sysfs_attribute(rebind_attr_path, busid, strlen(busid));
-	if (rc < 0) {
-		err("error rebinding");
-		goto err_close_udev;
-	}
-
-	ret = 0;
-	info("unbind device on busid %s: complete", busid);
-
-err_close_udev:
-	udev_device_unref(dev);
-	udev_unref(udev);
-
-	return ret;
-}
-
-int usbip_unbind(int argc, char *argv[])
-{
-	static const struct option opts[] = {
-		{ "busid", required_argument, NULL, 'b' },
-		{ NULL,    0,                 NULL,  0  }
-	};
-
-	int opt;
-	int ret = -1;
-
-	for (;;) {
-		opt = getopt_long(argc, argv, "b:", opts, NULL);
-
-		if (opt == -1)
-			break;
-
-		switch (opt) {
-		case 'b':
-			ret = unbind_device(optarg);
-			goto out;
-		default:
-			goto err_out;
-		}
-	}
-
-err_out:
-	usbip_unbind_usage();
-out:
-	return ret;
-}
diff --git a/drivers/staging/usbip/userspace/src/usbipd.c b/drivers/staging/usbip/userspace/src/usbipd.c
deleted file mode 100644
index 2f87f2d348ba..000000000000
--- a/drivers/staging/usbip/userspace/src/usbipd.c
+++ /dev/null
@@ -1,679 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifdef HAVE_CONFIG_H
-#include "../config.h"
-#endif
-
-#define _GNU_SOURCE
-#include <errno.h>
-#include <unistd.h>
-#include <netdb.h>
-#include <string.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <arpa/inet.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
-
-#ifdef HAVE_LIBWRAP
-#include <tcpd.h>
-#endif
-
-#include <getopt.h>
-#include <signal.h>
-#include <poll.h>
-
-#include "usbip_host_driver.h"
-#include "usbip_common.h"
-#include "usbip_network.h"
-#include "list.h"
-
-#undef  PROGNAME
-#define PROGNAME "usbipd"
-#define MAXSOCKFD 20
-
-#define MAIN_LOOP_TIMEOUT 10
-
-#define DEFAULT_PID_FILE "/var/run/" PROGNAME ".pid"
-
-static const char usbip_version_string[] = PACKAGE_STRING;
-
-static const char usbipd_help_string[] =
-	"usage: usbipd [options]\n"
-	"\n"
-	"	-4, --ipv4\n"
-	"		Bind to IPv4. Default is both.\n"
-	"\n"
-	"	-6, --ipv6\n"
-	"		Bind to IPv6. Default is both.\n"
-	"\n"
-	"	-D, --daemon\n"
-	"		Run as a daemon process.\n"
-	"\n"
-	"	-d, --debug\n"
-	"		Print debugging information.\n"
-	"\n"
-	"	-PFILE, --pid FILE\n"
-	"		Write process id to FILE.\n"
-	"		If no FILE specified, use " DEFAULT_PID_FILE "\n"
-	"\n"
-	"	-tPORT, --tcp-port PORT\n"
-	"		Listen on TCP/IP port PORT.\n"
-	"\n"
-	"	-h, --help\n"
-	"		Print this help.\n"
-	"\n"
-	"	-v, --version\n"
-	"		Show version.\n";
-
-static void usbipd_help(void)
-{
-	printf("%s\n", usbipd_help_string);
-}
-
-static int recv_request_import(int sockfd)
-{
-	struct op_import_request req;
-	struct op_common reply;
-	struct usbip_exported_device *edev;
-	struct usbip_usb_device pdu_udev;
-	struct list_head *i;
-	int found = 0;
-	int error = 0;
-	int rc;
-
-	memset(&req, 0, sizeof(req));
-	memset(&reply, 0, sizeof(reply));
-
-	rc = usbip_net_recv(sockfd, &req, sizeof(req));
-	if (rc < 0) {
-		dbg("usbip_net_recv failed: import request");
-		return -1;
-	}
-	PACK_OP_IMPORT_REQUEST(0, &req);
-
-	list_for_each(i, &host_driver->edev_list) {
-		edev = list_entry(i, struct usbip_exported_device, node);
-		if (!strncmp(req.busid, edev->udev.busid, SYSFS_BUS_ID_SIZE)) {
-			info("found requested device: %s", req.busid);
-			found = 1;
-			break;
-		}
-	}
-
-	if (found) {
-		/* should set TCP_NODELAY for usbip */
-		usbip_net_set_nodelay(sockfd);
-
-		/* export device needs a TCP/IP socket descriptor */
-		rc = usbip_host_export_device(edev, sockfd);
-		if (rc < 0)
-			error = 1;
-	} else {
-		info("requested device not found: %s", req.busid);
-		error = 1;
-	}
-
-	rc = usbip_net_send_op_common(sockfd, OP_REP_IMPORT,
-				      (!error ? ST_OK : ST_NA));
-	if (rc < 0) {
-		dbg("usbip_net_send_op_common failed: %#0x", OP_REP_IMPORT);
-		return -1;
-	}
-
-	if (error) {
-		dbg("import request busid %s: failed", req.busid);
-		return -1;
-	}
-
-	memcpy(&pdu_udev, &edev->udev, sizeof(pdu_udev));
-	usbip_net_pack_usb_device(1, &pdu_udev);
-
-	rc = usbip_net_send(sockfd, &pdu_udev, sizeof(pdu_udev));
-	if (rc < 0) {
-		dbg("usbip_net_send failed: devinfo");
-		return -1;
-	}
-
-	dbg("import request busid %s: complete", req.busid);
-
-	return 0;
-}
-
-static int send_reply_devlist(int connfd)
-{
-	struct usbip_exported_device *edev;
-	struct usbip_usb_device pdu_udev;
-	struct usbip_usb_interface pdu_uinf;
-	struct op_devlist_reply reply;
-	struct list_head *j;
-	int rc, i;
-
-	reply.ndev = 0;
-	/* number of exported devices */
-	list_for_each(j, &host_driver->edev_list) {
-		reply.ndev += 1;
-	}
-	info("exportable devices: %d", reply.ndev);
-
-	rc = usbip_net_send_op_common(connfd, OP_REP_DEVLIST, ST_OK);
-	if (rc < 0) {
-		dbg("usbip_net_send_op_common failed: %#0x", OP_REP_DEVLIST);
-		return -1;
-	}
-	PACK_OP_DEVLIST_REPLY(1, &reply);
-
-	rc = usbip_net_send(connfd, &reply, sizeof(reply));
-	if (rc < 0) {
-		dbg("usbip_net_send failed: %#0x", OP_REP_DEVLIST);
-		return -1;
-	}
-
-	list_for_each(j, &host_driver->edev_list) {
-		edev = list_entry(j, struct usbip_exported_device, node);
-		dump_usb_device(&edev->udev);
-		memcpy(&pdu_udev, &edev->udev, sizeof(pdu_udev));
-		usbip_net_pack_usb_device(1, &pdu_udev);
-
-		rc = usbip_net_send(connfd, &pdu_udev, sizeof(pdu_udev));
-		if (rc < 0) {
-			dbg("usbip_net_send failed: pdu_udev");
-			return -1;
-		}
-
-		for (i = 0; i < edev->udev.bNumInterfaces; i++) {
-			dump_usb_interface(&edev->uinf[i]);
-			memcpy(&pdu_uinf, &edev->uinf[i], sizeof(pdu_uinf));
-			usbip_net_pack_usb_interface(1, &pdu_uinf);
-
-			rc = usbip_net_send(connfd, &pdu_uinf,
-					sizeof(pdu_uinf));
-			if (rc < 0) {
-				err("usbip_net_send failed: pdu_uinf");
-				return -1;
-			}
-		}
-	}
-
-	return 0;
-}
-
-static int recv_request_devlist(int connfd)
-{
-	struct op_devlist_request req;
-	int rc;
-
-	memset(&req, 0, sizeof(req));
-
-	rc = usbip_net_recv(connfd, &req, sizeof(req));
-	if (rc < 0) {
-		dbg("usbip_net_recv failed: devlist request");
-		return -1;
-	}
-
-	rc = send_reply_devlist(connfd);
-	if (rc < 0) {
-		dbg("send_reply_devlist failed");
-		return -1;
-	}
-
-	return 0;
-}
-
-static int recv_pdu(int connfd)
-{
-	uint16_t code = OP_UNSPEC;
-	int ret;
-
-	ret = usbip_net_recv_op_common(connfd, &code);
-	if (ret < 0) {
-		dbg("could not receive opcode: %#0x", code);
-		return -1;
-	}
-
-	ret = usbip_host_refresh_device_list();
-	if (ret < 0) {
-		dbg("could not refresh device list: %d", ret);
-		return -1;
-	}
-
-	info("received request: %#0x(%d)", code, connfd);
-	switch (code) {
-	case OP_REQ_DEVLIST:
-		ret = recv_request_devlist(connfd);
-		break;
-	case OP_REQ_IMPORT:
-		ret = recv_request_import(connfd);
-		break;
-	case OP_REQ_DEVINFO:
-	case OP_REQ_CRYPKEY:
-	default:
-		err("received an unknown opcode: %#0x", code);
-		ret = -1;
-	}
-
-	if (ret == 0)
-		info("request %#0x(%d): complete", code, connfd);
-	else
-		info("request %#0x(%d): failed", code, connfd);
-
-	return ret;
-}
-
-#ifdef HAVE_LIBWRAP
-static int tcpd_auth(int connfd)
-{
-	struct request_info request;
-	int rc;
-
-	request_init(&request, RQ_DAEMON, PROGNAME, RQ_FILE, connfd, 0);
-	fromhost(&request);
-	rc = hosts_access(&request);
-	if (rc == 0)
-		return -1;
-
-	return 0;
-}
-#endif
-
-static int do_accept(int listenfd)
-{
-	int connfd;
-	struct sockaddr_storage ss;
-	socklen_t len = sizeof(ss);
-	char host[NI_MAXHOST], port[NI_MAXSERV];
-	int rc;
-
-	memset(&ss, 0, sizeof(ss));
-
-	connfd = accept(listenfd, (struct sockaddr *)&ss, &len);
-	if (connfd < 0) {
-		err("failed to accept connection");
-		return -1;
-	}
-
-	rc = getnameinfo((struct sockaddr *)&ss, len, host, sizeof(host),
-			 port, sizeof(port), NI_NUMERICHOST | NI_NUMERICSERV);
-	if (rc)
-		err("getnameinfo: %s", gai_strerror(rc));
-
-#ifdef HAVE_LIBWRAP
-	rc = tcpd_auth(connfd);
-	if (rc < 0) {
-		info("denied access from %s", host);
-		close(connfd);
-		return -1;
-	}
-#endif
-	info("connection from %s:%s", host, port);
-
-	return connfd;
-}
-
-int process_request(int listenfd)
-{
-	pid_t childpid;
-	int connfd;
-
-	connfd = do_accept(listenfd);
-	if (connfd < 0)
-		return -1;
-	childpid = fork();
-	if (childpid == 0) {
-		close(listenfd);
-		recv_pdu(connfd);
-		exit(0);
-	}
-	close(connfd);
-	return 0;
-}
-
-static void addrinfo_to_text(struct addrinfo *ai, char buf[],
-			     const size_t buf_size)
-{
-	char hbuf[NI_MAXHOST];
-	char sbuf[NI_MAXSERV];
-	int rc;
-
-	buf[0] = '\0';
-
-	rc = getnameinfo(ai->ai_addr, ai->ai_addrlen, hbuf, sizeof(hbuf),
-			 sbuf, sizeof(sbuf), NI_NUMERICHOST | NI_NUMERICSERV);
-	if (rc)
-		err("getnameinfo: %s", gai_strerror(rc));
-
-	snprintf(buf, buf_size, "%s:%s", hbuf, sbuf);
-}
-
-static int listen_all_addrinfo(struct addrinfo *ai_head, int sockfdlist[],
-			     int maxsockfd)
-{
-	struct addrinfo *ai;
-	int ret, nsockfd = 0;
-	const size_t ai_buf_size = NI_MAXHOST + NI_MAXSERV + 2;
-	char ai_buf[ai_buf_size];
-
-	for (ai = ai_head; ai && nsockfd < maxsockfd; ai = ai->ai_next) {
-		int sock;
-
-		addrinfo_to_text(ai, ai_buf, ai_buf_size);
-		dbg("opening %s", ai_buf);
-		sock = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
-		if (sock < 0) {
-			err("socket: %s: %d (%s)",
-			    ai_buf, errno, strerror(errno));
-			continue;
-		}
-
-		usbip_net_set_reuseaddr(sock);
-		usbip_net_set_nodelay(sock);
-		/* We use seperate sockets for IPv4 and IPv6
-		 * (see do_standalone_mode()) */
-		usbip_net_set_v6only(sock);
-
-		if (sock >= FD_SETSIZE) {
-			err("FD_SETSIZE: %s: sock=%d, max=%d",
-			    ai_buf, sock, FD_SETSIZE);
-			close(sock);
-			continue;
-		}
-
-		ret = bind(sock, ai->ai_addr, ai->ai_addrlen);
-		if (ret < 0) {
-			err("bind: %s: %d (%s)",
-			    ai_buf, errno, strerror(errno));
-			close(sock);
-			continue;
-		}
-
-		ret = listen(sock, SOMAXCONN);
-		if (ret < 0) {
-			err("listen: %s: %d (%s)",
-			    ai_buf, errno, strerror(errno));
-			close(sock);
-			continue;
-		}
-
-		info("listening on %s", ai_buf);
-		sockfdlist[nsockfd++] = sock;
-	}
-
-	return nsockfd;
-}
-
-static struct addrinfo *do_getaddrinfo(char *host, int ai_family)
-{
-	struct addrinfo hints, *ai_head;
-	int rc;
-
-	memset(&hints, 0, sizeof(hints));
-	hints.ai_family   = ai_family;
-	hints.ai_socktype = SOCK_STREAM;
-	hints.ai_flags    = AI_PASSIVE;
-
-	rc = getaddrinfo(host, usbip_port_string, &hints, &ai_head);
-	if (rc) {
-		err("failed to get a network address %s: %s", usbip_port_string,
-		    gai_strerror(rc));
-		return NULL;
-	}
-
-	return ai_head;
-}
-
-static void signal_handler(int i)
-{
-	dbg("received '%s' signal", strsignal(i));
-}
-
-static void set_signal(void)
-{
-	struct sigaction act;
-
-	memset(&act, 0, sizeof(act));
-	act.sa_handler = signal_handler;
-	sigemptyset(&act.sa_mask);
-	sigaction(SIGTERM, &act, NULL);
-	sigaction(SIGINT, &act, NULL);
-	act.sa_handler = SIG_IGN;
-	sigaction(SIGCLD, &act, NULL);
-}
-
-static const char *pid_file;
-
-static void write_pid_file(void)
-{
-	if (pid_file) {
-		dbg("creating pid file %s", pid_file);
-		FILE *fp;
-
-		fp = fopen(pid_file, "w");
-		if (!fp) {
-			err("pid_file: %s: %d (%s)",
-			    pid_file, errno, strerror(errno));
-			return;
-		}
-		fprintf(fp, "%d\n", getpid());
-		fclose(fp);
-	}
-}
-
-static void remove_pid_file(void)
-{
-	if (pid_file) {
-		dbg("removing pid file %s", pid_file);
-		unlink(pid_file);
-	}
-}
-
-static int do_standalone_mode(int daemonize, int ipv4, int ipv6)
-{
-	struct addrinfo *ai_head;
-	int sockfdlist[MAXSOCKFD];
-	int nsockfd, family;
-	int i, terminate;
-	struct pollfd *fds;
-	struct timespec timeout;
-	sigset_t sigmask;
-
-	if (usbip_host_driver_open()) {
-		err("please load " USBIP_CORE_MOD_NAME ".ko and "
-		    USBIP_HOST_DRV_NAME ".ko!");
-		return -1;
-	}
-
-	if (daemonize) {
-		if (daemon(0, 0) < 0) {
-			err("daemonizing failed: %s", strerror(errno));
-			usbip_host_driver_close();
-			return -1;
-		}
-		umask(0);
-		usbip_use_syslog = 1;
-	}
-	set_signal();
-	write_pid_file();
-
-	info("starting " PROGNAME " (%s)", usbip_version_string);
-
-	/*
-	 * To suppress warnings on systems with bindv6only disabled
-	 * (default), we use seperate sockets for IPv6 and IPv4 and set
-	 * IPV6_V6ONLY on the IPv6 sockets.
-	 */
-	if (ipv4 && ipv6)
-		family = AF_UNSPEC;
-	else if (ipv4)
-		family = AF_INET;
-	else
-		family = AF_INET6;
-
-	ai_head = do_getaddrinfo(NULL, family);
-	if (!ai_head) {
-		usbip_host_driver_close();
-		return -1;
-	}
-	nsockfd = listen_all_addrinfo(ai_head, sockfdlist,
-		sizeof(sockfdlist) / sizeof(*sockfdlist));
-	freeaddrinfo(ai_head);
-	if (nsockfd <= 0) {
-		err("failed to open a listening socket");
-		usbip_host_driver_close();
-		return -1;
-	}
-
-	dbg("listening on %d address%s", nsockfd, (nsockfd == 1) ? "" : "es");
-
-	fds = calloc(nsockfd, sizeof(struct pollfd));
-	for (i = 0; i < nsockfd; i++) {
-		fds[i].fd = sockfdlist[i];
-		fds[i].events = POLLIN;
-	}
-	timeout.tv_sec = MAIN_LOOP_TIMEOUT;
-	timeout.tv_nsec = 0;
-
-	sigfillset(&sigmask);
-	sigdelset(&sigmask, SIGTERM);
-	sigdelset(&sigmask, SIGINT);
-
-	terminate = 0;
-	while (!terminate) {
-		int r;
-
-		r = ppoll(fds, nsockfd, &timeout, &sigmask);
-		if (r < 0) {
-			dbg("%s", strerror(errno));
-			terminate = 1;
-		} else if (r) {
-			for (i = 0; i < nsockfd; i++) {
-				if (fds[i].revents & POLLIN) {
-					dbg("read event on fd[%d]=%d",
-					    i, sockfdlist[i]);
-					process_request(sockfdlist[i]);
-				}
-			}
-		} else {
-			dbg("heartbeat timeout on ppoll()");
-		}
-	}
-
-	info("shutting down " PROGNAME);
-	free(fds);
-	usbip_host_driver_close();
-
-	return 0;
-}
-
-int main(int argc, char *argv[])
-{
-	static const struct option longopts[] = {
-		{ "ipv4",     no_argument,       NULL, '4' },
-		{ "ipv6",     no_argument,       NULL, '6' },
-		{ "daemon",   no_argument,       NULL, 'D' },
-		{ "daemon",   no_argument,       NULL, 'D' },
-		{ "debug",    no_argument,       NULL, 'd' },
-		{ "pid",      optional_argument, NULL, 'P' },
-		{ "tcp-port", required_argument, NULL, 't' },
-		{ "help",     no_argument,       NULL, 'h' },
-		{ "version",  no_argument,       NULL, 'v' },
-		{ NULL,	      0,                 NULL,  0  }
-	};
-
-	enum {
-		cmd_standalone_mode = 1,
-		cmd_help,
-		cmd_version
-	} cmd;
-
-	int daemonize = 0;
-	int ipv4 = 0, ipv6 = 0;
-	int opt, rc = -1;
-
-	pid_file = NULL;
-
-	usbip_use_stderr = 1;
-	usbip_use_syslog = 0;
-
-	if (geteuid() != 0)
-		err("not running as root?");
-
-	cmd = cmd_standalone_mode;
-	for (;;) {
-		opt = getopt_long(argc, argv, "46DdP::t:hv", longopts, NULL);
-
-		if (opt == -1)
-			break;
-
-		switch (opt) {
-		case '4':
-			ipv4 = 1;
-			break;
-		case '6':
-			ipv6 = 1;
-			break;
-		case 'D':
-			daemonize = 1;
-			break;
-		case 'd':
-			usbip_use_debug = 1;
-			break;
-		case 'h':
-			cmd = cmd_help;
-			break;
-		case 'P':
-			pid_file = optarg ? optarg : DEFAULT_PID_FILE;
-			break;
-		case 't':
-			usbip_setup_port_number(optarg);
-			break;
-		case 'v':
-			cmd = cmd_version;
-			break;
-		case '?':
-			usbipd_help();
-		default:
-			goto err_out;
-		}
-	}
-
-	if (!ipv4 && !ipv6)
-		ipv4 = ipv6 = 1;
-
-	switch (cmd) {
-	case cmd_standalone_mode:
-		rc = do_standalone_mode(daemonize, ipv4, ipv6);
-		remove_pid_file();
-		break;
-	case cmd_version:
-		printf(PROGNAME " (%s)\n", usbip_version_string);
-		rc = 0;
-		break;
-	case cmd_help:
-		usbipd_help();
-		rc = 0;
-		break;
-	default:
-		usbipd_help();
-		goto err_out;
-	}
-
-err_out:
-	return (rc > -1 ? EXIT_SUCCESS : EXIT_FAILURE);
-}
diff --git a/drivers/staging/usbip/userspace/src/utils.c b/drivers/staging/usbip/userspace/src/utils.c
deleted file mode 100644
index 2b3d6d235015..000000000000
--- a/drivers/staging/usbip/userspace/src/utils.c
+++ /dev/null
@@ -1,52 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <errno.h>
-#include <stdio.h>
-#include <string.h>
-
-#include "usbip_common.h"
-#include "utils.h"
-#include "sysfs_utils.h"
-
-int modify_match_busid(char *busid, int add)
-{
-	char attr_name[] = "match_busid";
-	char command[SYSFS_BUS_ID_SIZE + 4];
-	char match_busid_attr_path[SYSFS_PATH_MAX];
-	int rc;
-
-	snprintf(match_busid_attr_path, sizeof(match_busid_attr_path),
-		 "%s/%s/%s/%s/%s/%s", SYSFS_MNT_PATH, SYSFS_BUS_NAME,
-		 SYSFS_BUS_TYPE, SYSFS_DRIVERS_NAME, USBIP_HOST_DRV_NAME,
-		 attr_name);
-
-	if (add)
-		snprintf(command, SYSFS_BUS_ID_SIZE + 4, "add %s", busid);
-	else
-		snprintf(command, SYSFS_BUS_ID_SIZE + 4, "del %s", busid);
-
-	rc = write_sysfs_attribute(match_busid_attr_path, command,
-				   sizeof(command));
-	if (rc < 0) {
-		dbg("failed to write match_busid: %s", strerror(errno));
-		return -1;
-	}
-
-	return 0;
-}
diff --git a/drivers/staging/usbip/userspace/src/utils.h b/drivers/staging/usbip/userspace/src/utils.h
deleted file mode 100644
index 5916fd3e02a6..000000000000
--- a/drivers/staging/usbip/userspace/src/utils.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*
- * Copyright (C) 2011 matt mooney <mfm@muteddisk.com>
- *               2005-2007 Takahiro Hirofuchi
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http://www.gnu.org/licenses/>.
- */
-
-#ifndef __UTILS_H
-#define __UTILS_H
-
-int modify_match_busid(char *busid, int add);
-
-#endif /* __UTILS_H */
-
diff --git a/drivers/staging/vme/devices/vme_pio2_gpio.c b/drivers/staging/vme/devices/vme_pio2_gpio.c
index f00af0786af3..c64776f71809 100644
--- a/drivers/staging/vme/devices/vme_pio2_gpio.c
+++ b/drivers/staging/vme/devices/vme_pio2_gpio.c
@@ -58,14 +58,14 @@ static int pio2_gpio_get(struct gpio_chip *chip, unsigned int offset)
 	if (reg & PIO2_CHANNEL_BIT[offset]) {
 		if (card->bank[PIO2_CHANNEL_BANK[offset]].config != BOTH)
 			return 0;
-		else
-			return 1;
-	} else {
-		if (card->bank[PIO2_CHANNEL_BANK[offset]].config != BOTH)
-			return 1;
-		else
-			return 0;
+
+		return 1;
 	}
+
+	if (card->bank[PIO2_CHANNEL_BANK[offset]].config != BOTH)
+		return 1;
+
+	return 0;
 }
 
 static void pio2_gpio_set(struct gpio_chip *chip, unsigned int offset,
@@ -221,9 +221,7 @@ void pio2_gpio_exit(struct pio2_card *card)
 {
 	const char *label = card->gc.label;
 
-	if (gpiochip_remove(&(card->gc)))
-		dev_err(&card->vdev->dev, "Failed to remove GPIO\n");
-
+	gpiochip_remove(&(card->gc));
 	kfree(label);
 }
 
diff --git a/drivers/staging/vme/devices/vme_user.c b/drivers/staging/vme/devices/vme_user.c
index 920e50addd3e..8b1f53331433 100644
--- a/drivers/staging/vme/devices/vme_user.c
+++ b/drivers/staging/vme/devices/vme_user.c
@@ -410,41 +410,19 @@ static ssize_t vme_user_write(struct file *file, const char __user *buf,
 
 static loff_t vme_user_llseek(struct file *file, loff_t off, int whence)
 {
-	loff_t absolute = -1;
 	unsigned int minor = MINOR(file_inode(file)->i_rdev);
 	size_t image_size;
+	loff_t res;
 
 	if (minor == CONTROL_MINOR)
 		return -EINVAL;
 
 	mutex_lock(&image[minor].mutex);
 	image_size = vme_get_size(image[minor].resource);
-
-	switch (whence) {
-	case SEEK_SET:
-		absolute = off;
-		break;
-	case SEEK_CUR:
-		absolute = file->f_pos + off;
-		break;
-	case SEEK_END:
-		absolute = image_size + off;
-		break;
-	default:
-		mutex_unlock(&image[minor].mutex);
-		return -EINVAL;
-	}
-
-	if ((absolute < 0) || (absolute >= image_size)) {
-		mutex_unlock(&image[minor].mutex);
-		return -EINVAL;
-	}
-
-	file->f_pos = absolute;
-
+	res = fixed_size_llseek(file, off, whence, image_size);
 	mutex_unlock(&image[minor].mutex);
 
-	return absolute;
+	return res;
 }
 
 /*
@@ -482,11 +460,9 @@ static int vme_user_ioctl(struct inode *inode, struct file *file,
 				return -EFAULT;
 			}
 
-			retval = vme_irq_generate(vme_user_bridge,
+			return vme_irq_generate(vme_user_bridge,
 						  irq_req.level,
 						  irq_req.statid);
-
-			return retval;
 		}
 		break;
 	case MASTER_MINOR:
diff --git a/drivers/staging/vt6655/80211hdr.h b/drivers/staging/vt6655/80211hdr.h
index e05d13544ea2..36e14ec50564 100644
--- a/drivers/staging/vt6655/80211hdr.h
+++ b/drivers/staging/vt6655/80211hdr.h
@@ -83,7 +83,8 @@
 #define WLAN_RATES_MAXLEN_11B       4
 #define WLAN_RSN_MAXLEN             32
 #define WLAN_DATA_MAXLEN            2312
-#define WLAN_A3FR_MAXLEN            (WLAN_HDR_ADDR3_LEN + WLAN_DATA_MAXLEN + WLAN_CRC_LEN)
+#define WLAN_A3FR_MAXLEN            (WLAN_HDR_ADDR3_LEN + WLAN_DATA_MAXLEN + \
+				     WLAN_CRC_LEN)
 
 #define WLAN_BEACON_FR_MAXLEN       WLAN_A3FR_MAXLEN
 #define WLAN_ATIM_FR_MAXLEN         (WLAN_HDR_ADDR3_LEN + 0)
diff --git a/drivers/staging/vt6655/80211mgr.c b/drivers/staging/vt6655/80211mgr.c
index 96b0d61623e4..7d2c6472ec9a 100644
--- a/drivers/staging/vt6655/80211mgr.c
+++ b/drivers/staging/vt6655/80211mgr.c
@@ -63,10 +63,6 @@
 
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-
-static int msglevel = MSG_LEVEL_INFO;
-/* static int          msglevel                =MSG_LEVEL_DEBUG; */
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -91,7 +87,7 @@ vMgrEncodeBeacon(
 	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
 	/* Fixed Fields */
-	pFrame->pqwTimestamp = (PQWORD)
+	pFrame->pqwTimestamp = (__le64 *)
 				(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 				 WLAN_BEACON_OFF_TS);
 	pFrame->pwBeaconInterval = (unsigned short *)
@@ -125,7 +121,7 @@ vMgrDecodeBeacon(
 	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
 	/* Fixed Fields */
-	pFrame->pqwTimestamp = (PQWORD)
+	pFrame->pqwTimestamp = (__le64 *)
 				(WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 				 WLAN_BEACON_OFF_TS);
 	pFrame->pwBeaconInterval = (unsigned short *)
@@ -218,9 +214,8 @@ vMgrDecodeBeacon(
 			break;
 
 		default:
-			DBG_PRT(MSG_LEVEL_DEBUG,
-				KERN_INFO "Unrecognized EID=%dd in beacon decode.\n",
-				pItem->byElementID);
+			pr_debug("Unrecognized EID=%dd in beacon decode\n",
+				 pItem->byElementID);
 			break;
 
 		}
@@ -406,9 +401,8 @@ vMgrDecodeAssocRequest(
 			break;
 
 		default:
-			DBG_PRT(MSG_LEVEL_DEBUG,
-				KERN_INFO "Unrecognized EID=%dd in assocreq decode.\n",
-				pItem->byElementID);
+			pr_debug("Unrecognized EID=%dd in assocreq decode\n",
+				 pItem->byElementID);
 			break;
 		}
 		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
@@ -489,9 +483,7 @@ vMgrDecodeAssocResponse(
 	if ((((unsigned char *)pItem) < (pFrame->pBuf + pFrame->len)) &&
 	    (pItem->byElementID == WLAN_EID_EXTSUPP_RATES)) {
 		pFrame->pExtSuppRates = (PWLAN_IE_SUPP_RATES)pItem;
-		DBG_PRT(MSG_LEVEL_DEBUG,
-			KERN_INFO "pFrame->pExtSuppRates=[%p].\n",
-			pItem);
+		pr_debug("pFrame->pExtSuppRates=[%p]\n", pItem);
 	} else {
 		pFrame->pExtSuppRates = NULL;
 	}
@@ -594,9 +586,8 @@ vMgrDecodeReassocRequest(
 						    (PWLAN_IE_SUPP_RATES)pItem;
 			break;
 		default:
-			DBG_PRT(MSG_LEVEL_DEBUG,
-				KERN_INFO "Unrecognized EID=%dd in reassocreq decode.\n",
-				pItem->byElementID);
+			pr_debug("Unrecognized EID=%dd in reassocreq decode\n",
+				 pItem->byElementID);
 			break;
 		}
 		pItem = (PWLAN_IE)(((unsigned char *)pItem) + 2 + pItem->len);
@@ -666,9 +657,8 @@ vMgrDecodeProbeRequest(
 			break;
 
 		default:
-			DBG_PRT(MSG_LEVEL_DEBUG,
-				KERN_INFO "Bad EID=%dd in probereq\n",
-				pItem->byElementID);
+			pr_debug("Bad EID=%dd in probereq\n",
+				 pItem->byElementID);
 			break;
 		}
 
@@ -695,7 +685,7 @@ vMgrEncodeProbeResponse(
 	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
 	/* Fixed Fields */
-	pFrame->pqwTimestamp = (PQWORD)
+	pFrame->pqwTimestamp = (__le64 *)
 			       (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 				WLAN_PROBERESP_OFF_TS);
 	pFrame->pwBeaconInterval = (unsigned short *)
@@ -730,7 +720,7 @@ vMgrDecodeProbeResponse(
 	pFrame->pHdr = (PUWLAN_80211HDR)pFrame->pBuf;
 
 	/* Fixed Fields */
-	pFrame->pqwTimestamp = (PQWORD)
+	pFrame->pqwTimestamp = (__le64 *)
 			       (WLAN_HDR_A3_DATA_PTR(&(pFrame->pHdr->sA3)) +
 				WLAN_PROBERESP_OFF_TS);
 	pFrame->pwBeaconInterval = (unsigned short *)
@@ -819,9 +809,8 @@ vMgrDecodeProbeResponse(
 			break;
 
 		default:
-			DBG_PRT(MSG_LEVEL_DEBUG,
-				KERN_INFO "Bad EID=%dd in proberesp\n",
-				pItem->byElementID);
+			pr_debug("Bad EID=%dd in proberesp\n",
+				 pItem->byElementID);
 			break;
 		}
 
diff --git a/drivers/staging/vt6655/80211mgr.h b/drivers/staging/vt6655/80211mgr.h
index 8b126bbd9fa5..d462a8af087b 100644
--- a/drivers/staging/vt6655/80211mgr.h
+++ b/drivers/staging/vt6655/80211mgr.h
@@ -31,23 +31,19 @@
 #ifndef __80211MGR_H__
 #define __80211MGR_H__
 
+#include <linux/types.h>
+#include "linux/ieee80211.h"
+
 #include "ttype.h"
 #include "80211hdr.h"
 
 #define WLAN_MIN_ARRAY          1
 
 /* Information Element ID value */
-#define WLAN_EID_SSID           0
-#define WLAN_EID_SUPP_RATES     1
 #define WLAN_EID_FH_PARMS       2
 #define WLAN_EID_DS_PARMS       3
 #define WLAN_EID_CF_PARMS       4
-#define WLAN_EID_TIM            5
 #define WLAN_EID_IBSS_PARMS     6
-#define WLAN_EID_COUNTRY        7
-#define WLAN_EID_CHALLENGE      16
-#define WLAN_EID_PWR_CONSTRAINT 32
-#define WLAN_EID_PWR_CAPABILITY 33
 #define WLAN_EID_TPC_REQ        34
 #define WLAN_EID_TPC_REP        35
 #define WLAN_EID_SUPP_CH        36
@@ -58,7 +54,6 @@
 #define WLAN_EID_IBSS_DFS       41
 #define WLAN_EID_ERP            42
 /* reference 802.11i 7.3.2 table 20 */
-#define WLAN_EID_RSN            48
 #define WLAN_EID_EXTSUPP_RATES  50
 /* reference WiFi WPA spec. */
 #define WLAN_EID_RSN_WPA        221
@@ -464,7 +459,7 @@ typedef struct tagWLAN_FR_BEACON {
 	unsigned int	len;
 	unsigned char *pBuf;
 	PUWLAN_80211HDR         pHdr;
-	PQWORD                  pqwTimestamp;
+	__le64 *pqwTimestamp;
 	unsigned short *pwBeaconInterval;
 	unsigned short *pwCapInfo;
 	PWLAN_IE_SSID           pSSID;
@@ -577,7 +572,7 @@ typedef struct tagWLAN_FR_PROBERESP {
 	unsigned int	len;
 	unsigned char *pBuf;
 	PUWLAN_80211HDR         pHdr;
-	PQWORD                  pqwTimestamp;
+	__le64 *pqwTimestamp;
 	unsigned short *pwBeaconInterval;
 	unsigned short *pwCapInfo;
 	PWLAN_IE_SSID           pSSID;
diff --git a/drivers/staging/vt6655/aes_ccmp.c b/drivers/staging/vt6655/aes_ccmp.c
index a25e6cf844ed..1dfcfcb3c69c 100644
--- a/drivers/staging/vt6655/aes_ccmp.c
+++ b/drivers/staging/vt6655/aes_ccmp.c
@@ -370,8 +370,5 @@ bool AESbGenCCMP(unsigned char *pbyRxKey, unsigned char *pbyFrame, unsigned shor
 	/* =>above is the dec-MIC from packet */
 	/* -------------------------------------------- */
 
-	if (!memcmp(abyMIC, abyTmp, 8))
-		return true;
-	else
-		return false;
+	return !memcmp(abyMIC, abyTmp, 8);
 }
diff --git a/drivers/staging/vt6655/baseband.c b/drivers/staging/vt6655/baseband.c
index f212b88c8cec..de54923e8861 100644
--- a/drivers/staging/vt6655/baseband.c
+++ b/drivers/staging/vt6655/baseband.c
@@ -56,10 +56,6 @@
 #include "srom.h"
 #include "rf.h"
 
-/*---------------------  Static Definitions -------------------------*/
-/* static int          msglevel                =MSG_LEVEL_DEBUG; */
-static int msglevel = MSG_LEVEL_INFO;
-
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -1704,25 +1700,26 @@ static unsigned char byVT3253B0_AGC[CB_VT3253B0_AGC][2] = {
 	{0xF0, 0x00},
 };
 
-static const unsigned short awcFrameTime[MAX_RATE] =
-{10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216};
+static const unsigned short awcFrameTime[MAX_RATE] = {
+		10, 20, 55, 110, 24, 36, 48, 72, 96, 144, 192, 216
+};
 
 /*---------------------  Static Functions  --------------------------*/
 
 static
 unsigned long
-s_ulGetRatio(PSDevice pDevice);
+s_ulGetRatio(struct vnt_private *pDevice);
 
 static
 void
 s_vChangeAntenna(
-	PSDevice pDevice
+	struct vnt_private *pDevice
 );
 
 static
 void
 s_vChangeAntenna(
-	PSDevice pDevice
+	struct vnt_private *pDevice
 )
 {
 	if (pDevice->dwRxAntennaSel == 0) {
@@ -1814,163 +1811,147 @@ BBuGetFrameTime(
  *
  * Parameters:
  *  In:
- *      pDevice         - Device Structure
- *      cbFrameLength   - Tx Frame Length
- *      wRate           - Tx Rate
+ *      priv         - Device Structure
+ *      frame_length   - Tx Frame Length
+ *      tx_rate           - Tx Rate
  *  Out:
- *      pwPhyLen        - pointer to Phy Length field
- *      pbyPhySrv       - pointer to Phy Service field
- *      pbyPhySgn       - pointer to Phy Signal field
+ *	struct vnt_phy_field *phy
+ *		- pointer to Phy Length field
+ *		- pointer to Phy Service field
+ *		- pointer to Phy Signal field
  *
  * Return Value: none
  *
  */
-void
-BBvCalculateParameter(
-	PSDevice pDevice,
-	unsigned int cbFrameLength,
-	unsigned short wRate,
-	unsigned char byPacketType,
-	unsigned short *pwPhyLen,
-	unsigned char *pbyPhySrv,
-	unsigned char *pbyPhySgn
-)
+void vnt_get_phy_field(struct vnt_private *priv, u32 frame_length,
+		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *phy)
 {
-	unsigned int cbBitCount;
-	unsigned int cbUsCount = 0;
-	unsigned int cbTmp;
-	bool bExtBit;
-	unsigned char byPreambleType = pDevice->byPreambleType;
-	bool bCCK = pDevice->bCCK;
+	u32 bit_count;
+	u32 count = 0;
+	u32 tmp;
+	int ext_bit;
+	u8 preamble_type = priv->byPreambleType;
 
-	cbBitCount = cbFrameLength * 8;
-	bExtBit = false;
+	bit_count = frame_length * 8;
+	ext_bit = false;
 
-	switch (wRate) {
+	switch (tx_rate) {
 	case RATE_1M:
-		cbUsCount = cbBitCount;
-		*pbyPhySgn = 0x00;
-		break;
+		count = bit_count;
+
+		phy->signal = 0x00;
 
-	case RATE_2M:
-		cbUsCount = cbBitCount / 2;
-		if (byPreambleType == 1)
-			*pbyPhySgn = 0x09;
-		else /* long preamble */
-			*pbyPhySgn = 0x01;
 		break;
+	case RATE_2M:
+		count = bit_count / 2;
+
+		if (preamble_type == 1)
+			phy->signal = 0x09;
+		else
+			phy->signal = 0x01;
 
-	case RATE_5M:
-		if (!bCCK)
-			cbBitCount++;
-		cbUsCount = (cbBitCount * 10) / 55;
-		cbTmp = (cbUsCount * 55) / 10;
-		if (cbTmp != cbBitCount)
-			cbUsCount++;
-		if (byPreambleType == 1)
-			*pbyPhySgn = 0x0a;
-		else /* long preamble */
-			*pbyPhySgn = 0x02;
 		break;
+	case RATE_5M:
+		count = (bit_count * 10) / 55;
+		tmp = (count * 55) / 10;
 
-	case RATE_11M:
+		if (tmp != bit_count)
+			count++;
+
+		if (preamble_type == 1)
+			phy->signal = 0x0a;
+		else
+			phy->signal = 0x02;
 
-		if (!bCCK)
-			cbBitCount++;
-		cbUsCount = cbBitCount / 11;
-		cbTmp = cbUsCount * 11;
-		if (cbTmp != cbBitCount) {
-			cbUsCount++;
-			if ((cbBitCount - cbTmp) <= 3)
-				bExtBit = true;
-		}
-		if (byPreambleType == 1)
-			*pbyPhySgn = 0x0b;
-		else /* long preamble */
-			*pbyPhySgn = 0x03;
 		break;
+	case RATE_11M:
+		count = bit_count / 11;
+		tmp = count * 11;
 
-	case RATE_6M:
-		if (byPacketType == PK_TYPE_11A) { /*11a, 5GHZ */
-			*pbyPhySgn = 0x9B; /* 1001 1011 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x8B; /* 1000 1011 */
+		if (tmp != bit_count) {
+			count++;
+
+			if ((bit_count - tmp) <= 3)
+				ext_bit = true;
 		}
+
+		if (preamble_type == 1)
+			phy->signal = 0x0b;
+		else
+			phy->signal = 0x03;
+
 		break;
+	case RATE_6M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9b;
+		else
+			phy->signal = 0x8b;
 
-	case RATE_9M:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x9F; /* 1001 1111 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x8F; /* 1000 1111 */
-		}
 		break;
+	case RATE_9M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9f;
+		else
+			phy->signal = 0x8f;
 
-	case RATE_12M:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x9A; /* 1001 1010 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x8A; /* 1000 1010 */
-		}
 		break;
+	case RATE_12M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9a;
+		else
+			phy->signal = 0x8a;
 
-	case RATE_18M:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x9E; /* 1001 1110 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x8E; /* 1000 1110 */
-		}
 		break;
+	case RATE_18M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9e;
+		else
+			phy->signal = 0x8e;
 
-	case RATE_24M:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x99; /* 1001 1001 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x89; /* 1000 1001 */
-		}
 		break;
+	case RATE_24M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x99;
+		else
+			phy->signal = 0x89;
 
-	case RATE_36M:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x9D; /* 1001 1101 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x8D; /* 1000 1101 */
-		}
 		break;
+	case RATE_36M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9d;
+		else
+			phy->signal = 0x8d;
 
-	case RATE_48M:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x98; /* 1001 1000 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x88; /* 1000 1000 */
-		}
 		break;
+	case RATE_48M:
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x98;
+		else
+			phy->signal = 0x88;
 
+		break;
 	case RATE_54M:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x9C; /* 1001 1100 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x8C; /* 1000 1100 */
-		}
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
 		break;
-
 	default:
-		if (byPacketType == PK_TYPE_11A) {/* 11a, 5GHZ */
-			*pbyPhySgn = 0x9C; /* 1001 1100 */
-		} else {/* 11g, 2.4GHZ */
-			*pbyPhySgn = 0x8C; /* 1000 1100 */
-		}
+		if (pkt_type == PK_TYPE_11A)
+			phy->signal = 0x9c;
+		else
+			phy->signal = 0x8c;
 		break;
 	}
 
-	if (byPacketType == PK_TYPE_11B) {
-		*pbyPhySrv = 0x00;
-		if (bExtBit)
-			*pbyPhySrv = *pbyPhySrv | 0x80;
-		*pwPhyLen = (unsigned short)cbUsCount;
+	if (pkt_type == PK_TYPE_11B) {
+		phy->service = 0x00;
+		if (ext_bit)
+			phy->service |= 0x80;
+		phy->len = cpu_to_le16((u16)count);
 	} else {
-		*pbyPhySrv = 0x00;
-		*pwPhyLen = (unsigned short)cbFrameLength;
+		phy->service = 0x00;
+		phy->len = cpu_to_le16((u16)frame_length);
 	}
 }
 
@@ -2009,7 +1990,7 @@ bool BBbReadEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned ch
 
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x30);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x30)\n");
+		pr_debug(" DBG_PORT80(0x30)\n");
 		return false;
 	}
 	return true;
@@ -2050,7 +2031,7 @@ bool BBbWriteEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned c
 
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x31);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x31)\n");
+		pr_debug(" DBG_PORT80(0x31)\n");
 		return false;
 	}
 	return true;
@@ -2115,7 +2096,7 @@ bool BBbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned ch
  *
  */
 
-bool BBbVT3253Init(PSDevice pDevice)
+bool BBbVT3253Init(struct vnt_private *pDevice)
 {
 	bool bResult = true;
 	int        ii;
@@ -2310,7 +2291,7 @@ void BBvReadAllRegs(void __iomem *dwIoBase, unsigned char *pbyBBRegs)
  *
  */
 
-void BBvLoopbackOn(PSDevice pDevice)
+void BBvLoopbackOn(struct vnt_private *pDevice)
 {
 	unsigned char byData;
 	void __iomem *dwIoBase = pDevice->PortOffset;
@@ -2363,7 +2344,7 @@ void BBvLoopbackOn(PSDevice pDevice)
  * Return Value: none
  *
  */
-void BBvLoopbackOff(PSDevice pDevice)
+void BBvLoopbackOff(struct vnt_private *pDevice)
 {
 	unsigned char byData;
 	void __iomem *dwIoBase = pDevice->PortOffset;
@@ -2398,7 +2379,7 @@ void BBvLoopbackOff(PSDevice pDevice)
  *
  */
 void
-BBvSetShortSlotTime(PSDevice pDevice)
+BBvSetShortSlotTime(struct vnt_private *pDevice)
 {
 	unsigned char byBBRxConf = 0;
 	unsigned char byBBVGA = 0;
@@ -2418,7 +2399,7 @@ BBvSetShortSlotTime(PSDevice pDevice)
 	BBbWriteEmbedded(pDevice->PortOffset, 0x0A, byBBRxConf); /* CR10 */
 }
 
-void BBvSetVGAGainOffset(PSDevice pDevice, unsigned char byData)
+void BBvSetVGAGainOffset(struct vnt_private *pDevice, unsigned char byData)
 {
 	unsigned char byBBRxConf = 0;
 
@@ -2594,7 +2575,7 @@ BBvExitDeepSleep(void __iomem *dwIoBase, unsigned char byLocalID)
 
 static
 unsigned long
-s_ulGetRatio(PSDevice pDevice)
+s_ulGetRatio(struct vnt_private *pDevice)
 {
 	unsigned long ulRatio = 0;
 	unsigned long ulMaxPacket;
@@ -2689,7 +2670,7 @@ s_ulGetRatio(PSDevice pDevice)
 }
 
 void
-BBvClearAntDivSQ3Value(PSDevice pDevice)
+BBvClearAntDivSQ3Value(struct vnt_private *pDevice)
 {
 	unsigned int ii;
 
@@ -2713,8 +2694,8 @@ BBvClearAntDivSQ3Value(PSDevice pDevice)
  *
  */
 
-void
-BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ3)
+void BBvAntennaDiversity(struct vnt_private *pDevice,
+			 unsigned char byRxRate, unsigned char bySQ3)
 {
 	if ((byRxRate >= MAX_RATE) || (pDevice->wAntDiversityMaxRate >= MAX_RATE))
 		return;
@@ -2725,18 +2706,22 @@ BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ
 
 	if (pDevice->byAntennaState == 0) {
 		if (pDevice->uDiversityCnt > pDevice->ulDiversityNValue) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ulDiversityNValue=[%d],54M-[%d]\n",
-				(int)pDevice->ulDiversityNValue, (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate]);
+			pr_debug("ulDiversityNValue=[%d],54M-[%d]\n",
+				 (int)pDevice->ulDiversityNValue,
+				 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate]);
 
 			if (pDevice->uNumSQ3[pDevice->wAntDiversityMaxRate] < pDevice->uDiversityCnt/2) {
 				pDevice->ulRatio_State0 = s_ulGetRatio(pDevice);
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SQ3_State0, rate = [%08x]\n", (int)pDevice->ulRatio_State0);
+				pr_debug("SQ3_State0, rate = [%08x]\n",
+					 (int)pDevice->ulRatio_State0);
 
 				if (pDevice->byTMax == 0)
 					return;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1.[%08x], uNumSQ3[%d]=%d, %d\n",
-					(int)pDevice->ulRatio_State0, (int)pDevice->wAntDiversityMaxRate,
-					(int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
+				pr_debug("1.[%08x], uNumSQ3[%d]=%d, %d\n",
+					 (int)pDevice->ulRatio_State0,
+					 (int)pDevice->wAntDiversityMaxRate,
+					 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate],
+					 (int)pDevice->uDiversityCnt);
 
 				s_vChangeAntenna(pDevice);
 				pDevice->byAntennaState = 1;
@@ -2758,14 +2743,17 @@ BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ
 			del_timer(&pDevice->TimerSQ3Tmax1);
 
 			pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "RX:SQ3_State1, rate0 = %08x,rate1 = %08x\n",
-				(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1);
+			pr_debug("RX:SQ3_State1, rate0 = %08x,rate1 = %08x\n",
+				 (int)pDevice->ulRatio_State0,
+				 (int)pDevice->ulRatio_State1);
 
 			if (pDevice->ulRatio_State1 < pDevice->ulRatio_State0) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
-					(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
-					(int)pDevice->wAntDiversityMaxRate,
-					(int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
+				pr_debug("2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
+					 (int)pDevice->ulRatio_State0,
+					 (int)pDevice->ulRatio_State1,
+					 (int)pDevice->wAntDiversityMaxRate,
+					 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate],
+					 (int)pDevice->uDiversityCnt);
 
 				s_vChangeAntenna(pDevice);
 				pDevice->TimerSQ3Tmax3.expires =  RUN_AT(pDevice->byTMax3 * HZ);
@@ -2798,12 +2786,14 @@ TimerSQ3CallBack(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TimerSQ3CallBack...");
+	pr_debug("TimerSQ3CallBack...\n");
 	spin_lock_irq(&pDevice->lock);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "3.[%08x][%08x], %d\n", (int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1, (int)pDevice->uDiversityCnt);
+	pr_debug("3.[%08x][%08x], %d\n",
+		 (int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
+		 (int)pDevice->uDiversityCnt);
 
 	s_vChangeAntenna(pDevice);
 	pDevice->byAntennaState = 0;
@@ -2840,9 +2830,9 @@ TimerState1CallBack(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TimerState1CallBack...");
+	pr_debug("TimerState1CallBack...\n");
 
 	spin_lock_irq(&pDevice->lock);
 	if (pDevice->uDiversityCnt < pDevice->ulDiversityMValue/100) {
@@ -2853,14 +2843,17 @@ TimerState1CallBack(
 		add_timer(&pDevice->TimerSQ3Tmax2);
 	} else {
 		pDevice->ulRatio_State1 = s_ulGetRatio(pDevice);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SQ3_State1, rate0 = %08x,rate1 = %08x\n",
-			(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1);
+		pr_debug("SQ3_State1, rate0 = %08x,rate1 = %08x\n",
+			 (int)pDevice->ulRatio_State0,
+			 (int)pDevice->ulRatio_State1);
 
 		if (pDevice->ulRatio_State1 < pDevice->ulRatio_State0) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
-				(int)pDevice->ulRatio_State0, (int)pDevice->ulRatio_State1,
-				(int)pDevice->wAntDiversityMaxRate,
-				(int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate], (int)pDevice->uDiversityCnt);
+			pr_debug("2.[%08x][%08x], uNumSQ3[%d]=%d, %d\n",
+				 (int)pDevice->ulRatio_State0,
+				 (int)pDevice->ulRatio_State1,
+				 (int)pDevice->wAntDiversityMaxRate,
+				 (int)pDevice->uNumSQ3[(int)pDevice->wAntDiversityMaxRate],
+				 (int)pDevice->uDiversityCnt);
 
 			s_vChangeAntenna(pDevice);
 
diff --git a/drivers/staging/vt6655/baseband.h b/drivers/staging/vt6655/baseband.h
index fcf1f9373672..31f2255519cf 100644
--- a/drivers/staging/vt6655/baseband.h
+++ b/drivers/staging/vt6655/baseband.h
@@ -34,14 +34,14 @@
 #include "tether.h"
 #include "device.h"
 
-//
-// Registers in the BASEBAND
-//
+/*
+ * Registers in the BASEBAND
+ */
 #define BB_MAX_CONTEXT_SIZE 256
 
-//
-// Baseband RF pair definition in eeprom (Bits 6..0)
-//
+/*
+ * Baseband RF pair definition in eeprom (Bits 6..0)
+ */
 
 #define PREAMBLE_LONG   0
 #define PREAMBLE_SHORT  1
@@ -76,30 +76,22 @@ BBuGetFrameTime(
 	unsigned short wRate
 );
 
-void
-BBvCalculateParameter(
-	PSDevice pDevice,
-	unsigned int cbFrameLength,
-	unsigned short wRate,
-	unsigned char byPacketType,
-	unsigned short *pwPhyLen,
-	unsigned char *pbyPhySrv,
-	unsigned char *pbyPhySgn
-);
+void vnt_get_phy_field(struct vnt_private *, u32 frame_length,
+		       u16 tx_rate, u8 pkt_type, struct vnt_phy_field *);
 
 bool BBbReadEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char *pbyData);
 bool BBbWriteEmbedded(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byData);
 
 void BBvReadAllRegs(void __iomem *dwIoBase, unsigned char *pbyBBRegs);
-void BBvLoopbackOn(PSDevice pDevice);
-void BBvLoopbackOff(PSDevice pDevice);
-void BBvSetShortSlotTime(PSDevice pDevice);
+void BBvLoopbackOn(struct vnt_private *pDevice);
+void BBvLoopbackOff(struct vnt_private *pDevice);
+void BBvSetShortSlotTime(struct vnt_private *pDevice);
 bool BBbIsRegBitsOn(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byTestBits);
 bool BBbIsRegBitsOff(void __iomem *dwIoBase, unsigned char byBBAddr, unsigned char byTestBits);
-void BBvSetVGAGainOffset(PSDevice pDevice, unsigned char byData);
+void BBvSetVGAGainOffset(struct vnt_private *pDevice, unsigned char byData);
 
-// VT3253 Baseband
-bool BBbVT3253Init(PSDevice pDevice);
+/* VT3253 Baseband */
+bool BBbVT3253Init(struct vnt_private *pDevice);
 void BBvSoftwareReset(void __iomem *dwIoBase);
 void BBvPowerSaveModeON(void __iomem *dwIoBase);
 void BBvPowerSaveModeOFF(void __iomem *dwIoBase);
@@ -108,7 +100,7 @@ void BBvSetRxAntennaMode(void __iomem *dwIoBase, unsigned char byAntennaMode);
 void BBvSetDeepSleep(void __iomem *dwIoBase, unsigned char byLocalID);
 void BBvExitDeepSleep(void __iomem *dwIoBase, unsigned char byLocalID);
 
-// timer for antenna diversity
+/* timer for antenna diversity */
 
 void
 TimerSQ3CallBack(
@@ -120,8 +112,9 @@ TimerState1CallBack(
 	void *hDeviceContext
 );
 
-void BBvAntennaDiversity(PSDevice pDevice, unsigned char byRxRate, unsigned char bySQ3);
+void BBvAntennaDiversity(struct vnt_private *pDevice,
+			 unsigned char byRxRate, unsigned char bySQ3);
 void
-BBvClearAntDivSQ3Value(PSDevice pDevice);
+BBvClearAntDivSQ3Value(struct vnt_private *pDevice);
 
-#endif // __BASEBAND_H__
+#endif /* __BASEBAND_H__ */
diff --git a/drivers/staging/vt6655/bssdb.c b/drivers/staging/vt6655/bssdb.c
index 9569f43a3ed7..996d3302ce3d 100644
--- a/drivers/staging/vt6655/bssdb.c
+++ b/drivers/staging/vt6655/bssdb.c
@@ -63,8 +63,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
-
 static const unsigned short awHWRetry0[5][5] = {
 	{RATE_18M, RATE_18M, RATE_12M, RATE_12M, RATE_12M},
 	{RATE_24M, RATE_24M, RATE_18M, RATE_12M, RATE_12M},
@@ -117,7 +115,7 @@ BSSpSearchBSSList(
 	CARD_PHY_TYPE  ePhyType
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned char *pbyBSSID = NULL;
 	PWLAN_IE_SSID   pSSID = NULL;
@@ -127,8 +125,7 @@ BSSpSearchBSSList(
 	unsigned int ii = 0;
 
 	if (pbyDesireBSSID != NULL) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-			"BSSpSearchBSSList BSSID[%pM]\n", pbyDesireBSSID);
+		pr_debug("BSSpSearchBSSList BSSID[%pM]\n", pbyDesireBSSID);
 		if ((!is_broadcast_ether_addr(pbyDesireBSSID)) &&
 		    (memcmp(pbyDesireBSSID, ZeroBSSID, 6) != 0))
 			pbyBSSID = pbyDesireBSSID;
@@ -194,7 +191,9 @@ BSSpSearchBSSList(
 				    ((pMgmt->eConfigMode == WMAC_CONFIG_ESS_STA) && WLAN_GET_CAP_INFO_IBSS(pCurrBSS->wCapInfo))
 ) {
 					/* Type not match skip this BSS */
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSS type mismatch.... Config[%d] BSS[0x%04x]\n", pMgmt->eConfigMode, pCurrBSS->wCapInfo);
+					pr_debug("BSS type mismatch.... Config[%d] BSS[0x%04x]\n",
+						 pMgmt->eConfigMode,
+						 pCurrBSS->wCapInfo);
 					continue;
 				}
 
@@ -202,7 +201,9 @@ BSSpSearchBSSList(
 					if (((ePhyType == PHY_TYPE_11A) && (PHY_TYPE_11A != pCurrBSS->eNetworkTypeInUse)) ||
 					    ((ePhyType != PHY_TYPE_11A) && (PHY_TYPE_11A == pCurrBSS->eNetworkTypeInUse))) {
 						/* PhyType not match skip this BSS */
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Physical type mismatch.... ePhyType[%d] BSS[%d]\n", ePhyType, pCurrBSS->eNetworkTypeInUse);
+						pr_debug("Physical type mismatch.... ePhyType[%d] BSS[%d]\n",
+							 ePhyType,
+							 pCurrBSS->eNetworkTypeInUse);
 						continue;
 					}
 				}
@@ -240,7 +241,7 @@ BSSvClearBSSList(
 	bool bKeepCurrBSSID
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int ii;
 
@@ -280,7 +281,7 @@ BSSpAddrIsInBSSList(
 	PWLAN_IE_SSID pSSID
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PKnownBSS       pBSSList = NULL;
 	unsigned int ii;
@@ -316,7 +317,7 @@ bool
 BSSbInsertToBSSList(
 	void *hDeviceContext,
 	unsigned char *abyBSSIDAddr,
-	QWORD qwTimestamp,
+	__le64 qwTimestamp,
 	unsigned short wBeaconInterval,
 	unsigned short wCapInfo,
 	unsigned char byCurrChannel,
@@ -333,7 +334,7 @@ BSSbInsertToBSSList(
 	void *pRxPacketContext
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
 	PKnownBSS       pBSSList = NULL;
@@ -350,14 +351,13 @@ BSSbInsertToBSSList(
 	}
 
 	if (ii == MAX_BSS_NUM) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get free KnowBSS node failed.\n");
+		pr_debug("Get free KnowBSS node failed\n");
 		return false;
 	}
 	/* save the BSS info */
 	pBSSList->bActive = true;
 	memcpy(pBSSList->abyBSSID, abyBSSIDAddr, WLAN_BSSID_LEN);
-	HIDWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(HIDWORD(qwTimestamp));
-	LODWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(LODWORD(qwTimestamp));
+	pBSSList->qwBSSTimestamp = le64_to_cpu(qwTimestamp);
 	pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
 	pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
 	pBSSList->uClearCount = 0;
@@ -376,7 +376,8 @@ BSSbInsertToBSSList(
 		if (pExtSuppRates->len > WLAN_RATES_MAXLEN)
 			pExtSuppRates->len = WLAN_RATES_MAXLEN;
 		memcpy(pBSSList->abyExtSuppRates, pExtSuppRates, pExtSuppRates->len + WLAN_IEHDR_LEN);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSbInsertToBSSList: pExtSuppRates->len = %d\n", pExtSuppRates->len);
+		pr_debug("BSSbInsertToBSSList: pExtSuppRates->len = %d\n",
+			 pExtSuppRates->len);
 
 	} else {
 		memset(pBSSList->abyExtSuppRates, 0, WLAN_IEHDR_LEN + WLAN_RATES_MAXLEN + 1);
@@ -521,7 +522,7 @@ BSSbInsertToBSSList(
 bool
 BSSbUpdateToBSSList(
 	void *hDeviceContext,
-	QWORD qwTimestamp,
+	__le64 qwTimestamp,
 	unsigned short wBeaconInterval,
 	unsigned short wCapInfo,
 	unsigned char byCurrChannel,
@@ -541,7 +542,7 @@ BSSbUpdateToBSSList(
 )
 {
 	int             ii;
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PSRxMgmtPacket  pRxPacket = (PSRxMgmtPacket)pRxPacketContext;
 	long            ldBm;
@@ -551,8 +552,7 @@ BSSbUpdateToBSSList(
 	if (pBSSList == NULL)
 		return false;
 
-	HIDWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(HIDWORD(qwTimestamp));
-	LODWORD(pBSSList->qwBSSTimestamp) = cpu_to_le32(LODWORD(qwTimestamp));
+	pBSSList->qwBSSTimestamp = le64_to_cpu(qwTimestamp);
 	pBSSList->wBeaconInterval = cpu_to_le16(wBeaconInterval);
 	pBSSList->wCapInfo = cpu_to_le16(wCapInfo);
 	pBSSList->uClearCount = 0;
@@ -717,7 +717,7 @@ BSSDBbIsSTAInNodeDB(void *pMgmtObject, unsigned char *abyDstAddr,
 void
 BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int ii;
 	unsigned int BigestCount = 0;
@@ -742,7 +742,7 @@ BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
 	/* if not found replace uInActiveCount is largest one */
 	if (ii == (MAX_NODE_NUM + 1)) {
 		*puNodeIndex = SelectIndex;
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Replace inactive node = %d\n", SelectIndex);
+		pr_info("Replace inactive node = %d\n", SelectIndex);
 		/* clear ps buffer */
 		if (pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue.next != NULL) {
 			while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue)) != NULL)
@@ -759,7 +759,7 @@ BSSvCreateOneNode(void *hDeviceContext, unsigned int *puNodeIndex)
 	skb_queue_head_init(&pMgmt->sNodeDBTable[*puNodeIndex].sTxPSQueue);
 	pMgmt->sNodeDBTable[*puNodeIndex].byAuthSequence = 0;
 	pMgmt->sNodeDBTable[*puNodeIndex].wEnQueueCnt = 0;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create node index = %d\n", ii);
+	pr_debug("Create node index = %d\n", ii);
 	return;
 };
 
@@ -779,7 +779,7 @@ BSSvRemoveOneNode(
 	unsigned int uNodeIndex
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned char byMask[8] = {1, 2, 4, 8, 0x10, 0x20, 0x40, 0x80};
 	struct sk_buff  *skb;
@@ -812,7 +812,7 @@ BSSvUpdateAPNode(
 	PWLAN_IE_SUPP_RATES pExtSuppRates
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uRateLen = WLAN_RATES_MAXLEN;
 
@@ -844,7 +844,8 @@ BSSvUpdateAPNode(
 	netdev_dbg(pDevice->dev, "BSSvUpdateAPNode:MaxSuppRate is %d\n",
 		   pMgmt->sNodeDBTable[0].wMaxSuppRate);
 	/* auto rate fallback function initiation */
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->sNodeDBTable[0].wTxDataRate = %d\n", pMgmt->sNodeDBTable[0].wTxDataRate);
+	pr_debug("pMgmt->sNodeDBTable[0].wTxDataRate = %d\n",
+		 pMgmt->sNodeDBTable[0].wTxDataRate);
 };
 
 /*+
@@ -863,7 +864,7 @@ BSSvAddMulticastNode(
 	void *hDeviceContext
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 
 	if (!pDevice->bEnableHostWEP)
@@ -906,7 +907,7 @@ BSSvSecondCallBack(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int ii;
 	PWLAN_IE_SSID   pItemSSID, pCurrSSID;
@@ -961,8 +962,8 @@ BSSvSecondCallBack(
 			if (ii > 0) {
 				if (pMgmt->sNodeDBTable[ii].uInActiveCount > MAX_INACTIVE_COUNT) {
 					BSSvRemoveOneNode(pDevice, ii);
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-						"Inactive timeout [%d] sec, STA index = [%d] remove\n", MAX_INACTIVE_COUNT, ii);
+					pr_debug("Inactive timeout [%d] sec, STA index = [%d] remove\n",
+						 MAX_INACTIVE_COUNT, ii);
 					continue;
 				}
 
@@ -1012,11 +1013,13 @@ BSSvSecondCallBack(
 
 			/* check if pending PS queue */
 			if (pMgmt->sNodeDBTable[ii].wEnQueueCnt != 0) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index= %d, Queue = %d pending\n",
-					ii, pMgmt->sNodeDBTable[ii].wEnQueueCnt);
+				pr_debug("Index= %d, Queue = %d pending\n",
+					 ii,
+					 pMgmt->sNodeDBTable[ii].wEnQueueCnt);
 				if ((ii > 0) && (pMgmt->sNodeDBTable[ii].wEnQueueCnt > 15)) {
 					BSSvRemoveOneNode(pDevice, ii);
-					DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Pending many queues PS STA Index = %d remove\n", ii);
+					pr_info("Pending many queues PS STA Index = %d remove\n",
+						ii);
 					continue;
 				}
 			}
@@ -1100,7 +1103,8 @@ BSSvSecondCallBack(
 				netif_stop_queue(pDevice->dev);
 				pDevice->bLinkPass = false;
 				pDevice->bRoaming = true;
-				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Lost AP beacon [%d] sec, disconnected !\n", pMgmt->sNodeDBTable[0].uInActiveCount);
+				pr_info("Lost AP beacon [%d] sec, disconnected !\n",
+					pMgmt->sNodeDBTable[0].uInActiveCount);
 				if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
 					wpahdr = (viawget_wpa_header *)pDevice->skb->data;
 					wpahdr->type = VIAWGET_DISASSOC_MSG;
@@ -1145,7 +1149,7 @@ BSSvSecondCallBack(
 				if (pDevice->bWPADEVUp)
 					pDevice->eEncryptionStatus = pDevice->eOldEncryptionStatus;
 
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Roaming ...\n");
+				pr_debug("Roaming ...\n");
 				BSSvClearBSSList((void *)pDevice, pDevice->bLinkPass);
 				pMgmt->eScanType = WMAC_SCAN_ACTIVE;
 				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, pMgmt->abyDesireSSID);
@@ -1161,7 +1165,7 @@ BSSvSecondCallBack(
 			if (pDevice->uAutoReConnectTime < 10) {
 				pDevice->uAutoReConnectTime++;
 			} else {
-				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Adhoc re-scanning ...\n");
+				pr_info("Adhoc re-scanning ...\n");
 				pMgmt->eScanType = WMAC_SCAN_ACTIVE;
 				bScheduleCommand((void *)pDevice, WLAN_CMD_BSSID_SCAN, NULL);
 				bScheduleCommand((void *)pDevice, WLAN_CMD_SSID, NULL);
@@ -1172,7 +1176,8 @@ BSSvSecondCallBack(
 			if (pDevice->bUpdateBBVGA)
 				s_vCheckPreEDThreshold((void *)pDevice);
 			if (pMgmt->sNodeDBTable[0].uInActiveCount >= ADHOC_LOST_BEACON_COUNT) {
-				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Lost other STA beacon [%d] sec, started !\n", pMgmt->sNodeDBTable[0].uInActiveCount);
+				pr_info("Lost other STA beacon [%d] sec, started !\n",
+					pMgmt->sNodeDBTable[0].uInActiveCount);
 				pMgmt->sNodeDBTable[0].uInActiveCount = 0;
 				pMgmt->eCurrState = WMAC_STATE_STARTED;
 				netif_stop_queue(pDevice->dev);
@@ -1209,7 +1214,7 @@ BSSvUpdateNodeTxCounter(
 	unsigned int uFIFOHeaderSize
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uNodeIndex = 0;
 	unsigned char byTxRetry = (byTsr0 & TSR0_NCR);
@@ -1231,7 +1236,8 @@ BSSvUpdateNodeTxCounter(
 
 	/* Only Unicast using support rates */
 	if (pTxBufHead->wFIFOCtl & FIFOCTL_NEEDACK) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wRate %04X, byTsr0 %02X, byTsr1 %02X\n", wRate, byTsr0, byTsr1);
+		pr_debug("wRate %04X, byTsr0 %02X, byTsr1 %02X\n",
+			 wRate, byTsr0, byTsr1);
 		if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
 			pMgmt->sNodeDBTable[0].uTxAttempts += 1;
 			if ((byTsr1 & TSR1_TERR) == 0) {
@@ -1362,7 +1368,7 @@ BSSvClearNodeDBTable(
 )
 
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	struct sk_buff  *skb;
 	unsigned int ii;
@@ -1372,7 +1378,7 @@ BSSvClearNodeDBTable(
 			/* check if sTxPSQueue has been initial */
 			if (pMgmt->sNodeDBTable[ii].sTxPSQueue.next != NULL) {
 				while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS skb != NULL %d\n", ii);
+					pr_debug("PS skb != NULL %d\n", ii);
 					dev_kfree_skb(skb);
 				}
 			}
@@ -1387,7 +1393,7 @@ void s_vCheckSensitivity(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PKnownBSS       pBSSList = NULL;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	int             ii;
@@ -1414,7 +1420,10 @@ void s_vCheckSensitivity(
 			if (uNumofdBm > 0) {
 				LocalldBmAverage = LocalldBmAverage/uNumofdBm;
 				for (ii = 0; ii < BB_VGA_LEVEL; ii++) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LocalldBmAverage:%ld, %ld %02x\n", LocalldBmAverage, pDevice->ldBmThreshold[ii], pDevice->abyBBVGA[ii]);
+					pr_debug("LocalldBmAverage:%ld, %ld %02x\n",
+						 LocalldBmAverage,
+						 pDevice->ldBmThreshold[ii],
+						 pDevice->abyBBVGA[ii]);
 					if (LocalldBmAverage < pDevice->ldBmThreshold[ii]) {
 						pDevice->byBBVGANew = pDevice->abyBBVGA[ii];
 						break;
@@ -1437,7 +1446,7 @@ BSSvClearAnyBSSJoinRecord(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int ii;
 
@@ -1450,7 +1459,7 @@ void s_uCalculateLinkQual(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	unsigned long TxOkRatio, TxCnt;
 	unsigned long RxOkRatio, RxCnt;
 	unsigned long RssiRatio;
@@ -1490,7 +1499,7 @@ void s_vCheckPreEDThreshold(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PKnownBSS       pBSSList = NULL;
 	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
 
diff --git a/drivers/staging/vt6655/bssdb.h b/drivers/staging/vt6655/bssdb.h
index bf299e3b8acf..5d4dd28b6223 100644
--- a/drivers/staging/vt6655/bssdb.h
+++ b/drivers/staging/vt6655/bssdb.h
@@ -131,8 +131,8 @@ typedef struct tagKnownBSS {
 
 	unsigned int	uClearCount;
 	unsigned int	uIELength;
-	QWORD           qwBSSTimestamp;
-	QWORD           qwLocalTSF;
+	u64 qwBSSTimestamp;
+	u64 qwLocalTSF;
 
 	CARD_PHY_TYPE   eNetworkTypeInUse;
 
@@ -233,7 +233,7 @@ bool
 BSSbInsertToBSSList(
 	void *hDeviceContext,
 	unsigned char *abyBSSIDAddr,
-	QWORD qwTimestamp,
+	__le64 qwTimestamp,
 	unsigned short wBeaconInterval,
 	unsigned short wCapInfo,
 	unsigned char byCurrChannel,
@@ -253,7 +253,7 @@ BSSbInsertToBSSList(
 bool
 BSSbUpdateToBSSList(
 	void *hDeviceContext,
-	QWORD qwTimestamp,
+	__le64 qwTimestamp,
 	unsigned short wBeaconInterval,
 	unsigned short wCapInfo,
 	unsigned char byCurrChannel,
diff --git a/drivers/staging/vt6655/card.c b/drivers/staging/vt6655/card.c
index 4ae8d9362edf..5a6950264bdc 100644
--- a/drivers/staging/vt6655/card.c
+++ b/drivers/staging/vt6655/card.c
@@ -59,8 +59,6 @@
 
 /*---------------------  Static Definitions -------------------------*/
 
-static int msglevel = MSG_LEVEL_INFO;
-
 #define C_SIFS_A        16      // micro sec.
 #define C_SIFS_BG       10
 
@@ -224,59 +222,54 @@ s_vCalculateOFDMRParameter(
  */
 static
 void
-s_vSetRSPINF(PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, void *pvExtSupportRateIEs)
+s_vSetRSPINF(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType,
+	     void *pvSupportRateIEs, void *pvExtSupportRateIEs)
 {
-	unsigned char byServ = 0, bySignal = 0; // For CCK
-	unsigned short wLen = 0;
+	union vnt_phy_field_swap phy;
 	unsigned char byTxRate = 0, byRsvTime = 0;    // For OFDM
 
 	//Set to Page1
 	MACvSelectPage1(pDevice->PortOffset);
 
-	//RSPINF_b_1
-	BBvCalculateParameter(pDevice,
-			      14,
-			      VNTWIFIbyGetACKTxRate(RATE_1M, pvSupportRateIEs, pvExtSupportRateIEs),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	/* RSPINF_b_1 */
+	vnt_get_phy_field(pDevice,
+			  14,
+			  VNTWIFIbyGetACKTxRate(RATE_1M, pvSupportRateIEs, pvExtSupportRateIEs),
+			  PK_TYPE_11B,
+			  &phy.field_read);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
-	///RSPINF_b_2
-	BBvCalculateParameter(pDevice,
-			      14,
-			      VNTWIFIbyGetACKTxRate(RATE_2M, pvSupportRateIEs, pvExtSupportRateIEs),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	 /* swap over to get correct write order */
+	swap(phy.swap[0], phy.swap[1]);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
-	//RSPINF_b_5
-	BBvCalculateParameter(pDevice,
-			      14,
-			      VNTWIFIbyGetACKTxRate(RATE_5M, pvSupportRateIEs, pvExtSupportRateIEs),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, phy.field_write);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
-	//RSPINF_b_11
-	BBvCalculateParameter(pDevice,
-			      14,
-			      VNTWIFIbyGetACKTxRate(RATE_11M, pvSupportRateIEs, pvExtSupportRateIEs),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	/* RSPINF_b_2 */
+	vnt_get_phy_field(pDevice, 14,
+			  VNTWIFIbyGetACKTxRate(RATE_2M, pvSupportRateIEs, pvExtSupportRateIEs),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, phy.field_write);
+
+	/* RSPINF_b_5 */
+	vnt_get_phy_field(pDevice, 14,
+			  VNTWIFIbyGetACKTxRate(RATE_5M, pvSupportRateIEs, pvExtSupportRateIEs),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, phy.field_write);
+
+	/* RSPINF_b_11 */
+	vnt_get_phy_field(pDevice, 14,
+			  VNTWIFIbyGetACKTxRate(RATE_11M, pvSupportRateIEs, pvExtSupportRateIEs),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, phy.field_write);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
 	//RSPINF_a_6
 	s_vCalculateOFDMRParameter(RATE_6M,
 				   ePHYType,
@@ -348,9 +341,8 @@ s_vSetRSPINF(PSDevice pDevice, CARD_PHY_TYPE ePHYType, void *pvSupportRateIEs, v
  * Return Value: true if short preamble; otherwise false
  *
  */
-bool CARDbIsShortPreamble(void *pDeviceHandler)
+bool CARDbIsShortPreamble(struct vnt_private *pDevice)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	if (pDevice->byPreambleType == 0)
 		return false;
@@ -370,9 +362,8 @@ bool CARDbIsShortPreamble(void *pDeviceHandler)
  * Return Value: true if short slot time; otherwise false
  *
  */
-bool CARDbIsShorSlotTime(void *pDeviceHandler)
+bool CARDbIsShorSlotTime(struct vnt_private *pDevice)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	return pDevice->bShortSlotTime;
 }
@@ -389,9 +380,10 @@ bool CARDbIsShorSlotTime(void *pDeviceHandler)
  * Return Value: None.
  *
  */
-bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned short wCapInfo, unsigned char byERPField, void *pvSupportRateIEs, void *pvExtSupportRateIEs)
+bool CARDbSetPhyParameter(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType,
+			  unsigned short wCapInfo, unsigned char byERPField,
+			  void *pvSupportRateIEs, void *pvExtSupportRateIEs)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	unsigned char byCWMaxMin = 0;
 	unsigned char bySlot = 0;
 	unsigned char bySIFS = 0;
@@ -573,21 +565,17 @@ bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned
  * Return Value: none
  *
  */
-bool CARDbUpdateTSF(void *pDeviceHandler, unsigned char byRxRate, QWORD qwBSSTimestamp, QWORD qwLocalTSF)
+bool CARDbUpdateTSF(struct vnt_private *pDevice, unsigned char byRxRate,
+		    u64 qwBSSTimestamp, u64 qwLocalTSF)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
-	QWORD       qwTSFOffset;
+	u64 qwTSFOffset = 0;
 
-	HIDWORD(qwTSFOffset) = 0;
-	LODWORD(qwTSFOffset) = 0;
-
-	if ((HIDWORD(qwBSSTimestamp) != HIDWORD(qwLocalTSF)) ||
-	    (LODWORD(qwBSSTimestamp) != LODWORD(qwLocalTSF))) {
+	if (qwBSSTimestamp != qwLocalTSF) {
 		qwTSFOffset = CARDqGetTSFOffset(byRxRate, qwBSSTimestamp, qwLocalTSF);
 		// adjust TSF
 		// HW's TSF add TSF Offset reg
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST, LODWORD(qwTSFOffset));
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST + 4, HIDWORD(qwTSFOffset));
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST, (u32)qwTSFOffset);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_TSFOFST + 4, (u32)(qwTSFOffset >> 32));
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TSFSYNCEN);
 	}
 	return true;
@@ -607,40 +595,21 @@ bool CARDbUpdateTSF(void *pDeviceHandler, unsigned char byRxRate, QWORD qwBSSTim
  * Return Value: true if succeed; otherwise false
  *
  */
-bool CARDbSetBeaconPeriod(void *pDeviceHandler, unsigned short wBeaconInterval)
+bool CARDbSetBeaconPeriod(struct vnt_private *pDevice,
+			  unsigned short wBeaconInterval)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
-	unsigned int uBeaconInterval = 0;
-	unsigned int uLowNextTBTT = 0;
-	unsigned int uHighRemain = 0;
-	unsigned int uLowRemain = 0;
-	QWORD       qwNextTBTT;
-
-	HIDWORD(qwNextTBTT) = 0;
-	LODWORD(qwNextTBTT) = 0;
+	u64 qwNextTBTT = 0;
+
 	CARDbGetCurrentTSF(pDevice->PortOffset, &qwNextTBTT); //Get Local TSF counter
-	uBeaconInterval = wBeaconInterval * 1024;
-	// Next TBTT = ((local_current_TSF / beacon_interval) + 1) * beacon_interval
-	uLowNextTBTT = (LODWORD(qwNextTBTT) >> 10) << 10;
-	uLowRemain = (uLowNextTBTT) % uBeaconInterval;
-	// high dword (mod) bcn
-	uHighRemain = (((0xffffffff % uBeaconInterval) + 1) * HIDWORD(qwNextTBTT))
-		% uBeaconInterval;
-	uLowRemain = (uHighRemain + uLowRemain) % uBeaconInterval;
-	uLowRemain = uBeaconInterval - uLowRemain;
-
-	// check if carry when add one beacon interval
-	if ((~uLowNextTBTT) < uLowRemain)
-		HIDWORD(qwNextTBTT)++;
-
-	LODWORD(qwNextTBTT) = uLowNextTBTT + uLowRemain;
+
+	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
 
 	// set HW beacon interval
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, wBeaconInterval);
 	pDevice->wBeaconInterval = wBeaconInterval;
 	// Set NextTBTT
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT, LODWORD(qwNextTBTT));
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT + 4, HIDWORD(qwNextTBTT));
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT, (u32)qwNextTBTT);
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_NEXTTBTT + 4, (u32)(qwNextTBTT >> 32));
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
 
 	return true;
@@ -659,9 +628,8 @@ bool CARDbSetBeaconPeriod(void *pDeviceHandler, unsigned short wBeaconInterval)
  * Return Value: true if all data packet complete; otherwise false.
  *
  */
-bool CARDbStopTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
+bool CARDbStopTxPacket(struct vnt_private *pDevice, CARD_PKT_TYPE ePktType)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	if (ePktType == PKT_TYPE_802_11_ALL) {
 		pDevice->bStopBeacon = true;
@@ -713,9 +681,8 @@ bool CARDbStopTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
  * Return Value: true if success; false if failed.
  *
  */
-bool CARDbStartTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
+bool CARDbStartTxPacket(struct vnt_private *pDevice, CARD_PKT_TYPE ePktType)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	if (ePktType == PKT_TYPE_802_11_ALL) {
 		pDevice->bStopBeacon = false;
@@ -731,7 +698,7 @@ bool CARDbStartTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
 
 	if ((pDevice->bStopBeacon == false) &&
 	    (pDevice->bBeaconBufReady == true) &&
-	    (pDevice->eOPMode == OP_MODE_ADHOC)) {
+	    (pDevice->op_mode == NL80211_IFTYPE_ADHOC)) {
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_TCR, TCR_AUTOBCNTX);
 	}
 
@@ -752,37 +719,37 @@ bool CARDbStartTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType)
  * Return Value: true if success; false if failed.
  *
  */
-bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE eOPMode)
+bool CARDbSetBSSID(struct vnt_private *pDevice,
+		   unsigned char *pbyBSSID, enum nl80211_iftype op_mode)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	MACvWriteBSSIDAddress(pDevice->PortOffset, pbyBSSID);
 	memcpy(pDevice->abyBSSID, pbyBSSID, WLAN_BSSID_LEN);
-	if (eOPMode == OP_MODE_ADHOC)
+	if (op_mode == NL80211_IFTYPE_ADHOC)
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
 	else
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_ADHOC);
 
-	if (eOPMode == OP_MODE_AP)
+	if (op_mode == NL80211_IFTYPE_AP)
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
 	else
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_HOSTCR, HOSTCR_AP);
 
-	if (eOPMode == OP_MODE_UNKNOWN) {
+	if (op_mode == NL80211_IFTYPE_UNSPECIFIED) {
 		MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
 		pDevice->bBSSIDFilter = false;
 		pDevice->byRxMode &= ~RCR_BSSID;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wcmd: rx_mode = %x\n", pDevice->byRxMode);
+		pr_debug("wcmd: rx_mode = %x\n", pDevice->byRxMode);
 	} else {
 		if (is_zero_ether_addr(pDevice->abyBSSID) == false) {
 			MACvRegBitsOn(pDevice->PortOffset, MAC_REG_RCR, RCR_BSSID);
 			pDevice->bBSSIDFilter = true;
 			pDevice->byRxMode |= RCR_BSSID;
 		}
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: rx_mode = %x\n", pDevice->byRxMode);
+		pr_debug("wmgr: rx_mode = %x\n", pDevice->byRxMode);
 	}
 	// Adopt BSS state in Adapter Device Object
-	pDevice->eOPMode = eOPMode;
+	pDevice->op_mode = op_mode;
 	return true;
 }
 
@@ -818,11 +785,10 @@ bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE e
  *
  */
 bool CARDbSetTxDataRate(
-	void *pDeviceHandler,
+	struct vnt_private *pDevice,
 	unsigned short wDataRate
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	pDevice->wCurrentRate = wDataRate;
 	return true;
@@ -844,10 +810,9 @@ bool CARDbSetTxDataRate(
  -*/
 bool
 CARDbPowerDown(
-	void *pDeviceHandler
+	struct vnt_private *pDevice
 )
 {
-	PSDevice        pDevice = (PSDevice)pDeviceHandler;
 	unsigned int uIdx;
 
 	// check if already in Doze mode
@@ -865,7 +830,7 @@ CARDbPowerDown(
 	}
 
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Go to Doze ZZZZZZZZZZZZZZZ\n");
+	pr_debug("Go to Doze ZZZZZZZZZZZZZZZ\n");
 	return true;
 }
 
@@ -881,9 +846,8 @@ CARDbPowerDown(
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbRadioPowerOff(void *pDeviceHandler)
+bool CARDbRadioPowerOff(struct vnt_private *pDevice)
 {
-	PSDevice    pDevice = (PSDevice)pDeviceHandler;
 	bool bResult = true;
 
 	if (pDevice->bRadioOff == true)
@@ -927,9 +891,8 @@ bool CARDbRadioPowerOff(void *pDeviceHandler)
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbRadioPowerOn(void *pDeviceHandler)
+bool CARDbRadioPowerOn(struct vnt_private *pDevice)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	bool bResult = true;
 
 	pr_debug("chester power on\n");
@@ -970,9 +933,8 @@ bool CARDbRadioPowerOn(void *pDeviceHandler)
 	return bResult;
 }
 
-bool CARDbRemoveKey(void *pDeviceHandler, unsigned char *pbyBSSID)
+bool CARDbRemoveKey(struct vnt_private *pDevice, unsigned char *pbyBSSID)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	KeybRemoveAllKey(&(pDevice->sKey), pbyBSSID, pDevice->PortOffset);
 	return true;
@@ -996,27 +958,27 @@ bool CARDbRemoveKey(void *pDeviceHandler, unsigned char *pbyBSSID)
  -*/
 bool
 CARDbAdd_PMKID_Candidate(
-	void *pDeviceHandler,
+	struct vnt_private *pDevice,
 	unsigned char *pbyBSSID,
 	bool bRSNCapExist,
 	unsigned short wRSNCap
 )
 {
-	PSDevice            pDevice = (PSDevice) pDeviceHandler;
 	struct pmkid_candidate *pCandidateList;
 	unsigned int ii = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bAdd_PMKID_Candidate START: (%d)\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+	pr_debug("bAdd_PMKID_Candidate START: (%d)\n",
+		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
 
 	if (pDevice->gsPMKIDCandidate.NumCandidates >= MAX_PMKIDLIST) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vFlush_PMKID_Candidate: 3\n");
+		pr_debug("vFlush_PMKID_Candidate: 3\n");
 		memset(&pDevice->gsPMKIDCandidate, 0, sizeof(SPMKIDCandidateEvent));
 	}
 
 	for (ii = 0; ii < 6; ii++)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02X ", *(pbyBSSID + ii));
+		pr_debug("%02X ", *(pbyBSSID + ii));
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+	pr_debug("\n");
 
 	// Update Old Candidate
 	for (ii = 0; ii < pDevice->gsPMKIDCandidate.NumCandidates; ii++) {
@@ -1040,16 +1002,16 @@ CARDbAdd_PMKID_Candidate(
 
 	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
 	pDevice->gsPMKIDCandidate.NumCandidates++;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+	pr_debug("NumCandidates:%d\n",
+		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
 	return true;
 }
 
 void *
 CARDpGetCurrentAddress(
-	void *pDeviceHandler
+	struct vnt_private *pDevice
 )
 {
-	PSDevice            pDevice = (PSDevice) pDeviceHandler;
 
 	return pDevice->abyCurrentNetAddr;
 }
@@ -1070,15 +1032,14 @@ CARDpGetCurrentAddress(
  -*/
 bool
 CARDbStartMeasure(
-	void *pDeviceHandler,
+	struct vnt_private *pDevice,
 	void *pvMeasureEIDs,
 	unsigned int uNumOfMeasureEIDs
 )
 {
-	PSDevice                pDevice = (PSDevice) pDeviceHandler;
 	PWLAN_IE_MEASURE_REQ    pEID = (PWLAN_IE_MEASURE_REQ) pvMeasureEIDs;
-	QWORD                   qwCurrTSF;
-	QWORD                   qwStartTSF;
+	u64 qwCurrTSF;
+	u64 qwStartTSF;
 	bool bExpired = true;
 	unsigned short wDuration = 0;
 
@@ -1109,32 +1070,22 @@ CARDbStartMeasure(
 		pDevice->uNumOfMeasureEIDs--;
 
 		if (pDevice->byLocalID > REV_ID_VT3253_B1) {
-			HIDWORD(qwStartTSF) = HIDWORD(*((PQWORD)(pDevice->pCurrMeasureEID->sReq.abyStartTime)));
-			LODWORD(qwStartTSF) = LODWORD(*((PQWORD)(pDevice->pCurrMeasureEID->sReq.abyStartTime)));
+			qwStartTSF = *((u64 *)(pDevice->pCurrMeasureEID->sReq.abyStartTime));
 			wDuration = *((unsigned short *)(pDevice->pCurrMeasureEID->sReq.abyDuration));
 			wDuration += 1; // 1 TU for channel switching
 
-			if ((LODWORD(qwStartTSF) == 0) && (HIDWORD(qwStartTSF) == 0)) {
+			if (qwStartTSF == 0) {
 				// start immediately by setting start TSF == current TSF + 2 TU
-				LODWORD(qwStartTSF) = LODWORD(qwCurrTSF) + 2048;
-				HIDWORD(qwStartTSF) = HIDWORD(qwCurrTSF);
-				if (LODWORD(qwCurrTSF) > LODWORD(qwStartTSF))
-					HIDWORD(qwStartTSF)++;
+				qwStartTSF = qwCurrTSF + 2048;
 
 				bExpired = false;
 				break;
 			} else {
 				// start at setting start TSF - 1TU(for channel switching)
-				if (LODWORD(qwStartTSF) < 1024)
-					HIDWORD(qwStartTSF)--;
-
-				LODWORD(qwStartTSF) -= 1024;
+				qwStartTSF -= 1024;
 			}
 
-			if ((HIDWORD(qwCurrTSF) < HIDWORD(qwStartTSF)) ||
-			    ((HIDWORD(qwCurrTSF) == HIDWORD(qwStartTSF)) &&
-			     (LODWORD(qwCurrTSF) < LODWORD(qwStartTSF)))
-) {
+			if (qwCurrTSF < qwStartTSF) {
 				bExpired = false;
 				break;
 			}
@@ -1161,8 +1112,8 @@ CARDbStartMeasure(
 
 	if (!bExpired) {
 		MACvSelectPage1(pDevice->PortOffset);
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART, LODWORD(qwStartTSF));
-		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART + 4, HIDWORD(qwStartTSF));
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART, (u32)qwStartTSF);
+		VNSvOutPortD(pDevice->PortOffset + MAC_REG_MSRSTART + 4, (u32)(qwStartTSF >> 32));
 		VNSvOutPortW(pDevice->PortOffset + MAC_REG_MSRDURATION, wDuration);
 		MACvRegBitsOn(pDevice->PortOffset, MAC_REG_MSRCTL, MSRCTL_EN);
 		MACvSelectPage0(pDevice->PortOffset);
@@ -1196,13 +1147,12 @@ CARDbStartMeasure(
  -*/
 bool
 CARDbChannelSwitch(
-	void *pDeviceHandler,
+	struct vnt_private *pDevice,
 	unsigned char byMode,
 	unsigned char byNewChannel,
 	unsigned char byCount
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	bool bResult = true;
 
 	if (byCount == 0) {
@@ -1238,7 +1188,7 @@ CARDbChannelSwitch(
  -*/
 bool
 CARDbSetQuiet(
-	void *pDeviceHandler,
+	struct vnt_private *pDevice,
 	bool bResetQuiet,
 	unsigned char byQuietCount,
 	unsigned char byQuietPeriod,
@@ -1246,7 +1196,6 @@ CARDbSetQuiet(
 	unsigned short wQuietOffset
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	unsigned int ii = 0;
 
 	if (bResetQuiet) {
@@ -1291,10 +1240,9 @@ CARDbSetQuiet(
  -*/
 bool
 CARDbStartQuiet(
-	void *pDeviceHandler
+	struct vnt_private *pDevice
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	unsigned int ii = 0;
 	unsigned long dwStartTime = 0xFFFFFFFF;
 	unsigned int uCurrentQuietIndex = 0;
@@ -1391,12 +1339,11 @@ CARDbStartQuiet(
  -*/
 void
 CARDvSetPowerConstraint(
-	void *pDeviceHandler,
+	struct vnt_private *pDevice,
 	unsigned char byChannel,
 	char byPower
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	if (byChannel > CB_MAX_CHANNEL_24G) {
 		if (pDevice->bCountryInfo5G == true)
@@ -1425,12 +1372,11 @@ CARDvSetPowerConstraint(
  -*/
 void
 CARDvGetPowerCapability(
-	void *pDeviceHandler,
+	struct vnt_private *pDevice,
 	unsigned char *pbyMinPower,
 	unsigned char *pbyMaxPower
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	unsigned char byDec = 0;
 
 	*pbyMaxPower = pDevice->abyOFDMDefaultPwr[pDevice->byCurrentCh];
@@ -1460,10 +1406,9 @@ CARDvGetPowerCapability(
  */
 char
 CARDbyGetTransmitPower(
-	void *pDeviceHandler
+	struct vnt_private *pDevice
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 
 	return pDevice->byCurPwrdBm;
 }
@@ -1471,10 +1416,9 @@ CARDbyGetTransmitPower(
 //xxx
 void
 CARDvSafeResetTx(
-	void *pDeviceHandler
+	struct vnt_private *pDevice
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	unsigned int uu;
 	PSTxDesc    pCurrTD;
 
@@ -1524,10 +1468,9 @@ CARDvSafeResetTx(
  -*/
 void
 CARDvSafeResetRx(
-	void *pDeviceHandler
+	struct vnt_private *pDevice
 )
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	unsigned int uu;
 	PSRxDesc    pDesc;
 
@@ -1578,9 +1521,9 @@ CARDvSafeResetRx(
  * Return Value: response Control frame rate
  *
  */
-static unsigned short CARDwGetCCKControlRate(void *pDeviceHandler, unsigned short wRateIdx)
+static unsigned short CARDwGetCCKControlRate(struct vnt_private *pDevice,
+					     unsigned short wRateIdx)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
 	unsigned int ui = (unsigned int) wRateIdx;
 
 	while (ui > RATE_1M) {
@@ -1605,27 +1548,27 @@ static unsigned short CARDwGetCCKControlRate(void *pDeviceHandler, unsigned shor
  * Return Value: response Control frame rate
  *
  */
-static unsigned short CARDwGetOFDMControlRate(void *pDeviceHandler, unsigned short wRateIdx)
+static unsigned short CARDwGetOFDMControlRate(struct vnt_private *pDevice,
+					      unsigned short wRateIdx)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
 	unsigned int ui = (unsigned int) wRateIdx;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BASIC RATE: %X\n", pDevice->wBasicRate);
+	pr_debug("BASIC RATE: %X\n", pDevice->wBasicRate);
 
 	if (!CARDbIsOFDMinBasicRate((void *)pDevice)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDwGetOFDMControlRate:(NO OFDM) %d\n", wRateIdx);
+		pr_debug("CARDwGetOFDMControlRate:(NO OFDM) %d\n", wRateIdx);
 		if (wRateIdx > RATE_24M)
 			wRateIdx = RATE_24M;
 		return wRateIdx;
 	}
 	while (ui > RATE_11M) {
 		if (pDevice->wBasicRate & ((unsigned short)1 << ui)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDwGetOFDMControlRate : %d\n", ui);
+			pr_debug("CARDwGetOFDMControlRate : %d\n", ui);
 			return (unsigned short)ui;
 		}
 		ui--;
 	}
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CARDwGetOFDMControlRate: 6M\n");
+	pr_debug("CARDwGetOFDMControlRate: 6M\n");
 	return (unsigned short)RATE_24M;
 }
 
@@ -1641,60 +1584,51 @@ static unsigned short CARDwGetOFDMControlRate(void *pDeviceHandler, unsigned sho
  * Return Value: None.
  *
  */
-void CARDvSetRSPINF(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
+void CARDvSetRSPINF(struct vnt_private *pDevice, CARD_PHY_TYPE ePHYType)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
-	unsigned char byServ = 0x00, bySignal = 0x00; //For CCK
-	unsigned short wLen = 0x0000;
+	union vnt_phy_field_swap phy;
 	unsigned char byTxRate, byRsvTime;             //For OFDM
 
 	//Set to Page1
 	MACvSelectPage1(pDevice->PortOffset);
 
-	//RSPINF_b_1
-	BBvCalculateParameter(pDevice,
-			      14,
-			      CARDwGetCCKControlRate((void *)pDevice, RATE_1M),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	/* RSPINF_b_1 */
+	vnt_get_phy_field(pDevice, 14,
+			  CARDwGetCCKControlRate(pDevice, RATE_1M),
+			  PK_TYPE_11B, &phy.field_read);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
-	///RSPINF_b_2
-	BBvCalculateParameter(pDevice,
-			      14,
-			      CARDwGetCCKControlRate((void *)pDevice, RATE_2M),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	 /* swap over to get correct write order */
+	swap(phy.swap[0], phy.swap[1]);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
-	//RSPINF_b_5
-	BBvCalculateParameter(pDevice,
-			      14,
-			      CARDwGetCCKControlRate((void *)pDevice, RATE_5M),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_1, phy.field_write);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
-	//RSPINF_b_11
-	BBvCalculateParameter(pDevice,
-			      14,
-			      CARDwGetCCKControlRate((void *)pDevice, RATE_11M),
-			      PK_TYPE_11B,
-			      &wLen,
-			      &byServ,
-			      &bySignal
-);
+	/* RSPINF_b_2 */
+	vnt_get_phy_field(pDevice, 14,
+			  CARDwGetCCKControlRate(pDevice, RATE_2M),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_2, phy.field_write);
+
+	/* RSPINF_b_5 */
+	vnt_get_phy_field(pDevice, 14,
+			  CARDwGetCCKControlRate(pDevice, RATE_5M),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_5, phy.field_write);
+
+	/* RSPINF_b_11 */
+	vnt_get_phy_field(pDevice, 14,
+			  CARDwGetCCKControlRate(pDevice, RATE_11M),
+			  PK_TYPE_11B, &phy.field_read);
+
+	swap(phy.swap[0], phy.swap[1]);
+
+	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, phy.field_write);
 
-	VNSvOutPortD(pDevice->PortOffset + MAC_REG_RSPINF_B_11, MAKEDWORD(wLen, MAKEWORD(bySignal, byServ)));
 	//RSPINF_a_6
 	s_vCalculateOFDMRParameter(RATE_6M,
 				   ePHYType,
@@ -1766,10 +1700,9 @@ void CARDvSetRSPINF(void *pDeviceHandler, CARD_PHY_TYPE ePHYType)
  * Return Value: None.
  *
  */
-void vUpdateIFS(void *pDeviceHandler)
+void vUpdateIFS(struct vnt_private *pDevice)
 {
-	//Set SIFS, DIFS, EIFS, SlotTime, CwMin
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	/* Set SIFS, DIFS, EIFS, SlotTime, CwMin */
 
 	unsigned char byMaxMin = 0;
 
@@ -1818,9 +1751,8 @@ void vUpdateIFS(void *pDeviceHandler)
 	VNSvOutPortB(pDevice->PortOffset + MAC_REG_CWMAXMIN0, (unsigned char)byMaxMin);
 }
 
-void CARDvUpdateBasicTopRate(void *pDeviceHandler)
+void CARDvUpdateBasicTopRate(struct vnt_private *pDevice)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
 	unsigned char byTopOFDM = RATE_24M, byTopCCK = RATE_1M;
 	unsigned char ii;
 
@@ -1844,9 +1776,8 @@ void CARDvUpdateBasicTopRate(void *pDeviceHandler)
 	pDevice->byTopCCKBasicRate = byTopCCK;
 }
 
-bool CARDbAddBasicRate(void *pDeviceHandler, unsigned short wRateIdx)
+bool CARDbAddBasicRate(struct vnt_private *pDevice, unsigned short wRateIdx)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
 	unsigned short wRate = (unsigned short)(1<<wRateIdx);
 
 	pDevice->wBasicRate |= wRate;
@@ -1857,9 +1788,8 @@ bool CARDbAddBasicRate(void *pDeviceHandler, unsigned short wRateIdx)
 	return true;
 }
 
-bool CARDbIsOFDMinBasicRate(void *pDeviceHandler)
+bool CARDbIsOFDMinBasicRate(struct vnt_private *pDevice)
 {
-	PSDevice pDevice = (PSDevice)pDeviceHandler;
 	int ii;
 
 	for (ii = RATE_54M; ii >= RATE_6M; ii--) {
@@ -1869,9 +1799,8 @@ bool CARDbIsOFDMinBasicRate(void *pDeviceHandler)
 	return false;
 }
 
-unsigned char CARDbyGetPktType(void *pDeviceHandler)
+unsigned char CARDbyGetPktType(struct vnt_private *pDevice)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
 
 	if (pDevice->byBBType == BB_TYPE_11A || pDevice->byBBType == BB_TYPE_11B)
 		return (unsigned char)pDevice->byBBType;
@@ -1922,9 +1851,8 @@ void CARDvSetLoopbackMode(void __iomem *dwIoBase, unsigned short wLoopbackMode)
  * Return Value: none
  *
  */
-bool CARDbSoftwareReset(void *pDeviceHandler)
+bool CARDbSoftwareReset(struct vnt_private *pDevice)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
 
 	// reset MAC
 	if (!MACbSafeSoftwareReset(pDevice->PortOffset))
@@ -1948,25 +1876,17 @@ bool CARDbSoftwareReset(void *pDeviceHandler)
  * Return Value: TSF Offset value
  *
  */
-QWORD CARDqGetTSFOffset(unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2)
+u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2)
 {
-	QWORD   qwTSFOffset;
+	u64 qwTSFOffset = 0;
 	unsigned short wRxBcnTSFOffst = 0;
 
-	HIDWORD(qwTSFOffset) = 0;
-	LODWORD(qwTSFOffset) = 0;
 	wRxBcnTSFOffst = cwRXBCNTSFOff[byRxRate%MAX_RATE];
-	(qwTSF2).u.dwLowDword += (unsigned long)(wRxBcnTSFOffst);
-	if ((qwTSF2).u.dwLowDword < (unsigned long)(wRxBcnTSFOffst))
-		(qwTSF2).u.dwHighDword++;
-
-	LODWORD(qwTSFOffset) = LODWORD(qwTSF1) - LODWORD(qwTSF2);
-	if (LODWORD(qwTSF1) < LODWORD(qwTSF2)) {
-		// if borrow needed
-		HIDWORD(qwTSFOffset) = HIDWORD(qwTSF1) - HIDWORD(qwTSF2) - 1;
-	} else {
-		HIDWORD(qwTSFOffset) = HIDWORD(qwTSF1) - HIDWORD(qwTSF2);
-	}
+
+	qwTSF2 += (u64)wRxBcnTSFOffst;
+
+	qwTSFOffset = qwTSF1 - qwTSF2;
+
 	return qwTSFOffset;
 }
 
@@ -1983,7 +1903,7 @@ QWORD CARDqGetTSFOffset(unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2)
  * Return Value: true if success; otherwise false
  *
  */
-bool CARDbGetCurrentTSF(void __iomem *dwIoBase, PQWORD pqwCurrTSF)
+bool CARDbGetCurrentTSF(void __iomem *dwIoBase, u64 *pqwCurrTSF)
 {
 	unsigned short ww;
 	unsigned char byData;
@@ -1996,8 +1916,8 @@ bool CARDbGetCurrentTSF(void __iomem *dwIoBase, PQWORD pqwCurrTSF)
 	}
 	if (ww == W_MAX_TIMEOUT)
 		return false;
-	VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR, &LODWORD(*pqwCurrTSF));
-	VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR + 4, &HIDWORD(*pqwCurrTSF));
+	VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR, (u32 *)pqwCurrTSF);
+	VNSvInPortD(dwIoBase + MAC_REG_TSFCNTR + 4, (u32 *)pqwCurrTSF + 1);
 
 	return true;
 }
@@ -2016,28 +1936,20 @@ bool CARDbGetCurrentTSF(void __iomem *dwIoBase, PQWORD pqwCurrTSF)
  * Return Value: TSF value of next Beacon
  *
  */
-QWORD CARDqGetNextTBTT(QWORD qwTSF, unsigned short wBeaconInterval)
+u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval)
 {
-	unsigned int uLowNextTBTT;
-	unsigned int uHighRemain, uLowRemain;
-	unsigned int uBeaconInterval;
-
-	uBeaconInterval = wBeaconInterval * 1024;
-	// Next TBTT = ((local_current_TSF / beacon_interval) + 1) * beacon_interval
-	uLowNextTBTT = (LODWORD(qwTSF) >> 10) << 10;
-	// low dword (mod) bcn
-	uLowRemain = (uLowNextTBTT) % uBeaconInterval;
-	// high dword (mod) bcn
-	uHighRemain = (((0xffffffff % uBeaconInterval) + 1) * HIDWORD(qwTSF))
-		% uBeaconInterval;
-	uLowRemain = (uHighRemain + uLowRemain) % uBeaconInterval;
-	uLowRemain = uBeaconInterval - uLowRemain;
-
-	// check if carry when add one beacon interval
-	if ((~uLowNextTBTT) < uLowRemain)
-		HIDWORD(qwTSF)++;
-
-	LODWORD(qwTSF) = uLowNextTBTT + uLowRemain;
+	u32 beacon_int;
+
+	beacon_int = wBeaconInterval * 1024;
+
+	/* Next TBTT =
+	*	((local_current_TSF / beacon_interval) + 1) * beacon_interval
+	*/
+	if (beacon_int) {
+		do_div(qwTSF, beacon_int);
+		qwTSF += 1;
+		qwTSF *= beacon_int;
+	}
 
 	return qwTSF;
 }
@@ -2058,15 +1970,14 @@ QWORD CARDqGetNextTBTT(QWORD qwTSF, unsigned short wBeaconInterval)
  */
 void CARDvSetFirstNextTBTT(void __iomem *dwIoBase, unsigned short wBeaconInterval)
 {
-	QWORD   qwNextTBTT;
+	u64 qwNextTBTT = 0;
 
-	HIDWORD(qwNextTBTT) = 0;
-	LODWORD(qwNextTBTT) = 0;
 	CARDbGetCurrentTSF(dwIoBase, &qwNextTBTT); //Get Local TSF counter
+
 	qwNextTBTT = CARDqGetNextTBTT(qwNextTBTT, wBeaconInterval);
 	// Set NextTBTT
-	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, LODWORD(qwNextTBTT));
-	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, HIDWORD(qwNextTBTT));
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, (u32)qwNextTBTT);
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, (u32)(qwNextTBTT >> 32));
 	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
 }
 
@@ -2085,13 +1996,12 @@ void CARDvSetFirstNextTBTT(void __iomem *dwIoBase, unsigned short wBeaconInterva
  * Return Value: none
  *
  */
-void CARDvUpdateNextTBTT(void __iomem *dwIoBase, QWORD qwTSF, unsigned short wBeaconInterval)
+void CARDvUpdateNextTBTT(void __iomem *dwIoBase, u64 qwTSF, unsigned short wBeaconInterval)
 {
 	qwTSF = CARDqGetNextTBTT(qwTSF, wBeaconInterval);
 	// Set NextTBTT
-	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, LODWORD(qwTSF));
-	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, HIDWORD(qwTSF));
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT, (u32)qwTSF);
+	VNSvOutPortD(dwIoBase + MAC_REG_NEXTTBTT + 4, (u32)(qwTSF >> 32));
 	MACvRegBitsOn(dwIoBase, MAC_REG_TFTCTL, TFTCTL_TBTTSYNCEN);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Card:Update Next TBTT[%8xh:%8xh]\n",
-		(unsigned int) HIDWORD(qwTSF), (unsigned int) LODWORD(qwTSF));
+	pr_debug("Card:Update Next TBTT[%8llx]\n", qwTSF);
 }
diff --git a/drivers/staging/vt6655/card.h b/drivers/staging/vt6655/card.h
index 2f1a05ef44a1..96f5b6c46e82 100644
--- a/drivers/staging/vt6655/card.h
+++ b/drivers/staging/vt6655/card.h
@@ -31,6 +31,7 @@
 
 #include "ttype.h"
 #include <linux/types.h>
+#include <linux/nl80211.h>
 
 //
 // Loopback mode
@@ -70,56 +71,48 @@ typedef enum _CARD_STATUS_TYPE {
 	CARD_STATUS_PMKID
 } CARD_STATUS_TYPE, *PCARD_STATUS_TYPE;
 
-typedef enum _CARD_OP_MODE {
-	OP_MODE_INFRASTRUCTURE,
-	OP_MODE_ADHOC,
-	OP_MODE_AP,
-	OP_MODE_UNKNOWN
-} CARD_OP_MODE, *PCARD_OP_MODE;
-
-void CARDvSetRSPINF(void *pDeviceHandler, CARD_PHY_TYPE ePHYType);
-void vUpdateIFS(void *pDeviceHandler);
-void CARDvUpdateBasicTopRate(void *pDeviceHandler);
-bool CARDbAddBasicRate(void *pDeviceHandler, unsigned short wRateIdx);
-bool CARDbIsOFDMinBasicRate(void *pDeviceHandler);
+struct vnt_private;
+
+void CARDvSetRSPINF(struct vnt_private *, CARD_PHY_TYPE ePHYType);
+void vUpdateIFS(struct vnt_private *);
+void CARDvUpdateBasicTopRate(struct vnt_private *);
+bool CARDbAddBasicRate(struct vnt_private *, unsigned short wRateIdx);
+bool CARDbIsOFDMinBasicRate(struct vnt_private *);
 void CARDvSetLoopbackMode(void __iomem *dwIoBase, unsigned short wLoopbackMode);
-bool CARDbSoftwareReset(void *pDeviceHandler);
+bool CARDbSoftwareReset(struct vnt_private *);
 void CARDvSetFirstNextTBTT(void __iomem *dwIoBase, unsigned short wBeaconInterval);
-void CARDvUpdateNextTBTT(void __iomem *dwIoBase, QWORD qwTSF, unsigned short wBeaconInterval);
-bool CARDbGetCurrentTSF(void __iomem *dwIoBase, PQWORD pqwCurrTSF);
-QWORD CARDqGetNextTBTT(QWORD qwTSF, unsigned short wBeaconInterval);
-QWORD CARDqGetTSFOffset(unsigned char byRxRate, QWORD qwTSF1, QWORD qwTSF2);
-bool CARDbSetTxPower(void *pDeviceHandler, unsigned long ulTxPower);
-unsigned char CARDbyGetPktType(void *pDeviceHandler);
-void CARDvSafeResetTx(void *pDeviceHandler);
-void CARDvSafeResetRx(void *pDeviceHandler);
-
-bool CARDbRadioPowerOff(void *pDeviceHandler);
-bool CARDbRadioPowerOn(void *pDeviceHandler);
-bool CARDbIsShortPreamble(void *pDeviceHandler);
-bool CARDbIsShorSlotTime(void *pDeviceHandler);
-bool CARDbSetPhyParameter(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, unsigned short wCapInfo, unsigned char byERPField, void *pvSupportRateIEs, void *pvExtSupportRateIEs);
-bool CARDbUpdateTSF(void *pDeviceHandler, unsigned char byRxRate, QWORD qwBSSTimestamp, QWORD qwLocalTSF);
-bool CARDbStopTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType);
-bool CARDbStartTxPacket(void *pDeviceHandler, CARD_PKT_TYPE ePktType);
-bool CARDbSetBeaconPeriod(void *pDeviceHandler, unsigned short wBeaconInterval);
-bool CARDbSetBSSID(void *pDeviceHandler, unsigned char *pbyBSSID, CARD_OP_MODE eOPMode);
-
-bool
-CARDbPowerDown(
-	void *pDeviceHandler
-);
-
-bool CARDbSetTxDataRate(
-	void *pDeviceHandler,
-	unsigned short wDataRate
-);
-
-bool CARDbRemoveKey(void *pDeviceHandler, unsigned char *pbyBSSID);
+void CARDvUpdateNextTBTT(void __iomem *dwIoBase, u64 qwTSF, unsigned short wBeaconInterval);
+bool CARDbGetCurrentTSF(void __iomem *dwIoBase, u64 *pqwCurrTSF);
+u64 CARDqGetNextTBTT(u64 qwTSF, unsigned short wBeaconInterval);
+u64 CARDqGetTSFOffset(unsigned char byRxRate, u64 qwTSF1, u64 qwTSF2);
+bool CARDbSetTxPower(struct vnt_private *, unsigned long ulTxPower);
+unsigned char CARDbyGetPktType(struct vnt_private *);
+void CARDvSafeResetTx(struct vnt_private *);
+void CARDvSafeResetRx(struct vnt_private *);
+bool CARDbRadioPowerOff(struct vnt_private *);
+bool CARDbRadioPowerOn(struct vnt_private *);
+bool CARDbIsShortPreamble(struct vnt_private *);
+bool CARDbIsShorSlotTime(struct vnt_private *);
+bool CARDbSetPhyParameter(struct vnt_private *, CARD_PHY_TYPE ePHYType,
+			  unsigned short wCapInfo, unsigned char byERPField,
+			  void *pvSupportRateIEs, void *pvExtSupportRateIEs);
+bool CARDbUpdateTSF(struct vnt_private *, unsigned char byRxRate,
+		    u64 qwBSSTimestamp, u64 qwLocalTSF);
+bool CARDbStopTxPacket(struct vnt_private *, CARD_PKT_TYPE ePktType);
+bool CARDbStartTxPacket(struct vnt_private *, CARD_PKT_TYPE ePktType);
+bool CARDbSetBeaconPeriod(struct vnt_private *, unsigned short wBeaconInterval);
+bool CARDbSetBSSID(struct vnt_private *,
+		   unsigned char *pbyBSSID, enum nl80211_iftype);
+
+bool CARDbPowerDown(struct vnt_private *);
+
+bool CARDbSetTxDataRate(struct vnt_private *, unsigned short wDataRate);
+
+bool CARDbRemoveKey(struct vnt_private *, unsigned char *pbyBSSID);
 
 bool
 CARDbAdd_PMKID_Candidate(
-	void *pDeviceHandler,
+	struct vnt_private *,
 	unsigned char *pbyBSSID,
 	bool bRSNCapExist,
 	unsigned short wRSNCap
@@ -127,19 +120,19 @@ CARDbAdd_PMKID_Candidate(
 
 void *
 CARDpGetCurrentAddress(
-	void *pDeviceHandler
+	struct vnt_private *
 );
 
 bool
 CARDbStartMeasure(
-	void *pDeviceHandler,
+	struct vnt_private *,
 	void *pvMeasureEIDs,
 	unsigned int uNumOfMeasureEIDs
 );
 
 bool
 CARDbChannelSwitch(
-	void *pDeviceHandler,
+	struct vnt_private *,
 	unsigned char byMode,
 	unsigned char byNewChannel,
 	unsigned char byCount
@@ -147,7 +140,7 @@ CARDbChannelSwitch(
 
 bool
 CARDbSetQuiet(
-	void *pDeviceHandler,
+	struct vnt_private *,
 	bool bResetQuiet,
 	unsigned char byQuietCount,
 	unsigned char byQuietPeriod,
@@ -157,26 +150,26 @@ CARDbSetQuiet(
 
 bool
 CARDbStartQuiet(
-	void *pDeviceHandler
+	struct vnt_private *
 );
 
 void
 CARDvSetPowerConstraint(
-	void *pDeviceHandler,
+	struct vnt_private *,
 	unsigned char byChannel,
 	char byPower
 );
 
 void
 CARDvGetPowerCapability(
-	void *pDeviceHandler,
+	struct vnt_private *,
 	unsigned char *pbyMinPower,
 	unsigned char *pbyMaxPower
 );
 
 char
 CARDbyGetTransmitPower(
-	void *pDeviceHandler
+	struct vnt_private *
 );
 
 #endif // __CARD_H__
diff --git a/drivers/staging/vt6655/channel.c b/drivers/staging/vt6655/channel.c
index d5b89b7aaf23..4ce964ba14b7 100644
--- a/drivers/staging/vt6655/channel.c
+++ b/drivers/staging/vt6655/channel.c
@@ -30,8 +30,6 @@
 
 #define CARD_MAX_CHANNEL_TBL    56
 
-static int msglevel = MSG_LEVEL_INFO;
-
 /*---------------------  Static Variables  --------------------------*/
 
 static SChannelTblElement sChannelTbl[CARD_MAX_CHANNEL_TBL + 1] =
@@ -416,7 +414,7 @@ bool channel_get_list(unsigned int uCountryCodeIdx, unsigned char *pbyChannelTab
 
 void init_channel_table(void *pDeviceHandler)
 {
-	PSDevice    pDevice = (PSDevice) pDeviceHandler;
+	struct vnt_private *pDevice = pDeviceHandler;
 	bool bMultiBand = false;
 	unsigned int ii;
 
@@ -480,7 +478,10 @@ void init_channel_table(void *pDeviceHandler)
 		}
 	}
 
-	DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Zone=[%d][%c][%c]!!\n", pDevice->byZoneType, ChannelRuleTab[pDevice->byZoneType].chCountryCode[0], ChannelRuleTab[pDevice->byZoneType].chCountryCode[1]);
+	pr_info("Zone=[%d][%c][%c]!!\n",
+		pDevice->byZoneType,
+		ChannelRuleTab[pDevice->byZoneType].chCountryCode[0],
+		ChannelRuleTab[pDevice->byZoneType].chCountryCode[1]);
 
 	for (ii = 0; ii < CARD_MAX_CHANNEL_TBL; ii++) {
 		if (pDevice->abyRegPwr[ii + 1] == 0)
@@ -521,7 +522,7 @@ unsigned char get_channel_number(void *pDeviceHandler, unsigned char byChannelIn
  */
 bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	struct vnt_private *pDevice = pDeviceHandler;
 	bool bResult = true;
 
 	if (pDevice->byCurrentCh == uConnectionChannel)
@@ -583,7 +584,7 @@ bool set_channel(void *pDeviceHandler, unsigned int uConnectionChannel)
 
 void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	struct vnt_private *pDevice = pDeviceHandler;
 	unsigned int ii = 0;
 	unsigned int uu = 0;
 	unsigned int step = 0;
@@ -632,7 +633,7 @@ void set_country_info(void *pDeviceHandler, CARD_PHY_TYPE ePHYType, void *pIE)
 
 unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	struct vnt_private *pDevice = pDeviceHandler;
 	unsigned int ii;
 	unsigned char byCount;
 	PWLAN_IE_SUPP_CH pIE = (PWLAN_IE_SUPP_CH) pbyIEs;
@@ -703,7 +704,7 @@ unsigned char set_support_channels(void *pDeviceHandler, unsigned char *pbyIEs)
 
 void set_country_IE(void *pDeviceHandler, void *pIE)
 {
-	PSDevice pDevice = (PSDevice) pDeviceHandler;
+	struct vnt_private *pDevice = pDeviceHandler;
 	unsigned int ii;
 	PWLAN_IE_COUNTRY pIECountry = (PWLAN_IE_COUNTRY) pIE;
 
diff --git a/drivers/staging/vt6655/datarate.c b/drivers/staging/vt6655/datarate.c
index 565028c4ab03..52907a4fae9d 100644
--- a/drivers/staging/vt6655/datarate.c
+++ b/drivers/staging/vt6655/datarate.c
@@ -49,7 +49,6 @@
 
 extern unsigned short TxRate_iwconfig; /* 2008-5-8 <add> by chester */
 /*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 static const unsigned char acbyIERate[MAX_RATE] = {
 0x02, 0x04, 0x0B, 0x16, 0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C
 };
@@ -195,7 +194,7 @@ RATEvParseMaxRate(
 	unsigned char *pbyTopOFDMRate
 )
 {
-	PSDevice  pDevice = (PSDevice) pDeviceHandler;
+	struct vnt_private *pDevice = pDeviceHandler;
 	unsigned int ii;
 	unsigned char byHighSuppRate = 0;
 	unsigned char byRate = 0;
@@ -208,7 +207,7 @@ RATEvParseMaxRate(
 	*pwSuppRate = 0;
 	uRateLen = pItemRates->len;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ParseMaxRate Len: %d\n", uRateLen);
+	pr_debug("ParseMaxRate Len: %d\n", uRateLen);
 	if (pDevice->eCurrentPHYType != PHY_TYPE_11B) {
 		if (uRateLen > WLAN_RATES_MAXLEN)
 			uRateLen = WLAN_RATES_MAXLEN;
@@ -222,7 +221,8 @@ RATEvParseMaxRate(
 		if (WLAN_MGMT_IS_BASICRATE(byRate) && bUpdateBasicRate)  {
 			/* Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate */
 			CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ParseMaxRate AddBasicRate: %d\n", wGetRateIdx(byRate));
+			pr_debug("ParseMaxRate AddBasicRate: %d\n",
+				 wGetRateIdx(byRate));
 		}
 		byRate = (unsigned char)(pItemRates->abyRates[ii]&0x7F);
 		if (byHighSuppRate == 0)
@@ -244,7 +244,8 @@ RATEvParseMaxRate(
 			if (WLAN_MGMT_IS_BASICRATE(pItemExtRates->abyRates[ii])) {
 				/* Add to basic rate set, update pDevice->byTopCCKBasicRate and pDevice->byTopOFDMBasicRate */
 				CARDbAddBasicRate((void *)pDevice, wGetRateIdx(byRate));
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ParseMaxRate AddBasicRate: %d\n", wGetRateIdx(byRate));
+				pr_debug("ParseMaxRate AddBasicRate: %d\n",
+					 wGetRateIdx(byRate));
 			}
 			byRate = (unsigned char)(pItemExtRates->abyRates[ii]&0x7F);
 			if (byHighSuppRate == 0)
@@ -268,7 +269,7 @@ RATEvParseMaxRate(
 	if (wOldBasicRate != pDevice->wBasicRate)
 		CARDvSetRSPINF((void *)pDevice, pDevice->eCurrentPHYType);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Exit ParseMaxRate\n");
+	pr_debug("Exit ParseMaxRate\n");
 }
 
 /*+
@@ -295,7 +296,7 @@ RATEvTxRateFallBack(
 	PKnownNodeDB psNodeDBTable
 )
 {
-	PSDevice        pDevice = (PSDevice) pDeviceHandler;
+	struct vnt_private *pDevice = pDeviceHandler;
 	unsigned short wIdxDownRate = 0;
 	unsigned int ii;
 	bool bAutoRate[MAX_RATE]    = {true, true, true, true, false, false, true, true, true, true, true, true};
diff --git a/drivers/staging/vt6655/desc.h b/drivers/staging/vt6655/desc.h
index 05efa4e1b682..5a2bbd2047d8 100644
--- a/drivers/staging/vt6655/desc.h
+++ b/drivers/staging/vt6655/desc.h
@@ -33,6 +33,7 @@
 
 #include <linux/types.h>
 #include <linux/mm.h>
+#include "linux/ieee80211.h"
 #include "ttype.h"
 #include "tether.h"
 
@@ -331,33 +332,6 @@ typedef const STxSyncDesc *PCSTxSyncDesc;
 //
 // RsvTime buffer header
 //
-typedef struct tagSRrvTime_gRTS {
-	unsigned short wRTSTxRrvTime_ba;
-	unsigned short wRTSTxRrvTime_aa;
-	unsigned short wRTSTxRrvTime_bb;
-	unsigned short wReserved;
-	unsigned short wTxRrvTime_b;
-	unsigned short wTxRrvTime_a;
-} __attribute__ ((__packed__))
-SRrvTime_gRTS, *PSRrvTime_gRTS;
-typedef const SRrvTime_gRTS *PCSRrvTime_gRTS;
-
-typedef struct tagSRrvTime_gCTS {
-	unsigned short wCTSTxRrvTime_ba;
-	unsigned short wReserved;
-	unsigned short wTxRrvTime_b;
-	unsigned short wTxRrvTime_a;
-} __attribute__ ((__packed__))
-SRrvTime_gCTS, *PSRrvTime_gCTS;
-typedef const SRrvTime_gCTS *PCSRrvTime_gCTS;
-
-typedef struct tagSRrvTime_ab {
-	unsigned short wRTSTxRrvTime;
-	unsigned short wTxRrvTime;
-} __attribute__ ((__packed__))
-SRrvTime_ab, *PSRrvTime_ab;
-typedef const SRrvTime_ab *PCSRrvTime_ab;
-
 typedef struct tagSRrvTime_atim {
 	unsigned short wCTSTxRrvTime_ba;
 	unsigned short wTxRrvTime_a;
@@ -365,112 +339,18 @@ typedef struct tagSRrvTime_atim {
 SRrvTime_atim, *PSRrvTime_atim;
 typedef const SRrvTime_atim *PCSRrvTime_atim;
 
-//
-// RTS buffer header
-//
-typedef struct tagSRTSData {
-	unsigned short wFrameControl;
-	unsigned short wDurationID;
-	unsigned char abyRA[ETH_ALEN];
-	unsigned char abyTA[ETH_ALEN];
-} __attribute__ ((__packed__))
-SRTSData, *PSRTSData;
-typedef const SRTSData *PCSRTSData;
-
-typedef struct tagSRTS_g {
-	unsigned char bySignalField_b;
-	unsigned char byServiceField_b;
-	unsigned short wTransmitLength_b;
-	unsigned char bySignalField_a;
-	unsigned char byServiceField_a;
-	unsigned short wTransmitLength_a;
-	unsigned short wDuration_ba;
-	unsigned short wDuration_aa;
-	unsigned short wDuration_bb;
-	unsigned short wReserved;
-	SRTSData    Data;
-} __attribute__ ((__packed__))
-SRTS_g, *PSRTS_g;
-typedef const SRTS_g *PCSRTS_g;
-
-typedef struct tagSRTS_g_FB {
-	unsigned char bySignalField_b;
-	unsigned char byServiceField_b;
-	unsigned short wTransmitLength_b;
-	unsigned char bySignalField_a;
-	unsigned char byServiceField_a;
-	unsigned short wTransmitLength_a;
-	unsigned short wDuration_ba;
-	unsigned short wDuration_aa;
-	unsigned short wDuration_bb;
-	unsigned short wReserved;
-	unsigned short wRTSDuration_ba_f0;
-	unsigned short wRTSDuration_aa_f0;
-	unsigned short wRTSDuration_ba_f1;
-	unsigned short wRTSDuration_aa_f1;
-	SRTSData    Data;
-} __attribute__ ((__packed__))
-SRTS_g_FB, *PSRTS_g_FB;
-typedef const SRTS_g_FB *PCSRTS_g_FB;
-
-typedef struct tagSRTS_ab {
-	unsigned char bySignalField;
-	unsigned char byServiceField;
-	unsigned short wTransmitLength;
-	unsigned short wDuration;
-	unsigned short wReserved;
-	SRTSData    Data;
-} __attribute__ ((__packed__))
-SRTS_ab, *PSRTS_ab;
-typedef const SRTS_ab *PCSRTS_ab;
-
-typedef struct tagSRTS_a_FB {
-	unsigned char bySignalField;
-	unsigned char byServiceField;
-	unsigned short wTransmitLength;
-	unsigned short wDuration;
-	unsigned short wReserved;
-	unsigned short wRTSDuration_f0;
-	unsigned short wRTSDuration_f1;
-	SRTSData    Data;
-} __attribute__ ((__packed__))
-SRTS_a_FB, *PSRTS_a_FB;
-typedef const SRTS_a_FB *PCSRTS_a_FB;
-
-//
-// CTS buffer header
-//
-typedef struct tagSCTSData {
-	unsigned short wFrameControl;
-	unsigned short wDurationID;
-	unsigned char abyRA[ETH_ALEN];
-	unsigned short wReserved;
-} __attribute__ ((__packed__))
-SCTSData, *PSCTSData;
+/* Length, Service, and Signal fields of Phy for Tx */
+struct vnt_phy_field {
+	u8 signal;
+	u8 service;
+	__le16 len;
+} __packed;
 
-typedef struct tagSCTS {
-	unsigned char bySignalField_b;
-	unsigned char byServiceField_b;
-	unsigned short wTransmitLength_b;
-	unsigned short wDuration_ba;
-	unsigned short wReserved;
-	SCTSData    Data;
-} __attribute__ ((__packed__))
-SCTS, *PSCTS;
-typedef const SCTS *PCSCTS;
-
-typedef struct tagSCTS_FB {
-	unsigned char bySignalField_b;
-	unsigned char byServiceField_b;
-	unsigned short wTransmitLength_b;
-	unsigned short wDuration_ba;
-	unsigned short wReserved;
-	unsigned short wCTSDuration_ba_f0;
-	unsigned short wCTSDuration_ba_f1;
-	SCTSData    Data;
-} __attribute__ ((__packed__))
-SCTS_FB, *PSCTS_FB;
-typedef const SCTS_FB *PCSCTS_FB;
+union vnt_phy_field_swap {
+	struct vnt_phy_field field_read;
+	u16 swap[2];
+	u32 field_write;
+};
 
 //
 // Tx FIFO header
@@ -486,81 +366,6 @@ typedef struct tagSTxBufHead {
 STxBufHead, *PSTxBufHead;
 typedef const STxBufHead *PCSTxBufHead;
 
-typedef struct tagSTxShortBufHead {
-	unsigned short wFIFOCtl;
-	unsigned short wTimeStamp;
-} __attribute__ ((__packed__))
-STxShortBufHead, *PSTxShortBufHead;
-typedef const STxShortBufHead *PCSTxShortBufHead;
-
-//
-// Tx data header
-//
-typedef struct tagSTxDataHead_g {
-	unsigned char bySignalField_b;
-	unsigned char byServiceField_b;
-	unsigned short wTransmitLength_b;
-	unsigned char bySignalField_a;
-	unsigned char byServiceField_a;
-	unsigned short wTransmitLength_a;
-	unsigned short wDuration_b;
-	unsigned short wDuration_a;
-	unsigned short wTimeStampOff_b;
-	unsigned short wTimeStampOff_a;
-} __attribute__ ((__packed__))
-STxDataHead_g, *PSTxDataHead_g;
-typedef const STxDataHead_g *PCSTxDataHead_g;
-
-typedef struct tagSTxDataHead_g_FB {
-	unsigned char bySignalField_b;
-	unsigned char byServiceField_b;
-	unsigned short wTransmitLength_b;
-	unsigned char bySignalField_a;
-	unsigned char byServiceField_a;
-	unsigned short wTransmitLength_a;
-	unsigned short wDuration_b;
-	unsigned short wDuration_a;
-	unsigned short wDuration_a_f0;
-	unsigned short wDuration_a_f1;
-	unsigned short wTimeStampOff_b;
-	unsigned short wTimeStampOff_a;
-} __attribute__ ((__packed__))
-STxDataHead_g_FB, *PSTxDataHead_g_FB;
-typedef const STxDataHead_g_FB *PCSTxDataHead_g_FB;
-
-typedef struct tagSTxDataHead_ab {
-	unsigned char bySignalField;
-	unsigned char byServiceField;
-	unsigned short wTransmitLength;
-	unsigned short wDuration;
-	unsigned short wTimeStampOff;
-} __attribute__ ((__packed__))
-STxDataHead_ab, *PSTxDataHead_ab;
-typedef const STxDataHead_ab *PCSTxDataHead_ab;
-
-typedef struct tagSTxDataHead_a_FB {
-	unsigned char bySignalField;
-	unsigned char byServiceField;
-	unsigned short wTransmitLength;
-	unsigned short wDuration;
-	unsigned short wTimeStampOff;
-	unsigned short wDuration_f0;
-	unsigned short wDuration_f1;
-} __attribute__ ((__packed__))
-STxDataHead_a_FB, *PSTxDataHead_a_FB;
-typedef const STxDataHead_a_FB *PCSTxDataHead_a_FB;
-
-//
-// MICHDR data header
-//
-typedef struct tagSMICHDRHead {
-	u32 adwHDR0[4];
-	u32 adwHDR1[4];
-	u32 adwHDR2[4];
-} __attribute__ ((__packed__))
-SMICHDRHead, *PSMICHDRHead;
-typedef const SMICHDRHead *PCSMICHDRHead;
-
 typedef struct tagSBEACONCtl {
 	u32 BufReady:1;
 	u32 TSF:15;
diff --git a/drivers/staging/vt6655/device.h b/drivers/staging/vt6655/device.h
index 9bf0ea9af66e..ddd356aa7eaf 100644
--- a/drivers/staging/vt6655/device.h
+++ b/drivers/staging/vt6655/device.h
@@ -49,17 +49,13 @@
 #include <linux/sched.h>
 #include <linux/io.h>
 #include <linux/if.h>
+#include <linux/crc32.h>
 //#include <linux/config.h>
 #include <linux/uaccess.h>
 #include <linux/proc_fs.h>
 #include <linux/inetdevice.h>
 #include <linux/reboot.h>
-#ifdef SIOCETHTOOL
-#define DEVICE_ETHTOOL_IOCTL_SUPPORT
 #include <linux/ethtool.h>
-#else
-#undef DEVICE_ETHTOOL_IOCTL_SUPPORT
-#endif
 /* Include Wireless Extension definition and check version - Jean II */
 #include <linux/wireless.h>
 #include <net/iw_handler.h>	// New driver API
@@ -140,20 +136,12 @@
 
 #define	AVAIL_TD(p, q)	((p)->sOpts.nTxDescs[(q)] - ((p)->iTDUsed[(q)]))
 
-//PLICE_DEBUG ->
 #define	NUM				64
-//PLICE_DEUBG <-
 
 #define PRIVATE_Message                 0
 
 /*---------------------  Export Types  ------------------------------*/
 
-#define DBG_PRT(l, p, args...)		\
-do {					\
-	if (l <= msglevel)		\
-		printk(p, ##args);	\
-} while (0)
-
 #define PRINT_K(p, args...)		\
 do {					\
 	if (PRIVATE_Message)		\
@@ -337,20 +325,11 @@ typedef struct __device_opt {
 	u32         flags;
 } OPTIONS, *POPTIONS;
 
-typedef struct __device_info {
-	struct __device_info *next;
-	struct __device_info *prev;
-
+struct vnt_private {
 	struct pci_dev *pcid;
 
-#ifdef CONFIG_PM
-	u32                         pci_state[16];
-#endif
-
 // netdev
 	struct net_device *dev;
-	struct net_device *next_module;
-	struct net_device_stats     stats;
 
 //dma addr, rx/tx pool
 	dma_addr_t                  pool_dma;
@@ -409,11 +388,9 @@ typedef struct __device_info {
 
 	spinlock_t                  lock;
 
-//PLICE_DEBUG ->
 	pid_t			MLMEThr_pid;
 	struct completion	notify;
 	struct semaphore	mlme_semaphore;
-//PLICE_DEBUG <-
 
 	u32                         rx_bytes;
 
@@ -483,13 +460,12 @@ typedef struct __device_info {
 	unsigned short wFragmentationThreshold;
 	unsigned char byShortRetryLimit;
 	unsigned char byLongRetryLimit;
-	CARD_OP_MODE                eOPMode;
+	enum nl80211_iftype op_mode;
 	unsigned char byOpMode;
 	bool bBSSIDFilter;
 	unsigned short wMaxTransmitMSDULifetime;
 	unsigned char abyBSSID[ETH_ALEN];
 	unsigned char abyDesireBSSID[ETH_ALEN];
-	unsigned short wCTSDuration;       // update while speed change
 	unsigned short wACKDuration;       // update while speed change
 	unsigned short wRTSTransmitLen;    // update while speed change
 	unsigned char byRTSServiceField;  // update while speed change
@@ -497,7 +473,6 @@ typedef struct __device_info {
 
 	unsigned long dwMaxReceiveLifetime;       // dot11MaxReceiveLifetime
 
-	bool bCCK;
 	bool bEncryptionEnable;
 	bool bLongHeader;
 	bool bShortSlotTime;
@@ -566,7 +541,7 @@ typedef struct __device_info {
 	SKeyManagement          sKey;
 	unsigned long dwIVCounter;
 
-	QWORD                   qwPacketNumber; //For CCMP and TKIP as TSC(6 bytes)
+	u64 qwPacketNumber; /* For CCMP and TKIP as TSC(6 bytes) */
 	unsigned int	uCurrentWEPMode;
 
 	RC4Ext                  SBox;
@@ -643,12 +618,10 @@ typedef struct __device_info {
 
 	// command timer
 	struct timer_list       sTimerCommand;
-#ifdef TxInSleep
 	struct timer_list       sTimerTxData;
 	unsigned long nTxDataTimeCout;
 	bool fTxDataInSleep;
 	bool IsTxDataTrigger;
-#endif
 
 #ifdef WPA_SM_Transtatus
 	bool fWPA_Authened;           //is WPA/WPA-PSK or WPA2/WPA2-PSK authen??
@@ -744,9 +717,10 @@ typedef struct __device_info {
 
 	struct iw_statistics	wstats;		// wireless stats
 	bool bCommit;
-} DEVICE_INFO, *PSDevice;
+};
 
-static inline bool device_get_ip(PSDevice pInfo) {
+static inline bool device_get_ip(struct vnt_private *pInfo)
+{
 	struct in_device *in_dev = (struct in_device *)pInfo->dev->ip_ptr;
 	struct in_ifaddr *ifa;
 
@@ -772,7 +746,10 @@ static inline PDEVICE_TD_INFO alloc_td_info(void)
 
 /*---------------------  Export Functions  --------------------------*/
 
-bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeIndex);
-bool device_alloc_frag_buf(PSDevice pDevice, PSDeFragControlBlock pDeF);
-int Config_FileOperation(PSDevice pDevice, bool fwrite, unsigned char *Parameter);
+bool device_dma0_xmit(struct vnt_private *pDevice,
+		      struct sk_buff *skb, unsigned int uNodeIndex);
+bool device_alloc_frag_buf(struct vnt_private *pDevice,
+			   PSDeFragControlBlock pDeF);
+int Config_FileOperation(struct vnt_private *pDevice,
+			 bool fwrite, unsigned char *Parameter);
 #endif
diff --git a/drivers/staging/vt6655/device_cfg.h b/drivers/staging/vt6655/device_cfg.h
index 8e50e538076f..7221824e4f23 100644
--- a/drivers/staging/vt6655/device_cfg.h
+++ b/drivers/staging/vt6655/device_cfg.h
@@ -67,9 +67,6 @@ struct _version {
 
 #define PKT_BUF_SZ          2390
 
-#define MAX_UINTS           8
-#define OPTION_DEFAULT      { [0 ... MAX_UINTS-1] = -1}
-
 typedef enum  _chip_type {
 	VT3253 = 1
 } CHIP_TYPE, *PCHIP_TYPE;
diff --git a/drivers/staging/vt6655/device_main.c b/drivers/staging/vt6655/device_main.c
index 0b583a37f5b3..54e16f40d8ed 100644
--- a/drivers/staging/vt6655/device_main.c
+++ b/drivers/staging/vt6655/device_main.c
@@ -89,8 +89,6 @@
 #include <linux/slab.h>
 
 /*---------------------  Static Definitions -------------------------*/
-static int          msglevel                =   MSG_LEVEL_INFO;
-
 //
 // Define module options
 //
@@ -247,11 +245,6 @@ DEVICE_PARAM(bDiversityANTEnable, "ANT diversity mode");
 //
 // Static vars definitions
 //
-
-static int          device_nics             = 0;
-static PSDevice     pDevice_Infos           = NULL;
-static struct net_device *root_device_dev = NULL;
-
 static CHIP_INFO chip_info_table[] = {
 	{ VT3253,       "VIA Networking Solomon-A/B/G Wireless LAN Adapter ",
 	  256, 1,     DEVICE_FLAGS_IP_ALIGN|DEVICE_FLAGS_TX_ALIGN },
@@ -266,12 +259,12 @@ static const struct pci_device_id vt6655_pci_id_table[] = {
 /*---------------------  Static Functions  --------------------------*/
 
 static int  vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent);
-static void vt6655_init_info(struct pci_dev *pcid, PSDevice *ppDevice, PCHIP_INFO);
-static void device_free_info(PSDevice pDevice);
-static bool device_get_pci_info(PSDevice, struct pci_dev *pcid);
-static void device_print_info(PSDevice pDevice);
-static struct net_device_stats *device_get_stats(struct net_device *dev);
-static void device_init_diversity_timer(PSDevice pDevice);
+static void vt6655_init_info(struct pci_dev *pcid,
+			     struct vnt_private **ppDevice, PCHIP_INFO);
+static void device_free_info(struct vnt_private *pDevice);
+static bool device_get_pci_info(struct vnt_private *, struct pci_dev *pcid);
+static void device_print_info(struct vnt_private *pDevice);
+static void device_init_diversity_timer(struct vnt_private *pDevice);
 static int  device_open(struct net_device *dev);
 static int  device_xmit(struct sk_buff *skb, struct net_device *dev);
 static  irqreturn_t  device_intr(int irq,  void *dev_instance);
@@ -290,28 +283,28 @@ static struct notifier_block device_notifier = {
 };
 #endif
 
-static void device_init_rd0_ring(PSDevice pDevice);
-static void device_init_rd1_ring(PSDevice pDevice);
-static void device_init_defrag_cb(PSDevice pDevice);
-static void device_init_td0_ring(PSDevice pDevice);
-static void device_init_td1_ring(PSDevice pDevice);
+static void device_init_rd0_ring(struct vnt_private *pDevice);
+static void device_init_rd1_ring(struct vnt_private *pDevice);
+static void device_init_defrag_cb(struct vnt_private *pDevice);
+static void device_init_td0_ring(struct vnt_private *pDevice);
+static void device_init_td1_ring(struct vnt_private *pDevice);
 
 static int  device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev);
 //2008-0714<Add>by Mike Liu
-static bool device_release_WPADEV(PSDevice pDevice);
+static bool device_release_WPADEV(struct vnt_private *pDevice);
 
 static int  ethtool_ioctl(struct net_device *dev, void __user *useraddr);
-static int  device_rx_srv(PSDevice pDevice, unsigned int uIdx);
-static int  device_tx_srv(PSDevice pDevice, unsigned int uIdx);
-static bool device_alloc_rx_buf(PSDevice pDevice, PSRxDesc pDesc);
-static void device_init_registers(PSDevice pDevice);
-static void device_free_tx_buf(PSDevice pDevice, PSTxDesc pDesc);
-static void device_free_td0_ring(PSDevice pDevice);
-static void device_free_td1_ring(PSDevice pDevice);
-static void device_free_rd0_ring(PSDevice pDevice);
-static void device_free_rd1_ring(PSDevice pDevice);
-static void device_free_rings(PSDevice pDevice);
-static void device_free_frag_buf(PSDevice pDevice);
+static int  device_rx_srv(struct vnt_private *pDevice, unsigned int uIdx);
+static int  device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx);
+static bool device_alloc_rx_buf(struct vnt_private *pDevice, PSRxDesc pDesc);
+static void device_init_registers(struct vnt_private *pDevice);
+static void device_free_tx_buf(struct vnt_private *pDevice, PSTxDesc pDesc);
+static void device_free_td0_ring(struct vnt_private *pDevice);
+static void device_free_td1_ring(struct vnt_private *pDevice);
+static void device_free_rd0_ring(struct vnt_private *pDevice);
+static void device_free_rd1_ring(struct vnt_private *pDevice);
+static void device_free_rings(struct vnt_private *pDevice);
+static void device_free_frag_buf(struct vnt_private *pDevice);
 static int Config_FileGetParameter(unsigned char *string,
 				   unsigned char *dest, unsigned char *source);
 
@@ -331,14 +324,14 @@ static char *get_chip_name(int chip_id)
 
 static void vt6655_remove(struct pci_dev *pcid)
 {
-	PSDevice pDevice = pci_get_drvdata(pcid);
+	struct vnt_private *pDevice = pci_get_drvdata(pcid);
 
 	if (pDevice == NULL)
 		return;
 	device_free_info(pDevice);
 }
 
-static void device_get_options(PSDevice pDevice, int index, char *devname)
+static void device_get_options(struct vnt_private *pDevice)
 {
 	POPTIONS pOpts = &(pDevice->sOpts);
 
@@ -363,7 +356,8 @@ static void device_get_options(PSDevice pDevice, int index, char *devname)
 }
 
 static void
-device_set_options(PSDevice pDevice) {
+device_set_options(struct vnt_private *pDevice)
+{
 	unsigned char abyBroadcastAddr[ETH_ALEN] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 	unsigned char abySNAP_RFC1042[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};
 	unsigned char abySNAP_Bridgetunnel[ETH_ALEN] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0xF8};
@@ -387,31 +381,29 @@ device_set_options(PSDevice pDevice) {
 	if (pDevice->uConnectionRate < RATE_AUTO)
 		pDevice->bFixRate = true;
 	pDevice->byBBType = pDevice->sOpts.bbp_type;
-	pDevice->byPacketType = pDevice->byBBType;
-
-//PLICE_DEBUG->
+	pDevice->byPacketType = (VIA_PKT_TYPE)pDevice->byBBType;
 	pDevice->byAutoFBCtrl = AUTO_FB_0;
-//PLICE_DEBUG<-
 	pDevice->bUpdateBBVGA = true;
 	pDevice->byFOETuning = 0;
-	pDevice->wCTSDuration = 0;
 	pDevice->byPreambleType = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " uChannel= %d\n", (int)pDevice->uChannel);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byOpMode= %d\n", (int)pDevice->byOpMode);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " ePSMode= %d\n", (int)pDevice->ePSMode);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " wRTSThreshold= %d\n", (int)pDevice->wRTSThreshold);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byShortRetryLimit= %d\n", (int)pDevice->byShortRetryLimit);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byLongRetryLimit= %d\n", (int)pDevice->byLongRetryLimit);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byPreambleType= %d\n", (int)pDevice->byPreambleType);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byShortPreamble= %d\n", (int)pDevice->byShortPreamble);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " uConnectionRate= %d\n", (int)pDevice->uConnectionRate);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " byBBType= %d\n", (int)pDevice->byBBType);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pDevice->b11hEnable= %d\n", (int)pDevice->b11hEnable);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pDevice->bDiversityRegCtlON= %d\n", (int)pDevice->bDiversityRegCtlON);
+	pr_debug(" uChannel= %d\n", (int)pDevice->uChannel);
+	pr_debug(" byOpMode= %d\n", (int)pDevice->byOpMode);
+	pr_debug(" ePSMode= %d\n", (int)pDevice->ePSMode);
+	pr_debug(" wRTSThreshold= %d\n", (int)pDevice->wRTSThreshold);
+	pr_debug(" byShortRetryLimit= %d\n", (int)pDevice->byShortRetryLimit);
+	pr_debug(" byLongRetryLimit= %d\n", (int)pDevice->byLongRetryLimit);
+	pr_debug(" byPreambleType= %d\n", (int)pDevice->byPreambleType);
+	pr_debug(" byShortPreamble= %d\n", (int)pDevice->byShortPreamble);
+	pr_debug(" uConnectionRate= %d\n", (int)pDevice->uConnectionRate);
+	pr_debug(" byBBType= %d\n", (int)pDevice->byBBType);
+	pr_debug(" pDevice->b11hEnable= %d\n", (int)pDevice->b11hEnable);
+	pr_debug(" pDevice->bDiversityRegCtlON= %d\n",
+		 (int)pDevice->bDiversityRegCtlON);
 }
 
-static void s_vCompleteCurrentMeasure(PSDevice pDevice, unsigned char byResult)
+static void s_vCompleteCurrentMeasure(struct vnt_private *pDevice,
+				      unsigned char byResult)
 {
 	unsigned int ii;
 	unsigned long dwDuration = 0;
@@ -453,7 +445,7 @@ static void s_vCompleteCurrentMeasure(PSDevice pDevice, unsigned char byResult)
 // Initialisation of MAC & BBP registers
 //
 
-static void device_init_registers(PSDevice pDevice)
+static void device_init_registers(struct vnt_private *pDevice)
 {
 	unsigned int ii;
 	unsigned char byValue;
@@ -469,8 +461,6 @@ static void device_init_registers(PSDevice pDevice)
 	/* Do MACbSoftwareReset in MACvInitialize */
 	MACbSoftwareReset(pDevice->PortOffset);
 
-	/* force CCK */
-	pDevice->bCCK = true;
 	pDevice->bAES = false;
 
 	/* Only used in 11g type, sync with ERP IE */
@@ -561,10 +551,10 @@ static void device_init_registers(PSDevice pDevice)
 		}
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO
-		"bDiversityEnable=[%d],NValue=[%d],MValue=[%d],TMax=[%d],TMax2=[%d]\n",
-		pDevice->bDiversityEnable, (int)pDevice->ulDiversityNValue,
-		(int)pDevice->ulDiversityMValue, pDevice->byTMax, pDevice->byTMax2);
+	pr_debug("bDiversityEnable=[%d],NValue=[%d],MValue=[%d],TMax=[%d],TMax2=[%d]\n",
+		 pDevice->bDiversityEnable, (int)pDevice->ulDiversityNValue,
+		 (int)pDevice->ulDiversityMValue, pDevice->byTMax,
+		 pDevice->byTMax2);
 
 	/* zonetype initial */
 	pDevice->byOriginalZonetype = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
@@ -577,7 +567,7 @@ static void device_init_registers(PSDevice pDevice)
 			pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0;
 			pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0B;
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Init Zone Type :USA\n");
+			pr_debug("Init Zone Type :USA\n");
 		} else if ((zonetype == 1) &&
 			 (pDevice->abyEEPROM[EEP_OFS_ZONETYPE] != 0x01)) {
 			/* for Japan */
@@ -589,7 +579,7 @@ static void device_init_registers(PSDevice pDevice)
 			pDevice->abyEEPROM[EEP_OFS_ZONETYPE] = 0x02;
 			pDevice->abyEEPROM[EEP_OFS_MAXCHANNEL] = 0x0D;
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Init Zone Type :Europe\n");
+			pr_debug("Init Zone Type :Europe\n");
 		} else {
 			if (zonetype != pDevice->abyEEPROM[EEP_OFS_ZONETYPE])
 				pr_debug("zonetype in file[%02x] mismatch with in EEPROM[%02x]\n",
@@ -612,12 +602,12 @@ static void device_init_registers(PSDevice pDevice)
 			pDevice->byRevId = 0x80;
 
 	pDevice->byRFType &= RF_MASK;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRFType = %x\n", pDevice->byRFType);
+	pr_debug("pDevice->byRFType = %x\n", pDevice->byRFType);
 
 	if (!pDevice->bZoneRegExist)
 		pDevice->byZoneType = pDevice->abyEEPROM[EEP_OFS_ZONETYPE];
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byZoneType = %x\n", pDevice->byZoneType);
+	pr_debug("pDevice->byZoneType = %x\n", pDevice->byZoneType);
 
 	/* Init RF module */
 	RFbInit(pDevice);
@@ -740,8 +730,7 @@ static void device_init_registers(PSDevice pDevice)
 
 	/* get Permanent network address */
 	SROMvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Network address = %pM\n",
-		pDevice->abyCurrentNetAddr);
+	pr_debug("Network address = %pM\n", pDevice->abyCurrentNetAddr);
 
 	/* reset Tx pointer */
 	CARDvSafeResetRx(pDevice);
@@ -763,7 +752,7 @@ static void device_init_registers(PSDevice pDevice)
 	netif_stop_queue(pDevice->dev);
 }
 
-static void device_init_diversity_timer(PSDevice pDevice)
+static void device_init_diversity_timer(struct vnt_private *pDevice)
 {
 	init_timer(&pDevice->TimerSQ3Tmax1);
 	pDevice->TimerSQ3Tmax1.data = (unsigned long) pDevice;
@@ -781,7 +770,7 @@ static void device_init_diversity_timer(PSDevice pDevice)
 	pDevice->TimerSQ3Tmax3.expires = RUN_AT(HZ);
 }
 
-static bool device_release_WPADEV(PSDevice pDevice)
+static bool device_release_WPADEV(struct vnt_private *pDevice)
 {
 	viawget_wpa_header *wpahdr;
 	int ii = 0;
@@ -816,7 +805,6 @@ static const struct net_device_ops device_netdev_ops = {
 	.ndo_open               = device_open,
 	.ndo_stop               = device_close,
 	.ndo_do_ioctl           = device_ioctl,
-	.ndo_get_stats          = device_get_stats,
 	.ndo_start_xmit         = device_xmit,
 	.ndo_set_rx_mode	= device_set_multi,
 };
@@ -827,17 +815,12 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	static bool bFirst = true;
 	struct net_device *dev = NULL;
 	PCHIP_INFO  pChip_info = (PCHIP_INFO)ent->driver_data;
-	PSDevice    pDevice;
+	struct vnt_private *pDevice;
 	int         rc;
 
-	if (device_nics++ >= MAX_UINTS) {
-		pr_notice(DEVICE_NAME ": already found %d NICs\n", device_nics);
-		return -ENODEV;
-	}
-
-	dev = alloc_etherdev(sizeof(DEVICE_INFO));
+	dev = alloc_etherdev(sizeof(*pDevice));
 
-	pDevice = (PSDevice) netdev_priv(dev);
+	pDevice = netdev_priv(dev);
 
 	if (dev == NULL) {
 		pr_err(DEVICE_NAME ": allocate net device failed\n");
@@ -855,8 +838,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 
 	vt6655_init_info(pcid, &pDevice, pChip_info);
 	pDevice->dev = dev;
-	pDevice->next_module = root_device_dev;
-	root_device_dev = dev;
 
 	if (pci_enable_device(pcid)) {
 		device_free_info(pDevice);
@@ -931,15 +912,6 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	}
 
 	dev->base_addr = pDevice->ioaddr;
-#ifdef	PLICE_DEBUG
-	unsigned char value;
-
-	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
-	pr_debug("Before write: value is %x\n", value);
-	VNSvOutPortB(pDevice->PortOffset, value);
-	VNSvInPortB(pDevice->PortOffset+0x4F, &value);
-	pr_debug("After write: value is %x\n", value);
-#endif
 	// do reset
 	if (!MACbSoftwareReset(pDevice->PortOffset)) {
 		pr_err(DEVICE_NAME ": Failed to access MAC hardware..\n");
@@ -950,7 +922,7 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	MACvInitialize(pDevice->PortOffset);
 	MACvReadEtherAddress(pDevice->PortOffset, dev->dev_addr);
 
-	device_get_options(pDevice, device_nics-1, dev->name);
+	device_get_options(pDevice);
 	device_set_options(pDevice);
 	//Mask out the options cannot be set to the chip
 	pDevice->sOpts.flags &= pChip_info->flags;
@@ -977,32 +949,21 @@ vt6655_probe(struct pci_dev *pcid, const struct pci_device_id *ent)
 	return 0;
 }
 
-static void device_print_info(PSDevice pDevice)
+static void device_print_info(struct vnt_private *pDevice)
 {
 	struct net_device *dev = pDevice->dev;
 
-	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: %s\n", dev->name, get_chip_name(pDevice->chip_id));
-	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "%s: MAC=%pM", dev->name, dev->dev_addr);
-
-	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO " IO=0x%lx Mem=0x%lx ",
-		(unsigned long)pDevice->ioaddr, (unsigned long)pDevice->PortOffset);
-	DBG_PRT(MSG_LEVEL_INFO, KERN_INFO " IRQ=%d\n", pDevice->dev->irq);
+	pr_info("%s: %s\n", dev->name, get_chip_name(pDevice->chip_id));
+	pr_info("%s: MAC=%pM IO=0x%lx Mem=0x%lx IRQ=%d\n",
+		dev->name, dev->dev_addr, (unsigned long)pDevice->ioaddr,
+		(unsigned long)pDevice->PortOffset, pDevice->dev->irq);
 }
 
-static void vt6655_init_info(struct pci_dev *pcid, PSDevice *ppDevice,
-			     PCHIP_INFO pChip_info) {
-	PSDevice p;
-
-	memset(*ppDevice, 0, sizeof(DEVICE_INFO));
-
-	if (pDevice_Infos == NULL) {
-		pDevice_Infos = *ppDevice;
-	} else {
-		for (p = pDevice_Infos; p->next != NULL; p = p->next)
-			do {} while (0);
-		p->next = *ppDevice;
-		(*ppDevice)->prev = p;
-	}
+static void vt6655_init_info(struct pci_dev *pcid,
+			     struct vnt_private **ppDevice,
+			     PCHIP_INFO pChip_info)
+{
+	memset(*ppDevice, 0, sizeof(**ppDevice));
 
 	(*ppDevice)->pcid = pcid;
 	(*ppDevice)->chip_id = pChip_info->chip_id;
@@ -1013,19 +974,12 @@ static void vt6655_init_info(struct pci_dev *pcid, PSDevice *ppDevice,
 	spin_lock_init(&((*ppDevice)->lock));
 }
 
-static bool device_get_pci_info(PSDevice pDevice, struct pci_dev *pcid)
+static bool device_get_pci_info(struct vnt_private *pDevice,
+				struct pci_dev *pcid)
 {
 	u16 pci_cmd;
 	u8  b;
 	unsigned int cis_addr;
-#ifdef	PLICE_DEBUG
-	unsigned char pci_config[256];
-	unsigned char value = 0x00;
-	int		ii, j;
-	u16	max_lat = 0x0000;
-
-	memset(pci_config, 0x00, 256);
-#endif
 
 	pci_read_config_byte(pcid, PCI_REVISION_ID, &pDevice->byRevId);
 	pci_read_config_word(pcid, PCI_SUBSYSTEM_ID, &pDevice->SubSystemID);
@@ -1044,26 +998,11 @@ static bool device_get_pci_info(PSDevice pDevice, struct pci_dev *pcid)
 	pci_read_config_byte(pcid, PCI_COMMAND, &b);
 	pci_write_config_byte(pcid, PCI_COMMAND, (b|PCI_COMMAND_MASTER));
 
-#ifdef	PLICE_DEBUG
-	for (ii = 0; ii < 0xFF; ii++) {
-		pci_read_config_byte(pcid, ii, &value);
-		pci_config[ii] = value;
-	}
-	for (ii = 0, j = 1; ii < 0x100; ii++, j++) {
-		if (j % 16 == 0) {
-			pr_debug("%x:", pci_config[ii]);
-			pr_debug("\n");
-		} else {
-			pr_debug("%x:", pci_config[ii]);
-		}
-	}
-#endif
 	return true;
 }
 
-static void device_free_info(PSDevice pDevice)
+static void device_free_info(struct vnt_private *pDevice)
 {
-	PSDevice         ptr;
 	struct net_device *dev = pDevice->dev;
 
 	ASSERT(pDevice);
@@ -1075,21 +1014,6 @@ static void device_free_info(PSDevice pDevice)
 	if (wpa_set_wpadev(pDevice, 0) != 0)
 		pr_err("unregister wpadev fail?\n");
 
-	if (pDevice_Infos == NULL)
-		return;
-
-	for (ptr = pDevice_Infos; ptr && (ptr != pDevice); ptr = ptr->next)
-		do {} while (0);
-
-	if (ptr == pDevice) {
-		if (ptr == pDevice_Infos)
-			pDevice_Infos = ptr->next;
-		else
-			ptr->prev->next = ptr->next;
-	} else {
-		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "info struct not found\n");
-		return;
-	}
 #ifdef HOSTAP
 	if (dev)
 		vt6655_hostap_set_hostapd(pDevice, 0, 0);
@@ -1106,7 +1030,7 @@ static void device_free_info(PSDevice pDevice)
 		free_netdev(dev);
 }
 
-static bool device_init_rings(PSDevice pDevice)
+static bool device_init_rings(struct vnt_private *pDevice)
 {
 	void *vir_pool;
 
@@ -1118,7 +1042,7 @@ static bool device_init_rings(PSDevice pDevice)
 					 pDevice->sOpts.nTxDescs[1] * sizeof(STxDesc),
 					 &pDevice->pool_dma);
 	if (vir_pool == NULL) {
-		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s : allocate desc dma memory failed\n", pDevice->dev->name);
+		dev_err(&pDevice->pcid->dev, "allocate desc dma memory failed\n");
 		return false;
 	}
 
@@ -1137,7 +1061,8 @@ static bool device_init_rings(PSDevice pDevice)
 						  CB_MAX_BUF_SIZE,
 						  &pDevice->tx_bufs_dma0);
 	if (pDevice->tx0_bufs == NULL) {
-		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: allocate buf dma memory failed\n", pDevice->dev->name);
+		dev_err(&pDevice->pcid->dev, "allocate buf dma memory failed\n");
+
 		pci_free_consistent(pDevice->pcid,
 				    pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
 				    pDevice->sOpts.nRxDescs1 * sizeof(SRxDesc) +
@@ -1182,7 +1107,7 @@ static bool device_init_rings(PSDevice pDevice)
 	return true;
 }
 
-static void device_free_rings(PSDevice pDevice)
+static void device_free_rings(struct vnt_private *pDevice)
 {
 	pci_free_consistent(pDevice->pcid,
 			    pDevice->sOpts.nRxDescs0 * sizeof(SRxDesc) +
@@ -1203,7 +1128,7 @@ static void device_free_rings(PSDevice pDevice)
 			);
 }
 
-static void device_init_rd0_ring(PSDevice pDevice)
+static void device_init_rd0_ring(struct vnt_private *pDevice)
 {
 	int i;
 	dma_addr_t      curr = pDevice->rd0_pool_dma;
@@ -1214,10 +1139,9 @@ static void device_init_rd0_ring(PSDevice pDevice)
 		pDesc = &(pDevice->aRD0Ring[i]);
 		pDesc->pRDInfo = alloc_rd_info();
 		ASSERT(pDesc->pRDInfo);
-		if (!device_alloc_rx_buf(pDevice, pDesc)) {
-			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc rx bufs\n",
-				pDevice->dev->name);
-		}
+		if (!device_alloc_rx_buf(pDevice, pDesc))
+			dev_err(&pDevice->pcid->dev, "can not alloc rx bufs\n");
+
 		pDesc->next = &(pDevice->aRD0Ring[(i+1) % pDevice->sOpts.nRxDescs0]);
 		pDesc->pRDInfo->curr_desc = cpu_to_le32(curr);
 		pDesc->next_desc = cpu_to_le32(curr + sizeof(SRxDesc));
@@ -1228,7 +1152,7 @@ static void device_init_rd0_ring(PSDevice pDevice)
 	pDevice->pCurrRD[0] = &(pDevice->aRD0Ring[0]);
 }
 
-static void device_init_rd1_ring(PSDevice pDevice)
+static void device_init_rd1_ring(struct vnt_private *pDevice)
 {
 	int i;
 	dma_addr_t      curr = pDevice->rd1_pool_dma;
@@ -1239,10 +1163,9 @@ static void device_init_rd1_ring(PSDevice pDevice)
 		pDesc = &(pDevice->aRD1Ring[i]);
 		pDesc->pRDInfo = alloc_rd_info();
 		ASSERT(pDesc->pRDInfo);
-		if (!device_alloc_rx_buf(pDevice, pDesc)) {
-			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc rx bufs\n",
-				pDevice->dev->name);
-		}
+		if (!device_alloc_rx_buf(pDevice, pDesc))
+			dev_err(&pDevice->pcid->dev, "can not alloc rx bufs\n");
+
 		pDesc->next = &(pDevice->aRD1Ring[(i+1) % pDevice->sOpts.nRxDescs1]);
 		pDesc->pRDInfo->curr_desc = cpu_to_le32(curr);
 		pDesc->next_desc = cpu_to_le32(curr + sizeof(SRxDesc));
@@ -1253,7 +1176,7 @@ static void device_init_rd1_ring(PSDevice pDevice)
 	pDevice->pCurrRD[1] = &(pDevice->aRD1Ring[0]);
 }
 
-static void device_init_defrag_cb(PSDevice pDevice)
+static void device_init_defrag_cb(struct vnt_private *pDevice)
 {
 	int i;
 	PSDeFragControlBlock pDeF;
@@ -1261,16 +1184,14 @@ static void device_init_defrag_cb(PSDevice pDevice)
 	/* Init the fragment ctl entries */
 	for (i = 0; i < CB_MAX_RX_FRAG; i++) {
 		pDeF = &(pDevice->sRxDFCB[i]);
-		if (!device_alloc_frag_buf(pDevice, pDeF)) {
-			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc frag bufs\n",
-				pDevice->dev->name);
-		}
+		if (!device_alloc_frag_buf(pDevice, pDeF))
+			dev_err(&pDevice->pcid->dev, "can not alloc frag bufs\n");
 	}
 	pDevice->cbDFCB = CB_MAX_RX_FRAG;
 	pDevice->cbFreeDFCB = pDevice->cbDFCB;
 }
 
-static void device_free_rd0_ring(PSDevice pDevice)
+static void device_free_rd0_ring(struct vnt_private *pDevice)
 {
 	int i;
 
@@ -1287,7 +1208,7 @@ static void device_free_rd0_ring(PSDevice pDevice)
 	}
 }
 
-static void device_free_rd1_ring(PSDevice pDevice)
+static void device_free_rd1_ring(struct vnt_private *pDevice)
 {
 	int i;
 
@@ -1304,7 +1225,7 @@ static void device_free_rd1_ring(PSDevice pDevice)
 	}
 }
 
-static void device_free_frag_buf(PSDevice pDevice)
+static void device_free_frag_buf(struct vnt_private *pDevice)
 {
 	PSDeFragControlBlock pDeF;
 	int i;
@@ -1318,7 +1239,7 @@ static void device_free_frag_buf(PSDevice pDevice)
 	}
 }
 
-static void device_init_td0_ring(PSDevice pDevice)
+static void device_init_td0_ring(struct vnt_private *pDevice)
 {
 	int i;
 	dma_addr_t  curr;
@@ -1343,7 +1264,7 @@ static void device_init_td0_ring(PSDevice pDevice)
 	pDevice->apTailTD[0] = pDevice->apCurrTD[0] = &(pDevice->apTD0Rings[0]);
 }
 
-static void device_init_td1_ring(PSDevice pDevice)
+static void device_init_td1_ring(struct vnt_private *pDevice)
 {
 	int i;
 	dma_addr_t  curr;
@@ -1369,7 +1290,7 @@ static void device_init_td1_ring(PSDevice pDevice)
 	pDevice->apTailTD[1] = pDevice->apCurrTD[1] = &(pDevice->apTD1Rings[0]);
 }
 
-static void device_free_td0_ring(PSDevice pDevice)
+static void device_free_td0_ring(struct vnt_private *pDevice)
 {
 	int i;
 
@@ -1388,7 +1309,7 @@ static void device_free_td0_ring(PSDevice pDevice)
 	}
 }
 
-static void device_free_td1_ring(PSDevice pDevice)
+static void device_free_td1_ring(struct vnt_private *pDevice)
 {
 	int i;
 
@@ -1409,7 +1330,7 @@ static void device_free_td1_ring(PSDevice pDevice)
 
 /*-----------------------------------------------------------------*/
 
-static int device_rx_srv(PSDevice pDevice, unsigned int uIdx)
+static int device_rx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 {
 	PSRxDesc    pRD;
 	int works = 0;
@@ -1421,8 +1342,8 @@ static int device_rx_srv(PSDevice pDevice, unsigned int uIdx)
 			break;
 		if (device_receive_frame(pDevice, pRD)) {
 			if (!device_alloc_rx_buf(pDevice, pRD)) {
-				DBG_PRT(MSG_LEVEL_ERR, KERN_ERR
-					"%s: can not allocate rx buf\n", pDevice->dev->name);
+				dev_err(&pDevice->pcid->dev,
+					"can not allocate rx buf\n");
 				break;
 			}
 		}
@@ -1435,7 +1356,7 @@ static int device_rx_srv(PSDevice pDevice, unsigned int uIdx)
 	return works;
 }
 
-static bool device_alloc_rx_buf(PSDevice pDevice, PSRxDesc pRD)
+static bool device_alloc_rx_buf(struct vnt_private *pDevice, PSRxDesc pRD)
 {
 	PDEVICE_RD_INFO pRDInfo = pRD->pRDInfo;
 
@@ -1456,7 +1377,8 @@ static bool device_alloc_rx_buf(PSDevice pDevice, PSRxDesc pRD)
 	return true;
 }
 
-bool device_alloc_frag_buf(PSDevice pDevice, PSDeFragControlBlock pDeF)
+bool device_alloc_frag_buf(struct vnt_private *pDevice,
+			   PSDeFragControlBlock pDeF)
 {
 	pDeF->skb = dev_alloc_skb((int)pDevice->rx_buf_sz);
 	if (pDeF->skb == NULL)
@@ -1467,7 +1389,7 @@ bool device_alloc_frag_buf(PSDevice pDevice, PSDeFragControlBlock pDeF)
 	return true;
 }
 
-static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
+static int device_tx_srv(struct vnt_private *pDevice, unsigned int uIdx)
 {
 	PSTxDesc                 pTD;
 	bool bFull = false;
@@ -1476,7 +1398,7 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 	unsigned char byTsr1;
 	unsigned int	uFrameSize, uFIFOHeaderSize;
 	PSTxBufHead              pTxBufHead;
-	struct net_device_stats *pStats = &pDevice->stats;
+	struct net_device_stats *pStats = &pDevice->dev->stats;
 	struct sk_buff *skb;
 	unsigned int	uNodeIndex;
 	PSMgmtObject             pMgmt = pDevice->pMgmt;
@@ -1512,8 +1434,9 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 
 				if (!(byTsr1 & TSR1_TERR)) {
 					if (byTsr0 != 0) {
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] OK but has error. tsr1[%02X] tsr0[%02X].\n",
-							(int)uIdx, byTsr1, byTsr0);
+						pr_debug(" Tx[%d] OK but has error. tsr1[%02X] tsr0[%02X]\n",
+							 (int)uIdx, byTsr1,
+							 byTsr0);
 					}
 					if ((pTxBufHead->wFragCtl & FRAGCTL_ENDFRAG) != FRAGCTL_NONFRAG)
 						pDevice->s802_11Counter.TransmittedFragmentCount++;
@@ -1521,8 +1444,8 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 					pStats->tx_packets++;
 					pStats->tx_bytes += pTD->pTDInfo->skb->len;
 				} else {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] dropped & tsr1[%02X] tsr0[%02X].\n",
-						(int)uIdx, byTsr1, byTsr0);
+					pr_debug(" Tx[%d] dropped & tsr1[%02X] tsr0[%02X]\n",
+						 (int)uIdx, byTsr1, byTsr0);
 					pStats->tx_errors++;
 					pStats->tx_dropped++;
 				}
@@ -1530,7 +1453,7 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 
 			if ((pTD->pTDInfo->byFlags & TD_FLAGS_PRIV_SKB) != 0) {
 				if (pDevice->bEnableHostapd) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "tx call back netif..\n");
+					pr_debug("tx call back netif..\n");
 					skb = pTD->pTDInfo->skb;
 					skb->dev = pDevice->apdev;
 					skb_reset_mac_header(skb);
@@ -1542,8 +1465,8 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 
 			if (byTsr1 & TSR1_TERR) {
 				if ((pTD->pTDInfo->byFlags & TD_FLAGS_PRIV_SKB) != 0) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Tx[%d] fail has error. tsr1[%02X] tsr0[%02X].\n",
-						(int)uIdx, byTsr1, byTsr0);
+					pr_debug(" Tx[%d] fail has error. tsr1[%02X] tsr0[%02X]\n",
+						 (int)uIdx, byTsr1, byTsr0);
 				}
 
 
@@ -1561,8 +1484,9 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 							wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
 							pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
 							pTD->pTDInfo->byFlags &= ~(TD_FLAGS_NETIF_SKB);
-							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "tx_srv:tx fail re-queue sta index= %d, QueCnt= %d\n"
-								, (int)uNodeIndex, pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt);
+							pr_debug("tx_srv:tx fail re-queue sta index= %d, QueCnt= %d\n",
+								 (int)uNodeIndex,
+								 pMgmt->sNodeDBTable[uNodeIndex].wEnQueueCnt);
 							pStats->tx_errors--;
 							pStats->tx_dropped--;
 						}
@@ -1579,7 +1503,8 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 
 		if (AVAIL_TD(pDevice, uIdx) < RESERV_AC0DMA) {
 			bFull = true;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " AC0DMA is Full = %d\n", pDevice->iTDUsed[uIdx]);
+			pr_debug(" AC0DMA is Full = %d\n",
+				 pDevice->iTDUsed[uIdx]);
 		}
 		if (netif_queue_stopped(pDevice->dev) && !bFull)
 			netif_wake_queue(pDevice->dev);
@@ -1591,12 +1516,11 @@ static int device_tx_srv(PSDevice pDevice, unsigned int uIdx)
 	return works;
 }
 
-static void device_error(PSDevice pDevice, unsigned short status)
+static void device_error(struct vnt_private *pDevice, unsigned short status)
 {
 	if (status & ISR_FETALERR) {
-		DBG_PRT(MSG_LEVEL_ERR, KERN_ERR
-			"%s: Hardware fatal error.\n",
-			pDevice->dev->name);
+		dev_err(&pDevice->pcid->dev, "Hardware fatal error\n");
+
 		netif_stop_queue(pDevice->dev);
 		del_timer(&pDevice->sTimerCommand);
 		del_timer(&(pDevice->pMgmt->sTimerSecondCallback));
@@ -1606,7 +1530,7 @@ static void device_error(PSDevice pDevice, unsigned short status)
 	}
 }
 
-static void device_free_tx_buf(PSDevice pDevice, PSTxDesc pDesc)
+static void device_free_tx_buf(struct vnt_private *pDevice, PSTxDesc pDesc)
 {
 	PDEVICE_TD_INFO  pTDInfo = pDesc->pTDInfo;
 	struct sk_buff *skb = pTDInfo->skb;
@@ -1627,7 +1551,7 @@ static void device_free_tx_buf(PSDevice pDevice, PSTxDesc pDesc)
 
 static int  device_open(struct net_device *dev)
 {
-	PSDevice pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	int i;
 #ifdef WPA_SM_Transtatus
 	extern SWPAResult wpa_Result;
@@ -1650,7 +1574,7 @@ static int  device_open(struct net_device *dev)
 	wpa_Result.authenticated = false;
 	pDevice->fWPA_Authened = false;
 #endif
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device init rd0 ring\n");
+	pr_debug("call device init rd0 ring\n");
 	device_init_rd0_ring(pDevice);
 	device_init_rd1_ring(pDevice);
 	device_init_defrag_cb(pDevice);
@@ -1663,7 +1587,7 @@ static int  device_open(struct net_device *dev)
 	vMgrObjectInit(pDevice);
 	vMgrTimerInit(pDevice);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call device_init_registers\n");
+	pr_debug("call device_init_registers\n");
 	device_init_registers(pDevice);
 
 	MACvReadEtherAddress(pDevice->PortOffset, pDevice->abyCurrentNetAddr);
@@ -1694,7 +1618,7 @@ static int  device_open(struct net_device *dev)
 		pDevice->eEncryptionStatus = Ndis802_11Encryption1Enabled;
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "call MACvIntEnable\n");
+	pr_debug("call MACvIntEnable\n");
 	MACvIntEnable(pDevice->PortOffset, IMR_MASK_VALUE);
 
 	if (pDevice->pMgmt->eConfigMode == WMAC_CONFIG_AP) {
@@ -1705,24 +1629,21 @@ static int  device_open(struct net_device *dev)
 	}
 	pDevice->flags |= DEVICE_FLAGS_OPENED;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_open success..\n");
+	pr_debug("device_open success..\n");
 	return 0;
 }
 
 static int  device_close(struct net_device *dev)
 {
-	PSDevice  pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject     pMgmt = pDevice->pMgmt;
-	//PLICE_DEBUG->
-//PLICE_DEBUG<-
 //2007-1121-02<Add>by EinsnLiu
 	if (pDevice->bLinkPass) {
 		bScheduleCommand((void *)pDevice, WLAN_CMD_DISASSOCIATE, NULL);
 		mdelay(30);
 	}
-#ifdef TxInSleep
+
 	del_timer(&pDevice->sTimerTxData);
-#endif
 	del_timer(&pDevice->sTimerCommand);
 	del_timer(&pMgmt->sTimerSecondCallback);
 	if (pDevice->bDiversityRegCtlON) {
@@ -1752,21 +1673,21 @@ static int  device_close(struct net_device *dev)
 	//2008-0714-01<Add>by chester
 	device_release_WPADEV(pDevice);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_close..\n");
+	pr_debug("device_close..\n");
 	return 0;
 }
 
 static int device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev)
 {
-	PSDevice pDevice = netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	unsigned char *pbMPDU;
 	unsigned int cbMPDULen = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_tx_80211\n");
+	pr_debug("device_dma0_tx_80211\n");
 	spin_lock_irq(&pDevice->lock);
 
 	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_tx_80211, td0 <=0\n");
+		pr_debug("device_dma0_tx_80211, td0 <=0\n");
 		dev_kfree_skb_irq(skb);
 		spin_unlock_irq(&pDevice->lock);
 		return 0;
@@ -1788,7 +1709,8 @@ static int device_dma0_tx_80211(struct sk_buff *skb, struct net_device *dev)
 	return 0;
 }
 
-bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeIndex)
+bool device_dma0_xmit(struct vnt_private *pDevice,
+		      struct sk_buff *skb, unsigned int uNodeIndex)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PSTxDesc        pHeadTD, pLastTD;
@@ -1808,14 +1730,14 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 
 	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 0) {
 		dev_kfree_skb_irq(skb);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_xmit, td0 <=0\n");
+		pr_debug("device_dma0_xmit, td0 <=0\n");
 		return false;
 	}
 
 	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 		if (pDevice->uAssocCount == 0) {
 			dev_kfree_skb_irq(skb);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "device_dma0_xmit, assocCount = 0\n");
+			pr_debug("device_dma0_xmit, assocCount = 0\n");
 			return false;
 		}
 	}
@@ -1861,7 +1783,7 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 	else
 		pDevice->byPreambleType = PREAMBLE_LONG;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dma0: pDevice->wCurrentRate = %d\n", pDevice->wCurrentRate);
+	pr_debug("dma0: pDevice->wCurrentRate = %d\n", pDevice->wCurrentRate);
 
 	if (pDevice->wCurrentRate <= RATE_11M) {
 		byPktType = PK_TYPE_11B;
@@ -1928,10 +1850,8 @@ bool device_dma0_xmit(PSDevice pDevice, struct sk_buff *skb, unsigned int uNodeI
 }
 
 //TYPE_AC0DMA data tx
-static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
-{
-	PSDevice pDevice = netdev_priv(dev);
-
+static int  device_xmit(struct sk_buff *skb, struct net_device *dev) {
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PSTxDesc        pHeadTD, pLastTD;
 	unsigned int uNodeIndex = 0;
@@ -1988,8 +1908,9 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 					// set tx map
 					wAID = pMgmt->sNodeDBTable[uNodeIndex].wAID;
 					pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set:pMgmt->abyPSTxMap[%d]= %d\n",
-						(wAID >> 3), pMgmt->abyPSTxMap[wAID >> 3]);
+					pr_debug("Set:pMgmt->abyPSTxMap[%d]= %d\n",
+						 (wAID >> 3),
+						 pMgmt->abyPSTxMap[wAID >> 3]);
 					spin_unlock_irq(&pDevice->lock);
 					return 0;
 				}
@@ -2005,7 +1926,7 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 		}
 
 		if (!bNodeExist) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Unknown STA not found in node DB\n");
+			pr_debug("Unknown STA not found in node DB\n");
 			dev_kfree_skb_irq(skb);
 			spin_unlock_irq(&pDevice->lock);
 			return 0;
@@ -2034,19 +1955,19 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 					// get group key
 					if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
 						bTKIP_UseGTK = true;
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get GTK.\n");
+						pr_debug("Get GTK\n");
 						break;
 					}
 				} else {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get PTK.\n");
+					pr_debug("Get PTK\n");
 					break;
 				}
 			} else if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
 				pbyBSSID = pDevice->sTxEthHeader.abyDstAddr;  //TO_DS = 0 and FROM_DS = 0 --> 802.11 MAC Address1
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "IBSS Serach Key:\n");
+				pr_debug("IBSS Serach Key:\n");
 				for (ii = 0; ii < 6; ii++)
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "%x\n", *(pbyBSSID+ii));
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "\n");
+					pr_debug("%x\n", *(pbyBSSID+ii));
+				pr_debug("\n");
 
 				// get pairwise key
 				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == true)
@@ -2057,18 +1978,20 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
 				pTransmitKey = NULL;
 				if (pDevice->pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
+					pr_debug("IBSS and KEY is NULL. [%d]\n",
+						 pDevice->pMgmt->eCurrMode);
 				else
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "NOT IBSS and KEY is NULL. [%d]\n", pDevice->pMgmt->eCurrMode);
+					pr_debug("NOT IBSS and KEY is NULL. [%d]\n",
+						 pDevice->pMgmt->eCurrMode);
 			} else {
 				bTKIP_UseGTK = true;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get GTK.\n");
+				pr_debug("Get GTK\n");
 			}
 		} while (false);
 	}
 
 	if (pDevice->bEnableHostWEP) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "acdma0: STA index %d\n", uNodeIndex);
+		pr_debug("acdma0: STA index %d\n", uNodeIndex);
 		if (pDevice->bEncryptionEnable) {
 			pTransmitKey = &STempKey;
 			pTransmitKey->byCipherSuite = pMgmt->sNodeDBTable[uNodeIndex].byCipherSuite;
@@ -2086,7 +2009,8 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 	uMACfragNum = cbGetFragCount(pDevice, pTransmitKey, cbFrameBodySize, &pDevice->sTxEthHeader);
 
 	if (uMACfragNum > AVAIL_TD(pDevice, TYPE_AC0DMA)) {
-		DBG_PRT(MSG_LEVEL_ERR, KERN_DEBUG "uMACfragNum > AVAIL_TD(TYPE_AC0DMA) = %d\n", uMACfragNum);
+		pr_debug("uMACfragNum > AVAIL_TD(TYPE_AC0DMA) = %d\n",
+			 uMACfragNum);
 		dev_kfree_skb_irq(skb);
 		spin_unlock_irq(&pDevice->lock);
 		return 0;
@@ -2102,10 +2026,6 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 	byPktType = (unsigned char)pDevice->byPacketType;
 
 	if (pDevice->bFixRate) {
-#ifdef	PLICE_DEBUG
-		pr_debug("Fix Rate: PhyType is %d,ConnectionRate is %d\n", pDevice->eCurrentPHYType, pDevice->uConnectionRate);
-#endif
-
 		if (pDevice->eCurrentPHYType == PHY_TYPE_11B) {
 			if (pDevice->uConnectionRate >= RATE_11M)
 				pDevice->wCurrentRate = RATE_11M;
@@ -2164,18 +2084,21 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	if (bNeedEncryption) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ntohs Pkt Type=%04x\n", ntohs(pDevice->sTxEthHeader.wType));
+		pr_debug("ntohs Pkt Type=%04x\n",
+			 ntohs(pDevice->sTxEthHeader.wType));
 		if ((pDevice->sTxEthHeader.wType) == TYPE_PKT_802_1x) {
 			bNeedEncryption = false;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pkt Type=%04x\n", (pDevice->sTxEthHeader.wType));
+			pr_debug("Pkt Type=%04x\n",
+				 (pDevice->sTxEthHeader.wType));
 			if ((pDevice->pMgmt->eCurrMode == WMAC_MODE_ESS_STA) && (pDevice->pMgmt->eCurrState == WMAC_STATE_ASSOC)) {
 				if (pTransmitKey == NULL) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Don't Find TX KEY\n");
+					pr_debug("Don't Find TX KEY\n");
 				} else {
 					if (bTKIP_UseGTK) {
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "error: KEY is GTK!!~~\n");
+						pr_debug("error: KEY is GTK!!~~\n");
 					} else {
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Find PTK [%lX]\n", pTransmitKey->dwKeyIndex);
+						pr_debug("Find PTK [%lX]\n",
+							 pTransmitKey->dwKeyIndex);
 						bNeedEncryption = true;
 					}
 				}
@@ -2189,13 +2112,14 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 			if (pDevice->bEnableHostWEP) {
 				if ((uNodeIndex != 0) &&
 				    (pMgmt->sNodeDBTable[uNodeIndex].dwKeyIndex & PAIRWISE_KEY)) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Find PTK [%lX]\n", pTransmitKey->dwKeyIndex);
+					pr_debug("Find PTK [%lX]\n",
+						 pTransmitKey->dwKeyIndex);
 					bNeedEncryption = true;
 				}
 			}
 		} else {
 			if (pTransmitKey == NULL) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return no tx key\n");
+				pr_debug("return no tx key\n");
 				dev_kfree_skb_irq(skb);
 				spin_unlock_irq(&pDevice->lock);
 				return 0;
@@ -2232,9 +2156,8 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 	pLastTD->pTDInfo->skb = skb;
 	pLastTD->pTDInfo->byFlags = 0;
 	pLastTD->pTDInfo->byFlags |= TD_FLAGS_NETIF_SKB;
-#ifdef TxInSleep
 	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
-#endif
+
 	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 1)
 		netif_stop_queue(dev);
 
@@ -2284,8 +2207,7 @@ static int  device_xmit(struct sk_buff *skb, struct net_device *dev)
 static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 {
 	struct net_device *dev = dev_instance;
-	PSDevice     pDevice = (PSDevice)netdev_priv(dev);
-
+	struct vnt_private *pDevice = netdev_priv(dev);
 	int             max_count = 0;
 	unsigned long dwMIBCounter = 0;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
@@ -2301,7 +2223,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 		return IRQ_RETVAL(handled);
 
 	if (pDevice->dwIsr == 0xffffffff) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwIsr = 0xffff\n");
+		pr_debug("dwIsr = 0xffff\n");
 		return IRQ_RETVAL(handled);
 	}
 
@@ -2328,7 +2250,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 		MACvWriteISR(pDevice->PortOffset, pDevice->dwIsr);
 
 		if (pDevice->dwIsr & ISR_FETALERR) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " ISR_FETALERR\n");
+			pr_debug(" ISR_FETALERR\n");
 			VNSvOutPortB(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, 0);
 			VNSvOutPortW(pDevice->PortOffset + MAC_REG_SOFTPWRCTL, SOFTPWRCTL_SWPECTI);
 			device_error(pDevice, pDevice->dwIsr);
@@ -2409,7 +2331,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 				}
 			}
 			if (pDevice->bChannelSwitch &&
-			    (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)) {
+			    (pDevice->op_mode == NL80211_IFTYPE_STATION)) {
 				pDevice->byChannelSwitchCount--;
 				if (pDevice->byChannelSwitchCount == 0) {
 					pDevice->bChannelSwitch = false;
@@ -2422,7 +2344,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 
 				}
 			}
-			if (pDevice->eOPMode != OP_MODE_ADHOC) {
+			if (pDevice->op_mode != NL80211_IFTYPE_ADHOC) {
 				if ((pDevice->bUpdateBBVGA) && pDevice->bLinkPass && (pDevice->uCurrRSSI != 0)) {
 					long            ldBm;
 
@@ -2438,12 +2360,18 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 						if (pDevice->uBBVGADiffCount == 1) {
 							// first VGA diff gain
 							BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
-							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "First RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
-								(int)ldBm, pDevice->byBBVGANew, pDevice->byBBVGACurrent, (int)pDevice->uBBVGADiffCount);
+							pr_debug("First RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
+								 (int)ldBm,
+								 pDevice->byBBVGANew,
+								 pDevice->byBBVGACurrent,
+								 (int)pDevice->uBBVGADiffCount);
 						}
 						if (pDevice->uBBVGADiffCount >= BB_VGA_CHANGE_THRESHOLD) {
-							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
-								(int)ldBm, pDevice->byBBVGANew, pDevice->byBBVGACurrent, (int)pDevice->uBBVGADiffCount);
+							pr_debug("RSSI[%d] NewGain[%d] OldGain[%d] Count[%d]\n",
+								 (int)ldBm,
+								 pDevice->byBBVGANew,
+								 pDevice->byBBVGACurrent,
+								 (int)pDevice->uBBVGADiffCount);
 							BBvSetVGAGainOffset(pDevice, pDevice->byBBVGANew);
 						}
 					} else {
@@ -2456,8 +2384,8 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 			if (pDevice->bEnablePSMode)
 				PSbIsNextTBTTWakeUp((void *)pDevice);
 
-			if ((pDevice->eOPMode == OP_MODE_AP) ||
-			    (pDevice->eOPMode == OP_MODE_ADHOC)) {
+			if ((pDevice->op_mode == NL80211_IFTYPE_AP) ||
+			    (pDevice->op_mode == NL80211_IFTYPE_ADHOC)) {
 				MACvOneShotTimer1MicroSec(pDevice->PortOffset,
 							  (pMgmt->wIBSSBeaconPeriod - MAKE_BEACON_RESERVED) << 10);
 			}
@@ -2467,12 +2395,12 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 		}
 
 		if (pDevice->dwIsr & ISR_BNTX) {
-			if (pDevice->eOPMode == OP_MODE_ADHOC) {
+			if (pDevice->op_mode == NL80211_IFTYPE_ADHOC) {
 				pDevice->bIsBeaconBufReadySet = false;
 				pDevice->cbBeaconBufReadySetCnt = 0;
 			}
 
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->op_mode == NL80211_IFTYPE_AP) {
 				if (pMgmt->byDTIMCount > 0) {
 					pMgmt->byDTIMCount--;
 					pMgmt->sNodeDBTable[0].bRxPSPoll = false;
@@ -2515,7 +2443,7 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 			max_count += device_tx_srv(pDevice, TYPE_AC0DMA);
 
 		if (pDevice->dwIsr & ISR_SOFTTIMER1) {
-			if (pDevice->eOPMode == OP_MODE_AP) {
+			if (pDevice->op_mode == NL80211_IFTYPE_AP) {
 				if (pDevice->bShortSlotTime)
 					pMgmt->wCurrCapInfo |= WLAN_SET_CAP_INFO_SHORTSLOTTIME(1);
 				else
@@ -2544,23 +2472,6 @@ static  irqreturn_t  device_intr(int irq,  void *dev_instance)
 	return IRQ_RETVAL(handled);
 }
 
-static unsigned const ethernet_polynomial = 0x04c11db7U;
-static inline u32 ether_crc(int length, unsigned char *data)
-{
-	int crc = -1;
-
-	while (--length >= 0) {
-		unsigned char current_octet = *data++;
-		int bit;
-
-		for (bit = 0; bit < 8; bit++, current_octet >>= 1) {
-			crc = (crc << 1) ^
-				((crc < 0) ^ (current_octet & 1) ? ethernet_polynomial : 0);
-		}
-	}
-	return crc;
-}
-
 //2008-8-4 <add> by chester
 static int Config_FileGetParameter(unsigned char *string,
 				   unsigned char *dest, unsigned char *source)
@@ -2577,7 +2488,8 @@ static int Config_FileGetParameter(unsigned char *string,
 	return true;
 }
 
-int Config_FileOperation(PSDevice pDevice, bool fwrite, unsigned char *Parameter)
+int Config_FileOperation(struct vnt_private *pDevice,
+			 bool fwrite, unsigned char *Parameter)
 {
 	unsigned char *buffer = kmalloc(1024, GFP_KERNEL);
 	unsigned char tmpbuffer[20];
@@ -2601,7 +2513,7 @@ int Config_FileOperation(PSDevice pDevice, bool fwrite, unsigned char *Parameter
 		goto error1;
 	}
 
-	if (Config_FileGetParameter("ZONETYPE",tmpbuffer,buffer)!=true) {
+	if (Config_FileGetParameter("ZONETYPE", tmpbuffer, buffer) != true) {
 		pr_err("get parameter error?\n");
 		result = -1;
 		goto error1;
@@ -2609,9 +2521,9 @@ int Config_FileOperation(PSDevice pDevice, bool fwrite, unsigned char *Parameter
 
 	if (memcmp(tmpbuffer, "USA", 3) == 0) {
 		result = ZoneType_USA;
-	} else if(memcmp(tmpbuffer, "JAPAN", 5) == 0) {
+	} else if (memcmp(tmpbuffer, "JAPAN", 5) == 0) {
 		result = ZoneType_Japan;
-	} else if(memcmp(tmpbuffer, "EUROPE", 5) == 0) {
+	} else if (memcmp(tmpbuffer, "EUROPE", 5) == 0) {
 		result = ZoneType_Europe;
 	} else {
 		result = -1;
@@ -2624,10 +2536,8 @@ error1:
 	return result;
 }
 
-static void device_set_multi(struct net_device *dev)
-{
-	PSDevice         pDevice = (PSDevice)netdev_priv(dev);
-
+static void device_set_multi(struct net_device *dev) {
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject     pMgmt = pDevice->pMgmt;
 	u32              mc_filter[2];
 	struct netdev_hw_addr *ha;
@@ -2635,7 +2545,7 @@ static void device_set_multi(struct net_device *dev)
 	VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
 
 	if (dev->flags & IFF_PROMISC) {         /* Set promiscuous. */
-		DBG_PRT(MSG_LEVEL_ERR, KERN_NOTICE "%s: Promiscuous mode enabled.\n", dev->name);
+		pr_notice("%s: Promiscuous mode enabled\n", dev->name);
 		/* Unconditionally log net taps. */
 		pDevice->byRxMode |= (RCR_MULTICAST|RCR_BROADCAST|RCR_UNICAST);
 	} else if ((netdev_mc_count(dev) > pDevice->multicast_limit)
@@ -2667,20 +2577,12 @@ static void device_set_multi(struct net_device *dev)
 	}
 
 	VNSvOutPortB(pDevice->PortOffset + MAC_REG_RCR, pDevice->byRxMode);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pDevice->byRxMode = %x\n", pDevice->byRxMode);
-}
-
-static struct net_device_stats *device_get_stats(struct net_device *dev)
-{
-	PSDevice pDevice = (PSDevice)netdev_priv(dev);
-
-	return &pDevice->stats;
+	pr_debug("pDevice->byRxMode = %x\n", pDevice->byRxMode);
 }
 
 static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
-
+	struct vnt_private *pDevice = netdev_priv(dev);
 	struct iwreq *wrq = (struct iwreq *)rq;
 	int rc = 0;
 	PSMgmtObject pMgmt = pDevice->pMgmt;
@@ -2758,13 +2660,13 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 
 		// Set desired station name
 	case SIOCSIWNICKN:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWNICKN\n");
+		pr_debug(" SIOCSIWNICKN\n");
 		rc = -EOPNOTSUPP;
 		break;
 
 		// Get current station name
 	case SIOCGIWNICKN:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWNICKN\n");
+		pr_debug(" SIOCGIWNICKN\n");
 		rc = -EOPNOTSUPP;
 		break;
 
@@ -2861,12 +2763,12 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 
 		// Get the current Tx-Power
 	case SIOCGIWTXPOW:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWTXPOW\n");
+		pr_debug(" SIOCGIWTXPOW\n");
 		rc = -EOPNOTSUPP;
 		break;
 
 	case SIOCSIWTXPOW:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWTXPOW\n");
+		pr_debug(" SIOCSIWTXPOW\n");
 		rc = -EOPNOTSUPP;
 		break;
 
@@ -2909,7 +2811,7 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 		break;
 
 	case SIOCSIWSENS:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSENS\n");
+		pr_debug(" SIOCSIWSENS\n");
 		rc = -EOPNOTSUPP;
 		break;
 
@@ -2933,50 +2835,50 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	// Set the spy list
 	case SIOCSIWSPY:
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSPY\n");
+		pr_debug(" SIOCSIWSPY\n");
 		rc = -EOPNOTSUPP;
 		break;
 
 		// Get the spy list
 	case SIOCGIWSPY:
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSPY\n");
+		pr_debug(" SIOCGIWSPY\n");
 		rc = -EOPNOTSUPP;
 		break;
 
 #endif // WIRELESS_SPY
 
 	case SIOCGIWPRIV:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPRIV\n");
+		pr_debug(" SIOCGIWPRIV\n");
 		rc = -EOPNOTSUPP;
 		break;
 
 //2008-0409-07, <Add> by Einsn Liu
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 	case SIOCSIWAUTH:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAUTH\n");
+		pr_debug(" SIOCSIWAUTH\n");
 		rc = iwctl_siwauth(dev, NULL, &(wrq->u.param), NULL);
 		break;
 
 	case SIOCGIWAUTH:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAUTH\n");
+		pr_debug(" SIOCGIWAUTH\n");
 		rc = iwctl_giwauth(dev, NULL, &(wrq->u.param), NULL);
 		break;
 
 	case SIOCSIWGENIE:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWGENIE\n");
+		pr_debug(" SIOCSIWGENIE\n");
 		rc = iwctl_siwgenie(dev, NULL, &(wrq->u.data), wrq->u.data.pointer);
 		break;
 
 	case SIOCGIWGENIE:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWGENIE\n");
+		pr_debug(" SIOCGIWGENIE\n");
 		rc = iwctl_giwgenie(dev, NULL, &(wrq->u.data), wrq->u.data.pointer);
 		break;
 
 	case SIOCSIWENCODEEXT: {
 		char extra[sizeof(struct iw_encode_ext)+MAX_KEY_LEN+1];
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODEEXT\n");
+		pr_debug(" SIOCSIWENCODEEXT\n");
 		if (wrq->u.encoding.pointer) {
 			memset(extra, 0, sizeof(struct iw_encode_ext)+MAX_KEY_LEN + 1);
 			if (wrq->u.encoding.length > (sizeof(struct iw_encode_ext) + MAX_KEY_LEN)) {
@@ -2996,12 +2898,12 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 	break;
 
 	case SIOCGIWENCODEEXT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODEEXT\n");
+		pr_debug(" SIOCGIWENCODEEXT\n");
 		rc = iwctl_giwencodeext(dev, NULL, &(wrq->u.encoding), NULL);
 		break;
 
 	case SIOCSIWMLME:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMLME\n");
+		pr_debug(" SIOCSIWMLME\n");
 		rc = iwctl_siwmlme(dev, NULL, &(wrq->u.data), wrq->u.data.pointer);
 		break;
 
@@ -3059,7 +2961,7 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 
 	default:
 		rc = -EOPNOTSUPP;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Ioctl command not support..%x\n", cmd);
+		pr_debug("Ioctl command not support..%x\n", cmd);
 
 	}
 
@@ -3070,7 +2972,7 @@ static int  device_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
 			bScheduleCommand((void *)pDevice, WLAN_CMD_RUN_AP, NULL);
 			spin_unlock_irq(&pDevice->lock);
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Commit the settings\n");
+			pr_debug("Commit the settings\n");
 			spin_lock_irq(&pDevice->lock);
 			pDevice->bLinkPass = false;
 			memset(pMgmt->abyCurrBSSID, 0, 6);
@@ -3177,7 +3079,7 @@ viawget_suspend(struct pci_dev *pcid, pm_message_t state)
 {
 	int power_status;   // to silence the compiler
 
-	PSDevice pDevice = pci_get_drvdata(pcid);
+	struct vnt_private *pDevice = pci_get_drvdata(pcid);
 	PSMgmtObject  pMgmt = pDevice->pMgmt;
 
 	netif_stop_queue(pDevice->dev);
@@ -3203,7 +3105,7 @@ viawget_suspend(struct pci_dev *pcid, pm_message_t state)
 static int
 viawget_resume(struct pci_dev *pcid)
 {
-	PSDevice  pDevice = pci_get_drvdata(pcid);
+	struct vnt_private *pDevice = pci_get_drvdata(pcid);
 	PSMgmtObject  pMgmt = pDevice->pMgmt;
 	int power_status;   // to silence the compiler
 
diff --git a/drivers/staging/vt6655/dpc.c b/drivers/staging/vt6655/dpc.c
index 0bcf6c7472fe..8515b8c80801 100644
--- a/drivers/staging/vt6655/dpc.c
+++ b/drivers/staging/vt6655/dpc.c
@@ -61,8 +61,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
-
 static const unsigned char acbyRxRate[MAX_RATE] =
 {2, 4, 11, 22, 12, 18, 24, 36, 48, 72, 96, 108};
 
@@ -79,18 +77,18 @@ s_vGetDASA(unsigned char *pbyRxBufferAddr, unsigned int *pcbHeaderSize,
 	   PSEthernetHeader psEthHeader);
 
 static void
-s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
+s_vProcessRxMACHeader(struct vnt_private *pDevice, unsigned char *pbyRxBufferAddr,
 		      unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
 		      unsigned int *pcbHeadSize);
 
 static bool s_bAPModeRxCtl(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyFrame,
 	int      iSANodeIndex
 );
 
 static bool s_bAPModeRxData(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	struct sk_buff *skb,
 	unsigned int FrameSize,
 	unsigned int cbHeaderOffset,
@@ -99,7 +97,7 @@ static bool s_bAPModeRxData(
 );
 
 static bool s_bHandleRxEncryption(
-	PSDevice     pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyFrame,
 	unsigned int FrameSize,
 	unsigned char *pbyRsr,
@@ -112,7 +110,7 @@ static bool s_bHandleRxEncryption(
 
 static bool s_bHostWepRxEncryption(
 
-	PSDevice     pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyFrame,
 	unsigned int FrameSize,
 	unsigned char *pbyRsr,
@@ -145,7 +143,8 @@ static bool s_bHostWepRxEncryption(
  *
  -*/
 static void
-s_vProcessRxMACHeader(PSDevice pDevice, unsigned char *pbyRxBufferAddr,
+s_vProcessRxMACHeader(struct vnt_private *pDevice,
+		      unsigned char *pbyRxBufferAddr,
 		      unsigned int cbPacketSize, bool bIsWEP, bool bExtIV,
 		      unsigned int *pcbHeadSize)
 {
@@ -270,12 +269,12 @@ s_vGetDASA(unsigned char *pbyRxBufferAddr, unsigned int *pcbHeaderSize,
 
 bool
 device_receive_frame(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSRxDesc pCurrRD
 )
 {
 	PDEVICE_RD_INFO  pRDInfo = pCurrRD->pRDInfo;
-	struct net_device_stats *pStats = &pDevice->stats;
+	struct net_device_stats *pStats = &pDevice->dev->stats;
 	struct sk_buff *skb;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PSRxMgmtPacket  pRxPacket = &(pDevice->pMgmt->sRxPacket);
@@ -283,7 +282,7 @@ device_receive_frame(
 	unsigned char *pbyRsr;
 	unsigned char *pbyNewRsr;
 	unsigned char *pbyRSSI;
-	PQWORD          pqwTSFTime;
+	__le64 *pqwTSFTime;
 	unsigned short *pwFrameSize;
 	unsigned char *pbyFrame;
 	bool bDeFragRx = false;
@@ -313,10 +312,9 @@ device_receive_frame(
 
 	skb = pRDInfo->skb;
 
-//PLICE_DEBUG->
 	pci_unmap_single(pDevice->pcid, pRDInfo->skb_dma,
 			 pDevice->rx_buf_sz, PCI_DMA_FROMDEVICE);
-//PLICE_DEBUG<-
+
 	pwFrameSize = (unsigned short *)(skb->data + 2);
 	FrameSize = cpu_to_le16(pCurrRD->m_rd1RD1.wReqCount) - cpu_to_le16(pCurrRD->m_rd0RD0.wResCount);
 
@@ -324,7 +322,7 @@ device_receive_frame(
 	// Min (ACK): 10HD +4CRC + 2Padding + 4Len + 8TSF + 4RSR
 	if ((FrameSize > 2364) || (FrameSize <= 32)) {
 		// Frame Size error drop this packet.
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---------- WRONG Length 1\n");
+		pr_debug("---------- WRONG Length 1\n");
 		return false;
 	}
 
@@ -334,7 +332,7 @@ device_receive_frame(
 	pbyRSSI = (unsigned char *)(skb->data + FrameSize - 2);
 	pbyNewRsr = (unsigned char *)(skb->data + FrameSize - 3);
 	pbySQ = (unsigned char *)(skb->data + FrameSize - 4);
-	pqwTSFTime = (PQWORD)(skb->data + FrameSize - 12);
+	pqwTSFTime = (__le64 *)(skb->data + FrameSize - 12);
 	pbyFrame = (unsigned char *)(skb->data + 4);
 
 	// get packet size
@@ -342,10 +340,10 @@ device_receive_frame(
 
 	if ((FrameSize > 2346)|(FrameSize < 14)) { // Max: 2312Payload + 30HD +4CRC
 		// Min: 14 bytes ACK
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "---------- WRONG Length 2\n");
+		pr_debug("---------- WRONG Length 2\n");
 		return false;
 	}
-//PLICE_DEBUG->
+
 	// update receive statistic counter
 	STAvUpdateRDStatCounter(&pDevice->scStatistic,
 				*pbyRsr,
@@ -355,7 +353,7 @@ device_receive_frame(
 				FrameSize);
 
 	pMACHeader = (PS802_11Header)((unsigned char *)(skb->data) + 8);
-//PLICE_DEBUG<-
+
 	if (pDevice->bMeasureInProgress) {
 		if ((*pbyRsr & RSR_CRCOK) != 0)
 			pDevice->byBasicMap |= 0x01;
@@ -420,7 +418,7 @@ device_receive_frame(
 	if (IS_FC_WEP(pbyFrame)) {
 		bool bRxDecryOK = false;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx WEP pkt\n");
+		pr_debug("rx WEP pkt\n");
 		bIsWEP = true;
 		if ((pDevice->bEnableHostWEP) && (iSANodeIndex >= 0)) {
 			pKey = &STempKey;
@@ -458,7 +456,7 @@ device_receive_frame(
 
 		if (bRxDecryOK) {
 			if ((*pbyNewRsr & NEWRSR_DECRYPTOK) == 0) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV Fail\n");
+				pr_debug("ICV Fail\n");
 				if ((pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
 				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
 				    (pDevice->pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) ||
@@ -472,7 +470,7 @@ device_receive_frame(
 				return false;
 			}
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WEP Func Fail\n");
+			pr_debug("WEP Func Fail\n");
 			return false;
 		}
 		if ((pKey != NULL) && (pKey->byCipherSuite == KEY_CTL_CCMP))
@@ -515,8 +513,7 @@ device_receive_frame(
 			pRxPacket->cbMPDULen = FrameSize;
 			pRxPacket->uRSSI = *pbyRSSI;
 			pRxPacket->bySQ = *pbySQ;
-			HIDWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(HIDWORD(*pqwTSFTime));
-			LODWORD(pRxPacket->qwLocalTSF) = cpu_to_le32(LODWORD(*pqwTSFTime));
+			pRxPacket->qwLocalTSF = le64_to_cpu(*pqwTSFTime);
 			if (bIsWEP) {
 				// strip IV
 				pbyData1 = WLAN_HDR_A3_DATA_PTR(skb->data+4);
@@ -554,8 +551,8 @@ device_receive_frame(
 			if (!(*pbyRsr & RSR_BSSIDOK)) {
 				if (bDeFragRx) {
 					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-						DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
-							pDevice->dev->name);
+						pr_err("%s: can not alloc more frag bufs\n",
+						       pDevice->dev->name);
 					}
 				}
 				return false;
@@ -565,8 +562,8 @@ device_receive_frame(
 			if (!pDevice->bLinkPass || !(*pbyRsr & RSR_BSSIDOK)) {
 				if (bDeFragRx) {
 					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-						DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
-							pDevice->dev->name);
+						pr_err("%s: can not alloc more frag bufs\n",
+						       pDevice->dev->name);
 					}
 				}
 				return false;
@@ -606,7 +603,7 @@ device_receive_frame(
 
 	// Now it only supports 802.11g Infrastructure Mode, and support rate must up to 54 Mbps
 	if (pDevice->bDiversityEnable && (FrameSize > 50) &&
-	    (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
+	    (pDevice->op_mode == NL80211_IFTYPE_STATION) &&
 	    pDevice->bLinkPass) {
 		BBvAntennaDiversity(pDevice, s_byGetRateIdx(*pbyRxRate), 0);
 	}
@@ -642,7 +639,7 @@ device_receive_frame(
 		wEtherType = (skb->data[cbIVOffset + 4 + 24 + 6] << 8) |
 			skb->data[cbIVOffset + 4 + 24 + 6 + 1];
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wEtherType = %04x\n", wEtherType);
+		pr_debug("wEtherType = %04x\n", wEtherType);
 		if (wEtherType == ETH_P_PAE) {
 			skb->dev = pDevice->apdev;
 
@@ -717,13 +714,13 @@ device_receive_frame(
 			if ((le32_to_cpu(*pdwMIC_L) != dwLocalMIC_L) ||
 			    (le32_to_cpu(*pdwMIC_R) != dwLocalMIC_R) ||
 			    pDevice->bRxMICFail) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC comparison is fail!\n");
+				pr_debug("MIC comparison is fail!\n");
 				pDevice->bRxMICFail = false;
 				pDevice->s802_11Counter.TKIPLocalMICFailures++;
 				if (bDeFragRx) {
 					if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-						DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
-							pDevice->dev->name);
+						pr_err("%s: can not alloc more frag bufs\n",
+						       pDevice->dev->name);
 					}
 				}
 				//2008-0409-07, <Add> by Einsn Liu
@@ -796,7 +793,7 @@ device_receive_frame(
 			RSC = dwRxTSC47_16;
 			RSC <<= 16;
 			RSC += wRxTSC15_0;
-			memcpy(&(pKey->KeyRSC), &RSC,  sizeof(QWORD));
+			pKey->KeyRSC = RSC;
 
 			if ((pDevice->sMgmtObj.eCurrMode == WMAC_MODE_ESS_STA) &&
 			    (pDevice->sMgmtObj.eCurrState == WMAC_STATE_ASSOC)) {
@@ -804,7 +801,7 @@ device_receive_frame(
 				if ((wRxTSC15_0 < wLocalTSC15_0) &&
 				    (dwRxTSC47_16 <= dwLocalTSC47_16) &&
 				    !((dwRxTSC47_16 == 0) && (dwLocalTSC47_16 == 0xFFFFFFFF))) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC is illegal~~!\n ");
+					pr_debug("TSC is illegal~~!\n ");
 					if (pKey->byCipherSuite == KEY_CTL_TKIP)
 						pDevice->s802_11Counter.TKIPReplays++;
 					else
@@ -812,8 +809,8 @@ device_receive_frame(
 
 					if (bDeFragRx) {
 						if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-							DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
-								pDevice->dev->name);
+							pr_err("%s: can not alloc more frag bufs\n",
+							       pDevice->dev->name);
 						}
 					}
 					return false;
@@ -840,8 +837,8 @@ device_receive_frame(
 )) {
 			if (bDeFragRx) {
 				if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-					DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
-						pDevice->dev->name);
+					pr_err("%s: can not alloc more frag bufs\n",
+					       pDevice->dev->name);
 				}
 			}
 			return false;
@@ -862,8 +859,8 @@ device_receive_frame(
 
 	if (bDeFragRx) {
 		if (!device_alloc_frag_buf(pDevice, &pDevice->sRxDFCB[pDevice->uCurrentDFCBIdx])) {
-			DBG_PRT(MSG_LEVEL_ERR, KERN_ERR "%s: can not alloc more frag bufs\n",
-				pDevice->dev->name);
+			pr_err("%s: can not alloc more frag bufs\n",
+			       pDevice->dev->name);
 		}
 		return false;
 	}
@@ -872,7 +869,7 @@ device_receive_frame(
 }
 
 static bool s_bAPModeRxCtl(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyFrame,
 	int      iSANodeIndex
 )
@@ -897,7 +894,7 @@ static bool s_bAPModeRxCtl(
 							     (WLAN_MGMT_REASON_CLASS2_NONAUTH),
 							     &Status
 );
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 1\n");
+					pr_debug("dpc: send vMgrDeAuthenBeginSta 1\n");
 					return true;
 				}
 				if (pMgmt->sNodeDBTable[iSANodeIndex].eNodeState < NODE_ASSOC) {
@@ -909,7 +906,7 @@ static bool s_bAPModeRxCtl(
 							     (WLAN_MGMT_REASON_CLASS3_NONASSOC),
 							     &Status
 );
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDisassocBeginSta 2\n");
+					pr_debug("dpc: send vMgrDisassocBeginSta 2\n");
 					return true;
 				}
 
@@ -918,7 +915,7 @@ static bool s_bAPModeRxCtl(
 					if (IS_CTL_PSPOLL(pbyFrame)) {
 						pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
 						bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 1\n");
+						pr_debug("dpc: WLAN_CMD_RX_PSPOLL 1\n");
 					} else {
 						// check Data PS state
 						// if PW bit off, send out all PS bufferring packets.
@@ -926,7 +923,7 @@ static bool s_bAPModeRxCtl(
 							pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
 							pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
 							bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 2\n");
+							pr_debug("dpc: WLAN_CMD_RX_PSPOLL 2\n");
 						}
 					}
 				} else {
@@ -940,7 +937,7 @@ static bool s_bAPModeRxCtl(
 							pMgmt->sNodeDBTable[iSANodeIndex].bPSEnable = false;
 							pMgmt->sNodeDBTable[iSANodeIndex].bRxPSPoll = true;
 							bScheduleCommand((void *)pDevice, WLAN_CMD_RX_PSPOLL, NULL);
-							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: WLAN_CMD_RX_PSPOLL 3\n");
+							pr_debug("dpc: WLAN_CMD_RX_PSPOLL 3\n");
 
 						}
 					}
@@ -952,16 +949,18 @@ static bool s_bAPModeRxCtl(
 						     (WLAN_MGMT_REASON_CLASS2_NONAUTH),
 						     &Status
 );
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: send vMgrDeAuthenBeginSta 3\n");
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BSSID:%pM\n",
-					p802_11Header->abyAddr3);
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR2:%pM\n",
-					p802_11Header->abyAddr2);
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ADDR1:%pM\n",
-					p802_11Header->abyAddr1);
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc: wFrameCtl= %x\n", p802_11Header->wFrameCtl);
+				pr_debug("dpc: send vMgrDeAuthenBeginSta 3\n");
+				pr_debug("BSSID:%pM\n",
+					 p802_11Header->abyAddr3);
+				pr_debug("ADDR2:%pM\n",
+					 p802_11Header->abyAddr2);
+				pr_debug("ADDR1:%pM\n",
+					 p802_11Header->abyAddr1);
+				pr_debug("dpc: wFrameCtl= %x\n",
+					 p802_11Header->wFrameCtl);
 				VNSvInPortB(pDevice->PortOffset + MAC_REG_RCR, &(pDevice->byRxMode));
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dpc:pDevice->byRxMode = %x\n", pDevice->byRxMode);
+				pr_debug("dpc:pDevice->byRxMode = %x\n",
+					 pDevice->byRxMode);
 				return true;
 			}
 		}
@@ -970,7 +969,7 @@ static bool s_bAPModeRxCtl(
 }
 
 static bool s_bHandleRxEncryption(
-	PSDevice     pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyFrame,
 	unsigned int FrameSize,
 	unsigned char *pbyRsr,
@@ -999,7 +998,7 @@ static bool s_bHandleRxEncryption(
 	}
 	byKeyIdx = (*(pbyIV+3) & 0xc0);
 	byKeyIdx >>= 6;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\nKeyIdx: %d\n", byKeyIdx);
+	pr_debug("\nKeyIdx: %d\n", byKeyIdx);
 
 	if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) ||
 	    (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK) ||
@@ -1009,14 +1008,14 @@ static bool s_bHandleRxEncryption(
 		if (((*pbyRsr & (RSR_ADDRBROAD | RSR_ADDRMULTI)) == 0) &&
 		    (pDevice->pMgmt->byCSSPK != KEY_CTL_NONE)) {
 			// unicast pkt use pairwise key
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "unicast pkt\n");
+			pr_debug("unicast pkt\n");
 			if (KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, 0xFFFFFFFF, &pKey) == true) {
 				if (pDevice->pMgmt->byCSSPK == KEY_CTL_TKIP)
 					byDecMode = KEY_CTL_TKIP;
 				else if (pDevice->pMgmt->byCSSPK == KEY_CTL_CCMP)
 					byDecMode = KEY_CTL_CCMP;
 			}
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "unicast pkt: %d, %p\n", byDecMode, pKey);
+			pr_debug("unicast pkt: %d, %p\n", byDecMode, pKey);
 		} else {
 			// use group key
 			KeybGetKey(&(pDevice->sKey), pDevice->abyBSSID, byKeyIdx, &pKey);
@@ -1024,7 +1023,8 @@ static bool s_bHandleRxEncryption(
 				byDecMode = KEY_CTL_TKIP;
 			else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
 				byDecMode = KEY_CTL_CCMP;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "group pkt: %d, %d, %p\n", byKeyIdx, byDecMode, pKey);
+			pr_debug("group pkt: %d, %d, %p\n",
+				 byKeyIdx, byDecMode, pKey);
 		}
 	}
 	// our WEP only support Default Key
@@ -1038,10 +1038,11 @@ static bool s_bHandleRxEncryption(
 	}
 	*pKeyOut = pKey;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
+	pr_debug("AES:%d %d %d\n",
+		 pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
 
 	if (pKey == NULL) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey == NULL\n");
+		pr_debug("pKey == NULL\n");
 
 		return false;
 	}
@@ -1074,13 +1075,13 @@ static bool s_bHandleRxEncryption(
 
 		PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
 		*pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ExtIV: %lx\n", *pdwRxTSC47_16);
+		pr_debug("ExtIV: %lx\n", *pdwRxTSC47_16);
 		if (byDecMode == KEY_CTL_TKIP)
 			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV + 2), *pbyIV));
 		else
 			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC0_15: %x\n", *pwRxTSC15_0);
+		pr_debug("TSC0_15: %x\n", *pwRxTSC15_0);
 
 		if ((byDecMode == KEY_CTL_TKIP) &&
 		    (pDevice->byLocalID <= REV_ID_VT3253_A1)) {
@@ -1093,10 +1094,10 @@ static bool s_bHandleRxEncryption(
 			rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
 			if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
 				*pbyNewRsr |= NEWRSR_DECRYPTOK;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV OK!\n");
+				pr_debug("ICV OK!\n");
 			} else {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV FAIL!!!\n");
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PayloadLen = %d\n", PayloadLen);
+				pr_debug("ICV FAIL!!!\n");
+				pr_debug("PayloadLen = %d\n", PayloadLen);
 			}
 		}
 	}// end of TKIP/AES
@@ -1107,7 +1108,7 @@ static bool s_bHandleRxEncryption(
 }
 
 static bool s_bHostWepRxEncryption(
-	PSDevice     pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyFrame,
 	unsigned int FrameSize,
 	unsigned char *pbyRsr,
@@ -1136,21 +1137,22 @@ static bool s_bHostWepRxEncryption(
 	}
 	byKeyIdx = (*(pbyIV+3) & 0xc0);
 	byKeyIdx >>= 6;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\nKeyIdx: %d\n", byKeyIdx);
+	pr_debug("\nKeyIdx: %d\n", byKeyIdx);
 
 	if (pDevice->pMgmt->byCSSGK == KEY_CTL_TKIP)
 		byDecMode = KEY_CTL_TKIP;
 	else if (pDevice->pMgmt->byCSSGK == KEY_CTL_CCMP)
 		byDecMode = KEY_CTL_CCMP;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AES:%d %d %d\n", pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
+	pr_debug("AES:%d %d %d\n",
+		 pDevice->pMgmt->byCSSPK, pDevice->pMgmt->byCSSGK, byDecMode);
 
 	if (byDecMode != pKey->byCipherSuite)
 		return false;
 
 	if (byDecMode == KEY_CTL_WEP) {
 		// handle WEP
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "byDecMode == KEY_CTL_WEP\n");
+		pr_debug("byDecMode == KEY_CTL_WEP\n");
 
 		if ((pDevice->byLocalID <= REV_ID_VT3253_A1) ||
 		    (((PSKeyTable)(pKey->pvKeyTable))->bSoftWEP == true) ||
@@ -1176,31 +1178,32 @@ static bool s_bHostWepRxEncryption(
 
 		PayloadLen -= (WLAN_HDR_ADDR3_LEN + 8 + 4); // 24 is 802.11 header, 8 is IV&ExtIV, 4 is crc
 		*pdwRxTSC47_16 = cpu_to_le32(*(unsigned long *)(pbyIV + 4));
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ExtIV: %lx\n", *pdwRxTSC47_16);
+		pr_debug("ExtIV: %lx\n", *pdwRxTSC47_16);
 
 		if (byDecMode == KEY_CTL_TKIP)
 			*pwRxTSC15_0 = cpu_to_le16(MAKEWORD(*(pbyIV+2), *pbyIV));
 		else
 			*pwRxTSC15_0 = cpu_to_le16(*(unsigned short *)pbyIV);
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "TSC0_15: %x\n", *pwRxTSC15_0);
+		pr_debug("TSC0_15: %x\n", *pwRxTSC15_0);
 
 		if (byDecMode == KEY_CTL_TKIP) {
 			if ((pDevice->byLocalID <= REV_ID_VT3253_A1) || !bOnFly) {
 				// Software TKIP
 				// 1. 3253 A
 				// 2. NotOnFly
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "soft KEY_CTL_TKIP\n");
+				pr_debug("soft KEY_CTL_TKIP\n");
 				pMACHeader = (PS802_11Header)(pbyFrame);
 				TKIPvMixKey(pKey->abyKey, pMACHeader->abyAddr2, *pwRxTSC15_0, *pdwRxTSC47_16, pDevice->abyPRNG);
 				rc4_init(&pDevice->SBox, pDevice->abyPRNG, TKIP_KEY_LEN);
 				rc4_encrypt(&pDevice->SBox, pbyIV+8, pbyIV+8, PayloadLen);
 				if (ETHbIsBufferCrc32Ok(pbyIV+8, PayloadLen)) {
 					*pbyNewRsr |= NEWRSR_DECRYPTOK;
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV OK!\n");
+					pr_debug("ICV OK!\n");
 				} else {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ICV FAIL!!!\n");
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PayloadLen = %d\n", PayloadLen);
+					pr_debug("ICV FAIL!!!\n");
+					pr_debug("PayloadLen = %d\n",
+						 PayloadLen);
 				}
 			}
 		}
@@ -1209,12 +1212,12 @@ static bool s_bHostWepRxEncryption(
 			if (!bOnFly) {
 				// Software CCMP
 				// NotOnFly
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "soft KEY_CTL_CCMP\n");
+				pr_debug("soft KEY_CTL_CCMP\n");
 				if (AESbGenCCMP(pKey->abyKey, pbyFrame, FrameSize)) {
 					*pbyNewRsr |= NEWRSR_DECRYPTOK;
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CCMP MIC compare OK!\n");
+					pr_debug("CCMP MIC compare OK!\n");
 				} else {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "CCMP MIC fail!\n");
+					pr_debug("CCMP MIC fail!\n");
 				}
 			}
 		}
@@ -1227,7 +1230,7 @@ static bool s_bHostWepRxEncryption(
 }
 
 static bool s_bAPModeRxData(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	struct sk_buff *skb,
 	unsigned int FrameSize,
 	unsigned int cbHeaderOffset,
@@ -1252,7 +1255,7 @@ static bool s_bAPModeRxData(
 
 			// if any node in PS mode, buffer packet until DTIM.
 			if (skbcpy == NULL) {
-				DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "relay multicast no skb available\n");
+				pr_info("relay multicast no skb available\n");
 			} else {
 				skbcpy->dev = pDevice->dev;
 				skbcpy->len = FrameSize;
@@ -1280,8 +1283,9 @@ static bool s_bAPModeRxData(
 					pMgmt->sNodeDBTable[iDANodeIndex].wEnQueueCnt++;
 					wAID = pMgmt->sNodeDBTable[iDANodeIndex].wAID;
 					pMgmt->abyPSTxMap[wAID >> 3] |=  byMask[wAID & 7];
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "relay: index= %d, pMgmt->abyPSTxMap[%d]= %d\n",
-						iDANodeIndex, (wAID >> 3), pMgmt->abyPSTxMap[wAID >> 3]);
+					pr_debug("relay: index= %d, pMgmt->abyPSTxMap[%d]= %d\n",
+						 iDANodeIndex, (wAID >> 3),
+						 pMgmt->abyPSTxMap[wAID >> 3]);
 					return true;
 				} else {
 					bRelayOnly = true;
diff --git a/drivers/staging/vt6655/dpc.h b/drivers/staging/vt6655/dpc.h
index 4914890115e4..a068b846b1be 100644
--- a/drivers/staging/vt6655/dpc.h
+++ b/drivers/staging/vt6655/dpc.h
@@ -35,7 +35,7 @@
 
 bool
 device_receive_frame(
-	PSDevice pDevice,
+	struct vnt_private *,
 	PSRxDesc pCurrRD
 );
 
diff --git a/drivers/staging/vt6655/hostap.c b/drivers/staging/vt6655/hostap.c
index f105c2ac091b..ae0dade229d8 100644
--- a/drivers/staging/vt6655/hostap.c
+++ b/drivers/staging/vt6655/hostap.c
@@ -47,9 +47,6 @@
 
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
-
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -68,16 +65,16 @@ static int msglevel = MSG_LEVEL_INFO;
  *
  */
 
-static int hostap_enable_hostapd(PSDevice pDevice, int rtnl_locked)
+static int hostap_enable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
 {
-	PSDevice apdev_priv;
+	struct vnt_private *apdev_priv;
 	struct net_device *dev = pDevice->dev;
 	int ret;
 	const struct net_device_ops apdev_netdev_ops = {
 		.ndo_start_xmit         = pDevice->tx_80211,
 	};
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Enabling hostapd mode\n", dev->name);
+	pr_debug("%s: Enabling hostapd mode\n", dev->name);
 
 	pDevice->apdev = alloc_etherdev(sizeof(*apdev_priv));
 	if (pDevice->apdev == NULL)
@@ -101,15 +98,15 @@ static int hostap_enable_hostapd(PSDevice pDevice, int rtnl_locked)
 	else
 		ret = register_netdev(pDevice->apdev);
 	if (ret) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: register_netdevice(AP) failed!\n",
-			dev->name);
+		pr_debug("%s: register_netdevice(AP) failed!\n",
+			 dev->name);
 		free_netdev(pDevice->apdev);
 		pDevice->apdev = NULL;
 		return -1;
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdevice %s for AP management\n",
-		dev->name, pDevice->apdev->name);
+	pr_debug("%s: Registered netdevice %s for AP management\n",
+		 dev->name, pDevice->apdev->name);
 
 	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
 
@@ -130,17 +127,17 @@ static int hostap_enable_hostapd(PSDevice pDevice, int rtnl_locked)
  *
  */
 
-static int hostap_disable_hostapd(PSDevice pDevice, int rtnl_locked)
+static int hostap_disable_hostapd(struct vnt_private *pDevice, int rtnl_locked)
 {
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: disabling hostapd mode\n", pDevice->dev->name);
+	pr_debug("%s: disabling hostapd mode\n", pDevice->dev->name);
 
 	if (pDevice->apdev && pDevice->apdev->name && pDevice->apdev->name[0]) {
 		if (rtnl_locked)
 			unregister_netdevice(pDevice->apdev);
 		else
 			unregister_netdev(pDevice->apdev);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Netdevice %s unregistered\n",
-			pDevice->dev->name, pDevice->apdev->name);
+		pr_debug("%s: Netdevice %s unregistered\n",
+			 pDevice->dev->name, pDevice->apdev->name);
 	}
 	if (pDevice->apdev)
 		free_netdev(pDevice->apdev);
@@ -149,8 +146,8 @@ static int hostap_disable_hostapd(PSDevice pDevice, int rtnl_locked)
 	pDevice->bEnableHostWEP = false;
 	pDevice->bEncryptionEnable = false;
 
-//4.2007-0118-03,<Add> by EinsnLiu
-//execute some clear work
+/* 4.2007-0118-03,<Add> by EinsnLiu */
+/* execute some clear work */
 	pDevice->pMgmt->byCSSPK = KEY_CTL_NONE;
 	pDevice->pMgmt->byCSSGK = KEY_CTL_NONE;
 	KeyvInitTable(&pDevice->sKey, pDevice->PortOffset);
@@ -172,7 +169,8 @@ static int hostap_disable_hostapd(PSDevice pDevice, int rtnl_locked)
  *
  */
 
-int vt6655_hostap_set_hostapd(PSDevice pDevice, int val, int rtnl_locked)
+int vt6655_hostap_set_hostapd(struct vnt_private *pDevice,
+			      int val, int rtnl_locked)
 {
 	if (val < 0 || val > 1)
 		return -EINVAL;
@@ -201,7 +199,7 @@ int vt6655_hostap_set_hostapd(PSDevice pDevice, int val, int rtnl_locked)
  * Return Value:
  *
  */
-static int hostap_remove_sta(PSDevice pDevice,
+static int hostap_remove_sta(struct vnt_private *pDevice,
 			     struct viawget_hostapd_param *param)
 {
 	unsigned int uNodeIndex;
@@ -227,28 +225,28 @@ static int hostap_remove_sta(PSDevice pDevice,
  * Return Value:
  *
  */
-static int hostap_add_sta(PSDevice pDevice,
+static int hostap_add_sta(struct vnt_private *pDevice,
 			  struct viawget_hostapd_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uNodeIndex;
 
 	if (!BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex))
-		BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
+		BSSvCreateOneNode(pDevice, &uNodeIndex);
 
 	memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, param->sta_addr, WLAN_ADDR_LEN);
 	pMgmt->sNodeDBTable[uNodeIndex].eNodeState = NODE_ASSOC;
 	pMgmt->sNodeDBTable[uNodeIndex].wCapInfo = param->u.add_sta.capability;
-// TODO listenInterval
+/* TODO listenInterval */
 	pMgmt->sNodeDBTable[uNodeIndex].bPSEnable = false;
 	pMgmt->sNodeDBTable[uNodeIndex].bySuppRate = param->u.add_sta.tx_supp_rates;
 
-	// set max tx rate
+	/* set max tx rate */
 	pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
 		pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-	// set max basic rate
+	/* set max basic rate */
 	pMgmt->sNodeDBTable[uNodeIndex].wMaxBasicRate = RATE_2M;
-	// Todo: check sta preamble, if ap can't support, set status code
+	/* Todo: check sta preamble, if ap can't support, set status code */
 	pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
 		WLAN_GET_CAP_INFO_SHORTPREAMBLE(pMgmt->sNodeDBTable[uNodeIndex].wCapInfo);
 
@@ -256,17 +254,10 @@ static int hostap_add_sta(PSDevice pDevice,
 
 	pMgmt->sNodeDBTable[uNodeIndex].ulLastRxJiffer = jiffies;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Add STA AID= %d\n", pMgmt->sNodeDBTable[uNodeIndex].wAID);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
-		param->sta_addr[0],
-		param->sta_addr[1],
-		param->sta_addr[2],
-		param->sta_addr[3],
-		param->sta_addr[4],
-		param->sta_addr[5]
-		);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Max Support rate = %d\n",
-		pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
+	pr_debug("Add STA AID= %d\n", pMgmt->sNodeDBTable[uNodeIndex].wAID);
+	pr_debug("MAC=%pM\n", param->sta_addr);
+	pr_debug("Max Support rate = %d\n",
+		 pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
 
 	return 0;
 }
@@ -285,7 +276,7 @@ static int hostap_add_sta(PSDevice pDevice,
  *
  */
 
-static int hostap_get_info_sta(PSDevice pDevice,
+static int hostap_get_info_sta(struct vnt_private *pDevice,
 			       struct viawget_hostapd_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
@@ -314,7 +305,7 @@ static int hostap_get_info_sta(PSDevice pDevice,
  * Return Value:
  *
  */
-static int hostap_set_flags_sta(PSDevice pDevice,
+static int hostap_set_flags_sta(struct vnt_private *pDevice,
 				struct viawget_hostapd_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
@@ -323,8 +314,8 @@ static int hostap_set_flags_sta(PSDevice pDevice,
 	if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &uNodeIndex)) {
 		pMgmt->sNodeDBTable[uNodeIndex].dwFlags |= param->u.set_flags_sta.flags_or;
 		pMgmt->sNodeDBTable[uNodeIndex].dwFlags &= param->u.set_flags_sta.flags_and;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " dwFlags = %x\n",
-			(unsigned int)pMgmt->sNodeDBTable[uNodeIndex].dwFlags);
+		pr_debug(" dwFlags = %x\n",
+			 (unsigned int)pMgmt->sNodeDBTable[uNodeIndex].dwFlags);
 	} else {
 		return -ENOENT;
 	}
@@ -345,11 +336,14 @@ static int hostap_set_flags_sta(PSDevice pDevice,
  * Return Value:
  *
  */
-static int hostap_set_generic_element(PSDevice pDevice,
+static int hostap_set_generic_element(struct vnt_private *pDevice,
 				      struct viawget_hostapd_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 
+	if (param->u.generic_elem.len > sizeof(pMgmt->abyWPAIE))
+		return -EINVAL;
+
 	memcpy(pMgmt->abyWPAIE,
 	       param->u.generic_elem.data,
 	       param->u.generic_elem.len
@@ -357,18 +351,18 @@ static int hostap_set_generic_element(PSDevice pDevice,
 
 	pMgmt->wWPAIELen = param->u.generic_elem.len;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pMgmt->wWPAIELen = %d\n", pMgmt->wWPAIELen);
+	pr_debug("pMgmt->wWPAIELen = %d\n", pMgmt->wWPAIELen);
 
-	// disable wpa
+	/* disable wpa */
 	if (pMgmt->wWPAIELen == 0) {
 		pMgmt->eAuthenMode = WMAC_AUTH_OPEN;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " No WPAIE, Disable WPA\n");
+		pr_debug(" No WPAIE, Disable WPA\n");
 	} else  {
-		// enable wpa
+		/* enable wpa */
 		if ((pMgmt->abyWPAIE[0] == WLAN_EID_RSN_WPA) ||
 		    (pMgmt->abyWPAIE[0] == WLAN_EID_RSN)) {
 			pMgmt->eAuthenMode = WMAC_AUTH_WPANONE;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set WPAIE enable WPA\n");
+			pr_debug("Set WPAIE enable WPA\n");
 		} else
 			return -EINVAL;
 	}
@@ -389,9 +383,9 @@ static int hostap_set_generic_element(PSDevice pDevice,
  *
  */
 
-static void hostap_flush_sta(PSDevice pDevice)
+static void hostap_flush_sta(struct vnt_private *pDevice)
 {
-	// reserved node index =0 for multicast node.
+	/* reserved node index =0 for multicast node. */
 	BSSvClearNodeDBTable(pDevice, 1);
 	pDevice->uAssocCount = 0;
 }
@@ -409,7 +403,7 @@ static void hostap_flush_sta(PSDevice pDevice)
  * Return Value:
  *
  */
-static int hostap_set_encryption(PSDevice pDevice,
+static int hostap_set_encryption(struct vnt_private *pDevice,
 				 struct viawget_hostapd_param *param,
 				 int param_len)
 {
@@ -417,7 +411,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 	unsigned long dwKeyIndex = 0;
 	unsigned char abyKey[MAX_KEY_LEN];
 	unsigned char abySeq[MAX_KEY_LEN];
-	unsigned long long KeyRSC;
+	u64 KeyRSC;
 	unsigned char byKeyDecMode = KEY_CTL_WEP;
 	int     iNodeIndex = -1;
 	int     ii;
@@ -431,7 +425,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 
 	if ((param->u.crypt.idx > 3) || (param->u.crypt.key_len > MAX_KEY_LEN)) {
 		param->u.crypt.err = HOSTAP_CRYPT_ERR_KEY_SET_FAILED;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " HOSTAP_CRYPT_ERR_KEY_SET_FAILED\n");
+		pr_debug(" HOSTAP_CRYPT_ERR_KEY_SET_FAILED\n");
 		return -EINVAL;
 	}
 
@@ -443,12 +437,12 @@ static int hostap_set_encryption(PSDevice pDevice,
 	} else {
 		if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
 			param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
+			pr_debug(" HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
 			return -EINVAL;
 		}
 	}
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " hostap_set_encryption: sta_index %d\n", iNodeIndex);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " hostap_set_encryption: alg %d\n", param->u.crypt.alg);
+	pr_debug(" hostap_set_encryption: sta_index %d\n", iNodeIndex);
+	pr_debug(" hostap_set_encryption: alg %d\n", param->u.crypt.alg);
 
 	if (param->u.crypt.alg == WPA_ALG_NONE) {
 		if (pMgmt->sNodeDBTable[iNodeIndex].bOnFly) {
@@ -456,7 +450,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 					  param->sta_addr,
 					  pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex,
 					  pDevice->PortOffset)) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybRemoveKey fail\n");
+				pr_debug("KeybRemoveKey fail\n");
 			}
 			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
 		}
@@ -476,7 +470,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 	}
 
 	memcpy(abyKey, param->u.crypt.key, param->u.crypt.key_len);
-	// copy to node key tbl
+	/* copy to node key tbl */
 	pMgmt->sNodeDBTable[iNodeIndex].byKeyIndex = param->u.crypt.idx;
 	pMgmt->sNodeDBTable[iNodeIndex].uWepKeyLength = param->u.crypt.key_len;
 	memcpy(&pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
@@ -503,13 +497,13 @@ static int hostap_set_encryption(PSDevice pDevice,
 					  pDevice->byLocalID);
 
 		} else {
-			// 8021x enable, individual key
-			dwKeyIndex |= (1 << 30); // set pairwise key
+			/* 8021x enable, individual key */
+			dwKeyIndex |= (1 << 30); /* set pairwise key */
 			if (KeybSetKey(&(pDevice->sKey),
 				       &param->sta_addr[0],
 				       dwKeyIndex & ~(USE_KEYRSC),
 				       param->u.crypt.key_len,
-				       (PQWORD) &(KeyRSC),
+				       (u64 *) &KeyRSC,
 				       (unsigned char *)abyKey,
 				       KEY_CTL_WEP,
 				       pDevice->PortOffset,
@@ -517,7 +511,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 				pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
 
 			} else {
-				// Key Table Full
+				/* Key Table Full */
 				pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
 				bKeyTableFull = true;
 			}
@@ -534,7 +528,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 	if (param->u.crypt.seq) {
 		memcpy(&abySeq, param->u.crypt.seq, 8);
 		for (ii = 0; ii < 8; ii++)
-			KeyRSC |= (unsigned long)abySeq[ii] << (ii * 8);
+			KeyRSC |= (u64)abySeq[ii] << (ii * 8);
 
 		dwKeyIndex |= 1 << 29;
 		pMgmt->sNodeDBTable[iNodeIndex].KeyRSC = KeyRSC;
@@ -563,7 +557,7 @@ static int hostap_set_encryption(PSDevice pDevice,
 		KeybSetDefaultKey(&(pDevice->sKey),
 				  dwKeyIndex,
 				  param->u.crypt.key_len,
-				  (PQWORD) &(KeyRSC),
+				  (u64 *) &KeyRSC,
 				  abyKey,
 				  byKeyDecMode,
 				  pDevice->PortOffset,
@@ -571,12 +565,12 @@ static int hostap_set_encryption(PSDevice pDevice,
 		pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
 
 	} else {
-		dwKeyIndex |= (1 << 30); // set pairwise key
+		dwKeyIndex |= (1 << 30); /* set pairwise key */
 		if (KeybSetKey(&(pDevice->sKey),
 			       &param->sta_addr[0],
 			       dwKeyIndex,
 			       param->u.crypt.key_len,
-			       (PQWORD) &(KeyRSC),
+			       (u64 *) &KeyRSC,
 			       (unsigned char *)abyKey,
 			       byKeyDecMode,
 			       pDevice->PortOffset,
@@ -584,35 +578,34 @@ static int hostap_set_encryption(PSDevice pDevice,
 			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = true;
 
 		} else {
-			// Key Table Full
+			/* Key Table Full */
 			pMgmt->sNodeDBTable[iNodeIndex].bOnFly = false;
 			bKeyTableFull = true;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Key Table Full\n");
+			pr_debug(" Key Table Full\n");
 		}
 
 	}
 
 	if (bKeyTableFull) {
-		wKeyCtl &= 0x7F00;              // clear all key control filed
+		wKeyCtl &= 0x7F00;              /* clear all key control filed */
 		wKeyCtl |= (byKeyDecMode << 4);
 		wKeyCtl |= (byKeyDecMode);
-		wKeyCtl |= 0x0044;              // use group key for all address
-		wKeyCtl |= 0x4000;              // disable KeyTable[MAX_KEY_TABLE-1] on-fly to genernate rx int
+		wKeyCtl |= 0x0044;              /* use group key for all address */
+		wKeyCtl |= 0x4000;              /* disable KeyTable[MAX_KEY_TABLE-1] on-fly to genernate rx int */
 		MACvSetDefaultKeyCtl(pDevice->PortOffset, wKeyCtl, MAX_KEY_TABLE-1, pDevice->byLocalID);
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set key sta_index= %d\n", iNodeIndex);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " tx_index=%d len=%d\n", param->u.crypt.idx,
-		param->u.crypt.key_len);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " key=%x-%x-%x-%x-%x-xxxxx\n",
-		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
-		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[1],
-		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[2],
-		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[3],
-		pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[4]
-);
-
-	// set wep key
+	pr_debug(" Set key sta_index= %d\n", iNodeIndex);
+	pr_debug(" tx_index=%d len=%d\n",
+		 param->u.crypt.idx, param->u.crypt.key_len);
+	pr_debug(" key=%x-%x-%x-%x-%x-xxxxx\n",
+		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[0],
+		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[1],
+		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[2],
+		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[3],
+		 pMgmt->sNodeDBTable[iNodeIndex].abyWepKey[4]);
+
+	/* set wep key */
 	pDevice->bEncryptionEnable = true;
 	pMgmt->sNodeDBTable[iNodeIndex].byCipherSuite = byKeyDecMode;
 	pMgmt->sNodeDBTable[iNodeIndex].dwKeyIndex = dwKeyIndex;
@@ -635,7 +628,7 @@ static int hostap_set_encryption(PSDevice pDevice,
  * Return Value:
  *
  */
-static int hostap_get_encryption(PSDevice pDevice,
+static int hostap_get_encryption(struct vnt_private *pDevice,
 				 struct viawget_hostapd_param *param,
 				 int param_len)
 {
@@ -650,11 +643,11 @@ static int hostap_get_encryption(PSDevice pDevice,
 	} else {
 		if (BSSDBbIsSTAInNodeDB(pMgmt, param->sta_addr, &iNodeIndex) == false) {
 			param->u.crypt.err = HOSTAP_CRYPT_ERR_UNKNOWN_ADDR;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
+			pr_debug("hostap_get_encryption: HOSTAP_CRYPT_ERR_UNKNOWN_ADDR\n");
 			return -EINVAL;
 		}
 	}
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "hostap_get_encryption: %d\n", iNodeIndex);
+	pr_debug("hostap_get_encryption: %d\n", iNodeIndex);
 	memset(param->u.crypt.seq, 0, 8);
 	for (ii = 0; ii < 8; ii++)
 		param->u.crypt.seq[ii] = (unsigned char)pMgmt->sNodeDBTable[iNodeIndex].KeyRSC >> (ii * 8);
@@ -675,7 +668,7 @@ static int hostap_get_encryption(PSDevice pDevice,
  * Return Value:
  *
  */
-int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
+int vt6655_hostap_ioctl(struct vnt_private *pDevice, struct iw_point *p)
 {
 	struct viawget_hostapd_param *param;
 	int ret = 0;
@@ -696,67 +689,67 @@ int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p)
 
 	switch (param->cmd) {
 	case VIAWGET_HOSTAPD_SET_ENCRYPTION:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_ENCRYPTION\n");
+		pr_debug("VIAWGET_HOSTAPD_SET_ENCRYPTION\n");
 		spin_lock_irq(&pDevice->lock);
 		ret = hostap_set_encryption(pDevice, param, p->length);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_GET_ENCRYPTION:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_GET_ENCRYPTION\n");
+		pr_debug("VIAWGET_HOSTAPD_GET_ENCRYPTION\n");
 		spin_lock_irq(&pDevice->lock);
 		ret = hostap_get_encryption(pDevice, param, p->length);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR\n");
+		pr_debug("VIAWGET_HOSTAPD_SET_ASSOC_AP_ADDR\n");
 		ret = -EOPNOTSUPP;
 		goto out;
 	case VIAWGET_HOSTAPD_FLUSH:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_FLUSH\n");
+		pr_debug("VIAWGET_HOSTAPD_FLUSH\n");
 		spin_lock_irq(&pDevice->lock);
 		hostap_flush_sta(pDevice);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_ADD_STA:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_ADD_STA\n");
+		pr_debug("VIAWGET_HOSTAPD_ADD_STA\n");
 		spin_lock_irq(&pDevice->lock);
 		ret = hostap_add_sta(pDevice, param);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_REMOVE_STA:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_REMOVE_STA\n");
+		pr_debug("VIAWGET_HOSTAPD_REMOVE_STA\n");
 		spin_lock_irq(&pDevice->lock);
 		ret = hostap_remove_sta(pDevice, param);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 	case VIAWGET_HOSTAPD_GET_INFO_STA:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_GET_INFO_STA\n");
+		pr_debug("VIAWGET_HOSTAPD_GET_INFO_STA\n");
 		ret = hostap_get_info_sta(pDevice, param);
 		ap_ioctl = 1;
 		break;
 	case VIAWGET_HOSTAPD_SET_FLAGS_STA:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_FLAGS_STA\n");
+		pr_debug("VIAWGET_HOSTAPD_SET_FLAGS_STA\n");
 		ret = hostap_set_flags_sta(pDevice, param);
 		break;
 	case VIAWGET_HOSTAPD_MLME:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_MLME\n");
+		pr_debug("VIAWGET_HOSTAPD_MLME\n");
 		ret = -EOPNOTSUPP;
 		goto out;
 	case VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT\n");
+		pr_debug("VIAWGET_HOSTAPD_SET_GENERIC_ELEMENT\n");
 		ret = hostap_set_generic_element(pDevice, param);
 		break;
 	case VIAWGET_HOSTAPD_SCAN_REQ:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_SCAN_REQ\n");
+		pr_debug("VIAWGET_HOSTAPD_SCAN_REQ\n");
 		ret = -EOPNOTSUPP;
 		goto out;
 	case VIAWGET_HOSTAPD_STA_CLEAR_STATS:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_HOSTAPD_STA_CLEAR_STATS\n");
+		pr_debug("VIAWGET_HOSTAPD_STA_CLEAR_STATS\n");
 		ret = -EOPNOTSUPP;
 		goto out;
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vt6655_hostap_ioctl: unknown cmd=%d\n",
-			(int)param->cmd);
+		pr_debug("vt6655_hostap_ioctl: unknown cmd=%d\n",
+			 (int)param->cmd);
 		ret = -EOPNOTSUPP;
 		goto out;
 	}
diff --git a/drivers/staging/vt6655/hostap.h b/drivers/staging/vt6655/hostap.h
index 6e801a428183..17df4e403fcf 100644
--- a/drivers/staging/vt6655/hostap.h
+++ b/drivers/staging/vt6655/hostap.h
@@ -52,7 +52,7 @@
 #define ARPHRD_IEEE80211 801
 #endif
 
-int vt6655_hostap_set_hostapd(PSDevice pDevice, int val, int rtnl_locked);
-int vt6655_hostap_ioctl(PSDevice pDevice, struct iw_point *p);
+int vt6655_hostap_set_hostapd(struct vnt_private *, int val, int rtnl_locked);
+int vt6655_hostap_ioctl(struct vnt_private *, struct iw_point *p);
 
 #endif // __HOSTAP_H__
diff --git a/drivers/staging/vt6655/ioctl.c b/drivers/staging/vt6655/ioctl.c
index 65e59336f03b..970e80d92fb9 100644
--- a/drivers/staging/vt6655/ioctl.c
+++ b/drivers/staging/vt6655/ioctl.c
@@ -38,13 +38,11 @@
 #include "wpactl.h"
 #include "rf.h"
 
-static int msglevel = MSG_LEVEL_INFO;
-
 #ifdef WPA_SM_Transtatus
 SWPAResult wpa_Result;
 #endif
 
-int private_ioctl(PSDevice pDevice, struct ifreq *rq)
+int private_ioctl(struct vnt_private *pDevice, struct ifreq *rq)
 {
 	PSCmdRequest	pReq = (PSCmdRequest)rq;
 	PSMgmtObject	pMgmt = pDevice->pMgmt;
@@ -74,7 +72,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 
 	switch (pReq->wCmdCode) {
 	case WLAN_CMD_BSS_SCAN:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_BSS_SCAN..begin\n");
+		pr_debug("WLAN_CMD_BSS_SCAN..begin\n");
 		if (copy_from_user(&sScanCmd, pReq->data, sizeof(SCmdScan))) {
 			result = -EFAULT;
 			break;
@@ -175,27 +173,27 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		memcpy(pMgmt->abyDesireSSID, pItemSSID, pItemSSID->len + WLAN_IEHDR_LEN);
 		if (sJoinCmd.wBSSType == ADHOC) {
 			pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ioct set to adhoc mode\n");
+			pr_debug("ioct set to adhoc mode\n");
 		} else {
 			pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ioct set to STA mode\n");
+			pr_debug("ioct set to STA mode\n");
 		}
 		if (sJoinCmd.bPSEnable == true) {
 			pDevice->ePSMode = WMAC_POWER_FAST;
 			pMgmt->wListenInterval = 2;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Power Saving On\n");
+			pr_debug("Power Saving On\n");
 		} else {
 			pDevice->ePSMode = WMAC_POWER_CAM;
 			pMgmt->wListenInterval = 1;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Power Saving Off\n");
+			pr_debug("Power Saving Off\n");
 		}
 
 		if (sJoinCmd.bShareKeyAuth == true) {
 			pMgmt->bShareKeyAlgorithm = true;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Share Key\n");
+			pr_debug("Share Key\n");
 		} else {
 			pMgmt->bShareKeyAlgorithm = false;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Open System\n");
+			pr_debug("Open System\n");
 		}
 		pDevice->uChannel = sJoinCmd.uChannel;
 		netif_stop_queue(pDevice->dev);
@@ -207,7 +205,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		break;
 
 	case WLAN_CMD_SET_WEP:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_SET_WEP Key.\n");
+		pr_debug("WLAN_CMD_SET_WEP Key\n");
 		memset(&sWEPCmd, 0, sizeof(SCmdSetWEP));
 		if (copy_from_user(&sWEPCmd, pReq->data, sizeof(SCmdSetWEP))) {
 			result = -EFAULT;
@@ -217,7 +215,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			pDevice->bEncryptionEnable = false;
 			pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 			MACvDisableDefaultKey(pDevice->PortOffset);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WEP function disable.\n");
+			pr_debug("WEP function disable\n");
 			break;
 		}
 
@@ -247,7 +245,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 	case WLAN_CMD_GET_LINK: {
 		SCmdLinkStatus sLinkStatus;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_GET_LINK status.\n");
+		pr_debug("WLAN_CMD_GET_LINK status\n");
 
 		memset(&sLinkStatus, 0, sizeof(sLinkStatus));
 
@@ -268,7 +266,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			memcpy(sLinkStatus.abySSID, pItemSSID->abySSID, pItemSSID->len);
 			memcpy(sLinkStatus.abyBSSID, pMgmt->abyCurrBSSID, WLAN_BSSID_LEN);
 			sLinkStatus.uLinkRate = pMgmt->sNodeDBTable[0].wTxDataRate;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Link Success!\n");
+			pr_debug(" Link Success!\n");
 		} else {
 			sLinkStatus.bLink = false;
 			sLinkStatus.uLinkRate = 0;
@@ -364,7 +362,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		break;
 
 	case WLAN_CMD_STOP_MAC:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_STOP_MAC\n");
+		pr_debug("WLAN_CMD_STOP_MAC\n");
 		netif_stop_queue(pDevice->dev);
 
 		spin_lock_irq(&pDevice->lock);
@@ -383,7 +381,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		break;
 
 	case WLAN_CMD_START_MAC:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_START_MAC\n");
+		pr_debug("WLAN_CMD_START_MAC\n");
 
 		if (pDevice->bMACSuspend == true) {
 			if (pDevice->bRadioOff == true)
@@ -396,7 +394,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		break;
 
 	case WLAN_CMD_SET_HOSTAPD:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_SET_HOSTAPD\n");
+		pr_debug("WLAN_CMD_SET_HOSTAPD\n");
 
 		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
 			result = -EFAULT;
@@ -404,23 +402,23 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 		}
 		if (sValue.dwValue == 1) {
 			if (vt6655_hostap_set_hostapd(pDevice, 1, 1) == 0) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable HOSTAP\n");
+				pr_debug("Enable HOSTAP\n");
 			} else {
 				result = -EFAULT;
 				break;
 			}
 		} else {
 			vt6655_hostap_set_hostapd(pDevice, 0, 1);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable HOSTAP\n");
+			pr_debug("Disable HOSTAP\n");
 		}
 		break;
 
 	case WLAN_CMD_SET_HOSTAPD_STA:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_SET_HOSTAPD_STA\n");
+		pr_debug("WLAN_CMD_SET_HOSTAPD_STA\n");
 		break;
 
 	case WLAN_CMD_SET_802_1X:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_SET_802_1X\n");
+		pr_debug("WLAN_CMD_SET_802_1X\n");
 		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
 			result = -EFAULT;
 			break;
@@ -428,15 +426,15 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 
 		if (sValue.dwValue == 1) {
 			pDevice->bEnable8021x = true;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable 802.1x\n");
+			pr_debug("Enable 802.1x\n");
 		} else {
 			pDevice->bEnable8021x = false;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable 802.1x\n");
+			pr_debug("Disable 802.1x\n");
 		}
 		break;
 
 	case WLAN_CMD_SET_HOST_WEP:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_SET_HOST_WEP\n");
+		pr_debug("WLAN_CMD_SET_HOST_WEP\n");
 		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
 			result = -EFAULT;
 			break;
@@ -444,32 +442,32 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 
 		if (sValue.dwValue == 1) {
 			pDevice->bEnableHostWEP = true;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable HostWEP\n");
+			pr_debug("Enable HostWEP\n");
 		} else {
 			pDevice->bEnableHostWEP = false;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable HostWEP\n");
+			pr_debug("Disable HostWEP\n");
 		}
 		break;
 
 	case WLAN_CMD_SET_WPA:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_SET_WPA\n");
+		pr_debug("WLAN_CMD_SET_WPA\n");
 
 		if (copy_from_user(&sValue, pReq->data, sizeof(SCmdValue))) {
 			result = -EFAULT;
 			break;
 		}
 		if (sValue.dwValue == 1) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "up wpadev\n");
+			pr_debug("up wpadev\n");
 			eth_hw_addr_inherit(pDevice->wpadev, pDevice->dev);
 			pDevice->bWPADEVUp = true;
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "close wpadev\n");
+			pr_debug("close wpadev\n");
 			pDevice->bWPADEVUp = false;
 		}
 		break;
 
 	case WLAN_CMD_AP_START:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WLAN_CMD_AP_START\n");
+		pr_debug("WLAN_CMD_AP_START\n");
 		if (pDevice->bRadioOff == true) {
 			CARDbRadioPowerOn(pDevice);
 			vMgrTimerInit(pDevice);
@@ -483,9 +481,9 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 
 		if (sStartAPCmd.wBSSType == AP) {
 			pMgmt->eConfigMode = WMAC_CONFIG_AP;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ioct set to AP mode\n");
+			pr_debug("ioct set to AP mode\n");
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ioct BSS type not set to AP mode\n");
+			pr_debug("ioct BSS type not set to AP mode\n");
 			result = -EFAULT;
 			break;
 		}
@@ -513,10 +511,10 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 
 		if (sStartAPCmd.bShareKeyAuth == true) {
 			pMgmt->bShareKeyAlgorithm = true;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Share Key\n");
+			pr_debug("Share Key\n");
 		} else {
 			pMgmt->bShareKeyAlgorithm = false;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Open System\n");
+			pr_debug("Open System\n");
 		}
 		memcpy(pMgmt->abyIBSSSuppRates, abySuppRates, 6);
 
@@ -540,8 +538,8 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 			pMgmt->abyIBSSSuppRates[3] |= BIT7;
 		}
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Support Rate= %*ph\n",
-			4, pMgmt->abyIBSSSuppRates + 2);
+		pr_debug("Support Rate= %*ph\n",
+			 4, pMgmt->abyIBSSSuppRates + 2);
 
 		netif_stop_queue(pDevice->dev);
 		spin_lock_irq(&pDevice->lock);
@@ -597,12 +595,12 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 				pNodeList->sNodeList[jj].byKeyIndex = pNode->byKeyIndex;
 				pNodeList->sNodeList[jj].wWepKeyLength = pNode->uWepKeyLength;
 				memcpy(&(pNodeList->sNodeList[jj].abyWepKey[0]), &(pNode->abyWepKey[0]), WEP_KEYMAXLEN);
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "key= %2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
-					pNodeList->sNodeList[jj].abyWepKey[0],
-					pNodeList->sNodeList[jj].abyWepKey[1],
-					pNodeList->sNodeList[jj].abyWepKey[2],
-					pNodeList->sNodeList[jj].abyWepKey[3],
-					pNodeList->sNodeList[jj].abyWepKey[4]);
+				pr_debug("key= %2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
+					 pNodeList->sNodeList[jj].abyWepKey[0],
+					 pNodeList->sNodeList[jj].abyWepKey[1],
+					 pNodeList->sNodeList[jj].abyWepKey[2],
+					 pNodeList->sNodeList[jj].abyWepKey[3],
+					 pNodeList->sNodeList[jj].abyWepKey[4]);
 				pNodeList->sNodeList[jj].bIsInFallback = pNode->bIsInFallback;
 				pNodeList->sNodeList[jj].uTxFailures = pNode->uTxFailures;
 				pNodeList->sNodeList[jj].uTxAttempts = pNode->uTxAttempts;
@@ -653,7 +651,7 @@ int private_ioctl(PSDevice pDevice, struct ifreq *rq)
 #endif
 
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Private command not support..\n");
+		pr_debug("Private command not support..\n");
 	}
 
 	return result;
diff --git a/drivers/staging/vt6655/ioctl.h b/drivers/staging/vt6655/ioctl.h
index 187fc915fd12..2dc5a5743e8d 100644
--- a/drivers/staging/vt6655/ioctl.h
+++ b/drivers/staging/vt6655/ioctl.h
@@ -31,6 +31,6 @@
 
 #include "device.h"
 
-int private_ioctl(PSDevice pDevice, struct ifreq *rq);
+int private_ioctl(struct vnt_private *, struct ifreq *rq);
 
 #endif // __IOCTL_H__
diff --git a/drivers/staging/vt6655/iowpa.h b/drivers/staging/vt6655/iowpa.h
index 772bc4c2a7b9..fe4b22ed49f4 100644
--- a/drivers/staging/vt6655/iowpa.h
+++ b/drivers/staging/vt6655/iowpa.h
@@ -75,7 +75,7 @@ struct viawget_wpa_param {
 			u8 bssid[6];
 			u8 ssid[32];
 			u8 ssid_len;
-			u8 *wpa_ie;
+			u8 __user *wpa_ie;
 			u16 wpa_ie_len;
 			int pairwise_suite;
 			int group_suite;
@@ -102,7 +102,7 @@ struct viawget_wpa_param {
 
 		struct {
 			u16 scan_count;
-			u8 *buf;
+			u8 __user *buf;
 		} scan_results;
 
 	} u;
diff --git a/drivers/staging/vt6655/iwctl.c b/drivers/staging/vt6655/iwctl.c
index 7ce23b57e78d..14a62bdae278 100644
--- a/drivers/staging/vt6655/iwctl.c
+++ b/drivers/staging/vt6655/iwctl.c
@@ -66,9 +66,6 @@ static const long frequency_list[] = {
 };
 
 /*---------------------  Static Classes  ----------------------------*/
-
-static int msglevel = MSG_LEVEL_INFO;
-
 /*---------------------  Static Variables  --------------------------*/
 /*---------------------  Static Functions  --------------------------*/
 
@@ -76,10 +73,10 @@ static int msglevel = MSG_LEVEL_INFO;
 
 struct iw_statistics *iwctl_get_wireless_stats(struct net_device *dev)
 {
-	PSDevice pDevice = netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	long ldBm;
 
-	pDevice->wstats.status = pDevice->eOPMode;
+	pDevice->wstats.status = pDevice->op_mode;
 #ifdef Calcu_LinkQual
 	if (pDevice->scStatistic.LinkQuality > 100)
 		pDevice->scStatistic.LinkQuality = 100;
@@ -108,7 +105,7 @@ static int iwctl_commit(struct net_device *dev,
 			void *wrq,
 			char *extra)
 {
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWCOMMIT\n");
+	pr_debug(" SIOCSIWCOMMIT\n");
 
 	return 0;
 }
@@ -134,13 +131,13 @@ static int iwctl_siwscan(struct net_device *dev,
 		  struct iw_point *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	struct iw_scan_req  *req = (struct iw_scan_req *)extra;
 	unsigned char abyScanSSID[WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1];
 	PWLAN_IE_SSID pItemSSID = NULL;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWSCAN\n");
+	pr_debug(" SIOCSIWSCAN\n");
 
 	if (pDevice->byReAssocCount > 0) {   //reject scan when re-associating!
 //send scan event to wpa_Supplicant
@@ -198,7 +195,7 @@ static int iwctl_giwscan(struct net_device *dev,
 		  char *extra)
 {
 	int ii, jj, kk;
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	PKnownBSS           pBSS;
 	PWLAN_IE_SSID       pItemSSID;
@@ -210,7 +207,7 @@ static int iwctl_giwscan(struct net_device *dev,
 	long ldBm;
 	char buf[MAX_WPA_IE_LEN * 2 + 30];
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSCAN\n");
+	pr_debug(" SIOCGIWSCAN\n");
 
 	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
 		// In scanning..
@@ -350,10 +347,10 @@ int iwctl_siwfreq(struct net_device *dev,
 		  struct iw_freq *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	int rc = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFREQ\n");
+	pr_debug(" SIOCSIWFREQ\n");
 
 	// If setting by frequency, convert to a channel
 	if ((wrq->e == 1) &&
@@ -374,11 +371,12 @@ int iwctl_siwfreq(struct net_device *dev,
 		int channel = wrq->m;
 
 		if ((channel < 1) || (channel > 14)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: New channel value of %d is invalid!\n", dev->name, wrq->m);
+			pr_debug("%s: New channel value of %d is invalid!\n",
+				 dev->name, wrq->m);
 			rc = -EINVAL;
 		} else {
 			// Yes ! We can set it !!!
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set to channel = %d\n", channel);
+			pr_debug(" Set to channel = %d\n", channel);
 			pDevice->uChannel = channel;
 			//2007-0207-04,<Add> by EinsnLiu
 			//Make change effect at once
@@ -398,10 +396,10 @@ int iwctl_giwfreq(struct net_device *dev,
 		  struct iw_freq *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFREQ\n");
+	pr_debug(" SIOCGIWFREQ\n");
 
 #ifdef WEXT_USECHANNELS
 	wrq->m = (int)pMgmt->uCurrChannel;
@@ -429,14 +427,14 @@ int iwctl_siwmode(struct net_device *dev,
 		  __u32 *wmode,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	int rc = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWMODE\n");
+	pr_debug(" SIOCSIWMODE\n");
 
 	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP && pDevice->bEnableHostapd) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Can't set operation mode, hostapd is running\n");
+		pr_debug("Can't set operation mode, hostapd is running\n");
 		return rc;
 	}
 
@@ -448,7 +446,7 @@ int iwctl_siwmode(struct net_device *dev,
 				pDevice->bCommit = true;
 
 		}
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to ad-hoc\n");
+		pr_debug("set mode to ad-hoc\n");
 		break;
 	case IW_MODE_AUTO:
 	case IW_MODE_INFRA:
@@ -458,7 +456,7 @@ int iwctl_siwmode(struct net_device *dev,
 				pDevice->bCommit = true;
 
 		}
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to infrastructure\n");
+		pr_debug("set mode to infrastructure\n");
 		break;
 	case IW_MODE_MASTER:
 
@@ -472,7 +470,7 @@ int iwctl_siwmode(struct net_device *dev,
 				pDevice->bCommit = true;
 
 		}
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set mode to Access Point\n");
+		pr_debug("set mode to Access Point\n");
 		break;
 
 	case IW_MODE_REPEAT:
@@ -495,10 +493,10 @@ int iwctl_giwmode(struct net_device *dev,
 		  __u32 *wmode,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWMODE\n");
+	pr_debug(" SIOCGIWMODE\n");
 	// If not managed, assume it's ad-hoc
 	switch (pMgmt->eConfigMode) {
 	case WMAC_CONFIG_ESS_STA:
@@ -533,7 +531,7 @@ int iwctl_giwrange(struct net_device *dev,
 	int i, k;
 	unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRANGE\n");
+	pr_debug(" SIOCGIWRANGE\n");
 	if (wrq->pointer) {
 		wrq->length = sizeof(struct iw_range);
 		memset(range, 0, sizeof(struct iw_range));
@@ -635,12 +633,12 @@ int iwctl_siwap(struct net_device *dev,
 		struct sockaddr *wrq,
 		char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	int rc = 0;
 	unsigned char ZeroBSSID[WLAN_BSSID_LEN] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAP\n");
+	pr_debug(" SIOCSIWAP\n");
 	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
 		// In scanning..
 		pr_debug("SIOCSIWAP(??)-->In scanning..\n");
@@ -694,10 +692,10 @@ int iwctl_giwap(struct net_device *dev,
 		struct sockaddr *wrq,
 		char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAP\n");
+	pr_debug(" SIOCGIWAP\n");
 
 	memcpy(wrq->sa_data, pMgmt->abyCurrBSSID, 6);
 	//2008-0410,<Modify> by Einsn Liu
@@ -728,10 +726,10 @@ int iwctl_giwaplist(struct net_device *dev,
 	struct iw_quality *q	= NULL;
 	PKnownBSS pBSS		= NULL;
 
-	PSDevice pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject pMgmt = &(pDevice->sMgmtObj);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWAPLIST\n");
+	pr_debug(" SIOCGIWAPLIST\n");
 
 	if (!capable(CAP_NET_ADMIN)) {
 		rc = -EPERM;
@@ -794,13 +792,13 @@ int iwctl_siwessid(struct net_device *dev,
 		   struct iw_point *wrq,
 		   char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	PWLAN_IE_SSID       pItemSSID;
 	//2008-0409-05, <Add> by Einsn Liu
 	unsigned char len;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWESSID\n");
+	pr_debug(" SIOCSIWESSID\n");
 	pDevice->fWPA_Authened = false;
 	if (pMgmt->eScanState ==  WMAC_IS_SCANNING) {
 		// In scanning..
@@ -885,7 +883,7 @@ int iwctl_siwessid(struct net_device *dev,
 		}
 #endif
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "set essid = %s\n", pItemSSID->abySSID);
+		pr_debug("set essid = %s\n", pItemSSID->abySSID);
 	}
 
 	if (pDevice->flags & DEVICE_FLAGS_OPENED)
@@ -903,11 +901,11 @@ int iwctl_giwessid(struct net_device *dev,
 		   struct iw_point *wrq,
 		   char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	PWLAN_IE_SSID       pItemSSID;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWESSID\n");
+	pr_debug(" SIOCGIWESSID\n");
 
 	// Note : if wrq->u.data.flags != 0, we should
 	// get the relevant SSID from the SSID list...
@@ -933,13 +931,13 @@ int iwctl_siwrate(struct net_device *dev,
 		  struct iw_param *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	int rc = 0;
 	u8	brate = 0;
 	int	i;
 	unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRATE\n");
+	pr_debug(" SIOCSIWRATE\n");
 	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
 		rc = -EINVAL;
 		return rc;
@@ -993,7 +991,8 @@ int iwctl_siwrate(struct net_device *dev,
 			pDevice->uConnectionRate = 3;
 		} else {
 			pDevice->uConnectionRate = brate;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Fixed to Rate %d\n", pDevice->uConnectionRate);
+			pr_debug("Fixed to Rate %d\n",
+				 pDevice->uConnectionRate);
 		}
 
 	} else {
@@ -1014,12 +1013,12 @@ int iwctl_giwrate(struct net_device *dev,
 		  struct iw_param *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 //2007-0118-05,<Mark> by EinsnLiu
 //Mark the unnecessary sentences.
 //    PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRATE\n");
+	pr_debug(" SIOCGIWRATE\n");
 	{
 		unsigned char abySupportedRates[13] = {0x02, 0x04, 0x0B, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x90};
 		int brate = 0;
@@ -1059,10 +1058,10 @@ int iwctl_siwrts(struct net_device *dev,
 		 struct iw_param *wrq,
 		 char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	int rc = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRTS\n");
+	pr_debug(" SIOCSIWRTS\n");
 
 	{
 		int rthr = wrq->value;
@@ -1088,9 +1087,9 @@ int iwctl_giwrts(struct net_device *dev,
 		 struct iw_param *wrq,
 		 char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRTS\n");
+	pr_debug(" SIOCGIWRTS\n");
 	wrq->value = pDevice->wRTSThreshold;
 	wrq->disabled = (wrq->value >= 2312);
 	wrq->fixed = 1;
@@ -1107,11 +1106,11 @@ int iwctl_siwfrag(struct net_device *dev,
 		  struct iw_param *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	int rc = 0;
 	int fthr = wrq->value;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWFRAG\n");
+	pr_debug(" SIOCSIWFRAG\n");
 
 	if (wrq->disabled)
 		fthr = 2312;
@@ -1134,9 +1133,9 @@ int iwctl_giwfrag(struct net_device *dev,
 		  struct iw_param *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWFRAG\n");
+	pr_debug(" SIOCGIWFRAG\n");
 	wrq->value = pDevice->wFragmentationThreshold;
 	wrq->disabled = (wrq->value >= 2312);
 	wrq->fixed = 1;
@@ -1152,10 +1151,10 @@ int iwctl_siwretry(struct net_device *dev,
 		   struct iw_param *wrq,
 		   char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	int rc = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWRETRY\n");
+	pr_debug(" SIOCSIWRETRY\n");
 
 	if (wrq->disabled) {
 		rc = -EINVAL;
@@ -1187,9 +1186,9 @@ int iwctl_giwretry(struct net_device *dev,
 		   struct iw_param *wrq,
 		   char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWRETRY\n");
+	pr_debug(" SIOCGIWRETRY\n");
 	wrq->disabled = 0;      // Can't be disabled
 
 	// Note : by default, display the min retry number
@@ -1217,7 +1216,7 @@ int iwctl_siwencode(struct net_device *dev,
 		    struct iw_point *wrq,
 		    char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	unsigned long dwKeyIndex = (unsigned long)(wrq->flags & IW_ENCODE_INDEX);
 	int ii, uu, rc = 0;
@@ -1234,7 +1233,7 @@ int iwctl_siwencode(struct net_device *dev,
 
 	PSKeyTable pkeytab;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWENCODE\n");
+	pr_debug(" SIOCSIWENCODE\n");
 
 	if ((wrq->flags & IW_ENCODE_DISABLED) == 0) {
 		//Not disable encryption
@@ -1262,11 +1261,12 @@ int iwctl_siwencode(struct net_device *dev,
 		if (wrq->length > 0) {//have key
 
 			if (wrq->length ==  WLAN_WEP232_KEYLEN) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 232 bit wep key\n");
+				pr_debug("Set 232 bit wep key\n");
 			} else if (wrq->length ==  WLAN_WEP104_KEYLEN) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 104 bit wep key\n");
+				pr_debug("Set 104 bit wep key\n");
 			} else if (wrq->length == WLAN_WEP40_KEYLEN) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Set 40 bit wep key, index= %d\n", (int)dwKeyIndex);
+				pr_debug("Set 40 bit wep key, index= %d\n",
+					 (int)dwKeyIndex);
 			} else {//no support length
 				rc = -EINVAL;
 				return rc;
@@ -1274,9 +1274,9 @@ int iwctl_siwencode(struct net_device *dev,
 			memset(pDevice->abyKey, 0, WLAN_WEP232_KEYLEN);
 			memcpy(pDevice->abyKey, extra, wrq->length);
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyKey: ");
+			pr_debug("abyKey: ");
 			for (ii = 0; ii < wrq->length; ii++)
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pDevice->abyKey[ii]);
+				pr_debug("%02x ", pDevice->abyKey[ii]);
 
 			if (pDevice->flags & DEVICE_FLAGS_OPENED) {
 				spin_lock_irq(&pDevice->lock);
@@ -1304,10 +1304,10 @@ int iwctl_siwencode(struct net_device *dev,
 				rc = -EINVAL;
 				return rc;
 			}
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Just set Default key Index:\n");
+			pr_debug("Just set Default key Index:\n");
 			pkeytab = &(pDevice->sKey.KeyTable[MAX_KEY_TABLE - 1]);
 			if (pkeytab->GroupKey[(unsigned char)dwKeyIndex].uKeyLength == 0) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Default key len is 0\n");
+				pr_debug("Default key len is 0\n");
 				rc = -EINVAL;
 				return rc;
 			}
@@ -1317,7 +1317,7 @@ int iwctl_siwencode(struct net_device *dev,
 		}
 
 	} else {//disable the key
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disable WEP function\n");
+		pr_debug("Disable WEP function\n");
 		if (pDevice->bEncryptionEnable == false)
 			return 0;
 		pMgmt->bShareKeyAlgorithm = false;
@@ -1333,11 +1333,11 @@ int iwctl_siwencode(struct net_device *dev,
 //End Modify,Einsn
 
 	if (wrq->flags & IW_ENCODE_RESTRICTED) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & ShareKey System\n");
+		pr_debug("Enable WEP & ShareKey System\n");
 		pMgmt->bShareKeyAlgorithm = true;
 	}
 	if (wrq->flags & IW_ENCODE_OPEN) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enable WEP & Open System\n");
+		pr_debug("Enable WEP & Open System\n");
 		pMgmt->bShareKeyAlgorithm = false;
 	}
 	return rc;
@@ -1348,14 +1348,14 @@ int iwctl_giwencode(struct net_device *dev,
 		    struct iw_point *wrq,
 		    char *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject		pMgmt = &(pDevice->sMgmtObj);
 	char abyKey[WLAN_WEP232_KEYLEN];
 
 	unsigned int index = (unsigned int)(wrq->flags & IW_ENCODE_INDEX);
 	PSKeyItem	pKey = NULL;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWENCODE\n");
+	pr_debug(" SIOCGIWENCODE\n");
 
 	if (index > WLAN_WEP_NKEYS)
 		return	-EINVAL;
@@ -1410,11 +1410,11 @@ int iwctl_siwpower(struct net_device *dev,
 		   struct iw_param *wrq,
 		   char *extra)
 {
-	PSDevice            pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	int rc = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER\n");
+	pr_debug(" SIOCSIWPOWER\n");
 
 	if (!(pDevice->flags & DEVICE_FLAGS_OPENED)) {
 		rc = -EINVAL;
@@ -1436,14 +1436,14 @@ int iwctl_siwpower(struct net_device *dev,
 	}
 	switch (wrq->flags & IW_POWER_MODE) {
 	case IW_POWER_UNICAST_R:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_UNICAST_R\n");
+		pr_debug(" SIOCSIWPOWER: IW_POWER_UNICAST_R\n");
 		rc = -EINVAL;
 		break;
 	case IW_POWER_ALL_R:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ALL_R\n");
+		pr_debug(" SIOCSIWPOWER: IW_POWER_ALL_R\n");
 		rc = -EINVAL;
 	case IW_POWER_ON:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWPOWER: IW_POWER_ON\n");
+		pr_debug(" SIOCSIWPOWER: IW_POWER_ON\n");
 		break;
 	default:
 		rc = -EINVAL;
@@ -1460,11 +1460,11 @@ int iwctl_giwpower(struct net_device *dev,
 		   struct iw_param *wrq,
 		   char *extra)
 {
-	PSDevice            pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject        pMgmt = &(pDevice->sMgmtObj);
 	int mode = pDevice->ePSMode;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWPOWER\n");
+	pr_debug(" SIOCGIWPOWER\n");
 
 	wrq->disabled = (mode == WMAC_POWER_CAM);
 	if (wrq->disabled)
@@ -1490,10 +1490,10 @@ int iwctl_giwsens(struct net_device *dev,
 		  struct iw_param *wrq,
 		  char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	long ldBm;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCGIWSENS\n");
+	pr_debug(" SIOCGIWSENS\n");
 	if (pDevice->bLinkPass == true) {
 		RFvRSSITodBm(pDevice, (unsigned char)(pDevice->uCurrRSSI), &ldBm);
 		wrq->value = ldBm;
@@ -1514,13 +1514,13 @@ int iwctl_siwauth(struct net_device *dev,
 		  struct iw_param *wrq,
 		  char *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
 	int ret = 0;
 	static int wpa_version = 0;  //must be static to save the last value,einsn liu
 	static int pairwise = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " SIOCSIWAUTH\n");
+	pr_debug(" SIOCSIWAUTH\n");
 	switch (wrq->flags & IW_AUTH_INDEX) {
 	case IW_AUTH_WPA_VERSION:
 		wpa_version = wrq->value;
@@ -1623,7 +1623,7 @@ int iwctl_siwgenie(struct net_device *dev,
 		   struct iw_point *wrq,
 		   char __user *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
 	int ret = 0;
 	char length;
@@ -1663,7 +1663,7 @@ int iwctl_giwgenie(struct net_device *dev,
 		   struct iw_point *wrq,
 		   char __user *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
 	int ret = 0;
 	int space = wrq->length;
@@ -1688,7 +1688,7 @@ int iwctl_siwencodeext(struct net_device *dev,
 		       struct iw_point *wrq,
 		       char *extra)
 {
-	PSDevice	        pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	struct iw_encode_ext *ext = (struct iw_encode_ext *)extra;
 	struct viawget_wpa_param *param = NULL;
 //original member
@@ -1810,7 +1810,7 @@ int iwctl_siwmlme(struct net_device *dev,
 		  struct iw_point *wrq,
 		  char __user *extra)
 {
-	PSDevice			pDevice = (PSDevice)netdev_priv(dev);
+	struct vnt_private *pDevice = netdev_priv(dev);
 	PSMgmtObject	pMgmt = &(pDevice->sMgmtObj);
 	struct iw_mlme mime;
 
diff --git a/drivers/staging/vt6655/key.c b/drivers/staging/vt6655/key.c
index 9339e2a4073a..211afae306c7 100644
--- a/drivers/staging/vt6655/key.c
+++ b/drivers/staging/vt6655/key.c
@@ -44,8 +44,6 @@
 
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -134,7 +132,7 @@ bool KeybGetKey(
 {
 	int i;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetKey()\n");
+	pr_debug("KeybGetKey()\n");
 
 	*pKey = NULL;
 	for (i = 0; i < MAX_KEY_TABLE; i++) {
@@ -184,7 +182,7 @@ bool KeybSetKey(
 	unsigned char *pbyBSSID,
 	unsigned long dwKeyIndex,
 	unsigned long uKeyLength,
-	PQWORD          pKeyRSC,
+	u64 *pKeyRSC,
 	unsigned char *pbyKey,
 	unsigned char byKeyDecMode,
 	void __iomem *dwIoBase,
@@ -196,7 +194,7 @@ bool KeybSetKey(
 	PSKeyItem   pKey;
 	unsigned int uKeyIdx;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetKey: %lX\n", dwKeyIndex);
+	pr_debug("Enter KeybSetKey: %lX\n", dwKeyIndex);
 
 	j = (MAX_KEY_TABLE-1);
 	for (i = 0; i < (MAX_KEY_TABLE - 1); i++) {
@@ -221,7 +219,8 @@ bool KeybSetKey(
 				if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
 					// Group transmit key
 					pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[i].dwGTKeyIndex, i);
+					pr_debug("Group transmit key(R)[%lX]: %d\n",
+						 pTable->KeyTable[i].dwGTKeyIndex, i);
 				}
 				pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
 				pTable->KeyTable[i].wKeyCtl |= (byKeyDecMode << 4);
@@ -245,24 +244,24 @@ bool KeybSetKey(
 
 			if ((dwKeyIndex & USE_KEYRSC) == 0) {
 				// RSC set by NIC
-				memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+				pKey->KeyRSC = 0;
 			} else {
-				memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+				pKey->KeyRSC = *pKeyRSC;
 			}
 			pKey->dwTSC47_16 = 0;
 			pKey->wTSC15_0 = 0;
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(R):\n");
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: ");
+			pr_debug("KeybSetKey(R):\n");
+			pr_debug("pKey->bKeyValid: %d\n ", pKey->bKeyValid);
+			pr_debug("pKey->abyKey: ");
 			for (ii = 0; ii < pKey->uKeyLength; ii++)
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
+				pr_debug("%02x ", pKey->abyKey[ii]);
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+			pr_debug("\n");
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
+			pr_debug("pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
+			pr_debug("pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
+			pr_debug("pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
 
 			return true;
 		}
@@ -284,7 +283,8 @@ bool KeybSetKey(
 			if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
 				// Group transmit key
 				pTable->KeyTable[j].dwGTKeyIndex = dwKeyIndex;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(N)[%lX]: %d\n", pTable->KeyTable[j].dwGTKeyIndex, j);
+				pr_debug("Group transmit key(N)[%lX]: %d\n",
+					 pTable->KeyTable[j].dwGTKeyIndex, j);
 			}
 			pTable->KeyTable[j].wKeyCtl &= 0xFF0F;          // clear group key control filed
 			pTable->KeyTable[j].wKeyCtl |= (byKeyDecMode << 4);
@@ -308,25 +308,25 @@ bool KeybSetKey(
 
 		if ((dwKeyIndex & USE_KEYRSC) == 0) {
 			// RSC set by NIC
-			memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+			pKey->KeyRSC = 0;
 		} else {
-			memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+			pKey->KeyRSC = *pKeyRSC;
 		}
 		pKey->dwTSC47_16 = 0;
 		pKey->wTSC15_0 = 0;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(N):\n");
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: ");
+		pr_debug("KeybSetKey(N):\n");
+		pr_debug("pKey->bKeyValid: %d\n ", pKey->bKeyValid);
+		pr_debug("pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
+		pr_debug("pKey->abyKey: ");
 		for (ii = 0; ii < pKey->uKeyLength; ii++)
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
+			pr_debug("%02x ", pKey->abyKey[ii]);
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+		pr_debug("\n");
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
+		pr_debug("pKey->dwTSC47_16: %lx\n ", pKey->dwTSC47_16);
+		pr_debug("pKey->wTSC15_0: %x\n ", pKey->wTSC15_0);
+		pr_debug("pKey->dwKeyIndex: %lx\n ", pKey->dwKeyIndex);
 
 		return true;
 	}
@@ -511,48 +511,51 @@ bool KeybGetTransmitKey(
 				if (pTable->KeyTable[i].PairwiseKey.bKeyValid) {
 					*pKey = &(pTable->KeyTable[i].PairwiseKey);
 
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetTransmitKey:");
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PAIRWISE_KEY: KeyTable.abyBSSID: ");
+					pr_debug("KeybGetTransmitKey:");
+					pr_debug("PAIRWISE_KEY: KeyTable.abyBSSID: ");
 					for (ii = 0; ii < 6; ii++)
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x ", pTable->KeyTable[i].abyBSSID[ii]);
+						pr_debug("%x ",
+							 pTable->KeyTable[i].abyBSSID[ii]);
 
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+					pr_debug("\n");
 
 					return true;
 				} else {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PairwiseKey.bKeyValid == false\n");
+					pr_debug("PairwiseKey.bKeyValid == false\n");
 					return false;
 				}
 			} // End of Type == PAIRWISE
 			else {
 				if (pTable->KeyTable[i].dwGTKeyIndex == 0) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ERROR: dwGTKeyIndex == 0 !!!\n");
+					pr_debug("ERROR: dwGTKeyIndex == 0 !!!\n");
 					return false;
 				}
 				if (pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)].bKeyValid) {
 					*pKey = &(pTable->KeyTable[i].GroupKey[(pTable->KeyTable[i].dwGTKeyIndex&0x000000FF)]);
 
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybGetTransmitKey:");
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP_KEY: KeyTable.abyBSSID\n");
+					pr_debug("KeybGetTransmitKey:");
+					pr_debug("GROUP_KEY: KeyTable.abyBSSID\n");
 					for (ii = 0; ii < 6; ii++)
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x ", pTable->KeyTable[i].abyBSSID[ii]);
+						pr_debug("%x ",
+							 pTable->KeyTable[i].abyBSSID[ii]);
 
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "dwGTKeyIndex: %lX\n", pTable->KeyTable[i].dwGTKeyIndex);
+					pr_debug("\n");
+					pr_debug("dwGTKeyIndex: %lX\n",
+						 pTable->KeyTable[i].dwGTKeyIndex);
 
 					return true;
 				} else {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GroupKey.bKeyValid == false\n");
+					pr_debug("GroupKey.bKeyValid == false\n");
 					return false;
 				}
 			} // End of Type = GROUP
 		} // BSSID match
 	}
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "ERROR: NO Match BSSID !!! ");
+	pr_debug("ERROR: NO Match BSSID !!! ");
 	for (ii = 0; ii < 6; ii++)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", *(pbyBSSID+ii));
+		pr_debug("%02x ", *(pbyBSSID+ii));
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+	pr_debug("\n");
 	return false;
 }
 
@@ -606,7 +609,7 @@ bool KeybSetDefaultKey(
 	PSKeyManagement pTable,
 	unsigned long dwKeyIndex,
 	unsigned long uKeyLength,
-	PQWORD          pKeyRSC,
+	u64 *pKeyRSC,
 	unsigned char *pbyKey,
 	unsigned char byKeyDecMode,
 	void __iomem *dwIoBase,
@@ -617,7 +620,8 @@ bool KeybSetDefaultKey(
 	PSKeyItem   pKey;
 	unsigned int uKeyIdx;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetDefaultKey: %1x, %d\n", (int)dwKeyIndex, (int)uKeyLength);
+	pr_debug("Enter KeybSetDefaultKey: %1x, %d\n",
+		 (int)dwKeyIndex, (int)uKeyLength);
 
 	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
 		return false;
@@ -636,7 +640,9 @@ bool KeybSetDefaultKey(
 	if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
 		// Group transmit key
 		pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex = dwKeyIndex;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex, MAX_KEY_TABLE-1);
+		pr_debug("Group transmit key(R)[%lX]: %d\n",
+			 pTable->KeyTable[MAX_KEY_TABLE-1].dwGTKeyIndex,
+			 MAX_KEY_TABLE-1);
 
 	}
 	pTable->KeyTable[MAX_KEY_TABLE-1].wKeyCtl &= 0x7F00;          // clear all key control filed
@@ -669,25 +675,25 @@ bool KeybSetDefaultKey(
 
 	if ((dwKeyIndex & USE_KEYRSC) == 0) {
 		// RSC set by NIC
-		memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+		pKey->KeyRSC = 0;
 	} else {
-		memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+		pKey->KeyRSC = *pKeyRSC;
 	}
 	pKey->dwTSC47_16 = 0;
 	pKey->wTSC15_0 = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(R):\n");
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n", pKey->bKeyValid);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->uKeyLength: %d\n", (int)pKey->uKeyLength);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey:\n");
+	pr_debug("KeybSetKey(R):\n");
+	pr_debug("pKey->bKeyValid: %d\n", pKey->bKeyValid);
+	pr_debug("pKey->uKeyLength: %d\n", (int)pKey->uKeyLength);
+	pr_debug("pKey->abyKey:\n");
 	for (ii = 0; ii < pKey->uKeyLength; ii++)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%x", pKey->abyKey[ii]);
+		pr_debug("%x", pKey->abyKey[ii]);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+	pr_debug("\n");
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwTSC47_16: %lx\n", pKey->dwTSC47_16);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->wTSC15_0: %x\n", pKey->wTSC15_0);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->dwKeyIndex: %lx\n", pKey->dwKeyIndex);
+	pr_debug("pKey->dwTSC47_16: %lx\n", pKey->dwTSC47_16);
+	pr_debug("pKey->wTSC15_0: %x\n", pKey->wTSC15_0);
+	pr_debug("pKey->dwKeyIndex: %lx\n", pKey->dwKeyIndex);
 
 	return true;
 }
@@ -712,7 +718,7 @@ bool KeybSetAllGroupKey(
 	PSKeyManagement pTable,
 	unsigned long dwKeyIndex,
 	unsigned long uKeyLength,
-	PQWORD          pKeyRSC,
+	u64 *pKeyRSC,
 	unsigned char *pbyKey,
 	unsigned char byKeyDecMode,
 	void __iomem *dwIoBase,
@@ -724,7 +730,7 @@ bool KeybSetAllGroupKey(
 	PSKeyItem   pKey;
 	unsigned int uKeyIdx;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Enter KeybSetAllGroupKey: %lX\n", dwKeyIndex);
+	pr_debug("Enter KeybSetAllGroupKey: %lX\n", dwKeyIndex);
 
 	if ((dwKeyIndex & PAIRWISE_KEY) != 0) // Pairwise key
 		return false;
@@ -739,7 +745,8 @@ bool KeybSetAllGroupKey(
 			if ((dwKeyIndex & TRANSMIT_KEY) != 0)  {
 				// Group transmit key
 				pTable->KeyTable[i].dwGTKeyIndex = dwKeyIndex;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Group transmit key(R)[%lX]: %d\n", pTable->KeyTable[i].dwGTKeyIndex, i);
+				pr_debug("Group transmit key(R)[%lX]: %d\n",
+					 pTable->KeyTable[i].dwGTKeyIndex, i);
 
 			}
 			pTable->KeyTable[i].wKeyCtl &= 0xFF0F;          // clear group key control filed
@@ -764,21 +771,22 @@ bool KeybSetAllGroupKey(
 
 			if ((dwKeyIndex & USE_KEYRSC) == 0) {
 				// RSC set by NIC
-				memset(&(pKey->KeyRSC), 0, sizeof(QWORD));
+				pKey->KeyRSC = 0;
 			} else {
-				memcpy(&(pKey->KeyRSC), pKeyRSC,  sizeof(QWORD));
+				pKey->KeyRSC = *pKeyRSC;
 			}
 			pKey->dwTSC47_16 = 0;
 			pKey->wTSC15_0 = 0;
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KeybSetKey(R):\n");
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->bKeyValid: %d\n ", pKey->bKeyValid);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->uKeyLength: %d\n ", (int)pKey->uKeyLength);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pKey->abyKey: ");
+			pr_debug("KeybSetKey(R):\n");
+			pr_debug("pKey->bKeyValid: %d\n ", pKey->bKeyValid);
+			pr_debug("pKey->uKeyLength: %d\n ",
+				 (int)pKey->uKeyLength);
+			pr_debug("pKey->abyKey: ");
 			for (ii = 0; ii < pKey->uKeyLength; ii++)
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%02x ", pKey->abyKey[ii]);
+				pr_debug("%02x ", pKey->abyKey[ii]);
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "\n");
+			pr_debug("\n");
 
 		} // (pTable->KeyTable[i].bInUse == true)
 	}
diff --git a/drivers/staging/vt6655/key.h b/drivers/staging/vt6655/key.h
index 3eb881b69a55..44efe18315af 100644
--- a/drivers/staging/vt6655/key.h
+++ b/drivers/staging/vt6655/key.h
@@ -57,7 +57,7 @@ typedef struct tagSKeyItem {
 	bool bKeyValid;
 	unsigned long uKeyLength;
 	unsigned char abyKey[MAX_KEY_LEN];
-	QWORD       KeyRSC;
+	u64 KeyRSC;
 	unsigned long dwTSC47_16;
 	unsigned short wTSC15_0;
 	unsigned char byCipherSuite;
@@ -108,7 +108,7 @@ bool KeybSetKey(
 	unsigned char *pbyBSSID,
 	unsigned long dwKeyIndex,
 	unsigned long uKeyLength,
-	PQWORD          pKeyRSC,
+	u64 *pKeyRSC,
 	unsigned char *pbyKey,
 	unsigned char byKeyDecMode,
 	void __iomem *dwIoBase,
@@ -119,7 +119,7 @@ bool KeybSetDefaultKey(
 	PSKeyManagement pTable,
 	unsigned long dwKeyIndex,
 	unsigned long uKeyLength,
-	PQWORD          pKeyRSC,
+	u64 *pKeyRSC,
 	unsigned char *pbyKey,
 	unsigned char byKeyDecMode,
 	void __iomem *dwIoBase,
@@ -166,7 +166,7 @@ bool KeybSetAllGroupKey(
 	PSKeyManagement pTable,
 	unsigned long dwKeyIndex,
 	unsigned long uKeyLength,
-	PQWORD          pKeyRSC,
+	u64 *pKeyRSC,
 	unsigned char *pbyKey,
 	unsigned char byKeyDecMode,
 	void __iomem *dwIoBase,
diff --git a/drivers/staging/vt6655/mac.c b/drivers/staging/vt6655/mac.c
index 9bbc873de702..e3b0b7f7ca85 100644
--- a/drivers/staging/vt6655/mac.c
+++ b/drivers/staging/vt6655/mac.c
@@ -73,8 +73,6 @@
 #include "mac.h"
 
 unsigned short TxRate_iwconfig;//2008-5-8 <add> by chester
-/*---------------------  Static Definitions -------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -837,7 +835,7 @@ bool MACbSafeRxOff(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x10);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x10)\n");
+		pr_debug(" DBG_PORT80(0x10)\n");
 		return false;
 	}
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
@@ -847,7 +845,7 @@ bool MACbSafeRxOff(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x11);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x11)\n");
+		pr_debug(" DBG_PORT80(0x11)\n");
 		return false;
 	}
 
@@ -861,7 +859,7 @@ bool MACbSafeRxOff(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x12);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x12)\n");
+		pr_debug(" DBG_PORT80(0x12)\n");
 		return false;
 	}
 	return true;
@@ -899,7 +897,7 @@ bool MACbSafeTxOff(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x20);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x20)\n");
+		pr_debug(" DBG_PORT80(0x20)\n");
 		return false;
 	}
 	for (ww = 0; ww < W_MAX_TIMEOUT; ww++) {
@@ -909,7 +907,7 @@ bool MACbSafeTxOff(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x21);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x21)\n");
+		pr_debug(" DBG_PORT80(0x21)\n");
 		return false;
 	}
 
@@ -924,7 +922,7 @@ bool MACbSafeTxOff(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x24);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x24)\n");
+		pr_debug(" DBG_PORT80(0x24)\n");
 		return false;
 	}
 	return true;
@@ -949,13 +947,13 @@ bool MACbSafeStop(void __iomem *dwIoBase)
 
 	if (!MACbSafeRxOff(dwIoBase)) {
 		DBG_PORT80(0xA1);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " MACbSafeRxOff == false)\n");
+		pr_debug(" MACbSafeRxOff == false)\n");
 		MACbSafeSoftwareReset(dwIoBase);
 		return false;
 	}
 	if (!MACbSafeTxOff(dwIoBase)) {
 		DBG_PORT80(0xA2);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " MACbSafeTxOff == false)\n");
+		pr_debug(" MACbSafeTxOff == false)\n");
 		MACbSafeSoftwareReset(dwIoBase);
 		return false;
 	}
@@ -1172,7 +1170,7 @@ void MACvSetCurrAC0DescAddrEx(void __iomem *dwIoBase, unsigned long dwCurrDescAd
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x26);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x26)\n");
+		pr_debug(" DBG_PORT80(0x26)\n");
 	}
 	VNSvOutPortD(dwIoBase + MAC_REG_AC0DMAPTR, dwCurrDescAddr);
 	if (byOrgDMACtl & DMACTL_RUN)
@@ -1295,7 +1293,7 @@ bool MACbTxDMAOff(void __iomem *dwIoBase, unsigned int idx)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x29);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x29)\n");
+		pr_debug(" DBG_PORT80(0x29)\n");
 		return false;
 	}
 	return true;
@@ -1319,7 +1317,7 @@ void MACvClearBusSusInd(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x33);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x33)\n");
+		pr_debug(" DBG_PORT80(0x33)\n");
 	}
 }
 
@@ -1341,7 +1339,7 @@ void MACvEnableBusSusEn(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x34);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x34)\n");
+		pr_debug(" DBG_PORT80(0x34)\n");
 	}
 }
 
@@ -1364,7 +1362,7 @@ bool MACbFlushSYNCFifo(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x35);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x33)\n");
+		pr_debug(" DBG_PORT80(0x33)\n");
 	}
 	return true;
 }
@@ -1388,7 +1386,7 @@ bool MACbPSWakeup(void __iomem *dwIoBase)
 	}
 	if (ww == W_MAX_TIMEOUT) {
 		DBG_PORT80(0x36);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " DBG_PORT80(0x33)\n");
+		pr_debug(" DBG_PORT80(0x33)\n");
 		return false;
 	}
 	return true;
@@ -1419,7 +1417,7 @@ void MACvSetKeyEntry(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned in
 	if (byLocalID <= 1)
 		return;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetKeyEntry\n");
+	pr_debug("MACvSetKeyEntry\n");
 	wOffset = MISCFIFO_KEYETRY0;
 	wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
 
@@ -1427,7 +1425,8 @@ void MACvSetKeyEntry(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned in
 	dwData |= wKeyCtl;
 	dwData <<= 16;
 	dwData |= MAKEWORD(*(pbyAddr+4), *(pbyAddr+5));
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1. wOffset: %d, Data: %X, KeyCtl:%X\n", wOffset, dwData, wKeyCtl);
+	pr_debug("1. wOffset: %d, Data: %X, KeyCtl:%X\n",
+		 wOffset, dwData, wKeyCtl);
 
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
 	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
@@ -1442,7 +1441,7 @@ void MACvSetKeyEntry(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned in
 	dwData |= *(pbyAddr+1);
 	dwData <<= 8;
 	dwData |= *(pbyAddr+0);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2. wOffset: %d, Data: %X\n", wOffset, dwData);
+	pr_debug("2. wOffset: %d, Data: %X\n", wOffset, dwData);
 
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
 	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
@@ -1452,7 +1451,8 @@ void MACvSetKeyEntry(void __iomem *dwIoBase, unsigned short wKeyCtl, unsigned in
 	wOffset += (uKeyIdx * 4);
 	for (ii = 0; ii < 4; ii++) {
 		// always push 128 bits
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "3.(%d) wOffset: %d, Data: %X\n", ii, wOffset+ii, *pdwKey);
+		pr_debug("3.(%d) wOffset: %d, Data: %X\n",
+			 ii, wOffset+ii, *pdwKey);
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
 		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
@@ -1510,7 +1510,7 @@ void MACvSetDefaultKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
 	if (byLocalID <= 1)
 		return;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetDefaultKeyEntry\n");
+	pr_debug("MACvSetDefaultKeyEntry\n");
 	wOffset = MISCFIFO_KEYETRY0;
 	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
 
@@ -1519,7 +1519,8 @@ void MACvSetDefaultKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
 	wOffset += (uKeyIdx * 4);
 	// always push 128 bits
 	for (ii = 0; ii < 3; ii++) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
+		pr_debug("(%d) wOffset: %d, Data: %lX\n",
+			 ii, wOffset+ii, *pdwKey);
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
 		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
@@ -1531,7 +1532,7 @@ void MACvSetDefaultKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+3);
 	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "End. wOffset: %d, Data: %lX\n", wOffset+3, dwData);
+	pr_debug("End. wOffset: %d, Data: %lX\n", wOffset+3, dwData);
 }
 
 /*
@@ -1564,7 +1565,7 @@ void MACvSetDefaultKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
   VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
   VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
   VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-  DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvEnableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
+  pr_debug("MACvEnableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
 
   }
 */
@@ -1595,7 +1596,8 @@ void MACvDisableDefaultKey(void __iomem *dwIoBase)
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
 	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvDisableDefaultKey: wOffset: %d, Data: %lX\n", wOffset, dwData);
+	pr_debug("MACvDisableDefaultKey: wOffset: %d, Data: %lX\n",
+		 wOffset, dwData);
 }
 
 /*
@@ -1622,7 +1624,7 @@ void MACvSetDefaultTKIPKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
 	if (byLocalID <= 1)
 		return;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetDefaultTKIPKeyEntry\n");
+	pr_debug("MACvSetDefaultTKIPKeyEntry\n");
 	wOffset = MISCFIFO_KEYETRY0;
 	// Kyle test : change offset from 10 -> 0
 	wOffset += (10 * MISCFIFO_KEYENTRYSIZE);
@@ -1640,10 +1642,12 @@ void MACvSetDefaultTKIPKeyEntry(void __iomem *dwIoBase, unsigned int uKeyLen,
 	wOffset++;
 
 	wOffset += (uKeyIdx * 4);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1. wOffset: %d, Data: %lX, idx:%d\n", wOffset, *pdwKey, uKeyIdx);
+	pr_debug("1. wOffset: %d, Data: %lX, idx:%d\n",
+		 wOffset, *pdwKey, uKeyIdx);
 	// always push 128 bits
 	for (ii = 0; ii < 4; ii++) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "2.(%d) wOffset: %d, Data: %lX\n", ii, wOffset+ii, *pdwKey);
+		pr_debug("2.(%d) wOffset: %d, Data: %lX\n",
+			 ii, wOffset+ii, *pdwKey);
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset+ii);
 		VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, *pdwKey++);
 		VNSvOutPortW(dwIoBase + MAC_REG_MISCFFCTL, MISCFFCTL_WRITE);
@@ -1673,7 +1677,7 @@ void MACvSetDefaultKeyCtl(void __iomem *dwIoBase, unsigned short wKeyCtl, unsign
 	if (byLocalID <= 1)
 		return;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MACvSetKeyEntry\n");
+	pr_debug("MACvSetKeyEntry\n");
 	wOffset = MISCFIFO_KEYETRY0;
 	wOffset += (uEntryIdx * MISCFIFO_KEYENTRYSIZE);
 
@@ -1681,7 +1685,8 @@ void MACvSetDefaultKeyCtl(void __iomem *dwIoBase, unsigned short wKeyCtl, unsign
 	dwData |= wKeyCtl;
 	dwData <<= 16;
 	dwData |= 0xffff;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "1. wOffset: %d, Data: %lX, KeyCtl:%X\n", wOffset, dwData, wKeyCtl);
+	pr_debug("1. wOffset: %d, Data: %lX, KeyCtl:%X\n",
+		 wOffset, dwData, wKeyCtl);
 
 	VNSvOutPortW(dwIoBase + MAC_REG_MISCFFNDEX, wOffset);
 	VNSvOutPortD(dwIoBase + MAC_REG_MISCFFDATA, dwData);
diff --git a/drivers/staging/vt6655/mib.c b/drivers/staging/vt6655/mib.c
index 36895970a892..111c01877086 100644
--- a/drivers/staging/vt6655/mib.c
+++ b/drivers/staging/vt6655/mib.c
@@ -44,8 +44,6 @@
 #include "wctl.h"
 #include "baseband.h"
 
-/*---------------------  Static Definitions -------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -190,75 +188,101 @@ void STAvUpdateRDStatCounter(PSStatCounter pStatistic,
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr11MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "11M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr11M, (int)pStatistic->CustomStat.ullRsr11MCRCOk, byRSR);
+		pr_debug("11M: ALL[%d], OK[%d]:[%02x]\n",
+			 (int)pStatistic->CustomStat.ullRsr11M,
+			 (int)pStatistic->CustomStat.ullRsr11MCRCOk, byRSR);
 	} else if (byRxRate == 11) {
 		pStatistic->CustomStat.ullRsr5M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr5MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 5M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr5M, (int)pStatistic->CustomStat.ullRsr5MCRCOk, byRSR);
+		pr_debug(" 5M: ALL[%d], OK[%d]:[%02x]\n",
+			 (int)pStatistic->CustomStat.ullRsr5M,
+			 (int)pStatistic->CustomStat.ullRsr5MCRCOk, byRSR);
 	} else if (byRxRate == 4) {
 		pStatistic->CustomStat.ullRsr2M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr2MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 2M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr2M, (int)pStatistic->CustomStat.ullRsr2MCRCOk, byRSR);
+		pr_debug(" 2M: ALL[%d], OK[%d]:[%02x]\n",
+			 (int)pStatistic->CustomStat.ullRsr2M,
+			 (int)pStatistic->CustomStat.ullRsr2MCRCOk, byRSR);
 	} else if (byRxRate == 2) {
 		pStatistic->CustomStat.ullRsr1M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr1MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 1M: ALL[%d], OK[%d]:[%02x]\n", (int)pStatistic->CustomStat.ullRsr1M, (int)pStatistic->CustomStat.ullRsr1MCRCOk, byRSR);
+		pr_debug(" 1M: ALL[%d], OK[%d]:[%02x]\n",
+			 (int)pStatistic->CustomStat.ullRsr1M,
+			 (int)pStatistic->CustomStat.ullRsr1MCRCOk, byRSR);
 	} else if (byRxRate == 12) {
 		pStatistic->CustomStat.ullRsr6M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr6MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 6M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr6M, (int)pStatistic->CustomStat.ullRsr6MCRCOk);
+		pr_debug(" 6M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr6M,
+			 (int)pStatistic->CustomStat.ullRsr6MCRCOk);
 	} else if (byRxRate == 18) {
 		pStatistic->CustomStat.ullRsr9M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr9MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " 9M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr9M, (int)pStatistic->CustomStat.ullRsr9MCRCOk);
+		pr_debug(" 9M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr9M,
+			 (int)pStatistic->CustomStat.ullRsr9MCRCOk);
 	} else if (byRxRate == 24) {
 		pStatistic->CustomStat.ullRsr12M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr12MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "12M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr12M, (int)pStatistic->CustomStat.ullRsr12MCRCOk);
+		pr_debug("12M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr12M,
+			 (int)pStatistic->CustomStat.ullRsr12MCRCOk);
 	} else if (byRxRate == 36) {
 		pStatistic->CustomStat.ullRsr18M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr18MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "18M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr18M, (int)pStatistic->CustomStat.ullRsr18MCRCOk);
+		pr_debug("18M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr18M,
+			 (int)pStatistic->CustomStat.ullRsr18MCRCOk);
 	} else if (byRxRate == 48) {
 		pStatistic->CustomStat.ullRsr24M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr24MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "24M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr24M, (int)pStatistic->CustomStat.ullRsr24MCRCOk);
+		pr_debug("24M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr24M,
+			 (int)pStatistic->CustomStat.ullRsr24MCRCOk);
 	} else if (byRxRate == 72) {
 		pStatistic->CustomStat.ullRsr36M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr36MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "36M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr36M, (int)pStatistic->CustomStat.ullRsr36MCRCOk);
+		pr_debug("36M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr36M,
+			 (int)pStatistic->CustomStat.ullRsr36MCRCOk);
 	} else if (byRxRate == 96) {
 		pStatistic->CustomStat.ullRsr48M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr48MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "48M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr48M, (int)pStatistic->CustomStat.ullRsr48MCRCOk);
+		pr_debug("48M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr48M,
+			 (int)pStatistic->CustomStat.ullRsr48MCRCOk);
 	} else if (byRxRate == 108) {
 		pStatistic->CustomStat.ullRsr54M++;
 		if (byRSR & RSR_CRCOK)
 			pStatistic->CustomStat.ullRsr54MCRCOk++;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "54M: ALL[%d], OK[%d]\n", (int)pStatistic->CustomStat.ullRsr54M, (int)pStatistic->CustomStat.ullRsr54MCRCOk);
+		pr_debug("54M: ALL[%d], OK[%d]\n",
+			 (int)pStatistic->CustomStat.ullRsr54M,
+			 (int)pStatistic->CustomStat.ullRsr54MCRCOk);
 	} else {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Unknown: Total[%d], CRCOK[%d]\n", (int)pStatistic->dwRsrRxPacket+1, (int)pStatistic->dwRsrCRCOk);
+		pr_debug("Unknown: Total[%d], CRCOK[%d]\n",
+			 (int)pStatistic->dwRsrRxPacket+1,
+			 (int)pStatistic->dwRsrCRCOk);
 	}
 
 	if (byRSR & RSR_BSSIDOK)
diff --git a/drivers/staging/vt6655/power.c b/drivers/staging/vt6655/power.c
index 2a21cbd1c6ac..08241b917777 100644
--- a/drivers/staging/vt6655/power.c
+++ b/drivers/staging/vt6655/power.c
@@ -50,8 +50,6 @@
 
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -74,13 +72,13 @@ PSvEnablePowerSaving(
 	unsigned short wListenInterval
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned short wAID = pMgmt->wCurrAID | BIT14 | BIT15;
 
 	// set period of power up before TBTT
 	VNSvOutPortW(pDevice->PortOffset + MAC_REG_PWBT, C_PWBT);
-	if (pDevice->eOPMode != OP_MODE_ADHOC) {
+	if (pDevice->op_mode != NL80211_IFTYPE_ADHOC) {
 		// set AID
 		VNSvOutPortW(pDevice->PortOffset + MAC_REG_AIDATIM, wAID);
 	} else {
@@ -109,11 +107,12 @@ PSvEnablePowerSaving(
 	pDevice->bEnablePSMode = true;
 
 	/* We don't send null pkt in ad hoc mode since beacon will handle this. */
-	if (pDevice->eOPMode != OP_MODE_ADHOC && pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)
+	if (pDevice->op_mode != NL80211_IFTYPE_ADHOC &&
+	    pDevice->op_mode == NL80211_IFTYPE_STATION)
 		PSbSendNullPacket(pDevice);
 
 	pDevice->bPWBitOn = true;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "PS:Power Saving Mode Enable...\n");
+	pr_debug("PS:Power Saving Mode Enable...\n");
 }
 
 /*+
@@ -131,7 +130,7 @@ PSvDisablePowerSaving(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 
 	// disable power saving hw function
 	MACbPSWakeup(pDevice->PortOffset);
@@ -144,7 +143,7 @@ PSvDisablePowerSaving(
 
 	pDevice->bEnablePSMode = false;
 
-	if (pDevice->eOPMode == OP_MODE_INFRASTRUCTURE)
+	if (pDevice->op_mode == NL80211_IFTYPE_STATION)
 		PSbSendNullPacket(pDevice);
 
 	pDevice->bPWBitOn = false;
@@ -167,7 +166,7 @@ PSbConsiderPowerDown(
 	bool bCheckCountToWakeUp
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	unsigned int uIdx;
 
@@ -210,7 +209,7 @@ PSbConsiderPowerDown(
 
 	// no Tx, no Rx isr, now go to Doze
 	MACvRegBitsOn(pDevice->PortOffset, MAC_REG_PSCTL, PSCTL_GO2DOZE);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Go to Doze ZZZZZZZZZZZZZZZ\n");
+	pr_debug("Go to Doze ZZZZZZZZZZZZZZZ\n");
 	return true;
 }
 
@@ -229,7 +228,7 @@ PSvSendPSPOLL(
 	void *hDeviceContext
 )
 {
-	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	PSTxMgmtPacket      pTxPacket = NULL;
 
@@ -249,7 +248,7 @@ PSvSendPSPOLL(
 	pTxPacket->cbPayloadLen = 0;
 	// send the frame
 	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send PS-Poll packet failed..\n");
+		pr_debug("Send PS-Poll packet failed..\n");
 }
 
 /*+
@@ -266,7 +265,7 @@ PSbSendNullPacket(
 	void *hDeviceContext
 )
 {
-	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSTxMgmtPacket      pTxPacket = NULL;
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	unsigned int uIdx;
@@ -274,13 +273,9 @@ PSbSendNullPacket(
 	if (!pDevice->bLinkPass)
 		return false;
 
-#ifdef TxInSleep
 	if (!pDevice->bEnablePSMode && !pDevice->fTxDataInSleep)
 		return false;
-#else
-	if (!pDevice->bEnablePSMode)
-		return false;
-#endif
+
 	if (pDevice->bEnablePSMode) {
 		for (uIdx = 0; uIdx < TYPE_MAXTD; uIdx++) {
 			if (pDevice->iTDUsed[uIdx] != 0)
@@ -318,7 +313,7 @@ PSbSendNullPacket(
 	pTxPacket->cbPayloadLen = 0;
 	// send the frame
 	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Null Packet failed !\n");
+		pr_debug("Send Null Packet failed !\n");
 		return false;
 	}
 
@@ -340,7 +335,7 @@ PSbIsNextTBTTWakeUp(
 	void *hDeviceContext
 )
 {
-	PSDevice         pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	bool bWakeUp = false;
 
diff --git a/drivers/staging/vt6655/rf.c b/drivers/staging/vt6655/rf.c
index 42b257f916d3..e505af91bfd0 100644
--- a/drivers/staging/vt6655/rf.c
+++ b/drivers/staging/vt6655/rf.c
@@ -746,7 +746,7 @@ static bool RFbAL2230SelectChannel(void __iomem *dwIoBase, unsigned char byChann
  *
  */
 bool RFbInit(
-	PSDevice  pDevice
+	struct vnt_private *pDevice
 )
 {
 	bool bResult = true;
@@ -897,7 +897,7 @@ bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigne
  *
  */
 bool RFbSetPower(
-	PSDevice  pDevice,
+	struct vnt_private *pDevice,
 	unsigned int uRATE,
 	unsigned int uCH
 )
@@ -978,7 +978,7 @@ bool RFbSetPower(
  */
 
 bool RFbRawSetPower(
-	PSDevice  pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPwr,
 	unsigned int uRATE
 )
@@ -1042,7 +1042,7 @@ bool RFbRawSetPower(
  -*/
 void
 RFvRSSITodBm(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byCurrRSSI,
 	long *pldBm
 	)
diff --git a/drivers/staging/vt6655/rf.h b/drivers/staging/vt6655/rf.h
index ba55561c45d2..be4ef88b7666 100644
--- a/drivers/staging/vt6655/rf.h
+++ b/drivers/staging/vt6655/rf.h
@@ -77,19 +77,19 @@
 bool IFRFbWriteEmbedded(void __iomem *dwIoBase, unsigned long dwData);
 bool RFbSelectChannel(void __iomem *dwIoBase, unsigned char byRFType, unsigned char byChannel);
 bool RFbInit(
-	PSDevice  pDevice
+	struct vnt_private *
 );
 bool RFvWriteWakeProgSyn(void __iomem *dwIoBase, unsigned char byRFType, unsigned int uChannel);
-bool RFbSetPower(PSDevice pDevice, unsigned int uRATE, unsigned int uCH);
+bool RFbSetPower(struct vnt_private *, unsigned int uRATE, unsigned int uCH);
 bool RFbRawSetPower(
-	PSDevice  pDevice,
+	struct vnt_private *,
 	unsigned char byPwr,
 	unsigned int uRATE
 );
 
 void
 RFvRSSITodBm(
-	PSDevice pDevice,
+	struct vnt_private *,
 	unsigned char byCurrRSSI,
 	long    *pldBm
 );
diff --git a/drivers/staging/vt6655/rxtx.c b/drivers/staging/vt6655/rxtx.c
index 0d45aa076fed..7a183f55e7eb 100644
--- a/drivers/staging/vt6655/rxtx.c
+++ b/drivers/staging/vt6655/rxtx.c
@@ -68,9 +68,6 @@
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
-
-#define	PLICE_DEBUG
 
 /*---------------------  Static Functions  --------------------------*/
 
@@ -112,7 +109,7 @@ static const unsigned short wFB_Opt1[2][5] = {
 static
 void
 s_vFillTxKey(
-	PSDevice   pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyBuf,
 	unsigned char *pbyIVHead,
 	PSKeyItem  pTransmitKey,
@@ -124,7 +121,7 @@ s_vFillTxKey(
 static
 void
 s_vFillRTSHead(
-	PSDevice         pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPktType,
 	void *pvRTS,
 	unsigned int	cbFrameLength,
@@ -138,7 +135,7 @@ s_vFillRTSHead(
 static
 void
 s_vGenerateTxParameter(
-	PSDevice         pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPktType,
 	void *pTxBufHead,
 	void *pvRrvTime,
@@ -152,7 +149,7 @@ s_vGenerateTxParameter(
 );
 
 static void s_vFillFragParameter(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyBuffer,
 	unsigned int	uTxType,
 	void *pvtdCurr,
@@ -161,15 +158,17 @@ static void s_vFillFragParameter(
 );
 
 static unsigned int
-s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyTxBufferAddr,
-		  unsigned int cbFrameBodySize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
-		  PSEthernetHeader psEthHeader, unsigned char *pPacket, bool bNeedEncrypt,
-		  PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum);
+s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
+		  unsigned char *pbyTxBufferAddr, unsigned int cbFrameBodySize,
+		  unsigned int uDMAIdx, PSTxDesc pHeadTD,
+		  PSEthernetHeader psEthHeader, unsigned char *pPacket,
+		  bool bNeedEncrypt, PSKeyItem pTransmitKey,
+		  unsigned int uNodeIndex, unsigned int *puMACfragNum);
 
 static
-unsigned int
+__le16
 s_uFillDataHead(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPktType,
 	void *pTxDataHead,
 	unsigned int cbFrameLength,
@@ -187,7 +186,7 @@ s_uFillDataHead(
 static
 void
 s_vFillTxKey(
-	PSDevice   pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyBuf,
 	unsigned char *pbyIVHead,
 	PSKeyItem  pTransmitKey,
@@ -196,9 +195,9 @@ s_vFillTxKey(
 	unsigned char *pMICHDR
 )
 {
+	struct vnt_mic_hdr *mic_hdr = (struct vnt_mic_hdr *)pMICHDR;
 	unsigned long *pdwIV = (unsigned long *)pbyIVHead;
 	unsigned long *pdwExtIV = (unsigned long *)((unsigned char *)pbyIVHead+4);
-	unsigned short wValue;
 	PS802_11Header  pMACHeader = (PS802_11Header)pbyHdrBuf;
 	unsigned long dwRevIVCounter;
 	unsigned char byKeyIndex = 0;
@@ -246,7 +245,7 @@ s_vFillTxKey(
 		*(pbyIVHead+3) = (unsigned char)(((byKeyIndex << 6) & 0xc0) | 0x20); // 0x20 is ExtIV
 		// Append IV&ExtIV after Mac Header
 		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vFillTxKey()---- pdwExtIV: %lx\n", *pdwExtIV);
+		pr_debug("vFillTxKey()---- pdwExtIV: %lx\n", *pdwExtIV);
 
 	} else if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
 		pTransmitKey->wTSC15_0++;
@@ -262,47 +261,45 @@ s_vFillTxKey(
 		//Append IV&ExtIV after Mac Header
 		*pdwExtIV = cpu_to_le32(pTransmitKey->dwTSC47_16);
 
-		//Fill MICHDR0
-		*pMICHDR = 0x59;
-		*((unsigned char *)(pMICHDR+1)) = 0; // TxPriority
-		memcpy(pMICHDR+2, &(pMACHeader->abyAddr2[0]), 6);
-		*((unsigned char *)(pMICHDR+8)) = HIBYTE(HIWORD(pTransmitKey->dwTSC47_16));
-		*((unsigned char *)(pMICHDR+9)) = LOBYTE(HIWORD(pTransmitKey->dwTSC47_16));
-		*((unsigned char *)(pMICHDR+10)) = HIBYTE(LOWORD(pTransmitKey->dwTSC47_16));
-		*((unsigned char *)(pMICHDR+11)) = LOBYTE(LOWORD(pTransmitKey->dwTSC47_16));
-		*((unsigned char *)(pMICHDR+12)) = HIBYTE(pTransmitKey->wTSC15_0);
-		*((unsigned char *)(pMICHDR+13)) = LOBYTE(pTransmitKey->wTSC15_0);
-		*((unsigned char *)(pMICHDR+14)) = HIBYTE(wPayloadLen);
-		*((unsigned char *)(pMICHDR+15)) = LOBYTE(wPayloadLen);
-
-		//Fill MICHDR1
-		*((unsigned char *)(pMICHDR+16)) = 0; // HLEN[15:8]
+		/* MICHDR0 */
+		mic_hdr->id = 0x59;
+		mic_hdr->tx_priority = 0;
+		memcpy(mic_hdr->mic_addr2, pMACHeader->abyAddr2, ETH_ALEN);
+
+		/* ccmp pn big endian order */
+		mic_hdr->ccmp_pn[0] = (u8)(pTransmitKey->dwTSC47_16 >> 24);
+		mic_hdr->ccmp_pn[1] = (u8)(pTransmitKey->dwTSC47_16 >> 16);
+		mic_hdr->ccmp_pn[2] = (u8)(pTransmitKey->dwTSC47_16 >> 8);
+		mic_hdr->ccmp_pn[3] = (u8)pTransmitKey->dwTSC47_16;
+		mic_hdr->ccmp_pn[4] = (u8)(pTransmitKey->wTSC15_0 >> 8);
+		mic_hdr->ccmp_pn[5] = (u8)pTransmitKey->wTSC15_0;
+
+		/* MICHDR1 */
+		mic_hdr->payload_len = cpu_to_be16(wPayloadLen);
+
 		if (pDevice->bLongHeader)
-			*((unsigned char *)(pMICHDR+17)) = 28; // HLEN[7:0]
+			mic_hdr->hlen = cpu_to_be16(28);
 		else
-			*((unsigned char *)(pMICHDR+17)) = 22; // HLEN[7:0]
-
-		wValue = cpu_to_le16(pMACHeader->wFrameCtl & 0xC78F);
-		memcpy(pMICHDR+18, (unsigned char *)&wValue, 2); // MSKFRACTL
-		memcpy(pMICHDR+20, &(pMACHeader->abyAddr1[0]), 6);
-		memcpy(pMICHDR+26, &(pMACHeader->abyAddr2[0]), 6);
-
-		//Fill MICHDR2
-		memcpy(pMICHDR+32, &(pMACHeader->abyAddr3[0]), 6);
-		wValue = pMACHeader->wSeqCtl;
-		wValue &= 0x000F;
-		wValue = cpu_to_le16(wValue);
-		memcpy(pMICHDR+38, (unsigned char *)&wValue, 2); // MSKSEQCTL
-		if (pDevice->bLongHeader)
-			memcpy(pMICHDR+40, &(pMACHeader->abyAddr4[0]), 6);
+			mic_hdr->hlen = cpu_to_be16(22);
+
+		memcpy(mic_hdr->addr1, pMACHeader->abyAddr1, ETH_ALEN);
+		memcpy(mic_hdr->addr2, pMACHeader->abyAddr2, ETH_ALEN);
 
+		/* MICHDR2 */
+		memcpy(mic_hdr->addr3, pMACHeader->abyAddr3, ETH_ALEN);
+		mic_hdr->frame_control =
+				cpu_to_le16(pMACHeader->wFrameCtl & 0xc78f);
+		mic_hdr->seq_ctrl = cpu_to_le16(pMACHeader->wSeqCtl & 0xf);
+
+		if (pDevice->bLongHeader)
+			memcpy(mic_hdr->addr4, pMACHeader->abyAddr4, ETH_ALEN);
 	}
 }
 
 static
 void
 s_vSWencryption(
-	PSDevice            pDevice,
+	struct vnt_private *pDevice,
 	PSKeyItem           pTransmitKey,
 	unsigned char *pbyPayloadHead,
 	unsigned short wPayloadSize
@@ -340,6 +337,12 @@ s_vSWencryption(
 	}
 }
 
+static __le16 vnt_time_stamp_off(struct vnt_private *priv, u16 rate)
+{
+	return cpu_to_le16(wTimeStampOff[priv->byPreambleType % 2]
+							[rate % MAX_RATE]);
+}
+
 /*byPktType : PK_TYPE_11A     0
   PK_TYPE_11B     1
   PK_TYPE_11GB    2
@@ -348,7 +351,7 @@ s_vSWencryption(
 static
 unsigned int
 s_uGetTxRsvTime(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPktType,
 	unsigned int cbFrameLength,
 	unsigned short wRate,
@@ -369,11 +372,18 @@ s_uGetTxRsvTime(
 		return uDataTime;
 }
 
+static __le16 vnt_rxtx_rsvtime_le16(struct vnt_private *priv, u8 pkt_type,
+				    u32 frame_length, u16 rate, bool need_ack)
+{
+	return cpu_to_le16((u16)s_uGetTxRsvTime(priv, pkt_type,
+						frame_length, rate, need_ack));
+}
+
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
 static
-unsigned int
+__le16
 s_uGetRTSCTSRsvTime(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byRTSRsvType,
 	unsigned char byPktType,
 	unsigned int cbFrameLength,
@@ -399,19 +409,19 @@ s_uGetRTSCTSRsvTime(
 		uCTSTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopCCKBasicRate);
 		uAckTime = BBuGetFrameTime(pDevice->byPreambleType, byPktType, 14, pDevice->byTopOFDMBasicRate);
 		uRrvTime = uCTSTime + uAckTime + uDataTime + 2*pDevice->uSIFS;
-		return uRrvTime;
+		return cpu_to_le16((u16)uRrvTime);
 	}
 
 	//RTSRrvTime
 	uRrvTime = uRTSTime + uCTSTime + uAckTime + uDataTime + 3*pDevice->uSIFS;
-	return uRrvTime;
+	return cpu_to_le16((u16)uRrvTime);
 }
 
 //byFreqType 0: 5GHz, 1:2.4Ghz
 static
 unsigned int
 s_uGetDataDuration(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byDurType,
 	unsigned int cbFrameLength,
 	unsigned char byPktType,
@@ -568,9 +578,9 @@ s_uGetDataDuration(
 
 //byFreqType: 0=>5GHZ 1=>2.4GHZ
 static
-unsigned int
+__le16
 s_uGetRTSCTSDuration(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byDurType,
 	unsigned int cbFrameLength,
 	unsigned char byPktType,
@@ -657,13 +667,13 @@ s_uGetRTSCTSDuration(
 		break;
 	}
 
-	return uDurTime;
+	return cpu_to_le16((u16)uDurTime);
 }
 
 static
-unsigned int
+__le16
 s_uFillDataHead(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPktType,
 	void *pTxDataHead,
 	unsigned int cbFrameLength,
@@ -676,7 +686,6 @@ s_uFillDataHead(
 	unsigned short wCurrentRate
 )
 {
-	unsigned short wLen = 0x0000;
 
 	if (pTxDataHead == NULL)
 		return 0;
@@ -684,107 +693,98 @@ s_uFillDataHead(
 
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption == AUTO_FB_NONE) {
-			PSTxDataHead_g pBuf = (PSTxDataHead_g)pTxDataHead;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-);
-			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-			BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-);
-			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-			//Get Duration and TimeStamp
-			pBuf->wDuration_a = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
-											   byPktType, wCurrentRate, bNeedAck, uFragIdx,
-											   cbLastFragmentSize, uMACfragNum,
-											   byFBOption)); //1: 2.4GHz
-			pBuf->wDuration_b = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
-											   PK_TYPE_11B, pDevice->byTopCCKBasicRate,
-											   bNeedAck, uFragIdx, cbLastFragmentSize,
-											   uMACfragNum, byFBOption)); //1: 2.4
-
-			pBuf->wTimeStampOff_a = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-			pBuf->wTimeStampOff_b = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][pDevice->byTopCCKBasicRate%MAX_RATE]);
-
-			return pBuf->wDuration_a;
+			struct vnt_tx_datahead_g *buf = pTxDataHead;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+					  byPktType, &buf->a);
+
+			vnt_get_phy_field(pDevice, cbFrameLength,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			/* Get Duration and TimeStamp */
+			buf->duration_a = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength,
+									      byPktType, wCurrentRate, bNeedAck, uFragIdx,
+									      cbLastFragmentSize, uMACfragNum,
+									      byFBOption));
+			buf->duration_b = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength,
+									      PK_TYPE_11B, pDevice->byTopCCKBasicRate,
+									      bNeedAck, uFragIdx, cbLastFragmentSize,
+									      uMACfragNum, byFBOption));
+
+			buf->time_stamp_off_a = vnt_time_stamp_off(pDevice, wCurrentRate);
+			buf->time_stamp_off_b = vnt_time_stamp_off(pDevice, pDevice->byTopCCKBasicRate);
+
+			return buf->duration_a;
 		} else {
-			// Auto Fallback
-			PSTxDataHead_g_FB pBuf = (PSTxDataHead_g_FB)pTxDataHead;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-);
-			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-			BBvCalculateParameter(pDevice, cbFrameLength, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-);
-			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-			//Get Duration and TimeStamp
-			pBuf->wDuration_a = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
-											   wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-			pBuf->wDuration_b = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, PK_TYPE_11B,
-											   pDevice->byTopCCKBasicRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-			pBuf->wDuration_a_f0 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
-											      wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-			pBuf->wDuration_a_f1 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
-											      wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //1: 2.4GHz
-
-			pBuf->wTimeStampOff_a = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-			pBuf->wTimeStampOff_b = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][pDevice->byTopCCKBasicRate%MAX_RATE]);
-
-			return pBuf->wDuration_a;
+			/* Auto Fallback */
+			struct vnt_tx_datahead_g_fb *buf = pTxDataHead;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+					  byPktType, &buf->a);
+
+			vnt_get_phy_field(pDevice, cbFrameLength,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+			/* Get Duration and TimeStamp */
+			buf->duration_a = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+									      wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->duration_b = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, PK_TYPE_11B,
+									       pDevice->byTopCCKBasicRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->duration_a_f0 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+										  wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->duration_a_f1 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+										 wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+
+			buf->time_stamp_off_a = vnt_time_stamp_off(pDevice, wCurrentRate);
+			buf->time_stamp_off_b = vnt_time_stamp_off(pDevice, pDevice->byTopCCKBasicRate);
+
+			return buf->duration_a;
 		} //if (byFBOption == AUTO_FB_NONE)
 	} else if (byPktType == PK_TYPE_11A) {
 		if ((byFBOption != AUTO_FB_NONE)) {
-			// Auto Fallback
-			PSTxDataHead_a_FB pBuf = (PSTxDataHead_a_FB)pTxDataHead;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-);
-			pBuf->wTransmitLength = cpu_to_le16(wLen);
-			//Get Duration and TimeStampOff
-
-			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
-											 wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
-			pBuf->wDuration_f0 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
-											    wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
-			pBuf->wDuration_f1 = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
-											    wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption)); //0: 5GHz
-			pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-			return pBuf->wDuration;
+			/* Auto Fallback */
+			struct vnt_tx_datahead_a_fb *buf = pTxDataHead;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+					  byPktType, &buf->a);
+
+			/* Get Duration and TimeStampOff */
+			buf->duration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+									    wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->duration_f0 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F0, cbFrameLength, byPktType,
+									       wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->duration_f1 = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A_F1, cbFrameLength, byPktType,
+										wCurrentRate, bNeedAck, uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption));
+			buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
+			return buf->duration;
 		} else {
-			PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-);
-			pBuf->wTransmitLength = cpu_to_le16(wLen);
-			//Get Duration and TimeStampOff
-
-			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
-											 wCurrentRate, bNeedAck, uFragIdx,
-											 cbLastFragmentSize, uMACfragNum,
-											 byFBOption));
-
-			pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-			return pBuf->wDuration;
+			struct vnt_tx_datahead_ab *buf = pTxDataHead;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+					  byPktType, &buf->ab);
+
+			/* Get Duration and TimeStampOff */
+			buf->duration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameLength, byPktType,
+									    wCurrentRate, bNeedAck, uFragIdx,
+									    cbLastFragmentSize, uMACfragNum,
+									    byFBOption));
+
+			buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
+			return buf->duration;
 		}
 	} else {
-		PSTxDataHead_ab pBuf = (PSTxDataHead_ab)pTxDataHead;
-		//Get SignalField,ServiceField,Length
-		BBvCalculateParameter(pDevice, cbFrameLength, wCurrentRate, byPktType,
-				      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-);
-		pBuf->wTransmitLength = cpu_to_le16(wLen);
-		//Get Duration and TimeStampOff
-		pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
-										 wCurrentRate, bNeedAck, uFragIdx,
-										 cbLastFragmentSize, uMACfragNum,
-										 byFBOption));
-		pBuf->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-		return pBuf->wDuration;
+		struct vnt_tx_datahead_ab *buf = pTxDataHead;
+		/* Get SignalField, ServiceField & Length */
+		vnt_get_phy_field(pDevice, cbFrameLength, wCurrentRate,
+				  byPktType, &buf->ab);
+		/* Get Duration and TimeStampOff */
+		buf->duration = cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameLength, byPktType,
+								    wCurrentRate, bNeedAck, uFragIdx,
+								    cbLastFragmentSize, uMACfragNum,
+								    byFBOption));
+		buf->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
+		return buf->duration;
 	}
 	return 0;
 }
@@ -792,7 +792,7 @@ s_uFillDataHead(
 static
 void
 s_vFillRTSHead(
-	PSDevice         pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPktType,
 	void *pvRTS,
 	unsigned int cbFrameLength,
@@ -804,7 +804,6 @@ s_vFillRTSHead(
 )
 {
 	unsigned int uRTSFrameLen = 20;
-	unsigned short wLen = 0x0000;
 
 	if (pvRTS == NULL)
 		return;
@@ -819,155 +818,222 @@ s_vFillRTSHead(
 	//       Otherwise, we need to modify codes for them.
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption == AUTO_FB_NONE) {
-			PSRTS_g pBuf = (PSRTS_g)pvRTS;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-);
-			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-);
-			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-			//Get Duration
-			pBuf->wDuration_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-			pBuf->wDuration_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3: 2.4G OFDMData
-			pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
-
-			pBuf->Data.wDurationID = pBuf->wDuration_aa;
-			//Get RTS Frame body
-			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-			    (pDevice->eOPMode == OP_MODE_AP)) {
-				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			struct vnt_rts_g *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->a);
+			/* Get Duration */
+			buf->duration_bb =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BB,
+						     cbFrameLength, PK_TYPE_11B,
+						     pDevice->byTopCCKBasicRate,
+						     bNeedAck, byFBOption);
+			buf->duration_aa =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			buf->data.duration = buf->duration_aa;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+
+			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
+				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
 			} else {
-				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
 			}
-			if (pDevice->eOPMode == OP_MODE_AP)
-				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			if (pDevice->op_mode == NL80211_IFTYPE_AP)
+				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
 			else
-				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
 
 		} else {
-			PSRTS_g_FB pBuf = (PSRTS_g_FB)pvRTS;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-);
-			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_a), (unsigned char *)&(pBuf->bySignalField_a)
-);
-			pBuf->wTransmitLength_a = cpu_to_le16(wLen);
-
-			//Get Duration
-			pBuf->wDuration_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, PK_TYPE_11B, pDevice->byTopCCKBasicRate, bNeedAck, byFBOption));    //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-			pBuf->wDuration_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //2:RTSDuration_aa, 1:2.4G, 2,3:2.4G OFDMData
-			pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //1:RTSDuration_ba, 1:2.4G, 2,3:2.4G OFDMData
-			pBuf->wRTSDuration_ba_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //4:wRTSDuration_ba_f0, 1:2.4G, 1:CCKData
-			pBuf->wRTSDuration_aa_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //5:wRTSDuration_aa_f0, 1:2.4G, 1:CCKData
-			pBuf->wRTSDuration_ba_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //6:wRTSDuration_ba_f1, 1:2.4G, 1:CCKData
-			pBuf->wRTSDuration_aa_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption));    //7:wRTSDuration_aa_f1, 1:2.4G, 1:CCKData
-			pBuf->Data.wDurationID = pBuf->wDuration_aa;
-			//Get RTS Frame body
-			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-			    (pDevice->eOPMode == OP_MODE_AP)) {
-				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			struct vnt_rts_g_fb *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->a);
+			/* Get Duration */
+			buf->duration_bb =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BB,
+						     cbFrameLength, PK_TYPE_11B,
+						     pDevice->byTopCCKBasicRate,
+						     bNeedAck, byFBOption);
+			buf->duration_aa =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_ba_f0 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_aa_f0 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_ba_f1 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_BA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_aa_f1 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->data.duration = buf->duration_aa;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+
+			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
+				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
 			} else {
-				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_AP)
-				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			if (pDevice->op_mode == NL80211_IFTYPE_AP)
+				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
 			else
-				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
 
 		} // if (byFBOption == AUTO_FB_NONE)
 	} else if (byPktType == PK_TYPE_11A) {
 		if (byFBOption == AUTO_FB_NONE) {
-			PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-);
-			pBuf->wTransmitLength = cpu_to_le16(wLen);
-			//Get Duration
-			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
-			pBuf->Data.wDurationID = pBuf->wDuration;
-			//Get RTS Frame body
-			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-			    (pDevice->eOPMode == OP_MODE_AP)) {
-				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			struct vnt_rts_ab *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->ab);
+			/* Get Duration */
+			buf->duration =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->data.duration = buf->duration;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+
+			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
+				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
 			} else {
-				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
 			}
 
-			if (pDevice->eOPMode == OP_MODE_AP)
-				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			if (pDevice->op_mode == NL80211_IFTYPE_AP)
+				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
 			else
-				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
 
 		} else {
-			PSRTS_a_FB pBuf = (PSRTS_a_FB)pvRTS;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopOFDMBasicRate, byPktType,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-);
-			pBuf->wTransmitLength = cpu_to_le16(wLen);
-			//Get Duration
-			pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_aa, 0:5G, 0: 5G OFDMData
-			pBuf->wRTSDuration_f0 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //5:RTSDuration_aa_f0, 0:5G, 0: 5G OFDMData
-			pBuf->wRTSDuration_f1 = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //7:RTSDuration_aa_f1, 0:5G, 0:
-			pBuf->Data.wDurationID = pBuf->wDuration;
-			//Get RTS Frame body
-			pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-			if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-			    (pDevice->eOPMode == OP_MODE_AP)) {
-				memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+			struct vnt_rts_a_fb *buf = pvRTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uRTSFrameLen,
+					  pDevice->byTopOFDMBasicRate,
+					  byPktType, &buf->a);
+			/* Get Duration */
+			buf->duration =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_f0 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->rts_duration_f1 =
+				s_uGetRTSCTSDuration(pDevice, RTSDUR_AA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+			buf->data.duration = buf->duration;
+			/* Get RTS Frame body */
+			buf->data.frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_CTL |
+						    IEEE80211_STYPE_RTS);
+
+			if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+			    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
+				memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
 			} else {
-				memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+				memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
 			}
-			if (pDevice->eOPMode == OP_MODE_AP)
-				memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			if (pDevice->op_mode == NL80211_IFTYPE_AP)
+				memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
 			else
-				memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+				memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
 		}
 	} else if (byPktType == PK_TYPE_11B) {
-		PSRTS_ab pBuf = (PSRTS_ab)pvRTS;
-		//Get SignalField,ServiceField,Length
-		BBvCalculateParameter(pDevice, uRTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-				      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField), (unsigned char *)&(pBuf->bySignalField)
-);
-		pBuf->wTransmitLength = cpu_to_le16(wLen);
-		//Get Duration
-		pBuf->wDuration = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //0:RTSDuration_bb, 1:2.4G, 1:CCKData
-		pBuf->Data.wDurationID = pBuf->wDuration;
-		//Get RTS Frame body
-		pBuf->Data.wFrameControl = TYPE_CTL_RTS;//0x00B4
-
-		if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-		    (pDevice->eOPMode == OP_MODE_AP)) {
-			memcpy(&(pBuf->Data.abyRA[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
+		struct vnt_rts_ab *buf = pvRTS;
+		/* Get SignalField, ServiceField & Length */
+		vnt_get_phy_field(pDevice, uRTSFrameLen,
+				  pDevice->byTopCCKBasicRate,
+				  PK_TYPE_11B, &buf->ab);
+		/* Get Duration */
+		buf->duration =
+			s_uGetRTSCTSDuration(pDevice, RTSDUR_BB, cbFrameLength,
+					     byPktType, wCurrentRate, bNeedAck,
+					     byFBOption);
+
+		buf->data.duration = buf->duration;
+		/* Get RTS Frame body */
+		buf->data.frame_control =
+			cpu_to_le16(IEEE80211_FTYPE_CTL | IEEE80211_STYPE_RTS);
+
+		if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+		    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
+			memcpy(&buf->data.ra, psEthHeader->abyDstAddr, ETH_ALEN);
 		} else {
-			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+			memcpy(&buf->data.ra, pDevice->abyBSSID, ETH_ALEN);
 		}
 
-		if (pDevice->eOPMode == OP_MODE_AP)
-			memcpy(&(pBuf->Data.abyTA[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
+		if (pDevice->op_mode == NL80211_IFTYPE_AP)
+			memcpy(&buf->data.ta, pDevice->abyBSSID, ETH_ALEN);
 		else
-			memcpy(&(pBuf->Data.abyTA[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
+			memcpy(&buf->data.ta, psEthHeader->abySrcAddr, ETH_ALEN);
 	}
 }
 
 static
 void
 s_vFillCTSHead(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned int uDMAIdx,
 	unsigned char byPktType,
 	void *pvCTS,
@@ -979,7 +1045,6 @@ s_vFillCTSHead(
 )
 {
 	unsigned int uCTSFrameLen = 14;
-	unsigned short wLen = 0x0000;
 
 	if (pvCTS == NULL)
 		return;
@@ -993,48 +1058,65 @@ s_vFillCTSHead(
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA) {
 			// Auto Fall back
-			PSCTS_FB pBuf = (PSCTS_FB)pvCTS;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-);
-
-			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-
-			pBuf->wDuration_ba = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
-			pBuf->wDuration_ba += pDevice->wCTSDuration;
-			pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
-			//Get CTSDuration_ba_f0
-			pBuf->wCTSDuration_ba_f0 = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F0, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //8:CTSDuration_ba_f0, 1:2.4G, 2,3:2.4G OFDM Data
-			pBuf->wCTSDuration_ba_f0 += pDevice->wCTSDuration;
-			pBuf->wCTSDuration_ba_f0 = cpu_to_le16(pBuf->wCTSDuration_ba_f0);
-			//Get CTSDuration_ba_f1
-			pBuf->wCTSDuration_ba_f1 = (unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F1, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption); //9:CTSDuration_ba_f1, 1:2.4G, 2,3:2.4G OFDM Data
-			pBuf->wCTSDuration_ba_f1 += pDevice->wCTSDuration;
-			pBuf->wCTSDuration_ba_f1 = cpu_to_le16(pBuf->wCTSDuration_ba_f1);
-			//Get CTS Frame body
-			pBuf->Data.wDurationID = pBuf->wDuration_ba;
-			pBuf->Data.wFrameControl = TYPE_CTL_CTS;//0x00C4
-			pBuf->Data.wReserved = 0x0000;
-			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyCurrentNetAddr[0]), ETH_ALEN);
-
+			struct vnt_cts_fb *buf = pvCTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uCTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTSDuration_ba_f0 */
+			buf->cts_duration_ba_f0 =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F0,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTSDuration_ba_f1 */
+			buf->cts_duration_ba_f1 =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA_F1,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTS Frame body */
+			buf->data.duration = buf->duration_ba;
+
+			buf->data.frame_control =
+				cpu_to_le16(IEEE80211_FTYPE_CTL |
+					    IEEE80211_STYPE_CTS);
+
+			buf->reserved2 = 0x0;
+
+			memcpy(&buf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
 		} else { //if (byFBOption != AUTO_FB_NONE && uDMAIdx != TYPE_ATIMDMA && uDMAIdx != TYPE_BEACONDMA)
-			PSCTS pBuf = (PSCTS)pvCTS;
-			//Get SignalField,ServiceField,Length
-			BBvCalculateParameter(pDevice, uCTSFrameLen, pDevice->byTopCCKBasicRate, PK_TYPE_11B,
-					      (unsigned short *)&(wLen), (unsigned char *)&(pBuf->byServiceField_b), (unsigned char *)&(pBuf->bySignalField_b)
-);
-			pBuf->wTransmitLength_b = cpu_to_le16(wLen);
-			//Get CTSDuration_ba
-			pBuf->wDuration_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSDuration(pDevice, CTSDUR_BA, cbFrameLength, byPktType, wCurrentRate, bNeedAck, byFBOption)); //3:CTSDuration_ba, 1:2.4G, 2,3:2.4G OFDM Data
-			pBuf->wDuration_ba += pDevice->wCTSDuration;
-			pBuf->wDuration_ba = cpu_to_le16(pBuf->wDuration_ba);
-
-			//Get CTS Frame body
-			pBuf->Data.wDurationID = pBuf->wDuration_ba;
-			pBuf->Data.wFrameControl = TYPE_CTL_CTS;//0x00C4
-			pBuf->Data.wReserved = 0x0000;
-			memcpy(&(pBuf->Data.abyRA[0]), &(pDevice->abyCurrentNetAddr[0]), ETH_ALEN);
+			struct vnt_cts *buf = pvCTS;
+			/* Get SignalField, ServiceField & Length */
+			vnt_get_phy_field(pDevice, uCTSFrameLen,
+					  pDevice->byTopCCKBasicRate,
+					  PK_TYPE_11B, &buf->b);
+
+			/* Get CTSDuration_ba */
+			buf->duration_ba =
+				s_uGetRTSCTSDuration(pDevice, CTSDUR_BA,
+						     cbFrameLength, byPktType,
+						     wCurrentRate, bNeedAck,
+						     byFBOption);
+
+			/* Get CTS Frame body */
+			buf->data.duration = buf->duration_ba;
+
+			buf->data.frame_control =
+				cpu_to_le16(IEEE80211_FTYPE_CTL |
+					    IEEE80211_STYPE_CTS);
+
+			buf->reserved2 = 0x0;
+			memcpy(&buf->data.ra, pDevice->abyCurrentNetAddr, ETH_ALEN);
 		}
 	}
 }
@@ -1065,7 +1147,7 @@ s_vFillCTSHead(
 static
 void
 s_vGenerateTxParameter(
-	PSDevice         pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byPktType,
 	void *pTxBufHead,
 	void *pvRrvTime,
@@ -1099,71 +1181,58 @@ s_vGenerateTxParameter(
 	if (pDevice->bLongHeader)
 		cbMACHdLen = WLAN_HDR_ADDR3_LEN + 6;
 
+	if (!pvRrvTime)
+		return;
+
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
 		if (pvRTS != NULL) { //RTS_need
-			//Fill RsvTime
-			if (pvRrvTime) {
-				PSRrvTime_gRTS pBuf = (PSRrvTime_gRTS)pvRrvTime;
-
-				pBuf->wRTSTxRrvTime_aa = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 1:2.4GHz
-				pBuf->wRTSTxRrvTime_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 1, byPktType, cbFrameSize, wCurrentRate));//1:RTSTxRrvTime_ba, 1:2.4GHz
-				pBuf->wRTSTxRrvTime_bb = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
-				pBuf->wTxRrvTime_a = cpu_to_le16((unsigned short) s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
-				pBuf->wTxRrvTime_b = cpu_to_le16((unsigned short) s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
-			}
-			//Fill RTS
+			/* Fill RsvTime */
+			struct vnt_rrv_time_rts *buf = pvRrvTime;
+
+			buf->rts_rrv_time_aa = s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate);
+			buf->rts_rrv_time_ba = s_uGetRTSCTSRsvTime(pDevice, 1, byPktType, cbFrameSize, wCurrentRate);
+			buf->rts_rrv_time_bb = s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate);
+			buf->rrv_time_a = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
+			buf->rrv_time_b = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK);
+
 			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
 		} else {//RTS_needless, PCF mode
+			struct vnt_rrv_time_cts *buf = pvRrvTime;
 
-			//Fill RsvTime
-			if (pvRrvTime) {
-				PSRrvTime_gCTS pBuf = (PSRrvTime_gCTS)pvRrvTime;
-
-				pBuf->wTxRrvTime_a = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//2.4G OFDM
-				pBuf->wTxRrvTime_b = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK));//1:CCK
-				pBuf->wCTSTxRrvTime_ba = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 3, byPktType, cbFrameSize, wCurrentRate));//3:CTSTxRrvTime_Ba, 1:2.4GHz
-			}
+			buf->rrv_time_a = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
+			buf->rrv_time_b = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, pDevice->byTopCCKBasicRate, bNeedACK);
+			buf->cts_rrv_time_ba = s_uGetRTSCTSRsvTime(pDevice, 3, byPktType, cbFrameSize, wCurrentRate);
 
 			//Fill CTS
 			s_vFillCTSHead(pDevice, uDMAIdx, byPktType, pvCTS, cbFrameSize, bNeedACK, bDisCRC, wCurrentRate, byFBOption);
 		}
 	} else if (byPktType == PK_TYPE_11A) {
 		if (pvRTS != NULL) {//RTS_need, non PCF mode
-			//Fill RsvTime
-			if (pvRrvTime) {
-				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
+
+			buf->rts_rrv_time = s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate);
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK);
 
-				pBuf->wRTSTxRrvTime = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 2, byPktType, cbFrameSize, wCurrentRate));//2:RTSTxRrvTime_aa, 0:5GHz
-				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, byPktType, cbFrameSize, wCurrentRate, bNeedACK));//0:OFDM
-			}
 			//Fill RTS
 			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
 		} else if (pvRTS == NULL) {//RTS_needless, non PCF mode
-			//Fill RsvTime
-			if (pvRrvTime) {
-				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
 
-				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK)); //0:OFDM
-			}
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11A, cbFrameSize, wCurrentRate, bNeedACK);
 		}
 	} else if (byPktType == PK_TYPE_11B) {
 		if ((pvRTS != NULL)) {//RTS_need, non PCF mode
-			//Fill RsvTime
-			if (pvRrvTime) {
-				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
+
+			buf->rts_rrv_time = s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate);
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK);
 
-				pBuf->wRTSTxRrvTime = cpu_to_le16((unsigned short)s_uGetRTSCTSRsvTime(pDevice, 0, byPktType, cbFrameSize, wCurrentRate));//0:RTSTxRrvTime_bb, 1:2.4GHz
-				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK));//1:CCK
-			}
 			//Fill RTS
 			s_vFillRTSHead(pDevice, byPktType, pvRTS, cbFrameSize, bNeedACK, bDisCRC, psEthHeader, wCurrentRate, byFBOption);
 		} else { //RTS_needless, non PCF mode
-			//Fill RsvTime
-			if (pvRrvTime) {
-				PSRrvTime_ab pBuf = (PSRrvTime_ab)pvRrvTime;
+			struct vnt_rrv_time_ab *buf = pvRrvTime;
 
-				pBuf->wTxRrvTime = cpu_to_le16((unsigned short)s_uGetTxRsvTime(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK)); //1:CCK
-			}
+			buf->rrv_time = vnt_rxtx_rsvtime_le16(pDevice, PK_TYPE_11B, cbFrameSize, wCurrentRate, bNeedACK);
 		}
 	}
 }
@@ -1171,7 +1240,7 @@ s_vGenerateTxParameter(
 static
 void
 s_vFillFragParameter(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyBuffer,
 	unsigned int uTxType,
 	void *pvtdCurr,
@@ -1207,10 +1276,12 @@ s_vFillFragParameter(
 }
 
 static unsigned int
-s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyTxBufferAddr,
-		  unsigned int cbFrameBodySize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
-		  PSEthernetHeader psEthHeader, unsigned char *pPacket, bool bNeedEncrypt,
-		  PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum)
+s_cbFillTxBufHead(struct vnt_private *pDevice, unsigned char byPktType,
+		  unsigned char *pbyTxBufferAddr, unsigned int cbFrameBodySize,
+		  unsigned int uDMAIdx, PSTxDesc pHeadTD,
+		  PSEthernetHeader psEthHeader, unsigned char *pPacket,
+		  bool bNeedEncrypt, PSKeyItem pTransmitKey,
+		  unsigned int uNodeIndex, unsigned int *puMACfragNum)
 {
 	unsigned int cbMACHdLen;
 	unsigned int cbFrameSize;
@@ -1223,7 +1294,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	unsigned char *pbyIVHead;
 	unsigned char *pbyMacHdr;
 	unsigned short wFragType; //00:Non-Frag, 01:Start, 10:Mid, 11:Last
-	unsigned int uDuration;
+	__le16 uDuration;
 	unsigned char *pbyBuffer;
 	unsigned int cbIVlen = 0;
 	unsigned int cbICVlen = 0;
@@ -1252,7 +1323,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 	PSTxBufHead    psTxBufHd = (PSTxBufHead) pbyTxBufferAddr;
 	unsigned int cbHeaderLength = 0;
 	void *pvRrvTime;
-	PSMICHDRHead   pMICHDR;
+	struct vnt_mic_hdr *pMICHDR;
 	void *pvRTS;
 	void *pvCTS;
 	void *pvTxDataHd;
@@ -1264,8 +1335,8 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 	pvRrvTime = pMICHDR = pvRTS = pvCTS = pvTxDataHd = NULL;
 
-	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-	    (pDevice->eOPMode == OP_MODE_AP)) {
+	if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+	    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
 		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
 			bNeedACK = false;
 		else
@@ -1297,7 +1368,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		if (pTransmitKey->byCipherSuite == KEY_CTL_CCMP) {
 			cbIVlen = 8;//RSN Header
 			cbICVlen = 8;//MIC
-			cbMICHDR = sizeof(SMICHDRHead);
+			cbMICHDR = sizeof(struct vnt_mic_hdr);
 		}
 		if (pDevice->byLocalID > REV_ID_VT3253_A1) {
 			//MAC Header should be padding 0 to DW alignment.
@@ -1332,72 +1403,87 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 		if (byFBOption == AUTO_FB_NONE) {
 			if (bRTS == true) {//RTS_need
-				pvRrvTime = (PSRrvTime_gRTS) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS));
-				pvRTS = (PSRTS_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR);
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
+				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
 				pvCTS = NULL;
-				pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g));
-				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g) + sizeof(STxDataHead_g);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+							cbMICHDR + sizeof(struct vnt_rts_g));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+							cbMICHDR + sizeof(struct vnt_rts_g) +
+							sizeof(struct vnt_tx_datahead_g);
 			} else { //RTS_needless
-				pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts));
 				pvRTS = NULL;
-				pvCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
-				pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS));
-				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS) + sizeof(STxDataHead_g);
+				pvCTS = (void *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts) + cbMICHDR);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+						sizeof(struct vnt_rrv_time_cts) + cbMICHDR + sizeof(struct vnt_cts));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+							cbMICHDR + sizeof(struct vnt_cts) + sizeof(struct vnt_tx_datahead_g);
 			}
 		} else {
 			// Auto Fall Back
 			if (bRTS == true) {//RTS_need
-				pvRrvTime = (PSRrvTime_gRTS) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS));
-				pvRTS = (PSRTS_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR);
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts));
+				pvRTS = (void *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) + cbMICHDR);
 				pvCTS = NULL;
-				pvTxDataHd = (PSTxDataHead_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g_FB));
-				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gRTS) + cbMICHDR + sizeof(SRTS_g_FB) + sizeof(STxDataHead_g_FB);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+					cbMICHDR + sizeof(struct vnt_rts_g_fb));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_rts) +
+					cbMICHDR + sizeof(struct vnt_rts_g_fb) + sizeof(struct vnt_tx_datahead_g_fb);
 			} else { //RTS_needless
-				pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts));
 				pvRTS = NULL;
-				pvCTS = (PSCTS_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
-				pvTxDataHd = (PSTxDataHead_g_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS_FB));
-				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS_FB) + sizeof(STxDataHead_g_FB);
+				pvCTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts) + cbMICHDR);
+				pvTxDataHd = (void  *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+					cbMICHDR + sizeof(struct vnt_cts_fb));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+					cbMICHDR + sizeof(struct vnt_cts_fb) + sizeof(struct vnt_tx_datahead_g_fb);
 			}
 		} // Auto Fall Back
 	} else {//802.11a/b packet
 
 		if (byFBOption == AUTO_FB_NONE) {
 			if (bRTS == true) {
-				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-				pvRTS = (PSRTS_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
+				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
 				pvCTS = NULL;
-				pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_ab));
-				cbHeaderLength = wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_ab) + sizeof(STxDataHead_ab);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+					sizeof(struct vnt_rrv_time_ab) + cbMICHDR + sizeof(struct vnt_rts_ab));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_rts_ab) + sizeof(struct vnt_tx_datahead_ab);
 			} else { //RTS_needless, need MICHDR
-				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 				pvRTS = NULL;
 				pvCTS = NULL;
-				pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_ab);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_tx_datahead_ab);
 			}
 		} else {
 			// Auto Fall Back
 			if (bRTS == true) {//RTS_need
-				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-				pvRTS = (PSRTS_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
+				pvRTS = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
 				pvCTS = NULL;
-				pvTxDataHd = (PSTxDataHead_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_a_FB));
-				cbHeaderLength = wTxBufSize + sizeof(PSRrvTime_ab) + cbMICHDR + sizeof(SRTS_a_FB) + sizeof(STxDataHead_a_FB);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+					sizeof(struct vnt_rrv_time_ab) + cbMICHDR + sizeof(struct vnt_rts_a_fb));
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_rts_a_fb) + sizeof(struct vnt_tx_datahead_a_fb);
 			} else { //RTS_needless
-				pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-				pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+				pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+				pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 				pvRTS = NULL;
 				pvCTS = NULL;
-				pvTxDataHd = (PSTxDataHead_a_FB) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-				cbHeaderLength = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_a_FB);
+				pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
+				cbHeaderLength = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+					cbMICHDR + sizeof(struct vnt_tx_datahead_a_fb);
 			}
 		} // Auto Fall Back
 	}
@@ -1420,7 +1506,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 		MIC_vAppend((unsigned char *)&(psEthHeader->abyDstAddr[0]), 12);
 		dwMIC_Priority = 0;
 		MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC KEY: %X, %X\n", dwMICKey0, dwMICKey1);
+		pr_debug("MIC KEY: %X, %X\n", dwMICKey0, dwMICKey1);
 	}
 
 ///////////////////////////////////////////////////////////////////
@@ -1450,7 +1536,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//=========================
 				//    Start Fragmentation
 				//=========================
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Start Fragmentation...\n");
+				pr_debug("Start Fragmentation...\n");
 				wFragType = FRAGCTL_STAFRAG;
 
 				//Fill FIFO,RrvTime,RTS,and CTS
@@ -1460,7 +1546,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				uDuration = s_uFillDataHead(pDevice, byPktType, pvTxDataHd, cbFragmentSize, uDMAIdx, bNeedACK,
 							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
 				// Generate TX MAC Header
-				vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+				vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
 						   wFragType, uDMAIdx, uFragIdx);
 
 				if (bNeedEncrypt == true) {
@@ -1503,7 +1589,8 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				uTotalCopyLength += cbFragPayloadSize - cb802_1_H_len;
 
 				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Start MIC: %d\n", cbFragPayloadSize);
+					pr_debug("Start MIC: %d\n",
+						 cbFragPayloadSize);
 					MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFragPayloadSize);
 
 				}
@@ -1538,7 +1625,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//=========================
 				//    Last Fragmentation
 				//=========================
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last Fragmentation...\n");
+				pr_debug("Last Fragmentation...\n");
 
 				wFragType = FRAGCTL_ENDFRAG;
 
@@ -1550,7 +1637,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
 
 				// Generate TX MAC Header
-				vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+				vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
 						   wFragType, uDMAIdx, uFragIdx);
 
 				if (bNeedEncrypt == true) {
@@ -1588,8 +1675,10 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 
 				}
 				if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LAST: uMICFragLen:%d, cbLastFragPayloadSize:%d, uTmpLen:%d\n",
-						uMICFragLen, cbLastFragPayloadSize, uTmpLen);
+					pr_debug("LAST: uMICFragLen:%d, cbLastFragPayloadSize:%d, uTmpLen:%d\n",
+						 uMICFragLen,
+						 cbLastFragPayloadSize,
+						 uTmpLen);
 
 					if (bMIC2Frag == false) {
 						if (uTmpLen != 0)
@@ -1597,22 +1686,23 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 						pdwMIC_L = (u32 *)(pbyBuffer + uLength + uTmpLen);
 						pdwMIC_R = (u32 *)(pbyBuffer + uLength + uTmpLen + 4);
 						MIC_vGetMIC(pdwMIC_L, pdwMIC_R);
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Last MIC:%X, %X\n", *pdwMIC_L, *pdwMIC_R);
+						pr_debug("Last MIC:%X, %X\n",
+							 *pdwMIC_L, *pdwMIC_R);
 					} else {
 						if (uMICFragLen >= 4) {
 							memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
 							       (cbMIClen - uMICFragLen));
-							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LAST: uMICFragLen >= 4: %X, %d\n",
-								*(unsigned char *)((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
-								(cbMIClen - uMICFragLen));
+							pr_debug("LAST: uMICFragLen >= 4: %X, %d\n",
+								 *(unsigned char *)((unsigned char *)&dwSafeMIC_R + (uMICFragLen - 4)),
+								 (cbMIClen - uMICFragLen));
 
 						} else {
 							memcpy((pbyBuffer + uLength), ((unsigned char *)&dwSafeMIC_L + uMICFragLen),
 							       (4 - uMICFragLen));
 							memcpy((pbyBuffer + uLength + (4 - uMICFragLen)), &dwSafeMIC_R, 4);
-							DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "LAST: uMICFragLen < 4: %X, %d\n",
-								*(unsigned char *)((unsigned char *)&dwSafeMIC_R + uMICFragLen - 4),
-								(cbMIClen - uMICFragLen));
+							pr_debug("LAST: uMICFragLen < 4: %X, %d\n",
+								 *(unsigned char *)((unsigned char *)&dwSafeMIC_R + uMICFragLen - 4),
+								 (cbMIClen - uMICFragLen));
 						}
 					}
 					MIC_vUnInit();
@@ -1653,7 +1743,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				//=========================
 				//    Middle Fragmentation
 				//=========================
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Middle Fragmentation...\n");
+				pr_debug("Middle Fragmentation...\n");
 
 				wFragType = FRAGCTL_MIDFRAG;
 
@@ -1665,7 +1755,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 							    uFragIdx, cbLastFragmentSize, uMACfragNum, byFBOption, pDevice->wCurrentRate);
 
 				// Generate TX MAC Header
-				vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+				vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
 						   wFragType, uDMAIdx, uFragIdx);
 
 				if (bNeedEncrypt == true) {
@@ -1713,12 +1803,17 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 						dwSafeMIC_L = *pdwMIC_L;
 						dwSafeMIC_R = *pdwMIC_R;
 
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIDDLE: uMICFragLen:%d, cbFragPayloadSize:%d, uTmpLen:%d\n",
-							uMICFragLen, cbFragPayloadSize, uTmpLen);
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Fill MIC in Middle frag [%d]\n", uMICFragLen);
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get MIC:%X, %X\n", *pdwMIC_L, *pdwMIC_R);
+						pr_debug("MIDDLE: uMICFragLen:%d, cbFragPayloadSize:%d, uTmpLen:%d\n",
+							 uMICFragLen,
+							 cbFragPayloadSize,
+							 uTmpLen);
+						pr_debug("Fill MIC in Middle frag [%d]\n",
+							 uMICFragLen);
+						pr_debug("Get MIC:%X, %X\n",
+							 *pdwMIC_L, *pdwMIC_R);
 					}
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Middle frag len: %d\n", uTmpLen);
+					pr_debug("Middle frag len: %d\n",
+						 uTmpLen);
 
 				} else {
 					ASSERT(uTmpLen == (cbFragPayloadSize));
@@ -1768,7 +1863,7 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 					    0, 0, uMACfragNum, byFBOption, pDevice->wCurrentRate);
 
 		// Generate TX MAC Header
-		vGenerateMACHeader(pDevice, pbyMacHdr, (unsigned short)uDuration, psEthHeader, bNeedEncrypt,
+		vGenerateMACHeader(pDevice, pbyMacHdr, uDuration, psEthHeader, bNeedEncrypt,
 				   wFragType, uDMAIdx, 0);
 
 		if (bNeedEncrypt == true) {
@@ -1812,7 +1907,8 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 );
 
 		if ((bNeedEncrypt == true) && (pTransmitKey != NULL) && (pTransmitKey->byCipherSuite == KEY_CTL_TKIP)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Length:%d, %d\n", cbFrameBodySize - cb802_1_H_len, uLength);
+			pr_debug("Length:%d, %d\n",
+				 cbFrameBodySize - cb802_1_H_len, uLength);
 
 			MIC_vAppend((pbyBuffer + uLength - cb802_1_H_len), cbFrameBodySize);
 
@@ -1828,9 +1924,10 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 				pDevice->bTxMICFail = false;
 			}
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "uLength: %d, %d\n", uLength, cbFrameBodySize);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "cbReqCount:%d, %d, %d, %d\n", cbReqCount, cbHeaderLength, uPadding, cbIVlen);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC:%x, %x\n", *pdwMIC_L, *pdwMIC_R);
+			pr_debug("uLength: %d, %d\n", uLength, cbFrameBodySize);
+			pr_debug("cbReqCount:%d, %d, %d, %d\n",
+				 cbReqCount, cbHeaderLength, uPadding, cbIVlen);
+			pr_debug("MIC:%x, %x\n", *pdwMIC_L, *pdwMIC_R);
 
 		}
 
@@ -1861,8 +1958,9 @@ s_cbFillTxBufHead(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyT
 }
 
 void
-vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pbyTxBufferAddr,
-		    bool bNeedEncrypt, unsigned int cbPayloadSize, unsigned int uDMAIdx,
+vGenerateFIFOHeader(struct vnt_private *pDevice, unsigned char byPktType,
+		    unsigned char *pbyTxBufferAddr, bool bNeedEncrypt,
+		    unsigned int cbPayloadSize, unsigned int uDMAIdx,
 		    PSTxDesc pHeadTD, PSEthernetHeader psEthHeader, unsigned char *pPacket,
 		    PSKeyItem pTransmitKey, unsigned int uNodeIndex, unsigned int *puMACfragNum,
 		    unsigned int *pcbHeaderSize)
@@ -1878,8 +1976,8 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
 	memset(pTxBufHead, 0, wTxBufSize);
 	//Set FIFOCTL_NEEDACK
 
-	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-	    (pDevice->eOPMode == OP_MODE_AP)) {
+	if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+	    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
 		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0]))) {
 			bNeedACK = false;
 			pTxBufHead->wFIFOCtl = pTxBufHead->wFIFOCtl & (~FIFOCTL_NEEDACK);
@@ -1957,9 +2055,8 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
 		}
 	}
 
-#ifdef	PLICE_DEBUG
 	RFbSetPower(pDevice, pDevice->wCurrentRate, pDevice->byCurrentCh);
-#endif
+
 	pTxBufHead->byTxPower = pDevice->byCurPwr;
 
 	*pcbHeaderSize = s_cbFillTxBufHead(pDevice, byPktType, pbyTxBufferAddr, cbPayloadSize,
@@ -1988,9 +2085,9 @@ vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktType, unsigned char *pb
 
 void
 vGenerateMACHeader(
-	PSDevice         pDevice,
+	struct vnt_private *pDevice,
 	unsigned char *pbyBufferAddr,
-	unsigned short wDuration,
+	__le16 wDuration,
 	PSEthernetHeader psEthHeader,
 	bool bNeedEncrypt,
 	unsigned short wFragType,
@@ -2007,13 +2104,13 @@ vGenerateMACHeader(
 	else
 		pMACHeader->wFrameCtl = TYPE_802_11_DATA;
 
-	if (pDevice->eOPMode == OP_MODE_AP) {
+	if (pDevice->op_mode == NL80211_IFTYPE_AP) {
 		memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
 		memcpy(&(pMACHeader->abyAddr2[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
 		memcpy(&(pMACHeader->abyAddr3[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
 		pMACHeader->wFrameCtl |= FC_FROMDS;
 	} else {
-		if (pDevice->eOPMode == OP_MODE_ADHOC) {
+		if (pDevice->op_mode == NL80211_IFTYPE_ADHOC) {
 			memcpy(&(pMACHeader->abyAddr1[0]), &(psEthHeader->abyDstAddr[0]), ETH_ALEN);
 			memcpy(&(pMACHeader->abyAddr2[0]), &(psEthHeader->abySrcAddr[0]), ETH_ALEN);
 			memcpy(&(pMACHeader->abyAddr3[0]), &(pDevice->abyBSSID[0]), ETH_ALEN);
@@ -2028,7 +2125,7 @@ vGenerateMACHeader(
 	if (bNeedEncrypt)
 		pMACHeader->wFrameCtl |= cpu_to_le16((unsigned short)WLAN_SET_FC_ISWEP(1));
 
-	pMACHeader->wDurationID = cpu_to_le16(wDuration);
+	pMACHeader->wDurationID = le16_to_cpu(wDuration);
 
 	if (pDevice->bLongHeader) {
 		PWLAN_80211HDR_A4 pMACA4Header  = (PWLAN_80211HDR_A4) pbyBufferAddr;
@@ -2051,13 +2148,13 @@ vGenerateMACHeader(
 		pMACHeader->wFrameCtl |= FC_MOREFRAG;
 }
 
-CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
+CMD_STATUS csMgmt_xmit(struct vnt_private *pDevice, PSTxMgmtPacket pPacket)
 {
 	PSTxDesc        pFrstTD;
 	unsigned char byPktType;
 	unsigned char *pbyTxBufferAddr;
 	void *pvRTS;
-	PSCTS           pCTS;
+	struct vnt_cts *pCTS;
 	void *pvTxDataHd;
 	unsigned int uDuration;
 	unsigned int cbReqCount;
@@ -2192,20 +2289,24 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 
 	//Set RrvTime/RTS/CTS Buffer
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
-
-		pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
+		pvRrvTime = (void *) (pbyTxBufferAddr + wTxBufSize);
 		pMICHDR = NULL;
 		pvRTS = NULL;
-		pCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
-		pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + sizeof(SCTS));
-		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_gCTS) + sizeof(SCTS) + sizeof(STxDataHead_g);
+		pCTS = (struct vnt_cts *)(pbyTxBufferAddr + wTxBufSize +
+					sizeof(struct vnt_rrv_time_cts));
+		pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+				sizeof(struct vnt_rrv_time_cts) + sizeof(struct vnt_cts));
+		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+				sizeof(struct vnt_cts) + sizeof(struct vnt_tx_datahead_g);
 	} else { // 802.11a/b packet
-		pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
+		pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
 		pMICHDR = NULL;
 		pvRTS = NULL;
 		pCTS = NULL;
-		pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
-		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_ab) + sizeof(STxDataHead_ab);
+		pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+			sizeof(struct vnt_rrv_time_ab));
+		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+			sizeof(struct vnt_tx_datahead_ab);
 	}
 
 	memset((void *)(pbyTxBufferAddr + wTxBufSize), 0, (cbHeaderSize - wTxBufSize));
@@ -2248,18 +2349,18 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 		// S/W or H/W Encryption
 		//---------------------------
 		do {
-			if ((pDevice->eOPMode == OP_MODE_INFRASTRUCTURE) &&
+			if ((pDevice->op_mode == NL80211_IFTYPE_STATION) &&
 			    (pDevice->bLinkPass == true)) {
 				pbyBSSID = pDevice->abyBSSID;
 				// get pairwise key
 				if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, PAIRWISE_KEY, &pTransmitKey) == false) {
 					// get group key
 					if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == true) {
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get GTK.\n");
+						pr_debug("Get GTK\n");
 						break;
 					}
 				} else {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get PTK.\n");
+					pr_debug("Get PTK\n");
 					break;
 				}
 			}
@@ -2267,9 +2368,10 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 			pbyBSSID = pDevice->abyBroadcastAddr;
 			if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID, GROUP_KEY, &pTransmitKey) == false) {
 				pTransmitKey = NULL;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "KEY is NULL. OP Mode[%d]\n", pDevice->eOPMode);
+				pr_debug("KEY is NULL. OP Mode[%d]\n",
+					 pDevice->op_mode);
 			} else {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Get GTK.\n");
+				pr_debug("Get GTK\n");
 			}
 		} while (false);
 		//Fill TXKEY
@@ -2296,10 +2398,10 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 		// in the same place of other packet's Duration-field).
 		// And it will cause Cisco-AP to issue Disassociation-packet
 		if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-			((PSTxDataHead_g)pvTxDataHd)->wDuration_a = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
-			((PSTxDataHead_g)pvTxDataHd)->wDuration_b = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
+			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_a = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
+			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_b = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
 		} else {
-			((PSTxDataHead_ab)pvTxDataHd)->wDuration = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
+			((struct vnt_tx_datahead_ab *)pvTxDataHd)->duration = cpu_to_le16(pPacket->p80211Header->sA2.wDurationID);
 		}
 	}
 
@@ -2324,13 +2426,11 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 	pDevice->iTDUsed[TYPE_TXDMA0]++;
 
 	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
+		pr_debug(" available td0 <= 1\n");
 
 	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
 
-#ifdef TxInSleep
 	pDevice->nTxDataTimeCout = 0; //2008-8-21 chester <add> for send null packet
-#endif
 
 	// Poll Transmit the adapter
 	MACvTransmit0(pDevice->PortOffset);
@@ -2338,20 +2438,18 @@ CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 	return CMD_STATUS_PENDING;
 }
 
-CMD_STATUS csBeacon_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
+CMD_STATUS csBeacon_xmit(struct vnt_private *pDevice, PSTxMgmtPacket pPacket)
 {
 	unsigned char byPktType;
 	unsigned char *pbyBuffer = (unsigned char *)pDevice->tx_beacon_bufs;
 	unsigned int cbFrameSize = pPacket->cbMPDULen + WLAN_FCS_LEN;
 	unsigned int cbHeaderSize = 0;
-	unsigned short wTxBufSize = sizeof(STxShortBufHead);
-	PSTxShortBufHead pTxBufHead = (PSTxShortBufHead) pbyBuffer;
-	PSTxDataHead_ab  pTxDataHead = (PSTxDataHead_ab) (pbyBuffer + wTxBufSize);
+	struct vnt_tx_short_buf_head *short_head =
+				(struct vnt_tx_short_buf_head *)pbyBuffer;
 	PS802_11Header   pMACHeader;
 	unsigned short wCurrentRate;
-	unsigned short wLen = 0x0000;
 
-	memset(pTxBufHead, 0, wTxBufSize);
+	memset(short_head, 0, sizeof(*short_head));
 
 	if (pDevice->eCurrentPHYType == PHY_TYPE_11A) {
 		wCurrentRate = RATE_6M;
@@ -2364,27 +2462,30 @@ CMD_STATUS csBeacon_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 	//Set Preamble type always long
 	pDevice->byPreambleType = PREAMBLE_LONG;
 
-	//Set FIFOCTL_GENINT
-
-	pTxBufHead->wFIFOCtl |= FIFOCTL_GENINT;
+	/* Set FIFOCTL_GENINT */
+	short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_GENINT);
 
-	//Set packet type & Get Duration
+	/* Set packet type & Get Duration */
 	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
-		pTxDataHead->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_A, cbFrameSize, byPktType,
-											wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
+		short_head->duration =
+			cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_A,
+				    cbFrameSize, byPktType, wCurrentRate, false,
+				    0, 0, 1, AUTO_FB_NONE));
 	} else if (byPktType == PK_TYPE_11B) {//0000 0001 0000 0000
-		pTxBufHead->wFIFOCtl |= FIFOCTL_11B;
-		pTxDataHead->wDuration = cpu_to_le16((unsigned short)s_uGetDataDuration(pDevice, DATADUR_B, cbFrameSize, byPktType,
-											wCurrentRate, false, 0, 0, 1, AUTO_FB_NONE));
+		short_head->fifo_ctl |= cpu_to_le16(FIFOCTL_11B);
+
+		short_head->duration =
+			cpu_to_le16((u16)s_uGetDataDuration(pDevice, DATADUR_B,
+				    cbFrameSize, byPktType, wCurrentRate, false,
+				    0, 0, 1, AUTO_FB_NONE));
 	}
 
-	BBvCalculateParameter(pDevice, cbFrameSize, wCurrentRate, byPktType,
-			      (unsigned short *)&(wLen), (unsigned char *)&(pTxDataHead->byServiceField), (unsigned char *)&(pTxDataHead->bySignalField)
-);
-	pTxDataHead->wTransmitLength = cpu_to_le16(wLen);
-	//Get TimeStampOff
-	pTxDataHead->wTimeStampOff = cpu_to_le16(wTimeStampOff[pDevice->byPreambleType%2][wCurrentRate%MAX_RATE]);
-	cbHeaderSize = wTxBufSize + sizeof(STxDataHead_ab);
+	vnt_get_phy_field(pDevice, cbFrameSize,
+			  wCurrentRate, byPktType, &short_head->ab);
+
+	/* Get TimeStampOff */
+	short_head->time_stamp_off = vnt_time_stamp_off(pDevice, wCurrentRate);
+	cbHeaderSize = sizeof(struct vnt_tx_short_buf_head);
 
 	//Generate Beacon Header
 	pMACHeader = (PS802_11Header)(pbyBuffer + cbHeaderSize);
@@ -2412,7 +2513,7 @@ CMD_STATUS csBeacon_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket)
 
 unsigned int
 cbGetFragCount(
-	PSDevice         pDevice,
+	struct vnt_private *pDevice,
 	PSKeyItem        pTransmitKey,
 	unsigned int cbFrameBodySize,
 	PSEthernetHeader psEthHeader
@@ -2430,8 +2531,8 @@ cbGetFragCount(
 	unsigned int uMACfragNum = 1;
 	bool bNeedACK;
 
-	if ((pDevice->eOPMode == OP_MODE_ADHOC) ||
-	    (pDevice->eOPMode == OP_MODE_AP)) {
+	if ((pDevice->op_mode == NL80211_IFTYPE_ADHOC) ||
+	    (pDevice->op_mode == NL80211_IFTYPE_AP)) {
 		if (is_multicast_ether_addr(&(psEthHeader->abyDstAddr[0])))
 			bNeedACK = false;
 		else
@@ -2489,8 +2590,9 @@ cbGetFragCount(
 	return uMACfragNum;
 }
 
-void
-vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, unsigned int cbMPDULen) {
+void vDMA0_tx_80211(struct vnt_private *pDevice, struct sk_buff *skb,
+		    unsigned char *pbMPDU, unsigned int cbMPDULen)
+{
 	PSTxDesc        pFrstTD;
 	unsigned char byPktType;
 	unsigned char *pbyTxBufferAddr;
@@ -2577,7 +2679,8 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 		}
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vDMA0_tx_80211: p80211Header->sA3.wFrameCtl = %x\n", p80211Header->sA3.wFrameCtl);
+	pr_debug("vDMA0_tx_80211: p80211Header->sA3.wFrameCtl = %x\n",
+		 p80211Header->sA3.wFrameCtl);
 
 	//Set packet type
 	if (byPktType == PK_TYPE_11A) {//0000 0000 0000 0000
@@ -2656,7 +2759,7 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 		} else if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
 			cbIVlen = 8;//RSN Header
 			cbICVlen = 8;//MIC
-			cbMICHDR = sizeof(SMICHDRHead);
+			cbMICHDR = sizeof(struct vnt_mic_hdr);
 			pTxBufHead->wFragCtl |= FRAGCTL_AES;
 			pDevice->bAES = true;
 		}
@@ -2675,21 +2778,28 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 
 	if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {//802.11g packet
 
-		pvRrvTime = (PSRrvTime_gCTS) (pbyTxBufferAddr + wTxBufSize);
-		pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS));
+		pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+		pMICHDR = (struct vnt_mic_hdr *)(pbyTxBufferAddr + wTxBufSize +
+					sizeof(struct vnt_rrv_time_cts));
 		pvRTS = NULL;
-		pvCTS = (PSCTS) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR);
-		pvTxDataHd = (PSTxDataHead_g) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS));
-		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_gCTS) + cbMICHDR + sizeof(SCTS) + sizeof(STxDataHead_g);
+		pvCTS = (struct vnt_cts *)(pbyTxBufferAddr + wTxBufSize +
+				sizeof(struct vnt_rrv_time_cts) + cbMICHDR);
+		pvTxDataHd = (void *)(pbyTxBufferAddr + wTxBufSize +
+			sizeof(struct vnt_rrv_time_cts) + cbMICHDR + sizeof(struct vnt_cts));
+		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_cts) +
+				cbMICHDR + sizeof(struct vnt_cts) + sizeof(struct vnt_tx_datahead_g);
 
 	} else {//802.11a/b packet
 
-		pvRrvTime = (PSRrvTime_ab) (pbyTxBufferAddr + wTxBufSize);
-		pMICHDR = (PSMICHDRHead) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab));
+		pvRrvTime = (void *)(pbyTxBufferAddr + wTxBufSize);
+		pMICHDR = (struct vnt_mic_hdr *) (pbyTxBufferAddr +
+				wTxBufSize + sizeof(struct vnt_rrv_time_ab));
 		pvRTS = NULL;
 		pvCTS = NULL;
-		pvTxDataHd = (PSTxDataHead_ab) (pbyTxBufferAddr + wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR);
-		cbHeaderSize = wTxBufSize + sizeof(SRrvTime_ab) + cbMICHDR + sizeof(STxDataHead_ab);
+		pvTxDataHd = (void *)(pbyTxBufferAddr +
+			wTxBufSize + sizeof(struct vnt_rrv_time_ab) + cbMICHDR);
+		cbHeaderSize = wTxBufSize + sizeof(struct vnt_rrv_time_ab) +
+				cbMICHDR + sizeof(struct vnt_tx_datahead_ab);
 
 	}
 
@@ -2764,7 +2874,8 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 			MIC_vAppend((unsigned char *)&(sEthHeader.abyDstAddr[0]), 12);
 			dwMIC_Priority = 0;
 			MIC_vAppend((unsigned char *)&dwMIC_Priority, 4);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "DMA0_tx_8021:MIC KEY: %X, %X\n", dwMICKey0, dwMICKey1);
+			pr_debug("DMA0_tx_8021:MIC KEY: %X, %X\n",
+				 dwMICKey0, dwMICKey1);
 
 			uLength = cbHeaderSize + cbMacHdLen + uPadding + cbIVlen;
 
@@ -2782,9 +2893,10 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 				pDevice->bTxMICFail = false;
 			}
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "uLength: %d, %d\n", uLength, cbFrameBodySize);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "cbReqCount:%d, %d, %d, %d\n", cbReqCount, cbHeaderSize, uPadding, cbIVlen);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "MIC:%x, %x\n", *pdwMIC_L, *pdwMIC_R);
+			pr_debug("uLength: %d, %d\n", uLength, cbFrameBodySize);
+			pr_debug("cbReqCount:%d, %d, %d, %d\n",
+				 cbReqCount, cbHeaderSize, uPadding, cbIVlen);
+			pr_debug("MIC:%x, %x\n", *pdwMIC_L, *pdwMIC_R);
 
 		}
 
@@ -2812,10 +2924,10 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 		// in the same place of other packet's Duration-field).
 		// And it will cause Cisco-AP to issue Disassociation-packet
 		if (byPktType == PK_TYPE_11GB || byPktType == PK_TYPE_11GA) {
-			((PSTxDataHead_g)pvTxDataHd)->wDuration_a = cpu_to_le16(p80211Header->sA2.wDurationID);
-			((PSTxDataHead_g)pvTxDataHd)->wDuration_b = cpu_to_le16(p80211Header->sA2.wDurationID);
+			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_a = cpu_to_le16(p80211Header->sA2.wDurationID);
+			((struct vnt_tx_datahead_g *)pvTxDataHd)->duration_b = cpu_to_le16(p80211Header->sA2.wDurationID);
 		} else {
-			((PSTxDataHead_ab)pvTxDataHd)->wDuration = cpu_to_le16(p80211Header->sA2.wDurationID);
+			((struct vnt_tx_datahead_ab *)pvTxDataHd)->duration = cpu_to_le16(p80211Header->sA2.wDurationID);
 		}
 	}
 
@@ -2842,7 +2954,7 @@ vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, un
 	pDevice->iTDUsed[TYPE_TXDMA0]++;
 
 	if (AVAIL_TD(pDevice, TYPE_TXDMA0) <= 1)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " available td0 <= 1\n");
+		pr_debug(" available td0 <= 1\n");
 
 	pDevice->apCurrTD[TYPE_TXDMA0] = pFrstTD->next;
 
diff --git a/drivers/staging/vt6655/rxtx.h b/drivers/staging/vt6655/rxtx.h
index 601bedb211d6..8ee62887dee5 100644
--- a/drivers/staging/vt6655/rxtx.h
+++ b/drivers/staging/vt6655/rxtx.h
@@ -39,9 +39,151 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
+/* MIC HDR data header */
+struct vnt_mic_hdr {
+	u8 id;
+	u8 tx_priority;
+	u8 mic_addr2[ETH_ALEN];
+	u8 ccmp_pn[IEEE80211_CCMP_PN_LEN];
+	__be16 payload_len;
+	__be16 hlen;
+	__le16 frame_control;
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	u8 addr3[ETH_ALEN];
+	__le16 seq_ctrl;
+	u8 addr4[ETH_ALEN];
+	u16 packing; /* packing to 48 bytes */
+} __packed;
+
+/* RsvTime buffer header */
+struct vnt_rrv_time_rts {
+	__le16 rts_rrv_time_ba;
+	__le16 rts_rrv_time_aa;
+	__le16 rts_rrv_time_bb;
+	u16 reserved;
+	__le16 rrv_time_b;
+	__le16 rrv_time_a;
+} __packed;
+
+struct vnt_rrv_time_cts {
+	__le16 cts_rrv_time_ba;
+	u16 reserved;
+	__le16 rrv_time_b;
+	__le16 rrv_time_a;
+} __packed;
+
+struct vnt_rrv_time_ab {
+	__le16 rts_rrv_time;
+	__le16 rrv_time;
+} __packed;
+
+/* TX data header */
+struct vnt_tx_datahead_g {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_b;
+	__le16 duration_a;
+	__le16 time_stamp_off_b;
+	__le16 time_stamp_off_a;
+} __packed;
+
+struct vnt_tx_datahead_g_fb {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_b;
+	__le16 duration_a;
+	__le16 duration_a_f0;
+	__le16 duration_a_f1;
+	__le16 time_stamp_off_b;
+	__le16 time_stamp_off_a;
+} __packed;
+
+struct vnt_tx_datahead_ab {
+	struct vnt_phy_field ab;
+	__le16 duration;
+	__le16 time_stamp_off;
+} __packed;
+
+struct vnt_tx_datahead_a_fb {
+	struct vnt_phy_field a;
+	__le16 duration;
+	__le16 time_stamp_off;
+	__le16 duration_f0;
+	__le16 duration_f1;
+} __packed;
+
+/* RTS buffer header */
+struct vnt_rts_g {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_ba;
+	__le16 duration_aa;
+	__le16 duration_bb;
+	u16 reserved;
+	struct ieee80211_rts data;
+} __packed;
+
+struct vnt_rts_g_fb {
+	struct vnt_phy_field b;
+	struct vnt_phy_field a;
+	__le16 duration_ba;
+	__le16 duration_aa;
+	__le16 duration_bb;
+	u16 wReserved;
+	__le16 rts_duration_ba_f0;
+	__le16 rts_duration_aa_f0;
+	__le16 rts_duration_ba_f1;
+	__le16 rts_duration_aa_f1;
+	struct ieee80211_rts data;
+} __packed;
+
+struct vnt_rts_ab {
+	struct vnt_phy_field ab;
+	__le16 duration;
+	u16 reserved;
+	struct ieee80211_rts data;
+} __packed;
+
+struct vnt_rts_a_fb {
+	struct vnt_phy_field a;
+	__le16 duration;
+	u16 reserved;
+	__le16 rts_duration_f0;
+	__le16 rts_duration_f1;
+	struct ieee80211_rts data;
+} __packed;
+
+/* CTS buffer header */
+struct vnt_cts {
+	struct vnt_phy_field b;
+	__le16 duration_ba;
+	u16 reserved;
+	struct ieee80211_cts data;
+	u16 reserved2;
+} __packed;
+
+struct vnt_cts_fb {
+	struct vnt_phy_field b;
+	__le16 duration_ba;
+	u16 reserved;
+	__le16 cts_duration_ba_f0;
+	__le16 cts_duration_ba_f1;
+	struct ieee80211_cts data;
+	u16 reserved2;
+} __packed;
+
+struct vnt_tx_short_buf_head {
+	__le16 fifo_ctl;
+	u16 time_stamp;
+	struct vnt_phy_field ab;
+	__le16 duration;
+	__le16 time_stamp_off;
+} __packed;
+
 void
 vGenerateMACHeader(
-	PSDevice         pDevice,
+	struct vnt_private *,
 	unsigned char *pbyBufferAddr,
 	unsigned short wDuration,
 	PSEthernetHeader psEthHeader,
@@ -53,20 +195,24 @@ vGenerateMACHeader(
 
 unsigned int
 cbGetFragCount(
-	PSDevice         pDevice,
+	struct vnt_private *,
 	PSKeyItem        pTransmitKey,
 	unsigned int	cbFrameBodySize,
 	PSEthernetHeader psEthHeader
 );
 
 void
-vGenerateFIFOHeader(PSDevice pDevice, unsigned char byPktTyp, unsigned char *pbyTxBufferAddr,
-		    bool bNeedEncrypt, unsigned int	cbPayloadSize, unsigned int uDMAIdx, PSTxDesc pHeadTD,
-		    PSEthernetHeader psEthHeader, unsigned char *pPacket, PSKeyItem pTransmitKey,
-		    unsigned int uNodeIndex, unsigned int *puMACfragNum, unsigned int *pcbHeaderSize);
-
-void vDMA0_tx_80211(PSDevice  pDevice, struct sk_buff *skb, unsigned char *pbMPDU, unsigned int cbMPDULen);
-CMD_STATUS csMgmt_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket);
-CMD_STATUS csBeacon_xmit(PSDevice pDevice, PSTxMgmtPacket pPacket);
+vGenerateFIFOHeader(struct vnt_private *, unsigned char byPktTyp,
+		    unsigned char *pbyTxBufferAddr, bool bNeedEncrypt,
+		    unsigned int cbPayloadSize, unsigned int uDMAIdx,
+		    PSTxDesc pHeadTD, PSEthernetHeader psEthHeader,
+		    unsigned char *pPacket, PSKeyItem pTransmitKey,
+		    unsigned int uNodeIndex, unsigned int *puMACfragNum,
+		    unsigned int *pcbHeaderSize);
+
+void vDMA0_tx_80211(struct vnt_private *, struct sk_buff *skb,
+		    unsigned char *pbMPDU, unsigned int cbMPDULen);
+CMD_STATUS csMgmt_xmit(struct vnt_private *, PSTxMgmtPacket pPacket);
+CMD_STATUS csBeacon_xmit(struct vnt_private *, PSTxMgmtPacket pPacket);
 
 #endif // __RXTX_H__
diff --git a/drivers/staging/vt6655/ttype.h b/drivers/staging/vt6655/ttype.h
index b1d8ae732774..747ef62ec9be 100644
--- a/drivers/staging/vt6655/ttype.h
+++ b/drivers/staging/vt6655/ttype.h
@@ -31,10 +31,6 @@
 
 /******* Common definitions and typedefs ***********************************/
 
-#ifndef TxInSleep
-#define TxInSleep
-#endif
-
 #ifndef WPA_SM_Transtatus
 #define WPA_SM_Transtatus
 #endif
@@ -43,27 +39,4 @@
 #define Calcu_LinkQual
 #endif
 
-/****** Simple typedefs  ***************************************************/
-
-/* These lines assume that your compiler's longs are 32 bits and
- * shorts are 16 bits. It is already assumed that chars are 8 bits,
- * but it doesn't matter if they're signed or unsigned.
- */
-
-// QWORD is for those situation that we want
-// an 8-byte-aligned 8 byte long structure
-// which is NOT really a floating point number.
-typedef union tagUQuadWord {
-	struct {
-		unsigned int dwLowDword;
-		unsigned int dwHighDword;
-	} u;
-	double      DoNotUseThisField;
-} UQuadWord;
-typedef UQuadWord       QWORD;          // 64-bit
-
-/****** Common pointer types ***********************************************/
-
-typedef QWORD *PQWORD;
-
 #endif // __TTYPE_H__
diff --git a/drivers/staging/vt6655/upc.h b/drivers/staging/vt6655/upc.h
index e262f1b00e66..c5c889cade25 100644
--- a/drivers/staging/vt6655/upc.h
+++ b/drivers/staging/vt6655/upc.h
@@ -40,61 +40,34 @@
 
 #define VNSvInPortB(dwIOAddress, pbyData)				\
 do {									\
-	*(pbyData) = readb(dwIOAddress);				\
+	*(pbyData) = ioread8(dwIOAddress);				\
 } while (0)
 
 #define VNSvInPortW(dwIOAddress, pwData)				\
 do {									\
-	*(pwData) = readw(dwIOAddress);					\
+	*(pwData) = ioread16(dwIOAddress);				\
 } while (0)
 
 #define VNSvInPortD(dwIOAddress, pdwData)				\
 do {									\
-	*(pdwData) = readl(dwIOAddress);				\
+	*(pdwData) = ioread32(dwIOAddress);				\
 } while (0)
 
 #define VNSvOutPortB(dwIOAddress, byData)				\
 do {									\
-	writeb((unsigned char)byData, dwIOAddress);			\
+	iowrite8((u8)byData, dwIOAddress);				\
 } while (0)
 
 #define VNSvOutPortW(dwIOAddress, wData)				\
 do {									\
-	writew((unsigned short)wData, dwIOAddress);			\
+	iowrite16((u16)wData, dwIOAddress);				\
 } while (0)
 
 #define VNSvOutPortD(dwIOAddress, dwData)				\
 do {									\
-	writel((unsigned long)dwData, dwIOAddress);			\
+	iowrite32((u32)dwData, dwIOAddress);				\
 } while (0)
 
-//
-// ALWAYS IO-Mapped IO when in 16-bit/32-bit environment
-//
-#define PCBvInPortB(dwIOAddress, pbyData)	\
-do {						\
-	*(pbyData) = inb(dwIOAddress);		\
-} while (0)
-
-#define PCBvInPortW(dwIOAddress, pwData)	\
-do {						\
-	*(pwData) = inw(dwIOAddress);		\
-} while (0)
-
-#define PCBvInPortD(dwIOAddress, pdwData)	\
-do {						\
-	*(pdwData) = inl(dwIOAddress);		\
-} while (0)
-
-#define PCBvOutPortB(dwIOAddress, byData)	\
-	outb(byData, dwIOAddress)
-
-#define PCBvOutPortW(dwIOAddress, wData)	\
-	outw(wData, dwIOAddress)
-
-#define PCBvOutPortD(dwIOAddress, dwData)	\
-	outl(dwData, dwIOAddress)
-
 #define PCAvDelayByIO(uDelayUnit)				\
 do {								\
 	unsigned char byData;					\
diff --git a/drivers/staging/vt6655/vntwifi.c b/drivers/staging/vt6655/vntwifi.c
index 4d425e0c76d4..59f66fe47352 100644
--- a/drivers/staging/vt6655/vntwifi.c
+++ b/drivers/staging/vt6655/vntwifi.c
@@ -543,10 +543,6 @@ VNTWIFIvGetTxRate(
 	} else { /* Infrastructure: rate decided from AP Node, index = 0 */
 
 		wTxDataRate = (pMgmt->sNodeDBTable[0].wTxDataRate);
-#ifdef	PLICE_DEBUG
-		pr_debug("GetTxRate:AP MAC is %pM,TxRate is %d\n",
-		       pMgmt->sNodeDBTable[0].abyMACAddr, wTxDataRate);
-#endif
 
 		pSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrSuppRates;
 		pExtSupportRateIEs = (PWLAN_IE_SUPP_RATES) pMgmt->abyCurrExtSuppRates;
diff --git a/drivers/staging/vt6655/wcmd.c b/drivers/staging/vt6655/wcmd.c
index f12eef064c45..985e1b99362d 100644
--- a/drivers/staging/vt6655/wcmd.c
+++ b/drivers/staging/vt6655/wcmd.c
@@ -58,20 +58,18 @@
 
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Functions  --------------------------*/
 
 static
 void
 s_vProbeChannel(
-	PSDevice pDevice
+	struct vnt_private *pDevice
 );
 
 static
 PSTxMgmtPacket
 s_MgrMakeProbeRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned char *pScanBSSID,
 	PWLAN_IE_SSID pSSID,
@@ -82,7 +80,7 @@ s_MgrMakeProbeRequest(
 static
 bool
 s_bCommandComplete(
-	PSDevice pDevice
+	struct vnt_private *pDevice
 );
 
 /*---------------------  Export Variables  --------------------------*/
@@ -104,7 +102,7 @@ s_bCommandComplete(
  */
 static
 void
-vAdHocBeaconStop(PSDevice  pDevice)
+vAdHocBeaconStop(struct vnt_private *pDevice)
 {
 	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
 	bool bStop;
@@ -151,7 +149,7 @@ vAdHocBeaconStop(PSDevice  pDevice)
  */
 static
 void
-vAdHocBeaconRestart(PSDevice pDevice)
+vAdHocBeaconRestart(struct vnt_private *pDevice)
 {
 	PSMgmtObject    pMgmt = &(pDevice->sMgmtObj);
 
@@ -181,7 +179,7 @@ vAdHocBeaconRestart(PSDevice pDevice)
 static
 void
 s_vProbeChannel(
-	PSDevice pDevice
+	struct vnt_private *pDevice
 )
 {
 	//1M,   2M,   5M,   11M,  18M,  24M,  36M,  54M
@@ -216,9 +214,9 @@ s_vProbeChannel(
 	if (pTxPacket != NULL) {
 		for (ii = 0; ii < 2; ii++) {
 			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request sending fail..\n");
+				pr_debug("Probe request sending fail..\n");
 			else
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe request is sending..\n");
+				pr_debug("Probe request is sending..\n");
 		}
 	}
 }
@@ -236,7 +234,7 @@ s_vProbeChannel(
 
 static PSTxMgmtPacket
 s_MgrMakeProbeRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned char *pScanBSSID,
 	PWLAN_IE_SSID pSSID,
@@ -287,7 +285,7 @@ vCommandTimerWait(
 	unsigned int MSecond
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 
 	init_timer(&pDevice->sTimerCommand);
 	pDevice->sTimerCommand.data = (unsigned long) pDevice;
@@ -302,7 +300,7 @@ vCommandTimer(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PWLAN_IE_SSID   pItemSSID;
 	PWLAN_IE_SSID   pItemSSIDCurr;
@@ -330,12 +328,12 @@ vCommandTimer(
 
 		if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 			s_bCommandComplete(pDevice);
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_AP);
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_AP);
 			spin_unlock_irq(&pDevice->lock);
 			return;
 		}
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState= WLAN_CMD_SCAN_START\n");
+		pr_debug("eCommandState= WLAN_CMD_SCAN_START\n");
 		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyScanSSID;
 		// wait all Data TD complete
 		if (pDevice->iTDUsed[TYPE_AC0DMA] != 0) {
@@ -355,11 +353,12 @@ vCommandTimer(
 			// Set Baseband's sensitivity back.
 			// Set channel back
 			set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
+			pr_debug("Scanning, set back to channel: [%d]\n",
+				 pMgmt->uCurrChannel);
 			if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
+				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_ADHOC);
 			else
-				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
+				CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_STATION);
 
 			vAdHocBeaconRestart(pDevice);
 			s_bCommandComplete(pDevice);
@@ -367,7 +366,8 @@ vCommandTimer(
 		} else {
 //2008-8-4 <add> by chester
 			if (!is_channel_valid(pMgmt->uScanChannel)) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Invalid channel pMgmt->uScanChannel = %d\n", pMgmt->uScanChannel);
+				pr_debug("Invalid channel pMgmt->uScanChannel = %d\n",
+					 pMgmt->uScanChannel);
 				s_bCommandComplete(pDevice);
 				spin_unlock_irq(&pDevice->lock);
 				return;
@@ -387,11 +387,13 @@ vCommandTimer(
 			vAdHocBeaconStop(pDevice);
 
 			if (set_channel(pMgmt->pAdapter, pMgmt->uScanChannel))
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SCAN Channel: %d\n", pMgmt->uScanChannel);
+				pr_debug("SCAN Channel: %d\n",
+					 pMgmt->uScanChannel);
 			else
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SET SCAN Channel Fail: %d\n", pMgmt->uScanChannel);
+				pr_debug("SET SCAN Channel Fail: %d\n",
+					 pMgmt->uScanChannel);
 
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_UNKNOWN);
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_UNSPECIFIED);
 			pMgmt->uScanChannel++;
 //2008-8-4 <modify> by chester
 			if (!is_channel_valid(pMgmt->uScanChannel) &&
@@ -422,11 +424,12 @@ vCommandTimer(
 		// Set Baseband's sensitivity back.
 		// Set channel back
 		set_channel(pMgmt->pAdapter, pMgmt->uCurrChannel);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Scanning, set back to channel: [%d]\n", pMgmt->uCurrChannel);
+		pr_debug("Scanning, set back to channel: [%d]\n",
+			 pMgmt->uCurrChannel);
 		if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA)
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_ADHOC);
 		else
-			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_INFRASTRUCTURE);
+			CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_STATION);
 
 		pMgmt->eScanState = WMAC_NO_SCANNING;
 		vAdHocBeaconRestart(pDevice);
@@ -451,7 +454,7 @@ vCommandTimer(
 			spin_unlock_irq(&pDevice->lock);
 			return;
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Send Disassociation Packet..\n");
+			pr_debug("Send Disassociation Packet..\n");
 			// reason = 8 : disassoc because sta has left
 			vMgrDisassocBeginSta((void *)pDevice, pMgmt, pMgmt->abyCurrBSSID, (8), &Status);
 			pDevice->bLinkPass = false;
@@ -470,7 +473,7 @@ vCommandTimer(
 			spin_unlock_irq(&pDevice->lock);
 			return;
 		}
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " CARDbRadioPowerOff\n");
+		pr_debug(" CARDbRadioPowerOff\n");
 		//2008-09-02  <mark>	by chester
 		s_bCommandComplete(pDevice);
 		break;
@@ -496,15 +499,16 @@ vCommandTimer(
 		pr_debug("chester-abyDesireSSID=%s\n", ((PWLAN_IE_SSID)pMgmt->abyDesireSSID)->abySSID);
 		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
 		pItemSSIDCurr = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " cmd: desire ssid = %s\n", pItemSSID->abySSID);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " cmd: curr ssid = %s\n", pItemSSIDCurr->abySSID);
+		pr_debug(" cmd: desire ssid = %s\n", pItemSSID->abySSID);
+		pr_debug(" cmd: curr ssid = %s\n", pItemSSIDCurr->abySSID);
 
 		if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Cmd pMgmt->eCurrState == WMAC_STATE_ASSOC\n");
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pItemSSID->len =%d\n", pItemSSID->len);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " pItemSSIDCurr->len = %d\n", pItemSSIDCurr->len);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " desire ssid = %s\n", pItemSSID->abySSID);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " curr ssid = %s\n", pItemSSIDCurr->abySSID);
+			pr_debug(" Cmd pMgmt->eCurrState == WMAC_STATE_ASSOC\n");
+			pr_debug(" pItemSSID->len =%d\n", pItemSSID->len);
+			pr_debug(" pItemSSIDCurr->len = %d\n",
+				 pItemSSIDCurr->len);
+			pr_debug(" desire ssid = %s\n", pItemSSID->abySSID);
+			pr_debug(" curr ssid = %s\n", pItemSSIDCurr->abySSID);
 		}
 
 		if ((pMgmt->eCurrState == WMAC_STATE_ASSOC) ||
@@ -541,7 +545,7 @@ vCommandTimer(
 				pDevice->eCommandState = WLAN_AUTHENTICATE_WAIT;
 				vCommandTimerWait((void *)pDevice, AUTHENTICATE_TIMEOUT);
 				spin_unlock_irq(&pDevice->lock);
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " Set eCommandState = WLAN_AUTHENTICATE_WAIT\n");
+				pr_debug(" Set eCommandState = WLAN_AUTHENTICATE_WAIT\n");
 				return;
 			}
 		}
@@ -560,7 +564,7 @@ vCommandTimer(
 				// start own IBSS
 				vMgrCreateOwnIBSS((void *)pDevice, &Status);
 				if (Status != CMD_STATUS_SUCCESS)
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail !\n");
+					pr_debug(" WLAN_CMD_IBSS_CREATE fail !\n");
 
 				BSSvAddMulticastNode(pDevice);
 			}
@@ -572,7 +576,7 @@ vCommandTimer(
 				// start own IBSS
 				vMgrCreateOwnIBSS((void *)pDevice, &Status);
 				if (Status != CMD_STATUS_SUCCESS)
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " WLAN_CMD_IBSS_CREATE fail !\n");
+					pr_debug(" WLAN_CMD_IBSS_CREATE fail !\n");
 
 				BSSvAddMulticastNode(pDevice);
 				if (netif_queue_stopped(pDevice->dev))
@@ -580,7 +584,7 @@ vCommandTimer(
 
 				pDevice->bLinkPass = true;
 			} else {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Disconnect SSID none\n");
+				pr_debug("Disconnect SSID none\n");
 #ifdef WPA_SUPPLICANT_DRIVER_WEXT_SUPPORT
 				{
 					union iwreq_data  wrqu;
@@ -598,15 +602,15 @@ vCommandTimer(
 		break;
 
 	case WLAN_AUTHENTICATE_WAIT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_AUTHENTICATE_WAIT\n");
+		pr_debug("eCommandState == WLAN_AUTHENTICATE_WAIT\n");
 		if (pMgmt->eCurrState == WMAC_STATE_AUTH) {
 			// Call mgr to begin the association
 			pDevice->byLinkWaitCount = 0;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCurrState == WMAC_STATE_AUTH\n");
+			pr_debug("eCurrState == WMAC_STATE_AUTH\n");
 			vMgrAssocBeginSta((void *)pDevice, pMgmt, &Status);
 			if (Status == CMD_STATUS_SUCCESS) {
 				pDevice->byLinkWaitCount = 0;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState = WLAN_ASSOCIATE_WAIT\n");
+				pr_debug("eCommandState = WLAN_ASSOCIATE_WAIT\n");
 				pDevice->eCommandState = WLAN_ASSOCIATE_WAIT;
 				vCommandTimerWait((void *)pDevice, ASSOCIATE_TIMEOUT);
 				spin_unlock_irq(&pDevice->lock);
@@ -629,7 +633,7 @@ vCommandTimer(
 
 	case WLAN_ASSOCIATE_WAIT:
 		if (pMgmt->eCurrState == WMAC_STATE_ASSOC) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCurrState == WMAC_STATE_ASSOC\n");
+			pr_debug("eCurrState == WMAC_STATE_ASSOC\n");
 			if (pDevice->ePSMode != WMAC_POWER_CAM)
 				PSvEnablePowerSaving((void *)pDevice, pMgmt->wListenInterval);
 
@@ -647,7 +651,6 @@ vCommandTimer(
 			if (netif_queue_stopped(pDevice->dev))
 				netif_wake_queue(pDevice->dev);
 
-#ifdef TxInSleep
 			if (pDevice->IsTxDataTrigger) {    //TxDataTimer is not triggered at the first time
 				del_timer(&pDevice->sTimerTxData);
 				init_timer(&pDevice->sTimerTxData);
@@ -660,7 +663,7 @@ vCommandTimer(
 
 			pDevice->IsTxDataTrigger = true;
 			add_timer(&pDevice->sTimerTxData);
-#endif
+
 		} else if (pMgmt->eCurrState < WMAC_STATE_ASSOCPENDING) {
 			printk("WLAN_ASSOCIATE_WAIT:Association Fail???\n");
 		} else if (pDevice->byLinkWaitCount <= 4) {    //mike add:wait another 2 sec if associated_frame delay!
@@ -676,7 +679,7 @@ vCommandTimer(
 		break;
 
 	case WLAN_CMD_AP_MODE_START:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_CMD_AP_MODE_START\n");
+		pr_debug("eCommandState == WLAN_CMD_AP_MODE_START\n");
 
 		if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
 			del_timer(&pMgmt->sTimerSecondCallback);
@@ -693,12 +696,12 @@ vCommandTimer(
 
 			vMgrCreateOwnIBSS((void *)pDevice, &Status);
 			if (Status != CMD_STATUS_SUCCESS)
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " vMgrCreateOwnIBSS fail !\n");
+				pr_debug(" vMgrCreateOwnIBSS fail !\n");
 
 			// alway turn off unicast bit
 			MACvRegBitsOff(pDevice->PortOffset, MAC_REG_RCR, RCR_UNICAST);
 			pDevice->byRxMode &= ~RCR_UNICAST;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wcmd: rx_mode = %x\n", pDevice->byRxMode);
+			pr_debug("wcmd: rx_mode = %x\n", pDevice->byRxMode);
 			BSSvAddMulticastNode(pDevice);
 			if (netif_queue_stopped(pDevice->dev))
 				netif_wake_queue(pDevice->dev);
@@ -720,7 +723,7 @@ vCommandTimer(
 					pDevice->bMoreData = true;
 				}
 				if (!device_dma0_xmit(pDevice, skb, 0))
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Multicast ps tx fail\n");
+					pr_debug("Multicast ps tx fail\n");
 
 				pMgmt->sNodeDBTable[0].wEnQueueCnt--;
 			}
@@ -730,8 +733,9 @@ vCommandTimer(
 		for (ii = 1; ii < (MAX_NODE_NUM + 1); ii++) {
 			if (pMgmt->sNodeDBTable[ii].bActive &&
 			    pMgmt->sNodeDBTable[ii].bRxPSPoll) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index=%d Enqueu Cnt= %d\n",
-					ii, pMgmt->sNodeDBTable[ii].wEnQueueCnt);
+				pr_debug("Index=%d Enqueu Cnt= %d\n",
+					 ii,
+					 pMgmt->sNodeDBTable[ii].wEnQueueCnt);
 				while ((skb = skb_dequeue(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) != NULL) {
 					if (skb_queue_empty(&pMgmt->sNodeDBTable[ii].sTxPSQueue)) {
 						// clear tx map
@@ -742,7 +746,7 @@ vCommandTimer(
 						pDevice->bMoreData = true;
 					}
 					if (!device_dma0_xmit(pDevice, skb, ii))
-						DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "sta ps tx fail\n");
+						pr_debug("sta ps tx fail\n");
 
 					pMgmt->sNodeDBTable[ii].wEnQueueCnt--;
 					// check if sta ps enabled, and wait next pspoll.
@@ -754,7 +758,8 @@ vCommandTimer(
 					// clear tx map
 					pMgmt->abyPSTxMap[pMgmt->sNodeDBTable[ii].wAID >> 3] &=
 						~byMask[pMgmt->sNodeDBTable[ii].wAID & 7];
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Index=%d PS queue clear\n", ii);
+					pr_debug("Index=%d PS queue clear\n",
+						 ii);
 				}
 				pMgmt->sNodeDBTable[ii].bRxPSPoll = false;
 			}
@@ -764,7 +769,7 @@ vCommandTimer(
 		break;
 
 	case WLAN_CMD_RADIO_START:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState == WLAN_CMD_RADIO_START\n");
+		pr_debug("eCommandState == WLAN_CMD_RADIO_START\n");
 		if (pDevice->bRadioCmd)
 			CARDbRadioPowerOn(pDevice);
 		else
@@ -787,7 +792,7 @@ vCommandTimer(
 		}
 		pDevice->byBBVGACurrent = pDevice->byBBVGANew;
 		BBvSetVGAGainOffset(pDevice, pDevice->byBBVGACurrent);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "SetVGAGainOffset %02X\n", pDevice->byBBVGACurrent);
+		pr_debug("SetVGAGainOffset %02X\n", pDevice->byBBVGACurrent);
 		s_bCommandComplete(pDevice);
 		break;
 
@@ -802,7 +807,7 @@ vCommandTimer(
 static
 bool
 s_bCommandComplete(
-	PSDevice pDevice
+	struct vnt_private *pDevice
 )
 {
 	PWLAN_IE_SSID pSSID;
@@ -825,7 +830,7 @@ s_bCommandComplete(
 		pDevice->bCmdRunning = true;
 		switch (pDevice->eCommand) {
 		case WLAN_CMD_BSSID_SCAN:
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState= WLAN_CMD_BSSID_SCAN\n");
+			pr_debug("eCommandState= WLAN_CMD_BSSID_SCAN\n");
 			pDevice->eCommandState = WLAN_CMD_SCAN_START;
 			pMgmt->uScanChannel = 0;
 			if (pSSID->len != 0)
@@ -840,7 +845,7 @@ s_bCommandComplete(
 				pSSID->len = WLAN_SSID_MAXLEN;
 			if (pSSID->len != 0)
 				memcpy(pDevice->pMgmt->abyDesireSSID, pSSID, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "eCommandState= WLAN_CMD_SSID_START\n");
+			pr_debug("eCommandState= WLAN_CMD_SSID_START\n");
 			break;
 		case WLAN_CMD_DISASSOCIATE:
 			pDevice->eCommandState = WLAN_CMD_DISASSOCIATE_START;
@@ -876,7 +881,7 @@ bool bScheduleCommand(
 	unsigned char *pbyItem0
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 
 	if (pDevice->cbFreeCmdQueue == 0)
 		return false;
@@ -945,7 +950,7 @@ bool bClearBSSID_SCAN(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	unsigned int uCmdDequeueIdx = pDevice->uCmdDequeueIdx;
 	unsigned int ii;
 
@@ -967,7 +972,7 @@ vResetCommandTimer(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 
 	//delete timer
 	del_timer(&pDevice->sTimerCommand);
@@ -984,13 +989,12 @@ vResetCommandTimer(
 	pDevice->bCmdClear = false;
 }
 
-#ifdef TxInSleep
 void
 BSSvSecondTxData(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject  pMgmt = &(pDevice->sMgmtObj);
 
 	pDevice->nTxDataTimeCout++;
@@ -1003,20 +1007,17 @@ BSSvSecondTxData(
 	}
 
 	spin_lock_irq(&pDevice->lock);
-#if 1
-	if ((pDevice->bLinkPass && (pMgmt->eAuthenMode < WMAC_AUTH_WPA)) ||  //open && sharekey linking
-	    pDevice->fWPA_Authened) {   //wpa linking
-#else
-		if (pDevice->bLinkPass == true) {
-#endif
-			pDevice->fTxDataInSleep = true;
-			PSbSendNullPacket(pDevice);      //send null packet
-			pDevice->fTxDataInSleep = false;
-		}
-		spin_unlock_irq(&pDevice->lock);
 
-		pDevice->sTimerTxData.expires = RUN_AT(10*HZ);      //10s callback
-		add_timer(&pDevice->sTimerTxData);
-		return;
+	/* open && sharekey linking */
+	if ((pDevice->bLinkPass && (pMgmt->eAuthenMode < WMAC_AUTH_WPA)) ||
+	    pDevice->fWPA_Authened) {   /* wpa linking */
+		pDevice->fTxDataInSleep = true;
+		PSbSendNullPacket(pDevice);	/* send null packet */
+		pDevice->fTxDataInSleep = false;
 	}
-#endif
+
+	spin_unlock_irq(&pDevice->lock);
+
+	pDevice->sTimerTxData.expires = RUN_AT(10*HZ); /* 10s callback */
+	add_timer(&pDevice->sTimerTxData);
+}
diff --git a/drivers/staging/vt6655/wcmd.h b/drivers/staging/vt6655/wcmd.h
index 126b61c48791..6ef04de69f37 100644
--- a/drivers/staging/vt6655/wcmd.h
+++ b/drivers/staging/vt6655/wcmd.h
@@ -114,11 +114,10 @@ vCommandTimerWait(
 	void *hDeviceContext,
 	unsigned int MSecond
 );
-#ifdef TxInSleep
+
 void
 BSSvSecondTxData(
 	void *hDeviceContext
 );
-#endif
 
 #endif //__WCMD_H__
diff --git a/drivers/staging/vt6655/wctl.c b/drivers/staging/vt6655/wctl.c
index fddea9fc9cfe..5a54d98d985a 100644
--- a/drivers/staging/vt6655/wctl.c
+++ b/drivers/staging/vt6655/wctl.c
@@ -105,7 +105,8 @@ bool WCTLbIsDuplicate(PSCache pCache, PS802_11Header pMACHeader)
  * Return Value: index number in Defragment Database
  *
  */
-unsigned int WCTLuSearchDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
+unsigned int WCTLuSearchDFCB(struct vnt_private *pDevice,
+			     PS802_11Header pMACHeader)
 {
 	unsigned int ii;
 
@@ -133,7 +134,7 @@ unsigned int WCTLuSearchDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
  * Return Value: index number in Defragment Database
  *
  */
-unsigned int WCTLuInsertDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
+unsigned int WCTLuInsertDFCB(struct vnt_private *pDevice, PS802_11Header pMACHeader)
 {
 	unsigned int ii;
 
@@ -169,7 +170,8 @@ unsigned int WCTLuInsertDFCB(PSDevice pDevice, PS802_11Header pMACHeader)
  * Return Value: true if it is valid fragment packet and we have resource to defragment; otherwise false
  *
  */
-bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader, unsigned int cbFrameLength, bool bWEP, bool bExtIV)
+bool WCTLbHandleFragment(struct vnt_private *pDevice, PS802_11Header pMACHeader,
+			 unsigned int cbFrameLength, bool bWEP, bool bExtIV)
 {
 	unsigned int uHeaderSize;
 
diff --git a/drivers/staging/vt6655/wctl.h b/drivers/staging/vt6655/wctl.h
index 1ffb2734cfae..f0995d86f71f 100644
--- a/drivers/staging/vt6655/wctl.h
+++ b/drivers/staging/vt6655/wctl.h
@@ -97,9 +97,9 @@ do {							\
 /*---------------------  Export Functions  --------------------------*/
 
 bool WCTLbIsDuplicate(PSCache pCache, PS802_11Header pMACHeader);
-bool WCTLbHandleFragment(PSDevice pDevice, PS802_11Header pMACHeader,
+bool WCTLbHandleFragment(struct vnt_private *, PS802_11Header pMACHeader,
 			 unsigned int cbFrameLength, bool bWEP, bool bExtIV);
-unsigned int WCTLuSearchDFCB(PSDevice pDevice, PS802_11Header pMACHeader);
-unsigned int WCTLuInsertDFCB(PSDevice pDevice, PS802_11Header pMACHeader);
+unsigned int WCTLuSearchDFCB(struct vnt_private *, PS802_11Header pMACHeader);
+unsigned int WCTLuInsertDFCB(struct vnt_private *, PS802_11Header pMACHeader);
 
 #endif // __WCTL_H__
diff --git a/drivers/staging/vt6655/wmgr.c b/drivers/staging/vt6655/wmgr.c
index e88e11606db0..c73c39d7adfd 100644
--- a/drivers/staging/vt6655/wmgr.c
+++ b/drivers/staging/vt6655/wmgr.c
@@ -80,19 +80,14 @@
 #include "rf.h"
 #include "iowpa.h"
 
-#define	PLICE_DEBUG
-
 /*---------------------  Static Definitions -------------------------*/
 
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
-
 /*---------------------  Static Functions  --------------------------*/
 //2008-8-4 <add> by chester
 static bool ChannelExceedZoneType(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byCurrChannel
 );
 
@@ -100,7 +95,7 @@ static bool ChannelExceedZoneType(
 static
 PSTxMgmtPacket
 s_MgrMakeAssocRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned char *pDAddr,
 	unsigned short wCurrCapInfo,
@@ -113,7 +108,7 @@ s_MgrMakeAssocRequest(
 static
 void
 s_vMgrRxAssocRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket,
 	unsigned int uNodeIndex
@@ -122,7 +117,7 @@ s_vMgrRxAssocRequest(
 static
 PSTxMgmtPacket
 s_MgrMakeReAssocRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned char *pDAddr,
 	unsigned short wCurrCapInfo,
@@ -135,7 +130,7 @@ s_MgrMakeReAssocRequest(
 static
 void
 s_vMgrRxAssocResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket,
 	bool bReAssocType
@@ -144,7 +139,7 @@ s_vMgrRxAssocResponse(
 static
 void
 s_vMgrRxDisassociation(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 );
@@ -153,7 +148,7 @@ s_vMgrRxDisassociation(
 static
 void
 s_vMgrRxAuthenSequence_1(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 );
@@ -161,7 +156,7 @@ s_vMgrRxAuthenSequence_1(
 static
 void
 s_vMgrRxAuthenSequence_2(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 );
@@ -169,7 +164,7 @@ s_vMgrRxAuthenSequence_2(
 static
 void
 s_vMgrRxAuthenSequence_3(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 );
@@ -177,7 +172,7 @@ s_vMgrRxAuthenSequence_3(
 static
 void
 s_vMgrRxAuthenSequence_4(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 );
@@ -185,7 +180,7 @@ s_vMgrRxAuthenSequence_4(
 static
 void
 s_vMgrRxAuthentication(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 );
@@ -193,7 +188,7 @@ s_vMgrRxAuthentication(
 static
 void
 s_vMgrRxDeauthentication(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 );
@@ -203,7 +198,7 @@ s_vMgrRxDeauthentication(
 static
 void
 s_vMgrRxProbeRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 );
@@ -211,7 +206,7 @@ s_vMgrRxProbeRequest(
 static
 void
 s_vMgrRxProbeResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 );
@@ -220,7 +215,7 @@ s_vMgrRxProbeResponse(
 static
 void
 s_vMgrRxBeacon(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket,
 	bool bInScan
@@ -236,7 +231,7 @@ s_vMgrFormatTIM(
 static
 PSTxMgmtPacket
 s_MgrMakeBeacon(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wCurrBeaconPeriod,
@@ -252,7 +247,7 @@ s_MgrMakeBeacon(
 static
 PSTxMgmtPacket
 s_MgrMakeAssocResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wAssocStatus,
@@ -266,7 +261,7 @@ s_MgrMakeAssocResponse(
 static
 PSTxMgmtPacket
 s_MgrMakeReAssocResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wAssocStatus,
@@ -280,7 +275,7 @@ s_MgrMakeReAssocResponse(
 static
 PSTxMgmtPacket
 s_MgrMakeProbeResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wCurrBeaconPeriod,
@@ -305,7 +300,7 @@ s_vMgrLogStatus(
 static
 void
 s_vMgrSynchBSS(
-	PSDevice      pDevice,
+	struct vnt_private *pDevice,
 	unsigned int uBSSMode,
 	PKnownBSS     pCurr,
 	PCMD_STATUS  pStatus
@@ -320,7 +315,7 @@ s_bCipherMatch(
 );
 
 static void  Encyption_Rebuild(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PKnownBSS pCurr
 );
 
@@ -343,7 +338,7 @@ vMgrObjectInit(
 	void *hDeviceContext
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	int ii;
 
@@ -375,7 +370,7 @@ vMgrTimerInit(
 	void *hDeviceContext
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 
 	init_timer(&pMgmt->sTimerSecondCallback);
@@ -388,7 +383,6 @@ vMgrTimerInit(
 	pDevice->sTimerCommand.function = (TimerFunction)vCommandTimer;
 	pDevice->sTimerCommand.expires = RUN_AT(HZ);
 
-#ifdef TxInSleep
 	init_timer(&pDevice->sTimerTxData);
 	pDevice->sTimerTxData.data = (unsigned long) pDevice;
 	pDevice->sTimerTxData.function = (TimerFunction)BSSvSecondTxData;
@@ -396,7 +390,6 @@ vMgrTimerInit(
 	pDevice->fTxDataInSleep = false;
 	pDevice->IsTxDataTrigger = false;
 	pDevice->nTxDataTimeCout = 0;
-#endif
 
 	pDevice->cbFreeCmdQueue = CMD_Q_SIZE;
 	pDevice->uCmdDequeueIdx = 0;
@@ -418,7 +411,7 @@ vMgrObjectReset(
 	void *hDeviceContext
 )
 {
-	PSDevice         pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 
 	pMgmt->eCurrMode = WMAC_MODE_STANDBY;
@@ -445,7 +438,7 @@ vMgrAssocBeginSta(
 	PCMD_STATUS pStatus
 )
 {
-	PSDevice             pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSTxMgmtPacket          pTxPacket;
 
 	pMgmt->wCurrCapInfo = 0;
@@ -511,7 +504,7 @@ vMgrReAssocBeginSta(
 	PCMD_STATUS pStatus
 )
 {
-	PSDevice             pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSTxMgmtPacket          pTxPacket;
 
 	pMgmt->wCurrCapInfo = 0;
@@ -553,9 +546,9 @@ vMgrReAssocBeginSta(
 		/* send the frame */
 		*pStatus = csMgmt_xmit(pDevice, pTxPacket);
 		if (*pStatus != CMD_STATUS_PENDING)
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx failed.\n");
+			pr_debug("Mgt:Reassociation tx failed\n");
 		else
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Reassociation tx sending.\n");
+			pr_debug("Mgt:Reassociation tx sending\n");
 	}
 }
 
@@ -578,7 +571,7 @@ vMgrDisassocBeginSta(
 	PCMD_STATUS pStatus
 )
 {
-	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSTxMgmtPacket      pTxPacket = NULL;
 	WLAN_FR_DISASSOC    sFrame;
 
@@ -630,7 +623,7 @@ vMgrDisassocBeginSta(
 static
 void
 s_vMgrRxAssocRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket,
 	unsigned int uNodeIndex
@@ -697,9 +690,9 @@ s_vMgrRxAssocRequest(
 		// set max tx rate
 		pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
 			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-#ifdef	PLICE_DEBUG
+
 		pr_debug("RxAssocRequest:wTxDataRate is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
-#endif
+
 		// Todo: check sta preamble, if ap can't support, set status code
 		pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
 			WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
@@ -720,8 +713,8 @@ s_vMgrRxAssocRequest(
 		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 			pDevice->bBarkerPreambleMd = true;
 
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Associate AID= %d\n", wAssocAID);
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
+		pr_info("Associate AID= %d\n", wAssocAID);
+		pr_info("MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
 			sFrame.pHdr->sA3.abyAddr2[0],
 			sFrame.pHdr->sA3.abyAddr2[1],
 			sFrame.pHdr->sA3.abyAddr2[2],
@@ -729,7 +722,7 @@ s_vMgrRxAssocRequest(
 			sFrame.pHdr->sA3.abyAddr2[4],
 			sFrame.pHdr->sA3.abyAddr2[5]
 			);
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d\n",
+		pr_info("Max Support rate = %d\n",
 			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
 	} else {
 		/* TODO: received STA under state1 handle */
@@ -755,9 +748,9 @@ s_vMgrRxAssocRequest(
 		/* send the frame */
 		Status = csMgmt_xmit(pDevice, pTxPacket);
 		if (Status != CMD_STATUS_PENDING)
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx failed\n");
+			pr_debug("Mgt:Assoc response tx failed\n");
 		else
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Assoc response tx sending..\n");
+			pr_debug("Mgt:Assoc response tx sending..\n");
 	}
 }
 
@@ -780,7 +773,7 @@ s_vMgrRxAssocRequest(
 static
 void
 s_vMgrRxReAssocRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket,
 	unsigned int uNodeIndex
@@ -845,9 +838,9 @@ s_vMgrRxReAssocRequest(
 		// set max tx rate
 		pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate =
 			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-#ifdef	PLICE_DEBUG
+
 		pr_debug("RxReAssocRequest:TxDataRate is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate);
-#endif
+
 		// Todo: check sta preamble, if ap can't support, set status code
 		pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble =
 			WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
@@ -869,8 +862,8 @@ s_vMgrRxReAssocRequest(
 		if (!pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble)
 			pDevice->bBarkerPreambleMd = true;
 
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Rx ReAssociate AID= %d\n", wAssocAID);
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
+		pr_info("Rx ReAssociate AID= %d\n", wAssocAID);
+		pr_info("MAC=%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X\n",
 			sFrame.pHdr->sA3.abyAddr2[0],
 			sFrame.pHdr->sA3.abyAddr2[1],
 			sFrame.pHdr->sA3.abyAddr2[2],
@@ -878,7 +871,7 @@ s_vMgrRxReAssocRequest(
 			sFrame.pHdr->sA3.abyAddr2[4],
 			sFrame.pHdr->sA3.abyAddr2[5]
 			);
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Max Support rate = %d\n",
+		pr_info("Max Support rate = %d\n",
 			pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate);
 
 	}
@@ -903,9 +896,9 @@ s_vMgrRxReAssocRequest(
 
 		Status = csMgmt_xmit(pDevice, pTxPacket);
 		if (Status != CMD_STATUS_PENDING)
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx failed\n");
+			pr_debug("Mgt:ReAssoc response tx failed\n");
 		else
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:ReAssoc response tx sending..\n");
+			pr_debug("Mgt:ReAssoc response tx sending..\n");
 	}
 }
 
@@ -922,7 +915,7 @@ s_vMgrRxReAssocRequest(
 static
 void
 s_vMgrRxAssocResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket,
 	bool bReAssocType
@@ -963,13 +956,14 @@ s_vMgrRxAssocResponse(
 			// set AID
 			pMgmt->wCurrAID = cpu_to_le16((*(sFrame.pwAid)));
 			if ((pMgmt->wCurrAID >> 14) != (BIT0 | BIT1))
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "AID from AP, has two msb clear.\n");
+				pr_debug("AID from AP, has two msb clear\n");
 
-			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Association Successful, AID=%d.\n", pMgmt->wCurrAID & ~(BIT14 | BIT15));
+			pr_info("Association Successful, AID=%d\n",
+				pMgmt->wCurrAID & ~(BIT14 | BIT15));
 			pMgmt->eCurrState = WMAC_STATE_ASSOC;
 			BSSvUpdateAPNode((void *)pDevice, sFrame.pwCapInfo, sFrame.pSuppRates, sFrame.pExtSuppRates);
 			pItemSSID = (PWLAN_IE_SSID)pMgmt->abyCurrSSID;
-			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Link with AP(SSID): %s\n", pItemSSID->abySSID);
+			pr_info("Link with AP(SSID): %s\n", pItemSSID->abySSID);
 			pDevice->bLinkPass = true;
 			pDevice->uBBVGADiffCount = 0;
 			if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
@@ -1075,7 +1069,7 @@ vMgrAuthenBeginSta(
 	PCMD_STATUS pStatus
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	WLAN_FR_AUTHEN  sFrame;
 	PSTxMgmtPacket  pTxPacket = NULL;
 
@@ -1131,7 +1125,7 @@ vMgrDeAuthenBeginSta(
 	PCMD_STATUS pStatus
 )
 {
-	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	WLAN_FR_DEAUTHEN    sFrame;
 	PSTxMgmtPacket      pTxPacket = NULL;
 
@@ -1175,7 +1169,7 @@ vMgrDeAuthenBeginSta(
 static
 void
 s_vMgrRxAuthentication(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 )
@@ -1208,8 +1202,8 @@ s_vMgrRxAuthentication(
 		s_vMgrRxAuthenSequence_4(pDevice, pMgmt, &sFrame);
 		break;
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Auth Sequence error, seq = %d\n",
-			cpu_to_le16((*(sFrame.pwAuthSequence))));
+		pr_debug("Auth Sequence error, seq = %d\n",
+			 cpu_to_le16((*(sFrame.pwAuthSequence))));
 		break;
 	}
 }
@@ -1229,7 +1223,7 @@ s_vMgrRxAuthentication(
 static
 void
 s_vMgrRxAuthenSequence_1(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 )
@@ -1241,7 +1235,7 @@ s_vMgrRxAuthenSequence_1(
 
 	// Insert a Node entry
 	if (!BSSDBbIsSTAInNodeDB(pMgmt, pFrame->pHdr->sA3.abyAddr2, &uNodeIndex)) {
-		BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
+		BSSvCreateOneNode(pDevice, &uNodeIndex);
 		memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, pFrame->pHdr->sA3.abyAddr2,
 		       WLAN_ADDR_LEN);
 	}
@@ -1308,9 +1302,9 @@ s_vMgrRxAuthenSequence_1(
 	if (pDevice->bEnableHostapd)
 		return;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx..\n");
+	pr_debug("Mgt:Authreq_reply sequence_1 tx..\n");
 	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_1 tx failed.\n");
+		pr_debug("Mgt:Authreq_reply sequence_1 tx failed\n");
 }
 
 /*+
@@ -1328,7 +1322,7 @@ s_vMgrRxAuthenSequence_1(
 static
 void
 s_vMgrRxAuthenSequence_2(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 )
@@ -1339,11 +1333,11 @@ s_vMgrRxAuthenSequence_2(
 	switch (cpu_to_le16((*(pFrame->pwAuthAlgorithm)))) {
 	case WLAN_AUTH_ALG_OPENSYSTEM:
 		if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
-			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (OPEN) Successful.\n");
+			pr_info("802.11 Authen (OPEN) Successful\n");
 			pMgmt->eCurrState = WMAC_STATE_AUTH;
 			timer_expire(pDevice->sTimerCommand, 0);
 		} else {
-			DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (OPEN) Failed.\n");
+			pr_info("802.11 Authen (OPEN) Failed\n");
 			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 		}
@@ -1383,16 +1377,17 @@ s_vMgrRxAuthenSequence_2(
 			pTxPacket->cbPayloadLen = sFrame.len - WLAN_HDR_ADDR3_LEN;
 			// send the frame
 			if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx failed.\n");
+				pr_debug("Mgt:Auth_reply sequence_2 tx failed\n");
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Auth_reply sequence_2 tx ...\n");
+			pr_debug("Mgt:Auth_reply sequence_2 tx ...\n");
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:rx Auth_reply sequence_2 status error ...\n");
+			pr_debug("Mgt:rx Auth_reply sequence_2 status error ...\n");
 			s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
 		}
 		break;
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt: rx auth.seq = 2 unknown AuthAlgorithm=%d\n", cpu_to_le16((*(pFrame->pwAuthAlgorithm))));
+		pr_debug("Mgt: rx auth.seq = 2 unknown AuthAlgorithm=%d\n",
+			 cpu_to_le16((*(pFrame->pwAuthAlgorithm))));
 		break;
 	}
 }
@@ -1413,7 +1408,7 @@ s_vMgrRxAuthenSequence_2(
 static
 void
 s_vMgrRxAuthenSequence_3(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 )
@@ -1446,7 +1441,7 @@ s_vMgrRxAuthenSequence_3(
 		pMgmt->sNodeDBTable[uNodeIndex].byAuthSequence = 0;
 	}
 	uStatusCode = WLAN_MGMT_STATUS_SUCCESS;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Challenge text check ok..\n");
+	pr_debug("Challenge text check ok..\n");
 
 reply:
 	// send auth reply
@@ -1479,7 +1474,7 @@ reply:
 		return;
 
 	if (csMgmt_xmit(pDevice, pTxPacket) != CMD_STATUS_PENDING)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Authreq_reply sequence_4 tx failed.\n");
+		pr_debug("Mgt:Authreq_reply sequence_4 tx failed\n");
 }
 
 /*+
@@ -1495,17 +1490,17 @@ reply:
 static
 void
 s_vMgrRxAuthenSequence_4(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PWLAN_FR_AUTHEN pFrame
 )
 {
 	if (cpu_to_le16((*(pFrame->pwStatus))) == WLAN_MGMT_STATUS_SUCCESS) {
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (SHAREDKEY) Successful.\n");
+		pr_info("802.11 Authen (SHAREDKEY) Successful\n");
 		pMgmt->eCurrState = WMAC_STATE_AUTH;
 		timer_expire(pDevice->sTimerCommand, 0);
 	} else{
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "802.11 Authen (SHAREDKEY) Failed.\n");
+		pr_info("802.11 Authen (SHAREDKEY) Failed\n");
 		s_vMgrLogStatus(pMgmt, cpu_to_le16((*(pFrame->pwStatus))));
 		pMgmt->eCurrState = WMAC_STATE_IDLE;
 	}
@@ -1525,7 +1520,7 @@ s_vMgrRxAuthenSequence_4(
 static
 void
 s_vMgrRxDisassociation(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 )
@@ -1542,13 +1537,14 @@ s_vMgrRxDisassociation(
 		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
 			BSSvRemoveOneNode(pDevice, uNodeIndex);
 		else
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx disassoc, sta not found\n");
+			pr_debug("Rx disassoc, sta not found\n");
 
 	} else if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
 		sFrame.len = pRxPacket->cbMPDULen;
 		sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
 		vMgrDecodeDisassociation(&sFrame);
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "AP disassociated me, reason=%d.\n", cpu_to_le16(*(sFrame.pwReason)));
+		pr_info("AP disassociated me, reason=%d\n",
+			cpu_to_le16(*(sFrame.pwReason)));
 		//TODO: do something let upper layer know or
 		//try to send associate packet again because of inactivity timeout
 		if ((pDevice->bWPADEVUp) && (pDevice->skb != NULL)) {
@@ -1595,7 +1591,7 @@ s_vMgrRxDisassociation(
 static
 void
 s_vMgrRxDeauthentication(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 )
@@ -1613,13 +1609,14 @@ s_vMgrRxDeauthentication(
 		if (BSSDBbIsSTAInNodeDB(pMgmt, pRxPacket->p80211Header->sA3.abyAddr2, &uNodeIndex))
 			BSSvRemoveOneNode(pDevice, uNodeIndex);
 		else
-			DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Rx deauth, sta not found\n");
+			pr_info("Rx deauth, sta not found\n");
 	} else {
 		if (pMgmt->eCurrMode == WMAC_MODE_ESS_STA) {
 			sFrame.len = pRxPacket->cbMPDULen;
 			sFrame.pBuf = (unsigned char *)pRxPacket->p80211Header;
 			vMgrDecodeDeauthen(&sFrame);
-			DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO  "AP deauthed me, reason=%d.\n", cpu_to_le16((*(sFrame.pwReason))));
+			pr_info("AP deauthed me, reason=%d\n",
+				cpu_to_le16((*(sFrame.pwReason))));
 			// TODO: update BSS list for specific BSSID if pre-authentication case
 			if (ether_addr_equal(sFrame.pHdr->sA3.abyAddr3,
 					     pMgmt->abyCurrBSSID)) {
@@ -1678,7 +1675,7 @@ s_vMgrRxDeauthentication(
  -*/
 static bool
 ChannelExceedZoneType(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	unsigned char byCurrChannel
 )
 {
@@ -1715,7 +1712,7 @@ ChannelExceedZoneType(
 static
 void
 s_vMgrRxBeacon(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket,
 	bool bInScan
@@ -1723,7 +1720,7 @@ s_vMgrRxBeacon(
 {
 	PKnownBSS           pBSSList;
 	WLAN_FR_BEACON      sFrame;
-	QWORD               qwTSFOffset;
+	u64 qwTSFOffset;
 	bool bIsBSSIDEqual = false;
 	bool bIsSSIDEqual = false;
 	bool bTSFLargeDiff = false;
@@ -1735,8 +1732,8 @@ s_vMgrRxBeacon(
 	unsigned char byTIMBitOn = 0;
 	unsigned short wAIDNumber = 0;
 	unsigned int uNodeIndex;
-	QWORD               qwTimestamp, qwLocalTSF;
-	QWORD               qwCurrTSF;
+	u64 qwTimestamp, qwLocalTSF;
+	u64 qwCurrTSF;
 	unsigned short wStartIndex = 0;
 	unsigned short wAIDIndex = 0;
 	unsigned char byCurrChannel = pRxPacket->byRxChannel;
@@ -1757,7 +1754,7 @@ s_vMgrRxBeacon(
 	    (sFrame.pwCapInfo == NULL) ||
 	    (sFrame.pSSID == NULL) ||
 	    (sFrame.pSuppRates == NULL)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx beacon frame error\n");
+		pr_debug("Rx beacon frame error\n");
 		return;
 	}
 
@@ -1792,7 +1789,7 @@ s_vMgrRxBeacon(
 
 	pBSSList = BSSpAddrIsInBSSList((void *)pDevice, sFrame.pHdr->sA3.abyAddr3, sFrame.pSSID);
 	if (pBSSList == NULL) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Beacon/insert: RxChannel = : %d\n", byCurrChannel);
+		pr_debug("Beacon/insert: RxChannel = : %d\n", byCurrChannel);
 		BSSbInsertToBSSList((void *)pDevice,
 				    sFrame.pHdr->sA3.abyAddr3,
 				    *sFrame.pqwTimestamp,
@@ -1966,7 +1963,7 @@ s_vMgrRxBeacon(
 		}
 	}
 
-//    DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Beacon 2\n");
+//    pr_debug("Beacon 2\n");
 	// check if CF field exists
 	if (WLAN_GET_CAP_INFO_ESS(*sFrame.pwCapInfo)) {
 		if (sFrame.pCFParms->wCFPDurRemaining > 0) {
@@ -1974,32 +1971,22 @@ s_vMgrRxBeacon(
 		}
 	}
 
-	HIDWORD(qwTimestamp) = cpu_to_le32(HIDWORD(*sFrame.pqwTimestamp));
-	LODWORD(qwTimestamp) = cpu_to_le32(LODWORD(*sFrame.pqwTimestamp));
-	HIDWORD(qwLocalTSF) = HIDWORD(pRxPacket->qwLocalTSF);
-	LODWORD(qwLocalTSF) = LODWORD(pRxPacket->qwLocalTSF);
+	qwTimestamp = le64_to_cpu(*sFrame.pqwTimestamp);
+	qwLocalTSF = pRxPacket->qwLocalTSF;
 
 	// check if beacon TSF larger or small than our local TSF
-	if (HIDWORD(qwTimestamp) == HIDWORD(qwLocalTSF)) {
-		if (LODWORD(qwTimestamp) >= LODWORD(qwLocalTSF))
-			bTSFOffsetPostive = true;
-		else
-			bTSFOffsetPostive = false;
-	} else if (HIDWORD(qwTimestamp) > HIDWORD(qwLocalTSF)) {
+	if (qwTimestamp >= qwLocalTSF)
 		bTSFOffsetPostive = true;
-	} else if (HIDWORD(qwTimestamp) < HIDWORD(qwLocalTSF)) {
+	else
 		bTSFOffsetPostive = false;
-	}
 
 	if (bTSFOffsetPostive)
 		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwTimestamp), (qwLocalTSF));
 	else
 		qwTSFOffset = CARDqGetTSFOffset(pRxPacket->byRxRate, (qwLocalTSF), (qwTimestamp));
 
-	if (HIDWORD(qwTSFOffset) != 0 ||
-	    (LODWORD(qwTSFOffset) > TRIVIAL_SYNC_DIFFERENCE)) {
+	if (qwTSFOffset > TRIVIAL_SYNC_DIFFERENCE)
 		bTSFLargeDiff = true;
-	}
 
 	// if infra mode
 	if (bIsAPBeacon) {
@@ -2038,18 +2025,18 @@ s_vMgrRxBeacon(
 				// send out ps-poll packet
 
 				if (pMgmt->bInTIM)
-					PSvSendPSPOLL((PSDevice)pDevice);
+					PSvSendPSPOLL(pDevice);
 
 			} else {
 				pMgmt->bInTIMWake = false;
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Not In TIM..\n");
+				pr_debug("BCN: Not In TIM..\n");
 				if (!pDevice->bPWBitOn) {
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Send Null Packet\n");
+					pr_debug("BCN: Send Null Packet\n");
 					if (PSbSendNullPacket(pDevice))
 						pDevice->bPWBitOn = true;
 				}
 				if (PSbConsiderPowerDown(pDevice, false, false))
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "BCN: Power down now...\n");
+					pr_debug("BCN: Power down now...\n");
 			}
 
 		}
@@ -2089,7 +2076,7 @@ s_vMgrRxBeacon(
 				pMgmt->sNodeDBTable[uNodeIndex].uInActiveCount = 0;
 			} else {
 				// Todo, initial Node content
-				BSSvCreateOneNode((PSDevice)pDevice, &uNodeIndex);
+				BSSvCreateOneNode(pDevice, &uNodeIndex);
 
 				pMgmt->abyCurrSuppRates[1] = RATEuSetIE((PWLAN_IE_SUPP_RATES)sFrame.pSuppRates,
 									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
@@ -2108,16 +2095,14 @@ s_vMgrRxBeacon(
 				memcpy(pMgmt->sNodeDBTable[uNodeIndex].abyMACAddr, sFrame.pHdr->sA3.abyAddr2, WLAN_ADDR_LEN);
 				pMgmt->sNodeDBTable[uNodeIndex].bShortPreamble = WLAN_GET_CAP_INFO_SHORTPREAMBLE(*sFrame.pwCapInfo);
 				pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate = pMgmt->sNodeDBTable[uNodeIndex].wMaxSuppRate;
-#ifdef	PLICE_DEBUG
 				{
 					pr_debug("s_vMgrRxBeacon:TxDataRate is %d,Index is %d\n", pMgmt->sNodeDBTable[uNodeIndex].wTxDataRate, uNodeIndex);
 				}
-#endif
 			}
 
 			// if other stations joined, indicate connection to upper layer..
 			if (pMgmt->eCurrState == WMAC_STATE_STARTED) {
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Current IBSS State: [Started]........to: [Jointed]\n");
+				pr_debug("Current IBSS State: [Started]........to: [Jointed]\n");
 				pMgmt->eCurrState = WMAC_STATE_JOINTED;
 				pDevice->bLinkPass = true;
 				if (netif_queue_stopped(pDevice->dev))
@@ -2143,7 +2128,7 @@ s_vMgrRxBeacon(
 									(PWLAN_IE_SUPP_RATES)pMgmt->abyCurrSuppRates,
 									WLAN_RATES_MAXLEN_11B);
 				// set HW beacon interval and re-synchronizing....
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rejoining to Other Adhoc group with same SSID........\n");
+				pr_debug("Rejoining to Other Adhoc group with same SSID........\n");
 				VNSvOutPortW(pDevice->PortOffset + MAC_REG_BI, pMgmt->wCurrBeaconPeriod);
 				CARDbUpdateTSF(pDevice, pRxPacket->byRxRate, qwTimestamp, qwLocalTSF);
 				CARDvUpdateNextTBTT(pDevice->PortOffset, qwTimestamp, pMgmt->wCurrBeaconPeriod);
@@ -2190,20 +2175,20 @@ vMgrCreateOwnIBSS(
 	PCMD_STATUS pStatus
 )
 {
-	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
 	unsigned short wMaxBasicRate;
 	unsigned short wMaxSuppRate;
 	unsigned char byTopCCKBasicRate;
 	unsigned char byTopOFDMBasicRate;
-	QWORD               qwCurrTSF;
+	u64 qwCurrTSF;
 	unsigned int ii;
 	unsigned char abyRATE[] = {0x82, 0x84, 0x8B, 0x96, 0x24, 0x30, 0x48, 0x6C, 0x0C, 0x12, 0x18, 0x60};
 	unsigned char abyCCK_RATE[] = {0x82, 0x84, 0x8B, 0x96};
 	unsigned char abyOFDM_RATE[] = {0x0C, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6C};
 	unsigned short wSuppRate;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Create Basic Service Set .......\n");
+	pr_debug("Create Basic Service Set .......\n");
 
 	if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA) {
 		if ((pMgmt->eAuthenMode == WMAC_AUTH_WPANONE) &&
@@ -2312,18 +2297,18 @@ vMgrCreateOwnIBSS(
 	if (pMgmt->eCurrMode == WMAC_MODE_ESS_AP) {
 		// AP mode BSSID = MAC addr
 		memcpy(pMgmt->abyCurrBSSID, pMgmt->abyMACAddr, WLAN_ADDR_LEN);
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "AP beacon created BSSID:%pM\n",
+		pr_info("AP beacon created BSSID:%pM\n",
 			pMgmt->abyCurrBSSID);
 	}
 
 	if (pMgmt->eCurrMode == WMAC_MODE_IBSS_STA) {
 		// BSSID selected must be randomized as spec 11.1.3
-		pMgmt->abyCurrBSSID[5] = (unsigned char) (LODWORD(qwCurrTSF) & 0x000000ff);
-		pMgmt->abyCurrBSSID[4] = (unsigned char)((LODWORD(qwCurrTSF) & 0x0000ff00) >> 8);
-		pMgmt->abyCurrBSSID[3] = (unsigned char)((LODWORD(qwCurrTSF) & 0x00ff0000) >> 16);
-		pMgmt->abyCurrBSSID[2] = (unsigned char)((LODWORD(qwCurrTSF) & 0x00000ff0) >> 4);
-		pMgmt->abyCurrBSSID[1] = (unsigned char)((LODWORD(qwCurrTSF) & 0x000ff000) >> 12);
-		pMgmt->abyCurrBSSID[0] = (unsigned char)((LODWORD(qwCurrTSF) & 0x0ff00000) >> 20);
+		pMgmt->abyCurrBSSID[5] = (u8) (qwCurrTSF & 0x000000ff);
+		pMgmt->abyCurrBSSID[4] = (u8) ((qwCurrTSF & 0x0000ff00) >> 8);
+		pMgmt->abyCurrBSSID[3] = (u8) ((qwCurrTSF & 0x00ff0000) >> 16);
+		pMgmt->abyCurrBSSID[2] = (u8) ((qwCurrTSF & 0x00000ff0) >> 4);
+		pMgmt->abyCurrBSSID[1] = (u8) ((qwCurrTSF & 0x000ff000) >> 12);
+		pMgmt->abyCurrBSSID[0] = (u8) ((qwCurrTSF & 0x0ff00000) >> 20);
 		pMgmt->abyCurrBSSID[5] ^= pMgmt->abyMACAddr[0];
 		pMgmt->abyCurrBSSID[4] ^= pMgmt->abyMACAddr[1];
 		pMgmt->abyCurrBSSID[3] ^= pMgmt->abyMACAddr[2];
@@ -2333,7 +2318,7 @@ vMgrCreateOwnIBSS(
 		pMgmt->abyCurrBSSID[0] &= ~IEEE_ADDR_GROUP;
 		pMgmt->abyCurrBSSID[0] |= IEEE_ADDR_UNIVERSAL;
 
-		DBG_PRT(MSG_LEVEL_INFO, KERN_INFO "Adhoc beacon created bssid:%pM\n",
+		pr_info("Adhoc beacon created bssid:%pM\n",
 			pMgmt->abyCurrBSSID);
 	}
 
@@ -2371,9 +2356,9 @@ vMgrCreateOwnIBSS(
 	pMgmt->byERPContext = 0;
 
 	if (pMgmt->eConfigMode == WMAC_CONFIG_AP) {
-		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_AP);
+		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_AP);
 	} else {
-		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, OP_MODE_ADHOC);
+		CARDbSetBSSID(pMgmt->pAdapter, pMgmt->abyCurrBSSID, NL80211_IFTYPE_ADHOC);
 	}
 
 	CARDbSetPhyParameter(pMgmt->pAdapter,
@@ -2426,7 +2411,7 @@ vMgrJoinBSSBegin(
 	PCMD_STATUS pStatus
 )
 {
-	PSDevice     pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
 	PKnownBSS       pCurr = NULL;
 	unsigned int ii, uu;
@@ -2447,7 +2432,7 @@ vMgrJoinBSSBegin(
 
 	if (ii == MAX_BSS_NUM) {
 		*pStatus = CMD_STATUS_RESOURCES;
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "BSS finding:BSS list is empty.\n");
+		pr_info("BSS finding:BSS list is empty\n");
 		return;
 	}
 
@@ -2462,11 +2447,12 @@ vMgrJoinBSSBegin(
 	if (pCurr == NULL) {
 		*pStatus = CMD_STATUS_RESOURCES;
 		pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Scanning [%s] not found, disconnected !\n", pItemSSID->abySSID);
+		pr_info("Scanning [%s] not found, disconnected !\n",
+			pItemSSID->abySSID);
 		return;
 	}
 
-	DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "AP(BSS) finding:Found a AP(BSS)..\n");
+	pr_info("AP(BSS) finding:Found a AP(BSS)..\n");
 	if (WLAN_GET_CAP_INFO_ESS(cpu_to_le16(pCurr->wCapInfo))) {
 		if ((pMgmt->eAuthenMode == WMAC_AUTH_WPA) || (pMgmt->eAuthenMode == WMAC_AUTH_WPAPSK)) {
 			// patch for CISCO migration mode
@@ -2544,10 +2530,11 @@ vMgrJoinBSSBegin(
 			if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2) {
 				bool bResult = bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
 
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bAdd_PMKID_Candidate: 1(%d)\n", bResult);
+				pr_debug("bAdd_PMKID_Candidate: 1(%d)\n",
+					 bResult);
 				if (!bResult) {
 					vFlush_PMKID_Candidate((void *)pDevice);
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "vFlush_PMKID_Candidate: 4\n");
+					pr_debug("vFlush_PMKID_Candidate: 4\n");
 					bAdd_PMKID_Candidate((void *)pDevice, pMgmt->abyCurrBSSID, &pCurr->sRSNCapObj);
 				}
 			}
@@ -2555,9 +2542,9 @@ vMgrJoinBSSBegin(
 			// Preamble type auto-switch: if AP can receive short-preamble cap,
 			// we can turn on too.
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Join ESS\n");
+			pr_debug("Join ESS\n");
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "End of Join AP -- A/B/G Action\n");
+			pr_debug("End of Join AP -- A/B/G Action\n");
 		} else {
 			pMgmt->eCurrState = WMAC_STATE_IDLE;
 		}
@@ -2615,8 +2602,8 @@ vMgrJoinBSSBegin(
 
 			pMgmt->eCurrState = WMAC_STATE_STARTED;
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Join IBSS ok:%pM\n",
-				pMgmt->abyCurrBSSID);
+			pr_debug("Join IBSS ok:%pM\n",
+				 pMgmt->abyCurrBSSID);
 			// Preamble type auto-switch: if AP can receive short-preamble cap,
 			// and if registry setting is short preamble we can turn on too.
 
@@ -2641,7 +2628,7 @@ vMgrJoinBSSBegin(
 static
 void
 s_vMgrSynchBSS(
-	PSDevice      pDevice,
+	struct vnt_private *pDevice,
 	unsigned int uBSSMode,
 	PKnownBSS     pCurr,
 	PCMD_STATUS  pStatus
@@ -2663,7 +2650,7 @@ s_vMgrSynchBSS(
 			   pDevice->eEncryptionStatus,
 			   &(pMgmt->byCSSPK),
 			   &(pMgmt->byCSSGK))) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "s_bCipherMatch Fail .......\n");
+		pr_debug("s_bCipherMatch Fail .......\n");
 		return;
 	}
 
@@ -2676,7 +2663,6 @@ s_vMgrSynchBSS(
 	}
 
 	// Init the BSS informations
-	pDevice->bCCK = true;
 	pDevice->bProtectMode = false;
 	MACvDisableProtectMD(pDevice->PortOffset);
 	pDevice->bBarkerPreambleMd = false;
@@ -2701,8 +2687,7 @@ s_vMgrSynchBSS(
 
 	MACvReadBSSIDAddress(pDevice->PortOffset, pMgmt->abyCurrBSSID);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Sync:set CurrBSSID address = "
-		"%pM\n", pMgmt->abyCurrBSSID);
+	pr_debug("Sync:set CurrBSSID address = %pM\n", pMgmt->abyCurrBSSID);
 
 	if (pCurr->eNetworkTypeInUse == PHY_TYPE_11A) {
 		if ((pMgmt->eConfigPHYMode == PHY_TYPE_11A) ||
@@ -2742,13 +2727,13 @@ s_vMgrSynchBSS(
 	}
 
 	if (WLAN_GET_CAP_INFO_ESS(pCurr->wCapInfo)) {
-		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, OP_MODE_INFRASTRUCTURE);
+		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, NL80211_IFTYPE_STATION);
 		// Add current BSS to Candidate list
 		// This should only works for WPA2 BSS, and WPA2 BSS check must be done before.
 		if (pMgmt->eAuthenMode == WMAC_AUTH_WPA2)
 			CARDbAdd_PMKID_Candidate(pMgmt->pAdapter, pMgmt->abyCurrBSSID, pCurr->sRSNCapObj.bRSNCapExist, pCurr->sRSNCapObj.wRSNCap);
 	} else {
-		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, OP_MODE_ADHOC);
+		CARDbSetBSSID(pMgmt->pAdapter, pCurr->abyBSSID, NL80211_IFTYPE_ADHOC);
 	}
 
 	if (!CARDbSetPhyParameter(pMgmt->pAdapter,
@@ -2757,19 +2742,20 @@ s_vMgrSynchBSS(
 				 pCurr->sERP.byERP,
 				 pMgmt->abyCurrSuppRates,
 				 pMgmt->abyCurrExtSuppRates)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Phy Mode Fail [%d]\n", ePhyType);
+		pr_debug("<----s_bSynchBSS Set Phy Mode Fail [%d]\n", ePhyType);
 		return;
 	}
 	// set channel and clear NAV
 	if (!set_channel(pMgmt->pAdapter, pCurr->uChannel)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "<----s_bSynchBSS Set Channel [%d]\n", pCurr->uChannel);
+		pr_debug("<----s_bSynchBSS Set Channel [%d]\n",
+			 pCurr->uChannel);
 		return;
 	}
 
 	pMgmt->uCurrChannel = pCurr->uChannel;
 	pMgmt->eCurrentPHYMode = ePhyType;
 	pMgmt->byERPContext = pCurr->sERP.byERP;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Sync:Set to channel = [%d]\n", (int)pCurr->uChannel);
+	pr_debug("Sync:Set to channel = [%d]\n", (int)pCurr->uChannel);
 
 	*pStatus = CMD_STATUS_SUCCESS;
 
@@ -2779,7 +2765,7 @@ s_vMgrSynchBSS(
 //mike add: fix NetworkManager 0.7.0 hidden ssid mode in WPA encryption
 //                   ,need reset eAuthenMode and eEncryptionStatus
 static void  Encyption_Rebuild(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PKnownBSS pCurr
 )
 {
@@ -2894,7 +2880,7 @@ s_vMgrFormatTIM(
 static
 PSTxMgmtPacket
 s_MgrMakeBeacon(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wCurrBeaconPeriod,
@@ -3113,7 +3099,7 @@ s_MgrMakeBeacon(
 
 static PSTxMgmtPacket
 s_MgrMakeProbeResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wCurrBeaconPeriod,
@@ -3295,7 +3281,7 @@ s_MgrMakeProbeResponse(
 
 static PSTxMgmtPacket
 s_MgrMakeAssocRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned char *pDAddr,
 	unsigned short wCurrCapInfo,
@@ -3555,7 +3541,7 @@ s_MgrMakeAssocRequest(
 
 static PSTxMgmtPacket
 s_MgrMakeReAssocRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned char *pDAddr,
 	unsigned short wCurrCapInfo,
@@ -3800,7 +3786,7 @@ s_MgrMakeReAssocRequest(
 
 static PSTxMgmtPacket
 s_MgrMakeAssocResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wAssocStatus,
@@ -3871,7 +3857,7 @@ s_MgrMakeAssocResponse(
 
 static PSTxMgmtPacket
 s_MgrMakeReAssocResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	unsigned short wCurrCapInfo,
 	unsigned short wAssocStatus,
@@ -3943,7 +3929,7 @@ s_MgrMakeReAssocResponse(
 static
 void
 s_vMgrRxProbeResponse(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 )
@@ -3966,13 +3952,14 @@ s_vMgrRxProbeResponse(
 	    (sFrame.pwCapInfo == NULL) ||
 	    (sFrame.pSSID == NULL) ||
 	    (sFrame.pSuppRates == NULL)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe resp:Fail addr:[%p]\n", pRxPacket->p80211Header);
+		pr_debug("Probe resp:Fail addr:[%p]\n",
+			 pRxPacket->p80211Header);
 		DBG_PORT80(0xCC);
 		return;
 	}
 
 	if (sFrame.pSSID->len == 0)
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Rx Probe resp: SSID len = 0\n");
+		pr_debug("Rx Probe resp: SSID len = 0\n");
 
 	if (sFrame.pDSParms != NULL) {
 		if (byCurrChannel > CB_MAX_CHANNEL_24G) {
@@ -4026,7 +4013,8 @@ s_vMgrRxProbeResponse(
 				    (void *)pRxPacket
 );
 	} else {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Probe resp/insert: RxChannel = : %d\n", byCurrChannel);
+		pr_debug("Probe resp/insert: RxChannel = : %d\n",
+			 byCurrChannel);
 		BSSbInsertToBSSList((void *)pDevice,
 				    sFrame.pHdr->sA3.abyAddr3,
 				    *sFrame.pqwTimestamp,
@@ -4062,7 +4050,7 @@ s_vMgrRxProbeResponse(
 static
 void
 s_vMgrRxProbeRequest(
-	PSDevice pDevice,
+	struct vnt_private *pDevice,
 	PSMgmtObject pMgmt,
 	PSRxMgmtPacket pRxPacket
 )
@@ -4115,7 +4103,7 @@ s_vMgrRxProbeRequest(
 			/* send the frame */
 			Status = csMgmt_xmit(pDevice, pTxPacket);
 			if (Status != CMD_STATUS_PENDING)
-				DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Mgt:Probe response tx failed\n");
+				pr_debug("Mgt:Probe response tx failed\n");
 		}
 	}
 }
@@ -4141,7 +4129,7 @@ vMgrRxManagePacket(
 	PSRxMgmtPacket pRxPacket
 )
 {
-	PSDevice    pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	bool bInScan = false;
 	unsigned int uNodeIndex = 0;
 	NODE_STATE  eNodeState = 0;
@@ -4155,7 +4143,7 @@ vMgrRxManagePacket(
 	switch (WLAN_GET_FC_FSTYPE((pRxPacket->p80211Header->sA3.wFrameCtl))) {
 	case WLAN_FSTYPE_ASSOCREQ:
 		// Frame Clase = 2
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocreq\n");
+		pr_debug("rx assocreq\n");
 		if (eNodeState < NODE_AUTH) {
 			// send deauth notification
 			// reason = (6) class 2 received from nonauth sta
@@ -4165,7 +4153,7 @@ vMgrRxManagePacket(
 					     (6),
 					     &Status
 );
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 1\n");
+			pr_debug("wmgr: send vMgrDeAuthenBeginSta 1\n");
 		} else {
 			s_vMgrRxAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
 		}
@@ -4173,14 +4161,14 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_ASSOCRESP:
 		// Frame Clase = 2
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocresp1\n");
+		pr_debug("rx assocresp1\n");
 		s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, false);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx assocresp2\n");
+		pr_debug("rx assocresp2\n");
 		break;
 
 	case WLAN_FSTYPE_REASSOCREQ:
 		// Frame Clase = 2
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx reassocreq\n");
+		pr_debug("rx reassocreq\n");
 		// Todo: reassoc
 		if (eNodeState < NODE_AUTH) {
 			// send deauth notification
@@ -4191,7 +4179,7 @@ vMgrRxManagePacket(
 					     (6),
 					     &Status
 );
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 2\n");
+			pr_debug("wmgr: send vMgrDeAuthenBeginSta 2\n");
 
 		}
 		s_vMgrRxReAssocRequest(pDevice, pMgmt, pRxPacket, uNodeIndex);
@@ -4199,7 +4187,7 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_REASSOCRESP:
 		// Frame Clase = 2
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx reassocresp\n");
+		pr_debug("rx reassocresp\n");
 		s_vMgrRxAssocResponse(pDevice, pMgmt, pRxPacket, true);
 		break;
 
@@ -4210,7 +4198,7 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_PROBERESP:
 		// Frame Clase = 0
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx proberesp\n");
+		pr_debug("rx proberesp\n");
 
 		s_vMgrRxProbeResponse(pDevice, pMgmt, pRxPacket);
 		break;
@@ -4225,12 +4213,12 @@ vMgrRxManagePacket(
 
 	case WLAN_FSTYPE_ATIM:
 		// Frame Clase = 1
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx atim\n");
+		pr_debug("rx atim\n");
 		break;
 
 	case WLAN_FSTYPE_DISASSOC:
 		// Frame Clase = 2
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx disassoc\n");
+		pr_debug("rx disassoc\n");
 		if (eNodeState < NODE_AUTH) {
 			// send deauth notification
 			// reason = (6) class 2 received from nonauth sta
@@ -4240,25 +4228,25 @@ vMgrRxManagePacket(
 					     (6),
 					     &Status
 );
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wmgr: send vMgrDeAuthenBeginSta 3\n");
+			pr_debug("wmgr: send vMgrDeAuthenBeginSta 3\n");
 		}
 		s_vMgrRxDisassociation(pDevice, pMgmt, pRxPacket);
 		break;
 
 	case WLAN_FSTYPE_AUTHEN:
 		// Frame Clase = 1
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO  "rx authen\n");
+		pr_debug("rx authen\n");
 		s_vMgrRxAuthentication(pDevice, pMgmt, pRxPacket);
 		break;
 
 	case WLAN_FSTYPE_DEAUTHEN:
 		// Frame Clase = 1
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx deauthen\n");
+		pr_debug("rx deauthen\n");
 		s_vMgrRxDeauthentication(pDevice, pMgmt, pRxPacket);
 		break;
 
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "rx unknown mgmt\n");
+		pr_debug("rx unknown mgmt\n");
 	}
 }
 
@@ -4279,7 +4267,7 @@ bMgrPrepareBeaconToSend(
 	PSMgmtObject pMgmt
 )
 {
-	PSDevice            pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	PSTxMgmtPacket      pTxPacket;
 
 	if (pDevice->bEncryptionEnable || pDevice->bEnable8021x)
@@ -4331,46 +4319,46 @@ s_vMgrLogStatus(
 {
 	switch (wStatus) {
 	case WLAN_MGMT_STATUS_UNSPEC_FAILURE:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Unspecified error.\n");
+		pr_info("Status code == Unspecified error\n");
 		break;
 	case WLAN_MGMT_STATUS_CAPS_UNSUPPORTED:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Can't support all requested capabilities.\n");
+		pr_info("Status code == Can't support all requested capabilities\n");
 		break;
 	case WLAN_MGMT_STATUS_REASSOC_NO_ASSOC:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Reassoc denied, can't confirm original Association.\n");
+		pr_info("Status code == Reassoc denied, can't confirm original Association\n");
 		break;
 	case WLAN_MGMT_STATUS_ASSOC_DENIED_UNSPEC:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, undefine in spec\n");
+		pr_info("Status code == Assoc denied, undefine in spec\n");
 		break;
 	case WLAN_MGMT_STATUS_UNSUPPORTED_AUTHALG:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Peer doesn't support authen algorithm.\n");
+		pr_info("Status code == Peer doesn't support authen algorithm\n");
 		break;
 	case WLAN_MGMT_STATUS_RX_AUTH_NOSEQ:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen frame received out of sequence.\n");
+		pr_info("Status code == Authen frame received out of sequence\n");
 		break;
 	case WLAN_MGMT_STATUS_CHALLENGE_FAIL:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen rejected, challenge  failure.\n");
+		pr_info("Status code == Authen rejected, challenge  failure\n");
 		break;
 	case WLAN_MGMT_STATUS_AUTH_TIMEOUT:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Authen rejected, timeout waiting for next frame.\n");
+		pr_info("Status code == Authen rejected, timeout waiting for next frame\n");
 		break;
 	case WLAN_MGMT_STATUS_ASSOC_DENIED_BUSY:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, AP too busy.\n");
+		pr_info("Status code == Assoc denied, AP too busy\n");
 		break;
 	case WLAN_MGMT_STATUS_ASSOC_DENIED_RATES:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we haven't enough basic rates.\n");
+		pr_info("Status code == Assoc denied, we haven't enough basic rates\n");
 		break;
 	case WLAN_MGMT_STATUS_ASSOC_DENIED_SHORTPREAMBLE:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support short preamble.\n");
+		pr_info("Status code == Assoc denied, we do not support short preamble\n");
 		break;
 	case WLAN_MGMT_STATUS_ASSOC_DENIED_PBCC:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support PBCC.\n");
+		pr_info("Status code == Assoc denied, we do not support PBCC\n");
 		break;
 	case WLAN_MGMT_STATUS_ASSOC_DENIED_AGILITY:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Status code == Assoc denied, we do not support channel agility.\n");
+		pr_info("Status code == Assoc denied, we do not support channel agility\n");
 		break;
 	default:
-		DBG_PRT(MSG_LEVEL_NOTICE, KERN_INFO "Unknown status code %d.\n", wStatus);
+		pr_info("Unknown status code %d\n", wStatus);
 		break;
 	}
 }
@@ -4398,11 +4386,12 @@ bAdd_PMKID_Candidate(
 	PSRSNCapObject psRSNCapObj
 )
 {
-	PSDevice         pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 	struct pmkid_candidate *pCandidateList;
 	unsigned int ii = 0;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "bAdd_PMKID_Candidate START: (%d)\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+	pr_debug("bAdd_PMKID_Candidate START: (%d)\n",
+		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
 
 	if ((pDevice == NULL) || (pbyBSSID == NULL) || (psRSNCapObj == NULL))
 		return false;
@@ -4432,7 +4421,8 @@ bAdd_PMKID_Candidate(
 
 	memcpy(pCandidateList->BSSID, pbyBSSID, ETH_ALEN);
 	pDevice->gsPMKIDCandidate.NumCandidates++;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "NumCandidates:%d\n", (int)pDevice->gsPMKIDCandidate.NumCandidates);
+	pr_debug("NumCandidates:%d\n",
+		 (int)pDevice->gsPMKIDCandidate.NumCandidates);
 	return true;
 }
 
@@ -4455,7 +4445,7 @@ vFlush_PMKID_Candidate(
 	void *hDeviceContext
 )
 {
-	PSDevice        pDevice = (PSDevice)hDeviceContext;
+	struct vnt_private *pDevice = hDeviceContext;
 
 	if (pDevice == NULL)
 		return;
@@ -4549,8 +4539,9 @@ s_bCipherMatch(
 		}
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%d, %d, %d, %d, EncStatus:%d\n",
-		byMulticastCipher, byCipherMask, pBSSNode->bWPAValid, pBSSNode->bWPA2Valid, EncStatus);
+	pr_debug("%d, %d, %d, %d, EncStatus:%d\n",
+		 byMulticastCipher, byCipherMask,
+		 pBSSNode->bWPAValid, pBSSNode->bWPA2Valid, EncStatus);
 
 	// mask our cap. with BSS
 	if (EncStatus == Ndis802_11Encryption1Enabled) {
diff --git a/drivers/staging/vt6655/wmgr.h b/drivers/staging/vt6655/wmgr.h
index a71daed7fa0b..ce939b30ac2a 100644
--- a/drivers/staging/vt6655/wmgr.h
+++ b/drivers/staging/vt6655/wmgr.h
@@ -175,7 +175,7 @@ typedef struct tagSTxMgmtPacket {
 // Rx Management Packet descriptor
 typedef struct tagSRxMgmtPacket {
 	PUWLAN_80211HDR     p80211Header;
-	QWORD               qwLocalTSF;
+	u64 qwLocalTSF;
 	unsigned int cbMPDULen;
 	unsigned int cbPayloadLen;
 	unsigned int uRSSI;
diff --git a/drivers/staging/vt6655/wpa.c b/drivers/staging/vt6655/wpa.c
index 7b1bab91a9cf..5d4eca8512af 100644
--- a/drivers/staging/vt6655/wpa.c
+++ b/drivers/staging/vt6655/wpa.c
@@ -43,8 +43,6 @@
 #include "80211mgr.h"
 
 /*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
-
 static const unsigned char abyOUI00[4] = { 0x00, 0x50, 0xf2, 0x00 };
 static const unsigned char abyOUI01[4] = { 0x00, 0x50, 0xf2, 0x01 };
 static const unsigned char abyOUI02[4] = { 0x00, 0x50, 0xf2, 0x02 };
@@ -115,13 +113,13 @@ WPA_ParseRSN(
 
 	WPA_ClearRSN(pBSSList);
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WPA_ParseRSN: [%d]\n", pRSN->len);
+	pr_debug("WPA_ParseRSN: [%d]\n", pRSN->len);
 
 	// information element header makes sense
 	if ((pRSN->len >= 6) // oui1(4)+ver(2)
 	    && (pRSN->byElementID == WLAN_EID_RSN_WPA) && !memcmp(pRSN->abyOUI, abyOUI01, 4)
 	    && (pRSN->wVersion == 1)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Legal RSN\n");
+		pr_debug("Legal RSN\n");
 		// update each variable if pRSN is long enough to contain the variable
 		if (pRSN->len >= 10) {
 			//OUI1(4)+ver(2)+GKSuite(4)
@@ -139,13 +137,14 @@ WPA_ParseRSN(
 				// any vendor checks here
 				pBSSList->byGKType = WPA_NONE;
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "byGKType: %x\n", pBSSList->byGKType);
+			pr_debug("byGKType: %x\n", pBSSList->byGKType);
 		}
 
 		if (pRSN->len >= 12) {
 			//oui1(4)+ver(2)+GKS(4)+PKSCnt(2)
 			j = 0;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wPKCount: %d, sizeof(pBSSList->abyPKType): %zu\n", pRSN->wPKCount, sizeof(pBSSList->abyPKType));
+			pr_debug("wPKCount: %d, sizeof(pBSSList->abyPKType): %zu\n",
+				 pRSN->wPKCount, sizeof(pBSSList->abyPKType));
 			for (i = 0; (i < pRSN->wPKCount) && (j < ARRAY_SIZE(pBSSList->abyPKType)); i++) {
 				if (pRSN->len >= 12+i*4+4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*i)
 					if (!memcmp(pRSN->PKSList[i].abyOUI, abyOUI00, 4))
@@ -163,19 +162,20 @@ WPA_ParseRSN(
 					break;
 			}
 			pBSSList->wPKCount = (unsigned short)j;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wPKCount: %d\n", pBSSList->wPKCount);
+			pr_debug("wPKCount: %d\n", pBSSList->wPKCount);
 		}
 
 		m = pRSN->wPKCount;
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "m: %d\n", m);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "14+m*4: %d\n", 14+m*4);
+		pr_debug("m: %d\n", m);
+		pr_debug("14+m*4: %d\n", 14+m*4);
 
 		if (pRSN->len >= 14+m*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)
 			// overlay IE_RSN_Auth structure into correct place
 			pIE_RSN_Auth = (PWLAN_IE_RSN_AUTH) pRSN->PKSList[m].abyOUI;
 			j = 0;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wAuthCount: %d, sizeof(pBSSList->abyAuthType): %zu\n",
-				pIE_RSN_Auth->wAuthCount, sizeof(pBSSList->abyAuthType));
+			pr_debug("wAuthCount: %d, sizeof(pBSSList->abyAuthType): %zu\n",
+				 pIE_RSN_Auth->wAuthCount,
+				 sizeof(pBSSList->abyAuthType));
 			for (i = 0; (i < pIE_RSN_Auth->wAuthCount) && (j < ARRAY_SIZE(pBSSList->abyAuthType)); i++) {
 				if (pRSN->len >= 14+4+(m+i)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*i)
 					if (!memcmp(pIE_RSN_Auth->AuthKSList[i].abyOUI, abyOUI01, 4))
@@ -191,14 +191,14 @@ WPA_ParseRSN(
 			}
 			if (j > 0)
 				pBSSList->wAuthCount = (unsigned short)j;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wAuthCount: %d\n", pBSSList->wAuthCount);
+			pr_debug("wAuthCount: %d\n", pBSSList->wAuthCount);
 		}
 
 		if (pIE_RSN_Auth != NULL) {
 			n = pIE_RSN_Auth->wAuthCount;
 
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "n: %d\n", n);
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "14+4+(m+n)*4: %d\n", 14+4+(m+n)*4);
+			pr_debug("n: %d\n", n);
+			pr_debug("14+4+(m+n)*4: %d\n", 14+4+(m+n)*4);
 
 			if (pRSN->len+2 >= 14+4+(m+n)*4) { //oui1(4)+ver(2)+GKS(4)+PKSCnt(2)+PKS(4*m)+AKC(2)+AKS(4*n)+Cap(2)
 				pbyCaps = (unsigned char *)pIE_RSN_Auth->AuthKSList[n].abyOUI;
diff --git a/drivers/staging/vt6655/wpa2.c b/drivers/staging/vt6655/wpa2.c
index 4e1b63be380f..bb335ef51172 100644
--- a/drivers/staging/vt6655/wpa2.c
+++ b/drivers/staging/vt6655/wpa2.c
@@ -35,8 +35,6 @@
 #include "device.h"
 #include "wmgr.h"
 
-/*---------------------  Static Definitions -------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Classes  ----------------------------*/
 
 /*---------------------  Static Variables  --------------------------*/
@@ -116,7 +114,7 @@ WPA2vParseRSN(
 	unsigned char *pbyOUI;
 	bool bUseGK = false;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "WPA2_ParseRSN: [%d]\n", pRSN->len);
+	pr_debug("WPA2_ParseRSN: [%d]\n", pRSN->len);
 
 	WPA2_ClearRSN(pBSSNode);
 
@@ -135,7 +133,7 @@ WPA2vParseRSN(
 	// information element header makes sense
 	if ((pRSN->byElementID == WLAN_EID_RSN) &&
 	    (pRSN->wVersion == 1)) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Legal 802.11i RSN\n");
+		pr_debug("Legal 802.11i RSN\n");
 
 		pbyOUI = &(pRSN->abyRSN[0]);
 		if (!memcmp(pbyOUI, abyOUIWEP40, 4))
@@ -153,7 +151,7 @@ WPA2vParseRSN(
 			// any vendor checks here
 			pBSSNode->byCSSGK = WLAN_11i_CSS_UNKNOWN;
 
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "802.11i CSS: %X\n", pBSSNode->byCSSGK);
+		pr_debug("802.11i CSS: %X\n", pBSSNode->byCSSGK);
 
 		if (pRSN->len == 6) {
 			pBSSNode->bWPA2Valid = true;
@@ -186,7 +184,8 @@ WPA2vParseRSN(
 						pBSSNode->abyCSSPK[j++] = WLAN_11i_CSS_UNKNOWN;
 					}
 					pbyOUI += 4;
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyCSSPK[%d]: %X\n", j-1, pBSSNode->abyCSSPK[j-1]);
+					pr_debug("abyCSSPK[%d]: %X\n",
+						 j-1, pBSSNode->abyCSSPK[j-1]);
 				} else
 					break;
 			} //for
@@ -206,7 +205,7 @@ WPA2vParseRSN(
 				return;
 			}
 			pBSSNode->wCSSPKCount = (unsigned short)j;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wCSSPKCount: %d\n", pBSSNode->wCSSPKCount);
+			pr_debug("wCSSPKCount: %d\n", pBSSNode->wCSSPKCount);
 		}
 
 		m = *((unsigned short *)&(pRSN->abyRSN[4]));
@@ -224,12 +223,15 @@ WPA2vParseRSN(
 					else
 						// any vendor checks here
 						pBSSNode->abyAKMSSAuthType[j++] = WLAN_11i_AKMSS_UNKNOWN;
-					DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "abyAKMSSAuthType[%d]: %X\n", j-1, pBSSNode->abyAKMSSAuthType[j-1]);
+					pr_debug("abyAKMSSAuthType[%d]: %X\n",
+						 j-1,
+						 pBSSNode->abyAKMSSAuthType[j-1]);
 				} else
 					break;
 			}
 			pBSSNode->wAKMSSAuthCount = (unsigned short)j;
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wAKMSSAuthCount: %d\n", pBSSNode->wAKMSSAuthCount);
+			pr_debug("wAKMSSAuthCount: %d\n",
+				 pBSSNode->wAKMSSAuthCount);
 
 			n = *((unsigned short *)&(pRSN->abyRSN[6+4*m]));
 			if (pRSN->len >= 12 + 4 * m + 4 * n) { // ver(2)+GK(4)+PKCnt(2)+PKS(4*m)+AKMSSCnt(2)+AKMSS(4*n)+Cap(2)
diff --git a/drivers/staging/vt6655/wpactl.c b/drivers/staging/vt6655/wpactl.c
index 5f454ca2a3bc..dab1e8078652 100644
--- a/drivers/staging/vt6655/wpactl.c
+++ b/drivers/staging/vt6655/wpactl.c
@@ -50,9 +50,6 @@ static const int frequency_list[] = {
 };
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
-
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -83,13 +80,13 @@ static void wpadev_setup(struct net_device *dev)
  *
  */
 
-static int wpa_init_wpadev(PSDevice pDevice)
+static int wpa_init_wpadev(struct vnt_private *pDevice)
 {
-	PSDevice wpadev_priv;
+	struct vnt_private *wpadev_priv;
 	struct net_device *dev = pDevice->dev;
 	int ret = 0;
 
-	pDevice->wpadev = alloc_netdev(sizeof(PSDevice), "vntwpa",
+	pDevice->wpadev = alloc_netdev(sizeof(*wpadev_priv), "vntwpa",
 				       NET_NAME_UNKNOWN, wpadev_setup);
 	if (pDevice->wpadev == NULL)
 		return -ENOMEM;
@@ -103,8 +100,7 @@ static int wpa_init_wpadev(PSDevice pDevice)
 	pDevice->wpadev->mem_end = dev->mem_end;
 	ret = register_netdev(pDevice->wpadev);
 	if (ret) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: register_netdev(WPA) failed!\n",
-			dev->name);
+		pr_debug("%s: register_netdev(WPA) failed!\n", dev->name);
 		free_netdev(pDevice->wpadev);
 		return -1;
 	}
@@ -115,8 +111,8 @@ static int wpa_init_wpadev(PSDevice pDevice)
 			return -ENOMEM;
 	}
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Registered netdev %s for WPA management\n",
-		dev->name, pDevice->wpadev->name);
+	pr_debug("%s: Registered netdev %s for WPA management\n",
+		 dev->name, pDevice->wpadev->name);
 
 	return 0;
 }
@@ -134,7 +130,7 @@ static int wpa_init_wpadev(PSDevice pDevice)
  *
  */
 
-static int wpa_release_wpadev(PSDevice pDevice)
+static int wpa_release_wpadev(struct vnt_private *pDevice)
 {
 	if (pDevice->skb) {
 		dev_kfree_skb(pDevice->skb);
@@ -142,8 +138,8 @@ static int wpa_release_wpadev(PSDevice pDevice)
 	}
 
 	if (pDevice->wpadev) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "%s: Netdevice %s unregistered\n",
-			pDevice->dev->name, pDevice->wpadev->name);
+		pr_debug("%s: Netdevice %s unregistered\n",
+			 pDevice->dev->name, pDevice->wpadev->name);
 		unregister_netdev(pDevice->wpadev);
 		free_netdev(pDevice->wpadev);
 		pDevice->wpadev = NULL;
@@ -166,7 +162,7 @@ static int wpa_release_wpadev(PSDevice pDevice)
  *
  */
 
-int wpa_set_wpadev(PSDevice pDevice, int val)
+int wpa_set_wpadev(struct vnt_private *pDevice, int val)
 {
 	if (val)
 		return wpa_init_wpadev(pDevice);
@@ -188,7 +184,7 @@ int wpa_set_wpadev(PSDevice pDevice, int val)
  *
  */
 
-int wpa_set_keys(PSDevice pDevice, void *ctx,
+int wpa_set_keys(struct vnt_private *pDevice, void *ctx,
 		 bool fcpfkernel) __must_hold(&pDevice->lock)
 {
 	struct viawget_wpa_param *param = ctx;
@@ -196,7 +192,7 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 	unsigned long dwKeyIndex = 0;
 	unsigned char abyKey[MAX_KEY_LEN];
 	unsigned char abySeq[MAX_KEY_LEN];
-	QWORD   KeyRSC;
+	u64 KeyRSC;
 	unsigned char byKeyDecMode = KEY_CTL_WEP;
 	int ret = 0;
 	int uu, ii;
@@ -206,7 +202,7 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 	    param->u.wpa_key.seq_len > MAX_KEY_LEN)
 		return -EINVAL;
 
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "param->u.wpa_key.alg_name = %d\n", param->u.wpa_key.alg_name);
+	pr_debug("param->u.wpa_key.alg_name = %d\n", param->u.wpa_key.alg_name);
 	if (param->u.wpa_key.alg_name == WPA_ALG_NONE) {
 		pDevice->eEncryptionStatus = Ndis802_11EncryptionDisabled;
 		pDevice->bEncryptionEnable = false;
@@ -224,7 +220,9 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 	} else {
 		spin_unlock_irq(&pDevice->lock);
 		if (param->u.wpa_key.key &&
-		    copy_from_user(&abyKey[0], param->u.wpa_key.key, param->u.wpa_key.key_len)) {
+		    copy_from_user(&abyKey[0],
+				   (void __user *)param->u.wpa_key.key,
+				   param->u.wpa_key.key_len)) {
 			spin_lock_irq(&pDevice->lock);
 			return -EINVAL;
 		}
@@ -262,7 +260,9 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 	} else {
 		spin_unlock_irq(&pDevice->lock);
 		if (param->u.wpa_key.seq &&
-		    copy_from_user(&abySeq[0], param->u.wpa_key.seq, param->u.wpa_key.seq_len)) {
+		    copy_from_user(&abySeq[0],
+				   (void __user *)param->u.wpa_key.seq,
+				   param->u.wpa_key.seq_len)) {
 			spin_lock_irq(&pDevice->lock);
 			return -EINVAL;
 		}
@@ -272,15 +272,15 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 	if (param->u.wpa_key.seq_len > 0) {
 		for (ii = 0; ii < param->u.wpa_key.seq_len; ii++) {
 			if (ii < 4)
-				LODWORD(KeyRSC) |= (abySeq[ii] << (ii * 8));
+				KeyRSC |= (u64)(abySeq[ii] << (ii * 8));
 			else
-				HIDWORD(KeyRSC) |= (abySeq[ii] << ((ii-4) * 8));
+				KeyRSC |= (u64)(abySeq[ii] << ((ii-4) * 8));
 		}
 		dwKeyIndex |= 1 << 29;
 	}
 
 	if (param->u.wpa_key.key_index >= MAX_GROUP_KEY) {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return  dwKeyIndex > 3\n");
+		pr_debug("return  dwKeyIndex > 3\n");
 		return -EINVAL;
 	}
 
@@ -300,7 +300,7 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 	else
 		byKeyDecMode = KEY_CTL_WEP;
 
-	// Fix HCT test that set 256 bits KEY and Ndis802_11Encryption3Enabled
+	/* Fix HCT test that set 256 bits KEY and Ndis802_11Encryption3Enabled */
 	if (pDevice->eEncryptionStatus == Ndis802_11Encryption3Enabled) {
 		if (param->u.wpa_key.key_len == MAX_KEY_LEN)
 			byKeyDecMode = KEY_CTL_TKIP;
@@ -315,29 +315,29 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 			byKeyDecMode = KEY_CTL_WEP;
 	}
 
-	// Check TKIP key length
+	/* Check TKIP key length */
 	if ((byKeyDecMode == KEY_CTL_TKIP) &&
 	    (param->u.wpa_key.key_len != MAX_KEY_LEN)) {
-		// TKIP Key must be 256 bits
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "return- TKIP Key must be 256 bits!\n");
+		/* TKIP Key must be 256 bits */
+		pr_debug("return- TKIP Key must be 256 bits!\n");
 		return -EINVAL;
 	}
-	// Check AES key length
+	/* Check AES key length */
 	if ((byKeyDecMode == KEY_CTL_CCMP) &&
 	    (param->u.wpa_key.key_len != AES_KEY_LEN)) {
-		// AES Key must be 128 bits
+		/* AES Key must be 128 bits */
 		return -EINVAL;
 	}
 
-	// spin_lock_irq(&pDevice->lock);
+	/* spin_lock_irq(&pDevice->lock); */
 	if (is_broadcast_ether_addr(&param->addr[0]) || (param->addr == NULL)) {
-		// If is_broadcast_ether_addr, set the key as every key entry's group key.
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Groupe Key Assign.\n");
+		/* If is_broadcast_ether_addr, set the key as every key entry's group key. */
+		pr_debug("Groupe Key Assign\n");
 
 		if (KeybSetAllGroupKey(&(pDevice->sKey),
 					dwKeyIndex,
 					param->u.wpa_key.key_len,
-					(PQWORD) &(KeyRSC),
+					(u64 *) &KeyRSC,
 					(unsigned char *)abyKey,
 					byKeyDecMode,
 					pDevice->PortOffset,
@@ -345,27 +345,27 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 		    KeybSetDefaultKey(&(pDevice->sKey),
 				       dwKeyIndex,
 				       param->u.wpa_key.key_len,
-				       (PQWORD) &(KeyRSC),
+				       (u64 *) &KeyRSC,
 				       (unsigned char *)abyKey,
 				       byKeyDecMode,
 				       pDevice->PortOffset,
 				       pDevice->byLocalID)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "GROUP Key Assign.\n");
+			pr_debug("GROUP Key Assign\n");
 
 		} else {
 			return -EINVAL;
 		}
 
 	} else {
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key Assign.\n");
-		// BSSID not 0xffffffffffff
-		// Pairwise Key can't be WEP
+		pr_debug("Pairwise Key Assign\n");
+		/* BSSID not 0xffffffffffff */
+		/* Pairwise Key can't be WEP */
 		if (byKeyDecMode == KEY_CTL_WEP) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key can't be WEP\n");
+			pr_debug("Pairwise Key can't be WEP\n");
 			return -EINVAL;
 		}
 
-		dwKeyIndex |= (1 << 30); // set pairwise key
+		dwKeyIndex |= (1 << 30); /* set pairwise key */
 		if (pMgmt->eConfigMode == WMAC_CONFIG_IBSS_STA)
 			return -EINVAL;
 
@@ -373,18 +373,18 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
 			       &param->addr[0],
 			       dwKeyIndex,
 			       param->u.wpa_key.key_len,
-			       (PQWORD) &(KeyRSC),
+			       (u64 *) &KeyRSC,
 			       (unsigned char *)abyKey,
 			       byKeyDecMode,
 			       pDevice->PortOffset,
 			       pDevice->byLocalID)) {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "Pairwise Key Set\n");
+			pr_debug("Pairwise Key Set\n");
 
 		} else {
-			// Key Table Full
+			/* Key Table Full */
 			return -EINVAL;
 		}
-	} // BSSID not 0xffffffffffff
+	} /* BSSID not 0xffffffffffff */
 	if ((ret == 0) && ((param->u.wpa_key.set_tx) != 0)) {
 		pDevice->byKeyIndex = (unsigned char)param->u.wpa_key.key_index;
 		pDevice->bTransmitKey = true;
@@ -408,7 +408,7 @@ int wpa_set_keys(PSDevice pDevice, void *ctx,
  *
  */
 
-static int wpa_set_wpa(PSDevice pDevice,
+static int wpa_set_wpa(struct vnt_private *pDevice,
 		       struct viawget_wpa_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
@@ -433,7 +433,7 @@ static int wpa_set_wpa(PSDevice pDevice,
  *
  */
 
-static int wpa_set_disassociate(PSDevice pDevice,
+static int wpa_set_disassociate(struct vnt_private *pDevice,
 				struct viawget_wpa_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
@@ -462,7 +462,7 @@ static int wpa_set_disassociate(PSDevice pDevice,
  *
  */
 
-static int wpa_set_scan(PSDevice pDevice,
+static int wpa_set_scan(struct vnt_private *pDevice,
 			struct viawget_wpa_param *param)
 {
 	spin_lock_irq(&pDevice->lock);
@@ -487,7 +487,7 @@ static int wpa_set_scan(PSDevice pDevice,
  *
  */
 
-static int wpa_get_bssid(PSDevice pDevice,
+static int wpa_get_bssid(struct vnt_private *pDevice,
 			 struct viawget_wpa_param *param)
 {
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
@@ -511,7 +511,7 @@ static int wpa_get_bssid(PSDevice pDevice,
  *
  */
 
-static int wpa_get_ssid(PSDevice pDevice,
+static int wpa_get_ssid(struct vnt_private *pDevice,
 			struct viawget_wpa_param *param)
 {
 	PSMgmtObject        pMgmt = pDevice->pMgmt;
@@ -539,7 +539,7 @@ static int wpa_get_ssid(PSDevice pDevice,
  *
  */
 
-static int wpa_get_scan(PSDevice pDevice,
+static int wpa_get_scan(struct vnt_private *pDevice,
 			struct viawget_wpa_param *param)
 {
 	struct viawget_scan_result *scan_buf;
@@ -636,7 +636,7 @@ static int wpa_get_scan(PSDevice pDevice,
 		ret = -EFAULT;
 
 	param->u.scan_results.scan_count = count;
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO " param->u.scan_results.scan_count = %d\n", count);
+	pr_debug(" param->u.scan_results.scan_count = %d\n", count);
 
 		kfree(pBuf);
 	return ret;
@@ -656,7 +656,7 @@ static int wpa_get_scan(PSDevice pDevice,
  *
  */
 
-static int wpa_set_associate(PSDevice pDevice,
+static int wpa_set_associate(struct vnt_private *pDevice,
 			     struct viawget_wpa_param *param)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
@@ -665,13 +665,15 @@ static int wpa_set_associate(PSDevice pDevice,
 	unsigned char abyWPAIE[64];
 	bool bWepEnabled = false;
 
-	// set key type & algorithm
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "pairwise_suite = %d\n", param->u.wpa_associate.pairwise_suite);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "group_suite = %d\n", param->u.wpa_associate.group_suite);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "key_mgmt_suite = %d\n", param->u.wpa_associate.key_mgmt_suite);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "auth_alg = %d\n", param->u.wpa_associate.auth_alg);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "mode = %d\n", param->u.wpa_associate.mode);
-	DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_ie_len = %d\n", param->u.wpa_associate.wpa_ie_len);
+	/* set key type & algorithm */
+	pr_debug("pairwise_suite = %d\n",
+		 param->u.wpa_associate.pairwise_suite);
+	pr_debug("group_suite = %d\n", param->u.wpa_associate.group_suite);
+	pr_debug("key_mgmt_suite = %d\n",
+		 param->u.wpa_associate.key_mgmt_suite);
+	pr_debug("auth_alg = %d\n", param->u.wpa_associate.auth_alg);
+	pr_debug("mode = %d\n", param->u.wpa_associate.mode);
+	pr_debug("wpa_ie_len = %d\n", param->u.wpa_associate.wpa_ie_len);
 
 	if (param->u.wpa_associate.wpa_ie_len) {
 		if (!param->u.wpa_associate.wpa_ie)
@@ -686,13 +688,13 @@ static int wpa_set_associate(PSDevice pDevice,
 		pMgmt->eConfigMode = WMAC_CONFIG_IBSS_STA;
 	else
 		pMgmt->eConfigMode = WMAC_CONFIG_ESS_STA;
-	// set ssid
+	/* set ssid */
 	memset(pMgmt->abyDesireSSID, 0, WLAN_IEHDR_LEN + WLAN_SSID_MAXLEN + 1);
 	pItemSSID = (PWLAN_IE_SSID)pMgmt->abyDesireSSID;
 	pItemSSID->byElementID = WLAN_EID_SSID;
 	pItemSSID->len = param->u.wpa_associate.ssid_len;
 	memcpy(pItemSSID->abySSID, param->u.wpa_associate.ssid, pItemSSID->len);
-	// set bssid
+	/* set bssid */
 	if (memcmp(param->u.wpa_associate.bssid, &abyNullAddr[0], 6) != 0)
 		memcpy(pMgmt->abyDesireBSSID, param->u.wpa_associate.bssid, 6);
 	else
@@ -800,7 +802,7 @@ static int wpa_set_associate(PSDevice pDevice,
  *
  */
 
-int wpa_ioctl(PSDevice pDevice, struct iw_point *p)
+int wpa_ioctl(struct vnt_private *pDevice, struct iw_point *p)
 {
 	struct viawget_wpa_param *param;
 	int ret = 0;
@@ -822,60 +824,60 @@ int wpa_ioctl(PSDevice pDevice, struct iw_point *p)
 	switch (param->cmd) {
 	case VIAWGET_SET_WPA:
 		ret = wpa_set_wpa(pDevice, param);
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_WPA\n");
+		pr_debug("VIAWGET_SET_WPA\n");
 		break;
 
 	case VIAWGET_SET_KEY:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_KEY\n");
+		pr_debug("VIAWGET_SET_KEY\n");
 		spin_lock_irq(&pDevice->lock);
 		ret = wpa_set_keys(pDevice, param, false);
 		spin_unlock_irq(&pDevice->lock);
 		break;
 
 	case VIAWGET_SET_SCAN:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_SCAN\n");
+		pr_debug("VIAWGET_SET_SCAN\n");
 		ret = wpa_set_scan(pDevice, param);
 		break;
 
 	case VIAWGET_GET_SCAN:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_SCAN\n");
+		pr_debug("VIAWGET_GET_SCAN\n");
 		ret = wpa_get_scan(pDevice, param);
 		wpa_ioctl = 1;
 		break;
 
 	case VIAWGET_GET_SSID:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_SSID\n");
+		pr_debug("VIAWGET_GET_SSID\n");
 		ret = wpa_get_ssid(pDevice, param);
 		wpa_ioctl = 1;
 		break;
 
 	case VIAWGET_GET_BSSID:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_GET_BSSID\n");
+		pr_debug("VIAWGET_GET_BSSID\n");
 		ret = wpa_get_bssid(pDevice, param);
 		wpa_ioctl = 1;
 		break;
 
 	case VIAWGET_SET_ASSOCIATE:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_ASSOCIATE\n");
+		pr_debug("VIAWGET_SET_ASSOCIATE\n");
 		ret = wpa_set_associate(pDevice, param);
 		break;
 
 	case VIAWGET_SET_DISASSOCIATE:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DISASSOCIATE\n");
+		pr_debug("VIAWGET_SET_DISASSOCIATE\n");
 		ret = wpa_set_disassociate(pDevice, param);
 		break;
 
 	case VIAWGET_SET_DROP_UNENCRYPT:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DROP_UNENCRYPT\n");
+		pr_debug("VIAWGET_SET_DROP_UNENCRYPT\n");
 		break;
 
 	case VIAWGET_SET_DEAUTHENTICATE:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "VIAWGET_SET_DEAUTHENTICATE\n");
+		pr_debug("VIAWGET_SET_DEAUTHENTICATE\n");
 		break;
 
 	default:
-		DBG_PRT(MSG_LEVEL_DEBUG, KERN_INFO "wpa_ioctl: unknown cmd=%d\n",
-			param->cmd);
+		pr_debug("wpa_ioctl: unknown cmd=%d\n",
+			 param->cmd);
 		ret = -EOPNOTSUPP;
 		goto out;
 	}
diff --git a/drivers/staging/vt6655/wpactl.h b/drivers/staging/vt6655/wpactl.h
index f7638baf340d..c1b4a7292061 100644
--- a/drivers/staging/vt6655/wpactl.h
+++ b/drivers/staging/vt6655/wpactl.h
@@ -57,8 +57,8 @@ enum wpa_key_mgmt { KEY_MGMT_802_1X, KEY_MGMT_CCKM, KEY_MGMT_PSK, KEY_MGMT_NONE,
 
 /*---------------------  Export Functions  --------------------------*/
 
-int wpa_set_wpadev(PSDevice pDevice, int val);
-int wpa_ioctl(PSDevice pDevice, struct iw_point *p);
-int wpa_set_keys(PSDevice pDevice, void *ctx, bool fcpfkernel);
+int wpa_set_wpadev(struct vnt_private *, int val);
+int wpa_ioctl(struct vnt_private *, struct iw_point *p);
+int wpa_set_keys(struct vnt_private *, void *ctx, bool fcpfkernel);
 
 #endif // __WPACL_H__
diff --git a/drivers/staging/vt6655/wroute.c b/drivers/staging/vt6655/wroute.c
index 4da3fef139dc..d1171fa3446e 100644
--- a/drivers/staging/vt6655/wroute.c
+++ b/drivers/staging/vt6655/wroute.c
@@ -42,8 +42,6 @@
 
 /*---------------------  Static Classes  ----------------------------*/
 
-/*---------------------  Static Variables  --------------------------*/
-static int msglevel = MSG_LEVEL_INFO;
 /*---------------------  Static Functions  --------------------------*/
 
 /*---------------------  Export Variables  --------------------------*/
@@ -62,7 +60,7 @@ static int msglevel = MSG_LEVEL_INFO;
  * Return Value: true if packet duplicate; otherwise false
  *
  */
-bool ROUTEbRelay(PSDevice pDevice, unsigned char *pbySkbData,
+bool ROUTEbRelay(struct vnt_private *pDevice, unsigned char *pbySkbData,
 		 unsigned int uDataLen, unsigned int uNodeIndex)
 {
 	PSMgmtObject    pMgmt = pDevice->pMgmt;
@@ -78,8 +76,7 @@ bool ROUTEbRelay(PSDevice pDevice, unsigned char *pbySkbData,
 	unsigned char *pbyBSSID;
 
 	if (AVAIL_TD(pDevice, TYPE_AC0DMA) <= 0) {
-		DBG_PRT(MSG_LEVEL_DEBUG,
-			KERN_INFO "Relay can't allocate TD1..\n");
+		pr_debug("Relay can't allocate TD1..\n");
 		return false;
 	}
 
@@ -102,11 +99,10 @@ bool ROUTEbRelay(PSDevice pDevice, unsigned char *pbySkbData,
 		if (KeybGetTransmitKey(&(pDevice->sKey), pbyBSSID,
 		    GROUP_KEY, &pTransmitKey) == false) {
 			pTransmitKey = NULL;
-			DBG_PRT(MSG_LEVEL_DEBUG,
-				KERN_DEBUG "KEY is NULL. [%d]\n",
-				pDevice->pMgmt->eCurrMode);
+			pr_debug("KEY is NULL. [%d]\n",
+				 pDevice->pMgmt->eCurrMode);
 		} else {
-			DBG_PRT(MSG_LEVEL_DEBUG, KERN_DEBUG "Get GTK.\n");
+			pr_debug("Get GTK\n");
 		}
 	}
 
diff --git a/drivers/staging/vt6655/wroute.h b/drivers/staging/vt6655/wroute.h
index 3abc1d36f89d..e59eec955cac 100644
--- a/drivers/staging/vt6655/wroute.h
+++ b/drivers/staging/vt6655/wroute.h
@@ -39,6 +39,7 @@
 
 /*---------------------  Export Functions  --------------------------*/
 
-bool ROUTEbRelay(PSDevice pDevice, unsigned char *pbySkbData, unsigned int uDataLen, unsigned int uNodeIndex);
+bool ROUTEbRelay(struct vnt_private *pDevice, unsigned char *pbySkbData,
+		 unsigned int uDataLen, unsigned int uNodeIndex);
 
 #endif /* __WROUTE_H__ */
diff --git a/drivers/staging/vt6656/baseband.c b/drivers/staging/vt6656/baseband.c
index e6c7b604ede9..26b16772fc40 100644
--- a/drivers/staging/vt6656/baseband.c
+++ b/drivers/staging/vt6656/baseband.c
@@ -167,20 +167,18 @@ unsigned int vnt_get_frame_time(u8 preamble_type, u8 pkt_type,
 			frame_time++;
 
 		return preamble + frame_time;
-	} else {
-		frame_time = (frame_length * 8 + 22) / rate;
-		tmp = ((frame_time * rate) - 22) / 8;
-
-		if (frame_length != tmp)
-			frame_time++;
+	}
+	frame_time = (frame_length * 8 + 22) / rate;
+	tmp = ((frame_time * rate) - 22) / 8;
 
-		frame_time = frame_time * 4;
+	if (frame_length != tmp)
+		frame_time++;
 
-		if (pkt_type != PK_TYPE_11A)
-			frame_time += 6;
+	frame_time = frame_time * 4;
 
-		return 20 + frame_time;
-	}
+	if (pkt_type != PK_TYPE_11A)
+		frame_time += 6;
+	return 20 + frame_time;
 }
 
 /*
diff --git a/drivers/staging/vt6656/card.c b/drivers/staging/vt6656/card.c
index 98567a7dc5b9..9340f1508cff 100644
--- a/drivers/staging/vt6656/card.c
+++ b/drivers/staging/vt6656/card.c
@@ -487,8 +487,7 @@ u8 vnt_get_pkt_type(struct vnt_private *priv)
 		return (u8)priv->bb_type;
 	else if (vnt_ofdm_min_rate(priv))
 		return PK_TYPE_11GA;
-	else
-		return PK_TYPE_11GB;
+	return PK_TYPE_11GB;
 }
 
 /*
diff --git a/drivers/staging/vt6656/firmware.c b/drivers/staging/vt6656/firmware.c
index eae4f32d9b66..a177645af83e 100644
--- a/drivers/staging/vt6656/firmware.c
+++ b/drivers/staging/vt6656/firmware.c
@@ -103,10 +103,7 @@ int vnt_firmware_branch_to_sram(struct vnt_private *priv)
 					0x0000,
 					0,
 					NULL);
-	if (status != STATUS_SUCCESS)
-		return false;
-	else
-		return true;
+	return status == STATUS_SUCCESS;
 }
 
 int vnt_check_firmware_version(struct vnt_private *priv)
diff --git a/drivers/staging/vt6656/key.c b/drivers/staging/vt6656/key.c
index 22f2961d2225..181745d8e250 100644
--- a/drivers/staging/vt6656/key.c
+++ b/drivers/staging/vt6656/key.c
@@ -37,17 +37,14 @@
 
 int vnt_key_init_table(struct vnt_private *priv)
 {
-	int ret;
 	u8 i;
 	u8 data[MAX_KEY_TABLE];
 
 	for (i = 0; i < MAX_KEY_TABLE; i++)
 		data[i] = i;
 
-	ret = vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY,
+	return vnt_control_out(priv, MESSAGE_TYPE_CLRKEYENTRY,
 			0, 0, ARRAY_SIZE(data), data);
-
-	return ret;
 }
 
 static int vnt_set_keymode(struct ieee80211_hw *hw, u8 *mac_addr,
diff --git a/drivers/staging/vt6656/main_usb.c b/drivers/staging/vt6656/main_usb.c
index 422fcbabafac..2fbff907ce8a 100644
--- a/drivers/staging/vt6656/main_usb.c
+++ b/drivers/staging/vt6656/main_usb.c
@@ -993,7 +993,6 @@ vt6656_probe(struct usb_interface *intf, const struct usb_device_id *id)
 		IEEE80211_HW_SIGNAL_DBM |
 		IEEE80211_HW_TIMING_BEACON_ONLY;
 
-	priv->hw->rate_control_algorithm = "pid";
 	priv->hw->max_signal = 100;
 
 	SET_IEEE80211_DEV(priv->hw, &intf->dev);
diff --git a/drivers/staging/vt6656/rf.c b/drivers/staging/vt6656/rf.c
index c0edcae55e30..c42cde59f598 100644
--- a/drivers/staging/vt6656/rf.c
+++ b/drivers/staging/vt6656/rf.c
@@ -623,7 +623,6 @@ int vnt_rf_write_embedded(struct vnt_private *priv, u32 data)
 /* Set Tx power by rate and channel number */
 int vnt_rf_setpower(struct vnt_private *priv, u32 rate, u32 channel)
 {
-	int ret = true;
 	u8 power = priv->cck_pwr;
 
 	if (channel == 0)
@@ -653,9 +652,7 @@ int vnt_rf_setpower(struct vnt_private *priv, u32 rate, u32 channel)
 		break;
 	}
 
-	ret = vnt_rf_set_txpower(priv, power, rate);
-
-	return ret;
+	return vnt_rf_set_txpower(priv, power, rate);
 }
 
 static u8 vnt_rf_addpower(struct vnt_private *priv)
diff --git a/drivers/staging/vt6656/rxtx.c b/drivers/staging/vt6656/rxtx.c
index 2d1ef88808ff..ea5140ab2b41 100644
--- a/drivers/staging/vt6656/rxtx.c
+++ b/drivers/staging/vt6656/rxtx.c
@@ -573,7 +573,7 @@ static u16 vnt_fill_cts_head(struct vnt_usb_send_context *tx_context,
 		memcpy(buf->data.ra, priv->current_net_addr, ETH_ALEN);
 
 		return vnt_rxtx_datahead_g(tx_context, &buf->data_head);
-        }
+	}
 
 	return 0;
 }
@@ -1036,6 +1036,7 @@ static int vnt_beacon_xmit(struct vnt_private *priv,
 	info = IEEE80211_SKB_CB(skb);
 	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
 		struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)mgmt_hdr;
+
 		hdr->duration_id = 0;
 		hdr->seq_ctrl = cpu_to_le16(priv->seq_counter << 4);
 	}
@@ -1082,8 +1083,6 @@ int vnt_beacon_make(struct vnt_private *priv, struct ieee80211_vif *vif)
 int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
 	struct ieee80211_bss_conf *conf)
 {
-	int ret;
-
 	vnt_mac_reg_bits_off(priv, MAC_REG_TCR, TCR_AUTOBCNTX);
 
 	vnt_mac_reg_bits_off(priv, MAC_REG_TFTCTL, TFTCTL_TSFCNTREN);
@@ -1096,7 +1095,5 @@ int vnt_beacon_enable(struct vnt_private *priv, struct ieee80211_vif *vif,
 
 	vnt_reset_next_tbtt(priv, conf->beacon_int);
 
-	ret = vnt_beacon_make(priv, vif);
-
-	return ret;
+	return vnt_beacon_make(priv, vif);
 }
diff --git a/drivers/staging/vt6656/usbpipe.c b/drivers/staging/vt6656/usbpipe.c
index cba653292996..88bf518f23eb 100644
--- a/drivers/staging/vt6656/usbpipe.c
+++ b/drivers/staging/vt6656/usbpipe.c
@@ -131,11 +131,10 @@ static void vnt_start_interrupt_urb_complete(struct urb *urb)
 	}
 
 	status = usb_submit_urb(priv->interrupt_urb, GFP_ATOMIC);
-	if (status) {
+	if (status)
 		dev_dbg(&priv->usb->dev, "Submit int URB failed %d\n", status);
-	} else {
+	else
 		priv->int_buf.in_use = true;
-	}
 }
 
 int vnt_start_interrupt_urb(struct vnt_private *priv)
diff --git a/drivers/staging/wlan-ng/cfg80211.c b/drivers/staging/wlan-ng/cfg80211.c
index 3727f6d25cf1..8942dcb44180 100644
--- a/drivers/staging/wlan-ng/cfg80211.c
+++ b/drivers/staging/wlan-ng/cfg80211.c
@@ -422,6 +422,7 @@ static int prism2_scan(struct wiphy *wiphy,
 						      IEEE80211_BAND_2GHZ);
 		bss = cfg80211_inform_bss(wiphy,
 			ieee80211_get_channel(wiphy, freq),
+			CFG80211_BSS_FTYPE_UNKNOWN,
 			(const u8 *) &(msg2.bssid.data.data),
 			msg2.timestamp.data, msg2.capinfo.data,
 			msg2.beaconperiod.data,
diff --git a/drivers/staging/wlan-ng/hfa384x_usb.c b/drivers/staging/wlan-ng/hfa384x_usb.c
index 2f63e0c6d445..898bde73c59a 100644
--- a/drivers/staging/wlan-ng/hfa384x_usb.c
+++ b/drivers/staging/wlan-ng/hfa384x_usb.c
@@ -3474,7 +3474,7 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 		/* If exclude and we receive an unencrypted, drop it */
 		if ((wlandev->hostwep & HOSTWEP_EXCLUDEUNENCRYPTED) &&
 		    !WLAN_GET_FC_ISWEP(fc)) {
-			goto done;
+			break;
 		}
 
 		data_len = le16_to_cpu(usbin->rxfrm.desc.data_len);
@@ -3528,12 +3528,8 @@ static void hfa384x_usbin_rx(wlandevice_t *wlandev, struct sk_buff *skb)
 		netdev_warn(hw->wlandev->netdev, "Received frame on unsupported port=%d\n",
 			    HFA384x_RXSTATUS_MACPORT_GET(
 				    usbin->rxfrm.desc.status));
-		goto done;
 		break;
 	}
-
-done:
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -3643,8 +3639,6 @@ static void hfa384x_int_rxmonitor(wlandevice_t *wlandev,
 
 	/* pass it back up */
 	prism2sta_ev_rx(wlandev, skb);
-
-	return;
 }
 
 /*----------------------------------------------------------------
@@ -4127,7 +4121,6 @@ static int hfa384x_isgood_pdrcode(u16 pdrcode)
 	case HFA384x_PDR_HFA3861_MANF_TESTI:
 		/* code is OK */
 		return 1;
-		break;
 	default:
 		if (pdrcode < 0x1000) {
 			/* code is OK, but we don't know exactly what it is */
@@ -4140,7 +4133,6 @@ static int hfa384x_isgood_pdrcode(u16 pdrcode)
 				 pdrcode);
 			return 0;
 		}
-		break;
 	}
 	return 0;		/* avoid compiler warnings */
 }
diff --git a/drivers/staging/wlan-ng/p80211netdev.h b/drivers/staging/wlan-ng/p80211netdev.h
index 69a4f59c8312..c547e1cb4c0d 100644
--- a/drivers/staging/wlan-ng/p80211netdev.h
+++ b/drivers/staging/wlan-ng/p80211netdev.h
@@ -180,16 +180,16 @@ typedef struct wlandevice {
 	unsigned int ethconv;
 
 	/* device methods (init by MSD, used by p80211 */
-	int (*open) (struct wlandevice *wlandev);
-	int (*close) (struct wlandevice *wlandev);
-	void (*reset) (struct wlandevice *wlandev);
-	int (*txframe) (struct wlandevice *wlandev, struct sk_buff *skb,
+	int (*open)(struct wlandevice *wlandev);
+	int (*close)(struct wlandevice *wlandev);
+	void (*reset)(struct wlandevice *wlandev);
+	int (*txframe)(struct wlandevice *wlandev, struct sk_buff *skb,
 			union p80211_hdr *p80211_hdr,
 			struct p80211_metawep *p80211_wep);
-	int (*mlmerequest) (struct wlandevice *wlandev, struct p80211msg *msg);
-	int (*set_multicast_list) (struct wlandevice *wlandev,
+	int (*mlmerequest)(struct wlandevice *wlandev, struct p80211msg *msg);
+	int (*set_multicast_list)(struct wlandevice *wlandev,
 				   netdevice_t *dev);
-	void (*tx_timeout) (struct wlandevice *wlandev);
+	void (*tx_timeout)(struct wlandevice *wlandev);
 
 	/* 802.11 State */
 	u8 bssid[WLAN_BSSID_LEN];
diff --git a/drivers/staging/wlan-ng/prism2fw.c b/drivers/staging/wlan-ng/prism2fw.c
index 42c14b0b6833..6c38f797d1ab 100644
--- a/drivers/staging/wlan-ng/prism2fw.c
+++ b/drivers/staging/wlan-ng/prism2fw.c
@@ -413,9 +413,7 @@ static int crcimage(struct imgchunk *fchunk, unsigned int nfchunks,
 				break;
 		}
 		if (c >= nfchunks) {
-			pr_err("Failed to find chunk for "
-			       "crcrec[%d], addr=0x%06x len=%d , "
-			       "aborting crc.\n",
+			pr_err("Failed to find chunk for crcrec[%d], addr=0x%06x len=%d , aborting crc.\n",
 			       i, s3crc[i].addr, s3crc[i].len);
 			return 1;
 		}
@@ -628,8 +626,8 @@ static int mkpdrlist(struct pda *pda)
 
 	}
 	if (curroff >= (HFA384x_PDA_LEN_MAX / 2)) {
-		pr_err("no end record found or invalid lengths in "
-		       "PDR data, exiting. %x %d\n", curroff, pda->nrec);
+		pr_err("no end record found or invalid lengths in PDR data, exiting. %x %d\n",
+		       curroff, pda->nrec);
 		return 1;
 	}
 	if (le16_to_cpu(pda16[curroff + 1]) == HFA384x_PDR_END_OF_PDA) {
@@ -685,8 +683,8 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 			j = -1;
 		}
 		if (j >= pda->nrec && j != -1) { /*  if no matching PDR, fail */
-			pr_warn("warning: Failed to find PDR for "
-			       "plugrec 0x%04x.\n", s3plug[i].itemcode);
+			pr_warn("warning: Failed to find PDR for plugrec 0x%04x.\n",
+				s3plug[i].itemcode);
 			continue;	/* and move on to the next PDR */
 #if 0
 			/* MSM: They swear that unless it's the MAC address,
@@ -703,8 +701,7 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 
 		/* Validate plug len against PDR len */
 		if (j != -1 && s3plug[i].len < le16_to_cpu(pda->rec[j]->len)) {
-			pr_err("error: Plug vs. PDR len mismatch for "
-			       "plugrec 0x%04x, abort plugging.\n",
+			pr_err("error: Plug vs. PDR len mismatch for plugrec 0x%04x, abort plugging.\n",
 			       s3plug[i].itemcode);
 			result = 1;
 			continue;
@@ -718,8 +715,8 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 				break;
 		}
 		if (c >= nfchunks) {
-			pr_err("error: Failed to find image chunk for "
-			       "plugrec 0x%04x.\n", s3plug[i].itemcode);
+			pr_err("error: Failed to find image chunk for plugrec 0x%04x.\n",
+			       s3plug[i].itemcode);
 			result = 1;
 			continue;
 		}
@@ -727,8 +724,7 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 		/* Plug data */
 		chunkoff = pstart - cstart;
 		dest = fchunk[c].data + chunkoff;
-		pr_debug("Plugging item 0x%04x @ 0x%06x, len=%d, "
-			 "cnum=%d coff=0x%06x\n",
+		pr_debug("Plugging item 0x%04x @ 0x%06x, len=%d, cnum=%d coff=0x%06x\n",
 			 s3plug[i].itemcode, pstart, s3plug[i].len,
 			 c, chunkoff);
 
@@ -764,30 +760,35 @@ static int plugimage(struct imgchunk *fchunk, unsigned int nfchunks,
 static int read_cardpda(struct pda *pda, wlandevice_t *wlandev)
 {
 	int result = 0;
-	struct p80211msg_p2req_readpda msg;
+	struct p80211msg_p2req_readpda *msg;
+
+	msg = kzalloc(sizeof(*msg), GFP_KERNEL);
+	if (!msg)
+		return -ENOMEM;
 
 	/* set up the msg */
-	msg.msgcode = DIDmsg_p2req_readpda;
-	msg.msglen = sizeof(msg);
-	strcpy(msg.devname, wlandev->name);
-	msg.pda.did = DIDmsg_p2req_readpda_pda;
-	msg.pda.len = HFA384x_PDA_LEN_MAX;
-	msg.pda.status = P80211ENUM_msgitem_status_no_value;
-	msg.resultcode.did = DIDmsg_p2req_readpda_resultcode;
-	msg.resultcode.len = sizeof(u32);
-	msg.resultcode.status = P80211ENUM_msgitem_status_no_value;
-
-	if (prism2mgmt_readpda(wlandev, &msg) != 0) {
+	msg->msgcode = DIDmsg_p2req_readpda;
+	msg->msglen = sizeof(msg);
+	strcpy(msg->devname, wlandev->name);
+	msg->pda.did = DIDmsg_p2req_readpda_pda;
+	msg->pda.len = HFA384x_PDA_LEN_MAX;
+	msg->pda.status = P80211ENUM_msgitem_status_no_value;
+	msg->resultcode.did = DIDmsg_p2req_readpda_resultcode;
+	msg->resultcode.len = sizeof(u32);
+	msg->resultcode.status = P80211ENUM_msgitem_status_no_value;
+
+	if (prism2mgmt_readpda(wlandev, msg) != 0) {
 		/* prism2mgmt_readpda prints an errno if appropriate */
 		result = -1;
-	} else if (msg.resultcode.data == P80211ENUM_resultcode_success) {
-		memcpy(pda->buf, msg.pda.data, HFA384x_PDA_LEN_MAX);
+	} else if (msg->resultcode.data == P80211ENUM_resultcode_success) {
+		memcpy(pda->buf, msg->pda.data, HFA384x_PDA_LEN_MAX);
 		result = mkpdrlist(pda);
 	} else {
 		/* resultcode must've been something other than success */
 		result = -1;
 	}
 
+	kfree(msg);
 	return result;
 }
 
@@ -876,8 +877,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 		switch (addr) {
 		case S3ADDR_START:
 			startaddr = *ptr32;
-			pr_debug("  S7 start addr, record=%d "
-				      " addr=0x%08x\n",
+			pr_debug("  S7 start addr, record=%d addr=0x%08x\n",
 				      rcnt,
 				      startaddr);
 			break;
@@ -886,8 +886,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 			s3plug[ns3plug].addr = *(ptr32 + 1);
 			s3plug[ns3plug].len = *(ptr32 + 2);
 
-			pr_debug("  S3 plugrec, record=%d "
-				      "itemcode=0x%08x addr=0x%08x len=%d\n",
+			pr_debug("  S3 plugrec, record=%d itemcode=0x%08x addr=0x%08x len=%d\n",
 				      rcnt,
 				      s3plug[ns3plug].itemcode,
 				      s3plug[ns3plug].addr,
@@ -904,8 +903,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 			s3crc[ns3crc].len = *(ptr32 + 1);
 			s3crc[ns3crc].dowrite = *(ptr32 + 2);
 
-			pr_debug("  S3 crcrec, record=%d "
-				      "addr=0x%08x len=%d write=0x%08x\n",
+			pr_debug("  S3 crcrec, record=%d addr=0x%08x len=%d write=0x%08x\n",
 				      rcnt,
 				      s3crc[ns3crc].addr,
 				      s3crc[ns3crc].len,
@@ -920,8 +918,7 @@ static int read_fwfile(const struct ihex_binrec *record)
 			s3info[ns3info].len = *ptr16;
 			s3info[ns3info].type = *(ptr16 + 1);
 
-			pr_debug("  S3 inforec, record=%d "
-			      "len=0x%04x type=0x%04x\n",
+			pr_debug("  S3 inforec, record=%d len=0x%04x type=0x%04x\n",
 				      rcnt,
 				      s3info[ns3info].len,
 				      s3info[ns3info].type);
@@ -995,8 +992,7 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 		kfree(rstmsg);
 		kfree(rwrmsg);
 		netdev_err(wlandev->netdev,
-			   "writeimage: no memory for firmware download, "
-			   "aborting download\n");
+			   "writeimage: no memory for firmware download, aborting download\n");
 		return -ENOMEM;
 	}
 
@@ -1040,15 +1036,15 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
 		netdev_err(wlandev->netdev,
-			   "writeimage state enable failed w/ result=%d, "
-			   "aborting download\n", result);
+			   "writeimage state enable failed w/ result=%d, aborting download\n",
+			   result);
 		goto free_result;
 	}
 	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
 		netdev_err(wlandev->netdev,
-			   "writeimage()->xxxdl_state msg indicates failure, "
-			   "w/ resultcode=%d, aborting download.\n", resultcode);
+			   "writeimage()->xxxdl_state msg indicates failure, w/ resultcode=%d, aborting download.\n",
+			   resultcode);
 		result = 1;
 		goto free_result;
 	}
@@ -1084,14 +1080,13 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 			/* Check the results */
 			if (result) {
 				netdev_err(wlandev->netdev,
-					   "writeimage chunk write failed w/ "
-					   "result=%d, aborting download\n", result);
+					   "writeimage chunk write failed w/ result=%d, aborting download\n",
+					   result);
 				goto free_result;
 			}
 			resultcode = rstmsg->resultcode.data;
 			if (resultcode != P80211ENUM_resultcode_success) {
-				pr_err("writeimage()->xxxdl_write msg indicates failure, "
-				       "w/ resultcode=%d, aborting download.\n",
+				pr_err("writeimage()->xxxdl_write msg indicates failure, w/ resultcode=%d, aborting download.\n",
 				       resultcode);
 				result = 1;
 				goto free_result;
@@ -1108,15 +1103,15 @@ static int writeimage(wlandevice_t *wlandev, struct imgchunk *fchunk,
 	result = prism2mgmt_ramdl_state(wlandev, rstmsg);
 	if (result) {
 		netdev_err(wlandev->netdev,
-			   "writeimage state disable failed w/ result=%d, "
-			   "aborting download\n", result);
+			   "writeimage state disable failed w/ result=%d, aborting download\n",
+			   result);
 		goto free_result;
 	}
 	resultcode = rstmsg->resultcode.data;
 	if (resultcode != P80211ENUM_resultcode_success) {
 		netdev_err(wlandev->netdev,
-			   "writeimage()->xxxdl_state msg indicates failure, "
-			   "w/ resultcode=%d, aborting download.\n", resultcode);
+			   "writeimage()->xxxdl_state msg indicates failure, w/ resultcode=%d, aborting download.\n",
+			   resultcode);
 		result = 1;
 		goto free_result;
 	}
diff --git a/drivers/staging/wlan-ng/prism2mgmt.c b/drivers/staging/wlan-ng/prism2mgmt.c
index e6a82d3303c1..013a6240f193 100644
--- a/drivers/staging/wlan-ng/prism2mgmt.c
+++ b/drivers/staging/wlan-ng/prism2mgmt.c
@@ -1107,8 +1107,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 		if (wlandev->netdev->type == ARPHRD_ETHER) {
 			msg->resultcode.data =
 			    P80211ENUM_resultcode_invalid_parameters;
-			result = 0;
-			goto exit;
+			return 0;
 		}
 		/* Disable monitor mode */
 		result = hfa384x_cmd_monitor(hw, HFA384x_MONITOR_DISABLE);
@@ -1166,9 +1165,7 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 
 		netdev_info(wlandev->netdev, "monitor mode disabled\n");
 		msg->resultcode.data = P80211ENUM_resultcode_success;
-		result = 0;
-		goto exit;
-		break;
+		return 0;
 	case P80211ENUM_truth_true:
 		/* Disable the port (if enabled), only check Port 0 */
 		if (hw->port_enabled[0]) {
@@ -1313,19 +1310,13 @@ int prism2mgmt_wlansniff(wlandevice_t *wlandev, void *msgp)
 		}
 
 		msg->resultcode.data = P80211ENUM_resultcode_success;
-		result = 0;
-		goto exit;
-		break;
+		return 0;
 	default:
 		msg->resultcode.data = P80211ENUM_resultcode_invalid_parameters;
-		result = 0;
-		goto exit;
-		break;
+		return 0;
 	}
 
 failed:
 	msg->resultcode.data = P80211ENUM_resultcode_refused;
-	result = 0;
-exit:
-	return result;
+	return 0;
 }
diff --git a/drivers/staging/wlan-ng/prism2mib.c b/drivers/staging/wlan-ng/prism2mib.c
index f4717089a80c..0163e062b650 100644
--- a/drivers/staging/wlan-ng/prism2mib.c
+++ b/drivers/staging/wlan-ng/prism2mib.c
@@ -717,8 +717,6 @@ static int prism2mib_priv(struct mibrec *mib,
 {
 	p80211pstrd_t *pstr = (p80211pstrd_t *) data;
 
-	int result;
-
 	switch (mib->did) {
 	case DIDmib_lnx_lnxConfigTable_lnxRSNAIE:{
 			hfa384x_WPAData_t wpa;
@@ -734,11 +732,10 @@ static int prism2mib_priv(struct mibrec *mib,
 				wpa.datalen = cpu_to_le16(pstr->len);
 				memcpy(wpa.data, pstr->data, pstr->len);
 
-				result =
-				    hfa384x_drvr_setconfig(hw,
-						   HFA384x_RID_CNFWPADATA,
-						   (u8 *) &wpa,
-						   sizeof(wpa));
+				hfa384x_drvr_setconfig(hw,
+						       HFA384x_RID_CNFWPADATA,
+						       (u8 *) &wpa,
+						       sizeof(wpa));
 			}
 			break;
 		}
diff --git a/drivers/staging/wlan-ng/prism2sta.c b/drivers/staging/wlan-ng/prism2sta.c
index 799ce8aa70ef..df577dfe7ffb 100644
--- a/drivers/staging/wlan-ng/prism2sta.c
+++ b/drivers/staging/wlan-ng/prism2sta.c
@@ -60,7 +60,6 @@
 #include <linux/netdevice.h>
 #include <linux/workqueue.h>
 #include <linux/byteorder/generic.h>
-#include <linux/ctype.h>
 
 #include <linux/io.h>
 #include <linux/delay.h>
@@ -81,27 +80,6 @@
 #include "hfa384x.h"
 #include "prism2mgmt.h"
 
-/* Create a string of printable chars from something that might not be */
-/* It's recommended that the str be 4*len + 1 bytes long */
-#define wlan_mkprintstr(buf, buflen, str, strlen) \
-{ \
-	int i = 0; \
-	int j = 0; \
-	memset(str, 0, (strlen)); \
-	for (i = 0; i < (buflen); i++) { \
-		if (isprint((buf)[i])) { \
-			(str)[j] = (buf)[i]; \
-			j++; \
-		} else { \
-			(str)[j] = '\\'; \
-			(str)[j+1] = 'x'; \
-			(str)[j+2] = hex_asc_hi((buf)[i]); \
-			(str)[j+3] = hex_asc_lo((buf)[i]); \
-			j += 4; \
-		} \
-	} \
-}
-
 static char *dev_info = "prism2_usb";
 static wlandevice_t *create_wlan(void);
 
@@ -607,7 +585,6 @@ static int prism2sta_getcardinfo(wlandevice_t *wlandev)
 	hfa384x_t *hw = (hfa384x_t *) wlandev->priv;
 	u16 temp;
 	u8 snum[HFA384x_RID_NICSERIALNUMBER_LEN];
-	char pstr[(HFA384x_RID_NICSERIALNUMBER_LEN * 4) + 1];
 
 	/* Collect version and compatibility info */
 	/*  Some are critical, some are not */
@@ -862,9 +839,8 @@ static int prism2sta_getcardinfo(wlandevice_t *wlandev)
 	result = hfa384x_drvr_getconfig(hw, HFA384x_RID_NICSERIALNUMBER,
 					snum, HFA384x_RID_NICSERIALNUMBER_LEN);
 	if (!result) {
-		wlan_mkprintstr(snum, HFA384x_RID_NICSERIALNUMBER_LEN,
-				pstr, sizeof(pstr));
-		netdev_info(wlandev->netdev, "Prism2 card SN: %s\n", pstr);
+		netdev_info(wlandev->netdev, "Prism2 card SN: %*pEhp\n",
+			    HFA384x_RID_NICSERIALNUMBER_LEN, snum);
 	} else {
 		netdev_err(wlandev->netdev, "Failed to retrieve Prism2 Card SN\n");
 		goto failed;
diff --git a/drivers/staging/xgifb/XGI_main_26.c b/drivers/staging/xgifb/XGI_main_26.c
index f82f0574bbdc..be7778b59118 100644
--- a/drivers/staging/xgifb/XGI_main_26.c
+++ b/drivers/staging/xgifb/XGI_main_26.c
@@ -588,13 +588,11 @@ static u8 XGIfb_search_refresh_rate(struct xgifb_video_info *xgifb_info,
 		}
 		i++;
 	}
-	if (xgifb_info->rate_idx > 0) {
+	if (xgifb_info->rate_idx > 0)
 		return xgifb_info->rate_idx;
-	} else {
-		pr_info("Unsupported rate %d for %dx%d\n",
-		       rate, xres, yres);
-		return 0;
-	}
+	pr_info("Unsupported rate %d for %dx%d\n",
+		rate, xres, yres);
+	return 0;
 }
 
 static void XGIfb_search_tvstd(const char *name)
diff --git a/drivers/staging/xgifb/vb_init.c b/drivers/staging/xgifb/vb_init.c
index ff210ddcbf9e..2b233af8eb80 100644
--- a/drivers/staging/xgifb/vb_init.c
+++ b/drivers/staging/xgifb/vb_init.c
@@ -63,14 +63,13 @@ XGINew_GetXG20DRAMType(struct xgi_hw_device_info *HwDeviceExtension,
 		/* ~HOTPLUG_SUPPORT */
 		xgifb_reg_or(pVBInfo->P3d4, 0xB4, 0x02);
 		return data;
-	} else {
-		data = xgifb_reg_get(pVBInfo->P3d4, 0x97) & 0x01;
+	}
+	data = xgifb_reg_get(pVBInfo->P3d4, 0x97) & 0x01;
 
-		if (data == 1)
-			data++;
+	if (data == 1)
+		data++;
 
-		return data;
-	}
+	return data;
 }
 
 static void XGINew_DDR1x_MRS_340(unsigned long P3c4,
@@ -578,9 +577,8 @@ static unsigned char XGINew_CheckFrequence(struct vb_device_info *pVBInfo)
 		data = xgifb_reg_get(pVBInfo->P3c4, 0x39);
 		data = (data & 0x02) >> 1;
 		return data;
-	} else {
-		return data & 0x01;
 	}
+	return data & 0x01;
 }
 
 static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
@@ -637,10 +635,9 @@ static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
 
 				if (XGINew_ReadWriteRest(23, 22, pVBInfo) == 1)
 					return;
-				else
-					xgifb_reg_set(pVBInfo->P3c4,
-						      0x13,
-						      0x31);
+				xgifb_reg_set(pVBInfo->P3c4,
+					      0x13,
+					      0x31);
 				udelay(15);
 			}
 
@@ -687,10 +684,11 @@ static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
 
 				if (XGINew_ReadWriteRest(22, 21, pVBInfo) == 1)
 					return;
-				else /* (0x31:12x8x2) 22bit + 1 rank */
-					xgifb_reg_set(pVBInfo->P3c4,
-						      0x13,
-						      0x31);
+
+				/* (0x31:12x8x2) 22bit + 1 rank */
+				xgifb_reg_set(pVBInfo->P3c4,
+					      0x13,
+					      0x31);
 				udelay(15);
 			}
 		}
@@ -729,10 +727,8 @@ static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
 
 			if (XGINew_ReadWriteRest(23, 22, pVBInfo) == 1)
 				return;
-			else {
-				xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
-				xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x30);
-			}
+			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
+			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x30);
 		} else { /* DDR */
 			pVBInfo->ram_bus = 64; /* 64 bits */
 			pVBInfo->ram_channel = 1; /* 1 channels */
@@ -741,10 +737,8 @@ static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
 
 			if (XGINew_ReadWriteRest(24, 23, pVBInfo) == 1)
 				return;
-			else {
-				xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
-				xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x42);
-			}
+			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
+			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x42);
 		}
 
 		break;
@@ -781,12 +775,10 @@ static void XGINew_CheckChannel(struct xgi_hw_device_info *HwDeviceExtension,
 			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0xA1);
 			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x5A);
 
-			if (XGINew_ReadWriteRest(25, 24, pVBInfo) == 1) {
+			if (XGINew_ReadWriteRest(25, 24, pVBInfo) == 1)
 				return;
-			} else {
-				xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
-				xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x4A);
-			}
+			xgifb_reg_set(pVBInfo->P3c4, 0x13, 0x21);
+			xgifb_reg_set(pVBInfo->P3c4, 0x14, 0x4A);
 		}
 		break;
 	}
diff --git a/drivers/staging/xgifb/vb_setmode.c b/drivers/staging/xgifb/vb_setmode.c
index 8795e173cb5a..d5f49d2a8db3 100644
--- a/drivers/staging/xgifb/vb_setmode.c
+++ b/drivers/staging/xgifb/vb_setmode.c
@@ -3895,8 +3895,7 @@ static struct XGI301C_Tap4TimingStruct const
 
 	if (tempax <= tempbx)
 		return &xgifb_tap4_timing[0];
-	else
-		Tap4TimingPtr = xgifb_ntsc_525_tap4_timing; /* NTSC */
+	Tap4TimingPtr = xgifb_ntsc_525_tap4_timing; /* NTSC */
 
 	if (pVBInfo->TVInfo & TVSetPAL)
 		Tap4TimingPtr = PALTap4Timing;
diff --git a/drivers/staging/xillybus/README b/drivers/staging/xillybus/README
deleted file mode 100644
index 81d111b4dc28..000000000000
--- a/drivers/staging/xillybus/README
+++ /dev/null
@@ -1,380 +0,0 @@
-
-               ==========================================
-               Xillybus driver for generic FPGA interface
-               ==========================================
-
-Author: Eli Billauer, Xillybus Ltd. (http://xillybus.com)
-Email:  eli.billauer@gmail.com or as advertised on Xillybus' site.
-
-Contents:
-
- - Introduction
-  -- Background
-  -- Xillybus Overview
-
- - Usage
-  -- User interface
-  -- Synchronization
-  -- Seekable pipes
-
-- Internals
-  -- Source code organization
-  -- Pipe attributes
-  -- Host never reads from the FPGA
-  -- Channels, pipes, and the message channel
-  -- Data streaming
-  -- Data granularity
-  -- Probing
-  -- Buffer allocation
-  -- The "nonempty" message (supporting poll)
-
-
-INTRODUCTION
-============
-
-Background
-----------
-
-An FPGA (Field Programmable Gate Array) is a piece of logic hardware, which
-can be programmed to become virtually anything that is usually found as a
-dedicated chipset: For instance, a display adapter, network interface card,
-or even a processor with its peripherals. FPGAs are the LEGO of hardware:
-Based upon certain building blocks, you make your own toys the way you like
-them. It's usually pointless to reimplement something that is already
-available on the market as a chipset, so FPGAs are mostly used when some
-special functionality is needed, and the production volume is relatively low
-(hence not justifying the development of an ASIC).
-
-The challenge with FPGAs is that everything is implemented at a very low
-level, even lower than assembly language. In order to allow FPGA designers to
-focus on their specific project, and not reinvent the wheel over and over
-again, pre-designed building blocks, IP cores, are often used. These are the
-FPGA parallels of library functions. IP cores may implement certain
-mathematical functions, a functional unit (e.g. a USB interface), an entire
-processor (e.g. ARM) or anything that might come handy. Think of them as a
-building block, with electrical wires dangling on the sides for connection to
-other blocks.
-
-One of the daunting tasks in FPGA design is communicating with a fullblown
-operating system (actually, with the processor running it): Implementing the
-low-level bus protocol and the somewhat higher-level interface with the host
-(registers, interrupts, DMA etc.) is a project in itself. When the FPGA's
-function is a well-known one (e.g. a video adapter card, or a NIC), it can
-make sense to design the FPGA's interface logic specifically for the project.
-A special driver is then written to present the FPGA as a well-known interface
-to the kernel and/or user space. In that case, there is no reason to treat the
-FPGA differently than any device on the bus.
-
-It's however common that the desired data communication doesn't fit any well-
-known peripheral function. Also, the effort of designing an elegant
-abstraction for the data exchange is often considered too big. In those cases,
-a quicker and possibly less elegant solution is sought: The driver is
-effectively written as a user space program, leaving the kernel space part
-with just elementary data transport. This still requires designing some
-interface logic for the FPGA, and write a simple ad-hoc driver for the kernel.
-
-Xillybus Overview
------------------
-
-Xillybus is an IP core and a Linux driver. Together, they form a kit for
-elementary data transport between an FPGA and the host, providing pipe-like
-data streams with a straightforward user interface. It's intended as a low-
-effort solution for mixed FPGA-host projects, for which it makes sense to
-have the project-specific part of the driver running in a user-space program.
-
-Since the communication requirements may vary significantly from one FPGA
-project to another (the number of data pipes needed in each direction and
-their attributes), there isn't one specific chunk of logic being the Xillybus
-IP core. Rather, the IP core is configured and built based upon a
-specification given by its end user.
-
-Xillybus presents independent data streams, which resemble pipes or TCP/IP
-communication to the user. At the host side, a character device file is used
-just like any pipe file. On the FPGA side, hardware FIFOs are used to stream
-the data. This is contrary to a common method of communicating through fixed-
-sized buffers (even though such buffers are used by Xillybus under the hood).
-There may be more than a hundred of these streams on a single IP core, but
-also no more than one, depending on the configuration.
-
-In order to ease the deployment of the Xillybus IP core, it contains a simple
-data structure which completely defines the core's configuration. The Linux
-driver fetches this data structure during its initialization process, and sets
-up the DMA buffers and character devices accordingly. As a result, a single
-driver is used to work out of the box with any Xillybus IP core.
-
-The data structure just mentioned should not be confused with PCI's
-configuration space or the Flattened Device Tree.
-
-USAGE
-=====
-
-User interface
---------------
-
-On the host, all interface with Xillybus is done through /dev/xillybus_*
-device files, which are generated automatically as the drivers loads. The
-names of these files depend on the IP core that is loaded in the FPGA (see
-Probing below). To communicate with the FPGA, open the device file that
-corresponds to the hardware FIFO you want to send data or receive data from,
-and use plain write() or read() calls, just like with a regular pipe. In
-particular, it makes perfect sense to go:
-
-$ cat mydata > /dev/xillybus_thisfifo
-
-$ cat /dev/xillybus_thatfifo > hisdata
-
-possibly pressing CTRL-C as some stage, even though the xillybus_* pipes have
-the capability to send an EOF (but may not use it).
-
-The driver and hardware are designed to behave sensibly as pipes, including:
-
-* Supporting non-blocking I/O (by setting O_NONBLOCK on open() ).
-
-* Supporting poll() and select().
-
-* Being bandwidth efficient under load (using DMA) but also handle small
-  pieces of data sent across (like TCP/IP) by autoflushing.
-
-A device file can be read only, write only or bidirectional. Bidirectional
-device files are treated like two independent pipes (except for sharing a
-"channel" structure in the implementation code).
-
-Synchronization
----------------
-
-Xillybus pipes are configured (on the IP core) to be either synchronous or
-asynchronous. For a synchronous pipe, write() returns successfully only after
-some data has been submitted and acknowledged by the FPGA. This slows down
-bulk data transfers, and is nearly impossible for use with streams that
-require data at a constant rate: There is no data transmitted to the FPGA
-between write() calls, in particular when the process loses the CPU.
-
-When a pipe is configured asynchronous, write() returns if there was enough
-room in the buffers to store any of the data in the buffers.
-
-For FPGA to host pipes, asynchronous pipes allow data transfer from the FPGA
-as soon as the respective device file is opened, regardless of if the data
-has been requested by a read() call. On synchronous pipes, only the amount
-of data requested by a read() call is transmitted.
-
-In summary, for synchronous pipes, data between the host and FPGA is
-transmitted only to satisfy the read() or write() call currently handled
-by the driver, and those calls wait for the transmission to complete before
-returning.
-
-Note that the synchronization attribute has nothing to do with the possibility
-that read() or write() completes less bytes than requested. There is a
-separate configuration flag ("allowpartial") that determines whether such a
-partial completion is allowed.
-
-Seekable pipes
---------------
-
-A synchronous pipe can be configured to have the stream's position exposed
-to the user logic at the FPGA. Such a pipe is also seekable on the host API.
-With this feature, a memory or register interface can be attached on the
-FPGA side to the seekable stream. Reading or writing to a certain address in
-the attached memory is done by seeking to the desired address, and calling
-read() or write() as required.
-
-
-INTERNALS
-=========
-
-Source code organization
-------------------------
-
-The Xillybus driver consists of a core module, xillybus_core.c, and modules
-that depend on the specific bus interface (xillybus_of.c and xillybus_pcie.c).
-
-The bus specific modules are those probed when a suitable device is found by
-the kernel. Since the DMA mapping and synchronization functions, which are bus
-dependent by their nature, are used by the core module, a
-xilly_endpoint_hardware structure is passed to the core module on
-initialization. This structure is populated with pointers to wrapper functions
-which execute the DMA-related operations on the bus.
-
-Pipe attributes
----------------
-
-Each pipe has a number of attributes which are set when the FPGA component
-(IP core) is built. They are fetched from the IDT (the data structure which
-defines the core's configuration, see Probing below) by xilly_setupchannels()
-in xillybus_core.c as follows:
-
-* is_writebuf: The pipe's direction. A non-zero value means it's an FPGA to
-  host pipe (the FPGA "writes").
-
-* channelnum: The pipe's identification number in communication between the
-  host and FPGA.
-
-* format: The underlying data width. See Data Granularity below.
-
-* allowpartial: A non-zero value means that a read() or write() (whichever
-  applies) may return with less than the requested number of bytes. The common
-  choice is a non-zero value, to match standard UNIX behavior.
-
-* synchronous: A non-zero value means that the pipe is synchronous. See
-  Syncronization above.
-
-* bufsize: Each DMA buffer's size. Always a power of two.
-
-* bufnum: The number of buffers allocated for this pipe. Always a power of two.
-
-* exclusive_open: A non-zero value forces exclusive opening of the associated
-  device file. If the device file is bidirectional, and already opened only in
-  one direction, the opposite direction may be opened once.
-
-* seekable: A non-zero value indicates that the pipe is seekable. See
-  Seekable pipes above.
-
-* supports_nonempty: A non-zero value (which is typical) indicates that the
-  hardware will send the messages that are necessary to support select() and
-  poll() for this pipe.
-
-Host never reads from the FPGA
-------------------------------
-
-Even though PCI Express is hotpluggable in general, a typical motherboard
-doesn't expect a card to go away all of the sudden. But since the PCIe card
-is based upon reprogrammable logic, a sudden disappearance from the bus is
-quite likely as a result of an accidental reprogramming of the FPGA while the
-host is up. In practice, nothing happens immediately in such a situation. But
-if the host attempts to read from an address that is mapped to the PCI Express
-device, that leads to an immediate freeze of the system on some motherboards,
-even though the PCIe standard requires a graceful recovery.
-
-In order to avoid these freezes, the Xillybus driver refrains completely from
-reading from the device's register space. All communication from the FPGA to
-the host is done through DMA. In particular, the Interrupt Service Routine
-doesn't follow the common practice of checking a status register when it's
-invoked. Rather, the FPGA prepares a small buffer which contains short
-messages, which inform the host what the interrupt was about.
-
-This mechanism is used on non-PCIe buses as well for the sake of uniformity.
-
-
-Channels, pipes, and the message channel
-----------------------------------------
-
-Each of the (possibly bidirectional) pipes presented to the user is allocated
-a data channel between the FPGA and the host. The distinction between channels
-and pipes is necessary only because of channel 0, which is used for interrupt-
-related messages from the FPGA, and has no pipe attached to it.
-
-Data streaming
---------------
-
-Even though a non-segmented data stream is presented to the user at both
-sides, the implementation relies on a set of DMA buffers which is allocated
-for each channel. For the sake of illustration, let's take the FPGA to host
-direction: As data streams into the respective channel's interface in the
-FPGA, the Xillybus IP core writes it to one of the DMA buffers. When the
-buffer is full, the FPGA informs the host about that (appending a
-XILLYMSG_OPCODE_RELEASEBUF message channel 0 and sending an interrupt if
-necessary). The host responds by making the data available for reading through
-the character device. When all data has been read, the host writes on the
-the FPGA's buffer control register, allowing the buffer's overwriting. Flow
-control mechanisms exist on both sides to prevent underflows and overflows.
-
-This is not good enough for creating a TCP/IP-like stream: If the data flow
-stops momentarily before a DMA buffer is filled, the intuitive expectation is
-that the partial data in buffer will arrive anyhow, despite the buffer not
-being completed. This is implemented by adding a field in the
-XILLYMSG_OPCODE_RELEASEBUF message, through which the FPGA informs not just
-which buffer is submitted, but how much data it contains.
-
-But the FPGA will submit a partially filled buffer only if directed to do so
-by the host. This situation occurs when the read() method has been blocking
-for XILLY_RX_TIMEOUT jiffies (currently 10 ms), after which the host commands
-the FPGA to submit a DMA buffer as soon as it can. This timeout mechanism
-balances between bus bandwidth efficiency (preventing a lot of partially
-filled buffers being sent) and a latency held fairly low for tails of data.
-
-A similar setting is used in the host to FPGA direction. The handling of
-partial DMA buffers is somewhat different, though. The user can tell the
-driver to submit all data it has in the buffers to the FPGA, by issuing a
-write() with the byte count set to zero. This is similar to a flush request,
-but it doesn't block. There is also an autoflushing mechanism, which triggers
-an equivalent flush roughly XILLY_RX_TIMEOUT jiffies after the last write().
-This allows the user to be oblivious about the underlying buffering mechanism
-and yet enjoy a stream-like interface.
-
-Note that the issue of partial buffer flushing is irrelevant for pipes having
-the "synchronous" attribute nonzero, since synchronous pipes don't allow data
-to lay around in the DMA buffers between read() and write() anyhow.
-
-Data granularity
-----------------
-
-The data arrives or is sent at the FPGA as 8, 16 or 32 bit wide words, as
-configured by the "format" attribute. Whenever possible, the driver attempts
-to hide this when the pipe is accessed differently from its natural alignment.
-For example, reading single bytes from a pipe with 32 bit granularity works
-with no issues. Writing single bytes to pipes with 16 or 32 bit granularity
-will also work, but the driver can't send partially completed words to the
-FPGA, so the transmission of up to one word may be held until it's fully
-occupied with user data.
-
-This somewhat complicates the handling of host to FPGA streams, because
-when a buffer is flushed, it may contain up to 3 bytes don't form a word in
-the FPGA, and hence can't be sent. To prevent loss of data, these leftover
-bytes need to be moved to the next buffer. The parts in xillybus_core.c
-that mention "leftovers" in some way are related to this complication.
-
-Probing
--------
-
-As mentioned earlier, the number of pipes that are created when the driver
-loads and their attributes depend on the Xillybus IP core in the FPGA. During
-the driver's initialization, a blob containing configuration info, the
-Interface Description Table (IDT), is sent from the FPGA to the host. The
-bootstrap process is done in three phases:
-
-1. Acquire the length of the IDT, so a buffer can be allocated for it. This
-   is done by sending a quiesce command to the device, since the acknowledge
-   for this command contains the IDT's buffer length.
-
-2. Acquire the IDT itself.
-
-3. Create the interfaces according to the IDT.
-
-Buffer allocation
------------------
-
-In order to simplify the logic that prevents illegal boundary crossings of
-PCIe packets, the following rule applies: If a buffer is smaller than 4kB,
-it must not cross a 4kB boundary. Otherwise, it must be 4kB aligned. The
-xilly_setupchannels() functions allocates these buffers by requesting whole
-pages from the kernel, and diving them into DMA buffers as necessary. Since
-all buffers' sizes are powers of two, it's possible to pack any set of such
-buffers, with a maximal waste of one page of memory.
-
-All buffers are allocated when the driver is loaded. This is necessary,
-since large continuous physical memory segments are sometimes requested,
-which are more likely to be available when the system is freshly booted.
-
-The allocation of buffer memory takes place in the same order they appear in
-the IDT. The driver relies on a rule that the pipes are sorted with decreasing
-buffer size in the IDT. If a requested buffer is larger or equal to a page,
-the necessary number of pages is requested from the kernel, and these are
-used for this buffer. If the requested buffer is smaller than a page, one
-single page is requested from the kernel, and that page is partially used.
-Or, if there already is a partially used page at hand, the buffer is packed
-into that page. It can be shown that all pages requested from the kernel
-(except possibly for the last) are 100% utilized this way.
-
-The "nonempty" message (supporting poll)
----------------------------------------
-
-In order to support the "poll" method (and hence select() ), there is a small
-catch regarding the FPGA to host direction: The FPGA may have filled a DMA
-buffer with some data, but not submitted that buffer. If the host waited for
-the buffer's submission by the FPGA, there would be a possibility that the
-FPGA side has sent data, but a select() call would still block, because the
-host has not received any notification about this. This is solved with
-XILLYMSG_OPCODE_NONEMPTY messages sent by the FPGA when a channel goes from
-completely empty to containing some data.
-
-These messages are used only to support poll() and select(). The IP core can
-be configured not to send them for a slight reduction of bandwidth.
diff --git a/drivers/staging/xillybus/TODO b/drivers/staging/xillybus/TODO
deleted file mode 100644
index 95cfe2f62fcd..000000000000
--- a/drivers/staging/xillybus/TODO
+++ /dev/null
@@ -1,5 +0,0 @@
-TODO:
-- have the driver reviewed
-
-Please send any patches and/or comments to Eli Billauer,
-<eli.billauer@gmail.com>.
diff --git a/drivers/target/Kconfig b/drivers/target/Kconfig
index dc2d84ac5a0e..81d44c477a5b 100644
--- a/drivers/target/Kconfig
+++ b/drivers/target/Kconfig
@@ -31,6 +31,13 @@ config TCM_PSCSI
 	Say Y here to enable the TCM/pSCSI subsystem plugin for non-buffered
 	passthrough access to Linux/SCSI device
 
+config TCM_USER
+	tristate "TCM/USER Subsystem Plugin for Linux"
+	depends on UIO && NET
+	help
+	Say Y here to enable the TCM/USER subsystem plugin for a userspace
+	process to handle requests
+
 source "drivers/target/loopback/Kconfig"
 source "drivers/target/tcm_fc/Kconfig"
 source "drivers/target/iscsi/Kconfig"
diff --git a/drivers/target/Makefile b/drivers/target/Makefile
index 85b012d2f89b..bbb4a7d638ef 100644
--- a/drivers/target/Makefile
+++ b/drivers/target/Makefile
@@ -22,6 +22,7 @@ obj-$(CONFIG_TARGET_CORE)	+= target_core_mod.o
 obj-$(CONFIG_TCM_IBLOCK)	+= target_core_iblock.o
 obj-$(CONFIG_TCM_FILEIO)	+= target_core_file.o
 obj-$(CONFIG_TCM_PSCSI)		+= target_core_pscsi.o
+obj-$(CONFIG_TCM_USER)		+= target_core_user.o
 
 # Fabric modules
 obj-$(CONFIG_LOOPBACK_TARGET)	+= loopback/
diff --git a/drivers/target/iscsi/iscsi_target.c b/drivers/target/iscsi/iscsi_target.c
index 1f4c794f5fcc..b19e4329ba00 100644
--- a/drivers/target/iscsi/iscsi_target.c
+++ b/drivers/target/iscsi/iscsi_target.c
@@ -3709,7 +3709,6 @@ static inline void iscsit_thread_check_cpumask(
 	struct task_struct *p,
 	int mode)
 {
-	char buf[128];
 	/*
 	 * mode == 1 signals iscsi_target_tx_thread() usage.
 	 * mode == 0 signals iscsi_target_rx_thread() usage.
@@ -3728,8 +3727,6 @@ static inline void iscsit_thread_check_cpumask(
 	 * both TX and RX kthreads are scheduled to run on the
 	 * same CPU.
 	 */
-	memset(buf, 0, 128);
-	cpumask_scnprintf(buf, 128, conn->conn_cpumask);
 	set_cpus_allowed_ptr(p, conn->conn_cpumask);
 }
 
@@ -4326,8 +4323,7 @@ int iscsit_close_connection(
 	if (conn->conn_tx_hash.tfm)
 		crypto_free_hash(conn->conn_tx_hash.tfm);
 
-	if (conn->conn_cpumask)
-		free_cpumask_var(conn->conn_cpumask);
+	free_cpumask_var(conn->conn_cpumask);
 
 	kfree(conn->conn_ops);
 	conn->conn_ops = NULL;
@@ -4540,6 +4536,7 @@ static void iscsit_logout_post_handler_diffcid(
 {
 	struct iscsi_conn *l_conn;
 	struct iscsi_session *sess = conn->sess;
+	bool conn_found = false;
 
 	if (!sess)
 		return;
@@ -4548,12 +4545,13 @@ static void iscsit_logout_post_handler_diffcid(
 	list_for_each_entry(l_conn, &sess->sess_conn_list, conn_list) {
 		if (l_conn->cid == cid) {
 			iscsit_inc_conn_usage_count(l_conn);
+			conn_found = true;
 			break;
 		}
 	}
 	spin_unlock_bh(&sess->conn_lock);
 
-	if (!l_conn)
+	if (!conn_found)
 		return;
 
 	if (l_conn->sock)
diff --git a/drivers/target/iscsi/iscsi_target_configfs.c b/drivers/target/iscsi/iscsi_target_configfs.c
index ae03f3e5de1e..9059c1e0b26e 100644
--- a/drivers/target/iscsi/iscsi_target_configfs.c
+++ b/drivers/target/iscsi/iscsi_target_configfs.c
@@ -669,12 +669,10 @@ static ssize_t lio_target_nacl_show_info(
 	} else {
 		sess = se_sess->fabric_sess_ptr;
 
-		if (sess->sess_ops->InitiatorName)
-			rb += sprintf(page+rb, "InitiatorName: %s\n",
-				sess->sess_ops->InitiatorName);
-		if (sess->sess_ops->InitiatorAlias)
-			rb += sprintf(page+rb, "InitiatorAlias: %s\n",
-				sess->sess_ops->InitiatorAlias);
+		rb += sprintf(page+rb, "InitiatorName: %s\n",
+			sess->sess_ops->InitiatorName);
+		rb += sprintf(page+rb, "InitiatorAlias: %s\n",
+			sess->sess_ops->InitiatorAlias);
 
 		rb += sprintf(page+rb, "LIO Session ID: %u   "
 			"ISID: 0x%02x %02x %02x %02x %02x %02x  "
diff --git a/drivers/target/iscsi/iscsi_target_erl0.c b/drivers/target/iscsi/iscsi_target_erl0.c
index 0d1e6ee3e992..a0ae5fc0ad75 100644
--- a/drivers/target/iscsi/iscsi_target_erl0.c
+++ b/drivers/target/iscsi/iscsi_target_erl0.c
@@ -345,7 +345,6 @@ static int iscsit_dataout_check_datasn(
 	struct iscsi_cmd *cmd,
 	unsigned char *buf)
 {
-	int dump = 0, recovery = 0;
 	u32 data_sn = 0;
 	struct iscsi_conn *conn = cmd->conn;
 	struct iscsi_data *hdr = (struct iscsi_data *) buf;
@@ -370,13 +369,11 @@ static int iscsit_dataout_check_datasn(
 		pr_err("Command ITT: 0x%08x, received DataSN: 0x%08x"
 			" higher than expected 0x%08x.\n", cmd->init_task_tag,
 				be32_to_cpu(hdr->datasn), data_sn);
-		recovery = 1;
 		goto recover;
 	} else if (be32_to_cpu(hdr->datasn) < data_sn) {
 		pr_err("Command ITT: 0x%08x, received DataSN: 0x%08x"
 			" lower than expected 0x%08x, discarding payload.\n",
 			cmd->init_task_tag, be32_to_cpu(hdr->datasn), data_sn);
-		dump = 1;
 		goto dump;
 	}
 
@@ -392,8 +389,7 @@ dump:
 	if (iscsit_dump_data_payload(conn, payload_length, 1) < 0)
 		return DATAOUT_CANNOT_RECOVER;
 
-	return (recovery || dump) ? DATAOUT_WITHIN_COMMAND_RECOVERY :
-				DATAOUT_NORMAL;
+	return DATAOUT_WITHIN_COMMAND_RECOVERY;
 }
 
 static int iscsit_dataout_pre_datapduinorder_yes(
diff --git a/drivers/target/iscsi/iscsi_target_login.c b/drivers/target/iscsi/iscsi_target_login.c
index 5e71ac609418..480f2e0ecc11 100644
--- a/drivers/target/iscsi/iscsi_target_login.c
+++ b/drivers/target/iscsi/iscsi_target_login.c
@@ -978,8 +978,7 @@ int iscsit_setup_np(
 	return 0;
 fail:
 	np->np_socket = NULL;
-	if (sock)
-		sock_release(sock);
+	sock_release(sock);
 	return ret;
 }
 
@@ -1190,8 +1189,7 @@ old_sess_out:
 	if (!IS_ERR(conn->conn_tx_hash.tfm))
 		crypto_free_hash(conn->conn_tx_hash.tfm);
 
-	if (conn->conn_cpumask)
-		free_cpumask_var(conn->conn_cpumask);
+	free_cpumask_var(conn->conn_cpumask);
 
 	kfree(conn->conn_ops);
 
@@ -1268,8 +1266,6 @@ static int __iscsi_target_login_thread(struct iscsi_np *np)
 			iscsit_put_transport(conn->conn_transport);
 			kfree(conn);
 			conn = NULL;
-			if (ret == -ENODEV)
-				goto out;
 			/* Get another socket */
 			return 1;
 		}
diff --git a/drivers/target/iscsi/iscsi_target_parameters.c b/drivers/target/iscsi/iscsi_target_parameters.c
index 02f9de26f38a..18c29260b4a2 100644
--- a/drivers/target/iscsi/iscsi_target_parameters.c
+++ b/drivers/target/iscsi/iscsi_target_parameters.c
@@ -601,7 +601,7 @@ int iscsi_copy_param_list(
 	param_list = kzalloc(sizeof(struct iscsi_param_list), GFP_KERNEL);
 	if (!param_list) {
 		pr_err("Unable to allocate memory for struct iscsi_param_list.\n");
-		goto err_out;
+		return -1;
 	}
 	INIT_LIST_HEAD(&param_list->param_list);
 	INIT_LIST_HEAD(&param_list->extra_response_list);
diff --git a/drivers/target/iscsi/iscsi_target_util.c b/drivers/target/iscsi/iscsi_target_util.c
index fd90b28f1d94..ce87ce9bdb9c 100644
--- a/drivers/target/iscsi/iscsi_target_util.c
+++ b/drivers/target/iscsi/iscsi_target_util.c
@@ -400,6 +400,8 @@ struct iscsi_cmd *iscsit_find_cmd_from_itt_or_dump(
 
 	spin_lock_bh(&conn->cmd_lock);
 	list_for_each_entry(cmd, &conn->conn_cmd_list, i_conn_node) {
+		if (cmd->cmd_flags & ICF_GOT_LAST_DATAOUT)
+			continue;
 		if (cmd->init_task_tag == init_task_tag) {
 			spin_unlock_bh(&conn->cmd_lock);
 			return cmd;
@@ -1479,8 +1481,9 @@ void iscsit_collect_login_stats(
 		if (conn->param_list)
 			intrname = iscsi_find_param_from_key(INITIATORNAME,
 							     conn->param_list);
-		strcpy(ls->last_intr_fail_name,
-		       (intrname ? intrname->value : "Unknown"));
+		strlcpy(ls->last_intr_fail_name,
+		       (intrname ? intrname->value : "Unknown"),
+		       sizeof(ls->last_intr_fail_name));
 
 		ls->last_intr_fail_ip_family = conn->login_family;
 
diff --git a/drivers/target/loopback/tcm_loop.c b/drivers/target/loopback/tcm_loop.c
index 340de9d92b15..ab3ab27d49b7 100644
--- a/drivers/target/loopback/tcm_loop.c
+++ b/drivers/target/loopback/tcm_loop.c
@@ -153,18 +153,11 @@ static int tcm_loop_change_queue_type(struct scsi_device *sdev, int tag)
 /*
  * Locate the SAM Task Attr from struct scsi_cmnd *
  */
-static int tcm_loop_sam_attr(struct scsi_cmnd *sc)
-{
-	if (sc->device->tagged_supported) {
-		switch (sc->tag) {
-		case HEAD_OF_QUEUE_TAG:
-			return MSG_HEAD_TAG;
-		case ORDERED_QUEUE_TAG:
-			return MSG_ORDERED_TAG;
-		default:
-			break;
-		}
-	}
+static int tcm_loop_sam_attr(struct scsi_cmnd *sc, int tag)
+{
+	if (sc->device->tagged_supported &&
+	    sc->device->ordered_tags && tag >= 0)
+		return MSG_ORDERED_TAG;
 
 	return MSG_SIMPLE_TAG;
 }
@@ -227,7 +220,7 @@ static void tcm_loop_submission_work(struct work_struct *work)
 
 	rc = target_submit_cmd_map_sgls(se_cmd, tl_nexus->se_sess, sc->cmnd,
 			&tl_cmd->tl_sense_buf[0], tl_cmd->sc->device->lun,
-			transfer_length, tcm_loop_sam_attr(sc),
+			transfer_length, tcm_loop_sam_attr(sc, tl_cmd->sc_cmd_tag),
 			sc->sc_data_direction, 0,
 			scsi_sglist(sc), scsi_sg_count(sc),
 			sgl_bidi, sgl_bidi_count,
@@ -266,7 +259,7 @@ static int tcm_loop_queuecommand(struct Scsi_Host *sh, struct scsi_cmnd *sc)
 	}
 
 	tl_cmd->sc = sc;
-	tl_cmd->sc_cmd_tag = sc->tag;
+	tl_cmd->sc_cmd_tag = sc->request->tag;
 	INIT_WORK(&tl_cmd->work, tcm_loop_submission_work);
 	queue_work(tcm_loop_workqueue, &tl_cmd->work);
 	return 0;
@@ -370,7 +363,7 @@ static int tcm_loop_abort_task(struct scsi_cmnd *sc)
 	 */
 	tl_tpg = &tl_hba->tl_hba_tpgs[sc->device->id];
 	ret = tcm_loop_issue_tmr(tl_tpg, tl_nexus, sc->device->lun,
-				 sc->tag, TMR_ABORT_TASK);
+				 sc->request->tag, TMR_ABORT_TASK);
 	return (ret == TMR_FUNCTION_COMPLETE) ? SUCCESS : FAILED;
 }
 
@@ -960,8 +953,7 @@ static int tcm_loop_port_link(
 				struct tcm_loop_tpg, tl_se_tpg);
 	struct tcm_loop_hba *tl_hba = tl_tpg->tl_hba;
 
-	atomic_inc(&tl_tpg->tl_tpg_port_count);
-	smp_mb__after_atomic();
+	atomic_inc_mb(&tl_tpg->tl_tpg_port_count);
 	/*
 	 * Add Linux/SCSI struct scsi_device by HCTL
 	 */
@@ -995,8 +987,7 @@ static void tcm_loop_port_unlink(
 	scsi_remove_device(sd);
 	scsi_device_put(sd);
 
-	atomic_dec(&tl_tpg->tl_tpg_port_count);
-	smp_mb__after_atomic();
+	atomic_dec_mb(&tl_tpg->tl_tpg_port_count);
 
 	pr_debug("TCM_Loop_ConfigFS: Port Unlink Successful\n");
 }
diff --git a/drivers/target/target_core_alua.c b/drivers/target/target_core_alua.c
index fbc5ebb5f761..fb87780929d2 100644
--- a/drivers/target/target_core_alua.c
+++ b/drivers/target/target_core_alua.c
@@ -392,8 +392,7 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 				if (tg_pt_id != tg_pt_gp->tg_pt_gp_id)
 					continue;
 
-				atomic_inc(&tg_pt_gp->tg_pt_gp_ref_cnt);
-				smp_mb__after_atomic();
+				atomic_inc_mb(&tg_pt_gp->tg_pt_gp_ref_cnt);
 
 				spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
@@ -403,8 +402,7 @@ target_emulate_set_target_port_groups(struct se_cmd *cmd)
 					found = true;
 
 				spin_lock(&dev->t10_alua.tg_pt_gps_lock);
-				atomic_dec(&tg_pt_gp->tg_pt_gp_ref_cnt);
-				smp_mb__after_atomic();
+				atomic_dec_mb(&tg_pt_gp->tg_pt_gp_ref_cnt);
 				break;
 			}
 			spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
@@ -998,8 +996,7 @@ static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 		 * every I_T nexus other than the I_T nexus on which the SET
 		 * TARGET PORT GROUPS command
 		 */
-		atomic_inc(&mem->tg_pt_gp_mem_ref_cnt);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&mem->tg_pt_gp_mem_ref_cnt);
 		spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
 
 		spin_lock_bh(&port->sep_alua_lock);
@@ -1028,8 +1025,7 @@ static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 		spin_unlock_bh(&port->sep_alua_lock);
 
 		spin_lock(&tg_pt_gp->tg_pt_gp_lock);
-		atomic_dec(&mem->tg_pt_gp_mem_ref_cnt);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&mem->tg_pt_gp_mem_ref_cnt);
 	}
 	spin_unlock(&tg_pt_gp->tg_pt_gp_lock);
 	/*
@@ -1063,7 +1059,6 @@ static void core_alua_do_transition_tg_pt_work(struct work_struct *work)
 		core_alua_dump_state(tg_pt_gp->tg_pt_gp_alua_pending_state));
 	spin_lock(&dev->t10_alua.tg_pt_gps_lock);
 	atomic_dec(&tg_pt_gp->tg_pt_gp_ref_cnt);
-	smp_mb__after_atomic();
 	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
 	if (tg_pt_gp->tg_pt_gp_transition_complete)
@@ -1125,7 +1120,6 @@ static int core_alua_do_transition_tg_pt(
 	 */
 	spin_lock(&dev->t10_alua.tg_pt_gps_lock);
 	atomic_inc(&tg_pt_gp->tg_pt_gp_ref_cnt);
-	smp_mb__after_atomic();
 	spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
 	if (!explicit && tg_pt_gp->tg_pt_gp_implicit_trans_secs) {
@@ -1168,7 +1162,6 @@ int core_alua_do_port_transition(
 	spin_lock(&local_lu_gp_mem->lu_gp_mem_lock);
 	lu_gp = local_lu_gp_mem->lu_gp;
 	atomic_inc(&lu_gp->lu_gp_ref_cnt);
-	smp_mb__after_atomic();
 	spin_unlock(&local_lu_gp_mem->lu_gp_mem_lock);
 	/*
 	 * For storage objects that are members of the 'default_lu_gp',
@@ -1184,8 +1177,7 @@ int core_alua_do_port_transition(
 		l_tg_pt_gp->tg_pt_gp_alua_nacl = l_nacl;
 		rc = core_alua_do_transition_tg_pt(l_tg_pt_gp,
 						   new_state, explicit);
-		atomic_dec(&lu_gp->lu_gp_ref_cnt);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&lu_gp->lu_gp_ref_cnt);
 		return rc;
 	}
 	/*
@@ -1198,8 +1190,7 @@ int core_alua_do_port_transition(
 				lu_gp_mem_list) {
 
 		dev = lu_gp_mem->lu_gp_mem_dev;
-		atomic_inc(&lu_gp_mem->lu_gp_mem_ref_cnt);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&lu_gp_mem->lu_gp_mem_ref_cnt);
 		spin_unlock(&lu_gp->lu_gp_lock);
 
 		spin_lock(&dev->t10_alua.tg_pt_gps_lock);
@@ -1227,8 +1218,7 @@ int core_alua_do_port_transition(
 				tg_pt_gp->tg_pt_gp_alua_port = NULL;
 				tg_pt_gp->tg_pt_gp_alua_nacl = NULL;
 			}
-			atomic_inc(&tg_pt_gp->tg_pt_gp_ref_cnt);
-			smp_mb__after_atomic();
+			atomic_inc_mb(&tg_pt_gp->tg_pt_gp_ref_cnt);
 			spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 			/*
 			 * core_alua_do_transition_tg_pt() will always return
@@ -1238,16 +1228,14 @@ int core_alua_do_port_transition(
 					new_state, explicit);
 
 			spin_lock(&dev->t10_alua.tg_pt_gps_lock);
-			atomic_dec(&tg_pt_gp->tg_pt_gp_ref_cnt);
-			smp_mb__after_atomic();
+			atomic_dec_mb(&tg_pt_gp->tg_pt_gp_ref_cnt);
 			if (rc)
 				break;
 		}
 		spin_unlock(&dev->t10_alua.tg_pt_gps_lock);
 
 		spin_lock(&lu_gp->lu_gp_lock);
-		atomic_dec(&lu_gp_mem->lu_gp_mem_ref_cnt);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&lu_gp_mem->lu_gp_mem_ref_cnt);
 	}
 	spin_unlock(&lu_gp->lu_gp_lock);
 
@@ -1260,8 +1248,7 @@ int core_alua_do_port_transition(
 			 core_alua_dump_state(new_state));
 	}
 
-	atomic_dec(&lu_gp->lu_gp_ref_cnt);
-	smp_mb__after_atomic();
+	atomic_dec_mb(&lu_gp->lu_gp_ref_cnt);
 	return rc;
 }
 
diff --git a/drivers/target/target_core_configfs.c b/drivers/target/target_core_configfs.c
index bf55c5a04cfa..79f9296a08ae 100644
--- a/drivers/target/target_core_configfs.c
+++ b/drivers/target/target_core_configfs.c
@@ -665,6 +665,9 @@ SE_DEV_ATTR(is_nonrot, S_IRUGO | S_IWUSR);
 DEF_DEV_ATTRIB(emulate_rest_reord);
 SE_DEV_ATTR(emulate_rest_reord, S_IRUGO | S_IWUSR);
 
+DEF_DEV_ATTRIB(force_pr_aptpl);
+SE_DEV_ATTR(force_pr_aptpl, S_IRUGO | S_IWUSR);
+
 DEF_DEV_ATTRIB_RO(hw_block_size);
 SE_DEV_ATTR_RO(hw_block_size);
 
@@ -719,6 +722,7 @@ static struct configfs_attribute *target_core_dev_attrib_attrs[] = {
 	&target_core_dev_attrib_hw_pi_prot_type.attr,
 	&target_core_dev_attrib_pi_prot_format.attr,
 	&target_core_dev_attrib_enforce_pr_isids.attr,
+	&target_core_dev_attrib_force_pr_aptpl.attr,
 	&target_core_dev_attrib_is_nonrot.attr,
 	&target_core_dev_attrib_emulate_rest_reord.attr,
 	&target_core_dev_attrib_hw_block_size.attr,
@@ -1263,7 +1267,7 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 {
 	unsigned char *i_fabric = NULL, *i_port = NULL, *isid = NULL;
 	unsigned char *t_fabric = NULL, *t_port = NULL;
-	char *orig, *ptr, *arg_p, *opts;
+	char *orig, *ptr, *opts;
 	substring_t args[MAX_OPT_ARGS];
 	unsigned long long tmp_ll;
 	u64 sa_res_key = 0;
@@ -1295,14 +1299,14 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 		token = match_token(ptr, tokens, args);
 		switch (token) {
 		case Opt_initiator_fabric:
-			i_fabric = match_strdup(&args[0]);
+			i_fabric = match_strdup(args);
 			if (!i_fabric) {
 				ret = -ENOMEM;
 				goto out;
 			}
 			break;
 		case Opt_initiator_node:
-			i_port = match_strdup(&args[0]);
+			i_port = match_strdup(args);
 			if (!i_port) {
 				ret = -ENOMEM;
 				goto out;
@@ -1316,7 +1320,7 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 			}
 			break;
 		case Opt_initiator_sid:
-			isid = match_strdup(&args[0]);
+			isid = match_strdup(args);
 			if (!isid) {
 				ret = -ENOMEM;
 				goto out;
@@ -1330,15 +1334,9 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 			}
 			break;
 		case Opt_sa_res_key:
-			arg_p = match_strdup(&args[0]);
-			if (!arg_p) {
-				ret = -ENOMEM;
-				goto out;
-			}
-			ret = kstrtoull(arg_p, 0, &tmp_ll);
+			ret = kstrtoull(args->from, 0, &tmp_ll);
 			if (ret < 0) {
-				pr_err("kstrtoull() failed for"
-					" sa_res_key=\n");
+				pr_err("kstrtoull() failed for sa_res_key=\n");
 				goto out;
 			}
 			sa_res_key = (u64)tmp_ll;
@@ -1370,14 +1368,14 @@ static ssize_t target_core_dev_pr_store_attr_res_aptpl_metadata(
 		 * PR APTPL Metadata for Target Port
 		 */
 		case Opt_target_fabric:
-			t_fabric = match_strdup(&args[0]);
+			t_fabric = match_strdup(args);
 			if (!t_fabric) {
 				ret = -ENOMEM;
 				goto out;
 			}
 			break;
 		case Opt_target_node:
-			t_port = match_strdup(&args[0]);
+			t_port = match_strdup(args);
 			if (!t_port) {
 				ret = -ENOMEM;
 				goto out;
@@ -2363,7 +2361,7 @@ static ssize_t target_core_alua_tg_pt_gp_store_attr_alua_support_##_name(\
 		pr_err("Invalid value '%ld', must be '0' or '1'\n", tmp); \
 		return -EINVAL;						\
 	}								\
-	if (!tmp)							\
+	if (tmp)							\
 		t->_var |= _bit;					\
 	else								\
 		t->_var &= ~_bit;					\
diff --git a/drivers/target/target_core_device.c b/drivers/target/target_core_device.c
index 98da90167159..c45f9e907e44 100644
--- a/drivers/target/target_core_device.c
+++ b/drivers/target/target_core_device.c
@@ -224,8 +224,7 @@ struct se_dev_entry *core_get_se_deve_from_rtpi(
 		if (port->sep_rtpi != rtpi)
 			continue;
 
-		atomic_inc(&deve->pr_ref_count);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&deve->pr_ref_count);
 		spin_unlock_irq(&nacl->device_list_lock);
 
 		return deve;
@@ -1019,6 +1018,23 @@ int se_dev_set_enforce_pr_isids(struct se_device *dev, int flag)
 	return 0;
 }
 
+int se_dev_set_force_pr_aptpl(struct se_device *dev, int flag)
+{
+	if ((flag != 0) && (flag != 1)) {
+		printk(KERN_ERR "Illegal value %d\n", flag);
+		return -EINVAL;
+	}
+	if (dev->export_count) {
+		pr_err("dev[%p]: Unable to set force_pr_aptpl while"
+		       " export_count is %d\n", dev, dev->export_count);
+		return -EINVAL;
+	}
+
+	dev->dev_attrib.force_pr_aptpl = flag;
+	pr_debug("dev[%p]: SE Device force_pr_aptpl: %d\n", dev, flag);
+	return 0;
+}
+
 int se_dev_set_is_nonrot(struct se_device *dev, int flag)
 {
 	if ((flag != 0) && (flag != 1)) {
@@ -1250,24 +1266,16 @@ struct se_lun *core_dev_add_lun(
  *
  *
  */
-int core_dev_del_lun(
+void core_dev_del_lun(
 	struct se_portal_group *tpg,
-	u32 unpacked_lun)
+	struct se_lun *lun)
 {
-	struct se_lun *lun;
-
-	lun = core_tpg_pre_dellun(tpg, unpacked_lun);
-	if (IS_ERR(lun))
-		return PTR_ERR(lun);
-
-	core_tpg_post_dellun(tpg, lun);
-
-	pr_debug("%s_TPG[%u]_LUN[%u] - Deactivated %s Logical Unit from"
+	pr_debug("%s_TPG[%u]_LUN[%u] - Deactivating %s Logical Unit from"
 		" device object\n", tpg->se_tpg_tfo->get_fabric_name(),
-		tpg->se_tpg_tfo->tpg_get_tag(tpg), unpacked_lun,
+		tpg->se_tpg_tfo->tpg_get_tag(tpg), lun->unpacked_lun,
 		tpg->se_tpg_tfo->get_fabric_name());
 
-	return 0;
+	core_tpg_remove_lun(tpg, lun);
 }
 
 struct se_lun *core_get_lun_from_tpg(struct se_portal_group *tpg, u32 unpacked_lun)
@@ -1396,8 +1404,7 @@ int core_dev_add_initiator_node_lun_acl(
 
 	spin_lock(&lun->lun_acl_lock);
 	list_add_tail(&lacl->lacl_list, &lun->lun_acl_list);
-	atomic_inc(&lun->lun_acl_count);
-	smp_mb__after_atomic();
+	atomic_inc_mb(&lun->lun_acl_count);
 	spin_unlock(&lun->lun_acl_lock);
 
 	pr_debug("%s_TPG[%hu]_LUN[%u->%u] - Added %s ACL for "
@@ -1409,7 +1416,8 @@ int core_dev_add_initiator_node_lun_acl(
 	 * Check to see if there are any existing persistent reservation APTPL
 	 * pre-registrations that need to be enabled for this LUN ACL..
 	 */
-	core_scsi3_check_aptpl_registration(lun->lun_se_dev, tpg, lun, lacl);
+	core_scsi3_check_aptpl_registration(lun->lun_se_dev, tpg, lun, nacl,
+					    lacl->mapped_lun);
 	return 0;
 }
 
@@ -1430,8 +1438,7 @@ int core_dev_del_initiator_node_lun_acl(
 
 	spin_lock(&lun->lun_acl_lock);
 	list_del(&lacl->lacl_list);
-	atomic_dec(&lun->lun_acl_count);
-	smp_mb__after_atomic();
+	atomic_dec_mb(&lun->lun_acl_count);
 	spin_unlock(&lun->lun_acl_lock);
 
 	core_disable_device_list_for_node(lun, NULL, lacl->mapped_lun,
@@ -1554,6 +1561,7 @@ struct se_device *target_alloc_device(struct se_hba *hba, const char *name)
 	dev->dev_attrib.emulate_3pc = DA_EMULATE_3PC;
 	dev->dev_attrib.pi_prot_type = TARGET_DIF_TYPE0_PROT;
 	dev->dev_attrib.enforce_pr_isids = DA_ENFORCE_PR_ISIDS;
+	dev->dev_attrib.force_pr_aptpl = DA_FORCE_PR_APTPL;
 	dev->dev_attrib.is_nonrot = DA_IS_NONROT;
 	dev->dev_attrib.emulate_rest_reord = DA_EMULATE_REST_REORD;
 	dev->dev_attrib.max_unmap_lba_count = DA_MAX_UNMAP_LBA_COUNT;
diff --git a/drivers/target/target_core_fabric_configfs.c b/drivers/target/target_core_fabric_configfs.c
index 7de9f0475d05..0c3f90130b7d 100644
--- a/drivers/target/target_core_fabric_configfs.c
+++ b/drivers/target/target_core_fabric_configfs.c
@@ -320,7 +320,7 @@ static struct config_group *target_fabric_make_mappedlun(
 			struct se_node_acl, acl_group);
 	struct se_portal_group *se_tpg = se_nacl->se_tpg;
 	struct target_fabric_configfs *tf = se_tpg->se_tpg_wwn->wwn_tf;
-	struct se_lun_acl *lacl;
+	struct se_lun_acl *lacl = NULL;
 	struct config_item *acl_ci;
 	struct config_group *lacl_cg = NULL, *ml_stat_grp = NULL;
 	char *buf;
@@ -406,6 +406,7 @@ static struct config_group *target_fabric_make_mappedlun(
 out:
 	if (lacl_cg)
 		kfree(lacl_cg->default_groups);
+	kfree(lacl);
 	kfree(buf);
 	return ERR_PTR(ret);
 }
@@ -821,7 +822,7 @@ static int target_fabric_port_unlink(
 		tf->tf_ops.fabric_pre_unlink(se_tpg, lun);
 	}
 
-	core_dev_del_lun(se_tpg, lun->unpacked_lun);
+	core_dev_del_lun(se_tpg, lun);
 	return 0;
 }
 
@@ -910,16 +911,12 @@ static struct config_group *target_fabric_make_lun(
 				GFP_KERNEL);
 	if (!port_stat_grp->default_groups) {
 		pr_err("Unable to allocate port_stat_grp->default_groups\n");
-		errno = -ENOMEM;
-		goto out;
+		kfree(lun_cg->default_groups);
+		return ERR_PTR(-ENOMEM);
 	}
 	target_stat_setup_port_default_groups(lun);
 
 	return &lun->lun_group;
-out:
-	if (lun_cg)
-		kfree(lun_cg->default_groups);
-	return ERR_PTR(errno);
 }
 
 static void target_fabric_drop_lun(
diff --git a/drivers/target/target_core_fabric_lib.c b/drivers/target/target_core_fabric_lib.c
index 0d1cf8b4f49f..35bfe77160d8 100644
--- a/drivers/target/target_core_fabric_lib.c
+++ b/drivers/target/target_core_fabric_lib.c
@@ -394,9 +394,9 @@ char *iscsi_parse_pr_out_transport_id(
 	 * If the caller wants the TransportID Length, we set that value for the
 	 * entire iSCSI Tarnsport ID now.
 	 */
-	 if (out_tid_len != NULL) {
-		add_len = ((buf[2] >> 8) & 0xff);
-		add_len |= (buf[3] & 0xff);
+	if (out_tid_len) {
+		/* The shift works thanks to integer promotion rules */
+		add_len = (buf[2] << 8) | buf[3];
 
 		tid_len = strlen(&buf[4]);
 		tid_len += 4; /* Add four bytes for iSCSI Transport ID header */
diff --git a/drivers/target/target_core_file.c b/drivers/target/target_core_file.c
index 7d6cddaec525..72c83d98662b 100644
--- a/drivers/target/target_core_file.c
+++ b/drivers/target/target_core_file.c
@@ -415,7 +415,7 @@ fd_execute_sync_cache(struct se_cmd *cmd)
 	} else {
 		start = cmd->t_task_lba * dev->dev_attrib.block_size;
 		if (cmd->data_length)
-			end = start + cmd->data_length;
+			end = start + cmd->data_length - 1;
 		else
 			end = LLONG_MAX;
 	}
@@ -680,7 +680,12 @@ fd_execute_rw(struct se_cmd *cmd, struct scatterlist *sgl, u32 sgl_nents,
 			struct fd_dev *fd_dev = FD_DEV(dev);
 			loff_t start = cmd->t_task_lba *
 				dev->dev_attrib.block_size;
-			loff_t end = start + cmd->data_length;
+			loff_t end;
+
+			if (cmd->data_length)
+				end = start + cmd->data_length - 1;
+			else
+				end = LLONG_MAX;
 
 			vfs_fsync_range(fd_dev->fd_file, start, end, 1);
 		}
@@ -762,7 +767,9 @@ static ssize_t fd_set_configfs_dev_params(struct se_device *dev,
 			fd_dev->fbd_flags |= FBDF_HAS_SIZE;
 			break;
 		case Opt_fd_buffered_io:
-			match_int(args, &arg);
+			ret = match_int(args, &arg);
+			if (ret)
+				goto out;
 			if (arg != 1) {
 				pr_err("bogus fd_buffered_io=%d value\n", arg);
 				ret = -EINVAL;
diff --git a/drivers/target/target_core_internal.h b/drivers/target/target_core_internal.h
index de9cab708f45..e31f42f369ff 100644
--- a/drivers/target/target_core_internal.h
+++ b/drivers/target/target_core_internal.h
@@ -38,6 +38,7 @@ int	se_dev_set_emulate_3pc(struct se_device *, int);
 int	se_dev_set_pi_prot_type(struct se_device *, int);
 int	se_dev_set_pi_prot_format(struct se_device *, int);
 int	se_dev_set_enforce_pr_isids(struct se_device *, int);
+int	se_dev_set_force_pr_aptpl(struct se_device *, int);
 int	se_dev_set_is_nonrot(struct se_device *, int);
 int	se_dev_set_emulate_rest_reord(struct se_device *dev, int);
 int	se_dev_set_queue_depth(struct se_device *, u32);
@@ -46,7 +47,7 @@ int	se_dev_set_fabric_max_sectors(struct se_device *, u32);
 int	se_dev_set_optimal_sectors(struct se_device *, u32);
 int	se_dev_set_block_size(struct se_device *, u32);
 struct se_lun *core_dev_add_lun(struct se_portal_group *, struct se_device *, u32);
-int	core_dev_del_lun(struct se_portal_group *, u32);
+void	core_dev_del_lun(struct se_portal_group *, struct se_lun *);
 struct se_lun *core_get_lun_from_tpg(struct se_portal_group *, u32);
 struct se_lun_acl *core_dev_init_initiator_node_lun_acl(struct se_portal_group *,
 		struct se_node_acl *, u32, int *);
@@ -82,8 +83,7 @@ void	core_tpg_wait_for_nacl_pr_ref(struct se_node_acl *);
 struct se_lun *core_tpg_alloc_lun(struct se_portal_group *, u32);
 int	core_tpg_add_lun(struct se_portal_group *, struct se_lun *,
 		u32, struct se_device *);
-struct se_lun *core_tpg_pre_dellun(struct se_portal_group *, u32 unpacked_lun);
-int	core_tpg_post_dellun(struct se_portal_group *, struct se_lun *);
+void core_tpg_remove_lun(struct se_portal_group *, struct se_lun *);
 
 /* target_core_transport.c */
 extern struct kmem_cache *se_tmr_req_cache;
diff --git a/drivers/target/target_core_pr.c b/drivers/target/target_core_pr.c
index df357862286e..8c60a1a1ae8d 100644
--- a/drivers/target/target_core_pr.c
+++ b/drivers/target/target_core_pr.c
@@ -674,8 +674,7 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 	 */
 	spin_lock(&dev->se_port_lock);
 	list_for_each_entry_safe(port, port_tmp, &dev->dev_sep_list, sep_list) {
-		atomic_inc(&port->sep_tg_pt_ref_cnt);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&port->sep_tg_pt_ref_cnt);
 		spin_unlock(&dev->se_port_lock);
 
 		spin_lock_bh(&port->sep_alua_lock);
@@ -709,8 +708,7 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 			if (strcmp(nacl->initiatorname, nacl_tmp->initiatorname))
 				continue;
 
-			atomic_inc(&deve_tmp->pr_ref_count);
-			smp_mb__after_atomic();
+			atomic_inc_mb(&deve_tmp->pr_ref_count);
 			spin_unlock_bh(&port->sep_alua_lock);
 			/*
 			 * Grab a configfs group dependency that is released
@@ -722,10 +720,8 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 			if (ret < 0) {
 				pr_err("core_scsi3_lunacl_depend"
 						"_item() failed\n");
-				atomic_dec(&port->sep_tg_pt_ref_cnt);
-				smp_mb__after_atomic();
-				atomic_dec(&deve_tmp->pr_ref_count);
-				smp_mb__after_atomic();
+				atomic_dec_mb(&port->sep_tg_pt_ref_cnt);
+				atomic_dec_mb(&deve_tmp->pr_ref_count);
 				goto out;
 			}
 			/*
@@ -739,10 +735,8 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 						nacl_tmp, deve_tmp, NULL,
 						sa_res_key, all_tg_pt, aptpl);
 			if (!pr_reg_atp) {
-				atomic_dec(&port->sep_tg_pt_ref_cnt);
-				smp_mb__after_atomic();
-				atomic_dec(&deve_tmp->pr_ref_count);
-				smp_mb__after_atomic();
+				atomic_dec_mb(&port->sep_tg_pt_ref_cnt);
+				atomic_dec_mb(&deve_tmp->pr_ref_count);
 				core_scsi3_lunacl_undepend_item(deve_tmp);
 				goto out;
 			}
@@ -754,8 +748,7 @@ static struct t10_pr_registration *__core_scsi3_alloc_registration(
 		spin_unlock_bh(&port->sep_alua_lock);
 
 		spin_lock(&dev->se_port_lock);
-		atomic_dec(&port->sep_tg_pt_ref_cnt);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&port->sep_tg_pt_ref_cnt);
 	}
 	spin_unlock(&dev->se_port_lock);
 
@@ -902,6 +895,7 @@ static int __core_scsi3_check_aptpl_registration(
 	spin_lock(&pr_tmpl->aptpl_reg_lock);
 	list_for_each_entry_safe(pr_reg, pr_reg_tmp, &pr_tmpl->aptpl_reg_list,
 				pr_reg_aptpl_list) {
+
 		if (!strcmp(pr_reg->pr_iport, i_port) &&
 		     (pr_reg->pr_res_mapped_lun == deve->mapped_lun) &&
 		    !(strcmp(pr_reg->pr_tport, t_port)) &&
@@ -944,10 +938,10 @@ int core_scsi3_check_aptpl_registration(
 	struct se_device *dev,
 	struct se_portal_group *tpg,
 	struct se_lun *lun,
-	struct se_lun_acl *lun_acl)
+	struct se_node_acl *nacl,
+	u32 mapped_lun)
 {
-	struct se_node_acl *nacl = lun_acl->se_lun_nacl;
-	struct se_dev_entry *deve = nacl->device_list[lun_acl->mapped_lun];
+	struct se_dev_entry *deve = nacl->device_list[mapped_lun];
 
 	if (dev->dev_reservation_flags & DRF_SPC2_RESERVATIONS)
 		return 0;
@@ -1109,8 +1103,7 @@ static struct t10_pr_registration *__core_scsi3_locate_pr_reg(
 				if (dev->dev_attrib.enforce_pr_isids)
 					continue;
 			}
-			atomic_inc(&pr_reg->pr_res_holders);
-			smp_mb__after_atomic();
+			atomic_inc_mb(&pr_reg->pr_res_holders);
 			spin_unlock(&pr_tmpl->registration_lock);
 			return pr_reg;
 		}
@@ -1124,8 +1117,7 @@ static struct t10_pr_registration *__core_scsi3_locate_pr_reg(
 		if (strcmp(isid, pr_reg->pr_reg_isid))
 			continue;
 
-		atomic_inc(&pr_reg->pr_res_holders);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&pr_reg->pr_res_holders);
 		spin_unlock(&pr_tmpl->registration_lock);
 		return pr_reg;
 	}
@@ -1154,8 +1146,7 @@ static struct t10_pr_registration *core_scsi3_locate_pr_reg(
 
 static void core_scsi3_put_pr_reg(struct t10_pr_registration *pr_reg)
 {
-	atomic_dec(&pr_reg->pr_res_holders);
-	smp_mb__after_atomic();
+	atomic_dec_mb(&pr_reg->pr_res_holders);
 }
 
 static int core_scsi3_check_implicit_release(
@@ -1348,8 +1339,7 @@ static void core_scsi3_tpg_undepend_item(struct se_portal_group *tpg)
 	configfs_undepend_item(tpg->se_tpg_tfo->tf_subsys,
 			&tpg->tpg_group.cg_item);
 
-	atomic_dec(&tpg->tpg_pr_ref_count);
-	smp_mb__after_atomic();
+	atomic_dec_mb(&tpg->tpg_pr_ref_count);
 }
 
 static int core_scsi3_nodeacl_depend_item(struct se_node_acl *nacl)
@@ -1368,16 +1358,14 @@ static void core_scsi3_nodeacl_undepend_item(struct se_node_acl *nacl)
 	struct se_portal_group *tpg = nacl->se_tpg;
 
 	if (nacl->dynamic_node_acl) {
-		atomic_dec(&nacl->acl_pr_ref_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&nacl->acl_pr_ref_count);
 		return;
 	}
 
 	configfs_undepend_item(tpg->se_tpg_tfo->tf_subsys,
 			&nacl->acl_group.cg_item);
 
-	atomic_dec(&nacl->acl_pr_ref_count);
-	smp_mb__after_atomic();
+	atomic_dec_mb(&nacl->acl_pr_ref_count);
 }
 
 static int core_scsi3_lunacl_depend_item(struct se_dev_entry *se_deve)
@@ -1407,8 +1395,7 @@ static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *se_deve)
 	 * For nacl->dynamic_node_acl=1
 	 */
 	if (!lun_acl) {
-		atomic_dec(&se_deve->pr_ref_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&se_deve->pr_ref_count);
 		return;
 	}
 	nacl = lun_acl->se_lun_nacl;
@@ -1417,8 +1404,7 @@ static void core_scsi3_lunacl_undepend_item(struct se_dev_entry *se_deve)
 	configfs_undepend_item(tpg->se_tpg_tfo->tf_subsys,
 			&lun_acl->se_lun_group.cg_item);
 
-	atomic_dec(&se_deve->pr_ref_count);
-	smp_mb__after_atomic();
+	atomic_dec_mb(&se_deve->pr_ref_count);
 }
 
 static sense_reason_t
@@ -1551,15 +1537,13 @@ core_scsi3_decode_spec_i_port(
 			if (!i_str)
 				continue;
 
-			atomic_inc(&tmp_tpg->tpg_pr_ref_count);
-			smp_mb__after_atomic();
+			atomic_inc_mb(&tmp_tpg->tpg_pr_ref_count);
 			spin_unlock(&dev->se_port_lock);
 
 			if (core_scsi3_tpg_depend_item(tmp_tpg)) {
 				pr_err(" core_scsi3_tpg_depend_item()"
 					" for tmp_tpg\n");
-				atomic_dec(&tmp_tpg->tpg_pr_ref_count);
-				smp_mb__after_atomic();
+				atomic_dec_mb(&tmp_tpg->tpg_pr_ref_count);
 				ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 				goto out_unmap;
 			}
@@ -1571,10 +1555,8 @@ core_scsi3_decode_spec_i_port(
 			spin_lock_irq(&tmp_tpg->acl_node_lock);
 			dest_node_acl = __core_tpg_get_initiator_node_acl(
 						tmp_tpg, i_str);
-			if (dest_node_acl) {
-				atomic_inc(&dest_node_acl->acl_pr_ref_count);
-				smp_mb__after_atomic();
-			}
+			if (dest_node_acl)
+				atomic_inc_mb(&dest_node_acl->acl_pr_ref_count);
 			spin_unlock_irq(&tmp_tpg->acl_node_lock);
 
 			if (!dest_node_acl) {
@@ -1586,8 +1568,7 @@ core_scsi3_decode_spec_i_port(
 			if (core_scsi3_nodeacl_depend_item(dest_node_acl)) {
 				pr_err("configfs_depend_item() failed"
 					" for dest_node_acl->acl_group\n");
-				atomic_dec(&dest_node_acl->acl_pr_ref_count);
-				smp_mb__after_atomic();
+				atomic_dec_mb(&dest_node_acl->acl_pr_ref_count);
 				core_scsi3_tpg_undepend_item(tmp_tpg);
 				ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 				goto out_unmap;
@@ -1646,8 +1627,7 @@ core_scsi3_decode_spec_i_port(
 		if (core_scsi3_lunacl_depend_item(dest_se_deve)) {
 			pr_err("core_scsi3_lunacl_depend_item()"
 					" failed\n");
-			atomic_dec(&dest_se_deve->pr_ref_count);
-			smp_mb__after_atomic();
+			atomic_dec_mb(&dest_se_deve->pr_ref_count);
 			core_scsi3_nodeacl_undepend_item(dest_node_acl);
 			core_scsi3_tpg_undepend_item(dest_tpg);
 			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
@@ -3167,15 +3147,13 @@ core_scsi3_emulate_pro_register_and_move(struct se_cmd *cmd, u64 res_key,
 		if (!dest_tf_ops)
 			continue;
 
-		atomic_inc(&dest_se_tpg->tpg_pr_ref_count);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&dest_se_tpg->tpg_pr_ref_count);
 		spin_unlock(&dev->se_port_lock);
 
 		if (core_scsi3_tpg_depend_item(dest_se_tpg)) {
 			pr_err("core_scsi3_tpg_depend_item() failed"
 				" for dest_se_tpg\n");
-			atomic_dec(&dest_se_tpg->tpg_pr_ref_count);
-			smp_mb__after_atomic();
+			atomic_dec_mb(&dest_se_tpg->tpg_pr_ref_count);
 			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 			goto out_put_pr_reg;
 		}
@@ -3271,10 +3249,8 @@ after_iport_check:
 	spin_lock_irq(&dest_se_tpg->acl_node_lock);
 	dest_node_acl = __core_tpg_get_initiator_node_acl(dest_se_tpg,
 				initiator_str);
-	if (dest_node_acl) {
-		atomic_inc(&dest_node_acl->acl_pr_ref_count);
-		smp_mb__after_atomic();
-	}
+	if (dest_node_acl)
+		atomic_inc_mb(&dest_node_acl->acl_pr_ref_count);
 	spin_unlock_irq(&dest_se_tpg->acl_node_lock);
 
 	if (!dest_node_acl) {
@@ -3288,8 +3264,7 @@ after_iport_check:
 	if (core_scsi3_nodeacl_depend_item(dest_node_acl)) {
 		pr_err("core_scsi3_nodeacl_depend_item() for"
 			" dest_node_acl\n");
-		atomic_dec(&dest_node_acl->acl_pr_ref_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&dest_node_acl->acl_pr_ref_count);
 		dest_node_acl = NULL;
 		ret = TCM_INVALID_PARAMETER_LIST;
 		goto out;
@@ -3313,8 +3288,7 @@ after_iport_check:
 
 	if (core_scsi3_lunacl_depend_item(dest_se_deve)) {
 		pr_err("core_scsi3_lunacl_depend_item() failed\n");
-		atomic_dec(&dest_se_deve->pr_ref_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&dest_se_deve->pr_ref_count);
 		dest_se_deve = NULL;
 		ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 		goto out;
@@ -3497,6 +3471,7 @@ static unsigned long long core_scsi3_extract_reservation_key(unsigned char *cdb)
 sense_reason_t
 target_scsi3_emulate_pr_out(struct se_cmd *cmd)
 {
+	struct se_device *dev = cmd->se_dev;
 	unsigned char *cdb = &cmd->t_task_cdb[0];
 	unsigned char *buf;
 	u64 res_key, sa_res_key;
@@ -3561,6 +3536,13 @@ target_scsi3_emulate_pr_out(struct se_cmd *cmd)
 		aptpl = (buf[17] & 0x01);
 		unreg = (buf[17] & 0x02);
 	}
+	/*
+	 * If the backend device has been configured to force APTPL metadata
+	 * write-out, go ahead and propigate aptpl=1 down now.
+	 */
+	if (dev->dev_attrib.force_pr_aptpl)
+		aptpl = 1;
+
 	transport_kunmap_data_sg(cmd);
 	buf = NULL;
 
@@ -3803,7 +3785,7 @@ core_scsi3_pri_report_capabilities(struct se_cmd *cmd)
 	if (!buf)
 		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 
-	buf[0] = ((add_len << 8) & 0xff);
+	buf[0] = ((add_len >> 8) & 0xff);
 	buf[1] = (add_len & 0xff);
 	buf[2] |= 0x10; /* CRH: Compatible Reservation Hanlding bit. */
 	buf[2] |= 0x08; /* SIP_C: Specify Initiator Ports Capable bit */
@@ -3879,8 +3861,7 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 		se_tpg = pr_reg->pr_reg_nacl->se_tpg;
 		add_desc_len = 0;
 
-		atomic_inc(&pr_reg->pr_res_holders);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&pr_reg->pr_res_holders);
 		spin_unlock(&pr_tmpl->registration_lock);
 		/*
 		 * Determine expected length of $FABRIC_MOD specific
@@ -3893,8 +3874,7 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 			pr_warn("SPC-3 PRIN READ_FULL_STATUS ran"
 				" out of buffer: %d\n", cmd->data_length);
 			spin_lock(&pr_tmpl->registration_lock);
-			atomic_dec(&pr_reg->pr_res_holders);
-			smp_mb__after_atomic();
+			atomic_dec_mb(&pr_reg->pr_res_holders);
 			break;
 		}
 		/*
@@ -3955,8 +3935,7 @@ core_scsi3_pri_read_full_status(struct se_cmd *cmd)
 				se_nacl, pr_reg, &format_code, &buf[off+4]);
 
 		spin_lock(&pr_tmpl->registration_lock);
-		atomic_dec(&pr_reg->pr_res_holders);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&pr_reg->pr_res_holders);
 		/*
 		 * Set the ADDITIONAL DESCRIPTOR LENGTH
 		 */
diff --git a/drivers/target/target_core_pr.h b/drivers/target/target_core_pr.h
index 2ee2936fa0bd..749fd7bb7510 100644
--- a/drivers/target/target_core_pr.h
+++ b/drivers/target/target_core_pr.h
@@ -60,7 +60,7 @@ extern int core_scsi3_alloc_aptpl_registration(
 			unsigned char *, u16, u32, int, int, u8);
 extern int core_scsi3_check_aptpl_registration(struct se_device *,
 			struct se_portal_group *, struct se_lun *,
-			struct se_lun_acl *);
+			struct se_node_acl *, u32);
 extern void core_scsi3_free_pr_reg_from_nacl(struct se_device *,
 					     struct se_node_acl *);
 extern void core_scsi3_free_all_registrations(struct se_device *);
diff --git a/drivers/target/target_core_pscsi.c b/drivers/target/target_core_pscsi.c
index 943b1dbe859a..7c8291f0bbbc 100644
--- a/drivers/target/target_core_pscsi.c
+++ b/drivers/target/target_core_pscsi.c
@@ -749,14 +749,18 @@ static ssize_t pscsi_set_configfs_dev_params(struct se_device *dev,
 				ret = -EINVAL;
 				goto out;
 			}
-			match_int(args, &arg);
+			ret = match_int(args, &arg);
+			if (ret)
+				goto out;
 			pdv->pdv_host_id = arg;
 			pr_debug("PSCSI[%d]: Referencing SCSI Host ID:"
 				" %d\n", phv->phv_host_id, pdv->pdv_host_id);
 			pdv->pdv_flags |= PDF_HAS_VIRT_HOST_ID;
 			break;
 		case Opt_scsi_channel_id:
-			match_int(args, &arg);
+			ret = match_int(args, &arg);
+			if (ret)
+				goto out;
 			pdv->pdv_channel_id = arg;
 			pr_debug("PSCSI[%d]: Referencing SCSI Channel"
 				" ID: %d\n",  phv->phv_host_id,
@@ -764,7 +768,9 @@ static ssize_t pscsi_set_configfs_dev_params(struct se_device *dev,
 			pdv->pdv_flags |= PDF_HAS_CHANNEL_ID;
 			break;
 		case Opt_scsi_target_id:
-			match_int(args, &arg);
+			ret = match_int(args, &arg);
+			if (ret)
+				goto out;
 			pdv->pdv_target_id = arg;
 			pr_debug("PSCSI[%d]: Referencing SCSI Target"
 				" ID: %d\n", phv->phv_host_id,
@@ -772,7 +778,9 @@ static ssize_t pscsi_set_configfs_dev_params(struct se_device *dev,
 			pdv->pdv_flags |= PDF_HAS_TARGET_ID;
 			break;
 		case Opt_scsi_lun_id:
-			match_int(args, &arg);
+			ret = match_int(args, &arg);
+			if (ret)
+				goto out;
 			pdv->pdv_lun_id = arg;
 			pr_debug("PSCSI[%d]: Referencing SCSI LUN ID:"
 				" %d\n", phv->phv_host_id, pdv->pdv_lun_id);
@@ -1050,7 +1058,7 @@ pscsi_execute_cmd(struct se_cmd *cmd)
 		req = blk_get_request(pdv->pdv_sd->request_queue,
 				(data_direction == DMA_TO_DEVICE),
 				GFP_KERNEL);
-		if (!req) {
+		if (IS_ERR(req)) {
 			pr_err("PSCSI: blk_get_request() failed\n");
 			ret = TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
 			goto fail;
diff --git a/drivers/target/target_core_sbc.c b/drivers/target/target_core_sbc.c
index bd78d9235ac6..ebe62afb957d 100644
--- a/drivers/target/target_core_sbc.c
+++ b/drivers/target/target_core_sbc.c
@@ -948,7 +948,7 @@ sbc_parse_cdb(struct se_cmd *cmd, struct sbc_ops *ops)
 	}
 
 	/* reject any command that we don't have a handler for */
-	if (!(cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) && !cmd->execute_cmd)
+	if (!cmd->execute_cmd)
 		return TCM_UNSUPPORTED_SCSI_OPCODE;
 
 	if (cmd->se_cmd_flags & SCF_SCSI_DATA_CDB) {
diff --git a/drivers/target/target_core_spc.c b/drivers/target/target_core_spc.c
index 6cd7222738fc..bc286a67af7c 100644
--- a/drivers/target/target_core_spc.c
+++ b/drivers/target/target_core_spc.c
@@ -664,7 +664,7 @@ spc_emulate_evpd_b3(struct se_cmd *cmd, unsigned char *buf)
 	buf[0] = dev->transport->get_device_type(dev);
 	buf[3] = 0x0c;
 	put_unaligned_be32(dev->t10_alua.lba_map_segment_size, &buf[8]);
-	put_unaligned_be32(dev->t10_alua.lba_map_segment_size, &buf[12]);
+	put_unaligned_be32(dev->t10_alua.lba_map_segment_multiplier, &buf[12]);
 
 	return 0;
 }
diff --git a/drivers/target/target_core_tmr.c b/drivers/target/target_core_tmr.c
index f7cd95e8111a..fa5e157db47b 100644
--- a/drivers/target/target_core_tmr.c
+++ b/drivers/target/target_core_tmr.c
@@ -64,21 +64,17 @@ int core_tmr_alloc_req(
 }
 EXPORT_SYMBOL(core_tmr_alloc_req);
 
-void core_tmr_release_req(
-	struct se_tmr_req *tmr)
+void core_tmr_release_req(struct se_tmr_req *tmr)
 {
 	struct se_device *dev = tmr->tmr_dev;
 	unsigned long flags;
 
-	if (!dev) {
-		kfree(tmr);
-		return;
+	if (dev) {
+		spin_lock_irqsave(&dev->se_tmr_lock, flags);
+		list_del(&tmr->tmr_list);
+		spin_unlock_irqrestore(&dev->se_tmr_lock, flags);
 	}
 
-	spin_lock_irqsave(&dev->se_tmr_lock, flags);
-	list_del(&tmr->tmr_list);
-	spin_unlock_irqrestore(&dev->se_tmr_lock, flags);
-
 	kfree(tmr);
 }
 
@@ -90,9 +86,8 @@ static void core_tmr_handle_tas_abort(
 	bool remove = true;
 	/*
 	 * TASK ABORTED status (TAS) bit support
-	*/
-	if ((tmr_nacl &&
-	     (tmr_nacl != cmd->se_sess->se_node_acl)) && tas) {
+	 */
+	if ((tmr_nacl && (tmr_nacl != cmd->se_sess->se_node_acl)) && tas) {
 		remove = false;
 		transport_send_task_abort(cmd);
 	}
@@ -120,13 +115,12 @@ void core_tmr_abort_task(
 	struct se_tmr_req *tmr,
 	struct se_session *se_sess)
 {
-	struct se_cmd *se_cmd, *tmp_cmd;
+	struct se_cmd *se_cmd;
 	unsigned long flags;
 	int ref_tag;
 
 	spin_lock_irqsave(&se_sess->sess_cmd_lock, flags);
-	list_for_each_entry_safe(se_cmd, tmp_cmd,
-			&se_sess->sess_cmd_list, se_cmd_list) {
+	list_for_each_entry(se_cmd, &se_sess->sess_cmd_list, se_cmd_list) {
 
 		if (dev != se_cmd->se_dev)
 			continue;
diff --git a/drivers/target/target_core_tpg.c b/drivers/target/target_core_tpg.c
index fddfae61222f..0696de9553d3 100644
--- a/drivers/target/target_core_tpg.c
+++ b/drivers/target/target_core_tpg.c
@@ -40,6 +40,7 @@
 #include <target/target_core_fabric.h>
 
 #include "target_core_internal.h"
+#include "target_core_pr.h"
 
 extern struct se_device *g_lun0_dev;
 
@@ -166,6 +167,13 @@ void core_tpg_add_node_to_devs(
 
 		core_enable_device_list_for_node(lun, NULL, lun->unpacked_lun,
 				lun_access, acl, tpg);
+		/*
+		 * Check to see if there are any existing persistent reservation
+		 * APTPL pre-registrations that need to be enabled for this dynamic
+		 * LUN ACL now..
+		 */
+		core_scsi3_check_aptpl_registration(dev, tpg, lun, acl,
+						    lun->unpacked_lun);
 		spin_lock(&tpg->tpg_lun_lock);
 	}
 	spin_unlock(&tpg->tpg_lun_lock);
@@ -335,7 +343,7 @@ void core_tpg_clear_object_luns(struct se_portal_group *tpg)
 			continue;
 
 		spin_unlock(&tpg->tpg_lun_lock);
-		core_dev_del_lun(tpg, lun->unpacked_lun);
+		core_dev_del_lun(tpg, lun);
 		spin_lock(&tpg->tpg_lun_lock);
 	}
 	spin_unlock(&tpg->tpg_lun_lock);
@@ -663,13 +671,6 @@ static int core_tpg_setup_virtual_lun0(struct se_portal_group *se_tpg)
 	return 0;
 }
 
-static void core_tpg_release_virtual_lun0(struct se_portal_group *se_tpg)
-{
-	struct se_lun *lun = &se_tpg->tpg_virt_lun0;
-
-	core_tpg_post_dellun(se_tpg, lun);
-}
-
 int core_tpg_register(
 	struct target_core_fabric_ops *tfo,
 	struct se_wwn *se_wwn,
@@ -773,7 +774,7 @@ int core_tpg_deregister(struct se_portal_group *se_tpg)
 	spin_unlock_irq(&se_tpg->acl_node_lock);
 
 	if (se_tpg->se_tpg_type == TRANSPORT_TPG_TYPE_NORMAL)
-		core_tpg_release_virtual_lun0(se_tpg);
+		core_tpg_remove_lun(se_tpg, &se_tpg->tpg_virt_lun0);
 
 	se_tpg->se_tpg_fabric_ptr = NULL;
 	array_free(se_tpg->tpg_lun_list, TRANSPORT_MAX_LUNS_PER_TPG);
@@ -819,7 +820,8 @@ int core_tpg_add_lun(
 {
 	int ret;
 
-	ret = percpu_ref_init(&lun->lun_ref, core_tpg_lun_ref_release);
+	ret = percpu_ref_init(&lun->lun_ref, core_tpg_lun_ref_release, 0,
+			      GFP_KERNEL);
 	if (ret < 0)
 		return ret;
 
@@ -837,37 +839,7 @@ int core_tpg_add_lun(
 	return 0;
 }
 
-struct se_lun *core_tpg_pre_dellun(
-	struct se_portal_group *tpg,
-	u32 unpacked_lun)
-{
-	struct se_lun *lun;
-
-	if (unpacked_lun > (TRANSPORT_MAX_LUNS_PER_TPG-1)) {
-		pr_err("%s LUN: %u exceeds TRANSPORT_MAX_LUNS_PER_TPG"
-			"-1: %u for Target Portal Group: %u\n",
-			tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
-			TRANSPORT_MAX_LUNS_PER_TPG-1,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		return ERR_PTR(-EOVERFLOW);
-	}
-
-	spin_lock(&tpg->tpg_lun_lock);
-	lun = tpg->tpg_lun_list[unpacked_lun];
-	if (lun->lun_status != TRANSPORT_LUN_STATUS_ACTIVE) {
-		pr_err("%s Logical Unit Number: %u is not active on"
-			" Target Portal Group: %u, ignoring request.\n",
-			tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
-			tpg->se_tpg_tfo->tpg_get_tag(tpg));
-		spin_unlock(&tpg->tpg_lun_lock);
-		return ERR_PTR(-ENODEV);
-	}
-	spin_unlock(&tpg->tpg_lun_lock);
-
-	return lun;
-}
-
-int core_tpg_post_dellun(
+void core_tpg_remove_lun(
 	struct se_portal_group *tpg,
 	struct se_lun *lun)
 {
@@ -881,6 +853,4 @@ int core_tpg_post_dellun(
 	spin_unlock(&tpg->tpg_lun_lock);
 
 	percpu_ref_exit(&lun->lun_ref);
-
-	return 0;
 }
diff --git a/drivers/target/target_core_transport.c b/drivers/target/target_core_transport.c
index 7fa62fc93e0b..9ea0d5f03f7a 100644
--- a/drivers/target/target_core_transport.c
+++ b/drivers/target/target_core_transport.c
@@ -232,6 +232,10 @@ void transport_subsystem_check_init(void)
 	if (ret != 0)
 		pr_err("Unable to load target_core_pscsi\n");
 
+	ret = request_module("target_core_user");
+	if (ret != 0)
+		pr_err("Unable to load target_core_user\n");
+
 	sub_api_initialized = 1;
 }
 
@@ -752,8 +756,7 @@ void target_qf_do_work(struct work_struct *work)
 
 	list_for_each_entry_safe(cmd, cmd_tmp, &qf_cmd_list, se_qf_node) {
 		list_del(&cmd->se_qf_node);
-		atomic_dec(&dev->dev_qf_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&dev->dev_qf_count);
 
 		pr_debug("Processing %s cmd: %p QUEUE_FULL in work queue"
 			" context: %s\n", cmd->se_tfo->get_fabric_name(), cmd,
@@ -1166,7 +1169,6 @@ transport_check_alloc_task_attr(struct se_cmd *cmd)
 	 * Dormant to Active status.
 	 */
 	cmd->se_ordered_id = atomic_inc_return(&dev->dev_ordered_id);
-	smp_mb__after_atomic();
 	pr_debug("Allocated se_ordered_id: %u for Task Attr: 0x%02x on %s\n",
 			cmd->se_ordered_id, cmd->sam_task_attr,
 			dev->transport->name);
@@ -1722,8 +1724,7 @@ static bool target_handle_task_attr(struct se_cmd *cmd)
 			 cmd->t_task_cdb[0], cmd->se_ordered_id);
 		return false;
 	case MSG_ORDERED_TAG:
-		atomic_inc(&dev->dev_ordered_sync);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&dev->dev_ordered_sync);
 
 		pr_debug("Added ORDERED for CDB: 0x%02x to ordered list, "
 			 " se_ordered_id: %u\n",
@@ -1740,8 +1741,7 @@ static bool target_handle_task_attr(struct se_cmd *cmd)
 		/*
 		 * For SIMPLE and UNTAGGED Task Attribute commands
 		 */
-		atomic_inc(&dev->simple_cmds);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&dev->simple_cmds);
 		break;
 	}
 
@@ -1845,8 +1845,7 @@ static void transport_complete_task_attr(struct se_cmd *cmd)
 		return;
 
 	if (cmd->sam_task_attr == MSG_SIMPLE_TAG) {
-		atomic_dec(&dev->simple_cmds);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&dev->simple_cmds);
 		dev->dev_cur_ordered_id++;
 		pr_debug("Incremented dev->dev_cur_ordered_id: %u for"
 			" SIMPLE: %u\n", dev->dev_cur_ordered_id,
@@ -1857,8 +1856,7 @@ static void transport_complete_task_attr(struct se_cmd *cmd)
 			" HEAD_OF_QUEUE: %u\n", dev->dev_cur_ordered_id,
 			cmd->se_ordered_id);
 	} else if (cmd->sam_task_attr == MSG_ORDERED_TAG) {
-		atomic_dec(&dev->dev_ordered_sync);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&dev->dev_ordered_sync);
 
 		dev->dev_cur_ordered_id++;
 		pr_debug("Incremented dev_cur_ordered_id: %u for ORDERED:"
@@ -1877,8 +1875,7 @@ static void transport_complete_qf(struct se_cmd *cmd)
 	if (cmd->se_cmd_flags & SCF_TRANSPORT_TASK_SENSE) {
 		trace_target_cmd_complete(cmd);
 		ret = cmd->se_tfo->queue_status(cmd);
-		if (ret)
-			goto out;
+		goto out;
 	}
 
 	switch (cmd->data_direction) {
@@ -1916,8 +1913,7 @@ static void transport_handle_queue_full(
 {
 	spin_lock_irq(&dev->qf_cmd_lock);
 	list_add_tail(&cmd->se_qf_node, &cmd->se_dev->qf_cmd_list);
-	atomic_inc(&dev->dev_qf_count);
-	smp_mb__after_atomic();
+	atomic_inc_mb(&dev->dev_qf_count);
 	spin_unlock_irq(&cmd->se_dev->qf_cmd_lock);
 
 	schedule_work(&cmd->se_dev->qf_work_queue);
@@ -2896,7 +2892,6 @@ void transport_send_task_abort(struct se_cmd *cmd)
 		if (cmd->se_tfo->write_pending_status(cmd) != 0) {
 			cmd->transport_state |= CMD_T_ABORTED;
 			cmd->se_cmd_flags |= SCF_SEND_DELAYED_TAS;
-			smp_mb__after_atomic();
 			return;
 		}
 	}
diff --git a/drivers/target/target_core_ua.c b/drivers/target/target_core_ua.c
index 101858e245b3..1738b1646988 100644
--- a/drivers/target/target_core_ua.c
+++ b/drivers/target/target_core_ua.c
@@ -161,8 +161,7 @@ int core_scsi3_ua_allocate(
 		spin_unlock(&deve->ua_lock);
 		spin_unlock_irq(&nacl->device_list_lock);
 
-		atomic_inc(&deve->ua_count);
-		smp_mb__after_atomic();
+		atomic_inc_mb(&deve->ua_count);
 		return 0;
 	}
 	list_add_tail(&ua->ua_nacl_list, &deve->ua_list);
@@ -174,8 +173,7 @@ int core_scsi3_ua_allocate(
 		nacl->se_tpg->se_tpg_tfo->get_fabric_name(), unpacked_lun,
 		asc, ascq);
 
-	atomic_inc(&deve->ua_count);
-	smp_mb__after_atomic();
+	atomic_inc_mb(&deve->ua_count);
 	return 0;
 }
 
@@ -189,8 +187,7 @@ void core_scsi3_ua_release_all(
 		list_del(&ua->ua_nacl_list);
 		kmem_cache_free(se_ua_cache, ua);
 
-		atomic_dec(&deve->ua_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&deve->ua_count);
 	}
 	spin_unlock(&deve->ua_lock);
 }
@@ -250,8 +247,7 @@ void core_scsi3_ua_for_check_condition(
 		list_del(&ua->ua_nacl_list);
 		kmem_cache_free(se_ua_cache, ua);
 
-		atomic_dec(&deve->ua_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&deve->ua_count);
 	}
 	spin_unlock(&deve->ua_lock);
 	spin_unlock_irq(&nacl->device_list_lock);
@@ -309,8 +305,7 @@ int core_scsi3_ua_clear_for_request_sense(
 		list_del(&ua->ua_nacl_list);
 		kmem_cache_free(se_ua_cache, ua);
 
-		atomic_dec(&deve->ua_count);
-		smp_mb__after_atomic();
+		atomic_dec_mb(&deve->ua_count);
 	}
 	spin_unlock(&deve->ua_lock);
 	spin_unlock_irq(&nacl->device_list_lock);
diff --git a/drivers/target/target_core_ua.h b/drivers/target/target_core_ua.h
index be912b36daae..a6b56b364e7a 100644
--- a/drivers/target/target_core_ua.h
+++ b/drivers/target/target_core_ua.h
@@ -1,4 +1,5 @@
 #ifndef TARGET_CORE_UA_H
+#define TARGET_CORE_UA_H
 
 /*
  * From spc4r17, Table D.1: ASC and ASCQ Assignement
diff --git a/drivers/target/target_core_user.c b/drivers/target/target_core_user.c
new file mode 100644
index 000000000000..9a1b314f6482
--- /dev/null
+++ b/drivers/target/target_core_user.c
@@ -0,0 +1,1167 @@
+/*
+ * Copyright (C) 2013 Shaohua Li <shli@kernel.org>
+ * Copyright (C) 2014 Red Hat, Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+#include <linux/spinlock.h>
+#include <linux/module.h>
+#include <linux/idr.h>
+#include <linux/timer.h>
+#include <linux/parser.h>
+#include <scsi/scsi.h>
+#include <scsi/scsi_host.h>
+#include <linux/uio_driver.h>
+#include <net/genetlink.h>
+#include <target/target_core_base.h>
+#include <target/target_core_fabric.h>
+#include <target/target_core_backend.h>
+#include <linux/target_core_user.h>
+
+/*
+ * Define a shared-memory interface for LIO to pass SCSI commands and
+ * data to userspace for processing. This is to allow backends that
+ * are too complex for in-kernel support to be possible.
+ *
+ * It uses the UIO framework to do a lot of the device-creation and
+ * introspection work for us.
+ *
+ * See the .h file for how the ring is laid out. Note that while the
+ * command ring is defined, the particulars of the data area are
+ * not. Offset values in the command entry point to other locations
+ * internal to the mmap()ed area. There is separate space outside the
+ * command ring for data buffers. This leaves maximum flexibility for
+ * moving buffer allocations, or even page flipping or other
+ * allocation techniques, without altering the command ring layout.
+ *
+ * SECURITY:
+ * The user process must be assumed to be malicious. There's no way to
+ * prevent it breaking the command ring protocol if it wants, but in
+ * order to prevent other issues we must only ever read *data* from
+ * the shared memory area, not offsets or sizes. This applies to
+ * command ring entries as well as the mailbox. Extra code needed for
+ * this may have a 'UAM' comment.
+ */
+
+
+#define TCMU_TIME_OUT (30 * MSEC_PER_SEC)
+
+#define CMDR_SIZE (16 * 4096)
+#define DATA_SIZE (257 * 4096)
+
+#define TCMU_RING_SIZE (CMDR_SIZE + DATA_SIZE)
+
+static struct device *tcmu_root_device;
+
+struct tcmu_hba {
+	u32 host_id;
+};
+
+/* User wants all cmds or just some */
+enum passthru_level {
+	TCMU_PASS_ALL = 0,
+	TCMU_PASS_IO,
+	TCMU_PASS_INVALID,
+};
+
+#define TCMU_CONFIG_LEN 256
+
+struct tcmu_dev {
+	struct se_device se_dev;
+
+	char *name;
+	struct se_hba *hba;
+
+#define TCMU_DEV_BIT_OPEN 0
+#define TCMU_DEV_BIT_BROKEN 1
+	unsigned long flags;
+	enum passthru_level pass_level;
+
+	struct uio_info uio_info;
+
+	struct tcmu_mailbox *mb_addr;
+	size_t dev_size;
+	u32 cmdr_size;
+	u32 cmdr_last_cleaned;
+	/* Offset of data ring from start of mb */
+	size_t data_off;
+	size_t data_size;
+	/* Ring head + tail values. */
+	/* Must add data_off and mb_addr to get the address */
+	size_t data_head;
+	size_t data_tail;
+
+	wait_queue_head_t wait_cmdr;
+	/* TODO should this be a mutex? */
+	spinlock_t cmdr_lock;
+
+	struct idr commands;
+	spinlock_t commands_lock;
+
+	struct timer_list timeout;
+
+	char dev_config[TCMU_CONFIG_LEN];
+};
+
+#define TCMU_DEV(_se_dev) container_of(_se_dev, struct tcmu_dev, se_dev)
+
+#define CMDR_OFF sizeof(struct tcmu_mailbox)
+
+struct tcmu_cmd {
+	struct se_cmd *se_cmd;
+	struct tcmu_dev *tcmu_dev;
+
+	uint16_t cmd_id;
+
+	/* Can't use se_cmd->data_length when cleaning up expired cmds, because if
+	   cmd has been completed then accessing se_cmd is off limits */
+	size_t data_length;
+
+	unsigned long deadline;
+
+#define TCMU_CMD_BIT_EXPIRED 0
+	unsigned long flags;
+};
+
+static struct kmem_cache *tcmu_cmd_cache;
+
+/* multicast group */
+enum tcmu_multicast_groups {
+	TCMU_MCGRP_CONFIG,
+};
+
+static const struct genl_multicast_group tcmu_mcgrps[] = {
+	[TCMU_MCGRP_CONFIG] = { .name = "config", },
+};
+
+/* Our generic netlink family */
+static struct genl_family tcmu_genl_family = {
+	.id = GENL_ID_GENERATE,
+	.hdrsize = 0,
+	.name = "TCM-USER",
+	.version = 1,
+	.maxattr = TCMU_ATTR_MAX,
+	.mcgrps = tcmu_mcgrps,
+	.n_mcgrps = ARRAY_SIZE(tcmu_mcgrps),
+};
+
+static struct tcmu_cmd *tcmu_alloc_cmd(struct se_cmd *se_cmd)
+{
+	struct se_device *se_dev = se_cmd->se_dev;
+	struct tcmu_dev *udev = TCMU_DEV(se_dev);
+	struct tcmu_cmd *tcmu_cmd;
+	int cmd_id;
+
+	tcmu_cmd = kmem_cache_zalloc(tcmu_cmd_cache, GFP_KERNEL);
+	if (!tcmu_cmd)
+		return NULL;
+
+	tcmu_cmd->se_cmd = se_cmd;
+	tcmu_cmd->tcmu_dev = udev;
+	tcmu_cmd->data_length = se_cmd->data_length;
+
+	tcmu_cmd->deadline = jiffies + msecs_to_jiffies(TCMU_TIME_OUT);
+
+	idr_preload(GFP_KERNEL);
+	spin_lock_irq(&udev->commands_lock);
+	cmd_id = idr_alloc(&udev->commands, tcmu_cmd, 0,
+		USHRT_MAX, GFP_NOWAIT);
+	spin_unlock_irq(&udev->commands_lock);
+	idr_preload_end();
+
+	if (cmd_id < 0) {
+		kmem_cache_free(tcmu_cmd_cache, tcmu_cmd);
+		return NULL;
+	}
+	tcmu_cmd->cmd_id = cmd_id;
+
+	return tcmu_cmd;
+}
+
+static inline void tcmu_flush_dcache_range(void *vaddr, size_t size)
+{
+	unsigned long offset = (unsigned long) vaddr & ~PAGE_MASK;
+
+	size = round_up(size+offset, PAGE_SIZE);
+	vaddr -= offset;
+
+	while (size) {
+		flush_dcache_page(virt_to_page(vaddr));
+		size -= PAGE_SIZE;
+	}
+}
+
+/*
+ * Some ring helper functions. We don't assume size is a power of 2 so
+ * we can't use circ_buf.h.
+ */
+static inline size_t spc_used(size_t head, size_t tail, size_t size)
+{
+	int diff = head - tail;
+
+	if (diff >= 0)
+		return diff;
+	else
+		return size + diff;
+}
+
+static inline size_t spc_free(size_t head, size_t tail, size_t size)
+{
+	/* Keep 1 byte unused or we can't tell full from empty */
+	return (size - spc_used(head, tail, size) - 1);
+}
+
+static inline size_t head_to_end(size_t head, size_t size)
+{
+	return size - head;
+}
+
+#define UPDATE_HEAD(head, used, size) smp_store_release(&head, ((head % size) + used) % size)
+
+/*
+ * We can't queue a command until we have space available on the cmd ring *and* space
+ * space avail on the data ring.
+ *
+ * Called with ring lock held.
+ */
+static bool is_ring_space_avail(struct tcmu_dev *udev, size_t cmd_size, size_t data_needed)
+{
+	struct tcmu_mailbox *mb = udev->mb_addr;
+	size_t space;
+	u32 cmd_head;
+	size_t cmd_needed;
+
+	tcmu_flush_dcache_range(mb, sizeof(*mb));
+
+	cmd_head = mb->cmd_head % udev->cmdr_size; /* UAM */
+
+	/*
+	 * If cmd end-of-ring space is too small then we need space for a NOP plus
+	 * original cmd - cmds are internally contiguous.
+	 */
+	if (head_to_end(cmd_head, udev->cmdr_size) >= cmd_size)
+		cmd_needed = cmd_size;
+	else
+		cmd_needed = cmd_size + head_to_end(cmd_head, udev->cmdr_size);
+
+	space = spc_free(cmd_head, udev->cmdr_last_cleaned, udev->cmdr_size);
+	if (space < cmd_needed) {
+		pr_debug("no cmd space: %u %u %u\n", cmd_head,
+		       udev->cmdr_last_cleaned, udev->cmdr_size);
+		return false;
+	}
+
+	space = spc_free(udev->data_head, udev->data_tail, udev->data_size);
+	if (space < data_needed) {
+		pr_debug("no data space: %zu %zu %zu\n", udev->data_head,
+		       udev->data_tail, udev->data_size);
+		return false;
+	}
+
+	return true;
+}
+
+static int tcmu_queue_cmd_ring(struct tcmu_cmd *tcmu_cmd)
+{
+	struct tcmu_dev *udev = tcmu_cmd->tcmu_dev;
+	struct se_cmd *se_cmd = tcmu_cmd->se_cmd;
+	size_t base_command_size, command_size;
+	struct tcmu_mailbox *mb;
+	struct tcmu_cmd_entry *entry;
+	int i;
+	struct scatterlist *sg;
+	struct iovec *iov;
+	int iov_cnt = 0;
+	uint32_t cmd_head;
+	uint64_t cdb_off;
+
+	if (test_bit(TCMU_DEV_BIT_BROKEN, &udev->flags))
+		return -EINVAL;
+
+	/*
+	 * Must be a certain minimum size for response sense info, but
+	 * also may be larger if the iov array is large.
+	 *
+	 * iovs = sgl_nents+1, for end-of-ring case, plus another 1
+	 * b/c size == offsetof one-past-element.
+	*/
+	base_command_size = max(offsetof(struct tcmu_cmd_entry,
+					 req.iov[se_cmd->t_data_nents + 2]),
+				sizeof(struct tcmu_cmd_entry));
+	command_size = base_command_size
+		+ round_up(scsi_command_size(se_cmd->t_task_cdb), TCMU_OP_ALIGN_SIZE);
+
+	WARN_ON(command_size & (TCMU_OP_ALIGN_SIZE-1));
+
+	spin_lock_irq(&udev->cmdr_lock);
+
+	mb = udev->mb_addr;
+	cmd_head = mb->cmd_head % udev->cmdr_size; /* UAM */
+	if ((command_size > (udev->cmdr_size / 2))
+	    || tcmu_cmd->data_length > (udev->data_size - 1))
+		pr_warn("TCMU: Request of size %zu/%zu may be too big for %u/%zu "
+			"cmd/data ring buffers\n", command_size, tcmu_cmd->data_length,
+			udev->cmdr_size, udev->data_size);
+
+	while (!is_ring_space_avail(udev, command_size, tcmu_cmd->data_length)) {
+		int ret;
+		DEFINE_WAIT(__wait);
+
+		prepare_to_wait(&udev->wait_cmdr, &__wait, TASK_INTERRUPTIBLE);
+
+		pr_debug("sleeping for ring space\n");
+		spin_unlock_irq(&udev->cmdr_lock);
+		ret = schedule_timeout(msecs_to_jiffies(TCMU_TIME_OUT));
+		finish_wait(&udev->wait_cmdr, &__wait);
+		if (!ret) {
+			pr_warn("tcmu: command timed out\n");
+			return -ETIMEDOUT;
+		}
+
+		spin_lock_irq(&udev->cmdr_lock);
+
+		/* We dropped cmdr_lock, cmd_head is stale */
+		cmd_head = mb->cmd_head % udev->cmdr_size; /* UAM */
+	}
+
+	/* Insert a PAD if end-of-ring space is too small */
+	if (head_to_end(cmd_head, udev->cmdr_size) < command_size) {
+		size_t pad_size = head_to_end(cmd_head, udev->cmdr_size);
+
+		entry = (void *) mb + CMDR_OFF + cmd_head;
+		tcmu_flush_dcache_range(entry, sizeof(*entry));
+		tcmu_hdr_set_op(&entry->hdr, TCMU_OP_PAD);
+		tcmu_hdr_set_len(&entry->hdr, pad_size);
+
+		UPDATE_HEAD(mb->cmd_head, pad_size, udev->cmdr_size);
+
+		cmd_head = mb->cmd_head % udev->cmdr_size; /* UAM */
+		WARN_ON(cmd_head != 0);
+	}
+
+	entry = (void *) mb + CMDR_OFF + cmd_head;
+	tcmu_flush_dcache_range(entry, sizeof(*entry));
+	tcmu_hdr_set_op(&entry->hdr, TCMU_OP_CMD);
+	tcmu_hdr_set_len(&entry->hdr, command_size);
+	entry->cmd_id = tcmu_cmd->cmd_id;
+
+	/*
+	 * Fix up iovecs, and handle if allocation in data ring wrapped.
+	 */
+	iov = &entry->req.iov[0];
+	for_each_sg(se_cmd->t_data_sg, sg, se_cmd->t_data_nents, i) {
+		size_t copy_bytes = min((size_t)sg->length,
+				     head_to_end(udev->data_head, udev->data_size));
+		void *from = kmap_atomic(sg_page(sg)) + sg->offset;
+		void *to = (void *) mb + udev->data_off + udev->data_head;
+
+		if (tcmu_cmd->se_cmd->data_direction == DMA_TO_DEVICE) {
+			memcpy(to, from, copy_bytes);
+			tcmu_flush_dcache_range(to, copy_bytes);
+		}
+
+		/* Even iov_base is relative to mb_addr */
+		iov->iov_len = copy_bytes;
+		iov->iov_base = (void *) udev->data_off + udev->data_head;
+		iov_cnt++;
+		iov++;
+
+		UPDATE_HEAD(udev->data_head, copy_bytes, udev->data_size);
+
+		/* Uh oh, we wrapped the buffer. Must split sg across 2 iovs. */
+		if (sg->length != copy_bytes) {
+			from += copy_bytes;
+			copy_bytes = sg->length - copy_bytes;
+
+			iov->iov_len = copy_bytes;
+			iov->iov_base = (void *) udev->data_off + udev->data_head;
+
+			if (se_cmd->data_direction == DMA_TO_DEVICE) {
+				to = (void *) mb + udev->data_off + udev->data_head;
+				memcpy(to, from, copy_bytes);
+				tcmu_flush_dcache_range(to, copy_bytes);
+			}
+
+			iov_cnt++;
+			iov++;
+
+			UPDATE_HEAD(udev->data_head, copy_bytes, udev->data_size);
+		}
+
+		kunmap_atomic(from);
+	}
+	entry->req.iov_cnt = iov_cnt;
+
+	/* All offsets relative to mb_addr, not start of entry! */
+	cdb_off = CMDR_OFF + cmd_head + base_command_size;
+	memcpy((void *) mb + cdb_off, se_cmd->t_task_cdb, scsi_command_size(se_cmd->t_task_cdb));
+	entry->req.cdb_off = cdb_off;
+	tcmu_flush_dcache_range(entry, sizeof(*entry));
+
+	UPDATE_HEAD(mb->cmd_head, command_size, udev->cmdr_size);
+	tcmu_flush_dcache_range(mb, sizeof(*mb));
+
+	spin_unlock_irq(&udev->cmdr_lock);
+
+	/* TODO: only if FLUSH and FUA? */
+	uio_event_notify(&udev->uio_info);
+
+	mod_timer(&udev->timeout,
+		round_jiffies_up(jiffies + msecs_to_jiffies(TCMU_TIME_OUT)));
+
+	return 0;
+}
+
+static int tcmu_queue_cmd(struct se_cmd *se_cmd)
+{
+	struct se_device *se_dev = se_cmd->se_dev;
+	struct tcmu_dev *udev = TCMU_DEV(se_dev);
+	struct tcmu_cmd *tcmu_cmd;
+	int ret;
+
+	tcmu_cmd = tcmu_alloc_cmd(se_cmd);
+	if (!tcmu_cmd)
+		return -ENOMEM;
+
+	ret = tcmu_queue_cmd_ring(tcmu_cmd);
+	if (ret < 0) {
+		pr_err("TCMU: Could not queue command\n");
+		spin_lock_irq(&udev->commands_lock);
+		idr_remove(&udev->commands, tcmu_cmd->cmd_id);
+		spin_unlock_irq(&udev->commands_lock);
+
+		kmem_cache_free(tcmu_cmd_cache, tcmu_cmd);
+	}
+
+	return ret;
+}
+
+static void tcmu_handle_completion(struct tcmu_cmd *cmd, struct tcmu_cmd_entry *entry)
+{
+	struct se_cmd *se_cmd = cmd->se_cmd;
+	struct tcmu_dev *udev = cmd->tcmu_dev;
+
+	if (test_bit(TCMU_CMD_BIT_EXPIRED, &cmd->flags)) {
+		/* cmd has been completed already from timeout, just reclaim data
+		   ring space */
+		UPDATE_HEAD(udev->data_tail, cmd->data_length, udev->data_size);
+		return;
+	}
+
+	if (entry->rsp.scsi_status == SAM_STAT_CHECK_CONDITION) {
+		memcpy(se_cmd->sense_buffer, entry->rsp.sense_buffer,
+			       se_cmd->scsi_sense_length);
+
+		UPDATE_HEAD(udev->data_tail, cmd->data_length, udev->data_size);
+	}
+	else if (se_cmd->data_direction == DMA_FROM_DEVICE) {
+		struct scatterlist *sg;
+		int i;
+
+		/* It'd be easier to look at entry's iovec again, but UAM */
+		for_each_sg(se_cmd->t_data_sg, sg, se_cmd->t_data_nents, i) {
+			size_t copy_bytes;
+			void *to;
+			void *from;
+
+			copy_bytes = min((size_t)sg->length,
+					 head_to_end(udev->data_tail, udev->data_size));
+
+			to = kmap_atomic(sg_page(sg)) + sg->offset;
+			WARN_ON(sg->length + sg->offset > PAGE_SIZE);
+			from = (void *) udev->mb_addr + udev->data_off + udev->data_tail;
+			tcmu_flush_dcache_range(from, copy_bytes);
+			memcpy(to, from, copy_bytes);
+
+			UPDATE_HEAD(udev->data_tail, copy_bytes, udev->data_size);
+
+			/* Uh oh, wrapped the data buffer for this sg's data */
+			if (sg->length != copy_bytes) {
+				from = (void *) udev->mb_addr + udev->data_off + udev->data_tail;
+				WARN_ON(udev->data_tail);
+				to += copy_bytes;
+				copy_bytes = sg->length - copy_bytes;
+				tcmu_flush_dcache_range(from, copy_bytes);
+				memcpy(to, from, copy_bytes);
+
+				UPDATE_HEAD(udev->data_tail, copy_bytes, udev->data_size);
+			}
+
+			kunmap_atomic(to);
+		}
+
+	} else if (se_cmd->data_direction == DMA_TO_DEVICE) {
+		UPDATE_HEAD(udev->data_tail, cmd->data_length, udev->data_size);
+	} else {
+		pr_warn("TCMU: data direction was %d!\n", se_cmd->data_direction);
+	}
+
+	target_complete_cmd(cmd->se_cmd, entry->rsp.scsi_status);
+	cmd->se_cmd = NULL;
+
+	kmem_cache_free(tcmu_cmd_cache, cmd);
+}
+
+static unsigned int tcmu_handle_completions(struct tcmu_dev *udev)
+{
+	struct tcmu_mailbox *mb;
+	LIST_HEAD(cpl_cmds);
+	unsigned long flags;
+	int handled = 0;
+
+	if (test_bit(TCMU_DEV_BIT_BROKEN, &udev->flags)) {
+		pr_err("ring broken, not handling completions\n");
+		return 0;
+	}
+
+	spin_lock_irqsave(&udev->cmdr_lock, flags);
+
+	mb = udev->mb_addr;
+	tcmu_flush_dcache_range(mb, sizeof(*mb));
+
+	while (udev->cmdr_last_cleaned != ACCESS_ONCE(mb->cmd_tail)) {
+
+		struct tcmu_cmd_entry *entry = (void *) mb + CMDR_OFF + udev->cmdr_last_cleaned;
+		struct tcmu_cmd *cmd;
+
+		tcmu_flush_dcache_range(entry, sizeof(*entry));
+
+		if (tcmu_hdr_get_op(&entry->hdr) == TCMU_OP_PAD) {
+			UPDATE_HEAD(udev->cmdr_last_cleaned, tcmu_hdr_get_len(&entry->hdr), udev->cmdr_size);
+			continue;
+		}
+		WARN_ON(tcmu_hdr_get_op(&entry->hdr) != TCMU_OP_CMD);
+
+		spin_lock(&udev->commands_lock);
+		cmd = idr_find(&udev->commands, entry->cmd_id);
+		if (cmd)
+			idr_remove(&udev->commands, cmd->cmd_id);
+		spin_unlock(&udev->commands_lock);
+
+		if (!cmd) {
+			pr_err("cmd_id not found, ring is broken\n");
+			set_bit(TCMU_DEV_BIT_BROKEN, &udev->flags);
+			break;
+		}
+
+		tcmu_handle_completion(cmd, entry);
+
+		UPDATE_HEAD(udev->cmdr_last_cleaned, tcmu_hdr_get_len(&entry->hdr), udev->cmdr_size);
+
+		handled++;
+	}
+
+	if (mb->cmd_tail == mb->cmd_head)
+		del_timer(&udev->timeout); /* no more pending cmds */
+
+	spin_unlock_irqrestore(&udev->cmdr_lock, flags);
+
+	wake_up(&udev->wait_cmdr);
+
+	return handled;
+}
+
+static int tcmu_check_expired_cmd(int id, void *p, void *data)
+{
+	struct tcmu_cmd *cmd = p;
+
+	if (test_bit(TCMU_CMD_BIT_EXPIRED, &cmd->flags))
+		return 0;
+
+	if (!time_after(cmd->deadline, jiffies))
+		return 0;
+
+	set_bit(TCMU_CMD_BIT_EXPIRED, &cmd->flags);
+	target_complete_cmd(cmd->se_cmd, SAM_STAT_CHECK_CONDITION);
+	cmd->se_cmd = NULL;
+
+	kmem_cache_free(tcmu_cmd_cache, cmd);
+
+	return 0;
+}
+
+static void tcmu_device_timedout(unsigned long data)
+{
+	struct tcmu_dev *udev = (struct tcmu_dev *)data;
+	unsigned long flags;
+	int handled;
+
+	handled = tcmu_handle_completions(udev);
+
+	pr_warn("%d completions handled from timeout\n", handled);
+
+	spin_lock_irqsave(&udev->commands_lock, flags);
+	idr_for_each(&udev->commands, tcmu_check_expired_cmd, NULL);
+	spin_unlock_irqrestore(&udev->commands_lock, flags);
+
+	/*
+	 * We don't need to wakeup threads on wait_cmdr since they have their
+	 * own timeout.
+	 */
+}
+
+static int tcmu_attach_hba(struct se_hba *hba, u32 host_id)
+{
+	struct tcmu_hba *tcmu_hba;
+
+	tcmu_hba = kzalloc(sizeof(struct tcmu_hba), GFP_KERNEL);
+	if (!tcmu_hba)
+		return -ENOMEM;
+
+	tcmu_hba->host_id = host_id;
+	hba->hba_ptr = tcmu_hba;
+
+	return 0;
+}
+
+static void tcmu_detach_hba(struct se_hba *hba)
+{
+	kfree(hba->hba_ptr);
+	hba->hba_ptr = NULL;
+}
+
+static struct se_device *tcmu_alloc_device(struct se_hba *hba, const char *name)
+{
+	struct tcmu_dev *udev;
+
+	udev = kzalloc(sizeof(struct tcmu_dev), GFP_KERNEL);
+	if (!udev)
+		return NULL;
+
+	udev->name = kstrdup(name, GFP_KERNEL);
+	if (!udev->name) {
+		kfree(udev);
+		return NULL;
+	}
+
+	udev->hba = hba;
+
+	init_waitqueue_head(&udev->wait_cmdr);
+	spin_lock_init(&udev->cmdr_lock);
+
+	idr_init(&udev->commands);
+	spin_lock_init(&udev->commands_lock);
+
+	setup_timer(&udev->timeout, tcmu_device_timedout,
+		(unsigned long)udev);
+
+	udev->pass_level = TCMU_PASS_ALL;
+
+	return &udev->se_dev;
+}
+
+static int tcmu_irqcontrol(struct uio_info *info, s32 irq_on)
+{
+	struct tcmu_dev *tcmu_dev = container_of(info, struct tcmu_dev, uio_info);
+
+	tcmu_handle_completions(tcmu_dev);
+
+	return 0;
+}
+
+/*
+ * mmap code from uio.c. Copied here because we want to hook mmap()
+ * and this stuff must come along.
+ */
+static int tcmu_find_mem_index(struct vm_area_struct *vma)
+{
+	struct tcmu_dev *udev = vma->vm_private_data;
+	struct uio_info *info = &udev->uio_info;
+
+	if (vma->vm_pgoff < MAX_UIO_MAPS) {
+		if (info->mem[vma->vm_pgoff].size == 0)
+			return -1;
+		return (int)vma->vm_pgoff;
+	}
+	return -1;
+}
+
+static int tcmu_vma_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct tcmu_dev *udev = vma->vm_private_data;
+	struct uio_info *info = &udev->uio_info;
+	struct page *page;
+	unsigned long offset;
+	void *addr;
+
+	int mi = tcmu_find_mem_index(vma);
+	if (mi < 0)
+		return VM_FAULT_SIGBUS;
+
+	/*
+	 * We need to subtract mi because userspace uses offset = N*PAGE_SIZE
+	 * to use mem[N].
+	 */
+	offset = (vmf->pgoff - mi) << PAGE_SHIFT;
+
+	addr = (void *)(unsigned long)info->mem[mi].addr + offset;
+	if (info->mem[mi].memtype == UIO_MEM_LOGICAL)
+		page = virt_to_page(addr);
+	else
+		page = vmalloc_to_page(addr);
+	get_page(page);
+	vmf->page = page;
+	return 0;
+}
+
+static const struct vm_operations_struct tcmu_vm_ops = {
+	.fault = tcmu_vma_fault,
+};
+
+static int tcmu_mmap(struct uio_info *info, struct vm_area_struct *vma)
+{
+	struct tcmu_dev *udev = container_of(info, struct tcmu_dev, uio_info);
+
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTDUMP;
+	vma->vm_ops = &tcmu_vm_ops;
+
+	vma->vm_private_data = udev;
+
+	/* Ensure the mmap is exactly the right size */
+	if (vma_pages(vma) != (TCMU_RING_SIZE >> PAGE_SHIFT))
+		return -EINVAL;
+
+	return 0;
+}
+
+static int tcmu_open(struct uio_info *info, struct inode *inode)
+{
+	struct tcmu_dev *udev = container_of(info, struct tcmu_dev, uio_info);
+
+	/* O_EXCL not supported for char devs, so fake it? */
+	if (test_and_set_bit(TCMU_DEV_BIT_OPEN, &udev->flags))
+		return -EBUSY;
+
+	pr_debug("open\n");
+
+	return 0;
+}
+
+static int tcmu_release(struct uio_info *info, struct inode *inode)
+{
+	struct tcmu_dev *udev = container_of(info, struct tcmu_dev, uio_info);
+
+	clear_bit(TCMU_DEV_BIT_OPEN, &udev->flags);
+
+	pr_debug("close\n");
+
+	return 0;
+}
+
+static int tcmu_netlink_event(enum tcmu_genl_cmd cmd, const char *name, int minor)
+{
+	struct sk_buff *skb;
+	void *msg_header;
+	int ret = -ENOMEM;
+
+	skb = genlmsg_new(NLMSG_GOODSIZE, GFP_KERNEL);
+	if (!skb)
+		return ret;
+
+	msg_header = genlmsg_put(skb, 0, 0, &tcmu_genl_family, 0, cmd);
+	if (!msg_header)
+		goto free_skb;
+
+	ret = nla_put_string(skb, TCMU_ATTR_DEVICE, name);
+	if (ret < 0)
+		goto free_skb;
+
+	ret = nla_put_u32(skb, TCMU_ATTR_MINOR, minor);
+	if (ret < 0)
+		goto free_skb;
+
+	ret = genlmsg_end(skb, msg_header);
+	if (ret < 0)
+		goto free_skb;
+
+	ret = genlmsg_multicast(&tcmu_genl_family, skb, 0,
+				TCMU_MCGRP_CONFIG, GFP_KERNEL);
+
+	/* We don't care if no one is listening */
+	if (ret == -ESRCH)
+		ret = 0;
+
+	return ret;
+free_skb:
+	nlmsg_free(skb);
+	return ret;
+}
+
+static int tcmu_configure_device(struct se_device *dev)
+{
+	struct tcmu_dev *udev = TCMU_DEV(dev);
+	struct tcmu_hba *hba = udev->hba->hba_ptr;
+	struct uio_info *info;
+	struct tcmu_mailbox *mb;
+	size_t size;
+	size_t used;
+	int ret = 0;
+	char *str;
+
+	info = &udev->uio_info;
+
+	size = snprintf(NULL, 0, "tcm-user/%u/%s/%s", hba->host_id, udev->name,
+			udev->dev_config);
+	size += 1; /* for \0 */
+	str = kmalloc(size, GFP_KERNEL);
+	if (!str)
+		return -ENOMEM;
+
+	used = snprintf(str, size, "tcm-user/%u/%s", hba->host_id, udev->name);
+
+	if (udev->dev_config[0])
+		snprintf(str + used, size - used, "/%s", udev->dev_config);
+
+	info->name = str;
+
+	udev->mb_addr = vzalloc(TCMU_RING_SIZE);
+	if (!udev->mb_addr) {
+		ret = -ENOMEM;
+		goto err_vzalloc;
+	}
+
+	/* mailbox fits in first part of CMDR space */
+	udev->cmdr_size = CMDR_SIZE - CMDR_OFF;
+	udev->data_off = CMDR_SIZE;
+	udev->data_size = TCMU_RING_SIZE - CMDR_SIZE;
+
+	mb = udev->mb_addr;
+	mb->version = 1;
+	mb->cmdr_off = CMDR_OFF;
+	mb->cmdr_size = udev->cmdr_size;
+
+	WARN_ON(!PAGE_ALIGNED(udev->data_off));
+	WARN_ON(udev->data_size % PAGE_SIZE);
+
+	info->version = "1";
+
+	info->mem[0].name = "tcm-user command & data buffer";
+	info->mem[0].addr = (phys_addr_t) udev->mb_addr;
+	info->mem[0].size = TCMU_RING_SIZE;
+	info->mem[0].memtype = UIO_MEM_VIRTUAL;
+
+	info->irqcontrol = tcmu_irqcontrol;
+	info->irq = UIO_IRQ_CUSTOM;
+
+	info->mmap = tcmu_mmap;
+	info->open = tcmu_open;
+	info->release = tcmu_release;
+
+	ret = uio_register_device(tcmu_root_device, info);
+	if (ret)
+		goto err_register;
+
+	/* Other attributes can be configured in userspace */
+	dev->dev_attrib.hw_block_size = 512;
+	dev->dev_attrib.hw_max_sectors = 128;
+	dev->dev_attrib.hw_queue_depth = 128;
+
+	ret = tcmu_netlink_event(TCMU_CMD_ADDED_DEVICE, udev->uio_info.name,
+				 udev->uio_info.uio_dev->minor);
+	if (ret)
+		goto err_netlink;
+
+	return 0;
+
+err_netlink:
+	uio_unregister_device(&udev->uio_info);
+err_register:
+	vfree(udev->mb_addr);
+err_vzalloc:
+	kfree(info->name);
+
+	return ret;
+}
+
+static int tcmu_check_pending_cmd(int id, void *p, void *data)
+{
+	struct tcmu_cmd *cmd = p;
+
+	if (test_bit(TCMU_CMD_BIT_EXPIRED, &cmd->flags))
+		return 0;
+	return -EINVAL;
+}
+
+static void tcmu_free_device(struct se_device *dev)
+{
+	struct tcmu_dev *udev = TCMU_DEV(dev);
+	int i;
+
+	del_timer_sync(&udev->timeout);
+
+	vfree(udev->mb_addr);
+
+	/* Upper layer should drain all requests before calling this */
+	spin_lock_irq(&udev->commands_lock);
+	i = idr_for_each(&udev->commands, tcmu_check_pending_cmd, NULL);
+	idr_destroy(&udev->commands);
+	spin_unlock_irq(&udev->commands_lock);
+	WARN_ON(i);
+
+	/* Device was configured */
+	if (udev->uio_info.uio_dev) {
+		tcmu_netlink_event(TCMU_CMD_REMOVED_DEVICE, udev->uio_info.name,
+				   udev->uio_info.uio_dev->minor);
+
+		uio_unregister_device(&udev->uio_info);
+		kfree(udev->uio_info.name);
+		kfree(udev->name);
+	}
+
+	kfree(udev);
+}
+
+enum {
+	Opt_dev_config, Opt_dev_size, Opt_err, Opt_pass_level,
+};
+
+static match_table_t tokens = {
+	{Opt_dev_config, "dev_config=%s"},
+	{Opt_dev_size, "dev_size=%u"},
+	{Opt_pass_level, "pass_level=%u"},
+	{Opt_err, NULL}
+};
+
+static ssize_t tcmu_set_configfs_dev_params(struct se_device *dev,
+		const char *page, ssize_t count)
+{
+	struct tcmu_dev *udev = TCMU_DEV(dev);
+	char *orig, *ptr, *opts, *arg_p;
+	substring_t args[MAX_OPT_ARGS];
+	int ret = 0, token;
+	int arg;
+
+	opts = kstrdup(page, GFP_KERNEL);
+	if (!opts)
+		return -ENOMEM;
+
+	orig = opts;
+
+	while ((ptr = strsep(&opts, ",\n")) != NULL) {
+		if (!*ptr)
+			continue;
+
+		token = match_token(ptr, tokens, args);
+		switch (token) {
+		case Opt_dev_config:
+			if (match_strlcpy(udev->dev_config, &args[0],
+					  TCMU_CONFIG_LEN) == 0) {
+				ret = -EINVAL;
+				break;
+			}
+			pr_debug("TCMU: Referencing Path: %s\n", udev->dev_config);
+			break;
+		case Opt_dev_size:
+			arg_p = match_strdup(&args[0]);
+			if (!arg_p) {
+				ret = -ENOMEM;
+				break;
+			}
+			ret = kstrtoul(arg_p, 0, (unsigned long *) &udev->dev_size);
+			kfree(arg_p);
+			if (ret < 0)
+				pr_err("kstrtoul() failed for dev_size=\n");
+			break;
+		case Opt_pass_level:
+			match_int(args, &arg);
+			if (arg >= TCMU_PASS_INVALID) {
+				pr_warn("TCMU: Invalid pass_level: %d\n", arg);
+				break;
+			}
+
+			pr_debug("TCMU: Setting pass_level to %d\n", arg);
+			udev->pass_level = arg;
+			break;
+		default:
+			break;
+		}
+	}
+
+	kfree(orig);
+	return (!ret) ? count : ret;
+}
+
+static ssize_t tcmu_show_configfs_dev_params(struct se_device *dev, char *b)
+{
+	struct tcmu_dev *udev = TCMU_DEV(dev);
+	ssize_t bl = 0;
+
+	bl = sprintf(b + bl, "Config: %s ",
+		     udev->dev_config[0] ? udev->dev_config : "NULL");
+	bl += sprintf(b + bl, "Size: %zu PassLevel: %u\n",
+		      udev->dev_size, udev->pass_level);
+
+	return bl;
+}
+
+static sector_t tcmu_get_blocks(struct se_device *dev)
+{
+	struct tcmu_dev *udev = TCMU_DEV(dev);
+
+	return div_u64(udev->dev_size - dev->dev_attrib.block_size,
+		       dev->dev_attrib.block_size);
+}
+
+static sense_reason_t
+tcmu_execute_rw(struct se_cmd *se_cmd, struct scatterlist *sgl, u32 sgl_nents,
+		enum dma_data_direction data_direction)
+{
+	int ret;
+
+	ret = tcmu_queue_cmd(se_cmd);
+
+	if (ret != 0)
+		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+	else
+		return TCM_NO_SENSE;
+}
+
+static sense_reason_t
+tcmu_pass_op(struct se_cmd *se_cmd)
+{
+	int ret = tcmu_queue_cmd(se_cmd);
+
+	if (ret != 0)
+		return TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE;
+	else
+		return TCM_NO_SENSE;
+}
+
+static struct sbc_ops tcmu_sbc_ops = {
+	.execute_rw = tcmu_execute_rw,
+	.execute_sync_cache	= tcmu_pass_op,
+	.execute_write_same	= tcmu_pass_op,
+	.execute_write_same_unmap = tcmu_pass_op,
+	.execute_unmap		= tcmu_pass_op,
+};
+
+static sense_reason_t
+tcmu_parse_cdb(struct se_cmd *cmd)
+{
+	unsigned char *cdb = cmd->t_task_cdb;
+	struct tcmu_dev *udev = TCMU_DEV(cmd->se_dev);
+	sense_reason_t ret;
+
+	switch (udev->pass_level) {
+	case TCMU_PASS_ALL:
+		/* We're just like pscsi, then */
+		/*
+		 * For REPORT LUNS we always need to emulate the response, for everything
+		 * else, pass it up.
+		 */
+		switch (cdb[0]) {
+		case REPORT_LUNS:
+			cmd->execute_cmd = spc_emulate_report_luns;
+			break;
+		case READ_6:
+		case READ_10:
+		case READ_12:
+		case READ_16:
+		case WRITE_6:
+		case WRITE_10:
+		case WRITE_12:
+		case WRITE_16:
+		case WRITE_VERIFY:
+			cmd->se_cmd_flags |= SCF_SCSI_DATA_CDB;
+			/* FALLTHROUGH */
+		default:
+			cmd->execute_cmd = tcmu_pass_op;
+		}
+		ret = TCM_NO_SENSE;
+		break;
+	case TCMU_PASS_IO:
+		ret = sbc_parse_cdb(cmd, &tcmu_sbc_ops);
+		break;
+	default:
+		pr_err("Unknown tcm-user pass level %d\n", udev->pass_level);
+		ret = TCM_CHECK_CONDITION_ABORT_CMD;
+	}
+
+	return ret;
+}
+
+static struct se_subsystem_api tcmu_template = {
+	.name			= "user",
+	.inquiry_prod		= "USER",
+	.inquiry_rev		= TCMU_VERSION,
+	.owner			= THIS_MODULE,
+	.transport_type		= TRANSPORT_PLUGIN_VHBA_PDEV,
+	.attach_hba		= tcmu_attach_hba,
+	.detach_hba		= tcmu_detach_hba,
+	.alloc_device		= tcmu_alloc_device,
+	.configure_device	= tcmu_configure_device,
+	.free_device		= tcmu_free_device,
+	.parse_cdb		= tcmu_parse_cdb,
+	.set_configfs_dev_params = tcmu_set_configfs_dev_params,
+	.show_configfs_dev_params = tcmu_show_configfs_dev_params,
+	.get_device_type	= sbc_get_device_type,
+	.get_blocks		= tcmu_get_blocks,
+};
+
+static int __init tcmu_module_init(void)
+{
+	int ret;
+
+	BUILD_BUG_ON((sizeof(struct tcmu_cmd_entry) % TCMU_OP_ALIGN_SIZE) != 0);
+
+	tcmu_cmd_cache = kmem_cache_create("tcmu_cmd_cache",
+				sizeof(struct tcmu_cmd),
+				__alignof__(struct tcmu_cmd),
+				0, NULL);
+	if (!tcmu_cmd_cache)
+		return -ENOMEM;
+
+	tcmu_root_device = root_device_register("tcm_user");
+	if (IS_ERR(tcmu_root_device)) {
+		ret = PTR_ERR(tcmu_root_device);
+		goto out_free_cache;
+	}
+
+	ret = genl_register_family(&tcmu_genl_family);
+	if (ret < 0) {
+		goto out_unreg_device;
+	}
+
+	ret = transport_subsystem_register(&tcmu_template);
+	if (ret)
+		goto out_unreg_genl;
+
+	return 0;
+
+out_unreg_genl:
+	genl_unregister_family(&tcmu_genl_family);
+out_unreg_device:
+	root_device_unregister(tcmu_root_device);
+out_free_cache:
+	kmem_cache_destroy(tcmu_cmd_cache);
+
+	return ret;
+}
+
+static void __exit tcmu_module_exit(void)
+{
+	transport_subsystem_release(&tcmu_template);
+	genl_unregister_family(&tcmu_genl_family);
+	root_device_unregister(tcmu_root_device);
+	kmem_cache_destroy(tcmu_cmd_cache);
+}
+
+MODULE_DESCRIPTION("TCM USER subsystem plugin");
+MODULE_AUTHOR("Shaohua Li <shli@kernel.org>");
+MODULE_AUTHOR("Andy Grover <agrover@redhat.com>");
+MODULE_LICENSE("GPL");
+
+module_init(tcmu_module_init);
+module_exit(tcmu_module_exit);
diff --git a/drivers/target/tcm_fc/tfc_sess.c b/drivers/target/tcm_fc/tfc_sess.c
index 21ce50880c79..ccee7e332a4d 100644
--- a/drivers/target/tcm_fc/tfc_sess.c
+++ b/drivers/target/tcm_fc/tfc_sess.c
@@ -98,7 +98,7 @@ static void ft_tport_delete(struct ft_tport *tport)
 	ft_sess_delete_all(tport);
 	lport = tport->lport;
 	BUG_ON(tport != lport->prov[FC_TYPE_FCP]);
-	rcu_assign_pointer(lport->prov[FC_TYPE_FCP], NULL);
+	RCU_INIT_POINTER(lport->prov[FC_TYPE_FCP], NULL);
 
 	tpg = tport->tpg;
 	if (tpg) {
diff --git a/drivers/thermal/Kconfig b/drivers/thermal/Kconfig
index 693208eb9047..f554d25b4399 100644
--- a/drivers/thermal/Kconfig
+++ b/drivers/thermal/Kconfig
@@ -84,6 +84,16 @@ config THERMAL_GOV_STEP_WISE
 	  Enable this to manage platform thermals using a simple linear
 	  governor.
 
+config THERMAL_GOV_BANG_BANG
+	bool "Bang Bang thermal governor"
+	default n
+	help
+	  Enable this to manage platform thermals using bang bang governor.
+
+	  Say 'Y' here if you want to use two point temperature regulation
+	  used for fans without throttling.  Some fan drivers depend on this
+	  governor to be enabled (e.g. acerhdf).
+
 config THERMAL_GOV_USER_SPACE
 	bool "User_space thermal governor"
 	help
@@ -143,7 +153,7 @@ config RCAR_THERMAL
 
 config KIRKWOOD_THERMAL
 	tristate "Temperature sensor on Marvell Kirkwood SoCs"
-	depends on ARCH_KIRKWOOD || MACH_KIRKWOOD
+	depends on MACH_KIRKWOOD
 	depends on OF
 	help
 	  Support for the Kirkwood thermal sensor driver into the Linux thermal
@@ -207,21 +217,6 @@ config X86_PKG_TEMP_THERMAL
 	  two trip points which can be set by user to get notifications via thermal
 	  notification methods.
 
-config ACPI_INT3403_THERMAL
-	tristate "ACPI INT3403 thermal driver"
-	depends on X86 && ACPI
-	help
-	  Newer laptops and tablets that use ACPI may have thermal sensors
-	  outside the core CPU/SOC for thermal safety reasons. These
-	  temperature sensors are also exposed for the OS to use via the so
-	  called INT3403 ACPI object. This driver will, on devices that have
-	  such sensors, expose the temperature information from these sensors
-	  to userspace via the normal thermal framework. This means that a wide
-	  range of applications and GUI widgets can show this information to
-	  the user or use this information for making decisions. For example,
-	  the Intel Thermal Daemon can use this information to allow the user
-	  to select his laptop to run without turning on the fans.
-
 config INTEL_SOC_DTS_THERMAL
 	tristate "Intel SoCs DTS thermal driver"
 	depends on X86 && IOSF_MBI
@@ -234,6 +229,30 @@ config INTEL_SOC_DTS_THERMAL
 	  notification methods.The other trip is a critical trip point, which
 	  was set by the driver based on the TJ MAX temperature.
 
+config INT340X_THERMAL
+	tristate "ACPI INT340X thermal drivers"
+	depends on X86 && ACPI
+	select THERMAL_GOV_USER_SPACE
+	select ACPI_THERMAL_REL
+	select ACPI_FAN
+	help
+	  Newer laptops and tablets that use ACPI may have thermal sensors and
+	  other devices with thermal control capabilities outside the core
+	  CPU/SOC, for thermal safety reasons.
+	  They are exposed for the OS to use via the INT3400 ACPI device object
+	  as the master, and INT3401~INT340B ACPI device objects as the slaves.
+	  Enable this to expose the temperature information and cooling ability
+	  from these objects to userspace via the normal thermal framework.
+	  This means that a wide range of applications and GUI widgets can show
+	  the information to the user or use this information for making
+	  decisions. For example, the Intel Thermal Daemon can use this
+	  information to allow the user to select his laptop to run without
+	  turning on the fans.
+
+config ACPI_THERMAL_REL
+	tristate
+	depends on ACPI
+
 menu "Texas Instruments thermal drivers"
 source "drivers/thermal/ti-soc-thermal/Kconfig"
 endmenu
diff --git a/drivers/thermal/Makefile b/drivers/thermal/Makefile
index 31e232f84b6b..39c4fe87da2f 100644
--- a/drivers/thermal/Makefile
+++ b/drivers/thermal/Makefile
@@ -11,6 +11,7 @@ thermal_sys-$(CONFIG_THERMAL_OF)		+= of-thermal.o
 
 # governors
 thermal_sys-$(CONFIG_THERMAL_GOV_FAIR_SHARE)	+= fair_share.o
+thermal_sys-$(CONFIG_THERMAL_GOV_BANG_BANG)	+= gov_bang_bang.o
 thermal_sys-$(CONFIG_THERMAL_GOV_STEP_WISE)	+= step_wise.o
 thermal_sys-$(CONFIG_THERMAL_GOV_USER_SPACE)	+= user_space.o
 
@@ -31,5 +32,5 @@ obj-$(CONFIG_INTEL_POWERCLAMP)	+= intel_powerclamp.o
 obj-$(CONFIG_X86_PKG_TEMP_THERMAL)	+= x86_pkg_temp_thermal.o
 obj-$(CONFIG_INTEL_SOC_DTS_THERMAL)	+= intel_soc_dts_thermal.o
 obj-$(CONFIG_TI_SOC_THERMAL)	+= ti-soc-thermal/
-obj-$(CONFIG_ACPI_INT3403_THERMAL)	+= int3403_thermal.o
+obj-$(CONFIG_INT340X_THERMAL)  += int340x_thermal/
 obj-$(CONFIG_ST_THERMAL)	+= st/
diff --git a/drivers/thermal/fair_share.c b/drivers/thermal/fair_share.c
index 944ba2f340c8..6e0a3fbfae86 100644
--- a/drivers/thermal/fair_share.c
+++ b/drivers/thermal/fair_share.c
@@ -23,6 +23,7 @@
  */
 
 #include <linux/thermal.h>
+#include <trace/events/thermal.h>
 
 #include "thermal_core.h"
 
@@ -34,6 +35,7 @@ static int get_trip_level(struct thermal_zone_device *tz)
 {
 	int count = 0;
 	unsigned long trip_temp;
+	enum thermal_trip_type trip_type;
 
 	if (tz->trips == 0 || !tz->ops->get_trip_temp)
 		return 0;
@@ -43,6 +45,16 @@ static int get_trip_level(struct thermal_zone_device *tz)
 		if (tz->temperature < trip_temp)
 			break;
 	}
+
+	/*
+	 * count > 0 only if temperature is greater than first trip
+	 * point, in which case, trip_point = count - 1
+	 */
+	if (count > 0) {
+		tz->ops->get_trip_type(tz, count - 1, &trip_type);
+		trace_thermal_zone_trip(tz, count - 1, trip_type);
+	}
+
 	return count;
 }
 
diff --git a/drivers/thermal/gov_bang_bang.c b/drivers/thermal/gov_bang_bang.c
new file mode 100644
index 000000000000..c5dd76b2ee74
--- /dev/null
+++ b/drivers/thermal/gov_bang_bang.c
@@ -0,0 +1,131 @@
+/*
+ *  gov_bang_bang.c - A simple thermal throttling governor using hysteresis
+ *
+ *  Copyright (C) 2014 Peter Feuerer <peter@piie.net>
+ *
+ *  Based on step_wise.c with following Copyrights:
+ *  Copyright (C) 2012 Intel Corp
+ *  Copyright (C) 2012 Durgadoss R <durgadoss.r@intel.com>
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 2.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See
+ * the GNU General Public License for more details.
+ *
+ */
+
+#include <linux/thermal.h>
+
+#include "thermal_core.h"
+
+static void thermal_zone_trip_update(struct thermal_zone_device *tz, int trip)
+{
+	long trip_temp;
+	unsigned long trip_hyst;
+	struct thermal_instance *instance;
+
+	tz->ops->get_trip_temp(tz, trip, &trip_temp);
+	tz->ops->get_trip_hyst(tz, trip, &trip_hyst);
+
+	dev_dbg(&tz->device, "Trip%d[temp=%ld]:temp=%d:hyst=%ld\n",
+				trip, trip_temp, tz->temperature,
+				trip_hyst);
+
+	mutex_lock(&tz->lock);
+
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node) {
+		if (instance->trip != trip)
+			continue;
+
+		/* in case fan is in initial state, switch the fan off */
+		if (instance->target == THERMAL_NO_TARGET)
+			instance->target = 0;
+
+		/* in case fan is neither on nor off set the fan to active */
+		if (instance->target != 0 && instance->target != 1) {
+			pr_warn("Thermal instance %s controlled by bang-bang has unexpected state: %ld\n",
+					instance->name, instance->target);
+			instance->target = 1;
+		}
+
+		/*
+		 * enable fan when temperature exceeds trip_temp and disable
+		 * the fan in case it falls below trip_temp minus hysteresis
+		 */
+		if (instance->target == 0 && tz->temperature >= trip_temp)
+			instance->target = 1;
+		else if (instance->target == 1 &&
+				tz->temperature < trip_temp - trip_hyst)
+			instance->target = 0;
+
+		dev_dbg(&instance->cdev->device, "target=%d\n",
+					(int)instance->target);
+
+		instance->cdev->updated = false; /* cdev needs update */
+	}
+
+	mutex_unlock(&tz->lock);
+}
+
+/**
+ * bang_bang_control - controls devices associated with the given zone
+ * @tz - thermal_zone_device
+ * @trip - the trip point
+ *
+ * Regulation Logic: a two point regulation, deliver cooling state depending
+ * on the previous state shown in this diagram:
+ *
+ *                Fan:   OFF    ON
+ *
+ *                              |
+ *                              |
+ *          trip_temp:    +---->+
+ *                        |     |        ^
+ *                        |     |        |
+ *                        |     |   Temperature
+ * (trip_temp - hyst):    +<----+
+ *                        |
+ *                        |
+ *                        |
+ *
+ *   * If the fan is not running and temperature exceeds trip_temp, the fan
+ *     gets turned on.
+ *   * In case the fan is running, temperature must fall below
+ *     (trip_temp - hyst) so that the fan gets turned off again.
+ *
+ */
+static int bang_bang_control(struct thermal_zone_device *tz, int trip)
+{
+	struct thermal_instance *instance;
+
+	thermal_zone_trip_update(tz, trip);
+
+	mutex_lock(&tz->lock);
+
+	list_for_each_entry(instance, &tz->thermal_instances, tz_node)
+		thermal_cdev_update(instance->cdev);
+
+	mutex_unlock(&tz->lock);
+
+	return 0;
+}
+
+static struct thermal_governor thermal_gov_bang_bang = {
+	.name		= "bang_bang",
+	.throttle	= bang_bang_control,
+};
+
+int thermal_gov_bang_bang_register(void)
+{
+	return thermal_register_governor(&thermal_gov_bang_bang);
+}
+
+void thermal_gov_bang_bang_unregister(void)
+{
+	thermal_unregister_governor(&thermal_gov_bang_bang);
+}
diff --git a/drivers/thermal/imx_thermal.c b/drivers/thermal/imx_thermal.c
index 2c516f2eebed..461bf3d033a0 100644
--- a/drivers/thermal/imx_thermal.c
+++ b/drivers/thermal/imx_thermal.c
@@ -19,6 +19,7 @@
 #include <linux/mfd/syscon.h>
 #include <linux/module.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/regmap.h>
 #include <linux/slab.h>
@@ -31,6 +32,11 @@
 
 #define MISC0				0x0150
 #define MISC0_REFTOP_SELBIASOFF		(1 << 3)
+#define MISC1				0x0160
+#define MISC1_IRQ_TEMPHIGH		(1 << 29)
+/* Below LOW and PANIC bits are only for TEMPMON_IMX6SX */
+#define MISC1_IRQ_TEMPLOW		(1 << 28)
+#define MISC1_IRQ_TEMPPANIC		(1 << 27)
 
 #define TEMPSENSE0			0x0180
 #define TEMPSENSE0_ALARM_VALUE_SHIFT	20
@@ -43,6 +49,12 @@
 
 #define TEMPSENSE1			0x0190
 #define TEMPSENSE1_MEASURE_FREQ		0xffff
+/* Below TEMPSENSE2 is only for TEMPMON_IMX6SX */
+#define TEMPSENSE2			0x0290
+#define TEMPSENSE2_LOW_VALUE_SHIFT	0
+#define TEMPSENSE2_LOW_VALUE_MASK	0xfff
+#define TEMPSENSE2_PANIC_VALUE_SHIFT	16
+#define TEMPSENSE2_PANIC_VALUE_MASK	0xfff0000
 
 #define OCOTP_ANA1			0x04e0
 
@@ -66,6 +78,21 @@ enum imx_thermal_trip {
 #define FACTOR1				15976
 #define FACTOR2				4297157
 
+#define TEMPMON_IMX6Q			1
+#define TEMPMON_IMX6SX			2
+
+struct thermal_soc_data {
+	u32 version;
+};
+
+static struct thermal_soc_data thermal_imx6q_data = {
+	.version = TEMPMON_IMX6Q,
+};
+
+static struct thermal_soc_data thermal_imx6sx_data = {
+	.version = TEMPMON_IMX6SX,
+};
+
 struct imx_thermal_data {
 	struct thermal_zone_device *tz;
 	struct thermal_cooling_device *cdev;
@@ -79,8 +106,21 @@ struct imx_thermal_data {
 	bool irq_enabled;
 	int irq;
 	struct clk *thermal_clk;
+	const struct thermal_soc_data *socdata;
 };
 
+static void imx_set_panic_temp(struct imx_thermal_data *data,
+			       signed long panic_temp)
+{
+	struct regmap *map = data->tempmon;
+	int critical_value;
+
+	critical_value = (data->c2 - panic_temp) / data->c1;
+	regmap_write(map, TEMPSENSE2 + REG_CLR, TEMPSENSE2_PANIC_VALUE_MASK);
+	regmap_write(map, TEMPSENSE2 + REG_SET, critical_value <<
+			TEMPSENSE2_PANIC_VALUE_SHIFT);
+}
+
 static void imx_set_alarm_temp(struct imx_thermal_data *data,
 			       signed long alarm_temp)
 {
@@ -142,13 +182,17 @@ static int imx_get_temp(struct thermal_zone_device *tz, unsigned long *temp)
 	/* See imx_get_sensor_data() for formula derivation */
 	*temp = data->c2 - n_meas * data->c1;
 
-	/* Update alarm value to next higher trip point */
-	if (data->alarm_temp == data->temp_passive && *temp >= data->temp_passive)
-		imx_set_alarm_temp(data, data->temp_critical);
-	if (data->alarm_temp == data->temp_critical && *temp < data->temp_passive) {
-		imx_set_alarm_temp(data, data->temp_passive);
-		dev_dbg(&tz->device, "thermal alarm off: T < %lu\n",
-			data->alarm_temp / 1000);
+	/* Update alarm value to next higher trip point for TEMPMON_IMX6Q */
+	if (data->socdata->version == TEMPMON_IMX6Q) {
+		if (data->alarm_temp == data->temp_passive &&
+			*temp >= data->temp_passive)
+			imx_set_alarm_temp(data, data->temp_critical);
+		if (data->alarm_temp == data->temp_critical &&
+			*temp < data->temp_passive) {
+			imx_set_alarm_temp(data, data->temp_passive);
+			dev_dbg(&tz->device, "thermal alarm off: T < %lu\n",
+				data->alarm_temp / 1000);
+		}
 	}
 
 	if (*temp != data->last_temp) {
@@ -398,8 +442,17 @@ static irqreturn_t imx_thermal_alarm_irq_thread(int irq, void *dev)
 	return IRQ_HANDLED;
 }
 
+static const struct of_device_id of_imx_thermal_match[] = {
+	{ .compatible = "fsl,imx6q-tempmon", .data = &thermal_imx6q_data, },
+	{ .compatible = "fsl,imx6sx-tempmon", .data = &thermal_imx6sx_data, },
+	{ /* end */ }
+};
+MODULE_DEVICE_TABLE(of, of_imx_thermal_match);
+
 static int imx_thermal_probe(struct platform_device *pdev)
 {
+	const struct of_device_id *of_id =
+		of_match_device(of_imx_thermal_match, &pdev->dev);
 	struct imx_thermal_data *data;
 	struct cpumask clip_cpus;
 	struct regmap *map;
@@ -418,6 +471,20 @@ static int imx_thermal_probe(struct platform_device *pdev)
 	}
 	data->tempmon = map;
 
+	data->socdata = of_id->data;
+
+	/* make sure the IRQ flag is clear before enabling irq on i.MX6SX */
+	if (data->socdata->version == TEMPMON_IMX6SX) {
+		regmap_write(map, MISC1 + REG_CLR, MISC1_IRQ_TEMPHIGH |
+			MISC1_IRQ_TEMPLOW | MISC1_IRQ_TEMPPANIC);
+		/*
+		 * reset value of LOW ALARM is incorrect, set it to lowest
+		 * value to avoid false trigger of low alarm.
+		 */
+		regmap_write(map, TEMPSENSE2 + REG_SET,
+			TEMPSENSE2_LOW_VALUE_MASK);
+	}
+
 	data->irq = platform_get_irq(pdev, 0);
 	if (data->irq < 0)
 		return data->irq;
@@ -489,6 +556,10 @@ static int imx_thermal_probe(struct platform_device *pdev)
 	measure_freq = DIV_ROUND_UP(32768, 10); /* 10 Hz */
 	regmap_write(map, TEMPSENSE1 + REG_SET, measure_freq);
 	imx_set_alarm_temp(data, data->temp_passive);
+
+	if (data->socdata->version == TEMPMON_IMX6SX)
+		imx_set_panic_temp(data, data->temp_critical);
+
 	regmap_write(map, TEMPSENSE0 + REG_CLR, TEMPSENSE0_POWER_DOWN);
 	regmap_write(map, TEMPSENSE0 + REG_SET, TEMPSENSE0_MEASURE_TEMP);
 
@@ -550,12 +621,6 @@ static int imx_thermal_resume(struct device *dev)
 static SIMPLE_DEV_PM_OPS(imx_thermal_pm_ops,
 			 imx_thermal_suspend, imx_thermal_resume);
 
-static const struct of_device_id of_imx_thermal_match[] = {
-	{ .compatible = "fsl,imx6q-tempmon", },
-	{ /* end */ }
-};
-MODULE_DEVICE_TABLE(of, of_imx_thermal_match);
-
 static struct platform_driver imx_thermal = {
 	.driver = {
 		.name	= "imx_thermal",
diff --git a/drivers/thermal/int3403_thermal.c b/drivers/thermal/int3403_thermal.c
deleted file mode 100644
index 17554eeb3953..000000000000
--- a/drivers/thermal/int3403_thermal.c
+++ /dev/null
@@ -1,296 +0,0 @@
-/*
- * ACPI INT3403 thermal driver
- * Copyright (c) 2013, Intel Corporation.
- *
- * This program is free software; you can redistribute it and/or modify it
- * under the terms and conditions of the GNU General Public License,
- * version 2, as published by the Free Software Foundation.
- *
- * This program is distributed in the hope it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- */
-
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/init.h>
-#include <linux/types.h>
-#include <linux/acpi.h>
-#include <linux/thermal.h>
-
-#define INT3403_TYPE_SENSOR		0x03
-#define INT3403_PERF_CHANGED_EVENT	0x80
-#define INT3403_THERMAL_EVENT		0x90
-
-#define DECI_KELVIN_TO_MILLI_CELSIUS(t, off) (((t) - (off)) * 100)
-#define KELVIN_OFFSET	2732
-#define MILLI_CELSIUS_TO_DECI_KELVIN(t, off) (((t) / 100) + (off))
-
-#define ACPI_INT3403_CLASS		"int3403"
-#define ACPI_INT3403_FILE_STATE		"state"
-
-struct int3403_sensor {
-	struct thermal_zone_device *tzone;
-	unsigned long *thresholds;
-	unsigned long	crit_temp;
-	int		crit_trip_id;
-	unsigned long	psv_temp;
-	int		psv_trip_id;
-};
-
-static int sys_get_curr_temp(struct thermal_zone_device *tzone,
-				unsigned long *temp)
-{
-	struct acpi_device *device = tzone->devdata;
-	unsigned long long tmp;
-	acpi_status status;
-
-	status = acpi_evaluate_integer(device->handle, "_TMP", NULL, &tmp);
-	if (ACPI_FAILURE(status))
-		return -EIO;
-
-	*temp = DECI_KELVIN_TO_MILLI_CELSIUS(tmp, KELVIN_OFFSET);
-
-	return 0;
-}
-
-static int sys_get_trip_hyst(struct thermal_zone_device *tzone,
-		int trip, unsigned long *temp)
-{
-	struct acpi_device *device = tzone->devdata;
-	unsigned long long hyst;
-	acpi_status status;
-
-	status = acpi_evaluate_integer(device->handle, "GTSH", NULL, &hyst);
-	if (ACPI_FAILURE(status))
-		return -EIO;
-
-	/*
-	 * Thermal hysteresis represents a temperature difference.
-	 * Kelvin and Celsius have same degree size. So the
-	 * conversion here between tenths of degree Kelvin unit
-	 * and Milli-Celsius unit is just to multiply 100.
-	 */
-	*temp = hyst * 100;
-
-	return 0;
-}
-
-static int sys_get_trip_temp(struct thermal_zone_device *tzone,
-		int trip, unsigned long *temp)
-{
-	struct acpi_device *device = tzone->devdata;
-	struct int3403_sensor *obj = acpi_driver_data(device);
-
-	if (trip == obj->crit_trip_id)
-		*temp = obj->crit_temp;
-	else if (trip == obj->psv_trip_id)
-		*temp = obj->psv_temp;
-	else {
-		/*
-		 * get_trip_temp is a mandatory callback but
-		 * PATx method doesn't return any value, so return
-		 * cached value, which was last set from user space.
-		 */
-		*temp = obj->thresholds[trip];
-	}
-
-	return 0;
-}
-
-static int sys_get_trip_type(struct thermal_zone_device *thermal,
-		int trip, enum thermal_trip_type *type)
-{
-	struct acpi_device *device = thermal->devdata;
-	struct int3403_sensor *obj = acpi_driver_data(device);
-
-	/* Mandatory callback, may not mean much here */
-	if (trip == obj->crit_trip_id)
-		*type = THERMAL_TRIP_CRITICAL;
-	else
-		*type = THERMAL_TRIP_PASSIVE;
-
-	return 0;
-}
-
-int sys_set_trip_temp(struct thermal_zone_device *tzone, int trip,
-							unsigned long temp)
-{
-	struct acpi_device *device = tzone->devdata;
-	acpi_status status;
-	char name[10];
-	int ret = 0;
-	struct int3403_sensor *obj = acpi_driver_data(device);
-
-	snprintf(name, sizeof(name), "PAT%d", trip);
-	if (acpi_has_method(device->handle, name)) {
-		status = acpi_execute_simple_method(device->handle, name,
-				MILLI_CELSIUS_TO_DECI_KELVIN(temp,
-							KELVIN_OFFSET));
-		if (ACPI_FAILURE(status))
-			ret = -EIO;
-		else
-			obj->thresholds[trip] = temp;
-	} else {
-		ret = -EIO;
-		dev_err(&device->dev, "sys_set_trip_temp: method not found\n");
-	}
-
-	return ret;
-}
-
-static struct thermal_zone_device_ops tzone_ops = {
-	.get_temp = sys_get_curr_temp,
-	.get_trip_temp = sys_get_trip_temp,
-	.get_trip_type = sys_get_trip_type,
-	.set_trip_temp = sys_set_trip_temp,
-	.get_trip_hyst =  sys_get_trip_hyst,
-};
-
-static void acpi_thermal_notify(struct acpi_device *device, u32 event)
-{
-	struct int3403_sensor *obj;
-
-	if (!device)
-		return;
-
-	obj = acpi_driver_data(device);
-	if (!obj)
-		return;
-
-	switch (event) {
-	case INT3403_PERF_CHANGED_EVENT:
-		break;
-	case INT3403_THERMAL_EVENT:
-		thermal_zone_device_update(obj->tzone);
-		break;
-	default:
-		dev_err(&device->dev, "Unsupported event [0x%x]\n", event);
-		break;
-	}
-}
-
-static int sys_get_trip_crt(struct acpi_device *device, unsigned long *temp)
-{
-	unsigned long long crt;
-	acpi_status status;
-
-	status = acpi_evaluate_integer(device->handle, "_CRT", NULL, &crt);
-	if (ACPI_FAILURE(status))
-		return -EIO;
-
-	*temp = DECI_KELVIN_TO_MILLI_CELSIUS(crt, KELVIN_OFFSET);
-
-	return 0;
-}
-
-static int sys_get_trip_psv(struct acpi_device *device, unsigned long *temp)
-{
-	unsigned long long psv;
-	acpi_status status;
-
-	status = acpi_evaluate_integer(device->handle, "_PSV", NULL, &psv);
-	if (ACPI_FAILURE(status))
-		return -EIO;
-
-	*temp = DECI_KELVIN_TO_MILLI_CELSIUS(psv, KELVIN_OFFSET);
-
-	return 0;
-}
-
-static int acpi_int3403_add(struct acpi_device *device)
-{
-	int result = 0;
-	unsigned long long ptyp;
-	acpi_status status;
-	struct int3403_sensor *obj;
-	unsigned long long trip_cnt;
-	int trip_mask = 0;
-
-	if (!device)
-		return -EINVAL;
-
-	status = acpi_evaluate_integer(device->handle, "PTYP", NULL, &ptyp);
-	if (ACPI_FAILURE(status))
-		return -EINVAL;
-
-	if (ptyp != INT3403_TYPE_SENSOR)
-		return -EINVAL;
-
-	obj = devm_kzalloc(&device->dev, sizeof(*obj), GFP_KERNEL);
-	if (!obj)
-		return -ENOMEM;
-
-	device->driver_data = obj;
-
-	status = acpi_evaluate_integer(device->handle, "PATC", NULL,
-						&trip_cnt);
-	if (ACPI_FAILURE(status))
-		trip_cnt = 0;
-
-	if (trip_cnt) {
-		/* We have to cache, thresholds can't be readback */
-		obj->thresholds = devm_kzalloc(&device->dev,
-					sizeof(*obj->thresholds) * trip_cnt,
-					GFP_KERNEL);
-		if (!obj->thresholds)
-			return -ENOMEM;
-		trip_mask = BIT(trip_cnt) - 1;
-	}
-
-	obj->psv_trip_id = -1;
-	if (!sys_get_trip_psv(device, &obj->psv_temp))
-		obj->psv_trip_id = trip_cnt++;
-
-	obj->crit_trip_id = -1;
-	if (!sys_get_trip_crt(device, &obj->crit_temp))
-		obj->crit_trip_id = trip_cnt++;
-
-	obj->tzone = thermal_zone_device_register(acpi_device_bid(device),
-				trip_cnt, trip_mask, device, &tzone_ops,
-				NULL, 0, 0);
-	if (IS_ERR(obj->tzone)) {
-		result = PTR_ERR(obj->tzone);
-		return result;
-	}
-
-	strcpy(acpi_device_name(device), "INT3403");
-	strcpy(acpi_device_class(device), ACPI_INT3403_CLASS);
-
-	return 0;
-}
-
-static int acpi_int3403_remove(struct acpi_device *device)
-{
-	struct int3403_sensor *obj;
-
-	obj = acpi_driver_data(device);
-	thermal_zone_device_unregister(obj->tzone);
-
-	return 0;
-}
-
-ACPI_MODULE_NAME("int3403");
-static const struct acpi_device_id int3403_device_ids[] = {
-	{"INT3403", 0},
-	{"", 0},
-};
-MODULE_DEVICE_TABLE(acpi, int3403_device_ids);
-
-static struct acpi_driver acpi_int3403_driver = {
-	.name = "INT3403",
-	.class = ACPI_INT3403_CLASS,
-	.ids = int3403_device_ids,
-	.ops = {
-		.add = acpi_int3403_add,
-		.remove = acpi_int3403_remove,
-		.notify = acpi_thermal_notify,
-		},
-};
-
-module_acpi_driver(acpi_int3403_driver);
-
-MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
-MODULE_LICENSE("GPL v2");
-MODULE_DESCRIPTION("ACPI INT3403 thermal driver");
diff --git a/drivers/thermal/int340x_thermal/Makefile b/drivers/thermal/int340x_thermal/Makefile
new file mode 100644
index 000000000000..ffe40bffaf1a
--- /dev/null
+++ b/drivers/thermal/int340x_thermal/Makefile
@@ -0,0 +1,4 @@
+obj-$(CONFIG_INT340X_THERMAL)	+= int3400_thermal.o
+obj-$(CONFIG_INT340X_THERMAL)	+= int3402_thermal.o
+obj-$(CONFIG_INT340X_THERMAL)	+= int3403_thermal.o
+obj-$(CONFIG_ACPI_THERMAL_REL)	+= acpi_thermal_rel.o
diff --git a/drivers/thermal/int340x_thermal/acpi_thermal_rel.c b/drivers/thermal/int340x_thermal/acpi_thermal_rel.c
new file mode 100644
index 000000000000..0d8db808f0ae
--- /dev/null
+++ b/drivers/thermal/int340x_thermal/acpi_thermal_rel.c
@@ -0,0 +1,400 @@
+/* acpi_thermal_rel.c driver for exporting ACPI thermal relationship
+ *
+ * Copyright (c) 2014 Intel Corp
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ *
+ */
+
+/*
+ * Two functionalities included:
+ * 1. Export _TRT, _ART, via misc device interface to the userspace.
+ * 2. Provide parsing result to kernel drivers
+ *
+ */
+#include <linux/init.h>
+#include <linux/export.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/acpi.h>
+#include <linux/uaccess.h>
+#include <linux/miscdevice.h>
+#include "acpi_thermal_rel.h"
+
+static acpi_handle acpi_thermal_rel_handle;
+static DEFINE_SPINLOCK(acpi_thermal_rel_chrdev_lock);
+static int acpi_thermal_rel_chrdev_count;	/* #times opened */
+static int acpi_thermal_rel_chrdev_exclu;	/* already open exclusive? */
+
+static int acpi_thermal_rel_open(struct inode *inode, struct file *file)
+{
+	spin_lock(&acpi_thermal_rel_chrdev_lock);
+	if (acpi_thermal_rel_chrdev_exclu ||
+	    (acpi_thermal_rel_chrdev_count && (file->f_flags & O_EXCL))) {
+		spin_unlock(&acpi_thermal_rel_chrdev_lock);
+		return -EBUSY;
+	}
+
+	if (file->f_flags & O_EXCL)
+		acpi_thermal_rel_chrdev_exclu = 1;
+	acpi_thermal_rel_chrdev_count++;
+
+	spin_unlock(&acpi_thermal_rel_chrdev_lock);
+
+	return nonseekable_open(inode, file);
+}
+
+static int acpi_thermal_rel_release(struct inode *inode, struct file *file)
+{
+	spin_lock(&acpi_thermal_rel_chrdev_lock);
+	acpi_thermal_rel_chrdev_count--;
+	acpi_thermal_rel_chrdev_exclu = 0;
+	spin_unlock(&acpi_thermal_rel_chrdev_lock);
+
+	return 0;
+}
+
+/**
+ * acpi_parse_trt - Thermal Relationship Table _TRT for passive cooling
+ *
+ * @handle: ACPI handle of the device contains _TRT
+ * @art_count: the number of valid entries resulted from parsing _TRT
+ * @artp: pointer to pointer of array of art entries in parsing result
+ * @create_dev: whether to create platform devices for target and source
+ *
+ */
+int acpi_parse_trt(acpi_handle handle, int *trt_count, struct trt **trtp,
+		bool create_dev)
+{
+	acpi_status status;
+	int result = 0;
+	int i;
+	int nr_bad_entries = 0;
+	struct trt *trts;
+	struct acpi_device *adev;
+	union acpi_object *p;
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	struct acpi_buffer element = { 0, NULL };
+	struct acpi_buffer trt_format = { sizeof("RRNNNNNN"), "RRNNNNNN" };
+
+	if (!acpi_has_method(handle, "_TRT"))
+		return 0;
+
+	status = acpi_evaluate_object(handle, "_TRT", NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	p = buffer.pointer;
+	if (!p || (p->type != ACPI_TYPE_PACKAGE)) {
+		pr_err("Invalid _TRT data\n");
+		result = -EFAULT;
+		goto end;
+	}
+
+	*trt_count = p->package.count;
+	trts = kzalloc(*trt_count * sizeof(struct trt), GFP_KERNEL);
+	if (!trts) {
+		result = -ENOMEM;
+		goto end;
+	}
+
+	for (i = 0; i < *trt_count; i++) {
+		struct trt *trt = &trts[i - nr_bad_entries];
+
+		element.length = sizeof(struct trt);
+		element.pointer = trt;
+
+		status = acpi_extract_package(&(p->package.elements[i]),
+					      &trt_format, &element);
+		if (ACPI_FAILURE(status)) {
+			nr_bad_entries++;
+			pr_warn("_TRT package %d is invalid, ignored\n", i);
+			continue;
+		}
+		if (!create_dev)
+			continue;
+
+		result = acpi_bus_get_device(trt->source, &adev);
+		if (!result)
+			acpi_create_platform_device(adev);
+		else
+			pr_warn("Failed to get source ACPI device\n");
+
+		result = acpi_bus_get_device(trt->target, &adev);
+		if (!result)
+			acpi_create_platform_device(adev);
+		else
+			pr_warn("Failed to get target ACPI device\n");
+	}
+
+	*trtp = trts;
+	/* don't count bad entries */
+	*trt_count -= nr_bad_entries;
+end:
+	kfree(buffer.pointer);
+	return result;
+}
+EXPORT_SYMBOL(acpi_parse_trt);
+
+/**
+ * acpi_parse_art - Parse Active Relationship Table _ART
+ *
+ * @handle: ACPI handle of the device contains _ART
+ * @art_count: the number of valid entries resulted from parsing _ART
+ * @artp: pointer to pointer of array of art entries in parsing result
+ * @create_dev: whether to create platform devices for target and source
+ *
+ */
+int acpi_parse_art(acpi_handle handle, int *art_count, struct art **artp,
+		bool create_dev)
+{
+	acpi_status status;
+	int result = 0;
+	int i;
+	int nr_bad_entries = 0;
+	struct art *arts;
+	struct acpi_device *adev;
+	union acpi_object *p;
+	struct acpi_buffer buffer = { ACPI_ALLOCATE_BUFFER, NULL };
+	struct acpi_buffer element = { 0, NULL };
+	struct acpi_buffer art_format =	{
+		sizeof("RRNNNNNNNNNNN"), "RRNNNNNNNNNNN" };
+
+	if (!acpi_has_method(handle, "_ART"))
+		return 0;
+
+	status = acpi_evaluate_object(handle, "_ART", NULL, &buffer);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	p = buffer.pointer;
+	if (!p || (p->type != ACPI_TYPE_PACKAGE)) {
+		pr_err("Invalid _ART data\n");
+		result = -EFAULT;
+		goto end;
+	}
+
+	/* ignore p->package.elements[0], as this is _ART Revision field */
+	*art_count = p->package.count - 1;
+	arts = kzalloc(*art_count * sizeof(struct art), GFP_KERNEL);
+	if (!arts) {
+		result = -ENOMEM;
+		goto end;
+	}
+
+	for (i = 0; i < *art_count; i++) {
+		struct art *art = &arts[i - nr_bad_entries];
+
+		element.length = sizeof(struct art);
+		element.pointer = art;
+
+		status = acpi_extract_package(&(p->package.elements[i + 1]),
+					      &art_format, &element);
+		if (ACPI_FAILURE(status)) {
+			pr_warn("_ART package %d is invalid, ignored", i);
+			nr_bad_entries++;
+			continue;
+		}
+		if (!create_dev)
+			continue;
+
+		if (art->source) {
+			result = acpi_bus_get_device(art->source, &adev);
+			if (!result)
+				acpi_create_platform_device(adev);
+			else
+				pr_warn("Failed to get source ACPI device\n");
+		}
+		if (art->target) {
+			result = acpi_bus_get_device(art->target, &adev);
+			if (!result)
+				acpi_create_platform_device(adev);
+			else
+				pr_warn("Failed to get source ACPI device\n");
+		}
+	}
+
+	*artp = arts;
+	/* don't count bad entries */
+	*art_count -= nr_bad_entries;
+end:
+	kfree(buffer.pointer);
+	return result;
+}
+EXPORT_SYMBOL(acpi_parse_art);
+
+
+/* get device name from acpi handle */
+static void get_single_name(acpi_handle handle, char *name)
+{
+	struct acpi_buffer buffer = {ACPI_ALLOCATE_BUFFER};
+
+	if (ACPI_FAILURE(acpi_get_name(handle, ACPI_SINGLE_NAME, &buffer)))
+		pr_warn("Failed get name from handle\n");
+	else {
+		memcpy(name, buffer.pointer, ACPI_NAME_SIZE);
+		kfree(buffer.pointer);
+	}
+}
+
+static int fill_art(char __user *ubuf)
+{
+	int i;
+	int ret;
+	int count;
+	int art_len;
+	struct art *arts = NULL;
+	union art_object *art_user;
+
+	ret = acpi_parse_art(acpi_thermal_rel_handle, &count, &arts, false);
+	if (ret)
+		goto free_art;
+	art_len = count * sizeof(union art_object);
+	art_user = kzalloc(art_len, GFP_KERNEL);
+	if (!art_user) {
+		ret = -ENOMEM;
+		goto free_art;
+	}
+	/* now fill in user art data */
+	for (i = 0; i < count; i++) {
+		/* userspace art needs device name instead of acpi reference */
+		get_single_name(arts[i].source, art_user[i].source_device);
+		get_single_name(arts[i].target, art_user[i].target_device);
+		/* copy the rest int data in addition to source and target */
+		memcpy(&art_user[i].weight, &arts[i].weight,
+			sizeof(u64) * (ACPI_NR_ART_ELEMENTS - 2));
+	}
+
+	if (copy_to_user(ubuf, art_user, art_len))
+		ret = -EFAULT;
+	kfree(art_user);
+free_art:
+	kfree(arts);
+	return ret;
+}
+
+static int fill_trt(char __user *ubuf)
+{
+	int i;
+	int ret;
+	int count;
+	int trt_len;
+	struct trt *trts = NULL;
+	union trt_object *trt_user;
+
+	ret = acpi_parse_trt(acpi_thermal_rel_handle, &count, &trts, false);
+	if (ret)
+		goto free_trt;
+	trt_len = count * sizeof(union trt_object);
+	trt_user = kzalloc(trt_len, GFP_KERNEL);
+	if (!trt_user) {
+		ret = -ENOMEM;
+		goto free_trt;
+	}
+	/* now fill in user trt data */
+	for (i = 0; i < count; i++) {
+		/* userspace trt needs device name instead of acpi reference */
+		get_single_name(trts[i].source, trt_user[i].source_device);
+		get_single_name(trts[i].target, trt_user[i].target_device);
+		trt_user[i].sample_period = trts[i].sample_period;
+		trt_user[i].influence = trts[i].influence;
+	}
+
+	if (copy_to_user(ubuf, trt_user, trt_len))
+		ret = -EFAULT;
+	kfree(trt_user);
+free_trt:
+	kfree(trts);
+	return ret;
+}
+
+static long acpi_thermal_rel_ioctl(struct file *f, unsigned int cmd,
+				   unsigned long __arg)
+{
+	int ret = 0;
+	unsigned long length = 0;
+	unsigned long count = 0;
+	char __user *arg = (void __user *)__arg;
+	struct trt *trts;
+	struct art *arts;
+
+	switch (cmd) {
+	case ACPI_THERMAL_GET_TRT_COUNT:
+		ret = acpi_parse_trt(acpi_thermal_rel_handle, (int *)&count,
+				&trts, false);
+		kfree(trts);
+		if (!ret)
+			return put_user(count, (unsigned long __user *)__arg);
+		return ret;
+	case ACPI_THERMAL_GET_TRT_LEN:
+		ret = acpi_parse_trt(acpi_thermal_rel_handle, (int *)&count,
+				&trts, false);
+		kfree(trts);
+		length = count * sizeof(union trt_object);
+		if (!ret)
+			return put_user(length, (unsigned long __user *)__arg);
+		return ret;
+	case ACPI_THERMAL_GET_TRT:
+		return fill_trt(arg);
+	case ACPI_THERMAL_GET_ART_COUNT:
+		ret = acpi_parse_art(acpi_thermal_rel_handle, (int *)&count,
+				&arts, false);
+		kfree(arts);
+		if (!ret)
+			return put_user(count, (unsigned long __user *)__arg);
+		return ret;
+	case ACPI_THERMAL_GET_ART_LEN:
+		ret = acpi_parse_art(acpi_thermal_rel_handle, (int *)&count,
+				&arts, false);
+		kfree(arts);
+		length = count * sizeof(union art_object);
+		if (!ret)
+			return put_user(length, (unsigned long __user *)__arg);
+		return ret;
+
+	case ACPI_THERMAL_GET_ART:
+		return fill_art(arg);
+
+	default:
+		return -ENOTTY;
+	}
+}
+
+static const struct file_operations acpi_thermal_rel_fops = {
+	.owner		= THIS_MODULE,
+	.open		= acpi_thermal_rel_open,
+	.release	= acpi_thermal_rel_release,
+	.unlocked_ioctl	= acpi_thermal_rel_ioctl,
+	.llseek		= no_llseek,
+};
+
+static struct miscdevice acpi_thermal_rel_misc_device = {
+	.minor	= MISC_DYNAMIC_MINOR,
+	"acpi_thermal_rel",
+	&acpi_thermal_rel_fops
+};
+
+int acpi_thermal_rel_misc_device_add(acpi_handle handle)
+{
+	acpi_thermal_rel_handle = handle;
+
+	return misc_register(&acpi_thermal_rel_misc_device);
+}
+EXPORT_SYMBOL(acpi_thermal_rel_misc_device_add);
+
+int acpi_thermal_rel_misc_device_remove(acpi_handle handle)
+{
+	misc_deregister(&acpi_thermal_rel_misc_device);
+
+	return 0;
+}
+EXPORT_SYMBOL(acpi_thermal_rel_misc_device_remove);
+
+MODULE_AUTHOR("Zhang Rui <rui.zhang@intel.com>");
+MODULE_AUTHOR("Jacob Pan <jacob.jun.pan@intel.com");
+MODULE_DESCRIPTION("Intel acpi thermal rel misc dev driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/thermal/int340x_thermal/acpi_thermal_rel.h b/drivers/thermal/int340x_thermal/acpi_thermal_rel.h
new file mode 100644
index 000000000000..f00700bc9d79
--- /dev/null
+++ b/drivers/thermal/int340x_thermal/acpi_thermal_rel.h
@@ -0,0 +1,84 @@
+#ifndef __ACPI_ACPI_THERMAL_H
+#define __ACPI_ACPI_THERMAL_H
+
+#include <asm/ioctl.h>
+
+#define ACPI_THERMAL_MAGIC 's'
+
+#define ACPI_THERMAL_GET_TRT_LEN _IOR(ACPI_THERMAL_MAGIC, 1, unsigned long)
+#define ACPI_THERMAL_GET_ART_LEN _IOR(ACPI_THERMAL_MAGIC, 2, unsigned long)
+#define ACPI_THERMAL_GET_TRT_COUNT _IOR(ACPI_THERMAL_MAGIC, 3, unsigned long)
+#define ACPI_THERMAL_GET_ART_COUNT _IOR(ACPI_THERMAL_MAGIC, 4, unsigned long)
+
+#define ACPI_THERMAL_GET_TRT	_IOR(ACPI_THERMAL_MAGIC, 5, unsigned long)
+#define ACPI_THERMAL_GET_ART	_IOR(ACPI_THERMAL_MAGIC, 6, unsigned long)
+
+struct art {
+	acpi_handle source;
+	acpi_handle target;
+	u64 weight;
+	u64 ac0_max;
+	u64 ac1_max;
+	u64 ac2_max;
+	u64 ac3_max;
+	u64 ac4_max;
+	u64 ac5_max;
+	u64 ac6_max;
+	u64 ac7_max;
+	u64 ac8_max;
+	u64 ac9_max;
+} __packed;
+
+struct trt {
+	acpi_handle source;
+	acpi_handle target;
+	u64 influence;
+	u64 sample_period;
+	u64 reverved1;
+	u64 reverved2;
+	u64 reverved3;
+	u64 reverved4;
+} __packed;
+
+#define ACPI_NR_ART_ELEMENTS 13
+/* for usrspace */
+union art_object {
+	struct {
+		char source_device[8]; /* ACPI single name */
+		char target_device[8]; /* ACPI single name */
+		u64 weight;
+		u64 ac0_max_level;
+		u64 ac1_max_level;
+		u64 ac2_max_level;
+		u64 ac3_max_level;
+		u64 ac4_max_level;
+		u64 ac5_max_level;
+		u64 ac6_max_level;
+		u64 ac7_max_level;
+		u64 ac8_max_level;
+		u64 ac9_max_level;
+	};
+	u64 __data[ACPI_NR_ART_ELEMENTS];
+};
+
+union trt_object {
+	struct {
+		char source_device[8]; /* ACPI single name */
+		char target_device[8]; /* ACPI single name */
+		u64 influence;
+		u64 sample_period;
+		u64 reserved[4];
+	};
+	u64 __data[8];
+};
+
+#ifdef __KERNEL__
+int acpi_thermal_rel_misc_device_add(acpi_handle handle);
+int acpi_thermal_rel_misc_device_remove(acpi_handle handle);
+int acpi_parse_art(acpi_handle handle, int *art_count, struct art **arts,
+		bool create_dev);
+int acpi_parse_trt(acpi_handle handle, int *trt_count, struct trt **trts,
+		bool create_dev);
+#endif
+
+#endif /* __ACPI_ACPI_THERMAL_H */
diff --git a/drivers/thermal/int340x_thermal/int3400_thermal.c b/drivers/thermal/int340x_thermal/int3400_thermal.c
new file mode 100644
index 000000000000..edc1cce117ba
--- /dev/null
+++ b/drivers/thermal/int340x_thermal/int3400_thermal.c
@@ -0,0 +1,271 @@
+/*
+ * INT3400 thermal driver
+ *
+ * Copyright (C) 2014, Intel Corporation
+ * Authors: Zhang Rui <rui.zhang@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/acpi.h>
+#include <linux/thermal.h>
+#include "acpi_thermal_rel.h"
+
+enum int3400_thermal_uuid {
+	INT3400_THERMAL_PASSIVE_1,
+	INT3400_THERMAL_PASSIVE_2,
+	INT3400_THERMAL_ACTIVE,
+	INT3400_THERMAL_CRITICAL,
+	INT3400_THERMAL_COOLING_MODE,
+	INT3400_THERMAL_MAXIMUM_UUID,
+};
+
+static u8 *int3400_thermal_uuids[INT3400_THERMAL_MAXIMUM_UUID] = {
+	"42A441D6-AE6A-462b-A84B-4A8CE79027D3",
+	"9E04115A-AE87-4D1C-9500-0F3E340BFE75",
+	"3A95C389-E4B8-4629-A526-C52C88626BAE",
+	"97C68AE7-15FA-499c-B8C9-5DA81D606E0A",
+	"16CAF1B7-DD38-40ed-B1C1-1B8A1913D531",
+};
+
+struct int3400_thermal_priv {
+	struct acpi_device *adev;
+	struct thermal_zone_device *thermal;
+	int mode;
+	int art_count;
+	struct art *arts;
+	int trt_count;
+	struct trt *trts;
+	u8 uuid_bitmap;
+	int rel_misc_dev_res;
+};
+
+static int int3400_thermal_get_uuids(struct int3400_thermal_priv *priv)
+{
+	struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER, NULL};
+	union acpi_object *obja, *objb;
+	int i, j;
+	int result = 0;
+	acpi_status status;
+
+	status = acpi_evaluate_object(priv->adev->handle, "IDSP", NULL, &buf);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	obja = (union acpi_object *)buf.pointer;
+	if (obja->type != ACPI_TYPE_PACKAGE) {
+		result = -EINVAL;
+		goto end;
+	}
+
+	for (i = 0; i < obja->package.count; i++) {
+		objb = &obja->package.elements[i];
+		if (objb->type != ACPI_TYPE_BUFFER) {
+			result = -EINVAL;
+			goto end;
+		}
+
+		/* UUID must be 16 bytes */
+		if (objb->buffer.length != 16) {
+			result = -EINVAL;
+			goto end;
+		}
+
+		for (j = 0; j < INT3400_THERMAL_MAXIMUM_UUID; j++) {
+			u8 uuid[16];
+
+			acpi_str_to_uuid(int3400_thermal_uuids[j], uuid);
+			if (!strncmp(uuid, objb->buffer.pointer, 16)) {
+				priv->uuid_bitmap |= (1 << j);
+				break;
+			}
+		}
+	}
+
+end:
+	kfree(buf.pointer);
+	return result;
+}
+
+static int int3400_thermal_run_osc(acpi_handle handle,
+				enum int3400_thermal_uuid uuid, bool enable)
+{
+	u32 ret, buf[2];
+	acpi_status status;
+	int result = 0;
+	struct acpi_osc_context context = {
+		.uuid_str = int3400_thermal_uuids[uuid],
+		.rev = 1,
+		.cap.length = 8,
+	};
+
+	buf[OSC_QUERY_DWORD] = 0;
+	buf[OSC_SUPPORT_DWORD] = enable;
+
+	context.cap.pointer = buf;
+
+	status = acpi_run_osc(handle, &context);
+	if (ACPI_SUCCESS(status)) {
+		ret = *((u32 *)(context.ret.pointer + 4));
+		if (ret != enable)
+			result = -EPERM;
+	} else
+		result = -EPERM;
+
+	kfree(context.ret.pointer);
+	return result;
+}
+
+static int int3400_thermal_get_temp(struct thermal_zone_device *thermal,
+			unsigned long *temp)
+{
+	*temp = 20 * 1000; /* faked temp sensor with 20C */
+	return 0;
+}
+
+static int int3400_thermal_get_mode(struct thermal_zone_device *thermal,
+				enum thermal_device_mode *mode)
+{
+	struct int3400_thermal_priv *priv = thermal->devdata;
+
+	if (!priv)
+		return -EINVAL;
+
+	*mode = priv->mode;
+
+	return 0;
+}
+
+static int int3400_thermal_set_mode(struct thermal_zone_device *thermal,
+				enum thermal_device_mode mode)
+{
+	struct int3400_thermal_priv *priv = thermal->devdata;
+	bool enable;
+	int result = 0;
+
+	if (!priv)
+		return -EINVAL;
+
+	if (mode == THERMAL_DEVICE_ENABLED)
+		enable = true;
+	else if (mode == THERMAL_DEVICE_DISABLED)
+		enable = false;
+	else
+		return -EINVAL;
+
+	if (enable != priv->mode) {
+		priv->mode = enable;
+		/* currently, only PASSIVE COOLING is supported */
+		result = int3400_thermal_run_osc(priv->adev->handle,
+					INT3400_THERMAL_PASSIVE_1, enable);
+	}
+	return result;
+}
+
+static struct thermal_zone_device_ops int3400_thermal_ops = {
+	.get_temp = int3400_thermal_get_temp,
+};
+
+static struct thermal_zone_params int3400_thermal_params = {
+	.governor_name = "user_space",
+	.no_hwmon = true,
+};
+
+static int int3400_thermal_probe(struct platform_device *pdev)
+{
+	struct acpi_device *adev = ACPI_COMPANION(&pdev->dev);
+	struct int3400_thermal_priv *priv;
+	int result;
+
+	if (!adev)
+		return -ENODEV;
+
+	priv = kzalloc(sizeof(struct int3400_thermal_priv), GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->adev = adev;
+
+	result = int3400_thermal_get_uuids(priv);
+	if (result)
+		goto free_priv;
+
+	result = acpi_parse_art(priv->adev->handle, &priv->art_count,
+				&priv->arts, true);
+	if (result)
+		goto free_priv;
+
+
+	result = acpi_parse_trt(priv->adev->handle, &priv->trt_count,
+				&priv->trts, true);
+	if (result)
+		goto free_art;
+
+	platform_set_drvdata(pdev, priv);
+
+	if (priv->uuid_bitmap & 1 << INT3400_THERMAL_PASSIVE_1) {
+		int3400_thermal_ops.get_mode = int3400_thermal_get_mode;
+		int3400_thermal_ops.set_mode = int3400_thermal_set_mode;
+	}
+	priv->thermal = thermal_zone_device_register("INT3400 Thermal", 0, 0,
+						priv, &int3400_thermal_ops,
+						&int3400_thermal_params, 0, 0);
+	if (IS_ERR(priv->thermal)) {
+		result = PTR_ERR(priv->thermal);
+		goto free_trt;
+	}
+
+	priv->rel_misc_dev_res = acpi_thermal_rel_misc_device_add(
+							priv->adev->handle);
+
+	return 0;
+free_trt:
+	kfree(priv->trts);
+free_art:
+	kfree(priv->arts);
+free_priv:
+	kfree(priv);
+	return result;
+}
+
+static int int3400_thermal_remove(struct platform_device *pdev)
+{
+	struct int3400_thermal_priv *priv = platform_get_drvdata(pdev);
+
+	if (!priv->rel_misc_dev_res)
+		acpi_thermal_rel_misc_device_remove(priv->adev->handle);
+
+	thermal_zone_device_unregister(priv->thermal);
+	kfree(priv->trts);
+	kfree(priv->arts);
+	kfree(priv);
+	return 0;
+}
+
+static const struct acpi_device_id int3400_thermal_match[] = {
+	{"INT3400", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, int3400_thermal_match);
+
+static struct platform_driver int3400_thermal_driver = {
+	.probe = int3400_thermal_probe,
+	.remove = int3400_thermal_remove,
+	.driver = {
+		   .name = "int3400 thermal",
+		   .owner = THIS_MODULE,
+		   .acpi_match_table = ACPI_PTR(int3400_thermal_match),
+		   },
+};
+
+module_platform_driver(int3400_thermal_driver);
+
+MODULE_DESCRIPTION("INT3400 Thermal driver");
+MODULE_AUTHOR("Zhang Rui <rui.zhang@intel.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/thermal/int340x_thermal/int3402_thermal.c b/drivers/thermal/int340x_thermal/int3402_thermal.c
new file mode 100644
index 000000000000..a5d08c14ba24
--- /dev/null
+++ b/drivers/thermal/int340x_thermal/int3402_thermal.c
@@ -0,0 +1,242 @@
+/*
+ * INT3402 thermal driver for memory temperature reporting
+ *
+ * Copyright (C) 2014, Intel Corporation
+ * Authors: Aaron Lu <aaron.lu@intel.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/acpi.h>
+#include <linux/thermal.h>
+
+#define ACPI_ACTIVE_COOLING_MAX_NR 10
+
+struct active_trip {
+	unsigned long temp;
+	int id;
+	bool valid;
+};
+
+struct int3402_thermal_data {
+	unsigned long *aux_trips;
+	int aux_trip_nr;
+	unsigned long psv_temp;
+	int psv_trip_id;
+	unsigned long crt_temp;
+	int crt_trip_id;
+	unsigned long hot_temp;
+	int hot_trip_id;
+	struct active_trip act_trips[ACPI_ACTIVE_COOLING_MAX_NR];
+	acpi_handle *handle;
+};
+
+static int int3402_thermal_get_zone_temp(struct thermal_zone_device *zone,
+					 unsigned long *temp)
+{
+	struct int3402_thermal_data *d = zone->devdata;
+	unsigned long long tmp;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(d->handle, "_TMP", NULL, &tmp);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	/* _TMP returns the temperature in tenths of degrees Kelvin */
+	*temp = DECI_KELVIN_TO_MILLICELSIUS(tmp);
+
+	return 0;
+}
+
+static int int3402_thermal_get_trip_temp(struct thermal_zone_device *zone,
+					 int trip, unsigned long *temp)
+{
+	struct int3402_thermal_data *d = zone->devdata;
+	int i;
+
+	if (trip < d->aux_trip_nr)
+		*temp = d->aux_trips[trip];
+	else if (trip == d->crt_trip_id)
+		*temp = d->crt_temp;
+	else if (trip == d->psv_trip_id)
+		*temp = d->psv_temp;
+	else if (trip == d->hot_trip_id)
+		*temp = d->hot_temp;
+	else {
+		for (i = 0; i < ACPI_ACTIVE_COOLING_MAX_NR; i++) {
+			if (d->act_trips[i].valid &&
+			    d->act_trips[i].id == trip) {
+				*temp = d->act_trips[i].temp;
+				break;
+			}
+		}
+		if (i == ACPI_ACTIVE_COOLING_MAX_NR)
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static int int3402_thermal_get_trip_type(struct thermal_zone_device *zone,
+					 int trip, enum thermal_trip_type *type)
+{
+	struct int3402_thermal_data *d = zone->devdata;
+	int i;
+
+	if (trip < d->aux_trip_nr)
+		*type = THERMAL_TRIP_PASSIVE;
+	else if (trip == d->crt_trip_id)
+		*type = THERMAL_TRIP_CRITICAL;
+	else if (trip == d->hot_trip_id)
+		*type = THERMAL_TRIP_HOT;
+	else if (trip == d->psv_trip_id)
+		*type = THERMAL_TRIP_PASSIVE;
+	else {
+		for (i = 0; i < ACPI_ACTIVE_COOLING_MAX_NR; i++) {
+			if (d->act_trips[i].valid &&
+			    d->act_trips[i].id == trip) {
+				*type = THERMAL_TRIP_ACTIVE;
+				break;
+			}
+		}
+		if (i == ACPI_ACTIVE_COOLING_MAX_NR)
+			return -EINVAL;
+	}
+	return 0;
+}
+
+static int int3402_thermal_set_trip_temp(struct thermal_zone_device *zone, int trip,
+				  unsigned long temp)
+{
+	struct int3402_thermal_data *d = zone->devdata;
+	acpi_status status;
+	char name[10];
+
+	snprintf(name, sizeof(name), "PAT%d", trip);
+	status = acpi_execute_simple_method(d->handle, name,
+			MILLICELSIUS_TO_DECI_KELVIN(temp));
+	if (ACPI_FAILURE(status))
+		return -EIO;
+
+	d->aux_trips[trip] = temp;
+	return 0;
+}
+
+static struct thermal_zone_device_ops int3402_thermal_zone_ops = {
+	.get_temp       = int3402_thermal_get_zone_temp,
+	.get_trip_temp	= int3402_thermal_get_trip_temp,
+	.get_trip_type	= int3402_thermal_get_trip_type,
+	.set_trip_temp	= int3402_thermal_set_trip_temp,
+};
+
+static struct thermal_zone_params int3402_thermal_params = {
+	.governor_name = "user_space",
+	.no_hwmon = true,
+};
+
+static int int3402_thermal_get_temp(acpi_handle handle, char *name,
+				    unsigned long *temp)
+{
+	unsigned long long r;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(handle, name, NULL, &r);
+	if (ACPI_FAILURE(status))
+		return -EIO;
+
+	*temp = DECI_KELVIN_TO_MILLICELSIUS(r);
+	return 0;
+}
+
+static int int3402_thermal_probe(struct platform_device *pdev)
+{
+	struct acpi_device *adev = ACPI_COMPANION(&pdev->dev);
+	struct int3402_thermal_data *d;
+	struct thermal_zone_device *zone;
+	acpi_status status;
+	unsigned long long trip_cnt;
+	int trip_mask = 0, i;
+
+	if (!acpi_has_method(adev->handle, "_TMP"))
+		return -ENODEV;
+
+	d = devm_kzalloc(&pdev->dev, sizeof(*d), GFP_KERNEL);
+	if (!d)
+		return -ENOMEM;
+
+	status = acpi_evaluate_integer(adev->handle, "PATC", NULL, &trip_cnt);
+	if (ACPI_FAILURE(status))
+		trip_cnt = 0;
+	else {
+		d->aux_trips = devm_kzalloc(&pdev->dev,
+				sizeof(*d->aux_trips) * trip_cnt, GFP_KERNEL);
+		if (!d->aux_trips)
+			return -ENOMEM;
+		trip_mask = trip_cnt - 1;
+		d->handle = adev->handle;
+		d->aux_trip_nr = trip_cnt;
+	}
+
+	d->crt_trip_id = -1;
+	if (!int3402_thermal_get_temp(adev->handle, "_CRT", &d->crt_temp))
+		d->crt_trip_id = trip_cnt++;
+	d->hot_trip_id = -1;
+	if (!int3402_thermal_get_temp(adev->handle, "_HOT", &d->hot_temp))
+		d->hot_trip_id = trip_cnt++;
+	d->psv_trip_id = -1;
+	if (!int3402_thermal_get_temp(adev->handle, "_PSV", &d->psv_temp))
+		d->psv_trip_id = trip_cnt++;
+	for (i = 0; i < ACPI_ACTIVE_COOLING_MAX_NR; i++) {
+		char name[5] = { '_', 'A', 'C', '0' + i, '\0' };
+		if (int3402_thermal_get_temp(adev->handle, name,
+					     &d->act_trips[i].temp))
+			break;
+		d->act_trips[i].id = trip_cnt++;
+		d->act_trips[i].valid = true;
+	}
+
+	zone = thermal_zone_device_register(acpi_device_bid(adev), trip_cnt,
+					    trip_mask, d,
+					    &int3402_thermal_zone_ops,
+					    &int3402_thermal_params,
+					    0, 0);
+	if (IS_ERR(zone))
+		return PTR_ERR(zone);
+	platform_set_drvdata(pdev, zone);
+
+	return 0;
+}
+
+static int int3402_thermal_remove(struct platform_device *pdev)
+{
+	struct thermal_zone_device *zone = platform_get_drvdata(pdev);
+
+	thermal_zone_device_unregister(zone);
+	return 0;
+}
+
+static const struct acpi_device_id int3402_thermal_match[] = {
+	{"INT3402", 0},
+	{}
+};
+
+MODULE_DEVICE_TABLE(acpi, int3402_thermal_match);
+
+static struct platform_driver int3402_thermal_driver = {
+	.probe = int3402_thermal_probe,
+	.remove = int3402_thermal_remove,
+	.driver = {
+		   .name = "int3402 thermal",
+		   .owner = THIS_MODULE,
+		   .acpi_match_table = int3402_thermal_match,
+		   },
+};
+
+module_platform_driver(int3402_thermal_driver);
+
+MODULE_DESCRIPTION("INT3402 Thermal driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/thermal/int340x_thermal/int3403_thermal.c b/drivers/thermal/int340x_thermal/int3403_thermal.c
new file mode 100644
index 000000000000..d20dba986f0f
--- /dev/null
+++ b/drivers/thermal/int340x_thermal/int3403_thermal.c
@@ -0,0 +1,477 @@
+/*
+ * ACPI INT3403 thermal driver
+ * Copyright (c) 2013, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/acpi.h>
+#include <linux/thermal.h>
+#include <linux/platform_device.h>
+
+#define INT3403_TYPE_SENSOR		0x03
+#define INT3403_TYPE_CHARGER		0x0B
+#define INT3403_TYPE_BATTERY		0x0C
+#define INT3403_PERF_CHANGED_EVENT	0x80
+#define INT3403_THERMAL_EVENT		0x90
+
+#define DECI_KELVIN_TO_MILLI_CELSIUS(t, off) (((t) - (off)) * 100)
+#define KELVIN_OFFSET	2732
+#define MILLI_CELSIUS_TO_DECI_KELVIN(t, off) (((t) / 100) + (off))
+
+struct int3403_sensor {
+	struct thermal_zone_device *tzone;
+	unsigned long *thresholds;
+	unsigned long	crit_temp;
+	int		crit_trip_id;
+	unsigned long	psv_temp;
+	int		psv_trip_id;
+
+};
+
+struct int3403_performance_state {
+	u64 performance;
+	u64 power;
+	u64 latency;
+	u64 linear;
+	u64 control;
+	u64 raw_performace;
+	char *raw_unit;
+	int reserved;
+};
+
+struct int3403_cdev {
+	struct thermal_cooling_device *cdev;
+	unsigned long max_state;
+};
+
+struct int3403_priv {
+	struct platform_device *pdev;
+	struct acpi_device *adev;
+	unsigned long long type;
+	void *priv;
+};
+
+static int sys_get_curr_temp(struct thermal_zone_device *tzone,
+				unsigned long *temp)
+{
+	struct int3403_priv *priv = tzone->devdata;
+	struct acpi_device *device = priv->adev;
+	unsigned long long tmp;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(device->handle, "_TMP", NULL, &tmp);
+	if (ACPI_FAILURE(status))
+		return -EIO;
+
+	*temp = DECI_KELVIN_TO_MILLI_CELSIUS(tmp, KELVIN_OFFSET);
+
+	return 0;
+}
+
+static int sys_get_trip_hyst(struct thermal_zone_device *tzone,
+		int trip, unsigned long *temp)
+{
+	struct int3403_priv *priv = tzone->devdata;
+	struct acpi_device *device = priv->adev;
+	unsigned long long hyst;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(device->handle, "GTSH", NULL, &hyst);
+	if (ACPI_FAILURE(status))
+		return -EIO;
+
+	*temp = DECI_KELVIN_TO_MILLI_CELSIUS(hyst, KELVIN_OFFSET);
+
+	return 0;
+}
+
+static int sys_get_trip_temp(struct thermal_zone_device *tzone,
+		int trip, unsigned long *temp)
+{
+	struct int3403_priv *priv = tzone->devdata;
+	struct int3403_sensor *obj = priv->priv;
+
+	if (priv->type != INT3403_TYPE_SENSOR || !obj)
+		return -EINVAL;
+
+	if (trip == obj->crit_trip_id)
+		*temp = obj->crit_temp;
+	else if (trip == obj->psv_trip_id)
+		*temp = obj->psv_temp;
+	else {
+		/*
+		 * get_trip_temp is a mandatory callback but
+		 * PATx method doesn't return any value, so return
+		 * cached value, which was last set from user space
+		 */
+		*temp = obj->thresholds[trip];
+	}
+
+	return 0;
+}
+
+static int sys_get_trip_type(struct thermal_zone_device *thermal,
+		int trip, enum thermal_trip_type *type)
+{
+	struct int3403_priv *priv = thermal->devdata;
+	struct int3403_sensor *obj = priv->priv;
+
+	/* Mandatory callback, may not mean much here */
+	if (trip == obj->crit_trip_id)
+		*type = THERMAL_TRIP_CRITICAL;
+	else
+		*type = THERMAL_TRIP_PASSIVE;
+
+	return 0;
+}
+
+int sys_set_trip_temp(struct thermal_zone_device *tzone, int trip,
+							unsigned long temp)
+{
+	struct int3403_priv *priv = tzone->devdata;
+	struct acpi_device *device = priv->adev;
+	struct int3403_sensor *obj = priv->priv;
+	acpi_status status;
+	char name[10];
+	int ret = 0;
+
+	snprintf(name, sizeof(name), "PAT%d", trip);
+	if (acpi_has_method(device->handle, name)) {
+		status = acpi_execute_simple_method(device->handle, name,
+				MILLI_CELSIUS_TO_DECI_KELVIN(temp,
+							KELVIN_OFFSET));
+		if (ACPI_FAILURE(status))
+			ret = -EIO;
+		else
+			obj->thresholds[trip] = temp;
+	} else {
+		ret = -EIO;
+		dev_err(&device->dev, "sys_set_trip_temp: method not found\n");
+	}
+
+	return ret;
+}
+
+static struct thermal_zone_device_ops tzone_ops = {
+	.get_temp = sys_get_curr_temp,
+	.get_trip_temp = sys_get_trip_temp,
+	.get_trip_type = sys_get_trip_type,
+	.set_trip_temp = sys_set_trip_temp,
+	.get_trip_hyst =  sys_get_trip_hyst,
+};
+
+static struct thermal_zone_params int3403_thermal_params = {
+	.governor_name = "user_space",
+	.no_hwmon = true,
+};
+
+static void int3403_notify(acpi_handle handle,
+		u32 event, void *data)
+{
+	struct int3403_priv *priv = data;
+	struct int3403_sensor *obj;
+
+	if (!priv)
+		return;
+
+	obj = priv->priv;
+	if (priv->type != INT3403_TYPE_SENSOR || !obj)
+		return;
+
+	switch (event) {
+	case INT3403_PERF_CHANGED_EVENT:
+		break;
+	case INT3403_THERMAL_EVENT:
+		thermal_zone_device_update(obj->tzone);
+		break;
+	default:
+		dev_err(&priv->pdev->dev, "Unsupported event [0x%x]\n", event);
+		break;
+	}
+}
+
+static int sys_get_trip_crt(struct acpi_device *device, unsigned long *temp)
+{
+	unsigned long long crt;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(device->handle, "_CRT", NULL, &crt);
+	if (ACPI_FAILURE(status))
+		return -EIO;
+
+	*temp = DECI_KELVIN_TO_MILLI_CELSIUS(crt, KELVIN_OFFSET);
+
+	return 0;
+}
+
+static int sys_get_trip_psv(struct acpi_device *device, unsigned long *temp)
+{
+	unsigned long long psv;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(device->handle, "_PSV", NULL, &psv);
+	if (ACPI_FAILURE(status))
+		return -EIO;
+
+	*temp = DECI_KELVIN_TO_MILLI_CELSIUS(psv, KELVIN_OFFSET);
+
+	return 0;
+}
+
+static int int3403_sensor_add(struct int3403_priv *priv)
+{
+	int result = 0;
+	acpi_status status;
+	struct int3403_sensor *obj;
+	unsigned long long trip_cnt;
+	int trip_mask = 0;
+
+	obj = devm_kzalloc(&priv->pdev->dev, sizeof(*obj), GFP_KERNEL);
+	if (!obj)
+		return -ENOMEM;
+
+	priv->priv = obj;
+
+	status = acpi_evaluate_integer(priv->adev->handle, "PATC", NULL,
+						&trip_cnt);
+	if (ACPI_FAILURE(status))
+		trip_cnt = 0;
+
+	if (trip_cnt) {
+		/* We have to cache, thresholds can't be readback */
+		obj->thresholds = devm_kzalloc(&priv->pdev->dev,
+					sizeof(*obj->thresholds) * trip_cnt,
+					GFP_KERNEL);
+		if (!obj->thresholds) {
+			result = -ENOMEM;
+			goto err_free_obj;
+		}
+		trip_mask = BIT(trip_cnt) - 1;
+	}
+
+	obj->psv_trip_id = -1;
+	if (!sys_get_trip_psv(priv->adev, &obj->psv_temp))
+		obj->psv_trip_id = trip_cnt++;
+
+	obj->crit_trip_id = -1;
+	if (!sys_get_trip_crt(priv->adev, &obj->crit_temp))
+		obj->crit_trip_id = trip_cnt++;
+
+	obj->tzone = thermal_zone_device_register(acpi_device_bid(priv->adev),
+				trip_cnt, trip_mask, priv, &tzone_ops,
+				&int3403_thermal_params, 0, 0);
+	if (IS_ERR(obj->tzone)) {
+		result = PTR_ERR(obj->tzone);
+		obj->tzone = NULL;
+		goto err_free_obj;
+	}
+
+	result = acpi_install_notify_handler(priv->adev->handle,
+			ACPI_DEVICE_NOTIFY, int3403_notify,
+			(void *)priv);
+	if (result)
+		goto err_free_obj;
+
+	return 0;
+
+ err_free_obj:
+	if (obj->tzone)
+		thermal_zone_device_unregister(obj->tzone);
+	return result;
+}
+
+static int int3403_sensor_remove(struct int3403_priv *priv)
+{
+	struct int3403_sensor *obj = priv->priv;
+
+	thermal_zone_device_unregister(obj->tzone);
+	return 0;
+}
+
+/* INT3403 Cooling devices */
+static int int3403_get_max_state(struct thermal_cooling_device *cdev,
+				 unsigned long *state)
+{
+	struct int3403_priv *priv = cdev->devdata;
+	struct int3403_cdev *obj = priv->priv;
+
+	*state = obj->max_state;
+	return 0;
+}
+
+static int int3403_get_cur_state(struct thermal_cooling_device *cdev,
+				 unsigned long *state)
+{
+	struct int3403_priv *priv = cdev->devdata;
+	unsigned long long level;
+	acpi_status status;
+
+	status = acpi_evaluate_integer(priv->adev->handle, "PPPC", NULL, &level);
+	if (ACPI_SUCCESS(status)) {
+		*state = level;
+		return 0;
+	} else
+		return -EINVAL;
+}
+
+static int
+int3403_set_cur_state(struct thermal_cooling_device *cdev, unsigned long state)
+{
+	struct int3403_priv *priv = cdev->devdata;
+	acpi_status status;
+
+	status = acpi_execute_simple_method(priv->adev->handle, "SPPC", state);
+	if (ACPI_SUCCESS(status))
+		return 0;
+	else
+		return -EINVAL;
+}
+
+static const struct thermal_cooling_device_ops int3403_cooling_ops = {
+	.get_max_state = int3403_get_max_state,
+	.get_cur_state = int3403_get_cur_state,
+	.set_cur_state = int3403_set_cur_state,
+};
+
+static int int3403_cdev_add(struct int3403_priv *priv)
+{
+	int result = 0;
+	acpi_status status;
+	struct int3403_cdev *obj;
+	struct acpi_buffer buf = { ACPI_ALLOCATE_BUFFER, NULL };
+	union acpi_object *p;
+
+	obj = devm_kzalloc(&priv->pdev->dev, sizeof(*obj), GFP_KERNEL);
+	if (!obj)
+		return -ENOMEM;
+
+	status = acpi_evaluate_object(priv->adev->handle, "PPSS", NULL, &buf);
+	if (ACPI_FAILURE(status))
+		return -ENODEV;
+
+	p = buf.pointer;
+	if (!p || (p->type != ACPI_TYPE_PACKAGE)) {
+		printk(KERN_WARNING "Invalid PPSS data\n");
+		return -EFAULT;
+	}
+
+	obj->max_state = p->package.count - 1;
+	obj->cdev =
+		thermal_cooling_device_register(acpi_device_bid(priv->adev),
+				priv, &int3403_cooling_ops);
+	if (IS_ERR(obj->cdev))
+		result = PTR_ERR(obj->cdev);
+
+	priv->priv = obj;
+
+	/* TODO: add ACPI notification support */
+
+	return result;
+}
+
+static int int3403_cdev_remove(struct int3403_priv *priv)
+{
+	struct int3403_cdev *obj = priv->priv;
+
+	thermal_cooling_device_unregister(obj->cdev);
+	return 0;
+}
+
+static int int3403_add(struct platform_device *pdev)
+{
+	struct int3403_priv *priv;
+	int result = 0;
+	acpi_status status;
+
+	priv = devm_kzalloc(&pdev->dev, sizeof(struct int3403_priv),
+			    GFP_KERNEL);
+	if (!priv)
+		return -ENOMEM;
+
+	priv->pdev = pdev;
+	priv->adev = ACPI_COMPANION(&(pdev->dev));
+	if (!priv->adev) {
+		result = -EINVAL;
+		goto err;
+	}
+
+	status = acpi_evaluate_integer(priv->adev->handle, "PTYP",
+				       NULL, &priv->type);
+	if (ACPI_FAILURE(status)) {
+		result = -EINVAL;
+		goto err;
+	}
+
+	platform_set_drvdata(pdev, priv);
+	switch (priv->type) {
+	case INT3403_TYPE_SENSOR:
+		result = int3403_sensor_add(priv);
+		break;
+	case INT3403_TYPE_CHARGER:
+	case INT3403_TYPE_BATTERY:
+		result = int3403_cdev_add(priv);
+		break;
+	default:
+		result = -EINVAL;
+	}
+
+	if (result)
+		goto err;
+	return result;
+
+err:
+	return result;
+}
+
+static int int3403_remove(struct platform_device *pdev)
+{
+	struct int3403_priv *priv = platform_get_drvdata(pdev);
+
+	switch (priv->type) {
+	case INT3403_TYPE_SENSOR:
+		int3403_sensor_remove(priv);
+		break;
+	case INT3403_TYPE_CHARGER:
+	case INT3403_TYPE_BATTERY:
+		int3403_cdev_remove(priv);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
+static const struct acpi_device_id int3403_device_ids[] = {
+	{"INT3403", 0},
+	{"", 0},
+};
+MODULE_DEVICE_TABLE(acpi, int3403_device_ids);
+
+static struct platform_driver int3403_driver = {
+	.probe = int3403_add,
+	.remove = int3403_remove,
+	.driver = {
+		.name = "int3403 thermal",
+		.owner  = THIS_MODULE,
+		.acpi_match_table = int3403_device_ids,
+	},
+};
+
+module_platform_driver(int3403_driver);
+
+MODULE_AUTHOR("Srinivas Pandruvada <srinivas.pandruvada@linux.intel.com>");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("ACPI INT3403 thermal driver");
diff --git a/drivers/thermal/of-thermal.c b/drivers/thermal/of-thermal.c
index 4b2b999b7611..f8eb625b8400 100644
--- a/drivers/thermal/of-thermal.c
+++ b/drivers/thermal/of-thermal.c
@@ -401,6 +401,10 @@ thermal_zone_of_sensor_register(struct device *dev, int sensor_id,
 		struct of_phandle_args sensor_specs;
 		int ret, id;
 
+		/* Check whether child is enabled or not */
+		if (!of_device_is_available(child))
+			continue;
+
 		/* For now, thermal framework supports only 1 sensor per zone */
 		ret = of_parse_phandle_with_args(child, "thermal-sensors",
 						 "#thermal-sensor-cells",
@@ -771,6 +775,10 @@ int __init of_parse_thermal_zones(void)
 		struct thermal_zone_device *zone;
 		struct thermal_zone_params *tzp;
 
+		/* Check whether child is enabled or not */
+		if (!of_device_is_available(child))
+			continue;
+
 		tz = thermal_of_build_thermal_zone(child);
 		if (IS_ERR(tz)) {
 			pr_err("failed to build thermal zone %s: %ld\n",
@@ -838,6 +846,10 @@ void of_thermal_destroy_zones(void)
 	for_each_child_of_node(np, child) {
 		struct thermal_zone_device *zone;
 
+		/* Check whether child is enabled or not */
+		if (!of_device_is_available(child))
+			continue;
+
 		zone = thermal_zone_get_zone_by_name(child->name);
 		if (IS_ERR(zone))
 			continue;
diff --git a/drivers/thermal/step_wise.c b/drivers/thermal/step_wise.c
index f251521baaa2..fdd1f523a1ed 100644
--- a/drivers/thermal/step_wise.c
+++ b/drivers/thermal/step_wise.c
@@ -23,6 +23,7 @@
  */
 
 #include <linux/thermal.h>
+#include <trace/events/thermal.h>
 
 #include "thermal_core.h"
 
@@ -76,7 +77,7 @@ static unsigned long get_target_state(struct thermal_instance *instance,
 			next_target = instance->upper;
 		break;
 	case THERMAL_TREND_DROPPING:
-		if (cur_state == instance->lower) {
+		if (cur_state <= instance->lower) {
 			if (!throttle)
 				next_target = THERMAL_NO_TARGET;
 		} else {
@@ -129,8 +130,10 @@ static void thermal_zone_trip_update(struct thermal_zone_device *tz, int trip)
 
 	trend = get_tz_trend(tz, trip);
 
-	if (tz->temperature >= trip_temp)
+	if (tz->temperature >= trip_temp) {
 		throttle = true;
+		trace_thermal_zone_trip(tz, trip, trip_type);
+	}
 
 	dev_dbg(&tz->device, "Trip%d[type=%d,temp=%ld]:trend=%d,throttle=%d\n",
 				trip, trip_type, trip_temp, trend, throttle);
diff --git a/drivers/thermal/thermal_core.c b/drivers/thermal/thermal_core.c
index 71b0ec0c370d..9bf10aa6069b 100644
--- a/drivers/thermal/thermal_core.c
+++ b/drivers/thermal/thermal_core.c
@@ -38,6 +38,9 @@
 #include <net/netlink.h>
 #include <net/genetlink.h>
 
+#define CREATE_TRACE_POINTS
+#include <trace/events/thermal.h>
+
 #include "thermal_core.h"
 #include "thermal_hwmon.h"
 
@@ -66,7 +69,7 @@ static struct thermal_governor *__find_governor(const char *name)
 		return def_governor;
 
 	list_for_each_entry(pos, &thermal_governor_list, governor_list)
-		if (!strnicmp(name, pos->name, THERMAL_NAME_LENGTH))
+		if (!strncasecmp(name, pos->name, THERMAL_NAME_LENGTH))
 			return pos;
 
 	return NULL;
@@ -104,7 +107,7 @@ int thermal_register_governor(struct thermal_governor *governor)
 
 		name = pos->tzp->governor_name;
 
-		if (!strnicmp(name, governor->name, THERMAL_NAME_LENGTH))
+		if (!strncasecmp(name, governor->name, THERMAL_NAME_LENGTH))
 			pos->governor = governor;
 	}
 
@@ -129,7 +132,7 @@ void thermal_unregister_governor(struct thermal_governor *governor)
 	mutex_lock(&thermal_list_lock);
 
 	list_for_each_entry(pos, &thermal_tz_list, node) {
-		if (!strnicmp(pos->governor->name, governor->name,
+		if (!strncasecmp(pos->governor->name, governor->name,
 						THERMAL_NAME_LENGTH))
 			pos->governor = NULL;
 	}
@@ -368,6 +371,8 @@ static void handle_critical_trips(struct thermal_zone_device *tz,
 	if (tz->temperature < trip_temp)
 		return;
 
+	trace_thermal_zone_trip(tz, trip, trip_type);
+
 	if (tz->ops->notify)
 		tz->ops->notify(tz, trip, trip_type);
 
@@ -463,6 +468,7 @@ static void update_temperature(struct thermal_zone_device *tz)
 	tz->temperature = temp;
 	mutex_unlock(&tz->lock);
 
+	trace_thermal_temperature(tz);
 	dev_dbg(&tz->device, "last_temperature=%d, current_temperature=%d\n",
 				tz->last_temperature, tz->temperature);
 }
@@ -1287,6 +1293,7 @@ void thermal_cdev_update(struct thermal_cooling_device *cdev)
 	mutex_unlock(&cdev->lock);
 	cdev->ops->set_cur_state(cdev, target);
 	cdev->updated = true;
+	trace_cdev_update(cdev, target);
 	dev_dbg(&cdev->device, "set to state %lu\n", target);
 }
 EXPORT_SYMBOL(thermal_cdev_update);
@@ -1665,7 +1672,7 @@ struct thermal_zone_device *thermal_zone_get_zone_by_name(const char *name)
 
 	mutex_lock(&thermal_list_lock);
 	list_for_each_entry(pos, &thermal_tz_list, node)
-		if (!strnicmp(name, pos->type, THERMAL_NAME_LENGTH)) {
+		if (!strncasecmp(name, pos->type, THERMAL_NAME_LENGTH)) {
 			found++;
 			ref = pos;
 		}
@@ -1790,6 +1797,10 @@ static int __init thermal_register_governors(void)
 	if (result)
 		return result;
 
+	result = thermal_gov_bang_bang_register();
+	if (result)
+		return result;
+
 	return thermal_gov_user_space_register();
 }
 
@@ -1797,6 +1808,7 @@ static void thermal_unregister_governors(void)
 {
 	thermal_gov_step_wise_unregister();
 	thermal_gov_fair_share_unregister();
+	thermal_gov_bang_bang_unregister();
 	thermal_gov_user_space_unregister();
 }
 
diff --git a/drivers/thermal/thermal_core.h b/drivers/thermal/thermal_core.h
index 3db339fb636f..d15d243de27a 100644
--- a/drivers/thermal/thermal_core.h
+++ b/drivers/thermal/thermal_core.h
@@ -69,6 +69,14 @@ static inline int thermal_gov_fair_share_register(void) { return 0; }
 static inline void thermal_gov_fair_share_unregister(void) {}
 #endif /* CONFIG_THERMAL_GOV_FAIR_SHARE */
 
+#ifdef CONFIG_THERMAL_GOV_BANG_BANG
+int thermal_gov_bang_bang_register(void);
+void thermal_gov_bang_bang_unregister(void);
+#else
+static inline int thermal_gov_bang_bang_register(void) { return 0; }
+static inline void thermal_gov_bang_bang_unregister(void) {}
+#endif /* CONFIG_THERMAL_GOV_BANG_BANG */
+
 #ifdef CONFIG_THERMAL_GOV_USER_SPACE
 int thermal_gov_user_space_register(void);
 void thermal_gov_user_space_unregister(void);
diff --git a/drivers/thunderbolt/path.c b/drivers/thunderbolt/path.c
index 8fcf8a7b6c22..9562cd026dc0 100644
--- a/drivers/thunderbolt/path.c
+++ b/drivers/thunderbolt/path.c
@@ -150,7 +150,26 @@ int tb_path_activate(struct tb_path *path)
 
 	/* Activate hops. */
 	for (i = path->path_length - 1; i >= 0; i--) {
-		struct tb_regs_hop hop;
+		struct tb_regs_hop hop = { 0 };
+
+		/*
+		 * We do (currently) not tear down paths setup by the firmeware.
+		 * If a firmware device is unplugged and plugged in again then
+		 * it can happen that we reuse some of the hops from the (now
+		 * defunct) firmeware path. This causes the hotplug operation to
+		 * fail (the pci device does not show up). Clearing the hop
+		 * before overwriting it fixes the problem.
+		 *
+		 * Should be removed once we discover and tear down firmeware
+		 * paths.
+		 */
+		res = tb_port_write(path->hops[i].in_port, &hop, TB_CFG_HOPS,
+				    2 * path->hops[i].in_hop_index, 2);
+		if (res) {
+			__tb_path_deactivate_hops(path, i);
+			__tb_path_deallocate_nfc(path, 0);
+			goto err;
+		}
 
 		/* dword 0 */
 		hop.next_hop = path->hops[i].next_hop_index;
diff --git a/drivers/tty/bfin_jtag_comm.c b/drivers/tty/bfin_jtag_comm.c
index 8096fcbe2dc1..d7b198c400c7 100644
--- a/drivers/tty/bfin_jtag_comm.c
+++ b/drivers/tty/bfin_jtag_comm.c
@@ -77,7 +77,6 @@ bfin_jc_emudat_manager(void *arg)
 			pr_debug("waiting for readers\n");
 			__set_current_state(TASK_UNINTERRUPTIBLE);
 			schedule();
-			__set_current_state(TASK_RUNNING);
 			continue;
 		}
 
diff --git a/drivers/tty/hvc/hvc_vio.c b/drivers/tty/hvc/hvc_vio.c
index 5618b5fc7500..f575a9b5ede7 100644
--- a/drivers/tty/hvc/hvc_vio.c
+++ b/drivers/tty/hvc/hvc_vio.c
@@ -452,7 +452,7 @@ void __init hvc_vio_init_early(void)
 		return;
 #endif
 	/* Check whether the user has requested a different console. */
-	if (!strstr(cmd_line, "console="))
+	if (!strstr(boot_command_line, "console="))
 		add_preferred_console("hvc", 0, NULL);
 	hvc_instantiate(0, 0, ops);
 }
diff --git a/drivers/tty/hvc/hvc_xen.c b/drivers/tty/hvc/hvc_xen.c
index 2dc2831840ca..f1e57425e39f 100644
--- a/drivers/tty/hvc/hvc_xen.c
+++ b/drivers/tty/hvc/hvc_xen.c
@@ -347,8 +347,6 @@ static int xen_console_remove(struct xencons_info *info)
 }
 
 #ifdef CONFIG_HVC_XEN_FRONTEND
-static struct xenbus_driver xencons_driver;
-
 static int xencons_remove(struct xenbus_device *dev)
 {
 	return xen_console_remove(dev_get_drvdata(&dev->dev));
@@ -402,9 +400,6 @@ static int xencons_connect_backend(struct xenbus_device *dev,
 			    evtchn);
 	if (ret)
 		goto error_xenbus;
-	ret = xenbus_printf(xbt, dev->nodename, "type", "ioemu");
-	if (ret)
-		goto error_xenbus;
 	ret = xenbus_transaction_end(xbt, 0);
 	if (ret) {
 		if (ret == -EAGAIN)
@@ -502,13 +497,14 @@ static const struct xenbus_device_id xencons_ids[] = {
 	{ "" }
 };
 
-
-static DEFINE_XENBUS_DRIVER(xencons, "xenconsole",
+static struct xenbus_driver xencons_driver = {
+	.name = "xenconsole",
+	.ids = xencons_ids,
 	.probe = xencons_probe,
 	.remove = xencons_remove,
 	.resume = xencons_resume,
 	.otherend_changed = xencons_backend_changed,
-);
+};
 #endif /* CONFIG_HVC_XEN_FRONTEND */
 
 static int __init xen_hvc_init(void)
diff --git a/drivers/tty/metag_da.c b/drivers/tty/metag_da.c
index 7332e2ca4615..3774600741d8 100644
--- a/drivers/tty/metag_da.c
+++ b/drivers/tty/metag_da.c
@@ -17,6 +17,7 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/kthread.h>
+#include <linux/moduleparam.h>
 #include <linux/mutex.h>
 #include <linux/sched.h>
 #include <linux/serial.h>
@@ -70,6 +71,15 @@ static struct tty_driver *channel_driver;
 static struct timer_list put_timer;
 static struct task_struct *dashtty_thread;
 
+/*
+ * The console_poll parameter determines whether the console channel should be
+ * polled for input.
+ * By default the console channel isn't polled at all, in order to avoid the
+ * overhead, but that means it isn't possible to have a login on /dev/console.
+ */
+static bool console_poll;
+module_param(console_poll, bool, S_IRUGO);
+
 #define RX_BUF_SIZE 1024
 
 enum {
@@ -353,7 +363,7 @@ static int dashtty_port_activate(struct tty_port *port, struct tty_struct *tty)
 	 * possible to have a login on /dev/console.
 	 *
 	 */
-	if (dport != &dashtty_ports[CONSOLE_CHANNEL])
+	if (console_poll || dport != &dashtty_ports[CONSOLE_CHANNEL])
 		if (atomic_inc_return(&num_channels_need_poll) == 1)
 			add_poll_timer(&poll_timer);
 
@@ -372,7 +382,7 @@ static void dashtty_port_shutdown(struct tty_port *port)
 	unsigned int count;
 
 	/* stop reading */
-	if (dport != &dashtty_ports[CONSOLE_CHANNEL])
+	if (console_poll || dport != &dashtty_ports[CONSOLE_CHANNEL])
 		if (atomic_dec_and_test(&num_channels_need_poll))
 			del_timer_sync(&poll_timer);
 
diff --git a/drivers/tty/moxa.c b/drivers/tty/moxa.c
index 1deaca4674e4..14c54e041065 100644
--- a/drivers/tty/moxa.c
+++ b/drivers/tty/moxa.c
@@ -1096,7 +1096,7 @@ static int __init moxa_init(void)
 				continue;
 			}
 
-			printk(KERN_INFO "MOXA isa board found at 0x%.8lu and "
+			printk(KERN_INFO "MOXA isa board found at 0x%.8lx and "
 					"ready (%u ports, firmware loaded)\n",
 					baseaddr[i], brd->numPorts);
 
diff --git a/drivers/tty/n_tty.c b/drivers/tty/n_tty.c
index f44f1ba762c3..89c4cee253e3 100644
--- a/drivers/tty/n_tty.c
+++ b/drivers/tty/n_tty.c
@@ -1166,7 +1166,7 @@ static void n_tty_receive_break(struct tty_struct *tty)
 	}
 	put_tty_queue('\0', ldata);
 	if (waitqueue_active(&tty->read_wait))
-		wake_up_interruptible(&tty->read_wait);
+		wake_up_interruptible_poll(&tty->read_wait, POLLIN);
 }
 
 /**
@@ -1226,7 +1226,7 @@ static void n_tty_receive_parity_error(struct tty_struct *tty, unsigned char c)
 	} else
 		put_tty_queue(c, ldata);
 	if (waitqueue_active(&tty->read_wait))
-		wake_up_interruptible(&tty->read_wait);
+		wake_up_interruptible_poll(&tty->read_wait, POLLIN);
 }
 
 static void
@@ -1378,7 +1378,7 @@ handle_newline:
 			ldata->canon_head = ldata->read_head;
 			kill_fasync(&tty->fasync, SIGIO, POLL_IN);
 			if (waitqueue_active(&tty->read_wait))
-				wake_up_interruptible(&tty->read_wait);
+				wake_up_interruptible_poll(&tty->read_wait, POLLIN);
 			return 0;
 		}
 	}
@@ -1679,7 +1679,7 @@ static void __receive_buf(struct tty_struct *tty, const unsigned char *cp,
 		L_EXTPROC(tty)) {
 		kill_fasync(&tty->fasync, SIGIO, POLL_IN);
 		if (waitqueue_active(&tty->read_wait))
-			wake_up_interruptible(&tty->read_wait);
+			wake_up_interruptible_poll(&tty->read_wait, POLLIN);
 	}
 }
 
diff --git a/drivers/tty/nozomi.c b/drivers/tty/nozomi.c
index cd0429369557..74885af8c7bd 100644
--- a/drivers/tty/nozomi.c
+++ b/drivers/tty/nozomi.c
@@ -523,7 +523,7 @@ static u32 write_mem32(void __iomem *mem_addr_start, const u32 *buf,
 }
 
 /* Setup pointers to different channels and also setup buffer sizes. */
-static void setup_memory(struct nozomi *dc)
+static void nozomi_setup_memory(struct nozomi *dc)
 {
 	void __iomem *offset = dc->base_addr + dc->config_table.dl_start;
 	/* The length reported is including the length field of 4 bytes,
@@ -671,7 +671,7 @@ static int nozomi_read_config_table(struct nozomi *dc)
 		int i;
 		DBG1("Second phase, configuring card");
 
-		setup_memory(dc);
+		nozomi_setup_memory(dc);
 
 		dc->port[PORT_MDM].toggle_ul = dc->config_table.toggle.mdm_ul;
 		dc->port[PORT_MDM].toggle_dl = dc->config_table.toggle.mdm_dl;
@@ -705,7 +705,7 @@ static int nozomi_read_config_table(struct nozomi *dc)
 			 dc->config_table.version);
 
 		/* Here we should disable all I/O over F32. */
-		setup_memory(dc);
+		nozomi_setup_memory(dc);
 
 		/*
 		 * We should send ALL channel pair tokens back along
diff --git a/drivers/tty/pty.c b/drivers/tty/pty.c
index 9bbdb1de12e2..7c4447a5c0f4 100644
--- a/drivers/tty/pty.c
+++ b/drivers/tty/pty.c
@@ -24,6 +24,7 @@
 #include <linux/devpts_fs.h>
 #include <linux/slab.h>
 #include <linux/mutex.h>
+#include <linux/poll.h>
 
 
 #ifdef CONFIG_UNIX98_PTYS
@@ -313,6 +314,42 @@ done:
 }
 
 /**
+ *	pty_start - start() handler
+ *	pty_stop  - stop() handler
+ *	@tty: tty being flow-controlled
+ *
+ *	Propagates the TIOCPKT status to the master pty.
+ *
+ *	NB: only the master pty can be in packet mode so only the slave
+ *	    needs start()/stop() handlers
+ */
+static void pty_start(struct tty_struct *tty)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&tty->ctrl_lock, flags);
+	if (tty->link && tty->link->packet) {
+		tty->ctrl_status &= ~TIOCPKT_STOP;
+		tty->ctrl_status |= TIOCPKT_START;
+		wake_up_interruptible_poll(&tty->link->read_wait, POLLIN);
+	}
+	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+}
+
+static void pty_stop(struct tty_struct *tty)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&tty->ctrl_lock, flags);
+	if (tty->link && tty->link->packet) {
+		tty->ctrl_status &= ~TIOCPKT_START;
+		tty->ctrl_status |= TIOCPKT_STOP;
+		wake_up_interruptible_poll(&tty->link->read_wait, POLLIN);
+	}
+	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+}
+
+/**
  *	pty_common_install		-	set up the pty pair
  *	@driver: the pty driver
  *	@tty: the tty being instantiated
@@ -471,6 +508,8 @@ static const struct tty_operations slave_pty_ops_bsd = {
 	.set_termios = pty_set_termios,
 	.cleanup = pty_cleanup,
 	.resize = pty_resize,
+	.start = pty_start,
+	.stop = pty_stop,
 	.remove = pty_remove
 };
 
@@ -646,6 +685,8 @@ static const struct tty_operations pty_unix98_ops = {
 	.chars_in_buffer = pty_chars_in_buffer,
 	.unthrottle = pty_unthrottle,
 	.set_termios = pty_set_termios,
+	.start = pty_start,
+	.stop = pty_stop,
 	.shutdown = pty_unix98_shutdown,
 	.cleanup = pty_cleanup,
 };
diff --git a/drivers/tty/serial/8250/8250.h b/drivers/tty/serial/8250/8250.h
index 1b08c918cd51..cb51be55989e 100644
--- a/drivers/tty/serial/8250/8250.h
+++ b/drivers/tty/serial/8250/8250.h
@@ -16,13 +16,13 @@
 #include <linux/dmaengine.h>
 
 struct uart_8250_dma {
+	/* Filter function */
 	dma_filter_fn		fn;
+
+	/* Parameter to the filter function */
 	void			*rx_param;
 	void			*tx_param;
 
-	int			rx_chan_id;
-	int			tx_chan_id;
-
 	struct dma_slave_config	rxconf;
 	struct dma_slave_config	txconf;
 
@@ -72,6 +72,7 @@ struct serial8250_config {
 #define UART_CAP_UUE	(1 << 12)	/* UART needs IER bit 6 set (Xscale) */
 #define UART_CAP_RTOIE	(1 << 13)	/* UART needs IER bit 4 set (Xscale, Tegra) */
 #define UART_CAP_HFIFO	(1 << 14)	/* UART has a "hidden" FIFO */
+#define UART_CAP_RPM	(1 << 15)	/* Runtime PM is active while idle */
 
 #define UART_BUG_QUOT	(1 << 0)	/* UART has buggy quot LSB */
 #define UART_BUG_TXEN	(1 << 1)	/* UART has buggy TX IIR status */
@@ -112,6 +113,8 @@ static inline void serial_dl_write(struct uart_8250_port *up, int value)
 	up->dl_write(up, value);
 }
 
+struct uart_8250_port *serial8250_get_port(int line);
+
 #if defined(__alpha__) && !defined(CONFIG_PCI)
 /*
  * Digital did something really horribly wrong with the OUT1 and OUT2
diff --git a/drivers/tty/serial/8250/8250_core.c b/drivers/tty/serial/8250/8250_core.c
index 1d42dba6121d..ca5cfdc1459a 100644
--- a/drivers/tty/serial/8250/8250_core.c
+++ b/drivers/tty/serial/8250/8250_core.c
@@ -37,6 +37,8 @@
 #include <linux/nmi.h>
 #include <linux/mutex.h>
 #include <linux/slab.h>
+#include <linux/uaccess.h>
+#include <linux/pm_runtime.h>
 #ifdef CONFIG_SPARC
 #include <linux/sunserialcore.h>
 #endif
@@ -539,6 +541,53 @@ void serial8250_clear_and_reinit_fifos(struct uart_8250_port *p)
 }
 EXPORT_SYMBOL_GPL(serial8250_clear_and_reinit_fifos);
 
+static void serial8250_rpm_get(struct uart_8250_port *p)
+{
+	if (!(p->capabilities & UART_CAP_RPM))
+		return;
+	pm_runtime_get_sync(p->port.dev);
+}
+
+static void serial8250_rpm_put(struct uart_8250_port *p)
+{
+	if (!(p->capabilities & UART_CAP_RPM))
+		return;
+	pm_runtime_mark_last_busy(p->port.dev);
+	pm_runtime_put_autosuspend(p->port.dev);
+}
+
+/*
+ * This two wrapper ensure, that enable_runtime_pm_tx() can be called more than
+ * once and disable_runtime_pm_tx() will still disable RPM because the fifo is
+ * empty and the HW can idle again.
+ */
+static void serial8250_rpm_get_tx(struct uart_8250_port *p)
+{
+	unsigned char rpm_active;
+
+	if (!(p->capabilities & UART_CAP_RPM))
+		return;
+
+	rpm_active = xchg(&p->rpm_tx_active, 1);
+	if (rpm_active)
+		return;
+	pm_runtime_get_sync(p->port.dev);
+}
+
+static void serial8250_rpm_put_tx(struct uart_8250_port *p)
+{
+	unsigned char rpm_active;
+
+	if (!(p->capabilities & UART_CAP_RPM))
+		return;
+
+	rpm_active = xchg(&p->rpm_tx_active, 0);
+	if (!rpm_active)
+		return;
+	pm_runtime_mark_last_busy(p->port.dev);
+	pm_runtime_put_autosuspend(p->port.dev);
+}
+
 /*
  * IER sleep support.  UARTs which have EFRs need the "extended
  * capability" bit enabled.  Note that on XR16C850s, we need to
@@ -553,10 +602,11 @@ static void serial8250_set_sleep(struct uart_8250_port *p, int sleep)
 	 * offset but the UART channel may only write to the corresponding
 	 * bit.
 	 */
+	serial8250_rpm_get(p);
 	if ((p->port.type == PORT_XR17V35X) ||
 	   (p->port.type == PORT_XR17D15X)) {
 		serial_out(p, UART_EXAR_SLEEP, sleep ? 0xff : 0);
-		return;
+		goto out;
 	}
 
 	if (p->capabilities & UART_CAP_SLEEP) {
@@ -572,6 +622,8 @@ static void serial8250_set_sleep(struct uart_8250_port *p, int sleep)
 			serial_out(p, UART_LCR, 0);
 		}
 	}
+out:
+	serial8250_rpm_put(p);
 }
 
 #ifdef CONFIG_SERIAL_8250_RSA
@@ -1272,6 +1324,7 @@ static inline void __stop_tx(struct uart_8250_port *p)
 	if (p->ier & UART_IER_THRI) {
 		p->ier &= ~UART_IER_THRI;
 		serial_out(p, UART_IER, p->ier);
+		serial8250_rpm_put_tx(p);
 	}
 }
 
@@ -1279,6 +1332,7 @@ static void serial8250_stop_tx(struct uart_port *port)
 {
 	struct uart_8250_port *up = up_to_u8250p(port);
 
+	serial8250_rpm_get(up);
 	__stop_tx(up);
 
 	/*
@@ -1288,12 +1342,14 @@ static void serial8250_stop_tx(struct uart_port *port)
 		up->acr |= UART_ACR_TXDIS;
 		serial_icr_write(up, UART_ACR, up->acr);
 	}
+	serial8250_rpm_put(up);
 }
 
 static void serial8250_start_tx(struct uart_port *port)
 {
 	struct uart_8250_port *up = up_to_u8250p(port);
 
+	serial8250_rpm_get_tx(up);
 	if (up->dma && !serial8250_tx_dma(up)) {
 		return;
 	} else if (!(up->ier & UART_IER_THRI)) {
@@ -1318,13 +1374,27 @@ static void serial8250_start_tx(struct uart_port *port)
 	}
 }
 
+static void serial8250_throttle(struct uart_port *port)
+{
+	port->throttle(port);
+}
+
+static void serial8250_unthrottle(struct uart_port *port)
+{
+	port->unthrottle(port);
+}
+
 static void serial8250_stop_rx(struct uart_port *port)
 {
 	struct uart_8250_port *up = up_to_u8250p(port);
 
-	up->ier &= ~UART_IER_RLSI;
+	serial8250_rpm_get(up);
+
+	up->ier &= ~(UART_IER_RLSI | UART_IER_RDI);
 	up->port.read_status_mask &= ~UART_LSR_DR;
 	serial_port_out(port, UART_IER, up->ier);
+
+	serial8250_rpm_put(up);
 }
 
 static void serial8250_enable_ms(struct uart_port *port)
@@ -1336,7 +1406,10 @@ static void serial8250_enable_ms(struct uart_port *port)
 		return;
 
 	up->ier |= UART_IER_MSI;
+
+	serial8250_rpm_get(up);
 	serial_port_out(port, UART_IER, up->ier);
+	serial8250_rpm_put(up);
 }
 
 /*
@@ -1458,11 +1531,17 @@ void serial8250_tx_chars(struct uart_8250_port *up)
 
 	DEBUG_INTR("THRE...");
 
-	if (uart_circ_empty(xmit))
+	/*
+	 * With RPM enabled, we have to wait once the FIFO is empty before the
+	 * HW can go idle. So we get here once again with empty FIFO and disable
+	 * the interrupt and RPM in __stop_tx()
+	 */
+	if (uart_circ_empty(xmit) && !(up->capabilities & UART_CAP_RPM))
 		__stop_tx(up);
 }
 EXPORT_SYMBOL_GPL(serial8250_tx_chars);
 
+/* Caller holds uart port lock */
 unsigned int serial8250_modem_status(struct uart_8250_port *up)
 {
 	struct uart_port *port = &up->port;
@@ -1525,9 +1604,17 @@ EXPORT_SYMBOL_GPL(serial8250_handle_irq);
 
 static int serial8250_default_handle_irq(struct uart_port *port)
 {
-	unsigned int iir = serial_port_in(port, UART_IIR);
+	struct uart_8250_port *up = up_to_u8250p(port);
+	unsigned int iir;
+	int ret;
+
+	serial8250_rpm_get(up);
 
-	return serial8250_handle_irq(port, iir);
+	iir = serial_port_in(port, UART_IIR);
+	ret = serial8250_handle_irq(port, iir);
+
+	serial8250_rpm_put(up);
+	return ret;
 }
 
 /*
@@ -1784,11 +1871,15 @@ static unsigned int serial8250_tx_empty(struct uart_port *port)
 	unsigned long flags;
 	unsigned int lsr;
 
+	serial8250_rpm_get(up);
+
 	spin_lock_irqsave(&port->lock, flags);
 	lsr = serial_port_in(port, UART_LSR);
 	up->lsr_saved_flags |= lsr & LSR_SAVE_FLAGS;
 	spin_unlock_irqrestore(&port->lock, flags);
 
+	serial8250_rpm_put(up);
+
 	return (lsr & BOTH_EMPTY) == BOTH_EMPTY ? TIOCSER_TEMT : 0;
 }
 
@@ -1798,7 +1889,9 @@ static unsigned int serial8250_get_mctrl(struct uart_port *port)
 	unsigned int status;
 	unsigned int ret;
 
+	serial8250_rpm_get(up);
 	status = serial8250_modem_status(up);
+	serial8250_rpm_put(up);
 
 	ret = 0;
 	if (status & UART_MSR_DCD)
@@ -1838,6 +1931,7 @@ static void serial8250_break_ctl(struct uart_port *port, int break_state)
 	struct uart_8250_port *up = up_to_u8250p(port);
 	unsigned long flags;
 
+	serial8250_rpm_get(up);
 	spin_lock_irqsave(&port->lock, flags);
 	if (break_state == -1)
 		up->lcr |= UART_LCR_SBC;
@@ -1845,6 +1939,7 @@ static void serial8250_break_ctl(struct uart_port *port, int break_state)
 		up->lcr &= ~UART_LCR_SBC;
 	serial_port_out(port, UART_LCR, up->lcr);
 	spin_unlock_irqrestore(&port->lock, flags);
+	serial8250_rpm_put(up);
 }
 
 /*
@@ -1889,12 +1984,23 @@ static void wait_for_xmitr(struct uart_8250_port *up, int bits)
 
 static int serial8250_get_poll_char(struct uart_port *port)
 {
-	unsigned char lsr = serial_port_in(port, UART_LSR);
+	struct uart_8250_port *up = up_to_u8250p(port);
+	unsigned char lsr;
+	int status;
+
+	serial8250_rpm_get(up);
+
+	lsr = serial_port_in(port, UART_LSR);
 
-	if (!(lsr & UART_LSR_DR))
-		return NO_POLL_CHAR;
+	if (!(lsr & UART_LSR_DR)) {
+		status = NO_POLL_CHAR;
+		goto out;
+	}
 
-	return serial_port_in(port, UART_RX);
+	status = serial_port_in(port, UART_RX);
+out:
+	serial8250_rpm_put(up);
+	return status;
 }
 
 
@@ -1904,6 +2010,7 @@ static void serial8250_put_poll_char(struct uart_port *port,
 	unsigned int ier;
 	struct uart_8250_port *up = up_to_u8250p(port);
 
+	serial8250_rpm_get(up);
 	/*
 	 *	First save the IER then disable the interrupts
 	 */
@@ -1925,11 +2032,12 @@ static void serial8250_put_poll_char(struct uart_port *port,
 	 */
 	wait_for_xmitr(up, BOTH_EMPTY);
 	serial_port_out(port, UART_IER, ier);
+	serial8250_rpm_put(up);
 }
 
 #endif /* CONFIG_CONSOLE_POLL */
 
-static int serial8250_startup(struct uart_port *port)
+int serial8250_do_startup(struct uart_port *port)
 {
 	struct uart_8250_port *up = up_to_u8250p(port);
 	unsigned long flags;
@@ -1950,6 +2058,7 @@ static int serial8250_startup(struct uart_port *port)
 	if (port->iotype != up->cur_iotype)
 		set_io_from_upio(port);
 
+	serial8250_rpm_get(up);
 	if (port->type == PORT_16C950) {
 		/* Wake up and initialize UART */
 		up->acr = 0;
@@ -1970,7 +2079,6 @@ static int serial8250_startup(struct uart_port *port)
 	 */
 	enable_rsa(up);
 #endif
-
 	/*
 	 * Clear the FIFO buffers and disable them.
 	 * (they will be reenabled in set_termios())
@@ -1980,8 +2088,8 @@ static int serial8250_startup(struct uart_port *port)
 	/*
 	 * Clear the interrupt registers.
 	 */
-	serial_port_in(port, UART_LSR);
-	serial_port_in(port, UART_RX);
+	if (serial_port_in(port, UART_LSR) & UART_LSR_DR)
+		serial_port_in(port, UART_RX);
 	serial_port_in(port, UART_IIR);
 	serial_port_in(port, UART_MSR);
 
@@ -1994,7 +2102,8 @@ static int serial8250_startup(struct uart_port *port)
 	    (serial_port_in(port, UART_LSR) == 0xff)) {
 		printk_ratelimited(KERN_INFO "ttyS%d: LSR safety check engaged!\n",
 				   serial_index(port));
-		return -ENODEV;
+		retval = -ENODEV;
+		goto out;
 	}
 
 	/*
@@ -2079,7 +2188,7 @@ static int serial8250_startup(struct uart_port *port)
 	} else {
 		retval = serial_link_irq_chain(up);
 		if (retval)
-			return retval;
+			goto out;
 	}
 
 	/*
@@ -2141,8 +2250,8 @@ dont_test_tx_en:
 	 * saved flags to avoid getting false values from polling
 	 * routines or the previous session.
 	 */
-	serial_port_in(port, UART_LSR);
-	serial_port_in(port, UART_RX);
+	if (serial_port_in(port, UART_LSR) & UART_LSR_DR)
+		serial_port_in(port, UART_RX);
 	serial_port_in(port, UART_IIR);
 	serial_port_in(port, UART_MSR);
 	up->lsr_saved_flags = 0;
@@ -2177,15 +2286,26 @@ dont_test_tx_en:
 		outb_p(0x80, icp);
 		inb_p(icp);
 	}
+	retval = 0;
+out:
+	serial8250_rpm_put(up);
+	return retval;
+}
+EXPORT_SYMBOL_GPL(serial8250_do_startup);
 
-	return 0;
+static int serial8250_startup(struct uart_port *port)
+{
+	if (port->startup)
+		return port->startup(port);
+	return serial8250_do_startup(port);
 }
 
-static void serial8250_shutdown(struct uart_port *port)
+void serial8250_do_shutdown(struct uart_port *port)
 {
 	struct uart_8250_port *up = up_to_u8250p(port);
 	unsigned long flags;
 
+	serial8250_rpm_get(up);
 	/*
 	 * Disable interrupts from this port
 	 */
@@ -2224,13 +2344,24 @@ static void serial8250_shutdown(struct uart_port *port)
 	 * Read data port to reset things, and then unlink from
 	 * the IRQ chain.
 	 */
-	serial_port_in(port, UART_RX);
+	if (serial_port_in(port, UART_LSR) & UART_LSR_DR)
+		serial_port_in(port, UART_RX);
+	serial8250_rpm_put(up);
 
 	del_timer_sync(&up->timer);
 	up->timer.function = serial8250_timeout;
 	if (port->irq)
 		serial_unlink_irq_chain(up);
 }
+EXPORT_SYMBOL_GPL(serial8250_do_shutdown);
+
+static void serial8250_shutdown(struct uart_port *port)
+{
+	if (port->shutdown)
+		port->shutdown(port);
+	else
+		serial8250_do_shutdown(port);
+}
 
 static unsigned int serial8250_get_divisor(struct uart_port *port, unsigned int baud)
 {
@@ -2319,11 +2450,9 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 	 * the trigger, or the MCR RTS bit is cleared.  In the case where
 	 * the remote UART is not using CTS auto flow control, we must
 	 * have sufficient FIFO entries for the latency of the remote
-	 * UART to respond.  IOW, at least 32 bytes of FIFO. Also enable
-	 * AFE if hw flow control is supported
+	 * UART to respond.  IOW, at least 32 bytes of FIFO.
 	 */
-	if ((up->capabilities & UART_CAP_AFE && (port->fifosize >= 32)) ||
-	    (port->flags & UPF_HARD_FLOW)) {
+	if (up->capabilities & UART_CAP_AFE && port->fifosize >= 32) {
 		up->mcr &= ~UART_MCR_AFE;
 		if (termios->c_cflag & CRTSCTS)
 			up->mcr |= UART_MCR_AFE;
@@ -2333,6 +2462,7 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 	 * Ok, we're now changing the port state.  Do it with
 	 * interrupts disabled.
 	 */
+	serial8250_rpm_get(up);
 	spin_lock_irqsave(&port->lock, flags);
 
 	/*
@@ -2454,6 +2584,8 @@ serial8250_do_set_termios(struct uart_port *port, struct ktermios *termios,
 	}
 	serial8250_set_mctrl(port, port->mctrl);
 	spin_unlock_irqrestore(&port->lock, flags);
+	serial8250_rpm_put(up);
+
 	/* Don't rewrite B0 */
 	if (tty_termios_baud_rate(termios))
 		tty_termios_encode_baud_rate(termios, baud, baud);
@@ -2843,6 +2975,42 @@ serial8250_verify_port(struct uart_port *port, struct serial_struct *ser)
 	return 0;
 }
 
+static int serial8250_ioctl(struct uart_port *port, unsigned int cmd,
+			   unsigned long arg)
+{
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+	int ret;
+	struct serial_rs485 rs485_config;
+
+	if (!up->rs485_config)
+		return -ENOIOCTLCMD;
+
+	switch (cmd) {
+	case TIOCSRS485:
+		if (copy_from_user(&rs485_config, (void __user *)arg,
+				   sizeof(rs485_config)))
+			return -EFAULT;
+
+		ret = up->rs485_config(up, &rs485_config);
+		if (ret)
+			return ret;
+
+		memcpy(&up->rs485, &rs485_config, sizeof(rs485_config));
+
+		return 0;
+	case TIOCGRS485:
+		if (copy_to_user((void __user *)arg, &up->rs485,
+				 sizeof(up->rs485)))
+			return -EFAULT;
+		return 0;
+	default:
+		break;
+	}
+
+	return -ENOIOCTLCMD;
+}
+
 static const char *
 serial8250_type(struct uart_port *port)
 {
@@ -2859,6 +3027,8 @@ static struct uart_ops serial8250_pops = {
 	.get_mctrl	= serial8250_get_mctrl,
 	.stop_tx	= serial8250_stop_tx,
 	.start_tx	= serial8250_start_tx,
+	.throttle	= serial8250_throttle,
+	.unthrottle	= serial8250_unthrottle,
 	.stop_rx	= serial8250_stop_rx,
 	.enable_ms	= serial8250_enable_ms,
 	.break_ctl	= serial8250_break_ctl,
@@ -2872,6 +3042,7 @@ static struct uart_ops serial8250_pops = {
 	.request_port	= serial8250_request_port,
 	.config_port	= serial8250_config_port,
 	.verify_port	= serial8250_verify_port,
+	.ioctl		= serial8250_ioctl,
 #ifdef CONFIG_CONSOLE_POLL
 	.poll_get_char = serial8250_get_poll_char,
 	.poll_put_char = serial8250_put_poll_char,
@@ -2880,6 +3051,24 @@ static struct uart_ops serial8250_pops = {
 
 static struct uart_8250_port serial8250_ports[UART_NR];
 
+/**
+ * serial8250_get_port - retrieve struct uart_8250_port
+ * @line: serial line number
+ *
+ * This function retrieves struct uart_8250_port for the specific line.
+ * This struct *must* *not* be used to perform a 8250 or serial core operation
+ * which is not accessible otherwise. Its only purpose is to make the struct
+ * accessible to the runtime-pm callbacks for context suspend/restore.
+ * The lock assumption made here is none because runtime-pm suspend/resume
+ * callbacks should not be invoked if there is any operation performed on the
+ * port.
+ */
+struct uart_8250_port *serial8250_get_port(int line)
+{
+	return &serial8250_ports[line];
+}
+EXPORT_SYMBOL_GPL(serial8250_get_port);
+
 static void (*serial8250_isa_config)(int port, struct uart_port *up,
 	unsigned short *capabilities);
 
@@ -3007,6 +3196,8 @@ serial8250_console_write(struct console *co, const char *s, unsigned int count)
 
 	touch_nmi_watchdog();
 
+	serial8250_rpm_get(up);
+
 	if (port->sysrq || oops_in_progress)
 		locked = spin_trylock_irqsave(&port->lock, flags);
 	else
@@ -3043,6 +3234,7 @@ serial8250_console_write(struct console *co, const char *s, unsigned int count)
 
 	if (locked)
 		spin_unlock_irqrestore(&port->lock, flags);
+	serial8250_rpm_put(up);
 }
 
 static int __init serial8250_console_setup(struct console *co, char *options)
@@ -3324,6 +3516,11 @@ static struct uart_8250_port *serial8250_find_match_or_unused(struct uart_port *
 		if (uart_match_port(&serial8250_ports[i].port, port))
 			return &serial8250_ports[i];
 
+	/* try line number first if still available */
+	i = port->line;
+	if (i < nr_uarts && serial8250_ports[i].port.type == PORT_UNKNOWN &&
+			serial8250_ports[i].port.iobase == 0)
+		return &serial8250_ports[i];
 	/*
 	 * We didn't find a matching entry, so look for the first
 	 * free entry.  We look for one which hasn't been previously
@@ -3388,6 +3585,10 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 		uart->port.fifosize	= up->port.fifosize;
 		uart->tx_loadsz		= up->tx_loadsz;
 		uart->capabilities	= up->capabilities;
+		uart->rs485_config	= up->rs485_config;
+		uart->rs485		= up->rs485;
+		uart->port.throttle	= up->port.throttle;
+		uart->port.unthrottle	= up->port.unthrottle;
 
 		/* Take tx_loadsz from fifosize if it wasn't set separately */
 		if (uart->port.fifosize && !uart->tx_loadsz)
@@ -3410,6 +3611,10 @@ int serial8250_register_8250_port(struct uart_8250_port *up)
 		/*  Possibly override set_termios call */
 		if (up->port.set_termios)
 			uart->port.set_termios = up->port.set_termios;
+		if (up->port.startup)
+			uart->port.startup = up->port.startup;
+		if (up->port.shutdown)
+			uart->port.shutdown = up->port.shutdown;
 		if (up->port.pm)
 			uart->port.pm = up->port.pm;
 		if (up->port.handle_break)
@@ -3587,7 +3792,7 @@ static void __used s8250_options(void)
 #ifdef CONFIG_SERIAL_8250_RSA
 	__module_param_call(MODULE_PARAM_PREFIX, probe_rsa,
 		&param_array_ops, .arr = &__param_arr_probe_rsa,
-		0444, -1);
+		0444, -1, 0);
 #endif
 }
 #else
diff --git a/drivers/tty/serial/8250/8250_dw.c b/drivers/tty/serial/8250/8250_dw.c
index 4db7987ec225..beea6ca73ee5 100644
--- a/drivers/tty/serial/8250/8250_dw.c
+++ b/drivers/tty/serial/8250/8250_dw.c
@@ -216,10 +216,7 @@ out:
 
 static bool dw8250_dma_filter(struct dma_chan *chan, void *param)
 {
-	struct dw8250_data *data = param;
-
-	return chan->chan_id == data->dma.tx_chan_id ||
-	       chan->chan_id == data->dma.rx_chan_id;
+	return false;
 }
 
 static void dw8250_setup_port(struct uart_8250_port *up)
@@ -399,8 +396,6 @@ static int dw8250_probe(struct platform_device *pdev)
 	if (!IS_ERR(data->rst))
 		reset_control_deassert(data->rst);
 
-	data->dma.rx_chan_id = -1;
-	data->dma.tx_chan_id = -1;
 	data->dma.rx_param = data;
 	data->dma.tx_param = data;
 	data->dma.fn = dw8250_dma_filter;
@@ -540,6 +535,7 @@ static const struct acpi_device_id dw8250_acpi_match[] = {
 	{ "INT3434", 0 },
 	{ "INT3435", 0 },
 	{ "80860F0A", 0 },
+	{ "8086228A", 0 },
 	{ },
 };
 MODULE_DEVICE_TABLE(acpi, dw8250_acpi_match);
diff --git a/drivers/tty/serial/8250/8250_fintek.c b/drivers/tty/serial/8250/8250_fintek.c
new file mode 100644
index 000000000000..1bb28cb69493
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_fintek.c
@@ -0,0 +1,249 @@
+/*
+ *  Probe for F81216A LPC to 4 UART
+ *
+ *  Based on drivers/tty/serial/8250_pnp.c, by Russell King, et al
+ *
+ *  Copyright (C) 2014 Ricardo Ribalda, Qtechnology A/S
+ *
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License.
+ */
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/pnp.h>
+#include <linux/kernel.h>
+#include <linux/serial_core.h>
+#include  "8250.h"
+
+#define ADDR_PORT 0x4E
+#define DATA_PORT 0x4F
+#define ENTRY_KEY 0x77
+#define EXIT_KEY 0xAA
+#define CHIP_ID1  0x20
+#define CHIP_ID1_VAL 0x02
+#define CHIP_ID2  0x21
+#define CHIP_ID2_VAL 0x16
+#define VENDOR_ID1 0x23
+#define VENDOR_ID1_VAL 0x19
+#define VENDOR_ID2 0x24
+#define VENDOR_ID2_VAL 0x34
+#define LDN 0x7
+
+#define RS485  0xF0
+#define RTS_INVERT BIT(5)
+#define RS485_URA BIT(4)
+#define RXW4C_IRA BIT(3)
+#define TXW4C_IRA BIT(2)
+
+#define DRIVER_NAME "8250_fintek"
+
+static int fintek_8250_enter_key(void){
+
+	if (!request_muxed_region(ADDR_PORT, 2, DRIVER_NAME))
+		return -EBUSY;
+
+	outb(ENTRY_KEY, ADDR_PORT);
+	outb(ENTRY_KEY, ADDR_PORT);
+	return 0;
+}
+
+static void fintek_8250_exit_key(void){
+
+	outb(EXIT_KEY, ADDR_PORT);
+	release_region(ADDR_PORT, 2);
+}
+
+static int fintek_8250_get_index(resource_size_t base_addr)
+{
+	resource_size_t base[] = {0x3f8, 0x2f8, 0x3e8, 0x2e8};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(base); i++)
+		if (base_addr == base[i])
+			return i;
+
+	return -ENODEV;
+}
+
+static int fintek_8250_check_id(void)
+{
+
+	outb(CHIP_ID1, ADDR_PORT);
+	if (inb(DATA_PORT) != CHIP_ID1_VAL)
+		return -ENODEV;
+
+	outb(CHIP_ID2, ADDR_PORT);
+	if (inb(DATA_PORT) != CHIP_ID2_VAL)
+		return -ENODEV;
+
+	outb(VENDOR_ID1, ADDR_PORT);
+	if (inb(DATA_PORT) != VENDOR_ID1_VAL)
+		return -ENODEV;
+
+	outb(VENDOR_ID2, ADDR_PORT);
+	if (inb(DATA_PORT) != VENDOR_ID2_VAL)
+		return -ENODEV;
+
+	return 0;
+}
+
+static int fintek_8250_rs4850_config(struct uart_8250_port *uart,
+			      struct serial_rs485 *rs485)
+{
+	uint8_t config = 0;
+	int index = fintek_8250_get_index(uart->port.iobase);
+
+	if (index < 0)
+		return -EINVAL;
+
+	if (rs485->flags & SER_RS485_ENABLED)
+		memset(rs485->padding, 0, sizeof(rs485->padding));
+	else
+		memset(rs485, 0, sizeof(*rs485));
+
+	rs485->flags &= SER_RS485_ENABLED | SER_RS485_RTS_ON_SEND |
+			SER_RS485_RTS_AFTER_SEND;
+
+	if (rs485->delay_rts_before_send) {
+		rs485->delay_rts_before_send = 1;
+		config |= TXW4C_IRA;
+	}
+
+	if (rs485->delay_rts_after_send) {
+		rs485->delay_rts_after_send = 1;
+		config |= RXW4C_IRA;
+	}
+
+	if ((!!(rs485->flags & SER_RS485_RTS_ON_SEND)) ==
+			(!!(rs485->flags & SER_RS485_RTS_AFTER_SEND)))
+		rs485->flags &= SER_RS485_ENABLED;
+	else
+		config |= RS485_URA;
+
+	if (rs485->flags & SER_RS485_RTS_ON_SEND)
+		config |= RTS_INVERT;
+
+	if (fintek_8250_enter_key())
+		return -EBUSY;
+
+	outb(LDN, ADDR_PORT);
+	outb(index, DATA_PORT);
+	outb(RS485, ADDR_PORT);
+	outb(config, DATA_PORT);
+	fintek_8250_exit_key();
+
+	return 0;
+}
+
+static int
+fintek_8250_probe(struct pnp_dev *dev, const struct pnp_device_id *dev_id)
+{
+	int line;
+	struct uart_8250_port uart;
+	int ret;
+
+	if (!pnp_port_valid(dev, 0))
+		return -ENODEV;
+
+	if (fintek_8250_get_index(pnp_port_start(dev, 0)) < 0)
+		return -ENODEV;
+
+	/* Enable configuration registers*/
+	if (fintek_8250_enter_key())
+		return -EBUSY;
+
+	/*Check ID*/
+	ret = fintek_8250_check_id();
+	fintek_8250_exit_key();
+	if (ret)
+		return ret;
+
+	memset(&uart, 0, sizeof(uart));
+	if (!pnp_irq_valid(dev, 0))
+		return -ENODEV;
+	uart.port.irq = pnp_irq(dev, 0);
+	uart.port.iobase = pnp_port_start(dev, 0);
+	uart.port.iotype = UPIO_PORT;
+	uart.rs485_config = fintek_8250_rs4850_config;
+
+	uart.port.flags |= UPF_SKIP_TEST | UPF_BOOT_AUTOCONF;
+	if (pnp_irq_flags(dev, 0) & IORESOURCE_IRQ_SHAREABLE)
+		uart.port.flags |= UPF_SHARE_IRQ;
+	uart.port.uartclk = 1843200;
+	uart.port.dev = &dev->dev;
+
+	line = serial8250_register_8250_port(&uart);
+	if (line < 0)
+		return -ENODEV;
+
+	pnp_set_drvdata(dev, (void *)((long)line + 1));
+	return 0;
+}
+
+static void fintek_8250_remove(struct pnp_dev *dev)
+{
+	long line = (long)pnp_get_drvdata(dev);
+
+	if (line)
+		serial8250_unregister_port(line - 1);
+}
+
+#ifdef CONFIG_PM
+static int fintek_8250_suspend(struct pnp_dev *dev, pm_message_t state)
+{
+	long line = (long)pnp_get_drvdata(dev);
+
+	if (!line)
+		return -ENODEV;
+	serial8250_suspend_port(line - 1);
+	return 0;
+}
+
+static int fintek_8250_resume(struct pnp_dev *dev)
+{
+	long line = (long)pnp_get_drvdata(dev);
+
+	if (!line)
+		return -ENODEV;
+	serial8250_resume_port(line - 1);
+	return 0;
+}
+#else
+#define fintek_8250_suspend NULL
+#define fintek_8250_resume NULL
+#endif /* CONFIG_PM */
+
+static const struct pnp_device_id fintek_dev_table[] = {
+	/* Qtechnology Panel PC / IO1000 */
+	{ "PNP0501"},
+	{}
+};
+
+MODULE_DEVICE_TABLE(pnp, fintek_dev_table);
+
+static struct pnp_driver fintek_8250_driver = {
+	.name		= DRIVER_NAME,
+	.probe		= fintek_8250_probe,
+	.remove		= fintek_8250_remove,
+	.suspend	= fintek_8250_suspend,
+	.resume		= fintek_8250_resume,
+	.id_table	= fintek_dev_table,
+};
+
+static int fintek_8250_init(void)
+{
+	return pnp_register_driver(&fintek_8250_driver);
+}
+module_init(fintek_8250_init);
+
+static void fintek_8250_exit(void)
+{
+	pnp_unregister_driver(&fintek_8250_driver);
+}
+module_exit(fintek_8250_exit);
+
+MODULE_DESCRIPTION("Fintek F812164 module");
+MODULE_AUTHOR("Ricardo Ribalda <ricardo.ribalda@gmail.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/tty/serial/8250/8250_hp300.c b/drivers/tty/serial/8250/8250_hp300.c
index 5bdaf271d395..afffe4d1f034 100644
--- a/drivers/tty/serial/8250/8250_hp300.c
+++ b/drivers/tty/serial/8250/8250_hp300.c
@@ -21,7 +21,7 @@
 #include "8250.h"
 
 #if !defined(CONFIG_HPDCA) && !defined(CONFIG_HPAPCI)
-#warning CONFIG_8250 defined but neither CONFIG_HPDCA nor CONFIG_HPAPCI defined, are you sure?
+#warning CONFIG_SERIAL_8250 defined but neither CONFIG_HPDCA nor CONFIG_HPAPCI defined, are you sure?
 #endif
 
 #ifdef CONFIG_HPAPCI
diff --git a/drivers/tty/serial/8250/8250_mtk.c b/drivers/tty/serial/8250/8250_mtk.c
new file mode 100644
index 000000000000..8f37d57165ec
--- /dev/null
+++ b/drivers/tty/serial/8250/8250_mtk.c
@@ -0,0 +1,294 @@
+/*
+ * Mediatek 8250 driver.
+ *
+ * Copyright (c) 2014 MundoReader S.L.
+ * Author: Matthias Brugger <matthias.bgg@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_irq.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/pm_runtime.h>
+#include <linux/serial_8250.h>
+#include <linux/serial_reg.h>
+
+#include "8250.h"
+
+#define UART_MTK_HIGHS		0x09	/* Highspeed register */
+#define UART_MTK_SAMPLE_COUNT	0x0a	/* Sample count register */
+#define UART_MTK_SAMPLE_POINT	0x0b	/* Sample point register */
+#define MTK_UART_RATE_FIX	0x0d	/* UART Rate Fix Register */
+
+struct mtk8250_data {
+	int			line;
+	struct clk		*uart_clk;
+};
+
+static void
+mtk8250_set_termios(struct uart_port *port, struct ktermios *termios,
+			struct ktermios *old)
+{
+	unsigned long flags;
+	unsigned int baud, quot;
+
+	struct uart_8250_port *up =
+		container_of(port, struct uart_8250_port, port);
+
+	serial8250_do_set_termios(port, termios, old);
+
+	/*
+	 * Mediatek UARTs use an extra highspeed register (UART_MTK_HIGHS)
+	 *
+	 * We need to recalcualte the quot register, as the claculation depends
+	 * on the vaule in the highspeed register.
+	 *
+	 * Some baudrates are not supported by the chip, so we use the next
+	 * lower rate supported and update termios c_flag.
+	 *
+	 * If highspeed register is set to 3, we need to specify sample count
+	 * and sample point to increase accuracy. If not, we reset the
+	 * registers to their default values.
+	 */
+	baud = uart_get_baud_rate(port, termios, old,
+				  port->uartclk / 16 / 0xffff,
+				  port->uartclk / 16);
+
+	if (baud <= 115200) {
+		serial_port_out(port, UART_MTK_HIGHS, 0x0);
+		quot = uart_get_divisor(port, baud);
+	} else if (baud <= 576000) {
+		serial_port_out(port, UART_MTK_HIGHS, 0x2);
+
+		/* Set to next lower baudrate supported */
+		if ((baud == 500000) || (baud == 576000))
+			baud = 460800;
+		quot = DIV_ROUND_CLOSEST(port->uartclk, 4 * baud);
+	} else {
+		serial_port_out(port, UART_MTK_HIGHS, 0x3);
+
+		/* Set to highest baudrate supported */
+		if (baud >= 1152000)
+			baud = 921600;
+		quot = DIV_ROUND_CLOSEST(port->uartclk, 256 * baud);
+	}
+
+	/*
+	 * Ok, we're now changing the port state.  Do it with
+	 * interrupts disabled.
+	 */
+	spin_lock_irqsave(&port->lock, flags);
+
+	/* set DLAB we have cval saved in up->lcr from the call to the core */
+	serial_port_out(port, UART_LCR, up->lcr | UART_LCR_DLAB);
+	serial_dl_write(up, quot);
+
+	/* reset DLAB */
+	serial_port_out(port, UART_LCR, up->lcr);
+
+	if (baud > 460800) {
+		unsigned int tmp;
+
+		tmp = DIV_ROUND_CLOSEST(port->uartclk, quot * baud);
+		serial_port_out(port, UART_MTK_SAMPLE_COUNT, tmp - 1);
+		serial_port_out(port, UART_MTK_SAMPLE_POINT,
+					(tmp - 2) >> 1);
+	} else {
+		serial_port_out(port, UART_MTK_SAMPLE_COUNT, 0x00);
+		serial_port_out(port, UART_MTK_SAMPLE_POINT, 0xff);
+	}
+
+	spin_unlock_irqrestore(&port->lock, flags);
+	/* Don't rewrite B0 */
+	if (tty_termios_baud_rate(termios))
+		tty_termios_encode_baud_rate(termios, baud, baud);
+}
+
+static void
+mtk8250_do_pm(struct uart_port *port, unsigned int state, unsigned int old)
+{
+	if (!state)
+		pm_runtime_get_sync(port->dev);
+
+	serial8250_do_pm(port, state, old);
+
+	if (state)
+		pm_runtime_put_sync_suspend(port->dev);
+}
+
+static int mtk8250_probe_of(struct platform_device *pdev, struct uart_port *p,
+			   struct mtk8250_data *data)
+{
+	int err;
+	struct device_node *np = pdev->dev.of_node;
+
+	data->uart_clk = of_clk_get(np, 0);
+	if (IS_ERR(data->uart_clk)) {
+		dev_warn(&pdev->dev, "Can't get timer clock\n");
+		return PTR_ERR(data->uart_clk);
+	}
+
+	err = clk_prepare_enable(data->uart_clk);
+	if (err) {
+		dev_warn(&pdev->dev, "Can't prepare clock\n");
+		clk_put(data->uart_clk);
+		return err;
+	}
+	p->uartclk = clk_get_rate(data->uart_clk);
+
+	return 0;
+}
+
+static int mtk8250_probe(struct platform_device *pdev)
+{
+	struct uart_8250_port uart = {};
+	struct resource *regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct resource *irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	struct mtk8250_data *data;
+	int err;
+
+	if (!regs || !irq) {
+		dev_err(&pdev->dev, "no registers/irq defined\n");
+		return -EINVAL;
+	}
+
+	uart.port.membase = devm_ioremap(&pdev->dev, regs->start,
+					 resource_size(regs));
+	if (!uart.port.membase)
+		return -ENOMEM;
+
+	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
+	if (!data)
+		return -ENOMEM;
+
+	if (pdev->dev.of_node) {
+		err = mtk8250_probe_of(pdev, &uart.port, data);
+		if (err)
+			return err;
+	} else
+		return -ENODEV;
+
+	spin_lock_init(&uart.port.lock);
+	uart.port.mapbase = regs->start;
+	uart.port.irq = irq->start;
+	uart.port.pm = mtk8250_do_pm;
+	uart.port.type = PORT_16550;
+	uart.port.flags = UPF_BOOT_AUTOCONF | UPF_FIXED_PORT;
+	uart.port.dev = &pdev->dev;
+	uart.port.iotype = UPIO_MEM32;
+	uart.port.regshift = 2;
+	uart.port.private_data = data;
+	uart.port.set_termios = mtk8250_set_termios;
+
+	/* Disable Rate Fix function */
+	writel(0x0, uart.port.membase +
+			(MTK_UART_RATE_FIX << uart.port.regshift));
+
+	data->line = serial8250_register_8250_port(&uart);
+	if (data->line < 0)
+		return data->line;
+
+	platform_set_drvdata(pdev, data);
+
+	pm_runtime_set_active(&pdev->dev);
+	pm_runtime_enable(&pdev->dev);
+
+	return 0;
+}
+
+static int mtk8250_remove(struct platform_device *pdev)
+{
+	struct mtk8250_data *data = platform_get_drvdata(pdev);
+
+	pm_runtime_get_sync(&pdev->dev);
+
+	serial8250_unregister_port(data->line);
+	if (!IS_ERR(data->uart_clk)) {
+		clk_disable_unprepare(data->uart_clk);
+		clk_put(data->uart_clk);
+	}
+
+	pm_runtime_disable(&pdev->dev);
+	pm_runtime_put_noidle(&pdev->dev);
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int mtk8250_suspend(struct device *dev)
+{
+	struct mtk8250_data *data = dev_get_drvdata(dev);
+
+	serial8250_suspend_port(data->line);
+
+	return 0;
+}
+
+static int mtk8250_resume(struct device *dev)
+{
+	struct mtk8250_data *data = dev_get_drvdata(dev);
+
+	serial8250_resume_port(data->line);
+
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+#ifdef CONFIG_PM_RUNTIME
+static int mtk8250_runtime_suspend(struct device *dev)
+{
+	struct mtk8250_data *data = dev_get_drvdata(dev);
+
+	if (!IS_ERR(data->uart_clk))
+		clk_disable_unprepare(data->uart_clk);
+
+	return 0;
+}
+
+static int mtk8250_runtime_resume(struct device *dev)
+{
+	struct mtk8250_data *data = dev_get_drvdata(dev);
+
+	if (!IS_ERR(data->uart_clk))
+		clk_prepare_enable(data->uart_clk);
+
+	return 0;
+}
+#endif
+
+static const struct dev_pm_ops mtk8250_pm_ops = {
+	SET_SYSTEM_SLEEP_PM_OPS(mtk8250_suspend, mtk8250_resume)
+	SET_RUNTIME_PM_OPS(mtk8250_runtime_suspend, mtk8250_runtime_resume,
+				NULL)
+};
+
+static const struct of_device_id mtk8250_of_match[] = {
+	{ .compatible = "mediatek,mt6577-uart" },
+	{ /* Sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mtk8250_of_match);
+
+static struct platform_driver mtk8250_platform_driver = {
+	.driver = {
+		.name		= "mt6577-uart",
+		.pm		= &mtk8250_pm_ops,
+		.of_match_table	= mtk8250_of_match,
+	},
+	.probe			= mtk8250_probe,
+	.remove			= mtk8250_remove,
+};
+module_platform_driver(mtk8250_platform_driver);
+
+MODULE_AUTHOR("Matthias Brugger");
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("Mediatek 8250 serial port driver");
diff --git a/drivers/tty/serial/8250/8250_pci.c b/drivers/tty/serial/8250/8250_pci.c
index 61830b1792eb..beb9d71cd47a 100644
--- a/drivers/tty/serial/8250/8250_pci.c
+++ b/drivers/tty/serial/8250/8250_pci.c
@@ -25,6 +25,9 @@
 #include <asm/byteorder.h>
 #include <asm/io.h>
 
+#include <linux/dmaengine.h>
+#include <linux/platform_data/dma-dw.h>
+
 #include "8250.h"
 
 /*
@@ -1349,15 +1352,15 @@ ce4100_serial_setup(struct serial_private *priv,
 #define PCI_DEVICE_ID_INTEL_BYT_UART1	0x0f0a
 #define PCI_DEVICE_ID_INTEL_BYT_UART2	0x0f0c
 
+#define PCI_DEVICE_ID_INTEL_BSW_UART1	0x228a
+#define PCI_DEVICE_ID_INTEL_BSW_UART2	0x228c
+
 #define BYT_PRV_CLK			0x800
 #define BYT_PRV_CLK_EN			(1 << 0)
 #define BYT_PRV_CLK_M_VAL_SHIFT		1
 #define BYT_PRV_CLK_N_VAL_SHIFT		16
 #define BYT_PRV_CLK_UPDATE		(1 << 31)
 
-#define BYT_GENERAL_REG			0x808
-#define BYT_GENERAL_DIS_RTS_N_OVERRIDE	(1 << 3)
-
 #define BYT_TX_OVF_INT			0x820
 #define BYT_TX_OVF_INT_MASK		(1 << 1)
 
@@ -1412,22 +1415,18 @@ byt_set_termios(struct uart_port *p, struct ktermios *termios,
 	reg |= BYT_PRV_CLK_EN | BYT_PRV_CLK_UPDATE;
 	writel(reg, p->membase + BYT_PRV_CLK);
 
-	/*
-	 * If auto-handshake mechanism is not enabled,
-	 * disable rts_n override
-	 */
-	reg = readl(p->membase + BYT_GENERAL_REG);
-	reg &= ~BYT_GENERAL_DIS_RTS_N_OVERRIDE;
-	if (termios->c_cflag & CRTSCTS)
-		reg |= BYT_GENERAL_DIS_RTS_N_OVERRIDE;
-	writel(reg, p->membase + BYT_GENERAL_REG);
-
 	serial8250_do_set_termios(p, termios, old);
 }
 
 static bool byt_dma_filter(struct dma_chan *chan, void *param)
 {
-	return chan->chan_id == *(int *)param;
+	struct dw_dma_slave *dws = param;
+
+	if (dws->dma_dev != chan->device->dev)
+		return false;
+
+	chan->private = dws;
+	return true;
 }
 
 static int
@@ -1435,35 +1434,57 @@ byt_serial_setup(struct serial_private *priv,
 		 const struct pciserial_board *board,
 		 struct uart_8250_port *port, int idx)
 {
+	struct pci_dev *pdev = priv->dev;
+	struct device *dev = port->port.dev;
 	struct uart_8250_dma *dma;
+	struct dw_dma_slave *tx_param, *rx_param;
+	struct pci_dev *dma_dev;
 	int ret;
 
-	dma = devm_kzalloc(port->port.dev, sizeof(*dma), GFP_KERNEL);
+	dma = devm_kzalloc(dev, sizeof(*dma), GFP_KERNEL);
 	if (!dma)
 		return -ENOMEM;
 
-	switch (priv->dev->device) {
+	tx_param = devm_kzalloc(dev, sizeof(*tx_param), GFP_KERNEL);
+	if (!tx_param)
+		return -ENOMEM;
+
+	rx_param = devm_kzalloc(dev, sizeof(*rx_param), GFP_KERNEL);
+	if (!rx_param)
+		return -ENOMEM;
+
+	switch (pdev->device) {
 	case PCI_DEVICE_ID_INTEL_BYT_UART1:
-		dma->rx_chan_id = 3;
-		dma->tx_chan_id = 2;
+	case PCI_DEVICE_ID_INTEL_BSW_UART1:
+		rx_param->src_id = 3;
+		tx_param->dst_id = 2;
 		break;
 	case PCI_DEVICE_ID_INTEL_BYT_UART2:
-		dma->rx_chan_id = 5;
-		dma->tx_chan_id = 4;
+	case PCI_DEVICE_ID_INTEL_BSW_UART2:
+		rx_param->src_id = 5;
+		tx_param->dst_id = 4;
 		break;
 	default:
 		return -EINVAL;
 	}
 
-	dma->rxconf.slave_id = dma->rx_chan_id;
+	rx_param->src_master = 1;
+	rx_param->dst_master = 0;
+
 	dma->rxconf.src_maxburst = 16;
 
-	dma->txconf.slave_id = dma->tx_chan_id;
+	tx_param->src_master = 1;
+	tx_param->dst_master = 0;
+
 	dma->txconf.dst_maxburst = 16;
 
+	dma_dev = pci_get_slot(pdev->bus, PCI_DEVFN(PCI_SLOT(pdev->devfn), 0));
+	rx_param->dma_dev = &dma_dev->dev;
+	tx_param->dma_dev = &dma_dev->dev;
+
 	dma->fn = byt_dma_filter;
-	dma->rx_param = &dma->rx_chan_id;
-	dma->tx_param = &dma->tx_chan_id;
+	dma->rx_param = rx_param;
+	dma->tx_param = tx_param;
 
 	ret = pci_default_setup(priv, board, port, idx);
 	port->port.iotype = UPIO_MEM;
@@ -1788,6 +1809,7 @@ pci_wch_ch353_setup(struct serial_private *priv,
 #define PCI_DEVICE_ID_COMMTECH_4222PCIE	0x0022
 #define PCI_DEVICE_ID_BROADCOM_TRUMANAGE 0x160a
 #define PCI_DEVICE_ID_AMCC_ADDIDATA_APCI7800 0x818e
+#define PCI_DEVICE_ID_INTEL_QRK_UART	0x0936
 
 #define PCI_VENDOR_ID_SUNIX		0x1fd4
 #define PCI_DEVICE_ID_SUNIX_1999	0x1999
@@ -1898,6 +1920,27 @@ static struct pci_serial_quirk pci_serial_quirks[] __refdata = {
 		.subdevice	= PCI_ANY_ID,
 		.setup		= byt_serial_setup,
 	},
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_QRK_UART,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= pci_default_setup,
+	},
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_BSW_UART1,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= byt_serial_setup,
+	},
+	{
+		.vendor		= PCI_VENDOR_ID_INTEL,
+		.device		= PCI_DEVICE_ID_INTEL_BSW_UART2,
+		.subvendor	= PCI_ANY_ID,
+		.subdevice	= PCI_ANY_ID,
+		.setup		= byt_serial_setup,
+	},
 	/*
 	 * ITE
 	 */
@@ -2740,6 +2783,7 @@ enum pci_board_num_t {
 	pbn_ADDIDATA_PCIe_8_3906250,
 	pbn_ce4100_1_115200,
 	pbn_byt,
+	pbn_qrk,
 	pbn_omegapci,
 	pbn_NETMOS9900_2s_115200,
 	pbn_brcm_trumanage,
@@ -3490,6 +3534,12 @@ static struct pciserial_board pci_boards[] = {
 		.uart_offset	= 0x80,
 		.reg_shift      = 2,
 	},
+	[pbn_qrk] = {
+		.flags		= FL_BASE0,
+		.num_ports	= 1,
+		.base_baud	= 2764800,
+		.reg_shift	= 2,
+	},
 	[pbn_omegapci] = {
 		.flags		= FL_BASE0,
 		.num_ports	= 8,
@@ -5190,8 +5240,22 @@ static struct pci_device_id serial_pci_tbl[] = {
 		PCI_ANY_ID,  PCI_ANY_ID,
 		PCI_CLASS_COMMUNICATION_SERIAL << 8, 0xff0000,
 		pbn_byt },
+	{	PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BSW_UART1,
+		PCI_ANY_ID,  PCI_ANY_ID,
+		PCI_CLASS_COMMUNICATION_SERIAL << 8, 0xff0000,
+		pbn_byt },
+	{	PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_BSW_UART2,
+		PCI_ANY_ID,  PCI_ANY_ID,
+		PCI_CLASS_COMMUNICATION_SERIAL << 8, 0xff0000,
+		pbn_byt },
 
 	/*
+	 * Intel Quark x1000
+	 */
+	{	PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_QRK_UART,
+		PCI_ANY_ID, PCI_ANY_ID, 0, 0,
+		pbn_qrk },
+	/*
 	 * Cronyx Omega PCI
 	 */
 	{	PCI_VENDOR_ID_PLX, PCI_DEVICE_ID_PLX_CRONYX_OMEGA,
diff --git a/drivers/tty/serial/8250/Kconfig b/drivers/tty/serial/8250/Kconfig
index 349ee598b34c..21eca79224e4 100644
--- a/drivers/tty/serial/8250/Kconfig
+++ b/drivers/tty/serial/8250/Kconfig
@@ -298,3 +298,18 @@ config SERIAL_8250_RT288X
 	  If you have a Ralink RT288x/RT305x SoC based board and want to use the
 	  serial port, say Y to this option. The driver can handle up to 2 serial
 	  ports. If unsure, say N.
+
+config SERIAL_8250_FINTEK
+	tristate "Support for Fintek F81216A LPC to 4 UART"
+	depends on SERIAL_8250 && PNP
+	help
+	  Selecting this option will add support for the Fintek F81216A
+	  LPC to 4 UART. This device has some RS485 functionality not available
+	  through the PNP driver. If unsure, say N.
+
+config SERIAL_8250_MT6577
+	bool "Mediatek serial port support"
+	depends on SERIAL_8250 && ARCH_MEDIATEK
+	help
+	  If you have a Mediatek based board and want to use the
+	  serial port, say Y to this option. If unsure, say N.
diff --git a/drivers/tty/serial/8250/Makefile b/drivers/tty/serial/8250/Makefile
index 36d68d054307..5256b894e46a 100644
--- a/drivers/tty/serial/8250/Makefile
+++ b/drivers/tty/serial/8250/Makefile
@@ -20,3 +20,5 @@ obj-$(CONFIG_SERIAL_8250_HUB6)		+= 8250_hub6.o
 obj-$(CONFIG_SERIAL_8250_FSL)		+= 8250_fsl.o
 obj-$(CONFIG_SERIAL_8250_DW)		+= 8250_dw.o
 obj-$(CONFIG_SERIAL_8250_EM)		+= 8250_em.o
+obj-$(CONFIG_SERIAL_8250_FINTEK)	+= 8250_fintek.o
+obj-$(CONFIG_SERIAL_8250_MT6577)	+= 8250_mtk.o
diff --git a/drivers/tty/serial/Kconfig b/drivers/tty/serial/Kconfig
index 26cec64dadd7..649b784081c7 100644
--- a/drivers/tty/serial/Kconfig
+++ b/drivers/tty/serial/Kconfig
@@ -200,10 +200,29 @@ config SERIAL_KS8695_CONSOLE
 	  receives all kernel messages and warnings and which allows
 	  logins in single user mode).
 
+config SERIAL_MESON
+	tristate "Meson serial port support"
+	depends on ARCH_MESON
+	select SERIAL_CORE
+	help
+	  This enables the driver for the on-chip UARTs of the Amlogic
+	  MesonX processors.
+
+config SERIAL_MESON_CONSOLE
+	bool "Support for console on meson"
+	depends on SERIAL_MESON=y
+	select SERIAL_CORE_CONSOLE
+	help
+	  Say Y here if you wish to use a Amlogic MesonX UART as the
+	  system console (the system console is the device which
+	  receives all kernel messages and warnings and which allows
+	  logins in single user mode) as /dev/ttyAMLx.
+
 config SERIAL_CLPS711X
 	tristate "CLPS711X serial port support"
 	depends on ARCH_CLPS711X || COMPILE_TEST
 	select SERIAL_CORE
+	select SERIAL_MCTRL_GPIO if GPIOLIB
 	help
 	  This enables the driver for the on-chip UARTs of the Cirrus
 	  Logic EP711x/EP721x/EP731x processors.
@@ -220,7 +239,7 @@ config SERIAL_CLPS711X_CONSOLE
 
 config SERIAL_SAMSUNG
 	tristate "Samsung SoC serial support"
-	depends on PLAT_SAMSUNG
+	depends on PLAT_SAMSUNG || ARCH_EXYNOS
 	select SERIAL_CORE
 	help
 	  Support for the on-chip UARTs on the Samsung S3C24XX series CPUs,
@@ -1051,6 +1070,7 @@ config SERIAL_MSM_CONSOLE
 	bool "MSM serial console support"
 	depends on SERIAL_MSM=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 
 config SERIAL_MSM_HS
 	tristate "MSM UART High Speed: Serial Driver"
@@ -1410,6 +1430,7 @@ config SERIAL_XILINX_PS_UART_CONSOLE
 	bool "Cadence UART console support"
 	depends on SERIAL_XILINX_PS_UART=y
 	select SERIAL_CORE_CONSOLE
+	select SERIAL_EARLYCON
 	help
 	  Enable a Cadence UART port to be the system console.
 
diff --git a/drivers/tty/serial/Makefile b/drivers/tty/serial/Makefile
index 0080cc362e09..9a548acf5fdc 100644
--- a/drivers/tty/serial/Makefile
+++ b/drivers/tty/serial/Makefile
@@ -48,6 +48,7 @@ obj-$(CONFIG_SERIAL_MPC52xx) += mpc52xx_uart.o
 obj-$(CONFIG_SERIAL_ICOM) += icom.o
 obj-$(CONFIG_SERIAL_M32R_SIO) += m32r_sio.o
 obj-$(CONFIG_SERIAL_MPSC) += mpsc.o
+obj-$(CONFIG_SERIAL_MESON) += meson_uart.o
 obj-$(CONFIG_SERIAL_SB1250_DUART) += sb1250-duart.o
 obj-$(CONFIG_ETRAX_SERIAL) += crisv10.o
 obj-$(CONFIG_SERIAL_SCCNXP) += sccnxp.o
diff --git a/drivers/tty/serial/altera_jtaguart.c b/drivers/tty/serial/altera_jtaguart.c
index d22e3d98ae23..932e01995c0a 100644
--- a/drivers/tty/serial/altera_jtaguart.c
+++ b/drivers/tty/serial/altera_jtaguart.c
@@ -462,7 +462,7 @@ static int altera_jtaguart_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id altera_jtaguart_match[] = {
+static const struct of_device_id altera_jtaguart_match[] = {
 	{ .compatible = "ALTR,juart-1.0", },
 	{ .compatible = "altr,juart-1.0", },
 	{},
diff --git a/drivers/tty/serial/altera_uart.c b/drivers/tty/serial/altera_uart.c
index 6a243239dbef..1cb2cdb1bc42 100644
--- a/drivers/tty/serial/altera_uart.c
+++ b/drivers/tty/serial/altera_uart.c
@@ -610,7 +610,7 @@ static int altera_uart_remove(struct platform_device *pdev)
 }
 
 #ifdef CONFIG_OF
-static struct of_device_id altera_uart_match[] = {
+static const struct of_device_id altera_uart_match[] = {
 	{ .compatible = "ALTR,uart-1.0", },
 	{ .compatible = "altr,uart-1.0", },
 	{},
diff --git a/drivers/tty/serial/amba-pl011.c b/drivers/tty/serial/amba-pl011.c
index 8572f2a57fc8..02016fcd91b8 100644
--- a/drivers/tty/serial/amba-pl011.c
+++ b/drivers/tty/serial/amba-pl011.c
@@ -678,7 +678,8 @@ static void pl011_dma_flush_buffer(struct uart_port *port)
 __releases(&uap->port.lock)
 __acquires(&uap->port.lock)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 
 	if (!uap->using_tx_dma)
 		return;
@@ -1163,7 +1164,8 @@ static inline bool pl011_dma_rx_running(struct uart_amba_port *uap)
 
 static void pl011_stop_tx(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 
 	uap->im &= ~UART011_TXIM;
 	writew(uap->im, uap->port.membase + UART011_IMSC);
@@ -1172,7 +1174,8 @@ static void pl011_stop_tx(struct uart_port *port)
 
 static void pl011_start_tx(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 
 	if (!pl011_dma_tx_start(uap)) {
 		uap->im |= UART011_TXIM;
@@ -1182,7 +1185,8 @@ static void pl011_start_tx(struct uart_port *port)
 
 static void pl011_stop_rx(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 
 	uap->im &= ~(UART011_RXIM|UART011_RTIM|UART011_FEIM|
 		     UART011_PEIM|UART011_BEIM|UART011_OEIM);
@@ -1193,7 +1197,8 @@ static void pl011_stop_rx(struct uart_port *port)
 
 static void pl011_enable_ms(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 
 	uap->im |= UART011_RIMIM|UART011_CTSMIM|UART011_DCDMIM|UART011_DSRMIM;
 	writew(uap->im, uap->port.membase + UART011_IMSC);
@@ -1349,14 +1354,16 @@ static irqreturn_t pl011_int(int irq, void *dev_id)
 
 static unsigned int pl011_tx_empty(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned int status = readw(uap->port.membase + UART01x_FR);
 	return status & (UART01x_FR_BUSY|UART01x_FR_TXFF) ? 0 : TIOCSER_TEMT;
 }
 
 static unsigned int pl011_get_mctrl(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned int result = 0;
 	unsigned int status = readw(uap->port.membase + UART01x_FR);
 
@@ -1374,7 +1381,8 @@ static unsigned int pl011_get_mctrl(struct uart_port *port)
 
 static void pl011_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned int cr;
 
 	cr = readw(uap->port.membase + UART011_CR);
@@ -1402,7 +1410,8 @@ static void pl011_set_mctrl(struct uart_port *port, unsigned int mctrl)
 
 static void pl011_break_ctl(struct uart_port *port, int break_state)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned long flags;
 	unsigned int lcr_h;
 
@@ -1420,7 +1429,8 @@ static void pl011_break_ctl(struct uart_port *port, int break_state)
 
 static void pl011_quiesce_irqs(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned char __iomem *regs = uap->port.membase;
 
 	writew(readw(regs + UART011_MIS), regs + UART011_ICR);
@@ -1442,7 +1452,8 @@ static void pl011_quiesce_irqs(struct uart_port *port)
 
 static int pl011_get_poll_char(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned int status;
 
 	/*
@@ -1461,7 +1472,8 @@ static int pl011_get_poll_char(struct uart_port *port)
 static void pl011_put_poll_char(struct uart_port *port,
 			 unsigned char ch)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 
 	while (readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF)
 		barrier();
@@ -1473,7 +1485,8 @@ static void pl011_put_poll_char(struct uart_port *port,
 
 static int pl011_hwinit(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	int retval;
 
 	/* Optionaly enable pins to be muxed in and configured */
@@ -1526,7 +1539,8 @@ static void pl011_write_lcr_h(struct uart_amba_port *uap, unsigned int lcr_h)
 
 static int pl011_startup(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned int cr, lcr_h, fbrd, ibrd;
 	int retval;
 
@@ -1618,7 +1632,8 @@ static void pl011_shutdown_channel(struct uart_amba_port *uap,
 
 static void pl011_shutdown(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned int cr;
 
 	/*
@@ -1680,7 +1695,8 @@ static void
 pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 		     struct ktermios *old)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	unsigned int lcr_h, old_cr;
 	unsigned long flags;
 	unsigned int baud, quot, clkdiv;
@@ -1822,7 +1838,8 @@ pl011_set_termios(struct uart_port *port, struct ktermios *termios,
 
 static const char *pl011_type(struct uart_port *port)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 	return uap->port.type == PORT_AMBA ? uap->type : NULL;
 }
 
@@ -1900,7 +1917,8 @@ static struct uart_amba_port *amba_ports[UART_NR];
 
 static void pl011_console_putchar(struct uart_port *port, int ch)
 {
-	struct uart_amba_port *uap = (struct uart_amba_port *)port;
+	struct uart_amba_port *uap =
+	    container_of(port, struct uart_amba_port, port);
 
 	while (readw(uap->port.membase + UART01x_FR) & UART01x_FR_TXFF)
 		barrier();
diff --git a/drivers/tty/serial/atmel_serial.c b/drivers/tty/serial/atmel_serial.c
index 7b63677475c1..edde3eca055d 100644
--- a/drivers/tty/serial/atmel_serial.c
+++ b/drivers/tty/serial/atmel_serial.c
@@ -37,6 +37,7 @@
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
 #include <linux/dma-mapping.h>
+#include <linux/dmaengine.h>
 #include <linux/atmel_pdc.h>
 #include <linux/atmel_serial.h>
 #include <linux/uaccess.h>
@@ -527,6 +528,45 @@ static void atmel_enable_ms(struct uart_port *port)
 }
 
 /*
+ * Disable modem status interrupts
+ */
+static void atmel_disable_ms(struct uart_port *port)
+{
+	struct atmel_uart_port *atmel_port = to_atmel_uart_port(port);
+	uint32_t idr = 0;
+
+	/*
+	 * Interrupt should not be disabled twice
+	 */
+	if (!atmel_port->ms_irq_enabled)
+		return;
+
+	atmel_port->ms_irq_enabled = false;
+
+	if (atmel_port->gpio_irq[UART_GPIO_CTS] >= 0)
+		disable_irq(atmel_port->gpio_irq[UART_GPIO_CTS]);
+	else
+		idr |= ATMEL_US_CTSIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_DSR] >= 0)
+		disable_irq(atmel_port->gpio_irq[UART_GPIO_DSR]);
+	else
+		idr |= ATMEL_US_DSRIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_RI] >= 0)
+		disable_irq(atmel_port->gpio_irq[UART_GPIO_RI]);
+	else
+		idr |= ATMEL_US_RIIC;
+
+	if (atmel_port->gpio_irq[UART_GPIO_DCD] >= 0)
+		disable_irq(atmel_port->gpio_irq[UART_GPIO_DCD]);
+	else
+		idr |= ATMEL_US_DCDIC;
+
+	UART_PUT_IDR(port, idr);
+}
+
+/*
  * Control the transmission of a break signal
  */
 static void atmel_break_ctl(struct uart_port *port, int break_state)
@@ -1993,7 +2033,9 @@ static void atmel_set_termios(struct uart_port *port, struct ktermios *termios,
 
 	/* CTS flow-control and modem-status interrupts */
 	if (UART_ENABLE_MS(port, termios->c_cflag))
-		port->ops->enable_ms(port);
+		atmel_enable_ms(port);
+	else
+		atmel_disable_ms(port);
 
 	spin_unlock_irqrestore(&port->lock, flags);
 }
diff --git a/drivers/tty/serial/bfin_sport_uart.c b/drivers/tty/serial/bfin_sport_uart.c
index 7810aa290edf..d62d8daac8ab 100644
--- a/drivers/tty/serial/bfin_sport_uart.c
+++ b/drivers/tty/serial/bfin_sport_uart.c
@@ -33,6 +33,7 @@
 #include <linux/tty.h>
 #include <linux/tty_flip.h>
 #include <linux/serial_core.h>
+#include <linux/gpio.h>
 
 #include <asm/bfin_sport.h>
 #include <asm/delay.h>
diff --git a/drivers/tty/serial/bfin_uart.c b/drivers/tty/serial/bfin_uart.c
index dec0fd725d80..7da9911e95f0 100644
--- a/drivers/tty/serial/bfin_uart.c
+++ b/drivers/tty/serial/bfin_uart.c
@@ -108,22 +108,23 @@ static void bfin_serial_set_mctrl(struct uart_port *port, unsigned int mctrl)
 static irqreturn_t bfin_serial_mctrl_cts_int(int irq, void *dev_id)
 {
 	struct bfin_serial_port *uart = dev_id;
-	unsigned int status = bfin_serial_get_mctrl(&uart->port);
+	struct uart_port *uport = &uart->port;
+	unsigned int status = bfin_serial_get_mctrl(uport);
 #ifdef CONFIG_SERIAL_BFIN_HARD_CTSRTS
-	struct tty_struct *tty = uart->port.state->port.tty;
 
 	UART_CLEAR_SCTS(uart);
-	if (tty->hw_stopped) {
+	if (uport->hw_stopped) {
 		if (status) {
-			tty->hw_stopped = 0;
-			uart_write_wakeup(&uart->port);
+			uport->hw_stopped = 0;
+			uart_write_wakeup(uport);
 		}
 	} else {
 		if (!status)
-			tty->hw_stopped = 1;
+			uport->hw_stopped = 1;
 	}
+#else
+	uart_handle_cts_change(uport, status & TIOCM_CTS);
 #endif
-	uart_handle_cts_change(&uart->port, status & TIOCM_CTS);
 
 	return IRQ_HANDLED;
 }
diff --git a/drivers/tty/serial/clps711x.c b/drivers/tty/serial/clps711x.c
index f5b4c3d7e38f..acfe31773643 100644
--- a/drivers/tty/serial/clps711x.c
+++ b/drivers/tty/serial/clps711x.c
@@ -33,6 +33,8 @@
 #include <linux/mfd/syscon.h>
 #include <linux/mfd/syscon/clps711x.h>
 
+#include "serial_mctrl_gpio.h"
+
 #define UART_CLPS711X_DEVNAME	"ttyCL"
 #define UART_CLPS711X_NR	2
 #define UART_CLPS711X_MAJOR	204
@@ -62,7 +64,7 @@ struct clps711x_port {
 	unsigned int		tx_enabled;
 	int			rx_irq;
 	struct regmap		*syscon;
-	bool			use_ms;
+	struct mctrl_gpios	*gpios;
 };
 
 static struct uart_driver clps711x_uart = {
@@ -198,28 +200,17 @@ static unsigned int uart_clps711x_tx_empty(struct uart_port *port)
 
 static unsigned int uart_clps711x_get_mctrl(struct uart_port *port)
 {
+	unsigned int result = TIOCM_DSR | TIOCM_CTS | TIOCM_CAR;
 	struct clps711x_port *s = dev_get_drvdata(port->dev);
-	unsigned int result = 0;
-
-	if (s->use_ms) {
-		u32 sysflg = 0;
-
-		regmap_read(s->syscon, SYSFLG_OFFSET, &sysflg);
-		if (sysflg & SYSFLG1_DCD)
-			result |= TIOCM_CAR;
-		if (sysflg & SYSFLG1_DSR)
-			result |= TIOCM_DSR;
-		if (sysflg & SYSFLG1_CTS)
-			result |= TIOCM_CTS;
-	} else
-		result = TIOCM_DSR | TIOCM_CTS | TIOCM_CAR;
 
-	return result;
+	return mctrl_gpio_get(s->gpios, &result);
 }
 
 static void uart_clps711x_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
-	/* Do nothing */
+	struct clps711x_port *s = dev_get_drvdata(port->dev);
+
+	mctrl_gpio_set(s->gpios, mctrl);
 }
 
 static void uart_clps711x_break_ctl(struct uart_port *port, int break_state)
@@ -490,15 +481,10 @@ static int uart_clps711x_probe(struct platform_device *pdev)
 		s->syscon = syscon_regmap_lookup_by_pdevname(syscon_name);
 		if (IS_ERR(s->syscon))
 			return PTR_ERR(s->syscon);
-
-		s->use_ms = !index;
 	} else {
 		s->syscon = syscon_regmap_lookup_by_phandle(np, "syscon");
 		if (IS_ERR(s->syscon))
 			return PTR_ERR(s->syscon);
-
-		if (!index)
-			s->use_ms = of_property_read_bool(np, "uart-use-ms");
 	}
 
 	s->port.line		= index;
@@ -513,6 +499,8 @@ static int uart_clps711x_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, s);
 
+	s->gpios = mctrl_gpio_init(&pdev->dev, 0);
+
 	ret = uart_add_one_port(&clps711x_uart, &s->port);
 	if (ret)
 		return ret;
diff --git a/drivers/tty/serial/imx.c b/drivers/tty/serial/imx.c
index 044e86d528ae..8f62a3cec23e 100644
--- a/drivers/tty/serial/imx.c
+++ b/drivers/tty/serial/imx.c
@@ -80,6 +80,7 @@
 #define URXD_FRMERR	(1<<12)
 #define URXD_BRK	(1<<11)
 #define URXD_PRERR	(1<<10)
+#define URXD_RX_DATA	(0xFF<<0)
 #define UCR1_ADEN	(1<<15) /* Auto detect interrupt */
 #define UCR1_ADBR	(1<<14) /* Auto detect baud rate */
 #define UCR1_TRDYEN	(1<<13) /* Transmitter ready interrupt enable */
@@ -435,12 +436,14 @@ static void imx_stop_rx(struct uart_port *port)
 	struct imx_port *sport = (struct imx_port *)port;
 	unsigned long temp;
 
-	/*
-	 * We are maybe in the SMP context, so if the DMA TX thread is running
-	 * on other cpu, we have to wait for it to finish.
-	 */
-	if (sport->dma_is_enabled && sport->dma_is_rxing)
-		return;
+	if (sport->dma_is_enabled && sport->dma_is_rxing) {
+		if (sport->port.suspended) {
+			dmaengine_terminate_all(sport->dma_chan_rx);
+			sport->dma_is_rxing = 0;
+		} else {
+			return;
+		}
+	}
 
 	temp = readl(sport->port.membase + UCR2);
 	writel(temp & ~UCR2_RXEN, sport->port.membase + UCR2);
@@ -464,9 +467,19 @@ static inline void imx_transmit_buffer(struct imx_port *sport)
 {
 	struct circ_buf *xmit = &sport->port.state->xmit;
 
+	if (sport->port.x_char) {
+		/* Send next char */
+		writel(sport->port.x_char, sport->port.membase + URTX0);
+		return;
+	}
+
+	if (uart_circ_empty(xmit) || uart_tx_stopped(&sport->port)) {
+		imx_stop_tx(&sport->port);
+		return;
+	}
+
 	while (!uart_circ_empty(xmit) &&
-			!(readl(sport->port.membase + uts_reg(sport))
-				& UTS_TXFULL)) {
+	       !(readl(sport->port.membase + uts_reg(sport)) & UTS_TXFULL)) {
 		/* send xmit->buf[xmit->tail]
 		 * out the port here */
 		writel(xmit->buf[xmit->tail], sport->port.membase + URTX0);
@@ -567,9 +580,6 @@ static void imx_start_tx(struct uart_port *port)
 	struct imx_port *sport = (struct imx_port *)port;
 	unsigned long temp;
 
-	if (uart_circ_empty(&port->state->xmit))
-		return;
-
 	if (USE_IRDA(sport)) {
 		/* half duplex in IrDA mode; have to disable receive mode */
 		temp = readl(sport->port.membase + UCR4);
@@ -604,7 +614,10 @@ static void imx_start_tx(struct uart_port *port)
 	}
 
 	if (sport->dma_is_enabled) {
-		imx_dma_tx(sport);
+		/* FIXME: port->x_char must be transmitted if != 0 */
+		if (!uart_circ_empty(&port->state->xmit) &&
+		    !uart_tx_stopped(port))
+			imx_dma_tx(sport);
 		return;
 	}
 
@@ -632,27 +645,10 @@ static irqreturn_t imx_rtsint(int irq, void *dev_id)
 static irqreturn_t imx_txint(int irq, void *dev_id)
 {
 	struct imx_port *sport = dev_id;
-	struct circ_buf *xmit = &sport->port.state->xmit;
 	unsigned long flags;
 
 	spin_lock_irqsave(&sport->port.lock, flags);
-	if (sport->port.x_char) {
-		/* Send next char */
-		writel(sport->port.x_char, sport->port.membase + URTX0);
-		goto out;
-	}
-
-	if (uart_circ_empty(xmit) || uart_tx_stopped(&sport->port)) {
-		imx_stop_tx(&sport->port);
-		goto out;
-	}
-
 	imx_transmit_buffer(sport);
-
-	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
-		uart_write_wakeup(&sport->port);
-
-out:
 	spin_unlock_irqrestore(&sport->port.lock, flags);
 	return IRQ_HANDLED;
 }
@@ -823,11 +819,9 @@ static void imx_set_mctrl(struct uart_port *port, unsigned int mctrl)
 	struct imx_port *sport = (struct imx_port *)port;
 	unsigned long temp;
 
-	temp = readl(sport->port.membase + UCR2) & ~UCR2_CTS;
-
+	temp = readl(sport->port.membase + UCR2) & ~(UCR2_CTS | UCR2_CTSC);
 	if (mctrl & TIOCM_RTS)
-		if (!sport->dma_is_enabled)
-			temp |= UCR2_CTS;
+		temp |= UCR2_CTS | UCR2_CTSC;
 
 	writel(temp, sport->port.membase + UCR2);
 
@@ -1225,9 +1219,18 @@ static void imx_shutdown(struct uart_port *port)
 	unsigned long flags;
 
 	if (sport->dma_is_enabled) {
+		int ret;
+
 		/* We have to wait for the DMA to finish. */
-		wait_event(sport->dma_wait,
+		ret = wait_event_interruptible(sport->dma_wait,
 			!sport->dma_is_rxing && !sport->dma_is_txing);
+		if (ret != 0) {
+			sport->dma_is_rxing = 0;
+			sport->dma_is_txing = 0;
+			dmaengine_terminate_all(sport->dma_chan_tx);
+			dmaengine_terminate_all(sport->dma_chan_rx);
+		}
+		imx_stop_tx(port);
 		imx_stop_rx(port);
 		imx_disable_dma(sport);
 		imx_uart_dma_exit(sport);
@@ -1506,32 +1509,10 @@ imx_verify_port(struct uart_port *port, struct serial_struct *ser)
 #if defined(CONFIG_CONSOLE_POLL)
 static int imx_poll_get_char(struct uart_port *port)
 {
-	struct imx_port_ucrs old_ucr;
-	unsigned int status;
-	unsigned char c;
-
-	/* save control registers */
-	imx_port_ucrs_save(port, &old_ucr);
-
-	/* disable interrupts */
-	writel(UCR1_UARTEN, port->membase + UCR1);
-	writel(old_ucr.ucr2 & ~(UCR2_ATEN | UCR2_RTSEN | UCR2_ESCI),
-	       port->membase + UCR2);
-	writel(old_ucr.ucr3 & ~(UCR3_DCD | UCR3_RI | UCR3_DTREN),
-	       port->membase + UCR3);
-
-	/* poll */
-	do {
-		status = readl(port->membase + USR2);
-	} while (~status & USR2_RDR);
-
-	/* read */
-	c = readl(port->membase + URXD0);
-
-	/* restore control registers */
-	imx_port_ucrs_restore(port, &old_ucr);
+	if (!(readl(port->membase + USR2) & USR2_RDR))
+		return NO_POLL_CHAR;
 
-	return c;
+	return readl(port->membase + URXD0) & URXD_RX_DATA;
 }
 
 static void imx_poll_put_char(struct uart_port *port, unsigned char c)
diff --git a/drivers/tty/serial/jsm/jsm.h b/drivers/tty/serial/jsm/jsm.h
index 844d5e4eb1aa..af7013488aeb 100644
--- a/drivers/tty/serial/jsm/jsm.h
+++ b/drivers/tty/serial/jsm/jsm.h
@@ -67,6 +67,16 @@ do {								\
 #define MAXPORTS	8
 #define MAX_STOPS_SENT	5
 
+/* Board ids */
+#define PCI_DEVICE_ID_NEO_4             0x00B0
+#define PCI_DEVICE_ID_NEO_1_422         0x00CC
+#define PCI_DEVICE_ID_NEO_1_422_485     0x00CD
+#define PCI_DEVICE_ID_NEO_2_422_485     0x00CE
+#define PCIE_DEVICE_ID_NEO_8            0x00F0
+#define PCIE_DEVICE_ID_NEO_4            0x00F1
+#define PCIE_DEVICE_ID_NEO_4RJ45        0x00F2
+#define PCIE_DEVICE_ID_NEO_8RJ45        0x00F3
+
 /* Board type definitions */
 
 #define T_NEO		0000
diff --git a/drivers/tty/serial/jsm/jsm_driver.c b/drivers/tty/serial/jsm/jsm_driver.c
index a47d882d6743..d2885a7bb090 100644
--- a/drivers/tty/serial/jsm/jsm_driver.c
+++ b/drivers/tty/serial/jsm/jsm_driver.c
@@ -93,12 +93,34 @@ static int jsm_probe_one(struct pci_dev *pdev, const struct pci_device_id *ent)
 	/* store the info for the board we've found */
 	brd->boardnum = adapter_count++;
 	brd->pci_dev = pdev;
-	if (pdev->device == PCIE_DEVICE_ID_NEO_4_IBM)
+
+	switch (pdev->device) {
+
+	case PCI_DEVICE_ID_NEO_2DB9:
+	case PCI_DEVICE_ID_NEO_2DB9PRI:
+	case PCI_DEVICE_ID_NEO_2RJ45:
+	case PCI_DEVICE_ID_NEO_2RJ45PRI:
+	case PCI_DEVICE_ID_NEO_2_422_485:
+		brd->maxports = 2;
+		break;
+
+	case PCI_DEVICE_ID_NEO_4:
+	case PCIE_DEVICE_ID_NEO_4:
+	case PCIE_DEVICE_ID_NEO_4RJ45:
+	case PCIE_DEVICE_ID_NEO_4_IBM:
 		brd->maxports = 4;
-	else if (pdev->device == PCI_DEVICE_ID_DIGI_NEO_8)
+		break;
+
+	case PCI_DEVICE_ID_DIGI_NEO_8:
+	case PCIE_DEVICE_ID_NEO_8:
+	case PCIE_DEVICE_ID_NEO_8RJ45:
 		brd->maxports = 8;
-	else
-		brd->maxports = 2;
+		break;
+
+	default:
+		brd->maxports = 1;
+		break;
+	}
 
 	spin_lock_init(&brd->bd_intr_lock);
 
@@ -209,6 +231,14 @@ static struct pci_device_id jsm_pci_tbl[] = {
 	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_NEO_2RJ45PRI), 0, 0, 3 },
 	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_4_IBM), 0, 0, 4 },
 	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_DIGI_NEO_8), 0, 0, 5 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_NEO_4), 0, 0, 6 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_NEO_1_422), 0, 0, 7 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_NEO_1_422_485), 0, 0, 8 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCI_DEVICE_ID_NEO_2_422_485), 0, 0, 9 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_8), 0, 0, 10 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_4), 0, 0, 11 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_4RJ45), 0, 0, 12 },
+	{ PCI_DEVICE(PCI_VENDOR_ID_DIGI, PCIE_DEVICE_ID_NEO_8RJ45), 0, 0, 13 },
 	{ 0, }
 };
 MODULE_DEVICE_TABLE(pci, jsm_pci_tbl);
diff --git a/drivers/tty/serial/kgdb_nmi.c b/drivers/tty/serial/kgdb_nmi.c
index 6ec7501b464d..129dc5be6028 100644
--- a/drivers/tty/serial/kgdb_nmi.c
+++ b/drivers/tty/serial/kgdb_nmi.c
@@ -46,6 +46,8 @@ static atomic_t kgdb_nmi_num_readers = ATOMIC_INIT(0);
 
 static int kgdb_nmi_console_setup(struct console *co, char *options)
 {
+	arch_kgdb_ops.enable_nmi(1);
+
 	/* The NMI console uses the dbg_io_ops to issue console messages. To
 	 * avoid duplicate messages during kdb sessions we must inform kdb's
 	 * I/O utilities that messages sent to the console will automatically
@@ -77,7 +79,7 @@ static struct console kgdb_nmi_console = {
 	.setup  = kgdb_nmi_console_setup,
 	.write	= kgdb_nmi_console_write,
 	.device	= kgdb_nmi_console_device,
-	.flags	= CON_PRINTBUFFER | CON_ANYTIME | CON_ENABLED,
+	.flags	= CON_PRINTBUFFER | CON_ANYTIME,
 	.index	= -1,
 };
 
@@ -354,7 +356,6 @@ int kgdb_register_nmi_console(void)
 	}
 
 	register_console(&kgdb_nmi_console);
-	arch_kgdb_ops.enable_nmi(1);
 
 	return 0;
 err_drv_reg:
diff --git a/drivers/tty/serial/max310x.c b/drivers/tty/serial/max310x.c
index 82573dc4d8cf..0041a64cc86e 100644
--- a/drivers/tty/serial/max310x.c
+++ b/drivers/tty/serial/max310x.c
@@ -1248,7 +1248,7 @@ static int max310x_probe(struct device *dev, struct max310x_devtype *devtype,
 	mutex_destroy(&s->mutex);
 
 #ifdef CONFIG_GPIOLIB
-	WARN_ON(gpiochip_remove(&s->gpio));
+	gpiochip_remove(&s->gpio);
 
 out_uart:
 #endif
@@ -1263,12 +1263,10 @@ out_clk:
 static int max310x_remove(struct device *dev)
 {
 	struct max310x_port *s = dev_get_drvdata(dev);
-	int i, ret = 0;
+	int i;
 
 #ifdef CONFIG_GPIOLIB
-	ret = gpiochip_remove(&s->gpio);
-	if (ret)
-		return ret;
+	gpiochip_remove(&s->gpio);
 #endif
 
 	for (i = 0; i < s->uart.nr; i++) {
@@ -1282,7 +1280,7 @@ static int max310x_remove(struct device *dev)
 	uart_unregister_driver(&s->uart);
 	clk_disable_unprepare(s->clk);
 
-	return ret;
+	return 0;
 }
 
 static const struct of_device_id __maybe_unused max310x_dt_ids[] = {
diff --git a/drivers/tty/serial/meson_uart.c b/drivers/tty/serial/meson_uart.c
new file mode 100644
index 000000000000..15c749753317
--- /dev/null
+++ b/drivers/tty/serial/meson_uart.c
@@ -0,0 +1,634 @@
+/*
+ *  Based on meson_uart.c, by AMLOGIC, INC.
+ *
+ * Copyright (C) 2014 Carlo Caione <carlo@caione.org>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published
+ * by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+
+#include <linux/clk.h>
+#include <linux/console.h>
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/serial.h>
+#include <linux/serial_core.h>
+#include <linux/tty.h>
+#include <linux/tty_flip.h>
+
+/* Register offsets */
+#define AML_UART_WFIFO			0x00
+#define AML_UART_RFIFO			0x04
+#define AML_UART_CONTROL		0x08
+#define AML_UART_STATUS			0x0c
+#define AML_UART_MISC			0x10
+#define AML_UART_REG5			0x14
+
+/* AML_UART_CONTROL bits */
+#define AML_UART_TX_EN			BIT(12)
+#define AML_UART_RX_EN			BIT(13)
+#define AML_UART_TX_RST			BIT(22)
+#define AML_UART_RX_RST			BIT(23)
+#define AML_UART_CLR_ERR		BIT(24)
+#define AML_UART_RX_INT_EN		BIT(27)
+#define AML_UART_TX_INT_EN		BIT(28)
+#define AML_UART_DATA_LEN_MASK		(0x03 << 20)
+#define AML_UART_DATA_LEN_8BIT		(0x00 << 20)
+#define AML_UART_DATA_LEN_7BIT		(0x01 << 20)
+#define AML_UART_DATA_LEN_6BIT		(0x02 << 20)
+#define AML_UART_DATA_LEN_5BIT		(0x03 << 20)
+
+/* AML_UART_STATUS bits */
+#define AML_UART_PARITY_ERR		BIT(16)
+#define AML_UART_FRAME_ERR		BIT(17)
+#define AML_UART_TX_FIFO_WERR		BIT(18)
+#define AML_UART_RX_EMPTY		BIT(20)
+#define AML_UART_TX_FULL		BIT(21)
+#define AML_UART_TX_EMPTY		BIT(22)
+#define AML_UART_ERR			(AML_UART_PARITY_ERR | \
+					 AML_UART_FRAME_ERR  | \
+					 AML_UART_TX_FIFO_WERR)
+
+/* AML_UART_CONTROL bits */
+#define AML_UART_TWO_WIRE_EN		BIT(15)
+#define AML_UART_PARITY_TYPE		BIT(18)
+#define AML_UART_PARITY_EN		BIT(19)
+#define AML_UART_CLEAR_ERR		BIT(24)
+#define AML_UART_STOP_BIN_LEN_MASK	(0x03 << 16)
+#define AML_UART_STOP_BIN_1SB		(0x00 << 16)
+#define AML_UART_STOP_BIN_2SB		(0x01 << 16)
+
+/* AML_UART_MISC bits */
+#define AML_UART_XMIT_IRQ(c)		(((c) & 0xff) << 8)
+#define AML_UART_RECV_IRQ(c)		((c) & 0xff)
+
+/* AML_UART_REG5 bits */
+#define AML_UART_BAUD_MASK		0x7fffff
+#define AML_UART_BAUD_USE		BIT(23)
+
+#define AML_UART_PORT_NUM		6
+#define AML_UART_DEV_NAME		"ttyAML"
+
+
+static struct uart_driver meson_uart_driver;
+
+static struct uart_port *meson_ports[AML_UART_PORT_NUM];
+
+static void meson_uart_set_mctrl(struct uart_port *port, unsigned int mctrl)
+{
+}
+
+static unsigned int meson_uart_get_mctrl(struct uart_port *port)
+{
+	return TIOCM_CTS;
+}
+
+static unsigned int meson_uart_tx_empty(struct uart_port *port)
+{
+	u32 val;
+
+	val = readl(port->membase + AML_UART_STATUS);
+	return (val & AML_UART_TX_EMPTY) ? TIOCSER_TEMT : 0;
+}
+
+static void meson_uart_stop_tx(struct uart_port *port)
+{
+	u32 val;
+
+	val = readl(port->membase + AML_UART_CONTROL);
+	val &= ~AML_UART_TX_EN;
+	writel(val, port->membase + AML_UART_CONTROL);
+}
+
+static void meson_uart_stop_rx(struct uart_port *port)
+{
+	u32 val;
+
+	val = readl(port->membase + AML_UART_CONTROL);
+	val &= ~AML_UART_RX_EN;
+	writel(val, port->membase + AML_UART_CONTROL);
+}
+
+static void meson_uart_shutdown(struct uart_port *port)
+{
+	unsigned long flags;
+	u32 val;
+
+	free_irq(port->irq, port);
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	val = readl(port->membase + AML_UART_CONTROL);
+	val &= ~(AML_UART_RX_EN | AML_UART_TX_EN);
+	val &= ~(AML_UART_RX_INT_EN | AML_UART_TX_INT_EN);
+	writel(val, port->membase + AML_UART_CONTROL);
+
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static void meson_uart_start_tx(struct uart_port *port)
+{
+	struct circ_buf *xmit = &port->state->xmit;
+	unsigned int ch;
+
+	if (uart_tx_stopped(port)) {
+		meson_uart_stop_tx(port);
+		return;
+	}
+
+	while (!(readl(port->membase + AML_UART_STATUS) & AML_UART_TX_FULL)) {
+		if (port->x_char) {
+			writel(port->x_char, port->membase + AML_UART_WFIFO);
+			port->icount.tx++;
+			port->x_char = 0;
+			continue;
+		}
+
+		if (uart_circ_empty(xmit))
+			break;
+
+		ch = xmit->buf[xmit->tail];
+		writel(ch, port->membase + AML_UART_WFIFO);
+		xmit->tail = (xmit->tail+1) & (SERIAL_XMIT_SIZE - 1);
+		port->icount.tx++;
+	}
+
+	if (uart_circ_chars_pending(xmit) < WAKEUP_CHARS)
+		uart_write_wakeup(port);
+}
+
+static void meson_receive_chars(struct uart_port *port)
+{
+	struct tty_port *tport = &port->state->port;
+	char flag;
+	u32 status, ch, mode;
+
+	do {
+		flag = TTY_NORMAL;
+		port->icount.rx++;
+		status = readl(port->membase + AML_UART_STATUS);
+
+		if (status & AML_UART_ERR) {
+			if (status & AML_UART_TX_FIFO_WERR)
+				port->icount.overrun++;
+			else if (status & AML_UART_FRAME_ERR)
+				port->icount.frame++;
+			else if (status & AML_UART_PARITY_ERR)
+				port->icount.frame++;
+
+			mode = readl(port->membase + AML_UART_CONTROL);
+			mode |= AML_UART_CLEAR_ERR;
+			writel(mode, port->membase + AML_UART_CONTROL);
+
+			/* It doesn't clear to 0 automatically */
+			mode &= ~AML_UART_CLEAR_ERR;
+			writel(mode, port->membase + AML_UART_CONTROL);
+
+			status &= port->read_status_mask;
+			if (status & AML_UART_FRAME_ERR)
+				flag = TTY_FRAME;
+			else if (status & AML_UART_PARITY_ERR)
+				flag = TTY_PARITY;
+		}
+
+		ch = readl(port->membase + AML_UART_RFIFO);
+		ch &= 0xff;
+
+		if ((status & port->ignore_status_mask) == 0)
+			tty_insert_flip_char(tport, ch, flag);
+
+		if (status & AML_UART_TX_FIFO_WERR)
+			tty_insert_flip_char(tport, 0, TTY_OVERRUN);
+
+	} while (!(readl(port->membase + AML_UART_STATUS) & AML_UART_RX_EMPTY));
+
+	spin_unlock(&port->lock);
+	tty_flip_buffer_push(tport);
+	spin_lock(&port->lock);
+}
+
+static irqreturn_t meson_uart_interrupt(int irq, void *dev_id)
+{
+	struct uart_port *port = (struct uart_port *)dev_id;
+
+	spin_lock(&port->lock);
+
+	if (!(readl(port->membase + AML_UART_STATUS) & AML_UART_RX_EMPTY))
+		meson_receive_chars(port);
+
+	if (!(readl(port->membase + AML_UART_STATUS) & AML_UART_TX_FULL))
+		meson_uart_start_tx(port);
+
+	spin_unlock(&port->lock);
+
+	return IRQ_HANDLED;
+}
+
+static const char *meson_uart_type(struct uart_port *port)
+{
+	return (port->type == PORT_MESON) ? "meson_uart" : NULL;
+}
+
+static int meson_uart_startup(struct uart_port *port)
+{
+	u32 val;
+	int ret = 0;
+
+	val = readl(port->membase + AML_UART_CONTROL);
+	val |= (AML_UART_RX_RST | AML_UART_TX_RST | AML_UART_CLR_ERR);
+	writel(val, port->membase + AML_UART_CONTROL);
+
+	val &= ~(AML_UART_RX_RST | AML_UART_TX_RST | AML_UART_CLR_ERR);
+	writel(val, port->membase + AML_UART_CONTROL);
+
+	val |= (AML_UART_RX_EN | AML_UART_TX_EN);
+	writel(val, port->membase + AML_UART_CONTROL);
+
+	val |= (AML_UART_RX_INT_EN | AML_UART_TX_INT_EN);
+	writel(val, port->membase + AML_UART_CONTROL);
+
+	val = (AML_UART_RECV_IRQ(1) | AML_UART_XMIT_IRQ(port->fifosize / 2));
+	writel(val, port->membase + AML_UART_MISC);
+
+	ret = request_irq(port->irq, meson_uart_interrupt, 0,
+			  meson_uart_type(port), port);
+
+	return ret;
+}
+
+static void meson_uart_change_speed(struct uart_port *port, unsigned long baud)
+{
+	u32 val;
+
+	while (!(readl(port->membase + AML_UART_STATUS) & AML_UART_TX_EMPTY))
+		cpu_relax();
+
+	val = readl(port->membase + AML_UART_REG5);
+	val &= ~AML_UART_BAUD_MASK;
+	val = ((port->uartclk * 10 / (baud * 4) + 5) / 10) - 1;
+	val |= AML_UART_BAUD_USE;
+	writel(val, port->membase + AML_UART_REG5);
+}
+
+static void meson_uart_set_termios(struct uart_port *port,
+				   struct ktermios *termios,
+				   struct ktermios *old)
+{
+	unsigned int cflags, iflags, baud;
+	unsigned long flags;
+	u32 val;
+
+	spin_lock_irqsave(&port->lock, flags);
+
+	cflags = termios->c_cflag;
+	iflags = termios->c_iflag;
+
+	val = readl(port->membase + AML_UART_CONTROL);
+
+	val &= ~AML_UART_DATA_LEN_MASK;
+	switch (cflags & CSIZE) {
+	case CS8:
+		val |= AML_UART_DATA_LEN_8BIT;
+		break;
+	case CS7:
+		val |= AML_UART_DATA_LEN_7BIT;
+		break;
+	case CS6:
+		val |= AML_UART_DATA_LEN_6BIT;
+		break;
+	case CS5:
+		val |= AML_UART_DATA_LEN_5BIT;
+		break;
+	}
+
+	if (cflags & PARENB)
+		val |= AML_UART_PARITY_EN;
+	else
+		val &= ~AML_UART_PARITY_EN;
+
+	if (cflags & PARODD)
+		val |= AML_UART_PARITY_TYPE;
+	else
+		val &= ~AML_UART_PARITY_TYPE;
+
+	val &= ~AML_UART_STOP_BIN_LEN_MASK;
+	if (cflags & CSTOPB)
+		val |= AML_UART_STOP_BIN_2SB;
+	else
+		val &= ~AML_UART_STOP_BIN_1SB;
+
+	if (cflags & CRTSCTS)
+		val &= ~AML_UART_TWO_WIRE_EN;
+	else
+		val |= AML_UART_TWO_WIRE_EN;
+
+	writel(val, port->membase + AML_UART_CONTROL);
+
+	baud = uart_get_baud_rate(port, termios, old, 9600, 115200);
+	meson_uart_change_speed(port, baud);
+
+	port->read_status_mask = AML_UART_TX_FIFO_WERR;
+	if (iflags & INPCK)
+		port->read_status_mask |= AML_UART_PARITY_ERR |
+					  AML_UART_FRAME_ERR;
+
+	port->ignore_status_mask = 0;
+	if (iflags & IGNPAR)
+		port->ignore_status_mask |= AML_UART_PARITY_ERR |
+					    AML_UART_FRAME_ERR;
+
+	uart_update_timeout(port, termios->c_cflag, baud);
+	spin_unlock_irqrestore(&port->lock, flags);
+}
+
+static int meson_uart_verify_port(struct uart_port *port,
+				  struct serial_struct *ser)
+{
+	int ret = 0;
+
+	if (port->type != PORT_MESON)
+		ret = -EINVAL;
+	if (port->irq != ser->irq)
+		ret = -EINVAL;
+	if (ser->baud_base < 9600)
+		ret = -EINVAL;
+	return ret;
+}
+
+static void meson_uart_release_port(struct uart_port *port)
+{
+	if (port->flags & UPF_IOREMAP) {
+		iounmap(port->membase);
+		port->membase = NULL;
+	}
+}
+
+static int meson_uart_request_port(struct uart_port *port)
+{
+	struct platform_device *pdev = to_platform_device(port->dev);
+	struct resource *res;
+	int size;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res) {
+		dev_err(&pdev->dev, "cannot obtain I/O memory region");
+		return -ENODEV;
+	}
+	size = resource_size(res);
+
+	if (!devm_request_mem_region(port->dev, port->mapbase, size,
+				     dev_name(port->dev))) {
+		dev_err(port->dev, "Memory region busy\n");
+		return -EBUSY;
+	}
+
+	if (port->flags & UPF_IOREMAP) {
+		port->membase = devm_ioremap_nocache(port->dev,
+						     port->mapbase,
+						     size);
+		if (port->membase == NULL)
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void meson_uart_config_port(struct uart_port *port, int flags)
+{
+	if (flags & UART_CONFIG_TYPE) {
+		port->type = PORT_MESON;
+		meson_uart_request_port(port);
+	}
+}
+
+static struct uart_ops meson_uart_ops = {
+	.set_mctrl      = meson_uart_set_mctrl,
+	.get_mctrl      = meson_uart_get_mctrl,
+	.tx_empty	= meson_uart_tx_empty,
+	.start_tx	= meson_uart_start_tx,
+	.stop_tx	= meson_uart_stop_tx,
+	.stop_rx	= meson_uart_stop_rx,
+	.startup	= meson_uart_startup,
+	.shutdown	= meson_uart_shutdown,
+	.set_termios	= meson_uart_set_termios,
+	.type		= meson_uart_type,
+	.config_port	= meson_uart_config_port,
+	.request_port	= meson_uart_request_port,
+	.release_port	= meson_uart_release_port,
+	.verify_port	= meson_uart_verify_port,
+};
+
+#ifdef CONFIG_SERIAL_MESON_CONSOLE
+
+static void meson_console_putchar(struct uart_port *port, int ch)
+{
+	if (!port->membase)
+		return;
+
+	while (readl(port->membase + AML_UART_STATUS) & AML_UART_TX_FULL)
+		cpu_relax();
+	writel(ch, port->membase + AML_UART_WFIFO);
+}
+
+static void meson_serial_console_write(struct console *co, const char *s,
+				       u_int count)
+{
+	struct uart_port *port;
+	unsigned long flags;
+	int locked;
+
+	port = meson_ports[co->index];
+	if (!port)
+		return;
+
+	local_irq_save(flags);
+	if (port->sysrq) {
+		locked = 0;
+	} else if (oops_in_progress) {
+		locked = spin_trylock(&port->lock);
+	} else {
+		spin_lock(&port->lock);
+		locked = 1;
+	}
+
+	uart_console_write(port, s, count, meson_console_putchar);
+
+	if (locked)
+		spin_unlock(&port->lock);
+	local_irq_restore(flags);
+}
+
+static int meson_serial_console_setup(struct console *co, char *options)
+{
+	struct uart_port *port;
+	int baud = 115200;
+	int bits = 8;
+	int parity = 'n';
+	int flow = 'n';
+
+	if (co->index < 0 || co->index >= AML_UART_PORT_NUM)
+		return -EINVAL;
+
+	port = meson_ports[co->index];
+	if (!port || !port->membase)
+		return -ENODEV;
+
+	if (options)
+		uart_parse_options(options, &baud, &parity, &bits, &flow);
+
+	return uart_set_options(port, co, baud, parity, bits, flow);
+}
+
+static struct console meson_serial_console = {
+	.name		= AML_UART_DEV_NAME,
+	.write		= meson_serial_console_write,
+	.device		= uart_console_device,
+	.setup		= meson_serial_console_setup,
+	.flags		= CON_PRINTBUFFER,
+	.index		= -1,
+	.data		= &meson_uart_driver,
+};
+
+static int __init meson_serial_console_init(void)
+{
+	register_console(&meson_serial_console);
+	return 0;
+}
+console_initcall(meson_serial_console_init);
+
+#define MESON_SERIAL_CONSOLE	(&meson_serial_console)
+#else
+#define MESON_SERIAL_CONSOLE	NULL
+#endif
+
+static struct uart_driver meson_uart_driver = {
+	.owner		= THIS_MODULE,
+	.driver_name	= "meson_uart",
+	.dev_name	= AML_UART_DEV_NAME,
+	.nr		= AML_UART_PORT_NUM,
+	.cons		= MESON_SERIAL_CONSOLE,
+};
+
+static int meson_uart_probe(struct platform_device *pdev)
+{
+	struct resource *res_mem, *res_irq;
+	struct uart_port *port;
+	struct clk *clk;
+	int ret = 0;
+
+	if (pdev->dev.of_node)
+		pdev->id = of_alias_get_id(pdev->dev.of_node, "serial");
+
+	if (pdev->id < 0 || pdev->id >= AML_UART_PORT_NUM)
+		return -EINVAL;
+
+	res_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	if (!res_mem)
+		return -ENODEV;
+
+	res_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!res_irq)
+		return -ENODEV;
+
+	if (meson_ports[pdev->id]) {
+		dev_err(&pdev->dev, "port %d already allocated\n", pdev->id);
+		return -EBUSY;
+	}
+
+	port = devm_kzalloc(&pdev->dev, sizeof(struct uart_port), GFP_KERNEL);
+	if (!port)
+		return -ENOMEM;
+
+	clk = clk_get(&pdev->dev, NULL);
+	if (IS_ERR(clk))
+		return PTR_ERR(clk);
+
+	port->uartclk = clk_get_rate(clk);
+	port->iotype = UPIO_MEM;
+	port->mapbase = res_mem->start;
+	port->irq = res_irq->start;
+	port->flags = UPF_BOOT_AUTOCONF | UPF_IOREMAP | UPF_LOW_LATENCY;
+	port->dev = &pdev->dev;
+	port->line = pdev->id;
+	port->type = PORT_MESON;
+	port->x_char = 0;
+	port->ops = &meson_uart_ops;
+	port->fifosize = 64;
+
+	meson_ports[pdev->id] = port;
+	platform_set_drvdata(pdev, port);
+
+	ret = uart_add_one_port(&meson_uart_driver, port);
+	if (ret)
+		meson_ports[pdev->id] = NULL;
+
+	return ret;
+}
+
+static int meson_uart_remove(struct platform_device *pdev)
+{
+	struct uart_port *port;
+
+	port = platform_get_drvdata(pdev);
+	uart_remove_one_port(&meson_uart_driver, port);
+	meson_ports[pdev->id] = NULL;
+
+	return 0;
+}
+
+
+static const struct of_device_id meson_uart_dt_match[] = {
+	{ .compatible = "amlogic,meson-uart" },
+	{ /* sentinel */ },
+};
+MODULE_DEVICE_TABLE(of, meson_uart_dt_match);
+
+static  struct platform_driver meson_uart_platform_driver = {
+	.probe		= meson_uart_probe,
+	.remove		= meson_uart_remove,
+	.driver		= {
+		.owner		= THIS_MODULE,
+		.name		= "meson_uart",
+		.of_match_table	= meson_uart_dt_match,
+	},
+};
+
+static int __init meson_uart_init(void)
+{
+	int ret;
+
+	ret = uart_register_driver(&meson_uart_driver);
+	if (ret)
+		return ret;
+
+	ret = platform_driver_register(&meson_uart_platform_driver);
+	if (ret)
+		uart_unregister_driver(&meson_uart_driver);
+
+	return ret;
+}
+
+static void __exit meson_uart_exit(void)
+{
+	platform_driver_unregister(&meson_uart_platform_driver);
+	uart_unregister_driver(&meson_uart_driver);
+}
+
+module_init(meson_uart_init);
+module_exit(meson_uart_exit);
+
+MODULE_AUTHOR("Carlo Caione <carlo@caione.org>");
+MODULE_DESCRIPTION("Amlogic Meson serial port driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/tty/serial/mpc52xx_uart.c b/drivers/tty/serial/mpc52xx_uart.c
index 97888f4900ec..a5f4e3648b15 100644
--- a/drivers/tty/serial/mpc52xx_uart.c
+++ b/drivers/tty/serial/mpc52xx_uart.c
@@ -1087,22 +1087,6 @@ mpc52xx_uart_start_tx(struct uart_port *port)
 }
 
 static void
-mpc52xx_uart_send_xchar(struct uart_port *port, char ch)
-{
-	unsigned long flags;
-	spin_lock_irqsave(&port->lock, flags);
-
-	port->x_char = ch;
-	if (ch) {
-		/* Make sure tx interrupts are on */
-		/* Truly necessary ??? They should be anyway */
-		psc_ops->start_tx(port);
-	}
-
-	spin_unlock_irqrestore(&port->lock, flags);
-}
-
-static void
 mpc52xx_uart_stop_rx(struct uart_port *port)
 {
 	/* port->lock taken by caller */
@@ -1361,7 +1345,6 @@ static struct uart_ops mpc52xx_uart_ops = {
 	.get_mctrl	= mpc52xx_uart_get_mctrl,
 	.stop_tx	= mpc52xx_uart_stop_tx,
 	.start_tx	= mpc52xx_uart_start_tx,
-	.send_xchar	= mpc52xx_uart_send_xchar,
 	.stop_rx	= mpc52xx_uart_stop_rx,
 	.enable_ms	= mpc52xx_uart_enable_ms,
 	.break_ctl	= mpc52xx_uart_break_ctl,
diff --git a/drivers/tty/serial/msm_serial.c b/drivers/tty/serial/msm_serial.c
index 0da0b5474e98..4b6c78331a64 100644
--- a/drivers/tty/serial/msm_serial.c
+++ b/drivers/tty/serial/msm_serial.c
@@ -190,11 +190,10 @@ static void handle_rx(struct uart_port *port)
 		/* Mask conditions we're ignorning. */
 		sr &= port->read_status_mask;
 
-		if (sr & UART_SR_RX_BREAK) {
+		if (sr & UART_SR_RX_BREAK)
 			flag = TTY_BREAK;
-		} else if (sr & UART_SR_PAR_FRAME_ERR) {
+		else if (sr & UART_SR_PAR_FRAME_ERR)
 			flag = TTY_FRAME;
-		}
 
 		if (!uart_handle_sysrq_char(port, c))
 			tty_insert_flip_char(tport, c, flag);
@@ -315,7 +314,6 @@ static unsigned int msm_get_mctrl(struct uart_port *port)
 	return TIOCM_CAR | TIOCM_CTS | TIOCM_DSR | TIOCM_RTS;
 }
 
-
 static void msm_reset(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
@@ -336,6 +334,7 @@ static void msm_reset(struct uart_port *port)
 static void msm_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
 	unsigned int mr;
+
 	mr = msm_read(port, UART_MR1);
 
 	if (!(mctrl & TIOCM_RTS)) {
@@ -431,7 +430,6 @@ static int msm_set_baud_rate(struct uart_port *port, unsigned int baud)
 	return baud;
 }
 
-
 static void msm_init_clock(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
@@ -646,6 +644,7 @@ fail_release_port:
 static void msm_config_port(struct uart_port *port, int flags)
 {
 	int ret;
+
 	if (flags & UART_CONFIG_TYPE) {
 		port->type = PORT_MSM;
 		ret = msm_request_port(port);
@@ -678,22 +677,11 @@ static void msm_power(struct uart_port *port, unsigned int state,
 		clk_disable_unprepare(msm_port->pclk);
 		break;
 	default:
-		printk(KERN_ERR "msm_serial: Unknown PM state %d\n", state);
+		pr_err("msm_serial: Unknown PM state %d\n", state);
 	}
 }
 
 #ifdef CONFIG_CONSOLE_POLL
-static int msm_poll_init(struct uart_port *port)
-{
-	struct msm_port *msm_port = UART_TO_MSM(port);
-
-	/* Enable single character mode on RX FIFO */
-	if (msm_port->is_uartdm >= UARTDM_1P4)
-		msm_write(port, UARTDM_DMEN_RX_SC_ENABLE, UARTDM_DMEN);
-
-	return 0;
-}
-
 static int msm_poll_get_char_single(struct uart_port *port)
 {
 	struct msm_port *msm_port = UART_TO_MSM(port);
@@ -701,11 +689,11 @@ static int msm_poll_get_char_single(struct uart_port *port)
 
 	if (!(msm_read(port, UART_SR) & UART_SR_RX_READY))
 		return NO_POLL_CHAR;
-	else
-		return msm_read(port, rf_reg) & 0xff;
+
+	return msm_read(port, rf_reg) & 0xff;
 }
 
-static int msm_poll_get_char_dm_1p3(struct uart_port *port)
+static int msm_poll_get_char_dm(struct uart_port *port)
 {
 	int c;
 	static u32 slop;
@@ -729,6 +717,10 @@ static int msm_poll_get_char_dm_1p3(struct uart_port *port)
 			slop = msm_read(port, UARTDM_RF);
 			c = sp[0];
 			count--;
+			msm_write(port, UART_CR_CMD_RESET_STALE_INT, UART_CR);
+			msm_write(port, 0xFFFFFF, UARTDM_DMRX);
+			msm_write(port, UART_CR_CMD_STALE_EVENT_ENABLE,
+				  UART_CR);
 		} else {
 			c = NO_POLL_CHAR;
 		}
@@ -752,8 +744,8 @@ static int msm_poll_get_char(struct uart_port *port)
 	imr = msm_read(port, UART_IMR);
 	msm_write(port, 0, UART_IMR);
 
-	if (msm_port->is_uartdm == UARTDM_1P3)
-		c = msm_poll_get_char_dm_1p3(port);
+	if (msm_port->is_uartdm)
+		c = msm_poll_get_char_dm(port);
 	else
 		c = msm_poll_get_char_single(port);
 
@@ -788,8 +780,6 @@ static void msm_poll_put_char(struct uart_port *port, unsigned char c)
 
 	/* Enable interrupts */
 	msm_write(port, imr, UART_IMR);
-
-	return;
 }
 #endif
 
@@ -812,7 +802,6 @@ static struct uart_ops msm_uart_pops = {
 	.verify_port = msm_verify_port,
 	.pm = msm_power,
 #ifdef CONFIG_CONSOLE_POLL
-	.poll_init = msm_poll_init,
 	.poll_get_char	= msm_poll_get_char,
 	.poll_put_char	= msm_poll_put_char,
 #endif
@@ -856,22 +845,15 @@ static inline struct uart_port *get_port_from_line(unsigned int line)
 }
 
 #ifdef CONFIG_SERIAL_MSM_CONSOLE
-static void msm_console_write(struct console *co, const char *s,
-			      unsigned int count)
+static void __msm_console_write(struct uart_port *port, const char *s,
+				unsigned int count, bool is_uartdm)
 {
 	int i;
-	struct uart_port *port;
-	struct msm_port *msm_port;
 	int num_newlines = 0;
 	bool replaced = false;
 	void __iomem *tf;
 
-	BUG_ON(co->index < 0 || co->index >= UART_NR);
-
-	port = get_port_from_line(co->index);
-	msm_port = UART_TO_MSM(port);
-
-	if (msm_port->is_uartdm)
+	if (is_uartdm)
 		tf = port->membase + UARTDM_TF;
 	else
 		tf = port->membase + UART_TF;
@@ -883,7 +865,7 @@ static void msm_console_write(struct console *co, const char *s,
 	count += num_newlines;
 
 	spin_lock(&port->lock);
-	if (msm_port->is_uartdm)
+	if (is_uartdm)
 		reset_dm_count(port, count);
 
 	i = 0;
@@ -892,7 +874,7 @@ static void msm_console_write(struct console *co, const char *s,
 		unsigned int num_chars;
 		char buf[4] = { 0 };
 
-		if (msm_port->is_uartdm)
+		if (is_uartdm)
 			num_chars = min(count - i, (unsigned int)sizeof(buf));
 		else
 			num_chars = 1;
@@ -921,6 +903,20 @@ static void msm_console_write(struct console *co, const char *s,
 	spin_unlock(&port->lock);
 }
 
+static void msm_console_write(struct console *co, const char *s,
+			      unsigned int count)
+{
+	struct uart_port *port;
+	struct msm_port *msm_port;
+
+	BUG_ON(co->index < 0 || co->index >= UART_NR);
+
+	port = get_port_from_line(co->index);
+	msm_port = UART_TO_MSM(port);
+
+	__msm_console_write(port, s, count, msm_port->is_uartdm);
+}
+
 static int __init msm_console_setup(struct console *co, char *options)
 {
 	struct uart_port *port;
@@ -958,11 +954,54 @@ static int __init msm_console_setup(struct console *co, char *options)
 		msm_write(port, UART_CR_TX_ENABLE, UART_CR);
 	}
 
-	printk(KERN_INFO "msm_serial: console setup on port #%d\n", port->line);
+	pr_info("msm_serial: console setup on port #%d\n", port->line);
 
 	return uart_set_options(port, co, baud, parity, bits, flow);
 }
 
+static void
+msm_serial_early_write(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+
+	__msm_console_write(&dev->port, s, n, false);
+}
+
+static int __init
+msm_serial_early_console_setup(struct earlycon_device *device, const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->con->write = msm_serial_early_write;
+	return 0;
+}
+EARLYCON_DECLARE(msm_serial, msm_serial_early_console_setup);
+OF_EARLYCON_DECLARE(msm_serial, "qcom,msm-uart",
+		    msm_serial_early_console_setup);
+
+static void
+msm_serial_early_write_dm(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+
+	__msm_console_write(&dev->port, s, n, true);
+}
+
+static int __init
+msm_serial_early_console_setup_dm(struct earlycon_device *device,
+				  const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->con->write = msm_serial_early_write_dm;
+	return 0;
+}
+EARLYCON_DECLARE(msm_serial_dm, msm_serial_early_console_setup_dm);
+OF_EARLYCON_DECLARE(msm_serial_dm, "qcom,msm-uartdm",
+		    msm_serial_early_console_setup_dm);
+
 static struct uart_driver msm_uart_driver;
 
 static struct console msm_console = {
@@ -1013,7 +1052,7 @@ static int msm_serial_probe(struct platform_device *pdev)
 	if (unlikely(pdev->id < 0 || pdev->id >= UART_NR))
 		return -ENXIO;
 
-	printk(KERN_INFO "msm_serial: detected port #%d\n", pdev->id);
+	dev_info(&pdev->dev, "msm_serial: detected port #%d\n", pdev->id);
 
 	port = get_port_from_line(pdev->id);
 	port->dev = &pdev->dev;
@@ -1038,8 +1077,7 @@ static int msm_serial_probe(struct platform_device *pdev)
 	}
 
 	port->uartclk = clk_get_rate(msm_port->clk);
-	printk(KERN_INFO "uartclk = %d\n", port->uartclk);
-
+	dev_info(&pdev->dev, "uartclk = %d\n", port->uartclk);
 
 	resource = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	if (unlikely(!resource))
@@ -1093,7 +1131,7 @@ static int __init msm_serial_init(void)
 	if (unlikely(ret))
 		uart_unregister_driver(&msm_uart_driver);
 
-	printk(KERN_INFO "msm_serial: driver initialized\n");
+	pr_info("msm_serial: driver initialized\n");
 
 	return ret;
 }
diff --git a/drivers/tty/serial/mxs-auart.c b/drivers/tty/serial/mxs-auart.c
index b5c329248c81..10c29334fe2f 100644
--- a/drivers/tty/serial/mxs-auart.c
+++ b/drivers/tty/serial/mxs-auart.c
@@ -408,7 +408,7 @@ static void mxs_auart_set_mctrl(struct uart_port *u, unsigned mctrl)
 
 	ctrl &= ~(AUART_CTRL2_RTSEN | AUART_CTRL2_RTS);
 	if (mctrl & TIOCM_RTS) {
-		if (tty_port_cts_enabled(&u->state->port))
+		if (uart_cts_enabled(u))
 			ctrl |= AUART_CTRL2_RTSEN;
 		else
 			ctrl |= AUART_CTRL2_RTS;
diff --git a/drivers/tty/serial/nwpserial.c b/drivers/tty/serial/nwpserial.c
index c06366b6bc29..5da7622e88c3 100644
--- a/drivers/tty/serial/nwpserial.c
+++ b/drivers/tty/serial/nwpserial.c
@@ -22,6 +22,7 @@
 #include <linux/of_platform.h>
 #include <linux/of_device.h>
 #include <linux/nwpserial.h>
+#include <linux/delay.h>
 #include <asm/prom.h>
 #include <asm/dcr.h>
 
diff --git a/drivers/tty/serial/of_serial.c b/drivers/tty/serial/of_serial.c
index 68d4455f3cf9..8bc2563335ae 100644
--- a/drivers/tty/serial/of_serial.c
+++ b/drivers/tty/serial/of_serial.c
@@ -183,10 +183,6 @@ static int of_platform_serial_probe(struct platform_device *ofdev)
 					  "auto-flow-control"))
 			port8250.capabilities |= UART_CAP_AFE;
 
-		if (of_property_read_bool(ofdev->dev.of_node,
-					  "has-hw-flow-control"))
-			port8250.port.flags |= UPF_HARD_FLOW;
-
 		ret = serial8250_register_8250_port(&port8250);
 		break;
 	}
@@ -244,6 +240,32 @@ static int of_platform_serial_remove(struct platform_device *ofdev)
 	return 0;
 }
 
+#ifdef CONFIG_PM_SLEEP
+static int of_serial_suspend(struct device *dev)
+{
+	struct of_serial_info *info = dev_get_drvdata(dev);
+
+	serial8250_suspend_port(info->line);
+	if (info->clk)
+		clk_disable_unprepare(info->clk);
+
+	return 0;
+}
+
+static int of_serial_resume(struct device *dev)
+{
+	struct of_serial_info *info = dev_get_drvdata(dev);
+
+	if (info->clk)
+		clk_prepare_enable(info->clk);
+
+	serial8250_resume_port(info->line);
+
+	return 0;
+}
+#endif
+static SIMPLE_DEV_PM_OPS(of_serial_pm_ops, of_serial_suspend, of_serial_resume);
+
 /*
  * A few common types, add more as needed.
  */
@@ -275,6 +297,7 @@ static struct platform_driver of_platform_serial_driver = {
 		.name = "of_serial",
 		.owner = THIS_MODULE,
 		.of_match_table = of_platform_serial_table,
+		.pm = &of_serial_pm_ops,
 	},
 	.probe = of_platform_serial_probe,
 	.remove = of_platform_serial_remove,
diff --git a/drivers/tty/serial/omap-serial.c b/drivers/tty/serial/omap-serial.c
index d017cec8a34a..18c30cabe27f 100644
--- a/drivers/tty/serial/omap-serial.c
+++ b/drivers/tty/serial/omap-serial.c
@@ -239,6 +239,26 @@ static void serial_omap_enable_wakeup(struct uart_omap_port *up, bool enable)
 }
 
 /*
+ * Calculate the absolute difference between the desired and actual baud
+ * rate for the given mode.
+ */
+static inline int calculate_baud_abs_diff(struct uart_port *port,
+				unsigned int baud, unsigned int mode)
+{
+	unsigned int n = port->uartclk / (mode * baud);
+	int abs_diff;
+
+	if (n == 0)
+		n = 1;
+
+	abs_diff = baud - (port->uartclk / (mode * n));
+	if (abs_diff < 0)
+		abs_diff = -abs_diff;
+
+	return abs_diff;
+}
+
+/*
  * serial_omap_baud_is_mode16 - check if baud rate is MODE16X
  * @port: uart port info
  * @baud: baudrate for which mode needs to be determined
@@ -252,16 +272,10 @@ static void serial_omap_enable_wakeup(struct uart_omap_port *up, bool enable)
 static bool
 serial_omap_baud_is_mode16(struct uart_port *port, unsigned int baud)
 {
-	unsigned int n13 = port->uartclk / (13 * baud);
-	unsigned int n16 = port->uartclk / (16 * baud);
-	int baudAbsDiff13 = baud - (port->uartclk / (13 * n13));
-	int baudAbsDiff16 = baud - (port->uartclk / (16 * n16));
-	if (baudAbsDiff13 < 0)
-		baudAbsDiff13 = -baudAbsDiff13;
-	if (baudAbsDiff16 < 0)
-		baudAbsDiff16 = -baudAbsDiff16;
-
-	return (baudAbsDiff13 >= baudAbsDiff16);
+	int abs_diff_13 = calculate_baud_abs_diff(port, baud, 13);
+	int abs_diff_16 = calculate_baud_abs_diff(port, baud, 16);
+
+	return (abs_diff_13 >= abs_diff_16);
 }
 
 /*
diff --git a/drivers/tty/serial/sc16is7xx.c b/drivers/tty/serial/sc16is7xx.c
index 3284c31085a7..6246820d7f05 100644
--- a/drivers/tty/serial/sc16is7xx.c
+++ b/drivers/tty/serial/sc16is7xx.c
@@ -1157,7 +1157,7 @@ static int sc16is7xx_probe(struct device *dev,
 
 #ifdef CONFIG_GPIOLIB
 	if (devtype->nr_gpio)
-		WARN_ON(gpiochip_remove(&s->gpio));
+		gpiochip_remove(&s->gpio);
 
 out_uart:
 #endif
@@ -1173,14 +1173,11 @@ out_clk:
 static int sc16is7xx_remove(struct device *dev)
 {
 	struct sc16is7xx_port *s = dev_get_drvdata(dev);
-	int i, ret = 0;
+	int i;
 
 #ifdef CONFIG_GPIOLIB
-	if (s->devtype->nr_gpio) {
-		ret = gpiochip_remove(&s->gpio);
-		if (ret)
-			return ret;
-	}
+	if (s->devtype->nr_gpio)
+		gpiochip_remove(&s->gpio);
 #endif
 
 	for (i = 0; i < s->uart.nr; i++) {
@@ -1195,7 +1192,7 @@ static int sc16is7xx_remove(struct device *dev)
 	if (!IS_ERR(s->clk))
 		clk_disable_unprepare(s->clk);
 
-	return ret;
+	return 0;
 }
 
 static const struct of_device_id __maybe_unused sc16is7xx_dt_ids[] = {
diff --git a/drivers/tty/serial/serial_core.c b/drivers/tty/serial/serial_core.c
index 29a7be47389a..df3a8c74358e 100644
--- a/drivers/tty/serial/serial_core.c
+++ b/drivers/tty/serial/serial_core.c
@@ -59,6 +59,11 @@ static void uart_change_pm(struct uart_state *state,
 
 static void uart_port_shutdown(struct tty_port *port);
 
+static int uart_dcd_enabled(struct uart_port *uport)
+{
+	return uport->status & UPSTAT_DCD_ENABLE;
+}
+
 /*
  * This routine is used by the interrupt handler to schedule processing in
  * the software interrupt portion of the driver.
@@ -90,7 +95,7 @@ static void __uart_start(struct tty_struct *tty)
 	struct uart_state *state = tty->driver_data;
 	struct uart_port *port = state->uart_port;
 
-	if (!tty->stopped && !tty->hw_stopped)
+	if (!uart_tx_stopped(port))
 		port->ops->start_tx(port);
 }
 
@@ -130,7 +135,6 @@ static int uart_port_startup(struct tty_struct *tty, struct uart_state *state,
 		int init_hw)
 {
 	struct uart_port *uport = state->uart_port;
-	struct tty_port *port = &state->port;
 	unsigned long page;
 	int retval = 0;
 
@@ -175,17 +179,14 @@ static int uart_port_startup(struct tty_struct *tty, struct uart_state *state,
 			if (tty->termios.c_cflag & CBAUD)
 				uart_set_mctrl(uport, TIOCM_RTS | TIOCM_DTR);
 		}
-		/*
-		 * if hw support flow control without software intervention,
-		 * then skip the below check
-		 */
-		if (tty_port_cts_enabled(port) &&
-		    !(uport->flags & UPF_HARD_FLOW)) {
-			spin_lock_irq(&uport->lock);
-			if (!(uport->ops->get_mctrl(uport) & TIOCM_CTS))
-				tty->hw_stopped = 1;
-			spin_unlock_irq(&uport->lock);
-		}
+
+		spin_lock_irq(&uport->lock);
+		if (uart_cts_enabled(uport) &&
+		    !(uport->ops->get_mctrl(uport) & TIOCM_CTS))
+			uport->hw_stopped = 1;
+		else
+			uport->hw_stopped = 0;
+		spin_unlock_irq(&uport->lock);
 	}
 
 	/*
@@ -439,7 +440,6 @@ EXPORT_SYMBOL(uart_get_divisor);
 static void uart_change_speed(struct tty_struct *tty, struct uart_state *state,
 					struct ktermios *old_termios)
 {
-	struct tty_port *port = &state->port;
 	struct uart_port *uport = state->uart_port;
 	struct ktermios *termios;
 
@@ -454,17 +454,19 @@ static void uart_change_speed(struct tty_struct *tty, struct uart_state *state,
 	uport->ops->set_termios(uport, termios, old_termios);
 
 	/*
-	 * Set flags based on termios cflag
+	 * Set modem status enables based on termios cflag
 	 */
+	spin_lock_irq(&uport->lock);
 	if (termios->c_cflag & CRTSCTS)
-		set_bit(ASYNCB_CTS_FLOW, &port->flags);
+		uport->status |= UPSTAT_CTS_ENABLE;
 	else
-		clear_bit(ASYNCB_CTS_FLOW, &port->flags);
+		uport->status &= ~UPSTAT_CTS_ENABLE;
 
 	if (termios->c_cflag & CLOCAL)
-		clear_bit(ASYNCB_CHECK_CD, &port->flags);
+		uport->status &= ~UPSTAT_DCD_ENABLE;
 	else
-		set_bit(ASYNCB_CHECK_CD, &port->flags);
+		uport->status |= UPSTAT_DCD_ENABLE;
+	spin_unlock_irq(&uport->lock);
 }
 
 static inline int __uart_put_char(struct uart_port *port,
@@ -604,12 +606,11 @@ static void uart_send_xchar(struct tty_struct *tty, char ch)
 	if (port->ops->send_xchar)
 		port->ops->send_xchar(port, ch);
 	else {
+		spin_lock_irqsave(&port->lock, flags);
 		port->x_char = ch;
-		if (ch) {
-			spin_lock_irqsave(&port->lock, flags);
+		if (ch)
 			port->ops->start_tx(port);
-			spin_unlock_irqrestore(&port->lock, flags);
-		}
+		spin_unlock_irqrestore(&port->lock, flags);
 	}
 }
 
@@ -652,12 +653,8 @@ static void uart_unthrottle(struct tty_struct *tty)
 		mask &= ~port->flags;
 	}
 
-	if (mask & UPF_SOFT_FLOW) {
-		if (port->x_char)
-			port->x_char = 0;
-		else
-			uart_send_xchar(tty, START_CHAR(tty));
-	}
+	if (mask & UPF_SOFT_FLOW)
+		uart_send_xchar(tty, START_CHAR(tty));
 
 	if (mask & UPF_HARD_FLOW)
 		uart_set_mctrl(port, TIOCM_RTS);
@@ -892,10 +889,11 @@ static int uart_set_info(struct tty_struct *tty, struct tty_port *port,
 			 */
 			if (uport->flags & UPF_SPD_MASK) {
 				char buf[64];
-				printk(KERN_NOTICE
-				       "%s sets custom speed on %s. This "
-				       "is deprecated.\n", current->comm,
-				       tty_name(port->tty, buf));
+
+				dev_notice(uport->dev,
+				       "%s sets custom speed on %s. This is deprecated.\n",
+				      current->comm,
+				      tty_name(port->tty, buf));
 			}
 			uart_change_speed(tty, state, NULL);
 		}
@@ -952,7 +950,7 @@ static int uart_get_lsr_info(struct tty_struct *tty,
 	 */
 	if (uport->x_char ||
 	    ((uart_circ_chars_pending(&state->xmit) > 0) &&
-	     !tty->stopped && !tty->hw_stopped))
+	     !uart_tx_stopped(uport)))
 		result &= ~TIOCSER_TEMT;
 
 	return put_user(result, value);
@@ -1251,7 +1249,6 @@ static void uart_set_termios(struct tty_struct *tty,
 {
 	struct uart_state *state = tty->driver_data;
 	struct uart_port *uport = state->uart_port;
-	unsigned long flags;
 	unsigned int cflag = tty->termios.c_cflag;
 	unsigned int iflag_mask = IGNBRK|BRKINT|IGNPAR|PARMRK|INPCK;
 	bool sw_changed = false;
@@ -1291,34 +1288,33 @@ static void uart_set_termios(struct tty_struct *tty,
 	/* Handle transition away from B0 status */
 	else if (!(old_termios->c_cflag & CBAUD) && (cflag & CBAUD)) {
 		unsigned int mask = TIOCM_DTR;
-		if (!(cflag & CRTSCTS) ||
-		    !test_bit(TTY_THROTTLED, &tty->flags))
+		if (!(cflag & CRTSCTS) || !test_bit(TTY_THROTTLED, &tty->flags))
 			mask |= TIOCM_RTS;
 		uart_set_mctrl(uport, mask);
 	}
 
 	/*
 	 * If the port is doing h/w assisted flow control, do nothing.
-	 * We assume that tty->hw_stopped has never been set.
+	 * We assume that port->hw_stopped has never been set.
 	 */
 	if (uport->flags & UPF_HARD_FLOW)
 		return;
 
 	/* Handle turning off CRTSCTS */
 	if ((old_termios->c_cflag & CRTSCTS) && !(cflag & CRTSCTS)) {
-		spin_lock_irqsave(&uport->lock, flags);
-		tty->hw_stopped = 0;
+		spin_lock_irq(&uport->lock);
+		uport->hw_stopped = 0;
 		__uart_start(tty);
-		spin_unlock_irqrestore(&uport->lock, flags);
+		spin_unlock_irq(&uport->lock);
 	}
 	/* Handle turning on CRTSCTS */
 	else if (!(old_termios->c_cflag & CRTSCTS) && (cflag & CRTSCTS)) {
-		spin_lock_irqsave(&uport->lock, flags);
+		spin_lock_irq(&uport->lock);
 		if (!(uport->ops->get_mctrl(uport) & TIOCM_CTS)) {
-			tty->hw_stopped = 1;
+			uport->hw_stopped = 1;
 			uport->ops->stop_tx(uport);
 		}
-		spin_unlock_irqrestore(&uport->lock, flags);
+		spin_unlock_irq(&uport->lock);
 	}
 }
 
@@ -1975,12 +1971,9 @@ int uart_suspend_port(struct uart_driver *drv, struct uart_port *uport)
 		for (tries = 3; !ops->tx_empty(uport) && tries; tries--)
 			msleep(10);
 		if (!tries)
-			printk(KERN_ERR "%s%s%s%d: Unable to drain "
-					"transmitter\n",
-			       uport->dev ? dev_name(uport->dev) : "",
-			       uport->dev ? ": " : "",
-			       drv->dev_name,
-			       drv->tty_driver->name_base + uport->line);
+			dev_err(uport->dev, "%s%d: Unable to drain transmitter\n",
+				drv->dev_name,
+				drv->tty_driver->name_base + uport->line);
 
 		if (console_suspend_enabled || !uart_console(uport))
 			ops->shutdown(uport);
@@ -2109,9 +2102,7 @@ uart_report_port(struct uart_driver *drv, struct uart_port *port)
 		break;
 	}
 
-	printk(KERN_INFO "%s%s%s%d at %s (irq = %d, base_baud = %d) is a %s\n",
-	       port->dev ? dev_name(port->dev) : "",
-	       port->dev ? ": " : "",
+	dev_info(port->dev, "%s%d at %s (irq = %d, base_baud = %d) is a %s\n",
 	       drv->dev_name,
 	       drv->tty_driver->name_base + port->line,
 	       address, port->irq, port->uartclk / 16, uart_type(port));
@@ -2640,7 +2631,7 @@ int uart_add_one_port(struct uart_driver *drv, struct uart_port *uport)
 	if (likely(!IS_ERR(tty_dev))) {
 		device_set_wakeup_capable(tty_dev, 1);
 	} else {
-		printk(KERN_ERR "Cannot register tty device on line %d\n",
+		dev_err(uport->dev, "Cannot register tty device on line %d\n",
 		       uport->line);
 	}
 
@@ -2675,7 +2666,7 @@ int uart_remove_one_port(struct uart_driver *drv, struct uart_port *uport)
 	BUG_ON(in_interrupt());
 
 	if (state->uart_port != uport)
-		printk(KERN_ALERT "Removing wrong port: %p != %p\n",
+		dev_alert(uport->dev, "Removing wrong port: %p != %p\n",
 			state->uart_port, uport);
 
 	mutex_lock(&port_mutex);
@@ -2757,22 +2748,29 @@ EXPORT_SYMBOL(uart_match_port);
  *	uart_handle_dcd_change - handle a change of carrier detect state
  *	@uport: uart_port structure for the open port
  *	@status: new carrier detect status, nonzero if active
+ *
+ *	Caller must hold uport->lock
  */
 void uart_handle_dcd_change(struct uart_port *uport, unsigned int status)
 {
 	struct tty_port *port = &uport->state->port;
 	struct tty_struct *tty = port->tty;
-	struct tty_ldisc *ld = tty ? tty_ldisc_ref(tty) : NULL;
+	struct tty_ldisc *ld;
 
-	if (ld) {
-		if (ld->ops->dcd_change)
-			ld->ops->dcd_change(tty, status);
-		tty_ldisc_deref(ld);
+	lockdep_assert_held_once(&uport->lock);
+
+	if (tty) {
+		ld = tty_ldisc_ref(tty);
+		if (ld) {
+			if (ld->ops->dcd_change)
+				ld->ops->dcd_change(tty, status);
+			tty_ldisc_deref(ld);
+		}
 	}
 
 	uport->icount.dcd++;
 
-	if (port->flags & ASYNC_CHECK_CD) {
+	if (uart_dcd_enabled(uport)) {
 		if (status)
 			wake_up_interruptible(&port->open_wait);
 		else if (tty)
@@ -2785,26 +2783,25 @@ EXPORT_SYMBOL_GPL(uart_handle_dcd_change);
  *	uart_handle_cts_change - handle a change of clear-to-send state
  *	@uport: uart_port structure for the open port
  *	@status: new clear to send status, nonzero if active
+ *
+ *	Caller must hold uport->lock
  */
 void uart_handle_cts_change(struct uart_port *uport, unsigned int status)
 {
-	struct tty_port *port = &uport->state->port;
-	struct tty_struct *tty = port->tty;
+	lockdep_assert_held_once(&uport->lock);
 
 	uport->icount.cts++;
 
-	/* skip below code if the hw flow control is supported */
-	if (tty_port_cts_enabled(port) &&
-	    !(uport->flags & UPF_HARD_FLOW)) {
-		if (tty->hw_stopped) {
+	if (uart_cts_enabled(uport)) {
+		if (uport->hw_stopped) {
 			if (status) {
-				tty->hw_stopped = 0;
+				uport->hw_stopped = 0;
 				uport->ops->start_tx(uport);
 				uart_write_wakeup(uport);
 			}
 		} else {
 			if (!status) {
-				tty->hw_stopped = 1;
+				uport->hw_stopped = 1;
 				uport->ops->stop_tx(uport);
 			}
 		}
diff --git a/drivers/tty/serial/serial_mctrl_gpio.c b/drivers/tty/serial/serial_mctrl_gpio.c
index bf9560ffe3f4..a3035f997b98 100644
--- a/drivers/tty/serial/serial_mctrl_gpio.c
+++ b/drivers/tty/serial/serial_mctrl_gpio.c
@@ -18,7 +18,7 @@
 #include <linux/err.h>
 #include <linux/device.h>
 #include <linux/gpio/consumer.h>
-#include <uapi/asm-generic/termios.h>
+#include <linux/termios.h>
 
 #include "serial_mctrl_gpio.h"
 
diff --git a/drivers/tty/serial/sh-sci.c b/drivers/tty/serial/sh-sci.c
index 3081e46085ce..eb17c7124e72 100644
--- a/drivers/tty/serial/sh-sci.c
+++ b/drivers/tty/serial/sh-sci.c
@@ -1403,7 +1403,7 @@ static void work_fn_rx(struct work_struct *work)
 		unsigned long flags;
 		int count;
 
-		chan->device->device_control(chan, DMA_TERMINATE_ALL, 0);
+		dmaengine_terminate_all(chan);
 		dev_dbg(port->dev, "Read %zu bytes with cookie %d\n",
 			sh_desc->partial, sh_desc->cookie);
 
diff --git a/drivers/tty/serial/st-asc.c b/drivers/tty/serial/st-asc.c
index 8b2d7356611d..a3165842ca29 100644
--- a/drivers/tty/serial/st-asc.c
+++ b/drivers/tty/serial/st-asc.c
@@ -151,12 +151,20 @@ static inline struct asc_port *to_asc_port(struct uart_port *port)
 
 static inline u32 asc_in(struct uart_port *port, u32 offset)
 {
+#ifdef readl_relaxed
+	return readl_relaxed(port->membase + offset);
+#else
 	return readl(port->membase + offset);
+#endif
 }
 
 static inline void asc_out(struct uart_port *port, u32 offset, u32 value)
 {
+#ifdef writel_relaxed
+	writel_relaxed(value, port->membase + offset);
+#else
 	writel(value, port->membase + offset);
+#endif
 }
 
 /*
diff --git a/drivers/tty/serial/sunhv.c b/drivers/tty/serial/sunhv.c
index 20521db2189f..25d43ce8b318 100644
--- a/drivers/tty/serial/sunhv.c
+++ b/drivers/tty/serial/sunhv.c
@@ -268,6 +268,9 @@ static void sunhv_send_xchar(struct uart_port *port, char ch)
 	unsigned long flags;
 	int limit = 10000;
 
+	if (ch == __DISABLED_CHAR)
+		return;
+
 	spin_lock_irqsave(&port->lock, flags);
 
 	while (limit-- > 0) {
diff --git a/drivers/tty/serial/sunsab.c b/drivers/tty/serial/sunsab.c
index b9598b227a45..b339fe4811cd 100644
--- a/drivers/tty/serial/sunsab.c
+++ b/drivers/tty/serial/sunsab.c
@@ -436,7 +436,7 @@ static void sunsab_start_tx(struct uart_port *port)
 	struct circ_buf *xmit = &up->port.state->xmit;
 	int i;
 
-	if (uart_circ_empty(xmit))
+	if (uart_circ_empty(xmit) || uart_tx_stopped(port))
 		return;
 
 	up->interrupt_mask1 &= ~(SAB82532_IMR1_ALLS|SAB82532_IMR1_XPR);
@@ -468,6 +468,9 @@ static void sunsab_send_xchar(struct uart_port *port, char ch)
 	struct uart_sunsab_port *up = (struct uart_sunsab_port *) port;
 	unsigned long flags;
 
+	if (ch == __DISABLED_CHAR)
+		return;
+
 	spin_lock_irqsave(&up->port.lock, flags);
 
 	sunsab_tec_wait(up);
diff --git a/drivers/tty/serial/uartlite.c b/drivers/tty/serial/uartlite.c
index 9fc22f40796e..189f52e3111f 100644
--- a/drivers/tty/serial/uartlite.c
+++ b/drivers/tty/serial/uartlite.c
@@ -665,7 +665,6 @@ static struct platform_driver ulite_platform_driver = {
 	.probe = ulite_probe,
 	.remove = ulite_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name  = "uartlite",
 		.of_match_table = of_match_ptr(ulite_of_match),
 	},
diff --git a/drivers/tty/serial/vr41xx_siu.c b/drivers/tty/serial/vr41xx_siu.c
index db0c8a4ab03e..d7f9d622cdcb 100644
--- a/drivers/tty/serial/vr41xx_siu.c
+++ b/drivers/tty/serial/vr41xx_siu.c
@@ -847,7 +847,6 @@ void __init vr41xx_siu_early_setup(struct uart_port *port)
 	siu_uart_ports[port->line].type = port->type;
 	siu_uart_ports[port->line].uartclk = SIU_BAUD_BASE * 16;
 	siu_uart_ports[port->line].mapbase = port->mapbase;
-	siu_uart_ports[port->line].mapbase = port->mapbase;
 	siu_uart_ports[port->line].ops = &siu_uart_ops;
 }
 
diff --git a/drivers/tty/serial/vt8500_serial.c b/drivers/tty/serial/vt8500_serial.c
index 15ad6fcda88b..b2bc9e8ba048 100644
--- a/drivers/tty/serial/vt8500_serial.c
+++ b/drivers/tty/serial/vt8500_serial.c
@@ -33,8 +33,8 @@
 #include <linux/serial.h>
 #include <linux/slab.h>
 #include <linux/clk.h>
-#include <linux/platform_device.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/err.h>
 
 /*
@@ -78,13 +78,40 @@
 #define RX_FIFO_INTS	(RXFAF | RXFF | RXOVER | PER | FER | RXTOUT)
 #define TX_FIFO_INTS	(TXFAE | TXFE | TXUDR)
 
+/*
+ * Line control bits
+ */
+
+#define VT8500_TXEN	(1 << 0)	/* Enable transmit logic */
+#define VT8500_RXEN	(1 << 1)	/* Enable receive logic */
+#define VT8500_CS8	(1 << 2)	/* 8-bit data length (vs. 7-bit) */
+#define VT8500_CSTOPB	(1 << 3)	/* 2 stop bits (vs. 1) */
+#define VT8500_PARENB	(1 << 4)	/* Enable parity */
+#define VT8500_PARODD	(1 << 5)	/* Odd parity (vs. even) */
+#define VT8500_RTS	(1 << 6)	/* Ready to send */
+#define VT8500_LOOPBK	(1 << 7)	/* Enable internal loopback */
+#define VT8500_DMA	(1 << 8)	/* Enable DMA mode (needs FIFO) */
+#define VT8500_BREAK	(1 << 9)	/* Initiate break signal */
+#define VT8500_PSLVERR	(1 << 10)	/* APB error upon empty RX FIFO read */
+#define VT8500_SWRTSCTS	(1 << 11)	/* Software-controlled RTS/CTS */
+
+/*
+ * Capability flags (driver-internal)
+ */
+
+#define VT8500_HAS_SWRTSCTS_SWITCH	(1 << 1)
+
+#define VT8500_RECOMMENDED_CLK		12000000
+#define VT8500_OVERSAMPLING_DIVISOR	13
 #define VT8500_MAX_PORTS	6
 
 struct vt8500_port {
 	struct uart_port	uart;
 	char			name[16];
 	struct clk		*clk;
+	unsigned int		clk_predivisor;
 	unsigned int		ier;
+	unsigned int		vt8500_uart_flags;
 };
 
 /*
@@ -267,31 +294,45 @@ static unsigned int vt8500_get_mctrl(struct uart_port *port)
 
 static void vt8500_set_mctrl(struct uart_port *port, unsigned int mctrl)
 {
+	unsigned int lcr = vt8500_read(port, VT8500_URLCR);
+
+	if (mctrl & TIOCM_RTS)
+		lcr |= VT8500_RTS;
+	else
+		lcr &= ~VT8500_RTS;
+
+	vt8500_write(port, lcr, VT8500_URLCR);
 }
 
 static void vt8500_break_ctl(struct uart_port *port, int break_ctl)
 {
 	if (break_ctl)
-		vt8500_write(port, vt8500_read(port, VT8500_URLCR) | (1 << 9),
+		vt8500_write(port,
+			     vt8500_read(port, VT8500_URLCR) | VT8500_BREAK,
 			     VT8500_URLCR);
 }
 
 static int vt8500_set_baud_rate(struct uart_port *port, unsigned int baud)
 {
+	struct vt8500_port *vt8500_port =
+			container_of(port, struct vt8500_port, uart);
 	unsigned long div;
 	unsigned int loops = 1000;
 
-	div = vt8500_read(port, VT8500_URDIV) & ~(0x3ff);
+	div = ((vt8500_port->clk_predivisor - 1) & 0xf) << 16;
+	div |= (uart_get_divisor(port, baud) - 1) & 0x3ff;
 
-	if (unlikely((baud < 900) || (baud > 921600)))
-		div |= 7;
-	else
-		div |= (921600 / baud) - 1;
+	/* Effective baud rate */
+	baud = port->uartclk / 16 / ((div & 0x3ff) + 1);
 
 	while ((vt8500_read(port, VT8500_URUSR) & (1 << 5)) && --loops)
 		cpu_relax();
+
 	vt8500_write(port, div, VT8500_URDIV);
 
+	/* Break signal timing depends on baud rate, update accordingly */
+	vt8500_write(port, mult_frac(baud, 4096, 1000000), VT8500_URBKR);
+
 	return baud;
 }
 
@@ -347,31 +388,35 @@ static void vt8500_set_termios(struct uart_port *port,
 
 	/* calculate parity */
 	lcr = vt8500_read(&vt8500_port->uart, VT8500_URLCR);
-	lcr &= ~((1 << 5) | (1 << 4));
+	lcr &= ~(VT8500_PARENB | VT8500_PARODD);
 	if (termios->c_cflag & PARENB) {
-		lcr |= (1 << 4);
+		lcr |= VT8500_PARENB;
 		termios->c_cflag &= ~CMSPAR;
 		if (termios->c_cflag & PARODD)
-			lcr |= (1 << 5);
+			lcr |= VT8500_PARODD;
 	}
 
 	/* calculate bits per char */
-	lcr &= ~(1 << 2);
+	lcr &= ~VT8500_CS8;
 	switch (termios->c_cflag & CSIZE) {
 	case CS7:
 		break;
 	case CS8:
 	default:
-		lcr |= (1 << 2);
+		lcr |= VT8500_CS8;
 		termios->c_cflag &= ~CSIZE;
 		termios->c_cflag |= CS8;
 		break;
 	}
 
 	/* calculate stop bits */
-	lcr &= ~(1 << 3);
+	lcr &= ~VT8500_CSTOPB;
 	if (termios->c_cflag & CSTOPB)
-		lcr |= (1 << 3);
+		lcr |= VT8500_CSTOPB;
+
+	lcr &= ~VT8500_SWRTSCTS;
+	if (vt8500_port->vt8500_uart_flags & VT8500_HAS_SWRTSCTS_SWITCH)
+		lcr |= VT8500_SWRTSCTS;
 
 	/* set parity, bits per char, and stop bit */
 	vt8500_write(&vt8500_port->uart, lcr, VT8500_URLCR);
@@ -521,6 +566,33 @@ static struct console vt8500_console = {
 #define VT8500_CONSOLE	NULL
 #endif
 
+#ifdef CONFIG_CONSOLE_POLL
+static int vt8500_get_poll_char(struct uart_port *port)
+{
+	unsigned int status = vt8500_read(port, VT8500_URFIDX);
+
+	if (!(status & 0x1f00))
+		return NO_POLL_CHAR;
+
+	return vt8500_read(port, VT8500_RXFIFO) & 0xff;
+}
+
+static void vt8500_put_poll_char(struct uart_port *port, unsigned char c)
+{
+	unsigned int status, tmout = 10000;
+
+	do {
+		status = vt8500_read(port, VT8500_URFIDX);
+
+		if (--tmout == 0)
+			break;
+		udelay(1);
+	} while (status & 0x10);
+
+	vt8500_write(port, c, VT8500_TXFIFO);
+}
+#endif
+
 static struct uart_ops vt8500_uart_pops = {
 	.tx_empty	= vt8500_tx_empty,
 	.set_mctrl	= vt8500_set_mctrl,
@@ -538,6 +610,10 @@ static struct uart_ops vt8500_uart_pops = {
 	.request_port	= vt8500_request_port,
 	.config_port	= vt8500_config_port,
 	.verify_port	= vt8500_verify_port,
+#ifdef CONFIG_CONSOLE_POLL
+	.poll_get_char	= vt8500_get_poll_char,
+	.poll_put_char	= vt8500_put_poll_char,
+#endif
 };
 
 static struct uart_driver vt8500_uart_driver = {
@@ -548,14 +624,31 @@ static struct uart_driver vt8500_uart_driver = {
 	.cons		= VT8500_CONSOLE,
 };
 
+static unsigned int vt8500_flags; /* none required so far */
+static unsigned int wm8880_flags = VT8500_HAS_SWRTSCTS_SWITCH;
+
+static const struct of_device_id wmt_dt_ids[] = {
+	{ .compatible = "via,vt8500-uart", .data = &vt8500_flags},
+	{ .compatible = "wm,wm8880-uart", .data = &wm8880_flags},
+	{}
+};
+
 static int vt8500_serial_probe(struct platform_device *pdev)
 {
 	struct vt8500_port *vt8500_port;
 	struct resource *mmres, *irqres;
 	struct device_node *np = pdev->dev.of_node;
+	const struct of_device_id *match;
+	const unsigned int *flags;
 	int ret;
 	int port;
 
+	match = of_match_device(wmt_dt_ids, &pdev->dev);
+	if (!match)
+		return -EINVAL;
+
+	flags = match->data;
+
 	mmres = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	irqres = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	if (!mmres || !irqres)
@@ -605,6 +698,11 @@ static int vt8500_serial_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	vt8500_port->vt8500_uart_flags = *flags;
+	vt8500_port->clk_predivisor = DIV_ROUND_CLOSEST(
+					clk_get_rate(vt8500_port->clk),
+					VT8500_RECOMMENDED_CLK
+				      );
 	vt8500_port->uart.type = PORT_VT8500;
 	vt8500_port->uart.iotype = UPIO_MEM;
 	vt8500_port->uart.mapbase = mmres->start;
@@ -615,7 +713,10 @@ static int vt8500_serial_probe(struct platform_device *pdev)
 	vt8500_port->uart.dev = &pdev->dev;
 	vt8500_port->uart.flags = UPF_IOREMAP | UPF_BOOT_AUTOCONF;
 
-	vt8500_port->uart.uartclk = clk_get_rate(vt8500_port->clk);
+	/* Serial core uses the magic "16" everywhere - adjust for it */
+	vt8500_port->uart.uartclk = 16 * clk_get_rate(vt8500_port->clk) /
+					vt8500_port->clk_predivisor /
+					VT8500_OVERSAMPLING_DIVISOR;
 
 	snprintf(vt8500_port->name, sizeof(vt8500_port->name),
 		 "VT8500 UART%d", pdev->id);
@@ -639,11 +740,6 @@ static int vt8500_serial_remove(struct platform_device *pdev)
 	return 0;
 }
 
-static const struct of_device_id wmt_dt_ids[] = {
-	{ .compatible = "via,vt8500-uart", },
-	{}
-};
-
 static struct platform_driver vt8500_platform_driver = {
 	.probe  = vt8500_serial_probe,
 	.remove = vt8500_serial_remove,
diff --git a/drivers/tty/serial/xilinx_uartps.c b/drivers/tty/serial/xilinx_uartps.c
index 01951d27cc03..200c1af2141b 100644
--- a/drivers/tty/serial/xilinx_uartps.c
+++ b/drivers/tty/serial/xilinx_uartps.c
@@ -581,7 +581,7 @@ static unsigned int cdns_uart_tx_empty(struct uart_port *port)
 {
 	unsigned int status;
 
-	status = cdns_uart_readl(CDNS_UART_ISR_OFFSET) & CDNS_UART_IXR_TXEMPTY;
+	status = cdns_uart_readl(CDNS_UART_SR_OFFSET) & CDNS_UART_SR_TXEMPTY;
 	return status ? TIOCSER_TEMT : 0;
 }
 
@@ -1051,6 +1051,25 @@ static void cdns_uart_console_putchar(struct uart_port *port, int ch)
 	cdns_uart_writel(ch, CDNS_UART_FIFO_OFFSET);
 }
 
+static void cdns_early_write(struct console *con, const char *s, unsigned n)
+{
+	struct earlycon_device *dev = con->data;
+
+	uart_console_write(&dev->port, s, n, cdns_uart_console_putchar);
+}
+
+static int __init cdns_early_console_setup(struct earlycon_device *device,
+					   const char *opt)
+{
+	if (!device->port.membase)
+		return -ENODEV;
+
+	device->con->write = cdns_early_write;
+
+	return 0;
+}
+EARLYCON_DECLARE(cdns, cdns_early_console_setup);
+
 /**
  * cdns_uart_console_write - perform write operation
  * @co: Console handle
@@ -1428,7 +1447,6 @@ static struct platform_driver cdns_uart_platform_driver = {
 	.probe   = cdns_uart_probe,
 	.remove  = cdns_uart_remove,
 	.driver  = {
-		.owner = THIS_MODULE,
 		.name = CDNS_UART_NAME,
 		.of_match_table = cdns_uart_of_match,
 		.pm = &cdns_uart_dev_pm_ops,
diff --git a/drivers/tty/tty_io.c b/drivers/tty/tty_io.c
index 8fbad3410c75..16a2c0237dd6 100644
--- a/drivers/tty/tty_io.c
+++ b/drivers/tty/tty_io.c
@@ -908,8 +908,7 @@ void no_tty(void)
  *	stop_tty	-	propagate flow control
  *	@tty: tty to stop
  *
- *	Perform flow control to the driver. For PTY/TTY pairs we
- *	must also propagate the TIOCKPKT status. May be called
+ *	Perform flow control to the driver. May be called
  *	on an already stopped device and will not re-call the driver
  *	method.
  *
@@ -919,64 +918,58 @@ void no_tty(void)
  *	but not always.
  *
  *	Locking:
- *		Uses the tty control lock internally
+ *		flow_lock
  */
 
-void stop_tty(struct tty_struct *tty)
+void __stop_tty(struct tty_struct *tty)
 {
-	unsigned long flags;
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
-	if (tty->stopped) {
-		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+	if (tty->stopped)
 		return;
-	}
 	tty->stopped = 1;
-	if (tty->link && tty->link->packet) {
-		tty->ctrl_status &= ~TIOCPKT_START;
-		tty->ctrl_status |= TIOCPKT_STOP;
-		wake_up_interruptible_poll(&tty->link->read_wait, POLLIN);
-	}
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 	if (tty->ops->stop)
 		(tty->ops->stop)(tty);
 }
 
+void stop_tty(struct tty_struct *tty)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&tty->flow_lock, flags);
+	__stop_tty(tty);
+	spin_unlock_irqrestore(&tty->flow_lock, flags);
+}
 EXPORT_SYMBOL(stop_tty);
 
 /**
  *	start_tty	-	propagate flow control
  *	@tty: tty to start
  *
- *	Start a tty that has been stopped if at all possible. Perform
- *	any necessary wakeups and propagate the TIOCPKT status. If this
- *	is the tty was previous stopped and is being started then the
- *	driver start method is invoked and the line discipline woken.
+ *	Start a tty that has been stopped if at all possible. If this
+ *	tty was previous stopped and is now being started, the driver
+ *	start method is invoked and the line discipline woken.
  *
  *	Locking:
- *		ctrl_lock
+ *		flow_lock
  */
 
-void start_tty(struct tty_struct *tty)
+void __start_tty(struct tty_struct *tty)
 {
-	unsigned long flags;
-	spin_lock_irqsave(&tty->ctrl_lock, flags);
-	if (!tty->stopped || tty->flow_stopped) {
-		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
+	if (!tty->stopped || tty->flow_stopped)
 		return;
-	}
 	tty->stopped = 0;
-	if (tty->link && tty->link->packet) {
-		tty->ctrl_status &= ~TIOCPKT_STOP;
-		tty->ctrl_status |= TIOCPKT_START;
-		wake_up_interruptible_poll(&tty->link->read_wait, POLLIN);
-	}
-	spin_unlock_irqrestore(&tty->ctrl_lock, flags);
 	if (tty->ops->start)
 		(tty->ops->start)(tty);
-	/* If we have a running line discipline it may need kicking */
 	tty_wakeup(tty);
 }
 
+void start_tty(struct tty_struct *tty)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&tty->flow_lock, flags);
+	__start_tty(tty);
+	spin_unlock_irqrestore(&tty->flow_lock, flags);
+}
 EXPORT_SYMBOL(start_tty);
 
 /* We limit tty time update visibility to every 8 seconds or so. */
@@ -1030,14 +1023,14 @@ static ssize_t tty_read(struct file *file, char __user *buf, size_t count,
 	return i;
 }
 
-void tty_write_unlock(struct tty_struct *tty)
+static void tty_write_unlock(struct tty_struct *tty)
 	__releases(&tty->atomic_write_lock)
 {
 	mutex_unlock(&tty->atomic_write_lock);
 	wake_up_interruptible_poll(&tty->write_wait, POLLOUT);
 }
 
-int tty_write_lock(struct tty_struct *tty, int ndelay)
+static int tty_write_lock(struct tty_struct *tty, int ndelay)
 	__acquires(&tty->atomic_write_lock)
 {
 	if (!mutex_trylock(&tty->atomic_write_lock)) {
@@ -1224,6 +1217,35 @@ ssize_t redirected_tty_write(struct file *file, const char __user *buf,
 	return tty_write(file, buf, count, ppos);
 }
 
+/**
+ *	tty_send_xchar	-	send priority character
+ *
+ *	Send a high priority character to the tty even if stopped
+ *
+ *	Locking: none for xchar method, write ordering for write method.
+ */
+
+int tty_send_xchar(struct tty_struct *tty, char ch)
+{
+	int	was_stopped = tty->stopped;
+
+	if (tty->ops->send_xchar) {
+		tty->ops->send_xchar(tty, ch);
+		return 0;
+	}
+
+	if (tty_write_lock(tty, 0) < 0)
+		return -ERESTARTSYS;
+
+	if (was_stopped)
+		start_tty(tty);
+	tty->ops->write(tty, &ch, 1);
+	if (was_stopped)
+		stop_tty(tty);
+	tty_write_unlock(tty);
+	return 0;
+}
+
 static char ptychar[] = "pqrstuvwxyzabcde";
 
 /**
@@ -1544,13 +1566,14 @@ static void release_one_tty(struct work_struct *work)
 	struct tty_struct *tty =
 		container_of(work, struct tty_struct, hangup_work);
 	struct tty_driver *driver = tty->driver;
+	struct module *owner = driver->owner;
 
 	if (tty->ops->cleanup)
 		tty->ops->cleanup(tty);
 
 	tty->magic = 0;
 	tty_driver_kref_put(driver);
-	module_put(driver->owner);
+	module_put(owner);
 
 	spin_lock(&tty_files_lock);
 	list_del_init(&tty->tty_files);
@@ -2163,8 +2186,9 @@ static int __tty_fasync(int fd, struct file *filp, int on)
 		}
 		get_pid(pid);
 		spin_unlock_irqrestore(&tty->ctrl_lock, flags);
-		retval = __f_setown(filp, pid, type, 0);
+		__f_setown(filp, pid, type, 0);
 		put_pid(pid);
+		retval = 0;
 	}
 out:
 	return retval;
@@ -3018,6 +3042,7 @@ struct tty_struct *alloc_tty_struct(struct tty_driver *driver, int idx)
 	INIT_WORK(&tty->hangup_work, do_tty_hangup);
 	mutex_init(&tty->atomic_write_lock);
 	spin_lock_init(&tty->ctrl_lock);
+	spin_lock_init(&tty->flow_lock);
 	INIT_LIST_HEAD(&tty->tty_files);
 	INIT_WORK(&tty->SAK_work, do_SAK_work);
 
diff --git a/drivers/tty/tty_ioctl.c b/drivers/tty/tty_ioctl.c
index 6fd60fece6b4..62380ccf70fb 100644
--- a/drivers/tty/tty_ioctl.c
+++ b/drivers/tty/tty_ioctl.c
@@ -402,7 +402,7 @@ void tty_termios_encode_baud_rate(struct ktermios *termios,
 
 #ifdef BOTHER
 	/* If the user asked for a precise weird speed give a precise weird
-	   answer. If they asked for a Bfoo speed they many have problems
+	   answer. If they asked for a Bfoo speed they may have problems
 	   digesting non-exact replies so fuzz a bit */
 
 	if ((termios->c_cflag & CBAUD) == BOTHER)
@@ -912,35 +912,6 @@ static int set_ltchars(struct tty_struct *tty, struct ltchars __user *ltchars)
 #endif
 
 /**
- *	send_prio_char		-	send priority character
- *
- *	Send a high priority character to the tty even if stopped
- *
- *	Locking: none for xchar method, write ordering for write method.
- */
-
-static int send_prio_char(struct tty_struct *tty, char ch)
-{
-	int	was_stopped = tty->stopped;
-
-	if (tty->ops->send_xchar) {
-		tty->ops->send_xchar(tty, ch);
-		return 0;
-	}
-
-	if (tty_write_lock(tty, 0) < 0)
-		return -ERESTARTSYS;
-
-	if (was_stopped)
-		start_tty(tty);
-	tty->ops->write(tty, &ch, 1);
-	if (was_stopped)
-		stop_tty(tty);
-	tty_write_unlock(tty);
-	return 0;
-}
-
-/**
  *	tty_change_softcar	-	carrier change ioctl helper
  *	@tty: tty to update
  *	@arg: enable/disable CLOCAL
@@ -1177,29 +1148,37 @@ int n_tty_ioctl_helper(struct tty_struct *tty, struct file *file,
 			return retval;
 		switch (arg) {
 		case TCOOFF:
+			spin_lock_irq(&tty->flow_lock);
 			if (!tty->flow_stopped) {
 				tty->flow_stopped = 1;
-				stop_tty(tty);
+				__stop_tty(tty);
 			}
+			spin_unlock_irq(&tty->flow_lock);
 			break;
 		case TCOON:
+			spin_lock_irq(&tty->flow_lock);
 			if (tty->flow_stopped) {
 				tty->flow_stopped = 0;
-				start_tty(tty);
+				__start_tty(tty);
 			}
+			spin_unlock_irq(&tty->flow_lock);
 			break;
 		case TCIOFF:
+			down_read(&tty->termios_rwsem);
 			if (STOP_CHAR(tty) != __DISABLED_CHAR)
-				return send_prio_char(tty, STOP_CHAR(tty));
+				retval = tty_send_xchar(tty, STOP_CHAR(tty));
+			up_read(&tty->termios_rwsem);
 			break;
 		case TCION:
+			down_read(&tty->termios_rwsem);
 			if (START_CHAR(tty) != __DISABLED_CHAR)
-				return send_prio_char(tty, START_CHAR(tty));
+				retval = tty_send_xchar(tty, START_CHAR(tty));
+			up_read(&tty->termios_rwsem);
 			break;
 		default:
 			return -EINVAL;
 		}
-		return 0;
+		return retval;
 	case TCFLSH:
 		retval = tty_check_change(tty);
 		if (retval)
diff --git a/drivers/tty/vt/keyboard.c b/drivers/tty/vt/keyboard.c
index d0e3a4497707..c039cfea5b11 100644
--- a/drivers/tty/vt/keyboard.c
+++ b/drivers/tty/vt/keyboard.c
@@ -268,30 +268,30 @@ EXPORT_SYMBOL(kd_mksound);
 static int kbd_rate_helper(struct input_handle *handle, void *data)
 {
 	struct input_dev *dev = handle->dev;
-	struct kbd_repeat *rep = data;
+	struct kbd_repeat *rpt = data;
 
 	if (test_bit(EV_REP, dev->evbit)) {
 
-		if (rep[0].delay > 0)
+		if (rpt[0].delay > 0)
 			input_inject_event(handle,
-					   EV_REP, REP_DELAY, rep[0].delay);
-		if (rep[0].period > 0)
+					   EV_REP, REP_DELAY, rpt[0].delay);
+		if (rpt[0].period > 0)
 			input_inject_event(handle,
-					   EV_REP, REP_PERIOD, rep[0].period);
+					   EV_REP, REP_PERIOD, rpt[0].period);
 
-		rep[1].delay = dev->rep[REP_DELAY];
-		rep[1].period = dev->rep[REP_PERIOD];
+		rpt[1].delay = dev->rep[REP_DELAY];
+		rpt[1].period = dev->rep[REP_PERIOD];
 	}
 
 	return 0;
 }
 
-int kbd_rate(struct kbd_repeat *rep)
+int kbd_rate(struct kbd_repeat *rpt)
 {
-	struct kbd_repeat data[2] = { *rep };
+	struct kbd_repeat data[2] = { *rpt };
 
 	input_handler_for_each_handle(&kbd_handler, data, kbd_rate_helper);
-	*rep = data[1];	/* Copy currently used settings */
+	*rpt = data[1];	/* Copy currently used settings */
 
 	return 0;
 }
@@ -971,15 +971,15 @@ static unsigned char getledstate(void)
 	return ledstate;
 }
 
-void setledstate(struct kbd_struct *kbd, unsigned int led)
+void setledstate(struct kbd_struct *kb, unsigned int led)
 {
         unsigned long flags;
         spin_lock_irqsave(&led_lock, flags);
 	if (!(led & ~7)) {
 		ledioctl = led;
-		kbd->ledmode = LED_SHOW_IOCTL;
+		kb->ledmode = LED_SHOW_IOCTL;
 	} else
-		kbd->ledmode = LED_SHOW_FLAGS;
+		kb->ledmode = LED_SHOW_FLAGS;
 
 	set_leds();
 	spin_unlock_irqrestore(&led_lock, flags);
@@ -987,12 +987,12 @@ void setledstate(struct kbd_struct *kbd, unsigned int led)
 
 static inline unsigned char getleds(void)
 {
-	struct kbd_struct *kbd = kbd_table + fg_console;
+	struct kbd_struct *kb = kbd_table + fg_console;
 
-	if (kbd->ledmode == LED_SHOW_IOCTL)
+	if (kb->ledmode == LED_SHOW_IOCTL)
 		return ledioctl;
 
-	return kbd->ledflagstate;
+	return kb->ledflagstate;
 }
 
 static int kbd_update_leds_helper(struct input_handle *handle, void *data)
@@ -1018,12 +1018,12 @@ static int kbd_update_leds_helper(struct input_handle *handle, void *data)
  */
 int vt_get_leds(int console, int flag)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	int ret;
 	unsigned long flags;
 
 	spin_lock_irqsave(&led_lock, flags);
-	ret = vc_kbd_led(kbd, flag);
+	ret = vc_kbd_led(kb, flag);
 	spin_unlock_irqrestore(&led_lock, flags);
 
 	return ret;
@@ -1040,8 +1040,8 @@ EXPORT_SYMBOL_GPL(vt_get_leds);
  */
 void vt_set_led_state(int console, int leds)
 {
-	struct kbd_struct * kbd = kbd_table + console;
-	setledstate(kbd, leds);
+	struct kbd_struct *kb = kbd_table + console;
+	setledstate(kb, leds);
 }
 
 /**
@@ -1059,10 +1059,10 @@ void vt_set_led_state(int console, int leds)
  */
 void vt_kbd_con_start(int console)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	unsigned long flags;
 	spin_lock_irqsave(&led_lock, flags);
-	clr_vc_kbd_led(kbd, VC_SCROLLOCK);
+	clr_vc_kbd_led(kb, VC_SCROLLOCK);
 	set_leds();
 	spin_unlock_irqrestore(&led_lock, flags);
 }
@@ -1076,10 +1076,10 @@ void vt_kbd_con_start(int console)
  */
 void vt_kbd_con_stop(int console)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	unsigned long flags;
 	spin_lock_irqsave(&led_lock, flags);
-	set_vc_kbd_led(kbd, VC_SCROLLOCK);
+	set_vc_kbd_led(kb, VC_SCROLLOCK);
 	set_leds();
 	spin_unlock_irqrestore(&led_lock, flags);
 }
@@ -1512,15 +1512,14 @@ int __init kbd_init(void)
 /**
  *	vt_do_diacrit		-	diacritical table updates
  *	@cmd: ioctl request
- *	@up: pointer to user data for ioctl
+ *	@udp: pointer to user data for ioctl
  *	@perm: permissions check computed by caller
  *
  *	Update the diacritical tables atomically and safely. Lock them
  *	against simultaneous keypresses
  */
-int vt_do_diacrit(unsigned int cmd, void __user *up, int perm)
+int vt_do_diacrit(unsigned int cmd, void __user *udp, int perm)
 {
-	struct kbdiacrs __user *a = up;
 	unsigned long flags;
 	int asize;
 	int ret = 0;
@@ -1528,12 +1527,13 @@ int vt_do_diacrit(unsigned int cmd, void __user *up, int perm)
 	switch (cmd) {
 	case KDGKBDIACR:
 	{
-		struct kbdiacr *diacr;
+		struct kbdiacrs __user *a = udp;
+		struct kbdiacr *dia;
 		int i;
 
-		diacr = kmalloc(MAX_DIACR * sizeof(struct kbdiacr),
+		dia = kmalloc(MAX_DIACR * sizeof(struct kbdiacr),
 								GFP_KERNEL);
-		if (diacr == NULL)
+		if (!dia)
 			return -ENOMEM;
 
 		/* Lock the diacriticals table, make a copy and then
@@ -1542,26 +1542,26 @@ int vt_do_diacrit(unsigned int cmd, void __user *up, int perm)
 
 		asize = accent_table_size;
 		for (i = 0; i < asize; i++) {
-			diacr[i].diacr = conv_uni_to_8bit(
+			dia[i].diacr = conv_uni_to_8bit(
 						accent_table[i].diacr);
-			diacr[i].base = conv_uni_to_8bit(
+			dia[i].base = conv_uni_to_8bit(
 						accent_table[i].base);
-			diacr[i].result = conv_uni_to_8bit(
+			dia[i].result = conv_uni_to_8bit(
 						accent_table[i].result);
 		}
 		spin_unlock_irqrestore(&kbd_event_lock, flags);
 
 		if (put_user(asize, &a->kb_cnt))
 			ret = -EFAULT;
-		else  if (copy_to_user(a->kbdiacr, diacr,
+		else  if (copy_to_user(a->kbdiacr, dia,
 				asize * sizeof(struct kbdiacr)))
 			ret = -EFAULT;
-		kfree(diacr);
+		kfree(dia);
 		return ret;
 	}
 	case KDGKBDIACRUC:
 	{
-		struct kbdiacrsuc __user *a = up;
+		struct kbdiacrsuc __user *a = udp;
 		void *buf;
 
 		buf = kmalloc(MAX_DIACR * sizeof(struct kbdiacruc),
@@ -1589,8 +1589,8 @@ int vt_do_diacrit(unsigned int cmd, void __user *up, int perm)
 
 	case KDSKBDIACR:
 	{
-		struct kbdiacrs __user *a = up;
-		struct kbdiacr *diacr = NULL;
+		struct kbdiacrs __user *a = udp;
+		struct kbdiacr *dia = NULL;
 		unsigned int ct;
 		int i;
 
@@ -1602,14 +1602,14 @@ int vt_do_diacrit(unsigned int cmd, void __user *up, int perm)
 			return -EINVAL;
 
 		if (ct) {
-			diacr = kmalloc(sizeof(struct kbdiacr) * ct,
+			dia = kmalloc(sizeof(struct kbdiacr) * ct,
 								GFP_KERNEL);
-			if (diacr == NULL)
+			if (!dia)
 				return -ENOMEM;
 
-			if (copy_from_user(diacr, a->kbdiacr,
+			if (copy_from_user(dia, a->kbdiacr,
 					sizeof(struct kbdiacr) * ct)) {
-				kfree(diacr);
+				kfree(dia);
 				return -EFAULT;
 			}
 		}
@@ -1618,20 +1618,20 @@ int vt_do_diacrit(unsigned int cmd, void __user *up, int perm)
 		accent_table_size = ct;
 		for (i = 0; i < ct; i++) {
 			accent_table[i].diacr =
-					conv_8bit_to_uni(diacr[i].diacr);
+					conv_8bit_to_uni(dia[i].diacr);
 			accent_table[i].base =
-					conv_8bit_to_uni(diacr[i].base);
+					conv_8bit_to_uni(dia[i].base);
 			accent_table[i].result =
-					conv_8bit_to_uni(diacr[i].result);
+					conv_8bit_to_uni(dia[i].result);
 		}
 		spin_unlock_irqrestore(&kbd_event_lock, flags);
-		kfree(diacr);
+		kfree(dia);
 		return 0;
 	}
 
 	case KDSKBDIACRUC:
 	{
-		struct kbdiacrsuc __user *a = up;
+		struct kbdiacrsuc __user *a = udp;
 		unsigned int ct;
 		void *buf = NULL;
 
@@ -1679,28 +1679,28 @@ int vt_do_diacrit(unsigned int cmd, void __user *up, int perm)
  */
 int vt_do_kdskbmode(int console, unsigned int arg)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	int ret = 0;
 	unsigned long flags;
 
 	spin_lock_irqsave(&kbd_event_lock, flags);
 	switch(arg) {
 	case K_RAW:
-		kbd->kbdmode = VC_RAW;
+		kb->kbdmode = VC_RAW;
 		break;
 	case K_MEDIUMRAW:
-		kbd->kbdmode = VC_MEDIUMRAW;
+		kb->kbdmode = VC_MEDIUMRAW;
 		break;
 	case K_XLATE:
-		kbd->kbdmode = VC_XLATE;
+		kb->kbdmode = VC_XLATE;
 		do_compute_shiftstate();
 		break;
 	case K_UNICODE:
-		kbd->kbdmode = VC_UNICODE;
+		kb->kbdmode = VC_UNICODE;
 		do_compute_shiftstate();
 		break;
 	case K_OFF:
-		kbd->kbdmode = VC_OFF;
+		kb->kbdmode = VC_OFF;
 		break;
 	default:
 		ret = -EINVAL;
@@ -1719,17 +1719,17 @@ int vt_do_kdskbmode(int console, unsigned int arg)
  */
 int vt_do_kdskbmeta(int console, unsigned int arg)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	int ret = 0;
 	unsigned long flags;
 
 	spin_lock_irqsave(&kbd_event_lock, flags);
 	switch(arg) {
 	case K_METABIT:
-		clr_vc_kbd_mode(kbd, VC_META);
+		clr_vc_kbd_mode(kb, VC_META);
 		break;
 	case K_ESCPREFIX:
-		set_vc_kbd_mode(kbd, VC_META);
+		set_vc_kbd_mode(kb, VC_META);
 		break;
 	default:
 		ret = -EINVAL;
@@ -1768,7 +1768,7 @@ int vt_do_kbkeycode_ioctl(int cmd, struct kbkeycode __user *user_kbkc,
 int vt_do_kdsk_ioctl(int cmd, struct kbentry __user *user_kbe, int perm,
 						int console)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	struct kbentry tmp;
 	ushort *key_map, *new_map, val, ov;
 	unsigned long flags;
@@ -1786,7 +1786,7 @@ int vt_do_kdsk_ioctl(int cmd, struct kbentry __user *user_kbe, int perm,
 		key_map = key_maps[s];
 		if (key_map) {
 		    val = U(key_map[i]);
-		    if (kbd->kbdmode != VC_UNICODE && KTYP(val) >= NR_TYPES)
+		    if (kb->kbdmode != VC_UNICODE && KTYP(val) >= NR_TYPES)
 			val = K_HOLE;
 		} else
 		    val = (i ? K_HOLE : K_NOSUCHMAP);
@@ -1814,7 +1814,7 @@ int vt_do_kdsk_ioctl(int cmd, struct kbentry __user *user_kbe, int perm,
 		    if (KVAL(v) > max_vals[KTYP(v)])
 				return -EINVAL;
 		} else
-		    if (kbd->kbdmode != VC_UNICODE)
+		    if (kb->kbdmode != VC_UNICODE)
 				return -EINVAL;
 
 		/* ++Geert: non-PC keyboards may generate keycode zero */
@@ -1985,7 +1985,7 @@ reterr:
 
 int vt_do_kdskled(int console, int cmd, unsigned long arg, int perm)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
         unsigned long flags;
 	unsigned char ucval;
 
@@ -1994,7 +1994,7 @@ int vt_do_kdskled(int console, int cmd, unsigned long arg, int perm)
 	/* don't use them - they will go away without warning */
 	case KDGKBLED:
                 spin_lock_irqsave(&kbd_event_lock, flags);
-		ucval = kbd->ledflagstate | (kbd->default_ledflagstate << 4);
+		ucval = kb->ledflagstate | (kb->default_ledflagstate << 4);
                 spin_unlock_irqrestore(&kbd_event_lock, flags);
 		return put_user(ucval, (char __user *)arg);
 
@@ -2004,8 +2004,8 @@ int vt_do_kdskled(int console, int cmd, unsigned long arg, int perm)
 		if (arg & ~0x77)
 			return -EINVAL;
                 spin_lock_irqsave(&led_lock, flags);
-		kbd->ledflagstate = (arg & 7);
-		kbd->default_ledflagstate = ((arg >> 4) & 7);
+		kb->ledflagstate = (arg & 7);
+		kb->default_ledflagstate = ((arg >> 4) & 7);
 		set_leds();
                 spin_unlock_irqrestore(&led_lock, flags);
 		return 0;
@@ -2019,7 +2019,7 @@ int vt_do_kdskled(int console, int cmd, unsigned long arg, int perm)
 	case KDSETLED:
 		if (!perm)
 			return -EPERM;
-		setledstate(kbd, arg);
+		setledstate(kb, arg);
 		return 0;
         }
         return -ENOIOCTLCMD;
@@ -2027,9 +2027,9 @@ int vt_do_kdskled(int console, int cmd, unsigned long arg, int perm)
 
 int vt_do_kdgkbmode(int console)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	/* This is a spot read so needs no locking */
-	switch (kbd->kbdmode) {
+	switch (kb->kbdmode) {
 	case VC_RAW:
 		return K_RAW;
 	case VC_MEDIUMRAW:
@@ -2051,9 +2051,9 @@ int vt_do_kdgkbmode(int console)
  */
 int vt_do_kdgkbmeta(int console)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
         /* Again a spot read so no locking */
-	return vc_kbd_mode(kbd, VC_META) ? K_ESCPREFIX : K_METABIT;
+	return vc_kbd_mode(kb, VC_META) ? K_ESCPREFIX : K_METABIT;
 }
 
 /**
@@ -2092,19 +2092,19 @@ int vt_get_shift_state(void)
  */
 void vt_reset_keyboard(int console)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	unsigned long flags;
 
 	spin_lock_irqsave(&kbd_event_lock, flags);
-	set_vc_kbd_mode(kbd, VC_REPEAT);
-	clr_vc_kbd_mode(kbd, VC_CKMODE);
-	clr_vc_kbd_mode(kbd, VC_APPLIC);
-	clr_vc_kbd_mode(kbd, VC_CRLF);
-	kbd->lockstate = 0;
-	kbd->slockstate = 0;
+	set_vc_kbd_mode(kb, VC_REPEAT);
+	clr_vc_kbd_mode(kb, VC_CKMODE);
+	clr_vc_kbd_mode(kb, VC_APPLIC);
+	clr_vc_kbd_mode(kb, VC_CRLF);
+	kb->lockstate = 0;
+	kb->slockstate = 0;
 	spin_lock(&led_lock);
-	kbd->ledmode = LED_SHOW_FLAGS;
-	kbd->ledflagstate = kbd->default_ledflagstate;
+	kb->ledmode = LED_SHOW_FLAGS;
+	kb->ledflagstate = kb->default_ledflagstate;
 	spin_unlock(&led_lock);
 	/* do not do set_leds here because this causes an endless tasklet loop
 	   when the keyboard hasn't been initialized yet */
@@ -2122,8 +2122,8 @@ void vt_reset_keyboard(int console)
 
 int vt_get_kbd_mode_bit(int console, int bit)
 {
-	struct kbd_struct * kbd = kbd_table + console;
-	return vc_kbd_mode(kbd, bit);
+	struct kbd_struct *kb = kbd_table + console;
+	return vc_kbd_mode(kb, bit);
 }
 
 /**
@@ -2137,11 +2137,11 @@ int vt_get_kbd_mode_bit(int console, int bit)
 
 void vt_set_kbd_mode_bit(int console, int bit)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	unsigned long flags;
 
 	spin_lock_irqsave(&kbd_event_lock, flags);
-	set_vc_kbd_mode(kbd, bit);
+	set_vc_kbd_mode(kb, bit);
 	spin_unlock_irqrestore(&kbd_event_lock, flags);
 }
 
@@ -2156,10 +2156,10 @@ void vt_set_kbd_mode_bit(int console, int bit)
 
 void vt_clr_kbd_mode_bit(int console, int bit)
 {
-	struct kbd_struct * kbd = kbd_table + console;
+	struct kbd_struct *kb = kbd_table + console;
 	unsigned long flags;
 
 	spin_lock_irqsave(&kbd_event_lock, flags);
-	clr_vc_kbd_mode(kbd, bit);
+	clr_vc_kbd_mode(kb, bit);
 	spin_unlock_irqrestore(&kbd_event_lock, flags);
 }
diff --git a/drivers/uio/uio.c b/drivers/uio/uio.c
index a673e5b6a2e0..60fa6278fbce 100644
--- a/drivers/uio/uio.c
+++ b/drivers/uio/uio.c
@@ -28,18 +28,6 @@
 
 #define UIO_MAX_DEVICES		(1U << MINORBITS)
 
-struct uio_device {
-	struct module		*owner;
-	struct device		*dev;
-	int			minor;
-	atomic_t		event;
-	struct fasync_struct	*async_queue;
-	wait_queue_head_t	wait;
-	struct uio_info		*info;
-	struct kobject		*map_dir;
-	struct kobject		*portio_dir;
-};
-
 static int uio_major;
 static struct cdev *uio_cdev;
 static DEFINE_IDR(uio_idr);
diff --git a/drivers/usb/Kconfig b/drivers/usb/Kconfig
index e0cad4418085..ae481c37a208 100644
--- a/drivers/usb/Kconfig
+++ b/drivers/usb/Kconfig
@@ -92,6 +92,8 @@ source "drivers/usb/storage/Kconfig"
 
 source "drivers/usb/image/Kconfig"
 
+source "drivers/usb/usbip/Kconfig"
+
 endif
 
 source "drivers/usb/musb/Kconfig"
@@ -147,4 +149,14 @@ source "drivers/usb/phy/Kconfig"
 
 source "drivers/usb/gadget/Kconfig"
 
+config USB_LED_TRIG
+	bool "USB LED Triggers"
+	depends on LEDS_CLASS && USB_COMMON && LEDS_TRIGGERS
+	help
+	  This option adds LED triggers for USB host and/or gadget activity.
+
+	  Say Y here if you are working on a system with led-class supported
+	  LEDs and you want to use them as activity indicators for USB host or
+	  gadget.
+
 endif # USB_SUPPORT
diff --git a/drivers/usb/Makefile b/drivers/usb/Makefile
index 3cba892b83a2..d7be71778059 100644
--- a/drivers/usb/Makefile
+++ b/drivers/usb/Makefile
@@ -60,3 +60,5 @@ obj-$(CONFIG_USB_RENESAS_USBHS)	+= renesas_usbhs/
 obj-$(CONFIG_USB_GADGET)	+= gadget/
 
 obj-$(CONFIG_USB_COMMON)	+= common/
+
+obj-$(CONFIG_USBIP_CORE)	+= usbip/
diff --git a/drivers/usb/README b/drivers/usb/README
index 284f46b3e1cc..2144e7dbfa41 100644
--- a/drivers/usb/README
+++ b/drivers/usb/README
@@ -24,7 +24,7 @@ Here is a list of what each subdirectory here is, and what is contained in
 them.
 
 core/		- This is for the core USB host code, including the
-		  usbfs files and the hub class driver ("khubd").
+		  usbfs files and the hub class driver ("hub_wq").
 
 host/		- This is for USB host controller drivers.  This
 		  includes UHCI, OHCI, EHCI, and others that might
diff --git a/drivers/usb/chipidea/ci.h b/drivers/usb/chipidea/ci.h
index 9563cb56d564..ea40626e0246 100644
--- a/drivers/usb/chipidea/ci.h
+++ b/drivers/usb/chipidea/ci.h
@@ -99,10 +99,10 @@ enum ci_role {
 
 /**
  * struct ci_role_driver - host/gadget role driver
- * start: start this role
- * stop: stop this role
- * irq: irq handler for this role
- * name: role name string (host/gadget)
+ * @start: start this role
+ * @stop: stop this role
+ * @irq: irq handler for this role
+ * @name: role name string (host/gadget)
  */
 struct ci_role_driver {
 	int		(*start)(struct ci_hdrc *);
@@ -245,6 +245,7 @@ static inline void ci_role_stop(struct ci_hdrc *ci)
 
 /**
  * hw_read: reads from a hw register
+ * @ci: the controller
  * @reg:  register index
  * @mask: bitfield mask
  *
@@ -277,6 +278,7 @@ static inline void __hw_write(struct ci_hdrc *ci, u32 val,
 
 /**
  * hw_write: writes to a hw register
+ * @ci: the controller
  * @reg:  register index
  * @mask: bitfield mask
  * @data: new value
@@ -293,6 +295,7 @@ static inline void hw_write(struct ci_hdrc *ci, enum ci_hw_regs reg,
 
 /**
  * hw_test_and_clear: tests & clears a hw register
+ * @ci: the controller
  * @reg:  register index
  * @mask: bitfield mask
  *
@@ -309,6 +312,7 @@ static inline u32 hw_test_and_clear(struct ci_hdrc *ci, enum ci_hw_regs reg,
 
 /**
  * hw_test_and_write: tests & writes a hw register
+ * @ci: the controller
  * @reg:  register index
  * @mask: bitfield mask
  * @data: new value
@@ -327,6 +331,8 @@ static inline u32 hw_test_and_write(struct ci_hdrc *ci, enum ci_hw_regs reg,
 /**
  * ci_otg_is_fsm_mode: runtime check if otg controller
  * is in otg fsm mode.
+ *
+ * @ci: chipidea device
  */
 static inline bool ci_otg_is_fsm_mode(struct ci_hdrc *ci)
 {
diff --git a/drivers/usb/chipidea/ci_hdrc_imx.c b/drivers/usb/chipidea/ci_hdrc_imx.c
index 65444b02bd68..a7ab0f15926e 100644
--- a/drivers/usb/chipidea/ci_hdrc_imx.c
+++ b/drivers/usb/chipidea/ci_hdrc_imx.c
@@ -54,6 +54,7 @@ struct ci_hdrc_imx_data {
 
 static struct imx_usbmisc_data *usbmisc_get_init_data(struct device *dev)
 {
+	struct platform_device *misc_pdev;
 	struct device_node *np = dev->of_node;
 	struct of_phandle_args args;
 	struct imx_usbmisc_data *data;
@@ -79,8 +80,15 @@ static struct imx_usbmisc_data *usbmisc_get_init_data(struct device *dev)
 	}
 
 	data->index = args.args[0];
+
+	misc_pdev = of_find_device_by_node(args.np);
 	of_node_put(args.np);
 
+	if (!misc_pdev)
+		return ERR_PTR(-EPROBE_DEFER);
+
+	data->dev = &misc_pdev->dev;
+
 	if (of_find_property(np, "disable-over-current", NULL))
 		data->disable_oc = 1;
 
diff --git a/drivers/usb/chipidea/ci_hdrc_imx.h b/drivers/usb/chipidea/ci_hdrc_imx.h
index 996ec93467b2..4ed828f75a1e 100644
--- a/drivers/usb/chipidea/ci_hdrc_imx.h
+++ b/drivers/usb/chipidea/ci_hdrc_imx.h
@@ -13,6 +13,7 @@
 #define __DRIVER_USB_CHIPIDEA_CI_HDRC_IMX_H
 
 struct imx_usbmisc_data {
+	struct device *dev;
 	int index;
 
 	unsigned int disable_oc:1; /* over current detect disabled */
diff --git a/drivers/usb/chipidea/ci_hdrc_msm.c b/drivers/usb/chipidea/ci_hdrc_msm.c
index d72b9d2de2c5..4935ac38fd00 100644
--- a/drivers/usb/chipidea/ci_hdrc_msm.c
+++ b/drivers/usb/chipidea/ci_hdrc_msm.c
@@ -20,13 +20,13 @@
 static void ci_hdrc_msm_notify_event(struct ci_hdrc *ci, unsigned event)
 {
 	struct device *dev = ci->gadget.dev.parent;
-	int val;
 
 	switch (event) {
 	case CI_HDRC_CONTROLLER_RESET_EVENT:
 		dev_dbg(dev, "CI_HDRC_CONTROLLER_RESET_EVENT received\n");
 		writel(0, USB_AHBBURST);
 		writel(0, USB_AHBMODE);
+		usb_phy_init(ci->transceiver);
 		break;
 	case CI_HDRC_CONTROLLER_STOPPED_EVENT:
 		dev_dbg(dev, "CI_HDRC_CONTROLLER_STOPPED_EVENT received\n");
@@ -34,10 +34,7 @@ static void ci_hdrc_msm_notify_event(struct ci_hdrc *ci, unsigned event)
 		 * Put the transceiver in non-driving mode. Otherwise host
 		 * may not detect soft-disconnection.
 		 */
-		val = usb_phy_io_read(ci->transceiver, ULPI_FUNC_CTRL);
-		val &= ~ULPI_FUNC_CTRL_OPMODE_MASK;
-		val |= ULPI_FUNC_CTRL_OPMODE_NONDRIVING;
-		usb_phy_io_write(ci->transceiver, val, ULPI_FUNC_CTRL);
+		usb_phy_notify_disconnect(ci->transceiver, USB_SPEED_UNKNOWN);
 		break;
 	default:
 		dev_dbg(dev, "unknown ci_hdrc event\n");
diff --git a/drivers/usb/chipidea/core.c b/drivers/usb/chipidea/core.c
index 619d13e29995..3df5005c554d 100644
--- a/drivers/usb/chipidea/core.c
+++ b/drivers/usb/chipidea/core.c
@@ -139,6 +139,8 @@ static int hw_alloc_regmap(struct ci_hdrc *ci, bool is_lpm)
 /**
  * hw_read_intr_enable: returns interrupt enable register
  *
+ * @ci: the controller
+ *
  * This function returns register data
  */
 u32 hw_read_intr_enable(struct ci_hdrc *ci)
@@ -149,6 +151,8 @@ u32 hw_read_intr_enable(struct ci_hdrc *ci)
 /**
  * hw_read_intr_status: returns interrupt status register
  *
+ * @ci: the controller
+ *
  * This function returns register data
  */
 u32 hw_read_intr_status(struct ci_hdrc *ci)
@@ -176,6 +180,8 @@ int hw_port_test_set(struct ci_hdrc *ci, u8 mode)
 /**
  * hw_port_test_get: reads port test mode value
  *
+ * @ci: the controller
+ *
  * This function returns port test mode value
  */
 u8 hw_port_test_get(struct ci_hdrc *ci)
@@ -295,7 +301,7 @@ static void hw_phymode_configure(struct ci_hdrc *ci)
 /**
  * ci_usb_phy_init: initialize phy according to different phy type
  * @ci: the controller
-  *
+ *
  * This function returns an error code if usb_phy_init has failed
  */
 static int ci_usb_phy_init(struct ci_hdrc *ci)
@@ -473,6 +479,10 @@ static int ci_get_platdata(struct device *dev,
 				PTR_ERR(platdata->reg_vbus));
 			return PTR_ERR(platdata->reg_vbus);
 		}
+		/* Get TPL support */
+		if (!platdata->tpl_support)
+			platdata->tpl_support =
+				of_usb_host_tpl_support(dev->of_node);
 	}
 
 	if (of_usb_get_maximum_speed(dev->of_node) == USB_SPEED_FULL)
@@ -658,7 +668,7 @@ static int ci_hdrc_probe(struct platform_device *pdev)
 		goto deinit_phy;
 	}
 
-	if (ci->is_otg) {
+	if (ci->is_otg && ci->roles[CI_ROLE_GADGET]) {
 		/* Disable and clear all OTG irq */
 		hw_write_otgsc(ci, OTGSC_INT_EN_BITS | OTGSC_INT_STATUS_BITS,
 							OTGSC_INT_STATUS_BITS);
diff --git a/drivers/usb/chipidea/host.c b/drivers/usb/chipidea/host.c
index a93d950e9468..ebde7b6ce687 100644
--- a/drivers/usb/chipidea/host.c
+++ b/drivers/usb/chipidea/host.c
@@ -59,7 +59,8 @@ static int host_start(struct ci_hdrc *ci)
 	hcd->has_tt = 1;
 
 	hcd->power_budget = ci->platdata->power_budget;
-	hcd->phy = ci->transceiver;
+	hcd->usb_phy = ci->transceiver;
+	hcd->tpl_support = ci->platdata->tpl_support;
 
 	ehci = hcd_to_ehci(hcd);
 	ehci->caps = ci->hw_bank.cap;
diff --git a/drivers/usb/chipidea/udc.c b/drivers/usb/chipidea/udc.c
index b8125aa64ad8..0444d3f8971a 100644
--- a/drivers/usb/chipidea/udc.c
+++ b/drivers/usb/chipidea/udc.c
@@ -627,7 +627,7 @@ __acquires(hwep->lock)
 
 		if (hwreq->req.complete != NULL) {
 			spin_unlock(hwep->lock);
-			hwreq->req.complete(&hwep->ep, &hwreq->req);
+			usb_gadget_giveback_request(&hwep->ep, &hwreq->req);
 			spin_lock(hwep->lock);
 		}
 	}
@@ -922,7 +922,7 @@ __acquires(hwep->lock)
 			if ((hwep->type == USB_ENDPOINT_XFER_CONTROL) &&
 					hwreq->req.length)
 				hweptemp = hwep->ci->ep0in;
-			hwreq->req.complete(&hweptemp->ep, &hwreq->req);
+			usb_gadget_giveback_request(&hweptemp->ep, &hwreq->req);
 			spin_lock(hwep->lock);
 		}
 	}
@@ -1347,7 +1347,7 @@ static int ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 
 	if (hwreq->req.complete != NULL) {
 		spin_unlock(hwep->lock);
-		hwreq->req.complete(&hwep->ep, &hwreq->req);
+		usb_gadget_giveback_request(&hwep->ep, &hwreq->req);
 		spin_lock(hwep->lock);
 	}
 
diff --git a/drivers/usb/chipidea/usbmisc_imx.c b/drivers/usb/chipidea/usbmisc_imx.c
index 85293b8b1df9..926c997ef310 100644
--- a/drivers/usb/chipidea/usbmisc_imx.c
+++ b/drivers/usb/chipidea/usbmisc_imx.c
@@ -57,6 +57,8 @@
 
 #define MX6_BM_OVER_CUR_DIS		BIT(7)
 
+#define VF610_OVER_CUR_DIS		BIT(7)
+
 struct usbmisc_ops {
 	/* It's called once when probe a usb device */
 	int (*init)(struct imx_usbmisc_data *data);
@@ -71,10 +73,9 @@ struct imx_usbmisc {
 	const struct usbmisc_ops *ops;
 };
 
-static struct imx_usbmisc *usbmisc;
-
 static int usbmisc_imx25_init(struct imx_usbmisc_data *data)
 {
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
 	unsigned long flags;
 	u32 val = 0;
 
@@ -108,6 +109,7 @@ static int usbmisc_imx25_init(struct imx_usbmisc_data *data)
 
 static int usbmisc_imx25_post(struct imx_usbmisc_data *data)
 {
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
 	void __iomem *reg;
 	unsigned long flags;
 	u32 val;
@@ -130,6 +132,7 @@ static int usbmisc_imx25_post(struct imx_usbmisc_data *data)
 
 static int usbmisc_imx27_init(struct imx_usbmisc_data *data)
 {
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
 	unsigned long flags;
 	u32 val;
 
@@ -160,6 +163,7 @@ static int usbmisc_imx27_init(struct imx_usbmisc_data *data)
 
 static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 {
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
 	void __iomem *reg = NULL;
 	unsigned long flags;
 	u32 val = 0;
@@ -204,6 +208,7 @@ static int usbmisc_imx53_init(struct imx_usbmisc_data *data)
 
 static int usbmisc_imx6q_init(struct imx_usbmisc_data *data)
 {
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
 	unsigned long flags;
 	u32 reg;
 
@@ -221,6 +226,26 @@ static int usbmisc_imx6q_init(struct imx_usbmisc_data *data)
 	return 0;
 }
 
+static int usbmisc_vf610_init(struct imx_usbmisc_data *data)
+{
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+	u32 reg;
+
+	/*
+	 * Vybrid only has one misc register set, but in two different
+	 * areas. These is reflected in two instances of this driver.
+	 */
+	if (data->index >= 1)
+		return -EINVAL;
+
+	if (data->disable_oc) {
+		reg = readl(usbmisc->base);
+		writel(reg | VF610_OVER_CUR_DIS, usbmisc->base);
+	}
+
+	return 0;
+}
+
 static const struct usbmisc_ops imx25_usbmisc_ops = {
 	.init = usbmisc_imx25_init,
 	.post = usbmisc_imx25_post,
@@ -238,10 +263,14 @@ static const struct usbmisc_ops imx6q_usbmisc_ops = {
 	.init = usbmisc_imx6q_init,
 };
 
+static const struct usbmisc_ops vf610_usbmisc_ops = {
+	.init = usbmisc_vf610_init,
+};
+
 int imx_usbmisc_init(struct imx_usbmisc_data *data)
 {
-	if (!usbmisc)
-		return -EPROBE_DEFER;
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+
 	if (!usbmisc->ops->init)
 		return 0;
 	return usbmisc->ops->init(data);
@@ -250,8 +279,8 @@ EXPORT_SYMBOL_GPL(imx_usbmisc_init);
 
 int imx_usbmisc_init_post(struct imx_usbmisc_data *data)
 {
-	if (!usbmisc)
-		return -EPROBE_DEFER;
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(data->dev);
+
 	if (!usbmisc->ops->post)
 		return 0;
 	return usbmisc->ops->post(data);
@@ -283,6 +312,10 @@ static const struct of_device_id usbmisc_imx_dt_ids[] = {
 		.compatible = "fsl,imx6q-usbmisc",
 		.data = &imx6q_usbmisc_ops,
 	},
+	{
+		.compatible = "fsl,vf610-usbmisc",
+		.data = &vf610_usbmisc_ops,
+	},
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, usbmisc_imx_dt_ids);
@@ -294,9 +327,6 @@ static int usbmisc_imx_probe(struct platform_device *pdev)
 	int ret;
 	struct of_device_id *tmp_dev;
 
-	if (usbmisc)
-		return -EBUSY;
-
 	data = devm_kzalloc(&pdev->dev, sizeof(*data), GFP_KERNEL);
 	if (!data)
 		return -ENOMEM;
@@ -325,15 +355,15 @@ static int usbmisc_imx_probe(struct platform_device *pdev)
 	tmp_dev = (struct of_device_id *)
 		of_match_device(usbmisc_imx_dt_ids, &pdev->dev);
 	data->ops = (const struct usbmisc_ops *)tmp_dev->data;
-	usbmisc = data;
+	platform_set_drvdata(pdev, data);
 
 	return 0;
 }
 
 static int usbmisc_imx_remove(struct platform_device *pdev)
 {
+	struct imx_usbmisc *usbmisc = dev_get_drvdata(&pdev->dev);
 	clk_disable_unprepare(usbmisc->clk);
-	usbmisc = NULL;
 	return 0;
 }
 
diff --git a/drivers/usb/common/Makefile b/drivers/usb/common/Makefile
index 752646167e1e..ca2f8bd0e431 100644
--- a/drivers/usb/common/Makefile
+++ b/drivers/usb/common/Makefile
@@ -2,5 +2,8 @@
 # Makefile for the usb common parts.
 #
 
-obj-$(CONFIG_USB_COMMON) += usb-common.o
+obj-$(CONFIG_USB_COMMON)	  += usb-common.o
+usb-common-y			  += common.o
+usb-common-$(CONFIG_USB_LED_TRIG) += led.o
+
 obj-$(CONFIG_USB_OTG_FSM) += usb-otg-fsm.o
diff --git a/drivers/usb/common/usb-common.c b/drivers/usb/common/common.c
index 6dfd30a863c7..b530fd403ffb 100644
--- a/drivers/usb/common/usb-common.c
+++ b/drivers/usb/common/common.c
@@ -139,6 +139,21 @@ enum usb_device_speed of_usb_get_maximum_speed(struct device_node *np)
 }
 EXPORT_SYMBOL_GPL(of_usb_get_maximum_speed);
 
+/**
+ * of_usb_host_tpl_support - to get if Targeted Peripheral List is supported
+ * for given targeted hosts (non-PC hosts)
+ * @np: Pointer to the given device_node
+ *
+ * The function gets if the targeted hosts support TPL or not
+ */
+bool of_usb_host_tpl_support(struct device_node *np)
+{
+	if (of_find_property(np, "tpl-support", NULL))
+		return true;
+
+	return false;
+}
+EXPORT_SYMBOL_GPL(of_usb_host_tpl_support);
 #endif
 
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/common/led.c b/drivers/usb/common/led.c
new file mode 100644
index 000000000000..df23da00a901
--- /dev/null
+++ b/drivers/usb/common/led.c
@@ -0,0 +1,57 @@
+/*
+ * LED Triggers for USB Activity
+ *
+ * Copyright 2014 Michal Sojka <sojka@merica.cz>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/leds.h>
+#include <linux/usb.h>
+
+#define BLINK_DELAY 30
+
+static unsigned long usb_blink_delay = BLINK_DELAY;
+
+DEFINE_LED_TRIGGER(ledtrig_usb_gadget);
+DEFINE_LED_TRIGGER(ledtrig_usb_host);
+
+void usb_led_activity(enum usb_led_event ev)
+{
+	struct led_trigger *trig = NULL;
+
+	switch (ev) {
+	case USB_LED_EVENT_GADGET:
+		trig = ledtrig_usb_gadget;
+		break;
+	case USB_LED_EVENT_HOST:
+		trig = ledtrig_usb_host;
+		break;
+	}
+	/* led_trigger_blink_oneshot() handles trig == NULL gracefully */
+	led_trigger_blink_oneshot(trig, &usb_blink_delay, &usb_blink_delay, 0);
+}
+EXPORT_SYMBOL_GPL(usb_led_activity);
+
+
+static int __init ledtrig_usb_init(void)
+{
+	led_trigger_register_simple("usb-gadget", &ledtrig_usb_gadget);
+	led_trigger_register_simple("usb-host", &ledtrig_usb_host);
+	return 0;
+}
+
+static void __exit ledtrig_usb_exit(void)
+{
+	led_trigger_unregister_simple(ledtrig_usb_gadget);
+	led_trigger_unregister_simple(ledtrig_usb_host);
+}
+
+module_init(ledtrig_usb_init);
+module_exit(ledtrig_usb_exit);
diff --git a/drivers/usb/core/Kconfig b/drivers/usb/core/Kconfig
index 1060657ca1b0..9cfda6a72194 100644
--- a/drivers/usb/core/Kconfig
+++ b/drivers/usb/core/Kconfig
@@ -56,22 +56,16 @@ config USB_OTG
 	  connector.
 
 config USB_OTG_WHITELIST
-	bool "Rely on OTG Targeted Peripherals List"
-	depends on USB_OTG || EXPERT
-	default y if USB_OTG
+	bool "Rely on OTG and EH Targeted Peripherals List"
+	depends on USB
 	help
 	  If you say Y here, the "otg_whitelist.h" file will be used as a
 	  product whitelist, so USB peripherals not listed there will be
 	  rejected during enumeration.  This behavior is required by the
-	  USB OTG specification for all devices not on your product's
+	  USB OTG and EH specification for all devices not on your product's
 	  "Targeted Peripherals List".  "Embedded Hosts" are likewise
 	  allowed to support only a limited number of peripherals.
 
-	  Otherwise, peripherals not listed there will only generate a
-	  warning and enumeration will continue.  That's more like what
-	  normal Linux-USB hosts do (other than the warning), and is
-	  convenient for many stages of product development.
-
 config USB_OTG_BLACKLIST_HUB
 	bool "Disable external hubs"
 	depends on USB_OTG || EXPERT
diff --git a/drivers/usb/core/hcd.c b/drivers/usb/core/hcd.c
index 487abcfcccd8..b84fb141e122 100644
--- a/drivers/usb/core/hcd.c
+++ b/drivers/usb/core/hcd.c
@@ -42,6 +42,7 @@
 #include <linux/pm_runtime.h>
 #include <linux/types.h>
 
+#include <linux/phy/phy.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
 #include <linux/usb/phy.h>
@@ -1272,7 +1273,7 @@ EXPORT_SYMBOL_GPL(usb_hcd_unlink_urb_from_ep);
  * The usb core itself is however optimized for host controllers that can dma
  * using regular system memory - like pci devices doing bus mastering.
  *
- * To support host controllers with limited dma capabilites we provide dma
+ * To support host controllers with limited dma capabilities we provide dma
  * bounce buffers. This feature can be enabled using the HCD_LOCAL_MEM flag.
  * For this to work properly the host controller code must first use the
  * function dma_declare_coherent_memory() to point out which memory area
@@ -1664,6 +1665,8 @@ static void __usb_hcd_giveback_urb(struct urb *urb)
 	usbmon_urb_complete(&hcd->self, urb, status);
 	usb_anchor_suspend_wakeups(anchor);
 	usb_unanchor_urb(urb);
+	if (likely(status == 0))
+		usb_led_activity(USB_LED_EVENT_HOST);
 
 	/* pass ownership to the completion handler */
 	urb->status = status;
@@ -2301,7 +2304,7 @@ EXPORT_SYMBOL_GPL(usb_hcd_resume_root_hub);
  * Context: in_interrupt()
  *
  * Starts enumeration, with an immediate reset followed later by
- * khubd identifying and possibly configuring the device.
+ * hub_wq identifying and possibly configuring the device.
  * This is needed by OTG controller drivers, where it helps meet
  * HNP protocol timing requirements for starting a port reset.
  *
@@ -2320,7 +2323,7 @@ int usb_bus_start_enum(struct usb_bus *bus, unsigned port_num)
 	if (port_num && hcd->driver->start_port_reset)
 		status = hcd->driver->start_port_reset(hcd, port_num);
 
-	/* run khubd shortly after (first) root port reset finishes;
+	/* allocate hub_wq shortly after (first) root port reset finishes;
 	 * it may issue others, until at least 50 msecs have passed.
 	 */
 	if (status == 0)
@@ -2383,20 +2386,20 @@ void usb_hc_died (struct usb_hcd *hcd)
 	if (hcd->rh_registered) {
 		clear_bit(HCD_FLAG_POLL_RH, &hcd->flags);
 
-		/* make khubd clean up old urbs and devices */
+		/* make hub_wq clean up old urbs and devices */
 		usb_set_device_state (hcd->self.root_hub,
 				USB_STATE_NOTATTACHED);
-		usb_kick_khubd (hcd->self.root_hub);
+		usb_kick_hub_wq(hcd->self.root_hub);
 	}
 	if (usb_hcd_is_primary_hcd(hcd) && hcd->shared_hcd) {
 		hcd = hcd->shared_hcd;
 		if (hcd->rh_registered) {
 			clear_bit(HCD_FLAG_POLL_RH, &hcd->flags);
 
-			/* make khubd clean up old urbs and devices */
+			/* make hub_wq clean up old urbs and devices */
 			usb_set_device_state(hcd->self.root_hub,
 					USB_STATE_NOTATTACHED);
-			usb_kick_khubd(hcd->self.root_hub);
+			usb_kick_hub_wq(hcd->self.root_hub);
 		}
 	}
 	spin_unlock_irqrestore (&hcd_root_hub_lock, flags);
@@ -2627,7 +2630,7 @@ int usb_add_hcd(struct usb_hcd *hcd,
 	int retval;
 	struct usb_device *rhdev;
 
-	if (IS_ENABLED(CONFIG_USB_PHY) && !hcd->phy) {
+	if (IS_ENABLED(CONFIG_USB_PHY) && !hcd->usb_phy) {
 		struct usb_phy *phy = usb_get_phy_dev(hcd->self.controller, 0);
 
 		if (IS_ERR(phy)) {
@@ -2640,11 +2643,34 @@ int usb_add_hcd(struct usb_hcd *hcd,
 				usb_put_phy(phy);
 				return retval;
 			}
-			hcd->phy = phy;
+			hcd->usb_phy = phy;
 			hcd->remove_phy = 1;
 		}
 	}
 
+	if (IS_ENABLED(CONFIG_GENERIC_PHY)) {
+		struct phy *phy = phy_get(hcd->self.controller, "usb");
+
+		if (IS_ERR(phy)) {
+			retval = PTR_ERR(phy);
+			if (retval == -EPROBE_DEFER)
+				goto err_phy;
+		} else {
+			retval = phy_init(phy);
+			if (retval) {
+				phy_put(phy);
+				goto err_phy;
+			}
+			retval = phy_power_on(phy);
+			if (retval) {
+				phy_exit(phy);
+				phy_put(phy);
+				goto err_phy;
+			}
+			hcd->phy = phy;
+		}
+	}
+
 	dev_info(hcd->self.controller, "%s\n", hcd->product_desc);
 
 	/* Keep old behaviour if authorized_default is not in [0, 1]. */
@@ -2655,12 +2681,12 @@ int usb_add_hcd(struct usb_hcd *hcd,
 	set_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
 
 	/* HC is in reset state, but accessible.  Now do the one-time init,
-	 * bottom up so that hcds can customize the root hubs before khubd
+	 * bottom up so that hcds can customize the root hubs before hub_wq
 	 * starts talking to them.  (Note, bus id is assigned early too.)
 	 */
 	if ((retval = hcd_buffer_create(hcd)) != 0) {
 		dev_dbg(hcd->self.controller, "pool alloc failed\n");
-		goto err_remove_phy;
+		goto err_create_buf;
 	}
 
 	if ((retval = usb_register_bus(&hcd->self)) < 0)
@@ -2787,12 +2813,19 @@ err_allocate_root_hub:
 	usb_deregister_bus(&hcd->self);
 err_register_bus:
 	hcd_buffer_destroy(hcd);
-err_remove_phy:
-	if (hcd->remove_phy && hcd->phy) {
-		usb_phy_shutdown(hcd->phy);
-		usb_put_phy(hcd->phy);
+err_create_buf:
+	if (IS_ENABLED(CONFIG_GENERIC_PHY) && hcd->phy) {
+		phy_power_off(hcd->phy);
+		phy_exit(hcd->phy);
+		phy_put(hcd->phy);
 		hcd->phy = NULL;
 	}
+err_phy:
+	if (hcd->remove_phy && hcd->usb_phy) {
+		usb_phy_shutdown(hcd->usb_phy);
+		usb_put_phy(hcd->usb_phy);
+		hcd->usb_phy = NULL;
+	}
 	return retval;
 }
 EXPORT_SYMBOL_GPL(usb_add_hcd);
@@ -2864,11 +2897,18 @@ void usb_remove_hcd(struct usb_hcd *hcd)
 
 	usb_deregister_bus(&hcd->self);
 	hcd_buffer_destroy(hcd);
-	if (hcd->remove_phy && hcd->phy) {
-		usb_phy_shutdown(hcd->phy);
-		usb_put_phy(hcd->phy);
+
+	if (IS_ENABLED(CONFIG_GENERIC_PHY) && hcd->phy) {
+		phy_power_off(hcd->phy);
+		phy_exit(hcd->phy);
+		phy_put(hcd->phy);
 		hcd->phy = NULL;
 	}
+	if (hcd->remove_phy && hcd->usb_phy) {
+		usb_phy_shutdown(hcd->usb_phy);
+		usb_put_phy(hcd->usb_phy);
+		hcd->usb_phy = NULL;
+	}
 
 	usb_put_invalidate_rhdev(hcd);
 }
diff --git a/drivers/usb/core/hub.c b/drivers/usb/core/hub.c
index 8a4dcbc7a75f..11e80ac31324 100644
--- a/drivers/usb/core/hub.c
+++ b/drivers/usb/core/hub.c
@@ -22,9 +22,8 @@
 #include <linux/usb/hcd.h>
 #include <linux/usb/otg.h>
 #include <linux/usb/quirks.h>
-#include <linux/kthread.h>
+#include <linux/workqueue.h>
 #include <linux/mutex.h>
-#include <linux/freezer.h>
 #include <linux/random.h>
 #include <linux/pm_qos.h>
 
@@ -32,6 +31,7 @@
 #include <asm/byteorder.h>
 
 #include "hub.h"
+#include "otg_whitelist.h"
 
 #define USB_VENDOR_GENESYS_LOGIC		0x05e3
 #define HUB_QUIRK_CHECK_PORT_AUTOSUSPEND	0x01
@@ -41,14 +41,9 @@
  * change to USB_STATE_NOTATTACHED even when the semaphore isn't held. */
 static DEFINE_SPINLOCK(device_state_lock);
 
-/* khubd's worklist and its lock */
-static DEFINE_SPINLOCK(hub_event_lock);
-static LIST_HEAD(hub_event_list);	/* List of hubs needing servicing */
-
-/* Wakes up khubd */
-static DECLARE_WAIT_QUEUE_HEAD(khubd_wait);
-
-static struct task_struct *khubd_task;
+/* workqueue to process hub events */
+static struct workqueue_struct *hub_wq;
+static void hub_event(struct work_struct *work);
 
 /* synchronize hub-port add/remove and peering operations */
 DEFINE_MUTEX(usb_port_peer_mutex);
@@ -104,6 +99,7 @@ EXPORT_SYMBOL_GPL(ehci_cf_port_reset_rwsem);
 #define HUB_DEBOUNCE_STEP	  25
 #define HUB_DEBOUNCE_STABLE	 100
 
+static void hub_release(struct kref *kref);
 static int usb_reset_and_verify_device(struct usb_device *udev);
 
 static inline char *portspeed(struct usb_hub *hub, int portstatus)
@@ -575,28 +571,39 @@ static int hub_port_status(struct usb_hub *hub, int port1,
 	return ret;
 }
 
-static void kick_khubd(struct usb_hub *hub)
+static void kick_hub_wq(struct usb_hub *hub)
 {
-	unsigned long	flags;
+	struct usb_interface *intf;
 
-	spin_lock_irqsave(&hub_event_lock, flags);
-	if (!hub->disconnected && list_empty(&hub->event_list)) {
-		list_add_tail(&hub->event_list, &hub_event_list);
+	if (hub->disconnected || work_pending(&hub->events))
+		return;
 
-		/* Suppress autosuspend until khubd runs */
-		usb_autopm_get_interface_no_resume(
-				to_usb_interface(hub->intfdev));
-		wake_up(&khubd_wait);
-	}
-	spin_unlock_irqrestore(&hub_event_lock, flags);
+	/*
+	 * Suppress autosuspend until the event is proceed.
+	 *
+	 * Be careful and make sure that the symmetric operation is
+	 * always called. We are here only when there is no pending
+	 * work for this hub. Therefore put the interface either when
+	 * the new work is called or when it is canceled.
+	 */
+	intf = to_usb_interface(hub->intfdev);
+	usb_autopm_get_interface_no_resume(intf);
+	kref_get(&hub->kref);
+
+	if (queue_work(hub_wq, &hub->events))
+		return;
+
+	/* the work has already been scheduled */
+	usb_autopm_put_interface_async(intf);
+	kref_put(&hub->kref, hub_release);
 }
 
-void usb_kick_khubd(struct usb_device *hdev)
+void usb_kick_hub_wq(struct usb_device *hdev)
 {
 	struct usb_hub *hub = usb_hub_to_struct_hub(hdev);
 
 	if (hub)
-		kick_khubd(hub);
+		kick_hub_wq(hub);
 }
 
 /*
@@ -618,7 +625,7 @@ void usb_wakeup_notification(struct usb_device *hdev,
 	hub = usb_hub_to_struct_hub(hdev);
 	if (hub) {
 		set_bit(portnum, hub->wakeup_bits);
-		kick_khubd(hub);
+		kick_hub_wq(hub);
 	}
 }
 EXPORT_SYMBOL_GPL(usb_wakeup_notification);
@@ -645,7 +652,7 @@ static void hub_irq(struct urb *urb)
 		hub->error = status;
 		/* FALL THROUGH */
 
-	/* let khubd handle things */
+	/* let hub_wq handle things */
 	case 0:			/* we got data:  port status changed */
 		bits = 0;
 		for (i = 0; i < urb->actual_length; ++i)
@@ -657,8 +664,8 @@ static void hub_irq(struct urb *urb)
 
 	hub->nerrors = 0;
 
-	/* Something happened, let khubd figure it out */
-	kick_khubd(hub);
+	/* Something happened, let hub_wq figure it out */
+	kick_hub_wq(hub);
 
 resubmit:
 	if (hub->quiescing)
@@ -688,7 +695,7 @@ hub_clear_tt_buffer (struct usb_device *hdev, u16 devinfo, u16 tt)
 }
 
 /*
- * enumeration blocks khubd for a long time. we use keventd instead, since
+ * enumeration blocks hub_wq for a long time. we use keventd instead, since
  * long blocking there is the exception, not the rule.  accordingly, HCDs
  * talking to TTs must queue control transfers (not just bulk and iso), so
  * both can talk to the same hub concurrently.
@@ -954,7 +961,7 @@ static int hub_port_disable(struct usb_hub *hub, int port1, int set_state)
 
 /*
  * Disable a port and mark a logical connect-change event, so that some
- * time later khubd will disconnect() any existing usb_device on the port
+ * time later hub_wq will disconnect() any existing usb_device on the port
  * and will re-enumerate if there actually is a device attached.
  */
 static void hub_port_logical_disconnect(struct usb_hub *hub, int port1)
@@ -967,12 +974,12 @@ static void hub_port_logical_disconnect(struct usb_hub *hub, int port1)
 	 *  - SRP saves power that way
 	 *  - ... new call, TBD ...
 	 * That's easy if this hub can switch power per-port, and
-	 * khubd reactivates the port later (timer, SRP, etc).
+	 * hub_wq reactivates the port later (timer, SRP, etc).
 	 * Powerdown must be optional, because of reset/DFU.
 	 */
 
 	set_bit(port1, hub->change_bits);
-	kick_khubd(hub);
+	kick_hub_wq(hub);
 }
 
 /**
@@ -980,7 +987,7 @@ static void hub_port_logical_disconnect(struct usb_hub *hub, int port1)
  * @udev: device to be disabled and removed
  * Context: @udev locked, must be able to sleep.
  *
- * After @udev's port has been disabled, khubd is notified and it will
+ * After @udev's port has been disabled, hub_wq is notified and it will
  * see that the device has been disconnected.  When the device is
  * physically unplugged and something is plugged in, the events will
  * be received and processed normally.
@@ -1100,7 +1107,7 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
  init2:
 
 	/*
-	 * Check each port and set hub->change_bits to let khubd know
+	 * Check each port and set hub->change_bits to let hub_wq know
 	 * which ports need attention.
 	 */
 	for (port1 = 1; port1 <= hdev->maxchild; ++port1) {
@@ -1167,7 +1174,7 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 			clear_bit(port1, hub->removed_bits);
 
 		if (!udev || udev->state == USB_STATE_NOTATTACHED) {
-			/* Tell khubd to disconnect the device or
+			/* Tell hub_wq to disconnect the device or
 			 * check for a new connection
 			 */
 			if (udev || (portstatus & USB_PORT_STAT_CONNECTION) ||
@@ -1180,7 +1187,7 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 				USB_SS_PORT_LS_U0;
 			/* The power session apparently survived the resume.
 			 * If there was an overcurrent or suspend change
-			 * (i.e., remote wakeup request), have khubd
+			 * (i.e., remote wakeup request), have hub_wq
 			 * take care of it.  Look at the port link state
 			 * for USB 3.0 hubs, since they don't have a suspend
 			 * change bit, and they don't set the port link change
@@ -1201,7 +1208,7 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 				set_bit(port1, hub->change_bits);
 
 		} else {
-			/* The power session is gone; tell khubd */
+			/* The power session is gone; tell hub_wq */
 			usb_set_device_state(udev, USB_STATE_NOTATTACHED);
 			set_bit(port1, hub->change_bits);
 		}
@@ -1209,10 +1216,10 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 
 	/* If no port-status-change flags were set, we don't need any
 	 * debouncing.  If flags were set we can try to debounce the
-	 * ports all at once right now, instead of letting khubd do them
+	 * ports all at once right now, instead of letting hub_wq do them
 	 * one at a time later on.
 	 *
-	 * If any port-status changes do occur during this delay, khubd
+	 * If any port-status changes do occur during this delay, hub_wq
 	 * will see them later and handle them normally.
 	 */
 	if (need_debounce_delay) {
@@ -1240,7 +1247,7 @@ static void hub_activate(struct usb_hub *hub, enum hub_activation_type type)
 				&hub->leds, LED_CYCLE_PERIOD);
 
 	/* Scan all ports that need attention */
-	kick_khubd(hub);
+	kick_hub_wq(hub);
 
 	/* Allow autosuspend if it was suppressed */
 	if (type <= HUB_INIT3)
@@ -1273,7 +1280,7 @@ static void hub_quiesce(struct usb_hub *hub, enum hub_quiescing_type type)
 
 	cancel_delayed_work_sync(&hub->init_work);
 
-	/* khubd and related activity won't re-trigger */
+	/* hub_wq and related activity won't re-trigger */
 	hub->quiescing = 1;
 
 	if (type != HUB_SUSPEND) {
@@ -1284,7 +1291,7 @@ static void hub_quiesce(struct usb_hub *hub, enum hub_quiescing_type type)
 		}
 	}
 
-	/* Stop khubd and related activity */
+	/* Stop hub_wq and related activity */
 	usb_kill_urb(hub->urb);
 	if (hub->has_indicators)
 		cancel_delayed_work_sync(&hub->leds);
@@ -1606,7 +1613,7 @@ static int hub_configure(struct usb_hub *hub,
 	if (ret < 0)
 		goto fail;
 
-	/* Update the HCD's internal representation of this hub before khubd
+	/* Update the HCD's internal representation of this hub before hub_wq
 	 * starts getting port status changes for devices under the hub.
 	 */
 	if (hcd->driver->update_hub_device) {
@@ -1634,6 +1641,7 @@ static void hub_release(struct kref *kref)
 {
 	struct usb_hub *hub = container_of(kref, struct usb_hub, kref);
 
+	usb_put_dev(hub->hdev);
 	usb_put_intf(to_usb_interface(hub->intfdev));
 	kfree(hub);
 }
@@ -1646,14 +1654,11 @@ static void hub_disconnect(struct usb_interface *intf)
 	struct usb_device *hdev = interface_to_usbdev(intf);
 	int port1;
 
-	/* Take the hub off the event list and don't let it be added again */
-	spin_lock_irq(&hub_event_lock);
-	if (!list_empty(&hub->event_list)) {
-		list_del_init(&hub->event_list);
-		usb_autopm_put_interface_no_suspend(intf);
-	}
+	/*
+	 * Stop adding new hub events. We do not want to block here and thus
+	 * will not try to remove any pending work item.
+	 */
 	hub->disconnected = 1;
-	spin_unlock_irq(&hub_event_lock);
 
 	/* Disconnect all children and quiesce the hub */
 	hub->error = 0;
@@ -1728,8 +1733,14 @@ static int hub_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	 * - Change autosuspend delay of hub can avoid unnecessary auto
 	 *   suspend timer for hub, also may decrease power consumption
 	 *   of USB bus.
+	 *
+	 * - If user has indicated to prevent autosuspend by passing
+	 *   usbcore.autosuspend = -1 then keep autosuspend disabled.
 	 */
-	pm_runtime_set_autosuspend_delay(&hdev->dev, 0);
+#ifdef CONFIG_PM_RUNTIME
+	if (hdev->dev.power.autosuspend_delay >= 0)
+		pm_runtime_set_autosuspend_delay(&hdev->dev, 0);
+#endif
 
 	/*
 	 * Hubs have proper suspend/resume support, except for root hubs
@@ -1787,12 +1798,13 @@ descriptor_error:
 	}
 
 	kref_init(&hub->kref);
-	INIT_LIST_HEAD(&hub->event_list);
 	hub->intfdev = &intf->dev;
 	hub->hdev = hdev;
 	INIT_DELAYED_WORK(&hub->leds, led_work);
 	INIT_DELAYED_WORK(&hub->init_work, NULL);
+	INIT_WORK(&hub->events, hub_event);
 	usb_get_intf(intf);
+	usb_get_dev(hdev);
 
 	usb_set_intfdata (intf, hub);
 	intf->needs_remote_wakeup = 1;
@@ -1977,8 +1989,10 @@ void usb_set_device_state(struct usb_device *udev,
 					|| new_state == USB_STATE_SUSPENDED)
 				;	/* No change to wakeup settings */
 			else if (new_state == USB_STATE_CONFIGURED)
-				wakeup = udev->actconfig->desc.bmAttributes
-					 & USB_CONFIG_ATT_WAKEUP;
+				wakeup = (udev->quirks &
+					USB_QUIRK_IGNORE_REMOTE_WAKEUP) ? 0 :
+					udev->actconfig->desc.bmAttributes &
+					USB_CONFIG_ATT_WAKEUP;
 			else
 				wakeup = 0;
 		}
@@ -2031,7 +2045,8 @@ static void choose_devnum(struct usb_device *udev)
 	int		devnum;
 	struct usb_bus	*bus = udev->bus;
 
-	/* If khubd ever becomes multithreaded, this will need a lock */
+	/* be safe when more hub events are proceed in parallel */
+	mutex_lock(&bus->usb_address0_mutex);
 	if (udev->wusb) {
 		devnum = udev->portnum + 1;
 		BUG_ON(test_bit(devnum, bus->devmap.devicemap));
@@ -2049,6 +2064,7 @@ static void choose_devnum(struct usb_device *udev)
 		set_bit(devnum, bus->devmap.devicemap);
 		udev->devnum = devnum;
 	}
+	mutex_unlock(&bus->usb_address0_mutex);
 }
 
 static void release_devnum(struct usb_device *udev)
@@ -2107,8 +2123,8 @@ void usb_disconnect(struct usb_device **pdev)
 {
 	struct usb_port *port_dev = NULL;
 	struct usb_device *udev = *pdev;
-	struct usb_hub *hub;
-	int port1;
+	struct usb_hub *hub = NULL;
+	int port1 = 1;
 
 	/* mark the device as inactive, so any further urb submissions for
 	 * this device (and any of its children) will fail immediately.
@@ -2199,9 +2215,6 @@ static void announce_device(struct usb_device *udev)
 static inline void announce_device(struct usb_device *udev) { }
 #endif
 
-#ifdef	CONFIG_USB_OTG
-#include "otg_whitelist.h"
-#endif
 
 /**
  * usb_enumerate_device_otg - FIXME (usbcore-internal)
@@ -2261,21 +2274,6 @@ static int usb_enumerate_device_otg(struct usb_device *udev)
 			}
 		}
 	}
-
-	if (!is_targeted(udev)) {
-
-		/* Maybe it can talk to us, though we can't talk to it.
-		 * (Includes HNP test device.)
-		 */
-		if (udev->bus->b_hnp_enable || udev->bus->is_b_host) {
-			err = usb_port_suspend(udev, PMSG_SUSPEND);
-			if (err < 0)
-				dev_dbg(&udev->dev, "HNP fail, %d\n", err);
-		}
-		err = -ENOTSUPP;
-		goto fail;
-	}
-fail:
 #endif
 	return err;
 }
@@ -2298,6 +2296,7 @@ fail:
 static int usb_enumerate_device(struct usb_device *udev)
 {
 	int err;
+	struct usb_hcd *hcd = bus_to_hcd(udev->bus);
 
 	if (udev->config == NULL) {
 		err = usb_get_configuration(udev);
@@ -2319,6 +2318,20 @@ static int usb_enumerate_device(struct usb_device *udev)
 	if (err < 0)
 		return err;
 
+	if (IS_ENABLED(CONFIG_USB_OTG_WHITELIST) && hcd->tpl_support &&
+		!is_targeted(udev)) {
+		/* Maybe it can talk to us, though we can't talk to it.
+		 * (Includes HNP test device.)
+		 */
+		if (IS_ENABLED(CONFIG_USB_OTG) && (udev->bus->b_hnp_enable
+			|| udev->bus->is_b_host)) {
+			err = usb_port_suspend(udev, PMSG_AUTO_SUSPEND);
+			if (err < 0)
+				dev_dbg(&udev->dev, "HNP fail, %d\n", err);
+		}
+		return -ENOTSUPP;
+	}
+
 	usb_detect_interface_quirks(udev);
 
 	return 0;
@@ -3064,7 +3077,7 @@ static unsigned wakeup_enabled_descendants(struct usb_device *udev)
  * Once VBUS drop breaks the circuit, the port it's using has to go through
  * normal re-enumeration procedures, starting with enabling VBUS power.
  * Other than re-initializing the hub (plug/unplug, except for root hubs),
- * Linux (2.6) currently has NO mechanisms to initiate that:  no khubd
+ * Linux (2.6) currently has NO mechanisms to initiate that:  no hub_wq
  * timer, no SRP, no requests through sysfs.
  *
  * If Runtime PM isn't enabled or used, non-SuperSpeed devices may not get
@@ -3206,7 +3219,7 @@ static int finish_port_resume(struct usb_device *udev)
 	/* usb ch9 identifies four variants of SUSPENDED, based on what
 	 * state the device resumes to.  Linux currently won't see the
 	 * first two on the host side; they'd be inside hub_port_init()
-	 * during many timeouts, but khubd can't suspend until later.
+	 * during many timeouts, but hub_wq can't suspend until later.
 	 */
 	usb_set_device_state(udev, udev->actconfig
 			? USB_STATE_CONFIGURED
@@ -3571,7 +3584,7 @@ static int hub_suspend(struct usb_interface *intf, pm_message_t msg)
 
 	dev_dbg(&intf->dev, "%s\n", __func__);
 
-	/* stop khubd and related activity */
+	/* stop hub_wq and related activity */
 	hub_quiesce(hub, HUB_SUSPEND);
 	return 0;
 }
@@ -4455,8 +4468,8 @@ hub_port_init (struct usb_hub *hub, struct usb_device *udev, int port1,
 	if (retval)
 		goto fail;
 
-	if (hcd->phy && !hdev->parent)
-		usb_phy_notify_connect(hcd->phy, udev->speed);
+	if (hcd->usb_phy && !hdev->parent)
+		usb_phy_notify_connect(hcd->usb_phy, udev->speed);
 
 	/*
 	 * Some superspeed devices have finished the link training process
@@ -4532,6 +4545,9 @@ check_highspeed (struct usb_hub *hub, struct usb_device *udev, int port1)
 	struct usb_qualifier_descriptor	*qual;
 	int				status;
 
+	if (udev->quirks & USB_QUIRK_DEVICE_QUALIFIER)
+		return;
+
 	qual = kmalloc (sizeof *qual, GFP_KERNEL);
 	if (qual == NULL)
 		return;
@@ -4611,9 +4627,9 @@ static void hub_port_connect(struct usb_hub *hub, int port1, u16 portstatus,
 
 	/* Disconnect any existing devices under this port */
 	if (udev) {
-		if (hcd->phy && !hdev->parent &&
+		if (hcd->usb_phy && !hdev->parent &&
 				!(portstatus & USB_PORT_STAT_CONNECTION))
-			usb_phy_notify_disconnect(hcd->phy, udev->speed);
+			usb_phy_notify_disconnect(hcd->usb_phy, udev->speed);
 		usb_disconnect(&port_dev->child);
 	}
 
@@ -4631,9 +4647,7 @@ static void hub_port_connect(struct usb_hub *hub, int port1, u16 portstatus,
 			if (status != -ENODEV &&
 				port1 != unreliable_port &&
 				printk_ratelimit())
-				dev_err(&udev->dev, "connect-debounce failed, port %d disabled\n",
-					port1);
-
+				dev_err(&port_dev->dev, "connect-debounce failed\n");
 			portstatus &= ~USB_PORT_STAT_CONNECTION;
 			unreliable_port = port1;
 		} else {
@@ -4966,10 +4980,10 @@ static void port_event(struct usb_hub *hub, int port1)
 	 * On disconnect USB3 protocol ports transit from U0 to
 	 * SS.Inactive to Rx.Detect. If this happens a warm-
 	 * reset is not needed, but a (re)connect may happen
-	 * before khubd runs and sees the disconnect, and the
+	 * before hub_wq runs and sees the disconnect, and the
 	 * device may be an unknown state.
 	 *
-	 * If the port went through SS.Inactive without khubd
+	 * If the port went through SS.Inactive without hub_wq
 	 * seeing it the C_LINK_STATE change flag will be set,
 	 * and we reset the dev to put it in a known state.
 	 */
@@ -4988,10 +5002,8 @@ static void port_event(struct usb_hub *hub, int port1)
 		hub_port_connect_change(hub, port1, portstatus, portchange);
 }
 
-
-static void hub_events(void)
+static void hub_event(struct work_struct *work)
 {
-	struct list_head *tmp;
 	struct usb_device *hdev;
 	struct usb_interface *intf;
 	struct usb_hub *hub;
@@ -5000,164 +5012,117 @@ static void hub_events(void)
 	u16 hubchange;
 	int i, ret;
 
-	/*
-	 *  We restart the list every time to avoid a deadlock with
-	 * deleting hubs downstream from this one. This should be
-	 * safe since we delete the hub from the event list.
-	 * Not the most efficient, but avoids deadlocks.
-	 */
-	while (1) {
+	hub = container_of(work, struct usb_hub, events);
+	hdev = hub->hdev;
+	hub_dev = hub->intfdev;
+	intf = to_usb_interface(hub_dev);
+
+	dev_dbg(hub_dev, "state %d ports %d chg %04x evt %04x\n",
+			hdev->state, hdev->maxchild,
+			/* NOTE: expects max 15 ports... */
+			(u16) hub->change_bits[0],
+			(u16) hub->event_bits[0]);
+
+	/* Lock the device, then check to see if we were
+	 * disconnected while waiting for the lock to succeed. */
+	usb_lock_device(hdev);
+	if (unlikely(hub->disconnected))
+		goto out_hdev_lock;
+
+	/* If the hub has died, clean up after it */
+	if (hdev->state == USB_STATE_NOTATTACHED) {
+		hub->error = -ENODEV;
+		hub_quiesce(hub, HUB_DISCONNECT);
+		goto out_hdev_lock;
+	}
+
+	/* Autoresume */
+	ret = usb_autopm_get_interface(intf);
+	if (ret) {
+		dev_dbg(hub_dev, "Can't autoresume: %d\n", ret);
+		goto out_hdev_lock;
+	}
 
-		/* Grab the first entry at the beginning of the list */
-		spin_lock_irq(&hub_event_lock);
-		if (list_empty(&hub_event_list)) {
-			spin_unlock_irq(&hub_event_lock);
-			break;
-		}
+	/* If this is an inactive hub, do nothing */
+	if (hub->quiescing)
+		goto out_autopm;
 
-		tmp = hub_event_list.next;
-		list_del_init(tmp);
-
-		hub = list_entry(tmp, struct usb_hub, event_list);
-		kref_get(&hub->kref);
-		spin_unlock_irq(&hub_event_lock);
-
-		hdev = hub->hdev;
-		hub_dev = hub->intfdev;
-		intf = to_usb_interface(hub_dev);
-		dev_dbg(hub_dev, "state %d ports %d chg %04x evt %04x\n",
-				hdev->state, hdev->maxchild,
-				/* NOTE: expects max 15 ports... */
-				(u16) hub->change_bits[0],
-				(u16) hub->event_bits[0]);
-
-		/* Lock the device, then check to see if we were
-		 * disconnected while waiting for the lock to succeed. */
-		usb_lock_device(hdev);
-		if (unlikely(hub->disconnected))
-			goto loop_disconnected;
-
-		/* If the hub has died, clean up after it */
-		if (hdev->state == USB_STATE_NOTATTACHED) {
-			hub->error = -ENODEV;
-			hub_quiesce(hub, HUB_DISCONNECT);
-			goto loop;
-		}
+	if (hub->error) {
+		dev_dbg(hub_dev, "resetting for error %d\n", hub->error);
 
-		/* Autoresume */
-		ret = usb_autopm_get_interface(intf);
+		ret = usb_reset_device(hdev);
 		if (ret) {
-			dev_dbg(hub_dev, "Can't autoresume: %d\n", ret);
-			goto loop;
+			dev_dbg(hub_dev, "error resetting hub: %d\n", ret);
+			goto out_autopm;
 		}
 
-		/* If this is an inactive hub, do nothing */
-		if (hub->quiescing)
-			goto loop_autopm;
-
-		if (hub->error) {
-			dev_dbg (hub_dev, "resetting for error %d\n",
-				hub->error);
+		hub->nerrors = 0;
+		hub->error = 0;
+	}
 
-			ret = usb_reset_device(hdev);
-			if (ret) {
-				dev_dbg (hub_dev,
-					"error resetting hub: %d\n", ret);
-				goto loop_autopm;
-			}
+	/* deal with port status changes */
+	for (i = 1; i <= hdev->maxchild; i++) {
+		struct usb_port *port_dev = hub->ports[i - 1];
 
-			hub->nerrors = 0;
-			hub->error = 0;
+		if (test_bit(i, hub->event_bits)
+				|| test_bit(i, hub->change_bits)
+				|| test_bit(i, hub->wakeup_bits)) {
+			/*
+			 * The get_noresume and barrier ensure that if
+			 * the port was in the process of resuming, we
+			 * flush that work and keep the port active for
+			 * the duration of the port_event().  However,
+			 * if the port is runtime pm suspended
+			 * (powered-off), we leave it in that state, run
+			 * an abbreviated port_event(), and move on.
+			 */
+			pm_runtime_get_noresume(&port_dev->dev);
+			pm_runtime_barrier(&port_dev->dev);
+			usb_lock_port(port_dev);
+			port_event(hub, i);
+			usb_unlock_port(port_dev);
+			pm_runtime_put_sync(&port_dev->dev);
 		}
+	}
 
-		/* deal with port status changes */
-		for (i = 1; i <= hdev->maxchild; i++) {
-			struct usb_port *port_dev = hub->ports[i - 1];
-
-			if (test_bit(i, hub->event_bits)
-					|| test_bit(i, hub->change_bits)
-					|| test_bit(i, hub->wakeup_bits)) {
-				/*
-				 * The get_noresume and barrier ensure that if
-				 * the port was in the process of resuming, we
-				 * flush that work and keep the port active for
-				 * the duration of the port_event().  However,
-				 * if the port is runtime pm suspended
-				 * (powered-off), we leave it in that state, run
-				 * an abbreviated port_event(), and move on.
-				 */
-				pm_runtime_get_noresume(&port_dev->dev);
-				pm_runtime_barrier(&port_dev->dev);
-				usb_lock_port(port_dev);
-				port_event(hub, i);
-				usb_unlock_port(port_dev);
-				pm_runtime_put_sync(&port_dev->dev);
-			}
+	/* deal with hub status changes */
+	if (test_and_clear_bit(0, hub->event_bits) == 0)
+		;	/* do nothing */
+	else if (hub_hub_status(hub, &hubstatus, &hubchange) < 0)
+		dev_err(hub_dev, "get_hub_status failed\n");
+	else {
+		if (hubchange & HUB_CHANGE_LOCAL_POWER) {
+			dev_dbg(hub_dev, "power change\n");
+			clear_hub_feature(hdev, C_HUB_LOCAL_POWER);
+			if (hubstatus & HUB_STATUS_LOCAL_POWER)
+				/* FIXME: Is this always true? */
+				hub->limited_power = 1;
+			else
+				hub->limited_power = 0;
 		}
+		if (hubchange & HUB_CHANGE_OVERCURRENT) {
+			u16 status = 0;
+			u16 unused;
 
-		/* deal with hub status changes */
-		if (test_and_clear_bit(0, hub->event_bits) == 0)
-			;	/* do nothing */
-		else if (hub_hub_status(hub, &hubstatus, &hubchange) < 0)
-			dev_err (hub_dev, "get_hub_status failed\n");
-		else {
-			if (hubchange & HUB_CHANGE_LOCAL_POWER) {
-				dev_dbg (hub_dev, "power change\n");
-				clear_hub_feature(hdev, C_HUB_LOCAL_POWER);
-				if (hubstatus & HUB_STATUS_LOCAL_POWER)
-					/* FIXME: Is this always true? */
-					hub->limited_power = 1;
-				else
-					hub->limited_power = 0;
-			}
-			if (hubchange & HUB_CHANGE_OVERCURRENT) {
-				u16 status = 0;
-				u16 unused;
-
-				dev_dbg(hub_dev, "over-current change\n");
-				clear_hub_feature(hdev, C_HUB_OVER_CURRENT);
-				msleep(500);	/* Cool down */
-				hub_power_on(hub, true);
-				hub_hub_status(hub, &status, &unused);
-				if (status & HUB_STATUS_OVERCURRENT)
-					dev_err(hub_dev, "over-current "
-						"condition\n");
-			}
+			dev_dbg(hub_dev, "over-current change\n");
+			clear_hub_feature(hdev, C_HUB_OVER_CURRENT);
+			msleep(500);	/* Cool down */
+			hub_power_on(hub, true);
+			hub_hub_status(hub, &status, &unused);
+			if (status & HUB_STATUS_OVERCURRENT)
+				dev_err(hub_dev, "over-current condition\n");
 		}
+	}
 
- loop_autopm:
-		/* Balance the usb_autopm_get_interface() above */
-		usb_autopm_put_interface_no_suspend(intf);
- loop:
-		/* Balance the usb_autopm_get_interface_no_resume() in
-		 * kick_khubd() and allow autosuspend.
-		 */
-		usb_autopm_put_interface(intf);
- loop_disconnected:
-		usb_unlock_device(hdev);
-		kref_put(&hub->kref, hub_release);
-
-	} /* end while (1) */
-}
-
-static int hub_thread(void *__unused)
-{
-	/* khubd needs to be freezable to avoid interfering with USB-PERSIST
-	 * port handover.  Otherwise it might see that a full-speed device
-	 * was gone before the EHCI controller had handed its port over to
-	 * the companion full-speed controller.
-	 */
-	set_freezable();
-
-	do {
-		hub_events();
-		wait_event_freezable(khubd_wait,
-				!list_empty(&hub_event_list) ||
-				kthread_should_stop());
-	} while (!kthread_should_stop() || !list_empty(&hub_event_list));
+out_autopm:
+	/* Balance the usb_autopm_get_interface() above */
+	usb_autopm_put_interface_no_suspend(intf);
+out_hdev_lock:
+	usb_unlock_device(hdev);
 
-	pr_debug("%s: khubd exiting\n", usbcore_name);
-	return 0;
+	/* Balance the stuff in kick_hub_wq() and allow autosuspend */
+	usb_autopm_put_interface(intf);
+	kref_put(&hub->kref, hub_release);
 }
 
 static const struct usb_device_id hub_id_table[] = {
@@ -5197,20 +5162,26 @@ int usb_hub_init(void)
 		return -1;
 	}
 
-	khubd_task = kthread_run(hub_thread, NULL, "khubd");
-	if (!IS_ERR(khubd_task))
+	/*
+	 * The workqueue needs to be freezable to avoid interfering with
+	 * USB-PERSIST port handover. Otherwise it might see that a full-speed
+	 * device was gone before the EHCI controller had handed its port
+	 * over to the companion full-speed controller.
+	 */
+	hub_wq = alloc_workqueue("usb_hub_wq", WQ_FREEZABLE, 0);
+	if (hub_wq)
 		return 0;
 
 	/* Fall through if kernel_thread failed */
 	usb_deregister(&hub_driver);
-	printk(KERN_ERR "%s: can't start khubd\n", usbcore_name);
+	pr_err("%s: can't allocate workqueue for usb hub\n", usbcore_name);
 
 	return -1;
 }
 
 void usb_hub_cleanup(void)
 {
-	kthread_stop(khubd_task);
+	destroy_workqueue(hub_wq);
 
 	/*
 	 * Hub resources are freed for us by usb_deregister. It calls
@@ -5319,7 +5290,7 @@ static int descriptors_changed(struct usb_device *udev,
  * former operating configuration.  If the reset fails, or the device's
  * descriptors change from their values before the reset, or the original
  * configuration and altsettings cannot be restored, a flag will be set
- * telling khubd to pretend the device has been disconnected and then
+ * telling hub_wq to pretend the device has been disconnected and then
  * re-connected.  All drivers will be unbound, and the device will be
  * re-enumerated and probed all over again.
  *
diff --git a/drivers/usb/core/hub.h b/drivers/usb/core/hub.h
index c77d8778af4b..688817fb3246 100644
--- a/drivers/usb/core/hub.h
+++ b/drivers/usb/core/hub.h
@@ -41,7 +41,6 @@ struct usb_hub {
 	int			error;		/* last reported error */
 	int			nerrors;	/* track consecutive errors */
 
-	struct list_head	event_list;	/* hubs w/data or errs ready */
 	unsigned long		event_bits[1];	/* status change bitmask */
 	unsigned long		change_bits[1];	/* ports with logical connect
 							status change */
@@ -77,6 +76,7 @@ struct usb_hub {
 	u8			indicator[USB_MAXCHILDREN];
 	struct delayed_work	leds;
 	struct delayed_work	init_work;
+	struct work_struct      events;
 	struct usb_port		**ports;
 };
 
diff --git a/drivers/usb/core/message.c b/drivers/usb/core/message.c
index 0c8a7fc4dad8..f7b7713cfb2a 100644
--- a/drivers/usb/core/message.c
+++ b/drivers/usb/core/message.c
@@ -770,9 +770,7 @@ static int usb_get_langid(struct usb_device *dev, unsigned char *tbuf)
 		dev->string_langid = 0x0409;
 		dev->have_langid = 1;
 		dev_err(&dev->dev,
-			"string descriptor 0 malformed (err = %d), "
-			"defaulting to 0x%04x\n",
-				err, dev->string_langid);
+			"language id specifier not provided by device, defaulting to English\n");
 		return 0;
 	}
 
diff --git a/drivers/usb/core/otg_whitelist.h b/drivers/usb/core/otg_whitelist.h
index e8cdce571bb1..de0c9c9d7091 100644
--- a/drivers/usb/core/otg_whitelist.h
+++ b/drivers/usb/core/otg_whitelist.h
@@ -10,8 +10,8 @@
  */
 
 /*
- * This OTG Whitelist is the OTG "Targeted Peripheral List".  It should
- * mostly use of USB_DEVICE() or USB_DEVICE_VER() entries..
+ * This OTG and Embedded Host Whitelist is "Targeted Peripheral List".
+ * It should mostly use of USB_DEVICE() or USB_DEVICE_VER() entries..
  *
  * YOU _SHOULD_ CHANGE THIS LIST TO MATCH YOUR PRODUCT AND ITS TESTING!
  */
@@ -50,10 +50,6 @@ static int is_targeted(struct usb_device *dev)
 {
 	struct usb_device_id	*id = whitelist_table;
 
-	/* possible in developer configs only! */
-	if (!dev->bus->otg_port)
-		return 1;
-
 	/* HNP test device is _never_ targeted (see OTG spec 6.6.6) */
 	if ((le16_to_cpu(dev->descriptor.idVendor) == 0x1a0a &&
 	     le16_to_cpu(dev->descriptor.idProduct) == 0xbadd))
@@ -103,10 +99,7 @@ static int is_targeted(struct usb_device *dev)
 	dev_err(&dev->dev, "device v%04x p%04x is not supported\n",
 		le16_to_cpu(dev->descriptor.idVendor),
 		le16_to_cpu(dev->descriptor.idProduct));
-#ifdef	CONFIG_USB_OTG_WHITELIST
+
 	return 0;
-#else
-	return 1;
-#endif
 }
 
diff --git a/drivers/usb/core/quirks.c b/drivers/usb/core/quirks.c
index bae636e2a1a3..5ae883dc21f5 100644
--- a/drivers/usb/core/quirks.c
+++ b/drivers/usb/core/quirks.c
@@ -93,6 +93,10 @@ static const struct usb_device_id usb_quirk_list[] = {
 	{ USB_DEVICE(0x04e8, 0x6601), .driver_info =
 			USB_QUIRK_CONFIG_INTF_STRINGS },
 
+	/* Elan Touchscreen */
+	{ USB_DEVICE(0x04f3, 0x0089), .driver_info =
+			USB_QUIRK_DEVICE_QUALIFIER },
+
 	/* Roland SC-8820 */
 	{ USB_DEVICE(0x0582, 0x0007), .driver_info = USB_QUIRK_RESET_RESUME },
 
@@ -159,6 +163,10 @@ static const struct usb_device_id usb_quirk_list[] = {
 	/* USB3503 */
 	{ USB_DEVICE(0x0424, 0x3503), .driver_info = USB_QUIRK_RESET_RESUME },
 
+	/* ASUS Base Station(T100) */
+	{ USB_DEVICE(0x0b05, 0x17e0), .driver_info =
+			USB_QUIRK_IGNORE_REMOTE_WAKEUP },
+
 	{ }  /* terminating entry must be last */
 };
 
diff --git a/drivers/usb/core/usb.h b/drivers/usb/core/usb.h
index d9d08720c386..b1b34d0557c9 100644
--- a/drivers/usb/core/usb.h
+++ b/drivers/usb/core/usb.h
@@ -48,7 +48,7 @@ static inline unsigned usb_get_max_power(struct usb_device *udev,
 	return c->desc.bMaxPower * mul;
 }
 
-extern void usb_kick_khubd(struct usb_device *dev);
+extern void usb_kick_hub_wq(struct usb_device *dev);
 extern int usb_match_one_id_intf(struct usb_device *dev,
 				 struct usb_host_interface *intf,
 				 const struct usb_device_id *id);
diff --git a/drivers/usb/dwc2/core.c b/drivers/usb/dwc2/core.c
index 27d2c9b8a034..d9269459d481 100644
--- a/drivers/usb/dwc2/core.c
+++ b/drivers/usb/dwc2/core.c
@@ -118,6 +118,7 @@ static int dwc2_core_reset(struct dwc2_hsotg *hsotg)
 {
 	u32 greset;
 	int count = 0;
+	u32 gusbcfg;
 
 	dev_vdbg(hsotg->dev, "%s()\n", __func__);
 
@@ -148,6 +149,23 @@ static int dwc2_core_reset(struct dwc2_hsotg *hsotg)
 		}
 	} while (greset & GRSTCTL_CSFTRST);
 
+	if (hsotg->dr_mode == USB_DR_MODE_HOST) {
+		gusbcfg = readl(hsotg->regs + GUSBCFG);
+		gusbcfg &= ~GUSBCFG_FORCEDEVMODE;
+		gusbcfg |= GUSBCFG_FORCEHOSTMODE;
+		writel(gusbcfg, hsotg->regs + GUSBCFG);
+	} else if (hsotg->dr_mode == USB_DR_MODE_PERIPHERAL) {
+		gusbcfg = readl(hsotg->regs + GUSBCFG);
+		gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
+		gusbcfg |= GUSBCFG_FORCEDEVMODE;
+		writel(gusbcfg, hsotg->regs + GUSBCFG);
+	} else if (hsotg->dr_mode == USB_DR_MODE_OTG) {
+		gusbcfg = readl(hsotg->regs + GUSBCFG);
+		gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
+		gusbcfg &= ~GUSBCFG_FORCEDEVMODE;
+		writel(gusbcfg, hsotg->regs + GUSBCFG);
+	}
+
 	/*
 	 * NOTE: This long sleep is _very_ important, otherwise the core will
 	 * not stay in host mode after a connector ID change!
@@ -2674,23 +2692,23 @@ int dwc2_get_hwparams(struct dwc2_hsotg *hsotg)
 	hwcfg2 = readl(hsotg->regs + GHWCFG2);
 	hwcfg3 = readl(hsotg->regs + GHWCFG3);
 	hwcfg4 = readl(hsotg->regs + GHWCFG4);
-	gnptxfsiz = readl(hsotg->regs + GNPTXFSIZ);
 	grxfsiz = readl(hsotg->regs + GRXFSIZ);
 
 	dev_dbg(hsotg->dev, "hwcfg1=%08x\n", hwcfg1);
 	dev_dbg(hsotg->dev, "hwcfg2=%08x\n", hwcfg2);
 	dev_dbg(hsotg->dev, "hwcfg3=%08x\n", hwcfg3);
 	dev_dbg(hsotg->dev, "hwcfg4=%08x\n", hwcfg4);
-	dev_dbg(hsotg->dev, "gnptxfsiz=%08x\n", gnptxfsiz);
 	dev_dbg(hsotg->dev, "grxfsiz=%08x\n", grxfsiz);
 
-	/* Force host mode to get HPTXFSIZ exact power on value */
+	/* Force host mode to get HPTXFSIZ / GNPTXFSIZ exact power on value */
 	gusbcfg = readl(hsotg->regs + GUSBCFG);
 	gusbcfg |= GUSBCFG_FORCEHOSTMODE;
 	writel(gusbcfg, hsotg->regs + GUSBCFG);
 	usleep_range(100000, 150000);
 
+	gnptxfsiz = readl(hsotg->regs + GNPTXFSIZ);
 	hptxfsiz = readl(hsotg->regs + HPTXFSIZ);
+	dev_dbg(hsotg->dev, "gnptxfsiz=%08x\n", gnptxfsiz);
 	dev_dbg(hsotg->dev, "hptxfsiz=%08x\n", hptxfsiz);
 	gusbcfg = readl(hsotg->regs + GUSBCFG);
 	gusbcfg &= ~GUSBCFG_FORCEHOSTMODE;
@@ -2725,6 +2743,13 @@ int dwc2_get_hwparams(struct dwc2_hsotg *hsotg)
 	width = (hwcfg3 & GHWCFG3_XFER_SIZE_CNTR_WIDTH_MASK) >>
 		GHWCFG3_XFER_SIZE_CNTR_WIDTH_SHIFT;
 	hw->max_transfer_size = (1 << (width + 11)) - 1;
+	/*
+	 * Clip max_transfer_size to 65535. dwc2_hc_setup_align_buf() allocates
+	 * coherent buffers with this size, and if it's too large we can
+	 * exhaust the coherent DMA pool.
+	 */
+	if (hw->max_transfer_size > 65535)
+		hw->max_transfer_size = 65535;
 	width = (hwcfg3 & GHWCFG3_PACKET_SIZE_CNTR_WIDTH_MASK) >>
 		GHWCFG3_PACKET_SIZE_CNTR_WIDTH_SHIFT;
 	hw->max_packet_count = (1 << (width + 4)) - 1;
diff --git a/drivers/usb/dwc2/core.h b/drivers/usb/dwc2/core.h
index 1efd10cc9629..bf015ab3b44c 100644
--- a/drivers/usb/dwc2/core.h
+++ b/drivers/usb/dwc2/core.h
@@ -139,6 +139,7 @@ struct s3c_hsotg_ep {
 	unsigned int            last_load;
 	unsigned int            fifo_load;
 	unsigned short          fifo_size;
+	unsigned short		fifo_index;
 
 	unsigned char           dir_in;
 	unsigned char           index;
@@ -194,8 +195,10 @@ struct s3c_hsotg {
 	struct regulator_bulk_data supplies[ARRAY_SIZE(s3c_hsotg_supply_names)];
 
 	u32                     phyif;
+	int			fifo_mem;
 	unsigned int            dedicated_fifos:1;
 	unsigned char           num_of_eps;
+	u32			fifo_map;
 
 	struct dentry           *debug_root;
 	struct dentry           *debug_file;
@@ -501,6 +504,10 @@ struct dwc2_hw_params {
  *                      a_peripheral and b_device=>b_host) this may not match
  *                      the core, but allows the software to determine
  *                      transitions
+ * @dr_mode:            Requested mode of operation, one of following:
+ *                      - USB_DR_MODE_PERIPHERAL
+ *                      - USB_DR_MODE_HOST
+ *                      - USB_DR_MODE_OTG
  * @queuing_high_bandwidth: True if multiple packets of a high-bandwidth
  *                      transfer are in process of being queued
  * @srp_success:        Stores status of SRP request in the case of a FS PHY
@@ -592,6 +599,7 @@ struct dwc2_hsotg {
 	/** Params to actually use */
 	struct dwc2_core_params *core_params;
 	enum usb_otg_state op_state;
+	enum usb_dr_mode dr_mode;
 
 	unsigned int queuing_high_bandwidth:1;
 	unsigned int srp_success:1;
diff --git a/drivers/usb/dwc2/gadget.c b/drivers/usb/dwc2/gadget.c
index 0ba9c335b584..7b5856fadd93 100644
--- a/drivers/usb/dwc2/gadget.c
+++ b/drivers/usb/dwc2/gadget.c
@@ -182,16 +182,33 @@ static void s3c_hsotg_init_fifo(struct s3c_hsotg *hsotg)
 
 	/* start at the end of the GNPTXFSIZ, rounded up */
 	addr = 2048 + 1024;
-	size = 768;
 
 	/*
-	 * currently we allocate TX FIFOs for all possible endpoints,
-	 * and assume that they are all the same size.
+	 * Because we have not enough memory to have each TX FIFO of size at
+	 * least 3072 bytes (the maximum single packet size), we create four
+	 * FIFOs of lenght 1024, and four of length 3072 bytes, and assing
+	 * them to endpoints dynamically according to maxpacket size value of
+	 * given endpoint.
 	 */
 
-	for (ep = 1; ep <= 15; ep++) {
+	/* 256*4=1024 bytes FIFO length */
+	size = 256;
+	for (ep = 1; ep <= 4; ep++) {
+		val = addr;
+		val |= size << FIFOSIZE_DEPTH_SHIFT;
+		WARN_ONCE(addr + size > hsotg->fifo_mem,
+			  "insufficient fifo memory");
+		addr += size;
+
+		writel(val, hsotg->regs + DPTXFSIZN(ep));
+	}
+	/* 768*4=3072 bytes FIFO length */
+	size = 768;
+	for (ep = 5; ep <= 8; ep++) {
 		val = addr;
 		val |= size << FIFOSIZE_DEPTH_SHIFT;
+		WARN_ONCE(addr + size > hsotg->fifo_mem,
+			  "insufficient fifo memory");
 		addr += size;
 
 		writel(val, hsotg->regs + DPTXFSIZN(ep));
@@ -987,8 +1004,8 @@ static int s3c_hsotg_process_req_feature(struct s3c_hsotg *hsotg,
 					hs_req = ep->req;
 					ep->req = NULL;
 					list_del_init(&hs_req->queue);
-					hs_req->req.complete(&ep->ep,
-							     &hs_req->req);
+					usb_gadget_giveback_request(&ep->ep,
+								    &hs_req->req);
 				}
 
 				/* If we have pending request, then start it */
@@ -1245,7 +1262,7 @@ static void s3c_hsotg_complete_request(struct s3c_hsotg *hsotg,
 
 	if (hs_req->req.complete) {
 		spin_unlock(&hsotg->lock);
-		hs_req->req.complete(&hs_ep->ep, &hs_req->req);
+		usb_gadget_giveback_request(&hs_ep->ep, &hs_req->req);
 		spin_lock(&hsotg->lock);
 	}
 
@@ -1649,6 +1666,7 @@ static void s3c_hsotg_txfifo_flush(struct s3c_hsotg *hsotg, unsigned int idx)
 			dev_err(hsotg->dev,
 				"%s: timeout flushing fifo (GRSTCTL=%08x)\n",
 				__func__, val);
+			break;
 		}
 
 		udelay(1);
@@ -1831,7 +1849,7 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 		if (dir_in) {
 			int epctl = readl(hsotg->regs + epctl_reg);
 
-			s3c_hsotg_txfifo_flush(hsotg, idx);
+			s3c_hsotg_txfifo_flush(hsotg, hs_ep->fifo_index);
 
 			if ((epctl & DXEPCTL_STALL) &&
 				(epctl & DXEPCTL_EPTYPE_BULK)) {
@@ -1901,7 +1919,7 @@ static void s3c_hsotg_epint(struct s3c_hsotg *hsotg, unsigned int idx,
 static void s3c_hsotg_irq_enumdone(struct s3c_hsotg *hsotg)
 {
 	u32 dsts = readl(hsotg->regs + DSTS);
-	int ep0_mps = 0, ep_mps;
+	int ep0_mps = 0, ep_mps = 8;
 
 	/*
 	 * This should signal the finish of the enumeration phase
@@ -1980,6 +1998,7 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 			      int result, bool force)
 {
 	struct s3c_hsotg_req *req, *treq;
+	unsigned size;
 
 	list_for_each_entry_safe(req, treq, &ep->queue, queue) {
 		/*
@@ -1993,9 +2012,11 @@ static void kill_all_requests(struct s3c_hsotg *hsotg,
 		s3c_hsotg_complete_request(hsotg, ep, req,
 					   result);
 	}
-	if (hsotg->dedicated_fifos)
-		if ((readl(hsotg->regs + DTXFSTS(ep->index)) & 0xffff) * 4 < 3072)
-			s3c_hsotg_txfifo_flush(hsotg, ep->index);
+	if (!hsotg->dedicated_fifos)
+		return;
+	size = (readl(hsotg->regs + DTXFSTS(ep->index)) & 0xffff) * 4;
+	if (size < ep->fifo_size)
+		s3c_hsotg_txfifo_flush(hsotg, ep->fifo_index);
 }
 
 /**
@@ -2436,6 +2457,7 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	u32 epctrl;
 	u32 mps;
 	int dir_in;
+	int i, val, size;
 	int ret = 0;
 
 	dev_dbg(hsotg->dev,
@@ -2508,17 +2530,8 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 		break;
 
 	case USB_ENDPOINT_XFER_INT:
-		if (dir_in) {
-			/*
-			 * Allocate our TxFNum by simply using the index
-			 * of the endpoint for the moment. We could do
-			 * something better if the host indicates how
-			 * many FIFOs we are expecting to use.
-			 */
-
+		if (dir_in)
 			hs_ep->periodic = 1;
-			epctrl |= DXEPCTL_TXFNUM(index);
-		}
 
 		epctrl |= DXEPCTL_EPTYPE_INTERRUPT;
 		break;
@@ -2532,8 +2545,25 @@ static int s3c_hsotg_ep_enable(struct usb_ep *ep,
 	 * if the hardware has dedicated fifos, we must give each IN EP
 	 * a unique tx-fifo even if it is non-periodic.
 	 */
-	if (dir_in && hsotg->dedicated_fifos)
-		epctrl |= DXEPCTL_TXFNUM(index);
+	if (dir_in && hsotg->dedicated_fifos) {
+		size = hs_ep->ep.maxpacket*hs_ep->mc;
+		for (i = 1; i <= 8; ++i) {
+			if (hsotg->fifo_map & (1<<i))
+				continue;
+			val = readl(hsotg->regs + DPTXFSIZN(i));
+			val = (val >> FIFOSIZE_DEPTH_SHIFT)*4;
+			if (val < size)
+				continue;
+			hsotg->fifo_map |= 1<<i;
+
+			epctrl |= DXEPCTL_TXFNUM(i);
+			hs_ep->fifo_index = i;
+			hs_ep->fifo_size = val;
+			break;
+		}
+		if (i == 8)
+			return -ENOMEM;
+	}
 
 	/* for non control endpoints, set PID to D0 */
 	if (index)
@@ -2567,7 +2597,7 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 	u32 epctrl_reg;
 	u32 ctrl;
 
-	dev_info(hsotg->dev, "%s(ep %p)\n", __func__, ep);
+	dev_dbg(hsotg->dev, "%s(ep %p)\n", __func__, ep);
 
 	if (ep == &hsotg->eps[0].ep) {
 		dev_err(hsotg->dev, "%s: called for ep0\n", __func__);
@@ -2580,6 +2610,9 @@ static int s3c_hsotg_ep_disable(struct usb_ep *ep)
 	/* terminate all requests with shutdown */
 	kill_all_requests(hsotg, hs_ep, -ESHUTDOWN, false);
 
+	hsotg->fifo_map &= ~(1<<hs_ep->fifo_index);
+	hs_ep->fifo_index = 0;
+	hs_ep->fifo_size = 0;
 
 	ctrl = readl(hsotg->regs + epctrl_reg);
 	ctrl &= ~DXEPCTL_EPENA;
@@ -2625,7 +2658,7 @@ static int s3c_hsotg_ep_dequeue(struct usb_ep *ep, struct usb_request *req)
 	struct s3c_hsotg *hs = hs_ep->parent;
 	unsigned long flags;
 
-	dev_info(hs->dev, "ep_dequeue(%p,%p)\n", ep, req);
+	dev_dbg(hs->dev, "ep_dequeue(%p,%p)\n", ep, req);
 
 	spin_lock_irqsave(&hs->lock, flags);
 
@@ -2747,13 +2780,14 @@ static void s3c_hsotg_phy_enable(struct s3c_hsotg *hsotg)
 
 	dev_dbg(hsotg->dev, "pdev 0x%p\n", pdev);
 
-	if (hsotg->phy) {
-		phy_init(hsotg->phy);
-		phy_power_on(hsotg->phy);
-	} else if (hsotg->uphy)
+	if (hsotg->uphy)
 		usb_phy_init(hsotg->uphy);
-	else if (hsotg->plat->phy_init)
+	else if (hsotg->plat && hsotg->plat->phy_init)
 		hsotg->plat->phy_init(pdev, hsotg->plat->phy_type);
+	else {
+		phy_init(hsotg->phy);
+		phy_power_on(hsotg->phy);
+	}
 }
 
 /**
@@ -2767,13 +2801,14 @@ static void s3c_hsotg_phy_disable(struct s3c_hsotg *hsotg)
 {
 	struct platform_device *pdev = to_platform_device(hsotg->dev);
 
-	if (hsotg->phy) {
-		phy_power_off(hsotg->phy);
-		phy_exit(hsotg->phy);
-	} else if (hsotg->uphy)
+	if (hsotg->uphy)
 		usb_phy_shutdown(hsotg->uphy);
-	else if (hsotg->plat->phy_exit)
+	else if (hsotg->plat && hsotg->plat->phy_exit)
 		hsotg->plat->phy_exit(pdev, hsotg->plat->phy_type);
+	else {
+		phy_power_off(hsotg->phy);
+		phy_exit(hsotg->phy);
+	}
 }
 
 /**
@@ -2858,6 +2893,8 @@ static int s3c_hsotg_udc_start(struct usb_gadget *gadget,
 	hsotg->gadget.dev.of_node = hsotg->dev->of_node;
 	hsotg->gadget.speed = USB_SPEED_UNKNOWN;
 
+	clk_enable(hsotg->clk);
+
 	ret = regulator_bulk_enable(ARRAY_SIZE(hsotg->supplies),
 				    hsotg->supplies);
 	if (ret) {
@@ -2892,13 +2929,11 @@ static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
 		return -ENODEV;
 
 	/* all endpoints should be shutdown */
-	for (ep = 0; ep < hsotg->num_of_eps; ep++)
+	for (ep = 1; ep < hsotg->num_of_eps; ep++)
 		s3c_hsotg_ep_disable(&hsotg->eps[ep].ep);
 
 	spin_lock_irqsave(&hsotg->lock, flags);
 
-	s3c_hsotg_phy_disable(hsotg);
-
 	if (!driver)
 		hsotg->driver = NULL;
 
@@ -2908,6 +2943,8 @@ static int s3c_hsotg_udc_stop(struct usb_gadget *gadget,
 
 	regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies), hsotg->supplies);
 
+	clk_disable(hsotg->clk);
+
 	return 0;
 }
 
@@ -2934,14 +2971,15 @@ static int s3c_hsotg_pullup(struct usb_gadget *gadget, int is_on)
 	struct s3c_hsotg *hsotg = to_hsotg(gadget);
 	unsigned long flags = 0;
 
-	dev_dbg(hsotg->dev, "%s: is_in: %d\n", __func__, is_on);
+	dev_dbg(hsotg->dev, "%s: is_on: %d\n", __func__, is_on);
 
 	spin_lock_irqsave(&hsotg->lock, flags);
 	if (is_on) {
 		s3c_hsotg_phy_enable(hsotg);
+		clk_enable(hsotg->clk);
 		s3c_hsotg_core_init(hsotg);
 	} else {
-		s3c_hsotg_disconnect(hsotg);
+		clk_disable(hsotg->clk);
 		s3c_hsotg_phy_disable(hsotg);
 	}
 
@@ -2972,7 +3010,6 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 				       struct s3c_hsotg_ep *hs_ep,
 				       int epnum)
 {
-	u32 ptxfifo;
 	char *dir;
 
 	if (epnum == 0)
@@ -3001,15 +3038,6 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
 	hs_ep->ep.ops = &s3c_hsotg_ep_ops;
 
 	/*
-	 * Read the FIFO size for the Periodic TX FIFO, even if we're
-	 * an OUT endpoint, we may as well do this if in future the
-	 * code is changed to make each endpoint's direction changeable.
-	 */
-
-	ptxfifo = readl(hsotg->regs + DPTXFSIZN(epnum));
-	hs_ep->fifo_size = FIFOSIZE_DEPTH_GET(ptxfifo) * 4;
-
-	/*
 	 * if we're using dma, we need to set the next-endpoint pointer
 	 * to be something valid.
 	 */
@@ -3029,19 +3057,22 @@ static void s3c_hsotg_initep(struct s3c_hsotg *hsotg,
  */
 static void s3c_hsotg_hw_cfg(struct s3c_hsotg *hsotg)
 {
-	u32 cfg2, cfg4;
+	u32 cfg2, cfg3, cfg4;
 	/* check hardware configuration */
 
 	cfg2 = readl(hsotg->regs + 0x48);
 	hsotg->num_of_eps = (cfg2 >> 10) & 0xF;
 
-	dev_info(hsotg->dev, "EPs:%d\n", hsotg->num_of_eps);
+	cfg3 = readl(hsotg->regs + 0x4C);
+	hsotg->fifo_mem = (cfg3 >> 16);
 
 	cfg4 = readl(hsotg->regs + 0x50);
 	hsotg->dedicated_fifos = (cfg4 >> 25) & 1;
 
-	dev_info(hsotg->dev, "%s fifos\n",
-		 hsotg->dedicated_fifos ? "dedicated" : "shared");
+	dev_info(hsotg->dev, "EPs: %d, %s fifos, %d entries in SPRAM\n",
+		 hsotg->num_of_eps,
+		 hsotg->dedicated_fifos ? "dedicated" : "shared",
+		 hsotg->fifo_mem);
 }
 
 /**
@@ -3392,6 +3423,9 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 	if (!hsotg)
 		return -ENOMEM;
 
+	/* Set default UTMI width */
+	hsotg->phyif = GUSBCFG_PHYIF16;
+
 	/*
 	 * Attempt to find a generic PHY, then look for an old style
 	 * USB PHY, finally fall back to pdata
@@ -3410,8 +3444,15 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 			hsotg->plat = plat;
 		} else
 			hsotg->uphy = uphy;
-	} else
+	} else {
 		hsotg->phy = phy;
+		/*
+		 * If using the generic PHY framework, check if the PHY bus
+		 * width is 8-bit and set the phyif appropriately.
+		 */
+		if (phy_get_bus_width(phy) == 8)
+			hsotg->phyif = GUSBCFG_PHYIF8;
+	}
 
 	hsotg->dev = dev;
 
@@ -3441,13 +3482,6 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 
 	hsotg->irq = ret;
 
-	ret = devm_request_irq(&pdev->dev, hsotg->irq, s3c_hsotg_irq, 0,
-				dev_name(dev), hsotg);
-	if (ret < 0) {
-		dev_err(dev, "cannot claim IRQ\n");
-		goto err_clk;
-	}
-
 	dev_info(dev, "regs %p, irq %d\n", hsotg->regs, hsotg->irq);
 
 	hsotg->gadget.max_speed = USB_SPEED_HIGH;
@@ -3478,25 +3512,23 @@ static int s3c_hsotg_probe(struct platform_device *pdev)
 		goto err_supplies;
 	}
 
-	/* Set default UTMI width */
-	hsotg->phyif = GUSBCFG_PHYIF16;
-
-	/*
-	 * If using the generic PHY framework, check if the PHY bus
-	 * width is 8-bit and set the phyif appropriately.
-	 */
-	if (hsotg->phy && (phy_get_bus_width(phy) == 8))
-		hsotg->phyif = GUSBCFG_PHYIF8;
-
-	if (hsotg->phy)
-		phy_init(hsotg->phy);
-
 	/* usb phy enable */
 	s3c_hsotg_phy_enable(hsotg);
 
 	s3c_hsotg_corereset(hsotg);
-	s3c_hsotg_init(hsotg);
 	s3c_hsotg_hw_cfg(hsotg);
+	s3c_hsotg_init(hsotg);
+
+	ret = devm_request_irq(&pdev->dev, hsotg->irq, s3c_hsotg_irq, 0,
+				dev_name(dev), hsotg);
+	if (ret < 0) {
+		s3c_hsotg_phy_disable(hsotg);
+		clk_disable_unprepare(hsotg->clk);
+		regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies),
+				       hsotg->supplies);
+		dev_err(dev, "cannot claim IRQ\n");
+		goto err_clk;
+	}
 
 	/* hsotg->num_of_eps holds number of EPs other than ep0 */
 
@@ -3582,9 +3614,6 @@ static int s3c_hsotg_remove(struct platform_device *pdev)
 		usb_gadget_unregister_driver(hsotg->driver);
 	}
 
-	s3c_hsotg_phy_disable(hsotg);
-	if (hsotg->phy)
-		phy_exit(hsotg->phy);
 	clk_disable_unprepare(hsotg->clk);
 
 	return 0;
@@ -3613,6 +3642,7 @@ static int s3c_hsotg_suspend(struct platform_device *pdev, pm_message_t state)
 
 		ret = regulator_bulk_disable(ARRAY_SIZE(hsotg->supplies),
 					     hsotg->supplies);
+		clk_disable(hsotg->clk);
 	}
 
 	return ret;
@@ -3627,6 +3657,8 @@ static int s3c_hsotg_resume(struct platform_device *pdev)
 	if (hsotg->driver) {
 		dev_info(hsotg->dev, "resuming usb gadget %s\n",
 			 hsotg->driver->driver.name);
+
+		clk_enable(hsotg->clk);
 		ret = regulator_bulk_enable(ARRAY_SIZE(hsotg->supplies),
 				      hsotg->supplies);
 	}
diff --git a/drivers/usb/dwc2/hcd.c b/drivers/usb/dwc2/hcd.c
index 4d918ed8d343..0a0e6f0ad15f 100644
--- a/drivers/usb/dwc2/hcd.c
+++ b/drivers/usb/dwc2/hcd.c
@@ -697,29 +697,45 @@ static void *dwc2_hc_init_xfer(struct dwc2_hsotg *hsotg,
 }
 
 static int dwc2_hc_setup_align_buf(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh,
-				   struct dwc2_host_chan *chan, void *bufptr)
+				   struct dwc2_host_chan *chan,
+				   struct dwc2_hcd_urb *urb, void *bufptr)
 {
 	u32 buf_size;
-
-	if (chan->ep_type != USB_ENDPOINT_XFER_ISOC)
-		buf_size = hsotg->core_params->max_transfer_size;
-	else
-		buf_size = 4096;
+	struct urb *usb_urb;
+	struct usb_hcd *hcd;
 
 	if (!qh->dw_align_buf) {
+		if (chan->ep_type != USB_ENDPOINT_XFER_ISOC)
+			buf_size = hsotg->core_params->max_transfer_size;
+		else
+			/* 3072 = 3 max-size Isoc packets */
+			buf_size = 3072;
+
 		qh->dw_align_buf = dma_alloc_coherent(hsotg->dev, buf_size,
 						      &qh->dw_align_buf_dma,
 						      GFP_ATOMIC);
 		if (!qh->dw_align_buf)
 			return -ENOMEM;
+		qh->dw_align_buf_size = buf_size;
 	}
 
-	if (!chan->ep_is_in && chan->xfer_len) {
-		dma_sync_single_for_cpu(hsotg->dev, chan->xfer_dma, buf_size,
-					DMA_TO_DEVICE);
-		memcpy(qh->dw_align_buf, bufptr, chan->xfer_len);
-		dma_sync_single_for_device(hsotg->dev, chan->xfer_dma, buf_size,
-					   DMA_TO_DEVICE);
+	if (chan->xfer_len) {
+		dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
+		usb_urb = urb->priv;
+
+		if (usb_urb) {
+			if (usb_urb->transfer_flags &
+			    (URB_SETUP_MAP_SINGLE | URB_DMA_MAP_SG |
+			     URB_DMA_MAP_PAGE | URB_DMA_MAP_SINGLE)) {
+				hcd = dwc2_hsotg_to_hcd(hsotg);
+				usb_hcd_unmap_urb_for_dma(hcd, usb_urb);
+			}
+			if (!chan->ep_is_in)
+				memcpy(qh->dw_align_buf, bufptr,
+				       chan->xfer_len);
+		} else {
+			dev_warn(hsotg->dev, "no URB in dwc2_urb\n");
+		}
 	}
 
 	chan->align_buf = qh->dw_align_buf_dma;
@@ -828,7 +844,7 @@ static int dwc2_assign_and_init_hc(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 	/* Non DWORD-aligned buffer case */
 	if (bufptr) {
 		dev_vdbg(hsotg->dev, "Non-aligned buffer\n");
-		if (dwc2_hc_setup_align_buf(hsotg, qh, chan, bufptr)) {
+		if (dwc2_hc_setup_align_buf(hsotg, qh, chan, urb, bufptr)) {
 			dev_err(hsotg->dev,
 				"%s: Failed to allocate memory to handle non-dword aligned buffer\n",
 				__func__);
diff --git a/drivers/usb/dwc2/hcd.h b/drivers/usb/dwc2/hcd.h
index fdc6d489084a..a12bb1538666 100644
--- a/drivers/usb/dwc2/hcd.h
+++ b/drivers/usb/dwc2/hcd.h
@@ -243,7 +243,8 @@ enum dwc2_transaction_type {
  * @ntd:                Actual number of transfer descriptors in a list
  * @dw_align_buf:       Used instead of original buffer if its physical address
  *                      is not dword-aligned
- * @dw_align_buf_dma:   DMA address for align_buf
+ * @dw_align_buf_size:  Size of dw_align_buf
+ * @dw_align_buf_dma:   DMA address for dw_align_buf
  * @qtd_list:           List of QTDs for this QH
  * @channel:            Host channel currently processing transfers for this QH
  * @qh_list_entry:      Entry for QH in either the periodic or non-periodic
@@ -276,6 +277,7 @@ struct dwc2_qh {
 	u16 start_split_frame;
 	u16 ntd;
 	u8 *dw_align_buf;
+	int dw_align_buf_size;
 	dma_addr_t dw_align_buf_dma;
 	struct list_head qtd_list;
 	struct dwc2_host_chan *channel;
diff --git a/drivers/usb/dwc2/hcd_intr.c b/drivers/usb/dwc2/hcd_intr.c
index 47b9eb5389b4..551ba878b003 100644
--- a/drivers/usb/dwc2/hcd_intr.c
+++ b/drivers/usb/dwc2/hcd_intr.c
@@ -465,12 +465,8 @@ static int dwc2_update_urb_state(struct dwc2_hsotg *hsotg,
 	/* Non DWORD-aligned buffer case handling */
 	if (chan->align_buf && xfer_length && chan->ep_is_in) {
 		dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
-		dma_sync_single_for_cpu(hsotg->dev, urb->dma, urb->length,
-					DMA_FROM_DEVICE);
 		memcpy(urb->buf + urb->actual_length, chan->qh->dw_align_buf,
 		       xfer_length);
-		dma_sync_single_for_device(hsotg->dev, urb->dma, urb->length,
-					   DMA_FROM_DEVICE);
 	}
 
 	dev_vdbg(hsotg->dev, "urb->actual_length=%d xfer_length=%d\n",
@@ -560,14 +556,9 @@ static enum dwc2_halt_status dwc2_update_isoc_urb_state(
 		    chan->ep_is_in) {
 			dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n",
 				 __func__);
-			dma_sync_single_for_cpu(hsotg->dev, urb->dma,
-						urb->length, DMA_FROM_DEVICE);
 			memcpy(urb->buf + frame_desc->offset +
 			       qtd->isoc_split_offset, chan->qh->dw_align_buf,
 			       frame_desc->actual_length);
-			dma_sync_single_for_device(hsotg->dev, urb->dma,
-						   urb->length,
-						   DMA_FROM_DEVICE);
 		}
 		break;
 	case DWC2_HC_XFER_FRAME_OVERRUN:
@@ -594,14 +585,9 @@ static enum dwc2_halt_status dwc2_update_isoc_urb_state(
 		    chan->ep_is_in) {
 			dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n",
 				 __func__);
-			dma_sync_single_for_cpu(hsotg->dev, urb->dma,
-						urb->length, DMA_FROM_DEVICE);
 			memcpy(urb->buf + frame_desc->offset +
 			       qtd->isoc_split_offset, chan->qh->dw_align_buf,
 			       frame_desc->actual_length);
-			dma_sync_single_for_device(hsotg->dev, urb->dma,
-						   urb->length,
-						   DMA_FROM_DEVICE);
 		}
 
 		/* Skip whole frame */
@@ -937,12 +923,8 @@ static int dwc2_xfercomp_isoc_split_in(struct dwc2_hsotg *hsotg,
 
 	if (chan->align_buf) {
 		dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
-		dma_sync_single_for_cpu(hsotg->dev, qtd->urb->dma,
-					qtd->urb->length, DMA_FROM_DEVICE);
 		memcpy(qtd->urb->buf + frame_desc->offset +
 		       qtd->isoc_split_offset, chan->qh->dw_align_buf, len);
-		dma_sync_single_for_device(hsotg->dev, qtd->urb->dma,
-					   qtd->urb->length, DMA_FROM_DEVICE);
 	}
 
 	qtd->isoc_split_offset += len;
@@ -1170,12 +1152,8 @@ static void dwc2_update_urb_state_abn(struct dwc2_hsotg *hsotg,
 	/* Non DWORD-aligned buffer case handling */
 	if (chan->align_buf && xfer_length && chan->ep_is_in) {
 		dev_vdbg(hsotg->dev, "%s(): non-aligned buffer\n", __func__);
-		dma_sync_single_for_cpu(hsotg->dev, urb->dma, urb->length,
-					DMA_FROM_DEVICE);
 		memcpy(urb->buf + urb->actual_length, chan->qh->dw_align_buf,
 		       xfer_length);
-		dma_sync_single_for_device(hsotg->dev, urb->dma, urb->length,
-					   DMA_FROM_DEVICE);
 	}
 
 	urb->actual_length += xfer_length;
@@ -1890,12 +1868,20 @@ static void dwc2_hc_chhltd_intr_dma(struct dwc2_hsotg *hsotg,
 					"hcint 0x%08x, intsts 0x%08x\n",
 					chan->hcint,
 					readl(hsotg->regs + GINTSTS));
+				goto error;
 			}
 		}
 	} else {
 		dev_info(hsotg->dev,
 			 "NYET/NAK/ACK/other in non-error case, 0x%08x\n",
 			 chan->hcint);
+error:
+		/* Failthrough: use 3-strikes rule */
+		qtd->error_count++;
+		dwc2_update_urb_state_abn(hsotg, chan, chnum, qtd->urb,
+					  qtd, DWC2_HC_XFER_XACT_ERR);
+		dwc2_hcd_save_data_toggle(hsotg, chan, chnum, qtd);
+		dwc2_halt_channel(hsotg, chan, qtd, DWC2_HC_XFER_XACT_ERR);
 	}
 }
 
diff --git a/drivers/usb/dwc2/hcd_queue.c b/drivers/usb/dwc2/hcd_queue.c
index 9540f7e1e20e..bb97838bc6c0 100644
--- a/drivers/usb/dwc2/hcd_queue.c
+++ b/drivers/usb/dwc2/hcd_queue.c
@@ -229,19 +229,11 @@ static struct dwc2_qh *dwc2_hcd_qh_create(struct dwc2_hsotg *hsotg,
  */
 void dwc2_hcd_qh_free(struct dwc2_hsotg *hsotg, struct dwc2_qh *qh)
 {
-	u32 buf_size;
-
-	if (hsotg->core_params->dma_desc_enable > 0) {
+	if (hsotg->core_params->dma_desc_enable > 0)
 		dwc2_hcd_qh_free_ddma(hsotg, qh);
-	} else if (qh->dw_align_buf) {
-		if (qh->ep_type == USB_ENDPOINT_XFER_ISOC)
-			buf_size = 4096;
-		else
-			buf_size = hsotg->core_params->max_transfer_size;
-		dma_free_coherent(hsotg->dev, buf_size, qh->dw_align_buf,
-				  qh->dw_align_buf_dma);
-	}
-
+	else if (qh->dw_align_buf)
+		dma_free_coherent(hsotg->dev, qh->dw_align_buf_size,
+				  qh->dw_align_buf, qh->dw_align_buf_dma);
 	kfree(qh);
 }
 
diff --git a/drivers/usb/dwc2/platform.c b/drivers/usb/dwc2/platform.c
index a10e7a353576..121dbdafc06b 100644
--- a/drivers/usb/dwc2/platform.c
+++ b/drivers/usb/dwc2/platform.c
@@ -42,6 +42,8 @@
 #include <linux/of_device.h>
 #include <linux/platform_device.h>
 
+#include <linux/usb/of.h>
+
 #include "core.h"
 #include "hcd.h"
 
@@ -75,6 +77,34 @@ static const struct dwc2_core_params params_bcm2835 = {
 	.uframe_sched			= 0,
 };
 
+static const struct dwc2_core_params params_rk3066 = {
+	.otg_cap			= 2,	/* non-HNP/non-SRP */
+	.otg_ver			= -1,
+	.dma_enable			= -1,
+	.dma_desc_enable		= 0,
+	.speed				= -1,
+	.enable_dynamic_fifo		= 1,
+	.en_multiple_tx_fifo		= -1,
+	.host_rx_fifo_size		= 520,	/* 520 DWORDs */
+	.host_nperio_tx_fifo_size	= 128,	/* 128 DWORDs */
+	.host_perio_tx_fifo_size	= 256,	/* 256 DWORDs */
+	.max_transfer_size		= 65535,
+	.max_packet_count		= -1,
+	.host_channels			= -1,
+	.phy_type			= -1,
+	.phy_utmi_width			= -1,
+	.phy_ulpi_ddr			= -1,
+	.phy_ulpi_ext_vbus		= -1,
+	.i2c_enable			= -1,
+	.ulpi_fs_ls			= -1,
+	.host_support_fs_ls_low_power	= -1,
+	.host_ls_low_power_phy_clk	= -1,
+	.ts_dline			= -1,
+	.reload_ctl			= -1,
+	.ahbcfg				= 0x7, /* INCR16 */
+	.uframe_sched			= -1,
+};
+
 /**
  * dwc2_driver_remove() - Called when the DWC_otg core is unregistered with the
  * DWC_otg driver
@@ -97,6 +127,7 @@ static int dwc2_driver_remove(struct platform_device *dev)
 
 static const struct of_device_id dwc2_of_match_table[] = {
 	{ .compatible = "brcm,bcm2835-usb", .data = &params_bcm2835 },
+	{ .compatible = "rockchip,rk3066-usb", .data = &params_rk3066 },
 	{ .compatible = "snps,dwc2", .data = NULL },
 	{},
 };
@@ -171,6 +202,8 @@ static int dwc2_driver_probe(struct platform_device *dev)
 	dev_dbg(&dev->dev, "mapped PA %08lx to VA %p\n",
 		(unsigned long)res->start, hsotg->regs);
 
+	hsotg->dr_mode = of_usb_get_dr_mode(dev->dev.of_node);
+
 	retval = dwc2_hcd_init(hsotg, irq, params);
 	if (retval)
 		return retval;
diff --git a/drivers/usb/dwc3/Kconfig b/drivers/usb/dwc3/Kconfig
index 785510a0a0c3..f4e5cc60db0b 100644
--- a/drivers/usb/dwc3/Kconfig
+++ b/drivers/usb/dwc3/Kconfig
@@ -80,6 +80,23 @@ config USB_DWC3_KEYSTONE
 	  Support of USB2/3 functionality in TI Keystone2 platforms.
 	  Say 'Y' or 'M' here if you have one such device
 
+config USB_DWC3_ST
+	tristate "STMicroelectronics Platforms"
+	depends on ARCH_STI && OF
+	default USB_DWC3
+	help
+	  STMicroelectronics SoCs with one DesignWare Core USB3 IP
+	  inside (i.e. STiH407).
+	  Say 'Y' or 'M' if you have one such device.
+
+config USB_DWC3_QCOM
+	tristate "Qualcomm Platforms"
+	depends on ARCH_QCOM || COMPILE_TEST
+	default USB_DWC3
+	help
+	  Recent Qualcomm SoCs ship with one DesignWare Core USB3 IP inside,
+	  say 'Y' or 'M' if you have one such device.
+
 comment "Debugging features"
 
 config USB_DWC3_DEBUG
diff --git a/drivers/usb/dwc3/Makefile b/drivers/usb/dwc3/Makefile
index 10ac3e72482e..bb34fbcfeab3 100644
--- a/drivers/usb/dwc3/Makefile
+++ b/drivers/usb/dwc3/Makefile
@@ -1,9 +1,12 @@
+# define_trace.h needs to know how to find our header
+CFLAGS_trace.o				:= -I$(src)
+
 ccflags-$(CONFIG_USB_DWC3_DEBUG)	:= -DDEBUG
 ccflags-$(CONFIG_USB_DWC3_VERBOSE)	+= -DVERBOSE_DEBUG
 
 obj-$(CONFIG_USB_DWC3)			+= dwc3.o
 
-dwc3-y					:= core.o
+dwc3-y					:= core.o debug.o trace.o
 
 ifneq ($(filter y,$(CONFIG_USB_DWC3_HOST) $(CONFIG_USB_DWC3_DUAL_ROLE)),)
 	dwc3-y				+= host.o
@@ -33,3 +36,5 @@ obj-$(CONFIG_USB_DWC3_OMAP)		+= dwc3-omap.o
 obj-$(CONFIG_USB_DWC3_EXYNOS)		+= dwc3-exynos.o
 obj-$(CONFIG_USB_DWC3_PCI)		+= dwc3-pci.o
 obj-$(CONFIG_USB_DWC3_KEYSTONE)		+= dwc3-keystone.o
+obj-$(CONFIG_USB_DWC3_QCOM)		+= dwc3-qcom.o
+obj-$(CONFIG_USB_DWC3_ST)		+= dwc3-st.o
diff --git a/drivers/usb/dwc3/core.c b/drivers/usb/dwc3/core.c
index b769c1faaf03..b0f4d52b7f04 100644
--- a/drivers/usb/dwc3/core.c
+++ b/drivers/usb/dwc3/core.c
@@ -186,10 +186,8 @@ static int dwc3_alloc_event_buffers(struct dwc3 *dwc, unsigned length)
 
 	dwc->ev_buffs = devm_kzalloc(dwc->dev, sizeof(*dwc->ev_buffs) * num,
 			GFP_KERNEL);
-	if (!dwc->ev_buffs) {
-		dev_err(dwc->dev, "can't allocate event buffers array\n");
+	if (!dwc->ev_buffs)
 		return -ENOMEM;
-	}
 
 	for (i = 0; i < num; i++) {
 		struct dwc3_event_buffer	*evt;
@@ -639,10 +637,9 @@ static int dwc3_probe(struct platform_device *pdev)
 	void			*mem;
 
 	mem = devm_kzalloc(dev, sizeof(*dwc) + DWC3_ALIGN_MASK, GFP_KERNEL);
-	if (!mem) {
-		dev_err(dev, "not enough memory\n");
+	if (!mem)
 		return -ENOMEM;
-	}
+
 	dwc = PTR_ALIGN(mem, DWC3_ALIGN_MASK + 1);
 	dwc->mem = mem;
 	dwc->dev = dev;
@@ -799,20 +796,21 @@ static int dwc3_remove(struct platform_device *pdev)
 {
 	struct dwc3	*dwc = platform_get_drvdata(pdev);
 
+	dwc3_debugfs_exit(dwc);
+	dwc3_core_exit_mode(dwc);
+	dwc3_event_buffers_cleanup(dwc);
+	dwc3_free_event_buffers(dwc);
+
 	usb_phy_set_suspend(dwc->usb2_phy, 1);
 	usb_phy_set_suspend(dwc->usb3_phy, 1);
 	phy_power_off(dwc->usb2_generic_phy);
 	phy_power_off(dwc->usb3_generic_phy);
 
+	dwc3_core_exit(dwc);
+
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
 
-	dwc3_debugfs_exit(dwc);
-	dwc3_core_exit_mode(dwc);
-	dwc3_event_buffers_cleanup(dwc);
-	dwc3_free_event_buffers(dwc);
-	dwc3_core_exit(dwc);
-
 	return 0;
 }
 
diff --git a/drivers/usb/dwc3/core.h b/drivers/usb/dwc3/core.h
index 48fb264065db..66f62563bcf9 100644
--- a/drivers/usb/dwc3/core.h
+++ b/drivers/usb/dwc3/core.h
@@ -33,6 +33,8 @@
 
 #include <linux/phy/phy.h>
 
+#define DWC3_MSG_MAX	500
+
 /* Global constants */
 #define DWC3_EP0_BOUNCE_SIZE	512
 #define DWC3_ENDPOINTS_NUM	32
@@ -938,7 +940,7 @@ int dwc3_gadget_get_link_state(struct dwc3 *dwc);
 int dwc3_gadget_set_link_state(struct dwc3 *dwc, enum dwc3_link_state state);
 int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
 		unsigned cmd, struct dwc3_gadget_ep_cmd_params *params);
-int dwc3_send_gadget_generic_command(struct dwc3 *dwc, int cmd, u32 param);
+int dwc3_send_gadget_generic_command(struct dwc3 *dwc, unsigned cmd, u32 param);
 #else
 static inline int dwc3_gadget_init(struct dwc3 *dwc)
 { return 0; }
diff --git a/drivers/usb/dwc3/debug.c b/drivers/usb/dwc3/debug.c
new file mode 100644
index 000000000000..0be6885bc370
--- /dev/null
+++ b/drivers/usb/dwc3/debug.c
@@ -0,0 +1,32 @@
+/**
+ * debug.c - DesignWare USB3 DRD Controller Debug/Trace Support
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Author: Felipe Balbi <balbi@ti.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include "debug.h"
+
+void dwc3_trace(void (*trace)(struct va_format *), const char *fmt, ...)
+{
+	struct va_format vaf;
+	va_list args;
+
+	va_start(args, fmt);
+	vaf.fmt = fmt;
+	vaf.va = &args;
+
+	trace(&vaf);
+
+	va_end(args);
+}
diff --git a/drivers/usb/dwc3/debug.h b/drivers/usb/dwc3/debug.h
index fceb39dc4bba..07fbc2d94fd4 100644
--- a/drivers/usb/dwc3/debug.h
+++ b/drivers/usb/dwc3/debug.h
@@ -16,8 +16,206 @@
  * GNU General Public License for more details.
  */
 
+#ifndef __DWC3_DEBUG_H
+#define __DWC3_DEBUG_H
+
 #include "core.h"
 
+/**
+ * dwc3_gadget_ep_cmd_string - returns endpoint command string
+ * @cmd: command code
+ */
+static inline const char *
+dwc3_gadget_ep_cmd_string(u8 cmd)
+{
+	switch (cmd) {
+	case DWC3_DEPCMD_DEPSTARTCFG:
+		return "Start New Configuration";
+	case DWC3_DEPCMD_ENDTRANSFER:
+		return "End Transfer";
+	case DWC3_DEPCMD_UPDATETRANSFER:
+		return "Update Transfer";
+	case DWC3_DEPCMD_STARTTRANSFER:
+		return "Start Transfer";
+	case DWC3_DEPCMD_CLEARSTALL:
+		return "Clear Stall";
+	case DWC3_DEPCMD_SETSTALL:
+		return "Set Stall";
+	case DWC3_DEPCMD_GETEPSTATE:
+		return "Get Endpoint State";
+	case DWC3_DEPCMD_SETTRANSFRESOURCE:
+		return "Set Endpoint Transfer Resource";
+	case DWC3_DEPCMD_SETEPCONFIG:
+		return "Set Endpoint Configuration";
+	default:
+		return "UNKNOWN command";
+	}
+}
+
+/**
+ * dwc3_gadget_generic_cmd_string - returns generic command string
+ * @cmd: command code
+ */
+static inline const char *
+dwc3_gadget_generic_cmd_string(u8 cmd)
+{
+	switch (cmd) {
+	case DWC3_DGCMD_SET_LMP:
+		return "Set LMP";
+	case DWC3_DGCMD_SET_PERIODIC_PAR:
+		return "Set Periodic Parameters";
+	case DWC3_DGCMD_XMIT_FUNCTION:
+		return "Transmit Function Wake Device Notification";
+	case DWC3_DGCMD_SET_SCRATCHPAD_ADDR_LO:
+		return "Set Scratchpad Buffer Array Address Lo";
+	case DWC3_DGCMD_SET_SCRATCHPAD_ADDR_HI:
+		return "Set Scratchpad Buffer Array Address Hi";
+	case DWC3_DGCMD_SELECTED_FIFO_FLUSH:
+		return "Selected FIFO Flush";
+	case DWC3_DGCMD_ALL_FIFO_FLUSH:
+		return "All FIFO Flush";
+	case DWC3_DGCMD_SET_ENDPOINT_NRDY:
+		return "Set Endpoint NRDY";
+	case DWC3_DGCMD_RUN_SOC_BUS_LOOPBACK:
+		return "Run SoC Bus Loopback Test";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+/**
+ * dwc3_gadget_link_string - returns link name
+ * @link_state: link state code
+ */
+static inline const char *
+dwc3_gadget_link_string(enum dwc3_link_state link_state)
+{
+	switch (link_state) {
+	case DWC3_LINK_STATE_U0:
+		return "U0";
+	case DWC3_LINK_STATE_U1:
+		return "U1";
+	case DWC3_LINK_STATE_U2:
+		return "U2";
+	case DWC3_LINK_STATE_U3:
+		return "U3";
+	case DWC3_LINK_STATE_SS_DIS:
+		return "SS.Disabled";
+	case DWC3_LINK_STATE_RX_DET:
+		return "RX.Detect";
+	case DWC3_LINK_STATE_SS_INACT:
+		return "SS.Inactive";
+	case DWC3_LINK_STATE_POLL:
+		return "Polling";
+	case DWC3_LINK_STATE_RECOV:
+		return "Recovery";
+	case DWC3_LINK_STATE_HRESET:
+		return "Hot Reset";
+	case DWC3_LINK_STATE_CMPLY:
+		return "Compliance";
+	case DWC3_LINK_STATE_LPBK:
+		return "Loopback";
+	case DWC3_LINK_STATE_RESET:
+		return "Reset";
+	case DWC3_LINK_STATE_RESUME:
+		return "Resume";
+	default:
+		return "UNKNOWN link state\n";
+	}
+}
+
+/**
+ * dwc3_gadget_event_string - returns event name
+ * @event: the event code
+ */
+static inline const char *dwc3_gadget_event_string(u8 event)
+{
+	switch (event) {
+	case DWC3_DEVICE_EVENT_DISCONNECT:
+		return "Disconnect";
+	case DWC3_DEVICE_EVENT_RESET:
+		return "Reset";
+	case DWC3_DEVICE_EVENT_CONNECT_DONE:
+		return "Connection Done";
+	case DWC3_DEVICE_EVENT_LINK_STATUS_CHANGE:
+		return "Link Status Change";
+	case DWC3_DEVICE_EVENT_WAKEUP:
+		return "WakeUp";
+	case DWC3_DEVICE_EVENT_EOPF:
+		return "End-Of-Frame";
+	case DWC3_DEVICE_EVENT_SOF:
+		return "Start-Of-Frame";
+	case DWC3_DEVICE_EVENT_ERRATIC_ERROR:
+		return "Erratic Error";
+	case DWC3_DEVICE_EVENT_CMD_CMPL:
+		return "Command Complete";
+	case DWC3_DEVICE_EVENT_OVERFLOW:
+		return "Overflow";
+	}
+
+	return "UNKNOWN";
+}
+
+/**
+ * dwc3_ep_event_string - returns event name
+ * @event: then event code
+ */
+static inline const char *dwc3_ep_event_string(u8 event)
+{
+	switch (event) {
+	case DWC3_DEPEVT_XFERCOMPLETE:
+		return "Transfer Complete";
+	case DWC3_DEPEVT_XFERINPROGRESS:
+		return "Transfer In-Progress";
+	case DWC3_DEPEVT_XFERNOTREADY:
+		return "Transfer Not Ready";
+	case DWC3_DEPEVT_RXTXFIFOEVT:
+		return "FIFO";
+	case DWC3_DEPEVT_STREAMEVT:
+		return "Stream";
+	case DWC3_DEPEVT_EPCMDCMPLT:
+		return "Endpoint Command Complete";
+	}
+
+	return "UNKNOWN";
+}
+
+/**
+ * dwc3_gadget_event_type_string - return event name
+ * @event: the event code
+ */
+static inline const char *dwc3_gadget_event_type_string(u8 event)
+{
+	switch (event) {
+	case DWC3_DEVICE_EVENT_DISCONNECT:
+		return "Disconnect";
+	case DWC3_DEVICE_EVENT_RESET:
+		return "Reset";
+	case DWC3_DEVICE_EVENT_CONNECT_DONE:
+		return "Connect Done";
+	case DWC3_DEVICE_EVENT_LINK_STATUS_CHANGE:
+		return "Link Status Change";
+	case DWC3_DEVICE_EVENT_WAKEUP:
+		return "Wake-Up";
+	case DWC3_DEVICE_EVENT_HIBER_REQ:
+		return "Hibernation";
+	case DWC3_DEVICE_EVENT_EOPF:
+		return "End of Periodic Frame";
+	case DWC3_DEVICE_EVENT_SOF:
+		return "Start of Frame";
+	case DWC3_DEVICE_EVENT_ERRATIC_ERROR:
+		return "Erratic Error";
+	case DWC3_DEVICE_EVENT_CMD_CMPL:
+		return "Command Complete";
+	case DWC3_DEVICE_EVENT_OVERFLOW:
+		return "Overflow";
+	default:
+		return "UNKNOWN";
+	}
+}
+
+void dwc3_trace(void (*trace)(struct va_format *), const char *fmt, ...);
+
 #ifdef CONFIG_DEBUG_FS
 extern int dwc3_debugfs_init(struct dwc3 *);
 extern void dwc3_debugfs_exit(struct dwc3 *);
@@ -27,4 +225,4 @@ static inline int dwc3_debugfs_init(struct dwc3 *d)
 static inline void dwc3_debugfs_exit(struct dwc3 *d)
 {  }
 #endif
-
+#endif /* __DWC3_DEBUG_H */
diff --git a/drivers/usb/dwc3/dwc3-exynos.c b/drivers/usb/dwc3/dwc3-exynos.c
index f9fb8adb785b..3951a65fea04 100644
--- a/drivers/usb/dwc3/dwc3-exynos.c
+++ b/drivers/usb/dwc3/dwc3-exynos.c
@@ -113,10 +113,8 @@ static int dwc3_exynos_probe(struct platform_device *pdev)
 	int			ret;
 
 	exynos = devm_kzalloc(dev, sizeof(*exynos), GFP_KERNEL);
-	if (!exynos) {
-		dev_err(dev, "not enough memory\n");
+	if (!exynos)
 		return -ENOMEM;
-	}
 
 	/*
 	 * Right now device-tree probed devices don't get dma_mask set.
diff --git a/drivers/usb/dwc3/dwc3-keystone.c b/drivers/usb/dwc3/dwc3-keystone.c
index 1fad1618df6e..7ec8495f4523 100644
--- a/drivers/usb/dwc3/dwc3-keystone.c
+++ b/drivers/usb/dwc3/dwc3-keystone.c
@@ -189,7 +189,6 @@ static struct platform_driver kdwc3_driver = {
 	.remove		= kdwc3_remove,
 	.driver		= {
 		.name	= "keystone-dwc3",
-		.owner	        = THIS_MODULE,
 		.of_match_table	= kdwc3_of_match,
 	},
 };
diff --git a/drivers/usb/dwc3/dwc3-omap.c b/drivers/usb/dwc3/dwc3-omap.c
index ef4936ff626c..2f537d588225 100644
--- a/drivers/usb/dwc3/dwc3-omap.c
+++ b/drivers/usb/dwc3/dwc3-omap.c
@@ -425,7 +425,7 @@ static void dwc3_omap_set_utmi_mode(struct dwc3_omap *omap)
 
 static int dwc3_omap_extcon_register(struct dwc3_omap *omap)
 {
-	u32			ret;
+	int			ret;
 	struct device_node	*node = omap->dev->of_node;
 	struct extcon_dev	*edev;
 
@@ -481,10 +481,8 @@ static int dwc3_omap_probe(struct platform_device *pdev)
 	}
 
 	omap = devm_kzalloc(dev, sizeof(*omap), GFP_KERNEL);
-	if (!omap) {
-		dev_err(dev, "not enough memory\n");
+	if (!omap)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(pdev, omap);
 
@@ -576,9 +574,9 @@ static int dwc3_omap_remove(struct platform_device *pdev)
 	if (omap->extcon_id_dev.edev)
 		extcon_unregister_interest(&omap->extcon_id_dev);
 	dwc3_omap_disable_irqs(omap);
+	device_for_each_child(&pdev->dev, NULL, dwc3_omap_remove_core);
 	pm_runtime_put_sync(&pdev->dev);
 	pm_runtime_disable(&pdev->dev);
-	device_for_each_child(&pdev->dev, NULL, dwc3_omap_remove_core);
 
 	return 0;
 }
diff --git a/drivers/usb/dwc3/dwc3-pci.c b/drivers/usb/dwc3/dwc3-pci.c
index a60bab7dfa0a..436fb08c40b8 100644
--- a/drivers/usb/dwc3/dwc3-pci.c
+++ b/drivers/usb/dwc3/dwc3-pci.c
@@ -103,10 +103,8 @@ static int dwc3_pci_probe(struct pci_dev *pci,
 	struct device		*dev = &pci->dev;
 
 	glue = devm_kzalloc(dev, sizeof(*glue), GFP_KERNEL);
-	if (!glue) {
-		dev_err(dev, "not enough memory\n");
+	if (!glue)
 		return -ENOMEM;
-	}
 
 	glue->dev = dev;
 
diff --git a/drivers/usb/dwc3/dwc3-qcom.c b/drivers/usb/dwc3/dwc3-qcom.c
new file mode 100644
index 000000000000..8c2e8eec80c2
--- /dev/null
+++ b/drivers/usb/dwc3/dwc3-qcom.c
@@ -0,0 +1,130 @@
+/* Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+
+struct dwc3_qcom {
+	struct device		*dev;
+
+	struct clk		*core_clk;
+	struct clk		*iface_clk;
+	struct clk		*sleep_clk;
+};
+
+static int dwc3_qcom_probe(struct platform_device *pdev)
+{
+	struct device_node *node = pdev->dev.of_node;
+	struct dwc3_qcom *qdwc;
+	int ret;
+
+	qdwc = devm_kzalloc(&pdev->dev, sizeof(*qdwc), GFP_KERNEL);
+	if (!qdwc)
+		return -ENOMEM;
+
+	platform_set_drvdata(pdev, qdwc);
+
+	qdwc->dev = &pdev->dev;
+
+	qdwc->core_clk = devm_clk_get(qdwc->dev, "core");
+	if (IS_ERR(qdwc->core_clk)) {
+		dev_err(qdwc->dev, "failed to get core clock\n");
+		return PTR_ERR(qdwc->core_clk);
+	}
+
+	qdwc->iface_clk = devm_clk_get(qdwc->dev, "iface");
+	if (IS_ERR(qdwc->iface_clk)) {
+		dev_dbg(qdwc->dev, "failed to get optional iface clock\n");
+		qdwc->iface_clk = NULL;
+	}
+
+	qdwc->sleep_clk = devm_clk_get(qdwc->dev, "sleep");
+	if (IS_ERR(qdwc->sleep_clk)) {
+		dev_dbg(qdwc->dev, "failed to get optional sleep clock\n");
+		qdwc->sleep_clk = NULL;
+	}
+
+	ret = clk_prepare_enable(qdwc->core_clk);
+	if (ret) {
+		dev_err(qdwc->dev, "failed to enable core clock\n");
+		goto err_core;
+	}
+
+	ret = clk_prepare_enable(qdwc->iface_clk);
+	if (ret) {
+		dev_err(qdwc->dev, "failed to enable optional iface clock\n");
+		goto err_iface;
+	}
+
+	ret = clk_prepare_enable(qdwc->sleep_clk);
+	if (ret) {
+		dev_err(qdwc->dev, "failed to enable optional sleep clock\n");
+		goto err_sleep;
+	}
+
+	ret = of_platform_populate(node, NULL, NULL, qdwc->dev);
+	if (ret) {
+		dev_err(qdwc->dev, "failed to register core - %d\n", ret);
+		goto err_clks;
+	}
+
+	return 0;
+
+err_clks:
+	clk_disable_unprepare(qdwc->sleep_clk);
+err_sleep:
+	clk_disable_unprepare(qdwc->iface_clk);
+err_iface:
+	clk_disable_unprepare(qdwc->core_clk);
+err_core:
+	return ret;
+}
+
+static int dwc3_qcom_remove(struct platform_device *pdev)
+{
+	struct dwc3_qcom *qdwc = platform_get_drvdata(pdev);
+
+	of_platform_depopulate(&pdev->dev);
+
+	clk_disable_unprepare(qdwc->sleep_clk);
+	clk_disable_unprepare(qdwc->iface_clk);
+	clk_disable_unprepare(qdwc->core_clk);
+
+	return 0;
+}
+
+static const struct of_device_id of_dwc3_match[] = {
+	{ .compatible = "qcom,dwc3" },
+	{ /* Sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, of_dwc3_match);
+
+static struct platform_driver dwc3_qcom_driver = {
+	.probe		= dwc3_qcom_probe,
+	.remove		= dwc3_qcom_remove,
+	.driver		= {
+		.name	= "qcom-dwc3",
+		.of_match_table	= of_dwc3_match,
+	},
+};
+
+module_platform_driver(dwc3_qcom_driver);
+
+MODULE_ALIAS("platform:qcom-dwc3");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("DesignWare USB3 QCOM Glue Layer");
+MODULE_AUTHOR("Ivan T. Ivanov <iivanov@mm-sol.com>");
diff --git a/drivers/usb/dwc3/dwc3-st.c b/drivers/usb/dwc3/dwc3-st.c
new file mode 100644
index 000000000000..c7602b5362ad
--- /dev/null
+++ b/drivers/usb/dwc3/dwc3-st.c
@@ -0,0 +1,367 @@
+/**
+ * dwc3-st.c Support for dwc3 platform devices on ST Microelectronics platforms
+ *
+ * This is a small driver for the dwc3 to provide the glue logic
+ * to configure the controller. Tested on STi platforms.
+ *
+ * Copyright (C) 2014 Stmicroelectronics
+ *
+ * Author: Giuseppe Cavallaro <peppe.cavallaro@st.com>
+ * Contributors: Aymen Bouattay <aymen.bouattay@st.com>
+ *               Peter Griffin <peter.griffin@linaro.org>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * Inspired by dwc3-omap.c and dwc3-exynos.c.
+ */
+
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <linux/kernel.h>
+#include <linux/mfd/syscon.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/regmap.h>
+#include <linux/reset.h>
+#include <linux/usb/of.h>
+
+#include "core.h"
+#include "io.h"
+
+/* glue registers */
+#define CLKRST_CTRL		0x00
+#define AUX_CLK_EN		BIT(0)
+#define SW_PIPEW_RESET_N	BIT(4)
+#define EXT_CFG_RESET_N		BIT(8)
+/*
+ * 1'b0 : The host controller complies with the xHCI revision 0.96
+ * 1'b1 : The host controller complies with the xHCI revision 1.0
+ */
+#define XHCI_REVISION		BIT(12)
+
+#define USB2_VBUS_MNGMNT_SEL1	0x2C
+/*
+ * For all fields in USB2_VBUS_MNGMNT_SEL1
+ * 2’b00 : Override value from Reg 0x30 is selected
+ * 2’b01 : utmiotg_<signal_name> from usb3_top is selected
+ * 2’b10 : pipew_<signal_name> from PIPEW instance is selected
+ * 2’b11 : value is 1'b0
+ */
+#define USB2_VBUS_REG30		0x0
+#define USB2_VBUS_UTMIOTG	0x1
+#define USB2_VBUS_PIPEW		0x2
+#define USB2_VBUS_ZERO		0x3
+
+#define SEL_OVERRIDE_VBUSVALID(n)	(n << 0)
+#define SEL_OVERRIDE_POWERPRESENT(n)	(n << 4)
+#define SEL_OVERRIDE_BVALID(n)		(n << 8)
+
+/* Static DRD configuration */
+#define USB3_CONTROL_MASK		0xf77
+
+#define USB3_DEVICE_NOT_HOST		BIT(0)
+#define USB3_FORCE_VBUSVALID		BIT(1)
+#define USB3_DELAY_VBUSVALID		BIT(2)
+#define USB3_SEL_FORCE_OPMODE		BIT(4)
+#define USB3_FORCE_OPMODE(n)		(n << 5)
+#define USB3_SEL_FORCE_DPPULLDOWN2	BIT(8)
+#define USB3_FORCE_DPPULLDOWN2		BIT(9)
+#define USB3_SEL_FORCE_DMPULLDOWN2	BIT(10)
+#define USB3_FORCE_DMPULLDOWN2		BIT(11)
+
+/**
+ * struct st_dwc3 - dwc3-st driver private structure
+ * @dev:		device pointer
+ * @glue_base:		ioaddr for the glue registers
+ * @regmap:		regmap pointer for getting syscfg
+ * @syscfg_reg_off:	usb syscfg control offset
+ * @dr_mode:		drd static host/device config
+ * @rstc_pwrdn:		rest controller for powerdown signal
+ * @rstc_rst:		reset controller for softreset signal
+ */
+
+struct st_dwc3 {
+	struct device *dev;
+	void __iomem *glue_base;
+	struct regmap *regmap;
+	int syscfg_reg_off;
+	enum usb_dr_mode dr_mode;
+	struct reset_control *rstc_pwrdn;
+	struct reset_control *rstc_rst;
+};
+
+static inline u32 st_dwc3_readl(void __iomem *base, u32 offset)
+{
+	return readl_relaxed(base + offset);
+}
+
+static inline void st_dwc3_writel(void __iomem *base, u32 offset, u32 value)
+{
+	writel_relaxed(value, base + offset);
+}
+
+/**
+ * st_dwc3_drd_init: program the port
+ * @dwc3_data: driver private structure
+ * Description: this function is to program the port as either host or device
+ * according to the static configuration passed from devicetree.
+ * OTG and dual role are not yet supported!
+ */
+static int st_dwc3_drd_init(struct st_dwc3 *dwc3_data)
+{
+	u32 val;
+	int err;
+
+	err = regmap_read(dwc3_data->regmap, dwc3_data->syscfg_reg_off, &val);
+	if (err)
+		return err;
+
+	val &= USB3_CONTROL_MASK;
+
+	switch (dwc3_data->dr_mode) {
+	case USB_DR_MODE_PERIPHERAL:
+
+		val &= ~(USB3_FORCE_VBUSVALID | USB3_DELAY_VBUSVALID
+			| USB3_SEL_FORCE_OPMODE | USB3_FORCE_OPMODE(0x3)
+			| USB3_SEL_FORCE_DPPULLDOWN2 | USB3_FORCE_DPPULLDOWN2
+			| USB3_SEL_FORCE_DMPULLDOWN2 | USB3_FORCE_DMPULLDOWN2);
+
+		val |= USB3_DEVICE_NOT_HOST;
+
+		dev_dbg(dwc3_data->dev, "Configuring as Device\n");
+		break;
+
+	case USB_DR_MODE_HOST:
+
+		val &= ~(USB3_DEVICE_NOT_HOST | USB3_FORCE_VBUSVALID
+			| USB3_SEL_FORCE_OPMODE	| USB3_FORCE_OPMODE(0x3)
+			| USB3_SEL_FORCE_DPPULLDOWN2 | USB3_FORCE_DPPULLDOWN2
+			| USB3_SEL_FORCE_DMPULLDOWN2 | USB3_FORCE_DMPULLDOWN2);
+
+		/*
+		 * USB3_DELAY_VBUSVALID is ANDed with USB_C_VBUSVALID. Thus,
+		 * when set to ‘0‘, it can delay the arrival of VBUSVALID
+		 * information to VBUSVLDEXT2 input of the pico PHY.
+		 * We don't want to do that so we set the bit to '1'.
+		 */
+
+		val |= USB3_DELAY_VBUSVALID;
+
+		dev_dbg(dwc3_data->dev, "Configuring as Host\n");
+		break;
+
+	default:
+		dev_err(dwc3_data->dev, "Unsupported mode of operation %d\n",
+			dwc3_data->dr_mode);
+		return -EINVAL;
+	}
+
+	return regmap_write(dwc3_data->regmap, dwc3_data->syscfg_reg_off, val);
+}
+
+/**
+ * st_dwc3_init: init the controller via glue logic
+ * @dwc3_data: driver private structure
+ */
+static void st_dwc3_init(struct st_dwc3 *dwc3_data)
+{
+	u32 reg = st_dwc3_readl(dwc3_data->glue_base, CLKRST_CTRL);
+
+	reg |= AUX_CLK_EN | EXT_CFG_RESET_N | XHCI_REVISION;
+	reg &= ~SW_PIPEW_RESET_N;
+	st_dwc3_writel(dwc3_data->glue_base, CLKRST_CTRL, reg);
+
+	/* configure mux for vbus, powerpresent and bvalid signals */
+	reg = st_dwc3_readl(dwc3_data->glue_base, USB2_VBUS_MNGMNT_SEL1);
+
+	reg |= SEL_OVERRIDE_VBUSVALID(USB2_VBUS_UTMIOTG) |
+		SEL_OVERRIDE_POWERPRESENT(USB2_VBUS_UTMIOTG) |
+		SEL_OVERRIDE_BVALID(USB2_VBUS_UTMIOTG);
+
+	st_dwc3_writel(dwc3_data->glue_base, USB2_VBUS_MNGMNT_SEL1, reg);
+
+	reg = st_dwc3_readl(dwc3_data->glue_base, CLKRST_CTRL);
+	reg |= SW_PIPEW_RESET_N;
+	st_dwc3_writel(dwc3_data->glue_base, CLKRST_CTRL, reg);
+}
+
+static int st_dwc3_probe(struct platform_device *pdev)
+{
+	struct st_dwc3 *dwc3_data;
+	struct resource *res;
+	struct device *dev = &pdev->dev;
+	struct device_node *node = dev->of_node, *child;
+	struct regmap *regmap;
+	int ret;
+
+	dwc3_data = devm_kzalloc(dev, sizeof(*dwc3_data), GFP_KERNEL);
+	if (!dwc3_data)
+		return -ENOMEM;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "reg-glue");
+	dwc3_data->glue_base = devm_ioremap_resource(dev, res);
+	if (IS_ERR(dwc3_data->glue_base))
+		return PTR_ERR(dwc3_data->glue_base);
+
+	regmap = syscon_regmap_lookup_by_phandle(node, "st,syscfg");
+	if (IS_ERR(regmap))
+		return PTR_ERR(regmap);
+
+	dma_set_coherent_mask(dev, dev->coherent_dma_mask);
+	dwc3_data->dev = dev;
+	dwc3_data->regmap = regmap;
+
+	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "syscfg-reg");
+	if (!res) {
+		ret = -ENXIO;
+		goto undo_platform_dev_alloc;
+	}
+
+	dwc3_data->syscfg_reg_off = res->start;
+
+	dev_vdbg(&pdev->dev, "glue-logic addr 0x%p, syscfg-reg offset 0x%x\n",
+		 dwc3_data->glue_base, dwc3_data->syscfg_reg_off);
+
+	dwc3_data->rstc_pwrdn = devm_reset_control_get(dev, "powerdown");
+	if (IS_ERR(dwc3_data->rstc_pwrdn)) {
+		dev_err(&pdev->dev, "could not get power controller\n");
+		ret = PTR_ERR(dwc3_data->rstc_pwrdn);
+		goto undo_platform_dev_alloc;
+	}
+
+	/* Manage PowerDown */
+	reset_control_deassert(dwc3_data->rstc_pwrdn);
+
+	dwc3_data->rstc_rst = devm_reset_control_get(dev, "softreset");
+	if (IS_ERR(dwc3_data->rstc_rst)) {
+		dev_err(&pdev->dev, "could not get reset controller\n");
+		ret = PTR_ERR(dwc3_data->rstc_pwrdn);
+		goto undo_powerdown;
+	}
+
+	/* Manage SoftReset */
+	reset_control_deassert(dwc3_data->rstc_rst);
+
+	child = of_get_child_by_name(node, "dwc3");
+	if (!child) {
+		dev_err(&pdev->dev, "failed to find dwc3 core node\n");
+		ret = -ENODEV;
+		goto undo_softreset;
+	}
+
+	dwc3_data->dr_mode = of_usb_get_dr_mode(child);
+
+	/* Allocate and initialize the core */
+	ret = of_platform_populate(node, NULL, NULL, dev);
+	if (ret) {
+		dev_err(dev, "failed to add dwc3 core\n");
+		goto undo_softreset;
+	}
+
+	/*
+	 * Configure the USB port as device or host according to the static
+	 * configuration passed from DT.
+	 * DRD is the only mode currently supported so this will be enhanced
+	 * as soon as OTG is available.
+	 */
+	ret = st_dwc3_drd_init(dwc3_data);
+	if (ret) {
+		dev_err(dev, "drd initialisation failed\n");
+		goto undo_softreset;
+	}
+
+	/* ST glue logic init */
+	st_dwc3_init(dwc3_data);
+
+	platform_set_drvdata(pdev, dwc3_data);
+	return 0;
+
+undo_softreset:
+	reset_control_assert(dwc3_data->rstc_rst);
+undo_powerdown:
+	reset_control_assert(dwc3_data->rstc_pwrdn);
+undo_platform_dev_alloc:
+	platform_device_put(pdev);
+	return ret;
+}
+
+static int st_dwc3_remove(struct platform_device *pdev)
+{
+	struct st_dwc3 *dwc3_data = platform_get_drvdata(pdev);
+
+	of_platform_depopulate(&pdev->dev);
+
+	reset_control_assert(dwc3_data->rstc_pwrdn);
+	reset_control_assert(dwc3_data->rstc_rst);
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int st_dwc3_suspend(struct device *dev)
+{
+	struct st_dwc3 *dwc3_data = dev_get_drvdata(dev);
+
+	reset_control_assert(dwc3_data->rstc_pwrdn);
+	reset_control_assert(dwc3_data->rstc_rst);
+
+	pinctrl_pm_select_sleep_state(dev);
+
+	return 0;
+}
+
+static int st_dwc3_resume(struct device *dev)
+{
+	struct st_dwc3 *dwc3_data = dev_get_drvdata(dev);
+	int ret;
+
+	pinctrl_pm_select_default_state(dev);
+
+	reset_control_deassert(dwc3_data->rstc_pwrdn);
+	reset_control_deassert(dwc3_data->rstc_rst);
+
+	ret = st_dwc3_drd_init(dwc3_data);
+	if (ret) {
+		dev_err(dev, "drd initialisation failed\n");
+		return ret;
+	}
+
+	/* ST glue logic init */
+	st_dwc3_init(dwc3_data);
+
+	return 0;
+}
+#endif /* CONFIG_PM_SLEEP */
+
+static SIMPLE_DEV_PM_OPS(st_dwc3_dev_pm_ops, st_dwc3_suspend, st_dwc3_resume);
+
+static const struct of_device_id st_dwc3_match[] = {
+	{ .compatible = "st,stih407-dwc3" },
+	{ /* sentinel */ },
+};
+
+MODULE_DEVICE_TABLE(of, st_dwc3_match);
+
+static struct platform_driver st_dwc3_driver = {
+	.probe = st_dwc3_probe,
+	.remove = st_dwc3_remove,
+	.driver = {
+		.name = "usb-st-dwc3",
+		.of_match_table = st_dwc3_match,
+		.pm = &st_dwc3_dev_pm_ops,
+	},
+};
+
+module_platform_driver(st_dwc3_driver);
+
+MODULE_AUTHOR("Giuseppe Cavallaro <peppe.cavallaro@st.com>");
+MODULE_DESCRIPTION("DesignWare USB3 STi Glue Layer");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/dwc3/ep0.c b/drivers/usb/dwc3/ep0.c
index 21a352079bc2..b35938777dde 100644
--- a/drivers/usb/dwc3/ep0.c
+++ b/drivers/usb/dwc3/ep0.c
@@ -31,6 +31,7 @@
 #include <linux/usb/composite.h>
 
 #include "core.h"
+#include "debug.h"
 #include "gadget.h"
 #include "io.h"
 
@@ -65,7 +66,7 @@ static int dwc3_ep0_start_trans(struct dwc3 *dwc, u8 epnum, dma_addr_t buf_dma,
 
 	dep = dwc->eps[epnum];
 	if (dep->flags & DWC3_EP_BUSY) {
-		dev_vdbg(dwc->dev, "%s: still busy\n", dep->name);
+		dwc3_trace(trace_dwc3_ep0, "%s still busy", dep->name);
 		return 0;
 	}
 
@@ -88,7 +89,8 @@ static int dwc3_ep0_start_trans(struct dwc3 *dwc, u8 epnum, dma_addr_t buf_dma,
 	ret = dwc3_send_gadget_ep_cmd(dwc, dep->number,
 			DWC3_DEPCMD_STARTTRANSFER, &params);
 	if (ret < 0) {
-		dev_dbg(dwc->dev, "failed to send STARTTRANSFER command\n");
+		dwc3_trace(trace_dwc3_ep0, "%s STARTTRANSFER failed",
+				dep->name);
 		return ret;
 	}
 
@@ -153,7 +155,8 @@ static int __dwc3_gadget_ep0_queue(struct dwc3_ep *dep,
 		if (dwc->ep0state == EP0_STATUS_PHASE)
 			__dwc3_ep0_do_control_status(dwc, dwc->eps[direction]);
 		else
-			dev_dbg(dwc->dev, "too early for delayed status\n");
+			dwc3_trace(trace_dwc3_ep0,
+					"too early for delayed status");
 
 		return 0;
 	}
@@ -217,7 +220,8 @@ int dwc3_gadget_ep0_queue(struct usb_ep *ep, struct usb_request *request,
 
 	spin_lock_irqsave(&dwc->lock, flags);
 	if (!dep->endpoint.desc) {
-		dev_dbg(dwc->dev, "trying to queue request %p to disabled %s\n",
+		dwc3_trace(trace_dwc3_ep0,
+				"trying to queue request %p to disabled %s",
 				request, dep->name);
 		ret = -ESHUTDOWN;
 		goto out;
@@ -229,7 +233,8 @@ int dwc3_gadget_ep0_queue(struct usb_ep *ep, struct usb_request *request,
 		goto out;
 	}
 
-	dev_vdbg(dwc->dev, "queueing request %p to %s length %d, state '%s'\n",
+	dwc3_trace(trace_dwc3_ep0,
+			"queueing request %p to %s length %d state '%s'",
 			request, dep->name, request->length,
 			dwc3_ep0_state_string(dwc->ep0state));
 
@@ -485,12 +490,13 @@ static int dwc3_ep0_set_address(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 
 	addr = le16_to_cpu(ctrl->wValue);
 	if (addr > 127) {
-		dev_dbg(dwc->dev, "invalid device address %d\n", addr);
+		dwc3_trace(trace_dwc3_ep0, "invalid device address %d", addr);
 		return -EINVAL;
 	}
 
 	if (state == USB_STATE_CONFIGURED) {
-		dev_dbg(dwc->dev, "trying to set address when configured\n");
+		dwc3_trace(trace_dwc3_ep0,
+				"trying to set address when configured");
 		return -EINVAL;
 	}
 
@@ -556,7 +562,7 @@ static int dwc3_ep0_set_config(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 			dwc3_writel(dwc->regs, DWC3_DCTL, reg);
 
 			dwc->resize_fifos = true;
-			dev_dbg(dwc->dev, "resize fifos flag SET\n");
+			dwc3_trace(trace_dwc3_ep0, "resize FIFOs flag SET");
 		}
 		break;
 
@@ -680,35 +686,35 @@ static int dwc3_ep0_std_request(struct dwc3 *dwc, struct usb_ctrlrequest *ctrl)
 
 	switch (ctrl->bRequest) {
 	case USB_REQ_GET_STATUS:
-		dev_vdbg(dwc->dev, "USB_REQ_GET_STATUS\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_GET_STATUS\n");
 		ret = dwc3_ep0_handle_status(dwc, ctrl);
 		break;
 	case USB_REQ_CLEAR_FEATURE:
-		dev_vdbg(dwc->dev, "USB_REQ_CLEAR_FEATURE\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_CLEAR_FEATURE\n");
 		ret = dwc3_ep0_handle_feature(dwc, ctrl, 0);
 		break;
 	case USB_REQ_SET_FEATURE:
-		dev_vdbg(dwc->dev, "USB_REQ_SET_FEATURE\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_FEATURE\n");
 		ret = dwc3_ep0_handle_feature(dwc, ctrl, 1);
 		break;
 	case USB_REQ_SET_ADDRESS:
-		dev_vdbg(dwc->dev, "USB_REQ_SET_ADDRESS\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_ADDRESS\n");
 		ret = dwc3_ep0_set_address(dwc, ctrl);
 		break;
 	case USB_REQ_SET_CONFIGURATION:
-		dev_vdbg(dwc->dev, "USB_REQ_SET_CONFIGURATION\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_CONFIGURATION\n");
 		ret = dwc3_ep0_set_config(dwc, ctrl);
 		break;
 	case USB_REQ_SET_SEL:
-		dev_vdbg(dwc->dev, "USB_REQ_SET_SEL\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_SEL\n");
 		ret = dwc3_ep0_set_sel(dwc, ctrl);
 		break;
 	case USB_REQ_SET_ISOCH_DELAY:
-		dev_vdbg(dwc->dev, "USB_REQ_SET_ISOCH_DELAY\n");
+		dwc3_trace(trace_dwc3_ep0, "USB_REQ_SET_ISOCH_DELAY\n");
 		ret = dwc3_ep0_set_isoch_delay(dwc, ctrl);
 		break;
 	default:
-		dev_vdbg(dwc->dev, "Forwarding to gadget driver\n");
+		dwc3_trace(trace_dwc3_ep0, "Forwarding to gadget driver\n");
 		ret = dwc3_ep0_delegate_req(dwc, ctrl);
 		break;
 	}
@@ -726,6 +732,8 @@ static void dwc3_ep0_inspect_setup(struct dwc3 *dwc,
 	if (!dwc->gadget_driver)
 		goto out;
 
+	trace_dwc3_ctrl_req(ctrl);
+
 	len = le16_to_cpu(ctrl->wLength);
 	if (!len) {
 		dwc->three_stage_setup = false;
@@ -774,7 +782,7 @@ static void dwc3_ep0_complete_data(struct dwc3 *dwc,
 
 	status = DWC3_TRB_SIZE_TRBSTS(trb->size);
 	if (status == DWC3_TRBSTS_SETUP_PENDING) {
-		dev_dbg(dwc->dev, "Setup Pending received\n");
+		dwc3_trace(trace_dwc3_ep0, "Setup Pending received");
 
 		if (r)
 			dwc3_gadget_giveback(ep0, r, -ECONNRESET);
@@ -834,7 +842,7 @@ static void dwc3_ep0_complete_status(struct dwc3 *dwc,
 
 		ret = dwc3_gadget_set_test_mode(dwc, dwc->test_mode_nr);
 		if (ret < 0) {
-			dev_dbg(dwc->dev, "Invalid Test #%d\n",
+			dwc3_trace(trace_dwc3_ep0, "Invalid Test #%d",
 					dwc->test_mode_nr);
 			dwc3_ep0_stall_and_restart(dwc);
 			return;
@@ -843,7 +851,7 @@ static void dwc3_ep0_complete_status(struct dwc3 *dwc,
 
 	status = DWC3_TRB_SIZE_TRBSTS(trb->size);
 	if (status == DWC3_TRBSTS_SETUP_PENDING)
-		dev_dbg(dwc->dev, "Setup Pending received\n");
+		dwc3_trace(trace_dwc3_ep0, "Setup Pending received\n");
 
 	dwc->ep0state = EP0_SETUP_PHASE;
 	dwc3_ep0_out_start(dwc);
@@ -860,17 +868,17 @@ static void dwc3_ep0_xfer_complete(struct dwc3 *dwc,
 
 	switch (dwc->ep0state) {
 	case EP0_SETUP_PHASE:
-		dev_vdbg(dwc->dev, "Inspecting Setup Bytes\n");
+		dwc3_trace(trace_dwc3_ep0, "Setup Phase");
 		dwc3_ep0_inspect_setup(dwc, event);
 		break;
 
 	case EP0_DATA_PHASE:
-		dev_vdbg(dwc->dev, "Data Phase\n");
+		dwc3_trace(trace_dwc3_ep0, "Data Phase");
 		dwc3_ep0_complete_data(dwc, event);
 		break;
 
 	case EP0_STATUS_PHASE:
-		dev_vdbg(dwc->dev, "Status Phase\n");
+		dwc3_trace(trace_dwc3_ep0, "Status Phase");
 		dwc3_ep0_complete_status(dwc, event);
 		break;
 	default:
@@ -946,7 +954,7 @@ static int dwc3_ep0_start_control_status(struct dwc3_ep *dep)
 static void __dwc3_ep0_do_control_status(struct dwc3 *dwc, struct dwc3_ep *dep)
 {
 	if (dwc->resize_fifos) {
-		dev_dbg(dwc->dev, "starting to resize fifos\n");
+		dwc3_trace(trace_dwc3_ep0, "Resizing FIFOs");
 		dwc3_gadget_resize_tx_fifos(dwc);
 		dwc->resize_fifos = 0;
 	}
@@ -987,7 +995,7 @@ static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 
 	switch (event->status) {
 	case DEPEVT_STATUS_CONTROL_DATA:
-		dev_vdbg(dwc->dev, "Control Data\n");
+		dwc3_trace(trace_dwc3_ep0, "Control Data");
 
 		/*
 		 * We already have a DATA transfer in the controller's cache,
@@ -1001,7 +1009,8 @@ static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 		if (dwc->ep0_expect_in != event->endpoint_number) {
 			struct dwc3_ep	*dep = dwc->eps[dwc->ep0_expect_in];
 
-			dev_vdbg(dwc->dev, "Wrong direction for Data phase\n");
+			dwc3_trace(trace_dwc3_ep0,
+					"Wrong direction for Data phase");
 			dwc3_ep0_end_control_data(dwc, dep);
 			dwc3_ep0_stall_and_restart(dwc);
 			return;
@@ -1013,13 +1022,13 @@ static void dwc3_ep0_xfernotready(struct dwc3 *dwc,
 		if (dwc->ep0_next_event != DWC3_EP0_NRDY_STATUS)
 			return;
 
-		dev_vdbg(dwc->dev, "Control Status\n");
+		dwc3_trace(trace_dwc3_ep0, "Control Status");
 
 		dwc->ep0state = EP0_STATUS_PHASE;
 
 		if (dwc->delayed_status) {
 			WARN_ON_ONCE(event->endpoint_number != 1);
-			dev_vdbg(dwc->dev, "Mass Storage delayed status\n");
+			dwc3_trace(trace_dwc3_ep0, "Delayed Status");
 			return;
 		}
 
@@ -1032,7 +1041,7 @@ void dwc3_ep0_interrupt(struct dwc3 *dwc,
 {
 	u8			epnum = event->endpoint_number;
 
-	dev_dbg(dwc->dev, "%s while ep%d%s in state '%s'\n",
+	dwc3_trace(trace_dwc3_ep0, "%s while ep%d%s in state '%s'",
 			dwc3_ep_event_string(event->endpoint_event),
 			epnum >> 1, (epnum & 1) ? "in" : "out",
 			dwc3_ep0_state_string(dwc->ep0state));
diff --git a/drivers/usb/dwc3/gadget.c b/drivers/usb/dwc3/gadget.c
index 349cacc577d8..3818b26bfc05 100644
--- a/drivers/usb/dwc3/gadget.c
+++ b/drivers/usb/dwc3/gadget.c
@@ -30,6 +30,7 @@
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
 
+#include "debug.h"
 #include "core.h"
 #include "gadget.h"
 #include "io.h"
@@ -266,107 +267,19 @@ void dwc3_gadget_giveback(struct dwc3_ep *dep, struct dwc3_request *req,
 	dev_dbg(dwc->dev, "request %p from %s completed %d/%d ===> %d\n",
 			req, dep->name, req->request.actual,
 			req->request.length, status);
+	trace_dwc3_gadget_giveback(req);
 
 	spin_unlock(&dwc->lock);
-	req->request.complete(&dep->endpoint, &req->request);
+	usb_gadget_giveback_request(&dep->endpoint, &req->request);
 	spin_lock(&dwc->lock);
 }
 
-static const char *dwc3_gadget_ep_cmd_string(u8 cmd)
-{
-	switch (cmd) {
-	case DWC3_DEPCMD_DEPSTARTCFG:
-		return "Start New Configuration";
-	case DWC3_DEPCMD_ENDTRANSFER:
-		return "End Transfer";
-	case DWC3_DEPCMD_UPDATETRANSFER:
-		return "Update Transfer";
-	case DWC3_DEPCMD_STARTTRANSFER:
-		return "Start Transfer";
-	case DWC3_DEPCMD_CLEARSTALL:
-		return "Clear Stall";
-	case DWC3_DEPCMD_SETSTALL:
-		return "Set Stall";
-	case DWC3_DEPCMD_GETEPSTATE:
-		return "Get Endpoint State";
-	case DWC3_DEPCMD_SETTRANSFRESOURCE:
-		return "Set Endpoint Transfer Resource";
-	case DWC3_DEPCMD_SETEPCONFIG:
-		return "Set Endpoint Configuration";
-	default:
-		return "UNKNOWN command";
-	}
-}
-
-static const char *dwc3_gadget_generic_cmd_string(u8 cmd)
-{
-	switch (cmd) {
-	case DWC3_DGCMD_SET_LMP:
-		return "Set LMP";
-	case DWC3_DGCMD_SET_PERIODIC_PAR:
-		return "Set Periodic Parameters";
-	case DWC3_DGCMD_XMIT_FUNCTION:
-		return "Transmit Function Wake Device Notification";
-	case DWC3_DGCMD_SET_SCRATCHPAD_ADDR_LO:
-		return "Set Scratchpad Buffer Array Address Lo";
-	case DWC3_DGCMD_SET_SCRATCHPAD_ADDR_HI:
-		return "Set Scratchpad Buffer Array Address Hi";
-	case DWC3_DGCMD_SELECTED_FIFO_FLUSH:
-		return "Selected FIFO Flush";
-	case DWC3_DGCMD_ALL_FIFO_FLUSH:
-		return "All FIFO Flush";
-	case DWC3_DGCMD_SET_ENDPOINT_NRDY:
-		return "Set Endpoint NRDY";
-	case DWC3_DGCMD_RUN_SOC_BUS_LOOPBACK:
-		return "Run SoC Bus Loopback Test";
-	default:
-		return "UNKNOWN";
-	}
-}
-
-static const char *dwc3_gadget_link_string(enum dwc3_link_state link_state)
-{
-	switch (link_state) {
-	case DWC3_LINK_STATE_U0:
-		return "U0";
-	case DWC3_LINK_STATE_U1:
-		return "U1";
-	case DWC3_LINK_STATE_U2:
-		return "U2";
-	case DWC3_LINK_STATE_U3:
-		return "U3";
-	case DWC3_LINK_STATE_SS_DIS:
-		return "SS.Disabled";
-	case DWC3_LINK_STATE_RX_DET:
-		return "RX.Detect";
-	case DWC3_LINK_STATE_SS_INACT:
-		return "SS.Inactive";
-	case DWC3_LINK_STATE_POLL:
-		return "Polling";
-	case DWC3_LINK_STATE_RECOV:
-		return "Recovery";
-	case DWC3_LINK_STATE_HRESET:
-		return "Hot Reset";
-	case DWC3_LINK_STATE_CMPLY:
-		return "Compliance";
-	case DWC3_LINK_STATE_LPBK:
-		return "Loopback";
-	case DWC3_LINK_STATE_RESET:
-		return "Reset";
-	case DWC3_LINK_STATE_RESUME:
-		return "Resume";
-	default:
-		return "UNKNOWN link state\n";
-	}
-}
-
-int dwc3_send_gadget_generic_command(struct dwc3 *dwc, int cmd, u32 param)
+int dwc3_send_gadget_generic_command(struct dwc3 *dwc, unsigned cmd, u32 param)
 {
 	u32		timeout = 500;
 	u32		reg;
 
-	dev_vdbg(dwc->dev, "generic cmd '%s' [%d] param %08x\n",
-			dwc3_gadget_generic_cmd_string(cmd), cmd, param);
+	trace_dwc3_gadget_generic_cmd(cmd, param);
 
 	dwc3_writel(dwc->regs, DWC3_DGCMDPAR, param);
 	dwc3_writel(dwc->regs, DWC3_DGCMD, cmd | DWC3_DGCMD_CMDACT);
@@ -397,10 +310,7 @@ int dwc3_send_gadget_ep_cmd(struct dwc3 *dwc, unsigned ep,
 	u32			timeout = 500;
 	u32			reg;
 
-	dev_vdbg(dwc->dev, "%s: cmd '%s' [%d] params %08x %08x %08x\n",
-			dep->name,
-			dwc3_gadget_ep_cmd_string(cmd), cmd, params->param0,
-			params->param1, params->param2);
+	trace_dwc3_gadget_ep_cmd(dep, cmd, params);
 
 	dwc3_writel(dwc->regs, DWC3_DEPCMDPAR0(ep), params->param0);
 	dwc3_writel(dwc->regs, DWC3_DEPCMDPAR1(ep), params->param1);
@@ -527,7 +437,7 @@ static int dwc3_gadget_set_ep_config(struct dwc3 *dwc, struct dwc3_ep *dep,
 		dep->stream_capable = true;
 	}
 
-	if (usb_endpoint_xfer_isoc(desc))
+	if (!usb_endpoint_xfer_control(desc))
 		params.param1 |= DWC3_DEPCFG_XFER_IN_PROGRESS_EN;
 
 	/*
@@ -789,17 +699,16 @@ static struct usb_request *dwc3_gadget_ep_alloc_request(struct usb_ep *ep,
 {
 	struct dwc3_request		*req;
 	struct dwc3_ep			*dep = to_dwc3_ep(ep);
-	struct dwc3			*dwc = dep->dwc;
 
 	req = kzalloc(sizeof(*req), gfp_flags);
-	if (!req) {
-		dev_err(dwc->dev, "not enough memory\n");
+	if (!req)
 		return NULL;
-	}
 
 	req->epnum	= dep->number;
 	req->dep	= dep;
 
+	trace_dwc3_alloc_request(req);
+
 	return &req->request;
 }
 
@@ -808,6 +717,7 @@ static void dwc3_gadget_ep_free_request(struct usb_ep *ep,
 {
 	struct dwc3_request		*req = to_dwc3_request(request);
 
+	trace_dwc3_free_request(req);
 	kfree(req);
 }
 
@@ -889,6 +799,8 @@ static void dwc3_prepare_one_trb(struct dwc3_ep *dep,
 		trb->ctrl |= DWC3_TRB_CTRL_SID_SOFN(req->request.stream_id);
 
 	trb->ctrl |= DWC3_TRB_CTRL_HWO;
+
+	trace_dwc3_prepare_trb(dep, trb);
 }
 
 /*
@@ -1225,16 +1137,18 @@ static int dwc3_gadget_ep_queue(struct usb_ep *ep, struct usb_request *request,
 
 	int				ret;
 
+	spin_lock_irqsave(&dwc->lock, flags);
 	if (!dep->endpoint.desc) {
 		dev_dbg(dwc->dev, "trying to queue request %p to disabled %s\n",
 				request, ep->name);
+		spin_unlock_irqrestore(&dwc->lock, flags);
 		return -ESHUTDOWN;
 	}
 
 	dev_vdbg(dwc->dev, "queing request %p to %s length %d\n",
 			request, ep->name, request->length);
+	trace_dwc3_ep_queue(req);
 
-	spin_lock_irqsave(&dwc->lock, flags);
 	ret = __dwc3_gadget_ep_queue(dep, req);
 	spin_unlock_irqrestore(&dwc->lock, flags);
 
@@ -1253,6 +1167,8 @@ static int dwc3_gadget_ep_dequeue(struct usb_ep *ep,
 	unsigned long			flags;
 	int				ret = 0;
 
+	trace_dwc3_ep_dequeue(req);
+
 	spin_lock_irqsave(&dwc->lock, flags);
 
 	list_for_each_entry(r, &dep->request_list, list) {
@@ -1743,11 +1659,8 @@ static int dwc3_gadget_init_hw_endpoints(struct dwc3 *dwc,
 		u8 epnum = (i << 1) | (!!direction);
 
 		dep = kzalloc(sizeof(*dep), GFP_KERNEL);
-		if (!dep) {
-			dev_err(dwc->dev, "can't allocate endpoint %d\n",
-					epnum);
+		if (!dep)
 			return -ENOMEM;
-		}
 
 		dep->dwc = dwc;
 		dep->number = epnum;
@@ -1846,6 +1759,8 @@ static int __dwc3_cleanup_done_trbs(struct dwc3 *dwc, struct dwc3_ep *dep,
 	unsigned int		s_pkt = 0;
 	unsigned int		trb_status;
 
+	trace_dwc3_complete_trb(dep, trb);
+
 	if ((trb->ctrl & DWC3_TRB_CTRL_HWO) && status != -ESHUTDOWN)
 		/*
 		 * We continue despite the error. There is not much we
@@ -2020,9 +1935,6 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 	if (!(dep->flags & DWC3_EP_ENABLED))
 		return;
 
-	dev_vdbg(dwc->dev, "%s: %s\n", dep->name,
-			dwc3_ep_event_string(event->endpoint_event));
-
 	if (epnum == 0 || epnum == 1) {
 		dwc3_ep0_interrupt(dwc, event);
 		return;
@@ -2041,12 +1953,6 @@ static void dwc3_endpoint_interrupt(struct dwc3 *dwc,
 		dwc3_endpoint_transfer_complete(dwc, dep, event);
 		break;
 	case DWC3_DEPEVT_XFERINPROGRESS:
-		if (!usb_endpoint_xfer_isoc(dep->endpoint.desc)) {
-			dev_dbg(dwc->dev, "%s is not an Isochronous endpoint\n",
-					dep->name);
-			return;
-		}
-
 		dwc3_endpoint_transfer_complete(dwc, dep, event);
 		break;
 	case DWC3_DEPEVT_XFERNOTREADY:
@@ -2215,8 +2121,6 @@ static void dwc3_gadget_disconnect_interrupt(struct dwc3 *dwc)
 {
 	int			reg;
 
-	dev_vdbg(dwc->dev, "%s\n", __func__);
-
 	reg = dwc3_readl(dwc->regs, DWC3_DCTL);
 	reg &= ~DWC3_DCTL_INITU1ENA;
 	dwc3_writel(dwc->regs, DWC3_DCTL, reg);
@@ -2235,8 +2139,6 @@ static void dwc3_gadget_reset_interrupt(struct dwc3 *dwc)
 {
 	u32			reg;
 
-	dev_vdbg(dwc->dev, "%s\n", __func__);
-
 	/*
 	 * WORKAROUND: DWC3 revisions <1.88a have an issue which
 	 * would cause a missing Disconnect Event if there's a
@@ -2321,8 +2223,6 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
 	u32			reg;
 	u8			speed;
 
-	dev_vdbg(dwc->dev, "%s\n", __func__);
-
 	reg = dwc3_readl(dwc->regs, DWC3_DSTS);
 	speed = reg & DWC3_DSTS_CONNECTSPD;
 	dwc->speed = speed;
@@ -2420,8 +2320,6 @@ static void dwc3_gadget_conndone_interrupt(struct dwc3 *dwc)
 
 static void dwc3_gadget_wakeup_interrupt(struct dwc3 *dwc)
 {
-	dev_vdbg(dwc->dev, "%s\n", __func__);
-
 	/*
 	 * TODO take core out of low power mode when that's
 	 * implemented.
@@ -2526,10 +2424,6 @@ static void dwc3_gadget_linksts_change_interrupt(struct dwc3 *dwc,
 		break;
 	}
 
-	dev_vdbg(dwc->dev, "link change: %s [%d] -> %s [%d]\n",
-			dwc3_gadget_link_string(dwc->link_state),
-			dwc->link_state, dwc3_gadget_link_string(next), next);
-
 	dwc->link_state = next;
 }
 
@@ -2606,6 +2500,8 @@ static void dwc3_gadget_interrupt(struct dwc3 *dwc,
 static void dwc3_process_event_entry(struct dwc3 *dwc,
 		const union dwc3_event *event)
 {
+	trace_dwc3_event(event->raw);
+
 	/* Endpoint IRQ, handle it and return early */
 	if (event->type.is_devspec == 0) {
 		/* depevt */
@@ -2759,7 +2655,6 @@ int dwc3_gadget_init(struct dwc3 *dwc)
 
 	dwc->setup_buf = kzalloc(DWC3_EP0_BOUNCE_SIZE, GFP_KERNEL);
 	if (!dwc->setup_buf) {
-		dev_err(dwc->dev, "failed to allocate setup buffer\n");
 		ret = -ENOMEM;
 		goto err2;
 	}
diff --git a/drivers/usb/dwc3/gadget.h b/drivers/usb/dwc3/gadget.h
index a0ee75b68a80..178ad8982206 100644
--- a/drivers/usb/dwc3/gadget.h
+++ b/drivers/usb/dwc3/gadget.h
@@ -103,60 +103,4 @@ static inline u32 dwc3_gadget_ep_get_transfer_index(struct dwc3 *dwc, u8 number)
 	return DWC3_DEPCMD_GET_RSC_IDX(res_id);
 }
 
-/**
- * dwc3_gadget_event_string - returns event name
- * @event: the event code
- */
-static inline const char *dwc3_gadget_event_string(u8 event)
-{
-	switch (event) {
-	case DWC3_DEVICE_EVENT_DISCONNECT:
-		return "Disconnect";
-	case DWC3_DEVICE_EVENT_RESET:
-		return "Reset";
-	case DWC3_DEVICE_EVENT_CONNECT_DONE:
-		return "Connection Done";
-	case DWC3_DEVICE_EVENT_LINK_STATUS_CHANGE:
-		return "Link Status Change";
-	case DWC3_DEVICE_EVENT_WAKEUP:
-		return "WakeUp";
-	case DWC3_DEVICE_EVENT_EOPF:
-		return "End-Of-Frame";
-	case DWC3_DEVICE_EVENT_SOF:
-		return "Start-Of-Frame";
-	case DWC3_DEVICE_EVENT_ERRATIC_ERROR:
-		return "Erratic Error";
-	case DWC3_DEVICE_EVENT_CMD_CMPL:
-		return "Command Complete";
-	case DWC3_DEVICE_EVENT_OVERFLOW:
-		return "Overflow";
-	}
-
-	return "UNKNOWN";
-}
-
-/**
- * dwc3_ep_event_string - returns event name
- * @event: then event code
- */
-static inline const char *dwc3_ep_event_string(u8 event)
-{
-	switch (event) {
-	case DWC3_DEPEVT_XFERCOMPLETE:
-		return "Transfer Complete";
-	case DWC3_DEPEVT_XFERINPROGRESS:
-		return "Transfer In-Progress";
-	case DWC3_DEPEVT_XFERNOTREADY:
-		return "Transfer Not Ready";
-	case DWC3_DEPEVT_RXTXFIFOEVT:
-		return "FIFO";
-	case DWC3_DEPEVT_STREAMEVT:
-		return "Stream";
-	case DWC3_DEPEVT_EPCMDCMPLT:
-		return "Endpoint Command Complete";
-	}
-
-	return "UNKNOWN";
-}
-
 #endif /* __DRIVERS_USB_DWC3_GADGET_H */
diff --git a/drivers/usb/dwc3/io.h b/drivers/usb/dwc3/io.h
index d94441c14d8c..6a79c8e66bbc 100644
--- a/drivers/usb/dwc3/io.h
+++ b/drivers/usb/dwc3/io.h
@@ -20,27 +20,51 @@
 #define __DRIVERS_USB_DWC3_IO_H
 
 #include <linux/io.h>
-
+#include "trace.h"
+#include "debug.h"
 #include "core.h"
 
 static inline u32 dwc3_readl(void __iomem *base, u32 offset)
 {
+	u32 offs = offset - DWC3_GLOBALS_REGS_START;
+	u32 value;
+
 	/*
 	 * We requested the mem region starting from the Globals address
 	 * space, see dwc3_probe in core.c.
 	 * However, the offsets are given starting from xHCI address space.
 	 */
-	return readl(base + (offset - DWC3_GLOBALS_REGS_START));
+	value = readl(base + offs);
+
+	/*
+	 * When tracing we want to make it easy to find the correct address on
+	 * documentation, so we revert it back to the proper addresses, the
+	 * same way they are described on SNPS documentation
+	 */
+	dwc3_trace(trace_dwc3_readl, "addr %p value %08x",
+			base - DWC3_GLOBALS_REGS_START + offset, value);
+
+	return value;
 }
 
 static inline void dwc3_writel(void __iomem *base, u32 offset, u32 value)
 {
+	u32 offs = offset - DWC3_GLOBALS_REGS_START;
+
 	/*
 	 * We requested the mem region starting from the Globals address
 	 * space, see dwc3_probe in core.c.
 	 * However, the offsets are given starting from xHCI address space.
 	 */
-	writel(value, base + (offset - DWC3_GLOBALS_REGS_START));
+	writel(value, base + offs);
+
+	/*
+	 * When tracing we want to make it easy to find the correct address on
+	 * documentation, so we revert it back to the proper addresses, the
+	 * same way they are described on SNPS documentation
+	 */
+	dwc3_trace(trace_dwc3_writel, "addr %p value %08x",
+			base - DWC3_GLOBALS_REGS_START + offset, value);
 }
 
 #endif /* __DRIVERS_USB_DWC3_IO_H */
diff --git a/drivers/usb/dwc3/trace.c b/drivers/usb/dwc3/trace.c
new file mode 100644
index 000000000000..6cd166412ad0
--- /dev/null
+++ b/drivers/usb/dwc3/trace.c
@@ -0,0 +1,19 @@
+/**
+ * trace.c - DesignWare USB3 DRD Controller Trace Support
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Author: Felipe Balbi <balbi@ti.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#define CREATE_TRACE_POINTS
+#include "trace.h"
diff --git a/drivers/usb/dwc3/trace.h b/drivers/usb/dwc3/trace.h
new file mode 100644
index 000000000000..78aff1da089a
--- /dev/null
+++ b/drivers/usb/dwc3/trace.h
@@ -0,0 +1,220 @@
+/**
+ * trace.h - DesignWare USB3 DRD Controller Trace Support
+ *
+ * Copyright (C) 2014 Texas Instruments Incorporated - http://www.ti.com
+ *
+ * Author: Felipe Balbi <balbi@ti.com>
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2  of
+ * the License as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#undef TRACE_SYSTEM
+#define TRACE_SYSTEM dwc3
+
+#if !defined(__DWC3_TRACE_H) || defined(TRACE_HEADER_MULTI_READ)
+#define __DWC3_TRACE_H
+
+#include <linux/types.h>
+#include <linux/tracepoint.h>
+#include <asm/byteorder.h>
+#include "core.h"
+#include "debug.h"
+
+DECLARE_EVENT_CLASS(dwc3_log_msg,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf),
+	TP_STRUCT__entry(__dynamic_array(char, msg, DWC3_MSG_MAX)),
+	TP_fast_assign(
+		vsnprintf(__get_str(msg), DWC3_MSG_MAX, vaf->fmt, *vaf->va);
+	),
+	TP_printk("%s", __get_str(msg))
+);
+
+DEFINE_EVENT(dwc3_log_msg, dwc3_readl,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(dwc3_log_msg, dwc3_writel,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DEFINE_EVENT(dwc3_log_msg, dwc3_ep0,
+	TP_PROTO(struct va_format *vaf),
+	TP_ARGS(vaf)
+);
+
+DECLARE_EVENT_CLASS(dwc3_log_event,
+	TP_PROTO(u32 event),
+	TP_ARGS(event),
+	TP_STRUCT__entry(
+		__field(u32, event)
+	),
+	TP_fast_assign(
+		__entry->event = event;
+	),
+	TP_printk("event %08x\n", __entry->event)
+);
+
+DEFINE_EVENT(dwc3_log_event, dwc3_event,
+	TP_PROTO(u32 event),
+	TP_ARGS(event)
+);
+
+DECLARE_EVENT_CLASS(dwc3_log_ctrl,
+	TP_PROTO(struct usb_ctrlrequest *ctrl),
+	TP_ARGS(ctrl),
+	TP_STRUCT__entry(
+		__field(struct usb_ctrlrequest *, ctrl)
+	),
+	TP_fast_assign(
+		__entry->ctrl = ctrl;
+	),
+	TP_printk("bRequestType %02x bRequest %02x wValue %04x wIndex %04x wLength %d",
+		__entry->ctrl->bRequestType, __entry->ctrl->bRequest,
+		le16_to_cpu(__entry->ctrl->wValue), le16_to_cpu(__entry->ctrl->wIndex),
+		le16_to_cpu(__entry->ctrl->wLength)
+	)
+);
+
+DEFINE_EVENT(dwc3_log_ctrl, dwc3_ctrl_req,
+	TP_PROTO(struct usb_ctrlrequest *ctrl),
+	TP_ARGS(ctrl)
+);
+
+DECLARE_EVENT_CLASS(dwc3_log_request,
+	TP_PROTO(struct dwc3_request *req),
+	TP_ARGS(req),
+	TP_STRUCT__entry(
+		__field(struct dwc3_request *, req)
+	),
+	TP_fast_assign(
+		__entry->req = req;
+	),
+	TP_printk("%s: req %p length %u/%u ==> %d",
+		__entry->req->dep->name, __entry->req,
+		__entry->req->request.actual, __entry->req->request.length,
+		__entry->req->request.status
+	)
+);
+
+DEFINE_EVENT(dwc3_log_request, dwc3_alloc_request,
+	TP_PROTO(struct dwc3_request *req),
+	TP_ARGS(req)
+);
+
+DEFINE_EVENT(dwc3_log_request, dwc3_free_request,
+	TP_PROTO(struct dwc3_request *req),
+	TP_ARGS(req)
+);
+
+DEFINE_EVENT(dwc3_log_request, dwc3_ep_queue,
+	TP_PROTO(struct dwc3_request *req),
+	TP_ARGS(req)
+);
+
+DEFINE_EVENT(dwc3_log_request, dwc3_ep_dequeue,
+	TP_PROTO(struct dwc3_request *req),
+	TP_ARGS(req)
+);
+
+DEFINE_EVENT(dwc3_log_request, dwc3_gadget_giveback,
+	TP_PROTO(struct dwc3_request *req),
+	TP_ARGS(req)
+);
+
+DECLARE_EVENT_CLASS(dwc3_log_generic_cmd,
+	TP_PROTO(unsigned int cmd, u32 param),
+	TP_ARGS(cmd, param),
+	TP_STRUCT__entry(
+		__field(unsigned int, cmd)
+		__field(u32, param)
+	),
+	TP_fast_assign(
+		__entry->cmd = cmd;
+		__entry->param = param;
+	),
+	TP_printk("cmd '%s' [%d] param %08x\n",
+		dwc3_gadget_generic_cmd_string(__entry->cmd),
+		__entry->cmd, __entry->param
+	)
+);
+
+DEFINE_EVENT(dwc3_log_generic_cmd, dwc3_gadget_generic_cmd,
+	TP_PROTO(unsigned int cmd, u32 param),
+	TP_ARGS(cmd, param)
+);
+
+DECLARE_EVENT_CLASS(dwc3_log_gadget_ep_cmd,
+	TP_PROTO(struct dwc3_ep *dep, unsigned int cmd,
+		struct dwc3_gadget_ep_cmd_params *params),
+	TP_ARGS(dep, cmd, params),
+	TP_STRUCT__entry(
+		__field(struct dwc3_ep *, dep)
+		__field(unsigned int, cmd)
+		__field(struct dwc3_gadget_ep_cmd_params *, params)
+	),
+	TP_fast_assign(
+		__entry->dep = dep;
+		__entry->cmd = cmd;
+		__entry->params = params;
+	),
+	TP_printk("%s: cmd '%s' [%d] params %08x %08x %08x\n",
+		__entry->dep->name, dwc3_gadget_ep_cmd_string(__entry->cmd),
+		__entry->cmd, __entry->params->param0,
+		__entry->params->param1, __entry->params->param2
+	)
+);
+
+DEFINE_EVENT(dwc3_log_gadget_ep_cmd, dwc3_gadget_ep_cmd,
+	TP_PROTO(struct dwc3_ep *dep, unsigned int cmd,
+		struct dwc3_gadget_ep_cmd_params *params),
+	TP_ARGS(dep, cmd, params)
+);
+
+DECLARE_EVENT_CLASS(dwc3_log_trb,
+	TP_PROTO(struct dwc3_ep *dep, struct dwc3_trb *trb),
+	TP_ARGS(dep, trb),
+	TP_STRUCT__entry(
+		__field(struct dwc3_ep *, dep)
+		__field(struct dwc3_trb *, trb)
+	),
+	TP_fast_assign(
+		__entry->dep = dep;
+		__entry->trb = trb;
+	),
+	TP_printk("%s: trb %p bph %08x bpl %08x size %08x ctrl %08x\n",
+		__entry->dep->name, __entry->trb, __entry->trb->bph,
+		__entry->trb->bpl, __entry->trb->size, __entry->trb->ctrl
+	)
+);
+
+DEFINE_EVENT(dwc3_log_trb, dwc3_prepare_trb,
+	TP_PROTO(struct dwc3_ep *dep, struct dwc3_trb *trb),
+	TP_ARGS(dep, trb)
+);
+
+DEFINE_EVENT(dwc3_log_trb, dwc3_complete_trb,
+	TP_PROTO(struct dwc3_ep *dep, struct dwc3_trb *trb),
+	TP_ARGS(dep, trb)
+);
+
+#endif /* __DWC3_TRACE_H */
+
+/* this part has to be here */
+
+#undef TRACE_INCLUDE_PATH
+#define TRACE_INCLUDE_PATH .
+
+#undef TRACE_INCLUDE_FILE
+#define TRACE_INCLUDE_FILE trace
+
+#include <trace/define_trace.h>
diff --git a/drivers/usb/gadget/Kconfig b/drivers/usb/gadget/Kconfig
index 5c822afb6d70..c4880fc0d86e 100644
--- a/drivers/usb/gadget/Kconfig
+++ b/drivers/usb/gadget/Kconfig
@@ -181,6 +181,15 @@ config USB_F_MASS_STORAGE
 config USB_F_FS
 	tristate
 
+config USB_F_UAC1
+	tristate
+
+config USB_F_UAC2
+	tristate
+
+config USB_F_UVC
+	tristate
+
 choice
 	tristate "USB Gadget Drivers"
 	default USB_ETH
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index a186afeaa700..598a67d6ba05 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -3,7 +3,7 @@
 #
 subdir-ccflags-$(CONFIG_USB_GADGET_DEBUG)	:= -DDEBUG
 subdir-ccflags-$(CONFIG_USB_GADGET_VERBOSE)	+= -DVERBOSE_DEBUG
-ccflags-y				+= -I$(PWD)/drivers/usb/gadget/udc
+ccflags-y				+= -I$(srctree)/drivers/usb/gadget/udc
 
 obj-$(CONFIG_USB_LIBCOMPOSITE)	+= libcomposite.o
 libcomposite-y			:= usbstring.o config.o epautoconf.o
diff --git a/drivers/usb/gadget/composite.c b/drivers/usb/gadget/composite.c
index 6935a822ce2b..a8c18df171c3 100644
--- a/drivers/usb/gadget/composite.c
+++ b/drivers/usb/gadget/composite.c
@@ -1956,7 +1956,6 @@ void composite_dev_cleanup(struct usb_composite_dev *cdev)
 	}
 	if (cdev->req) {
 		kfree(cdev->req->buf);
-		usb_ep_dequeue(cdev->gadget->ep0, cdev->req);
 		usb_ep_free_request(cdev->gadget->ep0, cdev->req);
 	}
 	cdev->next_string_id = 0;
@@ -2073,6 +2072,7 @@ static const struct usb_gadget_driver composite_driver_template = {
 	.unbind		= composite_unbind,
 
 	.setup		= composite_setup,
+	.reset		= composite_disconnect,
 	.disconnect	= composite_disconnect,
 
 	.suspend	= composite_suspend,
diff --git a/drivers/usb/gadget/configfs.c b/drivers/usb/gadget/configfs.c
index 811c2c7cc269..34034333f7f6 100644
--- a/drivers/usb/gadget/configfs.c
+++ b/drivers/usb/gadget/configfs.c
@@ -1450,6 +1450,7 @@ static const struct usb_gadget_driver configfs_driver_template = {
 	.unbind         = configfs_composite_unbind,
 
 	.setup          = composite_setup,
+	.reset          = composite_disconnect,
 	.disconnect     = composite_disconnect,
 
 	.max_speed	= USB_SPEED_SUPER,
diff --git a/drivers/usb/gadget/function/Makefile b/drivers/usb/gadget/function/Makefile
index 6d91f21b52a6..90701aa5a826 100644
--- a/drivers/usb/gadget/function/Makefile
+++ b/drivers/usb/gadget/function/Makefile
@@ -2,8 +2,8 @@
 # USB peripheral controller drivers
 #
 
-ccflags-y			:= -I$(PWD)/drivers/usb/gadget/
-ccflags-y			+= -I$(PWD)/drivers/usb/gadget/udc/
+ccflags-y			:= -I$(srctree)/drivers/usb/gadget/
+ccflags-y			+= -I$(srctree)/drivers/usb/gadget/udc/
 
 # USB Functions
 usb_f_acm-y			:= f_acm.o
@@ -32,3 +32,9 @@ usb_f_mass_storage-y		:= f_mass_storage.o storage_common.o
 obj-$(CONFIG_USB_F_MASS_STORAGE)+= usb_f_mass_storage.o
 usb_f_fs-y			:= f_fs.o
 obj-$(CONFIG_USB_F_FS)		+= usb_f_fs.o
+usb_f_uac1-y			:= f_uac1.o u_uac1.o
+obj-$(CONFIG_USB_F_UAC1)	+= usb_f_uac1.o
+usb_f_uac2-y			:= f_uac2.o
+obj-$(CONFIG_USB_F_UAC2)	+= usb_f_uac2.o
+usb_f_uvc-y			:= f_uvc.o uvc_queue.o uvc_v4l2.o uvc_video.o
+obj-$(CONFIG_USB_F_UVC)		+= usb_f_uvc.o
diff --git a/drivers/usb/gadget/function/f_acm.c b/drivers/usb/gadget/function/f_acm.c
index ab1065afbbd0..6da4685490ef 100644
--- a/drivers/usb/gadget/function/f_acm.c
+++ b/drivers/usb/gadget/function/f_acm.c
@@ -313,15 +313,15 @@ static void acm_complete_set_line_coding(struct usb_ep *ep,
 	struct usb_composite_dev *cdev = acm->port.func.config->cdev;
 
 	if (req->status != 0) {
-		DBG(cdev, "acm ttyGS%d completion, err %d\n",
-				acm->port_num, req->status);
+		dev_dbg(&cdev->gadget->dev, "acm ttyGS%d completion, err %d\n",
+			acm->port_num, req->status);
 		return;
 	}
 
 	/* normal completion */
 	if (req->actual != sizeof(acm->port_line_coding)) {
-		DBG(cdev, "acm ttyGS%d short resp, len %d\n",
-				acm->port_num, req->actual);
+		dev_dbg(&cdev->gadget->dev, "acm ttyGS%d short resp, len %d\n",
+			acm->port_num, req->actual);
 		usb_ep_set_halt(ep);
 	} else {
 		struct usb_cdc_line_coding	*value = req->buf;
@@ -397,14 +397,16 @@ static int acm_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 
 	default:
 invalid:
-		VDBG(cdev, "invalid control req%02x.%02x v%04x i%04x l%d\n",
-			ctrl->bRequestType, ctrl->bRequest,
-			w_value, w_index, w_length);
+		dev_vdbg(&cdev->gadget->dev,
+			 "invalid control req%02x.%02x v%04x i%04x l%d\n",
+			 ctrl->bRequestType, ctrl->bRequest,
+			 w_value, w_index, w_length);
 	}
 
 	/* respond with data transfer or status phase? */
 	if (value >= 0) {
-		DBG(cdev, "acm ttyGS%d req%02x.%02x v%04x i%04x l%d\n",
+		dev_dbg(&cdev->gadget->dev,
+			"acm ttyGS%d req%02x.%02x v%04x i%04x l%d\n",
 			acm->port_num, ctrl->bRequestType, ctrl->bRequest,
 			w_value, w_index, w_length);
 		req->zero = 0;
@@ -428,10 +430,12 @@ static int acm_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 
 	if (intf == acm->ctrl_id) {
 		if (acm->notify->driver_data) {
-			VDBG(cdev, "reset acm control interface %d\n", intf);
+			dev_vdbg(&cdev->gadget->dev,
+				 "reset acm control interface %d\n", intf);
 			usb_ep_disable(acm->notify);
 		} else {
-			VDBG(cdev, "init acm ctrl interface %d\n", intf);
+			dev_vdbg(&cdev->gadget->dev,
+				 "init acm ctrl interface %d\n", intf);
 			if (config_ep_by_speed(cdev->gadget, f, acm->notify))
 				return -EINVAL;
 		}
@@ -440,11 +444,13 @@ static int acm_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 
 	} else if (intf == acm->data_id) {
 		if (acm->port.in->driver_data) {
-			DBG(cdev, "reset acm ttyGS%d\n", acm->port_num);
+			dev_dbg(&cdev->gadget->dev,
+				"reset acm ttyGS%d\n", acm->port_num);
 			gserial_disconnect(&acm->port);
 		}
 		if (!acm->port.in->desc || !acm->port.out->desc) {
-			DBG(cdev, "activate acm ttyGS%d\n", acm->port_num);
+			dev_dbg(&cdev->gadget->dev,
+				"activate acm ttyGS%d\n", acm->port_num);
 			if (config_ep_by_speed(cdev->gadget, f,
 					       acm->port.in) ||
 			    config_ep_by_speed(cdev->gadget, f,
@@ -467,7 +473,7 @@ static void acm_disable(struct usb_function *f)
 	struct f_acm	*acm = func_to_acm(f);
 	struct usb_composite_dev *cdev = f->config->cdev;
 
-	DBG(cdev, "acm ttyGS%d deactivated\n", acm->port_num);
+	dev_dbg(&cdev->gadget->dev, "acm ttyGS%d deactivated\n", acm->port_num);
 	gserial_disconnect(&acm->port);
 	usb_ep_disable(acm->notify);
 	acm->notify->driver_data = NULL;
@@ -537,8 +543,8 @@ static int acm_notify_serial_state(struct f_acm *acm)
 
 	spin_lock(&acm->lock);
 	if (acm->notify_req) {
-		DBG(cdev, "acm ttyGS%d serial state %04x\n",
-				acm->port_num, acm->serial_state);
+		dev_dbg(&cdev->gadget->dev, "acm ttyGS%d serial state %04x\n",
+			acm->port_num, acm->serial_state);
 		status = acm_cdc_notify(acm, USB_CDC_NOTIFY_SERIAL_STATE,
 				0, &acm->serial_state, sizeof(acm->serial_state));
 	} else {
@@ -691,12 +697,13 @@ acm_bind(struct usb_configuration *c, struct usb_function *f)
 	if (status)
 		goto fail;
 
-	DBG(cdev, "acm ttyGS%d: %s speed IN/%s OUT/%s NOTIFY/%s\n",
-			acm->port_num,
-			gadget_is_superspeed(c->cdev->gadget) ? "super" :
-			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
-			acm->port.in->name, acm->port.out->name,
-			acm->notify->name);
+	dev_dbg(&cdev->gadget->dev,
+		"acm ttyGS%d: %s speed IN/%s OUT/%s NOTIFY/%s\n",
+		acm->port_num,
+		gadget_is_superspeed(c->cdev->gadget) ? "super" :
+		gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
+		acm->port.in->name, acm->port.out->name,
+		acm->notify->name);
 	return 0;
 
 fail:
diff --git a/drivers/usb/gadget/function/f_fs.c b/drivers/usb/gadget/function/f_fs.c
index dc30adf15a01..7c6771d027a2 100644
--- a/drivers/usb/gadget/function/f_fs.c
+++ b/drivers/usb/gadget/function/f_fs.c
@@ -155,13 +155,18 @@ struct ffs_io_data {
 	struct usb_request *req;
 };
 
+struct ffs_desc_helper {
+	struct ffs_data *ffs;
+	unsigned interfaces_count;
+	unsigned eps_count;
+};
+
 static int  __must_check ffs_epfiles_create(struct ffs_data *ffs);
 static void ffs_epfiles_destroy(struct ffs_epfile *epfiles, unsigned count);
 
-static struct inode *__must_check
+static struct dentry *
 ffs_sb_create_file(struct super_block *sb, const char *name, void *data,
-		   const struct file_operations *fops,
-		   struct dentry **dentry_p);
+		   const struct file_operations *fops);
 
 /* Devices management *******************************************************/
 
@@ -1026,6 +1031,29 @@ static long ffs_epfile_ioctl(struct file *file, unsigned code,
 		case FUNCTIONFS_ENDPOINT_REVMAP:
 			ret = epfile->ep->num;
 			break;
+		case FUNCTIONFS_ENDPOINT_DESC:
+		{
+			int desc_idx;
+			struct usb_endpoint_descriptor *desc;
+
+			switch (epfile->ffs->gadget->speed) {
+			case USB_SPEED_SUPER:
+				desc_idx = 2;
+				break;
+			case USB_SPEED_HIGH:
+				desc_idx = 1;
+				break;
+			default:
+				desc_idx = 0;
+			}
+			desc = epfile->ep->descs[desc_idx];
+
+			spin_unlock_irq(&epfile->ffs->eps_lock);
+			ret = copy_to_user((void *)value, desc, sizeof(*desc));
+			if (ret)
+				ret = -EFAULT;
+			return ret;
+		}
 		default:
 			ret = -ENOTTY;
 		}
@@ -1090,10 +1118,9 @@ ffs_sb_make_inode(struct super_block *sb, void *data,
 }
 
 /* Create "regular" file */
-static struct inode *ffs_sb_create_file(struct super_block *sb,
+static struct dentry *ffs_sb_create_file(struct super_block *sb,
 					const char *name, void *data,
-					const struct file_operations *fops,
-					struct dentry **dentry_p)
+					const struct file_operations *fops)
 {
 	struct ffs_data	*ffs = sb->s_fs_info;
 	struct dentry	*dentry;
@@ -1112,10 +1139,7 @@ static struct inode *ffs_sb_create_file(struct super_block *sb,
 	}
 
 	d_add(dentry, inode);
-	if (dentry_p)
-		*dentry_p = dentry;
-
-	return inode;
+	return dentry;
 }
 
 /* Super block */
@@ -1160,7 +1184,7 @@ static int ffs_sb_fill(struct super_block *sb, void *_data, int silent)
 
 	/* EP0 file */
 	if (unlikely(!ffs_sb_create_file(sb, "ep0", ffs,
-					 &ffs_ep0_operations, NULL)))
+					 &ffs_ep0_operations)))
 		return -ENOMEM;
 
 	return 0;
@@ -1528,10 +1552,14 @@ static int ffs_epfiles_create(struct ffs_data *ffs)
 		epfile->ffs = ffs;
 		mutex_init(&epfile->mutex);
 		init_waitqueue_head(&epfile->wait);
-		sprintf(epfiles->name, "ep%u",  i);
-		if (!unlikely(ffs_sb_create_file(ffs->sb, epfiles->name, epfile,
-						 &ffs_epfile_operations,
-						 &epfile->dentry))) {
+		if (ffs->user_flags & FUNCTIONFS_VIRTUAL_ADDR)
+			sprintf(epfiles->name, "ep%02x", ffs->eps_addrmap[i]);
+		else
+			sprintf(epfiles->name, "ep%u", i);
+		epfile->dentry = ffs_sb_create_file(ffs->sb, epfiles->name,
+						 epfile,
+						 &ffs_epfile_operations);
+		if (unlikely(!epfile->dentry)) {
 			ffs_epfiles_destroy(epfiles, i - 1);
 			return -ENOMEM;
 		}
@@ -1830,7 +1858,8 @@ static int __ffs_data_do_entity(enum ffs_entity_type type,
 				u8 *valuep, struct usb_descriptor_header *desc,
 				void *priv)
 {
-	struct ffs_data *ffs = priv;
+	struct ffs_desc_helper *helper = priv;
+	struct usb_endpoint_descriptor *d;
 
 	ENTER();
 
@@ -1844,8 +1873,8 @@ static int __ffs_data_do_entity(enum ffs_entity_type type,
 		 * encountered interface "n" then there are at least
 		 * "n+1" interfaces.
 		 */
-		if (*valuep >= ffs->interfaces_count)
-			ffs->interfaces_count = *valuep + 1;
+		if (*valuep >= helper->interfaces_count)
+			helper->interfaces_count = *valuep + 1;
 		break;
 
 	case FFS_STRING:
@@ -1853,14 +1882,22 @@ static int __ffs_data_do_entity(enum ffs_entity_type type,
 		 * Strings are indexed from 1 (0 is magic ;) reserved
 		 * for languages list or some such)
 		 */
-		if (*valuep > ffs->strings_count)
-			ffs->strings_count = *valuep;
+		if (*valuep > helper->ffs->strings_count)
+			helper->ffs->strings_count = *valuep;
 		break;
 
 	case FFS_ENDPOINT:
-		/* Endpoints are indexed from 1 as well. */
-		if ((*valuep & USB_ENDPOINT_NUMBER_MASK) > ffs->eps_count)
-			ffs->eps_count = (*valuep & USB_ENDPOINT_NUMBER_MASK);
+		d = (void *)desc;
+		helper->eps_count++;
+		if (helper->eps_count >= 15)
+			return -EINVAL;
+		/* Check if descriptors for any speed were already parsed */
+		if (!helper->ffs->eps_count && !helper->ffs->interfaces_count)
+			helper->ffs->eps_addrmap[helper->eps_count] =
+				d->bEndpointAddress;
+		else if (helper->ffs->eps_addrmap[helper->eps_count] !=
+				d->bEndpointAddress)
+			return -EINVAL;
 		break;
 	}
 
@@ -2053,6 +2090,7 @@ static int __ffs_data_got_descs(struct ffs_data *ffs,
 	char *data = _data, *raw_descs;
 	unsigned os_descs_count = 0, counts[3], flags;
 	int ret = -EINVAL, i;
+	struct ffs_desc_helper helper;
 
 	ENTER();
 
@@ -2067,10 +2105,12 @@ static int __ffs_data_got_descs(struct ffs_data *ffs,
 		break;
 	case FUNCTIONFS_DESCRIPTORS_MAGIC_V2:
 		flags = get_unaligned_le32(data + 8);
+		ffs->user_flags = flags;
 		if (flags & ~(FUNCTIONFS_HAS_FS_DESC |
 			      FUNCTIONFS_HAS_HS_DESC |
 			      FUNCTIONFS_HAS_SS_DESC |
-			      FUNCTIONFS_HAS_MS_OS_DESC)) {
+			      FUNCTIONFS_HAS_MS_OS_DESC |
+			      FUNCTIONFS_VIRTUAL_ADDR)) {
 			ret = -ENOSYS;
 			goto error;
 		}
@@ -2101,13 +2141,29 @@ static int __ffs_data_got_descs(struct ffs_data *ffs,
 
 	/* Read descriptors */
 	raw_descs = data;
+	helper.ffs = ffs;
 	for (i = 0; i < 3; ++i) {
 		if (!counts[i])
 			continue;
+		helper.interfaces_count = 0;
+		helper.eps_count = 0;
 		ret = ffs_do_descs(counts[i], data, len,
-				   __ffs_data_do_entity, ffs);
+				   __ffs_data_do_entity, &helper);
 		if (ret < 0)
 			goto error;
+		if (!ffs->eps_count && !ffs->interfaces_count) {
+			ffs->eps_count = helper.eps_count;
+			ffs->interfaces_count = helper.interfaces_count;
+		} else {
+			if (ffs->eps_count != helper.eps_count) {
+				ret = -EINVAL;
+				goto error;
+			}
+			if (ffs->interfaces_count != helper.interfaces_count) {
+				ret = -EINVAL;
+				goto error;
+			}
+		}
 		data += ret;
 		len  -= ret;
 	}
@@ -2314,7 +2370,8 @@ static void __ffs_event_add(struct ffs_data *ffs,
 		break;
 
 	default:
-		BUG();
+		WARN(1, "%d: unknown event, this should not happen\n", type);
+		return;
 	}
 
 	{
@@ -2342,9 +2399,18 @@ static void ffs_event_add(struct ffs_data *ffs,
 	spin_unlock_irqrestore(&ffs->ev.waitq.lock, flags);
 }
 
-
 /* Bind/unbind USB function hooks *******************************************/
 
+static int ffs_ep_addr2idx(struct ffs_data *ffs, u8 endpoint_address)
+{
+	int i;
+
+	for (i = 1; i < ARRAY_SIZE(ffs->eps_addrmap); ++i)
+		if (ffs->eps_addrmap[i] == endpoint_address)
+			return i;
+	return -ENOENT;
+}
+
 static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 				    struct usb_descriptor_header *desc,
 				    void *priv)
@@ -2352,7 +2418,8 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 	struct usb_endpoint_descriptor *ds = (void *)desc;
 	struct ffs_function *func = priv;
 	struct ffs_ep *ffs_ep;
-	unsigned ep_desc_id, idx;
+	unsigned ep_desc_id;
+	int idx;
 	static const char *speed_names[] = { "full", "high", "super" };
 
 	if (type != FFS_DESCRIPTOR)
@@ -2378,7 +2445,10 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 	if (!desc || desc->bDescriptorType != USB_DT_ENDPOINT)
 		return 0;
 
-	idx = (ds->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK) - 1;
+	idx = ffs_ep_addr2idx(func->ffs, ds->bEndpointAddress) - 1;
+	if (idx < 0)
+		return idx;
+
 	ffs_ep = func->eps + idx;
 
 	if (unlikely(ffs_ep->descs[ep_desc_id])) {
@@ -2397,7 +2467,13 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 	} else {
 		struct usb_request *req;
 		struct usb_ep *ep;
+		u8 bEndpointAddress;
 
+		/*
+		 * We back up bEndpointAddress because autoconfig overwrites
+		 * it with physical endpoint address.
+		 */
+		bEndpointAddress = ds->bEndpointAddress;
 		pr_vdebug("autoconfig\n");
 		ep = usb_ep_autoconfig(func->gadget, ds);
 		if (unlikely(!ep))
@@ -2412,6 +2488,12 @@ static int __ffs_func_bind_do_descs(enum ffs_entity_type type, u8 *valuep,
 		ffs_ep->req = req;
 		func->eps_revmap[ds->bEndpointAddress &
 				 USB_ENDPOINT_NUMBER_MASK] = idx + 1;
+		/*
+		 * If we use virtual address mapping, we restore
+		 * original bEndpointAddress value.
+		 */
+		if (func->ffs->user_flags & FUNCTIONFS_VIRTUAL_ADDR)
+			ds->bEndpointAddress = bEndpointAddress;
 	}
 	ffs_dump_mem(": Rewritten ep desc", ds, ds->bLength);
 
@@ -2856,6 +2938,8 @@ static int ffs_func_setup(struct usb_function *f,
 		ret = ffs_func_revmap_ep(func, le16_to_cpu(creq->wIndex));
 		if (unlikely(ret < 0))
 			return ret;
+		if (func->ffs->user_flags & FUNCTIONFS_VIRTUAL_ADDR)
+			ret = func->ffs->eps_addrmap[ret];
 		break;
 
 	default:
diff --git a/drivers/usb/gadget/function/f_loopback.c b/drivers/usb/gadget/function/f_loopback.c
index 4557cd03f0b1..bf04389137e6 100644
--- a/drivers/usb/gadget/function/f_loopback.c
+++ b/drivers/usb/gadget/function/f_loopback.c
@@ -298,7 +298,8 @@ static void disable_loopback(struct f_loopback *loop)
 	struct usb_composite_dev	*cdev;
 
 	cdev = loop->function.config->cdev;
-	disable_endpoints(cdev, loop->in_ep, loop->out_ep, NULL, NULL);
+	disable_endpoints(cdev, loop->in_ep, loop->out_ep, NULL, NULL, NULL,
+			NULL);
 	VDBG(cdev, "%s disabled\n", loop->function.name);
 }
 
diff --git a/drivers/usb/gadget/function/f_mass_storage.c b/drivers/usb/gadget/function/f_mass_storage.c
index b96393908860..811929cd4c9e 100644
--- a/drivers/usb/gadget/function/f_mass_storage.c
+++ b/drivers/usb/gadget/function/f_mass_storage.c
@@ -566,22 +566,22 @@ static void start_transfer(struct fsg_dev *fsg, struct usb_ep *ep,
 	*pbusy = 1;
 	*state = BUF_STATE_BUSY;
 	spin_unlock_irq(&fsg->common->lock);
+
 	rc = usb_ep_queue(ep, req, GFP_KERNEL);
-	if (rc != 0) {
-		*pbusy = 0;
-		*state = BUF_STATE_EMPTY;
+	if (rc == 0)
+		return;  /* All good, we're done */
 
-		/* We can't do much more than wait for a reset */
+	*pbusy = 0;
+	*state = BUF_STATE_EMPTY;
 
-		/*
-		 * Note: currently the net2280 driver fails zero-length
-		 * submissions if DMA is enabled.
-		 */
-		if (rc != -ESHUTDOWN &&
-		    !(rc == -EOPNOTSUPP && req->length == 0))
-			WARNING(fsg, "error in submission: %s --> %d\n",
-				ep->name, rc);
-	}
+	/* We can't do much more than wait for a reset */
+
+	/*
+	 * Note: currently the net2280 driver fails zero-length
+	 * submissions if DMA is enabled.
+	 */
+	if (rc != -ESHUTDOWN && !(rc == -EOPNOTSUPP && req->length == 0))
+		WARNING(fsg, "error in submission: %s --> %d\n", ep->name, rc);
 }
 
 static bool start_in_transfer(struct fsg_common *common, struct fsg_buffhd *bh)
@@ -3665,4 +3665,3 @@ void fsg_config_from_params(struct fsg_config *cfg,
 	cfg->fsg_num_buffers = fsg_num_buffers;
 }
 EXPORT_SYMBOL_GPL(fsg_config_from_params);
-
diff --git a/drivers/usb/gadget/function/f_ncm.c b/drivers/usb/gadget/function/f_ncm.c
index bcdc882cd415..146f48cc65d7 100644
--- a/drivers/usb/gadget/function/f_ncm.c
+++ b/drivers/usb/gadget/function/f_ncm.c
@@ -1101,7 +1101,15 @@ static void ncm_tx_tasklet(unsigned long data)
 	/* Only send if data is available. */
 	if (ncm->skb_tx_data) {
 		ncm->timer_force_tx = true;
+
+		/* XXX This allowance of a NULL skb argument to ndo_start_xmit
+		 * XXX is not sane.  The gadget layer should be redesigned so
+		 * XXX that the dev->wrap() invocations to build SKBs is transparent
+		 * XXX and performed in some way outside of the ndo_start_xmit
+		 * XXX interface.
+		 */
 		ncm->netdev->netdev_ops->ndo_start_xmit(NULL, ncm->netdev);
+
 		ncm->timer_force_tx = false;
 	}
 }
diff --git a/drivers/usb/gadget/function/f_obex.c b/drivers/usb/gadget/function/f_obex.c
index aebae1853bce..5f40080c92cc 100644
--- a/drivers/usb/gadget/function/f_obex.c
+++ b/drivers/usb/gadget/function/f_obex.c
@@ -200,19 +200,22 @@ static int obex_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 		if (alt != 0)
 			goto fail;
 		/* NOP */
-		DBG(cdev, "reset obex ttyGS%d control\n", obex->port_num);
+		dev_dbg(&cdev->gadget->dev,
+			"reset obex ttyGS%d control\n", obex->port_num);
 
 	} else if (intf == obex->data_id) {
 		if (alt > 1)
 			goto fail;
 
 		if (obex->port.in->driver_data) {
-			DBG(cdev, "reset obex ttyGS%d\n", obex->port_num);
+			dev_dbg(&cdev->gadget->dev,
+				"reset obex ttyGS%d\n", obex->port_num);
 			gserial_disconnect(&obex->port);
 		}
 
 		if (!obex->port.in->desc || !obex->port.out->desc) {
-			DBG(cdev, "init obex ttyGS%d\n", obex->port_num);
+			dev_dbg(&cdev->gadget->dev,
+				"init obex ttyGS%d\n", obex->port_num);
 			if (config_ep_by_speed(cdev->gadget, f,
 					       obex->port.in) ||
 			    config_ep_by_speed(cdev->gadget, f,
@@ -224,7 +227,8 @@ static int obex_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 		}
 
 		if (alt == 1) {
-			DBG(cdev, "activate obex ttyGS%d\n", obex->port_num);
+			dev_dbg(&cdev->gadget->dev,
+				"activate obex ttyGS%d\n", obex->port_num);
 			gserial_connect(&obex->port, obex->port_num);
 		}
 
@@ -252,7 +256,7 @@ static void obex_disable(struct usb_function *f)
 	struct f_obex	*obex = func_to_obex(f);
 	struct usb_composite_dev *cdev = f->config->cdev;
 
-	DBG(cdev, "obex ttyGS%d disable\n", obex->port_num);
+	dev_dbg(&cdev->gadget->dev, "obex ttyGS%d disable\n", obex->port_num);
 	gserial_disconnect(&obex->port);
 }
 
@@ -269,7 +273,8 @@ static void obex_connect(struct gserial *g)
 
 	status = usb_function_activate(&g->func);
 	if (status)
-		DBG(cdev, "obex ttyGS%d function activate --> %d\n",
+		dev_dbg(&cdev->gadget->dev,
+			"obex ttyGS%d function activate --> %d\n",
 			obex->port_num, status);
 }
 
@@ -284,7 +289,8 @@ static void obex_disconnect(struct gserial *g)
 
 	status = usb_function_deactivate(&g->func);
 	if (status)
-		DBG(cdev, "obex ttyGS%d function deactivate --> %d\n",
+		dev_dbg(&cdev->gadget->dev,
+			"obex ttyGS%d function deactivate --> %d\n",
 			obex->port_num, status);
 }
 
@@ -383,10 +389,10 @@ static int obex_bind(struct usb_configuration *c, struct usb_function *f)
 		obex->can_activate = true;
 
 
-	DBG(cdev, "obex ttyGS%d: %s speed IN/%s OUT/%s\n",
-			obex->port_num,
-			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
-			obex->port.in->name, obex->port.out->name);
+	dev_dbg(&cdev->gadget->dev, "obex ttyGS%d: %s speed IN/%s OUT/%s\n",
+		obex->port_num,
+		gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
+		obex->port.in->name, obex->port.out->name);
 
 	return 0;
 
diff --git a/drivers/usb/gadget/function/f_serial.c b/drivers/usb/gadget/function/f_serial.c
index 9ecbcbf36a45..2e02dfabc7ae 100644
--- a/drivers/usb/gadget/function/f_serial.c
+++ b/drivers/usb/gadget/function/f_serial.c
@@ -155,11 +155,13 @@ static int gser_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 	/* we know alt == 0, so this is an activation or a reset */
 
 	if (gser->port.in->driver_data) {
-		DBG(cdev, "reset generic ttyGS%d\n", gser->port_num);
+		dev_dbg(&cdev->gadget->dev,
+			"reset generic ttyGS%d\n", gser->port_num);
 		gserial_disconnect(&gser->port);
 	}
 	if (!gser->port.in->desc || !gser->port.out->desc) {
-		DBG(cdev, "activate generic ttyGS%d\n", gser->port_num);
+		dev_dbg(&cdev->gadget->dev,
+			"activate generic ttyGS%d\n", gser->port_num);
 		if (config_ep_by_speed(cdev->gadget, f, gser->port.in) ||
 		    config_ep_by_speed(cdev->gadget, f, gser->port.out)) {
 			gser->port.in->desc = NULL;
@@ -176,7 +178,8 @@ static void gser_disable(struct usb_function *f)
 	struct f_gser	*gser = func_to_gser(f);
 	struct usb_composite_dev *cdev = f->config->cdev;
 
-	DBG(cdev, "generic ttyGS%d deactivated\n", gser->port_num);
+	dev_dbg(&cdev->gadget->dev,
+		"generic ttyGS%d deactivated\n", gser->port_num);
 	gserial_disconnect(&gser->port);
 }
 
@@ -239,11 +242,11 @@ static int gser_bind(struct usb_configuration *c, struct usb_function *f)
 			gser_ss_function);
 	if (status)
 		goto fail;
-	DBG(cdev, "generic ttyGS%d: %s speed IN/%s OUT/%s\n",
-			gser->port_num,
-			gadget_is_superspeed(c->cdev->gadget) ? "super" :
-			gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
-			gser->port.in->name, gser->port.out->name);
+	dev_dbg(&cdev->gadget->dev, "generic ttyGS%d: %s speed IN/%s OUT/%s\n",
+		gser->port_num,
+		gadget_is_superspeed(c->cdev->gadget) ? "super" :
+		gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full",
+		gser->port.in->name, gser->port.out->name);
 	return 0;
 
 fail:
diff --git a/drivers/usb/gadget/function/f_sourcesink.c b/drivers/usb/gadget/function/f_sourcesink.c
index d3cd52db78fe..80be25b32cd7 100644
--- a/drivers/usb/gadget/function/f_sourcesink.c
+++ b/drivers/usb/gadget/function/f_sourcesink.c
@@ -23,6 +23,15 @@
 #include "gadget_chips.h"
 #include "u_f.h"
 
+#define USB_MS_TO_SS_INTERVAL(x) USB_MS_TO_HS_INTERVAL(x)
+
+enum eptype {
+	EP_CONTROL = 0,
+	EP_BULK,
+	EP_ISOC,
+	EP_INTERRUPT,
+};
+
 /*
  * SOURCE/SINK FUNCTION ... a primary testing vehicle for USB peripheral
  * controller drivers.
@@ -55,6 +64,8 @@ struct f_sourcesink {
 	struct usb_ep		*out_ep;
 	struct usb_ep		*iso_in_ep;
 	struct usb_ep		*iso_out_ep;
+	struct usb_ep		*int_in_ep;
+	struct usb_ep		*int_out_ep;
 	int			cur_alt;
 };
 
@@ -68,6 +79,10 @@ static unsigned isoc_interval;
 static unsigned isoc_maxpacket;
 static unsigned isoc_mult;
 static unsigned isoc_maxburst;
+static unsigned int_interval; /* In ms */
+static unsigned int_maxpacket;
+static unsigned int_mult;
+static unsigned int_maxburst;
 static unsigned buflen;
 
 /*-------------------------------------------------------------------------*/
@@ -92,6 +107,16 @@ static struct usb_interface_descriptor source_sink_intf_alt1 = {
 	/* .iInterface		= DYNAMIC */
 };
 
+static struct usb_interface_descriptor source_sink_intf_alt2 = {
+	.bLength =		USB_DT_INTERFACE_SIZE,
+	.bDescriptorType =	USB_DT_INTERFACE,
+
+	.bAlternateSetting =	2,
+	.bNumEndpoints =	2,
+	.bInterfaceClass =	USB_CLASS_VENDOR_SPEC,
+	/* .iInterface		= DYNAMIC */
+};
+
 /* full speed support: */
 
 static struct usb_endpoint_descriptor fs_source_desc = {
@@ -130,6 +155,26 @@ static struct usb_endpoint_descriptor fs_iso_sink_desc = {
 	.bInterval =		4,
 };
 
+static struct usb_endpoint_descriptor fs_int_source_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_IN,
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	cpu_to_le16(64),
+	.bInterval =		GZERO_INT_INTERVAL,
+};
+
+static struct usb_endpoint_descriptor fs_int_sink_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bEndpointAddress =	USB_DIR_OUT,
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	cpu_to_le16(64),
+	.bInterval =		GZERO_INT_INTERVAL,
+};
+
 static struct usb_descriptor_header *fs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &fs_sink_desc,
@@ -140,6 +185,10 @@ static struct usb_descriptor_header *fs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &fs_source_desc,
 	(struct usb_descriptor_header *) &fs_iso_sink_desc,
 	(struct usb_descriptor_header *) &fs_iso_source_desc,
+	(struct usb_descriptor_header *) &source_sink_intf_alt2,
+#define FS_ALT_IFC_2_OFFSET	8
+	(struct usb_descriptor_header *) &fs_int_sink_desc,
+	(struct usb_descriptor_header *) &fs_int_source_desc,
 	NULL,
 };
 
@@ -179,6 +228,24 @@ static struct usb_endpoint_descriptor hs_iso_sink_desc = {
 	.bInterval =		4,
 };
 
+static struct usb_endpoint_descriptor hs_int_source_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		USB_MS_TO_HS_INTERVAL(GZERO_INT_INTERVAL),
+};
+
+static struct usb_endpoint_descriptor hs_int_sink_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		USB_MS_TO_HS_INTERVAL(GZERO_INT_INTERVAL),
+};
+
 static struct usb_descriptor_header *hs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &hs_source_desc,
@@ -189,6 +256,10 @@ static struct usb_descriptor_header *hs_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &hs_sink_desc,
 	(struct usb_descriptor_header *) &hs_iso_source_desc,
 	(struct usb_descriptor_header *) &hs_iso_sink_desc,
+	(struct usb_descriptor_header *) &source_sink_intf_alt2,
+#define HS_ALT_IFC_2_OFFSET	8
+	(struct usb_descriptor_header *) &hs_int_source_desc,
+	(struct usb_descriptor_header *) &hs_int_sink_desc,
 	NULL,
 };
 
@@ -264,6 +335,42 @@ static struct usb_ss_ep_comp_descriptor ss_iso_sink_comp_desc = {
 	.wBytesPerInterval =	cpu_to_le16(1024),
 };
 
+static struct usb_endpoint_descriptor ss_int_source_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		USB_MS_TO_SS_INTERVAL(GZERO_INT_INTERVAL),
+};
+
+struct usb_ss_ep_comp_descriptor ss_int_source_comp_desc = {
+	.bLength =		USB_DT_SS_EP_COMP_SIZE,
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
+	.bMaxBurst =		0,
+	.bmAttributes =		0,
+	.wBytesPerInterval =	cpu_to_le16(1024),
+};
+
+static struct usb_endpoint_descriptor ss_int_sink_desc = {
+	.bLength =		USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType =	USB_DT_ENDPOINT,
+
+	.bmAttributes =		USB_ENDPOINT_XFER_INT,
+	.wMaxPacketSize =	cpu_to_le16(1024),
+	.bInterval =		USB_MS_TO_SS_INTERVAL(GZERO_INT_INTERVAL),
+};
+
+struct usb_ss_ep_comp_descriptor ss_int_sink_comp_desc = {
+	.bLength =		USB_DT_SS_EP_COMP_SIZE,
+	.bDescriptorType =	USB_DT_SS_ENDPOINT_COMP,
+
+	.bMaxBurst =		0,
+	.bmAttributes =		0,
+	.wBytesPerInterval =	cpu_to_le16(1024),
+};
+
 static struct usb_descriptor_header *ss_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &source_sink_intf_alt0,
 	(struct usb_descriptor_header *) &ss_source_desc,
@@ -280,6 +387,12 @@ static struct usb_descriptor_header *ss_source_sink_descs[] = {
 	(struct usb_descriptor_header *) &ss_iso_source_comp_desc,
 	(struct usb_descriptor_header *) &ss_iso_sink_desc,
 	(struct usb_descriptor_header *) &ss_iso_sink_comp_desc,
+	(struct usb_descriptor_header *) &source_sink_intf_alt2,
+#define SS_ALT_IFC_2_OFFSET	14
+	(struct usb_descriptor_header *) &ss_int_source_desc,
+	(struct usb_descriptor_header *) &ss_int_source_comp_desc,
+	(struct usb_descriptor_header *) &ss_int_sink_desc,
+	(struct usb_descriptor_header *) &ss_int_sink_comp_desc,
 	NULL,
 };
 
@@ -301,6 +414,21 @@ static struct usb_gadget_strings *sourcesink_strings[] = {
 };
 
 /*-------------------------------------------------------------------------*/
+static const char *get_ep_string(enum eptype ep_type)
+{
+	switch (ep_type) {
+	case EP_ISOC:
+		return "ISOC-";
+	case EP_INTERRUPT:
+		return "INTERRUPT-";
+	case EP_CONTROL:
+		return "CTRL-";
+	case EP_BULK:
+		return "BULK-";
+	default:
+		return "UNKNOWN-";
+	}
+}
 
 static inline struct usb_request *ss_alloc_ep_req(struct usb_ep *ep, int len)
 {
@@ -328,7 +456,8 @@ static void disable_ep(struct usb_composite_dev *cdev, struct usb_ep *ep)
 
 void disable_endpoints(struct usb_composite_dev *cdev,
 		struct usb_ep *in, struct usb_ep *out,
-		struct usb_ep *iso_in, struct usb_ep *iso_out)
+		struct usb_ep *iso_in, struct usb_ep *iso_out,
+		struct usb_ep *int_in, struct usb_ep *int_out)
 {
 	disable_ep(cdev, in);
 	disable_ep(cdev, out);
@@ -336,6 +465,10 @@ void disable_endpoints(struct usb_composite_dev *cdev,
 		disable_ep(cdev, iso_in);
 	if (iso_out)
 		disable_ep(cdev, iso_out);
+	if (int_in)
+		disable_ep(cdev, int_in);
+	if (int_out)
+		disable_ep(cdev, int_out);
 }
 
 static int
@@ -352,6 +485,7 @@ sourcesink_bind(struct usb_configuration *c, struct usb_function *f)
 		return id;
 	source_sink_intf_alt0.bInterfaceNumber = id;
 	source_sink_intf_alt1.bInterfaceNumber = id;
+	source_sink_intf_alt2.bInterfaceNumber = id;
 
 	/* allocate bulk endpoints */
 	ss->in_ep = usb_ep_autoconfig(cdev->gadget, &fs_source_desc);
@@ -412,14 +546,55 @@ no_iso:
 	if (isoc_maxpacket > 1024)
 		isoc_maxpacket = 1024;
 
+	/* sanity check the interrupt module parameters */
+	if (int_interval < 1)
+		int_interval = 1;
+	if (int_interval > 4096)
+		int_interval = 4096;
+	if (int_mult > 2)
+		int_mult = 2;
+	if (int_maxburst > 15)
+		int_maxburst = 15;
+
+	/* fill in the FS interrupt descriptors from the module parameters */
+	fs_int_source_desc.wMaxPacketSize = int_maxpacket > 64 ?
+						64 : int_maxpacket;
+	fs_int_source_desc.bInterval = int_interval > 255 ?
+						255 : int_interval;
+	fs_int_sink_desc.wMaxPacketSize = int_maxpacket > 64 ?
+						64 : int_maxpacket;
+	fs_int_sink_desc.bInterval = int_interval > 255 ?
+						255 : int_interval;
+
+	/* allocate int endpoints */
+	ss->int_in_ep = usb_ep_autoconfig(cdev->gadget, &fs_int_source_desc);
+	if (!ss->int_in_ep)
+		goto no_int;
+	ss->int_in_ep->driver_data = cdev;	/* claim */
+
+	ss->int_out_ep = usb_ep_autoconfig(cdev->gadget, &fs_int_sink_desc);
+	if (ss->int_out_ep) {
+		ss->int_out_ep->driver_data = cdev;	/* claim */
+	} else {
+		ss->int_in_ep->driver_data = NULL;
+		ss->int_in_ep = NULL;
+no_int:
+		fs_source_sink_descs[FS_ALT_IFC_2_OFFSET] = NULL;
+		hs_source_sink_descs[HS_ALT_IFC_2_OFFSET] = NULL;
+		ss_source_sink_descs[SS_ALT_IFC_2_OFFSET] = NULL;
+	}
+
+	if (int_maxpacket > 1024)
+		int_maxpacket = 1024;
+
 	/* support high speed hardware */
 	hs_source_desc.bEndpointAddress = fs_source_desc.bEndpointAddress;
 	hs_sink_desc.bEndpointAddress = fs_sink_desc.bEndpointAddress;
 
 	/*
-	 * Fill in the HS isoc descriptors from the module parameters.
-	 * We assume that the user knows what they are doing and won't
-	 * give parameters that their UDC doesn't support.
+	 * Fill in the HS isoc and interrupt descriptors from the module
+	 * parameters. We assume that the user knows what they are doing and
+	 * won't give parameters that their UDC doesn't support.
 	 */
 	hs_iso_source_desc.wMaxPacketSize = isoc_maxpacket;
 	hs_iso_source_desc.wMaxPacketSize |= isoc_mult << 11;
@@ -432,6 +607,17 @@ no_iso:
 	hs_iso_sink_desc.bInterval = isoc_interval;
 	hs_iso_sink_desc.bEndpointAddress = fs_iso_sink_desc.bEndpointAddress;
 
+	hs_int_source_desc.wMaxPacketSize = int_maxpacket;
+	hs_int_source_desc.wMaxPacketSize |= int_mult << 11;
+	hs_int_source_desc.bInterval = USB_MS_TO_HS_INTERVAL(int_interval);
+	hs_int_source_desc.bEndpointAddress =
+		fs_int_source_desc.bEndpointAddress;
+
+	hs_int_sink_desc.wMaxPacketSize = int_maxpacket;
+	hs_int_sink_desc.wMaxPacketSize |= int_mult << 11;
+	hs_int_sink_desc.bInterval = USB_MS_TO_HS_INTERVAL(int_interval);
+	hs_int_sink_desc.bEndpointAddress = fs_int_sink_desc.bEndpointAddress;
+
 	/* support super speed hardware */
 	ss_source_desc.bEndpointAddress =
 		fs_source_desc.bEndpointAddress;
@@ -439,9 +625,9 @@ no_iso:
 		fs_sink_desc.bEndpointAddress;
 
 	/*
-	 * Fill in the SS isoc descriptors from the module parameters.
-	 * We assume that the user knows what they are doing and won't
-	 * give parameters that their UDC doesn't support.
+	 * Fill in the SS isoc and interrupt descriptors from the module
+	 * parameters. We assume that the user knows what they are doing and
+	 * won't give parameters that their UDC doesn't support.
 	 */
 	ss_iso_source_desc.wMaxPacketSize = isoc_maxpacket;
 	ss_iso_source_desc.bInterval = isoc_interval;
@@ -460,17 +646,37 @@ no_iso:
 		isoc_maxpacket * (isoc_mult + 1) * (isoc_maxburst + 1);
 	ss_iso_sink_desc.bEndpointAddress = fs_iso_sink_desc.bEndpointAddress;
 
+	ss_int_source_desc.wMaxPacketSize = int_maxpacket;
+	ss_int_source_desc.bInterval = USB_MS_TO_SS_INTERVAL(int_interval);
+	ss_int_source_comp_desc.bmAttributes = int_mult;
+	ss_int_source_comp_desc.bMaxBurst = int_maxburst;
+	ss_int_source_comp_desc.wBytesPerInterval =
+		int_maxpacket * (int_mult + 1) * (int_maxburst + 1);
+	ss_int_source_desc.bEndpointAddress =
+		fs_int_source_desc.bEndpointAddress;
+
+	ss_int_sink_desc.wMaxPacketSize = int_maxpacket;
+	ss_int_sink_desc.bInterval = USB_MS_TO_SS_INTERVAL(int_interval);
+	ss_int_sink_comp_desc.bmAttributes = int_mult;
+	ss_int_sink_comp_desc.bMaxBurst = int_maxburst;
+	ss_int_sink_comp_desc.wBytesPerInterval =
+		int_maxpacket * (int_mult + 1) * (int_maxburst + 1);
+	ss_int_sink_desc.bEndpointAddress = fs_int_sink_desc.bEndpointAddress;
+
 	ret = usb_assign_descriptors(f, fs_source_sink_descs,
 			hs_source_sink_descs, ss_source_sink_descs);
 	if (ret)
 		return ret;
 
-	DBG(cdev, "%s speed %s: IN/%s, OUT/%s, ISO-IN/%s, ISO-OUT/%s\n",
+	DBG(cdev, "%s speed %s: IN/%s, OUT/%s, ISO-IN/%s, ISO-OUT/%s, "
+			"INT-IN/%s, INT-OUT/%s\n",
 	    (gadget_is_superspeed(c->cdev->gadget) ? "super" :
 	     (gadget_is_dualspeed(c->cdev->gadget) ? "dual" : "full")),
 			f->name, ss->in_ep->name, ss->out_ep->name,
 			ss->iso_in_ep ? ss->iso_in_ep->name : "<none>",
-			ss->iso_out_ep ? ss->iso_out_ep->name : "<none>");
+			ss->iso_out_ep ? ss->iso_out_ep->name : "<none>",
+			ss->int_in_ep ? ss->int_in_ep->name : "<none>",
+			ss->int_out_ep ? ss->int_out_ep->name : "<none>");
 	return 0;
 }
 
@@ -601,14 +807,15 @@ static void source_sink_complete(struct usb_ep *ep, struct usb_request *req)
 }
 
 static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
-		bool is_iso, int speed)
+		enum eptype ep_type, int speed)
 {
 	struct usb_ep		*ep;
 	struct usb_request	*req;
 	int			i, size, status;
 
 	for (i = 0; i < 8; i++) {
-		if (is_iso) {
+		switch (ep_type) {
+		case EP_ISOC:
 			switch (speed) {
 			case USB_SPEED_SUPER:
 				size = isoc_maxpacket * (isoc_mult + 1) *
@@ -624,9 +831,28 @@ static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
 			}
 			ep = is_in ? ss->iso_in_ep : ss->iso_out_ep;
 			req = ss_alloc_ep_req(ep, size);
-		} else {
+			break;
+		case EP_INTERRUPT:
+			switch (speed) {
+			case USB_SPEED_SUPER:
+				size = int_maxpacket * (int_mult + 1) *
+						(int_maxburst + 1);
+				break;
+			case USB_SPEED_HIGH:
+				size = int_maxpacket * (int_mult + 1);
+				break;
+			default:
+				size = int_maxpacket > 1023 ?
+						1023 : int_maxpacket;
+				break;
+			}
+			ep = is_in ? ss->int_in_ep : ss->int_out_ep;
+			req = ss_alloc_ep_req(ep, size);
+			break;
+		default:
 			ep = is_in ? ss->in_ep : ss->out_ep;
 			req = ss_alloc_ep_req(ep, 0);
+			break;
 		}
 
 		if (!req)
@@ -644,12 +870,12 @@ static int source_sink_start_ep(struct f_sourcesink *ss, bool is_in,
 
 			cdev = ss->function.config->cdev;
 			ERROR(cdev, "start %s%s %s --> %d\n",
-			      is_iso ? "ISO-" : "", is_in ? "IN" : "OUT",
-			      ep->name, status);
+				get_ep_string(ep_type), is_in ? "IN" : "OUT",
+				ep->name, status);
 			free_ep_req(ep, req);
 		}
 
-		if (!is_iso)
+		if (!(ep_type == EP_ISOC))
 			break;
 	}
 
@@ -662,7 +888,7 @@ static void disable_source_sink(struct f_sourcesink *ss)
 
 	cdev = ss->function.config->cdev;
 	disable_endpoints(cdev, ss->in_ep, ss->out_ep, ss->iso_in_ep,
-			ss->iso_out_ep);
+			ss->iso_out_ep, ss->int_in_ep, ss->int_out_ep);
 	VDBG(cdev, "%s disabled\n", ss->function.name);
 }
 
@@ -674,6 +900,62 @@ enable_source_sink(struct usb_composite_dev *cdev, struct f_sourcesink *ss,
 	int					speed = cdev->gadget->speed;
 	struct usb_ep				*ep;
 
+	if (alt == 2) {
+		/* Configure for periodic interrupt endpoint */
+		ep = ss->int_in_ep;
+		if (ep) {
+			result = config_ep_by_speed(cdev->gadget,
+					&(ss->function), ep);
+			if (result)
+				return result;
+
+			result = usb_ep_enable(ep);
+			if (result < 0)
+				return result;
+
+			ep->driver_data = ss;
+			result = source_sink_start_ep(ss, true, EP_INTERRUPT,
+					speed);
+			if (result < 0) {
+fail1:
+				ep = ss->int_in_ep;
+				if (ep) {
+					usb_ep_disable(ep);
+					ep->driver_data = NULL;
+				}
+				return result;
+			}
+		}
+
+		/*
+		 * one interrupt endpoint reads (sinks) anything OUT (from the
+		 * host)
+		 */
+		ep = ss->int_out_ep;
+		if (ep) {
+			result = config_ep_by_speed(cdev->gadget,
+					&(ss->function), ep);
+			if (result)
+				goto fail1;
+
+			result = usb_ep_enable(ep);
+			if (result < 0)
+				goto fail1;
+
+			ep->driver_data = ss;
+			result = source_sink_start_ep(ss, false, EP_INTERRUPT,
+					speed);
+			if (result < 0) {
+				ep = ss->int_out_ep;
+				usb_ep_disable(ep);
+				ep->driver_data = NULL;
+				goto fail1;
+			}
+		}
+
+		goto out;
+	}
+
 	/* one bulk endpoint writes (sources) zeroes IN (to the host) */
 	ep = ss->in_ep;
 	result = config_ep_by_speed(cdev->gadget, &(ss->function), ep);
@@ -684,7 +966,7 @@ enable_source_sink(struct usb_composite_dev *cdev, struct f_sourcesink *ss,
 		return result;
 	ep->driver_data = ss;
 
-	result = source_sink_start_ep(ss, true, false, speed);
+	result = source_sink_start_ep(ss, true, EP_BULK, speed);
 	if (result < 0) {
 fail:
 		ep = ss->in_ep;
@@ -703,7 +985,7 @@ fail:
 		goto fail;
 	ep->driver_data = ss;
 
-	result = source_sink_start_ep(ss, false, false, speed);
+	result = source_sink_start_ep(ss, false, EP_BULK, speed);
 	if (result < 0) {
 fail2:
 		ep = ss->out_ep;
@@ -726,7 +1008,7 @@ fail2:
 			goto fail2;
 		ep->driver_data = ss;
 
-		result = source_sink_start_ep(ss, true, true, speed);
+		result = source_sink_start_ep(ss, true, EP_ISOC, speed);
 		if (result < 0) {
 fail3:
 			ep = ss->iso_in_ep;
@@ -749,13 +1031,14 @@ fail3:
 			goto fail3;
 		ep->driver_data = ss;
 
-		result = source_sink_start_ep(ss, false, true, speed);
+		result = source_sink_start_ep(ss, false, EP_ISOC, speed);
 		if (result < 0) {
 			usb_ep_disable(ep);
 			ep->driver_data = NULL;
 			goto fail3;
 		}
 	}
+
 out:
 	ss->cur_alt = alt;
 
@@ -771,6 +1054,8 @@ static int sourcesink_set_alt(struct usb_function *f,
 
 	if (ss->in_ep->driver_data)
 		disable_source_sink(ss);
+	else if (alt == 2 && ss->int_in_ep->driver_data)
+		disable_source_sink(ss);
 	return enable_source_sink(cdev, ss, alt);
 }
 
@@ -883,6 +1168,10 @@ static struct usb_function *source_sink_alloc_func(
 	isoc_maxpacket = ss_opts->isoc_maxpacket;
 	isoc_mult = ss_opts->isoc_mult;
 	isoc_maxburst = ss_opts->isoc_maxburst;
+	int_interval = ss_opts->int_interval;
+	int_maxpacket = ss_opts->int_maxpacket;
+	int_mult = ss_opts->int_mult;
+	int_maxburst = ss_opts->int_maxburst;
 	buflen = ss_opts->bulk_buflen;
 
 	ss->function.name = "source/sink";
@@ -1179,6 +1468,182 @@ static struct f_ss_opts_attribute f_ss_opts_bulk_buflen =
 			f_ss_opts_bulk_buflen_show,
 			f_ss_opts_bulk_buflen_store);
 
+static ssize_t f_ss_opts_int_interval_show(struct f_ss_opts *opts, char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%d", opts->int_interval);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_ss_opts_int_interval_store(struct f_ss_opts *opts,
+				       const char *page, size_t len)
+{
+	int ret;
+	u32 num;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = kstrtou32(page, 0, &num);
+	if (ret)
+		goto end;
+
+	if (num > 4096) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	opts->int_interval = num;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+static struct f_ss_opts_attribute f_ss_opts_int_interval =
+	__CONFIGFS_ATTR(int_interval, S_IRUGO | S_IWUSR,
+			f_ss_opts_int_interval_show,
+			f_ss_opts_int_interval_store);
+
+static ssize_t f_ss_opts_int_maxpacket_show(struct f_ss_opts *opts, char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%d", opts->int_maxpacket);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_ss_opts_int_maxpacket_store(struct f_ss_opts *opts,
+				       const char *page, size_t len)
+{
+	int ret;
+	u16 num;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = kstrtou16(page, 0, &num);
+	if (ret)
+		goto end;
+
+	if (num > 1024) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	opts->int_maxpacket = num;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+static struct f_ss_opts_attribute f_ss_opts_int_maxpacket =
+	__CONFIGFS_ATTR(int_maxpacket, S_IRUGO | S_IWUSR,
+			f_ss_opts_int_maxpacket_show,
+			f_ss_opts_int_maxpacket_store);
+
+static ssize_t f_ss_opts_int_mult_show(struct f_ss_opts *opts, char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%d", opts->int_mult);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_ss_opts_int_mult_store(struct f_ss_opts *opts,
+				       const char *page, size_t len)
+{
+	int ret;
+	u8 num;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = kstrtou8(page, 0, &num);
+	if (ret)
+		goto end;
+
+	if (num > 2) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	opts->int_mult = num;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+static struct f_ss_opts_attribute f_ss_opts_int_mult =
+	__CONFIGFS_ATTR(int_mult, S_IRUGO | S_IWUSR,
+			f_ss_opts_int_mult_show,
+			f_ss_opts_int_mult_store);
+
+static ssize_t f_ss_opts_int_maxburst_show(struct f_ss_opts *opts, char *page)
+{
+	int result;
+
+	mutex_lock(&opts->lock);
+	result = sprintf(page, "%d", opts->int_maxburst);
+	mutex_unlock(&opts->lock);
+
+	return result;
+}
+
+static ssize_t f_ss_opts_int_maxburst_store(struct f_ss_opts *opts,
+				       const char *page, size_t len)
+{
+	int ret;
+	u8 num;
+
+	mutex_lock(&opts->lock);
+	if (opts->refcnt) {
+		ret = -EBUSY;
+		goto end;
+	}
+
+	ret = kstrtou8(page, 0, &num);
+	if (ret)
+		goto end;
+
+	if (num > 15) {
+		ret = -EINVAL;
+		goto end;
+	}
+
+	opts->int_maxburst = num;
+	ret = len;
+end:
+	mutex_unlock(&opts->lock);
+	return ret;
+}
+
+static struct f_ss_opts_attribute f_ss_opts_int_maxburst =
+	__CONFIGFS_ATTR(int_maxburst, S_IRUGO | S_IWUSR,
+			f_ss_opts_int_maxburst_show,
+			f_ss_opts_int_maxburst_store);
+
 static struct configfs_attribute *ss_attrs[] = {
 	&f_ss_opts_pattern.attr,
 	&f_ss_opts_isoc_interval.attr,
@@ -1186,6 +1651,10 @@ static struct configfs_attribute *ss_attrs[] = {
 	&f_ss_opts_isoc_mult.attr,
 	&f_ss_opts_isoc_maxburst.attr,
 	&f_ss_opts_bulk_buflen.attr,
+	&f_ss_opts_int_interval.attr,
+	&f_ss_opts_int_maxpacket.attr,
+	&f_ss_opts_int_mult.attr,
+	&f_ss_opts_int_maxburst.attr,
 	NULL,
 };
 
@@ -1215,6 +1684,8 @@ static struct usb_function_instance *source_sink_alloc_inst(void)
 	ss_opts->isoc_interval = GZERO_ISOC_INTERVAL;
 	ss_opts->isoc_maxpacket = GZERO_ISOC_MAXPACKET;
 	ss_opts->bulk_buflen = GZERO_BULK_BUFLEN;
+	ss_opts->int_interval = GZERO_INT_INTERVAL;
+	ss_opts->int_maxpacket = GZERO_INT_MAXPACKET;
 
 	config_group_init_type_name(&ss_opts->func_inst.group, "",
 				    &ss_func_type);
diff --git a/drivers/usb/gadget/function/f_uac1.c b/drivers/usb/gadget/function/f_uac1.c
index 2b4c82d84bfc..f7b203293205 100644
--- a/drivers/usb/gadget/function/f_uac1.c
+++ b/drivers/usb/gadget/function/f_uac1.c
@@ -11,24 +11,12 @@
 
 #include <linux/slab.h>
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/device.h>
 #include <linux/atomic.h>
 
 #include "u_uac1.h"
 
-#define OUT_EP_MAX_PACKET_SIZE	200
-static int req_buf_size = OUT_EP_MAX_PACKET_SIZE;
-module_param(req_buf_size, int, S_IRUGO);
-MODULE_PARM_DESC(req_buf_size, "ISO OUT endpoint request buffer size");
-
-static int req_count = 256;
-module_param(req_count, int, S_IRUGO);
-MODULE_PARM_DESC(req_count, "ISO OUT endpoint request count");
-
-static int audio_buf_size = 48000;
-module_param(audio_buf_size, int, S_IRUGO);
-MODULE_PARM_DESC(audio_buf_size, "Audio buffer size");
-
 static int generic_set_cmd(struct usb_audio_control *con, u8 cmd, int value);
 static int generic_get_cmd(struct usb_audio_control *con, u8 cmd);
 
@@ -46,7 +34,7 @@ static int generic_get_cmd(struct usb_audio_control *con, u8 cmd);
 #define F_AUDIO_NUM_INTERFACES	2
 
 /* B.3.1  Standard AC Interface Descriptor */
-static struct usb_interface_descriptor ac_interface_desc __initdata = {
+static struct usb_interface_descriptor ac_interface_desc = {
 	.bLength =		USB_DT_INTERFACE_SIZE,
 	.bDescriptorType =	USB_DT_INTERFACE,
 	.bNumEndpoints =	0,
@@ -183,12 +171,12 @@ static struct usb_endpoint_descriptor as_out_ep_desc  = {
 	.bEndpointAddress =	USB_DIR_OUT,
 	.bmAttributes =		USB_ENDPOINT_SYNC_ADAPTIVE
 				| USB_ENDPOINT_XFER_ISOC,
-	.wMaxPacketSize =	__constant_cpu_to_le16(OUT_EP_MAX_PACKET_SIZE),
+	.wMaxPacketSize	=	cpu_to_le16(UAC1_OUT_EP_MAX_PACKET_SIZE),
 	.bInterval =		4,
 };
 
 /* Class-specific AS ISO OUT Endpoint Descriptor */
-static struct uac_iso_endpoint_descriptor as_iso_out_desc __initdata = {
+static struct uac_iso_endpoint_descriptor as_iso_out_desc = {
 	.bLength =		UAC_ISO_ENDPOINT_DESC_SIZE,
 	.bDescriptorType =	USB_DT_CS_ENDPOINT,
 	.bDescriptorSubtype =	UAC_EP_GENERAL,
@@ -197,7 +185,7 @@ static struct uac_iso_endpoint_descriptor as_iso_out_desc __initdata = {
 	.wLockDelay =		__constant_cpu_to_le16(1),
 };
 
-static struct usb_descriptor_header *f_audio_desc[] __initdata = {
+static struct usb_descriptor_header *f_audio_desc[] = {
 	(struct usb_descriptor_header *)&ac_interface_desc,
 	(struct usb_descriptor_header *)&ac_header_desc,
 
@@ -216,6 +204,37 @@ static struct usb_descriptor_header *f_audio_desc[] __initdata = {
 	NULL,
 };
 
+enum {
+	STR_AC_IF,
+	STR_INPUT_TERMINAL,
+	STR_INPUT_TERMINAL_CH_NAMES,
+	STR_FEAT_DESC_0,
+	STR_OUTPUT_TERMINAL,
+	STR_AS_IF_ALT0,
+	STR_AS_IF_ALT1,
+};
+
+static struct usb_string strings_uac1[] = {
+	[STR_AC_IF].s = "AC Interface",
+	[STR_INPUT_TERMINAL].s = "Input terminal",
+	[STR_INPUT_TERMINAL_CH_NAMES].s = "Channels",
+	[STR_FEAT_DESC_0].s = "Volume control & mute",
+	[STR_OUTPUT_TERMINAL].s = "Output terminal",
+	[STR_AS_IF_ALT0].s = "AS Interface",
+	[STR_AS_IF_ALT1].s = "AS Interface",
+	{ },
+};
+
+static struct usb_gadget_strings str_uac1 = {
+	.language = 0x0409,	/* en-us */
+	.strings = strings_uac1,
+};
+
+static struct usb_gadget_strings *uac1_strings[] = {
+	&str_uac1,
+	NULL,
+};
+
 /*
  * This function is an ALSA sound card following USB Audio Class Spec 1.0.
  */
@@ -300,8 +319,14 @@ static int f_audio_out_ep_complete(struct usb_ep *ep, struct usb_request *req)
 	struct f_audio *audio = req->context;
 	struct usb_composite_dev *cdev = audio->card.func.config->cdev;
 	struct f_audio_buf *copy_buf = audio->copy_buf;
+	struct f_uac1_opts *opts;
+	int audio_buf_size;
 	int err;
 
+	opts = container_of(audio->card.func.fi, struct f_uac1_opts,
+			    func_inst);
+	audio_buf_size = opts->audio_buf_size;
+
 	if (!copy_buf)
 		return -EINVAL;
 
@@ -546,10 +571,17 @@ static int f_audio_set_alt(struct usb_function *f, unsigned intf, unsigned alt)
 	struct usb_composite_dev *cdev = f->config->cdev;
 	struct usb_ep *out_ep = audio->out_ep;
 	struct usb_request *req;
+	struct f_uac1_opts *opts;
+	int req_buf_size, req_count, audio_buf_size;
 	int i = 0, err = 0;
 
 	DBG(cdev, "intf %d, alt %d\n", intf, alt);
 
+	opts = container_of(f->fi, struct f_uac1_opts, func_inst);
+	req_buf_size = opts->req_buf_size;
+	req_count = opts->req_count;
+	audio_buf_size = opts->audio_buf_size;
+
 	if (intf == 1) {
 		if (alt == 1) {
 			usb_ep_enable(out_ep);
@@ -625,13 +657,37 @@ static void f_audio_build_desc(struct f_audio *audio)
 }
 
 /* audio function driver setup/binding */
-static int __init
+static int
 f_audio_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_composite_dev *cdev = c->cdev;
 	struct f_audio		*audio = func_to_audio(f);
+	struct usb_string	*us;
 	int			status;
 	struct usb_ep		*ep = NULL;
+	struct f_uac1_opts	*audio_opts;
+
+	audio_opts = container_of(f->fi, struct f_uac1_opts, func_inst);
+	audio->card.gadget = c->cdev->gadget;
+	audio_opts->card = &audio->card;
+	/* set up ASLA audio devices */
+	if (!audio_opts->bound) {
+		status = gaudio_setup(&audio->card);
+		if (status < 0)
+			return status;
+		audio_opts->bound = true;
+	}
+	us = usb_gstrings_attach(cdev, uac1_strings, ARRAY_SIZE(strings_uac1));
+	if (IS_ERR(us))
+		return PTR_ERR(us);
+	ac_interface_desc.iInterface = us[STR_AC_IF].id;
+	input_terminal_desc.iTerminal = us[STR_INPUT_TERMINAL].id;
+	input_terminal_desc.iChannelNames = us[STR_INPUT_TERMINAL_CH_NAMES].id;
+	feature_unit_desc.iFeature = us[STR_FEAT_DESC_0].id;
+	output_terminal_desc.iTerminal = us[STR_OUTPUT_TERMINAL].id;
+	as_interface_alt_0_desc.iInterface = us[STR_AS_IF_ALT0].id;
+	as_interface_alt_1_desc.iInterface = us[STR_AS_IF_ALT1].id;
+
 
 	f_audio_build_desc(audio);
 
@@ -666,20 +722,12 @@ f_audio_bind(struct usb_configuration *c, struct usb_function *f)
 	return 0;
 
 fail:
+	gaudio_cleanup(&audio->card);
 	if (ep)
 		ep->driver_data = NULL;
 	return status;
 }
 
-static void
-f_audio_unbind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct f_audio		*audio = func_to_audio(f);
-
-	usb_free_all_descriptors(f);
-	kfree(audio);
-}
-
 /*-------------------------------------------------------------------------*/
 
 static int generic_set_cmd(struct usb_audio_control *con, u8 cmd, int value)
@@ -695,7 +743,7 @@ static int generic_get_cmd(struct usb_audio_control *con, u8 cmd)
 }
 
 /* Todo: add more control selecotor dynamically */
-static int __init control_selector_init(struct f_audio *audio)
+static int control_selector_init(struct f_audio *audio)
 {
 	INIT_LIST_HEAD(&audio->cs);
 	list_add(&feature_unit.list, &audio->cs);
@@ -712,57 +760,226 @@ static int __init control_selector_init(struct f_audio *audio)
 	return 0;
 }
 
-/**
- * audio_bind_config - add USB audio function to a configuration
- * @c: the configuration to supcard the USB audio function
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- */
-static int __init audio_bind_config(struct usb_configuration *c)
+static inline struct f_uac1_opts *to_f_uac1_opts(struct config_item *item)
+{
+	return container_of(to_config_group(item), struct f_uac1_opts,
+			    func_inst.group);
+}
+
+CONFIGFS_ATTR_STRUCT(f_uac1_opts);
+CONFIGFS_ATTR_OPS(f_uac1_opts);
+
+static void f_uac1_attr_release(struct config_item *item)
+{
+	struct f_uac1_opts *opts = to_f_uac1_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
+}
+
+static struct configfs_item_operations f_uac1_item_ops = {
+	.release	= f_uac1_attr_release,
+	.show_attribute	= f_uac1_opts_attr_show,
+	.store_attribute = f_uac1_opts_attr_store,
+};
+
+#define UAC1_INT_ATTRIBUTE(name)					\
+static ssize_t f_uac1_opts_##name##_show(struct f_uac1_opts *opts,	\
+					 char *page)			\
+{									\
+	int result;							\
+									\
+	mutex_lock(&opts->lock);					\
+	result = sprintf(page, "%u\n", opts->name);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	return result;							\
+}									\
+									\
+static ssize_t f_uac1_opts_##name##_store(struct f_uac1_opts *opts,	\
+					  const char *page, size_t len)	\
+{									\
+	int ret;							\
+	u32 num;							\
+									\
+	mutex_lock(&opts->lock);					\
+	if (opts->refcnt) {						\
+		ret = -EBUSY;						\
+		goto end;						\
+	}								\
+									\
+	ret = kstrtou32(page, 0, &num);					\
+	if (ret)							\
+		goto end;						\
+									\
+	opts->name = num;						\
+	ret = len;							\
+									\
+end:									\
+	mutex_unlock(&opts->lock);					\
+	return ret;							\
+}									\
+									\
+static struct f_uac1_opts_attribute f_uac1_opts_##name =		\
+	__CONFIGFS_ATTR(name, S_IRUGO | S_IWUSR,			\
+			f_uac1_opts_##name##_show,			\
+			f_uac1_opts_##name##_store)
+
+UAC1_INT_ATTRIBUTE(req_buf_size);
+UAC1_INT_ATTRIBUTE(req_count);
+UAC1_INT_ATTRIBUTE(audio_buf_size);
+
+#define UAC1_STR_ATTRIBUTE(name)					\
+static ssize_t f_uac1_opts_##name##_show(struct f_uac1_opts *opts,	\
+					 char *page)			\
+{									\
+	int result;							\
+									\
+	mutex_lock(&opts->lock);					\
+	result = sprintf(page, "%s\n", opts->name);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	return result;							\
+}									\
+									\
+static ssize_t f_uac1_opts_##name##_store(struct f_uac1_opts *opts,	\
+					  const char *page, size_t len)	\
+{									\
+	int ret = -EBUSY;						\
+	char *tmp;							\
+									\
+	mutex_lock(&opts->lock);					\
+	if (opts->refcnt)						\
+		goto end;						\
+									\
+	tmp = kstrndup(page, len, GFP_KERNEL);				\
+	if (tmp) {							\
+		ret = -ENOMEM;						\
+		goto end;						\
+	}								\
+	if (opts->name##_alloc)						\
+		kfree(opts->name);					\
+	opts->name##_alloc = true;					\
+	opts->name = tmp;						\
+	ret = len;							\
+									\
+end:									\
+	mutex_unlock(&opts->lock);					\
+	return ret;							\
+}									\
+									\
+static struct f_uac1_opts_attribute f_uac1_opts_##name =		\
+	__CONFIGFS_ATTR(name, S_IRUGO | S_IWUSR,			\
+			f_uac1_opts_##name##_show,			\
+			f_uac1_opts_##name##_store)
+
+UAC1_STR_ATTRIBUTE(fn_play);
+UAC1_STR_ATTRIBUTE(fn_cap);
+UAC1_STR_ATTRIBUTE(fn_cntl);
+
+static struct configfs_attribute *f_uac1_attrs[] = {
+	&f_uac1_opts_req_buf_size.attr,
+	&f_uac1_opts_req_count.attr,
+	&f_uac1_opts_audio_buf_size.attr,
+	&f_uac1_opts_fn_play.attr,
+	&f_uac1_opts_fn_cap.attr,
+	&f_uac1_opts_fn_cntl.attr,
+	NULL,
+};
+
+static struct config_item_type f_uac1_func_type = {
+	.ct_item_ops	= &f_uac1_item_ops,
+	.ct_attrs	= f_uac1_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void f_audio_free_inst(struct usb_function_instance *f)
+{
+	struct f_uac1_opts *opts;
+
+	opts = container_of(f, struct f_uac1_opts, func_inst);
+	gaudio_cleanup(opts->card);
+	if (opts->fn_play_alloc)
+		kfree(opts->fn_play);
+	if (opts->fn_cap_alloc)
+		kfree(opts->fn_cap);
+	if (opts->fn_cntl_alloc)
+		kfree(opts->fn_cntl);
+	kfree(opts);
+}
+
+static struct usb_function_instance *f_audio_alloc_inst(void)
+{
+	struct f_uac1_opts *opts;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_init(&opts->lock);
+	opts->func_inst.free_func_inst = f_audio_free_inst;
+
+	config_group_init_type_name(&opts->func_inst.group, "",
+				    &f_uac1_func_type);
+
+	opts->req_buf_size = UAC1_OUT_EP_MAX_PACKET_SIZE;
+	opts->req_count = UAC1_REQ_COUNT;
+	opts->audio_buf_size = UAC1_AUDIO_BUF_SIZE;
+	opts->fn_play = FILE_PCM_PLAYBACK;
+	opts->fn_cap = FILE_PCM_CAPTURE;
+	opts->fn_cntl = FILE_CONTROL;
+	return &opts->func_inst;
+}
+
+static void f_audio_free(struct usb_function *f)
+{
+	struct f_audio *audio = func_to_audio(f);
+	struct f_uac1_opts *opts;
+
+	opts = container_of(f->fi, struct f_uac1_opts, func_inst);
+	kfree(audio);
+	mutex_lock(&opts->lock);
+	--opts->refcnt;
+	mutex_unlock(&opts->lock);
+}
+
+static void f_audio_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	usb_free_all_descriptors(f);
+}
+
+static struct usb_function *f_audio_alloc(struct usb_function_instance *fi)
 {
 	struct f_audio *audio;
-	int status;
+	struct f_uac1_opts *opts;
 
 	/* allocate and initialize one new instance */
-	audio = kzalloc(sizeof *audio, GFP_KERNEL);
+	audio = kzalloc(sizeof(*audio), GFP_KERNEL);
 	if (!audio)
-		return -ENOMEM;
+		return ERR_PTR(-ENOMEM);
 
 	audio->card.func.name = "g_audio";
-	audio->card.gadget = c->cdev->gadget;
 
+	opts = container_of(fi, struct f_uac1_opts, func_inst);
+	mutex_lock(&opts->lock);
+	++opts->refcnt;
+	mutex_unlock(&opts->lock);
 	INIT_LIST_HEAD(&audio->play_queue);
 	spin_lock_init(&audio->lock);
 
-	/* set up ASLA audio devices */
-	status = gaudio_setup(&audio->card);
-	if (status < 0)
-		goto setup_fail;
-
-	audio->card.func.strings = audio_strings;
 	audio->card.func.bind = f_audio_bind;
 	audio->card.func.unbind = f_audio_unbind;
 	audio->card.func.set_alt = f_audio_set_alt;
 	audio->card.func.setup = f_audio_setup;
 	audio->card.func.disable = f_audio_disable;
+	audio->card.func.free_func = f_audio_free;
 
 	control_selector_init(audio);
 
 	INIT_WORK(&audio->playback_work, f_audio_playback_work);
 
-	status = usb_add_function(c, &audio->card.func);
-	if (status)
-		goto add_fail;
-
-	INFO(c->cdev, "audio_buf_size %d, req_buf_size %d, req_count %d\n",
-		audio_buf_size, req_buf_size, req_count);
-
-	return status;
-
-add_fail:
-	gaudio_cleanup();
-setup_fail:
-	kfree(audio);
-	return status;
+	return &audio->card.func;
 }
+
+DECLARE_USB_FUNCTION_INIT(uac1, f_audio_alloc_inst, f_audio_alloc);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Bryan Wu");
diff --git a/drivers/usb/gadget/function/f_uac2.c b/drivers/usb/gadget/function/f_uac2.c
index 3ed89ecc8d6d..a5a27a504d67 100644
--- a/drivers/usb/gadget/function/f_uac2.c
+++ b/drivers/usb/gadget/function/f_uac2.c
@@ -20,35 +20,7 @@
 #include <sound/pcm.h>
 #include <sound/pcm_params.h>
 
-/* Playback(USB-IN) Default Stereo - Fl/Fr */
-static int p_chmask = 0x3;
-module_param(p_chmask, uint, S_IRUGO);
-MODULE_PARM_DESC(p_chmask, "Playback Channel Mask");
-
-/* Playback Default 48 KHz */
-static int p_srate = 48000;
-module_param(p_srate, uint, S_IRUGO);
-MODULE_PARM_DESC(p_srate, "Playback Sampling Rate");
-
-/* Playback Default 16bits/sample */
-static int p_ssize = 2;
-module_param(p_ssize, uint, S_IRUGO);
-MODULE_PARM_DESC(p_ssize, "Playback Sample Size(bytes)");
-
-/* Capture(USB-OUT) Default Stereo - Fl/Fr */
-static int c_chmask = 0x3;
-module_param(c_chmask, uint, S_IRUGO);
-MODULE_PARM_DESC(c_chmask, "Capture Channel Mask");
-
-/* Capture Default 64 KHz */
-static int c_srate = 64000;
-module_param(c_srate, uint, S_IRUGO);
-MODULE_PARM_DESC(c_srate, "Capture Sampling Rate");
-
-/* Capture Default 16bits/sample */
-static int c_ssize = 2;
-module_param(c_ssize, uint, S_IRUGO);
-MODULE_PARM_DESC(c_ssize, "Capture Sample Size(bytes)");
+#include "u_uac2.h"
 
 /* Keep everyone on toes */
 #define USB_XFERS	2
@@ -120,6 +92,15 @@ struct snd_uac2_chip {
 
 	struct snd_card *card;
 	struct snd_pcm *pcm;
+
+	/* timekeeping for the playback endpoint */
+	unsigned int p_interval;
+	unsigned int p_residue;
+
+	/* pre-calculated values for playback iso completion */
+	unsigned int p_pktsize;
+	unsigned int p_pktsize_residue;
+	unsigned int p_framesize;
 };
 
 #define BUFF_SIZE_MAX	(PAGE_SIZE * 16)
@@ -149,8 +130,6 @@ struct audio_dev {
 	struct snd_uac2_chip uac2;
 };
 
-static struct audio_dev *agdev_g;
-
 static inline
 struct audio_dev *func_to_agdev(struct usb_function *f)
 {
@@ -170,6 +149,12 @@ struct snd_uac2_chip *pdev_to_uac2(struct platform_device *p)
 }
 
 static inline
+struct f_uac2_opts *agdev_to_uac2_opts(struct audio_dev *agdev)
+{
+	return container_of(agdev->func.fi, struct f_uac2_opts, func_inst);
+}
+
+static inline
 uint num_channels(uint chanmask)
 {
 	uint num = 0;
@@ -187,8 +172,8 @@ agdev_iso_complete(struct usb_ep *ep, struct usb_request *req)
 {
 	unsigned pending;
 	unsigned long flags;
+	unsigned int hw_ptr;
 	bool update_alsa = false;
-	unsigned char *src, *dst;
 	int status = req->status;
 	struct uac2_req *ur = req->context;
 	struct snd_pcm_substream *substream;
@@ -216,12 +201,27 @@ agdev_iso_complete(struct usb_ep *ep, struct usb_request *req)
 	spin_lock_irqsave(&prm->lock, flags);
 
 	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
-		src = prm->dma_area + prm->hw_ptr;
+		/*
+		 * For each IN packet, take the quotient of the current data
+		 * rate and the endpoint's interval as the base packet size.
+		 * If there is a residue from this division, add it to the
+		 * residue accumulator.
+		 */
+		req->length = uac2->p_pktsize;
+		uac2->p_residue += uac2->p_pktsize_residue;
+
+		/*
+		 * Whenever there are more bytes in the accumulator than we
+		 * need to add one more sample frame, increase this packet's
+		 * size and decrease the accumulator.
+		 */
+		if (uac2->p_residue / uac2->p_interval >= uac2->p_framesize) {
+			req->length += uac2->p_framesize;
+			uac2->p_residue -= uac2->p_framesize *
+					   uac2->p_interval;
+		}
+
 		req->actual = req->length;
-		dst = req->buf;
-	} else {
-		dst = prm->dma_area + prm->hw_ptr;
-		src = req->buf;
 	}
 
 	pending = prm->hw_ptr % prm->period_size;
@@ -229,12 +229,32 @@ agdev_iso_complete(struct usb_ep *ep, struct usb_request *req)
 	if (pending >= prm->period_size)
 		update_alsa = true;
 
+	hw_ptr = prm->hw_ptr;
 	prm->hw_ptr = (prm->hw_ptr + req->actual) % prm->dma_bytes;
 
 	spin_unlock_irqrestore(&prm->lock, flags);
 
 	/* Pack USB load in ALSA ring buffer */
-	memcpy(dst, src, req->actual);
+	pending = prm->dma_bytes - hw_ptr;
+
+	if (substream->stream == SNDRV_PCM_STREAM_PLAYBACK) {
+		if (unlikely(pending < req->actual)) {
+			memcpy(req->buf, prm->dma_area + hw_ptr, pending);
+			memcpy(req->buf + pending, prm->dma_area,
+			       req->actual - pending);
+		} else {
+			memcpy(req->buf, prm->dma_area + hw_ptr, req->actual);
+		}
+	} else {
+		if (unlikely(pending < req->actual)) {
+			memcpy(prm->dma_area + hw_ptr, req->buf, pending);
+			memcpy(prm->dma_area, req->buf + pending,
+			       req->actual - pending);
+		} else {
+			memcpy(prm->dma_area + hw_ptr, req->buf, req->actual);
+		}
+	}
+
 exit:
 	if (usb_ep_queue(ep, req, GFP_ATOMIC))
 		dev_err(&uac2->pdev.dev, "%d Error!\n", __LINE__);
@@ -342,6 +362,21 @@ static int uac2_pcm_open(struct snd_pcm_substream *substream)
 {
 	struct snd_uac2_chip *uac2 = snd_pcm_substream_chip(substream);
 	struct snd_pcm_runtime *runtime = substream->runtime;
+	struct audio_dev *audio_dev;
+	struct f_uac2_opts *opts;
+	int p_ssize, c_ssize;
+	int p_srate, c_srate;
+	int p_chmask, c_chmask;
+
+	audio_dev = uac2_to_agdev(uac2);
+	opts = container_of(audio_dev->func.fi, struct f_uac2_opts, func_inst);
+	p_ssize = opts->p_ssize;
+	c_ssize = opts->c_ssize;
+	p_srate = opts->p_srate;
+	c_srate = opts->c_srate;
+	p_chmask = opts->p_chmask;
+	c_chmask = opts->c_chmask;
+	uac2->p_residue = 0;
 
 	runtime->hw = uac2_pcm_hardware;
 
@@ -411,7 +446,15 @@ static int snd_uac2_probe(struct platform_device *pdev)
 	struct snd_uac2_chip *uac2 = pdev_to_uac2(pdev);
 	struct snd_card *card;
 	struct snd_pcm *pcm;
+	struct audio_dev *audio_dev;
+	struct f_uac2_opts *opts;
 	int err;
+	int p_chmask, c_chmask;
+
+	audio_dev = uac2_to_agdev(uac2);
+	opts = container_of(audio_dev->func.fi, struct f_uac2_opts, func_inst);
+	p_chmask = opts->p_chmask;
+	c_chmask = opts->c_chmask;
 
 	/* Choose any slot, with no id */
 	err = snd_card_new(&pdev->dev, -1, NULL, THIS_MODULE, 0, &card);
@@ -919,20 +962,58 @@ free_ep(struct uac2_rtd_params *prm, struct usb_ep *ep)
 			"%s:%d Error!\n", __func__, __LINE__);
 }
 
-static int __init
+static int
 afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 {
 	struct audio_dev *agdev = func_to_agdev(fn);
 	struct snd_uac2_chip *uac2 = &agdev->uac2;
 	struct usb_composite_dev *cdev = cfg->cdev;
 	struct usb_gadget *gadget = cdev->gadget;
+	struct device *dev = &uac2->pdev.dev;
 	struct uac2_rtd_params *prm;
+	struct f_uac2_opts *uac2_opts;
+	struct usb_string *us;
 	int ret;
 
+	uac2_opts = container_of(fn->fi, struct f_uac2_opts, func_inst);
+
+	us = usb_gstrings_attach(cdev, fn_strings, ARRAY_SIZE(strings_fn));
+	if (IS_ERR(us))
+		return PTR_ERR(us);
+	iad_desc.iFunction = us[STR_ASSOC].id;
+	std_ac_if_desc.iInterface = us[STR_IF_CTRL].id;
+	in_clk_src_desc.iClockSource = us[STR_CLKSRC_IN].id;
+	out_clk_src_desc.iClockSource = us[STR_CLKSRC_OUT].id;
+	usb_out_it_desc.iTerminal = us[STR_USB_IT].id;
+	io_in_it_desc.iTerminal = us[STR_IO_IT].id;
+	usb_in_ot_desc.iTerminal = us[STR_USB_OT].id;
+	io_out_ot_desc.iTerminal = us[STR_IO_OT].id;
+	std_as_out_if0_desc.iInterface = us[STR_AS_OUT_ALT0].id;
+	std_as_out_if1_desc.iInterface = us[STR_AS_OUT_ALT1].id;
+	std_as_in_if0_desc.iInterface = us[STR_AS_IN_ALT0].id;
+	std_as_in_if1_desc.iInterface = us[STR_AS_IN_ALT1].id;
+
+
+	/* Initialize the configurable parameters */
+	usb_out_it_desc.bNrChannels = num_channels(uac2_opts->c_chmask);
+	usb_out_it_desc.bmChannelConfig = cpu_to_le32(uac2_opts->c_chmask);
+	io_in_it_desc.bNrChannels = num_channels(uac2_opts->p_chmask);
+	io_in_it_desc.bmChannelConfig = cpu_to_le32(uac2_opts->p_chmask);
+	as_out_hdr_desc.bNrChannels = num_channels(uac2_opts->c_chmask);
+	as_out_hdr_desc.bmChannelConfig = cpu_to_le32(uac2_opts->c_chmask);
+	as_in_hdr_desc.bNrChannels = num_channels(uac2_opts->p_chmask);
+	as_in_hdr_desc.bmChannelConfig = cpu_to_le32(uac2_opts->p_chmask);
+	as_out_fmt1_desc.bSubslotSize = uac2_opts->c_ssize;
+	as_out_fmt1_desc.bBitResolution = uac2_opts->c_ssize * 8;
+	as_in_fmt1_desc.bSubslotSize = uac2_opts->p_ssize;
+	as_in_fmt1_desc.bBitResolution = uac2_opts->p_ssize * 8;
+
+	snprintf(clksrc_in, sizeof(clksrc_in), "%uHz", uac2_opts->p_srate);
+	snprintf(clksrc_out, sizeof(clksrc_out), "%uHz", uac2_opts->c_srate);
+
 	ret = usb_interface_id(cfg, fn);
 	if (ret < 0) {
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		return ret;
 	}
 	std_ac_if_desc.bInterfaceNumber = ret;
@@ -941,8 +1022,7 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 
 	ret = usb_interface_id(cfg, fn);
 	if (ret < 0) {
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		return ret;
 	}
 	std_as_out_if0_desc.bInterfaceNumber = ret;
@@ -952,8 +1032,7 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 
 	ret = usb_interface_id(cfg, fn);
 	if (ret < 0) {
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		return ret;
 	}
 	std_as_in_if0_desc.bInterfaceNumber = ret;
@@ -963,16 +1042,14 @@ afunc_bind(struct usb_configuration *cfg, struct usb_function *fn)
 
 	agdev->out_ep = usb_ep_autoconfig(gadget, &fs_epout_desc);
 	if (!agdev->out_ep) {
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		goto err;
 	}
 	agdev->out_ep->driver_data = agdev;
 
 	agdev->in_ep = usb_ep_autoconfig(gadget, &fs_epin_desc);
 	if (!agdev->in_ep) {
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		goto err;
 	}
 	agdev->in_ep->driver_data = agdev;
@@ -1020,27 +1097,6 @@ err:
 	return -EINVAL;
 }
 
-static void
-afunc_unbind(struct usb_configuration *cfg, struct usb_function *fn)
-{
-	struct audio_dev *agdev = func_to_agdev(fn);
-	struct uac2_rtd_params *prm;
-
-	alsa_uac2_exit(agdev);
-
-	prm = &agdev->uac2.p_prm;
-	kfree(prm->rbuf);
-
-	prm = &agdev->uac2.c_prm;
-	kfree(prm->rbuf);
-	usb_free_all_descriptors(fn);
-
-	if (agdev->in_ep)
-		agdev->in_ep->driver_data = NULL;
-	if (agdev->out_ep)
-		agdev->out_ep->driver_data = NULL;
-}
-
 static int
 afunc_set_alt(struct usb_function *fn, unsigned intf, unsigned alt)
 {
@@ -1048,23 +1104,22 @@ afunc_set_alt(struct usb_function *fn, unsigned intf, unsigned alt)
 	struct audio_dev *agdev = func_to_agdev(fn);
 	struct snd_uac2_chip *uac2 = &agdev->uac2;
 	struct usb_gadget *gadget = cdev->gadget;
+	struct device *dev = &uac2->pdev.dev;
 	struct usb_request *req;
 	struct usb_ep *ep;
 	struct uac2_rtd_params *prm;
-	int i;
+	int req_len, i;
 
 	/* No i/f has more than 2 alt settings */
 	if (alt > 1) {
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		return -EINVAL;
 	}
 
 	if (intf == agdev->ac_intf) {
 		/* Control I/f has only 1 AltSetting - 0 */
 		if (alt) {
-			dev_err(&uac2->pdev.dev,
-				"%s:%d Error!\n", __func__, __LINE__);
+			dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 			return -EINVAL;
 		}
 		return 0;
@@ -1075,14 +1130,43 @@ afunc_set_alt(struct usb_function *fn, unsigned intf, unsigned alt)
 		prm = &uac2->c_prm;
 		config_ep_by_speed(gadget, fn, ep);
 		agdev->as_out_alt = alt;
+		req_len = prm->max_psize;
 	} else if (intf == agdev->as_in_intf) {
+		struct f_uac2_opts *opts = agdev_to_uac2_opts(agdev);
+		unsigned int factor, rate;
+		struct usb_endpoint_descriptor *ep_desc;
+
 		ep = agdev->in_ep;
 		prm = &uac2->p_prm;
 		config_ep_by_speed(gadget, fn, ep);
 		agdev->as_in_alt = alt;
+
+		/* pre-calculate the playback endpoint's interval */
+		if (gadget->speed == USB_SPEED_FULL) {
+			ep_desc = &fs_epin_desc;
+			factor = 1000;
+		} else {
+			ep_desc = &hs_epin_desc;
+			factor = 125;
+		}
+
+		/* pre-compute some values for iso_complete() */
+		uac2->p_framesize = opts->p_ssize *
+				    num_channels(opts->p_chmask);
+		rate = opts->p_srate * uac2->p_framesize;
+		uac2->p_interval = (1 << (ep_desc->bInterval - 1)) * factor;
+		uac2->p_pktsize = min_t(unsigned int, rate / uac2->p_interval,
+					prm->max_psize);
+
+		if (uac2->p_pktsize < prm->max_psize)
+			uac2->p_pktsize_residue = rate % uac2->p_interval;
+		else
+			uac2->p_pktsize_residue = 0;
+
+		req_len = uac2->p_pktsize;
+		uac2->p_residue = 0;
 	} else {
-		dev_err(&uac2->pdev.dev,
-			"%s:%d Error!\n", __func__, __LINE__);
+		dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 		return -EINVAL;
 	}
 
@@ -1095,31 +1179,23 @@ afunc_set_alt(struct usb_function *fn, unsigned intf, unsigned alt)
 	usb_ep_enable(ep);
 
 	for (i = 0; i < USB_XFERS; i++) {
-		if (prm->ureq[i].req) {
-			if (usb_ep_queue(ep, prm->ureq[i].req, GFP_ATOMIC))
-				dev_err(&uac2->pdev.dev, "%d Error!\n",
-					__LINE__);
-			continue;
-		}
-
-		req = usb_ep_alloc_request(ep, GFP_ATOMIC);
-		if (req == NULL) {
-			dev_err(&uac2->pdev.dev,
-				"%s:%d Error!\n", __func__, __LINE__);
-			return -EINVAL;
+		if (!prm->ureq[i].req) {
+			req = usb_ep_alloc_request(ep, GFP_ATOMIC);
+			if (req == NULL)
+				return -ENOMEM;
+
+			prm->ureq[i].req = req;
+			prm->ureq[i].pp = prm;
+
+			req->zero = 0;
+			req->context = &prm->ureq[i];
+			req->length = req_len;
+			req->complete = agdev_iso_complete;
+			req->buf = prm->rbuf + i * prm->max_psize;
 		}
 
-		prm->ureq[i].req = req;
-		prm->ureq[i].pp = prm;
-
-		req->zero = 0;
-		req->context = &prm->ureq[i];
-		req->length = prm->max_psize;
-		req->complete =	agdev_iso_complete;
-		req->buf = prm->rbuf + i * req->length;
-
-		if (usb_ep_queue(ep, req, GFP_ATOMIC))
-			dev_err(&uac2->pdev.dev, "%d Error!\n", __LINE__);
+		if (usb_ep_queue(ep, prm->ureq[i].req, GFP_ATOMIC))
+			dev_err(dev, "%s:%d Error!\n", __func__, __LINE__);
 	}
 
 	return 0;
@@ -1164,12 +1240,18 @@ in_rq_cur(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 	struct usb_request *req = fn->config->cdev->req;
 	struct audio_dev *agdev = func_to_agdev(fn);
 	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct f_uac2_opts *opts;
 	u16 w_length = le16_to_cpu(cr->wLength);
 	u16 w_index = le16_to_cpu(cr->wIndex);
 	u16 w_value = le16_to_cpu(cr->wValue);
 	u8 entity_id = (w_index >> 8) & 0xff;
 	u8 control_selector = w_value >> 8;
 	int value = -EOPNOTSUPP;
+	int p_srate, c_srate;
+
+	opts = agdev_to_uac2_opts(agdev);
+	p_srate = opts->p_srate;
+	c_srate = opts->c_srate;
 
 	if (control_selector == UAC2_CS_CONTROL_SAM_FREQ) {
 		struct cntrl_cur_lay3 c;
@@ -1199,6 +1281,7 @@ in_rq_range(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 	struct usb_request *req = fn->config->cdev->req;
 	struct audio_dev *agdev = func_to_agdev(fn);
 	struct snd_uac2_chip *uac2 = &agdev->uac2;
+	struct f_uac2_opts *opts;
 	u16 w_length = le16_to_cpu(cr->wLength);
 	u16 w_index = le16_to_cpu(cr->wIndex);
 	u16 w_value = le16_to_cpu(cr->wValue);
@@ -1206,6 +1289,11 @@ in_rq_range(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 	u8 control_selector = w_value >> 8;
 	struct cntrl_range_lay3 r;
 	int value = -EOPNOTSUPP;
+	int p_srate, c_srate;
+
+	opts = agdev_to_uac2_opts(agdev);
+	p_srate = opts->p_srate;
+	c_srate = opts->c_srate;
 
 	if (control_selector == UAC2_CS_CONTROL_SAM_FREQ) {
 		if (entity_id == USB_IN_CLK_ID)
@@ -1309,66 +1397,184 @@ afunc_setup(struct usb_function *fn, const struct usb_ctrlrequest *cr)
 	return value;
 }
 
-static int audio_bind_config(struct usb_configuration *cfg)
+static inline struct f_uac2_opts *to_f_uac2_opts(struct config_item *item)
 {
-	int res;
-
-	agdev_g = kzalloc(sizeof *agdev_g, GFP_KERNEL);
-	if (agdev_g == NULL)
-		return -ENOMEM;
-
-	res = usb_string_ids_tab(cfg->cdev, strings_fn);
-	if (res)
-		return res;
-	iad_desc.iFunction = strings_fn[STR_ASSOC].id;
-	std_ac_if_desc.iInterface = strings_fn[STR_IF_CTRL].id;
-	in_clk_src_desc.iClockSource = strings_fn[STR_CLKSRC_IN].id;
-	out_clk_src_desc.iClockSource = strings_fn[STR_CLKSRC_OUT].id;
-	usb_out_it_desc.iTerminal = strings_fn[STR_USB_IT].id;
-	io_in_it_desc.iTerminal = strings_fn[STR_IO_IT].id;
-	usb_in_ot_desc.iTerminal = strings_fn[STR_USB_OT].id;
-	io_out_ot_desc.iTerminal = strings_fn[STR_IO_OT].id;
-	std_as_out_if0_desc.iInterface = strings_fn[STR_AS_OUT_ALT0].id;
-	std_as_out_if1_desc.iInterface = strings_fn[STR_AS_OUT_ALT1].id;
-	std_as_in_if0_desc.iInterface = strings_fn[STR_AS_IN_ALT0].id;
-	std_as_in_if1_desc.iInterface = strings_fn[STR_AS_IN_ALT1].id;
-
-	agdev_g->func.name = "uac2_func";
-	agdev_g->func.strings = fn_strings;
-	agdev_g->func.bind = afunc_bind;
-	agdev_g->func.unbind = afunc_unbind;
-	agdev_g->func.set_alt = afunc_set_alt;
-	agdev_g->func.get_alt = afunc_get_alt;
-	agdev_g->func.disable = afunc_disable;
-	agdev_g->func.setup = afunc_setup;
+	return container_of(to_config_group(item), struct f_uac2_opts,
+			    func_inst.group);
+}
 
-	/* Initialize the configurable parameters */
-	usb_out_it_desc.bNrChannels = num_channels(c_chmask);
-	usb_out_it_desc.bmChannelConfig = cpu_to_le32(c_chmask);
-	io_in_it_desc.bNrChannels = num_channels(p_chmask);
-	io_in_it_desc.bmChannelConfig = cpu_to_le32(p_chmask);
-	as_out_hdr_desc.bNrChannels = num_channels(c_chmask);
-	as_out_hdr_desc.bmChannelConfig = cpu_to_le32(c_chmask);
-	as_in_hdr_desc.bNrChannels = num_channels(p_chmask);
-	as_in_hdr_desc.bmChannelConfig = cpu_to_le32(p_chmask);
-	as_out_fmt1_desc.bSubslotSize = c_ssize;
-	as_out_fmt1_desc.bBitResolution = c_ssize * 8;
-	as_in_fmt1_desc.bSubslotSize = p_ssize;
-	as_in_fmt1_desc.bBitResolution = p_ssize * 8;
-
-	snprintf(clksrc_in, sizeof(clksrc_in), "%uHz", p_srate);
-	snprintf(clksrc_out, sizeof(clksrc_out), "%uHz", c_srate);
-
-	res = usb_add_function(cfg, &agdev_g->func);
-	if (res < 0)
-		kfree(agdev_g);
-
-	return res;
+CONFIGFS_ATTR_STRUCT(f_uac2_opts);
+CONFIGFS_ATTR_OPS(f_uac2_opts);
+
+static void f_uac2_attr_release(struct config_item *item)
+{
+	struct f_uac2_opts *opts = to_f_uac2_opts(item);
+
+	usb_put_function_instance(&opts->func_inst);
 }
 
-static void
-uac2_unbind_config(struct usb_configuration *cfg)
+static struct configfs_item_operations f_uac2_item_ops = {
+	.release	= f_uac2_attr_release,
+	.show_attribute	= f_uac2_opts_attr_show,
+	.store_attribute = f_uac2_opts_attr_store,
+};
+
+#define UAC2_ATTRIBUTE(name)						\
+static ssize_t f_uac2_opts_##name##_show(struct f_uac2_opts *opts,	\
+					 char *page)			\
+{									\
+	int result;							\
+									\
+	mutex_lock(&opts->lock);					\
+	result = sprintf(page, "%u\n", opts->name);			\
+	mutex_unlock(&opts->lock);					\
+									\
+	return result;							\
+}									\
+									\
+static ssize_t f_uac2_opts_##name##_store(struct f_uac2_opts *opts,	\
+					  const char *page, size_t len)	\
+{									\
+	int ret;							\
+	u32 num;							\
+									\
+	mutex_lock(&opts->lock);					\
+	if (opts->refcnt) {						\
+		ret = -EBUSY;						\
+		goto end;						\
+	}								\
+									\
+	ret = kstrtou32(page, 0, &num);					\
+	if (ret)							\
+		goto end;						\
+									\
+	opts->name = num;						\
+	ret = len;							\
+									\
+end:									\
+	mutex_unlock(&opts->lock);					\
+	return ret;							\
+}									\
+									\
+static struct f_uac2_opts_attribute f_uac2_opts_##name =		\
+	__CONFIGFS_ATTR(name, S_IRUGO | S_IWUSR,			\
+			f_uac2_opts_##name##_show,			\
+			f_uac2_opts_##name##_store)
+
+UAC2_ATTRIBUTE(p_chmask);
+UAC2_ATTRIBUTE(p_srate);
+UAC2_ATTRIBUTE(p_ssize);
+UAC2_ATTRIBUTE(c_chmask);
+UAC2_ATTRIBUTE(c_srate);
+UAC2_ATTRIBUTE(c_ssize);
+
+static struct configfs_attribute *f_uac2_attrs[] = {
+	&f_uac2_opts_p_chmask.attr,
+	&f_uac2_opts_p_srate.attr,
+	&f_uac2_opts_p_ssize.attr,
+	&f_uac2_opts_c_chmask.attr,
+	&f_uac2_opts_c_srate.attr,
+	&f_uac2_opts_c_ssize.attr,
+	NULL,
+};
+
+static struct config_item_type f_uac2_func_type = {
+	.ct_item_ops	= &f_uac2_item_ops,
+	.ct_attrs	= f_uac2_attrs,
+	.ct_owner	= THIS_MODULE,
+};
+
+static void afunc_free_inst(struct usb_function_instance *f)
 {
-	kfree(agdev_g);
-	agdev_g = NULL;
+	struct f_uac2_opts *opts;
+
+	opts = container_of(f, struct f_uac2_opts, func_inst);
+	kfree(opts);
 }
+
+static struct usb_function_instance *afunc_alloc_inst(void)
+{
+	struct f_uac2_opts *opts;
+
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_init(&opts->lock);
+	opts->func_inst.free_func_inst = afunc_free_inst;
+
+	config_group_init_type_name(&opts->func_inst.group, "",
+				    &f_uac2_func_type);
+
+	opts->p_chmask = UAC2_DEF_PCHMASK;
+	opts->p_srate = UAC2_DEF_PSRATE;
+	opts->p_ssize = UAC2_DEF_PSSIZE;
+	opts->c_chmask = UAC2_DEF_CCHMASK;
+	opts->c_srate = UAC2_DEF_CSRATE;
+	opts->c_ssize = UAC2_DEF_CSSIZE;
+	return &opts->func_inst;
+}
+
+static void afunc_free(struct usb_function *f)
+{
+	struct audio_dev *agdev;
+	struct f_uac2_opts *opts;
+
+	agdev = func_to_agdev(f);
+	opts = container_of(f->fi, struct f_uac2_opts, func_inst);
+	kfree(agdev);
+	mutex_lock(&opts->lock);
+	--opts->refcnt;
+	mutex_unlock(&opts->lock);
+}
+
+static void afunc_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct audio_dev *agdev = func_to_agdev(f);
+	struct uac2_rtd_params *prm;
+
+	alsa_uac2_exit(agdev);
+
+	prm = &agdev->uac2.p_prm;
+	kfree(prm->rbuf);
+
+	prm = &agdev->uac2.c_prm;
+	kfree(prm->rbuf);
+	usb_free_all_descriptors(f);
+
+	if (agdev->in_ep)
+		agdev->in_ep->driver_data = NULL;
+	if (agdev->out_ep)
+		agdev->out_ep->driver_data = NULL;
+}
+
+struct usb_function *afunc_alloc(struct usb_function_instance *fi)
+{
+	struct audio_dev *agdev;
+	struct f_uac2_opts *opts;
+
+	agdev = kzalloc(sizeof(*agdev), GFP_KERNEL);
+	if (agdev == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	opts = container_of(fi, struct f_uac2_opts, func_inst);
+	mutex_lock(&opts->lock);
+	++opts->refcnt;
+	mutex_unlock(&opts->lock);
+
+	agdev->func.name = "uac2_func";
+	agdev->func.bind = afunc_bind;
+	agdev->func.unbind = afunc_unbind;
+	agdev->func.set_alt = afunc_set_alt;
+	agdev->func.get_alt = afunc_get_alt;
+	agdev->func.disable = afunc_disable;
+	agdev->func.setup = afunc_setup;
+	agdev->func.free_func = afunc_free;
+
+	return &agdev->func;
+}
+
+DECLARE_USB_FUNCTION_INIT(uac2, afunc_alloc_inst, afunc_alloc);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Yadwinder Singh");
+MODULE_AUTHOR("Jaswinder Singh");
diff --git a/drivers/usb/gadget/function/f_uvc.c b/drivers/usb/gadget/function/f_uvc.c
index e2a1f50bd93c..e126439e4b65 100644
--- a/drivers/usb/gadget/function/f_uvc.c
+++ b/drivers/usb/gadget/function/f_uvc.c
@@ -11,6 +11,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/device.h>
 #include <linux/errno.h>
 #include <linux/fs.h>
@@ -27,24 +28,12 @@
 #include <media/v4l2-event.h>
 
 #include "uvc.h"
+#include "uvc_v4l2.h"
+#include "uvc_video.h"
+#include "u_uvc.h"
 
 unsigned int uvc_gadget_trace_param;
 
-/*-------------------------------------------------------------------------*/
-
-/* module parameters specific to the Video streaming endpoint */
-static unsigned int streaming_interval = 1;
-module_param(streaming_interval, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(streaming_interval, "1 - 16");
-
-static unsigned int streaming_maxpacket = 1024;
-module_param(streaming_maxpacket, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(streaming_maxpacket, "1 - 1023 (FS), 1 - 3072 (hs/ss)");
-
-static unsigned int streaming_maxburst;
-module_param(streaming_maxburst, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(streaming_maxburst, "0 - 15 (ss only)");
-
 /* --------------------------------------------------------------------------
  * Function descriptors
  */
@@ -75,7 +64,7 @@ static struct usb_gadget_strings *uvc_function_strings[] = {
 
 #define UVC_STATUS_MAX_PACKET_SIZE		16	/* 16 bytes status */
 
-static struct usb_interface_assoc_descriptor uvc_iad __initdata = {
+static struct usb_interface_assoc_descriptor uvc_iad = {
 	.bLength		= sizeof(uvc_iad),
 	.bDescriptorType	= USB_DT_INTERFACE_ASSOCIATION,
 	.bFirstInterface	= 0,
@@ -86,7 +75,7 @@ static struct usb_interface_assoc_descriptor uvc_iad __initdata = {
 	.iFunction		= 0,
 };
 
-static struct usb_interface_descriptor uvc_control_intf __initdata = {
+static struct usb_interface_descriptor uvc_control_intf = {
 	.bLength		= USB_DT_INTERFACE_SIZE,
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= UVC_INTF_VIDEO_CONTROL,
@@ -98,7 +87,7 @@ static struct usb_interface_descriptor uvc_control_intf __initdata = {
 	.iInterface		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_control_ep __initdata = {
+static struct usb_endpoint_descriptor uvc_control_ep = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
@@ -107,7 +96,7 @@ static struct usb_endpoint_descriptor uvc_control_ep __initdata = {
 	.bInterval		= 8,
 };
 
-static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp __initdata = {
+static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp = {
 	.bLength		= sizeof(uvc_ss_control_comp),
 	.bDescriptorType	= USB_DT_SS_ENDPOINT_COMP,
 	/* The following 3 values can be tweaked if necessary. */
@@ -116,14 +105,14 @@ static struct usb_ss_ep_comp_descriptor uvc_ss_control_comp __initdata = {
 	.wBytesPerInterval	= cpu_to_le16(UVC_STATUS_MAX_PACKET_SIZE),
 };
 
-static struct uvc_control_endpoint_descriptor uvc_control_cs_ep __initdata = {
+static struct uvc_control_endpoint_descriptor uvc_control_cs_ep = {
 	.bLength		= UVC_DT_CONTROL_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_CS_ENDPOINT,
 	.bDescriptorSubType	= UVC_EP_INTERRUPT,
 	.wMaxTransferSize	= cpu_to_le16(UVC_STATUS_MAX_PACKET_SIZE),
 };
 
-static struct usb_interface_descriptor uvc_streaming_intf_alt0 __initdata = {
+static struct usb_interface_descriptor uvc_streaming_intf_alt0 = {
 	.bLength		= USB_DT_INTERFACE_SIZE,
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= UVC_INTF_VIDEO_STREAMING,
@@ -135,7 +124,7 @@ static struct usb_interface_descriptor uvc_streaming_intf_alt0 __initdata = {
 	.iInterface		= 0,
 };
 
-static struct usb_interface_descriptor uvc_streaming_intf_alt1 __initdata = {
+static struct usb_interface_descriptor uvc_streaming_intf_alt1 = {
 	.bLength		= USB_DT_INTERFACE_SIZE,
 	.bDescriptorType	= USB_DT_INTERFACE,
 	.bInterfaceNumber	= UVC_INTF_VIDEO_STREAMING,
@@ -147,7 +136,7 @@ static struct usb_interface_descriptor uvc_streaming_intf_alt1 __initdata = {
 	.iInterface		= 0,
 };
 
-static struct usb_endpoint_descriptor uvc_fs_streaming_ep __initdata = {
+static struct usb_endpoint_descriptor uvc_fs_streaming_ep = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
@@ -158,7 +147,7 @@ static struct usb_endpoint_descriptor uvc_fs_streaming_ep __initdata = {
 	 */
 };
 
-static struct usb_endpoint_descriptor uvc_hs_streaming_ep __initdata = {
+static struct usb_endpoint_descriptor uvc_hs_streaming_ep = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 	.bEndpointAddress	= USB_DIR_IN,
@@ -169,7 +158,7 @@ static struct usb_endpoint_descriptor uvc_hs_streaming_ep __initdata = {
 	 */
 };
 
-static struct usb_endpoint_descriptor uvc_ss_streaming_ep __initdata = {
+static struct usb_endpoint_descriptor uvc_ss_streaming_ep = {
 	.bLength		= USB_DT_ENDPOINT_SIZE,
 	.bDescriptorType	= USB_DT_ENDPOINT,
 
@@ -181,7 +170,7 @@ static struct usb_endpoint_descriptor uvc_ss_streaming_ep __initdata = {
 	 */
 };
 
-static struct usb_ss_ep_comp_descriptor uvc_ss_streaming_comp __initdata = {
+static struct usb_ss_ep_comp_descriptor uvc_ss_streaming_comp = {
 	.bLength		= sizeof(uvc_ss_streaming_comp),
 	.bDescriptorType	= USB_DT_SS_ENDPOINT_COMP,
 	/* The bMaxBurst, bmAttributes and wBytesPerInterval values will be
@@ -208,6 +197,12 @@ static const struct usb_descriptor_header * const uvc_ss_streaming[] = {
 	NULL,
 };
 
+void uvc_set_trace_param(unsigned int trace)
+{
+	uvc_gadget_trace_param = trace;
+}
+EXPORT_SYMBOL(uvc_set_trace_param);
+
 /* --------------------------------------------------------------------------
  * Control requests
  */
@@ -251,6 +246,12 @@ uvc_function_setup(struct usb_function *f, const struct usb_ctrlrequest *ctrl)
 	if (le16_to_cpu(ctrl->wLength) > UVC_MAX_REQUEST_SIZE)
 		return -EINVAL;
 
+	/* Tell the complete callback to generate an event for the next request
+	 * that will be enqueued by UVCIOC_SEND_RESPONSE.
+	 */
+	uvc->event_setup_out = !(ctrl->bRequestType & USB_DIR_IN);
+	uvc->event_length = le16_to_cpu(ctrl->wLength);
+
 	memset(&v4l2_event, 0, sizeof(v4l2_event));
 	v4l2_event.type = UVC_EVENT_SETUP;
 	memcpy(&uvc_event->req, ctrl, sizeof(uvc_event->req));
@@ -408,7 +409,9 @@ uvc_register_video(struct uvc_device *uvc)
 
 	video->v4l2_dev = &uvc->v4l2_dev;
 	video->fops = &uvc_v4l2_fops;
+	video->ioctl_ops = &uvc_v4l2_ioctl_ops;
 	video->release = video_device_release;
+	video->vfl_dir = VFL_DIR_TX;
 	strlcpy(video->name, cdev->gadget->name, sizeof(video->name));
 
 	uvc->vdev = video;
@@ -434,7 +437,7 @@ uvc_register_video(struct uvc_device *uvc)
 		} \
 	} while (0)
 
-static struct usb_descriptor_header ** __init
+static struct usb_descriptor_header **
 uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 {
 	struct uvc_input_header_descriptor *uvc_streaming_header;
@@ -554,45 +557,26 @@ uvc_copy_descriptors(struct uvc_device *uvc, enum usb_device_speed speed)
 	return hdr;
 }
 
-static void
-uvc_function_unbind(struct usb_configuration *c, struct usb_function *f)
-{
-	struct usb_composite_dev *cdev = c->cdev;
-	struct uvc_device *uvc = to_uvc(f);
-
-	INFO(cdev, "uvc_function_unbind\n");
-
-	video_unregister_device(uvc->vdev);
-	v4l2_device_unregister(&uvc->v4l2_dev);
-	uvc->control_ep->driver_data = NULL;
-	uvc->video.ep->driver_data = NULL;
-
-	uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id = 0;
-	usb_ep_free_request(cdev->gadget->ep0, uvc->control_req);
-	kfree(uvc->control_buf);
-
-	usb_free_all_descriptors(f);
-
-	kfree(uvc);
-}
-
-static int __init
+static int
 uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 {
 	struct usb_composite_dev *cdev = c->cdev;
 	struct uvc_device *uvc = to_uvc(f);
+	struct usb_string *us;
 	unsigned int max_packet_mult;
 	unsigned int max_packet_size;
 	struct usb_ep *ep;
+	struct f_uvc_opts *opts;
 	int ret = -EINVAL;
 
 	INFO(cdev, "uvc_function_bind\n");
 
+	opts = to_f_uvc_opts(f->fi);
 	/* Sanity check the streaming endpoint module parameters.
 	 */
-	streaming_interval = clamp(streaming_interval, 1U, 16U);
-	streaming_maxpacket = clamp(streaming_maxpacket, 1U, 3072U);
-	streaming_maxburst = min(streaming_maxburst, 15U);
+	opts->streaming_interval = clamp(opts->streaming_interval, 1U, 16U);
+	opts->streaming_maxpacket = clamp(opts->streaming_maxpacket, 1U, 3072U);
+	opts->streaming_maxburst = min(opts->streaming_maxburst, 15U);
 
 	/* Fill in the FS/HS/SS Video Streaming specific descriptors from the
 	 * module parameters.
@@ -600,30 +584,32 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	 * NOTE: We assume that the user knows what they are doing and won't
 	 * give parameters that their UDC doesn't support.
 	 */
-	if (streaming_maxpacket <= 1024) {
+	if (opts->streaming_maxpacket <= 1024) {
 		max_packet_mult = 1;
-		max_packet_size = streaming_maxpacket;
-	} else if (streaming_maxpacket <= 2048) {
+		max_packet_size = opts->streaming_maxpacket;
+	} else if (opts->streaming_maxpacket <= 2048) {
 		max_packet_mult = 2;
-		max_packet_size = streaming_maxpacket / 2;
+		max_packet_size = opts->streaming_maxpacket / 2;
 	} else {
 		max_packet_mult = 3;
-		max_packet_size = streaming_maxpacket / 3;
+		max_packet_size = opts->streaming_maxpacket / 3;
 	}
 
-	uvc_fs_streaming_ep.wMaxPacketSize = min(streaming_maxpacket, 1023U);
-	uvc_fs_streaming_ep.bInterval = streaming_interval;
+	uvc_fs_streaming_ep.wMaxPacketSize =
+		cpu_to_le16(min(opts->streaming_maxpacket, 1023U));
+	uvc_fs_streaming_ep.bInterval = opts->streaming_interval;
 
-	uvc_hs_streaming_ep.wMaxPacketSize = max_packet_size;
-	uvc_hs_streaming_ep.wMaxPacketSize |= ((max_packet_mult - 1) << 11);
-	uvc_hs_streaming_ep.bInterval = streaming_interval;
+	uvc_hs_streaming_ep.wMaxPacketSize =
+		cpu_to_le16(max_packet_size | ((max_packet_mult - 1) << 11));
+	uvc_hs_streaming_ep.bInterval = opts->streaming_interval;
 
-	uvc_ss_streaming_ep.wMaxPacketSize = max_packet_size;
-	uvc_ss_streaming_ep.bInterval = streaming_interval;
+	uvc_ss_streaming_ep.wMaxPacketSize = cpu_to_le16(max_packet_size);
+	uvc_ss_streaming_ep.bInterval = opts->streaming_interval;
 	uvc_ss_streaming_comp.bmAttributes = max_packet_mult - 1;
-	uvc_ss_streaming_comp.bMaxBurst = streaming_maxburst;
+	uvc_ss_streaming_comp.bMaxBurst = opts->streaming_maxburst;
 	uvc_ss_streaming_comp.wBytesPerInterval =
-		max_packet_size * max_packet_mult * streaming_maxburst;
+		cpu_to_le16(max_packet_size * max_packet_mult *
+			    opts->streaming_maxburst);
 
 	/* Allocate endpoints. */
 	ep = usb_ep_autoconfig(cdev->gadget, &uvc_control_ep);
@@ -653,6 +639,18 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	uvc_hs_streaming_ep.bEndpointAddress = uvc->video.ep->address;
 	uvc_ss_streaming_ep.bEndpointAddress = uvc->video.ep->address;
 
+	us = usb_gstrings_attach(cdev, uvc_function_strings,
+				 ARRAY_SIZE(uvc_en_us_strings));
+	if (IS_ERR(us)) {
+		ret = PTR_ERR(us);
+		goto error;
+	}
+	uvc_iad.iFunction = us[UVC_STRING_CONTROL_IDX].id;
+	uvc_control_intf.iInterface = us[UVC_STRING_CONTROL_IDX].id;
+	ret = us[UVC_STRING_STREAMING_IDX].id;
+	uvc_streaming_intf_alt0.iInterface = ret;
+	uvc_streaming_intf_alt1.iInterface = ret;
+
 	/* Allocate interface IDs. */
 	if ((ret = usb_interface_id(c, f)) < 0)
 		goto error;
@@ -697,7 +695,7 @@ uvc_function_bind(struct usb_configuration *c, struct usb_function *f)
 	}
 
 	/* Initialise video. */
-	ret = uvc_video_init(&uvc->video);
+	ret = uvcg_video_init(&uvc->video);
 	if (ret < 0)
 		goto error;
 
@@ -720,10 +718,9 @@ error:
 	if (uvc->video.ep)
 		uvc->video.ep->driver_data = NULL;
 
-	if (uvc->control_req) {
+	if (uvc->control_req)
 		usb_ep_free_request(cdev->gadget->ep0, uvc->control_req);
-		kfree(uvc->control_buf);
-	}
+	kfree(uvc->control_buf);
 
 	usb_free_all_descriptors(f);
 	return ret;
@@ -733,104 +730,81 @@ error:
  * USB gadget function
  */
 
-/**
- * uvc_bind_config - add a UVC function to a configuration
- * @c: the configuration to support the UVC instance
- * Context: single threaded during gadget setup
- *
- * Returns zero on success, else negative errno.
- *
- * Caller must have called @uvc_setup(). Caller is also responsible for
- * calling @uvc_cleanup() before module unload.
- */
-int __init
-uvc_bind_config(struct usb_configuration *c,
-		const struct uvc_descriptor_header * const *fs_control,
-		const struct uvc_descriptor_header * const *ss_control,
-		const struct uvc_descriptor_header * const *fs_streaming,
-		const struct uvc_descriptor_header * const *hs_streaming,
-		const struct uvc_descriptor_header * const *ss_streaming)
+static void uvc_free_inst(struct usb_function_instance *f)
 {
-	struct uvc_device *uvc;
-	int ret = 0;
+	struct f_uvc_opts *opts = to_f_uvc_opts(f);
 
-	/* TODO Check if the USB device controller supports the required
-	 * features.
-	 */
-	if (!gadget_is_dualspeed(c->cdev->gadget))
-		return -EINVAL;
+	kfree(opts);
+}
 
-	uvc = kzalloc(sizeof(*uvc), GFP_KERNEL);
-	if (uvc == NULL)
-		return -ENOMEM;
+static struct usb_function_instance *uvc_alloc_inst(void)
+{
+	struct f_uvc_opts *opts;
 
-	uvc->state = UVC_STATE_DISCONNECTED;
+	opts = kzalloc(sizeof(*opts), GFP_KERNEL);
+	if (!opts)
+		return ERR_PTR(-ENOMEM);
+	opts->func_inst.free_func_inst = uvc_free_inst;
 
-	/* Validate the descriptors. */
-	if (fs_control == NULL || fs_control[0] == NULL ||
-	    fs_control[0]->bDescriptorSubType != UVC_VC_HEADER)
-		goto error;
+	return &opts->func_inst;
+}
 
-	if (ss_control == NULL || ss_control[0] == NULL ||
-	    ss_control[0]->bDescriptorSubType != UVC_VC_HEADER)
-		goto error;
+static void uvc_free(struct usb_function *f)
+{
+	struct uvc_device *uvc = to_uvc(f);
 
-	if (fs_streaming == NULL || fs_streaming[0] == NULL ||
-	    fs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
-		goto error;
+	kfree(uvc);
+}
 
-	if (hs_streaming == NULL || hs_streaming[0] == NULL ||
-	    hs_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
-		goto error;
+static void uvc_unbind(struct usb_configuration *c, struct usb_function *f)
+{
+	struct usb_composite_dev *cdev = c->cdev;
+	struct uvc_device *uvc = to_uvc(f);
 
-	if (ss_streaming == NULL || ss_streaming[0] == NULL ||
-	    ss_streaming[0]->bDescriptorSubType != UVC_VS_INPUT_HEADER)
-		goto error;
+	INFO(cdev, "%s\n", __func__);
 
-	uvc->desc.fs_control = fs_control;
-	uvc->desc.ss_control = ss_control;
-	uvc->desc.fs_streaming = fs_streaming;
-	uvc->desc.hs_streaming = hs_streaming;
-	uvc->desc.ss_streaming = ss_streaming;
+	video_unregister_device(uvc->vdev);
+	v4l2_device_unregister(&uvc->v4l2_dev);
+	uvc->control_ep->driver_data = NULL;
+	uvc->video.ep->driver_data = NULL;
 
-	/* String descriptors are global, we only need to allocate string IDs
-	 * for the first UVC function. UVC functions beyond the first (if any)
-	 * will reuse the same IDs.
-	 */
-	if (uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id == 0) {
-		ret = usb_string_ids_tab(c->cdev, uvc_en_us_strings);
-		if (ret)
-			goto error;
-		uvc_iad.iFunction =
-			uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id;
-		uvc_control_intf.iInterface =
-			uvc_en_us_strings[UVC_STRING_CONTROL_IDX].id;
-		ret = uvc_en_us_strings[UVC_STRING_STREAMING_IDX].id;
-		uvc_streaming_intf_alt0.iInterface = ret;
-		uvc_streaming_intf_alt1.iInterface = ret;
-	}
+	usb_ep_free_request(cdev->gadget->ep0, uvc->control_req);
+	kfree(uvc->control_buf);
+
+	usb_free_all_descriptors(f);
+}
+
+static struct usb_function *uvc_alloc(struct usb_function_instance *fi)
+{
+	struct uvc_device *uvc;
+	struct f_uvc_opts *opts;
+
+	uvc = kzalloc(sizeof(*uvc), GFP_KERNEL);
+	if (uvc == NULL)
+		return ERR_PTR(-ENOMEM);
+
+	uvc->state = UVC_STATE_DISCONNECTED;
+	opts = to_f_uvc_opts(fi);
+
+	uvc->desc.fs_control = opts->fs_control;
+	uvc->desc.ss_control = opts->ss_control;
+	uvc->desc.fs_streaming = opts->fs_streaming;
+	uvc->desc.hs_streaming = opts->hs_streaming;
+	uvc->desc.ss_streaming = opts->ss_streaming;
 
 	/* Register the function. */
 	uvc->func.name = "uvc";
-	uvc->func.strings = uvc_function_strings;
 	uvc->func.bind = uvc_function_bind;
-	uvc->func.unbind = uvc_function_unbind;
+	uvc->func.unbind = uvc_unbind;
 	uvc->func.get_alt = uvc_function_get_alt;
 	uvc->func.set_alt = uvc_function_set_alt;
 	uvc->func.disable = uvc_function_disable;
 	uvc->func.setup = uvc_function_setup;
+	uvc->func.free_func = uvc_free;
 
-	ret = usb_add_function(c, &uvc->func);
-	if (ret)
-		kfree(uvc);
-
-	return ret;
-
-error:
-	kfree(uvc);
-	return ret;
+	return &uvc->func;
 }
 
-module_param_named(trace, uvc_gadget_trace_param, uint, S_IRUGO|S_IWUSR);
-MODULE_PARM_DESC(trace, "Trace level bitmask");
-
+DECLARE_USB_FUNCTION_INIT(uvc, uvc_alloc_inst, uvc_alloc);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Laurent Pinchart");
diff --git a/drivers/usb/gadget/function/f_uvc.h b/drivers/usb/gadget/function/f_uvc.h
index ec52752f7326..d0a73bdcbba1 100644
--- a/drivers/usb/gadget/function/f_uvc.h
+++ b/drivers/usb/gadget/function/f_uvc.h
@@ -16,12 +16,13 @@
 #include <linux/usb/composite.h>
 #include <linux/usb/video.h>
 
-int uvc_bind_config(struct usb_configuration *c,
-		    const struct uvc_descriptor_header * const *fs_control,
-		    const struct uvc_descriptor_header * const *hs_control,
-		    const struct uvc_descriptor_header * const *fs_streaming,
-		    const struct uvc_descriptor_header * const *hs_streaming,
-		    const struct uvc_descriptor_header * const *ss_streaming);
+#include "uvc.h"
+
+void uvc_function_setup_continue(struct uvc_device *uvc);
+
+void uvc_function_connect(struct uvc_device *uvc);
+
+void uvc_function_disconnect(struct uvc_device *uvc);
 
 #endif /* _F_UVC_H_ */
 
diff --git a/drivers/usb/gadget/function/g_zero.h b/drivers/usb/gadget/function/g_zero.h
index 15f180904f8a..2ce28b9d97cc 100644
--- a/drivers/usb/gadget/function/g_zero.h
+++ b/drivers/usb/gadget/function/g_zero.h
@@ -10,6 +10,8 @@
 #define GZERO_QLEN		32
 #define GZERO_ISOC_INTERVAL	4
 #define GZERO_ISOC_MAXPACKET	1024
+#define GZERO_INT_INTERVAL	1 /* Default interrupt interval = 1 ms */
+#define GZERO_INT_MAXPACKET	1024
 
 struct usb_zero_options {
 	unsigned pattern;
@@ -17,6 +19,10 @@ struct usb_zero_options {
 	unsigned isoc_maxpacket;
 	unsigned isoc_mult;
 	unsigned isoc_maxburst;
+	unsigned int_interval; /* In ms */
+	unsigned int_maxpacket;
+	unsigned int_mult;
+	unsigned int_maxburst;
 	unsigned bulk_buflen;
 	unsigned qlen;
 };
@@ -28,6 +34,10 @@ struct f_ss_opts {
 	unsigned isoc_maxpacket;
 	unsigned isoc_mult;
 	unsigned isoc_maxburst;
+	unsigned int_interval; /* In ms */
+	unsigned int_maxpacket;
+	unsigned int_mult;
+	unsigned int_maxburst;
 	unsigned bulk_buflen;
 
 	/*
@@ -62,6 +72,7 @@ int lb_modinit(void);
 void free_ep_req(struct usb_ep *ep, struct usb_request *req);
 void disable_endpoints(struct usb_composite_dev *cdev,
 		struct usb_ep *in, struct usb_ep *out,
-		struct usb_ep *iso_in, struct usb_ep *iso_out);
+		struct usb_ep *iso_in, struct usb_ep *iso_out,
+		struct usb_ep *int_in, struct usb_ep *int_out);
 
 #endif /* __G_ZERO_H */
diff --git a/drivers/usb/gadget/function/u_ether.c b/drivers/usb/gadget/function/u_ether.c
index d50adda913cf..6e6f87656e7b 100644
--- a/drivers/usb/gadget/function/u_ether.c
+++ b/drivers/usb/gadget/function/u_ether.c
@@ -1127,10 +1127,7 @@ void gether_disconnect(struct gether *link)
 
 	DBG(dev, "%s\n", __func__);
 
-	netif_tx_lock(dev->net);
 	netif_stop_queue(dev->net);
-	netif_tx_unlock(dev->net);
-
 	netif_carrier_off(dev->net);
 
 	/* disable endpoints, forcing (synchronous) completion
diff --git a/drivers/usb/gadget/function/u_fs.h b/drivers/usb/gadget/function/u_fs.h
index 63d6e71569c1..cd128e31f808 100644
--- a/drivers/usb/gadget/function/u_fs.h
+++ b/drivers/usb/gadget/function/u_fs.h
@@ -224,6 +224,10 @@ struct ffs_data {
 	void				*ms_os_descs_ext_prop_name_avail;
 	void				*ms_os_descs_ext_prop_data_avail;
 
+	unsigned			user_flags;
+
+	u8				eps_addrmap[15];
+
 	unsigned short			strings_count;
 	unsigned short			interfaces_count;
 	unsigned short			eps_count;
diff --git a/drivers/usb/gadget/function/u_serial.c b/drivers/usb/gadget/function/u_serial.c
index ad0aca812002..491082aaf103 100644
--- a/drivers/usb/gadget/function/u_serial.c
+++ b/drivers/usb/gadget/function/u_serial.c
@@ -55,11 +55,8 @@
  * for a telephone or fax link.  And ttyGS2 might be something that just
  * needs a simple byte stream interface for some messaging protocol that
  * is managed in userspace ... OBEX, PTP, and MTP have been mentioned.
- */
-
-#define PREFIX	"ttyGS"
-
-/*
+ *
+ *
  * gserial is the lifecycle interface, used by USB functions
  * gs_port is the I/O nexus, used by the tty driver
  * tty_struct links to the tty/filesystem framework
@@ -385,9 +382,9 @@ __acquires(&port->port_lock)
 		list_del(&req->list);
 		req->zero = (gs_buf_data_avail(&port->port_write_buf) == 0);
 
-		pr_vdebug(PREFIX "%d: tx len=%d, 0x%02x 0x%02x 0x%02x ...\n",
-				port->port_num, len, *((u8 *)req->buf),
-				*((u8 *)req->buf+1), *((u8 *)req->buf+2));
+		pr_vdebug("ttyGS%d: tx len=%d, 0x%02x 0x%02x 0x%02x ...\n",
+			  port->port_num, len, *((u8 *)req->buf),
+			  *((u8 *)req->buf+1), *((u8 *)req->buf+2));
 
 		/* Drop lock while we call out of driver; completions
 		 * could be issued while we do so.  Disconnection may
@@ -503,13 +500,13 @@ static void gs_rx_push(unsigned long _port)
 		switch (req->status) {
 		case -ESHUTDOWN:
 			disconnect = true;
-			pr_vdebug(PREFIX "%d: shutdown\n", port->port_num);
+			pr_vdebug("ttyGS%d: shutdown\n", port->port_num);
 			break;
 
 		default:
 			/* presumably a transient fault */
-			pr_warning(PREFIX "%d: unexpected RX status %d\n",
-					port->port_num, req->status);
+			pr_warn("ttyGS%d: unexpected RX status %d\n",
+				port->port_num, req->status);
 			/* FALLTHROUGH */
 		case 0:
 			/* normal completion */
@@ -537,9 +534,8 @@ static void gs_rx_push(unsigned long _port)
 			if (count != size) {
 				/* stop pushing; TTY layer can't handle more */
 				port->n_read += count;
-				pr_vdebug(PREFIX "%d: rx block %d/%d\n",
-						port->port_num,
-						count, req->actual);
+				pr_vdebug("ttyGS%d: rx block %d/%d\n",
+					  port->port_num, count, req->actual);
 				break;
 			}
 			port->n_read = 0;
@@ -569,7 +565,7 @@ static void gs_rx_push(unsigned long _port)
 			if (do_push)
 				tasklet_schedule(&port->push);
 			else
-				pr_warning(PREFIX "%d: RX not scheduled?\n",
+				pr_warn("ttyGS%d: RX not scheduled?\n",
 					port->port_num);
 		}
 	}
@@ -985,7 +981,7 @@ static void gs_unthrottle(struct tty_struct *tty)
 		 * read queue backs up enough we'll be NAKing OUT packets.
 		 */
 		tasklet_schedule(&port->push);
-		pr_vdebug(PREFIX "%d: unthrottle\n", port->port_num);
+		pr_vdebug("ttyGS%d: unthrottle\n", port->port_num);
 	}
 	spin_unlock_irqrestore(&port->port_lock, flags);
 }
@@ -1295,7 +1291,7 @@ static int userial_init(void)
 		return -ENOMEM;
 
 	gs_tty_driver->driver_name = "g_serial";
-	gs_tty_driver->name = PREFIX;
+	gs_tty_driver->name = "ttyGS";
 	/* uses dynamically assigned dev_t values */
 
 	gs_tty_driver->type = TTY_DRIVER_TYPE_SERIAL;
diff --git a/drivers/usb/gadget/function/u_uac1.c b/drivers/usb/gadget/function/u_uac1.c
index 7a55fea43430..a44a07f30281 100644
--- a/drivers/usb/gadget/function/u_uac1.c
+++ b/drivers/usb/gadget/function/u_uac1.c
@@ -10,6 +10,7 @@
  */
 
 #include <linux/kernel.h>
+#include <linux/module.h>
 #include <linux/slab.h>
 #include <linux/device.h>
 #include <linux/delay.h>
@@ -23,22 +24,6 @@
  * This component encapsulates the ALSA devices for USB audio gadget
  */
 
-#define FILE_PCM_PLAYBACK	"/dev/snd/pcmC0D0p"
-#define FILE_PCM_CAPTURE	"/dev/snd/pcmC0D0c"
-#define FILE_CONTROL		"/dev/snd/controlC0"
-
-static char *fn_play = FILE_PCM_PLAYBACK;
-module_param(fn_play, charp, S_IRUGO);
-MODULE_PARM_DESC(fn_play, "Playback PCM device file name");
-
-static char *fn_cap = FILE_PCM_CAPTURE;
-module_param(fn_cap, charp, S_IRUGO);
-MODULE_PARM_DESC(fn_cap, "Capture PCM device file name");
-
-static char *fn_cntl = FILE_CONTROL;
-module_param(fn_cntl, charp, S_IRUGO);
-MODULE_PARM_DESC(fn_cntl, "Control device file name");
-
 /*-------------------------------------------------------------------------*/
 
 /**
@@ -167,7 +152,7 @@ static int playback_default_hw_params(struct gaudio_snd_dev *snd)
 /**
  * Playback audio buffer data by ALSA PCM device
  */
-static size_t u_audio_playback(struct gaudio *card, void *buf, size_t count)
+size_t u_audio_playback(struct gaudio *card, void *buf, size_t count)
 {
 	struct gaudio_snd_dev	*snd = &card->playback;
 	struct snd_pcm_substream *substream = snd->substream;
@@ -202,12 +187,12 @@ try_again:
 	return 0;
 }
 
-static int u_audio_get_playback_channels(struct gaudio *card)
+int u_audio_get_playback_channels(struct gaudio *card)
 {
 	return card->playback.channels;
 }
 
-static int u_audio_get_playback_rate(struct gaudio *card)
+int u_audio_get_playback_rate(struct gaudio *card)
 {
 	return card->playback.rate;
 }
@@ -220,6 +205,13 @@ static int gaudio_open_snd_dev(struct gaudio *card)
 {
 	struct snd_pcm_file *pcm_file;
 	struct gaudio_snd_dev *snd;
+	struct f_uac1_opts *opts;
+	char *fn_play, *fn_cap, *fn_cntl;
+
+	opts = container_of(card->func.fi, struct f_uac1_opts, func_inst);
+	fn_play = opts->fn_play;
+	fn_cap = opts->fn_cap;
+	fn_cntl = opts->fn_cntl;
 
 	if (!card)
 		return -ENODEV;
@@ -293,7 +285,6 @@ static int gaudio_close_snd_dev(struct gaudio *gau)
 	return 0;
 }
 
-static struct gaudio *the_card;
 /**
  * gaudio_setup - setup ALSA interface and preparing for USB transfer
  *
@@ -301,15 +292,13 @@ static struct gaudio *the_card;
  *
  * Returns negative errno, or zero on success
  */
-int __init gaudio_setup(struct gaudio *card)
+int gaudio_setup(struct gaudio *card)
 {
 	int	ret;
 
 	ret = gaudio_open_snd_dev(card);
 	if (ret)
 		ERROR(card, "we need at least one control device\n");
-	else if (!the_card)
-		the_card = card;
 
 	return ret;
 
@@ -320,11 +309,10 @@ int __init gaudio_setup(struct gaudio *card)
  *
  * This is called to free all resources allocated by @gaudio_setup().
  */
-void gaudio_cleanup(void)
+void gaudio_cleanup(struct gaudio *the_card)
 {
 	if (the_card) {
 		gaudio_close_snd_dev(the_card);
-		the_card = NULL;
 	}
 }
 
diff --git a/drivers/usb/gadget/function/u_uac1.h b/drivers/usb/gadget/function/u_uac1.h
index 18c2e729faf6..f8b17fe82efe 100644
--- a/drivers/usb/gadget/function/u_uac1.h
+++ b/drivers/usb/gadget/function/u_uac1.h
@@ -23,6 +23,14 @@
 
 #include "gadget_chips.h"
 
+#define FILE_PCM_PLAYBACK	"/dev/snd/pcmC0D0p"
+#define FILE_PCM_CAPTURE	"/dev/snd/pcmC0D0c"
+#define FILE_CONTROL		"/dev/snd/controlC0"
+
+#define UAC1_OUT_EP_MAX_PACKET_SIZE	200
+#define UAC1_REQ_COUNT			256
+#define UAC1_AUDIO_BUF_SIZE		48000
+
 /*
  * This represents the USB side of an audio card device, managed by a USB
  * function which provides control and stream interfaces.
@@ -50,7 +58,28 @@ struct gaudio {
 	/* TODO */
 };
 
+struct f_uac1_opts {
+	struct usb_function_instance	func_inst;
+	int				req_buf_size;
+	int				req_count;
+	int				audio_buf_size;
+	char				*fn_play;
+	char				*fn_cap;
+	char				*fn_cntl;
+	unsigned			bound:1;
+	unsigned			fn_play_alloc:1;
+	unsigned			fn_cap_alloc:1;
+	unsigned			fn_cntl_alloc:1;
+	struct gaudio			*card;
+	struct mutex			lock;
+	int				refcnt;
+};
+
 int gaudio_setup(struct gaudio *card);
-void gaudio_cleanup(void);
+void gaudio_cleanup(struct gaudio *the_card);
+
+size_t u_audio_playback(struct gaudio *card, void *buf, size_t count);
+int u_audio_get_playback_channels(struct gaudio *card);
+int u_audio_get_playback_rate(struct gaudio *card);
 
 #endif /* __U_AUDIO_H */
diff --git a/drivers/usb/gadget/function/u_uac2.h b/drivers/usb/gadget/function/u_uac2.h
new file mode 100644
index 000000000000..78dd37279bd4
--- /dev/null
+++ b/drivers/usb/gadget/function/u_uac2.h
@@ -0,0 +1,42 @@
+/*
+ * u_uac2.h
+ *
+ * Utility definitions for UAC2 function
+ *
+ * Copyright (c) 2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef U_UAC2_H
+#define U_UAC2_H
+
+#include <linux/usb/composite.h>
+
+#define UAC2_DEF_PCHMASK 0x3
+#define UAC2_DEF_PSRATE 48000
+#define UAC2_DEF_PSSIZE 2
+#define UAC2_DEF_CCHMASK 0x3
+#define UAC2_DEF_CSRATE 64000
+#define UAC2_DEF_CSSIZE 2
+
+struct f_uac2_opts {
+	struct usb_function_instance	func_inst;
+	int				p_chmask;
+	int				p_srate;
+	int				p_ssize;
+	int				c_chmask;
+	int				c_srate;
+	int				c_ssize;
+	bool				bound;
+
+	struct mutex			lock;
+	int				refcnt;
+};
+
+#endif
diff --git a/drivers/usb/gadget/function/u_uvc.h b/drivers/usb/gadget/function/u_uvc.h
new file mode 100644
index 000000000000..2a8dfdff0332
--- /dev/null
+++ b/drivers/usb/gadget/function/u_uvc.h
@@ -0,0 +1,39 @@
+/*
+ * u_uvc.h
+ *
+ * Utility definitions for the uvc function
+ *
+ * Copyright (c) 2013-2014 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *
+ * Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef U_UVC_H
+#define U_UVC_H
+
+#include <linux/usb/composite.h>
+
+#define to_f_uvc_opts(f)	container_of(f, struct f_uvc_opts, func_inst)
+
+struct f_uvc_opts {
+	struct usb_function_instance			func_inst;
+	unsigned int					uvc_gadget_trace_param;
+	unsigned int					streaming_interval;
+	unsigned int					streaming_maxpacket;
+	unsigned int					streaming_maxburst;
+	const struct uvc_descriptor_header * const	*fs_control;
+	const struct uvc_descriptor_header * const	*ss_control;
+	const struct uvc_descriptor_header * const	*fs_streaming;
+	const struct uvc_descriptor_header * const	*hs_streaming;
+	const struct uvc_descriptor_header * const	*ss_streaming;
+};
+
+void uvc_set_trace_param(unsigned int trace);
+
+#endif /* U_UVC_H */
+
diff --git a/drivers/usb/gadget/function/uvc.h b/drivers/usb/gadget/function/uvc.h
index 7a9111de8054..f67695cb28f8 100644
--- a/drivers/usb/gadget/function/uvc.h
+++ b/drivers/usb/gadget/function/uvc.h
@@ -53,6 +53,7 @@ struct uvc_event
 #ifdef __KERNEL__
 
 #include <linux/usb.h>	/* For usb_endpoint_* */
+#include <linux/usb/composite.h>
 #include <linux/usb/gadget.h>
 #include <linux/videodev2.h>
 #include <linux/version.h>
@@ -96,9 +97,6 @@ extern unsigned int uvc_gadget_trace_param;
  * Driver specific constants
  */
 
-#define DRIVER_VERSION				"0.1.0"
-#define DRIVER_VERSION_NUMBER			KERNEL_VERSION(0, 1, 0)
-
 #define UVC_NUM_REQUESTS			4
 #define UVC_MAX_REQUEST_SIZE			64
 #define UVC_MAX_EVENTS				4
diff --git a/drivers/usb/gadget/function/uvc_queue.c b/drivers/usb/gadget/function/uvc_queue.c
index 1c29bc954db9..8ea8b3b227b4 100644
--- a/drivers/usb/gadget/function/uvc_queue.c
+++ b/drivers/usb/gadget/function/uvc_queue.c
@@ -28,7 +28,7 @@
 /* ------------------------------------------------------------------------
  * Video buffers queue management.
  *
- * Video queues is initialized by uvc_queue_init(). The function performs
+ * Video queues is initialized by uvcg_queue_init(). The function performs
  * basic initialization of the uvc_video_queue struct and never fails.
  *
  * Video buffers are managed by videobuf2. The driver uses a mutex to protect
@@ -126,13 +126,12 @@ static struct vb2_ops uvc_queue_qops = {
 	.wait_finish = uvc_wait_finish,
 };
 
-static int uvc_queue_init(struct uvc_video_queue *queue,
-			  enum v4l2_buf_type type)
+int uvcg_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type)
 {
 	int ret;
 
 	queue->queue.type = type;
-	queue->queue.io_modes = VB2_MMAP | VB2_USERPTR;
+	queue->queue.io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF;
 	queue->queue.drv_priv = queue;
 	queue->queue.buf_struct_size = sizeof(struct uvc_buffer);
 	queue->queue.ops = &uvc_queue_qops;
@@ -154,7 +153,7 @@ static int uvc_queue_init(struct uvc_video_queue *queue,
 /*
  * Free the video buffers.
  */
-static void uvc_free_buffers(struct uvc_video_queue *queue)
+void uvcg_free_buffers(struct uvc_video_queue *queue)
 {
 	mutex_lock(&queue->mutex);
 	vb2_queue_release(&queue->queue);
@@ -164,8 +163,8 @@ static void uvc_free_buffers(struct uvc_video_queue *queue)
 /*
  * Allocate the video buffers.
  */
-static int uvc_alloc_buffers(struct uvc_video_queue *queue,
-			     struct v4l2_requestbuffers *rb)
+int uvcg_alloc_buffers(struct uvc_video_queue *queue,
+			      struct v4l2_requestbuffers *rb)
 {
 	int ret;
 
@@ -176,8 +175,7 @@ static int uvc_alloc_buffers(struct uvc_video_queue *queue,
 	return ret ? ret : rb->count;
 }
 
-static int uvc_query_buffer(struct uvc_video_queue *queue,
-			    struct v4l2_buffer *buf)
+int uvcg_query_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf)
 {
 	int ret;
 
@@ -188,8 +186,7 @@ static int uvc_query_buffer(struct uvc_video_queue *queue,
 	return ret;
 }
 
-static int uvc_queue_buffer(struct uvc_video_queue *queue,
-			    struct v4l2_buffer *buf)
+int uvcg_queue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf)
 {
 	unsigned long flags;
 	int ret;
@@ -213,8 +210,8 @@ done:
  * Dequeue a video buffer. If nonblocking is false, block until a buffer is
  * available.
  */
-static int uvc_dequeue_buffer(struct uvc_video_queue *queue,
-			      struct v4l2_buffer *buf, int nonblocking)
+int uvcg_dequeue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf,
+			int nonblocking)
 {
 	int ret;
 
@@ -231,8 +228,8 @@ static int uvc_dequeue_buffer(struct uvc_video_queue *queue,
  * This function implements video queue polling and is intended to be used by
  * the device poll handler.
  */
-static unsigned int uvc_queue_poll(struct uvc_video_queue *queue,
-				   struct file *file, poll_table *wait)
+unsigned int uvcg_queue_poll(struct uvc_video_queue *queue, struct file *file,
+			     poll_table *wait)
 {
 	unsigned int ret;
 
@@ -243,8 +240,7 @@ static unsigned int uvc_queue_poll(struct uvc_video_queue *queue,
 	return ret;
 }
 
-static int uvc_queue_mmap(struct uvc_video_queue *queue,
-			  struct vm_area_struct *vma)
+int uvcg_queue_mmap(struct uvc_video_queue *queue, struct vm_area_struct *vma)
 {
 	int ret;
 
@@ -261,8 +257,8 @@ static int uvc_queue_mmap(struct uvc_video_queue *queue,
  *
  * NO-MMU arch need this function to make mmap() work correctly.
  */
-static unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
-		unsigned long pgoff)
+unsigned long uvcg_queue_get_unmapped_area(struct uvc_video_queue *queue,
+					   unsigned long pgoff)
 {
 	unsigned long ret;
 
@@ -285,7 +281,7 @@ static unsigned long uvc_queue_get_unmapped_area(struct uvc_video_queue *queue,
  * This function acquires the irq spinlock and can be called from interrupt
  * context.
  */
-static void uvc_queue_cancel(struct uvc_video_queue *queue, int disconnect)
+void uvcg_queue_cancel(struct uvc_video_queue *queue, int disconnect)
 {
 	struct uvc_buffer *buf;
 	unsigned long flags;
@@ -324,9 +320,9 @@ static void uvc_queue_cancel(struct uvc_video_queue *queue, int disconnect)
  * the main queue.
  *
  * This function can't be called from interrupt context. Use
- * uvc_queue_cancel() instead.
+ * uvcg_queue_cancel() instead.
  */
-static int uvc_queue_enable(struct uvc_video_queue *queue, int enable)
+int uvcg_queue_enable(struct uvc_video_queue *queue, int enable)
 {
 	unsigned long flags;
 	int ret = 0;
@@ -363,8 +359,8 @@ done:
 }
 
 /* called with &queue_irqlock held.. */
-static struct uvc_buffer *uvc_queue_next_buffer(struct uvc_video_queue *queue,
-						struct uvc_buffer *buf)
+struct uvc_buffer *uvcg_queue_next_buffer(struct uvc_video_queue *queue,
+					  struct uvc_buffer *buf)
 {
 	struct uvc_buffer *nextbuf;
 
@@ -392,7 +388,7 @@ static struct uvc_buffer *uvc_queue_next_buffer(struct uvc_video_queue *queue,
 	return nextbuf;
 }
 
-static struct uvc_buffer *uvc_queue_head(struct uvc_video_queue *queue)
+struct uvc_buffer *uvcg_queue_head(struct uvc_video_queue *queue)
 {
 	struct uvc_buffer *buf = NULL;
 
diff --git a/drivers/usb/gadget/function/uvc_queue.h b/drivers/usb/gadget/function/uvc_queue.h
index 8e76ce982f1e..03919c724961 100644
--- a/drivers/usb/gadget/function/uvc_queue.h
+++ b/drivers/usb/gadget/function/uvc_queue.h
@@ -57,6 +57,39 @@ static inline int uvc_queue_streaming(struct uvc_video_queue *queue)
 	return vb2_is_streaming(&queue->queue);
 }
 
+int uvcg_queue_init(struct uvc_video_queue *queue, enum v4l2_buf_type type);
+
+void uvcg_free_buffers(struct uvc_video_queue *queue);
+
+int uvcg_alloc_buffers(struct uvc_video_queue *queue,
+		       struct v4l2_requestbuffers *rb);
+
+int uvcg_query_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf);
+
+int uvcg_queue_buffer(struct uvc_video_queue *queue, struct v4l2_buffer *buf);
+
+int uvcg_dequeue_buffer(struct uvc_video_queue *queue,
+			struct v4l2_buffer *buf, int nonblocking);
+
+unsigned int uvcg_queue_poll(struct uvc_video_queue *queue,
+			     struct file *file, poll_table *wait);
+
+int uvcg_queue_mmap(struct uvc_video_queue *queue, struct vm_area_struct *vma);
+
+#ifndef CONFIG_MMU
+unsigned long uvcg_queue_get_unmapped_area(struct uvc_video_queue *queue,
+					   unsigned long pgoff);
+#endif /* CONFIG_MMU */
+
+void uvcg_queue_cancel(struct uvc_video_queue *queue, int disconnect);
+
+int uvcg_queue_enable(struct uvc_video_queue *queue, int enable);
+
+struct uvc_buffer *uvcg_queue_next_buffer(struct uvc_video_queue *queue,
+					  struct uvc_buffer *buf);
+
+struct uvc_buffer *uvcg_queue_head(struct uvc_video_queue *queue);
+
 #endif /* __KERNEL__ */
 
 #endif /* _UVC_QUEUE_H_ */
diff --git a/drivers/usb/gadget/function/uvc_v4l2.c b/drivers/usb/gadget/function/uvc_v4l2.c
index ad48e81155e2..5aad7fededa5 100644
--- a/drivers/usb/gadget/function/uvc_v4l2.c
+++ b/drivers/usb/gadget/function/uvc_v4l2.c
@@ -23,8 +23,10 @@
 #include <media/v4l2-event.h>
 #include <media/v4l2-ioctl.h>
 
+#include "f_uvc.h"
 #include "uvc.h"
 #include "uvc_queue.h"
+#include "uvc_video.h"
 
 /* --------------------------------------------------------------------------
  * Requests handling
@@ -48,7 +50,7 @@ uvc_send_response(struct uvc_device *uvc, struct uvc_request_data *data)
 }
 
 /* --------------------------------------------------------------------------
- * V4L2
+ * V4L2 ioctls
  */
 
 struct uvc_format
@@ -63,8 +65,29 @@ static struct uvc_format uvc_formats[] = {
 };
 
 static int
-uvc_v4l2_get_format(struct uvc_video *video, struct v4l2_format *fmt)
+uvc_v4l2_querycap(struct file *file, void *fh, struct v4l2_capability *cap)
 {
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct usb_composite_dev *cdev = uvc->func.config->cdev;
+
+	strlcpy(cap->driver, "g_uvc", sizeof(cap->driver));
+	strlcpy(cap->card, cdev->gadget->name, sizeof(cap->card));
+	strlcpy(cap->bus_info, dev_name(&cdev->gadget->dev),
+		sizeof(cap->bus_info));
+
+	cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
+
+	return 0;
+}
+
+static int
+uvc_v4l2_get_format(struct file *file, void *fh, struct v4l2_format *fmt)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_video *video = &uvc->video;
+
 	fmt->fmt.pix.pixelformat = video->fcc;
 	fmt->fmt.pix.width = video->width;
 	fmt->fmt.pix.height = video->height;
@@ -78,8 +101,11 @@ uvc_v4l2_get_format(struct uvc_video *video, struct v4l2_format *fmt)
 }
 
 static int
-uvc_v4l2_set_format(struct uvc_video *video, struct v4l2_format *fmt)
+uvc_v4l2_set_format(struct file *file, void *fh, struct v4l2_format *fmt)
 {
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_video *video = &uvc->video;
 	struct uvc_format *format;
 	unsigned int imagesize;
 	unsigned int bpl;
@@ -116,209 +142,184 @@ uvc_v4l2_set_format(struct uvc_video *video, struct v4l2_format *fmt)
 }
 
 static int
-uvc_v4l2_open(struct file *file)
+uvc_v4l2_reqbufs(struct file *file, void *fh, struct v4l2_requestbuffers *b)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
-	struct uvc_file_handle *handle;
-
-	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
-	if (handle == NULL)
-		return -ENOMEM;
-
-	v4l2_fh_init(&handle->vfh, vdev);
-	v4l2_fh_add(&handle->vfh);
+	struct uvc_video *video = &uvc->video;
 
-	handle->device = &uvc->video;
-	file->private_data = &handle->vfh;
+	if (b->type != video->queue.queue.type)
+		return -EINVAL;
 
-	uvc_function_connect(uvc);
-	return 0;
+	return uvcg_alloc_buffers(&video->queue, b);
 }
 
 static int
-uvc_v4l2_release(struct file *file)
+uvc_v4l2_querybuf(struct file *file, void *fh, struct v4l2_buffer *b)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
-	struct uvc_file_handle *handle = to_uvc_file_handle(file->private_data);
-	struct uvc_video *video = handle->device;
-
-	uvc_function_disconnect(uvc);
-
-	uvc_video_enable(video, 0);
-	uvc_free_buffers(&video->queue);
-
-	file->private_data = NULL;
-	v4l2_fh_del(&handle->vfh);
-	v4l2_fh_exit(&handle->vfh);
-	kfree(handle);
+	struct uvc_video *video = &uvc->video;
 
-	return 0;
+	return uvcg_query_buffer(&video->queue, b);
 }
 
-static long
-uvc_v4l2_do_ioctl(struct file *file, unsigned int cmd, void *arg)
+static int
+uvc_v4l2_qbuf(struct file *file, void *fh, struct v4l2_buffer *b)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
-	struct uvc_file_handle *handle = to_uvc_file_handle(file->private_data);
-	struct usb_composite_dev *cdev = uvc->func.config->cdev;
 	struct uvc_video *video = &uvc->video;
-	int ret = 0;
-
-	switch (cmd) {
-	/* Query capabilities */
-	case VIDIOC_QUERYCAP:
-	{
-		struct v4l2_capability *cap = arg;
-
-		memset(cap, 0, sizeof *cap);
-		strlcpy(cap->driver, "g_uvc", sizeof(cap->driver));
-		strlcpy(cap->card, cdev->gadget->name, sizeof(cap->card));
-		strlcpy(cap->bus_info, dev_name(&cdev->gadget->dev),
-			sizeof cap->bus_info);
-		cap->version = DRIVER_VERSION_NUMBER;
-		cap->capabilities = V4L2_CAP_VIDEO_OUTPUT | V4L2_CAP_STREAMING;
-		break;
-	}
-
-	/* Get & Set format */
-	case VIDIOC_G_FMT:
-	{
-		struct v4l2_format *fmt = arg;
+	int ret;
 
-		if (fmt->type != video->queue.queue.type)
-			return -EINVAL;
+	ret = uvcg_queue_buffer(&video->queue, b);
+	if (ret < 0)
+		return ret;
 
-		return uvc_v4l2_get_format(video, fmt);
-	}
+	return uvcg_video_pump(video);
+}
 
-	case VIDIOC_S_FMT:
-	{
-		struct v4l2_format *fmt = arg;
+static int
+uvc_v4l2_dqbuf(struct file *file, void *fh, struct v4l2_buffer *b)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_video *video = &uvc->video;
 
-		if (fmt->type != video->queue.queue.type)
-			return -EINVAL;
+	return uvcg_dequeue_buffer(&video->queue, b, file->f_flags & O_NONBLOCK);
+}
 
-		return uvc_v4l2_set_format(video, fmt);
-	}
+static int
+uvc_v4l2_streamon(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_video *video = &uvc->video;
+	int ret;
 
-	/* Buffers & streaming */
-	case VIDIOC_REQBUFS:
-	{
-		struct v4l2_requestbuffers *rb = arg;
+	if (type != video->queue.queue.type)
+		return -EINVAL;
 
-		if (rb->type != video->queue.queue.type)
-			return -EINVAL;
+	/* Enable UVC video. */
+	ret = uvcg_video_enable(video, 1);
+	if (ret < 0)
+		return ret;
 
-		ret = uvc_alloc_buffers(&video->queue, rb);
-		if (ret < 0)
-			return ret;
+	/*
+	 * Complete the alternate setting selection setup phase now that
+	 * userspace is ready to provide video frames.
+	 */
+	uvc_function_setup_continue(uvc);
+	uvc->state = UVC_STATE_STREAMING;
 
-		ret = 0;
-		break;
-	}
-
-	case VIDIOC_QUERYBUF:
-	{
-		struct v4l2_buffer *buf = arg;
+	return 0;
+}
 
-		return uvc_query_buffer(&video->queue, buf);
-	}
+static int
+uvc_v4l2_streamoff(struct file *file, void *fh, enum v4l2_buf_type type)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_video *video = &uvc->video;
 
-	case VIDIOC_QBUF:
-		if ((ret = uvc_queue_buffer(&video->queue, arg)) < 0)
-			return ret;
+	if (type != video->queue.queue.type)
+		return -EINVAL;
 
-		return uvc_video_pump(video);
+	return uvcg_video_enable(video, 0);
+}
 
-	case VIDIOC_DQBUF:
-		return uvc_dequeue_buffer(&video->queue, arg,
-			file->f_flags & O_NONBLOCK);
+static int
+uvc_v4l2_subscribe_event(struct v4l2_fh *fh,
+			 const struct v4l2_event_subscription *sub)
+{
+	if (sub->type < UVC_EVENT_FIRST || sub->type > UVC_EVENT_LAST)
+		return -EINVAL;
 
-	case VIDIOC_STREAMON:
-	{
-		int *type = arg;
+	return v4l2_event_subscribe(fh, sub, 2, NULL);
+}
 
-		if (*type != video->queue.queue.type)
-			return -EINVAL;
+static int
+uvc_v4l2_unsubscribe_event(struct v4l2_fh *fh,
+			   const struct v4l2_event_subscription *sub)
+{
+	return v4l2_event_unsubscribe(fh, sub);
+}
 
-		/* Enable UVC video. */
-		ret = uvc_video_enable(video, 1);
-		if (ret < 0)
-			return ret;
+static long
+uvc_v4l2_ioctl_default(struct file *file, void *fh, bool valid_prio,
+		       unsigned int cmd, void *arg)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
 
-		/*
-		 * Complete the alternate setting selection setup phase now that
-		 * userspace is ready to provide video frames.
-		 */
-		uvc_function_setup_continue(uvc);
-		uvc->state = UVC_STATE_STREAMING;
+	switch (cmd) {
+	case UVCIOC_SEND_RESPONSE:
+		return uvc_send_response(uvc, arg);
 
-		return 0;
+	default:
+		return -ENOIOCTLCMD;
 	}
+}
 
-	case VIDIOC_STREAMOFF:
-	{
-		int *type = arg;
-
-		if (*type != video->queue.queue.type)
-			return -EINVAL;
+const struct v4l2_ioctl_ops uvc_v4l2_ioctl_ops = {
+	.vidioc_querycap = uvc_v4l2_querycap,
+	.vidioc_g_fmt_vid_out = uvc_v4l2_get_format,
+	.vidioc_s_fmt_vid_out = uvc_v4l2_set_format,
+	.vidioc_reqbufs = uvc_v4l2_reqbufs,
+	.vidioc_querybuf = uvc_v4l2_querybuf,
+	.vidioc_qbuf = uvc_v4l2_qbuf,
+	.vidioc_dqbuf = uvc_v4l2_dqbuf,
+	.vidioc_streamon = uvc_v4l2_streamon,
+	.vidioc_streamoff = uvc_v4l2_streamoff,
+	.vidioc_subscribe_event = uvc_v4l2_subscribe_event,
+	.vidioc_unsubscribe_event = uvc_v4l2_unsubscribe_event,
+	.vidioc_default = uvc_v4l2_ioctl_default,
+};
 
-		return uvc_video_enable(video, 0);
-	}
+/* --------------------------------------------------------------------------
+ * V4L2
+ */
 
-	/* Events */
-	case VIDIOC_DQEVENT:
-	{
-		struct v4l2_event *event = arg;
-
-		ret = v4l2_event_dequeue(&handle->vfh, event,
-					 file->f_flags & O_NONBLOCK);
-		if (ret == 0 && event->type == UVC_EVENT_SETUP) {
-			struct uvc_event *uvc_event = (void *)&event->u.data;
-
-			/* Tell the complete callback to generate an event for
-			 * the next request that will be enqueued by
-			 * uvc_event_write.
-			 */
-			uvc->event_setup_out =
-				!(uvc_event->req.bRequestType & USB_DIR_IN);
-			uvc->event_length = uvc_event->req.wLength;
-		}
+static int
+uvc_v4l2_open(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_file_handle *handle;
 
-		return ret;
-	}
+	handle = kzalloc(sizeof(*handle), GFP_KERNEL);
+	if (handle == NULL)
+		return -ENOMEM;
 
-	case VIDIOC_SUBSCRIBE_EVENT:
-	{
-		struct v4l2_event_subscription *sub = arg;
+	v4l2_fh_init(&handle->vfh, vdev);
+	v4l2_fh_add(&handle->vfh);
 
-		if (sub->type < UVC_EVENT_FIRST || sub->type > UVC_EVENT_LAST)
-			return -EINVAL;
+	handle->device = &uvc->video;
+	file->private_data = &handle->vfh;
 
-		return v4l2_event_subscribe(&handle->vfh, arg, 2, NULL);
-	}
+	uvc_function_connect(uvc);
+	return 0;
+}
 
-	case VIDIOC_UNSUBSCRIBE_EVENT:
-		return v4l2_event_unsubscribe(&handle->vfh, arg);
+static int
+uvc_v4l2_release(struct file *file)
+{
+	struct video_device *vdev = video_devdata(file);
+	struct uvc_device *uvc = video_get_drvdata(vdev);
+	struct uvc_file_handle *handle = to_uvc_file_handle(file->private_data);
+	struct uvc_video *video = handle->device;
 
-	case UVCIOC_SEND_RESPONSE:
-		ret = uvc_send_response(uvc, arg);
-		break;
+	uvc_function_disconnect(uvc);
 
-	default:
-		return -ENOIOCTLCMD;
-	}
+	uvcg_video_enable(video, 0);
+	uvcg_free_buffers(&video->queue);
 
-	return ret;
-}
+	file->private_data = NULL;
+	v4l2_fh_del(&handle->vfh);
+	v4l2_fh_exit(&handle->vfh);
+	kfree(handle);
 
-static long
-uvc_v4l2_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
-{
-	return video_usercopy(file, cmd, arg, uvc_v4l2_do_ioctl);
+	return 0;
 }
 
 static int
@@ -327,7 +328,7 @@ uvc_v4l2_mmap(struct file *file, struct vm_area_struct *vma)
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
 
-	return uvc_queue_mmap(&uvc->video.queue, vma);
+	return uvcg_queue_mmap(&uvc->video.queue, vma);
 }
 
 static unsigned int
@@ -336,30 +337,30 @@ uvc_v4l2_poll(struct file *file, poll_table *wait)
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
 
-	return uvc_queue_poll(&uvc->video.queue, file, wait);
+	return uvcg_queue_poll(&uvc->video.queue, file, wait);
 }
 
 #ifndef CONFIG_MMU
-static unsigned long uvc_v4l2_get_unmapped_area(struct file *file,
+static unsigned long uvcg_v4l2_get_unmapped_area(struct file *file,
 		unsigned long addr, unsigned long len, unsigned long pgoff,
 		unsigned long flags)
 {
 	struct video_device *vdev = video_devdata(file);
 	struct uvc_device *uvc = video_get_drvdata(vdev);
 
-	return uvc_queue_get_unmapped_area(&uvc->video.queue, pgoff);
+	return uvcg_queue_get_unmapped_area(&uvc->video.queue, pgoff);
 }
 #endif
 
-static struct v4l2_file_operations uvc_v4l2_fops = {
+struct v4l2_file_operations uvc_v4l2_fops = {
 	.owner		= THIS_MODULE,
 	.open		= uvc_v4l2_open,
 	.release	= uvc_v4l2_release,
-	.ioctl		= uvc_v4l2_ioctl,
+	.ioctl		= video_ioctl2,
 	.mmap		= uvc_v4l2_mmap,
 	.poll		= uvc_v4l2_poll,
 #ifndef CONFIG_MMU
-	.get_unmapped_area = uvc_v4l2_get_unmapped_area,
+	.get_unmapped_area = uvcg_v4l2_get_unmapped_area,
 #endif
 };
 
diff --git a/drivers/usb/gadget/function/uvc_v4l2.h b/drivers/usb/gadget/function/uvc_v4l2.h
new file mode 100644
index 000000000000..2683b92fda65
--- /dev/null
+++ b/drivers/usb/gadget/function/uvc_v4l2.h
@@ -0,0 +1,22 @@
+/*
+ *	uvc_v4l2.h  --  USB Video Class Gadget driver
+ *
+ * Copyright (C) 2009-2010
+ *		Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *		Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef __UVC_V4L2_H__
+#define __UVC_V4L2_H__
+
+extern const struct v4l2_ioctl_ops uvc_v4l2_ioctl_ops;
+extern struct v4l2_file_operations uvc_v4l2_fops;
+
+#endif /* __UVC_V4L2_H__ */
diff --git a/drivers/usb/gadget/function/uvc_video.c b/drivers/usb/gadget/function/uvc_video.c
index 71e896d4c5ae..c3e1f27dbbef 100644
--- a/drivers/usb/gadget/function/uvc_video.c
+++ b/drivers/usb/gadget/function/uvc_video.c
@@ -15,6 +15,7 @@
 #include <linux/errno.h>
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb/video.h>
 
 #include <media/v4l2-dev.h>
 
@@ -85,7 +86,7 @@ uvc_video_encode_bulk(struct usb_request *req, struct uvc_video *video,
 	if (buf->bytesused == video->queue.buf_used) {
 		video->queue.buf_used = 0;
 		buf->state = UVC_BUF_STATE_DONE;
-		uvc_queue_next_buffer(&video->queue, buf);
+		uvcg_queue_next_buffer(&video->queue, buf);
 		video->fid ^= UVC_STREAM_FID;
 
 		video->payload_size = 0;
@@ -118,7 +119,7 @@ uvc_video_encode_isoc(struct usb_request *req, struct uvc_video *video,
 	if (buf->bytesused == video->queue.buf_used) {
 		video->queue.buf_used = 0;
 		buf->state = UVC_BUF_STATE_DONE;
-		uvc_queue_next_buffer(&video->queue, buf);
+		uvcg_queue_next_buffer(&video->queue, buf);
 		video->fid ^= UVC_STREAM_FID;
 	}
 }
@@ -171,19 +172,19 @@ uvc_video_complete(struct usb_ep *ep, struct usb_request *req)
 		break;
 
 	case -ESHUTDOWN:	/* disconnect from host. */
-		printk(KERN_INFO "VS request cancelled.\n");
-		uvc_queue_cancel(queue, 1);
+		printk(KERN_DEBUG "VS request cancelled.\n");
+		uvcg_queue_cancel(queue, 1);
 		goto requeue;
 
 	default:
 		printk(KERN_INFO "VS request completed with status %d.\n",
 			req->status);
-		uvc_queue_cancel(queue, 0);
+		uvcg_queue_cancel(queue, 0);
 		goto requeue;
 	}
 
 	spin_lock_irqsave(&video->queue.irqlock, flags);
-	buf = uvc_queue_head(&video->queue);
+	buf = uvcg_queue_head(&video->queue);
 	if (buf == NULL) {
 		spin_unlock_irqrestore(&video->queue.irqlock, flags);
 		goto requeue;
@@ -195,6 +196,7 @@ uvc_video_complete(struct usb_ep *ep, struct usb_request *req)
 		printk(KERN_INFO "Failed to queue request (%d).\n", ret);
 		usb_ep_set_halt(ep);
 		spin_unlock_irqrestore(&video->queue.irqlock, flags);
+		uvcg_queue_cancel(queue, 0);
 		goto requeue;
 	}
 	spin_unlock_irqrestore(&video->queue.irqlock, flags);
@@ -273,20 +275,20 @@ error:
  */
 
 /*
- * uvc_video_pump - Pump video data into the USB requests
+ * uvcg_video_pump - Pump video data into the USB requests
  *
  * This function fills the available USB requests (listed in req_free) with
  * video data from the queued buffers.
  */
-static int
-uvc_video_pump(struct uvc_video *video)
+int uvcg_video_pump(struct uvc_video *video)
 {
+	struct uvc_video_queue *queue = &video->queue;
 	struct usb_request *req;
 	struct uvc_buffer *buf;
 	unsigned long flags;
 	int ret;
 
-	/* FIXME TODO Race between uvc_video_pump and requests completion
+	/* FIXME TODO Race between uvcg_video_pump and requests completion
 	 * handler ???
 	 */
 
@@ -307,10 +309,10 @@ uvc_video_pump(struct uvc_video *video)
 		/* Retrieve the first available video buffer and fill the
 		 * request, protected by the video queue irqlock.
 		 */
-		spin_lock_irqsave(&video->queue.irqlock, flags);
-		buf = uvc_queue_head(&video->queue);
+		spin_lock_irqsave(&queue->irqlock, flags);
+		buf = uvcg_queue_head(queue);
 		if (buf == NULL) {
-			spin_unlock_irqrestore(&video->queue.irqlock, flags);
+			spin_unlock_irqrestore(&queue->irqlock, flags);
 			break;
 		}
 
@@ -321,10 +323,11 @@ uvc_video_pump(struct uvc_video *video)
 		if (ret < 0) {
 			printk(KERN_INFO "Failed to queue request (%d)\n", ret);
 			usb_ep_set_halt(video->ep);
-			spin_unlock_irqrestore(&video->queue.irqlock, flags);
+			spin_unlock_irqrestore(&queue->irqlock, flags);
+			uvcg_queue_cancel(queue, 0);
 			break;
 		}
-		spin_unlock_irqrestore(&video->queue.irqlock, flags);
+		spin_unlock_irqrestore(&queue->irqlock, flags);
 	}
 
 	spin_lock_irqsave(&video->req_lock, flags);
@@ -336,8 +339,7 @@ uvc_video_pump(struct uvc_video *video)
 /*
  * Enable or disable the video stream.
  */
-static int
-uvc_video_enable(struct uvc_video *video, int enable)
+int uvcg_video_enable(struct uvc_video *video, int enable)
 {
 	unsigned int i;
 	int ret;
@@ -353,11 +355,11 @@ uvc_video_enable(struct uvc_video *video, int enable)
 			usb_ep_dequeue(video->ep, video->req[i]);
 
 		uvc_video_free_requests(video);
-		uvc_queue_enable(&video->queue, 0);
+		uvcg_queue_enable(&video->queue, 0);
 		return 0;
 	}
 
-	if ((ret = uvc_queue_enable(&video->queue, 1)) < 0)
+	if ((ret = uvcg_queue_enable(&video->queue, 1)) < 0)
 		return ret;
 
 	if ((ret = uvc_video_alloc_requests(video)) < 0)
@@ -369,14 +371,13 @@ uvc_video_enable(struct uvc_video *video, int enable)
 	} else
 		video->encode = uvc_video_encode_isoc;
 
-	return uvc_video_pump(video);
+	return uvcg_video_pump(video);
 }
 
 /*
  * Initialize the UVC video stream.
  */
-static int
-uvc_video_init(struct uvc_video *video)
+int uvcg_video_init(struct uvc_video *video)
 {
 	INIT_LIST_HEAD(&video->req_free);
 	spin_lock_init(&video->req_lock);
@@ -388,7 +389,7 @@ uvc_video_init(struct uvc_video *video)
 	video->imagesize = 320 * 240 * 2;
 
 	/* Initialize the video buffers queue. */
-	uvc_queue_init(&video->queue, V4L2_BUF_TYPE_VIDEO_OUTPUT);
+	uvcg_queue_init(&video->queue, V4L2_BUF_TYPE_VIDEO_OUTPUT);
 	return 0;
 }
 
diff --git a/drivers/usb/gadget/function/uvc_video.h b/drivers/usb/gadget/function/uvc_video.h
new file mode 100644
index 000000000000..ef00f06fa00b
--- /dev/null
+++ b/drivers/usb/gadget/function/uvc_video.h
@@ -0,0 +1,24 @@
+/*
+ *	uvc_video.h  --  USB Video Class Gadget driver
+ *
+ * Copyright (C) 2009-2010
+ *		Laurent Pinchart (laurent.pinchart@ideasonboard.com)
+ *
+ * Copyright (c) 2013 Samsung Electronics Co., Ltd.
+ *		http://www.samsung.com
+ *		Author: Andrzej Pietrasiewicz <andrzej.p@samsung.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+#ifndef __UVC_VIDEO_H__
+#define __UVC_VIDEO_H__
+
+int uvcg_video_pump(struct uvc_video *video);
+
+int uvcg_video_enable(struct uvc_video *video, int enable);
+
+int uvcg_video_init(struct uvc_video *video);
+
+#endif /* __UVC_VIDEO_H__ */
diff --git a/drivers/usb/gadget/legacy/Kconfig b/drivers/usb/gadget/legacy/Kconfig
index aa376f006333..24392d269709 100644
--- a/drivers/usb/gadget/legacy/Kconfig
+++ b/drivers/usb/gadget/legacy/Kconfig
@@ -54,6 +54,8 @@ config USB_AUDIO
 	depends on SND
 	select USB_LIBCOMPOSITE
 	select SND_PCM
+	select USB_F_UAC1 if GADGET_UAC1
+	select USB_F_UAC2 if !GADGET_UAC1
 	help
 	  This Gadget Audio driver is compatible with USB Audio Class
 	  specification 2.0. It implements 1 AudioControl interface,
@@ -466,6 +468,7 @@ config USB_G_WEBCAM
 	depends on VIDEO_DEV
 	select USB_LIBCOMPOSITE
 	select VIDEOBUF2_VMALLOC
+	select USB_F_UVC
 	help
 	  The Webcam Gadget acts as a composite USB Audio and Video Class
 	  device. It provides a userspace API to process UVC control requests
diff --git a/drivers/usb/gadget/legacy/Makefile b/drivers/usb/gadget/legacy/Makefile
index a11aad5635df..7f485f25705e 100644
--- a/drivers/usb/gadget/legacy/Makefile
+++ b/drivers/usb/gadget/legacy/Makefile
@@ -2,9 +2,9 @@
 # USB gadget drivers
 #
 
-ccflags-y			:= -I$(PWD)/drivers/usb/gadget/
-ccflags-y			+= -I$(PWD)/drivers/usb/gadget/udc/
-ccflags-y			+= -I$(PWD)/drivers/usb/gadget/function/
+ccflags-y			:= -I$(srctree)/drivers/usb/gadget/
+ccflags-y			+= -I$(srctree)/drivers/usb/gadget/udc/
+ccflags-y			+= -I$(srctree)/drivers/usb/gadget/function/
 
 g_zero-y			:= zero.o
 g_audio-y			:= audio.o
diff --git a/drivers/usb/gadget/legacy/audio.c b/drivers/usb/gadget/legacy/audio.c
index 6eb695e5e43a..f46a3956e43d 100644
--- a/drivers/usb/gadget/legacy/audio.c
+++ b/drivers/usb/gadget/legacy/audio.c
@@ -21,6 +21,66 @@
 
 USB_GADGET_COMPOSITE_OPTIONS();
 
+#ifndef CONFIG_GADGET_UAC1
+#include "u_uac2.h"
+
+/* Playback(USB-IN) Default Stereo - Fl/Fr */
+static int p_chmask = UAC2_DEF_PCHMASK;
+module_param(p_chmask, uint, S_IRUGO);
+MODULE_PARM_DESC(p_chmask, "Playback Channel Mask");
+
+/* Playback Default 48 KHz */
+static int p_srate = UAC2_DEF_PSRATE;
+module_param(p_srate, uint, S_IRUGO);
+MODULE_PARM_DESC(p_srate, "Playback Sampling Rate");
+
+/* Playback Default 16bits/sample */
+static int p_ssize = UAC2_DEF_PSSIZE;
+module_param(p_ssize, uint, S_IRUGO);
+MODULE_PARM_DESC(p_ssize, "Playback Sample Size(bytes)");
+
+/* Capture(USB-OUT) Default Stereo - Fl/Fr */
+static int c_chmask = UAC2_DEF_CCHMASK;
+module_param(c_chmask, uint, S_IRUGO);
+MODULE_PARM_DESC(c_chmask, "Capture Channel Mask");
+
+/* Capture Default 64 KHz */
+static int c_srate = UAC2_DEF_CSRATE;
+module_param(c_srate, uint, S_IRUGO);
+MODULE_PARM_DESC(c_srate, "Capture Sampling Rate");
+
+/* Capture Default 16bits/sample */
+static int c_ssize = UAC2_DEF_CSSIZE;
+module_param(c_ssize, uint, S_IRUGO);
+MODULE_PARM_DESC(c_ssize, "Capture Sample Size(bytes)");
+#else
+#include "u_uac1.h"
+
+static char *fn_play = FILE_PCM_PLAYBACK;
+module_param(fn_play, charp, S_IRUGO);
+MODULE_PARM_DESC(fn_play, "Playback PCM device file name");
+
+static char *fn_cap = FILE_PCM_CAPTURE;
+module_param(fn_cap, charp, S_IRUGO);
+MODULE_PARM_DESC(fn_cap, "Capture PCM device file name");
+
+static char *fn_cntl = FILE_CONTROL;
+module_param(fn_cntl, charp, S_IRUGO);
+MODULE_PARM_DESC(fn_cntl, "Control device file name");
+
+static int req_buf_size = UAC1_OUT_EP_MAX_PACKET_SIZE;
+module_param(req_buf_size, int, S_IRUGO);
+MODULE_PARM_DESC(req_buf_size, "ISO OUT endpoint request buffer size");
+
+static int req_count = UAC1_REQ_COUNT;
+module_param(req_count, int, S_IRUGO);
+MODULE_PARM_DESC(req_count, "ISO OUT endpoint request count");
+
+static int audio_buf_size = UAC1_AUDIO_BUF_SIZE;
+module_param(audio_buf_size, int, S_IRUGO);
+MODULE_PARM_DESC(audio_buf_size, "Audio buffer size");
+#endif
+
 /* string IDs are assigned dynamically */
 
 static struct usb_string strings_dev[] = {
@@ -40,12 +100,12 @@ static struct usb_gadget_strings *audio_strings[] = {
 	NULL,
 };
 
-#ifdef CONFIG_GADGET_UAC1
-#include "u_uac1.h"
-#include "u_uac1.c"
-#include "f_uac1.c"
+#ifndef CONFIG_GADGET_UAC1
+static struct usb_function_instance *fi_uac2;
+static struct usb_function *f_uac2;
 #else
-#include "f_uac2.c"
+static struct usb_function_instance *fi_uac1;
+static struct usb_function *f_uac1;
 #endif
 
 /*-------------------------------------------------------------------------*/
@@ -109,6 +169,8 @@ static const struct usb_descriptor_header *otg_desc[] = {
 
 static int __init audio_do_config(struct usb_configuration *c)
 {
+	int status;
+
 	/* FIXME alloc iConfiguration string, set it in c->strings */
 
 	if (gadget_is_otg(c->cdev->gadget)) {
@@ -116,7 +178,31 @@ static int __init audio_do_config(struct usb_configuration *c)
 		c->bmAttributes |= USB_CONFIG_ATT_WAKEUP;
 	}
 
-	audio_bind_config(c);
+#ifdef CONFIG_GADGET_UAC1
+	f_uac1 = usb_get_function(fi_uac1);
+	if (IS_ERR(f_uac1)) {
+		status = PTR_ERR(f_uac1);
+		return status;
+	}
+
+	status = usb_add_function(c, f_uac1);
+	if (status < 0) {
+		usb_put_function(f_uac1);
+		return status;
+	}
+#else
+	f_uac2 = usb_get_function(fi_uac2);
+	if (IS_ERR(f_uac2)) {
+		status = PTR_ERR(f_uac2);
+		return status;
+	}
+
+	status = usb_add_function(c, f_uac2);
+	if (status < 0) {
+		usb_put_function(f_uac2);
+		return status;
+	}
+#endif
 
 	return 0;
 }
@@ -126,17 +212,47 @@ static struct usb_configuration audio_config_driver = {
 	.bConfigurationValue	= 1,
 	/* .iConfiguration = DYNAMIC */
 	.bmAttributes		= USB_CONFIG_ATT_SELFPOWER,
-#ifndef CONFIG_GADGET_UAC1
-	.unbind			= uac2_unbind_config,
-#endif
 };
 
 /*-------------------------------------------------------------------------*/
 
 static int __init audio_bind(struct usb_composite_dev *cdev)
 {
+#ifndef CONFIG_GADGET_UAC1
+	struct f_uac2_opts	*uac2_opts;
+#else
+	struct f_uac1_opts	*uac1_opts;
+#endif
 	int			status;
 
+#ifndef CONFIG_GADGET_UAC1
+	fi_uac2 = usb_get_function_instance("uac2");
+	if (IS_ERR(fi_uac2))
+		return PTR_ERR(fi_uac2);
+#else
+	fi_uac1 = usb_get_function_instance("uac1");
+	if (IS_ERR(fi_uac1))
+		return PTR_ERR(fi_uac1);
+#endif
+
+#ifndef CONFIG_GADGET_UAC1
+	uac2_opts = container_of(fi_uac2, struct f_uac2_opts, func_inst);
+	uac2_opts->p_chmask = p_chmask;
+	uac2_opts->p_srate = p_srate;
+	uac2_opts->p_ssize = p_ssize;
+	uac2_opts->c_chmask = c_chmask;
+	uac2_opts->c_srate = c_srate;
+	uac2_opts->c_ssize = c_ssize;
+#else
+	uac1_opts = container_of(fi_uac1, struct f_uac1_opts, func_inst);
+	uac1_opts->fn_play = fn_play;
+	uac1_opts->fn_cap = fn_cap;
+	uac1_opts->fn_cntl = fn_cntl;
+	uac1_opts->req_buf_size = req_buf_size;
+	uac1_opts->req_count = req_count;
+	uac1_opts->audio_buf_size = audio_buf_size;
+#endif
+
 	status = usb_string_ids_tab(cdev, strings_dev);
 	if (status < 0)
 		goto fail;
@@ -152,13 +268,26 @@ static int __init audio_bind(struct usb_composite_dev *cdev)
 	return 0;
 
 fail:
+#ifndef CONFIG_GADGET_UAC1
+	usb_put_function_instance(fi_uac2);
+#else
+	usb_put_function_instance(fi_uac1);
+#endif
 	return status;
 }
 
 static int __exit audio_unbind(struct usb_composite_dev *cdev)
 {
 #ifdef CONFIG_GADGET_UAC1
-	gaudio_cleanup();
+	if (!IS_ERR_OR_NULL(f_uac1))
+		usb_put_function(f_uac1);
+	if (!IS_ERR_OR_NULL(fi_uac1))
+		usb_put_function_instance(fi_uac1);
+#else
+	if (!IS_ERR_OR_NULL(f_uac2))
+		usb_put_function(f_uac2);
+	if (!IS_ERR_OR_NULL(fi_uac2))
+		usb_put_function_instance(fi_uac2);
 #endif
 	return 0;
 }
diff --git a/drivers/usb/gadget/legacy/dbgp.c b/drivers/usb/gadget/legacy/dbgp.c
index 986fc511a2ed..1b075132f8f1 100644
--- a/drivers/usb/gadget/legacy/dbgp.c
+++ b/drivers/usb/gadget/legacy/dbgp.c
@@ -222,10 +222,12 @@ static void dbgp_unbind(struct usb_gadget *gadget)
 {
 #ifdef CONFIG_USB_G_DBGP_SERIAL
 	kfree(dbgp.serial);
+	dbgp.serial = NULL;
 #endif
 	if (dbgp.req) {
 		kfree(dbgp.req->buf);
 		usb_ep_free_request(gadget->ep0, dbgp.req);
+		dbgp.req = NULL;
 	}
 
 	gadget->ep0->driver_data = NULL;
@@ -408,6 +410,7 @@ static __refdata struct usb_gadget_driver dbgp_driver = {
 	.bind = dbgp_bind,
 	.unbind = dbgp_unbind,
 	.setup = dbgp_setup,
+	.reset = dbgp_disconnect,
 	.disconnect = dbgp_disconnect,
 	.driver	= {
 		.owner = THIS_MODULE,
diff --git a/drivers/usb/gadget/legacy/inode.c b/drivers/usb/gadget/legacy/inode.c
index 2e4ce7704908..c744e4975d74 100644
--- a/drivers/usb/gadget/legacy/inode.c
+++ b/drivers/usb/gadget/legacy/inode.c
@@ -198,7 +198,6 @@ struct ep_data {
 	struct list_head		epfiles;
 	wait_queue_head_t		wait;
 	struct dentry			*dentry;
-	struct inode			*inode;
 };
 
 static inline void get_ep (struct ep_data *data)
@@ -440,7 +439,7 @@ ep_write (struct file *fd, const char __user *buf, size_t len, loff_t *ptr)
 
 	value = -ENOMEM;
 	kbuf = memdup_user(buf, len);
-	if (!kbuf) {
+	if (IS_ERR(kbuf)) {
 		value = PTR_ERR(kbuf);
 		goto free1;
 	}
@@ -1618,10 +1617,9 @@ static void destroy_ep_files (struct dev_data *dev)
 }
 
 
-static struct inode *
+static struct dentry *
 gadgetfs_create_file (struct super_block *sb, char const *name,
-		void *data, const struct file_operations *fops,
-		struct dentry **dentry_p);
+		void *data, const struct file_operations *fops);
 
 static int activate_ep_files (struct dev_data *dev)
 {
@@ -1649,10 +1647,9 @@ static int activate_ep_files (struct dev_data *dev)
 		if (!data->req)
 			goto enomem1;
 
-		data->inode = gadgetfs_create_file (dev->sb, data->name,
-				data, &ep_config_operations,
-				&data->dentry);
-		if (!data->inode)
+		data->dentry = gadgetfs_create_file (dev->sb, data->name,
+				data, &ep_config_operations);
+		if (!data->dentry)
 			goto enomem2;
 		list_add_tail (&data->epfiles, &dev->epfiles);
 	}
@@ -1775,6 +1772,7 @@ static struct usb_gadget_driver gadgetfs_driver = {
 	.bind		= gadgetfs_bind,
 	.unbind		= gadgetfs_unbind,
 	.setup		= gadgetfs_setup,
+	.reset		= gadgetfs_disconnect,
 	.disconnect	= gadgetfs_disconnect,
 	.suspend	= gadgetfs_suspend,
 
@@ -2011,10 +2009,9 @@ gadgetfs_make_inode (struct super_block *sb,
 /* creates in fs root directory, so non-renamable and non-linkable.
  * so inode and dentry are paired, until device reconfig.
  */
-static struct inode *
+static struct dentry *
 gadgetfs_create_file (struct super_block *sb, char const *name,
-		void *data, const struct file_operations *fops,
-		struct dentry **dentry_p)
+		void *data, const struct file_operations *fops)
 {
 	struct dentry	*dentry;
 	struct inode	*inode;
@@ -2030,8 +2027,7 @@ gadgetfs_create_file (struct super_block *sb, char const *name,
 		return NULL;
 	}
 	d_add (dentry, inode);
-	*dentry_p = dentry;
-	return inode;
+	return dentry;
 }
 
 static const struct super_operations gadget_fs_operations = {
@@ -2079,9 +2075,8 @@ gadgetfs_fill_super (struct super_block *sb, void *opts, int silent)
 		goto Enomem;
 
 	dev->sb = sb;
-	if (!gadgetfs_create_file (sb, CHIP,
-				dev, &dev_init_operations,
-				&dev->dentry)) {
+	dev->dentry = gadgetfs_create_file(sb, CHIP, dev, &dev_init_operations);
+	if (!dev->dentry) {
 		put_dev(dev);
 		goto Enomem;
 	}
diff --git a/drivers/usb/gadget/legacy/webcam.c b/drivers/usb/gadget/legacy/webcam.c
index a11d8e420bfe..04a3da20f742 100644
--- a/drivers/usb/gadget/legacy/webcam.c
+++ b/drivers/usb/gadget/legacy/webcam.c
@@ -12,23 +12,31 @@
 
 #include <linux/kernel.h>
 #include <linux/device.h>
+#include <linux/module.h>
 #include <linux/usb/video.h>
 
-#include "f_uvc.h"
-
-/*
- * Kbuild is not very cooperative with respect to linking separately
- * compiled library objects into one module.  So for now we won't use
- * separate compilation ... ensuring init/exit sections work to shrink
- * the runtime footprint, and giving us at least some parts of what
- * a "gcc --combine ... part1.c part2.c part3.c ... " build would.
- */
-#include "uvc_queue.c"
-#include "uvc_video.c"
-#include "uvc_v4l2.c"
-#include "f_uvc.c"
+#include "u_uvc.h"
 
 USB_GADGET_COMPOSITE_OPTIONS();
+
+/*-------------------------------------------------------------------------*/
+
+/* module parameters specific to the Video streaming endpoint */
+static unsigned int streaming_interval = 1;
+module_param(streaming_interval, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(streaming_interval, "1 - 16");
+
+static unsigned int streaming_maxpacket = 1024;
+module_param(streaming_maxpacket, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(streaming_maxpacket, "1 - 1023 (FS), 1 - 3072 (hs/ss)");
+
+static unsigned int streaming_maxburst;
+module_param(streaming_maxburst, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(streaming_maxburst, "0 - 15 (ss only)");
+
+static unsigned int trace;
+module_param(trace, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(trace, "Trace level bitmask");
 /* --------------------------------------------------------------------------
  * Device descriptor
  */
@@ -63,6 +71,9 @@ static struct usb_gadget_strings *webcam_device_strings[] = {
 	NULL,
 };
 
+static struct usb_function_instance *fi_uvc;
+static struct usb_function *f_uvc;
+
 static struct usb_device_descriptor webcam_device_descriptor = {
 	.bLength		= USB_DT_DEVICE_SIZE,
 	.bDescriptorType	= USB_DT_DEVICE,
@@ -326,9 +337,17 @@ static const struct uvc_descriptor_header * const uvc_ss_streaming_cls[] = {
 static int __init
 webcam_config_bind(struct usb_configuration *c)
 {
-	return uvc_bind_config(c, uvc_fs_control_cls, uvc_ss_control_cls,
-		uvc_fs_streaming_cls, uvc_hs_streaming_cls,
-		uvc_ss_streaming_cls);
+	int status = 0;
+
+	f_uvc = usb_get_function(fi_uvc);
+	if (IS_ERR(f_uvc))
+		return PTR_ERR(f_uvc);
+
+	status = usb_add_function(c, f_uvc);
+	if (status < 0)
+		usb_put_function(f_uvc);
+
+	return status;
 }
 
 static struct usb_configuration webcam_config_driver = {
@@ -342,14 +361,36 @@ static struct usb_configuration webcam_config_driver = {
 static int /* __init_or_exit */
 webcam_unbind(struct usb_composite_dev *cdev)
 {
+	if (!IS_ERR_OR_NULL(f_uvc))
+		usb_put_function(f_uvc);
+	if (!IS_ERR_OR_NULL(fi_uvc))
+		usb_put_function_instance(fi_uvc);
 	return 0;
 }
 
 static int __init
 webcam_bind(struct usb_composite_dev *cdev)
 {
+	struct f_uvc_opts *uvc_opts;
 	int ret;
 
+	fi_uvc = usb_get_function_instance("uvc");
+	if (IS_ERR(fi_uvc))
+		return PTR_ERR(fi_uvc);
+
+	uvc_opts = container_of(fi_uvc, struct f_uvc_opts, func_inst);
+
+	uvc_opts->streaming_interval = streaming_interval;
+	uvc_opts->streaming_maxpacket = streaming_maxpacket;
+	uvc_opts->streaming_maxburst = streaming_maxburst;
+	uvc_set_trace_param(trace);
+
+	uvc_opts->fs_control = uvc_fs_control_cls;
+	uvc_opts->ss_control = uvc_ss_control_cls;
+	uvc_opts->fs_streaming = uvc_fs_streaming_cls;
+	uvc_opts->hs_streaming = uvc_hs_streaming_cls;
+	uvc_opts->ss_streaming = uvc_ss_streaming_cls;
+
 	/* Allocate string descriptor numbers ... note that string contents
 	 * can be overridden by the composite_dev glue.
 	 */
@@ -373,7 +414,7 @@ webcam_bind(struct usb_composite_dev *cdev)
 	return 0;
 
 error:
-	webcam_unbind(cdev);
+	usb_put_function_instance(fi_uvc);
 	return ret;
 }
 
diff --git a/drivers/usb/gadget/legacy/zero.c b/drivers/usb/gadget/legacy/zero.c
index c3d496828b74..ebf09f439f3a 100644
--- a/drivers/usb/gadget/legacy/zero.c
+++ b/drivers/usb/gadget/legacy/zero.c
@@ -68,6 +68,8 @@ static struct usb_zero_options gzero_options = {
 	.isoc_maxpacket = GZERO_ISOC_MAXPACKET,
 	.bulk_buflen = GZERO_BULK_BUFLEN,
 	.qlen = GZERO_QLEN,
+	.int_interval = GZERO_INT_INTERVAL,
+	.int_maxpacket = GZERO_INT_MAXPACKET,
 };
 
 /*-------------------------------------------------------------------------*/
@@ -266,6 +268,21 @@ module_param_named(isoc_maxburst, gzero_options.isoc_maxburst, uint,
 		S_IRUGO|S_IWUSR);
 MODULE_PARM_DESC(isoc_maxburst, "0 - 15 (ss only)");
 
+module_param_named(int_interval, gzero_options.int_interval, uint,
+		S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(int_interval, "1 - 16");
+
+module_param_named(int_maxpacket, gzero_options.int_maxpacket, uint,
+		S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(int_maxpacket, "0 - 1023 (fs), 0 - 1024 (hs/ss)");
+
+module_param_named(int_mult, gzero_options.int_mult, uint, S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(int_mult, "0 - 2 (hs/ss only)");
+
+module_param_named(int_maxburst, gzero_options.int_maxburst, uint,
+		S_IRUGO|S_IWUSR);
+MODULE_PARM_DESC(int_maxburst, "0 - 15 (ss only)");
+
 static struct usb_function *func_lb;
 static struct usb_function_instance *func_inst_lb;
 
@@ -301,6 +318,10 @@ static int __init zero_bind(struct usb_composite_dev *cdev)
 	ss_opts->isoc_maxpacket = gzero_options.isoc_maxpacket;
 	ss_opts->isoc_mult = gzero_options.isoc_mult;
 	ss_opts->isoc_maxburst = gzero_options.isoc_maxburst;
+	ss_opts->int_interval = gzero_options.int_interval;
+	ss_opts->int_maxpacket = gzero_options.int_maxpacket;
+	ss_opts->int_mult = gzero_options.int_mult;
+	ss_opts->int_maxburst = gzero_options.int_maxburst;
 	ss_opts->bulk_buflen = gzero_options.bulk_buflen;
 
 	func_ss = usb_get_function(func_inst_ss);
diff --git a/drivers/usb/gadget/udc/Kconfig b/drivers/usb/gadget/udc/Kconfig
index 5151f947a4f5..3ea287b0e448 100644
--- a/drivers/usb/gadget/udc/Kconfig
+++ b/drivers/usb/gadget/udc/Kconfig
@@ -163,7 +163,7 @@ config USB_R8A66597
 
 config USB_RENESAS_USBHS_UDC
 	tristate 'Renesas USBHS controller'
-	depends on USB_RENESAS_USBHS
+	depends on USB_RENESAS_USBHS && HAS_DMA
 	help
 	   Renesas USBHS is a discrete USB host and peripheral controller chip
 	   that supports both full and high speed USB 2.0 data transfers.
@@ -332,7 +332,7 @@ config USB_GOKU
 	   gadget drivers to also be dynamically linked.
 
 config USB_EG20T
-	tristate "Intel EG20T PCH/LAPIS Semiconductor IOH(ML7213/ML7831) UDC"
+	tristate "Intel QUARK X1000/EG20T PCH/LAPIS Semiconductor IOH(ML7213/ML7831) UDC"
 	depends on PCI
 	help
 	  This is a USB device driver for EG20T PCH.
@@ -353,6 +353,22 @@ config USB_EG20T
 	  ML7213/ML7831 is companion chip for Intel Atom E6xx series.
 	  ML7213/ML7831 is completely compatible for Intel EG20T PCH.
 
+	  This driver can be used with Intel's Quark X1000 SOC platform
+
+config USB_GADGET_XILINX
+	tristate "Xilinx USB Driver"
+	depends on OF || COMPILE_TEST
+	help
+	  USB peripheral controller driver for Xilinx USB2 device.
+	  Xilinx USB2 device is a soft IP which supports both full
+	  and high speed USB 2.0 data transfers. It has seven configurable
+	  endpoints(bulk or interrupt or isochronous), as well as
+	  endpoint zero(for control transfers).
+
+	  Say "y" to link the driver statically, or "m" to build a
+	  dynamically linked module called "udc-xilinx" and force all
+	  gadget drivers to also be dynamically linked.
+
 #
 # LAST -- dummy/emulated controller
 #
diff --git a/drivers/usb/gadget/udc/Makefile b/drivers/usb/gadget/udc/Makefile
index 4096122bb283..a7f4491593f1 100644
--- a/drivers/usb/gadget/udc/Makefile
+++ b/drivers/usb/gadget/udc/Makefile
@@ -29,3 +29,4 @@ obj-$(CONFIG_USB_FUSB300)	+= fusb300_udc.o
 obj-$(CONFIG_USB_FOTG210_UDC)	+= fotg210-udc.o
 obj-$(CONFIG_USB_MV_U3D)	+= mv_u3d_core.o
 obj-$(CONFIG_USB_GR_UDC)	+= gr_udc.o
+obj-$(CONFIG_USB_GADGET_XILINX)	+= udc-xilinx.o
diff --git a/drivers/usb/gadget/udc/amd5536udc.c b/drivers/usb/gadget/udc/amd5536udc.c
index 41b062eb4de0..3b9d13848a4f 100644
--- a/drivers/usb/gadget/udc/amd5536udc.c
+++ b/drivers/usb/gadget/udc/amd5536udc.c
@@ -841,7 +841,7 @@ __acquires(ep->dev->lock)
 		&req->req, req->req.length, ep->ep.name, sts);
 
 	spin_unlock(&dev->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&dev->lock);
 	ep->halted = halted;
 }
diff --git a/drivers/usb/gadget/udc/at91_udc.c b/drivers/usb/gadget/udc/at91_udc.c
index cfd18bcca723..9968f5331fe4 100644
--- a/drivers/usb/gadget/udc/at91_udc.c
+++ b/drivers/usb/gadget/udc/at91_udc.c
@@ -267,7 +267,7 @@ static void done(struct at91_ep *ep, struct at91_request *req, int status)
 
 	ep->stopped = 1;
 	spin_unlock(&udc->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&udc->lock);
 	ep->stopped = stopped;
 
diff --git a/drivers/usb/gadget/udc/atmel_usba_udc.c b/drivers/usb/gadget/udc/atmel_usba_udc.c
index 906e65f0e4fa..1529926e20a0 100644
--- a/drivers/usb/gadget/udc/atmel_usba_udc.c
+++ b/drivers/usb/gadget/udc/atmel_usba_udc.c
@@ -463,7 +463,7 @@ static void receive_data(struct usba_ep *ep)
 			list_del_init(&req->queue);
 			usba_ep_writel(ep, CTL_DIS, USBA_RX_BK_RDY);
 			spin_unlock(&udc->lock);
-			req->req.complete(&ep->ep, &req->req);
+			usb_gadget_giveback_request(&ep->ep, &req->req);
 			spin_lock(&udc->lock);
 		}
 
@@ -495,7 +495,7 @@ request_complete(struct usba_ep *ep, struct usba_request *req, int status)
 		ep->ep.name, req, req->req.status, req->req.actual);
 
 	spin_unlock(&udc->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&udc->lock);
 }
 
@@ -1661,7 +1661,7 @@ static irqreturn_t usba_udc_irq(int irq, void *devid)
 	if (dma_status) {
 		int i;
 
-		for (i = 1; i < USBA_NR_DMAS; i++)
+		for (i = 1; i <= USBA_NR_DMAS; i++)
 			if (dma_status & (1 << i))
 				usba_dma_irq(udc, &udc->usba_ep[i]);
 	}
diff --git a/drivers/usb/gadget/udc/bcm63xx_udc.c b/drivers/usb/gadget/udc/bcm63xx_udc.c
index e969eb809a85..2235b8808700 100644
--- a/drivers/usb/gadget/udc/bcm63xx_udc.c
+++ b/drivers/usb/gadget/udc/bcm63xx_udc.c
@@ -1088,7 +1088,7 @@ static int bcm63xx_ep_disable(struct usb_ep *ep)
 			breq->req.status = -ESHUTDOWN;
 
 			spin_unlock_irqrestore(&udc->lock, flags);
-			breq->req.complete(&iudma->bep->ep, &breq->req);
+			usb_gadget_giveback_request(&iudma->bep->ep, &breq->req);
 			spin_lock_irqsave(&udc->lock, flags);
 		}
 	}
diff --git a/drivers/usb/gadget/udc/dummy_hcd.c b/drivers/usb/gadget/udc/dummy_hcd.c
index 2b54955d3166..81dc5959e36b 100644
--- a/drivers/usb/gadget/udc/dummy_hcd.c
+++ b/drivers/usb/gadget/udc/dummy_hcd.c
@@ -258,7 +258,7 @@ static void nuke(struct dummy *dum, struct dummy_ep *ep)
 		req->req.status = -ESHUTDOWN;
 
 		spin_unlock(&dum->lock);
-		req->req.complete(&ep->ep, &req->req);
+		usb_gadget_giveback_request(&ep->ep, &req->req);
 		spin_lock(&dum->lock);
 	}
 }
@@ -658,7 +658,7 @@ static int dummy_queue(struct usb_ep *_ep, struct usb_request *_req,
 		spin_unlock(&dum->lock);
 		_req->actual = _req->length;
 		_req->status = 0;
-		_req->complete(_ep, _req);
+		usb_gadget_giveback_request(_ep, _req);
 		spin_lock(&dum->lock);
 	}  else
 		list_add_tail(&req->queue, &ep->queue);
@@ -702,7 +702,7 @@ static int dummy_dequeue(struct usb_ep *_ep, struct usb_request *_req)
 		dev_dbg(udc_dev(dum),
 				"dequeued req %p from %s, len %d buf %p\n",
 				req, _ep->name, _req->length, _req->buf);
-		_req->complete(_ep, _req);
+		usb_gadget_giveback_request(_ep, _req);
 	}
 	local_irq_restore(flags);
 	return retval;
@@ -1385,7 +1385,7 @@ top:
 			list_del_init(&req->queue);
 
 			spin_unlock(&dum->lock);
-			req->req.complete(&ep->ep, &req->req);
+			usb_gadget_giveback_request(&ep->ep, &req->req);
 			spin_lock(&dum->lock);
 
 			/* requests might have been unlinked... */
@@ -1761,7 +1761,7 @@ restart:
 						req);
 
 				spin_unlock(&dum->lock);
-				req->req.complete(&ep->ep, &req->req);
+				usb_gadget_giveback_request(&ep->ep, &req->req);
 				spin_lock(&dum->lock);
 				ep->already_seen = 0;
 				goto restart;
diff --git a/drivers/usb/gadget/udc/fotg210-udc.c b/drivers/usb/gadget/udc/fotg210-udc.c
index e143d69f6017..1d315921bf34 100644
--- a/drivers/usb/gadget/udc/fotg210-udc.c
+++ b/drivers/usb/gadget/udc/fotg210-udc.c
@@ -70,7 +70,7 @@ static void fotg210_done(struct fotg210_ep *ep, struct fotg210_request *req,
 		req->req.status = status;
 
 	spin_unlock(&ep->fotg210->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&ep->fotg210->lock);
 
 	if (ep->epnum) {
diff --git a/drivers/usb/gadget/udc/fsl_qe_udc.c b/drivers/usb/gadget/udc/fsl_qe_udc.c
index 732430804841..dd18ea38e391 100644
--- a/drivers/usb/gadget/udc/fsl_qe_udc.c
+++ b/drivers/usb/gadget/udc/fsl_qe_udc.c
@@ -118,10 +118,7 @@ static void done(struct qe_ep *ep, struct qe_req *req, int status)
 	ep->stopped = 1;
 	spin_unlock(&udc->lock);
 
-	/* this complete() should a func implemented by gadget layer,
-	 * eg fsg->bulk_in_complete() */
-	if (req->req.complete)
-		req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 
 	spin_lock(&udc->lock);
 
@@ -2728,4 +2725,3 @@ module_platform_driver(udc_driver);
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR(DRIVER_AUTHOR);
 MODULE_LICENSE("GPL");
-
diff --git a/drivers/usb/gadget/udc/fsl_udc_core.c b/drivers/usb/gadget/udc/fsl_udc_core.c
index 75b23ea077a7..c3620791a315 100644
--- a/drivers/usb/gadget/udc/fsl_udc_core.c
+++ b/drivers/usb/gadget/udc/fsl_udc_core.c
@@ -197,10 +197,8 @@ __acquires(ep->udc->lock)
 	ep->stopped = 1;
 
 	spin_unlock(&ep->udc->lock);
-	/* complete() is from gadget layer,
-	 * eg fsg->bulk_in_complete() */
-	if (req->req.complete)
-		req->req.complete(&ep->ep, &req->req);
+
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 
 	spin_lock(&ep->udc->lock);
 	ep->stopped = stopped;
diff --git a/drivers/usb/gadget/udc/fusb300_udc.c b/drivers/usb/gadget/udc/fusb300_udc.c
index d40255f784df..8286df72add4 100644
--- a/drivers/usb/gadget/udc/fusb300_udc.c
+++ b/drivers/usb/gadget/udc/fusb300_udc.c
@@ -876,7 +876,7 @@ static void done(struct fusb300_ep *ep, struct fusb300_request *req,
 		req->req.status = status;
 
 	spin_unlock(&ep->fusb300->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&ep->fusb300->lock);
 
 	if (ep->epnum) {
@@ -1398,13 +1398,17 @@ static int fusb300_probe(struct platform_device *pdev)
 
 	/* initialize udc */
 	fusb300 = kzalloc(sizeof(struct fusb300), GFP_KERNEL);
-	if (fusb300 == NULL)
+	if (fusb300 == NULL) {
+		ret = -ENOMEM;
 		goto clean_up;
+	}
 
 	for (i = 0; i < FUSB300_MAX_NUM_EP; i++) {
 		_ep[i] = kzalloc(sizeof(struct fusb300_ep), GFP_KERNEL);
-		if (_ep[i] == NULL)
+		if (_ep[i] == NULL) {
+			ret = -ENOMEM;
 			goto clean_up;
+		}
 		fusb300->ep[i] = _ep[i];
 	}
 
diff --git a/drivers/usb/gadget/udc/fusb300_udc.h b/drivers/usb/gadget/udc/fusb300_udc.h
index ae811d8d38b4..ad39f892d200 100644
--- a/drivers/usb/gadget/udc/fusb300_udc.h
+++ b/drivers/usb/gadget/udc/fusb300_udc.h
@@ -12,7 +12,7 @@
 
 
 #ifndef __FUSB300_UDC_H__
-#define __FUSB300_UDC_H_
+#define __FUSB300_UDC_H__
 
 #include <linux/kernel.h>
 
diff --git a/drivers/usb/gadget/udc/goku_udc.c b/drivers/usb/gadget/udc/goku_udc.c
index 6c85839e15ad..bf9c5ef8b56b 100644
--- a/drivers/usb/gadget/udc/goku_udc.c
+++ b/drivers/usb/gadget/udc/goku_udc.c
@@ -320,7 +320,7 @@ done(struct goku_ep *ep, struct goku_request *req, int status)
 	/* don't modify queue heads during completion callback */
 	ep->stopped = 1;
 	spin_unlock(&dev->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&dev->lock);
 	ep->stopped = stopped;
 }
diff --git a/drivers/usb/gadget/udc/gr_udc.c b/drivers/usb/gadget/udc/gr_udc.c
index 08df5c4f46ce..1b3048a6a2a3 100644
--- a/drivers/usb/gadget/udc/gr_udc.c
+++ b/drivers/usb/gadget/udc/gr_udc.c
@@ -318,8 +318,26 @@ static void gr_finish_request(struct gr_ep *ep, struct gr_request *req,
 	usb_gadget_unmap_request(&dev->gadget, &req->req, ep->is_in);
 	gr_free_dma_desc_chain(dev, req);
 
-	if (ep->is_in) /* For OUT, actual gets updated bit by bit */
+	if (ep->is_in) { /* For OUT, req->req.actual gets updated bit by bit */
 		req->req.actual = req->req.length;
+	} else if (req->oddlen && req->req.actual > req->evenlen) {
+		/*
+		 * Copy to user buffer in this case where length was not evenly
+		 * divisible by ep->ep.maxpacket and the last descriptor was
+		 * actually used.
+		 */
+		char *buftail = ((char *)req->req.buf + req->evenlen);
+
+		memcpy(buftail, ep->tailbuf, req->oddlen);
+
+		if (req->req.actual > req->req.length) {
+			/* We got more data than was requested */
+			dev_dbg(ep->dev->dev, "Overflow for ep %s\n",
+				ep->ep.name);
+			gr_dbgprint_request("OVFL", ep, req);
+			req->req.status = -EOVERFLOW;
+		}
+	}
 
 	if (!status) {
 		if (ep->is_in)
@@ -339,7 +357,7 @@ static void gr_finish_request(struct gr_ep *ep, struct gr_request *req,
 	} else if (req->req.complete) {
 		spin_unlock(&dev->lock);
 
-		req->req.complete(&ep->ep, &req->req);
+		usb_gadget_giveback_request(&ep->ep, &req->req);
 
 		spin_lock(&dev->lock);
 	}
@@ -379,6 +397,15 @@ static void gr_start_dma(struct gr_ep *ep)
 	/* A descriptor should already have been allocated */
 	BUG_ON(!req->curr_desc);
 
+	/*
+	 * The DMA controller can not handle smaller OUT buffers than
+	 * ep->ep.maxpacket. It could lead to buffer overruns if an unexpectedly
+	 * long packet are received. Therefore an internal bounce buffer gets
+	 * used when such a request gets enabled.
+	 */
+	if (!ep->is_in && req->oddlen)
+		req->last_desc->data = ep->tailbuf_paddr;
+
 	wmb(); /* Make sure all is settled before handing it over to DMA */
 
 	/* Set the descriptor pointer in the hardware */
@@ -480,11 +507,11 @@ static int gr_setup_out_desc_list(struct gr_ep *ep, struct gr_request *req,
 		dma_addr_t start = req->req.dma + bytes_used;
 		u16 size = min(bytes_left, ep->bytes_per_buffer);
 
-		/* Should not happen however - gr_queue stops such lengths */
-		if (size < ep->bytes_per_buffer)
-			dev_warn(ep->dev->dev,
-				 "Buffer overrun risk: %u < %u bytes/buffer\n",
-				 size, ep->bytes_per_buffer);
+		if (size < ep->bytes_per_buffer) {
+			/* Prepare using bounce buffer */
+			req->evenlen = req->req.length - bytes_left;
+			req->oddlen = size;
+		}
 
 		ret = gr_add_dma_desc(ep, req, start, size, gfp_flags);
 		if (ret)
@@ -584,18 +611,6 @@ static int gr_queue(struct gr_ep *ep, struct gr_request *req, gfp_t gfp_flags)
 		return -EINVAL;
 	}
 
-	/*
-	 * The DMA controller can not handle smaller OUT buffers than
-	 * maxpacket. It could lead to buffer overruns if unexpectedly long
-	 * packet are received.
-	 */
-	if (!ep->is_in && (req->req.length % ep->ep.maxpacket) != 0) {
-		dev_err(dev->dev,
-			"OUT request length %d is not multiple of maxpacket\n",
-			req->req.length);
-		return -EMSGSIZE;
-	}
-
 	if (unlikely(!dev->driver || dev->gadget.speed == USB_SPEED_UNKNOWN)) {
 		dev_err(dev->dev, "-ESHUTDOWN");
 		return -ESHUTDOWN;
@@ -1286,8 +1301,8 @@ static int gr_handle_out_ep(struct gr_ep *ep)
 	if (ctrl & GR_DESC_OUT_CTRL_SE)
 		req->setup = 1;
 
-	if (len < ep->ep.maxpacket || req->req.actual == req->req.length) {
-		/* Short packet or the expected size - we are done */
+	if (len < ep->ep.maxpacket || req->req.actual >= req->req.length) {
+		/* Short packet or >= expected size - we are done */
 
 		if ((ep == &dev->epo[0]) && (dev->ep0state == GR_EP0_OSTATUS)) {
 			/*
@@ -2015,6 +2030,11 @@ static int gr_ep_init(struct gr_udc *dev, int num, int is_in, u32 maxplimit)
 	}
 	list_add_tail(&ep->ep_list, &dev->ep_list);
 
+	ep->tailbuf = dma_alloc_coherent(dev->dev, ep->ep.maxpacket_limit,
+					 &ep->tailbuf_paddr, GFP_ATOMIC);
+	if (!ep->tailbuf)
+		return -ENOMEM;
+
 	return 0;
 }
 
@@ -2067,9 +2087,24 @@ static int gr_udc_init(struct gr_udc *dev)
 	return 0;
 }
 
+static void gr_ep_remove(struct gr_udc *dev, int num, int is_in)
+{
+	struct gr_ep *ep;
+
+	if (is_in)
+		ep = &dev->epi[num];
+	else
+		ep = &dev->epo[num];
+
+	if (ep->tailbuf)
+		dma_free_coherent(dev->dev, ep->ep.maxpacket_limit,
+				  ep->tailbuf, ep->tailbuf_paddr);
+}
+
 static int gr_remove(struct platform_device *pdev)
 {
 	struct gr_udc *dev = platform_get_drvdata(pdev);
+	int i;
 
 	if (dev->added)
 		usb_del_gadget_udc(&dev->gadget); /* Shuts everything down */
@@ -2084,6 +2119,11 @@ static int gr_remove(struct platform_device *pdev)
 	gr_free_request(&dev->epi[0].ep, &dev->ep0reqi->req);
 	gr_free_request(&dev->epo[0].ep, &dev->ep0reqo->req);
 
+	for (i = 0; i < dev->nepo; i++)
+		gr_ep_remove(dev, i, 0);
+	for (i = 0; i < dev->nepi; i++)
+		gr_ep_remove(dev, i, 1);
+
 	return 0;
 }
 static int gr_request_irq(struct gr_udc *dev, int irq)
@@ -2131,7 +2171,6 @@ static int gr_probe(struct platform_device *pdev)
 	dev->gadget.name = driver_name;
 	dev->gadget.max_speed = USB_SPEED_HIGH;
 	dev->gadget.ops = &gr_ops;
-	dev->gadget.quirk_ep_out_aligned_size = true;
 
 	spin_lock_init(&dev->lock);
 	dev->regs = regs;
diff --git a/drivers/usb/gadget/udc/gr_udc.h b/drivers/usb/gadget/udc/gr_udc.h
index 8388897d9ec3..4297c4e8021f 100644
--- a/drivers/usb/gadget/udc/gr_udc.h
+++ b/drivers/usb/gadget/udc/gr_udc.h
@@ -156,6 +156,10 @@ struct gr_ep {
 	struct list_head queue;
 
 	struct list_head ep_list;
+
+	/* Bounce buffer for end of "odd" sized OUT requests */
+	void *tailbuf;
+	dma_addr_t tailbuf_paddr;
 };
 
 struct gr_request {
@@ -167,6 +171,9 @@ struct gr_request {
 	struct gr_dma_desc *curr_desc; /* Current descriptor */
 	struct gr_dma_desc *last_desc; /* Last in the chain */
 
+	u16 evenlen; /* Size of even length head (if oddlen != 0) */
+	u16 oddlen; /* Size of odd length tail if buffer length is "odd" */
+
 	u8 setup; /* Setup packet */
 };
 
diff --git a/drivers/usb/gadget/udc/lpc32xx_udc.c b/drivers/usb/gadget/udc/lpc32xx_udc.c
index 1629ad7dcb80..feab0bac8fdc 100644
--- a/drivers/usb/gadget/udc/lpc32xx_udc.c
+++ b/drivers/usb/gadget/udc/lpc32xx_udc.c
@@ -1479,7 +1479,7 @@ static void done(struct lpc32xx_ep *ep, struct lpc32xx_request *req, int status)
 
 	ep->req_pending = 0;
 	spin_unlock(&udc->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&udc->lock);
 }
 
diff --git a/drivers/usb/gadget/udc/m66592-udc.c b/drivers/usb/gadget/udc/m66592-udc.c
index de88d33b44b2..898565687a8c 100644
--- a/drivers/usb/gadget/udc/m66592-udc.c
+++ b/drivers/usb/gadget/udc/m66592-udc.c
@@ -729,7 +729,7 @@ __acquires(m66592->lock)
 		restart = 1;
 
 	spin_unlock(&ep->m66592->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&ep->m66592->lock);
 
 	if (restart) {
diff --git a/drivers/usb/gadget/udc/mv_u3d_core.c b/drivers/usb/gadget/udc/mv_u3d_core.c
index 16248711c152..046a1f808b0d 100644
--- a/drivers/usb/gadget/udc/mv_u3d_core.c
+++ b/drivers/usb/gadget/udc/mv_u3d_core.c
@@ -222,12 +222,8 @@ void mv_u3d_done(struct mv_u3d_ep *ep, struct mv_u3d_req *req, int status)
 	}
 
 	spin_unlock(&ep->u3d->lock);
-	/*
-	 * complete() is from gadget layer,
-	 * eg fsg->bulk_in_complete()
-	 */
-	if (req->req.complete)
-		req->req.complete(&ep->ep, &req->req);
+
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 
 	spin_lock(&ep->u3d->lock);
 }
diff --git a/drivers/usb/gadget/udc/mv_udc_core.c b/drivers/usb/gadget/udc/mv_udc_core.c
index 040fb169b162..3c5db80ae325 100644
--- a/drivers/usb/gadget/udc/mv_udc_core.c
+++ b/drivers/usb/gadget/udc/mv_udc_core.c
@@ -248,12 +248,8 @@ static void done(struct mv_ep *ep, struct mv_req *req, int status)
 	ep->stopped = 1;
 
 	spin_unlock(&ep->udc->lock);
-	/*
-	 * complete() is from gadget layer,
-	 * eg fsg->bulk_in_complete()
-	 */
-	if (req->req.complete)
-		req->req.complete(&ep->ep, &req->req);
+
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 
 	spin_lock(&ep->udc->lock);
 	ep->stopped = stopped;
diff --git a/drivers/usb/gadget/udc/net2272.c b/drivers/usb/gadget/udc/net2272.c
index 059cfe527982..84d7162a8022 100644
--- a/drivers/usb/gadget/udc/net2272.c
+++ b/drivers/usb/gadget/udc/net2272.c
@@ -394,7 +394,7 @@ net2272_done(struct net2272_ep *ep, struct net2272_request *req, int status)
 	/* don't modify queue heads during completion callback */
 	ep->stopped = 1;
 	spin_unlock(&dev->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&dev->lock);
 	ep->stopped = stopped;
 }
diff --git a/drivers/usb/gadget/udc/net2280.c b/drivers/usb/gadget/udc/net2280.c
index f4eac113690e..8d13337e2dde 100644
--- a/drivers/usb/gadget/udc/net2280.c
+++ b/drivers/usb/gadget/udc/net2280.c
@@ -928,7 +928,7 @@ done(struct net2280_ep *ep, struct net2280_request *req, int status)
 	/* don't modify queue heads during completion callback */
 	ep->stopped = 1;
 	spin_unlock(&dev->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&dev->lock);
 	ep->stopped = stopped;
 }
@@ -3320,7 +3320,7 @@ static void handle_stat1_irqs(struct net2280 *dev, u32 stat)
 	if (stat & tmp) {
 		writel(tmp, &dev->regs->irqstat1);
 		if ((((stat & BIT(ROOT_PORT_RESET_INTERRUPT)) &&
-				(readl(&dev->usb->usbstat) & mask)) ||
+				((readl(&dev->usb->usbstat) & mask) == 0)) ||
 				((readl(&dev->usb->usbctl) &
 					BIT(VBUS_PIN)) == 0)) &&
 				(dev->gadget.speed != USB_SPEED_UNKNOWN)) {
diff --git a/drivers/usb/gadget/udc/omap_udc.c b/drivers/usb/gadget/udc/omap_udc.c
index e731373fd4d7..dcdfea46003b 100644
--- a/drivers/usb/gadget/udc/omap_udc.c
+++ b/drivers/usb/gadget/udc/omap_udc.c
@@ -315,7 +315,7 @@ done(struct omap_ep *ep, struct omap_req *req, int status)
 	/* don't modify queue heads during completion callback */
 	ep->stopped = 1;
 	spin_unlock(&ep->udc->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&ep->udc->lock);
 	ep->stopped = stopped;
 }
diff --git a/drivers/usb/gadget/udc/pch_udc.c b/drivers/usb/gadget/udc/pch_udc.c
index eb8c3bedb57a..ccbe3d4a2a50 100644
--- a/drivers/usb/gadget/udc/pch_udc.c
+++ b/drivers/usb/gadget/udc/pch_udc.c
@@ -343,6 +343,7 @@ struct pch_vbus_gpio_data {
  * @setup_data:		Received setup data
  * @phys_addr:		of device memory
  * @base_addr:		for mapped device memory
+ * @bar:		Indicates which PCI BAR for USB regs
  * @irq:		IRQ line for the device
  * @cfg_data:		current cfg, intf, and alt in use
  * @vbus_gpio:		GPIO informaton for detecting VBUS
@@ -370,14 +371,17 @@ struct pch_udc_dev {
 	struct usb_ctrlrequest		setup_data;
 	unsigned long			phys_addr;
 	void __iomem			*base_addr;
+	unsigned			bar;
 	unsigned			irq;
 	struct pch_udc_cfg_data		cfg_data;
 	struct pch_vbus_gpio_data	vbus_gpio;
 };
 #define to_pch_udc(g)	(container_of((g), struct pch_udc_dev, gadget))
 
+#define PCH_UDC_PCI_BAR_QUARK_X1000	0
 #define PCH_UDC_PCI_BAR			1
 #define PCI_DEVICE_ID_INTEL_EG20T_UDC	0x8808
+#define PCI_DEVICE_ID_INTEL_QUARK_X1000_UDC	0x0939
 #define PCI_VENDOR_ID_ROHM		0x10DB
 #define PCI_DEVICE_ID_ML7213_IOH_UDC	0x801D
 #define PCI_DEVICE_ID_ML7831_IOH_UDC	0x8808
@@ -1486,7 +1490,7 @@ static void complete_req(struct pch_udc_ep *ep, struct pch_udc_request *req,
 	spin_unlock(&dev->lock);
 	if (!ep->in)
 		pch_udc_ep_clear_rrdy(ep);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&dev->lock);
 	ep->halted = halted;
 }
@@ -3076,7 +3080,7 @@ static void pch_udc_remove(struct pci_dev *pdev)
 		iounmap(dev->base_addr);
 	if (dev->mem_region)
 		release_mem_region(dev->phys_addr,
-				   pci_resource_len(pdev, PCH_UDC_PCI_BAR));
+				   pci_resource_len(pdev, dev->bar));
 	if (dev->active)
 		pci_disable_device(pdev);
 	kfree(dev);
@@ -3144,9 +3148,15 @@ static int pch_udc_probe(struct pci_dev *pdev,
 	dev->active = 1;
 	pci_set_drvdata(pdev, dev);
 
+	/* Determine BAR based on PCI ID */
+	if (id->device == PCI_DEVICE_ID_INTEL_QUARK_X1000_UDC)
+		dev->bar = PCH_UDC_PCI_BAR_QUARK_X1000;
+	else
+		dev->bar = PCH_UDC_PCI_BAR;
+
 	/* PCI resource allocation */
-	resource = pci_resource_start(pdev, 1);
-	len = pci_resource_len(pdev, 1);
+	resource = pci_resource_start(pdev, dev->bar);
+	len = pci_resource_len(pdev, dev->bar);
 
 	if (!request_mem_region(resource, len, KBUILD_MODNAME)) {
 		dev_err(&pdev->dev, "%s: pci device used already\n", __func__);
@@ -3212,6 +3222,12 @@ finished:
 
 static const struct pci_device_id pch_udc_pcidev_id[] = {
 	{
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL,
+			   PCI_DEVICE_ID_INTEL_QUARK_X1000_UDC),
+		.class = (PCI_CLASS_SERIAL_USB << 8) | 0xfe,
+		.class_mask = 0xffffffff,
+	},
+	{
 		PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_EG20T_UDC),
 		.class = (PCI_CLASS_SERIAL_USB << 8) | 0xfe,
 		.class_mask = 0xffffffff,
diff --git a/drivers/usb/gadget/udc/pxa25x_udc.c b/drivers/usb/gadget/udc/pxa25x_udc.c
index 251e4d5ee152..42f7eeb8ff6f 100644
--- a/drivers/usb/gadget/udc/pxa25x_udc.c
+++ b/drivers/usb/gadget/udc/pxa25x_udc.c
@@ -347,7 +347,7 @@ static void done(struct pxa25x_ep *ep, struct pxa25x_request *req, int status)
 
 	/* don't modify queue heads during completion callback */
 	ep->stopped = 1;
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	ep->stopped = stopped;
 }
 
diff --git a/drivers/usb/gadget/udc/pxa27x_udc.c b/drivers/usb/gadget/udc/pxa27x_udc.c
index 597d39f89420..4868369eeec6 100644
--- a/drivers/usb/gadget/udc/pxa27x_udc.c
+++ b/drivers/usb/gadget/udc/pxa27x_udc.c
@@ -758,7 +758,7 @@ static void req_done(struct pxa_ep *ep, struct pxa27x_request *req, int status,
 	if (pflags)
 		spin_unlock_irqrestore(&ep->lock, *pflags);
 	local_irq_save(flags);
-	req->req.complete(&req->udc_usb_ep->usb_ep, &req->req);
+	usb_gadget_giveback_request(&req->udc_usb_ep->usb_ep, &req->req);
 	local_irq_restore(flags);
 	if (pflags)
 		spin_lock_irqsave(&ep->lock, *pflags);
diff --git a/drivers/usb/gadget/udc/r8a66597-udc.c b/drivers/usb/gadget/udc/r8a66597-udc.c
index 46008421c1ec..f8186613b53e 100644
--- a/drivers/usb/gadget/udc/r8a66597-udc.c
+++ b/drivers/usb/gadget/udc/r8a66597-udc.c
@@ -430,7 +430,7 @@ static void r8a66597_ep_setting(struct r8a66597 *r8a66597,
 	ep->pipenum = pipenum;
 	ep->ep.maxpacket = usb_endpoint_maxp(desc);
 	r8a66597->pipenum2ep[pipenum] = ep;
-	r8a66597->epaddr2ep[desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK]
+	r8a66597->epaddr2ep[usb_endpoint_num(desc)]
 		= ep;
 	INIT_LIST_HEAD(&ep->queue);
 }
@@ -464,7 +464,7 @@ static int alloc_pipe_config(struct r8a66597_ep *ep,
 	if (ep->pipenum)	/* already allocated pipe  */
 		return 0;
 
-	switch (desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) {
+	switch (usb_endpoint_type(desc)) {
 	case USB_ENDPOINT_XFER_BULK:
 		if (r8a66597->bulk >= R8A66597_MAX_NUM_BULK) {
 			if (r8a66597->isochronous >= R8A66597_MAX_NUM_ISOC) {
@@ -509,7 +509,7 @@ static int alloc_pipe_config(struct r8a66597_ep *ep,
 	}
 	ep->type = info.type;
 
-	info.epnum = desc->bEndpointAddress & USB_ENDPOINT_NUMBER_MASK;
+	info.epnum = usb_endpoint_num(desc);
 	info.maxpacket = usb_endpoint_maxp(desc);
 	info.interval = desc->bInterval;
 	if (desc->bEndpointAddress & USB_ENDPOINT_DIR_MASK)
@@ -925,7 +925,7 @@ __acquires(r8a66597->lock)
 		sudmac_free_channel(ep->r8a66597, ep, req);
 
 	spin_unlock(&ep->r8a66597->lock);
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	spin_lock(&ep->r8a66597->lock);
 
 	if (restart) {
@@ -1846,10 +1846,8 @@ static int r8a66597_sudmac_ioremap(struct r8a66597 *r8a66597,
 
 	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "sudmac");
 	r8a66597->sudmac_reg = devm_ioremap_resource(&pdev->dev, res);
-	if (IS_ERR(r8a66597->sudmac_reg)) {
-		dev_err(&pdev->dev, "ioremap error(sudmac).\n");
+	if (IS_ERR(r8a66597->sudmac_reg))
 		return PTR_ERR(r8a66597->sudmac_reg);
-	}
 
 	return 0;
 }
@@ -1868,8 +1866,8 @@ static int r8a66597_probe(struct platform_device *pdev)
 
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	reg = devm_ioremap_resource(&pdev->dev, res);
-	if (!reg)
-		return -ENODEV;
+	if (IS_ERR(reg))
+		return PTR_ERR(reg);
 
 	ires = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
 	irq = ires->start;
diff --git a/drivers/usb/gadget/udc/s3c-hsudc.c b/drivers/usb/gadget/udc/s3c-hsudc.c
index 10c6a128250c..dfbf55797360 100644
--- a/drivers/usb/gadget/udc/s3c-hsudc.c
+++ b/drivers/usb/gadget/udc/s3c-hsudc.c
@@ -258,8 +258,7 @@ static void s3c_hsudc_complete_request(struct s3c_hsudc_ep *hsep,
 
 	hsep->stopped = 1;
 	spin_unlock(&hsudc->lock);
-	if (hsreq->req.complete != NULL)
-		hsreq->req.complete(&hsep->ep, &hsreq->req);
+	usb_gadget_giveback_request(&hsep->ep, &hsreq->req);
 	spin_lock(&hsudc->lock);
 	hsep->stopped = stopped;
 }
diff --git a/drivers/usb/gadget/udc/s3c2410_udc.c b/drivers/usb/gadget/udc/s3c2410_udc.c
index 357b58e0087b..ff423d15beff 100644
--- a/drivers/usb/gadget/udc/s3c2410_udc.c
+++ b/drivers/usb/gadget/udc/s3c2410_udc.c
@@ -272,7 +272,7 @@ static void s3c2410_udc_done(struct s3c2410_ep *ep,
 		status = req->req.status;
 
 	ep->halted = 1;
-	req->req.complete(&ep->ep, &req->req);
+	usb_gadget_giveback_request(&ep->ep, &req->req);
 	ep->halted = halted;
 }
 
diff --git a/drivers/usb/gadget/udc/udc-core.c b/drivers/usb/gadget/udc/udc-core.c
index b0d98172bc07..f107bb60a5ab 100644
--- a/drivers/usb/gadget/udc/udc-core.c
+++ b/drivers/usb/gadget/udc/udc-core.c
@@ -27,6 +27,7 @@
 
 #include <linux/usb/ch9.h>
 #include <linux/usb/gadget.h>
+#include <linux/usb.h>
 
 /**
  * struct usb_udc - describes one usb device controller
@@ -106,11 +107,42 @@ EXPORT_SYMBOL_GPL(usb_gadget_unmap_request);
 
 /* ------------------------------------------------------------------------- */
 
+/**
+ * usb_gadget_giveback_request - give the request back to the gadget layer
+ * Context: in_interrupt()
+ *
+ * This is called by device controller drivers in order to return the
+ * completed request back to the gadget layer.
+ */
+void usb_gadget_giveback_request(struct usb_ep *ep,
+		struct usb_request *req)
+{
+	if (likely(req->status == 0))
+		usb_led_activity(USB_LED_EVENT_GADGET);
+
+	req->complete(ep, req);
+}
+EXPORT_SYMBOL_GPL(usb_gadget_giveback_request);
+
+/* ------------------------------------------------------------------------- */
+
 static void usb_gadget_state_work(struct work_struct *work)
 {
 	struct usb_gadget	*gadget = work_to_gadget(work);
+	struct usb_udc		*udc = NULL;
+
+	mutex_lock(&udc_lock);
+	list_for_each_entry(udc, &udc_list, list)
+		if (udc->gadget == gadget)
+			goto found;
+	mutex_unlock(&udc_lock);
+
+	return;
+
+found:
+	mutex_unlock(&udc_lock);
 
-	sysfs_notify(&gadget->dev.kobj, NULL, "state");
+	sysfs_notify(&udc->dev.kobj, NULL, "state");
 }
 
 void usb_gadget_set_state(struct usb_gadget *gadget,
@@ -124,6 +156,23 @@ EXPORT_SYMBOL_GPL(usb_gadget_set_state);
 /* ------------------------------------------------------------------------- */
 
 /**
+ * usb_gadget_udc_reset - notifies the udc core that bus reset occurs
+ * @gadget: The gadget which bus reset occurs
+ * @driver: The gadget driver we want to notify
+ *
+ * If the udc driver has bus reset handler, it needs to call this when the bus
+ * reset occurs, it notifies the gadget driver that the bus reset occurs as
+ * well as updates gadget state.
+ */
+void usb_gadget_udc_reset(struct usb_gadget *gadget,
+		struct usb_gadget_driver *driver)
+{
+	driver->reset(gadget);
+	usb_gadget_set_state(gadget, USB_STATE_DEFAULT);
+}
+EXPORT_SYMBOL_GPL(usb_gadget_udc_reset);
+
+/**
  * usb_gadget_udc_start - tells usb device controller to start up
  * @gadget: The gadget we want to get started
  * @driver: The driver we want to bind to @gadget
diff --git a/drivers/usb/gadget/udc/udc-xilinx.c b/drivers/usb/gadget/udc/udc-xilinx.c
new file mode 100644
index 000000000000..ed27e1687a4e
--- /dev/null
+++ b/drivers/usb/gadget/udc/udc-xilinx.c
@@ -0,0 +1,2180 @@
+/*
+ * Xilinx USB peripheral controller driver
+ *
+ * Copyright (C) 2004 by Thomas Rathbone
+ * Copyright (C) 2005 by HP Labs
+ * Copyright (C) 2005 by David Brownell
+ * Copyright (C) 2010 - 2014 Xilinx, Inc.
+ *
+ * Some parts of this driver code is based on the driver for at91-series
+ * USB peripheral controller (at91_udc.c).
+ *
+ * This program is free software; you can redistribute it
+ * and/or modify it under the terms of the GNU General Public
+ * License as published by the Free Software Foundation;
+ * either version 2 of the License, or (at your option) any
+ * later version.
+ */
+
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/dma-mapping.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of_address.h>
+#include <linux/of_device.h>
+#include <linux/of_platform.h>
+#include <linux/of_irq.h>
+#include <linux/prefetch.h>
+#include <linux/usb/ch9.h>
+#include <linux/usb/gadget.h>
+
+/* Register offsets for the USB device.*/
+#define XUSB_EP0_CONFIG_OFFSET		0x0000  /* EP0 Config Reg Offset */
+#define XUSB_SETUP_PKT_ADDR_OFFSET	0x0080  /* Setup Packet Address */
+#define XUSB_ADDRESS_OFFSET		0x0100  /* Address Register */
+#define XUSB_CONTROL_OFFSET		0x0104  /* Control Register */
+#define XUSB_STATUS_OFFSET		0x0108  /* Status Register */
+#define XUSB_FRAMENUM_OFFSET		0x010C	/* Frame Number Register */
+#define XUSB_IER_OFFSET			0x0110	/* Interrupt Enable Register */
+#define XUSB_BUFFREADY_OFFSET		0x0114	/* Buffer Ready Register */
+#define XUSB_TESTMODE_OFFSET		0x0118	/* Test Mode Register */
+#define XUSB_DMA_RESET_OFFSET		0x0200  /* DMA Soft Reset Register */
+#define XUSB_DMA_CONTROL_OFFSET		0x0204	/* DMA Control Register */
+#define XUSB_DMA_DSAR_ADDR_OFFSET	0x0208	/* DMA source Address Reg */
+#define XUSB_DMA_DDAR_ADDR_OFFSET	0x020C	/* DMA destination Addr Reg */
+#define XUSB_DMA_LENGTH_OFFSET		0x0210	/* DMA Length Register */
+#define XUSB_DMA_STATUS_OFFSET		0x0214	/* DMA Status Register */
+
+/* Endpoint Configuration Space offsets */
+#define XUSB_EP_CFGSTATUS_OFFSET	0x00	/* Endpoint Config Status  */
+#define XUSB_EP_BUF0COUNT_OFFSET	0x08	/* Buffer 0 Count */
+#define XUSB_EP_BUF1COUNT_OFFSET	0x0C	/* Buffer 1 Count */
+
+#define XUSB_CONTROL_USB_READY_MASK	0x80000000 /* USB ready Mask */
+#define XUSB_CONTROL_USB_RMTWAKE_MASK	0x40000000 /* Remote wake up mask */
+
+/* Interrupt register related masks.*/
+#define XUSB_STATUS_GLOBAL_INTR_MASK	0x80000000 /* Global Intr Enable */
+#define XUSB_STATUS_DMADONE_MASK	0x04000000 /* DMA done Mask */
+#define XUSB_STATUS_DMAERR_MASK		0x02000000 /* DMA Error Mask */
+#define XUSB_STATUS_DMABUSY_MASK	0x80000000 /* DMA Error Mask */
+#define XUSB_STATUS_RESUME_MASK		0x01000000 /* USB Resume Mask */
+#define XUSB_STATUS_RESET_MASK		0x00800000 /* USB Reset Mask */
+#define XUSB_STATUS_SUSPEND_MASK	0x00400000 /* USB Suspend Mask */
+#define XUSB_STATUS_DISCONNECT_MASK	0x00200000 /* USB Disconnect Mask */
+#define XUSB_STATUS_FIFO_BUFF_RDY_MASK	0x00100000 /* FIFO Buff Ready Mask */
+#define XUSB_STATUS_FIFO_BUFF_FREE_MASK	0x00080000 /* FIFO Buff Free Mask */
+#define XUSB_STATUS_SETUP_PACKET_MASK	0x00040000 /* Setup packet received */
+#define XUSB_STATUS_EP1_BUFF2_COMP_MASK	0x00000200 /* EP 1 Buff 2 Processed */
+#define XUSB_STATUS_EP1_BUFF1_COMP_MASK	0x00000002 /* EP 1 Buff 1 Processed */
+#define XUSB_STATUS_EP0_BUFF2_COMP_MASK	0x00000100 /* EP 0 Buff 2 Processed */
+#define XUSB_STATUS_EP0_BUFF1_COMP_MASK	0x00000001 /* EP 0 Buff 1 Processed */
+#define XUSB_STATUS_HIGH_SPEED_MASK	0x00010000 /* USB Speed Mask */
+/* Suspend,Reset,Suspend and Disconnect Mask */
+#define XUSB_STATUS_INTR_EVENT_MASK	0x01E00000
+/* Buffers  completion Mask */
+#define XUSB_STATUS_INTR_BUFF_COMP_ALL_MASK	0x0000FEFF
+/* Mask for buffer 0 and buffer 1 completion for all Endpoints */
+#define XUSB_STATUS_INTR_BUFF_COMP_SHIFT_MASK	0x00000101
+#define XUSB_STATUS_EP_BUFF2_SHIFT	8	   /* EP buffer offset */
+
+/* Endpoint Configuration Status Register */
+#define XUSB_EP_CFG_VALID_MASK		0x80000000 /* Endpoint Valid bit */
+#define XUSB_EP_CFG_STALL_MASK		0x40000000 /* Endpoint Stall bit */
+#define XUSB_EP_CFG_DATA_TOGGLE_MASK	0x08000000 /* Endpoint Data toggle */
+
+/* USB device specific global configuration constants.*/
+#define XUSB_MAX_ENDPOINTS		8	/* Maximum End Points */
+#define XUSB_EP_NUMBER_ZERO		0	/* End point Zero */
+/* DPRAM is the source address for DMA transfer */
+#define XUSB_DMA_READ_FROM_DPRAM	0x80000000
+#define XUSB_DMA_DMASR_BUSY		0x80000000 /* DMA busy */
+#define XUSB_DMA_DMASR_ERROR		0x40000000 /* DMA Error */
+/*
+ * When this bit is set, the DMA buffer ready bit is set by hardware upon
+ * DMA transfer completion.
+ */
+#define XUSB_DMA_BRR_CTRL		0x40000000 /* DMA bufready ctrl bit */
+/* Phase States */
+#define SETUP_PHASE			0x0000	/* Setup Phase */
+#define DATA_PHASE			0x0001  /* Data Phase */
+#define STATUS_PHASE			0x0002  /* Status Phase */
+
+#define EP0_MAX_PACKET		64 /* Endpoint 0 maximum packet length */
+#define STATUSBUFF_SIZE		2  /* Buffer size for GET_STATUS command */
+#define EPNAME_SIZE		4  /* Buffer size for endpoint name */
+
+/* container_of helper macros */
+#define to_udc(g)	 container_of((g), struct xusb_udc, gadget)
+#define to_xusb_ep(ep)	 container_of((ep), struct xusb_ep, ep_usb)
+#define to_xusb_req(req) container_of((req), struct xusb_req, usb_req)
+
+/**
+ * struct xusb_req - Xilinx USB device request structure
+ * @usb_req: Linux usb request structure
+ * @queue: usb device request queue
+ * @ep: pointer to xusb_endpoint structure
+ */
+struct xusb_req {
+	struct usb_request usb_req;
+	struct list_head queue;
+	struct xusb_ep *ep;
+};
+
+/**
+ * struct xusb_ep - USB end point structure.
+ * @ep_usb: usb endpoint instance
+ * @queue: endpoint message queue
+ * @udc: xilinx usb peripheral driver instance pointer
+ * @desc: pointer to the usb endpoint descriptor
+ * @rambase: the endpoint buffer address
+ * @offset: the endpoint register offset value
+ * @name: name of the endpoint
+ * @epnumber: endpoint number
+ * @maxpacket: maximum packet size the endpoint can store
+ * @buffer0count: the size of the packet recieved in the first buffer
+ * @buffer1count: the size of the packet received in the second buffer
+ * @curbufnum: current buffer of endpoint that will be processed next
+ * @buffer0ready: the busy state of first buffer
+ * @buffer1ready: the busy state of second buffer
+ * @is_in: endpoint direction (IN or OUT)
+ * @is_iso: endpoint type(isochronous or non isochronous)
+ */
+struct xusb_ep {
+	struct usb_ep ep_usb;
+	struct list_head queue;
+	struct xusb_udc *udc;
+	const struct usb_endpoint_descriptor *desc;
+	u32  rambase;
+	u32  offset;
+	char name[4];
+	u16  epnumber;
+	u16  maxpacket;
+	u16  buffer0count;
+	u16  buffer1count;
+	u8   curbufnum;
+	bool buffer0ready;
+	bool buffer1ready;
+	bool is_in;
+	bool is_iso;
+};
+
+/**
+ * struct xusb_udc -  USB peripheral driver structure
+ * @gadget: USB gadget driver instance
+ * @ep: an array of endpoint structures
+ * @driver: pointer to the usb gadget driver instance
+ * @setup: usb_ctrlrequest structure for control requests
+ * @req: pointer to dummy request for get status command
+ * @dev: pointer to device structure in gadget
+ * @usb_state: device in suspended state or not
+ * @remote_wkp: remote wakeup enabled by host
+ * @setupseqtx: tx status
+ * @setupseqrx: rx status
+ * @addr: the usb device base address
+ * @lock: instance of spinlock
+ * @dma_enabled: flag indicating whether the dma is included in the system
+ * @read_fn: function pointer to read device registers
+ * @write_fn: function pointer to write to device registers
+ */
+struct xusb_udc {
+	struct usb_gadget gadget;
+	struct xusb_ep ep[8];
+	struct usb_gadget_driver *driver;
+	struct usb_ctrlrequest setup;
+	struct xusb_req *req;
+	struct device *dev;
+	u32 usb_state;
+	u32 remote_wkp;
+	u32 setupseqtx;
+	u32 setupseqrx;
+	void __iomem *addr;
+	spinlock_t lock;
+	bool dma_enabled;
+
+	unsigned int (*read_fn)(void __iomem *);
+	void (*write_fn)(void __iomem *, u32, u32);
+};
+
+/* Endpoint buffer start addresses in the core */
+static u32 rambase[8] = { 0x22, 0x1000, 0x1100, 0x1200, 0x1300, 0x1400, 0x1500,
+			  0x1600 };
+
+static const char driver_name[] = "xilinx-udc";
+static const char ep0name[] = "ep0";
+
+/* Control endpoint configuration.*/
+static const struct usb_endpoint_descriptor config_bulk_out_desc = {
+	.bLength		= USB_DT_ENDPOINT_SIZE,
+	.bDescriptorType	= USB_DT_ENDPOINT,
+	.bEndpointAddress	= USB_DIR_OUT,
+	.bmAttributes		= USB_ENDPOINT_XFER_BULK,
+	.wMaxPacketSize		= cpu_to_le16(EP0_MAX_PACKET),
+};
+
+/**
+ * xudc_write32 - little endian write to device registers
+ * @addr: base addr of device registers
+ * @offset: register offset
+ * @val: data to be written
+ */
+static void xudc_write32(void __iomem *addr, u32 offset, u32 val)
+{
+	iowrite32(val, addr + offset);
+}
+
+/**
+ * xudc_read32 - little endian read from device registers
+ * @addr: addr of device register
+ * Return: value at addr
+ */
+static unsigned int xudc_read32(void __iomem *addr)
+{
+	return ioread32(addr);
+}
+
+/**
+ * xudc_write32_be - big endian write to device registers
+ * @addr: base addr of device registers
+ * @offset: register offset
+ * @val: data to be written
+ */
+static void xudc_write32_be(void __iomem *addr, u32 offset, u32 val)
+{
+	iowrite32be(val, addr + offset);
+}
+
+/**
+ * xudc_read32_be - big endian read from device registers
+ * @addr: addr of device register
+ * Return: value at addr
+ */
+static unsigned int xudc_read32_be(void __iomem *addr)
+{
+	return ioread32be(addr);
+}
+
+/**
+ * xudc_wrstatus - Sets up the usb device status stages.
+ * @udc: pointer to the usb device controller structure.
+ */
+static void xudc_wrstatus(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep0 = &udc->ep[XUSB_EP_NUMBER_ZERO];
+	u32 epcfgreg;
+
+	epcfgreg = udc->read_fn(udc->addr + ep0->offset)|
+				XUSB_EP_CFG_DATA_TOGGLE_MASK;
+	udc->write_fn(udc->addr, ep0->offset, epcfgreg);
+	udc->write_fn(udc->addr, ep0->offset + XUSB_EP_BUF0COUNT_OFFSET, 0);
+	udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET, 1);
+}
+
+/**
+ * xudc_epconfig - Configures the given endpoint.
+ * @ep: pointer to the usb device endpoint structure.
+ * @udc: pointer to the usb peripheral controller structure.
+ *
+ * This function configures a specific endpoint with the given configuration
+ * data.
+ */
+static void xudc_epconfig(struct xusb_ep *ep, struct xusb_udc *udc)
+{
+	u32 epcfgreg;
+
+	/*
+	 * Configure the end point direction, type, Max Packet Size and the
+	 * EP buffer location.
+	 */
+	epcfgreg = ((ep->is_in << 29) | (ep->is_iso << 28) |
+		   (ep->ep_usb.maxpacket << 15) | (ep->rambase));
+	udc->write_fn(udc->addr, ep->offset, epcfgreg);
+
+	/* Set the Buffer count and the Buffer ready bits.*/
+	udc->write_fn(udc->addr, ep->offset + XUSB_EP_BUF0COUNT_OFFSET,
+		      ep->buffer0count);
+	udc->write_fn(udc->addr, ep->offset + XUSB_EP_BUF1COUNT_OFFSET,
+		      ep->buffer1count);
+	if (ep->buffer0ready)
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET,
+			      1 << ep->epnumber);
+	if (ep->buffer1ready)
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET,
+			      1 << (ep->epnumber + XUSB_STATUS_EP_BUFF2_SHIFT));
+}
+
+/**
+ * xudc_start_dma - Starts DMA transfer.
+ * @ep: pointer to the usb device endpoint structure.
+ * @src: DMA source address.
+ * @dst: DMA destination address.
+ * @length: number of bytes to transfer.
+ *
+ * Return: 0 on success, error code on failure
+ *
+ * This function starts DMA transfer by writing to DMA source,
+ * destination and lenth registers.
+ */
+static int xudc_start_dma(struct xusb_ep *ep, dma_addr_t src,
+			  dma_addr_t dst, u32 length)
+{
+	struct xusb_udc *udc = ep->udc;
+	int rc = 0;
+	u32 timeout = 500;
+	u32 reg;
+
+	/*
+	 * Set the addresses in the DMA source and
+	 * destination registers and then set the length
+	 * into the DMA length register.
+	 */
+	udc->write_fn(udc->addr, XUSB_DMA_DSAR_ADDR_OFFSET, src);
+	udc->write_fn(udc->addr, XUSB_DMA_DDAR_ADDR_OFFSET, dst);
+	udc->write_fn(udc->addr, XUSB_DMA_LENGTH_OFFSET, length);
+
+	/*
+	 * Wait till DMA transaction is complete and
+	 * check whether the DMA transaction was
+	 * successful.
+	 */
+	do {
+		reg = udc->read_fn(udc->addr + XUSB_DMA_STATUS_OFFSET);
+		if (!(reg &  XUSB_DMA_DMASR_BUSY))
+			break;
+
+		/*
+		 * We can't sleep here, because it's also called from
+		 * interrupt context.
+		 */
+		timeout--;
+		if (!timeout) {
+			dev_err(udc->dev, "DMA timeout\n");
+			return -ETIMEDOUT;
+		}
+		udelay(1);
+	} while (1);
+
+	if ((udc->read_fn(udc->addr + XUSB_DMA_STATUS_OFFSET) &
+			  XUSB_DMA_DMASR_ERROR) == XUSB_DMA_DMASR_ERROR){
+		dev_err(udc->dev, "DMA Error\n");
+		rc = -EINVAL;
+	}
+
+	return rc;
+}
+
+/**
+ * xudc_dma_send - Sends IN data using DMA.
+ * @ep: pointer to the usb device endpoint structure.
+ * @req: pointer to the usb request structure.
+ * @buffer: pointer to data to be sent.
+ * @length: number of bytes to send.
+ *
+ * Return: 0 on success, -EAGAIN if no buffer is free and error
+ *	   code on failure.
+ *
+ * This function sends data using DMA.
+ */
+static int xudc_dma_send(struct xusb_ep *ep, struct xusb_req *req,
+			 u8 *buffer, u32 length)
+{
+	u32 *eprambase;
+	dma_addr_t src;
+	dma_addr_t dst;
+	struct xusb_udc *udc = ep->udc;
+
+	src = req->usb_req.dma + req->usb_req.actual;
+	if (req->usb_req.length)
+		dma_sync_single_for_device(udc->dev, src,
+					   length, DMA_TO_DEVICE);
+	if (!ep->curbufnum && !ep->buffer0ready) {
+		/* Get the Buffer address and copy the transmit data.*/
+		eprambase = (u32 __force *)(udc->addr + ep->rambase);
+		dst = virt_to_phys(eprambase);
+		udc->write_fn(udc->addr, ep->offset +
+			      XUSB_EP_BUF0COUNT_OFFSET, length);
+		udc->write_fn(udc->addr, XUSB_DMA_CONTROL_OFFSET,
+			      XUSB_DMA_BRR_CTRL | (1 << ep->epnumber));
+		ep->buffer0ready = 1;
+		ep->curbufnum = 1;
+	} else if (ep->curbufnum && !ep->buffer1ready) {
+		/* Get the Buffer address and copy the transmit data.*/
+		eprambase = (u32 __force *)(udc->addr + ep->rambase +
+			     ep->ep_usb.maxpacket);
+		dst = virt_to_phys(eprambase);
+		udc->write_fn(udc->addr, ep->offset +
+			      XUSB_EP_BUF1COUNT_OFFSET, length);
+		udc->write_fn(udc->addr, XUSB_DMA_CONTROL_OFFSET,
+			      XUSB_DMA_BRR_CTRL | (1 << (ep->epnumber +
+			      XUSB_STATUS_EP_BUFF2_SHIFT)));
+		ep->buffer1ready = 1;
+		ep->curbufnum = 0;
+	} else {
+		/* None of ping pong buffers are ready currently .*/
+		return -EAGAIN;
+	}
+
+	return xudc_start_dma(ep, src, dst, length);
+}
+
+/**
+ * xudc_dma_receive - Receives OUT data using DMA.
+ * @ep: pointer to the usb device endpoint structure.
+ * @req: pointer to the usb request structure.
+ * @buffer: pointer to storage buffer of received data.
+ * @length: number of bytes to receive.
+ *
+ * Return: 0 on success, -EAGAIN if no buffer is free and error
+ *	   code on failure.
+ *
+ * This function receives data using DMA.
+ */
+static int xudc_dma_receive(struct xusb_ep *ep, struct xusb_req *req,
+			    u8 *buffer, u32 length)
+{
+	u32 *eprambase;
+	dma_addr_t src;
+	dma_addr_t dst;
+	struct xusb_udc *udc = ep->udc;
+
+	dst = req->usb_req.dma + req->usb_req.actual;
+	if (!ep->curbufnum && !ep->buffer0ready) {
+		/* Get the Buffer address and copy the transmit data */
+		eprambase = (u32 __force *)(udc->addr + ep->rambase);
+		src = virt_to_phys(eprambase);
+		udc->write_fn(udc->addr, XUSB_DMA_CONTROL_OFFSET,
+			      XUSB_DMA_BRR_CTRL | XUSB_DMA_READ_FROM_DPRAM |
+			      (1 << ep->epnumber));
+		ep->buffer0ready = 1;
+		ep->curbufnum = 1;
+	} else if (ep->curbufnum && !ep->buffer1ready) {
+		/* Get the Buffer address and copy the transmit data */
+		eprambase = (u32 __force *)(udc->addr +
+			     ep->rambase + ep->ep_usb.maxpacket);
+		src = virt_to_phys(eprambase);
+		udc->write_fn(udc->addr, XUSB_DMA_CONTROL_OFFSET,
+			      XUSB_DMA_BRR_CTRL | XUSB_DMA_READ_FROM_DPRAM |
+			      (1 << (ep->epnumber +
+			      XUSB_STATUS_EP_BUFF2_SHIFT)));
+		ep->buffer1ready = 1;
+		ep->curbufnum = 0;
+	} else {
+		/* None of the ping-pong buffers are ready currently */
+		return -EAGAIN;
+	}
+
+	return xudc_start_dma(ep, src, dst, length);
+}
+
+/**
+ * xudc_eptxrx - Transmits or receives data to or from an endpoint.
+ * @ep: pointer to the usb endpoint configuration structure.
+ * @req: pointer to the usb request structure.
+ * @bufferptr: pointer to buffer containing the data to be sent.
+ * @bufferlen: The number of data bytes to be sent.
+ *
+ * Return: 0 on success, -EAGAIN if no buffer is free.
+ *
+ * This function copies the transmit/receive data to/from the end point buffer
+ * and enables the buffer for transmission/reception.
+ */
+static int xudc_eptxrx(struct xusb_ep *ep, struct xusb_req *req,
+		       u8 *bufferptr, u32 bufferlen)
+{
+	u32 *eprambase;
+	u32 bytestosend;
+	int rc = 0;
+	struct xusb_udc *udc = ep->udc;
+
+	bytestosend = bufferlen;
+	if (udc->dma_enabled) {
+		if (ep->is_in)
+			rc = xudc_dma_send(ep, req, bufferptr, bufferlen);
+		else
+			rc = xudc_dma_receive(ep, req, bufferptr, bufferlen);
+		return rc;
+	}
+	/* Put the transmit buffer into the correct ping-pong buffer.*/
+	if (!ep->curbufnum && !ep->buffer0ready) {
+		/* Get the Buffer address and copy the transmit data.*/
+		eprambase = (u32 __force *)(udc->addr + ep->rambase);
+		if (ep->is_in) {
+			memcpy(eprambase, bufferptr, bytestosend);
+			udc->write_fn(udc->addr, ep->offset +
+				      XUSB_EP_BUF0COUNT_OFFSET, bufferlen);
+		} else {
+			memcpy(bufferptr, eprambase, bytestosend);
+		}
+		/*
+		 * Enable the buffer for transmission.
+		 */
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET,
+			      1 << ep->epnumber);
+		ep->buffer0ready = 1;
+		ep->curbufnum = 1;
+	} else if (ep->curbufnum && !ep->buffer1ready) {
+		/* Get the Buffer address and copy the transmit data.*/
+		eprambase = (u32 __force *)(udc->addr + ep->rambase +
+			     ep->ep_usb.maxpacket);
+		if (ep->is_in) {
+			memcpy(eprambase, bufferptr, bytestosend);
+			udc->write_fn(udc->addr, ep->offset +
+				      XUSB_EP_BUF1COUNT_OFFSET, bufferlen);
+		} else {
+			memcpy(bufferptr, eprambase, bytestosend);
+		}
+		/*
+		 * Enable the buffer for transmission.
+		 */
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET,
+			      1 << (ep->epnumber + XUSB_STATUS_EP_BUFF2_SHIFT));
+		ep->buffer1ready = 1;
+		ep->curbufnum = 0;
+	} else {
+		/* None of the ping-pong buffers are ready currently */
+		return -EAGAIN;
+	}
+	return rc;
+}
+
+/**
+ * xudc_done - Exeutes the endpoint data transfer completion tasks.
+ * @ep: pointer to the usb device endpoint structure.
+ * @req: pointer to the usb request structure.
+ * @status: Status of the data transfer.
+ *
+ * Deletes the message from the queue and updates data transfer completion
+ * status.
+ */
+static void xudc_done(struct xusb_ep *ep, struct xusb_req *req, int status)
+{
+	struct xusb_udc *udc = ep->udc;
+
+	list_del_init(&req->queue);
+
+	if (req->usb_req.status == -EINPROGRESS)
+		req->usb_req.status = status;
+	else
+		status = req->usb_req.status;
+
+	if (status && status != -ESHUTDOWN)
+		dev_dbg(udc->dev, "%s done %p, status %d\n",
+			ep->ep_usb.name, req, status);
+	/* unmap request if DMA is present*/
+	if (udc->dma_enabled && ep->epnumber && req->usb_req.length)
+		usb_gadget_unmap_request(&udc->gadget, &req->usb_req,
+					 ep->is_in);
+
+	if (req->usb_req.complete) {
+		spin_unlock(&udc->lock);
+		req->usb_req.complete(&ep->ep_usb, &req->usb_req);
+		spin_lock(&udc->lock);
+	}
+}
+
+/**
+ * xudc_read_fifo - Reads the data from the given endpoint buffer.
+ * @ep: pointer to the usb device endpoint structure.
+ * @req: pointer to the usb request structure.
+ *
+ * Return: 0 if request is completed and -EAGAIN if not completed.
+ *
+ * Pulls OUT packet data from the endpoint buffer.
+ */
+static int xudc_read_fifo(struct xusb_ep *ep, struct xusb_req *req)
+{
+	u8 *buf;
+	u32 is_short, count, bufferspace;
+	u8 bufoffset;
+	u8 two_pkts = 0;
+	int ret;
+	int retval = -EAGAIN;
+	struct xusb_udc *udc = ep->udc;
+
+	if (ep->buffer0ready && ep->buffer1ready) {
+		dev_dbg(udc->dev, "Packet NOT ready!\n");
+		return retval;
+	}
+top:
+	if (ep->curbufnum)
+		bufoffset = XUSB_EP_BUF1COUNT_OFFSET;
+	else
+		bufoffset = XUSB_EP_BUF0COUNT_OFFSET;
+
+	count = udc->read_fn(udc->addr + ep->offset + bufoffset);
+
+	if (!ep->buffer0ready && !ep->buffer1ready)
+		two_pkts = 1;
+
+	buf = req->usb_req.buf + req->usb_req.actual;
+	prefetchw(buf);
+	bufferspace = req->usb_req.length - req->usb_req.actual;
+	is_short = count < ep->ep_usb.maxpacket;
+
+	if (unlikely(!bufferspace)) {
+		/*
+		 * This happens when the driver's buffer
+		 * is smaller than what the host sent.
+		 * discard the extra data.
+		 */
+		if (req->usb_req.status != -EOVERFLOW)
+			dev_dbg(udc->dev, "%s overflow %d\n",
+				ep->ep_usb.name, count);
+		req->usb_req.status = -EOVERFLOW;
+		xudc_done(ep, req, -EOVERFLOW);
+		return 0;
+	}
+
+	ret = xudc_eptxrx(ep, req, buf, count);
+	switch (ret) {
+	case 0:
+		req->usb_req.actual += min(count, bufferspace);
+		dev_dbg(udc->dev, "read %s, %d bytes%s req %p %d/%d\n",
+			ep->ep_usb.name, count, is_short ? "/S" : "", req,
+			req->usb_req.actual, req->usb_req.length);
+		bufferspace -= count;
+		/* Completion */
+		if ((req->usb_req.actual == req->usb_req.length) || is_short) {
+			if (udc->dma_enabled && req->usb_req.length)
+				dma_sync_single_for_cpu(udc->dev,
+							req->usb_req.dma,
+							req->usb_req.actual,
+							DMA_FROM_DEVICE);
+			xudc_done(ep, req, 0);
+			return 0;
+		}
+		if (two_pkts) {
+			two_pkts = 0;
+			goto top;
+		}
+		break;
+	case -EAGAIN:
+		dev_dbg(udc->dev, "receive busy\n");
+		break;
+	case -EINVAL:
+	case -ETIMEDOUT:
+		/* DMA error, dequeue the request */
+		xudc_done(ep, req, -ECONNRESET);
+		retval = 0;
+		break;
+	}
+
+	return retval;
+}
+
+/**
+ * xudc_write_fifo - Writes data into the given endpoint buffer.
+ * @ep: pointer to the usb device endpoint structure.
+ * @req: pointer to the usb request structure.
+ *
+ * Return: 0 if request is completed and -EAGAIN if not completed.
+ *
+ * Loads endpoint buffer for an IN packet.
+ */
+static int xudc_write_fifo(struct xusb_ep *ep, struct xusb_req *req)
+{
+	u32 max;
+	u32 length;
+	int ret;
+	int retval = -EAGAIN;
+	struct xusb_udc *udc = ep->udc;
+	int is_last, is_short = 0;
+	u8 *buf;
+
+	max = le16_to_cpu(ep->desc->wMaxPacketSize);
+	buf = req->usb_req.buf + req->usb_req.actual;
+	prefetch(buf);
+	length = req->usb_req.length - req->usb_req.actual;
+	length = min(length, max);
+
+	ret = xudc_eptxrx(ep, req, buf, length);
+	switch (ret) {
+	case 0:
+		req->usb_req.actual += length;
+		if (unlikely(length != max)) {
+			is_last = is_short = 1;
+		} else {
+			if (likely(req->usb_req.length !=
+				   req->usb_req.actual) || req->usb_req.zero)
+				is_last = 0;
+			else
+				is_last = 1;
+		}
+		dev_dbg(udc->dev, "%s: wrote %s %d bytes%s%s %d left %p\n",
+			__func__, ep->ep_usb.name, length, is_last ? "/L" : "",
+			is_short ? "/S" : "",
+			req->usb_req.length - req->usb_req.actual, req);
+		/* completion */
+		if (is_last) {
+			xudc_done(ep, req, 0);
+			retval = 0;
+		}
+		break;
+	case -EAGAIN:
+		dev_dbg(udc->dev, "Send busy\n");
+		break;
+	case -EINVAL:
+	case -ETIMEDOUT:
+		/* DMA error, dequeue the request */
+		xudc_done(ep, req, -ECONNRESET);
+		retval = 0;
+		break;
+	}
+
+	return retval;
+}
+
+/**
+ * xudc_nuke - Cleans up the data transfer message list.
+ * @ep: pointer to the usb device endpoint structure.
+ * @status: Status of the data transfer.
+ */
+static void xudc_nuke(struct xusb_ep *ep, int status)
+{
+	struct xusb_req *req;
+
+	while (!list_empty(&ep->queue)) {
+		req = list_first_entry(&ep->queue, struct xusb_req, queue);
+		xudc_done(ep, req, status);
+	}
+}
+
+/**
+ * xudc_ep_set_halt - Stalls/unstalls the given endpoint.
+ * @_ep: pointer to the usb device endpoint structure.
+ * @value: value to indicate stall/unstall.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int xudc_ep_set_halt(struct usb_ep *_ep, int value)
+{
+	struct xusb_ep *ep = to_xusb_ep(_ep);
+	struct xusb_udc *udc;
+	unsigned long flags;
+	u32 epcfgreg;
+
+	if (!_ep || (!ep->desc && ep->epnumber)) {
+		pr_debug("%s: bad ep or descriptor\n", __func__);
+		return -EINVAL;
+	}
+	udc = ep->udc;
+
+	if (ep->is_in && (!list_empty(&ep->queue)) && value) {
+		dev_dbg(udc->dev, "requests pending can't halt\n");
+		return -EAGAIN;
+	}
+
+	if (ep->buffer0ready || ep->buffer1ready) {
+		dev_dbg(udc->dev, "HW buffers busy can't halt\n");
+		return -EAGAIN;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (value) {
+		/* Stall the device.*/
+		epcfgreg = udc->read_fn(udc->addr + ep->offset);
+		epcfgreg |= XUSB_EP_CFG_STALL_MASK;
+		udc->write_fn(udc->addr, ep->offset, epcfgreg);
+	} else {
+		/* Unstall the device.*/
+		epcfgreg = udc->read_fn(udc->addr + ep->offset);
+		epcfgreg &= ~XUSB_EP_CFG_STALL_MASK;
+		udc->write_fn(udc->addr, ep->offset, epcfgreg);
+		if (ep->epnumber) {
+			/* Reset the toggle bit.*/
+			epcfgreg = udc->read_fn(ep->udc->addr + ep->offset);
+			epcfgreg &= ~XUSB_EP_CFG_DATA_TOGGLE_MASK;
+			udc->write_fn(udc->addr, ep->offset, epcfgreg);
+		}
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return 0;
+}
+
+/**
+ * xudc_ep_enable - Enables the given endpoint.
+ * @ep: pointer to the xusb endpoint structure.
+ * @desc: pointer to usb endpoint descriptor.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int __xudc_ep_enable(struct xusb_ep *ep,
+			    const struct usb_endpoint_descriptor *desc)
+{
+	struct xusb_udc *udc = ep->udc;
+	u32 tmp;
+	u32 epcfg;
+	u32 ier;
+	u16 maxpacket;
+
+	ep->is_in = ((desc->bEndpointAddress & USB_DIR_IN) != 0);
+	/* Bit 3...0:endpoint number */
+	ep->epnumber = (desc->bEndpointAddress & 0x0f);
+	ep->desc = desc;
+	ep->ep_usb.desc = desc;
+	tmp = desc->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK;
+	ep->ep_usb.maxpacket = maxpacket = le16_to_cpu(desc->wMaxPacketSize);
+
+	switch (tmp) {
+	case USB_ENDPOINT_XFER_CONTROL:
+		dev_dbg(udc->dev, "only one control endpoint\n");
+		/* NON- ISO */
+		ep->is_iso = 0;
+		return -EINVAL;
+	case USB_ENDPOINT_XFER_INT:
+		/* NON- ISO */
+		ep->is_iso = 0;
+		if (maxpacket > 64) {
+			dev_dbg(udc->dev, "bogus maxpacket %d\n", maxpacket);
+			return -EINVAL;
+		}
+		break;
+	case USB_ENDPOINT_XFER_BULK:
+		/* NON- ISO */
+		ep->is_iso = 0;
+		if (!(is_power_of_2(maxpacket) && maxpacket >= 8 &&
+				maxpacket <= 512)) {
+			dev_dbg(udc->dev, "bogus maxpacket %d\n", maxpacket);
+			return -EINVAL;
+		}
+		break;
+	case USB_ENDPOINT_XFER_ISOC:
+		/* ISO */
+		ep->is_iso = 1;
+		break;
+	}
+
+	ep->buffer0ready = 0;
+	ep->buffer1ready = 0;
+	ep->curbufnum = 0;
+	ep->rambase = rambase[ep->epnumber];
+	xudc_epconfig(ep, udc);
+
+	dev_dbg(udc->dev, "Enable Endpoint %d max pkt is %d\n",
+		ep->epnumber, maxpacket);
+
+	/* Enable the End point.*/
+	epcfg = udc->read_fn(udc->addr + ep->offset);
+	epcfg |= XUSB_EP_CFG_VALID_MASK;
+	udc->write_fn(udc->addr, ep->offset, epcfg);
+	if (ep->epnumber)
+		ep->rambase <<= 2;
+
+	/* Enable buffer completion interrupts for endpoint */
+	ier = udc->read_fn(udc->addr + XUSB_IER_OFFSET);
+	ier |= (XUSB_STATUS_INTR_BUFF_COMP_SHIFT_MASK << ep->epnumber);
+	udc->write_fn(udc->addr, XUSB_IER_OFFSET, ier);
+
+	/* for OUT endpoint set buffers ready to receive */
+	if (ep->epnumber && !ep->is_in) {
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET,
+			      1 << ep->epnumber);
+		ep->buffer0ready = 1;
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET,
+			     (1 << (ep->epnumber +
+			      XUSB_STATUS_EP_BUFF2_SHIFT)));
+		ep->buffer1ready = 1;
+	}
+
+	return 0;
+}
+
+/**
+ * xudc_ep_enable - Enables the given endpoint.
+ * @_ep: pointer to the usb endpoint structure.
+ * @desc: pointer to usb endpoint descriptor.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int xudc_ep_enable(struct usb_ep *_ep,
+			  const struct usb_endpoint_descriptor *desc)
+{
+	struct xusb_ep *ep;
+	struct xusb_udc *udc;
+	unsigned long flags;
+	int ret;
+
+	if (!_ep || !desc || desc->bDescriptorType != USB_DT_ENDPOINT) {
+		pr_debug("%s: bad ep or descriptor\n", __func__);
+		return -EINVAL;
+	}
+
+	ep = to_xusb_ep(_ep);
+	udc = ep->udc;
+
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN) {
+		dev_dbg(udc->dev, "bogus device state\n");
+		return -ESHUTDOWN;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+	ret = __xudc_ep_enable(ep, desc);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return ret;
+}
+
+/**
+ * xudc_ep_disable - Disables the given endpoint.
+ * @_ep: pointer to the usb endpoint structure.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int xudc_ep_disable(struct usb_ep *_ep)
+{
+	struct xusb_ep *ep;
+	unsigned long flags;
+	u32 epcfg;
+	struct xusb_udc *udc;
+
+	if (!_ep) {
+		pr_debug("%s: invalid ep\n", __func__);
+		return -EINVAL;
+	}
+
+	ep = to_xusb_ep(_ep);
+	udc = ep->udc;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	xudc_nuke(ep, -ESHUTDOWN);
+
+	/* Restore the endpoint's pristine config */
+	ep->desc = NULL;
+	ep->ep_usb.desc = NULL;
+
+	dev_dbg(udc->dev, "USB Ep %d disable\n ", ep->epnumber);
+	/* Disable the endpoint.*/
+	epcfg = udc->read_fn(udc->addr + ep->offset);
+	epcfg &= ~XUSB_EP_CFG_VALID_MASK;
+	udc->write_fn(udc->addr, ep->offset, epcfg);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return 0;
+}
+
+/**
+ * xudc_ep_alloc_request - Initializes the request queue.
+ * @_ep: pointer to the usb endpoint structure.
+ * @gfp_flags: Flags related to the request call.
+ *
+ * Return: pointer to request structure on success and a NULL on failure.
+ */
+static struct usb_request *xudc_ep_alloc_request(struct usb_ep *_ep,
+						 gfp_t gfp_flags)
+{
+	struct xusb_ep *ep = to_xusb_ep(_ep);
+	struct xusb_udc *udc;
+	struct xusb_req *req;
+
+	udc = ep->udc;
+	req = kzalloc(sizeof(*req), gfp_flags);
+	if (!req) {
+		dev_err(udc->dev, "%s:not enough memory", __func__);
+		return NULL;
+	}
+
+	req->ep = ep;
+	INIT_LIST_HEAD(&req->queue);
+	return &req->usb_req;
+}
+
+/**
+ * xudc_free_request - Releases the request from queue.
+ * @_ep: pointer to the usb device endpoint structure.
+ * @_req: pointer to the usb request structure.
+ */
+static void xudc_free_request(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct xusb_req *req = to_xusb_req(_req);
+
+	kfree(req);
+}
+
+/**
+ * xudc_ep0_queue - Adds the request to endpoint 0 queue.
+ * @ep0: pointer to the xusb endpoint 0 structure.
+ * @req: pointer to the xusb request structure.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int __xudc_ep0_queue(struct xusb_ep *ep0, struct xusb_req *req)
+{
+	struct xusb_udc *udc = ep0->udc;
+	u32 length;
+	u8 *corebuf;
+
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN) {
+		dev_dbg(udc->dev, "%s, bogus device state\n", __func__);
+		return -EINVAL;
+	}
+	if (!list_empty(&ep0->queue)) {
+		dev_dbg(udc->dev, "%s:ep0 busy\n", __func__);
+		return -EBUSY;
+	}
+
+	req->usb_req.status = -EINPROGRESS;
+	req->usb_req.actual = 0;
+
+	list_add_tail(&req->queue, &ep0->queue);
+
+	if (udc->setup.bRequestType & USB_DIR_IN) {
+		prefetch(req->usb_req.buf);
+		length = req->usb_req.length;
+		corebuf = (void __force *) ((ep0->rambase << 2) +
+			   udc->addr);
+		length = req->usb_req.actual = min_t(u32, length,
+						     EP0_MAX_PACKET);
+		memcpy(corebuf, req->usb_req.buf, length);
+		udc->write_fn(udc->addr, XUSB_EP_BUF0COUNT_OFFSET, length);
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET, 1);
+	} else {
+		if (udc->setup.wLength) {
+			/* Enable EP0 buffer to receive data */
+			udc->write_fn(udc->addr, XUSB_EP_BUF0COUNT_OFFSET, 0);
+			udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET, 1);
+		} else {
+			xudc_wrstatus(udc);
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * xudc_ep0_queue - Adds the request to endpoint 0 queue.
+ * @_ep: pointer to the usb endpoint 0 structure.
+ * @_req: pointer to the usb request structure.
+ * @gfp_flags: Flags related to the request call.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int xudc_ep0_queue(struct usb_ep *_ep, struct usb_request *_req,
+			  gfp_t gfp_flags)
+{
+	struct xusb_req *req	= to_xusb_req(_req);
+	struct xusb_ep	*ep0	= to_xusb_ep(_ep);
+	struct xusb_udc *udc	= ep0->udc;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	ret = __xudc_ep0_queue(ep0, req);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return ret;
+}
+
+/**
+ * xudc_ep_queue - Adds the request to endpoint queue.
+ * @_ep: pointer to the usb endpoint structure.
+ * @_req: pointer to the usb request structure.
+ * @gfp_flags: Flags related to the request call.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int xudc_ep_queue(struct usb_ep *_ep, struct usb_request *_req,
+			 gfp_t gfp_flags)
+{
+	struct xusb_req *req = to_xusb_req(_req);
+	struct xusb_ep	*ep  = to_xusb_ep(_ep);
+	struct xusb_udc *udc = ep->udc;
+	int  ret;
+	unsigned long flags;
+
+	if (!ep->desc) {
+		dev_dbg(udc->dev, "%s:queing request to disabled %s\n",
+			__func__, ep->name);
+		return -ESHUTDOWN;
+	}
+
+	if (!udc->driver || udc->gadget.speed == USB_SPEED_UNKNOWN) {
+		dev_dbg(udc->dev, "%s, bogus device state\n", __func__);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	_req->status = -EINPROGRESS;
+	_req->actual = 0;
+
+	if (udc->dma_enabled) {
+		ret = usb_gadget_map_request(&udc->gadget, &req->usb_req,
+					     ep->is_in);
+		if (ret) {
+			dev_dbg(udc->dev, "gadget_map failed ep%d\n",
+				ep->epnumber);
+			spin_unlock_irqrestore(&udc->lock, flags);
+			return -EAGAIN;
+		}
+	}
+
+	if (list_empty(&ep->queue)) {
+		if (ep->is_in) {
+			dev_dbg(udc->dev, "xudc_write_fifo from ep_queue\n");
+			if (!xudc_write_fifo(ep, req))
+				req = NULL;
+		} else {
+			dev_dbg(udc->dev, "xudc_read_fifo from ep_queue\n");
+			if (!xudc_read_fifo(ep, req))
+				req = NULL;
+		}
+	}
+
+	if (req != NULL)
+		list_add_tail(&req->queue, &ep->queue);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return 0;
+}
+
+/**
+ * xudc_ep_dequeue - Removes the request from the queue.
+ * @_ep: pointer to the usb device endpoint structure.
+ * @_req: pointer to the usb request structure.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int xudc_ep_dequeue(struct usb_ep *_ep, struct usb_request *_req)
+{
+	struct xusb_ep *ep	= to_xusb_ep(_ep);
+	struct xusb_req *req	= to_xusb_req(_req);
+	struct xusb_udc *udc	= ep->udc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+	/* Make sure it's actually queued on this endpoint */
+	list_for_each_entry(req, &ep->queue, queue) {
+		if (&req->usb_req == _req)
+			break;
+	}
+	if (&req->usb_req != _req) {
+		spin_unlock_irqrestore(&ep->udc->lock, flags);
+		return -EINVAL;
+	}
+	xudc_done(ep, req, -ECONNRESET);
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/**
+ * xudc_ep0_enable - Enables the given endpoint.
+ * @ep: pointer to the usb endpoint structure.
+ * @desc: pointer to usb endpoint descriptor.
+ *
+ * Return: error always.
+ *
+ * endpoint 0 enable should not be called by gadget layer.
+ */
+static int xudc_ep0_enable(struct usb_ep *ep,
+			   const struct usb_endpoint_descriptor *desc)
+{
+	return -EINVAL;
+}
+
+/**
+ * xudc_ep0_disable - Disables the given endpoint.
+ * @ep: pointer to the usb endpoint structure.
+ *
+ * Return: error always.
+ *
+ * endpoint 0 disable should not be called by gadget layer.
+ */
+static int xudc_ep0_disable(struct usb_ep *ep)
+{
+	return -EINVAL;
+}
+
+static const struct usb_ep_ops xusb_ep0_ops = {
+	.enable		= xudc_ep0_enable,
+	.disable	= xudc_ep0_disable,
+	.alloc_request	= xudc_ep_alloc_request,
+	.free_request	= xudc_free_request,
+	.queue		= xudc_ep0_queue,
+	.dequeue	= xudc_ep_dequeue,
+	.set_halt	= xudc_ep_set_halt,
+};
+
+static const struct usb_ep_ops xusb_ep_ops = {
+	.enable		= xudc_ep_enable,
+	.disable	= xudc_ep_disable,
+	.alloc_request	= xudc_ep_alloc_request,
+	.free_request	= xudc_free_request,
+	.queue		= xudc_ep_queue,
+	.dequeue	= xudc_ep_dequeue,
+	.set_halt	= xudc_ep_set_halt,
+};
+
+/**
+ * xudc_get_frame - Reads the current usb frame number.
+ * @gadget: pointer to the usb gadget structure.
+ *
+ * Return: current frame number for success and error value on failure.
+ */
+static int xudc_get_frame(struct usb_gadget *gadget)
+{
+	struct xusb_udc *udc;
+	int frame;
+
+	if (!gadget)
+		return -ENODEV;
+
+	udc = to_udc(gadget);
+	frame = udc->read_fn(udc->addr + XUSB_FRAMENUM_OFFSET);
+	return frame;
+}
+
+/**
+ * xudc_wakeup - Send remote wakeup signal to host
+ * @gadget: pointer to the usb gadget structure.
+ *
+ * Return: 0 on success and error on failure
+ */
+static int xudc_wakeup(struct usb_gadget *gadget)
+{
+	struct xusb_udc *udc = to_udc(gadget);
+	u32 crtlreg;
+	int status = -EINVAL;
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/* Remote wake up not enabled by host */
+	if (!udc->remote_wkp)
+		goto done;
+
+	crtlreg = udc->read_fn(udc->addr + XUSB_CONTROL_OFFSET);
+	crtlreg |= XUSB_CONTROL_USB_RMTWAKE_MASK;
+	/* set remote wake up bit */
+	udc->write_fn(udc->addr, XUSB_CONTROL_OFFSET, crtlreg);
+	/*
+	 * wait for a while and reset remote wake up bit since this bit
+	 * is not cleared by HW after sending remote wakeup to host.
+	 */
+	mdelay(2);
+
+	crtlreg &= ~XUSB_CONTROL_USB_RMTWAKE_MASK;
+	udc->write_fn(udc->addr, XUSB_CONTROL_OFFSET, crtlreg);
+	status = 0;
+done:
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return status;
+}
+
+/**
+ * xudc_pullup - start/stop USB traffic
+ * @gadget: pointer to the usb gadget structure.
+ * @is_on: flag to start or stop
+ *
+ * Return: 0 always
+ *
+ * This function starts/stops SIE engine of IP based on is_on.
+ */
+static int xudc_pullup(struct usb_gadget *gadget, int is_on)
+{
+	struct xusb_udc *udc = to_udc(gadget);
+	unsigned long flags;
+	u32 crtlreg;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	crtlreg = udc->read_fn(udc->addr + XUSB_CONTROL_OFFSET);
+	if (is_on)
+		crtlreg |= XUSB_CONTROL_USB_READY_MASK;
+	else
+		crtlreg &= ~XUSB_CONTROL_USB_READY_MASK;
+
+	udc->write_fn(udc->addr, XUSB_CONTROL_OFFSET, crtlreg);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+/**
+ * xudc_eps_init - initialize endpoints.
+ * @udc: pointer to the usb device controller structure.
+ */
+static void xudc_eps_init(struct xusb_udc *udc)
+{
+	u32 ep_number;
+
+	INIT_LIST_HEAD(&udc->gadget.ep_list);
+
+	for (ep_number = 0; ep_number < XUSB_MAX_ENDPOINTS; ep_number++) {
+		struct xusb_ep *ep = &udc->ep[ep_number];
+
+		if (ep_number) {
+			list_add_tail(&ep->ep_usb.ep_list,
+				      &udc->gadget.ep_list);
+			usb_ep_set_maxpacket_limit(&ep->ep_usb,
+						  (unsigned short) ~0);
+			snprintf(ep->name, EPNAME_SIZE, "ep%d", ep_number);
+			ep->ep_usb.name = ep->name;
+			ep->ep_usb.ops = &xusb_ep_ops;
+		} else {
+			ep->ep_usb.name = ep0name;
+			usb_ep_set_maxpacket_limit(&ep->ep_usb, EP0_MAX_PACKET);
+			ep->ep_usb.ops = &xusb_ep0_ops;
+		}
+
+		ep->udc = udc;
+		ep->epnumber = ep_number;
+		ep->desc = NULL;
+		/*
+		 * The configuration register address offset between
+		 * each endpoint is 0x10.
+		 */
+		ep->offset = XUSB_EP0_CONFIG_OFFSET + (ep_number * 0x10);
+		ep->is_in = 0;
+		ep->is_iso = 0;
+		ep->maxpacket = 0;
+		xudc_epconfig(ep, udc);
+
+		/* Initialize one queue per endpoint */
+		INIT_LIST_HEAD(&ep->queue);
+	}
+}
+
+/**
+ * xudc_stop_activity - Stops any further activity on the device.
+ * @udc: pointer to the usb device controller structure.
+ */
+static void xudc_stop_activity(struct xusb_udc *udc)
+{
+	int i;
+	struct xusb_ep *ep;
+
+	for (i = 0; i < XUSB_MAX_ENDPOINTS; i++) {
+		ep = &udc->ep[i];
+		xudc_nuke(ep, -ESHUTDOWN);
+	}
+}
+
+/**
+ * xudc_start - Starts the device.
+ * @gadget: pointer to the usb gadget structure
+ * @driver: pointer to gadget driver structure
+ *
+ * Return: zero on success and error on failure
+ */
+static int xudc_start(struct usb_gadget *gadget,
+		      struct usb_gadget_driver *driver)
+{
+	struct xusb_udc *udc	= to_udc(gadget);
+	struct xusb_ep *ep0	= &udc->ep[XUSB_EP_NUMBER_ZERO];
+	const struct usb_endpoint_descriptor *desc = &config_bulk_out_desc;
+	unsigned long flags;
+	int ret = 0;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	if (udc->driver) {
+		dev_err(udc->dev, "%s is already bound to %s\n",
+			udc->gadget.name, udc->driver->driver.name);
+		ret = -EBUSY;
+		goto err;
+	}
+
+	/* hook up the driver */
+	udc->driver = driver;
+	udc->gadget.speed = driver->max_speed;
+
+	/* Enable the control endpoint. */
+	ret = __xudc_ep_enable(ep0, desc);
+
+	/* Set device address and remote wakeup to 0 */
+	udc->write_fn(udc->addr, XUSB_ADDRESS_OFFSET, 0);
+	udc->remote_wkp = 0;
+err:
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return ret;
+}
+
+/**
+ * xudc_stop - stops the device.
+ * @gadget: pointer to the usb gadget structure
+ * @driver: pointer to usb gadget driver structure
+ *
+ * Return: zero always
+ */
+static int xudc_stop(struct usb_gadget *gadget,
+		     struct usb_gadget_driver *driver)
+{
+	struct xusb_udc *udc = to_udc(gadget);
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->driver = NULL;
+
+	/* Set device address and remote wakeup to 0 */
+	udc->write_fn(udc->addr, XUSB_ADDRESS_OFFSET, 0);
+	udc->remote_wkp = 0;
+
+	xudc_stop_activity(udc);
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+
+	return 0;
+}
+
+static const struct usb_gadget_ops xusb_udc_ops = {
+	.get_frame	= xudc_get_frame,
+	.wakeup		= xudc_wakeup,
+	.pullup		= xudc_pullup,
+	.udc_start	= xudc_start,
+	.udc_stop	= xudc_stop,
+};
+
+/**
+ * xudc_clear_stall_all_ep - clears stall of every endpoint.
+ * @udc: pointer to the udc structure.
+ */
+static void xudc_clear_stall_all_ep(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep;
+	u32 epcfgreg;
+	int i;
+
+	for (i = 0; i < XUSB_MAX_ENDPOINTS; i++) {
+		ep = &udc->ep[i];
+		epcfgreg = udc->read_fn(udc->addr + ep->offset);
+		epcfgreg &= ~XUSB_EP_CFG_STALL_MASK;
+		udc->write_fn(udc->addr, ep->offset, epcfgreg);
+		if (ep->epnumber) {
+			/* Reset the toggle bit.*/
+			epcfgreg = udc->read_fn(udc->addr + ep->offset);
+			epcfgreg &= ~XUSB_EP_CFG_DATA_TOGGLE_MASK;
+			udc->write_fn(udc->addr, ep->offset, epcfgreg);
+		}
+	}
+}
+
+/**
+ * xudc_startup_handler - The usb device controller interrupt handler.
+ * @udc: pointer to the udc structure.
+ * @intrstatus: The mask value containing the interrupt sources.
+ *
+ * This function handles the RESET,SUSPEND,RESUME and DISCONNECT interrupts.
+ */
+static void xudc_startup_handler(struct xusb_udc *udc, u32 intrstatus)
+{
+	u32 intrreg;
+
+	if (intrstatus & XUSB_STATUS_RESET_MASK) {
+
+		dev_dbg(udc->dev, "Reset\n");
+
+		if (intrstatus & XUSB_STATUS_HIGH_SPEED_MASK)
+			udc->gadget.speed = USB_SPEED_HIGH;
+		else
+			udc->gadget.speed = USB_SPEED_FULL;
+
+		xudc_stop_activity(udc);
+		xudc_clear_stall_all_ep(udc);
+		udc->write_fn(udc->addr, XUSB_TESTMODE_OFFSET, 0);
+
+		/* Set device address and remote wakeup to 0 */
+		udc->write_fn(udc->addr, XUSB_ADDRESS_OFFSET, 0);
+		udc->remote_wkp = 0;
+
+		/* Enable the suspend, resume and disconnect */
+		intrreg = udc->read_fn(udc->addr + XUSB_IER_OFFSET);
+		intrreg |= XUSB_STATUS_SUSPEND_MASK | XUSB_STATUS_RESUME_MASK |
+			   XUSB_STATUS_DISCONNECT_MASK;
+		udc->write_fn(udc->addr, XUSB_IER_OFFSET, intrreg);
+	}
+	if (intrstatus & XUSB_STATUS_SUSPEND_MASK) {
+
+		dev_dbg(udc->dev, "Suspend\n");
+
+		/* Enable the reset, resume and disconnect */
+		intrreg = udc->read_fn(udc->addr + XUSB_IER_OFFSET);
+		intrreg |= XUSB_STATUS_RESET_MASK | XUSB_STATUS_RESUME_MASK |
+			   XUSB_STATUS_DISCONNECT_MASK;
+		udc->write_fn(udc->addr, XUSB_IER_OFFSET, intrreg);
+
+		udc->usb_state = USB_STATE_SUSPENDED;
+
+		if (udc->driver->suspend) {
+			spin_unlock(&udc->lock);
+			udc->driver->suspend(&udc->gadget);
+			spin_lock(&udc->lock);
+		}
+	}
+	if (intrstatus & XUSB_STATUS_RESUME_MASK) {
+		bool condition = (udc->usb_state != USB_STATE_SUSPENDED);
+
+		dev_WARN_ONCE(udc->dev, condition,
+				"Resume IRQ while not suspended\n");
+
+		dev_dbg(udc->dev, "Resume\n");
+
+		/* Enable the reset, suspend and disconnect */
+		intrreg = udc->read_fn(udc->addr + XUSB_IER_OFFSET);
+		intrreg |= XUSB_STATUS_RESET_MASK | XUSB_STATUS_SUSPEND_MASK |
+			   XUSB_STATUS_DISCONNECT_MASK;
+		udc->write_fn(udc->addr, XUSB_IER_OFFSET, intrreg);
+
+		udc->usb_state = 0;
+
+		if (udc->driver->resume) {
+			spin_unlock(&udc->lock);
+			udc->driver->resume(&udc->gadget);
+			spin_lock(&udc->lock);
+		}
+	}
+	if (intrstatus & XUSB_STATUS_DISCONNECT_MASK) {
+
+		dev_dbg(udc->dev, "Disconnect\n");
+
+		/* Enable the reset, resume and suspend */
+		intrreg = udc->read_fn(udc->addr + XUSB_IER_OFFSET);
+		intrreg |= XUSB_STATUS_RESET_MASK | XUSB_STATUS_RESUME_MASK |
+			   XUSB_STATUS_SUSPEND_MASK;
+		udc->write_fn(udc->addr, XUSB_IER_OFFSET, intrreg);
+
+		if (udc->driver && udc->driver->disconnect) {
+			spin_unlock(&udc->lock);
+			udc->driver->disconnect(&udc->gadget);
+			spin_lock(&udc->lock);
+		}
+	}
+}
+
+/**
+ * xudc_ep0_stall - Stall endpoint zero.
+ * @udc: pointer to the udc structure.
+ *
+ * This function stalls endpoint zero.
+ */
+static void xudc_ep0_stall(struct xusb_udc *udc)
+{
+	u32 epcfgreg;
+	struct xusb_ep *ep0 = &udc->ep[XUSB_EP_NUMBER_ZERO];
+
+	epcfgreg = udc->read_fn(udc->addr + ep0->offset);
+	epcfgreg |= XUSB_EP_CFG_STALL_MASK;
+	udc->write_fn(udc->addr, ep0->offset, epcfgreg);
+}
+
+/**
+ * xudc_setaddress - executes SET_ADDRESS command
+ * @udc: pointer to the udc structure.
+ *
+ * This function executes USB SET_ADDRESS command
+ */
+static void xudc_setaddress(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep0	= &udc->ep[0];
+	struct xusb_req *req	= udc->req;
+	int ret;
+
+	req->usb_req.length = 0;
+	ret = __xudc_ep0_queue(ep0, req);
+	if (ret == 0)
+		return;
+
+	dev_err(udc->dev, "Can't respond to SET ADDRESS request\n");
+	xudc_ep0_stall(udc);
+}
+
+/**
+ * xudc_getstatus - executes GET_STATUS command
+ * @udc: pointer to the udc structure.
+ *
+ * This function executes USB GET_STATUS command
+ */
+static void xudc_getstatus(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep0	= &udc->ep[0];
+	struct xusb_req *req	= udc->req;
+	struct xusb_ep *target_ep;
+	u16 status = 0;
+	u32 epcfgreg;
+	int epnum;
+	u32 halt;
+	int ret;
+
+	switch (udc->setup.bRequestType & USB_RECIP_MASK) {
+	case USB_RECIP_DEVICE:
+		/* Get device status */
+		status = 1 << USB_DEVICE_SELF_POWERED;
+		if (udc->remote_wkp)
+			status |= (1 << USB_DEVICE_REMOTE_WAKEUP);
+		break;
+	case USB_RECIP_INTERFACE:
+		break;
+	case USB_RECIP_ENDPOINT:
+		epnum = udc->setup.wIndex & USB_ENDPOINT_NUMBER_MASK;
+		target_ep = &udc->ep[epnum];
+		epcfgreg = udc->read_fn(udc->addr + target_ep->offset);
+		halt = epcfgreg & XUSB_EP_CFG_STALL_MASK;
+		if (udc->setup.wIndex & USB_DIR_IN) {
+			if (!target_ep->is_in)
+				goto stall;
+		} else {
+			if (target_ep->is_in)
+				goto stall;
+		}
+		if (halt)
+			status = 1 << USB_ENDPOINT_HALT;
+		break;
+	default:
+		goto stall;
+	}
+
+	req->usb_req.length = 2;
+	*(u16 *)req->usb_req.buf = cpu_to_le16(status);
+	ret = __xudc_ep0_queue(ep0, req);
+	if (ret == 0)
+		return;
+stall:
+	dev_err(udc->dev, "Can't respond to getstatus request\n");
+	xudc_ep0_stall(udc);
+}
+
+/**
+ * xudc_set_clear_feature - Executes the set feature and clear feature commands.
+ * @udc: pointer to the usb device controller structure.
+ *
+ * Processes the SET_FEATURE and CLEAR_FEATURE commands.
+ */
+static void xudc_set_clear_feature(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep0	= &udc->ep[0];
+	struct xusb_req *req	= udc->req;
+	struct xusb_ep *target_ep;
+	u8 endpoint;
+	u8 outinbit;
+	u32 epcfgreg;
+	int flag = (udc->setup.bRequest == USB_REQ_SET_FEATURE ? 1 : 0);
+	int ret;
+
+	switch (udc->setup.bRequestType) {
+	case USB_RECIP_DEVICE:
+		switch (udc->setup.wValue) {
+		case USB_DEVICE_TEST_MODE:
+			/*
+			 * The Test Mode will be executed
+			 * after the status phase.
+			 */
+			break;
+		case USB_DEVICE_REMOTE_WAKEUP:
+			if (flag)
+				udc->remote_wkp = 1;
+			else
+				udc->remote_wkp = 0;
+			break;
+		default:
+			xudc_ep0_stall(udc);
+			break;
+		}
+		break;
+	case USB_RECIP_ENDPOINT:
+		if (!udc->setup.wValue) {
+			endpoint = udc->setup.wIndex & USB_ENDPOINT_NUMBER_MASK;
+			target_ep = &udc->ep[endpoint];
+			outinbit = udc->setup.wIndex & USB_ENDPOINT_DIR_MASK;
+			outinbit = outinbit >> 7;
+
+			/* Make sure direction matches.*/
+			if (outinbit != target_ep->is_in) {
+				xudc_ep0_stall(udc);
+				return;
+			}
+			epcfgreg = udc->read_fn(udc->addr + target_ep->offset);
+			if (!endpoint) {
+				/* Clear the stall.*/
+				epcfgreg &= ~XUSB_EP_CFG_STALL_MASK;
+				udc->write_fn(udc->addr,
+					      target_ep->offset, epcfgreg);
+			} else {
+				if (flag) {
+					epcfgreg |= XUSB_EP_CFG_STALL_MASK;
+					udc->write_fn(udc->addr,
+						      target_ep->offset,
+						      epcfgreg);
+				} else {
+					/* Unstall the endpoint.*/
+					epcfgreg &= ~(XUSB_EP_CFG_STALL_MASK |
+						XUSB_EP_CFG_DATA_TOGGLE_MASK);
+					udc->write_fn(udc->addr,
+						      target_ep->offset,
+						      epcfgreg);
+				}
+			}
+		}
+		break;
+	default:
+		xudc_ep0_stall(udc);
+		return;
+	}
+
+	req->usb_req.length = 0;
+	ret = __xudc_ep0_queue(ep0, req);
+	if (ret == 0)
+		return;
+
+	dev_err(udc->dev, "Can't respond to SET/CLEAR FEATURE\n");
+	xudc_ep0_stall(udc);
+}
+
+/**
+ * xudc_handle_setup - Processes the setup packet.
+ * @udc: pointer to the usb device controller structure.
+ *
+ * Process setup packet and delegate to gadget layer.
+ */
+static void xudc_handle_setup(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep0 = &udc->ep[0];
+	struct usb_ctrlrequest setup;
+	u32 *ep0rambase;
+
+	/* Load up the chapter 9 command buffer.*/
+	ep0rambase = (u32 __force *) (udc->addr + XUSB_SETUP_PKT_ADDR_OFFSET);
+	memcpy(&setup, ep0rambase, 8);
+
+	udc->setup = setup;
+	udc->setup.wValue = cpu_to_le16(setup.wValue);
+	udc->setup.wIndex = cpu_to_le16(setup.wIndex);
+	udc->setup.wLength = cpu_to_le16(setup.wLength);
+
+	/* Clear previous requests */
+	xudc_nuke(ep0, -ECONNRESET);
+
+	if (udc->setup.bRequestType & USB_DIR_IN) {
+		/* Execute the get command.*/
+		udc->setupseqrx = STATUS_PHASE;
+		udc->setupseqtx = DATA_PHASE;
+	} else {
+		/* Execute the put command.*/
+		udc->setupseqrx = DATA_PHASE;
+		udc->setupseqtx = STATUS_PHASE;
+	}
+
+	switch (udc->setup.bRequest) {
+	case USB_REQ_GET_STATUS:
+		/* Data+Status phase form udc */
+		if ((udc->setup.bRequestType &
+				(USB_DIR_IN | USB_TYPE_MASK)) !=
+				(USB_DIR_IN | USB_TYPE_STANDARD))
+			break;
+		xudc_getstatus(udc);
+		return;
+	case USB_REQ_SET_ADDRESS:
+		/* Status phase from udc */
+		if (udc->setup.bRequestType != (USB_DIR_OUT |
+				USB_TYPE_STANDARD | USB_RECIP_DEVICE))
+			break;
+		xudc_setaddress(udc);
+		return;
+	case USB_REQ_CLEAR_FEATURE:
+	case USB_REQ_SET_FEATURE:
+		/* Requests with no data phase, status phase from udc */
+		if ((udc->setup.bRequestType & USB_TYPE_MASK)
+				!= USB_TYPE_STANDARD)
+			break;
+		xudc_set_clear_feature(udc);
+		return;
+	default:
+		break;
+	}
+
+	spin_unlock(&udc->lock);
+	if (udc->driver->setup(&udc->gadget, &setup) < 0)
+		xudc_ep0_stall(udc);
+	spin_lock(&udc->lock);
+}
+
+/**
+ * xudc_ep0_out - Processes the endpoint 0 OUT token.
+ * @udc: pointer to the usb device controller structure.
+ */
+static void xudc_ep0_out(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep0 = &udc->ep[0];
+	struct xusb_req *req;
+	u8 *ep0rambase;
+	unsigned int bytes_to_rx;
+	void *buffer;
+
+	req = list_first_entry(&ep0->queue, struct xusb_req, queue);
+
+	switch (udc->setupseqrx) {
+	case STATUS_PHASE:
+		/*
+		 * This resets both state machines for the next
+		 * Setup packet.
+		 */
+		udc->setupseqrx = SETUP_PHASE;
+		udc->setupseqtx = SETUP_PHASE;
+		req->usb_req.actual = req->usb_req.length;
+		xudc_done(ep0, req, 0);
+		break;
+	case DATA_PHASE:
+		bytes_to_rx = udc->read_fn(udc->addr +
+					   XUSB_EP_BUF0COUNT_OFFSET);
+		/* Copy the data to be received from the DPRAM. */
+		ep0rambase = (u8 __force *) (udc->addr +
+			     (ep0->rambase << 2));
+		buffer = req->usb_req.buf + req->usb_req.actual;
+		req->usb_req.actual = req->usb_req.actual + bytes_to_rx;
+		memcpy(buffer, ep0rambase, bytes_to_rx);
+
+		if (req->usb_req.length == req->usb_req.actual) {
+			/* Data transfer completed get ready for Status stage */
+			xudc_wrstatus(udc);
+		} else {
+			/* Enable EP0 buffer to receive data */
+			udc->write_fn(udc->addr, XUSB_EP_BUF0COUNT_OFFSET, 0);
+			udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET, 1);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+/**
+ * xudc_ep0_in - Processes the endpoint 0 IN token.
+ * @udc: pointer to the usb device controller structure.
+ */
+static void xudc_ep0_in(struct xusb_udc *udc)
+{
+	struct xusb_ep *ep0 = &udc->ep[0];
+	struct xusb_req *req;
+	unsigned int bytes_to_tx;
+	void *buffer;
+	u32 epcfgreg;
+	u16 count = 0;
+	u16 length;
+	u8 *ep0rambase;
+	u8 test_mode = udc->setup.wIndex >> 8;
+
+	req = list_first_entry(&ep0->queue, struct xusb_req, queue);
+	bytes_to_tx = req->usb_req.length - req->usb_req.actual;
+
+	switch (udc->setupseqtx) {
+	case STATUS_PHASE:
+		switch (udc->setup.bRequest) {
+		case USB_REQ_SET_ADDRESS:
+			/* Set the address of the device.*/
+			udc->write_fn(udc->addr, XUSB_ADDRESS_OFFSET,
+				      udc->setup.wValue);
+			break;
+		case USB_REQ_SET_FEATURE:
+			if (udc->setup.bRequestType ==
+					USB_RECIP_DEVICE) {
+				if (udc->setup.wValue ==
+						USB_DEVICE_TEST_MODE)
+					udc->write_fn(udc->addr,
+						      XUSB_TESTMODE_OFFSET,
+						      test_mode);
+			}
+			break;
+		}
+		req->usb_req.actual = req->usb_req.length;
+		xudc_done(ep0, req, 0);
+		break;
+	case DATA_PHASE:
+		if (!bytes_to_tx) {
+			/*
+			 * We're done with data transfer, next
+			 * will be zero length OUT with data toggle of
+			 * 1. Setup data_toggle.
+			 */
+			epcfgreg = udc->read_fn(udc->addr + ep0->offset);
+			epcfgreg |= XUSB_EP_CFG_DATA_TOGGLE_MASK;
+			udc->write_fn(udc->addr, ep0->offset, epcfgreg);
+			udc->setupseqtx = STATUS_PHASE;
+		} else {
+			length = count = min_t(u32, bytes_to_tx,
+					       EP0_MAX_PACKET);
+			/* Copy the data to be transmitted into the DPRAM. */
+			ep0rambase = (u8 __force *) (udc->addr +
+				     (ep0->rambase << 2));
+			buffer = req->usb_req.buf + req->usb_req.actual;
+			req->usb_req.actual = req->usb_req.actual + length;
+			memcpy(ep0rambase, buffer, length);
+		}
+		udc->write_fn(udc->addr, XUSB_EP_BUF0COUNT_OFFSET, count);
+		udc->write_fn(udc->addr, XUSB_BUFFREADY_OFFSET, 1);
+		break;
+	default:
+		break;
+	}
+}
+
+/**
+ * xudc_ctrl_ep_handler - Endpoint 0 interrupt handler.
+ * @udc: pointer to the udc structure.
+ * @intrstatus:	It's the mask value for the interrupt sources on endpoint 0.
+ *
+ * Processes the commands received during enumeration phase.
+ */
+static void xudc_ctrl_ep_handler(struct xusb_udc *udc, u32 intrstatus)
+{
+
+	if (intrstatus & XUSB_STATUS_SETUP_PACKET_MASK) {
+		xudc_handle_setup(udc);
+	} else {
+		if (intrstatus & XUSB_STATUS_FIFO_BUFF_RDY_MASK)
+			xudc_ep0_out(udc);
+		else if (intrstatus & XUSB_STATUS_FIFO_BUFF_FREE_MASK)
+			xudc_ep0_in(udc);
+	}
+}
+
+/**
+ * xudc_nonctrl_ep_handler - Non control endpoint interrupt handler.
+ * @udc: pointer to the udc structure.
+ * @epnum: End point number for which the interrupt is to be processed
+ * @intrstatus:	mask value for interrupt sources of endpoints other
+ *		than endpoint 0.
+ *
+ * Processes the buffer completion interrupts.
+ */
+static void xudc_nonctrl_ep_handler(struct xusb_udc *udc, u8 epnum,
+				    u32 intrstatus)
+{
+
+	struct xusb_req *req;
+	struct xusb_ep *ep;
+
+	ep = &udc->ep[epnum];
+	/* Process the End point interrupts.*/
+	if (intrstatus & (XUSB_STATUS_EP0_BUFF1_COMP_MASK << epnum))
+		ep->buffer0ready = 0;
+	if (intrstatus & (XUSB_STATUS_EP0_BUFF2_COMP_MASK << epnum))
+		ep->buffer1ready = 0;
+
+	if (list_empty(&ep->queue))
+		return;
+
+	req = list_first_entry(&ep->queue, struct xusb_req, queue);
+
+	if (ep->is_in)
+		xudc_write_fifo(ep, req);
+	else
+		xudc_read_fifo(ep, req);
+}
+
+/**
+ * xudc_irq - The main interrupt handler.
+ * @irq: The interrupt number.
+ * @_udc: pointer to the usb device controller structure.
+ *
+ * Return: IRQ_HANDLED after the interrupt is handled.
+ */
+static irqreturn_t xudc_irq(int irq, void *_udc)
+{
+	struct xusb_udc *udc = _udc;
+	u32 intrstatus;
+	u32 ier;
+	u8 index;
+	u32 bufintr;
+	unsigned long flags;
+
+	spin_lock_irqsave(&udc->lock, flags);
+
+	/*
+	 * Event interrupts are level sensitive hence first disable
+	 * IER, read ISR and figure out active interrupts.
+	 */
+	ier = udc->read_fn(udc->addr + XUSB_IER_OFFSET);
+	ier &= ~XUSB_STATUS_INTR_EVENT_MASK;
+	udc->write_fn(udc->addr, XUSB_IER_OFFSET, ier);
+
+	/* Read the Interrupt Status Register.*/
+	intrstatus = udc->read_fn(udc->addr + XUSB_STATUS_OFFSET);
+
+	/* Call the handler for the event interrupt.*/
+	if (intrstatus & XUSB_STATUS_INTR_EVENT_MASK) {
+		/*
+		 * Check if there is any action to be done for :
+		 * - USB Reset received {XUSB_STATUS_RESET_MASK}
+		 * - USB Suspend received {XUSB_STATUS_SUSPEND_MASK}
+		 * - USB Resume received {XUSB_STATUS_RESUME_MASK}
+		 * - USB Disconnect received {XUSB_STATUS_DISCONNECT_MASK}
+		 */
+		xudc_startup_handler(udc, intrstatus);
+	}
+
+	/* Check the buffer completion interrupts */
+	if (intrstatus & XUSB_STATUS_INTR_BUFF_COMP_ALL_MASK) {
+		/* Enable Reset, Suspend, Resume and Disconnect  */
+		ier = udc->read_fn(udc->addr + XUSB_IER_OFFSET);
+		ier |= XUSB_STATUS_INTR_EVENT_MASK;
+		udc->write_fn(udc->addr, XUSB_IER_OFFSET, ier);
+
+		if (intrstatus & XUSB_STATUS_EP0_BUFF1_COMP_MASK)
+			xudc_ctrl_ep_handler(udc, intrstatus);
+
+		for (index = 1; index < 8; index++) {
+			bufintr = ((intrstatus &
+				  (XUSB_STATUS_EP1_BUFF1_COMP_MASK <<
+				  (index - 1))) || (intrstatus &
+				  (XUSB_STATUS_EP1_BUFF2_COMP_MASK <<
+				  (index - 1))));
+			if (bufintr) {
+				xudc_nonctrl_ep_handler(udc, index,
+							intrstatus);
+			}
+		}
+	}
+
+	spin_unlock_irqrestore(&udc->lock, flags);
+	return IRQ_HANDLED;
+}
+
+/**
+ * xudc_probe - The device probe function for driver initialization.
+ * @pdev: pointer to the platform device structure.
+ *
+ * Return: 0 for success and error value on failure
+ */
+static int xudc_probe(struct platform_device *pdev)
+{
+	struct device_node *np = pdev->dev.of_node;
+	struct resource *res;
+	struct xusb_udc *udc;
+	struct xusb_ep *ep0;
+	int irq;
+	int ret;
+	u32 ier;
+	u8 *buff;
+
+	udc = devm_kzalloc(&pdev->dev, sizeof(*udc), GFP_KERNEL);
+	if (!udc)
+		return -ENOMEM;
+
+	/* Create a dummy request for GET_STATUS, SET_ADDRESS */
+	udc->req = devm_kzalloc(&pdev->dev, sizeof(struct xusb_req),
+				GFP_KERNEL);
+	if (!udc->req)
+		return -ENOMEM;
+
+	buff = devm_kzalloc(&pdev->dev, STATUSBUFF_SIZE, GFP_KERNEL);
+	if (!buff)
+		return -ENOMEM;
+
+	udc->req->usb_req.buf = buff;
+
+	/* Map the registers */
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	udc->addr = devm_ioremap_resource(&pdev->dev, res);
+	if (!udc->addr)
+		return -ENOMEM;
+
+	irq = platform_get_irq(pdev, 0);
+	if (irq < 0) {
+		dev_err(&pdev->dev, "unable to get irq\n");
+		return irq;
+	}
+	ret = devm_request_irq(&pdev->dev, irq, xudc_irq, 0,
+			       dev_name(&pdev->dev), udc);
+	if (ret < 0) {
+		dev_dbg(&pdev->dev, "unable to request irq %d", irq);
+		goto fail;
+	}
+
+	udc->dma_enabled = of_property_read_bool(np, "xlnx,has-builtin-dma");
+
+	/* Setup gadget structure */
+	udc->gadget.ops = &xusb_udc_ops;
+	udc->gadget.max_speed = USB_SPEED_HIGH;
+	udc->gadget.speed = USB_SPEED_UNKNOWN;
+	udc->gadget.ep0 = &udc->ep[XUSB_EP_NUMBER_ZERO].ep_usb;
+	udc->gadget.name = driver_name;
+
+	spin_lock_init(&udc->lock);
+
+	/* Check for IP endianness */
+	udc->write_fn = xudc_write32_be;
+	udc->read_fn = xudc_read32_be;
+	udc->write_fn(udc->addr, XUSB_TESTMODE_OFFSET, TEST_J);
+	if ((udc->read_fn(udc->addr + XUSB_TESTMODE_OFFSET))
+			!= TEST_J) {
+		udc->write_fn = xudc_write32;
+		udc->read_fn = xudc_read32;
+	}
+	udc->write_fn(udc->addr, XUSB_TESTMODE_OFFSET, 0);
+
+	xudc_eps_init(udc);
+
+	ep0 = &udc->ep[0];
+
+	/* Set device address to 0.*/
+	udc->write_fn(udc->addr, XUSB_ADDRESS_OFFSET, 0);
+
+	ret = usb_add_gadget_udc(&pdev->dev, &udc->gadget);
+	if (ret)
+		goto fail;
+
+	udc->dev = &udc->gadget.dev;
+
+	/* Enable the interrupts.*/
+	ier = XUSB_STATUS_GLOBAL_INTR_MASK | XUSB_STATUS_INTR_EVENT_MASK |
+	      XUSB_STATUS_FIFO_BUFF_RDY_MASK | XUSB_STATUS_FIFO_BUFF_FREE_MASK |
+	      XUSB_STATUS_SETUP_PACKET_MASK |
+	      XUSB_STATUS_INTR_BUFF_COMP_ALL_MASK;
+
+	udc->write_fn(udc->addr, XUSB_IER_OFFSET, ier);
+
+	platform_set_drvdata(pdev, udc);
+
+	dev_vdbg(&pdev->dev, "%s at 0x%08X mapped to 0x%08X %s\n",
+		 driver_name, (u32)res->start, (u32 __force)udc->addr,
+		 udc->dma_enabled ? "with DMA" : "without DMA");
+
+	return 0;
+fail:
+	dev_err(&pdev->dev, "probe failed, %d\n", ret);
+	return ret;
+}
+
+/**
+ * xudc_remove - Releases the resources allocated during the initialization.
+ * @pdev: pointer to the platform device structure.
+ *
+ * Return: 0 always
+ */
+static int xudc_remove(struct platform_device *pdev)
+{
+	struct xusb_udc *udc = platform_get_drvdata(pdev);
+
+	usb_del_gadget_udc(&udc->gadget);
+
+	return 0;
+}
+
+/* Match table for of_platform binding */
+static const struct of_device_id usb_of_match[] = {
+	{ .compatible = "xlnx,usb2-device-4.00.a", },
+	{ /* end of list */ },
+};
+MODULE_DEVICE_TABLE(of, usb_of_match);
+
+static struct platform_driver xudc_driver = {
+	.driver = {
+		.name = driver_name,
+		.of_match_table = usb_of_match,
+	},
+	.probe = xudc_probe,
+	.remove = xudc_remove,
+};
+
+module_platform_driver(xudc_driver);
+
+MODULE_DESCRIPTION("Xilinx udc driver");
+MODULE_AUTHOR("Xilinx, Inc");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/Kconfig b/drivers/usb/host/Kconfig
index 82800a775501..a8a30b1d4167 100644
--- a/drivers/usb/host/Kconfig
+++ b/drivers/usb/host/Kconfig
@@ -26,6 +26,11 @@ config USB_XHCI_HCD
 
 if USB_XHCI_HCD
 
+config USB_XHCI_PCI
+       tristate
+       depends on PCI
+       default y
+
 config USB_XHCI_PLATFORM
 	tristate
 
@@ -126,7 +131,7 @@ config XPS_USB_HCD_XILINX
 	select USB_EHCI_BIG_ENDIAN_DESC
 	select USB_EHCI_BIG_ENDIAN_MMIO
 	---help---
-		Xilinx xps USB host controller core is EHCI compilant and has
+		Xilinx xps USB host controller core is EHCI compliant and has
 		transaction translator built-in. It can be configured to either
 		support both high speed and full speed devices, or high speed
 		devices only.
@@ -174,6 +179,15 @@ config USB_EHCI_HCD_SPEAR
           Enables support for the on-chip EHCI controller on
           ST SPEAr chips.
 
+config USB_EHCI_HCD_STI
+	tristate "Support for ST STiHxxx on-chip EHCI USB controller"
+	depends on ARCH_STI && OF
+	select GENERIC_PHY
+	select USB_EHCI_HCD_PLATFORM
+	help
+	  Enable support for the on-chip EHCI controller found on
+	  STMicroelectronics consumer electronics SoC's.
+
 config USB_EHCI_HCD_AT91
         tristate  "Support for Atmel on-chip EHCI USB controller"
         depends on USB_EHCI_HCD && ARCH_AT91
@@ -402,6 +416,15 @@ config USB_OHCI_HCD_SPEAR
           Enables support for the on-chip OHCI controller on
           ST SPEAr chips.
 
+config USB_OHCI_HCD_STI
+	tristate "Support for ST STiHxxx on-chip OHCI USB controller"
+	depends on ARCH_STI && OF
+	select GENERIC_PHY
+	select USB_OHCI_HCD_PLATFORM
+	help
+	  Enable support for the on-chip OHCI controller found on
+	  STMicroelectronics consumer electronics SoC's.
+
 config USB_OHCI_HCD_S3C2410
         tristate "OHCI support for Samsung S3C24xx/S3C64xx SoC series"
         depends on USB_OHCI_HCD && (ARCH_S3C24XX || ARCH_S3C64XX)
@@ -709,7 +732,7 @@ config USB_WHCI_HCD
 
 config USB_HWA_HCD
 	tristate "Host Wire Adapter (HWA) driver"
-	depends on UWB
+	depends on USB && UWB
 	select USB_WUSB
 	select UWB_HWA
 	help
diff --git a/drivers/usb/host/Makefile b/drivers/usb/host/Makefile
index 144c038ef70f..348c24321562 100644
--- a/drivers/usb/host/Makefile
+++ b/drivers/usb/host/Makefile
@@ -15,22 +15,22 @@ fhci-$(CONFIG_FHCI_DEBUG) += fhci-dbg.o
 xhci-hcd-y := xhci.o xhci-mem.o
 xhci-hcd-y += xhci-ring.o xhci-hub.o xhci-dbg.o
 xhci-hcd-y += xhci-trace.o
-xhci-hcd-$(CONFIG_PCI)	+= xhci-pci.o
 
-ifneq ($(CONFIG_USB_XHCI_PLATFORM), )
-	xhci-hcd-y		+= xhci-plat.o
+xhci-plat-hcd-y := xhci-plat.o
 ifneq ($(CONFIG_USB_XHCI_MVEBU), )
-	xhci-hcd-y		+= xhci-mvebu.o
+	xhci-plat-hcd-y		+= xhci-mvebu.o
 endif
 ifneq ($(CONFIG_USB_XHCI_RCAR), )
-	xhci-hcd-y		+= xhci-rcar.o
-endif
+	xhci-plat-hcd-y		+= xhci-rcar.o
 endif
 
 obj-$(CONFIG_USB_WHCI_HCD)	+= whci/
 
 obj-$(CONFIG_PCI)		+= pci-quirks.o
 
+obj-$(CONFIG_USB_XHCI_PCI)	+= xhci-pci.o
+obj-$(CONFIG_USB_XHCI_PLATFORM) += xhci-plat-hcd.o
+
 obj-$(CONFIG_USB_EHCI_HCD)	+= ehci-hcd.o
 obj-$(CONFIG_USB_EHCI_PCI)	+= ehci-pci.o
 obj-$(CONFIG_USB_EHCI_HCD_PLATFORM)	+= ehci-platform.o
@@ -38,6 +38,7 @@ obj-$(CONFIG_USB_EHCI_MXC)	+= ehci-mxc.o
 obj-$(CONFIG_USB_EHCI_HCD_OMAP)	+= ehci-omap.o
 obj-$(CONFIG_USB_EHCI_HCD_ORION)	+= ehci-orion.o
 obj-$(CONFIG_USB_EHCI_HCD_SPEAR)	+= ehci-spear.o
+obj-$(CONFIG_USB_EHCI_HCD_STI)	+= ehci-st.o
 obj-$(CONFIG_USB_EHCI_EXYNOS)	+= ehci-exynos.o
 obj-$(CONFIG_USB_EHCI_HCD_AT91) += ehci-atmel.o
 obj-$(CONFIG_USB_EHCI_MSM)	+= ehci-msm.o
@@ -55,6 +56,7 @@ obj-$(CONFIG_USB_OHCI_EXYNOS)	+= ohci-exynos.o
 obj-$(CONFIG_USB_OHCI_HCD_OMAP1)	+= ohci-omap.o
 obj-$(CONFIG_USB_OHCI_HCD_OMAP3)	+= ohci-omap3.o
 obj-$(CONFIG_USB_OHCI_HCD_SPEAR)	+= ohci-spear.o
+obj-$(CONFIG_USB_OHCI_HCD_STI)	+= ohci-st.o
 obj-$(CONFIG_USB_OHCI_HCD_AT91)	+= ohci-at91.o
 obj-$(CONFIG_USB_OHCI_HCD_S3C2410)	+= ohci-s3c2410.o
 obj-$(CONFIG_USB_OHCI_HCD_LPC32XX)	+= ohci-nxp.o
diff --git a/drivers/usb/host/bcma-hcd.c b/drivers/usb/host/bcma-hcd.c
index 205f4a336583..cd6d0afb6b8f 100644
--- a/drivers/usb/host/bcma-hcd.c
+++ b/drivers/usb/host/bcma-hcd.c
@@ -237,7 +237,7 @@ static int bcma_hcd_probe(struct bcma_device *dev)
 	bcma_hcd_init_chip(dev);
 
 	/* In AI chips EHCI is addrspace 0, OHCI is 1 */
-	ohci_addr = dev->addr1;
+	ohci_addr = dev->addr_s[0];
 	if ((chipinfo->id == 0x5357 || chipinfo->id == 0x4749)
 	    && chipinfo->rev == 0)
 		ohci_addr = 0x18009000;
diff --git a/drivers/usb/host/ehci-exynos.c b/drivers/usb/host/ehci-exynos.c
index cda0a2f5c467..7189f2e32ac2 100644
--- a/drivers/usb/host/ehci-exynos.c
+++ b/drivers/usb/host/ehci-exynos.c
@@ -21,11 +21,8 @@
 #include <linux/of_gpio.h>
 #include <linux/phy/phy.h>
 #include <linux/platform_device.h>
-#include <linux/usb/phy.h>
-#include <linux/usb/samsung_usb_phy.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
-#include <linux/usb/otg.h>
 
 #include "ehci.h"
 
@@ -47,9 +44,7 @@ static struct hc_driver __read_mostly exynos_ehci_hc_driver;
 
 struct exynos_ehci_hcd {
 	struct clk *clk;
-	struct usb_phy *phy;
-	struct usb_otg *otg;
-	struct phy *phy_g[PHY_NUMBER];
+	struct phy *phy[PHY_NUMBER];
 };
 
 #define to_exynos_ehci(hcd) (struct exynos_ehci_hcd *)(hcd_to_ehci(hcd)->priv)
@@ -60,20 +55,9 @@ static int exynos_ehci_get_phy(struct device *dev,
 	struct device_node *child;
 	struct phy *phy;
 	int phy_number;
-	int ret = 0;
-
-	exynos_ehci->phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR(exynos_ehci->phy)) {
-		ret = PTR_ERR(exynos_ehci->phy);
-		if (ret != -ENXIO && ret != -ENODEV) {
-			dev_err(dev, "no usb2 phy configured\n");
-			return ret;
-		}
-		dev_dbg(dev, "Failed to get usb2 phy\n");
-	} else {
-		exynos_ehci->otg = exynos_ehci->phy->otg;
-	}
+	int ret;
 
+	/* Get PHYs for the controller */
 	for_each_available_child_of_node(dev->of_node, child) {
 		ret = of_property_read_u32(child, "reg", &phy_number);
 		if (ret) {
@@ -89,19 +73,21 @@ static int exynos_ehci_get_phy(struct device *dev,
 		}
 
 		phy = devm_of_phy_get(dev, child, NULL);
+		exynos_ehci->phy[phy_number] = phy;
 		of_node_put(child);
 		if (IS_ERR(phy)) {
 			ret = PTR_ERR(phy);
-			if (ret != -ENOSYS && ret != -ENODEV) {
-				dev_err(dev, "no usb2 phy configured\n");
+			if (ret == -EPROBE_DEFER) {
+				return ret;
+			} else if (ret != -ENOSYS && ret != -ENODEV) {
+				dev_err(dev,
+					"Error retrieving usb2 phy: %d\n", ret);
 				return ret;
 			}
-			dev_dbg(dev, "Failed to get usb2 phy\n");
 		}
-		exynos_ehci->phy_g[phy_number] = phy;
 	}
 
-	return ret;
+	return 0;
 }
 
 static int exynos_ehci_phy_enable(struct device *dev)
@@ -111,16 +97,13 @@ static int exynos_ehci_phy_enable(struct device *dev)
 	int i;
 	int ret = 0;
 
-	if (!IS_ERR(exynos_ehci->phy))
-		return usb_phy_init(exynos_ehci->phy);
-
 	for (i = 0; ret == 0 && i < PHY_NUMBER; i++)
-		if (!IS_ERR(exynos_ehci->phy_g[i]))
-			ret = phy_power_on(exynos_ehci->phy_g[i]);
+		if (!IS_ERR(exynos_ehci->phy[i]))
+			ret = phy_power_on(exynos_ehci->phy[i]);
 	if (ret)
 		for (i--; i >= 0; i--)
-			if (!IS_ERR(exynos_ehci->phy_g[i]))
-				phy_power_off(exynos_ehci->phy_g[i]);
+			if (!IS_ERR(exynos_ehci->phy[i]))
+				phy_power_off(exynos_ehci->phy[i]);
 
 	return ret;
 }
@@ -131,14 +114,9 @@ static void exynos_ehci_phy_disable(struct device *dev)
 	struct exynos_ehci_hcd *exynos_ehci = to_exynos_ehci(hcd);
 	int i;
 
-	if (!IS_ERR(exynos_ehci->phy)) {
-		usb_phy_shutdown(exynos_ehci->phy);
-		return;
-	}
-
 	for (i = 0; i < PHY_NUMBER; i++)
-		if (!IS_ERR(exynos_ehci->phy_g[i]))
-			phy_power_off(exynos_ehci->phy_g[i]);
+		if (!IS_ERR(exynos_ehci->phy[i]))
+			phy_power_off(exynos_ehci->phy[i]);
 }
 
 static void exynos_setup_vbus_gpio(struct device *dev)
@@ -231,9 +209,6 @@ skip_phy:
 		goto fail_io;
 	}
 
-	if (exynos_ehci->otg)
-		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
-
 	err = exynos_ehci_phy_enable(&pdev->dev);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to enable USB phy\n");
@@ -273,9 +248,6 @@ static int exynos_ehci_remove(struct platform_device *pdev)
 
 	usb_remove_hcd(hcd);
 
-	if (exynos_ehci->otg)
-		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
-
 	exynos_ehci_phy_disable(&pdev->dev);
 
 	clk_disable_unprepare(exynos_ehci->clk);
@@ -298,9 +270,6 @@ static int exynos_ehci_suspend(struct device *dev)
 	if (rc)
 		return rc;
 
-	if (exynos_ehci->otg)
-		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
-
 	exynos_ehci_phy_disable(dev);
 
 	clk_disable_unprepare(exynos_ehci->clk);
@@ -316,9 +285,6 @@ static int exynos_ehci_resume(struct device *dev)
 
 	clk_prepare_enable(exynos_ehci->clk);
 
-	if (exynos_ehci->otg)
-		exynos_ehci->otg->set_host(exynos_ehci->otg, &hcd->self);
-
 	ret = exynos_ehci_phy_enable(dev);
 	if (ret) {
 		dev_err(dev, "Failed to enable USB phy\n");
diff --git a/drivers/usb/host/ehci-fsl.c b/drivers/usb/host/ehci-fsl.c
index cf2734b532a7..2d2ae8db439e 100644
--- a/drivers/usb/host/ehci-fsl.c
+++ b/drivers/usb/host/ehci-fsl.c
@@ -136,15 +136,15 @@ static int usb_hcd_fsl_probe(const struct hc_driver *driver,
 	if (pdata->operating_mode == FSL_USB2_DR_OTG) {
 		struct ehci_hcd *ehci = hcd_to_ehci(hcd);
 
-		hcd->phy = usb_get_phy(USB_PHY_TYPE_USB2);
+		hcd->usb_phy = usb_get_phy(USB_PHY_TYPE_USB2);
 		dev_dbg(&pdev->dev, "hcd=0x%p  ehci=0x%p, phy=0x%p\n",
-			hcd, ehci, hcd->phy);
+			hcd, ehci, hcd->usb_phy);
 
-		if (!IS_ERR_OR_NULL(hcd->phy)) {
-			retval = otg_set_host(hcd->phy->otg,
+		if (!IS_ERR_OR_NULL(hcd->usb_phy)) {
+			retval = otg_set_host(hcd->usb_phy->otg,
 					      &ehci_to_hcd(ehci)->self);
 			if (retval) {
-				usb_put_phy(hcd->phy);
+				usb_put_phy(hcd->usb_phy);
 				goto err2;
 			}
 		} else {
@@ -181,9 +181,9 @@ static void usb_hcd_fsl_remove(struct usb_hcd *hcd,
 {
 	struct fsl_usb2_platform_data *pdata = dev_get_platdata(&pdev->dev);
 
-	if (!IS_ERR_OR_NULL(hcd->phy)) {
-		otg_set_host(hcd->phy->otg, NULL);
-		usb_put_phy(hcd->phy);
+	if (!IS_ERR_OR_NULL(hcd->usb_phy)) {
+		otg_set_host(hcd->usb_phy->otg, NULL);
+		usb_put_phy(hcd->usb_phy);
 	}
 
 	usb_remove_hcd(hcd);
@@ -627,7 +627,7 @@ static int ehci_start_port_reset(struct usb_hcd *hcd, unsigned port)
 	if (!(status & PORT_CONNECT))
 		return -ENODEV;
 
-	/* khubd will finish the reset later */
+	/* hub_wq will finish the reset later */
 	if (ehci_is_TDI(ehci)) {
 		writel(PORT_RESET |
 		       (status & ~(PORT_CSC | PORT_PEC | PORT_OCC)),
diff --git a/drivers/usb/host/ehci-hcd.c b/drivers/usb/host/ehci-hcd.c
index 81cda09b47e3..15feaf924b71 100644
--- a/drivers/usb/host/ehci-hcd.c
+++ b/drivers/usb/host/ehci-hcd.c
@@ -788,7 +788,7 @@ static irqreturn_t ehci_irq (struct usb_hcd *hcd)
 				continue;
 
 			/* start 20 msec resume signaling from this port,
-			 * and make khubd collect PORT_STAT_C_SUSPEND to
+			 * and make hub_wq collect PORT_STAT_C_SUSPEND to
 			 * stop that signaling.  Use 5 ms extra for safety,
 			 * like usb_port_resume() does.
 			 */
@@ -965,8 +965,6 @@ rescan:
 	}
 
 	qh->exception = 1;
-	if (ehci->rh_state < EHCI_RH_RUNNING)
-		qh->qh_state = QH_STATE_IDLE;
 	switch (qh->qh_state) {
 	case QH_STATE_LINKED:
 		WARN_ON(!list_empty(&qh->qtd_list));
diff --git a/drivers/usb/host/ehci-hub.c b/drivers/usb/host/ehci-hub.c
index cc305c71ac3d..5728829cf6ef 100644
--- a/drivers/usb/host/ehci-hub.c
+++ b/drivers/usb/host/ehci-hub.c
@@ -656,7 +656,7 @@ ehci_hub_status_data (struct usb_hcd *hcd, char *buf)
 
 		/*
 		 * Return status information even for ports with OWNER set.
-		 * Otherwise khubd wouldn't see the disconnect event when a
+		 * Otherwise hub_wq wouldn't see the disconnect event when a
 		 * high-speed device is switched over to the companion
 		 * controller by the user.
 		 */
@@ -902,7 +902,7 @@ int ehci_hub_control(
 
 		/*
 		 * Even if OWNER is set, so the port is owned by the
-		 * companion controller, khubd needs to be able to clear
+		 * companion controller, hub_wq needs to be able to clear
 		 * the port-change status bits (especially
 		 * USB_PORT_STAT_C_CONNECTION).
 		 */
@@ -922,7 +922,7 @@ int ehci_hub_control(
 #ifdef CONFIG_USB_OTG
 			if ((hcd->self.otg_port == (wIndex + 1))
 			    && hcd->self.b_hnp_enable) {
-				otg_start_hnp(hcd->phy->otg);
+				otg_start_hnp(hcd->usb_phy->otg);
 				break;
 			}
 #endif
@@ -1000,7 +1000,7 @@ int ehci_hub_control(
 			 * However, not all EHCI implementations do this
 			 * automatically, even if they _do_ support per-port
 			 * power switching; they're allowed to just limit the
-			 * current.  khubd will turn the power back on.
+			 * current.  hub_wq will turn the power back on.
 			 */
 			if (((temp & PORT_OC) || (ehci->need_oc_pp_cycle))
 					&& HCS_PPC(ehci->hcs_params)) {
@@ -1085,7 +1085,7 @@ int ehci_hub_control(
 		}
 
 		/*
-		 * Even if OWNER is set, there's no harm letting khubd
+		 * Even if OWNER is set, there's no harm letting hub_wq
 		 * see the wPortStatus values (they should all be 0 except
 		 * for PORT_POWER anyway).
 		 */
@@ -1230,7 +1230,7 @@ int ehci_hub_control(
 			if (selector == EHSET_TEST_SINGLE_STEP_SET_FEATURE) {
 				spin_unlock_irqrestore(&ehci->lock, flags);
 				retval = ehset_single_step_set_feature(hcd,
-									wIndex);
+								wIndex + 1);
 				spin_lock_irqsave(&ehci->lock, flags);
 				break;
 			}
diff --git a/drivers/usb/host/ehci-msm.c b/drivers/usb/host/ehci-msm.c
index 934b39d5e44a..9dc2118ae808 100644
--- a/drivers/usb/host/ehci-msm.c
+++ b/drivers/usb/host/ehci-msm.c
@@ -124,7 +124,7 @@ static int ehci_msm_probe(struct platform_device *pdev)
 		goto put_hcd;
 	}
 
-	hcd->phy = phy;
+	hcd->usb_phy = phy;
 	device_init_wakeup(&pdev->dev, 1);
 	/*
 	 * OTG device parent of HCD takes care of putting
@@ -151,7 +151,7 @@ static int ehci_msm_remove(struct platform_device *pdev)
 	pm_runtime_disable(&pdev->dev);
 	pm_runtime_set_suspended(&pdev->dev);
 
-	otg_set_host(hcd->phy->otg, NULL);
+	otg_set_host(hcd->usb_phy->otg, NULL);
 
 	/* FIXME: need to call usb_remove_hcd() here? */
 
diff --git a/drivers/usb/host/ehci-st.c b/drivers/usb/host/ehci-st.c
new file mode 100644
index 000000000000..7e4bd39cf757
--- /dev/null
+++ b/drivers/usb/host/ehci-st.c
@@ -0,0 +1,375 @@
+/*
+ * ST EHCI driver
+ *
+ * Copyright (C) 2014 STMicroelectronics – All Rights Reserved
+ *
+ * Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ * Derived from ehci-platform.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/err.h>
+#include <linux/kernel.h>
+#include <linux/hrtimer.h>
+#include <linux/io.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+#include <linux/usb/ehci_pdriver.h>
+
+#include "ehci.h"
+
+#define USB_MAX_CLKS 3
+
+struct st_ehci_platform_priv {
+	struct clk *clks[USB_MAX_CLKS];
+	struct clk *clk48;
+	struct reset_control *rst;
+	struct reset_control *pwr;
+	struct phy *phy;
+};
+
+#define DRIVER_DESC "EHCI STMicroelectronics driver"
+
+#define hcd_to_ehci_priv(h) \
+	((struct st_ehci_platform_priv *)hcd_to_ehci(h)->priv)
+
+static const char hcd_name[] = "ehci-st";
+
+#define EHCI_CAPS_SIZE 0x10
+#define AHB2STBUS_INSREG01 (EHCI_CAPS_SIZE + 0x84)
+
+static int st_ehci_platform_reset(struct usb_hcd *hcd)
+{
+	struct platform_device *pdev = to_platform_device(hcd->self.controller);
+	struct usb_ehci_pdata *pdata = dev_get_platdata(&pdev->dev);
+	struct ehci_hcd *ehci = hcd_to_ehci(hcd);
+	int retval;
+	u32 threshold;
+
+	/* Set EHCI packet buffer IN/OUT threshold to 128 bytes */
+	threshold = 128 | (128 << 16);
+	writel(threshold, hcd->regs + AHB2STBUS_INSREG01);
+
+	ehci->caps = hcd->regs + pdata->caps_offset;
+	retval = ehci_setup(hcd);
+	if (retval)
+		return retval;
+
+	return 0;
+}
+
+static int st_ehci_platform_power_on(struct platform_device *dev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(dev);
+	struct st_ehci_platform_priv *priv = hcd_to_ehci_priv(hcd);
+	int clk, ret;
+
+	ret = reset_control_deassert(priv->pwr);
+	if (ret)
+		return ret;
+
+	ret = reset_control_deassert(priv->rst);
+	if (ret)
+		goto err_assert_power;
+
+	/* some SoCs don't have a dedicated 48Mhz clock, but those that do
+	   need the rate to be explicitly set */
+	if (priv->clk48) {
+		ret = clk_set_rate(priv->clk48, 48000000);
+		if (ret)
+			goto err_assert_reset;
+	}
+
+	for (clk = 0; clk < USB_MAX_CLKS && priv->clks[clk]; clk++) {
+		ret = clk_prepare_enable(priv->clks[clk]);
+		if (ret)
+			goto err_disable_clks;
+	}
+
+	ret = phy_init(priv->phy);
+	if (ret)
+		goto err_disable_clks;
+
+	ret = phy_power_on(priv->phy);
+	if (ret)
+		goto err_exit_phy;
+
+	return 0;
+
+err_exit_phy:
+	phy_exit(priv->phy);
+err_disable_clks:
+	while (--clk >= 0)
+		clk_disable_unprepare(priv->clks[clk]);
+err_assert_reset:
+	reset_control_assert(priv->rst);
+err_assert_power:
+	reset_control_assert(priv->pwr);
+
+	return ret;
+}
+
+static void st_ehci_platform_power_off(struct platform_device *dev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(dev);
+	struct st_ehci_platform_priv *priv = hcd_to_ehci_priv(hcd);
+	int clk;
+
+	reset_control_assert(priv->pwr);
+
+	reset_control_assert(priv->rst);
+
+	phy_power_off(priv->phy);
+
+	phy_exit(priv->phy);
+
+	for (clk = USB_MAX_CLKS - 1; clk >= 0; clk--)
+		if (priv->clks[clk])
+			clk_disable_unprepare(priv->clks[clk]);
+
+}
+
+static struct hc_driver __read_mostly ehci_platform_hc_driver;
+
+static const struct ehci_driver_overrides platform_overrides __initconst = {
+	.reset =		st_ehci_platform_reset,
+	.extra_priv_size =	sizeof(struct st_ehci_platform_priv),
+};
+
+static struct usb_ehci_pdata ehci_platform_defaults = {
+	.power_on =		st_ehci_platform_power_on,
+	.power_suspend =	st_ehci_platform_power_off,
+	.power_off =		st_ehci_platform_power_off,
+};
+
+static int st_ehci_platform_probe(struct platform_device *dev)
+{
+	struct usb_hcd *hcd;
+	struct resource *res_mem;
+	struct usb_ehci_pdata *pdata = &ehci_platform_defaults;
+	struct st_ehci_platform_priv *priv;
+	struct ehci_hcd *ehci;
+	int err, irq, clk = 0;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	irq = platform_get_irq(dev, 0);
+	if (irq < 0) {
+		dev_err(&dev->dev, "no irq provided");
+		return irq;
+	}
+	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!res_mem) {
+		dev_err(&dev->dev, "no memory resource provided");
+		return -ENXIO;
+	}
+
+	hcd = usb_create_hcd(&ehci_platform_hc_driver, &dev->dev,
+			     dev_name(&dev->dev));
+	if (!hcd)
+		return -ENOMEM;
+
+	platform_set_drvdata(dev, hcd);
+	dev->dev.platform_data = pdata;
+	priv = hcd_to_ehci_priv(hcd);
+	ehci = hcd_to_ehci(hcd);
+
+	priv->phy = devm_phy_get(&dev->dev, "usb");
+	if (IS_ERR(priv->phy)) {
+		err = PTR_ERR(priv->phy);
+		goto err_put_hcd;
+	}
+
+	for (clk = 0; clk < USB_MAX_CLKS; clk++) {
+		priv->clks[clk] = of_clk_get(dev->dev.of_node, clk);
+		if (IS_ERR(priv->clks[clk])) {
+			err = PTR_ERR(priv->clks[clk]);
+			if (err == -EPROBE_DEFER)
+				goto err_put_clks;
+			priv->clks[clk] = NULL;
+			break;
+		}
+	}
+
+	/* some SoCs don't have a dedicated 48Mhz clock, but those that
+	   do need the rate to be explicitly set */
+	priv->clk48 = devm_clk_get(&dev->dev, "clk48");
+	if (IS_ERR(priv->clk48)) {
+		dev_info(&dev->dev, "48MHz clk not found\n");
+		priv->clk48 = NULL;
+	}
+
+	priv->pwr = devm_reset_control_get_optional(&dev->dev, "power");
+	if (IS_ERR(priv->pwr)) {
+		err = PTR_ERR(priv->pwr);
+		if (err == -EPROBE_DEFER)
+			goto err_put_clks;
+		priv->pwr = NULL;
+	}
+
+	priv->rst = devm_reset_control_get_optional(&dev->dev, "softreset");
+	if (IS_ERR(priv->rst)) {
+		err = PTR_ERR(priv->rst);
+		if (err == -EPROBE_DEFER)
+			goto err_put_clks;
+		priv->rst = NULL;
+	}
+
+	if (pdata->power_on) {
+		err = pdata->power_on(dev);
+		if (err < 0)
+			goto err_put_clks;
+	}
+
+	hcd->rsrc_start = res_mem->start;
+	hcd->rsrc_len = resource_size(res_mem);
+
+	hcd->regs = devm_ioremap_resource(&dev->dev, res_mem);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
+		goto err_put_clks;
+	}
+
+	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
+	if (err)
+		goto err_put_clks;
+
+	device_wakeup_enable(hcd->self.controller);
+	platform_set_drvdata(dev, hcd);
+
+	return err;
+
+err_put_clks:
+	while (--clk >= 0)
+		clk_put(priv->clks[clk]);
+err_put_hcd:
+	if (pdata == &ehci_platform_defaults)
+		dev->dev.platform_data = NULL;
+
+	usb_put_hcd(hcd);
+
+	return err;
+}
+
+static int st_ehci_platform_remove(struct platform_device *dev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(dev);
+	struct usb_ehci_pdata *pdata = dev_get_platdata(&dev->dev);
+	struct st_ehci_platform_priv *priv = hcd_to_ehci_priv(hcd);
+	int clk;
+
+	usb_remove_hcd(hcd);
+
+	if (pdata->power_off)
+		pdata->power_off(dev);
+
+	for (clk = 0; clk < USB_MAX_CLKS && priv->clks[clk]; clk++)
+		clk_put(priv->clks[clk]);
+
+	usb_put_hcd(hcd);
+
+	if (pdata == &ehci_platform_defaults)
+		dev->dev.platform_data = NULL;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+
+static int st_ehci_suspend(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct usb_ehci_pdata *pdata = dev_get_platdata(dev);
+	struct platform_device *pdev =
+		container_of(dev, struct platform_device, dev);
+	bool do_wakeup = device_may_wakeup(dev);
+	int ret;
+
+	ret = ehci_suspend(hcd, do_wakeup);
+	if (ret)
+		return ret;
+
+	if (pdata->power_suspend)
+		pdata->power_suspend(pdev);
+
+	pinctrl_pm_select_sleep_state(dev);
+
+	return ret;
+}
+
+static int st_ehci_resume(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct usb_ehci_pdata *pdata = dev_get_platdata(dev);
+	struct platform_device *pdev =
+		container_of(dev, struct platform_device, dev);
+	int err;
+
+	pinctrl_pm_select_default_state(dev);
+
+	if (pdata->power_on) {
+		err = pdata->power_on(pdev);
+		if (err < 0)
+			return err;
+	}
+
+	ehci_resume(hcd, false);
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(st_ehci_pm_ops, st_ehci_suspend, st_ehci_resume);
+
+#endif /* CONFIG_PM_SLEEP */
+
+static const struct of_device_id st_ehci_ids[] = {
+	{ .compatible = "st,st-ehci-300x", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, st_ehci_ids);
+
+static struct platform_driver ehci_platform_driver = {
+	.probe		= st_ehci_platform_probe,
+	.remove		= st_ehci_platform_remove,
+	.shutdown	= usb_hcd_platform_shutdown,
+	.driver		= {
+		.name	= "st-ehci",
+#ifdef CONFIG_PM_SLEEP
+		.pm	= &st_ehci_pm_ops,
+#endif
+		.of_match_table = st_ehci_ids,
+	}
+};
+
+static int __init ehci_platform_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+
+	ehci_init_driver(&ehci_platform_hc_driver, &platform_overrides);
+	return platform_driver_register(&ehci_platform_driver);
+}
+module_init(ehci_platform_init);
+
+static void __exit ehci_platform_cleanup(void)
+{
+	platform_driver_unregister(&ehci_platform_driver);
+}
+module_exit(ehci_platform_cleanup);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Peter Griffin <peter.griffin@linaro.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/ehci-tegra.c b/drivers/usb/host/ehci-tegra.c
index 7aafb05e7a40..aaa01971efe9 100644
--- a/drivers/usb/host/ehci-tegra.c
+++ b/drivers/usb/host/ehci-tegra.c
@@ -206,7 +206,7 @@ static int tegra_ehci_hub_control(
 		if (tegra->port_resuming && !(temp & PORT_SUSPEND)) {
 			/* Resume completed, re-enable disconnect detection */
 			tegra->port_resuming = 0;
-			tegra_usb_phy_postresume(hcd->phy);
+			tegra_usb_phy_postresume(hcd->usb_phy);
 		}
 	}
 
@@ -259,7 +259,7 @@ static int tegra_ehci_hub_control(
 			goto done;
 
 		/* Disable disconnect detection during port resume */
-		tegra_usb_phy_preresume(hcd->phy);
+		tegra_usb_phy_preresume(hcd->usb_phy);
 
 		ehci->reset_done[wIndex-1] = jiffies + msecs_to_jiffies(25);
 
@@ -454,7 +454,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 		err = PTR_ERR(u_phy);
 		goto cleanup_clk_en;
 	}
-	hcd->phy = u_phy;
+	hcd->usb_phy = u_phy;
 
 	tegra->needs_double_reset = of_property_read_bool(pdev->dev.of_node,
 		"nvidia,needs-double-reset");
@@ -475,7 +475,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	ehci->caps = hcd->regs + 0x100;
 	ehci->has_hostpc = soc_config->has_hostpc;
 
-	err = usb_phy_init(hcd->phy);
+	err = usb_phy_init(hcd->usb_phy);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to initialize phy\n");
 		goto cleanup_clk_en;
@@ -490,7 +490,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 	}
 	u_phy->otg->host = hcd_to_bus(hcd);
 
-	err = usb_phy_set_suspend(hcd->phy, 0);
+	err = usb_phy_set_suspend(hcd->usb_phy, 0);
 	if (err) {
 		dev_err(&pdev->dev, "Failed to power on the phy\n");
 		goto cleanup_phy;
@@ -517,7 +517,7 @@ static int tegra_ehci_probe(struct platform_device *pdev)
 cleanup_otg_set_host:
 	otg_set_host(u_phy->otg, NULL);
 cleanup_phy:
-	usb_phy_shutdown(hcd->phy);
+	usb_phy_shutdown(hcd->usb_phy);
 cleanup_clk_en:
 	clk_disable_unprepare(tegra->clk);
 cleanup_hcd_create:
@@ -531,9 +531,9 @@ static int tegra_ehci_remove(struct platform_device *pdev)
 	struct tegra_ehci_hcd *tegra =
 		(struct tegra_ehci_hcd *)hcd_to_ehci(hcd)->priv;
 
-	otg_set_host(hcd->phy->otg, NULL);
+	otg_set_host(hcd->usb_phy->otg, NULL);
 
-	usb_phy_shutdown(hcd->phy);
+	usb_phy_shutdown(hcd->usb_phy);
 	usb_remove_hcd(hcd);
 
 	clk_disable_unprepare(tegra->clk);
diff --git a/drivers/usb/host/ehci-xilinx-of.c b/drivers/usb/host/ehci-xilinx-of.c
index fe57710753e8..a2328361dc80 100644
--- a/drivers/usb/host/ehci-xilinx-of.c
+++ b/drivers/usb/host/ehci-xilinx-of.c
@@ -31,6 +31,7 @@
 #include <linux/of.h>
 #include <linux/of_platform.h>
 #include <linux/of_address.h>
+#include <linux/of_irq.h>
 
 /**
  * ehci_xilinx_port_handed_over - hand the port out if failed to enable it
diff --git a/drivers/usb/host/fhci-hcd.c b/drivers/usb/host/fhci-hcd.c
index 1cf68eaf2ed8..a1a1ef521436 100644
--- a/drivers/usb/host/fhci-hcd.c
+++ b/drivers/usb/host/fhci-hcd.c
@@ -360,12 +360,12 @@ static int fhci_start(struct usb_hcd *hcd)
 	hcd->state = HC_STATE_RUNNING;
 
 	/*
-	 * From here on, khubd concurrently accesses the root
+	 * From here on, hub_wq concurrently accesses the root
 	 * hub; drivers will be talking to enumerated devices.
-	 * (On restart paths, khubd already knows about the root
+	 * (On restart paths, hub_wq already knows about the root
 	 * hub and could find work as soon as we wrote FLAG_CF.)
 	 *
-	 * Before this point the HC was idle/ready.  After, khubd
+	 * Before this point the HC was idle/ready.  After, hub_wq
 	 * and device drivers may start it running.
 	 */
 	fhci_usb_enable(fhci);
diff --git a/drivers/usb/host/fotg210-hcd.c b/drivers/usb/host/fotg210-hcd.c
index adcd2050dced..3de1278677d0 100644
--- a/drivers/usb/host/fotg210-hcd.c
+++ b/drivers/usb/host/fotg210-hcd.c
@@ -1483,7 +1483,7 @@ fotg210_hub_status_data(struct usb_hcd *hcd, char *buf)
 
 	/*
 	 * Return status information even for ports with OWNER set.
-	 * Otherwise khubd wouldn't see the disconnect event when a
+	 * Otherwise hub_wq wouldn't see the disconnect event when a
 	 * high-speed device is switched over to the companion
 	 * controller by the user.
 	 */
@@ -1572,7 +1572,7 @@ static int fotg210_hub_control(
 
 		/*
 		 * Even if OWNER is set, so the port is owned by the
-		 * companion controller, khubd needs to be able to clear
+		 * companion controller, hub_wq needs to be able to clear
 		 * the port-change status bits (especially
 		 * USB_PORT_STAT_C_CONNECTION).
 		 */
@@ -1723,7 +1723,7 @@ static int fotg210_hub_control(
 		}
 
 		/*
-		 * Even if OWNER is set, there's no harm letting khubd
+		 * Even if OWNER is set, there's no harm letting hub_wq
 		 * see the wPortStatus values (they should all be 0 except
 		 * for PORT_POWER anyway).
 		 */
@@ -5445,7 +5445,7 @@ static irqreturn_t fotg210_irq(struct usb_hcd *hcd)
 				fotg210->reset_done[0] == 0) {
 
 			/* start 20 msec resume signaling from this port,
-			 * and make khubd collect PORT_STAT_C_SUSPEND to
+			 * and make hub_wq collect PORT_STAT_C_SUSPEND to
 			 * stop that signaling.  Use 5 ms extra for safety,
 			 * like usb_port_resume() does.
 			 */
diff --git a/drivers/usb/host/fusbh200-hcd.c b/drivers/usb/host/fusbh200-hcd.c
index ba9499060f63..abe42f31559f 100644
--- a/drivers/usb/host/fusbh200-hcd.c
+++ b/drivers/usb/host/fusbh200-hcd.c
@@ -1441,7 +1441,7 @@ fusbh200_hub_status_data (struct usb_hcd *hcd, char *buf)
 
 	/*
 	 * Return status information even for ports with OWNER set.
-	 * Otherwise khubd wouldn't see the disconnect event when a
+	 * Otherwise hub_wq wouldn't see the disconnect event when a
 	 * high-speed device is switched over to the companion
 	 * controller by the user.
 	 */
@@ -1530,7 +1530,7 @@ static int fusbh200_hub_control (
 
 		/*
 		 * Even if OWNER is set, so the port is owned by the
-		 * companion controller, khubd needs to be able to clear
+		 * companion controller, hub_wq needs to be able to clear
 		 * the port-change status bits (especially
 		 * USB_PORT_STAT_C_CONNECTION).
 		 */
@@ -1678,7 +1678,7 @@ static int fusbh200_hub_control (
 		}
 
 		/*
-		 * Even if OWNER is set, there's no harm letting khubd
+		 * Even if OWNER is set, there's no harm letting hub_wq
 		 * see the wPortStatus values (they should all be 0 except
 		 * for PORT_POWER anyway).
 		 */
@@ -5355,7 +5355,7 @@ static irqreturn_t fusbh200_irq (struct usb_hcd *hcd)
 				fusbh200->reset_done[0] == 0) {
 
 			/* start 20 msec resume signaling from this port,
-			 * and make khubd collect PORT_STAT_C_SUSPEND to
+			 * and make hub_wq collect PORT_STAT_C_SUSPEND to
 			 * stop that signaling.  Use 5 ms extra for safety,
 			 * like usb_port_resume() does.
 			 */
diff --git a/drivers/usb/host/isp1362-hcd.c b/drivers/usb/host/isp1362-hcd.c
index 875bcfd3ec1a..4bb37982855e 100644
--- a/drivers/usb/host/isp1362-hcd.c
+++ b/drivers/usb/host/isp1362-hcd.c
@@ -2616,30 +2616,10 @@ static int isp1362_remove(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd = platform_get_drvdata(pdev);
 	struct isp1362_hcd *isp1362_hcd = hcd_to_isp1362_hcd(hcd);
-	struct resource *res;
 
 	remove_debug_file(isp1362_hcd);
 	DBG(0, "%s: Removing HCD\n", __func__);
 	usb_remove_hcd(hcd);
-
-	DBG(0, "%s: Unmapping data_reg @ %p\n", __func__,
-	    isp1362_hcd->data_reg);
-	iounmap(isp1362_hcd->data_reg);
-
-	DBG(0, "%s: Unmapping addr_reg @ %p\n", __func__,
-	    isp1362_hcd->addr_reg);
-	iounmap(isp1362_hcd->addr_reg);
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	DBG(0, "%s: release mem_region: %08lx\n", __func__, (long unsigned int)res->start);
-	if (res)
-		release_mem_region(res->start, resource_size(res));
-
-	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	DBG(0, "%s: release mem_region: %08lx\n", __func__, (long unsigned int)res->start);
-	if (res)
-		release_mem_region(res->start, resource_size(res));
-
 	DBG(0, "%s: put_hcd\n", __func__);
 	usb_put_hcd(hcd);
 	DBG(0, "%s: Done\n", __func__);
@@ -2651,12 +2631,11 @@ static int isp1362_probe(struct platform_device *pdev)
 {
 	struct usb_hcd *hcd;
 	struct isp1362_hcd *isp1362_hcd;
-	struct resource *addr, *data;
+	struct resource *addr, *data, *irq_res;
 	void __iomem *addr_reg;
 	void __iomem *data_reg;
 	int irq;
 	int retval = 0;
-	struct resource *irq_res;
 	unsigned int irq_flags = 0;
 
 	if (usb_disabled())
@@ -2667,52 +2646,35 @@ static int isp1362_probe(struct platform_device *pdev)
 	 * specific platform_data.  we don't probe for IRQs, and do only
 	 * minimal sanity checking.
 	 */
-	if (pdev->num_resources < 3) {
-		retval = -ENODEV;
-		goto err1;
-	}
-
-	data = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	addr = platform_get_resource(pdev, IORESOURCE_MEM, 1);
-	irq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
-	if (!addr || !data || !irq_res) {
-		retval = -ENODEV;
-		goto err1;
-	}
-	irq = irq_res->start;
+	if (pdev->num_resources < 3)
+		return -ENODEV;
 
 	if (pdev->dev.dma_mask) {
 		DBG(1, "won't do DMA");
-		retval = -ENODEV;
-		goto err1;
+		return -ENODEV;
 	}
 
-	if (!request_mem_region(addr->start, resource_size(addr), hcd_name)) {
-		retval = -EBUSY;
-		goto err1;
-	}
-	addr_reg = ioremap(addr->start, resource_size(addr));
-	if (addr_reg == NULL) {
-		retval = -ENOMEM;
-		goto err2;
-	}
+	irq_res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
+	if (!irq_res)
+		return -ENODEV;
 
-	if (!request_mem_region(data->start, resource_size(data), hcd_name)) {
-		retval = -EBUSY;
-		goto err3;
-	}
-	data_reg = ioremap(data->start, resource_size(data));
-	if (data_reg == NULL) {
-		retval = -ENOMEM;
-		goto err4;
-	}
+	irq = irq_res->start;
+
+	addr = platform_get_resource(pdev, IORESOURCE_MEM, 1);
+	addr_reg = devm_ioremap_resource(&pdev->dev, addr);
+	if (IS_ERR(addr_reg))
+		return PTR_ERR(addr_reg);
+
+	data = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	data_reg = devm_ioremap_resource(&pdev->dev, data);
+	if (IS_ERR(data_reg))
+		return PTR_ERR(data_reg);
 
 	/* allocate and initialize hcd */
 	hcd = usb_create_hcd(&isp1362_hc_driver, &pdev->dev, dev_name(&pdev->dev));
-	if (!hcd) {
-		retval = -ENOMEM;
-		goto err5;
-	}
+	if (!hcd)
+		return -ENOMEM;
+
 	hcd->rsrc_start = data->start;
 	isp1362_hcd = hcd_to_isp1362_hcd(hcd);
 	isp1362_hcd->data_reg = data_reg;
@@ -2729,7 +2691,7 @@ static int isp1362_probe(struct platform_device *pdev)
 	if (!isp1362_hcd->board->delay) {
 		dev_err(hcd->self.controller, "No platform delay function given\n");
 		retval = -ENODEV;
-		goto err6;
+		goto err;
 	}
 #endif
 
@@ -2744,32 +2706,17 @@ static int isp1362_probe(struct platform_device *pdev)
 
 	retval = usb_add_hcd(hcd, irq, irq_flags | IRQF_SHARED);
 	if (retval != 0)
-		goto err6;
+		goto err;
 	device_wakeup_enable(hcd->self.controller);
 
-	pr_info("%s, irq %d\n", hcd->product_desc, irq);
+	dev_info(&pdev->dev, "%s, irq %d\n", hcd->product_desc, irq);
 
 	create_debug_file(isp1362_hcd);
 
 	return 0;
 
- err6:
-	DBG(0, "%s: Freeing dev %p\n", __func__, isp1362_hcd);
+ err:
 	usb_put_hcd(hcd);
- err5:
-	DBG(0, "%s: Unmapping data_reg @ %p\n", __func__, data_reg);
-	iounmap(data_reg);
- err4:
-	DBG(0, "%s: Releasing mem region %08lx\n", __func__, (long unsigned int)data->start);
-	release_mem_region(data->start, resource_size(data));
- err3:
-	DBG(0, "%s: Unmapping addr_reg @ %p\n", __func__, addr_reg);
-	iounmap(addr_reg);
- err2:
-	DBG(0, "%s: Releasing mem region %08lx\n", __func__, (long unsigned int)addr->start);
-	release_mem_region(addr->start, resource_size(addr));
- err1:
-	pr_err("%s: init error, %d\n", __func__, retval);
 
 	return retval;
 }
diff --git a/drivers/usb/host/isp1760-hcd.c b/drivers/usb/host/isp1760-hcd.c
index 51a0ae9cdd1d..e752c3098f38 100644
--- a/drivers/usb/host/isp1760-hcd.c
+++ b/drivers/usb/host/isp1760-hcd.c
@@ -1760,7 +1760,7 @@ static int isp1760_hub_status_data(struct usb_hcd *hcd, char *buf)
 
 	/*
 	 * Return status information even for ports with OWNER set.
-	 * Otherwise khubd wouldn't see the disconnect event when a
+	 * Otherwise hub_wq wouldn't see the disconnect event when a
 	 * high-speed device is switched over to the companion
 	 * controller by the user.
 	 */
@@ -1871,7 +1871,7 @@ static int isp1760_hub_control(struct usb_hcd *hcd, u16 typeReq,
 
 		/*
 		 * Even if OWNER is set, so the port is owned by the
-		 * companion controller, khubd needs to be able to clear
+		 * companion controller, hub_wq needs to be able to clear
 		 * the port-change status bits (especially
 		 * USB_PORT_STAT_C_CONNECTION).
 		 */
@@ -2000,7 +2000,7 @@ static int isp1760_hub_control(struct usb_hcd *hcd, u16 typeReq,
 					reg_read32(hcd->regs, HC_PORTSC1));
 		}
 		/*
-		 * Even if OWNER is set, there's no harm letting khubd
+		 * Even if OWNER is set, there's no harm letting hub_wq
 		 * see the wPortStatus values (they should all be 0 except
 		 * for PORT_POWER anyway).
 		 */
diff --git a/drivers/usb/host/ohci-exynos.c b/drivers/usb/host/ohci-exynos.c
index a72ab8fe8cd3..d28b6583ba02 100644
--- a/drivers/usb/host/ohci-exynos.c
+++ b/drivers/usb/host/ohci-exynos.c
@@ -19,11 +19,8 @@
 #include <linux/of.h>
 #include <linux/platform_device.h>
 #include <linux/phy/phy.h>
-#include <linux/usb/phy.h>
-#include <linux/usb/samsung_usb_phy.h>
 #include <linux/usb.h>
 #include <linux/usb/hcd.h>
-#include <linux/usb/otg.h>
 
 #include "ohci.h"
 
@@ -38,9 +35,7 @@ static struct hc_driver __read_mostly exynos_ohci_hc_driver;
 
 struct exynos_ohci_hcd {
 	struct clk *clk;
-	struct usb_phy *phy;
-	struct usb_otg *otg;
-	struct phy *phy_g[PHY_NUMBER];
+	struct phy *phy[PHY_NUMBER];
 };
 
 static int exynos_ohci_get_phy(struct device *dev,
@@ -49,30 +44,9 @@ static int exynos_ohci_get_phy(struct device *dev,
 	struct device_node *child;
 	struct phy *phy;
 	int phy_number;
-	int ret = 0;
-
-	exynos_ohci->phy = devm_usb_get_phy(dev, USB_PHY_TYPE_USB2);
-	if (IS_ERR(exynos_ohci->phy)) {
-		ret = PTR_ERR(exynos_ohci->phy);
-		if (ret != -ENXIO && ret != -ENODEV) {
-			dev_err(dev, "no usb2 phy configured\n");
-			return ret;
-		}
-		dev_dbg(dev, "Failed to get usb2 phy\n");
-	} else {
-		exynos_ohci->otg = exynos_ohci->phy->otg;
-	}
+	int ret;
 
-	/*
-	 * Getting generic phy:
-	 * We are keeping both types of phys as a part of transiting OHCI
-	 * to generic phy framework, so as to maintain backward compatibilty
-	 * with old DTB.
-	 * If there are existing devices using DTB files built from them,
-	 * to remove the support for old bindings in this driver,
-	 * we need to make sure that such devices have their DTBs
-	 * updated to ones built from new DTS.
-	 */
+	/* Get PHYs for the controller */
 	for_each_available_child_of_node(dev->of_node, child) {
 		ret = of_property_read_u32(child, "reg", &phy_number);
 		if (ret) {
@@ -88,19 +62,21 @@ static int exynos_ohci_get_phy(struct device *dev,
 		}
 
 		phy = devm_of_phy_get(dev, child, NULL);
+		exynos_ohci->phy[phy_number] = phy;
 		of_node_put(child);
 		if (IS_ERR(phy)) {
 			ret = PTR_ERR(phy);
-			if (ret != -ENOSYS && ret != -ENODEV) {
-				dev_err(dev, "no usb2 phy configured\n");
+			if (ret == -EPROBE_DEFER) {
+				return ret;
+			} else if (ret != -ENOSYS && ret != -ENODEV) {
+				dev_err(dev,
+					"Error retrieving usb2 phy: %d\n", ret);
 				return ret;
 			}
-			dev_dbg(dev, "Failed to get usb2 phy\n");
 		}
-		exynos_ohci->phy_g[phy_number] = phy;
 	}
 
-	return ret;
+	return 0;
 }
 
 static int exynos_ohci_phy_enable(struct device *dev)
@@ -110,16 +86,13 @@ static int exynos_ohci_phy_enable(struct device *dev)
 	int i;
 	int ret = 0;
 
-	if (!IS_ERR(exynos_ohci->phy))
-		return usb_phy_init(exynos_ohci->phy);
-
 	for (i = 0; ret == 0 && i < PHY_NUMBER; i++)
-		if (!IS_ERR(exynos_ohci->phy_g[i]))
-			ret = phy_power_on(exynos_ohci->phy_g[i]);
+		if (!IS_ERR(exynos_ohci->phy[i]))
+			ret = phy_power_on(exynos_ohci->phy[i]);
 	if (ret)
 		for (i--; i >= 0; i--)
-			if (!IS_ERR(exynos_ohci->phy_g[i]))
-				phy_power_off(exynos_ohci->phy_g[i]);
+			if (!IS_ERR(exynos_ohci->phy[i]))
+				phy_power_off(exynos_ohci->phy[i]);
 
 	return ret;
 }
@@ -130,14 +103,9 @@ static void exynos_ohci_phy_disable(struct device *dev)
 	struct exynos_ohci_hcd *exynos_ohci = to_exynos_ohci(hcd);
 	int i;
 
-	if (!IS_ERR(exynos_ohci->phy)) {
-		usb_phy_shutdown(exynos_ohci->phy);
-		return;
-	}
-
 	for (i = 0; i < PHY_NUMBER; i++)
-		if (!IS_ERR(exynos_ohci->phy_g[i]))
-			phy_power_off(exynos_ohci->phy_g[i]);
+		if (!IS_ERR(exynos_ohci->phy[i]))
+			phy_power_off(exynos_ohci->phy[i]);
 }
 
 static int exynos_ohci_probe(struct platform_device *pdev)
@@ -209,9 +177,6 @@ skip_phy:
 		goto fail_io;
 	}
 
-	if (exynos_ohci->otg)
-		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
-
 	platform_set_drvdata(pdev, hcd);
 
 	err = exynos_ohci_phy_enable(&pdev->dev);
@@ -244,9 +209,6 @@ static int exynos_ohci_remove(struct platform_device *pdev)
 
 	usb_remove_hcd(hcd);
 
-	if (exynos_ohci->otg)
-		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
-
 	exynos_ohci_phy_disable(&pdev->dev);
 
 	clk_disable_unprepare(exynos_ohci->clk);
@@ -275,9 +237,6 @@ static int exynos_ohci_suspend(struct device *dev)
 	if (rc)
 		return rc;
 
-	if (exynos_ohci->otg)
-		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
-
 	exynos_ohci_phy_disable(dev);
 
 	clk_disable_unprepare(exynos_ohci->clk);
@@ -293,9 +252,6 @@ static int exynos_ohci_resume(struct device *dev)
 
 	clk_prepare_enable(exynos_ohci->clk);
 
-	if (exynos_ohci->otg)
-		exynos_ohci->otg->set_host(exynos_ohci->otg, &hcd->self);
-
 	ret = exynos_ohci_phy_enable(dev);
 	if (ret) {
 		dev_err(dev, "Failed to enable USB phy\n");
diff --git a/drivers/usb/host/ohci-hcd.c b/drivers/usb/host/ohci-hcd.c
index 46987735a2e3..d664edabf14e 100644
--- a/drivers/usb/host/ohci-hcd.c
+++ b/drivers/usb/host/ohci-hcd.c
@@ -632,7 +632,7 @@ retry:
 		return -EOVERFLOW;
 	}
 
-	/* use rhsc irqs after khubd is fully initialized */
+	/* use rhsc irqs after hub_wq is allocated */
 	set_bit(HCD_FLAG_POLL_RH, &hcd->flags);
 	hcd->uses_new_polling = 1;
 
@@ -909,8 +909,8 @@ static irqreturn_t ohci_irq (struct usb_hcd *hcd)
 		 * choices for RHSC.  Many followed the spec; RHSC triggers
 		 * on an edge, like setting and maybe clearing a port status
 		 * change bit.  With others it's level-triggered, active
-		 * until khubd clears all the port status change bits.  We'll
-		 * always disable it here and rely on polling until khubd
+		 * until hub_wq clears all the port status change bits.  We'll
+		 * always disable it here and rely on polling until hub_wq
 		 * re-enables it.
 		 */
 		ohci_writel(ohci, OHCI_INTR_RHSC, &regs->intrdisable);
diff --git a/drivers/usb/host/ohci-hub.c b/drivers/usb/host/ohci-hub.c
index 17d32b0ea565..0aa17c937115 100644
--- a/drivers/usb/host/ohci-hub.c
+++ b/drivers/usb/host/ohci-hub.c
@@ -585,7 +585,7 @@ static int ohci_start_port_reset (struct usb_hcd *hcd, unsigned port)
 	if (!(status & RH_PS_CCS))
 		return -ENODEV;
 
-	/* khubd will finish the reset later */
+	/* hub_wq will finish the reset later */
 	ohci_writel(ohci, RH_PS_PRS, &ohci->regs->roothub.portstatus [port]);
 	return 0;
 }
@@ -610,7 +610,7 @@ static int ohci_start_port_reset (struct usb_hcd *hcd, unsigned port)
 /* wrap-aware logic morphed from <linux/jiffies.h> */
 #define tick_before(t1,t2) ((s16)(((s16)(t1))-((s16)(t2))) < 0)
 
-/* called from some task, normally khubd */
+/* called from some task, normally hub_wq */
 static inline int root_port_reset (struct ohci_hcd *ohci, unsigned port)
 {
 	__hc32 __iomem *portstat = &ohci->regs->roothub.portstatus [port];
diff --git a/drivers/usb/host/ohci-omap.c b/drivers/usb/host/ohci-omap.c
index c923cafcaca7..0231606d47c2 100644
--- a/drivers/usb/host/ohci-omap.c
+++ b/drivers/usb/host/ohci-omap.c
@@ -180,10 +180,10 @@ static void start_hnp(struct ohci_hcd *ohci)
 	unsigned long	flags;
 	u32 l;
 
-	otg_start_hnp(hcd->phy->otg);
+	otg_start_hnp(hcd->usb_phy->otg);
 
 	local_irq_save(flags);
-	hcd->phy->state = OTG_STATE_A_SUSPEND;
+	hcd->usb_phy->state = OTG_STATE_A_SUSPEND;
 	writel (RH_PS_PSS, &ohci->regs->roothub.portstatus [port]);
 	l = omap_readl(OTG_CTRL);
 	l &= ~OTG_A_BUSREQ;
@@ -220,14 +220,14 @@ static int ohci_omap_reset(struct usb_hcd *hcd)
 
 #ifdef	CONFIG_USB_OTG
 	if (need_transceiver) {
-		hcd->phy = usb_get_phy(USB_PHY_TYPE_USB2);
-		if (!IS_ERR_OR_NULL(hcd->phy)) {
-			int	status = otg_set_host(hcd->phy->otg,
+		hcd->usb_phy = usb_get_phy(USB_PHY_TYPE_USB2);
+		if (!IS_ERR_OR_NULL(hcd->usb_phy)) {
+			int	status = otg_set_host(hcd->usb_phy->otg,
 						&ohci_to_hcd(ohci)->self);
 			dev_dbg(hcd->self.controller, "init %s phy, status %d\n",
-					hcd->phy->label, status);
+					hcd->usb_phy->label, status);
 			if (status) {
-				usb_put_phy(hcd->phy);
+				usb_put_phy(hcd->usb_phy);
 				return status;
 			}
 		} else {
@@ -283,7 +283,7 @@ static int ohci_omap_reset(struct usb_hcd *hcd)
 		ohci_to_hcd(ohci)->power_budget = 0;
 	}
 
-	/* FIXME khubd hub requests should manage power switching */
+	/* FIXME hub_wq hub requests should manage power switching */
 	omap_ohci_transceiver_power(1);
 
 	/* board init will have already handled HMC and mux setup.
@@ -399,9 +399,9 @@ usb_hcd_omap_remove (struct usb_hcd *hcd, struct platform_device *pdev)
 	dev_dbg(hcd->self.controller, "stopping USB Controller\n");
 	usb_remove_hcd(hcd);
 	omap_ohci_clock_power(0);
-	if (!IS_ERR_OR_NULL(hcd->phy)) {
-		(void) otg_set_host(hcd->phy->otg, 0);
-		usb_put_phy(hcd->phy);
+	if (!IS_ERR_OR_NULL(hcd->usb_phy)) {
+		(void) otg_set_host(hcd->usb_phy->otg, 0);
+		usb_put_phy(hcd->usb_phy);
 	}
 	if (machine_is_omap_osk())
 		gpio_free(9);
diff --git a/drivers/usb/host/ohci-st.c b/drivers/usb/host/ohci-st.c
new file mode 100644
index 000000000000..df9028e0d9b4
--- /dev/null
+++ b/drivers/usb/host/ohci-st.c
@@ -0,0 +1,349 @@
+/*
+ * ST OHCI driver
+ *
+ * Copyright (C) 2014 STMicroelectronics – All Rights Reserved
+ *
+ * Author: Peter Griffin <peter.griffin@linaro.org>
+ *
+ * Derived from ohci-platform.c
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/clk.h>
+#include <linux/dma-mapping.h>
+#include <linux/hrtimer.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/phy/phy.h>
+#include <linux/platform_device.h>
+#include <linux/reset.h>
+#include <linux/usb/ohci_pdriver.h>
+#include <linux/usb.h>
+#include <linux/usb/hcd.h>
+
+#include "ohci.h"
+
+#define USB_MAX_CLKS 3
+
+struct st_ohci_platform_priv {
+	struct clk *clks[USB_MAX_CLKS];
+	struct clk *clk48;
+	struct reset_control *rst;
+	struct reset_control *pwr;
+	struct phy *phy;
+};
+
+#define DRIVER_DESC "OHCI STMicroelectronics driver"
+
+#define hcd_to_ohci_priv(h) \
+	((struct st_ohci_platform_priv *)hcd_to_ohci(h)->priv)
+
+static const char hcd_name[] = "ohci-st";
+
+static int st_ohci_platform_power_on(struct platform_device *dev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(dev);
+	struct st_ohci_platform_priv *priv = hcd_to_ohci_priv(hcd);
+	int clk, ret;
+
+	ret = reset_control_deassert(priv->pwr);
+	if (ret)
+		return ret;
+
+	ret = reset_control_deassert(priv->rst);
+	if (ret)
+		goto err_assert_power;
+
+	/* some SoCs don't have a dedicated 48Mhz clock, but those that do
+	   need the rate to be explicitly set */
+	if (priv->clk48) {
+		ret = clk_set_rate(priv->clk48, 48000000);
+		if (ret)
+			goto err_assert_reset;
+	}
+
+	for (clk = 0; clk < USB_MAX_CLKS && priv->clks[clk]; clk++) {
+		ret = clk_prepare_enable(priv->clks[clk]);
+		if (ret)
+			goto err_disable_clks;
+	}
+
+	ret = phy_init(priv->phy);
+	if (ret)
+		goto err_disable_clks;
+
+	ret = phy_power_on(priv->phy);
+	if (ret)
+		goto err_exit_phy;
+
+	return 0;
+
+err_exit_phy:
+	phy_exit(priv->phy);
+err_disable_clks:
+	while (--clk >= 0)
+		clk_disable_unprepare(priv->clks[clk]);
+err_assert_reset:
+	reset_control_assert(priv->rst);
+err_assert_power:
+	reset_control_assert(priv->pwr);
+
+	return ret;
+}
+
+static void st_ohci_platform_power_off(struct platform_device *dev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(dev);
+	struct st_ohci_platform_priv *priv = hcd_to_ohci_priv(hcd);
+
+	int clk;
+
+	reset_control_assert(priv->pwr);
+
+	reset_control_assert(priv->rst);
+
+	phy_power_off(priv->phy);
+
+	phy_exit(priv->phy);
+
+	for (clk = USB_MAX_CLKS - 1; clk >= 0; clk--)
+		if (priv->clks[clk])
+			clk_disable_unprepare(priv->clks[clk]);
+}
+
+static struct hc_driver __read_mostly ohci_platform_hc_driver;
+
+static const struct ohci_driver_overrides platform_overrides __initconst = {
+	.product_desc =		"ST OHCI controller",
+	.extra_priv_size =	sizeof(struct st_ohci_platform_priv),
+};
+
+static struct usb_ohci_pdata ohci_platform_defaults = {
+	.power_on =		st_ohci_platform_power_on,
+	.power_suspend =	st_ohci_platform_power_off,
+	.power_off =		st_ohci_platform_power_off,
+};
+
+static int st_ohci_platform_probe(struct platform_device *dev)
+{
+	struct usb_hcd *hcd;
+	struct resource *res_mem;
+	struct usb_ohci_pdata *pdata = &ohci_platform_defaults;
+	struct st_ohci_platform_priv *priv;
+	struct ohci_hcd *ohci;
+	int err, irq, clk = 0;
+
+	if (usb_disabled())
+		return -ENODEV;
+
+	irq = platform_get_irq(dev, 0);
+	if (irq < 0) {
+		dev_err(&dev->dev, "no irq provided");
+		return irq;
+	}
+
+	res_mem = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	if (!res_mem) {
+		dev_err(&dev->dev, "no memory resource provided");
+		return -ENXIO;
+	}
+
+	hcd = usb_create_hcd(&ohci_platform_hc_driver, &dev->dev,
+			dev_name(&dev->dev));
+	if (!hcd)
+		return -ENOMEM;
+
+	platform_set_drvdata(dev, hcd);
+	dev->dev.platform_data = pdata;
+	priv = hcd_to_ohci_priv(hcd);
+	ohci = hcd_to_ohci(hcd);
+
+	priv->phy = devm_phy_get(&dev->dev, "usb");
+	if (IS_ERR(priv->phy)) {
+		err = PTR_ERR(priv->phy);
+		goto err_put_hcd;
+	}
+
+	for (clk = 0; clk < USB_MAX_CLKS; clk++) {
+		priv->clks[clk] = of_clk_get(dev->dev.of_node, clk);
+		if (IS_ERR(priv->clks[clk])) {
+			err = PTR_ERR(priv->clks[clk]);
+			if (err == -EPROBE_DEFER)
+				goto err_put_clks;
+			priv->clks[clk] = NULL;
+			break;
+		}
+	}
+
+	/* some SoCs don't have a dedicated 48Mhz clock, but those that
+	   do need the rate to be explicitly set */
+	priv->clk48 = devm_clk_get(&dev->dev, "clk48");
+	if (IS_ERR(priv->clk48)) {
+		dev_info(&dev->dev, "48MHz clk not found\n");
+		priv->clk48 = NULL;
+	}
+
+	priv->pwr = devm_reset_control_get_optional(&dev->dev, "power");
+	if (IS_ERR(priv->pwr)) {
+		err = PTR_ERR(priv->pwr);
+		goto err_put_clks;
+	}
+
+	priv->rst = devm_reset_control_get_optional(&dev->dev, "softreset");
+	if (IS_ERR(priv->rst)) {
+		err = PTR_ERR(priv->rst);
+		goto err_put_clks;
+	}
+
+	if (pdata->power_on) {
+		err = pdata->power_on(dev);
+		if (err < 0)
+			goto err_power;
+	}
+
+	hcd->rsrc_start = res_mem->start;
+	hcd->rsrc_len = resource_size(res_mem);
+
+	hcd->regs = devm_ioremap_resource(&dev->dev, res_mem);
+	if (IS_ERR(hcd->regs)) {
+		err = PTR_ERR(hcd->regs);
+		goto err_power;
+	}
+	err = usb_add_hcd(hcd, irq, IRQF_SHARED);
+	if (err)
+		goto err_power;
+
+	device_wakeup_enable(hcd->self.controller);
+
+	platform_set_drvdata(dev, hcd);
+
+	return err;
+
+err_power:
+	if (pdata->power_off)
+		pdata->power_off(dev);
+
+err_put_clks:
+	while (--clk >= 0)
+		clk_put(priv->clks[clk]);
+err_put_hcd:
+	if (pdata == &ohci_platform_defaults)
+		dev->dev.platform_data = NULL;
+
+	usb_put_hcd(hcd);
+
+	return err;
+}
+
+static int st_ohci_platform_remove(struct platform_device *dev)
+{
+	struct usb_hcd *hcd = platform_get_drvdata(dev);
+	struct usb_ohci_pdata *pdata = dev_get_platdata(&dev->dev);
+	struct st_ohci_platform_priv *priv = hcd_to_ohci_priv(hcd);
+	int clk;
+
+	usb_remove_hcd(hcd);
+
+	if (pdata->power_off)
+		pdata->power_off(dev);
+
+
+	for (clk = 0; clk < USB_MAX_CLKS && priv->clks[clk]; clk++)
+		clk_put(priv->clks[clk]);
+
+	usb_put_hcd(hcd);
+
+	if (pdata == &ohci_platform_defaults)
+		dev->dev.platform_data = NULL;
+
+	return 0;
+}
+
+#ifdef CONFIG_PM_SLEEP
+
+static int st_ohci_suspend(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct usb_ohci_pdata *pdata = dev->platform_data;
+	struct platform_device *pdev =
+		container_of(dev, struct platform_device, dev);
+	bool do_wakeup = device_may_wakeup(dev);
+	int ret;
+
+	ret = ohci_suspend(hcd, do_wakeup);
+	if (ret)
+		return ret;
+
+	if (pdata->power_suspend)
+		pdata->power_suspend(pdev);
+
+	return ret;
+}
+
+static int st_ohci_resume(struct device *dev)
+{
+	struct usb_hcd *hcd = dev_get_drvdata(dev);
+	struct usb_ohci_pdata *pdata = dev_get_platdata(dev);
+	struct platform_device *pdev =
+		container_of(dev, struct platform_device, dev);
+	int err;
+
+	if (pdata->power_on) {
+		err = pdata->power_on(pdev);
+		if (err < 0)
+			return err;
+	}
+
+	ohci_resume(hcd, false);
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(st_ohci_pm_ops, st_ohci_suspend, st_ohci_resume);
+
+#endif /* CONFIG_PM_SLEEP */
+
+static const struct of_device_id st_ohci_platform_ids[] = {
+	{ .compatible = "st,st-ohci-300x", },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, st_ohci_platform_ids);
+
+static struct platform_driver ohci_platform_driver = {
+	.probe		= st_ohci_platform_probe,
+	.remove		= st_ohci_platform_remove,
+	.shutdown	= usb_hcd_platform_shutdown,
+	.driver		= {
+		.name	= "st-ohci",
+#ifdef CONFIG_PM_SLEEP
+		.pm	= &st_ohci_pm_ops,
+#endif
+		.of_match_table = st_ohci_platform_ids,
+	}
+};
+
+static int __init ohci_platform_init(void)
+{
+	if (usb_disabled())
+		return -ENODEV;
+
+	pr_info("%s: " DRIVER_DESC "\n", hcd_name);
+
+	ohci_init_driver(&ohci_platform_hc_driver, &platform_overrides);
+	return platform_driver_register(&ohci_platform_driver);
+}
+module_init(ohci_platform_init);
+
+static void __exit ohci_platform_cleanup(void)
+{
+	platform_driver_unregister(&ohci_platform_driver);
+}
+module_exit(ohci_platform_cleanup);
+
+MODULE_DESCRIPTION(DRIVER_DESC);
+MODULE_AUTHOR("Peter Griffin <peter.griffin@linaro.org>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/oxu210hp-hcd.c b/drivers/usb/host/oxu210hp-hcd.c
index da5fb0e3c363..4fe79a2d71a9 100644
--- a/drivers/usb/host/oxu210hp-hcd.c
+++ b/drivers/usb/host/oxu210hp-hcd.c
@@ -2046,7 +2046,7 @@ static void intr_deschedule(struct oxu_hcd *oxu, struct ehci_qh *qh)
 
 	/* simple/paranoid:  always delay, expecting the HC needs to read
 	 * qh->hw_next or finish a writeback after SPLIT/CSPLIT ... and
-	 * expect khubd to clean up after any CSPLITs we won't issue.
+	 * expect hub_wq to clean up after any CSPLITs we won't issue.
 	 * active high speed queues may need bigger delays...
 	 */
 	if (list_empty(&qh->qtd_list)
@@ -2501,7 +2501,7 @@ static irqreturn_t oxu210_hcd_irq(struct usb_hcd *hcd)
 				continue;
 
 			/* start 20 msec resume signaling from this port,
-			 * and make khubd collect PORT_STAT_C_SUSPEND to
+			 * and make hub_wq collect PORT_STAT_C_SUSPEND to
 			 * stop that signaling.
 			 */
 			oxu->reset_done[i] = jiffies + msecs_to_jiffies(20);
@@ -3119,7 +3119,7 @@ static int oxu_hub_status_data(struct usb_hcd *hcd, char *buf)
 
 		/*
 		 * Return status information even for ports with OWNER set.
-		 * Otherwise khubd wouldn't see the disconnect event when a
+		 * Otherwise hub_wq wouldn't see the disconnect event when a
 		 * high-speed device is switched over to the companion
 		 * controller by the user.
 		 */
@@ -3194,7 +3194,7 @@ static int oxu_hub_control(struct usb_hcd *hcd, u16 typeReq,
 
 		/*
 		 * Even if OWNER is set, so the port is owned by the
-		 * companion controller, khubd needs to be able to clear
+		 * companion controller, hub_wq needs to be able to clear
 		 * the port-change status bits (especially
 		 * USB_PORT_STAT_C_CONNECTION).
 		 */
@@ -3336,7 +3336,7 @@ static int oxu_hub_control(struct usb_hcd *hcd, u16 typeReq,
 		}
 
 		/*
-		 * Even if OWNER is set, there's no harm letting khubd
+		 * Even if OWNER is set, there's no harm letting hub_wq
 		 * see the wPortStatus values (they should all be 0 except
 		 * for PORT_POWER anyway).
 		 */
diff --git a/drivers/usb/host/sl811-hcd.c b/drivers/usb/host/sl811-hcd.c
index a517151867af..ad0c348e68e9 100644
--- a/drivers/usb/host/sl811-hcd.c
+++ b/drivers/usb/host/sl811-hcd.c
@@ -674,7 +674,7 @@ retry:
 			sl811->next_periodic = sl811->periodic[index];
 	}
 
-	/* khubd manages debouncing and wakeup */
+	/* hub_wq manages debouncing and wakeup */
 	if (irqstat & SL11H_INTMASK_INSRMV) {
 		sl811->stat_insrmv++;
 
@@ -714,7 +714,7 @@ retry:
 #endif
 
 		/* port status seems weird until after reset, so
-		 * force the reset and make khubd clean up later.
+		 * force the reset and make hub_wq clean up later.
 		 */
 		if (irqstat & SL11H_INTMASK_RD)
 			sl811->port1 &= ~USB_PORT_STAT_CONNECTION;
@@ -1079,7 +1079,7 @@ sl811h_hub_status_data(struct usb_hcd *hcd, char *buf)
 	if (!(sl811->port1 & (0xffff << 16)))
 		return 0;
 
-	/* tell khubd port 1 changed */
+	/* tell hub_wq port 1 changed */
 	*buf = (1 << 1);
 	return 1;
 }
@@ -1196,7 +1196,7 @@ sl811h_timer(unsigned long _sl811)
 		sl811_write(sl811, SL811_EP_A(SL11H_HOSTCTLREG),
 				SL11H_HCTLMASK_ARM);
 
-		/* khubd provides debounce delay */
+		/* hub_wq provides debounce delay */
 	} else {
 		sl811->ctrl1 = 0;
 	}
diff --git a/drivers/usb/host/xhci-dbg.c b/drivers/usb/host/xhci-dbg.c
index eb009a457fb5..bb89175ca6e5 100644
--- a/drivers/usb/host/xhci-dbg.c
+++ b/drivers/usb/host/xhci-dbg.c
@@ -594,3 +594,4 @@ void xhci_dbg_trace(struct xhci_hcd *xhci, void (*trace)(struct va_format *),
 	trace(&vaf);
 	va_end(args);
 }
+EXPORT_SYMBOL_GPL(xhci_dbg_trace);
diff --git a/drivers/usb/host/xhci-hub.c b/drivers/usb/host/xhci-hub.c
index aa79e8749040..696160d48ae8 100644
--- a/drivers/usb/host/xhci-hub.c
+++ b/drivers/usb/host/xhci-hub.c
@@ -319,12 +319,19 @@ static int xhci_stop_device(struct xhci_hcd *xhci, int slot_id, int suspend)
  */
 void xhci_ring_device(struct xhci_hcd *xhci, int slot_id)
 {
-	int i;
+	int i, s;
+	struct xhci_virt_ep *ep;
+
+	for (i = 0; i < LAST_EP_INDEX + 1; i++) {
+		ep = &xhci->devs[slot_id]->eps[i];
 
-	for (i = 0; i < LAST_EP_INDEX + 1; i++)
-		if (xhci->devs[slot_id]->eps[i].ring &&
-		    xhci->devs[slot_id]->eps[i].ring->dequeue)
+		if (ep->ep_state & EP_HAS_STREAMS) {
+			for (s = 1; s < ep->stream_info->num_streams; s++)
+				xhci_ring_ep_doorbell(xhci, slot_id, i, s);
+		} else if (ep->ring && ep->ring->dequeue) {
 			xhci_ring_ep_doorbell(xhci, slot_id, i, 0);
+		}
+	}
 
 	return;
 }
@@ -468,7 +475,8 @@ static void xhci_hub_report_usb2_link_state(u32 *status, u32 status_reg)
 }
 
 /* Updates Link Status for super Speed port */
-static void xhci_hub_report_usb3_link_state(u32 *status, u32 status_reg)
+static void xhci_hub_report_usb3_link_state(struct xhci_hcd *xhci,
+		u32 *status, u32 status_reg)
 {
 	u32 pls = status_reg & PORT_PLS_MASK;
 
@@ -507,7 +515,8 @@ static void xhci_hub_report_usb3_link_state(u32 *status, u32 status_reg)
 		 * in which sometimes the port enters compliance mode
 		 * caused by a delay on the host-device negotiation.
 		 */
-		if (pls == USB_SS_PORT_LS_COMP_MOD)
+		if ((xhci->quirks & XHCI_COMP_MODE_QUIRK) &&
+				(pls == USB_SS_PORT_LS_COMP_MOD))
 			pls |= USB_PORT_STAT_CONNECTION;
 	}
 
@@ -666,7 +675,7 @@ static u32 xhci_get_port_status(struct usb_hcd *hcd,
 	}
 	/* Update Port Link State */
 	if (hcd->speed == HCD_USB3) {
-		xhci_hub_report_usb3_link_state(&status, raw_port_status);
+		xhci_hub_report_usb3_link_state(xhci, &status, raw_port_status);
 		/*
 		 * Verify if all USB3 Ports Have entered U0 already.
 		 * Delete Compliance Mode Timer if so.
@@ -890,7 +899,7 @@ int xhci_hub_control(struct usb_hcd *hcd, u16 typeReq, u16 wValue,
 			/*
 			 * Turn on ports, even if there isn't per-port switching.
 			 * HC will report connect events even before this is set.
-			 * However, khubd will ignore the roothub events until
+			 * However, hub_wq will ignore the roothub events until
 			 * the roothub is registered.
 			 */
 			writel(temp | PORT_POWER, port_array[wIndex]);
diff --git a/drivers/usb/host/xhci-mem.c b/drivers/usb/host/xhci-mem.c
index 8056d90690ee..5cb3d7a10017 100644
--- a/drivers/usb/host/xhci-mem.c
+++ b/drivers/usb/host/xhci-mem.c
@@ -1812,6 +1812,7 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 
 	if (xhci->lpm_command)
 		xhci_free_command(xhci, xhci->lpm_command);
+	xhci->lpm_command = NULL;
 	if (xhci->cmd_ring)
 		xhci_ring_free(xhci, xhci->cmd_ring);
 	xhci->cmd_ring = NULL;
@@ -1819,7 +1820,7 @@ void xhci_mem_cleanup(struct xhci_hcd *xhci)
 	xhci_cleanup_command_queue(xhci);
 
 	num_ports = HCS_MAX_PORTS(xhci->hcs_params1);
-	for (i = 0; i < num_ports; i++) {
+	for (i = 0; i < num_ports && xhci->rh_bw; i++) {
 		struct xhci_interval_bw_table *bwt = &xhci->rh_bw[i].bw_table;
 		for (j = 0; j < XHCI_MAX_INTERVAL; j++) {
 			struct list_head *ep = &bwt->interval_bw[j].endpoints;
@@ -1903,7 +1904,7 @@ static int xhci_test_trb_in_td(struct xhci_hcd *xhci,
 	start_dma = xhci_trb_virt_to_dma(input_seg, start_trb);
 	end_dma = xhci_trb_virt_to_dma(input_seg, end_trb);
 
-	seg = trb_in_td(input_seg, start_trb, end_trb, input_dma);
+	seg = trb_in_td(xhci, input_seg, start_trb, end_trb, input_dma, false);
 	if (seg != result_seg) {
 		xhci_warn(xhci, "WARN: %s TRB math test %d failed!\n",
 				test_name, test_number);
@@ -1917,6 +1918,8 @@ static int xhci_test_trb_in_td(struct xhci_hcd *xhci,
 				end_trb, end_dma);
 		xhci_warn(xhci, "Expected seg %p, got seg %p\n",
 				result_seg, seg);
+		trb_in_td(xhci, input_seg, start_trb, end_trb, input_dma,
+			  true);
 		return -1;
 	}
 	return 0;
diff --git a/drivers/usb/host/xhci-pci.c b/drivers/usb/host/xhci-pci.c
index 687d36608155..280dde93abe5 100644
--- a/drivers/usb/host/xhci-pci.c
+++ b/drivers/usb/host/xhci-pci.c
@@ -40,6 +40,8 @@
 
 static const char hcd_name[] = "xhci_hcd";
 
+static struct hc_driver __read_mostly xhci_pci_hc_driver;
+
 /* called after powerup, by probe or system-pm "wakeup" */
 static int xhci_pci_reinit(struct xhci_hcd *xhci, struct pci_dev *pdev)
 {
@@ -101,6 +103,10 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 	/* AMD PLL quirk */
 	if (pdev->vendor == PCI_VENDOR_ID_AMD && usb_amd_find_chipset_info())
 		xhci->quirks |= XHCI_AMD_PLL_FIX;
+
+	if (pdev->vendor == PCI_VENDOR_ID_AMD)
+		xhci->quirks |= XHCI_TRUST_TX_LENGTH;
+
 	if (pdev->vendor == PCI_VENDOR_ID_INTEL) {
 		xhci->quirks |= XHCI_LPM_SUPPORT;
 		xhci->quirks |= XHCI_INTEL_HOST;
@@ -151,6 +157,11 @@ static void xhci_pci_quirks(struct device *dev, struct xhci_hcd *xhci)
 	if (pdev->vendor == PCI_VENDOR_ID_VIA)
 		xhci->quirks |= XHCI_RESET_ON_RESUME;
 
+	/* See https://bugzilla.kernel.org/show_bug.cgi?id=79511 */
+	if (pdev->vendor == PCI_VENDOR_ID_VIA &&
+			pdev->device == 0x3432)
+		xhci->quirks |= XHCI_BROKEN_STREAMS;
+
 	if (xhci->quirks & XHCI_RESET_ON_RESUME)
 		xhci_dbg_trace(xhci, trace_xhci_dbg_quirks,
 				"QUIRK: Resetting on resume");
@@ -277,7 +288,7 @@ static int xhci_pci_suspend(struct usb_hcd *hcd, bool do_wakeup)
 	 * Systems with the TI redriver that loses port status change events
 	 * need to have the registers polled during D3, so avoid D3cold.
 	 */
-	if (xhci_compliance_mode_recovery_timer_quirk_check())
+	if (xhci->quirks & XHCI_COMP_MODE_QUIRK)
 		pdev->no_d3cold = true;
 
 	return xhci_suspend(xhci);
@@ -315,68 +326,6 @@ static int xhci_pci_resume(struct usb_hcd *hcd, bool hibernated)
 }
 #endif /* CONFIG_PM */
 
-static const struct hc_driver xhci_pci_hc_driver = {
-	.description =		hcd_name,
-	.product_desc =		"xHCI Host Controller",
-	.hcd_priv_size =	sizeof(struct xhci_hcd *),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq =			xhci_irq,
-	.flags =		HCD_MEMORY | HCD_USB3 | HCD_SHARED,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.reset =		xhci_pci_setup,
-	.start =		xhci_run,
-#ifdef CONFIG_PM
-	.pci_suspend =          xhci_pci_suspend,
-	.pci_resume =           xhci_pci_resume,
-#endif
-	.stop =			xhci_stop,
-	.shutdown =		xhci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue =		xhci_urb_enqueue,
-	.urb_dequeue =		xhci_urb_dequeue,
-	.alloc_dev =		xhci_alloc_dev,
-	.free_dev =		xhci_free_dev,
-	.alloc_streams =	xhci_alloc_streams,
-	.free_streams =		xhci_free_streams,
-	.add_endpoint =		xhci_add_endpoint,
-	.drop_endpoint =	xhci_drop_endpoint,
-	.endpoint_reset =	xhci_endpoint_reset,
-	.check_bandwidth =	xhci_check_bandwidth,
-	.reset_bandwidth =	xhci_reset_bandwidth,
-	.address_device =	xhci_address_device,
-	.enable_device =	xhci_enable_device,
-	.update_hub_device =	xhci_update_hub_device,
-	.reset_device =		xhci_discover_or_reset_device,
-
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number =	xhci_get_frame,
-
-	/* Root hub support */
-	.hub_control =		xhci_hub_control,
-	.hub_status_data =	xhci_hub_status_data,
-	.bus_suspend =		xhci_bus_suspend,
-	.bus_resume =		xhci_bus_resume,
-	/*
-	 * call back when device connected and addressed
-	 */
-	.update_device =        xhci_update_device,
-	.set_usb2_hw_lpm =	xhci_set_usb2_hardware_lpm,
-	.enable_usb3_lpm_timeout =	xhci_enable_usb3_lpm_timeout,
-	.disable_usb3_lpm_timeout =	xhci_disable_usb3_lpm_timeout,
-	.find_raw_port_number =	xhci_find_raw_port_number,
-};
-
 /*-------------------------------------------------------------------------*/
 
 /* PCI driver selection metadata; PCI hotplugging uses this */
@@ -406,12 +355,22 @@ static struct pci_driver xhci_pci_driver = {
 #endif
 };
 
-int __init xhci_register_pci(void)
+static int __init xhci_pci_init(void)
 {
+	xhci_init_driver(&xhci_pci_hc_driver, xhci_pci_setup);
+#ifdef CONFIG_PM
+	xhci_pci_hc_driver.pci_suspend = xhci_pci_suspend;
+	xhci_pci_hc_driver.pci_resume = xhci_pci_resume;
+#endif
 	return pci_register_driver(&xhci_pci_driver);
 }
+module_init(xhci_pci_init);
 
-void xhci_unregister_pci(void)
+static void __exit xhci_pci_exit(void)
 {
 	pci_unregister_driver(&xhci_pci_driver);
 }
+module_exit(xhci_pci_exit);
+
+MODULE_DESCRIPTION("xHCI PCI Host Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/xhci-plat.c b/drivers/usb/host/xhci-plat.c
index 1a0cf9f31e43..3d78b0cd674b 100644
--- a/drivers/usb/host/xhci-plat.c
+++ b/drivers/usb/host/xhci-plat.c
@@ -23,6 +23,8 @@
 #include "xhci-mvebu.h"
 #include "xhci-rcar.h"
 
+static struct hc_driver __read_mostly xhci_plat_hc_driver;
+
 static void xhci_plat_quirks(struct device *dev, struct xhci_hcd *xhci)
 {
 	/*
@@ -60,59 +62,6 @@ static int xhci_plat_start(struct usb_hcd *hcd)
 	return xhci_run(hcd);
 }
 
-static const struct hc_driver xhci_plat_xhci_driver = {
-	.description =		"xhci-hcd",
-	.product_desc =		"xHCI Host Controller",
-	.hcd_priv_size =	sizeof(struct xhci_hcd *),
-
-	/*
-	 * generic hardware linkage
-	 */
-	.irq =			xhci_irq,
-	.flags =		HCD_MEMORY | HCD_USB3 | HCD_SHARED,
-
-	/*
-	 * basic lifecycle operations
-	 */
-	.reset =		xhci_plat_setup,
-	.start =		xhci_plat_start,
-	.stop =			xhci_stop,
-	.shutdown =		xhci_shutdown,
-
-	/*
-	 * managing i/o requests and associated device resources
-	 */
-	.urb_enqueue =		xhci_urb_enqueue,
-	.urb_dequeue =		xhci_urb_dequeue,
-	.alloc_dev =		xhci_alloc_dev,
-	.free_dev =		xhci_free_dev,
-	.alloc_streams =	xhci_alloc_streams,
-	.free_streams =		xhci_free_streams,
-	.add_endpoint =		xhci_add_endpoint,
-	.drop_endpoint =	xhci_drop_endpoint,
-	.endpoint_reset =	xhci_endpoint_reset,
-	.check_bandwidth =	xhci_check_bandwidth,
-	.reset_bandwidth =	xhci_reset_bandwidth,
-	.address_device =	xhci_address_device,
-	.enable_device =	xhci_enable_device,
-	.update_hub_device =	xhci_update_hub_device,
-	.reset_device =		xhci_discover_or_reset_device,
-
-	/*
-	 * scheduling support
-	 */
-	.get_frame_number =	xhci_get_frame,
-
-	/* Root hub support */
-	.hub_control =		xhci_hub_control,
-	.hub_status_data =	xhci_hub_status_data,
-	.bus_suspend =		xhci_bus_suspend,
-	.bus_resume =		xhci_bus_resume,
-
-	.enable_usb3_lpm_timeout =	xhci_enable_usb3_lpm_timeout,
-	.disable_usb3_lpm_timeout =	xhci_disable_usb3_lpm_timeout,
-};
-
 static int xhci_plat_probe(struct platform_device *pdev)
 {
 	struct device_node	*node = pdev->dev.of_node;
@@ -128,7 +77,7 @@ static int xhci_plat_probe(struct platform_device *pdev)
 	if (usb_disabled())
 		return -ENODEV;
 
-	driver = &xhci_plat_xhci_driver;
+	driver = &xhci_plat_hc_driver;
 
 	irq = platform_get_irq(pdev, 0);
 	if (irq < 0)
@@ -298,12 +247,19 @@ static struct platform_driver usb_xhci_driver = {
 };
 MODULE_ALIAS("platform:xhci-hcd");
 
-int xhci_register_plat(void)
+static int __init xhci_plat_init(void)
 {
+	xhci_init_driver(&xhci_plat_hc_driver, xhci_plat_setup);
+	xhci_plat_hc_driver.start = xhci_plat_start;
 	return platform_driver_register(&usb_xhci_driver);
 }
+module_init(xhci_plat_init);
 
-void xhci_unregister_plat(void)
+static void __exit xhci_plat_exit(void)
 {
 	platform_driver_unregister(&usb_xhci_driver);
 }
+module_exit(xhci_plat_exit);
+
+MODULE_DESCRIPTION("xHCI Platform Host Controller Driver");
+MODULE_LICENSE("GPL");
diff --git a/drivers/usb/host/xhci-ring.c b/drivers/usb/host/xhci-ring.c
index 60fb52ae864b..bc6fcbc16f61 100644
--- a/drivers/usb/host/xhci-ring.c
+++ b/drivers/usb/host/xhci-ring.c
@@ -327,7 +327,6 @@ void xhci_ring_ep_doorbell(struct xhci_hcd *xhci,
 	 * We don't want to restart any stream rings if there's a set dequeue
 	 * pointer command pending because the device can choose to start any
 	 * stream once the endpoint is on the HW schedule.
-	 * FIXME - check all the stream rings for pending cancellations.
 	 */
 	if ((ep_state & EP_HALT_PENDING) || (ep_state & SET_DEQ_PENDING) ||
 	    (ep_state & EP_HALTED))
@@ -364,32 +363,6 @@ static void ring_doorbell_for_active_rings(struct xhci_hcd *xhci,
 	}
 }
 
-/*
- * Find the segment that trb is in.  Start searching in start_seg.
- * If we must move past a segment that has a link TRB with a toggle cycle state
- * bit set, then we will toggle the value pointed at by cycle_state.
- */
-static struct xhci_segment *find_trb_seg(
-		struct xhci_segment *start_seg,
-		union xhci_trb	*trb, int *cycle_state)
-{
-	struct xhci_segment *cur_seg = start_seg;
-	struct xhci_generic_trb *generic_trb;
-
-	while (cur_seg->trbs > trb ||
-			&cur_seg->trbs[TRBS_PER_SEGMENT - 1] < trb) {
-		generic_trb = &cur_seg->trbs[TRBS_PER_SEGMENT - 1].generic;
-		if (generic_trb->field[3] & cpu_to_le32(LINK_TOGGLE))
-			*cycle_state ^= 0x1;
-		cur_seg = cur_seg->next;
-		if (cur_seg == start_seg)
-			/* Looped over the entire list.  Oops! */
-			return NULL;
-	}
-	return cur_seg;
-}
-
-
 static struct xhci_ring *xhci_triad_to_transfer_ring(struct xhci_hcd *xhci,
 		unsigned int slot_id, unsigned int ep_index,
 		unsigned int stream_id)
@@ -459,9 +432,12 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 	struct xhci_virt_device *dev = xhci->devs[slot_id];
 	struct xhci_virt_ep *ep = &dev->eps[ep_index];
 	struct xhci_ring *ep_ring;
-	struct xhci_generic_trb *trb;
+	struct xhci_segment *new_seg;
+	union xhci_trb *new_deq;
 	dma_addr_t addr;
 	u64 hw_dequeue;
+	bool cycle_found = false;
+	bool td_last_trb_found = false;
 
 	ep_ring = xhci_triad_to_transfer_ring(xhci, slot_id,
 			ep_index, stream_id);
@@ -486,45 +462,45 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 		hw_dequeue = le64_to_cpu(ep_ctx->deq);
 	}
 
-	/* Find virtual address and segment of hardware dequeue pointer */
-	state->new_deq_seg = ep_ring->deq_seg;
-	state->new_deq_ptr = ep_ring->dequeue;
-	while (xhci_trb_virt_to_dma(state->new_deq_seg, state->new_deq_ptr)
-			!= (dma_addr_t)(hw_dequeue & ~0xf)) {
-		next_trb(xhci, ep_ring, &state->new_deq_seg,
-					&state->new_deq_ptr);
-		if (state->new_deq_ptr == ep_ring->dequeue) {
-			WARN_ON(1);
-			return;
-		}
-	}
+	new_seg = ep_ring->deq_seg;
+	new_deq = ep_ring->dequeue;
+	state->new_cycle_state = hw_dequeue & 0x1;
+
 	/*
-	 * Find cycle state for last_trb, starting at old cycle state of
-	 * hw_dequeue. If there is only one segment ring, find_trb_seg() will
-	 * return immediately and cannot toggle the cycle state if this search
-	 * wraps around, so add one more toggle manually in that case.
+	 * We want to find the pointer, segment and cycle state of the new trb
+	 * (the one after current TD's last_trb). We know the cycle state at
+	 * hw_dequeue, so walk the ring until both hw_dequeue and last_trb are
+	 * found.
 	 */
-	state->new_cycle_state = hw_dequeue & 0x1;
-	if (ep_ring->first_seg == ep_ring->first_seg->next &&
-			cur_td->last_trb < state->new_deq_ptr)
-		state->new_cycle_state ^= 0x1;
+	do {
+		if (!cycle_found && xhci_trb_virt_to_dma(new_seg, new_deq)
+		    == (dma_addr_t)(hw_dequeue & ~0xf)) {
+			cycle_found = true;
+			if (td_last_trb_found)
+				break;
+		}
+		if (new_deq == cur_td->last_trb)
+			td_last_trb_found = true;
 
-	state->new_deq_ptr = cur_td->last_trb;
-	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
-			"Finding segment containing last TRB in TD.");
-	state->new_deq_seg = find_trb_seg(state->new_deq_seg,
-			state->new_deq_ptr, &state->new_cycle_state);
-	if (!state->new_deq_seg) {
-		WARN_ON(1);
-		return;
-	}
+		if (cycle_found &&
+		    TRB_TYPE_LINK_LE32(new_deq->generic.field[3]) &&
+		    new_deq->generic.field[3] & cpu_to_le32(LINK_TOGGLE))
+			state->new_cycle_state ^= 0x1;
+
+		next_trb(xhci, ep_ring, &new_seg, &new_deq);
 
-	/* Increment to find next TRB after last_trb. Cycle if appropriate. */
-	trb = &state->new_deq_ptr->generic;
-	if (TRB_TYPE_LINK_LE32(trb->field[3]) &&
-	    (trb->field[3] & cpu_to_le32(LINK_TOGGLE)))
-		state->new_cycle_state ^= 0x1;
-	next_trb(xhci, ep_ring, &state->new_deq_seg, &state->new_deq_ptr);
+		/* Search wrapped around, bail out */
+		if (new_deq == ep->ring->dequeue) {
+			xhci_err(xhci, "Error: Failed finding new dequeue state\n");
+			state->new_deq_seg = NULL;
+			state->new_deq_ptr = NULL;
+			return;
+		}
+
+	} while (!cycle_found || !td_last_trb_found);
+
+	state->new_deq_seg = new_seg;
+	state->new_deq_ptr = new_deq;
 
 	/* Don't update the ring cycle state for the producer (us). */
 	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
@@ -595,40 +571,6 @@ static void td_to_noop(struct xhci_hcd *xhci, struct xhci_ring *ep_ring,
 	}
 }
 
-static int queue_set_tr_deq(struct xhci_hcd *xhci,
-		struct xhci_command *cmd, int slot_id,
-		unsigned int ep_index, unsigned int stream_id,
-		struct xhci_segment *deq_seg,
-		union xhci_trb *deq_ptr, u32 cycle_state);
-
-void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
-		struct xhci_command *cmd,
-		unsigned int slot_id, unsigned int ep_index,
-		unsigned int stream_id,
-		struct xhci_dequeue_state *deq_state)
-{
-	struct xhci_virt_ep *ep = &xhci->devs[slot_id]->eps[ep_index];
-
-	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
-			"Set TR Deq Ptr cmd, new deq seg = %p (0x%llx dma), "
-			"new deq ptr = %p (0x%llx dma), new cycle = %u",
-			deq_state->new_deq_seg,
-			(unsigned long long)deq_state->new_deq_seg->dma,
-			deq_state->new_deq_ptr,
-			(unsigned long long)xhci_trb_virt_to_dma(deq_state->new_deq_seg, deq_state->new_deq_ptr),
-			deq_state->new_cycle_state);
-	queue_set_tr_deq(xhci, cmd, slot_id, ep_index, stream_id,
-			deq_state->new_deq_seg,
-			deq_state->new_deq_ptr,
-			(u32) deq_state->new_cycle_state);
-	/* Stop the TD queueing code from ringing the doorbell until
-	 * this command completes.  The HC won't set the dequeue pointer
-	 * if the ring is running, and ringing the doorbell starts the
-	 * ring running.
-	 */
-	ep->ep_state |= SET_DEQ_PENDING;
-}
-
 static void xhci_stop_watchdog_timer_in_irq(struct xhci_hcd *xhci,
 		struct xhci_virt_ep *ep)
 {
@@ -766,12 +708,8 @@ remove_finished_td:
 
 	/* If necessary, queue a Set Transfer Ring Dequeue Pointer command */
 	if (deq_state.new_deq_ptr && deq_state.new_deq_seg) {
-		struct xhci_command *command;
-		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
-		xhci_queue_new_dequeue_state(xhci, command,
-				slot_id, ep_index,
-				ep->stopped_td->urb->stream_id,
-				&deq_state);
+		xhci_queue_new_dequeue_state(xhci, slot_id, ep_index,
+				ep->stopped_td->urb->stream_id, &deq_state);
 		xhci_ring_cmd_db(xhci);
 	} else {
 		/* Otherwise ring the doorbell(s) to restart queued transfers */
@@ -1026,8 +964,7 @@ static void xhci_handle_cmd_set_deq(struct xhci_hcd *xhci, int slot_id,
 		xhci_warn(xhci, "WARN Set TR deq ptr command for freed stream ID %u\n",
 				stream_id);
 		/* XXX: Harmless??? */
-		dev->eps[ep_index].ep_state &= ~SET_DEQ_PENDING;
-		return;
+		goto cleanup;
 	}
 
 	ep_ctx = xhci_get_ep_ctx(xhci, dev->out_ctx, ep_index);
@@ -1092,6 +1029,7 @@ static void xhci_handle_cmd_set_deq(struct xhci_hcd *xhci, int slot_id,
 		}
 	}
 
+cleanup:
 	dev->eps[ep_index].ep_state &= ~SET_DEQ_PENDING;
 	dev->eps[ep_index].queued_deq_seg = NULL;
 	dev->eps[ep_index].queued_deq_ptr = NULL;
@@ -1722,10 +1660,12 @@ cleanup:
  * TRB in this TD, this function returns that TRB's segment.  Otherwise it
  * returns 0.
  */
-struct xhci_segment *trb_in_td(struct xhci_segment *start_seg,
+struct xhci_segment *trb_in_td(struct xhci_hcd *xhci,
+		struct xhci_segment *start_seg,
 		union xhci_trb	*start_trb,
 		union xhci_trb	*end_trb,
-		dma_addr_t	suspect_dma)
+		dma_addr_t	suspect_dma,
+		bool		debug)
 {
 	dma_addr_t start_dma;
 	dma_addr_t end_seg_dma;
@@ -1744,6 +1684,15 @@ struct xhci_segment *trb_in_td(struct xhci_segment *start_seg,
 		/* If the end TRB isn't in this segment, this is set to 0 */
 		end_trb_dma = xhci_trb_virt_to_dma(cur_seg, end_trb);
 
+		if (debug)
+			xhci_warn(xhci,
+				"Looking for event-dma %016llx trb-start %016llx trb-end %016llx seg-start %016llx seg-end %016llx\n",
+				(unsigned long long)suspect_dma,
+				(unsigned long long)start_dma,
+				(unsigned long long)end_trb_dma,
+				(unsigned long long)cur_seg->dma,
+				(unsigned long long)end_seg_dma);
+
 		if (end_trb_dma > 0) {
 			/* The end TRB is in this segment, so suspect should be here */
 			if (start_dma <= end_trb_dma) {
@@ -2476,8 +2425,8 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 			td_num--;
 
 		/* Is this a TRB in the currently executing TD? */
-		event_seg = trb_in_td(ep_ring->deq_seg, ep_ring->dequeue,
-				td->last_trb, event_dma);
+		event_seg = trb_in_td(xhci, ep_ring->deq_seg, ep_ring->dequeue,
+				td->last_trb, event_dma, false);
 
 		/*
 		 * Skip the Force Stopped Event. The event_trb(event_dma) of FSE
@@ -2487,7 +2436,8 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 		 * last TRB of the previous TD. The command completion handle
 		 * will take care the rest.
 		 */
-		if (!event_seg && trb_comp_code == COMP_STOP_INVAL) {
+		if (!event_seg && (trb_comp_code == COMP_STOP ||
+				   trb_comp_code == COMP_STOP_INVAL)) {
 			ret = 0;
 			goto cleanup;
 		}
@@ -2508,7 +2458,12 @@ static int handle_tx_event(struct xhci_hcd *xhci,
 				/* HC is busted, give up! */
 				xhci_err(xhci,
 					"ERROR Transfer event TRB DMA ptr not "
-					"part of current TD\n");
+					"part of current TD ep_index %d "
+					"comp_code %u\n", ep_index,
+					trb_comp_code);
+				trb_in_td(xhci, ep_ring->deq_seg,
+					  ep_ring->dequeue, td->last_trb,
+					  event_dma, true);
 				return -ESHUTDOWN;
 			}
 
@@ -3948,14 +3903,11 @@ int xhci_queue_stop_endpoint(struct xhci_hcd *xhci, struct xhci_command *cmd,
 			trb_slot_id | trb_ep_index | type | trb_suspend, false);
 }
 
-/* Set Transfer Ring Dequeue Pointer command.
- * This should not be used for endpoints that have streams enabled.
- */
-static int queue_set_tr_deq(struct xhci_hcd *xhci, struct xhci_command *cmd,
-			int slot_id,
-			unsigned int ep_index, unsigned int stream_id,
-			struct xhci_segment *deq_seg,
-			union xhci_trb *deq_ptr, u32 cycle_state)
+/* Set Transfer Ring Dequeue Pointer command */
+void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
+		unsigned int slot_id, unsigned int ep_index,
+		unsigned int stream_id,
+		struct xhci_dequeue_state *deq_state)
 {
 	dma_addr_t addr;
 	u32 trb_slot_id = SLOT_ID_FOR_TRB(slot_id);
@@ -3964,28 +3916,59 @@ static int queue_set_tr_deq(struct xhci_hcd *xhci, struct xhci_command *cmd,
 	u32 trb_sct = 0;
 	u32 type = TRB_TYPE(TRB_SET_DEQ);
 	struct xhci_virt_ep *ep;
+	struct xhci_command *cmd;
+	int ret;
 
-	addr = xhci_trb_virt_to_dma(deq_seg, deq_ptr);
+	xhci_dbg_trace(xhci, trace_xhci_dbg_cancel_urb,
+		"Set TR Deq Ptr cmd, new deq seg = %p (0x%llx dma), new deq ptr = %p (0x%llx dma), new cycle = %u",
+		deq_state->new_deq_seg,
+		(unsigned long long)deq_state->new_deq_seg->dma,
+		deq_state->new_deq_ptr,
+		(unsigned long long)xhci_trb_virt_to_dma(
+			deq_state->new_deq_seg, deq_state->new_deq_ptr),
+		deq_state->new_cycle_state);
+
+	addr = xhci_trb_virt_to_dma(deq_state->new_deq_seg,
+				    deq_state->new_deq_ptr);
 	if (addr == 0) {
 		xhci_warn(xhci, "WARN Cannot submit Set TR Deq Ptr\n");
 		xhci_warn(xhci, "WARN deq seg = %p, deq pt = %p\n",
-				deq_seg, deq_ptr);
-		return 0;
+			  deq_state->new_deq_seg, deq_state->new_deq_ptr);
+		return;
 	}
 	ep = &xhci->devs[slot_id]->eps[ep_index];
 	if ((ep->ep_state & SET_DEQ_PENDING)) {
 		xhci_warn(xhci, "WARN Cannot submit Set TR Deq Ptr\n");
 		xhci_warn(xhci, "A Set TR Deq Ptr command is pending.\n");
-		return 0;
+		return;
 	}
-	ep->queued_deq_seg = deq_seg;
-	ep->queued_deq_ptr = deq_ptr;
+
+	/* This function gets called from contexts where it cannot sleep */
+	cmd = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
+	if (!cmd) {
+		xhci_warn(xhci, "WARN Cannot submit Set TR Deq Ptr: ENOMEM\n");
+		return;
+	}
+
+	ep->queued_deq_seg = deq_state->new_deq_seg;
+	ep->queued_deq_ptr = deq_state->new_deq_ptr;
 	if (stream_id)
 		trb_sct = SCT_FOR_TRB(SCT_PRI_TR);
-	return queue_command(xhci, cmd,
-			lower_32_bits(addr) | trb_sct | cycle_state,
-			upper_32_bits(addr), trb_stream_id,
-			trb_slot_id | trb_ep_index | type, false);
+	ret = queue_command(xhci, cmd,
+		lower_32_bits(addr) | trb_sct | deq_state->new_cycle_state,
+		upper_32_bits(addr), trb_stream_id,
+		trb_slot_id | trb_ep_index | type, false);
+	if (ret < 0) {
+		xhci_free_command(xhci, cmd);
+		return;
+	}
+
+	/* Stop the TD queueing code from ringing the doorbell until
+	 * this command completes.  The HC won't set the dequeue pointer
+	 * if the ring is running, and ringing the doorbell starts the
+	 * ring running.
+	 */
+	ep->ep_state |= SET_DEQ_PENDING;
 }
 
 int xhci_queue_reset_ep(struct xhci_hcd *xhci, struct xhci_command *cmd,
diff --git a/drivers/usb/host/xhci-trace.c b/drivers/usb/host/xhci-trace.c
index 7cf30c83dcf3..367b630bdb3c 100644
--- a/drivers/usb/host/xhci-trace.c
+++ b/drivers/usb/host/xhci-trace.c
@@ -13,3 +13,5 @@
 
 #define CREATE_TRACE_POINTS
 #include "xhci-trace.h"
+
+EXPORT_TRACEPOINT_SYMBOL_GPL(xhci_dbg_quirks);
diff --git a/drivers/usb/host/xhci.c b/drivers/usb/host/xhci.c
index b6f21175b872..2a5d45b4cb15 100644
--- a/drivers/usb/host/xhci.c
+++ b/drivers/usb/host/xhci.c
@@ -491,7 +491,7 @@ static void compliance_mode_recovery_timer_init(struct xhci_hcd *xhci)
  * Systems:
  * Vendor: Hewlett-Packard -> System Models: Z420, Z620 and Z820
  */
-bool xhci_compliance_mode_recovery_timer_quirk_check(void)
+static bool xhci_compliance_mode_recovery_timer_quirk_check(void)
 {
 	const char *dmi_product_name, *dmi_sys_vendor;
 
@@ -653,6 +653,7 @@ int xhci_run(struct usb_hcd *hcd)
 			"Finished xhci_run for USB2 roothub");
 	return 0;
 }
+EXPORT_SYMBOL_GPL(xhci_run);
 
 static void xhci_only_stop_hcd(struct usb_hcd *hcd)
 {
@@ -871,6 +872,8 @@ int xhci_suspend(struct xhci_hcd *xhci)
 	xhci_dbg(xhci, "%s: stopping port polling.\n", __func__);
 	clear_bit(HCD_FLAG_POLL_RH, &hcd->flags);
 	del_timer_sync(&hcd->rh_timer);
+	clear_bit(HCD_FLAG_POLL_RH, &xhci->shared_hcd->flags);
+	del_timer_sync(&xhci->shared_hcd->rh_timer);
 
 	spin_lock_irq(&xhci->lock);
 	clear_bit(HCD_FLAG_HW_ACCESSIBLE, &hcd->flags);
@@ -927,6 +930,7 @@ int xhci_suspend(struct xhci_hcd *xhci)
 
 	return rc;
 }
+EXPORT_SYMBOL_GPL(xhci_suspend);
 
 /*
  * start xHC (not bus-specific)
@@ -1075,9 +1079,12 @@ int xhci_resume(struct xhci_hcd *xhci, bool hibernated)
 	xhci_dbg(xhci, "%s: starting port polling.\n", __func__);
 	set_bit(HCD_FLAG_POLL_RH, &hcd->flags);
 	usb_hcd_poll_rh_status(hcd);
+	set_bit(HCD_FLAG_POLL_RH, &xhci->shared_hcd->flags);
+	usb_hcd_poll_rh_status(xhci->shared_hcd);
 
 	return retval;
 }
+EXPORT_SYMBOL_GPL(xhci_resume);
 #endif	/* CONFIG_PM */
 
 /*-------------------------------------------------------------------------*/
@@ -2880,18 +2887,16 @@ void xhci_cleanup_stalled_ring(struct xhci_hcd *xhci,
 			ep_index, ep->stopped_stream, ep->stopped_td,
 			&deq_state);
 
+	if (!deq_state.new_deq_ptr || !deq_state.new_deq_seg)
+		return;
+
 	/* HW with the reset endpoint quirk will use the saved dequeue state to
 	 * issue a configure endpoint command later.
 	 */
 	if (!(xhci->quirks & XHCI_RESET_EP_QUIRK)) {
-		struct xhci_command *command;
-		/* Can't sleep if we're called from cleanup_halted_endpoint() */
-		command = xhci_alloc_command(xhci, false, false, GFP_ATOMIC);
-		if (!command)
-			return;
 		xhci_dbg_trace(xhci, trace_xhci_dbg_reset_ep,
 				"Queueing new dequeue state");
-		xhci_queue_new_dequeue_state(xhci, command, udev->slot_id,
+		xhci_queue_new_dequeue_state(xhci, udev->slot_id,
 				ep_index, ep->stopped_stream, &deq_state);
 	} else {
 		/* Better hope no one uses the input context between now and the
@@ -3758,8 +3763,8 @@ disable_slot:
 /*
  * Issue an Address Device command and optionally send a corresponding
  * SetAddress request to the device.
- * We should be protected by the usb_address0_mutex in khubd's hub_port_init, so
- * we should only issue and wait on one address command at the same time.
+ * We should be protected by the usb_address0_mutex in hub_wq's hub_port_init,
+ * so we should only issue and wait on one address command at the same time.
  */
 static int xhci_setup_device(struct usb_hcd *hcd, struct usb_device *udev,
 			     enum xhci_setup_dev setup)
@@ -3968,13 +3973,21 @@ static int __maybe_unused xhci_change_max_exit_latency(struct xhci_hcd *xhci,
 	int ret;
 
 	spin_lock_irqsave(&xhci->lock, flags);
-	if (max_exit_latency == xhci->devs[udev->slot_id]->current_mel) {
+
+	virt_dev = xhci->devs[udev->slot_id];
+
+	/*
+	 * virt_dev might not exists yet if xHC resumed from hibernate (S4) and
+	 * xHC was re-initialized. Exit latency will be set later after
+	 * hub_port_finish_reset() is done and xhci->devs[] are re-allocated
+	 */
+
+	if (!virt_dev || max_exit_latency == virt_dev->current_mel) {
 		spin_unlock_irqrestore(&xhci->lock, flags);
 		return 0;
 	}
 
 	/* Attempt to issue an Evaluate Context command to change the MEL. */
-	virt_dev = xhci->devs[udev->slot_id];
 	command = xhci->lpm_command;
 	ctrl_ctx = xhci_get_input_control_ctx(xhci, command->in_ctx);
 	if (!ctrl_ctx) {
@@ -4892,6 +4905,76 @@ error:
 	kfree(xhci);
 	return retval;
 }
+EXPORT_SYMBOL_GPL(xhci_gen_setup);
+
+static const struct hc_driver xhci_hc_driver = {
+	.description =		"xhci-hcd",
+	.product_desc =		"xHCI Host Controller",
+	.hcd_priv_size =	sizeof(struct xhci_hcd *),
+
+	/*
+	 * generic hardware linkage
+	 */
+	.irq =			xhci_irq,
+	.flags =		HCD_MEMORY | HCD_USB3 | HCD_SHARED,
+
+	/*
+	 * basic lifecycle operations
+	 */
+	.reset =		NULL, /* set in xhci_init_driver() */
+	.start =		xhci_run,
+	.stop =			xhci_stop,
+	.shutdown =		xhci_shutdown,
+
+	/*
+	 * managing i/o requests and associated device resources
+	 */
+	.urb_enqueue =		xhci_urb_enqueue,
+	.urb_dequeue =		xhci_urb_dequeue,
+	.alloc_dev =		xhci_alloc_dev,
+	.free_dev =		xhci_free_dev,
+	.alloc_streams =	xhci_alloc_streams,
+	.free_streams =		xhci_free_streams,
+	.add_endpoint =		xhci_add_endpoint,
+	.drop_endpoint =	xhci_drop_endpoint,
+	.endpoint_reset =	xhci_endpoint_reset,
+	.check_bandwidth =	xhci_check_bandwidth,
+	.reset_bandwidth =	xhci_reset_bandwidth,
+	.address_device =	xhci_address_device,
+	.enable_device =	xhci_enable_device,
+	.update_hub_device =	xhci_update_hub_device,
+	.reset_device =		xhci_discover_or_reset_device,
+
+	/*
+	 * scheduling support
+	 */
+	.get_frame_number =	xhci_get_frame,
+
+	/*
+	 * root hub support
+	 */
+	.hub_control =		xhci_hub_control,
+	.hub_status_data =	xhci_hub_status_data,
+	.bus_suspend =		xhci_bus_suspend,
+	.bus_resume =		xhci_bus_resume,
+
+	/*
+	 * call back when device connected and addressed
+	 */
+	.update_device =        xhci_update_device,
+	.set_usb2_hw_lpm =	xhci_set_usb2_hardware_lpm,
+	.enable_usb3_lpm_timeout =	xhci_enable_usb3_lpm_timeout,
+	.disable_usb3_lpm_timeout =	xhci_disable_usb3_lpm_timeout,
+	.find_raw_port_number =	xhci_find_raw_port_number,
+};
+
+void xhci_init_driver(struct hc_driver *drv, int (*setup_fn)(struct usb_hcd *))
+{
+	BUG_ON(!setup_fn);
+	*drv = xhci_hc_driver;
+	drv->reset = setup_fn;
+}
+EXPORT_SYMBOL_GPL(xhci_init_driver);
 
 MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_AUTHOR(DRIVER_AUTHOR);
@@ -4899,18 +4982,6 @@ MODULE_LICENSE("GPL");
 
 static int __init xhci_hcd_init(void)
 {
-	int retval;
-
-	retval = xhci_register_pci();
-	if (retval < 0) {
-		pr_debug("Problem registering PCI driver.\n");
-		return retval;
-	}
-	retval = xhci_register_plat();
-	if (retval < 0) {
-		pr_debug("Problem registering platform driver.\n");
-		goto unreg_pci;
-	}
 	/*
 	 * Check the compiler generated sizes of structures that must be laid
 	 * out in specific ways for hardware access.
@@ -4929,15 +5000,5 @@ static int __init xhci_hcd_init(void)
 	/* xhci_run_regs has eight fields and embeds 128 xhci_intr_regs */
 	BUILD_BUG_ON(sizeof(struct xhci_run_regs) != (8+8*128)*32/8);
 	return 0;
-unreg_pci:
-	xhci_unregister_pci();
-	return retval;
 }
 module_init(xhci_hcd_init);
-
-static void __exit xhci_hcd_cleanup(void)
-{
-	xhci_unregister_pci();
-	xhci_unregister_plat();
-}
-module_exit(xhci_hcd_cleanup);
diff --git a/drivers/usb/host/xhci.h b/drivers/usb/host/xhci.h
index dace5152e179..df76d642e719 100644
--- a/drivers/usb/host/xhci.h
+++ b/drivers/usb/host/xhci.h
@@ -1731,25 +1731,6 @@ void xhci_urb_free_priv(struct xhci_hcd *xhci, struct urb_priv *urb_priv);
 void xhci_free_command(struct xhci_hcd *xhci,
 		struct xhci_command *command);
 
-#ifdef CONFIG_PCI
-/* xHCI PCI glue */
-int xhci_register_pci(void);
-void xhci_unregister_pci(void);
-#else
-static inline int xhci_register_pci(void) { return 0; }
-static inline void xhci_unregister_pci(void) {}
-#endif
-
-#if IS_ENABLED(CONFIG_USB_XHCI_PLATFORM)
-int xhci_register_plat(void);
-void xhci_unregister_plat(void);
-#else
-static inline int xhci_register_plat(void)
-{ return 0; }
-static inline void xhci_unregister_plat(void)
-{  }
-#endif
-
 /* xHCI host controller glue */
 typedef void (*xhci_get_quirks_t)(struct device *, struct xhci_hcd *);
 int xhci_handshake(struct xhci_hcd *xhci, void __iomem *ptr,
@@ -1762,6 +1743,7 @@ int xhci_run(struct usb_hcd *hcd);
 void xhci_stop(struct usb_hcd *hcd);
 void xhci_shutdown(struct usb_hcd *hcd);
 int xhci_gen_setup(struct usb_hcd *hcd, xhci_get_quirks_t get_quirks);
+void xhci_init_driver(struct hc_driver *drv, int (*setup_fn)(struct usb_hcd *));
 
 #ifdef	CONFIG_PM
 int xhci_suspend(struct xhci_hcd *xhci);
@@ -1804,9 +1786,9 @@ void xhci_reset_bandwidth(struct usb_hcd *hcd, struct usb_device *udev);
 
 /* xHCI ring, segment, TRB, and TD functions */
 dma_addr_t xhci_trb_virt_to_dma(struct xhci_segment *seg, union xhci_trb *trb);
-struct xhci_segment *trb_in_td(struct xhci_segment *start_seg,
-		union xhci_trb *start_trb, union xhci_trb *end_trb,
-		dma_addr_t suspect_dma);
+struct xhci_segment *trb_in_td(struct xhci_hcd *xhci,
+		struct xhci_segment *start_seg, union xhci_trb *start_trb,
+		union xhci_trb *end_trb, dma_addr_t suspect_dma, bool debug);
 int xhci_is_vendor_info_code(struct xhci_hcd *xhci, unsigned int trb_comp_code);
 void xhci_ring_cmd_db(struct xhci_hcd *xhci);
 int xhci_queue_slot_control(struct xhci_hcd *xhci, struct xhci_command *cmd,
@@ -1839,7 +1821,6 @@ void xhci_find_new_dequeue_state(struct xhci_hcd *xhci,
 		unsigned int stream_id, struct xhci_td *cur_td,
 		struct xhci_dequeue_state *state);
 void xhci_queue_new_dequeue_state(struct xhci_hcd *xhci,
-		struct xhci_command *cmd,
 		unsigned int slot_id, unsigned int ep_index,
 		unsigned int stream_id,
 		struct xhci_dequeue_state *deq_state);
@@ -1887,7 +1868,4 @@ struct xhci_input_control_ctx *xhci_get_input_control_ctx(struct xhci_hcd *xhci,
 struct xhci_slot_ctx *xhci_get_slot_ctx(struct xhci_hcd *xhci, struct xhci_container_ctx *ctx);
 struct xhci_ep_ctx *xhci_get_ep_ctx(struct xhci_hcd *xhci, struct xhci_container_ctx *ctx, unsigned int ep_index);
 
-/* xHCI quirks */
-bool xhci_compliance_mode_recovery_timer_quirk_check(void);
-
 #endif /* __LINUX_XHCI_HCD_H */
diff --git a/drivers/usb/misc/lvstest.c b/drivers/usb/misc/lvstest.c
index 7d589c156fb1..62cb8cd08403 100644
--- a/drivers/usb/misc/lvstest.c
+++ b/drivers/usb/misc/lvstest.c
@@ -333,13 +333,13 @@ static void lvs_rh_work(struct work_struct *work)
 					USB_PORT_STAT_CONNECTION) {
 				lvs->present = true;
 				lvs->portnum = i;
-				if (hcd->phy)
-					usb_phy_notify_connect(hcd->phy,
+				if (hcd->usb_phy)
+					usb_phy_notify_connect(hcd->usb_phy,
 							USB_SPEED_SUPER);
 			} else {
 				lvs->present = false;
-				if (hcd->phy)
-					usb_phy_notify_disconnect(hcd->phy,
+				if (hcd->usb_phy)
+					usb_phy_notify_disconnect(hcd->usb_phy,
 							USB_SPEED_SUPER);
 			}
 			break;
diff --git a/drivers/usb/misc/sisusbvga/sisusb.c b/drivers/usb/misc/sisusbvga/sisusb.c
index 06b5d77cd9ad..633caf643122 100644
--- a/drivers/usb/misc/sisusbvga/sisusb.c
+++ b/drivers/usb/misc/sisusbvga/sisusb.c
@@ -3250,6 +3250,7 @@ static const struct usb_device_id sisusb_table[] = {
 	{ USB_DEVICE(0x0711, 0x0918) },
 	{ USB_DEVICE(0x0711, 0x0920) },
 	{ USB_DEVICE(0x0711, 0x0950) },
+	{ USB_DEVICE(0x0711, 0x5200) },
 	{ USB_DEVICE(0x182d, 0x021c) },
 	{ USB_DEVICE(0x182d, 0x0269) },
 	{ }
diff --git a/drivers/usb/misc/usb3503.c b/drivers/usb/misc/usb3503.c
index 47cb143716a1..ae7e1206ca54 100644
--- a/drivers/usb/misc/usb3503.c
+++ b/drivers/usb/misc/usb3503.c
@@ -98,7 +98,7 @@ static int usb3503_connect(struct usb3503 *hub)
 			return err;
 		}
 
-		/* PDS : Disable For Self Powered Operation */
+		/* PDS : Set the ports which are disabled in self-powered mode. */
 		if (hub->port_off_mask) {
 			err = regmap_update_bits(hub->regmap, USB3503_PDS,
 					hub->port_off_mask,
@@ -109,7 +109,7 @@ static int usb3503_connect(struct usb3503 *hub)
 			}
 		}
 
-		/* CFG1 : SELF_BUS_PWR -> Self-Powerd operation */
+		/* CFG1 : Set SELF_BUS_PWR, this enables self-powered operation. */
 		err = regmap_update_bits(hub->regmap, USB3503_CFG1,
 					 USB3503_SELF_BUS_PWR,
 					 USB3503_SELF_BUS_PWR);
@@ -271,7 +271,7 @@ static int usb3503_probe(struct usb3503 *hub)
 					    "usb3503 intn");
 		if (err) {
 			dev_err(dev,
-				"unable to request GPIO %d as connect pin (%d)\n",
+				"unable to request GPIO %d as interrupt pin (%d)\n",
 				hub->gpio_intn, err);
 			return err;
 		}
diff --git a/drivers/usb/misc/usbtest.c b/drivers/usb/misc/usbtest.c
index 829f446064ea..0bbafe795a72 100644
--- a/drivers/usb/misc/usbtest.c
+++ b/drivers/usb/misc/usbtest.c
@@ -54,6 +54,7 @@ struct usbtest_info {
 	unsigned		autoconf:1;
 	unsigned		ctrl_out:1;
 	unsigned		iso:1;		/* try iso in/out */
+	unsigned		intr:1;		/* try interrupt in/out */
 	int			alt;
 };
 
@@ -70,7 +71,10 @@ struct usbtest_dev {
 	int			out_pipe;
 	int			in_iso_pipe;
 	int			out_iso_pipe;
+	int			in_int_pipe;
+	int			out_int_pipe;
 	struct usb_endpoint_descriptor	*iso_in, *iso_out;
+	struct usb_endpoint_descriptor	*int_in, *int_out;
 	struct mutex		lock;
 
 #define TBUF_SIZE	256
@@ -101,6 +105,7 @@ get_endpoints(struct usbtest_dev *dev, struct usb_interface *intf)
 	struct usb_host_interface	*alt;
 	struct usb_host_endpoint	*in, *out;
 	struct usb_host_endpoint	*iso_in, *iso_out;
+	struct usb_host_endpoint	*int_in, *int_out;
 	struct usb_device		*udev;
 
 	for (tmp = 0; tmp < intf->num_altsetting; tmp++) {
@@ -108,6 +113,7 @@ get_endpoints(struct usbtest_dev *dev, struct usb_interface *intf)
 
 		in = out = NULL;
 		iso_in = iso_out = NULL;
+		int_in = int_out = NULL;
 		alt = intf->altsetting + tmp;
 
 		if (override_alt >= 0 &&
@@ -124,6 +130,9 @@ get_endpoints(struct usbtest_dev *dev, struct usb_interface *intf)
 			switch (usb_endpoint_type(&e->desc)) {
 			case USB_ENDPOINT_XFER_BULK:
 				break;
+			case USB_ENDPOINT_XFER_INT:
+				if (dev->info->intr)
+					goto try_intr;
 			case USB_ENDPOINT_XFER_ISOC:
 				if (dev->info->iso)
 					goto try_iso;
@@ -139,6 +148,15 @@ get_endpoints(struct usbtest_dev *dev, struct usb_interface *intf)
 					out = e;
 			}
 			continue;
+try_intr:
+			if (usb_endpoint_dir_in(&e->desc)) {
+				if (!int_in)
+					int_in = e;
+			} else {
+				if (!int_out)
+					int_out = e;
+			}
+			continue;
 try_iso:
 			if (usb_endpoint_dir_in(&e->desc)) {
 				if (!iso_in)
@@ -148,7 +166,7 @@ try_iso:
 					iso_out = e;
 			}
 		}
-		if ((in && out)  ||  iso_in || iso_out)
+		if ((in && out)  ||  iso_in || iso_out || int_in || int_out)
 			goto found;
 	}
 	return -EINVAL;
@@ -183,6 +201,20 @@ found:
 				iso_out->desc.bEndpointAddress
 					& USB_ENDPOINT_NUMBER_MASK);
 	}
+
+	if (int_in) {
+		dev->int_in = &int_in->desc;
+		dev->in_int_pipe = usb_rcvintpipe(udev,
+				int_in->desc.bEndpointAddress
+					& USB_ENDPOINT_NUMBER_MASK);
+	}
+
+	if (int_out) {
+		dev->int_out = &int_out->desc;
+		dev->out_int_pipe = usb_sndintpipe(udev,
+				int_out->desc.bEndpointAddress
+					& USB_ENDPOINT_NUMBER_MASK);
+	}
 	return 0;
 }
 
@@ -205,14 +237,22 @@ static struct urb *usbtest_alloc_urb(
 	int			pipe,
 	unsigned long		bytes,
 	unsigned		transfer_flags,
-	unsigned		offset)
+	unsigned		offset,
+	u8			bInterval)
 {
 	struct urb		*urb;
 
 	urb = usb_alloc_urb(0, GFP_KERNEL);
 	if (!urb)
 		return urb;
-	usb_fill_bulk_urb(urb, udev, pipe, NULL, bytes, simple_callback, NULL);
+
+	if (bInterval)
+		usb_fill_int_urb(urb, udev, pipe, NULL, bytes, simple_callback,
+				NULL, bInterval);
+	else
+		usb_fill_bulk_urb(urb, udev, pipe, NULL, bytes, simple_callback,
+				NULL);
+
 	urb->interval = (udev->speed == USB_SPEED_HIGH)
 			? (INTERRUPT_RATE << 3)
 			: INTERRUPT_RATE;
@@ -251,9 +291,11 @@ static struct urb *usbtest_alloc_urb(
 static struct urb *simple_alloc_urb(
 	struct usb_device	*udev,
 	int			pipe,
-	unsigned long		bytes)
+	unsigned long		bytes,
+	u8			bInterval)
 {
-	return usbtest_alloc_urb(udev, pipe, bytes, URB_NO_TRANSFER_DMA_MAP, 0);
+	return usbtest_alloc_urb(udev, pipe, bytes, URB_NO_TRANSFER_DMA_MAP, 0,
+			bInterval);
 }
 
 static unsigned pattern;
@@ -1255,7 +1297,7 @@ test_ctrl_queue(struct usbtest_dev *dev, struct usbtest_param *param)
 			goto cleanup;
 		}
 		req.wLength = cpu_to_le16(len);
-		urb[i] = u = simple_alloc_urb(udev, pipe, len);
+		urb[i] = u = simple_alloc_urb(udev, pipe, len, 0);
 		if (!u)
 			goto cleanup;
 
@@ -1328,7 +1370,7 @@ static int unlink1(struct usbtest_dev *dev, int pipe, int size, int async)
 	int			retval = 0;
 
 	init_completion(&completion);
-	urb = simple_alloc_urb(testdev_to_usbdev(dev), pipe, size);
+	urb = simple_alloc_urb(testdev_to_usbdev(dev), pipe, size, 0);
 	if (!urb)
 		return -ENOMEM;
 	urb->context = &completion;
@@ -1616,9 +1658,9 @@ static int halt_simple(struct usbtest_dev *dev)
 	struct usb_device	*udev = testdev_to_usbdev(dev);
 
 	if (udev->speed == USB_SPEED_SUPER)
-		urb = simple_alloc_urb(udev, 0, 1024);
+		urb = simple_alloc_urb(udev, 0, 1024, 0);
 	else
-		urb = simple_alloc_urb(udev, 0, 512);
+		urb = simple_alloc_urb(udev, 0, 512, 0);
 	if (urb == NULL)
 		return -ENOMEM;
 
@@ -1962,7 +2004,7 @@ static int test_unaligned_bulk(
 {
 	int retval;
 	struct urb *urb = usbtest_alloc_urb(
-		testdev_to_usbdev(tdev), pipe, length, transfer_flags, 1);
+		testdev_to_usbdev(tdev), pipe, length, transfer_flags, 1, 0);
 
 	if (!urb)
 		return -ENOMEM;
@@ -1989,7 +2031,7 @@ static int test_unaligned_bulk(
  *
  * WARNING:  Because usbfs grabs udev->dev.sem before calling this ioctl(),
  * it locks out usbcore in certain code paths.  Notably, if you disconnect
- * the device-under-test, khubd will wait block forever waiting for the
+ * the device-under-test, hub_wq will wait block forever waiting for the
  * ioctl to complete ... so that usb_disconnect() can abort the pending
  * urbs and then call usbtest_disconnect().  To abort a test, you're best
  * off just killing the userspace task and waiting for it to exit.
@@ -2068,7 +2110,7 @@ usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 		dev_info(&intf->dev,
 				"TEST 1:  write %d bytes %u times\n",
 				param->length, param->iterations);
-		urb = simple_alloc_urb(udev, dev->out_pipe, param->length);
+		urb = simple_alloc_urb(udev, dev->out_pipe, param->length, 0);
 		if (!urb) {
 			retval = -ENOMEM;
 			break;
@@ -2083,7 +2125,7 @@ usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 		dev_info(&intf->dev,
 				"TEST 2:  read %d bytes %u times\n",
 				param->length, param->iterations);
-		urb = simple_alloc_urb(udev, dev->in_pipe, param->length);
+		urb = simple_alloc_urb(udev, dev->in_pipe, param->length, 0);
 		if (!urb) {
 			retval = -ENOMEM;
 			break;
@@ -2098,7 +2140,7 @@ usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 		dev_info(&intf->dev,
 				"TEST 3:  write/%d 0..%d bytes %u times\n",
 				param->vary, param->length, param->iterations);
-		urb = simple_alloc_urb(udev, dev->out_pipe, param->length);
+		urb = simple_alloc_urb(udev, dev->out_pipe, param->length, 0);
 		if (!urb) {
 			retval = -ENOMEM;
 			break;
@@ -2114,7 +2156,7 @@ usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 		dev_info(&intf->dev,
 				"TEST 4:  read/%d 0..%d bytes %u times\n",
 				param->vary, param->length, param->iterations);
-		urb = simple_alloc_urb(udev, dev->in_pipe, param->length);
+		urb = simple_alloc_urb(udev, dev->in_pipe, param->length, 0);
 		if (!urb) {
 			retval = -ENOMEM;
 			break;
@@ -2411,6 +2453,39 @@ usbtest_ioctl(struct usb_interface *intf, unsigned int code, void *buf)
 		}
 		break;
 
+	/* Simple non-queued interrupt I/O tests */
+	case 25:
+		if (dev->out_int_pipe == 0)
+			break;
+		dev_info(&intf->dev,
+				"TEST 25: write %d bytes %u times\n",
+				param->length, param->iterations);
+		urb = simple_alloc_urb(udev, dev->out_int_pipe, param->length,
+				dev->int_out->bInterval);
+		if (!urb) {
+			retval = -ENOMEM;
+			break;
+		}
+		/* FIRMWARE: interrupt sink (maybe accepts short writes) */
+		retval = simple_io(dev, urb, param->iterations, 0, 0, "test25");
+		simple_free_urb(urb);
+		break;
+	case 26:
+		if (dev->in_int_pipe == 0)
+			break;
+		dev_info(&intf->dev,
+				"TEST 26: read %d bytes %u times\n",
+				param->length, param->iterations);
+		urb = simple_alloc_urb(udev, dev->in_int_pipe, param->length,
+				dev->int_in->bInterval);
+		if (!urb) {
+			retval = -ENOMEM;
+			break;
+		}
+		/* FIRMWARE: interrupt source (maybe generates short writes) */
+		retval = simple_io(dev, urb, param->iterations, 0, 0, "test26");
+		simple_free_urb(urb);
+		break;
 	}
 	do_gettimeofday(&param->duration);
 	param->duration.tv_sec -= start.tv_sec;
@@ -2447,6 +2522,7 @@ usbtest_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	struct usbtest_info	*info;
 	char			*rtest, *wtest;
 	char			*irtest, *iwtest;
+	char			*intrtest, *intwtest;
 
 	udev = interface_to_usbdev(intf);
 
@@ -2487,6 +2563,7 @@ usbtest_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	 */
 	rtest = wtest = "";
 	irtest = iwtest = "";
+	intrtest = intwtest = "";
 	if (force_interrupt || udev->speed == USB_SPEED_LOW) {
 		if (info->ep_in) {
 			dev->in_pipe = usb_rcvintpipe(udev, info->ep_in);
@@ -2525,15 +2602,20 @@ usbtest_probe(struct usb_interface *intf, const struct usb_device_id *id)
 			irtest = " iso-in";
 		if (dev->out_iso_pipe)
 			iwtest = " iso-out";
+		if (dev->in_int_pipe)
+			intrtest = " int-in";
+		if (dev->out_int_pipe)
+			intwtest = " int-out";
 	}
 
 	usb_set_intfdata(intf, dev);
 	dev_info(&intf->dev, "%s\n", info->name);
-	dev_info(&intf->dev, "%s {control%s%s%s%s%s} tests%s\n",
+	dev_info(&intf->dev, "%s {control%s%s%s%s%s%s%s} tests%s\n",
 			usb_speed_string(udev->speed),
 			info->ctrl_out ? " in/out" : "",
 			rtest, wtest,
 			irtest, iwtest,
+			intrtest, intwtest,
 			info->alt >= 0 ? " (+alt)" : "");
 	return 0;
 }
@@ -2607,6 +2689,7 @@ static struct usbtest_info gz_info = {
 	.autoconf	= 1,
 	.ctrl_out	= 1,
 	.iso		= 1,
+	.intr		= 1,
 	.alt		= 0,
 };
 
diff --git a/drivers/usb/misc/yurex.c b/drivers/usb/misc/yurex.c
index 1472805083de..c3a45da11610 100644
--- a/drivers/usb/misc/yurex.c
+++ b/drivers/usb/misc/yurex.c
@@ -358,7 +358,7 @@ static int yurex_fasync(int fd, struct file *file, int on)
 {
 	struct usb_yurex *dev;
 
-	dev = (struct usb_yurex *)file->private_data;
+	dev = file->private_data;
 	return fasync_helper(fd, file, on, &dev->async_queue);
 }
 
@@ -401,7 +401,7 @@ static int yurex_release(struct inode *inode, struct file *file)
 {
 	struct usb_yurex *dev;
 
-	dev = (struct usb_yurex *)file->private_data;
+	dev = file->private_data;
 	if (dev == NULL)
 		return -ENODEV;
 
@@ -418,7 +418,7 @@ static ssize_t yurex_read(struct file *file, char *buffer, size_t count, loff_t
 	char in_buffer[20];
 	unsigned long flags;
 
-	dev = (struct usb_yurex *)file->private_data;
+	dev = file->private_data;
 
 	mutex_lock(&dev->io_mutex);
 	if (!dev->interface) {		/* already disconnected */
@@ -455,7 +455,7 @@ static ssize_t yurex_write(struct file *file, const char *user_buffer, size_t co
 	DEFINE_WAIT(wait);
 
 	count = min(sizeof(buffer), count);
-	dev = (struct usb_yurex *)file->private_data;
+	dev = file->private_data;
 
 	/* verify that we actually have some data to write */
 	if (count == 0)
diff --git a/drivers/usb/musb/am35x.c b/drivers/usb/musb/am35x.c
index 0a34dd859555..a2735df24cc6 100644
--- a/drivers/usb/musb/am35x.c
+++ b/drivers/usb/musb/am35x.c
@@ -1,3 +1,4 @@
+
 /*
  * Texas Instruments AM35x "glue layer"
  *
diff --git a/drivers/usb/musb/musb_cppi41.c b/drivers/usb/musb/musb_cppi41.c
index 47ae6455d073..acdfb3e68a90 100644
--- a/drivers/usb/musb/musb_cppi41.c
+++ b/drivers/usb/musb/musb_cppi41.c
@@ -39,6 +39,7 @@ struct cppi41_dma_channel {
 	u32 transferred;
 	u32 packet_sz;
 	struct list_head tx_check;
+	int tx_zlp;
 };
 
 #define MUSB_DMA_NUM_CHANNELS 15
@@ -122,6 +123,8 @@ static void cppi41_trans_done(struct cppi41_dma_channel *cppi41_channel)
 {
 	struct musb_hw_ep *hw_ep = cppi41_channel->hw_ep;
 	struct musb *musb = hw_ep->musb;
+	void __iomem *epio = hw_ep->regs;
+	u16 csr;
 
 	if (!cppi41_channel->prog_len ||
 	    (cppi41_channel->channel.status == MUSB_DMA_STATUS_FREE)) {
@@ -131,15 +134,24 @@ static void cppi41_trans_done(struct cppi41_dma_channel *cppi41_channel)
 			cppi41_channel->transferred;
 		cppi41_channel->channel.status = MUSB_DMA_STATUS_FREE;
 		cppi41_channel->channel.rx_packet_done = true;
+
+		/*
+		 * transmit ZLP using PIO mode for transfers which size is
+		 * multiple of EP packet size.
+		 */
+		if (cppi41_channel->tx_zlp && (cppi41_channel->transferred %
+					cppi41_channel->packet_sz) == 0) {
+			musb_ep_select(musb->mregs, hw_ep->epnum);
+			csr = MUSB_TXCSR_MODE | MUSB_TXCSR_TXPKTRDY;
+			musb_writew(epio, MUSB_TXCSR, csr);
+		}
 		musb_dma_completion(musb, hw_ep->epnum, cppi41_channel->is_tx);
 	} else {
 		/* next iteration, reload */
 		struct dma_chan *dc = cppi41_channel->dc;
 		struct dma_async_tx_descriptor *dma_desc;
 		enum dma_transfer_direction direction;
-		u16 csr;
 		u32 remain_bytes;
-		void __iomem *epio = cppi41_channel->hw_ep->regs;
 
 		cppi41_channel->buf_addr += cppi41_channel->packet_sz;
 
@@ -200,7 +212,7 @@ static enum hrtimer_restart cppi41_recheck_tx_req(struct hrtimer *timer)
 	if (!list_empty(&controller->early_tx_list)) {
 		ret = HRTIMER_RESTART;
 		hrtimer_forward_now(&controller->early_tx,
-				ktime_set(0, 50 * NSEC_PER_USEC));
+				ktime_set(0, 20 * NSEC_PER_USEC));
 	}
 
 	spin_unlock_irqrestore(&musb->lock, flags);
@@ -278,7 +290,7 @@ static void cppi41_dma_callback(void *private_data)
 
 			hrtimer_start_range_ns(&controller->early_tx,
 				ktime_set(0, usecs * NSEC_PER_USEC),
-				40 * NSEC_PER_USEC,
+				20 * NSEC_PER_USEC,
 				HRTIMER_MODE_REL);
 		}
 	}
@@ -363,6 +375,7 @@ static bool cppi41_configure_channel(struct dma_channel *channel,
 	cppi41_channel->total_len = len;
 	cppi41_channel->transferred = 0;
 	cppi41_channel->packet_sz = packet_sz;
+	cppi41_channel->tx_zlp = (cppi41_channel->is_tx && mode) ? 1 : 0;
 
 	/*
 	 * Due to AM335x' Advisory 1.0.13 we are not allowed to transfer more
diff --git a/drivers/usb/musb/musb_dsps.c b/drivers/usb/musb/musb_dsps.c
index c791ba5da91a..154bcf1b5dfa 100644
--- a/drivers/usb/musb/musb_dsps.c
+++ b/drivers/usb/musb/musb_dsps.c
@@ -870,6 +870,7 @@ static int dsps_suspend(struct device *dev)
 	struct musb *musb = platform_get_drvdata(glue->musb);
 	void __iomem *mbase = musb->ctrl_base;
 
+	del_timer_sync(&glue->timer);
 	glue->context.control = dsps_readl(mbase, wrp->control);
 	glue->context.epintr = dsps_readl(mbase, wrp->epintr_set);
 	glue->context.coreintr = dsps_readl(mbase, wrp->coreintr_set);
@@ -895,6 +896,7 @@ static int dsps_resume(struct device *dev)
 	dsps_writel(mbase, wrp->mode, glue->context.mode);
 	dsps_writel(mbase, wrp->tx_mode, glue->context.tx_mode);
 	dsps_writel(mbase, wrp->rx_mode, glue->context.rx_mode);
+	setup_timer(&glue->timer, otg_timer, (unsigned long) musb);
 
 	return 0;
 }
diff --git a/drivers/usb/musb/musb_gadget.c b/drivers/usb/musb/musb_gadget.c
index d4aa779339f1..24c8c0219790 100644
--- a/drivers/usb/musb/musb_gadget.c
+++ b/drivers/usb/musb/musb_gadget.c
@@ -176,7 +176,7 @@ __acquires(ep->musb->lock)
 				ep->end_point.name, request,
 				req->request.actual, req->request.length,
 				request->status);
-	req->request.complete(&req->ep->end_point, &req->request);
+	usb_gadget_giveback_request(&req->ep->end_point, &req->request);
 	spin_lock(&musb->lock);
 	ep->busy = busy;
 }
diff --git a/drivers/usb/musb/musb_regs.h b/drivers/usb/musb/musb_regs.h
index b9bcda5e3945..37122a480bc1 100644
--- a/drivers/usb/musb/musb_regs.h
+++ b/drivers/usb/musb/musb_regs.h
@@ -577,7 +577,7 @@ static inline u16 musb_read_hwvers(void __iomem *mbase)
 {
 	/*
 	 * This register is invisible on Blackfin, actually the MUSB
-	 * RTL version of Blackfin is 1.9, so just harcode its value.
+	 * RTL version of Blackfin is 1.9, so just hardcode its value.
 	 */
 	return MUSB_HWVERS_1900;
 }
diff --git a/drivers/usb/musb/tusb6010.c b/drivers/usb/musb/tusb6010.c
index 7dfc6cb732c9..2daa779f1382 100644
--- a/drivers/usb/musb/tusb6010.c
+++ b/drivers/usb/musb/tusb6010.c
@@ -433,7 +433,7 @@ static void musb_do_idle(unsigned long _musb)
 	if (!musb->is_active) {
 		u32	wakeups;
 
-		/* wait until khubd handles port change status */
+		/* wait until hub_wq handles port change status */
 		if (is_host_active(musb) && (musb->port1_status >> 16))
 			goto done;
 
diff --git a/drivers/usb/musb/ux500_dma.c b/drivers/usb/musb/ux500_dma.c
index 9aad00f11bd5..221faed9f074 100644
--- a/drivers/usb/musb/ux500_dma.c
+++ b/drivers/usb/musb/ux500_dma.c
@@ -96,7 +96,7 @@ static bool ux500_configure_channel(struct dma_channel *channel,
 	struct musb *musb = ux500_channel->controller->private_data;
 
 	dev_dbg(musb->controller,
-		"packet_sz=%d, mode=%d, dma_addr=0x%llu, len=%d is_tx=%d\n",
+		"packet_sz=%d, mode=%d, dma_addr=0x%llx, len=%d is_tx=%d\n",
 		packet_sz, mode, (unsigned long long) dma_addr,
 		len, ux500_channel->is_tx);
 
diff --git a/drivers/usb/phy/Kconfig b/drivers/usb/phy/Kconfig
index e253fa05be68..0cd1f44f0ee8 100644
--- a/drivers/usb/phy/Kconfig
+++ b/drivers/usb/phy/Kconfig
@@ -78,22 +78,6 @@ config SAMSUNG_USBPHY
 	  This driver provides common interface to interact, for Samsung USB 2.0 PHY
 	  driver and later for Samsung USB 3.0 PHY driver.
 
-config SAMSUNG_USB2PHY
-	tristate "Samsung USB 2.0 PHY controller Driver"
-	select SAMSUNG_USBPHY
-	select USB_PHY
-	help
-	  Enable this to support Samsung USB 2.0 (High Speed) PHY controller
-	  driver for Samsung SoCs.
-
-config SAMSUNG_USB3PHY
-	tristate "Samsung USB 3.0 PHY controller Driver"
-	select SAMSUNG_USBPHY
-	select USB_PHY
-	help
-	  Enable this to support Samsung USB 3.0 (Super Speed) phy controller
-	  for samsung SoCs.
-
 config TWL6030_USB
 	tristate "TWL6030 USB Transceiver Driver"
 	depends on TWL4030_CORE && OMAP_USB2 && USB_MUSB_OMAP2PLUS
diff --git a/drivers/usb/phy/Makefile b/drivers/usb/phy/Makefile
index 24a91332d4ad..75f2bba58c84 100644
--- a/drivers/usb/phy/Makefile
+++ b/drivers/usb/phy/Makefile
@@ -15,8 +15,6 @@ obj-$(CONFIG_AM335X_CONTROL_USB)	+= phy-am335x-control.o
 obj-$(CONFIG_AM335X_PHY_USB)		+= phy-am335x.o
 obj-$(CONFIG_OMAP_OTG)			+= phy-omap-otg.o
 obj-$(CONFIG_SAMSUNG_USBPHY)		+= phy-samsung-usb.o
-obj-$(CONFIG_SAMSUNG_USB2PHY)		+= phy-samsung-usb2.o
-obj-$(CONFIG_SAMSUNG_USB3PHY)		+= phy-samsung-usb3.o
 obj-$(CONFIG_TWL6030_USB)		+= phy-twl6030-usb.o
 obj-$(CONFIG_USB_EHCI_TEGRA)		+= phy-tegra-usb.o
 obj-$(CONFIG_USB_GPIO_VBUS)		+= phy-gpio-vbus-usb.o
diff --git a/drivers/usb/phy/phy-fsl-usb.c b/drivers/usb/phy/phy-fsl-usb.c
index 2b0f968d9325..f1ea5990a50a 100644
--- a/drivers/usb/phy/phy-fsl-usb.c
+++ b/drivers/usb/phy/phy-fsl-usb.c
@@ -609,7 +609,7 @@ static int fsl_otg_set_host(struct usb_otg *otg, struct usb_bus *host)
 		otg->host->otg_port = fsl_otg_initdata.otg_port;
 		otg->host->is_b_host = otg_dev->fsm.id;
 		/*
-		 * must leave time for khubd to finish its thing
+		 * must leave time for hub_wq to finish its thing
 		 * before yanking the host driver out from under it,
 		 * so suspend the host after a short delay.
 		 */
diff --git a/drivers/usb/phy/phy-gpio-vbus-usb.c b/drivers/usb/phy/phy-gpio-vbus-usb.c
index ea9e705555df..f4b14bd97e14 100644
--- a/drivers/usb/phy/phy-gpio-vbus-usb.c
+++ b/drivers/usb/phy/phy-gpio-vbus-usb.c
@@ -260,10 +260,8 @@ static int gpio_vbus_probe(struct platform_device *pdev)
 
 	gpio_vbus->phy.otg = devm_kzalloc(&pdev->dev, sizeof(struct usb_otg),
 					  GFP_KERNEL);
-	if (!gpio_vbus->phy.otg) {
-		kfree(gpio_vbus);
+	if (!gpio_vbus->phy.otg)
 		return -ENOMEM;
-	}
 
 	platform_set_drvdata(pdev, gpio_vbus);
 	gpio_vbus->dev = &pdev->dev;
diff --git a/drivers/usb/phy/phy-isp1301-omap.c b/drivers/usb/phy/phy-isp1301-omap.c
index 69e49be8866b..8eea56d3ded6 100644
--- a/drivers/usb/phy/phy-isp1301-omap.c
+++ b/drivers/usb/phy/phy-isp1301-omap.c
@@ -1011,7 +1011,7 @@ static void isp_update_otg(struct isp1301 *isp, u8 stat)
 				break;
 			case OTG_STATE_A_WAIT_VFALL:
 				state = OTG_STATE_A_IDLE;
-				/* khubd may take a while to notice and
+				/* hub_wq may take a while to notice and
 				 * handle this disconnect, so don't go
 				 * to B_IDLE quite yet.
 				 */
diff --git a/drivers/usb/phy/phy-msm-usb.c b/drivers/usb/phy/phy-msm-usb.c
index e4108eec5ef4..7843ef7dd0ff 100644
--- a/drivers/usb/phy/phy-msm-usb.c
+++ b/drivers/usb/phy/phy-msm-usb.c
@@ -281,7 +281,7 @@ static int msm_otg_phy_clk_reset(struct msm_otg *motg)
 {
 	int ret = 0;
 
-	if (motg->pdata->phy_clk_reset && motg->phy_reset_clk)
+	if (motg->pdata->phy_clk_reset)
 		ret = motg->pdata->phy_clk_reset(motg->phy_reset_clk);
 	else if (motg->phy_rst)
 		ret = reset_control_reset(motg->phy_rst);
@@ -1394,7 +1394,7 @@ out:
 	return status;
 }
 
-const struct file_operations msm_otg_mode_fops = {
+static const struct file_operations msm_otg_mode_fops = {
 	.open = msm_otg_mode_open,
 	.read = seq_read,
 	.write = msm_otg_mode_write,
@@ -1554,11 +1554,14 @@ static int msm_otg_probe(struct platform_device *pdev)
 	phy = &motg->phy;
 	phy->dev = &pdev->dev;
 
-	motg->phy_reset_clk = devm_clk_get(&pdev->dev,
+	if (motg->pdata->phy_clk_reset) {
+		motg->phy_reset_clk = devm_clk_get(&pdev->dev,
 					   np ? "phy" : "usb_phy_clk");
-	if (IS_ERR(motg->phy_reset_clk)) {
-		dev_err(&pdev->dev, "failed to get usb_phy_clk\n");
-		motg->phy_reset_clk = NULL;
+
+		if (IS_ERR(motg->phy_reset_clk)) {
+			dev_err(&pdev->dev, "failed to get usb_phy_clk\n");
+			return PTR_ERR(motg->phy_reset_clk);
+		}
 	}
 
 	motg->clk = devm_clk_get(&pdev->dev, np ? "core" : "usb_hs_clk");
@@ -1601,8 +1604,8 @@ static int msm_otg_probe(struct platform_device *pdev)
 	 */
 	if (motg->phy_number) {
 		phy_select = devm_ioremap_nocache(&pdev->dev, USB2_PHY_SEL, 4);
-		if (IS_ERR(phy_select))
-			return PTR_ERR(phy_select);
+		if (!phy_select)
+			return -ENOMEM;
 		/* Enable second PHY with the OTG port */
 		writel(0x1, phy_select);
 	}
@@ -1838,7 +1841,6 @@ static struct platform_driver msm_otg_driver = {
 	.remove = msm_otg_remove,
 	.driver = {
 		.name = DRIVER_NAME,
-		.owner = THIS_MODULE,
 		.pm = &msm_otg_dev_pm_ops,
 		.of_match_table = msm_otg_dt_match,
 	},
diff --git a/drivers/usb/phy/phy-mxs-usb.c b/drivers/usb/phy/phy-mxs-usb.c
index c42bdf0c4a1f..0e0c41587a08 100644
--- a/drivers/usb/phy/phy-mxs-usb.c
+++ b/drivers/usb/phy/phy-mxs-usb.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2012-2013 Freescale Semiconductor, Inc.
+ * Copyright 2012-2014 Freescale Semiconductor, Inc.
  * Copyright (C) 2012 Marek Vasut <marex@denx.de>
  * on behalf of DENX Software Engineering GmbH
  *
@@ -125,10 +125,22 @@ static const struct mxs_phy_data imx6sl_phy_data = {
 		MXS_PHY_NEED_IP_FIX,
 };
 
+static const struct mxs_phy_data vf610_phy_data = {
+	.flags = MXS_PHY_DISCONNECT_LINE_WITHOUT_VBUS |
+		MXS_PHY_NEED_IP_FIX,
+};
+
+static const struct mxs_phy_data imx6sx_phy_data = {
+	.flags = MXS_PHY_DISCONNECT_LINE_WITHOUT_VBUS |
+		MXS_PHY_NEED_IP_FIX,
+};
+
 static const struct of_device_id mxs_phy_dt_ids[] = {
+	{ .compatible = "fsl,imx6sx-usbphy", .data = &imx6sx_phy_data, },
 	{ .compatible = "fsl,imx6sl-usbphy", .data = &imx6sl_phy_data, },
 	{ .compatible = "fsl,imx6q-usbphy", .data = &imx6q_phy_data, },
 	{ .compatible = "fsl,imx23-usbphy", .data = &imx23_phy_data, },
+	{ .compatible = "fsl,vf610-usbphy", .data = &vf610_phy_data, },
 	{ /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, mxs_phy_dt_ids);
diff --git a/drivers/usb/phy/phy-samsung-usb.c b/drivers/usb/phy/phy-samsung-usb.c
deleted file mode 100644
index ac025ca08425..000000000000
--- a/drivers/usb/phy/phy-samsung-usb.c
+++ /dev/null
@@ -1,241 +0,0 @@
-/* linux/drivers/usb/phy/phy-samsung-usb.c
- *
- * Copyright (c) 2012 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com
- *
- * Author: Praveen Paneri <p.paneri@samsung.com>
- *
- * Samsung USB-PHY helper driver with common function calls;
- * interacts with Samsung USB 2.0 PHY controller driver and later
- * with Samsung USB 3.0 PHY driver.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/device.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/of.h>
-#include <linux/of_address.h>
-#include <linux/usb/samsung_usb_phy.h>
-
-#include "phy-samsung-usb.h"
-
-int samsung_usbphy_parse_dt(struct samsung_usbphy *sphy)
-{
-	struct device_node *usbphy_sys;
-
-	/* Getting node for system controller interface for usb-phy */
-	usbphy_sys = of_get_child_by_name(sphy->dev->of_node, "usbphy-sys");
-	if (!usbphy_sys) {
-		dev_err(sphy->dev, "No sys-controller interface for usb-phy\n");
-		return -ENODEV;
-	}
-
-	sphy->pmuregs = of_iomap(usbphy_sys, 0);
-
-	if (sphy->pmuregs == NULL) {
-		dev_err(sphy->dev, "Can't get usb-phy pmu control register\n");
-		goto err0;
-	}
-
-	sphy->sysreg = of_iomap(usbphy_sys, 1);
-
-	/*
-	 * Not returning error code here, since this situation is not fatal.
-	 * Few SoCs may not have this switch available
-	 */
-	if (sphy->sysreg == NULL)
-		dev_warn(sphy->dev, "Can't get usb-phy sysreg cfg register\n");
-
-	of_node_put(usbphy_sys);
-
-	return 0;
-
-err0:
-	of_node_put(usbphy_sys);
-	return -ENXIO;
-}
-EXPORT_SYMBOL_GPL(samsung_usbphy_parse_dt);
-
-/*
- * Set isolation here for phy.
- * Here 'on = true' would mean USB PHY block is isolated, hence
- * de-activated and vice-versa.
- */
-void samsung_usbphy_set_isolation_4210(struct samsung_usbphy *sphy, bool on)
-{
-	void __iomem *reg = NULL;
-	u32 reg_val;
-	u32 en_mask = 0;
-
-	if (!sphy->pmuregs) {
-		dev_warn(sphy->dev, "Can't set pmu isolation\n");
-		return;
-	}
-
-	if (sphy->phy_type == USB_PHY_TYPE_DEVICE) {
-		reg = sphy->pmuregs + sphy->drv_data->devphy_reg_offset;
-		en_mask = sphy->drv_data->devphy_en_mask;
-	} else if (sphy->phy_type == USB_PHY_TYPE_HOST) {
-		reg = sphy->pmuregs + sphy->drv_data->hostphy_reg_offset;
-		en_mask = sphy->drv_data->hostphy_en_mask;
-	}
-
-	reg_val = readl(reg);
-
-	if (on)
-		reg_val &= ~en_mask;
-	else
-		reg_val |= en_mask;
-
-	writel(reg_val, reg);
-
-	if (sphy->drv_data->cpu_type == TYPE_EXYNOS4X12) {
-		writel(reg_val, sphy->pmuregs + EXYNOS4X12_PHY_HSIC_CTRL0);
-		writel(reg_val, sphy->pmuregs + EXYNOS4X12_PHY_HSIC_CTRL1);
-	}
-}
-EXPORT_SYMBOL_GPL(samsung_usbphy_set_isolation_4210);
-
-/*
- * Configure the mode of working of usb-phy here: HOST/DEVICE.
- */
-void samsung_usbphy_cfg_sel(struct samsung_usbphy *sphy)
-{
-	u32 reg;
-
-	if (!sphy->sysreg) {
-		dev_warn(sphy->dev, "Can't configure specified phy mode\n");
-		return;
-	}
-
-	reg = readl(sphy->sysreg);
-
-	if (sphy->phy_type == USB_PHY_TYPE_DEVICE)
-		reg &= ~EXYNOS_USB20PHY_CFG_HOST_LINK;
-	else if (sphy->phy_type == USB_PHY_TYPE_HOST)
-		reg |= EXYNOS_USB20PHY_CFG_HOST_LINK;
-
-	writel(reg, sphy->sysreg);
-}
-EXPORT_SYMBOL_GPL(samsung_usbphy_cfg_sel);
-
-/*
- * PHYs are different for USB Device and USB Host.
- * This make sure that correct PHY type is selected before
- * any operation on PHY.
- */
-int samsung_usbphy_set_type(struct usb_phy *phy,
-				enum samsung_usb_phy_type phy_type)
-{
-	struct samsung_usbphy *sphy = phy_to_sphy(phy);
-
-	sphy->phy_type = phy_type;
-
-	return 0;
-}
-EXPORT_SYMBOL_GPL(samsung_usbphy_set_type);
-
-int samsung_usbphy_rate_to_clksel_64xx(struct samsung_usbphy *sphy,
-							unsigned long rate)
-{
-	unsigned int clksel;
-
-	switch (rate) {
-	case 12 * MHZ:
-		clksel = PHYCLK_CLKSEL_12M;
-		break;
-	case 24 * MHZ:
-		clksel = PHYCLK_CLKSEL_24M;
-		break;
-	case 48 * MHZ:
-		clksel = PHYCLK_CLKSEL_48M;
-		break;
-	default:
-		dev_err(sphy->dev,
-			"Invalid reference clock frequency: %lu\n", rate);
-		return -EINVAL;
-	}
-
-	return clksel;
-}
-EXPORT_SYMBOL_GPL(samsung_usbphy_rate_to_clksel_64xx);
-
-int samsung_usbphy_rate_to_clksel_4x12(struct samsung_usbphy *sphy,
-							unsigned long rate)
-{
-	unsigned int clksel;
-
-	switch (rate) {
-	case 9600 * KHZ:
-		clksel = FSEL_CLKSEL_9600K;
-		break;
-	case 10 * MHZ:
-		clksel = FSEL_CLKSEL_10M;
-		break;
-	case 12 * MHZ:
-		clksel = FSEL_CLKSEL_12M;
-		break;
-	case 19200 * KHZ:
-		clksel = FSEL_CLKSEL_19200K;
-		break;
-	case 20 * MHZ:
-		clksel = FSEL_CLKSEL_20M;
-		break;
-	case 24 * MHZ:
-		clksel = FSEL_CLKSEL_24M;
-		break;
-	case 50 * MHZ:
-		clksel = FSEL_CLKSEL_50M;
-		break;
-	default:
-		dev_err(sphy->dev,
-			"Invalid reference clock frequency: %lu\n", rate);
-		return -EINVAL;
-	}
-
-	return clksel;
-}
-EXPORT_SYMBOL_GPL(samsung_usbphy_rate_to_clksel_4x12);
-
-/*
- * Returns reference clock frequency selection value
- */
-int samsung_usbphy_get_refclk_freq(struct samsung_usbphy *sphy)
-{
-	struct clk *ref_clk;
-	unsigned long rate;
-	int refclk_freq;
-
-	/*
-	 * In exynos5250 USB host and device PHY use
-	 * external crystal clock XXTI
-	 */
-	if (sphy->drv_data->cpu_type == TYPE_EXYNOS5250)
-		ref_clk = clk_get(sphy->dev, "ext_xtal");
-	else
-		ref_clk = clk_get(sphy->dev, "xusbxti");
-	if (IS_ERR(ref_clk)) {
-		dev_err(sphy->dev, "Failed to get reference clock\n");
-		return PTR_ERR(ref_clk);
-	}
-
-	rate = clk_get_rate(ref_clk);
-	refclk_freq = sphy->drv_data->rate_to_clksel(sphy, rate);
-
-	clk_put(ref_clk);
-
-	return refclk_freq;
-}
-EXPORT_SYMBOL_GPL(samsung_usbphy_get_refclk_freq);
diff --git a/drivers/usb/phy/phy-samsung-usb.h b/drivers/usb/phy/phy-samsung-usb.h
deleted file mode 100644
index 68771bfd1825..000000000000
--- a/drivers/usb/phy/phy-samsung-usb.h
+++ /dev/null
@@ -1,349 +0,0 @@
-/* linux/drivers/usb/phy/phy-samsung-usb.h
- *
- * Copyright (c) 2012 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com
- *
- * Samsung USB-PHY transceiver; talks to S3C HS OTG controller, EHCI-S5P and
- * OHCI-EXYNOS controllers.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/usb/phy.h>
-
-/* Register definitions */
-
-#define SAMSUNG_PHYPWR				(0x00)
-
-#define PHYPWR_NORMAL_MASK			(0x19 << 0)
-#define PHYPWR_OTG_DISABLE			(0x1 << 4)
-#define PHYPWR_ANALOG_POWERDOWN			(0x1 << 3)
-#define PHYPWR_FORCE_SUSPEND			(0x1 << 1)
-/* For Exynos4 */
-#define PHYPWR_NORMAL_MASK_PHY0			(0x39 << 0)
-#define PHYPWR_SLEEP_PHY0			(0x1 << 5)
-
-#define SAMSUNG_PHYCLK				(0x04)
-
-#define PHYCLK_MODE_USB11			(0x1 << 6)
-#define PHYCLK_EXT_OSC				(0x1 << 5)
-#define PHYCLK_COMMON_ON_N			(0x1 << 4)
-#define PHYCLK_ID_PULL				(0x1 << 2)
-#define PHYCLK_CLKSEL_MASK			(0x3 << 0)
-#define PHYCLK_CLKSEL_48M			(0x0 << 0)
-#define PHYCLK_CLKSEL_12M			(0x2 << 0)
-#define PHYCLK_CLKSEL_24M			(0x3 << 0)
-
-#define SAMSUNG_RSTCON				(0x08)
-
-#define RSTCON_PHYLINK_SWRST			(0x1 << 2)
-#define RSTCON_HLINK_SWRST			(0x1 << 1)
-#define RSTCON_SWRST				(0x1 << 0)
-
-/* EXYNOS4X12 */
-#define EXYNOS4X12_PHY_HSIC_CTRL0		(0x04)
-#define EXYNOS4X12_PHY_HSIC_CTRL1		(0x08)
-
-#define PHYPWR_NORMAL_MASK_HSIC1		(0x7 << 12)
-#define PHYPWR_NORMAL_MASK_HSIC0		(0x7 << 9)
-#define PHYPWR_NORMAL_MASK_PHY1			(0x7 << 6)
-
-#define RSTCON_HOSTPHY_SWRST			(0xf << 3)
-
-/* EXYNOS5 */
-#define EXYNOS5_PHY_HOST_CTRL0			(0x00)
-
-#define HOST_CTRL0_PHYSWRSTALL			(0x1 << 31)
-
-#define HOST_CTRL0_REFCLKSEL_MASK		(0x3 << 19)
-#define HOST_CTRL0_REFCLKSEL_XTAL		(0x0 << 19)
-#define HOST_CTRL0_REFCLKSEL_EXTL		(0x1 << 19)
-#define HOST_CTRL0_REFCLKSEL_CLKCORE		(0x2 << 19)
-
-#define HOST_CTRL0_FSEL_MASK			(0x7 << 16)
-#define HOST_CTRL0_FSEL(_x)			((_x) << 16)
-
-#define FSEL_CLKSEL_50M				(0x7)
-#define FSEL_CLKSEL_24M				(0x5)
-#define FSEL_CLKSEL_20M				(0x4)
-#define FSEL_CLKSEL_19200K			(0x3)
-#define FSEL_CLKSEL_12M				(0x2)
-#define FSEL_CLKSEL_10M				(0x1)
-#define FSEL_CLKSEL_9600K			(0x0)
-
-#define HOST_CTRL0_TESTBURNIN			(0x1 << 11)
-#define HOST_CTRL0_RETENABLE			(0x1 << 10)
-#define HOST_CTRL0_COMMONON_N			(0x1 << 9)
-#define HOST_CTRL0_SIDDQ			(0x1 << 6)
-#define HOST_CTRL0_FORCESLEEP			(0x1 << 5)
-#define HOST_CTRL0_FORCESUSPEND			(0x1 << 4)
-#define HOST_CTRL0_WORDINTERFACE		(0x1 << 3)
-#define HOST_CTRL0_UTMISWRST			(0x1 << 2)
-#define HOST_CTRL0_LINKSWRST			(0x1 << 1)
-#define HOST_CTRL0_PHYSWRST			(0x1 << 0)
-
-#define EXYNOS5_PHY_HOST_TUNE0			(0x04)
-
-#define EXYNOS5_PHY_HSIC_CTRL1			(0x10)
-
-#define EXYNOS5_PHY_HSIC_TUNE1			(0x14)
-
-#define EXYNOS5_PHY_HSIC_CTRL2			(0x20)
-
-#define EXYNOS5_PHY_HSIC_TUNE2			(0x24)
-
-#define HSIC_CTRL_REFCLKSEL_MASK		(0x3 << 23)
-#define HSIC_CTRL_REFCLKSEL			(0x2 << 23)
-
-#define HSIC_CTRL_REFCLKDIV_MASK		(0x7f << 16)
-#define HSIC_CTRL_REFCLKDIV(_x)			((_x) << 16)
-#define HSIC_CTRL_REFCLKDIV_12			(0x24 << 16)
-#define HSIC_CTRL_REFCLKDIV_15			(0x1c << 16)
-#define HSIC_CTRL_REFCLKDIV_16			(0x1a << 16)
-#define HSIC_CTRL_REFCLKDIV_19_2		(0x15 << 16)
-#define HSIC_CTRL_REFCLKDIV_20			(0x14 << 16)
-
-#define HSIC_CTRL_SIDDQ				(0x1 << 6)
-#define HSIC_CTRL_FORCESLEEP			(0x1 << 5)
-#define HSIC_CTRL_FORCESUSPEND			(0x1 << 4)
-#define HSIC_CTRL_WORDINTERFACE			(0x1 << 3)
-#define HSIC_CTRL_UTMISWRST			(0x1 << 2)
-#define HSIC_CTRL_PHYSWRST			(0x1 << 0)
-
-#define EXYNOS5_PHY_HOST_EHCICTRL		(0x30)
-
-#define HOST_EHCICTRL_ENAINCRXALIGN		(0x1 << 29)
-#define HOST_EHCICTRL_ENAINCR4			(0x1 << 28)
-#define HOST_EHCICTRL_ENAINCR8			(0x1 << 27)
-#define HOST_EHCICTRL_ENAINCR16			(0x1 << 26)
-
-#define EXYNOS5_PHY_HOST_OHCICTRL		(0x34)
-
-#define HOST_OHCICTRL_SUSPLGCY			(0x1 << 3)
-#define HOST_OHCICTRL_APPSTARTCLK		(0x1 << 2)
-#define HOST_OHCICTRL_CNTSEL			(0x1 << 1)
-#define HOST_OHCICTRL_CLKCKTRST			(0x1 << 0)
-
-#define EXYNOS5_PHY_OTG_SYS			(0x38)
-
-#define OTG_SYS_PHYLINK_SWRESET			(0x1 << 14)
-#define OTG_SYS_LINKSWRST_UOTG			(0x1 << 13)
-#define OTG_SYS_PHY0_SWRST			(0x1 << 12)
-
-#define OTG_SYS_REFCLKSEL_MASK			(0x3 << 9)
-#define OTG_SYS_REFCLKSEL_XTAL			(0x0 << 9)
-#define OTG_SYS_REFCLKSEL_EXTL			(0x1 << 9)
-#define OTG_SYS_REFCLKSEL_CLKCORE		(0x2 << 9)
-
-#define OTG_SYS_IDPULLUP_UOTG			(0x1 << 8)
-#define OTG_SYS_COMMON_ON			(0x1 << 7)
-
-#define OTG_SYS_FSEL_MASK			(0x7 << 4)
-#define OTG_SYS_FSEL(_x)			((_x) << 4)
-
-#define OTG_SYS_FORCESLEEP			(0x1 << 3)
-#define OTG_SYS_OTGDISABLE			(0x1 << 2)
-#define OTG_SYS_SIDDQ_UOTG			(0x1 << 1)
-#define OTG_SYS_FORCESUSPEND			(0x1 << 0)
-
-#define EXYNOS5_PHY_OTG_TUNE			(0x40)
-
-/* EXYNOS5: USB 3.0 DRD */
-#define EXYNOS5_DRD_LINKSYSTEM			(0x04)
-
-#define LINKSYSTEM_FLADJ_MASK			(0x3f << 1)
-#define LINKSYSTEM_FLADJ(_x)			((_x) << 1)
-#define LINKSYSTEM_XHCI_VERSION_CONTROL		(0x1 << 27)
-
-#define EXYNOS5_DRD_PHYUTMI			(0x08)
-
-#define PHYUTMI_OTGDISABLE			(0x1 << 6)
-#define PHYUTMI_FORCESUSPEND			(0x1 << 1)
-#define PHYUTMI_FORCESLEEP			(0x1 << 0)
-
-#define EXYNOS5_DRD_PHYPIPE			(0x0c)
-
-#define EXYNOS5_DRD_PHYCLKRST			(0x10)
-
-#define PHYCLKRST_SSC_REFCLKSEL_MASK		(0xff << 23)
-#define PHYCLKRST_SSC_REFCLKSEL(_x)		((_x) << 23)
-
-#define PHYCLKRST_SSC_RANGE_MASK		(0x03 << 21)
-#define PHYCLKRST_SSC_RANGE(_x)			((_x) << 21)
-
-#define PHYCLKRST_SSC_EN			(0x1 << 20)
-#define PHYCLKRST_REF_SSP_EN			(0x1 << 19)
-#define PHYCLKRST_REF_CLKDIV2			(0x1 << 18)
-
-#define PHYCLKRST_MPLL_MULTIPLIER_MASK		(0x7f << 11)
-#define PHYCLKRST_MPLL_MULTIPLIER_100MHZ_REF	(0x19 << 11)
-#define PHYCLKRST_MPLL_MULTIPLIER_50M_REF	(0x02 << 11)
-#define PHYCLKRST_MPLL_MULTIPLIER_24MHZ_REF	(0x68 << 11)
-#define PHYCLKRST_MPLL_MULTIPLIER_20MHZ_REF	(0x7d << 11)
-#define PHYCLKRST_MPLL_MULTIPLIER_19200KHZ_REF	(0x02 << 11)
-
-#define PHYCLKRST_FSEL_MASK			(0x3f << 5)
-#define PHYCLKRST_FSEL(_x)			((_x) << 5)
-#define PHYCLKRST_FSEL_PAD_100MHZ		(0x27 << 5)
-#define PHYCLKRST_FSEL_PAD_24MHZ		(0x2a << 5)
-#define PHYCLKRST_FSEL_PAD_20MHZ		(0x31 << 5)
-#define PHYCLKRST_FSEL_PAD_19_2MHZ		(0x38 << 5)
-
-#define PHYCLKRST_RETENABLEN			(0x1 << 4)
-
-#define PHYCLKRST_REFCLKSEL_MASK		(0x03 << 2)
-#define PHYCLKRST_REFCLKSEL_PAD_REFCLK		(0x2 << 2)
-#define PHYCLKRST_REFCLKSEL_EXT_REFCLK		(0x3 << 2)
-
-#define PHYCLKRST_PORTRESET			(0x1 << 1)
-#define PHYCLKRST_COMMONONN			(0x1 << 0)
-
-#define EXYNOS5_DRD_PHYREG0			(0x14)
-#define EXYNOS5_DRD_PHYREG1			(0x18)
-
-#define EXYNOS5_DRD_PHYPARAM0			(0x1c)
-
-#define PHYPARAM0_REF_USE_PAD			(0x1 << 31)
-#define PHYPARAM0_REF_LOSLEVEL_MASK		(0x1f << 26)
-#define PHYPARAM0_REF_LOSLEVEL			(0x9 << 26)
-
-#define EXYNOS5_DRD_PHYPARAM1			(0x20)
-
-#define PHYPARAM1_PCS_TXDEEMPH_MASK		(0x1f << 0)
-#define PHYPARAM1_PCS_TXDEEMPH			(0x1c)
-
-#define EXYNOS5_DRD_PHYTERM			(0x24)
-
-#define EXYNOS5_DRD_PHYTEST			(0x28)
-
-#define PHYTEST_POWERDOWN_SSP			(0x1 << 3)
-#define PHYTEST_POWERDOWN_HSP			(0x1 << 2)
-
-#define EXYNOS5_DRD_PHYADP			(0x2c)
-
-#define EXYNOS5_DRD_PHYBATCHG			(0x30)
-
-#define PHYBATCHG_UTMI_CLKSEL			(0x1 << 2)
-
-#define EXYNOS5_DRD_PHYRESUME			(0x34)
-#define EXYNOS5_DRD_LINKPORT			(0x44)
-
-#ifndef MHZ
-#define MHZ (1000*1000)
-#endif
-
-#ifndef KHZ
-#define KHZ (1000)
-#endif
-
-#define EXYNOS_USBHOST_PHY_CTRL_OFFSET		(0x4)
-#define S3C64XX_USBPHY_ENABLE			(0x1 << 16)
-#define EXYNOS_USBPHY_ENABLE			(0x1 << 0)
-#define EXYNOS_USB20PHY_CFG_HOST_LINK		(0x1 << 0)
-
-enum samsung_cpu_type {
-	TYPE_S3C64XX,
-	TYPE_EXYNOS4210,
-	TYPE_EXYNOS4X12,
-	TYPE_EXYNOS5250,
-};
-
-struct samsung_usbphy;
-
-/*
- * struct samsung_usbphy_drvdata - driver data for various SoC variants
- * @cpu_type: machine identifier
- * @devphy_en_mask: device phy enable mask for PHY CONTROL register
- * @hostphy_en_mask: host phy enable mask for PHY CONTROL register
- * @devphy_reg_offset: offset to DEVICE PHY CONTROL register from
- *		       mapped address of system controller.
- * @hostphy_reg_offset: offset to HOST PHY CONTROL register from
- *		       mapped address of system controller.
- *
- *	Here we have a separate mask for device type phy.
- *	Having different masks for host and device type phy helps
- *	in setting independent masks in case of SoCs like S5PV210,
- *	in which PHY0 and PHY1 enable bits belong to same register
- *	placed at position 0 and 1 respectively.
- *	Although for newer SoCs like exynos these bits belong to
- *	different registers altogether placed at position 0.
- */
-struct samsung_usbphy_drvdata {
-	int cpu_type;
-	int devphy_en_mask;
-	int hostphy_en_mask;
-	u32 devphy_reg_offset;
-	u32 hostphy_reg_offset;
-	int (*rate_to_clksel)(struct samsung_usbphy *, unsigned long);
-	void (*set_isolation)(struct samsung_usbphy *, bool);
-	void (*phy_enable)(struct samsung_usbphy *);
-	void (*phy_disable)(struct samsung_usbphy *);
-};
-
-/*
- * struct samsung_usbphy - transceiver driver state
- * @phy: transceiver structure
- * @plat: platform data
- * @dev: The parent device supplied to the probe function
- * @clk: usb phy clock
- * @regs: usb phy controller registers memory base
- * @pmuregs: USB device PHY_CONTROL register memory base
- * @sysreg: USB2.0 PHY_CFG register memory base
- * @ref_clk_freq: reference clock frequency selection
- * @drv_data: driver data available for different SoCs
- * @phy_type: Samsung SoCs specific phy types:	#HOST
- *						#DEVICE
- * @phy_usage: usage count for phy
- * @lock: lock for phy operations
- */
-struct samsung_usbphy {
-	struct usb_phy	phy;
-	struct samsung_usbphy_data *plat;
-	struct device	*dev;
-	struct clk	*clk;
-	void __iomem	*regs;
-	void __iomem	*pmuregs;
-	void __iomem	*sysreg;
-	int		ref_clk_freq;
-	const struct samsung_usbphy_drvdata *drv_data;
-	enum samsung_usb_phy_type phy_type;
-	atomic_t	phy_usage;
-	spinlock_t	lock;
-};
-
-#define phy_to_sphy(x)		container_of((x), struct samsung_usbphy, phy)
-
-static const struct of_device_id samsung_usbphy_dt_match[];
-
-static inline const struct samsung_usbphy_drvdata
-*samsung_usbphy_get_driver_data(struct platform_device *pdev)
-{
-	if (pdev->dev.of_node) {
-		const struct of_device_id *match;
-		match = of_match_node(samsung_usbphy_dt_match,
-							pdev->dev.of_node);
-		return match->data;
-	}
-
-	return (struct samsung_usbphy_drvdata *)
-				platform_get_device_id(pdev)->driver_data;
-}
-
-extern int samsung_usbphy_parse_dt(struct samsung_usbphy *sphy);
-extern void samsung_usbphy_set_isolation_4210(struct samsung_usbphy *sphy,
-								bool on);
-extern void samsung_usbphy_cfg_sel(struct samsung_usbphy *sphy);
-extern int samsung_usbphy_set_type(struct usb_phy *phy,
-					enum samsung_usb_phy_type phy_type);
-extern int samsung_usbphy_get_refclk_freq(struct samsung_usbphy *sphy);
-extern int samsung_usbphy_rate_to_clksel_64xx(struct samsung_usbphy *sphy,
-							unsigned long rate);
-extern int samsung_usbphy_rate_to_clksel_4x12(struct samsung_usbphy *sphy,
-							unsigned long rate);
diff --git a/drivers/usb/phy/phy-samsung-usb2.c b/drivers/usb/phy/phy-samsung-usb2.c
deleted file mode 100644
index b3ba86627b72..000000000000
--- a/drivers/usb/phy/phy-samsung-usb2.c
+++ /dev/null
@@ -1,541 +0,0 @@
-/* linux/drivers/usb/phy/phy-samsung-usb2.c
- *
- * Copyright (c) 2012 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com
- *
- * Author: Praveen Paneri <p.paneri@samsung.com>
- *
- * Samsung USB2.0 PHY transceiver; talks to S3C HS OTG controller, EHCI-S5P and
- * OHCI-EXYNOS controllers.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/delay.h>
-#include <linux/device.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/of.h>
-#include <linux/usb/otg.h>
-#include <linux/usb/samsung_usb_phy.h>
-#include <linux/platform_data/samsung-usbphy.h>
-
-#include "phy-samsung-usb.h"
-
-static int samsung_usbphy_set_host(struct usb_otg *otg, struct usb_bus *host)
-{
-	if (!otg)
-		return -ENODEV;
-
-	if (!otg->host)
-		otg->host = host;
-
-	return 0;
-}
-
-static bool exynos5_phyhost_is_on(void __iomem *regs)
-{
-	u32 reg;
-
-	reg = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	return !(reg & HOST_CTRL0_SIDDQ);
-}
-
-static void samsung_exynos5_usb2phy_enable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phyclk = sphy->ref_clk_freq;
-	u32 phyhost;
-	u32 phyotg;
-	u32 phyhsic;
-	u32 ehcictrl;
-	u32 ohcictrl;
-
-	/*
-	 * phy_usage helps in keeping usage count for phy
-	 * so that the first consumer enabling the phy is also
-	 * the last consumer to disable it.
-	 */
-
-	atomic_inc(&sphy->phy_usage);
-
-	if (exynos5_phyhost_is_on(regs)) {
-		dev_info(sphy->dev, "Already power on PHY\n");
-		return;
-	}
-
-	/* Host configuration */
-	phyhost = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	/* phy reference clock configuration */
-	phyhost &= ~HOST_CTRL0_FSEL_MASK;
-	phyhost |= HOST_CTRL0_FSEL(phyclk);
-
-	/* host phy reset */
-	phyhost &= ~(HOST_CTRL0_PHYSWRST |
-			HOST_CTRL0_PHYSWRSTALL |
-			HOST_CTRL0_SIDDQ |
-			/* Enable normal mode of operation */
-			HOST_CTRL0_FORCESUSPEND |
-			HOST_CTRL0_FORCESLEEP);
-
-	/* Link reset */
-	phyhost |= (HOST_CTRL0_LINKSWRST |
-			HOST_CTRL0_UTMISWRST |
-			/* COMMON Block configuration during suspend */
-			HOST_CTRL0_COMMONON_N);
-	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
-	udelay(10);
-	phyhost &= ~(HOST_CTRL0_LINKSWRST |
-			HOST_CTRL0_UTMISWRST);
-	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	/* OTG configuration */
-	phyotg = readl(regs + EXYNOS5_PHY_OTG_SYS);
-
-	/* phy reference clock configuration */
-	phyotg &= ~OTG_SYS_FSEL_MASK;
-	phyotg |= OTG_SYS_FSEL(phyclk);
-
-	/* Enable normal mode of operation */
-	phyotg &= ~(OTG_SYS_FORCESUSPEND |
-			OTG_SYS_SIDDQ_UOTG |
-			OTG_SYS_FORCESLEEP |
-			OTG_SYS_REFCLKSEL_MASK |
-			/* COMMON Block configuration during suspend */
-			OTG_SYS_COMMON_ON);
-
-	/* OTG phy & link reset */
-	phyotg |= (OTG_SYS_PHY0_SWRST |
-			OTG_SYS_LINKSWRST_UOTG |
-			OTG_SYS_PHYLINK_SWRESET |
-			OTG_SYS_OTGDISABLE |
-			/* Set phy refclk */
-			OTG_SYS_REFCLKSEL_CLKCORE);
-
-	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
-	udelay(10);
-	phyotg &= ~(OTG_SYS_PHY0_SWRST |
-			OTG_SYS_LINKSWRST_UOTG |
-			OTG_SYS_PHYLINK_SWRESET);
-	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
-
-	/* HSIC phy configuration */
-	phyhsic = (HSIC_CTRL_REFCLKDIV_12 |
-			HSIC_CTRL_REFCLKSEL |
-			HSIC_CTRL_PHYSWRST);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
-	udelay(10);
-	phyhsic &= ~HSIC_CTRL_PHYSWRST;
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
-
-	udelay(80);
-
-	/* enable EHCI DMA burst */
-	ehcictrl = readl(regs + EXYNOS5_PHY_HOST_EHCICTRL);
-	ehcictrl |= (HOST_EHCICTRL_ENAINCRXALIGN |
-				HOST_EHCICTRL_ENAINCR4 |
-				HOST_EHCICTRL_ENAINCR8 |
-				HOST_EHCICTRL_ENAINCR16);
-	writel(ehcictrl, regs + EXYNOS5_PHY_HOST_EHCICTRL);
-
-	/* set ohci_suspend_on_n */
-	ohcictrl = readl(regs + EXYNOS5_PHY_HOST_OHCICTRL);
-	ohcictrl |= HOST_OHCICTRL_SUSPLGCY;
-	writel(ohcictrl, regs + EXYNOS5_PHY_HOST_OHCICTRL);
-}
-
-static void samsung_usb2phy_enable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phypwr;
-	u32 phyclk;
-	u32 rstcon;
-
-	/* set clock frequency for PLL */
-	phyclk = sphy->ref_clk_freq;
-	phypwr = readl(regs + SAMSUNG_PHYPWR);
-	rstcon = readl(regs + SAMSUNG_RSTCON);
-
-	switch (sphy->drv_data->cpu_type) {
-	case TYPE_S3C64XX:
-		phyclk &= ~PHYCLK_COMMON_ON_N;
-		phypwr &= ~PHYPWR_NORMAL_MASK;
-		rstcon |= RSTCON_SWRST;
-		break;
-	case TYPE_EXYNOS4X12:
-		phypwr &= ~(PHYPWR_NORMAL_MASK_HSIC0 |
-				PHYPWR_NORMAL_MASK_HSIC1 |
-				PHYPWR_NORMAL_MASK_PHY1);
-		rstcon |= RSTCON_HOSTPHY_SWRST;
-	case TYPE_EXYNOS4210:
-		phypwr &= ~PHYPWR_NORMAL_MASK_PHY0;
-		rstcon |= RSTCON_SWRST;
-	default:
-		break;
-	}
-
-	writel(phyclk, regs + SAMSUNG_PHYCLK);
-	/* Configure PHY0 for normal operation*/
-	writel(phypwr, regs + SAMSUNG_PHYPWR);
-	/* reset all ports of PHY and Link */
-	writel(rstcon, regs + SAMSUNG_RSTCON);
-	udelay(10);
-	if (sphy->drv_data->cpu_type == TYPE_EXYNOS4X12)
-		rstcon &= ~RSTCON_HOSTPHY_SWRST;
-	rstcon &= ~RSTCON_SWRST;
-	writel(rstcon, regs + SAMSUNG_RSTCON);
-}
-
-static void samsung_exynos5_usb2phy_disable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phyhost;
-	u32 phyotg;
-	u32 phyhsic;
-
-	if (atomic_dec_return(&sphy->phy_usage) > 0) {
-		dev_info(sphy->dev, "still being used\n");
-		return;
-	}
-
-	phyhsic = (HSIC_CTRL_REFCLKDIV_12 |
-			HSIC_CTRL_REFCLKSEL |
-			HSIC_CTRL_SIDDQ |
-			HSIC_CTRL_FORCESLEEP |
-			HSIC_CTRL_FORCESUSPEND);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL1);
-	writel(phyhsic, regs + EXYNOS5_PHY_HSIC_CTRL2);
-
-	phyhost = readl(regs + EXYNOS5_PHY_HOST_CTRL0);
-	phyhost |= (HOST_CTRL0_SIDDQ |
-			HOST_CTRL0_FORCESUSPEND |
-			HOST_CTRL0_FORCESLEEP |
-			HOST_CTRL0_PHYSWRST |
-			HOST_CTRL0_PHYSWRSTALL);
-	writel(phyhost, regs + EXYNOS5_PHY_HOST_CTRL0);
-
-	phyotg = readl(regs + EXYNOS5_PHY_OTG_SYS);
-	phyotg |= (OTG_SYS_FORCESUSPEND |
-			OTG_SYS_SIDDQ_UOTG |
-			OTG_SYS_FORCESLEEP);
-	writel(phyotg, regs + EXYNOS5_PHY_OTG_SYS);
-}
-
-static void samsung_usb2phy_disable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phypwr;
-
-	phypwr = readl(regs + SAMSUNG_PHYPWR);
-
-	switch (sphy->drv_data->cpu_type) {
-	case TYPE_S3C64XX:
-		phypwr |= PHYPWR_NORMAL_MASK;
-		break;
-	case TYPE_EXYNOS4X12:
-		phypwr |= (PHYPWR_NORMAL_MASK_HSIC0 |
-				PHYPWR_NORMAL_MASK_HSIC1 |
-				PHYPWR_NORMAL_MASK_PHY1);
-	case TYPE_EXYNOS4210:
-		phypwr |= PHYPWR_NORMAL_MASK_PHY0;
-	default:
-		break;
-	}
-
-	/* Disable analog and otg block power */
-	writel(phypwr, regs + SAMSUNG_PHYPWR);
-}
-
-/*
- * The function passed to the usb driver for phy initialization
- */
-static int samsung_usb2phy_init(struct usb_phy *phy)
-{
-	struct samsung_usbphy *sphy;
-	struct usb_bus *host = NULL;
-	unsigned long flags;
-	int ret = 0;
-
-	sphy = phy_to_sphy(phy);
-
-	host = phy->otg->host;
-
-	/* Enable the phy clock */
-	ret = clk_prepare_enable(sphy->clk);
-	if (ret) {
-		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
-		return ret;
-	}
-
-	spin_lock_irqsave(&sphy->lock, flags);
-
-	if (host) {
-		/* setting default phy-type for USB 2.0 */
-		if (!strstr(dev_name(host->controller), "ehci") ||
-				!strstr(dev_name(host->controller), "ohci"))
-			samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_HOST);
-	} else {
-		samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
-	}
-
-	/* Disable phy isolation */
-	if (sphy->plat && sphy->plat->pmu_isolation)
-		sphy->plat->pmu_isolation(false);
-	else if (sphy->drv_data->set_isolation)
-		sphy->drv_data->set_isolation(sphy, false);
-
-	/* Selecting Host/OTG mode; After reset USB2.0PHY_CFG: HOST */
-	samsung_usbphy_cfg_sel(sphy);
-
-	/* Initialize usb phy registers */
-	sphy->drv_data->phy_enable(sphy);
-
-	spin_unlock_irqrestore(&sphy->lock, flags);
-
-	/* Disable the phy clock */
-	clk_disable_unprepare(sphy->clk);
-
-	return ret;
-}
-
-/*
- * The function passed to the usb driver for phy shutdown
- */
-static void samsung_usb2phy_shutdown(struct usb_phy *phy)
-{
-	struct samsung_usbphy *sphy;
-	struct usb_bus *host = NULL;
-	unsigned long flags;
-
-	sphy = phy_to_sphy(phy);
-
-	host = phy->otg->host;
-
-	if (clk_prepare_enable(sphy->clk)) {
-		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
-		return;
-	}
-
-	spin_lock_irqsave(&sphy->lock, flags);
-
-	if (host) {
-		/* setting default phy-type for USB 2.0 */
-		if (!strstr(dev_name(host->controller), "ehci") ||
-				!strstr(dev_name(host->controller), "ohci"))
-			samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_HOST);
-	} else {
-		samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
-	}
-
-	/* De-initialize usb phy registers */
-	sphy->drv_data->phy_disable(sphy);
-
-	/* Enable phy isolation */
-	if (sphy->plat && sphy->plat->pmu_isolation)
-		sphy->plat->pmu_isolation(true);
-	else if (sphy->drv_data->set_isolation)
-		sphy->drv_data->set_isolation(sphy, true);
-
-	spin_unlock_irqrestore(&sphy->lock, flags);
-
-	clk_disable_unprepare(sphy->clk);
-}
-
-static int samsung_usb2phy_probe(struct platform_device *pdev)
-{
-	struct samsung_usbphy *sphy;
-	struct usb_otg *otg;
-	struct samsung_usbphy_data *pdata = dev_get_platdata(&pdev->dev);
-	const struct samsung_usbphy_drvdata *drv_data;
-	struct device *dev = &pdev->dev;
-	struct resource *phy_mem;
-	void __iomem	*phy_base;
-	struct clk *clk;
-	int ret;
-
-	phy_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	phy_base = devm_ioremap_resource(dev, phy_mem);
-	if (IS_ERR(phy_base))
-		return PTR_ERR(phy_base);
-
-	sphy = devm_kzalloc(dev, sizeof(*sphy), GFP_KERNEL);
-	if (!sphy)
-		return -ENOMEM;
-
-	otg = devm_kzalloc(dev, sizeof(*otg), GFP_KERNEL);
-	if (!otg)
-		return -ENOMEM;
-
-	drv_data = samsung_usbphy_get_driver_data(pdev);
-
-	if (drv_data->cpu_type == TYPE_EXYNOS5250)
-		clk = devm_clk_get(dev, "usbhost");
-	else
-		clk = devm_clk_get(dev, "otg");
-
-	if (IS_ERR(clk)) {
-		dev_err(dev, "Failed to get usbhost/otg clock\n");
-		return PTR_ERR(clk);
-	}
-
-	sphy->dev = dev;
-
-	if (dev->of_node) {
-		ret = samsung_usbphy_parse_dt(sphy);
-		if (ret < 0)
-			return ret;
-	} else {
-		if (!pdata) {
-			dev_err(dev, "no platform data specified\n");
-			return -EINVAL;
-		}
-	}
-
-	sphy->plat		= pdata;
-	sphy->regs		= phy_base;
-	sphy->clk		= clk;
-	sphy->drv_data		= drv_data;
-	sphy->phy.dev		= sphy->dev;
-	sphy->phy.label		= "samsung-usb2phy";
-	sphy->phy.type		= USB_PHY_TYPE_USB2;
-	sphy->phy.init		= samsung_usb2phy_init;
-	sphy->phy.shutdown	= samsung_usb2phy_shutdown;
-
-	sphy->ref_clk_freq = samsung_usbphy_get_refclk_freq(sphy);
-	if (sphy->ref_clk_freq < 0)
-		return -EINVAL;
-
-	sphy->phy.otg		= otg;
-	sphy->phy.otg->phy	= &sphy->phy;
-	sphy->phy.otg->set_host = samsung_usbphy_set_host;
-
-	spin_lock_init(&sphy->lock);
-
-	platform_set_drvdata(pdev, sphy);
-
-	return usb_add_phy_dev(&sphy->phy);
-}
-
-static int samsung_usb2phy_remove(struct platform_device *pdev)
-{
-	struct samsung_usbphy *sphy = platform_get_drvdata(pdev);
-
-	usb_remove_phy(&sphy->phy);
-
-	if (sphy->pmuregs)
-		iounmap(sphy->pmuregs);
-	if (sphy->sysreg)
-		iounmap(sphy->sysreg);
-
-	return 0;
-}
-
-static const struct samsung_usbphy_drvdata usb2phy_s3c64xx = {
-	.cpu_type		= TYPE_S3C64XX,
-	.devphy_en_mask		= S3C64XX_USBPHY_ENABLE,
-	.rate_to_clksel		= samsung_usbphy_rate_to_clksel_64xx,
-	.set_isolation		= NULL, /* TODO */
-	.phy_enable		= samsung_usb2phy_enable,
-	.phy_disable		= samsung_usb2phy_disable,
-};
-
-static const struct samsung_usbphy_drvdata usb2phy_exynos4 = {
-	.cpu_type		= TYPE_EXYNOS4210,
-	.devphy_en_mask		= EXYNOS_USBPHY_ENABLE,
-	.hostphy_en_mask	= EXYNOS_USBPHY_ENABLE,
-	.rate_to_clksel		= samsung_usbphy_rate_to_clksel_64xx,
-	.set_isolation		= samsung_usbphy_set_isolation_4210,
-	.phy_enable		= samsung_usb2phy_enable,
-	.phy_disable		= samsung_usb2phy_disable,
-};
-
-static const struct samsung_usbphy_drvdata usb2phy_exynos4x12 = {
-	.cpu_type		= TYPE_EXYNOS4X12,
-	.devphy_en_mask		= EXYNOS_USBPHY_ENABLE,
-	.hostphy_en_mask	= EXYNOS_USBPHY_ENABLE,
-	.rate_to_clksel		= samsung_usbphy_rate_to_clksel_4x12,
-	.set_isolation		= samsung_usbphy_set_isolation_4210,
-	.phy_enable		= samsung_usb2phy_enable,
-	.phy_disable		= samsung_usb2phy_disable,
-};
-
-static struct samsung_usbphy_drvdata usb2phy_exynos5 = {
-	.cpu_type		= TYPE_EXYNOS5250,
-	.hostphy_en_mask	= EXYNOS_USBPHY_ENABLE,
-	.hostphy_reg_offset	= EXYNOS_USBHOST_PHY_CTRL_OFFSET,
-	.rate_to_clksel		= samsung_usbphy_rate_to_clksel_4x12,
-	.set_isolation		= samsung_usbphy_set_isolation_4210,
-	.phy_enable		= samsung_exynos5_usb2phy_enable,
-	.phy_disable		= samsung_exynos5_usb2phy_disable,
-};
-
-#ifdef CONFIG_OF
-static const struct of_device_id samsung_usbphy_dt_match[] = {
-	{
-		.compatible = "samsung,s3c64xx-usb2phy",
-		.data = &usb2phy_s3c64xx,
-	}, {
-		.compatible = "samsung,exynos4210-usb2phy",
-		.data = &usb2phy_exynos4,
-	}, {
-		.compatible = "samsung,exynos4x12-usb2phy",
-		.data = &usb2phy_exynos4x12,
-	}, {
-		.compatible = "samsung,exynos5250-usb2phy",
-		.data = &usb2phy_exynos5
-	},
-	{},
-};
-MODULE_DEVICE_TABLE(of, samsung_usbphy_dt_match);
-#endif
-
-static struct platform_device_id samsung_usbphy_driver_ids[] = {
-	{
-		.name		= "s3c64xx-usb2phy",
-		.driver_data	= (unsigned long)&usb2phy_s3c64xx,
-	}, {
-		.name		= "exynos4210-usb2phy",
-		.driver_data	= (unsigned long)&usb2phy_exynos4,
-	}, {
-		.name		= "exynos4x12-usb2phy",
-		.driver_data	= (unsigned long)&usb2phy_exynos4x12,
-	}, {
-		.name		= "exynos5250-usb2phy",
-		.driver_data	= (unsigned long)&usb2phy_exynos5,
-	},
-	{},
-};
-
-MODULE_DEVICE_TABLE(platform, samsung_usbphy_driver_ids);
-
-static struct platform_driver samsung_usb2phy_driver = {
-	.probe		= samsung_usb2phy_probe,
-	.remove		= samsung_usb2phy_remove,
-	.id_table	= samsung_usbphy_driver_ids,
-	.driver		= {
-		.name	= "samsung-usb2phy",
-		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(samsung_usbphy_dt_match),
-	},
-};
-
-module_platform_driver(samsung_usb2phy_driver);
-
-MODULE_DESCRIPTION("Samsung USB 2.0 phy controller");
-MODULE_AUTHOR("Praveen Paneri <p.paneri@samsung.com>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:samsung-usb2phy");
diff --git a/drivers/usb/phy/phy-samsung-usb3.c b/drivers/usb/phy/phy-samsung-usb3.c
deleted file mode 100644
index cc0819248acf..000000000000
--- a/drivers/usb/phy/phy-samsung-usb3.c
+++ /dev/null
@@ -1,350 +0,0 @@
-/* linux/drivers/usb/phy/phy-samsung-usb3.c
- *
- * Copyright (c) 2013 Samsung Electronics Co., Ltd.
- *              http://www.samsung.com
- *
- * Author: Vivek Gautam <gautam.vivek@samsung.com>
- *
- * Samsung USB 3.0 PHY transceiver; talks to DWC3 controller.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- */
-
-#include <linux/module.h>
-#include <linux/platform_device.h>
-#include <linux/clk.h>
-#include <linux/delay.h>
-#include <linux/err.h>
-#include <linux/io.h>
-#include <linux/of.h>
-#include <linux/usb/samsung_usb_phy.h>
-#include <linux/platform_data/samsung-usbphy.h>
-
-#include "phy-samsung-usb.h"
-
-/*
- * Sets the phy clk as EXTREFCLK (XXTI) which is internal clock from clock core.
- */
-static u32 samsung_usb3phy_set_refclk(struct samsung_usbphy *sphy)
-{
-	u32 reg;
-	u32 refclk;
-
-	refclk = sphy->ref_clk_freq;
-
-	reg = PHYCLKRST_REFCLKSEL_EXT_REFCLK |
-		PHYCLKRST_FSEL(refclk);
-
-	switch (refclk) {
-	case FSEL_CLKSEL_50M:
-		reg |= (PHYCLKRST_MPLL_MULTIPLIER_50M_REF |
-			PHYCLKRST_SSC_REFCLKSEL(0x00));
-		break;
-	case FSEL_CLKSEL_20M:
-		reg |= (PHYCLKRST_MPLL_MULTIPLIER_20MHZ_REF |
-			PHYCLKRST_SSC_REFCLKSEL(0x00));
-		break;
-	case FSEL_CLKSEL_19200K:
-		reg |= (PHYCLKRST_MPLL_MULTIPLIER_19200KHZ_REF |
-			PHYCLKRST_SSC_REFCLKSEL(0x88));
-		break;
-	case FSEL_CLKSEL_24M:
-	default:
-		reg |= (PHYCLKRST_MPLL_MULTIPLIER_24MHZ_REF |
-			PHYCLKRST_SSC_REFCLKSEL(0x88));
-		break;
-	}
-
-	return reg;
-}
-
-static void samsung_exynos5_usb3phy_enable(struct samsung_usbphy *sphy)
-{
-	void __iomem *regs = sphy->regs;
-	u32 phyparam0;
-	u32 phyparam1;
-	u32 linksystem;
-	u32 phybatchg;
-	u32 phytest;
-	u32 phyclkrst;
-
-	/* Reset USB 3.0 PHY */
-	writel(0x0, regs + EXYNOS5_DRD_PHYREG0);
-
-	phyparam0 = readl(regs + EXYNOS5_DRD_PHYPARAM0);
-	/* Select PHY CLK source */
-	phyparam0 &= ~PHYPARAM0_REF_USE_PAD;
-	/* Set Loss-of-Signal Detector sensitivity */
-	phyparam0 &= ~PHYPARAM0_REF_LOSLEVEL_MASK;
-	phyparam0 |= PHYPARAM0_REF_LOSLEVEL;
-	writel(phyparam0, regs + EXYNOS5_DRD_PHYPARAM0);
-
-	writel(0x0, regs + EXYNOS5_DRD_PHYRESUME);
-
-	/*
-	 * Setting the Frame length Adj value[6:1] to default 0x20
-	 * See xHCI 1.0 spec, 5.2.4
-	 */
-	linksystem = LINKSYSTEM_XHCI_VERSION_CONTROL |
-			LINKSYSTEM_FLADJ(0x20);
-	writel(linksystem, regs + EXYNOS5_DRD_LINKSYSTEM);
-
-	phyparam1 = readl(regs + EXYNOS5_DRD_PHYPARAM1);
-	/* Set Tx De-Emphasis level */
-	phyparam1 &= ~PHYPARAM1_PCS_TXDEEMPH_MASK;
-	phyparam1 |= PHYPARAM1_PCS_TXDEEMPH;
-	writel(phyparam1, regs + EXYNOS5_DRD_PHYPARAM1);
-
-	phybatchg = readl(regs + EXYNOS5_DRD_PHYBATCHG);
-	phybatchg |= PHYBATCHG_UTMI_CLKSEL;
-	writel(phybatchg, regs + EXYNOS5_DRD_PHYBATCHG);
-
-	/* PHYTEST POWERDOWN Control */
-	phytest = readl(regs + EXYNOS5_DRD_PHYTEST);
-	phytest &= ~(PHYTEST_POWERDOWN_SSP |
-			PHYTEST_POWERDOWN_HSP);
-	writel(phytest, regs + EXYNOS5_DRD_PHYTEST);
-
-	/* UTMI Power Control */
-	writel(PHYUTMI_OTGDISABLE, regs + EXYNOS5_DRD_PHYUTMI);
-
-	phyclkrst = samsung_usb3phy_set_refclk(sphy);
-
-	phyclkrst |= PHYCLKRST_PORTRESET |
-			/* Digital power supply in normal operating mode */
-			PHYCLKRST_RETENABLEN |
-			/* Enable ref clock for SS function */
-			PHYCLKRST_REF_SSP_EN |
-			/* Enable spread spectrum */
-			PHYCLKRST_SSC_EN |
-			/* Power down HS Bias and PLL blocks in suspend mode */
-			PHYCLKRST_COMMONONN;
-
-	writel(phyclkrst, regs + EXYNOS5_DRD_PHYCLKRST);
-
-	udelay(10);
-
-	phyclkrst &= ~(PHYCLKRST_PORTRESET);
-	writel(phyclkrst, regs + EXYNOS5_DRD_PHYCLKRST);
-}
-
-static void samsung_exynos5_usb3phy_disable(struct samsung_usbphy *sphy)
-{
-	u32 phyutmi;
-	u32 phyclkrst;
-	u32 phytest;
-	void __iomem *regs = sphy->regs;
-
-	phyutmi = PHYUTMI_OTGDISABLE |
-			PHYUTMI_FORCESUSPEND |
-			PHYUTMI_FORCESLEEP;
-	writel(phyutmi, regs + EXYNOS5_DRD_PHYUTMI);
-
-	/* Resetting the PHYCLKRST enable bits to reduce leakage current */
-	phyclkrst = readl(regs + EXYNOS5_DRD_PHYCLKRST);
-	phyclkrst &= ~(PHYCLKRST_REF_SSP_EN |
-			PHYCLKRST_SSC_EN |
-			PHYCLKRST_COMMONONN);
-	writel(phyclkrst, regs + EXYNOS5_DRD_PHYCLKRST);
-
-	/* Control PHYTEST to remove leakage current */
-	phytest = readl(regs + EXYNOS5_DRD_PHYTEST);
-	phytest |= (PHYTEST_POWERDOWN_SSP |
-			PHYTEST_POWERDOWN_HSP);
-	writel(phytest, regs + EXYNOS5_DRD_PHYTEST);
-}
-
-static int samsung_usb3phy_init(struct usb_phy *phy)
-{
-	struct samsung_usbphy *sphy;
-	unsigned long flags;
-	int ret = 0;
-
-	sphy = phy_to_sphy(phy);
-
-	/* Enable the phy clock */
-	ret = clk_prepare_enable(sphy->clk);
-	if (ret) {
-		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
-		return ret;
-	}
-
-	spin_lock_irqsave(&sphy->lock, flags);
-
-	/* setting default phy-type for USB 3.0 */
-	samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
-
-	/* Disable phy isolation */
-	if (sphy->drv_data->set_isolation)
-		sphy->drv_data->set_isolation(sphy, false);
-
-	/* Initialize usb phy registers */
-	sphy->drv_data->phy_enable(sphy);
-
-	spin_unlock_irqrestore(&sphy->lock, flags);
-
-	/* Disable the phy clock */
-	clk_disable_unprepare(sphy->clk);
-
-	return ret;
-}
-
-/*
- * The function passed to the usb driver for phy shutdown
- */
-static void samsung_usb3phy_shutdown(struct usb_phy *phy)
-{
-	struct samsung_usbphy *sphy;
-	unsigned long flags;
-
-	sphy = phy_to_sphy(phy);
-
-	if (clk_prepare_enable(sphy->clk)) {
-		dev_err(sphy->dev, "%s: clk_prepare_enable failed\n", __func__);
-		return;
-	}
-
-	spin_lock_irqsave(&sphy->lock, flags);
-
-	/* setting default phy-type for USB 3.0 */
-	samsung_usbphy_set_type(&sphy->phy, USB_PHY_TYPE_DEVICE);
-
-	/* De-initialize usb phy registers */
-	sphy->drv_data->phy_disable(sphy);
-
-	/* Enable phy isolation */
-	if (sphy->drv_data->set_isolation)
-		sphy->drv_data->set_isolation(sphy, true);
-
-	spin_unlock_irqrestore(&sphy->lock, flags);
-
-	clk_disable_unprepare(sphy->clk);
-}
-
-static int samsung_usb3phy_probe(struct platform_device *pdev)
-{
-	struct samsung_usbphy *sphy;
-	struct samsung_usbphy_data *pdata = dev_get_platdata(&pdev->dev);
-	struct device *dev = &pdev->dev;
-	struct resource *phy_mem;
-	void __iomem	*phy_base;
-	struct clk *clk;
-	int ret;
-
-	phy_mem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	phy_base = devm_ioremap_resource(dev, phy_mem);
-	if (IS_ERR(phy_base))
-		return PTR_ERR(phy_base);
-
-	sphy = devm_kzalloc(dev, sizeof(*sphy), GFP_KERNEL);
-	if (!sphy)
-		return -ENOMEM;
-
-	clk = devm_clk_get(dev, "usbdrd30");
-	if (IS_ERR(clk)) {
-		dev_err(dev, "Failed to get device clock\n");
-		return PTR_ERR(clk);
-	}
-
-	sphy->dev = dev;
-
-	if (dev->of_node) {
-		ret = samsung_usbphy_parse_dt(sphy);
-		if (ret < 0)
-			return ret;
-	} else {
-		if (!pdata) {
-			dev_err(dev, "no platform data specified\n");
-			return -EINVAL;
-		}
-	}
-
-	sphy->plat		= pdata;
-	sphy->regs		= phy_base;
-	sphy->clk		= clk;
-	sphy->phy.dev		= sphy->dev;
-	sphy->phy.label		= "samsung-usb3phy";
-	sphy->phy.type		= USB_PHY_TYPE_USB3;
-	sphy->phy.init		= samsung_usb3phy_init;
-	sphy->phy.shutdown	= samsung_usb3phy_shutdown;
-	sphy->drv_data		= samsung_usbphy_get_driver_data(pdev);
-
-	sphy->ref_clk_freq = samsung_usbphy_get_refclk_freq(sphy);
-	if (sphy->ref_clk_freq < 0)
-		return -EINVAL;
-
-	spin_lock_init(&sphy->lock);
-
-	platform_set_drvdata(pdev, sphy);
-
-	return usb_add_phy_dev(&sphy->phy);
-}
-
-static int samsung_usb3phy_remove(struct platform_device *pdev)
-{
-	struct samsung_usbphy *sphy = platform_get_drvdata(pdev);
-
-	usb_remove_phy(&sphy->phy);
-
-	if (sphy->pmuregs)
-		iounmap(sphy->pmuregs);
-	if (sphy->sysreg)
-		iounmap(sphy->sysreg);
-
-	return 0;
-}
-
-static struct samsung_usbphy_drvdata usb3phy_exynos5 = {
-	.cpu_type		= TYPE_EXYNOS5250,
-	.devphy_en_mask		= EXYNOS_USBPHY_ENABLE,
-	.rate_to_clksel		= samsung_usbphy_rate_to_clksel_4x12,
-	.set_isolation		= samsung_usbphy_set_isolation_4210,
-	.phy_enable		= samsung_exynos5_usb3phy_enable,
-	.phy_disable		= samsung_exynos5_usb3phy_disable,
-};
-
-#ifdef CONFIG_OF
-static const struct of_device_id samsung_usbphy_dt_match[] = {
-	{
-		.compatible = "samsung,exynos5250-usb3phy",
-		.data = &usb3phy_exynos5
-	},
-	{},
-};
-MODULE_DEVICE_TABLE(of, samsung_usbphy_dt_match);
-#endif
-
-static struct platform_device_id samsung_usbphy_driver_ids[] = {
-	{
-		.name		= "exynos5250-usb3phy",
-		.driver_data	= (unsigned long)&usb3phy_exynos5,
-	},
-	{},
-};
-
-MODULE_DEVICE_TABLE(platform, samsung_usbphy_driver_ids);
-
-static struct platform_driver samsung_usb3phy_driver = {
-	.probe		= samsung_usb3phy_probe,
-	.remove		= samsung_usb3phy_remove,
-	.id_table	= samsung_usbphy_driver_ids,
-	.driver		= {
-		.name	= "samsung-usb3phy",
-		.owner	= THIS_MODULE,
-		.of_match_table = of_match_ptr(samsung_usbphy_dt_match),
-	},
-};
-
-module_platform_driver(samsung_usb3phy_driver);
-
-MODULE_DESCRIPTION("Samsung USB 3.0 phy controller");
-MODULE_AUTHOR("Vivek Gautam <gautam.vivek@samsung.com>");
-MODULE_LICENSE("GPL");
-MODULE_ALIAS("platform:samsung-usb3phy");
diff --git a/drivers/usb/phy/phy-tegra-usb.c b/drivers/usb/phy/phy-tegra-usb.c
index 13b4fa287da8..886f1807a67b 100644
--- a/drivers/usb/phy/phy-tegra-usb.c
+++ b/drivers/usb/phy/phy-tegra-usb.c
@@ -878,8 +878,8 @@ static int utmi_phy_probe(struct tegra_usb_phy *tegra_phy,
 		return -ENOMEM;
 	}
 
-	tegra_phy->config = devm_kzalloc(&pdev->dev,
-		sizeof(*tegra_phy->config), GFP_KERNEL);
+	tegra_phy->config = devm_kzalloc(&pdev->dev, sizeof(*config),
+					 GFP_KERNEL);
 	if (!tegra_phy->config) {
 		dev_err(&pdev->dev,
 			"unable to allocate memory for USB UTMIP config\n");
diff --git a/drivers/usb/phy/phy-twl6030-usb.c b/drivers/usb/phy/phy-twl6030-usb.c
index 04778cf80d60..44ea082e40dc 100644
--- a/drivers/usb/phy/phy-twl6030-usb.c
+++ b/drivers/usb/phy/phy-twl6030-usb.c
@@ -104,7 +104,6 @@ struct twl6030_usb {
 	int			irq2;
 	enum omap_musb_vbus_id_status linkstat;
 	u8			asleep;
-	bool			irq_enabled;
 	bool			vbus_enable;
 	const char		*regulator;
 };
@@ -373,7 +372,6 @@ static int twl6030_usb_probe(struct platform_device *pdev)
 
 	INIT_WORK(&twl->set_vbus_work, otg_set_vbus_work);
 
-	twl->irq_enabled = true;
 	status = request_threaded_irq(twl->irq1, NULL, twl6030_usbotg_irq,
 			IRQF_TRIGGER_FALLING | IRQF_TRIGGER_RISING | IRQF_ONESHOT,
 			"twl6030_usb", twl);
diff --git a/drivers/usb/phy/phy.c b/drivers/usb/phy/phy.c
index 6d0f6080eceb..045cd309367a 100644
--- a/drivers/usb/phy/phy.c
+++ b/drivers/usb/phy/phy.c
@@ -232,6 +232,9 @@ struct usb_phy *usb_get_phy_dev(struct device *dev, u8 index)
 	phy = __usb_find_phy_dev(dev, &phy_bind_list, index);
 	if (IS_ERR(phy) || !try_module_get(phy->dev->driver->owner)) {
 		dev_dbg(dev, "unable to find transceiver\n");
+		if (!IS_ERR(phy))
+			phy = ERR_PTR(-ENODEV);
+
 		goto err0;
 	}
 
diff --git a/drivers/usb/renesas_usbhs/Kconfig b/drivers/usb/renesas_usbhs/Kconfig
index 1c4195abc108..de83b9d0cd5c 100644
--- a/drivers/usb/renesas_usbhs/Kconfig
+++ b/drivers/usb/renesas_usbhs/Kconfig
@@ -5,6 +5,7 @@
 config USB_RENESAS_USBHS
 	tristate 'Renesas USBHS controller'
 	depends on USB_GADGET
+	depends on ARCH_SHMOBILE || SUPERH || COMPILE_TEST
 	default n
 	help
 	  Renesas USBHS is a discrete USB host and peripheral controller chip
diff --git a/drivers/usb/renesas_usbhs/common.c b/drivers/usb/renesas_usbhs/common.c
index 1b9bf8d83235..b3b6813ab270 100644
--- a/drivers/usb/renesas_usbhs/common.c
+++ b/drivers/usb/renesas_usbhs/common.c
@@ -18,6 +18,8 @@
 #include <linux/gpio.h>
 #include <linux/io.h>
 #include <linux/module.h>
+#include <linux/of_device.h>
+#include <linux/of_gpio.h>
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 #include <linux/sysfs.h>
@@ -438,6 +440,43 @@ static int usbhsc_drvcllbck_notify_hotplug(struct platform_device *pdev)
 /*
  *		platform functions
  */
+static const struct of_device_id usbhs_of_match[] = {
+	{
+		.compatible = "renesas,usbhs-r8a7790",
+		.data = (void *)USBHS_TYPE_R8A7790,
+	},
+	{
+		.compatible = "renesas,usbhs-r8a7791",
+		.data = (void *)USBHS_TYPE_R8A7791,
+	},
+	{ },
+};
+MODULE_DEVICE_TABLE(of, usbhs_of_match);
+
+static struct renesas_usbhs_platform_info *usbhs_parse_dt(struct device *dev)
+{
+	struct renesas_usbhs_platform_info *info;
+	struct renesas_usbhs_driver_param *dparam;
+	const struct of_device_id *of_id = of_match_device(usbhs_of_match, dev);
+	u32 tmp;
+	int gpio;
+
+	info = devm_kzalloc(dev, sizeof(*info), GFP_KERNEL);
+	if (!info)
+		return NULL;
+
+	dparam = &info->driver_param;
+	dparam->type = of_id ? (u32)of_id->data : 0;
+	if (!of_property_read_u32(dev->of_node, "renesas,buswait", &tmp))
+		dparam->buswait_bwait = tmp;
+	gpio = of_get_named_gpio_flags(dev->of_node, "renesas,enable-gpio", 0,
+				       NULL);
+	if (gpio > 0)
+		dparam->enable_gpio = gpio;
+
+	return info;
+}
+
 static int usbhs_probe(struct platform_device *pdev)
 {
 	struct renesas_usbhs_platform_info *info = dev_get_platdata(&pdev->dev);
@@ -446,6 +485,10 @@ static int usbhs_probe(struct platform_device *pdev)
 	struct resource *res, *irq_res;
 	int ret;
 
+	/* check device node */
+	if (pdev->dev.of_node)
+		info = pdev->dev.platform_data = usbhs_parse_dt(&pdev->dev);
+
 	/* check platform information */
 	if (!info) {
 		dev_err(&pdev->dev, "no platform information\n");
@@ -689,6 +732,7 @@ static struct platform_driver renesas_usbhs_driver = {
 	.driver		= {
 		.name	= "renesas_usbhs",
 		.pm	= &usbhsc_pm_ops,
+		.of_match_table = of_match_ptr(usbhs_of_match),
 	},
 	.probe		= usbhs_probe,
 	.remove		= usbhs_remove,
diff --git a/drivers/usb/renesas_usbhs/fifo.c b/drivers/usb/renesas_usbhs/fifo.c
index 4fd36530bfa3..b0c97a3f1bfe 100644
--- a/drivers/usb/renesas_usbhs/fifo.c
+++ b/drivers/usb/renesas_usbhs/fifo.c
@@ -108,19 +108,45 @@ static struct usbhs_pkt *__usbhsf_pkt_get(struct usbhs_pipe *pipe)
 	return list_first_entry(&pipe->list, struct usbhs_pkt, node);
 }
 
+static void usbhsf_fifo_clear(struct usbhs_pipe *pipe,
+			      struct usbhs_fifo *fifo);
+static void usbhsf_fifo_unselect(struct usbhs_pipe *pipe,
+				 struct usbhs_fifo *fifo);
+static struct dma_chan *usbhsf_dma_chan_get(struct usbhs_fifo *fifo,
+					    struct usbhs_pkt *pkt);
+#define usbhsf_dma_map(p)	__usbhsf_dma_map_ctrl(p, 1)
+#define usbhsf_dma_unmap(p)	__usbhsf_dma_map_ctrl(p, 0)
+static int __usbhsf_dma_map_ctrl(struct usbhs_pkt *pkt, int map);
 struct usbhs_pkt *usbhs_pkt_pop(struct usbhs_pipe *pipe, struct usbhs_pkt *pkt)
 {
 	struct usbhs_priv *priv = usbhs_pipe_to_priv(pipe);
+	struct usbhs_fifo *fifo = usbhs_pipe_to_fifo(pipe);
 	unsigned long flags;
 
 	/********************  spin lock ********************/
 	usbhs_lock(priv, flags);
 
+	usbhs_pipe_disable(pipe);
+
 	if (!pkt)
 		pkt = __usbhsf_pkt_get(pipe);
 
-	if (pkt)
+	if (pkt) {
+		struct dma_chan *chan = NULL;
+
+		if (fifo)
+			chan = usbhsf_dma_chan_get(fifo, pkt);
+		if (chan) {
+			dmaengine_terminate_all(chan);
+			usbhsf_fifo_clear(pipe, fifo);
+			usbhsf_dma_unmap(pkt);
+		}
+
 		__usbhsf_pkt_del(pkt);
+	}
+
+	if (fifo)
+		usbhsf_fifo_unselect(pipe, fifo);
 
 	usbhs_unlock(priv, flags);
 	/********************  spin unlock ******************/
@@ -544,6 +570,7 @@ static int usbhsf_pio_try_push(struct usbhs_pkt *pkt, int *is_done)
 		usbhsf_send_terminator(pipe, fifo);
 
 	usbhsf_tx_irq_ctrl(pipe, !*is_done);
+	usbhs_pipe_running(pipe, !*is_done);
 	usbhs_pipe_enable(pipe);
 
 	dev_dbg(dev, "  send %d (%d/ %d/ %d/ %d)\n",
@@ -570,12 +597,21 @@ usbhs_fifo_write_busy:
 	 * retry in interrupt
 	 */
 	usbhsf_tx_irq_ctrl(pipe, 1);
+	usbhs_pipe_running(pipe, 1);
 
 	return ret;
 }
 
+static int usbhsf_pio_prepare_push(struct usbhs_pkt *pkt, int *is_done)
+{
+	if (usbhs_pipe_is_running(pkt->pipe))
+		return 0;
+
+	return usbhsf_pio_try_push(pkt, is_done);
+}
+
 struct usbhs_pkt_handle usbhs_fifo_pio_push_handler = {
-	.prepare = usbhsf_pio_try_push,
+	.prepare = usbhsf_pio_prepare_push,
 	.try_run = usbhsf_pio_try_push,
 };
 
@@ -589,6 +625,9 @@ static int usbhsf_prepare_pop(struct usbhs_pkt *pkt, int *is_done)
 	if (usbhs_pipe_is_busy(pipe))
 		return 0;
 
+	if (usbhs_pipe_is_running(pipe))
+		return 0;
+
 	/*
 	 * pipe enable to prepare packet receive
 	 */
@@ -597,6 +636,7 @@ static int usbhsf_prepare_pop(struct usbhs_pkt *pkt, int *is_done)
 
 	usbhs_pipe_set_trans_count_if_bulk(pipe, pkt->length);
 	usbhs_pipe_enable(pipe);
+	usbhs_pipe_running(pipe, 1);
 	usbhsf_rx_irq_ctrl(pipe, 1);
 
 	return 0;
@@ -642,6 +682,7 @@ static int usbhsf_pio_try_pop(struct usbhs_pkt *pkt, int *is_done)
 	    (total_len < maxp)) {		/* short packet */
 		*is_done = 1;
 		usbhsf_rx_irq_ctrl(pipe, 0);
+		usbhs_pipe_running(pipe, 0);
 		usbhs_pipe_disable(pipe);	/* disable pipe first */
 	}
 
@@ -763,8 +804,6 @@ static void __usbhsf_dma_ctrl(struct usbhs_pipe *pipe,
 	usbhs_bset(priv, fifo->sel, DREQE, dreqe);
 }
 
-#define usbhsf_dma_map(p)	__usbhsf_dma_map_ctrl(p, 1)
-#define usbhsf_dma_unmap(p)	__usbhsf_dma_map_ctrl(p, 0)
 static int __usbhsf_dma_map_ctrl(struct usbhs_pkt *pkt, int map)
 {
 	struct usbhs_pipe *pipe = pkt->pipe;
@@ -805,6 +844,7 @@ static void xfer_work(struct work_struct *work)
 	dev_dbg(dev, "  %s %d (%d/ %d)\n",
 		fifo->name, usbhs_pipe_number(pipe), pkt->length, pkt->zero);
 
+	usbhs_pipe_running(pipe, 1);
 	usbhs_pipe_set_trans_count_if_bulk(pipe, pkt->trans);
 	usbhs_pipe_enable(pipe);
 	usbhsf_dma_start(pipe, fifo);
@@ -836,6 +876,10 @@ static int usbhsf_dma_prepare_push(struct usbhs_pkt *pkt, int *is_done)
 	if ((uintptr_t)(pkt->buf + pkt->actual) & 0x7) /* 8byte alignment */
 		goto usbhsf_pio_prepare_push;
 
+	/* return at this time if the pipe is running */
+	if (usbhs_pipe_is_running(pipe))
+		return 0;
+
 	/* get enable DMA fifo */
 	fifo = usbhsf_get_dma_fifo(priv, pkt);
 	if (!fifo)
@@ -869,15 +913,29 @@ usbhsf_pio_prepare_push:
 static int usbhsf_dma_push_done(struct usbhs_pkt *pkt, int *is_done)
 {
 	struct usbhs_pipe *pipe = pkt->pipe;
+	int is_short = pkt->trans % usbhs_pipe_get_maxpacket(pipe);
+
+	pkt->actual += pkt->trans;
 
-	pkt->actual = pkt->trans;
+	if (pkt->actual < pkt->length)
+		*is_done = 0;		/* there are remainder data */
+	else if (is_short)
+		*is_done = 1;		/* short packet */
+	else
+		*is_done = !pkt->zero;	/* send zero packet? */
 
-	*is_done = !pkt->zero;	/* send zero packet ? */
+	usbhs_pipe_running(pipe, !*is_done);
 
 	usbhsf_dma_stop(pipe, pipe->fifo);
 	usbhsf_dma_unmap(pkt);
 	usbhsf_fifo_unselect(pipe, pipe->fifo);
 
+	if (!*is_done) {
+		/* change handler to PIO */
+		pkt->handler = &usbhs_fifo_pio_push_handler;
+		return pkt->handler->try_run(pkt, is_done);
+	}
+
 	return 0;
 }
 
@@ -972,8 +1030,10 @@ static int usbhsf_dma_pop_done(struct usbhs_pkt *pkt, int *is_done)
 	if ((pkt->actual == pkt->length) ||	/* receive all data */
 	    (pkt->trans < maxp)) {		/* short packet */
 		*is_done = 1;
+		usbhs_pipe_running(pipe, 0);
 	} else {
 		/* re-enable */
+		usbhs_pipe_running(pipe, 0);
 		usbhsf_prepare_pop(pkt, is_done);
 	}
 
diff --git a/drivers/usb/renesas_usbhs/mod.c b/drivers/usb/renesas_usbhs/mod.c
index 6a030b931a3b..9a705b15b3a1 100644
--- a/drivers/usb/renesas_usbhs/mod.c
+++ b/drivers/usb/renesas_usbhs/mod.c
@@ -213,7 +213,10 @@ static int usbhs_status_get_each_irq(struct usbhs_priv *priv,
 {
 	struct usbhs_mod *mod = usbhs_mod_get_current(priv);
 	u16 intenb0, intenb1;
+	unsigned long flags;
 
+	/********************  spin lock ********************/
+	usbhs_lock(priv, flags);
 	state->intsts0 = usbhs_read(priv, INTSTS0);
 	state->intsts1 = usbhs_read(priv, INTSTS1);
 
@@ -229,6 +232,8 @@ static int usbhs_status_get_each_irq(struct usbhs_priv *priv,
 		state->bempsts &= mod->irq_bempsts;
 		state->brdysts &= mod->irq_brdysts;
 	}
+	usbhs_unlock(priv, flags);
+	/********************  spin unlock ******************/
 
 	/*
 	 * Check whether the irq enable registers and the irq status are set
diff --git a/drivers/usb/renesas_usbhs/mod_gadget.c b/drivers/usb/renesas_usbhs/mod_gadget.c
index 04e6505777d0..2d17c10a0428 100644
--- a/drivers/usb/renesas_usbhs/mod_gadget.c
+++ b/drivers/usb/renesas_usbhs/mod_gadget.c
@@ -129,7 +129,7 @@ static void usbhsg_queue_pop(struct usbhsg_uep *uep,
 	dev_dbg(dev, "pipe %d : queue pop\n", usbhs_pipe_number(pipe));
 
 	ureq->req.status = status;
-	ureq->req.complete(&uep->ep, &ureq->req);
+	usb_gadget_giveback_request(&uep->ep, &ureq->req);
 }
 
 static void usbhsg_queue_done(struct usbhs_priv *priv, struct usbhs_pkt *pkt)
diff --git a/drivers/usb/renesas_usbhs/pipe.c b/drivers/usb/renesas_usbhs/pipe.c
index 75fbcf6b102e..040bcefcb040 100644
--- a/drivers/usb/renesas_usbhs/pipe.c
+++ b/drivers/usb/renesas_usbhs/pipe.c
@@ -578,6 +578,19 @@ int usbhs_pipe_is_dir_host(struct usbhs_pipe *pipe)
 	return usbhsp_flags_has(pipe, IS_DIR_HOST);
 }
 
+int usbhs_pipe_is_running(struct usbhs_pipe *pipe)
+{
+	return usbhsp_flags_has(pipe, IS_RUNNING);
+}
+
+void usbhs_pipe_running(struct usbhs_pipe *pipe, int running)
+{
+	if (running)
+		usbhsp_flags_set(pipe, IS_RUNNING);
+	else
+		usbhsp_flags_clr(pipe, IS_RUNNING);
+}
+
 void usbhs_pipe_data_sequence(struct usbhs_pipe *pipe, int sequence)
 {
 	u16 mask = (SQCLR | SQSET);
diff --git a/drivers/usb/renesas_usbhs/pipe.h b/drivers/usb/renesas_usbhs/pipe.h
index 406f36d050e4..d24a05972370 100644
--- a/drivers/usb/renesas_usbhs/pipe.h
+++ b/drivers/usb/renesas_usbhs/pipe.h
@@ -36,6 +36,7 @@ struct usbhs_pipe {
 #define USBHS_PIPE_FLAGS_IS_USED		(1 << 0)
 #define USBHS_PIPE_FLAGS_IS_DIR_IN		(1 << 1)
 #define USBHS_PIPE_FLAGS_IS_DIR_HOST		(1 << 2)
+#define USBHS_PIPE_FLAGS_IS_RUNNING		(1 << 3)
 
 	struct usbhs_pkt_handle *handler;
 
@@ -80,6 +81,9 @@ int usbhs_pipe_probe(struct usbhs_priv *priv);
 void usbhs_pipe_remove(struct usbhs_priv *priv);
 int usbhs_pipe_is_dir_in(struct usbhs_pipe *pipe);
 int usbhs_pipe_is_dir_host(struct usbhs_pipe *pipe);
+int usbhs_pipe_is_running(struct usbhs_pipe *pipe);
+void usbhs_pipe_running(struct usbhs_pipe *pipe, int running);
+
 void usbhs_pipe_init(struct usbhs_priv *priv,
 		     int (*dma_map_ctrl)(struct usbhs_pkt *pkt, int map));
 int usbhs_pipe_get_maxpacket(struct usbhs_pipe *pipe);
diff --git a/drivers/usb/serial/Kconfig b/drivers/usb/serial/Kconfig
index 3ce5c74b29e4..a69f7cd9d0bf 100644
--- a/drivers/usb/serial/Kconfig
+++ b/drivers/usb/serial/Kconfig
@@ -58,9 +58,11 @@ config USB_SERIAL_SIMPLE
 	  handles a wide range of very simple devices, all in one
 	  driver.  Specifically, it supports:
 		- Suunto ANT+ USB device.
+		- Medtronic CareLink USB device
 		- Fundamental Software dongle.
 		- HP4x calculators
 		- a number of Motorola phones
+		- Novatel Wireless GPS receivers
 		- Siemens USB/MPI adapter.
 		- ViVOtech ViVOpay USB device.
 		- Infineon Modem Flashloader USB interface
@@ -682,14 +684,6 @@ config USB_SERIAL_WISHBONE
 	  To compile this driver as a module, choose M here: the
 	  module will be called wishbone-serial.
 
-config USB_SERIAL_ZTE
-	tristate "ZTE USB serial driver"
-	help
-	  Say Y here if you want to use a ZTE USB to serial device.
-
-	  To compile this driver as a module, choose M here: the
-	  module will be called zte.
-
 config USB_SERIAL_SSU100
 	tristate "USB Quatech SSU-100 Single Port Serial Driver"
 	help
diff --git a/drivers/usb/serial/Makefile b/drivers/usb/serial/Makefile
index bfdafd349441..349d9df0895f 100644
--- a/drivers/usb/serial/Makefile
+++ b/drivers/usb/serial/Makefile
@@ -60,4 +60,3 @@ obj-$(CONFIG_USB_SERIAL_WISHBONE)		+= wishbone-serial.o
 obj-$(CONFIG_USB_SERIAL_WHITEHEAT)		+= whiteheat.o
 obj-$(CONFIG_USB_SERIAL_XIRCOM)			+= keyspan_pda.o
 obj-$(CONFIG_USB_SERIAL_XSENS_MT)		+= xsens_mt.o
-obj-$(CONFIG_USB_SERIAL_ZTE)			+= zte_ev.o
diff --git a/drivers/usb/serial/cp210x.c b/drivers/usb/serial/cp210x.c
index e4bb62225cb9..eca1747ca8c7 100644
--- a/drivers/usb/serial/cp210x.c
+++ b/drivers/usb/serial/cp210x.c
@@ -122,6 +122,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x10C4, 0x8665) }, /* AC-Services OBD-IF */
 	{ USB_DEVICE(0x10C4, 0x88A4) }, /* MMB Networks ZigBee USB Device */
 	{ USB_DEVICE(0x10C4, 0x88A5) }, /* Planet Innovation Ingeni ZigBee USB Device */
+	{ USB_DEVICE(0x10C4, 0x8946) }, /* Ketra N1 Wireless Interface */
 	{ USB_DEVICE(0x10C4, 0xEA60) }, /* Silicon Labs factory default */
 	{ USB_DEVICE(0x10C4, 0xEA61) }, /* Silicon Labs factory default */
 	{ USB_DEVICE(0x10C4, 0xEA70) }, /* Silicon Labs factory default */
@@ -155,6 +156,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(0x1ADB, 0x0001) }, /* Schweitzer Engineering C662 Cable */
 	{ USB_DEVICE(0x1B1C, 0x1C00) }, /* Corsair USB Dongle */
 	{ USB_DEVICE(0x1BE3, 0x07A6) }, /* WAGO 750-923 USB Service Cable */
+	{ USB_DEVICE(0x1D6F, 0x0010) }, /* Seluxit ApS RF Dongle */
 	{ USB_DEVICE(0x1E29, 0x0102) }, /* Festo CPX-USB */
 	{ USB_DEVICE(0x1E29, 0x0501) }, /* Festo CMSP */
 	{ USB_DEVICE(0x1FB9, 0x0100) }, /* Lake Shore Model 121 Current Source */
diff --git a/drivers/usb/serial/digi_acceleport.c b/drivers/usb/serial/digi_acceleport.c
index 8a23c53b946e..12b0e67473ba 100644
--- a/drivers/usb/serial/digi_acceleport.c
+++ b/drivers/usb/serial/digi_acceleport.c
@@ -834,7 +834,6 @@ static void digi_set_termios(struct tty_struct *tty,
 			arg |= DIGI_OUTPUT_FLOW_CONTROL_CTS;
 		} else {
 			arg &= ~DIGI_OUTPUT_FLOW_CONTROL_CTS;
-			tty->hw_stopped = 0;
 		}
 
 		buf[i++] = DIGI_CMD_SET_OUTPUT_FLOW_CONTROL;
@@ -1500,15 +1499,11 @@ static int digi_read_oob_callback(struct urb *urb)
 			if (val & DIGI_READ_INPUT_SIGNALS_CTS) {
 				priv->dp_modem_signals |= TIOCM_CTS;
 				/* port must be open to use tty struct */
-				if (rts) {
-					tty->hw_stopped = 0;
+				if (rts)
 					tty_port_tty_wakeup(&port->port);
-				}
 			} else {
 				priv->dp_modem_signals &= ~TIOCM_CTS;
 				/* port must be open to use tty struct */
-				if (rts)
-					tty->hw_stopped = 1;
 			}
 			if (val & DIGI_READ_INPUT_SIGNALS_DSR)
 				priv->dp_modem_signals |= TIOCM_DSR;
diff --git a/drivers/usb/serial/ftdi_sio.c b/drivers/usb/serial/ftdi_sio.c
index 216ce3078270..dc72b924c399 100644
--- a/drivers/usb/serial/ftdi_sio.c
+++ b/drivers/usb/serial/ftdi_sio.c
@@ -146,6 +146,7 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_AMC232_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_CANUSB_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_CANDAPTER_PID) },
+	{ USB_DEVICE(FTDI_VID, FTDI_BM_ATOM_NANO_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_NXTCAM_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_EV3CON_PID) },
 	{ USB_DEVICE(FTDI_VID, FTDI_SCS_DEVICE_0_PID) },
@@ -727,6 +728,7 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(FTDI_VID, FTDI_NDI_AURORA_SCU_PID),
 		.driver_info = (kernel_ulong_t)&ftdi_NDI_device_quirk },
 	{ USB_DEVICE(TELLDUS_VID, TELLDUS_TELLSTICK_PID) },
+	{ USB_DEVICE(NOVITUS_VID, NOVITUS_BONO_E_PID) },
 	{ USB_DEVICE(RTSYSTEMS_VID, RTSYSTEMS_USB_S03_PID) },
 	{ USB_DEVICE(RTSYSTEMS_VID, RTSYSTEMS_USB_59_PID) },
 	{ USB_DEVICE(RTSYSTEMS_VID, RTSYSTEMS_USB_57A_PID) },
@@ -934,8 +936,12 @@ static const struct usb_device_id id_table_combined[] = {
 	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_842_2_PID) },
 	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_842_3_PID) },
 	{ USB_DEVICE(BRAINBOXES_VID, BRAINBOXES_US_842_4_PID) },
+	/* ekey Devices */
+	{ USB_DEVICE(FTDI_VID, FTDI_EKEY_CONV_USB_PID) },
 	/* Infineon Devices */
 	{ USB_DEVICE_INTERFACE_NUMBER(INFINEON_VID, INFINEON_TRIBOARD_PID, 1) },
+	/* GE Healthcare devices */
+	{ USB_DEVICE(GE_HEALTHCARE_VID, GE_HEALTHCARE_NEMO_TRACKER_PID) },
 	{ }					/* Terminating entry */
 };
 
diff --git a/drivers/usb/serial/ftdi_sio_ids.h b/drivers/usb/serial/ftdi_sio_ids.h
index 1e58d90a0b6c..5937b2d242f2 100644
--- a/drivers/usb/serial/ftdi_sio_ids.h
+++ b/drivers/usb/serial/ftdi_sio_ids.h
@@ -42,6 +42,8 @@
 /* www.candapter.com Ewert Energy Systems CANdapter device */
 #define FTDI_CANDAPTER_PID 0x9F80 /* Product Id */
 
+#define FTDI_BM_ATOM_NANO_PID	0xa559	/* Basic Micro ATOM Nano USB2Serial */
+
 /*
  * Texas Instruments XDS100v2 JTAG / BeagleBone A3
  * http://processors.wiki.ti.com/index.php/XDS100
@@ -835,6 +837,12 @@
 #define TELLDUS_TELLSTICK_PID		0x0C30	/* RF control dongle 433 MHz using FT232RL */
 
 /*
+ * NOVITUS printers
+ */
+#define NOVITUS_VID			0x1a28
+#define NOVITUS_BONO_E_PID		0x6010
+
+/*
  * RT Systems programming cables for various ham radios
  */
 #define RTSYSTEMS_VID		0x2100	/* Vendor ID */
@@ -1378,3 +1386,14 @@
 #define BRAINBOXES_US_160_6_PID		0x9006 /* US-160 16xRS232 1Mbaud Port 11 and 12 */
 #define BRAINBOXES_US_160_7_PID		0x9007 /* US-160 16xRS232 1Mbaud Port 13 and 14 */
 #define BRAINBOXES_US_160_8_PID		0x9008 /* US-160 16xRS232 1Mbaud Port 15 and 16 */
+
+/*
+ * ekey biometric systems GmbH (http://ekey.net/)
+ */
+#define FTDI_EKEY_CONV_USB_PID		0xCB08	/* Converter USB */
+
+/*
+ * GE Healthcare devices
+ */
+#define GE_HEALTHCARE_VID		0x1901
+#define GE_HEALTHCARE_NEMO_TRACKER_PID	0x0015
diff --git a/drivers/usb/serial/io_ti.c b/drivers/usb/serial/io_ti.c
index c0a42e9e6777..ddbb8fe1046d 100644
--- a/drivers/usb/serial/io_ti.c
+++ b/drivers/usb/serial/io_ti.c
@@ -1456,12 +1456,8 @@ static void handle_new_msr(struct edgeport_port *edge_port, __u8 msr)
 	tty = tty_port_tty_get(&edge_port->port->port);
 	/* handle CTS flow control */
 	if (tty && C_CRTSCTS(tty)) {
-		if (msr & EDGEPORT_MSR_CTS) {
-			tty->hw_stopped = 0;
+		if (msr & EDGEPORT_MSR_CTS)
 			tty_wakeup(tty);
-		} else {
-			tty->hw_stopped = 1;
-		}
 	}
 	tty_kref_put(tty);
 }
@@ -2177,7 +2173,6 @@ static void change_port_settings(struct tty_struct *tty,
 		dev_dbg(dev, "%s - RTS/CTS is enabled\n", __func__);
 	} else {
 		dev_dbg(dev, "%s - RTS/CTS is disabled\n", __func__);
-		tty->hw_stopped = 0;
 		restart_read(edge_port);
 	}
 
diff --git a/drivers/usb/serial/option.c b/drivers/usb/serial/option.c
index a9688940543d..d1a3f6044c8a 100644
--- a/drivers/usb/serial/option.c
+++ b/drivers/usb/serial/option.c
@@ -275,8 +275,13 @@ static void option_instat_callback(struct urb *urb);
 #define ZTE_PRODUCT_MF622			0x0001
 #define ZTE_PRODUCT_MF628			0x0015
 #define ZTE_PRODUCT_MF626			0x0031
-#define ZTE_PRODUCT_MC2718			0xffe8
 #define ZTE_PRODUCT_AC2726			0xfff1
+#define ZTE_PRODUCT_MG880			0xfffd
+#define ZTE_PRODUCT_CDMA_TECH			0xfffe
+#define ZTE_PRODUCT_AC8710T			0xffff
+#define ZTE_PRODUCT_MC2718			0xffe8
+#define ZTE_PRODUCT_AD3812			0xffeb
+#define ZTE_PRODUCT_MC2716			0xffed
 
 #define BENQ_VENDOR_ID				0x04a5
 #define BENQ_PRODUCT_H10			0x4068
@@ -494,6 +499,10 @@ static void option_instat_callback(struct urb *urb);
 #define INOVIA_VENDOR_ID			0x20a6
 #define INOVIA_SEW858				0x1105
 
+/* VIA Telecom */
+#define VIATELECOM_VENDOR_ID			0x15eb
+#define VIATELECOM_PRODUCT_CDS7			0x0001
+
 /* some devices interfaces need special handling due to a number of reasons */
 enum option_blacklist_reason {
 		OPTION_BLACKLIST_NONE = 0,
@@ -527,10 +536,18 @@ static const struct option_blacklist_info zte_k3765_z_blacklist = {
 	.reserved = BIT(4),
 };
 
+static const struct option_blacklist_info zte_ad3812_z_blacklist = {
+	.sendsetup = BIT(0) | BIT(1) | BIT(2),
+};
+
 static const struct option_blacklist_info zte_mc2718_z_blacklist = {
 	.sendsetup = BIT(1) | BIT(2) | BIT(3) | BIT(4),
 };
 
+static const struct option_blacklist_info zte_mc2716_z_blacklist = {
+	.sendsetup = BIT(1) | BIT(2) | BIT(3),
+};
+
 static const struct option_blacklist_info huawei_cdc12_blacklist = {
 	.reserved = BIT(1) | BIT(2),
 };
@@ -1070,6 +1087,7 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_INTERFACE_CLASS(BANDRICH_VENDOR_ID, BANDRICH_PRODUCT_1012, 0xff) },
 	{ USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_PRODUCT_KPC650) },
 	{ USB_DEVICE(KYOCERA_VENDOR_ID, KYOCERA_PRODUCT_KPC680) },
+	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6000)}, /* ZTE AC8700 */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x6613)}, /* Onda H600/ZTE MF330 */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x0023)}, /* ONYX 3G device */
 	{ USB_DEVICE(QUALCOMM_VENDOR_ID, 0x9000)}, /* SIMCom SIM5218 */
@@ -1543,14 +1561,27 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff92, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff93, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff94, 0xff, 0xff, 0xff) },
-
-	/* NOTE: most ZTE CDMA devices should be driven by zte_ev, not option */
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffec, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xffee, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff6, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff7, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff8, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfff9, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfffb, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xfffc, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MG880, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_CDMA_TECH, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC2726, 0xff, 0xff, 0xff) },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC8710T, 0xff, 0xff, 0xff) },
 	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MC2718, 0xff, 0xff, 0xff),
 	 .driver_info = (kernel_ulong_t)&zte_mc2718_z_blacklist },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AD3812, 0xff, 0xff, 0xff),
+	 .driver_info = (kernel_ulong_t)&zte_ad3812_z_blacklist },
+	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_MC2716, 0xff, 0xff, 0xff),
+	 .driver_info = (kernel_ulong_t)&zte_mc2716_z_blacklist },
 	{ USB_VENDOR_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff, 0x02, 0x01) },
 	{ USB_VENDOR_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff, 0x02, 0x05) },
 	{ USB_VENDOR_AND_INTERFACE_INFO(ZTE_VENDOR_ID, 0xff, 0x86, 0x10) },
-	{ USB_DEVICE_AND_INTERFACE_INFO(ZTE_VENDOR_ID, ZTE_PRODUCT_AC2726, 0xff, 0xff, 0xff) },
 
 	{ USB_DEVICE(BENQ_VENDOR_ID, BENQ_PRODUCT_H10) },
 	{ USB_DEVICE(DLINK_VENDOR_ID, DLINK_PRODUCT_DWM_652) },
@@ -1724,6 +1755,7 @@ static const struct usb_device_id option_ids[] = {
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x07d1, 0x3e01, 0xff, 0xff, 0xff) }, /* D-Link DWM-152/C1 */
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x07d1, 0x3e02, 0xff, 0xff, 0xff) }, /* D-Link DWM-156/C1 */
 	{ USB_DEVICE(INOVIA_VENDOR_ID, INOVIA_SEW858) },
+	{ USB_DEVICE(VIATELECOM_VENDOR_ID, VIATELECOM_PRODUCT_CDS7) },
 	{ } /* Terminating entry */
 };
 MODULE_DEVICE_TABLE(usb, option_ids);
@@ -1916,6 +1948,8 @@ static void option_instat_callback(struct urb *urb)
 			dev_dbg(dev, "%s: type %x req %x\n", __func__,
 				req_pkt->bRequestType, req_pkt->bRequest);
 		}
+	} else if (status == -ENOENT || status == -ESHUTDOWN) {
+		dev_dbg(dev, "%s: urb stopped: %d\n", __func__, status);
 	} else
 		dev_err(dev, "%s: error %d\n", __func__, status);
 
diff --git a/drivers/usb/serial/pl2303.c b/drivers/usb/serial/pl2303.c
index b3d5a35c0d4b..0f872e6b2c87 100644
--- a/drivers/usb/serial/pl2303.c
+++ b/drivers/usb/serial/pl2303.c
@@ -45,6 +45,7 @@ static const struct usb_device_id id_table[] = {
 	{ USB_DEVICE(PL2303_VENDOR_ID, PL2303_PRODUCT_ID_GPRS) },
 	{ USB_DEVICE(PL2303_VENDOR_ID, PL2303_PRODUCT_ID_HCR331) },
 	{ USB_DEVICE(PL2303_VENDOR_ID, PL2303_PRODUCT_ID_MOTOROLA) },
+	{ USB_DEVICE(PL2303_VENDOR_ID, PL2303_PRODUCT_ID_ZTEK) },
 	{ USB_DEVICE(IODATA_VENDOR_ID, IODATA_PRODUCT_ID) },
 	{ USB_DEVICE(IODATA_VENDOR_ID, IODATA_PRODUCT_ID_RSAQ5) },
 	{ USB_DEVICE(ATEN_VENDOR_ID, ATEN_PRODUCT_ID) },
@@ -161,6 +162,9 @@ static const struct pl2303_type_data pl2303_type_data[TYPE_COUNT] = {
 		.max_baud_rate =	1228800,
 		.quirks =		PL2303_QUIRK_LEGACY,
 	},
+	[TYPE_HX] = {
+		.max_baud_rate =	12000000,
+	},
 };
 
 static int pl2303_vendor_read(struct usb_serial *serial, u16 value,
@@ -394,16 +398,14 @@ static void pl2303_encode_baud_rate(struct tty_struct *tty,
 	if (spriv->type->max_baud_rate)
 		baud = min_t(speed_t, baud, spriv->type->max_baud_rate);
 	/*
-	 * Set baud rate to nearest supported value.
-	 *
-	 * NOTE: Baud rate 500k can only be set using divisors.
+	 * Use direct method for supported baud rates, otherwise use divisors.
 	 */
 	baud_sup = pl2303_get_supported_baud_rate(baud);
 
-	if (baud == 500000)
-		baud = pl2303_encode_baud_rate_divisor(buf, baud);
+	if (baud == baud_sup)
+		baud = pl2303_encode_baud_rate_direct(buf, baud);
 	else
-		baud = pl2303_encode_baud_rate_direct(buf, baud_sup);
+		baud = pl2303_encode_baud_rate_divisor(buf, baud);
 
 	/* Save resulting baud rate */
 	tty_encode_baud_rate(tty, baud, baud);
diff --git a/drivers/usb/serial/pl2303.h b/drivers/usb/serial/pl2303.h
index 42bc082896ac..71fd9da1d6e7 100644
--- a/drivers/usb/serial/pl2303.h
+++ b/drivers/usb/serial/pl2303.h
@@ -22,6 +22,7 @@
 #define PL2303_PRODUCT_ID_GPRS		0x0609
 #define PL2303_PRODUCT_ID_HCR331	0x331a
 #define PL2303_PRODUCT_ID_MOTOROLA	0x0307
+#define PL2303_PRODUCT_ID_ZTEK		0xe1f1
 
 #define ATEN_VENDOR_ID		0x0557
 #define ATEN_VENDOR_ID2		0x0547
diff --git a/drivers/usb/serial/sierra.c b/drivers/usb/serial/sierra.c
index 6f7f01eb556a..46179a0828eb 100644
--- a/drivers/usb/serial/sierra.c
+++ b/drivers/usb/serial/sierra.c
@@ -282,14 +282,19 @@ static const struct usb_device_id id_table[] = {
 	/* Sierra Wireless HSPA Non-Composite Device */
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x1199, 0x6892, 0xFF, 0xFF, 0xFF)},
 	{ USB_DEVICE(0x1199, 0x6893) },	/* Sierra Wireless Device */
-	{ USB_DEVICE(0x1199, 0x68A3), 	/* Sierra Wireless Direct IP modems */
+	/* Sierra Wireless Direct IP modems */
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x1199, 0x68A3, 0xFF, 0xFF, 0xFF),
+	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
+	},
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x1199, 0x68AA, 0xFF, 0xFF, 0xFF),
 	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
 	},
 	/* AT&T Direct IP LTE modems */
 	{ USB_DEVICE_AND_INTERFACE_INFO(0x0F3D, 0x68AA, 0xFF, 0xFF, 0xFF),
 	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
 	},
-	{ USB_DEVICE(0x0f3d, 0x68A3), 	/* Airprime/Sierra Wireless Direct IP modems */
+	/* Airprime/Sierra Wireless Direct IP modems */
+	{ USB_DEVICE_AND_INTERFACE_INFO(0x0F3D, 0x68A3, 0xFF, 0xFF, 0xFF),
 	  .driver_info = (kernel_ulong_t)&direct_ip_interface_blacklist
 	},
 
diff --git a/drivers/usb/serial/ti_usb_3410_5052.c b/drivers/usb/serial/ti_usb_3410_5052.c
index 3dd3ff8c50d3..e9da41d9fe7f 100644
--- a/drivers/usb/serial/ti_usb_3410_5052.c
+++ b/drivers/usb/serial/ti_usb_3410_5052.c
@@ -773,7 +773,6 @@ static void ti_set_termios(struct tty_struct *tty,
 			config->wFlags |= TI_UART_ENABLE_RTS_IN;
 		config->wFlags |= TI_UART_ENABLE_CTS_OUT;
 	} else {
-		tty->hw_stopped = 0;
 		ti_restart_read(tport, tty);
 	}
 
@@ -1291,12 +1290,8 @@ static void ti_handle_new_msr(struct ti_port *tport, __u8 msr)
 	/* handle CTS flow control */
 	tty = tty_port_tty_get(&tport->tp_port->port);
 	if (tty && C_CRTSCTS(tty)) {
-		if (msr & TI_MSR_CTS) {
-			tty->hw_stopped = 0;
+		if (msr & TI_MSR_CTS)
 			tty_wakeup(tty);
-		} else {
-			tty->hw_stopped = 1;
-		}
 	}
 	tty_kref_put(tty);
 }
diff --git a/drivers/usb/serial/usb-serial-simple.c b/drivers/usb/serial/usb-serial-simple.c
index fb79775447b0..7064eb8d6142 100644
--- a/drivers/usb/serial/usb-serial-simple.c
+++ b/drivers/usb/serial/usb-serial-simple.c
@@ -20,7 +20,7 @@
 #include <linux/usb.h>
 #include <linux/usb/serial.h>
 
-#define DEVICE(vendor, IDS)					\
+#define DEVICE_N(vendor, IDS, nport)				\
 static const struct usb_device_id vendor##_id_table[] = {	\
 	IDS(),							\
 	{ },							\
@@ -31,9 +31,15 @@ static struct usb_serial_driver vendor##_device = {		\
 		.name =		#vendor,			\
 	},							\
 	.id_table =		vendor##_id_table,		\
-	.num_ports =		1,				\
+	.num_ports =		nport,				\
 };
 
+#define DEVICE(vendor, IDS)	DEVICE_N(vendor, IDS, 1)
+
+/* Medtronic CareLink USB driver */
+#define CARELINK_IDS()			\
+	{ USB_DEVICE(0x0a21, 0x8001) }	/* MMT-7305WW */
+DEVICE(carelink, CARELINK_IDS);
 
 /* ZIO Motherboard USB driver */
 #define ZIO_IDS()			\
@@ -64,6 +70,11 @@ DEVICE(vivopay, VIVOPAY_IDS);
 	{ USB_DEVICE(0x22b8, 0x2c64) }	/* Motorola V950 phone */
 DEVICE(moto_modem, MOTO_IDS);
 
+/* Novatel Wireless GPS driver */
+#define NOVATEL_IDS()			\
+	{ USB_DEVICE(0x09d7, 0x0100) }	/* NovAtel FlexPack GPS */
+DEVICE_N(novatel_gps, NOVATEL_IDS, 3);
+
 /* HP4x (48/49) Generic Serial driver */
 #define HP4X_IDS()			\
 	{ USB_DEVICE(0x03f0, 0x0121) }
@@ -82,11 +93,13 @@ DEVICE(siemens_mpi, SIEMENS_IDS);
 
 /* All of the above structures mushed into two lists */
 static struct usb_serial_driver * const serial_drivers[] = {
+	&carelink_device,
 	&zio_device,
 	&funsoft_device,
 	&flashloader_device,
 	&vivopay_device,
 	&moto_modem_device,
+	&novatel_gps_device,
 	&hp4x_device,
 	&suunto_device,
 	&siemens_mpi_device,
@@ -94,11 +107,13 @@ static struct usb_serial_driver * const serial_drivers[] = {
 };
 
 static const struct usb_device_id id_table[] = {
+	CARELINK_IDS(),
 	ZIO_IDS(),
 	FUNSOFT_IDS(),
 	FLASHLOADER_IDS(),
 	VIVOPAY_IDS(),
 	MOTO_IDS(),
+	NOVATEL_IDS(),
 	HP4X_IDS(),
 	SUUNTO_IDS(),
 	SIEMENS_IDS(),
diff --git a/drivers/usb/serial/usb-serial.c b/drivers/usb/serial/usb-serial.c
index 02de3110fe94..475723c006f9 100644
--- a/drivers/usb/serial/usb-serial.c
+++ b/drivers/usb/serial/usb-serial.c
@@ -764,29 +764,39 @@ static int usb_serial_probe(struct usb_interface *interface,
 		if (usb_endpoint_is_bulk_in(endpoint)) {
 			/* we found a bulk in endpoint */
 			dev_dbg(ddev, "found bulk in on endpoint %d\n", i);
-			bulk_in_endpoint[num_bulk_in] = endpoint;
-			++num_bulk_in;
+			if (num_bulk_in < MAX_NUM_PORTS) {
+				bulk_in_endpoint[num_bulk_in] = endpoint;
+				++num_bulk_in;
+			}
 		}
 
 		if (usb_endpoint_is_bulk_out(endpoint)) {
 			/* we found a bulk out endpoint */
 			dev_dbg(ddev, "found bulk out on endpoint %d\n", i);
-			bulk_out_endpoint[num_bulk_out] = endpoint;
-			++num_bulk_out;
+			if (num_bulk_out < MAX_NUM_PORTS) {
+				bulk_out_endpoint[num_bulk_out] = endpoint;
+				++num_bulk_out;
+			}
 		}
 
 		if (usb_endpoint_is_int_in(endpoint)) {
 			/* we found a interrupt in endpoint */
 			dev_dbg(ddev, "found interrupt in on endpoint %d\n", i);
-			interrupt_in_endpoint[num_interrupt_in] = endpoint;
-			++num_interrupt_in;
+			if (num_interrupt_in < MAX_NUM_PORTS) {
+				interrupt_in_endpoint[num_interrupt_in] =
+						endpoint;
+				++num_interrupt_in;
+			}
 		}
 
 		if (usb_endpoint_is_int_out(endpoint)) {
 			/* we found an interrupt out endpoint */
 			dev_dbg(ddev, "found interrupt out on endpoint %d\n", i);
-			interrupt_out_endpoint[num_interrupt_out] = endpoint;
-			++num_interrupt_out;
+			if (num_interrupt_out < MAX_NUM_PORTS) {
+				interrupt_out_endpoint[num_interrupt_out] =
+						endpoint;
+				++num_interrupt_out;
+			}
 		}
 	}
 
@@ -809,8 +819,10 @@ static int usb_serial_probe(struct usb_interface *interface,
 				if (usb_endpoint_is_int_in(endpoint)) {
 					/* we found a interrupt in endpoint */
 					dev_dbg(ddev, "found interrupt in for Prolific device on separate interface\n");
-					interrupt_in_endpoint[num_interrupt_in] = endpoint;
-					++num_interrupt_in;
+					if (num_interrupt_in < MAX_NUM_PORTS) {
+						interrupt_in_endpoint[num_interrupt_in] = endpoint;
+						++num_interrupt_in;
+					}
 				}
 			}
 		}
@@ -850,6 +862,11 @@ static int usb_serial_probe(struct usb_interface *interface,
 			num_ports = type->num_ports;
 	}
 
+	if (num_ports > MAX_NUM_PORTS) {
+		dev_warn(ddev, "too many ports requested: %d\n", num_ports);
+		num_ports = MAX_NUM_PORTS;
+	}
+
 	serial->num_ports = num_ports;
 	serial->num_bulk_in = num_bulk_in;
 	serial->num_bulk_out = num_bulk_out;
diff --git a/drivers/usb/serial/whiteheat.c b/drivers/usb/serial/whiteheat.c
index e62f2dff8b7d..6c3734d2b45a 100644
--- a/drivers/usb/serial/whiteheat.c
+++ b/drivers/usb/serial/whiteheat.c
@@ -514,6 +514,10 @@ static void command_port_read_callback(struct urb *urb)
 		dev_dbg(&urb->dev->dev, "%s - command_info is NULL, exiting.\n", __func__);
 		return;
 	}
+	if (!urb->actual_length) {
+		dev_dbg(&urb->dev->dev, "%s - empty response, exiting.\n", __func__);
+		return;
+	}
 	if (status) {
 		dev_dbg(&urb->dev->dev, "%s - nonzero urb status: %d\n", __func__, status);
 		if (status != -ENOENT)
@@ -534,7 +538,8 @@ static void command_port_read_callback(struct urb *urb)
 		/* These are unsolicited reports from the firmware, hence no
 		   waiting command to wakeup */
 		dev_dbg(&urb->dev->dev, "%s - event received\n", __func__);
-	} else if (data[0] == WHITEHEAT_GET_DTR_RTS) {
+	} else if ((data[0] == WHITEHEAT_GET_DTR_RTS) &&
+		(urb->actual_length - 1 <= sizeof(command_info->result_buffer))) {
 		memcpy(command_info->result_buffer, &data[1],
 						urb->actual_length - 1);
 		command_info->command_finished = WHITEHEAT_CMD_COMPLETE;
diff --git a/drivers/usb/serial/xsens_mt.c b/drivers/usb/serial/xsens_mt.c
index 4841fb57400c..3837d5113bb2 100644
--- a/drivers/usb/serial/xsens_mt.c
+++ b/drivers/usb/serial/xsens_mt.c
@@ -41,28 +41,13 @@ static const struct usb_device_id id_table[] = {
 };
 MODULE_DEVICE_TABLE(usb, id_table);
 
-static int has_required_endpoints(const struct usb_host_interface *interface)
-{
-	__u8 i;
-	int has_bulk_in = 0;
-	int has_bulk_out = 0;
-
-	for (i = 0; i < interface->desc.bNumEndpoints; ++i) {
-		if (usb_endpoint_is_bulk_in(&interface->endpoint[i].desc))
-			has_bulk_in = 1;
-		else if (usb_endpoint_is_bulk_out(&interface->endpoint[i].desc))
-			has_bulk_out = 1;
-	}
-
-	return has_bulk_in && has_bulk_out;
-}
-
 static int xsens_mt_probe(struct usb_serial *serial,
 					const struct usb_device_id *id)
 {
-	if (!has_required_endpoints(serial->interface->cur_altsetting))
-		return -ENODEV;
-	return 0;
+	if (serial->interface->cur_altsetting->desc.bInterfaceNumber == 1)
+		return 0;
+
+	return -ENODEV;
 }
 
 static struct usb_serial_driver xsens_mt_device = {
@@ -82,4 +67,6 @@ static struct usb_serial_driver * const serial_drivers[] = {
 
 module_usb_serial_driver(serial_drivers, id_table);
 
+MODULE_AUTHOR("Frans Klaver <frans.klaver@xsens.com>");
+MODULE_DESCRIPTION("USB-serial driver for Xsens motion trackers");
 MODULE_LICENSE("GPL");
diff --git a/drivers/usb/serial/zte_ev.c b/drivers/usb/serial/zte_ev.c
deleted file mode 100644
index e40ab739c4a6..000000000000
--- a/drivers/usb/serial/zte_ev.c
+++ /dev/null
@@ -1,317 +0,0 @@
-/*
- * ZTE_EV USB serial driver
- *
- * Copyright (C) 2012 Greg Kroah-Hartman <gregkh@linuxfoundation.org>
- * Copyright (C) 2012 Linux Foundation
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
- *
- * This driver is based on code found in a ZTE_ENV patch that modified
- * the usb-serial generic driver.  Comments were left in that I think
- * show the commands used to talk to the device, but I am not sure.
- */
-#include <linux/kernel.h>
-#include <linux/tty.h>
-#include <linux/slab.h>
-#include <linux/module.h>
-#include <linux/usb.h>
-#include <linux/usb/serial.h>
-#include <linux/uaccess.h>
-
-#define  MAX_SETUP_DATA_SIZE	32
-
-static void debug_data(struct device *dev, const char *function, int len,
-		       const unsigned char *data, int result)
-{
-	dev_dbg(dev, "result = %d\n", result);
-	if (result == len)
-		dev_dbg(dev, "%s - length = %d, data = %*ph\n", function,
-			len, len, data);
-}
-
-static int zte_ev_usb_serial_open(struct tty_struct *tty,
-				  struct usb_serial_port *port)
-{
-	struct usb_device *udev = port->serial->dev;
-	struct device *dev = &port->dev;
-	int result = 0;
-	int len;
-	unsigned char *buf;
-
-	buf = kmalloc(MAX_SETUP_DATA_SIZE, GFP_KERNEL);
-	if (!buf)
-		return -ENOMEM;
-
-	/* send 1st ctl cmd(CTL    21 22 01 00  00 00 00 00) */
-	len = 0;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x22, 0x21,
-				 0x0001, 0x0000, NULL, len,
-				 USB_CTRL_GET_TIMEOUT);
-	dev_dbg(dev, "result = %d\n", result);
-
-	/* send 2st cmd and receive data */
-	/*
-	 * 16.0  CTL    a1 21 00 00  00 00 07 00   CLASS              25.1.0(5)
-	 * 16.0  DI     00 96 00 00  00 00 08
-	 */
-	len = 0x0007;
-	result = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
-				 0x21, 0xa1,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-
-	/* send 3rd cmd */
-	/*
-	 * 16.0 CTL    21 20 00 00  00 00 07 00    CLASS                30.1.0
-	 * 16.0 DO     80 25 00 00  00 00 08       .%.....              30.2.0
-	 */
-	len = 0x0007;
-	buf[0] = 0x80;
-	buf[1] = 0x25;
-	buf[2] = 0x00;
-	buf[3] = 0x00;
-	buf[4] = 0x00;
-	buf[5] = 0x00;
-	buf[6] = 0x08;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x20, 0x21,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-
-	/* send 4th cmd */
-	/*
-	 * 16.0 CTL    21 22 03 00  00 00 00 00
-	 */
-	len = 0;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x22, 0x21,
-				 0x0003, 0x0000, NULL, len,
-				 USB_CTRL_GET_TIMEOUT);
-	dev_dbg(dev, "result = %d\n", result);
-
-	/* send 5th cmd */
-	/*
-	 * 16.0  CTL    a1 21 00 00  00 00 07 00   CLASS               33.1.0
-	 * 16.0  DI     80 25 00 00  00 00 08
-	 */
-	len = 0x0007;
-	result = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
-				 0x21, 0xa1,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-
-	/* send 6th cmd */
-	/*
-	 * 16.0  CTL    21 20 00 00  00 00 07 00    CLASS               34.1.0
-	 * 16.0  DO     80 25 00 00  00 00 08
-	 */
-	len = 0x0007;
-	buf[0] = 0x80;
-	buf[1] = 0x25;
-	buf[2] = 0x00;
-	buf[3] = 0x00;
-	buf[4] = 0x00;
-	buf[5] = 0x00;
-	buf[6] = 0x08;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x20, 0x21,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-	kfree(buf);
-
-	return usb_serial_generic_open(tty, port);
-}
-
-/*
- *       CTL    21 22 02 00  00 00 00 00         CLASS               338.1.0
- *
- * 16.1  DI     a1 20 00 00  00 00 02 00  02 00  . ........          340.1.0
- * 16.0  CTL    21 22 03 00  00 00 00 00         CLASS               341.1.0
- *
- * 16.0  CTL    a1 21 00 00  00 00 07 00         CLASS               346.1.0(3)
- * 16.0  DI     00 08 07 00  00 00 08            .......             346.2.0
- *
- * 16.0  CTL    21 20 00 00  00 00 07 00         CLASS               349.1.0
- * 16.0  DO     00 c2 01 00  00 00 08            .......             349.2.0
- *
- * 16.0  CTL    21 22 03 00  00 00 00 00         CLASS               350.1.0(2)
- *
- * 16.0  CTL    a1 21 00 00  00 00 07 00         CLASS               352.1.0
- * 16.0  DI     00 c2 01 00  00 00 08            .......             352.2.0
- *
- * 16.1  DI     a1 20 00 00  00 00 02 00  02 00  . ........          353.1.0
- *
- * 16.0  CTL    21 20 00 00  00 00 07 00         CLASS               354.1.0
- * 16.0  DO     00 c2 01 00  00 00 08            .......             354.2.0
- *
- * 16.0  CTL    21 22 03 00  00 00 00 00
-*/
-
-static void zte_ev_usb_serial_close(struct usb_serial_port *port)
-{
-	struct usb_device *udev = port->serial->dev;
-	struct device *dev = &port->dev;
-	int result = 0;
-	int len;
-	unsigned char *buf;
-
-	buf = kmalloc(MAX_SETUP_DATA_SIZE, GFP_KERNEL);
-	if (!buf)
-		return;
-
-	/* send 1st ctl cmd(CTL    21 22 02 00  00 00 00 00) */
-	len = 0;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x22, 0x21,
-				 0x0002, 0x0000, NULL, len,
-				 USB_CTRL_GET_TIMEOUT);
-	dev_dbg(dev, "result = %d\n", result);
-
-	/* send 2st ctl cmd(CTL    21 22 03 00  00 00 00 00 ) */
-	len = 0;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x22, 0x21,
-				 0x0003, 0x0000, NULL, len,
-				 USB_CTRL_GET_TIMEOUT);
-	dev_dbg(dev, "result = %d\n", result);
-
-	/* send  3st cmd and recieve data */
-	/*
-	 * 16.0  CTL    a1 21 00 00  00 00 07 00      CLASS         25.1.0(5)
-	 * 16.0  DI     00 08 07 00  00 00 08
-	 */
-	len = 0x0007;
-	result = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
-				 0x21, 0xa1,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-
-	/* send 4th cmd */
-	/*
-	 * 16.0 CTL    21 20 00 00  00 00 07 00      CLASS            30.1.0
-	 * 16.0  DO    00 c2 01 00  00 00 08         .%.....          30.2.0
-	 */
-	len = 0x0007;
-	buf[0] = 0x00;
-	buf[1] = 0xc2;
-	buf[2] = 0x01;
-	buf[3] = 0x00;
-	buf[4] = 0x00;
-	buf[5] = 0x00;
-	buf[6] = 0x08;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x20, 0x21,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-
-	/* send 5th cmd */
-	/*
-	 * 16.0 CTL    21 22 03 00  00 00 00 00
-	 */
-	len = 0;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x22, 0x21,
-				 0x0003, 0x0000, NULL, len,
-				 USB_CTRL_GET_TIMEOUT);
-	dev_dbg(dev, "result = %d\n", result);
-
-	/* send 6th cmd */
-	/*
-	 * 16.0  CTL    a1 21 00 00  00 00 07 00        CLASS          33.1.0
-	 * 16.0  DI     00 c2 01 00  00 00 08
-	 */
-	len = 0x0007;
-	result = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
-				 0x21, 0xa1,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-
-	/* send 7th cmd */
-	/*
-	 * 16.0  CTL    21 20 00 00  00 00 07 00  CLASS               354.1.0
-	 * 16.0  DO     00 c2 01 00  00 00 08     .......             354.2.0
-	 */
-	len = 0x0007;
-	buf[0] = 0x00;
-	buf[1] = 0xc2;
-	buf[2] = 0x01;
-	buf[3] = 0x00;
-	buf[4] = 0x00;
-	buf[5] = 0x00;
-	buf[6] = 0x08;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x20, 0x21,
-				 0x0000, 0x0000, buf, len,
-				 USB_CTRL_GET_TIMEOUT);
-	debug_data(dev, __func__, len, buf, result);
-
-	/* send 8th cmd */
-	/*
-	 * 16.0 CTL    21 22 03 00  00 00 00 00
-	 */
-	len = 0;
-	result = usb_control_msg(udev, usb_sndctrlpipe(udev, 0),
-				 0x22, 0x21,
-				 0x0003, 0x0000, NULL, len,
-				 USB_CTRL_GET_TIMEOUT);
-	dev_dbg(dev, "result = %d\n", result);
-
-	kfree(buf);
-
-	usb_serial_generic_close(port);
-}
-
-static const struct usb_device_id id_table[] = {
-	/* AC8710, AC8710T */
-	{ USB_DEVICE_AND_INTERFACE_INFO(0x19d2, 0xffff, 0xff, 0xff, 0xff) },
-	 /* AC8700 */
-	{ USB_DEVICE_AND_INTERFACE_INFO(0x19d2, 0xfffe, 0xff, 0xff, 0xff) },
-	/* MG880 */
-	{ USB_DEVICE(0x19d2, 0xfffd) },
-	{ USB_DEVICE(0x19d2, 0xfffc) },
-	{ USB_DEVICE(0x19d2, 0xfffb) },
-	/* AC8710_V3 */
-	{ USB_DEVICE(0x19d2, 0xfff6) },
-	{ USB_DEVICE(0x19d2, 0xfff7) },
-	{ USB_DEVICE(0x19d2, 0xfff8) },
-	{ USB_DEVICE(0x19d2, 0xfff9) },
-	{ USB_DEVICE(0x19d2, 0xffee) },
-	/* AC2716, MC2716 */
-	{ USB_DEVICE_AND_INTERFACE_INFO(0x19d2, 0xffed, 0xff, 0xff, 0xff) },
-	/* AD3812 */
-	{ USB_DEVICE_AND_INTERFACE_INFO(0x19d2, 0xffeb, 0xff, 0xff, 0xff) },
-	{ USB_DEVICE(0x19d2, 0xffec) },
-	{ USB_DEVICE(0x05C6, 0x3197) },
-	{ USB_DEVICE(0x05C6, 0x6000) },
-	{ USB_DEVICE(0x05C6, 0x9008) },
-	{ },
-};
-MODULE_DEVICE_TABLE(usb, id_table);
-
-static struct usb_serial_driver zio_device = {
-	.driver = {
-		.owner =	THIS_MODULE,
-		.name =		"zte_ev",
-	},
-	.id_table =		id_table,
-	.num_ports =		1,
-	.open =			zte_ev_usb_serial_open,
-	.close =		zte_ev_usb_serial_close,
-};
-
-static struct usb_serial_driver * const serial_drivers[] = {
-	&zio_device, NULL
-};
-
-module_usb_serial_driver(serial_drivers, id_table);
-MODULE_LICENSE("GPL v2");
diff --git a/drivers/usb/storage/alauda.c b/drivers/usb/storage/alauda.c
index 6636a583da12..62c2d9daa7d6 100644
--- a/drivers/usb/storage/alauda.c
+++ b/drivers/usb/storage/alauda.c
@@ -415,14 +415,11 @@ static int alauda_init_media(struct us_data *us)
 	if (alauda_get_media_signature(us, data) != USB_STOR_XFER_GOOD)
 		return USB_STOR_TRANSPORT_ERROR;
 
-	usb_stor_dbg(us, "Media signature: %02X %02X %02X %02X\n",
-		     data[0], data[1], data[2], data[3]);
+	usb_stor_dbg(us, "Media signature: %4ph\n", data);
 	media_info = alauda_card_find_id(data[1]);
 	if (media_info == NULL) {
-		printk(KERN_WARNING
-			"alauda_init_media: Unrecognised media signature: "
-			"%02X %02X %02X %02X\n",
-			data[0], data[1], data[2], data[3]);
+		pr_warn("alauda_init_media: Unrecognised media signature: %4ph\n",
+			data);
 		return USB_STOR_TRANSPORT_ERROR;
 	}
 
@@ -513,7 +510,7 @@ static int alauda_check_status2(struct us_data *us)
 	if (rc != USB_STOR_XFER_GOOD)
 		return rc;
 
-	usb_stor_dbg(us, "%02X %02X %02X\n", data[0], data[1], data[2]);
+	usb_stor_dbg(us, "%3ph\n", data);
 	if (data[0] & ALAUDA_STATUS_ERROR)
 		return USB_STOR_XFER_ERROR;
 
diff --git a/drivers/usb/storage/initializers.c b/drivers/usb/storage/initializers.c
index 5a8b5ff1e45b..4bc2fc98636e 100644
--- a/drivers/usb/storage/initializers.c
+++ b/drivers/usb/storage/initializers.c
@@ -52,7 +52,7 @@ int usb_stor_euscsi_init(struct us_data *us)
 	us->iobuf[0] = 0x1;
 	result = usb_stor_control_msg(us, us->send_ctrl_pipe,
 			0x0C, USB_RECIP_INTERFACE | USB_TYPE_VENDOR,
-			0x01, 0x0, us->iobuf, 0x1, 5000);
+			0x01, 0x0, us->iobuf, 0x1, USB_CTRL_SET_TIMEOUT);
 	usb_stor_dbg(us, "-- result is %d\n", result);
 
 	return 0;
diff --git a/drivers/usb/storage/realtek_cr.c b/drivers/usb/storage/realtek_cr.c
index 281be56d5648..8591d89a38e6 100644
--- a/drivers/usb/storage/realtek_cr.c
+++ b/drivers/usb/storage/realtek_cr.c
@@ -115,7 +115,7 @@ struct rts51x_chip {
 	enum RTS51X_STAT state;
 	int support_auto_delink;
 #endif
-	/* used to back up the protocal choosen in probe1 phase */
+	/* used to back up the protocol chosen in probe1 phase */
 	proto_cmnd proto_handler_backup;
 };
 
@@ -925,7 +925,7 @@ static int realtek_cr_autosuspend_setup(struct us_data *us)
 			(unsigned long)chip);
 	fw5895_init(us);
 
-	/* enable autosuspend funciton of the usb device */
+	/* enable autosuspend function of the usb device */
 	usb_enable_autosuspend(us->pusb_dev);
 
 	return 0;
diff --git a/drivers/usb/storage/scsiglue.c b/drivers/usb/storage/scsiglue.c
index 866b5df36ed1..0e400f382f3a 100644
--- a/drivers/usb/storage/scsiglue.c
+++ b/drivers/usb/storage/scsiglue.c
@@ -241,7 +241,7 @@ static int slave_configure(struct scsi_device *sdev)
 
 		/* Some USB cardreaders have trouble reading an sdcard's last
 		 * sector in a larger then 1 sector read, since the performance
-		 * impact is negible we set this flag for all USB disks */
+		 * impact is negligible we set this flag for all USB disks */
 		sdev->last_sector_bug = 1;
 
 		/* Enable last-sector hacks for single-target devices using
diff --git a/drivers/usb/storage/sddr09.c b/drivers/usb/storage/sddr09.c
index 38a4504ce450..3847053d732c 100644
--- a/drivers/usb/storage/sddr09.c
+++ b/drivers/usb/storage/sddr09.c
@@ -1155,8 +1155,7 @@ sddr09_get_cardinfo(struct us_data *us, unsigned char flags) {
 		return NULL;
 	}
 
-	sprintf(blurbtxt, "sddr09: Found Flash card, ID = %02X %02X %02X %02X",
-		deviceID[0], deviceID[1], deviceID[2], deviceID[3]);
+	sprintf(blurbtxt, "sddr09: Found Flash card, ID = %4ph", deviceID);
 
 	/* Byte 0 is the manufacturer */
 	sprintf(blurbtxt + strlen(blurbtxt),
diff --git a/drivers/usb/storage/uas-detect.h b/drivers/usb/storage/uas-detect.h
index 503ac5c8d80f..8a6f371ed6e7 100644
--- a/drivers/usb/storage/uas-detect.h
+++ b/drivers/usb/storage/uas-detect.h
@@ -59,10 +59,6 @@ static int uas_use_uas_driver(struct usb_interface *intf,
 	unsigned long flags = id->driver_info;
 	int r, alt;
 
-	usb_stor_adjust_quirks(udev, &flags);
-
-	if (flags & US_FL_IGNORE_UAS)
-		return 0;
 
 	alt = uas_find_uas_alt_setting(intf);
 	if (alt < 0)
@@ -72,6 +68,29 @@ static int uas_use_uas_driver(struct usb_interface *intf,
 	if (r < 0)
 		return 0;
 
+	/*
+	 * ASM1051 and older ASM1053 devices have the same usb-id, and UAS is
+	 * broken on the ASM1051, use the number of streams to differentiate.
+	 * New ASM1053-s also support 32 streams, but have a different prod-id.
+	 */
+	if (le16_to_cpu(udev->descriptor.idVendor) == 0x174c &&
+			le16_to_cpu(udev->descriptor.idProduct) == 0x55aa) {
+		if (udev->speed < USB_SPEED_SUPER) {
+			/* No streams info, assume ASM1051 */
+			flags |= US_FL_IGNORE_UAS;
+		} else if (usb_ss_max_streams(&eps[1]->ss_ep_comp) == 32) {
+			flags |= US_FL_IGNORE_UAS;
+		}
+	}
+
+	usb_stor_adjust_quirks(udev, &flags);
+
+	if (flags & US_FL_IGNORE_UAS) {
+		dev_warn(&udev->dev,
+			"UAS is blacklisted for this device, using usb-storage instead\n");
+		return 0;
+	}
+
 	if (udev->bus->sg_tablesize == 0) {
 		dev_warn(&udev->dev,
 			"The driver for the USB controller %s does not support scatter-gather which is\n",
diff --git a/drivers/usb/storage/uas.c b/drivers/usb/storage/uas.c
index 3f42785f653c..89b24349269e 100644
--- a/drivers/usb/storage/uas.c
+++ b/drivers/usb/storage/uas.c
@@ -2,7 +2,7 @@
  * USB Attached SCSI
  * Note that this is not the same as the USB Mass Storage driver
  *
- * Copyright Hans de Goede <hdegoede@redhat.com> for Red Hat, Inc. 2013
+ * Copyright Hans de Goede <hdegoede@redhat.com> for Red Hat, Inc. 2013 - 2014
  * Copyright Matthew Wilcox for Intel Corp, 2010
  * Copyright Sarah Sharp for Intel Corp, 2010
  *
@@ -28,20 +28,9 @@
 #include <scsi/scsi_tcq.h>
 
 #include "uas-detect.h"
+#include "scsiglue.h"
 
-/*
- * The r00-r01c specs define this version of the SENSE IU data structure.
- * It's still in use by several different firmware releases.
- */
-struct sense_iu_old {
-	__u8 iu_id;
-	__u8 rsvd1;
-	__be16 tag;
-	__be16 len;
-	__u8 status;
-	__u8 service_response;
-	__u8 sense[SCSI_SENSE_BUFFERSIZE];
-};
+#define MAX_CMNDS 256
 
 struct uas_dev_info {
 	struct usb_interface *intf;
@@ -49,18 +38,14 @@ struct uas_dev_info {
 	struct usb_anchor cmd_urbs;
 	struct usb_anchor sense_urbs;
 	struct usb_anchor data_urbs;
+	unsigned long flags;
 	int qdepth, resetting;
-	struct response_iu response;
 	unsigned cmd_pipe, status_pipe, data_in_pipe, data_out_pipe;
 	unsigned use_streams:1;
-	unsigned uas_sense_old:1;
-	unsigned running_task:1;
 	unsigned shutdown:1;
-	struct scsi_cmnd *cmnd;
+	struct scsi_cmnd *cmnd[MAX_CMNDS];
 	spinlock_t lock;
 	struct work_struct work;
-	struct list_head inflight_list;
-	struct list_head dead_list;
 };
 
 enum {
@@ -74,10 +59,8 @@ enum {
 	COMMAND_INFLIGHT        = (1 << 8),
 	DATA_IN_URB_INFLIGHT    = (1 << 9),
 	DATA_OUT_URB_INFLIGHT   = (1 << 10),
-	COMMAND_COMPLETED       = (1 << 11),
-	COMMAND_ABORTED         = (1 << 12),
-	UNLINK_DATA_URBS        = (1 << 13),
-	IS_IN_WORK_LIST         = (1 << 14),
+	COMMAND_ABORTED         = (1 << 11),
+	IS_IN_WORK_LIST         = (1 << 12),
 };
 
 /* Overrides scsi_pointer */
@@ -87,7 +70,6 @@ struct uas_cmd_info {
 	struct urb *cmd_urb;
 	struct urb *data_in_urb;
 	struct urb *data_out_urb;
-	struct list_head list;
 };
 
 /* I hate forward declarations, but I actually have a loop */
@@ -96,43 +78,29 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 static void uas_do_work(struct work_struct *work);
 static int uas_try_complete(struct scsi_cmnd *cmnd, const char *caller);
 static void uas_free_streams(struct uas_dev_info *devinfo);
-static void uas_log_cmd_state(struct scsi_cmnd *cmnd, const char *caller);
-
-/* Must be called with devinfo->lock held, will temporary unlock the lock */
-static void uas_unlink_data_urbs(struct uas_dev_info *devinfo,
-				 struct uas_cmd_info *cmdinfo,
-				 unsigned long *lock_flags)
-{
-	/*
-	 * The UNLINK_DATA_URBS flag makes sure uas_try_complete
-	 * (called by urb completion) doesn't release cmdinfo
-	 * underneath us.
-	 */
-	cmdinfo->state |= UNLINK_DATA_URBS;
-	spin_unlock_irqrestore(&devinfo->lock, *lock_flags);
-
-	if (cmdinfo->data_in_urb)
-		usb_unlink_urb(cmdinfo->data_in_urb);
-	if (cmdinfo->data_out_urb)
-		usb_unlink_urb(cmdinfo->data_out_urb);
-
-	spin_lock_irqsave(&devinfo->lock, *lock_flags);
-	cmdinfo->state &= ~UNLINK_DATA_URBS;
-}
+static void uas_log_cmd_state(struct scsi_cmnd *cmnd, const char *prefix,
+				int status);
 
 static void uas_do_work(struct work_struct *work)
 {
 	struct uas_dev_info *devinfo =
 		container_of(work, struct uas_dev_info, work);
 	struct uas_cmd_info *cmdinfo;
+	struct scsi_cmnd *cmnd;
 	unsigned long flags;
-	int err;
+	int i, err;
 
 	spin_lock_irqsave(&devinfo->lock, flags);
-	list_for_each_entry(cmdinfo, &devinfo->inflight_list, list) {
-		struct scsi_pointer *scp = (void *)cmdinfo;
-		struct scsi_cmnd *cmnd = container_of(scp, struct scsi_cmnd,
-						      SCp);
+
+	if (devinfo->resetting)
+		goto out;
+
+	for (i = 0; i < devinfo->qdepth; i++) {
+		if (!devinfo->cmnd[i])
+			continue;
+
+		cmnd = devinfo->cmnd[i];
+		cmdinfo = (void *)&cmnd->SCp;
 
 		if (!(cmdinfo->state & IS_IN_WORK_LIST))
 			continue;
@@ -143,35 +111,7 @@ static void uas_do_work(struct work_struct *work)
 		else
 			schedule_work(&devinfo->work);
 	}
-	spin_unlock_irqrestore(&devinfo->lock, flags);
-}
-
-static void uas_mark_cmd_dead(struct uas_dev_info *devinfo,
-			      struct uas_cmd_info *cmdinfo,
-			      int result, const char *caller)
-{
-	struct scsi_pointer *scp = (void *)cmdinfo;
-	struct scsi_cmnd *cmnd = container_of(scp, struct scsi_cmnd, SCp);
-
-	uas_log_cmd_state(cmnd, caller);
-	WARN_ON_ONCE(!spin_is_locked(&devinfo->lock));
-	WARN_ON_ONCE(cmdinfo->state & COMMAND_ABORTED);
-	cmdinfo->state |= COMMAND_ABORTED;
-	cmdinfo->state &= ~IS_IN_WORK_LIST;
-	cmnd->result = result << 16;
-	list_move_tail(&cmdinfo->list, &devinfo->dead_list);
-}
-
-static void uas_abort_inflight(struct uas_dev_info *devinfo, int result,
-			       const char *caller)
-{
-	struct uas_cmd_info *cmdinfo;
-	struct uas_cmd_info *temp;
-	unsigned long flags;
-
-	spin_lock_irqsave(&devinfo->lock, flags);
-	list_for_each_entry_safe(cmdinfo, temp, &devinfo->inflight_list, list)
-		uas_mark_cmd_dead(devinfo, cmdinfo, result, caller);
+out:
 	spin_unlock_irqrestore(&devinfo->lock, flags);
 }
 
@@ -181,31 +121,32 @@ static void uas_add_work(struct uas_cmd_info *cmdinfo)
 	struct scsi_cmnd *cmnd = container_of(scp, struct scsi_cmnd, SCp);
 	struct uas_dev_info *devinfo = cmnd->device->hostdata;
 
-	WARN_ON_ONCE(!spin_is_locked(&devinfo->lock));
+	lockdep_assert_held(&devinfo->lock);
 	cmdinfo->state |= IS_IN_WORK_LIST;
 	schedule_work(&devinfo->work);
 }
 
-static void uas_zap_dead(struct uas_dev_info *devinfo)
+static void uas_zap_pending(struct uas_dev_info *devinfo, int result)
 {
 	struct uas_cmd_info *cmdinfo;
-	struct uas_cmd_info *temp;
+	struct scsi_cmnd *cmnd;
 	unsigned long flags;
+	int i, err;
 
 	spin_lock_irqsave(&devinfo->lock, flags);
-	list_for_each_entry_safe(cmdinfo, temp, &devinfo->dead_list, list) {
-		struct scsi_pointer *scp = (void *)cmdinfo;
-		struct scsi_cmnd *cmnd = container_of(scp, struct scsi_cmnd,
-						      SCp);
-		uas_log_cmd_state(cmnd, __func__);
-		WARN_ON_ONCE(!(cmdinfo->state & COMMAND_ABORTED));
-		/* all urbs are killed, clear inflight bits */
-		cmdinfo->state &= ~(COMMAND_INFLIGHT |
-				    DATA_IN_URB_INFLIGHT |
-				    DATA_OUT_URB_INFLIGHT);
-		uas_try_complete(cmnd, __func__);
+	for (i = 0; i < devinfo->qdepth; i++) {
+		if (!devinfo->cmnd[i])
+			continue;
+
+		cmnd = devinfo->cmnd[i];
+		cmdinfo = (void *)&cmnd->SCp;
+		uas_log_cmd_state(cmnd, __func__, 0);
+		/* Sense urbs were killed, clear COMMAND_INFLIGHT manually */
+		cmdinfo->state &= ~COMMAND_INFLIGHT;
+		cmnd->result = result << 16;
+		err = uas_try_complete(cmnd, __func__);
+		WARN_ON(err != 0);
 	}
-	devinfo->running_task = 0;
 	spin_unlock_irqrestore(&devinfo->lock, flags);
 }
 
@@ -232,36 +173,30 @@ static void uas_sense(struct urb *urb, struct scsi_cmnd *cmnd)
 	cmnd->result = sense_iu->status;
 }
 
-static void uas_sense_old(struct urb *urb, struct scsi_cmnd *cmnd)
+/*
+ * scsi-tags go from 0 - (nr_tags - 1), uas tags need to match stream-ids,
+ * which go from 1 - nr_streams. And we use 1 for untagged commands.
+ */
+static int uas_get_tag(struct scsi_cmnd *cmnd)
 {
-	struct sense_iu_old *sense_iu = urb->transfer_buffer;
-	struct scsi_device *sdev = cmnd->device;
+	int tag;
 
-	if (urb->actual_length > 8) {
-		unsigned len = be16_to_cpup(&sense_iu->len) - 2;
-		if (len + 8 != urb->actual_length) {
-			int newlen = min(len + 8, urb->actual_length) - 8;
-			if (newlen < 0)
-				newlen = 0;
-			sdev_printk(KERN_INFO, sdev, "%s: urb length %d "
-				"disagrees with IU sense data length %d, "
-				"using %d bytes of sense data\n", __func__,
-					urb->actual_length, len, newlen);
-			len = newlen;
-		}
-		memcpy(cmnd->sense_buffer, sense_iu->sense, len);
-	}
+	if (blk_rq_tagged(cmnd->request))
+		tag = cmnd->request->tag + 2;
+	else
+		tag = 1;
 
-	cmnd->result = sense_iu->status;
+	return tag;
 }
 
-static void uas_log_cmd_state(struct scsi_cmnd *cmnd, const char *caller)
+static void uas_log_cmd_state(struct scsi_cmnd *cmnd, const char *prefix,
+			      int status)
 {
 	struct uas_cmd_info *ci = (void *)&cmnd->SCp;
 
-	scmd_printk(KERN_INFO, cmnd, "%s %p tag %d, inflight:"
-		    "%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
-		    caller, cmnd, cmnd->request->tag,
+	scmd_printk(KERN_INFO, cmnd,
+		    "%s %d tag %d inflight:%s%s%s%s%s%s%s%s%s%s%s%s ",
+		    prefix, status, uas_get_tag(cmnd),
 		    (ci->state & SUBMIT_STATUS_URB)     ? " s-st"  : "",
 		    (ci->state & ALLOC_DATA_IN_URB)     ? " a-in"  : "",
 		    (ci->state & SUBMIT_DATA_IN_URB)    ? " s-in"  : "",
@@ -272,10 +207,28 @@ static void uas_log_cmd_state(struct scsi_cmnd *cmnd, const char *caller)
 		    (ci->state & COMMAND_INFLIGHT)      ? " CMD"   : "",
 		    (ci->state & DATA_IN_URB_INFLIGHT)  ? " IN"    : "",
 		    (ci->state & DATA_OUT_URB_INFLIGHT) ? " OUT"   : "",
-		    (ci->state & COMMAND_COMPLETED)     ? " done"  : "",
 		    (ci->state & COMMAND_ABORTED)       ? " abort" : "",
-		    (ci->state & UNLINK_DATA_URBS)      ? " unlink": "",
 		    (ci->state & IS_IN_WORK_LIST)       ? " work"  : "");
+	scsi_print_command(cmnd);
+}
+
+static void uas_free_unsubmitted_urbs(struct scsi_cmnd *cmnd)
+{
+	struct uas_cmd_info *cmdinfo;
+
+	if (!cmnd)
+		return;
+
+	cmdinfo = (void *)&cmnd->SCp;
+
+	if (cmdinfo->state & SUBMIT_CMD_URB)
+		usb_free_urb(cmdinfo->cmd_urb);
+
+	/* data urbs may have never gotten their submit flag set */
+	if (!(cmdinfo->state & DATA_IN_URB_INFLIGHT))
+		usb_free_urb(cmdinfo->data_in_urb);
+	if (!(cmdinfo->state & DATA_OUT_URB_INFLIGHT))
+		usb_free_urb(cmdinfo->data_out_urb);
 }
 
 static int uas_try_complete(struct scsi_cmnd *cmnd, const char *caller)
@@ -283,19 +236,14 @@ static int uas_try_complete(struct scsi_cmnd *cmnd, const char *caller)
 	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	struct uas_dev_info *devinfo = (void *)cmnd->device->hostdata;
 
-	WARN_ON_ONCE(!spin_is_locked(&devinfo->lock));
+	lockdep_assert_held(&devinfo->lock);
 	if (cmdinfo->state & (COMMAND_INFLIGHT |
 			      DATA_IN_URB_INFLIGHT |
 			      DATA_OUT_URB_INFLIGHT |
-			      UNLINK_DATA_URBS))
+			      COMMAND_ABORTED))
 		return -EBUSY;
-	WARN_ON_ONCE(cmdinfo->state & COMMAND_COMPLETED);
-	cmdinfo->state |= COMMAND_COMPLETED;
-	usb_free_urb(cmdinfo->data_in_urb);
-	usb_free_urb(cmdinfo->data_out_urb);
-	if (cmdinfo->state & COMMAND_ABORTED)
-		scmd_printk(KERN_INFO, cmnd, "abort completed\n");
-	list_del(&cmdinfo->list);
+	devinfo->cmnd[uas_get_tag(cmnd) - 1] = NULL;
+	uas_free_unsubmitted_urbs(cmnd);
 	cmnd->scsi_done(cmnd);
 	return 0;
 }
@@ -318,63 +266,48 @@ static void uas_stat_cmplt(struct urb *urb)
 	struct iu *iu = urb->transfer_buffer;
 	struct Scsi_Host *shost = urb->context;
 	struct uas_dev_info *devinfo = (struct uas_dev_info *)shost->hostdata;
+	struct urb *data_in_urb = NULL;
+	struct urb *data_out_urb = NULL;
 	struct scsi_cmnd *cmnd;
 	struct uas_cmd_info *cmdinfo;
 	unsigned long flags;
-	u16 tag;
+	unsigned int idx;
+
+	spin_lock_irqsave(&devinfo->lock, flags);
+
+	if (devinfo->resetting)
+		goto out;
 
 	if (urb->status) {
-		if (urb->status == -ENOENT) {
-			dev_err(&urb->dev->dev, "stat urb: killed, stream %d\n",
-				urb->stream_id);
-		} else {
+		if (urb->status != -ENOENT && urb->status != -ECONNRESET) {
 			dev_err(&urb->dev->dev, "stat urb: status %d\n",
 				urb->status);
 		}
-		usb_free_urb(urb);
-		return;
+		goto out;
 	}
 
-	if (devinfo->resetting) {
-		usb_free_urb(urb);
-		return;
+	idx = be16_to_cpup(&iu->tag) - 1;
+	if (idx >= MAX_CMNDS || !devinfo->cmnd[idx]) {
+		dev_err(&urb->dev->dev,
+			"stat urb: no pending cmd for tag %d\n", idx + 1);
+		goto out;
 	}
 
-	spin_lock_irqsave(&devinfo->lock, flags);
-	tag = be16_to_cpup(&iu->tag) - 1;
-	if (tag == 0)
-		cmnd = devinfo->cmnd;
-	else
-		cmnd = scsi_host_find_tag(shost, tag - 1);
-
-	if (!cmnd) {
-		if (iu->iu_id == IU_ID_RESPONSE) {
-			if (!devinfo->running_task)
-				dev_warn(&urb->dev->dev,
-				    "stat urb: recv unexpected response iu\n");
-			/* store results for uas_eh_task_mgmt() */
-			memcpy(&devinfo->response, iu, sizeof(devinfo->response));
-		}
-		usb_free_urb(urb);
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		return;
+	cmnd = devinfo->cmnd[idx];
+	cmdinfo = (void *)&cmnd->SCp;
+
+	if (!(cmdinfo->state & COMMAND_INFLIGHT)) {
+		uas_log_cmd_state(cmnd, "unexpected status cmplt", 0);
+		goto out;
 	}
 
-	cmdinfo = (void *)&cmnd->SCp;
 	switch (iu->iu_id) {
 	case IU_ID_STATUS:
-		if (devinfo->cmnd == cmnd)
-			devinfo->cmnd = NULL;
-
-		if (urb->actual_length < 16)
-			devinfo->uas_sense_old = 1;
-		if (devinfo->uas_sense_old)
-			uas_sense_old(urb, cmnd);
-		else
-			uas_sense(urb, cmnd);
+		uas_sense(urb, cmnd);
 		if (cmnd->result != 0) {
 			/* cancel data transfers on error */
-			uas_unlink_data_urbs(devinfo, cmdinfo, &flags);
+			data_in_urb = usb_get_urb(cmdinfo->data_in_urb);
+			data_out_urb = usb_get_urb(cmdinfo->data_out_urb);
 		}
 		cmdinfo->state &= ~COMMAND_INFLIGHT;
 		uas_try_complete(cmnd, __func__);
@@ -382,7 +315,7 @@ static void uas_stat_cmplt(struct urb *urb)
 	case IU_ID_READ_READY:
 		if (!cmdinfo->data_in_urb ||
 				(cmdinfo->state & DATA_IN_URB_INFLIGHT)) {
-			scmd_printk(KERN_ERR, cmnd, "unexpected read rdy\n");
+			uas_log_cmd_state(cmnd, "unexpected read rdy", 0);
 			break;
 		}
 		uas_xfer_data(urb, cmnd, SUBMIT_DATA_IN_URB);
@@ -390,17 +323,37 @@ static void uas_stat_cmplt(struct urb *urb)
 	case IU_ID_WRITE_READY:
 		if (!cmdinfo->data_out_urb ||
 				(cmdinfo->state & DATA_OUT_URB_INFLIGHT)) {
-			scmd_printk(KERN_ERR, cmnd, "unexpected write rdy\n");
+			uas_log_cmd_state(cmnd, "unexpected write rdy", 0);
 			break;
 		}
 		uas_xfer_data(urb, cmnd, SUBMIT_DATA_OUT_URB);
 		break;
+	case IU_ID_RESPONSE:
+		uas_log_cmd_state(cmnd, "unexpected response iu",
+				  ((struct response_iu *)iu)->response_code);
+		/* Error, cancel data transfers */
+		data_in_urb = usb_get_urb(cmdinfo->data_in_urb);
+		data_out_urb = usb_get_urb(cmdinfo->data_out_urb);
+		cmdinfo->state &= ~COMMAND_INFLIGHT;
+		cmnd->result = DID_ERROR << 16;
+		uas_try_complete(cmnd, __func__);
+		break;
 	default:
-		scmd_printk(KERN_ERR, cmnd,
-			"Bogus IU (%d) received on status pipe\n", iu->iu_id);
+		uas_log_cmd_state(cmnd, "bogus IU", iu->iu_id);
 	}
+out:
 	usb_free_urb(urb);
 	spin_unlock_irqrestore(&devinfo->lock, flags);
+
+	/* Unlinking of data urbs must be done without holding the lock */
+	if (data_in_urb) {
+		usb_unlink_urb(data_in_urb);
+		usb_put_urb(data_in_urb);
+	}
+	if (data_out_urb) {
+		usb_unlink_urb(data_out_urb);
+		usb_put_urb(data_out_urb);
+	}
 }
 
 static void uas_data_cmplt(struct urb *urb)
@@ -412,57 +365,69 @@ static void uas_data_cmplt(struct urb *urb)
 	unsigned long flags;
 
 	spin_lock_irqsave(&devinfo->lock, flags);
+
 	if (cmdinfo->data_in_urb == urb) {
 		sdb = scsi_in(cmnd);
 		cmdinfo->state &= ~DATA_IN_URB_INFLIGHT;
+		cmdinfo->data_in_urb = NULL;
 	} else if (cmdinfo->data_out_urb == urb) {
 		sdb = scsi_out(cmnd);
 		cmdinfo->state &= ~DATA_OUT_URB_INFLIGHT;
+		cmdinfo->data_out_urb = NULL;
 	}
 	if (sdb == NULL) {
 		WARN_ON_ONCE(1);
-	} else if (urb->status) {
-		if (urb->status != -ECONNRESET) {
-			uas_log_cmd_state(cmnd, __func__);
-			scmd_printk(KERN_ERR, cmnd,
-				"data cmplt err %d stream %d\n",
-				urb->status, urb->stream_id);
-		}
+		goto out;
+	}
+
+	if (devinfo->resetting)
+		goto out;
+
+	/* Data urbs should not complete before the cmd urb is submitted */
+	if (cmdinfo->state & SUBMIT_CMD_URB) {
+		uas_log_cmd_state(cmnd, "unexpected data cmplt", 0);
+		goto out;
+	}
+
+	if (urb->status) {
+		if (urb->status != -ENOENT && urb->status != -ECONNRESET)
+			uas_log_cmd_state(cmnd, "data cmplt err", urb->status);
 		/* error: no data transfered */
 		sdb->resid = sdb->length;
 	} else {
 		sdb->resid = sdb->length - urb->actual_length;
 	}
 	uas_try_complete(cmnd, __func__);
+out:
+	usb_free_urb(urb);
 	spin_unlock_irqrestore(&devinfo->lock, flags);
 }
 
 static void uas_cmd_cmplt(struct urb *urb)
 {
-	struct scsi_cmnd *cmnd = urb->context;
+	if (urb->status)
+		dev_err(&urb->dev->dev, "cmd cmplt err %d\n", urb->status);
 
-	if (urb->status) {
-		uas_log_cmd_state(cmnd, __func__);
-		scmd_printk(KERN_ERR, cmnd, "cmd cmplt err %d\n", urb->status);
-	}
 	usb_free_urb(urb);
 }
 
 static struct urb *uas_alloc_data_urb(struct uas_dev_info *devinfo, gfp_t gfp,
-				      unsigned int pipe, u16 stream_id,
 				      struct scsi_cmnd *cmnd,
 				      enum dma_data_direction dir)
 {
 	struct usb_device *udev = devinfo->udev;
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	struct urb *urb = usb_alloc_urb(0, gfp);
 	struct scsi_data_buffer *sdb = (dir == DMA_FROM_DEVICE)
 		? scsi_in(cmnd) : scsi_out(cmnd);
+	unsigned int pipe = (dir == DMA_FROM_DEVICE)
+		? devinfo->data_in_pipe : devinfo->data_out_pipe;
 
 	if (!urb)
 		goto out;
 	usb_fill_bulk_urb(urb, udev, pipe, NULL, sdb->length,
 			  uas_data_cmplt, cmnd);
-	urb->stream_id = stream_id;
+	urb->stream_id = cmdinfo->stream;
 	urb->num_sgs = udev->bus->sg_tablesize ? sdb->table.nents : 0;
 	urb->sg = sdb->table.sgl;
  out:
@@ -470,9 +435,10 @@ static struct urb *uas_alloc_data_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 }
 
 static struct urb *uas_alloc_sense_urb(struct uas_dev_info *devinfo, gfp_t gfp,
-				       struct Scsi_Host *shost, u16 stream_id)
+				       struct scsi_cmnd *cmnd)
 {
 	struct usb_device *udev = devinfo->udev;
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	struct urb *urb = usb_alloc_urb(0, gfp);
 	struct sense_iu *iu;
 
@@ -484,8 +450,8 @@ static struct urb *uas_alloc_sense_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 		goto free;
 
 	usb_fill_bulk_urb(urb, udev, devinfo->status_pipe, iu, sizeof(*iu),
-						uas_stat_cmplt, shost);
-	urb->stream_id = stream_id;
+			  uas_stat_cmplt, cmnd->device->host);
+	urb->stream_id = cmdinfo->stream;
 	urb->transfer_flags |= URB_FREE_BUFFER;
  out:
 	return urb;
@@ -515,17 +481,14 @@ static struct urb *uas_alloc_cmd_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 		goto free;
 
 	iu->iu_id = IU_ID_COMMAND;
-	if (blk_rq_tagged(cmnd->request))
-		iu->tag = cpu_to_be16(cmnd->request->tag + 2);
-	else
-		iu->tag = cpu_to_be16(1);
+	iu->tag = cpu_to_be16(uas_get_tag(cmnd));
 	iu->prio_attr = UAS_SIMPLE_TAG;
 	iu->len = len;
 	int_to_scsilun(sdev->lun, &iu->lun);
 	memcpy(iu->cdb, cmnd->cmnd, cmnd->cmd_len);
 
 	usb_fill_bulk_urb(urb, udev, devinfo->cmd_pipe, iu, sizeof(*iu) + len,
-							uas_cmd_cmplt, cmnd);
+							uas_cmd_cmplt, NULL);
 	urb->transfer_flags |= URB_FREE_BUFFER;
  out:
 	return urb;
@@ -534,81 +497,26 @@ static struct urb *uas_alloc_cmd_urb(struct uas_dev_info *devinfo, gfp_t gfp,
 	return NULL;
 }
 
-static int uas_submit_task_urb(struct scsi_cmnd *cmnd, gfp_t gfp,
-			       u8 function, u16 stream_id)
-{
-	struct uas_dev_info *devinfo = (void *)cmnd->device->hostdata;
-	struct usb_device *udev = devinfo->udev;
-	struct urb *urb = usb_alloc_urb(0, gfp);
-	struct task_mgmt_iu *iu;
-	int err = -ENOMEM;
-
-	if (!urb)
-		goto err;
-
-	iu = kzalloc(sizeof(*iu), gfp);
-	if (!iu)
-		goto err;
-
-	iu->iu_id = IU_ID_TASK_MGMT;
-	iu->tag = cpu_to_be16(stream_id);
-	int_to_scsilun(cmnd->device->lun, &iu->lun);
-
-	iu->function = function;
-	switch (function) {
-	case TMF_ABORT_TASK:
-		if (blk_rq_tagged(cmnd->request))
-			iu->task_tag = cpu_to_be16(cmnd->request->tag + 2);
-		else
-			iu->task_tag = cpu_to_be16(1);
-		break;
-	}
-
-	usb_fill_bulk_urb(urb, udev, devinfo->cmd_pipe, iu, sizeof(*iu),
-			  uas_cmd_cmplt, cmnd);
-	urb->transfer_flags |= URB_FREE_BUFFER;
-
-	usb_anchor_urb(urb, &devinfo->cmd_urbs);
-	err = usb_submit_urb(urb, gfp);
-	if (err) {
-		usb_unanchor_urb(urb);
-		uas_log_cmd_state(cmnd, __func__);
-		scmd_printk(KERN_ERR, cmnd, "task submission err %d\n", err);
-		goto err;
-	}
-
-	return 0;
-
-err:
-	usb_free_urb(urb);
-	return err;
-}
-
 /*
  * Why should I request the Status IU before sending the Command IU?  Spec
  * says to, but also says the device may receive them in any order.  Seems
  * daft to me.
  */
 
-static struct urb *uas_submit_sense_urb(struct scsi_cmnd *cmnd,
-					gfp_t gfp, unsigned int stream)
+static struct urb *uas_submit_sense_urb(struct scsi_cmnd *cmnd, gfp_t gfp)
 {
-	struct Scsi_Host *shost = cmnd->device->host;
-	struct uas_dev_info *devinfo = (struct uas_dev_info *)shost->hostdata;
+	struct uas_dev_info *devinfo = cmnd->device->hostdata;
 	struct urb *urb;
 	int err;
 
-	urb = uas_alloc_sense_urb(devinfo, gfp, shost, stream);
+	urb = uas_alloc_sense_urb(devinfo, gfp, cmnd);
 	if (!urb)
 		return NULL;
 	usb_anchor_urb(urb, &devinfo->sense_urbs);
 	err = usb_submit_urb(urb, gfp);
 	if (err) {
 		usb_unanchor_urb(urb);
-		uas_log_cmd_state(cmnd, __func__);
-		shost_printk(KERN_INFO, shost,
-			     "sense urb submission error %d stream %d\n",
-			     err, stream);
+		uas_log_cmd_state(cmnd, "sense submit err", err);
 		usb_free_urb(urb);
 		return NULL;
 	}
@@ -622,9 +530,9 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 	struct urb *urb;
 	int err;
 
-	WARN_ON_ONCE(!spin_is_locked(&devinfo->lock));
+	lockdep_assert_held(&devinfo->lock);
 	if (cmdinfo->state & SUBMIT_STATUS_URB) {
-		urb = uas_submit_sense_urb(cmnd, gfp, cmdinfo->stream);
+		urb = uas_submit_sense_urb(cmnd, gfp);
 		if (!urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		cmdinfo->state &= ~SUBMIT_STATUS_URB;
@@ -632,8 +540,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 
 	if (cmdinfo->state & ALLOC_DATA_IN_URB) {
 		cmdinfo->data_in_urb = uas_alloc_data_urb(devinfo, gfp,
-					devinfo->data_in_pipe, cmdinfo->stream,
-					cmnd, DMA_FROM_DEVICE);
+							cmnd, DMA_FROM_DEVICE);
 		if (!cmdinfo->data_in_urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		cmdinfo->state &= ~ALLOC_DATA_IN_URB;
@@ -644,10 +551,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 		err = usb_submit_urb(cmdinfo->data_in_urb, gfp);
 		if (err) {
 			usb_unanchor_urb(cmdinfo->data_in_urb);
-			uas_log_cmd_state(cmnd, __func__);
-			scmd_printk(KERN_INFO, cmnd,
-				"data in urb submission error %d stream %d\n",
-				err, cmdinfo->data_in_urb->stream_id);
+			uas_log_cmd_state(cmnd, "data in submit err", err);
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		}
 		cmdinfo->state &= ~SUBMIT_DATA_IN_URB;
@@ -656,8 +560,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 
 	if (cmdinfo->state & ALLOC_DATA_OUT_URB) {
 		cmdinfo->data_out_urb = uas_alloc_data_urb(devinfo, gfp,
-					devinfo->data_out_pipe, cmdinfo->stream,
-					cmnd, DMA_TO_DEVICE);
+							cmnd, DMA_TO_DEVICE);
 		if (!cmdinfo->data_out_urb)
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		cmdinfo->state &= ~ALLOC_DATA_OUT_URB;
@@ -668,10 +571,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 		err = usb_submit_urb(cmdinfo->data_out_urb, gfp);
 		if (err) {
 			usb_unanchor_urb(cmdinfo->data_out_urb);
-			uas_log_cmd_state(cmnd, __func__);
-			scmd_printk(KERN_INFO, cmnd,
-				"data out urb submission error %d stream %d\n",
-				err, cmdinfo->data_out_urb->stream_id);
+			uas_log_cmd_state(cmnd, "data out submit err", err);
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		}
 		cmdinfo->state &= ~SUBMIT_DATA_OUT_URB;
@@ -690,9 +590,7 @@ static int uas_submit_urbs(struct scsi_cmnd *cmnd,
 		err = usb_submit_urb(cmdinfo->cmd_urb, gfp);
 		if (err) {
 			usb_unanchor_urb(cmdinfo->cmd_urb);
-			uas_log_cmd_state(cmnd, __func__);
-			scmd_printk(KERN_INFO, cmnd,
-				    "cmd urb submission error %d\n", err);
+			uas_log_cmd_state(cmnd, "cmd submit err", err);
 			return SCSI_MLQUEUE_DEVICE_BUSY;
 		}
 		cmdinfo->cmd_urb = NULL;
@@ -710,10 +608,24 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 	struct uas_dev_info *devinfo = sdev->hostdata;
 	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
 	unsigned long flags;
+	unsigned int stream;
 	int err;
 
 	BUILD_BUG_ON(sizeof(struct uas_cmd_info) > sizeof(struct scsi_pointer));
 
+	/* Re-check scsi_block_requests now that we've the host-lock */
+	if (cmnd->device->host->host_self_blocked)
+		return SCSI_MLQUEUE_DEVICE_BUSY;
+
+	if ((devinfo->flags & US_FL_NO_ATA_1X) &&
+			(cmnd->cmnd[0] == ATA_12 || cmnd->cmnd[0] == ATA_16)) {
+		memcpy(cmnd->sense_buffer, usb_stor_sense_invalidCDB,
+		       sizeof(usb_stor_sense_invalidCDB));
+		cmnd->result = SAM_STAT_CHECK_CONDITION;
+		cmnd->scsi_done(cmnd);
+		return 0;
+	}
+
 	spin_lock_irqsave(&devinfo->lock, flags);
 
 	if (devinfo->resetting) {
@@ -723,24 +635,17 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 		return 0;
 	}
 
-	if (devinfo->cmnd) {
+	stream = uas_get_tag(cmnd);
+	if (devinfo->cmnd[stream - 1]) {
 		spin_unlock_irqrestore(&devinfo->lock, flags);
 		return SCSI_MLQUEUE_DEVICE_BUSY;
 	}
 
-	memset(cmdinfo, 0, sizeof(*cmdinfo));
-
-	if (blk_rq_tagged(cmnd->request)) {
-		cmdinfo->stream = cmnd->request->tag + 2;
-	} else {
-		devinfo->cmnd = cmnd;
-		cmdinfo->stream = 1;
-	}
-
 	cmnd->scsi_done = done;
 
-	cmdinfo->state = SUBMIT_STATUS_URB |
-			ALLOC_CMD_URB | SUBMIT_CMD_URB;
+	memset(cmdinfo, 0, sizeof(*cmdinfo));
+	cmdinfo->stream = stream;
+	cmdinfo->state = SUBMIT_STATUS_URB | ALLOC_CMD_URB | SUBMIT_CMD_URB;
 
 	switch (cmnd->sc_data_direction) {
 	case DMA_FROM_DEVICE:
@@ -769,123 +674,54 @@ static int uas_queuecommand_lck(struct scsi_cmnd *cmnd,
 		uas_add_work(cmdinfo);
 	}
 
-	list_add_tail(&cmdinfo->list, &devinfo->inflight_list);
+	devinfo->cmnd[stream - 1] = cmnd;
 	spin_unlock_irqrestore(&devinfo->lock, flags);
 	return 0;
 }
 
 static DEF_SCSI_QCMD(uas_queuecommand)
 
-static int uas_eh_task_mgmt(struct scsi_cmnd *cmnd,
-			    const char *fname, u8 function)
+/*
+ * For now we do not support actually sending an abort to the device, so
+ * this eh always fails. Still we must define it to make sure that we've
+ * dropped all references to the cmnd in question once this function exits.
+ */
+static int uas_eh_abort_handler(struct scsi_cmnd *cmnd)
 {
-	struct Scsi_Host *shost = cmnd->device->host;
-	struct uas_dev_info *devinfo = (struct uas_dev_info *)shost->hostdata;
-	u16 tag = devinfo->qdepth;
+	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
+	struct uas_dev_info *devinfo = (void *)cmnd->device->hostdata;
+	struct urb *data_in_urb = NULL;
+	struct urb *data_out_urb = NULL;
 	unsigned long flags;
-	struct urb *sense_urb;
-	int result = SUCCESS;
 
 	spin_lock_irqsave(&devinfo->lock, flags);
 
-	if (devinfo->resetting) {
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		return FAILED;
-	}
+	uas_log_cmd_state(cmnd, __func__, 0);
 
-	if (devinfo->running_task) {
-		shost_printk(KERN_INFO, shost,
-			     "%s: %s: error already running a task\n",
-			     __func__, fname);
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		return FAILED;
-	}
+	/* Ensure that try_complete does not call scsi_done */
+	cmdinfo->state |= COMMAND_ABORTED;
 
-	devinfo->running_task = 1;
-	memset(&devinfo->response, 0, sizeof(devinfo->response));
-	sense_urb = uas_submit_sense_urb(cmnd, GFP_ATOMIC,
-					 devinfo->use_streams ? tag : 0);
-	if (!sense_urb) {
-		shost_printk(KERN_INFO, shost,
-			     "%s: %s: submit sense urb failed\n",
-			     __func__, fname);
-		devinfo->running_task = 0;
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		return FAILED;
-	}
-	if (uas_submit_task_urb(cmnd, GFP_ATOMIC, function, tag)) {
-		shost_printk(KERN_INFO, shost,
-			     "%s: %s: submit task mgmt urb failed\n",
-			     __func__, fname);
-		devinfo->running_task = 0;
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		usb_kill_urb(sense_urb);
-		return FAILED;
-	}
-	spin_unlock_irqrestore(&devinfo->lock, flags);
+	/* Drop all refs to this cmnd, kill data urbs to break their ref */
+	devinfo->cmnd[uas_get_tag(cmnd) - 1] = NULL;
+	if (cmdinfo->state & DATA_IN_URB_INFLIGHT)
+		data_in_urb = usb_get_urb(cmdinfo->data_in_urb);
+	if (cmdinfo->state & DATA_OUT_URB_INFLIGHT)
+		data_out_urb = usb_get_urb(cmdinfo->data_out_urb);
 
-	if (usb_wait_anchor_empty_timeout(&devinfo->sense_urbs, 3000) == 0) {
-		/*
-		 * Note we deliberately do not clear running_task here. If we
-		 * allow new tasks to be submitted, there is no way to figure
-		 * out if a received response_iu is for the failed task or for
-		 * the new one. A bus-reset will eventually clear running_task.
-		 */
-		shost_printk(KERN_INFO, shost,
-			     "%s: %s timed out\n", __func__, fname);
-		return FAILED;
-	}
+	uas_free_unsubmitted_urbs(cmnd);
 
-	spin_lock_irqsave(&devinfo->lock, flags);
-	devinfo->running_task = 0;
-	if (be16_to_cpu(devinfo->response.tag) != tag) {
-		shost_printk(KERN_INFO, shost,
-			     "%s: %s failed (wrong tag %d/%d)\n", __func__,
-			     fname, be16_to_cpu(devinfo->response.tag), tag);
-		result = FAILED;
-	} else if (devinfo->response.response_code != RC_TMF_COMPLETE) {
-		shost_printk(KERN_INFO, shost,
-			     "%s: %s failed (rc 0x%x)\n", __func__,
-			     fname, devinfo->response.response_code);
-		result = FAILED;
-	}
 	spin_unlock_irqrestore(&devinfo->lock, flags);
 
-	return result;
-}
-
-static int uas_eh_abort_handler(struct scsi_cmnd *cmnd)
-{
-	struct uas_cmd_info *cmdinfo = (void *)&cmnd->SCp;
-	struct uas_dev_info *devinfo = (void *)cmnd->device->hostdata;
-	unsigned long flags;
-	int ret;
-
-	spin_lock_irqsave(&devinfo->lock, flags);
-
-	if (devinfo->resetting) {
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		return FAILED;
+	if (data_in_urb) {
+		usb_kill_urb(data_in_urb);
+		usb_put_urb(data_in_urb);
 	}
-
-	uas_mark_cmd_dead(devinfo, cmdinfo, DID_ABORT, __func__);
-	if (cmdinfo->state & COMMAND_INFLIGHT) {
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		ret = uas_eh_task_mgmt(cmnd, "ABORT TASK", TMF_ABORT_TASK);
-	} else {
-		uas_unlink_data_urbs(devinfo, cmdinfo, &flags);
-		uas_try_complete(cmnd, __func__);
-		spin_unlock_irqrestore(&devinfo->lock, flags);
-		ret = SUCCESS;
+	if (data_out_urb) {
+		usb_kill_urb(data_out_urb);
+		usb_put_urb(data_out_urb);
 	}
-	return ret;
-}
 
-static int uas_eh_device_reset_handler(struct scsi_cmnd *cmnd)
-{
-	sdev_printk(KERN_INFO, cmnd->device, "%s\n", __func__);
-	return uas_eh_task_mgmt(cmnd, "LOGICAL UNIT RESET",
-				TMF_LOGICAL_UNIT_RESET);
+	return FAILED;
 }
 
 static int uas_eh_bus_reset_handler(struct scsi_cmnd *cmnd)
@@ -893,6 +729,7 @@ static int uas_eh_bus_reset_handler(struct scsi_cmnd *cmnd)
 	struct scsi_device *sdev = cmnd->device;
 	struct uas_dev_info *devinfo = sdev->hostdata;
 	struct usb_device *udev = devinfo->udev;
+	unsigned long flags;
 	int err;
 
 	err = usb_lock_device_for_reset(udev, devinfo->intf);
@@ -903,19 +740,27 @@ static int uas_eh_bus_reset_handler(struct scsi_cmnd *cmnd)
 	}
 
 	shost_printk(KERN_INFO, sdev->host, "%s start\n", __func__);
+
+	spin_lock_irqsave(&devinfo->lock, flags);
 	devinfo->resetting = 1;
-	uas_abort_inflight(devinfo, DID_RESET, __func__);
+	spin_unlock_irqrestore(&devinfo->lock, flags);
+
 	usb_kill_anchored_urbs(&devinfo->cmd_urbs);
 	usb_kill_anchored_urbs(&devinfo->sense_urbs);
 	usb_kill_anchored_urbs(&devinfo->data_urbs);
-	uas_zap_dead(devinfo);
+	uas_zap_pending(devinfo, DID_RESET);
+
 	err = usb_reset_device(udev);
+
+	spin_lock_irqsave(&devinfo->lock, flags);
 	devinfo->resetting = 0;
+	spin_unlock_irqrestore(&devinfo->lock, flags);
 
 	usb_unlock_device(udev);
 
 	if (err) {
-		shost_printk(KERN_INFO, sdev->host, "%s FAILED\n", __func__);
+		shost_printk(KERN_INFO, sdev->host, "%s FAILED err %d\n",
+			     __func__, err);
 		return FAILED;
 	}
 
@@ -950,6 +795,10 @@ static int uas_slave_alloc(struct scsi_device *sdev)
 static int uas_slave_configure(struct scsi_device *sdev)
 {
 	struct uas_dev_info *devinfo = sdev->hostdata;
+
+	if (devinfo->flags & US_FL_NO_REPORT_OPCODES)
+		sdev->no_report_opcodes = 1;
+
 	scsi_set_tag_type(sdev, MSG_ORDERED_TAG);
 	scsi_activate_tcq(sdev, devinfo->qdepth - 2);
 	return 0;
@@ -962,7 +811,6 @@ static struct scsi_host_template uas_host_template = {
 	.slave_alloc = uas_slave_alloc,
 	.slave_configure = uas_slave_configure,
 	.eh_abort_handler = uas_eh_abort_handler,
-	.eh_device_reset_handler = uas_eh_device_reset_handler,
 	.eh_bus_reset_handler = uas_eh_bus_reset_handler,
 	.can_queue = 65536,	/* Is there a limit on the _host_ ? */
 	.this_id = -1,
@@ -970,6 +818,13 @@ static struct scsi_host_template uas_host_template = {
 	.cmd_per_lun = 1,	/* until we override it */
 	.skip_settle_delay = 1,
 	.ordered_tag = 1,
+
+	/*
+	 * The uas drivers expects tags not to be bigger than the maximum
+	 * per-device queue depth, which is not true with the blk-mq tag
+	 * allocator.
+	 */
+	.disable_blk_mq = true,
 };
 
 #define UNUSUAL_DEV(id_vendor, id_product, bcdDeviceMin, bcdDeviceMax, \
@@ -982,8 +837,6 @@ static struct usb_device_id uas_usb_ids[] = {
 #	include "unusual_uas.h"
 	{ USB_INTERFACE_INFO(USB_CLASS_MASS_STORAGE, USB_SC_SCSI, USB_PR_BULK) },
 	{ USB_INTERFACE_INFO(USB_CLASS_MASS_STORAGE, USB_SC_SCSI, USB_PR_UAS) },
-	/* 0xaa is a prototype device I happen to have access to */
-	{ USB_INTERFACE_INFO(USB_CLASS_MASS_STORAGE, USB_SC_SCSI, 0xaa) },
 	{ }
 };
 MODULE_DEVICE_TABLE(usb, uas_usb_ids);
@@ -1009,9 +862,6 @@ static int uas_configure_endpoints(struct uas_dev_info *devinfo)
 	struct usb_device *udev = devinfo->udev;
 	int r;
 
-	devinfo->uas_sense_old = 0;
-	devinfo->cmnd = NULL;
-
 	r = uas_find_endpoints(devinfo->intf->cur_altsetting, eps);
 	if (r)
 		return r;
@@ -1025,12 +875,12 @@ static int uas_configure_endpoints(struct uas_dev_info *devinfo)
 	devinfo->data_out_pipe = usb_sndbulkpipe(udev,
 					    usb_endpoint_num(&eps[3]->desc));
 
-	if (udev->speed != USB_SPEED_SUPER) {
+	if (udev->speed < USB_SPEED_SUPER) {
 		devinfo->qdepth = 32;
 		devinfo->use_streams = 0;
 	} else {
 		devinfo->qdepth = usb_alloc_streams(devinfo->intf, eps + 1,
-						    3, 256, GFP_NOIO);
+						    3, MAX_CMNDS, GFP_NOIO);
 		if (devinfo->qdepth < 0)
 			return devinfo->qdepth;
 		devinfo->use_streams = 1;
@@ -1078,15 +928,14 @@ static int uas_probe(struct usb_interface *intf, const struct usb_device_id *id)
 	devinfo->intf = intf;
 	devinfo->udev = udev;
 	devinfo->resetting = 0;
-	devinfo->running_task = 0;
 	devinfo->shutdown = 0;
+	devinfo->flags = id->driver_info;
+	usb_stor_adjust_quirks(udev, &devinfo->flags);
 	init_usb_anchor(&devinfo->cmd_urbs);
 	init_usb_anchor(&devinfo->sense_urbs);
 	init_usb_anchor(&devinfo->data_urbs);
 	spin_lock_init(&devinfo->lock);
 	INIT_WORK(&devinfo->work, uas_do_work);
-	INIT_LIST_HEAD(&devinfo->inflight_list);
-	INIT_LIST_HEAD(&devinfo->dead_list);
 
 	result = uas_configure_endpoints(devinfo);
 	if (result)
@@ -1114,6 +963,54 @@ set_alt0:
 	return result;
 }
 
+static int uas_cmnd_list_empty(struct uas_dev_info *devinfo)
+{
+	unsigned long flags;
+	int i, r = 1;
+
+	spin_lock_irqsave(&devinfo->lock, flags);
+
+	for (i = 0; i < devinfo->qdepth; i++) {
+		if (devinfo->cmnd[i]) {
+			r = 0; /* Not empty */
+			break;
+		}
+	}
+
+	spin_unlock_irqrestore(&devinfo->lock, flags);
+
+	return r;
+}
+
+/*
+ * Wait for any pending cmnds to complete, on usb-2 sense_urbs may temporarily
+ * get empty while there still is more work to do due to sense-urbs completing
+ * with a READ/WRITE_READY iu code, so keep waiting until the list gets empty.
+ */
+static int uas_wait_for_pending_cmnds(struct uas_dev_info *devinfo)
+{
+	unsigned long start_time;
+	int r;
+
+	start_time = jiffies;
+	do {
+		flush_work(&devinfo->work);
+
+		r = usb_wait_anchor_empty_timeout(&devinfo->sense_urbs, 5000);
+		if (r == 0)
+			return -ETIME;
+
+		r = usb_wait_anchor_empty_timeout(&devinfo->data_urbs, 500);
+		if (r == 0)
+			return -ETIME;
+
+		if (time_after(jiffies, start_time + 5 * HZ))
+			return -ETIME;
+	} while (!uas_cmnd_list_empty(devinfo));
+
+	return 0;
+}
+
 static int uas_pre_reset(struct usb_interface *intf)
 {
 	struct Scsi_Host *shost = usb_get_intfdata(intf);
@@ -1128,10 +1025,9 @@ static int uas_pre_reset(struct usb_interface *intf)
 	scsi_block_requests(shost);
 	spin_unlock_irqrestore(shost->host_lock, flags);
 
-	/* Wait for any pending requests to complete */
-	flush_work(&devinfo->work);
-	if (usb_wait_anchor_empty_timeout(&devinfo->sense_urbs, 5000) == 0) {
+	if (uas_wait_for_pending_cmnds(devinfo) != 0) {
 		shost_printk(KERN_ERR, shost, "%s: timed out\n", __func__);
+		scsi_unblock_requests(shost);
 		return 1;
 	}
 
@@ -1145,13 +1041,16 @@ static int uas_post_reset(struct usb_interface *intf)
 	struct Scsi_Host *shost = usb_get_intfdata(intf);
 	struct uas_dev_info *devinfo = (struct uas_dev_info *)shost->hostdata;
 	unsigned long flags;
+	int err;
 
 	if (devinfo->shutdown)
 		return 0;
 
-	if (uas_configure_endpoints(devinfo) != 0) {
+	err = uas_configure_endpoints(devinfo);
+	if (err) {
 		shost_printk(KERN_ERR, shost,
-			     "%s: alloc streams error after reset", __func__);
+			     "%s: alloc streams error %d after reset",
+			     __func__, err);
 		return 1;
 	}
 
@@ -1169,9 +1068,7 @@ static int uas_suspend(struct usb_interface *intf, pm_message_t message)
 	struct Scsi_Host *shost = usb_get_intfdata(intf);
 	struct uas_dev_info *devinfo = (struct uas_dev_info *)shost->hostdata;
 
-	/* Wait for any pending requests to complete */
-	flush_work(&devinfo->work);
-	if (usb_wait_anchor_empty_timeout(&devinfo->sense_urbs, 5000) == 0) {
+	if (uas_wait_for_pending_cmnds(devinfo) != 0) {
 		shost_printk(KERN_ERR, shost, "%s: timed out\n", __func__);
 		return -ETIME;
 	}
@@ -1189,10 +1086,13 @@ static int uas_reset_resume(struct usb_interface *intf)
 	struct Scsi_Host *shost = usb_get_intfdata(intf);
 	struct uas_dev_info *devinfo = (struct uas_dev_info *)shost->hostdata;
 	unsigned long flags;
+	int err;
 
-	if (uas_configure_endpoints(devinfo) != 0) {
+	err = uas_configure_endpoints(devinfo);
+	if (err) {
 		shost_printk(KERN_ERR, shost,
-			     "%s: alloc streams error after reset", __func__);
+			     "%s: alloc streams error %d after reset",
+			     __func__, err);
 		return -EIO;
 	}
 
@@ -1207,14 +1107,18 @@ static void uas_disconnect(struct usb_interface *intf)
 {
 	struct Scsi_Host *shost = usb_get_intfdata(intf);
 	struct uas_dev_info *devinfo = (struct uas_dev_info *)shost->hostdata;
+	unsigned long flags;
 
+	spin_lock_irqsave(&devinfo->lock, flags);
 	devinfo->resetting = 1;
+	spin_unlock_irqrestore(&devinfo->lock, flags);
+
 	cancel_work_sync(&devinfo->work);
-	uas_abort_inflight(devinfo, DID_NO_CONNECT, __func__);
 	usb_kill_anchored_urbs(&devinfo->cmd_urbs);
 	usb_kill_anchored_urbs(&devinfo->sense_urbs);
 	usb_kill_anchored_urbs(&devinfo->data_urbs);
-	uas_zap_dead(devinfo);
+	uas_zap_pending(devinfo, DID_NO_CONNECT);
+
 	scsi_remove_host(shost);
 	uas_free_streams(devinfo);
 	scsi_host_put(shost);
diff --git a/drivers/usb/storage/unusual_devs.h b/drivers/usb/storage/unusual_devs.h
index 80a5b366255f..11c7a9676441 100644
--- a/drivers/usb/storage/unusual_devs.h
+++ b/drivers/usb/storage/unusual_devs.h
@@ -101,6 +101,12 @@ UNUSUAL_DEV(  0x03f0, 0x4002, 0x0001, 0x0001,
 		"PhotoSmart R707",
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL, US_FL_FIX_CAPACITY),
 
+UNUSUAL_DEV(  0x03f3, 0x0001, 0x0000, 0x9999,
+		"Adaptec",
+		"USBConnect 2000",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 /* Reported by Sebastian Kapfer <sebastian_kapfer@gmx.net>
  * and Olaf Hering <olh@suse.de> (different bcd's, same vendor/product)
  * for USB floppies that need the SINGLE_LUN enforcement.
@@ -486,18 +492,24 @@ UNUSUAL_DEV(  0x04e6, 0x000a, 0x0200, 0x0200,
 		"eUSB CompactFlash Adapter",
 		USB_SC_8020, USB_PR_CB, NULL, 0),
 
-UNUSUAL_DEV(  0x04e6, 0x000B, 0x0100, 0x0100,
+UNUSUAL_DEV(  0x04e6, 0x000b, 0x0100, 0x0100,
 		"Shuttle",
 		"eUSCSI Bridge",
 		USB_SC_SCSI, USB_PR_BULK, usb_stor_euscsi_init,
 		US_FL_SCM_MULT_TARG ), 
 
-UNUSUAL_DEV(  0x04e6, 0x000C, 0x0100, 0x0100,
+UNUSUAL_DEV(  0x04e6, 0x000c, 0x0100, 0x0100,
 		"Shuttle",
 		"eUSCSI Bridge",
 		USB_SC_SCSI, USB_PR_BULK, usb_stor_euscsi_init,
 		US_FL_SCM_MULT_TARG ),
 
+UNUSUAL_DEV(  0x04e6, 0x000f, 0x0000, 0x9999,
+		"SCM Microsystems",
+		"eUSB SCSI Adapter (Bus Powered)",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 UNUSUAL_DEV(  0x04e6, 0x0101, 0x0200, 0x0200,
 		"Shuttle",
 		"CD-RW Device",
@@ -741,6 +753,12 @@ UNUSUAL_DEV(  0x059b, 0x0001, 0x0100, 0x0100,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_SINGLE_LUN ),
 
+UNUSUAL_DEV(  0x059b, 0x0040, 0x0100, 0x0100,
+		"Iomega",
+		"Jaz USB Adapter",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_SINGLE_LUN ),
+
 /* Reported by <Hendryk.Pfeiffer@gmx.de> */
 UNUSUAL_DEV(  0x059f, 0x0643, 0x0000, 0x0000,
 		"LaCie",
@@ -922,6 +940,12 @@ UNUSUAL_DEV(  0x069b, 0x3004, 0x0001, 0x0001,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_FIX_CAPACITY ),
 
+UNUSUAL_DEV(  0x06ca, 0x2003, 0x0100, 0x0100,
+		"Newer Technology",
+		"uSCSI",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 /* Reported by Adrian Pilchowiec <adi1981@epf.pl> */
 UNUSUAL_DEV(  0x071b, 0x3203, 0x0000, 0x0000,
 		"RockChip",
@@ -1081,6 +1105,13 @@ UNUSUAL_DEV( 0x0840, 0x0085, 0x0001, 0x0001,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_FIX_CAPACITY),
 
+/* Supplied with some Castlewood ORB removable drives */
+UNUSUAL_DEV(  0x084b, 0xa001, 0x0000, 0x9999,
+		"Castlewood Systems",
+		"USB to SCSI cable",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 /* Entry and supporting patch by Theodore Kilgore <kilgota@auburn.edu>.
  * Flag will support Bulk devices which use a standards-violating 32-byte
  * Command Block Wrapper. Here, the "DC2MEGA" cameras (several brands) with
@@ -1113,6 +1144,18 @@ UNUSUAL_DEV(  0x0851, 0x1543, 0x0200, 0x0200,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_NOT_LOCKABLE),
 
+UNUSUAL_DEV(  0x085a, 0x0026, 0x0100, 0x0133,
+		"Xircom",
+		"PortGear USB-SCSI (Mac USB Dock)",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
+UNUSUAL_DEV(  0x085a, 0x0028, 0x0100, 0x0133,
+		"Xircom",
+		"PortGear USB to SCSI Converter",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 /* Submitted by Jan De Luyck <lkml@kcore.org> */
 UNUSUAL_DEV(  0x08bd, 0x1100, 0x0000, 0x0000,
 		"CITIZEN",
@@ -1952,6 +1995,14 @@ UNUSUAL_DEV(  0x152d, 0x2329, 0x0100, 0x0100,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_IGNORE_RESIDUE | US_FL_SANE_SENSE ),
 
+/* Entrega Technologies U1-SC25 (later Xircom PortGear PGSCSI)
+ * and Mac USB Dock USB-SCSI */
+UNUSUAL_DEV(  0x1645, 0x0007, 0x0100, 0x0133,
+		"Entrega Technologies",
+		"USB to SCSI Converter",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 /* Reported by Robert Schedel <r.schedel@yahoo.de>
  * Note: this is a 'super top' device like the above 14cd/6600 device */
 UNUSUAL_DEV(  0x1652, 0x6600, 0x0201, 0x0201,
@@ -1974,6 +2025,12 @@ UNUSUAL_DEV(  0x177f, 0x0400, 0x0000, 0x0000,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_BULK_IGNORE_TAG | US_FL_MAX_SECTORS_64 ),
 
+UNUSUAL_DEV(  0x1822, 0x0001, 0x0000, 0x9999,
+		"Ariston Technologies",
+		"iConnect USB to SCSI adapter",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 /* Reported by Hans de Goede <hdegoede@redhat.com>
  * These Appotech controllers are found in Picture Frames, they provide a
  * (buggy) emulation of a cdrom drive which contains the windows software
@@ -2019,6 +2076,13 @@ UNUSUAL_DEV( 0x1e74, 0x4621, 0x0000, 0x0000,
 		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
 		US_FL_BULK_IGNORE_TAG | US_FL_MAX_SECTORS_64 ),
 
+/* Supplied with some Castlewood ORB removable drives */
+UNUSUAL_DEV(  0x2027, 0xa001, 0x0000, 0x9999,
+		"Double-H Technology",
+		"USB to SCSI Intelligent Cable",
+		USB_SC_DEVICE, USB_PR_DEVICE, usb_stor_euscsi_init,
+		US_FL_SCM_MULT_TARG ),
+
 UNUSUAL_DEV( 0x2116, 0x0320, 0x0001, 0x0001,
 		"ST",
 		"2A",
diff --git a/drivers/usb/storage/unusual_uas.h b/drivers/usb/storage/unusual_uas.h
index 7244444df8ee..8511b54a65d9 100644
--- a/drivers/usb/storage/unusual_uas.h
+++ b/drivers/usb/storage/unusual_uas.h
@@ -40,13 +40,38 @@
  * and don't forget to CC: the USB development list <linux-usb@vger.kernel.org>
  */
 
-/*
- * This is an example entry for the US_FL_IGNORE_UAS flag. Once we have an
- * actual entry using US_FL_IGNORE_UAS this entry should be removed.
- *
- * UNUSUAL_DEV(  0xabcd, 0x1234, 0x0100, 0x0100,
- *		"Example",
- *		"Storage with broken UAS",
- *		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
- *		US_FL_IGNORE_UAS),
- */
+/* https://bugzilla.kernel.org/show_bug.cgi?id=79511 */
+UNUSUAL_DEV(0x0bc2, 0x2312, 0x0000, 0x9999,
+		"Seagate",
+		"Expansion Desk",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_NO_ATA_1X),
+
+/* https://bbs.archlinux.org/viewtopic.php?id=183190 */
+UNUSUAL_DEV(0x0bc2, 0x3312, 0x0000, 0x9999,
+		"Seagate",
+		"Expansion Desk",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_NO_ATA_1X),
+
+/* https://bbs.archlinux.org/viewtopic.php?id=183190 */
+UNUSUAL_DEV(0x0bc2, 0xab20, 0x0000, 0x9999,
+		"Seagate",
+		"Backup+ BK",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_NO_ATA_1X),
+
+/* Reported-by: Claudio Bizzarri <claudio.bizzarri@gmail.com> */
+UNUSUAL_DEV(0x152d, 0x0567, 0x0000, 0x9999,
+		"JMicron",
+		"JMS567",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_NO_REPORT_OPCODES),
+
+/* Most ASM1051 based devices have issues with uas, blacklist them all */
+/* Reported-by: Hans de Goede <hdegoede@redhat.com> */
+UNUSUAL_DEV(0x174c, 0x5106, 0x0000, 0x9999,
+		"ASMedia",
+		"ASM1051",
+		USB_SC_DEVICE, USB_PR_DEVICE, NULL,
+		US_FL_IGNORE_UAS),
diff --git a/drivers/usb/storage/usb.c b/drivers/usb/storage/usb.c
index cedb29252a92..9d66ce62542e 100644
--- a/drivers/usb/storage/usb.c
+++ b/drivers/usb/storage/usb.c
@@ -98,7 +98,7 @@ MODULE_PARM_DESC(quirks, "supplemental list of device IDs and their quirks");
 /* The vendor name should be kept at eight characters or less, and
  * the product name should be kept at 16 characters or less. If a device
  * has the US_FL_FIX_INQUIRY flag, then the vendor and product names
- * normally generated by a device thorugh the INQUIRY response will be
+ * normally generated by a device through the INQUIRY response will be
  * taken from this list, and this is the reason for the above size
  * restriction. However, if the flag is not present, then you
  * are free to use as many characters as you like.
@@ -478,7 +478,8 @@ void usb_stor_adjust_quirks(struct usb_device *udev, unsigned long *fflags)
 			US_FL_CAPACITY_OK | US_FL_IGNORE_RESIDUE |
 			US_FL_SINGLE_LUN | US_FL_NO_WP_DETECT |
 			US_FL_NO_READ_DISC_INFO | US_FL_NO_READ_CAPACITY_16 |
-			US_FL_INITIAL_READ10 | US_FL_WRITE_CACHE);
+			US_FL_INITIAL_READ10 | US_FL_WRITE_CACHE |
+			US_FL_NO_ATA_1X | US_FL_NO_REPORT_OPCODES);
 
 	p = quirks;
 	while (*p) {
@@ -516,6 +517,9 @@ void usb_stor_adjust_quirks(struct usb_device *udev, unsigned long *fflags)
 		case 'e':
 			f |= US_FL_NO_READ_CAPACITY_16;
 			break;
+		case 'f':
+			f |= US_FL_NO_REPORT_OPCODES;
+			break;
 		case 'h':
 			f |= US_FL_CAPACITY_HEURISTICS;
 			break;
@@ -543,6 +547,9 @@ void usb_stor_adjust_quirks(struct usb_device *udev, unsigned long *fflags)
 		case 's':
 			f |= US_FL_SINGLE_LUN;
 			break;
+		case 't':
+			f |= US_FL_NO_ATA_1X;
+			break;
 		case 'u':
 			f |= US_FL_IGNORE_UAS;
 			break;
@@ -983,6 +990,14 @@ int usb_stor_probe2(struct us_data *us)
 	if (!(us->fflags & US_FL_SCM_MULT_TARG))
 		us_to_host(us)->max_id = 1;
 
+	/*
+	 * Like Windows, we won't store the LUN bits in CDB[1] for SCSI-2
+	 * devices using the Bulk-Only transport (even though this violates
+	 * the SCSI spec).
+	 */
+	if (us->transport == usb_stor_Bulk_transport)
+		us_to_host(us)->no_scsi2_lun_in_cdb = 1;
+
 	/* Find the endpoints and calculate pipe values */
 	result = get_pipes(us);
 	if (result)
diff --git a/drivers/staging/usbip/Kconfig b/drivers/usb/usbip/Kconfig
index bd99e9e47e50..bd99e9e47e50 100644
--- a/drivers/staging/usbip/Kconfig
+++ b/drivers/usb/usbip/Kconfig
diff --git a/drivers/staging/usbip/Makefile b/drivers/usb/usbip/Makefile
index 9ecd61545be1..9ecd61545be1 100644
--- a/drivers/staging/usbip/Makefile
+++ b/drivers/usb/usbip/Makefile
diff --git a/drivers/staging/usbip/README b/drivers/usb/usbip/README
index 41a2cf2e77a6..41a2cf2e77a6 100644
--- a/drivers/staging/usbip/README
+++ b/drivers/usb/usbip/README
diff --git a/drivers/staging/usbip/stub.h b/drivers/usb/usbip/stub.h
index 266e2b0ce9a8..266e2b0ce9a8 100644
--- a/drivers/staging/usbip/stub.h
+++ b/drivers/usb/usbip/stub.h
diff --git a/drivers/staging/usbip/stub_dev.c b/drivers/usb/usbip/stub_dev.c
index 51d0c7188738..fac20e0434c0 100644
--- a/drivers/staging/usbip/stub_dev.c
+++ b/drivers/usb/usbip/stub_dev.c
@@ -26,33 +26,6 @@
 #include "stub.h"
 
 /*
- * Define device IDs here if you want to explicitly limit exportable devices.
- * In most cases, wildcard matching will be okay because driver binding can be
- * changed dynamically by a userland program.
- */
-static struct usb_device_id stub_table[] = {
-#if 0
-	/* just an example */
-	{ USB_DEVICE(0x05ac, 0x0301) },   /* Mac 1 button mouse */
-	{ USB_DEVICE(0x0430, 0x0009) },   /* Plat Home Keyboard */
-	{ USB_DEVICE(0x059b, 0x0001) },   /* Iomega USB Zip 100 */
-	{ USB_DEVICE(0x04b3, 0x4427) },   /* IBM USB CD-ROM */
-	{ USB_DEVICE(0x05a9, 0xa511) },   /* LifeView USB cam */
-	{ USB_DEVICE(0x55aa, 0x0201) },   /* Imation card reader */
-	{ USB_DEVICE(0x046d, 0x0870) },   /* Qcam Express(QV-30) */
-	{ USB_DEVICE(0x04bb, 0x0101) },   /* IO-DATA HD 120GB */
-	{ USB_DEVICE(0x04bb, 0x0904) },   /* IO-DATA USB-ET/TX */
-	{ USB_DEVICE(0x04bb, 0x0201) },   /* IO-DATA USB-ET/TX */
-	{ USB_DEVICE(0x08bb, 0x2702) },   /* ONKYO USB Speaker */
-	{ USB_DEVICE(0x046d, 0x08b2) },   /* Logicool Qcam 4000 Pro */
-#endif
-	/* magic for wild card */
-	{ .driver_info = 1 },
-	{ 0, }                                     /* Terminating entry */
-};
-MODULE_DEVICE_TABLE(usb, stub_table);
-
-/*
  * usbip_status shows the status of usbip-host as long as this driver is bound
  * to the target device.
  */
diff --git a/drivers/staging/usbip/stub_main.c b/drivers/usb/usbip/stub_main.c
index 44ab43fc4fcc..44ab43fc4fcc 100644
--- a/drivers/staging/usbip/stub_main.c
+++ b/drivers/usb/usbip/stub_main.c
diff --git a/drivers/staging/usbip/stub_rx.c b/drivers/usb/usbip/stub_rx.c
index 00e475c51a12..00e475c51a12 100644
--- a/drivers/staging/usbip/stub_rx.c
+++ b/drivers/usb/usbip/stub_rx.c
diff --git a/drivers/staging/usbip/stub_tx.c b/drivers/usb/usbip/stub_tx.c
index dbcabc9dbe0d..dbcabc9dbe0d 100644
--- a/drivers/staging/usbip/stub_tx.c
+++ b/drivers/usb/usbip/stub_tx.c
diff --git a/drivers/staging/usbip/usbip_common.c b/drivers/usb/usbip/usbip_common.c
index facaaf003f19..facaaf003f19 100644
--- a/drivers/staging/usbip/usbip_common.c
+++ b/drivers/usb/usbip/usbip_common.c
diff --git a/drivers/staging/usbip/usbip_common.h b/drivers/usb/usbip/usbip_common.h
index 4da3866a037d..86b08475c254 100644
--- a/drivers/staging/usbip/usbip_common.h
+++ b/drivers/usb/usbip/usbip_common.h
@@ -29,7 +29,7 @@
 #include <linux/types.h>
 #include <linux/usb.h>
 #include <linux/wait.h>
-#include "uapi/usbip.h"
+#include <uapi/linux/usbip.h>
 
 #define USBIP_VERSION "1.0.0"
 
diff --git a/drivers/staging/usbip/usbip_event.c b/drivers/usb/usbip/usbip_event.c
index 64933b993d7a..64933b993d7a 100644
--- a/drivers/staging/usbip/usbip_event.c
+++ b/drivers/usb/usbip/usbip_event.c
diff --git a/drivers/staging/usbip/usbip_protocol.txt b/drivers/usb/usbip/usbip_protocol.txt
index 16b6fe27284c..16b6fe27284c 100644
--- a/drivers/staging/usbip/usbip_protocol.txt
+++ b/drivers/usb/usbip/usbip_protocol.txt
diff --git a/drivers/staging/usbip/vhci.h b/drivers/usb/usbip/vhci.h
index a863a98a91ce..a863a98a91ce 100644
--- a/drivers/staging/usbip/vhci.h
+++ b/drivers/usb/usbip/vhci.h
diff --git a/drivers/staging/usbip/vhci_hcd.c b/drivers/usb/usbip/vhci_hcd.c
index c02374b6049c..c02374b6049c 100644
--- a/drivers/staging/usbip/vhci_hcd.c
+++ b/drivers/usb/usbip/vhci_hcd.c
diff --git a/drivers/staging/usbip/vhci_rx.c b/drivers/usb/usbip/vhci_rx.c
index 00e4a54308e4..00e4a54308e4 100644
--- a/drivers/staging/usbip/vhci_rx.c
+++ b/drivers/usb/usbip/vhci_rx.c
diff --git a/drivers/staging/usbip/vhci_sysfs.c b/drivers/usb/usbip/vhci_sysfs.c
index 211f43f67ea2..211f43f67ea2 100644
--- a/drivers/staging/usbip/vhci_sysfs.c
+++ b/drivers/usb/usbip/vhci_sysfs.c
diff --git a/drivers/staging/usbip/vhci_tx.c b/drivers/usb/usbip/vhci_tx.c
index 409fd99f3257..409fd99f3257 100644
--- a/drivers/staging/usbip/vhci_tx.c
+++ b/drivers/usb/usbip/vhci_tx.c
diff --git a/drivers/usb/wusbcore/Kconfig b/drivers/usb/wusbcore/Kconfig
index 0e17b966e1b4..348de1d6726e 100644
--- a/drivers/usb/wusbcore/Kconfig
+++ b/drivers/usb/wusbcore/Kconfig
@@ -3,7 +3,6 @@
 #
 config USB_WUSB
 	tristate "Enable Wireless USB extensions"
-	depends on PCI
 	depends on UWB
         select CRYPTO
         select CRYPTO_BLKCIPHER
@@ -18,6 +17,7 @@ config USB_WUSB
 
 config USB_WUSB_CBAF
 	tristate "Support WUSB Cable Based Association (CBA)"
+	depends on USB
 	help
 	  Some WUSB devices support Cable Based Association. It's used to
 	  enable the secure communication between the host and the
diff --git a/drivers/usb/wusbcore/crypto.c b/drivers/usb/wusbcore/crypto.c
index 9a95b2dc6d1b..50ce80d604f3 100644
--- a/drivers/usb/wusbcore/crypto.c
+++ b/drivers/usb/wusbcore/crypto.c
@@ -222,8 +222,6 @@ static int wusb_ccm_mac(struct crypto_blkcipher *tfm_cbc,
 	WARN_ON(sizeof(ax) != sizeof(struct aes_ccm_block));
 
 	result = -ENOMEM;
-	zero_padding = sizeof(struct aes_ccm_block)
-		- blen % sizeof(struct aes_ccm_block);
 	zero_padding = blen % sizeof(struct aes_ccm_block);
 	if (zero_padding)
 		zero_padding = sizeof(struct aes_ccm_block) - zero_padding;
diff --git a/drivers/usb/wusbcore/devconnect.c b/drivers/usb/wusbcore/devconnect.c
index 0677139c6065..3f4f5fbded55 100644
--- a/drivers/usb/wusbcore/devconnect.c
+++ b/drivers/usb/wusbcore/devconnect.c
@@ -329,7 +329,7 @@ void wusbhc_devconnect_ack(struct wusbhc *wusbhc, struct wusb_dn_connect *dnc,
 	port->wusb_dev = wusb_dev;
 	port->status |= USB_PORT_STAT_CONNECTION;
 	port->change |= USB_PORT_STAT_C_CONNECTION;
-	/* Now the port status changed to connected; khubd will
+	/* Now the port status changed to connected; hub_wq will
 	 * pick the change up and try to reset the port to bring it to
 	 * the enabled state--so this process returns up to the stack
 	 * and it calls back into wusbhc_rh_port_reset().
@@ -343,7 +343,7 @@ error_unlock:
 /*
  * Disconnect a Wireless USB device from its fake port
  *
- * Marks the port as disconnected so that khubd can pick up the change
+ * Marks the port as disconnected so that hub_wq can pick up the change
  * and drops our knowledge about the device.
  *
  * Assumes there is a device connected
@@ -379,7 +379,7 @@ static void __wusbhc_dev_disconnect(struct wusbhc *wusbhc,
 		wusbhc_gtk_rekey(wusbhc);
 
 	/* The Wireless USB part has forgotten about the device already; now
-	 * khubd's timer will pick up the disconnection and remove the USB
+	 * hub_wq's timer will pick up the disconnection and remove the USB
 	 * device from the system
 	 */
 }
diff --git a/drivers/usb/wusbcore/security.c b/drivers/usb/wusbcore/security.c
index 95be9953cd47..cc74d669c802 100644
--- a/drivers/usb/wusbcore/security.c
+++ b/drivers/usb/wusbcore/security.c
@@ -33,6 +33,20 @@ static void wusbhc_gtk_rekey_work(struct work_struct *work);
 
 int wusbhc_sec_create(struct wusbhc *wusbhc)
 {
+	/*
+	 * WQ is singlethread because we need to serialize rekey operations.
+	 * Use a separate workqueue for security operations instead of the
+	 * wusbd workqueue because security operations may need to communicate
+	 * directly with downstream wireless devices using synchronous URBs.
+	 * If a device is not responding, this could block other host
+	 * controller operations.
+	 */
+	wusbhc->wq_security = create_singlethread_workqueue("wusbd_security");
+	if (wusbhc->wq_security == NULL) {
+		pr_err("WUSB-core: Cannot create wusbd_security workqueue\n");
+		return -ENOMEM;
+	}
+
 	wusbhc->gtk.descr.bLength = sizeof(wusbhc->gtk.descr) +
 		sizeof(wusbhc->gtk.data);
 	wusbhc->gtk.descr.bDescriptorType = USB_DT_KEY;
@@ -48,6 +62,7 @@ int wusbhc_sec_create(struct wusbhc *wusbhc)
 /* Called when the HC is destroyed */
 void wusbhc_sec_destroy(struct wusbhc *wusbhc)
 {
+	destroy_workqueue(wusbhc->wq_security);
 }
 
 
@@ -596,5 +611,5 @@ void wusbhc_gtk_rekey(struct wusbhc *wusbhc)
 	 * and will cause a deadlock.  Instead, queue a work item to do
 	 * it when the lock is not held
 	 */
-	queue_work(wusbd, &wusbhc->gtk_rekey_work);
+	queue_work(wusbhc->wq_security, &wusbhc->gtk_rekey_work);
 }
diff --git a/drivers/usb/wusbcore/wa-hc.h b/drivers/usb/wusbcore/wa-hc.h
index f2a8d29e17b9..edc7267157f3 100644
--- a/drivers/usb/wusbcore/wa-hc.h
+++ b/drivers/usb/wusbcore/wa-hc.h
@@ -64,7 +64,7 @@
  *
  * Note much of the activity is difficult to follow. For example a
  * device connect goes to devconnect, which will cause the "fake" root
- * hub port to show a connect and stop there. Then khubd will notice
+ * hub port to show a connect and stop there. Then hub_wq will notice
  * and call into the rh.c:hwahc_rc_port_reset() code to authenticate
  * the device (and this might require user intervention) and enable
  * the port.
diff --git a/drivers/usb/wusbcore/wa-xfer.c b/drivers/usb/wusbcore/wa-xfer.c
index 3e2e4ed20157..69af4fd9e072 100644
--- a/drivers/usb/wusbcore/wa-xfer.c
+++ b/drivers/usb/wusbcore/wa-xfer.c
@@ -459,14 +459,25 @@ static void __wa_xfer_abort_cb(struct urb *urb)
 			__func__, urb->status);
 		if (xfer) {
 			unsigned long flags;
-			int done;
+			int done, seg_index = 0;
 			struct wa_rpipe *rpipe = xfer->ep->hcpriv;
 
 			dev_err(dev, "%s: cleaning up xfer %p ID 0x%08X.\n",
 				__func__, xfer, wa_xfer_id(xfer));
 			spin_lock_irqsave(&xfer->lock, flags);
-			/* mark all segs as aborted. */
-			wa_complete_remaining_xfer_segs(xfer, 0,
+			/* skip done segs. */
+			while (seg_index < xfer->segs) {
+				struct wa_seg *seg = xfer->seg[seg_index];
+
+				if ((seg->status == WA_SEG_DONE) ||
+					(seg->status == WA_SEG_ERROR)) {
+					++seg_index;
+				} else {
+					break;
+				}
+			}
+			/* mark remaining segs as aborted. */
+			wa_complete_remaining_xfer_segs(xfer, seg_index,
 				WA_SEG_ABORTED);
 			done = __wa_xfer_is_done(xfer);
 			spin_unlock_irqrestore(&xfer->lock, flags);
@@ -2602,6 +2613,7 @@ static void wa_buf_in_cb(struct urb *urb)
 	dev = &wa->usb_iface->dev;
 	--(wa->active_buf_in_urbs);
 	active_buf_in_urbs = wa->active_buf_in_urbs;
+	rpipe = xfer->ep->hcpriv;
 
 	if (usb_pipeisoc(xfer->urb->pipe)) {
 		struct usb_iso_packet_descriptor *iso_frame_desc =
@@ -2659,7 +2671,6 @@ static void wa_buf_in_cb(struct urb *urb)
 			  resubmit_dti = (isoc_data_frame_count ==
 							urb_frame_count);
 		} else if (active_buf_in_urbs == 0) {
-			rpipe = xfer->ep->hcpriv;
 			dev_dbg(dev,
 				"xfer %p 0x%08X#%u: data in done (%zu bytes)\n",
 				xfer, wa_xfer_id(xfer), seg->index,
@@ -2685,7 +2696,6 @@ static void wa_buf_in_cb(struct urb *urb)
 		 */
 		resubmit_dti = wa->dti_state != WA_DTI_TRANSFER_RESULT_PENDING;
 		spin_lock_irqsave(&xfer->lock, flags);
-		rpipe = xfer->ep->hcpriv;
 		if (printk_ratelimit())
 			dev_err(dev, "xfer %p 0x%08X#%u: data in error %d\n",
 				xfer, wa_xfer_id(xfer), seg->index,
diff --git a/drivers/usb/wusbcore/wusbhc.h b/drivers/usb/wusbcore/wusbhc.h
index 2384add45371..41838db7f85c 100644
--- a/drivers/usb/wusbcore/wusbhc.h
+++ b/drivers/usb/wusbcore/wusbhc.h
@@ -295,6 +295,9 @@ struct wusbhc {
 	} __attribute__((packed)) gtk;
 	u8 gtk_index;
 	u32 gtk_tkid;
+
+	/* workqueue for WUSB security related tasks. */
+	struct workqueue_struct *wq_security;
 	struct work_struct gtk_rekey_work;
 
 	struct usb_encryption_descriptor *ccm1_etd;
diff --git a/drivers/uwb/Kconfig b/drivers/uwb/Kconfig
index 2431eedbe6a5..c204094e1bb4 100644
--- a/drivers/uwb/Kconfig
+++ b/drivers/uwb/Kconfig
@@ -4,7 +4,6 @@
 
 menuconfig UWB
 	tristate "Ultra Wideband devices"
-	depends on PCI
 	default n
 	help
 	  UWB is a high-bandwidth, low-power, point-to-point radio
diff --git a/drivers/uwb/driver.c b/drivers/uwb/driver.c
index 3e5454aba5d4..776bcb3c2140 100644
--- a/drivers/uwb/driver.c
+++ b/drivers/uwb/driver.c
@@ -121,9 +121,19 @@ static int __init uwb_subsys_init(void)
 	result = class_register(&uwb_rc_class);
 	if (result < 0)
 		goto error_uwb_rc_class_register;
+
+	/* Register the UWB bus */
+	result = bus_register(&uwb_bus_type);
+	if (result) {
+		pr_err("%s - registering bus driver failed\n", __func__);
+		goto exit_bus;
+	}
+
 	uwb_dbg_init();
 	return 0;
 
+exit_bus:
+	class_unregister(&uwb_rc_class);
 error_uwb_rc_class_register:
 	uwb_est_destroy();
 error_est_init:
@@ -134,6 +144,7 @@ module_init(uwb_subsys_init);
 static void __exit uwb_subsys_exit(void)
 {
 	uwb_dbg_exit();
+	bus_unregister(&uwb_bus_type);
 	class_unregister(&uwb_rc_class);
 	uwb_est_destroy();
 	return;
diff --git a/drivers/uwb/lc-dev.c b/drivers/uwb/lc-dev.c
index 80079b8fed15..8c7cfab5cee3 100644
--- a/drivers/uwb/lc-dev.c
+++ b/drivers/uwb/lc-dev.c
@@ -255,6 +255,12 @@ static struct attribute *uwb_dev_attrs[] = {
 };
 ATTRIBUTE_GROUPS(uwb_dev);
 
+/* UWB bus type. */
+struct bus_type uwb_bus_type = {
+	.name =		"uwb",
+	.dev_groups =	uwb_dev_groups,
+};
+
 /**
  * Device SYSFS registration
  */
@@ -263,10 +269,6 @@ static int __uwb_dev_sys_add(struct uwb_dev *uwb_dev, struct device *parent_dev)
 	struct device *dev;
 
 	dev = &uwb_dev->dev;
-	/* Device sysfs files are only useful for neighbor devices not
-	   local radio controllers. */
-	if (&uwb_dev->rc->uwb_dev != uwb_dev)
-		dev->groups = uwb_dev_groups;
 	dev->parent = parent_dev;
 	dev_set_drvdata(dev, uwb_dev);
 
@@ -365,8 +367,8 @@ int __uwb_dev_offair(struct uwb_dev *uwb_dev, struct uwb_rc *rc)
 	uwb_dev_addr_print(devbuf, sizeof(devbuf), &uwb_dev->dev_addr);
 	dev_info(dev, "uwb device (mac %s dev %s) disconnected from %s %s\n",
 		 macbuf, devbuf,
-		 rc ? rc->uwb_dev.dev.parent->bus->name : "n/a",
-		 rc ? dev_name(rc->uwb_dev.dev.parent) : "");
+		 uwb_dev->dev.bus->name,
+		 rc ? dev_name(&(rc->uwb_dev.dev)) : "");
 	uwb_dev_rm(uwb_dev);
 	list_del(&uwb_dev->bce->node);
 	uwb_bce_put(uwb_dev->bce);
@@ -428,26 +430,32 @@ void uwbd_dev_onair(struct uwb_rc *rc, struct uwb_beca_e *bce)
 		return;
 	}
 	uwb_dev_init(uwb_dev);		/* This sets refcnt to one, we own it */
+	uwb_dev->dev.bus = &uwb_bus_type;
 	uwb_dev->mac_addr = *bce->mac_addr;
 	uwb_dev->dev_addr = bce->dev_addr;
 	dev_set_name(&uwb_dev->dev, "%s", macbuf);
+
+	/* plug the beacon cache */
+	bce->uwb_dev = uwb_dev;
+	uwb_dev->bce = bce;
+	uwb_bce_get(bce);		/* released in uwb_dev_sys_release() */
+
 	result = uwb_dev_add(uwb_dev, &rc->uwb_dev.dev, rc);
 	if (result < 0) {
 		dev_err(dev, "new device %s: cannot instantiate device\n",
 			macbuf);
 		goto error_dev_add;
 	}
-	/* plug the beacon cache */
-	bce->uwb_dev = uwb_dev;
-	uwb_dev->bce = bce;
-	uwb_bce_get(bce);		/* released in uwb_dev_sys_release() */
+
 	dev_info(dev, "uwb device (mac %s dev %s) connected to %s %s\n",
-		 macbuf, devbuf, rc->uwb_dev.dev.parent->bus->name,
-		 dev_name(rc->uwb_dev.dev.parent));
+		 macbuf, devbuf, uwb_dev->dev.bus->name,
+		 dev_name(&(rc->uwb_dev.dev)));
 	uwb_notify(rc, uwb_dev, UWB_NOTIF_ONAIR);
 	return;
 
 error_dev_add:
+	bce->uwb_dev = NULL;
+	uwb_bce_put(bce);
 	kfree(uwb_dev);
 	return;
 }
diff --git a/drivers/uwb/lc-rc.c b/drivers/uwb/lc-rc.c
index 3eca6ceb9844..d059ad4d0dbd 100644
--- a/drivers/uwb/lc-rc.c
+++ b/drivers/uwb/lc-rc.c
@@ -119,10 +119,109 @@ struct uwb_rc *uwb_rc_alloc(void)
 }
 EXPORT_SYMBOL_GPL(uwb_rc_alloc);
 
+/*
+ * Show the ASIE that is broadcast in the UWB beacon by this uwb_rc device.
+ */
+static ssize_t ASIE_show(struct device *dev,
+				struct device_attribute *attr, char *buf)
+{
+	struct uwb_dev *uwb_dev = to_uwb_dev(dev);
+	struct uwb_rc *rc = uwb_dev->rc;
+	struct uwb_ie_hdr *ie;
+	void *ptr;
+	size_t len;
+	int result = 0;
+
+	/* init empty buffer. */
+	result = scnprintf(buf, PAGE_SIZE, "\n");
+	mutex_lock(&rc->ies_mutex);
+	/* walk IEData looking for an ASIE. */
+	ptr = rc->ies->IEData;
+	len = le16_to_cpu(rc->ies->wIELength);
+	for (;;) {
+		ie = uwb_ie_next(&ptr, &len);
+		if (!ie)
+			break;
+		if (ie->element_id == UWB_APP_SPEC_IE) {
+			result = uwb_ie_dump_hex(ie,
+					ie->length + sizeof(struct uwb_ie_hdr),
+					buf, PAGE_SIZE);
+			break;
+		}
+	}
+	mutex_unlock(&rc->ies_mutex);
+
+	return result;
+}
+
+/*
+ * Update the ASIE that is broadcast in the UWB beacon by this uwb_rc device.
+ */
+static ssize_t ASIE_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t size)
+{
+	struct uwb_dev *uwb_dev = to_uwb_dev(dev);
+	struct uwb_rc *rc = uwb_dev->rc;
+	char ie_buf[255];
+	int result, ie_len = 0;
+	const char *cur_ptr = buf;
+	struct uwb_ie_hdr *ie;
+
+	/* empty string means clear the ASIE. */
+	if (strlen(buf) <= 1) {
+		uwb_rc_ie_rm(rc, UWB_APP_SPEC_IE);
+		return size;
+	}
+
+	/* if non-empty string, convert string of hex chars to binary. */
+	while (ie_len < sizeof(ie_buf)) {
+		int char_count;
+
+		if (sscanf(cur_ptr, " %02hhX %n",
+				&(ie_buf[ie_len]), &char_count) > 0) {
+			++ie_len;
+			/* skip chars read from cur_ptr. */
+			cur_ptr += char_count;
+		} else {
+			break;
+		}
+	}
+
+	/* validate IE length and type. */
+	if (ie_len < sizeof(struct uwb_ie_hdr)) {
+		dev_err(dev, "%s: Invalid ASIE size %d.\n", __func__, ie_len);
+		return -EINVAL;
+	}
+
+	ie = (struct uwb_ie_hdr *)ie_buf;
+	if (ie->element_id != UWB_APP_SPEC_IE) {
+		dev_err(dev, "%s: Invalid IE element type size = 0x%02X.\n",
+				__func__, ie->element_id);
+		return -EINVAL;
+	}
+
+	/* bounds check length field from user. */
+	if (ie->length > (ie_len - sizeof(struct uwb_ie_hdr)))
+		ie->length = ie_len - sizeof(struct uwb_ie_hdr);
+
+	/*
+	 * Valid ASIE received. Remove current ASIE then add the new one using
+	 * uwb_rc_ie_add.
+	 */
+	uwb_rc_ie_rm(rc, UWB_APP_SPEC_IE);
+
+	result = uwb_rc_ie_add(rc, ie, ie->length + sizeof(struct uwb_ie_hdr));
+
+	return result >= 0 ? size : result;
+}
+static DEVICE_ATTR_RW(ASIE);
+
 static struct attribute *rc_attrs[] = {
 		&dev_attr_mac_address.attr,
 		&dev_attr_scan.attr,
 		&dev_attr_beacon.attr,
+		&dev_attr_ASIE.attr,
 		NULL,
 };
 
diff --git a/drivers/uwb/uwb-internal.h b/drivers/uwb/uwb-internal.h
index 9a103b100f1e..353c0555a1f5 100644
--- a/drivers/uwb/uwb-internal.h
+++ b/drivers/uwb/uwb-internal.h
@@ -172,7 +172,8 @@ struct uwb_rsv_alloc_info {
 	int interval;
 };
 
-int uwb_rsv_find_best_allocation(struct uwb_rsv *rsv, struct uwb_mas_bm *available, 
+int uwb_rsv_find_best_allocation(struct uwb_rsv *rsv,
+				 struct uwb_mas_bm *available,
 				 struct uwb_mas_bm *result);
 void uwb_rsv_handle_drp_avail_change(struct uwb_rc *rc);
 /*
@@ -313,6 +314,7 @@ int uwb_radio_force_channel(struct uwb_rc *rc, int channel);
 
 /* -- UWB Sysfs representation */
 extern struct class uwb_rc_class;
+extern struct bus_type uwb_bus_type;
 extern struct device_attribute dev_attr_mac_address;
 extern struct device_attribute dev_attr_beacon;
 extern struct device_attribute dev_attr_scan;
diff --git a/drivers/vfio/pci/vfio_pci.c b/drivers/vfio/pci/vfio_pci.c
index f7825332a325..9558da3f06a0 100644
--- a/drivers/vfio/pci/vfio_pci.c
+++ b/drivers/vfio/pci/vfio_pci.c
@@ -876,15 +876,11 @@ static void vfio_pci_remove(struct pci_dev *pdev)
 {
 	struct vfio_pci_device *vdev;
 
-	mutex_lock(&driver_lock);
-
 	vdev = vfio_del_group_dev(&pdev->dev);
 	if (vdev) {
 		iommu_group_put(pdev->dev.iommu_group);
 		kfree(vdev);
 	}
-
-	mutex_unlock(&driver_lock);
 }
 
 static pci_ers_result_t vfio_pci_aer_err_detected(struct pci_dev *pdev,
@@ -927,108 +923,90 @@ static struct pci_driver vfio_pci_driver = {
 	.err_handler	= &vfio_err_handlers,
 };
 
-/*
- * Test whether a reset is necessary and possible.  We mark devices as
- * needs_reset when they are released, but don't have a function-local reset
- * available.  If any of these exist in the affected devices, we want to do
- * a bus/slot reset.  We also need all of the affected devices to be unused,
- * so we abort if any device has a non-zero refcnt.  driver_lock prevents a
- * device from being opened during the scan or unbound from vfio-pci.
- */
-static int vfio_pci_test_bus_reset(struct pci_dev *pdev, void *data)
-{
-	bool *needs_reset = data;
-	struct pci_driver *pci_drv = ACCESS_ONCE(pdev->driver);
-	int ret = -EBUSY;
-
-	if (pci_drv == &vfio_pci_driver) {
-		struct vfio_device *device;
-		struct vfio_pci_device *vdev;
-
-		device = vfio_device_get_from_dev(&pdev->dev);
-		if (!device)
-			return ret;
-
-		vdev = vfio_device_data(device);
-		if (vdev) {
-			if (vdev->needs_reset)
-				*needs_reset = true;
-
-			if (!vdev->refcnt)
-				ret = 0;
-		}
-
-		vfio_device_put(device);
-	}
-
-	/*
-	 * TODO: vfio-core considers groups to be viable even if some devices
-	 * are attached to known drivers, like pci-stub or pcieport.  We can't
-	 * freeze devices from being unbound to those drivers like we can
-	 * here though, so it would be racy to test for them.  We also can't
-	 * use device_lock() to prevent changes as that would interfere with
-	 * PCI-core taking device_lock during bus reset.  For now, we require
-	 * devices to be bound to vfio-pci to get a bus/slot reset on release.
-	 */
-
-	return ret;
-}
+struct vfio_devices {
+	struct vfio_device **devices;
+	int cur_index;
+	int max_index;
+};
 
-/* Clear needs_reset on all affected devices after successful bus/slot reset */
-static int vfio_pci_clear_needs_reset(struct pci_dev *pdev, void *data)
+static int vfio_pci_get_devs(struct pci_dev *pdev, void *data)
 {
+	struct vfio_devices *devs = data;
 	struct pci_driver *pci_drv = ACCESS_ONCE(pdev->driver);
 
-	if (pci_drv == &vfio_pci_driver) {
-		struct vfio_device *device;
-		struct vfio_pci_device *vdev;
+	if (pci_drv != &vfio_pci_driver)
+		return -EBUSY;
 
-		device = vfio_device_get_from_dev(&pdev->dev);
-		if (!device)
-			return 0;
+	if (devs->cur_index == devs->max_index)
+		return -ENOSPC;
 
-		vdev = vfio_device_data(device);
-		if (vdev)
-			vdev->needs_reset = false;
-
-		vfio_device_put(device);
-	}
+	devs->devices[devs->cur_index] = vfio_device_get_from_dev(&pdev->dev);
+	if (!devs->devices[devs->cur_index])
+		return -EINVAL;
 
+	devs->cur_index++;
 	return 0;
 }
 
 /*
  * Attempt to do a bus/slot reset if there are devices affected by a reset for
  * this device that are needs_reset and all of the affected devices are unused
- * (!refcnt).  Callers of this function are required to hold driver_lock such
- * that devices can not be unbound from vfio-pci or opened by a user while we
- * test for and perform a bus/slot reset.
+ * (!refcnt).  Callers are required to hold driver_lock when calling this to
+ * prevent device opens and concurrent bus reset attempts.  We prevent device
+ * unbinds by acquiring and holding a reference to the vfio_device.
+ *
+ * NB: vfio-core considers a group to be viable even if some devices are
+ * bound to drivers like pci-stub or pcieport.  Here we require all devices
+ * to be bound to vfio_pci since that's the only way we can be sure they
+ * stay put.
  */
 static void vfio_pci_try_bus_reset(struct vfio_pci_device *vdev)
 {
+	struct vfio_devices devs = { .cur_index = 0 };
+	int i = 0, ret = -EINVAL;
 	bool needs_reset = false, slot = false;
-	int ret;
+	struct vfio_pci_device *tmp;
 
 	if (!pci_probe_reset_slot(vdev->pdev->slot))
 		slot = true;
 	else if (pci_probe_reset_bus(vdev->pdev->bus))
 		return;
 
-	if (vfio_pci_for_each_slot_or_bus(vdev->pdev,
-					  vfio_pci_test_bus_reset,
-					  &needs_reset, slot) || !needs_reset)
+	if (vfio_pci_for_each_slot_or_bus(vdev->pdev, vfio_pci_count_devs,
+					  &i, slot) || !i)
 		return;
 
-	if (slot)
-		ret = pci_try_reset_slot(vdev->pdev->slot);
-	else
-		ret = pci_try_reset_bus(vdev->pdev->bus);
-
-	if (ret)
+	devs.max_index = i;
+	devs.devices = kcalloc(i, sizeof(struct vfio_device *), GFP_KERNEL);
+	if (!devs.devices)
 		return;
 
-	vfio_pci_for_each_slot_or_bus(vdev->pdev,
-				      vfio_pci_clear_needs_reset, NULL, slot);
+	if (vfio_pci_for_each_slot_or_bus(vdev->pdev,
+					  vfio_pci_get_devs, &devs, slot))
+		goto put_devs;
+
+	for (i = 0; i < devs.cur_index; i++) {
+		tmp = vfio_device_data(devs.devices[i]);
+		if (tmp->needs_reset)
+			needs_reset = true;
+		if (tmp->refcnt)
+			goto put_devs;
+	}
+
+	if (needs_reset)
+		ret = slot ? pci_try_reset_slot(vdev->pdev->slot) :
+			     pci_try_reset_bus(vdev->pdev->bus);
+
+put_devs:
+	for (i = 0; i < devs.cur_index; i++) {
+		if (!ret) {
+			tmp = vfio_device_data(devs.devices[i]);
+			tmp->needs_reset = false;
+		}
+		vfio_device_put(devs.devices[i]);
+	}
+
+	kfree(devs.devices);
 }
 
 static void __exit vfio_pci_cleanup(void)
diff --git a/drivers/vfio/pci/vfio_pci_config.c b/drivers/vfio/pci/vfio_pci_config.c
index e50790e91f76..1de3f94aa7de 100644
--- a/drivers/vfio/pci/vfio_pci_config.c
+++ b/drivers/vfio/pci/vfio_pci_config.c
@@ -727,7 +727,7 @@ static int __init init_pci_ext_cap_err_perm(struct perm_bits *perm)
 	p_setd(perm, 0, ALL_VIRT, NO_WRITE);
 
 	/* Writable bits mask */
-	mask =	PCI_ERR_UNC_TRAIN |		/* Training */
+	mask =	PCI_ERR_UNC_UND |		/* Undefined */
 		PCI_ERR_UNC_DLP |		/* Data Link Protocol */
 		PCI_ERR_UNC_SURPDN |		/* Surprise Down */
 		PCI_ERR_UNC_POISON_TLP |	/* Poisoned TLP */
diff --git a/drivers/vfio/pci/vfio_pci_intrs.c b/drivers/vfio/pci/vfio_pci_intrs.c
index 9dd49c9839ac..553212f037c3 100644
--- a/drivers/vfio/pci/vfio_pci_intrs.c
+++ b/drivers/vfio/pci/vfio_pci_intrs.c
@@ -16,6 +16,7 @@
 #include <linux/device.h>
 #include <linux/interrupt.h>
 #include <linux/eventfd.h>
+#include <linux/msi.h>
 #include <linux/pci.h>
 #include <linux/file.h>
 #include <linux/poll.h>
@@ -548,6 +549,20 @@ static int vfio_msi_set_vector_signal(struct vfio_pci_device *vdev,
 		return PTR_ERR(trigger);
 	}
 
+	/*
+	 * The MSIx vector table resides in device memory which may be cleared
+	 * via backdoor resets. We don't allow direct access to the vector
+	 * table so even if a userspace driver attempts to save/restore around
+	 * such a reset it would be unsuccessful. To avoid this, restore the
+	 * cached value of the message prior to enabling.
+	 */
+	if (msix) {
+		struct msi_msg msg;
+
+		get_cached_msi_msg(irq, &msg);
+		write_msi_msg(irq, &msg);
+	}
+
 	ret = request_irq(irq, vfio_msihandler, 0,
 			  vdev->ctx[vector].name, trigger);
 	if (ret) {
diff --git a/drivers/vfio/vfio_iommu_type1.c b/drivers/vfio/vfio_iommu_type1.c
index 0734fbe5b651..4a9d666f1e91 100644
--- a/drivers/vfio/vfio_iommu_type1.c
+++ b/drivers/vfio/vfio_iommu_type1.c
@@ -57,7 +57,8 @@ struct vfio_iommu {
 	struct list_head	domain_list;
 	struct mutex		lock;
 	struct rb_root		dma_list;
-	bool v2;
+	bool			v2;
+	bool			nesting;
 };
 
 struct vfio_domain {
@@ -705,6 +706,15 @@ static int vfio_iommu_type1_attach_group(void *iommu_data,
 		goto out_free;
 	}
 
+	if (iommu->nesting) {
+		int attr = 1;
+
+		ret = iommu_domain_set_attr(domain->domain, DOMAIN_ATTR_NESTING,
+					    &attr);
+		if (ret)
+			goto out_domain;
+	}
+
 	ret = iommu_attach_group(domain->domain, iommu_group);
 	if (ret)
 		goto out_domain;
@@ -713,14 +723,14 @@ static int vfio_iommu_type1_attach_group(void *iommu_data,
 	list_add(&group->next, &domain->group_list);
 
 	if (!allow_unsafe_interrupts &&
-	    !iommu_domain_has_cap(domain->domain, IOMMU_CAP_INTR_REMAP)) {
+	    !iommu_capable(bus, IOMMU_CAP_INTR_REMAP)) {
 		pr_warn("%s: No interrupt remapping support.  Use the module param \"allow_unsafe_interrupts\" to enable VFIO IOMMU support on this platform\n",
 		       __func__);
 		ret = -EPERM;
 		goto out_detach;
 	}
 
-	if (iommu_domain_has_cap(domain->domain, IOMMU_CAP_CACHE_COHERENCY))
+	if (iommu_capable(bus, IOMMU_CAP_CACHE_COHERENCY))
 		domain->prot |= IOMMU_CACHE;
 
 	/*
@@ -819,17 +829,26 @@ static void *vfio_iommu_type1_open(unsigned long arg)
 {
 	struct vfio_iommu *iommu;
 
-	if (arg != VFIO_TYPE1_IOMMU && arg != VFIO_TYPE1v2_IOMMU)
-		return ERR_PTR(-EINVAL);
-
 	iommu = kzalloc(sizeof(*iommu), GFP_KERNEL);
 	if (!iommu)
 		return ERR_PTR(-ENOMEM);
 
+	switch (arg) {
+	case VFIO_TYPE1_IOMMU:
+		break;
+	case VFIO_TYPE1_NESTING_IOMMU:
+		iommu->nesting = true;
+	case VFIO_TYPE1v2_IOMMU:
+		iommu->v2 = true;
+		break;
+	default:
+		kfree(iommu);
+		return ERR_PTR(-EINVAL);
+	}
+
 	INIT_LIST_HEAD(&iommu->domain_list);
 	iommu->dma_list = RB_ROOT;
 	mutex_init(&iommu->lock);
-	iommu->v2 = (arg == VFIO_TYPE1v2_IOMMU);
 
 	return iommu;
 }
@@ -885,6 +904,7 @@ static long vfio_iommu_type1_ioctl(void *iommu_data,
 		switch (arg) {
 		case VFIO_TYPE1_IOMMU:
 		case VFIO_TYPE1v2_IOMMU:
+		case VFIO_TYPE1_NESTING_IOMMU:
 			return 1;
 		case VFIO_DMA_CC_IOMMU:
 			if (!iommu)
diff --git a/drivers/vfio/vfio_spapr_eeh.c b/drivers/vfio/vfio_spapr_eeh.c
index 86dfceb9201f..5fa42db769ee 100644
--- a/drivers/vfio/vfio_spapr_eeh.c
+++ b/drivers/vfio/vfio_spapr_eeh.c
@@ -92,7 +92,7 @@ long vfio_spapr_iommu_eeh_ioctl(struct iommu_group *group,
 
 	return ret;
 }
-EXPORT_SYMBOL(vfio_spapr_iommu_eeh_ioctl);
+EXPORT_SYMBOL_GPL(vfio_spapr_iommu_eeh_ioctl);
 
 MODULE_VERSION(DRIVER_VERSION);
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index 7db5234462d0..9a23698b6fe8 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -216,7 +216,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
 	data->reg_duty_cycle = res->start;
 	res = platform_get_resource_byname(pdev, IORESOURCE_REG, "always on");
 	if (!res) {
-		dev_err(&pdev->dev, "No REG resorce for always on\n");
+		dev_err(&pdev->dev, "No REG resource for always on\n");
 		return -ENXIO;
 	}
 	data->reg_always_on = res->start;
@@ -265,7 +265,6 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
 static struct platform_driver pm860x_backlight_driver = {
 	.driver		= {
 		.name	= "88pm860x-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pm860x_backlight_probe,
 };
diff --git a/drivers/video/backlight/aat2870_bl.c b/drivers/video/backlight/aat2870_bl.c
index 86234c31d79c..50774e657700 100644
--- a/drivers/video/backlight/aat2870_bl.c
+++ b/drivers/video/backlight/aat2870_bl.c
@@ -211,7 +211,6 @@ static int aat2870_bl_remove(struct platform_device *pdev)
 static struct platform_driver aat2870_bl_driver = {
 	.driver = {
 		.name	= "aat2870-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= aat2870_bl_probe,
 	.remove		= aat2870_bl_remove,
diff --git a/drivers/video/backlight/adp5520_bl.c b/drivers/video/backlight/adp5520_bl.c
index f37097a261a2..dd88ba1d71ce 100644
--- a/drivers/video/backlight/adp5520_bl.c
+++ b/drivers/video/backlight/adp5520_bl.c
@@ -67,6 +67,7 @@ static int adp5520_bl_set(struct backlight_device *bl, int brightness)
 static int adp5520_bl_update_status(struct backlight_device *bl)
 {
 	int brightness = bl->props.brightness;
+
 	if (bl->props.power != FB_BLANK_UNBLANK)
 		brightness = 0;
 
@@ -374,7 +375,6 @@ static SIMPLE_DEV_PM_OPS(adp5520_bl_pm_ops, adp5520_bl_suspend,
 static struct platform_driver adp5520_bl_driver = {
 	.driver		= {
 		.name	= "adp5520-backlight",
-		.owner	= THIS_MODULE,
 		.pm	= &adp5520_bl_pm_ops,
 	},
 	.probe		= adp5520_bl_probe,
diff --git a/drivers/video/backlight/adp8860_bl.c b/drivers/video/backlight/adp8860_bl.c
index be8d83deca7d..71147f4461b8 100644
--- a/drivers/video/backlight/adp8860_bl.c
+++ b/drivers/video/backlight/adp8860_bl.c
@@ -181,6 +181,7 @@ static int adp8860_clr_bits(struct i2c_client *client, int reg, uint8_t bit_mask
 static void adp8860_led_work(struct work_struct *work)
 {
 	struct adp8860_led *led = container_of(work, struct adp8860_led, work);
+
 	adp8860_write(led->client, ADP8860_ISC1 - led->id + 1,
 			 led->new_brightness >> 1);
 }
@@ -362,6 +363,7 @@ static int adp8860_bl_set(struct backlight_device *bl, int brightness)
 static int adp8860_bl_update_status(struct backlight_device *bl)
 {
 	int brightness = bl->props.brightness;
+
 	if (bl->props.power != FB_BLANK_UNBLANK)
 		brightness = 0;
 
@@ -499,6 +501,7 @@ static ssize_t adp8860_bl_l1_daylight_max_store(struct device *dev,
 {
 	struct adp8860_bl *data = dev_get_drvdata(dev);
 	int ret = kstrtoul(buf, 10, &data->cached_daylight_max);
+
 	if (ret)
 		return ret;
 
diff --git a/drivers/video/backlight/adp8870_bl.c b/drivers/video/backlight/adp8870_bl.c
index 251af4d38d86..037e43083343 100644
--- a/drivers/video/backlight/adp8870_bl.c
+++ b/drivers/video/backlight/adp8870_bl.c
@@ -144,6 +144,7 @@ static int adp8870_read(struct i2c_client *client, int reg, uint8_t *val)
 static int adp8870_write(struct i2c_client *client, u8 reg, u8 val)
 {
 	int ret = i2c_smbus_write_byte_data(client, reg, val);
+
 	if (ret)
 		dev_err(&client->dev, "failed to write\n");
 
@@ -195,6 +196,7 @@ static int adp8870_clr_bits(struct i2c_client *client, int reg, uint8_t bit_mask
 static void adp8870_led_work(struct work_struct *work)
 {
 	struct adp8870_led *led = container_of(work, struct adp8870_led, work);
+
 	adp8870_write(led->client, ADP8870_ISC1 + led->id - 1,
 			 led->new_brightness >> 1);
 }
@@ -399,6 +401,7 @@ static int adp8870_bl_set(struct backlight_device *bl, int brightness)
 static int adp8870_bl_update_status(struct backlight_device *bl)
 {
 	int brightness = bl->props.brightness;
+
 	if (bl->props.power != FB_BLANK_UNBLANK)
 		brightness = 0;
 
@@ -649,6 +652,7 @@ static ssize_t adp8870_bl_l1_daylight_max_store(struct device *dev,
 {
 	struct adp8870_bl *data = dev_get_drvdata(dev);
 	int ret = kstrtoul(buf, 10, &data->cached_daylight_max);
+
 	if (ret)
 		return ret;
 
diff --git a/drivers/video/backlight/ams369fg06.c b/drivers/video/backlight/ams369fg06.c
index 4726c8be626f..5f897f99cc9b 100644
--- a/drivers/video/backlight/ams369fg06.c
+++ b/drivers/video/backlight/ams369fg06.c
@@ -325,11 +325,11 @@ static int ams369fg06_power_on(struct ams369fg06 *lcd)
 	if (!pd->reset) {
 		dev_err(lcd->dev, "reset is NULL.\n");
 		return -EINVAL;
-	} else {
-		pd->reset(lcd->ld);
-		msleep(pd->reset_delay);
 	}
 
+	pd->reset(lcd->ld);
+	msleep(pd->reset_delay);
+
 	ret = ams369fg06_ldi_init(lcd);
 	if (ret) {
 		dev_err(lcd->dev, "failed to initialize ldi.\n");
diff --git a/drivers/video/backlight/as3711_bl.c b/drivers/video/backlight/as3711_bl.c
index bb1fc45b7549..734a9158946b 100644
--- a/drivers/video/backlight/as3711_bl.c
+++ b/drivers/video/backlight/as3711_bl.c
@@ -467,7 +467,6 @@ static int as3711_backlight_probe(struct platform_device *pdev)
 static struct platform_driver as3711_backlight_driver = {
 	.driver		= {
 		.name	= "as3711-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= as3711_backlight_probe,
 };
diff --git a/drivers/video/backlight/corgi_lcd.c b/drivers/video/backlight/corgi_lcd.c
index 51d18d637e2b..d7c37a8ccd1f 100644
--- a/drivers/video/backlight/corgi_lcd.c
+++ b/drivers/video/backlight/corgi_lcd.c
@@ -143,6 +143,7 @@ static void lcdtg_i2c_send_byte(struct corgi_lcd *lcd,
 				uint8_t base, uint8_t data)
 {
 	int i;
+
 	for (i = 0; i < 8; i++) {
 		if (data & 0x80)
 			lcdtg_i2c_send_bit(lcd, base | POWER0_COM_DOUT);
diff --git a/drivers/video/backlight/cr_bllcd.c b/drivers/video/backlight/cr_bllcd.c
index f3fed9ef745f..3e3880fc8c8e 100644
--- a/drivers/video/backlight/cr_bllcd.c
+++ b/drivers/video/backlight/cr_bllcd.c
@@ -235,6 +235,7 @@ static int cr_backlight_probe(struct platform_device *pdev)
 static int cr_backlight_remove(struct platform_device *pdev)
 {
 	struct cr_panel *crp = platform_get_drvdata(pdev);
+
 	crp->cr_backlight_device->props.power = FB_BLANK_POWERDOWN;
 	crp->cr_backlight_device->props.brightness = 0;
 	crp->cr_backlight_device->props.max_brightness = 0;
diff --git a/drivers/video/backlight/da903x_bl.c b/drivers/video/backlight/da903x_bl.c
index 12c5d840c590..f793738f06fb 100644
--- a/drivers/video/backlight/da903x_bl.c
+++ b/drivers/video/backlight/da903x_bl.c
@@ -162,7 +162,6 @@ static int da903x_backlight_probe(struct platform_device *pdev)
 static struct platform_driver da903x_backlight_driver = {
 	.driver		= {
 		.name	= "da903x-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= da903x_backlight_probe,
 };
diff --git a/drivers/video/backlight/da9052_bl.c b/drivers/video/backlight/da9052_bl.c
index 20d55becaa74..d4bd74bd5070 100644
--- a/drivers/video/backlight/da9052_bl.c
+++ b/drivers/video/backlight/da9052_bl.c
@@ -173,7 +173,6 @@ static struct platform_driver da9052_wled_driver = {
 	.id_table	= da9052_wled_ids,
 	.driver	= {
 		.name	= "da9052-wled",
-		.owner	= THIS_MODULE,
 	},
 };
 
diff --git a/drivers/video/backlight/ep93xx_bl.c b/drivers/video/backlight/ep93xx_bl.c
index 0d1f633c6480..0067931821c6 100644
--- a/drivers/video/backlight/ep93xx_bl.c
+++ b/drivers/video/backlight/ep93xx_bl.c
@@ -128,7 +128,6 @@ static SIMPLE_DEV_PM_OPS(ep93xxbl_pm_ops, ep93xxbl_suspend, ep93xxbl_resume);
 static struct platform_driver ep93xxbl_driver = {
 	.driver		= {
 		.name	= "ep93xx-bl",
-		.owner	= THIS_MODULE,
 		.pm	= &ep93xxbl_pm_ops,
 	},
 	.probe		= ep93xxbl_probe,
diff --git a/drivers/video/backlight/generic_bl.c b/drivers/video/backlight/generic_bl.c
index 5d8d65200db7..67dfb939a514 100644
--- a/drivers/video/backlight/generic_bl.c
+++ b/drivers/video/backlight/generic_bl.c
@@ -52,24 +52,6 @@ static int genericbl_get_intensity(struct backlight_device *bd)
 	return genericbl_intensity;
 }
 
-/*
- * Called when the battery is low to limit the backlight intensity.
- * If limit==0 clear any limit, otherwise limit the intensity
- */
-void genericbl_limit_intensity(int limit)
-{
-	struct backlight_device *bd = generic_backlight_device;
-
-	mutex_lock(&bd->ops_lock);
-	if (limit)
-		bd->props.state |= GENERICBL_BATTLOW;
-	else
-		bd->props.state &= ~GENERICBL_BATTLOW;
-	backlight_update_status(generic_backlight_device);
-	mutex_unlock(&bd->ops_lock);
-}
-EXPORT_SYMBOL(genericbl_limit_intensity);
-
 static const struct backlight_ops genericbl_ops = {
 	.options = BL_CORE_SUSPENDRESUME,
 	.get_brightness = genericbl_get_intensity,
diff --git a/drivers/video/backlight/gpio_backlight.c b/drivers/video/backlight/gpio_backlight.c
index aaead04a2d54..439feb2389a8 100644
--- a/drivers/video/backlight/gpio_backlight.c
+++ b/drivers/video/backlight/gpio_backlight.c
@@ -151,7 +151,6 @@ static struct of_device_id gpio_backlight_of_match[] = {
 static struct platform_driver gpio_backlight_driver = {
 	.driver		= {
 		.name		= "gpio-backlight",
-		.owner		= THIS_MODULE,
 		.of_match_table = of_match_ptr(gpio_backlight_of_match),
 	},
 	.probe		= gpio_backlight_probe,
diff --git a/drivers/video/backlight/ili922x.c b/drivers/video/backlight/ili922x.c
index ea67fe199e34..e7f0890cc211 100644
--- a/drivers/video/backlight/ili922x.c
+++ b/drivers/video/backlight/ili922x.c
@@ -495,17 +495,18 @@ static int ili922x_probe(struct spi_device *spi)
 			"no LCD found: Chip ID 0x%x, ret %d\n",
 			reg, ret);
 		return -ENODEV;
-	} else {
-		dev_info(&spi->dev, "ILI%x found, SPI freq %d, mode %d\n",
-			 reg, spi->max_speed_hz, spi->mode);
 	}
 
+	dev_info(&spi->dev, "ILI%x found, SPI freq %d, mode %d\n",
+		 reg, spi->max_speed_hz, spi->mode);
+
 	ret = ili922x_read_status(spi, &reg);
 	if (ret) {
 		dev_err(&spi->dev, "reading RS failed...\n");
 		return ret;
-	} else
-		dev_dbg(&spi->dev, "status: 0x%x\n", reg);
+	}
+
+	dev_dbg(&spi->dev, "status: 0x%x\n", reg);
 
 	ili922x_display_init(spi);
 
diff --git a/drivers/video/backlight/jornada720_bl.c b/drivers/video/backlight/jornada720_bl.c
index 6ce96b4a8796..7e6ff5346892 100644
--- a/drivers/video/backlight/jornada720_bl.c
+++ b/drivers/video/backlight/jornada720_bl.c
@@ -41,11 +41,11 @@ static int jornada_bl_get_brightness(struct backlight_device *bd)
 		dev_err(&bd->dev, "get brightness timeout\n");
 		jornada_ssp_end();
 		return -ETIMEDOUT;
-	} else {
-		/* exchange txdummy for value */
-		ret = jornada_ssp_byte(TXDUMMY);
 	}
 
+	/* exchange txdummy for value */
+	ret = jornada_ssp_byte(TXDUMMY);
+
 	jornada_ssp_end();
 
 	return BL_MAX_BRIGHT - ret;
diff --git a/drivers/video/backlight/jornada720_lcd.c b/drivers/video/backlight/jornada720_lcd.c
index 228bc319de19..dfa0fa0d5c78 100644
--- a/drivers/video/backlight/jornada720_lcd.c
+++ b/drivers/video/backlight/jornada720_lcd.c
@@ -27,11 +27,7 @@
 
 static int jornada_lcd_get_power(struct lcd_device *ld)
 {
-	/* LDD2 in PPC = LCD POWER */
-	if (PPSR & PPC_LDD2)
-		return FB_BLANK_UNBLANK;	/* PW ON */
-	else
-		return FB_BLANK_POWERDOWN;	/* PW OFF */
+	return PPSR & PPC_LDD2 ? FB_BLANK_UNBLANK : FB_BLANK_POWERDOWN;
 }
 
 static int jornada_lcd_get_contrast(struct lcd_device *ld)
diff --git a/drivers/video/backlight/ld9040.c b/drivers/video/backlight/ld9040.c
index ccb44e8e4927..f71eaf10c4eb 100644
--- a/drivers/video/backlight/ld9040.c
+++ b/drivers/video/backlight/ld9040.c
@@ -566,11 +566,11 @@ static int ld9040_power_on(struct ld9040 *lcd)
 	if (!pd->reset) {
 		dev_err(lcd->dev, "reset is NULL.\n");
 		return -EINVAL;
-	} else {
-		pd->reset(lcd->ld);
-		msleep(pd->reset_delay);
 	}
 
+	pd->reset(lcd->ld);
+	msleep(pd->reset_delay);
+
 	ret = ld9040_ldi_init(lcd);
 	if (ret) {
 		dev_err(lcd->dev, "failed to initialize ldi.\n");
diff --git a/drivers/video/backlight/lm3533_bl.c b/drivers/video/backlight/lm3533_bl.c
index cff1fbe89a1b..0e2337f367b6 100644
--- a/drivers/video/backlight/lm3533_bl.c
+++ b/drivers/video/backlight/lm3533_bl.c
@@ -397,7 +397,6 @@ static void lm3533_bl_shutdown(struct platform_device *pdev)
 static struct platform_driver lm3533_bl_driver = {
 	.driver = {
 		.name	= "lm3533-backlight",
-		.owner	= THIS_MODULE,
 		.pm	= &lm3533_bl_pm_ops,
 	},
 	.probe		= lm3533_bl_probe,
diff --git a/drivers/video/backlight/lm3639_bl.c b/drivers/video/backlight/lm3639_bl.c
index 5f36808d214f..cd50df5807ea 100644
--- a/drivers/video/backlight/lm3639_bl.c
+++ b/drivers/video/backlight/lm3639_bl.c
@@ -254,7 +254,6 @@ static void lm3639_torch_brightness_set(struct led_classdev *cdev,
 	return;
 out:
 	dev_err(pchip->dev, "i2c failed to access register\n");
-	return;
 }
 
 /* flash */
@@ -293,7 +292,6 @@ static void lm3639_flash_brightness_set(struct led_classdev *cdev,
 	return;
 out:
 	dev_err(pchip->dev, "i2c failed to access register\n");
-	return;
 }
 
 static const struct regmap_config lm3639_regmap = {
diff --git a/drivers/video/backlight/lms501kf03.c b/drivers/video/backlight/lms501kf03.c
index 77258b7b04be..7e3810308c3e 100644
--- a/drivers/video/backlight/lms501kf03.c
+++ b/drivers/video/backlight/lms501kf03.c
@@ -232,19 +232,19 @@ static int lms501kf03_power_on(struct lms501kf03 *lcd)
 	if (!pd->power_on) {
 		dev_err(lcd->dev, "power_on is NULL.\n");
 		return -EINVAL;
-	} else {
-		pd->power_on(lcd->ld, 1);
-		msleep(pd->power_on_delay);
 	}
 
+	pd->power_on(lcd->ld, 1);
+	msleep(pd->power_on_delay);
+
 	if (!pd->reset) {
 		dev_err(lcd->dev, "reset is NULL.\n");
 		return -EINVAL;
-	} else {
-		pd->reset(lcd->ld);
-		msleep(pd->reset_delay);
 	}
 
+	pd->reset(lcd->ld);
+	msleep(pd->reset_delay);
+
 	ret = lms501kf03_ldi_init(lcd);
 	if (ret) {
 		dev_err(lcd->dev, "failed to initialize ldi.\n");
diff --git a/drivers/video/backlight/lp855x_bl.c b/drivers/video/backlight/lp855x_bl.c
index dcdd5443efcf..25fb8e3d75b1 100644
--- a/drivers/video/backlight/lp855x_bl.c
+++ b/drivers/video/backlight/lp855x_bl.c
@@ -268,6 +268,7 @@ static int lp855x_bl_update_status(struct backlight_device *bl)
 
 	} else if (lp->mode == REGISTER_BASED) {
 		u8 val = bl->props.brightness;
+
 		lp855x_write_byte(lp, lp->cfg->reg_brightness, val);
 	}
 
@@ -308,6 +309,7 @@ static ssize_t lp855x_get_chip_id(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
 	struct lp855x *lp = dev_get_drvdata(dev);
+
 	return scnprintf(buf, PAGE_SIZE, "%s\n", lp->chipname);
 }
 
diff --git a/drivers/video/backlight/lp8788_bl.c b/drivers/video/backlight/lp8788_bl.c
index d6c4f6a2d43e..e418d5b1aa55 100644
--- a/drivers/video/backlight/lp8788_bl.c
+++ b/drivers/video/backlight/lp8788_bl.c
@@ -315,7 +315,6 @@ static struct platform_driver lp8788_bl_driver = {
 	.remove = lp8788_backlight_remove,
 	.driver = {
 		.name = LP8788_DEV_BACKLIGHT,
-		.owner = THIS_MODULE,
 	},
 };
 module_platform_driver(lp8788_bl_driver);
diff --git a/drivers/video/backlight/max8925_bl.c b/drivers/video/backlight/max8925_bl.c
index 66fa08c920d2..7b738d60ecc2 100644
--- a/drivers/video/backlight/max8925_bl.c
+++ b/drivers/video/backlight/max8925_bl.c
@@ -197,7 +197,6 @@ static int max8925_backlight_probe(struct platform_device *pdev)
 static struct platform_driver max8925_backlight_driver = {
 	.driver		= {
 		.name	= "max8925-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= max8925_backlight_probe,
 };
diff --git a/drivers/video/backlight/omap1_bl.c b/drivers/video/backlight/omap1_bl.c
index a0dcd88ac74f..546d94df21d5 100644
--- a/drivers/video/backlight/omap1_bl.c
+++ b/drivers/video/backlight/omap1_bl.c
@@ -120,6 +120,7 @@ static int omapbl_update_status(struct backlight_device *dev)
 static int omapbl_get_intensity(struct backlight_device *dev)
 {
 	struct omap_backlight *bl = bl_get_data(dev);
+
 	return bl->current_intensity;
 }
 
diff --git a/drivers/video/backlight/ot200_bl.c b/drivers/video/backlight/ot200_bl.c
index f5a5202dd79d..3acdb9f646ed 100644
--- a/drivers/video/backlight/ot200_bl.c
+++ b/drivers/video/backlight/ot200_bl.c
@@ -152,7 +152,6 @@ static int ot200_backlight_remove(struct platform_device *pdev)
 static struct platform_driver ot200_backlight_driver = {
 	.driver		= {
 		.name	= "ot200-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= ot200_backlight_probe,
 	.remove		= ot200_backlight_remove,
diff --git a/drivers/video/backlight/pandora_bl.c b/drivers/video/backlight/pandora_bl.c
index 2e3f82063c03..5d8bb8b20183 100644
--- a/drivers/video/backlight/pandora_bl.c
+++ b/drivers/video/backlight/pandora_bl.c
@@ -142,7 +142,6 @@ static int pandora_backlight_probe(struct platform_device *pdev)
 static struct platform_driver pandora_backlight_driver = {
 	.driver		= {
 		.name	= "pandora-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= pandora_backlight_probe,
 };
diff --git a/drivers/video/backlight/pcf50633-backlight.c b/drivers/video/backlight/pcf50633-backlight.c
index b95d3b0aaffe..85bd573b6d15 100644
--- a/drivers/video/backlight/pcf50633-backlight.c
+++ b/drivers/video/backlight/pcf50633-backlight.c
@@ -90,6 +90,7 @@ static int pcf50633_bl_update_status(struct backlight_device *bl)
 static int pcf50633_bl_get_brightness(struct backlight_device *bl)
 {
 	struct pcf50633_bl *pcf_bl = bl_get_data(bl);
+
 	return pcf_bl->brightness;
 }
 
diff --git a/drivers/video/backlight/platform_lcd.c b/drivers/video/backlight/platform_lcd.c
index c3d2e209fc8f..872a3bf21faf 100644
--- a/drivers/video/backlight/platform_lcd.c
+++ b/drivers/video/backlight/platform_lcd.c
@@ -148,7 +148,6 @@ MODULE_DEVICE_TABLE(of, platform_lcd_of_match);
 static struct platform_driver platform_lcd_driver = {
 	.driver		= {
 		.name	= "platform-lcd",
-		.owner	= THIS_MODULE,
 		.pm	= &platform_lcd_pm_ops,
 		.of_match_table = of_match_ptr(platform_lcd_of_match),
 	},
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index d7a3d13e72ec..cb5ae4c08469 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -173,6 +173,7 @@ static int pwm_backlight_parse_dt(struct device *dev,
 		data->max_brightness--;
 	}
 
+	data->enable_gpio = -EINVAL;
 	return 0;
 }
 
@@ -389,7 +390,6 @@ static const struct dev_pm_ops pwm_backlight_pm_ops = {
 static struct platform_driver pwm_backlight_driver = {
 	.driver		= {
 		.name		= "pwm-backlight",
-		.owner		= THIS_MODULE,
 		.pm		= &pwm_backlight_pm_ops,
 		.of_match_table	= of_match_ptr(pwm_backlight_of_match),
 	},
diff --git a/drivers/video/backlight/s6e63m0.c b/drivers/video/backlight/s6e63m0.c
index f3a65c8940ed..28bfa127fee4 100644
--- a/drivers/video/backlight/s6e63m0.c
+++ b/drivers/video/backlight/s6e63m0.c
@@ -507,19 +507,19 @@ static int s6e63m0_power_on(struct s6e63m0 *lcd)
 	if (!pd->power_on) {
 		dev_err(lcd->dev, "power_on is NULL.\n");
 		return -EINVAL;
-	} else {
-		pd->power_on(lcd->ld, 1);
-		msleep(pd->power_on_delay);
 	}
 
+	pd->power_on(lcd->ld, 1);
+	msleep(pd->power_on_delay);
+
 	if (!pd->reset) {
 		dev_err(lcd->dev, "reset is NULL.\n");
 		return -EINVAL;
-	} else {
-		pd->reset(lcd->ld);
-		msleep(pd->reset_delay);
 	}
 
+	pd->reset(lcd->ld);
+	msleep(pd->reset_delay);
+
 	ret = s6e63m0_ldi_init(lcd);
 	if (ret) {
 		dev_err(lcd->dev, "failed to initialize ldi.\n");
diff --git a/drivers/video/backlight/tdo24m.c b/drivers/video/backlight/tdo24m.c
index 908016fc5829..30afce33ef2a 100644
--- a/drivers/video/backlight/tdo24m.c
+++ b/drivers/video/backlight/tdo24m.c
@@ -300,12 +300,14 @@ static int tdo24m_power(struct tdo24m *lcd, int power)
 static int tdo24m_set_power(struct lcd_device *ld, int power)
 {
 	struct tdo24m *lcd = lcd_get_data(ld);
+
 	return tdo24m_power(lcd, power);
 }
 
 static int tdo24m_get_power(struct lcd_device *ld)
 {
 	struct tdo24m *lcd = lcd_get_data(ld);
+
 	return lcd->power;
 }
 
diff --git a/drivers/video/backlight/tps65217_bl.c b/drivers/video/backlight/tps65217_bl.c
index 2e04d93aa0ef..61d72bffd402 100644
--- a/drivers/video/backlight/tps65217_bl.c
+++ b/drivers/video/backlight/tps65217_bl.c
@@ -323,7 +323,6 @@ static int tps65217_bl_probe(struct platform_device *pdev)
 static struct platform_driver tps65217_bl_driver = {
 	.probe		= tps65217_bl_probe,
 	.driver		= {
-		.owner	= THIS_MODULE,
 		.name	= "tps65217-bl",
 	},
 };
diff --git a/drivers/video/backlight/wm831x_bl.c b/drivers/video/backlight/wm831x_bl.c
index 8b9455e93069..6eab0d6c262a 100644
--- a/drivers/video/backlight/wm831x_bl.c
+++ b/drivers/video/backlight/wm831x_bl.c
@@ -111,6 +111,7 @@ static int wm831x_backlight_update_status(struct backlight_device *bl)
 static int wm831x_backlight_get_brightness(struct backlight_device *bl)
 {
 	struct wm831x_backlight_data *data = bl_get_data(bl);
+
 	return data->current_brightness;
 }
 
@@ -217,7 +218,6 @@ static int wm831x_backlight_probe(struct platform_device *pdev)
 static struct platform_driver wm831x_backlight_driver = {
 	.driver		= {
 		.name	= "wm831x-backlight",
-		.owner	= THIS_MODULE,
 	},
 	.probe		= wm831x_backlight_probe,
 };
diff --git a/drivers/video/console/bitblit.c b/drivers/video/console/bitblit.c
index 61b182bf32a2..dbfe4eecf12e 100644
--- a/drivers/video/console/bitblit.c
+++ b/drivers/video/console/bitblit.c
@@ -205,7 +205,6 @@ static void bit_putcs(struct vc_data *vc, struct fb_info *info,
 static void bit_clear_margins(struct vc_data *vc, struct fb_info *info,
 			      int bottom_only)
 {
-	int bgshift = (vc->vc_hi_font_mask) ? 13 : 12;
 	unsigned int cw = vc->vc_font.width;
 	unsigned int ch = vc->vc_font.height;
 	unsigned int rw = info->var.xres - (vc->vc_cols*cw);
@@ -214,7 +213,7 @@ static void bit_clear_margins(struct vc_data *vc, struct fb_info *info,
 	unsigned int bs = info->var.yres - bh;
 	struct fb_fillrect region;
 
-	region.color = attr_bgcol_ec(bgshift, vc, info);
+	region.color = 0;
 	region.rop = ROP_COPY;
 
 	if (rw && !bottom_only) {
diff --git a/drivers/video/console/fbcon_ccw.c b/drivers/video/console/fbcon_ccw.c
index 41b32ae23dac..5a3cbf6dff4d 100644
--- a/drivers/video/console/fbcon_ccw.c
+++ b/drivers/video/console/fbcon_ccw.c
@@ -197,9 +197,8 @@ static void ccw_clear_margins(struct vc_data *vc, struct fb_info *info,
 	unsigned int bh = info->var.xres - (vc->vc_rows*ch);
 	unsigned int bs = vc->vc_rows*ch;
 	struct fb_fillrect region;
-	int bgshift = (vc->vc_hi_font_mask) ? 13 : 12;
 
-	region.color = attr_bgcol_ec(bgshift,vc,info);
+	region.color = 0;
 	region.rop = ROP_COPY;
 
 	if (rw && !bottom_only) {
diff --git a/drivers/video/console/fbcon_cw.c b/drivers/video/console/fbcon_cw.c
index a93670ef7f89..e7ee44db4e98 100644
--- a/drivers/video/console/fbcon_cw.c
+++ b/drivers/video/console/fbcon_cw.c
@@ -180,9 +180,8 @@ static void cw_clear_margins(struct vc_data *vc, struct fb_info *info,
 	unsigned int bh = info->var.xres - (vc->vc_rows*ch);
 	unsigned int rs = info->var.yres - rw;
 	struct fb_fillrect region;
-	int bgshift = (vc->vc_hi_font_mask) ? 13 : 12;
 
-	region.color = attr_bgcol_ec(bgshift,vc,info);
+	region.color = 0;
 	region.rop = ROP_COPY;
 
 	if (rw && !bottom_only) {
diff --git a/drivers/video/console/fbcon_ud.c b/drivers/video/console/fbcon_ud.c
index ff0872c0498b..19e3714abfe8 100644
--- a/drivers/video/console/fbcon_ud.c
+++ b/drivers/video/console/fbcon_ud.c
@@ -227,9 +227,8 @@ static void ud_clear_margins(struct vc_data *vc, struct fb_info *info,
 	unsigned int rw = info->var.xres - (vc->vc_cols*cw);
 	unsigned int bh = info->var.yres - (vc->vc_rows*ch);
 	struct fb_fillrect region;
-	int bgshift = (vc->vc_hi_font_mask) ? 13 : 12;
 
-	region.color = attr_bgcol_ec(bgshift,vc,info);
+	region.color = 0;
 	region.rop = ROP_COPY;
 
 	if (rw && !bottom_only) {
diff --git a/drivers/video/fbdev/Kconfig b/drivers/video/fbdev/Kconfig
index e911b9c96e19..c7bf606a8706 100644
--- a/drivers/video/fbdev/Kconfig
+++ b/drivers/video/fbdev/Kconfig
@@ -4,6 +4,7 @@
 
 menuconfig FB
 	tristate "Support for frame buffer devices"
+	select FB_CMDLINE
 	---help---
 	  The frame buffer device provides an abstraction for the graphics
 	  hardware. It represents the frame buffer of some video hardware and
@@ -52,6 +53,9 @@ config FIRMWARE_EDID
 	 combination with certain motherboards and monitors are known to
 	 suffer from this problem.
 
+config FB_CMDLINE
+	bool
+
 config FB_DDC
        tristate
        depends on FB
@@ -2352,10 +2356,11 @@ config FB_MSM
 config FB_MX3
 	tristate "MX3 Framebuffer support"
 	depends on FB && MX3_IPU
+	select BACKLIGHT_CLASS_DEVICE
+	select BACKLIGHT_LCD_SUPPORT
 	select FB_CFB_FILLRECT
 	select FB_CFB_COPYAREA
 	select FB_CFB_IMAGEBLIT
-	select BACKLIGHT_CLASS_DEVICE
 	default y
 	help
 	  This is a framebuffer device for the i.MX31 LCD Controller. So
diff --git a/drivers/video/fbdev/amba-clcd.c b/drivers/video/fbdev/amba-clcd.c
index beadd3edaa17..6ad23bd3523a 100644
--- a/drivers/video/fbdev/amba-clcd.c
+++ b/drivers/video/fbdev/amba-clcd.c
@@ -24,6 +24,7 @@
 #include <linux/list.h>
 #include <linux/amba/bus.h>
 #include <linux/amba/clcd.h>
+#include <linux/bitops.h>
 #include <linux/clk.h>
 #include <linux/hardirq.h>
 #include <linux/dma-mapping.h>
@@ -638,9 +639,7 @@ static int clcdfb_of_init_tft_panel(struct clcd_fb *fb, u32 r0, u32 g0, u32 b0)
 		if (g0 != panels[i].g0)
 			continue;
 		if (r0 == panels[i].r0 && b0 == panels[i].b0)
-			fb->panel->caps = panels[i].caps & CLCD_CAP_RGB;
-		if (r0 == panels[i].b0 && b0 == panels[i].r0)
-			fb->panel->caps = panels[i].caps & CLCD_CAP_BGR;
+			fb->panel->caps = panels[i].caps;
 	}
 
 	return fb->panel->caps ? 0 : -EINVAL;
@@ -650,6 +649,7 @@ static int clcdfb_of_init_display(struct clcd_fb *fb)
 {
 	struct device_node *endpoint;
 	int err;
+	unsigned int bpp;
 	u32 max_bandwidth;
 	u32 tft_r0b0g0[3];
 
@@ -667,11 +667,22 @@ static int clcdfb_of_init_display(struct clcd_fb *fb)
 
 	err = of_property_read_u32(fb->dev->dev.of_node, "max-memory-bandwidth",
 			&max_bandwidth);
-	if (!err)
-		fb->panel->bpp = 8 * max_bandwidth / (fb->panel->mode.xres *
-				fb->panel->mode.yres * fb->panel->mode.refresh);
-	else
-		fb->panel->bpp = 32;
+	if (!err) {
+		/*
+		 * max_bandwidth is in bytes per second and pixclock in
+		 * pico-seconds, so the maximum allowed bits per pixel is
+		 *   8 * max_bandwidth / (PICOS2KHZ(pixclock) * 1000)
+		 * Rearrange this calculation to avoid overflow and then ensure
+		 * result is a valid format.
+		 */
+		bpp = max_bandwidth / (1000 / 8)
+			/ PICOS2KHZ(fb->panel->mode.pixclock);
+		bpp = rounddown_pow_of_two(bpp);
+		if (bpp > 32)
+			bpp = 32;
+	} else
+		bpp = 32;
+	fb->panel->bpp = bpp;
 
 #ifdef CONFIG_CPU_BIG_ENDIAN
 	fb->panel->cntl |= CNTL_BEBO;
diff --git a/drivers/video/fbdev/atmel_lcdfb.c b/drivers/video/fbdev/atmel_lcdfb.c
index 92640d46770a..3bf403150a2d 100644
--- a/drivers/video/fbdev/atmel_lcdfb.c
+++ b/drivers/video/fbdev/atmel_lcdfb.c
@@ -24,6 +24,7 @@
 #include <linux/of_device.h>
 #include <linux/of_gpio.h>
 #include <video/of_display_timing.h>
+#include <linux/regulator/consumer.h>
 #include <video/videomode.h>
 
 #include <mach/cpu.h>
@@ -60,6 +61,7 @@ struct atmel_lcdfb_info {
 	struct atmel_lcdfb_pdata pdata;
 
 	struct atmel_lcdfb_config *config;
+	struct regulator	*reg_lcd;
 };
 
 struct atmel_lcdfb_power_ctrl_gpio {
@@ -302,10 +304,24 @@ static void init_contrast(struct atmel_lcdfb_info *sinfo)
 
 static inline void atmel_lcdfb_power_control(struct atmel_lcdfb_info *sinfo, int on)
 {
+	int ret;
 	struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 
 	if (pdata->atmel_lcdfb_power_control)
 		pdata->atmel_lcdfb_power_control(pdata, on);
+	else if (sinfo->reg_lcd) {
+		if (on) {
+			ret = regulator_enable(sinfo->reg_lcd);
+			if (ret)
+				dev_err(&sinfo->pdev->dev,
+					"lcd regulator enable failed:	%d\n", ret);
+		} else {
+			ret = regulator_disable(sinfo->reg_lcd);
+			if (ret)
+				dev_err(&sinfo->pdev->dev,
+					"lcd regulator disable failed: %d\n", ret);
+		}
+	}
 }
 
 static struct fb_fix_screeninfo atmel_lcdfb_fix __initdata = {
@@ -1102,12 +1118,14 @@ static int atmel_lcdfb_of_init(struct atmel_lcdfb_info *sinfo)
 	timings = of_get_display_timings(display_np);
 	if (!timings) {
 		dev_err(dev, "failed to get display timings\n");
+		ret = -EINVAL;
 		goto put_display_node;
 	}
 
 	timings_np = of_find_node_by_name(display_np, "display-timings");
 	if (!timings_np) {
 		dev_err(dev, "failed to find display-timings node\n");
+		ret = -ENODEV;
 		goto put_display_node;
 	}
 
@@ -1193,6 +1211,10 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 	if (!sinfo->config)
 		goto free_info;
 
+	sinfo->reg_lcd = devm_regulator_get(&pdev->dev, "lcd");
+	if (IS_ERR(sinfo->reg_lcd))
+		sinfo->reg_lcd = NULL;
+
 	info->flags = ATMEL_LCDFB_FBINFO_DEFAULT;
 	info->pseudo_palette = sinfo->pseudo_palette;
 	info->fbops = &atmel_lcdfb_ops;
diff --git a/drivers/video/fbdev/aty/aty128fb.c b/drivers/video/fbdev/aty/aty128fb.c
index ff6070170d01..aedf2fbf9bf6 100644
--- a/drivers/video/fbdev/aty/aty128fb.c
+++ b/drivers/video/fbdev/aty/aty128fb.c
@@ -324,14 +324,61 @@ struct aty128_meminfo {
 };
 
 /* various memory configurations */
-static const struct aty128_meminfo sdr_128   =
-	{ 4, 4, 3, 3, 1, 3, 1, 16, 30, 16, "128-bit SDR SGRAM (1:1)" };
-static const struct aty128_meminfo sdr_64    =
-	{ 4, 8, 3, 3, 1, 3, 1, 17, 46, 17, "64-bit SDR SGRAM (1:1)" };
-static const struct aty128_meminfo sdr_sgram =
-	{ 4, 4, 1, 2, 1, 2, 1, 16, 24, 16, "64-bit SDR SGRAM (2:1)" };
-static const struct aty128_meminfo ddr_sgram =
-	{ 4, 4, 3, 3, 2, 3, 1, 16, 31, 16, "64-bit DDR SGRAM" };
+static const struct aty128_meminfo sdr_128 = {
+	.ML = 4,
+	.MB = 4,
+	.Trcd = 3,
+	.Trp = 3,
+	.Twr = 1,
+	.CL = 3,
+	.Tr2w = 1,
+	.LoopLatency = 16,
+	.DspOn = 30,
+	.Rloop = 16,
+	.name = "128-bit SDR SGRAM (1:1)",
+};
+
+static const struct aty128_meminfo sdr_64 = {
+	.ML = 4,
+	.MB = 8,
+	.Trcd = 3,
+	.Trp = 3,
+	.Twr = 1,
+	.CL = 3,
+	.Tr2w = 1,
+	.LoopLatency = 17,
+	.DspOn = 46,
+	.Rloop = 17,
+	.name = "64-bit SDR SGRAM (1:1)",
+};
+
+static const struct aty128_meminfo sdr_sgram = {
+	.ML = 4,
+	.MB = 4,
+	.Trcd = 1,
+	.Trp = 2,
+	.Twr = 1,
+	.CL = 2,
+	.Tr2w = 1,
+	.LoopLatency = 16,
+	.DspOn = 24,
+	.Rloop = 16,
+	.name = "64-bit SDR SGRAM (2:1)",
+};
+
+static const struct aty128_meminfo ddr_sgram = {
+	.ML = 4,
+	.MB = 4,
+	.Trcd = 3,
+	.Trp = 3,
+	.Twr = 2,
+	.CL = 3,
+	.Tr2w = 1,
+	.LoopLatency = 16,
+	.DspOn = 31,
+	.Rloop = 16,
+	.name = "64-bit DDR SGRAM",
+};
 
 static struct fb_fix_screeninfo aty128fb_fix = {
 	.id		= "ATY Rage128",
diff --git a/drivers/video/fbdev/au1200fb.c b/drivers/video/fbdev/au1200fb.c
index 40494dbdf519..18600d4e1b3f 100644
--- a/drivers/video/fbdev/au1200fb.c
+++ b/drivers/video/fbdev/au1200fb.c
@@ -1254,7 +1254,6 @@ static void set_global(u_int cmd, struct au1200_lcd_global_regs_t *pdata)
 			pdata->brightness = 30;
 		}
 		divider = (lcd->pwmdiv & 0x3FFFF) + 1;
-		hi1 = (lcd->pwmhi >> 16) + 1;
 		hi1 = (((pdata->brightness & 0xFF)+1) * divider >> 8);
 		lcd->pwmhi &= 0xFFFF;
 		lcd->pwmhi |= (hi1 << 16);
diff --git a/drivers/video/fbdev/chipsfb.c b/drivers/video/fbdev/chipsfb.c
index 206a66b61072..59abdc6a97f6 100644
--- a/drivers/video/fbdev/chipsfb.c
+++ b/drivers/video/fbdev/chipsfb.c
@@ -273,7 +273,7 @@ static struct chips_init_reg chips_init_xr[] = {
 	{ 0xa8, 0x00 }
 };
 
-static void __init chips_hw_init(void)
+static void chips_hw_init(void)
 {
 	int i;
 
diff --git a/drivers/video/fbdev/controlfb.c b/drivers/video/fbdev/controlfb.c
index fdadef979238..080fdd2a70f3 100644
--- a/drivers/video/fbdev/controlfb.c
+++ b/drivers/video/fbdev/controlfb.c
@@ -218,7 +218,8 @@ static int controlfb_check_var (struct fb_var_screeninfo *var, struct fb_info *i
  */
 static int controlfb_set_par (struct fb_info *info)
 {
-	struct fb_info_control *p = (struct fb_info_control *) info;
+	struct fb_info_control *p =
+		container_of(info, struct fb_info_control, info);
 	struct fb_par_control par;
 	int err;
 
@@ -258,7 +259,8 @@ static int controlfb_pan_display(struct fb_var_screeninfo *var,
 				 struct fb_info *info)
 {
 	unsigned int xoffset, hstep;
-	struct fb_info_control *p = (struct fb_info_control *)info;
+	struct fb_info_control *p =
+		container_of(info, struct fb_info_control, info);
 	struct fb_par_control *par = &p->par;
 
 	/*
@@ -309,7 +311,8 @@ static int controlfb_mmap(struct fb_info *info,
 
 static int controlfb_blank(int blank_mode, struct fb_info *info)
 {
-	struct fb_info_control *p = (struct fb_info_control *) info;
+	struct fb_info_control *p =
+		container_of(info, struct fb_info_control, info);
 	unsigned ctrl;
 
 	ctrl = ld_le32(CNTRL_REG(p,ctrl));
@@ -342,7 +345,8 @@ static int controlfb_blank(int blank_mode, struct fb_info *info)
 static int controlfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 			     u_int transp, struct fb_info *info)
 {
-	struct fb_info_control *p = (struct fb_info_control *) info;
+	struct fb_info_control *p =
+		container_of(info, struct fb_info_control, info);
 	__u8 r, g, b;
 
 	if (regno > 255)
@@ -833,7 +837,8 @@ static int control_var_to_par(struct fb_var_screeninfo *var,
 	unsigned hperiod, hssync, hsblank, hesync, heblank, piped, heq, hlfln,
 		 hserr, vperiod, vssync, vesync, veblank, vsblank, vswin, vewin;
 	unsigned long pixclock;
-	struct fb_info_control *p = (struct fb_info_control *) fb_info;
+	struct fb_info_control *p =
+		container_of(fb_info, struct fb_info_control, info);
 	struct control_regvals *r = &par->regvals;
 
 	switch (var->bits_per_pixel) {
diff --git a/drivers/video/fbdev/core/Makefile b/drivers/video/fbdev/core/Makefile
index fa306538dac2..67f28e20a892 100644
--- a/drivers/video/fbdev/core/Makefile
+++ b/drivers/video/fbdev/core/Makefile
@@ -1,4 +1,5 @@
 obj-y                             += fb_notify.o
+obj-$(CONFIG_FB_CMDLINE)          += fb_cmdline.o
 obj-$(CONFIG_FB)                  += fb.o
 fb-y                              := fbmem.o fbmon.o fbcmap.o fbsysfs.o \
                                      modedb.o fbcvt.o
diff --git a/drivers/video/fbdev/core/cfbcopyarea.c b/drivers/video/fbdev/core/cfbcopyarea.c
index bcb57235fcc7..6d4bfeecee35 100644
--- a/drivers/video/fbdev/core/cfbcopyarea.c
+++ b/drivers/video/fbdev/core/cfbcopyarea.c
@@ -55,8 +55,8 @@ bitcpy(struct fb_info *p, unsigned long __iomem *dst, unsigned dst_idx,
 	 * If you suspect bug in this function, compare it with this simple
 	 * memmove implementation.
 	 */
-	fb_memmove((char *)dst + ((dst_idx & (bits - 1))) / 8,
-		   (char *)src + ((src_idx & (bits - 1))) / 8, n / 8);
+	memmove((char *)dst + ((dst_idx & (bits - 1))) / 8,
+		(char *)src + ((src_idx & (bits - 1))) / 8, n / 8);
 	return;
 #endif
 
@@ -221,8 +221,8 @@ bitcpy_rev(struct fb_info *p, unsigned long __iomem *dst, unsigned dst_idx,
 	 * If you suspect bug in this function, compare it with this simple
 	 * memmove implementation.
 	 */
-	fb_memmove((char *)dst + ((dst_idx & (bits - 1))) / 8,
-		   (char *)src + ((src_idx & (bits - 1))) / 8, n / 8);
+	memmove((char *)dst + ((dst_idx & (bits - 1))) / 8,
+		(char *)src + ((src_idx & (bits - 1))) / 8, n / 8);
 	return;
 #endif
 
@@ -324,7 +324,10 @@ bitcpy_rev(struct fb_info *p, unsigned long __iomem *dst, unsigned dst_idx,
 				d0 = d0 << left | d1 >> right;
 			}
 			d0 = fb_rev_pixels_in_long(d0, bswapmask);
-			FB_WRITEL(comp(d0, FB_READL(dst), first), dst);
+			if (!first)
+				FB_WRITEL(d0, dst);
+			else
+				FB_WRITEL(comp(d0, FB_READL(dst), first), dst);
 			d0 = d1;
 			dst--;
 			n -= dst_idx+1;
diff --git a/drivers/video/fbdev/core/fb_cmdline.c b/drivers/video/fbdev/core/fb_cmdline.c
new file mode 100644
index 000000000000..39509ccd92f1
--- /dev/null
+++ b/drivers/video/fbdev/core/fb_cmdline.c
@@ -0,0 +1,110 @@
+/*
+ *  linux/drivers/video/fb_cmdline.c
+ *
+ *  Copyright (C) 2014 Intel Corp
+ *  Copyright (C) 1994 Martin Schaller
+ *
+ *	2001 - Documented with DocBook
+ *	- Brad Douglas <brad@neruo.com>
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License.  See the file COPYING in the main directory of this archive
+ * for more details.
+ *
+ * Authors:
+ *    Vetter <danie.vetter@ffwll.ch>
+ */
+#include <linux/init.h>
+#include <linux/fb.h>
+
+static char *video_options[FB_MAX] __read_mostly;
+static int ofonly __read_mostly;
+
+const char *fb_mode_option;
+EXPORT_SYMBOL_GPL(fb_mode_option);
+
+/**
+ * fb_get_options - get kernel boot parameters
+ * @name:   framebuffer name as it would appear in
+ *          the boot parameter line
+ *          (video=<name>:<options>)
+ * @option: the option will be stored here
+ *
+ * NOTE: Needed to maintain backwards compatibility
+ */
+int fb_get_options(const char *name, char **option)
+{
+	char *opt, *options = NULL;
+	int retval = 0;
+	int name_len = strlen(name), i;
+
+	if (name_len && ofonly && strncmp(name, "offb", 4))
+		retval = 1;
+
+	if (name_len && !retval) {
+		for (i = 0; i < FB_MAX; i++) {
+			if (video_options[i] == NULL)
+				continue;
+			if (!video_options[i][0])
+				continue;
+			opt = video_options[i];
+			if (!strncmp(name, opt, name_len) &&
+			    opt[name_len] == ':')
+				options = opt + name_len + 1;
+		}
+	}
+	/* No match, pass global option */
+	if (!options && option && fb_mode_option)
+		options = kstrdup(fb_mode_option, GFP_KERNEL);
+	if (options && !strncmp(options, "off", 3))
+		retval = 1;
+
+	if (option)
+		*option = options;
+
+	return retval;
+}
+EXPORT_SYMBOL(fb_get_options);
+
+/**
+ *	video_setup - process command line options
+ *	@options: string of options
+ *
+ *	Process command line options for frame buffer subsystem.
+ *
+ *	NOTE: This function is a __setup and __init function.
+ *            It only stores the options.  Drivers have to call
+ *            fb_get_options() as necessary.
+ *
+ *	Returns zero.
+ *
+ */
+static int __init video_setup(char *options)
+{
+	int i, global = 0;
+
+	if (!options || !*options)
+		global = 1;
+
+	if (!global && !strncmp(options, "ofonly", 6)) {
+		ofonly = 1;
+		global = 1;
+	}
+
+	if (!global && !strchr(options, ':')) {
+		fb_mode_option = options;
+		global = 1;
+	}
+
+	if (!global) {
+		for (i = 0; i < FB_MAX; i++) {
+			if (video_options[i] == NULL) {
+				video_options[i] = options;
+				break;
+			}
+		}
+	}
+
+	return 1;
+}
+__setup("video=", video_setup);
diff --git a/drivers/video/fbdev/core/fbmem.c b/drivers/video/fbdev/core/fbmem.c
index b5e85f6c1c26..0705d8883ede 100644
--- a/drivers/video/fbdev/core/fbmem.c
+++ b/drivers/video/fbdev/core/fbmem.c
@@ -1908,96 +1908,4 @@ int fb_new_modelist(struct fb_info *info)
 	return err;
 }
 
-static char *video_options[FB_MAX] __read_mostly;
-static int ofonly __read_mostly;
-
-/**
- * fb_get_options - get kernel boot parameters
- * @name:   framebuffer name as it would appear in
- *          the boot parameter line
- *          (video=<name>:<options>)
- * @option: the option will be stored here
- *
- * NOTE: Needed to maintain backwards compatibility
- */
-int fb_get_options(const char *name, char **option)
-{
-	char *opt, *options = NULL;
-	int retval = 0;
-	int name_len = strlen(name), i;
-
-	if (name_len && ofonly && strncmp(name, "offb", 4))
-		retval = 1;
-
-	if (name_len && !retval) {
-		for (i = 0; i < FB_MAX; i++) {
-			if (video_options[i] == NULL)
-				continue;
-			if (!video_options[i][0])
-				continue;
-			opt = video_options[i];
-			if (!strncmp(name, opt, name_len) &&
-			    opt[name_len] == ':')
-				options = opt + name_len + 1;
-		}
-	}
-	/* No match, pass global option */
-	if (!options && option && fb_mode_option)
-		options = kstrdup(fb_mode_option, GFP_KERNEL);
-	if (options && !strncmp(options, "off", 3))
-		retval = 1;
-
-	if (option)
-		*option = options;
-
-	return retval;
-}
-EXPORT_SYMBOL(fb_get_options);
-
-#ifndef MODULE
-/**
- *	video_setup - process command line options
- *	@options: string of options
- *
- *	Process command line options for frame buffer subsystem.
- *
- *	NOTE: This function is a __setup and __init function.
- *            It only stores the options.  Drivers have to call
- *            fb_get_options() as necessary.
- *
- *	Returns zero.
- *
- */
-static int __init video_setup(char *options)
-{
-	int i, global = 0;
-
-	if (!options || !*options)
- 		global = 1;
-
- 	if (!global && !strncmp(options, "ofonly", 6)) {
- 		ofonly = 1;
- 		global = 1;
- 	}
-
- 	if (!global && !strchr(options, ':')) {
- 		fb_mode_option = options;
- 		global = 1;
- 	}
-
- 	if (!global) {
- 		for (i = 0; i < FB_MAX; i++) {
- 			if (video_options[i] == NULL) {
- 				video_options[i] = options;
- 				break;
- 			}
-
-		}
-	}
-
-	return 1;
-}
-__setup("video=", video_setup);
-#endif
-
 MODULE_LICENSE("GPL");
diff --git a/drivers/video/fbdev/core/fbsysfs.c b/drivers/video/fbdev/core/fbsysfs.c
index 53444ac19fe0..60c3f0a16341 100644
--- a/drivers/video/fbdev/core/fbsysfs.c
+++ b/drivers/video/fbdev/core/fbsysfs.c
@@ -485,16 +485,8 @@ static ssize_t show_bl_curve(struct device *device,
 
 	mutex_lock(&fb_info->bl_curve_mutex);
 	for (i = 0; i < FB_BACKLIGHT_LEVELS; i += 8)
-		len += snprintf(&buf[len], PAGE_SIZE,
-				"%02x %02x %02x %02x %02x %02x %02x %02x\n",
-				fb_info->bl_curve[i + 0],
-				fb_info->bl_curve[i + 1],
-				fb_info->bl_curve[i + 2],
-				fb_info->bl_curve[i + 3],
-				fb_info->bl_curve[i + 4],
-				fb_info->bl_curve[i + 5],
-				fb_info->bl_curve[i + 6],
-				fb_info->bl_curve[i + 7]);
+		len += snprintf(&buf[len], PAGE_SIZE, "%8ph\n",
+				fb_info->bl_curve + i);
 	mutex_unlock(&fb_info->bl_curve_mutex);
 
 	return len;
diff --git a/drivers/video/fbdev/core/modedb.c b/drivers/video/fbdev/core/modedb.c
index a9a907c440d7..388f7971494b 100644
--- a/drivers/video/fbdev/core/modedb.c
+++ b/drivers/video/fbdev/core/modedb.c
@@ -29,9 +29,6 @@
 #define DPRINTK(fmt, args...)
 #endif
 
-const char *fb_mode_option;
-EXPORT_SYMBOL_GPL(fb_mode_option);
-
 /*
  *  Standard video mode definitions (taken from XFree86)
  */
diff --git a/drivers/video/fbdev/cyber2000fb.c b/drivers/video/fbdev/cyber2000fb.c
index b0a950f36970..99acf538a8b8 100644
--- a/drivers/video/fbdev/cyber2000fb.c
+++ b/drivers/video/fbdev/cyber2000fb.c
@@ -159,7 +159,7 @@ cyber2000_seqw(unsigned int reg, unsigned int val, struct cfb_info *cfb)
 static void
 cyber2000fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 	unsigned long dst, col;
 
 	if (!(cfb->fb.var.accel_flags & FB_ACCELF_TEXT)) {
@@ -191,7 +191,7 @@ cyber2000fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
 static void
 cyber2000fb_copyarea(struct fb_info *info, const struct fb_copyarea *region)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 	unsigned int cmd = CO_CMD_L_PATTERN_FGCOL;
 	unsigned long src, dst;
 
@@ -241,7 +241,7 @@ cyber2000fb_imageblit(struct fb_info *info, const struct fb_image *image)
 
 static int cyber2000fb_sync(struct fb_info *info)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 	int count = 100000;
 
 	if (!(cfb->fb.var.accel_flags & FB_ACCELF_TEXT))
@@ -276,7 +276,7 @@ static int
 cyber2000fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 		      u_int transp, struct fb_info *info)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 	struct fb_var_screeninfo *var = &cfb->fb.var;
 	u32 pseudo_val;
 	int ret = 1;
@@ -758,7 +758,7 @@ cyber2000fb_decode_clock(struct par_info *hw, struct cfb_info *cfb,
 static int
 cyber2000fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 	struct par_info hw;
 	unsigned int mem;
 	int err;
@@ -861,7 +861,7 @@ cyber2000fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 
 static int cyber2000fb_set_par(struct fb_info *info)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 	struct fb_var_screeninfo *var = &cfb->fb.var;
 	struct par_info hw;
 	unsigned int mem;
@@ -971,7 +971,7 @@ static int cyber2000fb_set_par(struct fb_info *info)
 static int
 cyber2000fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 
 	if (cyber2000fb_update_start(cfb, var))
 		return -EINVAL;
@@ -1007,7 +1007,7 @@ cyber2000fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
  */
 static int cyber2000fb_blank(int blank, struct fb_info *info)
 {
-	struct cfb_info *cfb = (struct cfb_info *)info;
+	struct cfb_info *cfb = container_of(info, struct cfb_info, fb);
 	unsigned int sync = 0;
 	int i;
 
diff --git a/drivers/video/fbdev/da8xx-fb.c b/drivers/video/fbdev/da8xx-fb.c
index 788f6b37fce7..10c876c95772 100644
--- a/drivers/video/fbdev/da8xx-fb.c
+++ b/drivers/video/fbdev/da8xx-fb.c
@@ -419,7 +419,7 @@ static void lcd_cfg_horizontal_sync(int back_porch, int pulse_width,
 {
 	u32 reg;
 
-	reg = lcdc_read(LCD_RASTER_TIMING_0_REG) & 0xf;
+	reg = lcdc_read(LCD_RASTER_TIMING_0_REG) & 0x3ff;
 	reg |= (((back_porch-1) & 0xff) << 24)
 	    | (((front_porch-1) & 0xff) << 16)
 	    | (((pulse_width-1) & 0x3f) << 10);
diff --git a/drivers/video/fbdev/intelfb/intelfbhw.c b/drivers/video/fbdev/intelfb/intelfbhw.c
index fbad61da359f..d31ed4e2c46f 100644
--- a/drivers/video/fbdev/intelfb/intelfbhw.c
+++ b/drivers/video/fbdev/intelfb/intelfbhw.c
@@ -1191,7 +1191,6 @@ int intelfbhw_mode_to_hw(struct intelfb_info *dinfo,
 	vsync_end = vsync_start + var->vsync_len;
 	vtotal = vsync_end + var->upper_margin;
 	vblank_start = vactive;
-	vblank_end = vtotal;
 	vblank_end = vsync_end + 1;
 
 	DBG_MSG("V: act %d, ss %d, se %d, tot %d bs %d, be %d\n",
@@ -1859,7 +1858,7 @@ void intelfbhw_cursor_init(struct intelfb_info *dinfo)
 		tmp = INREG(CURSOR_CONTROL);
 		tmp &= ~(CURSOR_FORMAT_MASK | CURSOR_GAMMA_ENABLE |
 			 CURSOR_ENABLE | CURSOR_STRIDE_MASK);
-		tmp = CURSOR_FORMAT_3C;
+		tmp |= CURSOR_FORMAT_3C;
 		OUTREG(CURSOR_CONTROL, tmp);
 		OUTREG(CURSOR_A_BASEADDR, dinfo->cursor.offset << 12);
 		tmp = (64 << CURSOR_SIZE_H_SHIFT) |
diff --git a/drivers/video/fbdev/matrox/matroxfb_base.c b/drivers/video/fbdev/matrox/matroxfb_base.c
index 7116c5309c7d..62539ca1cfa9 100644
--- a/drivers/video/fbdev/matrox/matroxfb_base.c
+++ b/drivers/video/fbdev/matrox/matroxfb_base.c
@@ -1341,19 +1341,57 @@ struct video_board {
 	struct matrox_switch* lowlevel;
 		 };
 #ifdef CONFIG_FB_MATROX_MILLENIUM
-static struct video_board vbMillennium		= {0x0800000, 0x0800000, FB_ACCEL_MATROX_MGA2064W,	&matrox_millennium};
-static struct video_board vbMillennium2		= {0x1000000, 0x0800000, FB_ACCEL_MATROX_MGA2164W,	&matrox_millennium};
-static struct video_board vbMillennium2A	= {0x1000000, 0x0800000, FB_ACCEL_MATROX_MGA2164W_AGP,	&matrox_millennium};
+static struct video_board vbMillennium = {
+	.maxvram = 0x0800000,
+	.maxdisplayable = 0x0800000,
+	.accelID = FB_ACCEL_MATROX_MGA2064W,
+	.lowlevel = &matrox_millennium
+};
+
+static struct video_board vbMillennium2 = {
+	.maxvram = 0x1000000,
+	.maxdisplayable = 0x0800000,
+	.accelID = FB_ACCEL_MATROX_MGA2164W,
+	.lowlevel = &matrox_millennium
+};
+
+static struct video_board vbMillennium2A = {
+	.maxvram = 0x1000000,
+	.maxdisplayable = 0x0800000,
+	.accelID = FB_ACCEL_MATROX_MGA2164W_AGP,
+	.lowlevel = &matrox_millennium
+};
 #endif	/* CONFIG_FB_MATROX_MILLENIUM */
 #ifdef CONFIG_FB_MATROX_MYSTIQUE
-static struct video_board vbMystique		= {0x0800000, 0x0800000, FB_ACCEL_MATROX_MGA1064SG,	&matrox_mystique};
+static struct video_board vbMystique = {
+	.maxvram = 0x0800000,
+	.maxdisplayable = 0x0800000,
+	.accelID = FB_ACCEL_MATROX_MGA1064SG,
+	.lowlevel = &matrox_mystique
+};
 #endif	/* CONFIG_FB_MATROX_MYSTIQUE */
 #ifdef CONFIG_FB_MATROX_G
-static struct video_board vbG100		= {0x0800000, 0x0800000, FB_ACCEL_MATROX_MGAG100,	&matrox_G100};
-static struct video_board vbG200		= {0x1000000, 0x1000000, FB_ACCEL_MATROX_MGAG200,	&matrox_G100};
+static struct video_board vbG100 = {
+	.maxvram = 0x0800000,
+	.maxdisplayable = 0x0800000,
+	.accelID = FB_ACCEL_MATROX_MGAG100,
+	.lowlevel = &matrox_G100
+};
+
+static struct video_board vbG200 = {
+	.maxvram = 0x1000000,
+	.maxdisplayable = 0x1000000,
+	.accelID = FB_ACCEL_MATROX_MGAG200,
+	.lowlevel = &matrox_G100
+};
 /* from doc it looks like that accelerator can draw only to low 16MB :-( Direct accesses & displaying are OK for
    whole 32MB */
-static struct video_board vbG400		= {0x2000000, 0x1000000, FB_ACCEL_MATROX_MGAG400,	&matrox_G100};
+static struct video_board vbG400 = {
+	.maxvram = 0x2000000,
+	.maxdisplayable = 0x1000000,
+	.accelID = FB_ACCEL_MATROX_MGAG400,
+	.lowlevel = &matrox_G100
+};
 #endif
 
 #define DEVF_VIDEO64BIT		0x0001
diff --git a/drivers/video/fbdev/matrox/matroxfb_maven.c b/drivers/video/fbdev/matrox/matroxfb_maven.c
index ee41a0f276b2..bf5ce04f9aea 100644
--- a/drivers/video/fbdev/matrox/matroxfb_maven.c
+++ b/drivers/video/fbdev/matrox/matroxfb_maven.c
@@ -201,21 +201,23 @@ struct matrox_pll_ctl {
 };
 
 static const struct matrox_pll_features2 maven1000_pll = {
-	 50000000,
-	300000000,
-	 5, 128,
-	 3,  32,
-	 3
+	.vco_freq_min = 50000000,
+	.vco_freq_max = 300000000,
+	.feed_div_min = 5,
+	.feed_div_max = 128,
+	.in_div_min = 3,
+	.in_div_max = 32,
+	.post_shift_max = 3
 };
 
 static const struct matrox_pll_ctl maven_PAL = {
-	540000,
-	    50
+	.ref_freq = 540000,
+	.den = 50
 };
 
 static const struct matrox_pll_ctl maven_NTSC = {
-	450450,	/* 27027000/60 == 27000000/59.94005994 */
-	    60
+	.ref_freq = 450450,	/* 27027000/60 == 27000000/59.94005994 */
+	.den = 60
 };
 
 static int matroxfb_PLL_mavenclock(const struct matrox_pll_features2* pll,
diff --git a/drivers/video/fbdev/msm/msm_fb.c b/drivers/video/fbdev/msm/msm_fb.c
index 1374803fbcd9..2979d7e72126 100644
--- a/drivers/video/fbdev/msm/msm_fb.c
+++ b/drivers/video/fbdev/msm/msm_fb.c
@@ -569,8 +569,13 @@ static int msmfb_probe(struct platform_device *pdev)
 	mutex_init(&msmfb->panel_init_lock);
 	init_waitqueue_head(&msmfb->frame_wq);
 	INIT_WORK(&msmfb->resume_work, power_on_panel);
-	msmfb->black = kzalloc(msmfb->fb->var.bits_per_pixel*msmfb->xres,
-			       GFP_KERNEL);
+	msmfb->black = devm_kzalloc(&pdev->dev,
+				    msmfb->fb->var.bits_per_pixel*msmfb->xres,
+				    GFP_KERNEL);
+	if (!msmfb->black) {
+		ret = -ENOMEM;
+		goto error_register_framebuffer;
+	}
 
 	printk(KERN_INFO "msmfb_probe() installing %d x %d panel\n",
 	       msmfb->xres, msmfb->yres);
@@ -589,6 +594,8 @@ static int msmfb_probe(struct platform_device *pdev)
 
 	msmfb->sleeping = WAKING;
 
+	platform_set_drvdata(pdev, msmfb);
+
 	return 0;
 
 error_register_framebuffer:
@@ -598,9 +605,23 @@ error_setup_fbmem:
 	return ret;
 }
 
+static int msmfb_remove(struct platform_device *pdev)
+{
+	struct msmfb_info *msmfb;
+
+	msmfb = platform_get_drvdata(pdev);
+
+	unregister_framebuffer(msmfb->fb);
+	iounmap(msmfb->fb->screen_base);
+	framebuffer_release(msmfb->fb);
+
+	return 0;
+}
+
 static struct platform_driver msm_panel_driver = {
 	/* need to write remove */
 	.probe = msmfb_probe,
+	.remove = msmfb_remove,
 	.driver = {.name = "msm_panel"},
 };
 
diff --git a/drivers/video/fbdev/mx3fb.c b/drivers/video/fbdev/mx3fb.c
index c645a0a0c341..23ec781e9a61 100644
--- a/drivers/video/fbdev/mx3fb.c
+++ b/drivers/video/fbdev/mx3fb.c
@@ -461,8 +461,7 @@ static void sdc_disable_channel(struct mx3fb_info *mx3_fbi)
 
 	spin_unlock_irqrestore(&mx3fb->lock, flags);
 
-	mx3_fbi->txd->chan->device->device_control(mx3_fbi->txd->chan,
-						   DMA_TERMINATE_ALL, 0);
+	dmaengine_terminate_all(mx3_fbi->txd->chan);
 	mx3_fbi->txd = NULL;
 	mx3_fbi->cookie = -EINVAL;
 }
@@ -1179,7 +1178,7 @@ static int mx3fb_pan_display(struct fb_var_screeninfo *var,
 
 	/*
 	 * We enable the End of Frame interrupt, which will free a tx-descriptor,
-	 * which we will need for the next device_prep_slave_sg(). The
+	 * which we will need for the next dmaengine_prep_slave_sg(). The
 	 * IRQ-handler will disable the IRQ again.
 	 */
 	init_completion(&mx3_fbi->flip_cmpl);
diff --git a/drivers/video/fbdev/omap2/dss/dispc-compat.c b/drivers/video/fbdev/omap2/dss/dispc-compat.c
index 83779c2b292a..633c461fbc6e 100644
--- a/drivers/video/fbdev/omap2/dss/dispc-compat.c
+++ b/drivers/video/fbdev/omap2/dss/dispc-compat.c
@@ -634,13 +634,14 @@ void dispc_mgr_disable_sync(enum omap_channel channel)
 		WARN_ON(1);
 }
 
+static inline void dispc_irq_wait_handler(void *data, u32 mask)
+{
+	complete((struct completion *)data);
+}
+
 int omap_dispc_wait_for_irq_interruptible_timeout(u32 irqmask,
 		unsigned long timeout)
 {
-	void dispc_irq_wait_handler(void *data, u32 mask)
-	{
-		complete((struct completion *)data);
-	}
 
 	int r;
 	DECLARE_COMPLETION_ONSTACK(completion);
diff --git a/drivers/video/fbdev/omap2/dss/dsi.c b/drivers/video/fbdev/omap2/dss/dsi.c
index 56b92444c54f..b6f6ae1d4664 100644
--- a/drivers/video/fbdev/omap2/dss/dsi.c
+++ b/drivers/video/fbdev/omap2/dss/dsi.c
@@ -2571,7 +2571,10 @@ static int dsi_sync_vc_vp(struct platform_device *dsidev, int channel)
 {
 	struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
 	DECLARE_COMPLETION_ONSTACK(completion);
-	struct dsi_packet_sent_handler_data vp_data = { dsidev, &completion };
+	struct dsi_packet_sent_handler_data vp_data = {
+		.dsidev = dsidev,
+		.completion = &completion
+	};
 	int r = 0;
 	u8 bit;
 
@@ -2617,7 +2620,10 @@ static void dsi_packet_sent_handler_l4(void *data, u32 mask)
 static int dsi_sync_vc_l4(struct platform_device *dsidev, int channel)
 {
 	DECLARE_COMPLETION_ONSTACK(completion);
-	struct dsi_packet_sent_handler_data l4_data = { dsidev, &completion };
+	struct dsi_packet_sent_handler_data l4_data = {
+		.dsidev = dsidev,
+		.completion = &completion
+	};
 	int r = 0;
 
 	r = dsi_register_isr_vc(dsidev, channel, dsi_packet_sent_handler_l4,
diff --git a/drivers/video/fbdev/omap2/dss/manager-sysfs.c b/drivers/video/fbdev/omap2/dss/manager-sysfs.c
index 37b59fe28dc8..a7414fb12830 100644
--- a/drivers/video/fbdev/omap2/dss/manager-sysfs.c
+++ b/drivers/video/fbdev/omap2/dss/manager-sysfs.c
@@ -44,6 +44,13 @@ static ssize_t manager_display_show(struct omap_overlay_manager *mgr, char *buf)
 			dssdev->name : "<none>");
 }
 
+static int manager_display_match(struct omap_dss_device *dssdev, void *data)
+{
+	const char *str = data;
+
+	return sysfs_streq(dssdev->name, str);
+}
+
 static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
 		const char *buf, size_t size)
 {
@@ -52,17 +59,12 @@ static ssize_t manager_display_store(struct omap_overlay_manager *mgr,
 	struct omap_dss_device *dssdev = NULL;
 	struct omap_dss_device *old_dssdev;
 
-	int match(struct omap_dss_device *dssdev, void *data)
-	{
-		const char *str = data;
-		return sysfs_streq(dssdev->name, str);
-	}
-
 	if (buf[size-1] == '\n')
 		--len;
 
 	if (len > 0)
-		dssdev = omap_dss_find_device((void *)buf, match);
+		dssdev = omap_dss_find_device((void *)buf,
+			manager_display_match);
 
 	if (len > 0 && dssdev == NULL)
 		return -EINVAL;
diff --git a/drivers/video/fbdev/omap2/omapfb/omapfb-main.c b/drivers/video/fbdev/omap2/omapfb/omapfb-main.c
index ec2d132c782d..15872433e0c6 100644
--- a/drivers/video/fbdev/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/fbdev/omap2/omapfb/omapfb-main.c
@@ -273,16 +273,16 @@ static struct omapfb_colormode omapfb_colormodes[] = {
 	},
 };
 
+static bool cmp_component(struct fb_bitfield *f1, struct fb_bitfield *f2)
+{
+	return f1->length == f2->length &&
+		f1->offset == f2->offset &&
+		f1->msb_right == f2->msb_right;
+}
+
 static bool cmp_var_to_colormode(struct fb_var_screeninfo *var,
 		struct omapfb_colormode *color)
 {
-	bool cmp_component(struct fb_bitfield *f1, struct fb_bitfield *f2)
-	{
-		return f1->length == f2->length &&
-			f1->offset == f2->offset &&
-			f1->msb_right == f2->msb_right;
-	}
-
 	if (var->bits_per_pixel == 0 ||
 			var->red.length == 0 ||
 			var->blue.length == 0 ||
diff --git a/drivers/video/fbdev/pvr2fb.c b/drivers/video/fbdev/pvr2fb.c
index 167cffff3d4e..7c74f58fc101 100644
--- a/drivers/video/fbdev/pvr2fb.c
+++ b/drivers/video/fbdev/pvr2fb.c
@@ -1001,7 +1001,7 @@ static int pvr2_get_param(const struct pvr2_params *p, const char *s, int val,
 
 	for (i = 0 ; i < size ; i++ ) {
 		if (s != NULL) {
-			if (!strnicmp(p[i].name, s, strlen(s)))
+			if (!strncasecmp(p[i].name, s, strlen(s)))
 				return p[i].val;
 		} else {
 			if (p[i].val == val)
diff --git a/drivers/video/fbdev/pxafb.c b/drivers/video/fbdev/pxafb.c
index 1ecd9cec2921..a5acca88fa63 100644
--- a/drivers/video/fbdev/pxafb.c
+++ b/drivers/video/fbdev/pxafb.c
@@ -138,7 +138,7 @@ static int
 pxafb_setpalettereg(u_int regno, u_int red, u_int green, u_int blue,
 		       u_int trans, struct fb_info *info)
 {
-	struct pxafb_info *fbi = (struct pxafb_info *)info;
+	struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
 	u_int val;
 
 	if (regno >= fbi->palette_size)
@@ -183,7 +183,7 @@ static int
 pxafb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 		   u_int trans, struct fb_info *info)
 {
-	struct pxafb_info *fbi = (struct pxafb_info *)info;
+	struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
 	unsigned int val;
 	int ret = 1;
 
@@ -456,7 +456,7 @@ static int pxafb_adjust_timing(struct pxafb_info *fbi,
  */
 static int pxafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
-	struct pxafb_info *fbi = (struct pxafb_info *)info;
+	struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
 	struct pxafb_mach_info *inf = dev_get_platdata(fbi->dev);
 	int err;
 
@@ -494,7 +494,7 @@ static int pxafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
  */
 static int pxafb_set_par(struct fb_info *info)
 {
-	struct pxafb_info *fbi = (struct pxafb_info *)info;
+	struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
 	struct fb_var_screeninfo *var = &info->var;
 
 	if (var->bits_per_pixel >= 16)
@@ -533,7 +533,7 @@ static int pxafb_set_par(struct fb_info *info)
 static int pxafb_pan_display(struct fb_var_screeninfo *var,
 			     struct fb_info *info)
 {
-	struct pxafb_info *fbi = (struct pxafb_info *)info;
+	struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
 	struct fb_var_screeninfo newvar;
 	int dma = DMA_MAX + DMA_BASE;
 
@@ -566,7 +566,7 @@ static int pxafb_pan_display(struct fb_var_screeninfo *var,
  */
 static int pxafb_blank(int blank, struct fb_info *info)
 {
-	struct pxafb_info *fbi = (struct pxafb_info *)info;
+	struct pxafb_info *fbi = container_of(info, struct pxafb_info, fb);
 	int i;
 
 	switch (blank) {
@@ -725,7 +725,7 @@ static struct pxafb_layer_ops ofb_ops[] = {
 
 static int overlayfb_open(struct fb_info *info, int user)
 {
-	struct pxafb_layer *ofb = (struct pxafb_layer *)info;
+	struct pxafb_layer *ofb = container_of(info, struct pxafb_layer, fb);
 
 	/* no support for framebuffer console on overlay */
 	if (user == 0)
@@ -743,7 +743,7 @@ static int overlayfb_open(struct fb_info *info, int user)
 
 static int overlayfb_release(struct fb_info *info, int user)
 {
-	struct pxafb_layer *ofb = (struct pxafb_layer*) info;
+	struct pxafb_layer *ofb = container_of(info, struct pxafb_layer, fb);
 
 	if (ofb->usage == 1) {
 		ofb->ops->disable(ofb);
@@ -760,7 +760,7 @@ static int overlayfb_release(struct fb_info *info, int user)
 static int overlayfb_check_var(struct fb_var_screeninfo *var,
 			       struct fb_info *info)
 {
-	struct pxafb_layer *ofb = (struct pxafb_layer *)info;
+	struct pxafb_layer *ofb = container_of(info, struct pxafb_layer, fb);
 	struct fb_var_screeninfo *base_var = &ofb->fbi->fb.var;
 	int xpos, ypos, pfor, bpp;
 
@@ -836,7 +836,7 @@ static int overlayfb_check_video_memory(struct pxafb_layer *ofb)
 
 static int overlayfb_set_par(struct fb_info *info)
 {
-	struct pxafb_layer *ofb = (struct pxafb_layer *)info;
+	struct pxafb_layer *ofb = container_of(info, struct pxafb_layer, fb);
 	struct fb_var_screeninfo *var = &info->var;
 	int xpos, ypos, pfor, bpp, ret;
 
diff --git a/drivers/video/fbdev/riva/riva_hw.c b/drivers/video/fbdev/riva/riva_hw.c
index 78fdbf5178d7..8bdf37f3013b 100644
--- a/drivers/video/fbdev/riva/riva_hw.c
+++ b/drivers/video/fbdev/riva/riva_hw.c
@@ -430,7 +430,6 @@ static char nv3_arb(nv3_fifo_info * res_info, nv3_sim_state * state,  nv3_arb_in
     int mmisses, gmisses, vmisses, eburst_size, mburst_size;
     int refresh_cycle;
 
-    refresh_cycle = 0;
     refresh_cycle = 2*(state->mclk_khz/state->pclk_khz) + 5;
     mmisses = 2;
     if (state->mem_aligned) gmisses = 2;
diff --git a/drivers/video/fbdev/s3c2410fb.c b/drivers/video/fbdev/s3c2410fb.c
index 43c63a4f3178..e350eb57f11d 100644
--- a/drivers/video/fbdev/s3c2410fb.c
+++ b/drivers/video/fbdev/s3c2410fb.c
@@ -601,12 +601,12 @@ static int s3c2410fb_debug_store(struct device *dev,
 	if (len < 1)
 		return -EINVAL;
 
-	if (strnicmp(buf, "on", 2) == 0 ||
-	    strnicmp(buf, "1", 1) == 0) {
+	if (strncasecmp(buf, "on", 2) == 0 ||
+	    strncasecmp(buf, "1", 1) == 0) {
 		debug = 1;
 		dev_dbg(dev, "s3c2410fb: Debug On");
-	} else if (strnicmp(buf, "off", 3) == 0 ||
-		   strnicmp(buf, "0", 1) == 0) {
+	} else if (strncasecmp(buf, "off", 3) == 0 ||
+		   strncasecmp(buf, "0", 1) == 0) {
 		debug = 0;
 		dev_dbg(dev, "s3c2410fb: Debug Off");
 	} else {
diff --git a/drivers/video/fbdev/sa1100fb.c b/drivers/video/fbdev/sa1100fb.c
index 580c444ec301..9690216d38ff 100644
--- a/drivers/video/fbdev/sa1100fb.c
+++ b/drivers/video/fbdev/sa1100fb.c
@@ -268,7 +268,8 @@ static int
 sa1100fb_setpalettereg(u_int regno, u_int red, u_int green, u_int blue,
 		       u_int trans, struct fb_info *info)
 {
-	struct sa1100fb_info *fbi = (struct sa1100fb_info *)info;
+	struct sa1100fb_info *fbi =
+		container_of(info, struct sa1100fb_info, fb);
 	u_int val, ret = 1;
 
 	if (regno < fbi->palette_size) {
@@ -289,7 +290,8 @@ static int
 sa1100fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 		   u_int trans, struct fb_info *info)
 {
-	struct sa1100fb_info *fbi = (struct sa1100fb_info *)info;
+	struct sa1100fb_info *fbi =
+		container_of(info, struct sa1100fb_info, fb);
 	unsigned int val;
 	int ret = 1;
 
@@ -366,7 +368,8 @@ static inline unsigned int sa1100fb_display_dma_period(struct fb_var_screeninfo
 static int
 sa1100fb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
-	struct sa1100fb_info *fbi = (struct sa1100fb_info *)info;
+	struct sa1100fb_info *fbi =
+		container_of(info, struct sa1100fb_info, fb);
 	int rgbidx;
 
 	if (var->xres < MIN_XRES)
@@ -433,7 +436,8 @@ static void sa1100fb_set_visual(struct sa1100fb_info *fbi, u32 visual)
  */
 static int sa1100fb_set_par(struct fb_info *info)
 {
-	struct sa1100fb_info *fbi = (struct sa1100fb_info *)info;
+	struct sa1100fb_info *fbi =
+		container_of(info, struct sa1100fb_info, fb);
 	struct fb_var_screeninfo *var = &info->var;
 	unsigned long palette_mem_size;
 
@@ -526,7 +530,8 @@ sa1100fb_set_cmap(struct fb_cmap *cmap, int kspc, int con,
  */
 static int sa1100fb_blank(int blank, struct fb_info *info)
 {
-	struct sa1100fb_info *fbi = (struct sa1100fb_info *)info;
+	struct sa1100fb_info *fbi =
+		container_of(info, struct sa1100fb_info, fb);
 	int i;
 
 	dev_dbg(fbi->dev, "sa1100fb_blank: blank=%d\n", blank);
@@ -555,7 +560,8 @@ static int sa1100fb_blank(int blank, struct fb_info *info)
 static int sa1100fb_mmap(struct fb_info *info,
 			 struct vm_area_struct *vma)
 {
-	struct sa1100fb_info *fbi = (struct sa1100fb_info *)info;
+	struct sa1100fb_info *fbi =
+		container_of(info, struct sa1100fb_info, fb);
 	unsigned long off = vma->vm_pgoff << PAGE_SHIFT;
 
 	if (off < info->fix.smem_len) {
diff --git a/drivers/video/fbdev/sh_mobile_hdmi.c b/drivers/video/fbdev/sh_mobile_hdmi.c
index 9a33ee0413fb..7c72a3f02056 100644
--- a/drivers/video/fbdev/sh_mobile_hdmi.c
+++ b/drivers/video/fbdev/sh_mobile_hdmi.c
@@ -281,6 +281,7 @@ struct sh_hdmi {
 	u8 edid_block_addr;
 	u8 edid_segment_nr;
 	u8 edid_blocks;
+	int irq;
 	struct clk *hdmi_clk;
 	struct device *dev;
 	struct delayed_work edid_work;
@@ -1299,6 +1300,7 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 	hdmi->dev = &pdev->dev;
 	hdmi->entity.owner = THIS_MODULE;
 	hdmi->entity.ops = &sh_hdmi_ops;
+	hdmi->irq = irq;
 
 	hdmi->hdmi_clk = clk_get(&pdev->dev, "ick");
 	if (IS_ERR(hdmi->hdmi_clk)) {
@@ -1415,12 +1417,11 @@ static int __exit sh_hdmi_remove(struct platform_device *pdev)
 {
 	struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
-	int irq = platform_get_irq(pdev, 0);
 
 	snd_soc_unregister_codec(&pdev->dev);
 
 	/* No new work will be scheduled, wait for running ISR */
-	free_irq(irq, hdmi);
+	free_irq(hdmi->irq, hdmi);
 	/* Wait for already scheduled work */
 	cancel_delayed_work_sync(&hdmi->edid_work);
 	pm_runtime_put(&pdev->dev);
@@ -1435,10 +1436,49 @@ static int __exit sh_hdmi_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static int sh_hdmi_suspend(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
+
+	disable_irq(hdmi->irq);
+	/* Wait for already scheduled work */
+	cancel_delayed_work_sync(&hdmi->edid_work);
+	return 0;
+}
+
+static int sh_hdmi_resume(struct device *dev)
+{
+	struct platform_device *pdev = to_platform_device(dev);
+	struct sh_mobile_hdmi_info *pdata = dev_get_platdata(dev);
+	struct sh_hdmi *hdmi = entity_to_sh_hdmi(platform_get_drvdata(pdev));
+
+	/* Re-init interrupt polarity */
+	if (pdata->flags & HDMI_OUTPUT_PUSH_PULL)
+		hdmi_bit_set(hdmi, 0x02, 0x02, HDMI_SYSTEM_CTRL);
+
+	if (pdata->flags & HDMI_OUTPUT_POLARITY_HI)
+		hdmi_bit_set(hdmi, 0x01, 0x01, HDMI_SYSTEM_CTRL);
+
+	/* Re-init htop1 */
+	if (hdmi->htop1)
+		sh_hdmi_htop1_init(hdmi);
+
+	/* Now it's safe to enable interrupts again */
+	enable_irq(hdmi->irq);
+	return 0;
+}
+
+static const struct dev_pm_ops sh_hdmi_pm_ops = {
+	.suspend	= sh_hdmi_suspend,
+	.resume		= sh_hdmi_resume,
+};
+
 static struct platform_driver sh_hdmi_driver = {
 	.remove		= __exit_p(sh_hdmi_remove),
 	.driver = {
 		.name	= "sh-mobile-hdmi",
+		.pm	= &sh_hdmi_pm_ops,
 	},
 };
 
diff --git a/drivers/video/fbdev/sis/init301.c b/drivers/video/fbdev/sis/init301.c
index a89e3cafd5ad..295e0dedaf1f 100644
--- a/drivers/video/fbdev/sis/init301.c
+++ b/drivers/video/fbdev/sis/init301.c
@@ -1714,7 +1714,7 @@ SiS_GetLCDResInfo(struct SiS_Private *SiS_Pr, unsigned short ModeNo, unsigned sh
 			       SiS_Pr->PanelVCLKIdx315 = VCLK81_315; /* ? */
 			    } else {
 			       SiS_Pr->PanelHT   = 1688; SiS_Pr->PanelVT   =  802;
-			       SiS_Pr->PanelHRS  =   48; SiS_Pr->PanelHRS  =  112;
+			       SiS_Pr->PanelHRS  =   48; SiS_Pr->PanelHRE  =  112;
 			       SiS_Pr->PanelVRS  =    3; SiS_Pr->PanelVRE  =    6;
 			       SiS_Pr->PanelVCLKIdx300 = VCLK81_300;
 			       SiS_Pr->PanelVCLKIdx315 = VCLK81_315;
diff --git a/drivers/video/fbdev/sis/sis_main.c b/drivers/video/fbdev/sis/sis_main.c
index 3f12a2dd959a..e5d11b1892e8 100644
--- a/drivers/video/fbdev/sis/sis_main.c
+++ b/drivers/video/fbdev/sis/sis_main.c
@@ -162,7 +162,7 @@ static void sisfb_search_mode(char *name, bool quiet)
 		return;
 	}
 
-	if(!strnicmp(name, sisbios_mode[MODE_INDEX_NONE].name, strlen(name))) {
+	if(!strncasecmp(name, sisbios_mode[MODE_INDEX_NONE].name, strlen(name))) {
 		if(!quiet)
 			printk(KERN_ERR "sisfb: Mode 'none' not supported anymore. Using default.\n");
 
@@ -201,7 +201,7 @@ static void sisfb_search_mode(char *name, bool quiet)
 
 	i = 0; j = 0;
 	while(sisbios_mode[i].mode_no[0] != 0) {
-		if(!strnicmp(nameptr, sisbios_mode[i++].name, strlen(nameptr))) {
+		if(!strncasecmp(nameptr, sisbios_mode[i++].name, strlen(nameptr))) {
 			if(sisfb_fstn) {
 				if(sisbios_mode[i-1].mode_no[1] == 0x50 ||
 				   sisbios_mode[i-1].mode_no[1] == 0x56 ||
@@ -262,7 +262,7 @@ sisfb_search_crt2type(const char *name)
 	if(name == NULL) return;
 
 	while(sis_crt2type[i].type_no != -1) {
-		if(!strnicmp(name, sis_crt2type[i].name, strlen(sis_crt2type[i].name))) {
+		if(!strncasecmp(name, sis_crt2type[i].name, strlen(sis_crt2type[i].name))) {
 			sisfb_crt2type = sis_crt2type[i].type_no;
 			sisfb_tvplug = sis_crt2type[i].tvplug_no;
 			sisfb_crt2flags = sis_crt2type[i].flags;
@@ -289,7 +289,7 @@ sisfb_search_tvstd(const char *name)
 		return;
 
 	while(sis_tvtype[i].type_no != -1) {
-		if(!strnicmp(name, sis_tvtype[i].name, strlen(sis_tvtype[i].name))) {
+		if(!strncasecmp(name, sis_tvtype[i].name, strlen(sis_tvtype[i].name))) {
 			sisfb_tvstd = sis_tvtype[i].type_no;
 			break;
 		}
@@ -308,12 +308,12 @@ sisfb_search_specialtiming(const char *name)
 	if(name == NULL)
 		return;
 
-	if(!strnicmp(name, "none", 4)) {
+	if(!strncasecmp(name, "none", 4)) {
 		sisfb_specialtiming = CUT_FORCENONE;
 		printk(KERN_DEBUG "sisfb: Special timing disabled\n");
 	} else {
 		while(mycustomttable[i].chipID != 0) {
-			if(!strnicmp(name,mycustomttable[i].optionName,
+			if(!strncasecmp(name,mycustomttable[i].optionName,
 			   strlen(mycustomttable[i].optionName))) {
 				sisfb_specialtiming = mycustomttable[i].SpecialID;
 				found = true;
@@ -3952,68 +3952,68 @@ static int __init sisfb_setup(char *options)
 
 		if(!(*this_opt)) continue;
 
-		if(!strnicmp(this_opt, "off", 3)) {
+		if(!strncasecmp(this_opt, "off", 3)) {
 			sisfb_off = 1;
-		} else if(!strnicmp(this_opt, "forcecrt2type:", 14)) {
+		} else if(!strncasecmp(this_opt, "forcecrt2type:", 14)) {
 			/* Need to check crt2 type first for fstn/dstn */
 			sisfb_search_crt2type(this_opt + 14);
-		} else if(!strnicmp(this_opt, "tvmode:",7)) {
+		} else if(!strncasecmp(this_opt, "tvmode:",7)) {
 			sisfb_search_tvstd(this_opt + 7);
-		} else if(!strnicmp(this_opt, "tvstandard:",11)) {
+		} else if(!strncasecmp(this_opt, "tvstandard:",11)) {
 			sisfb_search_tvstd(this_opt + 11);
-		} else if(!strnicmp(this_opt, "mode:", 5)) {
+		} else if(!strncasecmp(this_opt, "mode:", 5)) {
 			sisfb_search_mode(this_opt + 5, false);
-		} else if(!strnicmp(this_opt, "vesa:", 5)) {
+		} else if(!strncasecmp(this_opt, "vesa:", 5)) {
 			sisfb_search_vesamode(simple_strtoul(this_opt + 5, NULL, 0), false);
-		} else if(!strnicmp(this_opt, "rate:", 5)) {
+		} else if(!strncasecmp(this_opt, "rate:", 5)) {
 			sisfb_parm_rate = simple_strtoul(this_opt + 5, NULL, 0);
-		} else if(!strnicmp(this_opt, "forcecrt1:", 10)) {
+		} else if(!strncasecmp(this_opt, "forcecrt1:", 10)) {
 			sisfb_forcecrt1 = (int)simple_strtoul(this_opt + 10, NULL, 0);
-		} else if(!strnicmp(this_opt, "mem:",4)) {
+		} else if(!strncasecmp(this_opt, "mem:",4)) {
 			sisfb_parm_mem = simple_strtoul(this_opt + 4, NULL, 0);
-		} else if(!strnicmp(this_opt, "pdc:", 4)) {
+		} else if(!strncasecmp(this_opt, "pdc:", 4)) {
 			sisfb_pdc = simple_strtoul(this_opt + 4, NULL, 0);
-		} else if(!strnicmp(this_opt, "pdc1:", 5)) {
+		} else if(!strncasecmp(this_opt, "pdc1:", 5)) {
 			sisfb_pdca = simple_strtoul(this_opt + 5, NULL, 0);
-		} else if(!strnicmp(this_opt, "noaccel", 7)) {
+		} else if(!strncasecmp(this_opt, "noaccel", 7)) {
 			sisfb_accel = 0;
-		} else if(!strnicmp(this_opt, "accel", 5)) {
+		} else if(!strncasecmp(this_opt, "accel", 5)) {
 			sisfb_accel = -1;
-		} else if(!strnicmp(this_opt, "noypan", 6)) {
+		} else if(!strncasecmp(this_opt, "noypan", 6)) {
 			sisfb_ypan = 0;
-		} else if(!strnicmp(this_opt, "ypan", 4)) {
+		} else if(!strncasecmp(this_opt, "ypan", 4)) {
 			sisfb_ypan = -1;
-		} else if(!strnicmp(this_opt, "nomax", 5)) {
+		} else if(!strncasecmp(this_opt, "nomax", 5)) {
 			sisfb_max = 0;
-		} else if(!strnicmp(this_opt, "max", 3)) {
+		} else if(!strncasecmp(this_opt, "max", 3)) {
 			sisfb_max = -1;
-		} else if(!strnicmp(this_opt, "userom:", 7)) {
+		} else if(!strncasecmp(this_opt, "userom:", 7)) {
 			sisfb_userom = (int)simple_strtoul(this_opt + 7, NULL, 0);
-		} else if(!strnicmp(this_opt, "useoem:", 7)) {
+		} else if(!strncasecmp(this_opt, "useoem:", 7)) {
 			sisfb_useoem = (int)simple_strtoul(this_opt + 7, NULL, 0);
-		} else if(!strnicmp(this_opt, "nocrt2rate", 10)) {
+		} else if(!strncasecmp(this_opt, "nocrt2rate", 10)) {
 			sisfb_nocrt2rate = 1;
-		} else if(!strnicmp(this_opt, "scalelcd:", 9)) {
+		} else if(!strncasecmp(this_opt, "scalelcd:", 9)) {
 			unsigned long temp = 2;
 			temp = simple_strtoul(this_opt + 9, NULL, 0);
 			if((temp == 0) || (temp == 1)) {
 			   sisfb_scalelcd = temp ^ 1;
 			}
-		} else if(!strnicmp(this_opt, "tvxposoffset:", 13)) {
+		} else if(!strncasecmp(this_opt, "tvxposoffset:", 13)) {
 			int temp = 0;
 			temp = (int)simple_strtol(this_opt + 13, NULL, 0);
 			if((temp >= -32) && (temp <= 32)) {
 			   sisfb_tvxposoffset = temp;
 			}
-		} else if(!strnicmp(this_opt, "tvyposoffset:", 13)) {
+		} else if(!strncasecmp(this_opt, "tvyposoffset:", 13)) {
 			int temp = 0;
 			temp = (int)simple_strtol(this_opt + 13, NULL, 0);
 			if((temp >= -32) && (temp <= 32)) {
 			   sisfb_tvyposoffset = temp;
 			}
-		} else if(!strnicmp(this_opt, "specialtiming:", 14)) {
+		} else if(!strncasecmp(this_opt, "specialtiming:", 14)) {
 			sisfb_search_specialtiming(this_opt + 14);
-		} else if(!strnicmp(this_opt, "lvdshl:", 7)) {
+		} else if(!strncasecmp(this_opt, "lvdshl:", 7)) {
 			int temp = 4;
 			temp = simple_strtoul(this_opt + 7, NULL, 0);
 			if((temp >= 0) && (temp <= 3)) {
@@ -4022,9 +4022,9 @@ static int __init sisfb_setup(char *options)
 		} else if(this_opt[0] >= '0' && this_opt[0] <= '9') {
 			sisfb_search_mode(this_opt, true);
 #if !defined(__i386__) && !defined(__x86_64__)
-		} else if(!strnicmp(this_opt, "resetcard", 9)) {
+		} else if(!strncasecmp(this_opt, "resetcard", 9)) {
 			sisfb_resetcard = 1;
-	        } else if(!strnicmp(this_opt, "videoram:", 9)) {
+	        } else if(!strncasecmp(this_opt, "videoram:", 9)) {
 			sisfb_videoram = simple_strtoul(this_opt + 9, NULL, 0);
 #endif
 		} else {
@@ -5830,7 +5830,7 @@ static int sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 			ivideo->cardnumber++;
 	}
 
-	strncpy(ivideo->myid, chipinfo->chip_name, 30);
+	strlcpy(ivideo->myid, chipinfo->chip_name, sizeof(ivideo->myid));
 
 	ivideo->warncount = 0;
 	ivideo->chip_id = pdev->device;
diff --git a/drivers/video/fbdev/sm501fb.c b/drivers/video/fbdev/sm501fb.c
index c2c8eb668784..9e74e8fbe074 100644
--- a/drivers/video/fbdev/sm501fb.c
+++ b/drivers/video/fbdev/sm501fb.c
@@ -1187,9 +1187,9 @@ static ssize_t sm501fb_crtsrc_store(struct device *dev,
 	if (len < 1)
 		return -EINVAL;
 
-	if (strnicmp(buf, "crt", 3) == 0)
+	if (strncasecmp(buf, "crt", 3) == 0)
 		head = HEAD_CRT;
-	else if (strnicmp(buf, "panel", 5) == 0)
+	else if (strncasecmp(buf, "panel", 5) == 0)
 		head = HEAD_PANEL;
 	else
 		return -EINVAL;
diff --git a/drivers/video/fbdev/stifb.c b/drivers/video/fbdev/stifb.c
index cfe8a2f905c5..86621fabbb8b 100644
--- a/drivers/video/fbdev/stifb.c
+++ b/drivers/video/fbdev/stifb.c
@@ -918,7 +918,7 @@ static int
 stifb_setcolreg(u_int regno, u_int red, u_int green,
 	      u_int blue, u_int transp, struct fb_info *info)
 {
-	struct stifb_info *fb = (struct stifb_info *) info;
+	struct stifb_info *fb = container_of(info, struct stifb_info, info);
 	u32 color;
 
 	if (regno >= NR_PALETTE)
@@ -978,7 +978,7 @@ stifb_setcolreg(u_int regno, u_int red, u_int green,
 static int
 stifb_blank(int blank_mode, struct fb_info *info)
 {
-	struct stifb_info *fb = (struct stifb_info *) info;
+	struct stifb_info *fb = container_of(info, struct stifb_info, info);
 	int enable = (blank_mode == 0) ? ENABLE : DISABLE;
 
 	switch (fb->id) {
diff --git a/drivers/video/fbdev/udlfb.c b/drivers/video/fbdev/udlfb.c
index 77b890e4d296..046d51d83d74 100644
--- a/drivers/video/fbdev/udlfb.c
+++ b/drivers/video/fbdev/udlfb.c
@@ -1528,11 +1528,8 @@ static int dlfb_parse_vendor_descriptor(struct dlfb_data *dev,
 	}
 
 	if (total_len > 5) {
-		pr_info("vendor descriptor length:%x data:%02x %02x %02x %02x" \
-			"%02x %02x %02x %02x %02x %02x %02x\n",
-			total_len, desc[0],
-			desc[1], desc[2], desc[3], desc[4], desc[5], desc[6],
-			desc[7], desc[8], desc[9], desc[10]);
+		pr_info("vendor descriptor length:%x data:%11ph\n", total_len,
+			desc);
 
 		if ((desc[0] != total_len) || /* descriptor length */
 		    (desc[1] != 0x5f) ||   /* vendor descriptor type */
diff --git a/drivers/video/fbdev/valkyriefb.c b/drivers/video/fbdev/valkyriefb.c
index 97cb9bd1d1dd..275fb98236d3 100644
--- a/drivers/video/fbdev/valkyriefb.c
+++ b/drivers/video/fbdev/valkyriefb.c
@@ -136,7 +136,8 @@ static struct fb_ops valkyriefb_ops = {
 /* Sets the video mode according to info->var */
 static int valkyriefb_set_par(struct fb_info *info)
 {
-	struct fb_info_valkyrie *p = (struct fb_info_valkyrie *) info;
+	struct fb_info_valkyrie *p =
+		container_of(info, struct fb_info_valkyrie, info);
 	volatile struct valkyrie_regs __iomem *valkyrie_regs = p->valkyrie_regs;
 	struct fb_par_valkyrie *par = info->par;
 	struct valkyrie_regvals	*init;
@@ -194,7 +195,8 @@ valkyriefb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
  */
 static int valkyriefb_blank(int blank_mode, struct fb_info *info)
 {
-	struct fb_info_valkyrie *p = (struct fb_info_valkyrie *) info;
+	struct fb_info_valkyrie *p =
+		container_of(info, struct fb_info_valkyrie, info);
 	struct fb_par_valkyrie *par = info->par;
 	struct valkyrie_regvals	*init = par->init;
 
@@ -226,7 +228,8 @@ static int valkyriefb_blank(int blank_mode, struct fb_info *info)
 static int valkyriefb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 			     u_int transp, struct fb_info *info)
 {
-	struct fb_info_valkyrie *p = (struct fb_info_valkyrie *) info;
+	struct fb_info_valkyrie *p =
+		container_of(info, struct fb_info_valkyrie, info);
 	volatile struct cmap_regs __iomem *cmap_regs = p->cmap_regs;
 	struct fb_par_valkyrie *par = info->par;
 
@@ -263,10 +266,10 @@ static inline int valkyrie_vram_reqd(int video_mode, int color_mode)
 
 static void set_valkyrie_clock(unsigned char *params)
 {
+#ifdef CONFIG_ADB_CUDA
 	struct adb_request req;
 	int i;
 
-#ifdef CONFIG_ADB_CUDA
 	for (i = 0; i < 3; ++i) {
 		cuda_request(&req, NULL, 5, CUDA_PACKET, CUDA_GET_SET_IIC,
 			     0x50, i + 1, params[i]);
@@ -465,7 +468,8 @@ static int valkyrie_var_to_par(struct fb_var_screeninfo *var,
 {
 	int vmode, cmode;
 	struct valkyrie_regvals *init;
-	struct fb_info_valkyrie *p = (struct fb_info_valkyrie *) fb_info;
+	struct fb_info_valkyrie *p =
+		container_of(fb_info, struct fb_info_valkyrie, info);
 
 	if (mac_var_to_vmode(var, &vmode, &cmode) != 0) {
 		printk(KERN_ERR "valkyriefb: can't do %dx%dx%d.\n",
diff --git a/drivers/video/fbdev/vermilion/vermilion.c b/drivers/video/fbdev/vermilion/vermilion.c
index 048a66640b03..5f930aeccf1f 100644
--- a/drivers/video/fbdev/vermilion/vermilion.c
+++ b/drivers/video/fbdev/vermilion/vermilion.c
@@ -481,7 +481,6 @@ static int vml_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 	default:
 		err = -ENODEV;
 		goto out_err_1;
-		break;
 	}
 
 	info = &vinfo->info;
diff --git a/drivers/video/fbdev/via/via-gpio.c b/drivers/video/fbdev/via/via-gpio.c
index e408679081ab..6f433b8cee12 100644
--- a/drivers/video/fbdev/via/via-gpio.c
+++ b/drivers/video/fbdev/via/via-gpio.c
@@ -270,7 +270,7 @@ static int viafb_gpio_probe(struct platform_device *platdev)
 static int viafb_gpio_remove(struct platform_device *platdev)
 {
 	unsigned long flags;
-	int ret = 0, i;
+	int i;
 
 #ifdef CONFIG_PM
 	viafb_pm_unregister(&viafb_gpio_pm_hooks);
@@ -280,11 +280,7 @@ static int viafb_gpio_remove(struct platform_device *platdev)
 	 * Get unregistered.
 	 */
 	if (viafb_gpio_config.gpio_chip.ngpio > 0) {
-		ret = gpiochip_remove(&viafb_gpio_config.gpio_chip);
-		if (ret) { /* Somebody still using it? */
-			printk(KERN_ERR "Viafb: GPIO remove failed\n");
-			return ret;
-		}
+		gpiochip_remove(&viafb_gpio_config.gpio_chip);
 	}
 	/*
 	 * Disable the ports.
@@ -294,7 +290,7 @@ static int viafb_gpio_remove(struct platform_device *platdev)
 		viafb_gpio_disable(viafb_gpio_config.active_gpios[i]);
 	viafb_gpio_config.gpio_chip.ngpio = 0;
 	spin_unlock_irqrestore(&viafb_gpio_config.vdev->reg_lock, flags);
-	return ret;
+	return 0;
 }
 
 static struct platform_driver via_gpio_driver = {
diff --git a/drivers/video/fbdev/xen-fbfront.c b/drivers/video/fbdev/xen-fbfront.c
index 901014bbc821..09dc44736c1a 100644
--- a/drivers/video/fbdev/xen-fbfront.c
+++ b/drivers/video/fbdev/xen-fbfront.c
@@ -684,12 +684,13 @@ static const struct xenbus_device_id xenfb_ids[] = {
 	{ "" }
 };
 
-static DEFINE_XENBUS_DRIVER(xenfb, ,
+static struct xenbus_driver xenfb_driver = {
+	.ids = xenfb_ids,
 	.probe = xenfb_probe,
 	.remove = xenfb_remove,
 	.resume = xenfb_resume,
 	.otherend_changed = xenfb_backend_changed,
-);
+};
 
 static int __init xenfb_init(void)
 {
diff --git a/drivers/video/of_display_timing.c b/drivers/video/of_display_timing.c
index 987edf110038..32d8275e4c88 100644
--- a/drivers/video/of_display_timing.c
+++ b/drivers/video/of_display_timing.c
@@ -233,9 +233,9 @@ struct display_timings *of_get_display_timings(struct device_node *np)
 	return disp;
 
 timingfail:
-	if (native_mode)
-		of_node_put(native_mode);
+	of_node_put(native_mode);
 	display_timings_release(disp);
+	disp = NULL;
 entryfail:
 	kfree(disp);
 dispfail:
diff --git a/drivers/virtio/Kconfig b/drivers/virtio/Kconfig
index c6683f2e396c..00b228638274 100644
--- a/drivers/virtio/Kconfig
+++ b/drivers/virtio/Kconfig
@@ -25,6 +25,7 @@ config VIRTIO_PCI
 config VIRTIO_BALLOON
 	tristate "Virtio balloon driver"
 	depends on VIRTIO
+	select MEMORY_BALLOON
 	---help---
 	 This driver supports increasing and decreasing the amount
 	 of memory within a KVM guest.
diff --git a/drivers/virtio/virtio.c b/drivers/virtio/virtio.c
index fed0ce198ae3..df598dd8c5c8 100644
--- a/drivers/virtio/virtio.c
+++ b/drivers/virtio/virtio.c
@@ -117,6 +117,43 @@ void virtio_check_driver_offered_feature(const struct virtio_device *vdev,
 }
 EXPORT_SYMBOL_GPL(virtio_check_driver_offered_feature);
 
+static void __virtio_config_changed(struct virtio_device *dev)
+{
+	struct virtio_driver *drv = drv_to_virtio(dev->dev.driver);
+
+	if (!dev->config_enabled)
+		dev->config_change_pending = true;
+	else if (drv && drv->config_changed)
+		drv->config_changed(dev);
+}
+
+void virtio_config_changed(struct virtio_device *dev)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->config_lock, flags);
+	__virtio_config_changed(dev);
+	spin_unlock_irqrestore(&dev->config_lock, flags);
+}
+EXPORT_SYMBOL_GPL(virtio_config_changed);
+
+static void virtio_config_disable(struct virtio_device *dev)
+{
+	spin_lock_irq(&dev->config_lock);
+	dev->config_enabled = false;
+	spin_unlock_irq(&dev->config_lock);
+}
+
+static void virtio_config_enable(struct virtio_device *dev)
+{
+	spin_lock_irq(&dev->config_lock);
+	dev->config_enabled = true;
+	if (dev->config_change_pending)
+		__virtio_config_changed(dev);
+	dev->config_change_pending = false;
+	spin_unlock_irq(&dev->config_lock);
+}
+
 static int virtio_dev_probe(struct device *_d)
 {
 	int err, i;
@@ -153,6 +190,8 @@ static int virtio_dev_probe(struct device *_d)
 		add_status(dev, VIRTIO_CONFIG_S_DRIVER_OK);
 		if (drv->scan)
 			drv->scan(dev);
+
+		virtio_config_enable(dev);
 	}
 
 	return err;
@@ -163,6 +202,8 @@ static int virtio_dev_remove(struct device *_d)
 	struct virtio_device *dev = dev_to_virtio(_d);
 	struct virtio_driver *drv = drv_to_virtio(dev->dev.driver);
 
+	virtio_config_disable(dev);
+
 	drv->remove(dev);
 
 	/* Driver should have reset device. */
@@ -211,6 +252,10 @@ int register_virtio_device(struct virtio_device *dev)
 	dev->index = err;
 	dev_set_name(&dev->dev, "virtio%u", dev->index);
 
+	spin_lock_init(&dev->config_lock);
+	dev->config_enabled = false;
+	dev->config_change_pending = false;
+
 	/* We always start by resetting the device, in case a previous
 	 * driver messed it up.  This also tests that code path a little. */
 	dev->config->reset(dev);
@@ -239,6 +284,64 @@ void unregister_virtio_device(struct virtio_device *dev)
 }
 EXPORT_SYMBOL_GPL(unregister_virtio_device);
 
+#ifdef CONFIG_PM_SLEEP
+int virtio_device_freeze(struct virtio_device *dev)
+{
+	struct virtio_driver *drv = drv_to_virtio(dev->dev.driver);
+
+	virtio_config_disable(dev);
+
+	dev->failed = dev->config->get_status(dev) & VIRTIO_CONFIG_S_FAILED;
+
+	if (drv && drv->freeze)
+		return drv->freeze(dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(virtio_device_freeze);
+
+int virtio_device_restore(struct virtio_device *dev)
+{
+	struct virtio_driver *drv = drv_to_virtio(dev->dev.driver);
+
+	/* We always start by resetting the device, in case a previous
+	 * driver messed it up. */
+	dev->config->reset(dev);
+
+	/* Acknowledge that we've seen the device. */
+	add_status(dev, VIRTIO_CONFIG_S_ACKNOWLEDGE);
+
+	/* Maybe driver failed before freeze.
+	 * Restore the failed status, for debugging. */
+	if (dev->failed)
+		add_status(dev, VIRTIO_CONFIG_S_FAILED);
+
+	if (!drv)
+		return 0;
+
+	/* We have a driver! */
+	add_status(dev, VIRTIO_CONFIG_S_DRIVER);
+
+	dev->config->finalize_features(dev);
+
+	if (drv->restore) {
+		int ret = drv->restore(dev);
+		if (ret) {
+			add_status(dev, VIRTIO_CONFIG_S_FAILED);
+			return ret;
+		}
+	}
+
+	/* Finally, tell the device we're all set */
+	add_status(dev, VIRTIO_CONFIG_S_DRIVER_OK);
+
+	virtio_config_enable(dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(virtio_device_restore);
+#endif
+
 static int virtio_init(void)
 {
 	if (bus_register(&virtio_bus) != 0)
diff --git a/drivers/virtio/virtio_balloon.c b/drivers/virtio/virtio_balloon.c
index 25ebe8eecdb7..c9703d4d6f67 100644
--- a/drivers/virtio/virtio_balloon.c
+++ b/drivers/virtio/virtio_balloon.c
@@ -59,7 +59,7 @@ struct virtio_balloon
 	 * Each page on this list adds VIRTIO_BALLOON_PAGES_PER_PAGE
 	 * to num_pages above.
 	 */
-	struct balloon_dev_info *vb_dev_info;
+	struct balloon_dev_info vb_dev_info;
 
 	/* Synchronize access/update to this struct virtio_balloon elements */
 	struct mutex balloon_lock;
@@ -127,7 +127,7 @@ static void set_page_pfns(u32 pfns[], struct page *page)
 
 static void fill_balloon(struct virtio_balloon *vb, size_t num)
 {
-	struct balloon_dev_info *vb_dev_info = vb->vb_dev_info;
+	struct balloon_dev_info *vb_dev_info = &vb->vb_dev_info;
 
 	/* We can only do one array worth at a time. */
 	num = min(num, ARRAY_SIZE(vb->pfns));
@@ -163,15 +163,15 @@ static void release_pages_by_pfn(const u32 pfns[], unsigned int num)
 	/* Find pfns pointing at start of each page, get pages and free them. */
 	for (i = 0; i < num; i += VIRTIO_BALLOON_PAGES_PER_PAGE) {
 		struct page *page = balloon_pfn_to_page(pfns[i]);
-		balloon_page_free(page);
 		adjust_managed_page_count(page, 1);
+		put_page(page); /* balloon reference */
 	}
 }
 
 static void leak_balloon(struct virtio_balloon *vb, size_t num)
 {
 	struct page *page;
-	struct balloon_dev_info *vb_dev_info = vb->vb_dev_info;
+	struct balloon_dev_info *vb_dev_info = &vb->vb_dev_info;
 
 	/* We can only do one array worth at a time. */
 	num = min(num, ARRAY_SIZE(vb->pfns));
@@ -353,12 +353,11 @@ static int init_vqs(struct virtio_balloon *vb)
 	return 0;
 }
 
-static const struct address_space_operations virtio_balloon_aops;
 #ifdef CONFIG_BALLOON_COMPACTION
 /*
  * virtballoon_migratepage - perform the balloon page migration on behalf of
  *			     a compation thread.     (called under page lock)
- * @mapping: the page->mapping which will be assigned to the new migrated page.
+ * @vb_dev_info: the balloon device
  * @newpage: page that will replace the isolated page after migration finishes.
  * @page   : the isolated (old) page that is about to be migrated to newpage.
  * @mode   : compaction mode -- not used for balloon page migration.
@@ -373,17 +372,13 @@ static const struct address_space_operations virtio_balloon_aops;
  * This function preforms the balloon page migration task.
  * Called through balloon_mapping->a_ops->migratepage
  */
-static int virtballoon_migratepage(struct address_space *mapping,
+static int virtballoon_migratepage(struct balloon_dev_info *vb_dev_info,
 		struct page *newpage, struct page *page, enum migrate_mode mode)
 {
-	struct balloon_dev_info *vb_dev_info = balloon_page_device(page);
-	struct virtio_balloon *vb;
+	struct virtio_balloon *vb = container_of(vb_dev_info,
+			struct virtio_balloon, vb_dev_info);
 	unsigned long flags;
 
-	BUG_ON(!vb_dev_info);
-
-	vb = vb_dev_info->balloon_device;
-
 	/*
 	 * In order to avoid lock contention while migrating pages concurrently
 	 * to leak_balloon() or fill_balloon() we just give up the balloon_lock
@@ -395,21 +390,19 @@ static int virtballoon_migratepage(struct address_space *mapping,
 	if (!mutex_trylock(&vb->balloon_lock))
 		return -EAGAIN;
 
+	get_page(newpage); /* balloon reference */
+
 	/* balloon's page migration 1st step  -- inflate "newpage" */
 	spin_lock_irqsave(&vb_dev_info->pages_lock, flags);
-	balloon_page_insert(newpage, mapping, &vb_dev_info->pages);
+	balloon_page_insert(vb_dev_info, newpage);
 	vb_dev_info->isolated_pages--;
+	__count_vm_event(BALLOON_MIGRATE);
 	spin_unlock_irqrestore(&vb_dev_info->pages_lock, flags);
 	vb->num_pfns = VIRTIO_BALLOON_PAGES_PER_PAGE;
 	set_page_pfns(vb->pfns, newpage);
 	tell_host(vb, vb->inflate_vq);
 
-	/*
-	 * balloon's page migration 2nd step -- deflate "page"
-	 *
-	 * It's safe to delete page->lru here because this page is at
-	 * an isolated migration list, and this step is expected to happen here
-	 */
+	/* balloon's page migration 2nd step -- deflate "page" */
 	balloon_page_delete(page);
 	vb->num_pfns = VIRTIO_BALLOON_PAGES_PER_PAGE;
 	set_page_pfns(vb->pfns, page);
@@ -417,20 +410,15 @@ static int virtballoon_migratepage(struct address_space *mapping,
 
 	mutex_unlock(&vb->balloon_lock);
 
-	return MIGRATEPAGE_BALLOON_SUCCESS;
-}
+	put_page(page); /* balloon reference */
 
-/* define the balloon_mapping->a_ops callback to allow balloon page migration */
-static const struct address_space_operations virtio_balloon_aops = {
-			.migratepage = virtballoon_migratepage,
-};
+	return MIGRATEPAGE_SUCCESS;
+}
 #endif /* CONFIG_BALLOON_COMPACTION */
 
 static int virtballoon_probe(struct virtio_device *vdev)
 {
 	struct virtio_balloon *vb;
-	struct address_space *vb_mapping;
-	struct balloon_dev_info *vb_devinfo;
 	int err;
 
 	vdev->priv = vb = kmalloc(sizeof(*vb), GFP_KERNEL);
@@ -446,30 +434,14 @@ static int virtballoon_probe(struct virtio_device *vdev)
 	vb->vdev = vdev;
 	vb->need_stats_update = 0;
 
-	vb_devinfo = balloon_devinfo_alloc(vb);
-	if (IS_ERR(vb_devinfo)) {
-		err = PTR_ERR(vb_devinfo);
-		goto out_free_vb;
-	}
-
-	vb_mapping = balloon_mapping_alloc(vb_devinfo,
-					   (balloon_compaction_check()) ?
-					   &virtio_balloon_aops : NULL);
-	if (IS_ERR(vb_mapping)) {
-		/*
-		 * IS_ERR(vb_mapping) && PTR_ERR(vb_mapping) == -EOPNOTSUPP
-		 * This means !CONFIG_BALLOON_COMPACTION, otherwise we get off.
-		 */
-		err = PTR_ERR(vb_mapping);
-		if (err != -EOPNOTSUPP)
-			goto out_free_vb_devinfo;
-	}
-
-	vb->vb_dev_info = vb_devinfo;
+	balloon_devinfo_init(&vb->vb_dev_info);
+#ifdef CONFIG_BALLOON_COMPACTION
+	vb->vb_dev_info.migratepage = virtballoon_migratepage;
+#endif
 
 	err = init_vqs(vb);
 	if (err)
-		goto out_free_vb_mapping;
+		goto out_free_vb;
 
 	vb->thread = kthread_run(balloon, vb, "vballoon");
 	if (IS_ERR(vb->thread)) {
@@ -481,10 +453,6 @@ static int virtballoon_probe(struct virtio_device *vdev)
 
 out_del_vqs:
 	vdev->config->del_vqs(vdev);
-out_free_vb_mapping:
-	balloon_mapping_free(vb_mapping);
-out_free_vb_devinfo:
-	balloon_devinfo_free(vb_devinfo);
 out_free_vb:
 	kfree(vb);
 out:
@@ -510,8 +478,6 @@ static void virtballoon_remove(struct virtio_device *vdev)
 
 	kthread_stop(vb->thread);
 	remove_common(vb);
-	balloon_mapping_free(vb->vb_dev_info->mapping);
-	balloon_devinfo_free(vb->vb_dev_info);
 	kfree(vb);
 }
 
@@ -538,6 +504,8 @@ static int virtballoon_restore(struct virtio_device *vdev)
 	if (ret)
 		return ret;
 
+	virtio_device_ready(vdev);
+
 	fill_balloon(vb, towards_target(vb));
 	update_balloon_size(vb);
 	return 0;
diff --git a/drivers/virtio/virtio_mmio.c b/drivers/virtio/virtio_mmio.c
index c600ccfd6922..ef9a1650bb80 100644
--- a/drivers/virtio/virtio_mmio.c
+++ b/drivers/virtio/virtio_mmio.c
@@ -234,8 +234,6 @@ static irqreturn_t vm_interrupt(int irq, void *opaque)
 {
 	struct virtio_mmio_device *vm_dev = opaque;
 	struct virtio_mmio_vq_info *info;
-	struct virtio_driver *vdrv = container_of(vm_dev->vdev.dev.driver,
-			struct virtio_driver, driver);
 	unsigned long status;
 	unsigned long flags;
 	irqreturn_t ret = IRQ_NONE;
@@ -244,9 +242,8 @@ static irqreturn_t vm_interrupt(int irq, void *opaque)
 	status = readl(vm_dev->base + VIRTIO_MMIO_INTERRUPT_STATUS);
 	writel(status, vm_dev->base + VIRTIO_MMIO_INTERRUPT_ACK);
 
-	if (unlikely(status & VIRTIO_MMIO_INT_CONFIG)
-			&& vdrv && vdrv->config_changed) {
-		vdrv->config_changed(&vm_dev->vdev);
+	if (unlikely(status & VIRTIO_MMIO_INT_CONFIG)) {
+		virtio_config_changed(&vm_dev->vdev);
 		ret = IRQ_HANDLED;
 	}
 
diff --git a/drivers/virtio/virtio_pci.c b/drivers/virtio/virtio_pci.c
index 3d1463c6b120..d34ebfa604f3 100644
--- a/drivers/virtio/virtio_pci.c
+++ b/drivers/virtio/virtio_pci.c
@@ -57,9 +57,6 @@ struct virtio_pci_device
 	/* Vectors allocated, excluding per-vq vectors if any */
 	unsigned msix_used_vectors;
 
-	/* Status saved during hibernate/restore */
-	u8 saved_status;
-
 	/* Whether we have vector per vq */
 	bool per_vq_vectors;
 };
@@ -211,12 +208,8 @@ static bool vp_notify(struct virtqueue *vq)
 static irqreturn_t vp_config_changed(int irq, void *opaque)
 {
 	struct virtio_pci_device *vp_dev = opaque;
-	struct virtio_driver *drv;
-	drv = container_of(vp_dev->vdev.dev.driver,
-			   struct virtio_driver, driver);
 
-	if (drv && drv->config_changed)
-		drv->config_changed(&vp_dev->vdev);
+	virtio_config_changed(&vp_dev->vdev);
 	return IRQ_HANDLED;
 }
 
@@ -768,16 +761,9 @@ static int virtio_pci_freeze(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct virtio_pci_device *vp_dev = pci_get_drvdata(pci_dev);
-	struct virtio_driver *drv;
 	int ret;
 
-	drv = container_of(vp_dev->vdev.dev.driver,
-			   struct virtio_driver, driver);
-
-	ret = 0;
-	vp_dev->saved_status = vp_get_status(&vp_dev->vdev);
-	if (drv && drv->freeze)
-		ret = drv->freeze(&vp_dev->vdev);
+	ret = virtio_device_freeze(&vp_dev->vdev);
 
 	if (!ret)
 		pci_disable_device(pci_dev);
@@ -788,27 +774,14 @@ static int virtio_pci_restore(struct device *dev)
 {
 	struct pci_dev *pci_dev = to_pci_dev(dev);
 	struct virtio_pci_device *vp_dev = pci_get_drvdata(pci_dev);
-	struct virtio_driver *drv;
 	int ret;
 
-	drv = container_of(vp_dev->vdev.dev.driver,
-			   struct virtio_driver, driver);
-
 	ret = pci_enable_device(pci_dev);
 	if (ret)
 		return ret;
 
 	pci_set_master(pci_dev);
-	vp_finalize_features(&vp_dev->vdev);
-
-	if (drv && drv->restore)
-		ret = drv->restore(&vp_dev->vdev);
-
-	/* Finally, tell the device we're all set */
-	if (!ret)
-		vp_set_status(&vp_dev->vdev, vp_dev->saved_status);
-
-	return ret;
+	return virtio_device_restore(&vp_dev->vdev);
 }
 
 static const struct dev_pm_ops virtio_pci_pm_ops = {
diff --git a/drivers/virtio/virtio_ring.c b/drivers/virtio/virtio_ring.c
index 4d08f45a9c29..3b1f89b6e743 100644
--- a/drivers/virtio/virtio_ring.c
+++ b/drivers/virtio/virtio_ring.c
@@ -99,36 +99,10 @@ struct vring_virtqueue
 
 #define to_vvq(_vq) container_of(_vq, struct vring_virtqueue, vq)
 
-static inline struct scatterlist *sg_next_chained(struct scatterlist *sg,
-						  unsigned int *count)
-{
-	return sg_next(sg);
-}
-
-static inline struct scatterlist *sg_next_arr(struct scatterlist *sg,
-					      unsigned int *count)
-{
-	if (--(*count) == 0)
-		return NULL;
-	return sg + 1;
-}
-
-/* Set up an indirect table of descriptors and add it to the queue. */
-static inline int vring_add_indirect(struct vring_virtqueue *vq,
-				     struct scatterlist *sgs[],
-				     struct scatterlist *(*next)
-				       (struct scatterlist *, unsigned int *),
-				     unsigned int total_sg,
-				     unsigned int total_out,
-				     unsigned int total_in,
-				     unsigned int out_sgs,
-				     unsigned int in_sgs,
-				     gfp_t gfp)
+static struct vring_desc *alloc_indirect(unsigned int total_sg, gfp_t gfp)
 {
 	struct vring_desc *desc;
-	unsigned head;
-	struct scatterlist *sg;
-	int i, n;
+	unsigned int i;
 
 	/*
 	 * We require lowmem mappings for the descriptors because
@@ -139,57 +113,16 @@ static inline int vring_add_indirect(struct vring_virtqueue *vq,
 
 	desc = kmalloc(total_sg * sizeof(struct vring_desc), gfp);
 	if (!desc)
-		return -ENOMEM;
-
-	/* Transfer entries from the sg lists into the indirect page */
-	i = 0;
-	for (n = 0; n < out_sgs; n++) {
-		for (sg = sgs[n]; sg; sg = next(sg, &total_out)) {
-			desc[i].flags = VRING_DESC_F_NEXT;
-			desc[i].addr = sg_phys(sg);
-			desc[i].len = sg->length;
-			desc[i].next = i+1;
-			i++;
-		}
-	}
-	for (; n < (out_sgs + in_sgs); n++) {
-		for (sg = sgs[n]; sg; sg = next(sg, &total_in)) {
-			desc[i].flags = VRING_DESC_F_NEXT|VRING_DESC_F_WRITE;
-			desc[i].addr = sg_phys(sg);
-			desc[i].len = sg->length;
-			desc[i].next = i+1;
-			i++;
-		}
-	}
-	BUG_ON(i != total_sg);
-
-	/* Last one doesn't continue. */
-	desc[i-1].flags &= ~VRING_DESC_F_NEXT;
-	desc[i-1].next = 0;
-
-	/* We're about to use a buffer */
-	vq->vq.num_free--;
-
-	/* Use a single buffer which doesn't continue */
-	head = vq->free_head;
-	vq->vring.desc[head].flags = VRING_DESC_F_INDIRECT;
-	vq->vring.desc[head].addr = virt_to_phys(desc);
-	/* kmemleak gives a false positive, as it's hidden by virt_to_phys */
-	kmemleak_ignore(desc);
-	vq->vring.desc[head].len = i * sizeof(struct vring_desc);
-
-	/* Update free pointer */
-	vq->free_head = vq->vring.desc[head].next;
+		return NULL;
 
-	return head;
+	for (i = 0; i < total_sg; i++)
+		desc[i].next = i+1;
+	return desc;
 }
 
 static inline int virtqueue_add(struct virtqueue *_vq,
 				struct scatterlist *sgs[],
-				struct scatterlist *(*next)
-				  (struct scatterlist *, unsigned int *),
-				unsigned int total_out,
-				unsigned int total_in,
+				unsigned int total_sg,
 				unsigned int out_sgs,
 				unsigned int in_sgs,
 				void *data,
@@ -197,8 +130,10 @@ static inline int virtqueue_add(struct virtqueue *_vq,
 {
 	struct vring_virtqueue *vq = to_vvq(_vq);
 	struct scatterlist *sg;
-	unsigned int i, n, avail, uninitialized_var(prev), total_sg;
+	struct vring_desc *desc;
+	unsigned int i, n, avail, descs_used, uninitialized_var(prev);
 	int head;
+	bool indirect;
 
 	START_USE(vq);
 
@@ -222,24 +157,40 @@ static inline int virtqueue_add(struct virtqueue *_vq,
 	}
 #endif
 
-	total_sg = total_in + total_out;
+	BUG_ON(total_sg > vq->vring.num);
+	BUG_ON(total_sg == 0);
+
+	head = vq->free_head;
 
 	/* If the host supports indirect descriptor tables, and we have multiple
 	 * buffers, then go indirect. FIXME: tune this threshold */
-	if (vq->indirect && total_sg > 1 && vq->vq.num_free) {
-		head = vring_add_indirect(vq, sgs, next, total_sg, total_out,
-					  total_in,
-					  out_sgs, in_sgs, gfp);
-		if (likely(head >= 0))
-			goto add_head;
+	if (vq->indirect && total_sg > 1 && vq->vq.num_free)
+		desc = alloc_indirect(total_sg, gfp);
+	else
+		desc = NULL;
+
+	if (desc) {
+		/* Use a single buffer which doesn't continue */
+		vq->vring.desc[head].flags = VRING_DESC_F_INDIRECT;
+		vq->vring.desc[head].addr = virt_to_phys(desc);
+		/* avoid kmemleak false positive (hidden by virt_to_phys) */
+		kmemleak_ignore(desc);
+		vq->vring.desc[head].len = total_sg * sizeof(struct vring_desc);
+
+		/* Set up rest to use this indirect table. */
+		i = 0;
+		descs_used = 1;
+		indirect = true;
+	} else {
+		desc = vq->vring.desc;
+		i = head;
+		descs_used = total_sg;
+		indirect = false;
 	}
 
-	BUG_ON(total_sg > vq->vring.num);
-	BUG_ON(total_sg == 0);
-
-	if (vq->vq.num_free < total_sg) {
+	if (vq->vq.num_free < descs_used) {
 		pr_debug("Can't add buf len %i - avail = %i\n",
-			 total_sg, vq->vq.num_free);
+			 descs_used, vq->vq.num_free);
 		/* FIXME: for historical reasons, we force a notify here if
 		 * there are outgoing parts to the buffer.  Presumably the
 		 * host should service the ring ASAP. */
@@ -250,34 +201,35 @@ static inline int virtqueue_add(struct virtqueue *_vq,
 	}
 
 	/* We're about to use some buffers from the free list. */
-	vq->vq.num_free -= total_sg;
+	vq->vq.num_free -= descs_used;
 
-	head = i = vq->free_head;
 	for (n = 0; n < out_sgs; n++) {
-		for (sg = sgs[n]; sg; sg = next(sg, &total_out)) {
-			vq->vring.desc[i].flags = VRING_DESC_F_NEXT;
-			vq->vring.desc[i].addr = sg_phys(sg);
-			vq->vring.desc[i].len = sg->length;
+		for (sg = sgs[n]; sg; sg = sg_next(sg)) {
+			desc[i].flags = VRING_DESC_F_NEXT;
+			desc[i].addr = sg_phys(sg);
+			desc[i].len = sg->length;
 			prev = i;
-			i = vq->vring.desc[i].next;
+			i = desc[i].next;
 		}
 	}
 	for (; n < (out_sgs + in_sgs); n++) {
-		for (sg = sgs[n]; sg; sg = next(sg, &total_in)) {
-			vq->vring.desc[i].flags = VRING_DESC_F_NEXT|VRING_DESC_F_WRITE;
-			vq->vring.desc[i].addr = sg_phys(sg);
-			vq->vring.desc[i].len = sg->length;
+		for (sg = sgs[n]; sg; sg = sg_next(sg)) {
+			desc[i].flags = VRING_DESC_F_NEXT|VRING_DESC_F_WRITE;
+			desc[i].addr = sg_phys(sg);
+			desc[i].len = sg->length;
 			prev = i;
-			i = vq->vring.desc[i].next;
+			i = desc[i].next;
 		}
 	}
 	/* Last one doesn't continue. */
-	vq->vring.desc[prev].flags &= ~VRING_DESC_F_NEXT;
+	desc[prev].flags &= ~VRING_DESC_F_NEXT;
 
 	/* Update free pointer */
-	vq->free_head = i;
+	if (indirect)
+		vq->free_head = vq->vring.desc[head].next;
+	else
+		vq->free_head = i;
 
-add_head:
 	/* Set token. */
 	vq->data[head] = data;
 
@@ -324,29 +276,23 @@ int virtqueue_add_sgs(struct virtqueue *_vq,
 		      void *data,
 		      gfp_t gfp)
 {
-	unsigned int i, total_out, total_in;
+	unsigned int i, total_sg = 0;
 
 	/* Count them first. */
-	for (i = total_out = total_in = 0; i < out_sgs; i++) {
-		struct scatterlist *sg;
-		for (sg = sgs[i]; sg; sg = sg_next(sg))
-			total_out++;
-	}
-	for (; i < out_sgs + in_sgs; i++) {
+	for (i = 0; i < out_sgs + in_sgs; i++) {
 		struct scatterlist *sg;
 		for (sg = sgs[i]; sg; sg = sg_next(sg))
-			total_in++;
+			total_sg++;
 	}
-	return virtqueue_add(_vq, sgs, sg_next_chained,
-			     total_out, total_in, out_sgs, in_sgs, data, gfp);
+	return virtqueue_add(_vq, sgs, total_sg, out_sgs, in_sgs, data, gfp);
 }
 EXPORT_SYMBOL_GPL(virtqueue_add_sgs);
 
 /**
  * virtqueue_add_outbuf - expose output buffers to other end
  * @vq: the struct virtqueue we're talking about.
- * @sgs: array of scatterlists (need not be terminated!)
- * @num: the number of scatterlists readable by other side
+ * @sg: scatterlist (must be well-formed and terminated!)
+ * @num: the number of entries in @sg readable by other side
  * @data: the token identifying the buffer.
  * @gfp: how to do memory allocations (if necessary).
  *
@@ -356,19 +302,19 @@ EXPORT_SYMBOL_GPL(virtqueue_add_sgs);
  * Returns zero or a negative error (ie. ENOSPC, ENOMEM, EIO).
  */
 int virtqueue_add_outbuf(struct virtqueue *vq,
-			 struct scatterlist sg[], unsigned int num,
+			 struct scatterlist *sg, unsigned int num,
 			 void *data,
 			 gfp_t gfp)
 {
-	return virtqueue_add(vq, &sg, sg_next_arr, num, 0, 1, 0, data, gfp);
+	return virtqueue_add(vq, &sg, num, 1, 0, data, gfp);
 }
 EXPORT_SYMBOL_GPL(virtqueue_add_outbuf);
 
 /**
  * virtqueue_add_inbuf - expose input buffers to other end
  * @vq: the struct virtqueue we're talking about.
- * @sgs: array of scatterlists (need not be terminated!)
- * @num: the number of scatterlists writable by other side
+ * @sg: scatterlist (must be well-formed and terminated!)
+ * @num: the number of entries in @sg writable by other side
  * @data: the token identifying the buffer.
  * @gfp: how to do memory allocations (if necessary).
  *
@@ -378,11 +324,11 @@ EXPORT_SYMBOL_GPL(virtqueue_add_outbuf);
  * Returns zero or a negative error (ie. ENOSPC, ENOMEM, EIO).
  */
 int virtqueue_add_inbuf(struct virtqueue *vq,
-			struct scatterlist sg[], unsigned int num,
+			struct scatterlist *sg, unsigned int num,
 			void *data,
 			gfp_t gfp)
 {
-	return virtqueue_add(vq, &sg, sg_next_arr, 0, num, 0, 1, data, gfp);
+	return virtqueue_add(vq, &sg, num, 0, 1, data, gfp);
 }
 EXPORT_SYMBOL_GPL(virtqueue_add_inbuf);
 
diff --git a/drivers/watchdog/Kconfig b/drivers/watchdog/Kconfig
index f57312fced80..d0107d424ee4 100644
--- a/drivers/watchdog/Kconfig
+++ b/drivers/watchdog/Kconfig
@@ -87,6 +87,15 @@ config DA9055_WATCHDOG
 	  This driver can also be built as a module.  If so, the module
 	  will be called da9055_wdt.
 
+config DA9063_WATCHDOG
+	tristate "Dialog DA9063 Watchdog"
+	depends on MFD_DA9063
+	select WATCHDOG_CORE
+	help
+	  Support for the watchdog in the DA9063 PMIC.
+
+	  This driver can be built as a module. The module name is da9063_wdt.
+
 config GPIO_WATCHDOG
 	tristate "Watchdog device controlled through GPIO-line"
 	depends on OF_GPIO
@@ -95,6 +104,16 @@ config GPIO_WATCHDOG
 	  If you say yes here you get support for watchdog device
 	  controlled through GPIO-line.
 
+config MENF21BMC_WATCHDOG
+	tristate "MEN 14F021P00 BMC Watchdog"
+	depends on MFD_MENF21BMC
+	select WATCHDOG_CORE
+	help
+	  Say Y here to include support for the MEN 14F021P00 BMC Watchdog.
+
+	  This driver can also be built as a module. If so the module
+	  will be called menf21bmc_wdt.
+
 config WM831X_WATCHDOG
 	tristate "WM831x watchdog"
 	depends on MFD_WM831X
@@ -113,6 +132,7 @@ config WM8350_WATCHDOG
 
 config XILINX_WATCHDOG
 	tristate "Xilinx Watchdog timer"
+	depends on HAS_IOMEM
 	select WATCHDOG_CORE
 	help
 	  Watchdog driver for the xps_timebase_wdt ip core.
@@ -147,6 +167,14 @@ config AT91SAM9X_WATCHDOG
 	  Watchdog timer embedded into AT91SAM9X and AT91CAP9 chips. This will
 	  reboot your system when the timeout is reached.
 
+config CADENCE_WATCHDOG
+	tristate "Cadence Watchdog Timer"
+	depends on ARM
+	select WATCHDOG_CORE
+	help
+	  Say Y here if you want to include support for the watchdog
+	  timer in the Xilinx Zynq.
+
 config 21285_WATCHDOG
 	tristate "DC21285 watchdog"
 	depends on FOOTBRIDGE
@@ -301,7 +329,7 @@ config DAVINCI_WATCHDOG
 
 config ORION_WATCHDOG
 	tristate "Orion watchdog"
-	depends on ARCH_ORION5X || ARCH_KIRKWOOD || ARCH_DOVE || MACH_DOVE || ARCH_MVEBU
+	depends on ARCH_ORION5X || ARCH_DOVE || MACH_DOVE || ARCH_MVEBU
 	select WATCHDOG_CORE
 	help
 	  Say Y here if to include support for the watchdog timer
@@ -309,6 +337,17 @@ config ORION_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called orion_wdt.
 
+config RN5T618_WATCHDOG
+	tristate "Ricoh RN5T618 watchdog"
+	depends on MFD_RN5T618
+	select WATCHDOG_CORE
+	help
+	  If you say yes here you get support for watchdog on the Ricoh
+	  RN5T618 PMIC.
+
+	  This driver can also be built as a module.  If so, the module
+	  will be called rn5t618_wdt.
+
 config SUNXI_WATCHDOG
 	tristate "Allwinner SoCs watchdog support"
 	depends on ARCH_SUNXI
@@ -434,7 +473,7 @@ config SIRFSOC_WATCHDOG
 
 config TEGRA_WATCHDOG
 	tristate "Tegra watchdog"
-	depends on ARCH_TEGRA || COMPILE_TEST
+	depends on (ARCH_TEGRA || COMPILE_TEST) && HAS_IOMEM
 	select WATCHDOG_CORE
 	help
 	  Say Y here to include support for the watchdog timer
@@ -443,6 +482,29 @@ config TEGRA_WATCHDOG
 	  To compile this driver as a module, choose M here: the
 	  module will be called tegra_wdt.
 
+config QCOM_WDT
+	tristate "QCOM watchdog"
+	depends on HAS_IOMEM
+	depends on ARCH_QCOM
+	select WATCHDOG_CORE
+	help
+	  Say Y here to include Watchdog timer support for the watchdog found
+	  on QCOM chipsets.  Currently supported targets are the MSM8960,
+	  APQ8064, and IPQ8064.
+
+	  To compile this driver as a module, choose M here: the
+	  module will be called qcom_wdt.
+
+config MESON_WATCHDOG
+	tristate "Amlogic Meson SoCs watchdog support"
+	depends on ARCH_MESON
+	select WATCHDOG_CORE
+	help
+	  Say Y here to include support for the watchdog timer
+	  in Amlogic Meson SoCs.
+	  To compile this driver as a module, choose M here: the
+	  module will be called meson_wdt.
+
 # AVR32 Architecture
 
 config AT32AP700X_WDT
diff --git a/drivers/watchdog/Makefile b/drivers/watchdog/Makefile
index 468c3204c3b1..c569ec8f8a76 100644
--- a/drivers/watchdog/Makefile
+++ b/drivers/watchdog/Makefile
@@ -32,6 +32,7 @@ obj-$(CONFIG_USBPCWATCHDOG) += pcwd_usb.o
 obj-$(CONFIG_ARM_SP805_WATCHDOG) += sp805_wdt.o
 obj-$(CONFIG_AT91RM9200_WATCHDOG) += at91rm9200_wdt.o
 obj-$(CONFIG_AT91SAM9X_WATCHDOG) += at91sam9_wdt.o
+obj-$(CONFIG_CADENCE_WATCHDOG) += cadence_wdt.o
 obj-$(CONFIG_OMAP_WATCHDOG) += omap_wdt.o
 obj-$(CONFIG_TWL4030_WATCHDOG) += twl4030_wdt.o
 obj-$(CONFIG_21285_WATCHDOG) += wdt285.o
@@ -47,6 +48,7 @@ obj-$(CONFIG_IOP_WATCHDOG) += iop_wdt.o
 obj-$(CONFIG_DAVINCI_WATCHDOG) += davinci_wdt.o
 obj-$(CONFIG_ORION_WATCHDOG) += orion_wdt.o
 obj-$(CONFIG_SUNXI_WATCHDOG) += sunxi_wdt.o
+obj-$(CONFIG_RN5T618_WATCHDOG) += rn5t618_wdt.o
 obj-$(CONFIG_COH901327_WATCHDOG) += coh901327_wdt.o
 obj-$(CONFIG_STMP3XXX_RTC_WATCHDOG) += stmp3xxx_rtc_wdt.o
 obj-$(CONFIG_NUC900_WATCHDOG) += nuc900_wdt.o
@@ -57,8 +59,10 @@ obj-$(CONFIG_RETU_WATCHDOG) += retu_wdt.o
 obj-$(CONFIG_BCM2835_WDT) += bcm2835_wdt.o
 obj-$(CONFIG_MOXART_WDT) += moxart_wdt.o
 obj-$(CONFIG_SIRFSOC_WATCHDOG) += sirfsoc_wdt.o
+obj-$(CONFIG_QCOM_WDT) += qcom-wdt.o
 obj-$(CONFIG_BCM_KONA_WDT) += bcm_kona_wdt.o
 obj-$(CONFIG_TEGRA_WATCHDOG) += tegra_wdt.o
+obj-$(CONFIG_MESON_WATCHDOG) += meson_wdt.o
 
 # AVR32 Architecture
 obj-$(CONFIG_AT32AP700X_WDT) += at32ap700x_wdt.o
@@ -173,8 +177,10 @@ obj-$(CONFIG_XEN_WDT) += xen_wdt.o
 # Architecture Independent
 obj-$(CONFIG_DA9052_WATCHDOG) += da9052_wdt.o
 obj-$(CONFIG_DA9055_WATCHDOG) += da9055_wdt.o
+obj-$(CONFIG_DA9063_WATCHDOG) += da9063_wdt.o
 obj-$(CONFIG_GPIO_WATCHDOG)	+= gpio_wdt.o
 obj-$(CONFIG_WM831X_WATCHDOG) += wm831x_wdt.o
 obj-$(CONFIG_WM8350_WATCHDOG) += wm8350_wdt.o
 obj-$(CONFIG_MAX63XX_WATCHDOG) += max63xx_wdt.o
 obj-$(CONFIG_SOFT_WATCHDOG) += softdog.o
+obj-$(CONFIG_MENF21BMC_WATCHDOG) += menf21bmc_wdt.o
diff --git a/drivers/watchdog/alim7101_wdt.c b/drivers/watchdog/alim7101_wdt.c
index 996b2f7d330e..665e0e7dfe1e 100644
--- a/drivers/watchdog/alim7101_wdt.c
+++ b/drivers/watchdog/alim7101_wdt.c
@@ -301,6 +301,28 @@ static struct miscdevice wdt_miscdev = {
 	.fops	=	&wdt_fops,
 };
 
+static int wdt_restart_handle(struct notifier_block *this, unsigned long mode,
+			      void *cmd)
+{
+	/*
+	 * Cobalt devices have no way of rebooting themselves other
+	 * than getting the watchdog to pull reset, so we restart the
+	 * watchdog on reboot with no heartbeat.
+	 */
+	wdt_change(WDT_ENABLE);
+
+	/* loop until the watchdog fires */
+	while (true)
+		;
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block wdt_restart_handler = {
+	.notifier_call = wdt_restart_handle,
+	.priority = 128,
+};
+
 /*
  *	Notifier for system down
  */
@@ -311,15 +333,6 @@ static int wdt_notify_sys(struct notifier_block *this,
 	if (code == SYS_DOWN || code == SYS_HALT)
 		wdt_turnoff();
 
-	if (code == SYS_RESTART) {
-		/*
-		 * Cobalt devices have no way of rebooting themselves other
-		 * than getting the watchdog to pull reset, so we restart the
-		 * watchdog on reboot with no heartbeat
-		 */
-		wdt_change(WDT_ENABLE);
-		pr_info("Watchdog timer is now enabled with no heartbeat - should reboot in ~1 second\n");
-	}
 	return NOTIFY_DONE;
 }
 
@@ -338,6 +351,7 @@ static void __exit alim7101_wdt_unload(void)
 	/* Deregister */
 	misc_deregister(&wdt_miscdev);
 	unregister_reboot_notifier(&wdt_notifier);
+	unregister_restart_handler(&wdt_restart_handler);
 	pci_dev_put(alim7101_pmu);
 }
 
@@ -390,11 +404,17 @@ static int __init alim7101_wdt_init(void)
 		goto err_out;
 	}
 
+	rc = register_restart_handler(&wdt_restart_handler);
+	if (rc) {
+		pr_err("cannot register restart handler (err=%d)\n", rc);
+		goto err_out_reboot;
+	}
+
 	rc = misc_register(&wdt_miscdev);
 	if (rc) {
 		pr_err("cannot register miscdev on minor=%d (err=%d)\n",
 		       wdt_miscdev.minor, rc);
-		goto err_out_reboot;
+		goto err_out_restart;
 	}
 
 	if (nowayout)
@@ -404,6 +424,8 @@ static int __init alim7101_wdt_init(void)
 		timeout, nowayout);
 	return 0;
 
+err_out_restart:
+	unregister_restart_handler(&wdt_restart_handler);
 err_out_reboot:
 	unregister_reboot_notifier(&wdt_notifier);
 err_out:
diff --git a/drivers/watchdog/booke_wdt.c b/drivers/watchdog/booke_wdt.c
index 08a785398eac..e96b09b135c8 100644
--- a/drivers/watchdog/booke_wdt.c
+++ b/drivers/watchdog/booke_wdt.c
@@ -30,8 +30,6 @@
  * occur, and the final time the board will reset.
  */
 
-u32 booke_wdt_enabled;
-u32 booke_wdt_period = CONFIG_BOOKE_WDT_DEFAULT_TIMEOUT;
 
 #ifdef	CONFIG_PPC_FSL_BOOK3E
 #define WDTP(x)		((((x)&0x3)<<30)|(((x)&0x3c)<<15))
@@ -41,27 +39,10 @@ u32 booke_wdt_period = CONFIG_BOOKE_WDT_DEFAULT_TIMEOUT;
 #define WDTP_MASK	(TCR_WP_MASK)
 #endif
 
-/* Checks wdt=x and wdt_period=xx command-line option */
-notrace int __init early_parse_wdt(char *p)
-{
-	if (p && strncmp(p, "0", 1) != 0)
-		booke_wdt_enabled = 1;
-
-	return 0;
-}
-early_param("wdt", early_parse_wdt);
-
-int __init early_parse_wdt_period(char *p)
-{
-	unsigned long ret;
-	if (p) {
-		if (!kstrtol(p, 0, &ret))
-			booke_wdt_period = ret;
-	}
-
-	return 0;
-}
-early_param("wdt_period", early_parse_wdt_period);
+static bool booke_wdt_enabled;
+module_param(booke_wdt_enabled, bool, 0);
+static int  booke_wdt_period = CONFIG_BOOKE_WDT_DEFAULT_TIMEOUT;
+module_param(booke_wdt_period, int, 0);
 
 #ifdef CONFIG_PPC_FSL_BOOK3E
 
@@ -259,5 +240,6 @@ static int __init booke_wdt_init(void)
 module_init(booke_wdt_init);
 module_exit(booke_wdt_exit);
 
+MODULE_ALIAS("booke_wdt");
 MODULE_DESCRIPTION("PowerPC Book-E watchdog driver");
 MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/cadence_wdt.c b/drivers/watchdog/cadence_wdt.c
new file mode 100644
index 000000000000..5927c0a98a74
--- /dev/null
+++ b/drivers/watchdog/cadence_wdt.c
@@ -0,0 +1,516 @@
+/*
+ * Cadence WDT driver - Used by Xilinx Zynq
+ *
+ * Copyright (C) 2010 - 2014 Xilinx, Inc.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/init.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/watchdog.h>
+
+#define CDNS_WDT_DEFAULT_TIMEOUT	10
+/* Supports 1 - 516 sec */
+#define CDNS_WDT_MIN_TIMEOUT	1
+#define CDNS_WDT_MAX_TIMEOUT	516
+
+/* Restart key */
+#define CDNS_WDT_RESTART_KEY 0x00001999
+
+/* Counter register access key */
+#define CDNS_WDT_REGISTER_ACCESS_KEY 0x00920000
+
+/* Counter value divisor */
+#define CDNS_WDT_COUNTER_VALUE_DIVISOR 0x1000
+
+/* Clock prescaler value and selection */
+#define CDNS_WDT_PRESCALE_64	64
+#define CDNS_WDT_PRESCALE_512	512
+#define CDNS_WDT_PRESCALE_4096	4096
+#define CDNS_WDT_PRESCALE_SELECT_64	1
+#define CDNS_WDT_PRESCALE_SELECT_512	2
+#define CDNS_WDT_PRESCALE_SELECT_4096	3
+
+/* Input clock frequency */
+#define CDNS_WDT_CLK_10MHZ	10000000
+#define CDNS_WDT_CLK_75MHZ	75000000
+
+/* Counter maximum value */
+#define CDNS_WDT_COUNTER_MAX 0xFFF
+
+static int wdt_timeout = CDNS_WDT_DEFAULT_TIMEOUT;
+static int nowayout = WATCHDOG_NOWAYOUT;
+
+module_param(wdt_timeout, int, 0);
+MODULE_PARM_DESC(wdt_timeout,
+		 "Watchdog time in seconds. (default="
+		 __MODULE_STRING(CDNS_WDT_DEFAULT_TIMEOUT) ")");
+
+module_param(nowayout, int, 0);
+MODULE_PARM_DESC(nowayout,
+		 "Watchdog cannot be stopped once started (default="
+		 __MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+/**
+ * struct cdns_wdt - Watchdog device structure
+ * @regs: baseaddress of device
+ * @rst: reset flag
+ * @clk: struct clk * of a clock source
+ * @prescaler: for saving prescaler value
+ * @ctrl_clksel: counter clock prescaler selection
+ * @io_lock: spinlock for IO register access
+ * @cdns_wdt_device: watchdog device structure
+ * @cdns_wdt_notifier: notifier structure
+ *
+ * Structure containing parameters specific to cadence watchdog.
+ */
+struct cdns_wdt {
+	void __iomem		*regs;
+	bool			rst;
+	struct clk		*clk;
+	u32			prescaler;
+	u32			ctrl_clksel;
+	spinlock_t		io_lock;
+	struct watchdog_device	cdns_wdt_device;
+	struct notifier_block	cdns_wdt_notifier;
+};
+
+/* Write access to Registers */
+static inline void cdns_wdt_writereg(struct cdns_wdt *wdt, u32 offset, u32 val)
+{
+	writel_relaxed(val, wdt->regs + offset);
+}
+
+/*************************Register Map**************************************/
+
+/* Register Offsets for the WDT */
+#define CDNS_WDT_ZMR_OFFSET	0x0	/* Zero Mode Register */
+#define CDNS_WDT_CCR_OFFSET	0x4	/* Counter Control Register */
+#define CDNS_WDT_RESTART_OFFSET	0x8	/* Restart Register */
+#define CDNS_WDT_SR_OFFSET	0xC	/* Status Register */
+
+/*
+ * Zero Mode Register - This register controls how the time out is indicated
+ * and also contains the access code to allow writes to the register (0xABC).
+ */
+#define CDNS_WDT_ZMR_WDEN_MASK	0x00000001 /* Enable the WDT */
+#define CDNS_WDT_ZMR_RSTEN_MASK	0x00000002 /* Enable the reset output */
+#define CDNS_WDT_ZMR_IRQEN_MASK	0x00000004 /* Enable IRQ output */
+#define CDNS_WDT_ZMR_RSTLEN_16	0x00000030 /* Reset pulse of 16 pclk cycles */
+#define CDNS_WDT_ZMR_ZKEY_VAL	0x00ABC000 /* Access key, 0xABC << 12 */
+/*
+ * Counter Control register - This register controls how fast the timer runs
+ * and the reset value and also contains the access code to allow writes to
+ * the register.
+ */
+#define CDNS_WDT_CCR_CRV_MASK	0x00003FFC /* Counter reset value */
+
+/**
+ * cdns_wdt_stop - Stop the watchdog.
+ *
+ * @wdd: watchdog device
+ *
+ * Read the contents of the ZMR register, clear the WDEN bit
+ * in the register and set the access key for successful write.
+ *
+ * Return: always 0
+ */
+static int cdns_wdt_stop(struct watchdog_device *wdd)
+{
+	struct cdns_wdt *wdt = watchdog_get_drvdata(wdd);
+
+	spin_lock(&wdt->io_lock);
+	cdns_wdt_writereg(wdt, CDNS_WDT_ZMR_OFFSET,
+			  CDNS_WDT_ZMR_ZKEY_VAL & (~CDNS_WDT_ZMR_WDEN_MASK));
+	spin_unlock(&wdt->io_lock);
+
+	return 0;
+}
+
+/**
+ * cdns_wdt_reload - Reload the watchdog timer (i.e. pat the watchdog).
+ *
+ * @wdd: watchdog device
+ *
+ * Write the restart key value (0x00001999) to the restart register.
+ *
+ * Return: always 0
+ */
+static int cdns_wdt_reload(struct watchdog_device *wdd)
+{
+	struct cdns_wdt *wdt = watchdog_get_drvdata(wdd);
+
+	spin_lock(&wdt->io_lock);
+	cdns_wdt_writereg(wdt, CDNS_WDT_RESTART_OFFSET,
+			  CDNS_WDT_RESTART_KEY);
+	spin_unlock(&wdt->io_lock);
+
+	return 0;
+}
+
+/**
+ * cdns_wdt_start - Enable and start the watchdog.
+ *
+ * @wdd: watchdog device
+ *
+ * The counter value is calculated according to the formula:
+ *		calculated count = (timeout * clock) / prescaler + 1.
+ * The calculated count is divided by 0x1000 to obtain the field value
+ * to write to counter control register.
+ * Clears the contents of prescaler and counter reset value. Sets the
+ * prescaler to 4096 and the calculated count and access key
+ * to write to CCR Register.
+ * Sets the WDT (WDEN bit) and either the Reset signal(RSTEN bit)
+ * or Interrupt signal(IRQEN) with a specified cycles and the access
+ * key to write to ZMR Register.
+ *
+ * Return: always 0
+ */
+static int cdns_wdt_start(struct watchdog_device *wdd)
+{
+	struct cdns_wdt *wdt = watchdog_get_drvdata(wdd);
+	unsigned int data = 0;
+	unsigned short count;
+	unsigned long clock_f = clk_get_rate(wdt->clk);
+
+	/*
+	 * Counter value divisor to obtain the value of
+	 * counter reset to be written to control register.
+	 */
+	count = (wdd->timeout * (clock_f / wdt->prescaler)) /
+		 CDNS_WDT_COUNTER_VALUE_DIVISOR + 1;
+
+	if (count > CDNS_WDT_COUNTER_MAX)
+		count = CDNS_WDT_COUNTER_MAX;
+
+	spin_lock(&wdt->io_lock);
+	cdns_wdt_writereg(wdt, CDNS_WDT_ZMR_OFFSET,
+			  CDNS_WDT_ZMR_ZKEY_VAL);
+
+	count = (count << 2) & CDNS_WDT_CCR_CRV_MASK;
+
+	/* Write counter access key first to be able write to register */
+	data = count | CDNS_WDT_REGISTER_ACCESS_KEY | wdt->ctrl_clksel;
+	cdns_wdt_writereg(wdt, CDNS_WDT_CCR_OFFSET, data);
+	data = CDNS_WDT_ZMR_WDEN_MASK | CDNS_WDT_ZMR_RSTLEN_16 |
+	       CDNS_WDT_ZMR_ZKEY_VAL;
+
+	/* Reset on timeout if specified in device tree. */
+	if (wdt->rst) {
+		data |= CDNS_WDT_ZMR_RSTEN_MASK;
+		data &= ~CDNS_WDT_ZMR_IRQEN_MASK;
+	} else {
+		data &= ~CDNS_WDT_ZMR_RSTEN_MASK;
+		data |= CDNS_WDT_ZMR_IRQEN_MASK;
+	}
+	cdns_wdt_writereg(wdt, CDNS_WDT_ZMR_OFFSET, data);
+	cdns_wdt_writereg(wdt, CDNS_WDT_RESTART_OFFSET,
+			  CDNS_WDT_RESTART_KEY);
+	spin_unlock(&wdt->io_lock);
+
+	return 0;
+}
+
+/**
+ * cdns_wdt_settimeout - Set a new timeout value for the watchdog device.
+ *
+ * @wdd: watchdog device
+ * @new_time: new timeout value that needs to be set
+ * Return: 0 on success
+ *
+ * Update the watchdog_device timeout with new value which is used when
+ * cdns_wdt_start is called.
+ */
+static int cdns_wdt_settimeout(struct watchdog_device *wdd,
+			       unsigned int new_time)
+{
+	wdd->timeout = new_time;
+
+	return cdns_wdt_start(wdd);
+}
+
+/**
+ * cdns_wdt_irq_handler - Notifies of watchdog timeout.
+ *
+ * @irq: interrupt number
+ * @dev_id: pointer to a platform device structure
+ * Return: IRQ_HANDLED
+ *
+ * The handler is invoked when the watchdog times out and a
+ * reset on timeout has not been enabled.
+ */
+static irqreturn_t cdns_wdt_irq_handler(int irq, void *dev_id)
+{
+	struct platform_device *pdev = dev_id;
+
+	dev_info(&pdev->dev,
+		 "Watchdog timed out. Internal reset not enabled\n");
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Info structure used to indicate the features supported by the device
+ * to the upper layers. This is defined in watchdog.h header file.
+ */
+static struct watchdog_info cdns_wdt_info = {
+	.identity	= "cdns_wdt watchdog",
+	.options	= WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING |
+			  WDIOF_MAGICCLOSE,
+};
+
+/* Watchdog Core Ops */
+static struct watchdog_ops cdns_wdt_ops = {
+	.owner = THIS_MODULE,
+	.start = cdns_wdt_start,
+	.stop = cdns_wdt_stop,
+	.ping = cdns_wdt_reload,
+	.set_timeout = cdns_wdt_settimeout,
+};
+
+/**
+ * cdns_wdt_notify_sys - Notifier for reboot or shutdown.
+ *
+ * @this: handle to notifier block
+ * @code: turn off indicator
+ * @unused: unused
+ * Return: NOTIFY_DONE
+ *
+ * This notifier is invoked whenever the system reboot or shutdown occur
+ * because we need to disable the WDT before system goes down as WDT might
+ * reset on the next boot.
+ */
+static int cdns_wdt_notify_sys(struct notifier_block *this, unsigned long code,
+			       void *unused)
+{
+	struct cdns_wdt *wdt = container_of(this, struct cdns_wdt,
+					    cdns_wdt_notifier);
+	if (code == SYS_DOWN || code == SYS_HALT)
+		cdns_wdt_stop(&wdt->cdns_wdt_device);
+
+	return NOTIFY_DONE;
+}
+
+/************************Platform Operations*****************************/
+/**
+ * cdns_wdt_probe - Probe call for the device.
+ *
+ * @pdev: handle to the platform device structure.
+ * Return: 0 on success, negative error otherwise.
+ *
+ * It does all the memory allocation and registration for the device.
+ */
+static int cdns_wdt_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	int ret, irq;
+	unsigned long clock_f;
+	struct cdns_wdt *wdt;
+	struct watchdog_device *cdns_wdt_device;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+
+	cdns_wdt_device = &wdt->cdns_wdt_device;
+	cdns_wdt_device->info = &cdns_wdt_info;
+	cdns_wdt_device->ops = &cdns_wdt_ops;
+	cdns_wdt_device->timeout = CDNS_WDT_DEFAULT_TIMEOUT;
+	cdns_wdt_device->min_timeout = CDNS_WDT_MIN_TIMEOUT;
+	cdns_wdt_device->max_timeout = CDNS_WDT_MAX_TIMEOUT;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	wdt->regs = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(wdt->regs))
+		return PTR_ERR(wdt->regs);
+
+	/* Register the interrupt */
+	wdt->rst = of_property_read_bool(pdev->dev.of_node, "reset-on-timeout");
+	irq = platform_get_irq(pdev, 0);
+	if (!wdt->rst && irq >= 0) {
+		ret = devm_request_irq(&pdev->dev, irq, cdns_wdt_irq_handler, 0,
+				       pdev->name, pdev);
+		if (ret) {
+			dev_err(&pdev->dev,
+				"cannot register interrupt handler err=%d\n",
+				ret);
+			return ret;
+		}
+	}
+
+	/* Initialize the members of cdns_wdt structure */
+	cdns_wdt_device->parent = &pdev->dev;
+
+	ret = watchdog_init_timeout(cdns_wdt_device, wdt_timeout, &pdev->dev);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to set timeout value\n");
+		return ret;
+	}
+
+	watchdog_set_nowayout(cdns_wdt_device, nowayout);
+	watchdog_set_drvdata(cdns_wdt_device, wdt);
+
+	wdt->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(wdt->clk)) {
+		dev_err(&pdev->dev, "input clock not found\n");
+		ret = PTR_ERR(wdt->clk);
+		return ret;
+	}
+
+	ret = clk_prepare_enable(wdt->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "unable to enable clock\n");
+		return ret;
+	}
+
+	clock_f = clk_get_rate(wdt->clk);
+	if (clock_f <= CDNS_WDT_CLK_75MHZ) {
+		wdt->prescaler = CDNS_WDT_PRESCALE_512;
+		wdt->ctrl_clksel = CDNS_WDT_PRESCALE_SELECT_512;
+	} else {
+		wdt->prescaler = CDNS_WDT_PRESCALE_4096;
+		wdt->ctrl_clksel = CDNS_WDT_PRESCALE_SELECT_4096;
+	}
+
+	spin_lock_init(&wdt->io_lock);
+
+	wdt->cdns_wdt_notifier.notifier_call = &cdns_wdt_notify_sys;
+	ret = register_reboot_notifier(&wdt->cdns_wdt_notifier);
+	if (ret != 0) {
+		dev_err(&pdev->dev, "cannot register reboot notifier err=%d)\n",
+			ret);
+		goto err_clk_disable;
+	}
+
+	ret = watchdog_register_device(cdns_wdt_device);
+	if (ret) {
+		dev_err(&pdev->dev, "Failed to register wdt device\n");
+		goto err_clk_disable;
+	}
+	platform_set_drvdata(pdev, wdt);
+
+	dev_dbg(&pdev->dev, "Xilinx Watchdog Timer at %p with timeout %ds%s\n",
+		 wdt->regs, cdns_wdt_device->timeout,
+		 nowayout ? ", nowayout" : "");
+
+	return 0;
+
+err_clk_disable:
+	clk_disable_unprepare(wdt->clk);
+
+	return ret;
+}
+
+/**
+ * cdns_wdt_remove - Probe call for the device.
+ *
+ * @pdev: handle to the platform device structure.
+ * Return: 0 on success, otherwise negative error.
+ *
+ * Unregister the device after releasing the resources.
+ */
+static int cdns_wdt_remove(struct platform_device *pdev)
+{
+	struct cdns_wdt *wdt = platform_get_drvdata(pdev);
+
+	cdns_wdt_stop(&wdt->cdns_wdt_device);
+	watchdog_unregister_device(&wdt->cdns_wdt_device);
+	unregister_reboot_notifier(&wdt->cdns_wdt_notifier);
+	clk_disable_unprepare(wdt->clk);
+
+	return 0;
+}
+
+/**
+ * cdns_wdt_shutdown - Stop the device.
+ *
+ * @pdev: handle to the platform structure.
+ *
+ */
+static void cdns_wdt_shutdown(struct platform_device *pdev)
+{
+	struct cdns_wdt *wdt = platform_get_drvdata(pdev);
+
+	cdns_wdt_stop(&wdt->cdns_wdt_device);
+	clk_disable_unprepare(wdt->clk);
+}
+
+/**
+ * cdns_wdt_suspend - Stop the device.
+ *
+ * @dev: handle to the device structure.
+ * Return: 0 always.
+ */
+static int __maybe_unused cdns_wdt_suspend(struct device *dev)
+{
+	struct platform_device *pdev = container_of(dev,
+			struct platform_device, dev);
+	struct cdns_wdt *wdt = platform_get_drvdata(pdev);
+
+	cdns_wdt_stop(&wdt->cdns_wdt_device);
+	clk_disable_unprepare(wdt->clk);
+
+	return 0;
+}
+
+/**
+ * cdns_wdt_resume - Resume the device.
+ *
+ * @dev: handle to the device structure.
+ * Return: 0 on success, errno otherwise.
+ */
+static int __maybe_unused cdns_wdt_resume(struct device *dev)
+{
+	int ret;
+	struct platform_device *pdev = container_of(dev,
+			struct platform_device, dev);
+	struct cdns_wdt *wdt = platform_get_drvdata(pdev);
+
+	ret = clk_prepare_enable(wdt->clk);
+	if (ret) {
+		dev_err(dev, "unable to enable clock\n");
+		return ret;
+	}
+	cdns_wdt_start(&wdt->cdns_wdt_device);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(cdns_wdt_pm_ops, cdns_wdt_suspend, cdns_wdt_resume);
+
+static struct of_device_id cdns_wdt_of_match[] = {
+	{ .compatible = "cdns,wdt-r1p2", },
+	{ /* end of table */ }
+};
+MODULE_DEVICE_TABLE(of, cdns_wdt_of_match);
+
+/* Driver Structure */
+static struct platform_driver cdns_wdt_driver = {
+	.probe		= cdns_wdt_probe,
+	.remove		= cdns_wdt_remove,
+	.shutdown	= cdns_wdt_shutdown,
+	.driver		= {
+		.name	= "cdns-wdt",
+		.owner	= THIS_MODULE,
+		.of_match_table = cdns_wdt_of_match,
+		.pm	= &cdns_wdt_pm_ops,
+	},
+};
+
+module_platform_driver(cdns_wdt_driver);
+
+MODULE_AUTHOR("Xilinx, Inc.");
+MODULE_DESCRIPTION("Watchdog driver for Cadence WDT");
+MODULE_LICENSE("GPL");
diff --git a/drivers/watchdog/da9063_wdt.c b/drivers/watchdog/da9063_wdt.c
new file mode 100644
index 000000000000..2cd6b2c2dd2a
--- /dev/null
+++ b/drivers/watchdog/da9063_wdt.c
@@ -0,0 +1,191 @@
+/*
+ * Watchdog driver for DA9063 PMICs.
+ *
+ * Copyright(c) 2012 Dialog Semiconductor Ltd.
+ *
+ * Author: Mariusz Wojtasik <mariusz.wojtasik@diasemi.com>
+ *
+ * This program is free software; you can redistribute  it and/or modify it
+ * under  the terms of  the GNU General  Public License as published by the
+ * Free Software Foundation;  either version 2 of the  License, or (at your
+ * option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/watchdog.h>
+#include <linux/platform_device.h>
+#include <linux/uaccess.h>
+#include <linux/slab.h>
+#include <linux/delay.h>
+#include <linux/mfd/da9063/registers.h>
+#include <linux/mfd/da9063/core.h>
+#include <linux/regmap.h>
+
+/*
+ * Watchdog selector to timeout in seconds.
+ *   0: WDT disabled;
+ *   others: timeout = 2048 ms * 2^(TWDSCALE-1).
+ */
+static const unsigned int wdt_timeout[] = { 0, 2, 4, 8, 16, 32, 65, 131 };
+#define DA9063_TWDSCALE_DISABLE		0
+#define DA9063_TWDSCALE_MIN		1
+#define DA9063_TWDSCALE_MAX		(ARRAY_SIZE(wdt_timeout) - 1)
+#define DA9063_WDT_MIN_TIMEOUT		wdt_timeout[DA9063_TWDSCALE_MIN]
+#define DA9063_WDT_MAX_TIMEOUT		wdt_timeout[DA9063_TWDSCALE_MAX]
+#define DA9063_WDG_TIMEOUT		wdt_timeout[3]
+
+struct da9063_watchdog {
+	struct da9063 *da9063;
+	struct watchdog_device wdtdev;
+};
+
+static unsigned int da9063_wdt_timeout_to_sel(unsigned int secs)
+{
+	unsigned int i;
+
+	for (i = DA9063_TWDSCALE_MIN; i <= DA9063_TWDSCALE_MAX; i++) {
+		if (wdt_timeout[i] >= secs)
+			return i;
+	}
+
+	return DA9063_TWDSCALE_MAX;
+}
+
+static int _da9063_wdt_set_timeout(struct da9063 *da9063, unsigned int regval)
+{
+	return regmap_update_bits(da9063->regmap, DA9063_REG_CONTROL_D,
+				  DA9063_TWDSCALE_MASK, regval);
+}
+
+static int da9063_wdt_start(struct watchdog_device *wdd)
+{
+	struct da9063_watchdog *wdt = watchdog_get_drvdata(wdd);
+	unsigned int selector;
+	int ret;
+
+	selector = da9063_wdt_timeout_to_sel(wdt->wdtdev.timeout);
+	ret = _da9063_wdt_set_timeout(wdt->da9063, selector);
+	if (ret)
+		dev_err(wdt->da9063->dev, "Watchdog failed to start (err = %d)\n",
+			ret);
+
+	return ret;
+}
+
+static int da9063_wdt_stop(struct watchdog_device *wdd)
+{
+	struct da9063_watchdog *wdt = watchdog_get_drvdata(wdd);
+	int ret;
+
+	ret = regmap_update_bits(wdt->da9063->regmap, DA9063_REG_CONTROL_D,
+				 DA9063_TWDSCALE_MASK, DA9063_TWDSCALE_DISABLE);
+	if (ret)
+		dev_alert(wdt->da9063->dev, "Watchdog failed to stop (err = %d)\n",
+			  ret);
+
+	return ret;
+}
+
+static int da9063_wdt_ping(struct watchdog_device *wdd)
+{
+	struct da9063_watchdog *wdt = watchdog_get_drvdata(wdd);
+	int ret;
+
+	ret = regmap_write(wdt->da9063->regmap, DA9063_REG_CONTROL_F,
+			   DA9063_WATCHDOG);
+	if (ret)
+		dev_alert(wdt->da9063->dev, "Failed to ping the watchdog (err = %d)\n",
+			  ret);
+
+	return ret;
+}
+
+static int da9063_wdt_set_timeout(struct watchdog_device *wdd,
+				  unsigned int timeout)
+{
+	struct da9063_watchdog *wdt = watchdog_get_drvdata(wdd);
+	unsigned int selector;
+	int ret;
+
+	selector = da9063_wdt_timeout_to_sel(timeout);
+	ret = _da9063_wdt_set_timeout(wdt->da9063, selector);
+	if (ret)
+		dev_err(wdt->da9063->dev, "Failed to set watchdog timeout (err = %d)\n",
+			ret);
+	else
+		wdd->timeout = wdt_timeout[selector];
+
+	return ret;
+}
+
+static const struct watchdog_info da9063_watchdog_info = {
+	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING,
+	.identity = "DA9063 Watchdog",
+};
+
+static const struct watchdog_ops da9063_watchdog_ops = {
+	.owner = THIS_MODULE,
+	.start = da9063_wdt_start,
+	.stop = da9063_wdt_stop,
+	.ping = da9063_wdt_ping,
+	.set_timeout = da9063_wdt_set_timeout,
+};
+
+static int da9063_wdt_probe(struct platform_device *pdev)
+{
+	int ret;
+	struct da9063 *da9063;
+	struct da9063_watchdog *wdt;
+
+	if (!pdev->dev.parent)
+		return -EINVAL;
+
+	da9063 = dev_get_drvdata(pdev->dev.parent);
+	if (!da9063)
+		return -EINVAL;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+
+	wdt->da9063 = da9063;
+
+	wdt->wdtdev.info = &da9063_watchdog_info;
+	wdt->wdtdev.ops = &da9063_watchdog_ops;
+	wdt->wdtdev.min_timeout = DA9063_WDT_MIN_TIMEOUT;
+	wdt->wdtdev.max_timeout = DA9063_WDT_MAX_TIMEOUT;
+	wdt->wdtdev.timeout = DA9063_WDG_TIMEOUT;
+
+	wdt->wdtdev.status = WATCHDOG_NOWAYOUT_INIT_STATUS;
+
+	watchdog_set_drvdata(&wdt->wdtdev, wdt);
+	dev_set_drvdata(&pdev->dev, wdt);
+
+	ret = watchdog_register_device(&wdt->wdtdev);
+
+	return ret;
+}
+
+static int da9063_wdt_remove(struct platform_device *pdev)
+{
+	struct da9063_watchdog *wdt = dev_get_drvdata(&pdev->dev);
+
+	watchdog_unregister_device(&wdt->wdtdev);
+
+	return 0;
+}
+
+static struct platform_driver da9063_wdt_driver = {
+	.probe = da9063_wdt_probe,
+	.remove = da9063_wdt_remove,
+	.driver = {
+		.name = DA9063_DRVNAME_WATCHDOG,
+	},
+};
+module_platform_driver(da9063_wdt_driver);
+
+MODULE_AUTHOR("Mariusz Wojtasik <mariusz.wojtasik@diasemi.com>");
+MODULE_DESCRIPTION("Watchdog driver for Dialog DA9063");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:" DA9063_DRVNAME_WATCHDOG);
diff --git a/drivers/watchdog/dw_wdt.c b/drivers/watchdog/dw_wdt.c
index 9f210299de24..9e577a64ec9e 100644
--- a/drivers/watchdog/dw_wdt.c
+++ b/drivers/watchdog/dw_wdt.c
@@ -21,6 +21,7 @@
 
 #include <linux/bitops.h>
 #include <linux/clk.h>
+#include <linux/delay.h>
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/fs.h>
@@ -29,9 +30,11 @@
 #include <linux/miscdevice.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
+#include <linux/notifier.h>
 #include <linux/of.h>
 #include <linux/pm.h>
 #include <linux/platform_device.h>
+#include <linux/reboot.h>
 #include <linux/spinlock.h>
 #include <linux/timer.h>
 #include <linux/uaccess.h>
@@ -40,6 +43,7 @@
 #define WDOG_CONTROL_REG_OFFSET		    0x00
 #define WDOG_CONTROL_REG_WDT_EN_MASK	    0x01
 #define WDOG_TIMEOUT_RANGE_REG_OFFSET	    0x04
+#define WDOG_TIMEOUT_RANGE_TOPINIT_SHIFT    4
 #define WDOG_CURRENT_COUNT_REG_OFFSET	    0x08
 #define WDOG_COUNTER_RESTART_REG_OFFSET     0x0c
 #define WDOG_COUNTER_RESTART_KICK_VALUE	    0x76
@@ -62,6 +66,7 @@ static struct {
 	unsigned long		next_heartbeat;
 	struct timer_list	timer;
 	int			expect_close;
+	struct notifier_block	restart_handler;
 } dw_wdt;
 
 static inline int dw_wdt_is_enabled(void)
@@ -106,7 +111,8 @@ static int dw_wdt_set_top(unsigned top_s)
 		}
 
 	/* Set the new value in the watchdog. */
-	writel(top_val, dw_wdt.regs + WDOG_TIMEOUT_RANGE_REG_OFFSET);
+	writel(top_val | top_val << WDOG_TIMEOUT_RANGE_TOPINIT_SHIFT,
+		dw_wdt.regs + WDOG_TIMEOUT_RANGE_REG_OFFSET);
 
 	dw_wdt_set_next_heartbeat();
 
@@ -119,6 +125,26 @@ static void dw_wdt_keepalive(void)
 	       WDOG_COUNTER_RESTART_REG_OFFSET);
 }
 
+static int dw_wdt_restart_handle(struct notifier_block *this,
+				unsigned long mode, void *cmd)
+{
+	u32 val;
+
+	writel(0, dw_wdt.regs + WDOG_TIMEOUT_RANGE_REG_OFFSET);
+	val = readl(dw_wdt.regs + WDOG_CONTROL_REG_OFFSET);
+	if (val & WDOG_CONTROL_REG_WDT_EN_MASK)
+		writel(WDOG_COUNTER_RESTART_KICK_VALUE, dw_wdt.regs +
+			WDOG_COUNTER_RESTART_REG_OFFSET);
+	else
+		writel(WDOG_CONTROL_REG_WDT_EN_MASK,
+		       dw_wdt.regs + WDOG_CONTROL_REG_OFFSET);
+
+	/* wait for reset to assert... */
+	mdelay(500);
+
+	return NOTIFY_DONE;
+}
+
 static void dw_wdt_ping(unsigned long data)
 {
 	if (time_before(jiffies, dw_wdt.next_heartbeat) ||
@@ -314,6 +340,12 @@ static int dw_wdt_drv_probe(struct platform_device *pdev)
 	if (ret)
 		goto out_disable_clk;
 
+	dw_wdt.restart_handler.notifier_call = dw_wdt_restart_handle;
+	dw_wdt.restart_handler.priority = 128;
+	ret = register_restart_handler(&dw_wdt.restart_handler);
+	if (ret)
+		pr_warn("cannot register restart handler\n");
+
 	dw_wdt_set_next_heartbeat();
 	setup_timer(&dw_wdt.timer, dw_wdt_ping, 0);
 	mod_timer(&dw_wdt.timer, jiffies + WDT_TIMEOUT);
@@ -328,6 +360,8 @@ out_disable_clk:
 
 static int dw_wdt_drv_remove(struct platform_device *pdev)
 {
+	unregister_restart_handler(&dw_wdt.restart_handler);
+
 	misc_deregister(&dw_wdt_miscdev);
 
 	clk_disable_unprepare(dw_wdt.clk);
diff --git a/drivers/watchdog/imx2_wdt.c b/drivers/watchdog/imx2_wdt.c
index 68c3d379ffa8..7e12f88bb4a6 100644
--- a/drivers/watchdog/imx2_wdt.c
+++ b/drivers/watchdog/imx2_wdt.c
@@ -22,14 +22,17 @@
  */
 
 #include <linux/clk.h>
+#include <linux/delay.h>
 #include <linux/init.h>
 #include <linux/io.h>
 #include <linux/jiffies.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
+#include <linux/notifier.h>
 #include <linux/of_address.h>
 #include <linux/platform_device.h>
+#include <linux/reboot.h>
 #include <linux/regmap.h>
 #include <linux/timer.h>
 #include <linux/watchdog.h>
@@ -59,6 +62,7 @@ struct imx2_wdt_device {
 	struct regmap *regmap;
 	struct timer_list timer;	/* Pings the watchdog when closed */
 	struct watchdog_device wdog;
+	struct notifier_block restart_handler;
 };
 
 static bool nowayout = WATCHDOG_NOWAYOUT;
@@ -77,6 +81,31 @@ static const struct watchdog_info imx2_wdt_info = {
 	.options = WDIOF_KEEPALIVEPING | WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE,
 };
 
+static int imx2_restart_handler(struct notifier_block *this, unsigned long mode,
+				void *cmd)
+{
+	unsigned int wcr_enable = IMX2_WDT_WCR_WDE;
+	struct imx2_wdt_device *wdev = container_of(this,
+						    struct imx2_wdt_device,
+						    restart_handler);
+	/* Assert SRS signal */
+	regmap_write(wdev->regmap, 0, wcr_enable);
+	/*
+	 * Due to imx6q errata ERR004346 (WDOG: WDOG SRS bit requires to be
+	 * written twice), we add another two writes to ensure there must be at
+	 * least two writes happen in the same one 32kHz clock period.  We save
+	 * the target check here, since the writes shouldn't be a huge burden
+	 * for other platforms.
+	 */
+	regmap_write(wdev->regmap, 0, wcr_enable);
+	regmap_write(wdev->regmap, 0, wcr_enable);
+
+	/* wait for reset to assert... */
+	mdelay(500);
+
+	return NOTIFY_DONE;
+}
+
 static inline void imx2_wdt_setup(struct watchdog_device *wdog)
 {
 	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
@@ -191,12 +220,10 @@ static struct regmap_config imx2_wdt_regmap_config = {
 
 static int __init imx2_wdt_probe(struct platform_device *pdev)
 {
-	struct device_node *np = pdev->dev.of_node;
 	struct imx2_wdt_device *wdev;
 	struct watchdog_device *wdog;
 	struct resource *res;
 	void __iomem *base;
-	bool big_endian;
 	int ret;
 	u32 val;
 
@@ -204,10 +231,6 @@ static int __init imx2_wdt_probe(struct platform_device *pdev)
 	if (!wdev)
 		return -ENOMEM;
 
-	big_endian = of_property_read_bool(np, "big-endian");
-	if (big_endian)
-		imx2_wdt_regmap_config.val_format_endian = REGMAP_ENDIAN_BIG;
-
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(base))
@@ -257,6 +280,12 @@ static int __init imx2_wdt_probe(struct platform_device *pdev)
 		return ret;
 	}
 
+	wdev->restart_handler.notifier_call = imx2_restart_handler;
+	wdev->restart_handler.priority = 128;
+	ret = register_restart_handler(&wdev->restart_handler);
+	if (ret)
+		dev_err(&pdev->dev, "cannot register restart handler\n");
+
 	dev_info(&pdev->dev, "timeout %d sec (nowayout=%d)\n",
 		 wdog->timeout, nowayout);
 
@@ -268,6 +297,8 @@ static int __exit imx2_wdt_remove(struct platform_device *pdev)
 	struct watchdog_device *wdog = platform_get_drvdata(pdev);
 	struct imx2_wdt_device *wdev = watchdog_get_drvdata(wdog);
 
+	unregister_restart_handler(&wdev->restart_handler);
+
 	watchdog_unregister_device(wdog);
 
 	if (imx2_wdt_is_running(wdev)) {
diff --git a/drivers/watchdog/menf21bmc_wdt.c b/drivers/watchdog/menf21bmc_wdt.c
new file mode 100644
index 000000000000..2042874d5ce3
--- /dev/null
+++ b/drivers/watchdog/menf21bmc_wdt.c
@@ -0,0 +1,203 @@
+/*
+ *  MEN 14F021P00 Board Management Controller (BMC) Watchdog Driver.
+ *
+ *  Copyright (C) 2014 MEN Mikro Elektronik Nuernberg GmbH
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ */
+
+#include <linux/kernel.h>
+#include <linux/device.h>
+#include <linux/module.h>
+#include <linux/watchdog.h>
+#include <linux/platform_device.h>
+#include <linux/i2c.h>
+
+#define DEVNAME "menf21bmc_wdt"
+
+#define BMC_CMD_WD_ON		0x11
+#define BMC_CMD_WD_OFF		0x12
+#define BMC_CMD_WD_TRIG		0x13
+#define BMC_CMD_WD_TIME		0x14
+#define BMC_CMD_WD_STATE	0x17
+#define BMC_WD_OFF_VAL		0x69
+#define BMC_CMD_RST_RSN		0x92
+
+#define BMC_WD_TIMEOUT_MIN	1	/* in sec */
+#define BMC_WD_TIMEOUT_MAX	6553	/* in sec */
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
+				__MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+struct menf21bmc_wdt {
+	struct watchdog_device wdt;
+	struct i2c_client *i2c_client;
+};
+
+static int menf21bmc_wdt_set_bootstatus(struct menf21bmc_wdt *data)
+{
+	int rst_rsn;
+
+	rst_rsn = i2c_smbus_read_byte_data(data->i2c_client, BMC_CMD_RST_RSN);
+	if (rst_rsn < 0)
+		return rst_rsn;
+
+	if (rst_rsn == 0x02)
+		data->wdt.bootstatus |= WDIOF_CARDRESET;
+	else if (rst_rsn == 0x05)
+		data->wdt.bootstatus |= WDIOF_EXTERN1;
+	else if (rst_rsn == 0x06)
+		data->wdt.bootstatus |= WDIOF_EXTERN2;
+	else if (rst_rsn == 0x0A)
+		data->wdt.bootstatus |= WDIOF_POWERUNDER;
+
+	return 0;
+}
+
+static int menf21bmc_wdt_start(struct watchdog_device *wdt)
+{
+	struct menf21bmc_wdt *drv_data = watchdog_get_drvdata(wdt);
+
+	return i2c_smbus_write_byte(drv_data->i2c_client, BMC_CMD_WD_ON);
+}
+
+static int menf21bmc_wdt_stop(struct watchdog_device *wdt)
+{
+	struct menf21bmc_wdt *drv_data = watchdog_get_drvdata(wdt);
+
+	return i2c_smbus_write_byte_data(drv_data->i2c_client,
+					 BMC_CMD_WD_OFF, BMC_WD_OFF_VAL);
+}
+
+static int
+menf21bmc_wdt_settimeout(struct watchdog_device *wdt, unsigned int timeout)
+{
+	int ret;
+	struct menf21bmc_wdt *drv_data = watchdog_get_drvdata(wdt);
+
+	/*
+	 *  BMC Watchdog does have a resolution of 100ms.
+	 *  Watchdog API defines the timeout in seconds, so we have to
+	 *  multiply the value.
+	 */
+	ret = i2c_smbus_write_word_data(drv_data->i2c_client,
+					BMC_CMD_WD_TIME, timeout * 10);
+	if (ret < 0)
+		return ret;
+
+	wdt->timeout = timeout;
+
+	return 0;
+}
+
+static int menf21bmc_wdt_ping(struct watchdog_device *wdt)
+{
+	struct menf21bmc_wdt *drv_data = watchdog_get_drvdata(wdt);
+
+	return i2c_smbus_write_byte(drv_data->i2c_client, BMC_CMD_WD_TRIG);
+}
+
+static const struct watchdog_info menf21bmc_wdt_info = {
+	.options = WDIOF_SETTIMEOUT | WDIOF_KEEPALIVEPING,
+	.identity = DEVNAME,
+};
+
+static const struct watchdog_ops menf21bmc_wdt_ops = {
+	.owner		= THIS_MODULE,
+	.start		= menf21bmc_wdt_start,
+	.stop		= menf21bmc_wdt_stop,
+	.ping		= menf21bmc_wdt_ping,
+	.set_timeout	= menf21bmc_wdt_settimeout,
+};
+
+static int menf21bmc_wdt_probe(struct platform_device *pdev)
+{
+	int ret, bmc_timeout;
+	struct menf21bmc_wdt *drv_data;
+	struct i2c_client *i2c_client = to_i2c_client(pdev->dev.parent);
+
+	drv_data = devm_kzalloc(&pdev->dev,
+				sizeof(struct menf21bmc_wdt), GFP_KERNEL);
+	if (!drv_data)
+		return -ENOMEM;
+
+	drv_data->wdt.ops = &menf21bmc_wdt_ops;
+	drv_data->wdt.info = &menf21bmc_wdt_info;
+	drv_data->wdt.min_timeout = BMC_WD_TIMEOUT_MIN;
+	drv_data->wdt.max_timeout = BMC_WD_TIMEOUT_MAX;
+	drv_data->i2c_client = i2c_client;
+
+	/*
+	 * Get the current wdt timeout value from the BMC because
+	 * the BMC will save the value set before if the system restarts.
+	 */
+	bmc_timeout = i2c_smbus_read_word_data(drv_data->i2c_client,
+					       BMC_CMD_WD_TIME);
+	if (bmc_timeout < 0) {
+		dev_err(&pdev->dev, "failed to get current WDT timeout\n");
+		return bmc_timeout;
+	}
+
+	watchdog_init_timeout(&drv_data->wdt, bmc_timeout / 10, &pdev->dev);
+	watchdog_set_nowayout(&drv_data->wdt, nowayout);
+	watchdog_set_drvdata(&drv_data->wdt, drv_data);
+	platform_set_drvdata(pdev, drv_data);
+
+	ret = menf21bmc_wdt_set_bootstatus(drv_data);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "failed to set Watchdog bootstatus\n");
+		return ret;
+	}
+
+	ret = watchdog_register_device(&drv_data->wdt);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register Watchdog device\n");
+		return ret;
+	}
+
+	dev_info(&pdev->dev, "MEN 14F021P00 BMC Watchdog device enabled\n");
+
+	return 0;
+}
+
+static int menf21bmc_wdt_remove(struct platform_device *pdev)
+{
+	struct menf21bmc_wdt *drv_data = platform_get_drvdata(pdev);
+
+	dev_warn(&pdev->dev,
+		 "Unregister MEN 14F021P00 BMC Watchdog device, board may reset\n");
+
+	watchdog_unregister_device(&drv_data->wdt);
+
+	return 0;
+}
+
+static void menf21bmc_wdt_shutdown(struct platform_device *pdev)
+{
+	struct menf21bmc_wdt *drv_data = platform_get_drvdata(pdev);
+
+	i2c_smbus_write_word_data(drv_data->i2c_client,
+				  BMC_CMD_WD_OFF, BMC_WD_OFF_VAL);
+}
+
+static struct  platform_driver menf21bmc_wdt = {
+	.driver		= {
+		.owner = THIS_MODULE,
+		.name	= DEVNAME,
+	},
+	.probe		= menf21bmc_wdt_probe,
+	.remove		= menf21bmc_wdt_remove,
+	.shutdown	= menf21bmc_wdt_shutdown,
+};
+
+module_platform_driver(menf21bmc_wdt);
+
+MODULE_DESCRIPTION("MEN 14F021P00 BMC Watchdog driver");
+MODULE_AUTHOR("Andreas Werner <andreas.werner@men.de>");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:menf21bmc_wdt");
diff --git a/drivers/watchdog/meson_wdt.c b/drivers/watchdog/meson_wdt.c
new file mode 100644
index 000000000000..ef6a298e8c45
--- /dev/null
+++ b/drivers/watchdog/meson_wdt.c
@@ -0,0 +1,236 @@
+/*
+ *      Meson Watchdog Driver
+ *
+ *      Copyright (c) 2014 Carlo Caione
+ *
+ *      This program is free software; you can redistribute it and/or
+ *      modify it under the terms of the GNU General Public License
+ *      as published by the Free Software Foundation; either version
+ *      2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/moduleparam.h>
+#include <linux/notifier.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/types.h>
+#include <linux/watchdog.h>
+
+#define DRV_NAME		"meson_wdt"
+
+#define MESON_WDT_TC		0x00
+#define MESON_WDT_TC_EN		BIT(22)
+#define MESON_WDT_TC_TM_MASK	0x3fffff
+#define MESON_WDT_DC_RESET	(3 << 24)
+
+#define MESON_WDT_RESET		0x04
+
+#define MESON_WDT_TIMEOUT	30
+#define MESON_WDT_MIN_TIMEOUT	1
+#define MESON_WDT_MAX_TIMEOUT	(MESON_WDT_TC_TM_MASK / 100000)
+
+#define MESON_SEC_TO_TC(s)	((s) * 100000)
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+static unsigned int timeout = MESON_WDT_TIMEOUT;
+
+struct meson_wdt_dev {
+	struct watchdog_device wdt_dev;
+	void __iomem *wdt_base;
+	struct notifier_block restart_handler;
+};
+
+static int meson_restart_handle(struct notifier_block *this, unsigned long mode,
+				void *cmd)
+{
+	u32 tc_reboot = MESON_WDT_DC_RESET | MESON_WDT_TC_EN;
+	struct meson_wdt_dev *meson_wdt = container_of(this,
+						       struct meson_wdt_dev,
+						       restart_handler);
+
+	while (1) {
+		writel(tc_reboot, meson_wdt->wdt_base + MESON_WDT_TC);
+		mdelay(5);
+	}
+
+	return NOTIFY_DONE;
+}
+
+static int meson_wdt_ping(struct watchdog_device *wdt_dev)
+{
+	struct meson_wdt_dev *meson_wdt = watchdog_get_drvdata(wdt_dev);
+
+	writel(0, meson_wdt->wdt_base + MESON_WDT_RESET);
+
+	return 0;
+}
+
+static void meson_wdt_change_timeout(struct watchdog_device *wdt_dev,
+				     unsigned int timeout)
+{
+	struct meson_wdt_dev *meson_wdt = watchdog_get_drvdata(wdt_dev);
+	u32 reg;
+
+	reg = readl(meson_wdt->wdt_base + MESON_WDT_TC);
+	reg &= ~MESON_WDT_TC_TM_MASK;
+	reg |= MESON_SEC_TO_TC(timeout);
+	writel(reg, meson_wdt->wdt_base + MESON_WDT_TC);
+}
+
+static int meson_wdt_set_timeout(struct watchdog_device *wdt_dev,
+				 unsigned int timeout)
+{
+	wdt_dev->timeout = timeout;
+
+	meson_wdt_change_timeout(wdt_dev, timeout);
+	meson_wdt_ping(wdt_dev);
+
+	return 0;
+}
+
+static int meson_wdt_stop(struct watchdog_device *wdt_dev)
+{
+	struct meson_wdt_dev *meson_wdt = watchdog_get_drvdata(wdt_dev);
+	u32 reg;
+
+	reg = readl(meson_wdt->wdt_base + MESON_WDT_TC);
+	reg &= ~MESON_WDT_TC_EN;
+	writel(reg, meson_wdt->wdt_base + MESON_WDT_TC);
+
+	return 0;
+}
+
+static int meson_wdt_start(struct watchdog_device *wdt_dev)
+{
+	struct meson_wdt_dev *meson_wdt = watchdog_get_drvdata(wdt_dev);
+	u32 reg;
+
+	meson_wdt_change_timeout(wdt_dev, meson_wdt->wdt_dev.timeout);
+	meson_wdt_ping(wdt_dev);
+
+	reg = readl(meson_wdt->wdt_base + MESON_WDT_TC);
+	reg |= MESON_WDT_TC_EN;
+	writel(reg, meson_wdt->wdt_base + MESON_WDT_TC);
+
+	return 0;
+}
+
+static const struct watchdog_info meson_wdt_info = {
+	.identity	= DRV_NAME,
+	.options	= WDIOF_SETTIMEOUT |
+			  WDIOF_KEEPALIVEPING |
+			  WDIOF_MAGICCLOSE,
+};
+
+static const struct watchdog_ops meson_wdt_ops = {
+	.owner		= THIS_MODULE,
+	.start		= meson_wdt_start,
+	.stop		= meson_wdt_stop,
+	.ping		= meson_wdt_ping,
+	.set_timeout	= meson_wdt_set_timeout,
+};
+
+static int meson_wdt_probe(struct platform_device *pdev)
+{
+	struct resource *res;
+	struct meson_wdt_dev *meson_wdt;
+	int err;
+
+	meson_wdt = devm_kzalloc(&pdev->dev, sizeof(*meson_wdt), GFP_KERNEL);
+	if (!meson_wdt)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	meson_wdt->wdt_base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(meson_wdt->wdt_base))
+		return PTR_ERR(meson_wdt->wdt_base);
+
+	meson_wdt->wdt_dev.parent = &pdev->dev;
+	meson_wdt->wdt_dev.info = &meson_wdt_info;
+	meson_wdt->wdt_dev.ops = &meson_wdt_ops;
+	meson_wdt->wdt_dev.timeout = MESON_WDT_TIMEOUT;
+	meson_wdt->wdt_dev.max_timeout = MESON_WDT_MAX_TIMEOUT;
+	meson_wdt->wdt_dev.min_timeout = MESON_WDT_MIN_TIMEOUT;
+
+	watchdog_set_drvdata(&meson_wdt->wdt_dev, meson_wdt);
+
+	watchdog_init_timeout(&meson_wdt->wdt_dev, timeout, &pdev->dev);
+	watchdog_set_nowayout(&meson_wdt->wdt_dev, nowayout);
+
+	meson_wdt_stop(&meson_wdt->wdt_dev);
+
+	err = watchdog_register_device(&meson_wdt->wdt_dev);
+	if (err)
+		return err;
+
+	platform_set_drvdata(pdev, meson_wdt);
+
+	meson_wdt->restart_handler.notifier_call = meson_restart_handle;
+	meson_wdt->restart_handler.priority = 128;
+	err = register_restart_handler(&meson_wdt->restart_handler);
+	if (err)
+		dev_err(&pdev->dev,
+			"cannot register restart handler (err=%d)\n", err);
+
+	dev_info(&pdev->dev, "Watchdog enabled (timeout=%d sec, nowayout=%d)",
+		 meson_wdt->wdt_dev.timeout, nowayout);
+
+	return 0;
+}
+
+static int meson_wdt_remove(struct platform_device *pdev)
+{
+	struct meson_wdt_dev *meson_wdt = platform_get_drvdata(pdev);
+
+	unregister_restart_handler(&meson_wdt->restart_handler);
+
+	watchdog_unregister_device(&meson_wdt->wdt_dev);
+
+	return 0;
+}
+
+static void meson_wdt_shutdown(struct platform_device *pdev)
+{
+	struct meson_wdt_dev *meson_wdt = platform_get_drvdata(pdev);
+
+	meson_wdt_stop(&meson_wdt->wdt_dev);
+}
+
+static const struct of_device_id meson_wdt_dt_ids[] = {
+	{ .compatible = "amlogic,meson6-wdt" },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, meson_wdt_dt_ids);
+
+static struct platform_driver meson_wdt_driver = {
+	.probe		= meson_wdt_probe,
+	.remove		= meson_wdt_remove,
+	.shutdown	= meson_wdt_shutdown,
+	.driver		= {
+		.owner		= THIS_MODULE,
+		.name		= DRV_NAME,
+		.of_match_table	= meson_wdt_dt_ids,
+	},
+};
+
+module_platform_driver(meson_wdt_driver);
+
+module_param(timeout, uint, 0);
+MODULE_PARM_DESC(timeout, "Watchdog heartbeat in seconds");
+
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout,
+		 "Watchdog cannot be stopped once started (default="
+		 __MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("Carlo Caione <carlo@caione.org>");
+MODULE_DESCRIPTION("Meson Watchdog Timer Driver");
diff --git a/drivers/watchdog/moxart_wdt.c b/drivers/watchdog/moxart_wdt.c
index 4aa3a8a876fe..a64405b82596 100644
--- a/drivers/watchdog/moxart_wdt.c
+++ b/drivers/watchdog/moxart_wdt.c
@@ -15,12 +15,12 @@
 #include <linux/module.h>
 #include <linux/err.h>
 #include <linux/kernel.h>
+#include <linux/notifier.h>
 #include <linux/platform_device.h>
+#include <linux/reboot.h>
 #include <linux/watchdog.h>
 #include <linux/moduleparam.h>
 
-#include <asm/system_misc.h>
-
 #define REG_COUNT			0x4
 #define REG_MODE			0x8
 #define REG_ENABLE			0xC
@@ -29,17 +29,22 @@ struct moxart_wdt_dev {
 	struct watchdog_device dev;
 	void __iomem *base;
 	unsigned int clock_frequency;
+	struct notifier_block restart_handler;
 };
 
-static struct moxart_wdt_dev *moxart_restart_ctx;
-
 static int heartbeat;
 
-static void moxart_wdt_restart(enum reboot_mode reboot_mode, const char *cmd)
+static int moxart_restart_handle(struct notifier_block *this,
+				 unsigned long mode, void *cmd)
 {
-	writel(1, moxart_restart_ctx->base + REG_COUNT);
-	writel(0x5ab9, moxart_restart_ctx->base + REG_MODE);
-	writel(0x03, moxart_restart_ctx->base + REG_ENABLE);
+	struct moxart_wdt_dev *moxart_wdt = container_of(this,
+							 struct moxart_wdt_dev,
+							 restart_handler);
+	writel(1, moxart_wdt->base + REG_COUNT);
+	writel(0x5ab9, moxart_wdt->base + REG_MODE);
+	writel(0x03, moxart_wdt->base + REG_ENABLE);
+
+	return NOTIFY_DONE;
 }
 
 static int moxart_wdt_stop(struct watchdog_device *wdt_dev)
@@ -136,8 +141,12 @@ static int moxart_wdt_probe(struct platform_device *pdev)
 	if (err)
 		return err;
 
-	moxart_restart_ctx = moxart_wdt;
-	arm_pm_restart = moxart_wdt_restart;
+	moxart_wdt->restart_handler.notifier_call = moxart_restart_handle;
+	moxart_wdt->restart_handler.priority = 128;
+	err = register_restart_handler(&moxart_wdt->restart_handler);
+	if (err)
+		dev_err(dev, "cannot register restart notifier (err=%d)\n",
+			err);
 
 	dev_dbg(dev, "Watchdog enabled (heartbeat=%d sec, nowayout=%d)\n",
 		moxart_wdt->dev.timeout, nowayout);
@@ -149,9 +158,8 @@ static int moxart_wdt_remove(struct platform_device *pdev)
 {
 	struct moxart_wdt_dev *moxart_wdt = platform_get_drvdata(pdev);
 
-	arm_pm_restart = NULL;
+	unregister_restart_handler(&moxart_wdt->restart_handler);
 	moxart_wdt_stop(&moxart_wdt->dev);
-	watchdog_unregister_device(&moxart_wdt->dev);
 
 	return 0;
 }
diff --git a/drivers/watchdog/of_xilinx_wdt.c b/drivers/watchdog/of_xilinx_wdt.c
index 1e6e28df5d7b..b2e1b4cbbdc1 100644
--- a/drivers/watchdog/of_xilinx_wdt.c
+++ b/drivers/watchdog/of_xilinx_wdt.c
@@ -236,7 +236,6 @@ static struct platform_driver xwdt_driver = {
 	.probe       = xwdt_probe,
 	.remove      = xwdt_remove,
 	.driver = {
-		.owner = THIS_MODULE,
 		.name  = WATCHDOG_NAME,
 		.of_match_table = xwdt_of_match,
 	},
diff --git a/drivers/watchdog/qcom-wdt.c b/drivers/watchdog/qcom-wdt.c
new file mode 100644
index 000000000000..aa85618c4d03
--- /dev/null
+++ b/drivers/watchdog/qcom-wdt.c
@@ -0,0 +1,224 @@
+/* Copyright (c) 2014, The Linux Foundation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ */
+#include <linux/clk.h>
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/of.h>
+#include <linux/platform_device.h>
+#include <linux/reboot.h>
+#include <linux/watchdog.h>
+
+#define WDT_RST		0x0
+#define WDT_EN		0x8
+#define WDT_BITE_TIME	0x24
+
+struct qcom_wdt {
+	struct watchdog_device	wdd;
+	struct clk		*clk;
+	unsigned long		rate;
+	struct notifier_block	restart_nb;
+	void __iomem		*base;
+};
+
+static inline
+struct qcom_wdt *to_qcom_wdt(struct watchdog_device *wdd)
+{
+	return container_of(wdd, struct qcom_wdt, wdd);
+}
+
+static int qcom_wdt_start(struct watchdog_device *wdd)
+{
+	struct qcom_wdt *wdt = to_qcom_wdt(wdd);
+
+	writel(0, wdt->base + WDT_EN);
+	writel(1, wdt->base + WDT_RST);
+	writel(wdd->timeout * wdt->rate, wdt->base + WDT_BITE_TIME);
+	writel(1, wdt->base + WDT_EN);
+	return 0;
+}
+
+static int qcom_wdt_stop(struct watchdog_device *wdd)
+{
+	struct qcom_wdt *wdt = to_qcom_wdt(wdd);
+
+	writel(0, wdt->base + WDT_EN);
+	return 0;
+}
+
+static int qcom_wdt_ping(struct watchdog_device *wdd)
+{
+	struct qcom_wdt *wdt = to_qcom_wdt(wdd);
+
+	writel(1, wdt->base + WDT_RST);
+	return 0;
+}
+
+static int qcom_wdt_set_timeout(struct watchdog_device *wdd,
+				unsigned int timeout)
+{
+	wdd->timeout = timeout;
+	return qcom_wdt_start(wdd);
+}
+
+static const struct watchdog_ops qcom_wdt_ops = {
+	.start		= qcom_wdt_start,
+	.stop		= qcom_wdt_stop,
+	.ping		= qcom_wdt_ping,
+	.set_timeout	= qcom_wdt_set_timeout,
+	.owner		= THIS_MODULE,
+};
+
+static const struct watchdog_info qcom_wdt_info = {
+	.options	= WDIOF_KEEPALIVEPING
+			| WDIOF_MAGICCLOSE
+			| WDIOF_SETTIMEOUT,
+	.identity	= KBUILD_MODNAME,
+};
+
+static int qcom_wdt_restart(struct notifier_block *nb, unsigned long action,
+			    void *data)
+{
+	struct qcom_wdt *wdt = container_of(nb, struct qcom_wdt, restart_nb);
+	u32 timeout;
+
+	/*
+	 * Trigger watchdog bite:
+	 *    Setup BITE_TIME to be 128ms, and enable WDT.
+	 */
+	timeout = 128 * wdt->rate / 1000;
+
+	writel(0, wdt->base + WDT_EN);
+	writel(1, wdt->base + WDT_RST);
+	writel(timeout, wdt->base + WDT_BITE_TIME);
+	writel(1, wdt->base + WDT_EN);
+
+	/*
+	 * Actually make sure the above sequence hits hardware before sleeping.
+	 */
+	wmb();
+
+	msleep(150);
+	return NOTIFY_DONE;
+}
+
+static int qcom_wdt_probe(struct platform_device *pdev)
+{
+	struct qcom_wdt *wdt;
+	struct resource *res;
+	int ret;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(*wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+
+	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	wdt->base = devm_ioremap_resource(&pdev->dev, res);
+	if (IS_ERR(wdt->base))
+		return PTR_ERR(wdt->base);
+
+	wdt->clk = devm_clk_get(&pdev->dev, NULL);
+	if (IS_ERR(wdt->clk)) {
+		dev_err(&pdev->dev, "failed to get input clock\n");
+		return PTR_ERR(wdt->clk);
+	}
+
+	ret = clk_prepare_enable(wdt->clk);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to setup clock\n");
+		return ret;
+	}
+
+	/*
+	 * We use the clock rate to calculate the max timeout, so ensure it's
+	 * not zero to avoid a divide-by-zero exception.
+	 *
+	 * WATCHDOG_CORE assumes units of seconds, if the WDT is clocked such
+	 * that it would bite before a second elapses it's usefulness is
+	 * limited.  Bail if this is the case.
+	 */
+	wdt->rate = clk_get_rate(wdt->clk);
+	if (wdt->rate == 0 ||
+	    wdt->rate > 0x10000000U) {
+		dev_err(&pdev->dev, "invalid clock rate\n");
+		ret = -EINVAL;
+		goto err_clk_unprepare;
+	}
+
+	wdt->wdd.dev = &pdev->dev;
+	wdt->wdd.info = &qcom_wdt_info;
+	wdt->wdd.ops = &qcom_wdt_ops;
+	wdt->wdd.min_timeout = 1;
+	wdt->wdd.max_timeout = 0x10000000U / wdt->rate;
+
+	/*
+	 * If 'timeout-sec' unspecified in devicetree, assume a 30 second
+	 * default, unless the max timeout is less than 30 seconds, then use
+	 * the max instead.
+	 */
+	wdt->wdd.timeout = min(wdt->wdd.max_timeout, 30U);
+	watchdog_init_timeout(&wdt->wdd, 0, &pdev->dev);
+
+	ret = watchdog_register_device(&wdt->wdd);
+	if (ret) {
+		dev_err(&pdev->dev, "failed to register watchdog\n");
+		goto err_clk_unprepare;
+	}
+
+	/*
+	 * WDT restart notifier has priority 0 (use as a last resort)
+	 */
+	wdt->restart_nb.notifier_call = qcom_wdt_restart;
+	ret = register_restart_handler(&wdt->restart_nb);
+	if (ret)
+		dev_err(&pdev->dev, "failed to setup restart handler\n");
+
+	platform_set_drvdata(pdev, wdt);
+	return 0;
+
+err_clk_unprepare:
+	clk_disable_unprepare(wdt->clk);
+	return ret;
+}
+
+static int qcom_wdt_remove(struct platform_device *pdev)
+{
+	struct qcom_wdt *wdt = platform_get_drvdata(pdev);
+
+	unregister_restart_handler(&wdt->restart_nb);
+	watchdog_unregister_device(&wdt->wdd);
+	clk_disable_unprepare(wdt->clk);
+	return 0;
+}
+
+static const struct of_device_id qcom_wdt_of_table[] = {
+	{ .compatible = "qcom,kpss-wdt-msm8960", },
+	{ .compatible = "qcom,kpss-wdt-apq8064", },
+	{ .compatible = "qcom,kpss-wdt-ipq8064", },
+	{ },
+};
+MODULE_DEVICE_TABLE(of, qcom_wdt_of_table);
+
+static struct platform_driver qcom_watchdog_driver = {
+	.probe	= qcom_wdt_probe,
+	.remove	= qcom_wdt_remove,
+	.driver	= {
+		.name		= KBUILD_MODNAME,
+		.of_match_table	= qcom_wdt_of_table,
+	},
+};
+module_platform_driver(qcom_watchdog_driver);
+
+MODULE_DESCRIPTION("QCOM KPSS Watchdog Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/watchdog/rn5t618_wdt.c b/drivers/watchdog/rn5t618_wdt.c
new file mode 100644
index 000000000000..d1c12278cb6a
--- /dev/null
+++ b/drivers/watchdog/rn5t618_wdt.c
@@ -0,0 +1,198 @@
+/*
+ * Watchdog driver for Ricoh RN5T618 PMIC
+ *
+ * Copyright (C) 2014 Beniamino Galvani <b.galvani@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * version 2 as published by the Free Software Foundation.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/device.h>
+#include <linux/mfd/rn5t618.h>
+#include <linux/module.h>
+#include <linux/platform_device.h>
+#include <linux/watchdog.h>
+
+#define DRIVER_NAME "rn5t618-wdt"
+
+static bool nowayout = WATCHDOG_NOWAYOUT;
+static unsigned int timeout;
+
+module_param(timeout, uint, 0);
+MODULE_PARM_DESC(timeout, "Initial watchdog timeout in seconds");
+
+module_param(nowayout, bool, 0);
+MODULE_PARM_DESC(nowayout, "Watchdog cannot be stopped once started (default="
+		 __MODULE_STRING(WATCHDOG_NOWAYOUT) ")");
+
+struct rn5t618_wdt {
+	struct watchdog_device wdt_dev;
+	struct rn5t618 *rn5t618;
+};
+
+/*
+ * This array encodes the values of WDOGTIM field for the supported
+ * watchdog expiration times. If the watchdog is not accessed before
+ * the timer expiration, the PMU generates an interrupt and if the CPU
+ * doesn't clear it within one second the system is restarted.
+ */
+static const struct {
+	u8 reg_val;
+	unsigned int time;
+} rn5t618_wdt_map[] = {
+	{ 0, 1 },
+	{ 1, 8 },
+	{ 2, 32 },
+	{ 3, 128 },
+};
+
+static int rn5t618_wdt_set_timeout(struct watchdog_device *wdt_dev,
+				   unsigned int t)
+{
+	struct rn5t618_wdt *wdt = watchdog_get_drvdata(wdt_dev);
+	int ret, i;
+
+	for (i = 0; i < ARRAY_SIZE(rn5t618_wdt_map); i++) {
+		if (rn5t618_wdt_map[i].time + 1 >= t)
+			break;
+	}
+
+	if (i == ARRAY_SIZE(rn5t618_wdt_map))
+		return -EINVAL;
+
+	ret = regmap_update_bits(wdt->rn5t618->regmap, RN5T618_WATCHDOG,
+				 RN5T618_WATCHDOG_WDOGTIM_M,
+				 rn5t618_wdt_map[i].reg_val);
+	if (!ret)
+		wdt_dev->timeout = rn5t618_wdt_map[i].time;
+
+	return ret;
+}
+
+static int rn5t618_wdt_start(struct watchdog_device *wdt_dev)
+{
+	struct rn5t618_wdt *wdt = watchdog_get_drvdata(wdt_dev);
+	int ret;
+
+	ret = rn5t618_wdt_set_timeout(wdt_dev, wdt_dev->timeout);
+	if (ret)
+		return ret;
+
+	/* enable repower-on */
+	ret = regmap_update_bits(wdt->rn5t618->regmap, RN5T618_REPCNT,
+				 RN5T618_REPCNT_REPWRON,
+				 RN5T618_REPCNT_REPWRON);
+	if (ret)
+		return ret;
+
+	/* enable watchdog */
+	ret = regmap_update_bits(wdt->rn5t618->regmap, RN5T618_WATCHDOG,
+				 RN5T618_WATCHDOG_WDOGEN,
+				 RN5T618_WATCHDOG_WDOGEN);
+	if (ret)
+		return ret;
+
+	/* enable watchdog interrupt */
+	return regmap_update_bits(wdt->rn5t618->regmap, RN5T618_PWRIREN,
+				  RN5T618_PWRIRQ_IR_WDOG,
+				  RN5T618_PWRIRQ_IR_WDOG);
+}
+
+static int rn5t618_wdt_stop(struct watchdog_device *wdt_dev)
+{
+	struct rn5t618_wdt *wdt = watchdog_get_drvdata(wdt_dev);
+
+	return regmap_update_bits(wdt->rn5t618->regmap, RN5T618_WATCHDOG,
+				  RN5T618_WATCHDOG_WDOGEN, 0);
+}
+
+static int rn5t618_wdt_ping(struct watchdog_device *wdt_dev)
+{
+	struct rn5t618_wdt *wdt = watchdog_get_drvdata(wdt_dev);
+	unsigned int val;
+	int ret;
+
+	/* The counter is restarted after a R/W access to watchdog register */
+	ret = regmap_read(wdt->rn5t618->regmap, RN5T618_WATCHDOG, &val);
+	if (ret)
+		return ret;
+
+	ret = regmap_write(wdt->rn5t618->regmap, RN5T618_WATCHDOG, val);
+	if (ret)
+		return ret;
+
+	/* Clear pending watchdog interrupt */
+	return regmap_update_bits(wdt->rn5t618->regmap, RN5T618_PWRIRQ,
+				  RN5T618_PWRIRQ_IR_WDOG, 0);
+}
+
+static struct watchdog_info rn5t618_wdt_info = {
+	.options	= WDIOF_SETTIMEOUT | WDIOF_MAGICCLOSE |
+			  WDIOF_KEEPALIVEPING,
+	.identity	= DRIVER_NAME,
+};
+
+static struct watchdog_ops rn5t618_wdt_ops = {
+	.owner          = THIS_MODULE,
+	.start          = rn5t618_wdt_start,
+	.stop           = rn5t618_wdt_stop,
+	.ping           = rn5t618_wdt_ping,
+	.set_timeout    = rn5t618_wdt_set_timeout,
+};
+
+static int rn5t618_wdt_probe(struct platform_device *pdev)
+{
+	struct rn5t618 *rn5t618 = dev_get_drvdata(pdev->dev.parent);
+	struct rn5t618_wdt *wdt;
+	int min_timeout, max_timeout;
+
+	wdt = devm_kzalloc(&pdev->dev, sizeof(struct rn5t618_wdt), GFP_KERNEL);
+	if (!wdt)
+		return -ENOMEM;
+
+	min_timeout = rn5t618_wdt_map[0].time;
+	max_timeout = rn5t618_wdt_map[ARRAY_SIZE(rn5t618_wdt_map) - 1].time;
+
+	wdt->rn5t618 = rn5t618;
+	wdt->wdt_dev.info = &rn5t618_wdt_info;
+	wdt->wdt_dev.ops = &rn5t618_wdt_ops;
+	wdt->wdt_dev.min_timeout = min_timeout;
+	wdt->wdt_dev.max_timeout = max_timeout;
+	wdt->wdt_dev.timeout = max_timeout;
+	wdt->wdt_dev.parent = &pdev->dev;
+
+	watchdog_set_drvdata(&wdt->wdt_dev, wdt);
+	watchdog_init_timeout(&wdt->wdt_dev, timeout, &pdev->dev);
+	watchdog_set_nowayout(&wdt->wdt_dev, nowayout);
+
+	platform_set_drvdata(pdev, wdt);
+
+	return watchdog_register_device(&wdt->wdt_dev);
+}
+
+static int rn5t618_wdt_remove(struct platform_device *pdev)
+{
+	struct rn5t618_wdt *wdt = platform_get_drvdata(pdev);
+
+	watchdog_unregister_device(&wdt->wdt_dev);
+
+	return 0;
+}
+
+static struct platform_driver rn5t618_wdt_driver = {
+	.probe = rn5t618_wdt_probe,
+	.remove = rn5t618_wdt_remove,
+	.driver = {
+		.name	= DRIVER_NAME,
+	},
+};
+
+module_platform_driver(rn5t618_wdt_driver);
+
+MODULE_AUTHOR("Beniamino Galvani <b.galvani@gmail.com>");
+MODULE_DESCRIPTION("RN5T618 watchdog driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/watchdog/s3c2410_wdt.c b/drivers/watchdog/s3c2410_wdt.c
index 7c6ccd071baf..8532c3e2aea7 100644
--- a/drivers/watchdog/s3c2410_wdt.c
+++ b/drivers/watchdog/s3c2410_wdt.c
@@ -41,6 +41,8 @@
 #include <linux/of.h>
 #include <linux/mfd/syscon.h>
 #include <linux/regmap.h>
+#include <linux/reboot.h>
+#include <linux/delay.h>
 
 #define S3C2410_WTCON		0x00
 #define S3C2410_WTDAT		0x04
@@ -128,6 +130,7 @@ struct s3c2410_wdt {
 	unsigned long		wtdat_save;
 	struct watchdog_device	wdt_device;
 	struct notifier_block	freq_transition;
+	struct notifier_block	restart_handler;
 	struct s3c2410_wdt_variant *drv_data;
 	struct regmap *pmureg;
 };
@@ -155,6 +158,15 @@ static const struct s3c2410_wdt_variant drv_data_exynos5420 = {
 	.quirks = QUIRK_HAS_PMU_CONFIG | QUIRK_HAS_RST_STAT,
 };
 
+static const struct s3c2410_wdt_variant drv_data_exynos7 = {
+	.disable_reg = EXYNOS5_WDT_DISABLE_REG_OFFSET,
+	.mask_reset_reg = EXYNOS5_WDT_MASK_RESET_REG_OFFSET,
+	.mask_bit = 0,
+	.rst_stat_reg = EXYNOS5_RST_STAT_REG_OFFSET,
+	.rst_stat_bit = 23,	/* A57 WDTRESET */
+	.quirks = QUIRK_HAS_PMU_CONFIG | QUIRK_HAS_RST_STAT,
+};
+
 static const struct of_device_id s3c2410_wdt_match[] = {
 	{ .compatible = "samsung,s3c2410-wdt",
 	  .data = &drv_data_s3c2410 },
@@ -162,6 +174,8 @@ static const struct of_device_id s3c2410_wdt_match[] = {
 	  .data = &drv_data_exynos5250 },
 	{ .compatible = "samsung,exynos5420-wdt",
 	  .data = &drv_data_exynos5420 },
+	{ .compatible = "samsung,exynos7-wdt",
+	  .data = &drv_data_exynos7 },
 	{},
 };
 MODULE_DEVICE_TABLE(of, s3c2410_wdt_match);
@@ -438,6 +452,31 @@ static inline void s3c2410wdt_cpufreq_deregister(struct s3c2410_wdt *wdt)
 }
 #endif
 
+static int s3c2410wdt_restart(struct notifier_block *this,
+			      unsigned long mode, void *cmd)
+{
+	struct s3c2410_wdt *wdt = container_of(this, struct s3c2410_wdt,
+					       restart_handler);
+	void __iomem *wdt_base = wdt->reg_base;
+
+	/* disable watchdog, to be safe  */
+	writel(0, wdt_base + S3C2410_WTCON);
+
+	/* put initial values into count and data */
+	writel(0x80, wdt_base + S3C2410_WTCNT);
+	writel(0x80, wdt_base + S3C2410_WTDAT);
+
+	/* set the watchdog to go and reset... */
+	writel(S3C2410_WTCON_ENABLE | S3C2410_WTCON_DIV16 |
+		S3C2410_WTCON_RSTEN | S3C2410_WTCON_PRESCALE(0x20),
+		wdt_base + S3C2410_WTCON);
+
+	/* wait for reset to assert... */
+	mdelay(500);
+
+	return NOTIFY_DONE;
+}
+
 static inline unsigned int s3c2410wdt_get_bootstatus(struct s3c2410_wdt *wdt)
 {
 	unsigned int rst_stat;
@@ -592,6 +631,12 @@ static int s3c2410wdt_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, wdt);
 
+	wdt->restart_handler.notifier_call = s3c2410wdt_restart;
+	wdt->restart_handler.priority = 128;
+	ret = register_restart_handler(&wdt->restart_handler);
+	if (ret)
+		pr_err("cannot register restart handler, %d\n", ret);
+
 	/* print out a statement of readiness */
 
 	wtcon = readl(wdt->reg_base + S3C2410_WTCON);
@@ -621,6 +666,8 @@ static int s3c2410wdt_remove(struct platform_device *dev)
 	int ret;
 	struct s3c2410_wdt *wdt = platform_get_drvdata(dev);
 
+	unregister_restart_handler(&wdt->restart_handler);
+
 	ret = s3c2410wdt_mask_and_disable_reset(wdt, true);
 	if (ret < 0)
 		return ret;
diff --git a/drivers/watchdog/stmp3xxx_rtc_wdt.c b/drivers/watchdog/stmp3xxx_rtc_wdt.c
index 3804d5e9baea..a62b1b6decf4 100644
--- a/drivers/watchdog/stmp3xxx_rtc_wdt.c
+++ b/drivers/watchdog/stmp3xxx_rtc_wdt.c
@@ -94,9 +94,33 @@ static int stmp3xxx_wdt_remove(struct platform_device *pdev)
 	return 0;
 }
 
+static int __maybe_unused stmp3xxx_wdt_suspend(struct device *dev)
+{
+	struct watchdog_device *wdd = &stmp3xxx_wdd;
+
+	if (watchdog_active(wdd))
+		return wdt_stop(wdd);
+
+	return 0;
+}
+
+static int __maybe_unused stmp3xxx_wdt_resume(struct device *dev)
+{
+	struct watchdog_device *wdd = &stmp3xxx_wdd;
+
+	if (watchdog_active(wdd))
+		return wdt_start(wdd);
+
+	return 0;
+}
+
+static SIMPLE_DEV_PM_OPS(stmp3xxx_wdt_pm_ops,
+			 stmp3xxx_wdt_suspend, stmp3xxx_wdt_resume);
+
 static struct platform_driver stmp3xxx_wdt_driver = {
 	.driver = {
 		.name = "stmp3xxx_rtc_wdt",
+		.pm = &stmp3xxx_wdt_pm_ops,
 	},
 	.probe = stmp3xxx_wdt_probe,
 	.remove = stmp3xxx_wdt_remove,
diff --git a/drivers/watchdog/sunxi_wdt.c b/drivers/watchdog/sunxi_wdt.c
index 60deb9d304c0..b62301e74e5f 100644
--- a/drivers/watchdog/sunxi_wdt.c
+++ b/drivers/watchdog/sunxi_wdt.c
@@ -21,25 +21,21 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/moduleparam.h>
+#include <linux/notifier.h>
 #include <linux/of.h>
+#include <linux/of_device.h>
 #include <linux/platform_device.h>
 #include <linux/reboot.h>
 #include <linux/types.h>
 #include <linux/watchdog.h>
 
-#include <asm/system_misc.h>
-
 #define WDT_MAX_TIMEOUT         16
 #define WDT_MIN_TIMEOUT         1
-#define WDT_MODE_TIMEOUT(n)     ((n) << 3)
-#define WDT_TIMEOUT_MASK        WDT_MODE_TIMEOUT(0x0F)
+#define WDT_TIMEOUT_MASK        0x0F
 
-#define WDT_CTRL                0x00
 #define WDT_CTRL_RELOAD         ((1 << 0) | (0x0a57 << 1))
 
-#define WDT_MODE                0x04
 #define WDT_MODE_EN             (1 << 0)
-#define WDT_MODE_RST_EN         (1 << 1)
 
 #define DRV_NAME		"sunxi-wdt"
 #define DRV_VERSION		"1.0"
@@ -47,14 +43,29 @@
 static bool nowayout = WATCHDOG_NOWAYOUT;
 static unsigned int timeout = WDT_MAX_TIMEOUT;
 
+/*
+ * This structure stores the register offsets for different variants
+ * of Allwinner's watchdog hardware.
+ */
+struct sunxi_wdt_reg {
+	u8 wdt_ctrl;
+	u8 wdt_cfg;
+	u8 wdt_mode;
+	u8 wdt_timeout_shift;
+	u8 wdt_reset_mask;
+	u8 wdt_reset_val;
+};
+
 struct sunxi_wdt_dev {
 	struct watchdog_device wdt_dev;
 	void __iomem *wdt_base;
+	const struct sunxi_wdt_reg *wdt_regs;
+	struct notifier_block restart_handler;
 };
 
 /*
  * wdt_timeout_map maps the watchdog timer interval value in seconds to
- * the value of the register WDT_MODE bit 3:6
+ * the value of the register WDT_MODE at bits .wdt_timeout_shift ~ +3
  *
  * [timeout seconds] = register value
  *
@@ -74,32 +85,51 @@ static const int wdt_timeout_map[] = {
 	[16] = 0xB, /* 16s */
 };
 
-static void __iomem *reboot_wdt_base;
 
-static void sun4i_wdt_restart(enum reboot_mode mode, const char *cmd)
+static int sunxi_restart_handle(struct notifier_block *this, unsigned long mode,
+				void *cmd)
 {
-	/* Enable timer and set reset bit in the watchdog */
-	writel(WDT_MODE_EN | WDT_MODE_RST_EN, reboot_wdt_base + WDT_MODE);
+	struct sunxi_wdt_dev *sunxi_wdt = container_of(this,
+						       struct sunxi_wdt_dev,
+						       restart_handler);
+	void __iomem *wdt_base = sunxi_wdt->wdt_base;
+	const struct sunxi_wdt_reg *regs = sunxi_wdt->wdt_regs;
+	u32 val;
+
+	/* Set system reset function */
+	val = readl(wdt_base + regs->wdt_cfg);
+	val &= ~(regs->wdt_reset_mask);
+	val |= regs->wdt_reset_val;
+	writel(val, wdt_base + regs->wdt_cfg);
+
+	/* Set lowest timeout and enable watchdog */
+	val = readl(wdt_base + regs->wdt_mode);
+	val &= ~(WDT_TIMEOUT_MASK << regs->wdt_timeout_shift);
+	val |= WDT_MODE_EN;
+	writel(val, wdt_base + regs->wdt_mode);
 
 	/*
 	 * Restart the watchdog. The default (and lowest) interval
 	 * value for the watchdog is 0.5s.
 	 */
-	writel(WDT_CTRL_RELOAD, reboot_wdt_base + WDT_CTRL);
+	writel(WDT_CTRL_RELOAD, wdt_base + regs->wdt_ctrl);
 
 	while (1) {
 		mdelay(5);
-		writel(WDT_MODE_EN | WDT_MODE_RST_EN,
-		       reboot_wdt_base + WDT_MODE);
+		val = readl(wdt_base + regs->wdt_mode);
+		val |= WDT_MODE_EN;
+		writel(val, wdt_base + regs->wdt_mode);
 	}
+	return NOTIFY_DONE;
 }
 
 static int sunxi_wdt_ping(struct watchdog_device *wdt_dev)
 {
 	struct sunxi_wdt_dev *sunxi_wdt = watchdog_get_drvdata(wdt_dev);
 	void __iomem *wdt_base = sunxi_wdt->wdt_base;
+	const struct sunxi_wdt_reg *regs = sunxi_wdt->wdt_regs;
 
-	iowrite32(WDT_CTRL_RELOAD, wdt_base + WDT_CTRL);
+	writel(WDT_CTRL_RELOAD, wdt_base + regs->wdt_ctrl);
 
 	return 0;
 }
@@ -109,6 +139,7 @@ static int sunxi_wdt_set_timeout(struct watchdog_device *wdt_dev,
 {
 	struct sunxi_wdt_dev *sunxi_wdt = watchdog_get_drvdata(wdt_dev);
 	void __iomem *wdt_base = sunxi_wdt->wdt_base;
+	const struct sunxi_wdt_reg *regs = sunxi_wdt->wdt_regs;
 	u32 reg;
 
 	if (wdt_timeout_map[timeout] == 0)
@@ -116,10 +147,10 @@ static int sunxi_wdt_set_timeout(struct watchdog_device *wdt_dev,
 
 	sunxi_wdt->wdt_dev.timeout = timeout;
 
-	reg = ioread32(wdt_base + WDT_MODE);
-	reg &= ~WDT_TIMEOUT_MASK;
-	reg |= WDT_MODE_TIMEOUT(wdt_timeout_map[timeout]);
-	iowrite32(reg, wdt_base + WDT_MODE);
+	reg = readl(wdt_base + regs->wdt_mode);
+	reg &= ~(WDT_TIMEOUT_MASK << regs->wdt_timeout_shift);
+	reg |= wdt_timeout_map[timeout] << regs->wdt_timeout_shift;
+	writel(reg, wdt_base + regs->wdt_mode);
 
 	sunxi_wdt_ping(wdt_dev);
 
@@ -130,8 +161,9 @@ static int sunxi_wdt_stop(struct watchdog_device *wdt_dev)
 {
 	struct sunxi_wdt_dev *sunxi_wdt = watchdog_get_drvdata(wdt_dev);
 	void __iomem *wdt_base = sunxi_wdt->wdt_base;
+	const struct sunxi_wdt_reg *regs = sunxi_wdt->wdt_regs;
 
-	iowrite32(0, wdt_base + WDT_MODE);
+	writel(0, wdt_base + regs->wdt_mode);
 
 	return 0;
 }
@@ -141,6 +173,7 @@ static int sunxi_wdt_start(struct watchdog_device *wdt_dev)
 	u32 reg;
 	struct sunxi_wdt_dev *sunxi_wdt = watchdog_get_drvdata(wdt_dev);
 	void __iomem *wdt_base = sunxi_wdt->wdt_base;
+	const struct sunxi_wdt_reg *regs = sunxi_wdt->wdt_regs;
 	int ret;
 
 	ret = sunxi_wdt_set_timeout(&sunxi_wdt->wdt_dev,
@@ -148,9 +181,16 @@ static int sunxi_wdt_start(struct watchdog_device *wdt_dev)
 	if (ret < 0)
 		return ret;
 
-	reg = ioread32(wdt_base + WDT_MODE);
-	reg |= (WDT_MODE_RST_EN | WDT_MODE_EN);
-	iowrite32(reg, wdt_base + WDT_MODE);
+	/* Set system reset function */
+	reg = readl(wdt_base + regs->wdt_cfg);
+	reg &= ~(regs->wdt_reset_mask);
+	reg |= ~(regs->wdt_reset_val);
+	writel(reg, wdt_base + regs->wdt_cfg);
+
+	/* Enable watchdog */
+	reg = readl(wdt_base + regs->wdt_mode);
+	reg |= WDT_MODE_EN;
+	writel(reg, wdt_base + regs->wdt_mode);
 
 	return 0;
 }
@@ -170,9 +210,35 @@ static const struct watchdog_ops sunxi_wdt_ops = {
 	.set_timeout	= sunxi_wdt_set_timeout,
 };
 
+static const struct sunxi_wdt_reg sun4i_wdt_reg = {
+	.wdt_ctrl = 0x00,
+	.wdt_cfg = 0x04,
+	.wdt_mode = 0x04,
+	.wdt_timeout_shift = 3,
+	.wdt_reset_mask = 0x02,
+	.wdt_reset_val = 0x02,
+};
+
+static const struct sunxi_wdt_reg sun6i_wdt_reg = {
+	.wdt_ctrl = 0x10,
+	.wdt_cfg = 0x14,
+	.wdt_mode = 0x18,
+	.wdt_timeout_shift = 4,
+	.wdt_reset_mask = 0x03,
+	.wdt_reset_val = 0x01,
+};
+
+static const struct of_device_id sunxi_wdt_dt_ids[] = {
+	{ .compatible = "allwinner,sun4i-a10-wdt", .data = &sun4i_wdt_reg },
+	{ .compatible = "allwinner,sun6i-a31-wdt", .data = &sun6i_wdt_reg },
+	{ /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, sunxi_wdt_dt_ids);
+
 static int sunxi_wdt_probe(struct platform_device *pdev)
 {
 	struct sunxi_wdt_dev *sunxi_wdt;
+	const struct of_device_id *device;
 	struct resource *res;
 	int err;
 
@@ -182,6 +248,12 @@ static int sunxi_wdt_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, sunxi_wdt);
 
+	device = of_match_device(sunxi_wdt_dt_ids, &pdev->dev);
+	if (!device)
+		return -ENODEV;
+
+	sunxi_wdt->wdt_regs = device->data;
+
 	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
 	sunxi_wdt->wdt_base = devm_ioremap_resource(&pdev->dev, res);
 	if (IS_ERR(sunxi_wdt->wdt_base))
@@ -205,8 +277,12 @@ static int sunxi_wdt_probe(struct platform_device *pdev)
 	if (unlikely(err))
 		return err;
 
-	reboot_wdt_base = sunxi_wdt->wdt_base;
-	arm_pm_restart = sun4i_wdt_restart;
+	sunxi_wdt->restart_handler.notifier_call = sunxi_restart_handle;
+	sunxi_wdt->restart_handler.priority = 128;
+	err = register_restart_handler(&sunxi_wdt->restart_handler);
+	if (err)
+		dev_err(&pdev->dev,
+			"cannot register restart handler (err=%d)\n", err);
 
 	dev_info(&pdev->dev, "Watchdog enabled (timeout=%d sec, nowayout=%d)",
 			sunxi_wdt->wdt_dev.timeout, nowayout);
@@ -218,7 +294,7 @@ static int sunxi_wdt_remove(struct platform_device *pdev)
 {
 	struct sunxi_wdt_dev *sunxi_wdt = platform_get_drvdata(pdev);
 
-	arm_pm_restart = NULL;
+	unregister_restart_handler(&sunxi_wdt->restart_handler);
 
 	watchdog_unregister_device(&sunxi_wdt->wdt_dev);
 	watchdog_set_drvdata(&sunxi_wdt->wdt_dev, NULL);
@@ -233,12 +309,6 @@ static void sunxi_wdt_shutdown(struct platform_device *pdev)
 	sunxi_wdt_stop(&sunxi_wdt->wdt_dev);
 }
 
-static const struct of_device_id sunxi_wdt_dt_ids[] = {
-	{ .compatible = "allwinner,sun4i-a10-wdt" },
-	{ /* sentinel */ }
-};
-MODULE_DEVICE_TABLE(of, sunxi_wdt_dt_ids);
-
 static struct platform_driver sunxi_wdt_driver = {
 	.probe		= sunxi_wdt_probe,
 	.remove		= sunxi_wdt_remove,
diff --git a/drivers/watchdog/ts72xx_wdt.c b/drivers/watchdog/ts72xx_wdt.c
index afa9d6ef353a..dee9c6cbe6df 100644
--- a/drivers/watchdog/ts72xx_wdt.c
+++ b/drivers/watchdog/ts72xx_wdt.c
@@ -428,11 +428,7 @@ static int ts72xx_wdt_probe(struct platform_device *pdev)
 
 static int ts72xx_wdt_remove(struct platform_device *pdev)
 {
-	int error;
-
-	error = misc_deregister(&ts72xx_wdt_miscdev);
-
-	return error;
+	return misc_deregister(&ts72xx_wdt_miscdev);
 }
 
 static struct platform_driver ts72xx_wdt_driver = {
diff --git a/drivers/xen/Kconfig b/drivers/xen/Kconfig
index 8bc01838daf9..b812462083fc 100644
--- a/drivers/xen/Kconfig
+++ b/drivers/xen/Kconfig
@@ -172,6 +172,15 @@ config XEN_PCIDEV_BACKEND
 
 	  If in doubt, say m.
 
+config XEN_SCSI_BACKEND
+	tristate "XEN SCSI backend driver"
+	depends on XEN && XEN_BACKEND && TARGET_CORE
+	help
+	  The SCSI backend driver allows the kernel to export its SCSI Devices
+	  to other guests via a high-performance shared-memory interface.
+	  Only needed for systems running as XEN driver domains (e.g. Dom0) and
+	  if guests need generic access to SCSI devices.
+
 config XEN_PRIVCMD
 	tristate
 	depends on XEN
diff --git a/drivers/xen/Makefile b/drivers/xen/Makefile
index 84044b554e33..2140398a2a8c 100644
--- a/drivers/xen/Makefile
+++ b/drivers/xen/Makefile
@@ -36,6 +36,7 @@ obj-$(CONFIG_XEN_ACPI_HOTPLUG_MEMORY)	+= xen-acpi-memhotplug.o
 obj-$(CONFIG_XEN_ACPI_HOTPLUG_CPU)	+= xen-acpi-cpuhotplug.o
 obj-$(CONFIG_XEN_ACPI_PROCESSOR)	+= xen-acpi-processor.o
 obj-$(CONFIG_XEN_EFI)			+= efi.o
+obj-$(CONFIG_XEN_SCSI_BACKEND)		+= xen-scsiback.o
 xen-evtchn-y				:= evtchn.o
 xen-gntdev-y				:= gntdev.o
 xen-gntalloc-y				:= gntalloc.o
diff --git a/drivers/xen/balloon.c b/drivers/xen/balloon.c
index 5c660c77f03b..1e0a317d3dcd 100644
--- a/drivers/xen/balloon.c
+++ b/drivers/xen/balloon.c
@@ -230,8 +230,8 @@ static enum bp_state reserve_additional_memory(long credit)
 	rc = add_memory(nid, hotplug_start_paddr, balloon_hotplug << PAGE_SHIFT);
 
 	if (rc) {
-		pr_info("%s: add_memory() failed: %i\n", __func__, rc);
-		return BP_EAGAIN;
+		pr_warn("Cannot add additional memory (%i)\n", rc);
+		return BP_ECANCELED;
 	}
 
 	balloon_hotplug -= credit;
diff --git a/drivers/xen/efi.c b/drivers/xen/efi.c
index 31f618a49661..1f850c97482f 100644
--- a/drivers/xen/efi.c
+++ b/drivers/xen/efi.c
@@ -27,6 +27,8 @@
 #include <xen/interface/platform.h>
 #include <xen/xen.h>
 
+#include <asm/page.h>
+
 #include <asm/xen/hypercall.h>
 
 #define INIT_EFI_OP(name) \
diff --git a/drivers/xen/events/events_base.c b/drivers/xen/events/events_base.c
index 5b5c5ff273fd..b4bca2d4a7e5 100644
--- a/drivers/xen/events/events_base.c
+++ b/drivers/xen/events/events_base.c
@@ -900,8 +900,8 @@ static int bind_ipi_to_irq(unsigned int ipi, unsigned int cpu)
 	return irq;
 }
 
-static int bind_interdomain_evtchn_to_irq(unsigned int remote_domain,
-					  unsigned int remote_port)
+int bind_interdomain_evtchn_to_irq(unsigned int remote_domain,
+				   unsigned int remote_port)
 {
 	struct evtchn_bind_interdomain bind_interdomain;
 	int err;
@@ -914,6 +914,7 @@ static int bind_interdomain_evtchn_to_irq(unsigned int remote_domain,
 
 	return err ? : bind_evtchn_to_irq(bind_interdomain.local_port);
 }
+EXPORT_SYMBOL_GPL(bind_interdomain_evtchn_to_irq);
 
 static int find_virq(unsigned int virq, unsigned int cpu)
 {
diff --git a/drivers/xen/gntalloc.c b/drivers/xen/gntalloc.c
index 787d17945418..e53fe191738c 100644
--- a/drivers/xen/gntalloc.c
+++ b/drivers/xen/gntalloc.c
@@ -124,7 +124,7 @@ static int add_grefs(struct ioctl_gntalloc_alloc_gref *op,
 	int i, rc, readonly;
 	LIST_HEAD(queue_gref);
 	LIST_HEAD(queue_file);
-	struct gntalloc_gref *gref;
+	struct gntalloc_gref *gref, *next;
 
 	readonly = !(op->flags & GNTALLOC_FLAG_WRITABLE);
 	rc = -ENOMEM;
@@ -141,13 +141,11 @@ static int add_grefs(struct ioctl_gntalloc_alloc_gref *op,
 			goto undo;
 
 		/* Grant foreign access to the page. */
-		gref->gref_id = gnttab_grant_foreign_access(op->domid,
+		rc = gnttab_grant_foreign_access(op->domid,
 			pfn_to_mfn(page_to_pfn(gref->page)), readonly);
-		if ((int)gref->gref_id < 0) {
-			rc = gref->gref_id;
+		if (rc < 0)
 			goto undo;
-		}
-		gref_ids[i] = gref->gref_id;
+		gref_ids[i] = gref->gref_id = rc;
 	}
 
 	/* Add to gref lists. */
@@ -162,8 +160,8 @@ undo:
 	mutex_lock(&gref_mutex);
 	gref_size -= (op->count - i);
 
-	list_for_each_entry(gref, &queue_file, next_file) {
-		/* __del_gref does not remove from queue_file */
+	list_for_each_entry_safe(gref, next, &queue_file, next_file) {
+		list_del(&gref->next_file);
 		__del_gref(gref);
 	}
 
@@ -193,7 +191,7 @@ static void __del_gref(struct gntalloc_gref *gref)
 
 	gref->notify.flags = 0;
 
-	if (gref->gref_id > 0) {
+	if (gref->gref_id) {
 		if (gnttab_query_foreign_access(gref->gref_id))
 			return;
 
diff --git a/drivers/xen/grant-table.c b/drivers/xen/grant-table.c
index c254ae036f18..7786291ba229 100644
--- a/drivers/xen/grant-table.c
+++ b/drivers/xen/grant-table.c
@@ -592,7 +592,7 @@ static int grow_gnttab_list(unsigned int more_frames)
 	return 0;
 
 grow_nomem:
-	for ( ; i >= nr_glist_frames; i--)
+	while (i-- > nr_glist_frames)
 		free_page((unsigned long) gnttab_list[i]);
 	return -ENOMEM;
 }
diff --git a/drivers/xen/manage.c b/drivers/xen/manage.c
index 5f1e1f3cd186..f8bb36f9d9ce 100644
--- a/drivers/xen/manage.c
+++ b/drivers/xen/manage.c
@@ -103,16 +103,11 @@ static void do_suspend(void)
 
 	shutting_down = SHUTDOWN_SUSPEND;
 
-#ifdef CONFIG_PREEMPT
-	/* If the kernel is preemptible, we need to freeze all the processes
-	   to prevent them from being in the middle of a pagetable update
-	   during suspend. */
 	err = freeze_processes();
 	if (err) {
 		pr_err("%s: freeze failed %d\n", __func__, err);
 		goto out;
 	}
-#endif
 
 	err = dpm_suspend_start(PMSG_FREEZE);
 	if (err) {
@@ -157,10 +152,8 @@ out_resume:
 	dpm_resume_end(si.cancelled ? PMSG_THAW : PMSG_RESTORE);
 
 out_thaw:
-#ifdef CONFIG_PREEMPT
 	thaw_processes();
 out:
-#endif
 	shutting_down = SHUTDOWN_INVALID;
 }
 #endif	/* CONFIG_HIBERNATE_CALLBACKS */
diff --git a/drivers/xen/xen-pciback/pci_stub.c b/drivers/xen/xen-pciback/pci_stub.c
index 259ba2661543..017069a455d4 100644
--- a/drivers/xen/xen-pciback/pci_stub.c
+++ b/drivers/xen/xen-pciback/pci_stub.c
@@ -133,7 +133,7 @@ static void pcistub_device_release(struct kref *kref)
 	xen_pcibk_config_free_dyn_fields(dev);
 	xen_pcibk_config_free_dev(dev);
 
-	dev->dev_flags &= ~PCI_DEV_FLAGS_ASSIGNED;
+	pci_clear_dev_assigned(dev);
 	pci_dev_put(dev);
 
 	kfree(psdev);
@@ -413,7 +413,7 @@ static int pcistub_init_device(struct pci_dev *dev)
 	dev_dbg(&dev->dev, "reset device\n");
 	xen_pcibk_reset_device(dev);
 
-	dev->dev_flags |= PCI_DEV_FLAGS_ASSIGNED;
+	pci_set_dev_assigned(dev);
 	return 0;
 
 config_release:
diff --git a/drivers/xen/xen-pciback/xenbus.c b/drivers/xen/xen-pciback/xenbus.c
index c214daab4829..ad8d30c088fe 100644
--- a/drivers/xen/xen-pciback/xenbus.c
+++ b/drivers/xen/xen-pciback/xenbus.c
@@ -719,11 +719,13 @@ static const struct xenbus_device_id xen_pcibk_ids[] = {
 	{""},
 };
 
-static DEFINE_XENBUS_DRIVER(xen_pcibk, DRV_NAME,
+static struct xenbus_driver xen_pcibk_driver = {
+	.name                   = DRV_NAME,
+	.ids                    = xen_pcibk_ids,
 	.probe			= xen_pcibk_xenbus_probe,
 	.remove			= xen_pcibk_xenbus_remove,
 	.otherend_changed	= xen_pcibk_frontend_changed,
-);
+};
 
 const struct xen_pcibk_backend *__read_mostly xen_pcibk_backend;
 
diff --git a/drivers/xen/xen-scsiback.c b/drivers/xen/xen-scsiback.c
new file mode 100644
index 000000000000..3e32146472a5
--- /dev/null
+++ b/drivers/xen/xen-scsiback.c
@@ -0,0 +1,2126 @@
+/*
+ * Xen SCSI backend driver
+ *
+ * Copyright (c) 2008, FUJITSU Limited
+ *
+ * Based on the blkback driver code.
+ * Adaption to kernel taget core infrastructure taken from vhost/scsi.c
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation; or, when distributed
+ * separately from the Linux kernel or incorporated into other
+ * software packages, subject to the following license:
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this source file (the "Software"), to deal in the Software without
+ * restriction, including without limitation the rights to use, copy, modify,
+ * merge, publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so, subject to
+ * the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
+ * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
+ * IN THE SOFTWARE.
+ */
+
+#include <stdarg.h>
+
+#include <linux/module.h>
+#include <linux/utsname.h>
+#include <linux/interrupt.h>
+#include <linux/slab.h>
+#include <linux/wait.h>
+#include <linux/sched.h>
+#include <linux/list.h>
+#include <linux/gfp.h>
+#include <linux/delay.h>
+#include <linux/spinlock.h>
+#include <linux/configfs.h>
+
+#include <generated/utsrelease.h>
+
+#include <scsi/scsi_dbg.h>
+#include <scsi/scsi_eh.h>
+#include <scsi/scsi_tcq.h>
+
+#include <target/target_core_base.h>
+#include <target/target_core_fabric.h>
+#include <target/target_core_configfs.h>
+#include <target/target_core_fabric_configfs.h>
+
+#include <asm/hypervisor.h>
+
+#include <xen/xen.h>
+#include <xen/balloon.h>
+#include <xen/events.h>
+#include <xen/xenbus.h>
+#include <xen/grant_table.h>
+#include <xen/page.h>
+
+#include <xen/interface/grant_table.h>
+#include <xen/interface/io/vscsiif.h>
+
+#define DPRINTK(_f, _a...)			\
+	pr_debug("(file=%s, line=%d) " _f, __FILE__ , __LINE__ , ## _a)
+
+#define VSCSI_VERSION	"v0.1"
+#define VSCSI_NAMELEN	32
+
+struct ids_tuple {
+	unsigned int hst;		/* host    */
+	unsigned int chn;		/* channel */
+	unsigned int tgt;		/* target  */
+	unsigned int lun;		/* LUN     */
+};
+
+struct v2p_entry {
+	struct ids_tuple v;		/* translate from */
+	struct scsiback_tpg *tpg;	/* translate to   */
+	unsigned int lun;
+	struct kref kref;
+	struct list_head l;
+};
+
+struct vscsibk_info {
+	struct xenbus_device *dev;
+
+	domid_t domid;
+	unsigned int irq;
+
+	struct vscsiif_back_ring ring;
+	int ring_error;
+
+	spinlock_t ring_lock;
+	atomic_t nr_unreplied_reqs;
+
+	spinlock_t v2p_lock;
+	struct list_head v2p_entry_lists;
+
+	wait_queue_head_t waiting_to_free;
+};
+
+/* theoretical maximum of grants for one request */
+#define VSCSI_MAX_GRANTS	(SG_ALL + VSCSIIF_SG_TABLESIZE)
+
+/*
+ * VSCSI_GRANT_BATCH is the maximum number of grants to be processed in one
+ * call to map/unmap grants. Don't choose it too large, as there are arrays
+ * with VSCSI_GRANT_BATCH elements allocated on the stack.
+ */
+#define VSCSI_GRANT_BATCH	16
+
+struct vscsibk_pend {
+	uint16_t rqid;
+
+	uint8_t cmnd[VSCSIIF_MAX_COMMAND_SIZE];
+	uint8_t cmd_len;
+
+	uint8_t sc_data_direction;
+	uint16_t n_sg;		/* real length of SG list */
+	uint16_t n_grants;	/* SG pages and potentially SG list */
+	uint32_t data_len;
+	uint32_t result;
+
+	struct vscsibk_info *info;
+	struct v2p_entry *v2p;
+	struct scatterlist *sgl;
+
+	uint8_t sense_buffer[VSCSIIF_SENSE_BUFFERSIZE];
+
+	grant_handle_t grant_handles[VSCSI_MAX_GRANTS];
+	struct page *pages[VSCSI_MAX_GRANTS];
+
+	struct se_cmd se_cmd;
+};
+
+struct scsiback_tmr {
+	atomic_t tmr_complete;
+	wait_queue_head_t tmr_wait;
+};
+
+struct scsiback_nexus {
+	/* Pointer to TCM session for I_T Nexus */
+	struct se_session *tvn_se_sess;
+};
+
+struct scsiback_tport {
+	/* SCSI protocol the tport is providing */
+	u8 tport_proto_id;
+	/* Binary World Wide unique Port Name for pvscsi Target port */
+	u64 tport_wwpn;
+	/* ASCII formatted WWPN for pvscsi Target port */
+	char tport_name[VSCSI_NAMELEN];
+	/* Returned by scsiback_make_tport() */
+	struct se_wwn tport_wwn;
+};
+
+struct scsiback_tpg {
+	/* scsiback port target portal group tag for TCM */
+	u16 tport_tpgt;
+	/* track number of TPG Port/Lun Links wrt explicit I_T Nexus shutdown */
+	int tv_tpg_port_count;
+	/* xen-pvscsi references to tpg_nexus, protected by tv_tpg_mutex */
+	int tv_tpg_fe_count;
+	/* list for scsiback_list */
+	struct list_head tv_tpg_list;
+	/* Used to protect access for tpg_nexus */
+	struct mutex tv_tpg_mutex;
+	/* Pointer to the TCM pvscsi I_T Nexus for this TPG endpoint */
+	struct scsiback_nexus *tpg_nexus;
+	/* Pointer back to scsiback_tport */
+	struct scsiback_tport *tport;
+	/* Returned by scsiback_make_tpg() */
+	struct se_portal_group se_tpg;
+	/* alias used in xenstore */
+	char param_alias[VSCSI_NAMELEN];
+	/* list of info structures related to this target portal group */
+	struct list_head info_list;
+};
+
+#define SCSIBACK_INVALID_HANDLE (~0)
+
+static bool log_print_stat;
+module_param(log_print_stat, bool, 0644);
+
+static int scsiback_max_buffer_pages = 1024;
+module_param_named(max_buffer_pages, scsiback_max_buffer_pages, int, 0644);
+MODULE_PARM_DESC(max_buffer_pages,
+"Maximum number of free pages to keep in backend buffer");
+
+static struct kmem_cache *scsiback_cachep;
+static DEFINE_SPINLOCK(free_pages_lock);
+static int free_pages_num;
+static LIST_HEAD(scsiback_free_pages);
+
+/* Global spinlock to protect scsiback TPG list */
+static DEFINE_MUTEX(scsiback_mutex);
+static LIST_HEAD(scsiback_list);
+
+/* Local pointer to allocated TCM configfs fabric module */
+static struct target_fabric_configfs *scsiback_fabric_configfs;
+
+static void scsiback_get(struct vscsibk_info *info)
+{
+	atomic_inc(&info->nr_unreplied_reqs);
+}
+
+static void scsiback_put(struct vscsibk_info *info)
+{
+	if (atomic_dec_and_test(&info->nr_unreplied_reqs))
+		wake_up(&info->waiting_to_free);
+}
+
+static void put_free_pages(struct page **page, int num)
+{
+	unsigned long flags;
+	int i = free_pages_num + num, n = num;
+
+	if (num == 0)
+		return;
+	if (i > scsiback_max_buffer_pages) {
+		n = min(num, i - scsiback_max_buffer_pages);
+		free_xenballooned_pages(n, page + num - n);
+		n = num - n;
+	}
+	spin_lock_irqsave(&free_pages_lock, flags);
+	for (i = 0; i < n; i++)
+		list_add(&page[i]->lru, &scsiback_free_pages);
+	free_pages_num += n;
+	spin_unlock_irqrestore(&free_pages_lock, flags);
+}
+
+static int get_free_page(struct page **page)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&free_pages_lock, flags);
+	if (list_empty(&scsiback_free_pages)) {
+		spin_unlock_irqrestore(&free_pages_lock, flags);
+		return alloc_xenballooned_pages(1, page, false);
+	}
+	page[0] = list_first_entry(&scsiback_free_pages, struct page, lru);
+	list_del(&page[0]->lru);
+	free_pages_num--;
+	spin_unlock_irqrestore(&free_pages_lock, flags);
+	return 0;
+}
+
+static unsigned long vaddr_page(struct page *page)
+{
+	unsigned long pfn = page_to_pfn(page);
+
+	return (unsigned long)pfn_to_kaddr(pfn);
+}
+
+static unsigned long vaddr(struct vscsibk_pend *req, int seg)
+{
+	return vaddr_page(req->pages[seg]);
+}
+
+static void scsiback_print_status(char *sense_buffer, int errors,
+					struct vscsibk_pend *pending_req)
+{
+	struct scsiback_tpg *tpg = pending_req->v2p->tpg;
+
+	pr_err("xen-pvscsi[%s:%d] cmnd[0]=%02x -> st=%02x msg=%02x host=%02x drv=%02x\n",
+	       tpg->tport->tport_name, pending_req->v2p->lun,
+	       pending_req->cmnd[0], status_byte(errors), msg_byte(errors),
+	       host_byte(errors), driver_byte(errors));
+
+	if (CHECK_CONDITION & status_byte(errors))
+		__scsi_print_sense("xen-pvscsi", sense_buffer,
+				   SCSI_SENSE_BUFFERSIZE);
+}
+
+static void scsiback_fast_flush_area(struct vscsibk_pend *req)
+{
+	struct gnttab_unmap_grant_ref unmap[VSCSI_GRANT_BATCH];
+	struct page *pages[VSCSI_GRANT_BATCH];
+	unsigned int i, invcount = 0;
+	grant_handle_t handle;
+	int err;
+
+	kfree(req->sgl);
+	req->sgl = NULL;
+	req->n_sg = 0;
+
+	if (!req->n_grants)
+		return;
+
+	for (i = 0; i < req->n_grants; i++) {
+		handle = req->grant_handles[i];
+		if (handle == SCSIBACK_INVALID_HANDLE)
+			continue;
+		gnttab_set_unmap_op(&unmap[invcount], vaddr(req, i),
+				    GNTMAP_host_map, handle);
+		req->grant_handles[i] = SCSIBACK_INVALID_HANDLE;
+		pages[invcount] = req->pages[i];
+		put_page(pages[invcount]);
+		invcount++;
+		if (invcount < VSCSI_GRANT_BATCH)
+			continue;
+		err = gnttab_unmap_refs(unmap, NULL, pages, invcount);
+		BUG_ON(err);
+		invcount = 0;
+	}
+
+	if (invcount) {
+		err = gnttab_unmap_refs(unmap, NULL, pages, invcount);
+		BUG_ON(err);
+	}
+
+	put_free_pages(req->pages, req->n_grants);
+	req->n_grants = 0;
+}
+
+static void scsiback_free_translation_entry(struct kref *kref)
+{
+	struct v2p_entry *entry = container_of(kref, struct v2p_entry, kref);
+	struct scsiback_tpg *tpg = entry->tpg;
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	tpg->tv_tpg_fe_count--;
+	mutex_unlock(&tpg->tv_tpg_mutex);
+
+	kfree(entry);
+}
+
+static void scsiback_do_resp_with_sense(char *sense_buffer, int32_t result,
+			uint32_t resid, struct vscsibk_pend *pending_req)
+{
+	struct vscsiif_response *ring_res;
+	struct vscsibk_info *info = pending_req->info;
+	int notify;
+	struct scsi_sense_hdr sshdr;
+	unsigned long flags;
+	unsigned len;
+
+	spin_lock_irqsave(&info->ring_lock, flags);
+
+	ring_res = RING_GET_RESPONSE(&info->ring, info->ring.rsp_prod_pvt);
+	info->ring.rsp_prod_pvt++;
+
+	ring_res->rslt   = result;
+	ring_res->rqid   = pending_req->rqid;
+
+	if (sense_buffer != NULL &&
+	    scsi_normalize_sense(sense_buffer, VSCSIIF_SENSE_BUFFERSIZE,
+				 &sshdr)) {
+		len = min_t(unsigned, 8 + sense_buffer[7],
+			    VSCSIIF_SENSE_BUFFERSIZE);
+		memcpy(ring_res->sense_buffer, sense_buffer, len);
+		ring_res->sense_len = len;
+	} else {
+		ring_res->sense_len = 0;
+	}
+
+	ring_res->residual_len = resid;
+
+	RING_PUSH_RESPONSES_AND_CHECK_NOTIFY(&info->ring, notify);
+	spin_unlock_irqrestore(&info->ring_lock, flags);
+
+	if (notify)
+		notify_remote_via_irq(info->irq);
+
+	if (pending_req->v2p)
+		kref_put(&pending_req->v2p->kref,
+			 scsiback_free_translation_entry);
+}
+
+static void scsiback_cmd_done(struct vscsibk_pend *pending_req)
+{
+	struct vscsibk_info *info = pending_req->info;
+	unsigned char *sense_buffer;
+	unsigned int resid;
+	int errors;
+
+	sense_buffer = pending_req->sense_buffer;
+	resid        = pending_req->se_cmd.residual_count;
+	errors       = pending_req->result;
+
+	if (errors && log_print_stat)
+		scsiback_print_status(sense_buffer, errors, pending_req);
+
+	scsiback_fast_flush_area(pending_req);
+	scsiback_do_resp_with_sense(sense_buffer, errors, resid, pending_req);
+	scsiback_put(info);
+}
+
+static void scsiback_cmd_exec(struct vscsibk_pend *pending_req)
+{
+	struct se_cmd *se_cmd = &pending_req->se_cmd;
+	struct se_session *sess = pending_req->v2p->tpg->tpg_nexus->tvn_se_sess;
+	int rc;
+
+	memset(pending_req->sense_buffer, 0, VSCSIIF_SENSE_BUFFERSIZE);
+
+	memset(se_cmd, 0, sizeof(*se_cmd));
+
+	scsiback_get(pending_req->info);
+	rc = target_submit_cmd_map_sgls(se_cmd, sess, pending_req->cmnd,
+			pending_req->sense_buffer, pending_req->v2p->lun,
+			pending_req->data_len, 0,
+			pending_req->sc_data_direction, 0,
+			pending_req->sgl, pending_req->n_sg,
+			NULL, 0, NULL, 0);
+	if (rc < 0) {
+		transport_send_check_condition_and_sense(se_cmd,
+				TCM_LOGICAL_UNIT_COMMUNICATION_FAILURE, 0);
+		transport_generic_free_cmd(se_cmd, 0);
+	}
+}
+
+static int scsiback_gnttab_data_map_batch(struct gnttab_map_grant_ref *map,
+	struct page **pg, grant_handle_t *grant, int cnt)
+{
+	int err, i;
+
+	if (!cnt)
+		return 0;
+
+	err = gnttab_map_refs(map, NULL, pg, cnt);
+	BUG_ON(err);
+	for (i = 0; i < cnt; i++) {
+		if (unlikely(map[i].status != GNTST_okay)) {
+			pr_err("xen-pvscsi: invalid buffer -- could not remap it\n");
+			map[i].handle = SCSIBACK_INVALID_HANDLE;
+			err = -ENOMEM;
+		} else {
+			get_page(pg[i]);
+		}
+		grant[i] = map[i].handle;
+	}
+	return err;
+}
+
+static int scsiback_gnttab_data_map_list(struct vscsibk_pend *pending_req,
+			struct scsiif_request_segment *seg, struct page **pg,
+			grant_handle_t *grant, int cnt, u32 flags)
+{
+	int mapcount = 0, i, err = 0;
+	struct gnttab_map_grant_ref map[VSCSI_GRANT_BATCH];
+	struct vscsibk_info *info = pending_req->info;
+
+	for (i = 0; i < cnt; i++) {
+		if (get_free_page(pg + mapcount)) {
+			put_free_pages(pg, mapcount);
+			pr_err("xen-pvscsi: no grant page\n");
+			return -ENOMEM;
+		}
+		gnttab_set_map_op(&map[mapcount], vaddr_page(pg[mapcount]),
+				  flags, seg[i].gref, info->domid);
+		mapcount++;
+		if (mapcount < VSCSI_GRANT_BATCH)
+			continue;
+		err = scsiback_gnttab_data_map_batch(map, pg, grant, mapcount);
+		pg += mapcount;
+		grant += mapcount;
+		pending_req->n_grants += mapcount;
+		if (err)
+			return err;
+		mapcount = 0;
+	}
+	err = scsiback_gnttab_data_map_batch(map, pg, grant, mapcount);
+	pending_req->n_grants += mapcount;
+	return err;
+}
+
+static int scsiback_gnttab_data_map(struct vscsiif_request *ring_req,
+					struct vscsibk_pend *pending_req)
+{
+	u32 flags;
+	int i, err, n_segs, i_seg = 0;
+	struct page **pg;
+	struct scsiif_request_segment *seg;
+	unsigned long end_seg = 0;
+	unsigned int nr_segments = (unsigned int)ring_req->nr_segments;
+	unsigned int nr_sgl = 0;
+	struct scatterlist *sg;
+	grant_handle_t *grant;
+
+	pending_req->n_sg = 0;
+	pending_req->n_grants = 0;
+	pending_req->data_len = 0;
+
+	nr_segments &= ~VSCSIIF_SG_GRANT;
+	if (!nr_segments)
+		return 0;
+
+	if (nr_segments > VSCSIIF_SG_TABLESIZE) {
+		DPRINTK("xen-pvscsi: invalid parameter nr_seg = %d\n",
+			ring_req->nr_segments);
+		return -EINVAL;
+	}
+
+	if (ring_req->nr_segments & VSCSIIF_SG_GRANT) {
+		err = scsiback_gnttab_data_map_list(pending_req, ring_req->seg,
+			pending_req->pages, pending_req->grant_handles,
+			nr_segments, GNTMAP_host_map | GNTMAP_readonly);
+		if (err)
+			return err;
+		nr_sgl = nr_segments;
+		nr_segments = 0;
+		for (i = 0; i < nr_sgl; i++) {
+			n_segs = ring_req->seg[i].length /
+				 sizeof(struct scsiif_request_segment);
+			if ((unsigned)ring_req->seg[i].offset +
+			    (unsigned)ring_req->seg[i].length > PAGE_SIZE ||
+			    n_segs * sizeof(struct scsiif_request_segment) !=
+			    ring_req->seg[i].length)
+				return -EINVAL;
+			nr_segments += n_segs;
+		}
+		if (nr_segments > SG_ALL) {
+			DPRINTK("xen-pvscsi: invalid nr_seg = %d\n",
+				nr_segments);
+			return -EINVAL;
+		}
+	}
+
+	/* free of (sgl) in fast_flush_area()*/
+	pending_req->sgl = kmalloc_array(nr_segments,
+					sizeof(struct scatterlist), GFP_KERNEL);
+	if (!pending_req->sgl)
+		return -ENOMEM;
+
+	sg_init_table(pending_req->sgl, nr_segments);
+	pending_req->n_sg = nr_segments;
+
+	flags = GNTMAP_host_map;
+	if (pending_req->sc_data_direction == DMA_TO_DEVICE)
+		flags |= GNTMAP_readonly;
+
+	pg = pending_req->pages + nr_sgl;
+	grant = pending_req->grant_handles + nr_sgl;
+	if (!nr_sgl) {
+		seg = ring_req->seg;
+		err = scsiback_gnttab_data_map_list(pending_req, seg,
+			pg, grant, nr_segments, flags);
+		if (err)
+			return err;
+	} else {
+		for (i = 0; i < nr_sgl; i++) {
+			seg = (struct scsiif_request_segment *)(
+			      vaddr(pending_req, i) + ring_req->seg[i].offset);
+			n_segs = ring_req->seg[i].length /
+				 sizeof(struct scsiif_request_segment);
+			err = scsiback_gnttab_data_map_list(pending_req, seg,
+				pg, grant, n_segs, flags);
+			if (err)
+				return err;
+			pg += n_segs;
+			grant += n_segs;
+		}
+		end_seg = vaddr(pending_req, 0) + ring_req->seg[0].offset;
+		seg = (struct scsiif_request_segment *)end_seg;
+		end_seg += ring_req->seg[0].length;
+		pg = pending_req->pages + nr_sgl;
+	}
+
+	for_each_sg(pending_req->sgl, sg, nr_segments, i) {
+		sg_set_page(sg, pg[i], seg->length, seg->offset);
+		pending_req->data_len += seg->length;
+		seg++;
+		if (nr_sgl && (unsigned long)seg >= end_seg) {
+			i_seg++;
+			end_seg = vaddr(pending_req, i_seg) +
+				  ring_req->seg[i_seg].offset;
+			seg = (struct scsiif_request_segment *)end_seg;
+			end_seg += ring_req->seg[i_seg].length;
+		}
+		if (sg->offset >= PAGE_SIZE ||
+		    sg->length > PAGE_SIZE ||
+		    sg->offset + sg->length > PAGE_SIZE)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+static void scsiback_disconnect(struct vscsibk_info *info)
+{
+	wait_event(info->waiting_to_free,
+		atomic_read(&info->nr_unreplied_reqs) == 0);
+
+	unbind_from_irqhandler(info->irq, info);
+	info->irq = 0;
+	xenbus_unmap_ring_vfree(info->dev, info->ring.sring);
+}
+
+static void scsiback_device_action(struct vscsibk_pend *pending_req,
+	enum tcm_tmreq_table act, int tag)
+{
+	int rc, err = FAILED;
+	struct scsiback_tpg *tpg = pending_req->v2p->tpg;
+	struct se_cmd *se_cmd = &pending_req->se_cmd;
+	struct scsiback_tmr *tmr;
+
+	tmr = kzalloc(sizeof(struct scsiback_tmr), GFP_KERNEL);
+	if (!tmr)
+		goto out;
+
+	init_waitqueue_head(&tmr->tmr_wait);
+
+	transport_init_se_cmd(se_cmd, tpg->se_tpg.se_tpg_tfo,
+		tpg->tpg_nexus->tvn_se_sess, 0, DMA_NONE, MSG_SIMPLE_TAG,
+		&pending_req->sense_buffer[0]);
+
+	rc = core_tmr_alloc_req(se_cmd, tmr, act, GFP_KERNEL);
+	if (rc < 0)
+		goto out;
+
+	se_cmd->se_tmr_req->ref_task_tag = tag;
+
+	if (transport_lookup_tmr_lun(se_cmd, pending_req->v2p->lun) < 0)
+		goto out;
+
+	transport_generic_handle_tmr(se_cmd);
+	wait_event(tmr->tmr_wait, atomic_read(&tmr->tmr_complete));
+
+	err = (se_cmd->se_tmr_req->response == TMR_FUNCTION_COMPLETE) ?
+		SUCCESS : FAILED;
+
+out:
+	if (tmr) {
+		transport_generic_free_cmd(&pending_req->se_cmd, 1);
+		kfree(tmr);
+	}
+
+	scsiback_do_resp_with_sense(NULL, err, 0, pending_req);
+
+	kmem_cache_free(scsiback_cachep, pending_req);
+}
+
+/*
+  Perform virtual to physical translation
+*/
+static struct v2p_entry *scsiback_do_translation(struct vscsibk_info *info,
+			struct ids_tuple *v)
+{
+	struct v2p_entry *entry;
+	struct list_head *head = &(info->v2p_entry_lists);
+	unsigned long flags;
+
+	spin_lock_irqsave(&info->v2p_lock, flags);
+	list_for_each_entry(entry, head, l) {
+		if ((entry->v.chn == v->chn) &&
+		    (entry->v.tgt == v->tgt) &&
+		    (entry->v.lun == v->lun)) {
+			kref_get(&entry->kref);
+			goto out;
+		}
+	}
+	entry = NULL;
+
+out:
+	spin_unlock_irqrestore(&info->v2p_lock, flags);
+	return entry;
+}
+
+static int prepare_pending_reqs(struct vscsibk_info *info,
+				struct vscsiif_request *ring_req,
+				struct vscsibk_pend *pending_req)
+{
+	struct v2p_entry *v2p;
+	struct ids_tuple vir;
+
+	pending_req->rqid       = ring_req->rqid;
+	pending_req->info       = info;
+
+	vir.chn = ring_req->channel;
+	vir.tgt = ring_req->id;
+	vir.lun = ring_req->lun;
+
+	v2p = scsiback_do_translation(info, &vir);
+	if (!v2p) {
+		pending_req->v2p = NULL;
+		DPRINTK("xen-pvscsi: doesn't exist.\n");
+		return -ENODEV;
+	}
+	pending_req->v2p = v2p;
+
+	/* request range check from frontend */
+	pending_req->sc_data_direction = ring_req->sc_data_direction;
+	if ((pending_req->sc_data_direction != DMA_BIDIRECTIONAL) &&
+		(pending_req->sc_data_direction != DMA_TO_DEVICE) &&
+		(pending_req->sc_data_direction != DMA_FROM_DEVICE) &&
+		(pending_req->sc_data_direction != DMA_NONE)) {
+		DPRINTK("xen-pvscsi: invalid parameter data_dir = %d\n",
+			pending_req->sc_data_direction);
+		return -EINVAL;
+	}
+
+	pending_req->cmd_len = ring_req->cmd_len;
+	if (pending_req->cmd_len > VSCSIIF_MAX_COMMAND_SIZE) {
+		DPRINTK("xen-pvscsi: invalid parameter cmd_len = %d\n",
+			pending_req->cmd_len);
+		return -EINVAL;
+	}
+	memcpy(pending_req->cmnd, ring_req->cmnd, pending_req->cmd_len);
+
+	return 0;
+}
+
+static int scsiback_do_cmd_fn(struct vscsibk_info *info)
+{
+	struct vscsiif_back_ring *ring = &info->ring;
+	struct vscsiif_request *ring_req;
+	struct vscsibk_pend *pending_req;
+	RING_IDX rc, rp;
+	int err, more_to_do;
+	uint32_t result;
+	uint8_t act;
+
+	rc = ring->req_cons;
+	rp = ring->sring->req_prod;
+	rmb();	/* guest system is accessing ring, too */
+
+	if (RING_REQUEST_PROD_OVERFLOW(ring, rp)) {
+		rc = ring->rsp_prod_pvt;
+		pr_warn("xen-pvscsi: Dom%d provided bogus ring requests (%#x - %#x = %u). Halting ring processing\n",
+			   info->domid, rp, rc, rp - rc);
+		info->ring_error = 1;
+		return 0;
+	}
+
+	while ((rc != rp)) {
+		if (RING_REQUEST_CONS_OVERFLOW(ring, rc))
+			break;
+		pending_req = kmem_cache_alloc(scsiback_cachep, GFP_KERNEL);
+		if (!pending_req)
+			return 1;
+
+		ring_req = RING_GET_REQUEST(ring, rc);
+		ring->req_cons = ++rc;
+
+		act = ring_req->act;
+		err = prepare_pending_reqs(info, ring_req, pending_req);
+		if (err) {
+			switch (err) {
+			case -ENODEV:
+				result = DID_NO_CONNECT;
+				break;
+			default:
+				result = DRIVER_ERROR;
+				break;
+			}
+			scsiback_do_resp_with_sense(NULL, result << 24, 0,
+						    pending_req);
+			kmem_cache_free(scsiback_cachep, pending_req);
+			return 1;
+		}
+
+		switch (act) {
+		case VSCSIIF_ACT_SCSI_CDB:
+			if (scsiback_gnttab_data_map(ring_req, pending_req)) {
+				scsiback_fast_flush_area(pending_req);
+				scsiback_do_resp_with_sense(NULL,
+					DRIVER_ERROR << 24, 0, pending_req);
+				kmem_cache_free(scsiback_cachep, pending_req);
+			} else {
+				scsiback_cmd_exec(pending_req);
+			}
+			break;
+		case VSCSIIF_ACT_SCSI_ABORT:
+			scsiback_device_action(pending_req, TMR_ABORT_TASK,
+				ring_req->ref_rqid);
+			break;
+		case VSCSIIF_ACT_SCSI_RESET:
+			scsiback_device_action(pending_req, TMR_LUN_RESET, 0);
+			break;
+		default:
+			pr_err_ratelimited("xen-pvscsi: invalid request\n");
+			scsiback_do_resp_with_sense(NULL, DRIVER_ERROR << 24,
+						    0, pending_req);
+			kmem_cache_free(scsiback_cachep, pending_req);
+			break;
+		}
+
+		/* Yield point for this unbounded loop. */
+		cond_resched();
+	}
+
+	RING_FINAL_CHECK_FOR_REQUESTS(&info->ring, more_to_do);
+	return more_to_do;
+}
+
+static irqreturn_t scsiback_irq_fn(int irq, void *dev_id)
+{
+	struct vscsibk_info *info = dev_id;
+
+	if (info->ring_error)
+		return IRQ_HANDLED;
+
+	while (scsiback_do_cmd_fn(info))
+		cond_resched();
+
+	return IRQ_HANDLED;
+}
+
+static int scsiback_init_sring(struct vscsibk_info *info, grant_ref_t ring_ref,
+			evtchn_port_t evtchn)
+{
+	void *area;
+	struct vscsiif_sring *sring;
+	int err;
+
+	if (info->irq)
+		return -1;
+
+	err = xenbus_map_ring_valloc(info->dev, ring_ref, &area);
+	if (err)
+		return err;
+
+	sring = (struct vscsiif_sring *)area;
+	BACK_RING_INIT(&info->ring, sring, PAGE_SIZE);
+
+	err = bind_interdomain_evtchn_to_irq(info->domid, evtchn);
+	if (err < 0)
+		goto unmap_page;
+
+	info->irq = err;
+
+	err = request_threaded_irq(info->irq, NULL, scsiback_irq_fn,
+				   IRQF_ONESHOT, "vscsiif-backend", info);
+	if (err)
+		goto free_irq;
+
+	return 0;
+
+free_irq:
+	unbind_from_irqhandler(info->irq, info);
+	info->irq = 0;
+unmap_page:
+	xenbus_unmap_ring_vfree(info->dev, area);
+
+	return err;
+}
+
+static int scsiback_map(struct vscsibk_info *info)
+{
+	struct xenbus_device *dev = info->dev;
+	unsigned int ring_ref, evtchn;
+	int err;
+
+	err = xenbus_gather(XBT_NIL, dev->otherend,
+			"ring-ref", "%u", &ring_ref,
+			"event-channel", "%u", &evtchn, NULL);
+	if (err) {
+		xenbus_dev_fatal(dev, err, "reading %s ring", dev->otherend);
+		return err;
+	}
+
+	return scsiback_init_sring(info, ring_ref, evtchn);
+}
+
+/*
+  Add a new translation entry
+*/
+static int scsiback_add_translation_entry(struct vscsibk_info *info,
+					  char *phy, struct ids_tuple *v)
+{
+	int err = 0;
+	struct v2p_entry *entry;
+	struct v2p_entry *new;
+	struct list_head *head = &(info->v2p_entry_lists);
+	unsigned long flags;
+	char *lunp;
+	unsigned int lun;
+	struct scsiback_tpg *tpg_entry, *tpg = NULL;
+	char *error = "doesn't exist";
+
+	lunp = strrchr(phy, ':');
+	if (!lunp) {
+		pr_err("xen-pvscsi: illegal format of physical device %s\n",
+			phy);
+		return -EINVAL;
+	}
+	*lunp = 0;
+	lunp++;
+	if (kstrtouint(lunp, 10, &lun) || lun >= TRANSPORT_MAX_LUNS_PER_TPG) {
+		pr_err("xen-pvscsi: lun number not valid: %s\n", lunp);
+		return -EINVAL;
+	}
+
+	mutex_lock(&scsiback_mutex);
+	list_for_each_entry(tpg_entry, &scsiback_list, tv_tpg_list) {
+		if (!strcmp(phy, tpg_entry->tport->tport_name) ||
+		    !strcmp(phy, tpg_entry->param_alias)) {
+			spin_lock(&tpg_entry->se_tpg.tpg_lun_lock);
+			if (tpg_entry->se_tpg.tpg_lun_list[lun]->lun_status ==
+			    TRANSPORT_LUN_STATUS_ACTIVE) {
+				if (!tpg_entry->tpg_nexus)
+					error = "nexus undefined";
+				else
+					tpg = tpg_entry;
+			}
+			spin_unlock(&tpg_entry->se_tpg.tpg_lun_lock);
+			break;
+		}
+	}
+	if (tpg) {
+		mutex_lock(&tpg->tv_tpg_mutex);
+		tpg->tv_tpg_fe_count++;
+		mutex_unlock(&tpg->tv_tpg_mutex);
+	}
+	mutex_unlock(&scsiback_mutex);
+
+	if (!tpg) {
+		pr_err("xen-pvscsi: %s:%d %s\n", phy, lun, error);
+		return -ENODEV;
+	}
+
+	new = kmalloc(sizeof(struct v2p_entry), GFP_KERNEL);
+	if (new == NULL) {
+		err = -ENOMEM;
+		goto out_free;
+	}
+
+	spin_lock_irqsave(&info->v2p_lock, flags);
+
+	/* Check double assignment to identical virtual ID */
+	list_for_each_entry(entry, head, l) {
+		if ((entry->v.chn == v->chn) &&
+		    (entry->v.tgt == v->tgt) &&
+		    (entry->v.lun == v->lun)) {
+			pr_warn("xen-pvscsi: Virtual ID is already used. Assignment was not performed.\n");
+			err = -EEXIST;
+			goto out;
+		}
+
+	}
+
+	/* Create a new translation entry and add to the list */
+	kref_init(&new->kref);
+	new->v = *v;
+	new->tpg = tpg;
+	new->lun = lun;
+	list_add_tail(&new->l, head);
+
+out:
+	spin_unlock_irqrestore(&info->v2p_lock, flags);
+
+out_free:
+	mutex_lock(&tpg->tv_tpg_mutex);
+	tpg->tv_tpg_fe_count--;
+	mutex_unlock(&tpg->tv_tpg_mutex);
+
+	if (err)
+		kfree(new);
+
+	return err;
+}
+
+static void __scsiback_del_translation_entry(struct v2p_entry *entry)
+{
+	list_del(&entry->l);
+	kref_put(&entry->kref, scsiback_free_translation_entry);
+}
+
+/*
+  Delete the translation entry specfied
+*/
+static int scsiback_del_translation_entry(struct vscsibk_info *info,
+					  struct ids_tuple *v)
+{
+	struct v2p_entry *entry;
+	struct list_head *head = &(info->v2p_entry_lists);
+	unsigned long flags;
+
+	spin_lock_irqsave(&info->v2p_lock, flags);
+	/* Find out the translation entry specified */
+	list_for_each_entry(entry, head, l) {
+		if ((entry->v.chn == v->chn) &&
+		    (entry->v.tgt == v->tgt) &&
+		    (entry->v.lun == v->lun)) {
+			goto found;
+		}
+	}
+
+	spin_unlock_irqrestore(&info->v2p_lock, flags);
+	return 1;
+
+found:
+	/* Delete the translation entry specfied */
+	__scsiback_del_translation_entry(entry);
+
+	spin_unlock_irqrestore(&info->v2p_lock, flags);
+	return 0;
+}
+
+static void scsiback_do_add_lun(struct vscsibk_info *info, const char *state,
+				char *phy, struct ids_tuple *vir)
+{
+	if (!scsiback_add_translation_entry(info, phy, vir)) {
+		if (xenbus_printf(XBT_NIL, info->dev->nodename, state,
+				  "%d", XenbusStateInitialised)) {
+			pr_err("xen-pvscsi: xenbus_printf error %s\n", state);
+			scsiback_del_translation_entry(info, vir);
+		}
+	} else {
+		xenbus_printf(XBT_NIL, info->dev->nodename, state,
+			      "%d", XenbusStateClosed);
+	}
+}
+
+static void scsiback_do_del_lun(struct vscsibk_info *info, const char *state,
+				struct ids_tuple *vir)
+{
+	if (!scsiback_del_translation_entry(info, vir)) {
+		if (xenbus_printf(XBT_NIL, info->dev->nodename, state,
+				  "%d", XenbusStateClosed))
+			pr_err("xen-pvscsi: xenbus_printf error %s\n", state);
+	}
+}
+
+#define VSCSIBACK_OP_ADD_OR_DEL_LUN	1
+#define VSCSIBACK_OP_UPDATEDEV_STATE	2
+
+static void scsiback_do_1lun_hotplug(struct vscsibk_info *info, int op,
+				     char *ent)
+{
+	int err;
+	struct ids_tuple vir;
+	char *val;
+	int device_state;
+	char phy[VSCSI_NAMELEN];
+	char str[64];
+	char state[64];
+	struct xenbus_device *dev = info->dev;
+
+	/* read status */
+	snprintf(state, sizeof(state), "vscsi-devs/%s/state", ent);
+	err = xenbus_scanf(XBT_NIL, dev->nodename, state, "%u", &device_state);
+	if (XENBUS_EXIST_ERR(err))
+		return;
+
+	/* physical SCSI device */
+	snprintf(str, sizeof(str), "vscsi-devs/%s/p-dev", ent);
+	val = xenbus_read(XBT_NIL, dev->nodename, str, NULL);
+	if (IS_ERR(val)) {
+		xenbus_printf(XBT_NIL, dev->nodename, state,
+			      "%d", XenbusStateClosed);
+		return;
+	}
+	strlcpy(phy, val, VSCSI_NAMELEN);
+	kfree(val);
+
+	/* virtual SCSI device */
+	snprintf(str, sizeof(str), "vscsi-devs/%s/v-dev", ent);
+	err = xenbus_scanf(XBT_NIL, dev->nodename, str, "%u:%u:%u:%u",
+			   &vir.hst, &vir.chn, &vir.tgt, &vir.lun);
+	if (XENBUS_EXIST_ERR(err)) {
+		xenbus_printf(XBT_NIL, dev->nodename, state,
+			      "%d", XenbusStateClosed);
+		return;
+	}
+
+	switch (op) {
+	case VSCSIBACK_OP_ADD_OR_DEL_LUN:
+		if (device_state == XenbusStateInitialising)
+			scsiback_do_add_lun(info, state, phy, &vir);
+		if (device_state == XenbusStateClosing)
+			scsiback_do_del_lun(info, state, &vir);
+		break;
+
+	case VSCSIBACK_OP_UPDATEDEV_STATE:
+		if (device_state == XenbusStateInitialised) {
+			/* modify vscsi-devs/dev-x/state */
+			if (xenbus_printf(XBT_NIL, dev->nodename, state,
+					  "%d", XenbusStateConnected)) {
+				pr_err("xen-pvscsi: xenbus_printf error %s\n",
+				       str);
+				scsiback_del_translation_entry(info, &vir);
+				xenbus_printf(XBT_NIL, dev->nodename, state,
+					      "%d", XenbusStateClosed);
+			}
+		}
+		break;
+	/*When it is necessary, processing is added here.*/
+	default:
+		break;
+	}
+}
+
+static void scsiback_do_lun_hotplug(struct vscsibk_info *info, int op)
+{
+	int i;
+	char **dir;
+	unsigned int ndir = 0;
+
+	dir = xenbus_directory(XBT_NIL, info->dev->nodename, "vscsi-devs",
+			       &ndir);
+	if (IS_ERR(dir))
+		return;
+
+	for (i = 0; i < ndir; i++)
+		scsiback_do_1lun_hotplug(info, op, dir[i]);
+
+	kfree(dir);
+}
+
+static void scsiback_frontend_changed(struct xenbus_device *dev,
+					enum xenbus_state frontend_state)
+{
+	struct vscsibk_info *info = dev_get_drvdata(&dev->dev);
+
+	switch (frontend_state) {
+	case XenbusStateInitialising:
+		break;
+
+	case XenbusStateInitialised:
+		if (scsiback_map(info))
+			break;
+
+		scsiback_do_lun_hotplug(info, VSCSIBACK_OP_ADD_OR_DEL_LUN);
+		xenbus_switch_state(dev, XenbusStateConnected);
+		break;
+
+	case XenbusStateConnected:
+		scsiback_do_lun_hotplug(info, VSCSIBACK_OP_UPDATEDEV_STATE);
+
+		if (dev->state == XenbusStateConnected)
+			break;
+
+		xenbus_switch_state(dev, XenbusStateConnected);
+		break;
+
+	case XenbusStateClosing:
+		if (info->irq)
+			scsiback_disconnect(info);
+
+		xenbus_switch_state(dev, XenbusStateClosing);
+		break;
+
+	case XenbusStateClosed:
+		xenbus_switch_state(dev, XenbusStateClosed);
+		if (xenbus_dev_is_online(dev))
+			break;
+		/* fall through if not online */
+	case XenbusStateUnknown:
+		device_unregister(&dev->dev);
+		break;
+
+	case XenbusStateReconfiguring:
+		scsiback_do_lun_hotplug(info, VSCSIBACK_OP_ADD_OR_DEL_LUN);
+		xenbus_switch_state(dev, XenbusStateReconfigured);
+
+		break;
+
+	default:
+		xenbus_dev_fatal(dev, -EINVAL, "saw state %d at frontend",
+					frontend_state);
+		break;
+	}
+}
+
+/*
+  Release the translation entry specfied
+*/
+static void scsiback_release_translation_entry(struct vscsibk_info *info)
+{
+	struct v2p_entry *entry, *tmp;
+	struct list_head *head = &(info->v2p_entry_lists);
+	unsigned long flags;
+
+	spin_lock_irqsave(&info->v2p_lock, flags);
+
+	list_for_each_entry_safe(entry, tmp, head, l)
+		__scsiback_del_translation_entry(entry);
+
+	spin_unlock_irqrestore(&info->v2p_lock, flags);
+}
+
+static int scsiback_remove(struct xenbus_device *dev)
+{
+	struct vscsibk_info *info = dev_get_drvdata(&dev->dev);
+
+	if (info->irq)
+		scsiback_disconnect(info);
+
+	scsiback_release_translation_entry(info);
+
+	dev_set_drvdata(&dev->dev, NULL);
+
+	return 0;
+}
+
+static int scsiback_probe(struct xenbus_device *dev,
+			   const struct xenbus_device_id *id)
+{
+	int err;
+
+	struct vscsibk_info *info = kzalloc(sizeof(struct vscsibk_info),
+					    GFP_KERNEL);
+
+	DPRINTK("%p %d\n", dev, dev->otherend_id);
+
+	if (!info) {
+		xenbus_dev_fatal(dev, -ENOMEM, "allocating backend structure");
+		return -ENOMEM;
+	}
+	info->dev = dev;
+	dev_set_drvdata(&dev->dev, info);
+
+	info->domid = dev->otherend_id;
+	spin_lock_init(&info->ring_lock);
+	info->ring_error = 0;
+	atomic_set(&info->nr_unreplied_reqs, 0);
+	init_waitqueue_head(&info->waiting_to_free);
+	info->dev = dev;
+	info->irq = 0;
+	INIT_LIST_HEAD(&info->v2p_entry_lists);
+	spin_lock_init(&info->v2p_lock);
+
+	err = xenbus_printf(XBT_NIL, dev->nodename, "feature-sg-grant", "%u",
+			    SG_ALL);
+	if (err)
+		xenbus_dev_error(dev, err, "writing feature-sg-grant");
+
+	err = xenbus_switch_state(dev, XenbusStateInitWait);
+	if (err)
+		goto fail;
+
+	return 0;
+
+fail:
+	pr_warn("xen-pvscsi: %s failed\n", __func__);
+	scsiback_remove(dev);
+
+	return err;
+}
+
+static char *scsiback_dump_proto_id(struct scsiback_tport *tport)
+{
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+		return "SAS";
+	case SCSI_PROTOCOL_FCP:
+		return "FCP";
+	case SCSI_PROTOCOL_ISCSI:
+		return "iSCSI";
+	default:
+		break;
+	}
+
+	return "Unknown";
+}
+
+static u8 scsiback_get_fabric_proto_ident(struct se_portal_group *se_tpg)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	struct scsiback_tport *tport = tpg->tport;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+		return sas_get_fabric_proto_ident(se_tpg);
+	case SCSI_PROTOCOL_FCP:
+		return fc_get_fabric_proto_ident(se_tpg);
+	case SCSI_PROTOCOL_ISCSI:
+		return iscsi_get_fabric_proto_ident(se_tpg);
+	default:
+		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
+			tport->tport_proto_id);
+		break;
+	}
+
+	return sas_get_fabric_proto_ident(se_tpg);
+}
+
+static char *scsiback_get_fabric_wwn(struct se_portal_group *se_tpg)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	struct scsiback_tport *tport = tpg->tport;
+
+	return &tport->tport_name[0];
+}
+
+static u16 scsiback_get_tag(struct se_portal_group *se_tpg)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	return tpg->tport_tpgt;
+}
+
+static u32 scsiback_get_default_depth(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static u32
+scsiback_get_pr_transport_id(struct se_portal_group *se_tpg,
+			      struct se_node_acl *se_nacl,
+			      struct t10_pr_registration *pr_reg,
+			      int *format_code,
+			      unsigned char *buf)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	struct scsiback_tport *tport = tpg->tport;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+		return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
+					format_code, buf);
+	case SCSI_PROTOCOL_FCP:
+		return fc_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
+					format_code, buf);
+	case SCSI_PROTOCOL_ISCSI:
+		return iscsi_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
+					format_code, buf);
+	default:
+		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
+			tport->tport_proto_id);
+		break;
+	}
+
+	return sas_get_pr_transport_id(se_tpg, se_nacl, pr_reg,
+			format_code, buf);
+}
+
+static u32
+scsiback_get_pr_transport_id_len(struct se_portal_group *se_tpg,
+				  struct se_node_acl *se_nacl,
+				  struct t10_pr_registration *pr_reg,
+				  int *format_code)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	struct scsiback_tport *tport = tpg->tport;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+		return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
+					format_code);
+	case SCSI_PROTOCOL_FCP:
+		return fc_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
+					format_code);
+	case SCSI_PROTOCOL_ISCSI:
+		return iscsi_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
+					format_code);
+	default:
+		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
+			tport->tport_proto_id);
+		break;
+	}
+
+	return sas_get_pr_transport_id_len(se_tpg, se_nacl, pr_reg,
+			format_code);
+}
+
+static char *
+scsiback_parse_pr_out_transport_id(struct se_portal_group *se_tpg,
+				    const char *buf,
+				    u32 *out_tid_len,
+				    char **port_nexus_ptr)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	struct scsiback_tport *tport = tpg->tport;
+
+	switch (tport->tport_proto_id) {
+	case SCSI_PROTOCOL_SAS:
+		return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
+					port_nexus_ptr);
+	case SCSI_PROTOCOL_FCP:
+		return fc_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
+					port_nexus_ptr);
+	case SCSI_PROTOCOL_ISCSI:
+		return iscsi_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
+					port_nexus_ptr);
+	default:
+		pr_err("Unknown tport_proto_id: 0x%02x, using SAS emulation\n",
+			tport->tport_proto_id);
+		break;
+	}
+
+	return sas_parse_pr_out_transport_id(se_tpg, buf, out_tid_len,
+			port_nexus_ptr);
+}
+
+static struct se_wwn *
+scsiback_make_tport(struct target_fabric_configfs *tf,
+		     struct config_group *group,
+		     const char *name)
+{
+	struct scsiback_tport *tport;
+	char *ptr;
+	u64 wwpn = 0;
+	int off = 0;
+
+	tport = kzalloc(sizeof(struct scsiback_tport), GFP_KERNEL);
+	if (!tport)
+		return ERR_PTR(-ENOMEM);
+
+	tport->tport_wwpn = wwpn;
+	/*
+	 * Determine the emulated Protocol Identifier and Target Port Name
+	 * based on the incoming configfs directory name.
+	 */
+	ptr = strstr(name, "naa.");
+	if (ptr) {
+		tport->tport_proto_id = SCSI_PROTOCOL_SAS;
+		goto check_len;
+	}
+	ptr = strstr(name, "fc.");
+	if (ptr) {
+		tport->tport_proto_id = SCSI_PROTOCOL_FCP;
+		off = 3; /* Skip over "fc." */
+		goto check_len;
+	}
+	ptr = strstr(name, "iqn.");
+	if (ptr) {
+		tport->tport_proto_id = SCSI_PROTOCOL_ISCSI;
+		goto check_len;
+	}
+
+	pr_err("Unable to locate prefix for emulated Target Port: %s\n", name);
+	kfree(tport);
+	return ERR_PTR(-EINVAL);
+
+check_len:
+	if (strlen(name) >= VSCSI_NAMELEN) {
+		pr_err("Emulated %s Address: %s, exceeds max: %d\n", name,
+			scsiback_dump_proto_id(tport), VSCSI_NAMELEN);
+		kfree(tport);
+		return ERR_PTR(-EINVAL);
+	}
+	snprintf(&tport->tport_name[0], VSCSI_NAMELEN, "%s", &name[off]);
+
+	pr_debug("xen-pvscsi: Allocated emulated Target %s Address: %s\n",
+		 scsiback_dump_proto_id(tport), name);
+
+	return &tport->tport_wwn;
+}
+
+static void scsiback_drop_tport(struct se_wwn *wwn)
+{
+	struct scsiback_tport *tport = container_of(wwn,
+				struct scsiback_tport, tport_wwn);
+
+	pr_debug("xen-pvscsi: Deallocating emulated Target %s Address: %s\n",
+		 scsiback_dump_proto_id(tport), tport->tport_name);
+
+	kfree(tport);
+}
+
+static struct se_node_acl *
+scsiback_alloc_fabric_acl(struct se_portal_group *se_tpg)
+{
+	return kzalloc(sizeof(struct se_node_acl), GFP_KERNEL);
+}
+
+static void
+scsiback_release_fabric_acl(struct se_portal_group *se_tpg,
+			     struct se_node_acl *se_nacl)
+{
+	kfree(se_nacl);
+}
+
+static u32 scsiback_tpg_get_inst_index(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static int scsiback_check_stop_free(struct se_cmd *se_cmd)
+{
+	/*
+	 * Do not release struct se_cmd's containing a valid TMR
+	 * pointer.  These will be released directly in scsiback_device_action()
+	 * with transport_generic_free_cmd().
+	 */
+	if (se_cmd->se_cmd_flags & SCF_SCSI_TMR_CDB)
+		return 0;
+
+	transport_generic_free_cmd(se_cmd, 0);
+	return 1;
+}
+
+static void scsiback_release_cmd(struct se_cmd *se_cmd)
+{
+	struct vscsibk_pend *pending_req = container_of(se_cmd,
+				struct vscsibk_pend, se_cmd);
+
+	kmem_cache_free(scsiback_cachep, pending_req);
+}
+
+static int scsiback_shutdown_session(struct se_session *se_sess)
+{
+	return 0;
+}
+
+static void scsiback_close_session(struct se_session *se_sess)
+{
+}
+
+static u32 scsiback_sess_get_index(struct se_session *se_sess)
+{
+	return 0;
+}
+
+static int scsiback_write_pending(struct se_cmd *se_cmd)
+{
+	/* Go ahead and process the write immediately */
+	target_execute_cmd(se_cmd);
+
+	return 0;
+}
+
+static int scsiback_write_pending_status(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static void scsiback_set_default_node_attrs(struct se_node_acl *nacl)
+{
+}
+
+static u32 scsiback_get_task_tag(struct se_cmd *se_cmd)
+{
+	struct vscsibk_pend *pending_req = container_of(se_cmd,
+				struct vscsibk_pend, se_cmd);
+
+	return pending_req->rqid;
+}
+
+static int scsiback_get_cmd_state(struct se_cmd *se_cmd)
+{
+	return 0;
+}
+
+static int scsiback_queue_data_in(struct se_cmd *se_cmd)
+{
+	struct vscsibk_pend *pending_req = container_of(se_cmd,
+				struct vscsibk_pend, se_cmd);
+
+	pending_req->result = SAM_STAT_GOOD;
+	scsiback_cmd_done(pending_req);
+	return 0;
+}
+
+static int scsiback_queue_status(struct se_cmd *se_cmd)
+{
+	struct vscsibk_pend *pending_req = container_of(se_cmd,
+				struct vscsibk_pend, se_cmd);
+
+	if (se_cmd->sense_buffer &&
+	    ((se_cmd->se_cmd_flags & SCF_TRANSPORT_TASK_SENSE) ||
+	     (se_cmd->se_cmd_flags & SCF_EMULATED_TASK_SENSE)))
+		pending_req->result = (DRIVER_SENSE << 24) |
+				      SAM_STAT_CHECK_CONDITION;
+	else
+		pending_req->result = se_cmd->scsi_status;
+
+	scsiback_cmd_done(pending_req);
+	return 0;
+}
+
+static void scsiback_queue_tm_rsp(struct se_cmd *se_cmd)
+{
+	struct se_tmr_req *se_tmr = se_cmd->se_tmr_req;
+	struct scsiback_tmr *tmr = se_tmr->fabric_tmr_ptr;
+
+	atomic_set(&tmr->tmr_complete, 1);
+	wake_up(&tmr->tmr_wait);
+}
+
+static void scsiback_aborted_task(struct se_cmd *se_cmd)
+{
+}
+
+static ssize_t scsiback_tpg_param_show_alias(struct se_portal_group *se_tpg,
+					     char *page)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg, struct scsiback_tpg,
+						se_tpg);
+	ssize_t rb;
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	rb = snprintf(page, PAGE_SIZE, "%s\n", tpg->param_alias);
+	mutex_unlock(&tpg->tv_tpg_mutex);
+
+	return rb;
+}
+
+static ssize_t scsiback_tpg_param_store_alias(struct se_portal_group *se_tpg,
+					      const char *page, size_t count)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg, struct scsiback_tpg,
+						se_tpg);
+	int len;
+
+	if (strlen(page) >= VSCSI_NAMELEN) {
+		pr_err("param alias: %s, exceeds max: %d\n", page,
+			VSCSI_NAMELEN);
+		return -EINVAL;
+	}
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	len = snprintf(tpg->param_alias, VSCSI_NAMELEN, "%s", page);
+	if (tpg->param_alias[len - 1] == '\n')
+		tpg->param_alias[len - 1] = '\0';
+	mutex_unlock(&tpg->tv_tpg_mutex);
+
+	return count;
+}
+
+TF_TPG_PARAM_ATTR(scsiback, alias, S_IRUGO | S_IWUSR);
+
+static struct configfs_attribute *scsiback_param_attrs[] = {
+	&scsiback_tpg_param_alias.attr,
+	NULL,
+};
+
+static int scsiback_make_nexus(struct scsiback_tpg *tpg,
+				const char *name)
+{
+	struct se_portal_group *se_tpg;
+	struct se_session *se_sess;
+	struct scsiback_nexus *tv_nexus;
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	if (tpg->tpg_nexus) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		pr_debug("tpg->tpg_nexus already exists\n");
+		return -EEXIST;
+	}
+	se_tpg = &tpg->se_tpg;
+
+	tv_nexus = kzalloc(sizeof(struct scsiback_nexus), GFP_KERNEL);
+	if (!tv_nexus) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		return -ENOMEM;
+	}
+	/*
+	 *  Initialize the struct se_session pointer
+	 */
+	tv_nexus->tvn_se_sess = transport_init_session(TARGET_PROT_NORMAL);
+	if (IS_ERR(tv_nexus->tvn_se_sess)) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		kfree(tv_nexus);
+		return -ENOMEM;
+	}
+	se_sess = tv_nexus->tvn_se_sess;
+	/*
+	 * Since we are running in 'demo mode' this call with generate a
+	 * struct se_node_acl for the scsiback struct se_portal_group with
+	 * the SCSI Initiator port name of the passed configfs group 'name'.
+	 */
+	tv_nexus->tvn_se_sess->se_node_acl = core_tpg_check_initiator_node_acl(
+				se_tpg, (unsigned char *)name);
+	if (!tv_nexus->tvn_se_sess->se_node_acl) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		pr_debug("core_tpg_check_initiator_node_acl() failed for %s\n",
+			 name);
+		goto out;
+	}
+	/*
+	 * Now register the TCM pvscsi virtual I_T Nexus as active with the
+	 * call to __transport_register_session()
+	 */
+	__transport_register_session(se_tpg, tv_nexus->tvn_se_sess->se_node_acl,
+			tv_nexus->tvn_se_sess, tv_nexus);
+	tpg->tpg_nexus = tv_nexus;
+
+	mutex_unlock(&tpg->tv_tpg_mutex);
+	return 0;
+
+out:
+	transport_free_session(se_sess);
+	kfree(tv_nexus);
+	return -ENOMEM;
+}
+
+static int scsiback_drop_nexus(struct scsiback_tpg *tpg)
+{
+	struct se_session *se_sess;
+	struct scsiback_nexus *tv_nexus;
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		return -ENODEV;
+	}
+
+	se_sess = tv_nexus->tvn_se_sess;
+	if (!se_sess) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		return -ENODEV;
+	}
+
+	if (tpg->tv_tpg_port_count != 0) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		pr_err("Unable to remove xen-pvscsi I_T Nexus with active TPG port count: %d\n",
+			tpg->tv_tpg_port_count);
+		return -EBUSY;
+	}
+
+	if (tpg->tv_tpg_fe_count != 0) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		pr_err("Unable to remove xen-pvscsi I_T Nexus with active TPG frontend count: %d\n",
+			tpg->tv_tpg_fe_count);
+		return -EBUSY;
+	}
+
+	pr_debug("xen-pvscsi: Removing I_T Nexus to emulated %s Initiator Port: %s\n",
+		scsiback_dump_proto_id(tpg->tport),
+		tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
+
+	/*
+	 * Release the SCSI I_T Nexus to the emulated xen-pvscsi Target Port
+	 */
+	transport_deregister_session(tv_nexus->tvn_se_sess);
+	tpg->tpg_nexus = NULL;
+	mutex_unlock(&tpg->tv_tpg_mutex);
+
+	kfree(tv_nexus);
+	return 0;
+}
+
+static ssize_t scsiback_tpg_show_nexus(struct se_portal_group *se_tpg,
+					char *page)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	struct scsiback_nexus *tv_nexus;
+	ssize_t ret;
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	tv_nexus = tpg->tpg_nexus;
+	if (!tv_nexus) {
+		mutex_unlock(&tpg->tv_tpg_mutex);
+		return -ENODEV;
+	}
+	ret = snprintf(page, PAGE_SIZE, "%s\n",
+			tv_nexus->tvn_se_sess->se_node_acl->initiatorname);
+	mutex_unlock(&tpg->tv_tpg_mutex);
+
+	return ret;
+}
+
+static ssize_t scsiback_tpg_store_nexus(struct se_portal_group *se_tpg,
+					 const char *page,
+					 size_t count)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+	struct scsiback_tport *tport_wwn = tpg->tport;
+	unsigned char i_port[VSCSI_NAMELEN], *ptr, *port_ptr;
+	int ret;
+	/*
+	 * Shutdown the active I_T nexus if 'NULL' is passed..
+	 */
+	if (!strncmp(page, "NULL", 4)) {
+		ret = scsiback_drop_nexus(tpg);
+		return (!ret) ? count : ret;
+	}
+	/*
+	 * Otherwise make sure the passed virtual Initiator port WWN matches
+	 * the fabric protocol_id set in scsiback_make_tport(), and call
+	 * scsiback_make_nexus().
+	 */
+	if (strlen(page) >= VSCSI_NAMELEN) {
+		pr_err("Emulated NAA Sas Address: %s, exceeds max: %d\n",
+			page, VSCSI_NAMELEN);
+		return -EINVAL;
+	}
+	snprintf(&i_port[0], VSCSI_NAMELEN, "%s", page);
+
+	ptr = strstr(i_port, "naa.");
+	if (ptr) {
+		if (tport_wwn->tport_proto_id != SCSI_PROTOCOL_SAS) {
+			pr_err("Passed SAS Initiator Port %s does not match target port protoid: %s\n",
+				i_port, scsiback_dump_proto_id(tport_wwn));
+			return -EINVAL;
+		}
+		port_ptr = &i_port[0];
+		goto check_newline;
+	}
+	ptr = strstr(i_port, "fc.");
+	if (ptr) {
+		if (tport_wwn->tport_proto_id != SCSI_PROTOCOL_FCP) {
+			pr_err("Passed FCP Initiator Port %s does not match target port protoid: %s\n",
+				i_port, scsiback_dump_proto_id(tport_wwn));
+			return -EINVAL;
+		}
+		port_ptr = &i_port[3]; /* Skip over "fc." */
+		goto check_newline;
+	}
+	ptr = strstr(i_port, "iqn.");
+	if (ptr) {
+		if (tport_wwn->tport_proto_id != SCSI_PROTOCOL_ISCSI) {
+			pr_err("Passed iSCSI Initiator Port %s does not match target port protoid: %s\n",
+				i_port, scsiback_dump_proto_id(tport_wwn));
+			return -EINVAL;
+		}
+		port_ptr = &i_port[0];
+		goto check_newline;
+	}
+	pr_err("Unable to locate prefix for emulated Initiator Port: %s\n",
+		i_port);
+	return -EINVAL;
+	/*
+	 * Clear any trailing newline for the NAA WWN
+	 */
+check_newline:
+	if (i_port[strlen(i_port) - 1] == '\n')
+		i_port[strlen(i_port) - 1] = '\0';
+
+	ret = scsiback_make_nexus(tpg, port_ptr);
+	if (ret < 0)
+		return ret;
+
+	return count;
+}
+
+TF_TPG_BASE_ATTR(scsiback, nexus, S_IRUGO | S_IWUSR);
+
+static struct configfs_attribute *scsiback_tpg_attrs[] = {
+	&scsiback_tpg_nexus.attr,
+	NULL,
+};
+
+static ssize_t
+scsiback_wwn_show_attr_version(struct target_fabric_configfs *tf,
+				char *page)
+{
+	return sprintf(page, "xen-pvscsi fabric module %s on %s/%s on "
+		UTS_RELEASE"\n",
+		VSCSI_VERSION, utsname()->sysname, utsname()->machine);
+}
+
+TF_WWN_ATTR_RO(scsiback, version);
+
+static struct configfs_attribute *scsiback_wwn_attrs[] = {
+	&scsiback_wwn_version.attr,
+	NULL,
+};
+
+static char *scsiback_get_fabric_name(void)
+{
+	return "xen-pvscsi";
+}
+
+static int scsiback_port_link(struct se_portal_group *se_tpg,
+			       struct se_lun *lun)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	tpg->tv_tpg_port_count++;
+	mutex_unlock(&tpg->tv_tpg_mutex);
+
+	return 0;
+}
+
+static void scsiback_port_unlink(struct se_portal_group *se_tpg,
+				  struct se_lun *lun)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+
+	mutex_lock(&tpg->tv_tpg_mutex);
+	tpg->tv_tpg_port_count--;
+	mutex_unlock(&tpg->tv_tpg_mutex);
+}
+
+static struct se_portal_group *
+scsiback_make_tpg(struct se_wwn *wwn,
+		   struct config_group *group,
+		   const char *name)
+{
+	struct scsiback_tport *tport = container_of(wwn,
+			struct scsiback_tport, tport_wwn);
+
+	struct scsiback_tpg *tpg;
+	u16 tpgt;
+	int ret;
+
+	if (strstr(name, "tpgt_") != name)
+		return ERR_PTR(-EINVAL);
+	ret = kstrtou16(name + 5, 10, &tpgt);
+	if (ret)
+		return ERR_PTR(ret);
+
+	tpg = kzalloc(sizeof(struct scsiback_tpg), GFP_KERNEL);
+	if (!tpg)
+		return ERR_PTR(-ENOMEM);
+
+	mutex_init(&tpg->tv_tpg_mutex);
+	INIT_LIST_HEAD(&tpg->tv_tpg_list);
+	INIT_LIST_HEAD(&tpg->info_list);
+	tpg->tport = tport;
+	tpg->tport_tpgt = tpgt;
+
+	ret = core_tpg_register(&scsiback_fabric_configfs->tf_ops, wwn,
+				&tpg->se_tpg, tpg, TRANSPORT_TPG_TYPE_NORMAL);
+	if (ret < 0) {
+		kfree(tpg);
+		return NULL;
+	}
+	mutex_lock(&scsiback_mutex);
+	list_add_tail(&tpg->tv_tpg_list, &scsiback_list);
+	mutex_unlock(&scsiback_mutex);
+
+	return &tpg->se_tpg;
+}
+
+static void scsiback_drop_tpg(struct se_portal_group *se_tpg)
+{
+	struct scsiback_tpg *tpg = container_of(se_tpg,
+				struct scsiback_tpg, se_tpg);
+
+	mutex_lock(&scsiback_mutex);
+	list_del(&tpg->tv_tpg_list);
+	mutex_unlock(&scsiback_mutex);
+	/*
+	 * Release the virtual I_T Nexus for this xen-pvscsi TPG
+	 */
+	scsiback_drop_nexus(tpg);
+	/*
+	 * Deregister the se_tpg from TCM..
+	 */
+	core_tpg_deregister(se_tpg);
+	kfree(tpg);
+}
+
+static int scsiback_check_true(struct se_portal_group *se_tpg)
+{
+	return 1;
+}
+
+static int scsiback_check_false(struct se_portal_group *se_tpg)
+{
+	return 0;
+}
+
+static struct target_core_fabric_ops scsiback_ops = {
+	.get_fabric_name		= scsiback_get_fabric_name,
+	.get_fabric_proto_ident		= scsiback_get_fabric_proto_ident,
+	.tpg_get_wwn			= scsiback_get_fabric_wwn,
+	.tpg_get_tag			= scsiback_get_tag,
+	.tpg_get_default_depth		= scsiback_get_default_depth,
+	.tpg_get_pr_transport_id	= scsiback_get_pr_transport_id,
+	.tpg_get_pr_transport_id_len	= scsiback_get_pr_transport_id_len,
+	.tpg_parse_pr_out_transport_id	= scsiback_parse_pr_out_transport_id,
+	.tpg_check_demo_mode		= scsiback_check_true,
+	.tpg_check_demo_mode_cache	= scsiback_check_true,
+	.tpg_check_demo_mode_write_protect = scsiback_check_false,
+	.tpg_check_prod_mode_write_protect = scsiback_check_false,
+	.tpg_alloc_fabric_acl		= scsiback_alloc_fabric_acl,
+	.tpg_release_fabric_acl		= scsiback_release_fabric_acl,
+	.tpg_get_inst_index		= scsiback_tpg_get_inst_index,
+	.check_stop_free		= scsiback_check_stop_free,
+	.release_cmd			= scsiback_release_cmd,
+	.put_session			= NULL,
+	.shutdown_session		= scsiback_shutdown_session,
+	.close_session			= scsiback_close_session,
+	.sess_get_index			= scsiback_sess_get_index,
+	.sess_get_initiator_sid		= NULL,
+	.write_pending			= scsiback_write_pending,
+	.write_pending_status		= scsiback_write_pending_status,
+	.set_default_node_attributes	= scsiback_set_default_node_attrs,
+	.get_task_tag			= scsiback_get_task_tag,
+	.get_cmd_state			= scsiback_get_cmd_state,
+	.queue_data_in			= scsiback_queue_data_in,
+	.queue_status			= scsiback_queue_status,
+	.queue_tm_rsp			= scsiback_queue_tm_rsp,
+	.aborted_task			= scsiback_aborted_task,
+	/*
+	 * Setup callers for generic logic in target_core_fabric_configfs.c
+	 */
+	.fabric_make_wwn		= scsiback_make_tport,
+	.fabric_drop_wwn		= scsiback_drop_tport,
+	.fabric_make_tpg		= scsiback_make_tpg,
+	.fabric_drop_tpg		= scsiback_drop_tpg,
+	.fabric_post_link		= scsiback_port_link,
+	.fabric_pre_unlink		= scsiback_port_unlink,
+	.fabric_make_np			= NULL,
+	.fabric_drop_np			= NULL,
+#if 0
+	.fabric_make_nodeacl		= scsiback_make_nodeacl,
+	.fabric_drop_nodeacl		= scsiback_drop_nodeacl,
+#endif
+};
+
+static int scsiback_register_configfs(void)
+{
+	struct target_fabric_configfs *fabric;
+	int ret;
+
+	pr_debug("xen-pvscsi: fabric module %s on %s/%s on "UTS_RELEASE"\n",
+		 VSCSI_VERSION, utsname()->sysname, utsname()->machine);
+	/*
+	 * Register the top level struct config_item_type with TCM core
+	 */
+	fabric = target_fabric_configfs_init(THIS_MODULE, "xen-pvscsi");
+	if (IS_ERR(fabric))
+		return PTR_ERR(fabric);
+
+	/*
+	 * Setup fabric->tf_ops from our local scsiback_ops
+	 */
+	fabric->tf_ops = scsiback_ops;
+	/*
+	 * Setup default attribute lists for various fabric->tf_cit_tmpl
+	 */
+	fabric->tf_cit_tmpl.tfc_wwn_cit.ct_attrs = scsiback_wwn_attrs;
+	fabric->tf_cit_tmpl.tfc_tpg_base_cit.ct_attrs = scsiback_tpg_attrs;
+	fabric->tf_cit_tmpl.tfc_tpg_attrib_cit.ct_attrs = NULL;
+	fabric->tf_cit_tmpl.tfc_tpg_param_cit.ct_attrs = scsiback_param_attrs;
+	fabric->tf_cit_tmpl.tfc_tpg_np_base_cit.ct_attrs = NULL;
+	fabric->tf_cit_tmpl.tfc_tpg_nacl_base_cit.ct_attrs = NULL;
+	fabric->tf_cit_tmpl.tfc_tpg_nacl_attrib_cit.ct_attrs = NULL;
+	fabric->tf_cit_tmpl.tfc_tpg_nacl_auth_cit.ct_attrs = NULL;
+	fabric->tf_cit_tmpl.tfc_tpg_nacl_param_cit.ct_attrs = NULL;
+	/*
+	 * Register the fabric for use within TCM
+	 */
+	ret = target_fabric_configfs_register(fabric);
+	if (ret < 0) {
+		target_fabric_configfs_free(fabric);
+		return ret;
+	}
+	/*
+	 * Setup our local pointer to *fabric
+	 */
+	scsiback_fabric_configfs = fabric;
+	pr_debug("xen-pvscsi: Set fabric -> scsiback_fabric_configfs\n");
+	return 0;
+};
+
+static void scsiback_deregister_configfs(void)
+{
+	if (!scsiback_fabric_configfs)
+		return;
+
+	target_fabric_configfs_deregister(scsiback_fabric_configfs);
+	scsiback_fabric_configfs = NULL;
+	pr_debug("xen-pvscsi: Cleared scsiback_fabric_configfs\n");
+};
+
+static const struct xenbus_device_id scsiback_ids[] = {
+	{ "vscsi" },
+	{ "" }
+};
+
+static struct xenbus_driver scsiback_driver = {
+	.ids			= scsiback_ids,
+	.probe			= scsiback_probe,
+	.remove			= scsiback_remove,
+	.otherend_changed	= scsiback_frontend_changed
+};
+
+static void scsiback_init_pend(void *p)
+{
+	struct vscsibk_pend *pend = p;
+	int i;
+
+	memset(pend, 0, sizeof(*pend));
+	for (i = 0; i < VSCSI_MAX_GRANTS; i++)
+		pend->grant_handles[i] = SCSIBACK_INVALID_HANDLE;
+}
+
+static int __init scsiback_init(void)
+{
+	int ret;
+
+	if (!xen_domain())
+		return -ENODEV;
+
+	scsiback_cachep = kmem_cache_create("vscsiif_cache",
+		sizeof(struct vscsibk_pend), 0, 0, scsiback_init_pend);
+	if (!scsiback_cachep)
+		return -ENOMEM;
+
+	ret = xenbus_register_backend(&scsiback_driver);
+	if (ret)
+		goto out_cache_destroy;
+
+	ret = scsiback_register_configfs();
+	if (ret)
+		goto out_unregister_xenbus;
+
+	return 0;
+
+out_unregister_xenbus:
+	xenbus_unregister_driver(&scsiback_driver);
+out_cache_destroy:
+	kmem_cache_destroy(scsiback_cachep);
+	pr_err("xen-pvscsi: %s: error %d\n", __func__, ret);
+	return ret;
+}
+
+static void __exit scsiback_exit(void)
+{
+	struct page *page;
+
+	while (free_pages_num) {
+		if (get_free_page(&page))
+			BUG();
+		free_xenballooned_pages(1, &page);
+	}
+	scsiback_deregister_configfs();
+	xenbus_unregister_driver(&scsiback_driver);
+	kmem_cache_destroy(scsiback_cachep);
+}
+
+module_init(scsiback_init);
+module_exit(scsiback_exit);
+
+MODULE_DESCRIPTION("Xen SCSI backend driver");
+MODULE_LICENSE("Dual BSD/GPL");
+MODULE_ALIAS("xen-backend:vscsi");
+MODULE_AUTHOR("Juergen Gross <jgross@suse.com>");
diff --git a/drivers/xen/xenbus/xenbus_client.c b/drivers/xen/xenbus/xenbus_client.c
index 439c9dca9eee..ca744102b666 100644
--- a/drivers/xen/xenbus/xenbus_client.c
+++ b/drivers/xen/xenbus/xenbus_client.c
@@ -259,7 +259,6 @@ static char *error_path(struct xenbus_device *dev)
 static void xenbus_va_dev_error(struct xenbus_device *dev, int err,
 				const char *fmt, va_list ap)
 {
-	int ret;
 	unsigned int len;
 	char *printf_buffer = NULL;
 	char *path_buffer = NULL;
@@ -270,9 +269,7 @@ static void xenbus_va_dev_error(struct xenbus_device *dev, int err,
 		goto fail;
 
 	len = sprintf(printf_buffer, "%i ", -err);
-	ret = vsnprintf(printf_buffer+len, PRINTF_BUFFER_SIZE-len, fmt, ap);
-
-	BUG_ON(len + ret > PRINTF_BUFFER_SIZE-1);
+	vsnprintf(printf_buffer+len, PRINTF_BUFFER_SIZE-len, fmt, ap);
 
 	dev_err(&dev->dev, "%s\n", printf_buffer);
 
@@ -361,8 +358,8 @@ static void xenbus_switch_fatal(struct xenbus_device *dev, int depth, int err,
  * @ring_mfn: mfn of ring to grant
 
  * Grant access to the given @ring_mfn to the peer of the given device.  Return
- * 0 on success, or -errno on error.  On error, the device will switch to
- * XenbusStateClosing, and the error will be saved in the store.
+ * a grant reference on success, or -errno on error. On error, the device will
+ * switch to XenbusStateClosing, and the error will be saved in the store.
  */
 int xenbus_grant_ring(struct xenbus_device *dev, unsigned long ring_mfn)
 {
diff --git a/drivers/xen/xenbus/xenbus_probe.c b/drivers/xen/xenbus/xenbus_probe.c
index 3c0a74b3e9b1..564b31584860 100644
--- a/drivers/xen/xenbus/xenbus_probe.c
+++ b/drivers/xen/xenbus/xenbus_probe.c
@@ -297,9 +297,13 @@ void xenbus_dev_shutdown(struct device *_dev)
 EXPORT_SYMBOL_GPL(xenbus_dev_shutdown);
 
 int xenbus_register_driver_common(struct xenbus_driver *drv,
-				  struct xen_bus_type *bus)
+				  struct xen_bus_type *bus,
+				  struct module *owner, const char *mod_name)
 {
+	drv->driver.name = drv->name ? drv->name : drv->ids[0].devicetype;
 	drv->driver.bus = &bus->bus;
+	drv->driver.owner = owner;
+	drv->driver.mod_name = mod_name;
 
 	return driver_register(&drv->driver);
 }
diff --git a/drivers/xen/xenbus/xenbus_probe.h b/drivers/xen/xenbus/xenbus_probe.h
index 1085ec294a19..c9ec7ca1f7ab 100644
--- a/drivers/xen/xenbus/xenbus_probe.h
+++ b/drivers/xen/xenbus/xenbus_probe.h
@@ -60,7 +60,9 @@ extern int xenbus_match(struct device *_dev, struct device_driver *_drv);
 extern int xenbus_dev_probe(struct device *_dev);
 extern int xenbus_dev_remove(struct device *_dev);
 extern int xenbus_register_driver_common(struct xenbus_driver *drv,
-					 struct xen_bus_type *bus);
+					 struct xen_bus_type *bus,
+					 struct module *owner,
+					 const char *mod_name);
 extern int xenbus_probe_node(struct xen_bus_type *bus,
 			     const char *type,
 			     const char *nodename);
diff --git a/drivers/xen/xenbus/xenbus_probe_backend.c b/drivers/xen/xenbus/xenbus_probe_backend.c
index 5125dce11a60..04f7f85a5edf 100644
--- a/drivers/xen/xenbus/xenbus_probe_backend.c
+++ b/drivers/xen/xenbus/xenbus_probe_backend.c
@@ -234,13 +234,15 @@ int xenbus_dev_is_online(struct xenbus_device *dev)
 }
 EXPORT_SYMBOL_GPL(xenbus_dev_is_online);
 
-int xenbus_register_backend(struct xenbus_driver *drv)
+int __xenbus_register_backend(struct xenbus_driver *drv, struct module *owner,
+			      const char *mod_name)
 {
 	drv->read_otherend_details = read_frontend_details;
 
-	return xenbus_register_driver_common(drv, &xenbus_backend);
+	return xenbus_register_driver_common(drv, &xenbus_backend,
+					     owner, mod_name);
 }
-EXPORT_SYMBOL_GPL(xenbus_register_backend);
+EXPORT_SYMBOL_GPL(__xenbus_register_backend);
 
 static int backend_probe_and_watch(struct notifier_block *notifier,
 				   unsigned long event,
diff --git a/drivers/xen/xenbus/xenbus_probe_frontend.c b/drivers/xen/xenbus/xenbus_probe_frontend.c
index cb385c10d2b1..bcb53bdc469c 100644
--- a/drivers/xen/xenbus/xenbus_probe_frontend.c
+++ b/drivers/xen/xenbus/xenbus_probe_frontend.c
@@ -317,13 +317,15 @@ static void wait_for_devices(struct xenbus_driver *xendrv)
 			 print_device_status);
 }
 
-int xenbus_register_frontend(struct xenbus_driver *drv)
+int __xenbus_register_frontend(struct xenbus_driver *drv, struct module *owner,
+			       const char *mod_name)
 {
 	int ret;
 
 	drv->read_otherend_details = read_backend_details;
 
-	ret = xenbus_register_driver_common(drv, &xenbus_frontend);
+	ret = xenbus_register_driver_common(drv, &xenbus_frontend,
+					    owner, mod_name);
 	if (ret)
 		return ret;
 
@@ -332,7 +334,7 @@ int xenbus_register_frontend(struct xenbus_driver *drv)
 
 	return 0;
 }
-EXPORT_SYMBOL_GPL(xenbus_register_frontend);
+EXPORT_SYMBOL_GPL(__xenbus_register_frontend);
 
 static DECLARE_WAIT_QUEUE_HEAD(backend_state_wq);
 static int backend_state;